From 97edff950f389cc9c7880ed2e7ba4b3a652e3539 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 12 Jul 2019 16:38:45 +0200 Subject: [PATCH 001/612] fix World::getTransform (#1553) ... which was broken in 3e8feeb637 (#1439) --- moveit_core/collision_detection/src/world.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index 4fbf16e76c..f2a404ec8f 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -168,6 +168,7 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name) const const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& frame_found) const { + frame_found = true; std::map::const_iterator it = objects_.find(name); if (it != objects_.end()) return it->second->shape_poses_[0]; @@ -180,10 +181,7 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& fram { auto it = object.second->subframe_poses_.find(name.substr(object.first.length() + 1)); if (it != object.second->subframe_poses_.end()) - { - frame_found = true; return it->second; - } } } } From 56b7c61a88287bf027415d96ce038de668d2e05a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Fri, 12 Jul 2019 21:18:20 +0200 Subject: [PATCH 002/612] add RobotTrajectory::getDuration() (#1554) --- .../include/moveit/robot_trajectory/robot_trajectory.h | 2 ++ moveit_core/robot_trajectory/src/robot_trajectory.cpp | 8 ++++++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 64f4fe309e..7b3a4b9507 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -202,6 +202,8 @@ class RobotTrajectory void clear(); + double getDuration() const; + double getAverageSegmentDuration() const; void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory) const; diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index b0d5de749f..0c96f9c8dd 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -66,13 +66,17 @@ const std::string& RobotTrajectory::getGroupName() const return EMPTY; } +double RobotTrajectory::getDuration() const +{ + return std::accumulate(duration_from_previous_.begin(), duration_from_previous_.end(), 0.0); +} + double RobotTrajectory::getAverageSegmentDuration() const { if (duration_from_previous_.empty()) return 0.0; else - return std::accumulate(duration_from_previous_.begin(), duration_from_previous_.end(), 0.0) / - (double)duration_from_previous_.size(); + return getDuration() / static_cast(duration_from_previous_.size()); } void RobotTrajectory::swap(RobotTrajectory& other) From b853c6bc48c41af7475db8cbb4d11078bb8725eb Mon Sep 17 00:00:00 2001 From: Bianca Homberg Date: Tue, 16 Jul 2019 03:05:06 -0700 Subject: [PATCH 003/612] Ensure that if time parameterization fails, error is appropriately propogated (#1562) --- .../src/add_iterative_spline_parameterization.cpp | 5 ++++- .../src/add_time_optimal_parameterization.cpp | 5 ++++- .../src/add_time_parameterization.cpp | 5 ++++- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp index 6904160457..bf4f89a2b8 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp @@ -68,7 +68,10 @@ class AddIterativeSplineParameterization : public planning_request_adapter::Plan ROS_DEBUG("Running '%s'", getDescription().c_str()); if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) - ROS_WARN("Time parametrization for the solution path failed."); + { + ROS_ERROR("Time parametrization for the solution path failed."); + result = false; + } } return result; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp index 805e6b7329..336307fedb 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -71,7 +71,10 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning TimeOptimalTrajectoryGeneration totg; if (!totg.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) - ROS_WARN("Time parametrization for the solution path failed."); + { + ROS_ERROR("Time parametrization for the solution path failed."); + result = false; + } } return result; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp index be06624689..e3d1b17522 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp @@ -67,7 +67,10 @@ class AddTimeParameterization : public planning_request_adapter::PlanningRequest ROS_DEBUG("Running '%s'", getDescription().c_str()); if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) - ROS_WARN("Time parametrization for the solution path failed."); + { + ROS_ERROR("Time parametrization for the solution path failed."); + result = false; + } } return result; From 6c7e84c4c5185cf86c4b1daa1bdf898fd9a1e3fc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Fri, 19 Jul 2019 12:10:27 +0200 Subject: [PATCH 004/612] Use portable string() on filesystem::path. (#1571) --- .../src/tools/moveit_config_data.cpp | 2 +- .../widgets/configuration_files_widget.cpp | 4 ++-- .../src/widgets/start_screen_widget.cpp | 20 +++++++++---------- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index b563b7e882..124246f6c1 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -1693,7 +1693,7 @@ std::string MoveItConfigData::appendPaths(const std::string& path1, const std::s { fs::path result = path1; result /= path2; - return result.make_preferred().native(); + return result.make_preferred().string(); } srdf::Model::Group* MoveItConfigData::findGroupByName(const std::string& name) diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 1f87a5a506..4b1097d974 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -183,7 +183,7 @@ bool ConfigurationFilesWidget::loadGenFiles() fs::path template_package_path = config_data_->setup_assistant_path_; template_package_path /= "templates"; template_package_path /= "moveit_config_pkg_template"; - config_data_->template_package_path_ = template_package_path.make_preferred().native(); + config_data_->template_package_path_ = template_package_path.make_preferred().string(); if (!fs::is_directory(config_data_->template_package_path_)) { @@ -1008,7 +1008,7 @@ const std::string ConfigurationFilesWidget::getPackageName(std::string package_p std::string package_name; fs::path fs_package_path = package_path; - package_name = fs_package_path.filename().c_str(); + package_name = fs_package_path.filename().string(); // check for empty if (package_name.empty()) diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 8a8fadbd27..fe54c91fa2 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -400,14 +400,14 @@ bool StartScreenWidget::loadExistingFiles() fs::path kinematics_yaml_path = config_data_->config_pkg_path_; kinematics_yaml_path /= "config/kinematics.yaml"; - if (!config_data_->inputKinematicsYAML(kinematics_yaml_path.make_preferred().native())) + if (!config_data_->inputKinematicsYAML(kinematics_yaml_path.make_preferred().string())) { QMessageBox::warning(this, "No Kinematic YAML File", QString("Failed to parse kinematics yaml file. This file is not critical but any previous " "kinematic solver settings have been lost. To re-populate this file edit each " "existing planning group and choose a solver, then save each change. \n\nFile error " "at location ") - .append(kinematics_yaml_path.make_preferred().native().c_str())); + .append(kinematics_yaml_path.make_preferred().string().c_str())); } // Load 3d_sensors config file @@ -416,11 +416,11 @@ bool StartScreenWidget::loadExistingFiles() // Load ros controllers yaml file if available----------------------------------------------- fs::path ros_controllers_yaml_path = config_data_->config_pkg_path_; ros_controllers_yaml_path /= "config/ros_controllers.yaml"; - config_data_->inputROSControllersYAML(ros_controllers_yaml_path.make_preferred().native()); + config_data_->inputROSControllersYAML(ros_controllers_yaml_path.make_preferred().string()); fs::path ompl_yaml_path = config_data_->config_pkg_path_; ompl_yaml_path /= "config/ompl_planning.yaml"; - config_data_->inputOMPLYAML(ompl_yaml_path.make_preferred().native()); + config_data_->inputOMPLYAML(ompl_yaml_path.make_preferred().string()); // DONE LOADING -------------------------------------------------------------------------- @@ -645,7 +645,7 @@ bool StartScreenWidget::extractPackageNameFromPath() // Copy path into vector of parts for (fs::path::iterator it = urdf_directory.begin(); it != urdf_directory.end(); ++it) - path_parts.push_back(it->native()); + path_parts.push_back(it->string()); bool package_found = false; @@ -670,7 +670,7 @@ bool StartScreenWidget::extractPackageNameFromPath() // check if this directory has a package.xml package_path = sub_path; package_path /= "package.xml"; - ROS_DEBUG_STREAM("Checking for " << package_path.make_preferred().native()); + ROS_DEBUG_STREAM("Checking for " << package_path.make_preferred().string()); // Check if the files exist if (fs::is_regular_file(package_path) || fs::is_regular_file(sub_path / "manifest.xml")) @@ -711,7 +711,7 @@ bool StartScreenWidget::extractPackageNameFromPath() // Success config_data_->urdf_pkg_name_ = package_name; - config_data_->urdf_pkg_relative_path_ = relative_path.make_preferred().native(); + config_data_->urdf_pkg_relative_path_ = relative_path.make_preferred().string(); } ROS_DEBUG_STREAM("URDF Package Name: " << config_data_->urdf_pkg_name_); @@ -781,12 +781,12 @@ bool StartScreenWidget::load3DSensorsFile() if (!fs::is_regular_file(sensors_3d_yaml_path)) { - return config_data_->input3DSensorsYAML(default_sensors_3d_yaml_path.make_preferred().native()); + return config_data_->input3DSensorsYAML(default_sensors_3d_yaml_path.make_preferred().string()); } else { - return config_data_->input3DSensorsYAML(default_sensors_3d_yaml_path.make_preferred().native(), - sensors_3d_yaml_path.make_preferred().native()); + return config_data_->input3DSensorsYAML(default_sensors_3d_yaml_path.make_preferred().string(), + sensors_3d_yaml_path.make_preferred().string()); } } From fd0a52ebcc04accd1a9e0ef73fc7314ebbd3fc27 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 25 Jul 2019 08:56:02 -0600 Subject: [PATCH 005/612] MoveIt benchmark improvements (#1510) * Add pseudo experiment all_experiments to allow comparing all entries * Expose loadBenchmarkQueryData() for setting up custom queries * Add benchmark entry for comparing the 'final' result trajectory Different planners don't always use the same description pattern in their MotionPlanDetailedResponse. This makes it difficult to compare benchmark results. This adds an extra field where all values of the last solution trajectory are stored. * Add trajectory similarity function to measure repeatability * Address requested changes * Fill empty fields in all_experiments * Improve variable and function names * Add helper function computeTrajectoryDistance() --- .../moveit/benchmarks/BenchmarkExecutor.h | 21 ++ .../scripts/moveit_benchmark_statistics.py | 51 ++++- .../benchmarks/src/BenchmarkExecutor.cpp | 209 +++++++++++++++--- 3 files changed, 238 insertions(+), 43 deletions(-) diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index ed88584133..b2aafebe90 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -141,9 +141,30 @@ class BenchmarkExecutor virtual bool initializeBenchmarks(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg, std::vector& queries); + /// Initialize benchmark query data from start states and constraints + virtual bool loadBenchmarkQueryData(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg, + std::vector& start_states, + std::vector& path_constraints, + std::vector& goal_constraints, + std::vector& traj_constraints, + std::vector& queries); + virtual void collectMetrics(PlannerRunData& metrics, const planning_interface::MotionPlanDetailedResponse& mp_res, bool solved, double total_time); + /// Compute the similarity of each (final) trajectory to all other (final) trajectories in the experiment and write + /// the results to planner_data metrics + void computeAveragePathSimilarities(PlannerBenchmarkData& planner_data, + const std::vector& responses, + const std::vector& solved); + + /// Helper function used by computeAveragePathSimilarities() for computing a heuristic distance metric between two + /// robot trajectories. This function aligns both trajectories in a greedy fashion and computes the mean waypoint + /// distance averaged over all aligned waypoints. Using a greedy approach is more efficient than dynamic time warping, + /// and seems to be sufficient for similar trajectories. + bool computeTrajectoryDistance(const robot_trajectory::RobotTrajectory& traj_first, + const robot_trajectory::RobotTrajectory& traj_second, double& result_distance); + virtual void writeOutput(const BenchmarkRequest& brequest, const std::string& start_time, double benchmark_duration); void shiftConstraintsByOffset(moveit_msgs::Constraints& constraints, const std::vector& offset); diff --git a/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py b/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py index fd84005a33..666e64c60d 100755 --- a/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py +++ b/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py @@ -112,7 +112,7 @@ def readBenchmarkLog(dbname, filenames): # create all tables if they don't already exist c.executescript("""CREATE TABLE IF NOT EXISTS experiments - (id INTEGER PRIMARY KEY AUTOINCREMENT, name VARCHAR(512), + (id INTEGER PRIMARY KEY ON CONFLICT REPLACE AUTOINCREMENT, name VARCHAR(512), totaltime REAL, timelimit REAL, memorylimit REAL, runcount INTEGER, version VARCHAR(128), hostname VARCHAR(1024), cpuinfo TEXT, date DATETIME, seed INTEGER, setup TEXT); @@ -130,7 +130,19 @@ def readBenchmarkLog(dbname, filenames): (runid INTEGER, time REAL, PRIMARY KEY (runid, time), FOREIGN KEY (runid) REFERENCES runs(id) ON DELETE CASCADE)""") - for filename in filenames: + # add placeholder entry for all_experiments + allExperimentsName = "all_experiments" + allExperimentsValues = { + "totaltime": 0.0, "timelimit": 0.0, "memorylimit": 0.0, "runcount": 0, "version": "0.0.0", + "hostname": "", "cpuinfo": "", "date": 0, "seed": 0, "setup": "" + } + addAllExperiments = len(filenames) > 0 + if addAllExperiments: + c.execute('INSERT INTO experiments VALUES (?,?,?,?,?,?,?,?,?,?,?,?)', + (None, allExperimentsName) + tuple(allExperimentsValues.values())) + allExperimentsId = c.lastrowid + + for i, filename in enumerate(filenames): print('Processing ' + filename) logfile = open(filename,'r') start_pos = logfile.tell() @@ -155,7 +167,19 @@ def readBenchmarkLog(dbname, filenames): nrruns = -1 if nrrunsOrNone != None: nrruns = int(nrrunsOrNone) + allExperimentsValues['runcount'] += nrruns totaltime = float(readRequiredLogValue("total time", logfile, 0, {-3 : "collect", -2 : "the", -1 : "data"})) + # fill in fields of all_experiments + allExperimentsValues['totaltime'] += totaltime + allExperimentsValues['memorylimit'] = max(allExperimentsValues['memorylimit'], totaltime) + allExperimentsValues['timelimit'] = max(allExperimentsValues['timelimit'], totaltime) + # copy the fields of the first file to all_experiments so that they are not empty + if (i==0): + allExperimentsValues['version'] = version + allExperimentsValues['date'] = date + allExperimentsValues['setup'] = expsetup + allExperimentsValues['hostname'] = hostname + allExperimentsValues['cpuinfo'] = cpuinfo numEnums = 0 numEnumsOrNone = readOptionalLogValue(logfile, 0, {-2 : "enum"}) if numEnumsOrNone != None: @@ -213,13 +237,17 @@ def readBenchmarkLog(dbname, filenames): numRuns = int(logfile.readline().split()[0]) runIds = [] for j in range(numRuns): - values = tuple([experimentId, plannerId] + \ - [None if len(x) == 0 or x == 'nan' or x == 'inf' else x - for x in logfile.readline().split('; ')[:-1]]) + runValues = [None if len(x) == 0 or x == 'nan' or x == 'inf' else x + for x in logfile.readline().split('; ')[:-1]] + values = tuple([experimentId, plannerId] + runValues) c.execute(insertFmtStr, values) # extract primary key of each run row so we can reference them # in the planner progress data table if needed runIds.append(c.lastrowid) + # add all run data to all_experiments + if addAllExperiments: + values = tuple([allExperimentsId, plannerId] + runValues) + c.execute(insertFmtStr, values) nextLine = logfile.readline().strip() @@ -259,6 +287,15 @@ def readBenchmarkLog(dbname, filenames): logfile.readline() logfile.close() + + if addAllExperiments: + updateString = "UPDATE experiments SET" + for i, (key, val) in enumerate(allExperimentsValues.items()): + if i > 0: + updateString += "," + updateString += " " + str(key) + "='" + str(val) + "'" + updateString += "WHERE id='" + str(allExperimentsId) + "'" + c.execute(updateString) conn.commit() c.close() @@ -541,9 +578,9 @@ def computeViews(dbname): (options, args) = parser.parse_args() if len(args) == 0: - parser.error("No arguments were provided. Please provide full path of log file") + parser.error("No arguments were provided. Please provide full path of log file") - if len(args) == 1: + if len(args) > 0: readBenchmarkLog(options.dbname, args) # If we update the database, we recompute the views as well options.view = True diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index aa59bed3f9..ede31252e9 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -281,45 +281,16 @@ bool BenchmarkExecutor::initializeBenchmarks(const BenchmarkOptions& opts, movei if (!plannerConfigurationsExist(opts.getPlannerConfigurations(), opts.getGroupName())) return false; - try - { - warehouse_ros::DatabaseConnection::Ptr conn = dbloader.loadDatabase(); - conn->setParams(opts.getHostName(), opts.getPort(), 20); - if (conn->connect()) - { - pss_ = new moveit_warehouse::PlanningSceneStorage(conn); - psws_ = new moveit_warehouse::PlanningSceneWorldStorage(conn); - rs_ = new moveit_warehouse::RobotStateStorage(conn); - cs_ = new moveit_warehouse::ConstraintsStorage(conn); - tcs_ = new moveit_warehouse::TrajectoryConstraintsStorage(conn); - } - else - { - ROS_ERROR("Failed to connect to DB"); - return false; - } - } - catch (std::exception& e) - { - ROS_ERROR("Failed to initialize benchmark server: '%s'", e.what()); - return false; - } - std::vector start_states; std::vector path_constraints; std::vector goal_constraints; std::vector traj_constraints; std::vector queries; - bool ok = loadPlanningScene(opts.getSceneName(), scene_msg) && loadStates(opts.getStartStateRegex(), start_states) && - loadPathConstraints(opts.getGoalConstraintRegex(), goal_constraints) && - loadPathConstraints(opts.getPathConstraintRegex(), path_constraints) && - loadTrajectoryConstraints(opts.getTrajectoryConstraintRegex(), traj_constraints) && - loadQueries(opts.getQueryRegex(), opts.getSceneName(), queries); - - if (!ok) + if (!loadBenchmarkQueryData(opts, scene_msg, start_states, path_constraints, goal_constraints, traj_constraints, + queries)) { - ROS_ERROR("Failed to load benchmark stuff"); + ROS_ERROR("Failed to load benchmark query data"); return false; } @@ -429,6 +400,44 @@ bool BenchmarkExecutor::initializeBenchmarks(const BenchmarkOptions& opts, movei return true; } +bool BenchmarkExecutor::loadBenchmarkQueryData(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg, + std::vector& start_states, + std::vector& path_constraints, + std::vector& goal_constraints, + std::vector& traj_constraints, + std::vector& queries) +{ + try + { + warehouse_ros::DatabaseConnection::Ptr warehouse_connection = dbloader.loadDatabase(); + warehouse_connection->setParams(opts.getHostName(), opts.getPort(), 20); + if (warehouse_connection->connect()) + { + pss_ = new moveit_warehouse::PlanningSceneStorage(warehouse_connection); + psws_ = new moveit_warehouse::PlanningSceneWorldStorage(warehouse_connection); + rs_ = new moveit_warehouse::RobotStateStorage(warehouse_connection); + cs_ = new moveit_warehouse::ConstraintsStorage(warehouse_connection); + tcs_ = new moveit_warehouse::TrajectoryConstraintsStorage(warehouse_connection); + } + else + { + ROS_ERROR("Failed to connect to DB"); + return false; + } + } + catch (std::exception& e) + { + ROS_ERROR("Failed to initialize benchmark server: '%s'", e.what()); + return false; + } + + return loadPlanningScene(opts.getSceneName(), scene_msg) && loadStates(opts.getStartStateRegex(), start_states) && + loadPathConstraints(opts.getGoalConstraintRegex(), goal_constraints) && + loadPathConstraints(opts.getPathConstraintRegex(), path_constraints) && + loadTrajectoryConstraints(opts.getTrajectoryConstraintRegex(), traj_constraints) && + loadQueries(opts.getQueryRegex(), opts.getSceneName(), queries); +} + void BenchmarkExecutor::shiftConstraintsByOffset(moveit_msgs::Constraints& constraints, const std::vector& offset) { @@ -765,6 +774,9 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::MotionPlanRequest request, { // This container stores all of the benchmark data for this planner PlannerBenchmarkData planner_data(runs); + // This vector stores all motion plan results for further evaluation + std::vector responses(runs); + std::vector solved(runs); request.planner_id = planner.second[i]; @@ -781,9 +793,8 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::MotionPlanRequest request, pre_event_fn(request); // Solve problem - planning_interface::MotionPlanDetailedResponse mp_res; ros::WallTime start = ros::WallTime::now(); - bool solved = context->solve(mp_res); + solved[j] = context->solve(responses[j]); double total_time = (ros::WallTime::now() - start).toSec(); // Collect data @@ -791,14 +802,16 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::MotionPlanRequest request, // Post-run events for (PostRunEventFunction& post_event_fn : post_event_fns_) - post_event_fn(request, mp_res, planner_data[j]); - collectMetrics(planner_data[j], mp_res, solved, total_time); + post_event_fn(request, responses[j], planner_data[j]); + collectMetrics(planner_data[j], responses[j], solved[j], total_time); double metrics_time = (ros::WallTime::now() - start).toSec(); ROS_DEBUG("Spent %lf seconds collecting metrics", metrics_time); ++progress; } + computeAveragePathSimilarities(planner_data, responses, solved); + // Planner completion events for (PlannerCompletionEventFunction& planner_completion_fn : planner_completion_fns_) planner_completion_fn(request, planner_data); @@ -889,6 +902,15 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, metrics["path_" + mp_res.description_[j] + "_clearance REAL"] = moveit::core::toString(clearance); metrics["path_" + mp_res.description_[j] + "_smoothness REAL"] = moveit::core::toString(smoothness); metrics["path_" + mp_res.description_[j] + "_time REAL"] = moveit::core::toString(mp_res.processing_time_[j]); + + if (j == mp_res.trajectory_.size() - 1) + { + metrics["final_path_correct BOOLEAN"] = boost::lexical_cast(correct); + metrics["final_path_length REAL"] = moveit::core::toString(traj_len); + metrics["final_path_clearance REAL"] = moveit::core::toString(clearance); + metrics["final_path_smoothness REAL"] = moveit::core::toString(smoothness); + metrics["final_path_time REAL"] = moveit::core::toString(mp_res.processing_time_[j]); + } process_time -= mp_res.processing_time_[j]; } if (process_time <= 0.0) @@ -897,6 +919,121 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, } } +void BenchmarkExecutor::computeAveragePathSimilarities( + PlannerBenchmarkData& planner_data, const std::vector& responses, + const std::vector& solved) +{ + ROS_INFO("Computing result path similarity"); + const size_t result_count = planner_data.size(); + size_t unsolved = std::count_if(solved.begin(), solved.end(), [](bool s) { return !s; }); + std::vector average_distances(responses.size()); + for (size_t first_traj_i = 0; first_traj_i < result_count; ++first_traj_i) + { + // If trajectory was not solved there is no valid average distance so it's set to max double only + if (!solved[first_traj_i]) + { + average_distances[first_traj_i] = std::numeric_limits::max(); + continue; + } + // Iterate all result trajectories that haven't been compared yet + for (size_t second_traj_i = first_traj_i + 1; second_traj_i < result_count; ++second_traj_i) + { + // Ignore if other result has not been solved + if (!solved[second_traj_i]) + continue; + + // Get final trajectories + const robot_trajectory::RobotTrajectory& traj_first = *responses[first_traj_i].trajectory_.back(); + const robot_trajectory::RobotTrajectory& traj_second = *responses[second_traj_i].trajectory_.back(); + + // Compute trajectory distance + double trajectory_distance; + if (!computeTrajectoryDistance(traj_first, traj_second, trajectory_distance)) + continue; + + // Add average distance to counters of both trajectories + average_distances[first_traj_i] += trajectory_distance; + average_distances[second_traj_i] += trajectory_distance; + } + // Normalize average distance by number of actual comparisons + average_distances[first_traj_i] /= result_count - unsolved - 1; + } + + // Store results in planner_data + for (size_t i = 0; i < result_count; ++i) + planner_data[i]["average_waypoint_distance REAL"] = moveit::core::toString(average_distances[i]); +} + +bool BenchmarkExecutor::computeTrajectoryDistance(const robot_trajectory::RobotTrajectory& traj_first, + const robot_trajectory::RobotTrajectory& traj_second, + double& result_distance) +{ + // Abort if trajectories are empty + if (traj_first.empty() || traj_second.empty()) + return false; + + // Waypoint counter + size_t pos_first = 0; + size_t pos_second = 0; + const size_t max_pos_first = traj_first.getWayPointCount() - 1; + const size_t max_pos_second = traj_second.getWayPointCount() - 1; + + // Compute total distance between pairwise waypoints of both trajectories. + // The selection of waypoint pairs is based on what steps results in the minimal distance between the next pair of + // waypoints. We first check what steps are still possible or if we reached the end of the trajectories. Then we + // compute the pairwise waypoint distances of the pairs from increasing both, the first, or the second trajectory. + // Finally we select the pair that results in the minimal distance, summarize the total distance and iterate + // accordingly. After that we compute the average trajectory distance by normalizing over the number of steps. + double total_distance = 0; + size_t steps = 0; + double current_distance = traj_first.getWayPoint(pos_first).distance(traj_second.getWayPoint(pos_second)); + while (true) + { + // Keep track of total distance and number of comparisons + total_distance += current_distance; + ++steps; + if (pos_first == max_pos_first && pos_second == max_pos_second) // end reached + break; + + // Determine what steps are still possible + bool can_up_first = pos_first < max_pos_first; + bool can_up_second = pos_second < max_pos_second; + bool can_up_both = can_up_first && can_up_second; + + // Compute pair-wise waypoint distances (increasing both, first, or second trajectories). + double up_both = std::numeric_limits::max(); + double up_first = std::numeric_limits::max(); + double up_second = std::numeric_limits::max(); + if (can_up_both) + up_both = traj_first.getWayPoint(pos_first + 1).distance(traj_second.getWayPoint(pos_second + 1)); + if (can_up_first) + up_first = traj_first.getWayPoint(pos_first + 1).distance(traj_second.getWayPoint(pos_second)); + if (can_up_second) + up_second = traj_first.getWayPoint(pos_first).distance(traj_second.getWayPoint(pos_second + 1)); + + // Select actual step, store new distance value and iterate trajectory positions + if (can_up_both && up_both < up_first && up_both < up_second) + { + ++pos_first; + ++pos_second; + current_distance = up_both; + } + else if ((can_up_first && up_first < up_second) || !can_up_second) + { + ++pos_first; + current_distance = up_first; + } + else if (can_up_second) + { + ++pos_second; + current_distance = up_second; + } + } + // Normalize trajectory distance by number of comparison steps + result_distance = total_distance / static_cast(steps); + return true; +} + void BenchmarkExecutor::writeOutput(const BenchmarkRequest& brequest, const std::string& start_time, double benchmark_duration) { From 3cb561de9efb1a41792baabb9a62d72dcc2c1693 Mon Sep 17 00:00:00 2001 From: Dale Koenig Date: Wed, 31 Jul 2019 22:13:13 +0900 Subject: [PATCH 006/612] JogArm: Remove dependency on move_group node (#1569) --- moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h | 5 +---- moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp | 6 +++--- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h b/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h index e610dba2c1..ebc161f263 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h +++ b/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h @@ -42,7 +42,6 @@ #include #include -#include #include #include #include @@ -54,14 +53,12 @@ namespace jog_arm class JogCalcs { public: - JogCalcs(const JogArmParameters parameters, JogArmShared& shared_variables, pthread_mutex_t& mutex, + JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_variables, pthread_mutex_t& mutex, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); protected: ros::NodeHandle nh_; - moveit::planning_interface::MoveGroupInterface move_group_; - sensor_msgs::JointState incoming_jts_; bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables, pthread_mutex_t& mutex); diff --git a/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp b/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp index 55c76fb190..6839481eff 100644 --- a/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp +++ b/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp @@ -42,9 +42,9 @@ namespace jog_arm { // Constructor for the class that handles jogging calculations -JogCalcs::JogCalcs(const JogArmParameters parameters, JogArmShared& shared_variables, pthread_mutex_t& mutex, +JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_variables, pthread_mutex_t& mutex, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) - : move_group_(parameters.move_group_name), tf_listener_(tf_buffer_), parameters_(parameters) + : tf_listener_(tf_buffer_), parameters_(parameters) { // Publish collision status warning_pub_ = nh_.advertise(parameters_.warning_topic, 1); @@ -72,7 +72,7 @@ JogCalcs::JogCalcs(const JogArmParameters parameters, JogArmShared& shared_varia resetVelocityFilters(); - jt_state_.name = move_group_.getJointNames(); + jt_state_.name = joint_model_group_->getVariableNames(); num_joints_ = jt_state_.name.size(); jt_state_.position.resize(num_joints_); jt_state_.velocity.resize(num_joints_); From bc45aecc465daff77efe2b6c4f92f7c972925bbe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 31 Jul 2019 15:23:19 +0200 Subject: [PATCH 007/612] clang-tidy fixup (#1586) --- .../move_group_interface/src/wrap_python_move_group.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index c367477343..b3940182df 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -486,7 +486,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer std::string retimeTrajectory(const std::string& ref_state_str, const std::string& traj_str, double velocity_scaling_factor, double acceleration_scaling_factor, - std::string algorithm) + const std::string& algorithm) { // Convert reference state message to object moveit_msgs::RobotState ref_state_msg; From 985e34c2f6e9a91f4fe2bec9168eca366454e238 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Fri, 2 Aug 2019 06:42:48 -0400 Subject: [PATCH 008/612] Remove ! from MoveIt name (#1590) --- .docker/README.md | 2 +- .docker/ci/Dockerfile | 2 +- .docker/release/Dockerfile | 2 +- .github/CODEOWNERS | 2 +- .github/config.yml | 4 +- CONTRIBUTING.md | 2 +- MIGRATION.md | 2 +- README.md | 8 ++-- moveit.rosinstall | 4 +- moveit/CHANGELOG.rst | 4 +- moveit/package.xml | 2 +- moveit/scripts/README.md | 4 +- moveit/scripts/create_maintainer_table.py | 2 +- moveit/scripts/create_readme_table.py | 4 +- moveit/scripts/maintainer_table_template.html | 4 +- moveit_commander/README.md | 2 +- .../bin/moveit_commander_cmdline.py | 2 +- moveit_commander/package.xml | 2 +- moveit_core/CMakeLists.txt | 2 +- moveit_core/README.md | 2 +- .../background_processing.h | 2 +- .../controller_manager/controller_manager.h | 12 +++--- .../include/moveit/exceptions/exceptions.h | 2 +- .../dox/constraint_representation.dox | 2 +- moveit_core/package.xml | 4 +- .../planning_interface/planning_interface.h | 4 +- .../planning_scene/dox/planning_scene.dox | 2 +- .../include/moveit/robot_model/robot_model.h | 4 +- .../include/moveit/robot_state/conversions.h | 36 +++++++++--------- .../moveit/sensor_manager/sensor_manager.h | 2 +- moveit_core/transforms/src/transforms.cpp | 2 +- .../moveit/utils/robot_model_test_utils.h | 2 +- .../utils/src/robot_model_test_utils.cpp | 2 +- moveit_experimental/README.md | 4 +- .../jog_arm/config/sia5_simulated_config.yaml | 2 +- .../src/jog_arm/collision_check_thread.cpp | 2 +- .../jog_arm/src/jog_arm/jog_calcs.cpp | 2 +- .../test/arm_setup/config/jog_settings.yaml | 2 +- .../src/kinematics_cache_ros.cpp | 2 +- .../kinematics_constraint_aware.h | 2 +- .../kinematics_request_response.h | 2 +- moveit_experimental/package.xml | 4 +- .../cached_ik_kinematics_plugin/README.md | 2 +- .../ikfast_kinematics_plugin/README.md | 4 +- .../scripts/create_ikfast_moveit_plugin.py | 6 +-- .../ikfast61_moveit_plugin_template.cpp | 6 +-- .../kdl_kinematics_plugin.h | 2 +- .../lma_kinematics_plugin.h | 2 +- moveit_kinematics/package.xml | 4 +- .../srv_kinematics_plugin.h | 4 +- .../test/test_kinematics_plugin.cpp | 2 +- moveit_planners/README.md | 2 +- .../chomp/chomp_interface/CHANGELOG.rst | 2 +- .../chomp/chomp_interface/package.xml | 4 +- .../rrbot_chomp_planning_pipeline.launch.xml | 2 +- .../chomp/chomp_motion_planner/CHANGELOG.rst | 2 +- .../chomp/chomp_motion_planner/package.xml | 2 +- .../chomp/chomp_optimizer_adapter/package.xml | 2 +- moveit_planners/moveit_planners/package.xml | 2 +- moveit_planners/ompl/CHANGELOG.rst | 2 +- .../moveit/ompl_interface/ompl_interface.h | 2 +- .../model_based_state_space.h | 4 +- .../src/detail/state_validity_checker.cpp | 2 +- moveit_planners/ompl/package.xml | 4 +- moveit_plugins/README.md | 2 +- .../moveit_fake_controller_manager/README.md | 4 +- .../package.xml | 2 +- moveit_plugins/moveit_plugins/package.xml | 4 +- .../moveit_ros_control_interface/README.md | 12 +++--- .../moveit_core_plugins.xml | 4 +- .../moveit_ros_control_interface/package.xml | 4 +- .../gripper_controller_handle.h | 2 +- .../package.xml | 2 +- .../src/moveit_simple_controller_manager.cpp | 2 +- moveit_ros/README.md | 4 +- moveit_ros/benchmarks/CHANGELOG.rst | 4 +- moveit_ros/benchmarks/README.md | 2 +- moveit_ros/benchmarks/examples/demo1.yaml | 2 +- moveit_ros/benchmarks/examples/demo2.yaml | 2 +- .../benchmarks/examples/demo_obstacles.yaml | 2 +- .../examples/demo_panda_all_planners.yaml | 2 +- moveit_ros/benchmarks/package.xml | 4 +- .../benchmarks/src/BenchmarkExecutor.cpp | 2 +- moveit_ros/manipulation/package.xml | 4 +- moveit_ros/move_group/package.xml | 2 +- .../test_cancel_before_plan_execution.test | 2 +- moveit_ros/moveit_ros/CHANGELOG.rst | 2 +- moveit_ros/moveit_ros/package.xml | 4 +- moveit_ros/occupancy_map_monitor/package.xml | 4 +- moveit_ros/perception/package.xml | 4 +- .../semantic_world/src/semantic_world.cpp | 2 +- moveit_ros/planning/package.xml | 4 +- .../plan_execution/src/plan_with_sensing.cpp | 2 +- .../src/print_planning_scene_info.cpp | 2 +- .../planning/rdf_loader/src/rdf_loader.cpp | 2 +- .../dox/trajectory_execution.dox | 4 +- .../move_group_interface.h | 2 +- moveit_ros/planning_interface/package.xml | 4 +- .../py_bindings_tools/roscpp_initializer.h | 2 +- moveit_ros/robot_interaction/CHANGELOG.rst | 2 +- moveit_ros/robot_interaction/package.xml | 4 +- .../src/motion_planning_frame_context.cpp | 2 +- .../ui/motion_planning_rviz_plugin_frame.ui | 2 +- moveit_ros/visualization/package.xml | 4 +- .../moveitjoy_module.py | 2 +- moveit_ros/warehouse/CHANGELOG.rst | 2 +- moveit_ros/warehouse/package.xml | 4 +- .../warehouse/src/initialize_demo_db.cpp | 2 +- moveit_runtime/package.xml | 2 +- moveit_setup_assistant/CHANGELOG.rst | 2 +- moveit_setup_assistant/README.md | 2 +- .../tools/moveit_config_data.h | 6 +-- .../launch/setup_assistant.launch | 2 +- moveit_setup_assistant/package.xml | 4 +- .../resources/source/MoveIt_Setup_Asst_Sm.xcf | Bin 375316 -> 375312 bytes .../src/collisions_updater.cpp | 2 +- .../src/tools/moveit_config_data.cpp | 8 ++-- .../src/widgets/author_information_widget.cpp | 6 +-- .../widgets/configuration_files_widget.cpp | 29 +++++++------- .../src/widgets/group_edit_widget.cpp | 4 +- .../src/widgets/perception_widget.cpp | 2 +- .../src/widgets/planning_groups_widget.cpp | 2 +- .../src/widgets/robot_poses_widget.cpp | 6 +-- .../src/widgets/ros_controllers_widget.cpp | 4 +- .../src/widgets/setup_assistant_widget.cpp | 10 ++--- .../src/widgets/simulation_widget.cpp | 2 +- .../src/widgets/start_screen_widget.cpp | 18 ++++----- .../src/widgets/start_screen_widget.h | 4 +- .../launch/chomp_planning_pipeline.launch.xml | 2 +- .../launch/demo.launch | 4 +- .../launch/demo_gazebo.launch | 4 +- .../launch/edit_configuration_package.launch | 2 +- .../launch/ompl_planning_pipeline.launch.xml | 2 +- .../launch/trajectory_execution.launch.xml | 2 +- .../package.xml.template | 2 +- .../test/moveit_config_data_test.cpp | 2 +- 136 files changed, 243 insertions(+), 244 deletions(-) diff --git a/.docker/README.md b/.docker/README.md index a0cf726f20..b24af099ec 100644 --- a/.docker/README.md +++ b/.docker/README.md @@ -1,3 +1,3 @@ -# MoveIt! Docker Containers +# MoveIt Docker Containers For more information see [Continuous Integration and Docker](http://moveit.ros.org/documentation/contributing/continuous_integration.html) documentation. diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index dd4c77c91f..830c8abd09 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -33,7 +33,7 @@ RUN \ clang clang-format-3.9 clang-tidy clang-tools \ ccache && \ # - # Download all dependencies of MoveIt! + # Download all dependencies of MoveIt rosdep update && \ rosdep install -y --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \ # diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile index e06998b1a2..4611c98b20 100644 --- a/.docker/release/Dockerfile +++ b/.docker/release/Dockerfile @@ -1,5 +1,5 @@ # moveit/moveit:melodic-release -# Full debian-based install of MoveIt! using apt-get +# Full debian-based install of MoveIt using apt-get FROM ros:melodic-ros-base MAINTAINER Dave Coleman dave@picknik.ai diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index a6d58c6912..ca05e3da91 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,4 +1,4 @@ -# MoveIt! is a large project with pull requests being created often. In order +# MoveIt is a large project with pull requests being created often. In order # to ensure quick review turn around time from our maintainer team, we're # leveraging an automated "triage" approach to auto-assign reviews to new pull # requests. If you already know who should review your PR, then you can assign diff --git a/.github/config.yml b/.github/config.yml index fed71ab355..2810cc56b6 100644 --- a/.github/config.yml +++ b/.github/config.yml @@ -11,12 +11,12 @@ newIssueWelcomeComment: > # Comment to be posted to on PRs from first time contributors in your repository newPRWelcomeComment: > - Thanks for helping in improving MoveIt! + Thanks for helping in improving MoveIt # Configuration for first-pr-merge - https://github.com/behaviorbot/first-pr-merge # Comment to be posted to on pull requests merged by a first time user firstPRMergeComment: > - Congrats on getting your first MoveIt! pull request merged and improving open source robotics! + Congrats on getting your first MoveIt pull request merged and improving open source robotics! # It is recommended to include as many gifs and emojis as possible! diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index eeafb30ddf..609c244cf6 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,4 +1,4 @@ -# Contributing to MoveIt! +# Contributing to MoveIt Thanks for getting involved! Information on contributing can be found at [http://moveit.ros.org/documentation/contributing/](http://moveit.ros.org/documentation/contributing/) diff --git a/MIGRATION.md b/MIGRATION.md index 05740746a4..5eca3c2660 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -1,6 +1,6 @@ # Migration Notes -API changes in MoveIt! releases +API changes in MoveIt releases ## ROS Noetic (upcoming changes in master) - Extended the return value of `MoveitCommander.MoveGroup.plan()` from `trajectory` to a tuple of `(success, trajectory, planning_time, error_code)` to better match the C++ MoveGroupInterface ([790](https://github.com/ros-planning/moveit/pull/790/)) diff --git a/README.md b/README.md index 0ffd0e0e3b..182da15475 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,10 @@ -MoveIt! Logo +MoveIt Logo -The MoveIt! Motion Planning Framework +The MoveIt Motion Planning Framework Currently we support ROS Indigo, Kinetic, and Melodic. -- [Overview of MoveIt!](http://moveit.ros.org) +- [Overview of MoveIt](http://moveit.ros.org) - [Installation Instructions](http://moveit.ros.org/install/) - [Documentation](http://moveit.ros.org/documentation/) - [Get Involved](http://moveit.ros.org/documentation/contributing/) @@ -32,7 +32,7 @@ build farm | [![Build Status](http://build.ros.org/buildStatus/icon?job=Idev__mo [![Docker Automated build](https://img.shields.io/docker/automated/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit/) [![Docker Pulls](https://img.shields.io/docker/pulls/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit/) [![Docker Stars](https://img.shields.io/docker/stars/moveit/moveit.svg)](https://registry.hub.docker.com/moveit/moveit/) ## ROS Buildfarm -MoveIt! Package | Indigo Source | Indigo Debian | Kinetic Source | Kinetic Debian | Melodic Source | Melodic Debian +MoveIt Package | Indigo Source | Indigo Debian | Kinetic Source | Kinetic Debian | Melodic Source | Melodic Debian --------------- | ------------- | ------------- | -------------- | -------------- | -------------- | -------------- moveit | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit__ubuntu_bionic_amd64__binary) moveit_chomp_optimizer_adapter | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary) diff --git a/moveit.rosinstall b/moveit.rosinstall index f0ef21afa0..ecab1ac02f 100644 --- a/moveit.rosinstall +++ b/moveit.rosinstall @@ -1,5 +1,5 @@ -# This file is intended for users who want to build MoveIt! from source. -# Used with wstool, users can download source of all packages of MoveIt!. +# This file is intended for users who want to build MoveIt from source. +# Used with wstool, users can download source of all packages of MoveIt. - git: local-name: moveit_msgs uri: https://github.com/ros-planning/moveit_msgs.git diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst index 9f03a155e1..482cd91835 100644 --- a/moveit/CHANGELOG.rst +++ b/moveit/CHANGELOG.rst @@ -64,7 +64,7 @@ Changelog for package moveit 0.10.2 (2018-10-24) ------------------- -* [fix] Text refrences to MoveIt! (`#1020 `_) +* [fix] Text refrences to MoveIt (`#1020 `_) * [fix] Eigen alignment issuses due to missing aligned allocation (`#1039 `_) * [fix][chomp] changelogs: migration from tf -> tf2 only accidentally became part of 0.9.12's changelog * [fix] Chomp package handling issue `#1086 `_ that was introduced in `ubi-agni/hotfix-#1012 `_ @@ -82,7 +82,7 @@ Changelog for package moveit * [capability][chomp] Failure recovery options for CHOMP by tweaking parameters (`#987 `_) * [capability] New screen for automatically generating interfaces to low level controllers(`#951 `_, `#994 `_, `#908 `_) * [capability][moveit_setup_assistant] Perception screen for using laser scanner point clouds. (`#969 `_) -* [enhancement][GUI][moveit_setup_assistant] Logo for MoveIt! 2.0, cleanup appearance (`#1059 `_) +* [enhancement][GUI][moveit_setup_assistant] Logo for MoveIt 2.0, cleanup appearance (`#1059 `_) * [enhancement][GUI][moveit_setup_assistant] added a setup assistant window icon (`#1028 `_) * [capability][chomp] Addition of CHOMP planning adapter for optimizing result of other planners (`#1012 `_) * [capability][chomp] Failure recovery options for CHOMP by tweaking parameters (`#987 `_) diff --git a/moveit/package.xml b/moveit/package.xml index 0a60b4860e..5a50dbd315 100644 --- a/moveit/package.xml +++ b/moveit/package.xml @@ -2,7 +2,7 @@ moveit 1.0.1 - Meta package that contains all essential package of MoveIt!. Until Summer 2016 MoveIt! had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See the detailed discussion for the merge of several repositories. + Meta package that contains all essential package of MoveIt. Until Summer 2016 MoveIt had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See the detailed discussion for the merge of several repositories. Dave Coleman Michael Ferguson Michael Görner diff --git a/moveit/scripts/README.md b/moveit/scripts/README.md index ddc705b86f..e2da938586 100644 --- a/moveit/scripts/README.md +++ b/moveit/scripts/README.md @@ -1,8 +1,8 @@ -# Utility Scripts for MoveIt! +# Utility Scripts for MoveIt ## README Markdown Buildfarm Table Generator -This Python script will generate Github-style Markdown table with Jenkins badges for every package recursively from the current folder. It will order all packages alphabetically, but start with all MoveIt! packages first. For generating an updated README for a new ROS distribution, first update the `create_readme_table.py` script and add/remove the current MoveIt!-supported ROS distros and their corresponding Ubuntu distribution in the specified Python dictionary. Then, in the main MoveIt! `README.md` file, remove the "## ROS Buildfarm" header and the following table. Finally, from your catkin workspace with every MoveIt! package, run the `create_readme_table.py` script. For example: +This Python script will generate Github-style Markdown table with Jenkins badges for every package recursively from the current folder. It will order all packages alphabetically, but start with all MoveIt packages first. For generating an updated README for a new ROS distribution, first update the `create_readme_table.py` script and add/remove the current MoveIt-supported ROS distros and their corresponding Ubuntu distribution in the specified Python dictionary. Then, in the main MoveIt `README.md` file, remove the "## ROS Buildfarm" header and the following table. Finally, from your catkin workspace with every MoveIt package, run the `create_readme_table.py` script. For example: cd ~/ws_moveit/src python moveit/moveit/scripts/create_readme_table.py >> moveit/README.md diff --git a/moveit/scripts/create_maintainer_table.py b/moveit/scripts/create_maintainer_table.py index af7615c2a9..1557530ee2 100644 --- a/moveit/scripts/create_maintainer_table.py +++ b/moveit/scripts/create_maintainer_table.py @@ -98,7 +98,7 @@ def populate_package_data(path, package): def list_moveit_packages(): """ - Creates MoveIt! List + Creates MoveIt List """ output = '' packages = find_packages(os.getcwd()) diff --git a/moveit/scripts/create_readme_table.py b/moveit/scripts/create_readme_table.py index 9a5af08554..e854893b0b 100644 --- a/moveit/scripts/create_readme_table.py +++ b/moveit/scripts/create_readme_table.py @@ -11,7 +11,7 @@ def create_header(ros_ubuntu_dict): ros_distros = sorted(ros_ubuntu_dict.keys()) section_header = "## ROS Buildfarm\n" - header="MoveIt! Package" + header="MoveIt Package" header_lines = '-'*len(header) for ros in ros_distros: source = ' '.join([ros.capitalize(), "Source"]) @@ -49,7 +49,7 @@ def create_line(package, ros_ubuntu_dict): def create_moveit_buildfarm_table(): """ - Creates MoveIt! buildfarm badge table + Creates MoveIt buildfarm badge table """ # Update the following dictionary with the appropriate ROS-Ubuntu # combinations for supported distribitions. For instance, in Noetic, diff --git a/moveit/scripts/maintainer_table_template.html b/moveit/scripts/maintainer_table_template.html index b5c94f4020..584b1d68d6 100644 --- a/moveit/scripts/maintainer_table_template.html +++ b/moveit/scripts/maintainer_table_template.html @@ -2,7 +2,7 @@ - MoveIt! Package Tracking + MoveIt Package Tracking @@ -23,7 +23,7 @@ -

MoveIt! Package Tracking

+

MoveIt Package Tracking

diff --git a/moveit_commander/README.md b/moveit_commander/README.md index ecd51ed3b1..1b8f538576 100644 --- a/moveit_commander/README.md +++ b/moveit_commander/README.md @@ -1 +1 @@ -# MoveIt! Commander +# MoveIt Commander diff --git a/moveit_commander/bin/moveit_commander_cmdline.py b/moveit_commander/bin/moveit_commander_cmdline.py index eb3c524c74..2dffaa7630 100755 --- a/moveit_commander/bin/moveit_commander_cmdline.py +++ b/moveit_commander/bin/moveit_commander_cmdline.py @@ -151,7 +151,7 @@ def sigint_handler(signal, frame): rospy.init_node('move_group_interface_cmdline', anonymous=True, disable_signals=True) parser = argparse.ArgumentParser(usage = """%(prog)s [options] []""", - description = "Command Line Interface to MoveIt!") + description = "Command Line Interface to MoveIt") parser.add_argument("-i", "--interactive", action="store_true", dest="interactive", default=True, help="Run the command processing script in interactive mode (default)") parser.add_argument("-s", "--service", action="store_true", dest="service", default=False, diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml index 6680232adc..a962c6a2df 100644 --- a/moveit_commander/package.xml +++ b/moveit_commander/package.xml @@ -7,7 +7,7 @@ Michael Görner Robert Haschke - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 9fd0c3bc6e..4e450735db 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -174,7 +174,7 @@ string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_MINOR "${${PROJ string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_PATCH "${${PROJECT_NAME}_VERSION}") set(MOVEIT_VERSION_EXTRA "Alpha") set(MOVEIT_VERSION "${MOVEIT_VERSION_MAJOR}.${MOVEIT_VERSION_MINOR}.${MOVEIT_VERSION_PATCH}-${MOVEIT_VERSION_EXTRA}") -message(STATUS " *** Building MoveIt! ${MOVEIT_VERSION} ***") +message(STATUS " *** Building MoveIt ${MOVEIT_VERSION} ***") configure_file("version/version.h.in" "${VERSION_FILE_PATH}/moveit/version.h") #catkin_lint: ignore_once external_file install(FILES "${VERSION_FILE_PATH}/moveit/version.h" DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/moveit) diff --git a/moveit_core/README.md b/moveit_core/README.md index 2a54efedd4..5e602a457c 100644 --- a/moveit_core/README.md +++ b/moveit_core/README.md @@ -1,4 +1,4 @@ -# MoveIt! Core +# MoveIt Core This repository includes core libraries used by MoveIt: - representation of kinematic models diff --git a/moveit_core/background_processing/include/moveit/background_processing/background_processing.h b/moveit_core/background_processing/include/moveit/background_processing/background_processing.h index 30cd7380ae..270be22622 100644 --- a/moveit_core/background_processing/include/moveit/background_processing/background_processing.h +++ b/moveit_core/background_processing/include/moveit/background_processing/background_processing.h @@ -47,7 +47,7 @@ namespace moveit { /** \brief This namespace includes classes and functions that are - helpful in the implementation of other MoveIt! components. This is + helpful in the implementation of other MoveIt components. This is not code specific to the functionality provided by MoveIt. */ namespace tools { diff --git a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h index da193cb0c2..2a32ff04a8 100644 --- a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h +++ b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h @@ -42,7 +42,7 @@ #include #include -/// Namespace for the base class of a MoveIt! controller manager +/// Namespace for the base class of a MoveIt controller manager namespace moveit_controller_manager { /// The reported execution status @@ -101,7 +101,7 @@ struct ExecutionStatus MOVEIT_CLASS_FORWARD(MoveItControllerHandle); -/** \brief MoveIt! sends commands to a controller via a handle that satisfies this interface. */ +/** \brief MoveIt sends commands to a controller via a handle that satisfies this interface. */ class MoveItControllerHandle { public: @@ -149,7 +149,7 @@ class MoveItControllerHandle MOVEIT_CLASS_FORWARD(MoveItControllerManager); -/** @brief MoveIt! does not enforce how controllers are implemented. +/** @brief MoveIt does not enforce how controllers are implemented. To make your controllers usable by MoveIt, this interface needs to be implemented. The main purpose of this interface is to expose the set of known controllers and potentially to allow activating and deactivating them, if multiple controllers are available. @@ -157,7 +157,7 @@ MOVEIT_CLASS_FORWARD(MoveItControllerManager); class MoveItControllerManager { public: - /** \brief Each controller known to MoveIt! has a state. This + /** \brief Each controller known to MoveIt has a state. This structure describes that controller's state. */ struct ControllerState { @@ -165,12 +165,12 @@ class MoveItControllerManager { } - /** \brief A controller can be active or inactive. This means that MoveIt! could activate the controller when + /** \brief A controller can be active or inactive. This means that MoveIt could activate the controller when needed, and de-activate controllers that overlap (control the same set of joints) */ bool active_; /** \brief It is often the case that multiple controllers could be used to execute a motion. Marking a controller as - default makes MoveIt! prefer this controller when multiple options are available. */ + default makes MoveIt prefer this controller when multiple options are available. */ bool default_; }; diff --git a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h index 2dd4f93464..7918eef07d 100644 --- a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h +++ b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h @@ -39,7 +39,7 @@ #include -/** \brief Main namespace for MoveIt! */ +/** \brief Main namespace for MoveIt */ namespace moveit { /** \brief This may be thrown during construction of objects if errors occur */ diff --git a/moveit_core/kinematic_constraints/dox/constraint_representation.dox b/moveit_core/kinematic_constraints/dox/constraint_representation.dox index 886a3100e9..d786835f0c 100644 --- a/moveit_core/kinematic_constraints/dox/constraint_representation.dox +++ b/moveit_core/kinematic_constraints/dox/constraint_representation.dox @@ -1,7 +1,7 @@ /** \page constraint_representation Representation and Evaluation of Constraints -Constraints are integral component of MoveIt! and they are used both to constrain robot motion as well as to define planning goals. There following set of constraints are defined in the kinematic_constraints namespace: +Constraints are integral component of MoveIt and they are used both to constrain robot motion as well as to define planning goals. There following set of constraints are defined in the kinematic_constraints namespace: - kinematic_constraints::JointConstraint - kinematic_constraints::OrientationConstraint - kinematic_constraints::PositionConstraint diff --git a/moveit_core/package.xml b/moveit_core/package.xml index b12b48f6f3..121f19f3b3 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -1,7 +1,7 @@ moveit_core 1.0.1 - Core libraries used by MoveIt! + Core libraries used by MoveIt Ioan Sucan Sachin Chitta Acorn Pooley @@ -9,7 +9,7 @@ Dave Coleman Michael Görner Michael Ferguson - MoveIt! Release Team + MoveIt Release Team BSD http://moveit.ros.org diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index 68214f4a92..93748f43ef 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -48,7 +48,7 @@ namespace planning_scene MOVEIT_CLASS_FORWARD(PlanningScene); } -/** \brief This namespace includes the base class for MoveIt! planners */ +/** \brief This namespace includes the base class for MoveIt planners */ namespace planning_interface { /** @@ -147,7 +147,7 @@ class PlanningContext MOVEIT_CLASS_FORWARD(PlannerManager); -/** \brief Base class for a MoveIt! planner */ +/** \brief Base class for a MoveIt planner */ class PlannerManager { public: diff --git a/moveit_core/planning_scene/dox/planning_scene.dox b/moveit_core/planning_scene/dox/planning_scene.dox index 447b8f52b7..8bd31301b4 100644 --- a/moveit_core/planning_scene/dox/planning_scene.dox +++ b/moveit_core/planning_scene/dox/planning_scene.dox @@ -2,7 +2,7 @@ \page planning_scene_overview Planning Scene -The planning scene (planning_scene::PlanningScene) is the central class for motion planning in MoveIt!. A planning scene represents all the information needed to compute motion plans: The robot's current state, its representation (geometric, kinematic, dynamic) and the world representation. Using this information, things like forward kinematics, inverse kinematics, evaluation of constraints, collision checking, are all possible. +The planning scene (planning_scene::PlanningScene) is the central class for motion planning in MoveIt. A planning scene represents all the information needed to compute motion plans: The robot's current state, its representation (geometric, kinematic, dynamic) and the world representation. Using this information, things like forward kinematics, inverse kinematics, evaluation of constraints, collision checking, are all possible. The planning_scene::PlanningScene class is tightly connected to the planning_scene_monitor::PlannningSceneMonitor class, which maintains a planning scene using information from the ROS Parameter Server and subscription to topics. diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 2131a6f1a8..5e0b98061d 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -54,10 +54,10 @@ #include #include -/** \brief Main namespace for MoveIt! */ +/** \brief Main namespace for MoveIt */ namespace moveit { -/** \brief Core components of MoveIt! */ +/** \brief Core components of MoveIt */ namespace core { MOVEIT_CLASS_FORWARD(RobotModel); diff --git a/moveit_core/robot_state/include/moveit/robot_state/conversions.h b/moveit_core/robot_state/include/moveit/robot_state/conversions.h index dadd10adbc..655329acff 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/conversions.h +++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.h @@ -47,18 +47,18 @@ namespace moveit namespace core { /** - * @brief Convert a joint state to a MoveIt! robot state + * @brief Convert a joint state to a MoveIt robot state * @param joint_state The input joint state to be converted - * @param state The resultant MoveIt! robot state + * @param state The resultant MoveIt robot state * @return True if successful, false if failed for any reason */ bool jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state); /** - * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt! robot state + * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state * @param tf An instance of a transforms object * @param robot_state The input robot state msg - * @param state The resultant MoveIt! robot state + * @param state The resultant MoveIt robot state * @param copy_attached_bodies Flag to include attached objects in robot state copy * @return True if successful, false if failed for any reason */ @@ -66,9 +66,9 @@ bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::RobotSta bool copy_attached_bodies = true); /** - * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt! robot state + * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state * @param robot_state The input robot state msg - * @param state The resultant MoveIt! robot state + * @param state The resultant MoveIt robot state * @param copy_attached_bodies Flag to include attached objects in robot state copy * @return True if successful, false if failed for any reason */ @@ -76,8 +76,8 @@ bool robotStateMsgToRobotState(const moveit_msgs::RobotState& robot_state, Robot bool copy_attached_bodies = true); /** - * @brief Convert a MoveIt! robot state to a robot state message - * @param state The input MoveIt! robot state object + * @brief Convert a MoveIt robot state to a robot state message + * @param state The input MoveIt robot state object * @param robot_state The resultant RobotState *message * @param copy_attached_bodies Flag to include attached objects in robot state copy */ @@ -86,33 +86,33 @@ void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::RobotState& /** * @brief Convert AttachedBodies to AttachedCollisionObjects - * @param attached_bodies The input MoveIt! attached body objects + * @param attached_bodies The input MoveIt attached body objects * @param attached_collision_objs The resultant AttachedCollisionObject messages */ void attachedBodiesToAttachedCollisionObjectMsgs( const std::vector& attached_bodies, std::vector& attached_collision_objs); /** - * @brief Convert a MoveIt! robot state to a joint state message - * @param state The input MoveIt! robot state object + * @brief Convert a MoveIt robot state to a joint state message + * @param state The input MoveIt robot state object * @param robot_state The resultant JointState message */ void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState& joint_state); /** - * @brief Convert a joint trajectory point to a MoveIt! robot state + * @brief Convert a joint trajectory point to a MoveIt robot state * @param joint_trajectory The input msg * @param point_id The index of the trajectory point in the joint trajectory. - * @param state The resultant MoveIt! robot state + * @param state The resultant MoveIt robot state * @return True if successful, false if failed for any reason */ bool jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory& trajectory, std::size_t point_id, RobotState& state); /** - * @brief Convert a MoveIt! robot state to common separated values (CSV) on a single line that is + * @brief Convert a MoveIt robot state to common separated values (CSV) on a single line that is * outputted to a stream e.g. for file saving - * @param state - The input MoveIt! robot state object + * @param state - The input MoveIt robot state object * @param out - a file stream, or any other stream * @param include_header - flag to prefix the output with a line of joint names. * @param separator - allows to override the comma seperator with any symbol, such as a white space @@ -121,9 +121,9 @@ void robotStateToStream(const RobotState& state, std::ostream& out, bool include const std::string& separator = ","); /** - * @brief Convert a MoveIt! robot state to common separated values (CSV) on a single line that is + * @brief Convert a MoveIt robot state to common separated values (CSV) on a single line that is * outputted to a stream e.g. for file saving. This version can order by joint model groups - * @param state - The input MoveIt! robot state object + * @param state - The input MoveIt robot state object * @param out - a file stream, or any other stream * @param joint_group_ordering - output joints based on ordering of joint groups * @param include_header - flag to prefix the output with a line of joint names. @@ -135,7 +135,7 @@ void robotStateToStream(const RobotState& state, std::ostream& out, /** * \brief Convert a string of joint values from a file (CSV) or input source into a RobotState - * @param state - the output MoveIt! robot state object + * @param state - the output MoveIt robot state object * @param line - the input string of joint values * @param separator - allows to override the comma seperator with any symbol, such as a white space * \return true on success diff --git a/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h b/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h index 66c05d4036..a8f1a1615b 100644 --- a/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h +++ b/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h @@ -43,7 +43,7 @@ #include #include -/// Namespace for the base class of a MoveIt! sensor manager +/// Namespace for the base class of a MoveIt sensor manager namespace moveit_sensor_manager { /** \brief Define the frame of reference and the frustum of a sensor (usually this is a visual sensor) */ diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index b853319172..190fb10d0d 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -47,7 +47,7 @@ Transforms::Transforms(const std::string& target_frame) : target_frame_(target_f { boost::trim(target_frame_); if (target_frame_.empty()) - ROS_ERROR_NAMED("transforms", "The target frame for MoveIt! Transforms cannot be empty."); + ROS_ERROR_NAMED("transforms", "The target frame for MoveIt Transforms cannot be empty."); else { transforms_map_[target_frame_] = Eigen::Isometry3d::Identity(); diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h index fa9bbf989c..079cf49749 100644 --- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h @@ -14,7 +14,7 @@ * 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 MoveIt! nor the names of its + * * Neither the name of MoveIt nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index 9a083176a3..50221182ec 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -14,7 +14,7 @@ * 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 MoveIt! nor the names of its + * * Neither the name of MoveIt nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * diff --git a/moveit_experimental/README.md b/moveit_experimental/README.md index 2d250777f5..7ef8ed18c2 100644 --- a/moveit_experimental/README.md +++ b/moveit_experimental/README.md @@ -1,5 +1,5 @@ -# Experimental Packages for MoveIt! +# Experimental Packages for MoveIt -This repository includes possibly broken/unmaintained/unfinished libraries that are being worked on or have been worked on in the past. They are not part of MoveIt! officially. +This repository includes possibly broken/unmaintained/unfinished libraries that are being worked on or have been worked on in the past. They are not part of MoveIt officially. No support is offered for code in this repository, but you are welcome to use it if you see fit. diff --git a/moveit_experimental/jog_arm/config/sia5_simulated_config.yaml b/moveit_experimental/jog_arm/config/sia5_simulated_config.yaml index 0ce6b2614e..3a0a253ce0 100644 --- a/moveit_experimental/jog_arm/config/sia5_simulated_config.yaml +++ b/moveit_experimental/jog_arm/config/sia5_simulated_config.yaml @@ -14,7 +14,7 @@ move_group_name: arm # Often 'manipulator' or 'arm' lower_singularity_threshold: 15 # Start decelerating when the condition number hits this (close to singularity). Larger --> closer to singularity hard_stop_singularity_threshold: 30 # Stop when the condition number hits this. Larger --> closer to singularity collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] -planning_frame: base_link # The MoveIt! planning frame. Often 'base_link' +planning_frame: base_link # The MoveIt planning frame. Often 'base_link' low_pass_filter_coeff: 2. # Larger-> more smoothing of jog commands, but more lag. publish_period: 0.008 # 1/Nominal publish rate [seconds] publish_delay: 0.005 # delay between calculation and execution start of command diff --git a/moveit_experimental/jog_arm/src/jog_arm/collision_check_thread.cpp b/moveit_experimental/jog_arm/src/jog_arm/collision_check_thread.cpp index 47ecd0b10d..3b165a3e95 100644 --- a/moveit_experimental/jog_arm/src/jog_arm/collision_check_thread.cpp +++ b/moveit_experimental/jog_arm/src/jog_arm/collision_check_thread.cpp @@ -49,7 +49,7 @@ CollisionCheckThread::CollisionCheckThread(const jog_arm::JogArmParameters param // If user specified true in yaml file if (parameters.check_collisions) { - // MoveIt! Setup + // MoveIt Setup while (ros::ok() && !model_loader_ptr) { ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); diff --git a/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp b/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp index 6839481eff..2690865c67 100644 --- a/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp +++ b/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp @@ -49,7 +49,7 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari // Publish collision status warning_pub_ = nh_.advertise(parameters_.warning_topic, 1); - // MoveIt! Setup + // MoveIt Setup while (ros::ok() && !model_loader_ptr) { ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); diff --git a/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml b/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml index 15668db878..d9e3ff0db3 100644 --- a/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml +++ b/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml @@ -14,7 +14,7 @@ move_group_name: panda_arm # Often 'manipulator' or 'arm' lower_singularity_threshold: 30 # Start decelerating when the condition number hits this (close to singularity). Larger --> closer to singularity hard_stop_singularity_threshold: 45 # Stop when the condition number hits this. Larger --> closer to singularity collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] -planning_frame: base_link # The MoveIt! planning frame. Often 'base_link' +planning_frame: base_link # The MoveIt planning frame. Often 'base_link' low_pass_filter_coeff: 2. # Larger-> more smoothing to jog commands, but more lag. publish_period: 0.01 # 1/Nominal publish rate [seconds] publish_delay: 0.005 # delay between calculation and execution start of command diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/kinematics_cache_ros.cpp b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/kinematics_cache_ros.cpp index 40342b1d31..edc2f0cb64 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/kinematics_cache_ros.cpp +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/kinematics_cache_ros.cpp @@ -40,7 +40,7 @@ // ROS #include -// MoveIt! +// MoveIt #include #include #include diff --git a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h index 4e8ab4348d..2ec2f70299 100644 --- a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h +++ b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h @@ -51,7 +51,7 @@ // Plugin #include -// MoveIt! +// MoveIt #include #include #include diff --git a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h index b2f62e462b..6d2086a08a 100644 --- a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h +++ b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h @@ -44,7 +44,7 @@ // ROS msgs #include -// MoveIt! +// MoveIt #include #include #include diff --git a/moveit_experimental/package.xml b/moveit_experimental/package.xml index e34d9ee6dc..222652ff6a 100644 --- a/moveit_experimental/package.xml +++ b/moveit_experimental/package.xml @@ -1,8 +1,8 @@ moveit_experimental 1.0.1 - Experimental packages for MoveIt! - MoveIt! Release Team + Experimental packages for MoveIt + MoveIt Release Team BSD http://moveit.ros.org diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/README.md b/moveit_kinematics/cached_ik_kinematics_plugin/README.md index 571d53ab2c..77aa5697cd 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/README.md +++ b/moveit_kinematics/cached_ik_kinematics_plugin/README.md @@ -46,7 +46,7 @@ This illustrates two points: (1) if the IK solver is slow, significant improveme Below is a complete list of all arguments: -- `robot`: the name of the corresponding MoveIt! config package +- `robot`: the name of the corresponding MoveIt config package - `group`: the joint group to measure (by default performance is reported for all joint groups) - `tip`: the name of the end effector (by default the default end effectors are used) - `num`: the number of IK calls per joint group diff --git a/moveit_kinematics/ikfast_kinematics_plugin/README.md b/moveit_kinematics/ikfast_kinematics_plugin/README.md index 2f90c02007..c29d897011 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/README.md +++ b/moveit_kinematics/ikfast_kinematics_plugin/README.md @@ -1,9 +1,9 @@ -MoveIt! IKFast +MoveIt IKFast ========== * Authors: Dave Coleman, CU Boulder; Jeremy Zoss, SwRI; David Butterworth, KAIST, Mathias Lüdtke, Fraunhofer IPA * Date: 5/24/2013 -Generates a IKFast kinematics plugin for MoveIt! using [OpenRave](http://openrave.org/) generated cpp files. +Generates a IKFast kinematics plugin for MoveIt using [OpenRave](http://openrave.org/) generated cpp files. Tested on ROS Hydro and greater with Catkin using OpenRave 0.8 with a 6dof and 7dof robot arm manipulator. Does not work with greater than 7dof. diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py index ab5abaaead..e9ce8770d7 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py @@ -1,7 +1,7 @@ #! /usr/bin/env python from __future__ import print_function ''' -IKFast Plugin Generator for MoveIt! +IKFast Plugin Generator for MoveIt Creates a kinematics plugin using the output of IKFast from OpenRAVE. This plugin and the move_group node can be used as a general @@ -71,13 +71,13 @@ def get_pkg_dir(pkg_name): def create_parser(): parser = argparse.ArgumentParser( - description="Generate an IKFast MoveIt! kinematic plugin") + description="Generate an IKFast MoveIt kinematic plugin") parser.add_argument("robot_name", help="The name of your robot") parser.add_argument("planning_group_name", help="The name of the planning group for which your IKFast solution was generated") parser.add_argument("ikfast_plugin_pkg", - help="The name of the MoveIt! IKFast Kinematics Plugin to be created/updated") + help="The name of the MoveIt IKFast Kinematics Plugin to be created/updated") parser.add_argument("base_link_name", help="The name of the base link that was used when generating your IKFast solution") parser.add_argument("eef_link_name", diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index c76082e880..b2fd03982a 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -34,11 +34,11 @@ *********************************************************************/ /* - * IKFast Kinematics Solver Plugin for MoveIt! wrapping an ikfast solver from OpenRAVE. + * IKFast Kinematics Solver Plugin for MoveIt wrapping an ikfast solver from OpenRAVE. * * AUTO-GENERATED by create_ikfast_moveit_plugin.py in moveit_kinematics package. * - * This file, including the ikfast cpp from OpenRAVE below, forms a MoveIt! kinematics plugin. + * This file, including the ikfast cpp from OpenRAVE below, forms a MoveIt kinematics plugin. */ #include @@ -940,7 +940,7 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose if (!consistency_limits.empty()) { - // MoveIt! replaced consistency_limit (scalar) w/ consistency_limits (vector) + // MoveIt replaced consistency_limit (scalar) w/ consistency_limits (vector) // Assume [0]th free_params element for now. Probably wrong. double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]); double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]); diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h index 6f094a50e0..9c4a150045 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h @@ -53,7 +53,7 @@ #include #include -// MoveIt! +// MoveIt #include #include #include diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h index 996080593a..0ec6c0667d 100644 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h +++ b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h @@ -53,7 +53,7 @@ #include #include -// MoveIt! +// MoveIt #include #include #include diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 649b3ef22a..64ddccb0b9 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -2,7 +2,7 @@ moveit_kinematics 1.0.1 - Package for all inverse kinematics solvers in MoveIt! + Package for all inverse kinematics solvers in MoveIt Dave Coleman Ioan Sucan @@ -11,7 +11,7 @@ Dave Coleman G.A. vd. Hoorn Jorge Nicho - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h index b19d1e880a..50449662fe 100644 --- a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h +++ b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h @@ -33,7 +33,7 @@ *********************************************************************/ /* Author: Dave Coleman, Masaki Murooka - Desc: Connects MoveIt! to any inverse kinematics solver via a ROS service call + Desc: Connects MoveIt to any inverse kinematics solver via a ROS service call Supports planning groups with multiple tip frames \todo: better support for mimic joints \todo: better support for redundant joints @@ -55,7 +55,7 @@ #include #include -// MoveIt! +// MoveIt #include #include diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index 682a519909..1235bfd9f9 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -43,7 +43,7 @@ #include #include -// MoveIt! +// MoveIt #include #include #include diff --git a/moveit_planners/README.md b/moveit_planners/README.md index 0f0f685d07..5d35579eac 100644 --- a/moveit_planners/README.md +++ b/moveit_planners/README.md @@ -1,3 +1,3 @@ -# MoveIt! Planners +# MoveIt Planners Interfaces for the motion planning libraries used in MoveIt diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst index 3b2e6e9912..7184007695 100644 --- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst @@ -48,7 +48,7 @@ Changelog for package chomp_interface 0.10.1 (2018-05-25) ------------------- * [fix] dependencies for chomp interface test (`#778 `_) -* [maintenance] MoveIt! tf2 migration (`#830 `_) +* [maintenance] MoveIt tf2 migration (`#830 `_) * Contributors: Bence Magyar, Dave Coleman, Ian McMahon, Mikael Arguedas, Robert Haschke, Stephan, Will Baker 0.9.11 (2017-12-25) diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml index e7f232010e..bc27808754 100644 --- a/moveit_planners/chomp/chomp_interface/package.xml +++ b/moveit_planners/chomp/chomp_interface/package.xml @@ -1,13 +1,13 @@ moveit_planners_chomp - The interface for using CHOMP within MoveIt! + The interface for using CHOMP within MoveIt 1.0.1 Gil Jones Chittaranjan Srinivas Swaminathan Dave Coleman - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_planners/chomp/chomp_interface/test/rrbot_chomp_planning_pipeline.launch.xml b/moveit_planners/chomp/chomp_interface/test/rrbot_chomp_planning_pipeline.launch.xml index b3146c917b..4b3a1f6f54 100644 --- a/moveit_planners/chomp/chomp_interface/test/rrbot_chomp_planning_pipeline.launch.xml +++ b/moveit_planners/chomp/chomp_interface/test/rrbot_chomp_planning_pipeline.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst index 0dc05a2151..d32082addf 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst @@ -50,7 +50,7 @@ Changelog for package chomp_motion_planner 0.10.1 (2018-05-25) ------------------- * [fix] for chomp fixed base joint bug (`#870 `_) -* [maintenance] MoveIt! tf2 migration (`#830 `_) +* [maintenance] MoveIt tf2 migration (`#830 `_) * [maintenance] switch to ROS_LOGGER from CONSOLE_BRIDGE (`#874 `_) * Contributors: Bence Magyar, Dave Coleman, Ian McMahon, Mike Lautman, Xiaojian Ma diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml index 8abb126185..81c05a1bf6 100644 --- a/moveit_planners/chomp/chomp_motion_planner/package.xml +++ b/moveit_planners/chomp/chomp_motion_planner/package.xml @@ -7,7 +7,7 @@ Mrinal Kalakrishnan Chittaranjan Srinivas Swaminathan - MoveIt! Release Team + MoveIt Release Team BSD http://ros.org/wiki/chomp_motion_planner diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml index f5a6d016a7..4861c4e43c 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml +++ b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml @@ -6,7 +6,7 @@ 1.0.1 Raghavender Sahdev Raghavender Sahdev - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index 77d65918bf..a3ef9a50aa 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -6,7 +6,7 @@ Sachin Chitta Dave Coleman - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst index 06a4259f1c..37a663ac0b 100644 --- a/moveit_planners/ompl/CHANGELOG.rst +++ b/moveit_planners/ompl/CHANGELOG.rst @@ -117,7 +117,7 @@ Changelog for package moveit_planners_ompl * renamed newGoal to new_goal for keeping with formatting * setting GroupStateValidityCallbackFn member for constraint_sampler member and implementing callbacks for state validity checking * added functions to check validit of state, and also to act as callback for constraint sampler -* Added copy function from MoveIt! robot_state joint values to ompl state +* Added copy function from MoveIt robot_state joint values to ompl state * fix for demo constraints database linking error * Namespaced less useful debug output to allow to be easily silenced using ros console * Contributors: Dave Coleman, Dave Hershberger, Sachin Chitta, arjungm diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h index ef25ce8452..317272df62 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h @@ -48,7 +48,7 @@ #include #include -/** \brief The MoveIt! interface to OMPL */ +/** \brief The MoveIt interface to OMPL */ namespace ompl_interface { /** @class OMPLInterface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h index 3ef5246888..1bfa1379e6 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h @@ -241,10 +241,10 @@ class ModelBasedStateSpace : public ompl::base::StateSpace virtual void copyToOMPLState(ompl::base::State* state, const robot_state::RobotState& rstate) const; /** - * \brief Copy a single joint's values (which might have multiple variables) from a MoveIt! robot_state to an OMPL + * \brief Copy a single joint's values (which might have multiple variables) from a MoveIt robot_state to an OMPL * state. * \param state - output OMPL state with single joint modified - * \param robot_state - input MoveIt! state to get the joint value from + * \param robot_state - input MoveIt state to get the joint value from * \param joint_model - the joint to copy values of * \param ompl_state_joint_index - the index of the joint in the ompl state (passed in for efficiency, you should * cache this index) diff --git a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp index 5d47880ea6..5deb25fce8 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp @@ -119,7 +119,7 @@ bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base: return false; } - // convert ompl state to MoveIt! robot state + // convert ompl state to MoveIt robot state robot_state::RobotState* robot_state = tss_.getStateStorage(); planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index 1f2f7e6fc0..f7c87e00f2 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -1,12 +1,12 @@ moveit_planners_ompl 1.0.1 - MoveIt! interface to OMPL + MoveIt interface to OMPL Ioan Sucan Dave Coleman - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_plugins/README.md b/moveit_plugins/README.md index 62065de1f1..b6400b3a55 100644 --- a/moveit_plugins/README.md +++ b/moveit_plugins/README.md @@ -1,4 +1,4 @@ -# MoveIt! Plugins +# MoveIt Plugins This is a collection of plugins for MoveIt: - moveit_simple_controller_manager - A very basic controller manager plugin. Can connect to FollowJointTrajectoryAction and GripperCommandAction servers. diff --git a/moveit_plugins/moveit_fake_controller_manager/README.md b/moveit_plugins/moveit_fake_controller_manager/README.md index 102a296a23..cacbba6f9a 100644 --- a/moveit_plugins/moveit_fake_controller_manager/README.md +++ b/moveit_plugins/moveit_fake_controller_manager/README.md @@ -1,6 +1,6 @@ -# MoveIt! Fake Controller Manager +# MoveIt Fake Controller Manager -This package implements a series of fake trajectory controllers for MoveIt! -- to be used in simulation. +This package implements a series of fake trajectory controllers for MoveIt -- to be used in simulation. For example, the `demo.launch` generated by MoveIt's `setup_assistant`, employs fake controllers for nice visualization in `rviz`. For configuration, edit the file `config/fake_controllers.yaml`, and adjust the desired controller type. diff --git a/moveit_plugins/moveit_fake_controller_manager/package.xml b/moveit_plugins/moveit_fake_controller_manager/package.xml index 05c2662a48..d87a4a53bb 100644 --- a/moveit_plugins/moveit_fake_controller_manager/package.xml +++ b/moveit_plugins/moveit_fake_controller_manager/package.xml @@ -6,7 +6,7 @@ Ioan Sucan Michael Görner - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml index 265fa36725..c64dc69235 100644 --- a/moveit_plugins/moveit_plugins/package.xml +++ b/moveit_plugins/moveit_plugins/package.xml @@ -1,13 +1,13 @@ moveit_plugins 1.0.1 - Metapackage for MoveIt! plugins. + Metapackage for MoveIt plugins. Michael Ferguson Ioan Sucan Michael Görner - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_plugins/moveit_ros_control_interface/README.md b/moveit_plugins/moveit_ros_control_interface/README.md index d13be1b5a2..355006d366 100644 --- a/moveit_plugins/moveit_ros_control_interface/README.md +++ b/moveit_plugins/moveit_ros_control_interface/README.md @@ -1,4 +1,4 @@ -# MoveIt! ROS Control Plugin +# MoveIt ROS Control Plugin This package provides plugins of base class `moveit_controller_manager::MoveItControllerManager` and a new plugin base class for `moveit_controller_manager::MoveItControllerHandle` allocators. The allocator class is necessary because `moveit_controller_manager::MoveItControllerHandle` needs a name passed to the constructor. @@ -7,7 +7,7 @@ Two variantes are provided, `moveit_ros_control_interface::MoveItControllerManag ## moveit_ros_control_interface::MoveItControllerManager This plugin interfaces a single ros_control-driven node in the namespace given in the `~ros_control_namespace` ROS parameter. -It polls all controllers via the `list_controllers` service and passes their properties to MoveIt!. +It polls all controllers via the `list_controllers` service and passes their properties to MoveIt. The polling is throttled to 1 Hertz. ### Handle plugins @@ -17,7 +17,7 @@ These plugins should be registered with lookup names that match the correspondin Currently plugins for `position_controllers/JointTrajectoryController`, `velocity_controllers/JointTrajectoryController` and `effort_controllers/JointTrajectoryController` are available, which simply wrap `moveit_simple_controller_manager::FollowJointTrajectoryControllerHandle` instances. ### Setup -In your MoveIt! launch file (e.g. `ROBOT_moveit_config/launch/ROBOT_moveit_controller_manager.launch.xml`) set the `moveit_controller_manager` parameter: +In your MoveIt launch file (e.g. `ROBOT_moveit_config/launch/ROBOT_moveit_controller_manager.launch.xml`) set the `moveit_controller_manager` parameter: ``` ``` @@ -42,8 +42,8 @@ controller_list: ``` ### Controller switching -MoveIt! can decide which controllers have to be started and stopped. -Since only controller names with registered allocator plugins are handed over to MoveIt!, this implementation takes care of stopping other conflicting controllers based on their claimed resources and the resources for the to-be-started controllers. +MoveIt can decide which controllers have to be started and stopped. +Since only controller names with registered allocator plugins are handed over to MoveIt, this implementation takes care of stopping other conflicting controllers based on their claimed resources and the resources for the to-be-started controllers. ### Namespaces All controller names get prefixed by the namespace of the ros_control node. @@ -56,7 +56,7 @@ It spawns `moveit_ros_control_interface::MoveItControllerManager` instances with ### Setup -Just set the `moveit_controller_manager` parameter in your MoveIt! launch file (e.g. `ROBOT_moveit_config/launch/ROBOT_moveit_controller_manager.launch.xml`) +Just set the `moveit_controller_manager` parameter in your MoveIt launch file (e.g. `ROBOT_moveit_config/launch/ROBOT_moveit_controller_manager.launch.xml`) ``` ``` diff --git a/moveit_plugins/moveit_ros_control_interface/moveit_core_plugins.xml b/moveit_plugins/moveit_ros_control_interface/moveit_core_plugins.xml index f7461f59d5..3bd7e971cc 100644 --- a/moveit_plugins/moveit_ros_control_interface/moveit_core_plugins.xml +++ b/moveit_plugins/moveit_ros_control_interface/moveit_core_plugins.xml @@ -1,10 +1,10 @@ - ros_control controller manager interface for MoveIt! + ros_control controller manager interface for MoveIt - multiple ros_control controller managers interface for MoveIt! + multiple ros_control controller managers interface for MoveIt diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml index 98d0206d4b..92bd877cdc 100644 --- a/moveit_plugins/moveit_ros_control_interface/package.xml +++ b/moveit_plugins/moveit_ros_control_interface/package.xml @@ -2,12 +2,12 @@ moveit_ros_control_interface 1.0.1 - ros_control controller manager interface for MoveIt! + ros_control controller manager interface for MoveIt Mathias Lüdtke Mathias Lüdtke - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h index 8574dee376..e965d1b446 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h @@ -102,7 +102,7 @@ class GripperControllerHandle : public ActionBasedControllerHandleMichael Ferguson Michael Görner - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp index 2f15a4189a..e9921962db 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp @@ -145,7 +145,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo continue; } - /* add list of joints, used by controller manager and MoveIt! */ + /* add list of joints, used by controller manager and MoveIt */ for (int j = 0; j < controller_list[i]["joints"].size(); ++j) new_handle->addJoint(std::string(controller_list[i]["joints"][j])); diff --git a/moveit_ros/README.md b/moveit_ros/README.md index ba995b41ef..5755639551 100644 --- a/moveit_ros/README.md +++ b/moveit_ros/README.md @@ -1,6 +1,6 @@ -#MoveIt! ROS +#MoveIt ROS -This repository includes components of MoveIt! that use ROS. This is where much of the functionality MoveIt! provides is put together. Libraries from MoveIt! Core and various plugins are used to provide that functionality. +This repository includes components of MoveIt that use ROS. This is where much of the functionality MoveIt provides is put together. Libraries from MoveIt Core and various plugins are used to provide that functionality. - planning - planning interfaces - benchmarking diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst index 98e4914f29..0bfdc8ef5a 100644 --- a/moveit_ros/benchmarks/CHANGELOG.rst +++ b/moveit_ros/benchmarks/CHANGELOG.rst @@ -155,13 +155,13 @@ Changelog for package moveit_ros_benchmarks as a symlink pointing to the versioned file. Because this sets each library's SONAME to the *full version*, this enforces that *every* binary links with the versioned library file from now on and - has to be relinked with *each* new release of MoveIt!. + has to be relinked with *each* new release of MoveIt. The alternative would be to set the SONAME to `$MAJOR.$MINOR` and ignore the patch version, but because we currently stay with one `$MAJOR.$MINOR` number within each ROS distribution, we had (and likely will have) ABI changes in the `$PATCH` version releases too. The reason for this commit is that it is practically impossible to maintain full ABI compatibility within each ROS distribution and still add the the features/patches the community asks for. - This has resulted in more than one ABI-incompatible MoveIt! release in the recent past + This has resulted in more than one ABI-incompatible MoveIt release in the recent past within a ROS distribution. Because the libraries have not been versioned up to now, there was no way to indicate the incompatible changes and users who did not rebuild their whole workspace with the new release encountered weird and hard-to-track segfaults diff --git a/moveit_ros/benchmarks/README.md b/moveit_ros/benchmarks/README.md index 775e391586..ada7c8a485 100644 --- a/moveit_ros/benchmarks/README.md +++ b/moveit_ros/benchmarks/README.md @@ -1,4 +1,4 @@ -# MoveIt! ROS Benchmarks +# MoveIt ROS Benchmarks This package provides methods to benchmark motion planning algorithms and aggregate/plot statistics. Results can be viewed in [Planner Arena](http://plannerarena.org/). diff --git a/moveit_ros/benchmarks/examples/demo1.yaml b/moveit_ros/benchmarks/examples/demo1.yaml index 92227c3814..b930a8187a 100644 --- a/moveit_ros/benchmarks/examples/demo1.yaml +++ b/moveit_ros/benchmarks/examples/demo1.yaml @@ -1,5 +1,5 @@ # This is an example configuration that loads the "Kitchen" scene from the -# local MoveIt! warehouse and benchmarks the "manipulator" group in the Pick1 +# local MoveIt warehouse and benchmarks the "manipulator" group in the Pick1 # query using the Start1 initial state (all pre-stored in the local warehouse) # Three different planners from OMPL are run a total of 50 times each, with a diff --git a/moveit_ros/benchmarks/examples/demo2.yaml b/moveit_ros/benchmarks/examples/demo2.yaml index 9151abf075..ba6048b01a 100644 --- a/moveit_ros/benchmarks/examples/demo2.yaml +++ b/moveit_ros/benchmarks/examples/demo2.yaml @@ -1,5 +1,5 @@ # This is an example configuration that loads the "Kitchen" scene from the -# local MoveIt! warehouse and benchmarks the "manipulator" group over all pairs +# local MoveIt warehouse and benchmarks the "manipulator" group over all pairs # of motion plan queries and start states in the Kitchen scene. # Five planners from two different plugins are run a total of 50 times each, with a diff --git a/moveit_ros/benchmarks/examples/demo_obstacles.yaml b/moveit_ros/benchmarks/examples/demo_obstacles.yaml index 70c7604e03..8616b78596 100644 --- a/moveit_ros/benchmarks/examples/demo_obstacles.yaml +++ b/moveit_ros/benchmarks/examples/demo_obstacles.yaml @@ -1,5 +1,5 @@ # This is an example configuration that loads the "Kitchen" scene from the -# local MoveIt! warehouse and benchmarks the "manipulator" group over all pairs +# local MoveIt warehouse and benchmarks the "manipulator" group over all pairs # of motion plan queries and start states in the Kitchen scene. # Five planners from two different plugins are run a total of 50 times each, with a diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml b/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml index 00ac5066e6..aaa20a017d 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml @@ -1,5 +1,5 @@ # This is an example configuration that loads the "Kitchen" scene from the -# local MoveIt! warehouse and benchmarks the "manipulator" group over all pairs +# local MoveIt warehouse and benchmarks the "manipulator" group over all pairs # of motion plan queries and start states in the Kitchen scene. # Five planners from two different plugins are run a total of 50 times each, with a diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index cbb64dbe81..acc1789072 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -2,12 +2,12 @@ moveit_ros_benchmarks 1.0.1 - Enhanced tools for benchmarks in MoveIt! + Enhanced tools for benchmarks in MoveIt Ryan Luna Dave Coleman - MoveIt! Release Team + MoveIt Release Team http://moveit.ros.org https://github.com/ros-planning/moveit/issues diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index ede31252e9..b48dd98a95 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -1063,7 +1063,7 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& brequest, const std: return; } - out << "MoveIt! version " << MOVEIT_VERSION << std::endl; + out << "MoveIt version " << MOVEIT_VERSION << std::endl; out << "Experiment " << brequest.name << std::endl; out << "Running on " << hostname << std::endl; out << "Starting at " << start_time << std::endl; diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml index d8168e7329..6b1c82dddc 100644 --- a/moveit_ros/manipulation/package.xml +++ b/moveit_ros/manipulation/package.xml @@ -1,14 +1,14 @@ moveit_ros_manipulation 1.0.1 - Components of MoveIt! used for manipulation + Components of MoveIt used for manipulation Ioan Sucan Sachin Chitta Michael Görner Michael Ferguson - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index ffed75a12d..9c1130b9f1 100644 --- a/moveit_ros/move_group/package.xml +++ b/moveit_ros/move_group/package.xml @@ -7,7 +7,7 @@ Michael Ferguson Michael Görner - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/move_group/test/test_cancel_before_plan_execution.test b/moveit_ros/move_group/test/test_cancel_before_plan_execution.test index 51977ca919..edf4699d93 100644 --- a/moveit_ros/move_group/test/test_cancel_before_plan_execution.test +++ b/moveit_ros/move_group/test/test_cancel_before_plan_execution.test @@ -13,7 +13,7 @@ - + diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst index d3a8959214..be0815f44c 100644 --- a/moveit_ros/moveit_ros/CHANGELOG.rst +++ b/moveit_ros/moveit_ros/CHANGELOG.rst @@ -30,7 +30,7 @@ Changelog for package moveit_ros 0.10.2 (2018-10-24) ------------------- -* Fix references to MoveIt! (`#1020 `_) +* Fix references to MoveIt (`#1020 `_) * Contributors: Mohmmad Ayman 0.10.1 (2018-05-25) diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index a64adef04d..60671d1f8f 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -1,7 +1,7 @@ moveit_ros 1.0.1 - Components of MoveIt! that use ROS + Components of MoveIt that use ROS Ioan Sucan Sachin Chitta Acorn Pooley @@ -9,7 +9,7 @@ Michael Ferguson Michael Görner Dave Coleman - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml index 8c1e95689d..28c302748d 100644 --- a/moveit_ros/occupancy_map_monitor/package.xml +++ b/moveit_ros/occupancy_map_monitor/package.xml @@ -2,14 +2,14 @@ moveit_ros_occupancy_map_monitor 1.0.1 - Components of MoveIt! connecting to occupancy map + Components of MoveIt connecting to occupancy map Ioan Sucan Jon Binney Suat Gedikli Michael Ferguson - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml index 61033ec542..0d3a038102 100644 --- a/moveit_ros/perception/package.xml +++ b/moveit_ros/perception/package.xml @@ -1,14 +1,14 @@ moveit_ros_perception 1.0.1 - Components of MoveIt! connecting to perception + Components of MoveIt connecting to perception Ioan Sucan Jon Binney Suat Gedikli Michael Ferguson - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp index aa6d99403d..5407d287b7 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -38,7 +38,7 @@ #include #include -// MoveIt! +// MoveIt #include #include #include diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index ee9e4f923e..c5f426ebe1 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -2,13 +2,13 @@ moveit_ros_planning 1.0.1 - Planning components of MoveIt! that use ROS + Planning components of MoveIt that use ROS Ioan Sucan Sachin Chitta Michael Ferguson Michael Görner - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp index 6c62e6d411..8095e07b78 100644 --- a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp @@ -248,7 +248,7 @@ bool plan_execution::PlanWithSensing::lookAt(const std::set -// MoveIt! +// MoveIt #include static const std::string ROBOT_DESCRIPTION = "robot_description"; diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp index eb655c1710..11a37c9329 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan, Mathias Lüdtke, Dave Coleman */ -// MoveIt! +// MoveIt #include #include diff --git a/moveit_ros/planning/trajectory_execution_manager/dox/trajectory_execution.dox b/moveit_ros/planning/trajectory_execution_manager/dox/trajectory_execution.dox index bb4b9068a4..ab7a5e683e 100644 --- a/moveit_ros/planning/trajectory_execution_manager/dox/trajectory_execution.dox +++ b/moveit_ros/planning/trajectory_execution_manager/dox/trajectory_execution.dox @@ -1,11 +1,11 @@ /** \page trajectory_execution Trajectory Execution Manager -@b MoveIt! includes a library for managing controllers and the execution of trajectories. This code exists in the trajectory_execution_manager namespace. +@b MoveIt includes a library for managing controllers and the execution of trajectories. This code exists in the trajectory_execution_manager namespace. The trajectory_execution_manager::TrajectoryExecutionManager class allows two main operations: - trajectory_execution_manager::TrajectoryExecutionManager::push() adds trajectories specified as a moveit_msgs::RobotTrajectory message type to a queue of trajectories to be executed in sequence. Each trajectory can be specified for any set of joints in the robot. Because controllers may only be available for certain groups of joints, this function may decide to split one trajectory into multiple ones and pass them to corresponding controllers (this time in parallel, using the same time stamp for the trajectory points). This approach assumes that controllers respect the time stamps specified for the waypoints. - trajectory_execution_manager::TrajectoryExecutionManager::execute() passes the appropriate trajectories to different controllers, monitors execution, optionally waits for completion of the execution and, very importantly, switches active controllers as needed (optionally) to be able to execute the specified trajectories. -The functionality of the trajectory execution in MoveIt! usually needs robot-specific interaction with controllers. For this reason, the concept of a controller manager specific to MoveIt! (moveit_controller_manager::MoveItControllerManager) was defined. This is an abstract class that defines the functionality needed by trajectory_execution_manager::TrajectoryExecutionManager and needs to be implemented for each robot type. Often, the implementation of these plugins are quite similar and it is easy to modify existing code to achieve the desired functionality (see for example pr2_moveit_controller_manager::Pr2MoveItControllerManager). +The functionality of the trajectory execution in MoveIt usually needs robot-specific interaction with controllers. For this reason, the concept of a controller manager specific to MoveIt (moveit_controller_manager::MoveItControllerManager) was defined. This is an abstract class that defines the functionality needed by trajectory_execution_manager::TrajectoryExecutionManager and needs to be implemented for each robot type. Often, the implementation of these plugins are quite similar and it is easy to modify existing code to achieve the desired functionality (see for example pr2_moveit_controller_manager::Pr2MoveItControllerManager). */ \ No newline at end of file diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 727b626fbe..1f919b0960 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -58,7 +58,7 @@ namespace moveit { -/** \brief Simple interface to MoveIt! components */ +/** \brief Simple interface to MoveIt components */ namespace planning_interface { class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index 3690783e3f..5c57d296b1 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -1,13 +1,13 @@ moveit_ros_planning_interface 1.0.1 - Components of MoveIt! that offer simpler interfaces to planning and execution + Components of MoveIt that offer simpler interfaces to planning and execution Ioan Sucan Michael Ferguson Michael Görner - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h index 3796d64760..e1e54a6bc9 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h @@ -42,7 +42,7 @@ namespace moveit { -/** \brief Tools for creating python bindings for MoveIt! */ +/** \brief Tools for creating python bindings for MoveIt */ namespace py_bindings_tools { /** \brief The constructor of this class ensures that ros::init() has diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst index 93a596b313..9851f4f460 100644 --- a/moveit_ros/robot_interaction/CHANGELOG.rst +++ b/moveit_ros/robot_interaction/CHANGELOG.rst @@ -46,7 +46,7 @@ Changelog for package moveit_ros_robot_interaction 0.10.2 (2018-10-24) ------------------- -* [fix] Text refrences to MoveIt! (`#1020 `_) +* [fix] Text refrences to MoveIt (`#1020 `_) * Contributors: Mohmmad Ayman, Robert Haschke, mike lautman 0.10.1 (2018-05-25) diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml index 25cc5b7107..83116910cf 100644 --- a/moveit_ros/robot_interaction/package.xml +++ b/moveit_ros/robot_interaction/package.xml @@ -1,13 +1,13 @@ moveit_ros_robot_interaction 1.0.1 - Components of MoveIt! that offer interaction via interactive markers + Components of MoveIt that offer interaction via interactive markers Ioan Sucan Michael Ferguson Robert Haschke - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp index f2773ed915..87c1321723 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp @@ -81,7 +81,7 @@ void MotionPlanningFrame::planningAlgorithmIndexChanged(int index) void MotionPlanningFrame::resetDbButtonClicked() { - if (QMessageBox::warning(this, "Data about to be deleted", "The following dialog will allow you to drop a MoveIt! " + if (QMessageBox::warning(this, "Data about to be deleted", "The following dialog will allow you to drop a MoveIt " "Warehouse database. Are you sure you want to continue?", QMessageBox::Yes | QMessageBox::No) == QMessageBox::No) return; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index b0debfae37..0f6825b916 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -11,7 +11,7 @@ - MoveIt! Planning Frame + MoveIt Planning Frame diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index 7b93f92cb1..a0f4183906 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -1,7 +1,7 @@ moveit_ros_visualization 1.0.1 - Components of MoveIt! that offer visualization + Components of MoveIt that offer visualization Ioan Sucan Dave Coleman @@ -9,7 +9,7 @@ Jon Binney Michael Ferguson - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py index 0ad0a176d6..3868b0e3ad 100644 --- a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py +++ b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py @@ -35,7 +35,7 @@ # ********************************************************************/ # Author: Ryohei Ueda, Dave Coleman -# Desc: Interface between PS3/XBox controller and MoveIt! Motion Planning Rviz Plugin +# Desc: Interface between PS3/XBox controller and MoveIt Motion Planning Rviz Plugin from __future__ import print_function diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst index e3d3c6dd37..ee636c4c9d 100644 --- a/moveit_ros/warehouse/CHANGELOG.rst +++ b/moveit_ros/warehouse/CHANGELOG.rst @@ -32,7 +32,7 @@ Changelog for package moveit_ros_warehouse 0.10.2 (2018-10-24) ------------------- -* [fix] Text refrences to MoveIt! (`#1020 `_) +* [fix] Text refrences to MoveIt (`#1020 `_) * [enhancement] warehouse: added params for timeout + #retries (`#1008 `_) * [maintenance] various compiler warnings (`#1038 `_) * Contributors: Mohmmad Ayman, Robert Haschke, dg-shadow, mike lautman diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml index 976cdd1c79..14512ec42f 100644 --- a/moveit_ros/warehouse/package.xml +++ b/moveit_ros/warehouse/package.xml @@ -1,12 +1,12 @@ moveit_ros_warehouse 1.0.1 - Components of MoveIt! connecting to MongoDB + Components of MoveIt connecting to MongoDB Ioan Sucan Michael Ferguson - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp b/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp index 28cc32568f..595cae59c1 100644 --- a/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp +++ b/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp @@ -132,7 +132,7 @@ int main(int argc, char** argv) cs.addConstraints(cmsg, psm.getRobotModel()->getName(), jmg->getName()); ROS_INFO("Added default constraint: '%s'", cmsg.name.c_str()); } - ROS_INFO("Default MoveIt! Warehouse was reset."); + ROS_INFO("Default MoveIt Warehouse was reset."); ros::shutdown(); diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml index 8efd4a159f..f331186799 100644 --- a/moveit_runtime/package.xml +++ b/moveit_runtime/package.xml @@ -2,7 +2,7 @@ moveit_runtime 1.0.1 - moveit_runtime meta package contains MoveIt! packages that are essential for its runtime (e.g. running MoveIt! on robots). + moveit_runtime meta package contains MoveIt packages that are essential for its runtime (e.g. running MoveIt on robots). Isaac I. Y. Saito Isaac I. Y. Saito diff --git a/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/CHANGELOG.rst index a117cec6fa..dba4173b1f 100644 --- a/moveit_setup_assistant/CHANGELOG.rst +++ b/moveit_setup_assistant/CHANGELOG.rst @@ -51,7 +51,7 @@ Changelog for package moveit_setup_assistant * [capability][chomp] Failure recovery options for CHOMP by tweaking parameters (`#987 `_) * [capability] New screen for automatically generating interfaces to low level controllers(`#951 `_, `#994 `_, `#908 `_) * [capability] Perception screen for using laser scanner point clouds. (`#969 `_) -* [enhancement][GUI] Logo for MoveIt! 2.0, cleanup appearance (`#1059 `_) +* [enhancement][GUI] Logo for MoveIt 2.0, cleanup appearance (`#1059 `_) * [enhancement][GUI] added a setup assistant window icon (`#1028 `_) * [enhancement][GUI] Planning Groups screen (`#1017 `_) * [enhancement] use panda for test, and write test file in tmp dir (`#1042 `_) diff --git a/moveit_setup_assistant/README.md b/moveit_setup_assistant/README.md index 07feef853a..f346ead9ae 100644 --- a/moveit_setup_assistant/README.md +++ b/moveit_setup_assistant/README.md @@ -1 +1 @@ -# MoveIt! Setup Assistant +# MoveIt Setup Assistant diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 788cfa0111..59b598bc45 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -175,7 +175,7 @@ class GenericParameter MOVEIT_CLASS_FORWARD(MoveItConfigData); /** \brief This class is shared with all widgets and contains the common configuration data - needed for generating each robot's MoveIt! configuration package. + needed for generating each robot's MoveIt configuration package. All SRDF data is contained in a subclass of this class - srdf_writer.cpp. This class also contains the functions for writing @@ -203,7 +203,7 @@ class MoveItConfigData }; unsigned long changes; // bitfield of changes (composed of InformationFields) - // All of the data needed for creating a MoveIt! Configuration Files + // All of the data needed for creating a MoveIt Configuration Files // ****************************************************************************************** // URDF Data @@ -411,7 +411,7 @@ class MoveItConfigData bool createFullSRDFPath(const std::string& package_path); /** - * Input .setup_assistant file - contains data used for the MoveIt! Setup Assistant + * Input .setup_assistant file - contains data used for the MoveIt Setup Assistant * * @param file_path path to .setup_assistant file * @return true if the file was read correctly diff --git a/moveit_setup_assistant/launch/setup_assistant.launch b/moveit_setup_assistant/launch/setup_assistant.launch index 4d908ba325..852c19c4ce 100644 --- a/moveit_setup_assistant/launch/setup_assistant.launch +++ b/moveit_setup_assistant/launch/setup_assistant.launch @@ -1,4 +1,4 @@ - + diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml index 6db06b5a51..99c93b56e0 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/package.xml @@ -2,13 +2,13 @@ moveit_setup_assistant 1.0.1 - Generates a configuration package that makes it easy to use MoveIt! + Generates a configuration package that makes it easy to use MoveIt Dave Coleman Dave Coleman Robert Haschke - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_setup_assistant/resources/source/MoveIt_Setup_Asst_Sm.xcf b/moveit_setup_assistant/resources/source/MoveIt_Setup_Asst_Sm.xcf index a3935559fdfbbb8d36e572704d1b2a534e02501e..a468ef57307a08794ec3366349620ec59989c688 100644 GIT binary patch delta 60 zcmbR8MQp+su? diff --git a/moveit_experimental/jog_arm/test/launch/jog_arm_integration_test.test b/moveit_experimental/jog_arm/test/launch/jog_arm_integration_test.test index f9c48abc8c..8845fd0815 100644 --- a/moveit_experimental/jog_arm/test/launch/jog_arm_integration_test.test +++ b/moveit_experimental/jog_arm/test/launch/jog_arm_integration_test.test @@ -10,7 +10,7 @@ These aren't suitable for actually jogging a robot in RViz, but they are good enough to test jog node output. --> - + @@ -19,20 +19,20 @@ - + - + - + - + - + diff --git a/moveit_experimental/kinematics_cache/CATKIN_IGNORE b/moveit_experimental/kinematics_cache/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_experimental/kinematics_constraint_aware/CATKIN_IGNORE b/moveit_experimental/kinematics_constraint_aware/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_experimental/package.xml b/moveit_experimental/package.xml deleted file mode 100644 index 222652ff6a..0000000000 --- a/moveit_experimental/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - moveit_experimental - 1.0.1 - Experimental packages for MoveIt - MoveIt Release Team - - BSD - http://moveit.ros.org - - catkin - - control_msgs - geometry_msgs - joy_teleop - moveit_ros_planning_interface - rosparam_shortcuts - spacenav_node - tf2_ros - - ros_pytest - rostest - - - - - From ae3d522ceb2059b9fd9163be3dbfba5f9a46003e Mon Sep 17 00:00:00 2001 From: Jonathan Binney Date: Tue, 13 Aug 2019 00:44:54 -0700 Subject: [PATCH 024/612] Switch from include guards to pragma once (#1615) --- .../moveit/background_processing/background_processing.h | 5 +---- moveit_core/backtrace/include/moveit/backtrace/backtrace.h | 5 +---- .../allvalid/collision_detector_allocator_allvalid.h | 5 +---- .../collision_detection/allvalid/collision_robot_allvalid.h | 5 +---- .../collision_detection/allvalid/collision_world_allvalid.h | 5 +---- .../include/moveit/collision_detection/collision_common.h | 5 +---- .../collision_detection/collision_detector_allocator.h | 5 +---- .../include/moveit/collision_detection/collision_matrix.h | 5 +---- .../moveit/collision_detection/collision_octomap_filter.h | 5 +---- .../include/moveit/collision_detection/collision_plugin.h | 5 +---- .../include/moveit/collision_detection/collision_robot.h | 5 +---- .../include/moveit/collision_detection/collision_tools.h | 5 +---- .../include/moveit/collision_detection/collision_world.h | 5 +---- .../include/moveit/collision_detection/world.h | 5 +---- .../include/moveit/collision_detection/world_diff.h | 5 +---- .../moveit/collision_detection_fcl/collision_common.h | 5 +---- .../collision_detector_allocator_fcl.h | 5 +---- .../collision_detector_fcl_plugin_loader.h | 4 +--- .../moveit/collision_detection_fcl/collision_robot_fcl.h | 5 +---- .../moveit/collision_detection_fcl/collision_world_fcl.h | 5 +---- .../include/moveit/collision_detection_fcl/fcl_compat.h | 4 +--- .../collision_common_distance_field.h | 4 +--- .../collision_detector_allocator_distance_field.h | 5 +---- .../collision_detector_allocator_hybrid.h | 5 +---- .../collision_distance_field_types.h | 5 +---- .../collision_robot_distance_field.h | 5 +---- .../moveit/collision_distance_field/collision_robot_hybrid.h | 5 +---- .../collision_world_distance_field.h | 5 +---- .../moveit/collision_distance_field/collision_world_hybrid.h | 5 +---- .../include/moveit/constraint_samplers/constraint_sampler.h | 5 +---- .../constraint_samplers/constraint_sampler_allocator.h | 5 +---- .../moveit/constraint_samplers/constraint_sampler_manager.h | 5 +---- .../moveit/constraint_samplers/constraint_sampler_tools.h | 5 +---- .../moveit/constraint_samplers/default_constraint_samplers.h | 5 +---- .../moveit/constraint_samplers/union_constraint_sampler.h | 5 +---- moveit_core/constraint_samplers/test/pr2_arm_ik.h | 4 +--- .../constraint_samplers/test/pr2_arm_kinematics_plugin.h | 5 +---- .../include/moveit/controller_manager/controller_manager.h | 5 +---- .../include/moveit/distance_field/distance_field.h | 5 +---- .../include/moveit/distance_field/find_internal_points.h | 5 +---- .../moveit/distance_field/propagation_distance_field.h | 5 +---- .../include/moveit/distance_field/voxel_grid.h | 4 +--- .../include/moveit/dynamics_solver/dynamics_solver.h | 4 +--- .../exceptions/include/moveit/exceptions/exceptions.h | 5 +---- .../moveit/kinematic_constraints/kinematic_constraint.h | 5 +---- .../include/moveit/kinematic_constraints/utils.h | 4 +--- .../include/moveit/kinematics_base/kinematics_base.h | 5 +---- .../include/moveit/kinematics_metrics/kinematics_metrics.h | 5 +---- moveit_core/macros/include/moveit/macros/class_forward.h | 5 +---- moveit_core/macros/include/moveit/macros/console_colors.h | 5 +---- moveit_core/macros/include/moveit/macros/declare_ptr.h | 5 +---- moveit_core/macros/include/moveit/macros/deprecation.h | 5 +---- .../include/moveit/planning_interface/planning_interface.h | 5 +---- .../include/moveit/planning_interface/planning_request.h | 5 +---- .../include/moveit/planning_interface/planning_response.h | 5 +---- .../planning_request_adapter/planning_request_adapter.h | 5 +---- .../include/moveit/planning_scene/planning_scene.h | 5 +---- moveit_core/profiler/include/moveit/profiler/profiler.h | 5 +---- moveit_core/robot_model/include/moveit/robot_model/aabb.h | 5 +---- .../include/moveit/robot_model/fixed_joint_model.h | 5 +---- .../include/moveit/robot_model/floating_joint_model.h | 5 +---- .../robot_model/include/moveit/robot_model/joint_model.h | 5 +---- .../include/moveit/robot_model/joint_model_group.h | 5 +---- .../robot_model/include/moveit/robot_model/link_model.h | 5 +---- .../include/moveit/robot_model/planar_joint_model.h | 5 +---- .../include/moveit/robot_model/prismatic_joint_model.h | 5 +---- .../include/moveit/robot_model/revolute_joint_model.h | 5 +---- .../robot_model/include/moveit/robot_model/robot_model.h | 5 +---- .../robot_state/include/moveit/robot_state/attached_body.h | 5 +---- .../include/moveit/robot_state/cartesian_interpolator.h | 5 +---- .../robot_state/include/moveit/robot_state/conversions.h | 5 +---- .../robot_state/include/moveit/robot_state/robot_state.h | 5 +---- .../include/moveit/robot_trajectory/robot_trajectory.h | 5 +---- .../include/moveit/sensor_manager/sensor_manager.h | 5 +---- .../iterative_spline_parameterization.h | 5 +---- .../trajectory_processing/iterative_time_parameterization.h | 5 +---- .../time_optimal_trajectory_generation.h | 5 +---- .../include/moveit/trajectory_processing/trajectory_tools.h | 5 +---- .../transforms/include/moveit/transforms/transforms.h | 5 +---- moveit_core/utils/include/moveit/utils/lexical_casts.h | 5 +---- .../utils/include/moveit/utils/robot_model_test_utils.h | 5 +---- .../collision_distance_field_ros_helpers.h | 4 +--- .../collision_robot_distance_field_ros.h | 5 +---- .../hybrid_collision_robot_ros.h | 5 +---- .../jog_arm/include/jog_arm/collision_check_thread.h | 5 +---- moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h | 4 +--- moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h | 5 +---- .../jog_arm/include/jog_arm/jog_ros_interface.h | 5 +---- .../jog_arm/include/jog_arm/low_pass_filter.h | 4 +--- .../include/kinematics_cache/kinematics_cache.h | 5 +---- .../include/kinematics_cache_ros/kinematics_cache_ros.h | 5 +---- .../include/moveit/kinematics_cache/kinematics_cache.h | 5 +---- .../kinematics_constraint_aware.h | 5 +---- .../kinematics_request_response.h | 5 +---- .../cached_ik_kinematics_plugin.h | 4 +--- .../cached_ik_kinematics_plugin/detail/GreedyKCenters.h | 5 +---- .../cached_ik_kinematics_plugin/detail/NearestNeighbors.h | 5 +---- .../detail/NearestNeighborsGNAT.h | 5 +---- .../kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp | 4 +--- .../include/moveit/kdl_kinematics_plugin/joint_mimic.hpp | 5 +---- .../moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h | 5 +---- .../moveit/lma_kinematics_plugin/lma_kinematics_plugin.h | 5 +---- .../moveit/srv_kinematics_plugin/srv_kinematics_plugin.h | 5 +---- .../include/chomp_interface/chomp_interface.h | 5 +---- .../include/chomp_interface/chomp_planning_context.h | 5 +---- .../include/chomp_motion_planner/chomp_cost.h | 5 +---- .../include/chomp_motion_planner/chomp_optimizer.h | 5 +---- .../include/chomp_motion_planner/chomp_parameters.h | 5 +---- .../include/chomp_motion_planner/chomp_planner.h | 5 +---- .../include/chomp_motion_planner/chomp_trajectory.h | 5 +---- .../include/chomp_motion_planner/chomp_utils.h | 5 +---- .../include/chomp_motion_planner/multivariate_gaussian.h | 5 +---- .../cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h | 5 +---- .../include/moveit/ompl_interface/constraints_library.h | 5 +---- .../moveit/ompl_interface/detail/constrained_goal_sampler.h | 5 +---- .../moveit/ompl_interface/detail/constrained_sampler.h | 5 +---- .../ompl_interface/detail/constrained_valid_state_sampler.h | 5 +---- .../moveit/ompl_interface/detail/constraint_approximations.h | 5 +---- .../moveit/ompl_interface/detail/projection_evaluators.h | 5 +---- .../moveit/ompl_interface/detail/state_validity_checker.h | 5 +---- .../moveit/ompl_interface/detail/threadsafe_state_storage.h | 4 +--- .../moveit/ompl_interface/model_based_planning_context.h | 5 +---- .../include/moveit/ompl_interface/ompl_interface.h | 5 +---- .../parameterization/joint_space/joint_model_state_space.h | 5 +---- .../joint_space/joint_model_state_space_factory.h | 5 +---- .../parameterization/model_based_state_space.h | 5 +---- .../parameterization/model_based_state_space_factory.h | 5 +---- .../parameterization/work_space/pose_model_state_space.h | 5 +---- .../work_space/pose_model_state_space_factory.h | 5 +---- .../include/moveit/ompl_interface/planning_context_manager.h | 5 +---- .../sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h | 5 +---- .../core/sbpl_interface/include/sbpl_interface/bresenham.h | 5 +---- .../include/sbpl_interface/dummy_environment.h | 5 +---- .../include/sbpl_interface/environment_chain3d.h | 5 +---- .../include/sbpl_interface/environment_chain3d_types.h | 5 +---- .../sbpl_interface/include/sbpl_interface/sbpl_interface.h | 5 +---- .../include/sbpl_interface/sbpl_meta_interface.h | 5 +---- .../core/sbpl_interface/include/sbpl_interface/sbpl_params.h | 5 +---- .../include/moveit_ros_control_interface/ControllerHandle.h | 5 +---- .../action_based_controller_handle.h | 5 +---- .../follow_joint_trajectory_controller_handle.h | 5 +---- .../gripper_controller_handle.h | 5 +---- .../benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h | 5 +---- .../benchmarks/include/moveit/benchmarks/BenchmarkOptions.h | 5 +---- .../move_group_pick_place_capability/capability_names.h | 5 +---- .../src/pick_place_action_capability.h | 5 +---- .../include/moveit/pick_place/approach_and_translate_stage.h | 5 +---- .../include/moveit/pick_place/manipulation_pipeline.h | 5 +---- .../pick_place/include/moveit/pick_place/manipulation_plan.h | 5 +---- .../include/moveit/pick_place/manipulation_stage.h | 5 +---- .../pick_place/include/moveit/pick_place/pick_place.h | 5 +---- .../pick_place/include/moveit/pick_place/pick_place_params.h | 5 +---- .../pick_place/include/moveit/pick_place/plan_stage.h | 5 +---- .../include/moveit/pick_place/reachable_valid_pose_filter.h | 5 +---- .../move_group/include/moveit/move_group/capability_names.h | 5 +---- .../include/moveit/move_group/move_group_capability.h | 5 +---- .../include/moveit/move_group/move_group_context.h | 5 +---- moveit_ros/move_group/include/moveit/move_group/node_name.h | 5 +---- .../apply_planning_scene_service_capability.h | 5 +---- .../default_capabilities/cartesian_path_service_capability.h | 5 +---- .../default_capabilities/clear_octomap_service_capability.h | 5 +---- .../execute_trajectory_action_capability.h | 5 +---- .../get_planning_scene_service_capability.h | 5 +---- .../src/default_capabilities/kinematics_service_capability.h | 5 +---- .../src/default_capabilities/move_action_capability.h | 5 +---- .../src/default_capabilities/plan_service_capability.h | 5 +---- .../default_capabilities/query_planners_service_capability.h | 5 +---- .../state_validation_service_capability.h | 5 +---- .../include/moveit/occupancy_map_monitor/occupancy_map.h | 5 +---- .../moveit/occupancy_map_monitor/occupancy_map_monitor.h | 5 +---- .../moveit/occupancy_map_monitor/occupancy_map_updater.h | 5 +---- .../depth_image_octomap_updater.h | 5 +---- .../moveit/lazy_free_space_updater/lazy_free_space_updater.h | 5 +---- .../include/moveit/mesh_filter/depth_self_filter_nodelet.h | 5 +---- .../mesh_filter/include/moveit/mesh_filter/filter_job.h | 4 +--- .../mesh_filter/include/moveit/mesh_filter/gl_mesh.h | 4 +--- .../mesh_filter/include/moveit/mesh_filter/gl_renderer.h | 4 +--- .../mesh_filter/include/moveit/mesh_filter/mesh_filter.h | 4 +--- .../include/moveit/mesh_filter/mesh_filter_base.h | 5 +---- .../mesh_filter/include/moveit/mesh_filter/sensor_model.h | 5 +---- .../include/moveit/mesh_filter/stereo_camera_model.h | 4 +--- .../include/moveit/mesh_filter/transform_provider.h | 4 +--- .../include/moveit/point_containment_filter/shape_mask.h | 5 +---- .../pointcloud_octomap_updater/pointcloud_octomap_updater.h | 5 +---- .../include/moveit/semantic_world/semantic_world.h | 5 +---- .../moveit/collision_plugin_loader/collision_plugin_loader.h | 5 +---- .../constraint_sampler_manager_loader.h | 5 +---- .../kinematics_plugin_loader/kinematics_plugin_loader.h | 5 +---- .../include/moveit/plan_execution/plan_execution.h | 4 +--- .../include/moveit/plan_execution/plan_representation.h | 4 +--- .../include/moveit/plan_execution/plan_with_sensing.h | 4 +--- .../include/moveit/planning_pipeline/planning_pipeline.h | 5 +---- .../moveit/planning_scene_monitor/current_state_monitor.h | 5 +---- .../moveit/planning_scene_monitor/planning_scene_monitor.h | 5 +---- .../moveit/planning_scene_monitor/trajectory_monitor.h | 5 +---- .../rdf_loader/include/moveit/rdf_loader/rdf_loader.h | 4 +--- .../include/moveit/robot_model_loader/robot_model_loader.h | 4 +--- .../trajectory_execution_manager.h | 5 +---- .../test/test_moveit_controller_manager.h | 4 +--- .../common_planning_interface_objects/common_objects.h | 5 +---- .../moveit/move_group_interface/move_group_interface.h | 5 +---- .../planning_scene_interface/planning_scene_interface.h | 5 +---- .../include/moveit/py_bindings_tools/py_conversions.h | 5 +---- .../include/moveit/py_bindings_tools/roscpp_initializer.h | 5 +---- .../include/moveit/py_bindings_tools/serialize_msg.h | 5 +---- .../include/moveit/robot_interaction/interaction.h | 5 +---- .../include/moveit/robot_interaction/interaction_handler.h | 5 +---- .../moveit/robot_interaction/interactive_marker_helpers.h | 5 +---- .../include/moveit/robot_interaction/kinematic_options.h | 5 +---- .../include/moveit/robot_interaction/kinematic_options_map.h | 5 +---- .../include/moveit/robot_interaction/locked_robot_state.h | 5 +---- .../include/moveit/robot_interaction/robot_interaction.h | 5 +---- .../motion_planning_rviz_plugin/motion_planning_display.h | 5 +---- .../motion_planning_rviz_plugin/motion_planning_frame.h | 5 +---- .../motion_planning_param_widget.h | 5 +---- .../planning_scene_rviz_plugin/planning_scene_display.h | 5 +---- .../moveit/robot_state_rviz_plugin/robot_state_display.h | 5 +---- .../include/moveit/rviz_plugin_render_tools/octomap_render.h | 4 +--- .../moveit/rviz_plugin_render_tools/planning_link_updater.h | 5 +---- .../moveit/rviz_plugin_render_tools/planning_scene_render.h | 5 +---- .../include/moveit/rviz_plugin_render_tools/render_shapes.h | 5 +---- .../rviz_plugin_render_tools/robot_state_visualization.h | 5 +---- .../moveit/rviz_plugin_render_tools/trajectory_panel.h | 5 +---- .../rviz_plugin_render_tools/trajectory_visualization.h | 5 +---- .../moveit/trajectory_rviz_plugin/trajectory_display.h | 5 +---- .../warehouse/include/moveit/warehouse/constraints_storage.h | 5 +---- .../include/moveit/warehouse/moveit_message_storage.h | 5 +---- .../include/moveit/warehouse/planning_scene_storage.h | 5 +---- .../include/moveit/warehouse/planning_scene_world_storage.h | 5 +---- .../warehouse/include/moveit/warehouse/state_storage.h | 5 +---- .../moveit/warehouse/trajectory_constraints_storage.h | 5 +---- .../warehouse/include/moveit/warehouse/warehouse_connector.h | 5 +---- .../setup_assistant/tools/compute_default_collisions.h | 5 +---- .../moveit/setup_assistant/tools/moveit_config_data.h | 5 +---- moveit_setup_assistant/src/tools/collision_linear_model.h | 5 +---- moveit_setup_assistant/src/tools/collision_matrix_model.h | 5 +---- moveit_setup_assistant/src/tools/rotated_header_view.h | 5 +---- .../src/widgets/author_information_widget.h | 5 +---- .../src/widgets/configuration_files_widget.h | 5 +---- moveit_setup_assistant/src/widgets/controller_edit_widget.h | 5 +---- .../src/widgets/default_collisions_widget.h | 5 +---- moveit_setup_assistant/src/widgets/double_list_widget.h | 5 +---- moveit_setup_assistant/src/widgets/end_effectors_widget.h | 5 +---- moveit_setup_assistant/src/widgets/group_edit_widget.h | 5 +---- moveit_setup_assistant/src/widgets/header_widget.h | 5 +---- moveit_setup_assistant/src/widgets/kinematic_chain_widget.h | 5 +---- moveit_setup_assistant/src/widgets/navigation_widget.h | 5 +---- moveit_setup_assistant/src/widgets/passive_joints_widget.h | 5 +---- moveit_setup_assistant/src/widgets/perception_widget.h | 5 +---- moveit_setup_assistant/src/widgets/planning_groups_widget.h | 5 +---- moveit_setup_assistant/src/widgets/robot_poses_widget.h | 5 +---- moveit_setup_assistant/src/widgets/ros_controllers_widget.h | 5 +---- moveit_setup_assistant/src/widgets/setup_assistant_widget.h | 5 +---- moveit_setup_assistant/src/widgets/setup_screen_widget.h | 5 +---- moveit_setup_assistant/src/widgets/simulation_widget.h | 5 +---- moveit_setup_assistant/src/widgets/start_screen_widget.h | 5 +---- moveit_setup_assistant/src/widgets/virtual_joints_widget.h | 5 +---- 257 files changed, 257 insertions(+), 1002 deletions(-) diff --git a/moveit_core/background_processing/include/moveit/background_processing/background_processing.h b/moveit_core/background_processing/include/moveit/background_processing/background_processing.h index 270be22622..b87a82f997 100644 --- a/moveit_core/background_processing/include/moveit/background_processing/background_processing.h +++ b/moveit_core/background_processing/include/moveit/background_processing/background_processing.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_BACKGROUND_PROCESSING_ -#define MOVEIT_BACKGROUND_PROCESSING_ +#pragma once #include #include @@ -113,5 +112,3 @@ class BackgroundProcessing : private boost::noncopyable }; } } - -#endif diff --git a/moveit_core/backtrace/include/moveit/backtrace/backtrace.h b/moveit_core/backtrace/include/moveit/backtrace/backtrace.h index c00033f9bc..224c0fe2dc 100644 --- a/moveit_core/backtrace/include/moveit/backtrace/backtrace.h +++ b/moveit_core/backtrace/include/moveit/backtrace/backtrace.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_BACKTRACE_ -#define MOVEIT_BACKTRACE_ +#pragma once #include @@ -63,5 +62,3 @@ void get_backtrace(std::ostream& out) } #endif } - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h index 549a6d6990..0382b492d7 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALL_VALID_H_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALL_VALID_H_ +#pragma once #include #include @@ -52,5 +51,3 @@ class CollisionDetectorAllocatorAllValid static const std::string NAME; // defined in collision_world_allvalid.cpp }; } - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h index 9ac86aa6bb..9ce0e2dc15 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_ALLVALID_COLLISION_ROBOT_ALLVALID_ -#define MOVEIT_COLLISION_DETECTION_ALLVALID_COLLISION_ROBOT_ALLVALID_ +#pragma once #include @@ -84,5 +83,3 @@ class CollisionRobotAllValid : public CollisionRobot const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override; }; } - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h index c950579668..dcf045b554 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_ALLVALID_COLLISION_WORLD_ALLVALID_ -#define MOVEIT_COLLISION_DETECTION_ALLVALID_COLLISION_WORLD_ALLVALID_ +#pragma once #include @@ -75,5 +74,3 @@ class CollisionWorldAllValid : public CollisionWorld void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override; }; } - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 601755b0bb..c4ec8f25a5 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_COMMON_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_COMMON_ +#pragma once #include #include @@ -382,5 +381,3 @@ struct DistanceResult } }; } - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h index aed7eac019..018d6be5e0 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ +#pragma once #include #include @@ -108,5 +107,3 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator } }; } - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index 59eb26e535..1b9e36e36e 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, E. Gil Jones */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_MATRIX_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_MATRIX_ +#pragma once #include #include @@ -278,5 +277,3 @@ class AllowedCollisionMatrix std::map default_allowed_contacts_; }; } - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h index 466d58a685..1460b80ff0 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h @@ -34,8 +34,7 @@ /* Author: Adam Leeper */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_OCTOMAP_FILTER_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_OCTOMAP_FILTER_ +#pragma once #include #include @@ -62,5 +61,3 @@ int refineContactNormals(const World::ObjectConstPtr& object, CollisionResult& r double cell_bbx_search_distance = 1.0, double allowed_angle_divergence = 0.0, bool estimate_depth = false, double iso_value = 0.5, double metaball_radius_multiple = 1.5); } - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h index 156318f4bc..5142e41bb9 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_PLUGIN_H -#define MOVEIT_COLLISION_DETECTION_COLLISION_PLUGIN_H +#pragma once #include #include @@ -94,5 +93,3 @@ class CollisionPlugin }; } // namespace collision_detection - -#endif // MOVEIT_COLLISION_DETECTION_COLLISION_PLUGIN_H diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h index 1b6d476a89..2249b56c73 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_ROBOT_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_ROBOT_ +#pragma once #include #include @@ -309,5 +308,3 @@ class CollisionRobot std::map link_scale_; }; } - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h index 6893c41444..8b06686148 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_TOOLS_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_TOOLS_ +#pragma once #include #include @@ -72,5 +71,3 @@ bool getSensorPositioning(geometry_msgs::Point& point, const std::set @@ -278,5 +277,3 @@ class CollisionWorld : private boost::noncopyable WorldConstPtr world_const_; // always same as world_ }; } - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index f461043f4a..82b299673e 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Acorn Pooley, Sachin Chitta */ -#ifndef MOVEIT_COLLISION_DETECTION_WORLD_ -#define MOVEIT_COLLISION_DETECTION_WORLD_ +#pragma once #include @@ -300,5 +299,3 @@ class World std::vector observers_; }; } - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h index 5f2013d7f0..84ae6a522c 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley, Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_COLLISION_DETECTION_WORLD_DIFF_ -#define MOVEIT_COLLISION_DETECTION_WORLD_DIFF_ +#pragma once #include #include @@ -126,5 +125,3 @@ class WorldDiff WorldWeakPtr world_; }; } - -#endif diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index f9fce66b21..347a9426d1 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_COMMON_ -#define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_COMMON_ +#pragma once #include #include @@ -298,5 +297,3 @@ inline void fcl2costsource(const fcl::CostSourced& fcs, CostSource& cs) cs.cost = fcs.cost_density; } } - -#endif diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h index 6b7ec1ce52..a72b4fd5d6 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_FCL_H_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_FCL_H_ +#pragma once #include #include @@ -51,5 +50,3 @@ class CollisionDetectorAllocatorFCL static const std::string NAME; // defined in collision_world_fcl.cpp }; } - -#endif diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h index 18d7e8c40f..e1d749474c 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h @@ -2,8 +2,7 @@ * collision_detector_fcl_plugin_loader.h */ -#ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_DETECTOR_FCL_PLUGIN_LOADER_H_ -#define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_DETECTOR_FCL_PLUGIN_LOADER_H_ +#pragma once #include #include @@ -16,4 +15,3 @@ class CollisionDetectorFCLPluginLoader : public CollisionPlugin virtual bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const; }; } -#endif // MOVEIT_COLLISION_DETECTION_FCL_COLLISION_DETECTOR_FCL_PLUGIN_LOADER_H_ \ No newline at end of file diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h index a46794e999..917658bab3 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_ROBOT_ -#define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_ROBOT_ +#pragma once #include @@ -96,5 +95,3 @@ class CollisionRobotFCL : public CollisionRobot std::vector fcl_objs_; }; } - -#endif diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h index e6d9557630..65cc476471 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_WORLD_FCL_ -#define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_WORLD_FCL_ +#pragma once #include #include @@ -97,5 +96,3 @@ class CollisionWorldFCL : public CollisionWorld World::ObserverHandle observer_handle_; }; } - -#endif diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h index 9eeda077cd..123fac3c08 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h @@ -34,8 +34,7 @@ /* Author: Benjamin Scholz */ -#ifndef MOVEIT_COLLISION_DETECTION_FCL_COMPAT_ -#define MOVEIT_COLLISION_DETECTION_FCL_COMPAT_ +#pragma once #include @@ -98,4 +97,3 @@ class DynamicAABBTreeCollisionManager; using DynamicAABBTreeCollisionManagerd = fcl::DynamicAABBTreeCollisionManager; } #endif -#endif diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h index fa5910a581..024be61cfe 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h @@ -34,8 +34,7 @@ /* Author: E. Gil Jones */ -#ifndef MOVEIT_COLLISION_DETECTION_DISTANCE_FIELD_COLLISION_COMMON_ -#define MOVEIT_COLLISION_DETECTION_DISTANCE_FIELD_COLLISION_COMMON_ +#pragma once #include #include @@ -180,4 +179,3 @@ PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const rob void getBodySphereVisualizationMarkers(const GroupStateRepresentationPtr& gsr, const std::string& reference_frame, visualization_msgs::MarkerArray& body_marker_array); } -#endif diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h index 53c71b9f35..9581cefa7b 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_DISTANCE_FIELD_H_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_DISTANCE_FIELD_H_ +#pragma once #include #include @@ -52,5 +51,3 @@ class CollisionDetectorAllocatorDistanceField static const std::string NAME; // defined in collision_world_distance_field.cpp }; } - -#endif diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h index 6eb3277ee7..a2c643b282 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_HYBRID_H_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_HYBRID_H_ +#pragma once #include #include @@ -52,5 +51,3 @@ class CollisionDetectorAllocatorHybrid static const std::string NAME; // defined in collision_world_hybrid.cpp }; } - -#endif diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index 364386fb34..9816e4be1e 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -34,8 +34,7 @@ /* Author: E. Gil Jones */ -#ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_DISTANCE_FIELD_TYPES_ -#define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_DISTANCE_FIELD_TYPES_ +#pragma once #include #include @@ -524,5 +523,3 @@ void getCollisionMarkers(const std::string& frame_id, const std::string& ns, con const std::vector& posed_vector_decompositions, const std::vector& gradients, visualization_msgs::MarkerArray& arr); } - -#endif diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_distance_field.h index 2c968dac76..dd13b12043 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_distance_field.h @@ -34,8 +34,7 @@ /* Author: E. Gil Jones */ -#ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_ROBOT_DISTANCE_FIELD_ -#define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_ROBOT_DISTANCE_FIELD_ +#pragma once #include #include @@ -263,5 +262,3 @@ class CollisionRobotDistanceField : public CollisionRobot planning_scene::PlanningScenePtr planning_scene_; }; } - -#endif diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_hybrid.h index 528fe3b226..7b188ce97e 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_hybrid.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_hybrid.h @@ -34,8 +34,7 @@ /* Author: E. Gil Jones */ -#ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_ROBOT_HYBRID_H_ -#define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_ROBOT_HYBRID_H_ +#pragma once #include #include @@ -97,5 +96,3 @@ class CollisionRobotHybrid : public collision_detection::CollisionRobotFCL CollisionRobotDistanceFieldPtr crobot_distance_; }; } - -#endif diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h index d2a5e950fb..ff05802ab9 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h @@ -34,8 +34,7 @@ /* Author: E. Gil Jones */ -#ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_DISTANCE_FIELD_ -#define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_DISTANCE_FIELD_ +#pragma once #include #include @@ -199,5 +198,3 @@ class CollisionWorldDistanceField : public CollisionWorld World::ObserverHandle observer_handle_; }; } - -#endif diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_hybrid.h index 9b70e3c6dc..bb3d07e8cf 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_hybrid.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_hybrid.h @@ -34,8 +34,7 @@ /* Author: E. Gil Jones */ -#ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_HYBRID_H_ -#define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_HYBRID_H_ +#pragma once #include #include @@ -115,5 +114,3 @@ class CollisionWorldHybrid : public CollisionWorldFCL CollisionWorldDistanceFieldPtr cworld_distance_; }; } - -#endif diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h index ce2f2ed073..f86cc069fa 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_ -#define MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_ +#pragma once #include #include @@ -296,5 +295,3 @@ class ConstraintSampler bool verbose_; ///< True if verbosity is on }; } - -#endif diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h index abc68a0e28..89db5e830e 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_ALLOCATOR_ -#define MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_ALLOCATOR_ +#pragma once #include #include @@ -62,5 +61,3 @@ class ConstraintSamplerAllocator const moveit_msgs::Constraints& constr) const = 0; }; } - -#endif diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h index 05c88548e8..630e307c97 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_MANAGER_ -#define MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_MANAGER_ +#pragma once #include #include @@ -144,5 +143,3 @@ class ConstraintSamplerManager sampler_alloc_; /**< \brief Holds the constraint sampler allocators, which will be tested in order */ }; } - -#endif diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h index ce405b9d51..f222aea111 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_TOOLS_ -#define MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_TOOLS_ +#pragma once #include #include @@ -55,5 +54,3 @@ double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const robot_st double countSamplesPerSecond(const moveit_msgs::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, const std::string& group); } - -#endif diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h index 77a048bdb8..399e3cc7ef 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_CONSTRAINT_SAMPLERS_ -#define MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_CONSTRAINT_SAMPLERS_ +#pragma once #include #include @@ -526,5 +525,3 @@ class IKConstraintSampler : public ConstraintSampler Eigen::Isometry3d eef_to_ik_tip_transform_; /**< \brief Holds the transformation from end effector to IK tip frame */ }; } - -#endif diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h index a50c16298a..7b00590491 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_UNION_CONSTRAINT_SAMPLER_ -#define MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_UNION_CONSTRAINT_SAMPLER_ +#pragma once #include @@ -169,5 +168,3 @@ class UnionConstraintSampler : public ConstraintSampler std::vector samplers_; /**< \brief Holder for sorted internal list of samplers*/ }; } - -#endif diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.h b/moveit_core/constraint_samplers/test/pr2_arm_ik.h index 8ba4052ca9..46d4a7ef32 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.h @@ -34,8 +34,7 @@ /* Author: Sachin Chitta */ -#ifndef PR2_ARM_IK_H -#define PR2_ARM_IK_H +#pragma once #include #include @@ -190,4 +189,3 @@ class PR2ArmIK std::vector continuous_joint_; }; } -#endif // PR2_ARM_IK_H diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index 94bd16926e..a3cca94e0c 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -34,8 +34,7 @@ /* Author: Sachin Chitta */ -#ifndef PR2_ARM_IK_NODE_H -#define PR2_ARM_IK_NODE_H +#pragma once #include #include @@ -261,5 +260,3 @@ class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase moveit_msgs::MoveItErrorCodes& error_code) const; }; } - -#endif diff --git a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h index 2a32ff04a8..7af922b3bc 100644 --- a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h +++ b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_CONTROLLER_MANAGER_ -#define MOVEIT_MOVEIT_CONTROLLER_MANAGER_ +#pragma once #include #include @@ -209,5 +208,3 @@ class MoveItControllerManager const std::vector& deactivate) = 0; }; } - -#endif diff --git a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h index d1164a4935..b4bfa238d7 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan, Ken Anderson, E. Gil Jones */ -#ifndef MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_H -#define MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_H +#pragma once #include #include @@ -627,5 +626,3 @@ class DistanceField }; } // namespace distance_field - -#endif // MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_H diff --git a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h index be419651f9..27363466e1 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h +++ b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley */ -#ifndef MOVEIT_DISTANCE_FIELD__FIND_INTERNAL_POINTS_ -#define MOVEIT_DISTANCE_FIELD__FIND_INTERNAL_POINTS_ +#pragma once #include #include @@ -54,5 +53,3 @@ namespace distance_field */ void findInternalPointsConvex(const bodies::Body& body, double resolution, EigenSTL::vector_Vector3d& points); } - -#endif diff --git a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h index 66cbf2daa2..a278a7f6d6 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan, Ken Anderson */ -#ifndef MOVEIT_DISTANCE_FIELD_PROPAGATION_DISTANCE_FIELD_ -#define MOVEIT_DISTANCE_FIELD_PROPAGATION_DISTANCE_FIELD_ +#pragma once #include #include @@ -611,5 +610,3 @@ inline double PropagationDistanceField::getDistance(const PropDistanceFieldVoxel return sqrt_table_[object.distance_square_] - sqrt_table_[object.negative_distance_square_]; } } - -#endif diff --git a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h index f85cfbdcd7..6cecf4e050 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h +++ b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan, Acorn Pooley */ -#ifndef MOVEIT_DISTANCE_FIELD_VOXEL_GRID_ -#define MOVEIT_DISTANCE_FIELD_VOXEL_GRID_ +#pragma once #include #include @@ -579,4 +578,3 @@ inline bool VoxelGrid::worldToGrid(const Eigen::Vector3d& world, Eigen::Vecto } } // namespace distance_field -#endif diff --git a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h index 3e73f8b036..1f579d0c6f 100644 --- a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h +++ b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h @@ -34,8 +34,7 @@ /* Author: Sachin Chitta */ -#ifndef MOVEIT_DYNAMICS_SOLVER_DYNAMICS_SOLVER_ -#define MOVEIT_DYNAMICS_SOLVER_DYNAMICS_SOLVER_ +#pragma once // KDL #include @@ -151,4 +150,3 @@ class DynamicsSolver double gravity_; // Norm of the gravity vector passed in initialize() }; } -#endif diff --git a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h index 7918eef07d..34394e987c 100644 --- a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h +++ b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#ifndef MOVEIT_EXCEPTIONS_ -#define MOVEIT_EXCEPTIONS_ +#pragma once #include @@ -56,5 +55,3 @@ class Exception : public std::runtime_error explicit Exception(const std::string& what_arg); }; } - -#endif diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index 936ebf82fe..4d15e35d63 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_KINEMATIC_CONSTRAINTS_KINEMATIC_CONSTRAINT_ -#define MOVEIT_KINEMATIC_CONSTRAINTS_KINEMATIC_CONSTRAINT_ +#pragma once #include #include @@ -1069,5 +1068,3 @@ class KinematicConstraintSet moveit_msgs::Constraints all_constraints_; /**< \brief Messages corresponding to all internal constraints */ }; } - -#endif diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h index e5e9e5f74f..f2dfe6f84a 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_ -#define MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_ +#pragma once #include #include @@ -216,4 +215,3 @@ bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::Constraints& */ bool resolveConstraintFrames(const robot_state::RobotState& state, moveit_msgs::Constraints& constraints); } -#endif diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index a3323e95ae..59fc3ee7c4 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -34,8 +34,7 @@ /* Author: Sachin Chitta, Dave Coleman */ -#ifndef MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_ -#define MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_ +#pragma once #include #include @@ -665,5 +664,3 @@ class KinematicsBase std::string removeSlash(const std::string& str) const; }; }; - -#endif diff --git a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h index c92a7d5063..da5c213e49 100644 --- a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h +++ b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h @@ -34,8 +34,7 @@ /* Author: Sachin Chitta */ -#ifndef MOVEIT_KINEMATICS_METRICS_KINEMATICS_METRICS_ -#define MOVEIT_KINEMATICS_METRICS_KINEMATICS_METRICS_ +#pragma once #include #include @@ -158,5 +157,3 @@ class KinematicsMetrics double penalty_multiplier_; }; } - -#endif diff --git a/moveit_core/macros/include/moveit/macros/class_forward.h b/moveit_core/macros/include/moveit/macros/class_forward.h index 57bb5ce56b..1c5ab03544 100644 --- a/moveit_core/macros/include/moveit/macros/class_forward.h +++ b/moveit_core/macros/include/moveit/macros/class_forward.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MACROS_CLASS_FORWARD_ -#define MOVEIT_MACROS_CLASS_FORWARD_ +#pragma once #include @@ -58,5 +57,3 @@ #define MOVEIT_STRUCT_FORWARD(C) \ struct C; \ MOVEIT_DECLARE_PTR(C, C); - -#endif diff --git a/moveit_core/macros/include/moveit/macros/console_colors.h b/moveit_core/macros/include/moveit/macros/console_colors.h index 4ea7b65d39..0da6ae80de 100644 --- a/moveit_core/macros/include/moveit/macros/console_colors.h +++ b/moveit_core/macros/include/moveit/macros/console_colors.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSOLE_COLORS_ -#define MOVEIT_CONSOLE_COLORS_ +#pragma once #define MOVEIT_CONSOLE_COLOR_RESET "\033[0m" #define MOVEIT_CONSOLE_COLOR_GRAY "\033[90m" @@ -46,5 +45,3 @@ #define MOVEIT_CONSOLE_COLOR_BLUE "\033[94m" #define MOVEIT_CONSOLE_COLOR_PURPLE "\033[95m" #define MOVEIT_CONSOLE_COLOR_CYAN "\033[96m" - -#endif diff --git a/moveit_core/macros/include/moveit/macros/declare_ptr.h b/moveit_core/macros/include/moveit/macros/declare_ptr.h index 36506e618a..fc3eeb6e39 100644 --- a/moveit_core/macros/include/moveit/macros/declare_ptr.h +++ b/moveit_core/macros/include/moveit/macros/declare_ptr.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_MACROS_DECLARE_PTR_ -#define MOVEIT_MACROS_DECLARE_PTR_ +#pragma once #include @@ -70,5 +69,3 @@ typedef std::shared_ptr ConstPtr; \ typedef std::weak_ptr WeakPtr; \ typedef std::weak_ptr ConstWeakPtr; - -#endif diff --git a/moveit_core/macros/include/moveit/macros/deprecation.h b/moveit_core/macros/include/moveit/macros/deprecation.h index 1a69e998f0..20f24b06ac 100644 --- a/moveit_core/macros/include/moveit/macros/deprecation.h +++ b/moveit_core/macros/include/moveit/macros/deprecation.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_MACROS_DEPRECATION_ -#define MOVEIT_MACROS_DEPRECATION_ +#pragma once /** \def MOVEIT_DEPRECATED Macro that marks functions as deprecated */ @@ -47,5 +46,3 @@ #else #define MOVEIT_DEPRECATED /* Nothing */ #endif - -#endif diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index 93748f43ef..9cf2f206f4 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_INTERFACE_PLANNING_INTERFACE_ -#define MOVEIT_PLANNING_INTERFACE_PLANNING_INTERFACE_ +#pragma once #include #include @@ -213,5 +212,3 @@ class PlannerManager }; } // planning_interface - -#endif diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h index 6138283043..a738f0a0df 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_INTERFACE_PLANNING_REQUEST_ -#define MOVEIT_PLANNING_INTERFACE_PLANNING_REQUEST_ +#pragma once #include @@ -46,5 +45,3 @@ namespace planning_interface typedef moveit_msgs::MotionPlanRequest MotionPlanRequest; } // planning_interface - -#endif diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h index c902ffd741..e05209921d 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_INTERFACE_PLANNING_RESPONSE_ -#define MOVEIT_PLANNING_INTERFACE_PLANNING_RESPONSE_ +#pragma once #include #include @@ -68,5 +67,3 @@ struct MotionPlanDetailedResponse }; } // planning_interface - -#endif diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h index e950bfba11..fd2b34a424 100644 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_REQUEST_ADAPTER_PLANNING_REQUEST_ADAPTER_ -#define MOVEIT_PLANNING_REQUEST_ADAPTER_PLANNING_REQUEST_ADAPTER_ +#pragma once #include #include @@ -134,5 +133,3 @@ class PlanningRequestAdapterChain std::vector adapters_; }; } - -#endif diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 6503f7793a..32adb11f61 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Acorn Pooley */ -#ifndef MOVEIT_PLANNING_SCENE_PLANNING_SCENE_ -#define MOVEIT_PLANNING_SCENE_PLANNING_SCENE_ +#pragma once #include #include @@ -1056,5 +1055,3 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from std::unique_ptr object_types_; }; } - -#endif diff --git a/moveit_core/profiler/include/moveit/profiler/profiler.h b/moveit_core/profiler/include/moveit/profiler/profiler.h index b996af16f7..554ec2b755 100644 --- a/moveit_core/profiler/include/moveit/profiler/profiler.h +++ b/moveit_core/profiler/include/moveit/profiler/profiler.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PROFILER_ -#define MOVEIT_PROFILER_ +#pragma once #define MOVEIT_ENABLE_PROFILING 1 @@ -448,5 +447,3 @@ class Profiler } #endif - -#endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/aabb.h b/moveit_core/robot_model/include/moveit/robot_model/aabb.h index 2ed8ebf382..5c1b84df13 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/aabb.h +++ b/moveit_core/robot_model/include/moveit/robot_model/aabb.h @@ -34,8 +34,7 @@ /* Author: Martin Pecka */ -#ifndef MOVEIT_ROBOT_MODEL_AABB_H -#define MOVEIT_ROBOT_MODEL_AABB_H +#pragma once #include @@ -52,5 +51,3 @@ class AABB : public Eigen::AlignedBox3d }; } } - -#endif // MOVEIT_ROBOT_MODEL_AABB_H diff --git a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h index 265b13576c..d10b1622a7 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_FIXED_JOINT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_FIXED_JOINT_MODEL_ +#pragma once #include @@ -68,5 +67,3 @@ class FixedJointModel : public JointModel }; } } - -#endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h index a39a8471e5..6d721d9bf5 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_FLOATING_JOINT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_FLOATING_JOINT_MODEL_ +#pragma once #include @@ -91,5 +90,3 @@ class FloatingJointModel : public JointModel }; } } - -#endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index 982ae78bb8..aa07321d73 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -35,8 +35,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_ +#pragma once #include #include @@ -521,5 +520,3 @@ class JointModel std::ostream& operator<<(std::ostream& out, const VariableBounds& b); } } - -#endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index d46e1fc748..108e1a5ab3 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -35,8 +35,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_GROUP_ -#define MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_GROUP_ +#pragma once #include #include @@ -745,5 +744,3 @@ class JointModelGroup }; } } - -#endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.h b/moveit_core/robot_model/include/moveit/robot_model/link_model.h index decebc2a82..d586dfded0 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/link_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_LINK_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_LINK_MODEL_ +#pragma once #include #include @@ -283,5 +282,3 @@ class LinkModel }; } } - -#endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h index 0900e0b8d9..3524250e41 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_PLANAR_JOINT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_PLANAR_JOINT_MODEL_ +#pragma once #include @@ -86,5 +85,3 @@ class PlanarJointModel : public JointModel }; } } - -#endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h index f745947e18..87678c55a5 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_PRISMATIC_JOINT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_PRISMATIC_JOINT_MODEL_ +#pragma once #include @@ -86,5 +85,3 @@ class PrismaticJointModel : public JointModel }; } } - -#endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h index 33adc02839..76322c2b04 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_REVOLUTE_JOINT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_REVOLUTE_JOINT_MODEL_ +#pragma once #include @@ -97,5 +96,3 @@ class RevoluteJointModel : public JointModel }; } } - -#endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 5e0b98061d..835c675c04 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -35,8 +35,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_ +#pragma once #include #include @@ -621,5 +620,3 @@ class RobotModel namespace robot_model = moveit::core; namespace robot_state = moveit::core; - -#endif diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index 3453d3233d..bf5a4f5ec6 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_ROBOT_STATE_ATTACHED_BODY_ -#define MOVEIT_ROBOT_STATE_ATTACHED_BODY_ +#pragma once #include #include @@ -184,5 +183,3 @@ class AttachedBody }; } } - -#endif diff --git a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h index a5447f2f85..ecf843c99a 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h +++ b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h @@ -36,8 +36,7 @@ /* Author: Ioan Sucan, Mike Lautman */ -#ifndef MOVEIT_CORE_ROBOT_STATE_CARTESIAN_INTERPOLATOR_ -#define MOVEIT_CORE_ROBOT_STATE_CARTESIAN_INTERPOLATOR_ +#pragma once #include @@ -215,5 +214,3 @@ class CartesianInterpolator } // end of namespace core } // end of namespace moveit - -#endif diff --git a/moveit_core/robot_state/include/moveit/robot_state/conversions.h b/moveit_core/robot_state/include/moveit/robot_state/conversions.h index 655329acff..e5d1017d33 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/conversions.h +++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Dave Coleman */ -#ifndef MOVEIT_ROBOT_STATE_CONVERSIONS_ -#define MOVEIT_ROBOT_STATE_CONVERSIONS_ +#pragma once #include #include @@ -143,5 +142,3 @@ void robotStateToStream(const RobotState& state, std::ostream& out, void streamToRobotState(RobotState& state, const std::string& line, const std::string& separator = ","); } } - -#endif diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index c65a5e7eeb..126b20baf9 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -35,8 +35,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_STATE_ -#define MOVEIT_CORE_ROBOT_STATE_ +#pragma once #include #include @@ -1837,5 +1836,3 @@ as the new values that correspond to the group */ std::ostream& operator<<(std::ostream& out, const RobotState& s); } } - -#endif diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 7b3a4b9507..1601cd4279 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Adam Leeper */ -#ifndef MOVEIT_ROBOT_TRAJECTORY_KINEMATIC_TRAJECTORY_ -#define MOVEIT_ROBOT_TRAJECTORY_KINEMATIC_TRAJECTORY_ +#pragma once #include #include @@ -265,5 +264,3 @@ class RobotTrajectory std::deque duration_from_previous_; }; } - -#endif diff --git a/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h b/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h index a8f1a1615b..222e703504 100644 --- a/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h +++ b/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_SENSOR_MANAGER_ -#define MOVEIT_MOVEIT_SENSOR_MANAGER_ +#pragma once #include #include @@ -103,5 +102,3 @@ class MoveItSensorManager moveit_msgs::RobotTrajectory& sensor_trajectory) = 0; }; } - -#endif diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h index c3cb24cb12..67f4683047 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h @@ -35,8 +35,7 @@ /* Author: Ken Anderson */ -#ifndef MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_SPLINE_PARAMETERIZATION__ -#define MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_SPLINE_PARAMETERIZATION__ +#pragma once #include #include @@ -85,5 +84,3 @@ class IterativeSplineParameterization /// If false, move the 2nd and 2nd-last points. }; } - -#endif diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h index 59b883f089..64e3d2b5b9 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h @@ -34,8 +34,7 @@ /* Author: Ken Anderson */ -#ifndef MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_PARABOLIC_SMOOTHER_ -#define MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_PARABOLIC_SMOOTHER_ +#pragma once #include #include @@ -69,5 +68,3 @@ class IterativeParabolicTimeParameterization double findT2(const double d1, const double d2, const double t1, double t2, const double a_max) const; }; } - -#endif diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index 21db426fd1..3e8268a92c 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -36,8 +36,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef MOVEIT_TRAJECTORY_PROCESSING_TIME_OPTIMAL_TRAJECTORY_GENERATION_H -#define MOVEIT_TRAJECTORY_PROCESSING_TIME_OPTIMAL_TRAJECTORY_GENERATION_H +#pragma once #include #include @@ -174,5 +173,3 @@ class TimeOptimalTrajectoryGeneration const double resample_dt_; }; } // namespace trajectory_processing - -#endif // MOVEIT_TRAJECTORY_PROCESSING_TIME_OPTIMAL_TRAJECTORY_GENERATION_H diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h index 0c7658d39f..4c818af3e8 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_TRAJECTORY_PROCESSING_TRAJECTORY_TOOLS_ -#define MOVEIT_TRAJECTORY_PROCESSING_TRAJECTORY_TOOLS_ +#pragma once #include @@ -44,5 +43,3 @@ namespace trajectory_processing bool isTrajectoryEmpty(const moveit_msgs::RobotTrajectory& trajectory); std::size_t trajectoryWaypointCount(const moveit_msgs::RobotTrajectory& trajectory); } - -#endif diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 949a61341f..6833b606aa 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_TRANSFORMS_ -#define MOVEIT_TRANSFORMS_ +#pragma once #include #include @@ -200,5 +199,3 @@ class Transforms : private boost::noncopyable }; } } - -#endif diff --git a/moveit_core/utils/include/moveit/utils/lexical_casts.h b/moveit_core/utils/include/moveit/utils/lexical_casts.h index ce0fbcb5b5..3dc0e84ab8 100644 --- a/moveit_core/utils/include/moveit/utils/lexical_casts.h +++ b/moveit_core/utils/include/moveit/utils/lexical_casts.h @@ -34,8 +34,7 @@ /* Author: Simon Schmeisser */ -#ifndef MOVEIT_CORE_UTILS_LEXICAL_CASTS_ -#define MOVEIT_CORE_UTILS_LEXICAL_CASTS_ +#pragma once /** \file lexical_casts.h * \brief locale-agnostic conversion functions from floating point numbers to strings @@ -68,5 +67,3 @@ double toDouble(const std::string& s); float toFloat(const std::string& s); } } - -#endif diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h index 079cf49749..652d7111b6 100644 --- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h @@ -35,8 +35,7 @@ /* Author: Bryce Willey */ /** \brief convenience functions and classes used for making simple robot models for testing. */ -#ifndef MOVEIT_CORE_UTILS_TEST_ -#define MOVEIT_CORE_UTILS_TEST_ +#pragma once #include #include @@ -196,5 +195,3 @@ class RobotModelBuilder }; } } - -#endif diff --git a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_distance_field_ros_helpers.h b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_distance_field_ros_helpers.h index c90f312dc6..0495d74d2a 100644 --- a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_distance_field_ros_helpers.h +++ b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_distance_field_ros_helpers.h @@ -34,8 +34,7 @@ /** \author E. Gil Jones */ -#ifndef COLLISION_DISTANCE_FIELD_HELPERS_H_ -#define COLLISION_DISTANCE_FIELD_HELPERS_H_ +#pragma once #include #include @@ -126,4 +125,3 @@ static inline bool loadLinkBodySphereDecompositions( return true; } } -#endif diff --git a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_robot_distance_field_ros.h b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_robot_distance_field_ros.h index 8c9861b953..1fb7a58e56 100644 --- a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_robot_distance_field_ros.h +++ b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_robot_distance_field_ros.h @@ -34,8 +34,7 @@ /** \author E. Gil Jones */ -#ifndef _COLLISION_ROBOT_DISTANCE_FIELD_ROS_H_ -#define _COLLISION_ROBOT_DISTANCE_FIELD_ROS_H_ +#pragma once #include #include @@ -63,5 +62,3 @@ class CollisionRobotDistanceFieldROS : public CollisionRobotDistanceField } }; } - -#endif diff --git a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/hybrid_collision_robot_ros.h b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/hybrid_collision_robot_ros.h index 193d9debd2..8cb1c75f98 100644 --- a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/hybrid_collision_robot_ros.h +++ b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/hybrid_collision_robot_ros.h @@ -34,8 +34,7 @@ /** \author E. Gil Jones */ -#ifndef _COLLISION_ROBOT_HYBRID_ROS_H_ -#define _COLLISION_ROBOT_HYBRID_ROS_H_ +#pragma once #include #include @@ -63,5 +62,3 @@ class CollisionRobotHybridROS : public CollisionRobotHybrid } }; } - -#endif diff --git a/moveit_experimental/jog_arm/include/jog_arm/collision_check_thread.h b/moveit_experimental/jog_arm/include/jog_arm/collision_check_thread.h index 1049f26967..4b3680b5f3 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/collision_check_thread.h +++ b/moveit_experimental/jog_arm/include/jog_arm/collision_check_thread.h @@ -37,8 +37,7 @@ // /////////////////////////////////////////////////////////////////////////////// -#ifndef JOG_ARM_COLLISION_CHECK_THREAD_H -#define JOG_ARM_COLLISION_CHECK_THREAD_H +#pragma once #include #include @@ -54,5 +53,3 @@ class CollisionCheckThread pthread_mutex_t& mutex, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); }; } - -#endif // JOG_ARM_COLLISION_CHECK_THREAD_H diff --git a/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h b/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h index fa2129c8dc..cc8309dd62 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h +++ b/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h @@ -37,8 +37,7 @@ // /////////////////////////////////////////////////////////////////////////////// -#ifndef JOG_ARM_JOG_ARM_DATA_H -#define JOG_ARM_JOG_ARM_DATA_H +#pragma once #include #include @@ -93,4 +92,3 @@ struct JogArmParameters bool use_gazebo, check_collisions, publish_joint_positions, publish_joint_velocities, publish_joint_accelerations; }; } -#endif // JOG_ARM_DATA_H diff --git a/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h b/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h index ff278e76fe..f9ba6da0c9 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h +++ b/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h @@ -37,8 +37,7 @@ // /////////////////////////////////////////////////////////////////////////////// -#ifndef JOG_ARM_JOG_CALCS_H -#define JOG_ARM_JOG_CALCS_H +#pragma once #include #include @@ -135,5 +134,3 @@ class JogCalcs uint num_joints_; }; } // namespace jog_arm - -#endif // JOG_ARM_JOG_CALCS_H diff --git a/moveit_experimental/jog_arm/include/jog_arm/jog_ros_interface.h b/moveit_experimental/jog_arm/include/jog_arm/jog_ros_interface.h index 636e920afe..ef5f7d8d66 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/jog_ros_interface.h +++ b/moveit_experimental/jog_arm/include/jog_arm/jog_ros_interface.h @@ -39,8 +39,7 @@ // Server node for arm jogging with MoveIt. -#ifndef JOG_ARM_JOG_ROS_INTERFACE_H -#define JOG_ARM_JOG_ROS_INTERFACE_H +#pragma once #include #include @@ -87,5 +86,3 @@ class JogROSInterface JogArmParameters ros_parameters_; }; } // namespace jog_arm - -#endif // JOG_ARM_JOG_ROS_INTERFACE_H diff --git a/moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h b/moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h index d6b4402072..d1a3e937db 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h +++ b/moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h @@ -37,8 +37,7 @@ // /////////////////////////////////////////////////////////////////////////////// -#ifndef JOG_ARM_LOW_PASS_FILTER_H -#define JOG_ARM_LOW_PASS_FILTER_H +#pragma once namespace jog_arm { @@ -64,4 +63,3 @@ class LowPassFilter double filter_coeff_ = 10.; }; } // namespace jog_arm -#endif // JOG_ARM_LOW_PASS_FILTER_H diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache/include/kinematics_cache/kinematics_cache.h b/moveit_experimental/kinematics_cache/v1/kinematics_cache/include/kinematics_cache/kinematics_cache.h index 1d9f7fd217..952965a57e 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache/include/kinematics_cache/kinematics_cache.h +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache/include/kinematics_cache/kinematics_cache.h @@ -35,8 +35,7 @@ * Author: Sachin Chitta *********************************************************************/ -#ifndef KINEMATICS_CACHE_H_ -#define KINEMATICS_CACHE_H_ +#pragma once #include #include @@ -184,5 +183,3 @@ class KinematicsCache typedef std::shared_ptr KinematicsCachePtr; } - -#endif diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/include/kinematics_cache_ros/kinematics_cache_ros.h b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/include/kinematics_cache_ros/kinematics_cache_ros.h index ceaa9f86e6..187c133f40 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/include/kinematics_cache_ros/kinematics_cache_ros.h +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/include/kinematics_cache_ros/kinematics_cache_ros.h @@ -35,8 +35,7 @@ * Author: Sachin Chitta *********************************************************************/ -#ifndef KINEMATICS_CACHE_ROS_H_ -#define KINEMATICS_CACHE_ROS_H_ +#pragma once #include #include @@ -70,5 +69,3 @@ class KinematicsCacheROS : public kinematics_cache::KinematicsCache planning_models::RobotModelPtr kinematic_model_; /** A kinematics model */ }; } - -#endif diff --git a/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h b/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h index 0a7c793614..233ad5f27c 100644 --- a/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h +++ b/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h @@ -35,8 +35,7 @@ * Author: Sachin Chitta *********************************************************************/ -#ifndef KINEMATICS_CACHE_H_ -#define KINEMATICS_CACHE_H_ +#pragma once #include #include @@ -182,5 +181,3 @@ class KinematicsCache typedef std::shared_ptr KinematicsCachePtr; } - -#endif diff --git a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h index 2ec2f70299..4200f2761e 100644 --- a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h +++ b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h @@ -35,8 +35,7 @@ * Author: Sachin Chitta *********************************************************************/ -#ifndef MOVEIT_KINEMATICS_CONSTRAINT_AWARE_ -#define MOVEIT_KINEMATICS_CONSTRAINT_AWARE_ +#pragma once // System #include @@ -141,5 +140,3 @@ class KinematicsConstraintAware unsigned int ik_attempts_; }; } - -#endif diff --git a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h index 6d2086a08a..b54fd335e3 100644 --- a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h +++ b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h @@ -35,8 +35,7 @@ * Author: Sachin Chitta *********************************************************************/ -#ifndef MOVEIT_KINEMATICS_CONSTRAINT_AWARE_KINEMATICS_REQUEST_RESPONSE_ -#define MOVEIT_KINEMATICS_CONSTRAINT_AWARE_KINEMATICS_REQUEST_RESPONSE_ +#pragma once // System #include @@ -105,5 +104,3 @@ class KinematicsResponse bool result_; }; } - -#endif diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h index 649d89f7df..1d4a76286f 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h @@ -34,8 +34,7 @@ /* Author: Mark Moll */ -#ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_PLUGIN_ -#define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_PLUGIN_ +#pragma once #include #include @@ -348,4 +347,3 @@ class CachedMultiTipIKKinematicsPlugin : public CachedIKKinematicsPlugin -#ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_GREEDY_K_CENTERS_ -#define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_GREEDY_K_CENTERS_ +#pragma once #include #include @@ -129,5 +128,3 @@ class GreedyKCenters std::mt19937 generator_{ std::random_device{}() }; }; } - -#endif diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h index dc026872fe..e3fc410078 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h @@ -36,8 +36,7 @@ // This file is a slightly modified version of -#ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_NEAREST_NEIGHBORS_ -#define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_NEAREST_NEIGHBORS_ +#pragma once #include #include @@ -116,5 +115,3 @@ class NearestNeighbors DistanceFunction distFun_; }; } - -#endif diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h index 6d3b72f55e..dc21099fc6 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h @@ -36,8 +36,7 @@ // This file is a slightly modified version of -#ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_NEAREST_NEIGHBORS_GNAT_ -#define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_NEAREST_NEIGHBORS_GNAT_ +#pragma once #include "NearestNeighbors.h" #include "GreedyKCenters.h" @@ -720,5 +719,3 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> std::unordered_set removed_; }; } - -#endif diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp index 9cff711601..afefc41f3c 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp @@ -23,8 +23,7 @@ // linear relationship to that of another joint. // Copyright (C) 2013 Sachin Chitta, Willow Garage -#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_Mimic_HPP -#define KDL_CHAIN_IKSOLVERVEL_PINV_Mimic_HPP +#pragma once #include #include @@ -116,4 +115,3 @@ class ChainIkSolverVelMimicSVD : public ChainIkSolverVel Jacobian jac_reduced_; // reduced Jacobian with contributions of mimic joints mapped onto active DoFs }; } -#endif diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp index 6814441850..45ad0a0c47 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp @@ -34,8 +34,7 @@ /* Author: Sachin Chitta */ -#ifndef MOVEIT_KDL_KINEMATICS_PLUGIN_JOINT_MIMIC -#define MOVEIT_KDL_KINEMATICS_PLUGIN_JOINT_MIMIC +#pragma once namespace kdl_kinematics_plugin { @@ -71,5 +70,3 @@ class JointMimic } }; } - -#endif diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h index 9c4a150045..6e13e834f4 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h @@ -34,8 +34,7 @@ /* Author: Sachin Chitta, David Lu!!, Ugo Cupcic */ -#ifndef MOVEIT_ROS_PLANNING_KDL_KINEMATICS_PLUGIN_ -#define MOVEIT_ROS_PLANNING_KDL_KINEMATICS_PLUGIN_ +#pragma once // ROS #include @@ -196,5 +195,3 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase double orientation_vs_position_weight_; }; } - -#endif diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h index 0ec6c0667d..79c1155d67 100644 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h +++ b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h @@ -34,8 +34,7 @@ /* Author: Francisco Suarez-Ruiz */ -#ifndef MOVEIT_ROS_PLANNING_LMA_KINEMATICS_PLUGIN_H -#define MOVEIT_ROS_PLANNING_LMA_KINEMATICS_PLUGIN_H +#pragma once // ROS #include @@ -182,5 +181,3 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase double orientation_vs_position_weight_; }; } - -#endif diff --git a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h index 50449662fe..f0a1494648 100644 --- a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h +++ b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h @@ -39,8 +39,7 @@ \todo: better support for redundant joints */ -#ifndef MOVEIT_ROS_PLANNING_SRV_KINEMATICS_PLUGIN_ -#define MOVEIT_ROS_PLANNING_SRV_KINEMATICS_PLUGIN_ +#pragma once // ROS #include @@ -159,5 +158,3 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase std::shared_ptr ik_service_client_; }; } - -#endif diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h index 1272a130cc..464da4ab92 100644 --- a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h @@ -34,8 +34,7 @@ /* Author: E. Gil Jones */ -#ifndef CHOMP_INTERFACE_CHOMP_INTERFACE_H -#define CHOMP_INTERFACE_CHOMP_INTERFACE_H +#pragma once #include #include @@ -64,5 +63,3 @@ class CHOMPInterface : public chomp::ChompPlanner chomp::ChompParameters params_; }; } - -#endif diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h index 69169f8a66..2dee290b9b 100644 --- a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h @@ -34,8 +34,7 @@ /* Author: Chittaranjan Srinivas Swaminathan */ -#ifndef CHOMP_INTERFACE_CHOMP_PLANNING_CONTEXT_H -#define CHOMP_INTERFACE_CHOMP_PLANNING_CONTEXT_H +#pragma once #include #include @@ -65,5 +64,3 @@ class CHOMPPlanningContext : public planning_interface::PlanningContext }; } /* namespace chomp_interface */ - -#endif /* CHOMP_PLANNING_CONTEXT_H_ */ diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h index bda7cbd3f9..a60b1e1c71 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#ifndef CHOMP_COST_H_ -#define CHOMP_COST_H_ +#pragma once #include #include @@ -97,5 +96,3 @@ inline double ChompCost::getCost(Eigen::MatrixXd::ColXpr joint_trajectory) const } } // namespace chomp - -#endif /* CHOMP_COST_H_ */ diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h index fba516334b..f98009e133 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#ifndef CHOMP_OPTIMIZER_H_ -#define CHOMP_OPTIMIZER_H_ +#pragma once #include #include @@ -224,5 +223,3 @@ class ChompOptimizer bool isCurrentTrajectoryMeshToMeshCollisionFree() const; }; } - -#endif /* CHOMP_OPTIMIZER_H_ */ diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h index f1446c9d8a..ab78f274ca 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#ifndef CHOMP_PARAMETERS_H_ -#define CHOMP_PARAMETERS_H_ +#pragma once #include @@ -98,5 +97,3 @@ class ChompParameters }; } // namespace chomp - -#endif /* CHOMP_PARAMETERS_H_ */ diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h index 30d77fa7a0..a4d85fb673 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h @@ -34,8 +34,7 @@ /* Author: E. Gil Jones */ -#ifndef _CHOMP_PLANNER_H_ -#define _CHOMP_PLANNER_H_ +#pragma once #include #include @@ -55,5 +54,3 @@ class ChompPlanner planning_interface::MotionPlanDetailedResponse& res) const; }; } - -#endif diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h index 359ce88b24..559237aa12 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#ifndef CHOMP_TRAJECTORY_H_ -#define CHOMP_TRAJECTORY_H_ +#pragma once #include #include @@ -308,5 +307,3 @@ inline double ChompTrajectory::getDuration() const return duration_; } } - -#endif /* CHOMP_TRAJECTORY_H_ */ diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.h index 0b9a5214ae..b5fb5b7a66 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#ifndef CHOMP_UTILS_H_ -#define CHOMP_UTILS_H_ +#pragma once #include #include @@ -86,5 +85,3 @@ static inline double shortestAngularDistance(double start, double end) } } // namespace chomp - -#endif /* CHOMP_UTILS_H_ */ diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h index d906439cef..a1935d640e 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#ifndef MULTIVARIATE_GAUSSIAN_H_ -#define MULTIVARIATE_GAUSSIAN_H_ +#pragma once #include #include @@ -93,5 +92,3 @@ void MultivariateGaussian::sample(Eigen::MatrixBase& output) output = mean_ + covariance_cholesky_ * output; } } - -#endif /* MULTIVARIATE_GAUSSIAN_H_ */ diff --git a/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h b/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h index fba9e700d3..4e85adddb2 100644 --- a/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h +++ b/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h @@ -43,8 +43,7 @@ // Author: Blaise Gassend -#ifndef __ompl_interface_ros__OMPLDYNAMICRECONFIGURECONFIG_H__ -#define __ompl_interface_ros__OMPLDYNAMICRECONFIGURECONFIG_H__ +#pragma once #include #include @@ -532,5 +531,3 @@ inline const OMPLDynamicReconfigureConfigStatics* OMPLDynamicReconfigureConfig:: return statics; } } - -#endif // __OMPLDYNAMICRECONFIGURERECONFIGURATOR_H__ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/constraints_library.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/constraints_library.h index 7615ad1561..18c3888fc4 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/constraints_library.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/constraints_library.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_CONSTRAINTS_LIBRARY_ -#define MOVEIT_OMPL_INTERFACE_CONSTRAINTS_LIBRARY_ +#pragma once #include #include @@ -200,5 +199,3 @@ class ConstraintsLibrary std::map constraint_approximations_; }; } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h index 4eb0b16ec2..d054ab8e15 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINED_GOAL_SAMPLER_ -#define MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINED_GOAL_SAMPLER_ +#pragma once #include #include @@ -73,5 +72,3 @@ class ConstrainedGoalSampler : public ompl::base::GoalLazySamples unsigned int verbose_display_; }; } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h index a0554bfbf7..cb72a22811 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINED_SAMPLER_ -#define MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINED_SAMPLER_ +#pragma once #include #include @@ -78,5 +77,3 @@ class ConstrainedSampler : public ompl::base::StateSampler double inv_dim_; }; } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h index 7a504f7fbe..61e9b0d15d 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINED_VALID_STATE_SAMPLER_ -#define MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINED_VALID_STATE_SAMPLER_ +#pragma once #include #include @@ -70,5 +69,3 @@ class ValidConstrainedSampler : public ompl::base::ValidStateSampler ompl::RNG rng_; }; } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h index 5636257e46..ccf47358ae 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINT_APPROXIMATION_ -#define MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINT_APPROXIMATION_ +#pragma once #include #include @@ -75,5 +74,3 @@ struct ConstraintApproximation MOVEIT_DECLARE_PTR(ConstraintApproximations, std::vector) } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h index 8fd837ad9d..63c0b0917f 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DETAIL_PROJECTION_EVALUATORS_ -#define MOVEIT_OMPL_INTERFACE_DETAIL_PROJECTION_EVALUATORS_ +#pragma once #include #include @@ -83,5 +82,3 @@ class ProjectionEvaluatorJointValue : public ompl::base::ProjectionEvaluator std::vector variables_; }; } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h index c485879007..23e02f87fb 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DETAIL_STATE_VALIDITY_CHECKER_ -#define MOVEIT_OMPL_INTERFACE_DETAIL_STATE_VALIDITY_CHECKER_ +#pragma once #include #include @@ -89,5 +88,3 @@ class StateValidityChecker : public ompl::base::StateValidityChecker bool verbose_; }; } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h index a19b69bfda..a276ede06e 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DEATIL_THREADSAFE_STATE_STORAGE_ -#define MOVEIT_OMPL_INTERFACE_DEATIL_THREADSAFE_STATE_STORAGE_ +#pragma once #include #include @@ -58,4 +57,3 @@ class TSStateStorage mutable std::mutex lock_; }; } -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index 90f5034502..022aa4ab29 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_MODEL_BASED_PLANNING_CONTEXT_ -#define MOVEIT_OMPL_INTERFACE_MODEL_BASED_PLANNING_CONTEXT_ +#pragma once #include #include @@ -377,5 +376,3 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext bool simplify_solutions_; }; } // namespace ompl_interface - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h index 317272df62..5dc11716ee 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_OMPL_INTERFACE_ -#define MOVEIT_OMPL_INTERFACE_OMPL_INTERFACE_ +#pragma once #include #include @@ -193,5 +192,3 @@ class OMPLInterface constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_; }; } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h index 6b4dbb5229..4a8c9073fa 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_JOINT_SPACE_JOINT_MODEL_STATE_SPACE_ -#define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_JOINT_SPACE_JOINT_MODEL_STATE_SPACE_ +#pragma once #include @@ -49,5 +48,3 @@ class JointModelStateSpace : public ModelBasedStateSpace JointModelStateSpace(const ModelBasedStateSpaceSpecification& spec); }; } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h index 5ff8e4dab2..246f8a48e1 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_JOINT_SPACE_JOINT_MODEL_STATE_SPACE_FACTORY_ -#define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_JOINT_SPACE_JOINT_MODEL_STATE_SPACE_FACTORY_ +#pragma once #include @@ -53,5 +52,3 @@ class JointModelStateSpaceFactory : public ModelBasedStateSpaceFactory ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; }; } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h index 1bfa1379e6..106a4a2c41 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_MODEL_BASED_STATE_SPACE_ -#define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_MODEL_BASED_STATE_SPACE_ +#pragma once #include #include @@ -270,5 +269,3 @@ class ModelBasedStateSpace : public ompl::base::StateSpace double tag_snap_to_segment_complement_; }; } // namespace ompl_interface - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h index f369b52588..0bd363d9b2 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_MODEL_BASED_STATE_SPACE_FACTORY_ -#define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_MODEL_BASED_STATE_SPACE_FACTORY_ +#pragma once #include #include @@ -75,5 +74,3 @@ class ModelBasedStateSpaceFactory std::string type_; }; } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h index 9f6fbaf9ee..a0d632c527 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_WORK_SPACE_POSE_MODEL_STATE_SPACE_ -#define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_WORK_SPACE_POSE_MODEL_STATE_SPACE_ +#pragma once #include #include @@ -136,5 +135,3 @@ class PoseModelStateSpace : public ModelBasedStateSpace double jump_factor_; }; } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h index fc6d616d11..11f45ad066 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_WORK_SPACE_POSE_MODEL_STATE_SPACE_FACTORY_ -#define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_WORK_SPACE_POSE_MODEL_STATE_SPACE_FACTORY_ +#pragma once #include @@ -53,5 +52,3 @@ class PoseModelStateSpaceFactory : public ModelBasedStateSpaceFactory ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; }; } - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index 4d08768b4b..c8d2f9fcb7 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PLANNING_CONTEXT_MANAGER_ -#define MOVEIT_OMPL_INTERFACE_PLANNING_CONTEXT_MANAGER_ +#pragma once #include #include @@ -229,5 +228,3 @@ class PlanningContextManager CachedContextsPtr cached_contexts_; }; } - -#endif diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h index 8b60176b72..cf5700adf4 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h @@ -34,8 +34,7 @@ /** \Author: Benjamin Cohen /bcohen@willowgarage.com, E. Gil Jones **/ -#ifndef _SBPL_BFS_3D_H_ -#define _SBPL_BFS_3D_H_ +#pragma once #include @@ -77,5 +76,3 @@ class BFS_3D int getDistance(int, int, int); }; } - -#endif diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bresenham.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bresenham.h index 748bddf830..4f8f7847b9 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bresenham.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bresenham.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef _BRESENHAM3D_ -#define _BRESENHAM3D_ +#pragma once #include #include @@ -56,5 +55,3 @@ void get_bresenham3d_parameters(int p1x, int p1y, int p1z, int p2x, int p2y, int void get_current_point3d(bresenham3d_param_t* params, int* x, int* y, int* z); int get_next_point3d(bresenham3d_param_t* params); - -#endif diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/dummy_environment.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/dummy_environment.h index 3f9c43ba34..8dda73802e 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/dummy_environment.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/dummy_environment.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef _DUMMY_ENVIRONMENT_H_ -#define _DUMMY_ENVIRONMENT_H_ +#pragma once class DummyEnvironment : public DiscreteSpaceInformation { @@ -100,5 +99,3 @@ class DummyEnvironment : public DiscreteSpaceInformation { } }; - -#endif diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h index e33fdec39e..5158a7f49d 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h @@ -34,8 +34,7 @@ /** \Author: Benjamin Cohen /bcohen@willowgarage.com, E. Gil Jones **/ -#ifndef _ENVIRONMENT_CHAIN3D_H_ -#define _ENVIRONMENT_CHAIN3D_H_ +#pragma once #include #include @@ -334,5 +333,3 @@ inline void EnvironmentChain3D::convertJointAnglesToCoord(const std::vector #include @@ -318,5 +317,3 @@ class SingleJointMotionPrimitive : public JointMotionPrimitive double delta_; }; } - -#endif diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_interface.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_interface.h index 753211ccdb..61f5c91d80 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_interface.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_interface.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_SBPL_INTERFACE_H_ -#define MOVEIT_SBPL_INTERFACE_H_ +#pragma once #include #include @@ -68,5 +67,3 @@ class SBPLInterface // SBPLPlanner *planner_; }; } - -#endif diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_meta_interface.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_meta_interface.h index 654d41b11d..dc41e7037f 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_meta_interface.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_meta_interface.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_SBPL_META_INTERFACE_H_ -#define MOVEIT_SBPL_META_INTERFACE_H_ +#pragma once #include #include @@ -77,5 +76,3 @@ class SBPLMetaInterface PlanningStatistics last_planning_statistics_; }; } - -#endif diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_params.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_params.h index 17e309740f..56c5a53f51 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_params.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_params.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef _SBPL_PARAMS_H_ -#define _SBPL_PARAMS_H_ +#pragma once #include #include @@ -204,5 +203,3 @@ class SBPLParams std::vector motion_primitive_type_names_; }; } - -#endif diff --git a/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.h b/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.h index c9527dfacd..9d39837d79 100644 --- a/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.h +++ b/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.h @@ -34,8 +34,7 @@ /* Author: Mathias Lüdtke */ -#ifndef MOVEIT_ROS_CONTROL_INTERFACE_CONTROLLER_HANDLE_H -#define MOVEIT_ROS_CONTROL_INTERFACE_CONTROLLER_HANDLE_H +#pragma once #include #include @@ -58,5 +57,3 @@ class ControllerHandleAllocator }; } // namespace moveit_ros_control_interface - -#endif // MOVEIT_ROS_CONTROL_INTERFACE_CONTROLLER_HANDLE_H diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h index bfdee6d098..f3a5b8d059 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h @@ -35,8 +35,7 @@ /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ -#ifndef MOVEIT_PLUGINS_ACTION_BASED_CONTROLLER_HANDLE -#define MOVEIT_PLUGINS_ACTION_BASED_CONTROLLER_HANDLE +#pragma once #include #include @@ -186,5 +185,3 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase }; } // end namespace moveit_simple_controller_manager - -#endif // MOVEIT_PLUGINS_ACTION_BASED_CONTROLLER_HANDLE diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h index dc01ccf011..4675b1c332 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h @@ -35,8 +35,7 @@ /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ -#ifndef MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE -#define MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE +#pragma once #include #include @@ -77,5 +76,3 @@ class FollowJointTrajectoryControllerHandle }; } // end namespace moveit_simple_controller_manager - -#endif // MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h index e965d1b446..f081853850 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h @@ -35,8 +35,7 @@ /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ -#ifndef MOVEIT_PLUGINS_GRIPPER_CONTROLLER_HANDLE -#define MOVEIT_PLUGINS_GRIPPER_CONTROLLER_HANDLE +#pragma once #include #include @@ -218,5 +217,3 @@ class GripperControllerHandle : public ActionBasedControllerHandle @@ -226,5 +225,3 @@ class BenchmarkExecutor std::vector query_end_fns_; }; } - -#endif diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h index 0bc10321af..dbd46c4cdb 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h @@ -34,8 +34,7 @@ /* Author: Ryan Luna */ -#ifndef MOVEIT_ROS_BENCHMARK_BENCHMARK_OPTIONS_ -#define MOVEIT_ROS_BENCHMARK_BENCHMARK_OPTIONS_ +#pragma once #include #include @@ -109,5 +108,3 @@ class BenchmarkOptions moveit_msgs::WorkspaceParameters workspace_; }; } - -#endif diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h b/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h index 8058e204a4..95afa9e2b0 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h +++ b/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_PICK_PLACE_CAPABILITY_NAMES -#define MOVEIT_MOVE_GROUP_PICK_PLACE_CAPABILITY_NAMES +#pragma once #include @@ -44,5 +43,3 @@ namespace move_group static const std::string PICKUP_ACTION = "pickup"; // name of 'pickup' action static const std::string PLACE_ACTION = "place"; // name of 'place' action } - -#endif diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h index d76e6244f2..3ba4abdb2f 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h +++ b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_PICK_PLACE_ACTION_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_PICK_PLACE_ACTION_CAPABILITY_ +#pragma once #include #include @@ -101,5 +100,3 @@ class MoveGroupPickPlaceAction : public MoveGroupCapability ros::ServiceClient grasp_planning_service_; }; } - -#endif diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h index 2709de5136..fc1737229c 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_PICK_PLACE_APPROACH_AND_TRANSLATE_STAGE_ -#define MOVEIT_PICK_PLACE_APPROACH_AND_TRANSLATE_STAGE_ +#pragma once #include #include @@ -62,5 +61,3 @@ class ApproachAndTranslateStage : public ManipulationStage double jump_factor_; }; } - -#endif diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h index f1bc6a920f..12b9dbf855 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PICK_PLACE_MANIPULATION_PIPELINE_ -#define MOVEIT_PICK_PLACE_MANIPULATION_PIPELINE_ +#pragma once #include #include @@ -117,5 +116,3 @@ class ManipulationPipeline bool stop_processing_; }; } - -#endif diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h index cc3ff2fe74..a8415864cf 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_PICK_PLACE_MANIPULATION_PLAN_ -#define MOVEIT_PICK_PLACE_MANIPULATION_PLAN_ +#pragma once #include #include @@ -143,5 +142,3 @@ struct ManipulationPlan std::size_t id_; }; } - -#endif diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h index 5b36f9fb21..4b6e1f7b32 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_PICK_PLACE_MANIPULATION_STAGE_ -#define MOVEIT_PICK_PLACE_MANIPULATION_STAGE_ +#pragma once #include #include @@ -84,5 +83,3 @@ class ManipulationStage bool verbose_; }; } - -#endif diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h index 1507ebeaee..722dd3f79c 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PICK_PLACE_PICK_PLACE_ -#define MOVEIT_PICK_PLACE_PICK_PLACE_ +#pragma once #include #include @@ -165,5 +164,3 @@ class PickPlace : private boost::noncopyable, public std::enable_shared_from_thi constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_; }; } - -#endif diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h index baabe4963d..08ae38837f 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PICK_PLACE_PICK_PLACE_PARAMS_ -#define MOVEIT_PICK_PLACE_PICK_PLACE_PARAMS_ +#pragma once namespace pick_place { @@ -52,5 +51,3 @@ struct PickPlaceParams // Get access to a global variable that contains the pick & place params. const PickPlaceParams& GetGlobalPickPlaceParams(); } - -#endif diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h index 58a33e98d8..4810e44edf 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_PICK_PLACE_PLAN_STAGE_ -#define MOVEIT_PICK_PLACE_PLAN_STAGE_ +#pragma once #include #include @@ -58,5 +57,3 @@ class PlanStage : public ManipulationStage planning_pipeline::PlanningPipelinePtr planning_pipeline_; }; } - -#endif diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h index 7fe44f06bf..802c5dafe0 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_PICK_PLACE_REACHABLE_VALID_POSE_FILTER_ -#define MOVEIT_PICK_PLACE_REACHABLE_VALID_POSE_FILTER_ +#pragma once #include #include @@ -60,5 +59,3 @@ class ReachableAndValidPoseFilter : public ManipulationStage constraint_samplers::ConstraintSamplerManagerPtr constraints_sampler_manager_; }; } - -#endif diff --git a/moveit_ros/move_group/include/moveit/move_group/capability_names.h b/moveit_ros/move_group/include/moveit/move_group/capability_names.h index 7b76ff727b..53f36e07a5 100644 --- a/moveit_ros/move_group/include/moveit/move_group/capability_names.h +++ b/moveit_ros/move_group/include/moveit/move_group/capability_names.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_DEFAULT_CAPABILITY_NAMES -#define MOVEIT_MOVE_GROUP_DEFAULT_CAPABILITY_NAMES +#pragma once #include @@ -64,5 +63,3 @@ static const std::string APPLY_PLANNING_SCENE_SERVICE_NAME = static const std::string CLEAR_OCTOMAP_SERVICE_NAME = "clear_octomap"; // name of the service that can be used to clear the octomap } - -#endif diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h b/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h index e92735fe81..c34ef2363d 100644 --- a/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h +++ b/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_CAPABILITY_ +#pragma once #include #include @@ -99,5 +98,3 @@ class MoveGroupCapability MoveGroupContextPtr context_; }; } - -#endif diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h index 845e155b3f..7808a6f3ea 100644 --- a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h +++ b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_CONTEXT_ -#define MOVEIT_MOVE_GROUP_CONTEXT_ +#pragma once #include @@ -81,5 +80,3 @@ struct MoveGroupContext bool debug_; }; } - -#endif diff --git a/moveit_ros/move_group/include/moveit/move_group/node_name.h b/moveit_ros/move_group/include/moveit/move_group/node_name.h index 220dca0498..79d0ca4f16 100644 --- a/moveit_ros/move_group/include/moveit/move_group/node_name.h +++ b/moveit_ros/move_group/include/moveit/move_group/node_name.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_NODE_NAME -#define MOVEIT_MOVE_GROUP_NODE_NAME +#pragma once #include @@ -43,5 +42,3 @@ namespace move_group { static const std::string NODE_NAME = "move_group"; // name of node } - -#endif diff --git a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h index 3bcd7aeec2..9970cc8fef 100644 --- a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h @@ -34,8 +34,7 @@ /* Author: Michael 'v4hn' Goerner */ -#ifndef MOVEIT_MOVE_GROUP_APPLY_PLANNING_SCENE_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_APPLY_PLANNING_SCENE_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -59,5 +58,3 @@ class ApplyPlanningSceneService : public MoveGroupCapability ros::ServiceServer service_; }; } - -#endif // MOVEIT_MOVE_GROUP_APPLY_PLANNING_SCENE_SERVICE_CAPABILITY_ diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h index cf1fbe9a47..e90efe670c 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_CARTESIAN_PATH_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_CARTESIAN_PATH_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -57,5 +56,3 @@ class MoveGroupCartesianPathService : public MoveGroupCapability bool display_computed_paths_; }; } - -#endif diff --git a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h index 401a31f8f6..2c5e68b542 100644 --- a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h @@ -34,8 +34,7 @@ /* Author: David Hershberger */ -#ifndef MOVEIT_MOVE_GROUP_CLEAR_OCTOMAP_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_CLEAR_OCTOMAP_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -55,5 +54,3 @@ class ClearOctomapService : public MoveGroupCapability ros::ServiceServer service_; }; } - -#endif // MOVEIT_MOVE_GROUP_CLEAR_OCTOMAP_SERVICE_CAPABILITY_ diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.h b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.h index c143b1e20b..f98965c8ba 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.h @@ -38,8 +38,7 @@ * Author: Kentaro Wada * */ -#ifndef MOVEIT_MOVE_GROUP_EXECUTE_TRAJECTORY_ACTION_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_EXECUTE_TRAJECTORY_ACTION_CAPABILITY_ +#pragma once #include #include @@ -66,5 +65,3 @@ class MoveGroupExecuteTrajectoryAction : public MoveGroupCapability }; } // namespace move_group - -#endif // MOVEIT_MOVE_GROUP_EXECUTE_TRAJECTORY_ACTION_CAPABILITY_ diff --git a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h index 63fd7c35e7..c1cbfde2a7 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_GET_PLANNING_SCENE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_GET_PLANNING_SCENE_CAPABILITY_ +#pragma once #include #include @@ -56,5 +55,3 @@ class MoveGroupGetPlanningSceneService : public MoveGroupCapability ros::ServiceServer get_scene_service_; }; } - -#endif diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h index d1f10a96a5..30e300ef47 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_KINEMATICS_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_KINEMATICS_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -63,5 +62,3 @@ class MoveGroupKinematicsService : public MoveGroupCapability ros::ServiceServer ik_service_; }; } - -#endif diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.h b/moveit_ros/move_group/src/default_capabilities/move_action_capability.h index d8af647a38..56917e6a67 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_MOVE_ACTION_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_MOVE_ACTION_CAPABILITY_ +#pragma once #include #include @@ -71,5 +70,3 @@ class MoveGroupMoveAction : public MoveGroupCapability bool preempt_requested_; }; } - -#endif diff --git a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h index b3d7a94250..6ca33b6791 100644 --- a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_PLAN_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_PLAN_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -55,5 +54,3 @@ class MoveGroupPlanService : public MoveGroupCapability ros::ServiceServer plan_service_; }; } - -#endif diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h index 656c228a08..5d3a8b632f 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Robert Haschke */ -#ifndef MOVEIT_MOVE_GROUP_QUERY_PLANNERS_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_QUERY_PLANNERS_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -63,5 +62,3 @@ class MoveGroupQueryPlannersService : public MoveGroupCapability ros::ServiceServer set_service_; }; } - -#endif diff --git a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h index 2806a9586c..86dc6c8d52 100644 --- a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_STATE_VALIDATION_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_STATE_VALIDATION_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -55,5 +54,3 @@ class MoveGroupStateValidationService : public MoveGroupCapability ros::ServiceServer validity_service_; }; } - -#endif diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h index e1cff913dd..70413b3d2c 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Jon Binney */ -#ifndef MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_ -#define MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_ +#pragma once #include #include @@ -116,5 +115,3 @@ class OccMapTree : public octomap::OcTree typedef std::shared_ptr OccMapTreePtr; typedef std::shared_ptr OccMapTreeConstPtr; } - -#endif diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h index 1558c434ae..ece0658606 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Jon Binney */ -#ifndef MOVEIT_PERCEPTION_OCCUPANCY_MAP_MONITOR_ -#define MOVEIT_PERCEPTION_OCCUPANCY_MAP_MONITOR_ +#pragma once #include #include @@ -160,5 +159,3 @@ class OccupancyMapMonitor bool active_; }; } - -#endif diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h index cd91709567..07c2a0b0c8 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Jon Binney */ -#ifndef MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_UPDATER_ -#define MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_UPDATER_ +#pragma once #include #include @@ -112,5 +111,3 @@ class OccupancyMapUpdater static void readXmlParam(XmlRpc::XmlRpcValue& params, const std::string& param_name, unsigned int* value); }; } - -#endif diff --git a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h index d223e8d4b4..199c79204d 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h +++ b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OCCUPANCY_MAP_DEPTH_IMAGE_OCCUPANCY_MAP_UPDATER_ -#define MOVEIT_OCCUPANCY_MAP_DEPTH_IMAGE_OCCUPANCY_MAP_UPDATER_ +#pragma once #include #include @@ -107,5 +106,3 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater ros::WallTime last_depth_callback_start_; }; } - -#endif diff --git a/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h b/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h index cad8b0a48c..270ad565f1 100644 --- a/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h +++ b/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OCCUPANCY_MAP_MONITOR_LAZY_FREE_SPACE_UPDATER_ -#define MOVEIT_OCCUPANCY_MAP_MONITOR_LAZY_FREE_SPACE_UPDATER_ +#pragma once #include #include @@ -89,5 +88,3 @@ class LazyFreeSpaceUpdater boost::thread process_thread_; }; } - -#endif /* MOVEIT_OCCUPANCY_MAP_UPDATER_H_ */ diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h index 6ab4d6ab37..323bc03326 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_DEPTH_SELF_FILTER_NODELET_ -#define MOVEIT_DEPTH_SELF_FILTER_NODELET_ +#pragma once #include #include @@ -132,5 +131,3 @@ class DepthSelfFiltering : public nodelet::Nodelet }; } // namespace mesh_filter - -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h index a1f098f244..a817245040 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_FILTER_JOB_ -#define MOVEIT_MESH_FILTER_FILTER_JOB_ +#pragma once #include #include @@ -142,4 +141,3 @@ class FilterJob : public Job boost::function exec_; }; } -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h index 2ba940d7f7..a605425870 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_GLMESH_ -#define MOVEIT_MESH_FILTER_GLMESH_ +#pragma once #include #include @@ -85,4 +84,3 @@ class GLMesh unsigned int mesh_label_; }; } // namespace mesh_filter -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h index 67fa5abb1b..8ae3ee9f55 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_GLRENDERER_ -#define MOVEIT_MESH_FILTER_GLRENDERER_ +#pragma once #include #include @@ -303,4 +302,3 @@ class GLRenderer static bool glutInitialized_; }; } // namespace mesh_filter -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h index e488aa2883..ed27f7b136 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_MESHFILTER_ -#define MOVEIT_MESH_FILTER_MESHFILTER_ +#pragma once #include #include @@ -112,4 +111,3 @@ const typename SensorType::Parameters& MeshFilter::parameters() cons } } // namespace mesh_filter -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h index 86f85270e0..dc8ee5f3f2 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_MESH_FILTER_BASE_ -#define MOVEIT_MESH_FILTER_MESH_FILTER_BASE_ +#pragma once #include #include @@ -294,5 +293,3 @@ class MeshFilterBase float shadow_threshold_; }; } // namespace mesh_filter - -#endif /* __MESH_FILTER_MESH_FILTER_BASE_H__ */ diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h index 87239b0bd2..8c76cdccef 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_SENSOR_MODEL_ -#define MOVEIT_MESH_FILTER_SENSOR_MODEL_ +#pragma once #include #include @@ -170,5 +169,3 @@ class SensorModel virtual ~SensorModel(); }; } // namespace mesh_filter - -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h index 2dfac68ad6..d6dc185be1 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_STEREO_CAMERA_MODEL_ -#define MOVEIT_MESH_FILTER_STEREO_CAMERA_MODEL_ +#pragma once #include #include @@ -164,4 +163,3 @@ class StereoCameraModel : public SensorModel static const std::string FILTER_FRAGMENT_SHADER_SOURCE; }; } // namespace mesh_filter -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h index fe5547edad..eb03286787 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_TRANSFORM_PROVIDER_ -#define MOVEIT_MESH_FILTER_TRANSFORM_PROVIDER_ +#pragma once #include #include @@ -167,4 +166,3 @@ class TransformProvider /** \brief update interval in micro seconds*/ unsigned long interval_us_; }; -#endif diff --git a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h index 2b6ee820bd..2fdf49c7b7 100644 --- a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h +++ b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_POINT_CONTAINMENT_FILTER_SELF_MASK_ -#define MOVEIT_POINT_CONTAINMENT_FILTER_SELF_MASK_ +#pragma once #include #include @@ -129,5 +128,3 @@ class ShapeMask std::vector bspheres_; }; } - -#endif diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h index cf56d68dfe..80ecbfaa11 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h @@ -34,8 +34,7 @@ /* Author: Jon Binney, Ioan Sucan */ -#ifndef MOVEIT_PERCEPTION_POINTCLOUD_OCTOMAP_UPDATER_ -#define MOVEIT_PERCEPTION_POINTCLOUD_OCTOMAP_UPDATER_ +#pragma once #include #include @@ -101,5 +100,3 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater std::vector mask_; }; } - -#endif diff --git a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h index bed034f0c9..b53e280328 100644 --- a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h +++ b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h @@ -34,8 +34,7 @@ /* Author: Sachin Chitta */ -#ifndef MOVEIT_SEMANTIC_WORLD_ -#define MOVEIT_SEMANTIC_WORLD_ +#pragma once #include #include @@ -164,5 +163,3 @@ class SemanticWorld }; } } - -#endif diff --git a/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h b/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h index 4527ae450e..b5709e824a 100644 --- a/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h +++ b/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_COLLISION_PLUGIN_LOADER_COLLISION_PLUGIN_LOADER_H -#define MOVEIT_COLLISION_PLUGIN_LOADER_COLLISION_PLUGIN_LOADER_H +#pragma once #include #include @@ -68,5 +67,3 @@ class CollisionPluginLoader }; } // namespace collision_detection - -#endif // MOVEIT_COLLISION_PLUGIN_LOADER_COLLISION_PLUGIN_LOADER_H diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h b/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h index 5d24813893..84246d1851 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h +++ b/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLER_MANAGER_LOADER_ -#define MOVEIT_CONSTRAINT_SAMPLER_MANAGER_LOADER_ +#pragma once #include #include @@ -62,5 +61,3 @@ class ConstraintSamplerManagerLoader HelperPtr impl_; }; } - -#endif diff --git a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h index 9bc06594e3..f4c6b0a356 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h +++ b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Dave Coleman */ -#ifndef MOVEIT_KINEMATICS_PLUGIN_LOADER_ -#define MOVEIT_KINEMATICS_PLUGIN_LOADER_ +#pragma once #include #include @@ -115,5 +114,3 @@ class KinematicsPluginLoader double default_solver_timeout_; }; } - -#endif diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h index e673c04031..11023522a9 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLAN_EXECUTION_PLAN_EXECUTION_ -#define MOVEIT_PLAN_EXECUTION_PLAN_EXECUTION_ +#pragma once #include #include @@ -166,4 +165,3 @@ class PlanExecution DynamicReconfigureImpl* reconfigure_impl_; }; } -#endif diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h index c400874990..809cb259c7 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLAN_EXECUTION_PLAN_REPRESENTATION_ -#define MOVEIT_PLAN_EXECUTION_PLAN_REPRESENTATION_ +#pragma once #include #include @@ -83,4 +82,3 @@ struct ExecutableMotionPlan /// The signature of a function that can compute a motion plan typedef boost::function ExecutableMotionPlanComputationFn; } -#endif diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h index 55a7d57f8a..b0b468cd19 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLAN_EXECUTION_PLAN_WITH_SENSING_ -#define MOVEIT_PLAN_EXECUTION_PLAN_WITH_SENSING_ +#pragma once #include #include @@ -135,4 +134,3 @@ class PlanWithSensing DynamicReconfigureImpl* reconfigure_impl_; }; } -#endif diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index f08af40a04..313f770999 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_PIPELINE_PLANNING_PIPELINE_ -#define MOVEIT_PLANNING_PIPELINE_PLANNING_PIPELINE_ +#pragma once #include #include @@ -197,5 +196,3 @@ class PlanningPipeline MOVEIT_CLASS_FORWARD(PlanningPipeline); } - -#endif diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h index 709586792e..806db00daf 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_SCENE_MONITOR_CURRENT_STATE_MONITOR_ -#define MOVEIT_PLANNING_SCENE_MONITOR_CURRENT_STATE_MONITOR_ +#pragma once #include #include @@ -213,5 +212,3 @@ class CurrentStateMonitor MOVEIT_CLASS_FORWARD(CurrentStateMonitor); } - -#endif diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index d03afd28ec..49db508c9f 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_ -#define MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_ +#pragma once #include #include @@ -702,5 +701,3 @@ class LockedPlanningSceneRW : public LockedPlanningSceneRO } }; } - -#endif diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h index 76e5e4562c..f7638161a7 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_SCENE_MONITOR_TRAJECTORY_MONITOR_ -#define MOVEIT_PLANNING_SCENE_MONITOR_TRAJECTORY_MONITOR_ +#pragma once #include #include @@ -107,5 +106,3 @@ class TrajectoryMonitor TrajectoryStateAddedCallback state_add_callback_; }; } - -#endif diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h index 76efd13c8e..bf506a96bf 100644 --- a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Mathias Lüdtke, Dave Coleman */ -#ifndef MOVEIT_PLANNING_RDF_LOADER_ -#define MOVEIT_PLANNING_RDF_LOADER_ +#pragma once #include #include @@ -102,4 +101,3 @@ class RDFLoader urdf::ModelInterfaceSharedPtr urdf_; }; } -#endif diff --git a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h index def0d10d75..c39d9f1056 100644 --- a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h +++ b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_MODELS_LOADER_ROBOT_MODEL_LOADER_ -#define MOVEIT_PLANNING_MODELS_LOADER_ROBOT_MODEL_LOADER_ +#pragma once #include #include @@ -134,4 +133,3 @@ class RobotModelLoader kinematics_plugin_loader::KinematicsPluginLoaderPtr kinematics_loader_; }; } -#endif diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index ea2f1a0d6d..1800068f42 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_TRAJECTORY_EXECUTION_MANAGER_TRAJECTORY_EXECUTION_MANAGER_ -#define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_TRAJECTORY_EXECUTION_MANAGER_ +#pragma once #include #include @@ -362,5 +361,3 @@ class TrajectoryExecutionManager bool wait_for_trajectory_completion_; }; } - -#endif diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h index 731d2e65c4..46584f00cd 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef TEST_MOVEIT_CONTROLLER_MANAGER_ -#define TEST_MOVEIT_CONTROLLER_MANAGER_ +#pragma once #include @@ -160,4 +159,3 @@ class TestMoveItControllerManager : public moveit_controller_manager::MoveItCont std::map > controller_joints_; }; } -#endif diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h index bffa91d836..10248a0f96 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h +++ b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_INTERFACE_COMMON_OBJECTS_ -#define MOVEIT_PLANNING_INTERFACE_COMMON_OBJECTS_ +#pragma once #include @@ -73,5 +72,3 @@ planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot } // namespace planning interface } // namespace moveit - -#endif // end of MOVEIT_PLANNING_INTERFACE_COMMON_OBJECTS_ diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 1f919b0960..ae4d3a3b7d 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -35,8 +35,7 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_INTERFACE_ -#define MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_INTERFACE_ +#pragma once #include #include @@ -1003,5 +1002,3 @@ class MoveGroupInterface }; } // namespace planning_interface } // namespace moveit - -#endif diff --git a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index d421b5484a..9dc25385e0 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_INTERFACE_PLANNING_SCENE_INTERFACE_ -#define MOVEIT_PLANNING_INTERFACE_PLANNING_SCENE_INTERFACE_ +#pragma once #include #include @@ -146,5 +145,3 @@ class PlanningSceneInterface }; } } - -#endif diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h index fbf4d191ff..47825360ba 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PY_BINDINGS_TOOLS_PY_CONVERSIONS_ -#define MOVEIT_PY_BINDINGS_TOOLS_PY_CONVERSIONS_ +#pragma once #include #include @@ -95,5 +94,3 @@ boost::python::list listFromString(const std::vector& v) } } } - -#endif diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h index e1e54a6bc9..1effb467a5 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PY_BINDINGS_TOOLS_ROSCPP_INITIALIZER_ -#define MOVEIT_PY_BINDINGS_TOOLS_ROSCPP_INITIALIZER_ +#pragma once #include #include @@ -73,5 +72,3 @@ void roscpp_init(); void roscpp_shutdown(); } } - -#endif diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h index 5000fccbbe..5b9a9a306f 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PY_BINDINGS_TOOLS_SERIALIZE_MSG_ -#define MOVEIT_PY_BINDINGS_TOOLS_SERIALIZE_MSG_ +#pragma once #include @@ -71,5 +70,3 @@ void deserializeMsg(const std::string& data, T& msg) } } } - -#endif diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h index 1fdafbf667..b0027cf0f2 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley */ -#ifndef MOVEIT_ROBOT_INTERACTION_INTERACTION_ -#define MOVEIT_ROBOT_INTERACTION_INTERACTION_ +#pragma once #include #include @@ -175,5 +174,3 @@ struct JointInteraction double size; }; } - -#endif diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h index 5cc6ed89a5..3ea644e86a 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Adam Leeper */ -#ifndef MOVEIT_ROBOT_INTERACTION_INTERACTION_HANDLER_ -#define MOVEIT_ROBOT_INTERACTION_INTERACTION_HANDLER_ +#pragma once #include #include @@ -311,5 +310,3 @@ class InteractionHandler : public LockedRobotState static std::string fixName(std::string name); }; } - -#endif diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h index 5810e7d244..16e821eab0 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Acorn Pooley, Adam Leeper */ -#ifndef MOVEIT_ROBOT_INTERACTION_INTERACTIVE_MARKER_HELPERS_ -#define MOVEIT_ROBOT_INTERACTION_INTERACTIVE_MARKER_HELPERS_ +#pragma once #include #include @@ -69,5 +68,3 @@ void addPositionControl(visualization_msgs::InteractiveMarker& int_marker, bool void addViewPlaneControl(visualization_msgs::InteractiveMarker& int_marker, double radius, const std_msgs::ColorRGBA& color, bool position = true, bool orientation = true); } - -#endif diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h index 43b8e7bbc2..8a0248a60a 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley */ -#ifndef MOVEIT_ROBOT_INTERACTION_KINEMATIC_OPTIONS_ -#define MOVEIT_ROBOT_INTERACTION_KINEMATIC_OPTIONS_ +#pragma once #include #include @@ -89,5 +88,3 @@ struct KinematicOptions kinematics::KinematicsQueryOptions options_; }; } - -#endif diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h index 033fd0ac4b..f9c6a986b9 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley */ -#ifndef MOVEIT_ROBOT_INTERACTION_KINEMATIC_OPTIONS_MAP_ -#define MOVEIT_ROBOT_INTERACTION_KINEMATIC_OPTIONS_MAP_ +#pragma once #include #include @@ -104,5 +103,3 @@ class KinematicOptionsMap M_options options_; }; } - -#endif diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h index 286a86a95d..fd6b3f3616 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h @@ -35,8 +35,7 @@ /* Author: Ioan Sucan, Acorn Pooley */ -#ifndef MOVEIT_ROBOT_INTERACTION_LOCKED_ROBOT_STATE_ -#define MOVEIT_ROBOT_INTERACTION_LOCKED_ROBOT_STATE_ +#pragma once #include #include @@ -109,5 +108,3 @@ class LockedRobotState robot_state::RobotStatePtr state_; }; } - -#endif diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h index a5ebcc0001..494386af0f 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Adam Leeper */ -#ifndef MOVEIT_ROBOT_INTERACTION_ROBOT_INTERACTION_ -#define MOVEIT_ROBOT_INTERACTION_ROBOT_INTERACTION_ +#pragma once #include #include @@ -234,5 +233,3 @@ class RobotInteraction KinematicOptionsMapPtr kinematic_options_map_; }; } - -#endif diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index a8c30c9cb0..52cd9b5640 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Dave Coleman, Adam Leeper, Sachin Chitta */ -#ifndef MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_DISPLAY_ -#define MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_DISPLAY_ +#pragma once #include #include @@ -301,5 +300,3 @@ private Q_SLOTS: }; } // namespace moveit_rviz_plugin - -#endif diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index d49e100204..c212905f5f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_FRAME_ -#define MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_FRAME_ +#pragma once #include #include @@ -365,5 +364,3 @@ void MotionPlanningFrame::waitForAction(const T& action, const ros::NodeHandle& ROS_DEBUG("Connected to '%s'", name.c_str()); }; } - -#endif diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h index 91242225c4..99658c4f2d 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h @@ -34,8 +34,7 @@ /* Author: Robert Haschke */ -#ifndef MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_PARAM_WIDGET_ -#define MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_PARAM_WIDGET_ +#pragma once #include #include @@ -77,5 +76,3 @@ private Q_SLOTS: std::string planner_id_; }; } - -#endif diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index b18c4a0140..06e30bef58 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_PLUGIN_SCENE_DISPLAY_ -#define MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_PLUGIN_SCENE_DISPLAY_ +#pragma once #include @@ -213,5 +212,3 @@ protected Q_SLOTS: }; } // namespace moveit_rviz_plugin - -#endif diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h index c1692d94d7..e087dd3a62 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h +++ b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_VISUALIZATION_ROBOT_STATE_DISPLAY_RVIZ_ROBOT_STATE_DISPLAY_ -#define MOVEIT_VISUALIZATION_ROBOT_STATE_DISPLAY_RVIZ_ROBOT_STATE_DISPLAY_ +#pragma once #include @@ -146,5 +145,3 @@ private Q_SLOTS: }; } // namespace moveit_rviz_plugin - -#endif diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h index 92bd6dd2bb..9c4f3c60b2 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h @@ -34,8 +34,7 @@ /* Author: Julius Kammerl */ -#ifndef MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_OCTOMAP_RENDER_ -#define MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_OCTOMAP_RENDER_ +#pragma once #include #include @@ -100,4 +99,3 @@ class OcTreeRender std::size_t octree_depth_; }; } -#endif diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h index 0f8f417f37..0d757ef7c5 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_PLANNING_LINK_UPDATER_ -#define MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_PLANNING_LINK_UPDATER_ +#pragma once #include #include @@ -58,5 +57,3 @@ class PlanningLinkUpdater : public rviz::LinkUpdater robot_state::RobotStateConstPtr kinematic_state_; }; } - -#endif diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h index 84106db313..987425752c 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_PLUGIN_PLANNING_SCENE_RENDER_ -#define MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_PLUGIN_PLANNING_SCENE_RENDER_ +#pragma once #include #include @@ -88,5 +87,3 @@ class PlanningSceneRender RobotStateVisualizationPtr scene_robot_; }; } - -#endif diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h index fa891bcccd..808d91b710 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_RENDER_SHAPES_ -#define MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_RENDER_SHAPES_ +#pragma once #include #include @@ -79,5 +78,3 @@ class RenderShapes std::vector octree_voxel_grids_; }; } - -#endif diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index 24f246dcc3..49b299f1d1 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_ROBOT_STATE_VISUALIZATION_ -#define MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_ROBOT_STATE_VISUALIZATION_ +#pragma once #include #include @@ -105,5 +104,3 @@ class RobotStateVisualization bool collision_visible_; }; } - -#endif diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h index e4c7d731d1..c552f7ec84 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h @@ -34,8 +34,7 @@ /* Author: Yannick Jonetzko */ -#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TRAJECTORY_PANEL_ -#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TRAJECTORY_PANEL_ +#pragma once #ifndef Q_MOC_RUN #include @@ -93,5 +92,3 @@ private Q_SLOTS: }; } // namespace moveit_rviz_plugin - -#endif diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h index 1c309a46d7..23aa020bf6 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_VISUALIZATION -#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_VISUALIZATION +#pragma once #include #include @@ -170,5 +169,3 @@ private Q_SLOTS: }; } // namespace moveit_rviz_plugin - -#endif diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h index 209778d44f..d4c0ab28a8 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h +++ b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h @@ -36,8 +36,7 @@ Desc: Wraps a trajectory_visualization playback class for Rviz into a stand alone display */ -#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_DISPLAY -#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_DISPLAY +#pragma once #include @@ -96,5 +95,3 @@ private Q_SLOTS: }; } // namespace moveit_rviz_plugin - -#endif diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h index 4bee5469a5..7e507bb257 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_CONSTRAINTS_STORAGE_ -#define MOVEIT_MOVEIT_WAREHOUSE_CONSTRAINTS_STORAGE_ +#pragma once #include "moveit/warehouse/moveit_message_storage.h" #include @@ -84,5 +83,3 @@ class ConstraintsStorage : public MoveItMessageStorage ConstraintsCollection constraints_collection_; }; } - -#endif diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/moveit_message_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/moveit_message_storage.h index 6b8b179258..f6b6be0c3e 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/moveit_message_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/moveit_message_storage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_MOVEIT_MESSAGE_STORAGE_ -#define MOVEIT_MOVEIT_WAREHOUSE_MOVEIT_MESSAGE_STORAGE_ +#pragma once #include #include @@ -66,5 +65,3 @@ class MoveItMessageStorage /// \brief Load a database connection typename warehouse_ros::DatabaseConnection::Ptr loadDatabase(); } - -#endif diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h index ec7d65c4b3..1878942ecd 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_STORAGE_ -#define MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_STORAGE_ +#pragma once #include "moveit/warehouse/moveit_message_storage.h" #include @@ -120,5 +119,3 @@ class PlanningSceneStorage : public MoveItMessageStorage RobotTrajectoryCollection robot_trajectory_collection_; }; } - -#endif diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_world_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_world_storage.h index bafb6ab688..fec6384e54 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_world_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_world_storage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_WORLD_STORAGE_ -#define MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_WORLD_STORAGE_ +#pragma once #include #include @@ -73,5 +72,3 @@ class PlanningSceneWorldStorage : public MoveItMessageStorage PlanningSceneWorldCollection planning_scene_world_collection_; }; } - -#endif diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h index fed8808143..ce592f9fa0 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_STATE_STORAGE_ -#define MOVEIT_MOVEIT_WAREHOUSE_STATE_STORAGE_ +#pragma once #include #include @@ -79,5 +78,3 @@ class RobotStateStorage : public MoveItMessageStorage RobotStateCollection state_collection_; }; } - -#endif diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h index 1b0cbe501d..7246618a41 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h @@ -34,8 +34,7 @@ /* Author: Mario Prats, Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_TRAJECTORY_CONSTRAINTS_STORAGE_ -#define MOVEIT_MOVEIT_WAREHOUSE_TRAJECTORY_CONSTRAINTS_STORAGE_ +#pragma once #include "moveit/warehouse/moveit_message_storage.h" #include @@ -87,5 +86,3 @@ class TrajectoryConstraintsStorage : public MoveItMessageStorage TrajectoryConstraintsCollection constraints_collection_; }; } - -#endif diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/warehouse_connector.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/warehouse_connector.h index d61df45bae..3555097ccd 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/warehouse_connector.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/warehouse_connector.h @@ -34,8 +34,7 @@ /* Author: E. Gil Jones */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_WAREHOUSE_CONNECTOR_ -#define MOVEIT_MOVEIT_WAREHOUSE_WAREHOUSE_CONNECTOR_ +#pragma once #include @@ -55,5 +54,3 @@ class WarehouseConnector int child_pid_; }; } - -#endif diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h index 207e24bd13..c19bc2e0f5 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_TOOLS_COMPUTE_DEFAULT_COLLISIONS_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_TOOLS_COMPUTE_DEFAULT_COLLISIONS_ +#pragma once #include #include @@ -110,5 +109,3 @@ const std::string disabledReasonToString(DisabledReason reason); */ DisabledReason disabledReasonFromString(const std::string& reason); } - -#endif diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index c30ad62283..0e42bac586 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_TOOLS_MOVEIT_CONFIG_DATA_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_TOOLS_MOVEIT_CONFIG_DATA_ +#pragma once #include #include // for getting kinematic model @@ -525,5 +524,3 @@ class MoveItConfigData }; } // namespace - -#endif diff --git a/moveit_setup_assistant/src/tools/collision_linear_model.h b/moveit_setup_assistant/src/tools/collision_linear_model.h index 9744170243..9b702fcc73 100644 --- a/moveit_setup_assistant/src/tools/collision_linear_model.h +++ b/moveit_setup_assistant/src/tools/collision_linear_model.h @@ -34,8 +34,7 @@ /* Author: Robert Haschke */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_COLLISION_LINEAR_MODEL_H -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_COLLISION_LINEAR_MODEL_H +#pragma once #include #include @@ -100,5 +99,3 @@ private Q_SLOTS: QVector sort_columns_; // sorting history QVector sort_orders_; // corresponding sort orders }; - -#endif // MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_COLLISION_LINEAR_MODEL_H diff --git a/moveit_setup_assistant/src/tools/collision_matrix_model.h b/moveit_setup_assistant/src/tools/collision_matrix_model.h index dccbc2eed1..ed5d67e894 100644 --- a/moveit_setup_assistant/src/tools/collision_matrix_model.h +++ b/moveit_setup_assistant/src/tools/collision_matrix_model.h @@ -34,8 +34,7 @@ /* Author: Robert Haschke */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_COLLISION_MATRIX_MODEL_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_COLLISION_MATRIX_MODEL_ +#pragma once #include @@ -79,5 +78,3 @@ public Q_SLOTS: QList q_names; // names of links QList visual_to_index; // map from visual index to actual index }; - -#endif // MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_COLLISION_MATRIX_MODEL_ diff --git a/moveit_setup_assistant/src/tools/rotated_header_view.h b/moveit_setup_assistant/src/tools/rotated_header_view.h index 27854b7140..5bf288d84c 100644 --- a/moveit_setup_assistant/src/tools/rotated_header_view.h +++ b/moveit_setup_assistant/src/tools/rotated_header_view.h @@ -34,8 +34,7 @@ /* Author: Robert Haschke */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROTATED_HEADERVIEW_H -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROTATED_HEADERVIEW_H +#pragma once #include @@ -50,5 +49,3 @@ class RotatedHeaderView : public QHeaderView int sectionSizeHint(int logicalIndex) const; }; } - -#endif // MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROTATED_HEADERVIEW_H diff --git a/moveit_setup_assistant/src/widgets/author_information_widget.h b/moveit_setup_assistant/src/widgets/author_information_widget.h index 15ed0036ec..94763edf93 100644 --- a/moveit_setup_assistant/src/widgets/author_information_widget.h +++ b/moveit_setup_assistant/src/widgets/author_information_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman, Michael 'v4hn' Goerner */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_AUTHOR_INFORMATION_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_AUTHOR_INFORMATION_WIDGET_ +#pragma once #include #include @@ -86,5 +85,3 @@ private Q_SLOTS: }; } // namespace moveit_setup_assistant - -#endif diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.h b/moveit_setup_assistant/src/widgets/configuration_files_widget.h index f0787af004..da0946e4dd 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.h +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_CONFIGURATION_FILES_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_CONFIGURATION_FILES_WIDGET_ +#pragma once #include #include @@ -205,5 +204,3 @@ private Q_SLOTS: }; } // namespace moveit_setup_assistant - -#endif diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.h b/moveit_setup_assistant/src/widgets/controller_edit_widget.h index 2113005c8a..f7a2679164 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.h +++ b/moveit_setup_assistant/src/widgets/controller_edit_widget.h @@ -33,8 +33,7 @@ /* Author: Mohamad Ayman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_CONTROLLER_EDIT_WIDGET_H -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_CONTROLLER_EDIT_WIDGET_H +#pragma once #include #include @@ -142,5 +141,3 @@ private Q_SLOTS: moveit_setup_assistant::MoveItConfigDataPtr config_data_; }; } - -#endif diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.h b/moveit_setup_assistant/src/widgets/default_collisions_widget.h index cef78d3c0b..d69cedefd3 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.h +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_DEFAULT_COLLISIONS_WIDGET__ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_DEFAULT_COLLISIONS_WIDGET__ +#pragma once #include #include @@ -257,5 +256,3 @@ class MonitorThread : public QThread bool canceled_; }; } - -#endif diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.h b/moveit_setup_assistant/src/widgets/double_list_widget.h index c4e511bd14..1d983134aa 100644 --- a/moveit_setup_assistant/src/widgets/double_list_widget.h +++ b/moveit_setup_assistant/src/widgets/double_list_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_DOUBLE_LIST_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_DOUBLE_LIST_WIDGET_ +#pragma once #include #include @@ -141,5 +140,3 @@ private Q_SLOTS: }; } // namespace moveit_setup_assistant - -#endif diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.h b/moveit_setup_assistant/src/widgets/end_effectors_widget.h index 1c5b17247c..a6be88ed6f 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.h +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_END_EFFECTORS_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_END_EFFECTORS_WIDGET_ +#pragma once // Qt #include @@ -184,5 +183,3 @@ private Q_SLOTS: }; } // namespace - -#endif diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.h b/moveit_setup_assistant/src/widgets/group_edit_widget.h index e5ae0ec884..60a6a67b6e 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.h +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_GROUP_EDIT_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_GROUP_EDIT_WIDGET_ +#pragma once #include #include @@ -127,5 +126,3 @@ private Q_SLOTS: // ****************************************************************************************** }; } - -#endif diff --git a/moveit_setup_assistant/src/widgets/header_widget.h b/moveit_setup_assistant/src/widgets/header_widget.h index 40793d36a0..a5661beb83 100644 --- a/moveit_setup_assistant/src/widgets/header_widget.h +++ b/moveit_setup_assistant/src/widgets/header_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_HEADER_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_HEADER_WIDGET_ +#pragma once #include #include @@ -119,5 +118,3 @@ class LoadPathArgsWidget : public LoadPathWidget void setArgsEnabled(bool enabled = true); }; } - -#endif diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h index da63182675..4c5764cdfd 100644 --- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h +++ b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_KINEMATIC_CHAIN_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_KINEMATIC_CHAIN_WIDGET_ +#pragma once #include #include @@ -135,5 +134,3 @@ private Q_SLOTS: // ****************************************************************************************** }; } - -#endif diff --git a/moveit_setup_assistant/src/widgets/navigation_widget.h b/moveit_setup_assistant/src/widgets/navigation_widget.h index 962a7d1dc0..76084c869e 100644 --- a/moveit_setup_assistant/src/widgets/navigation_widget.h +++ b/moveit_setup_assistant/src/widgets/navigation_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_NAVIGATION_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_NAVIGATION_WIDGET_ +#pragma once #include #include @@ -84,5 +83,3 @@ class NavDelegate : public QStyledItemDelegate void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override; }; } - -#endif diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.h b/moveit_setup_assistant/src/widgets/passive_joints_widget.h index c7dc8dd1d5..344b133d3a 100644 --- a/moveit_setup_assistant/src/widgets/passive_joints_widget.h +++ b/moveit_setup_assistant/src/widgets/passive_joints_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PASSIVE_JOINTS_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PASSIVE_JOINTS_WIDGET_ +#pragma once // Qt #include @@ -102,5 +101,3 @@ private Q_SLOTS: }; } // namespace - -#endif diff --git a/moveit_setup_assistant/src/widgets/perception_widget.h b/moveit_setup_assistant/src/widgets/perception_widget.h index 336fe8a80e..e9c6952d6d 100644 --- a/moveit_setup_assistant/src/widgets/perception_widget.h +++ b/moveit_setup_assistant/src/widgets/perception_widget.h @@ -33,8 +33,7 @@ /* Author: Mohamad Ayman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PERCEPTION_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PERCEPTION_WIDGET_ +#pragma once // Qt #include @@ -125,5 +124,3 @@ private Q_SLOTS: }; } // namespace moveit_setup_assistant - -#endif diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.h b/moveit_setup_assistant/src/widgets/planning_groups_widget.h index 85bcf64259..0d40c6cc9e 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.h +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PLANNING_GROUPS_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PLANNING_GROUPS_WIDGET_ +#pragma once // Qt class QPushButton; @@ -232,5 +231,3 @@ class PlanGroupType }; Q_DECLARE_METATYPE(PlanGroupType); - -#endif diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.h b/moveit_setup_assistant/src/widgets/robot_poses_widget.h index 6614273cca..bdf6aa3e2f 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.h +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROBOT_POSES_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROBOT_POSES_WIDGET_ +#pragma once // Qt #include @@ -295,5 +294,3 @@ private Q_SLOTS: // Declare std::string as metatype so we can use it in a signal Q_DECLARE_METATYPE(std::string) - -#endif diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h index 57fc7d3c8b..c1c05e609b 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h @@ -33,8 +33,7 @@ /* Author: Mohamad Ayman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROS_CONTROLLERS_WIDGET_H -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROS_CONTROLLERS_WIDGET_H +#pragma once // Qt #include @@ -160,5 +159,3 @@ private Q_SLOTS: }; } // namespace moveit_setup_assistant - -#endif diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h index 48ccf7d26a..51f07cf56f 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_SETUP_ASSISTANT_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_SETUP_ASSISTANT_WIDGET_ +#pragma once // Qt #include @@ -237,5 +236,3 @@ private Q_SLOTS: // ****************************************************************************************** }; } - -#endif diff --git a/moveit_setup_assistant/src/widgets/setup_screen_widget.h b/moveit_setup_assistant/src/widgets/setup_screen_widget.h index 805350e9d5..7898b4f25e 100644 --- a/moveit_setup_assistant/src/widgets/setup_screen_widget.h +++ b/moveit_setup_assistant/src/widgets/setup_screen_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_SETUP_SCREEN_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_SETUP_SCREEN_WIDGET_ +#pragma once #include @@ -75,5 +74,3 @@ class SetupScreenWidget : public QWidget /// Event for telling rviz to unhighlight all links of the robot void unhighlightAll(); }; - -#endif diff --git a/moveit_setup_assistant/src/widgets/simulation_widget.h b/moveit_setup_assistant/src/widgets/simulation_widget.h index 2b3ac58331..029874d065 100644 --- a/moveit_setup_assistant/src/widgets/simulation_widget.h +++ b/moveit_setup_assistant/src/widgets/simulation_widget.h @@ -33,8 +33,7 @@ /* Author: Mohamad Ayman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_SIMULATION_WIDGET_H -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_SIMULATION_WIDGET_H +#pragma once // Qt #include @@ -93,5 +92,3 @@ private Q_SLOTS: }; } // namespace moveit_setup_assistant - -#endif diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.h b/moveit_setup_assistant/src/widgets/start_screen_widget.h index 8a75e38c58..d06dd6a6ce 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.h +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_START_SCREEN_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_START_SCREEN_WIDGET_ +#pragma once #include #include @@ -190,5 +189,3 @@ private Q_SLOTS: QLabel* widget_instructions_; }; } - -#endif diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h b/moveit_setup_assistant/src/widgets/virtual_joints_widget.h index b1bb87a68b..e3c6f5d4ec 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_VIRTUAL_JOINTS_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_VIRTUAL_JOINTS_WIDGET_ +#pragma once // Qt #include @@ -188,5 +187,3 @@ private Q_SLOTS: }; } // namespace - -#endif From a6f8ff994172ba504fecfeddbaf9de02a75010e1 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 13 Aug 2019 04:49:19 -0500 Subject: [PATCH 025/612] Update jog_arm README with rviz config (#1614) --- moveit_experimental/jog_arm/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_experimental/jog_arm/README.md b/moveit_experimental/jog_arm/README.md index 7e3626d94d..af983f78d6 100644 --- a/moveit_experimental/jog_arm/README.md +++ b/moveit_experimental/jog_arm/README.md @@ -16,7 +16,7 @@ Build and subsequently source the catkin workspace. Startup the robot and MoveIt roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true - roslaunch ur5_moveit_config moveit_rviz.launch + roslaunch ur5_moveit_config moveit_rviz.launch config:=true In RViz, "plan and execute" a motion to a non-singular position (not all zero joint angles) that is not close to a joint limit. From 9ddbfb25c92c35559090a6c8a3a1a2b501fa6653 Mon Sep 17 00:00:00 2001 From: Sean Yen Date: Tue, 13 Aug 2019 11:44:42 -0700 Subject: [PATCH 026/612] Fix binary artifact install locations. (#1575) --- moveit_core/background_processing/CMakeLists.txt | 3 ++- moveit_core/collision_detection/CMakeLists.txt | 3 ++- .../collision_detection_fcl/CMakeLists.txt | 3 ++- .../collision_distance_field/CMakeLists.txt | 2 +- moveit_core/constraint_samplers/CMakeLists.txt | 3 ++- moveit_core/distance_field/CMakeLists.txt | 3 ++- moveit_core/dynamics_solver/CMakeLists.txt | 3 ++- moveit_core/exceptions/CMakeLists.txt | 3 ++- moveit_core/kinematic_constraints/CMakeLists.txt | 3 ++- moveit_core/kinematics_base/CMakeLists.txt | 3 ++- moveit_core/kinematics_metrics/CMakeLists.txt | 3 ++- moveit_core/planning_interface/CMakeLists.txt | 3 ++- .../planning_request_adapter/CMakeLists.txt | 3 ++- moveit_core/planning_scene/CMakeLists.txt | 3 ++- moveit_core/profiler/CMakeLists.txt | 3 ++- moveit_core/robot_model/CMakeLists.txt | 3 ++- moveit_core/robot_state/CMakeLists.txt | 3 ++- moveit_core/robot_trajectory/CMakeLists.txt | 3 ++- moveit_core/trajectory_processing/CMakeLists.txt | 3 ++- moveit_core/transforms/CMakeLists.txt | 3 ++- moveit_core/utils/CMakeLists.txt | 3 ++- .../v2/kinematics_cache/CMakeLists.txt | 5 ++++- .../kinematics_constraint_aware/CMakeLists.txt | 5 ++++- .../cached_ik_kinematics_plugin/CMakeLists.txt | 15 ++++++++++++--- .../templates/CMakeLists.txt | 4 +++- .../kdl_kinematics_plugin/CMakeLists.txt | 5 ++++- .../lma_kinematics_plugin/CMakeLists.txt | 5 ++++- .../srv_kinematics_plugin/CMakeLists.txt | 5 ++++- moveit_kinematics/test/CMakeLists.txt | 3 ++- .../chomp/chomp_interface/CMakeLists.txt | 2 +- .../chomp/chomp_motion_planner/CMakeLists.txt | 2 +- .../chomp/chomp_optimizer_adapter/CMakeLists.txt | 2 +- .../ompl/ompl_interface/CMakeLists.txt | 5 ++++- .../moveit_fake_controller_manager/CMakeLists.txt | 5 ++++- .../moveit_ros_control_interface/CMakeLists.txt | 2 +- .../CMakeLists.txt | 5 ++++- moveit_ros/benchmarks/CMakeLists.txt | 8 +++++++- .../CMakeLists.txt | 3 ++- moveit_ros/manipulation/pick_place/CMakeLists.txt | 5 ++++- moveit_ros/move_group/CMakeLists.txt | 7 +++++-- moveit_ros/occupancy_map_monitor/CMakeLists.txt | 9 +++++++-- .../depth_image_octomap_updater/CMakeLists.txt | 5 ++++- .../lazy_free_space_updater/CMakeLists.txt | 5 ++++- moveit_ros/perception/mesh_filter/CMakeLists.txt | 5 ++++- .../point_containment_filter/CMakeLists.txt | 5 ++++- .../pointcloud_octomap_updater/CMakeLists.txt | 3 ++- .../perception/semantic_world/CMakeLists.txt | 3 ++- .../collision_plugin_loader/CMakeLists.txt | 5 ++++- .../CMakeLists.txt | 5 ++++- .../kinematics_plugin_loader/CMakeLists.txt | 5 ++++- moveit_ros/planning/plan_execution/CMakeLists.txt | 5 ++++- .../planning/planning_pipeline/CMakeLists.txt | 5 ++++- .../CMakeLists.txt | 6 +++++- .../planning_scene_monitor/CMakeLists.txt | 5 ++++- moveit_ros/planning/rdf_loader/CMakeLists.txt | 5 ++++- .../planning/robot_model_loader/CMakeLists.txt | 5 ++++- .../trajectory_execution_manager/CMakeLists.txt | 5 ++++- .../CMakeLists.txt | 3 ++- .../move_group_interface/CMakeLists.txt | 6 ++++-- .../planning_scene_interface/CMakeLists.txt | 6 +++--- .../py_bindings_tools/CMakeLists.txt | 6 +++--- .../robot_interface/CMakeLists.txt | 3 +-- moveit_ros/robot_interaction/CMakeLists.txt | 5 ++++- .../motion_planning_rviz_plugin/CMakeLists.txt | 3 ++- .../planning_scene_rviz_plugin/CMakeLists.txt | 3 ++- .../robot_state_rviz_plugin/CMakeLists.txt | 3 ++- .../rviz_plugin_render_tools/CMakeLists.txt | 3 ++- .../trajectory_rviz_plugin/CMakeLists.txt | 3 ++- moveit_ros/warehouse/warehouse/CMakeLists.txt | 7 ++++++- moveit_setup_assistant/CMakeLists.txt | 9 +++++++-- 70 files changed, 220 insertions(+), 81 deletions(-) diff --git a/moveit_core/background_processing/CMakeLists.txt b/moveit_core/background_processing/CMakeLists.txt index 0e09ddc409..f0b7ab4712 100644 --- a/moveit_core/background_processing/CMakeLists.txt +++ b/moveit_core/background_processing/CMakeLists.txt @@ -7,6 +7,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt index bfc4e8db6b..21d3bcb5d3 100644 --- a/moveit_core/collision_detection/CMakeLists.txt +++ b/moveit_core/collision_detection/CMakeLists.txt @@ -31,6 +31,7 @@ endif() install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index 8e73a17709..74149b6552 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -17,7 +17,8 @@ target_link_libraries(collision_detector_fcl_plugin ${catkin_LIBRARIES} ${MOVEIT install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_fcl_plugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/collision_distance_field/CMakeLists.txt b/moveit_core/collision_distance_field/CMakeLists.txt index 491b47f0e8..ad1850a61e 100644 --- a/moveit_core/collision_distance_field/CMakeLists.txt +++ b/moveit_core/collision_distance_field/CMakeLists.txt @@ -30,7 +30,7 @@ endif() install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index e988eb2108..958e37b563 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -19,7 +19,8 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/distance_field/CMakeLists.txt b/moveit_core/distance_field/CMakeLists.txt index ac60b52783..3955c94fd3 100644 --- a/moveit_core/distance_field/CMakeLists.txt +++ b/moveit_core/distance_field/CMakeLists.txt @@ -12,7 +12,8 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/dynamics_solver/CMakeLists.txt b/moveit_core/dynamics_solver/CMakeLists.txt index 61324a752d..07028fc9e5 100644 --- a/moveit_core/dynamics_solver/CMakeLists.txt +++ b/moveit_core/dynamics_solver/CMakeLists.txt @@ -8,6 +8,7 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/exceptions/CMakeLists.txt b/moveit_core/exceptions/CMakeLists.txt index 6b80cf3c28..70ab423d27 100644 --- a/moveit_core/exceptions/CMakeLists.txt +++ b/moveit_core/exceptions/CMakeLists.txt @@ -7,5 +7,6 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index ea45aa72f0..18ff796f70 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -12,7 +12,8 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/kinematics_base/CMakeLists.txt b/moveit_core/kinematics_base/CMakeLists.txt index f4d98357ef..2ec7a736f4 100644 --- a/moveit_core/kinematics_base/CMakeLists.txt +++ b/moveit_core/kinematics_base/CMakeLists.txt @@ -13,6 +13,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/kinematics_metrics/CMakeLists.txt b/moveit_core/kinematics_metrics/CMakeLists.txt index 8438c8641d..6a43296df8 100644 --- a/moveit_core/kinematics_metrics/CMakeLists.txt +++ b/moveit_core/kinematics_metrics/CMakeLists.txt @@ -8,6 +8,7 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/planning_interface/CMakeLists.txt b/moveit_core/planning_interface/CMakeLists.txt index 33c0e1a789..8a876192b5 100644 --- a/moveit_core/planning_interface/CMakeLists.txt +++ b/moveit_core/planning_interface/CMakeLists.txt @@ -11,6 +11,7 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/planning_request_adapter/CMakeLists.txt b/moveit_core/planning_request_adapter/CMakeLists.txt index 87038c1d31..401e955ded 100644 --- a/moveit_core/planning_request_adapter/CMakeLists.txt +++ b/moveit_core/planning_request_adapter/CMakeLists.txt @@ -8,6 +8,7 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt index 13174d3f1d..cfa146e3ec 100644 --- a/moveit_core/planning_scene/CMakeLists.txt +++ b/moveit_core/planning_scene/CMakeLists.txt @@ -18,7 +18,8 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) diff --git a/moveit_core/profiler/CMakeLists.txt b/moveit_core/profiler/CMakeLists.txt index 92671fd8cd..803b7ebdb8 100644 --- a/moveit_core/profiler/CMakeLists.txt +++ b/moveit_core/profiler/CMakeLists.txt @@ -7,5 +7,6 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index a3383680b3..40a6199da4 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -27,5 +27,6 @@ endif() install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index d2cffe5f6c..90b30c9db7 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -19,7 +19,8 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) # Unit tests diff --git a/moveit_core/robot_trajectory/CMakeLists.txt b/moveit_core/robot_trajectory/CMakeLists.txt index 9bcfe4ae86..29b89e5aa2 100644 --- a/moveit_core/robot_trajectory/CMakeLists.txt +++ b/moveit_core/robot_trajectory/CMakeLists.txt @@ -8,6 +8,7 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index fdd392a586..7c51bfbec5 100644 --- a/moveit_core/trajectory_processing/CMakeLists.txt +++ b/moveit_core/trajectory_processing/CMakeLists.txt @@ -13,7 +13,8 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/transforms/CMakeLists.txt b/moveit_core/transforms/CMakeLists.txt index 411b925a68..ac2bd777b3 100644 --- a/moveit_core/transforms/CMakeLists.txt +++ b/moveit_core/transforms/CMakeLists.txt @@ -8,7 +8,8 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) # Unit tests diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index 05f4bb4878..9ea399702a 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -22,6 +22,7 @@ install( ${MOVEIT_LIB_NAME} ${MOVEIT_TEST_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_experimental/kinematics_cache/v2/kinematics_cache/CMakeLists.txt b/moveit_experimental/kinematics_cache/v2/kinematics_cache/CMakeLists.txt index d22d017782..6dcd3680b0 100644 --- a/moveit_experimental/kinematics_cache/v2/kinematics_cache/CMakeLists.txt +++ b/moveit_experimental/kinematics_cache/v2/kinematics_cache/CMakeLists.txt @@ -9,6 +9,9 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_experimental/kinematics_constraint_aware/CMakeLists.txt b/moveit_experimental/kinematics_constraint_aware/CMakeLists.txt index 8aaa5b86dc..12d18cecef 100644 --- a/moveit_experimental/kinematics_constraint_aware/CMakeLists.txt +++ b/moveit_experimental/kinematics_constraint_aware/CMakeLists.txt @@ -7,5 +7,8 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt index 05005fa6a5..b295d3eb50 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt @@ -9,7 +9,10 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${Boost_FILESYSTEM_LIBRARY} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) if(trac_ik_kinematics_plugin_FOUND) include_directories(${trac_ik_kinematics_plugin_INCLUDE_DIRS}) @@ -27,7 +30,10 @@ if(trac_ik_kinematics_plugin_FOUND) target_link_libraries(${MOVEIT_LIB_NAME} ${trac_ik_kinematics_plugin_LIBRARIES}) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES COMPILE_DEFINITIONS "CACHED_IK_KINEMATICS_TRAC_IK") endif() -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) # This is just for testing purposes; the arms from Universal Robots have # analytic solvers, so caching just adds extra overhead. @@ -42,7 +48,10 @@ if(ur_kinematics_FOUND) moveit_cached_ik_kinematics_base ${ur${UR}_pluginlib} ${catkin_LIBRARIES}) - install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) endforeach() endif() diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt index de49b96da2..e4020ed280 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt @@ -26,7 +26,9 @@ target_compile_options(${IKFAST_LIBRARY_NAME} PRIVATE -Wno-unused-variable) install(TARGETS ${IKFAST_LIBRARY_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install( FILES diff --git a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt index 9388ae6f26..cd3ffc0891 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt @@ -7,5 +7,8 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt index 0397388312..1dadddcd56 100644 --- a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt @@ -5,5 +5,8 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt index b7829f2412..2cf14dd877 100644 --- a/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt @@ -5,5 +5,8 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_kinematics/test/CMakeLists.txt b/moveit_kinematics/test/CMakeLists.txt index b7dee1f18d..be739327c6 100644 --- a/moveit_kinematics/test/CMakeLists.txt +++ b/moveit_kinematics/test/CMakeLists.txt @@ -36,5 +36,6 @@ if(CATKIN_ENABLE_TESTING) target_link_libraries(benchmark_ik ${catkin_LIBRARIES} ${moveit_ros_planning_LIBRARIES} ${Boost_PROGRAM_OPTIONS_LIBRARY}) - install(TARGETS benchmark_ik DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + install(TARGETS benchmark_ik + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) endif() diff --git a/moveit_planners/chomp/chomp_interface/CMakeLists.txt b/moveit_planners/chomp/chomp_interface/CMakeLists.txt index 3e03d56474..a811145448 100644 --- a/moveit_planners/chomp/chomp_interface/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_interface/CMakeLists.txt @@ -54,7 +54,7 @@ install(DIRECTORY include/chomp_interface/ install(TARGETS ${PROJECT_NAME} chomp_planner_plugin ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) if(CATKIN_ENABLE_TESTING) diff --git a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt index 69655a2c84..d02a10255a 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt @@ -34,5 +34,5 @@ install(DIRECTORY include/${PROJECT_NAME}/ install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt index 91d6b6a455..0e69e70585 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt @@ -24,5 +24,5 @@ install(FILES chomp_optimizer_adapter_plugin_description.xml install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index cc9181efee..0f7c18bbb9 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -40,8 +40,11 @@ add_library(moveit_ompl_planner_plugin src/ompl_planner_manager.cpp) set_target_properties(moveit_ompl_planner_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(moveit_ompl_planner_plugin ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} moveit_ompl_planner moveit_ompl_planner_plugin moveit_generate_state_database +install(TARGETS ${MOVEIT_LIB_NAME} moveit_ompl_planner_plugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) +install(TARGETS moveit_ompl_planner moveit_generate_state_database RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt index d0e7a81bfe..7a24177c77 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt @@ -32,7 +32,10 @@ add_library(${PROJECT_NAME} set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(FILES moveit_fake_controller_manager_plugin_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt index 8b61d3b039..c590d3de6d 100644 --- a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt +++ b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt @@ -69,7 +69,7 @@ target_link_libraries(${PROJECT_NAME}_trajectory_plugin install(TARGETS ${PROJECT_NAME}_plugin ${PROJECT_NAME}_trajectory_plugin ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) ## Mark cpp header files for installation diff --git a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt index ec00ba8601..48c02ea8b5 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt @@ -33,7 +33,10 @@ add_library(${PROJECT_NAME} set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(FILES moveit_simple_controller_manager_plugin_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt index 1b41bfcd22..2463628a58 100644 --- a/moveit_ros/benchmarks/CMakeLists.txt +++ b/moveit_ros/benchmarks/CMakeLists.txt @@ -43,8 +43,14 @@ target_link_libraries(moveit_run_benchmark ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES install( TARGETS - ${MOVEIT_LIB_NAME} moveit_run_benchmark + ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +install( + TARGETS + moveit_run_benchmark RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/CMakeLists.txt b/moveit_ros/manipulation/move_group_pick_place_capability/CMakeLists.txt index 8cdfecab66..036197fc58 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/CMakeLists.txt +++ b/moveit_ros/manipulation/move_group_pick_place_capability/CMakeLists.txt @@ -11,6 +11,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} moveit_pick_place_planner ${catkin_LIBR install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/manipulation/pick_place/CMakeLists.txt b/moveit_ros/manipulation/pick_place/CMakeLists.txt index 4e41c8a4e5..fb371ae640 100644 --- a/moveit_ros/manipulation/pick_place/CMakeLists.txt +++ b/moveit_ros/manipulation/pick_place/CMakeLists.txt @@ -14,5 +14,8 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index 82578d5bf4..783f29faa7 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -67,10 +67,13 @@ target_link_libraries(move_group moveit_move_group_capabilities_base ${catkin_LI target_link_libraries(moveit_move_group_default_capabilities moveit_move_group_capabilities_base ${catkin_LIBRARIES} ${Boost_LIBRARIES}) target_link_libraries(list_move_group_capabilities ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS move_group list_move_group_capabilities moveit_move_group_capabilities_base moveit_move_group_default_capabilities +install(TARGETS move_group list_move_group_capabilities + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(TARGETS moveit_move_group_capabilities_base moveit_move_group_default_capabilities ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/occupancy_map_monitor/CMakeLists.txt b/moveit_ros/occupancy_map_monitor/CMakeLists.txt index 56305e5bde..bdb0bae0f1 100644 --- a/moveit_ros/occupancy_map_monitor/CMakeLists.txt +++ b/moveit_ros/occupancy_map_monitor/CMakeLists.txt @@ -56,8 +56,13 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_executable(moveit_ros_occupancy_map_server src/occupancy_map_server.cpp) target_link_libraries(moveit_ros_occupancy_map_server ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} moveit_ros_occupancy_map_server - LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION} +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(TARGETS moveit_ros_occupancy_map_server RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt b/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt index 81485c633d..4222cd346a 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt +++ b/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt @@ -10,5 +10,8 @@ add_library(${MOVEIT_LIB_NAME} src/updater_plugin.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/perception/lazy_free_space_updater/CMakeLists.txt b/moveit_ros/perception/lazy_free_space_updater/CMakeLists.txt index ce935b1f36..e9092875c1 100644 --- a/moveit_ros/perception/lazy_free_space_updater/CMakeLists.txt +++ b/moveit_ros/perception/lazy_free_space_updater/CMakeLists.txt @@ -8,5 +8,8 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${sensor_msgs_EXPORTED_TARGETS}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/perception/mesh_filter/CMakeLists.txt b/moveit_ros/perception/mesh_filter/CMakeLists.txt index 940772f8da..f08902a474 100644 --- a/moveit_ros/perception/mesh_filter/CMakeLists.txt +++ b/moveit_ros/perception/mesh_filter/CMakeLists.txt @@ -22,5 +22,8 @@ if (CATKIN_ENABLE_TESTING) endif() endif() -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/perception/point_containment_filter/CMakeLists.txt b/moveit_ros/perception/point_containment_filter/CMakeLists.txt index ed76980c0a..a672692acb 100644 --- a/moveit_ros/perception/point_containment_filter/CMakeLists.txt +++ b/moveit_ros/perception/point_containment_filter/CMakeLists.txt @@ -6,5 +6,8 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_F set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt b/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt index 503c46fb2f..425a12beed 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt +++ b/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt @@ -14,4 +14,5 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/perception/semantic_world/CMakeLists.txt b/moveit_ros/perception/semantic_world/CMakeLists.txt index 6ed3c99610..0c6ef4f3f0 100644 --- a/moveit_ros/perception/semantic_world/CMakeLists.txt +++ b/moveit_ros/perception/semantic_world/CMakeLists.txt @@ -9,4 +9,5 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt b/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt index 86eea90717..6ad2115652 100644 --- a/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt +++ b/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt @@ -4,5 +4,8 @@ add_library(${MOVEIT_LIB_NAME} src/collision_plugin_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt b/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt index 8e0dd2dc28..8e11bfaafa 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt +++ b/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt @@ -4,5 +4,8 @@ add_library(${MOVEIT_LIB_NAME} src/constraint_sampler_manager_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt b/moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt index ebcc035b60..48ce472264 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt +++ b/moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt @@ -4,5 +4,8 @@ add_library(${MOVEIT_LIB_NAME} src/kinematics_plugin_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} moveit_rdf_loader ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/plan_execution/CMakeLists.txt b/moveit_ros/planning/plan_execution/CMakeLists.txt index 6d2a0c24fe..6b579cfb11 100644 --- a/moveit_ros/planning/plan_execution/CMakeLists.txt +++ b/moveit_ros/planning/plan_execution/CMakeLists.txt @@ -11,5 +11,8 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/planning_pipeline/CMakeLists.txt b/moveit_ros/planning/planning_pipeline/CMakeLists.txt index 853d2efb11..abeb034e3a 100644 --- a/moveit_ros/planning/planning_pipeline/CMakeLists.txt +++ b/moveit_ros/planning/planning_pipeline/CMakeLists.txt @@ -4,5 +4,8 @@ add_library(${MOVEIT_LIB_NAME} src/planning_pipeline.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt index f3a9dbae8d..b32ffe5349 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt +++ b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt @@ -19,6 +19,10 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_executable(moveit_list_request_adapter_plugins src/list.cpp) target_link_libraries(moveit_list_request_adapter_plugins ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} moveit_list_request_adapter_plugins +install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +install(TARGETS moveit_list_request_adapter_plugins RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt index 9ad23d6175..6329feef36 100644 --- a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt +++ b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt @@ -14,5 +14,8 @@ target_link_libraries(${MOVEIT_LIB_NAME} add_executable(demo_scene demos/demo_scene.cpp) target_link_libraries(demo_scene ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/rdf_loader/CMakeLists.txt b/moveit_ros/planning/rdf_loader/CMakeLists.txt index 0a0dbe5055..0b91cafd81 100644 --- a/moveit_ros/planning/rdf_loader/CMakeLists.txt +++ b/moveit_ros/planning/rdf_loader/CMakeLists.txt @@ -4,5 +4,8 @@ add_library(${MOVEIT_LIB_NAME} src/rdf_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/robot_model_loader/CMakeLists.txt b/moveit_ros/planning/robot_model_loader/CMakeLists.txt index 13af8cdd78..9790d1bcc3 100644 --- a/moveit_ros/planning/robot_model_loader/CMakeLists.txt +++ b/moveit_ros/planning/robot_model_loader/CMakeLists.txt @@ -8,5 +8,8 @@ add_library(${MOVEIT_LIB_NAME} src/robot_model_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} moveit_rdf_loader moveit_kinematics_plugin_loader ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt index 51b7336ac1..14a2c23be8 100644 --- a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt +++ b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt @@ -5,7 +5,10 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} moveit_planning_scene_monitor moveit_robot_model_loader ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${moveit_ros_planning_EXPORTED_TARGETS}) # don't build until necessary msgs are finish -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) add_library(test_controller_manager_plugin test/test_moveit_controller_manager_plugin.cpp) diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt b/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt index f5b6953406..71b61285a4 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt +++ b/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt @@ -6,6 +6,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${PYTHON_LIBRARIES} install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt index eacfa2aecb..6ff6444e7a 100644 --- a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt @@ -14,11 +14,13 @@ set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECT install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME}_python ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt b/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt index 7658b2583e..11489f296c 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt @@ -12,10 +12,10 @@ set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECT install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME}_python - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt b/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt index 6d1e6899a6..1f3cb1c93b 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt +++ b/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt @@ -6,7 +6,8 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) add_library(${MOVEIT_LIB_NAME}_python src/wrap_python_roscpp_initializer.cpp) target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) @@ -15,7 +16,6 @@ set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_r set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") install(TARGETS ${MOVEIT_LIB_NAME}_python - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning_interface/robot_interface/CMakeLists.txt b/moveit_ros/planning_interface/robot_interface/CMakeLists.txt index 56cb7b4098..0c6a4a22bf 100644 --- a/moveit_ros/planning_interface/robot_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/robot_interface/CMakeLists.txt @@ -8,5 +8,4 @@ set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_r set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") install(TARGETS ${MOVEIT_LIB_NAME}_python - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) diff --git a/moveit_ros/robot_interaction/CMakeLists.txt b/moveit_ros/robot_interaction/CMakeLists.txt index 8050ff8d75..d18735a809 100644 --- a/moveit_ros/robot_interaction/CMakeLists.txt +++ b/moveit_ros/robot_interaction/CMakeLists.txt @@ -54,7 +54,10 @@ if (CATKIN_ENABLE_TESTING) ${Boost_LIBRARIES}) endif() -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(DIRECTORY resources DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt index 41c9b07b59..6f4ce1f245 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt @@ -36,4 +36,5 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt index 77a06066ac..109bb569f6 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt @@ -14,4 +14,5 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt index 6a4d92f2f7..8db0bf29fc 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt @@ -15,5 +15,6 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt index 9baca76cfb..a9769d25d1 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt +++ b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt @@ -35,4 +35,5 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt index 081058868c..97b09ca864 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt @@ -27,5 +27,6 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ${catkin_LIBRAR install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/warehouse/warehouse/CMakeLists.txt b/moveit_ros/warehouse/warehouse/CMakeLists.txt index 2a459f97c0..5f1b0e000c 100644 --- a/moveit_ros/warehouse/warehouse/CMakeLists.txt +++ b/moveit_ros/warehouse/warehouse/CMakeLists.txt @@ -32,12 +32,17 @@ target_link_libraries(moveit_warehouse_services ${catkin_LIBRARIES} ${MOVEIT_LIB install( TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +install( + TARGETS moveit_save_to_warehouse moveit_warehouse_broadcast moveit_warehouse_import_from_text moveit_warehouse_save_as_text moveit_init_demo_warehouse moveit_warehouse_services - LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/CMakeLists.txt index fa15457b8b..f01787d422 100644 --- a/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/CMakeLists.txt @@ -141,9 +141,14 @@ set_target_properties(${PROJECT_NAME}_updater PROPERTIES OUTPUT_NAME collisions_updater PREFIX "") -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_widgets ${PROJECT_NAME}_tools ${PROJECT_NAME}_updater - LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION} +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_updater RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(TARGETS ${PROJECT_NAME}_widgets ${PROJECT_NAME}_tools + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) From 889447f2dfe6bb997f7a32dd61069008018b0ec8 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Tue, 13 Aug 2019 13:41:37 -0600 Subject: [PATCH 027/612] Remove packages without Jenkins badges (#1617) --- README.md | 3 --- 1 file changed, 3 deletions(-) diff --git a/README.md b/README.md index 8de31d4900..0b330cc1bc 100644 --- a/README.md +++ b/README.md @@ -39,7 +39,6 @@ build farm | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__mo MoveIt Package | Kinetic Source | Kinetic Debian | Melodic Source | Melodic Debian --------------- | -------------- | -------------- | -------------- | -------------- moveit | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit__ubuntu_bionic_amd64__binary) -moveit_base | | | | moveit_chomp_optimizer_adapter | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary) moveit_commander | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_commander__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_commander__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_commander__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_commander__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary) moveit_core | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_core__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_core__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_core__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_core__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary) @@ -57,7 +56,6 @@ moveit_ros_benchmarks | [![Build Status](http://build.ros.org/buildStatus/icon?j moveit_ros_control_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary) moveit_ros_manipulation | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary) moveit_ros_move_group | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary) -moveit_ros_occupancy_map_monitor | | | | moveit_ros_perception | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_perception__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_perception__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary) moveit_ros_planning | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary) moveit_ros_planning_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary) @@ -67,7 +65,6 @@ moveit_ros_warehouse | [![Build Status](http://build.ros.org/buildStatus/icon?jo moveit_runtime | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_runtime__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_runtime__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_runtime__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_runtime__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary) moveit_setup_assistant | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary) moveit_simple_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary) -moveit_tutorials | | | | moveit_visual_tools | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_visual_tools__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_visual_tools__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary) chomp_motion_planner | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__chomp_motion_planner__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__chomp_motion_planner__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary) geometric_shapes | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__geometric_shapes__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__geometric_shapes__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__geometric_shapes__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__geometric_shapes__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary) From eae8f02070fd05ea4c53b476624a469eea7ca362 Mon Sep 17 00:00:00 2001 From: Jonathan Binney Date: Tue, 13 Aug 2019 21:31:22 -0700 Subject: [PATCH 028/612] Use standard cmake text for metapackages (#1620) --- moveit/CMakeLists.txt | 2 +- moveit_planners/moveit_planners/CMakeLists.txt | 2 +- moveit_plugins/moveit_plugins/CMakeLists.txt | 2 +- moveit_ros/moveit_ros/CMakeLists.txt | 2 +- moveit_runtime/CMakeLists.txt | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/moveit/CMakeLists.txt b/moveit/CMakeLists.txt index 9acc0a9b9e..9d8f3239d4 100644 --- a/moveit/CMakeLists.txt +++ b/moveit/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 2.8.3) project(moveit) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_planners/moveit_planners/CMakeLists.txt b/moveit_planners/moveit_planners/CMakeLists.txt index 753bb7b4d9..e6e2b34540 100644 --- a/moveit_planners/moveit_planners/CMakeLists.txt +++ b/moveit_planners/moveit_planners/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 2.8.3) project(moveit_planners) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_plugins/moveit_plugins/CMakeLists.txt b/moveit_plugins/moveit_plugins/CMakeLists.txt index 9afdc1d9d8..03c972ba5f 100644 --- a/moveit_plugins/moveit_plugins/CMakeLists.txt +++ b/moveit_plugins/moveit_plugins/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 2.8.3) project(moveit_plugins) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_ros/moveit_ros/CMakeLists.txt b/moveit_ros/moveit_ros/CMakeLists.txt index 5425d0c2e5..75cf534418 100644 --- a/moveit_ros/moveit_ros/CMakeLists.txt +++ b/moveit_ros/moveit_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 2.8.3) project(moveit_ros) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_runtime/CMakeLists.txt b/moveit_runtime/CMakeLists.txt index 0224cde26c..18700d6157 100644 --- a/moveit_runtime/CMakeLists.txt +++ b/moveit_runtime/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 2.8.3) project(moveit_runtime) find_package(catkin REQUIRED) catkin_metapackage() From d7e0dfde20cd9c601190aca1f20db71b5a898255 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Wed, 14 Aug 2019 09:17:13 +0300 Subject: [PATCH 029/612] Use QDir::currentPath() rather than getenv("PWD") (#1618) --- .../src/widgets/setup_assistant_widget.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index a2b1a1fb07..511463a968 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include // for loading all avail kinematic planners // Rviz #include @@ -113,13 +114,7 @@ SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, const boost::program } else { - // Open the directory where the MSA was started from. - // cf. http://stackoverflow.com/a/7413516/577001 - QString pwdir(""); - char* pwd; - pwd = getenv("PWD"); - pwdir.append(pwd); - start_screen_widget_->stack_path_->setPath(pwdir); + start_screen_widget_->stack_path_->setPath(QDir::currentPath()); } // Add Navigation Buttons (but do not load widgets yet except start screen) From f231ad7155090720ccaab265607e19f5fb2c7fc8 Mon Sep 17 00:00:00 2001 From: Sean Yen Date: Wed, 14 Aug 2019 00:29:46 -0700 Subject: [PATCH 030/612] remove GCC extension and alternative operator usage. (#1583) replace the gcc extension and alternative operator usage Also guard use of Linux-specific abi header --- .../planning_request_adapter.h | 8 +++++++- moveit_core/robot_state/src/robot_state.cpp | 2 +- moveit_core/utils/src/robot_model_test_utils.cpp | 14 +++++++------- .../chomp_motion_planner/src/chomp_trajectory.cpp | 2 +- .../src/current_state_monitor.cpp | 10 +++++----- 5 files changed, 21 insertions(+), 15 deletions(-) diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h index fd2b34a424..9c11302a02 100644 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h @@ -40,7 +40,9 @@ #include #include #include +#if !defined(_MSC_VER) #include +#endif /** \brief Generic interface to adapting motion planning requests */ namespace planning_request_adapter @@ -70,8 +72,12 @@ class PlanningRequestAdapter // Get name of derived adapter std::string adapter_name = typeid(*this).name(); // Try to demangle the name - int status; + int status = 1; +#if defined(_MSC_VER) + std::string demangled_name = adapter_name; +#else std::string demangled_name = abi::__cxa_demangle(adapter_name.c_str(), NULL, NULL, &status); +#endif if (status == 0) adapter_name = demangled_name; ROS_WARN_NAMED("planning_request_adapter", "Implementation of function initialize() is missing from '%s'." diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 756ba3a97c..abf7034f98 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1232,7 +1232,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link const JointModel* pjm = link->getParentJointModel(); if (pjm->getVariableCount() > 0) { - if (not group->hasJointModel(pjm->getName())) + if (!group->hasJointModel(pjm->getName())) { link = pjm->getParentLinkModel(); continue; diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index 50221182ec..e8bf5d3407 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -120,14 +120,14 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& return; } // First link should already be added. - if (not urdf_model_->getLink(link_names[0])) + if (!urdf_model_->getLink(link_names[0])) { ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_names[0].c_str()); is_valid_ = false; return; } - if (not joint_origins.empty() && link_names.size() - 1 != joint_origins.size()) + if (!joint_origins.empty() && link_names.size() - 1 != joint_origins.size()) { ROS_ERROR_NAMED(LOGNAME, "There should be one more link (%zu) than there are joint origins (%zu)", link_names.size(), joint_origins.size()); @@ -152,7 +152,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& joint->name = link_names[i - 1] + "-" + link_names[i] + "-joint"; // Default to Identity transform for origins. joint->parent_to_joint_origin_transform.clear(); - if (not joint_origins.empty()) + if (!joint_origins.empty()) { geometry_msgs::Pose o = joint_origins[i - 1]; joint->parent_to_joint_origin_transform.position = urdf::Vector3(o.position.x, o.position.y, o.position.z); @@ -197,7 +197,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& void RobotModelBuilder::addInertial(const std::string& link_name, double mass, geometry_msgs::Pose origin, double ixx, double ixy, double ixz, double iyy, double iyz, double izz) { - if (not urdf_model_->getLink(link_name)) + if (!urdf_model_->getLink(link_name)) { ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str()); is_valid_ = false; @@ -260,7 +260,7 @@ void RobotModelBuilder::addCollisionMesh(const std::string& link_name, const std void RobotModelBuilder::addLinkCollision(const std::string& link_name, const urdf::CollisionSharedPtr& collision, geometry_msgs::Pose origin) { - if (not urdf_model_->getLink(link_name)) + if (!urdf_model_->getLink(link_name)) { ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str()); is_valid_ = false; @@ -278,7 +278,7 @@ void RobotModelBuilder::addLinkCollision(const std::string& link_name, const urd void RobotModelBuilder::addLinkVisual(const std::string& link_name, const urdf::VisualSharedPtr& vis, geometry_msgs::Pose origin) { - if (not urdf_model_->getLink(link_name)) + if (!urdf_model_->getLink(link_name)) { ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str()); is_valid_ = false; @@ -290,7 +290,7 @@ void RobotModelBuilder::addLinkVisual(const std::string& link_name, const urdf:: urdf::LinkSharedPtr link; urdf_model_->getLink(link_name, link); - if (not link->visual_array.empty()) + if (!link->visual_array.empty()) { link->visual_array.push_back(vis); } diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp index 8580682a20..e1f56aa0e7 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp @@ -155,7 +155,7 @@ void ChompTrajectory::fillInMinJerk() // calculate the spline coefficients for each joint: // (these are for the special case of zero start and end vel and acc) - double coeff[num_joints_][6]; + std::vector coeff(num_joints_); for (size_t i = 0; i < num_joints_; i++) { double x0 = (*this)(start_index, i); diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index 43f1da2158..64ae7465d2 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -443,20 +443,20 @@ void planning_scene_monitor::CurrentStateMonitor::tfCallback() continue; joint_time_[joint] = latest_common_time; - double new_values[joint->getStateSpaceDimension()]; + std::vector new_values(joint->getStateSpaceDimension()); const robot_model::LinkModel* link = joint->getChildLinkModel(); if (link->jointOriginTransformIsIdentity()) - joint->computeVariablePositions(tf2::transformToEigen(transf), new_values); + joint->computeVariablePositions(tf2::transformToEigen(transf), new_values.data()); else joint->computeVariablePositions(link->getJointOriginTransform().inverse() * tf2::transformToEigen(transf), - new_values); + new_values.data()); - if (joint->distance(new_values, robot_state_.getJointPositions(joint)) > 1e-5) + if (joint->distance(new_values.data(), robot_state_.getJointPositions(joint)) > 1e-5) { changes = true; } - robot_state_.setJointPositions(joint, new_values); + robot_state_.setJointPositions(joint, new_values.data()); update = true; } } From 70537f037a87fbfa8bb51586cac247fd59a58e53 Mon Sep 17 00:00:00 2001 From: Daniel Wang Date: Wed, 14 Aug 2019 18:46:27 -0700 Subject: [PATCH 031/612] fix the invalid iterator issue (#1623) Signed-off-by: Daniel Wang --- moveit_setup_assistant/src/tools/moveit_config_data.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 77b4704390..0bbd81f4c2 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -775,7 +775,7 @@ void MoveItConfigData::outputFollowJointTrajectoryYAML(YAML::Emitter& emitter, emitter << YAML::EndMap; } } - ros_controllers_config_output.erase(controller_it); + controller_it = ros_controllers_config_output.erase(controller_it); emitter << YAML::EndMap; } else From 4acd3a75e4f5fdf3fdd1ac147648529758968c36 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 7 Aug 2019 14:01:55 -0600 Subject: [PATCH 032/612] Support benchmarking of full planning pipelines (#1531) ... instead of planning plugins only --- moveit_ros/benchmarks/README.md | 2 +- moveit_ros/benchmarks/examples/demo1.yaml | 4 +- moveit_ros/benchmarks/examples/demo2.yaml | 6 +- .../benchmarks/examples/demo_fanuc.launch | 2 +- .../benchmarks/examples/demo_obstacles.yaml | 8 +- .../benchmarks/examples/demo_panda.launch | 2 +- .../examples/demo_panda_all_planners.launch | 6 +- .../examples/demo_panda_all_planners.yaml | 8 +- .../demo_panda_all_planners_obstacles.launch | 6 +- .../moveit/benchmarks/BenchmarkExecutor.h | 5 +- .../moveit/benchmarks/BenchmarkOptions.h | 6 +- .../benchmarks/src/BenchmarkExecutor.cpp | 189 +++++++++--------- .../benchmarks/src/BenchmarkOptions.cpp | 44 ++-- moveit_ros/benchmarks/src/RunBenchmark.cpp | 6 +- 14 files changed, 148 insertions(+), 146 deletions(-) diff --git a/moveit_ros/benchmarks/README.md b/moveit_ros/benchmarks/README.md index ada7c8a485..4823b68253 100644 --- a/moveit_ros/benchmarks/README.md +++ b/moveit_ros/benchmarks/README.md @@ -2,4 +2,4 @@ This package provides methods to benchmark motion planning algorithms and aggregate/plot statistics. Results can be viewed in [Planner Arena](http://plannerarena.org/). -For more information and usage example please see [moveit tutorials](http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/benchmarking_tutorial.html). +For more information and usage example please see [moveit tutorials](https://ros-planning.github.io/moveit_tutorials/doc/benchmarking/benchmarking_tutorial.html). diff --git a/moveit_ros/benchmarks/examples/demo1.yaml b/moveit_ros/benchmarks/examples/demo1.yaml index b930a8187a..7d6e9882a2 100644 --- a/moveit_ros/benchmarks/examples/demo1.yaml +++ b/moveit_ros/benchmarks/examples/demo1.yaml @@ -18,8 +18,8 @@ benchmark_config: output_directory: /tmp/moveit_benchmarks/ queries: Pick1 start_states: Start1 - planners: - - plugin: ompl_interface/OMPLPlanner + planning_pipelines: + - name: ompl planners: - RRTConnectkConfigDefault - BKPIECEkConfigDefault diff --git a/moveit_ros/benchmarks/examples/demo2.yaml b/moveit_ros/benchmarks/examples/demo2.yaml index ba6048b01a..cb25a6c6c6 100644 --- a/moveit_ros/benchmarks/examples/demo2.yaml +++ b/moveit_ros/benchmarks/examples/demo2.yaml @@ -18,12 +18,12 @@ benchmark_config: output_directory: /home/benchmarks/ queries: .* start_states: .* - planners: - - plugin: ompl_interface/OMPLPlanner + planning_pipelines: + - name: ompl planners: - RRTConnectkConfigDefault - BKPIECEkConfigDefault - KPIECEkConfigDefault - - plugin: my_plugin_ns/my_plugin + - name: my_pipeline planners: - MyPlanner diff --git a/moveit_ros/benchmarks/examples/demo_fanuc.launch b/moveit_ros/benchmarks/examples/demo_fanuc.launch index 75b4f91b8d..cfecc6f5c5 100644 --- a/moveit_ros/benchmarks/examples/demo_fanuc.launch +++ b/moveit_ros/benchmarks/examples/demo_fanuc.launch @@ -8,7 +8,7 @@ - + diff --git a/moveit_ros/benchmarks/examples/demo_obstacles.yaml b/moveit_ros/benchmarks/examples/demo_obstacles.yaml index 8616b78596..d6a3c3b504 100644 --- a/moveit_ros/benchmarks/examples/demo_obstacles.yaml +++ b/moveit_ros/benchmarks/examples/demo_obstacles.yaml @@ -18,16 +18,16 @@ benchmark_config: output_directory: /tmp/moveit_benchmarks/ queries: .* start_states: .* - planners: - - plugin: ompl_interface/OMPLPlanner + planning_pipelines: + - name: ompl planners: - RRTConnectkConfigDefault - BKPIECEkConfigDefault - KPIECEkConfigDefault - - plugin: chomp_interface/CHOMPPlanner + - name: chomp planners: - CHOMP - - plugin: stomp_moveit/StompPlannerManager + - name: stomp planners: - STOMP diff --git a/moveit_ros/benchmarks/examples/demo_panda.launch b/moveit_ros/benchmarks/examples/demo_panda.launch index f82b5ef8c8..66742658e5 100644 --- a/moveit_ros/benchmarks/examples/demo_panda.launch +++ b/moveit_ros/benchmarks/examples/demo_panda.launch @@ -8,7 +8,7 @@ - + diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch b/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch index 161cd4478b..f0d60f6105 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch @@ -8,15 +8,15 @@ - + - + - + diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml b/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml index aaa20a017d..3fc13adebc 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml @@ -18,16 +18,16 @@ benchmark_config: output_directory: /tmp/moveit_benchmarks/ queries: .* start_states: .* - planners: - - plugin: ompl_interface/OMPLPlanner + planning_pipelines: + - name: ompl planners: - RRTConnectkConfigDefault - BKPIECEkConfigDefault - KPIECEkConfigDefault - - plugin: chomp_interface/CHOMPPlanner + - name: chomp planners: - CHOMP - - plugin: stomp_moveit/StompPlannerManager + - name: stomp planners: - STOMP diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch b/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch index 205bab2081..25b0a50715 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch @@ -8,15 +8,15 @@ - + - + - + diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index 9dfc5b8023..92a0a82a36 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include @@ -212,8 +212,7 @@ class BenchmarkExecutor BenchmarkOptions options_; - std::shared_ptr> planner_plugin_loader_; - std::map planner_interfaces_; + std::map planning_pipelines_; std::vector benchmark_data_; diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h index dbd46c4cdb..0006d42f39 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h @@ -68,8 +68,8 @@ class BenchmarkOptions const std::string& getPathConstraintRegex() const; const std::string& getTrajectoryConstraintRegex() const; void getGoalOffsets(std::vector& offsets) const; - const std::map>& getPlannerConfigurations() const; - void getPlannerPluginList(std::vector& plugin_list) const; + const std::map>& getPlanningPipelineConfigurations() const; + void getPlanningPipelineNames(std::vector& planning_pipeline_names) const; const std::string& getWorkspaceFrameID() const; const moveit_msgs::WorkspaceParameters& getWorkspaceParameters() const; @@ -103,7 +103,7 @@ class BenchmarkOptions double goal_offsets[6]; /// planner configurations - std::map> planners_; + std::map> planning_pipelines_; moveit_msgs::WorkspaceParameters workspace_; }; diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index b48dd98a95..4f717cd97a 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -71,17 +71,6 @@ BenchmarkExecutor::BenchmarkExecutor(const std::string& robot_description_param) tcs_ = nullptr; psm_ = new planning_scene_monitor::PlanningSceneMonitor(robot_description_param); planning_scene_ = psm_->getPlanningScene(); - - // Initialize the class loader for planner plugins - try - { - planner_plugin_loader_.reset(new pluginlib::ClassLoader( - "moveit_core", "planning_interface::PlannerManager")); - } - catch (pluginlib::PluginlibException& ex) - { - ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); - } } BenchmarkExecutor::~BenchmarkExecutor() @@ -94,46 +83,39 @@ BenchmarkExecutor::~BenchmarkExecutor() delete psm_; } -void BenchmarkExecutor::initialize(const std::vector& plugin_classes) +void BenchmarkExecutor::initialize(const std::vector& planning_pipeline_names) { - planner_interfaces_.clear(); + planning_pipelines_.clear(); - // Load the planning plugins - const std::vector& classes = planner_plugin_loader_->getDeclaredClasses(); - - for (const std::string& planner_plugin_name : plugin_classes) + ros::NodeHandle pnh("~"); + for (const std::string& planning_pipeline_name : planning_pipeline_names) { - std::vector::const_iterator it = std::find(classes.begin(), classes.end(), planner_plugin_name); - if (it == classes.end()) - { - ROS_ERROR("Failed to find plugin_class %s", planner_plugin_name.c_str()); - return; - } - - try - { - planning_interface::PlannerManagerPtr p = planner_plugin_loader_->createUniqueInstance(planner_plugin_name); - p->initialize(planning_scene_->getRobotModel(), ""); + // Initialize planning pipelines from configured child namespaces + ros::NodeHandle child_nh(pnh, planning_pipeline_name); + planning_pipeline::PlanningPipelinePtr pipeline(new planning_pipeline::PlanningPipeline( + planning_scene_->getRobotModel(), child_nh, "planning_plugin", "request_adapters")); - p->getPlannerConfigurations(); - planner_interfaces_[planner_plugin_name] = p; - } - catch (pluginlib::PluginlibException& ex) + // Verify the pipeline has successfully initialized a planner + if (!pipeline->getPlannerManager()) { - ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what()); + ROS_ERROR("Failed to initialize planning pipeline '%s'", planning_pipeline_name.c_str()); + continue; } + + // Disable visualizations and store pipeline + pipeline->displayComputedMotionPlans(false); + pipeline->checkSolutionPaths(false); + planning_pipelines_[planning_pipeline_name] = pipeline; } - // error check - if (planner_interfaces_.empty()) - ROS_ERROR("No planning plugins have been loaded. Nothing to do for the benchmarking service."); + // Error check + if (planning_pipelines_.empty()) + ROS_ERROR("No planning pipelines have been loaded. Nothing to do for the benchmarking service."); else { - std::stringstream ss; - for (std::map::const_iterator it = planner_interfaces_.begin(); - it != planner_interfaces_.end(); ++it) - ss << it->first << " "; - ROS_INFO("Available planner instances: %s", ss.str().c_str()); + ROS_INFO("Available planning pipelines:"); + for (const std::pair& entry : planning_pipelines_) + ROS_INFO_STREAM("Pipeline: " << entry.first << ", Planner: " << entry.second->getPlannerPluginName()); } } @@ -206,9 +188,9 @@ void BenchmarkExecutor::addQueryCompletionEvent(const QueryCompletionEventFuncti bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& opts) { - if (planner_interfaces_.empty()) + if (planning_pipelines_.empty()) { - ROS_ERROR("No planning interfaces configured. Did you call BenchmarkExecutor::initialize?"); + ROS_ERROR("No planning pipelines configured. Did you call BenchmarkExecutor::initialize?"); return false; } @@ -217,7 +199,7 @@ bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& opts) if (initializeBenchmarks(opts, scene_msg, queries)) { - if (!queriesAndPlannersCompatible(queries, opts.getPlannerConfigurations())) + if (!queriesAndPlannersCompatible(queries, opts.getPlanningPipelineConfigurations())) return false; for (std::size_t i = 0; i < queries.size(); ++i) @@ -241,7 +223,7 @@ bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& opts) ROS_INFO("Benchmarking query '%s' (%lu of %lu)", queries[i].name.c_str(), i + 1, queries.size()); ros::WallTime start_time = ros::WallTime::now(); - runBenchmark(queries[i].request, options_.getPlannerConfigurations(), options_.getNumRuns()); + runBenchmark(queries[i].request, options_.getPlanningPipelineConfigurations(), options_.getNumRuns()); double duration = (ros::WallTime::now() - start_time).toSec(); for (QueryCompletionEventFunction& query_end_fn : query_end_fns_) @@ -259,14 +241,15 @@ bool BenchmarkExecutor::queriesAndPlannersCompatible(const std::vector>& planners) { // Make sure that the planner interfaces can service the desired queries - for (std::map::const_iterator it = planner_interfaces_.begin(); - it != planner_interfaces_.end(); ++it) + for (const std::pair& pipeline_entry : planning_pipelines_) { for (const BenchmarkRequest& request : requests) { - if (!it->second->canServiceRequest(request.request)) + if (!pipeline_entry.second->getPlannerManager()->canServiceRequest(request.request)) { - ROS_ERROR("Interface '%s' cannot service the benchmark request '%s'", it->first.c_str(), request.name.c_str()); + ROS_ERROR("Interface '%s' in pipeline '%s' cannot service the benchmark request '%s'", + pipeline_entry.second->getPlannerPluginName().c_str(), pipeline_entry.first.c_str(), + request.name.c_str()); return false; } } @@ -278,7 +261,7 @@ bool BenchmarkExecutor::queriesAndPlannersCompatible(const std::vector& requests) { - if (!plannerConfigurationsExist(opts.getPlannerConfigurations(), opts.getGroupName())) + if (!plannerConfigurationsExist(opts.getPlanningPipelineConfigurations(), opts.getGroupName())) return false; std::vector start_states; @@ -484,6 +467,10 @@ void BenchmarkExecutor::createRequestCombinations(const BenchmarkRequest& breque { for (const StartState& start_state : start_states) { + // Skip start states that have the same name as the goal + if (start_state.name == brequest.name) + continue; + BenchmarkRequest new_brequest = brequest; new_brequest.request.start_state = start_state.state; @@ -504,33 +491,29 @@ void BenchmarkExecutor::createRequestCombinations(const BenchmarkRequest& breque } } -bool BenchmarkExecutor::plannerConfigurationsExist(const std::map>& planners, - const std::string& group_name) +bool BenchmarkExecutor::plannerConfigurationsExist( + const std::map>& pipeline_configurations, const std::string& group_name) { // Make sure planner plugins exist - for (std::map>::const_iterator it = planners.begin(); it != planners.end(); - ++it) + for (const std::pair>& pipeline_config_entry : pipeline_configurations) { - bool plugin_exists = false; - for (std::map::const_iterator planner_it = - planner_interfaces_.begin(); - planner_it != planner_interfaces_.end() && !plugin_exists; ++planner_it) + bool pipeline_exists = false; + for (const std::pair& pipeline_entry : planning_pipelines_) { - plugin_exists = planner_it->first == it->first; + pipeline_exists = pipeline_entry.first == pipeline_config_entry.first; } - if (!plugin_exists) + if (!pipeline_exists) { - ROS_ERROR("Planning plugin '%s' does NOT exist", it->first.c_str()); + ROS_ERROR("Planning pipeline '%s' does NOT exist", pipeline_config_entry.first.c_str()); return false; } } - // Make sure planning algorithms exist within those plugins - for (std::map>::const_iterator it = planners.begin(); it != planners.end(); - ++it) + // Make sure planners exist within those pipelines + for (const std::pair>& entry : pipeline_configurations) { - planning_interface::PlannerManagerPtr pm = planner_interfaces_[it->first]; + planning_interface::PlannerManagerPtr pm = planning_pipelines_[entry.first]->getPlannerManager(); const planning_interface::PlannerConfigurationMap& config_map = pm->getPlannerConfigurations(); // if the planner is chomp or stomp skip this function and return true for checking planner configurations for the @@ -539,24 +522,22 @@ bool BenchmarkExecutor::plannerConfigurationsExist(const std::mapgetDescription().compare("stomp") || pm->getDescription().compare("chomp")) continue; - for (std::size_t i = 0; i < it->second.size(); ++i) + for (std::size_t i = 0; i < entry.second.size(); ++i) { bool planner_exists = false; - for (planning_interface::PlannerConfigurationMap::const_iterator map_it = config_map.begin(); - map_it != config_map.end() && !planner_exists; ++map_it) + for (const std::pair& config_entry : config_map) { - std::string planner_name = group_name + "[" + it->second[i] + "]"; - planner_exists = (map_it->second.group == group_name && map_it->second.name == planner_name); + std::string planner_name = group_name + "[" + entry.second[i] + "]"; + planner_exists = (config_entry.second.group == group_name && config_entry.second.name == planner_name); } if (!planner_exists) { - ROS_ERROR("Planner '%s' does NOT exist for group '%s' in pipeline '%s'", it->second[i].c_str(), - group_name.c_str(), it->first.c_str()); + ROS_ERROR("Planner '%s' does NOT exist for group '%s' in pipeline '%s'", entry.second[i].c_str(), + group_name.c_str(), entry.first.c_str()); std::cout << "There are " << config_map.size() << " planner entries: " << std::endl; - for (planning_interface::PlannerConfigurationMap::const_iterator map_it = config_map.begin(); - map_it != config_map.end() && !planner_exists; ++map_it) - std::cout << map_it->second.name << std::endl; + for (const auto& config_map_entry : config_map) + std::cout << config_map_entry.second.name << std::endl; return false; } } @@ -756,21 +737,24 @@ bool BenchmarkExecutor::loadTrajectoryConstraints(const std::string& regex, } void BenchmarkExecutor::runBenchmark(moveit_msgs::MotionPlanRequest request, - const std::map>& planners, int runs) + const std::map>& pipeline_map, int runs) { benchmark_data_.clear(); unsigned int num_planners = 0; - for (const std::pair>& planner : planners) - num_planners += planner.second.size(); + for (const std::pair>& pipeline_entry : pipeline_map) + num_planners += pipeline_entry.second.size(); boost::progress_display progress(num_planners * runs, std::cout); - // Iterate through all planner plugins - for (const std::pair>& planner : planners) + // Iterate through all planning pipelines + for (const std::pair>& pipeline_entry : pipeline_map) { - // Iterate through all planners associated with the plugin - for (std::size_t i = 0; i < planner.second.size(); ++i) + planning_pipeline::PlanningPipelinePtr planning_pipeline = planning_pipelines_[pipeline_entry.first]; + // Use the planning context if the pipeline only contains the planner plugin + bool use_planning_context = planning_pipeline->getAdapterPluginNames().empty(); + // Iterate through all planners configured for the pipeline + for (const std::string& planner_id : pipeline_entry.second) { // This container stores all of the benchmark data for this planner PlannerBenchmarkData planner_data(runs); @@ -778,14 +762,17 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::MotionPlanRequest request, std::vector responses(runs); std::vector solved(runs); - request.planner_id = planner.second[i]; + request.planner_id = planner_id; // Planner start events for (PlannerStartEventFunction& planner_start_fn : planner_start_fns_) planner_start_fn(request, planner_data); - planning_interface::PlanningContextPtr context = - planner_interfaces_[planner.first]->getPlanningContext(planning_scene_, request); + planning_interface::PlanningContextPtr planning_context; + if (use_planning_context) + planning_context = planning_pipeline->getPlannerManager()->getPlanningContext(planning_scene_, request); + + // Iterate runs for (int j = 0; j < runs; ++j) { // Pre-run events @@ -794,7 +781,23 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::MotionPlanRequest request, // Solve problem ros::WallTime start = ros::WallTime::now(); - solved[j] = context->solve(responses[j]); + if (use_planning_context) + { + solved[j] = planning_context->solve(responses[j]); + } + else + { + // The planning pipeline does not support MotionPlanDetailedResponse + planning_interface::MotionPlanResponse response; + solved[j] = planning_pipeline->generatePlan(planning_scene_, request, response); + responses[j].error_code_ = response.error_code_; + if (response.trajectory_) + { + responses[j].description_.push_back("plan"); + responses[j].trajectory_.push_back(response.trajectory_); + responses[j].processing_time_.push_back(response.planning_time_); + } + } double total_time = (ros::WallTime::now() - start).toSec(); // Collect data @@ -1037,11 +1040,11 @@ bool BenchmarkExecutor::computeTrajectoryDistance(const robot_trajectory::RobotT void BenchmarkExecutor::writeOutput(const BenchmarkRequest& brequest, const std::string& start_time, double benchmark_duration) { - const std::map>& planners = options_.getPlannerConfigurations(); + const std::map>& pipelines = options_.getPlanningPipelineConfigurations(); size_t num_planners = 0; - for (const std::pair>& planner : planners) - num_planners += planner.second.size(); + for (const std::pair>& pipeline : pipelines) + num_planners += pipeline.second.size(); std::string hostname = getHostname(); if (hostname.empty()) @@ -1091,12 +1094,12 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& brequest, const std: out << num_planners << " planners" << std::endl; size_t run_id = 0; - for (const std::pair>& planner : planners) + for (const std::pair>& pipeline : pipelines) { - for (std::size_t i = 0; i < planner.second.size(); ++i, ++run_id) + for (std::size_t i = 0; i < pipeline.second.size(); ++i, ++run_id) { - // Write the name of the planner. - out << planner.second[i] << std::endl; + // Write the name of the planner and the used pipeline + out << pipeline.second[i] << " (" << pipeline.first << ")" << std::endl; // in general, we could have properties specific for a planner; // right now, we do not include such properties diff --git a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp index 134d1d4017..1ed9c512a1 100644 --- a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp @@ -142,16 +142,16 @@ void BenchmarkOptions::getGoalOffsets(std::vector& offsets) const memcpy(&offsets[0], goal_offsets, 6 * sizeof(double)); } -const std::map>& BenchmarkOptions::getPlannerConfigurations() const +const std::map>& BenchmarkOptions::getPlanningPipelineConfigurations() const { - return planners_; + return planning_pipelines_; } -void BenchmarkOptions::getPlannerPluginList(std::vector& plugin_list) const +void BenchmarkOptions::getPlanningPipelineNames(std::vector& planning_pipeline_names) const { - plugin_list.clear(); - for (const std::pair>& planner : planners_) - plugin_list.push_back(planner.first); + planning_pipeline_names.clear(); + for (const std::pair>& planning_pipeline : planning_pipelines_) + planning_pipeline_names.push_back(planning_pipeline.first); } const std::string& BenchmarkOptions::getWorkspaceFrameID() const @@ -237,48 +237,48 @@ void BenchmarkOptions::readWorkspaceParameters(ros::NodeHandle& nh) void BenchmarkOptions::readPlannerConfigs(ros::NodeHandle& nh) { - planners_.clear(); + planning_pipelines_.clear(); - XmlRpc::XmlRpcValue planner_configs; - if (nh.getParam("benchmark_config/planners", planner_configs)) + XmlRpc::XmlRpcValue pipeline_configs; + if (nh.getParam("benchmark_config/planning_pipelines", pipeline_configs)) { - if (planner_configs.getType() != XmlRpc::XmlRpcValue::TypeArray) + if (pipeline_configs.getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_ERROR("Expected a list of planner configurations to benchmark"); + ROS_ERROR("Expected a list of planning pipeline configurations to benchmark"); return; } - for (int i = 0; i < planner_configs.size(); ++i) // NOLINT(modernize-loop-convert) + for (int i = 0; i < pipeline_configs.size(); ++i) // NOLINT(modernize-loop-convert) { - if (planner_configs[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) + if (pipeline_configs[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_WARN("Improper YAML type for planner configurations"); + ROS_WARN("Improper YAML type for planning pipeline configurations"); continue; } - if (!planner_configs[i].hasMember("plugin") || !planner_configs[i].hasMember("planners")) + if (!pipeline_configs[i].hasMember("name") || !pipeline_configs[i].hasMember("planners")) { ROS_WARN("Malformed YAML for planner configurations"); continue; } - if (planner_configs[i]["planners"].getType() != XmlRpc::XmlRpcValue::TypeArray) + if (pipeline_configs[i]["planners"].getType() != XmlRpc::XmlRpcValue::TypeArray) { ROS_WARN("Expected a list of planners to benchmark"); continue; } - const std::string plugin = planner_configs[i]["plugin"]; - ROS_INFO("Reading in planner names for plugin '%s'", plugin.c_str()); + const std::string pipeline_name = pipeline_configs[i]["name"]; + ROS_INFO("Reading in planner names for planning pipeline '%s'", pipeline_name.c_str()); std::vector planners; - planners.reserve(planner_configs[i]["planners"].size()); - for (int j = 0; j < planner_configs[i]["planners"].size(); ++j) - planners.emplace_back(planner_configs[i]["planners"][j]); + planners.reserve(pipeline_configs[i]["planners"].size()); + for (int j = 0; j < pipeline_configs[i]["planners"].size(); ++j) + planners.emplace_back(pipeline_configs[i]["planners"][j]); for (std::size_t j = 0; j < planners.size(); ++j) ROS_INFO(" [%lu]: %s", j, planners[j].c_str()); - planners_[plugin] = planners; + planning_pipelines_[pipeline_name] = planners; } } } diff --git a/moveit_ros/benchmarks/src/RunBenchmark.cpp b/moveit_ros/benchmarks/src/RunBenchmark.cpp index 14082cbcdc..19496e54e6 100644 --- a/moveit_ros/benchmarks/src/RunBenchmark.cpp +++ b/moveit_ros/benchmarks/src/RunBenchmark.cpp @@ -51,9 +51,9 @@ int main(int argc, char** argv) // Setup benchmark server moveit_ros_benchmarks::BenchmarkExecutor server; - std::vector plugins; - opts.getPlannerPluginList(plugins); - server.initialize(plugins); + std::vector planning_pipelines; + opts.getPlanningPipelineNames(planning_pipelines); + server.initialize(planning_pipelines); // Running benchmarks if (!server.runBenchmarks(opts)) From af106bed1f1188ce85b4c7b6558c9d26ea773a65 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Thu, 15 Aug 2019 16:53:09 +0300 Subject: [PATCH 033/612] newRobotStateCallback(): use new .hide member of DisplayRobotState msg to visually hide the robot --- .../robot_state_rviz_plugin/src/robot_state_display.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 0865765a83..d843923605 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -314,6 +314,9 @@ void RobotStateDisplay::newRobotStateCallback(const moveit_msgs::DisplayRobotSta setStatus(rviz::StatusProperty::Error, "RobotState", e.what()); return; } + + robot_->setVisible(!state_msg->hide); + update_state_ = true; } From f6a24033bf784d4a299bd185a92480fa6bb592ef Mon Sep 17 00:00:00 2001 From: Omid Heidari Date: Thu, 15 Aug 2019 16:17:10 -0600 Subject: [PATCH 034/612] MVP TrajOpt Planner Plugin (#1593) * added solve from tesseract. added launch file improved test_trajopt node edited package.xml * TrajOpt Planner (Empty Template) (#1478) * Created EmptyPlan, a MoveIt planning plugin template * added trajopt from example_planner * fixed trajopt_planner_mannager * modified emaptyplan_planner_manager.cpp * applied Dave's modifications * The rest of the modifications from Dave's comments * removed moveit_ros_planning dependency from package.xml * fixed clangformat error and dependency issu * added include directory * edited package.xml * added include * remove include * remove version_gte from package.xml * updated the setup assitant to work with trajopt planner * removed unsuded part in initialize() from trajopt_planner_manager.cpp, kept the initialize() itself for later use * Update moveit_planners/trajopt/src/trajopt_planner_manager.cpp Co-Authored-By: Dave Coleman * added README added description to trajopt_interface_plugin_description.xml * updated resp req updated planning context working trajopt without any velocity constraint * added trajopt_interface class * changed the architectrue and used TrajoptInterface class to contain the solve function details. Created a TrajOptInterface object in TrajoptPlanningContext class instead * added kinematic_terms in problem_description, added comments anout term_type added ProblemInfo, TermInfo, InitInfo, BasicInfo in kinematic_terms, added operator() added ConstructProblem added some param for problem_info * moved rotVec and concat from kinematics_term header to implementation in problem_Description, added term_type used MotionPlaneRequest from planning_interface instead that of moveit_msgs for solve function input added generateInitiaTrajectory added basic_info to problem_info * added panda_omple_planning added checkParameterSize function in problem_description added JointPoseTermInfo::hatch in problem_description added res.error.code.val to check the request in trajopt_interface.cpp convert req.goal_constraint to JointPoseTermInfo in trajopt_interface.cpp feed the response with the trajopt solution in trajopt_interface.cpp remove comments from trajopt_planning_context.cpp * addressed the comments fixed start state corrected the yaml file multiple joint constraints * addressed some comments leave setup assistant changes for later on another PR cleaned up test_trajopt added unittest * added kinematic DOF of the active joint model group to TrajOptProblem class added name_ for ROS_DEBUG_STREAM_NAMED addressed the comments moved launch file to moveit_tutorial moved yaml file to panda_moveit_config used bullet for collision checking * handled reading the request from MotionPlanning Display in RViz * Added comments about initial trajectory changed my email address renamed rotVec and concat added tesseract license * clang format * applied Henning's comments * added catkin ignore to ignore this package for now as it has dependencies. The next PR wont have any dependencies * clang format * addressed the comments * renamed hatch to addObjectiveTerms * added a unittest for goal tolerance * addressed Dave's comments * added catkin_ignore * addressed Bryce's comments --- .gitignore | 6 +- moveit_planners/trajopt/CATKIN_IGNORE | 0 moveit_planners/trajopt/CMakeLists.txt | 113 ++++ moveit_planners/trajopt/README.md | 1 + .../trajopt_interface/kinematic_terms.h | 93 +++ .../trajopt_interface/problem_description.h | 367 +++++++++++ .../trajopt_interface/trajopt_interface.h | 77 +++ .../trajopt_planning_context.h | 32 + moveit_planners/trajopt/package.xml | 32 + .../trajopt/src/kinematic_terms.cpp | 64 ++ .../trajopt/src/problem_description.cpp | 599 ++++++++++++++++++ .../trajopt/src/trajopt_interface.cpp | 463 ++++++++++++++ .../trajopt/src/trajopt_planner_manager.cpp | 141 +++++ .../trajopt/src/trajopt_planning_context.cpp | 76 +++ .../trajopt/test/trajectory_test.cpp | 212 +++++++ .../trajopt/test/trajectory_test.test | 10 + .../trajopt_interface_plugin_description.xml | 7 + .../launch/run_benchmark_trajopt.launch | 22 + .../trajopt_planning_pipeline.launch.xml | 22 + 19 files changed, 2334 insertions(+), 3 deletions(-) create mode 100644 moveit_planners/trajopt/CATKIN_IGNORE create mode 100644 moveit_planners/trajopt/CMakeLists.txt create mode 100644 moveit_planners/trajopt/README.md create mode 100644 moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h create mode 100644 moveit_planners/trajopt/include/trajopt_interface/problem_description.h create mode 100644 moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h create mode 100644 moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h create mode 100644 moveit_planners/trajopt/package.xml create mode 100644 moveit_planners/trajopt/src/kinematic_terms.cpp create mode 100644 moveit_planners/trajopt/src/problem_description.cpp create mode 100644 moveit_planners/trajopt/src/trajopt_interface.cpp create mode 100644 moveit_planners/trajopt/src/trajopt_planner_manager.cpp create mode 100644 moveit_planners/trajopt/src/trajopt_planning_context.cpp create mode 100644 moveit_planners/trajopt/test/trajectory_test.cpp create mode 100644 moveit_planners/trajopt/test/trajectory_test.test create mode 100644 moveit_planners/trajopt/trajopt_interface_plugin_description.xml create mode 100644 moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch create mode 100644 moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml diff --git a/.gitignore b/.gitignore index 9877f9248d..808fe85341 100644 --- a/.gitignore +++ b/.gitignore @@ -51,10 +51,10 @@ qtcreator-* # Vim *.swp -# Catkin custom files -CATKIN_IGNORE - # Continous Integration .moveit_ci *.pyc + +#gdb +*.gdb diff --git a/moveit_planners/trajopt/CATKIN_IGNORE b/moveit_planners/trajopt/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_planners/trajopt/CMakeLists.txt b/moveit_planners/trajopt/CMakeLists.txt new file mode 100644 index 0000000000..f93e000d34 --- /dev/null +++ b/moveit_planners/trajopt/CMakeLists.txt @@ -0,0 +1,113 @@ +cmake_minimum_required(VERSION 2.8.12) +project(moveit_planners_trajopt) + +add_compile_options(-std=c++14) + +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +find_package(catkin REQUIRED + COMPONENTS + moveit_core + moveit_visual_tools + moveit_ros_planning + moveit_ros_planning_interface + pluginlib + roscpp + rosparam_shortcuts + trajopt +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES + ${PROJECT_NAME} + INCLUDE_DIRS + CATKIN_DEPENDS + roscpp + pluginlib + moveit_core + moveit_visual_tools + moveit_ros_planning_interface + rosparam_shortcuts +) + +# The following include_directory should have include folder of the new planner. +include_directories( + include + ${catkin_INCLUDE_DIRS} + SYSTEM + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +add_library( + ${PROJECT_NAME} + src/trajopt_interface.cpp + src/problem_description.cpp + src/kinematic_terms.cpp + src/trajopt_planning_context.cpp +) + +target_link_libraries( + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) + +set_target_properties( + ${PROJECT_NAME} + PROPERTIES + VERSION + "${${PROJECT_NAME}_VERSION}" +) + +# TrajOpt planning plugin +add_library(moveit_trajopt_planner_plugin src/trajopt_planner_manager.cpp) +set_target_properties(moveit_trajopt_planner_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_trajopt_planner_plugin ${PROJECT_NAME} ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark executables and/or libraries for installation +install( + TARGETS + moveit_trajopt_planner_plugin + ARCHIVE DESTINATION + ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION + ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION + ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install( + DIRECTORY + include/trajopt_interface/ + DESTINATION + ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(FILES trajopt_interface_plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +############# +## Testing ## +############# + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + add_rostest_gtest(trajectory_test test/trajectory_test.test test/trajectory_test.cpp) + target_link_libraries( + trajectory_test + ${LIBRARY_NAME} + ${catkin_LIBRARIES} + ${PROJECT_NAME} + ) + +endif() diff --git a/moveit_planners/trajopt/README.md b/moveit_planners/trajopt/README.md new file mode 100644 index 0000000000..98824eadb2 --- /dev/null +++ b/moveit_planners/trajopt/README.md @@ -0,0 +1 @@ +As of August 2019, this is a work in progress towards adding trajopt motion planning algorithm to MoveIt as a planner plugin. \ No newline at end of file diff --git a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h new file mode 100644 index 0000000000..96274cf9c7 --- /dev/null +++ b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h @@ -0,0 +1,93 @@ +#pragma once + +#include + +#include + +namespace trajopt_interface +{ +/** + * @brief Extracts the vector part of quaternion + */ +inline Eigen::Vector3d quaternionRotationVector(const Eigen::Matrix3d& matrix) +{ + Eigen::Quaterniond quaternion; + quaternion = matrix; + return Eigen::Vector3d(quaternion.x(), quaternion.y(), quaternion.z()); +} + +/** + * @brief Appends b to a of type VectorXd + */ +inline Eigen::VectorXd concatVector(const Eigen::VectorXd& vector_a, const Eigen::VectorXd& vector_b) +{ + Eigen::VectorXd out(vector_a.size() + vector_b.size()); + out.topRows(vector_a.size()) = vector_a; + out.middleRows(vector_a.size(), vector_b.size()) = vector_b; + return out; +} + +/** + * @brief Appends b to a of type T + */ +template +inline std::vector concatVector(const std::vector& vector_a, const std::vector& vector_b) +{ + std::vector out; + out.insert(out.end(), vector_a.begin(), vector_a.end()); + out.insert(out.end(), vector_b.begin(), vector_b.end()); + return out; +} + +/** + * @brief Used to calculate the error for StaticCartPoseTermInfo + * This is converted to a cost or constraint using TrajOptCostFromErrFunc or TrajOptConstraintFromErrFunc + */ +struct CartPoseErrCalculator : public sco::VectorOfVector +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Isometry3d target_pose_inv_; + planning_scene::PlanningSceneConstPtr planning_scene_; + std::string link_; + Eigen::Isometry3d tcp_; + + CartPoseErrCalculator(const Eigen::Isometry3d& pose, const planning_scene::PlanningSceneConstPtr planning_scene, + const std::string& link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) + : target_pose_inv_(pose.inverse()), planning_scene_(planning_scene), link_(link), tcp_(tcp) + { + } + + Eigen::VectorXd operator()(const Eigen::VectorXd& dof_vals) const override; +}; + +// TODO(omid): The following should be added and adjusted from trajopt +// JointPosEqCost +// JointPosIneqCost +// JointPosEqConstraint +// JointPosIneqConstraint + +struct JointVelErrCalculator : sco::VectorOfVector +{ + /** @brief Velocity target */ + double target_; + /** @brief Upper tolerance */ + double upper_tol_; + /** @brief Lower tolerance */ + double lower_tol_; + JointVelErrCalculator() : target_(0.0), upper_tol_(0.0), lower_tol_(0.0) + { + } + JointVelErrCalculator(double target, double upper_tol, double lower_tol) + : target_(target), upper_tol_(upper_tol), lower_tol_(lower_tol) + { + } + + Eigen::VectorXd operator()(const Eigen::VectorXd& var_vals) const; +}; + +struct JointVelJacobianCalculator : sco::MatrixOfVector +{ + Eigen::MatrixXd operator()(const Eigen::VectorXd& var_vals) const; +}; + +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h new file mode 100644 index 0000000000..825c82262d --- /dev/null +++ b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h @@ -0,0 +1,367 @@ +#pragma once +#include +#include +#include + +#include +#include + +#include + +namespace trajopt_interface +{ +/** +@brief Used to apply cost/constraint to joint-space velocity + +Term is applied to every step between first_step and last_step. It applies two limits, upper_limits/lower_limits, +to the joint velocity subject to the following cases. + +* term_type = TT_COST +** upper_limit = lower_limit = 0 - Cost is applied with a SQUARED error scaled for each joint by coeffs +** upper_limit != lower_limit - 2 hinge costs are applied scaled for each joint by coeffs. If velocity < upper_limit and +velocity > lower_limit, no penalty. + +* term_type = TT_CNT +** upper_limit = lower_limit = 0 - Equality constraint is applied +** upper_limit != lower_limit - 2 Inequality constraints are applied. These are both satisfied when velocity < +upper_limit and velocity > lower_limit + +Note: coeffs, upper_limits, and lower_limits are optional. If a term is not given it will default to 0 for all joints. +If one value is given, this will be broadcast to all joints. + +Note: Velocity is calculated numerically using forward finite difference + +\f{align*}{ + cost = \sum_{t=0}^{T-2} \sum_j c_j (x_{t+1,j} - x_{t,j})^2 +\f} +where j indexes over DOF, and \f$c_j\f$ are the coeffs. +*/ + +struct TermInfo; +MOVEIT_CLASS_FORWARD(TermInfo); + +class TrajOptProblem; +MOVEIT_CLASS_FORWARD(TrajOptProblem); + +struct JointPoseTermInfo; +MOVEIT_CLASS_FORWARD(JointPoseTermInfo); + +struct CartPoseTermInfo; +MOVEIT_CLASS_FORWARD(CartPoseTermInfo); + +struct JointVelTermInfo; +MOVEIT_CLASS_FORWARD(JointVelTermInfo); + +struct ProblemInfo; +TrajOptProblemPtr ConstructProblem(const ProblemInfo&); + +enum TermType +{ + TT_COST = 0x1, // 0000 0001 + TT_CNT = 0x2, // 0000 0010 + TT_USE_TIME = 0x4, // 0000 0100 +}; + +struct BasicInfo +{ + /** @brief If true, first time step is fixed with a joint level constraint + If this is false, the starting point of the trajectory will + not be the current position of the robot. The use case example is: if we are trying to execute a process like + sanding the critical part which is the actual process path not how we get to the start of the process path. So we + plan the + process path first leaving the start free to hopefully get the most optimal and then we plan from the current + location with + start fixed to the start of the process path. It depends on what we want the default behavior to be + */ + bool start_fixed; + /** @brief Number of time steps (rows) in the optimization matrix */ + int n_steps; + sco::IntVec dofs_fixed; // optional + sco::ModelType convex_solver; // which convex solver to use + + /** @brief If true, the last column in the optimization matrix will be 1/dt */ + bool use_time = false; + /** @brief The upper limit of 1/dt values allowed in the optimization*/ + double dt_upper_lim = 1.0; + /** @brief The lower limit of 1/dt values allowed in the optimization*/ + double dt_lower_lim = 1.0; +}; + +/** +Initialization info read from json +*/ +struct InitInfo +{ + /** @brief Methods of initializing the optimization matrix. This defines how robot moves from the current + state to the start state + + STATIONARY: Initializes all joint values to the initial value (the current value in the env) + JOINT_INTERPOLATED: Linearly interpolates between initial value and the joint position specified in InitInfo.data + GIVEN_TRAJ: Initializes the matrix to a given trajectory + + In all cases the dt column (if present) is appended the selected method is defined. + */ + enum Type + { + STATIONARY, + JOINT_INTERPOLATED, + GIVEN_TRAJ, + }; + /** @brief Specifies the type of initialization to use */ + Type type; + /** @brief Data used during initialization. Use depends on the initialization selected. This data will be used + to create initialization matrix. We need to give the goal information to this init info + */ + trajopt::TrajArray data; + // Eigen::VectorXd data_vec; + // trajopt::TrajArray data_trajectory; + /** @brief Default value the final column of the optimization is initialized too if time is being used */ + double dt = 1.0; +}; + +/** +When cost or constraint element of JSON doc is read, one of these guys gets +constructed to hold the parameters. +Then it later gets converted to a Cost object by the addObjectiveTerms method +*/ +struct TermInfo +{ + std::string name; + int term_type; + int getSupportedTypes() + { + return supported_term_types_; + } + // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; + virtual void addObjectiveTerms(TrajOptProblem& prob) = 0; + + static TermInfoPtr fromName(const std::string& type); + + /** + * Registers a user-defined TermInfo so you can use your own cost + * see function RegisterMakers.cpp + */ + using MakerFunc = TermInfoPtr (*)(void); + static void RegisterMaker(const std::string& type, MakerFunc); + + virtual ~TermInfo() = default; + +protected: + TermInfo(int supported_term_types) : supported_term_types_(supported_term_types) + { + } + +private: + static std::map name_to_maker_; + int supported_term_types_; +}; + +struct ProblemInfo +{ +public: + BasicInfo basic_info; + sco::BasicTrustRegionSQPParameters opt_info; + std::vector cost_infos; + std::vector cnt_infos; + InitInfo init_info; + + planning_scene::PlanningSceneConstPtr planning_scene; + std::string planning_group_name; + + ProblemInfo(planning_scene::PlanningSceneConstPtr ps, const std::string& pg) + : planning_scene(ps), planning_group_name(pg) + { + } +}; + +/** + * Holds all the data for a trajectory optimization problem + * so you can modify it programmatically, e.g. add your own costs + */ +class TrajOptProblem : public sco::OptProb +{ +public: + TrajOptProblem(); + TrajOptProblem(const ProblemInfo& problem_info); + virtual ~TrajOptProblem() = default; + /** @brief Returns the values of the specified joints (start_col to num_col) for the specified timestep i.*/ + sco::VarVector GetVarRow(int i, int start_col, int num_col) + { + return matrix_traj_vars.rblock(i, start_col, num_col); + } + /** @brief Returns the values of all joints for the specified timestep i.*/ + sco::VarVector GetVarRow(int i) + { + return matrix_traj_vars.row(i); + } + /** @brief Returns the value of the specified joint j for the specified timestep i.*/ + sco::Var& GetVar(int i, int j) + { + return matrix_traj_vars.at(i, j); + } + trajopt::VarArray& GetVars() + { + return matrix_traj_vars; + } + /** @brief Returns the number of steps in the problem. This is the number of rows in the optimization matrix.*/ + int GetNumSteps() + { + return matrix_traj_vars.rows(); + } + /** @brief Returns the problem DOF. This is the number of columns in the optization matrix. + * Note that this is not necessarily the same as the kinematic DOF.*/ + int GetNumDOF() + { + return matrix_traj_vars.cols(); + } + /** @brief Returns the kinematic DOF of the active joint model group + */ + int GetActiveGroupNumDOF() + { + return dof_; + } + planning_scene::PlanningSceneConstPtr GetPlanningScene() + { + return planning_scene_; + } + void SetInitTraj(const trajopt::TrajArray& x) + { + matrix_init_traj = x; + } + trajopt::TrajArray GetInitTraj() + { + return matrix_init_traj; + } + // friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&); + /** @brief Returns TrajOptProb.has_time */ + bool GetHasTime() + { + return has_time; + } + /** @brief Sets TrajOptProb.has_time */ + void SetHasTime(bool tmp) + { + has_time = tmp; + } + +private: + /** @brief If true, the last column in the optimization matrix will be 1/dt */ + bool has_time; + /** @brief is the matrix holding the joint values of the trajectory for all timesteps */ + trajopt::VarArray matrix_traj_vars; + planning_scene::PlanningSceneConstPtr planning_scene_; + std::string planning_group_; + /** @brief Kinematic DOF of the active joint model group */ + int dof_; + trajopt::TrajArray matrix_init_traj; +}; + +/** @brief This term is used when the goal frame is fixed in cartesian space + + Set term_type == TT_COST or TT_CNT for cost or constraint. +*/ +struct CartPoseTermInfo : public TermInfo +{ + // EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** @brief Timestep at which to apply term */ + int timestep; + /** @brief Cartesian position */ + Eigen::Vector3d xyz; + /** @brief Rotation quaternion */ + Eigen::Vector4d wxyz; + /** @brief coefficients for position and rotation */ + Eigen::Vector3d pos_coeffs, rot_coeffs; + /** @brief Link which should reach desired pose */ + std::string link; + /** @brief Static transform applied to the link */ + Eigen::Isometry3d tcp; + + CartPoseTermInfo(); + + /** @brief Used to add term to pci from json */ + // void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; + /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ + void addObjectiveTerms(TrajOptProblem& prob) override; + + static TermInfoPtr create() + { + TermInfoPtr out(new CartPoseTermInfo()); + return out; + } +}; + +/** + \brief Joint space position cost + Position operates on a single point (unlike velocity, etc). This is b/c the primary usecase is joint-space + position waypoints + + \f{align*}{ + \sum_i c_i (x_i - xtarg_i)^2 + \f} + where \f$i\f$ indexes over dof and \f$c_i\f$ are coeffs + */ +struct JointPoseTermInfo : public TermInfo +{ + /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec coeffs; + /** @brief Vector of position targets. This is a required value. Size should be the DOF of the system */ + trajopt::DblVec targets; + /** @brief Vector of position upper limits. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec upper_tols; + /** @brief Vector of position lower limits. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec lower_tols; + /** @brief First time step to which the term is applied. Default: 0 */ + int first_step = 0; + /** @brief Last time step to which the term is applied. Default: prob.GetNumSteps() - 1*/ + int last_step = -1; + + /** @brief Initialize term with it's supported types */ + JointPoseTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) + { + } + + /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ + void addObjectiveTerms(TrajOptProblem& prob) override; + + static TermInfoPtr create() + { + TermInfoPtr out(new JointPoseTermInfo()); + return out; + } +}; + +struct JointVelTermInfo : public TermInfo +{ + /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec coeffs; + /** @brief Vector of velocity targets. This is a required value. Size should be the DOF of the system */ + trajopt::DblVec targets; + /** @brief Vector of velocity upper limits. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec upper_tols; + /** @brief Vector of velocity lower limits. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec lower_tols; + /** @brief First time step to which the term is applied. Default: 0*/ + int first_step = 0; + /** @brief Last time step to which the term is applied. Default: prob.GetNumSteps() - 1*/ + int last_step = -1; + + /** @brief Initialize term with it's supported types */ + JointVelTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) + { + } + + /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ + void addObjectiveTerms(TrajOptProblem& prob) override; + + static TermInfoPtr create() + { + TermInfoPtr out(new JointVelTermInfo()); + return out; + } +}; + +void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, + trajopt::TrajArray& init_traj); + +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h new file mode 100644 index 0000000000..74432fa83c --- /dev/null +++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h @@ -0,0 +1,77 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik, LLC. + * 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 PickNik 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. + *********************************************************************/ + +/* Author: Omid Heidari */ +#pragma once + +#include +#include +#include +#include "problem_description.h" + +namespace trajopt_interface +{ +MOVEIT_CLASS_FORWARD(TrajOptInterface); + +class TrajOptInterface +{ +public: + TrajOptInterface(const ros::NodeHandle& nh = ros::NodeHandle("~")); + + const sco::BasicTrustRegionSQPParameters& getParams() const + { + return params_; + } + + bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, moveit_msgs::MotionPlanDetailedResponse& res); + +protected: + /** @brief Configure everything using the param server */ + void setTrajOptParams(sco::BasicTrustRegionSQPParameters& param); + void setDefaultTrajOPtParams(); + void setProblemInfoParam(ProblemInfo& problem_info); + void setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name); + trajopt::DblVec extractStartJointValues(const planning_interface::MotionPlanRequest& req, + const std::vector& group_joint_names); + + ros::NodeHandle nh_; /// The ROS node handle + sco::BasicTrustRegionSQPParameters params_; + std::vector optimizer_callbacks_; + TrajOptProblemPtr trajopt_problem_; + std::string name_; +}; + +void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res); +} diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h new file mode 100644 index 0000000000..12676f57e7 --- /dev/null +++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h @@ -0,0 +1,32 @@ +#pragma once + +#include + +#include +#include + +namespace trajopt_interface +{ +MOVEIT_CLASS_FORWARD(TrajOptPlanningContext); + +class TrajOptPlanningContext : public planning_interface::PlanningContext +{ +public: + TrajOptPlanningContext(const std::string& name, const std::string& group, + const robot_model::RobotModelConstPtr& model); + ~TrajOptPlanningContext() override + { + } + + bool solve(planning_interface::MotionPlanResponse& res) override; + bool solve(planning_interface::MotionPlanDetailedResponse& res) override; + + bool terminate() override; + void clear() override; + +private: + moveit::core::RobotModelConstPtr robot_model_; + + TrajOptInterfacePtr trajopt_interface_; +}; +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/package.xml b/moveit_planners/trajopt/package.xml new file mode 100644 index 0000000000..050d71b5d4 --- /dev/null +++ b/moveit_planners/trajopt/package.xml @@ -0,0 +1,32 @@ + + moveit_planners_trajopt + 1.0.1 + TrajOpt planning plugin, an optimization based motion planner + + Omid Heidari + + Omid Heidari + MoveIt! Release Team + + BSD + + http://moveit.ros.org + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + pluginlib + moveit_core + moveit_ros_planning + moveit_ros_planning_interface + moveit_visual_tools + roscpp + rosparam_shortcuts + trajopt + + + + + + diff --git a/moveit_planners/trajopt/src/kinematic_terms.cpp b/moveit_planners/trajopt/src/kinematic_terms.cpp new file mode 100644 index 0000000000..92d75fb78b --- /dev/null +++ b/moveit_planners/trajopt/src/kinematic_terms.cpp @@ -0,0 +1,64 @@ +#include +#include + +#include +#include + +#include "trajopt_interface/kinematic_terms.h" + +using namespace std; +using namespace sco; +using namespace Eigen; + +namespace trajopt_interface +{ +VectorXd CartPoseErrCalculator::operator()(const VectorXd& dof_vals) const +{ + // TODO: create the actual error function from information in planning scene + VectorXd err; + return err; +} + +VectorXd JointVelErrCalculator::operator()(const VectorXd& var_vals) const +{ + assert(var_vals.rows() % 2 == 0); + // var_vals = (theta_t1, theta_t2, theta_t3 ... 1/dt_1, 1/dt_2, 1/dt_3 ...) + int half = static_cast(var_vals.rows() / 2); + int num_vels = half - 1; + // (x1-x0)*(1/dt) + VectorXd vel = (var_vals.segment(1, num_vels) - var_vals.segment(0, num_vels)).array() * + var_vals.segment(half + 1, num_vels).array(); + + // Note that for equality terms tols are 0, so error is effectively doubled + VectorXd result(vel.rows() * 2); + result.topRows(vel.rows()) = -(upper_tol_ - (vel.array() - target_)); + result.bottomRows(vel.rows()) = lower_tol_ - (vel.array() - target_); + return result; +} + +MatrixXd JointVelJacobianCalculator::operator()(const VectorXd& var_vals) const +{ + // var_vals = (theta_t1, theta_t2, theta_t3 ... 1/dt_1, 1/dt_2, 1/dt_3 ...) + int num_vals = static_cast(var_vals.rows()); + int half = num_vals / 2; + int num_vels = half - 1; + MatrixXd jac = MatrixXd::Zero(num_vels * 2, num_vals); + + for (int i = 0; i < num_vels; i++) + { + // v = (j_i+1 - j_i)*(1/dt) + // We calculate v with the dt from the second pt + int time_index = i + half + 1; + jac(i, i) = -1.0 * var_vals(time_index); + jac(i, i + 1) = 1.0 * var_vals(time_index); + jac(i, time_index) = var_vals(i + 1) - var_vals(i); + // All others are 0 + } + + // bottom half is negative velocities + jac.bottomRows(num_vels) = -jac.topRows(num_vels); + + return jac; +} + +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/src/problem_description.cpp b/moveit_planners/trajopt/src/problem_description.cpp new file mode 100644 index 0000000000..7cd678282c --- /dev/null +++ b/moveit_planners/trajopt/src/problem_description.cpp @@ -0,0 +1,599 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, John Schulman + * 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. + * + * 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 HOLDER 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. + * + * http://opensource.org/licenses/BSD-2-Clause + *********************************************************************/ + +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "trajopt_interface/problem_description.h" +#include "trajopt_interface/kinematic_terms.h" + +/** + * @brief Checks the size of the parameter given and throws if incorrect + * @param parameter The vector whose size is getting checked + * @param expected_size The expected size of the vector + * @param name The name to use when printing an error or warning + * @param apply_first If true and only one value is given, broadcast value to length of expected_size + */ +void checkParameterSize(trajopt::DblVec& parameter, const unsigned int& expected_size, const std::string& name, + const bool& apply_first = true) +{ + if (apply_first == true && parameter.size() == 1) + { + parameter = trajopt::DblVec(expected_size, parameter[0]); + ROS_INFO("1 %s given. Applying to all %i joints", name.c_str(), expected_size); + } + else if (parameter.size() != expected_size) + { + PRINT_AND_THROW(boost::format("wrong number of %s. expected %i got %i") % name % expected_size % parameter.size()); + } +} + +namespace trajopt_interface +{ +TrajOptProblem::TrajOptProblem() +{ +} + +TrajOptProblem::TrajOptProblem(const ProblemInfo& problem_info) + : OptProb(problem_info.basic_info.convex_solver) + , planning_scene_(problem_info.planning_scene) + , planning_group_(problem_info.planning_group_name) +{ + robot_model::RobotModelConstPtr robot_model = planning_scene_->getRobotModel(); + robot_state::RobotState current_state = planning_scene_->getCurrentState(); + const robot_state::JointModelGroup* joint_model_group = current_state.getJointModelGroup(planning_group_); + + moveit::core::JointBoundsVector bounds = joint_model_group->getActiveJointModelsBounds(); + dof_ = joint_model_group->getActiveJointModelNames().size(); // or bounds.size(); + + int n_steps = problem_info.basic_info.n_steps; + + ROS_INFO(" ======================================= problem_description: limits"); + Eigen::MatrixX2d limits(dof_, 2); + for (int k = 0; k < limits.size() / 2; ++k) + { + moveit::core::JointModel::Bounds bound = *bounds[k]; + // In MoveIt, joints are considered to have multiple dofs but we only have single dof joints: + moveit::core::VariableBounds joint_bound = bound.front(); + + limits(k, 0) = joint_bound.min_position_; + limits(k, 1) = joint_bound.max_position_; + + ROS_INFO("joint %i with lower bound: %f, upper bound: %f", k, joint_bound.min_position_, joint_bound.max_position_); + } + + Eigen::VectorXd lower, upper; + lower = limits.col(0); + upper = limits.col(1); + + trajopt::DblVec vlower, vupper; + std::vector names; + for (int i = 0; i < n_steps; ++i) + { + for (int j = 0; j < dof_; ++j) + { + names.push_back((boost::format("j_%i_%i") % i % j).str()); + } + vlower.insert(vlower.end(), lower.data(), lower.data() + lower.size()); + vupper.insert(vupper.end(), upper.data(), upper.data() + upper.size()); + + if (problem_info.basic_info.use_time == true) + { + vlower.insert(vlower.end(), problem_info.basic_info.dt_lower_lim); + vupper.insert(vupper.end(), problem_info.basic_info.dt_upper_lim); + names.push_back((boost::format("dt_%i") % i).str()); + } + } + + sco::VarVector trajvarvec = createVariables(names, vlower, vupper); + matrix_traj_vars = trajopt::VarArray(n_steps, dof_ + (problem_info.basic_info.use_time ? 1 : 0), trajvarvec.data()); + // matrix_traj_vars is essentialy a matrix of elements like: + // j_0_0, j_0_1 ... + // j_1_0, j_1_1 ... + // its size is n_steps by n_dof +} + +TrajOptProblemPtr ConstructProblem(const ProblemInfo& pci) +{ + const BasicInfo& bi = pci.basic_info; + int n_steps = bi.n_steps; + + bool use_time = false; + // Check that all costs and constraints support the types that are specified in pci + for (TermInfoPtr cost : pci.cost_infos) + { + if (cost->term_type & TT_CNT) + ROS_WARN("%s is listed as a type TT_CNT but was added to cost_infos", (cost->name).c_str()); + if (!(cost->getSupportedTypes() & TT_COST)) + PRINT_AND_THROW(boost::format("%s is only a constraint, but you listed it as a cost") % cost->name); + if (cost->term_type & TT_USE_TIME) + { + use_time = true; + if (!(cost->getSupportedTypes() & TT_USE_TIME)) + PRINT_AND_THROW(boost::format("%s does not support time, but you listed it as a using time") % cost->name); + } + } + for (TermInfoPtr cnt : pci.cnt_infos) + { + if (cnt->term_type & TT_COST) + ROS_WARN("%s is listed as a type TT_COST but was added to cnt_infos", (cnt->name).c_str()); + if (!(cnt->getSupportedTypes() & TT_CNT)) + PRINT_AND_THROW(boost::format("%s is only a cost, but you listed it as a constraint") % cnt->name); + if (cnt->term_type & TT_USE_TIME) + { + use_time = true; + if (!(cnt->getSupportedTypes() & TT_USE_TIME)) + PRINT_AND_THROW(boost::format("%s does not support time, but you listed it as a using time") % cnt->name); + } + } + + // Check that if a cost or constraint uses time, basic_info is set accordingly + if ((use_time == true) && (pci.basic_info.use_time == false)) + PRINT_AND_THROW("A term is using time and basic_info is not set correctly. Try basic_info.use_time = true"); + + // This could be removed in the future once we are sure that all costs are + if ((use_time == false) && (pci.basic_info.use_time == true)) + PRINT_AND_THROW("No terms use time and basic_info is not set correctly. Try basic_info.use_time = false"); + + TrajOptProblemPtr prob(new TrajOptProblem(pci)); + + // Generate initial trajectory and check its size + robot_model::RobotModelConstPtr robot_model = pci.planning_scene->getRobotModel(); + robot_state::RobotState current_state = pci.planning_scene->getCurrentState(); + + const robot_state::JointModelGroup* joint_model_group = current_state.getJointModelGroup(pci.planning_group_name); + int n_dof = prob->GetNumDOF(); + + std::vector current_joint_values; + current_state.copyJointGroupPositions(joint_model_group, current_joint_values); + + trajopt::TrajArray init_traj; + generateInitialTrajectory(pci, current_joint_values, init_traj); + + if (pci.basic_info.use_time == true) + { + prob->SetHasTime(true); + if (init_traj.rows() != n_steps || init_traj.cols() != n_dof + 1) + { + PRINT_AND_THROW(boost::format("Initial trajectory is not the right size matrix\n" + "Expected %i rows (time steps) x %i columns (%i dof + 1 time column)\n" + "Got %i rows and %i columns") % + n_steps % (n_dof + 1) % n_dof % init_traj.rows() % init_traj.cols()); + } + } + else + { + prob->SetHasTime(false); + if (init_traj.rows() != n_steps || init_traj.cols() != n_dof) + { + PRINT_AND_THROW(boost::format("Initial trajectory is not the right size matrix\n" + "Expected %i rows (time steps) x %i columns\n" + "Got %i rows and %i columns") % + n_steps % n_dof % init_traj.rows() % init_traj.cols()); + } + } + prob->SetInitTraj(init_traj); + + trajopt::VarArray matrix_traj_vars_temp; + // If start_fixed, constrain the joint values for the first time step to be their initialized values + if (bi.start_fixed) + { + if (init_traj.rows() < 1) + { + PRINT_AND_THROW("Initial trajectory must contain at least the start state."); + } + + if (init_traj.cols() != (n_dof + (use_time ? 1 : 0))) + { + PRINT_AND_THROW("robot dof values don't match initialization. I don't " + "know what you want me to use for the dof values"); + } + + for (int j = 0; j < static_cast(n_dof); ++j) + { + matrix_traj_vars_temp = prob->GetVars(); + prob->addLinearConstraint(sco::exprSub(sco::AffExpr(matrix_traj_vars_temp(0, j)), init_traj(0, j)), sco::EQ); + } + } + + // Apply constraint to each fixed dof to its initial value for all timesteps (freeze that column) + if (!bi.dofs_fixed.empty()) + { + for (const int& dof_ind : bi.dofs_fixed) + { + for (int i = 1; i < prob->GetNumSteps(); ++i) + { + matrix_traj_vars_temp = prob->GetVars(); + prob->addLinearConstraint( + sco::exprSub(sco::AffExpr(matrix_traj_vars_temp(i, dof_ind)), sco::AffExpr(init_traj(0, dof_ind))), + sco::EQ); + } + } + } + + for (const TermInfoPtr& ci : pci.cost_infos) + { + ci->addObjectiveTerms(*prob); + } + + for (const TermInfoPtr& ci : pci.cnt_infos) + { + ci->addObjectiveTerms(*prob); + } + return prob; +} + +CartPoseTermInfo::CartPoseTermInfo() : TermInfo(TT_COST | TT_CNT) +{ + pos_coeffs = Eigen::Vector3d::Ones(); + rot_coeffs = Eigen::Vector3d::Ones(); + tcp.setIdentity(); +} + +void CartPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob) +{ + unsigned int n_dof = prob.GetActiveGroupNumDOF(); + + Eigen::Isometry3d input_pose; + Eigen::Quaterniond q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + input_pose.linear() = q.matrix(); + input_pose.translation() = xyz; + + if (term_type == (TT_COST | TT_USE_TIME)) + { + ROS_ERROR("Use time version of this term has not been defined."); + } + else if (term_type == (TT_CNT | TT_USE_TIME)) + { + ROS_ERROR("Use time version of this term has not been defined."); + } + else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) + { + sco::VectorOfVectorPtr f(new CartPoseErrCalculator(input_pose, prob.GetPlanningScene(), link, tcp)); + prob.addCost(sco::CostPtr(new sco::CostFromErrFunc(f, prob.GetVarRow(timestep, 0, n_dof), + concatVector(rot_coeffs, pos_coeffs), sco::ABS, name))); + } + else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) + { + sco::VectorOfVectorPtr f(new CartPoseErrCalculator(input_pose, prob.GetPlanningScene(), link, tcp)); + prob.addConstraint(sco::ConstraintPtr(new sco::ConstraintFromErrFunc( + f, prob.GetVarRow(timestep, 0, n_dof), concatVector(rot_coeffs, pos_coeffs), sco::EQ, name))); + } + else + { + ROS_WARN("CartPoseTermInfo does not have a valid term_type defined. No cost/constraint applied"); + } +} + +void JointPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob) +{ + unsigned int n_dof = prob.GetActiveGroupNumDOF(); + + // If optional parameter not given, set to default + if (coeffs.empty()) + coeffs = trajopt::DblVec(n_dof, 1); + if (upper_tols.empty()) + upper_tols = trajopt::DblVec(n_dof, 0); + if (lower_tols.empty()) + lower_tols = trajopt::DblVec(n_dof, 0); + if (last_step <= -1) + last_step = prob.GetNumSteps() - 1; + + // Check time step is valid + if ((prob.GetNumSteps() - 1) <= first_step) + first_step = prob.GetNumSteps() - 1; + if ((prob.GetNumSteps() - 1) <= last_step) + last_step = prob.GetNumSteps() - 1; + // if (last_step == first_step) + // last_step += 1; + if (last_step < first_step) + { + int tmp = first_step; + first_step = last_step; + last_step = tmp; + ROS_WARN("Last time step for JointPosTerm comes before first step. Reversing them."); + } + if (last_step == -1) // last_step not set + last_step = first_step; + + // Check if parameters are the correct size. + checkParameterSize(coeffs, n_dof, "JointPoseTermInfo coeffs", true); + checkParameterSize(targets, n_dof, "JointPoseTermInfo targets", true); + checkParameterSize(upper_tols, n_dof, "JointPoseTermInfo upper_tols", true); + checkParameterSize(lower_tols, n_dof, "JointPoseTermInfo lower_tols", true); + + // Check if tolerances are all zeros + bool is_upper_zeros = + std::all_of(upper_tols.begin(), upper_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); + bool is_lower_zeros = + std::all_of(lower_tols.begin(), lower_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); + + // Get vars associated with joints + trajopt::VarArray vars = prob.GetVars(); + trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); + if (prob.GetHasTime()) + ROS_INFO("JointPoseTermInfo does not differ based on setting of TT_USE_TIME"); + + if (term_type & TT_COST) + { + // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost + if (is_upper_zeros && is_lower_zeros) + { + prob.addCost(sco::CostPtr(new trajopt::JointPosEqCost(joint_vars, util::toVectorXd(coeffs), + util::toVectorXd(targets), first_step, last_step))); + prob.getCosts().back()->setName(name); + } + else + { + prob.addCost(sco::CostPtr(new trajopt::JointPosIneqCost(joint_vars, util::toVectorXd(coeffs), + util::toVectorXd(targets), util::toVectorXd(upper_tols), + util::toVectorXd(lower_tols), first_step, last_step))); + prob.getCosts().back()->setName(name); + } + } + else if (term_type & TT_CNT) + { + // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint + if (is_upper_zeros && is_lower_zeros) + { + prob.addConstraint(sco::ConstraintPtr(new trajopt::JointPosEqConstraint( + joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step))); + prob.getConstraints().back()->setName(name); + } + else + { + prob.addConstraint(sco::ConstraintPtr(new trajopt::JointPosIneqConstraint( + joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), util::toVectorXd(upper_tols), + util::toVectorXd(lower_tols), first_step, last_step))); + prob.getConstraints().back()->setName(name); + } + } + else + { + ROS_WARN("JointPosTermInfo does not have a valid term_type defined. No cost/constraint applied"); + } +} + +void JointVelTermInfo::addObjectiveTerms(TrajOptProblem& prob) +{ + unsigned int n_dof = prob.GetNumDOF(); + + // If optional parameter not given, set to default + if (coeffs.empty()) + coeffs = trajopt::DblVec(n_dof, 1); + if (upper_tols.empty()) + upper_tols = trajopt::DblVec(n_dof, 0); + if (lower_tols.empty()) + lower_tols = trajopt::DblVec(n_dof, 0); + if (last_step <= -1) + last_step = prob.GetNumSteps() - 1; + + // If only one time step is desired, calculate velocity with next step (2 steps are needed for 1 velocity calculation) + if ((prob.GetNumSteps() - 2) <= first_step) + first_step = prob.GetNumSteps() - 2; + if ((prob.GetNumSteps() - 1) <= last_step) + last_step = prob.GetNumSteps() - 1; + if (last_step == first_step) + last_step += 1; + if (last_step < first_step) + { + int tmp = first_step; + first_step = last_step; + last_step = tmp; + ROS_WARN("Last time step for JointVelTerm comes before first step. Reversing them."); + } + + // Check if parameters are the correct size. + checkParameterSize(coeffs, n_dof, "JointVelTermInfo coeffs", true); + checkParameterSize(targets, n_dof, "JointVelTermInfo targets", true); + checkParameterSize(upper_tols, n_dof, "JointVelTermInfo upper_tols", true); + checkParameterSize(lower_tols, n_dof, "JointVelTermInfo lower_tols", true); + assert(last_step > first_step); + assert(first_step >= 0); + + // Check if tolerances are all zeros + bool is_upper_zeros = + std::all_of(upper_tols.begin(), upper_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); + bool is_lower_zeros = + std::all_of(lower_tols.begin(), lower_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); + + // Get vars associated with joints + trajopt::VarArray vars = prob.GetVars(); + trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); + + if (term_type == (TT_COST | TT_USE_TIME)) + { + unsigned num_vels = last_step - first_step; + + // Apply seperate cost to each joint b/c that is how the error function is currently written + for (size_t j = 0; j < n_dof; j++) + { + // Get a vector of a single column of vars + sco::VarVector joint_vars_vec = joint_vars.cblock(first_step, j, last_step - first_step + 1); + sco::VarVector time_vars_vec = vars.cblock(first_step, vars.cols() - 1, last_step - first_step + 1); + + // If the tolerances are 0, an equality cost is set + if (is_upper_zeros && is_lower_zeros) + { + trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); + prob.addCost(sco::CostPtr(new sco::CostFromErrFunc( + sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])), + sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec), + util::toVectorXd(single_jnt_coeffs), sco::SQUARED, name + "_j" + std::to_string(j)))); + } + // Otherwise it's a hinged "inequality" cost + else + { + trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); + prob.addCost(sco::CostPtr(new sco::CostFromErrFunc( + sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])), + sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec), + util::toVectorXd(single_jnt_coeffs), sco::HINGE, name + "_j" + std::to_string(j)))); + } + } + } + else if (term_type == (TT_CNT | TT_USE_TIME)) + { + unsigned num_vels = last_step - first_step; + + // Apply seperate cnt to each joint b/c that is how the error function is currently written + for (size_t j = 0; j < n_dof; j++) + { + // Get a vector of a single column of vars + sco::VarVector joint_vars_vec = joint_vars.cblock(first_step, j, last_step - first_step + 1); + sco::VarVector time_vars_vec = vars.cblock(first_step, vars.cols() - 1, last_step - first_step + 1); + + // If the tolerances are 0, an equality cnt is set + if (is_upper_zeros && is_lower_zeros) + { + trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); + prob.addConstraint(sco::ConstraintPtr(new sco::ConstraintFromErrFunc( + sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])), + sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec), + util::toVectorXd(single_jnt_coeffs), sco::EQ, name + "_j" + std::to_string(j)))); + } + // Otherwise it's a hinged "inequality" constraint + else + { + trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); + prob.addConstraint(sco::ConstraintPtr(new sco::ConstraintFromErrFunc( + sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])), + sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec), + util::toVectorXd(single_jnt_coeffs), sco::INEQ, name + "_j" + std::to_string(j)))); + } + } + } + else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) + { + // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost + if (is_upper_zeros && is_lower_zeros) + { + prob.addCost(sco::CostPtr(new trajopt::JointVelEqCost(joint_vars, util::toVectorXd(coeffs), + util::toVectorXd(targets), first_step, last_step))); + prob.getCosts().back()->setName(name); + } + else + { + prob.addCost(sco::CostPtr(new trajopt::JointVelIneqCost(joint_vars, util::toVectorXd(coeffs), + util::toVectorXd(targets), util::toVectorXd(upper_tols), + util::toVectorXd(lower_tols), first_step, last_step))); + prob.getCosts().back()->setName(name); + } + } + else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) + { + // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint + if (is_upper_zeros && is_lower_zeros) + { + prob.addConstraint(sco::ConstraintPtr(new trajopt::JointVelEqConstraint( + joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step))); + prob.getConstraints().back()->setName(name); + } + else + { + prob.addConstraint(sco::ConstraintPtr(new trajopt::JointVelIneqConstraint( + joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), util::toVectorXd(upper_tols), + util::toVectorXd(lower_tols), first_step, last_step))); + prob.getConstraints().back()->setName(name); + } + } + else + { + ROS_WARN("JointVelTermInfo does not have a valid term_type defined. No cost/constraint applied"); + } +} + +void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, + trajopt::TrajArray& init_traj) +{ + Eigen::VectorXd current_pos(current_joint_values.size()); + for (int joint_index = 0; joint_index < current_joint_values.size(); ++joint_index) + { + current_pos(joint_index) = current_joint_values[joint_index]; + } + + InitInfo init_info = pci.init_info; + + // initialize based on type specified + if (init_info.type == InitInfo::STATIONARY) + { + // Initializes all joint values to the initial value (the current value in scene) + init_traj = current_pos.transpose().replicate(pci.basic_info.n_steps, 1); + } + else if (init_info.type == InitInfo::JOINT_INTERPOLATED) + { + // Linearly interpolates between initial value (current values in the scene) and the joint position specified in + // InitInfo.data + Eigen::VectorXd end_pos = init_info.data; + init_traj.resize(pci.basic_info.n_steps, end_pos.rows()); + for (int dof_index = 0; dof_index < current_pos.rows(); ++dof_index) + { + init_traj.col(dof_index) = + Eigen::VectorXd::LinSpaced(pci.basic_info.n_steps, current_pos(dof_index), end_pos(dof_index)); + } + } + else if (init_info.type == InitInfo::GIVEN_TRAJ) + { + // Initializes the matrix to a given trajectory + init_traj = init_info.data; + } + else + { + PRINT_AND_THROW("Init Info did not have a valid type. Valid types are " + "STATIONARY, JOINT_INTERPOLATED, or GIVEN_TRAJ"); + } + + // Currently all trajectories are generated without time then appended here + if (pci.basic_info.use_time) + { + // add on time (default to 1 sec) + init_traj.conservativeResize(Eigen::NoChange_t(), init_traj.cols() + 1); + + init_traj.block(0, init_traj.cols() - 1, init_traj.rows(), 1) = + Eigen::VectorXd::Constant(init_traj.rows(), init_info.dt); + } +} +} diff --git a/moveit_planners/trajopt/src/trajopt_interface.cpp b/moveit_planners/trajopt/src/trajopt_interface.cpp new file mode 100644 index 0000000000..4f76c1add2 --- /dev/null +++ b/moveit_planners/trajopt/src/trajopt_interface.cpp @@ -0,0 +1,463 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik, Inc. + * 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 PickNik 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. + *********************************************************************/ + +/* Author: Omid Heidari */ + +#include +#include + +#include + +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include "trajopt_interface/trajopt_interface.h" +#include "trajopt_interface/problem_description.h" + +namespace trajopt_interface +{ +TrajOptInterface::TrajOptInterface(const ros::NodeHandle& nh) : nh_(nh), name_("TrajOptInterface") +{ + trajopt_problem_ = TrajOptProblemPtr(new TrajOptProblem); + setDefaultTrajOPtParams(); + + // TODO: callbacks should be defined by the user + optimizer_callbacks_.push_back(callBackFunc); +} + +bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::MotionPlanDetailedResponse& res) +{ + ROS_INFO(" ======================================= From trajopt_interface, solve is called"); + setTrajOptParams(params_); + + if (!planning_scene) + { + ROS_ERROR_STREAM_NAMED(name_, "No planning scene initialized."); + res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; + return false; + } + + ROS_INFO(" ======================================= Extract current state information"); + ros::WallTime start_time = ros::WallTime::now(); + robot_model::RobotModelConstPtr robot_model = planning_scene->getRobotModel(); + bool robot_model_ok = static_cast(robot_model); + if (!robot_model_ok) + ROS_ERROR_STREAM_NAMED(name_, "robot model is not loaded properly"); + robot_state::RobotStatePtr current_state(new robot_state::RobotState(robot_model)); + *current_state = planning_scene->getCurrentState(); + const robot_state::JointModelGroup* joint_model_group = current_state->getJointModelGroup(req.group_name); + if (joint_model_group == nullptr) + ROS_ERROR_STREAM_NAMED(name_, "joint model group is empty"); + std::vector group_joint_names = joint_model_group->getActiveJointModelNames(); + int dof = group_joint_names.size(); + std::vector current_joint_values; + current_state->copyJointGroupPositions(joint_model_group, current_joint_values); + + // Current state is different from star state in general + ROS_INFO(" ======================================= Extract start state infromation"); + trajopt::DblVec start_joint_values = extractStartJointValues(req, group_joint_names); + + // check the start state for being empty or joint limit violiation + if (start_joint_values.empty()) + { + ROS_ERROR_STREAM_NAMED(name_, "Start_state is empty"); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; + return false; + } + + if (not joint_model_group->satisfiesPositionBounds(start_joint_values.data())) + { + ROS_ERROR_STREAM_NAMED(name_, "Start state violates joint limits"); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; + return false; + } + + ROS_INFO(" ======================================= Create ProblemInfo"); + ProblemInfo problem_info(planning_scene, req.group_name); + + setProblemInfoParam(problem_info); + + ROS_INFO(" ======================================= Populate init info, hard-coded"); + // TODO: init info should be defined by user. To this end, we need to add seed trajectories to MotionPlanRequest. + // JOINT_INTERPOLATED: data is the current joint values + // GIVEN_TRAJ: data is the joint values of the current state copied to all timesteps + Eigen::VectorXd current_joint_values_eigen(dof); + for (int joint_index = 0; joint_index < dof; ++joint_index) + { + current_joint_values_eigen(joint_index) = current_joint_values[joint_index]; + } + + if (problem_info.init_info.type == InitInfo::JOINT_INTERPOLATED) + { + problem_info.init_info.data = current_joint_values_eigen; + } + else if (problem_info.init_info.type == InitInfo::GIVEN_TRAJ) + { + problem_info.init_info.data = current_joint_values_eigen.transpose().replicate(problem_info.basic_info.n_steps, 1); + } + + ROS_INFO(" ======================================= Create Constraints"); + if (req.goal_constraints.empty()) + { + ROS_ERROR_STREAM_NAMED("trajopt_planner", "No goal constraints specified!"); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; + return false; + } + + ROS_INFO(" ======================================= Cartesian Constraints"); + if (!req.goal_constraints[0].position_constraints.empty() && !req.goal_constraints[0].orientation_constraints.empty()) + { + CartPoseTermInfoPtr cart_goal_pos(new CartPoseTermInfo); + + // TODO: Feed cart_goal_pos with request information and the needed param to the setup.yaml file + // TODO: Multiple Cartesian constraints + + // Add the constraint to problem_info + problem_info.cnt_infos.push_back(cart_goal_pos); + } + else if (req.goal_constraints[0].position_constraints.empty() && + !req.goal_constraints[0].orientation_constraints.empty()) + { + ROS_ERROR_STREAM_NAMED("trajopt_planner", "position constraint is not defined"); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; + return false; + } + else if (!req.goal_constraints[0].orientation_constraints.empty() && + req.goal_constraints[0].orientation_constraints.empty()) + { + ROS_ERROR_STREAM_NAMED("trajopt_planner", "orientation constraint is not defined"); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; + return false; + } + + ROS_INFO(" ======================================= Constraints from request goal_constraints"); + for (auto goal_cnt : req.goal_constraints) + { + JointPoseTermInfoPtr joint_pos_term(new JointPoseTermInfo); + // When using MotionPlanning Display in RViz, the created request has no name for the constriant + setJointPoseTermInfoParams(joint_pos_term, (goal_cnt.name != "") ? goal_cnt.name : "goal_tmp"); + + trajopt::DblVec joint_goal_constraints; + for (const moveit_msgs::JointConstraint& joint_goal_constraint : goal_cnt.joint_constraints) + { + joint_goal_constraints.push_back(joint_goal_constraint.position); + } + joint_pos_term->targets = joint_goal_constraints; + problem_info.cnt_infos.push_back(joint_pos_term); + } + + ROS_INFO(" ======================================= Constraints from request start_state"); + // add the start pos from request as a constraint + JointPoseTermInfoPtr joint_start_pos(new JointPoseTermInfo); + + joint_start_pos->targets = start_joint_values; + setJointPoseTermInfoParams(joint_start_pos, "start_pos"); + problem_info.cnt_infos.push_back(joint_start_pos); + + ROS_INFO(" ======================================= Velocity Constraints, hard-coded"); + // TODO: should be defined by user, its parametes should be added to setup.yaml + JointVelTermInfoPtr joint_vel(new JointVelTermInfo); + + joint_vel->coeffs = std::vector(dof, 5.0); + joint_vel->targets = std::vector(dof, 0.0); + joint_vel->first_step = 0; + joint_vel->last_step = problem_info.basic_info.n_steps - 1; + joint_vel->name = "joint_vel"; + joint_vel->term_type = trajopt_interface::TT_COST; + problem_info.cost_infos.push_back(joint_vel); + + ROS_INFO(" ======================================= Visibility Constraints"); + if (!req.goal_constraints[0].visibility_constraints.empty()) + { + // TODO: Add visibility constraint + } + + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.improve_ratio_threshold: " << params_.improve_ratio_threshold); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.min_trust_box_size: " << params_.min_trust_box_size); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.min_approx_improve: " << params_.min_approx_improve); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.params_.min_approx_improve_frac: " << params_.min_approx_improve_frac); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_iter: " << params_.max_iter); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_shrink_ratio: " << params_.trust_shrink_ratio); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_expand_ratio: " << params_.trust_expand_ratio); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.cnt_tolerance: " << params_.cnt_tolerance); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_merit_coeff_increases: " << params_.max_merit_coeff_increases); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.merit_coeff_increase_ratio: " << params_.merit_coeff_increase_ratio); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_time: " << params_.max_time); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.merit_error_coeff: " << params_.merit_error_coeff); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_box_size: " << params_.trust_box_size); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.n_steps: " << problem_info.basic_info.n_steps); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt_upper_lim: " << problem_info.basic_info.dt_upper_lim); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt_lower_lim: " << problem_info.basic_info.dt_lower_lim); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.start_fixed: " << problem_info.basic_info.start_fixed); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.use_time: " << problem_info.basic_info.use_time); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.convex_solver: " << problem_info.basic_info.convex_solver); + + std::string problem_info_type; + switch (problem_info.init_info.type) + { + case InitInfo::STATIONARY: + problem_info_type = "STATIONARY"; + break; + case InitInfo::JOINT_INTERPOLATED: + problem_info_type = "JOINT_INTERPOLATED"; + break; + case InitInfo::GIVEN_TRAJ: + problem_info_type = "GIVEN_TRAJ"; + break; + } + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.type: " << problem_info_type); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt: " << problem_info.init_info.dt); + + ROS_INFO(" ======================================= Construct problem"); + trajopt_problem_ = ConstructProblem(problem_info); + + ROS_INFO_STREAM_NAMED("num_cost", trajopt_problem_->getNumCosts()); + ROS_INFO_STREAM_NAMED("num_constraints", trajopt_problem_->getNumConstraints()); + + ROS_INFO(" ======================================= TrajOpt Optimization"); + sco::BasicTrustRegionSQP opt(trajopt_problem_); + + opt.setParameters(params_); + opt.initialize(trajopt::trajToDblVec(trajopt_problem_->GetInitTraj())); + + // Add all callbacks + for (const sco::Optimizer::Callback& callback : optimizer_callbacks_) + { + opt.addCallback(callback); + } + + // Optimize + ros::WallTime create_time = ros::WallTime::now(); + opt.optimize(); + + ROS_INFO(" ======================================= TrajOpt Solution"); + trajopt::TrajArray opt_solution = trajopt::getTraj(opt.x(), trajopt_problem_->GetVars()); + + // assume that the trajectory is now optimized, fill in the output structure: + ROS_INFO_STREAM_NAMED("num_rows", opt_solution.rows()); + ROS_INFO_STREAM_NAMED("num_cols", opt_solution.cols()); + + res.trajectory.resize(1); + res.trajectory[0].joint_trajectory.joint_names = group_joint_names; + res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header; + + // fill in the entire trajectory + res.trajectory[0].joint_trajectory.points.resize(opt_solution.rows()); + for (int i = 0; i < opt_solution.rows(); i++) + { + res.trajectory[0].joint_trajectory.points[i].positions.resize(opt_solution.cols()); + for (size_t j = 0; j < opt_solution.cols(); j++) + { + res.trajectory[0].joint_trajectory.points[i].positions[j] = opt_solution(i, j); + } + // Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints. + res.trajectory[0].joint_trajectory.points[i].time_from_start = ros::Duration(0.0); + } + + res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + res.processing_time.push_back((ros::WallTime::now() - start_time).toSec()); + + ROS_INFO(" ======================================= check if final state is within goal tolerances"); + kinematic_constraints::JointConstraint joint_cnt(planning_scene->getRobotModel()); + robot_state::RobotState last_state(*current_state); + last_state.setJointGroupPositions(req.group_name, res.trajectory[0].joint_trajectory.points.back().positions); + + for (int jn = 0; jn < res.trajectory[0].joint_trajectory.points.back().positions.size(); ++jn) + { + ROS_INFO_STREAM_NAMED("joint_value", res.trajectory[0].joint_trajectory.points.back().positions[jn] + << " " << req.goal_constraints.back().joint_constraints[jn].position); + } + + bool constraints_are_ok = true; + int joint_cnt_index = 0; + for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints.back().joint_constraints) + { + ROS_INFO("index %i: jc.configure(constraint)=> %d, jc.decide(last_state).satisfied=> %d, tolerance %f", + joint_cnt_index, joint_cnt.configure(constraint), joint_cnt.decide(last_state).satisfied, + constraint.tolerance_above); + constraints_are_ok = constraints_are_ok and joint_cnt.configure(constraint); + constraints_are_ok = constraints_are_ok and joint_cnt.decide(last_state).satisfied; + if (not constraints_are_ok) + { + ROS_ERROR_STREAM_NAMED("trajopt_planner", "Goal constraints are violated: " << constraint.joint_name); + res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED; + return false; + } + joint_cnt_index = joint_cnt_index + 1; + } + + ROS_INFO(" ==================================== Response"); + res.trajectory_start = req.start_state; + + ROS_INFO(" ==================================== Debug Response"); + ROS_INFO_STREAM_NAMED("group_name", res.group_name); + ROS_INFO_STREAM_NAMED("start_traj_name_size", res.trajectory_start.joint_state.name.size()); + ROS_INFO_STREAM_NAMED("start_traj_position_size", res.trajectory_start.joint_state.position.size()); + ROS_INFO_STREAM_NAMED("traj_name_size", res.trajectory[0].joint_trajectory.joint_names.size()); + ROS_INFO_STREAM_NAMED("traj_point_size", res.trajectory[0].joint_trajectory.points.size()); + return true; +} + +void TrajOptInterface::setDefaultTrajOPtParams() +{ + sco::BasicTrustRegionSQPParameters params; + params_ = params; +} + +void TrajOptInterface::setTrajOptParams(sco::BasicTrustRegionSQPParameters& params) +{ + nh_.param("trajopt_param/improve_ratio_threshold", params.improve_ratio_threshold, 0.25); + nh_.param("trajopt_param/min_trust_box_size", params.min_trust_box_size, 1e-4); + nh_.param("trajopt_param/min_approx_improve", params.min_approx_improve, 1e-4); + nh_.param("trajopt_param/min_approx_improve_frac", params.min_approx_improve_frac, -static_cast(INFINITY)); + nh_.param("trajopt_param/max_iter", params.max_iter, 100.0); + nh_.param("trajopt_param/trust_shrink_ratio", params.trust_shrink_ratio, 0.1); + + nh_.param("trajopt_param/trust_expand_ratio", params.trust_expand_ratio, 1.5); + nh_.param("trajopt_param/cnt_tolerance", params.cnt_tolerance, 1e-4); + nh_.param("trajopt_param/max_merit_coeff_increases", params.max_merit_coeff_increases, 5.0); + nh_.param("trajopt_param/merit_coeff_increase_ratio", params.merit_coeff_increase_ratio, 10.0); + nh_.param("trajopt_param/max_time", params.max_time, static_cast(INFINITY)); + nh_.param("trajopt_param/merit_error_coeff", params.merit_error_coeff, 10.0); + nh_.param("trajopt_param/trust_box_size", params.trust_box_size, 1e-1); +} + +void TrajOptInterface::setProblemInfoParam(ProblemInfo& problem_info) +{ + nh_.param("problem_info/basic_info/n_steps", problem_info.basic_info.n_steps, 20); + nh_.param("problem_info/basic_info/dt_upper_lim", problem_info.basic_info.dt_upper_lim, 2.0); + nh_.param("problem_info/basic_info/dt_lower_lim", problem_info.basic_info.dt_lower_lim, 100.0); + nh_.param("problem_info/basic_info/start_fixed", problem_info.basic_info.start_fixed, true); + nh_.param("problem_info/basic_info/use_time", problem_info.basic_info.use_time, false); + int convex_solver_index; + nh_.param("problem_info/basic_info/convex_solver", convex_solver_index, 1); + switch (convex_solver_index) + { + case 1: + problem_info.basic_info.convex_solver = sco::ModelType::AUTO_SOLVER; + break; + case 2: + problem_info.basic_info.convex_solver = sco::ModelType::BPMPD; + break; + case 3: + problem_info.basic_info.convex_solver = sco::ModelType::OSQP; + break; + case 4: + problem_info.basic_info.convex_solver = sco::ModelType::QPOASES; + break; + case 5: + problem_info.basic_info.convex_solver = sco::ModelType::GUROBI; + break; + } + + nh_.param("problem_info/init_info/dt", problem_info.init_info.dt, 0.5); + int type_index; + nh_.param("problem_info/init_info/type", type_index, 1); + switch (type_index) + { + case 1: + problem_info.init_info.type = InitInfo::STATIONARY; + break; + case 2: + problem_info.init_info.type = InitInfo::JOINT_INTERPOLATED; + break; + case 3: + problem_info.init_info.type = InitInfo::GIVEN_TRAJ; + break; + } +} + +void TrajOptInterface::setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name) +{ + int term_type_index; + std::string term_type_address = "joint_pos_term_info/" + name + "/term_type"; + nh_.param(term_type_address, term_type_index, 1); + + switch (term_type_index) + { + case 1: + jp->term_type = TT_COST; + break; + case 2: + jp->term_type = TT_CNT; + break; + case 3: + jp->term_type = TT_USE_TIME; + break; + } + + nh_.getParam("joint_pos_term_info/" + name + "/first_timestep", jp->first_step); + nh_.getParam("joint_pos_term_info/" + name + "/last_timestep", jp->last_step); + nh_.getParam("joint_pos_term_info/" + name + "/name", jp->name); +} + +trajopt::DblVec TrajOptInterface::extractStartJointValues(const planning_interface::MotionPlanRequest& req, + const std::vector& group_joint_names) +{ + std::unordered_map all_joints; + trajopt::DblVec start_joint_vals; + + for (int joint_index = 0; joint_index < req.start_state.joint_state.position.size(); ++joint_index) + { + all_joints[req.start_state.joint_state.name[joint_index]] = req.start_state.joint_state.position[joint_index]; + } + + for (auto joint_name : group_joint_names) + { + ROS_INFO(" joint position from start state, name: %s, value: %f", joint_name.c_str(), all_joints[joint_name]); + start_joint_vals.push_back(all_joints[joint_name]); + } + + return start_joint_vals; +} + +void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res) +{ + // TODO: Create the actuall implementation +} + +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/src/trajopt_planner_manager.cpp b/moveit_planners/trajopt/src/trajopt_planner_manager.cpp new file mode 100644 index 0000000000..760c58f823 --- /dev/null +++ b/moveit_planners/trajopt/src/trajopt_planner_manager.cpp @@ -0,0 +1,141 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * 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 PickNik LLC 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. + *********************************************************************/ + +/* Author: Omid Heidari + Desc: TrajOpt planning plugin +*/ + +#include + +#include "moveit/collision_detection_fcl/collision_detector_allocator_fcl.h" + +#include + +#include "trajopt_interface/trajopt_planning_context.h" + +namespace trajopt_interface +{ +class TrajOptPlannerManager : public planning_interface::PlannerManager +{ +public: + TrajOptPlannerManager() : planning_interface::PlannerManager() + { + } + + bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns) override + { + ROS_INFO(" ======================================= initialize gets called"); + + if (!ns.empty()) + nh_ = ros::NodeHandle(ns); + std::string trajopt_ns = ns.empty() ? "trajopt" : ns + "/trajopt"; + + for (const std::string& gpName : model->getJointModelGroupNames()) + { + ROS_INFO(" ======================================= group name: %s, robot model: %s", gpName.c_str(), + model->getName().c_str()); + planning_contexts_[gpName] = + TrajOptPlanningContextPtr(new TrajOptPlanningContext("trajopt_planning_context", gpName, model)); + } + + return true; + } + + bool canServiceRequest(const moveit_msgs::MotionPlanRequest& req) const override + { + return req.trajectory_constraints.constraints.empty(); + } + + std::string getDescription() const override + { + return "TrajOpt"; + } + + void getPlanningAlgorithms(std::vector& algs) const override + { + algs.clear(); + algs.push_back("trajopt"); + } + + planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::MoveItErrorCodes& error_code) const override + { + ROS_INFO(" ======================================= getPlanningContext() is called "); + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + + if (req.group_name.empty()) + { + ROS_ERROR("No group specified to plan for"); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; + return planning_interface::PlanningContextPtr(); + } + + if (!planning_scene) + { + ROS_ERROR("No planning scene supplied as input"); + error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; + return planning_interface::PlanningContextPtr(); + } + + // create PlanningScene using hybrid collision detector + planning_scene::PlanningScenePtr ps = planning_scene->diff(); + + // set FCL for the collision + ps->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create(), true); + + // retrieve and configure existing context + const TrajOptPlanningContextPtr& context = planning_contexts_.at(req.group_name); + + ROS_INFO(" ======================================= context is made "); + + context->setPlanningScene(ps); + context->setMotionPlanRequest(req); + + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + + return context; + } + +private: + ros::NodeHandle nh_; + +protected: + std::map planning_contexts_; +}; + +} // namespace trajopt_interface + +// register the TrajOptPlannerManager class as a plugin +CLASS_LOADER_REGISTER_CLASS(trajopt_interface::TrajOptPlannerManager, planning_interface::PlannerManager); diff --git a/moveit_planners/trajopt/src/trajopt_planning_context.cpp b/moveit_planners/trajopt/src/trajopt_planning_context.cpp new file mode 100644 index 0000000000..34b1e4c307 --- /dev/null +++ b/moveit_planners/trajopt/src/trajopt_planning_context.cpp @@ -0,0 +1,76 @@ +#include +#include +#include +#include + +#include + +#include + +#include "trajopt_interface/trajopt_planning_context.h" +#include "trajopt_interface/trajopt_interface.h" + +namespace trajopt_interface +{ +TrajOptPlanningContext::TrajOptPlanningContext(const std::string& context_name, const std::string& group_name, + const robot_model::RobotModelConstPtr& model) + : planning_interface::PlanningContext(context_name, group_name), robot_model_(model) +{ + ROS_INFO(" ======================================= TrajOptPlanningContext is constructed"); + trajopt_interface_ = TrajOptInterfacePtr(new TrajOptInterface()); +} + +bool TrajOptPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) +{ + moveit_msgs::MotionPlanDetailedResponse res_msg; + bool trajopt_solved = trajopt_interface_->solve(planning_scene_, request_, res_msg); + + if (trajopt_solved) + { + res.trajectory_.resize(1); + res.trajectory_[0] = + robot_trajectory::RobotTrajectoryPtr(new robot_trajectory::RobotTrajectory(robot_model_, getGroupName())); + + moveit::core::RobotState start_state(robot_model_); + robot_state::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); + res.trajectory_[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]); + + res.description_.push_back("plan"); + // TODO: Add the initial trajectory to res (MotionPlanDetailedResponse) + res.processing_time_ = res_msg.processing_time; + res.error_code_ = res_msg.error_code; + return true; + } + else + { + res.error_code_ = res_msg.error_code; + return false; + } +} + +bool TrajOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) +{ + planning_interface::MotionPlanDetailedResponse res_detailed; + bool planning_success = solve(res_detailed); + + res.error_code_ = res_detailed.error_code_; + + if (planning_success) + { + res.trajectory_ = res_detailed.trajectory_[0]; + res.planning_time_ = res_detailed.processing_time_[0]; + } + + return planning_success; +} + +bool TrajOptPlanningContext::terminate() +{ + ROS_ERROR_STREAM_NAMED("trajopt_planning_context", "TrajOpt is not interruptable yet"); + return false; +} +void TrajOptPlanningContext::clear() +{ +} + +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/test/trajectory_test.cpp b/moveit_planners/trajopt/test/trajectory_test.cpp new file mode 100644 index 0000000000..3494ba2d9e --- /dev/null +++ b/moveit_planners/trajopt/test/trajectory_test.cpp @@ -0,0 +1,212 @@ +/********************************************************************* + * Software License Agreement + * + * Copyright (c) 2019, PickNik Consulting + * All rights reserved. + * + *********************************************************************/ + +/* Author: Omid Heidari + Desc: Test the trajectory planned by trajopt + */ + +// C++ +#include +#include +#include + +// ROS +#include + +// Testing +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +class TrajectoryTest : public ::testing::Test +{ +public: + TrajectoryTest() + { + } + +protected: + void SetUp() override + { + node_handle_ = ros::NodeHandle("~"); + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + bool robot_model_ok_ = static_cast(robot_model_); + if (!robot_model_ok_) + ROS_ERROR_STREAM_NAMED("trajectory_test", "robot model is not loaded correctly"); + } + +protected: + robot_model::RobotModelPtr robot_model_; + std::vector group_joint_names_; + const std::string PLANNING_GROUP = "panda_arm"; + const double GOAL_TOLERANCE = 0.1; + ros::NodeHandle node_handle_; +}; // class TrajectoryTest + +TEST_F(TrajectoryTest, concatVectorValidation) +{ + std::vector vec_a = { 1, 2, 3, 4, 5 }; + std::vector vec_b = { 6, 7, 8, 9, 10 }; + std::vector vec_c = trajopt_interface::concatVector(vec_a, vec_b); + EXPECT_EQ(vec_c.size(), vec_a.size() + vec_b.size()); + + // Check if the output of concatVector is correct. + // Loop over the output and the input vectors to see if they match + std::size_t length_ab = vec_a.size() + vec_b.size(); + for (std::size_t index = 0; index < length_ab; ++index) + { + if (index < vec_a.size()) + { + EXPECT_EQ(vec_c[index], vec_a[index]); + } + else + { + EXPECT_EQ(vec_c[index], vec_b[index - vec_a.size()]); + } + } +} + +TEST_F(TrajectoryTest, goalTolerance) +{ + const std::string NODE_NAME = "trajectory_test"; + + // Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group + robot_state::RobotStatePtr current_state(new robot_state::RobotState(robot_model_)); + current_state->setToDefaultValues(); + + const robot_state::JointModelGroup* joint_model_group = current_state->getJointModelGroup(PLANNING_GROUP); + EXPECT_NE(joint_model_group, nullptr); + const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); + EXPECT_EQ(joint_names.size(), 7); + + planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model_)); + + // Create response and request + planning_interface::MotionPlanRequest req; + planning_interface::MotionPlanResponse res; + + // Set start state + // ====================================================================================== + std::vector start_joint_values = { 0.4, 0.3, 0.5, -0.55, 0.88, 1.0, -0.075 }; + robot_state::RobotStatePtr start_state(new robot_state::RobotState(robot_model_)); + start_state->setJointGroupPositions(joint_model_group, start_joint_values); + start_state->update(); + + req.start_state.joint_state.name = joint_names; + req.start_state.joint_state.position = start_joint_values; + req.goal_constraints.clear(); + req.group_name = PLANNING_GROUP; + + // Set the goal state and joints tolerance + // ======================================================================================== + robot_state::RobotStatePtr goal_state(new robot_state::RobotState(robot_model_)); + std::vector goal_joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1 }; + goal_state->setJointGroupPositions(joint_model_group, goal_joint_values); + goal_state->update(); + moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(*goal_state, joint_model_group); + req.goal_constraints.push_back(joint_goal); + req.goal_constraints[0].name = "goal_pos"; + // Set joint tolerance + std::vector goal_joint_constraint = req.goal_constraints[0].joint_constraints; + for (std::size_t x = 0; x < goal_joint_constraint.size(); ++x) + { + req.goal_constraints[0].joint_constraints[x].tolerance_above = 0.001; + req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.001; + } + + // Load planner + // ====================================================================================== + // We will now construct a loader to load a planner, by name. + // Note that we are using the ROS pluginlib library here. + boost::scoped_ptr> planner_plugin_loader; + planning_interface::PlannerManagerPtr planner_instance; + + std::string planner_plugin_name = "trajopt_interface/TrajOptPlanner"; + node_handle_.setParam("planning_plugin", planner_plugin_name); + + // Make sure the planner plugin is loaded + EXPECT_TRUE(node_handle_.getParam("planning_plugin", planner_plugin_name)); + try + { + planner_plugin_loader.reset(new pluginlib::ClassLoader( + "moveit_core", "planning_interface::PlannerManager")); + } + catch (pluginlib::PluginlibException& ex) + { + ROS_FATAL_STREAM_NAMED(NODE_NAME, "Exception while creating planning plugin loader " << ex.what()); + } + try + { + planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); + if (!planner_instance->initialize(robot_model_, node_handle_.getNamespace())) + ROS_FATAL_STREAM_NAMED(NODE_NAME, "Could not initialize planner instance"); + ROS_INFO_STREAM_NAMED(NODE_NAME, "Using planning interface '" << planner_instance->getDescription() << "'"); + } + catch (pluginlib::PluginlibException& ex) + { + const std::vector& classes = planner_plugin_loader->getDeclaredClasses(); + std::stringstream ss; + for (std::size_t i = 0; i < classes.size(); ++i) + ss << classes[i] << " "; + ROS_ERROR_STREAM_NAMED(NODE_NAME, "Exception while loading planner '" << planner_plugin_name << "': " << ex.what() + << std::endl + << "Available plugins: " << ss.str()); + } + + // Creat planning context + // ======================================================================================== + planning_interface::PlanningContextPtr context = + planner_instance->getPlanningContext(planning_scene, req, res.error_code_); + + context->solve(res); + EXPECT_EQ(res.error_code_.val, res.error_code_.SUCCESS); + + moveit_msgs::MotionPlanResponse response; + res.getMessage(response); + + // Check the difference between the last step in the solution and the goal + // ======================================================================================== + std::vector joints_values_last_step = response.trajectory.joint_trajectory.points.back().positions; + + for (std::size_t joint_index = 0; joint_index < joints_values_last_step.size(); ++joint_index) + { + double goal_error = + abs(joints_values_last_step[joint_index] - req.goal_constraints[0].joint_constraints[joint_index].position); + std::cerr << "goal_error: " << goal_error << std::endl; + EXPECT_LT(goal_error, GOAL_TOLERANCE); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "trajectory_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + int result = RUN_ALL_TESTS(); + ros::shutdown(); + + return result; +} diff --git a/moveit_planners/trajopt/test/trajectory_test.test b/moveit_planners/trajopt/test/trajectory_test.test new file mode 100644 index 0000000000..36314b49b5 --- /dev/null +++ b/moveit_planners/trajopt/test/trajectory_test.test @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/moveit_planners/trajopt/trajopt_interface_plugin_description.xml b/moveit_planners/trajopt/trajopt_interface_plugin_description.xml new file mode 100644 index 0000000000..051864285a --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface_plugin_description.xml @@ -0,0 +1,7 @@ + + + + The trajopt motion planner plugin which implements sequential convex optimization to solve motion planning problem. + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch new file mode 100644 index 0000000000..7185681292 --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml new file mode 100644 index 0000000000..cf5735429d --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + From 590a9c36d80ab55882d77f4c349c6344680a499b Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Fri, 16 Aug 2019 02:56:56 +0200 Subject: [PATCH 035/612] Add parameter to get_jacobian_matrix (#1595) * Add parameter to getJacobian Python interface * Add changes * Overload wrapper * Add test for reference_point_position * cleanup --- .../src/moveit_commander/move_group.py | 4 ++-- .../src/wrap_python_move_group.cpp | 20 +++++++++++++++---- .../test/python_move_group.py | 7 +++++++ 3 files changed, 25 insertions(+), 6 deletions(-) diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index a05c23cf85..0799e77c50 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/moveit_commander/src/moveit_commander/move_group.py @@ -596,7 +596,7 @@ def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor=1.0, traj_out.deserialize(ser_traj_out) return traj_out - def get_jacobian_matrix(self, joint_values): + def get_jacobian_matrix(self, joint_values, reference_point=None): """ Get the jacobian matrix of the group as a list""" - return self._g.get_jacobian_matrix(joint_values) + return self._g.get_jacobian_matrix(joint_values, [0.0, 0.0, 0.0] if reference_point is None else reference_point) diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index b3940182df..50646a8777 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -536,17 +536,28 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer } } - Eigen::MatrixXd getJacobianMatrixPython(bp::list& joint_values) + Eigen::MatrixXd getJacobianMatrixPython(const bp::list& joint_values, + const bp::object& reference_point = bp::object()) { - std::vector v = py_bindings_tools::doubleFromList(joint_values); + const std::vector v = py_bindings_tools::doubleFromList(joint_values); + std::vector ref; + if (reference_point.is_none()) + ref = { 0.0, 0.0, 0.0 }; + else + ref = py_bindings_tools::doubleFromList(reference_point); + if (ref.size() != 3) + throw std::invalid_argument("reference point needs to have 3 elements, got " + std::to_string(ref.size())); + robot_state::RobotState state(getRobotModel()); state.setToDefaultValues(); auto group = state.getJointModelGroup(getName()); state.setJointGroupPositions(group, v); - return state.getJacobian(group); + return state.getJacobian(group, Eigen::Map(&ref[0])); } }; +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getJacobianMatrixOverloads, getJacobianMatrixPython, 1, 2) + static void wrap_move_group_interface() { eigenpy::enableEigenPy(); @@ -683,7 +694,8 @@ static void wrap_move_group_interface() move_group_interface_class.def("get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython); move_group_interface_class.def("get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython); move_group_interface_class.def("get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython); - move_group_interface_class.def("get_jacobian_matrix", &MoveGroupInterfaceWrapper::getJacobianMatrixPython); + move_group_interface_class.def("get_jacobian_matrix", &MoveGroupInterfaceWrapper::getJacobianMatrixPython, + getJacobianMatrixOverloads()); } } // namespace planning_interface } // namespace moveit diff --git a/moveit_ros/planning_interface/test/python_move_group.py b/moveit_ros/planning_interface/test/python_move_group.py index 4e388a6f16..0cc27cd09d 100755 --- a/moveit_ros/planning_interface/test/python_move_group.py +++ b/moveit_ros/planning_interface/test/python_move_group.py @@ -80,6 +80,13 @@ def test_get_jacobian_matrix(self): [ 1. , 0. , 0. , 0. , 0. , 0. ]]) self.assertTrue(np.allclose(result, expected)) + result = self.group.get_jacobian_matrix(current, [1.0, 1.0, 1.0]) + expected = np.array([[ 1. , 1.8 , -1.2 , 0. , -1. , 0. ], + [ 1.89, 0. , 0. , 1. , 0. , 1. ], + [ 0. , -1.74, 1.74, 1. , 1.1 , 1. ], + [ 0. , 0. , 0. , -1. , 0. , -1. ], + [ 0. , 1. , -1. , 0. , -1. , 0. ], + [ 1. , 0. , 0. , 0. , 0. , 0. ]]) if __name__ == '__main__': PKGNAME = 'moveit_ros_planning_interface' From 0957a61ac2943cbe6cba52c38e83d0b0fe9a09a8 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 15 Aug 2019 22:16:29 +0200 Subject: [PATCH 036/612] hide display state: sanity-check to allow for empty state input --- .../robot_state_rviz_plugin/src/robot_state_display.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index d843923605..d62f710a8c 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -303,7 +304,8 @@ void RobotStateDisplay::newRobotStateCallback(const moveit_msgs::DisplayRobotSta // possibly use TF to construct a robot_state::Transforms object to pass in to the conversion function? try { - robot_state::robotStateMsgToRobotState(state_msg->state, *robot_state_); + if (!planning_scene::PlanningScene::isEmpty(state_msg->state)) + robot_state::robotStateMsgToRobotState(state_msg->state, *robot_state_); setRobotHighlights(state_msg->highlight_links); setStatus(rviz::StatusProperty::Ok, "RobotState", ""); } From b0f01b2dc79f99f5650933834c8e50905bdbe168 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 15 Aug 2019 22:22:32 +0200 Subject: [PATCH 037/612] display robot state: do not clutter rviz with default robot on enable --- .../robot_state_rviz_plugin/src/robot_state_display.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index d62f710a8c..0b00428a90 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -375,7 +375,6 @@ void RobotStateDisplay::loadRobotModel() changedEnableVisualVisible(); changedEnableCollisionVisible(); - robot_->setVisible(true); } else setStatus(rviz::StatusProperty::Error, "RobotState", "No Planning Model Loaded"); From c5e88677654654f0f8aca9945bd1bd2205616958 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 17 Aug 2019 12:34:32 +0200 Subject: [PATCH 038/612] add status to warn if RobotState display is hidden --- .../src/robot_state_display.cpp | 17 +++++++++++++---- .../robot_state_visualization.h | 5 +++++ 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 0b00428a90..70f2173d6a 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -290,6 +290,8 @@ void RobotStateDisplay::changedRobotStateTopic() if (static_cast(robot_state_)) robot_state_->setToDefaultValues(); update_state_ = true; + robot_->setVisible(false); + setStatus(rviz::StatusProperty::Warn, "RobotState", "No msg received"); robot_state_subscriber_ = root_nh_.subscribe(robot_state_topic_property_->getStdString(), 10, &RobotStateDisplay::newRobotStateCallback, this); @@ -307,17 +309,24 @@ void RobotStateDisplay::newRobotStateCallback(const moveit_msgs::DisplayRobotSta if (!planning_scene::PlanningScene::isEmpty(state_msg->state)) robot_state::robotStateMsgToRobotState(state_msg->state, *robot_state_); setRobotHighlights(state_msg->highlight_links); - setStatus(rviz::StatusProperty::Ok, "RobotState", ""); } catch (const moveit::Exception& e) { robot_state_->setToDefaultValues(); setRobotHighlights(moveit_msgs::DisplayRobotState::_highlight_links_type()); setStatus(rviz::StatusProperty::Error, "RobotState", e.what()); + robot_->setVisible(false); return; } - robot_->setVisible(!state_msg->hide); + if (robot_->isVisible() != !state_msg->hide) + { + robot_->setVisible(!state_msg->hide); + if (robot_->isVisible()) + setStatus(rviz::StatusProperty::Ok, "RobotState", ""); + else + setStatus(rviz::StatusProperty::Warn, "RobotState", "Hidden"); + } update_state_ = true; } @@ -371,13 +380,13 @@ void RobotStateDisplay::loadRobotModel() root_link_name_property_->setStdString(getRobotModel()->getRootLinkName()); root_link_name_property_->blockSignals(old_state); update_state_ = true; - setStatus(rviz::StatusProperty::Ok, "RobotState", "Planning Model Loaded Successfully"); + setStatus(rviz::StatusProperty::Ok, "RobotModel", "Loaded successfully"); changedEnableVisualVisible(); changedEnableCollisionVisible(); } else - setStatus(rviz::StatusProperty::Error, "RobotState", "No Planning Model Loaded"); + setStatus(rviz::StatusProperty::Error, "RobotModel", "Loading failed"); highlights_.clear(); } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index 49b299f1d1..09ffce9234 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -69,6 +69,11 @@ class RobotStateVisualization const std::map& color_map); void setDefaultAttachedObjectColor(const std_msgs::ColorRGBA& default_attached_object_color); + bool isVisible() const + { + return visible_; + } + /** * \brief Set the robot as a whole to be visible or not * @param visible Should we be visible? From 3854f7b58201c7590514828018e2ba1d70c60166 Mon Sep 17 00:00:00 2001 From: Raphael Druon Date: Sat, 17 Aug 2019 20:40:39 +0900 Subject: [PATCH 039/612] python planning_scene_interface: fix attaching objects (#1624) --- .../src/moveit_commander/planning_scene_interface.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/moveit_commander/src/moveit_commander/planning_scene_interface.py b/moveit_commander/src/moveit_commander/planning_scene_interface.py index a7fa6c9ad2..e0093e1508 100644 --- a/moveit_commander/src/moveit_commander/planning_scene_interface.py +++ b/moveit_commander/src/moveit_commander/planning_scene_interface.py @@ -69,7 +69,7 @@ def __init__(self, ns='', synchronous=False, service_timeout=5.0): def __submit(self, collision_object, attach=False): if self.__synchronous: - diff_req = self.__make_planning_scene_diff_req(collision_object) + diff_req = self.__make_planning_scene_diff_req(collision_object, attach) self._apply_planning_scene_diff.call(diff_req) else: if attach: @@ -306,10 +306,14 @@ def __make_cylinder(name, pose, height, radius): return co @staticmethod - def __make_planning_scene_diff_req(collision_object): + def __make_planning_scene_diff_req(collision_object, attach=False): scene = PlanningScene() scene.is_diff = True - scene.world.collision_objects = [collision_object] + scene.robot_state.is_diff = True + if attach: + scene.robot_state.attached_collision_objects = [collision_object] + else: + scene.world.collision_objects = [collision_object] planning_scene_diff_req = ApplyPlanningSceneRequest() planning_scene_diff_req.scene = scene return planning_scene_diff_req From 2e2552fef6b0a84b27db51a8c0bf319942019b4d Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Sat, 17 Aug 2019 15:33:34 +0200 Subject: [PATCH 040/612] Benchmark combinations of predefined poses (#1548) The CombinePredefinedPosesBenchmark allows conveniently planning trajectories between all pair-wise combinations of predefined poses that are specified in the benchmark config. --- moveit_ros/benchmarks/CMakeLists.txt | 4 + .../demo_panda_predefined_poses.launch | 33 ++++ .../examples/demo_panda_predefined_poses.yaml | 34 ++++ .../moveit/benchmarks/BenchmarkOptions.h | 28 ++++ .../benchmarks/src/BenchmarkExecutor.cpp | 2 + .../benchmarks/src/BenchmarkOptions.cpp | 12 ++ .../CombinePredefinedPosesBenchmark.cpp | 148 ++++++++++++++++++ 7 files changed, 261 insertions(+) create mode 100644 moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch create mode 100644 moveit_ros/benchmarks/examples/demo_panda_predefined_poses.yaml create mode 100644 moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt index 2463628a58..8a8f447e19 100644 --- a/moveit_ros/benchmarks/CMakeLists.txt +++ b/moveit_ros/benchmarks/CMakeLists.txt @@ -40,10 +40,14 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_executable(moveit_run_benchmark src/RunBenchmark.cpp) target_link_libraries(moveit_run_benchmark ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_executable(moveit_combine_predefined_poses_benchmark src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp) +target_link_libraries(moveit_combine_predefined_poses_benchmark ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) install( TARGETS ${MOVEIT_LIB_NAME} + moveit_run_benchmark + moveit_combine_predefined_poses_benchmark LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch new file mode 100644 index 0000000000..657fad0a6f --- /dev/null +++ b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.yaml b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.yaml new file mode 100644 index 0000000000..94f7803595 --- /dev/null +++ b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.yaml @@ -0,0 +1,34 @@ +# This is an example configuration that loads the "Kitchen" scene from the +# local MoveIt warehouse and benchmarks the "manipulator" group over all pairs +# of motion plan queries and start states in the Kitchen scene. + +# Five planners from two different plugins are run a total of 50 times each, with a +# maximum of 10 seconds per run. Output is stored in the /tmp/moveit_benchmarks directory. + +benchmark_config: + warehouse: + host: 127.0.0.1 + port: 33829 + scene_name: Kitchen # Required + parameters: + name: KitchenPick1 + runs: 50 + group: panda_arm # Required + timeout: 10.0 + output_directory: /tmp/moveit_benchmarks/ + predefined_poses_group: panda_arm_hand # Group where the predefined poses are specified + predefined_poses: # List of named targets + - ready + - extended + planning_pipelines: + - name: ompl + planners: + - RRTConnectkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - name: chomp + planners: + - CHOMP + - name: stomp + planners: + - STOMP diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h index 0006d42f39..92c7671529 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h @@ -47,31 +47,57 @@ namespace moveit_ros_benchmarks class BenchmarkOptions { public: + /** \brief Constructor */ BenchmarkOptions(); + /** \brief Constructor accepting a custom namespace for parameter lookup */ BenchmarkOptions(const std::string& ros_namespace); + /** \brief Destructor */ virtual ~BenchmarkOptions(); + /** \brief Set the ROS namespace the node handle should use for parameter lookup */ void setNamespace(const std::string& ros_namespace); + /** \brief Get the name of the warehouse database host server */ const std::string& getHostName() const; + /** \brief Get the port of the warehouse database host server */ int getPort() const; + /** \brief Get the reference name of the planning scene stored inside the warehouse database */ const std::string& getSceneName() const; + /** \brief Get the specified number of benchmark query runs */ int getNumRuns() const; + /** \brief Get the maximum timeout per planning attempt */ double getTimeout() const; + /** \brief Get the reference name of the benchmark */ const std::string& getBenchmarkName() const; + /** \brief Get the name of the planning group to run the benchmark with */ const std::string& getGroupName() const; + /** \brief Get the target directory for the generated benchmark result data */ const std::string& getOutputDirectory() const; + /** \brief Get the regex expression for matching the names of all queries to run */ const std::string& getQueryRegex() const; + /** \brief Get the regex expression for matching the names of all start states to plan from */ const std::string& getStartStateRegex() const; + /** \brief Get the regex expression for matching the names of all goal constraints to plan to */ const std::string& getGoalConstraintRegex() const; + /** \brief Get the regex expression for matching the names of all path constraints to plan with */ const std::string& getPathConstraintRegex() const; + /** \brief Get the regex expression for matching the names of all trajectory constraints to plan with */ const std::string& getTrajectoryConstraintRegex() const; + /** \brief Get the names of all predefined poses to consider for planning */ + const std::vector& getPredefinedPoses() const; + /** \brief Get the name of the planning group for which the predefined poses are defined */ + const std::string& getPredefinedPosesGroup() const; + /** \brief Get the constant position/orientation offset to be used for shifting all goal constraints */ void getGoalOffsets(std::vector& offsets) const; + /** \brief Get all planning pipeline names mapped to their parameter configuration */ const std::map>& getPlanningPipelineConfigurations() const; + /** \brief Get all planning pipeline names */ void getPlanningPipelineNames(std::vector& planning_pipeline_names) const; + /* \brief Get the frame id of the planning workspace */ const std::string& getWorkspaceFrameID() const; + /* \brief Get the parameter set of the planning workspace */ const moveit_msgs::WorkspaceParameters& getWorkspaceParameters() const; protected: @@ -100,6 +126,8 @@ class BenchmarkOptions std::string goal_constraint_regex_; std::string path_constraint_regex_; std::string trajectory_constraint_regex_; + std::vector predefined_poses_; + std::string predefined_poses_group_; double goal_offsets[6]; /// planner configurations diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 4f717cd97a..8e15bad1c1 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -501,6 +501,8 @@ bool BenchmarkExecutor::plannerConfigurationsExist( for (const std::pair& pipeline_entry : planning_pipelines_) { pipeline_exists = pipeline_entry.first == pipeline_config_entry.first; + if (pipeline_exists) + break; } if (!pipeline_exists) diff --git a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp index 1ed9c512a1..96dd820afb 100644 --- a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp @@ -136,6 +136,16 @@ const std::string& BenchmarkOptions::getTrajectoryConstraintRegex() const return trajectory_constraint_regex_; } +const std::vector& BenchmarkOptions::getPredefinedPoses() const +{ + return predefined_poses_; +} + +const std::string& BenchmarkOptions::getPredefinedPosesGroup() const +{ + return predefined_poses_group_; +} + void BenchmarkOptions::getGoalOffsets(std::vector& offsets) const { offsets.resize(6); @@ -189,6 +199,8 @@ void BenchmarkOptions::readBenchmarkParameters(ros::NodeHandle& nh) nh.param(std::string("benchmark_config/parameters/path_constraints"), path_constraint_regex_, std::string("")); nh.param(std::string("benchmark_config/parameters/trajectory_constraints"), trajectory_constraint_regex_, std::string("")); + nh.param(std::string("benchmark_config/parameters/predefined_poses"), predefined_poses_, {}); + nh.param(std::string("benchmark_config/parameters/predefined_poses_group"), predefined_poses_group_, std::string("")); if (!nh.getParam(std::string("benchmark_config/parameters/group"), group_name_)) ROS_WARN("Benchmark group NOT specified"); diff --git a/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp b/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp new file mode 100644 index 0000000000..5c6bdd4c51 --- /dev/null +++ b/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp @@ -0,0 +1,148 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2019, PickNik LLC +* 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 the PickNik LLC 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. +*********************************************************************/ + +/* Author: Henning Kayser */ +/* Description: A simple benchmark that plans trajectories for all combinations of specified predefined poses */ + +// MoveIt Benchmark +#include +#include + +// MoveIt +#include +#include +#include + +namespace moveit_ros_benchmarks +{ +constexpr char LOGNAME[] = "combine_predefined_poses_benchmark"; +class CombinePredefinedPosesBenchmark : public BenchmarkExecutor +{ +public: + virtual bool loadBenchmarkQueryData(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg, + std::vector& start_states, + std::vector& path_constraints, + std::vector& goal_constraints, + std::vector& traj_constraints, + std::vector& queries) + { + // Load planning scene + if (!psm_) + psm_.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description")); + if (!psm_->newPlanningSceneMessage(scene_msg)) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to load planning scene"); + return false; + } + + // Load robot model + if (!psm_->getRobotModel()) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to load robot model"); + return false; + } + + // Select planning group to use for predefined poses + std::string predefined_poses_group = opts.getPredefinedPosesGroup(); + if (predefined_poses_group.empty()) + { + ROS_WARN_NAMED(LOGNAME, "Parameter predefined_poses_group is not set, using default planning group instead"); + predefined_poses_group = opts.getGroupName(); + } + const auto& joint_model_group = psm_->getRobotModel()->getJointModelGroup(predefined_poses_group); + if (!joint_model_group) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Robot model has no joint model group named '" << predefined_poses_group << "'"); + return false; + } + + // Iterate over all predefined poses and use each as start and goal states + moveit::core::RobotState robot_state(psm_->getRobotModel()); + start_states.clear(); + goal_constraints.clear(); + for (const auto& pose_id : opts.getPredefinedPoses()) + { + if (!robot_state.setToDefaultValues(joint_model_group, pose_id)) + { + ROS_WARN_STREAM_NAMED(LOGNAME, "Failed to set robot state to named target '" << pose_id << "'"); + continue; + } + // Create start state + start_states.emplace_back(); + start_states.back().name = pose_id; + moveit::core::robotStateToRobotStateMsg(robot_state, start_states.back().state); + + // Create goal constraints + goal_constraints.emplace_back(); + goal_constraints.back().name = pose_id; + goal_constraints.back().constraints.push_back( + kinematic_constraints::constructGoalConstraints(robot_state, joint_model_group)); + } + if (start_states.empty() || goal_constraints.empty()) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to init start and goal states from predefined_poses"); + return false; + } + + // We don't use path/trajectory constraints or custom queries + path_constraints.clear(); + traj_constraints.clear(); + queries.clear(); + return true; + } + +private: + planning_scene_monitor::PlanningSceneMonitorPtr psm_; +}; +} // namespace moveit_ros_benchmarks + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "moveit_run_benchmark"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + // Read benchmark options from param server + moveit_ros_benchmarks::BenchmarkOptions opts(ros::this_node::getName()); + // Setup benchmark server + moveit_ros_benchmarks::CombinePredefinedPosesBenchmark server; + + std::vector planning_pipelines; + opts.getPlanningPipelineNames(planning_pipelines); + server.initialize(planning_pipelines); + + // Running benchmarks + if (!server.runBenchmarks(opts)) + ROS_ERROR("Failed to run all benchmarks"); +} From e7bc01e9d5557f0ad63724be65197d1bde15333c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Sat, 17 Aug 2019 17:18:58 +0200 Subject: [PATCH 041/612] PlanningRequestAdapter::initialize() = 0 (#1621) Implement initialize as pure virtual function. --- .../planning_request_adapter.h | 25 +++---------------- 1 file changed, 3 insertions(+), 22 deletions(-) diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h index 9c11302a02..daf9426ba0 100644 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h @@ -40,9 +40,6 @@ #include #include #include -#if !defined(_MSC_VER) -#include -#endif /** \brief Generic interface to adapting motion planning requests */ namespace planning_request_adapter @@ -65,25 +62,9 @@ class PlanningRequestAdapter { } - /// Initialize parameters using the passed NodeHandle - // TODO - Make initialize() a pure virtual function in O-turtle - virtual void initialize(const ros::NodeHandle& node_handle) - { - // Get name of derived adapter - std::string adapter_name = typeid(*this).name(); - // Try to demangle the name - int status = 1; -#if defined(_MSC_VER) - std::string demangled_name = adapter_name; -#else - std::string demangled_name = abi::__cxa_demangle(adapter_name.c_str(), NULL, NULL, &status); -#endif - if (status == 0) - adapter_name = demangled_name; - ROS_WARN_NAMED("planning_request_adapter", "Implementation of function initialize() is missing from '%s'." - "Any parameters should be loaded from the passed NodeHandle.", - adapter_name.c_str()); - } + /** \brief Initialize parameters using the passed NodeHandle + if no initialization is needed, simply implement as empty */ + virtual void initialize(const ros::NodeHandle& node_handle) = 0; /// Get a short string that identifies the planning request adapter virtual std::string getDescription() const From 4ca54cd0cda1bc0ceef072d769357ce3802e62da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Mon, 19 Aug 2019 11:23:58 +0200 Subject: [PATCH 042/612] move isEmpty test functions to moveit_core/utils (#1627) --- .../moveit/kinematic_constraints/utils.h | 2 +- .../kinematic_constraints/src/utils.cpp | 4 +- .../moveit/planning_scene/planning_scene.h | 8 +- .../planning_scene/src/planning_scene.cpp | 15 +--- .../test/test_planning_scene.cpp | 5 +- moveit_core/utils/CMakeLists.txt | 1 + .../include/moveit/utils/message_checks.h | 65 +++++++++++++++++ moveit_core/utils/src/message_checks.cpp | 73 +++++++++++++++++++ .../src/generate_state_database.cpp | 3 +- .../manipulation/pick_place/src/pick.cpp | 3 +- .../manipulation/pick_place/src/place.cpp | 3 +- .../cartesian_path_service_capability.cpp | 4 +- .../kinematics_service_capability.cpp | 4 +- .../move_action_capability.cpp | 10 +-- .../state_validation_service_capability.cpp | 4 +- .../plan_execution/src/plan_execution.cpp | 3 +- .../src/planning_scene_monitor.cpp | 5 +- .../src/robot_state_display.cpp | 4 +- 18 files changed, 178 insertions(+), 38 deletions(-) create mode 100644 moveit_core/utils/include/moveit/utils/message_checks.h create mode 100644 moveit_core/utils/src/message_checks.cpp diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h index f2dfe6f84a..54080fbacb 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h @@ -67,7 +67,7 @@ moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints& first, const moveit_msgs::Constraints& second); /** \brief Check if any constraints were specified */ -bool isEmpty(const moveit_msgs::Constraints& constr); +[[deprecated("Use moveit/utils/message_checks.h instead")]] bool isEmpty(const moveit_msgs::Constraints& constr); std::size_t countIndividualConstraints(const moveit_msgs::Constraints& constr); diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 568efbb95b..59beecec6c 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -38,6 +38,7 @@ #include #include #include +#include using namespace moveit::core; @@ -116,8 +117,7 @@ moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints& first, bool isEmpty(const moveit_msgs::Constraints& constr) { - return constr.position_constraints.empty() && constr.orientation_constraints.empty() && - constr.visibility_constraints.empty() && constr.joint_constraints.empty(); + return moveit::core::isEmpty(constr); } std::size_t countIndividualConstraints(const moveit_msgs::Constraints& constr) diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 32adb11f61..c7954cce12 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -953,14 +953,16 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check if a message includes any information about a planning scene, or it is just a default, empty message. */ - static bool isEmpty(const moveit_msgs::PlanningScene& msg); + [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool + isEmpty(const moveit_msgs::PlanningScene& msg); /** \brief Check if a message includes any information about a planning scene world, or it is just a default, empty * message. */ - static bool isEmpty(const moveit_msgs::PlanningSceneWorld& msg); + [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool + isEmpty(const moveit_msgs::PlanningSceneWorld& msg); /** \brief Check if a message includes any information about a robot state, or it is just a default, empty message. */ - static bool isEmpty(const moveit_msgs::RobotState& msg); + [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool isEmpty(const moveit_msgs::RobotState& msg); /** \brief Clone a planning scene. Even if the scene \e scene depends on a parent, the cloned scene will not. */ static PlanningScenePtr clone(const PlanningSceneConstPtr& scene); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 37b62f4d85..279f824e83 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -96,25 +97,17 @@ class SceneTransforms : public robot_state::Transforms bool PlanningScene::isEmpty(const moveit_msgs::PlanningScene& msg) { - return msg.name.empty() && msg.fixed_frame_transforms.empty() && msg.allowed_collision_matrix.entry_names.empty() && - msg.link_padding.empty() && msg.link_scale.empty() && isEmpty(msg.robot_state) && isEmpty(msg.world); + return moveit::core::isEmpty(msg); } bool PlanningScene::isEmpty(const moveit_msgs::RobotState& msg) { - /* a state is empty if it includes no information and it is a diff; if the state is not a diff, then the implicit - information is - that the set of attached bodies is empty, so they must be cleared from the state to be updated */ - return static_cast(msg.is_diff) && msg.multi_dof_joint_state.joint_names.empty() && - msg.joint_state.name.empty() && msg.attached_collision_objects.empty() && msg.joint_state.position.empty() && - msg.joint_state.velocity.empty() && msg.joint_state.effort.empty() && - msg.multi_dof_joint_state.transforms.empty() && msg.multi_dof_joint_state.twist.empty() && - msg.multi_dof_joint_state.wrench.empty(); + return moveit::core::isEmpty(msg); } bool PlanningScene::isEmpty(const moveit_msgs::PlanningSceneWorld& msg) { - return msg.collision_objects.empty() && msg.octomap.octomap.data.empty(); + return moveit::core::isEmpty(msg); } PlanningScene::PlanningScene(const robot_model::RobotModelConstPtr& robot_model, diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 73b68186e3..9d93148edb 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -104,10 +105,10 @@ TEST(PlanningScene, LoadRestoreDiff) moveit_msgs::PlanningScene ps_msg; ps_msg.robot_state.is_diff = true; - EXPECT_TRUE(planning_scene::PlanningScene::isEmpty(ps_msg)); + EXPECT_TRUE(moveit::core::isEmpty(ps_msg)); ps->getPlanningSceneMsg(ps_msg); ps->setPlanningSceneMsg(ps_msg); - EXPECT_FALSE(planning_scene::PlanningScene::isEmpty(ps_msg)); + EXPECT_FALSE(moveit::core::isEmpty(ps_msg)); EXPECT_TRUE(world.hasObject("sphere")); planning_scene::PlanningScenePtr next = ps->diff(); diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index 9ea399702a..8a9aaa49bd 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -3,6 +3,7 @@ set(MOVEIT_LIB_NAME moveit_utils) add_library(${MOVEIT_LIB_NAME} src/lexical_casts.cpp src/xmlrpc_casts.cpp + src/message_checks.cpp ) target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) diff --git a/moveit_core/utils/include/moveit/utils/message_checks.h b/moveit_core/utils/include/moveit/utils/message_checks.h new file mode 100644 index 0000000000..4ac3c0af8d --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/message_checks.h @@ -0,0 +1,65 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Universitaet Hamburg. + * 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. + *********************************************************************/ + +/* Author: Michael 'v4hn' Goerner */ + +#pragma once + +/** \file empty_msgs.h + * \brief Checks for empty MoveIt-related messages + * + */ + +#include +#include +#include +#include + +namespace moveit +{ +namespace core +{ +/** \brief Check if a message includes any information about a planning scene, or whether it is empty. */ +bool isEmpty(const moveit_msgs::PlanningScene& msg); + +/** \brief Check if a message includes any information about a planning scene world, or whether it is empty. */ +bool isEmpty(const moveit_msgs::PlanningSceneWorld& msg); + +/** \brief Check if a message includes any information about a robot state, or whether it is empty. */ +bool isEmpty(const moveit_msgs::RobotState& msg); + +/** \brief Check if a message specifies constraints */ +bool isEmpty(const moveit_msgs::Constraints& msg); +} +} diff --git a/moveit_core/utils/src/message_checks.cpp b/moveit_core/utils/src/message_checks.cpp new file mode 100644 index 0000000000..b466e4768b --- /dev/null +++ b/moveit_core/utils/src/message_checks.cpp @@ -0,0 +1,73 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Universitaet Hamburg. + * 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 Hamburg University 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. + *********************************************************************/ + +/* Author: Michael Goerner */ + +#include + +namespace moveit +{ +namespace core +{ +bool isEmpty(const moveit_msgs::PlanningScene& msg) +{ + return msg.name.empty() && msg.fixed_frame_transforms.empty() && msg.allowed_collision_matrix.entry_names.empty() && + msg.link_padding.empty() && msg.link_scale.empty() && isEmpty(msg.robot_state) && isEmpty(msg.world); +} + +bool isEmpty(const moveit_msgs::PlanningSceneWorld& msg) +{ + return msg.collision_objects.empty() && msg.octomap.octomap.data.empty(); +} + +bool isEmpty(const moveit_msgs::RobotState& msg) +{ + /* a state is empty if it includes no information and it is a diff; if the state is not a diff, then the implicit + information is + that the set of attached bodies is empty, so they must be cleared from the state to be updated */ + return static_cast(msg.is_diff) && msg.multi_dof_joint_state.joint_names.empty() && + msg.joint_state.name.empty() && msg.attached_collision_objects.empty() && msg.joint_state.position.empty() && + msg.joint_state.velocity.empty() && msg.joint_state.effort.empty() && + msg.multi_dof_joint_state.transforms.empty() && msg.multi_dof_joint_state.twist.empty() && + msg.multi_dof_joint_state.wrench.empty(); +} + +bool isEmpty(const moveit_msgs::Constraints& constr) +{ + return constr.position_constraints.empty() && constr.orientation_constraints.empty() && + constr.visibility_constraints.empty() && constr.joint_constraints.empty(); +} + +} // namespace core +} // namespace moveit diff --git a/moveit_planners/ompl/ompl_interface/src/generate_state_database.cpp b/moveit_planners/ompl/ompl_interface/src/generate_state_database.cpp index f1cd7d56c6..e2cbf39c25 100644 --- a/moveit_planners/ompl/ompl_interface/src/generate_state_database.cpp +++ b/moveit_planners/ompl/ompl_interface/src/generate_state_database.cpp @@ -42,6 +42,7 @@ #include #include +#include #include #include @@ -165,7 +166,7 @@ int main(int argc, char** argv) } } - if (kinematic_constraints::isEmpty(params.constraints)) + if (moveit::core::isEmpty(params.constraints)) { ROS_FATAL_NAMED(LOGNAME, "Abort. Constraint description is an empty set of constraints."); return 1; diff --git a/moveit_ros/manipulation/pick_place/src/pick.cpp b/moveit_ros/manipulation/pick_place/src/pick.cpp index 5b922a6589..f2e868ae79 100644 --- a/moveit_ros/manipulation/pick_place/src/pick.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include namespace pick_place @@ -230,7 +231,7 @@ PickPlanPtr PickPlace::planPick(const planning_scene::PlanningSceneConstPtr& pla { PickPlanPtr p(new PickPlan(shared_from_this())); - if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff)) + if (moveit::core::isEmpty(goal.planning_options.planning_scene_diff)) p->plan(planning_scene, goal); else p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal); diff --git a/moveit_ros/manipulation/pick_place/src/place.cpp b/moveit_ros/manipulation/pick_place/src/place.cpp index 098d5c1438..ae7e1f04cf 100644 --- a/moveit_ros/manipulation/pick_place/src/place.cpp +++ b/moveit_ros/manipulation/pick_place/src/place.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -369,7 +370,7 @@ PlacePlanPtr PickPlace::planPlace(const planning_scene::PlanningSceneConstPtr& p const moveit_msgs::PlaceGoal& goal) const { PlacePlanPtr p(new PlacePlan(shared_from_this())); - if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff)) + if (moveit::core::isEmpty(goal.planning_options.planning_scene_diff)) p->plan(planning_scene, goal); else p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal); diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 15d5d29c60..a2f467b1d9 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -36,7 +36,7 @@ #include "cartesian_path_service_capability.h" #include -#include +#include #include #include #include @@ -131,7 +131,7 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath robot_state::GroupStateValidityCallbackFn constraint_fn; std::unique_ptr ls; std::unique_ptr kset; - if (req.avoid_collisions || !kinematic_constraints::isEmpty(req.path_constraints)) + if (req.avoid_collisions || !moveit::core::isEmpty(req.path_constraints)) { ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)); kset.reset(new kinematic_constraints::KinematicConstraintSet((*ls)->getRobotModel())); diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index 73e3932e18..b1e86fa6b4 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -36,7 +36,7 @@ #include "kinematics_service_capability.h" #include -#include +#include #include #include @@ -148,7 +148,7 @@ bool MoveGroupKinematicsService::computeIKService(moveit_msgs::GetPositionIK::Re context_->planning_scene_monitor_->updateFrameTransforms(); // check if the planning scene needs to be kept locked; if so, call computeIK() in the scope of the lock - if (req.ik_request.avoid_collisions || !kinematic_constraints::isEmpty(req.ik_request.constraints)) + if (req.ik_request.avoid_collisions || !moveit::core::isEmpty(req.ik_request.constraints)) { planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_); kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel()); diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index cc6c36557a..26c619b7d8 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include namespace move_group @@ -102,7 +103,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const moveit_msgs::M ROS_INFO_NAMED(getName(), "Combined planning and execution request received for MoveGroup action. " "Forwarding to planning and execution pipeline."); - if (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff)) + if (moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) { planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); const robot_state::RobotState& current_state = lscene->getCurrentState(); @@ -122,10 +123,9 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const moveit_msgs::M plan_execution::PlanExecution::Options opt; const moveit_msgs::MotionPlanRequest& motion_plan_request = - planning_scene::PlanningScene::isEmpty(goal->request.start_state) ? goal->request : - clearRequestStartState(goal->request); + moveit::core::isEmpty(goal->request.start_state) ? goal->request : clearRequestStartState(goal->request); const moveit_msgs::PlanningScene& planning_scene_diff = - planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ? + moveit::core::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ? goal->planning_options.planning_scene_diff : clearSceneRobotState(goal->planning_options.planning_scene_diff); @@ -168,7 +168,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const moveit_msgs::MoveGro // lock the scene so that it does not modify the world representation while diff() is called planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); const planning_scene::PlanningSceneConstPtr& the_scene = - (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff)) ? + (moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) ? static_cast(lscene) : lscene->diff(goal->planning_options.planning_scene_diff); planning_interface::MotionPlanResponse res; diff --git a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp index 16e6714665..c333f4a9a7 100644 --- a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp @@ -36,7 +36,7 @@ #include "state_validation_service_capability.h" #include -#include +#include #include #include @@ -100,7 +100,7 @@ bool MoveGroupStateValidationService::computeService(moveit_msgs::GetStateValidi } // evaluate constraints - if (!kinematic_constraints::isEmpty(req.constraints)) + if (!moveit::core::isEmpty(req.constraints)) { kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel()); kset.add(req.constraints, ls->getTransforms()); diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 6621592343..e3f9bb84a9 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -141,7 +142,7 @@ void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan& plan, c void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan& plan, const moveit_msgs::PlanningScene& scene_diff, const Options& opt) { - if (planning_scene::PlanningScene::isEmpty(scene_diff)) + if (moveit::core::isEmpty(scene_diff)) planAndExecute(plan, opt); else { diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 23a0ca3f28..a2138f668d 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -565,13 +566,13 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningSc if (no_other_scene_upd) { upd = UPDATE_NONE; - if (!planning_scene::PlanningScene::isEmpty(scene.world)) + if (!moveit::core::isEmpty(scene.world)) upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY); if (!scene.fixed_frame_transforms.empty()) upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS); - if (!planning_scene::PlanningScene::isEmpty(scene.robot_state)) + if (!moveit::core::isEmpty(scene.robot_state)) { upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE); if (!scene.robot_state.attached_collision_objects.empty() || !static_cast(scene.robot_state.is_diff)) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 70f2173d6a..98e024fea0 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -36,7 +36,7 @@ #include #include -#include +#include #include #include @@ -306,7 +306,7 @@ void RobotStateDisplay::newRobotStateCallback(const moveit_msgs::DisplayRobotSta // possibly use TF to construct a robot_state::Transforms object to pass in to the conversion function? try { - if (!planning_scene::PlanningScene::isEmpty(state_msg->state)) + if (!moveit::core::isEmpty(state_msg->state)) robot_state::robotStateMsgToRobotState(state_msg->state, *robot_state_); setRobotHighlights(state_msg->highlight_links); } From 1f4de10e72e2a414a6df421df396a84677e8af5f Mon Sep 17 00:00:00 2001 From: Sean Yen Date: Wed, 21 Aug 2019 04:35:47 -0700 Subject: [PATCH 043/612] favor ros::Duration.sleep over sleep. (#1634) --- .../planning_components_tools/src/display_random_state.cpp | 2 +- .../planning_components_tools/src/publish_scene_from_text.cpp | 2 +- .../planning/planning_scene_monitor/demos/demo_scene.cpp | 2 +- .../planning_interface/move_group_interface/src/demo.cpp | 2 +- moveit_setup_assistant/src/widgets/start_screen_widget.cpp | 4 ++-- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp index f70c4e0860..ac7bf2c7ec 100644 --- a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp +++ b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp @@ -136,7 +136,7 @@ int main(int argc, char** argv) pub_scene.publish(psmsg); std::cout << psm.getPlanningScene()->getCurrentState() << std::endl; - sleep(1); + ros::Duration(1).sleep(); } } while (nh.ok()); diff --git a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp index 274e250433..b2f441d5c8 100644 --- a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp +++ b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp @@ -90,7 +90,7 @@ int main(int argc, char** argv) else pub_scene.publish(ps_msg.world); - sleep(1); + ros::Duration(1).sleep(); } } else diff --git a/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp b/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp index 681d6af0d2..976f1ba678 100644 --- a/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp +++ b/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp @@ -76,7 +76,7 @@ void sendKnife() co.primitive_poses[0].orientation.w = 1.0; pub_aco.publish(aco); - sleep(1); + ros::Duration(1).sleep(); pub_aco.publish(aco); ROS_INFO("Object published."); ros::Duration(1.5).sleep(); diff --git a/moveit_ros/planning_interface/move_group_interface/src/demo.cpp b/moveit_ros/planning_interface/move_group_interface/src/demo.cpp index 6aba08bcc6..0bd7017fce 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/demo.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/demo.cpp @@ -115,7 +115,7 @@ int main(int argc, char** argv) moveit::planning_interface::MoveGroupInterface group(argc > 1 ? argv[1] : "right_arm"); demoPlace(group); - sleep(2); + ros::Duration(2).sleep(); return 0; } diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 10320c3b1f..a980d61071 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -561,7 +561,7 @@ bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const st while (!nh.ok() && steps < 4) { ROS_WARN("Waiting for node handle"); - sleep(1); + ros::Duration(1).sleep(); steps++; ros::spinOnce(); } @@ -610,7 +610,7 @@ bool StartScreenWidget::setSRDFFile(const std::string& srdf_string) while (!nh.ok() && steps < 4) { ROS_WARN("Waiting for node handle"); - sleep(1); + ros::Duration(1).sleep(); steps++; ros::spinOnce(); } From 8066a509ddb3a4c61ca2ebe2add8f0e2997e95d0 Mon Sep 17 00:00:00 2001 From: Jens P Date: Wed, 21 Aug 2019 17:13:13 +0200 Subject: [PATCH 044/612] Unified Collision Environment Integration (#1584) * Unified collision environment * Integrating FCL unified environment into the planning scene * Distance field collision environment * Collision distance field and hybrid compiles * PR review: * collision environmnet test cases adapted * allocating of child planning scenes * valided padding and scaling added * reordering of member variables and functions * license adaptions * Unified all_valid collision detector * Replace references to CollisionWorld / CollisionRobot to new CollisionEnv * SBPL planner adapted for unified collision environment * PR review: * added as author * added documentation to collision environments * Added change description to migration notes. * Replaced getCollisionWorld/Robot with getCollisionEnv functions * PR review: * change to pragma once include guards * enable test --- MIGRATION.md | 1 + .../collision_detection/CMakeLists.txt | 6 +- .../collision_detector_allocator_allvalid.h | 8 +- ...ld_allvalid.h => collision_env_allvalid.h} | 54 +- .../allvalid/collision_robot_allvalid.h | 85 --- .../collision_detector_allocator.h | 34 +- .../{collision_robot.h => collision_env.h} | 231 +++---- .../collision_octomap_filter.h | 2 +- .../collision_detection/collision_plugin.h | 6 +- .../collision_detection/collision_world.h | 279 -------- ...llvalid.cpp => collision_env_allvalid.cpp} | 100 ++- .../src/allvalid/collision_robot_allvalid.cpp | 171 ----- ...{collision_robot.cpp => collision_env.cpp} | 91 ++- .../src/collision_octomap_filter.cpp | 1 - .../src/collision_world.cpp | 96 --- .../test/test_all_valid.cpp | 8 +- .../collision_detection_fcl/CMakeLists.txt | 6 +- .../collision_common.h | 2 +- .../collision_detector_allocator_fcl.h | 7 +- .../collision_env_fcl.h | 157 +++++ .../collision_robot_fcl.h | 97 --- .../collision_world_fcl.h | 98 --- .../src/collision_env_fcl.cpp | 434 +++++++++++++ .../src/collision_robot_fcl.cpp | 307 --------- .../src/collision_world_fcl.cpp | 299 --------- .../test/test_fcl_collision_detection.cpp | 111 ++-- .../test/test_fcl_env.cpp | 324 ++++++++++ .../collision_distance_field/CMakeLists.txt | 6 +- .../collision_common_distance_field.h | 2 +- ...lision_detector_allocator_distance_field.h | 8 +- .../collision_detector_allocator_hybrid.h | 8 +- ...field.h => collision_env_distance_field.h} | 208 +++--- .../collision_env_hybrid.h | 153 +++++ .../collision_robot_hybrid.h | 98 --- .../collision_world_distance_field.h | 200 ------ .../collision_world_hybrid.h | 116 ---- ...d.cpp => collision_env_distance_field.cpp} | 595 +++++++++++++++--- .../src/collision_env_hybrid.cpp | 185 ++++++ .../src/collision_robot_hybrid.cpp | 96 --- .../src/collision_world_distance_field.cpp | 568 ----------------- .../src/collision_world_hybrid.cpp | 160 ----- .../test/test_collision_distance_field.cpp | 56 +- .../kinematic_constraint.h | 4 +- .../src/kinematic_constraint.cpp | 12 +- .../moveit/planning_scene/planning_scene.h | 61 +- .../planning_scene/src/planning_scene.cpp | 183 ++---- .../chomp_motion_planner/chomp_optimizer.h | 6 +- .../src/chomp_optimizer.cpp | 19 +- .../sbpl_interface/environment_chain3d.h | 6 +- .../src/environment_chain3d.cpp | 27 +- .../src/planning_scene_monitor.cpp | 8 +- .../motion_planning_frame.h | 2 +- .../src/motion_planning_frame_objects.cpp | 12 +- .../src/planning_scene_render.cpp | 2 +- 54 files changed, 2401 insertions(+), 3415 deletions(-) rename moveit_core/collision_detection/include/moveit/collision_detection/allvalid/{collision_world_allvalid.h => collision_env_allvalid.h} (57%) delete mode 100644 moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h rename moveit_core/collision_detection/include/moveit/collision_detection/{collision_robot.h => collision_env.h} (60%) delete mode 100644 moveit_core/collision_detection/include/moveit/collision_detection/collision_world.h rename moveit_core/collision_detection/src/allvalid/{collision_world_allvalid.cpp => collision_env_allvalid.cpp} (53%) delete mode 100644 moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp rename moveit_core/collision_detection/src/{collision_robot.cpp => collision_env.cpp} (66%) delete mode 100644 moveit_core/collision_detection/src/collision_world.cpp create mode 100644 moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h delete mode 100644 moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h delete mode 100644 moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h create mode 100644 moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp delete mode 100644 moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp delete mode 100644 moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp create mode 100644 moveit_core/collision_detection_fcl/test/test_fcl_env.cpp rename moveit_core/collision_distance_field/include/moveit/collision_distance_field/{collision_robot_distance_field.h => collision_env_distance_field.h} (57%) create mode 100644 moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h delete mode 100644 moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_hybrid.h delete mode 100644 moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h delete mode 100644 moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_hybrid.h rename moveit_core/collision_distance_field/src/{collision_robot_distance_field.cpp => collision_env_distance_field.cpp} (68%) create mode 100644 moveit_core/collision_distance_field/src/collision_env_hybrid.cpp delete mode 100644 moveit_core/collision_distance_field/src/collision_robot_hybrid.cpp delete mode 100644 moveit_core/collision_distance_field/src/collision_world_distance_field.cpp delete mode 100644 moveit_core/collision_distance_field/src/collision_world_hybrid.cpp diff --git a/MIGRATION.md b/MIGRATION.md index 5eca3c2660..7fe1098d53 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -8,6 +8,7 @@ API changes in MoveIt releases - Moved the example package `moveit_controller_manager_example` into [moveit_tutorials](https://github.com/ros-planning/moveit_tutorials) - Requests to `get_planning_scene` service without explicitly setting "components" now return full scene - `moveit_ros_plannning` no longer depends on `moveit_ros_perception` +- `collision_robot` and `collision_world` are combined into a single `collision_env`. This applies for all derived collision checkers as `FCL`, `ALL_VALID`, `HYBRID` and `DISTANCE_FIELD`. `getCollisionRobot[Unpadded] / getCollisionWorld` functions return the new combined environment and will be deprecated in the future. ## ROS Melodic diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt index 21d3bcb5d3..262db3780d 100644 --- a/moveit_core/collision_detection/CMakeLists.txt +++ b/moveit_core/collision_detection/CMakeLists.txt @@ -1,15 +1,13 @@ set(MOVEIT_LIB_NAME moveit_collision_detection) add_library(${MOVEIT_LIB_NAME} - src/allvalid/collision_robot_allvalid.cpp - src/allvalid/collision_world_allvalid.cpp + src/allvalid/collision_env_allvalid.cpp src/collision_matrix.cpp src/collision_octomap_filter.cpp - src/collision_robot.cpp src/collision_tools.cpp - src/collision_world.cpp src/world.cpp src/world_diff.cpp + src/collision_env.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h index 0382b492d7..7782670035 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h @@ -37,17 +37,15 @@ #pragma once #include -#include -#include +#include namespace collision_detection { /** \brief An allocator for AllValid collision detectors */ class CollisionDetectorAllocatorAllValid - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_world_allvalid.cpp + static const std::string NAME; // defined in collision_env_allvalid.cpp }; } diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h similarity index 57% rename from moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h rename to moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h index dcf045b554..024a59496b 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h @@ -32,45 +32,45 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Jia Pan, Jens Petit */ #pragma once -#include +#include namespace collision_detection { -class CollisionRobotAllValid; - -class CollisionWorldAllValid : public CollisionWorld +/** \brief Collision environment which always just returns no collisions. + * + * This can be used to save resources if collision checking is not important. */ +class CollisionEnvAllValid : public CollisionEnv { public: - CollisionWorldAllValid(); - explicit CollisionWorldAllValid(const WorldPtr& world); - CollisionWorldAllValid(const CollisionWorld& other, const WorldPtr& world); + CollisionEnvAllValid(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); + CollisionEnvAllValid(const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, double padding = 0.0, + double scale = 1.0); + CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world); - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const override; - - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const override; - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, + const robot_state::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state) const; - virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const; - void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, + virtual double distanceRobot(const robot_state::RobotState& state) const; + virtual double distanceRobot(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; + void distanceRobot(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state) const override; - virtual double distanceWorld(const CollisionWorld& world) const; - virtual double distanceWorld(const CollisionWorld& world, const AllowedCollisionMatrix& acm) const; - void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override; + + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const override; + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + void distanceSelf(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const override; }; } diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h deleted file mode 100644 index 9ce0e2dc15..0000000000 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h +++ /dev/null @@ -1,85 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * 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. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#pragma once - -#include - -namespace collision_detection -{ -class CollisionRobotAllValid : public CollisionRobot -{ -public: - CollisionRobotAllValid(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); - CollisionRobotAllValid(const CollisionRobot& other); - - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const override; - - virtual double distanceSelf(const robot_state::RobotState& state) const; - virtual double distanceSelf(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; - void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override; - - virtual double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const; - virtual double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, const AllowedCollisionMatrix& acm) const; - void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override; -}; -} diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h index 018d6be5e0..f1bb41d857 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h @@ -36,8 +36,7 @@ #pragma once -#include -#include +#include #include namespace collision_detection @@ -56,22 +55,20 @@ class CollisionDetectorAllocator virtual const std::string& getName() const = 0; /** create a new CollisionWorld for checking collisions with the supplied world. */ - virtual CollisionWorldPtr allocateWorld(const WorldPtr& world) const = 0; + virtual CollisionEnvPtr allocateEnv(const WorldPtr& world, + const robot_model::RobotModelConstPtr& robot_model) const = 0; /** create a new CollisionWorld by copying an existing CollisionWorld of the same type.s * The world must be either the same world as used by \orig or a copy of that world which has not yet been modified. */ - virtual CollisionWorldPtr allocateWorld(const CollisionWorldConstPtr& orig, const WorldPtr& world) const = 0; + virtual CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const = 0; - /** create a new CollisionRobot given a robot_model */ - virtual CollisionRobotPtr allocateRobot(const robot_model::RobotModelConstPtr& robot_model) const = 0; - - /** create a new CollisionRobot by copying an existing CollisionRobot of the same type. */ - virtual CollisionRobotPtr allocateRobot(const CollisionRobotConstPtr& orig) const = 0; + /** create a new CollisionEnv given a robot_model with a new empty world */ + virtual CollisionEnvPtr allocateEnv(const robot_model::RobotModelConstPtr& robot_model) const = 0; }; /** \brief Template class to make it easy to create an allocator for a specific CollisionWorld/CollisionRobot pair. */ -template +template class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator { public: @@ -80,24 +77,19 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator return CollisionDetectorAllocatorType::NAME; } - CollisionWorldPtr allocateWorld(const WorldPtr& world) const override - { - return CollisionWorldPtr(new CollisionWorldType(world)); - } - - CollisionWorldPtr allocateWorld(const CollisionWorldConstPtr& orig, const WorldPtr& world) const override + CollisionEnvPtr allocateEnv(const WorldPtr& world, const robot_model::RobotModelConstPtr& robot_model) const override { - return CollisionWorldPtr(new CollisionWorldType(dynamic_cast(*orig), world)); + return CollisionEnvPtr(new CollisionEnvType(robot_model, world)); } - CollisionRobotPtr allocateRobot(const robot_model::RobotModelConstPtr& robot_model) const override + CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const override { - return CollisionRobotPtr(new CollisionRobotType(robot_model)); + return CollisionEnvPtr(new CollisionEnvType(dynamic_cast(*orig), world)); } - CollisionRobotPtr allocateRobot(const CollisionRobotConstPtr& orig) const override + CollisionEnvPtr allocateEnv(const robot_model::RobotModelConstPtr& robot_model) const override { - return CollisionRobotPtr(new CollisionRobotType(dynamic_cast(*orig))); + return CollisionEnvPtr(new CollisionEnvType(robot_model)); } /** Create an allocator for FCL collision detectors */ diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h similarity index 60% rename from moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h rename to moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h index 2249b56c73..c634ca8597 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h @@ -14,7 +14,7 @@ * 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 + * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Jens Petit */ #pragma once @@ -41,34 +41,43 @@ #include #include #include +#include namespace collision_detection { -MOVEIT_CLASS_FORWARD(CollisionRobot); +MOVEIT_CLASS_FORWARD(CollisionEnv); -/** @brief This class represents a collision model of the robot and can be used for self collision checks - (to check if the robot is in collision with itself) or in collision checks with a different robot. Collision checks - with - the environment are performed using the CollisionWorld class. */ -class CollisionRobot +/** \brief Provides the interface to the individual collision checking libraries. */ +class CollisionEnv { public: + CollisionEnv() = delete; + /** @brief Constructor * @param model A robot model to construct the collision robot from * @param padding The padding to use for all objects/links on the robot * @scale scale A common scaling to use for all objects/links on the robot */ - CollisionRobot(const robot_model::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); + CollisionEnv(const robot_model::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); - /** @brief A copy constructor*/ - CollisionRobot(const CollisionRobot& other); + /** @brief Constructor + * @param model A robot model to construct the collision robot from + * @param padding The padding to use for all objects/links on the robot + * @scale scale A common scaling to use for all objects/links on the robot + */ + CollisionEnv(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, + double scale = 1.0); + + /** \brief Copy constructor */ + CollisionEnv(const CollisionEnv& other, const WorldPtr& world); - virtual ~CollisionRobot() + virtual ~CollisionEnv() { } - /** @brief Check for self collision. Any collision between any pair of links is checked for, NO collisions are + /** @brief Check for robot self collision. Any collision between any pair of links is checked for, NO collisions are * ignored. + * * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result * @param state The kinematic state for which checks are being made */ @@ -84,82 +93,75 @@ class CollisionRobot virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; - /** @brief Check for self collision in a continuous manner. Any collision between any pair of links is checked for, - * NO collisions are ignored. + /** \brief Check whether the robot model is in collision with itself or the world at a particular state. + * Any collision between any pair of links is checked for, NO collisions are ignored. * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result - * @param state1 The kinematic state at the start of the segment for which checks are being made - * @param state2 The kinematic state at the end of the segment for which checks are being made */ - virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const = 0; + * @param state The kinematic state for which checks are being made */ + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const; - /** \brief Check for self collision. Allowed collisions specified by the allowed collision matrix are - * taken into account. + /** \brief Check whether the robot model is in collision with itself or the world at a particular state. + * Allowed collisions specified by the allowed collision matrix are taken into account. * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result - * @param state1 The kinematic state at the start of the segment for which checks are being made - * @param state2 The kinematic state at the end of the segment for which checks are being made + * @param state The kinematic state for which checks are being made * @param acm The allowed collision matrix. */ - virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const = 0; + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const; - /** \brief Check for collision with a different robot (possibly a different kinematic model as well). - * Any collision between any pair of links is checked for, NO collisions are ignored. + /** \brief Check whether the robot model is in collision with the world. Any collisions between a robot link + * and the world are considered. Self collisions are not checked. * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result - * @param state The kinematic state for which checks are being made. - * @param other_robot The collision representation for the other robot - * @param other_state The kinematic state corresponding to the other robot */ - virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const = 0; - - /** \brief Check for collision with a different robot (possibly a different kinematic model as well). - * Allowed collisions specified by the allowed collision matrix are taken into account. + * @robot robot The collision model for the robot + * @param state The kinematic state for which checks are being made + */ + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const = 0; + + /** \brief Check whether the robot model is in collision with the world. + * Allowed collisions are ignored. Self collisions are not checked. * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result - * @param state The kinematic state for which checks are being made. - * @param other_robot The collision representation for the other robot - * @param other_state The kinematic state corresponding to the other robot - * @param acm The allowed collision matrix. */ - virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const = 0; + * @robot robot The collision model for the robot + * @param state The kinematic state for which checks are being made + * @param acm The allowed collision matrix.*/ + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; - /** \brief Check for collision with a different robot (possibly a different kinematic model as well), in a continuous - * fashion. - * Any collision between any pair of links is checked for, NO collisions are ignored. + /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot + * states). + * Allowed collisions are ignored. Self collisions are not checked. * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result - * @param state1 The kinematic state at the start of the segment for which checks are being made (this robot) - * @param state2 The kinematic state at the end of the segment for which checks are being made (this robot) - * @param other_robot The collision representation for the other robot - * @param other_state1 The kinematic state at the start of the segment for which checks are being made (other robot) - * @param other_state2 The kinematic state at the end of the segment for which checks are being made (other robot) */ - virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + * @robot robot The collision model for the robot + * @param state1 The kinematic state at the start of the segment for which checks are being made + * @param state2 The kinematic state at the end of the segment for which checks are being made + * @param acm The allowed collision matrix.*/ + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const = 0; + const AllowedCollisionMatrix& acm) const = 0; - /** \brief Check for collision with a different robot (possibly a different kinematic model as well), in a continuous - * fashion. - * Allowed collisions specified by the allowed collision matrix are taken into account. + /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot + * states). + * Allowed collisions are ignored. Self collisions are not checked. * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result - * @param state1 The kinematic state at the start of the segment for which checks are being made (this robot) - * @param state2 The kinematic state at the end of the segment for which checks are being made (this robot) - * @param other_robot The collision representation for the other robot - * @param other_state1 The kinematic state at the start of the segment for which checks are being made (other robot) - * @param other_state2 The kinematic state at the end of the segment for which checks are being made (other robot) - * @param acm The allowed collision matrix. */ - virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const = 0; + * @robot robot The collision model for the robot + * @param state1 The kinematic state at the start of the segment for which checks are being made + * @param state2 The kinematic state at the end of the segment for which checks are being made + * @param acm The allowed collision matrix.*/ + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const = 0; + + /** \brief The distance to self-collision given the robot is at state \e state. + @param req A DistanceRequest object that encapsulates the distance request + @param res A DistanceResult object that encapsulates the distance result + @param state The state of this robot to consider */ + virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const = 0; /** \brief The distance to self-collision given the robot is at state \e state. */ inline double distanceSelf(const robot_state::RobotState& state) const @@ -185,58 +187,69 @@ class CollisionRobot return res.minimum_distance.distance; } - /** \brief The distance to self-collision given the robot is at state \e state. - @param req A DistanceRequest object that encapsulates the distance request - @param res A DistanceResult object that encapsulates the distance result - @param state The state of this robot to consider */ - virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const = 0; - - /** \brief The distance to another robot instance. - - - @param state The state of this robot to consider - @param other_robot The other robot instance to measure distance to - @param other_state The state of the other robot */ - inline double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const + /** \brief Compute the distance between a robot and the world + * @param req A DistanceRequest object that encapsulates the distance request + * @param res A DistanceResult object that encapsulates the distance result + * @param robot The robot to check distance for + * @param state The state for the robot to check distances from */ + virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const = 0; + + /** \brief Compute the shortest distance between a robot and the world + * @param robot The robot to check distance for + * @param state The state for the robot to check distances from + * @param verbose Output debug information about distance checks */ + inline double distanceRobot(const robot_state::RobotState& state, bool verbose = false) const { DistanceRequest req; DistanceResult res; + req.verbose = verbose; req.enableGroup(getRobotModel()); - distanceOther(req, res, state, other_robot, other_state); + + distanceRobot(req, res, state); return res.minimum_distance.distance; } - /** \brief The distance to another robot instance, ignoring distances between links that are allowed to always - collide. - - - @param state The state of this robot to consider - @param other_robot The other robot instance to measure distance to - @param other_state The state of the other robot - @param acm The collision matrix specifying which links are allowed to always collide */ - inline double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, const AllowedCollisionMatrix& acm) const + /** \brief Compute the shortest distance between a robot and the world + * @param robot The robot to check distance for + * @param state The state for the robot to check distances from + * @param acm Using an allowed collision matrix has the effect of ignoring distances from links that are always + * allowed to be in collision. + * @param verbose Output debug information about distance checks */ + inline double distanceRobot(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, + bool verbose = false) const { DistanceRequest req; DistanceResult res; - req.enableGroup(getRobotModel()); req.acm = &acm; - distanceOther(req, res, state, other_robot, other_state); + req.verbose = verbose; + req.enableGroup(getRobotModel()); + + distanceRobot(req, res, state); return res.minimum_distance.distance; } - /** \brief The distance to self-collision given the robot is at state \e state. - @param req A DistanceRequest object that encapsulates the distance request - @param res A DistanceResult object that encapsulates the distance result - @param state The state of this robot to consider - @param other_robot The other robot instance to measure distance to - @param other_state The state of the other robot */ - virtual void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const = 0; + /** set the world to use. + * This can be expensive unless the new and old world are empty. + * Passing NULL will result in a new empty world being created. */ + virtual void setWorld(const WorldPtr& world); + + /** access the world geometry */ + const WorldPtr& getWorld() + { + return world_; + } + + /** access the world geometry */ + const WorldConstPtr& getWorld() const + { + return world_const_; + } + + typedef World::ObjectPtr ObjectPtr; + typedef World::ObjectConstPtr ObjectConstPtr; /** @brief The kinematic model corresponding to this collision model*/ const robot_model::RobotModelConstPtr& getRobotModel() const @@ -306,5 +319,9 @@ class CollisionRobot /** @brief The internally maintained map (from link names to scaling)*/ std::map link_scale_; + +private: + WorldPtr world_; // The world always valid, never nullptr. + WorldConstPtr world_const_; // always same as world_ }; } diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h index 1460b80ff0..ed36a81113 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h @@ -37,7 +37,7 @@ #pragma once #include -#include +#include namespace collision_detection { diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h index 5142e41bb9..320265eb6b 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h @@ -35,8 +35,7 @@ #pragma once #include -#include -#include +#include #include namespace collision_detection @@ -52,8 +51,7 @@ MOVEIT_CLASS_FORWARD(CollisionPlugin); * { * * class MyCollisionDetectorAllocator : - * public collision_detection::CollisionDetectorAllocatorTemplate + * public collision_detection::CollisionDetectorAllocatorTemplate * { * public: * static const std::string NAME_; diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_world.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_world.h deleted file mode 100644 index 717e72ec09..0000000000 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_world.h +++ /dev/null @@ -1,279 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * 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. - *********************************************************************/ - -/* Author: Ioan Sucan, Sachin Chitta */ - -#pragma once - -#include - -#include -#include -#include -#include - -/** \brief Generic interface to collision detection */ -namespace collision_detection -{ -MOVEIT_CLASS_FORWARD(CollisionWorld); - -/** \brief Perform collision checking with the environment. The - * collision world maintains a representation of the environment - * that the robot is operating in. */ -class CollisionWorld : private boost::noncopyable -{ -public: - CollisionWorld(); - - explicit CollisionWorld(const WorldPtr& world); - - /** \brief A copy constructor. \e other should not be changed while the copy constructor is running. - * world must be the same world as used by other or a (not-yet-modified) copy of the world used by other */ - CollisionWorld(const CollisionWorld& other, const WorldPtr& world); - - virtual ~CollisionWorld() - { - } - - /**********************************************************************/ - /* Collision Checking Routines */ - /**********************************************************************/ - - /** \brief Check whether the robot model is in collision with itself or the world at a particular state. - * Any collision between any pair of links is checked for, NO collisions are ignored. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @param state The kinematic state for which checks are being made */ - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const; - - /** \brief Check whether the robot model is in collision with itself or the world at a particular state. - * Allowed collisions specified by the allowed collision matrix are taken into account. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @param state The kinematic state for which checks are being made - * @param acm The allowed collision matrix. */ - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; - - /** \brief Check whether the robot model is in collision with itself or the world in a continuous manner - * (between two robot states) - * Any collision between any pair of links is checked for, NO collisions are ignored. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @param state1 The kinematic state at the start of the segment for which checks are being made - * @param state2 The kinematic state at the end of the segment for which checks are being made */ - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2) const; - - /** \brief Check whether the robot model is in collision with itself or the world in a continuous manner - * (between two robot states). - * Allowed collisions specified by the allowed collision matrix are taken into account. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @param state1 The kinematic state at the start of the segment for which checks are being made - * @param state2 The kinematic state at the end of the segment for which checks are being made - * @param acm The allowed collision matrix. */ - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const; - - /** \brief Check whether the robot model is in collision with the world. Any collisions between a robot link - * and the world are considered. Self collisions are not checked. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @robot robot The collision model for the robot - * @param state The kinematic state for which checks are being made - */ - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const = 0; - - /** \brief Check whether the robot model is in collision with the world. - * Allowed collisions are ignored. Self collisions are not checked. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @robot robot The collision model for the robot - * @param state The kinematic state for which checks are being made - * @param acm The allowed collision matrix.*/ - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; - - /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot - * states). - * Any collisions between a robot link and the world are considered. Self collisions are not checked. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @robot robot The collision model for the robot - * @param state1 The kinematic state at the start of the segment for which checks are being made - * @param state2 The kinematic state at the end of the segment for which checks are being made */ - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const = 0; - - /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot - * states). - * Allowed collisions are ignored. Self collisions are not checked. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @robot robot The collision model for the robot - * @param state1 The kinematic state at the start of the segment for which checks are being made - * @param state2 The kinematic state at the end of the segment for which checks are being made - * @param acm The allowed collision matrix.*/ - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const = 0; - - /** \brief Check whether a given set of objects is in collision with objects from another world. - * Any contacts are considered. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @param other_world The other collision world - */ - virtual void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const = 0; - - /** \brief Check whether a given set of objects is in collision with objects from another world. - * Allowed collisions are ignored. Any contacts are considered. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @param other_world The other collision world - * @param acm The allowed collision matrix.*/ - virtual void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, - const AllowedCollisionMatrix& acm) const = 0; - - /** \brief Compute the shortest distance between a robot and the world - * @param robot The robot to check distance for - * @param state The state for the robot to check distances from - * @param verbose Output debug information about distance checks */ - inline double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state, - bool verbose = false) const - { - DistanceRequest req; - DistanceResult res; - - req.verbose = verbose; - req.enableGroup(robot.getRobotModel()); - - distanceRobot(req, res, robot, state); - return res.minimum_distance.distance; - } - - /** \brief Compute the shortest distance between a robot and the world - * @param robot The robot to check distance for - * @param state The state for the robot to check distances from - * @param acm Using an allowed collision matrix has the effect of ignoring distances from links that are always - * allowed to be in collision. - * @param verbose Output debug information about distance checks */ - inline double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm, bool verbose = false) const - { - DistanceRequest req; - DistanceResult res; - - req.acm = &acm; - req.verbose = verbose; - req.enableGroup(robot.getRobotModel()); - - distanceRobot(req, res, robot, state); - return res.minimum_distance.distance; - } - - /** \brief Compute the distance between a robot and the world - * @param req A DistanceRequest object that encapsulates the distance request - * @param res A DistanceResult object that encapsulates the distance result - * @param robot The robot to check distance for - * @param state The state for the robot to check distances from */ - virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const = 0; - - /** \brief The shortest distance to another world instance (\e world) - * @param verbose Output debug information about distance checks */ - inline double distanceWorld(const CollisionWorld& world, bool verbose = false) const - { - DistanceRequest req; - DistanceResult res; - - req.verbose = verbose; - distanceWorld(req, res, world); - - return res.minimum_distance.distance; - } - - /** \brief The shortest distance to another world instance (\e world), ignoring the distances between world elements - * that are allowed to collide (as specified by \e acm) - * @param verbose Output debug information about distance checks */ - inline double distanceWorld(const CollisionWorld& world, const AllowedCollisionMatrix& acm, - bool verbose = false) const - { - DistanceRequest req; - DistanceResult res; - - req.acm = &acm; - req.verbose = verbose; - distanceWorld(req, res, world); - - return res.minimum_distance.distance; - } - - /** \brief Compute the distance between another world - * @param req A DistanceRequest object that encapsulates the distance request - * @param res A DistanceResult object that encapsulates the distance result - * @param world The world to check distance for */ - virtual void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const = 0; - - /** set the world to use. - * This can be expensive unless the new and old world are empty. - * Passing NULL will result in a new empty world being created. */ - virtual void setWorld(const WorldPtr& world); - - /** access the world geometry */ - const WorldPtr& getWorld() - { - return world_; - } - - /** access the world geometry */ - const WorldConstPtr& getWorld() const - { - return world_const_; - } - - typedef World::ObjectPtr ObjectPtr; - typedef World::ObjectConstPtr ObjectConstPtr; - -private: - WorldPtr world_; // The world. Always valid. Never NULL. - WorldConstPtr world_const_; // always same as world_ -}; -} diff --git a/moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp similarity index 53% rename from moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp rename to moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp index 023f308a62..bf84110b23 100644 --- a/moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp +++ b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp @@ -32,118 +32,104 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Jia Pan, Jens Petit */ -#include +#include +#include -collision_detection::CollisionWorldAllValid::CollisionWorldAllValid() : CollisionWorld() -{ -} +const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME("ALL_VALID"); -collision_detection::CollisionWorldAllValid::CollisionWorldAllValid(const WorldPtr& world) : CollisionWorld(world) +collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const robot_model::RobotModelConstPtr& robot_model, + double padding, double scale) + : CollisionEnv(robot_model, padding, scale) { } -collision_detection::CollisionWorldAllValid::CollisionWorldAllValid(const CollisionWorld& other, const WorldPtr& world) - : CollisionWorld(other, world) +collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const robot_model::RobotModelConstPtr& robot_model, + const WorldPtr& world, double padding, double scale) + : CollisionEnv(robot_model, world, padding, scale) { } -void collision_detection::CollisionWorldAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state) const +collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world) + : CollisionEnv(other, world) { - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionWorldAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const +void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const { res.collision = false; if (req.verbose) ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionWorldAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const +void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const { res.collision = false; if (req.verbose) ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionWorldAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const +void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const { res.collision = false; if (req.verbose) ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionWorldAllValid::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const +void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const { res.collision = false; if (req.verbose) ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionWorldAllValid::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world, - const AllowedCollisionMatrix& acm) const +void collision_detection::CollisionEnvAllValid::distanceRobot(const collision_detection::DistanceRequest& req, + collision_detection::DistanceResult& res, + const moveit::core::RobotState& state) const { res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -double collision_detection::CollisionWorldAllValid::distanceRobot(const CollisionRobot& robot, - const robot_state::RobotState& state) const +double collision_detection::CollisionEnvAllValid::distanceRobot(const robot_state::RobotState& state) const { return 0.0; } -double collision_detection::CollisionWorldAllValid::distanceRobot(const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const +double collision_detection::CollisionEnvAllValid::distanceRobot(const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const { return 0.0; } -void collision_detection::CollisionWorldAllValid::distanceRobot(const collision_detection::DistanceRequest& req, - collision_detection::DistanceResult& res, - const collision_detection::CollisionRobot& robot, - const moveit::core::RobotState& state) const +void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const { res.collision = false; + if (req.verbose) + ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -double collision_detection::CollisionWorldAllValid::distanceWorld(const CollisionWorld& world) const -{ - return 0.0; -} - -double collision_detection::CollisionWorldAllValid::distanceWorld(const CollisionWorld& world, - const AllowedCollisionMatrix& acm) const +void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const { - return 0.0; + res.collision = false; + if (req.verbose) + ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionWorldAllValid::distanceWorld(const collision_detection::DistanceRequest& req, - collision_detection::DistanceResult& res, - const collision_detection::CollisionWorld& world) const +void collision_detection::CollisionEnvAllValid::distanceSelf(const collision_detection::DistanceRequest& req, + collision_detection::DistanceResult& res, + const moveit::core::RobotState& state) const { res.collision = false; } - -#include -const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME("ALL_VALID"); diff --git a/moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp deleted file mode 100644 index 963f374711..0000000000 --- a/moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp +++ /dev/null @@ -1,171 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * 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. - *********************************************************************/ - -/* Author: Ioan Sucan, Jia Pan */ - -#include - -collision_detection::CollisionRobotAllValid::CollisionRobotAllValid(const robot_model::RobotModelConstPtr& robot_model, - double padding, double scale) - : CollisionRobot(robot_model, padding, scale) -{ -} - -collision_detection::CollisionRobotAllValid::CollisionRobotAllValid(const CollisionRobot& other) : CollisionRobot(other) -{ -} - -void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -double collision_detection::CollisionRobotAllValid::distanceSelf(const robot_state::RobotState& state) const -{ - return 0.0; -} - -double collision_detection::CollisionRobotAllValid::distanceSelf(const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - return 0.0; -} - -void collision_detection::CollisionRobotAllValid::distanceSelf(const collision_detection::DistanceRequest& req, - collision_detection::DistanceResult& res, - const moveit::core::RobotState& state) const -{ - res.collision = false; -} - -double collision_detection::CollisionRobotAllValid::distanceOther(const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - return 0.0; -} - -double collision_detection::CollisionRobotAllValid::distanceOther(const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const -{ - return 0.0; -} - -void collision_detection::CollisionRobotAllValid::distanceOther(const collision_detection::DistanceRequest& req, - collision_detection::DistanceResult& res, - const moveit::core::RobotState& state, - const collision_detection::CollisionRobot& other_robot, - const moveit::core::RobotState& other_state) const -{ - res.collision = false; -} diff --git a/moveit_core/collision_detection/src/collision_robot.cpp b/moveit_core/collision_detection/src/collision_env.cpp similarity index 66% rename from moveit_core/collision_detection/src/collision_robot.cpp rename to moveit_core/collision_detection/src/collision_env.cpp index c3e27751a6..131b25676c 100644 --- a/moveit_core/collision_detection/src/collision_robot.cpp +++ b/moveit_core/collision_detection/src/collision_env.cpp @@ -14,7 +14,7 @@ * 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 + * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -32,9 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Jens Petit */ -#include +#include #include static inline bool validateScale(double scale) @@ -69,8 +69,8 @@ static inline bool validatePadding(double padding) namespace collision_detection { -CollisionRobot::CollisionRobot(const robot_model::RobotModelConstPtr& model, double padding, double scale) - : robot_model_(model) +CollisionEnv::CollisionEnv(const robot_model::RobotModelConstPtr& model, double padding, double scale) + : robot_model_(model), world_(new World()), world_const_(world_) { if (!validateScale(scale)) scale = 1.0; @@ -85,13 +85,30 @@ CollisionRobot::CollisionRobot(const robot_model::RobotModelConstPtr& model, dou } } -CollisionRobot::CollisionRobot(const CollisionRobot& other) : robot_model_(other.robot_model_) +CollisionEnv::CollisionEnv(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding, + double scale) + : robot_model_(model), world_(world), world_const_(world_) +{ + if (!validateScale(scale)) + scale = 1.0; + if (!validatePadding(padding)) + padding = 0.0; + + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + for (auto link : links) + { + link_padding_[link->getName()] = padding; + link_scale_[link->getName()] = scale; + } +} + +CollisionEnv::CollisionEnv(const CollisionEnv& other, const WorldPtr& world) + : robot_model_(other.robot_model_), world_(world), world_const_(world) { link_padding_ = other.link_padding_; link_scale_ = other.link_scale_; } - -void CollisionRobot::setPadding(double padding) +void CollisionEnv::setPadding(double padding) { if (!validatePadding(padding)) return; @@ -107,7 +124,7 @@ void CollisionRobot::setPadding(double padding) updatedPaddingOrScaling(u); } -void CollisionRobot::setScale(double scale) +void CollisionEnv::setScale(double scale) { if (!validateScale(scale)) return; @@ -123,8 +140,9 @@ void CollisionRobot::setScale(double scale) updatedPaddingOrScaling(u); } -void CollisionRobot::setLinkPadding(const std::string& link_name, double padding) +void CollisionEnv::setLinkPadding(const std::string& link_name, double padding) { + validatePadding(padding); bool update = getLinkPadding(link_name) != padding; link_padding_[link_name] = padding; if (update) @@ -134,7 +152,7 @@ void CollisionRobot::setLinkPadding(const std::string& link_name, double padding } } -double CollisionRobot::getLinkPadding(const std::string& link_name) const +double CollisionEnv::getLinkPadding(const std::string& link_name) const { auto it = link_padding_.find(link_name); if (it != link_padding_.end()) @@ -143,11 +161,12 @@ double CollisionRobot::getLinkPadding(const std::string& link_name) const return 0.0; } -void CollisionRobot::setLinkPadding(const std::map& padding) +void CollisionEnv::setLinkPadding(const std::map& padding) { std::vector u; for (const auto& link_pad_pair : padding) { + validatePadding(link_pad_pair.second); bool update = getLinkPadding(link_pad_pair.first) != link_pad_pair.second; link_padding_[link_pad_pair.first] = link_pad_pair.second; if (update) @@ -157,13 +176,14 @@ void CollisionRobot::setLinkPadding(const std::map& padding updatedPaddingOrScaling(u); } -const std::map& CollisionRobot::getLinkPadding() const +const std::map& CollisionEnv::getLinkPadding() const { return link_padding_; } -void CollisionRobot::setLinkScale(const std::string& link_name, double scale) +void CollisionEnv::setLinkScale(const std::string& link_name, double scale) { + validateScale(scale); bool update = getLinkScale(link_name) != scale; link_scale_[link_name] = scale; if (update) @@ -173,7 +193,7 @@ void CollisionRobot::setLinkScale(const std::string& link_name, double scale) } } -double CollisionRobot::getLinkScale(const std::string& link_name) const +double CollisionEnv::getLinkScale(const std::string& link_name) const { auto it = link_scale_.find(link_name); if (it != link_scale_.end()) @@ -182,7 +202,7 @@ double CollisionRobot::getLinkScale(const std::string& link_name) const return 1.0; } -void CollisionRobot::setLinkScale(const std::map& scale) +void CollisionEnv::setLinkScale(const std::map& scale) { std::vector u; for (const auto& link_scale_pair : scale) @@ -196,16 +216,17 @@ void CollisionRobot::setLinkScale(const std::map& scale) updatedPaddingOrScaling(u); } -const std::map& CollisionRobot::getLinkScale() const +const std::map& CollisionEnv::getLinkScale() const { return link_scale_; } -void CollisionRobot::setPadding(const std::vector& padding) +void CollisionEnv::setPadding(const std::vector& padding) { std::vector u; for (const auto& p : padding) { + validatePadding(p.padding); bool update = getLinkPadding(p.link_name) != p.padding; link_padding_[p.link_name] = p.padding; if (update) @@ -215,11 +236,12 @@ void CollisionRobot::setPadding(const std::vector& pad updatedPaddingOrScaling(u); } -void CollisionRobot::setScale(const std::vector& scale) +void CollisionEnv::setScale(const std::vector& scale) { std::vector u; for (const auto& s : scale) { + validateScale(s.scale); bool update = getLinkScale(s.link_name) != s.scale; link_scale_[s.link_name] = s.scale; if (update) @@ -229,7 +251,7 @@ void CollisionRobot::setScale(const std::vector& scale) updatedPaddingOrScaling(u); } -void CollisionRobot::getPadding(std::vector& padding) const +void CollisionEnv::getPadding(std::vector& padding) const { padding.clear(); for (const auto& lp : link_padding_) @@ -241,7 +263,7 @@ void CollisionRobot::getPadding(std::vector& padding) } } -void CollisionRobot::getScale(std::vector& scale) const +void CollisionEnv::getScale(std::vector& scale) const { scale.clear(); for (const auto& ls : link_scale_) @@ -253,8 +275,33 @@ void CollisionRobot::getScale(std::vector& scale) const } } -void CollisionRobot::updatedPaddingOrScaling(const std::vector& links) +void CollisionEnv::updatedPaddingOrScaling(const std::vector& links) +{ +} + +void CollisionEnv::setWorld(const WorldPtr& world) +{ + world_ = world; + if (!world_) + world_.reset(new World()); + + world_const_ = world; +} + +void CollisionEnv::checkCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const +{ + checkSelfCollision(req, res, state); + if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) + checkRobotCollision(req, res, state); +} + +void CollisionEnv::checkCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const { + checkSelfCollision(req, res, state, acm); + if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) + checkRobotCollision(req, res, state, acm); } } // end of namespace collision_detection diff --git a/moveit_core/collision_detection/src/collision_octomap_filter.cpp b/moveit_core/collision_detection/src/collision_octomap_filter.cpp index 5305727973..051a7afd25 100644 --- a/moveit_core/collision_detection/src/collision_octomap_filter.cpp +++ b/moveit_core/collision_detection/src/collision_octomap_filter.cpp @@ -36,7 +36,6 @@ #include #include -#include #include #include #include diff --git a/moveit_core/collision_detection/src/collision_world.cpp b/moveit_core/collision_detection/src/collision_world.cpp deleted file mode 100644 index ec2f3abcf9..0000000000 --- a/moveit_core/collision_detection/src/collision_world.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * 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. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include - -namespace collision_detection -{ -CollisionWorld::CollisionWorld() : world_(new World()), world_const_(world_) -{ -} - -CollisionWorld::CollisionWorld(const WorldPtr& world) : world_(world), world_const_(world) -{ -} - -CollisionWorld::CollisionWorld(const CollisionWorld& other, const WorldPtr& world) : world_(world), world_const_(world) -{ -} - -void CollisionWorld::checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - robot.checkSelfCollision(req, res, state); - if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) - checkRobotCollision(req, res, robot, state); -} - -void CollisionWorld::checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const -{ - robot.checkSelfCollision(req, res, state, acm); - if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) - checkRobotCollision(req, res, robot, state, acm); -} - -void CollisionWorld::checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2) const -{ - robot.checkSelfCollision(req, res, state1, state2); - if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) - checkRobotCollision(req, res, robot, state1, state2); -} - -void CollisionWorld::checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - robot.checkSelfCollision(req, res, state1, state2, acm); - if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) - checkRobotCollision(req, res, robot, state1, state2, acm); -} - -void CollisionWorld::setWorld(const WorldPtr& world) -{ - world_ = world; - if (!world_) - world_.reset(new World()); - - world_const_ = world; -} - -} // end of namespace collision_detection \ No newline at end of file diff --git a/moveit_core/collision_detection/test/test_all_valid.cpp b/moveit_core/collision_detection/test/test_all_valid.cpp index 47a33a81cf..435f473980 100644 --- a/moveit_core/collision_detection/test/test_all_valid.cpp +++ b/moveit_core/collision_detection/test/test_all_valid.cpp @@ -36,14 +36,12 @@ #include #include -#include -#include +#include TEST(AllValid, Instantiate) { using namespace collision_detection; CollisionDetectorAllocatorAllValid allocator; - CollisionWorldAllValid world; urdf::ModelInterfaceSharedPtr urdf_model; urdf_model.reset(new urdf::ModelInterface()); @@ -51,11 +49,11 @@ TEST(AllValid, Instantiate) srdf_model.reset(new srdf::Model()); robot_model::RobotModelConstPtr robot_model; robot_model.reset(new robot_model::RobotModel(urdf_model, srdf_model)); - CollisionRobotAllValid robot(robot_model); + CollisionEnvAllValid env(robot_model); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index 74149b6552..939f958d32 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -2,8 +2,7 @@ set(MOVEIT_LIB_NAME moveit_collision_detection_fcl) add_library(${MOVEIT_LIB_NAME} src/collision_common.cpp - src/collision_robot_fcl.cpp - src/collision_world_fcl.cpp + src/collision_env_fcl.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -30,4 +29,7 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection.cpp) target_link_libraries(test_fcl_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + + catkin_add_gtest(test_fcl_collision_env test/test_fcl_env.cpp) + target_link_libraries(test_fcl_collision_env moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) endif() diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index 347a9426d1..87fd375580 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -37,7 +37,7 @@ #pragma once #include -#include +#include #include #include diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h index a72b4fd5d6..88bcfcddae 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h @@ -37,16 +37,15 @@ #pragma once #include -#include -#include +#include namespace collision_detection { /** \brief An allocator for FCL collision detectors */ class CollisionDetectorAllocatorFCL - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_world_fcl.cpp + static const std::string NAME; // defined in collision_env_fcl.cpp }; } diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h new file mode 100644 index 0000000000..b2819a6147 --- /dev/null +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h @@ -0,0 +1,157 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Ioan Sucan, Jens Petit */ + +#pragma once + +#include +#include + +#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) +#include +#else +#include +#endif + +#include + +namespace collision_detection +{ +/** \brief FCL implementation of the CollisionEnv */ +class CollisionEnvFCL : public CollisionEnv +{ +public: + CollisionEnvFCL() = delete; + + CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); + + CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, + double scale = 1.0); + + CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& world); + + ~CollisionEnvFCL() override; + + virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const override; + + virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const override; + + virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const override; + + virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const override; + + void setWorld(const WorldPtr& world) override; + +protected: + /** \brief Updates the collision objects saved in the manager to reflect a new padding or scaling of the robot links + */ + void updatedPaddingOrScaling(const std::vector& links) override; + + /** \brief Bundles the different checkSelfCollision functions into a single function */ + void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const; + + void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; + + /** \brief Construct an FCL collision object from MoveIt's World::Object. */ + void constructFCLObjectWorld(const World::Object* obj, FCLObject& fcl_obj) const; + + /** \brief Updates the specified object in \m fcl_objs_ and in the manager from new data available in the World. + * + * If it does not exist in world, it is deleted. If it's not existing in \m fcl_objs_ yet, it's added there. */ + void updateFCLObject(const std::string& id); + + /** \brief Out of the current robot state and its attached bodies construct an FCLObject which can then be used to + * check for collision. + * + * The current state is used to recalculate the AABB of the FCL collision objects. However they are not computed from + * scratch (which would require call to computeLocalAABB()) but are only transformed according to the joint states. + * + * \param state The current robot state + * \param fcl_obj The newly filled object */ + void constructFCLObjectRobot(const robot_state::RobotState& state, FCLObject& fcl_obj) const; + + /** \brief Prepares for the collision check through constructing an FCL collision object out of the current robot + * state and specifying a broadphase collision manager of FCL where the constructed object is registered to. */ + void allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const; + + /** \brief Converts all shapes which make up an atttached body into a vector of FCLGeometryConstPtr. + * + * When they are converted, they can be added to the FCL representation of the robot for collision checking. + * + * \param ab Pointer to the attached body + * \param geoms Output vector of geometries + */ + void getAttachedBodyObjects(const robot_state::AttachedBody* ab, std::vector& geoms) const; + + /** \brief Vector of shared pointers to the FCL geometry for the objects in fcl_objs_. */ + std::vector robot_geoms_; + + /** \brief Vector of shared pointers to the FCL collision objects which make up the robot */ + std::vector robot_fcl_objs_; + + /// FCL collision manager which handles the collision checking process + std::unique_ptr manager_; + + std::map fcl_objs_; + +private: + /** \brief Callback function executed for each change to the world environment */ + void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); + + World::ObserverHandle observer_handle_; +}; +} diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h deleted file mode 100644 index 917658bab3..0000000000 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h +++ /dev/null @@ -1,97 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * 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. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#pragma once - -#include - -namespace collision_detection -{ -class CollisionRobotFCL : public CollisionRobot -{ - friend class CollisionWorldFCL; - -public: - CollisionRobotFCL(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); - - CollisionRobotFCL(const CollisionRobotFCL& other); - - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const override; - - void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override; - - void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override; - -protected: - void updatedPaddingOrScaling(const std::vector& links) override; - void constructFCLObject(const robot_state::RobotState& state, FCLObject& fcl_obj) const; - void allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const; - void getAttachedBodyObjects(const robot_state::AttachedBody* ab, std::vector& geoms) const; - - void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const; - void checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, const AllowedCollisionMatrix* acm) const; - - std::vector geoms_; - std::vector fcl_objs_; -}; -} diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h deleted file mode 100644 index 65cc476471..0000000000 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h +++ /dev/null @@ -1,98 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * 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. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#pragma once - -#include -#include - -#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) -#include -#else -#include -#endif - -#include - -namespace collision_detection -{ -class CollisionWorldFCL : public CollisionWorld -{ -public: - CollisionWorldFCL(); - explicit CollisionWorldFCL(const WorldPtr& world); - CollisionWorldFCL(const CollisionWorldFCL& other, const WorldPtr& world); - ~CollisionWorldFCL() override; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const override; - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const override; - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, - const AllowedCollisionMatrix& acm) const override; - - void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override; - - void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override; - - void setWorld(const WorldPtr& world) override; - -protected: - void checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, - const AllowedCollisionMatrix* acm) const; - void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; - - void constructFCLObject(const World::Object* obj, FCLObject& fcl_obj) const; - void updateFCLObject(const std::string& id); - - std::unique_ptr manager_; - std::map fcl_objs_; - -private: - void initialize(); - void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); - World::ObserverHandle observer_handle_; -}; -} diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp new file mode 100644 index 0000000000..20ef171fbc --- /dev/null +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -0,0 +1,434 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Ioan Sucan, Jens Petit */ + +#include +#include +#include + +#include + +#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) +#include +#endif + +namespace collision_detection +{ +const std::string CollisionDetectorAllocatorFCL::NAME("FCL"); + +CollisionEnvFCL::CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, double padding, double scale) + : CollisionEnv(model, padding, scale) +{ + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + std::size_t index; + robot_geoms_.resize(robot_model_->getLinkGeometryCount()); + robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount()); + // we keep the same order of objects as what RobotState *::getLinkState() returns + for (auto link : links) + for (std::size_t j = 0; j < link->getShapes().size(); ++j) + { + FCLGeometryConstPtr link_geometry = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()), + getLinkPadding(link->getName()), link, j); + if (link_geometry) + { + index = link->getFirstCollisionBodyTransformIndex() + j; + robot_geoms_[index] = link_geometry; + + // Need to store the FCL object so the AABB does not get recreated every time. + // Every time this object is created, g->computeLocalAABB() is called which is + // very expensive and should only be calculated once. To update the AABB, use the + // collObj->setTransform and then call collObj->computeAABB() to transform the AABB. + robot_fcl_objs_[index] = + FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(link_geometry->collision_geometry_)); + } + else + ROS_ERROR_NAMED("collision_detection.fcl", "Unable to construct collision geometry for link '%s'", + link->getName().c_str()); + } + + auto m = new fcl::DynamicAABBTreeCollisionManagerd(); + // m->tree_init_level = 2; + manager_.reset(m); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); +} + +CollisionEnvFCL::CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding, + double scale) + : CollisionEnv(model, world, padding, scale) +{ + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + std::size_t index; + robot_geoms_.resize(robot_model_->getLinkGeometryCount()); + robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount()); + // we keep the same order of objects as what RobotState *::getLinkState() returns + for (auto link : links) + for (std::size_t j = 0; j < link->getShapes().size(); ++j) + { + FCLGeometryConstPtr g = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()), + getLinkPadding(link->getName()), link, j); + if (g) + { + index = link->getFirstCollisionBodyTransformIndex() + j; + robot_geoms_[index] = g; + + // Need to store the FCL object so the AABB does not get recreated every time. + // Every time this object is created, g->computeLocalAABB() is called which is + // very expensive and should only be calculated once. To update the AABB, use the + // collObj->setTransform and then call collObj->computeAABB() to transform the AABB. + robot_fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_)); + } + else + ROS_ERROR_NAMED("collision_detection.fcl", "Unable to construct collision geometry for link '%s'", + link->getName().c_str()); + } + + auto m = new fcl::DynamicAABBTreeCollisionManagerd(); + // m->tree_init_level = 2; + manager_.reset(m); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +CollisionEnvFCL::~CollisionEnvFCL() +{ + getWorld()->removeObserver(observer_handle_); +} + +CollisionEnvFCL::CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& world) : CollisionEnv(other, world) +{ + robot_geoms_ = other.robot_geoms_; + robot_fcl_objs_ = other.robot_fcl_objs_; + + auto m = new fcl::DynamicAABBTreeCollisionManagerd(); + // m->tree_init_level = 2; + manager_.reset(m); + + fcl_objs_ = other.fcl_objs_; + for (auto& fcl_obj : fcl_objs_) + fcl_obj.second.registerTo(manager_.get()); + // manager_->update(); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); +} + +void CollisionEnvFCL::getAttachedBodyObjects(const robot_state::AttachedBody* ab, + std::vector& geoms) const +{ + const std::vector& shapes = ab->getShapes(); + for (std::size_t i = 0; i < shapes.size(); ++i) + { + FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], ab, i); + if (co) + geoms.push_back(co); + } +} + +void CollisionEnvFCL::constructFCLObjectWorld(const World::Object* obj, FCLObject& fcl_obj) const +{ + for (std::size_t i = 0; i < obj->shapes_.size(); ++i) + { + FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj); + if (g) + { + auto co = new fcl::CollisionObjectd(g->collision_geometry_, transform2fcl(obj->shape_poses_[i])); + fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(co)); + fcl_obj.collision_geometry_.push_back(g); + } + } +} + +void CollisionEnvFCL::constructFCLObjectRobot(const robot_state::RobotState& state, FCLObject& fcl_obj) const +{ + fcl_obj.collision_objects_.reserve(robot_geoms_.size()); + fcl::Transform3d fcl_tf; + + for (std::size_t i = 0; i < robot_geoms_.size(); ++i) + if (robot_geoms_[i] && robot_geoms_[i]->collision_geometry_) + { + transform2fcl(state.getCollisionBodyTransform(robot_geoms_[i]->collision_geometry_data_->ptr.link, + robot_geoms_[i]->collision_geometry_data_->shape_index), + fcl_tf); + auto coll_obj = new fcl::CollisionObjectd(*robot_fcl_objs_[i]); + coll_obj->setTransform(fcl_tf); + coll_obj->computeAABB(); + fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(coll_obj)); + } + + // TODO: Implement a method for caching fcl::CollisionObject's for robot_state::AttachedBody's + std::vector ab; + state.getAttachedBodies(ab); + for (auto& body : ab) + { + std::vector objs; + getAttachedBodyObjects(body, objs); + const EigenSTL::vector_Isometry3d& ab_t = body->getGlobalCollisionBodyTransforms(); + for (std::size_t k = 0; k < objs.size(); ++k) + if (objs[k]->collision_geometry_) + { + transform2fcl(ab_t[k], fcl_tf); + fcl_obj.collision_objects_.push_back( + FCLCollisionObjectPtr(new fcl::CollisionObjectd(objs[k]->collision_geometry_, fcl_tf))); + // we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself, + // and would be destroyed when objs goes out of scope. + fcl_obj.collision_geometry_.push_back(objs[k]); + } + } +} + +void CollisionEnvFCL::allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const +{ + auto m = new fcl::DynamicAABBTreeCollisionManagerd(); + // m->tree_init_level = 2; + manager.manager_.reset(m); + constructFCLObjectRobot(state, manager.object_); + manager.object_.registerTo(manager.manager_.get()); + // manager.manager_->update(); +} + +void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const +{ + checkSelfCollisionHelper(req, res, state, nullptr); +} + +void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const +{ + checkSelfCollisionHelper(req, res, state, &acm); +} + +void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + FCLManager manager; + allocSelfCollisionBroadPhase(state, manager); + CollisionData cd(&req, &res, acm); + cd.enableGroup(getRobotModel()); + manager.manager_->collide(&cd, &collisionCallback); + if (req.distance) + { + DistanceRequest dreq; + DistanceResult dres; + + dreq.group_name = req.group_name; + dreq.acm = acm; + dreq.enableGroup(getRobotModel()); + distanceSelf(dreq, dres, state); + res.distance = dres.minimum_distance.distance; + } +} + +void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const +{ + checkRobotCollisionHelper(req, res, state, nullptr); +} + +void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const +{ + checkRobotCollisionHelper(req, res, state, &acm); +} + +void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Continuous collision not implemented"); +} + +void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const +{ + ROS_ERROR_NAMED("collision_detection.fcl", "Not implemented"); +} + +void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + FCLObject fcl_obj; + constructFCLObjectRobot(state, fcl_obj); + + CollisionData cd(&req, &res, acm); + cd.enableGroup(getRobotModel()); + for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i) + manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback); + + if (req.distance) + { + DistanceRequest dreq; + DistanceResult dres; + + dreq.group_name = req.group_name; + dreq.acm = acm; + dreq.enableGroup(getRobotModel()); + distanceRobot(dreq, dres, state); + res.distance = dres.minimum_distance.distance; + } +} + +void CollisionEnvFCL::distanceSelf(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const +{ + FCLManager manager; + allocSelfCollisionBroadPhase(state, manager); + DistanceData drd(&req, &res); + + manager.manager_->distance(&drd, &distanceCallback); +} + +void CollisionEnvFCL::distanceRobot(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const +{ + FCLObject fcl_obj; + constructFCLObjectRobot(state, fcl_obj); + + DistanceData drd(&req, &res); + for (std::size_t i = 0; !drd.done && i < fcl_obj.collision_objects_.size(); ++i) + manager_->distance(fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback); +} + +void CollisionEnvFCL::updateFCLObject(const std::string& id) +{ + // remove FCL objects that correspond to this object + auto jt = fcl_objs_.find(id); + if (jt != fcl_objs_.end()) + { + jt->second.unregisterFrom(manager_.get()); + jt->second.clear(); + } + + // check to see if we have this object + auto it = getWorld()->find(id); + if (it != getWorld()->end()) + { + // construct FCL objects that correspond to this object + if (jt != fcl_objs_.end()) + { + constructFCLObjectWorld(it->second.get(), jt->second); + jt->second.registerTo(manager_.get()); + } + else + { + constructFCLObjectWorld(it->second.get(), fcl_objs_[id]); + fcl_objs_[id].registerTo(manager_.get()); + } + } + else + { + if (jt != fcl_objs_.end()) + fcl_objs_.erase(jt); + } + + // manager_->update(); +} + +void CollisionEnvFCL::setWorld(const WorldPtr& world) +{ + if (world == getWorld()) + return; + + // turn off notifications about old world + getWorld()->removeObserver(observer_handle_); + + // clear out objects from old world + manager_->clear(); + fcl_objs_.clear(); + cleanCollisionGeometryCache(); + + CollisionEnv::setWorld(world); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); + + // get notifications any objects already in the new world + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +void CollisionEnvFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Action action) +{ + if (action == World::DESTROY) + { + auto it = fcl_objs_.find(obj->id_); + if (it != fcl_objs_.end()) + { + it->second.unregisterFrom(manager_.get()); + it->second.clear(); + fcl_objs_.erase(it); + } + cleanCollisionGeometryCache(); + } + else + { + updateFCLObject(obj->id_); + if (action & (World::DESTROY | World::REMOVE_SHAPE)) + cleanCollisionGeometryCache(); + } +} + +void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector& links) +{ + std::size_t index; + for (const auto& link : links) + { + const robot_model::LinkModel* lmodel = robot_model_->getLinkModel(link); + if (lmodel) + { + for (std::size_t j = 0; j < lmodel->getShapes().size(); ++j) + { + FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShapes()[j], getLinkScale(lmodel->getName()), + getLinkPadding(lmodel->getName()), lmodel, j); + if (g) + { + index = lmodel->getFirstCollisionBodyTransformIndex() + j; + robot_geoms_[index] = g; + robot_fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_)); + } + } + } + else + ROS_ERROR_NAMED("collision_detection.fcl", "Updating padding or scaling for unknown link: '%s'", link.c_str()); + } +} + +} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp deleted file mode 100644 index 9b8a4cea68..0000000000 --- a/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp +++ /dev/null @@ -1,307 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * 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. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include - -#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) -#include -#endif - -namespace collision_detection -{ -CollisionRobotFCL::CollisionRobotFCL(const robot_model::RobotModelConstPtr& model, double padding, double scale) - : CollisionRobot(model, padding, scale) -{ - const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); - std::size_t index; - geoms_.resize(robot_model_->getLinkGeometryCount()); - fcl_objs_.resize(robot_model_->getLinkGeometryCount()); - // we keep the same order of objects as what RobotState *::getLinkState() returns - for (auto link : links) - for (std::size_t j = 0; j < link->getShapes().size(); ++j) - { - FCLGeometryConstPtr g = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()), - getLinkPadding(link->getName()), link, j); - if (g) - { - index = link->getFirstCollisionBodyTransformIndex() + j; - geoms_[index] = g; - - // Need to store the FCL object so the AABB does not get recreated every time. - // Every time this object is created, g->computeLocalAABB() is called which is - // very expensive and should only be calculated once. To update the AABB, use the - // collObj->setTransform and then call collObj->computeAABB() to transform the AABB. - fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_)); - } - else - ROS_ERROR_NAMED("collision_detection.fcl", "Unable to construct collision geometry for link '%s'", - link->getName().c_str()); - } -} - -CollisionRobotFCL::CollisionRobotFCL(const CollisionRobotFCL& other) : CollisionRobot(other) -{ - geoms_ = other.geoms_; - fcl_objs_ = other.fcl_objs_; -} - -void CollisionRobotFCL::getAttachedBodyObjects(const robot_state::AttachedBody* ab, - std::vector& geoms) const -{ - const std::vector& shapes = ab->getShapes(); - for (std::size_t i = 0; i < shapes.size(); ++i) - { - FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], ab, i); - if (co) - geoms.push_back(co); - } -} - -void CollisionRobotFCL::constructFCLObject(const robot_state::RobotState& state, FCLObject& fcl_obj) const -{ - fcl_obj.collision_objects_.reserve(geoms_.size()); - fcl::Transform3d fcl_tf; - - for (std::size_t i = 0; i < geoms_.size(); ++i) - if (geoms_[i] && geoms_[i]->collision_geometry_) - { - transform2fcl(state.getCollisionBodyTransform(geoms_[i]->collision_geometry_data_->ptr.link, - geoms_[i]->collision_geometry_data_->shape_index), - fcl_tf); - auto coll_obj = new fcl::CollisionObjectd(*fcl_objs_[i]); - coll_obj->setTransform(fcl_tf); - coll_obj->computeAABB(); - fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(coll_obj)); - } - - // TODO: Implement a method for caching fcl::CollisionObject's for robot_state::AttachedBody's - std::vector ab; - state.getAttachedBodies(ab); - for (auto& body : ab) - { - std::vector objs; - getAttachedBodyObjects(body, objs); - const EigenSTL::vector_Isometry3d& ab_t = body->getGlobalCollisionBodyTransforms(); - for (std::size_t k = 0; k < objs.size(); ++k) - if (objs[k]->collision_geometry_) - { - transform2fcl(ab_t[k], fcl_tf); - fcl_obj.collision_objects_.push_back( - FCLCollisionObjectPtr(new fcl::CollisionObjectd(objs[k]->collision_geometry_, fcl_tf))); - // we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself, - // and would be destroyed when objs goes out of scope. - fcl_obj.collision_geometry_.push_back(objs[k]); - } - } -} - -void CollisionRobotFCL::allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const -{ - auto m = new fcl::DynamicAABBTreeCollisionManagerd(); - // m->tree_init_level = 2; - manager.manager_.reset(m); - constructFCLObject(state, manager.object_); - manager.object_.registerTo(manager.manager_.get()); - // manager.manager_->update(); -} - -void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const -{ - checkSelfCollisionHelper(req, res, state, nullptr); -} - -void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - checkSelfCollisionHelper(req, res, state, &acm); -} - -void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const -{ - ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented"); -} - -void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented"); -} - -void CollisionRobotFCL::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const -{ - FCLManager manager; - allocSelfCollisionBroadPhase(state, manager); - CollisionData cd(&req, &res, acm); - cd.enableGroup(getRobotModel()); - manager.manager_->collide(&cd, &collisionCallback); - if (req.distance) - { - DistanceRequest dreq; - DistanceResult dres; - - dreq.group_name = req.group_name; - dreq.acm = acm; - dreq.enableGroup(getRobotModel()); - distanceSelf(dreq, dres, state); - res.distance = dres.minimum_distance.distance; - } -} - -void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - checkOtherCollisionHelper(req, res, state, other_robot, other_state, nullptr); -} - -void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const -{ - checkOtherCollisionHelper(req, res, state, other_robot, other_state, &acm); -} - -void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const -{ - ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented"); -} - -void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented"); -} - -void CollisionRobotFCL::checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix* acm) const -{ - FCLManager manager; - allocSelfCollisionBroadPhase(state, manager); - - const CollisionRobotFCL& fcl_rob = dynamic_cast(other_robot); - FCLObject other_fcl_obj; - fcl_rob.constructFCLObject(other_state, other_fcl_obj); - - CollisionData cd(&req, &res, acm); - cd.enableGroup(getRobotModel()); - for (std::size_t i = 0; !cd.done_ && i < other_fcl_obj.collision_objects_.size(); ++i) - manager.manager_->collide(other_fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback); - - if (req.distance) - { - DistanceRequest dreq; - DistanceResult dres; - - dreq.group_name = req.group_name; - dreq.acm = acm; - dreq.enableGroup(getRobotModel()); - distanceOther(dreq, dres, state, other_robot, other_state); - res.distance = dres.minimum_distance.distance; - } -} - -void CollisionRobotFCL::updatedPaddingOrScaling(const std::vector& links) -{ - std::size_t index; - for (const auto& link : links) - { - const robot_model::LinkModel* lmodel = robot_model_->getLinkModel(link); - if (lmodel) - { - for (std::size_t j = 0; j < lmodel->getShapes().size(); ++j) - { - FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShapes()[j], getLinkScale(lmodel->getName()), - getLinkPadding(lmodel->getName()), lmodel, j); - if (g) - { - index = lmodel->getFirstCollisionBodyTransformIndex() + j; - geoms_[index] = g; - fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_)); - } - } - } - else - ROS_ERROR_NAMED("collision_detection.fcl", "Updating padding or scaling for unknown link: '%s'", link.c_str()); - } -} - -void CollisionRobotFCL::distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const -{ - FCLManager manager; - allocSelfCollisionBroadPhase(state, manager); - DistanceData drd(&req, &res); - - manager.manager_->distance(&drd, &distanceCallback); -} - -void CollisionRobotFCL::distanceOther(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - FCLManager manager; - allocSelfCollisionBroadPhase(state, manager); - - const CollisionRobotFCL& fcl_rob = dynamic_cast(other_robot); - FCLObject other_fcl_obj; - fcl_rob.constructFCLObject(other_state, other_fcl_obj); - - DistanceData drd(&req, &res); - for (std::size_t i = 0; !drd.done && i < other_fcl_obj.collision_objects_.size(); ++i) - manager.manager_->distance(other_fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback); -} - -} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp deleted file mode 100644 index 8d6eb392f9..0000000000 --- a/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp +++ /dev/null @@ -1,299 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * 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. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include -#include - -#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) -#include -#include -#include -#include -#else -#include -#include -#include -#include -#endif - -#include - -namespace collision_detection -{ -const std::string CollisionDetectorAllocatorFCL::NAME("FCL"); - -CollisionWorldFCL::CollisionWorldFCL() : CollisionWorld() -{ - auto m = new fcl::DynamicAABBTreeCollisionManagerd(); - // m->tree_init_level = 2; - manager_.reset(m); - - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2)); -} - -CollisionWorldFCL::CollisionWorldFCL(const WorldPtr& world) : CollisionWorld(world) -{ - auto m = new fcl::DynamicAABBTreeCollisionManagerd(); - // m->tree_init_level = 2; - manager_.reset(m); - - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2)); - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -CollisionWorldFCL::CollisionWorldFCL(const CollisionWorldFCL& other, const WorldPtr& world) - : CollisionWorld(other, world) -{ - auto m = new fcl::DynamicAABBTreeCollisionManagerd(); - // m->tree_init_level = 2; - manager_.reset(m); - - fcl_objs_ = other.fcl_objs_; - for (auto& fcl_obj : fcl_objs_) - fcl_obj.second.registerTo(manager_.get()); - // manager_->update(); - - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2)); -} - -CollisionWorldFCL::~CollisionWorldFCL() -{ - getWorld()->removeObserver(observer_handle_); -} - -void CollisionWorldFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state) const -{ - checkRobotCollisionHelper(req, res, robot, state, nullptr); -} - -void CollisionWorldFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - checkRobotCollisionHelper(req, res, robot, state, &acm); -} - -void CollisionWorldFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const -{ - ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented"); -} - -void CollisionWorldFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented"); -} - -void CollisionWorldFCL::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const -{ - const CollisionRobotFCL& robot_fcl = dynamic_cast(robot); - FCLObject fcl_obj; - robot_fcl.constructFCLObject(state, fcl_obj); - - CollisionData cd(&req, &res, acm); - cd.enableGroup(robot.getRobotModel()); - for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i) - manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback); - - if (req.distance) - { - DistanceRequest dreq; - DistanceResult dres; - - dreq.group_name = req.group_name; - dreq.acm = acm; - dreq.enableGroup(robot.getRobotModel()); - distanceRobot(dreq, dres, robot, state); - res.distance = dres.minimum_distance.distance; - } -} - -void CollisionWorldFCL::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const -{ - checkWorldCollisionHelper(req, res, other_world, nullptr); -} - -void CollisionWorldFCL::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world, const AllowedCollisionMatrix& acm) const -{ - checkWorldCollisionHelper(req, res, other_world, &acm); -} - -void CollisionWorldFCL::checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world, - const AllowedCollisionMatrix* acm) const -{ - const CollisionWorldFCL& other_fcl_world = dynamic_cast(other_world); - CollisionData cd(&req, &res, acm); - manager_->collide(other_fcl_world.manager_.get(), &cd, &collisionCallback); - - if (req.distance) - { - DistanceRequest dreq; - DistanceResult dres; - - dreq.group_name = req.group_name; - dreq.acm = acm; - distanceWorld(dreq, dres, other_world); - res.distance = dres.minimum_distance.distance; - } -} - -void CollisionWorldFCL::constructFCLObject(const World::Object* obj, FCLObject& fcl_obj) const -{ - for (std::size_t i = 0; i < obj->shapes_.size(); ++i) - { - FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj); - if (g) - { - auto co = new fcl::CollisionObjectd(g->collision_geometry_, transform2fcl(obj->shape_poses_[i])); - fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(co)); - fcl_obj.collision_geometry_.push_back(g); - } - } -} - -void CollisionWorldFCL::updateFCLObject(const std::string& id) -{ - // remove FCL objects that correspond to this object - auto jt = fcl_objs_.find(id); - if (jt != fcl_objs_.end()) - { - jt->second.unregisterFrom(manager_.get()); - jt->second.clear(); - } - - // check to see if we have this object - auto it = getWorld()->find(id); - if (it != getWorld()->end()) - { - // construct FCL objects that correspond to this object - if (jt != fcl_objs_.end()) - { - constructFCLObject(it->second.get(), jt->second); - jt->second.registerTo(manager_.get()); - } - else - { - constructFCLObject(it->second.get(), fcl_objs_[id]); - fcl_objs_[id].registerTo(manager_.get()); - } - } - else - { - if (jt != fcl_objs_.end()) - fcl_objs_.erase(jt); - } - - // manager_->update(); -} - -void CollisionWorldFCL::setWorld(const WorldPtr& world) -{ - if (world == getWorld()) - return; - - // turn off notifications about old world - getWorld()->removeObserver(observer_handle_); - - // clear out objects from old world - manager_->clear(); - fcl_objs_.clear(); - cleanCollisionGeometryCache(); - - CollisionWorld::setWorld(world); - - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2)); - - // get notifications any objects already in the new world - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -void CollisionWorldFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Action action) -{ - if (action == World::DESTROY) - { - auto it = fcl_objs_.find(obj->id_); - if (it != fcl_objs_.end()) - { - it->second.unregisterFrom(manager_.get()); - it->second.clear(); - fcl_objs_.erase(it); - } - cleanCollisionGeometryCache(); - } - else - { - updateFCLObject(obj->id_); - if (action & (World::DESTROY | World::REMOVE_SHAPE)) - cleanCollisionGeometryCache(); - } -} - -void CollisionWorldFCL::distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - const CollisionRobotFCL& robot_fcl = dynamic_cast(robot); - FCLObject fcl_obj; - robot_fcl.constructFCLObject(state, fcl_obj); - - DistanceData drd(&req, &res); - for (std::size_t i = 0; !drd.done && i < fcl_obj.collision_objects_.size(); ++i) - manager_->distance(fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback); -} - -void CollisionWorldFCL::distanceWorld(const DistanceRequest& req, DistanceResult& res, - const CollisionWorld& world) const -{ - const CollisionWorldFCL& other_fcl_world = dynamic_cast(world); - DistanceData drd(&req, &res); - manager_->distance(other_fcl_world.manager_.get(), &drd, &distanceCallback); -} - -} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp index e606f632a3..e4ea360a95 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp @@ -36,8 +36,7 @@ #include #include -#include -#include +#include #include #include @@ -49,8 +48,7 @@ #include #include -typedef collision_detection::CollisionWorldFCL DefaultCWorldType; -typedef collision_detection::CollisionRobotFCL DefaultCRobotType; +typedef collision_detection::CollisionEnvFCL DefaultEnvType; class FclCollisionDetectionTester : public testing::Test { @@ -63,8 +61,7 @@ class FclCollisionDetectionTester : public testing::Test acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); - crobot_.reset(new DefaultCRobotType(robot_model_)); - cworld_.reset(new DefaultCWorldType()); + c_env_.reset(new DefaultEnvType(robot_model_)); } void TearDown() override @@ -76,8 +73,7 @@ class FclCollisionDetectionTester : public testing::Test robot_model::RobotModelPtr robot_model_; - collision_detection::CollisionRobotPtr crobot_; - collision_detection::CollisionWorldPtr cworld_; + collision_detection::CollisionEnvPtr c_env_; collision_detection::AllowedCollisionMatrixPtr acm_; @@ -97,7 +93,7 @@ TEST_F(FclCollisionDetectionTester, DefaultNotInCollision) collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + c_env_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_FALSE(res.collision); } @@ -124,11 +120,11 @@ TEST_F(FclCollisionDetectionTester, LinksInCollision) robot_state.update(); acm_->setEntry("base_link", "base_bellow_link", false); - crobot_->checkSelfCollision(req, res1, robot_state, *acm_); + c_env_->checkSelfCollision(req, res1, robot_state, *acm_); ASSERT_TRUE(res1.collision); acm_->setEntry("base_link", "base_bellow_link", true); - crobot_->checkSelfCollision(req, res2, robot_state, *acm_); + c_env_->checkSelfCollision(req, res2, robot_state, *acm_); ASSERT_FALSE(res2.collision); // req.verbose = true; @@ -139,7 +135,7 @@ TEST_F(FclCollisionDetectionTester, LinksInCollision) robot_state.update(); acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); - crobot_->checkSelfCollision(req, res3, robot_state, *acm_); + c_env_->checkSelfCollision(req, res3, robot_state, *acm_); ASSERT_TRUE(res3.collision); } @@ -171,7 +167,7 @@ TEST_F(FclCollisionDetectionTester, ContactReporting) acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); collision_detection::CollisionResult res; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + c_env_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); EXPECT_EQ(res.contacts.size(), 1u); EXPECT_EQ(res.contacts.begin()->second.size(), 1u); @@ -180,7 +176,7 @@ TEST_F(FclCollisionDetectionTester, ContactReporting) req.max_contacts = 2; req.max_contacts_per_pair = 1; // req.verbose = true; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + c_env_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); EXPECT_EQ(res.contacts.size(), 2u); EXPECT_EQ(res.contacts.begin()->second.size(), 1u); @@ -191,7 +187,7 @@ TEST_F(FclCollisionDetectionTester, ContactReporting) req.max_contacts = 10; req.max_contacts_per_pair = 2; acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false)); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + c_env_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); EXPECT_LE(res.contacts.size(), 10u); EXPECT_LE(res.contact_count, 10u); @@ -222,7 +218,7 @@ TEST_F(FclCollisionDetectionTester, ContactPositions) acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); collision_detection::CollisionResult res; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + c_env_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); ASSERT_EQ(res.contacts.size(), 1u); ASSERT_EQ(res.contacts.begin()->second.size(), 1u); @@ -242,7 +238,7 @@ TEST_F(FclCollisionDetectionTester, ContactPositions) robot_state.update(); collision_detection::CollisionResult res2; - crobot_->checkSelfCollision(req, res2, robot_state, *acm_); + c_env_->checkSelfCollision(req, res2, robot_state, *acm_); ASSERT_TRUE(res2.collision); ASSERT_EQ(res2.contacts.size(), 1u); ASSERT_EQ(res2.contacts.begin()->second.size(), 1u); @@ -262,7 +258,7 @@ TEST_F(FclCollisionDetectionTester, ContactPositions) robot_state.update(); collision_detection::CollisionResult res3; - crobot_->checkSelfCollision(req, res2, robot_state, *acm_); + c_env_->checkSelfCollision(req, res2, robot_state, *acm_); ASSERT_FALSE(res3.collision); } @@ -283,18 +279,18 @@ TEST_F(FclCollisionDetectionTester, AttachedBodyTester) // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); robot_state.update(); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + c_env_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_FALSE(res.collision); shapes::Shape* shape = new shapes::Box(.1, .1, .1); - cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1); + c_env_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1); res = collision_detection::CollisionResult(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_); + c_env_->checkRobotCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); // deletes shape - cworld_->getWorld()->removeObject("box"); + c_env_->getWorld()->removeObject("box"); shape = new shapes::Box(.1, .1, .1); std::vector shapes; @@ -305,7 +301,7 @@ TEST_F(FclCollisionDetectionTester, AttachedBodyTester) robot_state.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link"); res = collision_detection::CollisionResult(); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + c_env_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); // deletes shape @@ -318,19 +314,19 @@ TEST_F(FclCollisionDetectionTester, AttachedBodyTester) robot_state.update(); res = collision_detection::CollisionResult(); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + c_env_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_FALSE(res.collision); pos1.translation().x() = 5.01; shapes::Shape* coll = new shapes::Box(.1, .1, .1); - cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1); + c_env_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1); res = collision_detection::CollisionResult(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_); + c_env_->checkRobotCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); acm_->setEntry("coll", "r_gripper_palm_link", true); res = collision_detection::CollisionResult(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_); + c_env_->checkRobotCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); } @@ -342,15 +338,15 @@ TEST_F(FclCollisionDetectionTester, DiffSceneTester) collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - - collision_detection::CollisionRobotFCL new_crobot( - *(dynamic_cast(crobot_.get()))); + collision_detection::CollisionEnvFCL* casted_env_pointer = + dynamic_cast(c_env_.get()); + collision_detection::CollisionEnvFCL new_c_env(*casted_env_pointer, c_env_->getWorld()); ros::WallTime before = ros::WallTime::now(); - new_crobot.checkSelfCollision(req, res, robot_state); + new_c_env.checkSelfCollision(req, res, robot_state); double first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); - new_crobot.checkSelfCollision(req, res, robot_state); + new_c_env.checkSelfCollision(req, res, robot_state); double second_check = (ros::WallTime::now() - before).toSec(); EXPECT_LT(fabs(first_check - second_check), .05); @@ -367,22 +363,21 @@ TEST_F(FclCollisionDetectionTester, DiffSceneTester) robot_state.attachBody("kinect", shapes, poses, touch_links, "r_gripper_palm_link"); before = ros::WallTime::now(); - new_crobot.checkSelfCollision(req, res, robot_state); + new_c_env.checkSelfCollision(req, res, robot_state); first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); - new_crobot.checkSelfCollision(req, res, robot_state); + new_c_env.checkSelfCollision(req, res, robot_state); second_check = (ros::WallTime::now() - before).toSec(); // the first check is going to take a while, as data must be constructed EXPECT_LT(second_check, .1); - collision_detection::CollisionRobotFCL other_new_crobot( - *(dynamic_cast(crobot_.get()))); + collision_detection::CollisionEnvFCL other_new_c_env(*casted_env_pointer, c_env_->getWorld()); before = ros::WallTime::now(); - new_crobot.checkSelfCollision(req, res, robot_state); + new_c_env.checkSelfCollision(req, res, robot_state); first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); - new_crobot.checkSelfCollision(req, res, robot_state); + new_c_env.checkSelfCollision(req, res, robot_state); second_check = (ros::WallTime::now() - before).toSec(); EXPECT_LT(fabs(first_check - second_check), .05); @@ -398,23 +393,23 @@ TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached) Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity(); pos2.translation().x() = 10.0; - cworld_->getWorld()->addToObject("kinect", shape, pos1); + c_env_->getWorld()->addToObject("kinect", shape, pos1); robot_state::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); ros::WallTime before = ros::WallTime::now(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state); + c_env_->checkRobotCollision(req, res, robot_state); double first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state); + c_env_->checkRobotCollision(req, res, robot_state); double second_check = (ros::WallTime::now() - before).toSec(); EXPECT_LT(second_check, .05); - collision_detection::CollisionWorld::ObjectConstPtr object = cworld_->getWorld()->getObject("kinect"); - cworld_->getWorld()->removeObject("kinect"); + collision_detection::CollisionEnv::ObjectConstPtr object = c_env_->getWorld()->getObject("kinect"); + c_env_->getWorld()->removeObject("kinect"); robot_state::RobotState robot_state1(robot_model_); robot_state::RobotState robot_state2(robot_model_); @@ -434,17 +429,17 @@ TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached) // going to take a while, but that's fine res = collision_detection::CollisionResult(); - crobot_->checkSelfCollision(req, res, robot_state1); + c_env_->checkSelfCollision(req, res, robot_state1); EXPECT_TRUE(res.collision); before = ros::WallTime::now(); - crobot_->checkSelfCollision(req, res, robot_state1, *acm_); + c_env_->checkSelfCollision(req, res, robot_state1, *acm_); first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); req.verbose = true; res = collision_detection::CollisionResult(); - crobot_->checkSelfCollision(req, res, robot_state2, *acm_); + c_env_->checkSelfCollision(req, res, robot_state2, *acm_); second_check = (ros::WallTime::now() - before).toSec(); EXPECT_LT(first_check, .05); @@ -461,7 +456,7 @@ TEST_F(FclCollisionDetectionTester, TestCollisionMapAdditionSpeed) shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01))); } ros::WallTime start = ros::WallTime::now(); - cworld_->getWorld()->addToObject("map", shapes, poses); + c_env_->getWorld()->addToObject("map", shapes, poses); double t = (ros::WallTime::now() - start).toSec(); EXPECT_GE(1.0, t); // this is not really a failure; it is just that slow; @@ -480,16 +475,16 @@ TEST_F(FclCollisionDetectionTester, MoveMesh) shapes::ShapePtr kinect_shape; kinect_shape.reset(shapes::createMeshFromResource(kinect_dae_resource_)); - cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); + c_env_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); Eigen::Isometry3d np; for (unsigned int i = 0; i < 5; i++) { np = Eigen::Translation3d(i * .001, i * .001, i * .001) * Eigen::Quaterniond::Identity(); - cworld_->getWorld()->moveShapeInObject("kinect", kinect_shape, np); + c_env_->getWorld()->moveShapeInObject("kinect", kinect_shape, np); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - cworld_->checkCollision(req, res, *crobot_, robot_state1, *acm_); + c_env_->checkCollision(req, res, robot_state1, *acm_); } } @@ -508,37 +503,37 @@ TEST_F(FclCollisionDetectionTester, TestChangingShapeSize) std::vector shapes; for (unsigned int i = 0; i < 5; i++) { - cworld_->getWorld()->removeObject("shape"); + c_env_->getWorld()->removeObject("shape"); shapes.clear(); poses.clear(); shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001))); poses.push_back(Eigen::Isometry3d::Identity()); - cworld_->getWorld()->addToObject("shape", shapes, poses); + c_env_->getWorld()->addToObject("shape", shapes, poses); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - cworld_->checkCollision(req, res, *crobot_, robot_state1, *acm_); + c_env_->checkCollision(req, res, robot_state1, *acm_); ASSERT_TRUE(res.collision); } Eigen::Isometry3d kinect_pose = Eigen::Isometry3d::Identity(); shapes::ShapePtr kinect_shape; kinect_shape.reset(shapes::createMeshFromResource(kinect_dae_resource_)); - cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); + c_env_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); collision_detection::CollisionRequest req2; collision_detection::CollisionResult res2; - cworld_->checkCollision(req2, res2, *crobot_, robot_state1, *acm_); + c_env_->checkCollision(req2, res2, robot_state1, *acm_); ASSERT_TRUE(res2.collision); for (unsigned int i = 0; i < 5; i++) { - cworld_->getWorld()->removeObject("shape"); + c_env_->getWorld()->removeObject("shape"); shapes.clear(); poses.clear(); shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001))); poses.push_back(Eigen::Isometry3d::Identity()); - cworld_->getWorld()->addToObject("shape", shapes, poses); + c_env_->getWorld()->addToObject("shape", shapes, poses); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - cworld_->checkCollision(req, res, *crobot_, robot_state1, *acm_); + c_env_->checkCollision(req, res, robot_state1, *acm_); ASSERT_TRUE(res.collision); } } diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp new file mode 100644 index 0000000000..64f65b64c3 --- /dev/null +++ b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp @@ -0,0 +1,324 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include +#include + +/** \brief Brings the panda robot in user defined home position */ +inline void setToHome(robot_state::RobotState& panda_state) +{ + panda_state.setToDefaultValues(); + double joint2 = -0.785; + double joint4 = -2.356; + double joint6 = 1.571; + double joint7 = 0.785; + panda_state.setJointPositions("panda_joint2", &joint2); + panda_state.setJointPositions("panda_joint4", &joint4); + panda_state.setJointPositions("panda_joint6", &joint6); + panda_state.setJointPositions("panda_joint7", &joint7); + panda_state.update(); +} + +class CollisionDetectionEnvTest : public testing::Test +{ +protected: + void SetUp() override + { + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + robot_model_ok_ = static_cast(robot_model_); + + acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false)); + + acm_->setEntry("panda_link0", "panda_link1", true); + acm_->setEntry("panda_link1", "panda_link2", true); + acm_->setEntry("panda_link2", "panda_link3", true); + acm_->setEntry("panda_link3", "panda_link4", true); + acm_->setEntry("panda_link4", "panda_link5", true); + acm_->setEntry("panda_link5", "panda_link6", true); + acm_->setEntry("panda_link6", "panda_link7", true); + acm_->setEntry("panda_link7", "panda_hand", true); + acm_->setEntry("panda_hand", "panda_rightfinger", true); + acm_->setEntry("panda_hand", "panda_leftfinger", true); + acm_->setEntry("panda_rightfinger", "panda_leftfinger", true); + acm_->setEntry("panda_link5", "panda_link7", true); + acm_->setEntry("panda_link6", "panda_hand", true); + + c_env_.reset(new collision_detection::CollisionEnvFCL(robot_model_)); + + robot_state_.reset(new robot_state::RobotState(robot_model_)); + + setToHome(*robot_state_); + } + + void TearDown() override + { + } + +protected: + bool robot_model_ok_; + + robot_model::RobotModelPtr robot_model_; + + collision_detection::CollisionEnvPtr c_env_; + + collision_detection::AllowedCollisionMatrixPtr acm_; + + robot_state::RobotStatePtr robot_state_; +}; + +/** \brief Correct setup testing. */ +TEST_F(CollisionDetectionEnvTest, InitOK) +{ + ASSERT_TRUE(robot_model_ok_); +} + +/** \brief Tests the default values specified in the SRDF if they are collision free. */ +TEST_F(CollisionDetectionEnvTest, DefaultNotInCollision) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + c_env_->checkSelfCollision(req, res, *robot_state_, *acm_); + ASSERT_FALSE(res.collision); +} + +/** \brief A configuration where the robot should collide with itself. */ +TEST_F(CollisionDetectionEnvTest, LinksInCollision) +{ + // Sets the joint values to zero which is a colliding configuration + robot_state_->setToDefaultValues(); + robot_state_->update(); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + c_env_->checkSelfCollision(req, res, *robot_state_, *acm_); + ASSERT_TRUE(res.collision); +} + +/** \brief Adding obstacles to the world which are tested against the robot. Simple cases. */ +TEST_F(CollisionDetectionEnvTest, RobotWorldCollision_1) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + shapes::Shape* shape = new shapes::Box(.1, .1, .1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + pos1.translation().z() = 0.3; + c_env_->getWorld()->addToObject("box", shape_ptr, pos1); + + c_env_->checkSelfCollision(req, res, *robot_state_, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + c_env_->checkRobotCollision(req, res, *robot_state_, *acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + c_env_->getWorld()->moveObject("box", pos1); + c_env_->checkRobotCollision(req, res, *robot_state_, *acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + c_env_->getWorld()->moveObject("box", pos1); + ASSERT_FALSE(res.collision); +} + +/** \brief Adding obstacles to the world which are tested against the robot. */ +TEST_F(CollisionDetectionEnvTest, RobotWorldCollision_2) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + req.max_contacts = 10; + req.contacts = true; + req.verbose = true; + + shapes::Shape* shape = new shapes::Box(.4, .4, .4); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + pos1.translation().z() = 0.3; + c_env_->getWorld()->addToObject("box", shape_ptr, pos1); + c_env_->checkRobotCollision(req, res, *robot_state_, *acm_); + ASSERT_TRUE(res.collision); + ASSERT_GE(res.contact_count, 3u); + res.clear(); +} + +/** \brief Tests the padding through expanding the link geometry in such a way that a collision occurs. */ +TEST_F(CollisionDetectionEnvTest, PaddingTest) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 10; + collision_detection::CollisionResult res; + + c_env_->checkRobotCollision(req, res, *robot_state_, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + // Adding the box right in front of the robot hand + shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + pos.translation().x() = 0.43; + pos.translation().y() = 0; + pos.translation().z() = 0.55; + c_env_->getWorld()->addToObject("box", shape_ptr, pos); + + c_env_->setLinkPadding("panda_hand", 0.08); + c_env_->checkRobotCollision(req, res, *robot_state_, *acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + c_env_->setLinkPadding("panda_hand", 0.0); + c_env_->checkRobotCollision(req, res, *robot_state_, *acm_); + ASSERT_FALSE(res.collision); +} + +/** \brief Continuous self collision checks of the robot. + * + * Functionality not supported yet. */ +TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionSelf) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + robot_state::RobotState state1(robot_model_); + robot_state::RobotState state2(robot_model_); + + setToHome(state1); + double joint2 = 0.15; + double joint4 = -3.0; + double joint5 = -0.8; + double joint7 = -0.785; + state1.setJointPositions("panda_joint2", &joint2); + state1.setJointPositions("panda_joint4", &joint4); + state1.setJointPositions("panda_joint5", &joint5); + state1.setJointPositions("panda_joint7", &joint7); + state1.update(); + + c_env_->checkSelfCollision(req, res, state1, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + setToHome(state2); + double joint_5_other = 0.8; + state2.setJointPositions("panda_joint2", &joint2); + state2.setJointPositions("panda_joint4", &joint4); + state2.setJointPositions("panda_joint5", &joint_5_other); + state2.setJointPositions("panda_joint7", &joint7); + state2.update(); + + c_env_->checkSelfCollision(req, res, state2, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + // c_env_->checkSelfCollision(req, res, state1, state2, *acm_); + ROS_INFO_STREAM("Continous to continous collisions are not supported yet, therefore fail here."); + ASSERT_TRUE(res.collision); + res.clear(); +} + +/** \brief Two similar robot poses are used as start and end pose of a continuous collision check. + * + * Functionality not supported yet. */ +TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionWorld) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 10; + collision_detection::CollisionResult res; + + robot_state::RobotState state1(robot_model_); + robot_state::RobotState state2(robot_model_); + + setToHome(state1); + state1.update(); + + setToHome(state2); + double joint_2{ 0.05 }; + double joint_4{ -1.6 }; + state2.setJointPositions("panda_joint2", &joint_2); + state2.setJointPositions("panda_joint4", &joint_4); + state2.update(); + + c_env_->checkRobotCollision(req, res, state1, state2, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + // Adding the box which is not in collision with the individual states but sits right between them. + shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + pos.translation().x() = 0.43; + pos.translation().y() = 0; + pos.translation().z() = 0.55; + c_env_->getWorld()->addToObject("box", shape_ptr, pos); + + c_env_->checkRobotCollision(req, res, state1, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + c_env_->checkRobotCollision(req, res, state2, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + c_env_->checkRobotCollision(req, res, state1, state2, *acm_); + ASSERT_TRUE(res.collision); + ASSERT_EQ(res.contact_count, 4u); + res.clear(); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/collision_distance_field/CMakeLists.txt b/moveit_core/collision_distance_field/CMakeLists.txt index ad1850a61e..063a1331a3 100644 --- a/moveit_core/collision_distance_field/CMakeLists.txt +++ b/moveit_core/collision_distance_field/CMakeLists.txt @@ -8,10 +8,8 @@ endif() add_library(${MOVEIT_LIB_NAME} src/collision_distance_field_types.cpp src/collision_common_distance_field.cpp - src/collision_robot_distance_field.cpp - src/collision_world_distance_field.cpp - src/collision_robot_hybrid.cpp - src/collision_world_hybrid.cpp + src/collision_env_distance_field.cpp + src/collision_env_hybrid.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h index 024be61cfe..cfb6fe750d 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h index 9581cefa7b..b699170892 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h @@ -37,17 +37,15 @@ #pragma once #include -#include -#include +#include namespace collision_detection { /** \brief An allocator for Distance Field collision detectors */ class CollisionDetectorAllocatorDistanceField - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_world_distance_field.cpp + static const std::string NAME; // defined in collision_env_distance_field.cpp }; } diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h index a2c643b282..efce4d7b11 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h @@ -37,17 +37,15 @@ #pragma once #include -#include -#include +#include namespace collision_detection { /** \brief An allocator for Hybrid collision detectors */ class CollisionDetectorAllocatorHybrid - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_world_hybrid.cpp + static const std::string NAME; // defined in collision_env_hybrid.cpp }; } diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h similarity index 57% rename from moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_distance_field.h rename to moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h index dd13b12043..f9fb4366ad 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h @@ -37,9 +37,9 @@ #pragma once #include -#include #include #include +#include #include #include @@ -53,32 +53,36 @@ static const double DEFAULT_RESOLUTION = .02; static const double DEFAULT_COLLISION_TOLERANCE = 0.0; static const double DEFAULT_MAX_PROPOGATION_DISTANCE = .25; -MOVEIT_CLASS_FORWARD(CollisionRobotDistanceField); +MOVEIT_CLASS_FORWARD(CollisionEnvDistanceField); -class CollisionRobotDistanceField : public CollisionRobot +class CollisionEnvDistanceField : public CollisionEnv { - friend class CollisionWorldDistanceField; - public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - CollisionRobotDistanceField(const robot_model::RobotModelConstPtr& robot_model); - - CollisionRobotDistanceField(const CollisionRobot& col_robot, const Eigen::Vector3d& size, - const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, - double collision_tolerance, double max_propogation_distance, double padding); - - CollisionRobotDistanceField(const robot_model::RobotModelConstPtr& robot_model, - const std::map>& link_body_decompositions, - double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, - double size_z = DEFAULT_SIZE_Z, - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, - double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, - double scale = 1.0); - - CollisionRobotDistanceField(const CollisionRobotDistanceField& other); + CollisionEnvDistanceField(const robot_model::RobotModelConstPtr& robot_model, + const std::map>& link_body_decompositions = + std::map>(), + double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, + double size_z = DEFAULT_SIZE_Z, Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), + bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, + double resolution = DEFAULT_RESOLUTION, + double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, + double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, + double scale = 1.0); + + CollisionEnvDistanceField(const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, + const std::map>& link_body_decompositions = + std::map>(), + double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, + double size_z = DEFAULT_SIZE_Z, Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), + bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, + double resolution = DEFAULT_RESOLUTION, + double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, + double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, + double scale = 1.0); + + CollisionEnvDistanceField(const CollisionEnvDistanceField& other, const WorldPtr& world); void initialize(const std::map>& link_body_decompositions, const Eigen::Vector3d& size, const Eigen::Vector3d& origin, bool use_signed_distance_field, @@ -98,51 +102,6 @@ class CollisionRobotDistanceField : public CollisionRobot const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state1, const moveit::core::RobotState& state2) const override - { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - }; - - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, - const collision_detection::AllowedCollisionMatrix& acm) const override - { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - }; - - void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, const CollisionRobot& other_robot, - const moveit::core::RobotState& other_state) const override - { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - }; - - void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, const CollisionRobot& other_robot, - const moveit::core::RobotState& other_state, - const collision_detection::AllowedCollisionMatrix& acm) const override - { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - }; - - void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, - const CollisionRobot& other_robot, const moveit::core::RobotState& other_state1, - const moveit::core::RobotState& other_state2) const override - { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - }; - - void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, - const CollisionRobot& other_robot, const moveit::core::RobotState& other_state1, - const moveit::core::RobotState& other_state2, - const collision_detection::AllowedCollisionMatrix& acm) const override - { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - }; - void createCollisionModelMarker(const moveit::core::RobotState& state, visualization_msgs::MarkerArray& model_markers) const; @@ -150,22 +109,12 @@ class CollisionRobotDistanceField : public CollisionRobot { return 0.0; }; + virtual double distanceSelf(const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix& acm) const { return 0.0; }; - virtual double distanceOther(const moveit::core::RobotState& state, const CollisionRobot& other_robot, - const moveit::core::RobotState& other_state) const - { - return 0.0; - }; - virtual double distanceOther(const moveit::core::RobotState& state, const CollisionRobot& other_robot, - const moveit::core::RobotState& other_state, - const collision_detection::AllowedCollisionMatrix& acm) const - { - return 0.0; - }; void distanceSelf(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state) const override @@ -173,12 +122,6 @@ class CollisionRobotDistanceField : public CollisionRobot ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); } - void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override - { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - } - DistanceFieldCacheEntryConstPtr getLastDistanceFieldEntry() const { return distance_field_cache_entry_; @@ -191,6 +134,84 @@ class CollisionRobotDistanceField : public CollisionRobot // const // collision_detection::AllowedCollisionMatrix // &acm) const; + + MOVEIT_STRUCT_FORWARD(DistanceFieldCacheEntryWorld) + struct DistanceFieldCacheEntryWorld + { + std::map> posed_body_point_decompositions_; + distance_field::DistanceFieldPtr distance_field_; + }; + + ~CollisionEnvDistanceField() override; + + void checkCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const override; + + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + GroupStateRepresentationPtr& gsr) const; + + void checkCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const override; + + virtual double distanceRobot(const robot_state::RobotState& state, bool verbose = false) const + { + return 0.0; + } + + virtual double distanceRobot(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, + bool verbose = false) const + { + return 0.0; + } + + void distanceRobot(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const override + { + ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); + } + + void setWorld(const WorldPtr& world) override; + + distance_field::DistanceFieldConstPtr getDistanceField() const + { + return distance_field_cache_entry_->distance_field_; + } + + collision_detection::GroupStateRepresentationConstPtr getLastGroupStateRepresentation() const + { + return last_gsr_; + } + + void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; + + void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; + protected: bool getSelfProximityGradients(GroupStateRepresentationPtr& gsr) const; @@ -244,6 +265,20 @@ class CollisionRobotDistanceField : public CollisionRobot void updatedPaddingOrScaling(const std::vector& links) override{}; + DistanceFieldCacheEntryWorldPtr generateDistanceFieldCacheEntryWorld(); + + void updateDistanceObject(const std::string& id, CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr& dfce, + EigenSTL::vector_Vector3d& add_points, EigenSTL::vector_Vector3d& subtract_points); + + bool getEnvironmentCollisions(const CollisionRequest& req, CollisionResult& res, + const distance_field::DistanceFieldConstPtr& env_distance_field, + GroupStateRepresentationPtr& gsr) const; + + bool getEnvironmentProximityGradients(const distance_field::DistanceFieldConstPtr& env_distance_field, + GroupStateRepresentationPtr& gsr) const; + + static void notifyObjectChange(CollisionEnvDistanceField* self, const ObjectConstPtr& obj, World::Action action); + Eigen::Vector3d size_; Eigen::Vector3d origin_; bool use_signed_distance_field_; @@ -260,5 +295,10 @@ class CollisionRobotDistanceField : public CollisionRobot std::map pregenerated_group_state_representation_map_; planning_scene::PlanningScenePtr planning_scene_; + + mutable boost::mutex update_cache_lock_world_; + DistanceFieldCacheEntryWorldPtr distance_field_cache_entry_world_; + GroupStateRepresentationPtr last_gsr_; + World::ObserverHandle observer_handle_; }; } diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h new file mode 100644 index 0000000000..784623385d --- /dev/null +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h @@ -0,0 +1,153 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ + +/* Author: E. Gil Jones, Jens Petit */ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace collision_detection +{ +/** \brief This hybrid collision environment combines FCL and a distance field. Both can be used to calculate + * collisions. */ +class CollisionEnvHybrid : public collision_detection::CollisionEnvFCL +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + CollisionEnvHybrid(const robot_model::RobotModelConstPtr& robot_model, + const std::map>& link_body_decompositions = + std::map>(), + double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, + Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), + bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, + double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, + double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, + double scale = 1.0); + + CollisionEnvHybrid(const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, + const std::map>& link_body_decompositions = + std::map>(), + double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, + Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), + bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, + double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, + double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, + double scale = 1.0); + + CollisionEnvHybrid(const CollisionEnvHybrid& other, const WorldPtr& world); + + ~CollisionEnvHybrid() override + { + } + + void initializeRobotDistanceField(const std::map>& link_body_decompositions, + double size_x, double size_y, double size_z, bool use_signed_distance_field, + double resolution, double collision_tolerance, double max_propogation_distance) + { + cenv_distance_->initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), + Eigen::Vector3d(0, 0, 0), use_signed_distance_field, resolution, collision_tolerance, + max_propogation_distance); + } + + void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const robot_state::RobotState& state) const; + + void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, const robot_state::RobotState& state, + GroupStateRepresentationPtr& gsr) const; + + void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, const robot_state::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm) const; + + void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, const robot_state::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + const CollisionEnvDistanceFieldConstPtr getCollisionRobotDistanceField() const + { + return cenv_distance_; + } + + void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const; + + void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; + + void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; + + void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const; + + void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; + + void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + + void setWorld(const WorldPtr& world) override; + + void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; + + void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; + + const CollisionEnvDistanceFieldConstPtr getCollisionWorldDistanceField() const + { + return cenv_distance_; + } + +protected: + CollisionEnvDistanceFieldPtr cenv_distance_; +}; +}; diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_hybrid.h deleted file mode 100644 index 7b188ce97e..0000000000 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_hybrid.h +++ /dev/null @@ -1,98 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 the 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. - *********************************************************************/ - -/* Author: E. Gil Jones */ - -#pragma once - -#include -#include -#include -#include - -#include - -namespace collision_detection -{ -class CollisionRobotHybrid : public collision_detection::CollisionRobotFCL -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - CollisionRobotHybrid(const robot_model::RobotModelConstPtr& robot_model); - - CollisionRobotHybrid(const robot_model::RobotModelConstPtr& robot_model, - const std::map>& link_body_decompositions, - double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, - double scale = 1.0); - - CollisionRobotHybrid(const CollisionRobotHybrid& other); - - void initializeRobotDistanceField(const std::map>& link_body_decompositions, - double size_x, double size_y, double size_z, bool use_signed_distance_field, - double resolution, double collision_tolerance, double max_propogation_distance) - { - crobot_distance_->initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), - Eigen::Vector3d(0, 0, 0), use_signed_distance_field, resolution, collision_tolerance, - max_propogation_distance); - } - - void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const robot_state::RobotState& state) const; - - void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const robot_state::RobotState& state, - GroupStateRepresentationPtr& gsr) const; - - void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const robot_state::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm) const; - - void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const robot_state::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - const CollisionRobotDistanceFieldConstPtr getCollisionRobotDistanceField() const - { - return crobot_distance_; - } - -protected: - CollisionRobotDistanceFieldPtr crobot_distance_; -}; -} diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h deleted file mode 100644 index ff05802ab9..0000000000 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h +++ /dev/null @@ -1,200 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 the 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. - *********************************************************************/ - -/* Author: E. Gil Jones */ - -#pragma once - -#include -#include -#include -#include - -namespace collision_detection -{ -MOVEIT_CLASS_FORWARD(CollisionWorldDistanceField) - -class CollisionWorldDistanceField : public CollisionWorld -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - MOVEIT_STRUCT_FORWARD(DistanceFieldCacheEntry) - struct DistanceFieldCacheEntry - { - std::map> posed_body_point_decompositions_; - distance_field::DistanceFieldPtr distance_field_; - }; - - CollisionWorldDistanceField(Eigen::Vector3d size = Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z), - Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, - double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE); - - explicit CollisionWorldDistanceField( - const WorldPtr& world, Eigen::Vector3d size = Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z), - Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution = DEFAULT_RESOLUTION, - double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE); - - CollisionWorldDistanceField(const CollisionWorldDistanceField& other, const WorldPtr& world); - - ~CollisionWorldDistanceField() override; - - void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override; - - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; - - void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override; - - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override; - - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override; - - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2) const override - { - } - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const override - { - } - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const override - { - } - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, - const AllowedCollisionMatrix& acm) const override - { - } - - virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state, - bool verbose = false) const - { - return 0.0; - } - virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm, bool verbose = false) const - { - return 0.0; - } - virtual double distanceWorld(const CollisionWorld& world, bool verbose = false) const - { - return 0.0; - } - virtual double distanceWorld(const CollisionWorld& world, const AllowedCollisionMatrix& acm, - bool verbose = false) const - { - return 0.0; - } - - void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override - { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - } - - void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override - { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - } - - void setWorld(const WorldPtr& world) override; - - distance_field::DistanceFieldConstPtr getDistanceField() const - { - return distance_field_cache_entry_->distance_field_; - } - - collision_detection::GroupStateRepresentationConstPtr getLastGroupStateRepresentation() const - { - return last_gsr_; - } - - void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const; - - void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const; - -protected: - DistanceFieldCacheEntryPtr generateDistanceFieldCacheEntry(); - - void updateDistanceObject(const std::string& id, CollisionWorldDistanceField::DistanceFieldCacheEntryPtr& dfce, - EigenSTL::vector_Vector3d& add_points, EigenSTL::vector_Vector3d& subtract_points); - - bool getEnvironmentCollisions(const CollisionRequest& req, CollisionResult& res, - const distance_field::DistanceFieldConstPtr& env_distance_field, - GroupStateRepresentationPtr& gsr) const; - - bool getEnvironmentProximityGradients(const distance_field::DistanceFieldConstPtr& env_distance_field, - GroupStateRepresentationPtr& gsr) const; - - static void notifyObjectChange(CollisionWorldDistanceField* self, const ObjectConstPtr& obj, World::Action action); - - Eigen::Vector3d size_; - Eigen::Vector3d origin_; - bool use_signed_distance_field_; - double resolution_; - double collision_tolerance_; - double max_propogation_distance_; - - mutable boost::mutex update_cache_lock_; - DistanceFieldCacheEntryPtr distance_field_cache_entry_; - GroupStateRepresentationPtr last_gsr_; - World::ObserverHandle observer_handle_; -}; -} diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_hybrid.h deleted file mode 100644 index bb3d07e8cf..0000000000 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_hybrid.h +++ /dev/null @@ -1,116 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 the 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. - *********************************************************************/ - -/* Author: E. Gil Jones */ - -#pragma once - -#include -#include -#include -#include - -namespace collision_detection -{ -class CollisionRobotHybrid; - -class CollisionWorldHybrid : public CollisionWorldFCL -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - CollisionWorldHybrid(Eigen::Vector3d size = Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z), - Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE); - - explicit CollisionWorldHybrid(const WorldPtr& world, - Eigen::Vector3d size = Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z), - Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, - double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE); - - CollisionWorldHybrid(const CollisionWorldHybrid& other, const WorldPtr& world); - - ~CollisionWorldHybrid() override - { - } - - void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const; - - void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; - - void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; - - void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const; - - void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; - - void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - - void setWorld(const WorldPtr& world) override; - - void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const; - - void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const; - - const CollisionWorldDistanceFieldConstPtr getCollisionWorldDistanceField() const - { - return cworld_distance_; - } - -protected: - CollisionWorldDistanceFieldPtr cworld_distance_; -}; -} diff --git a/moveit_core/collision_distance_field/src/collision_robot_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp similarity index 68% rename from moveit_core/collision_distance_field/src/collision_robot_distance_field.cpp rename to moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index 85cf2246ee..0c1cc66d9f 100644 --- a/moveit_core/collision_distance_field/src/collision_robot_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -35,54 +35,61 @@ /* Author: E. Gil Jones */ #include -#include -#include -#include #include #include #include +#include +#include +#include +#include +#include +#include +#include namespace collision_detection { const double EPSILON = 0.001f; -CollisionRobotDistanceField::CollisionRobotDistanceField(const robot_model::RobotModelConstPtr& robot_model) - : CollisionRobot(robot_model) +const std::string collision_detection::CollisionDetectorAllocatorDistanceField::NAME("DISTANCE_FIELD"); + +CollisionEnvDistanceField::CollisionEnvDistanceField( + const robot_model::RobotModelConstPtr& robot_model, + const std::map>& link_body_decompositions, double size_x, double size_y, + double size_z, Eigen::Vector3d origin, bool use_signed_distance_field, double resolution, + double collision_tolerance, double max_propogation_distance, double padding, double scale) + : CollisionEnv(robot_model) { - // planning_scene_.reset(new planning_scene::PlanningScene(robot_model_)); + initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field, + resolution, collision_tolerance, max_propogation_distance); - std::map> link_body_decompositions; - Eigen::Vector3d size(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z); - initialize(link_body_decompositions, size, Eigen::Vector3d(0, 0, 0), DEFAULT_USE_SIGNED_DISTANCE_FIELD, - DEFAULT_RESOLUTION, DEFAULT_COLLISION_TOLERANCE, DEFAULT_MAX_PROPOGATION_DISTANCE); setPadding(0.0); + + distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld(); + + // request notifications about changes to world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); } -CollisionRobotDistanceField::CollisionRobotDistanceField( - const robot_model::RobotModelConstPtr& robot_model, +CollisionEnvDistanceField::CollisionEnvDistanceField( + const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, const std::map>& link_body_decompositions, double size_x, double size_y, - double size_z, bool use_signed_distance_field, double resolution, double collision_tolerance, - double max_propogation_distance, double padding, double scale) - : CollisionRobot(robot_model, padding, scale) + double size_z, Eigen::Vector3d origin, bool use_signed_distance_field, double resolution, + double collision_tolerance, double max_propogation_distance, double padding, double scale) + : CollisionEnv(robot_model, world, padding, scale) { - initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), Eigen::Vector3d(0, 0, 0), - use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance); -} + initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field, + resolution, collision_tolerance, max_propogation_distance); -CollisionRobotDistanceField::CollisionRobotDistanceField(const CollisionRobot& col_robot, const Eigen::Vector3d& size, - const Eigen::Vector3d& origin, bool use_signed_distance_field, - double resolution, double collision_tolerance, - double max_propogation_distance, double padding) - : CollisionRobot(col_robot) -{ - std::map> link_body_decompositions; - initialize(link_body_decompositions, size, origin, use_signed_distance_field, resolution, collision_tolerance, - max_propogation_distance); - setPadding(padding); + distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld(); + + // request notifications about changes to world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); } -CollisionRobotDistanceField::CollisionRobotDistanceField(const CollisionRobotDistanceField& other) - : CollisionRobot(other) +CollisionEnvDistanceField::CollisionEnvDistanceField(const CollisionEnvDistanceField& other, const WorldPtr& world) + : CollisionEnv(other, world) { size_ = other.size_; origin_ = other.origin_; @@ -94,11 +101,21 @@ CollisionRobotDistanceField::CollisionRobotDistanceField(const CollisionRobotDis link_body_decomposition_vector_ = other.link_body_decomposition_vector_; link_body_decomposition_index_map_ = other.link_body_decomposition_index_map_; in_group_update_map_ = other.in_group_update_map_; + distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld(); pregenerated_group_state_representation_map_ = other.pregenerated_group_state_representation_map_; planning_scene_.reset(new planning_scene::PlanningScene(robot_model_)); + + // request notifications about changes to world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +CollisionEnvDistanceField::~CollisionEnvDistanceField() +{ + getWorld()->removeObserver(observer_handle_); } -void CollisionRobotDistanceField::initialize( +void CollisionEnvDistanceField::initialize( const std::map>& link_body_decompositions, const Eigen::Vector3d& size, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance) @@ -130,7 +147,7 @@ void CollisionRobotDistanceField::initialize( } } -void CollisionRobotDistanceField::generateCollisionCheckingStructures( +void CollisionEnvDistanceField::generateCollisionCheckingStructures( const std::string& group_name, const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr, bool generate_distance_field) const @@ -143,17 +160,17 @@ void CollisionRobotDistanceField::generateCollisionCheckingStructures( DistanceFieldCacheEntryPtr new_dfce = generateDistanceFieldCacheEntry(group_name, state, acm, generate_distance_field); boost::mutex::scoped_lock slock(update_cache_lock_); - (const_cast(this))->distance_field_cache_entry_ = new_dfce; + (const_cast(this))->distance_field_cache_entry_ = new_dfce; dfce = new_dfce; } getGroupStateRepresentation(dfce, state, gsr); } -void CollisionRobotDistanceField::checkSelfCollisionHelper(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const +void CollisionEnvDistanceField::checkSelfCollisionHelper(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix* acm, + GroupStateRepresentationPtr& gsr) const { if (!gsr) { @@ -174,9 +191,9 @@ void CollisionRobotDistanceField::checkSelfCollisionHelper(const collision_detec } DistanceFieldCacheEntryConstPtr -CollisionRobotDistanceField::getDistanceFieldCacheEntry(const std::string& group_name, - const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix* acm) const +CollisionEnvDistanceField::getDistanceFieldCacheEntry(const std::string& group_name, + const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix* acm) const { DistanceFieldCacheEntryConstPtr ret; if (!distance_field_cache_entry_) @@ -205,36 +222,36 @@ CollisionRobotDistanceField::getDistanceFieldCacheEntry(const std::string& group return cur; } -void CollisionRobotDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const moveit::core::RobotState& state) const +void CollisionEnvDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state) const { GroupStateRepresentationPtr gsr; checkSelfCollisionHelper(req, res, state, nullptr, gsr); } -void CollisionRobotDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, - GroupStateRepresentationPtr& gsr) const +void CollisionEnvDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const { checkSelfCollisionHelper(req, res, state, nullptr, gsr); } -void CollisionRobotDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm) const +void CollisionEnvDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm) const { GroupStateRepresentationPtr gsr; checkSelfCollisionHelper(req, res, state, &acm, gsr); } -void CollisionRobotDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const +void CollisionEnvDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const { if (gsr) { @@ -244,9 +261,9 @@ void CollisionRobotDistanceField::checkSelfCollision(const collision_detection:: checkSelfCollisionHelper(req, res, state, &acm, gsr); } -bool CollisionRobotDistanceField::getSelfCollisions(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - GroupStateRepresentationPtr& gsr) const +bool CollisionEnvDistanceField::getSelfCollisions(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + GroupStateRepresentationPtr& gsr) const { for (unsigned int i = 0; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); i++) { @@ -324,7 +341,7 @@ bool CollisionRobotDistanceField::getSelfCollisions(const collision_detection::C return (res.contact_count >= req.max_contacts); } -bool CollisionRobotDistanceField::getSelfProximityGradients(GroupStateRepresentationPtr& gsr) const +bool CollisionEnvDistanceField::getSelfProximityGradients(GroupStateRepresentationPtr& gsr) const { bool in_collision = false; @@ -399,9 +416,9 @@ bool CollisionRobotDistanceField::getSelfProximityGradients(GroupStateRepresenta return in_collision; } -bool CollisionRobotDistanceField::getIntraGroupCollisions(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - GroupStateRepresentationPtr& gsr) const +bool CollisionEnvDistanceField::getIntraGroupCollisions(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + GroupStateRepresentationPtr& gsr) const { unsigned int num_links = gsr->dfce_->link_names_.size(); unsigned int num_attached_bodies = gsr->dfce_->attached_body_names_.size(); @@ -628,7 +645,7 @@ bool CollisionRobotDistanceField::getIntraGroupCollisions(const collision_detect return false; } -bool CollisionRobotDistanceField::getIntraGroupProximityGradients(GroupStateRepresentationPtr& gsr) const +bool CollisionEnvDistanceField::getIntraGroupProximityGradients(GroupStateRepresentationPtr& gsr) const { bool in_collision = false; unsigned int num_links = gsr->dfce_->link_names_.size(); @@ -694,7 +711,7 @@ bool CollisionRobotDistanceField::getIntraGroupProximityGradients(GroupStateRepr } return in_collision; } -DistanceFieldCacheEntryPtr CollisionRobotDistanceField::generateDistanceFieldCacheEntry( +DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCacheEntry( const std::string& group_name, const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix* acm, bool generate_distance_field) const { @@ -947,7 +964,7 @@ DistanceFieldCacheEntryPtr CollisionRobotDistanceField::generateDistanceFieldCac return dfce; } -void CollisionRobotDistanceField::addLinkBodyDecompositions(double resolution) +void CollisionEnvDistanceField::addLinkBodyDecompositions(double resolution) { const std::vector& link_models = robot_model_->getLinkModelsWithCollisionGeometry(); for (const moveit::core::LinkModel* link_model : link_models) @@ -967,8 +984,8 @@ void CollisionRobotDistanceField::addLinkBodyDecompositions(double resolution) } } -void CollisionRobotDistanceField::createCollisionModelMarker(const moveit::core::RobotState& state, - visualization_msgs::MarkerArray& model_markers) const +void CollisionEnvDistanceField::createCollisionModelMarker(const moveit::core::RobotState& state, + visualization_msgs::MarkerArray& model_markers) const { // creating colors std_msgs::ColorRGBA robot_color; @@ -1035,7 +1052,7 @@ void CollisionRobotDistanceField::createCollisionModelMarker(const moveit::core: } } -void CollisionRobotDistanceField::addLinkBodyDecompositions( +void CollisionEnvDistanceField::addLinkBodyDecompositions( double resolution, const std::map>& link_spheres) { ROS_ASSERT_MSG(robot_model_, "RobotModelPtr is invalid"); @@ -1065,7 +1082,7 @@ void CollisionRobotDistanceField::addLinkBodyDecompositions( ROS_DEBUG_STREAM(__FUNCTION__ << " Finished "); } -PosedBodySphereDecompositionPtr CollisionRobotDistanceField::getPosedLinkBodySphereDecomposition( +PosedBodySphereDecompositionPtr CollisionEnvDistanceField::getPosedLinkBodySphereDecomposition( const moveit::core::LinkModel* ls, unsigned int ind) const { PosedBodySphereDecompositionPtr ret; @@ -1074,7 +1091,7 @@ PosedBodySphereDecompositionPtr CollisionRobotDistanceField::getPosedLinkBodySph } PosedBodyPointDecompositionPtr -CollisionRobotDistanceField::getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel* ls) const +CollisionEnvDistanceField::getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel* ls) const { PosedBodyPointDecompositionPtr ret; std::map::const_iterator it = link_body_decomposition_index_map_.find(ls->getName()); @@ -1087,8 +1104,8 @@ CollisionRobotDistanceField::getPosedLinkBodyPointDecomposition(const moveit::co return ret; } -void CollisionRobotDistanceField::updateGroupStateRepresentationState(const moveit::core::RobotState& state, - GroupStateRepresentationPtr& gsr) const +void CollisionEnvDistanceField::updateGroupStateRepresentationState(const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const { for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++) { @@ -1141,9 +1158,9 @@ void CollisionRobotDistanceField::updateGroupStateRepresentationState(const move } } -void CollisionRobotDistanceField::getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr& dfce, - const moveit::core::RobotState& state, - GroupStateRepresentationPtr& gsr) const +void CollisionEnvDistanceField::getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr& dfce, + const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const { if (!dfce->pregenerated_group_state_representation_) { @@ -1239,8 +1256,8 @@ void CollisionRobotDistanceField::getGroupStateRepresentation(const DistanceFiel } } -bool CollisionRobotDistanceField::compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr& dfce, - const moveit::core::RobotState& state) const +bool CollisionEnvDistanceField::compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr& dfce, + const moveit::core::RobotState& state) const { std::vector new_state_values(state.getVariableCount()); for (unsigned int i = 0; i < new_state_values.size(); i++) @@ -1299,7 +1316,7 @@ bool CollisionRobotDistanceField::compareCacheEntryToState(const DistanceFieldCa return true; } -bool CollisionRobotDistanceField::compareCacheEntryToAllowedCollisionMatrix( +bool CollisionEnvDistanceField::compareCacheEntryToAllowedCollisionMatrix( const DistanceFieldCacheEntryConstPtr& dfce, const collision_detection::AllowedCollisionMatrix& acm) const { if (dfce->acm_.getSize() != acm.getSize()) @@ -1361,7 +1378,7 @@ bool CollisionRobotDistanceField::compareCacheEntryToAllowedCollisionMatrix( } // void -// CollisionRobotDistanceField::generateAllowedCollisionInformation(CollisionRobotDistanceField::DistanceFieldCacheEntryPtr& +// CollisionEnvDistanceField::generateAllowedCollisionInformation(CollisionEnvDistanceField::DistanceFieldCacheEntryPtr& // dfce) // { // for(unsigned int i = 0; i < dfce.link_names_.size(); i++) { @@ -1369,4 +1386,420 @@ bool CollisionRobotDistanceField::compareCacheEntryToAllowedCollisionMatrix( // if(dfce->acm.find // } // } + +void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const +{ + GroupStateRepresentationPtr gsr; + checkCollision(req, res, state, gsr); +} + +void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + GroupStateRepresentationPtr& gsr) const +{ + if (!gsr) + { + generateCollisionCheckingStructures(req.group_name, state, nullptr, gsr, true); + } + else + { + updateGroupStateRepresentationState(state, gsr); + } + bool done = getSelfCollisions(req, res, gsr); + if (!done) + { + done = getIntraGroupCollisions(req, res, gsr); + } + if (!done) + { + getEnvironmentCollisions(req, res, distance_field_cache_entry_world_->distance_field_, gsr); + } + + (const_cast(this))->last_gsr_ = gsr; +} + +void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + GroupStateRepresentationPtr gsr; + checkCollision(req, res, state, acm, gsr); +} + +void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const +{ + if (!gsr) + { + generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true); + } + else + { + updateGroupStateRepresentationState(state, gsr); + } + bool done = getSelfCollisions(req, res, gsr); + if (!done) + { + done = getIntraGroupCollisions(req, res, gsr); + } + if (!done) + { + getEnvironmentCollisions(req, res, distance_field_cache_entry_world_->distance_field_, gsr); + } + + (const_cast(this))->last_gsr_ = gsr; +} + +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const +{ + GroupStateRepresentationPtr gsr; + checkRobotCollision(req, res, state, gsr); +} + +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + GroupStateRepresentationPtr& gsr) const +{ + distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_; + if (!gsr) + { + generateCollisionCheckingStructures(req.group_name, state, nullptr, gsr, false); + } + else + { + updateGroupStateRepresentationState(state, gsr); + } + getEnvironmentCollisions(req, res, env_distance_field, gsr); + (const_cast(this))->last_gsr_ = gsr; + + // checkRobotCollisionHelper(req, res, robot, state, &acm); +} + +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + GroupStateRepresentationPtr gsr; + checkRobotCollision(req, res, state, acm, gsr); +} + +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const +{ + distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_; + + if (!gsr) + { + generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true); + } + else + { + updateGroupStateRepresentationState(state, gsr); + } + getEnvironmentCollisions(req, res, env_distance_field, gsr); + (const_cast(this))->last_gsr_ = gsr; + + // checkRobotCollisionHelper(req, res, robot, state, &acm); +} + +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const +{ + ROS_ERROR_NAMED("collision_detection.distance", "Continuous collision checking not implemented"); +} + +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const +{ + ROS_ERROR_NAMED("collision_detection.distance", "Continuous collision checking not implemented"); +} + +void CollisionEnvDistanceField::getCollisionGradients(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm, + GroupStateRepresentationPtr& gsr) const +{ + distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_; + + if (!gsr) + { + generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true); + } + else + { + updateGroupStateRepresentationState(state, gsr); + } + + getSelfProximityGradients(gsr); + getIntraGroupProximityGradients(gsr); + getEnvironmentProximityGradients(env_distance_field, gsr); + + (const_cast(this))->last_gsr_ = gsr; +} + +void CollisionEnvDistanceField::getAllCollisions(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm, + GroupStateRepresentationPtr& gsr) const +{ + if (!gsr) + { + generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true); + } + else + { + updateGroupStateRepresentationState(state, gsr); + } + getSelfCollisions(req, res, gsr); + getIntraGroupCollisions(req, res, gsr); + distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_; + getEnvironmentCollisions(req, res, env_distance_field, gsr); + + (const_cast(this))->last_gsr_ = gsr; +} + +bool CollisionEnvDistanceField::getEnvironmentCollisions( + const CollisionRequest& req, CollisionResult& res, const distance_field::DistanceFieldConstPtr& env_distance_field, + GroupStateRepresentationPtr& gsr) const +{ + for (unsigned int i = 0; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); i++) + { + bool is_link = i < gsr->dfce_->link_names_.size(); + std::string link_name = i < gsr->dfce_->link_names_.size() ? gsr->dfce_->link_names_[i] : "attached"; + if (is_link && !gsr->dfce_->link_has_geometry_[i]) + { + continue; + } + + const std::vector* collision_spheres_1; + const EigenSTL::vector_Vector3d* sphere_centers_1; + + if (is_link) + { + collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres()); + sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters()); + } + else + { + collision_spheres_1 = + &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres()); + sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()); + } + + if (req.contacts) + { + std::vector colls; + bool coll = getCollisionSphereCollision( + env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, max_propogation_distance_, + collision_tolerance_, std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls); + if (coll) + { + res.collision = true; + for (unsigned int col : colls) + { + Contact con; + if (is_link) + { + con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[col]; + con.body_type_1 = BodyTypes::ROBOT_LINK; + con.body_name_1 = gsr->dfce_->link_names_[i]; + } + else + { + con.pos = gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()[col]; + con.body_type_1 = BodyTypes::ROBOT_ATTACHED; + con.body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()]; + } + + con.body_type_2 = BodyTypes::WORLD_OBJECT; + con.body_name_2 = "environment"; + res.contact_count++; + res.contacts[std::pair(con.body_name_1, con.body_name_2)].push_back(con); + gsr->gradients_[i].types[col] = ENVIRONMENT; + // ROS_DEBUG_STREAM("Link " << dfce->link_names_[i] << " sphere " << + // colls[j] << " in env collision"); + } + + gsr->gradients_[i].collision = true; + if (res.contact_count >= req.max_contacts) + { + return true; + } + } + } + else + { + bool coll = getCollisionSphereCollision(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, + max_propogation_distance_, collision_tolerance_); + if (coll) + { + res.collision = true; + return true; + } + } + } + return (res.contact_count >= req.max_contacts); +} + +bool CollisionEnvDistanceField::getEnvironmentProximityGradients( + const distance_field::DistanceFieldConstPtr& env_distance_field, GroupStateRepresentationPtr& gsr) const +{ + bool in_collision = false; + for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++) + { + bool is_link = i < gsr->dfce_->link_names_.size(); + + if (is_link && !gsr->dfce_->link_has_geometry_[i]) + { + continue; + } + + const std::vector* collision_spheres_1; + const EigenSTL::vector_Vector3d* sphere_centers_1; + if (is_link) + { + collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres()); + sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters()); + } + else + { + collision_spheres_1 = + &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres()); + sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()); + } + + bool coll = getCollisionSphereGradients(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, + gsr->gradients_[i], ENVIRONMENT, collision_tolerance_, false, + max_propogation_distance_, false); + if (coll) + { + in_collision = true; + } + } + return in_collision; +} + +void CollisionEnvDistanceField::setWorld(const WorldPtr& world) +{ + if (world == getWorld()) + return; + + // turn off notifications about old world + getWorld()->removeObserver(observer_handle_); + + // clear out objects from old world + distance_field_cache_entry_world_->distance_field_->reset(); + + CollisionEnv::setWorld(world); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + + // get notifications any objects already in the new world + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +void CollisionEnvDistanceField::notifyObjectChange(CollisionEnvDistanceField* self, const ObjectConstPtr& obj, + World::Action action) +{ + ros::WallTime n = ros::WallTime::now(); + + EigenSTL::vector_Vector3d add_points; + EigenSTL::vector_Vector3d subtract_points; + self->updateDistanceObject(obj->id_, self->distance_field_cache_entry_world_, add_points, subtract_points); + + if (action == World::DESTROY) + { + self->distance_field_cache_entry_world_->distance_field_->removePointsFromField(subtract_points); + } + else if (action & (World::MOVE_SHAPE | World::REMOVE_SHAPE)) + { + self->distance_field_cache_entry_world_->distance_field_->removePointsFromField(subtract_points); + self->distance_field_cache_entry_world_->distance_field_->addPointsToField(add_points); + } + else + { + self->distance_field_cache_entry_world_->distance_field_->addPointsToField(add_points); + } + + ROS_DEBUG_NAMED("collision_distance_field", "Modifying object %s took %lf s", obj->id_.c_str(), + (ros::WallTime::now() - n).toSec()); +} + +void CollisionEnvDistanceField::updateDistanceObject(const std::string& id, DistanceFieldCacheEntryWorldPtr& dfce, + EigenSTL::vector_Vector3d& add_points, + EigenSTL::vector_Vector3d& subtract_points) +{ + std::map>::iterator cur_it = + dfce->posed_body_point_decompositions_.find(id); + if (cur_it != dfce->posed_body_point_decompositions_.end()) + { + for (collision_detection::PosedBodyPointDecompositionPtr& posed_body_point_decomposition : cur_it->second) + { + subtract_points.insert(subtract_points.end(), posed_body_point_decomposition->getCollisionPoints().begin(), + posed_body_point_decomposition->getCollisionPoints().end()); + } + } + + World::ObjectConstPtr object = getWorld()->getObject(id); + if (object) + { + ROS_DEBUG_STREAM("Updating/Adding Object '" << object->id_ << "' with " << object->shapes_.size() + << " shapes to CollisionEnvDistanceField"); + std::vector shape_points; + for (unsigned int i = 0; i < object->shapes_.size(); i++) + { + shapes::ShapeConstPtr shape = object->shapes_[i]; + if (shape->type == shapes::OCTREE) + { + const shapes::OcTree* octree_shape = static_cast(shape.get()); + std::shared_ptr octree = octree_shape->octree; + + shape_points.push_back(std::make_shared(octree)); + } + else + { + BodyDecompositionConstPtr bd = getBodyDecompositionCacheEntry(shape, resolution_); + + shape_points.push_back(std::make_shared(bd, object->shape_poses_[i])); + } + + add_points.insert(add_points.end(), shape_points.back()->getCollisionPoints().begin(), + shape_points.back()->getCollisionPoints().end()); + } + + dfce->posed_body_point_decompositions_[id] = shape_points; + } + else + { + ROS_DEBUG_STREAM("Removing Object '" << id << "' from CollisionEnvDistanceField"); + dfce->posed_body_point_decompositions_.erase(id); + } +} + +CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr +CollisionEnvDistanceField::generateDistanceFieldCacheEntryWorld() +{ + DistanceFieldCacheEntryWorldPtr dfce(new DistanceFieldCacheEntryWorld()); + dfce->distance_field_.reset(new distance_field::PropagationDistanceField( + size_.x(), size_.y(), size_.z(), resolution_, origin_.x() - 0.5 * size_.x(), origin_.y() - 0.5 * size_.y(), + origin_.z() - 0.5 * size_.z(), max_propogation_distance_, use_signed_distance_field_)); + + EigenSTL::vector_Vector3d add_points; + EigenSTL::vector_Vector3d subtract_points; + for (const std::pair& object : *getWorld()) + { + updateDistanceObject(object.first, dfce, add_points, subtract_points); + } + dfce->distance_field_->addPointsToField(add_points); + return dfce; +} } // namespace collision_detection diff --git a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp new file mode 100644 index 0000000000..9c7928112d --- /dev/null +++ b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp @@ -0,0 +1,185 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ + +/* Author: E. Gil Jones, Jens Petit */ + +#include +#include +#include + +namespace collision_detection +{ +const std::string collision_detection::CollisionDetectorAllocatorHybrid::NAME("HYBRID"); + +CollisionEnvHybrid::CollisionEnvHybrid( + const robot_model::RobotModelConstPtr& robot_model, + const std::map>& link_body_decompositions, double size_x, double size_y, + double size_z, Eigen::Vector3d origin, bool use_signed_distance_field, double resolution, + double collision_tolerance, double max_propogation_distance, double padding, double scale) + : CollisionEnvFCL(robot_model) + , cenv_distance_(new collision_detection::CollisionEnvDistanceField( + robot_model, getWorld(), link_body_decompositions, size_x, size_y, size_z, std::move(origin), + use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance, padding, scale)) +{ +} + +CollisionEnvHybrid::CollisionEnvHybrid( + const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, + const std::map>& link_body_decompositions, double size_x, double size_y, + double size_z, Eigen::Vector3d origin, bool use_signed_distance_field, double resolution, + double collision_tolerance, double max_propogation_distance, double padding, double scale) + : CollisionEnvFCL(robot_model, world, padding, scale) + , cenv_distance_(new collision_detection::CollisionEnvDistanceField( + robot_model, getWorld(), link_body_decompositions, size_x, size_y, size_z, std::move(origin), + use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance, padding, scale)) +{ +} + +CollisionEnvHybrid::CollisionEnvHybrid(const CollisionEnvHybrid& other, const WorldPtr& world) + : CollisionEnvFCL(other, world) + , cenv_distance_(new collision_detection::CollisionEnvDistanceField(*other.getCollisionWorldDistanceField(), world)) +{ +} + +void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const robot_state::RobotState& state) const +{ + cenv_distance_->checkSelfCollision(req, res, state); +} + +void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const robot_state::RobotState& state, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->checkSelfCollision(req, res, state, gsr); +} + +void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const robot_state::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm) const +{ + cenv_distance_->checkSelfCollision(req, res, state, acm); +} + +void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const robot_state::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->checkSelfCollision(req, res, state, acm, gsr); +} + +void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const +{ + cenv_distance_->checkCollision(req, res, state); +} + +void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->checkCollision(req, res, state, gsr); +} + +void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + cenv_distance_->checkCollision(req, res, state, acm); +} + +void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->checkCollision(req, res, state, acm, gsr); +} + +void CollisionEnvHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const +{ + cenv_distance_->checkRobotCollision(req, res, state); +} + +void CollisionEnvHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->checkRobotCollision(req, res, state, gsr); +} + +void CollisionEnvHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + cenv_distance_->checkRobotCollision(req, res, state, acm); +} + +void CollisionEnvHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->checkRobotCollision(req, res, state, acm, gsr); +} + +void CollisionEnvHybrid::setWorld(const WorldPtr& world) +{ + if (world == getWorld()) + return; + + cenv_distance_->setWorld(world); + CollisionEnvFCL::setWorld(world); +} + +void CollisionEnvHybrid::getCollisionGradients(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix* acm, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->getCollisionGradients(req, res, state, acm, gsr); +} + +void CollisionEnvHybrid::getAllCollisions(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix* acm, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->getAllCollisions(req, res, state, acm, gsr); +} +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/src/collision_robot_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_robot_hybrid.cpp deleted file mode 100644 index 45f35aea88..0000000000 --- a/moveit_core/collision_distance_field/src/collision_robot_hybrid.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 the 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. - *********************************************************************/ - -/* Author: E. Gil Jones */ - -#include - -namespace collision_detection -{ -CollisionRobotHybrid::CollisionRobotHybrid(const robot_model::RobotModelConstPtr& robot_model) - : CollisionRobotFCL(robot_model) -{ - crobot_distance_.reset(new collision_detection::CollisionRobotDistanceField(robot_model)); -} - -CollisionRobotHybrid::CollisionRobotHybrid( - const robot_model::RobotModelConstPtr& robot_model, - const std::map>& link_body_decompositions, double size_x, double size_y, - double size_z, bool use_signed_distance_field, double resolution, double collision_tolerance, - double max_propogation_distance, double padding, double scale) - : CollisionRobotFCL(robot_model) -{ - crobot_distance_.reset(new collision_detection::CollisionRobotDistanceField( - robot_model, link_body_decompositions, size_x, size_y, size_z, use_signed_distance_field, resolution, - collision_tolerance, max_propogation_distance, padding, scale)); -} - -CollisionRobotHybrid::CollisionRobotHybrid(const CollisionRobotHybrid& other) : CollisionRobotFCL(other) -{ - crobot_distance_.reset( - new collision_detection::CollisionRobotDistanceField(*other.getCollisionRobotDistanceField().get())); -} - -void CollisionRobotHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const robot_state::RobotState& state) const -{ - crobot_distance_->checkSelfCollision(req, res, state); -} - -void CollisionRobotHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const robot_state::RobotState& state, - GroupStateRepresentationPtr& gsr) const -{ - crobot_distance_->checkSelfCollision(req, res, state, gsr); -} - -void CollisionRobotHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const robot_state::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm) const -{ - crobot_distance_->checkSelfCollision(req, res, state, acm); -} - -void CollisionRobotHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const robot_state::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const -{ - crobot_distance_->checkSelfCollision(req, res, state, acm, gsr); -} -} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/src/collision_world_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_world_distance_field.cpp deleted file mode 100644 index f9d0fb515b..0000000000 --- a/moveit_core/collision_distance_field/src/collision_world_distance_field.cpp +++ /dev/null @@ -1,568 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 the 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. - *********************************************************************/ - -/* Author: E. Gil Jones */ - -#include -#include -#include -#include -#include -#include -#include - -namespace collision_detection -{ -CollisionWorldDistanceField::~CollisionWorldDistanceField() -{ - getWorld()->removeObserver(observer_handle_); -} - -CollisionWorldDistanceField::CollisionWorldDistanceField(Eigen::Vector3d size, Eigen::Vector3d origin, - bool use_signed_distance_field, double resolution, - double collision_tolerance, double max_propogation_distance) - : CollisionWorld() - , size_(std::move(size)) - , origin_(std::move(origin)) - , use_signed_distance_field_(use_signed_distance_field) - , resolution_(resolution) - , collision_tolerance_(collision_tolerance) - , max_propogation_distance_(max_propogation_distance) -{ - distance_field_cache_entry_ = generateDistanceFieldCacheEntry(); - - // request notifications about changes to world - observer_handle_ = - getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2)); -} - -CollisionWorldDistanceField::CollisionWorldDistanceField(const WorldPtr& world, Eigen::Vector3d size, - Eigen::Vector3d origin, bool use_signed_distance_field, - double resolution, double collision_tolerance, - double max_propogation_distance) - : CollisionWorld(world) - , size_(std::move(size)) - , origin_(std::move(origin)) - , use_signed_distance_field_(use_signed_distance_field) - , resolution_(resolution) - , collision_tolerance_(collision_tolerance) - , max_propogation_distance_(max_propogation_distance) -{ - distance_field_cache_entry_ = generateDistanceFieldCacheEntry(); - - // request notifications about changes to world - observer_handle_ = - getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2)); - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -CollisionWorldDistanceField::CollisionWorldDistanceField(const CollisionWorldDistanceField& other, - const WorldPtr& world) - : CollisionWorld(other, world) -{ - size_ = other.size_; - origin_ = other.origin_; - use_signed_distance_field_ = other.use_signed_distance_field_; - resolution_ = other.resolution_; - collision_tolerance_ = other.collision_tolerance_; - max_propogation_distance_ = other.max_propogation_distance_; - distance_field_cache_entry_ = generateDistanceFieldCacheEntry(); - - // request notifications about changes to world - observer_handle_ = - getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2)); - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -void CollisionWorldDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - GroupStateRepresentationPtr gsr; - checkCollision(req, res, robot, state, gsr); -} - -void CollisionWorldDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - GroupStateRepresentationPtr& gsr) const -{ - try - { - const CollisionRobotDistanceField& cdr = dynamic_cast(robot); - if (!gsr) - { - cdr.generateCollisionCheckingStructures(req.group_name, state, nullptr, gsr, true); - } - else - { - cdr.updateGroupStateRepresentationState(state, gsr); - } - bool done = cdr.getSelfCollisions(req, res, gsr); - if (!done) - { - done = cdr.getIntraGroupCollisions(req, res, gsr); - } - if (!done) - { - getEnvironmentCollisions(req, res, distance_field_cache_entry_->distance_field_, gsr); - } - } - catch (const std::bad_cast& e) - { - ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what()); - return; - } - - (const_cast(this))->last_gsr_ = gsr; -} - -void CollisionWorldDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - GroupStateRepresentationPtr gsr; - checkCollision(req, res, robot, state, acm, gsr); -} - -void CollisionWorldDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const -{ - try - { - const CollisionRobotDistanceField& cdr = dynamic_cast(robot); - if (!gsr) - { - cdr.generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true); - } - else - { - cdr.updateGroupStateRepresentationState(state, gsr); - } - bool done = cdr.getSelfCollisions(req, res, gsr); - if (!done) - { - done = cdr.getIntraGroupCollisions(req, res, gsr); - } - if (!done) - { - getEnvironmentCollisions(req, res, distance_field_cache_entry_->distance_field_, gsr); - } - } - catch (const std::bad_cast& e) - { - ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what()); - return; - } - - (const_cast(this))->last_gsr_ = gsr; -} - -void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - GroupStateRepresentationPtr gsr; - checkRobotCollision(req, res, robot, state, gsr); -} - -void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - GroupStateRepresentationPtr& gsr) const -{ - distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_->distance_field_; - try - { - const CollisionRobotDistanceField& cdr = dynamic_cast(robot); - DistanceFieldCacheEntryConstPtr dfce; - if (!gsr) - { - cdr.generateCollisionCheckingStructures(req.group_name, state, nullptr, gsr, false); - } - else - { - cdr.updateGroupStateRepresentationState(state, gsr); - } - getEnvironmentCollisions(req, res, env_distance_field, gsr); - (const_cast(this))->last_gsr_ = gsr; - - // checkRobotCollisionHelper(req, res, robot, state, &acm); - } - catch (const std::bad_cast& e) - { - ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what()); - return; - } -} - -void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - GroupStateRepresentationPtr gsr; - checkRobotCollision(req, res, robot, state, acm, gsr); -} - -void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const -{ - distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_->distance_field_; - try - { - const CollisionRobotDistanceField& cdr = dynamic_cast(robot); - DistanceFieldCacheEntryPtr dfce; - if (!gsr) - { - cdr.generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true); - } - else - { - cdr.updateGroupStateRepresentationState(state, gsr); - } - getEnvironmentCollisions(req, res, env_distance_field, gsr); - (const_cast(this))->last_gsr_ = gsr; - - // checkRobotCollisionHelper(req, res, robot, state, &acm); - } - catch (const std::bad_cast& e) - { - ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what()); - return; - } -} - -void CollisionWorldDistanceField::getCollisionGradients(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const -{ - distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_->distance_field_; - try - { - const CollisionRobotDistanceField& cdr = dynamic_cast(robot); - if (!gsr) - { - cdr.generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true); - } - else - { - cdr.updateGroupStateRepresentationState(state, gsr); - } - cdr.getSelfProximityGradients(gsr); - cdr.getIntraGroupProximityGradients(gsr); - getEnvironmentProximityGradients(env_distance_field, gsr); - } - catch (const std::bad_cast& e) - { - ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what()); - return; - } - - (const_cast(this))->last_gsr_ = gsr; -} - -void CollisionWorldDistanceField::getAllCollisions(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const -{ - try - { - const CollisionRobotDistanceField& cdr = dynamic_cast(robot); - if (!gsr) - { - cdr.generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true); - } - else - { - cdr.updateGroupStateRepresentationState(state, gsr); - } - cdr.getSelfCollisions(req, res, gsr); - cdr.getIntraGroupCollisions(req, res, gsr); - distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_->distance_field_; - getEnvironmentCollisions(req, res, env_distance_field, gsr); - } - catch (const std::bad_cast& e) - { - ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what()); - return; - } - - (const_cast(this))->last_gsr_ = gsr; -} - -bool CollisionWorldDistanceField::getEnvironmentCollisions( - const CollisionRequest& req, CollisionResult& res, const distance_field::DistanceFieldConstPtr& env_distance_field, - GroupStateRepresentationPtr& gsr) const -{ - for (unsigned int i = 0; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); i++) - { - bool is_link = i < gsr->dfce_->link_names_.size(); - std::string link_name = i < gsr->dfce_->link_names_.size() ? gsr->dfce_->link_names_[i] : "attached"; - if (is_link && !gsr->dfce_->link_has_geometry_[i]) - { - continue; - } - - const std::vector* collision_spheres_1; - const EigenSTL::vector_Vector3d* sphere_centers_1; - - if (is_link) - { - collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres()); - sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters()); - } - else - { - collision_spheres_1 = - &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres()); - sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()); - } - - if (req.contacts) - { - std::vector colls; - bool coll = getCollisionSphereCollision( - env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, max_propogation_distance_, - collision_tolerance_, std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls); - if (coll) - { - res.collision = true; - for (unsigned int col : colls) - { - Contact con; - if (is_link) - { - con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[col]; - con.body_type_1 = BodyTypes::ROBOT_LINK; - con.body_name_1 = gsr->dfce_->link_names_[i]; - } - else - { - con.pos = gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()[col]; - con.body_type_1 = BodyTypes::ROBOT_ATTACHED; - con.body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()]; - } - - con.body_type_2 = BodyTypes::WORLD_OBJECT; - con.body_name_2 = "environment"; - res.contact_count++; - res.contacts[std::pair(con.body_name_1, con.body_name_2)].push_back(con); - gsr->gradients_[i].types[col] = ENVIRONMENT; - // ROS_DEBUG_STREAM("Link " << dfce->link_names_[i] << " sphere " << - // colls[j] << " in env collision"); - } - - gsr->gradients_[i].collision = true; - if (res.contact_count >= req.max_contacts) - { - return true; - } - } - } - else - { - bool coll = getCollisionSphereCollision(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, - max_propogation_distance_, collision_tolerance_); - if (coll) - { - res.collision = true; - return true; - } - } - } - return (res.contact_count >= req.max_contacts); -} - -bool CollisionWorldDistanceField::getEnvironmentProximityGradients( - const distance_field::DistanceFieldConstPtr& env_distance_field, GroupStateRepresentationPtr& gsr) const -{ - bool in_collision = false; - for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++) - { - bool is_link = i < gsr->dfce_->link_names_.size(); - - if (is_link && !gsr->dfce_->link_has_geometry_[i]) - { - continue; - } - - const std::vector* collision_spheres_1; - const EigenSTL::vector_Vector3d* sphere_centers_1; - if (is_link) - { - collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres()); - sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters()); - } - else - { - collision_spheres_1 = - &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres()); - sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()); - } - - bool coll = getCollisionSphereGradients(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, - gsr->gradients_[i], ENVIRONMENT, collision_tolerance_, false, - max_propogation_distance_, false); - if (coll) - { - in_collision = true; - } - } - return in_collision; -} - -void CollisionWorldDistanceField::setWorld(const WorldPtr& world) -{ - if (world == getWorld()) - return; - - // turn off notifications about old world - getWorld()->removeObserver(observer_handle_); - - // clear out objects from old world - distance_field_cache_entry_->distance_field_->reset(); - - CollisionWorld::setWorld(world); - - // request notifications about changes to new world - observer_handle_ = - getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2)); - - // get notifications any objects already in the new world - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -void CollisionWorldDistanceField::notifyObjectChange(CollisionWorldDistanceField* self, const ObjectConstPtr& obj, - World::Action action) -{ - ros::WallTime n = ros::WallTime::now(); - - EigenSTL::vector_Vector3d add_points; - EigenSTL::vector_Vector3d subtract_points; - self->updateDistanceObject(obj->id_, self->distance_field_cache_entry_, add_points, subtract_points); - - if (action == World::DESTROY) - { - self->distance_field_cache_entry_->distance_field_->removePointsFromField(subtract_points); - } - else if (action & (World::MOVE_SHAPE | World::REMOVE_SHAPE)) - { - self->distance_field_cache_entry_->distance_field_->removePointsFromField(subtract_points); - self->distance_field_cache_entry_->distance_field_->addPointsToField(add_points); - } - else - { - self->distance_field_cache_entry_->distance_field_->addPointsToField(add_points); - } - - ROS_DEBUG_NAMED("collision_distance_field", "Modifying object %s took %lf s", obj->id_.c_str(), - (ros::WallTime::now() - n).toSec()); -} - -void CollisionWorldDistanceField::updateDistanceObject(const std::string& id, DistanceFieldCacheEntryPtr& dfce, - EigenSTL::vector_Vector3d& add_points, - EigenSTL::vector_Vector3d& subtract_points) -{ - std::map>::iterator cur_it = - dfce->posed_body_point_decompositions_.find(id); - if (cur_it != dfce->posed_body_point_decompositions_.end()) - { - for (collision_detection::PosedBodyPointDecompositionPtr& posed_body_point_decomposition : cur_it->second) - { - subtract_points.insert(subtract_points.end(), posed_body_point_decomposition->getCollisionPoints().begin(), - posed_body_point_decomposition->getCollisionPoints().end()); - } - } - - World::ObjectConstPtr object = getWorld()->getObject(id); - if (object) - { - ROS_DEBUG_STREAM("Updating/Adding Object '" << object->id_ << "' with " << object->shapes_.size() - << " shapes to CollisionWorldDistanceField"); - std::vector shape_points; - for (unsigned int i = 0; i < object->shapes_.size(); i++) - { - shapes::ShapeConstPtr shape = object->shapes_[i]; - if (shape->type == shapes::OCTREE) - { - const shapes::OcTree* octree_shape = static_cast(shape.get()); - std::shared_ptr octree = octree_shape->octree; - - shape_points.push_back(std::make_shared(octree)); - } - else - { - BodyDecompositionConstPtr bd = getBodyDecompositionCacheEntry(shape, resolution_); - - shape_points.push_back(std::make_shared(bd, object->shape_poses_[i])); - } - - add_points.insert(add_points.end(), shape_points.back()->getCollisionPoints().begin(), - shape_points.back()->getCollisionPoints().end()); - } - - dfce->posed_body_point_decompositions_[id] = shape_points; - } - else - { - ROS_DEBUG_STREAM("Removing Object '" << id << "' from CollisionWorldDistanceField"); - dfce->posed_body_point_decompositions_.erase(id); - } -} - -CollisionWorldDistanceField::DistanceFieldCacheEntryPtr CollisionWorldDistanceField::generateDistanceFieldCacheEntry() -{ - DistanceFieldCacheEntryPtr dfce(new DistanceFieldCacheEntry()); - dfce->distance_field_.reset(new distance_field::PropagationDistanceField( - size_.x(), size_.y(), size_.z(), resolution_, origin_.x() - 0.5 * size_.x(), origin_.y() - 0.5 * size_.y(), - origin_.z() - 0.5 * size_.z(), max_propogation_distance_, use_signed_distance_field_)); - - EigenSTL::vector_Vector3d add_points; - EigenSTL::vector_Vector3d subtract_points; - for (const std::pair& object : *getWorld()) - { - updateDistanceObject(object.first, dfce, add_points, subtract_points); - } - dfce->distance_field_->addPointsToField(add_points); - return dfce; -} -} // namespace collision_detection - -#include -const std::string collision_detection::CollisionDetectorAllocatorDistanceField::NAME("DISTANCE_FIELD"); diff --git a/moveit_core/collision_distance_field/src/collision_world_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_world_hybrid.cpp deleted file mode 100644 index 551937cd6a..0000000000 --- a/moveit_core/collision_distance_field/src/collision_world_hybrid.cpp +++ /dev/null @@ -1,160 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 the 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. - *********************************************************************/ - -/* Author: E. Gil Jones */ - -#include -#include - -namespace collision_detection -{ -CollisionWorldHybrid::CollisionWorldHybrid(Eigen::Vector3d size, Eigen::Vector3d origin, bool use_signed_distance_field, - double resolution, double collision_tolerance, - double max_propogation_distance) - : CollisionWorldFCL() - , cworld_distance_(new collision_detection::CollisionWorldDistanceField( - getWorld(), std::move(size), std::move(origin), use_signed_distance_field, resolution, collision_tolerance, - max_propogation_distance)) - -{ -} - -CollisionWorldHybrid::CollisionWorldHybrid(const WorldPtr& world, Eigen::Vector3d size, Eigen::Vector3d origin, - bool use_signed_distance_field, double resolution, - double collision_tolerance, double max_propogation_distance) - : CollisionWorldFCL(world) - , cworld_distance_(new collision_detection::CollisionWorldDistanceField( - getWorld(), std::move(size), std::move(origin), use_signed_distance_field, resolution, collision_tolerance, - max_propogation_distance)) -{ -} - -CollisionWorldHybrid::CollisionWorldHybrid(const CollisionWorldHybrid& other, const WorldPtr& world) - : CollisionWorldFCL(other, world) - , cworld_distance_( - new collision_detection::CollisionWorldDistanceField(*other.getCollisionWorldDistanceField(), world)) -{ -} - -void CollisionWorldHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - cworld_distance_->checkCollision(req, res, robot, state); -} - -void CollisionWorldHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - GroupStateRepresentationPtr& gsr) const -{ - cworld_distance_->checkCollision(req, res, robot, state, gsr); -} - -void CollisionWorldHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - cworld_distance_->checkCollision(req, res, robot, state, acm); -} - -void CollisionWorldHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const -{ - cworld_distance_->checkCollision(req, res, robot, state, acm, gsr); -} - -void CollisionWorldHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - cworld_distance_->checkRobotCollision(req, res, robot, state); -} - -void CollisionWorldHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - GroupStateRepresentationPtr& gsr) const -{ - cworld_distance_->checkRobotCollision(req, res, robot, state, gsr); -} - -void CollisionWorldHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - cworld_distance_->checkRobotCollision(req, res, robot, state, acm); -} - -void CollisionWorldHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const -{ - cworld_distance_->checkRobotCollision(req, res, robot, state, acm, gsr); -} - -void CollisionWorldHybrid::setWorld(const WorldPtr& world) -{ - if (world == getWorld()) - return; - - cworld_distance_->setWorld(world); - CollisionWorldFCL::setWorld(world); -} - -void CollisionWorldHybrid::getCollisionGradients(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const -{ - cworld_distance_->getCollisionGradients(req, res, robot, state, acm, gsr); -} - -void CollisionWorldHybrid::getAllCollisions(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const -{ - cworld_distance_->getAllCollisions(req, res, robot, state, acm, gsr); -} -} // namespace collision_detection - -#include -const std::string collision_detection::CollisionDetectorAllocatorHybrid::NAME("HYBRID"); diff --git a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp index 19445964db..c744150492 100644 --- a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp +++ b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp @@ -38,8 +38,7 @@ #include #include #include -#include -#include +#include #include #include @@ -53,8 +52,7 @@ #include -typedef collision_detection::CollisionWorldDistanceField DefaultCWorldType; -typedef collision_detection::CollisionRobotDistanceField DefaultCRobotType; +typedef collision_detection::CollisionEnvDistanceField DefaultCEnvType; class DistanceFieldCollisionDetectionTester : public testing::Test { @@ -87,8 +85,7 @@ class DistanceFieldCollisionDetectionTester : public testing::Test acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); std::map> link_body_decompositions; - crobot_.reset(new DefaultCRobotType(robot_model_, link_body_decompositions)); - cworld_.reset(new DefaultCWorldType()); + cenv_.reset(new DefaultCEnvType(robot_model_, link_body_decompositions)); } void TearDown() override @@ -107,8 +104,7 @@ class DistanceFieldCollisionDetectionTester : public testing::Test robot_state::TransformsPtr ftf_; robot_state::TransformsConstPtr ftf_const_; - collision_detection::CollisionRobotPtr crobot_; - collision_detection::CollisionWorldPtr cworld_; + collision_detection::CollisionEnvPtr cenv_; collision_detection::AllowedCollisionMatrixPtr acm_; }; @@ -124,7 +120,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, DefaultNotInCollision) collision_detection::CollisionRequest req; collision_detection::CollisionResult res; req.group_name = "whole_body"; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_FALSE(res.collision); } @@ -139,13 +135,13 @@ TEST_F(DistanceFieldCollisionDetectionTester, ChangeTorsoPosition) collision_detection::CollisionResult res2; req.group_name = "right_arm"; - crobot_->checkSelfCollision(req, res1, robot_state, *acm_); + cenv_->checkSelfCollision(req, res1, robot_state, *acm_); std::map torso_val; torso_val["torso_lift_joint"] = .15; robot_state.setVariablePositions(torso_val); robot_state.update(); - crobot_->checkSelfCollision(req, res1, robot_state, *acm_); - crobot_->checkSelfCollision(req, res1, robot_state, *acm_); + cenv_->checkSelfCollision(req, res1, robot_state, *acm_); + cenv_->checkSelfCollision(req, res1, robot_state, *acm_); } TEST_F(DistanceFieldCollisionDetectionTester, LinksInCollision) @@ -168,18 +164,18 @@ TEST_F(DistanceFieldCollisionDetectionTester, LinksInCollision) robot_state.updateStateWithLinkAt("base_bellow_link", offset); acm_->setEntry("base_link", "base_bellow_link", false); - crobot_->checkSelfCollision(req, res1, robot_state, *acm_); + cenv_->checkSelfCollision(req, res1, robot_state, *acm_); ASSERT_TRUE(res1.collision); acm_->setEntry("base_link", "base_bellow_link", true); - crobot_->checkSelfCollision(req, res2, robot_state, *acm_); + cenv_->checkSelfCollision(req, res2, robot_state, *acm_); ASSERT_FALSE(res2.collision); robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity()); robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset); acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); - crobot_->checkSelfCollision(req, res3, robot_state, *acm_); + cenv_->checkSelfCollision(req, res3, robot_state, *acm_); ASSERT_TRUE(res3.collision); } @@ -206,7 +202,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactReporting) acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); collision_detection::CollisionResult res; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); EXPECT_EQ(res.contacts.size(), 1u); EXPECT_EQ(res.contacts.begin()->second.size(), 1u); @@ -215,7 +211,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactReporting) req.max_contacts = 2; req.max_contacts_per_pair = 1; // req.verbose = true; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); EXPECT_EQ(res.contact_count, 2u); EXPECT_EQ(res.contacts.begin()->second.size(), 1u); @@ -226,7 +222,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactReporting) req.max_contacts = 10; req.max_contacts_per_pair = 2; acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false)); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); EXPECT_LE(res.contacts.size(), 10u); EXPECT_LE(res.contact_count, 10u); @@ -254,7 +250,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactPositions) acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); collision_detection::CollisionResult res; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); ASSERT_EQ(res.contacts.size(), 1u); ASSERT_EQ(res.contacts.begin()->second.size(), 1u); @@ -272,7 +268,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactPositions) robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); collision_detection::CollisionResult res2; - crobot_->checkSelfCollision(req, res2, robot_state, *acm_); + cenv_->checkSelfCollision(req, res2, robot_state, *acm_); ASSERT_TRUE(res2.collision); ASSERT_EQ(res2.contacts.size(), 1u); ASSERT_EQ(res2.contacts.begin()->second.size(), 1u); @@ -290,7 +286,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactPositions) robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); collision_detection::CollisionResult res3; - crobot_->checkSelfCollision(req, res2, robot_state, *acm_); + cenv_->checkSelfCollision(req, res2, robot_state, *acm_); ASSERT_FALSE(res3.collision); } @@ -311,18 +307,18 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) pos1.translation().x() = 1.0; robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_FALSE(res.collision); shapes::Shape* shape = new shapes::Box(.25, .25, .25); - cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1); + cenv_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1); res = collision_detection::CollisionResult(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_); + cenv_->checkRobotCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); // deletes shape - cworld_->getWorld()->removeObject("box"); + cenv_->getWorld()->removeObject("box"); std::vector shapes; EigenSTL::vector_Isometry3d poses; @@ -336,7 +332,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) robot_state.attachBody(attached_body); res = collision_detection::CollisionResult(); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); // deletes shape @@ -350,19 +346,19 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) robot_state.attachBody(attached_body_1); res = collision_detection::CollisionResult(); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); // ASSERT_FALSE(res.collision); pos1.translation().x() = 1.01; shapes::Shape* coll = new shapes::Box(.1, .1, .1); - cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1); + cenv_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1); res = collision_detection::CollisionResult(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_); + cenv_->checkRobotCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); acm_->setEntry("coll", "r_gripper_palm_link", true); res = collision_detection::CollisionResult(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_); + cenv_->checkRobotCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); } diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index 4d15e35d63..a716e8bd86 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include @@ -831,7 +831,7 @@ class VisibilityConstraint : public KinematicConstraint */ bool decideContact(const collision_detection::Contact& contact) const; - collision_detection::CollisionRobotPtr collision_robot_; /**< \brief A copy of the collision robot maintained for + collision_detection::CollisionEnvPtr collision_env_; /**< \brief A copy of the collision robot maintained for collision checking the cone against robot links */ bool mobile_sensor_frame_; /**< \brief True if the sensor is a non-fixed frame relative to the transform frame */ bool mobile_target_frame_; /**< \brief True if the target is a non-fixed frame relative to the transform frame */ diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 526fe4e3d3..884a1dca51 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -38,8 +38,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -644,7 +643,7 @@ void OrientationConstraint::print(std::ostream& out) const } VisibilityConstraint::VisibilityConstraint(const robot_model::RobotModelConstPtr& model) - : KinematicConstraint(model), collision_robot_(new collision_detection::CollisionRobotFCL(model)) + : KinematicConstraint(model), collision_env_(new collision_detection::CollisionEnvFCL(model)) { type_ = VISIBILITY_CONSTRAINT; } @@ -984,8 +983,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot return ConstraintEvaluationResult(false, 0.0); // add the visibility cone as an object - collision_detection::CollisionWorldFCL collision_world; - collision_world.getWorld()->addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Isometry3d::Identity()); + collision_env_->getWorld()->addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Isometry3d::Identity()); // check for collisions between the robot and the cone collision_detection::CollisionRequest req; @@ -995,7 +993,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot req.contacts = true; req.verbose = verbose; req.max_contacts = 1; - collision_world.checkRobotCollision(req, res, *collision_robot_, state, acm); + collision_env_->checkRobotCollision(req, res, state, acm); if (verbose) { @@ -1005,6 +1003,8 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot res.collision ? "not " : "", ss.str().c_str()); } + collision_env_->getWorld()->removeObject("cone"); + return ConstraintEvaluationResult(!res.collision, res.collision ? res.contacts.begin()->second.front().depth : 0.0); } diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index c7954cce12..6faff3b6aa 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -293,43 +294,31 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from return world_; } - /** \brief Get the active collision detector for the world */ - const collision_detection::CollisionWorldConstPtr& getCollisionWorld() const + /** \brief Get the active collision environment */ + const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const { - // we always have a world representation after configure is called. - return active_collision_->cworld_const_; + return active_collision_->getCollisionEnv(); } /** \brief Get the active collision detector for the robot */ - const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const + const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const { - return active_collision_->getCollisionRobot(); - } - - /** \brief Get the active collision detector for the robot */ - const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const - { - return active_collision_->getCollisionRobotUnpadded(); + return active_collision_->getCollisionEnvUnpadded(); } /** \brief Get a specific collision detector for the world. If not found return active CollisionWorld. */ - const collision_detection::CollisionWorldConstPtr& - getCollisionWorld(const std::string& collision_detector_name) const; - - /** \brief Get a specific collision detector for the padded robot. If no found return active CollisionRobot. */ - const collision_detection::CollisionRobotConstPtr& - getCollisionRobot(const std::string& collision_detector_name) const; + const collision_detection::CollisionEnvConstPtr& getCollisionEnv(const std::string& collision_detector_name) const; /** \brief Get a specific collision detector for the unpadded robot. If no found return active unpadded * CollisionRobot. */ - const collision_detection::CollisionRobotConstPtr& - getCollisionRobotUnpadded(const std::string& collision_detector_name) const; + const collision_detection::CollisionEnvConstPtr& + getCollisionEnvUnpadded(const std::string& collision_detector_name) const; /** \brief Get the representation of the collision robot * This can be used to set padding and link scale on the active collision_robot. * NOTE: After modifying padding and scale on the active robot call * propogateRobotPadding() to copy it to all the other collision detectors. */ - const collision_detection::CollisionRobotPtr& getCollisionRobotNonConst(); + const collision_detection::CollisionEnvPtr& getCollisionEnvNonConst(); /** \brief Copy scale and padding from active CollisionRobot to other CollisionRobots. * This should be called after any changes are made to the scale or padding of the active @@ -501,7 +490,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from const robot_state::RobotState& robot_state) const { // do self-collision checking with the unpadded version of the robot - getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix()); + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix()); } /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given @@ -521,7 +510,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from const collision_detection::AllowedCollisionMatrix& acm) const { // do self-collision checking with the unpadded version of the robot - getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, acm); + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); } /** \brief Get the names of the links that are involved in collisions for the current state */ @@ -624,7 +613,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from */ double distanceToCollision(const robot_state::RobotState& robot_state) const { - return getCollisionWorld()->distanceRobot(*getCollisionRobot(), robot_state, getAllowedCollisionMatrix()); + return getCollisionEnv()->distanceRobot(robot_state, getAllowedCollisionMatrix()); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring @@ -639,7 +628,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from * self-collisions), if the robot has no padding */ double distanceToCollisionUnpadded(const robot_state::RobotState& robot_state) const { - return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), robot_state, getAllowedCollisionMatrix()); + return getCollisionEnvUnpadded()->distanceRobot(robot_state, getAllowedCollisionMatrix()); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring @@ -658,7 +647,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from double distanceToCollision(const robot_state::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { - return getCollisionWorld()->distanceRobot(*getCollisionRobot(), robot_state, acm); + return getCollisionEnv()->distanceRobot(robot_state, acm); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring @@ -677,7 +666,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from double distanceToCollisionUnpadded(const robot_state::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { - return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), robot_state, acm); + return getCollisionEnvUnpadded()->distanceRobot(robot_state, acm); } /**@}*/ @@ -993,23 +982,21 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from struct CollisionDetector { collision_detection::CollisionDetectorAllocatorPtr alloc_; - collision_detection::CollisionRobotPtr crobot_unpadded_; // if NULL use parent's - collision_detection::CollisionRobotConstPtr crobot_unpadded_const_; - collision_detection::CollisionRobotPtr crobot_; // if NULL use parent's - collision_detection::CollisionRobotConstPtr crobot_const_; + collision_detection::CollisionEnvPtr cenv_; // never NULL + collision_detection::CollisionEnvConstPtr cenv_const_; - collision_detection::CollisionWorldPtr cworld_; // never NULL - collision_detection::CollisionWorldConstPtr cworld_const_; + collision_detection::CollisionEnvPtr cenv_unpadded_; + collision_detection::CollisionEnvConstPtr cenv_unpadded_const_; CollisionDetectorConstPtr parent_; // may be NULL - const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const + const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const { - return crobot_const_ ? crobot_const_ : parent_->getCollisionRobot(); + return cenv_const_ ? cenv_const_ : parent_->getCollisionEnv(); } - const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const + const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const { - return crobot_unpadded_const_ ? crobot_unpadded_const_ : parent_->getCollisionRobotUnpadded(); + return cenv_unpadded_const_ ? cenv_unpadded_const_ : parent_->getCollisionEnvUnpadded(); } void findParent(const PlanningScene& scene); void copyPadding(const CollisionDetector& src); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 279f824e83..ed8a94dac9 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -201,15 +201,13 @@ PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(pare detector->alloc_ = parent_detector->alloc_; detector->parent_ = parent_detector; - detector->cworld_ = detector->alloc_->allocateWorld(parent_detector->cworld_, world_); - detector->cworld_const_ = detector->cworld_; + detector->cenv_ = detector->alloc_->allocateEnv(parent_detector->cenv_, world_); + detector->cenv_const_ = detector->cenv_; - // leave these empty and use parent collision_robot_ unless/until a non-const one + // leave these empty and use parent collision_env_ unless/until a non-const one // is requested (e.g. to modify link padding or scale) - detector->crobot_.reset(); - detector->crobot_const_.reset(); - detector->crobot_unpadded_.reset(); - detector->crobot_unpadded_const_.reset(); + detector->cenv_unpadded_.reset(); + detector->cenv_unpadded_const_.reset(); } setActiveCollisionDetector(parent_->getActiveCollisionDetectorName()); } @@ -236,21 +234,12 @@ PlanningScenePtr PlanningScene::diff(const moveit_msgs::PlanningScene& msg) cons void PlanningScene::CollisionDetector::copyPadding(const PlanningScene::CollisionDetector& src) { - if (!crobot_) - { - alloc_->allocateRobot(parent_->getCollisionRobot()); - crobot_const_ = crobot_; - } - - crobot_->setLinkPadding(src.getCollisionRobot()->getLinkPadding()); - crobot_->setLinkScale(src.getCollisionRobot()->getLinkScale()); + cenv_->setLinkPadding(src.getCollisionEnv()->getLinkPadding()); + cenv_->setLinkScale(src.getCollisionEnv()->getLinkScale()); } void PlanningScene::propogateRobotPadding() { - if (!active_collision_->crobot_) - return; - for (std::pair& it : collision_) { if (it.second != active_collision_) @@ -285,25 +274,23 @@ void PlanningScene::addCollisionDetector(const collision_detection::CollisionDet detector->findParent(*this); - detector->cworld_ = detector->alloc_->allocateWorld(world_); - detector->cworld_const_ = detector->cworld_; + detector->cenv_ = detector->alloc_->allocateEnv(world_, getRobotModel()); + detector->cenv_const_ = detector->cenv_; - // Allocate CollisionRobot unless we can use the parent's crobot_. - // If active_collision_->crobot_ is non-NULL there is local padding and we cannot use the parent's crobot_. - if (!detector->parent_ || active_collision_->crobot_) + // if the current active detector is not the added one, copy its padding to the new one and allocate unpadded + if (detector != active_collision_) { - detector->crobot_ = detector->alloc_->allocateRobot(getRobotModel()); - detector->crobot_const_ = detector->crobot_; + detector->cenv_unpadded_ = detector->alloc_->allocateEnv(world_, getRobotModel()); + detector->cenv_unpadded_const_ = detector->cenv_unpadded_; - if (detector != active_collision_) - detector->copyPadding(*active_collision_); + detector->copyPadding(*active_collision_); } - // Allocate CollisionRobot unless we can use the parent's crobot_unpadded_. + // If we don't have a parent, allocate unpadded versions, otherwise use the parent's cenv_unpadded_. if (!detector->parent_) { - detector->crobot_unpadded_ = detector->alloc_->allocateRobot(getRobotModel()); - detector->crobot_unpadded_const_ = detector->crobot_unpadded_; + detector->cenv_unpadded_ = detector->alloc_->allocateEnv(world_, getRobotModel()); + detector->cenv_unpadded_const_ = detector->cenv_unpadded_; } } @@ -357,36 +344,22 @@ void PlanningScene::getCollisionDetectorNames(std::vector& names) c names.push_back(it.first); } -const collision_detection::CollisionWorldConstPtr& -PlanningScene::getCollisionWorld(const std::string& collision_detector_name) const -{ - CollisionDetectorConstIterator it = collision_.find(collision_detector_name); - if (it == collision_.end()) - { - ROS_ERROR_NAMED(LOGNAME, "Could not get CollisionWorld named '%s'. Returning active CollisionWorld '%s' instead", - collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); - return active_collision_->cworld_const_; - } - - return it->second->cworld_const_; -} - -const collision_detection::CollisionRobotConstPtr& -PlanningScene::getCollisionRobot(const std::string& collision_detector_name) const +const collision_detection::CollisionEnvConstPtr& +PlanningScene::getCollisionEnv(const std::string& collision_detector_name) const { CollisionDetectorConstIterator it = collision_.find(collision_detector_name); if (it == collision_.end()) { ROS_ERROR_NAMED(LOGNAME, "Could not get CollisionRobot named '%s'. Returning active CollisionRobot '%s' instead", collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); - return active_collision_->getCollisionRobot(); + return active_collision_->getCollisionEnv(); } - return it->second->getCollisionRobot(); + return it->second->getCollisionEnv(); } -const collision_detection::CollisionRobotConstPtr& -PlanningScene::getCollisionRobotUnpadded(const std::string& collision_detector_name) const +const collision_detection::CollisionEnvConstPtr& +PlanningScene::getCollisionEnvUnpadded(const std::string& collision_detector_name) const { CollisionDetectorConstIterator it = collision_.find(collision_detector_name); if (it == collision_.end()) @@ -394,10 +367,10 @@ PlanningScene::getCollisionRobotUnpadded(const std::string& collision_detector_n ROS_ERROR_NAMED(LOGNAME, "Could not get CollisionRobotUnpadded named '%s'. " "Returning active CollisionRobotUnpadded '%s' instead", collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); - return active_collision_->getCollisionRobotUnpadded(); + return active_collision_->getCollisionEnvUnpadded(); } - return it->second->getCollisionRobotUnpadded(); + return it->second->getCollisionEnvUnpadded(); } void PlanningScene::clearDiffs() @@ -420,20 +393,18 @@ void PlanningScene::clearDiffs() if (it.second->parent_) { - it.second->crobot_.reset(); - it.second->crobot_const_.reset(); - it.second->crobot_unpadded_.reset(); - it.second->crobot_unpadded_const_.reset(); + it.second->cenv_unpadded_.reset(); + it.second->cenv_unpadded_const_.reset(); - it.second->cworld_ = it.second->alloc_->allocateWorld(it.second->parent_->cworld_, world_); - it.second->cworld_const_ = it.second->cworld_; + it.second->cenv_ = it.second->alloc_->allocateEnv(it.second->parent_->cenv_, world_); + it.second->cenv_const_ = it.second->cenv_; } else { it.second->copyPadding(*parent_->active_collision_); - it.second->cworld_ = it.second->alloc_->allocateWorld(world_); - it.second->cworld_const_ = it.second->cworld_; + it.second->cenv_ = it.second->alloc_->allocateEnv(it.second->parent_->cenv_, world_); + it.second->cenv_const_ = it.second->cenv_; } } @@ -471,13 +442,10 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene) if (acm_) scene->getAllowedCollisionMatrixNonConst() = *acm_; - if (active_collision_->crobot_) - { - collision_detection::CollisionRobotPtr active_crobot = scene->getCollisionRobotNonConst(); - active_crobot->setLinkPadding(active_collision_->crobot_->getLinkPadding()); - active_crobot->setLinkScale(active_collision_->crobot_->getLinkScale()); - scene->propogateRobotPadding(); - } + collision_detection::CollisionEnvPtr active_cenv = scene->getCollisionEnvNonConst(); + active_cenv->setLinkPadding(active_collision_->cenv_->getLinkPadding()); + active_cenv->setLinkScale(active_collision_->cenv_->getLinkScale()); + scene->propogateRobotPadding(); if (world_diff_) { @@ -519,12 +487,12 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest& const robot_state::RobotState& robot_state) const { // check collision with the world using the padded version - getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobot(), robot_state, getAllowedCollisionMatrix()); + getCollisionEnv()->checkRobotCollision(req, res, robot_state, getAllowedCollisionMatrix()); if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) { // do self-collision checking with the unpadded version of the robot - getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix()); + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix()); } } @@ -543,11 +511,11 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest& const collision_detection::AllowedCollisionMatrix& acm) const { // check collision with the world using the padded version - getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobot(), robot_state, acm); + getCollisionEnv()->checkRobotCollision(req, res, robot_state, acm); // do self-collision checking with the unpadded version of the robot if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) - getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, acm); + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); } void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req, @@ -565,12 +533,12 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR const collision_detection::AllowedCollisionMatrix& acm) const { // check collision with the world using the unpadded version - getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobotUnpadded(), robot_state, acm); + getCollisionEnvUnpadded()->checkRobotCollision(req, res, robot_state, acm); // do self-collision checking with the unpadded version of the robot if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) { - getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, acm); + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); } } @@ -620,15 +588,9 @@ void PlanningScene::getCollidingLinks(std::vector& links, const rob } } -const collision_detection::CollisionRobotPtr& PlanningScene::getCollisionRobotNonConst() +const collision_detection::CollisionEnvPtr& PlanningScene::getCollisionEnvNonConst() { - if (!active_collision_->crobot_) - { - active_collision_->crobot_ = - active_collision_->alloc_->allocateRobot(active_collision_->parent_->getCollisionRobot()); - active_collision_->crobot_const_ = active_collision_->crobot_; - } - return active_collision_->crobot_; + return active_collision_->cenv_; } robot_state::RobotState& PlanningScene::getCurrentStateNonConst() @@ -717,16 +679,8 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene_ms else scene_msg.allowed_collision_matrix = moveit_msgs::AllowedCollisionMatrix(); - if (active_collision_->crobot_) - { - active_collision_->crobot_->getPadding(scene_msg.link_padding); - active_collision_->crobot_->getScale(scene_msg.link_scale); - } - else - { - scene_msg.link_padding.clear(); - scene_msg.link_scale.clear(); - } + active_collision_->cenv_->getPadding(scene_msg.link_padding); + active_collision_->cenv_->getScale(scene_msg.link_scale); scene_msg.object_colors.clear(); if (object_colors_) @@ -812,7 +766,7 @@ bool PlanningScene::getCollisionObjectMsg(moveit_msgs::CollisionObject& collisio collision_obj.header.frame_id = getPlanningFrame(); collision_obj.id = ns; collision_obj.operation = moveit_msgs::CollisionObject::ADD; - collision_detection::CollisionWorld::ObjectConstPtr obj = world_->getObject(ns); + collision_detection::CollisionEnv::ObjectConstPtr obj = world_->getObject(ns); if (!obj) return false; ShapeVisitorAddToCollisionObject sv(&collision_obj); @@ -884,7 +838,7 @@ bool PlanningScene::getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const octomap.header.frame_id = getPlanningFrame(); octomap.octomap = octomap_msgs::Octomap(); - collision_detection::CollisionWorld::ObjectConstPtr map = world_->getObject(OCTOMAP_NS); + collision_detection::CollisionEnv::ObjectConstPtr map = world_->getObject(OCTOMAP_NS); if (map) { if (map->shapes_.size() == 1) @@ -924,8 +878,8 @@ void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg) c robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state); getAllowedCollisionMatrix().getMessage(scene_msg.allowed_collision_matrix); - getCollisionRobot()->getPadding(scene_msg.link_padding); - getCollisionRobot()->getScale(scene_msg.link_scale); + getCollisionEnv()->getPadding(scene_msg.link_padding); + getCollisionEnv()->getScale(scene_msg.link_scale); getObjectColorMsgs(scene_msg.object_colors); @@ -971,8 +925,8 @@ void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg, if (comp.components & moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING) { - getCollisionRobot()->getPadding(scene_msg.link_padding); - getCollisionRobot()->getScale(scene_msg.link_scale); + getCollisionEnv()->getPadding(scene_msg.link_padding); + getCollisionEnv()->getScale(scene_msg.link_scale); } if (comp.components & moveit_msgs::PlanningSceneComponents::OBJECT_COLORS) @@ -1009,7 +963,7 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const for (const std::string& id : ids) if (id != OCTOMAP_NS) { - collision_detection::CollisionWorld::ObjectConstPtr obj = world_->getObject(id); + collision_detection::CollisionEnv::ObjectConstPtr obj = world_->getObject(id); if (obj) { out << "* " << id << std::endl; @@ -1184,15 +1138,10 @@ void PlanningScene::decoupleParent() for (std::pair& it : collision_) { - if (!it.second->crobot_) - { - it.second->crobot_ = it.second->alloc_->allocateRobot(it.second->parent_->getCollisionRobot()); - it.second->crobot_const_ = it.second->crobot_; - } - if (!it.second->crobot_unpadded_) + if (!it.second->cenv_unpadded_) { - it.second->crobot_unpadded_ = it.second->alloc_->allocateRobot(it.second->parent_->getCollisionRobotUnpadded()); - it.second->crobot_unpadded_const_ = it.second->crobot_unpadded_; + it.second->cenv_unpadded_ = it.second->alloc_->allocateEnv(it.second->parent_->getCollisionEnvUnpadded(), world_); + it.second->cenv_unpadded_const_ = it.second->cenv_unpadded_; } it.second->parent_.reset(); } @@ -1265,13 +1214,8 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& sc { for (std::pair& it : collision_) { - if (!it.second->crobot_) - { - it.second->crobot_ = it.second->alloc_->allocateRobot(it.second->parent_->getCollisionRobot()); - it.second->crobot_const_ = it.second->crobot_; - } - it.second->crobot_->setPadding(scene_msg.link_padding); - it.second->crobot_->setScale(scene_msg.link_scale); + it.second->cenv_->setPadding(scene_msg.link_padding); + it.second->cenv_->setScale(scene_msg.link_scale); } } @@ -1308,13 +1252,8 @@ bool PlanningScene::setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene_ acm_.reset(new collision_detection::AllowedCollisionMatrix(scene_msg.allowed_collision_matrix)); for (std::pair& it : collision_) { - if (!it.second->crobot_) - { - it.second->crobot_ = it.second->alloc_->allocateRobot(it.second->parent_->getCollisionRobot()); - it.second->crobot_const_ = it.second->crobot_; - } - it.second->crobot_->setPadding(scene_msg.link_padding); - it.second->crobot_->setScale(scene_msg.link_scale); + it.second->cenv_->setPadding(scene_msg.link_padding); + it.second->cenv_->setScale(scene_msg.link_scale); } object_colors_.reset(new ObjectColorMap()); for (const moveit_msgs::ObjectColor& object_color : scene_msg.object_colors) @@ -1402,7 +1341,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose& map) void PlanningScene::processOctomapPtr(const std::shared_ptr& octree, const Eigen::Isometry3d& t) { - collision_detection::CollisionWorld::ObjectConstPtr map = world_->getObject(OCTOMAP_NS); + collision_detection::CollisionEnv::ObjectConstPtr map = world_->getObject(OCTOMAP_NS); if (map) { if (map->shapes_.size() == 1) @@ -1503,7 +1442,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache // TODO(felixvd): This code may be duplicated in robot_state/conversions.cpp // STEP 1.1: Get shapes and poses from existing world object or message. - collision_detection::CollisionWorld::ObjectConstPtr obj_in_world = world_->getObject(object.object.id); + collision_detection::CollisionEnv::ObjectConstPtr obj_in_world = world_->getObject(object.object.id); if (object.object.operation == moveit_msgs::CollisionObject::ADD && object.object.primitives.empty() && object.object.meshes.empty() && object.object.planes.empty()) { diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h index f98009e133..05d6c235c4 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h @@ -42,8 +42,7 @@ #include #include #include -#include -#include +#include #include #include @@ -135,8 +134,7 @@ class ChompOptimizer moveit::core::RobotState state_; moveit::core::RobotState start_state_; const moveit::core::JointModelGroup* joint_model_group_; - const collision_detection::CollisionWorldHybrid* hy_world_; - const collision_detection::CollisionRobotHybrid* hy_robot_; + const collision_detection::CollisionEnvHybrid* hy_env_; std::vector joint_costs_; collision_detection::GroupStateRepresentationPtr gsr_; diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index 6158d6f3e5..ae30b79aad 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -75,21 +75,14 @@ ChompOptimizer::ChompOptimizer(ChompTrajectory* trajectory, const planning_scene ROS_INFO_STREAM("Active collision detector is: " + planning_scene->getActiveCollisionDetectorName()); - hy_world_ = dynamic_cast( - planning_scene->getCollisionWorld(planning_scene->getActiveCollisionDetectorName()).get()); - if (!hy_world_) + hy_env_ = dynamic_cast( + planning_scene->getCollisionEnv(planning_scene->getActiveCollisionDetectorName()).get()); + if (!hy_env_) { ROS_WARN_STREAM("Could not initialize hybrid collision world from planning scene"); return; } - hy_robot_ = dynamic_cast( - planning_scene->getCollisionRobot(planning_scene->getActiveCollisionDetectorName()).get()); - if (!hy_robot_) - { - ROS_WARN_STREAM("Could not initialize hybrid collision robot from planning scene"); - return; - } initialize(); } @@ -107,8 +100,7 @@ void ChompOptimizer::initialize() collision_detection::CollisionResult res; req.group_name = planning_group_; ros::WallTime wt = ros::WallTime::now(); - hy_world_->getCollisionGradients(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), state_, - &planning_scene_->getAllowedCollisionMatrix(), gsr_); + hy_env_->getCollisionGradients(req, res, state_, &planning_scene_->getAllowedCollisionMatrix(), gsr_); ROS_INFO_STREAM("First coll check took " << (ros::WallTime::now() - wt)); num_collision_points_ = 0; for (const collision_detection::GradientInfo& gradient : gsr_->gradients_) @@ -937,8 +929,7 @@ void ChompOptimizer::performForwardKinematics() setRobotStateFromPoint(group_trajectory_, i); ros::WallTime grad = ros::WallTime::now(); - hy_world_->getCollisionGradients(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), state_, nullptr, - gsr_); + hy_env_->getCollisionGradients(req, res, state_, nullptr, gsr_); total_dur += (ros::WallTime::now() - grad); computeJointProperties(i); state_is_in_collision_[i] = false; diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h index 5158a7f49d..53b258ced3 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h @@ -50,8 +50,7 @@ #include #include #include -#include -#include +#include #include #include @@ -248,8 +247,7 @@ class EnvironmentChain3D : public DiscreteSpaceInformation std::vector > joint_motion_wrappers_; std::vector > possible_actions_; planning_models::RobotState* state_; - const collision_detection::CollisionWorldHybrid* hy_world_; - const collision_detection::CollisionRobotHybrid* hy_robot_; + const collision_detection::CollisionEnvHybrid* hy_env_; planning_models::RobotState* ::JointStateGroup* joint_state_group_; boost::shared_ptr gsr_; // boost::shared_ptr kinematics_solver_; diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp index 6e84c9ac11..5ff2829e1f 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp @@ -247,8 +247,7 @@ void EnvironmentChain3D::GetSuccs(int source_state_ID, std::vector* succ_id req.group_name = planning_group_; if (!planning_parameters_.use_standard_collision_checking_) { - hy_world_->checkCollisionDistanceField(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), state_, - gsr_); + hy_env_->checkCollisionDistanceField(req, res, *hy_env_->getCollisionRobotDistanceField().get(), state_, gsr_); } else { @@ -436,18 +435,16 @@ bool EnvironmentChain3D::setupForMotionPlan(const planning_scene::PlanningSceneC if (!planning_parameters_.use_standard_collision_checking_) { - hy_world_ = - dynamic_cast(planning_scene->getCollisionWorld().get()); - if (!hy_world_) + hy_env_ = dynamic_cast(planning_scene->getCollisionEnv().get()); + if (!hy_env_) { ROS_WARN_STREAM("Could not initialize hybrid collision world from planning scene"); mres.error_code.val = moveit_msgs::MoveItErrorCodes::COLLISION_CHECKING_UNAVAILABLE; return false; } - hy_robot_ = - dynamic_cast(planning_scene->getCollisionRobot().get()); - if (!hy_robot_) + hy_env_ = dynamic_cast(planning_scene->getCollisionEnv().get()); + if (!hy_env_) { ROS_WARN_STREAM("Could not initialize hybrid collision robot from planning scene"); mres.error_code.val = moveit_msgs::MoveItErrorCodes::COLLISION_CHECKING_UNAVAILABLE; @@ -473,8 +470,8 @@ bool EnvironmentChain3D::setupForMotionPlan(const planning_scene::PlanningSceneC req.group_name = planning_group_; if (!planning_parameters_.use_standard_collision_checking_) { - hy_world_->checkCollisionDistanceField(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), state_, - planning_scene_->getAllowedCollisionMatrix(), gsr_); + hy_env_->checkCollisionDistanceField(req, res, *hy_env_->getCollisionRobotDistanceField().get(), state_, + planning_scene_->getAllowedCollisionMatrix(), gsr_); } else { @@ -507,7 +504,7 @@ bool EnvironmentChain3D::setupForMotionPlan(const planning_scene::PlanningSceneC gsr_->dfce_->distance_field_->getZNumCells()); boost::shared_ptr world_distance_field = - hy_world_->getCollisionWorldDistanceField()->getDistanceField(); + hy_env_->getCollisionWorldDistanceField()->getDistanceField(); if (world_distance_field->getXNumCells() != gsr_->dfce_->distance_field_->getXNumCells() || world_distance_field->getYNumCells() != gsr_->dfce_->distance_field_->getYNumCells() || world_distance_field->getZNumCells() != gsr_->dfce_->distance_field_->getZNumCells()) @@ -579,8 +576,8 @@ bool EnvironmentChain3D::setupForMotionPlan(const planning_scene::PlanningSceneC goal_state.setStateValues(goal_vals); if (!planning_parameters_.use_standard_collision_checking_) { - hy_world_->checkCollisionDistanceField(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), goal_state, - planning_scene_->getAllowedCollisionMatrix(), gsr_); + hy_env_->checkCollisionDistanceField(req, res, *hy_env_->getCollisionRobotDistanceField().get(), goal_state, + planning_scene_->getAllowedCollisionMatrix(), gsr_); } else { @@ -1101,8 +1098,8 @@ bool EnvironmentChain3D::interpolateAndCollisionCheck(const std::vector collision_detection::CollisionResult res; if (!planning_parameters_.use_standard_collision_checking_) { - hy_world_->checkCollisionDistanceField(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), - interpolation_state_temp_, gsr_); + hy_env_->checkCollisionDistanceField(req, res, *hy_env_->getCollisionRobotDistanceField().get(), + interpolation_state_temp_, gsr_); } else { diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index a2138f668d..de9a628b5a 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -203,15 +203,15 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc configureCollisionMatrix(scene_); configureDefaultPadding(); - scene_->getCollisionRobotNonConst()->setPadding(default_robot_padd_); - scene_->getCollisionRobotNonConst()->setScale(default_robot_scale_); + scene_->getCollisionEnvNonConst()->setPadding(default_robot_padd_); + scene_->getCollisionEnvNonConst()->setScale(default_robot_scale_); for (const std::pair& it : default_robot_link_padd_) { - scene_->getCollisionRobotNonConst()->setLinkPadding(it.first, it.second); + scene_->getCollisionEnvNonConst()->setLinkPadding(it.first, it.second); } for (const std::pair& it : default_robot_link_scale_) { - scene_->getCollisionRobotNonConst()->setLinkScale(it.first, it.second); + scene_->getCollisionEnvNonConst()->setLinkScale(it.first, it.second); } scene_->propogateRobotPadding(); } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index c212905f5f..e9a7398a90 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -314,7 +314,7 @@ private Q_SLOTS: ros::Publisher planning_scene_publisher_; ros::Publisher planning_scene_world_publisher_; - collision_detection::CollisionWorld::ObjectConstPtr scaled_object_; + collision_detection::CollisionEnv::ObjectConstPtr scaled_object_; std::vector > known_collision_objects_; long unsigned int known_collision_objects_version_; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 9914f945de..81b7e43565 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -156,7 +156,7 @@ void MotionPlanningFrame::removeObjectButtonClicked() } } -static QString decideStatusText(const collision_detection::CollisionWorld::ObjectConstPtr& obj) +static QString decideStatusText(const collision_detection::CollisionEnv::ObjectConstPtr& obj) { QString status_text = "'" + QString::fromStdString(obj->id_) + "' is a collision object with "; if (obj->shapes_.empty()) @@ -228,7 +228,7 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() Eigen::Isometry3d obj_pose; { const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); - const collision_detection::CollisionWorld::ObjectConstPtr& obj = + const collision_detection::CollisionEnv::ObjectConstPtr& obj = ps->getWorld()->getObject(sel[0]->text().toStdString()); if (obj) { @@ -302,7 +302,7 @@ void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position) planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); if (ps) { - collision_detection::CollisionWorld::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString()); + collision_detection::CollisionEnv::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString()); if (obj && obj->shapes_.size() == 1) { Eigen::Isometry3d p; @@ -392,7 +392,7 @@ void MotionPlanningFrame::copySelectedCollisionObject() for (const QListWidgetItem* item : sel) { std::string name = item->text().toStdString(); - collision_detection::CollisionWorld::ObjectConstPtr obj = ps->getWorld()->getObject(name); + collision_detection::CollisionEnv::ObjectConstPtr obj = ps->getWorld()->getObject(name); if (!obj) continue; @@ -689,7 +689,7 @@ void MotionPlanningFrame::createSceneInteractiveMarker() if (!ps) return; - const collision_detection::CollisionWorld::ObjectConstPtr& obj = + const collision_detection::CollisionEnv::ObjectConstPtr& obj = ps->getWorld()->getObject(sel[0]->text().toStdString()); if (obj && obj->shapes_.size() == 1) { @@ -755,7 +755,7 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item) if (item->checkState() == Qt::Unchecked) { planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); - collision_detection::CollisionWorld::ObjectConstPtr obj = + collision_detection::CollisionEnv::ObjectConstPtr obj = ps->getWorld()->getObject(known_collision_objects_[item->type()].first); if (obj) { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp index 85ba49b3e3..4ec1d4aa4e 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp @@ -90,7 +90,7 @@ void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningScen const std::vector& ids = scene->getWorld()->getObjectIds(); for (const std::string& id : ids) { - collision_detection::CollisionWorld::ObjectConstPtr object = scene->getWorld()->getObject(id); + collision_detection::CollisionEnv::ObjectConstPtr object = scene->getWorld()->getObject(id); rviz::Color color = default_env_color; float alpha = default_scene_alpha; if (scene->hasObjectColor(id)) From 39171c81f53a5ef8253c8e319cb760a0cc4727ce Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 21 Aug 2019 19:10:52 +0200 Subject: [PATCH 045/612] Cleanup launch + config files (#1631) --- moveit_ros/benchmarks/examples/demo1.yaml | 2 +- .../examples/demo_panda_all_planners.launch | 27 ++++++++++--------- .../examples/demo_panda_all_planners.yaml | 1 - .../demo_panda_all_planners_obstacles.launch | 26 +++++++++--------- 4 files changed, 29 insertions(+), 27 deletions(-) diff --git a/moveit_ros/benchmarks/examples/demo1.yaml b/moveit_ros/benchmarks/examples/demo1.yaml index 7d6e9882a2..d31a58594a 100644 --- a/moveit_ros/benchmarks/examples/demo1.yaml +++ b/moveit_ros/benchmarks/examples/demo1.yaml @@ -1,5 +1,5 @@ # This is an example configuration that loads the "Kitchen" scene from the -# local MoveIt warehouse and benchmarks the "manipulator" group in the Pick1 +# local MoveIt warehouse and benchmarks the "panda_arm" group in the Pick1 # query using the Start1 initial state (all pre-stored in the local warehouse) # Three different planners from OMPL are run a total of 50 times each, with a diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch b/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch index f0d60f6105..21b2ad58a8 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch @@ -7,25 +7,26 @@ - - - - + + - - - + + + + + - - - + + + - - + + + + - diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml b/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml index 3fc13adebc..5a101151f9 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml @@ -30,4 +30,3 @@ benchmark_config: - name: stomp planners: - STOMP - diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch b/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch index 25b0a50715..763d16e86a 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch @@ -7,21 +7,23 @@ - - - - + + - - - + + + + + - - - + + + - - + + + + From 06d67ab119fd7fda0e45db8a59a7554fa7d55bbb Mon Sep 17 00:00:00 2001 From: Jens P Date: Wed, 21 Aug 2019 23:04:49 +0200 Subject: [PATCH 046/612] Fix clang-tidy for unified collision environment (#1638) --- .../collision_env_distance_field.h | 4 ++-- .../collision_distance_field/collision_env_hybrid.h | 4 ++-- .../src/collision_env_distance_field.cpp | 4 ++-- .../src/collision_env_hybrid.cpp | 12 ++++++------ 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h index f9fb4366ad..93ec59597b 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h @@ -64,7 +64,7 @@ class CollisionEnvDistanceField : public CollisionEnv const std::map>& link_body_decompositions = std::map>(), double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, - double size_z = DEFAULT_SIZE_Z, Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), + double size_z = DEFAULT_SIZE_Z, const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, @@ -75,7 +75,7 @@ class CollisionEnvDistanceField : public CollisionEnv const std::map>& link_body_decompositions = std::map>(), double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, - double size_z = DEFAULT_SIZE_Z, Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), + double size_z = DEFAULT_SIZE_Z, const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h index 784623385d..6acf5da2ae 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h @@ -56,7 +56,7 @@ class CollisionEnvHybrid : public collision_detection::CollisionEnvFCL const std::map>& link_body_decompositions = std::map>(), double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, - Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), + const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, @@ -66,7 +66,7 @@ class CollisionEnvHybrid : public collision_detection::CollisionEnvFCL const std::map>& link_body_decompositions = std::map>(), double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, - Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), + const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index 0c1cc66d9f..6ddac01a3f 100644 --- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -55,7 +55,7 @@ const std::string collision_detection::CollisionDetectorAllocatorDistanceField:: CollisionEnvDistanceField::CollisionEnvDistanceField( const robot_model::RobotModelConstPtr& robot_model, const std::map>& link_body_decompositions, double size_x, double size_y, - double size_z, Eigen::Vector3d origin, bool use_signed_distance_field, double resolution, + double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double padding, double scale) : CollisionEnv(robot_model) { @@ -73,7 +73,7 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( CollisionEnvDistanceField::CollisionEnvDistanceField( const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, const std::map>& link_body_decompositions, double size_x, double size_y, - double size_z, Eigen::Vector3d origin, bool use_signed_distance_field, double resolution, + double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double padding, double scale) : CollisionEnv(robot_model, world, padding, scale) { diff --git a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp index 9c7928112d..7a367c012f 100644 --- a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp @@ -45,24 +45,24 @@ const std::string collision_detection::CollisionDetectorAllocatorHybrid::NAME("H CollisionEnvHybrid::CollisionEnvHybrid( const robot_model::RobotModelConstPtr& robot_model, const std::map>& link_body_decompositions, double size_x, double size_y, - double size_z, Eigen::Vector3d origin, bool use_signed_distance_field, double resolution, + double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double padding, double scale) : CollisionEnvFCL(robot_model) , cenv_distance_(new collision_detection::CollisionEnvDistanceField( - robot_model, getWorld(), link_body_decompositions, size_x, size_y, size_z, std::move(origin), - use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance, padding, scale)) + robot_model, getWorld(), link_body_decompositions, size_x, size_y, size_z, origin, use_signed_distance_field, + resolution, collision_tolerance, max_propogation_distance, padding, scale)) { } CollisionEnvHybrid::CollisionEnvHybrid( const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, const std::map>& link_body_decompositions, double size_x, double size_y, - double size_z, Eigen::Vector3d origin, bool use_signed_distance_field, double resolution, + double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double padding, double scale) : CollisionEnvFCL(robot_model, world, padding, scale) , cenv_distance_(new collision_detection::CollisionEnvDistanceField( - robot_model, getWorld(), link_body_decompositions, size_x, size_y, size_z, std::move(origin), - use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance, padding, scale)) + robot_model, getWorld(), link_body_decompositions, size_x, size_y, size_z, origin, use_signed_distance_field, + resolution, collision_tolerance, max_propogation_distance, padding, scale)) { } From c384470cbe67c3647a4e30ef65af77c2095d4a0a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 22 Aug 2019 15:47:55 +0200 Subject: [PATCH 047/612] Fix clang-tidy issues in merge commit 180e2b56db095cb659dc95c7ccfaa8aeea546371 --- .../src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp b/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp index 5c6bdd4c51..6675564c6c 100644 --- a/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp +++ b/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp @@ -50,12 +50,12 @@ constexpr char LOGNAME[] = "combine_predefined_poses_benchmark"; class CombinePredefinedPosesBenchmark : public BenchmarkExecutor { public: - virtual bool loadBenchmarkQueryData(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg, + bool loadBenchmarkQueryData(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg, std::vector& start_states, std::vector& path_constraints, std::vector& goal_constraints, std::vector& traj_constraints, - std::vector& queries) + std::vector& queries) override { // Load planning scene if (!psm_) From d6c2236653e8cbac1c1eb485ee241acb1668cf57 Mon Sep 17 00:00:00 2001 From: Jens P Date: Thu, 22 Aug 2019 19:33:58 +0200 Subject: [PATCH 048/612] Changed migration notes for new unified collision environment. (#1639) --- MIGRATION.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MIGRATION.md b/MIGRATION.md index 7fe1098d53..fb4a5c16be 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -8,7 +8,7 @@ API changes in MoveIt releases - Moved the example package `moveit_controller_manager_example` into [moveit_tutorials](https://github.com/ros-planning/moveit_tutorials) - Requests to `get_planning_scene` service without explicitly setting "components" now return full scene - `moveit_ros_plannning` no longer depends on `moveit_ros_perception` -- `collision_robot` and `collision_world` are combined into a single `collision_env`. This applies for all derived collision checkers as `FCL`, `ALL_VALID`, `HYBRID` and `DISTANCE_FIELD`. `getCollisionRobot[Unpadded] / getCollisionWorld` functions return the new combined environment and will be deprecated in the future. +- `CollisionRobot` and `CollisionWorld` are combined into a single `CollisionEnv` class. This applies for all derived collision checkers as `FCL`, `ALL_VALID`, `HYBRID` and `DISTANCE_FIELD`. Consequently, `getCollisionRobot[Unpadded] / getCollisionWorld` functions are replaced through a `getCollisionEnv` in the planning scene and return the new combined environment. This unified collision environment provides the union of all member functions of `CollisionRobot` and `CollisionWorld`. ## ROS Melodic From b7b4197d79328e04111647004fbae1b7483a090e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 22 Aug 2019 17:27:40 +0200 Subject: [PATCH 049/612] fix clang-format --- .../CombinePredefinedPosesBenchmark.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp b/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp index 6675564c6c..1bd9818579 100644 --- a/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp +++ b/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp @@ -51,11 +51,10 @@ class CombinePredefinedPosesBenchmark : public BenchmarkExecutor { public: bool loadBenchmarkQueryData(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg, - std::vector& start_states, - std::vector& path_constraints, - std::vector& goal_constraints, - std::vector& traj_constraints, - std::vector& queries) override + std::vector& start_states, std::vector& path_constraints, + std::vector& goal_constraints, + std::vector& traj_constraints, + std::vector& queries) override { // Load planning scene if (!psm_) From e6d0f3442c39e2b6948f443ccd2e93be190c4a61 Mon Sep 17 00:00:00 2001 From: Sean Yen Date: Thu, 22 Aug 2019 10:44:55 -0700 Subject: [PATCH 050/612] fallback to using pyreadline for Window. (#1635) --- moveit_commander/bin/moveit_commander_cmdline.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/moveit_commander/bin/moveit_commander_cmdline.py b/moveit_commander/bin/moveit_commander_cmdline.py index 2dffaa7630..61a37e8898 100755 --- a/moveit_commander/bin/moveit_commander_cmdline.py +++ b/moveit_commander/bin/moveit_commander_cmdline.py @@ -4,7 +4,10 @@ import roslib import rospy -import readline +try: + import readline +except ImportError: + import pyreadline as readline # for Windows import sys import os import signal From 48c326a0badc11a54c5ab2a441436c2485ee423c Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Tue, 27 Aug 2019 14:56:41 +0300 Subject: [PATCH 051/612] Handle the updated plan() function of MoveGroupCommander (#1640) --- moveit_commander/src/moveit_commander/interpreter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_commander/src/moveit_commander/interpreter.py b/moveit_commander/src/moveit_commander/interpreter.py index 3505e6deb5..02cc10e4d4 100644 --- a/moveit_commander/src/moveit_commander/interpreter.py +++ b/moveit_commander/src/moveit_commander/interpreter.py @@ -336,11 +336,11 @@ def execute_group_command(self, g, cmdorig): if clist[1] == "rand" or clist[1] == "random": vals = g.get_random_joint_values() g.set_joint_value_target(vals) - self._last_plan = g.plan() + self._last_plan = g.plan()[1] # The trajectory msg else: try: g.set_named_target(clist[1]) - self._last_plan = g.plan() + self._last_plan = g.plan()[1] # The trajectory msg except MoveItCommanderException as e: return (MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e)) except: From e2bfd7198b2c645a4baeba3f6fffb336f4399600 Mon Sep 17 00:00:00 2001 From: Sean Yen Date: Tue, 27 Aug 2019 06:21:21 -0700 Subject: [PATCH 052/612] Use `.pyd` as the output suffix for Python module on Windows. (#1637) There is no other standard way to mark this in cmake --- .../planning_interface/move_group_interface/CMakeLists.txt | 3 +++ .../planning_interface/planning_scene_interface/CMakeLists.txt | 3 +++ moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt | 3 +++ moveit_ros/planning_interface/robot_interface/CMakeLists.txt | 3 +++ 4 files changed, 12 insertions(+) diff --git a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt index 6ff6444e7a..b9a4eeaa01 100644 --- a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt @@ -11,6 +11,9 @@ add_dependencies(${MOVEIT_LIB_NAME}_python ${catkin_EXPORTED_TARGETS}) set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_move_group_interface PREFIX "") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") +if(WIN32) + set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES SUFFIX .pyd) +endif(WIN32) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt b/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt index 11489f296c..6a3b1a3f9b 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt @@ -9,6 +9,9 @@ set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_ target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_planning_scene_interface PREFIX "") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") +if(WIN32) + set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES SUFFIX .pyd) +endif(WIN32) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt b/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt index 1f3cb1c93b..1f82d7e4dc 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt +++ b/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt @@ -14,6 +14,9 @@ target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${PYTHON_LIBR set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_roscpp_initializer PREFIX "") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") +if(WIN32) + set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES SUFFIX .pyd) +endif(WIN32) install(TARGETS ${MOVEIT_LIB_NAME}_python DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) diff --git a/moveit_ros/planning_interface/robot_interface/CMakeLists.txt b/moveit_ros/planning_interface/robot_interface/CMakeLists.txt index 0c6a4a22bf..3944155fbf 100644 --- a/moveit_ros/planning_interface/robot_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/robot_interface/CMakeLists.txt @@ -6,6 +6,9 @@ target_link_libraries(${MOVEIT_LIB_NAME}_python ${PYTHON_LIBRARIES} ${catkin_LIB set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_robot_interface PREFIX "") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") +if(WIN32) + set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES SUFFIX .pyd) +endif(WIN32) install(TARGETS ${MOVEIT_LIB_NAME}_python DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) From 57d4a4acd20bd6c371faf956acbba4281a31e3fe Mon Sep 17 00:00:00 2001 From: Paul Verhoeckx <43780894+PaulRuvu@users.noreply.github.com> Date: Tue, 27 Aug 2019 16:38:09 +0200 Subject: [PATCH 053/612] jog_arm: Do not wait for command msg to start spinning (#1603) * Do not wait for a message on the cartesian_command_in_topic to start spinning, but only for the message we try to smooth with the low-pass filter (JointState). This also allows the initial command to be of the JointJog type. * Do not assume the command to be Cartesian when the zero_joint_cmd_flag is set, could be a JointJog command with zero velocity as well. * Remove unused publish_delay parameter * Remove mutex from jointJogCalcs() declaration --- .../jog_arm/include/jog_arm/jog_arm_data.h | 2 +- .../jog_arm/include/jog_arm/jog_calcs.h | 5 +- .../jog_arm/src/jog_arm/jog_calcs.cpp | 82 ++++++++----------- .../jog_arm/src/jog_arm/jog_ros_interface.cpp | 3 - .../test/arm_setup/config/jog_settings.yaml | 1 - 5 files changed, 37 insertions(+), 56 deletions(-) diff --git a/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h b/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h index cc8309dd62..499166c427 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h +++ b/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h @@ -86,7 +86,7 @@ struct JogArmParameters std::string move_group_name, joint_topic, cartesian_command_in_topic, command_frame, command_out_topic, planning_frame, warning_topic, joint_command_in_topic, command_in_type, command_out_type; double linear_scale, rotational_scale, joint_scale, lower_singularity_threshold, hard_stop_singularity_threshold, - collision_proximity_threshold, low_pass_filter_coeff, publish_period, publish_delay, incoming_command_timeout, + collision_proximity_threshold, low_pass_filter_coeff, publish_period, incoming_command_timeout, joint_limit_margin, collision_check_rate; int num_halt_msgs_to_publish; bool use_gazebo, check_collisions, publish_joint_positions, publish_joint_velocities, publish_joint_accelerations; diff --git a/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h b/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h index f9ba6da0c9..fcdafa529b 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h +++ b/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h @@ -62,7 +62,7 @@ class JogCalcs bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables, pthread_mutex_t& mutex); - bool jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables, pthread_mutex_t& mutex); + bool jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables); // Parse the incoming joint msg for the joints of our MoveGroup bool updateJoints(); @@ -97,8 +97,7 @@ class JogCalcs trajectory_msgs::JointTrajectory& new_jt_traj, const Eigen::VectorXd& delta_theta, double singularity_scale); - trajectory_msgs::JointTrajectory composeOutgoingMessage(sensor_msgs::JointState& joint_state, - const ros::Time& stamp) const; + trajectory_msgs::JointTrajectory composeOutgoingMessage(sensor_msgs::JointState& joint_state) const; void lowPassFilterVelocities(const Eigen::VectorXd& joint_vel); diff --git a/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp b/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp index 64bd635bf2..8db2351350 100644 --- a/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp +++ b/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp @@ -66,10 +66,6 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari ros::topic::waitForMessage(parameters_.joint_topic); ROS_INFO_NAMED(LOGNAME, "Received first joint msg."); - ROS_INFO_NAMED(LOGNAME, "Waiting for first command msg."); - ros::topic::waitForMessage(parameters_.cartesian_command_in_topic); - ROS_INFO_NAMED(LOGNAME, "Received first command msg."); - resetVelocityFilters(); jt_state_.name = joint_model_group_->getVariableNames(); @@ -128,8 +124,14 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari pthread_mutex_lock(&mutex); bool zero_cartesian_cmd_flag = shared_variables.zero_cartesian_cmd_flag; bool zero_joint_cmd_flag = shared_variables.zero_joint_cmd_flag; + pthread_mutex_unlock(&mutex); + + if (zero_cartesian_cmd_flag && zero_joint_cmd_flag) + // Reset low-pass filters + resetVelocityFilters(); // Pull data from the shared variables. + pthread_mutex_lock(&mutex); incoming_jts_ = shared_variables.joints; pthread_mutex_unlock(&mutex); @@ -142,8 +144,8 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari ros::Duration(WHILE_LOOP_WAIT).sleep(); } - // Assume cartesian jogging command unless the joint command has some nonzero values - if (zero_joint_cmd_flag) + // Prioritize cartesian jogging above joint jogging + if (!zero_cartesian_cmd_flag) { pthread_mutex_lock(&mutex); cartesian_deltas = shared_variables.command_deltas; @@ -152,32 +154,41 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari if (!cartesianJogCalcs(cartesian_deltas, shared_variables, mutex)) continue; } - // If joint jogging command is not all zeros - else + else if (!zero_joint_cmd_flag) { pthread_mutex_lock(&mutex); joint_deltas = shared_variables.joint_command_deltas; pthread_mutex_unlock(&mutex); - if (!jointJogCalcs(joint_deltas, shared_variables, mutex)) + if (!jointJogCalcs(joint_deltas, shared_variables)) continue; } + else + { + original_jt_state_ = jt_state_; + outgoing_command_ = composeOutgoingMessage(jt_state_); + } - // Skip publishing if the command is stale and outgoing velocities are all nearly zero. - // (It takes some time for the low-pass velocity filters to settle at zero.) + // Halt if the command is stale or inputs are all zero, or commands were zero pthread_mutex_lock(&mutex); - bool skip_publication = - shared_variables.command_is_stale && - (std::all_of(outgoing_command_.points[0].velocities.begin(), outgoing_command_.points[0].velocities.end(), - [](double i) { return i < 0.000001; })); + bool stale_command = shared_variables.command_is_stale; pthread_mutex_unlock(&mutex); + if (stale_command || (zero_cartesian_cmd_flag && zero_joint_cmd_flag)) + { + suddenHalt(outgoing_command_); + zero_cartesian_cmd_flag = true; + zero_joint_cmd_flag = true; + } + + bool valid_nonzero_command = !zero_cartesian_cmd_flag || !zero_joint_cmd_flag; + // Send the newest target joints if (!outgoing_command_.joint_names.empty()) { pthread_mutex_lock(&mutex); - // If everything normal, share the new traj to be published. - if (!skip_publication) + // If everything normal, share the new traj to be published + if (valid_nonzero_command) { shared_variables.outgoing_command = outgoing_command_; shared_variables.ok_to_publish = true; @@ -191,7 +202,7 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari // Reset the velocity filters so robot doesn't jump when commands resume resetVelocityFilters(); } - // The command is stale but we are publishing num_halt_msgs_to_publish + // The command is invalid but we are publishing num_halt_msgs_to_publish else { shared_variables.outgoing_command = outgoing_command_; @@ -298,22 +309,10 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& // Include a velocity estimate for velocity-controlled robots Eigen::VectorXd joint_vel(delta_theta_ / parameters_.publish_period); - // Smoothly halt if the command is stale - // Positions are set to original positions, and velocities are set to zero. - // The filters will be applied after this. - pthread_mutex_lock(&mutex); - if (shared_variables.command_is_stale) - { - joint_vel.setZero(); - jt_state_ = original_jt_state_; - } - pthread_mutex_unlock(&mutex); - lowPassFilterVelocities(joint_vel); lowPassFilterPositions(); - const ros::Time next_time = ros::Time::now() + ros::Duration(parameters_.publish_period); - outgoing_command_ = composeOutgoingMessage(jt_state_, next_time); + outgoing_command_ = composeOutgoingMessage(jt_state_); // If close to a collision or a singularity, decelerate applyVelocityScaling(shared_variables, mutex, outgoing_command_, delta_theta_, @@ -336,7 +335,7 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& return true; } -bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables, pthread_mutex_t& mutex) +bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables) { // Check for nan's or |delta|>1 in the incoming command for (double velocity : cmd.velocities) @@ -360,25 +359,13 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& sh // Include a velocity estimate for velocity-controlled robots Eigen::VectorXd joint_vel(delta / parameters_.publish_period); - // Smoothly halt if the command is stale - // Positions are set to original positions, and velocities are set to zero. - // The filters will be applied after this. - pthread_mutex_lock(&mutex); - if (shared_variables.command_is_stale) - { - joint_vel.setZero(); - jt_state_ = original_jt_state_; - } - pthread_mutex_unlock(&mutex); - lowPassFilterVelocities(joint_vel); lowPassFilterPositions(); // update joint state with new values kinematic_state_->setVariableValues(jt_state_); - const ros::Time next_time = ros::Time::now() + ros::Duration(parameters_.publish_delay); - outgoing_command_ = composeOutgoingMessage(jt_state_, next_time); + outgoing_command_ = composeOutgoingMessage(jt_state_); // check if new joint state is valid if (!checkIfJointsWithinURDFBounds(outgoing_command_)) @@ -445,12 +432,11 @@ void JogCalcs::lowPassFilterVelocities(const Eigen::VectorXd& joint_vel) } } -trajectory_msgs::JointTrajectory JogCalcs::composeOutgoingMessage(sensor_msgs::JointState& joint_state, - const ros::Time& stamp) const +trajectory_msgs::JointTrajectory JogCalcs::composeOutgoingMessage(sensor_msgs::JointState& joint_state) const { trajectory_msgs::JointTrajectory new_jt_traj; new_jt_traj.header.frame_id = parameters_.planning_frame; - new_jt_traj.header.stamp = stamp; + new_jt_traj.header.stamp = ros::Time::now(); new_jt_traj.joint_names = joint_state.name; trajectory_msgs::JointTrajectoryPoint point; diff --git a/moveit_experimental/jog_arm/src/jog_arm/jog_ros_interface.cpp b/moveit_experimental/jog_arm/src/jog_arm/jog_ros_interface.cpp index 76315b79fb..1114179b52 100644 --- a/moveit_experimental/jog_arm/src/jog_arm/jog_ros_interface.cpp +++ b/moveit_experimental/jog_arm/src/jog_arm/jog_ros_interface.cpp @@ -87,8 +87,6 @@ JogROSInterface::JogROSInterface() // Wait for incoming topics to appear ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); ros::topic::waitForMessage(ros_parameters_.joint_topic); - ROS_DEBUG_NAMED(LOGNAME, "Waiting for Cartesian command topic"); - ros::topic::waitForMessage(ros_parameters_.cartesian_command_in_topic); // Wait for low pass filters to stabilize ROS_INFO_STREAM_NAMED(LOGNAME, "Waiting for low-pass filters to stabilize."); @@ -251,7 +249,6 @@ bool JogROSInterface::readParameters(ros::NodeHandle& n) } error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_period", ros_parameters_.publish_period); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_delay", ros_parameters_.publish_delay); error += !rosparam_shortcuts::get("", n, parameter_ns + "/collision_check_rate", ros_parameters_.collision_check_rate); error += !rosparam_shortcuts::get("", n, parameter_ns + "/num_halt_msgs_to_publish", diff --git a/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml b/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml index d9e3ff0db3..c33b0ab748 100644 --- a/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml +++ b/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml @@ -17,7 +17,6 @@ collision_proximity_threshold: 0.03 # Start decelerating when a collision is thi planning_frame: base_link # The MoveIt planning frame. Often 'base_link' low_pass_filter_coeff: 2. # Larger-> more smoothing to jog commands, but more lag. publish_period: 0.01 # 1/Nominal publish rate [seconds] -publish_delay: 0.005 # delay between calculation and execution start of command collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. num_halt_msgs_to_publish: 1 # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish # Publish boolean warnings to this topic From 0ef84b85430a514fb9c095b8085954b582e2a38c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Thu, 29 Aug 2019 17:04:25 +0200 Subject: [PATCH 054/612] Cleanup OMPL dynamic reconfigure config (#1649) * reduce minimum number of waypoints in solution to 2 There is no real reason to specify 10 here by default. So instead, allow for the bare minimum of waypoints if simple interpolation from start to end succeeds and the maximum segment length suffices. Fixes #19. * ompl dyn reconfig: clean wrongly specified levels The levels are treated as bitmasks in the reconfigure callback, so counting up in natural numbers does not serve any purpose. The respective callback ignores the values either way, so this change just simplifies reading the config. --- .../ompl/ompl_interface/cfg/OMPLDynamicReconfigure.cfg | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/cfg/OMPLDynamicReconfigure.cfg b/moveit_planners/ompl/ompl_interface/cfg/OMPLDynamicReconfigure.cfg index 50757b73df..e9db604e9d 100755 --- a/moveit_planners/ompl/ompl_interface/cfg/OMPLDynamicReconfigure.cfg +++ b/moveit_planners/ompl/ompl_interface/cfg/OMPLDynamicReconfigure.cfg @@ -4,10 +4,10 @@ PACKAGE = "moveit_planners_ompl" from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("simplify_solutions", bool_t, 1, "Flag indicating whether computed motion plans are also simplified", True) -gen.add("minimum_waypoint_count", int_t, 2, "Set the minimum number of waypoints to include in a motion plan", 10, 2, 10000) -gen.add("maximum_waypoint_distance", double_t, 3, "The maximum distance between consecutive waypoints along the solution path (0.0 means 'ignore')", 0.0, 0.0, 50.0) -gen.add("link_for_exploration_tree", str_t, 4, "Show the exploration tree for a particular link", "") -gen.add("display_random_valid_states", bool_t, 5, "Flag indicating whether random valid states are to be published", False) +gen.add("simplify_solutions", bool_t, 0, "Flag indicating whether computed motion plans are also simplified", True) +gen.add("minimum_waypoint_count", int_t, 0, "Set the minimum number of waypoints to include in a motion plan", 2, 2, 10000) +gen.add("maximum_waypoint_distance", double_t, 0, "The maximum distance between consecutive waypoints along the solution path (0.0 means 'ignore')", 0.0, 0.0, 50.0) +gen.add("link_for_exploration_tree", str_t, 0, "Show the exploration tree for a particular link", "") +gen.add("display_random_valid_states", bool_t, 0, "Flag indicating whether random valid states are to be published", False) exit(gen.generate(PACKAGE, PACKAGE, "OMPLDynamicReconfigure")) From a9f232331a8d6f7f9f6d69ab30f44672334906a2 Mon Sep 17 00:00:00 2001 From: Sean Yen Date: Fri, 30 Aug 2019 01:46:30 -0700 Subject: [PATCH 055/612] [windows] some more header inclusion and MSVC build error fixes. (#1636) * Fix header inclusion for Windows build. * replace random() with c++11 usage. --- .../chomp/chomp_motion_planner/src/chomp_optimizer.cpp | 7 +++++-- moveit_ros/benchmarks/src/BenchmarkExecutor.cpp | 4 ++++ .../include/moveit/occupancy_map_monitor/occupancy_map.h | 1 + .../include/moveit/point_containment_filter/shape_mask.h | 2 +- moveit_ros/planning/plan_execution/src/plan_execution.cpp | 2 +- moveit_ros/planning/rdf_loader/src/rdf_loader.cpp | 5 +++++ moveit_setup_assistant/src/widgets/start_screen_widget.cpp | 7 +++++++ 7 files changed, 24 insertions(+), 4 deletions(-) diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index ae30b79aad..bd8b655928 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -43,12 +43,15 @@ #include #include #include +#include namespace chomp { double getRandomDouble() { - return ((double)random() / (double)RAND_MAX); + std::default_random_engine seed; + std::uniform_real_distribution<> uniform(0.0, 1.0); + return uniform(seed); } ChompOptimizer::ChompOptimizer(ChompTrajectory* trajectory, const planning_scene::PlanningSceneConstPtr& planning_scene, @@ -607,7 +610,7 @@ void ChompOptimizer::calculateCollisionIncrements() // This is faster and guaranteed to converge, but it may take more iterations in the worst case. if (parameters_->use_stochastic_descent_) { - start_point = (int)(((double)random() / (double)RAND_MAX) * (free_vars_end_ - free_vars_start_) + free_vars_start_); + start_point = (int)(getRandomDouble() * (free_vars_end_ - free_vars_start_) + free_vars_start_); if (start_point < free_vars_start_) start_point = free_vars_start_; if (start_point > free_vars_end_) diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 8e15bad1c1..1f55bf7102 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -44,7 +44,11 @@ #include #include #include +#ifndef _WIN32 #include +#else +#include +#endif using namespace moveit_ros_benchmarks; diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h index 70413b3d2c..ee98f95de5 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h @@ -37,6 +37,7 @@ #pragma once #include +#include #include #include #include diff --git a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h index 2fdf49c7b7..67b1b01c7f 100644 --- a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h +++ b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h @@ -105,7 +105,7 @@ class ShapeMask struct SortBodies { - bool operator()(const SeeShape& b1, const SeeShape& b2) + bool operator()(const SeeShape& b1, const SeeShape& b2) const { if (b1.volume > b2.volume) return true; diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index e3f9bb84a9..fb8ee5fae1 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -517,7 +517,7 @@ void plan_execution::PlanExecution::successfulTrajectorySegmentExecution(const E if (index < plan->plan_components_.size() && plan->plan_components_[index].trajectory_ && !plan->plan_components_[index].trajectory_->empty()) { - if (!isRemainingPathValid(*plan, std::make_pair(index, 0))) + if (!isRemainingPathValid(*plan, std::make_pair(static_cast(index), 0))) path_became_invalid_ = true; } } diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp index 11a37c9329..d4885f1fd3 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -50,6 +50,11 @@ #include #include +#ifdef _WIN32 +#define popen _popen +#define pclose _pclose +#endif + rdf_loader::RDFLoader::RDFLoader(const std::string& robot_description) { moveit::tools::Profiler::ScopedStart prof_start; diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index a980d61071..110ba6391a 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -59,6 +59,13 @@ // MoveIt #include +// chdir +#ifdef _WIN32 +#include +#else +#include +#endif + namespace moveit_setup_assistant { // Boost file system From 84db86fdf91cf8d6b8ec495d11d47970b46caaa1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Fri, 30 Aug 2019 15:31:24 +0200 Subject: [PATCH 056/612] depth_image_octomap_updater: correctly set properties of debug images (#1652) (#1653) * calculate step width independent from input depth image * Also set endianness based on the host system, as the data stems from local computation. * depth updater: streamline Image properties * depth updater: enhance readability for filtered image generation --- .../src/depth_image_octomap_updater.cpp | 41 ++++++++++--------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index 4a33d7c40a..9d1840a118 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -394,33 +394,32 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP { sensor_msgs::Image debug_msg; debug_msg.header = depth_msg->header; - debug_msg.height = depth_msg->height; - debug_msg.width = depth_msg->width; + debug_msg.height = h; + debug_msg.width = w; + debug_msg.is_bigendian = HOST_IS_BIG_ENDIAN; debug_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - debug_msg.is_bigendian = depth_msg->is_bigendian; - debug_msg.step = depth_msg->step; + debug_msg.step = w * sizeof(float); debug_msg.data.resize(img_size * sizeof(float)); mesh_filter_->getModelDepth(reinterpret_cast(&debug_msg.data[0])); pub_model_depth_image_.publish(debug_msg, *info_msg); sensor_msgs::Image filtered_depth_msg; filtered_depth_msg.header = depth_msg->header; - filtered_depth_msg.height = depth_msg->height; - filtered_depth_msg.width = depth_msg->width; + filtered_depth_msg.height = h; + filtered_depth_msg.width = w; + filtered_depth_msg.is_bigendian = HOST_IS_BIG_ENDIAN; filtered_depth_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - filtered_depth_msg.is_bigendian = depth_msg->is_bigendian; - filtered_depth_msg.step = depth_msg->step; + filtered_depth_msg.step = w * sizeof(float); filtered_depth_msg.data.resize(img_size * sizeof(float)); - mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_depth_msg.data[0])); pub_filtered_depth_image_.publish(filtered_depth_msg, *info_msg); sensor_msgs::Image label_msg; label_msg.header = depth_msg->header; - label_msg.height = depth_msg->height; - label_msg.width = depth_msg->width; + label_msg.height = h; + label_msg.width = w; + label_msg.is_bigendian = HOST_IS_BIG_ENDIAN; label_msg.encoding = sensor_msgs::image_encodings::RGBA8; - label_msg.is_bigendian = depth_msg->is_bigendian; label_msg.step = w * sizeof(unsigned int); label_msg.data.resize(img_size * sizeof(unsigned int)); mesh_filter_->getFilteredLabels(reinterpret_cast(&label_msg.data[0])); @@ -430,22 +429,26 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP if (!filtered_cloud_topic_.empty()) { - static std::vector filtered_data; sensor_msgs::Image filtered_msg; filtered_msg.header = depth_msg->header; - filtered_msg.height = depth_msg->height; - filtered_msg.width = depth_msg->width; + filtered_msg.height = h; + filtered_msg.width = w; + filtered_msg.is_bigendian = HOST_IS_BIG_ENDIAN; filtered_msg.encoding = sensor_msgs::image_encodings::TYPE_16UC1; - filtered_msg.is_bigendian = depth_msg->is_bigendian; - filtered_msg.step = depth_msg->step; + filtered_msg.step = w * sizeof(unsigned short); filtered_msg.data.resize(img_size * sizeof(unsigned short)); + + // reuse float buffer across callbacks + static std::vector filtered_data; if (filtered_data.size() < img_size) filtered_data.resize(img_size); + mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_data[0])); - unsigned short* tmp_ptr = (unsigned short*)&filtered_msg.data[0]; + unsigned short* msg_data = reinterpret_cast(&filtered_msg.data[0]); for (std::size_t i = 0; i < img_size; ++i) { - tmp_ptr[i] = (unsigned short)(filtered_data[i] * 1000 + 0.5); + // rescale depth to millimeter to work with `unsigned short` + msg_data[i] = static_cast(filtered_data[i] * 1000 + 0.5); } pub_filtered_depth_image_.publish(filtered_msg, *info_msg); } From 8fcff1db6d4eb7c04236ad532f0e54b2804a6b6f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 31 Aug 2019 20:38:41 +0200 Subject: [PATCH 057/612] chomp: fix potential calculation (#1651) --- .../chomp_motion_planner/chomp_optimizer.h | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h index 05d6c235c4..dafda7f0b2 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h @@ -84,25 +84,21 @@ class ChompOptimizer inline double getPotential(double field_distance, double radius, double clearence) { double d = field_distance - radius; - double potential = 0.0; - // three cases below: - if (d >= clearence) + if (d >= clearence) // everything is fine { - potential = 0.0; + return 0.0; } - else if (d >= 0.0) + else if (d >= 0.0) // transition phase, no collision yet { - double diff = (d - clearence); - double gradient_magnitude = diff * clearence; // (diff / clearance) - potential = 0.5 * gradient_magnitude * diff; + const double diff = (d - clearence); + const double gradient_magnitude = diff / clearence; + return 0.5 * gradient_magnitude * diff; // 0.5 * (d - clearance)^2 / clearance } - else // if d < 0.0 + else // d < 0.0: collision { - potential = -d + 0.5 * clearence; + return -d + 0.5 * clearence; // linearly increase, starting from 0.5 * clearance } - - return potential; } template void getJacobian(int trajectoryPoint, Eigen::Vector3d& collision_point_pos, std::string& jointName, From 062381c75847ab8dc68e022e8987ebf514c8484b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Thu, 12 Sep 2019 10:54:56 +0200 Subject: [PATCH 058/612] Fix plot details, correcting xlabels positions and cleaning the graph (#1658) (#1668) --- .../scripts/moveit_benchmark_statistics.py | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py b/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py index 666e64c60d..50896a037d 100755 --- a/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py +++ b/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py @@ -343,7 +343,7 @@ def plotAttribute(cur, planners, attribute, typename): color=matplotlib.cm.hot(int(floor(i*256/numValues))), label=descriptions[i]) heights = heights + measurements[i] - xtickNames = plt.xticks([x+width/2. for x in ind], labels, rotation=30) + xtickNames = plt.xticks([x + width / 2. for x in ind], labels, rotation=30, fontsize=8, ha='right') ax.set_ylabel(attribute.replace('_',' ') + ' (%)') box = ax.get_position() ax.set_position([box.x0, box.y0, box.width * 0.8, box.height]) @@ -355,7 +355,11 @@ def plotAttribute(cur, planners, attribute, typename): measurementsPercentage = [sum(m) * 100. / len(m) for m in measurements] ind = range(len(measurements)) plt.bar(ind, measurementsPercentage, width) - xtickNames = plt.xticks([x + width / 2. for x in ind], labels, rotation=30, fontsize=8) + ### uncommenting this line will remove the term 'kConfigDefault' from the labels for OMPL Solvers. + ### Fits situations where you need more control in the plot, such as in an academic publication for example + #labels = [l.replace('kConfigDefault', '') for l in labels] + + xtickNames = plt.xticks([x + width / 2. for x in ind], labels, rotation=30, fontsize=8, ha='right') ax.set_ylabel(attribute.replace('_',' ') + ' (%)') plt.subplots_adjust(bottom=0.3) # Squish the plot into the upper 2/3 of the page. Leave room for labels else: @@ -367,19 +371,24 @@ def plotAttribute(cur, planners, attribute, typename): #xtickNames = plt.xticks(labels, rotation=30, fontsize=10) #plt.subplots_adjust(bottom=0.3) # Squish the plot into the upper 2/3 of the page. Leave room for labels - + + ### uncommenting this line will remove the term 'kConfigDefault' from the labels for OMPL Solvers. + ### Fits situations where you need more control in the plot, such as in an academic publication for example + #labels = [l.replace('kConfigDefault', '') for l in labels] + xtickNames = plt.setp(ax,xticklabels=labels) - plt.setp(xtickNames, rotation=30) + plt.setp(xtickNames, rotation=30, fontsize=8, ha='right') for tick in ax.xaxis.get_major_ticks(): # shrink the font size of the x tick labels tick.label.set_fontsize(8) plt.subplots_adjust(bottom=0.3) # Squish the plot into the upper 2/3 of the page. Leave room for labels - ax.set_xlabel('Motion planning algorithm') + ax.set_xlabel('Motion planning algorithm', fontsize=12) ax.yaxis.grid(True, linestyle='-', which='major', color='lightgrey', alpha=0.5) if max(nanCounts)>0: maxy = max([max(y) for y in measurements]) for i in range(len(labels)): x = i+width/2 if typename=='BOOLEAN' else i+1 - ax.text(x, .95*maxy, str(nanCounts[i]), horizontalalignment='center', size='small') + ### uncommenting the next line, the number of failed planning attempts will be added to each bar + # ax.text(x, .95*maxy, str(nanCounts[i]), horizontalalignment='center', size='small') plt.show() def plotProgressAttribute(cur, planners, attribute): From cbe534a72639c46212d9c6caa9ee2f4c01ccd0a0 Mon Sep 17 00:00:00 2001 From: Jens P Date: Tue, 17 Sep 2019 11:22:47 +0200 Subject: [PATCH 059/612] Migration notes for unified collision environment (#1647) --- MIGRATION.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MIGRATION.md b/MIGRATION.md index fb4a5c16be..00cf53f4fb 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -8,7 +8,7 @@ API changes in MoveIt releases - Moved the example package `moveit_controller_manager_example` into [moveit_tutorials](https://github.com/ros-planning/moveit_tutorials) - Requests to `get_planning_scene` service without explicitly setting "components" now return full scene - `moveit_ros_plannning` no longer depends on `moveit_ros_perception` -- `CollisionRobot` and `CollisionWorld` are combined into a single `CollisionEnv` class. This applies for all derived collision checkers as `FCL`, `ALL_VALID`, `HYBRID` and `DISTANCE_FIELD`. Consequently, `getCollisionRobot[Unpadded] / getCollisionWorld` functions are replaced through a `getCollisionEnv` in the planning scene and return the new combined environment. This unified collision environment provides the union of all member functions of `CollisionRobot` and `CollisionWorld`. +- `CollisionRobot` and `CollisionWorld` are combined into a single `CollisionEnv` class. This applies for all derived collision checkers as `FCL`, `ALL_VALID`, `HYBRID` and `DISTANCE_FIELD`. Consequently, `getCollisionRobot[Unpadded] / getCollisionWorld` functions are replaced through a `getCollisionEnv` in the planning scene and return the new combined environment. This unified collision environment provides the union of all member functions of `CollisionRobot` and `CollisionWorld`. Note that calling `checkRobotCollision` of the `CollisionEnv` does not take a `CollisionRobot` as an argument anymore as it is implicitly contained in the `CollisionEnv`. ## ROS Melodic From 871f3a0ded8acad77c658ce70c76855bb4ff3292 Mon Sep 17 00:00:00 2001 From: Jens P Date: Tue, 17 Sep 2019 11:23:20 +0200 Subject: [PATCH 060/612] Adding documentation to collision detection (#1645) * Collision detection documentation from #1488 * Collision detection documentation from #1505 --- .../collision_detection/collision_common.h | 23 ++--- .../collision_detector_allocator.h | 2 +- .../moveit/collision_detection/world.h | 4 +- .../collision_common.h | 87 ++++++++++++++++--- .../collision_env_fcl.h | 10 ++- .../src/collision_common.cpp | 28 ++++-- 6 files changed, 121 insertions(+), 33 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index c4ec8f25a5..2991951395 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -161,11 +161,10 @@ struct CollisionResult /** \brief Number of contacts returned */ std::size_t contact_count; - /** \brief A map returning the pairs of ids of the bodies in contact, plus information about the contacts themselves - */ + /** \brief A map returning the pairs of body ids in contact, plus their contact details */ ContactMap contacts; - /** \brief When costs are computed, the individual cost sources are */ + /** \brief These are the individual cost sources when costs are computed */ std::set cost_sources; }; @@ -196,7 +195,7 @@ struct CollisionRequest /** \brief If true, a collision cost is computed */ bool cost; - /** \brief If true, compute contacts */ + /** \brief If true, compute contacts. Otherwise only a binary collision yes/no is reported. */ bool contacts; /** \brief Overall maximum number of contacts to compute */ @@ -223,14 +222,15 @@ namespace DistanceRequestTypes { enum DistanceRequestType { - GLOBAL, /// Find the global minimum - SINGLE, /// Find the global minimum for each pair - LIMITED, /// Find a limited(max_contacts_per_body) set of contacts for a given pair - ALL /// Find all the contacts for a given pair + GLOBAL, ///< Find the global minimum + SINGLE, ///< Find the global minimum for each pair + LIMITED, ///< Find a limited(max_contacts_per_body) set of contacts for a given pair + ALL ///< Find all the contacts for a given pair }; } typedef DistanceRequestTypes::DistanceRequestType DistanceRequestType; +/** \brief Representation of a distance-reporting request */ struct DistanceRequest { DistanceRequest() @@ -263,7 +263,7 @@ struct DistanceRequest /// Indicate the type of distance request. If using type=ALL, it is /// recommended to set max_contacts_per_body to the expected number - /// of contacts per pair becaused it is uesed to reserving space. + /// of contacts per pair because it is used to reserve space. DistanceRequestType type; /// Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold) @@ -279,7 +279,7 @@ struct DistanceRequest const AllowedCollisionMatrix* acm; /// Only calculate distances for objects within this threshold to each other. - /// If set this can significantly to reduce number of queries. + /// If set, this can significantly reduce the number of queries. double distance_threshold; /// Log debug information @@ -290,6 +290,7 @@ struct DistanceRequest bool compute_gradient; }; +/** \brief Generic representation of the distance information for a pair of objects */ struct DistanceResultsData { DistanceResultsData() @@ -355,8 +356,10 @@ struct DistanceResultsData } }; +/** \brief Mapping between the names of the collision objects and the DistanceResultData. */ typedef std::map, std::vector > DistanceMap; +/** \brief Result of a distance request. */ struct DistanceResult { DistanceResult() : collision(false) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h index f1bb41d857..cbfb39a161 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h @@ -92,7 +92,7 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator return CollisionEnvPtr(new CollisionEnvType(robot_model)); } - /** Create an allocator for FCL collision detectors */ + /** Create an allocator for collision detectors. */ static CollisionDetectorAllocatorPtr create() { return CollisionDetectorAllocatorPtr(new CollisionDetectorAllocatorType()); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index 82b299673e..ad21d3a034 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -287,7 +287,7 @@ class World /** The objects maintained in the world */ std::map objects_; - /* observers to call when something changes */ + /** Wrapper for a callback function to call when something changes in the world */ class Observer { public: @@ -296,6 +296,8 @@ class World } ObserverCallbackFn callback_; }; + + /// All registered observers of this world representation std::vector observers_; }; } diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index 87fd375580..c5b89cca28 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -58,24 +58,29 @@ namespace collision_detection { MOVEIT_STRUCT_FORWARD(CollisionGeometryData); +/** \brief Wrapper around world, link and attached objects' geometry data. */ struct CollisionGeometryData { + /** \brief Constructor for a robot link collision geometry object. */ CollisionGeometryData(const robot_model::LinkModel* link, int index) : type(BodyTypes::ROBOT_LINK), shape_index(index) { ptr.link = link; } + /** \brief Constructor for a new collision geometry object which is attached to the robot. */ CollisionGeometryData(const robot_state::AttachedBody* ab, int index) : type(BodyTypes::ROBOT_ATTACHED), shape_index(index) { ptr.ab = ab; } + /** \brief Constructor for a new world collision geometry. */ CollisionGeometryData(const World::Object* obj, int index) : type(BodyTypes::WORLD_OBJECT), shape_index(index) { ptr.obj = obj; } + /** \brief Returns the name which is saved in the member pointed to in \e ptr. */ const std::string& getID() const { switch (type) @@ -90,6 +95,7 @@ struct CollisionGeometryData return ptr.obj->id_; } + /** \brief Returns a string of the corresponding \e type. */ std::string getTypeString() const { switch (type) @@ -104,14 +110,22 @@ struct CollisionGeometryData return "Object"; } - /** \brief Check if two CollisionGeometryData objects point to the same source object */ + /** \brief Check if two CollisionGeometryData objects point to the same source object. */ bool sameObject(const CollisionGeometryData& other) const { return type == other.type && ptr.raw == other.ptr.raw; } + /** \brief Indicates the body type of the object. */ BodyType type; + + /** \brief Multiple \e CollisionGeometryData objects construct a collision object. The collision object refers to an + * array of coordinate transformations at a certain start index. The index of the transformation of a child \e + * CollisionGeometryData object is then given by adding the parent collision object index and the \e shape_index of a + * geometry data object. */ int shape_index; + + /** \brief Points to the type of body which contains the geometry. */ union { const robot_model::LinkModel* link; @@ -121,6 +135,7 @@ struct CollisionGeometryData } ptr; }; +/** \brief Data structure which is passed to the collision callback function of the collision manager. */ struct CollisionData { CollisionData() : req_(NULL), active_components_only_(NULL), res_(NULL), acm_(NULL), done_(false) @@ -136,27 +151,29 @@ struct CollisionData { } - /// Compute \e active_components_only_ based on \e req_ + /** \brief Compute \e active_components_only_ based on the joint group specified in \e req_ */ void enableGroup(const robot_model::RobotModelConstPtr& robot_model); - /// The collision request passed by the user + /** \brief The collision request passed by the user */ const CollisionRequest* req_; - /// If the collision request includes a group name, this set contains the pointers to the link models that are - /// considered for collision; - /// If the pointer is NULL, all collisions are considered. + /** \brief If the collision request includes a group name, this set contains the pointers to the link models that + * are considered for collision. + * + * If the pointer is NULL, all collisions are considered. */ const std::set* active_components_only_; - /// The user specified response location + /** \brief The user-specified response location. */ CollisionResult* res_; - /// The user specified collision matrix (may be NULL) + /** \brief The user-specified collision matrix (may be NULL). */ const AllowedCollisionMatrix* acm_; - /// Flag indicating whether collision checking is complete + /** \brief Flag indicating whether collision checking is complete. */ bool done_; }; +/** \brief Data structure which is passed to the distance callback function of the collision manager. */ struct DistanceData { DistanceData(const DistanceRequest* req, DistanceResult* res) : req(req), res(res), done(false) @@ -166,42 +183,48 @@ struct DistanceData { } - /// Distance query request information + /** \brief Distance query request information. */ const DistanceRequest* req; - /// Distance query results information + /** \brief Distance query results information. */ DistanceResult* res; - /// Indicates if distance query is finished. + /** \brief Indicates if distance query is finished. */ bool done; }; MOVEIT_STRUCT_FORWARD(FCLGeometry); +/** \brief Bundles the \e CollisionGeometryData and FCL collision geometry representation into a single class. */ struct FCLGeometry { FCLGeometry() { } + /** \brief Constructor for a robot link. */ FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const robot_model::LinkModel* link, int shape_index) : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(link, shape_index)) { collision_geometry_->setUserData(collision_geometry_data_.get()); } + /** \brief Constructor for an attached body. */ FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const robot_state::AttachedBody* ab, int shape_index) : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(ab, shape_index)) { collision_geometry_->setUserData(collision_geometry_data_.get()); } + /** \brief Constructor for a world object. */ FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const World::Object* obj, int shape_index) : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(obj, shape_index)) { collision_geometry_->setUserData(collision_geometry_data_.get()); } + /** \brief Updates the \e collision_geometry_data_ with new data while also setting the \e collision_geometry_ to the + * new data. */ template void updateCollisionGeometryData(const T* data, int shape_index, bool newType) { @@ -212,13 +235,18 @@ struct FCLGeometry collision_geometry_->setUserData(collision_geometry_data_.get()); } + /** \brief Pointer to FCL collision geometry. */ std::shared_ptr collision_geometry_; + + /** \brief Pointer to the user-defined geometry data. */ CollisionGeometryDataPtr collision_geometry_data_; }; typedef std::shared_ptr FCLCollisionObjectPtr; typedef std::shared_ptr FCLCollisionObjectConstPtr; +/** \brief A general high-level object which consists of multiple \e FCLCollisionObjects. It is the top level data + * structure which is used in the collision checking process. */ struct FCLObject { void registerTo(fcl::BroadPhaseCollisionManagerd* manager); @@ -226,33 +254,65 @@ struct FCLObject void clear(); std::vector collision_objects_; + + /** \brief Geometry data corresponding to \e collision_objects_. */ std::vector collision_geometry_; }; +/** \brief Bundles an \e FCLObject and a broadphase FCL collision manager. */ struct FCLManager { FCLObject object_; std::shared_ptr manager_; }; +/** \brief Callback function used by the FCLManager used for each pair of collision objects to +* calculate object contact information. +* +* \param o1 First FCL collision object +* \param o2 Second FCL collision object +* \data General pointer to arbitrary data which is used during the callback +* \return True terminates the distance check, false continues it to the next pair of objects */ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data); +/** \brief Callback function used by the FCLManager used for each pair of collision objects to +* calculate collisions and distances. +* +* \param o1 First FCL collision object +* \param o2 Second FCL collision object +* \data General pointer to arbitrary data which is used during the callback +* \return True terminates the collision check, false continues it to the next pair of objects */ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& min_dist); +/** \brief Create new FCLGeometry object out of robot link model. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link, int shape_index); + +/** \brief Create new FCLGeometry object out of attached body. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab, int shape_index); + +/** \brief Create new FCLGeometry object out of a world object. + * + * A world object always consists only of a single shape, therefore we don't need the \e shape_index. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj); +/** \brief Create new scaled and / or padded FCLGeometry object out of robot link model. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, const robot_model::LinkModel* link, int shape_index); + +/** \brief Create new scaled and / or padded FCLGeometry object out of an attached body. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, const robot_state::AttachedBody* ab, int shape_index); + +/** \brief Create new scaled and / or padded FCLGeometry object out of an world object. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, const World::Object* obj); + +/** \brief Increases the counter of the caches which can trigger the cleaning of expired entries from them. */ void cleanCollisionGeometryCache(); +/** \brief Transforms an Eigen Isometry3d to FCL coordinate transformation */ inline void transform2fcl(const Eigen::Isometry3d& b, fcl::Transform3d& f) { #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) @@ -264,6 +324,7 @@ inline void transform2fcl(const Eigen::Isometry3d& b, fcl::Transform3d& f) #endif } +/** \brief Transforms an Eigen Isometry3d to FCL coordinate transformation */ inline fcl::Transform3d transform2fcl(const Eigen::Isometry3d& b) { fcl::Transform3d t; @@ -271,6 +332,7 @@ inline fcl::Transform3d transform2fcl(const Eigen::Isometry3d& b) return t; } +/** \brief Transforms an FCL contact into a MoveIt contact point. */ inline void fcl2contact(const fcl::Contactd& fc, Contact& c) { c.pos = Eigen::Vector3d(fc.pos[0], fc.pos[1], fc.pos[2]); @@ -284,6 +346,7 @@ inline void fcl2contact(const fcl::Contactd& fc, Contact& c) c.body_type_2 = cgd2->type; } +/** \brief Transforms the FCL internal representation to the MoveIt \e CostSource data structure. */ inline void fcl2costsource(const fcl::CostSourced& fcs, CostSource& cs) { cs.aabb_min[0] = fcs.aabb_min[0]; diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h index b2819a6147..8906f8155f 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h @@ -95,14 +95,20 @@ class CollisionEnvFCL : public CollisionEnv void setWorld(const WorldPtr& world) override; protected: - /** \brief Updates the collision objects saved in the manager to reflect a new padding or scaling of the robot links - */ + /** \brief Updates the FCL collision geometry and objects saved in the CollisionRobotFCL members to reflect a new + * padding or scaling of the robot links. + * + * It searches for the link through the pointed-to robot model of the CollisionRobot and then constructs new FCL + * collision objects and geometries depending on the changed robot model. + * + * \param links The names of the links which have been updated in the robot model */ void updatedPaddingOrScaling(const std::vector& links) override; /** \brief Bundles the different checkSelfCollision functions into a single function */ void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; + /** \brief Bundles the different checkRobotCollision functions into a single function */ void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index e977b025a2..0cf3446237 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -356,6 +356,9 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi return cdata->done_; } +/** \brief Cache for an arbitrary type of shape. It is assigned during the execution of \e createCollisionGeometry(). + * + * Only a single cache per thread and object type is created as it is a quasi-singleton instance. */ struct FCLShapeCache { using ShapeKey = shapes::ShapeConstWeakPtr; @@ -390,7 +393,10 @@ struct FCLShapeCache static const unsigned int MAX_CLEAN_COUNT = 100; // every this many uses of the cache, a cleaning operation is // executed (this is only removal of expired entries) + /** \brief Map of weak pointers to the FCLGeometry. */ ShapeMap map_; + + /** \brief Counts cache usage and triggers clearing of cache when \m MAX_CLEAN_COUNT is exceeded. */ unsigned int clean_count_; }; @@ -624,7 +630,9 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void return cdata->done; } -/* We template the function so we get a different cache for each of the template arguments combinations */ +/* Templated function to get a different cache for each of the template arguments combinations. + * + * The returned cache is a quasi-singleton for each thread as it is created \e thread_local. */ template FCLShapeCache& GetShapeCache() { @@ -659,6 +667,11 @@ struct IfSameType }; }; +/** \brief Templated helper function creating new collision geometry out of general object using an arbitrary bounding + * volume (BV). + * + * It assigns a thread-local cache for each type of shape and minimizes memory usage and copying through utilizing the + * cache. */ template FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const T* data, int shape_index) { @@ -839,7 +852,6 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, return FCLGeometryConstPtr(); } -///////////////////////////////////////////////////// FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link, int shape_index) { @@ -857,6 +869,8 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, return createCollisionGeometry(shape, obj, 0); } +/** \brief Templated helper function creating new collision geometry out of general object using an arbitrary bounding + * volume (BV). This can include padding and scaling. */ template FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, const T* data, int shape_index) @@ -901,9 +915,8 @@ void cleanCollisionGeometryCache() cache2.bumpUseCount(true); } } -} // namespace collision_detection -void collision_detection::CollisionData::enableGroup(const robot_model::RobotModelConstPtr& robot_model) +void CollisionData::enableGroup(const robot_model::RobotModelConstPtr& robot_model) { if (robot_model->hasJointModelGroup(req_->group_name)) active_components_only_ = &robot_model->getJointModelGroup(req_->group_name)->getUpdatedLinkModelsSet(); @@ -911,7 +924,7 @@ void collision_detection::CollisionData::enableGroup(const robot_model::RobotMod active_components_only_ = nullptr; } -void collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManagerd* manager) +void FCLObject::registerTo(fcl::BroadPhaseCollisionManagerd* manager) { std::vector collision_objects(collision_objects_.size()); for (std::size_t i = 0; i < collision_objects_.size(); ++i) @@ -920,14 +933,15 @@ void collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManagerd manager->registerObjects(collision_objects); } -void collision_detection::FCLObject::unregisterFrom(fcl::BroadPhaseCollisionManagerd* manager) +void FCLObject::unregisterFrom(fcl::BroadPhaseCollisionManagerd* manager) { for (auto& collision_object : collision_objects_) manager->unregisterObject(collision_object.get()); } -void collision_detection::FCLObject::clear() +void FCLObject::clear() { collision_objects_.clear(); collision_geometry_.clear(); } +} // namespace collision_detection From ff71fc44bfead147b252dccefb78bee5d894841c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 19 Sep 2019 08:20:41 -0700 Subject: [PATCH 061/612] Rename jog_arm->moveit_jog_arm (#1663) * Rename jog_arm->moveit_jog_arm * Address @rhaschke's feedback * apply clang-format --- .../CMakeLists.txt | 30 +++++++++---------- .../{jog_arm => moveit_jog_arm}/README.md | 4 +-- .../config/spacenav_via_teleop_tools.yaml | 2 +- .../config/ur_simulated_config.yaml | 6 ++-- .../moveit_jog_arm}/collision_check_thread.h | 13 ++++---- .../include/moveit_jog_arm}/jog_arm_data.h | 6 ++-- .../include/moveit_jog_arm}/jog_calcs.h | 10 +++---- .../moveit_jog_arm}/jog_ros_interface.h | 14 ++++----- .../include/moveit_jog_arm}/low_pass_filter.h | 6 ++-- .../launch/spacenav_cpp.launch | 8 ++--- .../launch/spacenav_teleop_tools.launch | 8 ++--- .../{jog_arm => moveit_jog_arm}/package.xml | 4 +-- .../src}/collision_check_thread.cpp | 12 ++++---- .../src}/jog_calcs.cpp | 8 ++--- .../src}/jog_ros_interface.cpp | 10 +++---- .../src/jog_server.cpp} | 10 +++---- .../src}/low_pass_filter.cpp | 8 ++--- .../teleop_examples/spacenav_to_twist.cpp | 10 +++---- .../arm_setup/config/initial_position.yaml | 0 .../test/arm_setup/config/jog_settings.yaml | 8 ++--- .../arm_setup/launch/panda_move_group.launch | 0 .../integration/test_jog_arm_integration.py | 6 ++-- .../test/launch/jog_arm_integration_test.test | 16 +++++----- 23 files changed, 100 insertions(+), 99 deletions(-) rename moveit_experimental/{jog_arm => moveit_jog_arm}/CMakeLists.txt (57%) rename moveit_experimental/{jog_arm => moveit_jog_arm}/README.md (91%) rename moveit_experimental/{jog_arm => moveit_jog_arm}/config/spacenav_via_teleop_tools.yaml (93%) rename moveit_experimental/{jog_arm => moveit_jog_arm}/config/ur_simulated_config.yaml (91%) rename moveit_experimental/{jog_arm/include/jog_arm => moveit_jog_arm/include/moveit_jog_arm}/collision_check_thread.h (84%) rename moveit_experimental/{jog_arm/include/jog_arm => moveit_jog_arm/include/moveit_jog_arm}/jog_arm_data.h (97%) rename moveit_experimental/{jog_arm/include/jog_arm => moveit_jog_arm/include/moveit_jog_arm}/jog_calcs.h (97%) rename moveit_experimental/{jog_arm/include/jog_arm => moveit_jog_arm/include/moveit_jog_arm}/jog_ros_interface.h (93%) rename moveit_experimental/{jog_arm/include/jog_arm => moveit_jog_arm/include/moveit_jog_arm}/low_pass_filter.h (96%) rename moveit_experimental/{jog_arm => moveit_jog_arm}/launch/spacenav_cpp.launch (65%) rename moveit_experimental/{jog_arm => moveit_jog_arm}/launch/spacenav_teleop_tools.launch (72%) rename moveit_experimental/{jog_arm => moveit_jog_arm}/package.xml (89%) rename moveit_experimental/{jog_arm/src/jog_arm => moveit_jog_arm/src}/collision_check_thread.cpp (94%) rename moveit_experimental/{jog_arm/src/jog_arm => moveit_jog_arm/src}/jog_calcs.cpp (99%) rename moveit_experimental/{jog_arm/src/jog_arm => moveit_jog_arm/src}/jog_ros_interface.cpp (98%) rename moveit_experimental/{jog_arm/src/jog_arm/jog_arm_server.cpp => moveit_jog_arm/src/jog_server.cpp} (89%) rename moveit_experimental/{jog_arm/src/jog_arm => moveit_jog_arm/src}/low_pass_filter.cpp (95%) rename moveit_experimental/{jog_arm/src/jog_arm => moveit_jog_arm/src}/teleop_examples/spacenav_to_twist.cpp (91%) rename moveit_experimental/{jog_arm => moveit_jog_arm}/test/arm_setup/config/initial_position.yaml (100%) rename moveit_experimental/{jog_arm => moveit_jog_arm}/test/arm_setup/config/jog_settings.yaml (86%) rename moveit_experimental/{jog_arm => moveit_jog_arm}/test/arm_setup/launch/panda_move_group.launch (100%) rename moveit_experimental/{jog_arm => moveit_jog_arm}/test/integration/test_jog_arm_integration.py (93%) rename moveit_experimental/{jog_arm => moveit_jog_arm}/test/launch/jog_arm_integration_test.test (58%) diff --git a/moveit_experimental/jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt similarity index 57% rename from moveit_experimental/jog_arm/CMakeLists.txt rename to moveit_experimental/moveit_jog_arm/CMakeLists.txt index 733d4dd328..e95c156c6b 100644 --- a/moveit_experimental/jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(jog_arm) +project(moveit_jog_arm) add_compile_options(-std=c++11) @@ -37,27 +37,27 @@ include_directories( ${EIGEN3_INCLUDE_DIRS} ) -add_executable(${PROJECT_NAME}_server - src/jog_arm/collision_check_thread.cpp - src/jog_arm/jog_arm_server.cpp - src/jog_arm/jog_calcs.cpp - src/jog_arm/jog_ros_interface.cpp - src/jog_arm/low_pass_filter.cpp +add_executable(jog_server + src/collision_check_thread.cpp + src/jog_server.cpp + src/jog_calcs.cpp + src/jog_ros_interface.cpp + src/low_pass_filter.cpp ) -add_dependencies(${PROJECT_NAME}_server ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_server ${catkin_LIBRARIES} ${Eigen_LIBRARIES}) +add_dependencies(jog_server ${catkin_EXPORTED_TARGETS}) +target_link_libraries(jog_server ${catkin_LIBRARIES} ${Eigen_LIBRARIES}) -add_executable(${PROJECT_NAME}_spacenav_to_twist - src/jog_arm/teleop_examples/spacenav_to_twist.cpp +add_executable(spacenav_to_twist + src/teleop_examples/spacenav_to_twist.cpp ) -add_dependencies(${PROJECT_NAME}_spacenav_to_twist ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_spacenav_to_twist ${catkin_LIBRARIES}) +add_dependencies(spacenav_to_twist ${catkin_EXPORTED_TARGETS}) +target_link_libraries(spacenav_to_twist ${catkin_LIBRARIES}) install( TARGETS - ${PROJECT_NAME}_server - ${PROJECT_NAME}_spacenav_to_twist + jog_server + spacenav_to_twist RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/moveit_experimental/jog_arm/README.md b/moveit_experimental/moveit_jog_arm/README.md similarity index 91% rename from moveit_experimental/jog_arm/README.md rename to moveit_experimental/moveit_jog_arm/README.md index af983f78d6..0543aed922 100644 --- a/moveit_experimental/jog_arm/README.md +++ b/moveit_experimental/moveit_jog_arm/README.md @@ -32,12 +32,12 @@ strictness: 2" Launch the jog node. This example uses commands from a SpaceNavigator joystick-like device: - roslaunch jog_arm spacenav_cpp.launch + roslaunch moveit_jog_arm spacenav_cpp.launch If you dont have a SpaceNavigator, send commands like this: ```sh -rostopic pub -r 100 /jog_arm_server/delta_jog_cmds geometry_msgs/TwistStamped "header: auto +rostopic pub -r 100 /jog_server/delta_jog_cmds geometry_msgs/TwistStamped "header: auto twist: linear: x: 0.0 diff --git a/moveit_experimental/jog_arm/config/spacenav_via_teleop_tools.yaml b/moveit_experimental/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml similarity index 93% rename from moveit_experimental/jog_arm/config/spacenav_via_teleop_tools.yaml rename to moveit_experimental/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml index cd9e53c9df..2308e0c5a0 100644 --- a/moveit_experimental/jog_arm/config/spacenav_via_teleop_tools.yaml +++ b/moveit_experimental/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml @@ -2,7 +2,7 @@ teleop: cartesian_twist_command: type: topic message_type: geometry_msgs/TwistStamped - topic_name: jog_arm_server/delta_jog_cmds + topic_name: jog_server/delta_jog_cmds axis_mappings: - axis: 0 diff --git a/moveit_experimental/jog_arm/config/ur_simulated_config.yaml b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml similarity index 91% rename from moveit_experimental/jog_arm/config/ur_simulated_config.yaml rename to moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml index 8e6d32fb73..839a4b16a5 100644 --- a/moveit_experimental/jog_arm/config/ur_simulated_config.yaml +++ b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml @@ -42,10 +42,10 @@ hard_stop_singularity_threshold: 30 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names -cartesian_command_in_topic: jog_arm_server/delta_jog_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: jog_arm_server/joint_delta_jog_cmds # Topic for incoming joint angle commands +cartesian_command_in_topic: jog_server/delta_jog_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: jog_server/joint_delta_jog_cmds # Topic for incoming joint angle commands joint_topic: joint_states -warning_topic: jog_arm_server/halted # Publish boolean warnings to this topic +warning_topic: jog_server/halted # Publish boolean warnings to this topic command_out_topic: joint_group_position_controller/command # Publish outgoing commands here ## Collision checking diff --git a/moveit_experimental/jog_arm/include/jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h similarity index 84% rename from moveit_experimental/jog_arm/include/jog_arm/collision_check_thread.h rename to moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index 4b3680b5f3..d5c8551576 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////////// // Title : collision_check_thread.h -// Project : jog_arm +// Project : moveit_jog_arm // Created : 1/11/2019 // Author : Brian O'Neil, Andy Zelenak, Blake Anderson // @@ -39,17 +39,18 @@ #pragma once -#include -#include +#include "jog_arm_data.h" +#include "low_pass_filter.h" #include #include -namespace jog_arm +namespace moveit_jog_arm { class CollisionCheckThread { public: - CollisionCheckThread(const jog_arm::JogArmParameters parameters, jog_arm::JogArmShared& shared_variables, - pthread_mutex_t& mutex, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); + CollisionCheckThread(const moveit_jog_arm::JogArmParameters parameters, + moveit_jog_arm::JogArmShared& shared_variables, pthread_mutex_t& mutex, + const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); }; } diff --git a/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h similarity index 97% rename from moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h rename to moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 499166c427..0950d07405 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////////// // Title : jog_arm_data.h -// Project : jog_arm +// Project : moveit_jog_arm // Created : 1/11/2019 // Author : Brian O'Neil, Andy Zelenak, Blake Anderson // @@ -45,9 +45,9 @@ #include #include -namespace jog_arm +namespace moveit_jog_arm { -static const std::string LOGNAME = "jog_arm_server"; +static const std::string LOGNAME = "jog_server"; static const double WHILE_LOOP_WAIT = 0.001; // Variables to share between threads, and their mutexes diff --git a/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h similarity index 97% rename from moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h rename to moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index fcdafa529b..4d01690c7a 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////////// // Title : jog_calcs.h -// Project : jog_arm +// Project : moveit_jog_arm // Created : 1/11/2019 // Author : Brian O'Neil, Andy Zelenak, Blake Anderson // @@ -39,15 +39,15 @@ #pragma once -#include -#include +#include "jog_arm_data.h" +#include "low_pass_filter.h" #include #include #include #include #include -namespace jog_arm +namespace moveit_jog_arm { class JogCalcs { @@ -132,4 +132,4 @@ class JogCalcs uint num_joints_; }; -} // namespace jog_arm +} // namespace moveit_jog_arm diff --git a/moveit_experimental/jog_arm/include/jog_arm/jog_ros_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h similarity index 93% rename from moveit_experimental/jog_arm/include/jog_arm/jog_ros_interface.h rename to moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h index ef5f7d8d66..8d69686430 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/jog_ros_interface.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////////// // Title : jog_ros_interface.h -// Project : jog_arm +// Project : moveit_jog_arm // Created : 3/9/2017 // Author : Brian O'Neil, Blake Anderson, Andy Zelenak // @@ -41,17 +41,17 @@ #pragma once +#include "collision_check_thread.h" #include -#include -#include -#include -#include +#include "jog_arm_data.h" +#include "jog_calcs.h" +#include "low_pass_filter.h" #include #include #include #include -namespace jog_arm +namespace moveit_jog_arm { /** * Class JogROSInterface - Instantiated in main(). Handles ROS subs & pubs and creates the worker threads. @@ -85,4 +85,4 @@ class JogROSInterface // Store the parameters that were read from ROS server JogArmParameters ros_parameters_; }; -} // namespace jog_arm +} // namespace moveit_jog_arm diff --git a/moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h similarity index 96% rename from moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h rename to moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h index d1a3e937db..424ab6e2f1 100644 --- a/moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////////// // Title : low_pass_filter.h -// Project : jog_arm +// Project : moveit_jog_arm // Created : 1/11/2019 // Author : Andy Zelenak // @@ -39,7 +39,7 @@ #pragma once -namespace jog_arm +namespace moveit_jog_arm { /** * Class LowPassFilter - Filter a signal to soften jerks. @@ -62,4 +62,4 @@ class LowPassFilter // https://www.wolframalpha.com/input/?i=plot+arccot(c) double filter_coeff_ = 10.; }; -} // namespace jog_arm +} // namespace moveit_jog_arm diff --git a/moveit_experimental/jog_arm/launch/spacenav_cpp.launch b/moveit_experimental/moveit_jog_arm/launch/spacenav_cpp.launch similarity index 65% rename from moveit_experimental/jog_arm/launch/spacenav_cpp.launch rename to moveit_experimental/moveit_jog_arm/launch/spacenav_cpp.launch index 4b5b023842..c78dff5b6b 100644 --- a/moveit_experimental/jog_arm/launch/spacenav_cpp.launch +++ b/moveit_experimental/moveit_jog_arm/launch/spacenav_cpp.launch @@ -10,11 +10,11 @@ - + - - - + + + diff --git a/moveit_experimental/jog_arm/launch/spacenav_teleop_tools.launch b/moveit_experimental/moveit_jog_arm/launch/spacenav_teleop_tools.launch similarity index 72% rename from moveit_experimental/jog_arm/launch/spacenav_teleop_tools.launch rename to moveit_experimental/moveit_jog_arm/launch/spacenav_teleop_tools.launch index 5df96be11b..6b3a97cf92 100644 --- a/moveit_experimental/jog_arm/launch/spacenav_teleop_tools.launch +++ b/moveit_experimental/moveit_jog_arm/launch/spacenav_teleop_tools.launch @@ -9,14 +9,14 @@ - - - + + + - + diff --git a/moveit_experimental/jog_arm/package.xml b/moveit_experimental/moveit_jog_arm/package.xml similarity index 89% rename from moveit_experimental/jog_arm/package.xml rename to moveit_experimental/moveit_jog_arm/package.xml index 6bf404a71d..ed1227fce6 100644 --- a/moveit_experimental/jog_arm/package.xml +++ b/moveit_experimental/moveit_jog_arm/package.xml @@ -1,6 +1,6 @@ - jog_arm + moveit_jog_arm 0.0.3 Provides real-time manipulator Cartesian jogging. @@ -10,7 +10,7 @@ BSD 3-Clause - http://wiki.ros.org/jog_arm + https://ros-planning.github.io/moveit_tutorials Brian O'Neil Andy Zelenak diff --git a/moveit_experimental/jog_arm/src/jog_arm/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp similarity index 94% rename from moveit_experimental/jog_arm/src/jog_arm/collision_check_thread.cpp rename to moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 3b165a3e95..56cb82d164 100644 --- a/moveit_experimental/jog_arm/src/jog_arm/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////////// // Title : collision_check_thread.cpp -// Project : jog_arm +// Project : moveit_jog_arm // Created : 1/11/2019 // Author : Brian O'Neil, Andy Zelenak, Blake Anderson // @@ -37,13 +37,13 @@ // /////////////////////////////////////////////////////////////////////////////// -#include +#include -namespace jog_arm +namespace moveit_jog_arm { // Constructor for the class that handles collision checking -CollisionCheckThread::CollisionCheckThread(const jog_arm::JogArmParameters parameters, JogArmShared& shared_variables, - pthread_mutex_t& mutex, +CollisionCheckThread::CollisionCheckThread(const moveit_jog_arm::JogArmParameters parameters, + JogArmShared& shared_variables, pthread_mutex_t& mutex, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) { // If user specified true in yaml file @@ -134,4 +134,4 @@ CollisionCheckThread::CollisionCheckThread(const jog_arm::JogArmParameters param } } } -} // namespace jog_arm +} // namespace moveit_jog_arm diff --git a/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp similarity index 99% rename from moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp rename to moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 8db2351350..b90cd6fd70 100644 --- a/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////////// // Title : jog_calcs.h -// Project : jog_arm +// Project : moveit_jog_arm // Created : 1/11/2019 // Author : Brian O'Neil, Andy Zelenak, Blake Anderson // @@ -37,9 +37,9 @@ // /////////////////////////////////////////////////////////////////////////////// -#include +#include -namespace jog_arm +namespace moveit_jog_arm { // Constructor for the class that handles jogging calculations JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_variables, pthread_mutex_t& mutex, @@ -762,4 +762,4 @@ bool JogCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen:: return true; } -} // namespace jog_arm +} // namespace moveit_jog_arm diff --git a/moveit_experimental/jog_arm/src/jog_arm/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp similarity index 98% rename from moveit_experimental/jog_arm/src/jog_arm/jog_ros_interface.cpp rename to moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp index 1114179b52..6eea959af4 100644 --- a/moveit_experimental/jog_arm/src/jog_arm/jog_ros_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////////// // Title : jog_ros_interface.cpp -// Project : jog_arm +// Project : moveit_jog_arm // Created : 3/9/2017 // Author : Brian O'Neil, Andy Zelenak, Blake Anderson // @@ -39,9 +39,9 @@ // Server node for arm jogging with MoveIt. -#include +#include -namespace jog_arm +namespace moveit_jog_arm { ///////////////////////////////////////////////////////////////////////////////// // JogROSInterface handles ROS subscriptions and instantiates the worker threads. @@ -244,7 +244,7 @@ bool JogROSInterface::readParameters(ros::NodeHandle& n) ROS_ERROR_STREAM_NAMED(LOGNAME, "A namespace must be specified in the launch file, like:"); ROS_ERROR_STREAM_NAMED(LOGNAME, ""); + "value=\"left_jog_server\" />"); return false; } @@ -367,4 +367,4 @@ bool JogROSInterface::readParameters(ros::NodeHandle& n) return true; } -} // namespace jog_arm +} // namespace moveit_jog_arm diff --git a/moveit_experimental/jog_arm/src/jog_arm/jog_arm_server.cpp b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp similarity index 89% rename from moveit_experimental/jog_arm/src/jog_arm/jog_arm_server.cpp rename to moveit_experimental/moveit_jog_arm/src/jog_server.cpp index 2d2c545a70..ba96f3049f 100644 --- a/moveit_experimental/jog_arm/src/jog_arm/jog_arm_server.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////////// -// Title : jog_arm_server.cpp -// Project : jog_arm +// Title : jog_server.cpp +// Project : moveit_jog_arm // Created : 12/31/2018 // Author : Andy Zelenak // @@ -37,13 +37,13 @@ // /////////////////////////////////////////////////////////////////////////////// -#include +#include int main(int argc, char** argv) { - ros::init(argc, argv, jog_arm::LOGNAME); + ros::init(argc, argv, moveit_jog_arm::LOGNAME); - jog_arm::JogROSInterface ros_interface; + moveit_jog_arm::JogROSInterface ros_interface; return 0; } diff --git a/moveit_experimental/jog_arm/src/jog_arm/low_pass_filter.cpp b/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp similarity index 95% rename from moveit_experimental/jog_arm/src/jog_arm/low_pass_filter.cpp rename to moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp index 719011f9e1..8ee2809a5c 100644 --- a/moveit_experimental/jog_arm/src/jog_arm/low_pass_filter.cpp +++ b/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////////// // Title : low_pass_filter.cpp -// Project : jog_arm +// Project : moveit_jog_arm // Created : 1/11/2019 // Author : Andy Zelenak // @@ -37,9 +37,9 @@ // /////////////////////////////////////////////////////////////////////////////// -#include +#include -namespace jog_arm +namespace moveit_jog_arm { LowPassFilter::LowPassFilter(double low_pass_filter_coeff) { @@ -68,4 +68,4 @@ double LowPassFilter::filter(double new_measurement) return new_filtered_msrmt; } -} // namespace jog_arm +} // namespace moveit_jog_arm diff --git a/moveit_experimental/jog_arm/src/jog_arm/teleop_examples/spacenav_to_twist.cpp b/moveit_experimental/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp similarity index 91% rename from moveit_experimental/jog_arm/src/jog_arm/teleop_examples/spacenav_to_twist.cpp rename to moveit_experimental/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp index f795d07529..20ca24a382 100644 --- a/moveit_experimental/jog_arm/src/jog_arm/teleop_examples/spacenav_to_twist.cpp +++ b/moveit_experimental/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp @@ -3,7 +3,7 @@ #include "ros/ros.h" #include "sensor_msgs/Joy.h" -namespace jog_arm +namespace moveit_jog_arm { static const int NUM_SPINNERS = 1; static const int QUEUE_LENGTH = 1; @@ -14,8 +14,8 @@ class SpaceNavToTwist SpaceNavToTwist() : spinner_(NUM_SPINNERS) { joy_sub_ = n_.subscribe("spacenav/joy", QUEUE_LENGTH, &SpaceNavToTwist::joyCallback, this); - twist_pub_ = n_.advertise("jog_arm_server/delta_jog_cmds", QUEUE_LENGTH); - joint_delta_pub_ = n_.advertise("jog_arm_server/joint_delta_jog_cmds", QUEUE_LENGTH); + twist_pub_ = n_.advertise("jog_server/delta_jog_cmds", QUEUE_LENGTH); + joint_delta_pub_ = n_.advertise("jog_server/joint_delta_jog_cmds", QUEUE_LENGTH); spinner_.start(); ros::waitForShutdown(); @@ -57,13 +57,13 @@ class SpaceNavToTwist ros::Publisher twist_pub_, joint_delta_pub_; ros::AsyncSpinner spinner_; }; -} // namespace jog_arm +} // namespace moveit_jog_arm int main(int argc, char** argv) { ros::init(argc, argv, "spacenav_to_twist"); - jog_arm::SpaceNavToTwist to_twist; + moveit_jog_arm::SpaceNavToTwist to_twist; return 0; } diff --git a/moveit_experimental/jog_arm/test/arm_setup/config/initial_position.yaml b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/initial_position.yaml similarity index 100% rename from moveit_experimental/jog_arm/test/arm_setup/config/initial_position.yaml rename to moveit_experimental/moveit_jog_arm/test/arm_setup/config/initial_position.yaml diff --git a/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml similarity index 86% rename from moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml rename to moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml index c33b0ab748..6fd1119af4 100644 --- a/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml +++ b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml @@ -5,8 +5,8 @@ scale: # Only used if command_in_type=="unitless" linear: 0.003 # Max linear velocity. Meters per publish_period. rotational: 0.006 # Max angular velocity. Rads per publish_period. joint: 0.01 # Max joint angular/linear velocity. Rads or Meters per publish period. -cartesian_command_in_topic: jog_arm_server/delta_jog_cmds # Topic for xyz commands -joint_command_in_topic: jog_arm_server/joint_delta_jog_cmds # Topic for angle commands +cartesian_command_in_topic: jog_server/delta_jog_cmds # Topic for xyz commands +joint_command_in_topic: jog_server/joint_delta_jog_cmds # Topic for angle commands command_frame: base_link # TF frame that incoming cmds are given in incoming_command_timeout: 1 # Stop jogging if X seconds elapsed without a new cmd joint_topic: joint_states @@ -20,9 +20,9 @@ publish_period: 0.01 # 1/Nominal publish rate [seconds] collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. num_halt_msgs_to_publish: 1 # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish # Publish boolean warnings to this topic -warning_topic: jog_arm_server/warning +warning_topic: jog_server/warning joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. -command_out_topic: jog_arm_server/command # Find this topic by 'rostopic list' or 'rostopic list | grep command' or 'rqt_graph' +command_out_topic: jog_server/command # Find this topic by 'rostopic list' or 'rostopic list | grep command' or 'rqt_graph' # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController) # or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) diff --git a/moveit_experimental/jog_arm/test/arm_setup/launch/panda_move_group.launch b/moveit_experimental/moveit_jog_arm/test/arm_setup/launch/panda_move_group.launch similarity index 100% rename from moveit_experimental/jog_arm/test/arm_setup/launch/panda_move_group.launch rename to moveit_experimental/moveit_jog_arm/test/arm_setup/launch/panda_move_group.launch diff --git a/moveit_experimental/jog_arm/test/integration/test_jog_arm_integration.py b/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py similarity index 93% rename from moveit_experimental/jog_arm/test/integration/test_jog_arm_integration.py rename to moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py index 6689c0224a..ca30f5f3ed 100755 --- a/moveit_experimental/jog_arm/test/integration/test_jog_arm_integration.py +++ b/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py @@ -11,10 +11,10 @@ JOG_ARM_SETTLE_TIME_S = 10 ROS_SETTLE_TIME_S = 10 -JOINT_JOG_COMMAND_TOPIC = 'jog_arm_server/joint_delta_jog_cmds' -CARTESIAN_JOG_COMMAND_TOPIC = 'jog_arm_server/delta_jog_cmds' +JOINT_JOG_COMMAND_TOPIC = 'jog_server/joint_delta_jog_cmds' +CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' -COMMAND_OUT_TOPIC = 'jog_arm_server/command' +COMMAND_OUT_TOPIC = 'jog_server/command' @pytest.fixture diff --git a/moveit_experimental/jog_arm/test/launch/jog_arm_integration_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_integration_test.test similarity index 58% rename from moveit_experimental/jog_arm/test/launch/jog_arm_integration_test.test rename to moveit_experimental/moveit_jog_arm/test/launch/jog_arm_integration_test.test index 8845fd0815..4cd7ccb4c2 100644 --- a/moveit_experimental/jog_arm/test/launch/jog_arm_integration_test.test +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_integration_test.test @@ -10,7 +10,7 @@ These aren't suitable for actually jogging a robot in RViz, but they are good enough to test jog node output. --> - + @@ -19,20 +19,20 @@ - + - - + + - - - + + + - + From ff8f1d8503d907789acca7ef3a84f8c361bd8749 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 19 Sep 2019 19:32:25 +0200 Subject: [PATCH 062/612] Fix double mutex unlock (#1672) --- moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index b90cd6fd70..0ff378a533 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -465,7 +465,7 @@ bool JogCalcs::applyVelocityScaling(JogArmShared& shared_variables, pthread_mute trajectory_msgs::JointTrajectory& new_jt_traj, const Eigen::VectorXd& delta_theta, double singularity_scale) { - pthread_mutex_unlock(&mutex); + pthread_mutex_lock(&mutex); double collision_scale = shared_variables.collision_velocity_scale; pthread_mutex_unlock(&mutex); From 977d9ad3e64895f5a669f3766f6441b46955b509 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Thu, 26 Sep 2019 08:13:31 +0300 Subject: [PATCH 063/612] Fix typo (#1675) --- .../include/moveit/robot_state/cartesian_interpolator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h index ecf843c99a..95fa387d4e 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h +++ b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h @@ -49,7 +49,7 @@ namespace core /** \brief Struct for containing jump_threshold. For the purposes of maintaining API, we support both \e jump_threshold_factor which provides a scaling factor for - detecting joint space jumps and \e revolute_jump_threshold and \e prismatic_jump_threshold which provide abolute + detecting joint space jumps and \e revolute_jump_threshold and \e prismatic_jump_threshold which provide absolute thresholds for detecting joint space jumps. */ struct JumpThreshold { From 877042fa7cdfe72d86c8a5d931e64b085204bd54 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Fri, 27 Sep 2019 07:24:37 -0600 Subject: [PATCH 064/612] Add @mamoll to codeowners (#1678) --- .github/CODEOWNERS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index ca05e3da91..3820b617c8 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -95,7 +95,7 @@ /moveit_setup_assistant/ @davetcoleman @rhaschke @MohmadAyman -/moveit_planners/ompl/ @BryceStevenWilley @zkingston +/moveit_planners/ompl/ @BryceStevenWilley @mamoll /moveit_planners/chomp/chomp_interface/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/chomp/chomp_optimizer_adapter/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/chomp/chomp_motion_planner/ @raghavendersahdev @knorth55 @bmagyar From 2a3d8f8d4b729b3c7aaf1ed9175df478ff52b5ea Mon Sep 17 00:00:00 2001 From: jschleicher Date: Mon, 30 Sep 2019 17:45:40 +0200 Subject: [PATCH 065/612] optional prefix for link names in ikfast plugin (#1599) If you pass a `link_prefix` parameter in your `kinematics.yaml`, this string is prepended to the base and tip links. It allows multi-robot setups (e.g. dual-arm) and still instantiate the same solver for both manipulators. --- .../ikfast61_moveit_plugin_template.cpp | 48 +++++++++++++------ 1 file changed, 34 insertions(+), 14 deletions(-) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index b2fd03982a..b90eb5faf7 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -166,6 +166,9 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase const std::string IKFAST_TIP_FRAME_ = "_EEF_LINK_"; const std::string IKFAST_BASE_FRAME_ = "_BASE_LINK_"; + // prefix added to tip- and baseframe to allow different namespaces or multi-robot setups + std::string link_prefix_; + // The transform tip and base bool are set to true if this solver is used with a kinematic // chain that extends beyond the ikfast tip and base frame. The solution will be valid so // long as there are no active, passive, or mimic joints between either the ikfast_tip_frame @@ -377,17 +380,14 @@ bool IKFastKinematicsPlugin::computeRelativeTransform(const std::string& from, c robot_state.reset(new RobotState(robot_model_)); robot_state->setToDefaultValues(); - auto* from_link = robot_state->getLinkModel(from); - auto* to_link = robot_state->getLinkModel(to); - if (!from_link) - ROS_ERROR_STREAM_NAMED(name_, "Could not find frame " << from); - if (!to_link) - ROS_ERROR_STREAM_NAMED(name_, "Could not find frame " << to); + bool has_link; // to suppress ROS_ERRORs for non-existent frames + auto* from_link = robot_model_->getLinkModel(from, &has_link); + auto* to_link = robot_model_->getLinkModel(to, &has_link); if (!from_link || !to_link) return false; - if (robot_state->getRobotModel()->getRigidlyConnectedParentLinkModel(from_link) != - robot_state->getRobotModel()->getRigidlyConnectedParentLinkModel(to_link)) + if (robot_model_->getRigidlyConnectedParentLinkModel(from_link) != + robot_model_->getRigidlyConnectedParentLinkModel(to_link)) { ROS_ERROR_STREAM_NAMED(name_, "Link frames " << from << " and " << to << " are not rigidly connected."); return false; @@ -409,19 +409,39 @@ bool IKFastKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo } storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); + if (!lookupParam("link_prefix", link_prefix_, std::string(""))) + { + ROS_INFO_NAMED(name_, "Using empty link_prefix."); + } + else + { + ROS_INFO_STREAM_NAMED(name_, "Using link_prefix: '" << link_prefix_ << "'"); + } + // verbose error output. subsequent checks in computeRelativeTransform return false then + if (!robot_model.hasLinkModel(tip_frames_[0])) + ROS_ERROR_STREAM_NAMED(name_, "tip frame '" << tip_frames_[0] << "' does not exist."); + if (!robot_model.hasLinkModel(base_frame_)) + ROS_ERROR_STREAM_NAMED(name_, "base_frame '" << base_frame_ << "' does not exist."); + + if (!robot_model.hasLinkModel(link_prefix_ + IKFAST_TIP_FRAME_)) + ROS_ERROR_STREAM_NAMED(name_, "prefixed tip frame '" << link_prefix_ + IKFAST_TIP_FRAME_ + << "' does not exist. " + "Please check your link_prefix parameter."); + if (!robot_model.hasLinkModel(link_prefix_ + IKFAST_BASE_FRAME_)) + ROS_ERROR_STREAM_NAMED(name_, "prefixed base frame '" << link_prefix_ + IKFAST_BASE_FRAME_ + << "' does not exist. " + "Please check your link_prefix parameter."); // This IKFast solution was generated with IKFAST_TIP_FRAME_ and IKFAST_BASE_FRAME_. // It is often the case that fixed joints are added to these links to model things like // a robot mounted on a table or a robot with an end effector attached to the last link. // To support these use cases, we store the transform from the IKFAST_BASE_FRAME_ to the // base_frame_ and IKFAST_TIP_FRAME_ the tip_frame_ and transform to the input pose accordingly - if (!computeRelativeTransform(tip_frames_[0], IKFAST_TIP_FRAME_, group_tip_to_chain_tip_, tip_transform_required_) || - !computeRelativeTransform(IKFAST_BASE_FRAME_, base_frame_, chain_base_to_group_base_, base_transform_required_)) + if (!computeRelativeTransform(tip_frames_[0], link_prefix_ + IKFAST_TIP_FRAME_, group_tip_to_chain_tip_, + tip_transform_required_) || + !computeRelativeTransform(link_prefix_ + IKFAST_BASE_FRAME_, base_frame_, chain_base_to_group_base_, + base_transform_required_)) { - if (!computeRelativeTransform(tip_frames_[0], IKFAST_TIP_FRAME_, group_tip_to_chain_tip_, tip_transform_required_)) - ROS_ERROR_NAMED(name_, "Failed to compute transform between IKFAST_TIP_FRAME_ and tip_frames_[0]"); - if (!computeRelativeTransform(IKFAST_BASE_FRAME_, base_frame_, chain_base_to_group_base_, base_transform_required_)) - ROS_ERROR_NAMED(name_, "Failed to compute transform between base_frame_ and IKFAST_BASE_FRAME_"); return false; } From c1f1882b89cfc4c51b667b7434ca65a249b90902 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Fri, 11 Oct 2019 15:05:15 +0900 Subject: [PATCH 066/612] Improve gui-docker documentation (#1687) --- .docker/README.md | 3 ++- .docker/gui-docker | 19 ++++++++++++------- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/.docker/README.md b/.docker/README.md index b24af099ec..d250ee12a1 100644 --- a/.docker/README.md +++ b/.docker/README.md @@ -1,3 +1,4 @@ # MoveIt Docker Containers -For more information see [Continuous Integration and Docker](http://moveit.ros.org/documentation/contributing/continuous_integration.html) documentation. + +For more information see the pages [Continuous Integration and Docker](http://moveit.ros.org/documentation/contributing/continuous_integration.html) and [Using Docker Containers with MoveIt](https://moveit.ros.org/install/docker/). diff --git a/.docker/gui-docker b/.docker/gui-docker index 2688c52cf3..871b04b0f6 100755 --- a/.docker/gui-docker +++ b/.docker/gui-docker @@ -1,11 +1,16 @@ #!/bin/bash -u # This script is used to run a docker container with graphics support. -# http://wiki.ros.org/docker/Tutorials/Hardware%20Acceleration +# All arguments to this script except "-c " will be appended to a docker run command. +# If a container name is specified, and this container already exists, the container is re-entered, +# which easily allows entering the same persistent container from multiple terminals. +# See documentation for detailed examples: https://moveit.ros.org/install/docker/ -# All arguments to this script will be appended to a docker run command. -# Example command line: -# ./gui-docker -it --rm moveit/moveit:master-source /bin/bash +# Example commands: +# ./gui-docker --rm -it moveit/moveit:master-source /bin/bash # Run a (randomly named) container that is removed on exit +# ./gui-docker -v ~/ros_ws:/root/ros_ws --rm -it moveit/moveit:master-source /bin/bash # Same, but also link host volume ~/ros_ws to /root/ros_ws in the container +# ./gui-docker -c container_name # Start (or continue) an interactive bash in a moveit/moveit:master-source container +# ./gui-docker # Same, but use the default container name "default_moveit_container" function check_nvidia2() { # If we don't have an NVIDIA graphics card, bail out @@ -78,8 +83,8 @@ function count_positional_args() { } if [ $# -eq 0 ] ; then - # If not options are specified at all, just run the default_container - CONTAINER_NAME=default_container + # If no options are specified at all, use the name "default_moveit_container" + CONTAINER_NAME=default_moveit_container else # Check for option -c or --container in first position case "$1" in @@ -91,7 +96,7 @@ else shift fi # Set default container name if still undefined - CONTAINER_NAME="${CONTAINER_NAME:-default_container}" + CONTAINER_NAME="${CONTAINER_NAME:-default_moveit_container}" ;; esac fi From ebb5f28ea7450d2167cac70655d94626b9e5070c Mon Sep 17 00:00:00 2001 From: MarqRazz Date: Sun, 13 Oct 2019 05:27:20 -0600 Subject: [PATCH 067/612] fixed REALTIME planned path trajectory playback (#1683) --- .../src/trajectory_visualization.cpp | 26 +++++++++++++------ 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 64afac8b56..428aac9414 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -43,17 +43,17 @@ #include #include -#include -#include -#include +#include +#include #include +#include +#include #include +#include +#include #include -#include -#include -#include +#include #include -#include #include namespace moveit_rviz_plugin @@ -413,7 +413,10 @@ void TrajectoryVisualization::update(float wall_dt, float ros_dt) float tm = getStateDisplayTime(); if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible() && trajectory_slider_panel_->isPaused()) + { current_state_ = trajectory_slider_panel_->getSliderPosition(); + current_state_time_ = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_); + } else if (current_state_ < 0) { // special case indicating restart of animation current_state_ = 0; @@ -426,6 +429,9 @@ void TrajectoryVisualization::update(float wall_dt, float ros_dt) current_state_time_) { current_state_time_ -= tm; + if (tm < current_state_time_) // if we are stuck in the while loop we should + // move the robot along the path to keep up + display_path_robot_->update(displaying_trajectory_message_->getWayPointPtr(current_state_)); ++current_state_; } } @@ -449,7 +455,11 @@ void TrajectoryVisualization::update(float wall_dt, float ros_dt) } else { - animating_path_ = false; // animation finished + animating_path_ = false; // animation finished + if (trajectory_slider_panel_) // make sure we move the slider to the end + // so the user can re-play + trajectory_slider_panel_->setSliderPosition(waypoint_count); + display_path_robot_->update(displaying_trajectory_message_->getWayPointPtr(waypoint_count - 1)); display_path_robot_->setVisible(loop_display_property_->getBool()); if (!loop_display_property_->getBool() && trajectory_slider_panel_) trajectory_slider_panel_->pauseButton(true); From 71e06f43ff32f6dc05f9b92a33d2d3e998c70b80 Mon Sep 17 00:00:00 2001 From: Matthias Nieuwenhuisen Date: Wed, 9 Oct 2019 10:51:22 +0200 Subject: [PATCH 068/612] depth_image_octomap_updater: reset depth transfer function to standard values (#1661) Use attribute stack to keep track of GL_DEPTH_SCALE and GL_DEPTH_BIAS --- moveit_ros/perception/mesh_filter/src/gl_renderer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index 03d19a6b5d..7e4092cff3 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -182,7 +182,7 @@ void mesh_filter::GLRenderer::deleteFrameBuffers() void mesh_filter::GLRenderer::begin() const { - glPushAttrib(GL_VIEWPORT_BIT | GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_POLYGON_BIT); + glPushAttrib(GL_VIEWPORT_BIT | GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_POLYGON_BIT | GL_PIXEL_MODE_BIT); glBindFramebuffer(GL_FRAMEBUFFER, fbo_id_); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glViewport(0, 0, width_, height_); From b1b1b98f3e93880e3bcf3b3d109acee5a5cb9edc Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 15 Oct 2019 21:38:32 +0200 Subject: [PATCH 069/612] Always copy dynamics if enabled in CurrentStateMonitor (#1676) --- .../src/current_state_monitor.cpp | 36 ++++++++++--------- 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index 64ae7465d2..0f4dbe8f20 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -364,22 +364,6 @@ void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const senso update = true; robot_state_.setJointPositions(jm, &(joint_state->position[i])); - // optionally copy velocities and effort - if (copy_dynamics_) - { - // check if velocities exist - if (joint_state->name.size() == joint_state->velocity.size()) - { - robot_state_.setJointVelocities(jm, &(joint_state->velocity[i])); - - // check if effort exist. assume they are not useful if no velocities were passed in - if (joint_state->name.size() == joint_state->effort.size()) - { - robot_state_.setJointEfforts(jm, &(joint_state->effort[i])); - } - } - } - // continuous joints wrap, so we don't modify them (even if they are outside bounds!) if (jm->getType() == moveit::core::JointModel::REVOLUTE) if (static_cast(jm)->isContinuous()) @@ -395,6 +379,26 @@ void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const senso else if (joint_state->position[i] > b.max_position_ && joint_state->position[i] <= b.max_position_ + error_) robot_state_.setJointPositions(jm, &b.max_position_); } + + // optionally copy velocities and effort + if (copy_dynamics_) + { + // update joint velocities + if (joint_state->name.size() == joint_state->velocity.size() && + robot_state_.getJointVelocities(jm)[0] != joint_state->velocity[i]) + { + update = true; + robot_state_.setJointVelocities(jm, &(joint_state->velocity[i])); + } + + // update joint efforts + if (joint_state->name.size() == joint_state->effort.size() && + robot_state_.getJointEffort(jm)[0] != joint_state->effort[i]) + { + update = true; + robot_state_.setJointEfforts(jm, &(joint_state->effort[i])); + } + } } } From e3c385b78f9229077b76d0aaad051ce01191b32c Mon Sep 17 00:00:00 2001 From: Mark Moll Date: Thu, 17 Oct 2019 13:27:18 -0500 Subject: [PATCH 070/612] Add code of conduct, based on the Django one (#1684) Add code of conduct, based on the Django one --- CODE_OF_CONDUCT.md | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 CODE_OF_CONDUCT.md diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md new file mode 100644 index 0000000000..6dfaeeff96 --- /dev/null +++ b/CODE_OF_CONDUCT.md @@ -0,0 +1,15 @@ +# MoveIt Code of Conduct + +Like the technical community as a whole, the MoveIt team and community is made up of a mixture of professionals and volunteers from all over the world, working on every aspect of the mission - including mentorship, teaching, and connecting people. + +Diversity is one of our huge strengths, but it can also lead to communication issues and unhappiness. To that end, we have a few basic rules that we ask people to adhere to. This code applies equally to founders, mentors and those seeking help and guidance. +It applies to all kinds of communication, including discussions on GitHub (issue tracker, pull requests), MoveIt maintainer meetings and any other forums created by the project team, which the community uses for communication. + +- **Be friendly and patient.** +- **Be welcoming.** We strive to be a community that welcomes and supports people of all backgrounds and identities. +- **Be considerate.** Your work will be used by other people, and you in turn will depend on the work of others. Any decision you take will affect users and colleagues, and you should take those consequences into account when making decisions. Remember that we're a world-wide community, so you might not be communicating in someone else's primary language. +- **Be respectful.** Not all of us will agree all the time, but disagreement is no excuse for poor behavior and poor manners. We might all experience some frustration now and then, but we cannot allow that frustration to turn into a personal attack. It's important to remember that a community where people feel uncomfortable or threatened is not a productive one. Members of the MoveIt community should be respectful when dealing with other members as well as with people outside the MoveIt community. +- **Be careful in the words that you choose.** We are a community of professionals, and we conduct ourselves professionally. Be kind to others. Do not insult or put down other participants. Harassment and other exclusionary behavior aren't acceptable. Treat others like you want to be treated. +- **When we disagree, try to understand why.** Disagreements, both social and technical, happen all the time and MoveIt is no exception. It is important that we resolve disagreements and differing views constructively. Remember that we're different. The strength of MoveIt comes from its varied community, people from a wide range of backgrounds. Different people have different perspectives on issues. Being unable to understand why someone holds a viewpoint doesn't mean that they're wrong. Don't forget that it is human to err and blaming each other doesn't get us anywhere. Instead, focus on helping to resolve issues and learning from mistakes. + +Original text courtesy of the [Speak Up! project](http://web.archive.org/web/20141109123859/http://speakup.io/coc.html). From 55e9dbfd9c860eb8ebd72fc21ad88ce7351ae2cd Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Fri, 18 Oct 2019 08:55:49 -0600 Subject: [PATCH 071/612] Code of conduct tweaks (#1697) - Customize naming of people involved with the MoveIt project specifically. We don't really call anyone founders or mentors, rather just maintainers and contributors. This is a concept from a different community. - Clarify that this CoC is based on Djangos, which is a fairly modified version of Speak Ups, and theirs is a fairly modified version of Fedora and Python's. So we should have a full chain of references. --- CODE_OF_CONDUCT.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md index 6dfaeeff96..185db208a8 100644 --- a/CODE_OF_CONDUCT.md +++ b/CODE_OF_CONDUCT.md @@ -2,7 +2,7 @@ Like the technical community as a whole, the MoveIt team and community is made up of a mixture of professionals and volunteers from all over the world, working on every aspect of the mission - including mentorship, teaching, and connecting people. -Diversity is one of our huge strengths, but it can also lead to communication issues and unhappiness. To that end, we have a few basic rules that we ask people to adhere to. This code applies equally to founders, mentors and those seeking help and guidance. +Diversity is one of our huge strengths, but it can also lead to communication issues and unhappiness. To that end, we have a few basic rules that we ask people to adhere to. This code applies equally to maintainers, contributors, and those seeking help and guidance. It applies to all kinds of communication, including discussions on GitHub (issue tracker, pull requests), MoveIt maintainer meetings and any other forums created by the project team, which the community uses for communication. - **Be friendly and patient.** @@ -12,4 +12,4 @@ It applies to all kinds of communication, including discussions on GitHub (issue - **Be careful in the words that you choose.** We are a community of professionals, and we conduct ourselves professionally. Be kind to others. Do not insult or put down other participants. Harassment and other exclusionary behavior aren't acceptable. Treat others like you want to be treated. - **When we disagree, try to understand why.** Disagreements, both social and technical, happen all the time and MoveIt is no exception. It is important that we resolve disagreements and differing views constructively. Remember that we're different. The strength of MoveIt comes from its varied community, people from a wide range of backgrounds. Different people have different perspectives on issues. Being unable to understand why someone holds a viewpoint doesn't mean that they're wrong. Don't forget that it is human to err and blaming each other doesn't get us anywhere. Instead, focus on helping to resolve issues and learning from mistakes. -Original text courtesy of the [Speak Up! project](http://web.archive.org/web/20141109123859/http://speakup.io/coc.html). +Original text courtesy of the [Django project](https://www.djangoproject.com/conduct/). From d8ad6303b83372e9984f9d3b0f99eb60d1ec28dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Tue, 29 Oct 2019 15:51:15 +0100 Subject: [PATCH 072/612] Do not install helper scripts in global bin destination (#1704) --- moveit_ros/benchmarks/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt index 8a8f447e19..b3cd004a94 100644 --- a/moveit_ros/benchmarks/CMakeLists.txt +++ b/moveit_ros/benchmarks/CMakeLists.txt @@ -46,8 +46,6 @@ target_link_libraries(moveit_combine_predefined_poses_benchmark ${MOVEIT_LIB_NAM install( TARGETS ${MOVEIT_LIB_NAME} - moveit_run_benchmark - moveit_combine_predefined_poses_benchmark LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) @@ -55,6 +53,7 @@ install( install( TARGETS moveit_run_benchmark + moveit_combine_predefined_poses_benchmark RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) From 9af7a5e13588c906cf245fa567d0d6d869b88471 Mon Sep 17 00:00:00 2001 From: Mark Moll Date: Fri, 1 Nov 2019 00:14:24 +0800 Subject: [PATCH 073/612] Add OMPL planner 'AnytimePathShortening' (#1686) * Add OMPL planner 'AnytimePathShortening' * clean up registration of planner allocators * add parameter to geometric::AnytimePathShortening method to set planner instances and their parameters via config file * clang-format * make anytime path planning backwards compatible --- .../src/planning_context_manager.cpp | 100 +++++------------- moveit_setup_assistant/CMakeLists.txt | 3 +- moveit_setup_assistant/package.xml | 1 + .../src/tools/moveit_config_data.cpp | 16 +++ 4 files changed, 45 insertions(+), 75 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 88a0666871..8e28a585c4 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -114,7 +115,6 @@ static ompl::base::PlannerPtr allocatePlanner(const ob::SpaceInformationPtr& si, if (!new_name.empty()) planner->setName(new_name); planner->params().setParams(spec.config_, true); - planner->setup(); return planner; } } // namespace @@ -134,79 +134,31 @@ ompl_interface::PlanningContextManager::plannerSelector(const std::string& plann void ompl_interface::PlanningContextManager::registerDefaultPlanners() { - registerPlannerAllocator( // - "geometric::RRT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::RRTConnect", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::LazyRRT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::TRRT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::EST", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::SBL", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::KPIECE", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::BKPIECE", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::LBKPIECE", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::RRTstar", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::PRM", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::PRMstar", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::FMT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::BFMT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::PDST", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::STRIDE", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::BiTRRT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::LBTRRT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::BiEST", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::ProjEST", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::LazyPRM", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::LazyPRMstar", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::SPARS", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::SPARStwo", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + registerPlannerAllocator("geometric::AnytimePathShortening", allocatePlanner); + registerPlannerAllocator("geometric::BFMT", allocatePlanner); + registerPlannerAllocator("geometric::BiEST", allocatePlanner); + registerPlannerAllocator("geometric::BiTRRT", allocatePlanner); + registerPlannerAllocator("geometric::BKPIECE", allocatePlanner); + registerPlannerAllocator("geometric::EST", allocatePlanner); + registerPlannerAllocator("geometric::FMT", allocatePlanner); + registerPlannerAllocator("geometric::KPIECE", allocatePlanner); + registerPlannerAllocator("geometric::LazyPRM", allocatePlanner); + registerPlannerAllocator("geometric::LazyPRMstar", allocatePlanner); + registerPlannerAllocator("geometric::LazyRRT", allocatePlanner); + registerPlannerAllocator("geometric::LBKPIECE", allocatePlanner); + registerPlannerAllocator("geometric::LBTRRT", allocatePlanner); + registerPlannerAllocator("geometric::PDST", allocatePlanner); + registerPlannerAllocator("geometric::PRM", allocatePlanner); + registerPlannerAllocator("geometric::PRMstar", allocatePlanner); + registerPlannerAllocator("geometric::ProjEST", allocatePlanner); + registerPlannerAllocator("geometric::RRT", allocatePlanner); + registerPlannerAllocator("geometric::RRTConnect", allocatePlanner); + registerPlannerAllocator("geometric::RRTstar", allocatePlanner); + registerPlannerAllocator("geometric::SBL", allocatePlanner); + registerPlannerAllocator("geometric::SPARS", allocatePlanner); + registerPlannerAllocator("geometric::SPARStwo", allocatePlanner); + registerPlannerAllocator("geometric::STRIDE", allocatePlanner); + registerPlannerAllocator("geometric::TRRT", allocatePlanner); } void ompl_interface::PlanningContextManager::registerDefaultStateSpaces() diff --git a/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/CMakeLists.txt index f01787d422..e70d73f585 100644 --- a/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/CMakeLists.txt @@ -27,7 +27,8 @@ find_package(catkin REQUIRED COMPONENTS urdf srdfdom ) -include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +find_package(ompl REQUIRED) +include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${OMPL_INCLUDE_DIRS}) # Qt Stuff if(rviz_QT_VERSION VERSION_LESS "5") diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml index 99c93b56e0..3a8581b32a 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/package.xml @@ -21,6 +21,7 @@ libogre-dev qtbase5-dev libqt5-opengl-dev + ompl moveit_core moveit_ros_planning diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 0bbd81f4c2..c747a2d3ee 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -46,6 +46,9 @@ #include #include // for getting file path for loading images +// OMPL version +#include + namespace moveit_setup_assistant { // File system @@ -546,6 +549,19 @@ std::vector MoveItConfigData::getOMPLPlanners() { std::vector planner_des; + OMPLPlannerDescription aps("AnytimePathShortening", "geometric"); + aps.addParameter("shortcut", "true", "Attempt to shortcut all new solution paths"); + aps.addParameter("hybridize", "true", "Compute hybrid solution trajectories"); + aps.addParameter("max_hybrid_paths", "24", "Number of hybrid paths generated per iteration"); + aps.addParameter("num_planners", "4", "The number of default planners to use for planning"); +#if OMPL_VERSION_VALUE >= 1005000 + // This parameter was added in OMPL 1.5.0 + aps.addParameter("planners", "", "A comma-separated list of planner types (e.g., \"PRM,EST,RRTConnect\"" + "Optionally, planner parameters can be passed to change the default:" + "\"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]\""); +#endif + planner_des.push_back(aps); + OMPLPlannerDescription sbl("SBL", "geometric"); sbl.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()"); planner_des.push_back(sbl); From 2bef2b62a722353f047bb66fe5e68cb528396ccc Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Fri, 1 Nov 2019 05:29:56 -0600 Subject: [PATCH 074/612] Fix jog_arm segfault (#1692) Co-Authored-By: Robert Haschke --- moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 0ff378a533..e89087f1a1 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -568,6 +568,7 @@ void JogCalcs::enforceJointVelocityLimits(Eigen::VectorXd& calculated_joint_chan bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& new_jt_traj) { bool halting = false; + for (auto joint : joint_model_group_->getJointModels()) { if (!kinematic_state_->satisfiesVelocityBounds(joint)) @@ -575,8 +576,9 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, ros::this_node::getName() << " " << joint->getName() << " " << " close to a " " velocity limit. Enforcing limit."); + kinematic_state_->enforceVelocityBounds(joint); - for (std::size_t c = 0; c < num_joints_; ++c) + for (std::size_t c = 0; c < new_jt_traj.joint_names.size(); ++c) { if (new_jt_traj.joint_names[c] == joint->getName()) { @@ -588,7 +590,7 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n // Halt if we're past a joint margin and joint velocity is moving even farther past double joint_angle = 0; - for (std::size_t c = 0; c < num_joints_; ++c) + for (std::size_t c = 0; c < original_jt_state_.name.size(); ++c) { if (original_jt_state_.name[c] == joint->getName()) { @@ -596,7 +598,6 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n break; } } - if (!kinematic_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin)) { const std::vector limits = joint->getVariableBoundsMsg(); From ea399e6d4631519aa1ba60a9ddb2cbb04d59cb5f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Tue, 5 Nov 2019 06:43:03 +0100 Subject: [PATCH 075/612] plan_execution: refine logging for invalid paths (#1705) This differentiates between checks caused by scene updates and checks caused by changing trajectory components. --- .../moveit/plan_execution/plan_execution.h | 1 - .../plan_execution/src/plan_execution.cpp | 23 ++++++++----------- 2 files changed, 10 insertions(+), 14 deletions(-) diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h index 11023522a9..7c93c012fb 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h @@ -141,7 +141,6 @@ class PlanExecution private: void planAndExecuteHelper(ExecutableMotionPlan& plan, const Options& opt); - bool isRemainingPathValid(const ExecutableMotionPlan& plan); bool isRemainingPathValid(const ExecutableMotionPlan& plan, const std::pair& path_segment); void planningSceneUpdatedCallback(const planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type); diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index fb8ee5fae1..1a4ecf0d1a 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -262,13 +262,6 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p getErrorCodeString(plan.error_code_).c_str()); } -bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionPlan& plan) -{ - // check the validity of the currently executed path segment only, since there could be - // changes in the world in between path segments - return isRemainingPathValid(plan, trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex()); -} - bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionPlan& plan, const std::pair& path_segment) { @@ -296,10 +289,6 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP if (res.collision || !plan.planning_scene_->isStateFeasible(t.getWayPoint(i), false)) { - // Dave's debacle - ROS_INFO_NAMED("plan_execution", "Trajectory component '%s' is invalid", - plan.plan_components_[path_segment.first].description_.c_str()); - // call the same functions again, in verbose mode, to show what issues have been detected plan.planning_scene_->isStateFeasible(t.getWayPoint(i), true); req.verbose = true; @@ -418,8 +407,11 @@ moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(E if (new_scene_update_) { new_scene_update_ = false; - if (!isRemainingPathValid(plan)) + std::pair current_index = trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex(); + if (!isRemainingPathValid(plan, current_index)) { + ROS_INFO_NAMED("plan_execution", "Trajectory component '%s' is invalid after scene update", + plan.plan_components_[current_index.first].description_.c_str()); path_became_invalid_ = true; break; } @@ -517,7 +509,12 @@ void plan_execution::PlanExecution::successfulTrajectorySegmentExecution(const E if (index < plan->plan_components_.size() && plan->plan_components_[index].trajectory_ && !plan->plan_components_[index].trajectory_->empty()) { - if (!isRemainingPathValid(*plan, std::make_pair(static_cast(index), 0))) + std::pair next_index(static_cast(index), 0); + if (!isRemainingPathValid(*plan, next_index)) + { + ROS_INFO_NAMED("plan_execution", "Upcoming trajectory component '%s' is invalid", + plan->plan_components_[next_index.first].description_.c_str()); path_became_invalid_ = true; + } } } From d3b2db31dba1b7f292e0dfb2b92f656e6cc2e555 Mon Sep 17 00:00:00 2001 From: livanov93 Date: Fri, 8 Nov 2019 05:28:25 +0100 Subject: [PATCH 076/612] TrajectoryExecutionManager: fix race condition (#1709) Fix race condition accessing execution_thread_ by adding a new mutex. --- .../trajectory_execution_manager.h | 1 + .../src/trajectory_execution_manager.cpp | 16 ++++++++++++---- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index 1800068f42..44091052bd 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -320,6 +320,7 @@ class TrajectoryExecutionManager boost::mutex execution_state_mutex_; boost::mutex continuous_execution_mutex_; + boost::mutex execution_thread_mutex_; boost::condition_variable continuous_execution_condition_; diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index feea5e7496..7ab6d1ca73 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -1170,8 +1170,12 @@ void TrajectoryExecutionManager::stopExecution(bool auto_clear) ROS_INFO_NAMED(name_, "Stopped trajectory execution."); // wait for the execution thread to finish - execution_thread_->join(); - execution_thread_.reset(); + boost::mutex::scoped_lock lock(execution_thread_mutex_); + if (execution_thread_) + { + execution_thread_->join(); + execution_thread_.reset(); + } if (auto_clear) clear(); @@ -1182,8 +1186,12 @@ void TrajectoryExecutionManager::stopExecution(bool auto_clear) else if (execution_thread_) // just in case we have some thread waiting to be joined from some point in the past, we // join it now { - execution_thread_->join(); - execution_thread_.reset(); + boost::mutex::scoped_lock lock(execution_thread_mutex_); + if (execution_thread_) + { + execution_thread_->join(); + execution_thread_.reset(); + } } } From cdf4c9bc986203eb023e0ed6f4fa5ffba526de7a Mon Sep 17 00:00:00 2001 From: Jens P Date: Sun, 10 Nov 2019 20:44:36 +0100 Subject: [PATCH 077/612] Added missing licenses (#1716) (#1720) --- .../collision_detector_fcl_plugin_loader.h | 38 +++++++++++++++++-- .../collision_detector_fcl_plugin_loader.cpp | 36 ++++++++++++++++++ 2 files changed, 71 insertions(+), 3 deletions(-) diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h index e1d749474c..213b8ccc97 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h @@ -1,6 +1,38 @@ -/* - * collision_detector_fcl_plugin_loader.h - */ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Bryce Willey + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Bryce Willey */ #pragma once diff --git a/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp b/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp index d2fa9cb71d..f397a7ae94 100644 --- a/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp @@ -1,3 +1,39 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Bryce Willey + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Bryce Willey */ + #include #include From ea0d711b8fd5aeff953a063b39156d2b5573ac95 Mon Sep 17 00:00:00 2001 From: Jere Liukkonen Date: Sun, 17 Nov 2019 04:14:14 -0700 Subject: [PATCH 078/612] Improve move_group_interface's const correctness (#1715) * Add const qualifier to all the member functions which do not modify the state of the class. * Added noexcept modifiers to move constructors, which should always be marked as noexcept. --- .../move_group_interface.h | 42 +++++----- .../src/move_group_interface.cpp | 81 +++++++++---------- 2 files changed, 61 insertions(+), 62 deletions(-) diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index ae4d3a3b7d..71590ac54a 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -53,6 +53,7 @@ #include #include #include +#include #include namespace moveit @@ -105,9 +106,9 @@ class MoveGroupInterface /** \brief Specification of options to use when constructing the MoveGroupInterface class */ struct Options { - Options(const std::string& group_name, const std::string& desc = ROBOT_DESCRIPTION, + Options(std::string group_name, std::string desc = ROBOT_DESCRIPTION, const ros::NodeHandle& node_handle = ros::NodeHandle()) - : group_name_(group_name), robot_description_(desc), node_handle_(node_handle) + : group_name_(std::move(group_name)), robot_description_(std::move(desc)), node_handle_(node_handle) { } @@ -177,15 +178,14 @@ class MoveGroupInterface MoveGroupInterface(const MoveGroupInterface&) = delete; MoveGroupInterface& operator=(const MoveGroupInterface&) = delete; - MoveGroupInterface(MoveGroupInterface&& other); - MoveGroupInterface& operator=(MoveGroupInterface&& other); - + MoveGroupInterface(MoveGroupInterface&& other) noexcept; + MoveGroupInterface& operator=(MoveGroupInterface&& other) noexcept; /** \brief Get the name of the group this instance operates on */ const std::string& getName() const; /** \brief Get the names of the named robot states available as targets, both either remembered states or default * states from srdf */ - const std::vector getNamedTargets(); + const std::vector& getNamedTargets() const; /** \brief Get the RobotModel object. */ robot_model::RobotModelConstPtr getRobotModel() const; @@ -200,13 +200,13 @@ class MoveGroupInterface const std::vector& getJointModelGroupNames() const; /** \brief Get vector of names of joints available in move group */ - const std::vector& getJointNames(); + const std::vector& getJointNames() const; /** \brief Get vector of names of links available in move group */ - const std::vector& getLinkNames(); + const std::vector& getLinkNames() const; /** \brief Get the joint angles for targets specified by name */ - std::map getNamedTargetValues(const std::string& name); + std::map getNamedTargetValues(const std::string& name) const; /** \brief Get only the active (actuated) joints this instance operates on */ const std::vector& getActiveJoints() const; @@ -219,10 +219,11 @@ class MoveGroupInterface unsigned int getVariableCount() const; /** \brief Get the description of the planning plugin loaded by the action server */ - bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc); + bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) const; /** \brief Get the planner parameters for given group and planner_id */ - std::map getPlannerParams(const std::string& planner_id, const std::string& group = ""); + std::map getPlannerParams(const std::string& planner_id, + const std::string& group = "") const; /** \brief Set the planner parameters for given group and planner_id */ void setPlannerParams(const std::string& planner_id, const std::string& group, @@ -778,14 +779,15 @@ class MoveGroupInterface /** \brief Build a PickupGoal for an object named \e object and store it in \e goal_out */ moveit_msgs::PickupGoal constructPickupGoal(const std::string& object, std::vector grasps, - bool plan_only); + bool plan_only) const; /** \brief Build a PlaceGoal for an object named \e object and store it in \e goal_out */ moveit_msgs::PlaceGoal constructPlaceGoal(const std::string& object, - std::vector locations, bool plan_only); + std::vector locations, bool plan_only) const; /** \brief Convert a vector of PoseStamped to a vector of PlaceLocation */ - std::vector posesToPlaceLocations(const std::vector& poses); + std::vector + posesToPlaceLocations(const std::vector& poses) const; /**@}*/ @@ -902,28 +904,28 @@ class MoveGroupInterface bool startStateMonitor(double wait = 1.0); /** \brief Get the current joint values for the joints planned for by this instance (see getJoints()) */ - std::vector getCurrentJointValues(); + std::vector getCurrentJointValues() const; /** \brief Get the current state of the robot within the duration specified by wait. */ - robot_state::RobotStatePtr getCurrentState(double wait = 1); + robot_state::RobotStatePtr getCurrentState(double wait = 1) const; /** \brief Get the pose for the end-effector \e end_effector_link. If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed */ - geometry_msgs::PoseStamped getCurrentPose(const std::string& end_effector_link = ""); + geometry_msgs::PoseStamped getCurrentPose(const std::string& end_effector_link = "") const; /** \brief Get the roll-pitch-yaw (XYZ) for the end-effector \e end_effector_link. If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed */ - std::vector getCurrentRPY(const std::string& end_effector_link = ""); + std::vector getCurrentRPY(const std::string& end_effector_link = "") const; /** \brief Get random joint values for the joints planned for by this instance (see getJoints()) */ - std::vector getRandomJointValues(); + std::vector getRandomJointValues() const; /** \brief Get a random reachable pose for the end-effector \e end_effector_link. If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed */ - geometry_msgs::PoseStamped getRandomPose(const std::string& end_effector_link = ""); + geometry_msgs::PoseStamped getRandomPose(const std::string& end_effector_link = "") const; /**@}*/ diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 9216668fd4..6069606410 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -177,7 +177,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } template - void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) + void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) const { ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str()); @@ -350,6 +350,11 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return *joint_state_target_; } + const robot_state::RobotState& getTargetRobotState() const + { + return *joint_state_target_; + } + void setStartState(const robot_state::RobotState& start_state) { considered_start_state_.reset(new robot_state::RobotState(start_state)); @@ -578,7 +583,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } /** \brief Convert a vector of PoseStamped to a vector of PlaceLocation */ - std::vector posesToPlaceLocations(const std::vector& poses) + std::vector + posesToPlaceLocations(const std::vector& poses) const { std::vector locations; for (const geometry_msgs::PoseStamped& pose : poses) @@ -992,7 +998,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return replan_delay_; } - void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request) + void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request) const { request.group_name = opt_.group_name_; request.num_planning_attempts = num_planning_attempts_; @@ -1017,9 +1023,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { // find out how many goals are specified std::size_t goal_count = 0; - for (std::map >::const_iterator it = pose_targets_.begin(); - it != pose_targets_.end(); ++it) - goal_count = std::max(goal_count, it->second.size()); + for (const auto& pose_target : pose_targets_) + goal_count = std::max(goal_count, pose_target.second.size()); // start filling the goals; // each end effector has a number of possible poses (K) as valid goals @@ -1027,13 +1032,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // to reach the goal that corresponds to the goals of the other end effectors request.goal_constraints.resize(goal_count); - for (std::map >::const_iterator it = pose_targets_.begin(); - it != pose_targets_.end(); ++it) + for (const auto& pose_target : pose_targets_) { - for (std::size_t i = 0; i < it->second.size(); ++i) + for (std::size_t i = 0; i < pose_target.second.size(); ++i) { moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints( - it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_); + pose_target.first, pose_target.second[i], goal_position_tolerance_, goal_orientation_tolerance_); if (active_target_ == ORIENTATION) c.position_constraints.clear(); if (active_target_ == POSITION) @@ -1051,13 +1055,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.trajectory_constraints = *trajectory_constraints_; } - void constructGoal(moveit_msgs::MoveGroupGoal& goal) + void constructGoal(moveit_msgs::MoveGroupGoal& goal) const { constructMotionPlanRequest(goal.request); } moveit_msgs::PickupGoal constructPickupGoal(const std::string& object, std::vector&& grasps, - bool plan_only = false) + bool plan_only = false) const { moveit_msgs::PickupGoal goal; goal.target_name = object; @@ -1084,7 +1088,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } moveit_msgs::PlaceGoal constructPlaceGoal(const std::string& object, - std::vector&& locations, bool plan_only = false) + std::vector&& locations, + bool plan_only = false) const { moveit_msgs::PlaceGoal goal; goal.group_name = opt_.group_name_; @@ -1304,13 +1309,13 @@ MoveGroupInterface::~MoveGroupInterface() delete impl_; } -MoveGroupInterface::MoveGroupInterface(MoveGroupInterface&& other) +MoveGroupInterface::MoveGroupInterface(MoveGroupInterface&& other) noexcept : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_) { other.impl_ = nullptr; } -MoveGroupInterface& MoveGroupInterface::operator=(MoveGroupInterface&& other) +MoveGroupInterface& MoveGroupInterface::operator=(MoveGroupInterface&& other) noexcept { if (this != &other) { @@ -1328,19 +1333,11 @@ const std::string& MoveGroupInterface::getName() const return impl_->getOptions().group_name_; } -const std::vector MoveGroupInterface::getNamedTargets() +const std::vector& MoveGroupInterface::getNamedTargets() const { - const robot_model::RobotModelConstPtr& robot = getRobotModel(); - const std::string& group = getName(); - const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group); - - if (joint_group) - { - return joint_group->getDefaultStateNames(); - } - - std::vector empty; - return empty; + // The pointer returned by getJointModelGroup is guaranteed by the class + // constructor to always be non-null + return impl_->getJointModelGroup()->getDefaultStateNames(); } robot_model::RobotModelConstPtr MoveGroupInterface::getRobotModel() const @@ -1353,13 +1350,13 @@ const ros::NodeHandle& MoveGroupInterface::getNodeHandle() const return impl_->getOptions().node_handle_; } -bool MoveGroupInterface::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) +bool MoveGroupInterface::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) const { return impl_->getInterfaceDescription(desc); } std::map MoveGroupInterface::getPlannerParams(const std::string& planner_id, - const std::string& group) + const std::string& group) const { return impl_->getPlannerParams(planner_id, group); } @@ -1432,20 +1429,20 @@ MoveItErrorCode MoveGroupInterface::plan(Plan& plan) moveit_msgs::PickupGoal MoveGroupInterface::constructPickupGoal(const std::string& object, std::vector grasps, - bool plan_only = false) + bool plan_only = false) const { return impl_->constructPickupGoal(object, std::move(grasps), plan_only); } moveit_msgs::PlaceGoal MoveGroupInterface::constructPlaceGoal(const std::string& object, std::vector locations, - bool plan_only = false) + bool plan_only = false) const { return impl_->constructPlaceGoal(object, std::move(locations), plan_only); } std::vector -MoveGroupInterface::posesToPlaceLocations(const std::vector& poses) +MoveGroupInterface::posesToPlaceLocations(const std::vector& poses) const { return impl_->posesToPlaceLocations(poses); } @@ -1526,22 +1523,22 @@ void MoveGroupInterface::setRandomTarget() impl_->setTargetType(JOINT); } -const std::vector& MoveGroupInterface::getJointNames() +const std::vector& MoveGroupInterface::getJointNames() const { return impl_->getJointModelGroup()->getVariableNames(); } -const std::vector& MoveGroupInterface::getLinkNames() +const std::vector& MoveGroupInterface::getLinkNames() const { return impl_->getJointModelGroup()->getLinkModelNames(); } -std::map MoveGroupInterface::getNamedTargetValues(const std::string& name) +std::map MoveGroupInterface::getNamedTargetValues(const std::string& name) const { std::map >::const_iterator it = remembered_joint_values_.find(name); std::map positions; - if (it != remembered_joint_values_.end()) + if (it != remembered_joint_values_.cend()) { std::vector names = impl_->getJointModelGroup()->getVariableNames(); for (size_t x = 0; x < names.size(); ++x) @@ -1966,7 +1963,7 @@ bool MoveGroupInterface::startStateMonitor(double wait) return impl_->startStateMonitor(wait); } -std::vector MoveGroupInterface::getCurrentJointValues() +std::vector MoveGroupInterface::getCurrentJointValues() const { robot_state::RobotStatePtr current_state; std::vector values; @@ -1975,14 +1972,14 @@ std::vector MoveGroupInterface::getCurrentJointValues() return values; } -std::vector MoveGroupInterface::getRandomJointValues() +std::vector MoveGroupInterface::getRandomJointValues() const { std::vector r; impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getTargetRobotState().getRandomNumberGenerator(), r); return r; } -geometry_msgs::PoseStamped MoveGroupInterface::getRandomPose(const std::string& end_effector_link) +geometry_msgs::PoseStamped MoveGroupInterface::getRandomPose(const std::string& end_effector_link) const { const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; Eigen::Isometry3d pose; @@ -2007,7 +2004,7 @@ geometry_msgs::PoseStamped MoveGroupInterface::getRandomPose(const std::string& return pose_msg; } -geometry_msgs::PoseStamped MoveGroupInterface::getCurrentPose(const std::string& end_effector_link) +geometry_msgs::PoseStamped MoveGroupInterface::getCurrentPose(const std::string& end_effector_link) const { const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; Eigen::Isometry3d pose; @@ -2031,7 +2028,7 @@ geometry_msgs::PoseStamped MoveGroupInterface::getCurrentPose(const std::string& return pose_msg; } -std::vector MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link) +std::vector MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link) const { std::vector result; const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; @@ -2073,7 +2070,7 @@ unsigned int MoveGroupInterface::getVariableCount() const return impl_->getJointModelGroup()->getVariableCount(); } -robot_state::RobotStatePtr MoveGroupInterface::getCurrentState(double wait) +robot_state::RobotStatePtr MoveGroupInterface::getCurrentState(double wait) const { robot_state::RobotStatePtr current_state; impl_->getCurrentState(current_state, wait); From bc9c4ce5ec7ad8b37de9d9f2e6c77d4871e09c85 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 17 Nov 2019 16:01:09 +0100 Subject: [PATCH 079/612] eigenpy: changed to system package (#1737) --- moveit_ros/planning_interface/CMakeLists.txt | 3 ++- .../planning_interface/move_group_interface/CMakeLists.txt | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index d1b5594b18..4c7f6a591b 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -19,7 +19,6 @@ find_package(catkin REQUIRED COMPONENTS tf2_eigen tf2_geometry_msgs tf2_ros - eigenpy roscpp actionlib rospy @@ -45,6 +44,7 @@ find_package(Boost REQUIRED COMPONENTS thread ) find_package(Eigen3 REQUIRED) +find_package(eigenpy REQUIRED) set(THIS_PACKAGE_INCLUDE_DIRS py_bindings_tools/include @@ -81,6 +81,7 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS} include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} + ${eigenpy_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS}) add_subdirectory(py_bindings_tools) diff --git a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt index b9a4eeaa01..31536f08c9 100644 --- a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt @@ -6,7 +6,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_object add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) add_library(${MOVEIT_LIB_NAME}_python src/wrap_python_move_group.cpp) -target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) +target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${eigenpy_LIBRARIES} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) add_dependencies(${MOVEIT_LIB_NAME}_python ${catkin_EXPORTED_TARGETS}) set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_move_group_interface PREFIX "") From 5b44334bcc5666ebf615b63ebd6f80ebb800b905 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 19 Nov 2019 10:56:29 +0100 Subject: [PATCH 080/612] Fixup #1625: MSA needs to explicitly call setVisible(true) (#1738) ... to actually show the robot of the RobotStateDisplay (which is hidden initially since #1625). Fixes b0f01b2dc79f99f5650933834c8e50905bdbe168. --- .../moveit/robot_state_rviz_plugin/robot_state_display.h | 3 +++ .../robot_state_rviz_plugin/src/robot_state_display.cpp | 8 ++++++++ .../src/widgets/setup_assistant_widget.cpp | 1 + 3 files changed, 12 insertions(+) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h index e087dd3a62..e4d0465d6d 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h +++ b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h @@ -83,6 +83,9 @@ class RobotStateDisplay : public rviz::Display void setLinkColor(const std::string& link_name, const QColor& color); void unsetLinkColor(const std::string& link_name); +public Q_SLOTS: + void setVisible(bool visible); + private Q_SLOTS: // ****************************************************************************************** diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 98e024fea0..64d5773fd8 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -341,6 +341,11 @@ void RobotStateDisplay::unsetLinkColor(const std::string& link_name) unsetLinkColor(&robot_->getRobot(), link_name); } +void RobotStateDisplay::setVisible(bool visible) +{ + robot_->setVisible(visible); +} + void RobotStateDisplay::setLinkColor(rviz::Robot* robot, const std::string& link_name, const QColor& color) { rviz::RobotLink* link = robot->getLink(link_name); @@ -416,7 +421,10 @@ void RobotStateDisplay::update(float wall_dt, float ros_dt) if (load_robot_model_) { loadRobotModel(); + // The following call to changedRobotStateTopic() should not change visibility + bool visible = robot_->isVisible(); changedRobotStateTopic(); + robot_->setVisible(visible); } calculateOffsetPosition(); diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 511463a968..75080ac11e 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -397,6 +397,7 @@ void SetupAssistantWidget::loadRviz() // Set robot description robot_state_display_->subProp("Robot Description")->setValue(QString::fromStdString(ROBOT_DESCRIPTION)); + robot_state_display_->setVisible(true); // Zoom into robot rviz::ViewController* view = rviz_manager_->getViewManager()->getCurrent(); From 44eb78cdb7ec31e779d36af980164ea857a20f78 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 20 Nov 2019 14:50:14 +0100 Subject: [PATCH 081/612] add missing dependencies to library (#1746) otherwise this can fail when building with moveit_msgs in one workspace. --- moveit_core/utils/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index 8a9aaa49bd..51bc2c8a71 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -5,7 +5,7 @@ add_library(${MOVEIT_LIB_NAME} src/xmlrpc_casts.cpp src/message_checks.cpp ) - +add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") From 868439fda818f73bd1768fcaf006e584520121b0 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Wed, 20 Nov 2019 17:47:03 +0100 Subject: [PATCH 082/612] Allow subclassing of point_containment_filter::ShapeMask. (#1457) --- .../point_containment_filter/shape_mask.h | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h index 67b1b01c7f..facd1d1319 100644 --- a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h +++ b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h @@ -68,7 +68,7 @@ class ShapeMask ShapeMask(const TransformCallback& transform_callback = TransformCallback()); /** \brief Destructor to clean up */ - ~ShapeMask(); + virtual ~ShapeMask(); ShapeHandle addShape(const shapes::ShapeConstPtr& shape, double scale = 1.0, double padding = 0.0); void removeShape(ShapeHandle handle); @@ -90,7 +90,7 @@ class ShapeMask It is assumed the point is in the frame corresponding to the TransformCallback */ int getMaskContainment(const Eigen::Vector3d& pt) const; -private: +protected: struct SeeShape { SeeShape() @@ -115,16 +115,19 @@ class ShapeMask } }; + TransformCallback transform_callback_; + + /** \brief Protects, bodies_ and bspheres_. All public methods acquire this mutex for their whole duration. */ + mutable boost::mutex shapes_lock_; + std::set bodies_; + std::vector bspheres_; + +private: /** \brief Free memory. */ void freeMemory(); - TransformCallback transform_callback_; ShapeHandle next_handle_; ShapeHandle min_handle_; - - mutable boost::mutex shapes_lock_; - std::set bodies_; std::map::iterator> used_handles_; - std::vector bspheres_; }; } From 683d6554bae0848f975e2916ab8da85b52bb21fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 20 Nov 2019 20:36:48 +0100 Subject: [PATCH 083/612] delete IKCache copy constructor (#1750) It never existed, because the class contains a mutex object (not copyable). clang complained about this discrepancy. --- .../cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h index 1d4a76286f..69d5a766a8 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h @@ -90,7 +90,7 @@ class IKCache IKCache(); ~IKCache(); - IKCache(const IKCache&) = default; + IKCache(const IKCache&) = delete; /** get the entry from the IK cache that best matches a given pose */ const IKEntry& getBestApproximateIKSolution(const Pose& pose) const; From cd202abcabbc13191bbe601446b0039b372cca7f Mon Sep 17 00:00:00 2001 From: Bryce Willey Date: Wed, 20 Nov 2019 15:38:57 -0500 Subject: [PATCH 084/612] Constraints Library Refactor (#1428) * Moved constraints library internal to the ModelBasedPlanningContext In preparation to a larger change that moves much of the context initialization internal to the ModelBasedPlanningContext as well. * Removed unused OMPLPlannerService. * Fixed tests since ModelBasedStateSpace is now virtual * Corrected switched comments * Fixed compile issues, and got construct database to work. Also added launch file for ease of use. * Only install the generate state db script to CATKIN_PACKAGE_BIN_DEST --- .../ompl/ompl_interface/CMakeLists.txt | 10 +- .../{ => detail}/constraints_library.h | 15 +- .../model_based_planning_context.h | 27 ++- .../moveit/ompl_interface/ompl_interface.h | 28 --- .../joint_space/joint_model_state_space.h | 5 + .../model_based_state_space.h | 2 + .../work_space/pose_model_state_space.h | 5 + .../ompl_interface/planning_context_manager.h | 6 +- .../launch/generate_state_database.launch | 14 ++ .../generate_state_database.cpp | 29 +++- .../src/{ => detail}/constraints_library.cpp | 164 ++++++++++-------- .../src/model_based_planning_context.cpp | 159 ++++++++++------- .../ompl_interface/src/ompl_interface.cpp | 56 +----- .../ompl/ompl_interface/src/ompl_planner.cpp | 153 ---------------- .../src/planning_context_manager.cpp | 28 +-- .../ompl_interface/test/test_state_space.cpp | 8 +- 16 files changed, 285 insertions(+), 424 deletions(-) rename moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/{ => detail}/constraints_library.h (91%) create mode 100644 moveit_planners/ompl/ompl_interface/launch/generate_state_database.launch rename moveit_planners/ompl/ompl_interface/{src => scripts}/generate_state_database.cpp (84%) rename moveit_planners/ompl/ompl_interface/src/{ => detail}/constraints_library.cpp (79%) delete mode 100644 moveit_planners/ompl/ompl_interface/src/ompl_planner.cpp diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index 0f7c18bbb9..9e9bc57664 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -3,7 +3,6 @@ set(MOVEIT_LIB_NAME moveit_ompl_interface) add_library(${MOVEIT_LIB_NAME} src/ompl_interface.cpp src/planning_context_manager.cpp - src/constraints_library.cpp src/model_based_planning_context.cpp src/parameterization/model_based_state_space.cpp src/parameterization/model_based_state_space_factory.cpp @@ -15,6 +14,7 @@ add_library(${MOVEIT_LIB_NAME} src/detail/state_validity_checker.cpp src/detail/projection_evaluators.cpp src/detail/goal_union.cpp + src/detail/constraints_library.cpp src/detail/constrained_sampler.cpp src/detail/constrained_valid_state_sampler.cpp src/detail/constrained_goal_sampler.cpp @@ -27,11 +27,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES} $ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") -add_executable(moveit_ompl_planner src/ompl_planner.cpp) -target_link_libraries(moveit_ompl_planner ${MOVEIT_LIB_NAME}) -set_target_properties(moveit_ompl_planner PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") - -add_executable(moveit_generate_state_database src/generate_state_database.cpp) +add_executable(moveit_generate_state_database scripts/generate_state_database.cpp) target_link_libraries(moveit_generate_state_database ${MOVEIT_LIB_NAME}) set_target_properties(moveit_generate_state_database PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") set_target_properties(moveit_generate_state_database PROPERTIES OUTPUT_NAME "generate_state_database") @@ -44,7 +40,7 @@ install(TARGETS ${MOVEIT_LIB_NAME} moveit_ompl_planner_plugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) -install(TARGETS moveit_ompl_planner moveit_generate_state_database +install(TARGETS moveit_generate_state_database RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/constraints_library.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h similarity index 91% rename from moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/constraints_library.h rename to moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h index 18c3888fc4..4e93e0ff8b 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/constraints_library.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h @@ -37,7 +37,7 @@ #pragma once #include -#include +#include #include #include #include @@ -160,7 +160,7 @@ MOVEIT_CLASS_FORWARD(ConstraintsLibrary); class ConstraintsLibrary { public: - ConstraintsLibrary(const PlanningContextManager& pcontext) : context_manager_(pcontext) + ConstraintsLibrary(ModelBasedPlanningContext* pcontext) : context_(pcontext) { } @@ -190,12 +190,13 @@ class ConstraintsLibrary const ConstraintApproximationPtr& getConstraintApproximation(const moveit_msgs::Constraints& msg) const; private: - ompl::base::StateStoragePtr constructConstraintApproximation( - const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling, - const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options, - ConstraintApproximationConstructionResults& result); + ompl::base::StateStoragePtr + constructConstraintApproximation(ModelBasedPlanningContext* pcontext, const moveit_msgs::Constraints& constr_sampling, + const moveit_msgs::Constraints& constr_hard, + const ConstraintApproximationConstructionOptions& options, + ConstraintApproximationConstructionResults& result); - const PlanningContextManager& context_manager_; + ModelBasedPlanningContext* context_; std::map constraint_approximations_; }; } diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index 022aa4ab29..3ce1fa57b7 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -65,7 +65,6 @@ struct ModelBasedPlanningContextSpecification { std::map config_; ConfiguredPlannerSelector planner_selector_; - ConstraintsLibraryConstPtr constraints_library_; constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; ModelBasedStateSpacePtr state_space_; @@ -241,9 +240,19 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext const moveit_msgs::Constraints& path_constraints, moveit_msgs::MoveItErrorCodes* error); bool setPathConstraints(const moveit_msgs::Constraints& path_constraints, moveit_msgs::MoveItErrorCodes* error); - void setConstraintsApproximations(const ConstraintsLibraryConstPtr& constraints_library) + void setConstraintsApproximations(const ConstraintsLibraryPtr& constraints_library) { - spec_.constraints_library_ = constraints_library; + constraints_library_ = constraints_library; + } + + ConstraintsLibraryPtr getConstraintsLibraryNonConst() + { + return constraints_library_; + } + + const ConstraintsLibraryPtr& getConstraintsLibrary() const + { + return constraints_library_; } bool useStateValidityCache() const @@ -304,7 +313,15 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext void convertPath(const og::PathGeometric& pg, robot_trajectory::RobotTrajectory& traj) const; - virtual void configure(); + /** @brief Look up param server 'constraint_approximations' and use its value as the path to load constraint + * approximations to */ + bool loadConstraintApproximations(const ros::NodeHandle& nh); + + /** @brief Look up param server 'constraint_approximations' and use its value as the path to save constraint + * approximations to */ + bool saveConstraintApproximations(const ros::NodeHandle& nh); + + virtual void configure(const ros::NodeHandle& nh, bool use_constraints_approximations); protected: void preSolve(); @@ -373,6 +390,8 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext bool use_state_validity_cache_; + ConstraintsLibraryPtr constraints_library_; + bool simplify_solutions_; }; } // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h index 5dc11716ee..e856b1af14 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h @@ -37,7 +37,6 @@ #pragma once #include -#include #include #include #include @@ -86,9 +85,6 @@ class OMPLInterface const planning_interface::MotionPlanRequest& req, moveit_msgs::MoveItErrorCodes& error_code) const; - ModelBasedPlanningContextPtr getPlanningContext(const std::string& config, - const std::string& factory_type = "") const; - const PlanningContextManager& getPlanningContextManager() const { return context_manager_; @@ -99,16 +95,6 @@ class OMPLInterface return context_manager_; } - ConstraintsLibrary& getConstraintsLibrary() - { - return *constraints_library_; - } - - const ConstraintsLibrary& getConstraintsLibrary() const - { - return *constraints_library_; - } - constraint_samplers::ConstraintSamplerManager& getConstraintSamplerManager() { return *constraint_sampler_manager_; @@ -128,11 +114,6 @@ class OMPLInterface { return use_constraints_approximations_; } - - void loadConstraintApproximations(const std::string& path); - - void saveConstraintApproximations(const std::string& path); - bool simplifySolutions() const { return simplify_solutions_; @@ -143,14 +124,6 @@ class OMPLInterface simplify_solutions_ = flag; } - /** @brief Look up param server 'constraint_approximations' and use its value as the path to save constraint - * approximations to */ - bool saveConstraintApproximations(); - - /** @brief Look up param server 'constraint_approximations' and use its value as the path to load constraint - * approximations to */ - bool loadConstraintApproximations(); - /** @brief Print the status of this node*/ void printStatus(); @@ -183,7 +156,6 @@ class OMPLInterface PlanningContextManager context_manager_; - ConstraintsLibraryPtr constraints_library_; bool use_constraints_approximations_; bool simplify_solutions_; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h index 4a8c9073fa..c82897307b 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h @@ -46,5 +46,10 @@ class JointModelStateSpace : public ModelBasedStateSpace static const std::string PARAMETERIZATION_TYPE; JointModelStateSpace(const ModelBasedStateSpaceSpecification& spec); + + const std::string& getParameterizationType() const override + { + return PARAMETERIZATION_TYPE; + } }; } diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h index 106a4a2c41..dbad573239 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h @@ -200,6 +200,8 @@ class ModelBasedStateSpace : public ompl::base::StateSpace ompl::base::StateSamplerPtr allocDefaultStateSampler() const override; + virtual const std::string& getParameterizationType() const = 0; + const robot_model::RobotModelConstPtr& getRobotModel() const { return spec_.robot_model_; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h index a0d632c527..b38d5f782b 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h @@ -110,6 +110,11 @@ class PoseModelStateSpace : public ModelBasedStateSpace void copyToOMPLState(ompl::base::State* state, const robot_state::RobotState& rstate) const override; void sanityChecks() const override; + const std::string& getParameterizationType() const override + { + return PARAMETERIZATION_TYPE; + } + private: struct PoseComponent { diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index c8d2f9fcb7..29a39e2542 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -140,12 +140,10 @@ class PlanningContextManager return robot_model_; } - ModelBasedPlanningContextPtr getPlanningContext(const std::string& config, - const std::string& factory_type = "") const; - ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, - moveit_msgs::MoveItErrorCodes& error_code) const; + moveit_msgs::MoveItErrorCodes& error_code, const ros::NodeHandle& nh, + bool use_constraints_approximations) const; void registerPlannerAllocator(const std::string& planner_id, const ConfiguredPlannerAllocator& pa) { diff --git a/moveit_planners/ompl/ompl_interface/launch/generate_state_database.launch b/moveit_planners/ompl/ompl_interface/launch/generate_state_database.launch new file mode 100644 index 0000000000..7aa2479d8a --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/launch/generate_state_database.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/moveit_planners/ompl/ompl_interface/src/generate_state_database.cpp b/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp similarity index 84% rename from moveit_planners/ompl/ompl_interface/src/generate_state_database.cpp rename to moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp index e2cbf39c25..d089536586 100644 --- a/moveit_planners/ompl/ompl_interface/src/generate_state_database.cpp +++ b/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp @@ -36,10 +36,12 @@ /* Authors: Ioan Sucan, Michael Goerner */ #include +#include #include #include #include +#include #include #include @@ -119,26 +121,47 @@ void computeDB(const planning_scene::PlanningScenePtr& scene, struct GenerateSta scene->getCurrentStateNonConst().update(); ompl_interface::OMPLInterface ompl_interface(scene->getRobotModel()); + planning_interface::MotionPlanRequest req; + req.group_name = params.planning_group; + req.path_constraints = params.constraints; + moveit::core::robotStateToRobotStateMsg(scene->getCurrentState(), req.start_state); + req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + scene->getCurrentState(), scene->getRobotModel()->getJointModelGroup(params.planning_group))); + + ompl_interface::ModelBasedPlanningContextPtr context = ompl_interface.getPlanningContext(scene, req); ROS_INFO_STREAM_NAMED(LOGNAME, "Generating Joint Space Constraint Approximation Database for constraint:\n" << params.constraints); ompl_interface::ConstraintApproximationConstructionResults result = - ompl_interface.getConstraintsLibrary().addConstraintApproximation(params.constraints, params.planning_group, - scene, params.construction_opts); + context->getConstraintsLibraryNonConst()->addConstraintApproximation(params.constraints, params.planning_group, + scene, params.construction_opts); if (!result.approx) { ROS_FATAL_NAMED(LOGNAME, "Failed to generate approximation."); return; } - ompl_interface.getConstraintsLibrary().saveConstraintApproximations(params.output_folder); + context->getConstraintsLibraryNonConst()->saveConstraintApproximations(params.output_folder); ROS_INFO_STREAM_NAMED(LOGNAME, "Successfully generated Joint Space Constraint Approximation Database for constraint:\n" << params.constraints); ROS_INFO_STREAM_NAMED(LOGNAME, "The database has been saved in your local folder '" << params.output_folder << "'"); } +/** + * Generates a database of states that follow the given constraints. + * An example of the constraint yaml that should be loaded to rosparam: + * "name: tool0_upright + * constraints: + * - type: orientation + * frame_id: world + * link_name: tool0 + * orientation: [0, 0, 0] + * tolerances: [0.01, 0.01, 3.15] + * weight: 1.0 + * " + */ int main(int argc, char** argv) { ros::init(argc, argv, "construct_tool_constraint_database", ros::init_options::AnonymousName); diff --git a/moveit_planners/ompl/ompl_interface/src/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp similarity index 79% rename from moveit_planners/ompl/ompl_interface/src/constraints_library.cpp rename to moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index 9c65e53513..7e77582313 100644 --- a/moveit_planners/ompl/ompl_interface/src/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include @@ -301,30 +301,35 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: if (fin.eof()) break; fin >> filename; - ROS_INFO_NAMED("constraints_library", "Loading constraint approximation of type '%s' for group '%s' from '%s'...", - state_space_parameterization.c_str(), group.c_str(), filename.c_str()); - const ModelBasedPlanningContextPtr& pc = context_manager_.getPlanningContext(group, state_space_parameterization); - if (pc) + + if (context_->getGroupName() != group && + context_->getOMPLStateSpace()->getParameterizationType() != state_space_parameterization) { - moveit_msgs::Constraints msg; - hexToMsg(serialization, msg); - auto* cass = new ConstraintApproximationStateStorage(pc->getOMPLSimpleSetup()->getStateSpace()); - cass->load((path + "/" + filename).c_str()); - ConstraintApproximationPtr cap(new ConstraintApproximation(group, state_space_parameterization, explicit_motions, - msg, filename, ompl::base::StateStoragePtr(cass), - milestones)); - if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end()) - ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", - cap->getName().c_str()); - constraint_approximations_[cap->getName()] = cap; - std::size_t sum = 0; - for (std::size_t i = 0; i < cass->size(); ++i) - sum += cass->getMetadata(i).first.size(); - ROS_INFO_NAMED("constraints_library", "Loaded %lu states (%lu milestones) and %lu connections (%0.1lf per state) " - "for constraint named '%s'%s", - cass->size(), cap->getMilestoneCount(), sum, (double)sum / (double)cap->getMilestoneCount(), - msg.name.c_str(), explicit_motions ? ". Explicit motions included." : ""); + ROS_INFO_NAMED("constraints_library", + "Ignoring constraint approximation of type '%s' for group '%s' from '%s'...", + state_space_parameterization.c_str(), group.c_str(), filename.c_str()); + continue; } + + ROS_INFO_NAMED("constraints_library", "Loading constraint approximation of type '%s' for group '%s' from '%s'...", + state_space_parameterization.c_str(), group.c_str(), filename.c_str()); + moveit_msgs::Constraints msg; + hexToMsg(serialization, msg); + auto* cass = new ConstraintApproximationStateStorage(context_->getOMPLSimpleSetup()->getStateSpace()); + cass->load((path + "/" + filename).c_str()); + ConstraintApproximationPtr cap(new ConstraintApproximation(group, state_space_parameterization, explicit_motions, + msg, filename, ompl::base::StateStoragePtr(cass), + milestones)); + if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end()) + ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", cap->getName().c_str()); + constraint_approximations_[cap->getName()] = cap; + std::size_t sum = 0; + for (std::size_t i = 0; i < cass->size(); ++i) + sum += cass->getMetadata(i).first.size(); + ROS_INFO_NAMED("constraints_library", "Loaded %lu states (%lu milestones) and %lu connections (%0.1lf per state) " + "for constraint named '%s'%s", + cass->size(), cap->getMilestoneCount(), sum, (double)sum / (double)cap->getMilestoneCount(), + msg.name.c_str(), explicit_motions ? ". Explicit motions included." : ""); } ROS_INFO_NAMED("constraints_library", "Done loading constrained space approximations."); } @@ -407,44 +412,50 @@ ompl_interface::ConstraintsLibrary::addConstraintApproximation( const ConstraintApproximationConstructionOptions& options) { ConstraintApproximationConstructionResults res; - ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, options.state_space_parameterization); - if (pc) + if (context_->getGroupName() != group && + context_->getOMPLStateSpace()->getParameterizationType() != options.state_space_parameterization) { - pc->clear(); - pc->setPlanningScene(scene); - pc->setCompleteInitialState(scene->getCurrentState()); - - ros::WallTime start = ros::WallTime::now(); - ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, options, res); - ROS_INFO_NAMED("constraints_library", "Spent %lf seconds constructing the database", - (ros::WallTime::now() - start).toSec()); - if (ss) - { - ConstraintApproximationPtr ca(new ConstraintApproximation( - group, options.state_space_parameterization, options.explicit_motions, constr_hard, - group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + - ".ompldb", - ss, res.milestones)); - if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end()) - ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", ca->getName().c_str()); - constraint_approximations_[ca->getName()] = ca; - res.approx = ca; - } - else - ROS_ERROR_NAMED("constraints_library", "Unable to construct constraint approximation for group '%s'", - group.c_str()); + ROS_INFO_NAMED("constraints_library", "Ignoring constraint approximation of type '%s' for group '%s'...", + options.state_space_parameterization.c_str(), group.c_str()); + return res; } + + context_->clear(); + context_->setPlanningScene(scene); + context_->setCompleteInitialState(scene->getCurrentState()); + + ros::WallTime start = ros::WallTime::now(); + ompl::base::StateStoragePtr state_storage = + constructConstraintApproximation(context_, constr_sampling, constr_hard, options, res); + ROS_INFO_NAMED("constraints_library", "Spent %lf seconds constructing the database", + (ros::WallTime::now() - start).toSec()); + if (state_storage) + { + ConstraintApproximationPtr constraint_approx(new ConstraintApproximation( + group, options.state_space_parameterization, options.explicit_motions, constr_hard, + group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + + ".ompldb", + state_storage, res.milestones)); + if (constraint_approximations_.find(constraint_approx->getName()) != constraint_approximations_.end()) + ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", + constraint_approx->getName().c_str()); + constraint_approximations_[constraint_approx->getName()] = constraint_approx; + res.approx = constraint_approx; + } + else + ROS_ERROR_NAMED("constraints_library", "Unable to construct constraint approximation for group '%s'", + group.c_str()); return res; } ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstraintApproximation( - const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling, + ModelBasedPlanningContext* pcontext, const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options, ConstraintApproximationConstructionResults& result) { // state storage structure ConstraintApproximationStateStorage* cass = new ConstraintApproximationStateStorage(pcontext->getOMPLStateSpace()); - ob::StateStoragePtr sstor(cass); + ob::StateStoragePtr state_storage(cass); // construct a sampler for the sampling constraints kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel()); @@ -464,39 +475,40 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra robot_state::RobotState robot_state(default_state); const constraint_samplers::ConstraintSamplerManagerPtr& csmng = pcontext->getConstraintSamplerManager(); - ConstrainedSampler* csmp = nullptr; + ConstrainedSampler* constrained_sampler = nullptr; if (csmng) { - constraint_samplers::ConstraintSamplerPtr cs = + constraint_samplers::ConstraintSamplerPtr constraint_sampler = csmng->selectSampler(pcontext->getPlanningScene(), pcontext->getJointModelGroup()->getName(), constr_sampling); - if (cs) - csmp = new ConstrainedSampler(pcontext.get(), cs); + if (constraint_sampler) + constrained_sampler = new ConstrainedSampler(pcontext, constraint_sampler); } - ob::StateSamplerPtr ss(csmp ? ob::StateSamplerPtr(csmp) : pcontext->getOMPLStateSpace()->allocDefaultStateSampler()); + ob::StateSamplerPtr ss(constrained_sampler ? ob::StateSamplerPtr(constrained_sampler) : + pcontext->getOMPLStateSpace()->allocDefaultStateSampler()); ompl::base::ScopedState<> temp(pcontext->getOMPLStateSpace()); int done = -1; bool slow_warn = false; ompl::time::point start = ompl::time::now(); - while (sstor->size() < options.samples) + while (state_storage->size() < options.samples) { ++attempts; - int done_now = 100 * sstor->size() / options.samples; + int done_now = 100 * state_storage->size() / options.samples; if (done != done_now) { done = done_now; ROS_INFO_NAMED("constraints_library", "%d%% complete (kept %0.1lf%% sampled states)", done, - 100.0 * (double)sstor->size() / (double)attempts); + 100.0 * (double)state_storage->size() / (double)attempts); } - if (!slow_warn && attempts > 10 && attempts > sstor->size() * 100) + if (!slow_warn && attempts > 10 && attempts > state_storage->size() * 100) { slow_warn = true; ROS_WARN_NAMED("constraints_library", "Computation of valid state database is very slow..."); } - if (attempts > options.samples && sstor->size() == 0) + if (attempts > options.samples && state_storage->size() == 0) { ROS_ERROR_NAMED("constraints_library", "Unable to generate any samples"); break; @@ -506,24 +518,24 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra pcontext->getOMPLStateSpace()->copyToRobotState(robot_state, temp.get()); if (kset.decide(robot_state).satisfied) { - if (sstor->size() < options.samples) + if (state_storage->size() < options.samples) { - temp->as()->tag = sstor->size(); - sstor->addState(temp.get()); + temp->as()->tag = state_storage->size(); + state_storage->addState(temp.get()); } } } result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start); - ROS_INFO_NAMED("constraints_library", "Generated %u states in %lf seconds", (unsigned int)sstor->size(), + ROS_INFO_NAMED("constraints_library", "Generated %u states in %lf seconds", (unsigned int)state_storage->size(), result.state_sampling_time); - if (csmp) + if (constrained_sampler) { - result.sampling_success_rate = csmp->getConstrainedSamplingRate(); + result.sampling_success_rate = constrained_sampler->getConstrainedSamplingRate(); ROS_INFO_NAMED("constraints_library", "Constrained sampling rate: %lf", result.sampling_success_rate); } - result.milestones = sstor->size(); + result.milestones = state_storage->size(); if (options.edges_per_sample > 0) { ROS_INFO_NAMED("constraints_library", "Computing graph connections (max %u edges per sample) ...", @@ -531,7 +543,7 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra // construct connexions const ob::StateSpacePtr& space = pcontext->getOMPLSimpleSetup()->getStateSpace(); - unsigned int milestones = sstor->size(); + unsigned int milestones = state_storage->size(); std::vector int_states(options.max_explicit_points, nullptr); pcontext->getOMPLSimpleSetup()->getSpaceInformation()->allocStates(int_states); @@ -550,20 +562,20 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra if (cass->getMetadata(j).first.size() >= options.edges_per_sample) continue; - const ob::State* sj = sstor->getState(j); + const ob::State* sj = state_storage->getState(j); for (std::size_t i = j + 1; i < milestones; ++i) { if (cass->getMetadata(i).first.size() >= options.edges_per_sample) continue; - double d = space->distance(sstor->getState(i), sj); + double d = space->distance(state_storage->getState(i), sj); if (d >= options.max_edge_length) continue; unsigned int isteps = std::min(options.max_explicit_points, d / options.explicit_points_resolution); double step = 1.0 / (double)isteps; bool ok = true; - space->interpolate(sstor->getState(i), sj, step, int_states[0]); + space->interpolate(state_storage->getState(i), sj, step, int_states[0]); for (unsigned int k = 1; k < isteps; ++k) { double this_step = step / (1.0 - (k - 1) * step); @@ -583,13 +595,13 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra if (options.explicit_motions) { - cass->getMetadata(i).second[j].first = sstor->size(); + cass->getMetadata(i).second[j].first = state_storage->size(); for (unsigned int k = 0; k < isteps; ++k) { int_states[k]->as()->tag = -1; - sstor->addState(int_states[k]); + state_storage->addState(int_states[k]); } - cass->getMetadata(i).second[j].second = sstor->size(); + cass->getMetadata(i).second[j].second = state_storage->size(); cass->getMetadata(j).second[i] = cass->getMetadata(i).second[j]; } @@ -605,11 +617,11 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra result.state_connection_time, good); pcontext->getOMPLSimpleSetup()->getSpaceInformation()->freeStates(int_states); - return sstor; + return state_storage; } // TODO(davetcoleman): this function did not originally return a value, causing compiler warnings in ROS Melodic // Update with more intelligent logic as needed ROS_ERROR_NAMED("constraints_library", "No StateStoragePtr found - implement better solution here."); - return sstor; + return state_storage; } diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 16aeb154e3..609431be37 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -42,7 +42,8 @@ #include #include #include -#include +#include + #include #include #include @@ -80,9 +81,43 @@ ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std:: , simplify_solutions_(true) { complete_initial_robot_state_.update(); + + constraints_library_ = std::make_shared(this); +} + +void ompl_interface::ModelBasedPlanningContext::configure(const ros::NodeHandle& nh, + bool use_constraints_approximations) +{ + loadConstraintApproximations(nh); + if (!use_constraints_approximations) + { + setConstraintsApproximations(ConstraintsLibraryPtr()); + } + complete_initial_robot_state_.update(); ompl_simple_setup_->getStateSpace()->computeSignature(space_signature_); ompl_simple_setup_->getStateSpace()->setStateSamplerAllocator( std::bind(&ModelBasedPlanningContext::allocPathConstrainedSampler, this, std::placeholders::_1)); + + // convert the input state to the corresponding OMPL state + ompl::base::ScopedState<> ompl_start_state(spec_.state_space_); + spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState()); + ompl_simple_setup_->setStartState(ompl_start_state); + ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(new StateValidityChecker(this))); + + if (path_constraints_ && constraints_library_) + { + const ConstraintApproximationPtr& constraint_approx = + constraints_library_->getConstraintApproximation(path_constraints_msg_); + if (constraint_approx) + { + getOMPLStateSpace()->setInterpolationFunction(constraint_approx->getInterpolationFunction()); + ROS_INFO_NAMED("model_based_planning_context", "Using precomputed interpolation states"); + } + } + + useConfig(); + if (ompl_simple_setup_->getGoal()) + ompl_simple_setup_->setup(); } void ompl_interface::ModelBasedPlanningContext::setProjectionEvaluator(const std::string& peval) @@ -92,9 +127,9 @@ void ompl_interface::ModelBasedPlanningContext::setProjectionEvaluator(const std ROS_ERROR_NAMED("model_based_planning_context", "No state space is configured yet"); return; } - ob::ProjectionEvaluatorPtr pe = getProjectionEvaluator(peval); - if (pe) - spec_.state_space_->registerDefaultProjection(pe); + ob::ProjectionEvaluatorPtr projection_eval = getProjectionEvaluator(peval); + if (projection_eval) + spec_.state_space_->registerDefaultProjection(projection_eval); } ompl::base::ProjectionEvaluatorPtr @@ -119,26 +154,26 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str std::stringstream ss(joints); while (ss.good() && !ss.eof()) { - std::string v; - ss >> v >> std::ws; - if (getJointModelGroup()->hasJointModel(v)) + std::string joint; + ss >> joint >> std::ws; + if (getJointModelGroup()->hasJointModel(joint)) { - unsigned int vc = getJointModelGroup()->getJointModel(v)->getVariableCount(); - if (vc > 0) + unsigned int variable_count = getJointModelGroup()->getJointModel(joint)->getVariableCount(); + if (variable_count > 0) { - int idx = getJointModelGroup()->getVariableGroupIndex(v); - for (unsigned int q = 0; q < vc; ++q) + int idx = getJointModelGroup()->getVariableGroupIndex(joint); + for (unsigned int q = 0; q < variable_count; ++q) j.push_back(idx + q); } else ROS_WARN_NAMED("model_based_planning_context", "%s: Ignoring joint '%s' in projection since it has 0 DOF", - name_.c_str(), v.c_str()); + name_.c_str(), joint.c_str()); } else ROS_ERROR_NAMED("model_based_planning_context", "%s: Attempted to set projection evaluator with respect to value of joint " "'%s', but that joint is not known to the group '%s'.", - name_.c_str(), v.c_str(), getGroupName().c_str()); + name_.c_str(), joint.c_str(), getGroupName().c_str()); } if (j.empty()) ROS_ERROR_NAMED("model_based_planning_context", "%s: No valid joints specified for joint projection", @@ -153,9 +188,9 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str } ompl::base::StateSamplerPtr -ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const ompl::base::StateSpace* ss) const +ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const ompl::base::StateSpace* state_space) const { - if (spec_.state_space_.get() != ss) + if (spec_.state_space_.get() != state_space) { ROS_ERROR_NAMED("model_based_planning_context", "%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str()); @@ -167,66 +202,43 @@ ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const omp if (path_constraints_) { - if (spec_.constraints_library_) + if (constraints_library_) { - const ConstraintApproximationPtr& ca = - spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_); - if (ca) + const ConstraintApproximationPtr& constraint_approx = + constraints_library_->getConstraintApproximation(path_constraints_msg_); + if (constraint_approx) { - ompl::base::StateSamplerAllocator c_ssa = ca->getStateSamplerAllocator(path_constraints_msg_); - if (c_ssa) + ompl::base::StateSamplerAllocator state_sampler_allocator = + constraint_approx->getStateSamplerAllocator(path_constraints_msg_); + if (state_sampler_allocator) { - ompl::base::StateSamplerPtr res = c_ssa(ss); - if (res) + ompl::base::StateSamplerPtr state_sampler = state_sampler_allocator(state_space); + if (state_sampler) { ROS_INFO_NAMED("model_based_planning_context", "%s: Using precomputed state sampler (approximated constraint space) for constraint '%s'", name_.c_str(), path_constraints_msg_.name.c_str()); - return res; + return state_sampler; } } } } - constraint_samplers::ConstraintSamplerPtr cs; + constraint_samplers::ConstraintSamplerPtr constraint_sampler; if (spec_.constraint_sampler_manager_) - cs = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(), - path_constraints_->getAllConstraints()); + constraint_sampler = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(), + path_constraints_->getAllConstraints()); - if (cs) + if (constraint_sampler) { ROS_INFO_NAMED("model_based_planning_context", "%s: Allocating specialized state sampler for state space", name_.c_str()); - return ob::StateSamplerPtr(new ConstrainedSampler(this, cs)); + return ob::StateSamplerPtr(new ConstrainedSampler(this, constraint_sampler)); } } ROS_DEBUG_NAMED("model_based_planning_context", "%s: Allocating default state sampler for state space", name_.c_str()); - return ss->allocDefaultStateSampler(); -} - -void ompl_interface::ModelBasedPlanningContext::configure() -{ - // convert the input state to the corresponding OMPL state - ompl::base::ScopedState<> ompl_start_state(spec_.state_space_); - spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState()); - ompl_simple_setup_->setStartState(ompl_start_state); - ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(new StateValidityChecker(this))); - - if (path_constraints_ && spec_.constraints_library_) - { - const ConstraintApproximationPtr& ca = - spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_); - if (ca) - { - getOMPLStateSpace()->setInterpolationFunction(ca->getInterpolationFunction()); - ROS_INFO_NAMED("model_based_planning_context", "Using precomputed interpolation states"); - } - } - - useConfig(); - if (ompl_simple_setup_->getGoal()) - ompl_simple_setup_->setup(); + return state_space->allocDefaultStateSampler(); } void ompl_interface::ModelBasedPlanningContext::useConfig() @@ -424,14 +436,14 @@ ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal() std::vector goals; for (kinematic_constraints::KinematicConstraintSetPtr& goal_constraint : goal_constraints_) { - constraint_samplers::ConstraintSamplerPtr cs; + constraint_samplers::ConstraintSamplerPtr constraint_sampler; if (spec_.constraint_sampler_manager_) - cs = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(), - goal_constraint->getAllConstraints()); - if (cs) + constraint_sampler = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(), + goal_constraint->getAllConstraints()); + if (constraint_sampler) { - ob::GoalPtr g = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraint, cs)); - goals.push_back(g); + ob::GoalPtr goal = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraint, constraint_sampler)); + goals.push_back(goal); } } @@ -737,3 +749,30 @@ bool ompl_interface::ModelBasedPlanningContext::terminate() ptc_->terminate(); return true; } + +bool ompl_interface::ModelBasedPlanningContext::saveConstraintApproximations(const ros::NodeHandle& nh) +{ + std::string constraint_path; + if (nh.getParam("constraint_approximations_path", constraint_path)) + { + constraints_library_->saveConstraintApproximations(constraint_path); + return true; + } + ROS_WARN("ROS param 'constraint_approximations' not found. Unable to save constraint approximations"); + return false; +} + +bool ompl_interface::ModelBasedPlanningContext::loadConstraintApproximations(const ros::NodeHandle& nh) +{ + std::string constraint_path; + if (nh.getParam("constraint_approximations_path", constraint_path)) + { + loadConstraintApproximations(constraint_path); + constraints_library_->loadConstraintApproximations(constraint_path); + std::stringstream ss; + constraints_library_->printConstraintApproximations(ss); + ROS_INFO_STREAM(ss.str()); + return true; + } + return false; +} diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index d5ee41b812..8f68566bd2 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -48,13 +48,11 @@ ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstP , robot_model_(robot_model) , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager()) , context_manager_(robot_model, constraint_sampler_manager_) - , constraints_library_(new ConstraintsLibrary(context_manager_)) , use_constraints_approximations_(true) , simplify_solutions_(true) { ROS_INFO("Initializing OMPL interface using ROS parameters"); loadPlannerConfigurations(); - loadConstraintApproximations(); loadConstraintSamplers(); } @@ -65,13 +63,11 @@ ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstP , robot_model_(robot_model) , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager()) , context_manager_(robot_model, constraint_sampler_manager_) - , constraints_library_(new ConstraintsLibrary(context_manager_)) , use_constraints_approximations_(true) , simplify_solutions_(true) { ROS_INFO("Initializing OMPL interface using specified configuration"); setPlannerConfigurations(pconfig); - loadConstraintApproximations(); loadConstraintSamplers(); } @@ -107,16 +103,8 @@ ompl_interface::OMPLInterface::getPlanningContext(const planning_scene::Planning const planning_interface::MotionPlanRequest& req, moveit_msgs::MoveItErrorCodes& error_code) const { - ModelBasedPlanningContextPtr ctx = context_manager_.getPlanningContext(planning_scene, req, error_code); - if (ctx) - configureContext(ctx); - return ctx; -} - -ompl_interface::ModelBasedPlanningContextPtr -ompl_interface::OMPLInterface::getPlanningContext(const std::string& config, const std::string& factory_type) const -{ - ModelBasedPlanningContextPtr ctx = context_manager_.getPlanningContext(config, factory_type); + ModelBasedPlanningContextPtr ctx = + context_manager_.getPlanningContext(planning_scene, req, error_code, nh_, use_constraints_approximations_); if (ctx) configureContext(ctx); return ctx; @@ -124,49 +112,9 @@ ompl_interface::OMPLInterface::getPlanningContext(const std::string& config, con void ompl_interface::OMPLInterface::configureContext(const ModelBasedPlanningContextPtr& context) const { - if (use_constraints_approximations_) - context->setConstraintsApproximations(constraints_library_); - else - context->setConstraintsApproximations(ConstraintsLibraryPtr()); context->simplifySolutions(simplify_solutions_); } -void ompl_interface::OMPLInterface::loadConstraintApproximations(const std::string& path) -{ - constraints_library_->loadConstraintApproximations(path); - std::stringstream ss; - constraints_library_->printConstraintApproximations(ss); - ROS_INFO_STREAM(ss.str()); -} - -void ompl_interface::OMPLInterface::saveConstraintApproximations(const std::string& path) -{ - constraints_library_->saveConstraintApproximations(path); -} - -bool ompl_interface::OMPLInterface::saveConstraintApproximations() -{ - std::string cpath; - if (nh_.getParam("constraint_approximations_path", cpath)) - { - saveConstraintApproximations(cpath); - return true; - } - ROS_WARN("ROS param 'constraint_approximations' not found. Unable to save constraint approximations"); - return false; -} - -bool ompl_interface::OMPLInterface::loadConstraintApproximations() -{ - std::string cpath; - if (nh_.getParam("constraint_approximations_path", cpath)) - { - loadConstraintApproximations(cpath); - return true; - } - return false; -} - void ompl_interface::OMPLInterface::loadConstraintSamplers() { constraint_sampler_manager_loader_.reset( diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner.cpp deleted file mode 100644 index 4f4390f496..0000000000 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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. -*********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include -#include -#include -#include -#include -#include -#include - -static const std::string PLANNER_NODE_NAME = "ompl_planning"; // name of node -static const std::string PLANNER_SERVICE_NAME = - "plan_kinematic_path"; // name of the advertised service (within the ~ namespace) -static const std::string ROBOT_DESCRIPTION = - "robot_description"; // name of the robot description (a param name, so it can be changed externally) - -class OMPLPlannerService -{ -public: - OMPLPlannerService(planning_scene_monitor::PlanningSceneMonitor& psm, bool debug = false) - : nh_("~"), psm_(psm), ompl_interface_(psm.getPlanningScene()->getRobotModel()), debug_(debug) - { - plan_service_ = nh_.advertiseService(PLANNER_SERVICE_NAME, &OMPLPlannerService::computePlan, this); - if (debug_) - { - pub_plan_ = nh_.advertise("display_motion_plan", 100); - pub_request_ = nh_.advertise("motion_plan_request", 100); - } - } - - bool computePlan(moveit_msgs::GetMotionPlan::Request& req, moveit_msgs::GetMotionPlan::Response& res) - { - ROS_INFO("Received new planning request..."); - if (debug_) - pub_request_.publish(req.motion_plan_request); - planning_interface::MotionPlanResponse response; - - ompl_interface::ModelBasedPlanningContextPtr context = - ompl_interface_.getPlanningContext(psm_.getPlanningScene(), req.motion_plan_request); - if (!context) - { - ROS_ERROR_STREAM_NAMED("computePlan", "No planning context found"); - return false; - } - context->clear(); - - bool result = context->solve(response); - - if (debug_) - { - if (result) - displaySolution(res.motion_plan_response); - std::stringstream ss; - ROS_INFO("%s", ss.str().c_str()); - } - return result; - } - - void displaySolution(const moveit_msgs::MotionPlanResponse& mplan_res) - { - moveit_msgs::DisplayTrajectory d; - d.model_id = psm_.getPlanningScene()->getRobotModel()->getName(); - d.trajectory_start = mplan_res.trajectory_start; - d.trajectory.resize(1, mplan_res.trajectory); - pub_plan_.publish(d); - } - - void status() - { - ompl_interface_.printStatus(); - ROS_INFO("Responding to planning and bechmark requests"); - if (debug_) - ROS_INFO("Publishing debug information"); - } - -private: - ros::NodeHandle nh_; - planning_scene_monitor::PlanningSceneMonitor& psm_; - ompl_interface::OMPLInterface ompl_interface_; - ros::ServiceServer plan_service_; - ros::ServiceServer display_states_service_; - ros::Publisher pub_plan_; - ros::Publisher pub_request_; - bool debug_; -}; - -int main(int argc, char** argv) -{ - ros::init(argc, argv, PLANNER_NODE_NAME); - - bool debug = false; - for (int i = 1; i < argc; ++i) - if (strncmp(argv[i], "--debug", 7) == 0) - debug = true; - - ros::AsyncSpinner spinner(1); - spinner.start(); - ros::NodeHandle nh; - - std::shared_ptr tf_buffer = std::make_shared(); - std::shared_ptr tf_listener = - std::make_shared(*tf_buffer, nh); - planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION, tf_buffer); - if (psm.getPlanningScene()) - { - psm.startWorldGeometryMonitor(); - psm.startSceneMonitor(); - psm.startStateMonitor(); - - OMPLPlannerService pservice(psm, debug); - pservice.status(); - ros::waitForShutdown(); - } - else - ROS_ERROR("Planning scene not configured"); - - return 0; -} diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 8e28a585c4..b7665991b6 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -178,25 +178,6 @@ void ompl_interface::PlanningContextManager::setPlannerConfigurations( planner_configs_ = pconfig; } -ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext( - const std::string& config, const std::string& factory_type) const -{ - auto pc = planner_configs_.find(config); - - if (pc != planner_configs_.end()) - { - moveit_msgs::MotionPlanRequest req; // dummy request with default values - return getPlanningContext( - pc->second, - std::bind(&PlanningContextManager::getStateSpaceFactory1, this, std::placeholders::_1, factory_type), req); - } - else - { - ROS_ERROR_NAMED("planning_context_manager", "Planning configuration '%s' was not found", config.c_str()); - return ModelBasedPlanningContextPtr(); - } -} - ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext( const planning_interface::PlannerConfigurationSettings& config, const StateSpaceFactoryTypeSelector& factory_selector, const moveit_msgs::MotionPlanRequest& req) const @@ -320,10 +301,9 @@ const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningCo } } -ompl_interface::ModelBasedPlanningContextPtr -ompl_interface::PlanningContextManager::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::MotionPlanRequest& req, - moveit_msgs::MoveItErrorCodes& error_code) const +ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext( + const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::MotionPlanRequest& req, + moveit_msgs::MoveItErrorCodes& error_code, const ros::NodeHandle& nh, bool use_constraints_approximation) const { if (req.group_name.empty()) { @@ -402,7 +382,7 @@ ompl_interface::PlanningContextManager::getPlanningContext(const planning_scene: try { - context->configure(); + context->configure(nh, use_constraints_approximation); ROS_DEBUG_NAMED("planning_context_manager", "%s: New planning context is set.", context->getName().c_str()); error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; } diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp index cb477140eb..b704a83abd 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp @@ -107,19 +107,19 @@ TEST_F(LoadPlanningModelsPr2, StateSpace) TEST_F(LoadPlanningModelsPr2, StateSpaces) { ompl_interface::ModelBasedStateSpaceSpecification spec1(robot_model_, "right_arm"); - ompl_interface::ModelBasedStateSpace ss1(spec1); + ompl_interface::JointModelStateSpace ss1(spec1); ss1.setup(); ompl_interface::ModelBasedStateSpaceSpecification spec2(robot_model_, "left_arm"); - ompl_interface::ModelBasedStateSpace ss2(spec2); + ompl_interface::JointModelStateSpace ss2(spec2); ss2.setup(); ompl_interface::ModelBasedStateSpaceSpecification spec3(robot_model_, "whole_body"); - ompl_interface::ModelBasedStateSpace ss3(spec3); + ompl_interface::JointModelStateSpace ss3(spec3); ss3.setup(); ompl_interface::ModelBasedStateSpaceSpecification spec4(robot_model_, "arms"); - ompl_interface::ModelBasedStateSpace ss4(spec4); + ompl_interface::JointModelStateSpace ss4(spec4); ss4.setup(); std::ofstream fout("ompl_interface_test_state_space_diagram2.dot"); From bfbdaf51717149fa39fe2a59929c9dd90cb00559 Mon Sep 17 00:00:00 2001 From: Jens P Date: Wed, 20 Nov 2019 21:53:26 +0100 Subject: [PATCH 085/612] Multi threaded collision checking test (#1657) * Multi threaded collision checking test * PR review: * Moved test into planning_scene * Assert for each collision check * Fixed includes * Fixup --- moveit_core/planning_scene/CMakeLists.txt | 3 + .../test/test_multi_threaded.cpp | 135 ++++++++++++++++++ 2 files changed, 138 insertions(+) create mode 100644 moveit_core/planning_scene/test/test_multi_threaded.cpp diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt index cfa146e3ec..3ecfc4831a 100644 --- a/moveit_core/planning_scene/CMakeLists.txt +++ b/moveit_core/planning_scene/CMakeLists.txt @@ -28,4 +28,7 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_planning_scene test/test_planning_scene.cpp) target_link_libraries(test_planning_scene ${MOVEIT_LIB_NAME}) + + catkin_add_gtest(test_multi_threaded test/test_multi_threaded.cpp) + target_link_libraries(test_multi_threaded moveit_planning_scene moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES}) endif() diff --git a/moveit_core/planning_scene/test/test_multi_threaded.cpp b/moveit_core/planning_scene/test/test_multi_threaded.cpp new file mode 100644 index 0000000000..fd3aa56a1a --- /dev/null +++ b/moveit_core/planning_scene/test/test_multi_threaded.cpp @@ -0,0 +1,135 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +const int TRIALS = 1000; +const int THREADS = 2; + +bool runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene* scene, + const robot_state::RobotState* state) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + for (unsigned int i = 0; i < trials; ++i) + { + res.clear(); + scene->checkCollision(req, res, *state); + } + return res.collision; +} + +void runCollisionDetectionAssert(unsigned int id, unsigned int trials, const planning_scene::PlanningScene* scene, + const robot_state::RobotState* state, bool expected_result) +{ + ASSERT_EQ(expected_result, runCollisionDetection(id, trials, scene, state)); +} + +class CollisionDetectorThreadedTest : public testing::Test +{ +protected: + void SetUp() override + { + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + robot_model_ok_ = static_cast(robot_model_); + robot_state_.reset(new robot_state::RobotState(robot_model_)); + planning_scene_.reset(new planning_scene::PlanningScene(robot_model_)); + } + + void TearDown() override + { + } + + bool robot_model_ok_; + + robot_model::RobotModelPtr robot_model_; + + collision_detection::CollisionEnvPtr cenv_; + + collision_detection::AllowedCollisionMatrixPtr acm_; + + robot_state::RobotStatePtr robot_state_; + + planning_scene::PlanningScenePtr planning_scene_; +}; + +TEST_F(CollisionDetectorThreadedTest, InitOk) +{ + ASSERT_TRUE(robot_model_ok_); +} + +/** \brief Tests the FCL collision detector in multiple threads. */ +TEST_F(CollisionDetectorThreadedTest, FCLThreaded) +{ + std::vector states; + std::vector threads; + std::vector collisions; + + for (unsigned int i = 0; i < THREADS; ++i) + { + robot_state::RobotState* state = new robot_state::RobotState(planning_scene_->getRobotModel()); + collision_detection::CollisionRequest req; + state->setToRandomPositions(); + state->update(); + states.push_back(robot_state::RobotStatePtr(state)); + collisions.push_back(runCollisionDetection(0, 1, planning_scene_.get(), state)); + } + + for (unsigned int i = 0; i < THREADS; ++i) + threads.push_back(new std::thread( + std::bind(&runCollisionDetectionAssert, i, TRIALS, planning_scene_.get(), states[i].get(), collisions[i]))); + + for (unsigned int i = 0; i < states.size(); ++i) + { + threads[i]->join(); + delete threads[i]; + } +} + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 7566496332b32dae8293823b14162a5ae761c4e2 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 20 Nov 2019 14:12:32 -0700 Subject: [PATCH 086/612] Don't over promise looking at issues 'asap' (#1751) * Don't overpromise looking at issues 'asap' * Update .github/config.yml * Update .github/config.yml --- .github/config.yml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/.github/config.yml b/.github/config.yml index 2810cc56b6..3dd3ae1f8c 100644 --- a/.github/config.yml +++ b/.github/config.yml @@ -4,14 +4,13 @@ # Comment to be posted to on first time issues newIssueWelcomeComment: > - Thanks for reporting an issue. We will have a look asap. - If you can think of a fix, please consider providing it as a pull request. + Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed. # Configuration for new-pr-welcome - https://github.com/behaviorbot/new-pr-welcome # Comment to be posted to on PRs from first time contributors in your repository newPRWelcomeComment: > - Thanks for helping in improving MoveIt + Thanks for helping in improving MoveIt and open source robotics! # Configuration for first-pr-merge - https://github.com/behaviorbot/first-pr-merge From f96e1271940b96dcce3f3a72cabfcbb1758caa15 Mon Sep 17 00:00:00 2001 From: Mark Moll Date: Wed, 20 Nov 2019 19:16:08 -0600 Subject: [PATCH 087/612] Allow user to specify planner termination condition. (#1695) * Add OMPL planner 'AnytimePathShortening' * clean up registration of planner allocators * add parameter to geometric::AnytimePathShortening method to set planner instances and their parameters via config file * clang-format * add support for specifying planner termination conditions via ompl_planning.yaml file * add OMPL version check to make new feature backwards compatible with older versions of MoveIt * fix catkin_lint warning * fix compiler warning * fix logical error (ROS_ERROR is not fatal) * add docs for setting termination condition * add note to remove #if guard for OMPL version once support for Melodic and older has been dropped * correct/add TODOs for OMPL version checks --- .../detail/projection_evaluators.h | 1 + .../model_based_planning_context.h | 28 +++++++ .../src/model_based_planning_context.cpp | 79 +++++++++++++++++-- .../src/tools/moveit_config_data.cpp | 1 + 4 files changed, 103 insertions(+), 6 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h index 63c0b0917f..92784cfe37 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h @@ -40,6 +40,7 @@ #include #include +// TODO: remove when ROS Lunar and older are no longer supported #if OMPL_VERSION_VALUE >= 1004000 // Version greater than 1.4.0 typedef Eigen::Ref OMPLProjection; #else // All other versions diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index 3ce1fa57b7..2e5a046070 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -335,6 +335,34 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext virtual void useConfig(); virtual ob::GoalPtr constructGoal(); + /* @brief Construct a planner termination condition, by default a simple time limit + @param timeout The maximum time (in seconds) that can be used for planning + @param start The point in time from which planning is considered to have started + + An additional planner termination condition can be specified per planner + configuration in ompl_planning.yaml via the `termination_condition` parameter. + Possible values are: + + * `Iteration[num]`: Terminate after `num` iterations. Here, `num` should be replaced + with a positive integer. + * `CostConvergence[solutionsWindow,epsilon]`: Terminate after the cost (as specified + by an optimization objective) has converged. The parameter `solutionsWindow` + specifies the minimum number of solutions to use in deciding whether a planner has + converged. The parameter `epsilon` is the threshold to consider for convergence. + This should be a positive number close to 0. If the cumulative moving average does + not change by a relative fraction of epsilon after a new better solution is found, + convergence has been reached. + * `ExactSolution`: Terminate as soon as an exact solution is found or a timeout + occurs. This modifies the behavior of anytime/optimizing planners to terminate + upon discovering the first feasible solution. + + In all cases, the planner will terminate when either the user-specified termination + condition is satisfied or the time limit specified by `timeout` has been reached, + whichever occurs first. + */ + virtual ob::PlannerTerminationCondition constructPlannerTerminationCondition(double timeout, + const ompl::time::point& start); + void registerTerminationCondition(const ob::PlannerTerminationCondition& ptc); void unregisterTerminationCondition(); diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 609431be37..873b11c7e8 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -35,6 +35,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include @@ -48,11 +49,21 @@ #include #include +#include #include #include #include #include #include +// TODO: remove when ROS Melodic and older are no longer supported +#if OMPL_VERSION_VALUE < 1005000 +#include +#else +// IterationTerminationCondition was moved to a separate file and +// CostConvergenceTerminationCondition was added in OMPL 1.5.0. +#include +#include +#endif #include "ompl/base/objectives/PathLengthOptimizationObjective.h" #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h" @@ -455,6 +466,65 @@ ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal() return ob::GoalPtr(); } +ompl::base::PlannerTerminationCondition ompl_interface::ModelBasedPlanningContext::constructPlannerTerminationCondition( + double timeout, const ompl::time::point& start) +{ + auto it = spec_.config_.find("termination_condition"); + if (it == spec_.config_.end()) + return ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)); + std::string termination_string = it->second; + std::vector termination_and_params; + boost::split(termination_and_params, termination_string, boost::is_any_of("[ ,]")); + + if (termination_and_params.empty()) + ROS_ERROR_NAMED("model_based_planning_context", "Termination condition not specified"); + // Terminate if a maximum number of iterations is exceeded or a timeout occurs. + // The semantics of "iterations" are planner-specific, but typically it corresponds to the number of times + // an attempt was made to grow a roadmap/tree. + else if (termination_and_params[0] == "Iteration") + { + if (termination_and_params.size() > 1) + return ob::plannerOrTerminationCondition( + ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)), + ob::IterationTerminationCondition(std::stoul(termination_and_params[1]))); + else + ROS_ERROR_NAMED("model_based_planning_context", "Missing argument to Iteration termination condition"); + } +// TODO: remove when ROS Melodic and older are no longer supported +#if OMPL_VERSION_VALUE >= 1005000 + // Terminate if the cost has converged or a timeout occurs. + // Only useful for anytime/optimizing planners. + else if (termination_and_params[0] == "CostConvergence") + { + std::size_t solutionsWindow = 10u; + double epsilon = 0.1; + if (termination_and_params.size() > 1) + { + solutionsWindow = std::stoul(termination_and_params[1]); + if (termination_and_params.size() > 2) + epsilon = moveit::core::toDouble(termination_and_params[2]); + } + return ob::plannerOrTerminationCondition( + ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)), + ob::CostConvergenceTerminationCondition(ompl_simple_setup_->getProblemDefinition(), solutionsWindow, epsilon)); + } +#endif + // Terminate as soon as an exact solution is found or a timeout occurs. + // This modifies the behavior of anytime/optimizing planners to terminate upon discovering + // the first feasible solution. + else if (termination_and_params[0] == "ExactSolution") + { + return ob::plannerOrTerminationCondition( + ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)), + ob::exactSolnPlannerTerminationCondition(ompl_simple_setup_->getProblemDefinition())); + } + else + ROS_ERROR_NAMED("model_based_planning_context", "Unknown planner termination condition"); + + // return a planner termination condition to suppress compiler warning + return ob::plannerAlwaysTerminatingCondition(); +} + void ompl_interface::ModelBasedPlanningContext::setCompleteInitialState( const robot_state::RobotState& complete_initial_robot_state) { @@ -659,8 +729,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i if (count <= 1) { ROS_DEBUG_NAMED("model_based_planning_context", "%s: Solving the planning problem once...", name_.c_str()); - ob::PlannerTerminationCondition ptc = - ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)); + ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); registerTerminationCondition(ptc); result = ompl_simple_setup_->solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION; last_plan_time_ = ompl_simple_setup_->getLastPlanComputationTime(); @@ -681,8 +750,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i for (unsigned int i = 0; i < count; ++i) ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal())); - ob::PlannerTerminationCondition ptc = - ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)); + ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); registerTerminationCondition(ptc); result = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION; last_plan_time_ = ompl::time::seconds(ompl::time::now() - start); @@ -690,8 +758,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i } else { - ob::PlannerTerminationCondition ptc = - ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)); + ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); registerTerminationCondition(ptc); int n = count / max_planning_threads_; result = true; diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index c747a2d3ee..da6ce39b47 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -554,6 +554,7 @@ std::vector MoveItConfigData::getOMPLPlanners() aps.addParameter("hybridize", "true", "Compute hybrid solution trajectories"); aps.addParameter("max_hybrid_paths", "24", "Number of hybrid paths generated per iteration"); aps.addParameter("num_planners", "4", "The number of default planners to use for planning"); +// TODO: remove when ROS Melodic and older are no longer supported #if OMPL_VERSION_VALUE >= 1005000 // This parameter was added in OMPL 1.5.0 aps.addParameter("planners", "", "A comma-separated list of planner types (e.g., \"PRM,EST,RRTConnect\"" From f226b9b17e3d92908b3ae034a90e76e7d3f0bbf0 Mon Sep 17 00:00:00 2001 From: Niklas Fiedler Date: Wed, 20 Nov 2019 20:28:15 +0100 Subject: [PATCH 088/612] Updating deprecation method (#1748) * replaced MOVEIT_DEPRECATED with [[deprecated]] * issuing a warning when including deprecation.h --- .../moveit/distance_field/distance_field.h | 9 ++++----- .../moveit/kinematics_base/kinematics_base.h | 17 ++++++++--------- .../macros/include/moveit/macros/deprecation.h | 3 ++- .../moveit/planning_scene/planning_scene.h | 1 - .../include/moveit/robot_state/robot_state.h | 9 ++++----- .../moveit/robot_trajectory/robot_trajectory.h | 3 +-- .../move_group_interface/move_group_interface.h | 11 +++++------ 7 files changed, 24 insertions(+), 29 deletions(-) diff --git a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h index b4bfa238d7..b8473a1239 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h @@ -37,7 +37,6 @@ #pragma once #include -#include #include #include #include @@ -191,7 +190,7 @@ class DistanceField void addShapeToField(const shapes::Shape* shape, const Eigen::Isometry3d& pose); // DEPRECATED form - MOVEIT_DEPRECATED void addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose); + [[deprecated]] void addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose); /** * \brief Adds an octree to the distance field. Cells that are @@ -230,8 +229,8 @@ class DistanceField const Eigen::Isometry3d& new_pose); // DEPRECATED form - MOVEIT_DEPRECATED void moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose, - const geometry_msgs::Pose& new_pose); + [[deprecated]] void moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose, + const geometry_msgs::Pose& new_pose); /** * \brief All points corresponding to the shape are removed from the @@ -245,7 +244,7 @@ class DistanceField void removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose); // DEPRECATED form - MOVEIT_DEPRECATED void removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose); + [[deprecated]] void removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose); /** * \brief Resets all points in the distance field to an uninitialize diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 59fc3ee7c4..b4d41e06ce 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -39,7 +39,6 @@ #include #include #include -#include #include #include @@ -341,9 +340,9 @@ class KinematicsBase * @param search_discretization The discretization of the search when the solver steps through the redundancy */ /* Replace by tip_frames-based method! */ - MOVEIT_DEPRECATED virtual void setValues(const std::string& robot_description, const std::string& group_name, - const std::string& base_frame, const std::string& tip_frame, - double search_discretization); + [[deprecated]] virtual void setValues(const std::string& robot_description, const std::string& group_name, + const std::string& base_frame, const std::string& tip_frame, + double search_discretization); /** * @brief Set the parameters for the solver, for use with non-chain IK solvers @@ -373,9 +372,9 @@ class KinematicsBase * Instead of this method, use the method passing in a RobotModel! * Default implementation returns false, indicating that this API is not supported. */ - MOVEIT_DEPRECATED 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); + [[deprecated]] 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); /** * @brief Initialization function for the kinematics, for use with non-chain IK solvers @@ -593,8 +592,8 @@ class KinematicsBase // The next two variables still exists for backwards compatibility // with previously generated custom ik solvers like IKFast // Replace tip_frame_ -> tip_frames_[0], search_discretization_ -> redundant_joint_discretization_ - MOVEIT_DEPRECATED std::string tip_frame_; - MOVEIT_DEPRECATED double search_discretization_; + [[deprecated]] std::string tip_frame_; + [[deprecated]] double search_discretization_; double default_timeout_; std::vector redundant_joint_indices_; diff --git a/moveit_core/macros/include/moveit/macros/deprecation.h b/moveit_core/macros/include/moveit/macros/deprecation.h index 20f24b06ac..2162988004 100644 --- a/moveit_core/macros/include/moveit/macros/deprecation.h +++ b/moveit_core/macros/include/moveit/macros/deprecation.h @@ -35,8 +35,9 @@ #pragma once /** \def MOVEIT_DEPRECATED - Macro that marks functions as deprecated */ + Deprecated macro that marks functions as deprecated (TODO: Remove for Noetic) */ +#warning "The usage of MOVEIT_DEPRECATED is deprecated. Use the CPP14 [[deprecated]] instead." #ifdef __GNUC__ #define MOVEIT_DEPRECATED __attribute__((deprecated)) #elif defined(_MSC_VER) diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 6faff3b6aa..acc43ab92c 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -46,7 +46,6 @@ #include #include #include -#include #include #include #include diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 126b20baf9..f16c031abd 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -40,7 +40,6 @@ #include #include #include -#include #include #include #include @@ -1119,7 +1118,7 @@ as the new values that correspond to the group */ NOTE: As of ROS-Melodic these are deprecated and should not be used */ - MOVEIT_DEPRECATED double + [[deprecated]] double computeCartesianPath(const JointModelGroup* group, std::vector& traj, const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, double max_step, double jump_threshold_factor, @@ -1135,7 +1134,7 @@ as the new values that correspond to the group */ NOTE: As of ROS-Melodic these are deprecated and should not be used */ - MOVEIT_DEPRECATED double + [[deprecated]] double computeCartesianPath(const JointModelGroup* group, std::vector& traj, const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, double max_step, double jump_threshold_factor, @@ -1151,7 +1150,7 @@ as the new values that correspond to the group */ NOTE: As of ROS-Melodic these are deprecated and should not be used */ - MOVEIT_DEPRECATED double + [[deprecated]] double computeCartesianPath(const JointModelGroup* group, std::vector& traj, const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, double max_step, double jump_threshold_factor, @@ -1757,7 +1756,7 @@ as the new values that correspond to the group */ /** \brief Update a set of joints that are certain to be mimicking other joints */ /* use updateMimicJoints() instead, which also marks joints dirty */ - MOVEIT_DEPRECATED void updateMimicJoint(const std::vector& mim) + [[deprecated]] void updateMimicJoint(const std::vector& mim) { for (std::size_t i = 0; i < mim.size(); ++i) { diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 1601cd4279..dd2ce58cc8 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -37,7 +37,6 @@ #pragma once #include -#include #include #include #include @@ -116,7 +115,7 @@ class RobotTrajectory */ double getWayPointDurationFromStart(std::size_t index) const; - MOVEIT_DEPRECATED double getWaypointDurationFromStart(std::size_t index) const; + [[deprecated]] double getWaypointDurationFromStart(std::size_t index) const; double getWayPointDurationFromPrevious(std::size_t index) const { diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 71590ac54a..e7d2b3bdcc 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -38,7 +38,6 @@ #pragma once #include -#include #include #include #include @@ -152,8 +151,8 @@ class MoveGroupInterface MoveGroupInterface(const Options& opt, const std::shared_ptr& tf_buffer = std::shared_ptr(), const ros::WallDuration& wait_for_servers = ros::WallDuration()); - MOVEIT_DEPRECATED MoveGroupInterface(const Options& opt, const std::shared_ptr& tf_buffer, - const ros::Duration& wait_for_servers); + [[deprecated]] MoveGroupInterface(const Options& opt, const std::shared_ptr& tf_buffer, + const ros::Duration& wait_for_servers); /** \brief Construct a client for the MoveGroup action for a particular \e group. @@ -165,8 +164,8 @@ class MoveGroupInterface MoveGroupInterface(const std::string& group, const std::shared_ptr& tf_buffer = std::shared_ptr(), const ros::WallDuration& wait_for_servers = ros::WallDuration()); - MOVEIT_DEPRECATED MoveGroupInterface(const std::string& group, const std::shared_ptr& tf_buffer, - const ros::Duration& wait_for_servers); + [[deprecated]] MoveGroupInterface(const std::string& group, const std::shared_ptr& tf_buffer, + const ros::Duration& wait_for_servers); ~MoveGroupInterface(); @@ -517,7 +516,7 @@ class MoveGroupInterface void getJointValueTarget(std::vector& group_variable_values) const; /// Get the currently set joint state goal, replaced by private getTargetRobotState() - MOVEIT_DEPRECATED const robot_state::RobotState& getJointValueTarget() const; + [[deprecated]] const robot_state::RobotState& getJointValueTarget() const; /**@}*/ From 303c3217f6157ecd546f2691f69646db38889780 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Thu, 21 Nov 2019 00:07:00 -0700 Subject: [PATCH 089/612] Improve variable naming in RobotModelLoader (#1759) --- .../src/robot_model_loader.cpp | 35 ++++++++++--------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp index 80a8556dc2..1ba8e75359 100644 --- a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp +++ b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp @@ -107,52 +107,53 @@ void RobotModelLoader::configure(const Options& opt) // if there are additional joint limits specified in some .yaml file, read those in ros::NodeHandle nh("~"); - for (moveit::core::JointModel* jmodel : model_->getJointModels()) + for (moveit::core::JointModel* joint_model : model_->getJointModels()) { - std::vector jlim = jmodel->getVariableBoundsMsg(); - for (std::size_t j = 0; j < jlim.size(); ++j) + std::vector joint_limit = joint_model->getVariableBoundsMsg(); + for (std::size_t joint_id = 0; joint_id < joint_limit.size(); ++joint_id) { - std::string prefix = rdf_loader_->getRobotDescription() + "_planning/joint_limits/" + jlim[j].joint_name + "/"; + std::string prefix = + rdf_loader_->getRobotDescription() + "_planning/joint_limits/" + joint_limit[joint_id].joint_name + "/"; double max_position; if (nh.getParam(prefix + "max_position", max_position)) { - if (canSpecifyPosition(jmodel, j)) + if (canSpecifyPosition(joint_model, joint_id)) { - jlim[j].has_position_limits = true; - jlim[j].max_position = max_position; + joint_limit[joint_id].has_position_limits = true; + joint_limit[joint_id].max_position = max_position; } } double min_position; if (nh.getParam(prefix + "min_position", min_position)) { - if (canSpecifyPosition(jmodel, j)) + if (canSpecifyPosition(joint_model, joint_id)) { - jlim[j].has_position_limits = true; - jlim[j].min_position = min_position; + joint_limit[joint_id].has_position_limits = true; + joint_limit[joint_id].min_position = min_position; } } double max_velocity; if (nh.getParam(prefix + "max_velocity", max_velocity)) { - jlim[j].has_velocity_limits = true; - jlim[j].max_velocity = max_velocity; + joint_limit[joint_id].has_velocity_limits = true; + joint_limit[joint_id].max_velocity = max_velocity; } bool has_vel_limits; if (nh.getParam(prefix + "has_velocity_limits", has_vel_limits)) - jlim[j].has_velocity_limits = has_vel_limits; + joint_limit[joint_id].has_velocity_limits = has_vel_limits; double max_acc; if (nh.getParam(prefix + "max_acceleration", max_acc)) { - jlim[j].has_acceleration_limits = true; - jlim[j].max_acceleration = max_acc; + joint_limit[joint_id].has_acceleration_limits = true; + joint_limit[joint_id].max_acceleration = max_acc; } bool has_acc_limits; if (nh.getParam(prefix + "has_acceleration_limits", has_acc_limits)) - jlim[j].has_acceleration_limits = has_acc_limits; + joint_limit[joint_id].has_acceleration_limits = has_acc_limits; } - jmodel->setVariableBounds(jlim); + joint_model->setVariableBounds(joint_limit); } } From d4ffb9aa33355a875eef26cc6e9e6c883c3e21da Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Thu, 21 Nov 2019 01:27:06 -0700 Subject: [PATCH 090/612] [trivial] Improve variable name in RobotModel (#1752) --- moveit_core/robot_model/src/robot_model.cpp | 85 +++++++++++---------- 1 file changed, 43 insertions(+), 42 deletions(-) diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 700613ffe5..d67af3b052 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -844,7 +844,7 @@ static inline VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint) JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const urdf::Link* child_link, const srdf::Model& srdf_model) { - JointModel* result = nullptr; + JointModel* new_joint_model = nullptr; // if urdf_joint exists, must be the root link transform if (urdf_joint) @@ -857,7 +857,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint)); j->setContinuous(false); j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z)); - result = j; + new_joint_model = j; } break; case urdf::Joint::CONTINUOUS: @@ -866,7 +866,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint)); j->setContinuous(true); j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z)); - result = j; + new_joint_model = j; } break; case urdf::Joint::PRISMATIC: @@ -874,17 +874,17 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const PrismaticJointModel* j = new PrismaticJointModel(urdf_joint->name); j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint)); j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z)); - result = j; + new_joint_model = j; } break; case urdf::Joint::FLOATING: - result = new FloatingJointModel(urdf_joint->name); + new_joint_model = new FloatingJointModel(urdf_joint->name); break; case urdf::Joint::PLANAR: - result = new PlanarJointModel(urdf_joint->name); + new_joint_model = new PlanarJointModel(urdf_joint->name); break; case urdf::Joint::FIXED: - result = new FixedJointModel(urdf_joint->name); + new_joint_model = new FixedJointModel(urdf_joint->name); break; default: ROS_ERROR_NAMED(LOGNAME, "Unknown joint type: %d", (int)urdf_joint->type); @@ -910,12 +910,12 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const else { if (virtual_joint.type_ == "fixed") - result = new FixedJointModel(virtual_joint.name_); + new_joint_model = new FixedJointModel(virtual_joint.name_); else if (virtual_joint.type_ == "planar") - result = new PlanarJointModel(virtual_joint.name_); + new_joint_model = new PlanarJointModel(virtual_joint.name_); else if (virtual_joint.type_ == "floating") - result = new FloatingJointModel(virtual_joint.name_); - if (result) + new_joint_model = new FloatingJointModel(virtual_joint.name_); + if (new_joint_model) { // for fixed frames we still use the robot root link if (virtual_joint.type_ != "fixed") @@ -926,28 +926,28 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const } } } - if (!result) + if (!new_joint_model) { ROS_INFO_NAMED(LOGNAME, "No root/virtual joint specified in SRDF. Assuming fixed joint"); - result = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT"); + new_joint_model = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT"); } } - if (result) + if (new_joint_model) { - result->setDistanceFactor(result->getStateSpaceDimension()); + new_joint_model->setDistanceFactor(new_joint_model->getStateSpaceDimension()); const std::vector& pjoints = srdf_model.getPassiveJoints(); for (const srdf::Model::PassiveJoint& pjoint : pjoints) { - if (result->getName() == pjoint.name_) + if (new_joint_model->getName() == pjoint.name_) { - result->setPassive(true); + new_joint_model->setPassive(true); break; } } } - return result; + return new_joint_model; } namespace @@ -962,7 +962,7 @@ static inline Eigen::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose) LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) { - LinkModel* result = new LinkModel(urdf_link->name); + LinkModel* new_link_model = new LinkModel(urdf_link->name); const std::vector& col_array = urdf_link->collision_array.empty() ? std::vector(1, urdf_link->collision) : @@ -998,7 +998,7 @@ LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) } } - result->setGeometry(shapes, poses); + new_link_model->setGeometry(shapes, poses); // figure out visual mesh (try visual urdf tag first, collision tag otherwise if (urdf_link->visual && urdf_link->visual->geometry) @@ -1007,8 +1007,8 @@ LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) { const urdf::Mesh* mesh = static_cast(urdf_link->visual->geometry.get()); if (!mesh->filename.empty()) - result->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->visual->origin), - Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z)); + new_link_model->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->visual->origin), + Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z)); } } else if (urdf_link->collision && urdf_link->collision->geometry) @@ -1017,36 +1017,37 @@ LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) { const urdf::Mesh* mesh = static_cast(urdf_link->collision->geometry.get()); if (!mesh->filename.empty()) - result->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->collision->origin), - Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z)); + new_link_model->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->collision->origin), + Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z)); } } if (urdf_link->parent_joint) - result->setJointOriginTransform(urdfPose2Isometry3d(urdf_link->parent_joint->parent_to_joint_origin_transform)); + new_link_model->setJointOriginTransform( + urdfPose2Isometry3d(urdf_link->parent_joint->parent_to_joint_origin_transform)); - return result; + return new_link_model; } shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom) { moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::constructShape"); - shapes::Shape* result = nullptr; + shapes::Shape* new_shape = nullptr; switch (geom->type) { case urdf::Geometry::SPHERE: - result = new shapes::Sphere(static_cast(geom)->radius); + new_shape = new shapes::Sphere(static_cast(geom)->radius); break; case urdf::Geometry::BOX: { urdf::Vector3 dim = static_cast(geom)->dim; - result = new shapes::Box(dim.x, dim.y, dim.z); + new_shape = new shapes::Box(dim.x, dim.y, dim.z); } break; case urdf::Geometry::CYLINDER: - result = new shapes::Cylinder(static_cast(geom)->radius, - static_cast(geom)->length); + new_shape = new shapes::Cylinder(static_cast(geom)->radius, + static_cast(geom)->length); break; case urdf::Geometry::MESH: { @@ -1055,7 +1056,7 @@ shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom) { Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale); - result = m; + new_shape = m; } } break; @@ -1064,7 +1065,7 @@ shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom) break; } - return shapes::ShapePtr(result); + return shapes::ShapePtr(new_shape); } bool RobotModel::hasJointModel(const std::string& name) const @@ -1277,9 +1278,9 @@ void RobotModel::setKinematicsAllocators(const std::map::const_iterator jt = allocators.find(jmg->getName()); if (jt != allocators.end()) { - std::pair result; - result.first = jt->second; - jmg->setSolverAllocators(result); + std::pair solver_allocator_pair; + solver_allocator_pair.first = jt->second; + jmg->setSolverAllocators(solver_allocator_pair); } } @@ -1287,7 +1288,7 @@ void RobotModel::setKinematicsAllocators(const std::map result; + std::pair solver_allocator_pair; std::map::const_iterator jt = allocators.find(jmg->getName()); if (jt == allocators.end()) { @@ -1311,16 +1312,16 @@ void RobotModel::setKinematicsAllocators(const std::map resultj; + std::set joint_model_set; std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(), - std::inserter(resultj, resultj.end())); + std::inserter(joint_model_set, joint_model_set.end())); // TODO: instead of maintaining disjoint joint sets here, // should we leave that work to JMG's setSolverAllocators() / computeIKIndexBijection()? // There, a disjoint bijection from joints to solvers is computed anyway. // Underlying question: How do we resolve overlaps? Now the first considered sub group "wins" // But, if the overlap only involves fixed joints, we could consider all sub groups subs.push_back(sub); - joints.swap(resultj); + joints.swap(joint_model_set); } } @@ -1331,12 +1332,12 @@ void RobotModel::setKinematicsAllocators(const std::mapgetName() << " "; - result.second[sub] = allocators.find(sub->getName())->second; + solver_allocator_pair.second[sub] = allocators.find(sub->getName())->second; } ROS_DEBUG_NAMED(LOGNAME, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), ss.str().c_str()); } - jmg->setSolverAllocators(result); + jmg->setSolverAllocators(solver_allocator_pair); } } } From 0d31da73bb523bd5b6efa6bc63400e06a395360f Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Thu, 21 Nov 2019 05:50:34 -0700 Subject: [PATCH 091/612] Update issue template to non-legacy workflow (#1756) Currently we're using the old single-template issue approach: https://help.github.com/en/github/building-a-strong-community/manually-creating-a-single-issue-template-for-your-repository Switching to dual approach: bug vs feature request --- .../bug_report.md} | 15 +++++++++++--- .github/ISSUE_TEMPLATE/feature_request.md | 20 +++++++++++++++++++ 2 files changed, 32 insertions(+), 3 deletions(-) rename .github/{ISSUE_TEMPLATE.md => ISSUE_TEMPLATE/bug_report.md} (71%) create mode 100644 .github/ISSUE_TEMPLATE/feature_request.md diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE/bug_report.md similarity index 71% rename from .github/ISSUE_TEMPLATE.md rename to .github/ISSUE_TEMPLATE/bug_report.md index 62f551be4a..c7c77614ea 100644 --- a/.github/ISSUE_TEMPLATE.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -1,13 +1,22 @@ +--- +name: Bug report +about: Create a report to help us improve +title: '' +labels: bug +assignees: '' + +--- + ### Description Overview of your issue here. ### Your environment -* ROS Distro: [Indigo|Jade|Kinetic] -* OS Version: e.g. Ubuntu 16.04 +* ROS Distro: [Kinetic|Melodic] +* OS Version: e.g. Ubuntu 18.04 * Source or Binary build? * If binary, which release version? -* If source, which git commit or tag? +* If source, which branch? ### Steps to reproduce Tell us how to reproduce this issue. Attempt to provide a working demo, perhaps using Docker. diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 0000000000..11fc491ef1 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,20 @@ +--- +name: Feature request +about: Suggest an idea for this project +title: '' +labels: enhancement +assignees: '' + +--- + +**Is your feature request related to a problem? Please describe.** +A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] + +**Describe the solution you'd like** +A clear and concise description of what you want to happen. + +**Describe alternatives you've considered** +A clear and concise description of any alternative solutions or features you've considered. + +**Additional context** +Add any other context or screenshots about the feature request here. From 8a454813f5ea6a9725947a77851139a75b8d2eda Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 15 Oct 2019 21:38:32 +0200 Subject: [PATCH 092/612] Initial implementation of MoveItCpp --- moveit_ros/planning_interface/CMakeLists.txt | 3 + .../moveit_cpp/CMakeLists.txt | 12 + .../include/moveit/moveit_cpp/moveit_cpp.h | 203 ++++++++++ .../moveit/moveit_cpp/planning_component.h | 216 +++++++++++ .../moveit_cpp/src/moveit_cpp.cpp | 333 ++++++++++++++++ .../moveit_cpp/src/planning_component.cpp | 354 ++++++++++++++++++ 6 files changed, 1121 insertions(+) create mode 100644 moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt create mode 100644 moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h create mode 100644 moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h create mode 100644 moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp create mode 100644 moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 4c7f6a591b..8a907efba1 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -51,6 +51,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS common_planning_interface_objects/include planning_scene_interface/include move_group_interface/include + moveit_cpp/include ) catkin_python_setup() @@ -60,6 +61,7 @@ catkin_package( moveit_common_planning_interface_objects moveit_planning_scene_interface moveit_move_group_interface + moveit_cpp INCLUDE_DIRS ${THIS_PACKAGE_INCLUDE_DIRS} CATKIN_DEPENDS @@ -90,3 +92,4 @@ add_subdirectory(planning_scene_interface) add_subdirectory(move_group_interface) add_subdirectory(robot_interface) add_subdirectory(test) +add_subdirectory(moveit_cpp) diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt new file mode 100644 index 0000000000..098becde4b --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -0,0 +1,12 @@ +set(MOVEIT_LIB_NAME moveit_cpp) + +add_library(${MOVEIT_LIB_NAME} src/moveit_cpp.cpp src/planning_component.cpp) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) + +install(TARGETS ${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h new file mode 100644 index 0000000000..fd81721ec5 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -0,0 +1,203 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * 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 PickNik LLC 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. + *********************************************************************/ + +/* Author: Henning Kayser */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(MoveItCpp); + +class MoveItCpp +{ +public: + /// Specification of options to use when constructing the MoveItCpp class + struct PlanningSceneMonitorOptions + { + void load(const ros::NodeHandle& nh) + { + std::string ns = "planning_scene_monitor_options/"; + nh.param(ns + "name", name, "planning_scene_monitor"); + nh.param(ns + "robot_description", robot_description, "robot_description"); + nh.param(ns + "joint_state_topic", joint_state_topic, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC); + nh.param(ns + "attached_collision_object_topic", attached_collision_object_topic, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC); + nh.param(ns + "monitored_planning_scene_topic", monitored_planning_scene_topic, + planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC); + nh.param(ns + "publish_planning_scene_topic", publish_planning_scene_topic, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC); + nh.param(ns + "wait_for_initial_state_timeout", wait_for_initial_state_timeout, 0.0); + } + std::string name; + std::string robot_description; + std::string joint_state_topic; + std::string attached_collision_object_topic; + std::string monitored_planning_scene_topic; + std::string publish_planning_scene_topic; + double wait_for_initial_state_timeout; + }; + struct PlanningPipelineOptions + { + void load(const ros::NodeHandle& nh) + { + std::string ns = "planning_pipelines/"; + nh.getParam(ns + "pipeline_names", pipeline_names); + nh.getParam(ns + "namespace", parent_namespace); + } + std::vector pipeline_names; + std::string parent_namespace; + }; + struct PlannerOptions + { + void load(const ros::NodeHandle& nh) + { + std::string ns = "default_planner_options/"; + nh.getParam(ns + "planning_attempts", planning_attempts); + nh.getParam(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor); + nh.getParam(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor); + } + int planning_attempts; + double planning_time; + double max_velocity_scaling_factor; + double max_acceleration_scaling_factor; + }; + + /// Parameter container for initializing MoveItCpp + struct Options + { + Options(const ros::NodeHandle& nh) + { + planning_scene_monitor_options.load(nh); + default_planner_options.load(nh); + planning_pipeline_options.load(nh); + } + + PlanningSceneMonitorOptions planning_scene_monitor_options; + PlanningPipelineOptions planning_pipeline_options; + PlannerOptions default_planner_options; + }; + + /** \brief Constructor */ + MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); + MoveItCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + MoveItCpp(const MoveItCpp&) = delete; + MoveItCpp& operator=(const MoveItCpp&) = delete; + + MoveItCpp(MoveItCpp&& other); + MoveItCpp& operator=(MoveItCpp&& other); + + /** \brief Destructor */ + ~MoveItCpp(); + + /** \brief Get the RobotModel object. */ + robot_model::RobotModelConstPtr getRobotModel() const; + + /** \brief Get the ROS node handle of this instance operates on */ + const ros::NodeHandle& getNodeHandle() const; + + /** \brief Get the current state queried from the current state monitor */ + bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds); + + /** \brief Get the current state queried from the current state monitor */ + robot_state::RobotStatePtr getCurrentState(double wait_seconds = 0.0); + + /** \brief Get all loaded planning pipeline instances mapped to their reference names */ + const std::map& getPlanningPipelines() const; + + /** \brief Get the names of all loaded planning pipelines. Specify group_name to filter the results by planning group + */ + std::set getPlanningPipelineNames(const std::string& group_name = "") const; + + /** \brief Get the stored instance of the planning scene monitor */ + const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const; + planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst(); + + const std::shared_ptr& getTFBuffer() const; + + /** \brief Get the stored instance of the trajectory execution manager */ + const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const; + trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst(); + + /** \brief Execute a trajectory on the planning group specified by group_name using the trajectory execution manager. + * If blocking is set to false, the execution is run in background and the function returns immediately. */ + bool execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + bool blocking = true); + +protected: + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + +private: + // Core properties and instances + ros::NodeHandle node_handle_; + std::string robot_description_; + robot_model::RobotModelConstPtr robot_model_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // Planning + std::map planning_pipelines_; + std::map> groups_pipelines_map_; + std::map> groups_algorithms_map_; + + // Execution + trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; + + /** \brief Reset all member variables */ + void clearContents(); + + /** \brief Initialize and setup the planning scene monitor */ + bool loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt); + + /** \brief Initialize and setup the planning pipelines */ + bool loadPlanningPipelines(const PlanningPipelineOptions& opt); +}; +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h new file mode 100644 index 0000000000..06d8124edf --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -0,0 +1,216 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * 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 PickNik LLC 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. + *********************************************************************/ + +/* Author: Henning Kayser */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(PlanningComponent); + +class PlanningComponent +{ +public: + MOVEIT_STRUCT_FORWARD(PlanSolution); + + class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes + { + public: + MoveItErrorCode() + { + val = 0; + } + MoveItErrorCode(int code) + { + val = code; + } + MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code) + { + val = code.val; + } + explicit operator bool() const + { + return val == moveit_msgs::MoveItErrorCodes::SUCCESS; + } + bool operator==(const int c) const + { + return val == c; + } + bool operator!=(const int c) const + { + return val != c; + } + }; + + /// The representation of a plan solution + struct PlanSolution + { + /// The full starting state used for planning + moveit_msgs::RobotState start_state; + + /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) + robot_trajectory::RobotTrajectoryPtr trajectory; + + /// Error code + MoveItErrorCode error_code; + + explicit operator bool() const + { + return bool(error_code); + } + }; + + /// Planner parameters provided with the MotionPlanRequest + struct PlanRequestParameters + { + std::string planner_id; + std::string planning_pipeline; + size_t planning_attempts; + double planning_time; + double max_velocity_scaling_factor; + double max_acceleration_scaling_factor; + }; + + /** \brief Constructor */ + PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh); + PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_context); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + PlanningComponent(const PlanningComponent&) = delete; + PlanningComponent& operator=(const PlanningComponent&) = delete; + + PlanningComponent(PlanningComponent&& other); + PlanningComponent& operator=(PlanningComponent&& other); + + /** \brief Destructor */ + ~PlanningComponent(); + + /** \brief Get the name of the planning group */ + const std::string& getName() const; + + /** \brief Get the names of the named robot states available as targets */ + const std::vector getNamedTargets(); + + /** \brief Get the joint values for targets specified by name */ + std::map getNamedTargetValues(const std::string& name); + + /** \brief Specify the workspace bounding box. + The box is specified in the planning frame (i.e. relative to the robot root link start position). + This is useful when the planning group contains the root joint of the robot -- i.e. when planning motion for the + robot relative to the world. */ + void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz); + + /** \brief Remove the workspace bounding box from planning */ + void unsetWorkspace(); + + /** \brief Get the considered start state if defined, otherwise get the current state */ + robot_state::RobotStatePtr getStartState(); + + /** \brief Set the robot state that should be considered as start state for planning */ + bool setStartState(const robot_state::RobotState& start_state); + /** \brief Set the named robot state that should be considered as start state for planning */ + bool setStartState(const std::string& named_state); + + /** \brief Set the start state for planning to be the current state of the robot */ + void setStartStateToCurrentState(); + + /** \brief Set the goal constraints used for planning */ + bool setGoal(const std::vector& goal_constraints); + /** \brief Set the goal constraints generated from a target state */ + bool setGoal(const robot_state::RobotState& goal_state); + /** \brief Set the goal constraints generated from target pose and robot link */ + bool setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name); + /** \brief Set the goal constraints generated from a named target state */ + bool setGoal(const std::string& named_target); + + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using + * default parameters. */ + PlanSolution plan(); + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the + * provided PlanRequestParameters. */ + PlanSolution plan(const PlanRequestParameters& parameters); + + /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates + * after the execution is complete. The execution can be run in background by setting blocking to false. */ + bool execute(bool blocking = true); + + /** \brief Return the last plan solution*/ + const PlanSolutionPtr getLastPlanSolution(); + +private: + // Core properties and instances + ros::NodeHandle nh_; + MoveItCppPtr moveit_cpp_; + const std::string group_name_; + // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources + const moveit::core::JointModelGroup* joint_model_group_; + + // Planning + std::set planning_pipeline_names_; + robot_state::RobotStatePtr considered_start_state_; + std::vector current_goal_constraints_; + PlanRequestParameters plan_request_parameters_; + moveit_msgs::WorkspaceParameters workspace_parameters_; + bool workspace_parameters_set_ = false; + PlanSolutionPtr last_plan_solution_; + + // common properties for goals + // TODO(henningkayser): support goal tolerances + // double goal_joint_tolerance_; + // double goal_position_tolerance_; + // double goal_orientation_tolerance_; + // TODO(henningkayser): implment path/trajectory constraints + // std::unique_ptr path_constraints_; + // std::unique_ptr trajectory_constraints_; + + /** \brief Reset all member variables */ + void clearContents(); +}; +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp new file mode 100644 index 0000000000..f27efed06b --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -0,0 +1,333 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * 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 PickNik LLC 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. + *********************************************************************/ + +/* Author: Henning Kayser */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +constexpr char LOGNAME[] = "moveit_cpp"; +constexpr char PLANNING_SCENE_MONITOR_NAME[] = "moveit_cpp_planning_scene"; +constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin"; + +MoveItCpp::MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) + : MoveItCpp(Options(nh), nh, tf_buffer) +{ +} + +MoveItCpp::MoveItCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) +{ + if (!tf_buffer_) + tf_buffer_.reset(new tf2_ros::Buffer()); + tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_)); + + // Configure planning scene monitor + if (!loadPlanningSceneMonitor(opt.planning_scene_monitor_options)) + { + std::string error = "Unable to configure planning scene monitor"; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + robot_model_ = planning_scene_monitor_->getRobotModel(); + if (!robot_model_) + { + std::string error = "Unable to construct robot model. Please make sure all needed information is on the " + "parameter server."; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + bool load_planning_pipelines = true; + if (load_planning_pipelines && !loadPlanningPipelines(opt.planning_pipeline_options)) + { + std::string error = "Failed to load planning pipelines from parameter server"; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + // TODO(henningkayser): configure trajectory execution manager + trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager( + robot_model_, planning_scene_monitor_->getStateMonitor())); + + ROS_INFO_NAMED(LOGNAME, "MoveItCpp running"); +} + +MoveItCpp::MoveItCpp(MoveItCpp&& other) +{ + other.clearContents(); +} + +MoveItCpp::~MoveItCpp() +{ + ROS_INFO_NAMED(LOGNAME, "Deleting MoveItCpp"); + clearContents(); +} + +MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) +{ + if (this != &other) + { + this->robot_description_ = other.robot_description_; + this->node_handle_ = other.node_handle_; + this->tf_buffer_ = other.tf_buffer_; + this->robot_model_ = other.robot_model_; + this->planning_scene_monitor_ = other.planning_scene_monitor_; + other.clearContents(); + } + + return *this; +} + +bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt) +{ + planning_scene_monitor_.reset( + new planning_scene_monitor::PlanningSceneMonitor(opt.robot_description, tf_buffer_, opt.name)); + // Allows us to sycronize to Rviz and also publish collision objects to ourselves + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Configuring Planning Scene Monitor"); + if (planning_scene_monitor_->getPlanningScene()) + { + // Start state and scene monitors + ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for joint states", opt.joint_state_topic.c_str()); + planning_scene_monitor_->startStateMonitor(opt.joint_state_topic, opt.attached_collision_object_topic); + planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, + opt.publish_planning_scene_topic); + planning_scene_monitor_->startSceneMonitor(opt.monitored_planning_scene_topic); + } + else + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning scene not configured"); + return false; + } + // TODO(henningkayser): fix and remove lines below + ros::spinOnce(); + ros::Duration(0.5).sleep(); // when at 0.1, i believe sometimes vjoint not properly loaded + + // Wait for complete state to be recieved + if (opt.wait_for_initial_state_timeout > 0.0) + { + return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), + opt.wait_for_initial_state_timeout); + } + + return true; +} + +bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& opt) +{ + ros::NodeHandle node_handle(opt.parent_namespace.empty() ? "~" : opt.parent_namespace); + for (const auto& planning_pipeline_name : opt.pipeline_names) + { + if (planning_pipelines_.count(planning_pipeline_name) > 0) + { + ROS_WARN_NAMED(LOGNAME, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); + continue; + } + ROS_INFO_NAMED(LOGNAME, "Loading planning pipeline '%s'", planning_pipeline_name.c_str()); + ros::NodeHandle child_nh(node_handle, planning_pipeline_name); + planning_pipeline::PlanningPipelinePtr pipeline; + pipeline.reset(new planning_pipeline::PlanningPipeline(robot_model_, child_nh, PLANNING_PLUGIN_PARAM)); + + if (!pipeline->getPlannerManager()) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); + continue; + } + planning_pipelines_[planning_pipeline_name] = pipeline; + } + + if (planning_pipelines_.empty()) + { + return false; + ROS_ERROR_NAMED(LOGNAME, "Failed to load any planning pipelines."); + } + + // Retrieve group/pipeline mapping for faster lookup + std::vector group_names = robot_model_->getJointModelGroupNames(); + for (const auto& pipeline_entry : planning_pipelines_) + { + for (const auto& group_name : group_names) + { + groups_pipelines_map_[group_name] = {}; + const auto& pipeline = pipeline_entry.second; + for (const auto& planner_configuration : pipeline->getPlannerManager()->getPlannerConfigurations()) + { + if (planner_configuration.second.group == group_name) + { + groups_pipelines_map_[group_name].insert(pipeline_entry.first); + } + } + } + } + + return true; +} + +robot_model::RobotModelConstPtr MoveItCpp::getRobotModel() const +{ + ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp::getRobotModel()"); + return robot_model_; +} + +const ros::NodeHandle& MoveItCpp::getNodeHandle() const +{ + ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp::getNodeHandle()"); + return node_handle_; +} + +bool MoveItCpp::getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds) +{ + if (wait_seconds > 0.0 && + !planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), wait_seconds)) + { + ROS_ERROR_NAMED(LOGNAME, "Did not receive robot state"); + return false; + } + { // Lock planning scene + planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor_); + current_state.reset(new moveit::core::RobotState(scene->getCurrentState())); + } // Unlock planning scene + return true; +} + +robot_state::RobotStatePtr MoveItCpp::getCurrentState(double wait) +{ + robot_state::RobotStatePtr current_state; + getCurrentState(current_state, wait); + return current_state; +} + +const std::map& MoveItCpp::getPlanningPipelines() const +{ + return planning_pipelines_; +} + +std::set MoveItCpp::getPlanningPipelineNames(const std::string& group_name) const +{ + std::set result_names; + if (!group_name.empty() && groups_pipelines_map_.count(group_name) == 0) + { + ROS_ERROR_NAMED(LOGNAME, "There are no planning pipelines loaded for group '%s'.", group_name.c_str()); + return result_names; // empty + } + for (const auto& pipeline_entry : planning_pipelines_) + { + const std::string& pipeline_name = pipeline_entry.first; + // If group_name is defined and valid, skip pipelines that don't belong to the planning group + if (!group_name.empty()) + { + const auto& group_pipelines = groups_pipelines_map_.at(group_name); + if (group_pipelines.find(pipeline_name) == group_pipelines.end()) + continue; + } + result_names.insert(pipeline_name); + } + return result_names; +} + +const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const +{ + return planning_scene_monitor_; +} +planning_scene_monitor::PlanningSceneMonitorPtr MoveItCpp::getPlanningSceneMonitorNonConst() +{ + return planning_scene_monitor_; +} + +const trajectory_execution_manager::TrajectoryExecutionManagerPtr& MoveItCpp::getTrajectoryExecutionManager() const +{ + return trajectory_execution_manager_; +} + +trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveItCpp::getTrajectoryExecutionManagerNonConst() +{ + return trajectory_execution_manager_; +} + +bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + bool blocking) +{ + if (!robot_trajectory) + { + ROS_ERROR_NAMED(LOGNAME, "Robot trajectory is undefined"); + return false; + } + + // Check if there are controllers that can handle the execution + if (!trajectory_execution_manager_->ensureActiveControllersForGroup(group_name)) + { + ROS_ERROR_NAMED(LOGNAME, "Execution failed! No active controllers configured for group '%s'", group_name.c_str()); + return false; + } + + // Execute trajectory + moveit_msgs::RobotTrajectory robot_trajectory_msg; + robot_trajectory->getRobotTrajectoryMsg(robot_trajectory_msg); + if (blocking) + { + trajectory_execution_manager_->push(robot_trajectory_msg); + trajectory_execution_manager_->execute(); + return trajectory_execution_manager_->waitForExecution(); + } + trajectory_execution_manager_->pushAndExecute(robot_trajectory_msg); + return true; +} + +const std::shared_ptr& MoveItCpp::getTFBuffer() const +{ + return tf_buffer_; +} + +void MoveItCpp::clearContents() +{ + robot_description_.clear(); + tf_buffer_.reset(); + planning_scene_monitor_.reset(); + robot_model_.reset(); + planning_pipelines_.clear(); +} +} // planning_interface +} // moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp new file mode 100644 index 0000000000..2ce083b17d --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -0,0 +1,354 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * 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 PickNik LLC 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. + *********************************************************************/ + +/* Author: Henning Kayser */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +constexpr char LOGNAME[] = "planning_component"; + +PlanningComponent::PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_context) + : group_name_(group_name), nh_(moveit_context->getNodeHandle()), moveit_cpp_(moveit_context) +{ + joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name); + if (!joint_model_group_) + { + std::string error = "Could not find joint model group '" + group_name + "'."; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name); +} + +PlanningComponent::PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh) + : group_name_(group_name), nh_(nh), moveit_cpp_(new MoveItCpp(nh)) +{ +} + +PlanningComponent::~PlanningComponent() +{ + ROS_INFO_NAMED(LOGNAME, "Deleting PlanningComponent '%s'", group_name_.c_str()); + clearContents(); +} + +PlanningComponent& PlanningComponent::operator=(PlanningComponent&& other) +{ + if (this != &other) + { + this->considered_start_state_ = other.considered_start_state_; + this->workspace_parameters_ = other.workspace_parameters_; + this->last_plan_solution_ = other.last_plan_solution_; + other.clearContents(); + } + return *this; +} + +const std::vector PlanningComponent::getNamedTargets() +{ + if (joint_model_group_) + { + return joint_model_group_->getDefaultStateNames(); + } + else + { + ROS_WARN_NAMED(LOGNAME, "Unable to find joint group with name '%s'.", group_name_.c_str()); + } + + std::vector empty; + return empty; +} + +const std::string& PlanningComponent::getName() const +{ + return group_name_; +} + +PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParameters& parameters) +{ + last_plan_solution_.reset(new PlanSolution()); + if (!joint_model_group_) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); + return *last_plan_solution_; + } + + // Clone current planning scene + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = + moveit_cpp_->getPlanningSceneMonitorNonConst(); + planning_scene_monitor->updateFrameTransforms(); + planning_scene_monitor->lockSceneRead(); // LOCK planning scene + planning_scene::PlanningScenePtr planning_scene = + planning_scene::PlanningScene::clone(planning_scene_monitor->getPlanningScene()); + planning_scene_monitor->unlockSceneRead(); // UNLOCK planning scene + planning_scene_monitor.reset(); // release this pointer + + // Init MotionPlanRequest + ::planning_interface::MotionPlanRequest req; + req.group_name = group_name_; + req.planner_id = parameters.planner_id; + req.allowed_planning_time = parameters.planning_time; + req.max_velocity_scaling_factor = parameters.max_velocity_scaling_factor; + req.max_acceleration_scaling_factor = parameters.max_acceleration_scaling_factor; + if (workspace_parameters_set_) + req.workspace_parameters = workspace_parameters_; + + // Set start state + moveit::core::RobotStatePtr start_state = considered_start_state_; + if (!start_state) + start_state = moveit_cpp_->getCurrentState(); + moveit::core::robotStateToRobotStateMsg(*start_state, req.start_state); + planning_scene->setCurrentState(*start_state); + + // Set goal constraints + if (current_goal_constraints_.empty()) + { + ROS_ERROR_NAMED(LOGNAME, "No goal constraints set for planning request"); + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + return *last_plan_solution_; + } + req.goal_constraints = current_goal_constraints_; + + // Run planning attempt + ::planning_interface::MotionPlanResponse res; + if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end()) + { + ROS_ERROR_NAMED(LOGNAME, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return *last_plan_solution_; + } + const planning_pipeline::PlanningPipelinePtr pipeline = + moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline); + pipeline->generatePlan(planning_scene, req, res); + last_plan_solution_->error_code = res.error_code_.val; + if (res.error_code_.val != res.error_code_.SUCCESS) + { + ROS_ERROR("Could not compute plan successfully"); + return *last_plan_solution_; + } + last_plan_solution_->start_state = req.start_state; + last_plan_solution_->trajectory = res.trajectory_; + // TODO(henningkayser): Visualize trajectory + // std::vector eef_links; + // if (joint_model_group->getEndEffectorTips(eef_links)) + //{ + // for (const auto& eef_link : eef_links) + // { + // ROS_INFO_STREAM("Publishing trajectory for end effector " << eef_link->getName()); + // visual_tools_->publishTrajectoryLine(last_solution_trajectory_, eef_link); + // visual_tools_->publishTrajectoryPath(last_solution_trajectory_, false); + // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT); + // } + //} + return *last_plan_solution_; +} + +PlanningComponent::PlanSolution PlanningComponent::plan() +{ + PlanRequestParameters default_parameters; + default_parameters.planning_attempts = 1; + default_parameters.planning_time = 5.0; + default_parameters.max_velocity_scaling_factor = 1.0; + default_parameters.max_acceleration_scaling_factor = 1.0; + if (!planning_pipeline_names_.empty()) + default_parameters.planning_pipeline = *planning_pipeline_names_.begin(); + return plan(default_parameters); +} + +bool PlanningComponent::setStartState(const robot_state::RobotState& start_state) +{ + considered_start_state_.reset(new robot_state::RobotState(start_state)); + return true; +} + +robot_state::RobotStatePtr PlanningComponent::getStartState() +{ + if (considered_start_state_) + return considered_start_state_; + else + { + robot_state::RobotStatePtr s; + moveit_cpp_->getCurrentState(s, 1.0); + return s; + } +} + +bool PlanningComponent::setStartState(const std::string& start_state_name) +{ + const auto& named_targets = getNamedTargets(); + if (std::find(named_targets.begin(), named_targets.end(), start_state_name) == named_targets.end()) + { + ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", start_state_name.c_str()); + return false; + } + robot_state::RobotState start_state(moveit_cpp_->getRobotModel()); + start_state.setToDefaultValues(joint_model_group_, start_state_name); + return setStartState(start_state); +} + +void PlanningComponent::setStartStateToCurrentState() +{ + considered_start_state_.reset(); +} + +std::map PlanningComponent::getNamedTargetValues(const std::string& name) +{ + // TODO(henningkayser): verify result + std::map positions; + joint_model_group_->getVariableDefaultPositions(name, positions); + return positions; +} + +void PlanningComponent::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) +{ + workspace_parameters_.header.frame_id = moveit_cpp_->getRobotModel()->getModelFrame(); + workspace_parameters_.header.stamp = ros::Time::now(); + workspace_parameters_.min_corner.x = minx; + workspace_parameters_.min_corner.y = miny; + workspace_parameters_.min_corner.z = minz; + workspace_parameters_.max_corner.x = maxx; + workspace_parameters_.max_corner.y = maxy; + workspace_parameters_.max_corner.z = maxz; + workspace_parameters_set_ = true; +} + +void PlanningComponent::unsetWorkspace() +{ + workspace_parameters_set_ = false; +} + +bool PlanningComponent::setGoal(const std::vector& goal_constraints) +{ + current_goal_constraints_ = goal_constraints; + return true; +} + +bool PlanningComponent::setGoal(const robot_state::RobotState& goal_state) +{ + current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_) }; + return true; +} + +bool PlanningComponent::setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name) +{ + const auto& joint_names = joint_model_group_->getLinkModelNames(); + // if (std::find(joint_names.begin(), joint_names.end(), link_name) == joint_names.end()) + //{ + // ROS_ERROR_NAMED(LOGNAME, "Link '%s' is not part of joint model group '%s'.", link_name.c_str(), + // group_name_.c_str()); + // return false; + //} + current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(link_name, goal_pose) }; + return true; +} + +bool PlanningComponent::setGoal(const std::string& goal_state_name) +{ + const auto& named_targets = getNamedTargets(); + if (std::find(named_targets.begin(), named_targets.end(), goal_state_name) == named_targets.end()) + { + ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", goal_state_name.c_str()); + return false; + } + robot_state::RobotState goal_state(moveit_cpp_->getRobotModel()); + goal_state.setToDefaultValues(joint_model_group_, goal_state_name); + return setGoal(goal_state); +} + +bool PlanningComponent::execute(bool blocking) +{ + if (!last_plan_solution_) + { + ROS_ERROR_NAMED(LOGNAME, "There is no successfull plan to execute"); + return false; + } + + // TODO(henningkayser): parameterize timestamps if required + // trajectory_processing::TimeOptimalTrajectoryGeneration totg; + // if (!totg.computeTimeStamps(*last_solution_trajectory_, max_velocity_scaling_factor_, + // max_acceleration_scaling_factor_)) + //{ + // ROS_ERROR("Failed to parameterize trajectory"); + // return false; + //} + return moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking); +} + +const PlanningComponent::PlanSolutionPtr PlanningComponent::getLastPlanSolution() +{ + return last_plan_solution_; +} + +void PlanningComponent::clearContents() +{ + considered_start_state_.reset(); + last_plan_solution_.reset(); + current_goal_constraints_.clear(); + moveit_cpp_.reset(); + planning_pipeline_names_.clear(); +} +} // planning_interface +} // moveit From 849e2fbba1a25e429869198c6172131246c3008c Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Wed, 25 Sep 2019 14:05:30 +0300 Subject: [PATCH 093/612] Initial unit tests for MoveItCpp (#4) --- .../planning_interface/test/CMakeLists.txt | 2 + .../planning_interface/test/moveit_cpp.yaml | 17 ++ .../test/moveit_cpp_test.cpp | 178 ++++++++++++++++++ .../test/moveit_cpp_test.test | 34 ++++ 4 files changed, 231 insertions(+) create mode 100644 moveit_ros/planning_interface/test/moveit_cpp.yaml create mode 100644 moveit_ros/planning_interface/test/moveit_cpp_test.cpp create mode 100644 moveit_ros/planning_interface/test/moveit_cpp_test.test diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 979b4e1f92..7228694591 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -5,6 +5,8 @@ if (CATKIN_ENABLE_TESTING) add_executable(test_cleanup test_cleanup.cpp) target_link_libraries(test_cleanup moveit_move_group_interface) + add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) + target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) add_rostest(python_move_group.test) add_rostest(python_move_group_ns.test) add_rostest(robot_state_update.test) diff --git a/moveit_ros/planning_interface/test/moveit_cpp.yaml b/moveit_ros/planning_interface/test/moveit_cpp.yaml new file mode 100644 index 0000000000..910b85c700 --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp.yaml @@ -0,0 +1,17 @@ +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/planning_scene_monitor" + publish_planning_scene_topic: "/publish_planning_scene" + monitored_planning_scene_topic: "/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +planning_pipelines: + pipeline_names: + - ompl + +default_planner_options: + planning_attempts: 1 + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 \ No newline at end of file diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp new file mode 100644 index 0000000000..ebea62a82d --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp @@ -0,0 +1,178 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * 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 PickNik LLC 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. + *********************************************************************/ + +/* Author: Jafar Abdi + Desc: Test the MoveItCpp and PlanningComponent interfaces +*/ + +// ROS +#include + +// Testing +#include + +// Main class +#include +#include +// Msgs +#include + +namespace moveit +{ +namespace planning_interface +{ +class MoveItCppTest : public ::testing::Test +{ +public: + void SetUp() override + { + nh_ = ros::NodeHandle("/moveit_cpp_test"); + moveit_cpp_ptr = std::make_shared(nh_); + planning_component_ptr = std::make_shared(PLANNING_GROUP, moveit_cpp_ptr); + robot_model_ptr = moveit_cpp_ptr->getRobotModel(); + jmg_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); + + target_pose1.header.frame_id = "panda_link0"; + target_pose1.pose.orientation.w = 1.0; + target_pose1.pose.position.x = 0.28; + target_pose1.pose.position.y = -0.2; + target_pose1.pose.position.z = 0.5; + + start_pose.orientation.w = 1.0; + start_pose.position.x = 0.55; + start_pose.position.y = 0.0; + start_pose.position.z = 0.6; + + target_pose2.orientation.w = 1.0; + target_pose2.position.x = 0.55; + target_pose2.position.y = -0.05; + target_pose2.position.z = 0.8; + } + +protected: + ros::NodeHandle nh_; + MoveItCppPtr moveit_cpp_ptr; + PlanningComponentPtr planning_component_ptr; + robot_model::RobotModelConstPtr robot_model_ptr; + const moveit::core::JointModelGroup* jmg_ptr; + const std::string PLANNING_GROUP = "panda_arm"; + geometry_msgs::PoseStamped target_pose1; + geometry_msgs::Pose target_pose2; + geometry_msgs::Pose start_pose; +}; + +// Test the current and the initial state of the Panda robot +TEST_F(MoveItCppTest, GetCurrentStateTest) +{ + ros::Duration(1.0).sleep(); // Otherwise joint_states will result in an invalid robot state + auto robot_model = moveit_cpp_ptr->getRobotModel(); + auto robot_state = std::make_shared(robot_model); + EXPECT_TRUE(moveit_cpp_ptr->getCurrentState(robot_state, 0.0)); + // Make sure the Panda robot is in "ready" state which is loaded from fake_controller.yaml + std::vector joints_vals; + robot_state->copyJointGroupPositions(PLANNING_GROUP, joints_vals); + EXPECT_NEAR(joints_vals[0], 0.0, 0.001); // panda_joint1 + EXPECT_NEAR(joints_vals[1], -0.785, 0.001); // panda_joint2 + EXPECT_NEAR(joints_vals[2], 0.0, 0.001); // panda_joint3 + EXPECT_NEAR(joints_vals[3], -2.356, 0.001); // panda_joint4 + EXPECT_NEAR(joints_vals[4], 0.0, 0.001); // panda_joint5 + EXPECT_NEAR(joints_vals[5], 1.571, 0.001); // panda_joint6 + EXPECT_NEAR(joints_vals[6], 0.785, 0.001); // panda_joint7 +} + +// Test the name of the planning group used by PlanningComponent for the Panda robot +TEST_F(MoveItCppTest, NameOfPlanningGroupTest) +{ + EXPECT_STREQ(planning_component_ptr->getPlanningGroupName().c_str(), "panda_arm"); +} + +// Test setting the start state of the plan to the current state of the robot +TEST_F(MoveItCppTest, TestSetStartStateToCurrentState) +{ + planning_component_ptr->setStartStateToCurrentState(); + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(static_cast(planning_component_ptr->plan())); + // TODO(JafarAbdi) adding testing to the soln state +} + +// Test setting the goal using geometry_msgs::PoseStamped and a robot's link name +TEST_F(MoveItCppTest, TestSetGoalFromPoseStamped) +{ + planning_component_ptr->setStartStateToCurrentState(); + + geometry_msgs::PoseStamped target_pose1; + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(static_cast(planning_component_ptr->plan())); +} + +// Test setting the plan start state using robot_state::RobotState +TEST_F(MoveItCppTest, TestSetStartStateFromRobotState) +{ + auto start_state = *(moveit_cpp_ptr->getCurrentState()); + start_state.setFromIK(jmg_ptr, start_pose); + + planning_component_ptr->setStartState(start_state); + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(static_cast(planning_component_ptr->plan())); +} + +// Test settting the goal of the plan using a robot_state::RobotState +TEST_F(MoveItCppTest, TestSetGoalFromRobotState) +{ + auto target_state = *(moveit_cpp_ptr->getCurrentState()); + + target_state.setFromIK(jmg_ptr, target_pose2); + + planning_component_ptr->setGoal(target_state); + + ASSERT_TRUE(static_cast(planning_component_ptr->plan())); +} +} // namespace planning_interface +} // namespace moveit + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "moveit_cpp_test"); + + ros::AsyncSpinner spinner(4); + spinner.start(); + + int result = RUN_ALL_TESTS(); + + return result; +} diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.test b/moveit_ros/planning_interface/test/moveit_cpp_test.test new file mode 100644 index 0000000000..dac1ee0807 --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.test @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + ["/moveit_cpp_test/fake_controller_joint_states"] + + + + + + + + From 195e9a1455dd59c5f78112d9458ac08452189ce8 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 18 Nov 2019 17:07:42 +0300 Subject: [PATCH 094/612] Fix warnings + improve readability in MoveItCpp --- .../moveit_cpp/CMakeLists.txt | 11 +++- .../include/moveit/moveit_cpp/moveit_cpp.h | 36 +++++-------- .../moveit/moveit_cpp/planning_component.h | 22 ++++---- .../moveit_cpp/src/moveit_cpp.cpp | 50 ++++++++----------- .../moveit_cpp/src/planning_component.cpp | 27 ++++------ 5 files changed, 66 insertions(+), 80 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt index 098becde4b..a61a60f06f 100644 --- a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -1,8 +1,15 @@ set(MOVEIT_LIB_NAME moveit_cpp) -add_library(${MOVEIT_LIB_NAME} src/moveit_cpp.cpp src/planning_component.cpp) +add_library(${MOVEIT_LIB_NAME} + src/moveit_cpp.cpp + src/planning_component.cpp + ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME} + moveit_common_planning_interface_objects + moveit_planning_scene_interface + ${catkin_LIBRARIES} + ${Boost_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index fd81721ec5..affeceb1a7 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -32,7 +32,10 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Henning Kayser */ +/* Author: Henning Kayser + Desc: A high level interface that does not require the use of ROS Actions, Services, and Messages to access the + core MoveIt functionality +*/ #pragma once @@ -78,6 +81,8 @@ class MoveItCpp std::string publish_planning_scene_topic; double wait_for_initial_state_timeout; }; + + /// struct contains the the variables used for loading the planning pipeline struct PlanningPipelineOptions { void load(const ros::NodeHandle& nh) @@ -89,20 +94,6 @@ class MoveItCpp std::vector pipeline_names; std::string parent_namespace; }; - struct PlannerOptions - { - void load(const ros::NodeHandle& nh) - { - std::string ns = "default_planner_options/"; - nh.getParam(ns + "planning_attempts", planning_attempts); - nh.getParam(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor); - nh.getParam(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor); - } - int planning_attempts; - double planning_time; - double max_velocity_scaling_factor; - double max_acceleration_scaling_factor; - }; /// Parameter container for initializing MoveItCpp struct Options @@ -110,18 +101,16 @@ class MoveItCpp Options(const ros::NodeHandle& nh) { planning_scene_monitor_options.load(nh); - default_planner_options.load(nh); planning_pipeline_options.load(nh); } PlanningSceneMonitorOptions planning_scene_monitor_options; PlanningPipelineOptions planning_pipeline_options; - PlannerOptions default_planner_options; }; /** \brief Constructor */ MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); - MoveItCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); + MoveItCpp(const Options& options, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); /** * @brief This class owns unique resources (e.g. action clients, threads) and its not very @@ -143,10 +132,12 @@ class MoveItCpp /** \brief Get the ROS node handle of this instance operates on */ const ros::NodeHandle& getNodeHandle() const; - /** \brief Get the current state queried from the current state monitor */ + /** \brief Get the current state queried from the current state monitor + \param wait_seconds the time in seconds for the state monitor to wait for a robot state. */ bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds); - /** \brief Get the current state queried from the current state monitor */ + /** \brief Get the current state queried from the current state monitor + \param wait_seconds the time in seconds for the state monitor to wait for a robot state. */ robot_state::RobotStatePtr getCurrentState(double wait_seconds = 0.0); /** \brief Get all loaded planning pipeline instances mapped to their reference names */ @@ -178,7 +169,6 @@ class MoveItCpp private: // Core properties and instances ros::NodeHandle node_handle_; - std::string robot_description_; robot_model::RobotModelConstPtr robot_model_; planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; @@ -194,10 +184,10 @@ class MoveItCpp void clearContents(); /** \brief Initialize and setup the planning scene monitor */ - bool loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt); + bool loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options); /** \brief Initialize and setup the planning pipelines */ - bool loadPlanningPipelines(const PlanningPipelineOptions& opt); + bool loadPlanningPipelines(const PlanningPipelineOptions& options); }; } // namespace planning_interface } // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 06d8124edf..51032ee3ec 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -32,7 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Henning Kayser */ +/* Author: Henning Kayser + Desc: API for planning and execution capabilities of a JointModelGroup */ #pragma once @@ -74,13 +75,13 @@ class PlanningComponent { return val == moveit_msgs::MoveItErrorCodes::SUCCESS; } - bool operator==(const int c) const + bool operator==(const int code) const { - return val == c; + return val == code; } - bool operator!=(const int c) const + bool operator!=(const int code) const { - return val != c; + return val != code; } }; @@ -93,7 +94,7 @@ class PlanningComponent /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) robot_trajectory::RobotTrajectoryPtr trajectory; - /// Error code + /// Reason why the plan failed MoveItErrorCode error_code; explicit operator bool() const @@ -115,7 +116,7 @@ class PlanningComponent /** \brief Constructor */ PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh); - PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_context); + PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp); /** * @brief This class owns unique resources (e.g. action clients, threads) and its not very @@ -132,13 +133,13 @@ class PlanningComponent ~PlanningComponent(); /** \brief Get the name of the planning group */ - const std::string& getName() const; + const std::string& getPlanningGroupName() const; /** \brief Get the names of the named robot states available as targets */ - const std::vector getNamedTargets(); + const std::vector getNamedTargetStates(); /** \brief Get the joint values for targets specified by name */ - std::map getNamedTargetValues(const std::string& name); + std::map getNamedTargetStateValues(const std::string& name); /** \brief Specify the workspace bounding box. The box is specified in the planning frame (i.e. relative to the robot root link start position). @@ -193,6 +194,7 @@ class PlanningComponent // Planning std::set planning_pipeline_names_; + // The start state used in the planning motion request robot_state::RobotStatePtr considered_start_state_; std::vector current_goal_constraints_; PlanRequestParameters plan_request_parameters_; diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index f27efed06b..c3dd2ee9f9 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -50,7 +50,6 @@ namespace moveit namespace planning_interface { constexpr char LOGNAME[] = "moveit_cpp"; -constexpr char PLANNING_SCENE_MONITOR_NAME[] = "moveit_cpp_planning_scene"; constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin"; MoveItCpp::MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) @@ -58,16 +57,17 @@ MoveItCpp::MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) +MoveItCpp::MoveItCpp(const Options& options, const ros::NodeHandle& nh, + const std::shared_ptr& tf_buffer) { if (!tf_buffer_) tf_buffer_.reset(new tf2_ros::Buffer()); tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_)); // Configure planning scene monitor - if (!loadPlanningSceneMonitor(opt.planning_scene_monitor_options)) + if (!loadPlanningSceneMonitor(options.planning_scene_monitor_options)) { - std::string error = "Unable to configure planning scene monitor"; + const std::string error = "Unable to configure planning scene monitor"; ROS_FATAL_STREAM_NAMED(LOGNAME, error); throw std::runtime_error(error); } @@ -75,16 +75,16 @@ MoveItCpp::MoveItCpp(const Options& opt, const ros::NodeHandle& nh, const std::s robot_model_ = planning_scene_monitor_->getRobotModel(); if (!robot_model_) { - std::string error = "Unable to construct robot model. Please make sure all needed information is on the " - "parameter server."; + const std::string error = "Unable to construct robot model. Please make sure all needed information is on the " + "parameter server."; ROS_FATAL_STREAM_NAMED(LOGNAME, error); throw std::runtime_error(error); } bool load_planning_pipelines = true; - if (load_planning_pipelines && !loadPlanningPipelines(opt.planning_pipeline_options)) + if (load_planning_pipelines && !loadPlanningPipelines(options.planning_pipeline_options)) { - std::string error = "Failed to load planning pipelines from parameter server"; + const std::string error = "Failed to load planning pipelines from parameter server"; ROS_FATAL_STREAM_NAMED(LOGNAME, error); throw std::runtime_error(error); } @@ -111,7 +111,6 @@ MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) { if (this != &other) { - this->robot_description_ = other.robot_description_; this->node_handle_ = other.node_handle_; this->tf_buffer_ = other.tf_buffer_; this->robot_model_ = other.robot_model_; @@ -122,44 +121,41 @@ MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) return *this; } -bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt) +bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options) { planning_scene_monitor_.reset( - new planning_scene_monitor::PlanningSceneMonitor(opt.robot_description, tf_buffer_, opt.name)); + new planning_scene_monitor::PlanningSceneMonitor(options.robot_description, tf_buffer_, options.name)); // Allows us to sycronize to Rviz and also publish collision objects to ourselves ROS_DEBUG_STREAM_NAMED(LOGNAME, "Configuring Planning Scene Monitor"); if (planning_scene_monitor_->getPlanningScene()) { // Start state and scene monitors - ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for joint states", opt.joint_state_topic.c_str()); - planning_scene_monitor_->startStateMonitor(opt.joint_state_topic, opt.attached_collision_object_topic); + ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for joint states", options.joint_state_topic.c_str()); + planning_scene_monitor_->startStateMonitor(options.joint_state_topic, options.attached_collision_object_topic); planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, - opt.publish_planning_scene_topic); - planning_scene_monitor_->startSceneMonitor(opt.monitored_planning_scene_topic); + options.publish_planning_scene_topic); + planning_scene_monitor_->startSceneMonitor(options.monitored_planning_scene_topic); } else { ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning scene not configured"); return false; } - // TODO(henningkayser): fix and remove lines below - ros::spinOnce(); - ros::Duration(0.5).sleep(); // when at 0.1, i believe sometimes vjoint not properly loaded // Wait for complete state to be recieved - if (opt.wait_for_initial_state_timeout > 0.0) + if (options.wait_for_initial_state_timeout > 0.0) { return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), - opt.wait_for_initial_state_timeout); + options.wait_for_initial_state_timeout); } return true; } -bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& opt) +bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options) { - ros::NodeHandle node_handle(opt.parent_namespace.empty() ? "~" : opt.parent_namespace); - for (const auto& planning_pipeline_name : opt.pipeline_names) + ros::NodeHandle node_handle(options.parent_namespace.empty() ? "~" : options.parent_namespace); + for (const auto& planning_pipeline_name : options.pipeline_names) { if (planning_pipelines_.count(planning_pipeline_name) > 0) { @@ -208,13 +204,11 @@ bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& opt) robot_model::RobotModelConstPtr MoveItCpp::getRobotModel() const { - ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp::getRobotModel()"); return robot_model_; } const ros::NodeHandle& MoveItCpp::getNodeHandle() const { - ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp::getNodeHandle()"); return node_handle_; } @@ -272,6 +266,7 @@ const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSce { return planning_scene_monitor_; } + planning_scene_monitor::PlanningSceneMonitorPtr MoveItCpp::getPlanningSceneMonitorNonConst() { return planning_scene_monitor_; @@ -323,11 +318,10 @@ const std::shared_ptr& MoveItCpp::getTFBuffer() const void MoveItCpp::clearContents() { - robot_description_.clear(); tf_buffer_.reset(); planning_scene_monitor_.reset(); robot_model_.reset(); planning_pipelines_.clear(); } -} // planning_interface -} // moveit +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp index 2ce083b17d..896ecb783d 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -70,8 +70,8 @@ namespace planning_interface { constexpr char LOGNAME[] = "planning_component"; -PlanningComponent::PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_context) - : group_name_(group_name), nh_(moveit_context->getNodeHandle()), moveit_cpp_(moveit_context) +PlanningComponent::PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp) + : nh_(moveit_cpp->getNodeHandle()), moveit_cpp_(moveit_cpp), group_name_(group_name) { joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name); if (!joint_model_group_) @@ -84,7 +84,7 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const MoveIt } PlanningComponent::PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh) - : group_name_(group_name), nh_(nh), moveit_cpp_(new MoveItCpp(nh)) + : nh_(nh), moveit_cpp_(new MoveItCpp(nh)), group_name_(group_name) { } @@ -106,7 +106,7 @@ PlanningComponent& PlanningComponent::operator=(PlanningComponent&& other) return *this; } -const std::vector PlanningComponent::getNamedTargets() +const std::vector PlanningComponent::getNamedTargetStates() { if (joint_model_group_) { @@ -121,7 +121,7 @@ const std::vector PlanningComponent::getNamedTargets() return empty; } -const std::string& PlanningComponent::getName() const +const std::string& PlanningComponent::getPlanningGroupName() const { return group_name_; } @@ -238,7 +238,7 @@ robot_state::RobotStatePtr PlanningComponent::getStartState() bool PlanningComponent::setStartState(const std::string& start_state_name) { - const auto& named_targets = getNamedTargets(); + const auto& named_targets = getNamedTargetStates(); if (std::find(named_targets.begin(), named_targets.end(), start_state_name) == named_targets.end()) { ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", start_state_name.c_str()); @@ -254,7 +254,7 @@ void PlanningComponent::setStartStateToCurrentState() considered_start_state_.reset(); } -std::map PlanningComponent::getNamedTargetValues(const std::string& name) +std::map PlanningComponent::getNamedTargetStateValues(const std::string& name) { // TODO(henningkayser): verify result std::map positions; @@ -294,20 +294,13 @@ bool PlanningComponent::setGoal(const robot_state::RobotState& goal_state) bool PlanningComponent::setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name) { - const auto& joint_names = joint_model_group_->getLinkModelNames(); - // if (std::find(joint_names.begin(), joint_names.end(), link_name) == joint_names.end()) - //{ - // ROS_ERROR_NAMED(LOGNAME, "Link '%s' is not part of joint model group '%s'.", link_name.c_str(), - // group_name_.c_str()); - // return false; - //} current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(link_name, goal_pose) }; return true; } bool PlanningComponent::setGoal(const std::string& goal_state_name) { - const auto& named_targets = getNamedTargets(); + const auto& named_targets = getNamedTargetStates(); if (std::find(named_targets.begin(), named_targets.end(), goal_state_name) == named_targets.end()) { ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", goal_state_name.c_str()); @@ -350,5 +343,5 @@ void PlanningComponent::clearContents() moveit_cpp_.reset(); planning_pipeline_names_.clear(); } -} // planning_interface -} // moveit +} // namespace planning_interface +} // namespace moveit From 0924e99de84783a3a4cae4a57e51017d5cc48a59 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Thu, 21 Nov 2019 15:26:47 -0700 Subject: [PATCH 095/612] Unify jog_arm package to be C++14 (#1762) --- moveit_experimental/moveit_jog_arm/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index e95c156c6b..899efdad17 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(moveit_jog_arm) -add_compile_options(-std=c++11) +add_compile_options(-std=c++14) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) From c249475220c3fee3a5797f3764c00714d4f1ef69 Mon Sep 17 00:00:00 2001 From: Takara Kasai Date: Sat, 23 Nov 2019 09:02:18 +0900 Subject: [PATCH 096/612] Add "" robot state to RViz motion panel (#1742) --- .../motion_planning_display.h | 8 ++++++++ .../src/motion_planning_display.cpp | 12 +++++++++--- .../src/motion_planning_frame.cpp | 2 ++ .../src/motion_planning_frame_planning.cpp | 13 +++++++++++++ 4 files changed, 32 insertions(+), 3 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index 52cd9b5640..84e911f1a5 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -107,6 +107,11 @@ class MotionPlanningDisplay : public PlanningSceneDisplay return query_goal_state_->getState(); } + const robot_state::RobotState& getPreviousState() const + { + return *previous_state_; + } + const robot_interaction::RobotInteractionPtr& getRobotInteraction() const { return robot_interaction_; @@ -132,6 +137,7 @@ class MotionPlanningDisplay : public PlanningSceneDisplay void updateQueryStartState(); void updateQueryGoalState(); + void rememberPreviousStartState(); void useApproximateIK(bool flag); @@ -253,6 +259,8 @@ private Q_SLOTS: std::shared_ptr menu_handler_goal_; std::map status_links_start_; std::map status_links_goal_; + /// remember previous start state (updated before starting execution) + robot_state::RobotStatePtr previous_state_; /// Hold the names of the groups for which the query states have been updated (and should not be altered when new info /// is received from the planning scene) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 49de9cd852..2087245192 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -936,6 +936,11 @@ void MotionPlanningDisplay::updateQueryGoalState() context_->queueRender(); } +void MotionPlanningDisplay::rememberPreviousStartState() +{ + *previous_state_ = *query_start_state_->getState(); +} + void MotionPlanningDisplay::setQueryStartState(const robot_state::RobotState& start) { query_start_state_->setState(start); @@ -1135,10 +1140,11 @@ void MotionPlanningDisplay::onRobotModelLoaded() query_robot_start_->load(*getRobotModel()->getURDF()); query_robot_goal_->load(*getRobotModel()->getURDF()); - robot_state::RobotStatePtr ks(new robot_state::RobotState(getPlanningSceneRO()->getCurrentState())); - query_start_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "start", *ks, + // initialize previous state to current state + previous_state_ = std::make_shared(getPlanningSceneRO()->getCurrentState()); + query_start_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "start", *previous_state_, planning_scene_monitor_->getTFClient())); - query_goal_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "goal", *ks, + query_goal_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "goal", *previous_state_, planning_scene_monitor_->getTFClient())); query_start_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this, _1, _2)); query_goal_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this, _1, _2)); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 9a5e5ab110..458354b8dd 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -281,11 +281,13 @@ void MotionPlanningFrame::fillStateSelectionOptions() ui_->start_state_combo_box->addItem(QString("")); ui_->start_state_combo_box->addItem(QString("")); ui_->start_state_combo_box->addItem(QString("")); + ui_->start_state_combo_box->addItem(QString("")); ui_->goal_state_combo_box->addItem(QString("")); ui_->goal_state_combo_box->addItem(QString("")); ui_->goal_state_combo_box->addItem(QString("")); ui_->goal_state_combo_box->addItem(QString("")); + ui_->goal_state_combo_box->addItem(QString("")); const std::vector& known_states = jmg->getDefaultStateNames(); if (!known_states.empty()) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 7febadd195..88417e1033 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -172,6 +172,7 @@ void MotionPlanningFrame::computePlanButtonClicked() ui_->result_label->setText("Planning..."); configureForPlanning(); + planning_display_->rememberPreviousStartState(); bool success = (ui_->use_cartesian_path->isEnabled() && ui_->use_cartesian_path->checkState()) ? computeCartesianPlan() : computeJointSpacePlan(); @@ -206,6 +207,7 @@ void MotionPlanningFrame::computePlanAndExecuteButtonClicked() if (!move_group_) return; configureForPlanning(); + planning_display_->rememberPreviousStartState(); // move_group::move() on the server side, will always start from the current state // to suppress a warning, we pass an empty state (which encodes "start from current state") move_group_->setStartStateToCurrentState(); @@ -242,6 +244,11 @@ void MotionPlanningFrame::onFinishedExecution(bool success) // update query start state to current if neccessary if (ui_->start_state_combo_box->currentText() == "") startStateTextChanged(ui_->start_state_combo_box->currentText()); + + // auto-update goal to stored previous state (but only on success) + // on failure, the user must update the goal to the previous state himself + if (ui_->goal_state_combo_box->currentText() == "") + goalStateTextChanged(ui_->goal_state_combo_box->currentText()); } void MotionPlanningFrame::startStateTextChanged(const QString& start_state) @@ -344,6 +351,12 @@ void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState& state, return; } + if (v == "") + { + state = planning_display_->getPreviousState(); + return; + } + // maybe it is a named state if (const robot_model::JointModelGroup* jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) state.setToDefaultValues(jmg, v); From fd82ce5e26ffaaa855930792f479929bfd94f52d Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Sat, 23 Nov 2019 02:47:14 -0700 Subject: [PATCH 097/612] Fix infinite recursive call to loadConstraintApproximations (#1768) Fixup for #1428. --- .../ompl/ompl_interface/src/model_based_planning_context.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 873b11c7e8..4d05cedd2d 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -834,7 +834,6 @@ bool ompl_interface::ModelBasedPlanningContext::loadConstraintApproximations(con std::string constraint_path; if (nh.getParam("constraint_approximations_path", constraint_path)) { - loadConstraintApproximations(constraint_path); constraints_library_->loadConstraintApproximations(constraint_path); std::stringstream ss; constraints_library_->printConstraintApproximations(ss); From 36f68175065b6a4be8e08d6787e3104611513a61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Sun, 24 Nov 2019 13:16:21 +0100 Subject: [PATCH 098/612] Provide UniquePtr macros (#1771) --- .../include/moveit/macros/class_forward.h | 4 +-- .../include/moveit/macros/declare_ptr.h | 25 +++++++++++-------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/moveit_core/macros/include/moveit/macros/class_forward.h b/moveit_core/macros/include/moveit/macros/class_forward.h index 1c5ab03544..be4e6918b2 100644 --- a/moveit_core/macros/include/moveit/macros/class_forward.h +++ b/moveit_core/macros/include/moveit/macros/class_forward.h @@ -40,9 +40,7 @@ /** * \def MOVEIT_CLASS_FORWARD - * Macro that forward declares a class and defines two shared ptrs types: - * - ${Class}Ptr = shared_ptr<${Class}> - * - ${Class}ConstPtr = shared_ptr + * Macro that forward declares a class and defines the respective smartpointers through MOVEIT_DECLARE_PTR. */ #define MOVEIT_CLASS_FORWARD(C) \ diff --git a/moveit_core/macros/include/moveit/macros/declare_ptr.h b/moveit_core/macros/include/moveit/macros/declare_ptr.h index fc3eeb6e39..1da6dcd7b5 100644 --- a/moveit_core/macros/include/moveit/macros/declare_ptr.h +++ b/moveit_core/macros/include/moveit/macros/declare_ptr.h @@ -39,8 +39,12 @@ /** * \def MOVEIT_DELCARE_PTR * Macro that given a Name and a Type declares the following types: - * - ${Name}Ptr = shared_ptr<${Type}> - * - ${Name}ConstPtr = shared_ptr + * - ${Name}Ptr = shared_ptr<${Type}> + * - ${Name}ConstPtr = shared_ptr + * - ${Name}WeakPtr = weak_ptr<${Type}> + * - ${Name}ConstWeakPtr = weak_ptr + * - ${Name}UniquePtr = unique_ptr<${Type}> + * - ${Name}ConstUniquePtr = unique_ptr * * For best portability the exact type of shared_ptr declared by the macro * should be considered to be an implementation detail, liable to change in @@ -51,21 +55,22 @@ typedef std::shared_ptr Name##Ptr; \ typedef std::shared_ptr Name##ConstPtr; \ typedef std::weak_ptr Name##WeakPtr; \ - typedef std::weak_ptr Name##ConstWeakPtr; + typedef std::weak_ptr Name##ConstWeakPtr; \ + typedef std::unique_ptr Name##UniquePtr; \ + typedef std::unique_ptr Name##ConstUniquePtr; /** * \def MOVEIT_DELCARE_PTR_MEMBER - * Macro that given a Type declares the following types: - * - Ptr = shared_ptr<${Type}> - * - ConstPtr = shared_ptr + * The macro defines the same typedefs as MOVEIT_DECLARE_PTR, but shortens the new names to their suffix. * - * This macro is intended for declaring the pointer typedefs as members of a - * class template. In other situations, MOVEIT_CLASS_FORWARD and - * MOVEIT_DECLARE_PTR should be preferred. + * This can be used to create `Classname::Ptr` style names, but in most situations in MoveIt's codebase, + * MOVEIT_CLASS_FORWARD and MOVEIT_DECLARE_PTR should be preferred. */ #define MOVEIT_DECLARE_PTR_MEMBER(Type) \ typedef std::shared_ptr Ptr; \ typedef std::shared_ptr ConstPtr; \ typedef std::weak_ptr WeakPtr; \ - typedef std::weak_ptr ConstWeakPtr; + typedef std::weak_ptr ConstWeakPtr; \ + typedef std::unique_ptr UniquePtr; \ + typedef std::unique_ptr ConstUniquePtr; From 11c82ece67302e9190bfa45418ee5e162d8950a2 Mon Sep 17 00:00:00 2001 From: Yannick Jonetzko Date: Sat, 23 Nov 2019 22:41:00 +0100 Subject: [PATCH 099/612] Show attached bodies trail in RViz (#1766) --- .../rviz_plugin_render_tools/render_shapes.h | 1 + .../robot_state_visualization.h | 2 + .../trajectory_visualization.h | 3 +- .../src/render_shapes.cpp | 7 +++ .../src/robot_state_visualization.cpp | 6 ++ .../src/trajectory_visualization.cpp | 55 ++++++++++--------- 6 files changed, 46 insertions(+), 28 deletions(-) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h index 808d91b710..a58697c244 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h @@ -69,6 +69,7 @@ class RenderShapes void renderShape(Ogre::SceneNode* node, const shapes::Shape* s, const Eigen::Isometry3d& p, OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode, const rviz::Color& color, float alpha); + void updateShapeColors(float r, float g, float b, float a); void clear(); private: diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index 09ffce9234..01444a817f 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -68,6 +68,8 @@ class RobotStateVisualization const std_msgs::ColorRGBA& default_attached_object_color, const std::map& color_map); void setDefaultAttachedObjectColor(const std_msgs::ColorRGBA& default_attached_object_color); + /// update color of all attached object shapes + void updateAttachedObjectColors(const std_msgs::ColorRGBA& attached_object_color); bool isVisible() const { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h index 23aa020bf6..025fbc0c82 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h @@ -127,6 +127,7 @@ private Q_SLOTS: // Handles actually drawing the robot along motion plans RobotStateVisualizationPtr display_path_robot_; + std_msgs::ColorRGBA default_attached_object_color_; // Handle colouring of robot void setRobotColor(rviz::Robot* robot, const QColor& color); @@ -134,7 +135,7 @@ private Q_SLOTS: robot_trajectory::RobotTrajectoryPtr displaying_trajectory_message_; robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_; - std::vector trajectory_trail_; + std::vector trajectory_trail_; ros::Subscriber trajectory_topic_sub_; bool animating_path_; bool drop_displaying_trajectory_; diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index 586efb91f2..0743a5e823 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -186,4 +186,11 @@ void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, co scene_shapes_.emplace_back(ogre_shape); } } + +void RenderShapes::updateShapeColors(float r, float g, float b, float a) +{ + for (auto it = scene_shapes_.begin(), end = scene_shapes_.end(); it != end; ++it) + (**it).setColor(r, g, b, a); +} + } // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp index db6192c7ad..6460d1f083 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp @@ -80,6 +80,12 @@ void RobotStateVisualization::setDefaultAttachedObjectColor(const std_msgs::Colo default_attached_object_color_ = default_attached_object_color; } +void RobotStateVisualization::updateAttachedObjectColors(const std_msgs::ColorRGBA& attached_object_color) +{ + render_shapes_->updateShapeColors(attached_object_color.r, attached_object_color.g, attached_object_color.b, + robot_.getAlpha()); +} + void RobotStateVisualization::update(const robot_state::RobotStateConstPtr& kinematic_state) { updateHelper(kinematic_state, default_attached_object_color_, nullptr); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 428aac9414..39cbd4e569 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -201,8 +201,6 @@ void TrajectoryVisualization::reset() void TrajectoryVisualization::clearTrajectoryTrail() { - for (rviz::Robot* waypoint_state : trajectory_trail_) - delete waypoint_state; trajectory_trail_.clear(); } @@ -231,17 +229,17 @@ void TrajectoryVisualization::changedShowTrail() for (std::size_t i = 0; i < trajectory_trail_.size(); i++) { int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point - rviz::Robot* r = - new rviz::Robot(scene_node_, context_, "Trail Robot " + boost::lexical_cast(i), nullptr); + auto r = std::make_unique(scene_node_, context_, + "Trail Robot " + boost::lexical_cast(i), nullptr); r->load(*robot_model_->getURDF()); r->setVisualVisible(display_path_visual_enabled_property_->getBool()); r->setCollisionVisible(display_path_collision_enabled_property_->getBool()); r->setAlpha(robot_path_alpha_property_->getFloat()); - r->update(PlanningLinkUpdater(t->getWayPointPtr(waypoint_i))); + r->update(t->getWayPointPtr(waypoint_i), default_attached_object_color_); if (enable_robot_color_property_->getBool()) - setRobotColor(r, robot_color_property_->getColor()); + setRobotColor(&(r->getRobot()), robot_color_property_->getColor()); r->setVisible(display_->isEnabled() && (!animating_path_ || waypoint_i <= current_state_)); - trajectory_trail_[i] = r; + trajectory_trail_[i] = std::move(r); } } @@ -254,8 +252,8 @@ void TrajectoryVisualization::changedTrailStepSize() void TrajectoryVisualization::changedRobotPathAlpha() { display_path_robot_->setAlpha(robot_path_alpha_property_->getFloat()); - for (rviz::Robot* waypoint_state : trajectory_trail_) - waypoint_state->setAlpha(robot_path_alpha_property_->getFloat()); + for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_) + r->setAlpha(robot_path_alpha_property_->getFloat()); } void TrajectoryVisualization::changedTrajectoryTopic() @@ -275,8 +273,8 @@ void TrajectoryVisualization::changedDisplayPathVisualEnabled() { display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool()); display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_); - for (rviz::Robot* waypoint_state : trajectory_trail_) - waypoint_state->setVisualVisible(display_path_visual_enabled_property_->getBool()); + for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_) + r->setVisualVisible(display_path_visual_enabled_property_->getBool()); } } @@ -290,8 +288,8 @@ void TrajectoryVisualization::changedDisplayPathCollisionEnabled() { display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool()); display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_); - for (rviz::Robot* waypoint_state : trajectory_trail_) - waypoint_state->setCollisionVisible(display_path_collision_enabled_property_->getBool()); + for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_) + r->setCollisionVisible(display_path_collision_enabled_property_->getBool()); } } @@ -302,11 +300,11 @@ void TrajectoryVisualization::onEnable() display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool()); display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool()); display_path_robot_->setVisible(displaying_trajectory_message_ && animating_path_); - for (rviz::Robot* waypoint_state : trajectory_trail_) + for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_) { - waypoint_state->setVisualVisible(display_path_visual_enabled_property_->getBool()); - waypoint_state->setCollisionVisible(display_path_collision_enabled_property_->getBool()); - waypoint_state->setVisible(true); + r->setVisualVisible(display_path_visual_enabled_property_->getBool()); + r->setCollisionVisible(display_path_collision_enabled_property_->getBool()); + r->setVisible(true); } changedTrajectoryTopic(); // load topic at startup if default used @@ -315,8 +313,8 @@ void TrajectoryVisualization::onEnable() void TrajectoryVisualization::onDisable() { display_path_robot_->setVisible(false); - for (rviz::Robot* waypoint_state : trajectory_trail_) - waypoint_state->setVisible(false); + for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_) + r->setVisible(false); displaying_trajectory_message_.reset(); animating_path_ = false; if (trajectory_slider_panel_) @@ -541,15 +539,18 @@ void TrajectoryVisualization::unsetRobotColor(rviz::Robot* robot) void TrajectoryVisualization::setDefaultAttachedObjectColor(const QColor& color) { - if (!display_path_robot_) - return; + default_attached_object_color_.r = color.redF(); + default_attached_object_color_.g = color.greenF(); + default_attached_object_color_.b = color.blueF(); + default_attached_object_color_.a = color.alphaF(); - std_msgs::ColorRGBA color_msg; - color_msg.r = color.redF(); - color_msg.g = color.greenF(); - color_msg.b = color.blueF(); - color_msg.a = color.alphaF(); - display_path_robot_->setDefaultAttachedObjectColor(color_msg); + if (display_path_robot_) + { + display_path_robot_->setDefaultAttachedObjectColor(default_attached_object_color_); + display_path_robot_->updateAttachedObjectColors(default_attached_object_color_); + } + for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_) + r->updateAttachedObjectColors(default_attached_object_color_); } void TrajectoryVisualization::setRobotColor(rviz::Robot* robot, const QColor& color) From fd1dba5d96491cf31b7b854e7da8fcd81796b314 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 25 Nov 2019 11:57:31 +0100 Subject: [PATCH 100/612] rviz panel: pass start state as empty to move_group (#1772) ... to ensure that the most recent current state is actually used for planning --- .../src/motion_planning_frame_planning.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 88417e1033..2e6588064f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -172,6 +172,9 @@ void MotionPlanningFrame::computePlanButtonClicked() ui_->result_label->setText("Planning..."); configureForPlanning(); + // move_group node uses an empty start state to refer to the most recent current state + if (ui_->start_state_combo_box->currentText() == "") + move_group_->setStartStateToCurrentState(); planning_display_->rememberPreviousStartState(); bool success = (ui_->use_cartesian_path->isEnabled() && ui_->use_cartesian_path->checkState()) ? computeCartesianPlan() : From 4ff87ded55d4f9a14e188fcda0fae6691be3611c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 25 Nov 2019 10:58:59 -0600 Subject: [PATCH 101/612] Update license formatting (#1764) --- .../moveit_jog_arm/collision_check_thread.h | 75 +++++++++---------- .../include/moveit_jog_arm/jog_arm_data.h | 75 +++++++++---------- .../include/moveit_jog_arm/jog_calcs.h | 75 +++++++++---------- .../moveit_jog_arm/jog_ros_interface.h | 75 +++++++++---------- .../include/moveit_jog_arm/low_pass_filter.h | 75 +++++++++---------- .../src/collision_check_thread.cpp | 75 +++++++++---------- .../moveit_jog_arm/src/jog_calcs.cpp | 75 +++++++++---------- .../moveit_jog_arm/src/jog_ros_interface.cpp | 75 +++++++++---------- .../moveit_jog_arm/src/jog_server.cpp | 75 +++++++++---------- .../moveit_jog_arm/src/low_pass_filter.cpp | 75 +++++++++---------- .../src/teleop_examples/spacenav_to_twist.cpp | 38 ++++++++++ 11 files changed, 408 insertions(+), 380 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index d5c8551576..3c18a34898 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -1,41 +1,40 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : collision_check_thread.h -// Project : moveit_jog_arm -// Created : 1/11/2019 -// Author : Brian O'Neil, Andy Zelenak, Blake Anderson -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// 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 the copyright holder 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 HOLDER 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. -// -/////////////////////////////////////////////////////////////////////////////// +/* + Title : collision_check_thread.h + Project : moveit_jog_arm + Created : 1/11/2019 + Author : Brian O'Neil, Andy Zelenak, Blake Anderson + +BSD 3-Clause License + +Copyright (c) 2019, Los Alamos National Security, LLC +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 the copyright holder 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 HOLDER 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 diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 0950d07405..5b2d384f34 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -1,41 +1,40 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : jog_arm_data.h -// Project : moveit_jog_arm -// Created : 1/11/2019 -// Author : Brian O'Neil, Andy Zelenak, Blake Anderson -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// 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 the copyright holder 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 HOLDER 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. -// -/////////////////////////////////////////////////////////////////////////////// +/* + Title : jog_arm_data.h + Project : moveit_jog_arm + Created : 1/11/2019 + Author : Brian O'Neil, Andy Zelenak, Blake Anderson + +BSD 3-Clause License + +Copyright (c) 2019, Los Alamos National Security, LLC +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 the copyright holder 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 HOLDER 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 diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 4d01690c7a..e80197db22 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -1,41 +1,40 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : jog_calcs.h -// Project : moveit_jog_arm -// Created : 1/11/2019 -// Author : Brian O'Neil, Andy Zelenak, Blake Anderson -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// 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 the copyright holder 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 HOLDER 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. -// -/////////////////////////////////////////////////////////////////////////////// +/* + Title : jog_calcs.h + Project : moveit_jog_arm + Created : 1/11/2019 + Author : Brian O'Neil, Andy Zelenak, Blake Anderson + +BSD 3-Clause License + +Copyright (c) 2019, Los Alamos National Security, LLC +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 the copyright holder 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 HOLDER 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 diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h index 8d69686430..9a3e023c06 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h @@ -1,41 +1,40 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : jog_ros_interface.h -// Project : moveit_jog_arm -// Created : 3/9/2017 -// Author : Brian O'Neil, Blake Anderson, Andy Zelenak -// -// BSD 3-Clause License -// -// Copyright (c) 2018, Los Alamos National Security, LLC -// 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 the copyright holder 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 HOLDER 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. -// -/////////////////////////////////////////////////////////////////////////////// +/* + Title : jog_ros_interface.h + Project : moveit_jog_arm + Created : 3/9/2017 + Author : Brian O'Neil, Blake Anderson, Andy Zelenak + +BSD 3-Clause License + +Copyright (c) 2018, Los Alamos National Security, LLC +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 the copyright holder 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 HOLDER 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. +*/ // Server node for arm jogging with MoveIt. diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h index 424ab6e2f1..dd592524c1 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h @@ -1,41 +1,40 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : low_pass_filter.h -// Project : moveit_jog_arm -// Created : 1/11/2019 -// Author : Andy Zelenak -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// 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 the copyright holder 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 HOLDER 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. -// -/////////////////////////////////////////////////////////////////////////////// +/* + Title : low_pass_filter.h + Project : moveit_jog_arm + Created : 1/11/2019 + Author : Andy Zelenak + +BSD 3-Clause License + +Copyright (c) 2019, Los Alamos National Security, LLC +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 the copyright holder 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 HOLDER 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 diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 56cb82d164..4df88d329a 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -1,41 +1,40 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : collision_check_thread.cpp -// Project : moveit_jog_arm -// Created : 1/11/2019 -// Author : Brian O'Neil, Andy Zelenak, Blake Anderson -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// 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 the copyright holder 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 HOLDER 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. -// -/////////////////////////////////////////////////////////////////////////////// +/* + Title : collision_check_thread.cpp + Project : moveit_jog_arm + Created : 1/11/2019 + Author : Brian O'Neil, Andy Zelenak, Blake Anderson + +BSD 3-Clause License + +Copyright (c) 2019, Los Alamos National Security, LLC +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 the copyright holder 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 HOLDER 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 diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index e89087f1a1..ec2ee3acd9 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -1,41 +1,40 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : jog_calcs.h -// Project : moveit_jog_arm -// Created : 1/11/2019 -// Author : Brian O'Neil, Andy Zelenak, Blake Anderson -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// 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 the copyright holder 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 HOLDER 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. -// -/////////////////////////////////////////////////////////////////////////////// +/* + Title : jog_calcs.h + Project : moveit_jog_arm + Created : 1/11/2019 + Author : Brian O'Neil, Andy Zelenak, Blake Anderson + +BSD 3-Clause License + +Copyright (c) 2019, Los Alamos National Security, LLC +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 the copyright holder 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 HOLDER 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 diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp index 6eea959af4..c83c59e46a 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp @@ -1,41 +1,40 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : jog_ros_interface.cpp -// Project : moveit_jog_arm -// Created : 3/9/2017 -// Author : Brian O'Neil, Andy Zelenak, Blake Anderson -// -// BSD 3-Clause License -// -// Copyright (c) 2018, Los Alamos National Security, LLC -// 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 the copyright holder 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 HOLDER 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. -// -/////////////////////////////////////////////////////////////////////////////// +/* + Title : jog_ros_interface.cpp + Project : moveit_jog_arm + Created : 3/9/2017 + Author : Brian O'Neil, Andy Zelenak, Blake Anderson + +BSD 3-Clause License + +Copyright (c) 2018, Los Alamos National Security, LLC +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 the copyright holder 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 HOLDER 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. +*/ // Server node for arm jogging with MoveIt. diff --git a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp index ba96f3049f..64c03a37ca 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp @@ -1,41 +1,40 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : jog_server.cpp -// Project : moveit_jog_arm -// Created : 12/31/2018 -// Author : Andy Zelenak -// -// BSD 3-Clause License -// -// Copyright (c) 2018, Los Alamos National Security, LLC -// 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 the copyright holder 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 HOLDER 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. -// -/////////////////////////////////////////////////////////////////////////////// +/* + Title : jog_server.cpp + Project : moveit_jog_arm + Created : 12/31/2018 + Author : Andy Zelenak + +BSD 3-Clause License + +Copyright (c) 2018, Los Alamos National Security, LLC +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 the copyright holder 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 HOLDER 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 diff --git a/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp b/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp index 8ee2809a5c..ce1c6158f5 100644 --- a/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp +++ b/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp @@ -1,41 +1,40 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : low_pass_filter.cpp -// Project : moveit_jog_arm -// Created : 1/11/2019 -// Author : Andy Zelenak -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// 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 the copyright holder 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 HOLDER 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. -// -/////////////////////////////////////////////////////////////////////////////// +/* + Title : low_pass_filter.cpp + Project : moveit_jog_arm + Created : 1/11/2019 + Author : Andy Zelenak + +BSD 3-Clause License + +Copyright (c) 2019, Los Alamos National Security, LLC +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 the copyright holder 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 HOLDER 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 diff --git a/moveit_experimental/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp b/moveit_experimental/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp index 20ca24a382..033f39fd10 100644 --- a/moveit_experimental/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp +++ b/moveit_experimental/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp @@ -1,3 +1,41 @@ +/* + Title : collision_check_thread.cpp + Project : moveit_jog_arm + Created : 1/11/2019 + Author : Brian O'Neil, Andy Zelenak, Blake Anderson + +BSD 3-Clause License + +Copyright (c) 2019, Los Alamos National Security, LLC +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 the copyright holder 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 HOLDER 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 "geometry_msgs/TwistStamped.h" #include "control_msgs/JointJog.h" #include "ros/ros.h" From 09f49e44e5dfa31ded82b23906d82f61ad832c2a Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Mon, 25 Nov 2019 13:39:53 -0700 Subject: [PATCH 102/612] Added new CODEOWNERS for new folders and new core contributors (#1755) * Added new codeowners for new folders and new core contributors * Update CODEOWNERS --- .github/CODEOWNERS | 33 +++++++++++++++++---------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 3820b617c8..b8bf3f5483 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -37,24 +37,24 @@ /moveit_plugins/moveit_simple_controller_manager/ @mikeferguson @v4hn /moveit_plugins/moveit_controller_manager_example/ @v4hn -/moveit_core/background_processing/ @mlautman -/moveit_core/backtrace/ @mlautman -/moveit_core/collision_detection/ @BryceStevenWilley -/moveit_core/collision_detection_fcl/ @rhaschke -/moveit_core/collision_distance_field/ @rhaschke +/moveit_core/background_processing/ @tylerjw +/moveit_core/backtrace/ @tylerjw +/moveit_core/collision_detection/ @j-petit +/moveit_core/collision_detection_fcl/ @j-petit +/moveit_core/collision_distance_field/ @j-petit /moveit_core/constraint_samplers/ @v4hn /moveit_core/controller_manager/ @v4hn /moveit_core/distance_field/ @v4hn -/moveit_core/dynamics_solver/ @mlautman -/moveit_core/exceptions/ @mlautman +/moveit_core/dynamics_solver/ @tylerjw +/moveit_core/exceptions/ @tylerjw /moveit_core/kinematic_constraints/ @rhaschke /moveit_core/kinematics_base/ @rhaschke @mlautman /moveit_core/kinematics_metrics/ @gavanderhoorn -/moveit_core/macros/ @mlautman +/moveit_core/macros/ @tylerjw /moveit_core/planning_interface/ @rhaschke /moveit_core/planning_request_adapter/ @rhaschke /moveit_core/planning_scene/ @rhaschke -/moveit_core/profiler/ @mlautman +/moveit_core/profiler/ @tylerjw /moveit_core/robot_model/ @jonbinney /moveit_core/robot_state/ @rhaschke @mlautman /moveit_core/robot_trajectory/ @mlautman @@ -72,30 +72,31 @@ /moveit_experimental/ @AndyZe -/moveit_ros/perception/ @mikeferguson @jonbinney +/moveit_ros/perception/ @jliukkonen @RoboticsYY /moveit_ros/manipulation/ @v4hn @felixvd /moveit_ros/benchmarks/ @henningkayser @MohmadAyman /moveit_ros/planning_interface/ @mintar @rhaschke /moveit_ros/robot_interaction/ @mikeferguson @rhaschke /moveit_ros/warehouse/ @mikeferguson @dg-shadow /moveit_ros/move_group/ @rhaschke @IanTheEngineer -/moveit_ros/visualization/ @rhaschke @jonbinney @christian-rauch +/moveit_ros/visualization/ @rhaschke @jonbinney @RoboticsYY -/moveit_ros/planning/collision_plugin_loader/ @rhaschke -/moveit_ros/planning/constraint_sampler_manager_loader/ @henningkayser +/moveit_ros/planning/collision_plugin_loader/ @j-petit +/moveit_ros/planning/constraint_sampler_manager_loader/ @nbbrooks /moveit_ros/planning/kinematics_plugin_loader/ @gavanderhoorn /moveit_ros/planning/plan_execution/ @v4hn /moveit_ros/planning/planning_components_tools/ @henningkayser /moveit_ros/planning/planning_pipeline/ @v4hn /moveit_ros/planning/planning_request_adapter_plugins/ @v4hn /moveit_ros/planning/planning_scene_monitor/ @rhaschke -/moveit_ros/planning/rdf_loader/ @henningkayser -/moveit_ros/planning/robot_model_loader/ @henningkayser +/moveit_ros/planning/rdf_loader/ @nbbrooks +/moveit_ros/planning/robot_model_loader/ @nbbrooks /moveit_ros/planning/trajectory_execution_manager/ @rhaschke -/moveit_setup_assistant/ @davetcoleman @rhaschke @MohmadAyman +/moveit_setup_assistant/ @RoboticsYY @rhaschke @MohmadAyman /moveit_planners/ompl/ @BryceStevenWilley @mamoll /moveit_planners/chomp/chomp_interface/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/chomp/chomp_optimizer_adapter/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/chomp/chomp_motion_planner/ @raghavendersahdev @knorth55 @bmagyar +/moveit_planners/trajopt @ommmid @mamoll From f5b5ed601564ea1874163ccb920d2fa542493dda Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 11 Sep 2019 08:05:50 +0200 Subject: [PATCH 103/612] remove unused variables --- moveit_ros/robot_interaction/src/robot_interaction.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index ac11dfc48c..698b4ddd3b 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -448,7 +448,6 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle { // If scale is left at default size of 0, scale will be based on end effector link size. a good value is between 0-1 std::vector ims; - ros::NodeHandle nh; { boost::unique_lock ulock(marker_access_lock_); robot_state::RobotStateConstPtr s = handler->getState(); @@ -465,7 +464,6 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle im.scale); } } - ros::NodeHandle nh; for (std::size_t i = 0; i < active_eef_.size(); ++i) { @@ -554,7 +552,6 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle void RobotInteraction::registerMoveInteractiveMarkerTopic(const std::string& marker_name, const std::string& name) { - ros::NodeHandle nh; std::stringstream ss; ss << "/rviz/moveit/move_marker/"; ss << name; From 08b390db4fd7f5d37051f06c2143ef107fa9b7f4 Mon Sep 17 00:00:00 2001 From: Jens P Date: Tue, 26 Nov 2019 13:31:09 +0100 Subject: [PATCH 104/612] Cleanup #1657: move ASSERT() into test setup (#1777) --- moveit_core/planning_scene/test/test_multi_threaded.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/moveit_core/planning_scene/test/test_multi_threaded.cpp b/moveit_core/planning_scene/test/test_multi_threaded.cpp index fd3aa56a1a..eb9d1d1bb0 100644 --- a/moveit_core/planning_scene/test/test_multi_threaded.cpp +++ b/moveit_core/planning_scene/test/test_multi_threaded.cpp @@ -73,7 +73,8 @@ class CollisionDetectorThreadedTest : public testing::Test void SetUp() override { robot_model_ = moveit::core::loadTestingRobotModel("panda"); - robot_model_ok_ = static_cast(robot_model_); + ASSERT_TRUE(static_cast(robot_model_)); + robot_state_.reset(new robot_state::RobotState(robot_model_)); planning_scene_.reset(new planning_scene::PlanningScene(robot_model_)); } @@ -95,11 +96,6 @@ class CollisionDetectorThreadedTest : public testing::Test planning_scene::PlanningScenePtr planning_scene_; }; -TEST_F(CollisionDetectorThreadedTest, InitOk) -{ - ASSERT_TRUE(robot_model_ok_); -} - /** \brief Tests the FCL collision detector in multiple threads. */ TEST_F(CollisionDetectorThreadedTest, FCLThreaded) { From 5ac79ec48e4a040e95d0a5f1cda2fb20ebf01b47 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 26 Nov 2019 16:03:44 +0100 Subject: [PATCH 105/612] disable flaky moveit_cpp_test (#1782) --- moveit_ros/planning_interface/test/CMakeLists.txt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 7228694591..37ff35f2f1 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -5,8 +5,10 @@ if (CATKIN_ENABLE_TESTING) add_executable(test_cleanup test_cleanup.cpp) target_link_libraries(test_cleanup moveit_move_group_interface) - add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) - target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) + # TODO: Fix flaky test + #add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) + #target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) + add_rostest(python_move_group.test) add_rostest(python_move_group_ns.test) add_rostest(robot_state_update.test) From 8bdf9e57b5e8878f96c0457ab5f498cb06a5d883 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 25 Nov 2019 15:44:11 +0100 Subject: [PATCH 106/612] fix unused parameter warnings --- .../collision_env_distance_field.h | 25 +++++++++++-------- .../moveit/distance_field/voxel_grid.h | 2 +- .../moveit/kinematics_base/kinematics_base.h | 3 ++- .../include/moveit/robot_state/robot_state.h | 23 +++++++++-------- .../moveit_jog_arm/src/jog_calcs.cpp | 2 +- .../chainiksolver_vel_mimic_svd.hpp | 2 +- .../src/srv_kinematics_plugin.cpp | 8 +++--- .../test/test_kinematics_plugin.cpp | 6 ++--- .../chomp_interface/src/chomp_plugin.cpp | 4 +-- .../chomp_motion_planner/src/chomp_cost.cpp | 4 +-- .../src/chomp_optimizer_adapter.cpp | 2 +- .../src/detail/constraints_library.cpp | 2 +- .../src/model_based_planning_context.cpp | 2 +- .../src/ompl_planner_manager.cpp | 2 +- .../joint_model_state_space_factory.cpp | 4 +-- .../src/planning_context_manager.cpp | 2 +- .../src/moveit_fake_controller_manager.cpp | 5 ++-- .../joint_trajectory_controller_plugin.cpp | 2 +- .../action_based_controller_handle.h | 2 +- .../gripper_controller_handle.h | 4 +-- ...low_joint_trajectory_controller_handle.cpp | 2 +- .../src/moveit_simple_controller_manager.cpp | 5 ++-- .../benchmarks/src/BenchmarkExecutor.cpp | 4 +-- .../pick_place/src/pick_place_params.cpp | 2 +- .../clear_octomap_service_capability.cpp | 3 ++- .../query_planners_service_capability.cpp | 4 +-- .../move_group/src/list_capabilities.cpp | 2 +- .../mesh_filter/src/sensor_model.cpp | 2 ++ .../src/shape_mask.cpp | 2 +- .../src/pointcloud_octomap_updater.cpp | 4 +-- .../kinematics_plugin_loader.h | 2 +- .../plan_execution/src/plan_execution.cpp | 4 +-- .../plan_execution/src/plan_with_sensing.cpp | 2 +- .../add_iterative_spline_parameterization.cpp | 4 +-- .../src/add_time_optimal_parameterization.cpp | 4 +-- .../src/add_time_parameterization.cpp | 4 +-- .../src/empty.cpp | 4 +-- .../src/fix_start_state_path_constraints.cpp | 2 +- .../src/fix_workspace_bounds.cpp | 2 +- .../src/resolve_constraint_frames.cpp | 4 +-- .../src/planning_scene_monitor.cpp | 4 +-- .../src/trajectory_execution_manager.cpp | 2 +- .../test/test_moveit_controller_manager.h | 3 ++- .../moveit_cpp/src/moveit_cpp.cpp | 2 +- .../src/interaction_handler.cpp | 2 +- .../src/motion_planning_frame.cpp | 2 +- .../motion_planning_frame_manipulation.cpp | 6 ++--- .../src/motion_planning_frame_planning.cpp | 10 ++++---- .../src/planning_scene_display.cpp | 4 +-- .../src/octomap_render.cpp | 2 +- .../src/trajectory_visualization.cpp | 2 +- .../warehouse/src/warehouse_services.cpp | 5 ++-- .../src/setup_assistant_main.cpp | 2 +- .../src/tools/collision_linear_model.cpp | 8 +++--- .../src/tools/moveit_config_data.cpp | 2 +- .../src/widgets/double_list_widget.cpp | 4 +-- .../src/widgets/end_effectors_widget.cpp | 4 +-- .../src/widgets/navigation_widget.cpp | 2 +- .../src/widgets/robot_poses_widget.cpp | 4 +-- .../src/widgets/ros_controllers_widget.cpp | 2 +- .../src/widgets/setup_assistant_widget.cpp | 2 +- .../src/widgets/simulation_widget.cpp | 2 +- .../src/widgets/start_screen_widget.cpp | 2 +- .../src/widgets/virtual_joints_widget.cpp | 4 +-- 64 files changed, 132 insertions(+), 118 deletions(-) diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h index 93ec59597b..2a5e4a62b6 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h @@ -105,19 +105,19 @@ class CollisionEnvDistanceField : public CollisionEnv void createCollisionModelMarker(const moveit::core::RobotState& state, visualization_msgs::MarkerArray& model_markers) const; - virtual double distanceSelf(const moveit::core::RobotState& state) const + virtual double distanceSelf(const moveit::core::RobotState& /* state */) const { return 0.0; - }; + } - virtual double distanceSelf(const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm) const + virtual double distanceSelf(const moveit::core::RobotState& /* state */, + const collision_detection::AllowedCollisionMatrix& /* acm */) const { return 0.0; - }; + } - void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override + void distanceSelf(const DistanceRequest& /* req */, DistanceResult& /* res */, + const robot_state::RobotState& /* state */) const override { ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); } @@ -179,17 +179,22 @@ class CollisionEnvDistanceField : public CollisionEnv virtual double distanceRobot(const robot_state::RobotState& state, bool verbose = false) const { + (void)state; + (void)verbose; return 0.0; } virtual double distanceRobot(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, bool verbose = false) const { + (void)state; + (void)acm; + (void)verbose; return 0.0; } - void distanceRobot(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override + void distanceRobot(const DistanceRequest& /* req */, DistanceResult& /* res */, + const robot_state::RobotState& /* state */) const override { ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); } @@ -263,7 +268,7 @@ class CollisionEnvDistanceField : public CollisionEnv bool compareCacheEntryToAllowedCollisionMatrix(const DistanceFieldCacheEntryConstPtr& dfce, const collision_detection::AllowedCollisionMatrix& acm) const; - void updatedPaddingOrScaling(const std::vector& links) override{}; + void updatedPaddingOrScaling(const std::vector& /*links*/) override{}; DistanceFieldCacheEntryWorldPtr generateDistanceFieldCacheEntryWorld(); diff --git a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h index 6cecf4e050..82a296cabf 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h +++ b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h @@ -441,7 +441,7 @@ inline double VoxelGrid::getResolution() const } template -inline double VoxelGrid::getResolution(Dimension dim) const +inline double VoxelGrid::getResolution(Dimension /*dim*/) const { return resolution_; } diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index b4d41e06ce..a3dae858a2 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -297,8 +297,9 @@ class KinematicsBase 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 + const moveit::core::RobotState* context_state = nullptr) const { + (void)context_state; // For IK solvers that do not support multiple poses, fall back to single pose call if (ik_poses.size() == 1) { diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index f16c031abd..43789a908d 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -915,8 +915,8 @@ as the new values that correspond to the group */ const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int attempts, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int /* attempts */, + double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { return setFromIK(group, pose, timeout, constraint, options); @@ -934,7 +934,7 @@ as the new values that correspond to the group */ const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip, - unsigned int attempts, double timeout = 0.0, + unsigned int /* attempts */, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { @@ -951,8 +951,8 @@ as the new values that correspond to the group */ const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int attempts, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int /* attempts */, + double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { return setFromIK(group, pose, timeout, constraint, options); @@ -968,8 +968,9 @@ as the new values that correspond to the group */ double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, unsigned int attempts, - double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, + unsigned int /* attempts */, double timeout = 0.0, + const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { return setFromIK(group, pose, tip, timeout, constraint, options); @@ -989,7 +990,7 @@ as the new values that correspond to the group */ const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, - const std::vector& consistency_limits, unsigned int attempts, double timeout = 0.0, + const std::vector& consistency_limits, unsigned int /* attempts */, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { @@ -1011,7 +1012,7 @@ as the new values that correspond to the group */ const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, unsigned int attempts, double timeout = 0.0, + const std::vector& tips, unsigned int /* attempts */, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { @@ -1035,7 +1036,7 @@ as the new values that correspond to the group */ [[deprecated("The attempts argument is not supported anymore.")]] bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, const std::vector& tips, const std::vector >& consistency_limits, - unsigned int attempts, double timeout = 0.0, + unsigned int /* attempts */, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { @@ -1058,7 +1059,7 @@ as the new values that correspond to the group */ [[deprecated("The attempts argument is not supported anymore.")]] bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, const std::vector& tips, const std::vector >& consistency_limits, - unsigned int attempts, double timeout = 0.0, + unsigned int /* attempts */, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index e89087f1a1..562c11dcc1 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -335,7 +335,7 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& return true; } -bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables) +bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& /*shared_variables*/) { // Check for nan's or |delta|>1 in the incoming command for (double velocity : cmd.velocities) diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp index afefc41f3c..34292a9262 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp @@ -87,7 +87,7 @@ class ChainIkSolverVelMimicSVD : public ChainIkSolverVel const Eigen::Matrix& cartesian_weights); /// not implemented. - int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out) override + int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/) override { return -1; } diff --git a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp index 0f33d7af73..b8f98f914f 100644 --- a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp +++ b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp @@ -233,11 +233,11 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c } bool SrvKinematicsPlugin::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, + 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) const + const kinematics::KinematicsQueryOptions& /*options*/) const { // Check if active if (!active_) diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index 1235bfd9f9..2554516f5a 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -203,7 +203,7 @@ class KinematicsTest : public ::testing::Test } public: - testing::AssertionResult isNear(const char* expr1, const char* expr2, const char* abs_error_expr, + testing::AssertionResult isNear(const char* expr1, const char* expr2, const char* /*abs_error_expr*/, const geometry_msgs::Point& val1, const geometry_msgs::Point& val2, double abs_error) { // clang-format off @@ -218,7 +218,7 @@ class KinematicsTest : public ::testing::Test << "Actual: " << expr2 << " [" << val2.x << ", " << val2.y << ", " << val2.z << "]"; // clang-format on } - testing::AssertionResult isNear(const char* expr1, const char* expr2, const char* abs_error_expr, + testing::AssertionResult isNear(const char* expr1, const char* expr2, const char* /*abs_error_expr*/, const geometry_msgs::Quaternion& val1, const geometry_msgs::Quaternion& val2, double abs_error) { @@ -261,7 +261,7 @@ class KinematicsTest : public ::testing::Test return testing::AssertionSuccess(); } - void searchIKCallback(const geometry_msgs::Pose& ik_pose, const std::vector& joint_state, + void searchIKCallback(const geometry_msgs::Pose& /*ik_pose*/, const std::vector& joint_state, moveit_msgs::MoveItErrorCodes& error_code) { std::vector link_names = { tip_link_ }; diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp index 92697969b6..6a8288b78e 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp @@ -50,7 +50,7 @@ class CHOMPPlannerManager : public planning_interface::PlannerManager { } - bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns) override + bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& /*ns*/) override { for (const std::string& group : model->getJointModelGroupNames()) { @@ -92,7 +92,7 @@ class CHOMPPlannerManager : public planning_interface::PlannerManager return context; } - bool canServiceRequest(const planning_interface::MotionPlanRequest& req) const override + bool canServiceRequest(const planning_interface::MotionPlanRequest& /*req*/) const override { // TODO: this is a dummy implementation // capabilities.dummy = false; diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_cost.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_cost.cpp index 3543ba073a..9a8ef78bb3 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_cost.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_cost.cpp @@ -43,8 +43,8 @@ using namespace std; namespace chomp { -ChompCost::ChompCost(const ChompTrajectory& trajectory, int joint_number, const std::vector& derivative_costs, - double ridge_factor) +ChompCost::ChompCost(const ChompTrajectory& trajectory, int /* joint_number */, + const std::vector& derivative_costs, double ridge_factor) { int num_vars_all = trajectory.getNumPoints(); int num_vars_free = num_vars_all - 2 * (DIFF_RULE_LENGTH - 1); diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp index ec025a3114..f57e080bf2 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp +++ b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp @@ -166,7 +166,7 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { // following call to planner() calls the OMPL planner and stores the trajectory inside the MotionPlanResponse res // variable which is then used by CHOMP for optimization of the computed trajectory diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index 7e77582313..8e99b06dac 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -219,7 +219,7 @@ ompl_interface::ConstraintApproximation::ConstraintApproximation( } ompl::base::StateSamplerAllocator -ompl_interface::ConstraintApproximation::getStateSamplerAllocator(const moveit_msgs::Constraints& msg) const +ompl_interface::ConstraintApproximation::getStateSamplerAllocator(const moveit_msgs::Constraints& /*unused*/) const { if (state_storage_->size() == 0) return ompl::base::StateSamplerAllocator(); diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 4d05cedd2d..10534e1d06 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -544,7 +544,7 @@ void ompl_interface::ModelBasedPlanningContext::clear() } bool ompl_interface::ModelBasedPlanningContext::setPathConstraints(const moveit_msgs::Constraints& path_constraints, - moveit_msgs::MoveItErrorCodes* error) + moveit_msgs::MoveItErrorCodes* /*error*/) { // ******************* set the path constraints to use path_constraints_.reset(new kinematic_constraints::KinematicConstraintSet(getRobotModel())); diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index e111784818..2c8c67fe40 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -259,7 +259,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager } */ - void dynamicReconfigureCallback(OMPLDynamicReconfigureConfig& config, uint32_t level) + void dynamicReconfigureCallback(OMPLDynamicReconfigureConfig& config, uint32_t /*level*/) { if (config.link_for_exploration_tree.empty() && !planner_data_link_name_.empty()) { diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp index cbaa560146..6943b91b91 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp @@ -43,8 +43,8 @@ ompl_interface::JointModelStateSpaceFactory::JointModelStateSpaceFactory() : Mod } int ompl_interface::JointModelStateSpaceFactory::canRepresentProblem( - const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const robot_model::RobotModelConstPtr& robot_model) const + const std::string& /*group*/, const moveit_msgs::MotionPlanRequest& /*req*/, + const robot_model::RobotModelConstPtr& /*robot_model*/) const { return 100; } diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index b7665991b6..3d6c6b21e3 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -180,7 +180,7 @@ void ompl_interface::PlanningContextManager::setPlannerConfigurations( ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext( const planning_interface::PlannerConfigurationSettings& config, - const StateSpaceFactoryTypeSelector& factory_selector, const moveit_msgs::MotionPlanRequest& req) const + const StateSpaceFactoryTypeSelector& factory_selector, const moveit_msgs::MotionPlanRequest& /*req*/) const { const ompl_interface::ModelBasedStateSpaceFactoryPtr& factory = factory_selector(config.group); diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp index baa51f4f16..ee375ead3a 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp @@ -261,7 +261,7 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont * Controllers are all active and default. */ moveit_controller_manager::MoveItControllerManager::ControllerState - getControllerState(const std::string& name) override + getControllerState(const std::string& /*name*/) override { moveit_controller_manager::MoveItControllerManager::ControllerState state; state.active_ = true; @@ -270,7 +270,8 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont } /* Cannot switch our controllers */ - bool switchControllers(const std::vector& activate, const std::vector& deactivate) override + bool switchControllers(const std::vector& /*activate*/, + const std::vector& /*deactivate*/) override { return false; } diff --git a/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp index fbe5c1c224..9f8a048aec 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp @@ -49,7 +49,7 @@ class JointTrajectoryControllerAllocator : public ControllerHandleAllocator { public: moveit_controller_manager::MoveItControllerHandlePtr alloc(const std::string& name, - const std::vector& resources) override + const std::vector& /* resources */) override { return std::make_shared( name, "follow_joint_trajectory"); diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h index f3a5b8d059..adcc2efce0 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h @@ -56,7 +56,7 @@ class ActionBasedControllerHandleBase : public moveit_controller_manager::MoveIt virtual void addJoint(const std::string& name) = 0; virtual void getJoints(std::vector& joints) = 0; - virtual void configure(XmlRpc::XmlRpcValue& config) + virtual void configure(XmlRpc::XmlRpcValue& /* config */) { } }; diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h index f081853850..c52b53fcca 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h @@ -170,7 +170,7 @@ class GripperControllerHandle : public ActionBasedControllerHandle& activate, const std::vector& deactivate) override + bool switchControllers(const std::vector& /* activate */, + const std::vector& /* deactivate */) override { return false; } diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 1f55bf7102..51d8cd07e5 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -241,8 +241,8 @@ bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& opts) return false; } -bool BenchmarkExecutor::queriesAndPlannersCompatible(const std::vector& requests, - const std::map>& planners) +bool BenchmarkExecutor::queriesAndPlannersCompatible( + const std::vector& requests, const std::map>& /*planners*/) { // Make sure that the planner interfaces can service the desired queries for (const std::pair& pipeline_entry : planning_pipelines_) diff --git a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp index 250c42b621..addc270d44 100644 --- a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp @@ -61,7 +61,7 @@ class DynamicReconfigureImpl private: PickPlaceParams params_; - void dynamicReconfigureCallback(PickPlaceDynamicReconfigureConfig& config, uint32_t level) + void dynamicReconfigureCallback(PickPlaceDynamicReconfigureConfig& config, uint32_t /*level*/) { params_.max_goal_count_ = config.max_attempted_states_per_pose; params_.max_fail_ = config.max_consecutive_fail_attempts; diff --git a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp index 48aa594454..e2c8d1a814 100644 --- a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp @@ -46,7 +46,8 @@ void move_group::ClearOctomapService::initialize() service_ = root_node_handle_.advertiseService(CLEAR_OCTOMAP_SERVICE_NAME, &ClearOctomapService::clearOctomap, this); } -bool move_group::ClearOctomapService::clearOctomap(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) +bool move_group::ClearOctomapService::clearOctomap(std_srvs::Empty::Request& /*unused*/, + std_srvs::Empty::Response& /*unused*/) { if (!context_->planning_scene_monitor_) { diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp index 9afab4f90e..806b65d395 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp @@ -55,7 +55,7 @@ void MoveGroupQueryPlannersService::initialize() &MoveGroupQueryPlannersService::setParams, this); } -bool MoveGroupQueryPlannersService::queryInterface(moveit_msgs::QueryPlannerInterfaces::Request& req, +bool MoveGroupQueryPlannersService::queryInterface(moveit_msgs::QueryPlannerInterfaces::Request& /*req*/, moveit_msgs::QueryPlannerInterfaces::Response& res) { const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager(); @@ -103,7 +103,7 @@ bool MoveGroupQueryPlannersService::getParams(moveit_msgs::GetPlannerParams::Req } bool MoveGroupQueryPlannersService::setParams(moveit_msgs::SetPlannerParams::Request& req, - moveit_msgs::SetPlannerParams::Response& res) + moveit_msgs::SetPlannerParams::Response& /*res*/) { const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager(); if (req.params.keys.size() != req.params.values.size()) diff --git a/moveit_ros/move_group/src/list_capabilities.cpp b/moveit_ros/move_group/src/list_capabilities.cpp index f1a972b1f1..9001ad49c2 100644 --- a/moveit_ros/move_group/src/list_capabilities.cpp +++ b/moveit_ros/move_group/src/list_capabilities.cpp @@ -38,7 +38,7 @@ #include #include -int main(int argc, char** argv) +int main(int /*argc*/, char** /*argv*/) { try { diff --git a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp index d09cb649d9..96ed1ae78c 100644 --- a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp +++ b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp @@ -91,6 +91,7 @@ float mesh_filter::SensorModel::Parameters::getFarClippingPlaneDistance() const namespace { +#if HAVE_SSE_EXTENSIONS inline unsigned alignment16(const void* pointer) { return ((uintptr_t)pointer & 15); @@ -99,6 +100,7 @@ inline bool isAligned16(const void* pointer) { return (((uintptr_t)pointer & 15) == 0); } +#endif } // namespace void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(float* depth) const diff --git a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp index 7c6c33f07f..6013d0ffc5 100644 --- a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp +++ b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp @@ -111,7 +111,7 @@ void point_containment_filter::ShapeMask::removeShape(ShapeHandle handle) } void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::PointCloud2& data_in, - const Eigen::Vector3d& sensor_origin, + const Eigen::Vector3d& /*sensor_origin*/, const double min_sensor_dist, const double max_sensor_dist, std::vector& mask) { diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index af47bd5b9a..1fd300ed75 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -162,8 +162,8 @@ bool PointCloudOctomapUpdater::getShapeTransform(ShapeHandle h, Eigen::Isometry3 return it != transform_cache_.end(); } -void PointCloudOctomapUpdater::updateMask(const sensor_msgs::PointCloud2& cloud, const Eigen::Vector3d& sensor_origin, - std::vector& mask) +void PointCloudOctomapUpdater::updateMask(const sensor_msgs::PointCloud2& /*cloud*/, + const Eigen::Vector3d& /*sensor_origin*/, std::vector& /*mask*/) { } diff --git a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h index f4c6b0a356..07c6545208 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h +++ b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h @@ -67,7 +67,7 @@ class KinematicsPluginLoader parameter under which the robot description can be found. This is passed to the kinematics solver initialization as well as used to read the SRDF document when needed. */ - KinematicsPluginLoader(const std::string& solver_plugin, double solve_timeout, unsigned int ik_attempts, + KinematicsPluginLoader(const std::string& solver_plugin, double solve_timeout, unsigned int /*ik_attempts*/, const std::string& robot_description = "robot_description", double default_search_resolution = 0.0) : robot_description_(robot_description) diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 1a4ecf0d1a..f669e03af8 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -59,7 +59,7 @@ class PlanExecution::DynamicReconfigureImpl } private: - void dynamicReconfigureCallback(PlanExecutionDynamicReconfigureConfig& config, uint32_t level) + void dynamicReconfigureCallback(PlanExecutionDynamicReconfigureConfig& config, uint32_t /*level*/) { owner_->setMaxReplanAttempts(config.max_replan_attempts); owner_->setTrajectoryStateRecordingFrequency(config.record_trajectory_state_frequency); @@ -479,7 +479,7 @@ void plan_execution::PlanExecution::planningSceneUpdatedCallback( } void plan_execution::PlanExecution::doneWithTrajectoryExecution( - const moveit_controller_manager::ExecutionStatus& status) + const moveit_controller_manager::ExecutionStatus& /*status*/) { execution_complete_ = true; } diff --git a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp index 8095e07b78..0078ad5578 100644 --- a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp @@ -57,7 +57,7 @@ class PlanWithSensing::DynamicReconfigureImpl } private: - void dynamicReconfigureCallback(SenseForPlanDynamicReconfigureConfig& config, uint32_t level) + void dynamicReconfigureCallback(SenseForPlanDynamicReconfigureConfig& config, uint32_t /*level*/) { owner_->setMaxSafePathCost(config.max_safe_path_cost); owner_->setMaxCostSources(config.max_cost_sources); diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp index bf4f89a2b8..716890f341 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp @@ -49,7 +49,7 @@ class AddIterativeSplineParameterization : public planning_request_adapter::Plan { } - void initialize(const ros::NodeHandle& nh) override + void initialize(const ros::NodeHandle& /*nh*/) override { } @@ -60,7 +60,7 @@ class AddIterativeSplineParameterization : public planning_request_adapter::Plan bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { bool result = planner(planning_scene, req, res); if (result && res.trajectory_) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp index 336307fedb..c72f20b854 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -51,7 +51,7 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning { } - void initialize(const ros::NodeHandle& nh) override + void initialize(const ros::NodeHandle& /*nh*/) override { } @@ -62,7 +62,7 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { bool result = planner(planning_scene, req, res); if (result && res.trajectory_) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp index e3d1b17522..dced1a8d87 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp @@ -48,7 +48,7 @@ class AddTimeParameterization : public planning_request_adapter::PlanningRequest { } - void initialize(const ros::NodeHandle& nh) override + void initialize(const ros::NodeHandle& /*nh*/) override { } @@ -59,7 +59,7 @@ class AddTimeParameterization : public planning_request_adapter::PlanningRequest bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { bool result = planner(planning_scene, req, res); if (result && res.trajectory_) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp index 92a48ca821..7bb0555e76 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp @@ -49,12 +49,12 @@ class Empty : public planning_request_adapter::PlanningRequestAdapter bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { return planner(planning_scene, req, res); } - void initialize(const ros::NodeHandle& node_handle) override + void initialize(const ros::NodeHandle& /*nh*/) override { } }; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp index 3d5f5b3135..80a6f058ce 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp @@ -50,7 +50,7 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe { } - void initialize(const ros::NodeHandle& nh) override + void initialize(const ros::NodeHandle& /*nh*/) override { } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp index fc36ca06f7..ea7586c583 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp @@ -68,7 +68,7 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { ROS_DEBUG("Running '%s'", getDescription().c_str()); const moveit_msgs::WorkspaceParameters& wparams = req.workspace_parameters; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index a8682a5ce6..0c5cdb3dd2 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -48,7 +48,7 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest { } - void initialize(const ros::NodeHandle& nh) override + void initialize(const ros::NodeHandle& /*nh*/) override { } @@ -59,7 +59,7 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { ROS_DEBUG("Running '%s'", getDescription().c_str()); planning_interface::MotionPlanRequest modified = req; diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index de9a628b5a..21d4ed1e43 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -84,7 +84,7 @@ class PlanningSceneMonitor::DynamicReconfigureImpl return ns; } - void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig& config, uint32_t level) + void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig& config, uint32_t /*level*/) { PlanningSceneMonitor::SceneUpdateType event = PlanningSceneMonitor::UPDATE_NONE; if (config.publish_geometry_updates) @@ -1143,7 +1143,7 @@ void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr& updateSceneWithCurrentState(); } -void PlanningSceneMonitor::stateUpdateTimerCallback(const ros::WallTimerEvent& event) +void PlanningSceneMonitor::stateUpdateTimerCallback(const ros::WallTimerEvent& /*unused*/) { if (state_update_pending_) { diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 7ab6d1ca73..71d180cefe 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -64,7 +64,7 @@ class TrajectoryExecutionManager::DynamicReconfigureImpl } private: - void dynamicReconfigureCallback(TrajectoryExecutionDynamicReconfigureConfig& config, uint32_t level) + void dynamicReconfigureCallback(TrajectoryExecutionDynamicReconfigureConfig& config, uint32_t /*level*/) { owner_->enableExecutionDurationMonitoring(config.execution_duration_monitoring); owner_->setAllowedExecutionDurationScaling(config.allowed_execution_duration_scaling); diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h index 46584f00cd..732457e801 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h @@ -47,7 +47,7 @@ class TestMoveItControllerHandle : public moveit_controller_manager::MoveItContr { } - bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory) override + bool sendTrajectory(const moveit_msgs::RobotTrajectory& /*trajectory*/) override { return true; } @@ -59,6 +59,7 @@ class TestMoveItControllerHandle : public moveit_controller_manager::MoveItContr bool waitForExecution(const ros::Duration& timeout = ros::Duration(0)) override { + (void)timeout; return false; } diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index c3dd2ee9f9..8d91554da2 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -57,7 +57,7 @@ MoveItCpp::MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) { if (!tf_buffer_) diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index f437f20c71..3c8492fda8 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -330,7 +330,7 @@ bool InteractionHandler::inError(const GenericInteraction& g) const return getErrorState(g.marker_name_suffix); } -bool InteractionHandler::inError(const JointInteraction& vj) const +bool InteractionHandler::inError(const JointInteraction& /*unused*/) const { return false; } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 458354b8dd..970060a362 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -513,7 +513,7 @@ void MotionPlanningFrame::tabChanged(int index) selectedCollisionObjectChanged(); } -void MotionPlanningFrame::updateSceneMarkers(float wall_dt, float ros_dt) +void MotionPlanningFrame::updateSceneMarkers(float wall_dt, float /*ros_dt*/) { if (scene_marker_) scene_marker_->update(wall_dt); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp index aea72abcf2..4159cc4ff0 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp @@ -113,7 +113,7 @@ void MotionPlanningFrame::selectedDetectedObjectChanged() } } -void MotionPlanningFrame::detectedObjectChanged(QListWidgetItem* item) +void MotionPlanningFrame::detectedObjectChanged(QListWidgetItem* /*item*/) { } @@ -147,14 +147,14 @@ void MotionPlanningFrame::triggerObjectDetection() } } -void MotionPlanningFrame::listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& msg) +void MotionPlanningFrame::listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& /*msg*/) { ros::Duration(1.0).sleep(); planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::processDetectedObjects, this)); } void MotionPlanningFrame::updateDetectedObjectsList(const std::vector& object_ids, - const std::vector& objects) + const std::vector& /*objects*/) { ui_->detected_objects_list->setUpdatesEnabled(false); bool old_state = ui_->detected_objects_list->blockSignals(true); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 88417e1033..55006806df 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -492,22 +492,22 @@ void MotionPlanningFrame::configureForPlanning() planning_display_->dropVisualizedTrajectory(); } -void MotionPlanningFrame::remotePlanCallback(const std_msgs::EmptyConstPtr& msg) +void MotionPlanningFrame::remotePlanCallback(const std_msgs::EmptyConstPtr& /*msg*/) { planButtonClicked(); } -void MotionPlanningFrame::remoteExecuteCallback(const std_msgs::EmptyConstPtr& msg) +void MotionPlanningFrame::remoteExecuteCallback(const std_msgs::EmptyConstPtr& /*msg*/) { executeButtonClicked(); } -void MotionPlanningFrame::remoteStopCallback(const std_msgs::EmptyConstPtr& msg) +void MotionPlanningFrame::remoteStopCallback(const std_msgs::EmptyConstPtr& /*msg*/) { stopButtonClicked(); } -void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr& msg) +void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr& /*msg*/) { if (move_group_ && planning_display_) { @@ -521,7 +521,7 @@ void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::EmptyCo } } -void MotionPlanningFrame::remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr& msg) +void MotionPlanningFrame::remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr& /*msg*/) { if (move_group_ && planning_display_) { diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 5a08240a98..f3cdfda14c 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -561,7 +561,7 @@ void PlanningSceneDisplay::sceneMonitorReceivedUpdate( } void PlanningSceneDisplay::onSceneMonitorReceivedUpdate( - planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) + planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType /*update_type*/) { bool old_state = scene_name_property_->blockSignals(true); getPlanningSceneRW()->getCurrentStateNonConst().update(); @@ -621,7 +621,7 @@ void PlanningSceneDisplay::update(float wall_dt, float ros_dt) updateInternal(wall_dt, ros_dt); } -void PlanningSceneDisplay::updateInternal(float wall_dt, float ros_dt) +void PlanningSceneDisplay::updateInternal(float wall_dt, float /*ros_dt*/) { current_scene_time_ += wall_dt; if (current_scene_time_ > scene_display_time_property_->getFloat() && planning_scene_render_ && diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp index f427a7315b..dbe2f667ae 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp @@ -51,7 +51,7 @@ typedef std::vector VVPoint; OcTreeRender::OcTreeRender(const std::shared_ptr& octree, OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode, - std::size_t max_octree_depth, Ogre::SceneManager* scene_manager, + std::size_t max_octree_depth, Ogre::SceneManager* /*scene_manager*/, Ogre::SceneNode* parent_node = nullptr) : octree_(octree), colorFactor_(0.8) { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 39cbd4e569..3d5b4c2acc 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -357,7 +357,7 @@ void TrajectoryVisualization::dropTrajectory() drop_displaying_trajectory_ = true; } -void TrajectoryVisualization::update(float wall_dt, float ros_dt) +void TrajectoryVisualization::update(float wall_dt, float /*ros_dt*/) { if (drop_displaying_trajectory_) { diff --git a/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp b/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp index 97e89d29d7..cc1614f668 100644 --- a/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp +++ b/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp @@ -96,7 +96,8 @@ bool getState(moveit_msgs::GetRobotStateFromWarehouse::Request& request, } bool renameState(moveit_msgs::RenameRobotStateInWarehouse::Request& request, - moveit_msgs::RenameRobotStateInWarehouse::Response& response, moveit_warehouse::RobotStateStorage* rs) + moveit_msgs::RenameRobotStateInWarehouse::Response& /*response*/, + moveit_warehouse::RobotStateStorage* rs) { if (!rs->hasRobotState(request.old_name, request.robot)) { @@ -108,7 +109,7 @@ bool renameState(moveit_msgs::RenameRobotStateInWarehouse::Request& request, } bool deleteState(moveit_msgs::DeleteRobotStateFromWarehouse::Request& request, - moveit_msgs::DeleteRobotStateFromWarehouse::Response& response, + moveit_msgs::DeleteRobotStateFromWarehouse::Response& /*response*/, moveit_warehouse::RobotStateStorage* rs) { if (!rs->hasRobotState(request.name, request.robot)) diff --git a/moveit_setup_assistant/src/setup_assistant_main.cpp b/moveit_setup_assistant/src/setup_assistant_main.cpp index a514696622..88577607a5 100644 --- a/moveit_setup_assistant/src/setup_assistant_main.cpp +++ b/moveit_setup_assistant/src/setup_assistant_main.cpp @@ -42,7 +42,7 @@ #include #include -static void siginthandler(int param) +static void siginthandler(int /*param*/) { QApplication::quit(); } diff --git a/moveit_setup_assistant/src/tools/collision_linear_model.cpp b/moveit_setup_assistant/src/tools/collision_linear_model.cpp index ea737dc63a..66fd858872 100644 --- a/moveit_setup_assistant/src/tools/collision_linear_model.cpp +++ b/moveit_setup_assistant/src/tools/collision_linear_model.cpp @@ -78,23 +78,23 @@ QModelIndex CollisionLinearModel::mapToSource(const QModelIndex& proxyIndex) con return sourceModel()->index(r, c); } -int CollisionLinearModel::rowCount(const QModelIndex& parent) const +int CollisionLinearModel::rowCount(const QModelIndex& /*parent*/) const { int n = this->sourceModel()->rowCount(); return (n * (n - 1) / 2); } -int CollisionLinearModel::columnCount(const QModelIndex& parent) const +int CollisionLinearModel::columnCount(const QModelIndex& /*parent*/) const { return 4; } -QModelIndex CollisionLinearModel::index(int row, int column, const QModelIndex& parent) const +QModelIndex CollisionLinearModel::index(int row, int column, const QModelIndex& /*parent*/) const { return createIndex(row, column); } -QModelIndex CollisionLinearModel::parent(const QModelIndex& child) const +QModelIndex CollisionLinearModel::parent(const QModelIndex& /*child*/) const { return QModelIndex(); } diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index da6ce39b47..d925af3807 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -1827,7 +1827,7 @@ std::vector& MoveItConfigData::getROSControllers() // Used to add a sensor plugin configuation parameter to the sensor plugin configuration parameter list // ****************************************************************************************** void MoveItConfigData::addGenericParameterToSensorPluginConfig(const std::string& name, const std::string& value, - const std::string& comment) + const std::string& /*comment*/) { // Use index 0 since we only write one plugin GenericParameter new_parameter; diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.cpp b/moveit_setup_assistant/src/widgets/double_list_widget.cpp index f3769bfa70..ede1601910 100644 --- a/moveit_setup_assistant/src/widgets/double_list_widget.cpp +++ b/moveit_setup_assistant/src/widgets/double_list_widget.cpp @@ -299,7 +299,7 @@ void DoubleListWidget::deselectDataButtonClicked() // ****************************************************************************************** // Highlight links of robot for left list // ****************************************************************************************** -void DoubleListWidget::previewSelectedLeft(const QItemSelection& selected, const QItemSelection& deselected) +void DoubleListWidget::previewSelectedLeft(const QItemSelection& /*selected*/, const QItemSelection& /*deselected*/) { const QList selected_items = data_table_->selectedItems(); previewSelected(selected_items); @@ -308,7 +308,7 @@ void DoubleListWidget::previewSelectedLeft(const QItemSelection& selected, const // ****************************************************************************************** // Highlight links of robot for right list // ****************************************************************************************** -void DoubleListWidget::previewSelectedRight(const QItemSelection& selected, const QItemSelection& deselected) +void DoubleListWidget::previewSelectedRight(const QItemSelection& /*selected*/, const QItemSelection& /*deselected*/) { const QList selected_items = selected_data_table_->selectedItems(); previewSelected(selected_items); diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp index 4c3889108b..a0d53ee39f 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp @@ -247,7 +247,7 @@ void EndEffectorsWidget::showNewScreen() // ****************************************************************************************** // Edit whatever element is selected // ****************************************************************************************** -void EndEffectorsWidget::editDoubleClicked(int row, int column) +void EndEffectorsWidget::editDoubleClicked(int /*row*/, int /*column*/) { editSelected(); } @@ -255,7 +255,7 @@ void EndEffectorsWidget::editDoubleClicked(int row, int column) // ****************************************************************************************** // Preview whatever element is selected // ****************************************************************************************** -void EndEffectorsWidget::previewClicked(int row, int column) +void EndEffectorsWidget::previewClicked(int /*row*/, int /*column*/) { // Get list of all selected items QList selected = data_table_->selectedItems(); diff --git a/moveit_setup_assistant/src/widgets/navigation_widget.cpp b/moveit_setup_assistant/src/widgets/navigation_widget.cpp index 703e1936b1..3d62d771f0 100644 --- a/moveit_setup_assistant/src/widgets/navigation_widget.cpp +++ b/moveit_setup_assistant/src/widgets/navigation_widget.cpp @@ -116,7 +116,7 @@ NavDelegate::NavDelegate(QObject* parent) : QStyledItemDelegate(parent) { } -QSize NavDelegate::sizeHint(const QStyleOptionViewItem& option, const QModelIndex& index) const +QSize NavDelegate::sizeHint(const QStyleOptionViewItem& option, const QModelIndex& /*index*/) const { return QSize(option.rect.width(), 45); } diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index 6d8c9ab840..c83e5e9a45 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -308,7 +308,7 @@ void RobotPosesWidget::showNewScreen() // ****************************************************************************************** // Edit whatever element is selected // ****************************************************************************************** -void RobotPosesWidget::editDoubleClicked(int row, int column) +void RobotPosesWidget::editDoubleClicked(int /*row*/, int /*column*/) { // We'll just base the edit on the selection highlight editSelected(); @@ -317,7 +317,7 @@ void RobotPosesWidget::editDoubleClicked(int row, int column) // ****************************************************************************************** // Preview whatever element is selected // ****************************************************************************************** -void RobotPosesWidget::previewClicked(int row, int column) +void RobotPosesWidget::previewClicked(int row, int /*column*/) { const std::string& name = data_table_->item(row, 0)->text().toStdString(); const std::string& group = data_table_->item(row, 1)->text().toStdString(); diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index 815045ca34..9922043012 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -512,7 +512,7 @@ void ROSControllersWidget::previewSelectedGroup(std::vector groups) // ****************************************************************************************** // Called when an item is seleceted from the controllers tree // ****************************************************************************************** -void ROSControllersWidget::previewSelected(QTreeWidgetItem* selected_item, int column) +void ROSControllersWidget::previewSelected(QTreeWidgetItem* selected_item, int /*column*/) { // Get the user custom properties of the currently selected row int type = selected_item->data(0, Qt::UserRole).value(); diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 75080ac11e..2b2d140361 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -495,7 +495,7 @@ void SetupAssistantWidget::closeEvent(QCloseEvent* event) // ****************************************************************************************** // Qt Error Handling - TODO // ****************************************************************************************** -bool SetupAssistantWidget::notify(QObject* reciever, QEvent* event) +bool SetupAssistantWidget::notify(QObject* /*receiver*/, QEvent* /*event*/) { QMessageBox::critical(this, "Error", "An error occurred and was caught by Qt notify event handler.", QMessageBox::Ok); diff --git a/moveit_setup_assistant/src/widgets/simulation_widget.cpp b/moveit_setup_assistant/src/widgets/simulation_widget.cpp index e5bc3ed2df..14b9d5a805 100644 --- a/moveit_setup_assistant/src/widgets/simulation_widget.cpp +++ b/moveit_setup_assistant/src/widgets/simulation_widget.cpp @@ -218,7 +218,7 @@ void SimulationWidget::generateURDFClick() // ****************************************************************************************** // Called the copy to clipboard button is clicked // ****************************************************************************************** -void SimulationWidget::copyURDF(const QString& link) +void SimulationWidget::copyURDF(const QString& /*link*/) { simulation_text_->selectAll(); simulation_text_->copy(); diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 110ba6391a..5c22805d09 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -298,7 +298,7 @@ void StartScreenWidget::loadFilesClick() } } -void StartScreenWidget::onPackagePathChanged(const QString& path) +void StartScreenWidget::onPackagePathChanged(const QString& /*path*/) { if (!loadPackageSettings(false)) return; diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp index b07398daf8..1080649b48 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp @@ -245,7 +245,7 @@ void VirtualJointsWidget::showNewScreen() // ****************************************************************************************** // Edit whatever element is selected // ****************************************************************************************** -void VirtualJointsWidget::editDoubleClicked(int row, int column) +void VirtualJointsWidget::editDoubleClicked(int /*row*/, int /*column*/) { editSelected(); } @@ -253,7 +253,7 @@ void VirtualJointsWidget::editDoubleClicked(int row, int column) // ****************************************************************************************** // Preview whatever element is selected // ****************************************************************************************** -void VirtualJointsWidget::previewClicked(int row, int column) +void VirtualJointsWidget::previewClicked(int /*row*/, int /*column*/) { // TODO: highlight the virtual joint? From a87d95542575e6cfdddb7de819f4691fece63881 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 25 Nov 2019 15:59:24 +0100 Subject: [PATCH 107/612] MoveItCpp: correctly initialize tf_buffer_ --- moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index 8d91554da2..dfa5d12bf5 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -59,6 +59,7 @@ MoveItCpp::MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) + : tf_buffer_(tf_buffer) { if (!tf_buffer_) tf_buffer_.reset(new tf2_ros::Buffer()); From 7b86d4d2ce36eab017f7f6e11cc179613478039e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 25 Nov 2019 17:02:14 +0100 Subject: [PATCH 108/612] silent warnings outside MoveIt code base --- .../collision_distance_field_types.h | 4 + .../include/moveit/macros/diagnostics.h | 73 +++++++++++++++++++ .../moveit/benchmarks/BenchmarkExecutor.h | 4 + .../occupancy_map_monitor/occupancy_map.h | 4 + .../pointcloud_octomap_updater.h | 4 + .../motion_planning_frame.h | 4 + .../motion_planning_param_widget.h | 7 +- .../src/motion_planning_display.cpp | 5 ++ .../src/robot_state_display.cpp | 4 + .../rviz_plugin_render_tools/octomap_render.h | 5 +- .../planning_link_updater.h | 4 + .../robot_state_visualization.h | 4 + .../src/octomap_render.cpp | 3 + .../src/render_shapes.cpp | 4 + .../warehouse/src/moveit_message_storage.cpp | 5 +- .../src/widgets/setup_assistant_widget.cpp | 4 + 16 files changed, 135 insertions(+), 3 deletions(-) create mode 100644 moveit_core/macros/include/moveit/macros/diagnostics.h diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index 9816e4be1e..c9b7ae066c 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -45,7 +45,11 @@ #include #include +#include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include +DIAGNOSTIC_POP #include #include diff --git a/moveit_core/macros/include/moveit/macros/diagnostics.h b/moveit_core/macros/include/moveit/macros/diagnostics.h new file mode 100644 index 0000000000..6d3e78caa8 --- /dev/null +++ b/moveit_core/macros/include/moveit/macros/diagnostics.h @@ -0,0 +1,73 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Bielefeld University + * 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 Bielefeld University 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 + +/** The following macros allow to temporarily disable specific warnings, + * which in turn allows us to compile the MoveIt code base with rather strict + * compiler warnings, and still avoid corresponding issues in external sources. + * Just wrap an offending include with: + * + * DIAGNOSTIC_PUSH + * SILENT_UNUSED_PARAM + * #include + * DIAGNOSTIC_PUSH + * + * The push and pop operations ensure that the old status is restored afterwards. + */ + +#if defined(_MSC_VER) +#define DIAGNOSTIC_PUSH __pragma(warning(push)) +#define DIAGNOSTIC_POP __pragma(warning(pop)) +#define SILENT_UNUSED_PARAM __pragma(warning(disable : 4100)) + +#elif defined(__GNUC__) || defined(__clang__) +#define DO_PRAGMA(X) _Pragma(#X) +#define DIAGNOSTIC_PUSH DO_PRAGMA(GCC diagnostic push) +#define DIAGNOSTIC_POP DO_PRAGMA(GCC diagnostic pop) + +#if defined(__clang__) +#define SILENT_UNUSED_PARAM DO_PRAGMA(GCC diagnostic ignored "-Wunused-parameter") +#else +#define SILENT_UNUSED_PARAM \ + DO_PRAGMA(GCC diagnostic ignored "-Wunused-parameter") \ + DO_PRAGMA(GCC diagnostic ignored "-Wunused-but-set-parameter") +#endif + +#else +#define DIAGNOSTIC_PUSH +#define DIAGNOSTIC_POP +#define SILENT_UNUSED_PARAM + +#endif diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index 92a0a82a36..e7b50d07bc 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -46,7 +46,11 @@ #include #include #include +#include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include +DIAGNOSTIC_PUSH #include #include diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h index ee98f95de5..88e8730d6d 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h @@ -36,7 +36,11 @@ #pragma once +#include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include +DIAGNOSTIC_PUSH #include #include #include diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h index 80ecbfaa11..5eba41256d 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h @@ -37,8 +37,12 @@ #pragma once #include +#include #include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include +DIAGNOSTIC_POP #include #include #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index e9a7398a90..50996db00e 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -48,8 +48,12 @@ #include #include #include +#include #include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include +DIAGNOSTIC_POP #include #include #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h index 99658c4f2d..0869a1a8ed 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h @@ -36,8 +36,13 @@ #pragma once -#include #include +#include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM +#include +DIAGNOSTIC_POP + namespace moveit { namespace planning_interface diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 2087245192..b635d954f5 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -38,6 +38,10 @@ #include #include #include +#include + +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include #include #include @@ -59,6 +63,7 @@ #include #include #include +DIAGNOSTIC_POP #include #include diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 64d5773fd8..24d739ec9c 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -38,6 +38,9 @@ #include #include +#include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include #include #include @@ -53,6 +56,7 @@ #include #include +DIAGNOSTIC_POP namespace moveit_rviz_plugin { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h index 9c4f3c60b2..fb11b11fe8 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h @@ -38,8 +38,11 @@ #include #include +#include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include - +DIAGNOSTIC_POP #include namespace octomap diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h index 0d757ef7c5..b1d1358a28 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h @@ -36,7 +36,11 @@ #pragma once +#include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include +DIAGNOSTIC_POP #include namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index 01444a817f..52b81c9f07 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -39,7 +39,11 @@ #include #include #include +#include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include +DIAGNOSTIC_POP namespace moveit_rviz_plugin { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp index dbe2f667ae..e0ff33fbff 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp @@ -36,8 +36,11 @@ #include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include #include +DIAGNOSTIC_POP #include #include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index 0743a5e823..e463c22198 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -46,8 +46,12 @@ #include #include +#include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include #include +DIAGNOSTIC_POP #include #include diff --git a/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp b/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp index b0f5bef94e..3511f8e8b6 100644 --- a/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp +++ b/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp @@ -35,8 +35,11 @@ /* Author: Ioan Sucan */ #include +#include +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include -//#include +DIAGNOSTIC_PUSH #include #include #include diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 2b2d140361..fee2c0455b 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -34,6 +34,7 @@ /* Author: Dave Coleman */ +#include // SA #include "setup_screen_widget.h" // a base class for screens in the setup assistant #include "setup_assistant_widget.h" @@ -50,11 +51,14 @@ #include #include // for loading all avail kinematic planners // Rviz +DIAGNOSTIC_PUSH +SILENT_UNUSED_PARAM #include #include #include #include #include +DIAGNOSTIC_POP namespace moveit_setup_assistant { From b2cadf602013f8bf02047f5886b9aa9b0d4da95d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 25 Nov 2019 22:31:08 +0100 Subject: [PATCH 109/612] remove unused include --- .../moveit/motion_planning_rviz_plugin/motion_planning_display.h | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index 84e911f1a5..09910b555f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -37,7 +37,6 @@ #pragma once #include -#include #include #include #include From 049b865e9e47d2b5f2dcb89f0dd49490227e8e61 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 26 Nov 2019 21:29:29 +0100 Subject: [PATCH 110/612] fix clang-tidy issues --- .../rviz_plugin_render_tools/src/render_shapes.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index e463c22198..3a0b6b7ee0 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -193,8 +193,8 @@ void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, co void RenderShapes::updateShapeColors(float r, float g, float b, float a) { - for (auto it = scene_shapes_.begin(), end = scene_shapes_.end(); it != end; ++it) - (**it).setColor(r, g, b, a); + for (const std::unique_ptr& shape : scene_shapes_) + shape->setColor(r, g, b, a); } } // namespace moveit_rviz_plugin From e0c8591b6e277727c28c2d6b0e742dd44a671390 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 26 Nov 2019 23:20:57 +0100 Subject: [PATCH 111/612] fix -Woverloaded-virtual warnings Due to a different order of arguments, there was an extra layer of function forwarding for searchPositionIK. --- .../kdl_kinematics_plugin.h | 17 ------------- .../src/kdl_kinematics_plugin.cpp | 19 ++++----------- .../lma_kinematics_plugin.h | 18 -------------- .../src/lma_kinematics_plugin.cpp | 23 +++++------------- .../srv_kinematics_plugin.h | 18 +++++--------- .../src/srv_kinematics_plugin.cpp | 24 ++++++------------- 6 files changed, 23 insertions(+), 96 deletions(-) diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h index 6e13e834f4..61865c482b 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h @@ -122,23 +122,6 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase const std::vector& getLinkNames() const override; protected: - /** - * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solutions by randomly re-seeding on failure. - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param timeout The amount of time (in seconds) available to the solver - * @param solution the solution vector - * @param solution_callback A callback to validate an IK solution - * @param error_code an error code that encodes the reason for failure or success - * @param consistency_limits The returned solutuion will not deviate more than these from the seed - * @return True if a valid solution was found, false otherwise - */ - 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 std::vector& consistency_limits, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; - typedef Eigen::Matrix Twist; /// Solve position IK given initial joint values diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp index ace343ae39..9ba7d0e4aa 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp @@ -278,7 +278,7 @@ bool KDLKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, cons std::vector consistency_limits; // limit search to a single attempt by setting a timeout of zero - return searchPositionIK(ik_pose, ik_seed_state, 0.0, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, 0.0, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -289,7 +289,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -298,7 +298,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -309,7 +309,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c const kinematics::KinematicsQueryOptions& options) const { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options); } @@ -318,17 +318,6 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const -{ - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, - options); -} - -bool KDLKinematicsPlugin::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 std::vector& consistency_limits, - const kinematics::KinematicsQueryOptions& options) const { ros::WallTime start_time = ros::WallTime::now(); if (!initialized_) diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h index 79c1155d67..d9c3ab754b 100644 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h +++ b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h @@ -115,24 +115,6 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase */ const std::vector& getLinkNames() const override; -protected: - /** - * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solutions by randomly re-seeding on failure. - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param timeout The amount of time (in seconds) available to the solver - * @param solution the solution vector - * @param solution_callback A callback to validate an IK solution - * @param error_code an error code that encodes the reason for failure or success - * @param consistency_limits The returned solutuion will not deviate more than these from the seed - * @return True if a valid solution was found, false otherwise - */ - 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 std::vector& consistency_limits, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; - private: bool timedOut(const ros::WallTime& start_time, double duration) const; diff --git a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp index 2eda07ee0b..0dc4f4496f 100644 --- a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp +++ b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp @@ -154,7 +154,7 @@ bool LMAKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, cons std::vector consistency_limits; // limit search to a single attempt by setting a timeout of zero - return searchPositionIK(ik_pose, ik_seed_state, 0.0, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, 0.0, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -165,7 +165,7 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -174,7 +174,7 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -185,17 +185,7 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c const kinematics::KinematicsQueryOptions& options) const { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, - options); -} - -bool LMAKinematicsPlugin::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) const -{ - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options); } @@ -216,10 +206,9 @@ bool LMAKinematicsPlugin::obeysLimits(const Eigen::VectorXd& values) const } bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, - double timeout, std::vector& solution, - const IKCallbackFn& solution_callback, + double timeout, const std::vector& consistency_limits, + std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, - const std::vector& consistency_limits, const kinematics::KinematicsQueryOptions& options) const { ros::WallTime start_time = ros::WallTime::now(); diff --git a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h index f0a1494648..3f23c70150 100644 --- a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h +++ b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h @@ -99,6 +99,12 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + 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 = nullptr) const override; + bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, std::vector& poses) const override; @@ -122,18 +128,6 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase const std::vector& getVariableNames() const; protected: - 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 std::vector& consistency_limits, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; - - 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; - bool setRedundantJoints(const std::vector& redundant_joint_indices) override; private: diff --git a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp index b8f98f914f..e876e79ca7 100644 --- a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp +++ b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp @@ -172,8 +172,8 @@ bool SrvKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, cons { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, solution, IKCallbackFn(), error_code, - consistency_limits, options); + return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, consistency_limits, solution, IKCallbackFn(), + error_code, options); } bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, @@ -183,7 +183,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -192,7 +192,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -203,7 +203,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c const kinematics::KinematicsQueryOptions& options) const { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options); } @@ -212,17 +212,6 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const -{ - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, - options); -} - -bool SrvKinematicsPlugin::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 std::vector& consistency_limits, - const kinematics::KinematicsQueryOptions& options) const { // Convert single pose into a vector of one pose std::vector ik_poses; @@ -237,7 +226,8 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vector& /*consistency_limits*/, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& /*options*/) const + const kinematics::KinematicsQueryOptions& /*options*/, + const moveit::core::RobotState* /*context_state*/) const { // Check if active if (!active_) From ded7b0aaac06daa5c7da83338485f32ffcf649e8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 25 Nov 2019 23:36:38 +0100 Subject: [PATCH 112/612] Travis: enable all warnings --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index f37cd1bfb5..bee34f948e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -19,7 +19,7 @@ env: - UPSTREAM_WORKSPACE=moveit.rosinstall # moveit_ros_perception: mesh_filter_test fails due to broken Mesa OpenGL - TEST_BLACKLIST="moveit_ros_perception" - - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-unused-function" + - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" - WARNINGS_OK=false matrix: - TEST="clang-format catkin_lint" @@ -33,7 +33,7 @@ matrix: # Add a separate config to the matrix, using clang as compiler env: ROS_REPO=ros-shadow-fixed TEST=clang-tidy-fix BEFORE_DOCKER_SCRIPT="source moveit_kinematics/test/test_ikfast_plugins.sh" - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unused-parameter -Wno-unused-function -Wno-overloaded-virtual" + CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-overloaded-virtual" before_script: - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci From 9b7ab3d5a0b67fe6b80681048043ae768f1412dc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 27 Nov 2019 13:25:51 +0100 Subject: [PATCH 113/612] ControllerManager: wait for done-callback (#1783) --- .../action_based_controller_handle.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h index adcc2efce0..72218f3e2e 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h @@ -126,6 +126,12 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase { if (controller_action_client_ && !done_) return controller_action_client_->waitForResult(timeout); +#if 1 // TODO: remove when https://github.com/ros/actionlib/issues/155 is fixed + // workaround for actionlib issue: waitForResult() might return before our doneCB finished + ros::Time deadline = ros::Time::now() + ros::Duration(0.1); // limit waiting to 0.1s + while (!done_ && ros::ok() && ros::Time::now() < deadline) // Check the done_ flag explicitly, + ros::Duration(0.0001).sleep(); // which is eventually set in finishControllerExecution +#endif return true; } From e16436cb49db9c4b82a496d4a2b969fcb9a697df Mon Sep 17 00:00:00 2001 From: JonasTietz Date: Wed, 27 Nov 2019 16:12:39 +0100 Subject: [PATCH 114/612] move_group capability for publishing planning scene frames to the tf system (#1761) Added a move_group capability, which publishes the frames of planning scene objects to the tf system. --- moveit_ros/move_group/CMakeLists.txt | 1 + ...efault_capabilities_plugin_description.xml | 6 + .../tf_publisher_capability.cpp | 106 ++++++++++++++++++ .../tf_publisher_capability.h | 59 ++++++++++ 4 files changed, 172 insertions(+) create mode 100644 moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp create mode 100644 moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index 783f29faa7..a3f4015b0d 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -58,6 +58,7 @@ add_library(moveit_move_group_default_capabilities src/default_capabilities/get_planning_scene_service_capability.cpp src/default_capabilities/apply_planning_scene_service_capability.cpp src/default_capabilities/clear_octomap_service_capability.cpp + src/default_capabilities/tf_publisher_capability.cpp ) set_target_properties(moveit_move_group_default_capabilities PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") add_dependencies(moveit_move_group_default_capabilities ${catkin_EXPORTED_TARGETS}) diff --git a/moveit_ros/move_group/default_capabilities_plugin_description.xml b/moveit_ros/move_group/default_capabilities_plugin_description.xml index b7bd5d9f0d..aff9edd872 100644 --- a/moveit_ros/move_group/default_capabilities_plugin_description.xml +++ b/moveit_ros/move_group/default_capabilities_plugin_description.xml @@ -65,5 +65,11 @@ Provide a ROS service that allows for clearing the octomap + + + + Provide a capability that publishes PlanningScene frames to the tf system + + diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp new file mode 100644 index 0000000000..13a0e3480f --- /dev/null +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -0,0 +1,106 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Hamburg University + * 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 Hamburg University 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. + *********************************************************************/ + +/* Author: Jonas Tietz */ + +#include "tf_publisher_capability.h" +#include +#include +#include +#include + +namespace move_group +{ +TfPublisher::TfPublisher() : MoveGroupCapability("TfPublisher") +{ +} + +TfPublisher::~TfPublisher() +{ + keep_running_ = false; + thread_.join(); +} + +void TfPublisher::publishPlanningSceneFrames() +{ + tf2_ros::TransformBroadcaster broadcaster; + geometry_msgs::TransformStamped transform; + ros::Rate rate(rate_); + + while (keep_running_) + { + { + planning_scene_monitor::LockedPlanningSceneRO locked_planning_scene(context_->planning_scene_monitor_); + collision_detection::WorldConstPtr world = locked_planning_scene->getWorld(); + std::string planning_frame = locked_planning_scene->getPlanningFrame(); + + for (const auto& obj : *world) + { + std::string parent_frame = prefix_ + obj.second->id_; + transform = tf2::eigenToTransform(obj.second->shape_poses_[0]); + transform.child_frame_id = parent_frame; + transform.header.frame_id = planning_frame; + broadcaster.sendTransform(transform); + + moveit::core::FixedTransformsMap subframes = obj.second->subframe_poses_; + for (auto& subframe : subframes) + { + transform = tf2::eigenToTransform(subframe.second); + transform.child_frame_id = parent_frame + "/" + subframe.first; + transform.header.frame_id = parent_frame; + broadcaster.sendTransform(transform); + } + } + } + + rate.sleep(); + } +} + +void TfPublisher::initialize() +{ + ros::NodeHandle nh = ros::NodeHandle("~"); + + std::string prefix = nh.getNamespace() + "/"; + nh.param("planning_scene_frame_publishing_rate", rate_, 10); + nh.param("planning_scene_tf_prefix", prefix_, prefix); + keep_running_ = true; + + ROS_INFO("Initializing MoveGroupTfPublisher with a frame publishing rate of %d", rate_); + thread_ = std::thread(&TfPublisher::publishPlanningSceneFrames, this); +} +} // namespace move_group + +#include +CLASS_LOADER_REGISTER_CLASS(move_group::TfPublisher, move_group::MoveGroupCapability) diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h new file mode 100644 index 0000000000..2a15f4d283 --- /dev/null +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h @@ -0,0 +1,59 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Hamburg University + * 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 Hamburg University 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. + *********************************************************************/ + +/* Author: Jonas Tietz */ + +#pragma once + +#include +#include + +namespace move_group +{ +class TfPublisher : public MoveGroupCapability +{ +public: + TfPublisher(); + ~TfPublisher(); + + void initialize() override; + +private: + void publishPlanningSceneFrames(); + int rate_; + std::string prefix_; + std::thread thread_; + bool keep_running_; +}; +} From d48d57f856b33433c18754497712be8d185a371f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 28 Nov 2019 08:16:37 +0100 Subject: [PATCH 115/612] Enable code-coverage test (#1776) --- .travis.yml | 1 + moveit_core/CMakeLists.txt | 10 ---------- moveit_core/package.xml | 1 - 3 files changed, 1 insertion(+), 11 deletions(-) diff --git a/.travis.yml b/.travis.yml index bee34f948e..6ed584a5ff 100644 --- a/.travis.yml +++ b/.travis.yml @@ -23,6 +23,7 @@ env: - WARNINGS_OK=false matrix: - TEST="clang-format catkin_lint" + - TEST=code-coverage - ROS_DISTRO=melodic - ROS_DISTRO=kinetic diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 813c41187e..64f05c178d 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -145,16 +145,6 @@ catkin_package( urdfdom_headers ) - -# to run: catkin_make -DENABLE_COVERAGE_TESTING=ON package_name_coverage -if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) - find_package(code_coverage REQUIRED) # catkin package ros-*-code-coverage - include(CodeCoverage) - APPEND_COVERAGE_COMPILER_FLAGS() - set(COVERAGE_EXCLUDES "*/test/*") - add_code_coverage(NAME ${PROJECT_NAME}_coverage) -endif() - include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} ${LIBFCL_INCLUDE_DIRS} ) diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 121f19f3b3..70a185040c 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -53,7 +53,6 @@ tf2_kdl orocos_kdl rosunit - code_coverage From a1c0adea514d7fc94b94736212835c4a16e85a6b Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Thu, 28 Nov 2019 14:55:30 +0100 Subject: [PATCH 116/612] [moveit_commander] python3 import fixes (#1786) - force relative import - try to import StringIO from StringIO module first, then from io module --- moveit_commander/src/moveit_commander/conversions.py | 10 ++++++++-- .../src/moveit_commander/planning_scene_interface.py | 4 ++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/moveit_commander/src/moveit_commander/conversions.py b/moveit_commander/src/moveit_commander/conversions.py index 9a241d3992..71c2dfe147 100644 --- a/moveit_commander/src/moveit_commander/conversions.py +++ b/moveit_commander/src/moveit_commander/conversions.py @@ -32,14 +32,20 @@ # # Author: Ioan Sucan -import StringIO +try: + # Try Python 2.7 behaviour first + from StringIO import StringIO +except ImportError: + # Use Python 3.x behaviour as fallback + from io import StringIO + from moveit_commander import MoveItCommanderException from geometry_msgs.msg import Pose, PoseStamped, Transform import rospy import tf def msg_to_string(msg): - buf = StringIO.StringIO() + buf = StringIO() msg.serialize(buf) return buf.getvalue() diff --git a/moveit_commander/src/moveit_commander/planning_scene_interface.py b/moveit_commander/src/moveit_commander/planning_scene_interface.py index e0093e1508..ff393c67de 100644 --- a/moveit_commander/src/moveit_commander/planning_scene_interface.py +++ b/moveit_commander/src/moveit_commander/planning_scene_interface.py @@ -33,13 +33,13 @@ # Author: Ioan Sucan, Felix Messmer import rospy -import conversions +from . import conversions from moveit_msgs.msg import PlanningScene, CollisionObject, AttachedCollisionObject from moveit_ros_planning_interface import _moveit_planning_scene_interface from geometry_msgs.msg import Pose, Point from shape_msgs.msg import SolidPrimitive, Plane, Mesh, MeshTriangle -from exception import MoveItCommanderException +from .exception import MoveItCommanderException from moveit_msgs.srv import ApplyPlanningScene, ApplyPlanningSceneRequest try: From acead73aae0df309eaeb1a71ebe3f0bffce8efba Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 28 Nov 2019 22:02:15 +0100 Subject: [PATCH 117/612] rviz plugin: Joints tab (#1308) --- .../CMakeLists.txt | 7 +- .../motion_planning_display.h | 5 + .../motion_planning_frame.h | 2 + .../motion_planning_frame_joints_widget.h | 254 ++++++++ .../src/motion_planning_display.cpp | 10 +- .../src/motion_planning_frame.cpp | 9 + .../motion_planning_frame_joints_widget.cpp | 561 ++++++++++++++++++ ...otion_planning_rviz_plugin_frame_joints.ui | 61 ++ 8 files changed, 902 insertions(+), 7 deletions(-) create mode 100644 moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h create mode 100644 moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp create mode 100644 moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt index 6f4ce1f245..938e0a86e2 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt @@ -1,9 +1,13 @@ set(HEADERS include/moveit/motion_planning_rviz_plugin/motion_planning_display.h include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h + include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h ) -qt_wrap_ui(UIC_FILES src/ui/motion_planning_rviz_plugin_frame.ui) +qt_wrap_ui(UIC_FILES + src/ui/motion_planning_rviz_plugin_frame.ui + src/ui/motion_planning_rviz_plugin_frame_joints.ui +) include_directories(${CMAKE_CURRENT_BINARY_DIR}) @@ -15,6 +19,7 @@ set(SOURCE_FILES src/motion_planning_frame_objects.cpp src/motion_planning_frame_scenes.cpp src/motion_planning_frame_states.cpp + src/motion_planning_frame_joints_widget.cpp src/motion_planning_display.cpp src/motion_planning_frame_manipulation.cpp src/motion_planning_param_widget.cpp diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index 09910b555f..1df45780c5 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -156,6 +156,11 @@ class MotionPlanningDisplay : public PlanningSceneDisplay void toggleSelectPlanningGroupSubscription(bool enable); +Q_SIGNALS: + // signals issued when start/goal states of a query changed + void queryStartStateChanged(); + void queryGoalStateChanged(); + private Q_SLOTS: // ****************************************************************************************** diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 50996db00e..df3e4edeec 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -86,6 +86,7 @@ MOVEIT_CLASS_FORWARD(RobotStateStorage); namespace moveit_rviz_plugin { class MotionPlanningDisplay; +class MotionPlanningFrameJointsWidget; const std::string OBJECT_RECOGNITION_ACTION = "/recognize_objects"; @@ -125,6 +126,7 @@ class MotionPlanningFrame : public QWidget MotionPlanningDisplay* planning_display_; rviz::DisplayContext* context_; Ui::MotionPlanningUI* ui_; + MotionPlanningFrameJointsWidget* joints_tab_; moveit::planning_interface::MoveGroupInterfacePtr move_group_; moveit::planning_interface::PlanningSceneInterfacePtr planning_scene_interface_; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h new file mode 100644 index 0000000000..27363dd63c --- /dev/null +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h @@ -0,0 +1,254 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, CITEC, Bielefeld University + * 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 CITEC / Bielefeld University 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. + *********************************************************************/ + +/* Author: Robert Haschke */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +class QSlider; + +namespace Ui +{ +class MotionPlanningFrameJointsUI; +} +namespace robot_interaction +{ +MOVEIT_CLASS_FORWARD(InteractionHandler) +} +namespace moveit_rviz_plugin +{ +/** TableModel to display joint values of a referenced RobotState. + * + * Unfortunately we cannot store the RobotStatePtr (and thus ensure existence of the state during + * the lifetime of this class instance), because RobotInteraction (which is the initial use case) + * allocates internally a new RobotState if any other copy is held somewhere else. + * Hence, we also store an (unsafe) raw pointer. Lifetime of this raw pointer needs to be ensured. + */ +class JMGItemModel : public QAbstractTableModel +{ + Q_OBJECT + moveit::core::RobotState robot_state_; + const moveit::core::JointModelGroup* jmg_; + +public: + JMGItemModel(const moveit::core::RobotState& robot_state, const std::string& group_name, QObject* parent = nullptr); + + // QAbstractItemModel interface + int rowCount(const QModelIndex& parent = QModelIndex()) const override; + int columnCount(const QModelIndex& parent = QModelIndex()) const override; + Qt::ItemFlags flags(const QModelIndex& index) const override; + QVariant data(const QModelIndex& index, int role) const override; + QVariant headerData(int section, Qt::Orientation orientation, int role) const override; + bool setData(const QModelIndex& index, const QVariant& value, int role) override; + + /// call this on any external change of the RobotState + void updateRobotState(const moveit::core::RobotState& state); + + moveit::core::RobotState& getRobotState() + { + return robot_state_; + } + const moveit::core::RobotState& getRobotState() const + { + return robot_state_; + } + const moveit::core::JointModelGroup* getJointModelGroup() const + { + return jmg_; + } + +private: + /// retrieve the JointModel corresponding to the variable referenced by index + const moveit::core::JointModel* getJointModel(const QModelIndex& index) const; + /// retrieve the variable bounds referenced by variable index + const moveit::core::VariableBounds* getVariableBounds(const moveit::core::JointModel* jm, + const QModelIndex& index) const; +}; + +class JointsWidgetEventFilter : public QObject +{ + Q_OBJECT + +public: + JointsWidgetEventFilter(QAbstractItemView* view); + +protected: + bool eventFilter(QObject* target, QEvent* event) override; +}; + +class MotionPlanningDisplay; +class MotionPlanningFrameJointsWidget : public QWidget +{ + Q_OBJECT + +public: + MotionPlanningFrameJointsWidget(const MotionPlanningFrameJointsWidget&) = delete; + MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent = nullptr); + ~MotionPlanningFrameJointsWidget(); + + void changePlanningGroup(const std::string& group_name, + const robot_interaction::InteractionHandlerPtr& start_state_handler, + const robot_interaction::InteractionHandlerPtr& goal_state_handler); + +public Q_SLOTS: + void queryStartStateChanged(); + void queryGoalStateChanged(); + void jogNullspace(double value); + +protected: + void setActiveModel(JMGItemModel* model); + void triggerUpdate(JMGItemModel* model); + void updateNullspaceSliders(); + QSlider* createNSSlider(int i); + +private: + Ui::MotionPlanningFrameJointsUI* ui_; + MotionPlanningDisplay* planning_display_; + robot_interaction::InteractionHandlerPtr start_state_handler_; + robot_interaction::InteractionHandlerPtr goal_state_handler_; + std::unique_ptr start_state_model_; + std::unique_ptr goal_state_model_; + // break circular loop of stateChanged() -> dataChanged() |-> PlanningDisplay::setQuery*State() + bool ignore_state_changes_ = false; + + Eigen::JacobiSVD svd_; + Eigen::MatrixXd nullspace_; + std::vector ns_sliders_; +}; + +/// Delegate to show the joint value as with a progress bar indicator between min and max. +class ProgressBarDelegate : public QStyledItemDelegate +{ + Q_OBJECT + +public: + enum CustomRole + { + JointTypeRole = Qt::UserRole, + VariableBoundsRole + }; + + ProgressBarDelegate(QWidget* parent = 0) : QStyledItemDelegate(parent) + { + } + + void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override; + QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option, const QModelIndex& index) const override; + +private Q_SLOTS: + void commitAndCloseEditor(); +}; + +/// Number editor via progress bar dragging +class ProgressBarEditor : public QWidget +{ + Q_OBJECT + Q_PROPERTY(float value READ value WRITE setValue NOTIFY valueChanged USER true) + +public: + /// Create a progressbar-like slider for editing values in range mix..max + ProgressBarEditor(QWidget* parent = nullptr, float min = -1.0, float max = 0.0, int digits = 0); + + void setValue(float value) + { + value_ = value; + } + float value() const + { + return value_; + } + +Q_SIGNALS: + void valueChanged(float value); + void editingFinished(); + +protected: + void paintEvent(QPaintEvent* event) override; + void mousePressEvent(QMouseEvent* event) override; + void mouseMoveEvent(QMouseEvent* event) override; + void mouseReleaseEvent(QMouseEvent* event) override; + +private: + float value_; + float min_; + float max_; + int digits_; ///< number of decimal digits for formatting of the value +}; + +/// Slider that jumps back to zero +class JogSlider : public QSlider +{ + Q_OBJECT + int timer_id_; + int timer_interval_; // ms + double maximum_; + +public: + JogSlider(QWidget* parent = nullptr); + + int timerInterval() const + { + return timer_interval_; + } + void setTimerInterval(int ms); + void setResolution(unsigned int resolution); + void setMaximum(double value); + double value() const + { + return QSlider::value() * maximum_ / QSlider::maximum(); + } + +protected: + void timerEvent(QTimerEvent* event) override; + void mousePressEvent(QMouseEvent* event) override; + void mouseReleaseEvent(QMouseEvent* event) override; + +private: + using QSlider::setMinimum; + using QSlider::setMaximum; + using QSlider::setRange; + +Q_SIGNALS: + void triggered(double value); +}; +} diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index b635d954f5..a30790f242 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -910,9 +910,7 @@ void MotionPlanningDisplay::scheduleDrawQueryStartState(robot_interaction::Inter return; addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed), "publishInteractiveMarkers"); - recomputeQueryStartStateMetrics(); - addMainLoopJob(boost::bind(&MotionPlanningDisplay::drawQueryStartState, this)); - context_->queueRender(); + updateQueryStartState(); } void MotionPlanningDisplay::scheduleDrawQueryGoalState(robot_interaction::InteractionHandler* /*unused*/, @@ -922,13 +920,12 @@ void MotionPlanningDisplay::scheduleDrawQueryGoalState(robot_interaction::Intera return; addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed), "publishInteractiveMarkers"); - recomputeQueryGoalStateMetrics(); - addMainLoopJob(boost::bind(&MotionPlanningDisplay::drawQueryGoalState, this)); - context_->queueRender(); + updateQueryGoalState(); } void MotionPlanningDisplay::updateQueryStartState() { + queryStartStateChanged(); recomputeQueryStartStateMetrics(); addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedQueryStartState, this)); context_->queueRender(); @@ -936,6 +933,7 @@ void MotionPlanningDisplay::updateQueryStartState() void MotionPlanningDisplay::updateQueryGoalState() { + queryGoalStateChanged(); recomputeQueryGoalStateMetrics(); addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedQueryGoalState, this)); context_->queueRender(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 970060a362..834d204f0f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -66,6 +67,11 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: { // set up the GUI ui_->setupUi(this); + // add more tabs + joints_tab_ = new MotionPlanningFrameJointsWidget(planning_display_, ui_->tabWidget); + ui_->tabWidget->addTab(joints_tab_, "Joints"); + connect(planning_display_, SIGNAL(queryStartStateChanged()), joints_tab_, SLOT(queryStartStateChanged())); + connect(planning_display_, SIGNAL(queryGoalStateChanged()), joints_tab_, SLOT(queryGoalStateChanged())); // connect bottons to actions; each action usually registers the function pointer for the actual computation, // to keep the GUI more responsive (using the background job processing) @@ -385,6 +391,9 @@ void MotionPlanningFrame::changePlanningGroup() { planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::changePlanningGroupHelper, this), "Frame::changePlanningGroup"); + joints_tab_->changePlanningGroup(planning_display_->getCurrentPlanningGroup(), + planning_display_->getQueryStartStateHandler(), + planning_display_->getQueryGoalStateHandler()); } void MotionPlanningFrame::sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp new file mode 100644 index 0000000000..7b9d683c75 --- /dev/null +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -0,0 +1,561 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, CITEC, Bielefeld University + * 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 CITEC / Bielefeld University 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. + *********************************************************************/ + +/* Author: Robert Haschke */ + +#include +#include + +#include "ui_motion_planning_rviz_plugin_frame_joints.h" +#include +#include + +namespace moveit_rviz_plugin +{ +JMGItemModel::JMGItemModel(const moveit::core::RobotState& robot_state, const std::string& group_name, QObject* parent) + : QAbstractTableModel(parent), robot_state_(robot_state), jmg_(nullptr) +{ + if (robot_state_.getRobotModel()->hasJointModelGroup(group_name)) + jmg_ = robot_state_.getRobotModel()->getJointModelGroup(group_name); +} + +int JMGItemModel::rowCount(const QModelIndex& parent) const +{ + if (!jmg_) + return robot_state_.getVariableCount(); + else + return jmg_->getVariableCount(); +} + +int JMGItemModel::columnCount(const QModelIndex& parent) const +{ + return 2; +} + +Qt::ItemFlags JMGItemModel::flags(const QModelIndex& index) const +{ + if (!index.isValid()) + return Qt::ItemFlags(); + + Qt::ItemFlags f = QAbstractTableModel::flags(index); + if (index.column() == 1) + { + const moveit::core::JointModel* jm = getJointModel(index); + if (!jm->isPassive() && !jm->getMimic()) // these are not editable + f |= Qt::ItemIsEditable; + } + return f; +} + +QVariant JMGItemModel::data(const QModelIndex& index, int role) const +{ + if (!index.isValid()) + return QVariant(); + + int idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row(); + switch (index.column()) + { + case 0: // joint name column + switch (role) + { + case Qt::DisplayRole: + return QString::fromStdString(robot_state_.getVariableNames()[idx]); + case Qt::TextAlignmentRole: + return Qt::AlignLeft; + } + break; + case 1: // joint value column + { + double value = robot_state_.getVariablePosition(idx); + const moveit::core::JointModel* jm = robot_state_.getRobotModel()->getJointOfVariable(idx); + switch (role) + { + case Qt::DisplayRole: + return value; + case Qt::EditRole: + if (jm) + return jm->getType() == moveit::core::JointModel::REVOLUTE ? value * 180 / M_PI : value; + break; + case ProgressBarDelegate::JointTypeRole: + if (jm) + return jm->getType(); + break; + case ProgressBarDelegate::VariableBoundsRole: + if (const moveit::core::VariableBounds* bounds = getVariableBounds(jm, index)) + return QPointF(bounds->min_position_, bounds->max_position_); + break; + case Qt::TextAlignmentRole: + return Qt::AlignLeft; + } + } + } + return QVariant(); +} + +QVariant JMGItemModel::headerData(int section, Qt::Orientation orientation, int role) const +{ + if (orientation == Qt::Horizontal && role == Qt::DisplayRole) + return section == 0 ? "Joint Name" : "Value"; + return QAbstractTableModel::headerData(section, orientation, role); +} + +bool JMGItemModel::setData(const QModelIndex& index, const QVariant& value, int role) +{ + if (index.column() != 1 || role != Qt::EditRole) + return false; + + int var_idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row(); + const moveit::core::JointModel* jm = robot_state_.getRobotModel()->getJointOfVariable(var_idx); + if (!value.canConvert()) + return false; + + bool ok; + double v = value.toDouble(&ok); + if (!ok) + return false; + + // for revolute joints, we convert degrees to radians + if (jm && jm->getType() == moveit::core::JointModel::REVOLUTE) + v *= M_PI / 180; + + robot_state_.setVariablePosition(var_idx, v); + jm->enforcePositionBounds(robot_state_.getVariablePositions() + jm->getFirstVariableIndex()); + dataChanged(index, index); + return true; +} + +const moveit::core::JointModel* JMGItemModel::getJointModel(const QModelIndex& index) const +{ + if (!index.isValid()) + return nullptr; + int var_idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row(); + return robot_state_.getRobotModel()->getJointOfVariable(var_idx); +} + +const moveit::core::VariableBounds* JMGItemModel::getVariableBounds(const moveit::core::JointModel* jm, + const QModelIndex& index) const +{ + if (!jm) + return nullptr; + int var_idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row(); + const moveit::core::VariableBounds* bounds = &jm->getVariableBounds()[var_idx - jm->getFirstVariableIndex()]; + return bounds->position_bounded_ ? bounds : nullptr; +} + +// copy positions from new_state and notify about these changes +void JMGItemModel::updateRobotState(const moveit::core::RobotState& state) +{ + if (robot_state_.getRobotModel() != state.getRobotModel()) + return; + robot_state_.setVariablePositions(state.getVariablePositions()); + + dataChanged(index(0, 1), index(rowCount() - 1, 1)); +} + +MotionPlanningFrameJointsWidget::MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent) + : QWidget(parent), ui_(new Ui::MotionPlanningFrameJointsUI()), planning_display_(display) +{ + ui_->setupUi(this); + // intercept mouse events delivered to joints_view_ to open editor on first mouse press + ui_->joints_view_->viewport()->installEventFilter(new JointsWidgetEventFilter(ui_->joints_view_)); + ui_->joints_view_->setItemDelegateForColumn(1, new ProgressBarDelegate(this)); + svd_.setThreshold(0.001); +} + +MotionPlanningFrameJointsWidget::~MotionPlanningFrameJointsWidget() +{ + delete ui_; +} + +void MotionPlanningFrameJointsWidget::changePlanningGroup( + const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler, + const robot_interaction::InteractionHandlerPtr& goal_state_handler) +{ + // release previous models (if any) + ui_->joints_view_->setModel(nullptr); + start_state_model_.reset(); + goal_state_model_.reset(); + + // create new models + start_state_handler_ = start_state_handler; + goal_state_handler_ = goal_state_handler; + start_state_model_.reset(new JMGItemModel(*start_state_handler_->getState(), group_name, this)); + goal_state_model_.reset(new JMGItemModel(*goal_state_handler_->getState(), group_name, this)); + + // forward model updates to the PlanningDisplay + connect(start_state_model_.get(), &JMGItemModel::dataChanged, this, [this]() { + if (!ignore_state_changes_) + planning_display_->setQueryStartState(start_state_model_->getRobotState()); + }); + connect(goal_state_model_.get(), &JMGItemModel::dataChanged, this, [this]() { + if (!ignore_state_changes_) + planning_display_->setQueryGoalState(goal_state_model_->getRobotState()); + }); + + // show the goal state by default + setActiveModel(goal_state_model_.get()); + updateNullspaceSliders(); +} + +void MotionPlanningFrameJointsWidget::queryStartStateChanged() +{ + if (!start_state_model_ || !start_state_handler_) + return; + ignore_state_changes_ = true; + start_state_model_->updateRobotState(*start_state_handler_->getState()); + ignore_state_changes_ = false; + setActiveModel(start_state_model_.get()); + updateNullspaceSliders(); +} + +void MotionPlanningFrameJointsWidget::queryGoalStateChanged() +{ + if (!goal_state_model_ || !goal_state_handler_) + return; + ignore_state_changes_ = true; + goal_state_model_->updateRobotState(*goal_state_handler_->getState()); + ignore_state_changes_ = false; + setActiveModel(goal_state_model_.get()); + updateNullspaceSliders(); +} + +void MotionPlanningFrameJointsWidget::setActiveModel(JMGItemModel* model) +{ + ui_->joints_view_->setModel(model); + ui_->joints_view_label_->setText( + QString("Group joints of %1 state").arg(model == start_state_model_.get() ? "start" : "goal")); +} + +void MotionPlanningFrameJointsWidget::triggerUpdate(JMGItemModel* model) +{ + if (model == start_state_model_.get()) + planning_display_->setQueryStartState(model->getRobotState()); + else + planning_display_->setQueryGoalState(model->getRobotState()); +} + +// Find matching key vector in columns of haystack and return the best-aligned column index. +// Only consider available indexes (> 0). If no match is found, return largest available index. +// sign becomes -1 if key has changed sign compared to the matching haystack column. +static Eigen::Index findMatching(const Eigen::VectorXd& key, const Eigen::MatrixXd& haystack, + const Eigen::VectorXi& available, double& sign) +{ + Eigen::Index result = available.array().maxCoeff(); + double best_match = 0.0; + for (unsigned int i = 0; i < available.rows(); ++i) + { + int index = available[i]; + if (index < 0) // index already taken + continue; + if (index >= haystack.cols()) + return result; + double match = haystack.col(available[i]).transpose() * key; + double abs_match = std::abs(match); + if (abs_match > 0.5 && abs_match > best_match) + { + best_match = abs_match; + result = index; + sign = match > 0 ? 1.0 : -1.0; + } + } + return result; +} + +void MotionPlanningFrameJointsWidget::updateNullspaceSliders() +{ + JMGItemModel* model = dynamic_cast(ui_->joints_view_->model()); + std::size_t i = 0; + if (model && model->getJointModelGroup() && model->getJointModelGroup()->isChain()) + { + model->getRobotState().updateLinkTransforms(); + Eigen::MatrixXd jacobian; + if (!model->getRobotState().getJacobian(model->getJointModelGroup(), + model->getJointModelGroup()->getLinkModels().back(), + Eigen::Vector3d::Zero(), jacobian, false)) + goto cleanup; + + svd_.compute(jacobian, Eigen::ComputeFullV); + Eigen::Index rank = svd_.rank(); + std::size_t ns_dim = svd_.cols() - rank; + Eigen::MatrixXd ns(svd_.cols(), ns_dim); + Eigen::VectorXi available(ns_dim); + for (std::size_t j = 0; j < ns_dim; ++j) + available[j] = j; + + ns_sliders_.reserve(ns_dim); + // create/unhide sliders + for (; i < ns_dim; ++i) + { + if (i >= ns_sliders_.size()) + ns_sliders_.push_back(createNSSlider(i + 1)); + ns_sliders_[i]->show(); + + // Find matching null-space basis vector in previous nullspace_ + double sign = 1.0; + const Eigen::VectorXd& current = svd_.matrixV().col(rank + i); + Eigen::Index index = findMatching(current, nullspace_, available, sign); + ns.col(index).noalias() = sign * current; + available[index] = -1; // mark index as taken + } + nullspace_ = ns; + } + +cleanup: + if (i == 0) + nullspace_.resize(0, 0); + + // hide remaining sliders + for (; i < ns_sliders_.size(); ++i) + ns_sliders_[i]->hide(); +} + +QSlider* MotionPlanningFrameJointsWidget::createNSSlider(int i) +{ + JogSlider* slider = new JogSlider(this); + slider->setOrientation(Qt::Horizontal); + slider->setMaximum(0.1); + slider->setToolTip(QString("Nullspace dim #%1").arg(i)); + ui_->nullspace_layout_->addWidget(slider); + connect(slider, SIGNAL(triggered(double)), this, SLOT(jogNullspace(double))); + return slider; +} + +void MotionPlanningFrameJointsWidget::jogNullspace(double value) +{ + if (value == 0) + return; + + std::size_t index = std::find(ns_sliders_.begin(), ns_sliders_.end(), sender()) - ns_sliders_.begin(); + if (static_cast(index) >= nullspace_.cols()) + return; + + JMGItemModel* model = dynamic_cast(ui_->joints_view_->model()); + if (!model) + return; + + Eigen::VectorXd values; + model->getRobotState().copyJointGroupPositions(model->getJointModelGroup(), values); + values += value * nullspace_.col(index); + model->getRobotState().setJointGroupPositions(model->getJointModelGroup(), values); + model->getRobotState().harmonizePositions(model->getJointModelGroup()); + triggerUpdate(model); +} + +void ProgressBarDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const +{ + // copied from QStyledItemDelegate::paint + QStyle* style = option.widget ? option.widget->style() : QApplication::style(); + QStyleOptionViewItem style_option = option; + initStyleOption(&style_option, index); + + if (index.column() == 1) + { + QVariant joint_type = index.data(JointTypeRole); + double value = index.data().toDouble(); + bool is_revolute = joint_type.isValid() && joint_type.toInt() == moveit::core::JointModel::REVOLUTE; + style_option.text = option.locale.toString(is_revolute ? value * 180 / M_PI : value, 'f', is_revolute ? 0 : 3); + + QVariant vbounds = index.data(VariableBoundsRole); + if (vbounds.isValid()) + { + QPointF bounds = vbounds.toPointF(); + const float min = bounds.x(); + const float max = bounds.y(); + + QStyleOptionProgressBar opt; + opt.rect = option.rect; + opt.minimum = 0; + opt.maximum = 1000; + opt.progress = 1000. * (value - min) / (max - min); + opt.text = style_option.text; + opt.textAlignment = style_option.displayAlignment; + opt.textVisible = true; + style->drawControl(QStyle::CE_ProgressBar, &opt, painter); + return; + } + } + + style->drawControl(QStyle::CE_ItemViewItem, &style_option, painter, option.widget); +} + +QWidget* ProgressBarDelegate::createEditor(QWidget* parent, const QStyleOptionViewItem& option, + const QModelIndex& index) const + +{ + if (index.column() == 1) + { + QVariant vbounds = index.data(VariableBoundsRole); + if (vbounds.isValid()) + { + QPointF bounds = vbounds.toPointF(); + float min = bounds.x(); + float max = bounds.y(); + bool is_revolute = (index.data(JointTypeRole).toInt() == moveit::core::JointModel::REVOLUTE); + if (is_revolute) + { + min *= 180. / M_PI; + max *= 180. / M_PI; + } + auto* editor = new ProgressBarEditor(parent, min, max, is_revolute ? 0 : 3); + connect(editor, &ProgressBarEditor::editingFinished, this, &ProgressBarDelegate::commitAndCloseEditor); + connect(editor, &ProgressBarEditor::valueChanged, this, [=](float value) { + const_cast(index.model())->setData(index, value, Qt::EditRole); + }); + return editor; + } + } + return QStyledItemDelegate::createEditor(parent, option, index); +} + +void ProgressBarDelegate::commitAndCloseEditor() +{ + ProgressBarEditor* editor = qobject_cast(sender()); + commitData(editor); + closeEditor(editor); +} + +JointsWidgetEventFilter::JointsWidgetEventFilter(QAbstractItemView* view) : QObject(view) +{ +} + +bool JointsWidgetEventFilter::eventFilter(QObject* target, QEvent* event) +{ + if (event->type() == QEvent::MouseButtonPress) + { + QAbstractItemView* view = qobject_cast(parent()); + QModelIndex index = view->indexAt(static_cast(event)->pos()); + if (index.isValid() && index.column() == 1) // mouse event on any of joint indexes? + { + view->setCurrentIndex(index); + view->edit(index); + return true; // event handled + } + } + return false; +} + +ProgressBarEditor::ProgressBarEditor(QWidget* parent, float min, float max, int digits) + : QWidget(parent), min_(min), max_(max), digits_(digits) +{ + // if left mouse button is pressed, grab all future mouse events until button(s) released + if (QApplication::mouseButtons() & Qt::LeftButton) + this->grabMouse(); +} + +void ProgressBarEditor::paintEvent(QPaintEvent* /*event*/) +{ + QPainter painter(this); + + QStyleOptionProgressBar opt; + opt.rect = rect(); + opt.palette = this->palette(); + opt.minimum = 0; + opt.maximum = 1000; + opt.progress = 1000. * (value_ - min_) / (max_ - min_); + opt.text = QLocale().toString(value_, 'f', digits_); + opt.textAlignment = Qt::AlignRight; + opt.textVisible = true; + style()->drawControl(QStyle::CE_ProgressBar, &opt, &painter); +} + +void ProgressBarEditor::mousePressEvent(QMouseEvent* event) +{ + if (event->button() == Qt::LeftButton) + mouseMoveEvent(event); +} + +void ProgressBarEditor::mouseMoveEvent(QMouseEvent* event) +{ + float v = std::min(max_, std::max(min_, min_ + event->x() * (max_ - min_) / width())); + if (value_ != v) + { + value_ = v; + valueChanged(v); + update(); + } + event->accept(); +} + +void ProgressBarEditor::mouseReleaseEvent(QMouseEvent* event) +{ + if (event->button() == Qt::LeftButton) + { + event->accept(); + editingFinished(); + } +} + +JogSlider::JogSlider(QWidget* parent) : QSlider(parent) +{ + setTimerInterval(50); + setResolution(1000); + setMaximum(1.0); +} + +void JogSlider::setTimerInterval(int ms) +{ + timer_interval_ = ms; +} + +void JogSlider::setResolution(unsigned int resolution) +{ + QSlider::setRange(-resolution, +resolution); +} + +void JogSlider::setMaximum(double value) +{ + maximum_ = value; +} + +void JogSlider::timerEvent(QTimerEvent* event) +{ + QSlider::timerEvent(event); + if (event->timerId() == timer_id_) + triggered(value()); +} + +void JogSlider::mousePressEvent(QMouseEvent* event) +{ + QSlider::mousePressEvent(event); + timer_id_ = startTimer(timer_interval_); +} + +void JogSlider::mouseReleaseEvent(QMouseEvent* event) +{ + killTimer(timer_id_); + QSlider::mouseReleaseEvent(event); + setValue(0); +} + +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui new file mode 100644 index 0000000000..389365e9bd --- /dev/null +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui @@ -0,0 +1,61 @@ + + + MotionPlanningFrameJointsUI + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + + Group joints: + + + + + + + QAbstractItemView::EditKeyPressed + + + QAbstractItemView::NoSelection + + + false + + + false + + + false + + + + + + + Nullspace exploration: + + + + + + + + + + + + + From 9305824f6d568d1a92227c292f71dd5dd42be5de Mon Sep 17 00:00:00 2001 From: Jeroen Date: Fri, 29 Nov 2019 15:42:14 +0100 Subject: [PATCH 118/612] Fix doc string OrientationConstraint (#1793) Implementation uses XYZ Euler angles (RPY), but documentation said ZXZ. --- .../include/moveit/kinematic_constraints/kinematic_constraint.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index a716e8bd86..12804a01b1 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -339,7 +339,7 @@ MOVEIT_CLASS_FORWARD(OrientationConstraint); * This class expresses an orientation constraint on a particular * link. The constraint is specified in terms of a quaternion, with * tolerances on X,Y, and Z axes. The rotation difference is computed - * based on the ZXZ Euler angle formulation. The header on the + * based on the XYZ Euler angle formulation (intrinsic rotations). The header on the * quaternion can be specified in terms of either a fixed frame or a * mobile frame. The type value will return ORIENTATION_CONSTRAINT. * From 609923adaada4a195a62ae74d3410b61e80c6ed4 Mon Sep 17 00:00:00 2001 From: JonasTietz Date: Sun, 1 Dec 2019 12:56:34 +0100 Subject: [PATCH 119/612] TfPublisher: fixup and add attached collsion objects (#1792) * fixed transform timestamp * The move_group TfPublisher capability now also publishes attached collision objects and their subframes to the tf system --- .../tf_publisher_capability.cpp | 51 +++++++++++++++---- 1 file changed, 41 insertions(+), 10 deletions(-) diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp index 13a0e3480f..bb666dda04 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -39,6 +39,8 @@ #include #include #include +#include +#include namespace move_group { @@ -52,6 +54,23 @@ TfPublisher::~TfPublisher() thread_.join(); } +namespace +{ +void publishSubframes(tf2_ros::TransformBroadcaster& broadcaster, const moveit::core::FixedTransformsMap& subframes, + const std::string& parent_frame, const ros::Time& stamp) +{ + geometry_msgs::TransformStamped transform; + for (auto& subframe : subframes) + { + transform = tf2::eigenToTransform(subframe.second); + transform.child_frame_id = parent_frame + "/" + subframe.first; + transform.header.stamp = stamp; + transform.header.frame_id = parent_frame; + broadcaster.sendTransform(transform); + } +} +} // namespace + void TfPublisher::publishPlanningSceneFrames() { tf2_ros::TransformBroadcaster broadcaster; @@ -61,26 +80,38 @@ void TfPublisher::publishPlanningSceneFrames() while (keep_running_) { { + ros::Time stamp = ros::Time::now(); planning_scene_monitor::LockedPlanningSceneRO locked_planning_scene(context_->planning_scene_monitor_); collision_detection::WorldConstPtr world = locked_planning_scene->getWorld(); std::string planning_frame = locked_planning_scene->getPlanningFrame(); for (const auto& obj : *world) { - std::string parent_frame = prefix_ + obj.second->id_; + std::string object_frame = prefix_ + obj.second->id_; transform = tf2::eigenToTransform(obj.second->shape_poses_[0]); - transform.child_frame_id = parent_frame; + transform.child_frame_id = object_frame; + transform.header.stamp = stamp; transform.header.frame_id = planning_frame; broadcaster.sendTransform(transform); - moveit::core::FixedTransformsMap subframes = obj.second->subframe_poses_; - for (auto& subframe : subframes) - { - transform = tf2::eigenToTransform(subframe.second); - transform.child_frame_id = parent_frame + "/" + subframe.first; - transform.header.frame_id = parent_frame; - broadcaster.sendTransform(transform); - } + const moveit::core::FixedTransformsMap& subframes = obj.second->subframe_poses_; + publishSubframes(broadcaster, subframes, object_frame, stamp); + } + + const robot_state::RobotState& rs = locked_planning_scene->getCurrentState(); + std::vector attached_collision_objects; + rs.getAttachedBodies(attached_collision_objects); + for (const robot_state::AttachedBody* attached_body : attached_collision_objects) + { + std::string object_frame = prefix_ + attached_body->getName(); + transform = tf2::eigenToTransform(attached_body->getFixedTransforms()[0]); + transform.child_frame_id = object_frame; + transform.header.stamp = stamp; + transform.header.frame_id = attached_body->getAttachedLinkName(); + broadcaster.sendTransform(transform); + + const moveit::core::FixedTransformsMap& subframes = attached_body->getSubframeTransforms(); + publishSubframes(broadcaster, subframes, object_frame, stamp); } } From 90bd960ff0fcd49b4d9c61a25162918560346f8c Mon Sep 17 00:00:00 2001 From: Chittaranjan Srinivas Swaminathan Date: Mon, 2 Dec 2019 18:48:44 +0100 Subject: [PATCH 120/612] Add Missing License (#1779) --- .../src/chomp_planning_context.cpp | 39 ++++++++++++++++--- 1 file changed, 34 insertions(+), 5 deletions(-) diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp index 248435dee9..35ddbc8a63 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp @@ -1,9 +1,38 @@ -/* - * chomp_planning_context.cpp +/********************************************************************* + * Software License Agreement (BSD License) * - * Created on: 27-Jul-2016 - * Author: ace - */ + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ + +/* Author: Chittaranjan Srinivas Swaminathan */ #include #include From 5fab74029a09444269c8a427822cc8f783de6439 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Tue, 3 Dec 2019 01:48:39 +0300 Subject: [PATCH 121/612] Fix totg bug giving invalid accelerations (#1729) --- .../time_optimal_trajectory_generation.cpp | 6 ++ ...est_time_optimal_trajectory_generation.cpp | 77 +++++++++++++++++++ 2 files changed, 83 insertions(+) diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 68fe5a8ccf..19d1c38cab 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -538,6 +538,12 @@ bool Trajectory::integrateForward(std::list& trajectory, double if (next_discontinuity != switching_points.end() && path_pos > next_discontinuity->first) { + // Avoid having a TrajectoryStep with path_pos near a switching point which will cause an almost identical + // TrajectoryStep get added in the next run (https://github.com/ros-planning/moveit/issues/1665) + if (path_pos - next_discontinuity->first < EPS) + { + continue; + } path_vel = old_path_vel + (next_discontinuity->first - old_path_pos) * (path_vel - old_path_vel) / (path_pos - old_path_pos); path_pos = next_discontinuity->first; diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp index d05a423ce9..7413afac44 100644 --- a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp @@ -150,6 +150,83 @@ TEST(time_optimal_trajectory_generation, test3) EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]); } +// Test that totg algorithm doesn't give large acceleration +TEST(time_optimal_trajectory_generation, testLargeAccel) +{ + double path_tolerance = 0.1; + double resample_dt = 0.1; + Eigen::VectorXd waypoint(6); + std::list waypoints; + Eigen::VectorXd max_velocities(6); + Eigen::VectorXd max_accelerations(6); + + // Waypoints + // clang-format off + waypoint << 1.6113056281076339, + -0.21400163389235427, + -1.974502599739185, + 9.9653618690354051e-12, + -1.3810916877429624, + 1.5293902838041467; + waypoints.push_back(waypoint); + + waypoint << 1.6088016187976597, + -0.21792862470933924, + -1.9758628799742952, + 0.00010424017303217738, + -1.3835690515335755, + 1.5279972853269816; + waypoints.push_back(waypoint); + + waypoint << 1.5887695443178671, + -0.24934455124521923, + -1.9867451218551782, + 0.00093816147756670078, + -1.4033879618584812, + 1.5168532975096607; + waypoints.push_back(waypoint); + + waypoint << 1.1647412393815282, + -0.91434018564402375, + -2.2170946337498498, + 0.018590164397622583, + -1.8229041212673529, + 1.2809632867583278; + waypoints.push_back(waypoint); + + // Max velocities + max_velocities << 0.89535390627300004, + 0.89535390627300004, + 0.79587013890930003, + 0.92022484811399996, + 0.82074108075029995, + 1.3927727430915; + // Max accelerations + max_accelerations << 0.82673490883799994, + 0.78539816339699997, + 0.60883578557700002, + 3.2074759432319997, + 1.4398966328939999, + 4.7292792634680003; + // clang-format on + + Trajectory parameterized(Path(waypoints, path_tolerance), max_velocities, max_accelerations, 0.001); + + ASSERT_TRUE(parameterized.isValid()); + + size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt); + for (size_t sample = 0; sample <= sample_count; ++sample) + { + // always sample the end of the trajectory as well + double t = std::min(parameterized.getDuration(), sample * resample_dt); + Eigen::VectorXd acceleration = parameterized.getAcceleration(t); + + ASSERT_EQ(acceleration.size(), 6); + for (std::size_t i = 0; i < 6; ++i) + EXPECT_NEAR(acceleration(i), 0.0, 100.0) << "Invalid acceleration at position " << sample_count << "\n"; + } +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 58804ed9ad78e02b07bf57a9c8f9fb558103ef1c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 26 Nov 2019 12:55:29 +0100 Subject: [PATCH 122/612] Handle incomplete group states - issue a warning when building the robot model - use a RobotState initialized to joint defaults for fake controllers --- moveit_core/robot_model/src/robot_model.cpp | 7 +++++++ .../src/moveit_fake_controller_manager.cpp | 3 ++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index d67af3b052..2abd64c912 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -327,6 +327,7 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) if (hasJointModelGroup(group_state.group_)) { JointModelGroup* jmg = getJointModelGroup(group_state.group_); + std::vector remaining_joints = jmg->getActiveJointModels(); std::map state; for (std::map>::const_iterator jt = group_state.joint_values_.begin(); jt != group_state.joint_values_.end(); ++jt) @@ -335,6 +336,10 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) { const JointModel* jm = jmg->getJointModel(jt->first); const std::vector& vn = jm->getVariableNames(); + // Remove current joint name from remaining list. + auto it_found = std::find(remaining_joints.begin(), remaining_joints.end(), jm); + if (it_found != remaining_joints.end()) + remaining_joints.erase(it_found); if (vn.size() == jt->second.size()) for (std::size_t j = 0; j < vn.size(); ++j) state[vn[j]] = jt->second[j]; @@ -349,6 +354,8 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) "but that joint is not part of group '%s'", group_state.name_.c_str(), jt->first.c_str(), jmg->getName().c_str()); } + if (!remaining_joints.empty()) + ROS_WARN_NAMED(LOGNAME, "Group state '%s' doesn't specify all group joints.", group_state.name_.c_str()); if (!state.empty()) jmg->addDefaultState(group_state.name_, state); } diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp index ee375ead3a..f1fb8a7db4 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp @@ -136,9 +136,11 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont robot_model_loader::RobotModelLoader robot_model_loader(ROBOT_DESCRIPTION); const robot_model::RobotModelPtr& robot_model = robot_model_loader.getModel(); + moveit::core::RobotState robot_state(robot_model); typedef std::map JointPoseMap; JointPoseMap joints; + robot_state.setToDefaultValues(); // initialize all joint values (just in case...) for (int i = 0, end = param.size(); i != end; ++i) { try @@ -151,7 +153,6 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont continue; } moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group_name); - moveit::core::RobotState robot_state(robot_model); const std::vector& joint_names = jmg->getActiveJointModelNames(); if (!robot_state.setToDefaultValues(jmg, pose_name)) From 601f550ad416e72755308dbfd0d7889c08f6fb08 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Sat, 30 Nov 2019 14:19:51 +0300 Subject: [PATCH 123/612] Fix flaky moveit_cpp test - update robot state before planning - release tf_listener_ and tf_buffer_ in correct order - publish virtual joint --- moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp | 1 + .../planning_interface/moveit_cpp/src/planning_component.cpp | 1 + moveit_ros/planning_interface/test/CMakeLists.txt | 5 ++--- moveit_ros/planning_interface/test/moveit_cpp_test.cpp | 3 --- moveit_ros/planning_interface/test/moveit_cpp_test.test | 2 +- 5 files changed, 5 insertions(+), 7 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index dfa5d12bf5..3c0da8bf5f 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -319,6 +319,7 @@ const std::shared_ptr& MoveItCpp::getTFBuffer() const void MoveItCpp::clearContents() { + tf_listener_.reset(); tf_buffer_.reset(); planning_scene_monitor_.reset(); robot_model_.reset(); diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp index 896ecb783d..ea57f7a833 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -160,6 +160,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet moveit::core::RobotStatePtr start_state = considered_start_state_; if (!start_state) start_state = moveit_cpp_->getCurrentState(); + start_state->update(); moveit::core::robotStateToRobotStateMsg(*start_state, req.start_state); planning_scene->setCurrentState(*start_state); diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 37ff35f2f1..eff482cec1 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -5,9 +5,8 @@ if (CATKIN_ENABLE_TESTING) add_executable(test_cleanup test_cleanup.cpp) target_link_libraries(test_cleanup moveit_move_group_interface) - # TODO: Fix flaky test - #add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) - #target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) + add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) + target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) add_rostest(python_move_group.test) add_rostest(python_move_group_ns.test) diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp index ebea62a82d..675d7c761b 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp @@ -130,9 +130,6 @@ TEST_F(MoveItCppTest, TestSetStartStateToCurrentState) // Test setting the goal using geometry_msgs::PoseStamped and a robot's link name TEST_F(MoveItCppTest, TestSetGoalFromPoseStamped) { - planning_component_ptr->setStartStateToCurrentState(); - - geometry_msgs::PoseStamped target_pose1; planning_component_ptr->setGoal(target_pose1, "panda_link8"); ASSERT_TRUE(static_cast(planning_component_ptr->plan())); diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.test b/moveit_ros/planning_interface/test/moveit_cpp_test.test index dac1ee0807..2e3d327b6a 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.test +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.test @@ -19,7 +19,7 @@ - + From 3adbfe9de007218fbe1548530cb23ae962361222 Mon Sep 17 00:00:00 2001 From: Mark Moll Date: Thu, 5 Dec 2019 13:58:32 -0700 Subject: [PATCH 124/612] Add support for multi-query planning in OMPL (#1799) If enabled, the internal state of of planners does not get cleared. This can be enabled per planner. It only makes sense for roadmap-based planners (or if the start state does not change). Additionally, for PRM, PRM*, LazyPRM, and LazyPRM* it is possible (when using OMPL 1.5.0) to load/save roadmaps from disk upon creation/destruction. This is set in ompl_planning.yaml like so: planner_configs: PersistentPRM: type: geometric::PRM multi_query_planning_enabled: true store_planner_data: true load_planner_data: true planner_data_path: /tmp/roadmap.graph This commit also fixes a bug for PRM-family planners where they would always run as optimizing planners and not terminate when the first solution was found. This occurs when the default optimization objective is set when none are specified. The fix is to not set an objective when none are specified. Co-Authored-By: Robert Haschke --- .../model_based_planning_context.h | 3 + .../ompl_interface/planning_context_manager.h | 36 +++ .../src/model_based_planning_context.cpp | 76 ++++--- .../src/planning_context_manager.cpp | 212 ++++++++++++++---- 4 files changed, 249 insertions(+), 78 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index 2e5a046070..65acf98d8f 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -418,6 +418,9 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext bool use_state_validity_cache_; + /// when false, clears planners before running solve() + bool multi_query_planning_enabled_; + ConstraintsLibraryPtr constraints_library_; bool simplify_solutions_; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index 29a39e2542..8bff59c4dd 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -41,12 +41,42 @@ #include #include +#include + #include #include #include namespace ompl_interface { +class MultiQueryPlannerAllocator +{ +public: + MultiQueryPlannerAllocator() = default; + ~MultiQueryPlannerAllocator(); + + template + ob::PlannerPtr allocatePlanner(const ob::SpaceInformationPtr& si, const std::string& new_name, + const ModelBasedPlanningContextSpecification& spec); + +private: + template + ob::PlannerPtr allocatePlannerImpl(const ob::SpaceInformationPtr& si, const std::string& new_name, + const ModelBasedPlanningContextSpecification& spec, bool load_planner_data = false, + bool store_planner_data = false, const std::string& file_path = ""); + + template + inline ob::Planner* allocatePersistentPlanner(const ob::PlannerData& data); + + // Storing multi-query planners + std::map planners_; + + std::map planner_data_storage_paths_; + + // Store and load planner data + ob::PlannerDataStorage storage_; +}; + class PlanningContextManager { public: @@ -175,6 +205,9 @@ class PlanningContextManager void registerDefaultPlanners(); void registerDefaultStateSpaces(); + template + void registerPlannerAllocatorHelper(const std::string& planner_id); + /** \brief This is the function that constructs new planning contexts if no previous ones exist that are suitable */ ModelBasedPlanningContextPtr getPlanningContext(const planning_interface::PlannerConfigurationSettings& config, const StateSpaceFactoryTypeSelector& factory_selector, @@ -221,6 +254,9 @@ class PlanningContextManager /// needed) unsigned int minimum_waypoint_count_; + /// Multi-query planner allocator + MultiQueryPlannerAllocator planner_allocator_; + private: MOVEIT_STRUCT_FORWARD(CachedContexts); CachedContextsPtr cached_contexts_; diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 10534e1d06..6528ea8bd2 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -89,6 +89,7 @@ ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std:: , max_solution_segment_length_(0.0) , minimum_waypoint_count_(0) , use_state_validity_cache_(true) + , multi_query_planning_enabled_(false) // maintain "old" behavior by default , simplify_solutions_(true) { complete_initial_robot_state_.update(); @@ -298,44 +299,43 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() std::string optimizer; ompl::base::OptimizationObjectivePtr objective; it = cfg.find("optimization_objective"); - if (it == cfg.end()) - { - optimizer = "PathLengthOptimizationObjective"; - ROS_DEBUG_NAMED("model_based_planning_context", "No optimization objective specified, defaulting to %s", - optimizer.c_str()); - } - else + if (it != cfg.end()) { optimizer = it->second; cfg.erase(it); - } - if (optimizer == "PathLengthOptimizationObjective") - { - objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); - } - else if (optimizer == "MinimaxObjective") - { - objective.reset(new ompl::base::MinimaxObjective(ompl_simple_setup_->getSpaceInformation())); - } - else if (optimizer == "StateCostIntegralObjective") - { - objective.reset(new ompl::base::StateCostIntegralObjective(ompl_simple_setup_->getSpaceInformation())); - } - else if (optimizer == "MechanicalWorkOptimizationObjective") - { - objective.reset(new ompl::base::MechanicalWorkOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); - } - else if (optimizer == "MaximizeMinClearanceObjective") - { - objective.reset(new ompl::base::MaximizeMinClearanceObjective(ompl_simple_setup_->getSpaceInformation())); - } - else - { - objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); + if (optimizer == "PathLengthOptimizationObjective") + { + objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); + } + else if (optimizer == "MinimaxObjective") + { + objective.reset(new ompl::base::MinimaxObjective(ompl_simple_setup_->getSpaceInformation())); + } + else if (optimizer == "StateCostIntegralObjective") + { + objective.reset(new ompl::base::StateCostIntegralObjective(ompl_simple_setup_->getSpaceInformation())); + } + else if (optimizer == "MechanicalWorkOptimizationObjective") + { + objective.reset(new ompl::base::MechanicalWorkOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); + } + else if (optimizer == "MaximizeMinClearanceObjective") + { + objective.reset(new ompl::base::MaximizeMinClearanceObjective(ompl_simple_setup_->getSpaceInformation())); + } + else + { + objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); + } + + ompl_simple_setup_->setOptimizationObjective(objective); } - ompl_simple_setup_->setOptimizationObjective(objective); + // Don't clear planner data if multi-query planning is enabled + it = cfg.find("multi_query_planning_enabled"); + if (it != cfg.end()) + multi_query_planning_enabled_ = boost::lexical_cast(it->second); // remove the 'type' parameter; the rest are parameters for the planner itself it = cfg.find("type"); @@ -349,8 +349,9 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() { std::string type = it->second; cfg.erase(it); - ompl_simple_setup_->setPlannerAllocator(std::bind(spec_.planner_selector_(type), std::placeholders::_1, - name_ != getGroupName() ? name_ : "", std::cref(spec_))); + const std::string planner_name = getGroupName() + "/" + name_; + ompl_simple_setup_->setPlannerAllocator( + std::bind(spec_.planner_selector_(type), std::placeholders::_1, planner_name, std::cref(spec_))); ROS_INFO_NAMED("model_based_planning_context", "Planner configuration '%s' will use planner '%s'. " "Additional configuration parameters will be set when the planner is constructed.", @@ -534,7 +535,8 @@ void ompl_interface::ModelBasedPlanningContext::setCompleteInitialState( void ompl_interface::ModelBasedPlanningContext::clear() { - ompl_simple_setup_->clear(); + if (!multi_query_planning_enabled_) + ompl_simple_setup_->clear(); ompl_simple_setup_->clearStartStates(); ompl_simple_setup_->setGoal(ob::GoalPtr()); ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr()); @@ -627,7 +629,7 @@ void ompl_interface::ModelBasedPlanningContext::preSolve() // clear previously computed solutions ompl_simple_setup_->getProblemDefinition()->clearSolutionPaths(); const ob::PlannerPtr planner = ompl_simple_setup_->getPlanner(); - if (planner) + if (planner && !multi_query_planning_enabled_) planner->clear(); startSampling(); ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter(); @@ -726,7 +728,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i preSolve(); bool result = false; - if (count <= 1) + if (count <= 1 || multi_query_planning_enabled_) // multi-query planners should always run in single instances { ROS_DEBUG_NAMED("model_based_planning_context", "%s: Solving the planning problem once...", name_.c_str()); ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 3d6c6b21e3..fa2e88a308 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -85,6 +85,143 @@ struct PlanningContextManager::CachedContexts } // namespace ompl_interface +ompl_interface::MultiQueryPlannerAllocator::~MultiQueryPlannerAllocator() +{ + // Store all planner data + for (const auto& entry : planner_data_storage_paths_) + { + ROS_INFO("Storing planner data"); + ob::PlannerData data(planners_[entry.first]->getSpaceInformation()); + planners_[entry.first]->getPlannerData(data); + storage_.store(data, entry.second.c_str()); + } +} + +template +ompl::base::PlannerPtr ompl_interface::MultiQueryPlannerAllocator::allocatePlanner( + const ob::SpaceInformationPtr& si, const std::string& new_name, const ModelBasedPlanningContextSpecification& spec) +{ + // Store planner instance if multi-query planning is enabled + auto cfg = spec.config_; + auto it = cfg.find("multi_query_planning_enabled"); + bool multi_query_planning_enabled = false; + if (it != cfg.end()) + { + multi_query_planning_enabled = boost::lexical_cast(it->second); + cfg.erase(it); + } + if (multi_query_planning_enabled) + { + // If we already have an instance, use that one + auto planner_map_it = planners_.find(new_name); + if (planner_map_it != planners_.end()) + return planner_map_it->second; + + // Certain multi-query planners allow loading and storing the generated planner data. This feature can be + // selectively enabled for loading and storing using the bool parameters 'load_planner_data' and + // 'store_planner_data'. The storage file path is set using the parameter 'planner_data_path'. + // File read and write access are handled by the PlannerDataStorage class. If the file path is invalid + // an error message is printed and the planner is constructed/destructed with default values. + it = cfg.find("load_planner_data"); + bool load_planner_data = false; + if (it != cfg.end()) + { + load_planner_data = boost::lexical_cast(it->second); + cfg.erase(it); + } + it = cfg.find("store_planner_data"); + bool store_planner_data = false; + if (it != cfg.end()) + { + store_planner_data = boost::lexical_cast(it->second); + cfg.erase(it); + } + it = cfg.find("planner_data_path"); + std::string planner_data_path; + if (it != cfg.end()) + { + planner_data_path = it->second; + cfg.erase(it); + } + // Store planner instance for multi-query use + planners_[new_name] = + allocatePlannerImpl(si, new_name, spec, load_planner_data, store_planner_data, planner_data_path); + return planners_[new_name]; + } + else + { + // Return single-shot planner instance + return allocatePlannerImpl(si, new_name, spec); + } +} + +template +ompl::base::PlannerPtr ompl_interface::MultiQueryPlannerAllocator::allocatePlannerImpl( + const ob::SpaceInformationPtr& si, const std::string& new_name, const ModelBasedPlanningContextSpecification& spec, + bool load_planner_data, bool store_planner_data, const std::string& file_path) +{ + ob::PlannerPtr planner; + // Try to initialize planner with loaded planner data + if (load_planner_data) + { + ROS_INFO("Loading planner data"); + ob::PlannerData data(si); + storage_.load(file_path.c_str(), data); + planner.reset(allocatePersistentPlanner(data)); + if (!planner) + ROS_ERROR_NAMED("planning_context_manager", + "Creating a '%s' planner from persistent data is not supported. Going to create a new instance.", + new_name.c_str()); + } + if (!planner) + planner.reset(new T(si)); + if (!new_name.empty()) + planner->setName(new_name); + planner->params().setParams(spec.config_, true); + // Remember which planner instances to store when the destructor is called + if (store_planner_data) + planner_data_storage_paths_[new_name] = file_path; + return planner; +} + +// default implementation +template +inline ompl::base::Planner* +ompl_interface::MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& /*data*/) +{ + return nullptr; +}; +// TODO: remove when ROS Melodic and older are no longer supported +#if OMPL_VERSION_VALUE >= 1005000 +template <> +inline ompl::base::Planner* +ompl_interface::MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) +{ + return new og::PRM(data); +}; +template <> +inline ompl::base::Planner* +ompl_interface::MultiQueryPlannerAllocator::allocatePersistentPlanner( + const ob::PlannerData& data) +{ + return new og::PRMstar(data); +}; +template <> +inline ompl::base::Planner* +ompl_interface::MultiQueryPlannerAllocator::allocatePersistentPlanner( + const ob::PlannerData& data) +{ + return new og::LazyPRM(data); +}; +template <> +inline ompl::base::Planner* +ompl_interface::MultiQueryPlannerAllocator::allocatePersistentPlanner( + const ob::PlannerData& data) +{ + return new og::LazyPRMstar(data); +}; +#endif + ompl_interface::PlanningContextManager::PlanningContextManager(robot_model::RobotModelConstPtr robot_model, constraint_samplers::ConstraintSamplerManagerPtr csm) : robot_model_(std::move(robot_model)) @@ -103,22 +240,6 @@ ompl_interface::PlanningContextManager::PlanningContextManager(robot_model::Robo ompl_interface::PlanningContextManager::~PlanningContextManager() = default; -namespace -{ -using namespace ompl_interface; - -template -static ompl::base::PlannerPtr allocatePlanner(const ob::SpaceInformationPtr& si, const std::string& new_name, - const ModelBasedPlanningContextSpecification& spec) -{ - ompl::base::PlannerPtr planner(new T(si)); - if (!new_name.empty()) - planner->setName(new_name); - planner->params().setParams(spec.config_, true); - return planner; -} -} // namespace - ompl_interface::ConfiguredPlannerAllocator ompl_interface::PlanningContextManager::plannerSelector(const std::string& planner) const { @@ -132,33 +253,42 @@ ompl_interface::PlanningContextManager::plannerSelector(const std::string& plann } } +template +void ompl_interface::PlanningContextManager::registerPlannerAllocatorHelper(const std::string& planner_id) +{ + registerPlannerAllocator(planner_id, [&](const ob::SpaceInformationPtr& si, const std::string& new_name, + const ModelBasedPlanningContextSpecification& spec) { + return planner_allocator_.allocatePlanner(si, new_name, spec); + }); +} + void ompl_interface::PlanningContextManager::registerDefaultPlanners() { - registerPlannerAllocator("geometric::AnytimePathShortening", allocatePlanner); - registerPlannerAllocator("geometric::BFMT", allocatePlanner); - registerPlannerAllocator("geometric::BiEST", allocatePlanner); - registerPlannerAllocator("geometric::BiTRRT", allocatePlanner); - registerPlannerAllocator("geometric::BKPIECE", allocatePlanner); - registerPlannerAllocator("geometric::EST", allocatePlanner); - registerPlannerAllocator("geometric::FMT", allocatePlanner); - registerPlannerAllocator("geometric::KPIECE", allocatePlanner); - registerPlannerAllocator("geometric::LazyPRM", allocatePlanner); - registerPlannerAllocator("geometric::LazyPRMstar", allocatePlanner); - registerPlannerAllocator("geometric::LazyRRT", allocatePlanner); - registerPlannerAllocator("geometric::LBKPIECE", allocatePlanner); - registerPlannerAllocator("geometric::LBTRRT", allocatePlanner); - registerPlannerAllocator("geometric::PDST", allocatePlanner); - registerPlannerAllocator("geometric::PRM", allocatePlanner); - registerPlannerAllocator("geometric::PRMstar", allocatePlanner); - registerPlannerAllocator("geometric::ProjEST", allocatePlanner); - registerPlannerAllocator("geometric::RRT", allocatePlanner); - registerPlannerAllocator("geometric::RRTConnect", allocatePlanner); - registerPlannerAllocator("geometric::RRTstar", allocatePlanner); - registerPlannerAllocator("geometric::SBL", allocatePlanner); - registerPlannerAllocator("geometric::SPARS", allocatePlanner); - registerPlannerAllocator("geometric::SPARStwo", allocatePlanner); - registerPlannerAllocator("geometric::STRIDE", allocatePlanner); - registerPlannerAllocator("geometric::TRRT", allocatePlanner); + registerPlannerAllocatorHelper("geometric::AnytimePathShortening"); + registerPlannerAllocatorHelper("geometric::BFMT"); + registerPlannerAllocatorHelper("geometric::BiEST"); + registerPlannerAllocatorHelper("geometric::BiTRRT"); + registerPlannerAllocatorHelper("geometric::BKPIECE"); + registerPlannerAllocatorHelper("geometric::EST"); + registerPlannerAllocatorHelper("geometric::FMT"); + registerPlannerAllocatorHelper("geometric::KPIECE"); + registerPlannerAllocatorHelper("geometric::LazyPRM"); + registerPlannerAllocatorHelper("geometric::LazyPRMstar"); + registerPlannerAllocatorHelper("geometric::LazyRRT"); + registerPlannerAllocatorHelper("geometric::LBKPIECE"); + registerPlannerAllocatorHelper("geometric::LBTRRT"); + registerPlannerAllocatorHelper("geometric::PDST"); + registerPlannerAllocatorHelper("geometric::PRM"); + registerPlannerAllocatorHelper("geometric::PRMstar"); + registerPlannerAllocatorHelper("geometric::ProjEST"); + registerPlannerAllocatorHelper("geometric::RRT"); + registerPlannerAllocatorHelper("geometric::RRTConnect"); + registerPlannerAllocatorHelper("geometric::RRTstar"); + registerPlannerAllocatorHelper("geometric::SBL"); + registerPlannerAllocatorHelper("geometric::SPARS"); + registerPlannerAllocatorHelper("geometric::SPARStwo"); + registerPlannerAllocatorHelper("geometric::STRIDE"); + registerPlannerAllocatorHelper("geometric::TRRT"); } void ompl_interface::PlanningContextManager::registerDefaultStateSpaces() From fe39430d971fdfadcec0c7316fa67d41bf48b281 Mon Sep 17 00:00:00 2001 From: Mark Moll Date: Sat, 7 Dec 2019 13:23:41 -0700 Subject: [PATCH 125/612] clear roadmap validity for LazyPRM/LazyPRMstar when using multi-query planning is enabled (#1802) * clear roadmap validity for LazyPRM/LazyPRMstar when using multi-query planning is enabled * add #if guard for OMPL version --- .../src/model_based_planning_context.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 6528ea8bd2..bd64bfadcd 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -70,6 +70,7 @@ #include "ompl/base/objectives/MinimaxObjective.h" #include "ompl/base/objectives/StateCostIntegralObjective.h" #include "ompl/base/objectives/MaximizeMinClearanceObjective.h" +#include ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std::string& name, const ModelBasedPlanningContextSpecification& spec) @@ -537,6 +538,19 @@ void ompl_interface::ModelBasedPlanningContext::clear() { if (!multi_query_planning_enabled_) ompl_simple_setup_->clear(); +// TODO: remove when ROS Melodic and older are no longer supported +#if OMPL_VERSION_VALUE >= 1005000 + else + { + // For LazyPRM and LazyPRMstar we assume that the environment *could* have changed + // This means that we need to reset the validity flags for every node and edge in + // the roadmap. For PRM and PRMstar we assume that the environment is static. If + // this is not the case, then multi-query planning should not be enabled. + auto planner = dynamic_cast(ompl_simple_setup_->getPlanner().get()); + if (planner != nullptr) + planner->clearValidity(); + } +#endif ompl_simple_setup_->clearStartStates(); ompl_simple_setup_->setGoal(ob::GoalPtr()); ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr()); From 76c16e61374ee7a80cc8e738b57a181da34f7c42 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 11 Dec 2019 13:50:31 -0600 Subject: [PATCH 126/612] jog_arm: add a C++ API (#1763) * Example to launch C++ interface in a new thread * A better multithread example Start a new folder, src/cpp_interface_example * Update license formatting in new files only * JogROSInterface inherits from a base class * Base interface class is don * The new API half-works :P * Sharing data between threads workks * It works! The arm moves * Add C++ interface launch file * Implement joint commands * Debug the demo transition from cartesian to joint commands * Add an example of retrieving the current joint state * Clang format * Satisfy linter re. function names * Ensure the library is available to other packages * Update license formatting again * Clang format * Switch pthread to std::thread, change C++14 compiler options * Clang format --- .../moveit_jog_arm/CMakeLists.txt | 64 ++++- .../config/ur_simulated_config.yaml | 2 +- .../moveit_jog_arm/collision_check_thread.h | 76 ++--- .../include/moveit_jog_arm/jog_arm_data.h | 75 ++--- .../include/moveit_jog_arm/jog_calcs.h | 80 +++--- .../moveit_jog_arm/jog_cpp_interface.h | 70 +++++ .../moveit_jog_arm/jog_interface_base.h | 78 ++++++ .../moveit_jog_arm/jog_ros_interface.h | 105 +++---- .../include/moveit_jog_arm/low_pass_filter.h | 74 ++--- .../launch/cpp_interface_example.launch | 9 + .../moveit_jog_arm/launch/spacenav_cpp.launch | 2 +- .../launch/spacenav_teleop_tools.launch | 2 +- .../src/collision_check_thread.cpp | 88 +++--- .../cpp_interface_example.cpp | 98 +++++++ .../moveit_jog_arm/src/jog_calcs.cpp | 125 +++++---- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 200 ++++++++++++++ .../moveit_jog_arm/src/jog_interface_base.cpp | 201 ++++++++++++++ .../moveit_jog_arm/src/jog_ros_interface.cpp | 259 ++++-------------- .../moveit_jog_arm/src/jog_server.cpp | 74 ++--- .../moveit_jog_arm/src/low_pass_filter.cpp | 74 ++--- .../src/teleop_examples/spacenav_to_twist.cpp | 76 ++--- 21 files changed, 1176 insertions(+), 656 deletions(-) create mode 100644 moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h create mode 100644 moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h create mode 100644 moveit_experimental/moveit_jog_arm/launch/cpp_interface_example.launch create mode 100644 moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp create mode 100644 moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp create mode 100644 moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index 899efdad17..3502c17a89 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -1,7 +1,11 @@ cmake_minimum_required(VERSION 2.8.3) project(moveit_jog_arm) -add_compile_options(-std=c++14) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +set(LIBRARY_NAME jog_arm_cpp_api) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -16,11 +20,11 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(Eigen3 REQUIRED) - catkin_package( INCLUDE_DIRS include LIBRARIES + ${LIBRARY_NAME} CATKIN_DEPENDS control_msgs geometry_msgs @@ -37,31 +41,83 @@ include_directories( ${EIGEN3_INCLUDE_DIRS} ) +######################################### +## A library providing a C++ interface ## +######################################### + +add_library(${LIBRARY_NAME} SHARED + src/collision_check_thread.cpp + src/jog_calcs.cpp + src/jog_cpp_interface.cpp + src/jog_interface_base.cpp + src/low_pass_filter.cpp +) +add_dependencies(${LIBRARY_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${LIBRARY_NAME} + ${catkin_LIBRARIES} + ${Eigen_LIBRARIES} +) + +# An example of using the C++ library +add_executable(cpp_interface_example + src/collision_check_thread.cpp + src/cpp_interface_example/cpp_interface_example.cpp + src/jog_calcs.cpp + src/low_pass_filter.cpp +) +add_dependencies(cpp_interface_example ${catkin_EXPORTED_TARGETS}) +target_link_libraries(cpp_interface_example + ${catkin_LIBRARIES} + ${Eigen_LIBRARIES} + ${LIBRARY_NAME} +) + +############################ +## ROS message-based node ## +############################ + add_executable(jog_server src/collision_check_thread.cpp src/jog_server.cpp src/jog_calcs.cpp + src/jog_interface_base.cpp src/jog_ros_interface.cpp src/low_pass_filter.cpp ) - add_dependencies(jog_server ${catkin_EXPORTED_TARGETS}) target_link_libraries(jog_server ${catkin_LIBRARIES} ${Eigen_LIBRARIES}) +################################################ +## An example of converting joystick commands ## +################################################ + add_executable(spacenav_to_twist src/teleop_examples/spacenav_to_twist.cpp ) add_dependencies(spacenav_to_twist ${catkin_EXPORTED_TARGETS}) target_link_libraries(spacenav_to_twist ${catkin_LIBRARIES}) +################## +## INSTALLATION ## +################## + install( TARGETS + ${LIBRARY_NAME} jog_server spacenav_to_twist + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +############# +## TESTING ## +############# if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) diff --git a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml index 839a4b16a5..fe09ddf6ac 100644 --- a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml +++ b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml @@ -31,7 +31,7 @@ move_group_name: manipulator # Often 'manipulator' or 'arm' planning_frame: world # The MoveIt! planning frame. Often 'base_link' ## Stopping behaviour -incoming_command_timeout: 2 # Stop jogging if X seconds elapse without a new cmd +incoming_command_timeout: 0.1 # Stop jogging if X seconds elapse without a new cmd # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. # Important because ROS may drop some messages and we need the robot to halt reliably. num_halt_msgs_to_publish: 4 diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index 3c18a34898..4cae9182a7 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -1,40 +1,40 @@ -/* - Title : collision_check_thread.h - Project : moveit_jog_arm - Created : 1/11/2019 - Author : Brian O'Neil, Andy Zelenak, Blake Anderson - -BSD 3-Clause License - -Copyright (c) 2019, Los Alamos National Security, LLC -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 the copyright holder 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 HOLDER 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. -*/ +/******************************************************************************* + * Title : collision_check_thread.h + * Project : moveit_jog_arm + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 the copyright holder 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 HOLDER 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 @@ -49,7 +49,7 @@ class CollisionCheckThread { public: CollisionCheckThread(const moveit_jog_arm::JogArmParameters parameters, - moveit_jog_arm::JogArmShared& shared_variables, pthread_mutex_t& mutex, + moveit_jog_arm::JogArmShared& shared_variables, std::mutex& mutex, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); }; } diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 5b2d384f34..01aa989c73 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -1,45 +1,46 @@ -/* - Title : jog_arm_data.h - Project : moveit_jog_arm - Created : 1/11/2019 - Author : Brian O'Neil, Andy Zelenak, Blake Anderson - -BSD 3-Clause License - -Copyright (c) 2019, Los Alamos National Security, LLC -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 the copyright holder 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 HOLDER 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. -*/ +/******************************************************************************* + * Title : jog_arm_data.h + * Project : moveit_jog_arm + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 the copyright holder 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 HOLDER 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 diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index e80197db22..ff6af3aa45 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -1,40 +1,40 @@ -/* - Title : jog_calcs.h - Project : moveit_jog_arm - Created : 1/11/2019 - Author : Brian O'Neil, Andy Zelenak, Blake Anderson - -BSD 3-Clause License - -Copyright (c) 2019, Los Alamos National Security, LLC -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 the copyright holder 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 HOLDER 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. -*/ +/******************************************************************************* + * Title : jog_calcs.h + * Project : moveit_jog_arm + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 the copyright holder 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 HOLDER 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 @@ -51,7 +51,7 @@ namespace moveit_jog_arm class JogCalcs { public: - JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_variables, pthread_mutex_t& mutex, + JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_variables, std::mutex& mutex, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); protected: @@ -59,7 +59,7 @@ class JogCalcs sensor_msgs::JointState incoming_jts_; - bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables, pthread_mutex_t& mutex); + bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables, std::mutex& mutex); bool jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables); @@ -92,7 +92,7 @@ class JogCalcs const Eigen::JacobiSVD& svd); // Apply velocity scaling for proximity of collisions and singularities - bool applyVelocityScaling(JogArmShared& shared_variables, pthread_mutex_t& mutex, + bool applyVelocityScaling(JogArmShared& shared_variables, std::mutex& mutex, trajectory_msgs::JointTrajectory& new_jt_traj, const Eigen::VectorXd& delta_theta, double singularity_scale); diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h new file mode 100644 index 0000000000..4a8075f921 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h @@ -0,0 +1,70 @@ +/******************************************************************************* + * Title : jog_cpp_interface.h + * Project : moveit_jog_arm + * Created : 11/20/2019 + * Author : Andy Zelenak + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 the copyright holder 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 HOLDER 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 "jog_interface_base.h" + +namespace moveit_jog_arm +{ +/** +* Class JogCppApi - This class should be instantiated in a new thread +* See cpp_interface_example.cpp +*/ +class JogCppApi : JogInterfaceBase +{ +public: + JogCppApi(); + + void mainLoop(); + + // Provide a Cartesian velocity command to the jogger. + // The units are determined by settings in the yaml file. + void provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command); + + // Send joint position(s) commands + void provideJointCommand(const control_msgs::JointJog& joint_command); + + // Returns the most recent JointState that the jogger has received. + // May eliminate the need to create your own joint_state subscriber. + sensor_msgs::JointState getJointState(); + +private: + ros::NodeHandle nh_; +}; +} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h new file mode 100644 index 0000000000..3a39a7f87d --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h @@ -0,0 +1,78 @@ +/******************************************************************************* + * Title : jog_interface_base.cpp + * Project : moveit_jog_arm + * Created : 3/9/2017 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 the copyright holder 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 HOLDER 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 "collision_check_thread.h" +#include +#include "jog_arm_data.h" +#include "jog_calcs.h" +#include "low_pass_filter.h" +#include +#include +#include +#include + +namespace moveit_jog_arm +{ +/** + * Class JogInterfaceBase - Base class for C++ interface and ROS node. + * Handles ROS subs & pubs and creates the worker threads. + */ +class JogInterfaceBase +{ +public: + void jointsCB(const sensor_msgs::JointStateConstPtr& msg); + + // Jogging calculation thread + bool startJogCalcThread(); + + // Collision checking thread + bool startCollisionCheckThread(); + +protected: + bool readParameters(ros::NodeHandle& n); + + robot_model_loader::RobotModelLoaderPtr model_loader_ptr_; + + // Store the parameters that were read from ROS server + JogArmParameters ros_parameters_; + + // Share data between threads + JogArmShared shared_variables_; + std::mutex shared_variables_mutex_; +}; +} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h index 9a3e023c06..f840fc3a99 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h @@ -1,87 +1,60 @@ -/* - Title : jog_ros_interface.h - Project : moveit_jog_arm - Created : 3/9/2017 - Author : Brian O'Neil, Blake Anderson, Andy Zelenak - -BSD 3-Clause License - -Copyright (c) 2018, Los Alamos National Security, LLC -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 the copyright holder 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 HOLDER 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. -*/ +/******************************************************************************* + * Title : jog_ros_interface.h + * Project : moveit_jog_arm + * Created : 3/9/2017 + * Author : Brian O'Neil, Blake Anderson, Andy Zelenak + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 the copyright holder 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 HOLDER 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. +*******************************************************************************/ // Server node for arm jogging with MoveIt. #pragma once -#include "collision_check_thread.h" -#include -#include "jog_arm_data.h" -#include "jog_calcs.h" -#include "low_pass_filter.h" -#include -#include -#include -#include +#include "jog_interface_base.h" namespace moveit_jog_arm { /** * Class JogROSInterface - Instantiated in main(). Handles ROS subs & pubs and creates the worker threads. */ -class JogROSInterface +class JogROSInterface : JogInterfaceBase { public: JogROSInterface(); - ~JogROSInterface(); private: // ROS subscriber callbacks void deltaCartesianCmdCB(const geometry_msgs::TwistStampedConstPtr& msg); void deltaJointCmdCB(const control_msgs::JointJogConstPtr& msg); - void jointsCB(const sensor_msgs::JointStateConstPtr& msg); - - bool readParameters(ros::NodeHandle& n); - - // Jogging calculation thread - bool startJogCalcThread(); - - // Collision checking thread - bool startCollisionCheckThread(); - - // Share data between threads - JogArmShared shared_variables_; - pthread_mutex_t shared_variables_mutex_; - - robot_model_loader::RobotModelLoaderPtr model_loader_ptr_; - - // Store the parameters that were read from ROS server - JogArmParameters ros_parameters_; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h index dd592524c1..1c753cf11e 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h @@ -1,40 +1,40 @@ -/* - Title : low_pass_filter.h - Project : moveit_jog_arm - Created : 1/11/2019 - Author : Andy Zelenak - -BSD 3-Clause License - -Copyright (c) 2019, Los Alamos National Security, LLC -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 the copyright holder 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 HOLDER 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. -*/ +/******************************************************************************* + * Title : low_pass_filter.h + * Project : moveit_jog_arm + * Created : 1/11/2019 + * Author : Andy Zelenak + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 the copyright holder 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 HOLDER 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 diff --git a/moveit_experimental/moveit_jog_arm/launch/cpp_interface_example.launch b/moveit_experimental/moveit_jog_arm/launch/cpp_interface_example.launch new file mode 100644 index 0000000000..bc347ec658 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/launch/cpp_interface_example.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/moveit_experimental/moveit_jog_arm/launch/spacenav_cpp.launch b/moveit_experimental/moveit_jog_arm/launch/spacenav_cpp.launch index c78dff5b6b..9aa9f6b0c0 100644 --- a/moveit_experimental/moveit_jog_arm/launch/spacenav_cpp.launch +++ b/moveit_experimental/moveit_jog_arm/launch/spacenav_cpp.launch @@ -1,5 +1,5 @@ - - + From 073ab762ba68d28edfb2ac35c311915352da299d Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Sun, 1 Dec 2019 15:56:45 +0900 Subject: [PATCH 128/612] Resize scene marker with collision object --- .../motion_planning_frame.h | 2 + .../src/motion_planning_frame_objects.cpp | 45 ++++++++++--------- 2 files changed, 25 insertions(+), 22 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index df3e4edeec..94d4c5c171 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -251,6 +251,8 @@ private Q_SLOTS: void populateCollisionObjectsList(); void computeImportFromText(const std::string& path); void computeExportAsText(const std::string& path); + visualization_msgs::InteractiveMarker + createObjectMarkerMsg(const collision_detection::CollisionEnv::ObjectConstPtr& obj); // Stored scenes tab void computeSaveSceneButtonClicked(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 81b7e43565..b4944874a0 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -106,6 +106,7 @@ void MotionPlanningFrame::sceneScaleChanged(int value) ps->getWorldNonConst()->addToObject(scaled_object_->id_, shapes::ShapeConstPtr(s), scaled_object_->shape_poses_[i]); } + scene_marker_->processMessage(createObjectMarkerMsg(ps->getWorld()->getObject(scaled_object_->id_))); planning_display_->queueRenderSceneGeometry(); } else @@ -679,6 +680,24 @@ void MotionPlanningFrame::addObject(const collision_detection::WorldPtr& world, planning_display_->queueRenderSceneGeometry(); } +visualization_msgs::InteractiveMarker +MotionPlanningFrame::createObjectMarkerMsg(const collision_detection::CollisionEnv::ObjectConstPtr& obj) +{ + Eigen::Vector3d center; + double scale; + shapes::computeShapeBoundingSphere(obj->shapes_[0].get(), center, scale); + geometry_msgs::PoseStamped shape_pose = tf2::toMsg(tf2::Stamped( + obj->shape_poses_[0], ros::Time(), planning_display_->getRobotModel()->getModelFrame())); + scale = (scale + center.cwiseAbs().maxCoeff()) * 2.0 * 1.2; // add padding of 20% size + + // create an interactive marker msg for the given shape + visualization_msgs::InteractiveMarker imarker = + robot_interaction::make6DOFMarker("marker_scene_object", shape_pose, scale); + imarker.description = obj->id_; + interactive_markers::autoComplete(imarker); + return imarker; +} + void MotionPlanningFrame::createSceneInteractiveMarker() { QList sel = ui_->collision_objects_list->selectedItems(); @@ -693,30 +712,12 @@ void MotionPlanningFrame::createSceneInteractiveMarker() ps->getWorld()->getObject(sel[0]->text().toStdString()); if (obj && obj->shapes_.size() == 1) { - Eigen::Quaterniond eq(obj->shape_poses_[0].rotation()); - geometry_msgs::PoseStamped shape_pose; - shape_pose.pose.position.x = obj->shape_poses_[0].translation()[0]; - shape_pose.pose.position.y = obj->shape_poses_[0].translation()[1]; - shape_pose.pose.position.z = obj->shape_poses_[0].translation()[2]; - shape_pose.pose.orientation.x = eq.x(); - shape_pose.pose.orientation.y = eq.y(); - shape_pose.pose.orientation.z = eq.z(); - shape_pose.pose.orientation.w = eq.w(); - - // create an interactive marker for moving the shape in the world - visualization_msgs::InteractiveMarker int_marker = - robot_interaction::make6DOFMarker(std::string("marker_") + sel[0]->text().toStdString(), shape_pose, 1.0); - int_marker.header.frame_id = planning_display_->getRobotModel()->getModelFrame(); - int_marker.description = sel[0]->text().toStdString(); - - rviz::InteractiveMarker* imarker = new rviz::InteractiveMarker(planning_display_->getSceneNode(), context_); - interactive_markers::autoComplete(int_marker); - imarker->processMessage(int_marker); - imarker->setShowAxes(false); - scene_marker_.reset(imarker); + scene_marker_ = std::make_shared(planning_display_->getSceneNode(), context_); + scene_marker_->processMessage(createObjectMarkerMsg(obj)); + scene_marker_->setShowAxes(false); // Connect signals - connect(imarker, SIGNAL(userFeedback(visualization_msgs::InteractiveMarkerFeedback&)), this, + connect(scene_marker_.get(), SIGNAL(userFeedback(visualization_msgs::InteractiveMarkerFeedback&)), this, SLOT(imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback&))); } else From a8d832892368f1986ea46f51b793508ba458645e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 11 Dec 2019 19:08:38 +0100 Subject: [PATCH 129/612] Configure PSM's updateCallback before calling onRobotModelLoaded() ... to ensure that the callback is triggered with the initial requestPlanningSceneState(). Otherwise, the list of scene objects in the motion planning panel is not populated. --- .../src/motion_planning_display.cpp | 4 ++-- .../src/planning_scene_display.cpp | 6 ++---- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index a30790f242..777b0f8a7f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -1213,14 +1213,14 @@ void MotionPlanningDisplay::onSceneMonitorReceivedUpdate( robot_state::RobotState current_state = getPlanningSceneRO()->getCurrentState(); std::string group = planning_group_property_->getStdString(); - if (query_start_state_property_->getBool() && !group.empty()) + if (query_start_state_ && query_start_state_property_->getBool() && !group.empty()) { robot_state::RobotState start = *getQueryStartState(); updateStateExceptModified(start, current_state); setQueryStartState(start); } - if (query_goal_state_property_->getBool() && !group.empty()) + if (query_goal_state_ && query_goal_state_property_->getBool() && !group.empty()) { robot_state::RobotState goal = *getQueryGoalState(); updateStateExceptModified(goal, current_state); diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index f3cdfda14c..d04a3f25c4 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -518,6 +518,8 @@ void PlanningSceneDisplay::loadRobotModel() if (psm->getPlanningScene()) { planning_scene_monitor_.swap(psm); + planning_scene_monitor_->addUpdateCallback( + boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, _1)); addMainLoopJob(boost::bind(&PlanningSceneDisplay::onRobotModelLoaded, this)); setStatus(rviz::StatusProperty::Ok, "PlanningScene", "Planning Scene Loaded Successfully"); waitForAllMainLoopJobs(); @@ -527,10 +529,6 @@ void PlanningSceneDisplay::loadRobotModel() setStatus(rviz::StatusProperty::Error, "PlanningScene", "No Planning Scene Loaded"); } - if (planning_scene_monitor_) - planning_scene_monitor_->addUpdateCallback( - boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, _1)); - model_is_loading_ = false; } From 6c859a65857f980e54a1c1677022cdb6122494db Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 11 Dec 2019 20:19:17 +0100 Subject: [PATCH 130/612] Enable/disable pose+scale group box when collision object is selected/deselected --- .../src/motion_planning_frame_objects.cpp | 6 +++--- .../src/ui/motion_planning_rviz_plugin_frame.ui | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index b4944874a0..7f865d531b 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -217,14 +217,14 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() ui_->object_status->setText(""); scene_marker_.reset(); - ui_->scene_scale->setEnabled(false); + ui_->pose_scale_group_box->setEnabled(false); } else if (planning_display_->getPlanningSceneMonitor()) { // if this is a CollisionWorld element if (sel[0]->checkState() == Qt::Unchecked) { - ui_->scene_scale->setEnabled(true); + ui_->pose_scale_group_box->setEnabled(true); bool update_scene_marker = false; Eigen::Isometry3d obj_pose; { @@ -276,7 +276,7 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() } else { - ui_->scene_scale->setEnabled(false); + ui_->pose_scale_group_box->setEnabled(false); // if it is an attached object scene_marker_.reset(); const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 0f6825b916..ce4b982193 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -966,7 +966,7 @@ - + Manage Pose and Scale From fd4c7b55e462eee5caa4f49ae0b3ecf8a9b19e06 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 11 Dec 2019 20:24:14 +0100 Subject: [PATCH 131/612] Enable/disable motion planning panel with display --- .../src/motion_planning_display.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 777b0f8a7f..2d6285fd22 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -1249,8 +1249,7 @@ void MotionPlanningDisplay::onEnable() int_marker_display_->setEnabled(true); int_marker_display_->setFixedFrame(fixed_frame_); - if (frame_ && frame_->parentWidget()) - frame_->parentWidget()->show(); + frame_->enable(); } // ****************************************************************************************** @@ -1271,8 +1270,7 @@ void MotionPlanningDisplay::onDisable() // Planned Path Display trajectory_visual_->onDisable(); - if (frame_ && frame_->parentWidget()) - frame_->parentWidget()->hide(); + frame_->disable(); } // ****************************************************************************************** From 65e7b31d91c75d142b12881b229486576e767cca Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 11 Dec 2019 20:33:55 +0100 Subject: [PATCH 132/612] Reset scene_marker_ when disabling motion planning panel --- .../motion_planning_rviz_plugin/src/motion_planning_frame.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 834d204f0f..18e9ad7922 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -511,6 +511,7 @@ void MotionPlanningFrame::enable() void MotionPlanningFrame::disable() { move_group_.reset(); + scene_marker_.reset(); parentWidget()->hide(); } From 54d1e4944ef84ae44e3c0a007312db3b2649243d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 11 Dec 2019 21:48:25 +0100 Subject: [PATCH 133/612] Reduce step size for pose-adapting widgets --- .../src/ui/motion_planning_rviz_plugin_frame.ui | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index ce4b982193..a5f6f15f3a 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -988,6 +988,9 @@ 1000.000000000000000 + + 0.100000000000000 + @@ -998,6 +1001,9 @@ 1000.000000000000000 + + 0.100000000000000 + @@ -1008,6 +1014,9 @@ 1000.000000000000000 + + 0.100000000000000 + @@ -1030,7 +1039,7 @@ 3.140000000000000 - 0.200000000000000 + 0.100000000000000 @@ -1043,7 +1052,7 @@ 3.140000000000000 - 0.200000000000000 + 0.100000000000000 @@ -1056,7 +1065,7 @@ 3.140000000000000 - 0.200000000000000 + 0.100000000000000 From 01ff0358b58ef14aeedb1ceee98887c3c49f8911 Mon Sep 17 00:00:00 2001 From: Shivang Patel Date: Thu, 19 Dec 2019 13:49:41 -0500 Subject: [PATCH 134/612] deepcopy option for RobotTrajectory's copy constructor (#1760) --- MIGRATION.md | 1 + moveit_core/robot_trajectory/CMakeLists.txt | 8 + .../robot_trajectory/robot_trajectory.h | 9 + .../robot_trajectory/src/robot_trajectory.cpp | 13 ++ .../test/test_robot_trajectory.cpp | 211 ++++++++++++++++++ 5 files changed, 242 insertions(+) create mode 100644 moveit_core/robot_trajectory/test/test_robot_trajectory.cpp diff --git a/MIGRATION.md b/MIGRATION.md index 00cf53f4fb..2c2da8af98 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -9,6 +9,7 @@ API changes in MoveIt releases - Requests to `get_planning_scene` service without explicitly setting "components" now return full scene - `moveit_ros_plannning` no longer depends on `moveit_ros_perception` - `CollisionRobot` and `CollisionWorld` are combined into a single `CollisionEnv` class. This applies for all derived collision checkers as `FCL`, `ALL_VALID`, `HYBRID` and `DISTANCE_FIELD`. Consequently, `getCollisionRobot[Unpadded] / getCollisionWorld` functions are replaced through a `getCollisionEnv` in the planning scene and return the new combined environment. This unified collision environment provides the union of all member functions of `CollisionRobot` and `CollisionWorld`. Note that calling `checkRobotCollision` of the `CollisionEnv` does not take a `CollisionRobot` as an argument anymore as it is implicitly contained in the `CollisionEnv`. +- `RobotTrajectory` provides a copy constructor that allows a shallow (default) and deep copy of waypoints ## ROS Melodic diff --git a/moveit_core/robot_trajectory/CMakeLists.txt b/moveit_core/robot_trajectory/CMakeLists.txt index 29b89e5aa2..4f2a069d91 100644 --- a/moveit_core/robot_trajectory/CMakeLists.txt +++ b/moveit_core/robot_trajectory/CMakeLists.txt @@ -12,3 +12,11 @@ install(TARGETS ${MOVEIT_LIB_NAME} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + +if(CATKIN_ENABLE_TESTING) + find_package(moveit_resources REQUIRED) + include_directories(${moveit_resources_INCLUDE_DIRS}) + + catkin_add_gtest(test_robot_trajectory test/test_robot_trajectory.cpp) + target_link_libraries(test_robot_trajectory moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) +endif() diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index dd2ce58cc8..ae8865e85b 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -55,6 +55,15 @@ class RobotTrajectory RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const robot_model::JointModelGroup* group); + /** Assignment operator, performing a shallow copy, i.e. copying waypoints by pointer */ + RobotTrajectory& operator=(const RobotTrajectory&) = default; + + /** @brief Copy constructor allowing a shallow or deep copy of waypoints + * @param other - RobotTrajectory to copy from + * @param deepcopy - copy waypoints by value (true) or by pointer (false)? + */ + RobotTrajectory(const RobotTrajectory& other, bool deepcopy = false); + const robot_model::RobotModelConstPtr& getRobotModel() const { return robot_model_; diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 0c96f9c8dd..5f75bf5b62 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -53,6 +53,19 @@ RobotTrajectory::RobotTrajectory(const robot_model::RobotModelConstPtr& robot_mo { } +RobotTrajectory::RobotTrajectory(const RobotTrajectory& other, bool deepcopy) +{ + *this = other; // default assignment operator performs a shallow copy + this->waypoints_.clear(); + if (deepcopy) + { + for (const auto& waypoint : other.waypoints_) + { + this->waypoints_.emplace_back(std::make_shared(*waypoint)); + } + } +} + void RobotTrajectory::setGroupName(const std::string& group_name) { group_ = robot_model_->getJointModelGroup(group_name); diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp new file mode 100644 index 0000000000..ba57080bbd --- /dev/null +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -0,0 +1,211 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2013, Willow Garage, Inc. +* 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 the 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. +*********************************************************************/ + +/* Author: Ioan Sucan */ + +#include +#include +#include +#include +#include + +class RobotTrajectoryTestFixture : public testing::Test +{ +protected: + moveit::core::RobotModelConstPtr robot_model_; + robot_state::RobotStatePtr robot_state_; + const std::string robot_model_name_ = "panda"; + const std::string arm_jmg_name_ = "panda_arm"; + +protected: + void SetUp() override + { + robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_); + robot_state_ = std::make_shared(robot_model_); + robot_state_->setToDefaultValues(); + robot_state_->update(); + } + + void TearDown() override + { + } + + void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) + { + // Init a traj + ASSERT_TRUE(robot_model_->hasJointModelGroup(arm_jmg_name_)) << "Robot model does not have group: " + << arm_jmg_name_; + + trajectory = std::make_shared(robot_model_, arm_jmg_name_); + + EXPECT_EQ(trajectory->getGroupName(), arm_jmg_name_) << "Generated trajectory group name does not match"; + EXPECT_TRUE(trajectory->empty()) << "Generated trajectory not empty"; + + double duration_from_previous = 0.1; + std::size_t waypoint_count = 5; + for (std::size_t ix = 0; ix < waypoint_count; ++ix) + trajectory->addSuffixWayPoint(robot_state_, duration_from_previous); + // Quick check that getDuration is working correctly + EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count) + << "Generated trajectory duration incorrect"; + EXPECT_EQ(waypoint_count, trajectory->getWayPointDurations().size()) + << "Generated trajectory has the wrong number of waypoints"; + } + + void copyTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, + robot_trajectory::RobotTrajectoryPtr& trajectory_copy, bool deepcopy) + { + // Copy the trajectory + trajectory_copy = std::make_shared(*trajectory, deepcopy); + // Quick check that the getDuration values match + EXPECT_EQ(trajectory_copy->getDuration(), trajectory->getDuration()); + EXPECT_EQ(trajectory_copy->getWayPointDurations().size(), trajectory->getWayPointDurations().size()); + } + + void modifyFirstWaypointPtrAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) + { + /////////////////////////// + // Get the first waypoint by POINTER, modify it, and check that the value WAS updated in trajectory + /////////////////////////// + // Get the first waypoint by shared pointer + robot_state::RobotStatePtr trajectory_first_waypoint = trajectory->getWayPointPtr(0); + // Get the first waypoint joint values + std::vector trajectory_first_state; + trajectory_first_waypoint->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state); + + // Modify the first waypoint joint values + trajectory_first_state[0] += 0.01; + trajectory_first_waypoint->setJointGroupPositions(arm_jmg_name_, trajectory_first_state); + + // Check that the trajectory's first waypoint was updated + robot_state::RobotStatePtr trajectory_first_waypoint_after_update = trajectory->getWayPointPtr(0); + std::vector trajectory_first_state_after_update; + trajectory_first_waypoint_after_update->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); + EXPECT_EQ(trajectory_first_state[0], trajectory_first_state_after_update[0]); + } + + void modifyFirstWaypointAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) + { + /////////////////////////// + // Get the first waypoint by VALUE, modify it, and check that the value WAS NOT updated in trajectory + /////////////////////////// + // Get the first waypoint by shared pointer + robot_state::RobotState trajectory_first_waypoint = trajectory->getWayPoint(0); + // Get the first waypoint joint values + std::vector trajectory_first_state; + trajectory_first_waypoint.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state); + + // Modify the first waypoint joint values + trajectory_first_state[0] += 0.01; + trajectory_first_waypoint.setJointGroupPositions(arm_jmg_name_, trajectory_first_state); + + // Check that the trajectory's first waypoint was updated + robot_state::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0); + std::vector trajectory_first_state_after_update; + trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); + EXPECT_NE(trajectory_first_state[0], trajectory_first_state_after_update[0]); + } +}; + +TEST_F(RobotTrajectoryTestFixture, ModifyFirstWaypointByPtr) +{ + robot_trajectory::RobotTrajectoryPtr trajectory; + initTestTrajectory(trajectory); + modifyFirstWaypointPtrAndCheckTrajectory(trajectory); +} + +TEST_F(RobotTrajectoryTestFixture, ModifyFirstWaypointByValue) +{ + robot_trajectory::RobotTrajectoryPtr trajectory; + initTestTrajectory(trajectory); + modifyFirstWaypointAndCheckTrajectory(trajectory); +} + +TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryShallowCopy) +{ + bool deepcopy = false; + + robot_trajectory::RobotTrajectoryPtr trajectory; + robot_trajectory::RobotTrajectoryPtr trajectory_copy; + + initTestTrajectory(trajectory); + copyTrajectory(trajectory, trajectory_copy, deepcopy); + modifyFirstWaypointPtrAndCheckTrajectory(trajectory); + + // Check that modifying the waypoint also modified the trajectory + robot_state::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0); + std::vector trajectory_first_state_after_update; + trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); + + // Get the first waypoint in the modified trajectory_copy + robot_state::RobotState trajectory_copy_first_waypoint_after_update = trajectory_copy->getWayPoint(0); + std::vector trajectory_copy_first_state_after_update; + trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, + trajectory_copy_first_state_after_update); + + // Check that we updated the value correctly in the trajectory + EXPECT_EQ(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]); +} + +TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDeepCopy) +{ + bool deepcopy = true; + + robot_trajectory::RobotTrajectoryPtr trajectory; + robot_trajectory::RobotTrajectoryPtr trajectory_copy; + + initTestTrajectory(trajectory); + copyTrajectory(trajectory, trajectory_copy, deepcopy); + modifyFirstWaypointPtrAndCheckTrajectory(trajectory); + + // Check that modifying the waypoint also modified the trajectory + robot_state::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0); + std::vector trajectory_first_state_after_update; + trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); + + // Get the first waypoint in the modified trajectory_copy + robot_state::RobotState trajectory_copy_first_waypoint_after_update = trajectory_copy->getWayPoint(0); + std::vector trajectory_copy_first_state_after_update; + trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, + trajectory_copy_first_state_after_update); + + // Check that we updated the value correctly in the trajectory + EXPECT_NE(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From e0b4f854edd3a792ec124bc3d1af816dc3cc190a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 23 Dec 2019 16:09:15 -0700 Subject: [PATCH 135/612] moveit_jog_arm: Get transforms from RobotState instead of TF (#1803) --- .../moveit_jog_arm/CMakeLists.txt | 8 +- .../config/ur_simulated_config.yaml | 18 +- .../include/moveit_jog_arm/jog_arm_data.h | 8 +- .../include/moveit_jog_arm/jog_calcs.h | 19 +- .../moveit_jog_arm/jog_cpp_interface.h | 4 + .../moveit_jog_arm/package.xml | 1 - .../moveit_jog_arm/src/jog_calcs.cpp | 177 ++++++++---------- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 17 +- .../moveit_jog_arm/src/jog_interface_base.cpp | 12 +- .../moveit_jog_arm/src/jog_ros_interface.cpp | 4 +- .../test/arm_setup/config/jog_settings.yaml | 70 ++++--- .../integration/test_jog_arm_integration.py | 17 +- 12 files changed, 191 insertions(+), 164 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index 3502c17a89..58daac76d8 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -7,6 +7,8 @@ set(CMAKE_CXX_EXTENSIONS OFF) set(LIBRARY_NAME jog_arm_cpp_api) +set(LIBRARY_NAME jog_arm_cpp_api) + if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() @@ -16,7 +18,6 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs moveit_ros_planning_interface rosparam_shortcuts - tf2_ros ) find_package(Eigen3 REQUIRED) @@ -30,7 +31,6 @@ catkin_package( geometry_msgs moveit_ros_planning_interface rosparam_shortcuts - tf2_ros DEPENDS EIGEN3 ) @@ -119,6 +119,10 @@ install(DIRECTORY include/${PROJECT_NAME}/ ## TESTING ## ############# +############# +## TESTING ## +############# + if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) find_package(ros_pytest REQUIRED) diff --git a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml index fe09ddf6ac..67a383643e 100644 --- a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml +++ b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml @@ -4,17 +4,17 @@ use_gazebo: true # Whether the robot is started in a Gazebo simulation environment ## Properties of incoming commands -command_frame: world # TF frame that incoming cmds are given in +robot_link_command_frame: world # commands must be given in the frame of a robot link. Usually either the base or end effector command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s -scale: # Only used if command_in_type=="unitless" - linear: 0.6 # Max linear velocity. Meters per publish_period. Units is [m/s] - rotational: 0.3 # Max angular velocity. Rads per publish_period. Units is [rad/s] +scale: + linear: 0.6 # Max linear velocity. Meters per publish_period. Units is [m/s]. Only used if command_in_type=="unitless" + rotational: 0.3 # Max angular velocity. Rads per publish_period. Units is [rad/s]. Only used if command_in_type=="unitless" joint: 0.3 # Max joint angular/linear velocity. Rads or Meters per publish period. Units is [rad/s] or [m/s]. low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less. ## Properties of outgoing commands publish_period: 0.008 # 1/Nominal publish rate [seconds] -publish_delay: 0.005 # delay between calculation and execution start of command +publish_delay: 0.005 # Delay between calculation and execution start of command # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) @@ -28,13 +28,13 @@ publish_joint_accelerations: false ## MoveIt properties move_group_name: manipulator # Often 'manipulator' or 'arm' -planning_frame: world # The MoveIt! planning frame. Often 'base_link' +planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' ## Stopping behaviour -incoming_command_timeout: 0.1 # Stop jogging if X seconds elapse without a new cmd +incoming_command_timeout: 0.1 # Stop jogging if X seconds elapse without a new command # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. # Important because ROS may drop some messages and we need the robot to halt reliably. -num_halt_msgs_to_publish: 4 +num_outgoing_halt_msgs_to_publish: 4 ## Configure handling of singularities and joint limits lower_singularity_threshold: 17 # Start decelerating when the condition number hits this (close to singularity) @@ -48,7 +48,7 @@ joint_topic: joint_states warning_topic: jog_server/halted # Publish boolean warnings to this topic command_out_topic: joint_group_position_controller/command # Publish outgoing commands here -## Collision checking +## Collision checking for the entire robot body check_collisions: true # Check collisions? collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m] diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 01aa989c73..c96af15726 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -39,6 +39,7 @@ #pragma once #include +#include #include #include #include @@ -78,17 +79,20 @@ struct JogArmShared // Indicates no collision, etc, so outgoing commands can be sent bool ok_to_publish = false; + + // The transform from the MoveIt planning frame to robot_link_command_frame + Eigen::Isometry3d tf_moveit_to_cmd_frame; }; // ROS params to be read. See the yaml file in /config for a description of each. struct JogArmParameters { - std::string move_group_name, joint_topic, cartesian_command_in_topic, command_frame, command_out_topic, + std::string move_group_name, joint_topic, cartesian_command_in_topic, robot_link_command_frame, command_out_topic, planning_frame, warning_topic, joint_command_in_topic, command_in_type, command_out_type; double linear_scale, rotational_scale, joint_scale, lower_singularity_threshold, hard_stop_singularity_threshold, collision_proximity_threshold, low_pass_filter_coeff, publish_period, incoming_command_timeout, joint_limit_margin, collision_check_rate; - int num_halt_msgs_to_publish; + int num_outgoing_halt_msgs_to_publish; bool use_gazebo, check_collisions, publish_joint_positions, publish_joint_velocities, publish_joint_accelerations; }; } diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index ff6af3aa45..70cfbf24a1 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -43,8 +43,6 @@ #include #include #include -#include -#include namespace moveit_jog_arm { @@ -57,7 +55,7 @@ class JogCalcs protected: ros::NodeHandle nh_; - sensor_msgs::JointState incoming_jts_; + sensor_msgs::JointState incoming_joints_; bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables, std::mutex& mutex); @@ -80,11 +78,11 @@ class JogCalcs // Suddenly halt for a joint limit or other critical issue. // Is handled differently for position vs. velocity control. - void suddenHalt(trajectory_msgs::JointTrajectory& jt_traj); + void suddenHalt(trajectory_msgs::JointTrajectory& joint_traj); void publishWarning(bool active) const; - bool checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory_>& new_jt_traj); + bool checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory_>& new_joint_traj); // Possibly calculate a velocity scaling factor, due to proximity of // singularity and direction of motion @@ -93,7 +91,7 @@ class JogCalcs // Apply velocity scaling for proximity of collisions and singularities bool applyVelocityScaling(JogArmShared& shared_variables, std::mutex& mutex, - trajectory_msgs::JointTrajectory& new_jt_traj, const Eigen::VectorXd& delta_theta, + trajectory_msgs::JointTrajectory& new_joint_traj, const Eigen::VectorXd& delta_theta, double singularity_scale); trajectory_msgs::JointTrajectory composeOutgoingMessage(sensor_msgs::JointState& joint_state) const; @@ -108,13 +106,10 @@ class JogCalcs robot_state::RobotStatePtr kinematic_state_; - sensor_msgs::JointState jt_state_, original_jt_state_; - std::map jt_state_name_map_; + sensor_msgs::JointState joint_state_, original_joint_state_; + std::map joint_state_name_map_; trajectory_msgs::JointTrajectory outgoing_command_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - std::vector velocity_filters_; std::vector position_filters_; @@ -127,6 +122,8 @@ class JogCalcs Eigen::JacobiSVD svd_; Eigen::VectorXd delta_theta_; + Eigen::Isometry3d tf_moveit_to_cmd_frame_; + const int gazebo_redundant_message_count_ = 30; uint num_joints_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h index 4a8075f921..42fa925ae9 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h @@ -64,6 +64,10 @@ class JogCppApi : JogInterfaceBase // May eliminate the need to create your own joint_state subscriber. sensor_msgs::JointState getJointState(); + // Get planning link transform. + // The transform from the MoveIt planning frame to robot_link_command_frame + Eigen::Isometry3d getCommandFrameTransform(); + private: ros::NodeHandle nh_; }; diff --git a/moveit_experimental/moveit_jog_arm/package.xml b/moveit_experimental/moveit_jog_arm/package.xml index ed1227fce6..9926ba24e7 100644 --- a/moveit_experimental/moveit_jog_arm/package.xml +++ b/moveit_experimental/moveit_jog_arm/package.xml @@ -23,7 +23,6 @@ geometry_msgs moveit_ros_planning_interface rosparam_shortcuts - tf2_ros joy_teleop diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 539f8f118e..975e4aaf69 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -43,7 +43,7 @@ namespace moveit_jog_arm // Constructor for the class that handles jogging calculations JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_variables, std::mutex& mutex, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) - : tf_listener_(tf_buffer_), parameters_(parameters) + : parameters_(parameters) { // Publish collision status warning_pub_ = nh_.advertise(parameters_.warning_topic, 1); @@ -67,15 +67,15 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari resetVelocityFilters(); - jt_state_.name = joint_model_group_->getVariableNames(); - num_joints_ = jt_state_.name.size(); - jt_state_.position.resize(num_joints_); - jt_state_.velocity.resize(num_joints_); - jt_state_.effort.resize(num_joints_); + joint_state_.name = joint_model_group_->getVariableNames(); + num_joints_ = joint_state_.name.size(); + joint_state_.position.resize(num_joints_); + joint_state_.velocity.resize(num_joints_); + joint_state_.effort.resize(num_joints_); // A map for the indices of incoming joint commands - for (std::size_t i = 0; i < jt_state_.name.size(); ++i) + for (std::size_t i = 0; i < joint_state_.name.size(); ++i) { - jt_state_name_map_[jt_state_.name[i]] = i; + joint_state_name_map_[joint_state_.name[i]] = i; } // Low-pass filters for the joint positions & velocities @@ -89,12 +89,12 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari while (!updateJoints() && ros::ok()) { mutex.lock(); - incoming_jts_ = shared_variables.joints; + incoming_joints_ = shared_variables.joints; mutex.unlock(); ros::Duration(WHILE_LOOP_WAIT).sleep(); } for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(jt_state_.position[i]); + position_filters_[i].reset(joint_state_.position[i]); // Wait for the first jogging cmd. // Store it in a class member for further calcs, then free up the shared variable again. @@ -131,14 +131,23 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari // Pull data from the shared variables. mutex.lock(); - incoming_jts_ = shared_variables.joints; + incoming_joints_ = shared_variables.joints; + mutex.unlock(); + + kinematic_state_->setVariableValues(joint_state_); + original_joint_state_ = joint_state_; + + // Get the transform from MoveIt planning frame to jogging command frame + tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); + mutex.lock(); + shared_variables.tf_moveit_to_cmd_frame = tf_moveit_to_cmd_frame_; mutex.unlock(); // Initialize the position filters to initial robot joints while (!updateJoints() && ros::ok()) { mutex.lock(); - incoming_jts_ = shared_variables.joints; + incoming_joints_ = shared_variables.joints; mutex.unlock(); ros::Duration(WHILE_LOOP_WAIT).sleep(); } @@ -164,8 +173,8 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari } else { - original_jt_state_ = jt_state_; - outgoing_command_ = composeOutgoingMessage(jt_state_); + original_joint_state_ = joint_state_; + outgoing_command_ = composeOutgoingMessage(joint_state_); } // Halt if the command is stale or inputs are all zero, or commands were zero @@ -193,15 +202,15 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari shared_variables.ok_to_publish = true; } // Skip the jogging publication if all inputs have been zero for several cycles in a row. - // num_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. - else if ((parameters_.num_halt_msgs_to_publish != 0) && - (zero_velocity_count > parameters_.num_halt_msgs_to_publish)) + // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. + else if ((parameters_.num_outgoing_halt_msgs_to_publish != 0) && + (zero_velocity_count > parameters_.num_outgoing_halt_msgs_to_publish)) { shared_variables.ok_to_publish = false; // Reset the velocity filters so robot doesn't jump when commands resume resetVelocityFilters(); } - // The command is invalid but we are publishing num_halt_msgs_to_publish + // The command is invalid but we are publishing num_outgoing_halt_msgs_to_publish else { shared_variables.outgoing_command = outgoing_command_; @@ -247,51 +256,23 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& } } - // Transform the command to the MoveGroup planning frame. - geometry_msgs::TransformStamped command_frame_to_planning_frame; - try - { - command_frame_to_planning_frame = - tf_buffer_.lookupTransform(parameters_.planning_frame, cmd.header.frame_id, ros::Time(0), ros::Duration(1.0)); - } - catch (tf2::TransformException& ex) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, ros::this_node::getName() << ": " << ex.what()); - return false; - } - - geometry_msgs::Vector3 lin_vector = cmd.twist.linear; - try - { - tf2::doTransform(lin_vector, lin_vector, command_frame_to_planning_frame); - } - catch (const tf2::TransformException& ex) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, ros::this_node::getName() << ": " << ex.what()); - return false; - } - - geometry_msgs::Vector3 rot_vector = cmd.twist.angular; - try - { - tf2::doTransform(rot_vector, rot_vector, command_frame_to_planning_frame); - } - catch (const tf2::TransformException& ex) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, ros::this_node::getName() << ": " << ex.what()); - return false; - } + // Transform the command to the MoveGroup planning frame + Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z); + Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z); + translation_vector = tf_moveit_to_cmd_frame_.linear() * translation_vector; + angular_vector = tf_moveit_to_cmd_frame_.linear() * angular_vector; // Put these components back into a TwistStamped cmd.header.frame_id = parameters_.planning_frame; - cmd.twist.linear = lin_vector; - cmd.twist.angular = rot_vector; + cmd.twist.linear.x = translation_vector(0); + cmd.twist.linear.y = translation_vector(1); + cmd.twist.linear.z = translation_vector(2); + cmd.twist.angular.x = angular_vector(0); + cmd.twist.angular.y = angular_vector(1); + cmd.twist.angular.z = angular_vector(2); const Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); - kinematic_state_->setVariableValues(jt_state_); - original_jt_state_ = jt_state_; - // Convert from cartesian commands to joint commands jacobian_ = kinematic_state_->getJacobian(joint_model_group_); svd_ = Eigen::JacobiSVD(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV); @@ -301,7 +282,7 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& enforceJointVelocityLimits(delta_theta_); - if (!addJointIncrements(jt_state_, delta_theta_)) + if (!addJointIncrements(joint_state_, delta_theta_)) return false; // Include a velocity estimate for velocity-controlled robots @@ -310,7 +291,7 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& lowPassFilterVelocities(joint_vel); lowPassFilterPositions(); - outgoing_command_ = composeOutgoingMessage(jt_state_); + outgoing_command_ = composeOutgoingMessage(joint_state_); // If close to a collision or a singularity, decelerate applyVelocityScaling(shared_variables, mutex, outgoing_command_, delta_theta_, @@ -348,10 +329,10 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& /* // Apply user-defined scaling const Eigen::VectorXd delta = scaleJointCommand(cmd); - kinematic_state_->setVariableValues(jt_state_); - original_jt_state_ = jt_state_; + kinematic_state_->setVariableValues(joint_state_); + original_joint_state_ = joint_state_; - if (!addJointIncrements(jt_state_, delta)) + if (!addJointIncrements(joint_state_, delta)) return false; // Include a velocity estimate for velocity-controlled robots @@ -361,9 +342,9 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& /* lowPassFilterPositions(); // update joint state with new values - kinematic_state_->setVariableValues(jt_state_); + kinematic_state_->setVariableValues(joint_state_); - outgoing_command_ = composeOutgoingMessage(jt_state_); + outgoing_command_ = composeOutgoingMessage(joint_state_); // check if new joint state is valid if (!checkIfJointsWithinURDFBounds(outgoing_command_)) @@ -403,13 +384,13 @@ void JogCalcs::lowPassFilterPositions() { for (size_t i = 0; i < num_joints_; ++i) { - jt_state_.position[i] = position_filters_[i].filter(jt_state_.position[i]); + joint_state_.position[i] = position_filters_[i].filter(joint_state_.position[i]); // Check for nan's - if (std::isnan(jt_state_.position[i])) + if (std::isnan(joint_state_.position[i])) { - jt_state_.position[i] = original_jt_state_.position[i]; - jt_state_.velocity[i] = 0.; + joint_state_.position[i] = original_joint_state_.position[i]; + joint_state_.velocity[i] = 0.; } } } @@ -418,13 +399,13 @@ void JogCalcs::lowPassFilterVelocities(const Eigen::VectorXd& joint_vel) { for (size_t i = 0; i < num_joints_; ++i) { - jt_state_.velocity[i] = velocity_filters_[i].filter(joint_vel[static_cast(i)]); + joint_state_.velocity[i] = velocity_filters_[i].filter(joint_vel[static_cast(i)]); // Check for nan's - if (std::isnan(jt_state_.velocity[static_cast(i)])) + if (std::isnan(joint_state_.velocity[static_cast(i)])) { - jt_state_.position[i] = original_jt_state_.position[i]; - jt_state_.velocity[i] = 0.; + joint_state_.position[i] = original_joint_state_.position[i]; + joint_state_.velocity[i] = 0.; ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "nan in velocity filter"); } } @@ -432,10 +413,10 @@ void JogCalcs::lowPassFilterVelocities(const Eigen::VectorXd& joint_vel) trajectory_msgs::JointTrajectory JogCalcs::composeOutgoingMessage(sensor_msgs::JointState& joint_state) const { - trajectory_msgs::JointTrajectory new_jt_traj; - new_jt_traj.header.frame_id = parameters_.planning_frame; - new_jt_traj.header.stamp = ros::Time::now(); - new_jt_traj.joint_names = joint_state.name; + trajectory_msgs::JointTrajectory new_joint_traj; + new_joint_traj.header.frame_id = parameters_.planning_frame; + new_joint_traj.header.stamp = ros::Time::now(); + new_joint_traj.joint_names = joint_state.name; trajectory_msgs::JointTrajectoryPoint point; point.time_from_start = ros::Duration(parameters_.publish_period); @@ -451,17 +432,17 @@ trajectory_msgs::JointTrajectory JogCalcs::composeOutgoingMessage(sensor_msgs::J std::vector acceleration(num_joints_); point.accelerations = acceleration; } - new_jt_traj.points.push_back(point); + new_joint_traj.points.push_back(point); - return new_jt_traj; + return new_joint_traj; } // Apply velocity scaling for proximity of collisions and singularities. // Scale for collisions is read from a shared variable. // Key equation: new_velocity = collision_scale*singularity_scale*previous_velocity bool JogCalcs::applyVelocityScaling(JogArmShared& shared_variables, std::mutex& mutex, - trajectory_msgs::JointTrajectory& new_jt_traj, const Eigen::VectorXd& delta_theta, - double singularity_scale) + trajectory_msgs::JointTrajectory& new_joint_traj, + const Eigen::VectorXd& delta_theta, double singularity_scale) { mutex.lock(); double collision_scale = shared_variables.collision_velocity_scale; @@ -472,12 +453,12 @@ bool JogCalcs::applyVelocityScaling(JogArmShared& shared_variables, std::mutex& if (parameters_.publish_joint_positions) { // If close to a singularity or joint limit, undo any change to the joint angles - new_jt_traj.points[0].positions[i] = - new_jt_traj.points[0].positions[i] - + new_joint_traj.points[0].positions[i] = + new_joint_traj.points[0].positions[i] - (1. - singularity_scale * collision_scale) * delta_theta[static_cast(i)]; } if (parameters_.publish_joint_velocities) - new_jt_traj.points[0].velocities[i] *= singularity_scale * collision_scale; + new_joint_traj.points[0].velocities[i] *= singularity_scale * collision_scale; } return true; @@ -563,7 +544,7 @@ void JogCalcs::enforceJointVelocityLimits(Eigen::VectorXd& calculated_joint_chan } } -bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& new_jt_traj) +bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& new_joint_traj) { bool halting = false; @@ -576,11 +557,11 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n " velocity limit. Enforcing limit."); kinematic_state_->enforceVelocityBounds(joint); - for (std::size_t c = 0; c < new_jt_traj.joint_names.size(); ++c) + for (std::size_t c = 0; c < new_joint_traj.joint_names.size(); ++c) { - if (new_jt_traj.joint_names[c] == joint->getName()) + if (new_joint_traj.joint_names[c] == joint->getName()) { - new_jt_traj.points[0].velocities[c] = kinematic_state_->getJointVelocities(joint)[0]; + new_joint_traj.points[0].velocities[c] = kinematic_state_->getJointVelocities(joint)[0]; break; } } @@ -588,11 +569,11 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n // Halt if we're past a joint margin and joint velocity is moving even farther past double joint_angle = 0; - for (std::size_t c = 0; c < original_jt_state_.name.size(); ++c) + for (std::size_t c = 0; c < original_joint_state_.name.size(); ++c) { - if (original_jt_state_.name[c] == joint->getName()) + if (original_joint_state_.name[c] == joint->getName()) { - joint_angle = original_jt_state_.position.at(c); + joint_angle = original_joint_state_.position.at(c); break; } } @@ -629,17 +610,17 @@ void JogCalcs::publishWarning(bool active) const // Suddenly halt for a joint limit or other critical issue. // Is handled differently for position vs. velocity control. -void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& jt_traj) +void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_traj) { for (std::size_t i = 0; i < num_joints_; ++i) { // For position-controlled robots, can reset the joints to a known, good state if (parameters_.publish_joint_positions) - jt_traj.points[0].positions[i] = original_jt_state_.position[i]; + joint_traj.points[0].positions[i] = original_joint_state_.position[i]; // For velocity-controlled robots, stop if (parameters_.publish_joint_velocities) - jt_traj.points[0].velocities[i] = 0; + joint_traj.points[0].velocities[i] = 0; } } @@ -654,16 +635,16 @@ void JogCalcs::resetVelocityFilters() bool JogCalcs::updateJoints() { // Check that the msg contains enough joints - if (incoming_jts_.name.size() < num_joints_) + if (incoming_joints_.name.size() < num_joints_) return false; // Store joints in a member variable - for (std::size_t m = 0; m < incoming_jts_.name.size(); ++m) + for (std::size_t m = 0; m < incoming_joints_.name.size(); ++m) { std::size_t c; try { - c = jt_state_name_map_.at(incoming_jts_.name[m]); + c = joint_state_name_map_.at(incoming_joints_.name[m]); } catch (const std::out_of_range& e) { @@ -671,7 +652,7 @@ bool JogCalcs::updateJoints() continue; } - jt_state_.position[c] = incoming_jts_.position[m]; + joint_state_.position[c] = incoming_joints_.position[m]; } return true; @@ -722,7 +703,7 @@ Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& comman { try { - c = jt_state_name_map_.at(command.joint_names[m]); + c = joint_state_name_map_.at(command.joint_names[m]); } catch (const std::out_of_range& e) { diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp index b0ddf2e13a..5064beba98 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp @@ -87,11 +87,10 @@ void JogCppApi::mainLoop() shared_variables_mutex_.lock(); trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; - // Check for stale cmds + // Check if incoming commands are stale if ((ros::Time::now() - shared_variables_.latest_nonzero_cmd_stamp) < ros::Duration(ros_parameters_.incoming_command_timeout)) { - // Mark that incoming commands are not stale shared_variables_.command_is_stale = false; } else @@ -142,18 +141,16 @@ void JogCppApi::provideTwistStampedCommand(const geometry_msgs::TwistStamped& ve { shared_variables_mutex_.lock(); - // Copy everything but the frame name. The frame name is set by yaml file at startup. - // (so it isn't copied over and over) shared_variables_.command_deltas.twist = velocity_command.twist; shared_variables_.command_deltas.header = velocity_command.header; // Input frame determined by YAML file if not passed with message if (shared_variables_.command_deltas.header.frame_id.empty()) { - shared_variables_.command_deltas.header.frame_id = ros_parameters_.command_frame; + shared_variables_.command_deltas.header.frame_id = ros_parameters_.robot_link_command_frame; } - // Check if input is all zeros. Flag it if so to skip calculations/publication after num_halt_msgs_to_publish + // Check if input is all zeros. Flag it if so to skip calculations/publication after num_outgoing_halt_msgs_to_publish shared_variables_.zero_cartesian_cmd_flag = shared_variables_.command_deltas.twist.linear.x == 0.0 && shared_variables_.command_deltas.twist.linear.y == 0.0 && shared_variables_.command_deltas.twist.linear.z == 0.0 && @@ -197,4 +194,12 @@ sensor_msgs::JointState JogCppApi::getJointState() return current_joints; } +Eigen::Isometry3d JogCppApi::getCommandFrameTransform() +{ + shared_variables_mutex_.lock(); + Eigen::Isometry3d tf_moveit_to_cmd_frame = shared_variables_.tf_moveit_to_cmd_frame; + shared_variables_mutex_.unlock(); + + return tf_moveit_to_cmd_frame; +} } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index 7cbb0f59bb..f838bdcdaf 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -60,8 +60,8 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_period", ros_parameters_.publish_period); error += !rosparam_shortcuts::get("", n, parameter_ns + "/collision_check_rate", ros_parameters_.collision_check_rate); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/num_halt_msgs_to_publish", - ros_parameters_.num_halt_msgs_to_publish); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/num_outgoing_halt_msgs_to_publish", + ros_parameters_.num_outgoing_halt_msgs_to_publish); error += !rosparam_shortcuts::get("", n, parameter_ns + "/scale/linear", ros_parameters_.linear_scale); error += !rosparam_shortcuts::get("", n, parameter_ns + "/scale/rotational", ros_parameters_.rotational_scale); error += !rosparam_shortcuts::get("", n, parameter_ns + "/scale/joint", ros_parameters_.joint_scale); @@ -73,7 +73,8 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) ros_parameters_.cartesian_command_in_topic); error += !rosparam_shortcuts::get("", n, parameter_ns + "/joint_command_in_topic", ros_parameters_.joint_command_in_topic); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_frame", ros_parameters_.command_frame); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/robot_link_command_frame", + ros_parameters_.robot_link_command_frame); error += !rosparam_shortcuts::get("", n, parameter_ns + "/incoming_command_timeout", ros_parameters_.incoming_command_timeout); error += !rosparam_shortcuts::get("", n, parameter_ns + "/lower_singularity_threshold", @@ -100,9 +101,10 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) rosparam_shortcuts::shutdownIfError(parameter_ns, error); // Input checking - if (ros_parameters_.num_halt_msgs_to_publish < 0) + if (ros_parameters_.num_outgoing_halt_msgs_to_publish < 0) { - ROS_WARN_NAMED(LOGNAME, "Parameter 'num_halt_msgs_to_publish' should be greater than zero. Check yaml file."); + ROS_WARN_NAMED(LOGNAME, + "Parameter 'num_outgoing_halt_msgs_to_publish' should be greater than zero. Check yaml file."); return false; } if (ros_parameters_.hard_stop_singularity_threshold < ros_parameters_.lower_singularity_threshold) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp index b7e2fb5540..978d523273 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp @@ -163,10 +163,10 @@ void JogROSInterface::deltaCartesianCmdCB(const geometry_msgs::TwistStampedConst // Input frame determined by YAML file if not passed with message if (shared_variables_.command_deltas.header.frame_id.empty()) { - shared_variables_.command_deltas.header.frame_id = ros_parameters_.command_frame; + shared_variables_.command_deltas.header.frame_id = ros_parameters_.robot_link_command_frame; } - // Check if input is all zeros. Flag it if so to skip calculations/publication after num_halt_msgs_to_publish + // Check if input is all zeros. Flag it if so to skip calculations/publication after num_outgoing_halt_msgs_to_publish shared_variables_.zero_cartesian_cmd_flag = shared_variables_.command_deltas.twist.linear.x == 0.0 && shared_variables_.command_deltas.twist.linear.y == 0.0 && shared_variables_.command_deltas.twist.linear.z == 0.0 && diff --git a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml index 6fd1119af4..dc2294d889 100644 --- a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml +++ b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml @@ -1,33 +1,55 @@ +############################################### +# Modify all parameters related to jogging here +############################################### use_gazebo: false # Whether the robot is started in a Gazebo simulation environment -check_collisions: true # Check collisions? + +## Properties of incoming commands +robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s -scale: # Only used if command_in_type=="unitless" - linear: 0.003 # Max linear velocity. Meters per publish_period. - rotational: 0.006 # Max angular velocity. Rads per publish_period. - joint: 0.01 # Max joint angular/linear velocity. Rads or Meters per publish period. -cartesian_command_in_topic: jog_server/delta_jog_cmds # Topic for xyz commands -joint_command_in_topic: jog_server/joint_delta_jog_cmds # Topic for angle commands -command_frame: base_link # TF frame that incoming cmds are given in -incoming_command_timeout: 1 # Stop jogging if X seconds elapsed without a new cmd -joint_topic: joint_states -move_group_name: panda_arm # Often 'manipulator' or 'arm' -lower_singularity_threshold: 30 # Start decelerating when the condition number hits this (close to singularity). Larger --> closer to singularity -hard_stop_singularity_threshold: 45 # Stop when the condition number hits this. Larger --> closer to singularity -collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] -planning_frame: base_link # The MoveIt planning frame. Often 'base_link' -low_pass_filter_coeff: 2. # Larger-> more smoothing to jog commands, but more lag. +scale: + linear: 0.003 # Max linear velocity. Meters per publish_period. Units is [m/s]. Only used if command_in_type=="unitless" + rotational: 0.006 # Max angular velocity. Rads per publish_period. Units is [rad/s]. Only used if command_in_type=="unitless" + joint: 0.01 # Max joint angular/linear velocity. Rads or Meters per publish period. Units is [rad/s] or [m/s]. +low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less. + +## Properties of outgoing commands publish_period: 0.01 # 1/Nominal publish rate [seconds] -collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. -num_halt_msgs_to_publish: 1 # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish -# Publish boolean warnings to this topic -warning_topic: jog_server/warning -joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. -command_out_topic: jog_server/command # Find this topic by 'rostopic list' or 'rostopic list | grep command' or 'rqt_graph' +publish_delay: 0.005 # Delay between calculation and execution start of command + # What type of topic does your robot driver expect? -# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController) +# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) # or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) command_out_type: trajectory_msgs/JointTrajectory -# Can save some bandwidth as most robots only require positions or velocities + +# What to publish? Can save some bandwidth as most robots only require positions or velocities publish_joint_positions: true publish_joint_velocities: true publish_joint_accelerations: false + +## MoveIt properties +move_group_name: panda_arm # Often 'manipulator' or 'arm' +planning_frame: panda_link0 # The MoveIt planning frame. Often 'panda_link0' or 'world' + +## Stopping behaviour +incoming_command_timeout: 1 # Stop jogging if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 1 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 30 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 45 # Stop when the condition number hits this +joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + +## Topic names +cartesian_command_in_topic: jog_server/delta_jog_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: jog_server/joint_delta_jog_cmds # Topic for incoming joint angle commands +joint_topic: joint_states +warning_topic: jog_server/halted # Publish boolean warnings to this topic +command_out_topic: jog_server/command # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. +collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] +hard_stop_collision_proximity_threshold: 0.0005 # Stop when a collision is this far [m] diff --git a/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py b/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py index ca30f5f3ed..833cad8ea4 100755 --- a/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py +++ b/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py @@ -8,8 +8,11 @@ from control_msgs.msg import JointJog from trajectory_msgs.msg import JointTrajectory -JOG_ARM_SETTLE_TIME_S = 10 -ROS_SETTLE_TIME_S = 10 +# This can be run as part of a pytest, or like a normal ROS executable: +# rosrun moveit_jog_arm test_jog_arm_integration.py + +JOG_ARM_SETTLE_TIME_S = 3 +ROS_SETTLE_TIME_S = 3 JOINT_JOG_COMMAND_TOPIC = 'jog_server/joint_delta_jog_cmds' CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' @@ -47,6 +50,7 @@ def send_cmd(self, linear, angular): ts.twist.angular.x, ts.twist.angular.y, ts.twist.angular.z = angular self._pub.publish(ts) + def test_jog_arm_generates_joint_trajectory_when_joint_jog_command_is_received(node): sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda x: received.append(x) @@ -64,7 +68,7 @@ def test_jog_arm_generates_joint_trajectory_when_joint_jog_command_is_received(n cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) received = [] rospy.sleep(1) - assert len(received) <= 2 # 2 is 'num_halt_msgs_to_publish' in the config file + assert len(received) <= 2 # 2 is 'num_outgoing_halt_msgs_to_publish' in the config file # This nonzero command should produce jogging output # A subscriber in a different thread fills `received` @@ -74,6 +78,11 @@ def test_jog_arm_generates_joint_trajectory_when_joint_jog_command_is_received(n received = [] rospy.sleep(test_duration) # test_duration/publish_period is the expected number of messages in this duration. - # Allow a small +/- window due to rounding/timing errors + # Allow a small +/- window due to rounding/timing errors assert len(received) >= test_duration/publish_period - 5 assert len(received) <= test_duration/publish_period + 5 + + +if __name__ == '__main__': + node = node() + test_jog_arm_generates_joint_trajectory_when_joint_jog_command_is_received(node) From bc752b7c8aef3826df3409efad3756c832ccad7e Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Mon, 30 Dec 2019 22:57:13 +0100 Subject: [PATCH 136/612] Add atomic bool flags for terminating JogArm threads gracefully (#1816) --- .../moveit_jog_arm/collision_check_thread.h | 16 +- .../include/moveit_jog_arm/jog_calcs.h | 11 +- .../moveit_jog_arm/jog_cpp_interface.h | 9 +- .../moveit_jog_arm/jog_interface_base.h | 10 ++ .../src/collision_check_thread.cpp | 157 +++++++++--------- .../cpp_interface_example.cpp | 3 +- .../moveit_jog_arm/src/jog_calcs.cpp | 29 ++-- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 27 ++- .../moveit_jog_arm/src/jog_interface_base.cpp | 46 ++++- .../moveit_jog_arm/src/jog_ros_interface.cpp | 9 +- 10 files changed, 209 insertions(+), 108 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index 4cae9182a7..73ddd1780c 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -38,6 +38,7 @@ #pragma once +#include #include "jog_arm_data.h" #include "low_pass_filter.h" #include @@ -48,8 +49,19 @@ namespace moveit_jog_arm class CollisionCheckThread { public: - CollisionCheckThread(const moveit_jog_arm::JogArmParameters parameters, - moveit_jog_arm::JogArmShared& shared_variables, std::mutex& mutex, + CollisionCheckThread(const moveit_jog_arm::JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); + + void startMainLoop(moveit_jog_arm::JogArmShared& shared_variables, std::mutex& mutex); + + void stopMainLoop(); + +private: + // Loop termination flag + std::atomic stop_requested_; + + const moveit_jog_arm::JogArmParameters parameters_; + + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; }; } diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 70cfbf24a1..058aec32d9 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -38,6 +38,7 @@ #pragma once +#include #include "jog_arm_data.h" #include "low_pass_filter.h" #include @@ -49,12 +50,18 @@ namespace moveit_jog_arm class JogCalcs { public: - JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_variables, std::mutex& mutex, - const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); + JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); + + void startMainLoop(JogArmShared& shared_variables, std::mutex& mutex); + + void stopMainLoop(); protected: ros::NodeHandle nh_; + // Loop termination flag + std::atomic stop_requested_; + sensor_msgs::JointState incoming_joints_; bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables, std::mutex& mutex); diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h index 42fa925ae9..4b174a04f4 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h @@ -38,6 +38,7 @@ #pragma once +#include #include "jog_interface_base.h" namespace moveit_jog_arm @@ -51,7 +52,11 @@ class JogCppApi : JogInterfaceBase public: JogCppApi(); - void mainLoop(); + ~JogCppApi(); + + void startMainLoop(); + + void stopMainLoop(); // Provide a Cartesian velocity command to the jogger. // The units are determined by settings in the yaml file. @@ -70,5 +75,7 @@ class JogCppApi : JogInterfaceBase private: ros::NodeHandle nh_; + + std::atomic stop_requested_; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h index 3a39a7f87d..ed186be292 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h @@ -59,9 +59,11 @@ class JogInterfaceBase // Jogging calculation thread bool startJogCalcThread(); + bool stopJogCalcThread(); // Collision checking thread bool startCollisionCheckThread(); + bool stopCollisionCheckThread(); protected: bool readParameters(ros::NodeHandle& n); @@ -74,5 +76,13 @@ class JogInterfaceBase // Share data between threads JogArmShared shared_variables_; std::mutex shared_variables_mutex_; + + // Jog calcs + std::unique_ptr jog_calcs_; + std::unique_ptr jog_calc_thread_; + + // Collision checks + std::unique_ptr collision_checker_; + std::unique_ptr collision_check_thread_; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 83551f56c8..db121d13e4 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -41,96 +41,93 @@ namespace moveit_jog_arm { // Constructor for the class that handles collision checking -CollisionCheckThread::CollisionCheckThread(const moveit_jog_arm::JogArmParameters parameters, - JogArmShared& shared_variables, std::mutex& mutex, +CollisionCheckThread::CollisionCheckThread(const moveit_jog_arm::JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) + : parameters_(parameters) { - // If user specified true in yaml file - if (parameters.check_collisions) + // MoveIt Setup + while (ros::ok() && !model_loader_ptr) { - // MoveIt Setup - while (ros::ok() && !model_loader_ptr) - { - ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); - ros::Duration(WHILE_LOOP_WAIT).sleep(); - } - const robot_model::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); - planning_scene::PlanningScene planning_scene(kinematic_model); - collision_detection::CollisionRequest collision_request; - collision_request.group_name = parameters.move_group_name; - collision_request.distance = true; - collision_detection::CollisionResult collision_result; - robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst(); - - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; - planning_scene_monitor.reset(new planning_scene_monitor::PlanningSceneMonitor(model_loader_ptr)); - planning_scene_monitor->startSceneMonitor(); - planning_scene_monitor->startStateMonitor(); - - if (planning_scene_monitor->getPlanningScene()) - { - planning_scene_monitor->startSceneMonitor("/planning_scene"); - planning_scene_monitor->startWorldGeometryMonitor(); - planning_scene_monitor->startStateMonitor(); - } - else - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } + ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); + ros::Duration(WHILE_LOOP_WAIT).sleep(); + } - double velocity_scale_coefficient = -log(0.001) / parameters.collision_proximity_threshold; + planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(model_loader_ptr)); + planning_scene_monitor_->startSceneMonitor(); + planning_scene_monitor_->startStateMonitor(); - // Wait for initial messages - ROS_INFO_NAMED(LOGNAME, "collision_check_thread: Waiting for first joint msg."); - ros::topic::waitForMessage(parameters.joint_topic); - ROS_INFO_NAMED(LOGNAME, "collision_check_thread: Received first joint msg."); + if (planning_scene_monitor_->getPlanningScene()) + { + planning_scene_monitor_->startSceneMonitor("/planning_scene"); + planning_scene_monitor_->startWorldGeometryMonitor(); + planning_scene_monitor_->startStateMonitor(); + } + else + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); + exit(EXIT_FAILURE); + } +} - ROS_INFO_NAMED(LOGNAME, "Waiting for first command msg."); - ros::topic::waitForMessage(parameters.cartesian_command_in_topic); - ROS_INFO_NAMED(LOGNAME, "Received first command msg."); +void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) +{ + // Reset loop termination flag + stop_requested_ = false; - ros::Rate collision_rate(parameters.collision_check_rate); + // Init collision request and current state + collision_detection::CollisionRequest collision_request; + collision_request.group_name = parameters_.move_group_name; + collision_request.distance = true; + collision_detection::CollisionResult collision_result; + robot_state::RobotState& current_state = planning_scene_monitor_->getPlanningScene()->getCurrentStateNonConst(); - ///////////////////////////////////////////////// - // Spin while checking collisions - ///////////////////////////////////////////////// - while (ros::ok()) + double velocity_scale_coefficient = -log(0.001) / parameters_.collision_proximity_threshold; + + ros::Rate collision_rate(parameters_.collision_check_rate); + + ///////////////////////////////////////////////// + // Spin while checking collisions + ///////////////////////////////////////////////// + while (ros::ok() && !stop_requested_) + { + mutex.lock(); + sensor_msgs::JointState jts = shared_variables.joints; + mutex.unlock(); + + for (std::size_t i = 0; i < jts.position.size(); ++i) + current_state.setJointPositions(jts.name[i], &jts.position[i]); + + collision_result.clear(); + planning_scene_monitor_->getPlanningScene()->checkCollision(collision_request, collision_result, current_state); + + // Scale robot velocity according to collision proximity and user-defined thresholds. + // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. + // Proximity decreasing --> decelerate + double velocity_scale = 1; + + // If we are far from a collision, velocity_scale should be 1. + // If we are very close to a collision, velocity_scale should be ~zero. + // When collision_proximity_threshold is breached, start decelerating exponentially. + if (collision_result.distance < parameters_.collision_proximity_threshold) { - mutex.lock(); - sensor_msgs::JointState jts = shared_variables.joints; - mutex.unlock(); - - for (std::size_t i = 0; i < jts.position.size(); ++i) - current_state.setJointPositions(jts.name[i], &jts.position[i]); - - collision_result.clear(); - planning_scene_monitor->getPlanningScene()->checkCollision(collision_request, collision_result, current_state); - - // Scale robot velocity according to collision proximity and user-defined thresholds. - // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. - // Proximity decreasing --> decelerate - double velocity_scale = 1; - - // If we are far from a collision, velocity_scale should be 1. - // If we are very close to a collision, velocity_scale should be ~zero. - // When collision_proximity_threshold is breached, start decelerating exponentially. - if (collision_result.distance < parameters.collision_proximity_threshold) - { - // velocity_scale = e ^ k * (collision_distance - threshold) - // k = - ln(0.001) / collision_proximity_threshold - // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. - // velocity_scale should equal 0.001 when collision_distance is at zero. - velocity_scale = - exp(velocity_scale_coefficient * (collision_result.distance - parameters.collision_proximity_threshold)); - } - - mutex.lock(); - shared_variables.collision_velocity_scale = velocity_scale; - mutex.unlock(); - - collision_rate.sleep(); + // velocity_scale = e ^ k * (collision_distance - threshold) + // k = - ln(0.001) / collision_proximity_threshold + // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. + // velocity_scale should equal 0.001 when collision_distance is at zero. + velocity_scale = + exp(velocity_scale_coefficient * (collision_result.distance - parameters_.collision_proximity_threshold)); } + + mutex.lock(); + shared_variables.collision_velocity_scale = velocity_scale; + mutex.unlock(); + + collision_rate.sleep(); } } + +void CollisionCheckThread::stopMainLoop() +{ + stop_requested_ = true; +} } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index 921f803a8d..d51e5c9dcc 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -49,7 +49,7 @@ int main(int argc, char** argv) // Run the jogging C++ interface in a new thread to ensure a constant outgoing message rate. moveit_jog_arm::JogCppApi jog_interface; - std::thread jogging_thread(&moveit_jog_arm::JogCppApi::mainLoop, &jog_interface); + std::thread jogging_thread([&]() { jog_interface.startMainLoop(); }); // Make a Cartesian velocity message geometry_msgs::TwistStamped velocity_msg; @@ -93,6 +93,7 @@ int main(int argc, char** argv) sensor_msgs::JointState current_joint_state = jog_interface.getJointState(); ROS_INFO_STREAM(current_joint_state); + jog_interface.stopMainLoop(); jogging_thread.join(); return 0; } diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 975e4aaf69..c5f6648a47 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -41,8 +41,7 @@ namespace moveit_jog_arm { // Constructor for the class that handles jogging calculations -JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_variables, std::mutex& mutex, - const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) +JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) : parameters_(parameters) { // Publish collision status @@ -59,14 +58,18 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari kinematic_state_->setToDefaultValues(); joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); +} + +void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) +{ + // Reset loop termination flag + stop_requested_ = false; // Wait for initial messages ROS_INFO_NAMED(LOGNAME, "jog_calcs_thread: Waiting for first joint msg."); ros::topic::waitForMessage(parameters_.joint_topic); ROS_INFO_NAMED(LOGNAME, "jog_calcs_thread: Received first joint msg."); - resetVelocityFilters(); - joint_state_.name = joint_model_group_->getVariableNames(); num_joints_ = joint_state_.name.size(); joint_state_.position.resize(num_joints_); @@ -86,7 +89,7 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari } // Initialize the position filters to initial robot joints - while (!updateJoints() && ros::ok()) + while (!updateJoints() && ros::ok() && !stop_requested_) { mutex.lock(); incoming_joints_ = shared_variables.joints; @@ -100,7 +103,8 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari // Store it in a class member for further calcs, then free up the shared variable again. geometry_msgs::TwistStamped cartesian_deltas; control_msgs::JointJog joint_deltas; - while (ros::ok() && (cartesian_deltas.header.stamp == ros::Time(0.)) && (joint_deltas.header.stamp == ros::Time(0.))) + while (ros::ok() && !stop_requested_ && (cartesian_deltas.header.stamp == ros::Time(0.)) && + (joint_deltas.header.stamp == ros::Time(0.))) { ros::Duration(WHILE_LOOP_WAIT).sleep(); @@ -117,7 +121,7 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari ros::Rate loop_rate(1. / parameters_.publish_period); // Now do jogging calcs - while (ros::ok()) + while (ros::ok() && !stop_requested_) { // Flag that incoming commands are all zero. May be used to skip calculations/publication mutex.lock(); @@ -144,7 +148,7 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari mutex.unlock(); // Initialize the position filters to initial robot joints - while (!updateJoints() && ros::ok()) + while (!updateJoints() && ros::ok() && !stop_requested_) { mutex.lock(); incoming_joints_ = shared_variables.joints; @@ -234,6 +238,11 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, JogArmShared& shared_vari } } +void JogCalcs::stopMainLoop() +{ + stop_requested_ = true; +} + // Perform the jogging calculations bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables, std::mutex& mutex) { @@ -648,7 +657,7 @@ bool JogCalcs::updateJoints() } catch (const std::out_of_range& e) { - ROS_ERROR_STREAM_THROTTLE_NAMED(5, LOGNAME, "Command joint name unknown."); + ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joints_.name[m]); continue; } @@ -707,7 +716,7 @@ Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& comman } catch (const std::out_of_range& e) { - ROS_ERROR_STREAM_THROTTLE_NAMED(5, LOGNAME, "Command joint name unknown."); + ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joints_.name[m]); continue; } // Apply user-defined scaling if inputs are unitless [-1:1] diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp index 5064beba98..b964b61e2d 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp @@ -50,13 +50,22 @@ JogCppApi::JogCppApi() model_loader_ptr_ = std::shared_ptr(new robot_model_loader::RobotModelLoader); } -void JogCppApi::mainLoop() +JogCppApi::~JogCppApi() { + stopMainLoop(); +} + +void JogCppApi::startMainLoop() +{ + // Reset loop termination flag + stop_requested_ = false; + // Crunch the numbers in this thread - std::thread jogging_thread(&JogInterfaceBase::startJogCalcThread, dynamic_cast(this)); + startJogCalcThread(); // Check collisions in this thread - std::thread collision_thread(&JogInterfaceBase::startCollisionCheckThread, dynamic_cast(this)); + if (ros_parameters_.check_collisions) + startCollisionCheckThread(); // ROS subscriptions. Share the data with the worker threads ros::Subscriber joints_sub = @@ -80,7 +89,7 @@ void JogCppApi::mainLoop() ros::Rate main_rate(1. / ros_parameters_.publish_period); - while (ros::ok()) + while (ros::ok() && !stop_requested_) { ros::spinOnce(); @@ -133,8 +142,14 @@ void JogCppApi::mainLoop() main_rate.sleep(); } - jogging_thread.join(); - collision_thread.join(); + ROS_ERROR("Stopping main loop"); + stopJogCalcThread(); + stopCollisionCheckThread(); +} + +void JogCppApi::stopMainLoop() +{ + stop_requested_ = true; } void JogCppApi::provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index f838bdcdaf..1238a4043f 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -190,14 +190,56 @@ void JogInterfaceBase::jointsCB(const sensor_msgs::JointStateConstPtr& msg) // A separate thread for the heavy jogging calculations. bool JogInterfaceBase::startJogCalcThread() { - JogCalcs ja(ros_parameters_, shared_variables_, shared_variables_mutex_, model_loader_ptr_); + if (!jog_calcs_) + jog_calcs_.reset(new JogCalcs(ros_parameters_, model_loader_ptr_)); + + jog_calc_thread_.reset( + new std::thread([&]() { jog_calcs_->startMainLoop(shared_variables_, shared_variables_mutex_); })); + + return true; +} + +bool JogInterfaceBase::stopJogCalcThread() +{ + if (jog_calcs_) + jog_calcs_->stopMainLoop(); + + if (jog_calc_thread_) + { + if (jog_calc_thread_->joinable()) + jog_calc_thread_->join(); + + jog_calc_thread_.reset(); + } + return true; } // A separate thread for collision checking. bool JogInterfaceBase::startCollisionCheckThread() { - CollisionCheckThread cc(ros_parameters_, shared_variables_, shared_variables_mutex_, model_loader_ptr_); + if (!collision_checker_) + collision_checker_.reset(new CollisionCheckThread(ros_parameters_, model_loader_ptr_)); + + collision_check_thread_.reset( + new std::thread([&]() { collision_checker_->startMainLoop(shared_variables_, shared_variables_mutex_); })); + + return true; +} + +bool JogInterfaceBase::stopCollisionCheckThread() +{ + if (collision_checker_) + collision_checker_->stopMainLoop(); + + if (collision_check_thread_) + { + if (collision_check_thread_->joinable()) + collision_check_thread_->join(); + + collision_check_thread_.reset(); + } + return true; } } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp index 978d523273..bf001ab3e6 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp @@ -61,10 +61,11 @@ JogROSInterface::JogROSInterface() model_loader_ptr_ = std::shared_ptr(new robot_model_loader::RobotModelLoader); // Crunch the numbers in this thread - std::thread jogging_thread(&JogInterfaceBase::startJogCalcThread, dynamic_cast(this)); + startJogCalcThread(); // Check collisions in this thread - std::thread collision_thread(&JogInterfaceBase::startCollisionCheckThread, dynamic_cast(this)); + if (ros_parameters_.check_collisions) + startCollisionCheckThread(); // ROS subscriptions. Share the data with the worker threads ros::Subscriber cmd_sub = @@ -146,8 +147,8 @@ JogROSInterface::JogROSInterface() main_rate.sleep(); } - jogging_thread.join(); - collision_thread.join(); + stopJogCalcThread(); + stopCollisionCheckThread(); } // Listen to cartesian delta commands. Store them in a shared variable. From 1d0ccdfd7be184036bf4920b14d4341d269487d6 Mon Sep 17 00:00:00 2001 From: Max Krichenbauer Date: Tue, 31 Dec 2019 17:22:05 +0100 Subject: [PATCH 137/612] Add the conditional branch scope when the condition is false in inport_from_text.cpp. (#1818) --- moveit_ros/warehouse/warehouse/src/import_from_text.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) mode change 100644 => 100755 moveit_ros/warehouse/warehouse/src/import_from_text.cpp diff --git a/moveit_ros/warehouse/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/warehouse/src/import_from_text.cpp old mode 100644 new mode 100755 index 78fcdfd91b..d1e6229831 --- a/moveit_ros/warehouse/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/warehouse/src/import_from_text.cpp @@ -73,8 +73,10 @@ void parseStart(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* if (marker != "=") joint = "."; else + { in >> value; - v[joint] = value; + v[joint] = value; + } if (joint != ".") in >> joint; } From 312c354ee561d0bdd6653f26f3b82910f1a4710e Mon Sep 17 00:00:00 2001 From: Pavel-P Date: Tue, 31 Dec 2019 12:51:36 -0500 Subject: [PATCH 138/612] Fix planning scene interface not respecting custom namespace (#1815) --- .../src/moveit_commander/planning_scene_interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_commander/src/moveit_commander/planning_scene_interface.py b/moveit_commander/src/moveit_commander/planning_scene_interface.py index ff393c67de..43c6769cd2 100644 --- a/moveit_commander/src/moveit_commander/planning_scene_interface.py +++ b/moveit_commander/src/moveit_commander/planning_scene_interface.py @@ -60,8 +60,8 @@ def __init__(self, ns='', synchronous=False, service_timeout=5.0): """ Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics. """ self._psi = _moveit_planning_scene_interface.PlanningSceneInterface(ns) - self._pub_co = rospy.Publisher('/collision_object', CollisionObject, queue_size=100) - self._pub_aco = rospy.Publisher('/attached_collision_object', AttachedCollisionObject, queue_size=100) + self._pub_co = rospy.Publisher(ns + '/collision_object', CollisionObject, queue_size=100) + self._pub_aco = rospy.Publisher(ns + '/attached_collision_object', AttachedCollisionObject, queue_size=100) self.__synchronous = synchronous if self.__synchronous: self._apply_planning_scene_diff = rospy.ServiceProxy('apply_planning_scene', ApplyPlanningScene) From f82ed9e9a3e5a1076f3f50a9852a1a01c88c7418 Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Wed, 1 Jan 2020 02:51:25 -0800 Subject: [PATCH 139/612] Delete attached body before adding a new one with same id (#1821) --- moveit_core/robot_state/src/robot_state.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index abf7034f98..6fea83bf05 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -880,6 +880,9 @@ const AttachedBody* RobotState::getAttachedBody(const std::string& id) const void RobotState::attachBody(AttachedBody* attached_body) { + // If an attached body with the same id exists, remove it + clearAttachedBody(attached_body->getName()); + attached_body_map_[attached_body->getName()] = attached_body; attached_body->computeTransform(getGlobalLinkTransform(attached_body->getAttachedLink())); if (attached_body_update_callback_) From b35dac6bb6bacec1c08a2c9a99b68b081dab2f5b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 18 Dec 2019 23:11:13 +0100 Subject: [PATCH 140/612] remove unset ${OGRE_LIBRARIES} from cmake files These libs are pulled in from rviz. --- .../visualization/motion_planning_rviz_plugin/CMakeLists.txt | 2 +- .../visualization/planning_scene_rviz_plugin/CMakeLists.txt | 2 +- .../visualization/robot_state_rviz_plugin/CMakeLists.txt | 3 +-- .../visualization/rviz_plugin_render_tools/CMakeLists.txt | 1 - moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt | 2 +- 5 files changed, 4 insertions(+), 6 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt index 938e0a86e2..cfc3325fb5 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt @@ -31,7 +31,7 @@ set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NA target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools moveit_planning_scene_rviz_plugin_core - ${catkin_LIBRARIES} ${rviz_DEFAULT_PLUGIN_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) + ${catkin_LIBRARIES} ${rviz_DEFAULT_PLUGIN_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) add_library(${MOVEIT_LIB_NAME} src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt index 109bb569f6..a5d5a1d205 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt @@ -4,7 +4,7 @@ add_library(${MOVEIT_LIB_NAME}_core src/planning_scene_display.cpp include/moveit/planning_scene_rviz_plugin/planning_scene_display.h) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools ${catkin_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools ${catkin_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) add_library(${MOVEIT_LIB_NAME} src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt index 8db0bf29fc..e70644888d 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt @@ -5,7 +5,7 @@ add_library(${MOVEIT_LIB_NAME}_core include/moveit/robot_state_rviz_plugin/robot_state_display.h) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools - ${catkin_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) + ${catkin_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) add_library(${MOVEIT_LIB_NAME} src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -17,4 +17,3 @@ install(TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) - diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt index a9769d25d1..000aac5ecc 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt +++ b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt @@ -26,7 +26,6 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} - ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} ) diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt index 97b09ca864..1708a975e9 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt @@ -16,7 +16,7 @@ set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NA target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools moveit_planning_scene_rviz_plugin_core - ${catkin_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} + ${catkin_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} ) # Plugin Initializer From f9716d47c43c045a2aee604eaef19c5a63864660 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 18 Dec 2019 23:19:05 +0100 Subject: [PATCH 141/612] fixes for Ogre 1.10..1.12 --- .../src/planning_scene_display.cpp | 2 +- .../moveit/rviz_plugin_render_tools/octomap_render.h | 10 +--------- .../src/planning_scene_render.cpp | 2 +- 3 files changed, 3 insertions(+), 11 deletions(-) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index d04a3f25c4..33af096a85 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -162,7 +162,7 @@ PlanningSceneDisplay::~PlanningSceneDisplay() planning_scene_render_.reset(); if (context_ && context_->getSceneManager() && planning_scene_node_) - context_->getSceneManager()->destroySceneNode(planning_scene_node_->getName()); + context_->getSceneManager()->destroySceneNode(planning_scene_node_); if (planning_scene_robot_) planning_scene_robot_.reset(); planning_scene_monitor_.reset(); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h index fb11b11fe8..edffaeed3f 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h @@ -44,21 +44,13 @@ SILENT_UNUSED_PARAM #include DIAGNOSTIC_POP #include +#include namespace octomap { class OcTree; } -namespace Ogre -{ -class SceneManager; -class SceneNode; -class AxisAlignedBox; -class Vector3; -class Quaternion; -} - namespace moveit_rviz_plugin { enum OctreeVoxelRenderMode diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp index 4ec1d4aa4e..fdfe93fc59 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp @@ -53,7 +53,7 @@ PlanningSceneRender::PlanningSceneRender(Ogre::SceneNode* node, rviz::DisplayCon PlanningSceneRender::~PlanningSceneRender() { - context_->getSceneManager()->destroySceneNode(planning_scene_geometry_node_->getName()); + context_->getSceneManager()->destroySceneNode(planning_scene_geometry_node_); } void PlanningSceneRender::clear() From d8678597530a8cbdb4af5075cd230fdaf1af6aae Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 1 Jan 2020 12:19:14 +0100 Subject: [PATCH 142/612] Cleanup OcTreeRender's constructor interface (#1822) Require a valid parent_node to be passed. Modified cherry-pick of #1817. --- .../moveit/rviz_plugin_render_tools/octomap_render.h | 4 +--- .../rviz_plugin_render_tools/src/octomap_render.cpp | 8 +------- .../rviz_plugin_render_tools/src/render_shapes.cpp | 2 +- 3 files changed, 3 insertions(+), 11 deletions(-) mode change 100644 => 100755 moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h index edffaeed3f..1e086e2156 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h @@ -69,8 +69,7 @@ class OcTreeRender { public: OcTreeRender(const std::shared_ptr& octree, OctreeVoxelRenderMode octree_voxel_rendering, - OctreeVoxelColorMode octree_color_mode, std::size_t max_octree_depth, Ogre::SceneManager* scene_manager, - Ogre::SceneNode* parent_node); + OctreeVoxelColorMode octree_color_mode, std::size_t max_octree_depth, Ogre::SceneNode* parent_node); virtual ~OcTreeRender(); void setPosition(const Ogre::Vector3& position); @@ -88,7 +87,6 @@ class OcTreeRender std::shared_ptr octree_; Ogre::SceneNode* scene_node_; - Ogre::SceneManager* scene_manager_; double colorFactor_; std::size_t octree_depth_; diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp old mode 100644 new mode 100755 index e0ff33fbff..00851951eb --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp @@ -54,15 +54,9 @@ typedef std::vector VVPoint; OcTreeRender::OcTreeRender(const std::shared_ptr& octree, OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode, - std::size_t max_octree_depth, Ogre::SceneManager* /*scene_manager*/, - Ogre::SceneNode* parent_node = nullptr) + std::size_t max_octree_depth, Ogre::SceneNode* parent_node) : octree_(octree), colorFactor_(0.8) { - if (!parent_node) - { - parent_node = scene_manager_->getRootSceneNode(); - } - if (!max_octree_depth) { octree_depth_ = octree->getTreeDepth(); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index 3a0b6b7ee0..55ad73109e 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -161,7 +161,7 @@ void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, co case shapes::OCTREE: { OcTreeRenderPtr octree(new OcTreeRender(static_cast(s)->octree, octree_voxel_rendering, - octree_color_mode, 0u, context_->getSceneManager(), node)); + octree_color_mode, 0u, node)); octree->setPosition(position); octree->setOrientation(orientation); octree_voxel_grids_.push_back(octree); From 844a0212865d66541a3d6dc6ac6abef23b0da526 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 3 Jan 2020 14:06:47 -0700 Subject: [PATCH 143/612] update link transforms before calling checkCollision on robot state in jog_arm (#1825) --- .../moveit_jog_arm/src/collision_check_thread.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index db121d13e4..de07e0a817 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -98,6 +98,7 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables, std::mu current_state.setJointPositions(jts.name[i], &jts.position[i]); collision_result.clear(); + current_state.updateCollisionBodyTransforms(); planning_scene_monitor_->getPlanningScene()->checkCollision(collision_request, collision_result, current_state); // Scale robot velocity according to collision proximity and user-defined thresholds. From d2c515ac99bf46474371a770cffe6321dce0deb9 Mon Sep 17 00:00:00 2001 From: Max Krichenbauer Date: Thu, 9 Jan 2020 08:20:10 +0100 Subject: [PATCH 144/612] Add delete process to the doneEditing() function in end_effectors_widgets (#1829) - Correct the indication by the static analysis tool. Co-authored-by: Kazuki-Hazama <57083115+KazukiHazama@users.noreply.github.com> --- moveit_setup_assistant/src/widgets/end_effectors_widget.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp index a0d53ee39f..b66487a929 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp @@ -564,6 +564,7 @@ void EndEffectorsWidget::doneEditing() if (is_new) { config_data_->srdf_->end_effectors_.push_back(*searched_data); + delete searched_data; } // Finish up ------------------------------------------------------ From 16715632306dd0e70248198c937e26d5465e42c2 Mon Sep 17 00:00:00 2001 From: Max Krichenbauer Date: Thu, 9 Jan 2020 11:54:38 +0100 Subject: [PATCH 145/612] Fix potential memory leak in RDFLoader (#1828) Use smart pointers to avoid explicit new/delete (and fix missing deletion of urdf model in error case). Explicitly express with the code that the internal pointers urdf_ and srdf_ should be initialized only if their initialization was successful. --- moveit_ros/planning/rdf_loader/src/rdf_loader.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp index d4885f1fd3..3c412ef5ee 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -70,13 +70,13 @@ rdf_loader::RDFLoader::RDFLoader(const std::string& robot_description) return; } - urdf::Model* umodel = new urdf::Model(); - if (!umodel->initString(content)) + std::unique_ptr urdf(new urdf::Model()); + if (!urdf->initString(content)) { ROS_ERROR_NAMED("rdf_loader", "Unable to parse URDF from parameter '%s'", robot_description_.c_str()); return; } - urdf_.reset(umodel); + urdf_ = std::move(urdf); const std::string srdf_description(robot_description_ + "_semantic"); std::string scontent; @@ -87,13 +87,13 @@ rdf_loader::RDFLoader::RDFLoader(const std::string& robot_description) return; } - srdf_.reset(new srdf::Model()); - if (!srdf_->initString(*urdf_, scontent)) + srdf::ModelSharedPtr srdf(new srdf::Model()); + if (!srdf->initString(*urdf_, scontent)) { ROS_ERROR_NAMED("rdf_loader", "Unable to parse SRDF from parameter '%s'", srdf_description.c_str()); - srdf_.reset(); return; } + srdf_ = std::move(srdf); ROS_DEBUG_STREAM_NAMED("rdf", "Loaded robot model in " << (ros::WallTime::now() - start).toSec() << " seconds"); } From cb5945b69f4dfbd1e5819a2b7bee3e256aee766c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Jan 2020 12:29:32 +0100 Subject: [PATCH 146/612] Cleanup: use range-based for-loop (#1830) --- .../src/widgets/end_effectors_widget.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp index b66487a929..1e31472d13 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp @@ -486,13 +486,12 @@ void EndEffectorsWidget::doneEditing() } // Check that the effector name is unique - for (std::vector::const_iterator data_it = config_data_->srdf_->end_effectors_.begin(); - data_it != config_data_->srdf_->end_effectors_.end(); ++data_it) + for (const auto& eef : config_data_->srdf_->end_effectors_) { - if (data_it->name_.compare(effector_name) == 0) // the names are the same + if (eef.name_ == effector_name) { // is this our existing effector? check if effector pointers are same - if (&(*data_it) != searched_data) + if (&eef != searched_data) { QMessageBox::warning( this, "Error Saving", From a8913a71fdf46c1a47a51dbcdc003fc10fb5f113 Mon Sep 17 00:00:00 2001 From: Jens P Date: Wed, 26 Jun 2019 19:57:31 +0200 Subject: [PATCH 147/612] Empty collision checker template for usage with tesseract and bullet (#1499) * Adding documentation to collision detection (#1488) * CMake adaptions for Tesseract integration * Empty collision detector template for tesseract and bullet * Making bullet as a collision plugin available * Adding missing error messages * Adding new folders and libraries to cmake * Fixing the BSD license * clang-format --- moveit_core/CMakeLists.txt | 3 + .../collision_detection_bullet/CMakeLists.txt | 29 ++++ .../collision_detection_bullet/README.md | 6 + .../collision_common.h | 48 ++++++ .../collision_detector_allocator_bt.h | 55 ++++++ .../collision_detector_bt_plugin_loader.h | 51 ++++++ .../collision_robot_bt.h | 94 +++++++++++ .../collision_world_bt.h | 86 ++++++++++ .../src/collision_common.cpp | 43 +++++ .../collision_detector_bt_plugin_loader.cpp | 49 ++++++ .../src/collision_robot_bt.cpp | 143 ++++++++++++++++ .../src/collision_world_bt.cpp | 156 ++++++++++++++++++ .../collision_detector_bt_description.xml | 8 + moveit_core/package.xml | 1 + 14 files changed, 772 insertions(+) create mode 100644 moveit_core/collision_detection_bullet/CMakeLists.txt create mode 100644 moveit_core/collision_detection_bullet/README.md create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_common.h create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bt.h create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bt_plugin_loader.h create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h create mode 100644 moveit_core/collision_detection_bullet/src/collision_common.cpp create mode 100644 moveit_core/collision_detection_bullet/src/collision_detector_bt_plugin_loader.cpp create mode 100644 moveit_core/collision_detection_bullet/src/collision_robot_bt.cpp create mode 100644 moveit_core/collision_detection_bullet/src/collision_world_bt.cpp create mode 100644 moveit_core/collision_detector_bt_description.xml diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 64f05c178d..14207baef4 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -71,6 +71,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS backtrace/include collision_detection/include collision_detection_fcl/include + collision_detection_bullet/include constraint_samplers/include controller_manager/include distance_field/include @@ -107,6 +108,7 @@ catkin_package( moveit_planning_interface moveit_collision_detection moveit_collision_detection_fcl + moveit_collision_detection_bullet moveit_kinematic_constraints moveit_planning_scene moveit_constraint_samplers @@ -186,6 +188,7 @@ add_subdirectory(robot_state) add_subdirectory(robot_trajectory) add_subdirectory(collision_detection) add_subdirectory(collision_detection_fcl) +add_subdirectory(collision_detection_bullet) add_subdirectory(kinematic_constraints) add_subdirectory(planning_scene) add_subdirectory(constraint_samplers) diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt new file mode 100644 index 0000000000..3e87ca6c78 --- /dev/null +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -0,0 +1,29 @@ +set(MOVEIT_LIB_NAME moveit_collision_detection_bullet) + +add_library(${MOVEIT_LIB_NAME} + src/collision_common.cpp + src/collision_robot_bt.cpp + src/collision_world_bt.cpp +) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +target_link_libraries(${MOVEIT_LIB_NAME} moveit_collision_detection ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${LIBFCL_LIBRARIES} ${Boost_LIBRARIES}) +add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) + +add_library(collision_detector_bt_plugin src/collision_detector_bt_plugin_loader.cpp) +set_target_properties(collision_detector_bt_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(collision_detector_bt_plugin ${catkin_LIBRARIES} ${MOVEIT_LIB_NAME}) + + +install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_bt_plugin + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + +install(FILES ../collision_detector_bt_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +if(CATKIN_ENABLE_TESTING) + find_package(moveit_resources REQUIRED) + include_directories(${moveit_resources_INCLUDE_DIRS}) +endif() diff --git a/moveit_core/collision_detection_bullet/README.md b/moveit_core/collision_detection_bullet/README.md new file mode 100644 index 0000000000..c0b85486c0 --- /dev/null +++ b/moveit_core/collision_detection_bullet/README.md @@ -0,0 +1,6 @@ +### Using Bullet as a collision checker [experimental] +To use Bullet as a collision checker you can: +a) create a new planning scene and manually switch to Bullet using the planning_scene.setActiveCollisionDetector() +b) manually switch to Bullet using the `CollisionPluginLoader` +c) manually instantiate a `CollisionRobotBt` and `CollisionWorldBt` +d) use rosparam to set the collision checker (preferred but not tested yet) when starting a new `move_group` diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_common.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_common.h new file mode 100644 index 0000000000..ac0a27aa40 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_common.h @@ -0,0 +1,48 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_COMMON_ +#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_COMMON_ + +#include +#include + +namespace collision_detection +{ +// TODO: Add common functionality for all Bullet collision checkers. +} + +#endif diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bt.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bt.h new file mode 100644 index 0000000000..b914e8c2a5 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bt.h @@ -0,0 +1,55 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_BT_H_ +#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_BT_H_ + +#include +#include +#include + +namespace collision_detection +{ +/** \brief An allocator for Bullet collision detectors */ +class CollisionDetectorAllocatorBt + : public CollisionDetectorAllocatorTemplate +{ +public: + static const std::string NAME; // defined in collision_world_bt.cpp +}; +} + +#endif diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bt_plugin_loader.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bt_plugin_loader.h new file mode 100644 index 0000000000..d60a29ce64 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bt_plugin_loader.h @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_DETECTOR_BT_PLUGIN_LOADER_H_ +#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_DETECTOR_BT_PLUGIN_LOADER_H_ + +#include +#include + +namespace collision_detection +{ +class CollisionDetectorBtPluginLoader : public CollisionPlugin +{ +public: + virtual bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const; +}; +} +#endif // MOVEIT_COLLISION_DETECTION_BT_COLLISION_DETECTOR_BT_PLUGIN_LOADER_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h new file mode 100644 index 0000000000..a0aadf4b4a --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h @@ -0,0 +1,94 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_ROBOT_ +#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_ROBOT_ + +#include + +namespace collision_detection +{ +class CollisionRobotBt : public CollisionRobot +{ + friend class CollisionWorldBt; + +public: + CollisionRobotBt(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); + + CollisionRobotBt(const CollisionRobotBt& other); + + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const override; + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const override; + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, + const robot_state::RobotState& state2, const AllowedCollisionMatrix& acm) const override; + + void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const CollisionRobot& other_robot, + const robot_state::RobotState& other_state) const override; + void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const CollisionRobot& other_robot, const robot_state::RobotState& other_state, + const AllowedCollisionMatrix& acm) const override; + void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, + const robot_state::RobotState& state2, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state1, + const robot_state::RobotState& other_state2) const override; + void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, + const robot_state::RobotState& state2, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state1, const robot_state::RobotState& other_state2, + const AllowedCollisionMatrix& acm) const override; + + void distanceSelf(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const override; + + void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state, + const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override; + +protected: + void updatedPaddingOrScaling(const std::vector& links) override; + + void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const; + void checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state, const AllowedCollisionMatrix* acm) const; +}; +} + +#endif diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h new file mode 100644 index 0000000000..fb614b3c18 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h @@ -0,0 +1,86 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_WORLD_BT_ +#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_WORLD_BT_ + +#include + +namespace collision_detection +{ +class CollisionWorldBt : public CollisionWorld +{ +public: + CollisionWorldBt(); + explicit CollisionWorldBt(const WorldPtr& world); + CollisionWorldBt(const CollisionWorldBt& other, const WorldPtr& world); + ~CollisionWorldBt() override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, + const robot_state::RobotState& state) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, + const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, + const robot_state::RobotState& state1, const robot_state::RobotState& state2) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const override; + void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionWorld& other_world) const override; + void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, + const AllowedCollisionMatrix& acm) const override; + + void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, + const robot_state::RobotState& state) const override; + + void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override; + + void setWorld(const WorldPtr& world) override; + +protected: + void checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, + const AllowedCollisionMatrix* acm) const; + void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, + const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; + +private: + void initialize(); + void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); + World::ObserverHandle observer_handle_; +}; +} + +#endif diff --git a/moveit_core/collision_detection_bullet/src/collision_common.cpp b/moveit_core/collision_detection_bullet/src/collision_common.cpp new file mode 100644 index 0000000000..ecae7a988f --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/collision_common.cpp @@ -0,0 +1,43 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +namespace collision_detection +{ +// TODO: Add common functionality for all collision detection here. +} // namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/collision_detector_bt_plugin_loader.cpp b/moveit_core/collision_detection_bullet/src/collision_detector_bt_plugin_loader.cpp new file mode 100644 index 0000000000..461ea927e3 --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/collision_detector_bt_plugin_loader.cpp @@ -0,0 +1,49 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +namespace collision_detection +{ +bool CollisionDetectorBtPluginLoader::initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const +{ + scene->setActiveCollisionDetector(CollisionDetectorAllocatorBt::create(), exclusive); + return true; +} +} + +PLUGINLIB_EXPORT_CLASS(collision_detection::CollisionDetectorBtPluginLoader, collision_detection::CollisionPlugin) diff --git a/moveit_core/collision_detection_bullet/src/collision_robot_bt.cpp b/moveit_core/collision_detection_bullet/src/collision_robot_bt.cpp new file mode 100644 index 0000000000..95755456fe --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/collision_robot_bt.cpp @@ -0,0 +1,143 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include + +namespace collision_detection +{ +CollisionRobotBt::CollisionRobotBt(const robot_model::RobotModelConstPtr& model, double padding, double scale) + : CollisionRobot(model, padding, scale) +{ +} + +CollisionRobotBt::CollisionRobotBt(const CollisionRobotBt& other) : CollisionRobot(other) +{ +} + +void CollisionRobotBt::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const +{ + checkSelfCollisionHelper(req, res, state, nullptr); +} + +void CollisionRobotBt::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const +{ + checkSelfCollisionHelper(req, res, state, &acm); +} + +void CollisionRobotBt::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); +} + +void CollisionRobotBt::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); +} + +void CollisionRobotBt::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet self collision checking not yet implemented"); +} + +void CollisionRobotBt::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state) const +{ + checkOtherCollisionHelper(req, res, state, other_robot, other_state, nullptr); +} + +void CollisionRobotBt::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state, + const AllowedCollisionMatrix& acm) const +{ + checkOtherCollisionHelper(req, res, state, other_robot, other_state, &acm); +} + +void CollisionRobotBt::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const CollisionRobot& other_robot, + const robot_state::RobotState& other_state1, + const robot_state::RobotState& other_state2) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); +} + +void CollisionRobotBt::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const CollisionRobot& other_robot, + const robot_state::RobotState& other_state1, + const robot_state::RobotState& other_state2, + const AllowedCollisionMatrix& acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); +} + +void CollisionRobotBt::checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const CollisionRobot& other_robot, + const robot_state::RobotState& other_state, + const AllowedCollisionMatrix* acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Other collision not implemented yet."); +} + +void CollisionRobotBt::updatedPaddingOrScaling(const std::vector& links) +{ +} + +void CollisionRobotBt::distanceSelf(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Collision distance to self not implemented yet."); +} + +void CollisionRobotBt::distanceOther(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Collision distance to other not implemented yet."); +} + +} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/collision_world_bt.cpp b/moveit_core/collision_detection_bullet/src/collision_world_bt.cpp new file mode 100644 index 0000000000..530c2d8d00 --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/collision_world_bt.cpp @@ -0,0 +1,156 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +#include + +namespace collision_detection +{ +const std::string CollisionDetectorAllocatorBt::NAME("Bullet"); + +CollisionWorldBt::CollisionWorldBt() : CollisionWorld() +{ + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBt::notifyObjectChange, this, _1, _2)); +} + +CollisionWorldBt::CollisionWorldBt(const WorldPtr& world) : CollisionWorld(world) +{ + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBt::notifyObjectChange, this, _1, _2)); + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +CollisionWorldBt::CollisionWorldBt(const CollisionWorldBt& other, const WorldPtr& world) : CollisionWorld(other, world) +{ + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBt::notifyObjectChange, this, _1, _2)); +} + +CollisionWorldBt::~CollisionWorldBt() +{ + getWorld()->removeObserver(observer_handle_); +} + +void CollisionWorldBt::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state) const +{ + checkRobotCollisionHelper(req, res, robot, state, nullptr); +} + +void CollisionWorldBt::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + checkRobotCollisionHelper(req, res, robot, state, &acm); +} + +void CollisionWorldBt::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); +} + +void CollisionWorldBt::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); +} + +void CollisionWorldBt::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet robot world collision checking not yet implemented"); +} + +void CollisionWorldBt::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionWorld& other_world) const +{ + checkWorldCollisionHelper(req, res, other_world, nullptr); +} + +void CollisionWorldBt::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionWorld& other_world, const AllowedCollisionMatrix& acm) const +{ + checkWorldCollisionHelper(req, res, other_world, &acm); +} + +void CollisionWorldBt::checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const CollisionWorld& other_world, + const AllowedCollisionMatrix* acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet checking with other world not implemented yet."); +} + +void CollisionWorldBt::setWorld(const WorldPtr& world) +{ + if (world == getWorld()) + return; + + // turn off notifications about old world + getWorld()->removeObserver(observer_handle_); + + CollisionWorld::setWorld(world); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBt::notifyObjectChange, this, _1, _2)); + + // get notifications any objects already in the new world + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +void CollisionWorldBt::notifyObjectChange(const ObjectConstPtr& obj, World::Action action) +{ +} + +void CollisionWorldBt::distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, + const robot_state::RobotState& state) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet checking with other world not implemented yet."); +} + +void CollisionWorldBt::distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet checking with other world not implemented yet."); +} + +} // end of namespace collision_detection diff --git a/moveit_core/collision_detector_bt_description.xml b/moveit_core/collision_detector_bt_description.xml new file mode 100644 index 0000000000..4065ee048d --- /dev/null +++ b/moveit_core/collision_detector_bt_description.xml @@ -0,0 +1,8 @@ + + + + Bullet Collision Detector + + + diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 70a185040c..f0b2e0774f 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -56,5 +56,6 @@ + From 60d7fc9434e91f034330083069f0c055093e197b Mon Sep 17 00:00:00 2001 From: Jens P Date: Thu, 11 Jul 2019 15:07:44 +0200 Subject: [PATCH 148/612] Generic collision detection test suite (#1543) Generalize collision detection tests by using a templated test fixture. --- .../test_collision_common.h | 554 ++++++++++++++++++ .../test/test_fcl_collision_detection.cpp | 514 +--------------- 2 files changed, 562 insertions(+), 506 deletions(-) create mode 100644 moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h new file mode 100644 index 0000000000..c3ae2f9c9f --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h @@ -0,0 +1,554 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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. + *********************************************************************/ + +/* Author: E. Gil Jones */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +template +class CollisionDetectorTest : public ::testing::Test +{ +public: + std::shared_ptr value_; + +protected: + CollisionDetectorTest() + { + } + + void SetUp() override + { + value_.reset(new CollisionAllocatorType); + robot_model_ = moveit::core::loadTestingRobotModel("pr2"); + robot_model_ok_ = static_cast(robot_model_); + kinect_dae_resource_ = "package://moveit_resources/pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae"; + + acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); + + crobot_ = value_->allocateRobot(robot_model_); + cworld_ = value_->allocateWorld(collision_detection::WorldPtr(new collision_detection::World())); + } + + void TearDown() override + { + } + + bool robot_model_ok_; + + robot_model::RobotModelPtr robot_model_; + + collision_detection::CollisionRobotPtr crobot_; + collision_detection::CollisionWorldPtr cworld_; + + collision_detection::AllowedCollisionMatrixPtr acm_; + + std::string kinect_dae_resource_; +}; + +TYPED_TEST_CASE_P(CollisionDetectorTest); + +TYPED_TEST_P(CollisionDetectorTest, InitOK) +{ + ASSERT_TRUE(this->robot_model_ok_); +} + +TYPED_TEST_P(CollisionDetectorTest, DefaultNotInCollision) +{ + robot_state::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_FALSE(res.collision); +} + +TYPED_TEST_P(CollisionDetectorTest, LinksInCollision) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res1; + collision_detection::CollisionResult res2; + collision_detection::CollisionResult res3; + // req.contacts = true; + // req.max_contacts = 100; + + robot_state::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + Eigen::Isometry3d offset = Eigen::Isometry3d::Identity(); + offset.translation().x() = .01; + + // robot_state.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); + // robot_state.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset); + robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity()); + robot_state.updateStateWithLinkAt("base_bellow_link", offset); + robot_state.update(); + + this->acm_->setEntry("base_link", "base_bellow_link", false); + this->crobot_->checkSelfCollision(req, res1, robot_state, *this->acm_); + ASSERT_TRUE(res1.collision); + + this->acm_->setEntry("base_link", "base_bellow_link", true); + this->crobot_->checkSelfCollision(req, res2, robot_state, *this->acm_); + ASSERT_FALSE(res2.collision); + + // req.verbose = true; + // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); + // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset); + robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity()); + robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset); + robot_state.update(); + + this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); + this->crobot_->checkSelfCollision(req, res3, robot_state, *this->acm_); + ASSERT_TRUE(res3.collision); +} + +TYPED_TEST_P(CollisionDetectorTest, ContactReporting) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 1; + + robot_state::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + Eigen::Isometry3d offset = Eigen::Isometry3d::Identity(); + offset.translation().x() = .01; + + // robot_state.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); + // robot_state.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset); + // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); + // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset); + + robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity()); + robot_state.updateStateWithLinkAt("base_bellow_link", offset); + robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity()); + robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset); + robot_state.update(); + + this->acm_->setEntry("base_link", "base_bellow_link", false); + this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); + + collision_detection::CollisionResult res; + this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + EXPECT_EQ(res.contacts.size(), 1u); + EXPECT_EQ(res.contacts.begin()->second.size(), 1u); + + res.clear(); + req.max_contacts = 2; + req.max_contacts_per_pair = 1; + // req.verbose = true; + this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + EXPECT_EQ(res.contacts.size(), 2u); + EXPECT_EQ(res.contacts.begin()->second.size(), 1u); + + res.contacts.clear(); + res.contact_count = 0; + + req.max_contacts = 10; + req.max_contacts_per_pair = 2; + this->acm_.reset(new collision_detection::AllowedCollisionMatrix(this->robot_model_->getLinkModelNames(), false)); + this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + EXPECT_LE(res.contacts.size(), 10u); + EXPECT_LE(res.contact_count, 10u); +} + +TYPED_TEST_P(CollisionDetectorTest, ContactPositions) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 1; + + robot_state::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity(); + + pos1.translation().x() = 5.0; + pos2.translation().x() = 5.01; + + // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); + // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); + robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); + robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); + robot_state.update(); + + this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); + + collision_detection::CollisionResult res; + this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + ASSERT_EQ(res.contacts.size(), 1u); + ASSERT_EQ(res.contacts.begin()->second.size(), 1u); + + for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin(); + it != res.contacts.end(); it++) + { + EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33); + } + + pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0)); + // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); + // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); + robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); + robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); + robot_state.update(); + + collision_detection::CollisionResult res2; + this->crobot_->checkSelfCollision(req, res2, robot_state, *this->acm_); + ASSERT_TRUE(res2.collision); + ASSERT_EQ(res2.contacts.size(), 1u); + ASSERT_EQ(res2.contacts.begin()->second.size(), 1u); + + for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin(); + it != res2.contacts.end(); it++) + { + EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33); + } + + pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0)); + // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); + // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); + robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); + robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); + robot_state.update(); + + collision_detection::CollisionResult res3; + this->crobot_->checkSelfCollision(req, res2, robot_state, *this->acm_); + ASSERT_FALSE(res3.collision); +} + +TYPED_TEST_P(CollisionDetectorTest, AttachedBodyTester) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + this->acm_.reset(new collision_detection::AllowedCollisionMatrix(this->robot_model_->getLinkModelNames(), true)); + + robot_state::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + pos1.translation().x() = 5.0; + + // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); + robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); + robot_state.update(); + this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_FALSE(res.collision); + + shapes::Shape* shape = new shapes::Box(.1, .1, .1); + this->cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1); + + res = collision_detection::CollisionResult(); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + + // deletes shape + this->cworld_->getWorld()->removeObject("box"); + + shape = new shapes::Box(.1, .1, .1); + std::vector shapes; + EigenSTL::vector_Isometry3d poses; + shapes.push_back(shapes::ShapeConstPtr(shape)); + poses.push_back(Eigen::Isometry3d::Identity()); + std::vector touch_links; + robot_state.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link"); + + res = collision_detection::CollisionResult(); + this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + + // deletes shape + robot_state.clearAttachedBody("box"); + + touch_links.push_back("r_gripper_palm_link"); + touch_links.push_back("r_gripper_motor_accelerometer_link"); + shapes[0].reset(new shapes::Box(.1, .1, .1)); + robot_state.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link"); + robot_state.update(); + + res = collision_detection::CollisionResult(); + this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_FALSE(res.collision); + + pos1.translation().x() = 5.01; + shapes::Shape* coll = new shapes::Box(.1, .1, .1); + this->cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1); + res = collision_detection::CollisionResult(); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + + this->acm_->setEntry("coll", "r_gripper_palm_link", true); + res = collision_detection::CollisionResult(); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); +} + +TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) +{ + robot_state::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + collision_detection::CollisionRobotPtr new_crobot = this->value_->allocateRobot(this->crobot_); + + ros::WallTime before = ros::WallTime::now(); + new_crobot->checkSelfCollision(req, res, robot_state); + double first_check = (ros::WallTime::now() - before).toSec(); + before = ros::WallTime::now(); + new_crobot->checkSelfCollision(req, res, robot_state); + double second_check = (ros::WallTime::now() - before).toSec(); + + EXPECT_LT(fabs(first_check - second_check), .05); + + std::vector shapes; + shapes.resize(1); + + shapes[0].reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); + + EigenSTL::vector_Isometry3d poses; + poses.push_back(Eigen::Isometry3d::Identity()); + + std::vector touch_links; + robot_state.attachBody("kinect", shapes, poses, touch_links, "r_gripper_palm_link"); + + before = ros::WallTime::now(); + new_crobot->checkSelfCollision(req, res, robot_state); + first_check = (ros::WallTime::now() - before).toSec(); + before = ros::WallTime::now(); + new_crobot->checkSelfCollision(req, res, robot_state); + second_check = (ros::WallTime::now() - before).toSec(); + + // the first check is going to take a while, as data must be constructed + EXPECT_LT(second_check, .1); + + collision_detection::CollisionRobotPtr other_new_crobot = this->value_->allocateRobot(this->crobot_); + before = ros::WallTime::now(); + new_crobot->checkSelfCollision(req, res, robot_state); + first_check = (ros::WallTime::now() - before).toSec(); + before = ros::WallTime::now(); + new_crobot->checkSelfCollision(req, res, robot_state); + second_check = (ros::WallTime::now() - before).toSec(); + + EXPECT_LT(fabs(first_check - second_check), .05); +} + +TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + shapes::ShapeConstPtr shape(shapes::createMeshFromResource(this->kinect_dae_resource_)); + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity(); + pos2.translation().x() = 10.0; + + this->cworld_->getWorld()->addToObject("kinect", shape, pos1); + + robot_state::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + ros::WallTime before = ros::WallTime::now(); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, robot_state); + double first_check = (ros::WallTime::now() - before).toSec(); + before = ros::WallTime::now(); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, robot_state); + double second_check = (ros::WallTime::now() - before).toSec(); + + EXPECT_LT(second_check, .05); + + collision_detection::CollisionWorld::ObjectConstPtr object = this->cworld_->getWorld()->getObject("kinect"); + this->cworld_->getWorld()->removeObject("kinect"); + + robot_state::RobotState robot_state1(this->robot_model_); + robot_state::RobotState robot_state2(this->robot_model_); + robot_state1.setToDefaultValues(); + robot_state2.setToDefaultValues(); + robot_state1.update(); + robot_state2.update(); + + std::vector touch_links; + robot_state1.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link"); + + EigenSTL::vector_Isometry3d other_poses; + other_poses.push_back(pos2); + + // This creates a new set of constant properties for the attached body, which happens to be the same as the one above; + robot_state2.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link"); + + // going to take a while, but that's fine + res = collision_detection::CollisionResult(); + this->crobot_->checkSelfCollision(req, res, robot_state1); + + EXPECT_TRUE(res.collision); + + before = ros::WallTime::now(); + this->crobot_->checkSelfCollision(req, res, robot_state1, *this->acm_); + first_check = (ros::WallTime::now() - before).toSec(); + before = ros::WallTime::now(); + req.verbose = true; + res = collision_detection::CollisionResult(); + this->crobot_->checkSelfCollision(req, res, robot_state2, *this->acm_); + second_check = (ros::WallTime::now() - before).toSec(); + + EXPECT_LT(first_check, .05); + EXPECT_LT(fabs(first_check - second_check), .1); +} + +TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed) +{ + EigenSTL::vector_Isometry3d poses; + std::vector shapes; + for (unsigned int i = 0; i < 10000; i++) + { + poses.push_back(Eigen::Isometry3d::Identity()); + shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01))); + } + ros::WallTime start = ros::WallTime::now(); + this->cworld_->getWorld()->addToObject("map", shapes, poses); + double t = (ros::WallTime::now() - start).toSec(); + EXPECT_GE(1.0, t); + // this is not really a failure; it is just that slow; + // looking into doing collision checking with a voxel grid. + ROS_INFO_NAMED("collision_detection.bullet", "Adding boxes took %g", t); +} + +TYPED_TEST_P(CollisionDetectorTest, MoveMesh) +{ + robot_state::RobotState robot_state1(this->robot_model_); + robot_state1.setToDefaultValues(); + robot_state1.update(); + + Eigen::Isometry3d kinect_pose; + kinect_pose.setIdentity(); + shapes::ShapePtr kinect_shape; + kinect_shape.reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); + + this->cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); + + Eigen::Isometry3d np; + for (unsigned int i = 0; i < 5; i++) + { + np = Eigen::Translation3d(i * .001, i * .001, i * .001) * Eigen::Quaterniond::Identity(); + this->cworld_->getWorld()->moveShapeInObject("kinect", kinect_shape, np); + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->cworld_->checkCollision(req, res, *this->crobot_, robot_state1, *this->acm_); + } +} + +TYPED_TEST_P(CollisionDetectorTest, TestChangingShapeSize) +{ + robot_state::RobotState robot_state1(this->robot_model_); + robot_state1.setToDefaultValues(); + robot_state1.update(); + + collision_detection::CollisionRequest req1; + collision_detection::CollisionResult res1; + + ASSERT_FALSE(res1.collision); + + EigenSTL::vector_Isometry3d poses; + std::vector shapes; + for (unsigned int i = 0; i < 5; i++) + { + this->cworld_->getWorld()->removeObject("shape"); + shapes.clear(); + poses.clear(); + shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001))); + poses.push_back(Eigen::Isometry3d::Identity()); + this->cworld_->getWorld()->addToObject("shape", shapes, poses); + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->cworld_->checkCollision(req, res, *this->crobot_, robot_state1, *this->acm_); + ASSERT_TRUE(res.collision); + } + + Eigen::Isometry3d kinect_pose = Eigen::Isometry3d::Identity(); + shapes::ShapePtr kinect_shape; + kinect_shape.reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); + this->cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); + collision_detection::CollisionRequest req2; + collision_detection::CollisionResult res2; + this->cworld_->checkCollision(req2, res2, *this->crobot_, robot_state1, *this->acm_); + ASSERT_TRUE(res2.collision); + for (unsigned int i = 0; i < 5; i++) + { + this->cworld_->getWorld()->removeObject("shape"); + shapes.clear(); + poses.clear(); + shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001))); + poses.push_back(Eigen::Isometry3d::Identity()); + this->cworld_->getWorld()->addToObject("shape", shapes, poses); + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->cworld_->checkCollision(req, res, *this->crobot_, robot_state1, *this->acm_); + ASSERT_TRUE(res.collision); + } +} + +REGISTER_TYPED_TEST_CASE_P(CollisionDetectorTest, InitOK, DefaultNotInCollision, LinksInCollision, ContactReporting, + ContactPositions, AttachedBodyTester, DiffSceneTester, ConvertObjectToAttached, + TestCollisionMapAdditionSpeed, MoveMesh, TestChangingShapeSize); diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp index e4ea360a95..5dffe19a8b 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2019, Jens Petit * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +14,7 @@ * 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 + * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -32,513 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: E. Gil Jones */ +/* Author: Jens Petit */ -#include -#include -#include -#include +#include +#include -#include -#include +INSTANTIATE_TYPED_TEST_CASE_P(FCLCollisionCheck, CollisionDetectorTest, + collision_detection::CollisionDetectorAllocatorFCL); -#include -#include -#include -#include -#include - -typedef collision_detection::CollisionEnvFCL DefaultEnvType; - -class FclCollisionDetectionTester : public testing::Test -{ -protected: - void SetUp() override - { - robot_model_ = moveit::core::loadTestingRobotModel("pr2"); - robot_model_ok_ = static_cast(robot_model_); - kinect_dae_resource_ = "package://moveit_resources/pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae"; - - acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); - - c_env_.reset(new DefaultEnvType(robot_model_)); - } - - void TearDown() override - { - } - -protected: - bool robot_model_ok_; - - robot_model::RobotModelPtr robot_model_; - - collision_detection::CollisionEnvPtr c_env_; - - collision_detection::AllowedCollisionMatrixPtr acm_; - - std::string kinect_dae_resource_; -}; - -TEST_F(FclCollisionDetectionTester, InitOK) -{ - ASSERT_TRUE(robot_model_ok_); -} - -TEST_F(FclCollisionDetectionTester, DefaultNotInCollision) -{ - robot_state::RobotState robot_state(robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - c_env_->checkSelfCollision(req, res, robot_state, *acm_); - ASSERT_FALSE(res.collision); -} - -TEST_F(FclCollisionDetectionTester, LinksInCollision) -{ - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res1; - collision_detection::CollisionResult res2; - collision_detection::CollisionResult res3; - // req.contacts = true; - // req.max_contacts = 100; - - robot_state::RobotState robot_state(robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - Eigen::Isometry3d offset = Eigen::Isometry3d::Identity(); - offset.translation().x() = .01; - - // robot_state.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); - // robot_state.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset); - robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity()); - robot_state.updateStateWithLinkAt("base_bellow_link", offset); - robot_state.update(); - - acm_->setEntry("base_link", "base_bellow_link", false); - c_env_->checkSelfCollision(req, res1, robot_state, *acm_); - ASSERT_TRUE(res1.collision); - - acm_->setEntry("base_link", "base_bellow_link", true); - c_env_->checkSelfCollision(req, res2, robot_state, *acm_); - ASSERT_FALSE(res2.collision); - - // req.verbose = true; - // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); - // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset); - robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity()); - robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset); - robot_state.update(); - - acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); - c_env_->checkSelfCollision(req, res3, robot_state, *acm_); - ASSERT_TRUE(res3.collision); -} - -TEST_F(FclCollisionDetectionTester, ContactReporting) -{ - collision_detection::CollisionRequest req; - req.contacts = true; - req.max_contacts = 1; - - robot_state::RobotState robot_state(robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - Eigen::Isometry3d offset = Eigen::Isometry3d::Identity(); - offset.translation().x() = .01; - - // robot_state.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); - // robot_state.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset); - // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); - // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset); - - robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity()); - robot_state.updateStateWithLinkAt("base_bellow_link", offset); - robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity()); - robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset); - robot_state.update(); - - acm_->setEntry("base_link", "base_bellow_link", false); - acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); - - collision_detection::CollisionResult res; - c_env_->checkSelfCollision(req, res, robot_state, *acm_); - ASSERT_TRUE(res.collision); - EXPECT_EQ(res.contacts.size(), 1u); - EXPECT_EQ(res.contacts.begin()->second.size(), 1u); - - res.clear(); - req.max_contacts = 2; - req.max_contacts_per_pair = 1; - // req.verbose = true; - c_env_->checkSelfCollision(req, res, robot_state, *acm_); - ASSERT_TRUE(res.collision); - EXPECT_EQ(res.contacts.size(), 2u); - EXPECT_EQ(res.contacts.begin()->second.size(), 1u); - - res.contacts.clear(); - res.contact_count = 0; - - req.max_contacts = 10; - req.max_contacts_per_pair = 2; - acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false)); - c_env_->checkSelfCollision(req, res, robot_state, *acm_); - ASSERT_TRUE(res.collision); - EXPECT_LE(res.contacts.size(), 10u); - EXPECT_LE(res.contact_count, 10u); -} - -TEST_F(FclCollisionDetectionTester, ContactPositions) -{ - collision_detection::CollisionRequest req; - req.contacts = true; - req.max_contacts = 1; - - robot_state::RobotState robot_state(robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); - Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity(); - - pos1.translation().x() = 5.0; - pos2.translation().x() = 5.01; - - // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); - // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); - robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); - robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); - robot_state.update(); - - acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); - - collision_detection::CollisionResult res; - c_env_->checkSelfCollision(req, res, robot_state, *acm_); - ASSERT_TRUE(res.collision); - ASSERT_EQ(res.contacts.size(), 1u); - ASSERT_EQ(res.contacts.begin()->second.size(), 1u); - - for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin(); - it != res.contacts.end(); it++) - { - EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33); - } - - pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); - pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0)); - // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); - // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); - robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); - robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); - robot_state.update(); - - collision_detection::CollisionResult res2; - c_env_->checkSelfCollision(req, res2, robot_state, *acm_); - ASSERT_TRUE(res2.collision); - ASSERT_EQ(res2.contacts.size(), 1u); - ASSERT_EQ(res2.contacts.begin()->second.size(), 1u); - - for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin(); - it != res2.contacts.end(); it++) - { - EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33); - } - - pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); - pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0)); - // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); - // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); - robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); - robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); - robot_state.update(); - - collision_detection::CollisionResult res3; - c_env_->checkSelfCollision(req, res2, robot_state, *acm_); - ASSERT_FALSE(res3.collision); -} - -TEST_F(FclCollisionDetectionTester, AttachedBodyTester) -{ - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - - acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); - - robot_state::RobotState robot_state(robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); - pos1.translation().x() = 5.0; - - // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); - robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); - robot_state.update(); - c_env_->checkSelfCollision(req, res, robot_state, *acm_); - ASSERT_FALSE(res.collision); - - shapes::Shape* shape = new shapes::Box(.1, .1, .1); - c_env_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1); - - res = collision_detection::CollisionResult(); - c_env_->checkRobotCollision(req, res, robot_state, *acm_); - ASSERT_TRUE(res.collision); - - // deletes shape - c_env_->getWorld()->removeObject("box"); - - shape = new shapes::Box(.1, .1, .1); - std::vector shapes; - EigenSTL::vector_Isometry3d poses; - shapes.push_back(shapes::ShapeConstPtr(shape)); - poses.push_back(Eigen::Isometry3d::Identity()); - std::vector touch_links; - robot_state.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link"); - - res = collision_detection::CollisionResult(); - c_env_->checkSelfCollision(req, res, robot_state, *acm_); - ASSERT_TRUE(res.collision); - - // deletes shape - robot_state.clearAttachedBody("box"); - - touch_links.push_back("r_gripper_palm_link"); - touch_links.push_back("r_gripper_motor_accelerometer_link"); - shapes[0].reset(new shapes::Box(.1, .1, .1)); - robot_state.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link"); - robot_state.update(); - - res = collision_detection::CollisionResult(); - c_env_->checkSelfCollision(req, res, robot_state, *acm_); - ASSERT_FALSE(res.collision); - - pos1.translation().x() = 5.01; - shapes::Shape* coll = new shapes::Box(.1, .1, .1); - c_env_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1); - res = collision_detection::CollisionResult(); - c_env_->checkRobotCollision(req, res, robot_state, *acm_); - ASSERT_TRUE(res.collision); - - acm_->setEntry("coll", "r_gripper_palm_link", true); - res = collision_detection::CollisionResult(); - c_env_->checkRobotCollision(req, res, robot_state, *acm_); - ASSERT_TRUE(res.collision); -} - -TEST_F(FclCollisionDetectionTester, DiffSceneTester) -{ - robot_state::RobotState robot_state(robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - collision_detection::CollisionEnvFCL* casted_env_pointer = - dynamic_cast(c_env_.get()); - collision_detection::CollisionEnvFCL new_c_env(*casted_env_pointer, c_env_->getWorld()); - - ros::WallTime before = ros::WallTime::now(); - new_c_env.checkSelfCollision(req, res, robot_state); - double first_check = (ros::WallTime::now() - before).toSec(); - before = ros::WallTime::now(); - new_c_env.checkSelfCollision(req, res, robot_state); - double second_check = (ros::WallTime::now() - before).toSec(); - - EXPECT_LT(fabs(first_check - second_check), .05); - - std::vector shapes; - shapes.resize(1); - - shapes[0].reset(shapes::createMeshFromResource(kinect_dae_resource_)); - - EigenSTL::vector_Isometry3d poses; - poses.push_back(Eigen::Isometry3d::Identity()); - - std::vector touch_links; - robot_state.attachBody("kinect", shapes, poses, touch_links, "r_gripper_palm_link"); - - before = ros::WallTime::now(); - new_c_env.checkSelfCollision(req, res, robot_state); - first_check = (ros::WallTime::now() - before).toSec(); - before = ros::WallTime::now(); - new_c_env.checkSelfCollision(req, res, robot_state); - second_check = (ros::WallTime::now() - before).toSec(); - - // the first check is going to take a while, as data must be constructed - EXPECT_LT(second_check, .1); - - collision_detection::CollisionEnvFCL other_new_c_env(*casted_env_pointer, c_env_->getWorld()); - before = ros::WallTime::now(); - new_c_env.checkSelfCollision(req, res, robot_state); - first_check = (ros::WallTime::now() - before).toSec(); - before = ros::WallTime::now(); - new_c_env.checkSelfCollision(req, res, robot_state); - second_check = (ros::WallTime::now() - before).toSec(); - - EXPECT_LT(fabs(first_check - second_check), .05); -} - -TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached) -{ - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - - shapes::ShapeConstPtr shape(shapes::createMeshFromResource(kinect_dae_resource_)); - Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); - Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity(); - pos2.translation().x() = 10.0; - - c_env_->getWorld()->addToObject("kinect", shape, pos1); - - robot_state::RobotState robot_state(robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - ros::WallTime before = ros::WallTime::now(); - c_env_->checkRobotCollision(req, res, robot_state); - double first_check = (ros::WallTime::now() - before).toSec(); - before = ros::WallTime::now(); - c_env_->checkRobotCollision(req, res, robot_state); - double second_check = (ros::WallTime::now() - before).toSec(); - - EXPECT_LT(second_check, .05); - - collision_detection::CollisionEnv::ObjectConstPtr object = c_env_->getWorld()->getObject("kinect"); - c_env_->getWorld()->removeObject("kinect"); - - robot_state::RobotState robot_state1(robot_model_); - robot_state::RobotState robot_state2(robot_model_); - robot_state1.setToDefaultValues(); - robot_state2.setToDefaultValues(); - robot_state1.update(); - robot_state2.update(); - - std::vector touch_links; - robot_state1.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link"); - - EigenSTL::vector_Isometry3d other_poses; - other_poses.push_back(pos2); - - // This creates a new set of constant properties for the attached body, which happens to be the same as the one above; - robot_state2.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link"); - - // going to take a while, but that's fine - res = collision_detection::CollisionResult(); - c_env_->checkSelfCollision(req, res, robot_state1); - - EXPECT_TRUE(res.collision); - - before = ros::WallTime::now(); - c_env_->checkSelfCollision(req, res, robot_state1, *acm_); - first_check = (ros::WallTime::now() - before).toSec(); - before = ros::WallTime::now(); - req.verbose = true; - res = collision_detection::CollisionResult(); - c_env_->checkSelfCollision(req, res, robot_state2, *acm_); - second_check = (ros::WallTime::now() - before).toSec(); - - EXPECT_LT(first_check, .05); - EXPECT_LT(fabs(first_check - second_check), .1); -} - -TEST_F(FclCollisionDetectionTester, TestCollisionMapAdditionSpeed) -{ - EigenSTL::vector_Isometry3d poses; - std::vector shapes; - for (unsigned int i = 0; i < 10000; i++) - { - poses.push_back(Eigen::Isometry3d::Identity()); - shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01))); - } - ros::WallTime start = ros::WallTime::now(); - c_env_->getWorld()->addToObject("map", shapes, poses); - double t = (ros::WallTime::now() - start).toSec(); - EXPECT_GE(1.0, t); - // this is not really a failure; it is just that slow; - // looking into doing collision checking with a voxel grid. - ROS_INFO_NAMED("collision_detection.fcl", "Adding boxes took %g", t); -} - -TEST_F(FclCollisionDetectionTester, MoveMesh) -{ - robot_state::RobotState robot_state1(robot_model_); - robot_state1.setToDefaultValues(); - robot_state1.update(); - - Eigen::Isometry3d kinect_pose; - kinect_pose.setIdentity(); - shapes::ShapePtr kinect_shape; - kinect_shape.reset(shapes::createMeshFromResource(kinect_dae_resource_)); - - c_env_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); - - Eigen::Isometry3d np; - for (unsigned int i = 0; i < 5; i++) - { - np = Eigen::Translation3d(i * .001, i * .001, i * .001) * Eigen::Quaterniond::Identity(); - c_env_->getWorld()->moveShapeInObject("kinect", kinect_shape, np); - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - c_env_->checkCollision(req, res, robot_state1, *acm_); - } -} - -TEST_F(FclCollisionDetectionTester, TestChangingShapeSize) -{ - robot_state::RobotState robot_state1(robot_model_); - robot_state1.setToDefaultValues(); - robot_state1.update(); - - collision_detection::CollisionRequest req1; - collision_detection::CollisionResult res1; - - ASSERT_FALSE(res1.collision); - - EigenSTL::vector_Isometry3d poses; - std::vector shapes; - for (unsigned int i = 0; i < 5; i++) - { - c_env_->getWorld()->removeObject("shape"); - shapes.clear(); - poses.clear(); - shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001))); - poses.push_back(Eigen::Isometry3d::Identity()); - c_env_->getWorld()->addToObject("shape", shapes, poses); - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - c_env_->checkCollision(req, res, robot_state1, *acm_); - ASSERT_TRUE(res.collision); - } - - Eigen::Isometry3d kinect_pose = Eigen::Isometry3d::Identity(); - shapes::ShapePtr kinect_shape; - kinect_shape.reset(shapes::createMeshFromResource(kinect_dae_resource_)); - c_env_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); - collision_detection::CollisionRequest req2; - collision_detection::CollisionResult res2; - c_env_->checkCollision(req2, res2, robot_state1, *acm_); - ASSERT_TRUE(res2.collision); - for (unsigned int i = 0; i < 5; i++) - { - c_env_->getWorld()->removeObject("shape"); - shapes.clear(); - poses.clear(); - shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001))); - poses.push_back(Eigen::Isometry3d::Identity()); - c_env_->getWorld()->addToObject("shape", shapes, poses); - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - c_env_->checkCollision(req, res, robot_state1, *acm_); - ASSERT_TRUE(res.collision); - } -} - -int main(int argc, char** argv) +int main(int argc, char* argv[]) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); From ef174182d10dc64797dd844ef1530b74963815dc Mon Sep 17 00:00:00 2001 From: Jens P Date: Tue, 23 Jul 2019 22:46:48 +0200 Subject: [PATCH 149/612] Bullet Collision Detection (#1504) * Added discrete BVH Bullet manager * Added continuous collision detection * Cleanup of tesseract code * removed simple collision managers * changed enum to enum classes * fixed typos * removing debugging statements * removing tesseract allowed collision matrix * removed tesseract macros * replaced typedefs of stl containers * removed tesseract attached object code * ACM members of collision robot and world removed and ACM check into callback out of class * remove ContactTestType and replace through MoveIt CollisionRequest * BodyType int changed to enum * removed tesseract_msgs dependency and unnecessary tesseract code * changed dependency from bullet3_ros to debian Bullet package * Adding bullet and tesseract simple collision manager to the template: * added benchmark case for checking collision speed * tests for bullet collision checking * Adding missing features: * attached objects * contact reporting max number of contacts fixed * Bullet plugin xml name fixed * padding and scaling for robot added * updated tests --- .travis.yml | 1 - moveit_core/CMakeLists.txt | 6 +- .../collision_detection/collision_common.h | 9 + .../test_collision_common.h | 3 +- .../test_collision_common_panda.h | 258 ++++++ .../collision_detection_bullet/CMakeLists.txt | 28 +- .../collision_detection_bullet/README.md | 2 +- .../bullet_integration/basic_types.h | 106 +++ .../bullet_discrete_bvh_manager.h | 191 +++++ .../bullet_integration/bullet_utils.h | 803 ++++++++++++++++++ .../contact_checker_common.h | 271 ++++++ .../bullet_integration/ros_bullet_utils.h | 109 +++ ... => collision_detector_allocator_bullet.h} | 19 +- ...collision_detector_bullet_plugin_loader.h} | 10 +- ...on_robot_bt.h => collision_robot_bullet.h} | 43 +- ...on_world_bt.h => collision_world_bullet.h} | 50 +- .../bullet_discrete_bvh_manager.cpp | 306 +++++++ .../src/bullet_integration/bullet_utils.cpp | 343 ++++++++ ...llision_detector_bullet_plugin_loader.cpp} | 4 +- .../src/collision_robot_bt.cpp | 143 ---- .../src/collision_robot_bullet.cpp | 267 ++++++ .../src/collision_world_bt.cpp | 156 ---- .../src/collision_world_bullet.cpp | 247 ++++++ .../test_bullet_collision_detection.cpp} | 15 +- .../test_bullet_collision_detection_panda.cpp | 47 + .../collision_detection_fcl/CMakeLists.txt | 4 +- .../test_fcl_collision_detection_panda.cpp} | 14 +- ...collision_detector_bullet_description.xml} | 0 moveit_core/package.xml | 3 +- .../launch/collision_checker_compare.launch | 10 + 30 files changed, 3103 insertions(+), 365 deletions(-) create mode 100644 moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h rename moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/{collision_detector_allocator_bt.h => collision_detector_allocator_bullet.h} (76%) rename moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/{collision_detector_bt_plugin_loader.h => collision_detector_bullet_plugin_loader.h} (84%) rename moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/{collision_robot_bt.h => collision_robot_bullet.h} (69%) rename moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/{collision_world_bt.h => collision_world_bullet.h} (66%) create mode 100644 moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp create mode 100644 moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp rename moveit_core/collision_detection_bullet/src/{collision_detector_bt_plugin_loader.cpp => collision_detector_bullet_plugin_loader.cpp} (95%) delete mode 100644 moveit_core/collision_detection_bullet/src/collision_robot_bt.cpp create mode 100644 moveit_core/collision_detection_bullet/src/collision_robot_bullet.cpp delete mode 100644 moveit_core/collision_detection_bullet/src/collision_world_bt.cpp create mode 100644 moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp rename moveit_core/collision_detection_bullet/{include/moveit/collision_detection_bullet/collision_common.h => test/test_bullet_collision_detection.cpp} (81%) create mode 100644 moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp rename moveit_core/{collision_detection_bullet/src/collision_common.cpp => collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp} (81%) rename moveit_core/{collision_detector_bt_description.xml => collision_detector_bullet_description.xml} (100%) create mode 100644 moveit_ros/planning/launch/collision_checker_compare.launch diff --git a/.travis.yml b/.travis.yml index 6ed584a5ff..c51f7602da 100644 --- a/.travis.yml +++ b/.travis.yml @@ -25,7 +25,6 @@ env: - TEST="clang-format catkin_lint" - TEST=code-coverage - ROS_DISTRO=melodic - - ROS_DISTRO=kinetic matrix: # Add a separate config to the matrix, using clang as compiler include: diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 14207baef4..57f19a86be 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -14,8 +14,6 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") endif() -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - 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) @@ -23,6 +21,8 @@ endif() find_package(Boost REQUIRED system filesystem date_time thread iostreams) find_package(Eigen3 REQUIRED) + # TODO: Move collision detection into separate packages +find_package(Bullet REQUIRED) find_package(PkgConfig REQUIRED) pkg_check_modules(LIBFCL_PC REQUIRED fcl) @@ -145,10 +145,12 @@ catkin_package( OCTOMAP urdfdom urdfdom_headers + BULLET ) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} ${LIBFCL_INCLUDE_DIRS} + ${BULLET_INCLUDE_DIRS} ) #catkin_lint: ignore_once external_directory (${VERSION_FILE_PATH}) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 2991951395..96f3343d0b 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -94,6 +94,15 @@ struct Contact /** \brief The type of the second body involved in the contact */ BodyType body_type_2; + + /** \brief The distance percentage between casted poses until collision. + * + * If the value is 0, then the collision occured in the start pose. If the value is 1, then the collision occured in + * the end pose. */ + double percent_interpolation; + + /** \brief The two nearest points connecting the two bodies */ + Eigen::Vector3d nearest_points[2]; }; /** \brief When collision costs are computed, this structure contains information about the partial cost incurred in a diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h index c3ae2f9c9f..331c5a154e 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h @@ -469,7 +469,8 @@ TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed) ros::WallTime start = ros::WallTime::now(); this->cworld_->getWorld()->addToObject("map", shapes, poses); double t = (ros::WallTime::now() - start).toSec(); - EXPECT_GE(1.0, t); + // TODO: investigate why bullet collision checking is considerably slower here + EXPECT_GE(5.0, t); // this is not really a failure; it is just that slow; // looking into doing collision checking with a voxel grid. ROS_INFO_NAMED("collision_detection.bullet", "Adding boxes took %g", t); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h new file mode 100644 index 0000000000..a57733f3dd --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -0,0 +1,258 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +/** \brief Brings the panda robot in user defined home position */ +inline void setToHome(robot_state::RobotState& panda_state) +{ + panda_state.setToDefaultValues(); + double joint2 = -0.785; + double joint4 = -2.356; + double joint6 = 1.571; + double joint7 = 0.785; + panda_state.setJointPositions("panda_joint2", &joint2); + panda_state.setJointPositions("panda_joint4", &joint4); + panda_state.setJointPositions("panda_joint6", &joint6); + panda_state.setJointPositions("panda_joint7", &joint7); + panda_state.update(); +} + +template +class CollisionDetectorPandaTest : public testing::Test +{ +public: + std::shared_ptr value_; + +protected: + void SetUp() override + { + value_.reset(new CollisionAllocatorType); + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + robot_model_ok_ = static_cast(robot_model_); + + acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false)); + + acm_->setEntry("panda_link0", "panda_link1", true); + acm_->setEntry("panda_link1", "panda_link2", true); + acm_->setEntry("panda_link2", "panda_link3", true); + acm_->setEntry("panda_link3", "panda_link4", true); + acm_->setEntry("panda_link4", "panda_link5", true); + acm_->setEntry("panda_link5", "panda_link6", true); + acm_->setEntry("panda_link6", "panda_link7", true); + acm_->setEntry("panda_link7", "panda_hand", true); + acm_->setEntry("panda_hand", "panda_rightfinger", true); + acm_->setEntry("panda_hand", "panda_leftfinger", true); + acm_->setEntry("panda_rightfinger", "panda_leftfinger", true); + acm_->setEntry("panda_link5", "panda_link7", true); + acm_->setEntry("panda_link6", "panda_hand", true); + + crobot_ = value_->allocateRobot(robot_model_); + cworld_ = value_->allocateWorld(collision_detection::WorldPtr(new collision_detection::World())); + + robot_state_.reset(new robot_state::RobotState(robot_model_)); + setToHome(*robot_state_); + } + + void TearDown() override + { + } + + bool robot_model_ok_; + + robot_model::RobotModelPtr robot_model_; + + collision_detection::CollisionWorldPtr cworld_; + collision_detection::CollisionRobotPtr crobot_; + + collision_detection::AllowedCollisionMatrixPtr acm_; + + robot_state::RobotStatePtr robot_state_; +}; + +TYPED_TEST_CASE_P(CollisionDetectorPandaTest); + +/** \brief Correct setup testing. */ +TYPED_TEST_P(CollisionDetectorPandaTest, InitOK) +{ + ASSERT_TRUE(this->robot_model_ok_); +} + +/** \brief Tests the default values specified in the SRDF if they are collision free. */ +TYPED_TEST_P(CollisionDetectorPandaTest, DefaultNotInCollision) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); +} + +/** \brief A configuration where the robot should collide with itself. */ +TYPED_TEST_P(CollisionDetectorPandaTest, LinksInCollision) +{ + // Sets the joint values to zero which is a colliding configuration + this->robot_state_->setToDefaultValues(); + this->robot_state_->update(); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); +} + +// TODO: Add collision check capability within world itself and then enable test +/** \brief Two boxes in collision in the world environment. */ +TYPED_TEST_P(CollisionDetectorPandaTest, DISABLED_WorldToWorldCollision) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + shapes::Shape* shape = new shapes::Box(.5, .5, .5); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos_1 = Eigen::Isometry3d::Identity(); + pos_1.translation().x() = 1; + this->cworld_->getWorld()->addToObject("box", shape_ptr, pos_1); + + Eigen::Isometry3d pos_2 = Eigen::Isometry3d::Identity(); + pos_2.translation().x() = 1.2; + this->cworld_->getWorld()->addToObject("box_2", shape_ptr, pos_2); + + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + + ASSERT_TRUE(res.collision); +} + +/** \brief Adding obstacles to the world which are tested against the robot. Simple cases. */ +TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_1) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + shapes::Shape* shape = new shapes::Box(.1, .1, .1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + pos1.translation().z() = 0.3; + this->cworld_->getWorld()->addToObject("box", shape_ptr, pos1); + + this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + this->cworld_->getWorld()->moveObject("box", pos1); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + this->cworld_->getWorld()->moveObject("box", pos1); + ASSERT_FALSE(res.collision); +} + +/** \brief Adding obstacles to the world which are tested against the robot. */ +TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_2) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + req.max_contacts = 10; + req.contacts = true; + req.verbose = true; + + shapes::Shape* shape = new shapes::Box(.4, .4, .4); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + pos1.translation().z() = 0.3; + this->cworld_->getWorld()->addToObject("box", shape_ptr, pos1); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + ASSERT_GE(res.contact_count, 3u); + res.clear(); +} + +/** \brief Tests the padding through expanding the link geometry in such a way that a collision occurs. */ +TYPED_TEST_P(CollisionDetectorPandaTest, PaddingTest) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 10; + collision_detection::CollisionResult res; + + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + // Adding the box right in front of the robot hand + shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + pos.translation().x() = 0.43; + pos.translation().y() = 0; + pos.translation().z() = 0.55; + this->cworld_->getWorld()->addToObject("box", shape_ptr, pos); + + this->crobot_->setLinkPadding("panda_hand", 0.08); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + this->crobot_->setLinkPadding("panda_hand", 0.0); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); +} + +REGISTER_TYPED_TEST_CASE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision, + DISABLED_WorldToWorldCollision, RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest); diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index 3e87ca6c78..6e1ba5526f 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -1,29 +1,39 @@ set(MOVEIT_LIB_NAME moveit_collision_detection_bullet) add_library(${MOVEIT_LIB_NAME} - src/collision_common.cpp - src/collision_robot_bt.cpp - src/collision_world_bt.cpp + src/collision_robot_bullet.cpp + src/collision_world_bullet.cpp + src/bullet_integration/bullet_utils.cpp + src/bullet_integration/bullet_discrete_bvh_manager.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME} moveit_collision_detection ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${LIBFCL_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME} moveit_collision_detection + ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} + ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES} + ${BULLET_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) -add_library(collision_detector_bt_plugin src/collision_detector_bt_plugin_loader.cpp) -set_target_properties(collision_detector_bt_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(collision_detector_bt_plugin ${catkin_LIBRARIES} ${MOVEIT_LIB_NAME}) +add_library(collision_detector_bullet_plugin src/collision_detector_bullet_plugin_loader.cpp) +set_target_properties(collision_detector_bullet_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(collision_detector_bullet_plugin ${catkin_LIBRARIES} ${MOVEIT_LIB_NAME}) -install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_bt_plugin +install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_bullet_plugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) -install(FILES ../collision_detector_bt_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(FILES ../collision_detector_bullet_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if(CATKIN_ENABLE_TESTING) find_package(moveit_resources REQUIRED) include_directories(${moveit_resources_INCLUDE_DIRS}) + + catkin_add_gtest(test_bullet_collision_detection test/test_bullet_collision_detection.cpp) + target_link_libraries(test_bullet_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + + catkin_add_gtest(test_bullet_collision_detection_panda test/test_bullet_collision_detection_panda.cpp) + target_link_libraries(test_bullet_collision_detection_panda moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) endif() diff --git a/moveit_core/collision_detection_bullet/README.md b/moveit_core/collision_detection_bullet/README.md index c0b85486c0..b808244650 100644 --- a/moveit_core/collision_detection_bullet/README.md +++ b/moveit_core/collision_detection_bullet/README.md @@ -2,5 +2,5 @@ To use Bullet as a collision checker you can: a) create a new planning scene and manually switch to Bullet using the planning_scene.setActiveCollisionDetector() b) manually switch to Bullet using the `CollisionPluginLoader` -c) manually instantiate a `CollisionRobotBt` and `CollisionWorldBt` +c) manually instantiate a `CollisionRobotBullet` and `CollisionWorldBullet` d) use rosparam to set the collision checker (preferred but not tested yet) when starting a new `move_group` diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h new file mode 100644 index 0000000000..385ed46f23 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h @@ -0,0 +1,106 @@ +/********************************************************************* + * Software License Agreement (Apache License) + * + * Copyright (c) 2013, Southwest Research Institute + * All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BASIC_TYPES_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BASIC_TYPES_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace collision_detection_bullet +{ +template +using AlignedVector = std::vector>; + +template +using AlignedMap = std::map, Eigen::aligned_allocator>>; + +template +using AlignedUnorderedMap = std::unordered_map, std::equal_to, + Eigen::aligned_allocator>>; + +/** @brief Checks if contact is allowed or not. Returns true if yes, otherwise false. + * + * The order of strings should not matter, the function should handled by the function. */ +typedef std::function + IsContactAllowedFn; + +enum class CollisionObjectType +{ + USE_SHAPE_TYPE = 0, /**< @brief Infer the type from the type specified in the shapes::Shape class */ + + // all of the following convert the meshes to custom collision objects + CONVEX_HULL = 1, /**< @brief Use the mesh in shapes::Shape but make it a convex hulls collision object (if not convex + it will be converted) */ + MULTI_SPHERE = 2, /**< @brief Use the mesh and represent it by multiple spheres collision object */ + SDF = 3 /**< @brief Use the mesh and rpresent it by a signed distance fields collision object */ +}; + +/** \brief Bundles the data for a collision query */ +struct ContactTestData +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ContactTestData(const std::vector& active, const double& contact_distance, const IsContactAllowedFn& fn, + collision_detection::CollisionResult& res, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm) + : active(active) + , contact_distance(contact_distance) + , fn(fn) + , acm(acm) + , res(res) + , req(req) + , done(false) + , pair_done(false) + { + } + + const std::vector& active; + + /** \brief If after a positive broadphase check the distance is below this threshold, a contact is added. */ + const double& contact_distance; + + /** \brief User defined function which checks if contact is allowed between two objects */ + const IsContactAllowedFn& fn; + + /** \brief Indicates collision objects which are allowed to be in contact */ + const collision_detection::AllowedCollisionMatrix* acm; + + collision_detection::CollisionResult& res; + const collision_detection::CollisionRequest& req; + + /// Indicates if search is finished + bool done; + + /// Indicates if search between a single pair is finished + bool pair_done; +}; + +} // namespace collision_detection_bullet + +#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BASIC_TYPES_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h new file mode 100644 index 0000000000..8067a160de --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h @@ -0,0 +1,191 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * 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. + * + * 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. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_COLLISION_BULLET_DISCRETE_BVH_MANAGER_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_COLLISION_BULLET_DISCRETE_BVH_MANAGER_H_ + +#include +#include + +namespace collision_detection_bullet +{ +MOVEIT_CLASS_FORWARD(BulletDiscreteBVHManager) + +/** @brief A bounding volume hierarchy (BVH) implementaiton of a discrete bullet manager */ +class BulletDiscreteBVHManager +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Constructor */ + BulletDiscreteBVHManager(); + + ~BulletDiscreteBVHManager(); + + /**@brief Clone the manager + * + * This is to be used for multi threaded applications. A user should make a clone for each thread. */ + BulletDiscreteBVHManagerPtr clone() const; + + /**@brief Add a object to the checker + * @param name The name of the object, must be unique. + * @param mask_id User defined id which gets stored in the results structure. + * @param shapes A vector of shapes that make up the collision object. + * @param shape_poses A vector of poses for each shape, must be same length as shapes + * @param shape_types A vector of shape types for encode the collision object. If the vector is of length 1 it is + * used for all shapes. + * @param collision_object_types A int identifying a conversion mode for the object. (ex. convert meshes to convex + * hulls) + * @return true if successfully added, otherwise false. */ + bool addCollisionObject(const std::string& name, const collision_detection::BodyType& mask_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, bool enabled = true); + + /**@brief Find if a collision object already exists + * @param name The name of the collision object + * @return true if it exists, otherwise false. */ + bool hasCollisionObject(const std::string& name) const; + + /**@brief Remove an object from the checker + * @param name The name of the object + * @return true if successfully removed, otherwise false. */ + bool removeCollisionObject(const std::string& name); + + /**@brief Enable an object + * @param name The name of the object */ + bool enableCollisionObject(const std::string& name); + + /**@brief Disable an object + * @param name The name of the object */ + bool disableCollisionObject(const std::string& name); + + /**@brief Set a single collision object's tansform + * @param name The name of the object + * @param pose The tranformation in world */ + void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose); + + /**@brief Set a single collision object's tansform + * @param name The name of the object + * @param pose The tranformation in world */ + void setCollisionObjectsTransform(const std::string& name, const btTransform& pose); + + /**@brief Set a series of collision objects' tranforms + * @param names The names of the object + * @param poses The tranformations in world */ + void setCollisionObjectsTransform(const std::vector& names, + const AlignedVector& poses); + + /**@brief Set a series of collision objects' tranforms + * @param transforms A transform map */ + void setCollisionObjectsTransform(const AlignedMap& transforms); + + /**@brief Set which collision objects can move + * @param names A vector of collision object names */ + void setActiveCollisionObjects(const std::vector& names); + + /**@brief Get which collision objects can move + * @return A list of collision object names */ + const std::vector& getActiveCollisionObjects() const; + + /**@brief Set the contact distance threshold for which collision should be considered. + * @param contact_distance The contact distance */ + void setContactDistanceThreshold(double contact_distance); + + /**@brief Get the contact distance threshold + * @return The contact distance */ + double getContactDistanceThreshold() const; + + /** @brief Set the active function for determining if two links are allowed to be in collision */ + void setIsContactAllowedFn(IsContactAllowedFn fn); + + /** @brief Get the active function for determining if two links are allowed to be in collision */ + IsContactAllowedFn getIsContactAllowedFn() const; + + /**@brief Perform a contact test for all objects in the manager + * @param collisions The Contact results data + * @param acm The allowed collision matrix + * @param req The contact request */ + void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm); + + /**@brief Perform a contact test for all objects in the manager and the external ones provided + * @param collisions The Contact results data + * @param req The contact request + * @param acm The allowed collision matrix + * @param req The contact request + * @param cows_external Objects to check which are not part of the manager */ + void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, + const std::vector cows_external); + + /**@brief Add a bullet collision object to the manager + * @param cow The bullet collision object */ + void addCollisionObject(const CollisionObjectWrapperPtr& cow); + + /**@brief Return collision objects + * @return A map of collision objects */ + const std::map& getCollisionObjects() const; + +private: + /** @brief A list of the active collision objects */ + std::vector active_; + + /** @brief The contact distance threshold */ + double contact_distance_; + + /** @brief The is allowed collision function */ + IsContactAllowedFn fn_; + + /** @brief The bullet collision dispatcher used for getting object to object collison algorithm */ + std::unique_ptr dispatcher_; + + /** @brief The bullet collision dispatcher configuration information */ + btDispatcherInfo dispatch_info_; + + /** @brief The bullet collision configuration */ + btDefaultCollisionConfiguration coll_config_; + + /** @brief The bullet broadphase interface */ + std::unique_ptr broadphase_; + + /** @brief A map of all (static and active) collision objects being managed */ + std::map link2cow_; + + /**@brief Perform a contact test for the provided object which is not part of the manager + * @param cow The Collision object + * @param collisions The collision results */ + void contactTest(const CollisionObjectWrapperPtr& cow, ContactTestData& collisions); +}; +} // namespace collision_detection_bullet +#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_COLLISION_BULLET_DISCRETE_BVH_MANAGER_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h new file mode 100644 index 0000000000..d6462ebefe --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h @@ -0,0 +1,803 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * Copyright (c) 2013, John Schulman + * 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. + * + * 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. + *********************************************************************/ + +/* Authors: John Schulman, Levi Armstrong */ + +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BULLET_UTILS_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BULLET_UTILS_H_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace collision_detection_bullet +{ +#define METERS + +const btScalar BULLET_MARGIN = 0.0f; +const btScalar BULLET_SUPPORT_FUNC_TOLERANCE = 0.01f METERS; +const btScalar BULLET_LENGTH_TOLERANCE = 0.001f METERS; +const btScalar BULLET_EPSILON = 1e-3f; // numerical precision limit +const btScalar BULLET_DEFAULT_CONTACT_DISTANCE = 0.00f; // All pairs closer than this distance get reported +const bool BULLET_COMPOUND_USE_DYNAMIC_AABB = true; + +MOVEIT_CLASS_FORWARD(CollisionObjectWrapper) + +/** \brief Converts eigen vector to bullet vector */ +inline btVector3 convertEigenToBt(const Eigen::Vector3d& v) +{ + return btVector3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2])); +} + +/** \brief Converts bullet vector to eigen vector */ +inline Eigen::Vector3d convertBtToEigen(const btVector3& v) +{ + return Eigen::Vector3d(static_cast(v.x()), static_cast(v.y()), static_cast(v.z())); +} + +/** \brief Converts eigen quaternion to bullet quaternion */ +inline btQuaternion convertEigenToBt(const Eigen::Quaterniond& q) +{ + return btQuaternion(static_cast(q.x()), static_cast(q.y()), static_cast(q.z()), + static_cast(q.w())); +} + +/** \brief Converts eigen matrix to bullet matrix */ +inline btMatrix3x3 convertEigenToBt(const Eigen::Matrix3d& r) +{ + return btMatrix3x3(static_cast(r(0, 0)), static_cast(r(0, 1)), static_cast(r(0, 2)), + static_cast(r(1, 0)), static_cast(r(1, 1)), static_cast(r(1, 2)), + static_cast(r(2, 0)), static_cast(r(2, 1)), static_cast(r(2, 2))); +} + +/** \brief Converts bullet transform to eigen transform */ +inline btTransform convertEigenToBt(const Eigen::Isometry3d& t) +{ + const Eigen::Matrix3d& rot = t.matrix().block<3, 3>(0, 0); + const Eigen::Vector3d& tran = t.translation(); + + btMatrix3x3 mat = convertEigenToBt(rot); + btVector3 translation = convertEigenToBt(tran); + + return btTransform(mat, translation); +} + +/** @brief Tesseract bullet collision object. + * + * A wrapper around bullet's collision object which contains specific information related to bullet */ +class CollisionObjectWrapper : public btCollisionObject +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Standard constructor */ + CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types); + + /** \brief Constructor for attached robot objects */ + CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::set& touch_links); + + /** \brief Bitfield specifies to which group the object belongs */ + short int m_collision_filter_group; + + /** \brief Bitfield specifies against which other groups the object is collision checked */ + short int m_collision_filter_mask; + + /** \brief Indicates if the collision object is used for a collision check */ + bool m_enabled; + + /** \brief The robot links the collision objects is allowed to touch */ + std::set m_touch_links; + + /** @brief Get the collision object name */ + const std::string& getName() const + { + return m_name; + } + + /** @brief Get a user defined type */ + const collision_detection::BodyType& getTypeID() const + { + return m_type_id; + } + + /** \brief Check if two CollisionObjectWrapper objects point to the same source object + * \return True if same objects, false otherwise */ + bool sameObject(const CollisionObjectWrapper& other) const + { + return m_name == other.m_name && m_type_id == other.m_type_id && m_shapes.size() == other.m_shapes.size() && + m_shape_poses.size() == other.m_shape_poses.size() && + std::equal(m_shapes.begin(), m_shapes.end(), other.m_shapes.begin()) && + std::equal(m_shape_poses.begin(), m_shape_poses.end(), other.m_shape_poses.begin(), + [](const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2) { return t1.isApprox(t2); }); + } + + /** @brief Get the collision objects axis aligned bounding box + * @param aabb_min The minimum point + * @param aabb_max The maximum point */ + void getAABB(btVector3& aabb_min, btVector3& aabb_max) const + { + getCollisionShape()->getAabb(getWorldTransform(), aabb_min, aabb_max); + const btScalar& d = getContactProcessingThreshold(); + btVector3 contact_threshold(d, d, d); + aabb_min -= contact_threshold; + aabb_max += contact_threshold; + } + + /** @brief Clones the collision objects but not the collision shape wich is const. + * @return Shared Pointer to the cloned collision object */ + std::shared_ptr clone() + { + std::shared_ptr clone_cow( + new CollisionObjectWrapper(m_name, m_type_id, m_shapes, m_shape_poses, m_collision_object_types, m_data)); + clone_cow->setCollisionShape(getCollisionShape()); + clone_cow->setWorldTransform(getWorldTransform()); + clone_cow->m_collision_filter_group = m_collision_filter_group; + clone_cow->m_collision_filter_mask = m_collision_filter_mask; + clone_cow->m_enabled = m_enabled; + clone_cow->setBroadphaseHandle(nullptr); + clone_cow->m_touch_links = m_touch_links; + clone_cow->setContactProcessingThreshold(this->getContactProcessingThreshold()); + return clone_cow; + } + + /** \brief Manage memory of a raw pointer shape */ + template + void manage(T* t) + { + m_data.push_back(std::shared_ptr(t)); + } + + /** \brief Manage memory of a shared pointer shape */ + template + void manage(std::shared_ptr t) + { + m_data.push_back(t); + } + +protected: + /** @brief Special constructor used by the clone method */ + CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::vector>& data); + + std::string m_name; + collision_detection::BodyType m_type_id; + + /** @brief The shapes that define the collison object */ + std::vector m_shapes; + + /** @brief The poses of the shapes */ + AlignedVector m_shape_poses; + + /** @brief The shape collision object type to be used */ + std::vector m_collision_object_types; + + /** @brief Manages the collision shape pointer so they get destroyed */ + std::vector> m_data; +}; + +/** \brief Computes the local supporting vertex of a convex shape. + * + * If multiple vertices with equal support products exists, their average is calculated and returned. + * + * \param shape The convex shape to check + * \param localNormal The support direction to search for in shape local coordinates + * \param outsupport The value of the calculated support mapping + * \param outpt The computed support point */ +inline void getAverageSupport(const btConvexShape* shape, const btVector3& localNormal, float& outsupport, + btVector3& outpt) +{ + btVector3 pt_sum(0, 0, 0); + float pt_count = 0; + float max_support = -1000; + + const btPolyhedralConvexShape* pshape = dynamic_cast(shape); + if (pshape) + { + int n_pts = pshape->getNumVertices(); + + for (int i = 0; i < n_pts; ++i) + { + btVector3 pt; + pshape->getVertex(i, pt); + + float sup = pt.dot(localNormal); + if (sup > max_support + BULLET_EPSILON) + { + pt_count = 1; + pt_sum = pt; + max_support = sup; + } + else if (sup < max_support - BULLET_EPSILON) + { + } + else + { + pt_count += 1; + pt_sum += pt; + } + } + outsupport = max_support; + outpt = pt_sum / pt_count; + } + else + { + outpt = shape->localGetSupportingVertexWithoutMargin(localNormal); + outsupport = localNormal.dot(outpt); + } +} + +/** @brief Check if a collision check is required between the provided collision objects + * @param cow1 The first collision object + * @param cow2 The second collision object + * @param acm The contact allowed function pointer + * @param verbose Indicate if verbose information should be printed + * @return True if the two collision objects should be checked for collision, otherwise false */ +inline bool needsCollisionCheck(const CollisionObjectWrapper& cow1, const CollisionObjectWrapper& cow2, + const IsContactAllowedFn& allowed_fn, + const collision_detection::AllowedCollisionMatrix* acm, bool verbose = false) +{ + if (!cow1.m_enabled) + return false; + + if (!cow2.m_enabled) + return false; + + if (!((cow2.m_collision_filter_group & cow1.m_collision_filter_mask) && + (cow1.m_collision_filter_group & cow2.m_collision_filter_mask))) + return false; + + if (isContactAllowed(cow1.getName(), cow2.getName(), allowed_fn, acm, verbose)) + return false; + + if (cow1.getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && + cow2.getTypeID() == collision_detection::BodyType::ROBOT_LINK) + if (cow1.m_touch_links.find(cow2.getName()) != cow1.m_touch_links.end()) + return false; + + if (cow2.getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && + cow1.getTypeID() == collision_detection::BodyType::ROBOT_LINK) + if (cow2.m_touch_links.find(cow1.getName()) == cow2.m_touch_links.end()) + return false; + + // TODO: Add check for two objects attached to the same link + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Checking collision btw " << cow1.getName() << " vs " + << cow2.getName()); + + return true; +} + +/** \brief Converts a bullet contact result to MoveIt format and adds it to the result data structure */ +inline btScalar addDiscreteSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, + const btCollisionObjectWrapper* colObj1Wrap, ContactTestData& collisions) +{ + assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); + assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); + const CollisionObjectWrapper* cd0 = static_cast(colObj0Wrap->getCollisionObject()); + const CollisionObjectWrapper* cd1 = static_cast(colObj1Wrap->getCollisionObject()); + + std::pair pc = getObjectPairKey(cd0->getName(), cd1->getName()); + + const auto& it = collisions.res.contacts.find(pc); + bool found = (it != collisions.res.contacts.end()); + + collision_detection::Contact contact; + contact.body_name_1 = cd0->getName(); + contact.body_name_2 = cd1->getName(); + contact.depth = static_cast(cp.m_distance1); + contact.normal = convertBtToEigen(-1 * cp.m_normalWorldOnB); + contact.pos = convertBtToEigen(cp.m_positionWorldOnA); + contact.nearest_points[0] = contact.pos; + contact.nearest_points[1] = convertBtToEigen(cp.m_positionWorldOnB); + + contact.body_type_1 = cd0->getTypeID(); + contact.body_type_2 = cd0->getTypeID(); + + if (!processResult(collisions, contact, pc, found)) + { + return 0; + } + + return 1; +} + +/** \brief Processes a contact point */ +struct TesseractBridgedManifoldResult : public btManifoldResult +{ + btCollisionWorld::ContactResultCallback& m_resultCallback; + + TesseractBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap, + btCollisionWorld::ContactResultCallback& resultCallback) + : btManifoldResult(obj0Wrap, obj1Wrap), m_resultCallback(resultCallback) + { + } + + void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) override + { + bool is_swapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + btVector3 point_a = pointInWorld + normalOnBInWorld * depth; + btVector3 local_a; + btVector3 local_b; + if (is_swapped) + { + local_a = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); + local_b = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + else + { + local_a = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); + local_b = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + + btManifoldPoint new_pt(local_a, local_b, normalOnBInWorld, depth); + new_pt.m_positionWorldOnA = point_a; + new_pt.m_positionWorldOnB = pointInWorld; + + // BP mod, store contact triangles. + if (is_swapped) + { + new_pt.m_partId0 = m_partId1; + new_pt.m_partId1 = m_partId0; + new_pt.m_index0 = m_index1; + new_pt.m_index1 = m_index0; + } + else + { + new_pt.m_partId0 = m_partId0; + new_pt.m_partId1 = m_partId1; + new_pt.m_index0 = m_index0; + new_pt.m_index1 = m_index1; + } + + // experimental feature info, for per-triangle material etc. + const btCollisionObjectWrapper* obj0_wrap = is_swapped ? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1_wrap = is_swapped ? m_body0Wrap : m_body1Wrap; + m_resultCallback.addSingleResult(new_pt, obj0_wrap, new_pt.m_partId0, new_pt.m_index0, obj1_wrap, new_pt.m_partId1, + new_pt.m_index1); + } +}; + +/** @brief Abstract interface for both discrete and continuous reporting of contact points */ +struct BroadphaseContactResultCallback +{ + ContactTestData& collisions_; + double contact_distance_; + bool verbose_; + + BroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false) + : collisions_(collisions), contact_distance_(contact_distance), verbose_(verbose) + { + } + + virtual ~BroadphaseContactResultCallback() = default; + + virtual bool needsCollision(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) const + { + return !collisions_.done && needsCollisionCheck(*cow0, *cow1, collisions_.fn, collisions_.acm, verbose_); + } + + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, + int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, + int index1) = 0; +}; + +/** \brief addSingleResult of this struct is called each time the broadphase check indicates a collision. */ +struct DiscreteBroadphaseContactResultCallback : public BroadphaseContactResultCallback +{ + DiscreteBroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false) + : BroadphaseContactResultCallback(collisions, contact_distance, verbose) + { + } + + btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, + int /*index0*/, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, + int /*index1*/) override + { + if (cp.m_distance1 > static_cast(contact_distance_)) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not close enough for collision with " << cp.m_distance1); + return 0; + } + + return addDiscreteSingleResult(cp, colObj0Wrap, colObj1Wrap, collisions_); + } +}; + +/** \brief Processes a contact point */ +struct TesseractBroadphaseBridgedManifoldResult : public btManifoldResult +{ + BroadphaseContactResultCallback& result_callback_; + + TesseractBroadphaseBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, + const btCollisionObjectWrapper* obj1Wrap, + BroadphaseContactResultCallback& result_callback) + : btManifoldResult(obj0Wrap, obj1Wrap), result_callback_(result_callback) + { + } + + void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) override + { + if (result_callback_.collisions_.done || result_callback_.collisions_.pair_done || + depth > static_cast(result_callback_.contact_distance_)) + { + return; + } + + bool is_swapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + btVector3 point_a = pointInWorld + normalOnBInWorld * depth; + btVector3 local_a; + btVector3 local_b; + if (is_swapped) + { + local_a = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); + local_b = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + else + { + local_a = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); + local_b = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + + btManifoldPoint new_pt(local_a, local_b, normalOnBInWorld, depth); + new_pt.m_positionWorldOnA = point_a; + new_pt.m_positionWorldOnB = pointInWorld; + + // BP mod, store contact triangles. + if (is_swapped) + { + new_pt.m_partId0 = m_partId1; + new_pt.m_partId1 = m_partId0; + new_pt.m_index0 = m_index1; + new_pt.m_index1 = m_index0; + } + else + { + new_pt.m_partId0 = m_partId0; + new_pt.m_partId1 = m_partId1; + new_pt.m_index0 = m_index0; + new_pt.m_index1 = m_index1; + } + + // experimental feature info, for per-triangle material etc. + const btCollisionObjectWrapper* obj0_wrap = is_swapped ? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1_wrap = is_swapped ? m_body0Wrap : m_body1Wrap; + result_callback_.addSingleResult(new_pt, obj0_wrap, new_pt.m_partId0, new_pt.m_index0, obj1_wrap, new_pt.m_partId1, + new_pt.m_index1); + } +}; + +/** @brief Check a collision object not in the broadphase to the broadphase which may eventually be exposed. + * + * This is copied directly out of BulletWorld */ +struct TesseractSingleContactCallback : public btBroadphaseAabbCallback +{ + btCollisionObject* m_collisionObject; /**< @brief The bullet collision object */ + btCollisionDispatcher* m_dispatcher; /**< @brief The bullet collision dispatcher used for getting object to object + collison algorithm */ + const btDispatcherInfo& m_dispatch_info; /**< @brief The bullet collision dispatcher configuration information */ + btCollisionWorld::ContactResultCallback& m_resultCallback; + + TesseractSingleContactCallback(btCollisionObject* collisionObject, btCollisionDispatcher* dispatcher, + const btDispatcherInfo& dispatch_info, + btCollisionWorld::ContactResultCallback& resultCallback) + : m_collisionObject(collisionObject) + , m_dispatcher(dispatcher) + , m_dispatch_info(dispatch_info) + , m_resultCallback(resultCallback) + { + } + + bool process(const btBroadphaseProxy* proxy) override + { + btCollisionObject* collision_object = static_cast(proxy->m_clientObject); + if (collision_object == m_collisionObject) + return true; + + if (m_resultCallback.needsCollision(collision_object->getBroadphaseHandle())) + { + btCollisionObjectWrapper ob0(nullptr, m_collisionObject->getCollisionShape(), m_collisionObject, + m_collisionObject->getWorldTransform(), -1, -1); + btCollisionObjectWrapper ob1(nullptr, collision_object->getCollisionShape(), collision_object, + collision_object->getWorldTransform(), -1, -1); + + btCollisionAlgorithm* algorithm = m_dispatcher->findAlgorithm(&ob0, &ob1, nullptr, BT_CLOSEST_POINT_ALGORITHMS); + if (algorithm) + { + TesseractBridgedManifoldResult contact_point_result(&ob0, &ob1, m_resultCallback); + contact_point_result.m_closestPointDistanceThreshold = m_resultCallback.m_closestDistanceThreshold; + + // discrete collision detection query + algorithm->processCollision(&ob0, &ob1, m_dispatch_info, &contact_point_result); + + algorithm->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algorithm); + } + } + return true; + } +}; + +/** @brief A callback function that is called as part of the broadphase collision checking. + * + * If the AABB of two collision objects are overlapping the processOverlap method is called and they are checked for + * collision/distance and the results are stored in collision_. */ +class TesseractCollisionPairCallback : public btOverlapCallback +{ + const btDispatcherInfo& dispatch_info_; + btCollisionDispatcher* dispatcher_; + BroadphaseContactResultCallback& results_callback_; + +public: + TesseractCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher, + BroadphaseContactResultCallback& results_callback) + : dispatch_info_(dispatchInfo), dispatcher_(dispatcher), results_callback_(results_callback) + { + } + + ~TesseractCollisionPairCallback() override = default; + + bool processOverlap(btBroadphasePair& pair) override + { + results_callback_.collisions_.pair_done = false; + + if (results_callback_.collisions_.done) + { + return false; + } + + const CollisionObjectWrapper* cow0 = static_cast(pair.m_pProxy0->m_clientObject); + const CollisionObjectWrapper* cow1 = static_cast(pair.m_pProxy1->m_clientObject); + + std::pair pair_names{ cow0->getName(), cow1->getName() }; + ROS_DEBUG_STREAM("Processing " << cow0->getName() << " vs " << cow1->getName()); + + if (results_callback_.needsCollision(cow0, cow1)) + { + btCollisionObjectWrapper obj0_wrap(nullptr, cow0->getCollisionShape(), cow0, cow0->getWorldTransform(), -1, -1); + btCollisionObjectWrapper obj1_wrap(nullptr, cow1->getCollisionShape(), cow1, cow1->getWorldTransform(), -1, -1); + + // dispatcher will keep algorithms persistent in the collision pair + if (!pair.m_algorithm) + { + pair.m_algorithm = dispatcher_->findAlgorithm(&obj0_wrap, &obj1_wrap, nullptr, BT_CLOSEST_POINT_ALGORITHMS); + } + + if (pair.m_algorithm) + { + TesseractBroadphaseBridgedManifoldResult contact_point_result(&obj0_wrap, &obj1_wrap, results_callback_); + contact_point_result.m_closestPointDistanceThreshold = + static_cast(results_callback_.contact_distance_); + + // discrete collision detection query + pair.m_algorithm->processCollision(&obj0_wrap, &obj1_wrap, dispatch_info_, &contact_point_result); + } + } + return false; + } +}; + +/** \brief Casts a geometric shape into a btCollisionShape */ +btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom, + const CollisionObjectType& collision_object_type, CollisionObjectWrapper* cow); + +/** @brief Update a collision objects filters + * @param active A list of active collision objects + * @param cow The collision object to update. + * @param continuous Indicate if the object is a continuous collision object. + * + * Currently continuous collision objects can only be checked against static objects. Continuous to Continuous + * collision checking is currently not supports. TODO LEVI: Add support for Continuous to Continuous collision + * checking. */ +inline void updateCollisionObjectFilters(const std::vector& active, CollisionObjectWrapper& cow, + bool continuous) +{ + cow.m_collision_filter_group = btBroadphaseProxy::KinematicFilter; + + if (!isLinkActive(active, cow.getName())) + { + cow.m_collision_filter_group = btBroadphaseProxy::StaticFilter; + } + + if (cow.m_collision_filter_group == btBroadphaseProxy::StaticFilter) + { + cow.m_collision_filter_mask = btBroadphaseProxy::KinematicFilter; + } + else + { + (continuous) ? (cow.m_collision_filter_mask = btBroadphaseProxy::StaticFilter) : + (cow.m_collision_filter_mask = btBroadphaseProxy::StaticFilter | btBroadphaseProxy::KinematicFilter); + } + + if (cow.getBroadphaseHandle()) + { + cow.getBroadphaseHandle()->m_collisionFilterGroup = cow.m_collision_filter_group; + cow.getBroadphaseHandle()->m_collisionFilterMask = cow.m_collision_filter_mask; + } +} + +/** \brief Wrapper around constructing a CollisionObjectWrapper */ +inline CollisionObjectWrapperPtr createCollisionObject(const std::string& name, + const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + bool enabled = true) +{ + // dont add object that does not have geometry + if (shapes.empty() || shape_poses.empty() || (shapes.size() != shape_poses.size())) + { + ROS_DEBUG_NAMED("collision_detection.bullet", "ignoring link %s", name.c_str()); + return nullptr; + } + + CollisionObjectWrapperPtr new_cow( + new CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types)); + + new_cow->m_enabled = enabled; + new_cow->setContactProcessingThreshold(BULLET_DEFAULT_CONTACT_DISTANCE); + + ROS_DEBUG_NAMED("collision_detection.bullet", "Created collision object for link %s", new_cow->getName().c_str()); + return new_cow; +} + +// TODO: Unify with other createCollisionObject functions +/** \brief Wrapper around constructing a CollisionObjectWrapper for attached objects */ +inline CollisionObjectWrapperPtr createCollisionObject(const std::string& name, + const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::set& touch_links, bool enabled = true) +{ + // dont add object that does not have geometry + if (shapes.empty() || shape_poses.empty() || (shapes.size() != shape_poses.size())) + { + ROS_DEBUG_NAMED("collision_detection.bullet", "ignoring link %s", name.c_str()); + return nullptr; + } + + CollisionObjectWrapperPtr new_cow( + new CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types, touch_links)); + + new_cow->m_enabled = enabled; + new_cow->setContactProcessingThreshold(BULLET_DEFAULT_CONTACT_DISTANCE); + + ROS_DEBUG_NAMED("collision_detection.bullet", "Created collision object for link %s", new_cow->getName().c_str()); + return new_cow; +} + +/** \brief Processes a contact after positive broadphase check */ +struct DiscreteCollisionCollector : public btCollisionWorld::ContactResultCallback +{ + ContactTestData& collisions_; + const CollisionObjectWrapperPtr cow_; + double contact_distance_; + bool verbose_; + + DiscreteCollisionCollector(ContactTestData& collisions, const CollisionObjectWrapperPtr& cow, double contact_distance, + bool verbose = false) + : collisions_(collisions), cow_(cow), contact_distance_(contact_distance), verbose_(verbose) + { + m_closestDistanceThreshold = static_cast(contact_distance); + m_collisionFilterGroup = cow->m_collision_filter_group; + m_collisionFilterMask = cow->m_collision_filter_mask; + } + + btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, + int /*index0*/, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, + int /*index1*/) override + { + if (cp.m_distance1 > static_cast(contact_distance_)) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not close enough for collision with " << cp.m_distance1); + return 0; + } + + return addDiscreteSingleResult(cp, colObj0Wrap, colObj1Wrap, collisions_); + } + + bool needsCollision(btBroadphaseProxy* proxy0) const override + { + return !collisions_.done && + needsCollisionCheck(*cow_, *(static_cast(proxy0->m_clientObject)), collisions_.fn, + collisions_.acm, verbose_); + } +}; + +/** @brief Update the Broadphase AABB for the input collision object + * @param cow The collision objects + * @param broadphase The bullet broadphase interface + * @param dispatcher The bullet collision dispatcher */ +inline void updateBroadphaseAABB(const CollisionObjectWrapperPtr& cow, + const std::unique_ptr& broadphase, + const std::unique_ptr& dispatcher) +{ + btVector3 aabb_min, aabb_max; + cow->getAABB(aabb_min, aabb_max); + + // Update the broadphase aabb + assert(cow->getBroadphaseHandle() != nullptr); + broadphase->setAabb(cow->getBroadphaseHandle(), aabb_min, aabb_max, dispatcher.get()); +} + +/** @brief Remove the collision object from broadphase + * @param cow The collision objects + * @param broadphase The bullet broadphase interface + * @param dispatcher The bullet collision dispatcher */ +inline void removeCollisionObjectFromBroadphase(const CollisionObjectWrapperPtr& cow, + const std::unique_ptr& broadphase, + const std::unique_ptr& dispatcher) +{ + btBroadphaseProxy* bp = cow->getBroadphaseHandle(); + if (bp) + { + // only clear the cached algorithms + broadphase->getOverlappingPairCache()->cleanProxyFromPairs(bp, dispatcher.get()); + broadphase->destroyProxy(bp, dispatcher.get()); + cow->setBroadphaseHandle(nullptr); + } +} + +/** @brief Add the collision object to broadphase + * @param cow The collision objects + * @param broadphase The bullet broadphase interface + * @param dispatcher The bullet collision dispatcher */ +inline void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr& cow, + const std::unique_ptr& broadphase, + const std::unique_ptr& dispatcher) +{ + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Added " << cow->getName() << " to broadphase"); + btVector3 aabb_min, aabb_max; + cow->getAABB(aabb_min, aabb_max); + + // Add the active collision object to the broadphase + int type = cow->getCollisionShape()->getShapeType(); + cow->setBroadphaseHandle(broadphase->createProxy(aabb_min, aabb_max, type, cow.get(), cow->m_collision_filter_group, + cow->m_collision_filter_mask, dispatcher.get())); +} +} // namespace collision_detection_bullet +#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BULLET_UTILS_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h new file mode 100644 index 0000000000..b4929e6dfb --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h @@ -0,0 +1,271 @@ +/********************************************************************* + * Software License Agreement (Apache License) + * + * Copyright (c) 2017, Southwest Research Institute + * All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_CONTACT_CHECKER_COMMON_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_CONTACT_CHECKER_COMMON_H_ + +#include +#include +#include +#include + +#include +#include +#include + +namespace collision_detection_bullet +{ +/** + * @brief Get a key for two object to search the collision matrix + * @param obj1 First collision object name + * @param obj2 Second collision object name + * @return The collision pair key + */ +inline std::pair getObjectPairKey(const std::string& obj1, const std::string& obj2) +{ + return obj1 < obj2 ? std::make_pair(obj1, obj2) : std::make_pair(obj2, obj1); +} + +/** + * @brief This will check if a link is active provided a list. If the list is empty the link is considered active. + * @param active List of active link names + * @param name The name of link to check if it is active. + */ +inline bool isLinkActive(const std::vector& active, const std::string& name) +{ + return active.empty() || (std::find(active.begin(), active.end(), name) != active.end()); +} + +/** + * @brief Determine if contact is allowed between two objects. + * @param name1 The name of the first object + * @param name2 The name of the second object + * @param acm The contact allowed function + * @param verbose If true print debug informaton + * @return True if contact is allowed between the two object, otherwise false. + */ +inline bool isContactAllowed(const std::string& name1, const std::string& name2, const IsContactAllowedFn& acm_fn, + const collision_detection::AllowedCollisionMatrix* acm, bool verbose = false) +{ + // do not distance check geoms part of the same object / link / attached body + if (name1 == name2) + return true; + + if (acm_fn != nullptr && acm_fn(name1, name2, acm)) + { + if (verbose) + { + ROS_DEBUG_NAMED("collision_detection.bullet", + "Collision between '%s' and '%s' is allowed. No contacts are computed.", name1.c_str(), + name2.c_str()); + } + return true; + } + + if (verbose) + { + ROS_DEBUG_NAMED("collision_detection.bullet", "Actually checking collisions between %s and %s", name1.c_str(), + name2.c_str()); + } + + return false; +} + +/** \brief Stores a single contact result in the requested way. +* \param found Indicates if a contact for this pair of objects has already been found +* \return Pointer to the newly inserted contact */ +inline collision_detection::Contact* processResult(ContactTestData& cdata, collision_detection::Contact& contact, + const std::pair& key, bool found) +{ + // add deepest penetration / smallest distance to result + if (cdata.req.distance) + { + if (contact.depth < cdata.res.distance) + { + cdata.res.distance = contact.depth; + } + } + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Contact btw " << key.first << " and " << key.second + << " dist: " << contact.depth); + if (!found) + { + cdata.res.collision = true; + std::vector data; + if (!cdata.req.contacts) + { + cdata.done = true; + return nullptr; + } + else + { + data.reserve(cdata.req.max_contacts_per_pair); + data.emplace_back(contact); + cdata.res.contact_count++; + } + + if (cdata.res.contact_count >= cdata.req.max_contacts) + { + cdata.done = true; + } + + if (cdata.req.max_contacts_per_pair == 1u) + { + cdata.pair_done = true; + } + + return &(cdata.res.contacts.insert(std::make_pair(key, data)).first->second.back()); + } + else + { + std::vector& dr = cdata.res.contacts[key]; + dr.emplace_back(contact); + cdata.res.contact_count++; + + if (dr.size() >= cdata.req.max_contacts_per_pair) + { + cdata.pair_done = true; + } + + if (cdata.res.contact_count >= cdata.req.max_contacts) + { + cdata.done = true; + } + + return &(dr.back()); + } + + return nullptr; +} + +/** + * @brief Create a convex hull from vertices using Bullet Convex Hull Computer + * @param (Output) vertices A vector of vertices + * @param (Output) faces The first values indicates the number of vertices that define the face followed by the vertice + * index + * @param (input) input A vector of point to create a convex hull from + * @param (input) shrink If positive, the convex hull is shrunken by that amount (each face is moved by "shrink" length + * units towards the center along its normal). + * @param (input) shrinkClamp If positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where + * "innerRadius" is the minimum distance of a face to the center of the convex hull. + * @return The number of faces. If less than zero an error occured when trying to create the convex hull + */ +inline int createConvexHull(AlignedVector& vertices, std::vector& faces, + const AlignedVector& input, double shrink = -1, double shrinkClamp = -1) +{ + vertices.clear(); + faces.clear(); + + btConvexHullComputer conv; + btAlignedObjectArray points; + points.reserve(static_cast(input.size())); + for (const Eigen::Vector3d& v : input) + { + points.push_back(btVector3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2]))); + } + + btScalar val = conv.compute(&points[0].getX(), sizeof(btVector3), points.size(), static_cast(shrink), + static_cast(shrinkClamp)); + if (val < 0) + { + ROS_ERROR("Failed to create convex hull"); + return -1; + } + + int num_verts = conv.vertices.size(); + vertices.reserve(static_cast(num_verts)); + for (int i = 0; i < num_verts; i++) + { + btVector3& v = conv.vertices[i]; + vertices.push_back(Eigen::Vector3d(v.getX(), v.getY(), v.getZ())); + } + + int num_faces = conv.faces.size(); + faces.reserve(static_cast(3 * num_faces)); + for (int i = 0; i < num_faces; i++) + { + std::vector face; + face.reserve(3); + + const btConvexHullComputer::Edge* source_edge = &(conv.edges[conv.faces[i]]); + int a = source_edge->getSourceVertex(); + face.push_back(a); + + int b = source_edge->getTargetVertex(); + face.push_back(b); + + const btConvexHullComputer::Edge* edge = source_edge->getNextEdgeOfFace(); + int c = edge->getTargetVertex(); + face.push_back(c); + + edge = edge->getNextEdgeOfFace(); + c = edge->getTargetVertex(); + while (c != a) + { + face.push_back(c); + + edge = edge->getNextEdgeOfFace(); + c = edge->getTargetVertex(); + } + faces.push_back(static_cast(face.size())); + faces.insert(faces.end(), face.begin(), face.end()); + } + + return num_faces; +} + +inline bool allowedCollisionCheck(const std::string& body_1, const std::string& body_2, + const collision_detection::AllowedCollisionMatrix* acm) +{ + collision_detection::AllowedCollision::Type allowed_type; + + if (acm != nullptr) + { + if (acm->getEntry(body_1, body_2, allowed_type)) + { + if (allowed_type == collision_detection::AllowedCollision::Type::NEVER) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not allowed entry in ACM found, collision check between " + << body_1 << " and " << body_2); + return false; + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Entry in ACM found, skipping collision check as allowed " + << body_1 << " and " << body_2); + return true; + } + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No entry in ACM found, collision check between " + << body_1 << " and " << body_2); + return false; + } + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No ACM, collision check between " << body_1 << " and " + << body_2); + return false; + } +} +} // namespace collision_detection_bullet + +#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_CONTACT_CHECKER_COMMON_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h new file mode 100644 index 0000000000..c192b86df9 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h @@ -0,0 +1,109 @@ +/********************************************************************* + * Software License Agreement (Apache License) + * + * Copyright (c) 2018, Southwest Research Institute + * All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_ROS_BULLET_UTILS_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_ROS_BULLET_UTILS_H_ + +#include +#include +#include +#include +#include + +#include + +namespace collision_detection_bullet +{ +/** \brief Recursively traverses robot from root to get all active links +* +* \param active_links Stores the active links +* \param urdf_link The current urdf link representation +* \param active Indicates if link is considered active */ +static inline void getActiveLinkNamesRecursive(std::vector& active_links, + const urdf::LinkConstSharedPtr& urdf_link, bool active) +{ + if (active) + { + active_links.push_back(urdf_link->name); + for (const auto& child_link : urdf_link->child_links) + { + getActiveLinkNamesRecursive(active_links, child_link, active); + } + } + else + { + for (std::size_t i = 0; i < urdf_link->child_links.size(); ++i) + { + const urdf::LinkConstSharedPtr child_link = urdf_link->child_links[i]; + if ((child_link->parent_joint) && (child_link->parent_joint->type != urdf::Joint::FIXED)) + getActiveLinkNamesRecursive(active_links, child_link, true); + else + getActiveLinkNamesRecursive(active_links, child_link, active); + } + } +} + +inline shapes::ShapePtr constructShape(const urdf::Geometry* geom) +{ + shapes::Shape* result = nullptr; + switch (geom->type) + { + case urdf::Geometry::SPHERE: + result = new shapes::Sphere(static_cast(geom)->radius); + break; + case urdf::Geometry::BOX: + { + urdf::Vector3 dim = static_cast(geom)->dim; + result = new shapes::Box(dim.x, dim.y, dim.z); + } + break; + case urdf::Geometry::CYLINDER: + result = new shapes::Cylinder(static_cast(geom)->radius, + static_cast(geom)->length); + break; + case urdf::Geometry::MESH: + { + const urdf::Mesh* mesh = static_cast(geom); + if (!mesh->filename.empty()) + { + Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); + shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale); + result = m; + } + } + break; + default: + ROS_ERROR("Unknown geometry type: %d", static_cast(geom->type)); + break; + } + + return shapes::ShapePtr(result); +} + +inline Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose& pose) +{ + Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z); + Eigen::Isometry3d result; + result.translation() = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z); + result.linear() = q.toRotationMatrix(); + return result; +} +} // namespace collision_detection_bullet +#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_ROS_BULLET_UTILS_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bt.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h similarity index 76% rename from moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bt.h rename to moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h index b914e8c2a5..513bea0e16 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bt.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h @@ -34,22 +34,23 @@ /* Author: Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_BT_H_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_BT_H_ +#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALLOCATOR_BULLET_H_ +#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALLOCATOR_BULLET_H_ #include -#include -#include +#include +#include namespace collision_detection { /** \brief An allocator for Bullet collision detectors */ -class CollisionDetectorAllocatorBt - : public CollisionDetectorAllocatorTemplate +class CollisionDetectorAllocatorBullet + : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_world_bt.cpp + static const std::string NAME; // defined in collision_world_bullet.cpp }; -} +} // namespace collision_detection -#endif +#endif // MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALLOCATOR_BULLET_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bt_plugin_loader.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h similarity index 84% rename from moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bt_plugin_loader.h rename to moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h index d60a29ce64..76182e582c 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bt_plugin_loader.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h @@ -34,18 +34,18 @@ /* Author: Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_DETECTOR_BT_PLUGIN_LOADER_H_ -#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_DETECTOR_BT_PLUGIN_LOADER_H_ +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_DETECTOR_BULLET_PLUGIN_LOADER_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_DETECTOR_BULLET_PLUGIN_LOADER_H_ #include -#include +#include namespace collision_detection { class CollisionDetectorBtPluginLoader : public CollisionPlugin { public: - virtual bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const; + bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const override; }; } -#endif // MOVEIT_COLLISION_DETECTION_BT_COLLISION_DETECTOR_BT_PLUGIN_LOADER_H_ +#endif // MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_DETECTOR_BULLET_PLUGIN_LOADER_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h similarity index 69% rename from moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h rename to moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h index a0aadf4b4a..de04d45100 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h @@ -34,21 +34,22 @@ /* Author: Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_ROBOT_ -#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_ROBOT_ +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_ROBOT_BULLET_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_ROBOT_BULLET_H_ -#include +#include +#include namespace collision_detection { -class CollisionRobotBt : public CollisionRobot +class CollisionRobotBullet : public CollisionRobot { - friend class CollisionWorldBt; + friend class CollisionWorldBullet; public: - CollisionRobotBt(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); + CollisionRobotBullet(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); - CollisionRobotBt(const CollisionRobotBt& other); + CollisionRobotBullet(const CollisionRobotBullet& other); void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state) const override; @@ -81,14 +82,38 @@ class CollisionRobotBt : public CollisionRobot const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override; protected: + /** \brief Updates the poses of the objects in the manager according to given robot state */ + void updateTransformsFromState(const robot_state::RobotState& state, + const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const; + + /** \brief Updates the collision objects saved in the manager to reflect a new padding or scaling of the robot links + */ void updatedPaddingOrScaling(const std::vector& links) override; + /** \brief All of the attached objects in the robot state are wrapped into bullet collision objects */ + void addAttachedOjects(const robot_state::RobotState& state, + std::vector& cows) const; + + /** \brief Bundles the different checkSelfCollision functions into a single function */ void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; + + /** \brief Bundles the different continuous checkSelfCollision functions into a single function */ + void checkSelfCollisionCCDHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const AllowedCollisionMatrix* acm) const; + + /** \brief Bundles the different checkOtherCollision functions into a single function */ void checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, const CollisionRobot& other_robot, const robot_state::RobotState& other_state, const AllowedCollisionMatrix* acm) const; + + /** \brief Construts a bullet collision object out of a robot link */ + void addLinkAsCollisionObjectWrapper(const urdf::LinkSharedPtr& link); + + /** \brief Handles all self collision checks */ + collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_; }; -} +} // namespace collision_detection -#endif +#endif // MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_ROBOT_BULLET_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h similarity index 66% rename from moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h rename to moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h index fb614b3c18..ea8473d3dc 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h @@ -34,20 +34,26 @@ /* Author: Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_WORLD_BT_ -#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_WORLD_BT_ +#ifndef MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_WORLD_BULLET_H_ +#define MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_WORLD_BULLET_H_ -#include +#include +#include +#include +#include namespace collision_detection { -class CollisionWorldBt : public CollisionWorld +class CollisionWorldBullet : public CollisionWorld { public: - CollisionWorldBt(); - explicit CollisionWorldBt(const WorldPtr& world); - CollisionWorldBt(const CollisionWorldBt& other, const WorldPtr& world); - ~CollisionWorldBt() override; + CollisionWorldBullet(); + + explicit CollisionWorldBullet(const WorldPtr& world); + + CollisionWorldBullet(const CollisionWorldBullet& other, const WorldPtr& world); + + ~CollisionWorldBullet() override; void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, const robot_state::RobotState& state) const override; @@ -71,16 +77,38 @@ class CollisionWorldBt : public CollisionWorld void setWorld(const WorldPtr& world) override; protected: + /** \brief Bundles the different checkWorldCollision functions into a single function */ void checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, const AllowedCollisionMatrix* acm) const; + + /** \brief Bundles the different checkRobotCollision functions into a single function */ void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; + /** \brief Bundles the different continuous checkRobotCollision functions into a single function */ + void checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const AllowedCollisionMatrix* acm) const; + + /** \brief Adds a world object to the collision managers */ + void addToManager(const World::Object* obj); + + /** \brief Updates a managed collision object with its world representation. + * + * We have three cases: 1) the object is part of the manager and not of world --> delete it + * 2) the object is not in the manager, therefore register to manager, + * 3) the object is in the manager then delete and add the modified */ + void updateManagedObject(const std::string& id); + + /** \brief Handles all discrete collision checks */ + collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_; + private: - void initialize(); + /** \brief Callback function executed for each change to the world environment */ void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); + World::ObserverHandle observer_handle_; }; -} +} // namespace collision_detection -#endif +#endif // MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_WORLD_BULLET_H_ diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp new file mode 100644 index 0000000000..b55b3a7c02 --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp @@ -0,0 +1,306 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * 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. + * + * 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. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#include + +#include "moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h" + +namespace collision_detection_bullet +{ +BulletDiscreteBVHManager::BulletDiscreteBVHManager() +{ + dispatcher_.reset(new btCollisionDispatcher(&coll_config_)); + + dispatcher_->registerCollisionCreateFunc( + BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE, + coll_config_.getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE, CONVEX_SHAPE_PROXYTYPE)); + + dispatcher_->setDispatcherFlags(dispatcher_->getDispatcherFlags() & + ~btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD); + + broadphase_.reset(new btDbvtBroadphase()); + + contact_distance_ = 0; +} + +BulletDiscreteBVHManager::~BulletDiscreteBVHManager() +{ + // clean up remaining objects + for (std::pair& cow : link2cow_) + { + removeCollisionObjectFromBroadphase(cow.second, broadphase_, dispatcher_); + } +} + +BulletDiscreteBVHManagerPtr BulletDiscreteBVHManager::clone() const +{ + BulletDiscreteBVHManagerPtr manager(new BulletDiscreteBVHManager()); + + for (const std::pair& cow : link2cow_) + { + CollisionObjectWrapperPtr new_cow = cow.second->clone(); + + assert(new_cow->getCollisionShape()); + assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); + + new_cow->setWorldTransform(cow.second->getWorldTransform()); + new_cow->setContactProcessingThreshold(static_cast(contact_distance_)); + manager->addCollisionObject(new_cow); + } + + manager->setActiveCollisionObjects(active_); + manager->setContactDistanceThreshold(contact_distance_); + manager->setIsContactAllowedFn(fn_); + + return manager; +} + +bool BulletDiscreteBVHManager::addCollisionObject(const std::string& name, const collision_detection::BodyType& mask_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + bool enabled) +{ + CollisionObjectWrapperPtr new_cow = + createCollisionObject(name, mask_id, shapes, shape_poses, collision_object_types, enabled); + if (new_cow != nullptr) + { + addCollisionObject(new_cow); + return true; + } + return false; +} + +bool BulletDiscreteBVHManager::hasCollisionObject(const std::string& name) const +{ + return (link2cow_.find(name) != link2cow_.end()); +} + +bool BulletDiscreteBVHManager::removeCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); // Levi TODO: Should these check be removed? + if (it != link2cow_.end()) + { + removeCollisionObjectFromBroadphase(it->second, broadphase_, dispatcher_); + link2cow_.erase(name); + return true; + } + + return false; +} + +bool BulletDiscreteBVHManager::enableCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); // Levi TODO: Should these check be removed? + if (it != link2cow_.end()) + { + it->second->m_enabled = true; + return true; + } + return false; +} + +bool BulletDiscreteBVHManager::disableCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); // Levi TODO: Should these check be removed? + if (it != link2cow_.end()) + { + it->second->m_enabled = false; + return true; + } + return false; +} + +void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) +{ + // TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with + // geometry + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + CollisionObjectWrapperPtr& cow = it->second; + cow->setWorldTransform(convertEigenToBt(pose)); + + // Update Collision Object Broadphase AABB + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } +} + +void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::string& name, const btTransform& pose) +{ + // TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with + // geometry + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + CollisionObjectWrapperPtr& cow = it->second; + cow->setWorldTransform(pose); + + // Update Collision Object Broadphase AABB + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } +} + +void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::vector& names, + const AlignedVector& poses) +{ + assert(names.size() == poses.size()); + for (size_t i = 0u; i < names.size(); ++i) + { + setCollisionObjectsTransform(names[i], poses[i]); + } +} + +void BulletDiscreteBVHManager::setCollisionObjectsTransform( + const AlignedMap& transforms) +{ + for (const std::pair& transform : transforms) + { + setCollisionObjectsTransform(transform.first, transform.second); + } +} + +void BulletDiscreteBVHManager::setActiveCollisionObjects(const std::vector& names) +{ + active_ = names; + + // Now need to update the broadphase with correct aabb + for (std::pair& co : link2cow_) + { + CollisionObjectWrapperPtr& cow = co.second; + updateCollisionObjectFilters(active_, *cow, false); + } +} + +const std::vector& BulletDiscreteBVHManager::getActiveCollisionObjects() const +{ + return active_; +} + +void BulletDiscreteBVHManager::setContactDistanceThreshold(double contact_distance) +{ + contact_distance_ = contact_distance; + + for (std::pair& co : link2cow_) + { + CollisionObjectWrapperPtr& cow = co.second; + cow->setContactProcessingThreshold(static_cast(contact_distance)); + assert(cow->getBroadphaseHandle() != nullptr); + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } +} + +double BulletDiscreteBVHManager::getContactDistanceThreshold() const +{ + return contact_distance_; +} + +void BulletDiscreteBVHManager::setIsContactAllowedFn(IsContactAllowedFn fn) +{ + fn_ = std::move(fn); +} + +IsContactAllowedFn BulletDiscreteBVHManager::getIsContactAllowedFn() const +{ + return fn_; +} + +void BulletDiscreteBVHManager::contactTest(collision_detection::CollisionResult& collisions, + const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm) +{ + ContactTestData cdata(active_, contact_distance_, fn_, collisions, req, acm); + + broadphase_->calculateOverlappingPairs(dispatcher_.get()); + btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Num overlapping candidates " + << pair_cache->getNumOverlappingPairs()); + + DiscreteBroadphaseContactResultCallback cc(cdata, contact_distance_); + TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); + pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get()); + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", (collisions.collision ? "In" : "No") << " collision with " + << collisions.contact_count + << " collisions"); +} + +void BulletDiscreteBVHManager::contactTest( + collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, + const std::vector cows_external) +{ + ContactTestData cdata(active_, contact_distance_, fn_, collisions, req, acm); + + broadphase_->calculateOverlappingPairs(dispatcher_.get()); + btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Num overlapping candidates " + << pair_cache->getNumOverlappingPairs()); + + DiscreteBroadphaseContactResultCallback cc(cdata, contact_distance_); + TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); + pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get()); + + for (const CollisionObjectWrapperPtr& cow : cows_external) + { + contactTest(cow, cdata); + } + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", (collisions.collision ? "In" : "No") << " collision with " + << collisions.contact_count + << " collisions"); +} + +void BulletDiscreteBVHManager::addCollisionObject(const CollisionObjectWrapperPtr& cow) +{ + link2cow_[cow->getName()] = cow; + addCollisionObjectToBroadphase(cow, broadphase_, dispatcher_); +} + +const std::map& BulletDiscreteBVHManager::getCollisionObjects() const +{ + return link2cow_; +} + +void BulletDiscreteBVHManager::contactTest(const CollisionObjectWrapperPtr& cow, ContactTestData& collisions) +{ + btVector3 min_aabb, max_aabb; + cow->getAABB(min_aabb, max_aabb); + + DiscreteCollisionCollector cc(collisions, cow, static_cast(cow->getContactProcessingThreshold())); + TesseractSingleContactCallback contact_cb(cow.get(), dispatcher_.get(), dispatch_info_, cc); + broadphase_->aabbTest(min_aabb, max_aabb, contact_cb); +} +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp new file mode 100644 index 0000000000..18b21bafaf --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp @@ -0,0 +1,343 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * Copyright (c) 2013, John Schulman + * 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. + * + * 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. + *********************************************************************/ + +/* Authors: John Schulman, Levi Armstrong */ + +#include "moveit/collision_detection_bullet/bullet_integration/bullet_utils.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace collision_detection_bullet +{ +btCollisionShape* createShapePrimitive(const shapes::Box* geom, const CollisionObjectType& collision_object_type) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); + const double* size = geom->size; + btScalar a = static_cast(size[0] / 2); + btScalar b = static_cast(size[1] / 2); + btScalar c = static_cast(size[2] / 2); + + return (new btBoxShape(btVector3(a, b, c))); +} + +btCollisionShape* createShapePrimitive(const shapes::Sphere* geom, const CollisionObjectType& collision_object_type) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); + return (new btSphereShape(static_cast(geom->radius))); +} + +btCollisionShape* createShapePrimitive(const shapes::Cylinder* geom, const CollisionObjectType& collision_object_type) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); + btScalar r = static_cast(geom->radius); + btScalar l = static_cast(geom->length / 2); + return (new btCylinderShapeZ(btVector3(r, r, l))); +} + +btCollisionShape* createShapePrimitive(const shapes::Cone* geom, const CollisionObjectType& collision_object_type) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); + btScalar r = static_cast(geom->radius); + btScalar l = static_cast(geom->length); + return (new btConeShapeZ(r, l)); +} + +btCollisionShape* createShapePrimitive(const shapes::Mesh* geom, const CollisionObjectType& collision_object_type, + CollisionObjectWrapper* cow) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE || + collision_object_type == CollisionObjectType::CONVEX_HULL || + collision_object_type == CollisionObjectType::SDF); + + if (geom->vertex_count > 0 && geom->triangle_count > 0) + { + // convert the mesh to the assigned collision object type + switch (collision_object_type) + { + case CollisionObjectType::CONVEX_HULL: + { + // Create a convex hull shape to approximate Trimesh + collision_detection_bullet::AlignedVector input; + collision_detection_bullet::AlignedVector vertices; + std::vector faces; + + input.reserve(geom->vertex_count); + for (unsigned int i = 0; i < geom->vertex_count; ++i) + input.push_back(Eigen::Vector3d(geom->vertices[3 * i], geom->vertices[3 * i + 1], geom->vertices[3 * i + 2])); + + if (collision_detection_bullet::createConvexHull(vertices, faces, input) < 0) + return nullptr; + + btConvexHullShape* subshape = new btConvexHullShape(); + for (const Eigen::Vector3d& v : vertices) + subshape->addPoint( + btVector3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2]))); + + return subshape; + } + case CollisionObjectType::USE_SHAPE_TYPE: + { + btCompoundShape* compound = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(geom->triangle_count)); + compound->setMargin( + BULLET_MARGIN); // margin: compound seems to have no effect when positive but has an effect when negative + + for (unsigned i = 0; i < geom->triangle_count; ++i) + { + btVector3 v[3]; + for (unsigned x = 0; x < 3; ++x) + { + unsigned idx = geom->triangles[3 * i + x]; + for (unsigned y = 0; y < 3; ++y) + { + v[x][y] = static_cast(geom->vertices[3 * idx + y]); + } + } + + btCollisionShape* subshape = new btTriangleShapeEx(v[0], v[1], v[2]); + if (subshape != nullptr) + { + cow->manage(subshape); + subshape->setMargin(BULLET_MARGIN); + btTransform geom_trans; + geom_trans.setIdentity(); + compound->addChildShape(geom_trans, subshape); + } + } + + return compound; + } + default: + { + ROS_ERROR("This bullet shape type (%d) is not supported for geometry meshs", + static_cast(collision_object_type)); + return nullptr; + } + } + } + ROS_ERROR("The mesh is empty!"); + return nullptr; +} + +btCollisionShape* createShapePrimitive(const shapes::OcTree* geom, const CollisionObjectType& collision_object_type, + CollisionObjectWrapper* cow) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE || + collision_object_type == CollisionObjectType::CONVEX_HULL || + collision_object_type == CollisionObjectType::SDF || + collision_object_type == CollisionObjectType::MULTI_SPHERE); + + btCompoundShape* subshape = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(geom->octree->size())); + double occupancy_threshold = geom->octree->getOccupancyThres(); + + // convert the mesh to the assigned collision object type + switch (collision_object_type) + { + case CollisionObjectType::USE_SHAPE_TYPE: + { + for (auto it = geom->octree->begin(static_cast(geom->octree->getTreeDepth())), + end = geom->octree->end(); + it != end; ++it) + { + if (it->getOccupancy() >= occupancy_threshold) + { + double size = it.getSize(); + btTransform geom_trans; + geom_trans.setIdentity(); + geom_trans.setOrigin(btVector3(static_cast(it.getX()), static_cast(it.getY()), + static_cast(it.getZ()))); + btScalar l = static_cast(size / 2); + btBoxShape* childshape = new btBoxShape(btVector3(l, l, l)); + childshape->setMargin(BULLET_MARGIN); + cow->manage(childshape); + + subshape->addChildShape(geom_trans, childshape); + } + } + return subshape; + } + case CollisionObjectType::MULTI_SPHERE: + { + for (auto it = geom->octree->begin(static_cast(geom->octree->getTreeDepth())), + end = geom->octree->end(); + it != end; ++it) + { + if (it->getOccupancy() >= occupancy_threshold) + { + double size = it.getSize(); + btTransform geom_trans; + geom_trans.setIdentity(); + geom_trans.setOrigin(btVector3(static_cast(it.getX()), static_cast(it.getY()), + static_cast(it.getZ()))); + btSphereShape* childshape = + new btSphereShape(static_cast(std::sqrt(2 * ((size / 2) * (size / 2))))); + childshape->setMargin(BULLET_MARGIN); + cow->manage(childshape); + + subshape->addChildShape(geom_trans, childshape); + } + } + return subshape; + } + default: + { + ROS_ERROR("This bullet shape type (%d) is not supported for geometry octree", + static_cast(collision_object_type)); + return nullptr; + } + } +} + +btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom, + const CollisionObjectType& collision_object_type, CollisionObjectWrapper* cow) +{ + switch (geom->type) + { + case shapes::BOX: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type); + } + case shapes::SPHERE: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type); + } + case shapes::CYLINDER: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type); + } + case shapes::CONE: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type); + } + case shapes::MESH: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type, cow); + } + case shapes::OCTREE: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type, cow); + } + default: + { + ROS_ERROR("This geometric shape type (%d) is not supported using BULLET yet", static_cast(geom->type)); + return nullptr; + } + } +} + +CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types) + : m_name(name) + , m_type_id(type_id) + , m_shapes(shapes) + , m_shape_poses(shape_poses) + , m_collision_object_types(collision_object_types) +{ + this->setContactProcessingThreshold(0); + assert(!shapes.empty()); + assert(!shape_poses.empty()); + assert(!collision_object_types.empty()); + assert(!name.empty()); + assert(shapes.size() == shape_poses.size()); + assert(shapes.size() == collision_object_types.size()); + + m_collision_filter_group = btBroadphaseProxy::KinematicFilter; + m_collision_filter_mask = btBroadphaseProxy::StaticFilter | btBroadphaseProxy::KinematicFilter; + + if (shapes.size() == 1) + { + btCollisionShape* shape = createShapePrimitive(m_shapes[0], collision_object_types[0], this); + shape->setMargin(BULLET_MARGIN); + manage(shape); + setCollisionShape(shape); + setWorldTransform(convertEigenToBt(m_shape_poses[0])); + } + else + { + btCompoundShape* compound = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(m_shapes.size())); + manage(compound); + // margin on compound seems to have no effect when positive but has an effect when negative + compound->setMargin(BULLET_MARGIN); + setCollisionShape(compound); + + setWorldTransform(convertEigenToBt(m_shape_poses[0])); + Eigen::Isometry3d inv_world = m_shape_poses[0].inverse(); + + for (std::size_t j = 0; j < m_shapes.size(); ++j) + { + btCollisionShape* subshape = createShapePrimitive(m_shapes[j], collision_object_types[j], this); + if (subshape != nullptr) + { + manage(subshape); + subshape->setMargin(BULLET_MARGIN); + btTransform geom_trans = convertEigenToBt(inv_world * m_shape_poses[j]); + compound->addChildShape(geom_trans, subshape); + } + } + } +} + +CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::set& touch_links) + : CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types) +{ + m_touch_links = touch_links; +} + +CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::vector>& data) + : m_name(name) + , m_type_id(type_id) + , m_shapes(shapes) + , m_shape_poses(shape_poses) + , m_collision_object_types(collision_object_types) + , m_data(data) +{ + this->setContactProcessingThreshold(0); +} +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/collision_detector_bt_plugin_loader.cpp b/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp similarity index 95% rename from moveit_core/collision_detection_bullet/src/collision_detector_bt_plugin_loader.cpp rename to moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp index 461ea927e3..8279cf6d0d 100644 --- a/moveit_core/collision_detection_bullet/src/collision_detector_bt_plugin_loader.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp @@ -34,14 +34,14 @@ /* Author: Jens Petit */ -#include +#include #include namespace collision_detection { bool CollisionDetectorBtPluginLoader::initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const { - scene->setActiveCollisionDetector(CollisionDetectorAllocatorBt::create(), exclusive); + scene->setActiveCollisionDetector(CollisionDetectorAllocatorBullet::create(), exclusive); return true; } } diff --git a/moveit_core/collision_detection_bullet/src/collision_robot_bt.cpp b/moveit_core/collision_detection_bullet/src/collision_robot_bt.cpp deleted file mode 100644 index 95755456fe..0000000000 --- a/moveit_core/collision_detection_bullet/src/collision_robot_bt.cpp +++ /dev/null @@ -1,143 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Jens Petit - * 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 the copyright holder 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. - *********************************************************************/ - -/* Author: Jens Petit */ - -#include - -namespace collision_detection -{ -CollisionRobotBt::CollisionRobotBt(const robot_model::RobotModelConstPtr& model, double padding, double scale) - : CollisionRobot(model, padding, scale) -{ -} - -CollisionRobotBt::CollisionRobotBt(const CollisionRobotBt& other) : CollisionRobot(other) -{ -} - -void CollisionRobotBt::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const -{ - checkSelfCollisionHelper(req, res, state, nullptr); -} - -void CollisionRobotBt::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const -{ - checkSelfCollisionHelper(req, res, state, &acm); -} - -void CollisionRobotBt::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); -} - -void CollisionRobotBt::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); -} - -void CollisionRobotBt::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet self collision checking not yet implemented"); -} - -void CollisionRobotBt::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - checkOtherCollisionHelper(req, res, state, other_robot, other_state, nullptr); -} - -void CollisionRobotBt::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const -{ - checkOtherCollisionHelper(req, res, state, other_robot, other_state, &acm); -} - -void CollisionRobotBt::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); -} - -void CollisionRobotBt::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); -} - -void CollisionRobotBt::checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix* acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Other collision not implemented yet."); -} - -void CollisionRobotBt::updatedPaddingOrScaling(const std::vector& links) -{ -} - -void CollisionRobotBt::distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Collision distance to self not implemented yet."); -} - -void CollisionRobotBt::distanceOther(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Collision distance to other not implemented yet."); -} - -} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/collision_robot_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_robot_bullet.cpp new file mode 100644 index 0000000000..43a2a2683f --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/collision_robot_bullet.cpp @@ -0,0 +1,267 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include + +namespace collision_detection +{ +CollisionRobotBullet::CollisionRobotBullet(const robot_model::RobotModelConstPtr& model, double padding, double scale) + : CollisionRobot(model, padding, scale), manager_(new collision_detection_bullet::BulletDiscreteBVHManager) +{ + auto fun = std::bind(&collision_detection_bullet::allowedCollisionCheck, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3); + + manager_->setIsContactAllowedFn(fun); + + for (const std::pair& link : robot_model_->getURDF()->links_) + { + addLinkAsCollisionObjectWrapper(link.second); + } + + std::vector active; + collision_detection_bullet::getActiveLinkNamesRecursive(active, robot_model_->getURDF()->getRoot(), true); + manager_->setActiveCollisionObjects(active); +} + +CollisionRobotBullet::CollisionRobotBullet(const CollisionRobotBullet& other) + : CollisionRobot(other), manager_(other.manager_->clone()) +{ +} + +void CollisionRobotBullet::addAttachedOjects( + const robot_state::RobotState& state, + std::vector& cows) const +{ + std::vector attached_bodies; + state.getAttachedBodies(attached_bodies); + for (const robot_state::AttachedBody*& body : attached_bodies) + { + const EigenSTL::vector_Isometry3d& attached_body_transform = body->getGlobalCollisionBodyTransforms(); + std::vector collision_object_types( + attached_body_transform.size(), collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + + cows.emplace_back(collision_detection_bullet::createCollisionObject( + body->getName(), collision_detection::BodyType::ROBOT_ATTACHED, body->getShapes(), attached_body_transform, + collision_object_types, body->getTouchLinks(), true)); + } +} + +void CollisionRobotBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const +{ + checkSelfCollisionHelper(req, res, state, nullptr); +} + +void CollisionRobotBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + checkSelfCollisionHelper(req, res, state, &acm); +} + +void CollisionRobotBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet self continuous collision checking not yet implemented"); +} + +void CollisionRobotBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet self continuous collision checking not yet implemented"); +} + +void CollisionRobotBullet::checkSelfCollisionCCDHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix* acm) const +{ + // TODO: Not in bullet yet +} + +void CollisionRobotBullet::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + std::vector cows; + addAttachedOjects(state, cows); + collision_detection_bullet::BulletDiscreteBVHManagerPtr discrete_clone_manager = manager_->clone(); + updateTransformsFromState(state, discrete_clone_manager); + discrete_clone_manager->contactTest(res, req, acm, cows); +} + +void CollisionRobotBullet::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state) const +{ + checkOtherCollisionHelper(req, res, state, other_robot, other_state, nullptr); +} + +void CollisionRobotBullet::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state, + const AllowedCollisionMatrix& acm) const +{ + checkOtherCollisionHelper(req, res, state, other_robot, other_state, &acm); +} + +void CollisionRobotBullet::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state1, + const robot_state::RobotState& other_state2) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet other robot continuous collision checking not yet implemented"); +} + +void CollisionRobotBullet::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state1, + const robot_state::RobotState& other_state2, + const AllowedCollisionMatrix& acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet other robot continuous collision checking not yet implemented"); +} + +void CollisionRobotBullet::checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const CollisionRobot& other_robot, + const robot_state::RobotState& other_state, + const AllowedCollisionMatrix* acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Other collision not implemented yet."); +} + +void CollisionRobotBullet::updatedPaddingOrScaling(const std::vector& links) +{ + for (const std::string& link : links) + { + if (robot_model_->getURDF()->links_.find(link) != robot_model_->getURDF()->links_.end()) + { + addLinkAsCollisionObjectWrapper(robot_model_->getURDF()->links_[link]); + } + else + { + ROS_ERROR_NAMED("collision_detection.bullet", "Updating padding or scaling for unknown link: '%s'", link.c_str()); + } + } +} + +void CollisionRobotBullet::distanceSelf(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Collision distance to self not implemented yet."); +} + +void CollisionRobotBullet::distanceOther(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state, const CollisionRobot& other_robot, + const robot_state::RobotState& other_state) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Collision distance to other not implemented yet."); +} + +void CollisionRobotBullet::updateTransformsFromState( + const robot_state::RobotState& state, const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const +{ + // updating link positions with the current robot state + for (const std::pair& link : + manager->getCollisionObjects()) + { + // select the first of the transformations for each link (composed of multiple shapes...) + manager->setCollisionObjectsTransform(link.first, state.getCollisionBodyTransform(link.first, 0)); + } +} + +void CollisionRobotBullet::addLinkAsCollisionObjectWrapper(const urdf::LinkSharedPtr& link) +{ + if (!link->collision_array.empty()) + { + const std::vector& col_array = + link->collision_array.empty() ? std::vector(1, link->collision) : + link->collision_array; + + std::vector shapes; + collision_detection_bullet::AlignedVector shape_poses; + std::vector collision_object_types; + + for (const auto& i : col_array) + { + if (i && i->geometry) + { + shapes::ShapePtr shape = collision_detection_bullet::constructShape(i->geometry.get()); + + if (shape) + { + if (fabs(getLinkScale(link->name) - 1.0) >= std::numeric_limits::epsilon() || + fabs(getLinkPadding(link->name)) >= std::numeric_limits::epsilon()) + { + shape->scaleAndPadd(getLinkScale(link->name), getLinkPadding(link->name)); + } + + shapes.push_back(shape); + shape_poses.push_back(collision_detection_bullet::urdfPose2Eigen(i->origin)); + + if (shape->type == shapes::MESH) + { + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); + } + else + { + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + } + } + } + } + + collision_detection_bullet::CollisionObjectWrapperPtr cow = collision_detection_bullet::createCollisionObject( + link->name, collision_detection::BodyType::ROBOT_LINK, shapes, shape_poses, collision_object_types, true); + + if (manager_->hasCollisionObject(link->name)) + { + manager_->removeCollisionObject(link->name); + } + manager_->addCollisionObject(cow); + } +} + +} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/collision_world_bt.cpp b/moveit_core/collision_detection_bullet/src/collision_world_bt.cpp deleted file mode 100644 index 530c2d8d00..0000000000 --- a/moveit_core/collision_detection_bullet/src/collision_world_bt.cpp +++ /dev/null @@ -1,156 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Jens Petit - * 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 the copyright holder 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. - *********************************************************************/ - -/* Author: Jens Petit */ - -#include -#include - -#include - -namespace collision_detection -{ -const std::string CollisionDetectorAllocatorBt::NAME("Bullet"); - -CollisionWorldBt::CollisionWorldBt() : CollisionWorld() -{ - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBt::notifyObjectChange, this, _1, _2)); -} - -CollisionWorldBt::CollisionWorldBt(const WorldPtr& world) : CollisionWorld(world) -{ - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBt::notifyObjectChange, this, _1, _2)); - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -CollisionWorldBt::CollisionWorldBt(const CollisionWorldBt& other, const WorldPtr& world) : CollisionWorld(other, world) -{ - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBt::notifyObjectChange, this, _1, _2)); -} - -CollisionWorldBt::~CollisionWorldBt() -{ - getWorld()->removeObserver(observer_handle_); -} - -void CollisionWorldBt::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state) const -{ - checkRobotCollisionHelper(req, res, robot, state, nullptr); -} - -void CollisionWorldBt::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - checkRobotCollisionHelper(req, res, robot, state, &acm); -} - -void CollisionWorldBt::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); -} - -void CollisionWorldBt::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet continuous collision checking not yet implemented"); -} - -void CollisionWorldBt::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet robot world collision checking not yet implemented"); -} - -void CollisionWorldBt::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const -{ - checkWorldCollisionHelper(req, res, other_world, nullptr); -} - -void CollisionWorldBt::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world, const AllowedCollisionMatrix& acm) const -{ - checkWorldCollisionHelper(req, res, other_world, &acm); -} - -void CollisionWorldBt::checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world, - const AllowedCollisionMatrix* acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet checking with other world not implemented yet."); -} - -void CollisionWorldBt::setWorld(const WorldPtr& world) -{ - if (world == getWorld()) - return; - - // turn off notifications about old world - getWorld()->removeObserver(observer_handle_); - - CollisionWorld::setWorld(world); - - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBt::notifyObjectChange, this, _1, _2)); - - // get notifications any objects already in the new world - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -void CollisionWorldBt::notifyObjectChange(const ObjectConstPtr& obj, World::Action action) -{ -} - -void CollisionWorldBt::distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet checking with other world not implemented yet."); -} - -void CollisionWorldBt::distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet checking with other world not implemented yet."); -} - -} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp new file mode 100644 index 0000000000..9b5d164681 --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp @@ -0,0 +1,247 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include +#include +#include + +namespace collision_detection +{ +const std::string CollisionDetectorAllocatorBullet::NAME("Bullet"); + +CollisionWorldBullet::CollisionWorldBullet() + : CollisionWorld(), manager_(new collision_detection_bullet::BulletDiscreteBVHManager) +{ + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); + + auto fun = std::bind(&collision_detection_bullet::allowedCollisionCheck, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3); + manager_->setIsContactAllowedFn(fun); +} + +CollisionWorldBullet::CollisionWorldBullet(const WorldPtr& world) + : CollisionWorld(world), manager_(new collision_detection_bullet::BulletDiscreteBVHManager) +{ + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); + + auto fun = std::bind(&collision_detection_bullet::allowedCollisionCheck, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3); + manager_->setIsContactAllowedFn(fun); +} + +CollisionWorldBullet::CollisionWorldBullet(const CollisionWorldBullet& other, const WorldPtr& world) + : CollisionWorld(other, world) +{ + manager_ = other.manager_->clone(); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); +} + +CollisionWorldBullet::~CollisionWorldBullet() +{ + getWorld()->removeObserver(observer_handle_); +} + +void CollisionWorldBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state) const +{ + checkRobotCollisionHelper(req, res, robot, state, nullptr); +} + +void CollisionWorldBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + checkRobotCollisionHelper(req, res, robot, state, &acm); +} + +void CollisionWorldBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const +{ + checkRobotCollisionHelperCCD(req, res, robot, state1, state2, nullptr); +} + +void CollisionWorldBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const +{ + checkRobotCollisionHelperCCD(req, res, robot, state1, state2, &acm); +} + +void CollisionWorldBullet::checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix* acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Continuous collision checking not implemented yet"); +} + +void CollisionWorldBullet::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const CollisionRobot& robot, const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + const CollisionRobotBullet& robot_bt = dynamic_cast(robot); + + collision_detection_bullet::BulletDiscreteBVHManagerPtr discrete_clone_manager = manager_->clone(); + std::vector attached_cows; + robot_bt.addAttachedOjects(state, attached_cows); + + for (const std::pair& cow : + robot_bt.manager_->getCollisionObjects()) + { + collision_detection_bullet::CollisionObjectWrapperPtr new_cow = cow.second->clone(); + discrete_clone_manager->addCollisionObject(new_cow); + discrete_clone_manager->setCollisionObjectsTransform(new_cow->getName(), + state.getCollisionBodyTransform(new_cow->getName(), 0)); + } + + discrete_clone_manager->setActiveCollisionObjects(robot_bt.manager_->getActiveCollisionObjects()); + + discrete_clone_manager->contactTest(res, req, acm, attached_cows); +} + +void CollisionWorldBullet::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionWorld& other_world) const +{ + checkWorldCollisionHelper(req, res, other_world, nullptr); +} + +void CollisionWorldBullet::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, + const CollisionWorld& other_world, + const AllowedCollisionMatrix& acm) const +{ + checkWorldCollisionHelper(req, res, other_world, &acm); +} + +void CollisionWorldBullet::checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const CollisionWorld& other_world, + const AllowedCollisionMatrix* acm) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet checking with other world not implemented yet."); +} + +void CollisionWorldBullet::addToManager(const World::Object* obj) +{ + std::vector collision_object_types; + + for (const shapes::ShapeConstPtr& shape : obj->shapes_) + { + if (shape->type == shapes::MESH) + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); + else + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + } + + manager_->addCollisionObject(obj->id_, collision_detection::BodyType::WORLD_OBJECT, obj->shapes_, obj->shape_poses_, + collision_object_types, true); +} + +void CollisionWorldBullet::updateManagedObject(const std::string& id) +{ + if (getWorld()->hasObject(id)) + { + auto it = getWorld()->find(id); + if (manager_->hasCollisionObject(id)) + { + manager_->removeCollisionObject(id); + addToManager(it->second.get()); + } + else + { + addToManager(it->second.get()); + } + } + else + { + if (manager_->hasCollisionObject(id)) + { + manager_->removeCollisionObject(id); + } + } +} + +void CollisionWorldBullet::setWorld(const WorldPtr& world) +{ + if (world == getWorld()) + return; + + // turn off notifications about old world + getWorld()->removeObserver(observer_handle_); + + CollisionWorld::setWorld(world); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); + + // get notifications any objects already in the new world + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +void CollisionWorldBullet::notifyObjectChange(const ObjectConstPtr& obj, World::Action action) +{ + if (action == World::DESTROY) + { + manager_->removeCollisionObject(obj->id_); + } + else + { + updateManagedObject(obj->id_); + } +} + +void CollisionWorldBullet::distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, + const robot_state::RobotState& state) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet distance calculation not implemented yet."); +} + +void CollisionWorldBullet::distanceWorld(const DistanceRequest& req, DistanceResult& res, + const CollisionWorld& world) const +{ + ROS_ERROR_NAMED("collision_detection.bullet", "Bullet distance calculation not implemented yet."); +} + +} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_common.h b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection.cpp similarity index 81% rename from moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_common.h rename to moveit_core/collision_detection_bullet/test/test_bullet_collision_detection.cpp index ac0a27aa40..3f22bab5f0 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_common.h +++ b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection.cpp @@ -34,15 +34,14 @@ /* Author: Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_BT_COLLISION_COMMON_ -#define MOVEIT_COLLISION_DETECTION_BT_COLLISION_COMMON_ +#include +#include -#include -#include +INSTANTIATE_TYPED_TEST_CASE_P(BulletCollisionCheck, CollisionDetectorTest, + collision_detection::CollisionDetectorAllocatorBullet); -namespace collision_detection +int main(int argc, char* argv[]) { -// TODO: Add common functionality for all Bullet collision checkers. + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } - -#endif diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp new file mode 100644 index 0000000000..339cd15c3b --- /dev/null +++ b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp @@ -0,0 +1,47 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +INSTANTIATE_TYPED_TEST_CASE_P(BulletCollisionCheckPanda, CollisionDetectorPandaTest, + collision_detection::CollisionDetectorAllocatorBullet); + +int main(int argc, char* argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index 939f958d32..fbeefa76fd 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -30,6 +30,6 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection.cpp) target_link_libraries(test_fcl_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) - catkin_add_gtest(test_fcl_collision_env test/test_fcl_env.cpp) - target_link_libraries(test_fcl_collision_env moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + catkin_add_gtest(test_fcl_collision_detection_panda test/test_fcl_collision_detection_panda.cpp) + target_link_libraries(test_fcl_collision_detection_panda moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) endif() diff --git a/moveit_core/collision_detection_bullet/src/collision_common.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp similarity index 81% rename from moveit_core/collision_detection_bullet/src/collision_common.cpp rename to moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp index ecae7a988f..b83b267171 100644 --- a/moveit_core/collision_detection_bullet/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp @@ -34,10 +34,14 @@ /* Author: Jens Petit */ -#include -#include +#include +#include -namespace collision_detection +INSTANTIATE_TYPED_TEST_CASE_P(FCLCollisionCheckPanda, CollisionDetectorPandaTest, + collision_detection::CollisionDetectorAllocatorFCL); + +int main(int argc, char* argv[]) { -// TODO: Add common functionality for all collision detection here. -} // namespace collision_detection + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/collision_detector_bt_description.xml b/moveit_core/collision_detector_bullet_description.xml similarity index 100% rename from moveit_core/collision_detector_bt_description.xml rename to moveit_core/collision_detector_bullet_description.xml diff --git a/moveit_core/package.xml b/moveit_core/package.xml index f0b2e0774f..2e540b1fc3 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -22,6 +22,7 @@ assimp boost eigen + bullet eigen_stl_containers libfcl-dev geometric_shapes @@ -56,6 +57,6 @@ - + diff --git a/moveit_ros/planning/launch/collision_checker_compare.launch b/moveit_ros/planning/launch/collision_checker_compare.launch new file mode 100644 index 0000000000..5c9526b876 --- /dev/null +++ b/moveit_ros/planning/launch/collision_checker_compare.launch @@ -0,0 +1,10 @@ + + + + + + + + + + From 7297e06e6e5f19e678f541b9708cffbfd0369d77 Mon Sep 17 00:00:00 2001 From: Jens P Date: Tue, 20 Aug 2019 03:44:05 +0200 Subject: [PATCH 150/612] Adding continuous collision detection to Bullet (#1551) * Adding continous collision detection: * check only active links added * benchmark FCL vs Bullet * renaming files from bt to bullet * renaming variables from bt to bullet * clang-tidy and clang-format * renaming of variables to be in moveit format * continuous tests added * CCD active links changed * distance testing added to panda test suite * New collision features: * broadphase early culling * minimal distance reporting * renaming collision filter and group * removed extra margin on AABB for bullet * ACM in test from SRDF * PR review --- .../test_collision_common_panda.h | 67 ++- .../collision_detection_bullet/CMakeLists.txt | 5 +- .../bullet_cast_bvh_manager.h | 217 ++++++++ .../bullet_discrete_bvh_manager.h | 3 + .../bullet_integration/bullet_utils.h | 495 +++++++++++++++++- .../contact_checker_common.h | 25 +- .../collision_robot_bullet.h | 3 + .../collision_world_bullet.h | 4 + .../bullet_cast_bvh_manager.cpp | 410 +++++++++++++++ .../bullet_discrete_bvh_manager.cpp | 7 +- .../src/bullet_integration/bullet_utils.cpp | 4 +- .../src/collision_robot_bullet.cpp | 12 +- .../src/collision_world_bullet.cpp | 58 +- ...t_bullet_continuous_collision_checking.cpp | 401 ++++++++++++++ .../planning_components_tools/CMakeLists.txt | 3 + ...re_collision_speed_checking_fcl_bullet.cpp | 355 +++++++++++++ 16 files changed, 2009 insertions(+), 60 deletions(-) create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h create mode 100644 moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp create mode 100644 moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp create mode 100644 moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index a57733f3dd..02c3ac73bf 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -79,21 +79,15 @@ class CollisionDetectorPandaTest : public testing::Test robot_model_ = moveit::core::loadTestingRobotModel("panda"); robot_model_ok_ = static_cast(robot_model_); - acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false)); - - acm_->setEntry("panda_link0", "panda_link1", true); - acm_->setEntry("panda_link1", "panda_link2", true); - acm_->setEntry("panda_link2", "panda_link3", true); - acm_->setEntry("panda_link3", "panda_link4", true); - acm_->setEntry("panda_link4", "panda_link5", true); - acm_->setEntry("panda_link5", "panda_link6", true); - acm_->setEntry("panda_link6", "panda_link7", true); - acm_->setEntry("panda_link7", "panda_hand", true); - acm_->setEntry("panda_hand", "panda_rightfinger", true); - acm_->setEntry("panda_hand", "panda_leftfinger", true); - acm_->setEntry("panda_rightfinger", "panda_leftfinger", true); - acm_->setEntry("panda_link5", "panda_link7", true); - acm_->setEntry("panda_link6", "panda_hand", true); + acm_.reset(new collision_detection::AllowedCollisionMatrix()); + // Use default collision operations in the SRDF to setup the acm + const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); + acm_->setEntry(collision_links, collision_links, false); + + // allow collisions for pairs that have been disabled + const std::vector& dc = robot_model_->getSRDF()->getDisabledCollisionPairs(); + for (const srdf::Model::DisabledCollision& it : dc) + acm_->setEntry(it.link1_, it.link2_, true); crobot_ = value_->allocateRobot(robot_model_); cworld_ = value_->allocateWorld(collision_detection::WorldPtr(new collision_detection::World())); @@ -138,8 +132,11 @@ TYPED_TEST_P(CollisionDetectorPandaTest, DefaultNotInCollision) /** \brief A configuration where the robot should collide with itself. */ TYPED_TEST_P(CollisionDetectorPandaTest, LinksInCollision) { - // Sets the joint values to zero which is a colliding configuration - this->robot_state_->setToDefaultValues(); + // Sets the joints into a colliding configuration + double joint2 = 0.15; + double joint4 = -3.0; + this->robot_state_->setJointPositions("panda_joint2", &joint2); + this->robot_state_->setJointPositions("panda_joint4", &joint4); this->robot_state_->update(); collision_detection::CollisionRequest req; @@ -254,5 +251,39 @@ TYPED_TEST_P(CollisionDetectorPandaTest, PaddingTest) ASSERT_FALSE(res.collision); } +/** \brief Tests the distance reporting with the robot itself */ +TYPED_TEST_P(CollisionDetectorPandaTest, DistanceSelf) +{ + collision_detection::CollisionRequest req; + req.distance = true; + collision_detection::CollisionResult res; + this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + EXPECT_NEAR(res.distance, 0.13, 0.01); +} + +TYPED_TEST_P(CollisionDetectorPandaTest, DistanceWorld) +{ + collision_detection::CollisionRequest req; + req.distance = true; + collision_detection::CollisionResult res; + + // Adding the box right in front of the robot hand + shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + pos.translation().x() = 0.43; + pos.translation().y() = 0; + pos.translation().z() = 0.55; + this->cworld_->getWorld()->addToObject("box", shape_ptr, pos); + + this->crobot_->setLinkPadding("panda_hand", 0.0); + this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + EXPECT_NEAR(res.distance, 0.029, 0.01); +} + REGISTER_TYPED_TEST_CASE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision, - DISABLED_WorldToWorldCollision, RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest); + DISABLED_WorldToWorldCollision, RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, + DistanceSelf, DistanceWorld); diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index 6e1ba5526f..d50c5a9ed4 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -5,6 +5,7 @@ add_library(${MOVEIT_LIB_NAME} src/collision_world_bullet.cpp src/bullet_integration/bullet_utils.cpp src/bullet_integration/bullet_discrete_bvh_manager.cpp + src/bullet_integration/bullet_cast_bvh_manager.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -18,7 +19,6 @@ add_library(collision_detector_bullet_plugin src/collision_detector_bullet_plugi set_target_properties(collision_detector_bullet_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(collision_detector_bullet_plugin ${catkin_LIBRARIES} ${MOVEIT_LIB_NAME}) - install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_bullet_plugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) @@ -36,4 +36,7 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_bullet_collision_detection_panda test/test_bullet_collision_detection_panda.cpp) target_link_libraries(test_bullet_collision_detection_panda moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + + catkin_add_gtest(test_bullet_continuous_collision_checking test/test_bullet_continuous_collision_checking.cpp) + target_link_libraries(test_bullet_continuous_collision_checking moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) endif() diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h new file mode 100644 index 0000000000..40085ff76c --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h @@ -0,0 +1,217 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * 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. + * + * 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. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#ifndef TESSERACT_COLLISION_BULLET_CAST_BVH_MANAGERS_H +#define TESSERACT_COLLISION_BULLET_CAST_BVH_MANAGERS_H + +#include +#include + +namespace collision_detection_bullet +{ +MOVEIT_CLASS_FORWARD(BulletCastBVHManager) + +/** @brief A bounding volume hierarchy (BVH) implementation of a tesseract contact manager */ +class BulletCastBVHManager +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Constructor */ + BulletCastBVHManager(); + + ~BulletCastBVHManager(); + + /** @brief Clone the manager + * + * This is to be used for multi threaded applications. A user should make a clone for each thread. */ + BulletCastBVHManagerPtr clone() const; + + /**@brief Add a collision object to the checker + * + * All objects are added should initially be added as static objects. Use the setContactRequest method of defining + * which collision objects are moving. + * + * @param name The name of the object, must be unique. + * @param mask_id User defined id which gets stored in the results structure. + * @param shapes A vector of shapes that make up the collision object. + * @param shape_poses A vector of poses for each shape, must be same length as shapes + * @param shape_types A vector of shape types for encode the collision object. If the vector is of length 1 it is + * used for all shapes. + * @param collision_object_types A int identifying a conversion mode for the object. (ex. convert meshes to convex + * hulls) + * @param enabled Indicate if the object is enabled for collision checking. + * @return true if successfully added, otherwise false. */ + bool addCollisionObject(const std::string& name, const collision_detection::BodyType& mask_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, bool enabled = true); + + /**@brief Find if a collision object already exists + * @param name The name of the collision object + * @return true if it exists, otherwise false. */ + bool hasCollisionObject(const std::string& name) const; + + /**@brief Remove an object from the checker + * @param name The name of the object + * @return true if successfully removed, otherwise false. */ + bool removeCollisionObject(const std::string& name); + + /**@brief Enable an object + * @param name The name of the object + * @return true if successfully enabled, otherwise false. */ + bool enableCollisionObject(const std::string& name); + + /**@brief Disable an object + * @param name The name of the object + * @return true if successfully disabled, otherwise false. */ + bool disableCollisionObject(const std::string& name); + + /**@brief Set a single static collision object's tansform + * @param name The name of the object + * @param pose The tranformation in world */ + void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose); + + /**@brief Set a series of static collision objects' tranforms + * @param names The names of the objects + * @param poses The tranformations in world */ + void setCollisionObjectsTransform(const std::vector& names, + const AlignedVector& poses); + + /**@brief Set a series of static collision objects' tranforms + * @param transforms A transform map */ + void setCollisionObjectsTransform(const AlignedMap& transforms); + + /**@brief Set a single cast (moving) collision object's tansforms + * + * This should only be used for moving objects. + * + * @param name The name of the object + * @param pose1 The start tranformation in world + * @param pose2 The end tranformation in world */ + void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose1, + const Eigen::Isometry3d& pose2); + + /**@brief Set a series of cast (moving) collision objects' tranforms + * + * This should only be used for moving objects. Use the base class methods for static objects. + * + * @param names The name of the object + * @param pose1 The start tranformations in world + * @param pose2 The end tranformations in world */ + void setCollisionObjectsTransform(const std::vector& names, + const AlignedVector& pose1, + const AlignedVector& pose2); + + /**@brief Set a series of cast (moving) collision objects' tranforms + * + * This should only be used for moving objects. Use the base class methods for static objects. + * + * @param pose1 A start transform map + * @param pose2 A end transform map */ + void setCollisionObjectsTransform(const AlignedMap& pose1, + const AlignedMap& pose2); + + /**@brief Set which collision objects are active + * @param names A vector of collision object names */ + void setActiveCollisionObjects(const std::vector& names); + + /**@brief Get which collision objects are active + * @return A vector of collision object names */ + const std::vector& getActiveCollisionObjects() const; + + /**@brief Set the contact distance threshold for which collision should be considered through expanding the AABB by + * the contact_distance for all links. + * + * @param contact_distance The contact distance */ + void setContactDistanceThreshold(double contact_distance); + + /**@brief Get the contact distance threshold + * @return The contact distance */ + double getContactDistanceThreshold() const; + + /** @brief Set the function for determining if two links are allowed to be in collision */ + void setIsContactAllowedFn(IsContactAllowedFn fn); + + /** @brief Get the function for determining if two links are allowed to be in collision */ + IsContactAllowedFn getIsContactAllowedFn() const; + + /**@brief Perform a contact test for all objects + * @param collisions The Contact results data + * @param req The collision request data + * @param acm The allowed collision matrix */ + void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm); + + /**@brief Add a tesseract collision object to the manager + * @param cow The tesseract bullet collision object */ + void addCollisionObject(const CollisionObjectWrapperPtr& cow); + + /** \brief Callback function for culling objects before a broadphase check */ + BroadphaseFilterCallback filter_callback_; + +private: + /** @brief A list of the active collision objects */ + std::vector active_; + + /** @brief The contact distance threshold */ + double contact_distance_; + + /** @brief The is allowed collision function */ + IsContactAllowedFn fn_; + + /** @brief The bullet collision dispatcher used for getting object to object collison algorithm */ + std::unique_ptr dispatcher_; + + /** @brief The bullet collision dispatcher configuration information */ + btDispatcherInfo dispatch_info_; + + /** @brief The bullet collision configuration */ + btDefaultCollisionConfiguration coll_config_; + + /** @brief The bullet broadphase interface */ + std::unique_ptr broadphase_; + + /** @brief A map of collision objects being managed */ + std::map link2cow_; + + /** @brief A map of cast collision objects being managed. */ + std::map link2castcow_; + + /**@brief Perform a contact test for the provided object which is not part of the manager + * @param cow The Collision object + * @param collisions The collision results */ + void contactTest(const CollisionObjectWrapperPtr& cow, ContactTestData& collisions); +}; +} // namespace collision_detection_bullet +#endif // TESSERACT_COLLISION_BULLET_CAST_BVH_MANAGERS_H diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h index 8067a160de..b9e8e91e96 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h @@ -157,6 +157,9 @@ class BulletDiscreteBVHManager * @return A map of collision objects */ const std::map& getCollisionObjects() const; + /** \brief Callback function for culling objects before a broadphase check */ + BroadphaseFilterCallback filter_callback_; + private: /** @brief A list of the active collision objects */ std::vector active_; diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h index d6462ebefe..c2a604837e 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h @@ -119,10 +119,10 @@ class CollisionObjectWrapper : public btCollisionObject const std::set& touch_links); /** \brief Bitfield specifies to which group the object belongs */ - short int m_collision_filter_group; + short int m_collisionFilterGroup; /** \brief Bitfield specifies against which other groups the object is collision checked */ - short int m_collision_filter_mask; + short int m_collisionFilterMask; /** \brief Indicates if the collision object is used for a collision check */ bool m_enabled; @@ -160,7 +160,9 @@ class CollisionObjectWrapper : public btCollisionObject { getCollisionShape()->getAabb(getWorldTransform(), aabb_min, aabb_max); const btScalar& d = getContactProcessingThreshold(); - btVector3 contact_threshold(d, d, d); + // added as bullet expands each AABB by 4 cm + double dis = d; // - 0.04; + btVector3 contact_threshold(dis, dis, dis); aabb_min -= contact_threshold; aabb_max += contact_threshold; } @@ -173,8 +175,8 @@ class CollisionObjectWrapper : public btCollisionObject new CollisionObjectWrapper(m_name, m_type_id, m_shapes, m_shape_poses, m_collision_object_types, m_data)); clone_cow->setCollisionShape(getCollisionShape()); clone_cow->setWorldTransform(getWorldTransform()); - clone_cow->m_collision_filter_group = m_collision_filter_group; - clone_cow->m_collision_filter_mask = m_collision_filter_mask; + clone_cow->m_collisionFilterGroup = m_collisionFilterGroup; + clone_cow->m_collisionFilterMask = m_collisionFilterMask; clone_cow->m_enabled = m_enabled; clone_cow->setBroadphaseHandle(nullptr); clone_cow->m_touch_links = m_touch_links; @@ -220,6 +222,106 @@ class CollisionObjectWrapper : public btCollisionObject std::vector> m_data; }; +/** @brief Casted collision shape used for checking if an object is collision free between two discrete poses + * + * The cast is not explicitely computed but implicitely represented through the single shape and the transformation + * between the first and second pose. */ +struct CastHullShape : public btConvexShape +{ +public: + /** \brief The casted shape */ + btConvexShape* m_shape; + + /** \brief Transformation from the first pose to the second pose */ + btTransform m_shape_transform; + + CastHullShape(btConvexShape* shape, const btTransform& t01) : m_shape(shape), m_shape_transform(t01) + { + m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE; + } + + void updateCastTransform(const btTransform& t01) + { + m_shape_transform = t01; + } + + /** \brief From both shape poses computes the support vertex and returns the larger one. */ + btVector3 localGetSupportingVertex(const btVector3& vec) const override + { + btVector3 sv0 = m_shape->localGetSupportingVertex(vec); + btVector3 sv1 = m_shape_transform * m_shape->localGetSupportingVertex(vec * m_shape_transform.getBasis()); + return (vec.dot(sv0) > vec.dot(sv1)) ? sv0 : sv1; + } + + void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* /*vectors*/, + btVector3* /*supportVerticesOut*/, + int /*numVectors*/) const override + { + throw std::runtime_error("not implemented"); + } + + /** \brief Shape specific fast recalculation of the AABB at a certain pose + * + * The AABB is not recalculated from scratch but updated depending on the given transformation. */ + void getAabb(const btTransform& t_w0, btVector3& aabbMin, btVector3& aabbMax) const override + { + m_shape->getAabb(t_w0, aabbMin, aabbMax); + btVector3 min1, max1; + m_shape->getAabb(t_w0 * m_shape_transform, min1, max1); + aabbMin.setMin(min1); + aabbMax.setMax(max1); + } + + void getAabbSlow(const btTransform& /*t*/, btVector3& /*aabbMin*/, btVector3& /*aabbMax*/) const override + { + throw std::runtime_error("shouldn't happen"); + } + + void setLocalScaling(const btVector3& /*scaling*/) override + { + } + + const btVector3& getLocalScaling() const override + { + static btVector3 out(1, 1, 1); + return out; + } + + void setMargin(btScalar /*margin*/) override + { + } + + btScalar getMargin() const override + { + return 0; + } + + int getNumPreferredPenetrationDirections() const override + { + return 0; + } + + void getPreferredPenetrationDirection(int /*index*/, btVector3& /*penetrationVector*/) const override + { + throw std::runtime_error("not implemented"); + } + + void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const override + { + throw std::runtime_error("not implemented"); + } + + const char* getName() const override + { + return "CastHull"; + } + + btVector3 localGetSupportingVertexWithoutMargin(const btVector3& v) const override + { + return localGetSupportingVertex(v); + } +}; + /** \brief Computes the local supporting vertex of a convex shape. * * If multiple vertices with equal support products exists, their average is calculated and returned. @@ -287,8 +389,8 @@ inline bool needsCollisionCheck(const CollisionObjectWrapper& cow1, const Collis if (!cow2.m_enabled) return false; - if (!((cow2.m_collision_filter_group & cow1.m_collision_filter_mask) && - (cow1.m_collision_filter_group & cow2.m_collision_filter_mask))) + if ((!(cow2.m_collisionFilterGroup & cow1.m_collisionFilterMask) || + !(cow1.m_collisionFilterGroup & cow2.m_collisionFilterMask))) return false; if (isContactAllowed(cow1.getName(), cow2.getName(), allowed_fn, acm, verbose)) @@ -345,7 +447,107 @@ inline btScalar addDiscreteSingleResult(btManifoldPoint& cp, const btCollisionOb return 1; } -/** \brief Processes a contact point */ +inline btScalar addCastSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*index0*/, + const btCollisionObjectWrapper* colObj1Wrap, int /*index1*/, + ContactTestData& collisions) +{ + assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); + assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); + + const CollisionObjectWrapper* cd0 = static_cast(colObj0Wrap->getCollisionObject()); + const CollisionObjectWrapper* cd1 = static_cast(colObj1Wrap->getCollisionObject()); + + std::pair pc = getObjectPairKey(cd0->getName(), cd1->getName()); + + const auto& it = collisions.res.contacts.find(pc); + bool found = (it != collisions.res.contacts.end()); + + collision_detection::Contact contact; + contact.body_name_1 = cd0->getName(); + contact.body_name_2 = cd1->getName(); + contact.depth = static_cast(cp.m_distance1); + contact.normal = convertBtToEigen(-1 * cp.m_normalWorldOnB); + contact.pos = convertBtToEigen(cp.m_positionWorldOnA); + + contact.body_type_1 = cd0->getTypeID(); + contact.body_type_2 = cd0->getTypeID(); + + collision_detection::Contact* col = processResult(collisions, contact, pc, found); + + if (!col) + { + return 0; + } + + assert(!((cd0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) && + (cd1->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter))); + + bool cast_shape_is_first = cd0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter; + + btVector3 normal_world_from_cast = (cast_shape_is_first ? -1 : 1) * cp.m_normalWorldOnB; + const btCollisionObjectWrapper* first_col_obj_wrap = (cast_shape_is_first ? colObj0Wrap : colObj1Wrap); + + // we want the contact information of the non-casted object come first, therefore swap values + if (cast_shape_is_first) + { + std::swap(col->nearest_points[0], col->nearest_points[1]); + contact.pos = convertBtToEigen(cp.m_positionWorldOnB); + std::swap(col->body_name_1, col->body_name_2); + std::swap(col->body_type_1, col->body_type_2); + col->normal *= -1; + } + + btTransform tf_world0, tf_world1; + const CastHullShape* shape = static_cast(first_col_obj_wrap->getCollisionShape()); + + tf_world0 = first_col_obj_wrap->getWorldTransform(); + tf_world1 = first_col_obj_wrap->getWorldTransform() * shape->m_shape_transform; + + // transform normals into pose local coordinate systems + btVector3 normal_local0 = normal_world_from_cast * tf_world0.getBasis(); + btVector3 normal_local1 = normal_world_from_cast * tf_world1.getBasis(); + + btVector3 pt_local0; + float localsup0; + getAverageSupport(shape->m_shape, normal_local0, localsup0, pt_local0); + btVector3 pt_world0 = tf_world0 * pt_local0; + btVector3 pt_local1; + float localsup1; + getAverageSupport(shape->m_shape, normal_local1, localsup1, pt_local1); + btVector3 pt_world1 = tf_world1 * pt_local1; + + float sup0 = normal_world_from_cast.dot(pt_world0); + float sup1 = normal_world_from_cast.dot(pt_world1); + + // TODO: this section is potentially problematic. think hard about the math + if (sup0 - sup1 > BULLET_SUPPORT_FUNC_TOLERANCE) + { + col->percent_interpolation = 0; + } + else if (sup1 - sup0 > BULLET_SUPPORT_FUNC_TOLERANCE) + { + col->percent_interpolation = 1; + } + else + { + const btVector3& pt_on_cast = cast_shape_is_first ? cp.m_positionWorldOnA : cp.m_positionWorldOnB; + float l0c = (pt_on_cast - pt_world0).length(); + float l1c = (pt_on_cast - pt_world1).length(); + + if (l0c + l1c < BULLET_LENGTH_TOLERANCE) + { + col->percent_interpolation = .5; + } + else + { + col->percent_interpolation = static_cast(l0c / (l0c + l1c)); + } + } + + return 1; +} + +/** @brief This is copied directly out of BulletWorld */ struct TesseractBridgedManifoldResult : public btManifoldResult { btCollisionWorld::ContactResultCallback& m_resultCallback; @@ -401,7 +603,10 @@ struct TesseractBridgedManifoldResult : public btManifoldResult } }; -/** @brief Abstract interface for both discrete and continuous reporting of contact points */ +/** @brief Abstract interface for both discrete and continuous broadphase collision pair + * + * /e needsCollision is the callback executed before a narrowphase check is executed. + * /e addSingleResult is the callback executed after the narrowphase check delivers a result. */ struct BroadphaseContactResultCallback { ContactTestData& collisions_; @@ -447,7 +652,28 @@ struct DiscreteBroadphaseContactResultCallback : public BroadphaseContactResultC } }; -/** \brief Processes a contact point */ +/** \brief addSingleResult of this struct is called each time the broadphase check indicates a collision. */ +struct CastBroadphaseContactResultCallback : public BroadphaseContactResultCallback +{ + CastBroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false) + : BroadphaseContactResultCallback(collisions, contact_distance, verbose) + { + } + + btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, + int index0, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, + int index1) override + { + if (cp.m_distance1 > static_cast(contact_distance_)) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not close enough for collision with " << cp.m_distance1); + return 0; + } + + return addCastSingleResult(cp, colObj0Wrap, index0, colObj1Wrap, index1, collisions_); + } +}; + struct TesseractBroadphaseBridgedManifoldResult : public btManifoldResult { BroadphaseContactResultCallback& result_callback_; @@ -593,10 +819,11 @@ class TesseractCollisionPairCallback : public btOverlapCallback const CollisionObjectWrapper* cow1 = static_cast(pair.m_pProxy1->m_clientObject); std::pair pair_names{ cow0->getName(), cow1->getName() }; - ROS_DEBUG_STREAM("Processing " << cow0->getName() << " vs " << cow1->getName()); if (results_callback_.needsCollision(cow0, cow1)) { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Processing " << cow0->getName() << " vs " + << cow1->getName()); btCollisionObjectWrapper obj0_wrap(nullptr, cow0->getCollisionShape(), cow0, cow0->getWorldTransform(), -1, -1); btCollisionObjectWrapper obj1_wrap(nullptr, cow1->getCollisionShape(), cow1, cow1->getWorldTransform(), -1, -1); @@ -616,6 +843,11 @@ class TesseractCollisionPairCallback : public btOverlapCallback pair.m_algorithm->processCollision(&obj0_wrap, &obj1_wrap, dispatch_info_, &contact_point_result); } } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not processing " << cow0->getName() << " vs " + << cow1->getName()); + } return false; } }; @@ -635,28 +867,25 @@ btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom, inline void updateCollisionObjectFilters(const std::vector& active, CollisionObjectWrapper& cow, bool continuous) { - cow.m_collision_filter_group = btBroadphaseProxy::KinematicFilter; - if (!isLinkActive(active, cow.getName())) { - cow.m_collision_filter_group = btBroadphaseProxy::StaticFilter; - } - - if (cow.m_collision_filter_group == btBroadphaseProxy::StaticFilter) - { - cow.m_collision_filter_mask = btBroadphaseProxy::KinematicFilter; + cow.m_collisionFilterGroup = btBroadphaseProxy::StaticFilter; + cow.m_collisionFilterMask = btBroadphaseProxy::KinematicFilter; } else { - (continuous) ? (cow.m_collision_filter_mask = btBroadphaseProxy::StaticFilter) : - (cow.m_collision_filter_mask = btBroadphaseProxy::StaticFilter | btBroadphaseProxy::KinematicFilter); + cow.m_collisionFilterGroup = btBroadphaseProxy::KinematicFilter; + cow.m_collisionFilterMask = btBroadphaseProxy::StaticFilter; } if (cow.getBroadphaseHandle()) { - cow.getBroadphaseHandle()->m_collisionFilterGroup = cow.m_collision_filter_group; - cow.getBroadphaseHandle()->m_collisionFilterMask = cow.m_collision_filter_mask; + cow.getBroadphaseHandle()->m_collisionFilterGroup = cow.m_collisionFilterGroup; + cow.getBroadphaseHandle()->m_collisionFilterMask = cow.m_collisionFilterMask; } + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "COW " << cow.getName() << " group " + << cow.m_collisionFilterGroup << " mask " + << cow.m_collisionFilterMask); } /** \brief Wrapper around constructing a CollisionObjectWrapper */ @@ -723,8 +952,8 @@ struct DiscreteCollisionCollector : public btCollisionWorld::ContactResultCallba : collisions_(collisions), cow_(cow), contact_distance_(contact_distance), verbose_(verbose) { m_closestDistanceThreshold = static_cast(contact_distance); - m_collisionFilterGroup = cow->m_collision_filter_group; - m_collisionFilterMask = cow->m_collision_filter_mask; + m_collisionFilterGroup = cow->m_collisionFilterGroup; + m_collisionFilterMask = cow->m_collisionFilterMask; } btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, @@ -748,6 +977,137 @@ struct DiscreteCollisionCollector : public btCollisionWorld::ContactResultCallba } }; +struct CastCollisionCollector : public btCollisionWorld::ContactResultCallback +{ + ContactTestData& collisions_; + const CollisionObjectWrapperPtr cow_; + double contact_distance_; + bool verbose_; + + CastCollisionCollector(ContactTestData& collisions, const CollisionObjectWrapperPtr& cow, double contact_distance, + bool verbose = false) + : collisions_(collisions), cow_(cow), contact_distance_(contact_distance), verbose_(verbose) + { + m_closestDistanceThreshold = static_cast(contact_distance); + m_collisionFilterGroup = cow->m_collisionFilterGroup; + m_collisionFilterMask = cow->m_collisionFilterMask; + } + + btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, + int index0, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, + int index1) override + { + if (cp.m_distance1 > static_cast(contact_distance_)) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not close enough for collision with " << cp.m_distance1); + return 0; + } + + return addCastSingleResult(cp, colObj0Wrap, index0, colObj1Wrap, index1, collisions_); + } + + bool needsCollision(btBroadphaseProxy* proxy0) const override + { + return !collisions_.done && + needsCollisionCheck(*cow_, *(static_cast(proxy0->m_clientObject)), collisions_.fn, + collisions_.acm, verbose_); + } +}; + +inline CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr& cow) +{ + CollisionObjectWrapperPtr new_cow = cow->clone(); + + btTransform tf; + tf.setIdentity(); + + if (btBroadphaseProxy::isConvex(new_cow->getCollisionShape()->getShapeType())) + { + assert(dynamic_cast(new_cow->getCollisionShape()) != nullptr); + btConvexShape* convex = static_cast(new_cow->getCollisionShape()); + + // This checks if the collision object is already a cast collision object + assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); + + CastHullShape* shape = new CastHullShape(convex, tf); + + new_cow->manage(shape); + new_cow->setCollisionShape(shape); + } + else if (btBroadphaseProxy::isCompound(new_cow->getCollisionShape()->getShapeType())) + { + btCompoundShape* compound = static_cast(new_cow->getCollisionShape()); + btCompoundShape* new_compound = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, compound->getNumChildShapes()); + + for (int i = 0; i < compound->getNumChildShapes(); ++i) + { + if (btBroadphaseProxy::isConvex(compound->getChildShape(i)->getShapeType())) + { + btConvexShape* convex = static_cast(compound->getChildShape(i)); + assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); // This checks if already a cast collision object + + btTransform geom_trans = compound->getChildTransform(i); + + btCollisionShape* subshape = new CastHullShape(convex, tf); + + new_cow->manage(subshape); + subshape->setMargin(BULLET_MARGIN); + new_compound->addChildShape(geom_trans, subshape); + } + else if (btBroadphaseProxy::isCompound(compound->getChildShape(i)->getShapeType())) + { + btCompoundShape* second_compound = static_cast(compound->getChildShape(i)); + btCompoundShape* new_second_compound = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, second_compound->getNumChildShapes()); + for (int j = 0; j < second_compound->getNumChildShapes(); ++j) + { + assert(!btBroadphaseProxy::isCompound(second_compound->getChildShape(j)->getShapeType())); + + btConvexShape* convex = static_cast(second_compound->getChildShape(j)); + assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); // This checks if already a cast collision object + + btTransform geom_trans = second_compound->getChildTransform(j); + + btCollisionShape* subshape = new CastHullShape(convex, tf); + + new_cow->manage(subshape); + subshape->setMargin(BULLET_MARGIN); + new_second_compound->addChildShape(geom_trans, subshape); + } + + btTransform geom_trans = compound->getChildTransform(i); + + new_cow->manage(new_second_compound); + + // margin on compound seems to have no effect when positive but has an effect when negative + new_second_compound->setMargin(BULLET_MARGIN); + new_compound->addChildShape(geom_trans, new_second_compound); + } + else + { + ROS_ERROR_NAMED("collision_detection.bullet", + "I can only collision check convex shapes and compound shapes made of convex shapes"); + throw std::runtime_error("I can only collision check convex shapes and compound shapes made of convex shapes"); + } + } + + // margin on compound seems to have no effect when positive but has an effect when negative + new_compound->setMargin(BULLET_MARGIN); + new_cow->manage(new_compound); + new_cow->setCollisionShape(new_compound); + new_cow->setWorldTransform(cow->getWorldTransform()); + } + else + { + ROS_ERROR_NAMED("collision_detection.bullet", + "I can only collision check convex shapes and compound shapes made of convex shapes"); + throw std::runtime_error("I can only collision check convex shapes and compound shapes made of convex shapes"); + } + + return new_cow; +} + /** @brief Update the Broadphase AABB for the input collision object * @param cow The collision objects * @param broadphase The bullet broadphase interface @@ -796,8 +1156,89 @@ inline void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr& cow, // Add the active collision object to the broadphase int type = cow->getCollisionShape()->getShapeType(); - cow->setBroadphaseHandle(broadphase->createProxy(aabb_min, aabb_max, type, cow.get(), cow->m_collision_filter_group, - cow->m_collision_filter_mask, dispatcher.get())); + cow->setBroadphaseHandle(broadphase->createProxy(aabb_min, aabb_max, type, cow.get(), cow->m_collisionFilterGroup, + cow->m_collisionFilterMask, dispatcher.get())); } + +struct BroadphaseFilterCallback : public btOverlapFilterCallback +{ + virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override + { + bool cull = !(proxy0->m_collisionFilterMask & proxy1->m_collisionFilterGroup); + cull = cull || !(proxy1->m_collisionFilterMask & proxy0->m_collisionFilterGroup); + + if (cull) + return false; + + const CollisionObjectWrapper* cow0 = static_cast(proxy0->m_clientObject); + const CollisionObjectWrapper* cow1 = static_cast(proxy1->m_clientObject); + + if (!cow0->m_enabled) + return false; + + if (!cow1->m_enabled) + return false; + + if (acm_ && acmCheck(cow0->getName(), cow1->getName())) + return false; + + if (cow0->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && + cow1->getTypeID() == collision_detection::BodyType::ROBOT_LINK) + if (cow0->m_touch_links.find(cow1->getName()) != cow0->m_touch_links.end()) + return false; + + if (cow1->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && + cow0->getTypeID() == collision_detection::BodyType::ROBOT_LINK) + if (cow1->m_touch_links.find(cow0->getName()) == cow1->m_touch_links.end()) + return false; + + // TODO: Add check for two objects attached to the same link + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Broadphase pass " << cow0->getName() << " vs " + << cow1->getName()); + + return true; + } + + bool acmCheck(const std::string& body_1, const std::string& body_2) const + { + collision_detection::AllowedCollision::Type allowed_type; + + if (!acm_) + { + if (acm_->getEntry(body_1, body_2, allowed_type)) + { + if (allowed_type == collision_detection::AllowedCollision::Type::NEVER) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Not allowed entry in ACM found, collision check between " << body_1 << " and " + << body_2); + return false; + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Entry in ACM found, skipping collision check as allowed " << body_1 << " and " + << body_2); + return true; + } + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No entry in ACM found, collision check between " + << body_1 << " and " << body_2); + return false; + } + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No ACM, collision check between " << body_1 << " and " + << body_2); + return false; + } + } + + const collision_detection::AllowedCollisionMatrix* acm_{ nullptr }; +}; + } // namespace collision_detection_bullet #endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BULLET_UTILS_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h index b4929e6dfb..4acf787851 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h @@ -102,15 +102,26 @@ inline collision_detection::Contact* processResult(ContactTestData& cdata, colli cdata.res.distance = contact.depth; } } + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Contact btw " << key.first << " and " << key.second << " dist: " << contact.depth); + // case if pair hasn't a contact yet if (!found) { - cdata.res.collision = true; + if (contact.depth <= 0) + { + cdata.res.collision = true; + } + std::vector data; + + // if we dont want contacts we are done here if (!cdata.req.contacts) { - cdata.done = true; + if (!cdata.req.distance) + { + cdata.done = true; + } return nullptr; } else @@ -122,7 +133,10 @@ inline collision_detection::Contact* processResult(ContactTestData& cdata, colli if (cdata.res.contact_count >= cdata.req.max_contacts) { - cdata.done = true; + if (!cdata.req.distance) + { + cdata.done = true; + } } if (cdata.req.max_contacts_per_pair == 1u) @@ -145,7 +159,10 @@ inline collision_detection::Contact* processResult(ContactTestData& cdata, colli if (cdata.res.contact_count >= cdata.req.max_contacts) { - cdata.done = true; + if (!cdata.req.distance) + { + cdata.done = true; + } } return &(dr.back()); diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h index de04d45100..9299a0511d 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h @@ -113,6 +113,9 @@ class CollisionRobotBullet : public CollisionRobot /** \brief Handles all self collision checks */ collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_; + + /** \brief Active links corresponding to all links of the robot */ + std::vector active_; }; } // namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h index ea8473d3dc..06b35a1e04 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h @@ -40,6 +40,7 @@ #include #include #include +#include #include namespace collision_detection @@ -103,6 +104,9 @@ class CollisionWorldBullet : public CollisionWorld /** \brief Handles all discrete collision checks */ collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_; + /** \brief Handles all continuous collision checks */ + collision_detection_bullet::BulletCastBVHManagerPtr bt_manager_CCD_; + private: /** \brief Callback function executed for each change to the world environment */ void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp new file mode 100644 index 0000000000..83da7d1c3d --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp @@ -0,0 +1,410 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * 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. + * + * 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. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#include +#include +#include + +namespace collision_detection_bullet +{ +BulletCastBVHManager::BulletCastBVHManager() +{ + dispatcher_.reset(new btCollisionDispatcher(&coll_config_)); + + dispatcher_->registerCollisionCreateFunc( + BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE, + coll_config_.getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE, CONVEX_SHAPE_PROXYTYPE)); + + dispatcher_->setDispatcherFlags(dispatcher_->getDispatcherFlags() & + ~btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD); + + broadphase_.reset(new btDbvtBroadphase()); + + broadphase_->getOverlappingPairCache()->setOverlapFilterCallback(&filter_callback_); + + contact_distance_ = 0; +} + +BulletCastBVHManager::~BulletCastBVHManager() +{ + // clean up remaining objects + for (const std::pair& cow : link2cow_) + removeCollisionObjectFromBroadphase(cow.second, broadphase_, dispatcher_); + + // clean up remaining objects + for (const std::pair& cow : link2castcow_) + removeCollisionObjectFromBroadphase(cow.second, broadphase_, dispatcher_); +} + +BulletCastBVHManagerPtr BulletCastBVHManager::clone() const +{ + BulletCastBVHManagerPtr manager(new BulletCastBVHManager()); + + for (const std::pair& cow : link2cow_) + { + CollisionObjectWrapperPtr new_cow = cow.second->clone(); + + assert(new_cow->getCollisionShape()); + assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); + + new_cow->setWorldTransform(cow.second->getWorldTransform()); + new_cow->setContactProcessingThreshold(static_cast(contact_distance_)); + manager->addCollisionObject(new_cow); + } + + manager->setActiveCollisionObjects(active_); + manager->setContactDistanceThreshold(contact_distance_); + manager->setIsContactAllowedFn(fn_); + + return manager; +} + +bool BulletCastBVHManager::addCollisionObject(const std::string& name, const collision_detection::BodyType& mask_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + bool enabled) +{ + CollisionObjectWrapperPtr new_cow = + createCollisionObject(name, mask_id, shapes, shape_poses, collision_object_types, enabled); + if (new_cow != nullptr) + { + addCollisionObject(new_cow); + return true; + } + + return false; +} + +bool BulletCastBVHManager::hasCollisionObject(const std::string& name) const +{ + return (link2cow_.find(name) != link2cow_.end()); +} + +bool BulletCastBVHManager::removeCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + CollisionObjectWrapperPtr& cow1 = it->second; + removeCollisionObjectFromBroadphase(cow1, broadphase_, dispatcher_); + link2cow_.erase(name); + + CollisionObjectWrapperPtr& cow2 = link2castcow_[name]; + removeCollisionObjectFromBroadphase(cow2, broadphase_, dispatcher_); + link2castcow_.erase(name); + + return true; + } + + return false; +} + +bool BulletCastBVHManager::enableCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + it->second->m_enabled = true; + link2castcow_[name]->m_enabled = true; + return true; + } + + return false; +} + +bool BulletCastBVHManager::disableCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + it->second->m_enabled = false; + link2castcow_[name]->m_enabled = false; + return true; + } + + return false; +} + +void BulletCastBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) +{ + // TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with + // geometry + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + CollisionObjectWrapperPtr& cow = it->second; + btTransform tf = convertEigenToBt(pose); + cow->setWorldTransform(tf); + link2castcow_[name]->setWorldTransform(tf); + + // Now update Broadphase AABB (See BulletWorld updateSingleAabb function) + if (cow->getBroadphaseHandle()) + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } +} + +void BulletCastBVHManager::setCollisionObjectsTransform(const std::vector& names, + const AlignedVector& poses) +{ + assert(names.size() == poses.size()); + for (size_t i = 0u; i < names.size(); ++i) + { + setCollisionObjectsTransform(names[i], poses[i]); + } +} + +void BulletCastBVHManager::setCollisionObjectsTransform(const AlignedMap& transforms) +{ + for (const std::pair& transform : transforms) + { + setCollisionObjectsTransform(transform.first, transform.second); + } +} + +void BulletCastBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose1, + const Eigen::Isometry3d& pose2) +{ + // TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with + // geometry + auto it = link2castcow_.find(name); + if (it != link2castcow_.end()) + { + CollisionObjectWrapperPtr& cow = it->second; + assert(cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter); + + btTransform tf1 = convertEigenToBt(pose1); + btTransform tf2 = convertEigenToBt(pose2); + + cow->setWorldTransform(tf1); + link2cow_[name]->setWorldTransform(tf1); + + // If collision object is disabled dont proceed + if (cow->m_enabled) + { + if (btBroadphaseProxy::isConvex(cow->getCollisionShape()->getShapeType())) + { + static_cast(cow->getCollisionShape())->updateCastTransform(tf1.inverseTimes(tf2)); + } + else if (btBroadphaseProxy::isCompound(cow->getCollisionShape()->getShapeType())) + { + btCompoundShape* compound = static_cast(cow->getCollisionShape()); + + for (int i = 0; i < compound->getNumChildShapes(); ++i) + { + if (btBroadphaseProxy::isConvex(compound->getChildShape(i)->getShapeType())) + { + const btTransform& local_tf = compound->getChildTransform(i); + + btTransform delta_tf = (tf1 * local_tf).inverseTimes(tf2 * local_tf); + static_cast(compound->getChildShape(i))->updateCastTransform(delta_tf); + compound->updateChildTransform(i, local_tf, false); // This is required to update the BVH tree + } + else if (btBroadphaseProxy::isCompound(compound->getChildShape(i)->getShapeType())) + { + btCompoundShape* second_compound = static_cast(compound->getChildShape(i)); + + for (int j = 0; j < second_compound->getNumChildShapes(); ++j) + { + assert(!btBroadphaseProxy::isCompound(second_compound->getChildShape(j)->getShapeType())); + const btTransform& local_tf = second_compound->getChildTransform(j); + + btTransform delta_tf = (tf1 * local_tf).inverseTimes(tf2 * local_tf); + static_cast(second_compound->getChildShape(j))->updateCastTransform(delta_tf); + second_compound->updateChildTransform(j, local_tf, false); // This is required to update the BVH tree + } + second_compound->recalculateLocalAabb(); + } + } + compound->recalculateLocalAabb(); + } + else + { + ROS_ERROR_NAMED( + "collision_detection.bullet", + "I can only continuous collision check convex shapes and compound shapes made of convex shapes"); + throw std::runtime_error( + "I can only continuous collision check convex shapes and compound shapes made of convex shapes"); + } + + // Now update Broadphase AABB (See BulletWorld updateSingleAabb function) + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } + } +} + +void BulletCastBVHManager::setCollisionObjectsTransform(const std::vector& names, + const AlignedVector& pose1, + const AlignedVector& pose2) +{ + if (names.size() != pose1.size() || names.size() != pose2.size()) + { + ROS_ERROR_NAMED("collision_detection.bullet", "The number of poses does not fit the number of collision objects."); + } + + for (size_t i = 0u; i < names.size(); ++i) + setCollisionObjectsTransform(names[i], pose1[i], pose2[i]); +} + +void BulletCastBVHManager::setCollisionObjectsTransform(const AlignedMap& pose1, + const AlignedMap& pose2) +{ + assert(pose1.size() == pose2.size()); + auto it1 = pose1.begin(); + auto it2 = pose2.begin(); + while (it1 != pose1.end()) + { + assert(pose1.find(it1->first) != pose2.end()); + setCollisionObjectsTransform(it1->first, it1->second, it2->second); + std::advance(it1, 1); + std::advance(it2, 1); + } +} + +void BulletCastBVHManager::setActiveCollisionObjects(const std::vector& names) +{ + active_ = names; + + // Now need to update the broadphase with correct aabb + for (std::pair& co : link2cow_) + { + CollisionObjectWrapperPtr& cow = co.second; + + // Need to check if a collision object is still active, select cast or normal to broadphase + if (isLinkActive(active_, cow->getName())) + { + updateCollisionObjectFilters(active_, *cow, false); + CollisionObjectWrapperPtr& active_cow = link2castcow_[cow->getName()]; + updateCollisionObjectFilters(active_, *active_cow, true); + + removeCollisionObjectFromBroadphase(active_cow, broadphase_, dispatcher_); + removeCollisionObjectFromBroadphase(cow, broadphase_, dispatcher_); + addCollisionObjectToBroadphase(active_cow, broadphase_, dispatcher_); + } + else + { + updateCollisionObjectFilters(active_, *cow, false); + CollisionObjectWrapperPtr& cast_cow = link2castcow_[cow->getName()]; + updateCollisionObjectFilters(active_, *cast_cow, true); + + removeCollisionObjectFromBroadphase(cast_cow, broadphase_, dispatcher_); + removeCollisionObjectFromBroadphase(cow, broadphase_, dispatcher_); + addCollisionObjectToBroadphase(cow, broadphase_, dispatcher_); + } + } +} + +const std::vector& BulletCastBVHManager::getActiveCollisionObjects() const +{ + return active_; +} + +void BulletCastBVHManager::setContactDistanceThreshold(double contact_distance) +{ + contact_distance_ = contact_distance; + + for (std::pair& co : link2cow_) + { + CollisionObjectWrapperPtr& cow = co.second; + cow->setContactProcessingThreshold(static_cast(contact_distance)); + if (cow->getBroadphaseHandle()) + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } + + for (std::pair& co : link2castcow_) + { + CollisionObjectWrapperPtr& cow = co.second; + cow->setContactProcessingThreshold(static_cast(contact_distance)); + if (cow->getBroadphaseHandle()) + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } +} + +double BulletCastBVHManager::getContactDistanceThreshold() const +{ + return contact_distance_; +} + +void BulletCastBVHManager::setIsContactAllowedFn(IsContactAllowedFn fn) +{ + fn_ = std::move(fn); +} + +IsContactAllowedFn BulletCastBVHManager::getIsContactAllowedFn() const +{ + return fn_; +} + +void BulletCastBVHManager::contactTest(collision_detection::CollisionResult& collisions, + const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm) +{ + ContactTestData cdata(active_, contact_distance_, fn_, collisions, req, acm); + broadphase_->calculateOverlappingPairs(dispatcher_.get()); + btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); + + ROS_DEBUG_STREAM_NAMED("collision_detection.bulelt", "Number overlapping candidates " + << pair_cache->getNumOverlappingPairs()); + + CastBroadphaseContactResultCallback cc(cdata, contact_distance_); + TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); + pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get()); +} + +void BulletCastBVHManager::addCollisionObject(const CollisionObjectWrapperPtr& cow) +{ + link2cow_[cow->getName()] = cow; + CollisionObjectWrapperPtr cast_cow = makeCastCollisionObject(cow); + link2castcow_[cast_cow->getName()] = cast_cow; + + const CollisionObjectWrapperPtr& selected_cow = + (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) ? cast_cow : cow; + btVector3 aabb_min, aabb_max; + selected_cow->getAABB(aabb_min, aabb_max); + + int type = selected_cow->getCollisionShape()->getShapeType(); + selected_cow->setBroadphaseHandle(broadphase_->createProxy(aabb_min, aabb_max, type, selected_cow.get(), + selected_cow->m_collisionFilterGroup, + selected_cow->m_collisionFilterMask, dispatcher_.get())); +} + +void BulletCastBVHManager::contactTest(const CollisionObjectWrapperPtr& cow, ContactTestData& collisions) +{ + btVector3 aabb_min, aabb_max; + cow->getAABB(aabb_min, aabb_max); + + CastCollisionCollector cc(collisions, cow, static_cast(cow->getContactProcessingThreshold())); + TesseractSingleContactCallback contact_cb(cow.get(), dispatcher_.get(), dispatch_info_, cc); + broadphase_->aabbTest(aabb_min, aabb_max, contact_cb); +} +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp index b55b3a7c02..73640cf3a8 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp @@ -50,6 +50,8 @@ BulletDiscreteBVHManager::BulletDiscreteBVHManager() broadphase_.reset(new btDbvtBroadphase()); + broadphase_->getOverlappingPairCache()->setOverlapFilterCallback(&filter_callback_); + contact_distance_ = 0; } @@ -78,7 +80,6 @@ BulletDiscreteBVHManagerPtr BulletDiscreteBVHManager::clone() const manager->addCollisionObject(new_cow); } - manager->setActiveCollisionObjects(active_); manager->setContactDistanceThreshold(contact_distance_); manager->setIsContactAllowedFn(fn_); @@ -199,6 +200,10 @@ void BulletDiscreteBVHManager::setActiveCollisionObjects(const std::vector active; - collision_detection_bullet::getActiveLinkNamesRecursive(active, robot_model_->getURDF()->getRoot(), true); - manager_->setActiveCollisionObjects(active); + collision_detection_bullet::getActiveLinkNamesRecursive(active_, robot_model_->getURDF()->getRoot(), true); } CollisionRobotBullet::CollisionRobotBullet(const CollisionRobotBullet& other) @@ -126,6 +124,14 @@ void CollisionRobotBullet::checkSelfCollisionHelper(const CollisionRequest& req, addAttachedOjects(state, cows); collision_detection_bullet::BulletDiscreteBVHManagerPtr discrete_clone_manager = manager_->clone(); updateTransformsFromState(state, discrete_clone_manager); + + if (req.distance) + { + discrete_clone_manager->setContactDistanceThreshold(2); + } + + discrete_clone_manager->filter_callback_.acm_ = acm; + discrete_clone_manager->contactTest(res, req, acm, cows); } diff --git a/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp index 9b5d164681..837fd0706d 100644 --- a/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp @@ -46,7 +46,9 @@ namespace collision_detection const std::string CollisionDetectorAllocatorBullet::NAME("Bullet"); CollisionWorldBullet::CollisionWorldBullet() - : CollisionWorld(), manager_(new collision_detection_bullet::BulletDiscreteBVHManager) + : CollisionWorld() + , manager_(new collision_detection_bullet::BulletDiscreteBVHManager) + , bt_manager_CCD_(new collision_detection_bullet::BulletCastBVHManager) { // request notifications about changes to new world observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); @@ -54,10 +56,13 @@ CollisionWorldBullet::CollisionWorldBullet() auto fun = std::bind(&collision_detection_bullet::allowedCollisionCheck, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); manager_->setIsContactAllowedFn(fun); + bt_manager_CCD_->setIsContactAllowedFn(fun); } CollisionWorldBullet::CollisionWorldBullet(const WorldPtr& world) - : CollisionWorld(world), manager_(new collision_detection_bullet::BulletDiscreteBVHManager) + : CollisionWorld(world) + , manager_(new collision_detection_bullet::BulletDiscreteBVHManager) + , bt_manager_CCD_(new collision_detection_bullet::BulletCastBVHManager) { // request notifications about changes to new world observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); @@ -66,12 +71,14 @@ CollisionWorldBullet::CollisionWorldBullet(const WorldPtr& world) auto fun = std::bind(&collision_detection_bullet::allowedCollisionCheck, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); manager_->setIsContactAllowedFn(fun); + bt_manager_CCD_->setIsContactAllowedFn(fun); } CollisionWorldBullet::CollisionWorldBullet(const CollisionWorldBullet& other, const WorldPtr& world) : CollisionWorld(other, world) { manager_ = other.manager_->clone(); + bt_manager_CCD_ = other.bt_manager_CCD_->clone(); // request notifications about changes to new world observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); @@ -116,7 +123,37 @@ void CollisionWorldBullet::checkRobotCollisionHelperCCD(const CollisionRequest& const robot_state::RobotState& state2, const AllowedCollisionMatrix* acm) const { - ROS_ERROR_NAMED("collision_detection.bullet", "Continuous collision checking not implemented yet"); + const CollisionRobotBullet& robot_bt = dynamic_cast(robot); + + collision_detection_bullet::BulletCastBVHManagerPtr cast_clone_manager = bt_manager_CCD_->clone(); + std::vector attached_cows; + robot_bt.addAttachedOjects(state1, attached_cows); + std::vector active_objects; + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows) + { + cast_clone_manager->addCollisionObject(cow); + cast_clone_manager->setCollisionObjectsTransform( + cow->getName(), state1.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0], + state2.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0]); + active_objects.push_back(cow->getName()); + } + + for (const std::pair& cow : + robot_bt.manager_->getCollisionObjects()) + { + collision_detection_bullet::CollisionObjectWrapperPtr new_cow = cow.second->clone(); + cast_clone_manager->addCollisionObject(new_cow); + cast_clone_manager->setCollisionObjectsTransform(new_cow->getName(), + state1.getCollisionBodyTransform(new_cow->getName(), 0), + state2.getCollisionBodyTransform(new_cow->getName(), 0)); + } + + collision_detection_bullet::getActiveLinkNamesRecursive(active_objects, + robot_bt.getRobotModel()->getURDF()->getRoot(), true); + cast_clone_manager->setActiveCollisionObjects(active_objects); + cast_clone_manager->filter_callback_.acm_ = acm; + cast_clone_manager->contactTest(res, req, acm); } void CollisionWorldBullet::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, @@ -138,7 +175,14 @@ void CollisionWorldBullet::checkRobotCollisionHelper(const CollisionRequest& req state.getCollisionBodyTransform(new_cow->getName(), 0)); } - discrete_clone_manager->setActiveCollisionObjects(robot_bt.manager_->getActiveCollisionObjects()); + discrete_clone_manager->setActiveCollisionObjects(robot_bt.active_); + + if (req.distance) + { + discrete_clone_manager->setContactDistanceThreshold(std::numeric_limits::max()); + } + + discrete_clone_manager->filter_callback_.acm_ = acm; discrete_clone_manager->contactTest(res, req, acm, attached_cows); } @@ -177,6 +221,9 @@ void CollisionWorldBullet::addToManager(const World::Object* obj) manager_->addCollisionObject(obj->id_, collision_detection::BodyType::WORLD_OBJECT, obj->shapes_, obj->shape_poses_, collision_object_types, true); + + bt_manager_CCD_->addCollisionObject(obj->id_, collision_detection::BodyType::WORLD_OBJECT, obj->shapes_, + obj->shape_poses_, collision_object_types, true); } void CollisionWorldBullet::updateManagedObject(const std::string& id) @@ -187,6 +234,7 @@ void CollisionWorldBullet::updateManagedObject(const std::string& id) if (manager_->hasCollisionObject(id)) { manager_->removeCollisionObject(id); + bt_manager_CCD_->removeCollisionObject(id); addToManager(it->second.get()); } else @@ -199,6 +247,7 @@ void CollisionWorldBullet::updateManagedObject(const std::string& id) if (manager_->hasCollisionObject(id)) { manager_->removeCollisionObject(id); + bt_manager_CCD_->removeCollisionObject(id); } } } @@ -225,6 +274,7 @@ void CollisionWorldBullet::notifyObjectChange(const ObjectConstPtr& obj, World:: if (action == World::DESTROY) { manager_->removeCollisionObject(obj->id_); + bt_manager_CCD_->removeCollisionObject(obj->id_); } else { diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp new file mode 100644 index 0000000000..f5b5014c33 --- /dev/null +++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp @@ -0,0 +1,401 @@ +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +/** \brief Brings the panda robot in user defined home position */ +inline void setToHome(robot_state::RobotState& panda_state) +{ + panda_state.setToDefaultValues(); + double joint2 = -0.785; + double joint4 = -2.356; + double joint6 = 1.571; + double joint7 = 0.785; + panda_state.setJointPositions("panda_joint2", &joint2); + panda_state.setJointPositions("panda_joint4", &joint4); + panda_state.setJointPositions("panda_joint6", &joint6); + panda_state.setJointPositions("panda_joint7", &joint7); + panda_state.update(); +} + +class BulletCollisionDetectionTester : public testing::Test +{ +protected: + void SetUp() override + { + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + robot_model_ok_ = static_cast(robot_model_); + + acm_.reset(new collision_detection::AllowedCollisionMatrix()); + // Use default collision operations in the SRDF to setup the acm + const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); + acm_->setEntry(collision_links, collision_links, false); + + // allow collisions for pairs that have been disabled + const std::vector& dc = robot_model_->getSRDF()->getDisabledCollisionPairs(); + for (const srdf::Model::DisabledCollision& it : dc) + acm_->setEntry(it.link1_, it.link2_, true); + + crobot_.reset(new collision_detection::CollisionRobotBullet(robot_model_)); + cworld_.reset(new collision_detection::CollisionWorldBullet()); + + robot_state_.reset(new robot_state::RobotState(robot_model_)); + + setToHome(*robot_state_); + } + + void TearDown() override + { + } + +protected: + bool robot_model_ok_; + + robot_model::RobotModelPtr robot_model_; + + collision_detection::CollisionWorldPtr cworld_; + collision_detection::CollisionRobotPtr crobot_; + + collision_detection::AllowedCollisionMatrixPtr acm_; + + robot_state::RobotStatePtr robot_state_; +}; + +void addCollisionObjects(collision_detection_bullet::BulletCastBVHManager& checker) +{ + //////////////////////////// + // Add static box to checker + //////////////////////////// + shapes::ShapePtr static_box(new shapes::Box(1, 1, 1)); + Eigen::Isometry3d static_box_pose; + static_box_pose.setIdentity(); + + std::vector obj1_shapes; + collision_detection_bullet::AlignedVector obj1_poses; + std::vector obj1_types; + obj1_shapes.push_back(static_box); + obj1_poses.push_back(static_box_pose); + obj1_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + + checker.addCollisionObject("static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, + obj1_types); + + //////////////////////////// + // Add moving box to checker + //////////////////////////// + shapes::ShapePtr moving_box(new shapes::Box(0.2, 0.2, 0.2)); + Eigen::Isometry3d moving_box_pose; + + moving_box_pose.setIdentity(); + moving_box_pose.translation() = Eigen::Vector3d(0.5, -0.5, 0); + + std::vector obj2_shapes; + collision_detection_bullet::AlignedVector obj2_poses; + std::vector obj2_types; + obj2_shapes.push_back(moving_box); + obj2_poses.push_back(moving_box_pose); + obj2_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + + checker.addCollisionObject("moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, + obj2_types); +} + +void addCollisionObjectsMesh(collision_detection_bullet::BulletCastBVHManager& checker) +{ + //////////////////////////// + // Add static box to checker + //////////////////////////// + shapes::ShapePtr static_box(new shapes::Box(0.3, 0.3, 0.3)); + Eigen::Isometry3d static_box_pose; + static_box_pose.setIdentity(); + + std::vector obj1_shapes; + collision_detection_bullet::AlignedVector obj1_poses; + std::vector obj1_types; + obj1_shapes.push_back(static_box); + obj1_poses.push_back(static_box_pose); + obj1_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + + checker.addCollisionObject("static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, + obj1_types); + + //////////////////////////// + // Add moving mesh to checker + //////////////////////////// + + std::vector obj2_shapes; + collision_detection_bullet::AlignedVector obj2_poses; + std::vector obj2_types; + + obj1_poses.push_back(static_box_pose); + obj1_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + + Eigen::Isometry3d s_pose; + s_pose.setIdentity(); + + std::string kinect = "package://moveit_resources/panda_description/meshes/collision/hand.stl"; + shapes::ShapeConstPtr s; + s.reset(shapes::createMeshFromResource(kinect)); + obj2_shapes.push_back(s); + obj2_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); + obj2_poses.push_back(s_pose); + + checker.addCollisionObject("moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, + obj2_types); +} + +void runTest(collision_detection_bullet::BulletCastBVHManager& checker, collision_detection::CollisionResult& result, + std::vector& result_vector, Eigen::Isometry3d& start_pos, + Eigen::Isometry3d& end_pos) +{ + ////////////////////////////////////// + // Test when object is inside another + ////////////////////////////////////// + checker.setActiveCollisionObjects({ "moving_box_link" }); + checker.setContactDistanceThreshold(0.1); + + // Set the collision object transforms + checker.setCollisionObjectsTransform("static_box_link", Eigen::Isometry3d::Identity()); + checker.setCollisionObjectsTransform("moving_box_link", start_pos, end_pos); + + // Perform collision check + collision_detection::CollisionRequest request; + request.contacts = true; + // collision_detection_bullet::ContactResultMap result; + checker.contactTest(result, request, nullptr); + + for (const auto& contacts_all : result.contacts) + { + for (const auto& contact : contacts_all.second) + { + result_vector.push_back(contact); + } + } +} + +// TODO: Add continuous to continuous collision checking +/** \brief Continuous self collision checks are not supported yet by the bullet integration */ +TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + robot_state::RobotState state1(robot_model_); + robot_state::RobotState state2(robot_model_); + + setToHome(state1); + double joint2 = 0.15; + double joint4 = -3.0; + double joint5 = -0.8; + double joint7 = -0.785; + state1.setJointPositions("panda_joint2", &joint2); + state1.setJointPositions("panda_joint4", &joint4); + state1.setJointPositions("panda_joint5", &joint5); + state1.setJointPositions("panda_joint7", &joint7); + state1.update(); + + crobot_->checkSelfCollision(req, res, state1, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + setToHome(state2); + double joint_5_other = 0.8; + state2.setJointPositions("panda_joint2", &joint2); + state2.setJointPositions("panda_joint4", &joint4); + state2.setJointPositions("panda_joint5", &joint_5_other); + state2.setJointPositions("panda_joint7", &joint7); + state2.update(); + + crobot_->checkSelfCollision(req, res, state2, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + crobot_->checkSelfCollision(req, res, state1, state2, *acm_); + ROS_INFO_STREAM("Continous to continous collisions are not supported yet, therefore fail here."); + ASSERT_TRUE(res.collision); + res.clear(); +} + +/** \brief Two similar robot poses are used as start and end pose of a continuous collision check. */ +TEST_F(BulletCollisionDetectionTester, ContinuousCollisionWorld) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 10; + collision_detection::CollisionResult res; + + robot_state::RobotState state1(robot_model_); + robot_state::RobotState state2(robot_model_); + + setToHome(state1); + state1.update(); + + setToHome(state2); + double joint_2{ 0.05 }; + double joint_4{ -1.6 }; + state2.setJointPositions("panda_joint2", &joint_2); + state2.setJointPositions("panda_joint4", &joint_4); + state2.update(); + + cworld_->checkRobotCollision(req, res, *crobot_, state1, state2, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + // Adding the box which is not in collision with the individual states but with the casted one. + shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + pos.translation().x() = 0.43; + pos.translation().y() = 0; + pos.translation().z() = 0.55; + cworld_->getWorld()->addToObject("box", shape_ptr, pos); + + cworld_->checkRobotCollision(req, res, *crobot_, state1, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + cworld_->checkRobotCollision(req, res, *crobot_, state2, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + cworld_->checkRobotCollision(req, res, *crobot_, state1, state2, *acm_); + ASSERT_TRUE(res.collision); + ASSERT_EQ(res.contact_count, 4u); + res.clear(); +} + +TEST(ContinuousCollisionUnit, BulletCastBVHCollisionBoxBoxUnit) +{ + collision_detection::CollisionResult result; + std::vector result_vector; + + Eigen::Isometry3d start_pos, end_pos; + start_pos.setIdentity(); + start_pos.translation().x() = -2; + end_pos.setIdentity(); + end_pos.translation().x() = 2; + + collision_detection_bullet::BulletCastBVHManager checker; + addCollisionObjects(checker); + runTest(checker, result, result_vector, start_pos, end_pos); + + ASSERT_TRUE(result.collision); + EXPECT_NEAR(result_vector[0].depth, -0.6, 0.001); + EXPECT_NEAR(result_vector[0].percent_interpolation, 0.6, 0.001); +} + +TEST(ContinuousCollisionUnit, BulletCastMeshVsBox) +{ + collision_detection_bullet::BulletCastBVHManager checker; + addCollisionObjectsMesh(checker); + + Eigen::Isometry3d start_pos, end_pos; + start_pos.setIdentity(); + start_pos.translation().x() = -1.9; + end_pos.setIdentity(); + end_pos.translation().x() = 1.9; + + collision_detection::CollisionResult result; + std::vector result_vector; + + runTest(checker, result, result_vector, start_pos, end_pos); + + ASSERT_TRUE(result.collision); +} + +TEST(ContinuousCollisionUnit, TwoManagers) +{ + collision_detection_bullet::BulletCastBVHManager checker_continuous; + collision_detection_bullet::BulletDiscreteBVHManager checker_discrete; + + //////////////////////////// + // Add static box to checker + //////////////////////////// + shapes::ShapePtr static_box(new shapes::Box(.1, .1, .1)); + Eigen::Isometry3d static_box_pose; + static_box_pose.setIdentity(); + + std::vector obj1_shapes; + collision_detection_bullet::AlignedVector obj1_poses; + std::vector obj1_types; + obj1_shapes.push_back(static_box); + obj1_poses.push_back(static_box_pose); + obj1_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + + checker_continuous.addCollisionObject("static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, + obj1_poses, obj1_types); + checker_discrete.addCollisionObject("static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, + obj1_poses, obj1_types); + + //////////////////////// + // Add moving sphere to checker + //////////////////////// + shapes::ShapePtr sphere; + sphere.reset( + shapes::createMeshFromResource("package://moveit_resources/panda_description/meshes/collision/link0.stl")); + + Eigen::Isometry3d sphere_pose; + sphere_pose.setIdentity(); + + std::vector obj2_shapes; + collision_detection_bullet::AlignedVector obj2_poses; + std::vector obj2_types; + obj2_shapes.push_back(sphere); + obj2_poses.push_back(sphere_pose); + obj2_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); + + checker_continuous.addCollisionObject("moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, + obj2_poses, obj2_types); + checker_discrete.addCollisionObject("moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, + obj2_poses, obj2_types); + + checker_continuous.setActiveCollisionObjects({ "moving_box_link" }); + checker_continuous.setContactDistanceThreshold(0.1); + + // Set the collision object transforms + checker_continuous.setCollisionObjectsTransform("static_box_link", Eigen::Isometry3d::Identity()); + checker_discrete.setCollisionObjectsTransform("static_box_link", Eigen::Isometry3d::Identity()); + + Eigen::Isometry3d start_pos, end_pos; + start_pos.setIdentity(); + start_pos.translation().x() = -1.9; + end_pos.setIdentity(); + end_pos.translation().x() = 1.9; + checker_continuous.setCollisionObjectsTransform("moving_box_link", start_pos, end_pos); + + Eigen::Isometry3d start_pos_2; + start_pos_2.setIdentity(); + start_pos_2.translation().x() = -1.9; + checker_discrete.setCollisionObjectsTransform("moving_box_link", start_pos_2); + + collision_detection::CollisionResult result; + collision_detection::CollisionResult result_2; + collision_detection::CollisionRequest request; + + // Perform collision check + checker_discrete.contactTest(result_2, request, nullptr); + checker_continuous.contactTest(result, request, nullptr); + + ASSERT_TRUE(result.collision); + ASSERT_FALSE(result_2.collision); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_ros/planning/planning_components_tools/CMakeLists.txt b/moveit_ros/planning/planning_components_tools/CMakeLists.txt index 53924e04e5..77f51dacd2 100644 --- a/moveit_ros/planning/planning_components_tools/CMakeLists.txt +++ b/moveit_ros/planning/planning_components_tools/CMakeLists.txt @@ -14,6 +14,9 @@ target_link_libraries(moveit_visualize_robot_collision_volume moveit_planning_sc add_executable(moveit_evaluate_collision_checking_speed src/evaluate_collision_checking_speed.cpp) target_link_libraries(moveit_evaluate_collision_checking_speed moveit_planning_scene_monitor ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_executable(moveit_compare_collision_checking_speed_fcl_bullet src/compare_collision_speed_checking_fcl_bullet.cpp) +target_link_libraries(moveit_compare_collision_checking_speed_fcl_bullet moveit_planning_scene_monitor ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + add_executable(moveit_kinematics_speed_and_validity_evaluator src/kinematics_speed_and_validity_evaluator.cpp) target_link_libraries(moveit_kinematics_speed_and_validity_evaluator moveit_robot_model_loader ${catkin_LIBRARIES} ${Boost_LIBRARIES}) diff --git a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp new file mode 100644 index 0000000000..3afa3dfdd5 --- /dev/null +++ b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp @@ -0,0 +1,355 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +static const std::string ROBOT_DESCRIPTION = "robot_description"; +static const int MAX_SEARCH_FACTOR_CLUTTER = 3; +static const int MAX_SEARCH_FACTOR_STATES = 30; + +enum class RobotStateSelector +{ + IN_COLLISION, + NOT_IN_COLLISION, + RANDOM, +}; + +enum class CollisionDetector +{ + FCL, + BULLET, +}; + +enum class CollisionObjectType +{ + MESH, + BOX, +}; + +/** \brief Clutters the world of the planning scene with random objects in a certain area around the origin. All added + * objects are not in collision. +* +* \param planning_scene The planning scene +* \param num_objects The number of objects to be cluttered +* \param CollisionObjectType Type of object to clutter (mesh or box) */ +void clutterWorld(const planning_scene::PlanningScenePtr& planning_scene, const size_t num_objects, + CollisionObjectType type) +{ + ROS_INFO("Cluttering scene..."); + + random_numbers::RandomNumberGenerator num_generator = random_numbers::RandomNumberGenerator(123); + + // allow all robot links to be in collision for world check + collision_detection::AllowedCollisionMatrix acm{ collision_detection::AllowedCollisionMatrix( + planning_scene->getRobotModel()->getLinkModelNames(), true) }; + + // set the robot state to home position + robot_state::RobotState& current_state{ planning_scene->getCurrentStateNonConst() }; + collision_detection::CollisionRequest req; + current_state.setToDefaultValues(current_state.getJointModelGroup("panda_arm"), "home"); + current_state.update(); + + // load panda link5 as world collision object + std::string name; + shapes::ShapeConstPtr shape; + std::string kinect = "package://moveit_resources/panda_description/meshes/collision/link5.stl"; + + Eigen::Quaterniond quat; + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + + size_t added_objects{ 0 }; + size_t i{ 0 }; + // create random objects until as many added as desired or quit if too many attempts + while (added_objects < num_objects && i < num_objects * MAX_SEARCH_FACTOR_CLUTTER) + { + // add with random size and random position + pos.translation().x() = num_generator.uniformReal(-1.0, 1.0); + pos.translation().y() = num_generator.uniformReal(-1.0, 1.0); + pos.translation().z() = num_generator.uniformReal(0.0, 1.0); + + quat.x() = num_generator.uniformReal(-1.0, 1.0); + quat.y() = num_generator.uniformReal(-1.0, 1.0); + quat.z() = num_generator.uniformReal(-1.0, 1.0); + quat.w() = num_generator.uniformReal(-1.0, 1.0); + quat.normalize(); + pos.rotate(quat); + + switch (type) + { + case CollisionObjectType::MESH: + { + shapes::Mesh* mesh = shapes::createMeshFromResource(kinect); + mesh->scale(num_generator.uniformReal(0.3, 1.0)); + shape.reset(mesh); + name = "mesh"; + break; + } + case CollisionObjectType::BOX: + { + shape.reset(new shapes::Box(num_generator.uniformReal(0.05, 0.2), num_generator.uniformReal(0.05, 0.2), + num_generator.uniformReal(0.05, 0.2))); + name = "box"; + break; + } + } + + name.append(std::to_string(i)); + planning_scene->getWorldNonConst()->addToObject(name, shape, pos); + + // try if it isn't in collision if yes, ok, if no remove. + collision_detection::CollisionResult res; + planning_scene->checkCollision(req, res, current_state, acm); + + if (!res.collision) + { + added_objects++; + } + else + { + ROS_DEBUG_STREAM("Object was in collision, remove"); + planning_scene->getWorldNonConst()->removeObject(name); + } + + i++; + } + ROS_INFO_STREAM("Cluttered the planning scene with " << added_objects << " objects"); +} + +/** \brief Runs a collision detection benchmark and measures the time. +* +* \param trials The number of repeated collision checks for each state +* \param scene The planning scene +* \param CollisionDetector The type of collision detector +* \param only_self Flag for only self collision check performed */ +void runCollisionDetection(unsigned int trials, const planning_scene::PlanningScenePtr& scene, + const std::vector& states, const CollisionDetector col_detector, + bool only_self, bool distance = false) +{ + collision_detection::AllowedCollisionMatrix acm{ collision_detection::AllowedCollisionMatrix( + scene->getRobotModel()->getLinkModelNames(), true) }; + + ROS_INFO_STREAM("Starting detection using " << (col_detector == CollisionDetector::FCL ? "FCL" : "Bullet")); + + if (col_detector == CollisionDetector::FCL) + { + scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); + } + else + { + scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorBullet::create()); + } + + collision_detection::CollisionResult res; + collision_detection::CollisionRequest req; + + req.distance = distance; + // for world collision request detailed information + if (!only_self) + { + req.contacts = true; + req.max_contacts = 999; + req.max_contacts_per_pair = 99; + } + + ros::WallTime start = ros::WallTime::now(); + for (unsigned int i = 0; i < trials; ++i) + { + for (auto& state : states) + { + res.clear(); + + if (only_self) + { + scene->checkSelfCollision(req, res); + } + else + { + scene->checkCollision(req, res, state); + } + } + } + double duration = (ros::WallTime::now() - start).toSec(); + ROS_INFO("Performed %lf collision checks per second", (double)trials * states.size() / duration); + ROS_INFO_STREAM("Total number was " << trials * states.size() << " checks."); + ROS_INFO_STREAM("We had " << states.size() << " different robot states which were " + << (res.collision ? "in collison " : "not in collision ") << "with " << res.contact_count); + + // color collided objects red + for (auto& contact : res.contacts) + { + ROS_INFO_STREAM("Between: " << contact.first.first << " and " << contact.first.second); + std_msgs::ColorRGBA red; + red.a = 0.8; + red.r = 1; + red.g = 0; + red.b = 0; + scene->setObjectColor(contact.first.first, red); + scene->setObjectColor(contact.first.second, red); + } + + scene->setCurrentState(states.back()); +} + +/** \brief Samples valid states of the robot which can be in collision if desired. + * \param desired_states Specifier for type for desired state + * \param num_states Number of desired states + * \param scene The planning scene + * \param robot_states Result vector */ +void findStates(const RobotStateSelector desired_states, unsigned int num_states, + const planning_scene::PlanningScenePtr& scene, std::vector& robot_states) +{ + robot_state::RobotState& current_state{ scene->getCurrentStateNonConst() }; + collision_detection::CollisionRequest req; + + size_t i{ 0 }; + while (robot_states.size() < num_states && i < num_states * MAX_SEARCH_FACTOR_STATES) + { + current_state.setToRandomPositions(); + current_state.update(); + collision_detection::CollisionResult res; + scene->checkSelfCollision(req, res); + ROS_INFO_STREAM("Found state " << (res.collision ? "in collision" : "not in collision")); + + switch (desired_states) + { + case RobotStateSelector::IN_COLLISION: + if (res.collision) + robot_states.push_back(current_state); + break; + case RobotStateSelector::NOT_IN_COLLISION: + if (!res.collision) + robot_states.push_back(current_state); + break; + case RobotStateSelector::RANDOM: + robot_states.push_back(current_state); + break; + } + i++; + } + + if (!robot_states.empty()) + { + scene->setCurrentState(robot_states[0]); + } + else + { + ROS_ERROR_STREAM("Did not find any correct states."); + } +} + +int main(int argc, char** argv) +{ + robot_model::RobotModelPtr robot_model; + ros::init(argc, argv, "compare_collision_checking_speed"); + ros::NodeHandle node_handle; + + ros::Publisher planning_scene_diff_publisher = node_handle.advertise("planning_scene", 1); + + unsigned int trials = 5000; + + ros::AsyncSpinner spinner(1); + spinner.start(); + ros::WallDuration sleep_t(2.5); + + robot_model = moveit::core::loadTestingRobotModel("panda"); + + planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); + planning_scene_monitor::PlanningSceneMonitor psm(planning_scene, ROBOT_DESCRIPTION); + psm.startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); + psm.startSceneMonitor(); + planning_scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorBullet::create()); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + collision_detection::AllowedCollisionMatrix acm{ collision_detection::AllowedCollisionMatrix( + robot_model->getLinkModelNames(), true) }; + planning_scene->checkCollision(req, res, planning_scene->getCurrentState(), acm); + + ROS_INFO("Starting..."); + + if (psm.getPlanningScene()) + { + ros::Duration(0.5).sleep(); + + robot_state::RobotState& current_state{ planning_scene->getCurrentStateNonConst() }; + current_state.setToDefaultValues(current_state.getJointModelGroup("panda_arm"), "home"); + current_state.update(); + + std::vector sampled_states; + sampled_states.push_back(current_state); + + ROS_INFO("Starting benchmark: Robot in empty world, only self collision check"); + runCollisionDetection(trials, planning_scene, sampled_states, CollisionDetector::BULLET, true); + runCollisionDetection(trials, planning_scene, sampled_states, CollisionDetector::FCL, true); + + planning_scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorBullet::create()); + + clutterWorld(planning_scene, 100, CollisionObjectType::MESH); + + ROS_INFO("Starting benchmark: Robot in cluttered world, no collision with world"); + runCollisionDetection(trials, planning_scene, sampled_states, CollisionDetector::BULLET, false); + runCollisionDetection(trials, planning_scene, sampled_states, CollisionDetector::FCL, false); + + double joint_2 = 1.5; + current_state.setJointPositions("panda_joint2", &joint_2); + current_state.update(); + + std::vector sampled_states_2; + sampled_states_2.push_back(current_state); + + ROS_INFO("Starting benchmark: Robot in cluttered world, in collision with world"); + runCollisionDetection(trials, planning_scene, sampled_states_2, CollisionDetector::BULLET, false); + runCollisionDetection(trials, planning_scene, sampled_states_2, CollisionDetector::FCL, false); + + moveit_msgs::PlanningScene planning_scene_msg; + planning_scene->getPlanningSceneMsg(planning_scene_msg); + planning_scene_diff_publisher.publish(planning_scene_msg); + } + else + { + ROS_ERROR("Planning scene not configured"); + } + + return 0; +} From eb62957e4559bcce3fef6fd774943b4dfd6d99a6 Mon Sep 17 00:00:00 2001 From: Jens P Date: Mon, 11 Nov 2019 20:08:23 +0100 Subject: [PATCH 151/612] Unified Collision Environment Bullet (#1572) * Templated tests adapted for unified collision env * Unified Bullet collision environment: * broadphase filtering adapted for early culling * ACM from SRDF in test * cleanup for bullet single collision env * removed link2castcow for CCD and only use constructor of COW directly * parent class for collision managers * removed function pointer for ACM check * removed extra self-collision manager and use only single manager * speed benchmark for unified environment * PR review: * more descriptive variable names * added user to TODO * PR review: * shortening namespace * documentation improvements * virtual destructor of BVH manager * remove extra speed benchmark * bugfix for not initialized managers * Licenses revised of old tesseract files * PR review: * replaced include guards through pragma * used default instead of empty {} for ctor/dtor * Comments in Bullet readme about thread safety and speed --- .../test_collision_common.h | 103 ++-- .../test_collision_common_panda.h | 75 +-- .../collision_detection_bullet/CMakeLists.txt | 4 +- .../collision_detection_bullet/README.md | 12 +- .../bullet_integration/basic_types.h | 31 +- .../bullet_integration/bullet_bvh_manager.h | 157 ++++++ .../bullet_cast_bvh_manager.h | 158 +----- .../bullet_discrete_bvh_manager.h | 139 +---- .../bullet_integration/bullet_utils.h | 498 ++++-------------- .../contact_checker_common.h | 76 +-- .../bullet_integration/ros_bullet_utils.h | 4 +- .../collision_detector_allocator_bullet.h | 13 +- .../collision_detector_bullet_plugin_loader.h | 4 +- .../collision_env_bullet.h | 148 ++++++ .../collision_robot_bullet.h | 122 ----- .../collision_world_bullet.h | 118 ----- .../bullet_integration/bullet_bvh_manager.cpp | 168 ++++++ .../bullet_cast_bvh_manager.cpp | 283 +--------- .../bullet_discrete_bvh_manager.cpp | 234 +------- .../src/bullet_integration/bullet_utils.cpp | 31 +- .../src/collision_env_bullet.cpp | 432 +++++++++++++++ .../src/collision_robot_bullet.cpp | 273 ---------- .../src/collision_world_bullet.cpp | 297 ----------- ...t_bullet_continuous_collision_checking.cpp | 204 +++---- .../collision_common.h | 2 +- .../src/collision_common.cpp | 2 +- 26 files changed, 1261 insertions(+), 2327 deletions(-) create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h create mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h delete mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h delete mode 100644 moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h create mode 100644 moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp create mode 100644 moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp delete mode 100644 moveit_core/collision_detection_bullet/src/collision_robot_bullet.cpp delete mode 100644 moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h index 331c5a154e..d62c1e8872 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h @@ -38,8 +38,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -70,8 +69,7 @@ class CollisionDetectorTest : public ::testing::Test acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); - crobot_ = value_->allocateRobot(robot_model_); - cworld_ = value_->allocateWorld(collision_detection::WorldPtr(new collision_detection::World())); + cenv_ = value_->allocateEnv(robot_model_); } void TearDown() override @@ -82,8 +80,7 @@ class CollisionDetectorTest : public ::testing::Test robot_model::RobotModelPtr robot_model_; - collision_detection::CollisionRobotPtr crobot_; - collision_detection::CollisionWorldPtr cworld_; + collision_detection::CollisionEnvPtr cenv_; collision_detection::AllowedCollisionMatrixPtr acm_; @@ -105,7 +102,7 @@ TYPED_TEST_P(CollisionDetectorTest, DefaultNotInCollision) collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_FALSE(res.collision); } @@ -132,11 +129,11 @@ TYPED_TEST_P(CollisionDetectorTest, LinksInCollision) robot_state.update(); this->acm_->setEntry("base_link", "base_bellow_link", false); - this->crobot_->checkSelfCollision(req, res1, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res1, robot_state, *this->acm_); ASSERT_TRUE(res1.collision); this->acm_->setEntry("base_link", "base_bellow_link", true); - this->crobot_->checkSelfCollision(req, res2, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_); ASSERT_FALSE(res2.collision); // req.verbose = true; @@ -147,7 +144,7 @@ TYPED_TEST_P(CollisionDetectorTest, LinksInCollision) robot_state.update(); this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); - this->crobot_->checkSelfCollision(req, res3, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res3, robot_state, *this->acm_); ASSERT_TRUE(res3.collision); } @@ -179,7 +176,7 @@ TYPED_TEST_P(CollisionDetectorTest, ContactReporting) this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); collision_detection::CollisionResult res; - this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); EXPECT_EQ(res.contacts.size(), 1u); EXPECT_EQ(res.contacts.begin()->second.size(), 1u); @@ -188,7 +185,7 @@ TYPED_TEST_P(CollisionDetectorTest, ContactReporting) req.max_contacts = 2; req.max_contacts_per_pair = 1; // req.verbose = true; - this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); EXPECT_EQ(res.contacts.size(), 2u); EXPECT_EQ(res.contacts.begin()->second.size(), 1u); @@ -199,7 +196,7 @@ TYPED_TEST_P(CollisionDetectorTest, ContactReporting) req.max_contacts = 10; req.max_contacts_per_pair = 2; this->acm_.reset(new collision_detection::AllowedCollisionMatrix(this->robot_model_->getLinkModelNames(), false)); - this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); EXPECT_LE(res.contacts.size(), 10u); EXPECT_LE(res.contact_count, 10u); @@ -230,7 +227,7 @@ TYPED_TEST_P(CollisionDetectorTest, ContactPositions) this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); collision_detection::CollisionResult res; - this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); ASSERT_EQ(res.contacts.size(), 1u); ASSERT_EQ(res.contacts.begin()->second.size(), 1u); @@ -250,7 +247,7 @@ TYPED_TEST_P(CollisionDetectorTest, ContactPositions) robot_state.update(); collision_detection::CollisionResult res2; - this->crobot_->checkSelfCollision(req, res2, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_); ASSERT_TRUE(res2.collision); ASSERT_EQ(res2.contacts.size(), 1u); ASSERT_EQ(res2.contacts.begin()->second.size(), 1u); @@ -270,7 +267,7 @@ TYPED_TEST_P(CollisionDetectorTest, ContactPositions) robot_state.update(); collision_detection::CollisionResult res3; - this->crobot_->checkSelfCollision(req, res2, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_); ASSERT_FALSE(res3.collision); } @@ -291,18 +288,18 @@ TYPED_TEST_P(CollisionDetectorTest, AttachedBodyTester) // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); robot_state.update(); - this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_FALSE(res.collision); shapes::Shape* shape = new shapes::Box(.1, .1, .1); - this->cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1); + this->cenv_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1); res = collision_detection::CollisionResult(); - this->cworld_->checkRobotCollision(req, res, *this->crobot_, robot_state, *this->acm_); + this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); // deletes shape - this->cworld_->getWorld()->removeObject("box"); + this->cenv_->getWorld()->removeObject("box"); shape = new shapes::Box(.1, .1, .1); std::vector shapes; @@ -313,7 +310,7 @@ TYPED_TEST_P(CollisionDetectorTest, AttachedBodyTester) robot_state.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link"); res = collision_detection::CollisionResult(); - this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); // deletes shape @@ -326,19 +323,19 @@ TYPED_TEST_P(CollisionDetectorTest, AttachedBodyTester) robot_state.update(); res = collision_detection::CollisionResult(); - this->crobot_->checkSelfCollision(req, res, robot_state, *this->acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_FALSE(res.collision); pos1.translation().x() = 5.01; shapes::Shape* coll = new shapes::Box(.1, .1, .1); - this->cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1); + this->cenv_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1); res = collision_detection::CollisionResult(); - this->cworld_->checkRobotCollision(req, res, *this->crobot_, robot_state, *this->acm_); + this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); this->acm_->setEntry("coll", "r_gripper_palm_link", true); res = collision_detection::CollisionResult(); - this->cworld_->checkRobotCollision(req, res, *this->crobot_, robot_state, *this->acm_); + this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); } @@ -351,13 +348,13 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - collision_detection::CollisionRobotPtr new_crobot = this->value_->allocateRobot(this->crobot_); + collision_detection::CollisionEnvPtr new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld()); ros::WallTime before = ros::WallTime::now(); - new_crobot->checkSelfCollision(req, res, robot_state); + new_cenv->checkSelfCollision(req, res, robot_state); double first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); - new_crobot->checkSelfCollision(req, res, robot_state); + new_cenv->checkSelfCollision(req, res, robot_state); double second_check = (ros::WallTime::now() - before).toSec(); EXPECT_LT(fabs(first_check - second_check), .05); @@ -374,21 +371,21 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) robot_state.attachBody("kinect", shapes, poses, touch_links, "r_gripper_palm_link"); before = ros::WallTime::now(); - new_crobot->checkSelfCollision(req, res, robot_state); + new_cenv->checkSelfCollision(req, res, robot_state); first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); - new_crobot->checkSelfCollision(req, res, robot_state); + new_cenv->checkSelfCollision(req, res, robot_state); second_check = (ros::WallTime::now() - before).toSec(); // the first check is going to take a while, as data must be constructed EXPECT_LT(second_check, .1); - collision_detection::CollisionRobotPtr other_new_crobot = this->value_->allocateRobot(this->crobot_); + collision_detection::CollisionEnvPtr other_new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld()); before = ros::WallTime::now(); - new_crobot->checkSelfCollision(req, res, robot_state); + new_cenv->checkSelfCollision(req, res, robot_state); first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); - new_crobot->checkSelfCollision(req, res, robot_state); + new_cenv->checkSelfCollision(req, res, robot_state); second_check = (ros::WallTime::now() - before).toSec(); EXPECT_LT(fabs(first_check - second_check), .05); @@ -404,23 +401,23 @@ TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity(); pos2.translation().x() = 10.0; - this->cworld_->getWorld()->addToObject("kinect", shape, pos1); + this->cenv_->getWorld()->addToObject("kinect", shape, pos1); robot_state::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); ros::WallTime before = ros::WallTime::now(); - this->cworld_->checkRobotCollision(req, res, *this->crobot_, robot_state); + this->cenv_->checkRobotCollision(req, res, robot_state); double first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); - this->cworld_->checkRobotCollision(req, res, *this->crobot_, robot_state); + this->cenv_->checkRobotCollision(req, res, robot_state); double second_check = (ros::WallTime::now() - before).toSec(); EXPECT_LT(second_check, .05); - collision_detection::CollisionWorld::ObjectConstPtr object = this->cworld_->getWorld()->getObject("kinect"); - this->cworld_->getWorld()->removeObject("kinect"); + collision_detection::CollisionEnv::ObjectConstPtr object = this->cenv_->getWorld()->getObject("kinect"); + this->cenv_->getWorld()->removeObject("kinect"); robot_state::RobotState robot_state1(this->robot_model_); robot_state::RobotState robot_state2(this->robot_model_); @@ -440,17 +437,17 @@ TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) // going to take a while, but that's fine res = collision_detection::CollisionResult(); - this->crobot_->checkSelfCollision(req, res, robot_state1); + this->cenv_->checkSelfCollision(req, res, robot_state1); EXPECT_TRUE(res.collision); before = ros::WallTime::now(); - this->crobot_->checkSelfCollision(req, res, robot_state1, *this->acm_); + this->cenv_->checkSelfCollision(req, res, robot_state1, *this->acm_); first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); req.verbose = true; res = collision_detection::CollisionResult(); - this->crobot_->checkSelfCollision(req, res, robot_state2, *this->acm_); + this->cenv_->checkSelfCollision(req, res, robot_state2, *this->acm_); second_check = (ros::WallTime::now() - before).toSec(); EXPECT_LT(first_check, .05); @@ -467,7 +464,7 @@ TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed) shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01))); } ros::WallTime start = ros::WallTime::now(); - this->cworld_->getWorld()->addToObject("map", shapes, poses); + this->cenv_->getWorld()->addToObject("map", shapes, poses); double t = (ros::WallTime::now() - start).toSec(); // TODO: investigate why bullet collision checking is considerably slower here EXPECT_GE(5.0, t); @@ -487,16 +484,16 @@ TYPED_TEST_P(CollisionDetectorTest, MoveMesh) shapes::ShapePtr kinect_shape; kinect_shape.reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); - this->cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); + this->cenv_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); Eigen::Isometry3d np; for (unsigned int i = 0; i < 5; i++) { np = Eigen::Translation3d(i * .001, i * .001, i * .001) * Eigen::Quaterniond::Identity(); - this->cworld_->getWorld()->moveShapeInObject("kinect", kinect_shape, np); + this->cenv_->getWorld()->moveShapeInObject("kinect", kinect_shape, np); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - this->cworld_->checkCollision(req, res, *this->crobot_, robot_state1, *this->acm_); + this->cenv_->checkCollision(req, res, robot_state1, *this->acm_); } } @@ -515,37 +512,37 @@ TYPED_TEST_P(CollisionDetectorTest, TestChangingShapeSize) std::vector shapes; for (unsigned int i = 0; i < 5; i++) { - this->cworld_->getWorld()->removeObject("shape"); + this->cenv_->getWorld()->removeObject("shape"); shapes.clear(); poses.clear(); shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001))); poses.push_back(Eigen::Isometry3d::Identity()); - this->cworld_->getWorld()->addToObject("shape", shapes, poses); + this->cenv_->getWorld()->addToObject("shape", shapes, poses); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - this->cworld_->checkCollision(req, res, *this->crobot_, robot_state1, *this->acm_); + this->cenv_->checkCollision(req, res, robot_state1, *this->acm_); ASSERT_TRUE(res.collision); } Eigen::Isometry3d kinect_pose = Eigen::Isometry3d::Identity(); shapes::ShapePtr kinect_shape; kinect_shape.reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); - this->cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); + this->cenv_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); collision_detection::CollisionRequest req2; collision_detection::CollisionResult res2; - this->cworld_->checkCollision(req2, res2, *this->crobot_, robot_state1, *this->acm_); + this->cenv_->checkCollision(req2, res2, robot_state1, *this->acm_); ASSERT_TRUE(res2.collision); for (unsigned int i = 0; i < 5; i++) { - this->cworld_->getWorld()->removeObject("shape"); + this->cenv_->getWorld()->removeObject("shape"); shapes.clear(); poses.clear(); shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001))); poses.push_back(Eigen::Isometry3d::Identity()); - this->cworld_->getWorld()->addToObject("shape", shapes, poses); + this->cenv_->getWorld()->addToObject("shape", shapes, poses); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - this->cworld_->checkCollision(req, res, *this->crobot_, robot_state1, *this->acm_); + this->cenv_->checkCollision(req, res, robot_state1, *this->acm_); ASSERT_TRUE(res.collision); } } diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 02c3ac73bf..42f356a4d8 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -38,8 +38,7 @@ #include #include #include -#include -#include +#include #include #include @@ -89,8 +88,7 @@ class CollisionDetectorPandaTest : public testing::Test for (const srdf::Model::DisabledCollision& it : dc) acm_->setEntry(it.link1_, it.link2_, true); - crobot_ = value_->allocateRobot(robot_model_); - cworld_ = value_->allocateWorld(collision_detection::WorldPtr(new collision_detection::World())); + cenv_ = value_->allocateEnv(robot_model_); robot_state_.reset(new robot_state::RobotState(robot_model_)); setToHome(*robot_state_); @@ -104,8 +102,7 @@ class CollisionDetectorPandaTest : public testing::Test robot_model::RobotModelPtr robot_model_; - collision_detection::CollisionWorldPtr cworld_; - collision_detection::CollisionRobotPtr crobot_; + collision_detection::CollisionEnvPtr cenv_; collision_detection::AllowedCollisionMatrixPtr acm_; @@ -125,7 +122,7 @@ TYPED_TEST_P(CollisionDetectorPandaTest, DefaultNotInCollision) { collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_FALSE(res.collision); } @@ -141,30 +138,7 @@ TYPED_TEST_P(CollisionDetectorPandaTest, LinksInCollision) collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); - ASSERT_TRUE(res.collision); -} - -// TODO: Add collision check capability within world itself and then enable test -/** \brief Two boxes in collision in the world environment. */ -TYPED_TEST_P(CollisionDetectorPandaTest, DISABLED_WorldToWorldCollision) -{ - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - - shapes::Shape* shape = new shapes::Box(.5, .5, .5); - shapes::ShapeConstPtr shape_ptr(shape); - - Eigen::Isometry3d pos_1 = Eigen::Isometry3d::Identity(); - pos_1.translation().x() = 1; - this->cworld_->getWorld()->addToObject("box", shape_ptr, pos_1); - - Eigen::Isometry3d pos_2 = Eigen::Isometry3d::Identity(); - pos_2.translation().x() = 1.2; - this->cworld_->getWorld()->addToObject("box_2", shape_ptr, pos_2); - - this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); - + this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_TRUE(res.collision); } @@ -179,22 +153,22 @@ TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_1) Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); pos1.translation().z() = 0.3; - this->cworld_->getWorld()->addToObject("box", shape_ptr, pos1); + this->cenv_->getWorld()->addToObject("box", shape_ptr, pos1); - this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_FALSE(res.collision); res.clear(); - this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_TRUE(res.collision); res.clear(); - this->cworld_->getWorld()->moveObject("box", pos1); - this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_TRUE(res.collision); res.clear(); - this->cworld_->getWorld()->moveObject("box", pos1); + this->cenv_->getWorld()->moveObject("box", pos1); ASSERT_FALSE(res.collision); } @@ -212,8 +186,8 @@ TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_2) Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); pos1.translation().z() = 0.3; - this->cworld_->getWorld()->addToObject("box", shape_ptr, pos1); - this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + this->cenv_->getWorld()->addToObject("box", shape_ptr, pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_TRUE(res.collision); ASSERT_GE(res.contact_count, 3u); res.clear(); @@ -227,7 +201,7 @@ TYPED_TEST_P(CollisionDetectorPandaTest, PaddingTest) req.max_contacts = 10; collision_detection::CollisionResult res; - this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_FALSE(res.collision); res.clear(); @@ -239,15 +213,15 @@ TYPED_TEST_P(CollisionDetectorPandaTest, PaddingTest) pos.translation().x() = 0.43; pos.translation().y() = 0; pos.translation().z() = 0.55; - this->cworld_->getWorld()->addToObject("box", shape_ptr, pos); + this->cenv_->getWorld()->addToObject("box", shape_ptr, pos); - this->crobot_->setLinkPadding("panda_hand", 0.08); - this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + this->cenv_->setLinkPadding("panda_hand", 0.08); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_TRUE(res.collision); res.clear(); - this->crobot_->setLinkPadding("panda_hand", 0.0); - this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + this->cenv_->setLinkPadding("panda_hand", 0.0); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_FALSE(res.collision); } @@ -257,7 +231,7 @@ TYPED_TEST_P(CollisionDetectorPandaTest, DistanceSelf) collision_detection::CollisionRequest req; req.distance = true; collision_detection::CollisionResult res; - this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_FALSE(res.collision); EXPECT_NEAR(res.distance, 0.13, 0.01); } @@ -276,14 +250,13 @@ TYPED_TEST_P(CollisionDetectorPandaTest, DistanceWorld) pos.translation().x() = 0.43; pos.translation().y() = 0; pos.translation().z() = 0.55; - this->cworld_->getWorld()->addToObject("box", shape_ptr, pos); + this->cenv_->getWorld()->addToObject("box", shape_ptr, pos); - this->crobot_->setLinkPadding("panda_hand", 0.0); - this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_); + this->cenv_->setLinkPadding("panda_hand", 0.0); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_FALSE(res.collision); EXPECT_NEAR(res.distance, 0.029, 0.01); } REGISTER_TYPED_TEST_CASE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision, - DISABLED_WorldToWorldCollision, RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, - DistanceSelf, DistanceWorld); + RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, DistanceSelf, DistanceWorld); diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index d50c5a9ed4..4b4ccf6547 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -1,11 +1,11 @@ set(MOVEIT_LIB_NAME moveit_collision_detection_bullet) add_library(${MOVEIT_LIB_NAME} - src/collision_robot_bullet.cpp - src/collision_world_bullet.cpp src/bullet_integration/bullet_utils.cpp src/bullet_integration/bullet_discrete_bvh_manager.cpp src/bullet_integration/bullet_cast_bvh_manager.cpp + src/collision_env_bullet.cpp + src/bullet_integration/bullet_bvh_manager.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") diff --git a/moveit_core/collision_detection_bullet/README.md b/moveit_core/collision_detection_bullet/README.md index b808244650..63b95739f6 100644 --- a/moveit_core/collision_detection_bullet/README.md +++ b/moveit_core/collision_detection_bullet/README.md @@ -1,6 +1,8 @@ ### Using Bullet as a collision checker [experimental] -To use Bullet as a collision checker you can: -a) create a new planning scene and manually switch to Bullet using the planning_scene.setActiveCollisionDetector() -b) manually switch to Bullet using the `CollisionPluginLoader` -c) manually instantiate a `CollisionRobotBullet` and `CollisionWorldBullet` -d) use rosparam to set the collision checker (preferred but not tested yet) when starting a new `move_group` +To use Bullet as a collision checker you can manually switch to Bullet using the planning_scene.setActiveCollisionDetector() + +### Speed Benchmarks +For speed comparisons to FCL please see (relative link to README of benchmark script will be added). + +### Current Limitations +The collision detection using Bullet is not thread safe as the internal collision managers are `mutable` members of the collision environment. diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h index 385ed46f23..d243c030be 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h @@ -18,8 +18,7 @@ /* Author: Levi Armstrong */ -#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BASIC_TYPES_H_ -#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BASIC_TYPES_H_ +#pragma once #include #include @@ -44,12 +43,6 @@ template using AlignedUnorderedMap = std::unordered_map, std::equal_to, Eigen::aligned_allocator>>; -/** @brief Checks if contact is allowed or not. Returns true if yes, otherwise false. - * - * The order of strings should not matter, the function should handled by the function. */ -typedef std::function - IsContactAllowedFn; - enum class CollisionObjectType { USE_SHAPE_TYPE = 0, /**< @brief Infer the type from the type specified in the shapes::Shape class */ @@ -66,17 +59,9 @@ struct ContactTestData { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - ContactTestData(const std::vector& active, const double& contact_distance, const IsContactAllowedFn& fn, - collision_detection::CollisionResult& res, const collision_detection::CollisionRequest& req, - const collision_detection::AllowedCollisionMatrix* acm) - : active(active) - , contact_distance(contact_distance) - , fn(fn) - , acm(acm) - , res(res) - , req(req) - , done(false) - , pair_done(false) + ContactTestData(const std::vector& active, const double& contact_distance, + collision_detection::CollisionResult& res, const collision_detection::CollisionRequest& req) + : active(active), contact_distance(contact_distance), res(res), req(req), done(false), pair_done(false) { } @@ -85,12 +70,6 @@ struct ContactTestData /** \brief If after a positive broadphase check the distance is below this threshold, a contact is added. */ const double& contact_distance; - /** \brief User defined function which checks if contact is allowed between two objects */ - const IsContactAllowedFn& fn; - - /** \brief Indicates collision objects which are allowed to be in contact */ - const collision_detection::AllowedCollisionMatrix* acm; - collision_detection::CollisionResult& res; const collision_detection::CollisionRequest& req; @@ -102,5 +81,3 @@ struct ContactTestData }; } // namespace collision_detection_bullet - -#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BASIC_TYPES_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h new file mode 100644 index 0000000000..140f684f16 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h @@ -0,0 +1,157 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * 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. + * + * 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. + *********************************************************************/ + +/* Authors: Levi Armstrong, Jens Petit */ + +#pragma once + +#include +#include + +namespace collision_detection_bullet +{ +MOVEIT_CLASS_FORWARD(BulletBVHManager) + +/** @brief A bounding volume hierarchy (BVH) implementation of a tesseract contact manager */ +class BulletBVHManager +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Constructor */ + BulletBVHManager(); + + virtual ~BulletBVHManager(); + + /** @brief Clone the manager + * + * This is to be used for multi threaded applications. A user should make a clone for each thread. */ + BulletBVHManagerPtr clone() const; + + /**@brief Find if a collision object already exists + * @param name The name of the collision object + * @return true if it exists, otherwise false. */ + bool hasCollisionObject(const std::string& name) const; + + /**@brief Remove an object from the checker + * @param name The name of the object + * @return true if successfully removed, otherwise false. */ + bool removeCollisionObject(const std::string& name); + + /**@brief Enable an object + * @param name The name of the object + * @return true if successfully enabled, otherwise false. */ + bool enableCollisionObject(const std::string& name); + + /**@brief Disable an object + * @param name The name of the object + * @return true if successfully disabled, otherwise false. */ + bool disableCollisionObject(const std::string& name); + + /**@brief Set a single static collision object's tansform + * @param name The name of the object + * @param pose The tranformation in world */ + void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose); + + /**@brief Set which collision objects are active + * @param names A vector of collision object names */ + void setActiveCollisionObjects(const std::vector& names); + + /**@brief Get which collision objects are active + * @return A vector of collision object names */ + const std::vector& getActiveCollisionObjects() const; + + /**@brief Set the contact distance threshold for which collision should be considered through expanding the AABB by + * the + * contact_distance for all links. + * @param contact_distance The contact distance */ + void setContactDistanceThreshold(double contact_distance); + + /**@brief Get the contact distance threshold + * @return The contact distance */ + double getContactDistanceThreshold() const; + + /**@brief Perform a contact test for all objects + * @param collisions The Contact results data + * @param req The collision request data + * @param acm The allowed collision matrix + * @param self Used for indicating self collision checks */ + virtual void contactTest(collision_detection::CollisionResult& collisions, + const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, bool self) = 0; + + /**@brief Add a collision object to the checker + * + * All objects are added should initially be added as static objects. Use the setContactRequest method of defining + * which collision objects are moving. + * + * @param name The name of the object, must be unique. + * @param mask_id User defined id which gets stored in the results structure. + * @param shapes A vector of shapes that make up the collision object. + * @param shape_poses A vector of poses for each shape, must be same length as shapes + * @param shape_types A vector of shape types for encode the collision object. If the vector is of length 1 it is + * used for all shapes. + * @param collision_object_types A int identifying a conversion mode for the object. (ex. convert meshes to convex + * hulls) + * @param enabled Indicate if the object is enabled for collision checking. + * @return true if successfully added, otherwise false. */ + /**@brief Add a tesseract collision object to the manager + * @param cow The tesseract bullet collision object */ + virtual void addCollisionObject(const CollisionObjectWrapperPtr& cow) = 0; + + const std::map& getCollisionObjects() const; + +protected: + /** @brief A map of collision objects being managed */ + std::map link2cow_; + + /** @brief A list of the active collision links */ + std::vector active_; + + /** @brief The contact distance threshold */ + double contact_distance_; + + /** @brief The bullet collision dispatcher used for getting object to object collison algorithm */ + std::unique_ptr dispatcher_; + + /** @brief The bullet collision dispatcher configuration information */ + btDispatcherInfo dispatch_info_; + + /** @brief The bullet collision configuration */ + btDefaultCollisionConfiguration coll_config_; + + /** @brief The bullet broadphase interface */ + std::unique_ptr broadphase_; + + /** \brief Callback function for culling objects before a broadphase check */ + BroadphaseFilterCallback filter_callback_; +}; +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h index 40085ff76c..45013d2113 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h @@ -29,12 +29,12 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Levi Armstrong */ +/* Author: Levi Armstrong, Jens Petit */ -#ifndef TESSERACT_COLLISION_BULLET_CAST_BVH_MANAGERS_H -#define TESSERACT_COLLISION_BULLET_CAST_BVH_MANAGERS_H +#pragma once #include +#include #include namespace collision_detection_bullet @@ -42,76 +42,21 @@ namespace collision_detection_bullet MOVEIT_CLASS_FORWARD(BulletCastBVHManager) /** @brief A bounding volume hierarchy (BVH) implementation of a tesseract contact manager */ -class BulletCastBVHManager +class BulletCastBVHManager : public BulletBVHManager { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /** \brief Constructor */ - BulletCastBVHManager(); + BulletCastBVHManager() = default; - ~BulletCastBVHManager(); + virtual ~BulletCastBVHManager() = default; - /** @brief Clone the manager + /**@brief Clone the manager * * This is to be used for multi threaded applications. A user should make a clone for each thread. */ BulletCastBVHManagerPtr clone() const; - /**@brief Add a collision object to the checker - * - * All objects are added should initially be added as static objects. Use the setContactRequest method of defining - * which collision objects are moving. - * - * @param name The name of the object, must be unique. - * @param mask_id User defined id which gets stored in the results structure. - * @param shapes A vector of shapes that make up the collision object. - * @param shape_poses A vector of poses for each shape, must be same length as shapes - * @param shape_types A vector of shape types for encode the collision object. If the vector is of length 1 it is - * used for all shapes. - * @param collision_object_types A int identifying a conversion mode for the object. (ex. convert meshes to convex - * hulls) - * @param enabled Indicate if the object is enabled for collision checking. - * @return true if successfully added, otherwise false. */ - bool addCollisionObject(const std::string& name, const collision_detection::BodyType& mask_id, - const std::vector& shapes, - const AlignedVector& shape_poses, - const std::vector& collision_object_types, bool enabled = true); - - /**@brief Find if a collision object already exists - * @param name The name of the collision object - * @return true if it exists, otherwise false. */ - bool hasCollisionObject(const std::string& name) const; - - /**@brief Remove an object from the checker - * @param name The name of the object - * @return true if successfully removed, otherwise false. */ - bool removeCollisionObject(const std::string& name); - - /**@brief Enable an object - * @param name The name of the object - * @return true if successfully enabled, otherwise false. */ - bool enableCollisionObject(const std::string& name); - - /**@brief Disable an object - * @param name The name of the object - * @return true if successfully disabled, otherwise false. */ - bool disableCollisionObject(const std::string& name); - - /**@brief Set a single static collision object's tansform - * @param name The name of the object - * @param pose The tranformation in world */ - void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose); - - /**@brief Set a series of static collision objects' tranforms - * @param names The names of the objects - * @param poses The tranformations in world */ - void setCollisionObjectsTransform(const std::vector& names, - const AlignedVector& poses); - - /**@brief Set a series of static collision objects' tranforms - * @param transforms A transform map */ - void setCollisionObjectsTransform(const AlignedMap& transforms); - /**@brief Set a single cast (moving) collision object's tansforms * * This should only be used for moving objects. @@ -119,99 +64,18 @@ class BulletCastBVHManager * @param name The name of the object * @param pose1 The start tranformation in world * @param pose2 The end tranformation in world */ - void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose1, - const Eigen::Isometry3d& pose2); - - /**@brief Set a series of cast (moving) collision objects' tranforms - * - * This should only be used for moving objects. Use the base class methods for static objects. - * - * @param names The name of the object - * @param pose1 The start tranformations in world - * @param pose2 The end tranformations in world */ - void setCollisionObjectsTransform(const std::vector& names, - const AlignedVector& pose1, - const AlignedVector& pose2); - - /**@brief Set a series of cast (moving) collision objects' tranforms - * - * This should only be used for moving objects. Use the base class methods for static objects. - * - * @param pose1 A start transform map - * @param pose2 A end transform map */ - void setCollisionObjectsTransform(const AlignedMap& pose1, - const AlignedMap& pose2); - - /**@brief Set which collision objects are active - * @param names A vector of collision object names */ - void setActiveCollisionObjects(const std::vector& names); - - /**@brief Get which collision objects are active - * @return A vector of collision object names */ - const std::vector& getActiveCollisionObjects() const; - - /**@brief Set the contact distance threshold for which collision should be considered through expanding the AABB by - * the contact_distance for all links. - * - * @param contact_distance The contact distance */ - void setContactDistanceThreshold(double contact_distance); - - /**@brief Get the contact distance threshold - * @return The contact distance */ - double getContactDistanceThreshold() const; - - /** @brief Set the function for determining if two links are allowed to be in collision */ - void setIsContactAllowedFn(IsContactAllowedFn fn); - - /** @brief Get the function for determining if two links are allowed to be in collision */ - IsContactAllowedFn getIsContactAllowedFn() const; + void setCastCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose1, + const Eigen::Isometry3d& pose2); /**@brief Perform a contact test for all objects * @param collisions The Contact results data * @param req The collision request data * @param acm The allowed collision matrix */ void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, - const collision_detection::AllowedCollisionMatrix* acm); + const collision_detection::AllowedCollisionMatrix* acm, bool self) override; /**@brief Add a tesseract collision object to the manager * @param cow The tesseract bullet collision object */ - void addCollisionObject(const CollisionObjectWrapperPtr& cow); - - /** \brief Callback function for culling objects before a broadphase check */ - BroadphaseFilterCallback filter_callback_; - -private: - /** @brief A list of the active collision objects */ - std::vector active_; - - /** @brief The contact distance threshold */ - double contact_distance_; - - /** @brief The is allowed collision function */ - IsContactAllowedFn fn_; - - /** @brief The bullet collision dispatcher used for getting object to object collison algorithm */ - std::unique_ptr dispatcher_; - - /** @brief The bullet collision dispatcher configuration information */ - btDispatcherInfo dispatch_info_; - - /** @brief The bullet collision configuration */ - btDefaultCollisionConfiguration coll_config_; - - /** @brief The bullet broadphase interface */ - std::unique_ptr broadphase_; - - /** @brief A map of collision objects being managed */ - std::map link2cow_; - - /** @brief A map of cast collision objects being managed. */ - std::map link2castcow_; - - /**@brief Perform a contact test for the provided object which is not part of the manager - * @param cow The Collision object - * @param collisions The collision results */ - void contactTest(const CollisionObjectWrapperPtr& cow, ContactTestData& collisions); + void addCollisionObject(const CollisionObjectWrapperPtr& cow) override; }; } // namespace collision_detection_bullet -#endif // TESSERACT_COLLISION_BULLET_CAST_BVH_MANAGERS_H diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h index b9e8e91e96..eea9e17820 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h @@ -29,12 +29,12 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Levi Armstrong */ +/* Author: Levi Armstrong, Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_COLLISION_BULLET_DISCRETE_BVH_MANAGER_H_ -#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_COLLISION_BULLET_DISCRETE_BVH_MANAGER_H_ +#pragma once #include +#include #include namespace collision_detection_bullet @@ -42,153 +42,30 @@ namespace collision_detection_bullet MOVEIT_CLASS_FORWARD(BulletDiscreteBVHManager) /** @brief A bounding volume hierarchy (BVH) implementaiton of a discrete bullet manager */ -class BulletDiscreteBVHManager +class BulletDiscreteBVHManager : public BulletBVHManager { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /** \brief Constructor */ - BulletDiscreteBVHManager(); + BulletDiscreteBVHManager() = default; - ~BulletDiscreteBVHManager(); + virtual ~BulletDiscreteBVHManager() = default; /**@brief Clone the manager * * This is to be used for multi threaded applications. A user should make a clone for each thread. */ BulletDiscreteBVHManagerPtr clone() const; - /**@brief Add a object to the checker - * @param name The name of the object, must be unique. - * @param mask_id User defined id which gets stored in the results structure. - * @param shapes A vector of shapes that make up the collision object. - * @param shape_poses A vector of poses for each shape, must be same length as shapes - * @param shape_types A vector of shape types for encode the collision object. If the vector is of length 1 it is - * used for all shapes. - * @param collision_object_types A int identifying a conversion mode for the object. (ex. convert meshes to convex - * hulls) - * @return true if successfully added, otherwise false. */ - bool addCollisionObject(const std::string& name, const collision_detection::BodyType& mask_id, - const std::vector& shapes, - const AlignedVector& shape_poses, - const std::vector& collision_object_types, bool enabled = true); - - /**@brief Find if a collision object already exists - * @param name The name of the collision object - * @return true if it exists, otherwise false. */ - bool hasCollisionObject(const std::string& name) const; - - /**@brief Remove an object from the checker - * @param name The name of the object - * @return true if successfully removed, otherwise false. */ - bool removeCollisionObject(const std::string& name); - - /**@brief Enable an object - * @param name The name of the object */ - bool enableCollisionObject(const std::string& name); - - /**@brief Disable an object - * @param name The name of the object */ - bool disableCollisionObject(const std::string& name); - - /**@brief Set a single collision object's tansform - * @param name The name of the object - * @param pose The tranformation in world */ - void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose); - - /**@brief Set a single collision object's tansform - * @param name The name of the object - * @param pose The tranformation in world */ - void setCollisionObjectsTransform(const std::string& name, const btTransform& pose); - - /**@brief Set a series of collision objects' tranforms - * @param names The names of the object - * @param poses The tranformations in world */ - void setCollisionObjectsTransform(const std::vector& names, - const AlignedVector& poses); - - /**@brief Set a series of collision objects' tranforms - * @param transforms A transform map */ - void setCollisionObjectsTransform(const AlignedMap& transforms); - - /**@brief Set which collision objects can move - * @param names A vector of collision object names */ - void setActiveCollisionObjects(const std::vector& names); - - /**@brief Get which collision objects can move - * @return A list of collision object names */ - const std::vector& getActiveCollisionObjects() const; - - /**@brief Set the contact distance threshold for which collision should be considered. - * @param contact_distance The contact distance */ - void setContactDistanceThreshold(double contact_distance); - - /**@brief Get the contact distance threshold - * @return The contact distance */ - double getContactDistanceThreshold() const; - - /** @brief Set the active function for determining if two links are allowed to be in collision */ - void setIsContactAllowedFn(IsContactAllowedFn fn); - - /** @brief Get the active function for determining if two links are allowed to be in collision */ - IsContactAllowedFn getIsContactAllowedFn() const; - /**@brief Perform a contact test for all objects in the manager * @param collisions The Contact results data * @param acm The allowed collision matrix * @param req The contact request */ void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, - const collision_detection::AllowedCollisionMatrix* acm); - - /**@brief Perform a contact test for all objects in the manager and the external ones provided - * @param collisions The Contact results data - * @param req The contact request - * @param acm The allowed collision matrix - * @param req The contact request - * @param cows_external Objects to check which are not part of the manager */ - void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, - const collision_detection::AllowedCollisionMatrix* acm, - const std::vector cows_external); + const collision_detection::AllowedCollisionMatrix* acm, bool self) override; /**@brief Add a bullet collision object to the manager * @param cow The bullet collision object */ - void addCollisionObject(const CollisionObjectWrapperPtr& cow); - - /**@brief Return collision objects - * @return A map of collision objects */ - const std::map& getCollisionObjects() const; - - /** \brief Callback function for culling objects before a broadphase check */ - BroadphaseFilterCallback filter_callback_; - -private: - /** @brief A list of the active collision objects */ - std::vector active_; - - /** @brief The contact distance threshold */ - double contact_distance_; - - /** @brief The is allowed collision function */ - IsContactAllowedFn fn_; - - /** @brief The bullet collision dispatcher used for getting object to object collison algorithm */ - std::unique_ptr dispatcher_; - - /** @brief The bullet collision dispatcher configuration information */ - btDispatcherInfo dispatch_info_; - - /** @brief The bullet collision configuration */ - btDefaultCollisionConfiguration coll_config_; - - /** @brief The bullet broadphase interface */ - std::unique_ptr broadphase_; - - /** @brief A map of all (static and active) collision objects being managed */ - std::map link2cow_; - - /**@brief Perform a contact test for the provided object which is not part of the manager - * @param cow The Collision object - * @param collisions The collision results */ - void contactTest(const CollisionObjectWrapperPtr& cow, ContactTestData& collisions); + void addCollisionObject(const CollisionObjectWrapperPtr& cow) override; }; } // namespace collision_detection_bullet -#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_COLLISION_BULLET_DISCRETE_BVH_MANAGER_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h index c2a604837e..29f2434724 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h @@ -32,8 +32,7 @@ /* Authors: John Schulman, Levi Armstrong */ -#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BULLET_UTILS_H_ -#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BULLET_UTILS_H_ +#pragma once #include #include @@ -58,6 +57,44 @@ const bool BULLET_COMPOUND_USE_DYNAMIC_AABB = true; MOVEIT_CLASS_FORWARD(CollisionObjectWrapper) +/** \brief Allowed = true */ +inline bool acmCheck(const std::string& body_1, const std::string& body_2, + const collision_detection::AllowedCollisionMatrix* acm) +{ + collision_detection::AllowedCollision::Type allowed_type; + + if (acm != nullptr) + { + if (acm->getEntry(body_1, body_2, allowed_type)) + { + if (allowed_type == collision_detection::AllowedCollision::Type::NEVER) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not allowed entry in ACM found, collision check between " + << body_1 << " and " << body_2); + return false; + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Entry in ACM found, skipping collision check as allowed " + << body_1 << " and " << body_2); + return true; + } + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No entry in ACM found, collision check between " + << body_1 << " and " << body_2); + return false; + } + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No ACM, collision check between " << body_1 << " and " + << body_2); + return false; + } +} + /** \brief Converts eigen vector to bullet vector */ inline btVector3 convertEigenToBt(const Eigen::Vector3d& v) { @@ -99,17 +136,22 @@ inline btTransform convertEigenToBt(const Eigen::Isometry3d& t) /** @brief Tesseract bullet collision object. * - * A wrapper around bullet's collision object which contains specific information related to bullet */ + * A wrapper around bullet's collision object which contains specific information related to bullet. One of the main + * differences is that a bullet collision object has a single world transformation and all shapes have transformation + * relative to this world transform. The default collision object category is active and active objects are checked + * against active objects and static objects whereas static objects are only checked against active ones. */ class CollisionObjectWrapper : public btCollisionObject { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /** \brief Standard constructor */ + /** \brief Standard constructor + * + * \param shape_poses Assumes all poses are in a single global frame */ CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, const std::vector& shapes, const AlignedVector& shape_poses, - const std::vector& collision_object_types); + const std::vector& collision_object_types, bool active = true); /** \brief Constructor for attached robot objects */ CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, @@ -125,7 +167,7 @@ class CollisionObjectWrapper : public btCollisionObject short int m_collisionFilterMask; /** \brief Indicates if the collision object is used for a collision check */ - bool m_enabled; + bool m_enabled{ true }; /** \brief The robot links the collision objects is allowed to touch */ std::set m_touch_links; @@ -159,10 +201,9 @@ class CollisionObjectWrapper : public btCollisionObject void getAABB(btVector3& aabb_min, btVector3& aabb_max) const { getCollisionShape()->getAabb(getWorldTransform(), aabb_min, aabb_max); - const btScalar& d = getContactProcessingThreshold(); - // added as bullet expands each AABB by 4 cm - double dis = d; // - 0.04; - btVector3 contact_threshold(dis, dis, dis); + const btScalar& distance = getContactProcessingThreshold(); + // note that bullet expands each AABB by 4 cm + btVector3 contact_threshold(distance, distance, distance); aabb_min -= contact_threshold; aabb_max += contact_threshold; } @@ -206,16 +247,18 @@ class CollisionObjectWrapper : public btCollisionObject const std::vector& collision_object_types, const std::vector>& data); + /** \brief The name of the object, must be unique. */ std::string m_name; + collision_detection::BodyType m_type_id; /** @brief The shapes that define the collison object */ std::vector m_shapes; - /** @brief The poses of the shapes */ + /** @brief The poses of the shapes, must be same length as m_shapes */ AlignedVector m_shape_poses; - /** @brief The shape collision object type to be used */ + /** @brief The shape collision object type to be used. */ std::vector m_collision_object_types; /** @brief Manages the collision shape pointer so they get destroyed */ @@ -240,17 +283,18 @@ struct CastHullShape : public btConvexShape m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE; } - void updateCastTransform(const btTransform& t01) + void updateCastTransform(const btTransform& cast_transform) { - m_shape_transform = t01; + m_shape_transform = cast_transform; } /** \brief From both shape poses computes the support vertex and returns the larger one. */ btVector3 localGetSupportingVertex(const btVector3& vec) const override { - btVector3 sv0 = m_shape->localGetSupportingVertex(vec); - btVector3 sv1 = m_shape_transform * m_shape->localGetSupportingVertex(vec * m_shape_transform.getBasis()); - return (vec.dot(sv0) > vec.dot(sv1)) ? sv0 : sv1; + btVector3 support_vector_0 = m_shape->localGetSupportingVertex(vec); + btVector3 support_vector_1 = + m_shape_transform * m_shape->localGetSupportingVertex(vec * m_shape_transform.getBasis()); + return (vec.dot(support_vector_0) > vec.dot(support_vector_1)) ? support_vector_0 : support_vector_1; } void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* /*vectors*/, @@ -263,11 +307,11 @@ struct CastHullShape : public btConvexShape /** \brief Shape specific fast recalculation of the AABB at a certain pose * * The AABB is not recalculated from scratch but updated depending on the given transformation. */ - void getAabb(const btTransform& t_w0, btVector3& aabbMin, btVector3& aabbMax) const override + void getAabb(const btTransform& transform_world, btVector3& aabbMin, btVector3& aabbMax) const override { - m_shape->getAabb(t_w0, aabbMin, aabbMax); + m_shape->getAabb(transform_world, aabbMin, aabbMax); btVector3 min1, max1; - m_shape->getAabb(t_w0 * m_shape_transform, min1, max1); + m_shape->getAabb(transform_world * m_shape_transform, min1, max1); aabbMin.setMin(min1); aabbMax.setMax(max1); } @@ -373,46 +417,6 @@ inline void getAverageSupport(const btConvexShape* shape, const btVector3& local } } -/** @brief Check if a collision check is required between the provided collision objects - * @param cow1 The first collision object - * @param cow2 The second collision object - * @param acm The contact allowed function pointer - * @param verbose Indicate if verbose information should be printed - * @return True if the two collision objects should be checked for collision, otherwise false */ -inline bool needsCollisionCheck(const CollisionObjectWrapper& cow1, const CollisionObjectWrapper& cow2, - const IsContactAllowedFn& allowed_fn, - const collision_detection::AllowedCollisionMatrix* acm, bool verbose = false) -{ - if (!cow1.m_enabled) - return false; - - if (!cow2.m_enabled) - return false; - - if ((!(cow2.m_collisionFilterGroup & cow1.m_collisionFilterMask) || - !(cow1.m_collisionFilterGroup & cow2.m_collisionFilterMask))) - return false; - - if (isContactAllowed(cow1.getName(), cow2.getName(), allowed_fn, acm, verbose)) - return false; - - if (cow1.getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && - cow2.getTypeID() == collision_detection::BodyType::ROBOT_LINK) - if (cow1.m_touch_links.find(cow2.getName()) != cow1.m_touch_links.end()) - return false; - - if (cow2.getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && - cow1.getTypeID() == collision_detection::BodyType::ROBOT_LINK) - if (cow2.m_touch_links.find(cow1.getName()) == cow2.m_touch_links.end()) - return false; - - // TODO: Add check for two objects attached to the same link - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Checking collision btw " << cow1.getName() << " vs " - << cow2.getName()); - - return true; -} - /** \brief Converts a bullet contact result to MoveIt format and adds it to the result data structure */ inline btScalar addDiscreteSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, const btCollisionObjectWrapper* colObj1Wrap, ContactTestData& collisions) @@ -547,63 +551,14 @@ inline btScalar addCastSingleResult(btManifoldPoint& cp, const btCollisionObject return 1; } -/** @brief This is copied directly out of BulletWorld */ -struct TesseractBridgedManifoldResult : public btManifoldResult +/** \brief Checks if the collision pair is kinematic vs kinematic objects */ +inline bool isOnlyKinematic(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) { - btCollisionWorld::ContactResultCallback& m_resultCallback; - - TesseractBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap, - btCollisionWorld::ContactResultCallback& resultCallback) - : btManifoldResult(obj0Wrap, obj1Wrap), m_resultCallback(resultCallback) - { - } - - void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) override - { - bool is_swapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); - btVector3 point_a = pointInWorld + normalOnBInWorld * depth; - btVector3 local_a; - btVector3 local_b; - if (is_swapped) - { - local_a = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); - local_b = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } - else - { - local_a = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); - local_b = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } - - btManifoldPoint new_pt(local_a, local_b, normalOnBInWorld, depth); - new_pt.m_positionWorldOnA = point_a; - new_pt.m_positionWorldOnB = pointInWorld; - - // BP mod, store contact triangles. - if (is_swapped) - { - new_pt.m_partId0 = m_partId1; - new_pt.m_partId1 = m_partId0; - new_pt.m_index0 = m_index1; - new_pt.m_index1 = m_index0; - } - else - { - new_pt.m_partId0 = m_partId0; - new_pt.m_partId1 = m_partId1; - new_pt.m_index0 = m_index0; - new_pt.m_index1 = m_index1; - } - - // experimental feature info, for per-triangle material etc. - const btCollisionObjectWrapper* obj0_wrap = is_swapped ? m_body1Wrap : m_body0Wrap; - const btCollisionObjectWrapper* obj1_wrap = is_swapped ? m_body0Wrap : m_body1Wrap; - m_resultCallback.addSingleResult(new_pt, obj0_wrap, new_pt.m_partId0, new_pt.m_index0, obj1_wrap, new_pt.m_partId1, - new_pt.m_index1); - } -}; + return cow0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter && + cow1->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter; +} -/** @brief Abstract interface for both discrete and continuous broadphase collision pair +/** @brief Callback structure for both discrete and continuous broadphase collision pair * * /e needsCollision is the callback executed before a narrowphase check is executed. * /e addSingleResult is the callback executed after the narrowphase check delivers a result. */ @@ -611,58 +566,41 @@ struct BroadphaseContactResultCallback { ContactTestData& collisions_; double contact_distance_; - bool verbose_; + const collision_detection::AllowedCollisionMatrix* acm_{ nullptr }; - BroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false) - : collisions_(collisions), contact_distance_(contact_distance), verbose_(verbose) - { - } + /** \brief Indicates if the callback is used for only self-collision checking */ + bool self_; - virtual ~BroadphaseContactResultCallback() = default; + /** \brief Indicates if the callback is used for casted collisions */ + bool cast_{ false }; - virtual bool needsCollision(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) const + BroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, + const collision_detection::AllowedCollisionMatrix* acm, bool self, bool cast = false) + : collisions_(collisions), contact_distance_(contact_distance), acm_(acm), self_(self), cast_(cast) { - return !collisions_.done && needsCollisionCheck(*cow0, *cow1, collisions_.fn, collisions_.acm, verbose_); } - virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, - int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, - int index1) = 0; -}; - -/** \brief addSingleResult of this struct is called each time the broadphase check indicates a collision. */ -struct DiscreteBroadphaseContactResultCallback : public BroadphaseContactResultCallback -{ - DiscreteBroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false) - : BroadphaseContactResultCallback(collisions, contact_distance, verbose) - { - } + ~BroadphaseContactResultCallback() = default; - btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, - int /*index0*/, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, - int /*index1*/) override + /** \brief This callback is used for each overlapping pair in a pair cache of the broadphase interface to check if a + * collision check should done for the pair. */ + // TODO: Add check for two objects attached to the same link + bool needsCollision(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) const { - if (cp.m_distance1 > static_cast(contact_distance_)) + if (cast_) { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not close enough for collision with " << cp.m_distance1); - return 0; + return !collisions_.done && !isOnlyKinematic(cow0, cow1) && !acmCheck(cow0->getName(), cow1->getName(), acm_); + } + else + { + return !collisions_.done && (self_ ? isOnlyKinematic(cow0, cow1) : !isOnlyKinematic(cow0, cow1)) && + !acmCheck(cow0->getName(), cow1->getName(), acm_); } - - return addDiscreteSingleResult(cp, colObj0Wrap, colObj1Wrap, collisions_); - } -}; - -/** \brief addSingleResult of this struct is called each time the broadphase check indicates a collision. */ -struct CastBroadphaseContactResultCallback : public BroadphaseContactResultCallback -{ - CastBroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false) - : BroadphaseContactResultCallback(collisions, contact_distance, verbose) - { } - btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, - int index0, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, - int index1) override + /** \brief This callback is used after btManifoldResult processed a collision result. */ + btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, + const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) { if (cp.m_distance1 > static_cast(contact_distance_)) { @@ -670,7 +608,14 @@ struct CastBroadphaseContactResultCallback : public BroadphaseContactResultCallb return 0; } - return addCastSingleResult(cp, colObj0Wrap, index0, colObj1Wrap, index1, collisions_); + if (cast_) + { + return addCastSingleResult(cp, colObj0Wrap, index0, colObj1Wrap, index1, collisions_); + } + else + { + return addDiscreteSingleResult(cp, colObj0Wrap, colObj1Wrap, collisions_); + } } }; @@ -736,57 +681,6 @@ struct TesseractBroadphaseBridgedManifoldResult : public btManifoldResult } }; -/** @brief Check a collision object not in the broadphase to the broadphase which may eventually be exposed. - * - * This is copied directly out of BulletWorld */ -struct TesseractSingleContactCallback : public btBroadphaseAabbCallback -{ - btCollisionObject* m_collisionObject; /**< @brief The bullet collision object */ - btCollisionDispatcher* m_dispatcher; /**< @brief The bullet collision dispatcher used for getting object to object - collison algorithm */ - const btDispatcherInfo& m_dispatch_info; /**< @brief The bullet collision dispatcher configuration information */ - btCollisionWorld::ContactResultCallback& m_resultCallback; - - TesseractSingleContactCallback(btCollisionObject* collisionObject, btCollisionDispatcher* dispatcher, - const btDispatcherInfo& dispatch_info, - btCollisionWorld::ContactResultCallback& resultCallback) - : m_collisionObject(collisionObject) - , m_dispatcher(dispatcher) - , m_dispatch_info(dispatch_info) - , m_resultCallback(resultCallback) - { - } - - bool process(const btBroadphaseProxy* proxy) override - { - btCollisionObject* collision_object = static_cast(proxy->m_clientObject); - if (collision_object == m_collisionObject) - return true; - - if (m_resultCallback.needsCollision(collision_object->getBroadphaseHandle())) - { - btCollisionObjectWrapper ob0(nullptr, m_collisionObject->getCollisionShape(), m_collisionObject, - m_collisionObject->getWorldTransform(), -1, -1); - btCollisionObjectWrapper ob1(nullptr, collision_object->getCollisionShape(), collision_object, - collision_object->getWorldTransform(), -1, -1); - - btCollisionAlgorithm* algorithm = m_dispatcher->findAlgorithm(&ob0, &ob1, nullptr, BT_CLOSEST_POINT_ALGORITHMS); - if (algorithm) - { - TesseractBridgedManifoldResult contact_point_result(&ob0, &ob1, m_resultCallback); - contact_point_result.m_closestPointDistanceThreshold = m_resultCallback.m_closestDistanceThreshold; - - // discrete collision detection query - algorithm->processCollision(&ob0, &ob1, m_dispatch_info, &contact_point_result); - - algorithm->~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm(algorithm); - } - } - return true; - } -}; - /** @brief A callback function that is called as part of the broadphase collision checking. * * If the AABB of two collision objects are overlapping the processOverlap method is called and they are checked for @@ -795,6 +689,8 @@ class TesseractCollisionPairCallback : public btOverlapCallback { const btDispatcherInfo& dispatch_info_; btCollisionDispatcher* dispatcher_; + + /** \brief Callback executed for each broadphase pair to check if needs collision */ BroadphaseContactResultCallback& results_callback_; public: @@ -819,7 +715,6 @@ class TesseractCollisionPairCallback : public btOverlapCallback const CollisionObjectWrapper* cow1 = static_cast(pair.m_pProxy1->m_clientObject); std::pair pair_names{ cow0->getName(), cow1->getName() }; - if (results_callback_.needsCollision(cow0, cow1)) { ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Processing " << cow0->getName() << " vs " @@ -864,9 +759,9 @@ btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom, * Currently continuous collision objects can only be checked against static objects. Continuous to Continuous * collision checking is currently not supports. TODO LEVI: Add support for Continuous to Continuous collision * checking. */ -inline void updateCollisionObjectFilters(const std::vector& active, CollisionObjectWrapper& cow, - bool continuous) +inline void updateCollisionObjectFilters(const std::vector& active, CollisionObjectWrapper& cow) { + // if not active make cow part of static if (!isLinkActive(active, cow.getName())) { cow.m_collisionFilterGroup = btBroadphaseProxy::StaticFilter; @@ -875,7 +770,7 @@ inline void updateCollisionObjectFilters(const std::vector& active, else { cow.m_collisionFilterGroup = btBroadphaseProxy::KinematicFilter; - cow.m_collisionFilterMask = btBroadphaseProxy::StaticFilter; + cow.m_collisionFilterMask = btBroadphaseProxy::KinematicFilter | btBroadphaseProxy::StaticFilter; } if (cow.getBroadphaseHandle()) @@ -888,132 +783,6 @@ inline void updateCollisionObjectFilters(const std::vector& active, << cow.m_collisionFilterMask); } -/** \brief Wrapper around constructing a CollisionObjectWrapper */ -inline CollisionObjectWrapperPtr createCollisionObject(const std::string& name, - const collision_detection::BodyType& type_id, - const std::vector& shapes, - const AlignedVector& shape_poses, - const std::vector& collision_object_types, - bool enabled = true) -{ - // dont add object that does not have geometry - if (shapes.empty() || shape_poses.empty() || (shapes.size() != shape_poses.size())) - { - ROS_DEBUG_NAMED("collision_detection.bullet", "ignoring link %s", name.c_str()); - return nullptr; - } - - CollisionObjectWrapperPtr new_cow( - new CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types)); - - new_cow->m_enabled = enabled; - new_cow->setContactProcessingThreshold(BULLET_DEFAULT_CONTACT_DISTANCE); - - ROS_DEBUG_NAMED("collision_detection.bullet", "Created collision object for link %s", new_cow->getName().c_str()); - return new_cow; -} - -// TODO: Unify with other createCollisionObject functions -/** \brief Wrapper around constructing a CollisionObjectWrapper for attached objects */ -inline CollisionObjectWrapperPtr createCollisionObject(const std::string& name, - const collision_detection::BodyType& type_id, - const std::vector& shapes, - const AlignedVector& shape_poses, - const std::vector& collision_object_types, - const std::set& touch_links, bool enabled = true) -{ - // dont add object that does not have geometry - if (shapes.empty() || shape_poses.empty() || (shapes.size() != shape_poses.size())) - { - ROS_DEBUG_NAMED("collision_detection.bullet", "ignoring link %s", name.c_str()); - return nullptr; - } - - CollisionObjectWrapperPtr new_cow( - new CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types, touch_links)); - - new_cow->m_enabled = enabled; - new_cow->setContactProcessingThreshold(BULLET_DEFAULT_CONTACT_DISTANCE); - - ROS_DEBUG_NAMED("collision_detection.bullet", "Created collision object for link %s", new_cow->getName().c_str()); - return new_cow; -} - -/** \brief Processes a contact after positive broadphase check */ -struct DiscreteCollisionCollector : public btCollisionWorld::ContactResultCallback -{ - ContactTestData& collisions_; - const CollisionObjectWrapperPtr cow_; - double contact_distance_; - bool verbose_; - - DiscreteCollisionCollector(ContactTestData& collisions, const CollisionObjectWrapperPtr& cow, double contact_distance, - bool verbose = false) - : collisions_(collisions), cow_(cow), contact_distance_(contact_distance), verbose_(verbose) - { - m_closestDistanceThreshold = static_cast(contact_distance); - m_collisionFilterGroup = cow->m_collisionFilterGroup; - m_collisionFilterMask = cow->m_collisionFilterMask; - } - - btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, - int /*index0*/, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, - int /*index1*/) override - { - if (cp.m_distance1 > static_cast(contact_distance_)) - { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not close enough for collision with " << cp.m_distance1); - return 0; - } - - return addDiscreteSingleResult(cp, colObj0Wrap, colObj1Wrap, collisions_); - } - - bool needsCollision(btBroadphaseProxy* proxy0) const override - { - return !collisions_.done && - needsCollisionCheck(*cow_, *(static_cast(proxy0->m_clientObject)), collisions_.fn, - collisions_.acm, verbose_); - } -}; - -struct CastCollisionCollector : public btCollisionWorld::ContactResultCallback -{ - ContactTestData& collisions_; - const CollisionObjectWrapperPtr cow_; - double contact_distance_; - bool verbose_; - - CastCollisionCollector(ContactTestData& collisions, const CollisionObjectWrapperPtr& cow, double contact_distance, - bool verbose = false) - : collisions_(collisions), cow_(cow), contact_distance_(contact_distance), verbose_(verbose) - { - m_closestDistanceThreshold = static_cast(contact_distance); - m_collisionFilterGroup = cow->m_collisionFilterGroup; - m_collisionFilterMask = cow->m_collisionFilterMask; - } - - btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, - int index0, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, - int index1) override - { - if (cp.m_distance1 > static_cast(contact_distance_)) - { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not close enough for collision with " << cp.m_distance1); - return 0; - } - - return addCastSingleResult(cp, colObj0Wrap, index0, colObj1Wrap, index1, collisions_); - } - - bool needsCollision(btBroadphaseProxy* proxy0) const override - { - return !collisions_.done && - needsCollisionCheck(*cow_, *(static_cast(proxy0->m_clientObject)), collisions_.fn, - collisions_.acm, verbose_); - } -}; - inline CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr& cow) { CollisionObjectWrapperPtr new_cow = cow->clone(); @@ -1119,7 +888,6 @@ inline void updateBroadphaseAABB(const CollisionObjectWrapperPtr& cow, btVector3 aabb_min, aabb_max; cow->getAABB(aabb_min, aabb_max); - // Update the broadphase aabb assert(cow->getBroadphaseHandle() != nullptr); broadphase->setAabb(cow->getBroadphaseHandle(), aabb_min, aabb_max, dispatcher.get()); } @@ -1154,7 +922,6 @@ inline void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr& cow, btVector3 aabb_min, aabb_max; cow->getAABB(aabb_min, aabb_max); - // Add the active collision object to the broadphase int type = cow->getCollisionShape()->getShapeType(); cow->setBroadphaseHandle(broadphase->createProxy(aabb_min, aabb_max, type, cow.get(), cow->m_collisionFilterGroup, cow->m_collisionFilterMask, dispatcher.get())); @@ -1179,9 +946,6 @@ struct BroadphaseFilterCallback : public btOverlapFilterCallback if (!cow1->m_enabled) return false; - if (acm_ && acmCheck(cow0->getName(), cow1->getName())) - return false; - if (cow0->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && cow1->getTypeID() == collision_detection::BodyType::ROBOT_LINK) if (cow0->m_touch_links.find(cow1->getName()) != cow0->m_touch_links.end()) @@ -1189,56 +953,18 @@ struct BroadphaseFilterCallback : public btOverlapFilterCallback if (cow1->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && cow0->getTypeID() == collision_detection::BodyType::ROBOT_LINK) - if (cow1->m_touch_links.find(cow0->getName()) == cow1->m_touch_links.end()) + if (cow1->m_touch_links.find(cow0->getName()) != cow1->m_touch_links.end()) + return false; + + if (cow0->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && + cow1->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED) + if (cow0->m_touch_links == cow1->m_touch_links) return false; - // TODO: Add check for two objects attached to the same link ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Broadphase pass " << cow0->getName() << " vs " << cow1->getName()); - return true; } - - bool acmCheck(const std::string& body_1, const std::string& body_2) const - { - collision_detection::AllowedCollision::Type allowed_type; - - if (!acm_) - { - if (acm_->getEntry(body_1, body_2, allowed_type)) - { - if (allowed_type == collision_detection::AllowedCollision::Type::NEVER) - { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", - "Not allowed entry in ACM found, collision check between " << body_1 << " and " - << body_2); - return false; - } - else - { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", - "Entry in ACM found, skipping collision check as allowed " << body_1 << " and " - << body_2); - return true; - } - } - else - { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No entry in ACM found, collision check between " - << body_1 << " and " << body_2); - return false; - } - } - else - { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No ACM, collision check between " << body_1 << " and " - << body_2); - return false; - } - } - - const collision_detection::AllowedCollisionMatrix* acm_{ nullptr }; }; } // namespace collision_detection_bullet -#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BULLET_UTILS_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h index 4acf787851..d6e2861398 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h @@ -18,8 +18,7 @@ /* Author: Levi Armstrong */ -#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_CONTACT_CHECKER_COMMON_H_ -#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_CONTACT_CHECKER_COMMON_H_ +#pragma once #include #include @@ -53,41 +52,6 @@ inline bool isLinkActive(const std::vector& active, const std::stri return active.empty() || (std::find(active.begin(), active.end(), name) != active.end()); } -/** - * @brief Determine if contact is allowed between two objects. - * @param name1 The name of the first object - * @param name2 The name of the second object - * @param acm The contact allowed function - * @param verbose If true print debug informaton - * @return True if contact is allowed between the two object, otherwise false. - */ -inline bool isContactAllowed(const std::string& name1, const std::string& name2, const IsContactAllowedFn& acm_fn, - const collision_detection::AllowedCollisionMatrix* acm, bool verbose = false) -{ - // do not distance check geoms part of the same object / link / attached body - if (name1 == name2) - return true; - - if (acm_fn != nullptr && acm_fn(name1, name2, acm)) - { - if (verbose) - { - ROS_DEBUG_NAMED("collision_detection.bullet", - "Collision between '%s' and '%s' is allowed. No contacts are computed.", name1.c_str(), - name2.c_str()); - } - return true; - } - - if (verbose) - { - ROS_DEBUG_NAMED("collision_detection.bullet", "Actually checking collisions between %s and %s", name1.c_str(), - name2.c_str()); - } - - return false; -} - /** \brief Stores a single contact result in the requested way. * \param found Indicates if a contact for this pair of objects has already been found * \return Pointer to the newly inserted contact */ @@ -247,42 +211,4 @@ inline int createConvexHull(AlignedVector& vertices, std::vecto return num_faces; } -inline bool allowedCollisionCheck(const std::string& body_1, const std::string& body_2, - const collision_detection::AllowedCollisionMatrix* acm) -{ - collision_detection::AllowedCollision::Type allowed_type; - - if (acm != nullptr) - { - if (acm->getEntry(body_1, body_2, allowed_type)) - { - if (allowed_type == collision_detection::AllowedCollision::Type::NEVER) - { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not allowed entry in ACM found, collision check between " - << body_1 << " and " << body_2); - return false; - } - else - { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Entry in ACM found, skipping collision check as allowed " - << body_1 << " and " << body_2); - return true; - } - } - else - { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No entry in ACM found, collision check between " - << body_1 << " and " << body_2); - return false; - } - } - else - { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No ACM, collision check between " << body_1 << " and " - << body_2); - return false; - } -} } // namespace collision_detection_bullet - -#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_CONTACT_CHECKER_COMMON_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h index c192b86df9..7eb2e43f5a 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h @@ -18,8 +18,7 @@ /* Author: Levi Armstrong */ -#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_ROS_BULLET_UTILS_H_ -#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_ROS_BULLET_UTILS_H_ +#pragma once #include #include @@ -106,4 +105,3 @@ inline Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose& pose) return result; } } // namespace collision_detection_bullet -#endif // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_ROS_BULLET_UTILS_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h index 513bea0e16..fec946f357 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h @@ -34,23 +34,18 @@ /* Author: Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALLOCATOR_BULLET_H_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALLOCATOR_BULLET_H_ +#pragma once #include -#include -#include +#include namespace collision_detection { /** \brief An allocator for Bullet collision detectors */ class CollisionDetectorAllocatorBullet - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_world_bullet.cpp + static const std::string NAME; // defined in collision_env_bullet.cpp }; } // namespace collision_detection - -#endif // MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALLOCATOR_BULLET_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h index 76182e582c..46c28ae63c 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h @@ -34,8 +34,7 @@ /* Author: Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_DETECTOR_BULLET_PLUGIN_LOADER_H_ -#define MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_DETECTOR_BULLET_PLUGIN_LOADER_H_ +#pragma once #include #include @@ -48,4 +47,3 @@ class CollisionDetectorBtPluginLoader : public CollisionPlugin bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const override; }; } -#endif // MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_DETECTOR_BULLET_PLUGIN_LOADER_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h new file mode 100644 index 0000000000..31a0a9e070 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h @@ -0,0 +1,148 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#pragma once + +#include +#include +#include + +namespace collision_detection +{ +/** \brief */ +class CollisionEnvBullet : public CollisionEnv +{ +public: + CollisionEnvBullet() = delete; + + CollisionEnvBullet(const robot_model::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); + + CollisionEnvBullet(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, + double scale = 1.0); + + CollisionEnvBullet(const CollisionEnvBullet& other, const WorldPtr& world); + + ~CollisionEnvBullet() override; + + CollisionEnvBullet(CollisionEnvBullet&) = delete; + + virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const override; + + virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const override; + + virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const override; + + virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const override; + + void setWorld(const WorldPtr& world) override; + +protected: + /** \brief Updates the poses of the objects in the manager according to given robot state */ + void updateTransformsFromState(const robot_state::RobotState& state, + const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const; + + /** \brief Updates the collision objects saved in the manager to reflect a new padding or scaling of the robot links + */ + void updatedPaddingOrScaling(const std::vector& links) override; + + /** \brief All of the attached objects in the robot state are wrapped into bullet collision objects */ + void addAttachedOjects(const robot_state::RobotState& state, + std::vector& cows) const; + + /** \brief Bundles the different checkSelfCollision functions into a single function */ + void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const; + + void checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const AllowedCollisionMatrix* acm) const; + + void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; + + /** \brief Construts a bullet collision object out of a robot link */ + void addLinkAsCollisionObject(const urdf::LinkSharedPtr& link); + + /** \brief Handles self collision checks */ + mutable collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_{ + new collision_detection_bullet::BulletDiscreteBVHManager() + }; + + /** \brief Handles continuous robot world collision checks */ + mutable collision_detection_bullet::BulletCastBVHManagerPtr manager_CCD_{ + new collision_detection_bullet::BulletCastBVHManager() + }; + + /** \brief Adds a world object to the collision managers */ + void addToManager(const World::Object* obj); + + /** \brief Updates a managed collision object with its world representation. + * + * We have three cases: 1) the object is part of the manager and not of world --> delete it + * 2) the object is not in the manager, therefore register to manager, + * 3) the object is in the manager then delete and add the modified */ + void updateManagedObject(const std::string& id); + + /** \brief The active links where active refers to the group which can collide with everything */ + std::vector active_; + +private: + /** \brief Callback function executed for each change to the world environment */ + void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); + + World::ObserverHandle observer_handle_; +}; +} diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h deleted file mode 100644 index 9299a0511d..0000000000 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bullet.h +++ /dev/null @@ -1,122 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Jens Petit - * 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 the copyright holder 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. - *********************************************************************/ - -/* Author: Jens Petit */ - -#ifndef MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_ROBOT_BULLET_H_ -#define MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_ROBOT_BULLET_H_ - -#include -#include - -namespace collision_detection -{ -class CollisionRobotBullet : public CollisionRobot -{ - friend class CollisionWorldBullet; - -public: - CollisionRobotBullet(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); - - CollisionRobotBullet(const CollisionRobotBullet& other); - - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const override; - - void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override; - - void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override; - -protected: - /** \brief Updates the poses of the objects in the manager according to given robot state */ - void updateTransformsFromState(const robot_state::RobotState& state, - const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const; - - /** \brief Updates the collision objects saved in the manager to reflect a new padding or scaling of the robot links - */ - void updatedPaddingOrScaling(const std::vector& links) override; - - /** \brief All of the attached objects in the robot state are wrapped into bullet collision objects */ - void addAttachedOjects(const robot_state::RobotState& state, - std::vector& cows) const; - - /** \brief Bundles the different checkSelfCollision functions into a single function */ - void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const; - - /** \brief Bundles the different continuous checkSelfCollision functions into a single function */ - void checkSelfCollisionCCDHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix* acm) const; - - /** \brief Bundles the different checkOtherCollision functions into a single function */ - void checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, const AllowedCollisionMatrix* acm) const; - - /** \brief Construts a bullet collision object out of a robot link */ - void addLinkAsCollisionObjectWrapper(const urdf::LinkSharedPtr& link); - - /** \brief Handles all self collision checks */ - collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_; - - /** \brief Active links corresponding to all links of the robot */ - std::vector active_; -}; -} // namespace collision_detection - -#endif // MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_ROBOT_BULLET_H_ diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h deleted file mode 100644 index 06b35a1e04..0000000000 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bullet.h +++ /dev/null @@ -1,118 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Jens Petit - * 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 the copyright holder 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. - *********************************************************************/ - -/* Author: Jens Petit */ - -#ifndef MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_WORLD_BULLET_H_ -#define MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_WORLD_BULLET_H_ - -#include -#include -#include -#include -#include - -namespace collision_detection -{ -class CollisionWorldBullet : public CollisionWorld -{ -public: - CollisionWorldBullet(); - - explicit CollisionWorldBullet(const WorldPtr& world); - - CollisionWorldBullet(const CollisionWorldBullet& other, const WorldPtr& world); - - ~CollisionWorldBullet() override; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const override; - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const override; - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, - const AllowedCollisionMatrix& acm) const override; - - void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override; - - void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override; - - void setWorld(const WorldPtr& world) override; - -protected: - /** \brief Bundles the different checkWorldCollision functions into a single function */ - void checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, - const AllowedCollisionMatrix* acm) const; - - /** \brief Bundles the different checkRobotCollision functions into a single function */ - void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; - - /** \brief Bundles the different continuous checkRobotCollision functions into a single function */ - void checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix* acm) const; - - /** \brief Adds a world object to the collision managers */ - void addToManager(const World::Object* obj); - - /** \brief Updates a managed collision object with its world representation. - * - * We have three cases: 1) the object is part of the manager and not of world --> delete it - * 2) the object is not in the manager, therefore register to manager, - * 3) the object is in the manager then delete and add the modified */ - void updateManagedObject(const std::string& id); - - /** \brief Handles all discrete collision checks */ - collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_; - - /** \brief Handles all continuous collision checks */ - collision_detection_bullet::BulletCastBVHManagerPtr bt_manager_CCD_; - -private: - /** \brief Callback function executed for each change to the world environment */ - void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); - - World::ObserverHandle observer_handle_; -}; -} // namespace collision_detection - -#endif // MOVEIT_COLLISION_DETECTION_BULLET_COLLISION_WORLD_BULLET_H_ diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp new file mode 100644 index 0000000000..800fe90fd0 --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp @@ -0,0 +1,168 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * 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. + * + * 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. + *********************************************************************/ + +/* Authors: Levi Armstrong, Jens Petit */ + +#include +#include +#include + +namespace collision_detection_bullet +{ +BulletBVHManager::BulletBVHManager() +{ + dispatcher_.reset(new btCollisionDispatcher(&coll_config_)); + + dispatcher_->registerCollisionCreateFunc( + BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE, + coll_config_.getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE, CONVEX_SHAPE_PROXYTYPE)); + + dispatcher_->setDispatcherFlags(dispatcher_->getDispatcherFlags() & + ~btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD); + + broadphase_.reset(new btDbvtBroadphase()); + + broadphase_->getOverlappingPairCache()->setOverlapFilterCallback(&filter_callback_); + + contact_distance_ = BULLET_DEFAULT_CONTACT_DISTANCE; +} + +BulletBVHManager::~BulletBVHManager() +{ + // clean up remaining objects + for (const std::pair& cow : link2cow_) + removeCollisionObjectFromBroadphase(cow.second, broadphase_, dispatcher_); +} + +bool BulletBVHManager::hasCollisionObject(const std::string& name) const +{ + return (link2cow_.find(name) != link2cow_.end()); +} + +bool BulletBVHManager::removeCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + CollisionObjectWrapperPtr& cow = it->second; + removeCollisionObjectFromBroadphase(cow, broadphase_, dispatcher_); + link2cow_.erase(name); + return true; + } + + return false; +} + +bool BulletBVHManager::enableCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + it->second->m_enabled = true; + return true; + } + + return false; +} + +bool BulletBVHManager::disableCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + it->second->m_enabled = false; + return true; + } + + return false; +} + +void BulletBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) +{ + // TODO(j-petit): Find a way to remove this check. Need to store information in CollisionEnv transforms with geometry + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + CollisionObjectWrapperPtr& cow = it->second; + btTransform tf = convertEigenToBt(pose); + cow->setWorldTransform(tf); + + // Now update Broadphase AABB (See BulletWorld updateSingleAabb function) + if (cow->getBroadphaseHandle()) + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } +} + +void BulletBVHManager::setActiveCollisionObjects(const std::vector& names) +{ + active_ = names; + + // Now need to update the broadphase with correct aabb + for (std::pair& co : link2cow_) + { + CollisionObjectWrapperPtr& cow = co.second; + updateCollisionObjectFilters(active_, *cow); + + // The broadphase tree structure has to be updated, therefore remove and add is necessary + removeCollisionObjectFromBroadphase(cow, broadphase_, dispatcher_); + addCollisionObjectToBroadphase(cow, broadphase_, dispatcher_); + } +} + +const std::vector& BulletBVHManager::getActiveCollisionObjects() const +{ + return active_; +} + +void BulletBVHManager::setContactDistanceThreshold(double contact_distance) +{ + contact_distance_ = contact_distance; + + for (std::pair& co : link2cow_) + { + CollisionObjectWrapperPtr& cow = co.second; + cow->setContactProcessingThreshold(static_cast(contact_distance)); + if (cow->getBroadphaseHandle()) + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } +} + +double BulletBVHManager::getContactDistanceThreshold() const +{ + return contact_distance_; +} + +const std::map& BulletBVHManager::getCollisionObjects() const +{ + return link2cow_; +} + +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp index 83da7d1c3d..e047be3324 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp @@ -29,7 +29,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Levi Armstrong */ +/* Author: Levi Armstrong, Jens Petit */ #include #include @@ -37,35 +37,6 @@ namespace collision_detection_bullet { -BulletCastBVHManager::BulletCastBVHManager() -{ - dispatcher_.reset(new btCollisionDispatcher(&coll_config_)); - - dispatcher_->registerCollisionCreateFunc( - BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE, - coll_config_.getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE, CONVEX_SHAPE_PROXYTYPE)); - - dispatcher_->setDispatcherFlags(dispatcher_->getDispatcherFlags() & - ~btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD); - - broadphase_.reset(new btDbvtBroadphase()); - - broadphase_->getOverlappingPairCache()->setOverlapFilterCallback(&filter_callback_); - - contact_distance_ = 0; -} - -BulletCastBVHManager::~BulletCastBVHManager() -{ - // clean up remaining objects - for (const std::pair& cow : link2cow_) - removeCollisionObjectFromBroadphase(cow.second, broadphase_, dispatcher_); - - // clean up remaining objects - for (const std::pair& cow : link2castcow_) - removeCollisionObjectFromBroadphase(cow.second, broadphase_, dispatcher_); -} - BulletCastBVHManagerPtr BulletCastBVHManager::clone() const { BulletCastBVHManagerPtr manager(new BulletCastBVHManager()); @@ -84,121 +55,17 @@ BulletCastBVHManagerPtr BulletCastBVHManager::clone() const manager->setActiveCollisionObjects(active_); manager->setContactDistanceThreshold(contact_distance_); - manager->setIsContactAllowedFn(fn_); return manager; } -bool BulletCastBVHManager::addCollisionObject(const std::string& name, const collision_detection::BodyType& mask_id, - const std::vector& shapes, - const AlignedVector& shape_poses, - const std::vector& collision_object_types, - bool enabled) -{ - CollisionObjectWrapperPtr new_cow = - createCollisionObject(name, mask_id, shapes, shape_poses, collision_object_types, enabled); - if (new_cow != nullptr) - { - addCollisionObject(new_cow); - return true; - } - - return false; -} - -bool BulletCastBVHManager::hasCollisionObject(const std::string& name) const -{ - return (link2cow_.find(name) != link2cow_.end()); -} - -bool BulletCastBVHManager::removeCollisionObject(const std::string& name) -{ - auto it = link2cow_.find(name); - if (it != link2cow_.end()) - { - CollisionObjectWrapperPtr& cow1 = it->second; - removeCollisionObjectFromBroadphase(cow1, broadphase_, dispatcher_); - link2cow_.erase(name); - - CollisionObjectWrapperPtr& cow2 = link2castcow_[name]; - removeCollisionObjectFromBroadphase(cow2, broadphase_, dispatcher_); - link2castcow_.erase(name); - - return true; - } - - return false; -} - -bool BulletCastBVHManager::enableCollisionObject(const std::string& name) -{ - auto it = link2cow_.find(name); - if (it != link2cow_.end()) - { - it->second->m_enabled = true; - link2castcow_[name]->m_enabled = true; - return true; - } - - return false; -} - -bool BulletCastBVHManager::disableCollisionObject(const std::string& name) -{ - auto it = link2cow_.find(name); - if (it != link2cow_.end()) - { - it->second->m_enabled = false; - link2castcow_[name]->m_enabled = false; - return true; - } - - return false; -} - -void BulletCastBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) +void BulletCastBVHManager::setCastCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose1, + const Eigen::Isometry3d& pose2) { // TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with // geometry auto it = link2cow_.find(name); if (it != link2cow_.end()) - { - CollisionObjectWrapperPtr& cow = it->second; - btTransform tf = convertEigenToBt(pose); - cow->setWorldTransform(tf); - link2castcow_[name]->setWorldTransform(tf); - - // Now update Broadphase AABB (See BulletWorld updateSingleAabb function) - if (cow->getBroadphaseHandle()) - updateBroadphaseAABB(cow, broadphase_, dispatcher_); - } -} - -void BulletCastBVHManager::setCollisionObjectsTransform(const std::vector& names, - const AlignedVector& poses) -{ - assert(names.size() == poses.size()); - for (size_t i = 0u; i < names.size(); ++i) - { - setCollisionObjectsTransform(names[i], poses[i]); - } -} - -void BulletCastBVHManager::setCollisionObjectsTransform(const AlignedMap& transforms) -{ - for (const std::pair& transform : transforms) - { - setCollisionObjectsTransform(transform.first, transform.second); - } -} - -void BulletCastBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose1, - const Eigen::Isometry3d& pose2) -{ - // TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with - // geometry - auto it = link2castcow_.find(name); - if (it != link2castcow_.end()) { CollisionObjectWrapperPtr& cow = it->second; assert(cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter); @@ -263,148 +130,42 @@ void BulletCastBVHManager::setCollisionObjectsTransform(const std::string& name, } } -void BulletCastBVHManager::setCollisionObjectsTransform(const std::vector& names, - const AlignedVector& pose1, - const AlignedVector& pose2) -{ - if (names.size() != pose1.size() || names.size() != pose2.size()) - { - ROS_ERROR_NAMED("collision_detection.bullet", "The number of poses does not fit the number of collision objects."); - } - - for (size_t i = 0u; i < names.size(); ++i) - setCollisionObjectsTransform(names[i], pose1[i], pose2[i]); -} - -void BulletCastBVHManager::setCollisionObjectsTransform(const AlignedMap& pose1, - const AlignedMap& pose2) -{ - assert(pose1.size() == pose2.size()); - auto it1 = pose1.begin(); - auto it2 = pose2.begin(); - while (it1 != pose1.end()) - { - assert(pose1.find(it1->first) != pose2.end()); - setCollisionObjectsTransform(it1->first, it1->second, it2->second); - std::advance(it1, 1); - std::advance(it2, 1); - } -} - -void BulletCastBVHManager::setActiveCollisionObjects(const std::vector& names) -{ - active_ = names; - - // Now need to update the broadphase with correct aabb - for (std::pair& co : link2cow_) - { - CollisionObjectWrapperPtr& cow = co.second; - - // Need to check if a collision object is still active, select cast or normal to broadphase - if (isLinkActive(active_, cow->getName())) - { - updateCollisionObjectFilters(active_, *cow, false); - CollisionObjectWrapperPtr& active_cow = link2castcow_[cow->getName()]; - updateCollisionObjectFilters(active_, *active_cow, true); - - removeCollisionObjectFromBroadphase(active_cow, broadphase_, dispatcher_); - removeCollisionObjectFromBroadphase(cow, broadphase_, dispatcher_); - addCollisionObjectToBroadphase(active_cow, broadphase_, dispatcher_); - } - else - { - updateCollisionObjectFilters(active_, *cow, false); - CollisionObjectWrapperPtr& cast_cow = link2castcow_[cow->getName()]; - updateCollisionObjectFilters(active_, *cast_cow, true); - - removeCollisionObjectFromBroadphase(cast_cow, broadphase_, dispatcher_); - removeCollisionObjectFromBroadphase(cow, broadphase_, dispatcher_); - addCollisionObjectToBroadphase(cow, broadphase_, dispatcher_); - } - } -} - -const std::vector& BulletCastBVHManager::getActiveCollisionObjects() const -{ - return active_; -} - -void BulletCastBVHManager::setContactDistanceThreshold(double contact_distance) -{ - contact_distance_ = contact_distance; - - for (std::pair& co : link2cow_) - { - CollisionObjectWrapperPtr& cow = co.second; - cow->setContactProcessingThreshold(static_cast(contact_distance)); - if (cow->getBroadphaseHandle()) - updateBroadphaseAABB(cow, broadphase_, dispatcher_); - } - - for (std::pair& co : link2castcow_) - { - CollisionObjectWrapperPtr& cow = co.second; - cow->setContactProcessingThreshold(static_cast(contact_distance)); - if (cow->getBroadphaseHandle()) - updateBroadphaseAABB(cow, broadphase_, dispatcher_); - } -} - -double BulletCastBVHManager::getContactDistanceThreshold() const -{ - return contact_distance_; -} - -void BulletCastBVHManager::setIsContactAllowedFn(IsContactAllowedFn fn) -{ - fn_ = std::move(fn); -} - -IsContactAllowedFn BulletCastBVHManager::getIsContactAllowedFn() const -{ - return fn_; -} - void BulletCastBVHManager::contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, - const collision_detection::AllowedCollisionMatrix* acm) + const collision_detection::AllowedCollisionMatrix* acm, bool self = false) { - ContactTestData cdata(active_, contact_distance_, fn_, collisions, req, acm); + ContactTestData cdata(active_, contact_distance_, collisions, req); broadphase_->calculateOverlappingPairs(dispatcher_.get()); btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); - ROS_DEBUG_STREAM_NAMED("collision_detection.bulelt", "Number overlapping candidates " + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Number overlapping candidates " << pair_cache->getNumOverlappingPairs()); - CastBroadphaseContactResultCallback cc(cdata, contact_distance_); + BroadphaseContactResultCallback cc(cdata, contact_distance_, acm, false, true); TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get()); } void BulletCastBVHManager::addCollisionObject(const CollisionObjectWrapperPtr& cow) { - link2cow_[cow->getName()] = cow; - CollisionObjectWrapperPtr cast_cow = makeCastCollisionObject(cow); - link2castcow_[cast_cow->getName()] = cast_cow; + std::string name = cow->getName(); + if (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) + { + CollisionObjectWrapperPtr cast_cow = makeCastCollisionObject(cow); + link2cow_[name] = cast_cow; + } + else + { + link2cow_[name] = cow; + } - const CollisionObjectWrapperPtr& selected_cow = - (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) ? cast_cow : cow; btVector3 aabb_min, aabb_max; - selected_cow->getAABB(aabb_min, aabb_max); + link2cow_[name]->getAABB(aabb_min, aabb_max); - int type = selected_cow->getCollisionShape()->getShapeType(); - selected_cow->setBroadphaseHandle(broadphase_->createProxy(aabb_min, aabb_max, type, selected_cow.get(), - selected_cow->m_collisionFilterGroup, - selected_cow->m_collisionFilterMask, dispatcher_.get())); + int type = link2cow_[name]->getCollisionShape()->getShapeType(); + link2cow_[name]->setBroadphaseHandle( + broadphase_->createProxy(aabb_min, aabb_max, type, link2cow_[name].get(), link2cow_[name]->m_collisionFilterGroup, + link2cow_[name]->m_collisionFilterMask, dispatcher_.get())); } -void BulletCastBVHManager::contactTest(const CollisionObjectWrapperPtr& cow, ContactTestData& collisions) -{ - btVector3 aabb_min, aabb_max; - cow->getAABB(aabb_min, aabb_max); - - CastCollisionCollector cc(collisions, cow, static_cast(cow->getContactProcessingThreshold())); - TesseractSingleContactCallback contact_cb(cow.get(), dispatcher_.get(), dispatch_info_, cc); - broadphase_->aabbTest(aabb_min, aabb_max, contact_cb); -} } // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp index 73640cf3a8..68eda45091 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp @@ -29,41 +29,12 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Levi Armstrong */ - -#include +/* Author: Levi Armstrong, Jens Petit */ #include "moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h" namespace collision_detection_bullet { -BulletDiscreteBVHManager::BulletDiscreteBVHManager() -{ - dispatcher_.reset(new btCollisionDispatcher(&coll_config_)); - - dispatcher_->registerCollisionCreateFunc( - BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE, - coll_config_.getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE, CONVEX_SHAPE_PROXYTYPE)); - - dispatcher_->setDispatcherFlags(dispatcher_->getDispatcherFlags() & - ~btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD); - - broadphase_.reset(new btDbvtBroadphase()); - - broadphase_->getOverlappingPairCache()->setOverlapFilterCallback(&filter_callback_); - - contact_distance_ = 0; -} - -BulletDiscreteBVHManager::~BulletDiscreteBVHManager() -{ - // clean up remaining objects - for (std::pair& cow : link2cow_) - { - removeCollisionObjectFromBroadphase(cow.second, broadphase_, dispatcher_); - } -} - BulletDiscreteBVHManagerPtr BulletDiscreteBVHManager::clone() const { BulletDiscreteBVHManagerPtr manager(new BulletDiscreteBVHManager()); @@ -80,171 +51,17 @@ BulletDiscreteBVHManagerPtr BulletDiscreteBVHManager::clone() const manager->addCollisionObject(new_cow); } + manager->setActiveCollisionObjects(active_); manager->setContactDistanceThreshold(contact_distance_); - manager->setIsContactAllowedFn(fn_); return manager; } -bool BulletDiscreteBVHManager::addCollisionObject(const std::string& name, const collision_detection::BodyType& mask_id, - const std::vector& shapes, - const AlignedVector& shape_poses, - const std::vector& collision_object_types, - bool enabled) -{ - CollisionObjectWrapperPtr new_cow = - createCollisionObject(name, mask_id, shapes, shape_poses, collision_object_types, enabled); - if (new_cow != nullptr) - { - addCollisionObject(new_cow); - return true; - } - return false; -} - -bool BulletDiscreteBVHManager::hasCollisionObject(const std::string& name) const -{ - return (link2cow_.find(name) != link2cow_.end()); -} - -bool BulletDiscreteBVHManager::removeCollisionObject(const std::string& name) -{ - auto it = link2cow_.find(name); // Levi TODO: Should these check be removed? - if (it != link2cow_.end()) - { - removeCollisionObjectFromBroadphase(it->second, broadphase_, dispatcher_); - link2cow_.erase(name); - return true; - } - - return false; -} - -bool BulletDiscreteBVHManager::enableCollisionObject(const std::string& name) -{ - auto it = link2cow_.find(name); // Levi TODO: Should these check be removed? - if (it != link2cow_.end()) - { - it->second->m_enabled = true; - return true; - } - return false; -} - -bool BulletDiscreteBVHManager::disableCollisionObject(const std::string& name) -{ - auto it = link2cow_.find(name); // Levi TODO: Should these check be removed? - if (it != link2cow_.end()) - { - it->second->m_enabled = false; - return true; - } - return false; -} - -void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) -{ - // TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with - // geometry - auto it = link2cow_.find(name); - if (it != link2cow_.end()) - { - CollisionObjectWrapperPtr& cow = it->second; - cow->setWorldTransform(convertEigenToBt(pose)); - - // Update Collision Object Broadphase AABB - updateBroadphaseAABB(cow, broadphase_, dispatcher_); - } -} - -void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::string& name, const btTransform& pose) -{ - // TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with - // geometry - auto it = link2cow_.find(name); - if (it != link2cow_.end()) - { - CollisionObjectWrapperPtr& cow = it->second; - cow->setWorldTransform(pose); - - // Update Collision Object Broadphase AABB - updateBroadphaseAABB(cow, broadphase_, dispatcher_); - } -} - -void BulletDiscreteBVHManager::setCollisionObjectsTransform(const std::vector& names, - const AlignedVector& poses) -{ - assert(names.size() == poses.size()); - for (size_t i = 0u; i < names.size(); ++i) - { - setCollisionObjectsTransform(names[i], poses[i]); - } -} - -void BulletDiscreteBVHManager::setCollisionObjectsTransform( - const AlignedMap& transforms) -{ - for (const std::pair& transform : transforms) - { - setCollisionObjectsTransform(transform.first, transform.second); - } -} - -void BulletDiscreteBVHManager::setActiveCollisionObjects(const std::vector& names) -{ - active_ = names; - - // Now need to update the broadphase with correct aabb - for (std::pair& co : link2cow_) - { - CollisionObjectWrapperPtr& cow = co.second; - updateCollisionObjectFilters(active_, *cow, false); - - // The broadphase tree structure has to be updated, therefore remove and add is necessary - removeCollisionObjectFromBroadphase(cow, broadphase_, dispatcher_); - addCollisionObjectToBroadphase(cow, broadphase_, dispatcher_); - } -} - -const std::vector& BulletDiscreteBVHManager::getActiveCollisionObjects() const -{ - return active_; -} - -void BulletDiscreteBVHManager::setContactDistanceThreshold(double contact_distance) -{ - contact_distance_ = contact_distance; - - for (std::pair& co : link2cow_) - { - CollisionObjectWrapperPtr& cow = co.second; - cow->setContactProcessingThreshold(static_cast(contact_distance)); - assert(cow->getBroadphaseHandle() != nullptr); - updateBroadphaseAABB(cow, broadphase_, dispatcher_); - } -} - -double BulletDiscreteBVHManager::getContactDistanceThreshold() const -{ - return contact_distance_; -} - -void BulletDiscreteBVHManager::setIsContactAllowedFn(IsContactAllowedFn fn) -{ - fn_ = std::move(fn); -} - -IsContactAllowedFn BulletDiscreteBVHManager::getIsContactAllowedFn() const -{ - return fn_; -} - void BulletDiscreteBVHManager::contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, - const collision_detection::AllowedCollisionMatrix* acm) + const collision_detection::AllowedCollisionMatrix* acm, bool self) { - ContactTestData cdata(active_, contact_distance_, fn_, collisions, req, acm); + ContactTestData cdata(active_, contact_distance_, collisions, req); broadphase_->calculateOverlappingPairs(dispatcher_.get()); btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); @@ -252,7 +69,7 @@ void BulletDiscreteBVHManager::contactTest(collision_detection::CollisionResult& ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Num overlapping candidates " << pair_cache->getNumOverlappingPairs()); - DiscreteBroadphaseContactResultCallback cc(cdata, contact_distance_); + BroadphaseContactResultCallback cc(cdata, contact_distance_, acm, self); TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get()); @@ -261,51 +78,10 @@ void BulletDiscreteBVHManager::contactTest(collision_detection::CollisionResult& << " collisions"); } -void BulletDiscreteBVHManager::contactTest( - collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, - const collision_detection::AllowedCollisionMatrix* acm, - const std::vector cows_external) -{ - ContactTestData cdata(active_, contact_distance_, fn_, collisions, req, acm); - - broadphase_->calculateOverlappingPairs(dispatcher_.get()); - btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); - - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Num overlapping candidates " - << pair_cache->getNumOverlappingPairs()); - - DiscreteBroadphaseContactResultCallback cc(cdata, contact_distance_); - TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); - pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get()); - - for (const CollisionObjectWrapperPtr& cow : cows_external) - { - contactTest(cow, cdata); - } - - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", (collisions.collision ? "In" : "No") << " collision with " - << collisions.contact_count - << " collisions"); -} - void BulletDiscreteBVHManager::addCollisionObject(const CollisionObjectWrapperPtr& cow) { link2cow_[cow->getName()] = cow; addCollisionObjectToBroadphase(cow, broadphase_, dispatcher_); } -const std::map& BulletDiscreteBVHManager::getCollisionObjects() const -{ - return link2cow_; -} - -void BulletDiscreteBVHManager::contactTest(const CollisionObjectWrapperPtr& cow, ContactTestData& collisions) -{ - btVector3 min_aabb, max_aabb; - cow->getAABB(min_aabb, max_aabb); - - DiscreteCollisionCollector cc(collisions, cow, static_cast(cow->getContactProcessingThreshold())); - TesseractSingleContactCallback contact_cb(cow.get(), dispatcher_.get(), dispatch_info_, cc); - broadphase_->aabbTest(min_aabb, max_aabb, contact_cb); -} } // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp index c8704a8a8e..2617787cb4 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp @@ -264,23 +264,33 @@ btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom, CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, const std::vector& shapes, const AlignedVector& shape_poses, - const std::vector& collision_object_types) + const std::vector& collision_object_types, + bool active) : m_name(name) , m_type_id(type_id) , m_shapes(shapes) , m_shape_poses(shape_poses) , m_collision_object_types(collision_object_types) { - this->setContactProcessingThreshold(0); - assert(!shapes.empty()); - assert(!shape_poses.empty()); - assert(!collision_object_types.empty()); + if (shapes.empty() || shape_poses.empty() || (shapes.size() != shape_poses.size() || collision_object_types.empty() || + shapes.size() != collision_object_types.size())) + { + throw std::exception(); + } + + this->setContactProcessingThreshold(BULLET_DEFAULT_CONTACT_DISTANCE); assert(!name.empty()); - assert(shapes.size() == shape_poses.size()); - assert(shapes.size() == collision_object_types.size()); - m_collisionFilterGroup = btBroadphaseProxy::KinematicFilter; - m_collisionFilterMask = btBroadphaseProxy::KinematicFilter; + if (active) + { + m_collisionFilterGroup = btBroadphaseProxy::KinematicFilter; + m_collisionFilterMask = btBroadphaseProxy::KinematicFilter | btBroadphaseProxy::StaticFilter; + } + else + { + m_collisionFilterGroup = btBroadphaseProxy::StaticFilter; + m_collisionFilterMask = btBroadphaseProxy::KinematicFilter; + } if (shapes.size() == 1) { @@ -321,7 +331,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const co const AlignedVector& shape_poses, const std::vector& collision_object_types, const std::set& touch_links) - : CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types) + : CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types, true) { m_touch_links = touch_links; } @@ -338,6 +348,5 @@ CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const co , m_collision_object_types(collision_object_types) , m_data(data) { - this->setContactProcessingThreshold(0); } } // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp new file mode 100644 index 0000000000..f42bc09b6e --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp @@ -0,0 +1,432 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include +#include +#include + +namespace collision_detection +{ +const std::string CollisionDetectorAllocatorBullet::NAME("Bullet"); +const double MAX_DISTANCE_MARGIN = 99; + +CollisionEnvBullet::CollisionEnvBullet(const robot_model::RobotModelConstPtr& model, double padding, double scale) + : CollisionEnv(model, padding, scale) +{ + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + + for (const std::pair& link : robot_model_->getURDF()->links_) + { + addLinkAsCollisionObject(link.second); + } +} + +CollisionEnvBullet::CollisionEnvBullet(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, + double padding, double scale) + : CollisionEnv(model, world, padding, scale) +{ + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + + for (const std::pair& link : robot_model_->getURDF()->links_) + { + addLinkAsCollisionObject(link.second); + } + + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +CollisionEnvBullet::CollisionEnvBullet(const CollisionEnvBullet& other, const WorldPtr& world) + : CollisionEnv(other, world) +{ + // TODO(j-petit): Verify this constructor + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + + for (const std::pair& link : other.robot_model_->getURDF()->links_) + { + addLinkAsCollisionObject(link.second); + } +} + +CollisionEnvBullet::~CollisionEnvBullet() +{ + getWorld()->removeObserver(observer_handle_); +} + +void CollisionEnvBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const +{ + checkSelfCollisionHelper(req, res, state, nullptr); +} + +void CollisionEnvBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + checkSelfCollisionHelper(req, res, state, &acm); +} + +void CollisionEnvBullet::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + std::vector cows; + addAttachedOjects(state, cows); + + if (req.distance) + { + manager_->setContactDistanceThreshold(MAX_DISTANCE_MARGIN); + } + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : cows) + { + manager_->addCollisionObject(cow); + manager_->setCollisionObjectsTransform( + cow->getName(), state.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0]); + } + + // updating link positions with the current robot state + for (const std::string& link : active_) + { + manager_->setCollisionObjectsTransform(link, state.getCollisionBodyTransform(link, 0)); + } + + manager_->contactTest(res, req, acm, true); + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : cows) + { + manager_->removeCollisionObject(cow->getName()); + } +} + +void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state) const +{ + checkRobotCollisionHelper(req, res, state, nullptr); +} + +void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + checkRobotCollisionHelper(req, res, state, &acm); +} + +void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2) const +{ + checkRobotCollisionHelperCCD(req, res, state1, state2, nullptr); +} + +void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix& acm) const +{ + checkRobotCollisionHelperCCD(req, res, state1, state2, &acm); +} + +void CollisionEnvBullet::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + if (req.distance) + { + manager_->setContactDistanceThreshold(MAX_DISTANCE_MARGIN); + } + + std::vector attached_cows; + addAttachedOjects(state, attached_cows); + updateTransformsFromState(state, manager_); + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows) + { + manager_->addCollisionObject(cow); + manager_->setCollisionObjectsTransform( + cow->getName(), state.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0]); + } + + manager_->contactTest(res, req, acm, false); + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows) + { + manager_->removeCollisionObject(cow->getName()); + } +} + +void CollisionEnvBullet::checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, + const robot_state::RobotState& state1, + const robot_state::RobotState& state2, + const AllowedCollisionMatrix* acm) const +{ + std::vector attached_cows; + addAttachedOjects(state1, attached_cows); + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows) + { + manager_CCD_->addCollisionObject(cow); + manager_CCD_->setCastCollisionObjectsTransform( + cow->getName(), state1.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0], + state2.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0]); + } + + for (const std::string& link : active_) + { + manager_CCD_->setCastCollisionObjectsTransform(link, state1.getCollisionBodyTransform(link, 0), + state2.getCollisionBodyTransform(link, 0)); + } + + manager_CCD_->contactTest(res, req, acm, false); + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows) + { + manager_CCD_->removeCollisionObject(cow->getName()); + } +} + +void CollisionEnvBullet::distanceSelf(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const +{ + ROS_INFO_NAMED("collision_detection.bullet", "Distance to self not implemented yet."); +} + +void CollisionEnvBullet::distanceRobot(const DistanceRequest& req, DistanceResult& res, + const robot_state::RobotState& state) const +{ + ROS_INFO_NAMED("collision_detection.bullet", "Distance to self not implemented yet."); +} + +void CollisionEnvBullet::addToManager(const World::Object* obj) +{ + std::vector collision_object_types; + + for (const shapes::ShapeConstPtr& shape : obj->shapes_) + { + if (shape->type == shapes::MESH) + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); + else + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + } + + collision_detection_bullet::CollisionObjectWrapperPtr cow(new collision_detection_bullet::CollisionObjectWrapper( + obj->id_, collision_detection::BodyType::WORLD_OBJECT, obj->shapes_, obj->shape_poses_, collision_object_types, + false)); + + manager_->addCollisionObject(cow); + manager_CCD_->addCollisionObject(cow->clone()); +} + +void CollisionEnvBullet::updateManagedObject(const std::string& id) +{ + if (getWorld()->hasObject(id)) + { + auto it = getWorld()->find(id); + if (manager_->hasCollisionObject(id)) + { + manager_->removeCollisionObject(id); + manager_CCD_->removeCollisionObject(id); + addToManager(it->second.get()); + } + else + { + addToManager(it->second.get()); + } + } + else + { + if (manager_->hasCollisionObject(id)) + { + manager_->removeCollisionObject(id); + manager_CCD_->removeCollisionObject(id); + } + } +} + +void CollisionEnvBullet::setWorld(const WorldPtr& world) +{ + if (world == getWorld()) + return; + + // turn off notifications about old world + getWorld()->removeObserver(observer_handle_); + + CollisionEnv::setWorld(world); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + + // get notifications any objects already in the new world + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +void CollisionEnvBullet::notifyObjectChange(const ObjectConstPtr& obj, World::Action action) +{ + if (action == World::DESTROY) + { + manager_->removeCollisionObject(obj->id_); + manager_CCD_->removeCollisionObject(obj->id_); + } + else + { + updateManagedObject(obj->id_); + } +} + +void CollisionEnvBullet::addAttachedOjects( + const robot_state::RobotState& state, + std::vector& cows) const +{ + std::vector attached_bodies; + state.getAttachedBodies(attached_bodies); + + for (const robot_state::AttachedBody*& body : attached_bodies) + { + const EigenSTL::vector_Isometry3d& attached_body_transform = body->getGlobalCollisionBodyTransforms(); + + std::vector collision_object_types( + attached_body_transform.size(), collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + + try + { + collision_detection_bullet::CollisionObjectWrapperPtr cow(new collision_detection_bullet::CollisionObjectWrapper( + body->getName(), collision_detection::BodyType::ROBOT_ATTACHED, body->getShapes(), attached_body_transform, + collision_object_types, body->getTouchLinks())); + cows.push_back(cow); + } + catch (std::exception&) + { + ROS_ERROR_STREAM_NAMED("collision_detetction.bullet", "Not adding " << body->getName() + << " due to bad arguments."); + } + } +} + +void CollisionEnvBullet::updatedPaddingOrScaling(const std::vector& links) +{ + for (const std::string& link : links) + { + if (robot_model_->getURDF()->links_.find(link) != robot_model_->getURDF()->links_.end()) + { + addLinkAsCollisionObject(robot_model_->getURDF()->links_[link]); + } + else + { + ROS_ERROR_NAMED("collision_detection.bullet", "Updating padding or scaling for unknown link: '%s'", link.c_str()); + } + } +} + +void CollisionEnvBullet::updateTransformsFromState( + const robot_state::RobotState& state, const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const +{ + // updating link positions with the current robot state + for (const std::string& link : active_) + { + // select the first of the transformations for each link (composed of multiple shapes...) + manager->setCollisionObjectsTransform(link, state.getCollisionBodyTransform(link, 0)); + } +} + +void CollisionEnvBullet::addLinkAsCollisionObject(const urdf::LinkSharedPtr& link) +{ + if (!link->collision_array.empty()) + { + const std::vector& col_array = + link->collision_array.empty() ? std::vector(1, link->collision) : + link->collision_array; + + std::vector shapes; + collision_detection_bullet::AlignedVector shape_poses; + std::vector collision_object_types; + + for (const auto& i : col_array) + { + if (i && i->geometry) + { + shapes::ShapePtr shape = collision_detection_bullet::constructShape(i->geometry.get()); + + if (shape) + { + if (fabs(getLinkScale(link->name) - 1.0) >= std::numeric_limits::epsilon() || + fabs(getLinkPadding(link->name)) >= std::numeric_limits::epsilon()) + { + shape->scaleAndPadd(getLinkScale(link->name), getLinkPadding(link->name)); + } + + shapes.push_back(shape); + shape_poses.push_back(collision_detection_bullet::urdfPose2Eigen(i->origin)); + + if (shape->type == shapes::MESH) + { + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); + } + else + { + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + } + } + } + } + + if (manager_->hasCollisionObject(link->name)) + { + manager_->removeCollisionObject(link->name); + manager_CCD_->removeCollisionObject(link->name); + } + + try + { + collision_detection_bullet::CollisionObjectWrapperPtr cow(new collision_detection_bullet::CollisionObjectWrapper( + link->name, collision_detection::BodyType::ROBOT_LINK, shapes, shape_poses, collision_object_types, true)); + manager_->addCollisionObject(cow); + manager_CCD_->addCollisionObject(cow->clone()); + active_.push_back(cow->getName()); + } + catch (std::exception&) + { + ROS_ERROR_STREAM_NAMED("collision_detetction.bullet", "Not adding " << link->name << " due to bad arguments."); + } + } +} + +} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/collision_robot_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_robot_bullet.cpp deleted file mode 100644 index bb2ed2f584..0000000000 --- a/moveit_core/collision_detection_bullet/src/collision_robot_bullet.cpp +++ /dev/null @@ -1,273 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Jens Petit - * 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 the copyright holder 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. - *********************************************************************/ - -/* Author: Jens Petit */ - -#include -#include -#include -#include - -namespace collision_detection -{ -CollisionRobotBullet::CollisionRobotBullet(const robot_model::RobotModelConstPtr& model, double padding, double scale) - : CollisionRobot(model, padding, scale), manager_(new collision_detection_bullet::BulletDiscreteBVHManager) -{ - auto fun = std::bind(&collision_detection_bullet::allowedCollisionCheck, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3); - - manager_->setIsContactAllowedFn(fun); - - for (const std::pair& link : robot_model_->getURDF()->links_) - { - addLinkAsCollisionObjectWrapper(link.second); - } - - collision_detection_bullet::getActiveLinkNamesRecursive(active_, robot_model_->getURDF()->getRoot(), true); -} - -CollisionRobotBullet::CollisionRobotBullet(const CollisionRobotBullet& other) - : CollisionRobot(other), manager_(other.manager_->clone()) -{ -} - -void CollisionRobotBullet::addAttachedOjects( - const robot_state::RobotState& state, - std::vector& cows) const -{ - std::vector attached_bodies; - state.getAttachedBodies(attached_bodies); - for (const robot_state::AttachedBody*& body : attached_bodies) - { - const EigenSTL::vector_Isometry3d& attached_body_transform = body->getGlobalCollisionBodyTransforms(); - std::vector collision_object_types( - attached_body_transform.size(), collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); - - cows.emplace_back(collision_detection_bullet::createCollisionObject( - body->getName(), collision_detection::BodyType::ROBOT_ATTACHED, body->getShapes(), attached_body_transform, - collision_object_types, body->getTouchLinks(), true)); - } -} - -void CollisionRobotBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const -{ - checkSelfCollisionHelper(req, res, state, nullptr); -} - -void CollisionRobotBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - checkSelfCollisionHelper(req, res, state, &acm); -} - -void CollisionRobotBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet self continuous collision checking not yet implemented"); -} - -void CollisionRobotBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet self continuous collision checking not yet implemented"); -} - -void CollisionRobotBullet::checkSelfCollisionCCDHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const AllowedCollisionMatrix* acm) const -{ - // TODO: Not in bullet yet -} - -void CollisionRobotBullet::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const -{ - std::vector cows; - addAttachedOjects(state, cows); - collision_detection_bullet::BulletDiscreteBVHManagerPtr discrete_clone_manager = manager_->clone(); - updateTransformsFromState(state, discrete_clone_manager); - - if (req.distance) - { - discrete_clone_manager->setContactDistanceThreshold(2); - } - - discrete_clone_manager->filter_callback_.acm_ = acm; - - discrete_clone_manager->contactTest(res, req, acm, cows); -} - -void CollisionRobotBullet::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - checkOtherCollisionHelper(req, res, state, other_robot, other_state, nullptr); -} - -void CollisionRobotBullet::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const -{ - checkOtherCollisionHelper(req, res, state, other_robot, other_state, &acm); -} - -void CollisionRobotBullet::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet other robot continuous collision checking not yet implemented"); -} - -void CollisionRobotBullet::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet other robot continuous collision checking not yet implemented"); -} - -void CollisionRobotBullet::checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix* acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Other collision not implemented yet."); -} - -void CollisionRobotBullet::updatedPaddingOrScaling(const std::vector& links) -{ - for (const std::string& link : links) - { - if (robot_model_->getURDF()->links_.find(link) != robot_model_->getURDF()->links_.end()) - { - addLinkAsCollisionObjectWrapper(robot_model_->getURDF()->links_[link]); - } - else - { - ROS_ERROR_NAMED("collision_detection.bullet", "Updating padding or scaling for unknown link: '%s'", link.c_str()); - } - } -} - -void CollisionRobotBullet::distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Collision distance to self not implemented yet."); -} - -void CollisionRobotBullet::distanceOther(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Collision distance to other not implemented yet."); -} - -void CollisionRobotBullet::updateTransformsFromState( - const robot_state::RobotState& state, const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const -{ - // updating link positions with the current robot state - for (const std::pair& link : - manager->getCollisionObjects()) - { - // select the first of the transformations for each link (composed of multiple shapes...) - manager->setCollisionObjectsTransform(link.first, state.getCollisionBodyTransform(link.first, 0)); - } -} - -void CollisionRobotBullet::addLinkAsCollisionObjectWrapper(const urdf::LinkSharedPtr& link) -{ - if (!link->collision_array.empty()) - { - const std::vector& col_array = - link->collision_array.empty() ? std::vector(1, link->collision) : - link->collision_array; - - std::vector shapes; - collision_detection_bullet::AlignedVector shape_poses; - std::vector collision_object_types; - - for (const auto& i : col_array) - { - if (i && i->geometry) - { - shapes::ShapePtr shape = collision_detection_bullet::constructShape(i->geometry.get()); - - if (shape) - { - if (fabs(getLinkScale(link->name) - 1.0) >= std::numeric_limits::epsilon() || - fabs(getLinkPadding(link->name)) >= std::numeric_limits::epsilon()) - { - shape->scaleAndPadd(getLinkScale(link->name), getLinkPadding(link->name)); - } - - shapes.push_back(shape); - shape_poses.push_back(collision_detection_bullet::urdfPose2Eigen(i->origin)); - - if (shape->type == shapes::MESH) - { - collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); - } - else - { - collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); - } - } - } - } - - collision_detection_bullet::CollisionObjectWrapperPtr cow = collision_detection_bullet::createCollisionObject( - link->name, collision_detection::BodyType::ROBOT_LINK, shapes, shape_poses, collision_object_types, true); - - if (manager_->hasCollisionObject(link->name)) - { - manager_->removeCollisionObject(link->name); - } - manager_->addCollisionObject(cow); - } -} - -} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp deleted file mode 100644 index 837fd0706d..0000000000 --- a/moveit_core/collision_detection_bullet/src/collision_world_bullet.cpp +++ /dev/null @@ -1,297 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Jens Petit - * 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 the copyright holder 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. - *********************************************************************/ - -/* Author: Jens Petit */ - -#include -#include -#include -#include -#include -#include - -namespace collision_detection -{ -const std::string CollisionDetectorAllocatorBullet::NAME("Bullet"); - -CollisionWorldBullet::CollisionWorldBullet() - : CollisionWorld() - , manager_(new collision_detection_bullet::BulletDiscreteBVHManager) - , bt_manager_CCD_(new collision_detection_bullet::BulletCastBVHManager) -{ - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); - - auto fun = std::bind(&collision_detection_bullet::allowedCollisionCheck, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3); - manager_->setIsContactAllowedFn(fun); - bt_manager_CCD_->setIsContactAllowedFn(fun); -} - -CollisionWorldBullet::CollisionWorldBullet(const WorldPtr& world) - : CollisionWorld(world) - , manager_(new collision_detection_bullet::BulletDiscreteBVHManager) - , bt_manager_CCD_(new collision_detection_bullet::BulletCastBVHManager) -{ - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); - - auto fun = std::bind(&collision_detection_bullet::allowedCollisionCheck, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3); - manager_->setIsContactAllowedFn(fun); - bt_manager_CCD_->setIsContactAllowedFn(fun); -} - -CollisionWorldBullet::CollisionWorldBullet(const CollisionWorldBullet& other, const WorldPtr& world) - : CollisionWorld(other, world) -{ - manager_ = other.manager_->clone(); - bt_manager_CCD_ = other.bt_manager_CCD_->clone(); - - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); -} - -CollisionWorldBullet::~CollisionWorldBullet() -{ - getWorld()->removeObserver(observer_handle_); -} - -void CollisionWorldBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state) const -{ - checkRobotCollisionHelper(req, res, robot, state, nullptr); -} - -void CollisionWorldBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - checkRobotCollisionHelper(req, res, robot, state, &acm); -} - -void CollisionWorldBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const -{ - checkRobotCollisionHelperCCD(req, res, robot, state1, state2, nullptr); -} - -void CollisionWorldBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - checkRobotCollisionHelperCCD(req, res, robot, state1, state2, &acm); -} - -void CollisionWorldBullet::checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const AllowedCollisionMatrix* acm) const -{ - const CollisionRobotBullet& robot_bt = dynamic_cast(robot); - - collision_detection_bullet::BulletCastBVHManagerPtr cast_clone_manager = bt_manager_CCD_->clone(); - std::vector attached_cows; - robot_bt.addAttachedOjects(state1, attached_cows); - std::vector active_objects; - - for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows) - { - cast_clone_manager->addCollisionObject(cow); - cast_clone_manager->setCollisionObjectsTransform( - cow->getName(), state1.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0], - state2.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0]); - active_objects.push_back(cow->getName()); - } - - for (const std::pair& cow : - robot_bt.manager_->getCollisionObjects()) - { - collision_detection_bullet::CollisionObjectWrapperPtr new_cow = cow.second->clone(); - cast_clone_manager->addCollisionObject(new_cow); - cast_clone_manager->setCollisionObjectsTransform(new_cow->getName(), - state1.getCollisionBodyTransform(new_cow->getName(), 0), - state2.getCollisionBodyTransform(new_cow->getName(), 0)); - } - - collision_detection_bullet::getActiveLinkNamesRecursive(active_objects, - robot_bt.getRobotModel()->getURDF()->getRoot(), true); - cast_clone_manager->setActiveCollisionObjects(active_objects); - cast_clone_manager->filter_callback_.acm_ = acm; - cast_clone_manager->contactTest(res, req, acm); -} - -void CollisionWorldBullet::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const -{ - const CollisionRobotBullet& robot_bt = dynamic_cast(robot); - - collision_detection_bullet::BulletDiscreteBVHManagerPtr discrete_clone_manager = manager_->clone(); - std::vector attached_cows; - robot_bt.addAttachedOjects(state, attached_cows); - - for (const std::pair& cow : - robot_bt.manager_->getCollisionObjects()) - { - collision_detection_bullet::CollisionObjectWrapperPtr new_cow = cow.second->clone(); - discrete_clone_manager->addCollisionObject(new_cow); - discrete_clone_manager->setCollisionObjectsTransform(new_cow->getName(), - state.getCollisionBodyTransform(new_cow->getName(), 0)); - } - - discrete_clone_manager->setActiveCollisionObjects(robot_bt.active_); - - if (req.distance) - { - discrete_clone_manager->setContactDistanceThreshold(std::numeric_limits::max()); - } - - discrete_clone_manager->filter_callback_.acm_ = acm; - - discrete_clone_manager->contactTest(res, req, acm, attached_cows); -} - -void CollisionWorldBullet::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const -{ - checkWorldCollisionHelper(req, res, other_world, nullptr); -} - -void CollisionWorldBullet::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world, - const AllowedCollisionMatrix& acm) const -{ - checkWorldCollisionHelper(req, res, other_world, &acm); -} - -void CollisionWorldBullet::checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world, - const AllowedCollisionMatrix* acm) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet checking with other world not implemented yet."); -} - -void CollisionWorldBullet::addToManager(const World::Object* obj) -{ - std::vector collision_object_types; - - for (const shapes::ShapeConstPtr& shape : obj->shapes_) - { - if (shape->type == shapes::MESH) - collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); - else - collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); - } - - manager_->addCollisionObject(obj->id_, collision_detection::BodyType::WORLD_OBJECT, obj->shapes_, obj->shape_poses_, - collision_object_types, true); - - bt_manager_CCD_->addCollisionObject(obj->id_, collision_detection::BodyType::WORLD_OBJECT, obj->shapes_, - obj->shape_poses_, collision_object_types, true); -} - -void CollisionWorldBullet::updateManagedObject(const std::string& id) -{ - if (getWorld()->hasObject(id)) - { - auto it = getWorld()->find(id); - if (manager_->hasCollisionObject(id)) - { - manager_->removeCollisionObject(id); - bt_manager_CCD_->removeCollisionObject(id); - addToManager(it->second.get()); - } - else - { - addToManager(it->second.get()); - } - } - else - { - if (manager_->hasCollisionObject(id)) - { - manager_->removeCollisionObject(id); - bt_manager_CCD_->removeCollisionObject(id); - } - } -} - -void CollisionWorldBullet::setWorld(const WorldPtr& world) -{ - if (world == getWorld()) - return; - - // turn off notifications about old world - getWorld()->removeObserver(observer_handle_); - - CollisionWorld::setWorld(world); - - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldBullet::notifyObjectChange, this, _1, _2)); - - // get notifications any objects already in the new world - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -void CollisionWorldBullet::notifyObjectChange(const ObjectConstPtr& obj, World::Action action) -{ - if (action == World::DESTROY) - { - manager_->removeCollisionObject(obj->id_); - bt_manager_CCD_->removeCollisionObject(obj->id_); - } - else - { - updateManagedObject(obj->id_); - } -} - -void CollisionWorldBullet::distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet distance calculation not implemented yet."); -} - -void CollisionWorldBullet::distanceWorld(const DistanceRequest& req, DistanceResult& res, - const CollisionWorld& world) const -{ - ROS_ERROR_NAMED("collision_detection.bullet", "Bullet distance calculation not implemented yet."); -} - -} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp index f5b5014c33..4dd5115d2b 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp @@ -1,3 +1,39 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * 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 the copyright holder 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. + *********************************************************************/ + +/* Author: Jens Petit */ + #include #include @@ -9,13 +45,14 @@ #include #include -#include -#include +#include #include #include #include +namespace cb = collision_detection_bullet; + /** \brief Brings the panda robot in user defined home position */ inline void setToHome(robot_state::RobotState& panda_state) { @@ -49,8 +86,7 @@ class BulletCollisionDetectionTester : public testing::Test for (const srdf::Model::DisabledCollision& it : dc) acm_->setEntry(it.link1_, it.link2_, true); - crobot_.reset(new collision_detection::CollisionRobotBullet(robot_model_)); - cworld_.reset(new collision_detection::CollisionWorldBullet()); + cenv_.reset(new collision_detection::CollisionEnvBullet(robot_model_)); robot_state_.reset(new robot_state::RobotState(robot_model_)); @@ -66,15 +102,14 @@ class BulletCollisionDetectionTester : public testing::Test robot_model::RobotModelPtr robot_model_; - collision_detection::CollisionWorldPtr cworld_; - collision_detection::CollisionRobotPtr crobot_; + collision_detection::CollisionEnvPtr cenv_; collision_detection::AllowedCollisionMatrixPtr acm_; robot_state::RobotStatePtr robot_state_; }; -void addCollisionObjects(collision_detection_bullet::BulletCastBVHManager& checker) +void addCollisionObjects(cb::BulletCastBVHManager& checker) { //////////////////////////// // Add static box to checker @@ -84,14 +119,15 @@ void addCollisionObjects(collision_detection_bullet::BulletCastBVHManager& check static_box_pose.setIdentity(); std::vector obj1_shapes; - collision_detection_bullet::AlignedVector obj1_poses; - std::vector obj1_types; + cb::AlignedVector obj1_poses; + std::vector obj1_types; obj1_shapes.push_back(static_box); obj1_poses.push_back(static_box_pose); - obj1_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE); - checker.addCollisionObject("static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, - obj1_types); + cb::CollisionObjectWrapperPtr cow_1(new cb::CollisionObjectWrapper( + "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types)); + checker.addCollisionObject(cow_1); //////////////////////////// // Add moving box to checker @@ -103,17 +139,18 @@ void addCollisionObjects(collision_detection_bullet::BulletCastBVHManager& check moving_box_pose.translation() = Eigen::Vector3d(0.5, -0.5, 0); std::vector obj2_shapes; - collision_detection_bullet::AlignedVector obj2_poses; - std::vector obj2_types; + cb::AlignedVector obj2_poses; + std::vector obj2_types; obj2_shapes.push_back(moving_box); obj2_poses.push_back(moving_box_pose); - obj2_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + obj2_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE); - checker.addCollisionObject("moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, - obj2_types); + cb::CollisionObjectWrapperPtr cow_2(new cb::CollisionObjectWrapper( + "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types)); + checker.addCollisionObject(cow_2); } -void addCollisionObjectsMesh(collision_detection_bullet::BulletCastBVHManager& checker) +void addCollisionObjectsMesh(cb::BulletCastBVHManager& checker) { //////////////////////////// // Add static box to checker @@ -123,25 +160,25 @@ void addCollisionObjectsMesh(collision_detection_bullet::BulletCastBVHManager& c static_box_pose.setIdentity(); std::vector obj1_shapes; - collision_detection_bullet::AlignedVector obj1_poses; - std::vector obj1_types; + cb::AlignedVector obj1_poses; + std::vector obj1_types; obj1_shapes.push_back(static_box); obj1_poses.push_back(static_box_pose); - obj1_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); - - checker.addCollisionObject("static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, - obj1_types); + obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE); + cb::CollisionObjectWrapperPtr cow_1(new cb::CollisionObjectWrapper( + "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types)); + checker.addCollisionObject(cow_1); //////////////////////////// // Add moving mesh to checker //////////////////////////// std::vector obj2_shapes; - collision_detection_bullet::AlignedVector obj2_poses; - std::vector obj2_types; + cb::AlignedVector obj2_poses; + std::vector obj2_types; obj1_poses.push_back(static_box_pose); - obj1_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE); Eigen::Isometry3d s_pose; s_pose.setIdentity(); @@ -150,14 +187,15 @@ void addCollisionObjectsMesh(collision_detection_bullet::BulletCastBVHManager& c shapes::ShapeConstPtr s; s.reset(shapes::createMeshFromResource(kinect)); obj2_shapes.push_back(s); - obj2_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); + obj2_types.push_back(cb::CollisionObjectType::CONVEX_HULL); obj2_poses.push_back(s_pose); - checker.addCollisionObject("moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, - obj2_types); + cb::CollisionObjectWrapperPtr cow_2(new cb::CollisionObjectWrapper( + "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types)); + checker.addCollisionObject(cow_2); } -void runTest(collision_detection_bullet::BulletCastBVHManager& checker, collision_detection::CollisionResult& result, +void runTest(cb::BulletCastBVHManager& checker, collision_detection::CollisionResult& result, std::vector& result_vector, Eigen::Isometry3d& start_pos, Eigen::Isometry3d& end_pos) { @@ -169,13 +207,13 @@ void runTest(collision_detection_bullet::BulletCastBVHManager& checker, collisio // Set the collision object transforms checker.setCollisionObjectsTransform("static_box_link", Eigen::Isometry3d::Identity()); - checker.setCollisionObjectsTransform("moving_box_link", start_pos, end_pos); + checker.setCastCollisionObjectsTransform("moving_box_link", start_pos, end_pos); // Perform collision check collision_detection::CollisionRequest request; request.contacts = true; - // collision_detection_bullet::ContactResultMap result; - checker.contactTest(result, request, nullptr); + // cb::ContactResultMap result; + checker.contactTest(result, request, nullptr, false); for (const auto& contacts_all : result.contacts) { @@ -186,7 +224,7 @@ void runTest(collision_detection_bullet::BulletCastBVHManager& checker, collisio } } -// TODO: Add continuous to continuous collision checking +// TODO(j-petit): Add continuous to continuous collision checking /** \brief Continuous self collision checks are not supported yet by the bullet integration */ TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf) { @@ -207,7 +245,7 @@ TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf) state1.setJointPositions("panda_joint7", &joint7); state1.update(); - crobot_->checkSelfCollision(req, res, state1, *acm_); + cenv_->checkSelfCollision(req, res, state1, *acm_); ASSERT_FALSE(res.collision); res.clear(); @@ -219,11 +257,10 @@ TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf) state2.setJointPositions("panda_joint7", &joint7); state2.update(); - crobot_->checkSelfCollision(req, res, state2, *acm_); + cenv_->checkSelfCollision(req, res, state2, *acm_); ASSERT_FALSE(res.collision); res.clear(); - crobot_->checkSelfCollision(req, res, state1, state2, *acm_); ROS_INFO_STREAM("Continous to continous collisions are not supported yet, therefore fail here."); ASSERT_TRUE(res.collision); res.clear(); @@ -250,7 +287,7 @@ TEST_F(BulletCollisionDetectionTester, ContinuousCollisionWorld) state2.setJointPositions("panda_joint4", &joint_4); state2.update(); - cworld_->checkRobotCollision(req, res, *crobot_, state1, state2, *acm_); + cenv_->checkRobotCollision(req, res, state1, state2, *acm_); ASSERT_FALSE(res.collision); res.clear(); @@ -262,17 +299,17 @@ TEST_F(BulletCollisionDetectionTester, ContinuousCollisionWorld) pos.translation().x() = 0.43; pos.translation().y() = 0; pos.translation().z() = 0.55; - cworld_->getWorld()->addToObject("box", shape_ptr, pos); + cenv_->getWorld()->addToObject("box", shape_ptr, pos); - cworld_->checkRobotCollision(req, res, *crobot_, state1, *acm_); + cenv_->checkRobotCollision(req, res, state1, *acm_); ASSERT_FALSE(res.collision); res.clear(); - cworld_->checkRobotCollision(req, res, *crobot_, state2, *acm_); + cenv_->checkRobotCollision(req, res, state2, *acm_); ASSERT_FALSE(res.collision); res.clear(); - cworld_->checkRobotCollision(req, res, *crobot_, state1, state2, *acm_); + cenv_->checkRobotCollision(req, res, state1, state2, *acm_); ASSERT_TRUE(res.collision); ASSERT_EQ(res.contact_count, 4u); res.clear(); @@ -289,7 +326,7 @@ TEST(ContinuousCollisionUnit, BulletCastBVHCollisionBoxBoxUnit) end_pos.setIdentity(); end_pos.translation().x() = 2; - collision_detection_bullet::BulletCastBVHManager checker; + cb::BulletCastBVHManager checker; addCollisionObjects(checker); runTest(checker, result, result_vector, start_pos, end_pos); @@ -300,7 +337,7 @@ TEST(ContinuousCollisionUnit, BulletCastBVHCollisionBoxBoxUnit) TEST(ContinuousCollisionUnit, BulletCastMeshVsBox) { - collision_detection_bullet::BulletCastBVHManager checker; + cb::BulletCastBVHManager checker; addCollisionObjectsMesh(checker); Eigen::Isometry3d start_pos, end_pos; @@ -317,83 +354,6 @@ TEST(ContinuousCollisionUnit, BulletCastMeshVsBox) ASSERT_TRUE(result.collision); } -TEST(ContinuousCollisionUnit, TwoManagers) -{ - collision_detection_bullet::BulletCastBVHManager checker_continuous; - collision_detection_bullet::BulletDiscreteBVHManager checker_discrete; - - //////////////////////////// - // Add static box to checker - //////////////////////////// - shapes::ShapePtr static_box(new shapes::Box(.1, .1, .1)); - Eigen::Isometry3d static_box_pose; - static_box_pose.setIdentity(); - - std::vector obj1_shapes; - collision_detection_bullet::AlignedVector obj1_poses; - std::vector obj1_types; - obj1_shapes.push_back(static_box); - obj1_poses.push_back(static_box_pose); - obj1_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); - - checker_continuous.addCollisionObject("static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, - obj1_poses, obj1_types); - checker_discrete.addCollisionObject("static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, - obj1_poses, obj1_types); - - //////////////////////// - // Add moving sphere to checker - //////////////////////// - shapes::ShapePtr sphere; - sphere.reset( - shapes::createMeshFromResource("package://moveit_resources/panda_description/meshes/collision/link0.stl")); - - Eigen::Isometry3d sphere_pose; - sphere_pose.setIdentity(); - - std::vector obj2_shapes; - collision_detection_bullet::AlignedVector obj2_poses; - std::vector obj2_types; - obj2_shapes.push_back(sphere); - obj2_poses.push_back(sphere_pose); - obj2_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); - - checker_continuous.addCollisionObject("moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, - obj2_poses, obj2_types); - checker_discrete.addCollisionObject("moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, - obj2_poses, obj2_types); - - checker_continuous.setActiveCollisionObjects({ "moving_box_link" }); - checker_continuous.setContactDistanceThreshold(0.1); - - // Set the collision object transforms - checker_continuous.setCollisionObjectsTransform("static_box_link", Eigen::Isometry3d::Identity()); - checker_discrete.setCollisionObjectsTransform("static_box_link", Eigen::Isometry3d::Identity()); - - Eigen::Isometry3d start_pos, end_pos; - start_pos.setIdentity(); - start_pos.translation().x() = -1.9; - end_pos.setIdentity(); - end_pos.translation().x() = 1.9; - checker_continuous.setCollisionObjectsTransform("moving_box_link", start_pos, end_pos); - - Eigen::Isometry3d start_pos_2; - start_pos_2.setIdentity(); - start_pos_2.translation().x() = -1.9; - checker_discrete.setCollisionObjectsTransform("moving_box_link", start_pos_2); - - collision_detection::CollisionResult result; - collision_detection::CollisionResult result_2; - collision_detection::CollisionRequest request; - - // Perform collision check - checker_discrete.contactTest(result_2, request, nullptr); - checker_continuous.contactTest(result, request, nullptr); - - ASSERT_TRUE(result.collision); - ASSERT_FALSE(result_2.collision); -} - int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index c5b89cca28..9df7168c69 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Jens Petit */ #pragma once diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index 0cf3446237..7a936b2771 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan, Jia Pan */ +/* Author: Ioan Sucan, Jia Pan, Jens Petit */ #include #include From 753604ca172b47c4a00fd297c84c37f30b6204bc Mon Sep 17 00:00:00 2001 From: Jens P Date: Tue, 26 Nov 2019 16:23:11 +0100 Subject: [PATCH 152/612] Readme for speed benchmark (#1648) * FCL Bullet benchmark readme * Benchmark script and launch file updated * PR review fixups --- .../launch/collision_checker_compare.launch | 6 ++++ .../planning_components_tools/README.md | 22 +++++++++++++ .../collision_speed_100_box_collision.png | Bin 0 -> 148089 bytes .../collision_speed_100_meshes_collision.png | Bin 0 -> 419183 bytes ...ollision_speed_100_meshes_no_collision.png | Bin 0 -> 359517 bytes ...re_collision_speed_checking_fcl_bullet.cpp | 31 ++++++++++++++---- 6 files changed, 52 insertions(+), 7 deletions(-) create mode 100644 moveit_ros/planning/planning_components_tools/README.md create mode 100644 moveit_ros/planning/planning_components_tools/images/collision_speed_100_box_collision.png create mode 100644 moveit_ros/planning/planning_components_tools/images/collision_speed_100_meshes_collision.png create mode 100644 moveit_ros/planning/planning_components_tools/images/collision_speed_100_meshes_no_collision.png diff --git a/moveit_ros/planning/launch/collision_checker_compare.launch b/moveit_ros/planning/launch/collision_checker_compare.launch index 5c9526b876..3be9255091 100644 --- a/moveit_ros/planning/launch/collision_checker_compare.launch +++ b/moveit_ros/planning/launch/collision_checker_compare.launch @@ -1,4 +1,6 @@ + + @@ -6,5 +8,9 @@ + + + + diff --git a/moveit_ros/planning/planning_components_tools/README.md b/moveit_ros/planning/planning_components_tools/README.md new file mode 100644 index 0000000000..766a4776f8 --- /dev/null +++ b/moveit_ros/planning/planning_components_tools/README.md @@ -0,0 +1,22 @@ +# Planning Component Tools + +## Compare collision checking speeds: FCL vs Bullet +The launch file `collision_checker_compare.launch` starts a benchmark between FCL and Bullet as a collision detection library. The script `compare_collision_speed_checking_fcl_bullet.cpp` performs the actual speed tests. + +The collision scenarios can be visualized. By default, visualization is turned off in the launch file but can be activated through `visualization:=true` as an argument to the `roslaunch`. If activated, rviz is launched and visualizations like in the images below are created. For the speed test, either meshes or other available shape primitives like the box can be used. + +The clutter world objects are randomly placed such that they don't collide with the robot. The robot can then be moved to a colliding configuration. In the images, the red color of world objects indicates a collision with the robot. + +

+ + + +

+ +The results of the speed test are printed to the console. Their unit is collision checks per second. A current overview over the results is given in the table below. Note that absolute values are not relevant because they are depending on the computing power of the local machine but the relative differences between Bullet and FCL should be reproducible. + +| Collision environment | Bullet | FCL | +| :--------------- |------------:| ----:| +| Robot self check, no collision | 220,000 | 110,000 | +| World clutter 100 meshes, no collision | 37,000 | 38,000 | +| World clutter 100 meshes, 4 in collision | 8,000 | 3,000 | diff --git a/moveit_ros/planning/planning_components_tools/images/collision_speed_100_box_collision.png b/moveit_ros/planning/planning_components_tools/images/collision_speed_100_box_collision.png new file mode 100644 index 0000000000000000000000000000000000000000..463d4d5f1089072e66143bee050a9919c71767ea GIT binary patch literal 148089 zcmYJacRZE<|37~0O_EX4X(5Crq8u3&lER^^V>FDc%1X{fR;VBa5j*%3F z?5sofJl1iJbIx^Lze}&r`}@28apO48@fi2V{V}fC%a_iJ2<{R@p->{m7Yr>>C@u(v z;+*E^MgDT+*jr`f2lrilV=I2-dBA`D5%T-adq!99S^C_(7wCNF2I`i#kJk;=yRLU` z-0;5Z=5vqAMb<^3_M(gp&sqh&T^RO!e0VxoaRatXSxQTLef-wcQ1Enyp+%xu=YDzH zV;@Sn7X+7gd2(LbfAHL=4bBmDfo%smQik__kEuKRSfSD#;D~j51Gva~Jg(1T@K^uDbO-^823|l@qZtR@&fyfa&|L9PY)zqFuP_zP8cIRdgOL|dG1|~^ObNk>L(xZ zR4VBZBAM>(C%>u-MFYs)mt2t(e0RGQnS07XfbD;2t~~+^?DyXx0_y(z+za8%Pi~V_JPw3wMzkTYFeV+Av zKC)=$r*--T15~8Z3=XBnea3E*1 zW6LfNnbVhj%ai=WRpR2FH%`mLXBDMTm58>R+vQxPtAt_6=jdc#$ADwNo~^Yu8G>f- zQnKD*l$TlSceQ`2C^Q0`)}T_t$^qAlo zwe={DsbvjXI`ic6GRMjHgFyMX6XhU=_y_FrX<@uw-|>a7 z?&8*_UWaOxEpJWi>`&p#gJuzrFlGxtM!DM{MJmB*`3d94#+!dO{U_;nFM)AV&A6ZJ zVgTohuEgam%x@EaexDI{u|RWRr?bZ;6|pTB>3S{tlJ41cQ^ewUhdP z2)*kFG_??XbG`VA=RZ@zywm0cUafuNl0&p<^_Z35@V#~9Zc2sH-VuS*+10Z(;<(YZ z$I4&V+A%naipUjXVn3M>b(d1VLZ-GEE)f?-hMN9!42zE6tf$*!Hbv5u12i7U386kG zv>mBg0(BO2G=^rc#cv=HqDZ}8b|*6;fZVc-&L);YB_$;H-OTfbUrxaX2i*_Y?{<@l zz?7PFg=1zWR21`>*}j|cDQ{|pKD;VvWXJKsO^a3~sBOX^YVWP>y^EW7ABv_^Yb2ir zo;|SetER}Af$KF)!iXXkFu4z-JOOYf8v}!3 zuoP;U&0cK3@AFrRuj0tF+n;ga#=-s~3#}0$VAt3)%<2shaOrE5#v#SWiE@xRW;^)y zgO|JSDC+1JwQByo_8cerW*zd(#NZoy+HEutylgSi?fAKNZQ*ZSdm5-N&6nvlm8Y&m zS_%&hE}kPiKF<=rJi-lUw#cBgSTl!L!NSRg*Qd7W>$OY>?awv!*;mICxkL6hPf_d` zc@}{o1of{-nbzAKb(iMT@G|9~GhSMp(>btPR7?U>I9rGQS_A!HW`{0>zQe+10c%{E z$AL@K-;VB*u+}wcR0E3{il3nyE4k)D^S4{9!_4~ENG^83uD{7)P~QEnz=%gXteYUb zq`#M6SUn*z{f;TH{Sw1`dUTL4pXrlxLW2FiTbv#0E`{oh#uM(v*Hn8X_s)8u97JF* zk$;d;@{}DcK;IN4>t%J9IY$nMG-X*E%Q6Bn#qnd7jiuu*)o zxxzwhNA|!6-EX3<3xFt8xIGqb@|ssWQC{WM=Wv_lV`>L%4>oStEa6&gKk>+>yDCV2 zzs1?-&XIaxJlAk=DYCu<1Dn6p{OA5np>dxr3^ExE(**16p_m$sS)v5Rr zv3;Cm5`2MBkKwTfHP7$Npq%j!k9e_knRlc1$)2`{@WT7!{UIBx%?Bg#xPh~P-v>1v z%qFm2EFJ*e%bu-zZYs?M$RK9R^CKaEzFDZ`9xXmcwyJKdIP)XUwjggS#=+MS`SM~F z7}Ju%n`5D1=X2k~ir5*PQ!8*d)FuXs6rEQRnj*b1qrAIV+1X2oMtpj)@>?uy$bI_v zx?KNh{CT}A)y$73UizbS#IQv3py5FJN|hNU%0i3kZ>>*0Q{4C9TuCMtE-YiuS7|d7 zXdI}ej;J;I?iq46^fS>+kbU;_Qo^I0ou+muLASup{aM3*$e%Tq^G+A3t6PIFi{~g_ z2wHnE)e;-NERC)oxYdpeHh}f(i9nw%^vhG>XMC$9p0$(8%JX1QBFmtI2Pf+}`D$z0j@RRy+KVnW5ioYD}f8wtP1Ymm2mdtaP(=qq5Co#(Ftc6>* zA5n$ea95@&KkCSj5$clT_?PhcuX)^`3B=Hez^B=wYTpYUZiC#g{|6;!hoZgqS4?K; z#gjnj4E>b4b_+Lb<~48LuxwuPdZPUvGGn2L8 z=ytOo%>`(zzKC3Nj~OGl8GPqs3|+0#Y^}=ub@7JP_ez9wAOInpCUb<5UAGTd?A>r> zHGCLz=%~n6`=Gk-vYBb6kBgg#zN7nxjEmKCC!$UX! zbCksn!wJ=hOoMmHFcCTTcRXOdIge1gZn@R>50>)n{`}K>st_OE zn%>$-#Q2|;kYGDB|2iq6+v8QGw%g`OxVOH|Mytn4`WH?hDF9rEJ zNEeRl>*)y^aXSA_28N@iG$ab=5OtR{LjAoYUIeQ$cbkk*#Evk0@|Sh-ypTun9GM6V zG$R3dXpq8bc5ylIqvJ`(OY*mec|V5z)=Z}hEv=-*Ks*9gHBT^4j`4!V2KxOp(6#05 z5W%Rk2(za>dk_7Z^lEA(l&77a5jDQn+&?%F0o^QaJ}%zV7@8KJ(*XP?lDp+2RXp4`eGu=? zj7F>#=e7Fj>nujQX^gr%6AnUm6(~jf+`(^qK8Gk8k6qYOwB|Xj<-1Ne@G9+A64@&? zb(9(deIU1YLTE`Q_CRD#N{1bSy zNX$#iTnot;bDy$Uo;FB?3#%uuJq!nyyHV0<FGqOrTK;|0I{%}J{L_eqF(^*}mgCC@ZjSt)lhq^*9 z;@-G0Pi$qOD$4y^S_zcmBByE;Z>`QcakL)2Y|D1g$utlBrOWnTge^?80xD}pZf}}@ zw3^XG{YeQEt>}zWyL!^PzOqg%CLU&e%MOQ-%G_ml7oT*}BLLX7wcmhsYv{J2v``7Vh+g3b{bS zF@}UVN6$Y<)QJcupV7#g1C|%LtT*j)koELdYoYG)qde>3X)2M1SWy0;A(t0QxsnpL;iq-CEY=^hG5>n|(q{w(O1_kk z^EbXE)RT?!)1rRFOU$Ux2(#rnl8Bf*_F`{0H?Y?z9)6oRPyZuisplpzYv>|AW!ER@ z=-LX5X8^x+p%SeH(yLEgfJ-UnJNN%fR#u{OjE5+cGu5^};ZCDe^ogPS##!zmJp?S| zUBO}upR4s=_#G-fVWltp-tq#mgM80*b^zv?dPx(CUOLv@vr2KBY5(0z7L5UK9EhS< zEs<5R(S+O+@{#g;)s3|f--2S@K3i36ftO7xJN@g#ioiO3b_TJX_yJqm^wH2^-}Tz} z$Tpp|(22+?toZRLJg#K}*cT-oX?6J?hyEKl_L`^r;q{c=j0mj-^=mzg?B|0%+rNiL zz6u(n{4Z=Vjvpkxd7$^ak1S#_x#g-jO}Fr2T~&W(OgAfckQ3mAb{==+irb_ipz>ZC;6n)LfQro=4 z7nPC^q>6&q^1ym)ytCCdX6RZ}RL-)xeU$POBWg?amVX>&|8}m)pop;>J#1Oyj7CT! z3C)=2XPL&aAA>|7jFpd+p{!CAQ){_obN+%LDuVptYaZu-7v!c9OWeYas;pT7=!-rX zHB>wwg?}I2y`N(?nh1*?lZ&yD-)A_stk5R3j?N48z9@m1v;XiDVc6m&jSs&gZ-do< z1;WM%Ich@`9GOVp^7R&%9^muLbT>2ja|^}H%xszO6Lp?$fEmSRdkTPO$L7dUmrB@g z`Yx`M=ru>K#^~;CKPkY(B4X;0WAiOhNj7r-0`3hYrUjR! zT{&?5f9u{^n3uj9i68&O0j6hk&)C3EHdlb;U04A1MY*E>2iW0u7epqmY%aF%wRNTtAS<9`2dohJUI;e{bUeYVn@O zT<`mj2FO2q<{MHNSVGwU<)~*5X0D>vgb0Z&j+>jBd^y?(**YxJjjX@|9jOrzN103n zGKszb0D2(czpRDHinZY>5p#dLOg>wd_NRgIW_*CWwDik?7kcV(=M`bT;!DgXZO{B! z*_=`hRsUHLrsagUfSF$9k1;jxJLK+aU$wU{IITpQ*OKbd8`%Oz_-B$c`lS>_Gl4Pw ztAU5Nsxe)C*;>GZ+I5nFVRxc70=X^rwU>7znTqM~KLr>3=n%W^vm#v?R`y;ADScUd z?->KkOe(A|Xcv<5lL4iJ_629+yB#Q0N~{Ojoa+)fG_Hz#SGue3&8+?YfrCqHi@|Et zhn2|6`3x2FYUOkcu`w9FS)yPO`hOb;AaZ3o=rlG(@T#w4y5rfNN;j0I`=__FKdmk# z>=-Fo|F$GuO_GyzqB+P--8kOk7aD&lT0fBc+2_r}k;JF=hhL9tcbu&GO~6T3S}m4S z`?WU36kk>AhQGpWE5B*iuREj$dWwtF>49<)fY1!Nb(Cz(OjuVDfxs@^GRmK+<=Us7 zcOC*$q2Sux$#H$M1$Xi-(F;DjsE$!iy&2uvs{0?unJu?Bee4*4Swar?vOiz)_;kDn zdXG8XFhTqAN9Q6+s)L}RBNmg>wZ%P7{tJtC1~6|U;J&`8+wC43VrOcN-2&5J9TNyd zkN*zkJVJUkvW;BEkMX(yn;mx$KvJRZZR8Ul_U-%d{Y-U-`mO zwso_LwZTB^5p@v(X8~(gY>wsJ07c9<0q{gF760Pc_pv`E?tfBzu{(duS!_50OOuq{ zW|x-vjxEhghXtR!G2>%E&j8rCUm?v|Djc(+>T8iu)8fS2RrLdStvd?)3N+XqZP8$< z!(bk`At!qt2-<v5cF;$W3STG)5X4z%<()IlHR_(^5OKQ*EScg zQf;GrVvq}FG#nv!_dtg;$X@IXcoc%;R{Qs_;l?=EStx;!csaVlL2(=J+}GkyT&4O5qYxn%T`RgC2QuX9)2- z=xJ{k-96cWBt>U4M%!!HZ?e}r(8b$oEF{x_$DUt`ZUy{J3`X6`05FZzn>umLw>Y^_ zQ3@{Qe2FV3Tlm5FH;4OI$#+7Yaw{zI!(Mwf`mSc74_PgWiBe6E1qGo{uXN8eE&pQ)MYWL6y@NHTZ!qPBCD=L-G z!)u*bW$(9X#nhEz|!IY z^08`l`H=SLHEx9eb>sd2B`S93VF$U@xX6&_A-*mJxvMce>bC40X|>3G=PWrma9X1bt8NG-lB0EYHj{ zq$T*FTJz(@KS z?M+ObrkIuOC!-LCtOx~za=wQ}$`T=YIi#o{eo;0lqX2WszNbM9vTdAqo!?;6%~`_i z#1<31SLJEyi#qM!PZ^7XQ%Rc>&~``4hs1*2)z(sLaRTFi**B`v-mVmqSpOBDm;3NND#W)(Hi`HWBfpFpQG?)B_s0We0p#HUElz|g~a z*+*%G7`Cy`>oc91Ei;mmj2XsUOj<^l_7OrhmGUu5u9cFOJn@y6)VD#ymVG*fUQdo0 z04WpK>W+|7kEj`9MzRvK+T_K(TRLz~r5UPYxLIewEOefGyYyfyKf5-^gG`#OftFYf z7jWVPllAgV~s`SkPp*W~U&H zx}dcBUKsTEbos~M@5@@!Na-zTg~yChu@llz>ENz)_1T%4mSw}u4}t=PbU))U4DEO5 zS~KzZozY7}cRp_3;{R!48vLXOFsBH8JxW>qlm{YqU@oWkWvolCswD=<5Rl(s^=)S! z=f<&>o<`*H%9T3lexc^ zVwS`MKXyhh6WM`y)+PBs#oS+*oI)a*7MO2Dt6lag77E+3mA82Om$wMKD`3vAb>Lop z2mlCMaDSgCPh6PQUGZv}urX6x(eW)AzE|QP0yeWo_BW?y(~jkV*9Ykq!b9)8AQ%!e(MD6;;MGKdY8Fz>N50#R|plq1xP-!7;TL zIMaEnH_CBbsJC#`8Y*n;%w)@B8&>>5e29{3y^{&$SvOh}w^DP|;l;K+I|msJWT$y^7M6GrVK@#$q7_`M)E zTtuY5&f5*W*|lNQLa){vCEX1o)V`$pm&T2QNKHlsm*Rl3`W!;`YY&MJc@6-Obep;q z56Z_mES__*lQ>F{ujbn1dazS8P%K4?qm1fh!=%~bXg)Hsr$={4n7Q9tF1828}53*_=RuS$Tg(CbE z8$+1aSD zsz7S*22C^9P7ltyX<(jPPhp~~6@PT~)!>F3wBUE z^wgJZ@3{c@h{&$TgS=dr9yMi#PR)b(t4sY+A=_;Yg<);!AmMqP`?bliC^rylLw`Cy zC}5u>eS5`RmK`6BNBOQlAX^&6riC5pVg2z;wVuTTHh~gV-jVyHUb55TKzQZTRElaib!kFyc1*9jNN4aBXRxFmHdMd&d}j?`rXa^KwEjHRo_A zz26^`;&=;x4`0I@;YxOR&LD+F8kB&bS^BK3qm&#=YFjLAHg<~3um*`O$rVVCGFk_^%teaTUsNh zDV?Jv{X0$fjsT&NAadn5AWQ6^Z>&SFx`PMSU5fJa(9J-LrZkqe0h zIhTHIxH**!C-VYpsRC;jGkz1S8%G`k7uVvf@9?tRrjKax3 z%Alf85g3&4$gXs{$*Nbt$#p&i%B?b6GEtXt<$7ISke)$3=FuAJlW7%yM^Fzr3Jay@ z1i&Ro2WgBj=RzGs$~=FBZuNqoe+(Yin+#;+Nitp-M&oh%4D|zylwuO44s{vGwKLqCt6vZb>S$7OuEUiL#WOG&ydVMo>8 z(fxV>^HLnU3Yg_3E$FGV%A%1{ry5V^jg7;Wqhc@BlDY&8dvt48>Zpq|4u|rbqhzkB^!_bQE{jNPfSN=!@EjF`e*J44ry&u9tyS`PCH@ua~%MS$l{;08P z@e(%@;Dz(OAXB6mgq2~2PTi3BhOb14?*tm;46P{ z+(&A!*MYga9s;d{%!tSxBYX2V2R}Xj%HM%hE?WGWy&ud-=S;Hi&41+aGHunJ#9hhN z$BagW!tUoHN2p{w3CC6-`lmy{jLmDfD$jxuXrJxNke0a8qF>n(j@HUc#JV%rcQk6& z%$r0*ZoMz1@)cDBI45efZz@RSZ%wCJPv3cU5FT19&=tkSVb$VF(qI}{y{W6hH%|amwq}JYM*}vM z9Y)1`&x*J|yoOnYNFVtsRuwwCktQ-Nz_%o!mak}&koNI7L~q7%k8+~?jmZ?;VL{YL zgP!jd)YT=aL!S;UcNmDNO6{?_Vw*<>af8pK=c^8l{y@#rMg|*~%4foY)H<^FqyV4j-kOd# zA>!$|$^hfOTC>9iiH(R}(JAH83%kc&T%RPgq+|`C^TIpCKDcf6jSg!z9drA;jjT0> z#-^g*Nd)DE2vv42>t6Xx6aWh^d*~n4{a{$Y}TGL@W|bu7xk-+X^9FKl%7X|xq4(-(^o{{`Y8iuTPVYRwjMZm zu3sYLHa)vjCWd!8Xth~B8j2P=)%HC+)QwcaR@W!Pu;5`-q7-xdAopfg)K5VJatn*~ zK1T|Q7iHPm(6*)nuF4sJ4>QoqX(}I7kQ5{QGqvtdpw_l$IY!#j>gF%s?i)ddLrU^Izx|%{smIBc+?fjX;G9 zqucj=3};~h^Sp2AXO&CNA<~zK_Igc9I?H_6-%FV72kbc`Fg=O^II?~Qr5rJhZL-Wz ze;e3T2-I8O0l=goMqgjcB@Y4jRs^k;qu@*y(vW49mbOsyst3>#Ob%->@0%+}Yf>)RT^j5R`=K?S(^Fs>-8o5#%r37O}5mnX8Kh|kp7xj`_w{k^@V9rnA5VU++MtHof718MUGJCCoF50~2^Nd-sJ{yjYXu^j z`F5|KH;ET=Uvg*Z!^=_oc>DN#6+NTH3lB^kH! z{W?u4Y@wNz+c2d08%Re3R=3W&BgOk-IbJvwxoyrhVMx5efTOC6zbJ=^JA|Ow?EbpS zI^6i{k(#`o$h+2H?qQ33Ue3S;HU0J%(Fu!o*>qpAi1`s+C$IGSitBZ?r60L#&;8zj zKk5S$NAaIVJb*WJ`gwP4w$Q$G0agD}c%5`0n*Qa2e>K5`H$kL>tW8b!Tulsc<(Ap1CC~#tSfzd(h7VZDa{e7@QHj#(|$O?0$YW-vJ)BE z<(`sU`qCc?3W}}??K)j^nr%wy8%vwYXSYvYL%HR5qBSFmky@#~8m#Mo)^ZHT&*5N7 z*Ow_=B@J1d5}~QuSjZrBW-uHmk6x9Ury*jij*;&djoh|-y>Bn#+&$Teqg2O5Yoom; zebJn4Bd_Z5YG5S1|AV}=^wR-7Q&Cw#UMO8Tnq53hXmMJ+rb6{}ok7Hrh*_Ufo^%FLP!rrk{4M) zvafil;VDZ{ok(x6Owmy1{8@XZaiU&;m--_p&HFX!$qg&IY>;YB<}iYK72Lc)3zacY zOuu?(+prXArZ}{Av=%x-?(|xajk1GMyfsTumsx(HwUKc^7Z<243&{ywBnrXq`hbNU ztlc2>ELt1$@*R5k8k)fsnMR(&StFw_b0jsgklw$tkE=B53de(FmFIJcT>K7)!SZhT zOXPsK`jF$i@P2wd?ut$IJB+d9B7{03alDLWsn*u6RVNS@TVA7ZY+dDaJ^Jc!AG*21 z$tmEN3e8iy2MDy-5a_vFbHhlY@nfdO&DWxsshKJi7PR|c8 zT_tmvI*OttY4ij&*D7Qtd+zRroEA&8oO7FG-6-z)@(&a?fFqLYOjwo>yF&UHhYw|H zy}4K+%_C`o-}${BMlbsye{$X3CT>dykX1I)fd7PP=yYfZm)rqfCq zgiy%1$$4Lux=~oxR~B`}B#JOLVLE(&h19T|yD!>Y#F&UE*pAw5W4^RyBfNSO zxASlIF9J?QE8y@Gr)?gUO&GyOp!!1x?&c`Y&8-7hs0>_fBf>4S^2f`UIZ=7g;`mdC z{^{H6b)E{DCn`#=z0Y~caSryjx|VK!17CbOpa&zf5n6M;lJ?aeTLoKP+{t!VCAUx= zwS;OmLS@@R`P#0y@m!4PwDag(m`|`g*9s5(0L>1gB2#6oh#P!I*(8&Jc3edP@B}GR z8RfB^jfjwQJr8OxO9N#M4mQeF{pPwId&qhv4t5>B|C`i`$U}jW7Suebl%7untVT%& z!!JP&(l9xW2y;s;WHxk3<@6*I*&G{ZWH&Nu?9W&_QGH%igo5s6)E!=%=Arh4fB;}_ z0d=`}0R43txalxM`p|;5w(KD%BK5~tF|crgs>rUlC%y2&4Aj@Zi;JWh-thv;Oq7B^cvM9S!qQYt@ys=83dJ{(!>g~l*#R<{G5h` z&urLkWA-i1o2&S=0#oD6DYWe@1O)d^ky`Y~$VA$F3#Z2y>TWFMOmhGPglGZ%>(P>6eIlA3LB(=tF$ ztLn4|?DWv9LZB(GTS(Nnr}5&HWIHk`PEa_>UmmjfyYn^r$xdClySiW1 zcQcVT<`Ve3=A1q`kgw z{WkZ7@6tI?joC3Wp4ms;UPJ~VXZLMdcG9yW!J(h}=gG)8 zH<~bhzpkIe=qm_60203j0D|+UZV8>GV*=5=?_NEX6`&T6;u@9jEowhFdLIYu6bkfH zg>z#VlUNrPU@7$K#_WBDX7o3c8Pbg)oD>f)h()UY@o$@1l9Ht&b;WZ0`?O|A&Pa=G zQc1T5cA+~CX}+Tw2)$Q-lH7>j227X|Q_Py9VLttUYHjDGsfb+UeV9tH7@^wv3-bgUT8n69eKKro06d#o zpPJ8oCw6|fr1IsU1n(W=4}aUw(0|1ozPShL%dnYL6J~!FTSVT( zu+&~}kDX;wYrgonP@C!9g5X{EJ(~Wi&#oy-EkUhSGUTJ*7*G6Ead*2;(dDA?nSwiP zYZKM|UF8XPMicM|c;t%KheD>i_2$+*ih(l=WD$&`=MqY$wqj~R2x(n-C)vrmZPSV4 zZcBX%TVmg+z#AD)STGZ6VVR!!k>PR$;f>wBXwwfmBc#V1fbsenxo@uDT#Ba|$jlu0 zRjq8K3On03Y}E!bx}+ZEyb|q^I#gb5H^Hc`o*0U;Q7OLR)I(?}d$%}4_I*~kO3lmQ za&pH3yT?i^Qi3_uyj+<(qCVk%o4*MG69O-{ z+|{3`g$~M7SvG~MV#sLFqg*KPs;n|>oI!qq`Mh?1+HYy@OK%h&H6N3lIWn%^h=tR~ z+tKR&9G5i^Kcu<+sq_#0SvwfMcRGgK)~O29qp)M6@q?@&d`;3Gb+w9wG5%2in?wV9 z%RrnU57pBcX)?UW$Q6**%YPz;?-U=0ZyRG8_bG1Fw=!5MJEU#8-J9!sGy*QV!qCKz zFK0|jov~)+&r?%=BGVxMf$+NtOyUt(Q=(DqAUo~Hqh(wdrydcBgo#%{8x1yBo)Ah7 zxM8izZZ=y==zc-?HEEaP=R1y?>K}agk(|+}zgxyg}xQ&E4u{0e)#p?a6Uy8nbSJ4hM5)IXMq*}r6Q7+H5@k5 z;~}NpzeKLr<@j!{is`Tr%U3zhW)bGUQqC2Ig2slyi`1o`SVi`E@MX#7-FaR&ZFbvXu&9|6yw1u%#`|it-if_IAd$U!y#N6$2h@4sPKloo3Nf8uN*X=z zessla8Mh@=)8$dIm*>9Ra?{M3Jp<{AZocmXoi>3eg->#p)88WD?R$>tKU&?ODL$JC zU`V?fwWOXz@|J0Y^|`zO1`t^G48~b;p7HMG>nrE98{t0j?v2 zaqWw4x9H6e`t#-2S$EEu8~NVykcZ8LkumTF0GWo3YemY^o@?pki}CLyTaV4CKT-4H zdRCTyVttS$csoPhBRoKNt^CTU&Y7yC@le&W`GKXAO#oh~{a7(4K;rr&?J%4msYM+m z=cu{L5BF1k;y#a=8ob39553Pg=Ms6QI4143dsB2=uykG0XSk>A#P`>cI%a66tOM`! zQo@@7YaNVZJ>tSp2j<#Buu0oNL(=AveK+^hXO$f?ko?N-_WZToz$2V@T%vK=O*^d$ zzsrn(`;?h_=z>n;i;kvFsYV3^QvR&qHv(Hh-8}##gE|s&D|&?M%XMiY3Ym+Hk>#h1rElPCfmV&IAn-v-B9uCIDQ=M8_sdd;2ajMyxqgB6FW*A26* zPzWi96mAC=&N#laY@YA;8xx6Wy~-0GT75Hk<|lb#Im&}h%l?8kBEp?X&pzKI9RGUp zc6Tl0Pt0LU%l*yPe?zA9;?B+R4LHzZqLYkTJ~jY<5cpBM&4ti53sX}9&F;|E6`(3p zr8=jtrjBK;=6DY_Xd5ASL+Hc%c}el_I14{X~NK^OlQ93_ftQLEfG?QD&^# zCNaYE^6sDT&{1sCg8E<>#|2Y0&T2IK^IJ=GhcDI3<&5~v!4LCpV^I*~&+{=-i26l^ z+C;u5f5f0B34mGE&W-HOL!Va(o{tDFZD>L}G6MIcpxt|nK}DzFtNgcBeqVoJlSD*# zKXjyS*nELgPo)wi!;I~Isl#Y(P6Zmgo=B;Z4$B=#av@be`+K{%4z_|k4qN?R5RJWF81k}fUJ3nB+E1{I9P?lPFjB787YT1tny1`;4fc@?XXvF$Y`(Mun!z|&`$i<( zsSg;$|FHY4y0B+)YM_~54eNwnB$=yJfh~*(NGb)LS(V~DQqAkcqOc-tF3sc4867YTogW}NJq!eNsz`ShqgZ-p+^ zPG?5*(HxXFJCJ6Ju^G3$4#zI zCePVuI18@tnRDKVxDyh5YQo$hi-+yMUGQF}B1*?$o_uezg>z(;&uMa{nwF_8{rb6} z6)b%=vt?QR)KB$-{4t7d$msO6DTF4rCAu2o;?o93x78^f8wtw*vlzuma z&MMS^3;sy$dBCbi5)BglNph|3CKfm`xFlw!Glq#*xU{vi2cOXtA@FK^;q&-a^+f6O zQO0%so*#2R#2x?_LNuP@PJNrj8B%A}b=V1C`H$CgX9&)Bm_X?kWT%#dH&j-_U^ChK z-1%28whZArCpHo|dc{@NLe||PLcvDeZb17BdEo#v&TVZ4ue`gQKL`ug^Yv8D2&tj0 zO8L`Gkl?2YpdVSK07Te`M*~~hv2ZRh!nT{|Q^`R?a=7&5u9T98UZC+$A%*pfrz_}6zfIC7w}Ics~(%zXR0G_1O`@qR5{Oyyu{IEgVt-5j@e0d(#-c_wKI>) z#LtXhRysK*=$6WB6GZuLVgN*u4)}{uTQd)lFW&jzEWoc2FiLIj+bIPW>wW_5NU(qB zOz0zv9t{*$A0wXC7zgwWdJH8l<+Bp)R2= z7O|;qyO^hbl1?Cvm=Dpv2~U-UKrHq-s7&@iD(*xGq~`oCOncq?AdhTbN(3&y9^Ar` zfm>Xz?!WXYV}AxEv24kpn!C-8Jl{yqcBWJV)g>PS#d|Vtd_0VWv?9*)6$Rus7{E7O zzRf~{MYSIF>>OsZO-{WCpi6`ikE7OKki~`|)sK&V^7R0n&>#F=?2D z$)KKzBjh{ofhT5gy~W^^#?8US&2Pl3NbIAn;G-{`WPR{JLT{1JpC_VB)A!%qQCr>h zmJRvLvLBL2H`(Fvnw8$L5v4_SW0M8r?zRF+=b3WkJNAc``I3&VAVuuCB2iCoI^QuK zpV|QY~%& zwr5dl>0a*73*g!@zUW!COqC!vp?%h{TR28v?}DEc+jIZ!6#pfdu+rA< z+6h2qk;P=Th?{3CuoH2DIBO`|r6mB47P-H8dqtoOLf&>BRQG-l2T|s?Z*!UL&%Xd! z)>S6NGOFwyxbKW$6IZix7DUral7OSuIR=@Nk9~G^0P6Qo2D0!`N$}f&j$ZO5yF92P z{bCPURT4gQWe{a03K}N1@fvmUM%GqsEN;~DAB=r7gQnG1+Co_h``)_XJXfaT!@q3ca;y?3GIt zai6f;KRth9ztXta?y`jmqPZyfO+e#KPn^k4B#cx%Tx1zx=&o2yhRhR76@~$ zR0KPy3aTxhndxbL575Vp%Z+XIqAa{DnhyTS(?Yu zHs5JGQP9V1yzQ%8U=ixn6q-lmyw5&||FyJzd+)Z`ZJZPv`;|XAazcf6A!wIvsc3&~ z4I*r}7$Db)UEoAaE@RpS7J`4;oA=8U$^Q#Wvk1)#Xlu)(n5<-RbgM;kzev)2f+z0= zJ1cG}7Nz2}^iuM!i~L*Pi8m{ZgUV-KLJM3CmiFo?wyRaLtnRsWhDWl(=!oFb2_U4lQ^qS_*P z(~r3+vG#Q}8VX_xNl~IRD$-vE*tbT1+^2~^fR9zIO1Hr`lISZ9RneKGVowKH2({$B z|Chc-%r6V-DmLr~E=OzrriM)X@RH$UNy&Oe0ehSB2tWJ&33p!G(y%ex>@H(ZBX(AJ zHPSv32+U8qAdC3vvG@MOvF;f_H^A}DD!lv!`nxsJ_L}FOkss_e&5OZ_PeAn{=Mda> zFU^FtBKuFCHme5_YEA4+h%oiCcRf@Jjuba|F&ggakV_ouKli z*x~|71%7l%r5t43y_1cwGutxi6T}SiMZS~!ISF~619ED2?n2{hMF5_!9oi-Bd5k?< zW)WT___6^!ZlRf-h_ViBpIE(DJXNPLedn(UuV!JTP1O&K{af)9QyqXCYSH9v)1N&j zxv|FWs{9n(uugHi3_yCu?)P@ctx0+HCPt)(YI=pN>iKI^%Jf>>2i?0Ur zGxisFPGFM)De(FtVw#q|0bAyHR)6(^__(eA{qt#mHgc$6>h>YuS7{Si30k16{1c7n7EfO?3z1yFIwa zPGhUqGF>U{A5xMRGD*qnfw5yUxf<*C1z^MF;#Hb_2z{@%^HeT-Y^XL*yPzLK z5^H|8O51IgcHun@7M;0}ioawd?InecIUON5H{FEmsw!bL&o>}eEMF1q=9i2C1()4e z0IgHM@dn=e49JegFQ)S}@G&LyPI;86q!3U}dozY$4G*SmgkG|>pMqgj7qe&~ozJNd zmG1$0OW7=ch~3y~=-46Pkv^+1AuDNfC|OC%C=A`j6d|%%_K{{4a_E!ow+0`JRGgB+ z-Sn%!bipZ`_B0ZrQRh_!#Lo-|3`Qg8@&M}1$Ad_?8bXKLtFPaH54tfioD1eFShjE; zcN0)jFc_eq(k|VzlU-bmumkTS@smN)L=_f{{Z3q1XfAueM?}Jv3#hz}OkC+cKs)C= zC1fN?O**|nz^HMD{y2-#pAJrYzbevDqvR+qg;m@NZVg_sINpi4<0suZKQ!uc^Nj5K zf_;Pr;W0sSCst<&0{^}*ZsO|0e-|zF5XgBWRoQgJBVm*x4<@AZ2>#q_`D)06I>#~l zzo#EcByj<6OY*!f?wwcN%-ORE<+tijhVwZRKee^MAYH&3-5=(}4j_g4$S9#l2%N+D zQ`I{?%}F`k=meQVLR_nol9C$n_OhzZ!EMezM$O(;Ey?Vx8JlL2%J#IZf52QWM`xfJ zdRqM69_rfGb40BSVb5nv9Or|JC8i<`R{osYp%Z(qO8$CCkEk712yuD88Z??X=xfNM zkI3v&R>s~tu&rziWCL_TOdGNN^e5zN-S9CkI-J^zC2}Y_U7Oh&4sRLw6m_{LTy9^t zVpp^ZbxmR-^_CHyeA%e4AB##S*l8F5SN&uzE6 z(Z6IG+Ix@kK0%u8&FS!$+g{t!%U*RS)bd~BzNMCk-ov)jujec*@oHWhO}_7h-e>A| ziB`fd+&qNMaJ^$=z3Aq})JGqU5OEAqQglGAlA8ucK%rcXM0A72Ia10Zu$P^phrKtS z7Nmw{CsEjMHny6#Q_96M{CUK_P@Pk1v?~T@aERa}31=a?{HxXFxh|rc6q_5-#BuwC z75(+RMQ25qdNX{t1mU5LkY}uJDJBR^?%G0mlk;au<@Q5sq^0+v5{E5M=R&>rq14C? zm?=5_xAGq&Ppu;hM+n4Rd!v-NAfcoDb1U`(Z&dc!rjh1S^)>V8Q3ihfu+I9LCwb%+`V!<7BzqjMKZ8k8dYUg7M z@=kmVgIXhx#hyK{$AaOne%5&4U!Yfs+gNv0)#8_}NVoMClJ#(sz0VXL;?SD1#8To> zul!dSK}PpODFJE?Lzlp<061XTFCN^*L6V0}r|*WI_UTkp5nwe2RTW04>598XbJQcJ-c|yRbS;Q zP5ja-u}O!|*4SCc&rB=pDK95YL+h0+tgMuTN@Ks$*ALt*o=6TQp|%E{hW|V1FE^1; z@-yG%xE6(}m7Gf0mxGXCMig-=#3oQAWTVE<7UX7!+Fv|x?5b#uK@ls57(n(@e+f%T zNJ)ZKxJY5*{*ekD%${EX3m>Ku<#wHg2Jn5gZUs5G?apz!)3R5&yAb5LuP`=SCXX_#(nB z(1XuZwPt5=w?Iwn^4ga)LEiQ*& zHG~+m2H~Tn=pLD4NeUUpx|o*xKjj|$U_h6BP3gx#F~(5z8NdICiyIzkina)e0h?=2 ztHWL-az;i?TmVva-z8;Ob+o&3n>kb!-@P5~*^e$0&v{Ww=xYy~%u{+m{C$ZEh;9Wb zhum_Ye6WZr+Xuu0Po3K1)E{5gK3%CYQaY*^a^w-Lm@LLO8b=1{B!cEXQi z=ltFgE~VkOfGYzf-@$P)a#iP(K%J<@uR2v?e*DcpBH{7mL1nhb8>85X*+(Mlx?l-eDvt^ z{Z@Cby>pZTEkBvnAV|;z(=`#rME$M{T zFFa#5e-YA+#r-Znp$&k-ayN*FoRow>beO%}F%%h~T9?X!i)0e#x%1*5d=E2nts6z> zi$DOLk|xQ8G>jw^3N6*nMt)n|U6Cx4>&E>UP)1i7tmbLpMD?fF@p8zV@!nIrDbmof zNiWBRzb}dVLAO^T3=p&egTFzf9(d+4j8##bG8_-P=zg+WO8iwIBJFv73{hTbI`%#+ z^hSv<3CL-=%bk^I)#*=Mw@CD*fKK&$sR`2YWB99uJBqTuYb07zS1^vGK3Ittlch0JlxFtToXsPE&>RZqDx2qS+e|`s4-7fh? z>#$@3jXwX0rYN3$+(6AYi!`^J$4bpGUSvbPvXt+7=&?d*%5c;FByh_+B!OULA_|l#Jc6#kczV1l2 zNzA@WZZv%~$`Ab*1;}Zr-@igl?K-w{C7=exH}pC(!NBXZ5m^>gnD?I6P*Y6i3qQb* z>VJ?6m}+~$Q-8OpUyU;m8Rf)?$voBGBh+bQowNB9_E4u$n7JgW&hI_CV~utde|9XU z+k?=t>j3|@n5CD26)x}I5L+yId_3#8Qe~Ibfh;T7MJh|4N$^dlJ{-J%DBj1O|ef;!3 zg0SXls$vad@?84@H}E|5B3qA{)zRGJs0q5n2e|wMO*Y8WHMEsZwg9*O&Tph^U-0%B z0RS9%eO#bOHg>4>Qp9#yycQ3IGs017i~znKSe(7#xV(LmTWAa-8cr21hlJ8O<#r+iwuv<2SH`MjL7_9y8Z?P(1bvtF2a1YWbi zf4k8(!gag1XaBA;8)jHsn9_)jja3#UD0v1kV*fI1ujq)+pMC>OB8tn+sxZ+KY#=%I zL@pTrkDX~~+I4NpC}KmqlYj|V1>E$O@$e30L7SMOTI6K}K;gLiJO8Ow1f1)kuvO~; zB%;2z93D*E0-^a~*h>#ks1FnR{3s5&K8-Z0j=a+Za|Kn^AA}c=r|%C ze=ZE#b_6sR``MFx3AC*MQF`|mjE^6}=O_?_ef7B%{`#+8N7C(9nTOM+%|EIj(>&^% zW9=oF#pQRfJ84XtI2pu*I_x>)DeWSD;Bau%$nIk@4 zPPo&Y_ukcd8X@wUWcYIcXhV_~i^?U1*^T|q^bUMrfKP@JHggpWD z5h*tcOJTqo*ryXV9IjTq3rBlHa3b6!Wt@AtC*t_k27=O)GJ;y$YF;Y#J6$O+GJODY zV&>!{K@C!||I?}Yv<$8XOt63Gf^lKNs}S!`en21vD0(d$kdcM3(3|Z+wT*i;1ORJX zLjphm+lnGsE8)VZ#Db@M{*m?Sw*LZ#+{90Ra>3Rp^g}mC;{# zGvTx(l(qKuZ`z#h#;EN(_s;(CIxo;Q)-Td8w>r-&daR5WAt?ZB;oW(pwTvfR1=eN6 z0B))%N73;Uu*fSu2eRwKRcl_5y@PHemZMbHd-RpB`HN+(sVz@c#)G!YgPxvTkQXC> z%oPZZa9hUDeo==v6Y>KdSrHkRl^s^9bkf2Q>;Ni3Dp+#vXe8e7g~v4PO3ZhF5g&Mm zkZ?T$Uf0%!?;L}pRbbop_6JAy{@X^@8;rk!Dz81i^Bv7z>h#c!6)3dyzPL4^_bzTU zN&2vD&35wa8RNa;XdV|T!H~LfT95$%CmR%5Ow)_iR*WnIs1S2O4W<%AADkp;3#`(i z5xVyg5M+EgDK-4F%!b41DlSq1Ouj8itVZDNmu>O5Bl*271S6cR11x|(v_SMu6U;#! zo?eQW!(nS;t0msW8s@i4jZ-%P62l-@V0zOme4A9wSd-zL3~o;nXFZi%`HuUcYly@- zQ@3&&u&MJ}v2TIgDJ7Uk!lIz1de&5Ub+Q#uL}UI|$-a_`4agM)+&XkV0+PJrq|mS{ zzhyyO3X=T$wMBwxQm4IT3u>X9^|4?B?RWoWZ(bmUvYpai^y-2@ z!*&(Fwpil(dP2JVkd&J>Ea$E{>j+J#q4(9593KXakeK#mZ6lPjS_>0le#Snb zh!kerv@50A{8e%I9SqbFocJkJhn*m}fPYE`GwzC&Dm}nTJKf1UfY@Tpe}o;16TX1HWYi{J z?4ugu0r2nlGD7>U17c_YPuhW4n|+UjTBYXaOT%K!gx_03pO5}| zaHE15zBURk+DQ=_o8cg&bz@mW{#l@e7zyx3cTONLYabJC2mb`#g_g}ZU>bph8zS@9 zQ1>LGTP&}uDnOUYo?S5jan4ABwpB0!M{XL({D~G12|)Kwtq5`7yB!8SuWT&Dj5ZKn zmH>#y2H3Npn`rGP)(?!5TXw-8ok{qdEbYZ^vV~>yVl3p&vMu!YadU>TDAzP>UdtF% z#U3=u@-MDuec3&RK7WT7RT*LMC%`V2&mr<>$?~>5*x*w^B#2dZ9kEcHF zME2}pd`BEJ`Z8S5BGr3Hp+U+}9$XWl<1C9{Ffk;RSO=9F3hJVT@nxWk1EEe?g^oY- zf}KN()|s%L*AfUF{1hOZMt&Y&{_`KK2AY+`5-ZD75|-dNvRRS~>zj0-A=$*er4mYo zgSLjPr%RwvrX_g6&s{>p#OUjRhq4P^Z;+!2@kpY1e?6DBK9}fQYz%h0N!hN zVo`5k>BHVV1aFl0@bMYDib{~ciN@!L&gw^tO17{n-)!l3RYfMA?_S|^HF-eP#+bC% zlkn6_8p82&ewY>A?KxG)-9n1~=L z$iKi8(tj{}-l$u82+_X{7qTU&cPU)@%Ie`Bu|NbQgr>Jg4ObiOZyO8+3aa6SmN|h} zAiv5uP7UjTpMssmLP^V3r|_g}isW;%mSUu!+rv$&zMk}i;N<4s#T#XCOYKXZ&K$i) z3{>h>)y9_`aV6-f&zISw*XLx_7{at;C-~YGrZax1o*Fcp`@5^*mEt{Q)?wAtk|ZrA zVSjw?n|RRQ+?-$%65pI};ObSkKWh+uTiDLm2{UrI!Q41BqAB*@r7pz$K2IvMgGf5x z12IwkvsKmnCmZix<0Il*ncstME5}V@dtWaRmv(;c*Ub?dvFnPvU)&SCIQ;PGRoLY` zix4>bNo@RZs)7A?A|*1WhRI7%#b7aWCXYk?a6<9_V*z}5`IsO#0wJSxx93(NdIG+9 zj~qeJ4U*2F5LFHiB_;it-yny4q6W*eQ-IF6l)Z0a@E^8CV&24#EK{$crRn68PHSN)tPaDU(Ro=|xI8p<>h>l^ zl%-8d7q#6ZP=7S8)oFUgBTyR$K{Hic!2Q zecoilUo>|yivULO)I);o)+nEP_`HH+gFec^OKlb$)4pS$>awMYX08RweF0_&*Kmi=6Iq|KDKW%zW8{31OFcae9eQbb)s zxN18Ife+&*{+=*0r|lUry$~6A1Aohg^}Mwp+VLgdJ_ydbV$NF(5>Y4T4_sxWiz8Cc zwh9QAN12h`s7)g_hJX9^ZQE{O@7ybwENt!A!bNs(n1iw19~J4p2BA>Y0W9J3cOw5; zWC=}1njvPtkJQc`M@bDr%0`HAk7j>0#5NhAh{pJytCB3?a}0EDRYHKN9nHEY z+0WC_DFcoyKb*ePk}3Q6wUw1N5Qs`&@XxQO6D`D<)Ae2rPQb_VU*l;64&phH>6@Fu zx+yazp$8U=9c1JC;_=&F2?9~!#;3#GLK69`5&op8=jGLo|e%nXI8a!aMa|-Hc?eG?#rgSTb(87ycfK6 z?XbV?!rK_B1=4&mKC4r4W5NE4AqVZyd!20ek5q1|T595(18-py|D30jJd-xN(({}_ z5=8}S8mu^YNwz4;Kb2I_f-ZOt1 zs$G&RQf$eHVp)=K4(+qDFG4dXKLrW$QNY=~57;YLXRN&pRw8BUG0@uQi9dN_47mXT zGcwY6M!EeD92_nKOAx+}>tn$)DW;3_1^~PLRpUiN+7(ImgA7?3+*mpW`N$%pUzmnK zeU+$nD}JT%-}gZND!}O1SM_Le(Y{Nenq(Gl*fbAq($nWVu&{5vkf0EMdaOMoa~w^kOEL@tRcwSv4lQ)71+Ns1QJe&9robmRG%@X~t#7`<)zinf zzfqFSgxk%VRi7DEe2BkrYb)_sr|r(9*swXG}-y7l7&+)pkPzZU2hBLEoh#^XqB$Vv3(NZJ`3j?{r%IMdvkcGStLWrVM8m#p7B#*b* z6`&IsUzG6OcCXKGu5@S@g)sdpC>Iy~W36^R|E32fI*Ta&jigvFPrp$|&f%hEfiv30 zkmV*4E5><;6FwBrmZNL>@|lODF&y`J5mcY`L9>PksSeU#$i8?uMR z5C0Na-qEg7iqcpf9$mZbFK%Svjr;YK`Z;tW>U{U_se3>3B7^dagdQ9d z^P?R>TkB-?9p|yvp=r=@H-Czto=RV|+zc#=G5bcPOo3Qt2m$$UHR^;A{!i8gTD0@rIkPg>*se0d(o zv}{e(#b}%@&aHt#BcDc%5$}r|8PtblJ0Cl%Q^551u!EDVHu&DM8UA0SWo~81KgamI zk^)eV0bj_N9Y0b8$@kI)j+9a#2EP`hib+WqTWY6q4t z=B+bsaEJ}-$V1?mK{sq8PsqmMS`$KR7i3`sUjNhzoueA@;yti-2t2ij6Bq83+AIh5 zbegDU5Ate{^uvR1B_KqFb{lr_U+8{sBwbdxExUwE`TkY?6mf#mdsYb^#|};UzqWfZM`@ldtP|CX`TKRktRO^ReEdi zHGz5-9hA4=+Xg4#kD(>KM*UkqGdz4ToxK6Fw)VUX)wq4k)?sXW z{I@p&)w z2(`dne^7@cSJvd<$SWfGrl~rfDt<6mmYC?V$KMeNJ62N#& z*|MH!Avx8*6UiNEiMo@QcpPHV(qPuw5NVb(K%^!px^G<*o;_b zQj6r>%@hTBpKP^JSZ~~Btz&e)J;p`(@lI zd+v9$_@P8B*MnjS>Q-K{x^kYqwTqe$h)oFix=-yY?CbTZ0~guTYk1I@%@uoC9{FzM z)%vM-ibhR;P-YWUG9gOq_huoQ5%3gA*gzdrQP@LORDpa5=A5f7DQUaE3_JP7G6gqb zyapZn*6qM1UVG3c}knp5Wt!kx6;c>NmjJ*>OL+ z_bG#uX(>tK;=t@t$&|2OiZm+!YCq@-b^BRe^=U0Lz_5_^;;&N+!O;!h--5d-9?-al z|9vU@skUvI2e^$LN>HB(Q49E!+i+B9Hjj^5fHR+)V%y>@`rr>ZYG{KHCm0KJGSRg| zQ`xL1=^dj4WSK+W<~Q|9cm4frFmV6E=J!ypS9qFmzI?%LQMI-SF9(XNc$1maBn>fL z(Ywamtw}wQni&25W7N(2i$Mo9`EWGPVceF^Bf`@Xq`hS$k(D^U)MNvbPqL}gyw>U4N#3T(B;t5l8{O`OBJXi!M;uA75p!Tg~jaa@V{ef9?n!8{MM&UU3Yzm_lUswDW)Mk0qH*DNo{WE{@4%gb$~5MEd>3eVfUkM| z<>nT}zZ}GLyw3X1nhSVm+1dOo{8)+PHPre6)UpwKnqks@c)O`(bB)W5S7@GKURPL6 zLcx#;sp*%9S@sHKngP=6=zBbpRsa+6J_>~V!siJe)?Fp49FD-nAl5+{Ii51z?{Lpbet&i7ERJMD zh4mKkQr0kgJeD}#POX=QH{l@+|o`0_|ILJa#>kzw|0Kg1rN9B&Fx}qHHBI2q-+RMQ!JBq9-5jmDdte zxQviR)9-nH#K7KxyN zJ-^Y+u;Qj~FKd!t_7 z+&BWsuKoA!yGdFOx3vQev-a?5Sk{j)?{MZ@>?Je*A`6dxYpgvJWiKS*QgAc#H8o7r zdKRLy`kSJ}K3SN47J z`Y0+U^iBYNUX={1L3trgH{p_RH0%E!R-pXE;70mdtL81pPwuxh^ZEaI!i%S$G=Ud+ zMO;V(ZoH~c3vG|>bz3r=h2DhrZ?|||>T6vF{7$w@^!KrKXETuwr@8d;gxNGv(!3NN zDy3PUs%{?%BnjtB0QE~Q^ggS{M z1qqow9YU14X?!lv2cBHlQzx+i&Qy{uHi*!!Bno9fv??L$OgsD%x!4GSG@)y}+xD}m zXpzknGiun@cKeq#MBIMk4;J||uKqy3rABnp#6c2;W5N+j7HFM>e4*qcKJofS{{a=o zR!_?6aj)qoke2|CT-KpUZ1#Lwt={~r70 zvS8TE5AY?k4^e%%T!}n&apAkMaePkCc5U(hO3?#${1Apyxb^lQx`(Q%DQ{I-ntvnv z0{wVIN#$SEHOcP@*C|FBao<(s4se_&P$-THgR>oh3Z7@cA0p~~v;_X5?d zRw8FEoJ5_9pRs;s?hjew1?4Ua?B__ zLNVhnbV|QGgQq2we?mw>dtlglg*QHy9ogsJp7f(Ar1%Mll#7YznT_@)$G70ucd@x< z@6%#AZ!tewi(N5tgm>{leJ|d@!JS}M+#F_KM^WggYzBpU!l0Y(y>X_8Xs{q+#5@TV z*-s%wUv9x&uBVM!fOuvs$aDTLgw@u5)F{AHhi9;z3Sm5Sa4e@oW|1(_b-iNOh@m_0 za%zfs5r&tDc(dppL#z&mAL*0yq2djb&Y02achC`J?zjQ1m95^`mhbtvY3P&NF^tvVl`xUX%oD<0e1mN|WEk*&}Wh|$T&ItrLMZ(W@|HZ7%LOF-e(acB3Ou9$vDVGn8fq{Wq1HZE@Wfhcusb}vY%7iJxqXY=r! zWU3SoaTZrIyLEcVu$q`i*hdDK8L54mxs6@>-M^STV(*<-jT^?nd)s{_x=&kA(?%bM z#)$d+FC9j|Wy34%R$jqtMnfGWt^swHp=7SZhB-5_XJ3|e71>w`%Rx)!equH{K;54< zFSnSjNjyIc@n~K`!OvRWVGPZ+BeYV4i1x@KU4kcY1FnodC^RlQYG@6lb$U0(<~ zP`gR6^OW{Fo{E7=nb}5`mWe9F0lx!yTZYV9x~5;91`NMG@@1N$6cq1dI4ns~$FnZg zbW5K}u4W#4aXlAb-jC9gu{4wh4(#4~H~Ap=QgdF%Vx~{O8ZtjQFEVk%d2M?FT)*9T zb8FDjG(GANj;X5bI|+0gGkTpvJioJRUs3QnfX1y+bGG~GS7jVTlZQYP9+UorXI7Es zGUy~(F;Mkp!ew04jEa>Ke(a_pu(~S`n{FU7u5T- z|GQf(^$x+Ov(^4Xa=Fwa&MVA9*ZHa^Pb2c%Z?<>n+kV&hjrOIy_W*97c5EcXh+W)? zBNq0bJm}S1z9(_K!pOj^`T^?RX@B|qO5JQ=-t+Sh@8ce_3%*SNa-=KxR-eGM|9{0_ zU(@f5OP`R`0AuzA!g-evjELTb{w$)P?ozuS+V=y9U9r6H9>%W*G_HOO?ek+gU{l(Xty zK{%qX5(WN?lNvF>x4t}~BL8w}>Z=2cg|>dD#KQioM7y2_9NL?WGg^jJTw%P1ITSob zf~}A8;OpF0^NV!_AT^wjFb)s+juYw6;m*3{{Gb;vltTO-d;8)G`Xtntl;Rwi7~H1v zl1!iqldthhnlLx7o?q{JV0ZNueirmUx_ZzrQY(qh1tel4{-DwHbzVYSF~D%7&2^)E z)YWV)&jtq6`rbq9P3_e>H)!S;vFx!fzg@=OkDqT+qes8f+i8PTd6EW zU<_c;#W6BHK4QF261uIciAILJ^ulQw-X({C_74vQQhzl;yrKqFZwJlDu0Q7E6V+`c ze0>0?zeC))p*0J$qkyNrm;#iul@npaH-*0;Iv%Zx40SnD>zn7Rlcl`Zhl*mr1$b8`Jw5*vSacKt(oe?py>C`Ci~X=llqU zW)FHV2K`-+AxV{xW*pFVB-FGw8Q{Xd9F~SWC1m}Je3x>{Tki_ViF^qU>Fbuq^FeCV zPOOBd58y_?+j9M!ztJcBI?BK5x`^gOtje4-;Fb8Bid8F%k8 z@)~iH8E5#Pvr&iLg{FGPb1%7c?E)GYW+T?`;VhS>bQ^3GRxb#>g@Ni1*6U=T-CzEO zVifnrcqhKNYi6tSzb5%gLoMum=A!NJ(k18b(%bS7q*YU$JIBmEm#Qe!fHT4gKhUjEcPutM~VzonPLp;%S=abe+5857$UW2hXcsR#G^H`!ETUgYzxAPv56w!Hl*ELRNhKprv zzhBaI6pMJG$e%?Dh@uUDwT|;@F!+n)2+6;PNfjLJe2uKU^cx>Y&%@y;zteBRVtExEcjNq{ewP0p;0<)qeEyNnOt<&#-rX^1q?mD2 znKZQfV`tuaGE{@KRS)SH&bJl`_SW&E!8}I|iRXjg+D7@ei9}h?vk5&3H+|@J$BQZc zAtfiH!q^*MGH2lhw-B@K-^y75@|{=cPR{3o4>Cnwts=!R8r0D2++3c4lVBpr+sKFw zji{A%+U&^CqzvaLB_{sbkp`DVQ{dF$p7Ge> zK0A}g!~O1kcNa_4xyK|kFvehb`#|gTT-a*XI+UEloJ?yv)Fuass<5APabgyu>sS)x z74C$5ivJK+x#xNv|Ke4B)s*p@66urL!o2Mgya#>7*1B%Z?Q3M2&XhlQaz}WzaE*5E zU(tta19=h#)f$vLJ~TL~n-xI3_`5>I?Vj0bDYymWzT3TJiW?1I0ly;!6~y8cCqyu0 zMQ4n%QUzFCa)@wTpMgwar;*ynVZ8gk6>L4Yi#C>Ywi4@NuiA}aq;rXxGx$B!;sdSvjdUHp$n&Ab@Ok%qHZxBU!QZdz@(@L36X^b?9;cT`zaSPHeRewS&iFWyBW@{xP?$2IxBolF$f;C}Q1b0VXEr+(+lz2Q1w~M}zq`yyHhj1-;`z?0i zc3b9(zskeUd%^26?rPhIBw=6G)VCmS&7*M1@iS>`LwnsO)bg6n;fjC2% zazjIzfDhxI^Tg@9U8BwT`ue&hqhNhlcu43ywyHN*AvZ$Pb&rhnzL|8|d3bFThP4$_ z+Hw+XkI2+^s^9%_Th)d&)GOHC3GSV3*g%RGPn;KThLEclrfeoD=;E-2A@xfl1d>84 z+-SiH%4wVo{VM{xcM#RmxiQ3Si&#!ynsndO8@8yvrz7?O$*_Yfe>^VMA zUz<}W22t0IsK;anJirXn!lW`pyaGK5uv<`LtMfFFvFamAg$KQ0FBz?p)X*jWOxh=z_? zZxubjN&NtpRpX?fkS>I?JLrob+7Xln|Gu&y)f9h7OA-r>t7|4WFULPXb8Yn{?}2r; zTUW<0u`LA9E3aQ9^k2?Km_cgu#vSm!PN>Q}MaYM+@agI4RV+al*-keKGSWOT)x2MK zu27j{x6^7gZ(`h}vT*fHrWWPH(f*+JswwfOdxYX0OzLkV@{Msl3b0gqaR^5bX+fKP zfisLm2k5;!k%ounWj)?kNkNqg4>yRlS!eXIu;>-4d{#V2=Pck~c~$YPcs$P~0vQTTikRH(^m@|<-s@}NBsc95m; z>a`P7zWyFKIFK2CmLd({Xg@D-tOFS$&eE1XzQ~g24-w4Y{MJ5#mJ2~ys zA!ve>IQ;59&$9o2EPxwTMaVFoGB#L>do6Ft zVTGu2C$MK|pD|#f^=+IF?6|S@_F-B}HTG z%L-)sx5;mFy$~9L_mD3JZq@`p%GfFp9wS66ytFR~W54q;;q%-)cIloRoXsGYF%}wN znW%1`_45?pL;bpchN82CYs|>6&a-apO+CqhPK1A9r+cQfHBZeb$Wd$MEQlXeL~j2K zIIsG=X*DLIO!Q}4=myeO>oAdf%ERv`x+X!NM~!3!rrVX5aqoSmK;o|U(qh*&W5w>+ zb$!8zZ)Fb$_KmjgXPL+v-(cO6KrxGby>jjFeZxmbCIWZ#85i@MIWztp?2jf;0GpMS zr0KnR^1c{FsSfaGTIfz6vk0)Sb9x%CTxa-STJHG2v>aDFN!;Nnc10kfGGjY!vVG5< z^9;9UIZ#ku2IA&pb-NUZJ4H-`N70{N&~Z>R{Zp+cNL=bX!dX~YU?&n|p%YWvUe_u; z+wIq0;Yog^pDRhyxBNP`-)u$YCYp=8K7W+QHxI;4D-hb)IzpPH7_iJz;S?);mBsVfX2hHeZ<`0q5GuW}b=@lgFOM zwz%0o%~>OQZ%LaTBTTLFjrO}s!pGpxecvd7viUX-DEUeNJ8wkjH2o5l*037D07ldI ztNGyxtCZS7D$OjVRSUS1*yQb(B*2Z~?kK;Mh?JIPN*JUkjQ9cl zkVR7n=n&U;aSx+!T(g%9SDZX}_3*>H^9kkGw%U)767BgV=jjA9Qlf@oJx< zy<}~DA6}K5a$lcjhn41EF$4rQX!0O)M%NecdA!^s`4p8n;fIwK4dZ4}wbJoV$qr^0 z9K<5b@(Fy7nH#(y_fx^_u&GZbw~a5Q(vGvimv~RJm5WDvH(bB)0S0^Ks@l*u_8y<|h*HW&BbIr-_U=Q}5)t&pc-;V+%Gf83 zGl-UQ4XIrg6{7Y=!7*2uEGXXzI4d(d;o}Z&ez1F?% z``&B$v$m4C$c%H1mT3S9_TwIup35w6cf7xrRq)ocAP6i@XHxY*vn5~E5%{O3& zgf4_z>W^!$j+x$R+=SI?mf~nlhd~a}ONCSpnbX7jGN`lLZEru|eEzF1dw)b$Y;!-X zneNtVFO7#V>L_AeP@|?i`a%O7sMvy^wdq2^680 zWMWdREQwPj03mlG_zpHAWUV`D9RFywk}i46p!Qq1Cm&lCw=KVMKaYV(`DNipB(ZLj zzDtVueWD!T+!JYehzq(qP5DP>uiIewPH8#egrT3|?X9+p%rY6v8MBc`zul!7}H8JPcTfx-XU~ezlWI_RKTuI)2&x-eY zXcS&L9bCyiKI?U>DB>MaW;? zF}Zzv50+*8Wdr?SyQ8lEAOVW>(Io22 z4QgdZ+xXB<_rJ_DP*sIVIHA(o7ZfuwK;RtZB;?0`O=T>Qt2f@Hj;Q}BZJ5=>3}N9< zn^uCwWHNV)6o&bQ;oOFKZ!p#u>buzH_r8NKYE2>e3^%sD=KHkz*Hpa7UJ!81Nj4V6 zvSFKV2b$rfCh#u{{Og#YfqlhxP;yhkCPdDcwbQC{_ljiy^8N?(^!-o72|RJZV9>$6 zdu!W`jfKI~y)R!aIpyAX*YlTbXI)MQzvZn#MMz*KUIpufSIfWj0{^kZ5s9+-v4pJ+^sInLbk zO8H|Nh;3%<=<33L_)j-Fp>NnBQx_!Quin3juh>ls-6>t z?(|NjsJ~^5?NqAjP>`DNYy7z}K|=fl@Lm{;YehSc?)&UV-KI7Ww6?-Za#zDB2>%6^ zfvVYW^xD;9KC4%NTsWy;zdZ3$FoMBGG*3N{Z@{>Lkt58gX9c_zhVq=IOy^Hn1J>s-B~BDkK!67G+!1Prvvi4;r&XD;f$IrOh87;FZ`yF z9D@>KnvIYQ_?h8^44*z_Th!At`>#L(y0y@}X&(XE9jmK9sQ zsN>}2iz76V_{kVs47B>|*%p`K&G(l3#XDrcq|LQ{De(b-JXEn#y(faBJE|uR=lRDN z`uyP`_Un-kesoVJja0k_R1R+^DVTF#?_B=+)eMWIPc=2sALxXfJ>H;P^mVX!Dl(3T z95fZ~CHAe@6Ub6uau;?y> zh?H=c*B8qD&$P#UN2uz$wu{jAXAhfG!m*Ti#+%r!WILGwwOw*FlM4Gv1~}$qf^oV3 zr>qpXxIk~b`(IwUewSG(-|LssA*#_{oFBUAxK$b-*-}Obe3d-c#^Uu?8R6X-`-(5Y zKTY;X{69RD3AKo}d}-$|dGyRK_UMW+?zpH0>xZY@s6s7iv(`l=e~*tL`KuCLIL23g zYRv0B+*3{q&JX&F0cK*nr2Z(h`E282(`ZO_@PYE#tv1r{3Pv@h6`IeOQJZ>nz3=`# zIj3ExCKTDGOqV4&O{KnZubl^y&F|tLR&ruCA&uowHeyr55g10c@^jAZN0A9UO zJb}M>LpU(0!Uqv=x#kwWPUHEwPQZZKf;I?8Ua>`69aa zli0f-hWYUscm8Zp{zzjRyl+U4*EWL+Sgx52p!Y^}BX(x=?|sKjD=&D7_Xou|v2Mz< zot7TxpPzZ07`CN)nt8oX(C4fuhk4eIz>Z+Q{_b@6N&kKas>084&>`6~59agLaL3?X z1-|B5OxZPi7np?QvlR#YNUUVHDQT&wwdgKXg;@T2!i(bhw2gEfa_l6nELIl{&h>6}VqNq<=$_JOjr z50Oo_hVyKW3Hx5EDAPxKEy%F%q1;uV3EzQd%#QCl@FrurNRI6z<($k>%p(SWg--rM z1@Lwhi*pD5%f>Sl*K=5GJ#B=Ace=~u6LS@PM+q0641No~E2Pc{<=JNfx~a852A5C} zptrcVnB35y@OFgr@ql2$TU_GTuoDeZon@?5nerx=ii)0Nhgz9z z7SS3DDY-U*A{aJi+(Nd3*)Hx0IhX@4cc6Lt*&*c{(i>BqL1%5o_Qn4Gdg#UOoIRvV zgj6MLoaFp$ZJTMuHlyQsyZSHswsYujGiDUnZo%+8IY{D*vY*yN)Qy6GP+msCf_}J# zO)L*{2%n0oy?~f86MpG((>(Ed2Ko+5dPh;C{Kym}a>jQX;OwO4y&tZ=Vm06AZihTUZeR zrjNqwnX*>NfL-ly^ourr_n5a7l{qgSTJL*Rnkb3bZzR%)bKgtb$&n6MK&G9FUYq#0 zd|a?gp`Td&9Z4aIeDghxsrWayhPaRbGHuPy&TX{-;zth9eT0^Ll{zIo7v$2kG+5IZ z!N$6!{U6}iSPbx@{VPuB>|MJ@WPZTaht=;3lPC4Qx>Mjb+umMoy_5wEq@*6Svj}wQ*j;9>3K6qNIWmfqN!?qj zF{=ad(MY8|se{pzlamYO!p9xVD;iYC-VT<@bQ08gE)U4OF3f^-A}tR{p^sW|_+@Kj zZqtX*tydnA-H!wT9U%T{-tIZ$G4j1kaim_}qC<42rpj zKJ2z=@Ejbt!6PORyMg63UbjDevbDa~ue3n4#4;S&%UG1t+6>{7$1U32mI z&zu-lR+btNZ6rO0zp*X~_B0*&0#{vtnTugFiM(hasR{u1~eG`dY>&YZf4IU6mW0^5fim>;jeb^N}%XSU(IoZukCca^w&u~R~7 zYVrx0f5Me72qf(KU8X4_aK!WsTRvra?V*c)9&MB1Po(X?vv{$$0;X7gT&Ez)|g%^>uN935%E2@WcXORO%RbEU~&7lA)#e(lQ zqr1W>QG#EccsF)-q^6_`fBM3nLv!DQ_Lq;C1@wHOt6DB99jF_2iI7Rv>qF&F)Ap;+ z^z}UUUjIrw{dWyGv7W&GqiXr_NIDZQkKrkXeMRK{4b2_IRc_ASxZUhFo-w%g#}(bX zenSaklPXXV=!mDVorgsA;y6=6m8lM~C*5`ue=56S$vP}3igkl4kQ%7}a?c*oL59C9 z^9rT&2X9!u&RS$iN>kQgWY%~Qm^n4PdnT$BJGDM=q4+=kF&Z(p{H14swkma;+wtRB^pLRk_z78fD>iHytoTof zh^Kfpur{drktmLUl?kElCwk`M(&M(Mk#)7-^n$x(teAC;2vGYCQ>C@t`?Z^tMy^$* zoD&NAHepokX-WLZ8L!|UlJ)0wyutUpUjo~HjNU^HbS1IT@$7Ey9=^eQ<(~du>F=;d zkJ!Z>#+a7L7zg>*|6FLFK!IwG04hfT=C~la{b&25rP-g5#qGX4lJD8y2_NE5)yB0C z2DOI{av>&?=Sn@Mb6W6)k|qfHo(eOOLg%Uerp%t z^UP&Z7J9>Q@xS-Tm+PJri zT-1CZ<9r{;-ID(fP#Z*Ca`be7|MiX3kp6{q^R$x_I-l zIOn?grsYHql#Qa=BVO+G0MGOMCwH&HbDt0cOWH6yVq$7mSvG+6VLAcfhCvVQgZCe@ zw!hH@n1``?hTYy2SEi!D2no9Gf&7ueg(Gc&Nbd4&{8v;{fd{pRk5%y3ma={ZH{SVhoAw&qVRR|E~#IaP@ml5$o(yZ^p?S0D6ZuLV02 zFsZr>qHKcnf?Wgwy7<*2o(_HJgUr{Z7E28Xqwo*@sJZV0_4pSFQA=L*Y;8YcJ($gk z>ICSruE&3W4{GyuXoPz&@&pnXqDMq+sQm z%>7bv5zlyy9vyK@3VIC_3yZ;XE% z4#inmMMKSHo;=WbVaJ6C$ZA!LnWin2bZ3rbQpR_B#df}%g(x>I&2CCQX@Vz;IswG= z=+?e((Fki%iz62&t$yU;{;GAmPKYgs_wpz3i7o2w?RPUsF!PHmOBX36sf_>LjsRv! zRw2c$atB@8PpHsnZ}|27HS}YT{sdjMkNG^re`5{T*)eqD!wM2+?tFaynQs za+xm|0jY%!-!txG6G%}FBM>bFG^RXpJYZAY?;HHd633sc=VKtflq78Y*M%biocf)4 z#iFVjyEVl$c~dP4d=q=;oCs>`Ici_1S6Ah>#T`YwDBb) zIIwTPKKh{*D2_`9?h!0;Z0ILRyb$L9w=yIzybiukVX*BAx-3cmf$HZ3s{3+2yA@cz z&il3GyP)6fFx#GTm}TKHvL^lMBHrKeXjolhuvi4Mjxo8zzvKlRd@(zHFAQ8uiYj^J zKDMl}e9nJUy=?85uj zMmS>CYZCpJVpuv0g%`F-j*Z|Cmy}6!-?x+Vp)m|jQ1v^hKUr?$R|UuaUwlot1ivZ7 z3-XoQ+?`d&!V_Mpx@(eIdV^Tc+5x80A;U69oB!tpATVRKSb2OSJOM)C71zP%@4ZND zX7cQ&^IqQ_v>j}rgz#zVy6WOpcvlL%3r=I>s2tX_+ShgGkR?bM_lVVuP|B?W)f|xI z+(=>5$lSi2nB9t6SS!;)by)ec_-(jfht^Q6It)LJ!37O?J<5b*PkBe;5!`;`t;fj@xh&*{3}JkTjz~#VYkCYhJZrDMZzxv-a~lT7K+?E3mWG z-KzGFcRh>$in;)_<>$=_7a5l2LY~Rn;S&v*qO%Biag3oP(xdhk{r#2Lx?XiTw5iL= zlYr1Aj=+p9qsv}u|H?(`AvAaLdfYzk3+weG%zqHRKJ%v$E?+SBFotmpY>HvQ^LHur zabO_j*2gyoWRXl*^{Sa1Z5%r9>)p`~@8||@+kFn+?M<})c3mbShwsf2nU>9tz)pl?>K&9v)2v;3w zcROSN{33_~{>92FLx^i*8k{qc&A&LwQ{RtRWbl zlJn)zpkK$Gj(ln&0o&MI^z5-DHp>%N&&I-&GBgupz>o!mZ6p>^?z2l|w(C?`LxE!! z0oK_I2wpblCOY=T3sRyoRCpa0~}9bLb3fsRyMi-L7~`|Q6=AJF^}yYR)`0<|VD z;vxHWlsj7V^5-PpJPDg(L>Xn^ugzSMLKVQerp?li+gKL+ItETF!mO@_)nkGh76MVh zCss%OtzrBZbUVGN2h>1Q`oYM^z1NKjPLk19_hyP%{ZR zbd9HL5a^<`ccyuM_1}8@UmL-vgQB{U!*fv04mZKiOWHID4+vZ!n&PMk`4&gYUQ12% zw?T-9PhI{Q(+FQZ2Yb39$&Kn+F)kD~=$ic+T*UN8q1{}a6Yj)@d!&znBy3jT5aR>B;1+YXerfS@mMnRDn%I}sJoSX*dD~unjBR?|-W3FO&HVv_P`}e&mj9G7X)hQZ;EV1#96&wybJc!I6?$bhk1X_1DSW`>{cyTmX`G zp?8d?YM(d`{E{;-3bf4`b3iBP`zw`Mn)_*oNf#bt2dPzZx$eF>{_u+UNqa53ui&0d+4vA)5>#QoGU+~6+$dio;U zHXS?a&^ixgF3E;-1Ud2Ls4`En6iyP$KYb1vRZ?nVme~KN5{Fa%are282-kWytPRlX z#2y{czDe5~#X8JXKPg12+QF-ht_vLk?(Bh-RNuZoui*zUZk?fqH48$Kyt%Xfu96lk z$%l6Hy1Q9F5#X%&%FLI&OwbLgfiQPuU_8DJ;!2|D7i_-mUeacnnE%tQmGHX!?}d*bXQk_(8oqQ1t~^MNC2&A|K83eKv_HUDsHtmL ztcU%s#)7xgyFP{^_7zE1{r3>WL>XPs^m5)sJGHarUP*G>3vBnHSKd{4qZ==R9;|V) zhdeqs2Pq{n9^d&*VFHE5M!N+flf@d~jzS478W-qS7MRrY3js4b6BprWI5P(D9{hcg z^YEz|m%D-d;IP7VJWwPCimYA=sdKW*hw$XDe##eI7RpB9CR&4NY(A68|`a7@J`;poE8~aF=KUQ8{K{Yk;*78G%9|T0I*`lE{OrH{<``hE;3^9fIOQ{Tk2-?PzBeY8rT>`H*tigl8aHi8xY_5}d zAE7TJsQ&8E`~VLr9T#Y(R^Bw0-})iGN_w`e7`_P%~B1jrRg+S^gU7w8}4V* z4mDF5h&R$YC*qC7*$>zOO5584*lp_RdNq-azZjReC!+P>{`~VdM}G~*j5dyvc0X7^ z77ekl8zD~bT{$M@TxRi}Pa+^{gV0F7h(G6FQ#8&*?VRcC2?ahQclj8I!V*c=!wtb+& zof3}X5Fy1~+`s|)?u&Ph?*@Djw4F>X+t1OSRI^tSk*W+?-LXr+sxn~S#k~U&uJn6o(EgI#OAbsH zc1I}cAr^hHiijy^QW_=FeqL;AU}pEli-E2#&MXS$gWo`wEy=RFI>ftkl-+&=PY*ts zxu!kcsP4rL7J@||U%0T5x`|@q=ewqGcwc3V3I=s^u$c3{YOIZ$0Q4v>>`Y!nT?!>I z@qg+teuW9U;6mb?Qj{`u2?su<-AoPj4M0az$W+KdfRv!5o^f<2u(SmJrr;5%+RkvM zvqg0q4^LYtagW&1LDMZL!AqUG+5MI{3?hOEKCp%y8lFBp>NqGbM72)*21FhS7T12L z1JyIzJ)!`A%grQg!ZZZ}b`Rltnw?hnM^W9;pJ+gs3KdYMSg`a*kyWdzCZ{!J9rprM?dsz z<@q8g)|#!myr9D{YS>9CQ=>gs^Yi3JKr<=H3S~8}v%0ZS?P(yWd& z+Z#4P@(S)khCiDgY4(!oh-s@LiqBUVatV={V{Ac5`l7GOId~H8k#|nq#yRX>5V796 zMXm*?@O+bD4NQ!s)ZH%34+`fI&ec zOzhtT$B|2N(ZQQnTwl64k5cmqmHqSzPn0w7>`W7Ood$ z>^=Gf9kB1hvQ3yT2;DW_Ir- zGVJ6v(e>(}4ROEzv269V43YNRQS1+?xUdp&`HRB1ydH+z4BprS zMuNdGyag!O#4e*!LqZbf#fD!EU{w@5fbZY3iL}<u_a$vh{4Ls*^zRu0S)v|AkbtieYe#(7 z^ZZ$`SrnU;ck$h)OKraTSC@a@ddWGLIA-x@ynX;V=B#OSHe48VlyPmanwy{3DVx@= zn?J|~*@m2u0QGbRgidB`@3C~zfLTuwzab}awd8(4zlOFT$+D;rt0Wq;`bpmY>fTyH z#BIS%up?C>sLkPl!aD#9P+1n)N*Zk8?_zuC>_+#_k2s=|fJ>X2F^5K9rKrpWGaa(K z#!tO3AT?@Q_&fDe6{2M9UckQH{EiWiJuTiAUFF;Hi=1gO%U7qU)xTp}^?BP#@`^g1 zrUF*cZGlPe4lh<*n-5PLk9;HRilElArCkzpb#D=IU+aFla`cLNP$n$w&~6CiLtO15 zGyway*rt7St7=Uk9af2SEGN7VqIT#^ldhBwsV6=>E9!s{tG<(xkUD;kS901JWAR>z zmv;bxNdoIxWXu~Qk_2W%=fQ`o-1J15 z(xqT8PF$Dt9P)Vm0$+n$U{?3WF_YcX7)2?8OU5@MJChez?#IGBEuJu%=gPyn_e-2g z`cppzqHi_^ZE*hWxHgBt1Iaz2?S!8DbNbXNP^2!`fgNB`i`9MB5ZLj{mQ(3@`K2zV z0S@mUf^B3Yy!hFLp3cK>&Uak>j0q?ywT=HJ4PPg|Y#T+<5K66xe)?d677m+DXSf;p zFG7dRqFO1z&bV0|J5Cmg({enPF!Lj-j4shoT-Iqc8zjPaG6r_Cj;w*npn+2D)E4aEviKR&K?`V`6hB_Yr|$CId#CVGvU zb;r%w$e3Gp`QNw#5siy;aI{-t7nHV9dT#EG)EukjXL0?{;=gOl`#wIKG5u-!sdVq~ zXe*nIjXn??`zuwEk6#A=ceeHZQ;yN@7CSu~ThI{(3_H5QOerDE8X+UU7RL>t&`%iz z&mRg$R2$*i1u$7NXEA-A_#R~O5z2+JpoGz)pNx7y?yy1*CoeBFU1i8b`_l!t^D-G# zz5Ac_xL>ltcW#_6=LxG81HwmC4-kjcZCyE`20wUFjl5R4`~U7lu+r@ytON+vv2ph$ zFf)QYH0s13W2`xvh=or4EAOmqfvwYreKy{9v&()4#2H`TSp=NN1=eVZoGLNB^_B|> zy~_|*CMo)c0$A+;`c_~1nBz+Ku%vrE6(L<15H5eGc3}R*pH6Gj347Vdr`tO7dW~AXU#>%LhJDQCFj$;3^lbVzdR}iFNb@R!3R7(gswmw zeToku=h1b6oWlXpt2y-IVU~AQVXnFz7e?#rjZVuxvj)J&a3&4jp`$BBz`YU@^GCT| zjjx0B1}3xUA>f29goqreWlEmbz*tZD4gA$kG26d}0uJZnSaLpM1eNoA=o30#NU<<=dHuD(Sj!$8tcRY`oroX@^KK!l>6DeLTKTI ziUsEW`kKzSxMF%CkrV@=KtGdQ#=*%%A7geOWfX0{@u)2Mc*QwPbdj64I|a`N>fR;? z$tM0`c@A9Kekq5Qbif;^6dKJE^YcG<#n593BAOzP}-0UB5S+~I7;cmK7U?ZGrLpDe}zzpE&fL(1dIMgg( zU;v&hi{`F2Ct$YYi2RT7ml`hwJBq}k`C0m((U~VCh?u~&)BU7vTey-Je5K%sE!GOh z%csqjE4mo0E7`Ri7G*z1xh6@!619B#@FtmN-)Ci|GSU zmoX66yN%YqHJm${*D;xeB`)1pvofoS*FUthXkdbVo~)&;eS6>jK~7RfACK4Ztjyxr z>YayH_vT3UIDpkhZFbCe$aU9)2x;ZguB!(@sewy5IT?zJ|CVX&Th0y_EJHJ7z<|)2 zWZrgUPF{{b7Nc3#_x-s^=h*#H7L73Xa(@~Zrrcm(Z$EsS*;9j;y#np&WU5@M$=8bS~=u^&R(KJd$IPyF;tD#P4ey@5gkjk-h zyHs6{-BCkW*UY{%BxIX}%=4zs2|dP5iruku=HaA^p2zV5#I$srO7@L|hDEY^LTRxB z8?k-($+<#l8hIw_5>3-G>Hx40X^q|^D=nV(cu=;@{q*lxby^+nw_?*JWZ@RICf?$g zk~Wt015FsXbtZ;y+xj|~mx_4U=$nzb1`$C|Ic_xOZt*X1JJ0)4q~s<4%1^T4D~vZC zT4@;D)~8KY5%u#${5<2%_v-wQnD2th-!b4ee+%^EZqMq3b$`z&_lG-%{+S@K|1lHZ z``j$xfrRn;jd+NyPotXkbEM>~n(r5}Mc7es(1rvQYj7;tDq1#HV9W-V6c2STd6R`p z-Q^Ee`HIzQx7KmXkM!th;ATiY$OWw12qs8KB)Hp_usQ_vsti3li@JxXy$>Juw%p;v zXucSSPlP6Ne}EU8IeSwYXs+JnmV0c8D$i^ik9f|Q-I_3DvC6tFze9vgxZ&=0XJci} zn)L@LF~B#A+e(C-O*&=R?#D-dDIU`yk4{R&UN7bMgWT0l7f2xem5~@-HrGcSufe+@ zGN~%5tNR4GP9>Rk!bD(ez*{hs4P?+hE+Rfu!vs{o7p%Be%_-^P)R}aO#1w}OFc)Qt zDvfF4!iRnDEJc;WN$$tN(tVuQXN3&3#)|cU{79F^9A5PqxNO9&W{BXWuGM7TMZV@ z-|}+k1Sc@KxUhiN^zSu8om~Uer3bq+=>C!`XaieQ#XZ75gJKfDIp~3a5ORzlP9v9* zMg;_I=IAuRuWkkVYJW(ew|y%S5$sr%1q!$64P`CwL(#Qs6#d8!ii)Bkj1iTdQ*Dp^ z)GKqBDp6X8X$1aJdl=44<=#j|M{W85VsRjF1Cvv*(DE^jV^M=Q>)AQ`N6cl&+H#+r)8KeniE6ifQs$@eRL(i7$}ZitqhCC6Fx#nLEe4 zkh1Ul`XmV3+pxA$p+UH^O54)31T&KghfB}HZ~3tU$I|KRTt6wuKgK^w`4aoU&BVk+ z(}9l2w=d|$XbbhB&@87VM3SiA-1Qk@_42QC>rs0hg)ILiVOsi!GcYIfoB@Ci9K9@Z zMN6oQWMh~WN@-0e8U@(qA7KkbxQl;!#PTL7K4#&^i3=F<^pfs3F_K=g3S?-%7#|*c zNk>FCvJhHo#;OwKP+u8%yO?fn9#o!cXVSX@idy($ZrvdyzU%Ufc5NS{s`0cCoX{3S zajROMLriq;)^w-w{kAAnF)8d&1S>7rho-%5SpoIf$tFq?to>;)U*;p}Q;BSN=Uzf$Uy7$MMD~1QwVRYf`klNXk3?Q#$_Pxm*0b!@CUiBm)u}(>>`~-W708~h% zm)^xEXdOx14{=}GSRR>6b%fL2`$KiG9}_v`?6%LY0BUlB<=E+oDT@8tJygW|@tnpK zE-QK}b*S{wL4L1>Z_A$=Z*uiQz%M)Eu89CF9S@nc<3$iP-P-nq5dy@{vHYwTZioyL z$IUu9_K6q1vSC%SSdAcT@S{1641c!{?>oQ35Jz4(;RZKr(+K8A=s(R@ z8)6(b+`dm&x2uwBcXjz+kyvOq8*#ki8v5I+aF10}OMZOsI#?UYRzG`pgeFWLzdf!`*);MuQp;&ME&kc~+HbE-~?f#AqWkb2u1oix#`ioYFs5mtzpwhf^Y2 zAk5^yE@l=1Kul~1828~(|5@ra)4_OsPwm}}zs0zlr$$$mjj`2OlNmigbX&lL`lkb* z%gCAoO?iOK{8gEmJohn#TQDos$jwwn_6_rO)Ktg1JOK5(7hx^=6;|@LJmFhiS~=g=ueqQId#lp6tS3!8j-_5zvgDYT$0l=#Sz+PBlb^4sL3Y?AHRAt3=t z0rd7O!P-~rEO&l42}z$!G8$I7HND$awEDz;uA=U(7O>J`n8qZa`_%xuL<%Xl%~`%f zTrY5vdiVo%wcU}bb@9`g>{Q4X1f)$ra`fN_NKB)cOoQ3CEPhuWjrF~?FtZx6Wg~9) z{i&bs?Si^OpWaDmoBRs2PvyA1Yck>R?jFmved2TZ#VR=rHE?zGZXKR+I5QX0& z#gEzC=tt>Xo>;Xc9CX^Y3IPhdj5t559>MC#{m2k9u^Sqp(tJqop`=fh!2x4D@}x^f+HO+9m7!gOF#Wb-p7m>UCGejw<{ z(d?z-0|ja6xYSho#r8}R!VG1`A;i^(W7!1UA0<=aZ;KN&>?gF8FhE1GD@;CUr1Grj z5oIhig_+Y5w@YiR#r_4UdudHS^~O^BVNG^JBYpHWk*nikDQT>pREg1i64p&V(Q%}$ z;|AI}KU|i*NS}BJLyWbJygf}Oq6^!wQ(i&RFaTKLHZ5{cvh0!4ls zEfY-G4M%J^`XT;y)yq9ngFD}PktOn*1E+<&+0Q=IT}%m;4voSOa~Y6Q_AE(e| zLF>Qi%Z@` z^x)_k+JuJkGkED62U69v>li@{kkZ2I#E9EM%G}i@DL55j;%(c@%PPQWmad(2%V07i zeLL1=v3?);6hGI=VxphT^f(UNZY*{tJa^C-@rjBh)?N1g=t6L>4i}lS(qu^!j*%WO zdi-GiJO)_eo@k659<@}zBs4hndM(GHcL#DzfZiHTXg6~79?HSUd~1ywzO{7~L1v2` zL(sCfh)U}f$zJ-;>;;l$z#1%_Zn;dc6eu63?2zrklE7` z)B1V0G2vdTGj3C3viEgB|J0?#JxKx}TN#DvL{V#AV`Q+)tY{kcJ=g-*33nEIk8jlW zuqs1TsuW9GxcHo9ZZ8c*0<0-DaixZ8IX4#hceSyo!ovc$6g;VK<&3VFJWSang<5l^ zl>RV;17fTx&s)xD$m3-Lvj`mL*wBshSex+WyH`h%=d!Nf*k?fdXfEOr?3w4^lLtOe z)8xrAU9|7GPMs5twt(-1B;tB;a$(mm&(cqK9H0~1=NNigS7_dZC(M@7X~vGqCbpy=m_fF z5oUD64gV3x3^MM{ibg$x>FQ-G;}1tU+g~}k*9?re9FV95gml&kw9--ODs>utbL-U#89!I z`#8g*_!oCxrPdU^`q1|&!_GyeVF8P8qKxz~Qf8puT)u?x-0va<3|02xBbmEKM??VU z;;i0*9|zCQ&l*gaKN(L398^pBV4$ky!kt#quHYo7-S*(8MWzPi!_w zu`dwXc%sYj|jQZ`Z^kMgNW zzc2 zve8ge9p)|`vYWs$4@uR*C%E49t0S$&zbUwYm9u7{UxKH1HsQ3PRFw7eZBOLc{c-5+ zygL;dC{8N;p42SY-+Zp`{z*CL>^1l%-5K(?nAnt&E{%!xwmQRuO69-gyC`}{SIeP7 z<8AE;YSvD)JGIS2n@JP>nOxzoKQA%rWv_8ARn4SVqL?SDB+qDoe@M|xgZX_C4Z7$| zByY4guihbU<$w=RA|%fIabx{hP$??g7|P>wH>@GmUUx>LCd~US@$0nGi!!>?m1^a zd+%o->q5oQy9R}?3KEe;G_Oc&G8Xq1*mBtjpKqjG_4Qh$W!3fOPrd8(<6n7yUGesZ z54o$>(HQ_f7BnFW5cOTDOzYzXPjrn zvY0vLoD)4cZT?t?B55hAw79+` z{He;*e9n16!d)O5Pg5zgPFj!@Z84LU8~5%F=DFwo1CU@J-Qo=DndZG|gJ|kgZm#Nj z+WR9ej2p+HIGG=OI!FEH6+gHK@729U!Km_QS6tnDEKf3RXGd|rHUf$XrgO~V-rvIn zssXhK4t|QEP=7{mr7xd~UkydI4&dP;7av~umhxZi8en6yCw{$ztL3RIOP-(f{T(1Y zW@9>F+9&uvuSJVHue~*)zCCo7_hDLh zApCMh&}D~G@a-9ZyA38)>We)+_O~W~G7wZEeKWkvhz3ojK{{f5jbRp#AQ3wMkSGm# z-L;T4E=67mj&5iD+jpGIk5^N73g!^a>&rWbh$ofRuNd1lK{ z$GT5T8e=ziK2fXm{dXY)Toew9$0OuLyxp2^8t?M z^@(l3a)=CD`Q^y$d zE7S;yf6sG8kP>5BDD3?p!DA)x7c~tB^(_mLhc8=>n&`jYR=oJHE|pq<*KDs?KKwRM zKF;C&(|7Cr)kOv0(g}&dEgrVHiTq^pt*+x_u_zg`{!~fi|2R}}s#T(VE;Ly)v^e9I z*GjQEL9Vdi7$UliNpDA#(7;se)-b>^uOtW1*n-MtrHMUBsd=gh%WTceK z{K#f|ggC~6&)U-y$3eI?<}TXbcG8$IXE_V?!!*yr71fn^2i{gbIB+(by=+q5PK_}; zIeAJk3D>~^*}!EZ=Dy1WN3f&inU!I*I=7N48khp5S9;(rJr&^aN<}wdGQF%7kD-4KAug?n!Z_ zNpAxInym+T24Z%4%@9f^sk4)`%M;v!)+`q_=^lz@j_2n&fu2-ns4VYSUpq+#ieb@4 zyxxHstn~cKVbhQg$^V>rm{{=o zp7tGqI+jR16R$N&i46_H1)|o!+rIE1%J)7EHulSfao~!@f$DC0SOwHmKN5Rhw>A$zZci`CBEHxfHI6r)2A3haya>7@j!;!xX}z{ zXZ%!K`WGriiZQ3zOaY?aN>p$^J@ zwZZoEFI!;>mdG4ds9q2J;0HXGx!7G>b?f6y1QE8@lvzgk@6@vLOkQ|)MZxJdw!Ws$ z&`Tft^#?O_R1lWPa~rY0CirC^iHU?luI6N8^eJT=A2|R0r^JNB>&%#RsHv%$@T&ER zxv0GWApxBlBCnw5jeeODDD3qd$q)h-!=hyDx0@oNm%ypUm`;WVUPHzjNkMvH3qs$2 zKLvEJ(j+KH!VCEHX`$sj8i#-k8$l!nN&S#Y{1t9iZ_r_S-7xcxjvYnP#)YXIL0hXZ#}vLXV#xXq7Ywi|u*wADCYba$ zEouf))0g8%%#oHp@}Bx)(vJl@T&z!g+C7TZ=JAi55e)iKr3iXnL^VXKd$fHbPJKoI z^Mo&`&x|(sSs7#e9aqlkUOF#h5KSNP?@q{3B<_?h* zBuvf9Z@HcF=o@Ec*7efA?KJ^HD4KM^)JoJvxKIqXfXpFM(c81s#O_$5Krqf{wJ zW%Mc3d*1lS@vYzZnN3s+vAWy#Zq(9S*V0FUJ0`sMB7kFsy;*uyu>3{Ig|O=(Ns{fn zX?m1pgf7@jv#fm#Sm;q_S6mY_8Q zg3AJfT%a$&l8sJI5?}FMXARnyN%~@+F6c(0)D?BleM6AXi2?d6D+{gGG4GBAl-01= zSru>`=Ao?Fz#X^66pW0_Vi{m{QjC~*6U$YMI?eJIi3oNLK2HtB$IAz!K)GcCl4CiM zI+xjrQ2Xm=5@4R*yf>BJcE%Bcnq(QlkA4wB=c;4IuVeG0)Htk~h<-S)Elog=vhQyycBMG32!eck=D~3CM(Dl6~%ZNdG`c3~^LR7`t#mt)Ee?V7Hhk zjJAU!NO|qqF&6KKldYzQJ%UqhZ1};@_8zFd_w)UE4qDKZu8F=#%#8j$)vQIWrA*_S z2Wc0k_B`2ks4xyK86~|9_>Jy0{(_Y74djgiF6e93NUmV|;Q~gP1h;fpU<|!94?M{e z1NS$RYt?MYD7UpV1mJ;Z(ZjN-tbOA|2b$9au!#m_BKsC$1wDfW1^l*5-m3Wr(-;gS zoAArB2s#=O2llNgJ}zV!d;It)=nK+X;xW@D}E#GyhvVv*v2<12zqsd^Cl zr6y(x1MUjl zamJucZF0K=si;td`2b*c0B+IBE%}iGku0$N^EB{(e#fcd?8tS%FGrD>;(=pzn+_F7 zc)`|8EpsLeK&)tI1@T)ZujCUvo<+RhTnPEMJvp7iKrKoAvt1mh!Hy7QBP=y^z4A*Q z7&nnT9n1E2_8ut>Tu-Mc4XjD0UmE^UnP8;z{9>y}g5OYSK|x~D2I;mAtQESeF8P-W zS7Be?(&h`$M25_4v4JjI$rMTo?gi*@qo~?SyLShnMbi&5*J-G~R-bg9HL!Wwf(uM+ z4bT0l(?;SUFYPgoSsSVmFsO}PgPppl_(?0GJv|VDT{qt0fB<$zdDcj-n2E*2U?1jf zeIoxnQ1{S10sB>Djh8MhIpOB76S?I~GV)r~USZI>h=_<9#m`|hf)b-c1gC2z7&-8x z7zmOA-^d3gC?x#g7b06~_$7U_zGqv1{^RqOcV|6-5Lk4TS<~6$p3pigM?udk2h%E% zez-vTj}wTN(9#ZTk%n-O=FGtZ%2)skt@?-bBt;PsBjl#?=LdAz-FWRahcyH#QV>}c(_{~ z+MpzUBuZP5J9ME!;@u0zOKx|mgh z_GB{`;+|3cBgrhQjb5+R6K`&w*{>1@KMj4vAf8KSCt}Gvj0!3jeuOU@y|BboR1@)kn1S7v$%06vdOv6-toGNLEmM(+99t_o$rfROMEqn zvvFdyTkYyNF(dIM+l$fppB;;dW-J#-i3B_3J|&B2jPR#&RMrEp6+1T2YfP-aYyFIu zXcb%ADva4K3pz#k*DyB)rbMv;8SsGSLXx4`m#@vqo}!np&=p+*F?_P-^{0pjdiEo; zdLh^!E`PSKl4oz_JRx{_bkRWyX_y`LJYmA!BR~TGLKN~pilkGUuheZVyq~opM~VdJ zTa_{|{u48GGj}#zuxtniU$HL{dz5L04@mMZftM=Lj-gkd!KvZS)=L$M2T*v$TvSAS zG1ld3Udxg?m!Wj~4hh2h!=*`U#+agjx?h;8mrHWX(zIu*SGo$~J=5VLxs?WC2xGZ69EZJK-<*dH&rG&b^ zP~DECDW#iAbFZj0iA5)3!8E;x+7&HnSvmf1zNZT?{2{A%7c)K8{tIknW!6%&MZ_D``wcLdtDmJuj4HT=%6qP+-!j? zS5j=fSqoOiwV39)J-YoHoQfN|$~3?-O3AcS`P+&go8*E~iZ9$s;i&{T@2e9}IK#cM z9|VTsP7UQ4<}(uDqshX03tn)63l0P2a|p0%Lje3ASP3KLe*;dLzPO;bM?}u)05B?y zsJ&;h`Z$~SrsEVr7+aObG4vyJ#4KZ=4t-T5``+x=4erp7nAfHfZOQ{%{tV}>BzDys zet|8uJe9Iys2_rB3s>F0Xd_)^&gQ-Rps=m=F;eRNR_|c7gB;wB^8tM?yrjsU;h7IyFRd4nClq&FVLk( z&3yn)&i|ZQ2mC=gq>`Y?WCm3|^*S+%H&7D-`VnkRb1U&A1v@7yJc8qWCE3hzAgb2EJ`R6i3wO;Up5`hA2^w2n+CJ?xu&T zP~c9LOO$?-c>Qn#z`Lj1pr(FnX=4HZSDS%!{V%u?c>2K}@ACV&%GmmD;F2 zgC&(iQ?{CvDn2V3@@rT}YMoND@=$90^nl`&nwREZvg~fW*Y)UZS4gAB0oiVla%9Dr z+6fYb=r`cH4tcq+ARkSp-W>*vVN*w~x~19v(Dti%o?m`34q5jy2TWx*{zX!II9`_9 zXytVp5)cg4iv!ZK>oBeQj%``aPwT!I*|0lX+uRW1nQk(SCSVx4ApR-G!G*>W%PS~( z_R{!t9KGAaOk4=+T#lW>vp8QRT`J*NE3@i%>HrmuZe>5K*d-7kT%*jKC;NMn?ure& z5Qcwxd&`Om%MJGy~i{eVkbTk$7D3AX}HKwxHjgj zG+ji)*;?z6KmlDBbH`+{)I#1L&-$#)pstPbjfiFaPnlB>=ovR*<#_a5KxN^}q89v`6w?--F)aVtrA^Yj_|bK5^?or*PBN zQP2mcD}>XO-H1_d(-ndet6%48CU@`W5O`-<>+>e2)B!x{Jla42&p7R6nZc=O?%<&? zaWo{1FD4NGSb_%MR&YkyGxn4*{O42mE5< zcORrRse7L7hWKn1bzmjrD0f>nOG^7tC{NgBYq39RiajT5Nrno%EJx)W0qE&xe&}M2jr*` zpL(MSu9$GPr7f<=UOZS}Uoc`m7Dk%9RT1n9I8qf`E?6cjjj4mIQe{yA%(*`t5 zyWNA=#mS*e=4 z@EL_-RAP`b?HSIpO#7rPY1-M1zvg)#*B4_v6)nQ;o)x1FVJyNGua1zgUe(gdnVN7R z!cf@XKuQWMmQK1DD@y~CanD~iBxnvrHO&;IA5SQ@AFG7Xk0#VkH?Bj?oDUd1EZ(`l znIT#L(xSm=hhy2zUr?{rKk<#?1IOq#6$)QW)9N`{%Z%U0g%NZMB9*+$cF8an{=XJg z->_tIT%ckZpYr(zwlDOY<2+h(uudWQ2p5RO@QK}?(d-kKA5uece&?$?JMdz+T)rKJ zoQss}3wqf*a#1zuUtu`MKtzqUW{Ac0ihCf-r>h={bxPUQMWOWswpdR89~XjU&ZVq}pIF4U}Ev34$GTiaB zr$W;qKfy?nziMjdB;=<&Fwpz$o9eGpicaYLM%>)R1vLA@=AAf1NzzJy?@eD$Iz!z; z_YoPsm*8Cj>=I|?FOc|L@A#-IE z7)FA90+_G~6x#428p4SRjtGw7Viy++4Q+r4(-H&KTU~$c`NTi(FP~LB`-yQmQQg4z zjUY_$;l>L#$5Has#CnxfXK7ia&+z=BTI|*VX?CHbO9-X&RImul$z1#)TJ^lFRMrEQg zI8X%UZZUNNQ{o$hsuix*LUYofoY9oj>NzVOigPo+p;#hZVLP7yuBR%+=Ce2n2wMo@}Su*07R|uW;vx|i8Tzo!0>w3|Fh|f}^ZA7}}!Bhbi zvGhW5`ELZ3Y78EQwXfN_qWz{0yn%bFk<CXq(^yl}}QVr|te%ezVJodn?VJZ4XtZ zNa$y2VmNDByfI#tpv3(mKtp{ImwUK3_vXbpytt}>Pcl;mdhJUWba3ZSVuw~G`4ix0 zcii7UdafinlRj(*OhPq|5T^zwNaL$E{0PE$r5<>NEc!JA`th&acjB3F+_G zgvr5;6gWwNJRCV75SHTsHVcFPp1{F)6eNkT=QF$~9#0%sU@lfN>}%{3PPGGE;bx2v z1vGP{DM?6nl^XTo7QaM=3V0X*`Y_69lVa?voyZFs&k(trMSCKpuOKCZ29$h6&nnZi z;smeOAVu&*wSI`w7M!3kC(>vbo-w60Oba!i)7gjTN%!y7Y~{qI;P#9mD~t?Et1C*g z%9Ih`@imiO9Q<0+71$}%DZl8;RgiRWeT!JXT_Nd)0m$`#e()$s&c3UUEI_$cUam6h z4cubjnT`BDP2ONa8VX+PiGJvJ+`P*?UW>ZKZr-+AJk*_iG9^Je?=q8qc@Yt;o^T!1 z+JmuCdUt-}wo0eV1%5#Vd_dKKAkb8pLD*YamK*1RFYA=dLjxh=l~nkcR0sj>>In#F+ zgcX+MW@XBsqPk`ulw!15qSdhT`4((*D6mxsag||U!t0wZ$TPy)7esGFlqqn5ni#0Z z5Nw>AU-av_v;SjoR16KNW7YS5#{+^8(AlD2_Q^oNZfJMDcE&09%f|9auoM}a;%CTh zHw3q0-^S(<0vTb|s9<0?z2YF78me4&_uXo4wxKghv5N!uI~@x=8My|Qbw3jQ<+dD3t0{9F)l1s!9t*R7 z<}*2M5+MG)#M{WUI~=m}hRc2)f0_G+f_1S!Z$V)FFbE~jpPL2S^fSDt$nZ}f_LaN7 z+d5FkDqDx{F|>gSYI?kix9+fNwaqZ;S8c=q&?hHP=RiurfT^I%p}PI>PloSz$Gbqi zW~fxuZv&+^)2phRlZX2b?^7@g{`d?sVTM*T>bU9Xk`v9Fw&T^1QPpxRduaAnO(0bgk^s(499K0oZ1p)7 zeB6qbm6V#vD-(Dhs>AD7u$?)VYp~jg5_Z)w8v~enn+uFf4u8&6@DWNl-vbSHaiuVfyljOYveTtigSm zv&EG_nGdhXUJqu{gcUPb=D6gysmZM9L0w=|piwfJD3~E{(Wk_(-e96$`MNt{tToEh zVSIf>7{js=VfBb>sn8jL@RK(ZKLCS<4ck(|!vV74W zhzWJ+YxR-P?hb!c9i~MD79)B)PW~hmdy5uwf!|o&H^RW2@Uv;|Ux|;_b zFN;n*sI2;a-|g{7#}-X1u&V|5oG;jN`UlFAt^t$;9H6&H`BwS+G)VJx`8(>AVQt*zgWi=5L&U~ zPhG2=^}1i9M2(#ByG7og?_{3sn7VZy9x~%{JmCkKS$Z43pYU|l?Nz)Rk$q&*5Ezaazt>Y-YdhYaznzI4_x~Eze%@cFW>MQ9e4+|aBt3S6vw!3d5Ets(N< zw&onfW-RD!u$g&}lm{J(<D78l>6$eE=HJHAi2w@nv5xr>wcm^1}p0VY+!$sHd@6!z-5 zdr@QP{@fF>Ct>u~ztlpRmaG1cpZUp+2`gZz9b-Ym_D_>q=_@^(PXTP!jsQfMT90)ot3Bp1KzR~raipH-9-KQB|gwy2(c zSuw3ANNYupU)8+BI5IZ2aolneQGDQcOaSTd=BXFT2uVwj1;v+`I1k$S-(=+tj2_{nfxhe!7?E-iv;_;4HvqSI z0D;$Hy)(b9e_hxi!Y9&m&8e}B@@ZmO9(*Su30hyEJC9LqJgQ((GuB%wn^D^Pc6xz$ z(3vThdS_%_0md}6n%O8|fRNc0(S<*atrOlLaJ2o#_-MMkBSE0p#TU+tPhF)9x!%NK zjP{K=oBme7l;tSxw2<3?oBqe}>j{@H0@JQ8)0nIJr|-Zqek(StocgB|YK3F$TAXAX zJ-YWS3o0xE7~rS0`S&8d3N`WhlU1VA^a^)++E_Y4X?G5QplS6F9SsHt$%fYk^igES z;c;9A!WL70e#~V#TfLpaPsV&v1Dg6ck^o$1f8z7FTX#Po6)7^)ZP*Pa*&&;Z6C7gtyI^kFMc&xgOOY(9Qutgo-X-OoWg zf;u3R_kgA=p^I?DflajTaml zpSU#Nm|OHV6&s{nwuqoZ&Xlk~)h6lzGQ~fw?X+Z2Yd7Pn)Ft4 zc9hA?e{N_%YCNNE*RNrbya<+trA2njh~SBk`{!*y5OOd8#`OfAj}t+{RIy;(N;o@D zcOvG|iq;j4;S7iJ8}Rn_h%3Y_41sm^llUqzhCwX$WhcqRW3Y8?xk_9@WQJmtqWH#7yu|9f@(;k(R>nIZjPIk|SMtXBl}nK8UW>g7Ty| za-hBy186d!)gc~z^>I14-Ml)CBDA?#Mo>_2EzQ&oixPJU(;?-0)p56N$MveL*pevB zlxRJCzFFpb`!27DxLnxt=A|>{%K~v>Yinz4kjqu49Q4}Z>`FMWGwqfsV*SN^_re<_ z)%UH{tfO=;qMrMzP;*oBLI`Vujkhxk^mpl|xTyQp#8cQI+@cZAM3RcFsurHRm2Gzt zIXgUtg{U%#qfwHc2Gz**kvjCjd%37()^{o2yX_5(vcy1OyE99+pJuALqcuC{hbZsz{<(l|EZ$j1oY9IpfYnYx$piNvP{RyATuE| z3YY5`I(A2^cWtwnbahfQiOYF!j#Z9C`Iib`2~V=Aqk^dij4og^av7MKu~nupgWXkr z`n#Jh%33Qb8P2cC38m9m;)U$o3^Y$ETj-zNhswm)_pU$K&F;_m?r5n=TJVQ#?Dt7{ zb_Sa0lndRfOCx$-Nh$XbkXivni`fnJd8uU^5vUERHmLC(RG>}h7nPKZuoa62;Clcr zP*7152 z{9DF*yrkQJkPK8aDT-QMbdgLy=65mP5YU3v*B7ygp1YNvaciZ-9F0IWQ);jK(*s{n z%>&k^l5e};l%3Y442a)m(a$WC|KazkptJ&SK+wYd0jjrmDEzNXvPpLrg>3IAFqcP; z8VncW+*QtB&-h~Fdh5iTSF*npgG)v=|+wo6H2WU#1a zZ32DAhng`WutM-GaVXX(7Ab?I?$wb5;ioqb zyBmW6K|Vbyo2v1pfpbS#RWca z(!KwQuJwmi#ek3*VgQGN634fda6?TR@Kp$}feCnkK}x)>!KUQZ`Tm|6r-(X|jJX4I zah8CvM|his3HnOvKuJ3G2}w|ZtF6LMi z2gm*%M;Z^io`f; zHkjFAT|1Qd`Uvs3KfnHRYZ3QpwU4Umw=CxW_w@WX8ea%#_{ar-zPVv9qTzcg`vJj5 z%ikr!z$zH`iw#6Uw>d!N89Fw}TrQ%y{H-=Z>M}O|$WM8RI>}EqMC`koP9E{~Br5MM z#t2dyGo(QCIh!?H>|vf**rTjRLBBthJu4BB6peTzXzy^J!Sx-0mmA}mw`W*b_)UXs zObIE2Tz~hUBo7wOkFOBZi|n^^X#zkp_e=ikiun^8`(S{|h{*?Mj#>u?v-&Q5AYi>4 z+the2=GJy3DStoE#QuC`nC7`z1JWn9ediEkstcz{OP`Ium4@Bg$yciqCrjjZ#)EdHTtP1na(<@trMFMsenqF`R9I>nL;pc{A|f z`sk%U$<+%1YdV|wS*}<7Fzidd@Rv_`2UL3H*E8)(wvJTxuX3M}cPy?Ylo9!HV@>Ei zln%vh?X@6g0x>~Ouhvv5{~Tp?TmA!`t9(n%&tc_2K8{8yE)V zj?O3&lsO^{qU36YWpVqL?)|FxO}JuJ{)EP-&rnO|`SHVG3p!st5UfPslAf9{KCf(r z!iedXhscBJ(axyA>KZ&ATj>@yytyBMvm|#ko6( z>#5S0)7WgE=OIn*pat>!lytBBB4=sZQ z$b+h7s_v5Rb=D09K%k0nnt_C&5Z(n>ZVa{_I+(!*DjET_WCCNzB^2qSnSAIIWG;aM zxkR_8Pvy8D_1k!POnYEu2X`favoQ8DduZXuAGIJ|xME%;iOT76`E+{Vp~tZCVY zi8bhu$TKJ3a_GKUu+h+eq*m)dHVzmp<~8BTw^j?fJv7naB1 zL?uTxp?VfY6%|9qQCAM^QP{y7M<*S%o4}6CuSYFjrlbT?d4CrdD~%W$mi9mA`j3}^ zpu{Ja;RbwbX8|YPhxI87!4cBDX8>lg(BP>}w?kyqT4gpT@}iudCFGyR!lSmLAO6Ci z5of)^!L^$7s3P|ng>5XFz6JY3M(CW;$* z;ZeXoT53CZXqx077W!N%&n-ANcKgBgMf$%u^9*?p$WY$B*7SvpoymSwU?aBZ?^W|7 z71FbV5lsccXgg(Cu_bKoRBX=ei6&YCq@bZ9nSoswPIKkRH1IG8t=J;;rW4mnWQ?;r z@l+~o<(cxz(<*^xxLC>0RD{vgPzMDDT$p_lGBPX?vNQNd>jqmRWJFeYTV@qK^c zc$easjL`NSzG#oiG^+alYCbPrG^x@ZCm!boC&2px7hn!a=E~@Ehq?alcR! z+ET(B5wgL$V!{pMBXmznJQgcTk5wuU>Ss(qEId z7Rmv3Sy597DhD71WI{U&lXyx_NsB7KNcc8I5OSU?6Y)$N&4TbVR6gz$+M0NqziIx< zBH(Df7_X(S>QfcMqE~=67@D{F8+wNje=NfU%Uvi5lV_WsR{y+hk zG6UdHOp1`FjP@irCf{|-sCt$?CSO~Nv>R6JSUT-A;k|r}FQTXz&*a`pnE<(?=F-G3 z!f(IWD-eQI`%Cw}u0S&%CIK#1cQyeeze2 z{c^A;(P=8_k3$BBx&f%@&q>A!;t&tfLSu;!x-Z2ZaI;TpO?JU_RWM&-eViUd^{>jT zh;5RC%8Pvrci}9BihLh6(Wyo>0SNQcZ?|nRC4)ZRL6uh^ZR=|KS<^ z5!ss0m-__}4ooA)oF*Qn5*rwX^ThzKxc2c{t{FTMa&M+t4eI)FR5FFk3E1*((sH2) zu&(BnD&;u-pBI4k`mh(6msM}w%)`DKU(lo1PcUSIw#1-b7T*;-6++-a-~>MK*XEeq zhuZ;4xfgCZ58XP?7v_yVoNs9w2&D1=p+UZ@idx#*8%x8&n{nyiNHGhYSx0p0ls(C} z(0P&fEHF82mlFNQn=3bMVfgO*?RZju93EITDoOlMFI?Sd zHKl<)VON&P#~r#SCxS{(AgUBMsz(}lFEfj@R~v)FwEl&0d_I7#xi+$+f=^9r*reZ6 zJoLh5+TgKu9?AQ%P&mEjCho<7%1JH(Xb(z2d&~$<+xA`EsYeGYKmu5Ba;|pU3~Y@X z#eohlkmb<8{Pn?=EJ?Rgo7?^V-fuGP$wLs(A4Ja}I!qPM28l1p4yFaY|98#S|1bf9 zuY4L$@#>8Z`Y^B#aXkAnrVT;+Qh+uy2RHF~$youN#V2b#q2tzz$67ktn=dSorT|@l zyv_367)UB0d&Pa+0|GsUf1nJwIqvNYBVeI|N^!kZ;>qm2l3M*7UB)jz`*d{8PuO9v zKU5Ig;k_x~HOcIyQp8_#EBbKsErbnhb%)(53Q34O`Gr$$J4%%c(CI(}f&6)tZ;f@- z^;$iWB(O2(nd8daS52Il;8P}>C5euJYe8^^7QTS?`9fdf61iy4aX9W};>Zm{>a2ik zJ*rk_IPsYn_nz#6gXKd0Fs=wE3g$RO0OGQ;8$3v}4Jt{264xV@K=0y0xj4a&j9OQ) zh{nG`!lxmi8|$xl*talHf_W(@!UO$S)aAfovm-k!ZZTkVF*sJH< zs*>1>;(Hn+6JZ#Q3_VtY0AiJY-#y%OcZ?&P+Scr8{Fa^?r5c6H22Tt}RBFg>7}k&N z)yr}1rsc?c1sZXSY-Nuhe{>QwPmB?Y*-IB(s{mxF2xkirg3383_4~=YW#WnHkH1A_ zuqgrd;9|##C!>Phb^j2&yrU#LKn8=Pq@=3{VwsoK{m!2#{f~RQu4^wMsoM&q6cGf(Y!MzpfF+5HzU60bL?W{vZSSdANKVri< zbu867uRRLZfJ0IF%L!H2ggmyn`wXFhhjDkh zOJ3PhA1Y-ixZe8M)AZ0GW*^QA51**t1Lgy64JN9>M}LI=nM{LPh)|c8B>(aB^M{1x zhT<=uMghxnj8j8QYU*~#gHvDyrud`$vG!aAVRZF?)fgip2KDA&$`=+3gFm(=uz^V7 zJLZdG={21BYj$y=4GLfp@pYKR|5Pb&VyB6!&vC!G?pR9$UkdNNeJE2hu25Fi`Q01ciy&QJY~8?Khk`PB1~RLc}0cJ z)oaC>7}X|8)ti~7K2?y1wxrW*e@0^t+x!!mZ@hVK?Oxk0KQr<+^u<~JC5(-$tVf}l z_{C47(9ZcscfRKMgea$}M~~KAJ!L$RoV~TRP51iM@z^bWfOS{=pP@S2d2`J)^#nW3 zLcUXMVVE=_RgdyzNc|t+fE;_n*T-zeC$qxP)OoX@eC3&bP>aB$M+p}8b~`sKH|>$9 zyIq8C#2oUhPx-~+|Arq_DBrUTH(?K$7=QXg(dWGB*%Wu*&?`ir3%6fe_;WCKi(sNS zB>q^Qw9ziDU-o6-^5LJ71G+V@?cG@^Yz)a)aV)O<$HPc0Ff$pTDJp-z^Vl$T?DxF_n+WLx_GKjb6C(Z(f;r!bR`4CXMQVe(#{yCP5+M z`r2pQ1-`l)wpt?Tl^xAXC4G99P|#LjvvLR*JS%~bWpJlG=cd13svl3cT6egb^%_}0 zZFTPjtYNMFqY+&l%aRin6AMkZ2%h&`Zep|_`hKGTvP{23Q|>NK6SDrYyCWJD<}zzv z?-@Zea3#Hcc|I;`v8;e<@i{8@OUt|XgpI?(lEopSB@nv(liWEkoOXsB8Qo7M;znPp zWB8KA!b-A1OYrFqJTZ@ry|r!Sv2bC>km<10ZH2Nty%2t1PYjOE^qid+}(F3$_KXb+Pcn8DXZoX73ky{+h-V zgt&}p1WJj)IpdMQbTz5OZ^$mf-;u{w$)oT&oC|s7u!0AYKV4Imz6Kgp0zP4nq{bR= zK(A1~?YuvW^fihm2eugvVS~0zRd3rv_1--eZ}B}*47^~nh}1Pa>rl<m%FxJY&`n)0TrQ*q>)>6H>PcLU5v2lPZ&=*a_2%Ze?dWyo$Ki45XS&Hzj zOiJ%ughz}?K#A6*EFC;7{LVEmxl=kOC=7DgBj)PD!u^O%csW&+ ze85jwL#WuE*uz$HfG}cwvClGIgnFPHS0Tc?dkofgz|5xcjz5TW3gf~&#)oo9>p2~6 zgH<`GjqM+|Db6Y7G?zKL!OzE6Bi`0UP!Vf(_4 zopTH-E{v^YP(T9uQ>|uYa{VD;nIQ}tk5(}8gFq@BmS@jO>=ce?k0M(QTe(}`KXSKy zqFGngc4t4s#rqF;=t_=znD+In938BV(5jo*By=_RJ}=O2DK#Nsw+AK3yFV$9g32F@ ztUkylTF_|=hG2)l!qzqucMZ4LUH0Wh2_!%t*VBZ-U+`S!?QGRr}FLDlo3N;U*=KYv!`%j9TV4P+iS? z62G>y%SgEnWXV=pt{s&Q?{u7^pMlG6dPzU_Q{adR!7>k){h}RO_~u$RBs@dRax)ZX zoGOPc)gd^rf9$9~kxk8c$tc&vOv2r>Os+HRWUDVRP!i|ILemrGn@09;ANoY{C~(Ix zq`oQhvYTDBi7gE_-Mt>d3DMKcjihwp6xHmzpun`Rr$Q=3!7x#fHY;rSMc$kG1rw34 zOTRLX8*X}P-_h!84ZS$*QSC=9ard(?mQKdVd^h)Dx!og8KEZ!yX`b z&iX#ZD$lu;XdLKcxs42-k(B7?=6Zd{M7BK8)yk6#Ju&?vk52?ad&JCG2j0ZSnz_0O z`dMyr6N;X*p;&FueKGI-$KeT!_2$gvg86JX{b~QJIM+e?D-QG4h&7haT7&cWJ8oo4Kk%f78 zJ=N89-$Eg;|BAOg_}+%ZIc%fT=OCFn09D z)5$zSHO?5?h#Z`~zOx0|cfz1ma=)vKsjVmHY`=3VGbsN$Jdx9?vR#sOn)yz51q4p7 zrYbDGY%sG1zG}WONKr$LW<8A-u^SQnuEqyRj=^_90>J#2D&3ScZifRRBl$+$8W6~~bO^p01-XhXbCL@dO+@{E4*UJ; zS}c~%<^amryOHBQJC025q&!%xRnG}_oQ_xgO|)1HdQ~4B{_)WreamJJ2!n7TVoc1_ zhA5k$X-jIK*ZThQ^4`7=d(NKGA43L0EqUoT{zhEoLUiYbW z{bKO^cw~~v#^5zIHftg-NVXo0^xy+dLXcXCnM6*4zIR!}wPWkda=pj0TDmk)9hvhu z;?O*u_06N%+lBXr$k)rw+lu@7&FA-eA7SsE%FV8plrEVdrvjUaa3KLB)Fi4fYI!6V z*TCQQeBNOgqXVjjD&Z{!3~Au)F{T6F?+G|PhQ3v20$IGP76&;;_G_uSO9lb zihnbYV*c%t6|yTnVpBs@TYET)-4^gcNsD4Wa0&Ut-&6CNQu=s|^TX)O6cXvX5DCNp zd*GyAzs9C(ZWfa8V@O(Cn?Ai>^Sf%jlCDLT=r}9}A1hpK4sXLYX|5rN{TVy7UyZj( zc{#R*^?5n@xq(+MP9@}J)XD1m^N}{j`0y+7rRRB?9hVGGAzTU!C-SaL+G+2mIOfTK zDlTF?lgxvUr3OI&u79NC9UdaBu&z47UN~yn2BBzK+MuZ!f{rf7!!@Qr(15O1JNASa zU37qeO7XU&EvL$4?I~kVPUQ&HPnje!kaai-M&->-gKTu^lb(SORZ)IIHrr~Ogrk|2 zK6`V+!=Pf2!eWthJh&-Jq$E$?=+?zQiqM}-#Wpo zH9NNZNfh#qO&TBu#c5C2=Vl?(2Rer>fst|DS7F-cKJG~(WrF`-5+djLrm^|Wu zM)vyPqqg9%kb~J)x;1NSze}9^5cBK(_KwSgxs%x;ftBNxu3{Z-YI?7uuyxz*DPGVz z0?;Z{AQ(>oIRV8R#is4TfuXj>r=SpsN?n0wSsB~YW~q}B4d{D7nS%cod^fw3)kHSx z?W4-1dnpPm*&L>)dtV8PuY50Oh)J!jzr-zWZdiGEkS%PRom~Xv&G14ydFBK-`M=%5 zF(*V%-uvKdiNua|52k*p;Pm^>hDrGV|2QSRJXel@kC#D}EVj~tBscrQ_4($~x9%l! zl&#dAc=ga(+VwDw@HHD349mgbS@}96>B%>wxm9_4*=qg^%LsZEgLg01j4Xs^5Xq2H zlJ+yyoBf?O42awHaX2JoT+4X%R*MjGyNUnpKPg$GNmM@9m?%-<8+>6>{75BinM4&_ zE3(odZ~r6rMtEas>Gtk!SU%V1&!0n?|fQ3HwkZV7b<6lSdioPe}GNQXTcrE=U_3u9U0f&VV50577abY19~zWjyM9U zk+sOJY~=!BDr%W~l%$uJm({(gZ=L~{)m8RW%`(I5B6wXgh7!u#v~T|xp;U8I_h)bC zX=Z7NY=b9NSRV6W0a@Eks=4{O?EIm<*pP<(ok_;APvVME{%Y8y{h9C6yoVI_d6C~c zYbpiHNI~1l#m_O9g9!RU7$=NU^|(xZ?c4lAptoBi%GQABJ>oxuQ0R>2Vu<=rbuzg6@r|RD z`Hh{|@TNSbQIyGZx$k#gE=lZJFajl1%(e0tt&`xP8r3t``5*6xFp$Nx*!4~`VJhsa zf*dUY7o~#YUzUx4s&?t9FTPV=XYXo5iFIZ6R*)>l!pZ4RHMFT>&Q=a`*m@-UQe7;- z*~`a!WpkN`@ceRhxifssJ9s7vW|o_h8Dh%scDyVPL@gLVgXyhr-O1Sqewo)_rliVY^n9!>R+Dp0KD&>z6Ej(FHQZK5mE}{d& z;=lIx=>9+><+}!f<#&1{Rb0>O8@xP*yh6M-!XZ0(p#$rxS!a}seq9(kUtN`l!FbtF zEGF8w=OPl~yie!MqgVBpN+ zFUvZXoR`K=M;)?$Me#hIF(){tqb0`dwsKcyUiN9WbbkTkr6f1J)8xWbn~3<9otYW4 zPfl7iRg*cbHuy^f>5}950rRgQAA#07WJir`H=fm-ft{$qx%<`}N$x+8XAqD) zh#W#^W(+~u#6(8}s36c^vM0TG%H__DnX}sU=hfNNVRNT8?;TS~;ip^IV(f+ryRtCj z8EV%%AXn@;E=W z_VX<2b=ABWoaxE`ow&p|BLnvV`4Eo7WL2T_iP=`;BKO92feuC+tHU$nsL0c&wp^B5 zU7GARcB~5K5BNs9T}d?&_`pzldi1Jg?{tbbHtWXx1=wpev{{L{o-uuSP%oO~VzAf9 z8}jlHDDhZx+Q zv|kcLcj029vwEhhuxoNrU^b3!cqr^LfpQ}E#e|=YEQXmfPLhy5w9yA%!(#E>!5qV1 zGgrIBA}ENy@Xadd%2_3<+%4ED5GxPQ5ebZtV<%GFX>XLm;ZgW?tGhkj6D40UpHG4W z+BthIfodAwgZ{{h`Ql&kTeNl;P<;F6x0Rn`*RFoz$#7i^OVQP;ROzRO=nR09YIa!!pA>+Pvc zsa8W0;H`2Ji>s*M*NU>?i7EwOyiFr(D0BfN_vysA2y(5E)SLxg7~A~kEkPd+fl-( zLswLQ;oVl`q|=5nW)7Xf>5__VO<2j=tIw#J)uEh^IjSQ^>+P7-LKuxW9&g(xS?`CQ z{jV2|Hz1c8w+e#{|G5G0TGD+404IU>?~EP}xyb8#z((&sFjeD#48yBMJi1|? z!aCW;<4MyPB`>ehGfO}jZs|; z-(`=!_?o~3@vpy^3D5zBe`fNZ1|4w)n9!yTUM1-b1sU_^QN_+lG?q)<-VM})YHN1iBUlYot`^}$aTtOa^NP4+krkZ%& zJb5L6^L^$yG~_tkGD3FAV5=v_Q8BpO*w{);tD52@*uU|N26?h!TM! zlEF$3KoWV+o0Z{ZUZJu1{g=xTUZ$XCN9gRG77hBv!>c6L*bIrr2eyEb8`KFTKk_=hW<2~wl(N{2j!&> zmTxp+s`AH+%=O3xv3IT8W>*6D91`O=w2oIhnW=cqgf}f;Z8aaGah}KKOQ0Kibi|Dq z8ganQmpmYpBklK$P0h`8mjqQYLZjE@K>rL3K;X8sl0LmFy+LyQoj*$Od>zd#JoA8% zm{?JZwwY6K%bwH@^+{Eu+EaMY)>xkn;{r>{}Z=c`?rG{XgNz8tJuzg)!gDi>8FcWpfZ z?a@M1GcjXCKExdHI|)*?x}IayNW&IYB;{GMzDNYHj@RnHcn3Q)dL3-;B=PB-!v`pD z%me?`q@d64stiiBVGwd%>Ms7)ed?5t2RX#5Z+7g0mgjrx&7Bq3kzP;0W{Q@mmwcL|Nyy=6tYe1x;iKo_}YikgOqF<6uPV+AILx!fM3AWFMvZ*AWm zfDI@^8*VqJA^q>|Y7NKJ)xp@f#%Ty84M^L6g_KS78|2m|0ptI3>8f$}RHoDQZ%mvw zb-ZEH0yr0}oHx~Ac#*|~XmTej34+qIyxJ27&L4yj(bw$Rq?0SeMUYaC+fwiq8st60 z)2Jj)jh{8VpjYX;84V|t?&wg(@l!_M&8wjD()on{&Jnv#jTSLZ+~EId(h`k$t-j(+ z9~Fo0HE9kvlE1Wz_rCwbk&wian^-(#3pw?2`LlIt%BZ<)=+j0hy!}nDjOhLSq=RnD z)_0WWkt2xU=Tccde7aS!oQRC46 z`f1hg&zW^|u^zAV(w3VO0|VNrzs2#wlk5}kFQWeeTXPJ1n>zwyI z@7)@KX*D!RF+XFBP5CPK|7!s%Oq+qC4hM)q_;ICVlwPGo3-bd}>y{FequPW8wGZ*F zlUO)NlSFi4%+!SVG}1KQq&r1$ou#1|sM?xL-;QMhI>v-|tw4hjTz!4vHu3DL#-IF( zc$xT9O$@VuIOyBBlK;Ct^lgs=RFfoqgFiNtJv+B_*FgIkN3<+pC3SRr087~;lz*`1 z;i6t&tVYY}3XYC9ryqKuHs#IG`z{o4EWr7-yTDzl_b`uW2wu$F@=9*5C{=%H{wKWz z*uCvFGqHLcO?yKEQ$dEY^f{9=2m{qSafV<`X3=dhV_ zu!l@a1wy6>o-$%S_Y1 zrLvV)E#)McbKh9?(Mo2G*1M zVc_+L{|fT3(2M^B>pIo8-?53aQ_#`DIfvW7zM{s7{6tRf3zPWqZtgHXDRL6Dzp zmABiFspn`7K!}xoB>D>&wr8HWKIgMaFVtk!?EL_|eE%v(@ZBX#qKe}2kbNb1^!PxN&sE!m5?{()?M7Gi3zZ0t&~ECJpECXsV_22ieU z{TcpdQ4$Wx`E~tfe0`xRb-eQ^*!#lm>E9bD8dv+S`>-{54+`aoN@pI2(8DEnHq-pm zpLb`ptx*y#8~U3$u-TpN_$r@i?fu97I9HCAxaq`SM_0Z{<^b2q{ruv>pBoJjt5(&Y z6|r7D*&>M2JhrSDj->w5DCzDwcBwmEV>{Fm1sh7d#!y#mKRk5t^7aOu!%Gc~IX)$V zXY=$eq2NZWbcAC32sof!47@UxKHav~gv|ed5rIm3-8hPnUW8wn))l?_N8bA!&c^^f z!^$XXe{|W-fxY9>XxWzBYxqlUUWU+Wb;Dx!<-t=o6tY(BAob;8Xs?{7Y);o?nLa%q z(*V@!QT1;_)Wb(eHEF4l{T%r_&AXo>^;It8fF`RlTdquY7^!#t{jC3FhMP=Tpy8^6 z<0mz^3LjSPP8~s!X-G%HxR68VlDOqoT}yF&LOVj+Y86-?QIh0 zEV7sgdv7X2?D2(#c!2p;3*E`bs)&s4CxtR`t|B}Q+ccK)g4Yb))20J$FKmxTx4+N( zn&|3+0TR5QZrZi3Sp>Ml~ z1+8T)p+kaBSCp%&o9R+y%utDcHQ$UJ?oS`(Iq79AkV;YV$%~lp32~ zScGCldj)R>il`dBo4<6BEPm*K3#ER(OYpe(;@;&UZx>nLf8(1*2!AZdxAcWK7LWFj zGPO5q3MY>*eac5<;0rAFzp~;~SuP-(!68Px^9Z!puU`WR71+Hu4(1qpQHN$*oN(Oe zuKa8$WgNKw7FJdYAJEk=cUG9(nvTwzg45O6BUEo|XLt(>3u#rV=+dsVF~U_BdjaH1O!BPt67e_3xXOzF91ZZil4@eihien{D=a!b?jmN(7_LamTLMEG7zJ~=JfmPDjncE) z&iy`K9inP4hkvnV46l~8qa3$egPxfXyz`)8^ZpIr9%F%Y%(=G5$59Ey&cZ60AD6tt z1-~*-AWnH}_1aQn|K9Nd4R6nhjJOB!w_nINjRxgLsP7-IhwQ)K+}Y;?Xw|eF0n-Ifv{xWQ6uTO!*~2IX6?I?ukqTX z<5~}*GR`(MMdJtp(7x$rwaex=tEWEA z<;MKN}q#iAl2CK96J`|H)~-IBmQUgbp-65pBJGyYEb`V z3AIfu6P|tIH?QUK3{4Rq^Y>Aq3BUE(CSRQPrv>hB&r@>T%4(1idKvAM66r+v0MhKX zIsr`x79_;HWaeV$NddO1X&VnR=(B(Uq@zvzEYLt^uadY4y8R?P`P-4kB?swITvyOo zM6%&RwMV$E5rOI&RX<9c=f>Pcto_X|wj1*VVYqdov88B)oov zR!~ez+H&KD zd@xG7wwy$9B`)7}zDUX#lByi?c||_vD0wXhoLIUPn9F_9DKE%exzCqA15H6p&MYUTLfZLa!NN8~QLT_+0~> zr??FX^v?nA_#oHF=8WbsvK)V;fo0wVikCo{ zRMS&&&{g;p(t_HY7{>^(!vzTy{X)m?tg5*_agM8C=7mH7+LD61?wuxAM3C%~=6&U! z)aBtqoV_S_lZz%#hgJ34dL268%y2*JWp>abT3C=do_ulo>lb0{jr1Z>D@OZLbVtTp*{nCM_Kb@#rSIMiQh=kB@Iv>o>oXYi0>k+geY$O)xg}9$Jqr?(bI%+nJ zNVIpb{LuhR1sbFOk^y#HPk#I?3oWFF7O}%7MZxts$ej#!op`U# z7%JD1M~`=Rfa44lUnJK5y0e>4A3 zB*HTq#<~^~JbZSCWc{qP^&t59(#ah%AFJ4sT7l7wm-3mOzl8-P?Iq<(uFN7iIk|*q zFGt#rmr=160tN+`%<9Pxi71K>nd#|amei~B;C%!pla{wzsGA(kvdQB}fgrTG2G_3b zZLW7#Xtl4Bw)ghB2FP96z|GH6Ujr=B4=)rwC=ONDeGmSRL!^t>2%Qu;&j8&ZKrmxo z6`8iD{#Q_CyQlY3U441}=8T$#DOzC6&qZBbT_H)D2EwdYxwQnJLUUSZdf1LQpV&Uy z>oJENz{RL<$DbsWCF^1Q8FZC280Lkp=-JmZs&@!||)V>LB9M>_Cu9j_>K=~|m& zL3ALp7T3g4sdb-|i;s$&UNMX73l!78%5{0A)$Hi}xm33%#f!J4=DxQoboc9mwrlx| zHmZ5{d!DS5E8y)$9%XbDSY=0NM}H|r<4zc{XBY#S+JT z%lUSlmn{_$XuFGchn8mK)tDIgz&|}T2-fxu4zv^v?MUbSh2|-io{NuXL&T^WdK$NE z@$m5Qe2lOb4HM*YeNK?sjB}RV&CTtGp5vd$P`l{IBC5=I-}AY+Qis3^fbIuOc%TeX z)6mGz+0#X>DvpV$KlvMk4OcoZGRsuhdHP68nC$nyC-*GP!|v=sCBMf}83Sz4qPElI z%G}CIVxSUj669S|N)6*eh##EYI`hN}Z?h;gjeAVH$ba42O%d9vejDjxixd;a= zF681dL09DiHpYawC3XmD9eU;nDx@|dVmYNXC0wH=GN)qiANQfs{g4+SU@G4KQkeb$ zTh8dykun2b{GpoTP8_D?o2?vQYP{c06Qy40K_FgcEH8R+q+GtzI~MR zB07)B_D;2cmEzY;D`4CKg%Ig!VbPzNHHc)5@AhZwNL<@()^`?Eq%Z!l>m}O{$2anXgPWsm zVgy5VngI>!=elx4^HQEav45zbs1y;dIND5qrtGr7-YL?rIV!Tq3DIG#IgJ^h4-=SQ zd+{?vy$HCSO+{*i{XN<0JX(b;?D`2J!yLu%KQDnSdF&k5x8`^7eG05t0}w8KcTBx1 z?4SVsL&FXjS|vO;Uv?RcT@JqA4{BcziJ80glM7KH(?_z%gKoPxOzE?{V|oJ73%~8E^(Y4UY2k2VtuubN-_mSClsbx~X1YZ*Ok={^lzM^&s3WYu z8>wN^OOG}b{5cbd;Ag&?q<;+;B>-*ngXo?{)iq^9f0!AGU%lA_61QVA()OjJ4{JcW zV2Rluu35+M>89u`ruRKMwxXiqKX1xPBo+8NQw+n1tHnU#n;=GWrB-xhw-~f${yUFOZuLQE7U;ln7G0Dt;h+g{68^l3ZR-D z!~exnD3)SmgB0XLAg4GdhJJN%ir30PT-+Eptj8r$|2~0)_XoKccpR;a<3`+0ZGEwH zX)Me>{aU}5M=*+T)Txb3b5jcao4lN@(D6sa(u1!kpXcmkVDSja2ok8tJ}5C_<~j)> zH?+-kbt5I|M}Ifj)`#|5_dCYgo&s|krB|U(xPBuxXQFW1J@RNRJMd7Rml9)adv&yk zbRIWEdi47GhobyN z1yZ`Y3|cpg&->K_?Lzj<9?@9^zj6k@9Geq>={ipiw_4|ACJ%))>uUc1R4{dCOP(F} z>uooqv>%DxNG54_%82SLT>WLqNM=HEK9?XN1_0l{l(ph!a86nz#0sbEIsz!gZw`$n--`EK|^aw^(+Rfmz!+TLCz(^~YoQZD#W|Ljiu zIG!?*QMl8v)-Z&#B3pu#ro2H%Bh-eNRh`1^Xd4+bD42Kyv|n0#IRiGwSw}(2n0)rK zQt!|8b#+0LG5^CIz%TW^hI0SyjDYbyT}r@ojq*R}J3S0Gn%Gy0#CLOMDF4?|0fuNQq$ z4?-5uEWgVLP;zga{xkxD{IaZrh9F7`&lB$4lnFS&9#(m7$3b}E69o`y5J>Ul-d8Fb zhylq(O8bA4FRt;7MogPZK{1pr{`O-$2d1XH^OOEfrAg(Fk%mvRL0WgN2EfwYAiX!r z^i}6sB$}<74-mlKm)?Y29=_1m@WN{cj9TC2xuu%2<9p`teP; zuGH|^jyB#+!|D4S#Q_#2&ivKu@uN@VF#fsCt>hr}-&-DIAtU1W$3Wc)q`vgaL|tx~ zfe63fH&*E!`Qu!(b8|w{((hto@PO|gPXJOuyBumFOm=<~bREBRO$4CG7zo<;;%_?K zerLntpw^)Qg8({%uT%<{FAhD;^=sBW8vP%kYQj-g%&G`L!gCNJfxz1x^%G0Eeu+^I zlA?CowIaBBB2mM@pqV>u3%PX3H*sJeBVQx;TOU_O>qtgq6vl`gW%mC1B?}o0l;jkEECPYVZ9U#2@5lh*x@?kY`-=gI z-CaoM2klBv(;$bQ>7t0=_k<){6g`jgIcE|~VMhTE^$q@v@Y?SH`c7(G^Q+i9#-IW1 zuhz1}l@81onN^d}GDGzSuL)tx&iAmP(?d!dhHbeqYX0 z3gH$kTjmkuM!O){f6oB@USo zPL!UfZ!F$C$k6j^2-a9#Z~w$+c|9wLUW+DQ@pxOSWZIT;(qU5mr+-M?Win3EHvJcC z`M!^^dkU3Y2wn-g#G9|)hAEGvxFJ4n%sHdhfFuQ=13(7c>FpV%IKa?gT<-)#&E1N` zG7%tE10hnL0-yx^wz7ONci@PVKDyq7BM~eZKM$9G*YJ|2mtf?x;%=! zFM-d&v@Cv52}>Mx+?!q@a^@;>91R|^hNa!4{!2;?JS}192M06|HzIsZJPKm`I&}15 z>jC|`@A!~kL3X_v5r1u0SRodc>a?jif~|;)K=51`&zp07zoL-A;y@qxW+ct$g5*a( zV+%XNY#XkV2I-dFRo*bl%YA!J!(Spq&o}0f627j_cZc@($%ovZvK42~9=a&n-|kAX z3H*DZrMz5CrJpmDbgt-(K@atq+}z zTrK?L%&*Ujpa(|EM=2Lu$Aj+=W@>S$1kAi_rqPP>7&)6Wx5?KbkR0_7B1rZ<&hY0R zgNzQ(b;(X^;3WXIOi7)`*Yg@w@&zxl9A07Atgd6Y-zf|Ob zf)ostKxo)6Li)-f!9JY>w8@r+l!H*7FQ-X>YVQ0j8wKXnKSxCtKhFuVbxu96qK^*+ z>%K*mFsaA9u|BYVyYMc?olO8keep%L+`Z1EOJCa1hJ~RI6^5?vv&(E9681ZeA|zOp z1j5Kh_;HvDAl++7@jo6@W_rosVWI;z5hxJ;sWebm?Fm391}v&t3i47zvUEyqN3|*2V3~)`05{^5GAiw3o@lJ44Mo> zLWCTqF+WW;HGT`5{pyY-=XW;v#|A`)WQyat-AFpuRjLelSON1RTJ*es0YJ1qr8m<_ zEi3Gr;%bH?+G>ssNJ{Rj67S~lz$(&270g5xC7XhnWAXvcuuf2Jh}ssfPD0Z^mww3T`2Zli|cR+Y>*yL0pF9N(P4 z{pR|VwA;@q3^|qgl9%BQg#+!KHT&5Ybq^Td<{h0~I(;JAYIo=Fdf(&p(SNlYFR}B} z-qp?%Qdss*5pvTeCybuLAXjL%+8QSyAmI3PNPvLszqxL;eU(Ts&?`ly;g*bUDT>lQ zYX=AXz62&;Ow)C^*LxV47cn4Cvvqi}`4)SB%LUcQ z!5n#_6d(<`r|-SUZTWZXWxO~qXwhEM%3sIeehnq`1oSByz9}UcM0WcNQ6Q; z!sS^C^hSq_;ogH^lYG0rS$Cd)y?t`CGLrbIk$mWeWlzO}GE&GG&K}h)hvaufv?_(q z8k?ilI$xmCF-x>v`e&V$*P*wwp5zbzzZSsL=G9ji!(U{%z7lsmuSlZm{3@S2&Wk($ zVOdfcR%<1XA1rjXMz*iSSUqe$^x}zfmW9+}e-1B0_v?_;)g8x}Cm6Q5vV8`zw716v z^ze>X)5^-?fu%XeFBhN$**-V`{$hLfD*}I(V|dWQY0LcAc=l`HNP@MS7fY^71-22g z8Yl(4AX6VIxS}9jUcfL5zR(>nj90z$XJ|L(gSMt%C?i_(M)`(*ZVqR-pKohb!`dE5 z<@hzf@VMgafh3LqpbTUVplkc{K*{Ju>EYSUKarf5RJ@Jw&N0by_b1fHN-W!f8_Az| zobgYvh_>hRQ#?d3Tapxn^p$od$-vYGAY34S5E2!QSiYx>c#|e*$$H|3UcldcxU^g6 z_No!j+2{RwQX@+;d`z~iG*%VtumRq+)dm5aLfrZ2)$^SRO^Rn;;1%Jr>jLxJM?kag zJXg;`8D}|H?+lo~pCX-tgqHKlgE0Ua{8Ch;%=Kt}78ank8()+ifDcd?Rtx#|$hJ0F zlP0ehB==zaopYzZ7Fu?u?d$8S_TJOW^uIE7RcyDF)o}a0G%+z@v2ABa>2oywUVO<9 zC=*hvfH`xXo?gk^wbEMBZlon#M=H{$#(8`WWW(Lp@bHQnIDW45eNxWyg0H#&j8h-2 zL3l`jZHysA(m7ws(EH}^twPzbIRTR`kR1_N_f!({(12F#2`5GpX`9Hq-MZk=%Lcw+ zDyok+p1eKVqh;AkL=JBL&j2mCV19=2q@&L0E9?<-65@hm&llVdGVcxw2=i~H%E#72 zdo`^p(EFmjCT4NDFLcn<`$6`c`;89+{Lxf-u9Qe565ymxw|5>d&_j=grl$?US;NS= zSq)51)rV3Bs7CjjCvQAIs+-dMT&c3}Q|U{>@p&6TCd7^>VLKbznC^lPAww%{7ic~I zXkWmA6S3X|lbn`64Q9m6bziksyz}0l7P72R{_TC6c_~6o7<(HGVf~`<gpQH`;z`G-~OS{PDVt#pg0Q5CDqP=RRf>_6m^ndO2l+?^y@&pFTs|Xq(cLC zN=}OK+f1_q^k<|tEB%AG5NyGz-h;)!qaN4$OV?&>J$qwdUH~x;{36HG)(I@sgjNm? zgVlUJR^w0RM(5DUR&bavv%)@|y+BMpGSR>_S);1Iu?g4E;bBH8ckr|_L{v*Oq=FJt@=8SF?aG~hY`KX5Hr#QNlM zt2thOIRbZ&W#}mCILq?WNU~luqSCFe`ybz9WUs`-WpT7s8i}dT3K2ZJALjK(PP%XP z_&-Jqd2NHDj}y8zCfvcCpZi&3Sh+wuCZym`aj$}8+8bAC>3<%TWi@^_F=!J zZ%IjIZ&cbxli01=kx6y&{HPoeIwjNmU2SbWo18UGb700yar02K6FVf%xl1t<>&C9jd5@odl)?iFkhS?n&*>lTU$zb8IN~X~ zcR*@v*Ee)txlAAJl0O^szg%Owrv`^X3`Gdm%%d$eD+cMB>|hGszgv^@ShIOYnvBGx$#GGADE z<6Q@uNo&Bv`@I>a<$p59b_7p*82oziiZ;c6C!9E9##{tgkvdAqUDvWEis&Z>)QkmE zf#f#v&1kLxZ>+P~L9}s$qyHgrGgf{};BBWv&%kJS_V<6Khmtay*ikd2;;h=hr1-VI zDM8(ze;p66bP^6?ym7%u9g=I92F)}40-fouoJkMR)x}!bVu#EAJdz{Qaf)MR`!1H8Eo4XtlrodN@cLo8`(b-O2)tl@|UjtukXJ_ z^=$uwU%t#FB)deVWrZQ{f?oQiWDkl0ZJ=Nmo5ypskgFv)lk$6ddEqpt;vL^tA#OiM z!&S{nand3_N`<^-@l7h)0Q^vOB@ppf8@%k_YFw2wgIc88I!#yQbsm^8a^BJrVn5TA z8~H=txW3iC|D|J;?Y1JQV!Q>?EyR*Tg+E9gg$6oyso0+UYu=pEFF;(ysgdo4sE9ED zFH<0{`=H=;Ojlgt6VKT_ky$+{JrBX-@`lB*($eWBZypd8VV%rCdIltyBr!_Izf}b! zzh0eAkNxoR@c}LFKO4>49xI|56xgjmW8C0^C761!HM9~-bgBWsk0NFLTWJvSsUNw0@vB zPI3OH^a%(O@Zjfx%x`Wey^#a~Rq8-T=3x!>U&xFYoKu>(xdVVL*kdHwFB6}biA;GR zaBs3`mqv1S^SDdR5CxzW;VO?StOxueTiu_-c}Hl961G#XL#y^JNb5hb~J{0 z39i0>W0zhST+cVorqyMzFB@KN~@p)}3MSRf@0~~n|Xh%Z<(L1Q_ zWvZ;9K@7W{$cgog>-~8DzQ1gh$NUCg%w^s_RbqWp_$SzDN%j>!nl1C=?y#BeKi+Nn zdR|rP#~+J2;sMip*9+nVeP+xR=$QI#_M?SoVdAUAkm#d!rr^{{NL7{N6@TW_KuQts zNDNC5C^JK;E3Yqo0cDIhh9lQL*3XrF{|h8)5JNntwGqvQJxg#zC> z;(>!Lj&-}p}ivC*eOWGr1;r>{vhJ9$3p)a`>BoS^~@-Y60GTizXSU<&`As5SUEZU(i&mQ(KvO4p-!f1aBzuzu+Y(3ke723 ze#guJDg=d4iFtP;)x0;uwQYxa_w?v7;8%Bwa(cswW#0mi3BVEiE?Z7|=|It9gIJtx z4qF*YyD9X6=VJi!1h#vgpP&C@SGV_kN6jj~cDL8ozA7&Jk(WaSsuy_0wjhp4PWK`E zst^~X@X_gL7K&={P=LzC5GL;3VHJkfqV&4F>Jum>dObT!I2Kquf?veH@HA9`PQ!BC zU?3NgLwr24DIRW2`9O!y$J)E`Fafn#{2^$dbuqGTVZ&egvorrIbB4-%DYHoEnlBOL z{G|)}?7>tfP_2ewj4;-DeZaX2N$VfCN`ZbIKhBm!$n8$iEqKjwmXam*$Q6k`+3s8mZDg|?wjh*Eq zIY8ShC?m5V$l8d4x1A8} z&wf`W3C^C`vdPA6HjalbxyWWEbTxWz(RXq}tmqnVYyjqB2%i-nJbwl0uHn^6`bP)H zD1m@bqJqGF?X>dV6s-*p2&b|9c2w_g)XLBBkWOw z&|zSHAdhK;bZ&3sf&$9bZWOC+VQNmJROvgkhL*X&#@q9=Fk9{TVWzmVjD`89xY>D( zuEOirCsG?482-#Ni4nAVq(n+96T!vaqcSFvDl#n_t-Xtr8vP!aNRzyZ2Wh5kI@4b@9%*wo^D0`HZ zagB^*rIL`n)3x1e-T(7`zW?7jj#Hg(UHAPO&*x)3>vb;-+??5VYgJMb9~#~|*sh~j z<@vU3vfTD6cxi^z?M3)l$kvd5cXnc-!jv;tWvI)}m&LQqKP?77?96|?aEjnCb5x2? zzzg1?TAv!;HxxTJyO1ZWfjPu3f7sXsEDGHP3xU06vw5BPV=Et_&fwOZr-63N7m z&T+CW>MA0>uGM)xG^%&)=39;?K&Vw`lbb}oklIfv;JE!jLo~HS_~v17fq=v*w;^Mj ziBzb2K3Ra-OOccZyi%6@T~6Z?lU#lDdb%U(fl|fy8~*$?Aw5*7Rx;#xq75nL#FlVf!R| z&|i`aB1?)sC1BJ0RrIm^@kiZvQ$IzyU1cw;^%ov|2(hfGh71i*7J)M9{6R4yiVAD)c@&4d zT@#?D)I;!Tcd_N*kD9NqG@NzQgFJ%%y#*;A3WqC&zncB8 z>W->It=G@a&fWo9R+25jZ{mSVbgSTw0UZGx2~8FAX@y!AiJx^~8bG8f+{SQVs~M(3 zZ7%~q*SQ)5iy+CdqS5cTKyLZ}-EP8*yLj(co;7_hd`FtKUms-8lK^ zgjYsDW`x_n$?yDyi<(TN#LlA`{ z;NaZDs`IX1r7$0^OUY5>O5|gYm$H5DaE$R6=4Q;}T+a0Nr&<-aVi`L9M3QS;^EEL( zXUex9(;rU2^(uc4N}OV32)pv(Na6#wXXKdV#`CgsI~%;R`+MtSkih9aawS56#!rx} z62;K-(P_*8^a9h<;>|FNn>{aRTbo1v5#w(#fB+T{Q?ngeFSe>HS%tvK`*)9y!H;m<{A%{S43&P22ihwZ^`4olyx#y-q+ z_+cy6yNSTcCzZ#I8BGx>j(^^^i)Ih@d=|A4B{P0}h&)|IJ_PxD#nU`SRaA@jdDuU`P@=FTq z*5bN}y~cgt%t*Xc!|&5q<-@P`n>%d>fel9m(H!d;eIJ7?w{{|Mi@;05kqxyFNc|NR z6@g9i!^}okgR|zDXQqzH{@?M)9_~;M;M}@uhm?fPmmQu=4Qfe)ibDZ1&fukxojkIZ z6p{_EhdqS{Xn^UsEGxVEZbLcbSOyR(QL0@Z^dHfXPj4<2e2dk2OW0p?Sxzn%SeLj& z{jb>@Ry31TC%rcd%+!=ogZrKwa$e*Jx2{m)+Hte`%Cei2u_B=pqs|E;xO-`gbU$xq zxBXZ6m~dP2bKlKwA#m}n{ef-{d1ASfOzKQG5UTMVRU6K^P=c2<6zB{H7p z8tlQ6xwNc4A$pf*%yP8Gw&O-ma)P+vl;m`fMCdJbY+$8NuuD<&Zc}+VwT+F90n;a@ z@Hg13qVpUyosxRin_70{5O0w4DNRSC_xL0PukQg%P$46No3&L`0l~w~*qT_A_4&nd zzv?AvJ96iK<D^HqeQH&te?!2N7n~(liE-{t>Ag8681ESt^6@5o7vSH zS;>!59tBvHI#quCQW6w;2Wf>oHh$#2C#85Zgr3tlH?3bhH1kL9wCpr?aG4IXqudjZ z_KR*nPc6l?W%LM5JJmVc`1!GMAN}gt#DjRZZ{G$`yUV$L>RG?orzhh_w1|p^If~;` zG)}2?|6^rAVWHaB@JZ5IX8fo}kYa-;>^v}(20}AYtd@x32C}2WLpNa39|EZo`x=b3 z&*x#l4BRZ>KvOkj0<9!Wn5Q{|a`X;(fXLMM+yE3*3n$`k0$o^I658Y`g%)5Ws3%wfdL&ut}L zlHw$45t$n@RqAWuq}8q(`0pTC-9WKfjdSa|*t!*{WdBUcxm6)|R9Wl=JCpKf*U-=s zk{kFB_^J}Q`TZz`FpMjwvCLASU!h+IxpVLFJyJw2nJIVUNq{V@wo^iBwx3yAp0 zb7k}~`T1nY_Z>s)CfS;Y*TTQ8hktjzU#VM>BP(D~bbGMO(__WQy#8x#&>x_-)9vu? zx#yaNyf6M~E-l)8vfFb&9ybK#=F0oxi5QUEfC>|oL+0G~iX5^UtN=EAH&tZ0-yF|F zAAE@*iHN@DTZeGN^__2*+&U`@O{;F6#_jtq6f1vrn#R&FNoM^}*v}lAS%_A8T8}Hn z;b}j)io!_})mDAm2tM&bthwHU($Z2&-|1j!m`AX}?sb<2Oet(Yzx*eXQ?ar_3SC$Q z=Z2LPH*{6Wx&l|^<%J_8VUa*G2VQ?2H3y(7kPe8uhZAZ9x+UvKBvPx+2^ze$mLHri{Sr6R!t%cxLZt{|jyXz8y%iK<;v|8bC@UHM%-I zw12c&?PK0Me(aNPM?~5CjHPPgSLAxxdR4wC0zoB7jtO{q=wl!^5)%JCDt#lPngxrC zG8#hL?+jT%gbWlsUq@tQm(J?&9;?UM?I^p@X?11QxJYaryzKsDciQs&>1&Z6{{2tm z68d`hj#%hci@m4!>*{9%r;oV6Yqj`p+9iX{KiQ9$&qGfpUiIJBpmu3?MEeWWD0(|_ z!rpTJfHy|r7k+T>*}%rQu#{mFqmagUPMk>gK&)}ShdumTZ8sN>4|n9eWP5kM)eg;74~3+&geCZG zx07v6M$*plfnmXSG9++xFC$WL*OE)TE&X81mzyhK<$Sf6-H~Yolwu4+iU&F5sEFlSfUR83|y=tB0Xm#m|O?xvbgFUNnxTbLjL9oZ*(1%;lg5Kj!648|WOd z_elsA+CDmcek5~p1U?5PlW0!|l&_M?gpqU7^7|R4glNm2{EsA4CepEv^dujvmL$T` zc#l--%J=GArUFzS9Zgjk?X;;25On^Yl#>w0j2}~y-Eq1(egbi?AiEB}c4DxvV{A;% z!Sisa@0y^ddfgOxsIWiIgJ&*(J%BO!HG*VXe5m2e8>H|#_}QpBAD4Vy+T^|FvhCbR z9C33+_UxM{Ev5pN6>S+qnG$<0c0~OoVvngT^XKoSFZQPQ#}X>pyJQAekk0Jz+n33n zDOR~H#N@$tFmhMtEdv16Pd<$-4N2pIhUGATu|S3B}=~r zx0zxsgL*{*4{q|y{o6;Ivo`Z?O6N9ng&%8OzlEJBQzBYH*_jkNbEOgFyg-D>PK68{ zeZGEpX5$e~KEWGKOS+AO*G!Yt3N=dc;tZGR`xny1*(bG8iAmp1os!I)0HgJ*FvVOmO9?VRJ`=*>@|nCOZg zCsshAFu-mkK&CV?v_H44p+zN^@HH5aeDg)wE*%(*4B+PQ1$5o#;O+s| z$abDTK|6YeNBUjJnzcKv@KylS0B#$2dyX;rch_o;_L`{ky|og2P}C>Twh39oJ+%IC z<)HT0P}Oit)!+T>3kTEhMA|HB3*P-3n=_Z+5433W!G)Ihu7*;dz6?-3usc->y3z3K z!cl2dacP_G?vMr|f9mj*(qe9!!jb^d@{TV+io&_cz3=*?l^n)U2TT-j zO2M52iR`12stbVZ_BI&md%!115e4rwTX#`rwxG^hVpg#C&8td2bjrC(!}N^EBTBam zgz=dYy-Z2)W+Oxp@hsLL`-}Fk7pa_(aR`#|=P@P_+%rGhen!#PyZ8Zxv~J2L!CwBC zHQScsM~in@C+~-&)m#QvG(zk$+nT?Mjt-6$42`Zzf_et%tcF%Nd)VV+8%xL5X+Dyv z?#otfJYyx6=1aZFI2lKVMIX!U_#b?E^hp~#rBocOxSNPu322$5t98x^o*|e#KoMa& z#e-CBaZ}y%K5g59(V;&pN;JBur6345?|7|3bYKLoQgc!=_QO zw-@~X>=_j)=bJo2M9o5W4sP)<1m*BfE7R}RLo8Zil4`V>e-3g&tUVgZ#r6c}^w2R6 zJk3G@p|#y=TCeC#(N~HAXO8s)!Nl7kXifHoC$d(F_KVoY5s6!0A9J2IDzLh6GJ~tS zH=L{XiHA2n zaiqmk3nF7>I9w>C{OE)I&st>+Mh%5$=UC+t`G;WTa9e+C{ji}p*AJAx_jM3fxKU*i zmcGzZRBE$g{33oA@g*wE@l?3XJ71&mKZmt^{80(tNN@~#EKF!-UV#A`7CK~G*5i?)%AxjDR+L2b* z3(rD4>e|5j1EjUSqki4T7L@GEBP;Tc$Jr#64^$pI;cHiqk{aut{n`^Hv1@Qa{jf2c z8*Z(Ed+J<}P|;tH2>^KI@0$H9$TJX54RI={*o8 zxiUp)+D~ExP!C{3YHc_82{jDiOzgTc+1)W~w^EObKk>6pym{2hBE#x|i;0Qx?%%Fw zG?$&OITj{q+i!2@c|epl%A6EjOr5d*mhjy*aTk*5??XLr!)^|Qx$;%>&gst(gta;% zZLpei4<&Z*)2c(=yK|u3N}B*;6F?QAN&{a5WHRWU3XSTIyYt=yIuv4oZz`*QBbNwY zvW@+;*}vY8mW5&f)|u`o(?YsENzAWppI|^BVUomOp3I*2C9(KOj5Jfrc4JeEMH!l* ztp54r+NWMFJ3Bj<^=`i-^YgL%7pJm37$YM{BdJu8K6CWObN<3ve(&_eP9o+SZUpV0 zUv+qL?IHLMV_*L_O~`x9VwqfS^GV5lS%e_QWz1B}o#$H@^&te&0Zz)o!qPPzVz!4O zGH8ELbrCbqEb}s>&+~)%uvV){c^reo$@>w*AT%TLaigU?&o<5hae&*inzuj)m`+Rp zG66JjnLm}OrT+!Emj;R!%L#)y+Kv~+-TOuBXP^lIH}0!FJ$-%3L{f?`K)Wq?$U=cF zCyj^?48KP-I4~f)FHY7hG~B7Ye30KT`&#TU|yvX zx-ni*%HW?X8|ke)g+_NWxAv%)i&keM?nhLyi0BuGl2An4qa&Az?C3wS*url!DGX`Q zxfjOtp@g)EE+_ZNdltcGRZgqWAx(yxF|COvfH``4dh*_8ry?yVxdifTzMa0ezjOX3 zo#ph+2l6^1=*u$~h0cMi?_G@U=Xs~~-NQ%9ySyyiG35;f$@uxb;?qC7fgtjt*#z|v zB-kX2ruQ>r0xJ=p!9EPYNL5u8&wl%I#LtYO_@Ngf=Zmqkv4|{gxoH^Nr*nDbRgf5# z=mHrnA&0C7F(_I=OLhIqRyG9p(U^G%^Xj6fzLLpePEeICFt^I2^t$(JhpZ$+VV%M{ zc_xY&Pr+*>-h~ilL}IaU)IG3JX?z()=`!}HaSw+j;=b$C;&Y`6bNF5v%NTU(dfIJx zFikfUXIV;Epi}_x<1EF7ODDVyoN-2 z$}5HE;Fz6|`gSkq2!RkAAMaK>Z`w@PTa_A$UN}mzGQ~FAswq2)n!#XfA*QUsl+a}v z1?rR6%AG6w#yYFgD*xK@%uaoh4sTGL;Ix#Pob3)qqasY4PoMmHch8vkiG4KVpD98& zQljH0ZHbhe>f0wzP8&P3o-5C$`VbCF)iH)4H(WI(km~PiW#`fL-*>8wpr|E}afZ)* z@b|8p9=h}gINz&!Yt{nzgw<@*Bu4SY!xJt#p6fS=a}O%WCbgACeN*lik2>I`4iZP| zyBCl2`rq~VHkz|M3b`YFYq<`1g&bK=QqbTf%s1GT-A@kHaOgd;x3`b_noCYb&mcS6 z>-4iRgHW){JYcB4{LZDhtS$dy?)vI)oqU{;OCl+KGjJPve(*XTNep=kFsICS+7Hx2 zSst%HPcRb>a3K`dfy7MG6{v<4!9*iTh7GbzQla>6z!RIG@2O#j@Ur?c_TE@(`X&MD zJmayCQzOC5XZt*dz$qWq1zbw#wjk#C;Sw_Co_N{k5hTdEpJrxe8oP!9r-2`DgQu8s zegDM31Vbts8hju$oW`k6a4JFm!rx!jL@wU;pUU$?{^BBlI{QaCncSp zZX$9x9xNO*WfxePnzAx!&`*&}&ZJ`xBz6OzEZdak4D11BqLYpbrB*{XHq;!u4A-yb znT4X>bG&XlI1VKtAwtZQZ2v$#G#wl*)^{F$9*nuG3>Vx$53Dpo`jaOQ2;K+u#1bOj zGL~OhS6cOrMh+ZQbAc}vpGyWwpfVgRMIl*5?UH(1Ji6VX2#z-|Bo@?}E*^c3;UpV+ zJX)GVy}S@*pNMSTT55~!5v%`OqZKLC3I0{S3etM^eYtX(<+s)Re2E=Vj}swcN? zSN9Qw6>25M?&OzA4zN0_(fR~6>dM$MbR@P<-#$`jm#2M4CsRH9(obTq*7+O&Rrp3i z(;ol~y{U5xJ4*zJ5Dx!L1(y1qx7gR~&;vX@gm{AI?nd#-oLT~|HOX(vDw38*L{WTd z0?8Y5@U+ZQx~BE0)z}b8>XFC72(IHnU%UWcu6s}N(fZoj_Hh{}32MC7eq?+cFH;1g z3m&F{su{knd#it;wit5u zI^q6h9;LIa{c~HyF0EJ-1RXy`7~1xq&Fxue-;j4r=k#Aue|;czx%Txf#5h?q-%`fQ zf@vK($%qw+X71J?%-u83>2CX^*^Zh^+_ZhM*5|L8!PNy}@Vy^W{JE;sT~^z()B9;B zYecfu244!t@^ZAr+$EypA_BixmaoKa_N!MaVi}B@c(*5@{P4VMM5w5#t)D!hg5I=Z z+qJ&z<3z*7n=*h9-4Gi(++p$hz)6d6Ba$$aL$mM~uta&Fp6y>q36bM!`4&V#$3+WJ z``65?;D+F3Q}4+Hh427=}}^(j=5%ROpJ}+=?c-QyhSLub)tpMNN+h(1}q*h zwBZ)2Maax%XQI^fm0QHTPr?o`59{K6FHs~AVo-t8>z7Vk>-nwudivl}z>lY@Z`~_L zgwcjMXdA_yf?q0qk{|6MO5GKQEbv(yPPfk2MK0f|NXf}xm0Vzj%^Yg9z z9tO{cSkqb%@ih@pBNtq9&^SE9^h(Zu`weu~_{zCQj$7@aQ1#!|dk^D1aGu(Sx4E?o z<~KMq)bUit2W;(nol`necQ}l6_V-7pdTy#s_ggdPHZ9nG(ajBw+!921{@(q~0SDy4 z?7TVNSl;_qTVg=_VrXxXi4*_(VzL$Rt1Ns-T_w$V^yE&oSc)W^ioN_9mJZ60i~=ni zuR*)TKj#US5EzzphLVz!jhj2#nhmG%Wm#rv2?4;wh3=e-zdz+GRkocEqx&%F|+Q7q<7It z@F7A3;f{R~uQ<+{r9|R%9!yA87%}N0FkRfZE2rP0j*&{i?6I#K>bCD+l#_=D>aEAx9gSqc&XNe-~U%4l%+IU0HGoYQ?SlZF~3sqd(yE_wwa7ryzh^8i+8m_Xt-;{eN?nLKOI9Z&0OMuhesFEq@$mWE>pSuw6L-$4#M!9Z5{~ znC9`=rGw>G>T@yS`m{Vj18<#mid`Lu6-;iPw&}cN?$k)0=c9T(^6wqTjvQ)Fp0pNlf4sYLmJI*-S7g7;$V&}k;Rxs zZnN_Q2zK!~VOCaFDGSp#NoH8+@V^DV9Hh1C-R3k1HwKuJepXw|e%Ur{OgNS4j8n?uX4v za?;3##q|3Kgw^SGhWig=Q){Sz!5)-Kx|yFImzT%oH1=Kr~2}`W(m44^wJYonnz2tTh(iThm{t&5)*GMwU~!wGVW)tGl_}g+A<3jA}m*4oDqEjkGg!uen*@u=jxok z+dA>)qZvKj2d^D0riX{36Fu=f)X0dT^()P;q(6-93s0FJD|E5G&;Sl2WThaB1EWo# z5=-!<=}VEjhe4QpRewJF3~cBdg7L{Ivk*Z85ldeO2CVzjMe#%)Tpfm$V^L3XBnCiK z0xiR9CKaEsj2`WovqvyW6#gSf`7pk*a?$O2X2`g0+(FUu(K6a$uu&foYy!5*Lxtxm z>sxgeiG6_}bY=uKo8s1i^{<( z0IlIH*>N;W=ZTbpsGi?hbwBUE zoeV9!a7u*aQki=u#Z2A*xmS#LG&J}{!td+lZ)cHldyca`wa2@E#F;nYI<=UTS?)rj zUCQiH{Pvvqa|iHyXefQWK^^u&i6-I+G7tkeX^V*f^ z=gLJv>v*O^x5umhnDOH5DLL#F*gx$*%kN*2k|G7C8F5~SZHG=c!Fag&;H_1cp(sPmp*TkF#bk6@JKMsN_2RM5?i zLDk5x=|{z-@TH=*HY>WNG*W8Bb99eghK0aBKJXl-yhr{zryWy}iw(+t9AJ>@msC84@s@+2+wpP_0m3zSeL@`p(eh7Hw3a%1-cpZB zy6^ur1FRi{60rW@JBS7$mK^-3otK{yoD=-FYw%TXX@wxCSEOBdT^~7u-nH~eC_c3K zElqn1CaC{DFg$}Qt!(}6)ta+uSR@CtFrfrwlOBRIdbIR?IC5)(vAI>CE~2)clEEfeY`G!L*sh2(x(K#IrK6ScHEoM z)gteepum8DpSpLEp#PZkU(QWHtAqDPw0<%I9|;8q7VkN|66V8@KY&hPx}{sg;f45Z z=i_5SeDJ>vL-3ai-6MQiPfjbgMI(Sv7goK6f%^;~ItaJ6o{;iroUB>?5sQI04Fg2+ zR49U-cEWNkhS?^{0~as3>m=Vl)M^ylL{k^5fMYZDOgMVe!4)`k%d)+^VpkpQsv{*L-l zIq0K6Rh}~qwp|!tA>k{Qz3j4;T2MUno&(`lN0z#{yPKCng$)K8w7|fP?v|VnOuQ0E zp?BF6CQxKM*xi+J{1+3pv(-dG3NQ@b)&uonTtY&u=TfF)MgQo3d0iuN;&5o(xdc8t ze4wIA{x-E^_KkLmkOBW0&Gh{pPr&M5WoI)Mc`|M0`&U2{b>|YV8>&U{c~#%IucZ%4 zk-)DTNyhJI5YauJ&}?NHeFGQpeYak&j&(LYl8p$#P#3R;;5yu|FCd+A>Z9&NQHnQ* z@o6QEL!A`Ai^d@V9x&8bKH1G$9%K|wI&vjmwG!(W|2N_}YWpy_8Xo_( z0^WezOpC~L#j)`fSOo1vgdy)ySTf~obSHK8t>Y;Of`0a}8h?9t4oFYOuC8hdB9zHz zI(wFQQu-cPTNA)xgHJOvy9{h%j5?6G0J8$PUm!$1ws)^wpyq^sU}?!!okezJZrn|F zU3|>e6X-Z#&gWJ}3K8k#XD%WD{JS<=+J1bj5c`=I)^ZfUcK0^JKp%XzQXVg&0r^Zk zOas5&`)f6ANhSTHzx-sy^VZ=4N=+c8J(*8yv)}6Un|o{O2EWAduo~?(i)g52@KJ4r zh9hb%!0Lf)Dtn+qQcp`*5p+s61y0?1L=KrEeZ_k#z7O7;hkxf|5|YqkgewAI0%Eos z7Aot*?m}$mZ*KdaA&eFwE9OB6j%=epaP`#VPSF+_t^!XcG^tP<2+NBZ$p<{ApkjVk zYB*h(s6>IltY|6!RzH=Om(RWGSwo5&sFb@-A00z=R#4m?v%iymgg zdCd~LH3dkji8^X*7X7e;x!2ey?|qIu|rm23%6iNPJ>6S3~z(WI^lrgnB8r~{PQEw zOEKG#@>hzd=>U2EDl@aaqaz|RsDk+8J!)*wAH(ZFesLB>H-AD@))ynX02{<>z*kot zR3F+BX~?m}Bm0*eEPitfjzJ(QtYItM;n)>LMbRbUG##KqMko@O5+&ZLKY@ajo+cX4 z`U3BWPuW;kzi>_xt)8&3-ciXdi1X6T^IxuU>3~a8A~TMUk5+`lXywoABQ>$ee3fvB z5sU7mu_INb$=O`YsS51FO(}s$SzkHc{`4QZ*)`ve2VKd@+hSCc`$0ta&ZU*#HJ?@I z5~1n>A@j${een7Is?|BeB z`D9`t+tY+P7U3)-`NYh3&ldkxmg1UV%ESfan~R-K>BH||M@NRjLOuxXrqA_8&v5&8 zicrSQQ-wpiUtV*{r>pKgXgIXyXbHl}5QqmudBQ*sb0dHoXH*GB$0&S@yc~K%U{$12 zb(scjE`a{Yod=Ec@zv8WfG~hS00|&!lOf9mEc}IB&O_*S@i%`A(~7g$dJ>&2=-cQ zwr7!lN4*QMAPTh~%F}==SWkM27;deWt9FKcW=Q_gnr6#ZDFvBXK|$ukH7lo_TwKt{wu46?J5I)d+iMzfbU+ve9vl$oK)4g|EO5}xS`(u7 zBg}|Y@f3DOR$GPX&oD+B=|f;AZ1ItH8K6yojKQA*>^GR5t!h{{oq}) z2uH6Iz>c`HiN1s+^QSXjw!~43&qvL#KXDtr&UtKYbm_c9gNl*v< zZ-^EL7X?B+lR#V_Zbr6q=O7unGs8Fo@Jdit55j81_oslkU)%77#0fYfq046I zhj!zq?lEHwu}jOIiF)Bkk@LkQ8#9)QrnO7Wn7J4Dzq7`bh0l`9c4;~T2o&Cz$8RMj zp8Vq?;D%{Bc2{otz%&w2c)w6#Bw6-*kPt+G5X4%EO@xBOJ?P-mzlV3JQPW<6)+3Z8KCePYf9W&g{KnQHR(Zj_`@ z$300bUqr9 zv1lly=V8JmjjzW`^w_#LXs;l55QLGc=@;EleR3E1AY~N_vgp!^CY`v%C3%(?F=2^C z4iGXG!Dcvcm6jjjMtn)ex@d>jNK1*1f4Sn$C7N%V5}I;cXa)k6ztKz2){Y5^$Jui! zrP=u1n2F2#kJ|eWa-AnIJxBj`nxz}qAIgS?nH*#^=T1npiXB{gsqsRrwnZGSu!pl}fSPaJGTD}9)p+})|DEPiXU(P80e8W*Cli6Vl- z%_FrPo&d1cvyt4#JmAa0CkBA-xqzV{JuR$u(FRIsIRDgfnf7_@fJTMvnT8sts%^ex zY8nH13|P;Eux4VlxqY^qU#|{5pU$orU@o{eR7k}2@AR2<*^4gZ*tYZ!bS1<3+)5~c z+IzVnWl5%pYxcRmMIp=V8(z!}2JytfL`vMt>{*98^X(-*1HY&IO=G{sycJ^J8@9Lz z<@zESA_((~AGlUvs#WJ!2xQZApx{2AdBIG~DpCIDXy>P#mOWlfn6IWg1Yh9Q7Qj~V zr^S&k=AnfNP9jNpsJsx%%4^rpC&t9Q-G1BHD2t#jKJjS$D`TZ$xY@<1|C~L*Vl3)j z#9C?$YDXj(erSj{g&^i|I5L^Die!K9Z5;;RGI>wJrQNFik&Z%?o`$y->J2>p1$u7$ zSP#k?uK#p58lj#cdj9)q`x+C&Te8^Yt^~$0D7Uq!rAd-*sGhf7|6{6ntYp4rG;Az5(PyMP&bDZmm9R2pC_K9ca7#Bd4)j-xh&cRO&@=U+@5#VE0swAo?>EfHkO{(E ze)jAc0!5#i+^-JDMzr#=HU0#^?ANz#kc&8GJ$rEZISj*?y+3`(0=p1WC7(>N9MHcv zSwRzo`<|7BN*M}?Kd1Zkc*!x!XqlRw+d2C`ifx$xC<7n=KmT#e((|MYByKU(y(>)( zN4E{UKF%bZ>;-UofAaS4Y*KB1DI9Y2gNRk~`_tH8)K{J&8!kH?Nwb$;W0=%U0H_`N z>&ZJJPa@=POXGZ{x7SyV>0Z&G{wYc)bTzjkPiHGxCSfuzLhSQB9)NDI!5bWJQs*7v zA16zfc%=T#?`JmGUs=P&t`}UF7In*5p?O_eT7sBkH3x(Dci@K37%(tW6tR!n06(JR`sn>I6RY#{s!#Qe|aynobXaV$WC;FlMaOQH;AQge9?as zT~E5ul~chy5D3la1#%|38RZXC@zQ^UDp=DlLtmhU3Oo0AYp2jjIPh$*hyOiR1Y`;U zI}K`15^ZTJwYU@=8KGAwkW}&{)#_jwSv4a41va|MJzB|ABIi^F zRF36Zw7?|0ov&XyyuecpRi}^p%R(u(AUk(upkC90}%;-Pa2h8VWFx>O#Si=6k`l19>;+h`857cVafLmmmz9!whCJOZZkSJy$75YScZ}2yib2UN zkW%5j1I86)9CqlwWj3g~o+3W5P?vGMoDBWQfcqY8wBi>?_T7cqWFRmp{4UC!#>aUp z@566{dbiuk8Ufx@2v$z4pinKv_sC1BNUPs(Y1hvT60DpAq*GvC26MAgd~cmzJIaw* zV|X8G+^%_vKu1tVNa6O~pTn*nH{}U1o9-^Eq+a;cP8NOs)SB*QliaZi&kl`#3{hLg zaaLq{6oe~FoK4=V{NV^|9gqz`MmMfYWO@utCMF~_JVHwou}c!CBD%M2dJ~ro%T2&0 zuRguyCPM`=8qHr8kV}!c&AJ!`giTh_N{0$^B*b~58xQ)?L<28Ia#kP>L1Hh2x_&gn zAAKNIIktY?fycgmRWPF;A6bA6vZRxs;HL|ppIjclS4*AiJQ3oFQ7Iwp zZ7`=q6vj&o3*cw1=_>KnjWAIeqDbtmoz}RrOU#GliET2w0 zVJ9a!{v5O(KxHI17**=yr>8tMiU%^UjhRQ{ZP`rmuOGfLV5vMxFK}jKn~aSlv!uHq z=!M&H`jYs0@%VcmSPSrx+)!5;mUP{}86bOtgatJXkfZs~*~i2j5jZ~b;7@;eYS;Bn zS*K1+omQkbR|&kDYl)tJu+$%6BEC*A`?c~hMe2ijK{^VN;G(NUZB3qm8m8&|C;je! zUc_O@$`B8vYhY?-i9vVJx3-bH+NK0%oIIt3M*vgDs0ZN-3B?b;107L;=`2E5UyK)= z5vUKe8L``KpJ@^%Soj&dPAxl%K0;tNtKGq1h1W5tCw-l}!ApZuU5SbF=#v`i{zfF1 zET3I5VvF)5VecAZ7yAr`?FIq(biNrc7UPTw%7S5;!y$ z)yv))b8Dx)KjEb9P}u)vgO?uDbY2F7FNa4!$qRw&0{v^Us=RLz^lLCQp{D7LdHzw@ zpA@g`Zrzq2@AYhCIDFob3EjSVRESE18Ik-9bAB)!B-yYCLC{wGn94lLsNBST=4hI% zD(I0#$n-7EKTgV)2O;u3vjD6%h;npyyq6dtE!nUK(x#_mUO1Kh0AvdYjym3K%e&!NCNGSl6=aBcIg5xG$sQkP)C1OM| zGF4__86aE^PR7Z>p379fx*l9yZ%bIP!WxrMPFvn}uY=;yR5W8-D8yl7KMva^xAvMn zO#1FfmHNWejh2aj;=J3Hx>&Q*9fI;8qB>_MOt6>l!E*+M?Oj>-xd!COfxD{8#)0cnz98KC_xaFQp~ zwPoq=7vah-h~@a#Kw}=RI&EFaqf>3}o;!LPPO!U2JELr=uz)$&70Fz~5N3bMtkDiK zL4b?FCNzJfx}$%!&yeBw{lCW}y|v5DPDHfiV58F#{s{jVtR$dXBG~Up>nRcCRE;@a z!)wUAyuOcU8W=J`Um5)ng1oC~`l=lUMf$G;jj@0vqj-G!9JPgD%G4)>s-JdF@L+mc zGSp@BOKZIv#@`27Dt^yy*WoY1fUm~9ve0GXof%7+0rkOm5%pcUFZVbJh&d$IZAG4W zh(*8VCkgGJeAV_(uK9SoKW{$$hk)gBuOgEzb7V;gbWe|sF7<{}`{$82D|CoH z_n6un>pVn}ZEm=R-j53>OkzrUjXpb7`Gz>Z%^0RH$bk;T9TwJ$e2QRh(f;co^E>#u zI$Hd>`rcfvSpP#rY`L@(>v*IY4|#rk#S7m4>B`QZPks@c`#3jub;={>LvFG5t)F9G z+_4|N95t!jLHP7(?s+j6NDmF+G%^UA9e4WXi2hALr+DP#`MpNor#JfAmYF&&j0p)lA#E8{*#EcWTI z1UysXNL2vRtbb{Fs!vfv%GE$M*sMeWCjgSolJv2%nB*VaUQx zhj#hgZxi~USZr!aH?FSOy*(9`*xg7yQ%2su;gUi`1drai=~=)m!|F}Hys#UmnsuX| zl%&JeWBoUVqZ2E2ufsO%`Bn*RfVne#&{>ZTsw zW(Eo2`P`C{*Q`OHBNnZ)b!cSupV>CAZ}L71*{fi11##o+L1h^J2ET>@nkry=1c|(+ zZfC6O8=wdpck{l}6Z;Pe$uFc=lR zJIlV-4quOW-4(-}R{hSC`1Cz{X%I$|ZzKkTO$se5`B-8a7(x*%ihgvs^!{08*9n1i z`mMB~&+CD|Xd1S!i;d?mF0AtKy<2~GP5UI{PqrpW-(4n1k;JzPh?(C`m7ixjqk+EO zEO!kqT~%0765P{kvFzF1mBi1}#p6q}%%BWOylOu<15`yqz@f=m9ON-CIBOr!!Yxm_ zt-Ni|7cX7XA5DNVaekxp>Us|+=WyWO1#KMf+M~d(?ixePIywzNm35AD7#TKH=9S}pv*TAc%X2I|YHxjTjbyF+at`kylq>{*%LSMo} z0p&XUIbgBSigKi+Qn$3UJS_97?8}2Nb8DP*>&SPKuo-5mP2SV(0GG;|oLoX76}4~! z${!D&>Hcck(Al+{6h5hV%1lfb#{41dlKM|&gbHo6waF#Dqvuhb@3M+)%;a)e(2Z(J z@l#HD>B5Tz`J|?@2KNo_bsuPZ+E#7#jXFm0_*A`2JZ-nG>P1C zhE*?maljuz`8_-~77gS9piU4Q$pnHJ2RWn(IqtK>Qn%CNtqjFi-eTn7_vmfqd>rRA zGenxyaAmgXB(~;6!o7`c4LxRdZN?re&BFRnzGX3jV9SpXcv)dY+Z|NTp|x?>bdHY_ z!q(#~+{^roT*>Pi26Q}e@!s23!pFA|NX=KipRGgPy%p3X`0kbye z2_OV`t5h#gGkq;MSh%L5_)Ytw2dKx*gZF;OKywC#lhg!{6f8N2wfUdD2Q+sOg7hKd zPU_nPk|GB@SxHusQI4%hA$uJu zH0+Umj*%!yQY3PWhRTYpj3Yb8jO;x#k9`hj{ci8i@9Tko&w0G=`*mISeLa_l+F{3; z7j9WIodK(cOZ8gPdj8{;=F)#KR~64=V@{~7?gp{^HLLK->)jTvilc$Zy4>fryk!97 z-QrV<>1$V|Qoefu3k0xyPkJ=A>WGs#-sM-n?+X3Wpj-*ueLb@)jY5E99e_*!to%6h zPsc7Oo9tt)zg3p|q+Ms8oHzHu6597yg;(Q=*Z3Tc2eLjo@Q+PVQ!^1Ds|8=5fgTD5 z2*d#JKm{97AuXPYYM|^gc}sCEjPc(`4IuFd&%#-8a5- z!vc3xEYhg?`*c88h1G{T1^T9Gd28S7ibH`17cS;Jd2$pZNz(gFKBD}MV%#yjc5sOW z`6VhjsqJe5Mrp`XT~k%PQf?d5r>&1rh3>&1TJlnbZzigA!2kpNYN3skGXnP-qyjE| zrP9AEr_@;s`3@Zgl=aTK3-N-;o9XFfPt`=gRRhN}fbGl4$u%mn0%R+pI&y?a}GK(CM3ZiyjGDZN>+yRZ6l^A%(0Z$HG3tnez z9idDIfYVzNu$6cDYxP&fk2- z=kSs%4RP<{12Il@3??U0r;M*g^A5Y*6$r3VW!2~Hme+iBO>V*8bYZ&C(l@x`S$mAm zu{)d``rW&e0oY#5d)y~Mu}?N21~pzIuv#p=JMYDkHcTwzYC?G2IR^V&=aT&La<1QZ z=SSVa2pHH398-RsL6jh|ibpVRtTL(3V|>E><9Q0SabiB8yCqQR(_dMvLYqy+xH!ew z?0)q&L+KoAb>7+1qSmAdV@`SK;JF~ncD`<&m_)eH#m4F;-^)+Gamfze;ahoJcoKW< zAetrpUsd8?0j8sf*7)aEoYbA|vNeC@qgwviVCa^gSaZ?@?1uV;A&E;;55$W|b`4*> zaYG_qc;`#;JCl;h)hvhoPoHhXzwb4Gke#Y3K44P}dK^HHK~)7fDMx3_;bADa<^=`@ z0+lw%0X=R3f~z!OR@^@@5KC}vs{n*R;P7+I^;HvUJ9cU5aq#Y49-_Qliopb9N!S^w z5KF^G6Qz5?zk{P*{5DaR!}>caOht+cvE|#!HC1|nljUrJFe^RqJp}V)QEH`^<`{{G znT``8YJ+4BF~h?wSU{_owQ(7*lu^nJ0JA878iDKe%Nn5Y0Hi1Z!2AV%=D;1`Ejp`3 z0r=Pm;kjqLZy3$2so3Y_>Kq-CO`;*;U+5gKSQ?#oj(7iJrb2hIf{l)55au;f8lJ(FfO<~iqhqA^(-7Y@v#|K4m?cia4x zG;>~=#WULa6}7b_*a=v{i3W?tANurMDoP7OFrOxBaI8E3^1Gt_7{Mg(Z1OO!5HU4f zQ@lOx0`|e6^2I?cmUh!pI~p3mcdLIBy7l5CV*k#O_t{m+qX?Ci(uqhi|M)KlpNJWo4V+}{57?NMX}o5Axc5&i(pm)w5bosV5p zLc~GWA$69Jm_fU64qK6|+xuezSW;PM-aDLIq56NAE~22Zn};699zB#9c1mNvY%+qK znSZO8RRs;Cn^4;RdzNbZHnKe5Ux4xbo1WfE!M3P{mp70wz{S-BhIa^%q}yHzdced( zZnK<|sh}8v0nu2wIS~VTDk!p9SHgM0)(u$!@-iiQifmO9mobCkYCTe}TlaL3<((6; z8Y%rYkTdu6DlQg0ny=n|AawEL3;UcW?=198?<*`~yGi!qkbCb13RBt~0gf#3>KZLy z$sIW%Lp6T=xFx57(p?NSxLqD}_W5Ku(DwZOP?n1?Q(%a9I14$e56~9WIT;T9k{x8{ zqM>pO5!!c5bpCqZ1>0z05radEv?qSp^{R#fk;(Pyqwrr|=&{c>gnyDI6j-7@YWqt2 zKdL74DHvKvd+rmh8N;gKL*)lHUN>)Hv{fJmw`ga5Jo~23I^JC7E&D$%fNa6}L6`25 z#<#hDNq=eXdT&M^0%W>9VH*g*A22~cpv)$8- zfE)^-TX1~`%lm-*yDrHCUyz;EA?Y?>ulzS_Z~#Yp$snqDO?+1bFw2UpZ}ZawvOQqN z0zpDtzatPs1D1M?W81NaXkehcc$5n84uJ501^`rEdOr$49JBZq2P^$CWDTU5GppZ9 z0K0Pg1gI$S{UYfL;VH;aHitu~nN)+VoW__iD3QmSu@fiM7Y;y>>+nmV!>5J2KYbox zjphM>4d#(e%ejv7i4V0OtAoUcAqStu2&ZB`?bs?SH;@XG1k;u*#| zcd}0^S&vVr15(d-iZH|(MGl%}`E^Z8hfTO9Qu?r7T~Sf=su~@n<3gTu$)z_zeM+5A zo?86=qQ8J*0rX(Pc!vW`=gU~lK&l0YPQ%hPc{A9c@!RBc)Yb)^dsW5a%O~wGBUwJZ z)ts@s_hG8a7CT*al^-TAcpj!+JN@|LNotVKV%4>KfMS|6g3Z#*bO0N{-s8k-UDz*r zh`c_mA(8Ied7dp8luu-6qUWYzE!YSw|7`>)pFj{VPZ6q`EQ@t) zxcQ;{O%2-NJ^Z*a05$1tsjH*QfbQ64JTH1u3qJj_x0vrxx@mB7^1QvUVZM2;n&@wh z)6_OWAAsRB4L2NI1UwIAl66aVI8BG|w`Qq$*kV7{=Z@|v@Z4%9W)>e73e8VuZ;UrUIVMf7P z&`8+nim(&U8gzB|{x_hBFksAx#pZ!T*Gdeo2h_%;nPjj&IbOImhE&S=f z6Mp`H2Yf#lRVXvFwzbLppP%>%zwxTfPO;KRxAjb}NKF@*aGi3n=yJzFq8x_S!0PfE zKC}{L*Mw7U_&ekJ@~}h=d((Dy(C##E;fppudDvA)3f`+1*)u?V3<~yORO+@PAT*2;z|l(Q&oFc>7^G%`Xjn##d+(n3s>zKVarOfO z7a)On`GgA^8X6w+8r1Gml~?ecr10((*3Rb+X^Xy#3~7ufFxG)WHLOu-qmS~v(FZzr z^18JC;vCZA0yWOBw$=Xp3BKL!cT#|Jc8r{ZJ)fwx?zjSm>WSIMQi&vBfyxWw9u)vc z4VbXY_=lO8T~KB8mEVKwk<0 z@ulD0u)Nz&bXmiCY#jZ{lukHgq z;r_|LE52LHZP6q`(k(~vL&u?BEF)wFSTJR8$@#@*w!lezw`u9?pJJq+7Q-q>-$|Za z@#)+#F;nrG18@%1KIitxu)O^3FVqLX10dQd7(KCTP5~BB3c_1aQ3zSz?b`+|2=Dn} zX1?-%#5uxyNQCc?Tfx1*ulV{=0t$!MfZ`P3iO0gtdePVpt=}Gto&vo&p((8PQd~%H z!sj&&AfFI>5F7nu7tToqH6L`&1V;7IwVyA*y#zQ^7Ct2ngxvbV4NOu*iNGvi{fZmq z3&WM)L&r(jg2%ZT2Ge2c&6|dMhosQIM*-kKmLm2U4R}$vKHmTlGaV`29}YjuY4j4H zCZ{Z&15Afs8lRQH0#aJ*ey#<_fC^9=M}f}`uA4ty4FnKEcsd&ajO<#}$*N+$a z*tV;I(5e6oP({$IpK77n=iYjGevXvg|dhW!@>aNYFGt;LPeNRD3 zwIOflw)X2d5*Tt?UK^Hdx31;OFdMguKZ}F@XcfIRi$%2Y@!`IM7Ux%Vzc{2Xewty2 z+*FdP#y8LcV7oINSKJ6EnmJZkaz^j`DztxrQ*z1oGf~-OF+Bko)-#yyAk5=HMlYrMid4w)pj@tr%GeK7;)#ORD&c#Pjv%s^F2 z5eiKugXtM-ZjUe-G?3{i+@F}^wRACPnJ!8~^U2fYV-{zIn4;2%)zveS-OHcC_)HFv z6<`_XXr@$5!NC~ZN0ohVFstP*FX%{dc?UI@4NjXLh_J;UN8$hpl~JYra1iLYV2BHh zT$G$m2lR_VfJ-db_UGNtBI4vg)>lUZA@`C0_C)|lx@CmgGl~&XNdFb=(Ed=cN}1WY z`F!jLV1&G8o}{`Fix6Bj)3(C{pM7p9_W~HlfVvDs&BsEQW6L~%d?^_UL*{TRZv)}m zK_S`+f-DD^L#Y{MkqzU)OVEv1DDNdBS~L+>5X7@RwqqE5WPSS0@Cn*d{lJwli-#EE z!2QR(Sk+_P^o-+XZwZU^5ozqV#PQw3zK9um;}puG!#DOySU1lzkR?I;wopLjVwASp zf|{@!ZiVvt!a>%;?*9FHa3wjOrHYRj+*yA4m$e`BTs&~?&vatt!nW7l!)W@IUaENV z2DNVT0#>2fWYfoClbKhcupK|In|f4YKZJ=^e5BfdUj4wlPdGBNIEF;M?|rsxv|-3X z*>Y~DYLZzP_#7t5HHH=T$D;yZ9DGXk2BZ4zfHU~I?Du*->)49c z%ex&+F2EN?<2|?raT0;B3#b@CQ3cl&fKP&V6RapGtkL zh7mcgSuxvx?n;v5{1yy#>D9=9}z%|c>DpK0Dr?o(``qvsDU;YB>`rptY|CrX z%sc(6SCiTIBeR0lA&7=!x3Y4&h9ON=dPz{Fo_2Vp@ceD6N?nv5EW5ZDS>SWXbL#RU zWxo~u$Omt7bqO<7MsX`AZ}LcFD?SovydgS>sa(e>jN-prgJGvOlX>H^{xb79af-7S zsm}MyDe`#+VF=(yB2P7jGNLDZgjkyaH%;5e$0rpK6K}2^K+by*>cfTyRB46PX4cF_ zbAU%6V5R|iF(9DpIwR!M!JJls^#J(ngv{W8J>joMAf{yILhq_FlRZar!~URcvT-fN<&T-KSRt-5!JvO;>tx z_CcZm4suoO{4FaUAPTIRx9@B*7yg{y9vv~=aR=AAoRXF8{nOC#Y^mJ`bk0cPaEBW| zj;3#530e0Xdd52c$+YD+ZSI2LVRLkV&4W70)L&PAi3+=c;-mKZOqug9E z0HJVtiu0i?CORI7LOSlIXI~>wz|>24Q0Un?`x~_A81uTl$IugjH&YoB?!N=_ND!D^ zXxqC{ts)KKpuAU{!?$q?u~yzZ8t>W(knp)y9Fl3>g8XPlt(ejE+k=aglbmh8kkBRv zG#;wcyAEAOtl~1Yu$BS`R04#?15mGv;o2I1pbAhVw%HK)!!YoM*Jhrl zopDcm4$v`xh8Fq#Mf zM-e`P<1V@8+RwB1^e+Cx!fyyQ zr^TtAA$x=Q#_OYimn$N#4d&r0SE&`}s~((i=7q@7K{YH+%j7=qbgcOjXlTw&dXO6> zB44?e@51h+Ap9ZsIUKE544D}&0v6sbF4}jA>go!-i7y@+-JMWO@2GXN1HhvvWQf{3 zmC10;e-i(GI$hzyx3Axhvh%|~F~GW9==J7AwRJ`H6C|`x=C$gbbW0sHHgVl3wJwtx z&ojvUS?FZc@gbmx}D~l}iwd!0vT8ra2s;#XBL?RoSsOQ(l zf-&Qr0$&tf&E6`qj@({WV`FXJx>?pdCnl%S^1Ffq<7XFPXW23Oz-}+mM9f})6fQ=) zA2!K1rd?WRyBDhz=>-Irjpn0U~#b}Ch4;Rgo^%d-5zM6{{8y+74^wnrCdVu6|y;!gVLE;PPYTxF&5K|%chuQj-$7Zshyd~6SL`sdT=ypnW=!VG4$^Fm7@hHYx-Y6~Z zdVW8k^6hmqrHWS?o!y5)#QQ{@Ydc_f5)Pv#U>1*&N$YKdBi-d5CA5MFs&1Q=UI2i<{7q!A@Su@h}i|x_0#Hq z%s1iJMk#D+zI+Mj!(MLsE&6yCh94u$5tA<2tx!9?n;@ zLmY=Vk8#z_y4@Sop@_Rj=dnA|CLL%H2xk86l8Yu=cZQyr8Z9dMoKGH%FcxEo9*p zs%8&kCIg95xEE&&jvBsBd^L_e!BkAdp)xNYAD0LW9L5go6#g-ZDVkpJm0Jjk(~BIj zyg8sk0)seFEx%sTS>ms5h3W?XZgC_EV#ZgNZST+3XY(hwo3%9^ z-hM=u(s(n(GL;b+Vt6j%H`iz0KZ)+Gh_T*TzFIuQn2wK!L5`<_DyDk%&UjYhgNmrD z>EAp($90N$98ZpO4=*pff3Gc1>b5=?rMM4V6SHj^U13Ra^qsBB{QU0{P5Wx@F$8*j zubL}@o5weN&_k@x*Qe;D!C93Uf8^0yn>O^1_@hFcVG<%#`ov5nH2<@wU-|jn?NAK$ z&cVq}30#Me(#;T_>%+DlW!_oyRPJZDMIWB=bE;qtl=**%RcD!?G*DI5>FA3OF8P8=9vevs8bGc0z!d{PA=51*dX= zv3#*yT~lLBkxwq=YO=gbZtYOnMliZ@%5X%d{8y^T)6__6ymy}`X5`U`0rfO+vQ{{X z^W|%AuJpo4I25-rrY{>=-ATwX94Qwg2{gsPClJ8kD}SnGz9lHsu>KzpLsYi~({rUR zAJ0Hxu5kYrYH+^Sl93us$hT$GO9;J2S7j5?pzU$2+nY(fc9o!eo4pS%5(AX6)<${z)pLb7dp!OvcyaCuDe z9i;|hmvE);uck~VEq#%ikmL(Y{0fP3UHw;OHIqY2douFl=hO3i@ybUg$wCmw0fsYY zwH7|Yp-a+o=;xC!!!^BkW}|2PE<^h`>n~d(@=!f0C*e(%B>q%<6_5NmKB!Fs5)ik0 zm-{_c$rfZ+rJvkt&wNqgYK&qpqA$lf^ESJNf^g#qp+Dj*dj%U4kH)>CQ|{fq1FwnG zVT0(q&C&T^G^_@*ceHN?GDK6m@4b_=k?^S0X6i5xR@Fo@Dxk~9cv`*4*xNgxR4O)2 z5V5fv468op8u}^)^ElLf%Tps--$t2-Ap1XUc=`&4w>yupA%~K^A-K+pbC9Se7@gcy zMjf*Uhd}Q|v!erAV9W(RsLx%+H&gloac%q25?eCgvK>D$NqrvvM-HQ3WsWrZ&0O8~ z-Tf1G>2s@{?~Gi?^iyNfxlivU3}XQHe1+pL&C|!IF(u@rnIVeI$$OM41Tp(~_{^Je z{*oB9qY&hS$eZL!P7ic=*Vwa}SvoOptm0lo> z^L>KwtaX$p{^G9DNAT{LpF(Zd&5?n2)jP3WYq#q{^sxM9?Nr!QO;S=%qizw;k5jP( z)QLO;OpeHQZ|1fgrDIuUpMij|La1R zXt^AHcXJ5?%TS&fq4f22edHy}L(H7$83x*)Ankn1p#&eUnDcoH7yMtH_fc#S445#C zGM+eh)LQ9&p@e-pYFL4A6G9bn=i-W6Z&3p_Em5w8F^uSr@_p4 zR_(?I89pvZ3#IdvWetvctWcu-OQsY&M599N2tmI~&UeyL7k;PCWD&Xe7hbIuXkR1v zNKq%7QcpIuOMVSTZ1%F_&~mGb?VVaD=YpPg(t=N(8mh=R_4dMr4y$i7fyyn^TCT!W zTT&G(=_rn$_Q!%7IrL3zl`)l?o^&GXJ-S_?w@bgW4>CnO1TKtxK+549GD`=?DhQDx zck632Wyak+QAmagL!R0smz~*8gyZIjw0Vv1g3$|Y^i6AYkC&Ej4SZ&w2>ZuPJ=3{k zlrpk{&{KqPhduPojJ~K#srSl7b*fLRGcTZ2fUgz?&k*HVy4jE`PVB>4+Yp?tYrjH=_!@iQJ93f+R^-#?0f0@V-1QWyQonlRh~v5V%YGj@sW!qZ z2ce!yn6t?Hf+2e%`8d%AqmxkEGMlCba4;T93a8wj9mb z9(#2V);-{BHVhpSXE&0=o=03hR#-8}(%!w(Cc`O$zw9TDV50|8Tx!_sm+7rYOEc{T z1tAxIA%@{%C)n~tFCiXpP)O|3qpJ&x&Gr4Y%E6g`lIj;po)i_$E*SVQKi%A@z$C6I zf9Ij;%gZ#sx!#5e^9qDC25n7GC}%6OysgEL`)v>e(k>Od;z}B#mPPt~DP;PI=3l43 z{SXU6s&uK8}KLp?{4rYnuz_ZBJWd zADG>n4igEyxR>Y(Pr@fM@|QBlI&4qDC2mI1{}F*r9eL8EZ3T-r^I}NH`~02GW&;II{J1( zLE-r{?T%{M6AKUr56kb2syq}DqkS>o=(%sg(v*QTKc#)P6_8i^+5L1Nhp#wdKvs(Z zy-t%b#DB=qnU~@aZxBvsC}lwnhWSgu9XmyOcCb>9#XDT=tQQ&*_B$^Bx-@JYvT<1S zq7Xjw6qU!tp&{h4gVe{c-li2TD%^zFQ9jt-fCZ*Bc`;>a-iGy3m~K>_#1BuYCf}Tn zr1k4`$8BG!+xc!|uZd%dJR!h{FZ=k~9L@oP>L4z&S6DiVmQK2rG~cz_R45=>(Y1!B z3L@DAy$oZ^SEBA+5h8_)dXUT69<#A{`&GZWQ37+_3zkb*tEcdkF+*$zZa}8mS>2p} z$v+~=HJs_Zl29$`+~|T5(tvp3sZAd3f@Zg54!FQ+z3D@}<-1lg9z)X@Vo&$;zlR@m zr=;$43+hS7=rQb@>8KV9uW2-YyRxnmi3}9Z4}MrA^$~k{0_x?<0w*RJnUvtycL-~93 zMr(cckf(%a97=v-ZKPM62@SWbgfm;;?lb26rOZZJ4^P(DYWwMad#$&sl*};xejK(N z1M#JSnr*t>I{Ue=hAqANZH*88#|x5*A$F6<7L@fO1>%Az?~nNYbfSBHU&+5xNG02| z5!uhu^48-zzw~p;B^eJgK04%0Iq9@n)LV(s1z3nKf`99c*$yiqs#uaJk+&!MVDy7t zm1PY^H-*mJj^g)&Igp29m$ZHBuF<;&qK&g%dy-%#^7TqbN`8T5r}5UgHb4X#nVM`% z<;!m2D!JUxixZNmue>5)!ut;`gMuFaQ6Fq0W|I^0GL-n3qp-NxklF1}Y1YlWhX@)#b!1e)3*dyxc1ufa zjLyG8U#(W8X9BxO5fl`J)HN2=`Rwefy@2r*BYuY8NrR$%gr%u>FO@wH;h%#)%|O_A zLc$@k7NP!Pu~QVR|}CvzYHFwSE86OKXy$BUl!3#nQ7CnWp{;_ zVWQyh%&Gr|82^p!&)iRWz&^>jM#M0D=92hXVl!44Nto^U z9;W@dnr}Rza{)WttiyKL;9tW!prRQg>iXXIENZ0V=Ih-=m*~q}$bf(gW=kSdBIA|6 zHt+Ne#h`Wt%eg~{mv2z+{~!df$?sXHI@jFqB8h*2RC7A&A27Ob=5+kp4{Q?m%7lDV zGIN62N55>R{$@sQWY2`ud)rY|X6BAP zm5q7tYbW6xcm4;PZ;+^Hd%C?P6VpCATH{CCj?-n_b zfW(QjvG6`Rl<)LDbyU1OBjRq&%lK+!c}6sgR)4tNCqjt2*@kJpDMGnlcmL4zd#|8s zE(^XhewfpW=JqcU>Eishxgfo|P2dsr@uMWlJ@j=B&v<&fpnnf+p4v3P62>V_pqs6X z2X#D%5%nFh6LuTyg_Y#hS#}nmHh$(f-MhSqI`8jS)D=Y^*E??FJa7)_FXm^C(~7~3 z9oP>tY9(M`PKjFm^-3n@VUe6zl+#8aYn6J|vkRpJksl=@mu8r#j8|dWv5 zE7gd%77rmAX&6xdPSq^hz*drq75lDSa4gKu@L$D~Ick3**TyXl;1%K1Ol z(`#sTE*HjAhU&y~+^Nxpe_QsE;GvIQdXV7y(!W%K@lMTs4-3dR9T+q@|Xbx{}qITWybN%*W8X{vzwhkstNylJb{JraS~B&8CV4a;c%%O zcKV5}f~=xA162PLqUQjh=I~9@;VxuG1n=9JJL(yOw85uNqz~0KY%F5FJQ6OaX{BoC zP1F0TQ0} zv0+b3k#Fv@6e14^8a!Yy|HY6xhl+VMIax+JEsh3nUC~s;pVjVRaLCEH!rf~?jZ|at zO8;@)>KDm6=Xyq$E7%80xqAb$9N|xz4*30p7yBX}sL`WpCPI>Y%I;dUMxOj>3aSFV z*o`e%ziYrjMfv+RgDj?}GujX+iL{Sy$6I6gg1K>xne6rImFZU=d_a^m1n0c}BGNK= zQ}l=g5Opx-1HNW}kw)smaFn?|&y`vE{JPR<+2KK_Q9os5%ziSpQQ?;gd`;KZu~nD6-WuI{{%yVpXE;?XzYT-jMZLoZ=}rIs6nQPz?D>moYZ^|D ze@9=k>w_PKG)+$+nh=|{aAZU`X3A%p{6P#j`4p%^TD3j5lCJ)m)>+dx-a8;l-{4%n znGyqU@%V;r1%LZK7{8UIUtW)u=dP6zPZRs|p<*Y%#NF23UiXWoZuuw}L*cQ1PPYRc zf2rI#v$>mR_oX3>DME`;B30wwV4+WI+zYL3FM08b^vQ{`m-aey86UOO~D(4*KPNThV)89}eNTA-)`lt^1J% zQ({V^GDvBsD_Io}2v@4Ju+qt-JC(`B}AZ7ydm7wqIaCDR(p-JUmFRm`VShjsN<9-v3AhHZiAWsm?mJuss?9 zbv#plf>3xa4PPUD6(5{7gl{+Y_p=#?iEiNZjpBa2HGe^2Zru}wLa#K3vf!K=dvF_f zj~3c(UU>YhmpcX{YS&JQY)bo~AD8yo&_E3AJL|Hp@0moVLh3}LrD6jz-2 zS&WDjh0+`+HM2JV;~?&A6nG*XbM!#C;P@0T$8t=m5@JL~VYAX!DtR(>*h@tKX%(B* z&7OG_mqdKL#TSEkO#r+Z!~E!DWzu7g_03KCY(y<0_B$`P7Og}E0iFgEmasi?T@JsJ zM4);@;=_x1@{;9H+#hIOKA~?AQX!jYG_c?6Rh5>f@ndRubdLv}r|#)g69d47M#xJY z55#PP?t^|DF8_wM$&b&3;BO>+5K(Yv`2*@%b#V%Dbq(h_Gpw*+ z*te%}V~-`J#I8vDE!So3oDSmbrh5XGNY$Ye=x5ran_OeU&0WW(6NzKz zRVV>b+S*k}syQ<klv1iKn`V~^D*bOtA_^zK)^vO0)PQP&qO?=J4t6hHmscfILNmI z$X%enKf`pZmBh&GUa>2T7dbP8aB|wmlA2E)6(Ff<{R$pO(K%QQ$8Hl|8`+5yjvAeBth!jh7F5b9-L1U{*KS3gACXjVUWdDnn_v{3sDWlhSBi+npJ~{c97*OOm+z6( z!Zz*YIl5j^Fipi!t6Pw7TW(NKp{^J`jo8izn9VpCav@@XkkSoLwl`^OrxezM)uFRTe2RWryU)ri)V4bKpI-NQx217C>9yS)d00r zFQoSJ0@2mZb};cNr0g6fL^yOWJLI{{ZhAPO`6CGel_ZyeQh)*2vX2itxYgZL%>)ZL zGB#!`N1(fALJ3WscPK$OdyORWQ1zunRS3?el1CS0H*j>LXBrqyuqa^5=~&}0iHy^z zv)F!}nJh4VNhakJYJ!M%XRE_2{#N_3A31oQ!wmJ4tw${@J?lFQQpfc(m8P{yW|Xh} z8e=3I8$G??#(YYk{<( z3_T?Lz%;6lxG9~qB@tzDpZATi9UX*C?MoSX0=uaY>3eocPN6QrNKP-tm`2!<4}uI` z1_UKFt;@vQ5e}4b=r`%})4feIH}CG2IMx6Y<{1yt+Hok}RynwW-Fvr$cqUilW_K(n zeo<3XqkRCb75YyfWmG#;L7O1_I|Yx{c++(or|^_2jqu? z_mgXza}?znYZ>m}S9uJ3e=Z;l0XPc>r8MsPS5MSho!de7Xmr`t*h+?oJQ7n5YlI9{ zY(fvWZLluWin+~?wEk%(qdgo12x4G<2wzi3ye*c^Bo;#2ZU?S_2pW#f<#Zycd4507 zo_E0|+RYA}3;LQB;D%GqwtLudb_KpU;VAUqbS!0Y9a%oQ2ct`KJDfw@#S13UfBh?4 zSycr<(%a*e_%IuFflC)>xKH!*U+<8RJuFf7>|JTZpP&0gJ$a?_MgkR=E0K|bJ~89J zI(b_CxI*%&%yb*gq>CW>!Rudu~ub>EcTX;yRS>7&D3|7woBV@L~yoaJx4O7t(AGG>l* z)x9Q3VmN;&{qBi_x7NoN^6OkF8ks+Li8K%)3L{1_1Ai#p(GgKbxfTVZju6_b6Zfx} z*DJ_;1X9Cv1%x5M?10Y;I5L?KTQ#05p=##U2&*=jXE#wt0}Y4h0!EGwny4-QD|U_X z4zL#LarQ#D`+&k|`USG?+r*8yc4C|n?)MyGJ#j>VlaZq;zw1c<6Q=(jAqJJ`dAs&! z(pa2;FPC;26X9gM=5o_{cWK_#G*Y>}&o;IAXJV}}6Y`OfTEpaGh{wwqG`R^Q_Yj25pw)l+aj0V!$wR;zWLml^cg8b|8Kl&0U*xzUz7@m0(gk!c}4Z@6tw;fv-2kN-f@I_Ca?* z5!L;ClZ2ms#|^6P0@vK)F0p&bNa<@CMeX@V&oKddsr+=bn^lzmo{h|>)CQq|XUoq@ z8)hT>u~4|@b4j#|e_LryP4wq?0~dJBIB$Zt{kx-*(}$h|m>TKow~>r+>2qZ>$s32s zqKT`J(F7B<^Hf!;R~@=R#JZ=cW~pW-fbFazU{*g04&Da(WR|P|#5&d;nH2y=k#jc4 zMVp-apfyE%qDBDH3&pN-o6crae0^s?Fb3dzrg%Xi;zp$3-X#AF45RhqNkZ#9Fv4*h z-XCXv*Zqk)T_(4FPqRSv>xf2O+)>xdG7QsI`<H z=G_K0Rb-J-PMC-rIlSNJ|DJ7e6C#ziKH{`VBdIx{AR?DpK0Wk+Kj7k3!|UJSOoB9> zSe^l$2hymV_Z#e7TI?%+;_6+TQD`MCSaCoEJ5ks&N!I(zpi%ifF^L+8F{!EbphkM=+mR(_uknkAor<#?$CG4yr}7K zjA}kz3l;x(XjYZsVcEf3Or*<<^lnu^00HIA%Kx!6`VjZ7y(#$o-g<@V3jtrOH1yiyLYI=_Xn3#b3qrX)5+w+C zkNU#pr6noApW06HiO3H?F)=;g4nobs5a|ZnrH^V-Cuw5z1}5^zH%|T!IxlTIIX;Wrg(V zuDg85y%D2BW8AmO%l|M9;o`I{YeX-+$k(cMg7@(^=)cO&gLB*i&*X22=C?#D{HZ44 zFSOlMiY&MW88Cp4QvOUz;+{psw9KM~=+kI^w~}5K(KLkoOPs+91=k1ir8Nu*!`@i6 zA%GB>MVIl>qesvwn!Fz(_ZS|u23LH#j~sHQ^^MzNOWsKon3R^KJLZ5`%4Re3M8njz zMU@Y9|0RNFTE)$ntB|=D*N&^N#*43ZYdm7ZUW)($%a6C5D?S(tY7 zL?VjqChe$K`HySMW2DQ-hyX=I_94Fa&TJBkB70RC$tq+HWAw`W+yj}cb2$`8-EFiY zR)iR#5A*p^me1)5XZ2%Dn0KQPnIU~?gsM4AM^iMUpO<5iog=+lVcgB}YrW7Z8AVxV z2ZPx-=pJ?h&KN2Z9d1O_lqVtIz}Y7jqeW?|)Yk=7RZ#(4HRJw@$>FHr`ImUdqd)h` zvHN@$w8;lwUW#yYLO^31U{I&rOlIF>@~NA+y0C+aSX5hbU#1V0ZRIYji(mwM<}1`i z#A0c2unBdT;`Vw|{?P8>`_+w8`}Uf5Otqn>JjGTc2Q;o?$86?i=bA8oLY*33cdbEu zCTvE!Hb*Wc>kzY^u<*$f41??}zF3x)mxJ`1+x%14%J^~8FVGx^?>^BPQy2e)Lr%xQ z*B96!z+Sd#4H#}~+rj%bm-*IyyGQEIBtDA;9K2*GcO>!HnQk{kxOUNL&||fL`@lR* zlb(L4d;$HBcI>*DXyjfi{U#Z`_@ZWe@KaymZ$2t6`@N}ZrNWa`^yN`rlCs8056Z^F zM>r%^tBhp#r;5M;VR33j34t<@m5JhE{t=a5!=qxy1&#=+f|+09klgE$a-tN?O&>CB zhQV(`Q1vUy!GQc_(^WG#6F+&Df$j+pN65CG0u*9RSu|D-z#u8l?+agu7O34Sf_5ov zpv~i%Ffy2K&W^Uxt{@R~$c$XFh$x%d8GI78> za*7_W#79V4M}M~5kv#*tSmbS-+6&yCLQB2OK6~<5>ba`BB9K$xNkFor7Q870wzc^C zBd_s;bIkgNh`rT-p8k6(b$;Q}0;P^D`}08~Sk-j{!QNSQA55_{hbQsz4@qha!WKK9 zkEg*SFTN!QpSWpeT1KD>wjJ@3cX8{5qnd^oMms%A zm;)32hc@Igl=%>Q2oH_)dZt+=w%|2(B=QjhB1A8~or9u~RJSQr+cgwV4v2LhH;J5ATo0QT!3xQtrU_Yh!Fw;q-5hHHMuEA*Uq zUHsRXb2DE}lu-n7k;%pXl*BmukLy3|q1FQ`sH`+Vx5+?poE;GPN&6&oo55ak)1nPs zKIGLF$=UTw2mL`5b0rI60F;dt_WQ}{9S7KU4~2*xzn@L-k(kgp2ZV`FU};cbf+uH(m!{C zo!jS;+je_xTn_BZft{b;M~gO)QS$YBXAeBUwG|F|X$n2Ede(W1$Mfxq!>>OtRb|W~ zf39jCp$UYi3pZ7|3aPa8JjT>=+A!y*s5n=F-}(gDqzT0;1Cu9W2?0+iehXg`{}_pA z!c<&mjLeZdaGOjCJ!T8=qpOk4pA*D*g(Yd=mhgd z@VM2vb(>SNH?2w!!($J1_w-()8;>yF$>{5nr06|pB$^r3#JGgK^}YlYe`EJ15lCn9 zn=>YqH(NHWOtUV`i4R>D(mu@~J?&O%{_pX)-n0WV9 zvU5as`)eA%8+v_>ms(OSKb7wPz+hDl^;U&UQ5Y}n!J!XQN(sXW(yz{LYsHuOeZV>u zmRdP}k>Wej{<#x*3Y>fH1lQ6*o4`%+@b~oNug@rA44Coo`Zu5JTKd#tAdneV$HDBa zPj6mNSvT!~Mufe`_+viMjeidH=P^c45nrHz07v6D*C%hyIa1#Ivtf-<+cryuNX1Is zRQ;_dg_0EsM~J5kE*CV&Jry8IA6#)Xgv(wZcznN-E^Uhr61BKEGonwtEjTo~ohacY z^-+`f_(H)MszTx7c6QZT$XcAhi)_4>L-C4kNpf&nCAit$X^Y~-klqSe=ej9gX|K2#2E<@%T#!;*n4(Ex*^haykZ~Ei5^MS&j5a+mxl-6byni7FDpjdY@Zh}!l~L0l)lO+~AXe(zFbHA{M=ab)C1x)Vl@ z?sp^o8sDUd&-e1j&TAkIBj37FVinKkA>7|}QOb0CC|rhZQj}A72qxogHy`|t5rBCd z<|mvxO#Zis`-ewETK*qR-{DBr|NnokU3O&aQVAi1thgvCnW14_Gn5&UaL+~dtVGE! zB@&q-?zQ(y$h=l45WQ*i#)jADGGxob!U;%O z$qGucr>3&+-;4K@-o5ysXy4x43G%S7WyE!{HjSVCB&dEZ2wngUg00?`@+nE=(PSE@~$dC3q8Z+_eK%6(E5l zUZat=g@s#7J_^ARK-m=+QgraowiX+{JopdukV`XoLu;YIY7D*GW=?pplxu1{y@R`{ ztuNs1@B22Sx1CZtAW&eu?4v86XiVRMlMwJPGLt%xSo(?3Xn)(jSJ*I@elI4lxzeAAd~)*+{7Z^H>wvEf1b-cSOn zdjiQ4!ZFx4i1Ey>no3>?DC6!A+8+m{pyBNf-u2gg3WEPp32+(T}TtJI5F-pg>xZ-P`Xbn;aYIg`+WX zku=KepAAPV;I0i{COED=Xh+E?QAj89)=C?bSR}&7VBo#UjU<-c;lz-pYmZtMAQewsu>X$!_taoOaNTL)p6}Bk zgJ6*GR)6id>Q`=y~1jo)!Gr1zV;9WV?KC5 zDN*(uj}D3GGJp*736iwlqenv)%=Uo3FJ(q6Cgs$F3~GuV8Cmf;rvQ|4rTD1<#n+d9 zm7P?1{qX*>?)z_$RwBs+NVQ$8A-I(~ADm2_>7UKa$!Ry!?9M$PWYNBa2Ms)aVK zJvIz_WmaFJyvK)b(nkzYDK}Vxf@2KmnF%(`v5M-iU~S@gHZLc>d@z%$2%ioMGTOD` zf5F7^D0fiW8PR?>Rf(&ISxmU1=YW2;yE0$c%K%Km)~xFYg861!9r@MgVN0D)@4Vj# z>?Q7Q^_z7ddmw){RRiPrl^k><@c4jQ?6t`=6zj@O(Qz2*BU`j$@bBj@b2c*Gd{6}M zohDLavX6p9m@(}Fr#76HRbR-`&}@@}C3=flDo4(7dCJXf zq@-x|30;*x;58*W#)iwyluB!|{?3Gkn!{t7Vh_0C_)Y*(` z((55cpZdmtrqh5B!WJC2l8LvaE{eAJzy?}Qm>qnQG`y8Ie zDX^*4&~qKqO(~49dWjRG%K3k#i?06uSf8=qwX)MR440h7uVnZ&ZL+PeYqK4^TxCX+ z|6DFcmL!L$o=b)zJ9?T}O3D}2Eu#E*^l7SSXY(Cf{*(?El0oKgcq+N{sBHoB7M8nK ze}(_Z-00t*3#(qzqQ@oxplIi#KU~Y%!6H5)oq{W}Usf7x&_y~rw_gG-!^qHw8Wx!A zx9(Fy#J|&>lK>zFyAK&n7opA~>goZ^42BMHd9%SxFsYI*6cgIM;G9TV@6X1Z{C9RoBQ{nX~BZR(l>+B;rFin*}Sz* z(HNy%1d}?{NUd)J<=Ps#sLz*CHwK=!oT2FHA)he#4m8^?BU4UNvz#|Ssau{Z&AVM< zoVJDYnLr6jP#boK_n9_nPNHq1wgLe>{|linZuuv&y0u2_#&57vx6~T_637Z87l@2x zH?_m(ha&^;q128E0FwBHiZ0*QO7lb%XW)GW`r9RST$?FzEn-t=0 zTnco>Az^W^r{#-2h*$fpEO7(NQ{%GA_6ORmFVF3{a>TDPztAMpW5rh#pkoK@#l+$e z=xWF2`h~C{WqbFz9?3F>%|XjU`fWNytJ0b|1Hf`d%(&ngMo;ngI(+JG)M;nPRnNNo zUBb2G`b@7~zW?)8vR2Mu?j`J$TR<=J?zL=_+mACv-`a-19uEf7Q}iPT;3Xe_gK>9rT{ukSJA2k566(@ zpL`DKXnNG|a275qU>*+amj7Wc?R$6b5^Bn3z>Z_{(NgknoktA5*2$z+vBm!6HhKpd z%@eB0Fi6dpscm(^03AVCkMdLYK@(O3&9}MluOhs!A{}1Eegz$!`d$}Q(EKF57#OmA zF@NWlS9~_16L&yl9LAV0U{o;yCE)~5Tp}LaZf{F*OoUJ>3yQ|OqGU<+UdV8)Oe@VT za!0?txLh1Z+;m}FaXG%m?0cV>0V7lPIA7AkbQ4ercaPoS`SR}ecA11v{ zpZ$AU3J?p^5p#Zr@lHGq72 z2W!2~qR~&98Bg>bz^QAkzpzX!;?Q&okNIDOTmjNSL~$;CCT{Roa>B9^4{d)6E&tkW zyEP=_$;@bp7JTv7rBfrym@@E11zB%NvU^!l|G%1q(R-gZ-SgWS;>goj4>ad551~yG ze>mC)7YqPf)spZ|E7Zvgf2b7k2s&Vao=zv!OotDm?WSWp1EfZ|#X?a~Xxi2tDLtrS zQVeiB1;lQK4kLiLi)6b~!cp>@HW}uG-mH-MEYL2&?+RSyptbv)(7Ni*$6a?@0nbY$ z<&i88CxS}D;lAVwG(UA`+ehW?w?1{$iyv`aJDi#Qrt!CbI(&J*=+h8hjAf{k{!KfnO1zh!13EdM^S zUaS@-@*dDZm{5WG`y}-hdk~@Pdlkuobdi+W9b}l*6ysW6Uh(coZlcHbUgZ$#6W}F0 z4*Cnx5x%tlqUD{bWc<@~e0z<5c`H5i6sMQZ;_9ZigJ`GU+{I}4&;757s|h?uIq15TGc_|+`pcA3pB$21!_Ocsa+E3 zWLohoDnO=UR5&#d)KkkXPhrIR6Ze*ehZ&6o1#A~u&RvpqI&#W=bo}MpysU^FDo*(7 zL(}}>Sf(WRyk`+7j%Tmtm4eo5NSxdQySYouC5pb9c6s3nR?E=o@Sa=zY?c2@5^66& zb<%||4VyVVfKK`ZszX3+aduVvboN%Pe^RV@Nb5lA)f0i6JYPM7eI^E1%?vwF0p;+T z`3ol3`21hHE7{l8oIWK^h0TqxEN?-}%lI?c@MMQ}x0OHUXD@z#@!?S58L>T>{9C-* zK$kvBWclwC+!f~PG6cs?oA1>X8F)zH8s&l?eK-)_-A|>1apZkH#KE|?(j)0Kh8y#M zV@wQ8?8o1807VzUs0D1E-oSBvxY6!ew5-9QVh6zCZn@j`Rccpqc;ac@II1Z265T0# z5wM`GdHX|>s-t5Ftiie=aT0R&$x2xiBdqd2D`P@yl!k9>W1?6Y$){I*QGcbH@J~3v z$5NWjZv(Ps&tBX_ffB^XRD)w%O5GhK_W0nlRHeCEMr;Fb0dDC7j!&$N8Sn+1g5SZI z#Ro_ooX>8S(tAWKc-R1|V}~VIl+7Zv_}>L6hq_W@se%5i8s%XHJ*jtjO{G| zm%?_pMiPhX$J<(1VH#pl--GYX8z zEXU*)v446$n&-dLiNk#GfZG319#v<1AyV{aG^IQYvYQtl^dWsRAM;e!1?c%tGljl$ z`FS5KE<>E!k)ZqT2Tf|D930TB3JJmYP}?)6jq&^)-v=3-Jt1|3H)^>e$qov79LL+3 zfDemjO3kEBFvIM^M?|uRV0wP;!#dvwi@P${d%q=3o)cXu zj_iZyz>FFe|{QF5P5UBF+`sj#aOoFPiS=9lfi!j%Q>9 zMK*4ng!EM%1UkCtU?L(ML30nbF|49I%p%82XVYPOnaAIs=N(Bd*0c~|x!}$}^rRjJ zKma{D0HU@!B&_Vt#!V;~R=b|TLQlWIGE=|sR?fFeXx$2m{b1SBH15Ru)m;;1I--W> zXGsFN^VrVo-R7!mXs4N~5&GG$1P;WAJYnx)CM2;8_4jG0;{ULw3jOnOKOEBz8`4>8 z|70s$yRvp94(C{C<;N<5`zwDJXJ5r#IU^K%4V&Zadn|G|GgiS%f)shtvl7!%FLdS2 zYqqBp;k6{{snx2dGj?pAW0Km=U)GpDG(l%VyM2F@4_oJUd6kyVjW%cMw+hyZ(i@J0 zqP9<+m%LuHp7WK2)))@q%z!QV#V!pUh3=OCQ};o_OM&9y$MT5q+v0Y=1^)=PW66D;i`H)8#;D=qU^FAQ?9eZ{TFZFik^*z zuACux;jHM zn6_rfawzUA_n!l>mw)onZd^YhwkoNn5cd)wb|27hpitR=o8}1mA#K03XK&+xxj~im zN*ZkGI9m4Qp!7glQ!TX-CN6&!lXWy6ID>e@^NKUtk)cPs(m@~^WnO^8lvRf!VrfO6 zOM}OHAH99}0i>Zgfws8Nylf8_?g{d8)VbA%(#kfjbnm)wnFfoYXOy;m(%2=4BuDIL z+5z0t-~wmjj&Q{QI(YT@hA6h6jUMk$bO44ExG{M2u;TojBhl#yY9c6Cb#=P z%Sd2L`sLQDps~p#k&O;Iol(6tI5|}Ic9k$bA-RBf(3e}s=UMwJ1qPT`$TMKF`d1PS zAXryc3C9CuN&b@{^CWzHv#p9buK$DA`@O;e?vL@aY%lgx*6_^M@5miehl{BtF!WK3%5!(EZ3v=dZwm;dY-^ZUWipbLm$%{!LbNZHQ8FLTa4T27jE)@out+iLr75mAZ% z-p0rK0SDy%tQ=kT>gVw^drvQ~qj1vg8`mT%9219lhvXy!Hfe(zQ@O5j zpwKWL@!?N|rUz&6y%8}KzKRgVJz1cJGA2A19Ab{4y6otLvX)g5|!rsa<> zy1V_pE@X+M&85n7&naW^n@TM*O_P~`XWrqMw!u-7Aoq2?dniT4n7wf^_;2RyYAIw5E<@A3 z5P(gqM&WWY`Q4dtBhbMTBB)QFTiMWoEwWvBn2q%v8*oX@TjihHGN!ZnH#}lMD?HbH z1^;-{yUc{ec6?)%z~4xDB!e!p?TVLWr(|WmwBBQV-8=VFWKicp#Bj>$r-&=30f!uw zhTF&4A`}{w1L&i@v7SjI^wzmSQxO}#%0;JWSuXwS`}Y7eRa$hCeSa4oWf>woG9jXdmBHN3 z(%M-KGgh5985S8w7KVz1$vT~hu)CqXg*zgbxfoKlfEHSKp0vZy8l^vYeDu$2Td+_p z^0P;#{8l^amSLC$f*Wv1Jj$3q9;4e;T#Ngp?y%_)@uH6HbH};I({SmU*_&x-%|FSQ zErz^Z-pwylL5$e)n+GJv1ETD4{z-||y)x-vnwDdTWE7F&mYXH|V^+kdi|4)DSgx(4 z{X+mvbo<-%3y0rZ|IIx2tu(%UY%tQK(Sn%uhFKil+o&nBT-vG3#BS}foa=4;SU5v3 zzl-VLGfnc{OL0cTf;u^k8*y$U%G)ih@(zK4C!wb56%ZvJ%Bl5;#qyn98%y}2!!`phYx<}p_)xcLSEb)DMk@;YV_g9=R0KEFtP`gj}BCG zl;^b7ULOwX?e!Whd_y>$NEMWpD*VV#19aS^S7nXY>4&_s?1ku?98V{p0I?MAp_jay z8xum;OjxbIl3sTyMD5e<1qK?vlK3x+8qP(EA9iO%J!D>SS?sIR*U&EIK-+OfFk#g5}-Cz-@vky!KLf!;sxM~0jhIupO}+&i)I z3oRy;1s}0H3Xpdt942^z5N`kL>!#YY#+2s9Rz?a_)Io!s?h%{*sbJ+(>v*lq>Q#QD ztUt&nMp_=ORAzT$7Gao)(oMpInWUO$b zf*d!$VT7UY^ieszg(_29%()@v{Ko2oGz8!H7gfAjYM;%J?4iOVb^Ow~ zZZ+&V(@pL?kJ)3bJIooR**k(A0s+O$PZwP0;olOw$SDz|JyIx~vF;;MEJ53RGq=6_ zB^L!C-@HeD+u8T?9N2n-5^;l@_Fl@B8n68iwR$r|2F~eExC2!0&e!S zsz#6GK9TkxpCllJJk)6J&)-bY%NKwf9?E>k+>*k0zm73V)3%Iw7=pk~(NH0G8(v9& zSz9or?SXTenB5IXlH?p1Tnp%AdSbesWNZqCUQiKdII?Y$J5)K*wccrUJkfBA9?SU$ z@e^g;U1Z76JEbTfp zk$s&^4`^M}O&N|8W)#r>lD>AH$XJZSXdc<@x@Y-ZjR#fgeGbnm5+5obm=4)@{Psq= zQX1zjFb^-LyhTW3M|ISi^<1INf`G|c>i&G=Wk~u|+2>Fd@__TdB^H^ZW~7?xV;3HN z-&r5!eCoWLd8ctsclY;Wu6$zF$}xo-&Eb$#GNKupDc`JHsg`Lz7d5Ux)0A-{nU%2|WHhD5tV)p~^iKd@z}Oh)Lt;4i^!=UzErvxYUud8g-hLfP4e zD&i^J0tyZoXTz-FXq~i)-1FPHq2~hAa8ecI8xB8tX#9}wUgLGjZWKf>Ne@60d+boE z1JRD`GW`P>AE-jBM%tly9-6!9tIczzs z$$?+@IGFMNHp4$$Vm+MK*RJ-;5`TjJ&j*c>rt4vBWV0as^KP_V%u9nsW}&YO;Lidi z)|RQnGwJ6xqNWY;z|Qd?!!e@YRw=ytV485_6W!}ZZPqZQBh&8}NoywGa5yPp!#zCx zIj=OF*mBsP9I2z?#ERtk*@e(~sI+m{Ui%qDbBM@?hDM6zbn<_JQ0?xub$!xKLaW*| zC_nd}fu&`jgJ0er3pIoPJ3`d~L{eHkJb|`$+O~5)D?(TQd1m~@JWqBGCJ?m+b?V3>rHpd zY;d1+HrGreLSk5JT6E+lpDQk0h?xrLJWX`H_UF(vF#!J-)j^ifO{C*ZR02|~+7O9< z3HW5cs?i6P-uZil%{IBb6VK0LHM}pX;BV3yEC9s7N1`!?F%Fg*gW=xOzt~tpxmnau zp>jifd33Pj?MqP6&*{M*l^!44#M3&^)W<*itLdj}rO?RSwG(oU7ylWu+KR%X`@Pw4Ks+Zz;mH(xt{<9dX2&27KMT*N z*yo0XIgoR>4k}hNLXMzx)dpCopf+AVL!?k1STVoOFuwbf=k@gO@`o=iinZdKz*}dt zO{iM%pS0JQv$25A4|8;@1GSrK5c)Uw$9!lPRL*5uEQ0NlslIWD>qa|LnCGrX%r3i! z=Wb)FC;c0J#dIwB&x1ge)6T!TgS+Bx{Tn~L9$J=;VTDEsMv^mvC-})}w-8YG?h;Em z@k$Al(kDBIyhES;Y7)2U#;dkcE@V9TJLfO(4S5W!!ZFUCzDF-`QyNr7Rc*ZkCSAz| zqPlyen98GqB;8n=)H?+CdTeB)kB(m;n}QMjL7?!#@=`3SS)mQXB4BPfKd^T%J*OF3 z%rgU6a>TdcFb}i);IO-@@(|{OSc>tljEvLieknV`!BE(r(TwDslC1rD_`|8Vv&##n zGXw|F#~Gw}d7k}?VkMH^A%G?C?tOa?x<7(q-B3ts8dpBj>=VxeLk|Ox zw=6Lrm&Bpu+Z!UkYmpUX09(9)|8cDe!lGov?Jxs*WEfOD)Hw#W|9mkv3_iG*610;+ z4*+yt0Mtl)7eJ6@8P@s4WW1P@TIeNSvUYmSlzbw1K97-QV1Niq`)sw;VuY7P+?7<%U7Ku%QpyYYOH!s2=lWje3?Y&u$E^eX0HniBL^1CQ{=SB%%cP&i-cm zJ_s9@SIS?=tz$FkhxMCt89M1}51|kpjBzjO4<()0Iv)za(6WZ)RuPv<1d(k&5*||T}1a)fIrt~enSX*}T+t_aH!Wmd0LeZO5a$rgGx!-1lJYlx?J7I#^h)hzehga%c!K( ze_>7}FJaN&*?TLe`g?`!-@W;F3{_PMse}COt$Ch1YbySZ z6z?B1$dtTxI&W%eb}aw|Rj=0IYUIrNWhTaB$5-TIxz#8BhXt6+Y7G24O@ zbLA~E+xxE$bhh*z?S0ZtZ=&|V4XEFEP^Q%3aF5S+xkcF@?L~Uy-w_oIzxk>TUi&M< z^rv>k*7IMKV;NNDn&GF3SR+t##0)+Y z;TLiI#RtjfNGJn##4_BWZ>Y@Fg8nqTM`Cjk!P!e_luW zm~R6(Kp{~cXc)8a-zsdNcHZ(HZBOB^If`_#7uO0Q`OmQ~8%IDUYL{8ffBEypZi9P&b#J1Ek9Gt$u6Yo~u;ryu8yK&5!h{0Bz)nKbAK)A_N(*F2Dtmqr%a-oCJYh=kNgf_)%m0ojRS1g# z^C%kCnw_%52G@`_2J|ttT-OVcRy7WN0%DfOW#62U+ID;SW+SD=R_VD-KP(0y6NV8q z)CS>B7@0X?!6w10bpZA{V7KWk+k-9s`!CutiT@t3#=o*$pp*ev)&~cMoNd2*Uaci} zRUjhmRRhXGv)#+HmI@mo9gB?Ke0v`tEAnsEJBHa1C%}bmfGUT2bR^k0lbVC%BQ4Zi zo;1BgeaYCQK;&6V7!JttcZY9Hp1lr_OjGLLBXH6B9EbL(^t|_+#@F)}X)M~ABz;Iv zX`Z1?!ac{m;J{0zkFqh{MQ}vylS*W10X!?Kh}~krL4F1X-+4wrwWBNDr=L@ZufCW z`MBv#9$#%;CyjWCGw@Vh>oh6z{G+dF z-)uLTgWPJ|>O-E^7neLL3Vv8zQc_=1yw*TNM7*9i8Z_$nt^G*F?|vU7eX1Lv$3G7P zro+gU6MdzdBuNS>h48Yh;R+RH6}A7e+goVF8_&PuFBs8+x!f|4GvWX0;Bc_t}j@YO@GsF`eBSv8*8p#4UnDT-0qxn4u z$#*{Q9?aupJk7*b=*?yn4@WvmrmljulNcv%oG`KRC0Rn8fp=8BN8ZrNUg|h+?;Cc9 zDgKiIFj#9=OGS?#&%P!JL+?N(5e9^}HU|mX9X5Y5Qnop0yG?dhR<9AR^{Tdh8p31j zlt)SL3AcIZUhiKAt*Kx=0-o$k<## z?lS}XaLc>7^2jYW8`?69W>4AfSCd2L?kE5#P{(j7Xl9Ro=_w1yDh;Z6PW88Y)ruL1 zrU8L(TsqawbEefi1Sw3t@m@+rE%J6@W8^y7b1K>+bI5X|V6yQnZ%$6K_%rO-7+>E6 zTX8YA#?M~m4*kcKhF#$J0y}<|4+LG2o7)-V=iz5GVJ;1rP~0Id(J_~HW{TXHi!gdI zgy$sQEU|1#2f?Wl6&cfBOihV8>$xs(#exU<$lD~>Rc15yQH&>v5_}mB!~Om_gz+1H{~~R(p7&! z`+M>i$(5y`lALCbJcsZl*%0b6U5^~*<=uLnSA2BO&WU9@Ip*aK4f5wJ6_F94JK_iU z_3`uMo3mc9*(g9Aj%I~fO0ZF@w*M**BX-Wxc1bWRtKlJG?>SL6CRWZ@ufX1l>F#}9 zaU(0MUt_^T)id9O$@{LX-(UH3k#~N}4@RMAYZdYGSDL0*II&tVtj(WVuPHlVNSVD8 z&vMLlgtxZ$!xa@e^@}v!U zFt~(r>$w$}_stPxBfphfm%FvE)7D4p8;{bEc70Z)@K+~650YCg)Ma67#r9>EO{xg78+{zGgjNLQT@AT2 zxDzME-yTTo2pMCMo6cPk1Jt6o$OYQ+zn zyRz@or^l_Vwr7(FVxEIyjl1$^_>Wua2Z9|7kAYn<)Qw{s-wQU8EIM0Q?B^(}BYODS zG1}OTdeBW(f*f*0uMOFUH;RWArJg$w|MDnR;-T2TiNk8Pbqk~Adu)vW~c`Jh(;Kl4$( z`vIgtzd6`Us{i1jrm5+lLfE}c<3w;PxcnuDWZ2LzjgJP1LNahq1ERZINBS^?x|>=Rv-)C4xEZFc0jzns$#Fb(`jp!~uzV;4T;>0>5;hYwF(kY~>=N8l^A zeBSW!YT=48HNx9>ZvEIoR`NmE_!8ZY+wb$n)F@!i$wbGR73Nn= z4u_YqDr^5lZ$Bm`1h!JS8d_(5B78=l^)s(N4iVwK9_0XC$vDkn%muz;3ioCM2{Rkx zC^Yo*NIaze!xir1Y$JDt(i%s>qJg3MqF~hh{=6$GfqgR?O^>!t5(1}c_?yb|;;0qH z_}?>A^3Tv8Jf2{VwCzy~uBumihvb>Xt#@w-1IJk@euRQ$_0>VmA^8^!^Oy(u2*U)- z9q1s$-cqQZYafdWHu3U$ne_Cpl3%E8g;ayz*>|;g+zFJbEMs;48${0EefU}oB%%*{;-vd(m-lAy&o zUi(GfK0=Zl^6{vb5L~O!g_W0%r~5Z2#AJ^l5?faMwWX*3){8i~L!yx}BjhWH79ZIR zE6T3O9>2`qO}`dU--aCS4!w=lVg%s+pX>DB*c!x|I!2r$@{ca(TD}MmzcER?GeKRy z#3Z0V<`?tS#wH1y`kRTlMl|QT)9-_g2m(%b9vN-(nx8~%y_?S1s+@QJJnyIng_AX5 zbBVT_VsTPs#NlW(vTPx8gLA+N9~rSQy%&Z%1qK`Cv(a{bzon@Do7hXx+D7j-W^o?y z=-IMuRC==EM4cX6W52q|@I^!LBoquqCn1w4AKS^5K^hTFe!F6s-^IabHqCoDa?FObhE)> zDa+G}Nd!S$i6?!-a#KQlnLv?uhIsaSqaT!@nQBL8Or=`bM>nyWK05@?S2q4#S*k-9 zrt0Uw#Gt1b0x30wg2bUjanr;P5DI~CUS?=ki|Wf~(n08milRz+y>HqZkjPru-bJ>Koz3Pu zR~YuSsCNsgts6-Pxs9=ZKK^Sr4~3uMb9j-~No!=}k6XUPOr;x5l)g~4*Co>-)>H2` zNwtm*tSl8oJm&x_d~s&Q$C)Z|nI@O&&OOIZ0EZjz({9R7Y{Gvob2@BFA@t3+^j4zk z*0^hYrv_#+R^Z@c{O8w0DNjNST3f($Iod-j5+%Z#Ci8Pa-c#|;hHA>0^@(HV@x`=Y zh$wGsum;sLRble+B3CBb-OlnT>fCh09d{7{(@E~ePtAWS8F}R3T4;=rXgQYD6Sk- z!`@FjWvAhbNS1PA%>%5CbjcRtmmmPNf`%^%Mk>C3R{gU5=s1W*!n71;=h^9nzZ|jvHv~IT|;} zu~vC%1Hc6aE`SM~bg;@@#g`tG*qOT0*su!+orTc$Gf0A9MLEF5OPvX$DNGYog^i)U za-G$q+J%{uYU=9Y>Pq>O{E@WpA;g&>5s7!f%&VnhB@{<5isN@1k?+}no15E{qRVYa zDNr$}lrAUL6Zw5(uhHW5;!^44!>!;WEGe)U}x#t|rb#BudA#k)C_Z(*%ybnyw(wtLz z8pPo5!Z&7^`Mw<|zy2DdY)ySt^TMtgcQfzAJW^m6v7`*SVZmeq{e>2U47ut8%^YA+1WL{k+hsaQLo{pkCCbL_HLKe1%4BK6m5_RDS#-$8J?ounpx zX?{$ji|S|I;yk`ogCPFExvgW1NdIj@;^ro ziKr-$a=(=My8k{DwOc4XIY7JIOIH#b%F-SES(uMll|Z-aquV(j+aJ<3fj*#FFNTf0 z6DZDLe+i;GeFH9|yo3|1z}=s1ht0o^QBF?|hkr_V5`YxZoyT1)y{?+1dr@$;HXg`Z zaGL*c_PYTA)}4a4DU`JW3uG<7_9F(*gR3f6LpcU3HgbEK;alqPye8aMFB`eXgGyUty z;9PK2l1S>811^K#iS#FSE&zL|ij`<6pw4@6o;Tmt2yg0(O)FpcnOe{j1vKY1Yw*NkwJ?m8N!cz9WPsCWdZUQKV zo29BnQjdFVb6WTw5^4|WC#*;kw<(`46hBVO9b!oHPWCsxZ6?%RR!A{-TlEo|&n1yd z;&x%hNkM?6%pEwgy)ipPA9>AhHqhR42G=r1DedD2lBp})$(Z0o%HYNo+MhHqTv9-c z07Z+!)IXscR4Off9^$KzijVNg4rK+Esb^Tk9R}`~%k_$$cg;L^|D~V_PBV!XRw$SB zj&$1k7v9If=Sco$VXu|L;b6n0{MWyxft8z7s;msv)nkodBY8;wCmJl)9$+rwV*-m` zeUqaFW*pF=PiF};<+B8(GMmCAlO#NL2@s^! zKNr*rUZDqeSz%e6W|yf4A1D97_rvXE*=bhVJwAfl_qUR+{a^$H_kP=>1oz~o9(_2R z;*aZAV4n`5p4z%RZ2^pp4rS9!>$P#lVl`di>31SUQ>Ryvm8!%Mo)M$Sy?%ndKt~WU8+}APK(n^{tKXIqI@sbgLj;`Cn73Wv!kv%KSdq$s&GhgDCy*6*6A)r_Y0#TV&{F@FFS6oGh2gU)VJ*jxXuNJ!+(}uOq1MiTK(QMa6LK5uPV@TuQ2V$7)8!j3GuA- zq0N(t!vkw^@J$sTaNE4;rM+e5> zxicv{HUt4om6ToKiE#M!+J2n8$Ki@LxMO_@4$S3Yl_Aep%2-Vv-baE4oy^;!Bh6xQ8o`*&H%ifrW7Nq5K-|$vtF+1D=^#dso@|$ABv=ZrS=!;pM1#q|Yat+D!4ijz;c1Y*9;gFiBTa}oLrGkkM)EOVjgCy;MCAj{FLa#o=d zPq8e=Tu8_v-@)ddMQ}^W4>}#8QL1Y`@`sSn2f~N1bY7FJz(G}LKJr8jS}tqNBFOL* z@$1>WUP6h}9~7-FjHLJ|G^dkr^mWqILxFV8q^wkJVDCN=IW8bjK3g$F3FW`NwjiHf zK1LJe3b?#Z$LKw0>a00*z9jiZ(wSc90}l+QUeCT+SWj}wS#ei_lI$&c245TOWhj(n znN6piH{OGU?etir^xnX+ZrPl3yIJP)_WYm99;JuD#FFw+V;Kd7mm15QS#1O`O38Np z2hr5F1b|U=hCim9v8q?7D*F-b!7+_6hizRbu3fxMpu``id_J(S?#p(HP83MVsuQ3( zlRYJ37|qq&lw+{+6H3DI#cHD0PgYVUyx0_Cc_G)C`(uvtFpHDx^K_8gRq0Q9y|ju4 zD6=7C`GOQCFf9<~F!O*`%m*SG%dRw(HH*BCvzg!tgr#{RX%}5_Q*i0Mn=2%WV>IWV zgh+Zy5l5oEBA8s*!K40o>pBwIC<}E zjHeWj{JzS2|4|T~Sf?}Dup$X}$usuaPC|Dj^hyl>y^@~E;v?W~6_=Rz?Du;~nuZ0+ zVI9Gav}gDf8uK91{sy5p%4!#~z4l*(ckuKJxD_zt#XcRHw+hIkHtZAO{kV%)J{}q< z5}R3;C!Rue?*Lfd=}}bSyS(kCUeu~2}#eR@=O7JTDQkyJjEX5D2R0u+oN9}`(M2y{G1n1JR+A?}OS z%MI%JU~*L3;79!!6!sGt@nlhVl?2^{_^KPbFadJh?d|n!PHU-DxhszfeaSxs-c^dD zo@`x`?X71y5)9-?)3s*>I#`(!`PtTOr*4qd7dF?BAE0A9Md?2STrb_hmlE%s+e41! zRSo!D=nd%Z)dTz1ssh~<&)qm&(LodhxL9wD$X)jjm!e?C>iglLMCX3G@nGIy1Xb-mm8o%iirTss+F0SgkPB}~u`pub za-J#bdO)M99vfL7-zz(N%2=$ouhQIwLA(waKG`2mT4 zVqtKRB*m)FaqxCJd+rwP77AIu^a>2QU*>rPT?pp8M<0p*oSPI%Jn#G9@vG8KDj1RY zNsJpxJXd&Zpfl^>Z`(ZX>iL!Sm1eJPd2aEY28$pYYNefNe5rpeGzxG;#c`E7uVlR7 z3^4xOHxKIff^VXUQNu)`r@5Q+OK(m1yWguwAI8W2Sh0ihemeA#Zg}u=>JCDQtG(@k zcCafnhGn#{h0G75=tMlgq%HSXktfZQ*L!t-p}$?VEQT{PAVHS(0w zRPe0KpMCvdYSC_}L@Q=kMQtwS0l_oY;(u6xUQIZZtfsDnLM4Ra?@*dz?&{hI{mhFk z$YRY#=mEg!P6-(c=l{a$Bno&6k9cjvCu!3WwB&w~=cR4Qhj9=o%J9Oa`a zf3AOHzwIoov3GNpLWTaR=t=cogQq4GkgEB%kCQ+0c)^Cx&*4!wWN3lzN6b*pPqp|u zu4~D6ua9}o4`nGNf;I4vE{3l0dNDxdcXg*%f2s>(Nu^SQBq-~ zX`!-|T{R?GBB_upL&6|@K1C{Nh7wt_lqK6(Q+E20eNDZNX7pK;9i*1?Q0VmgtKURR`UtD_93jc6B~jp09rR-~?CaY zU}ovp7QW3u==-kxI9NW87WH1qHb^!EA4FJZF(kd-339mIq!-@)_wrUE=SsF4%|*f{ z2{%r<%ul6cqa)9ymidJk)J;zAjznkq_CZGBCkQ3F!T!AKs(X|&QumPG!u2@BM6bKn zi5lzLd7;cJ{lvxV-BXB54kt;0L_#15r!+8S;h=G+X#AKCmS4?^D%7@~nF9G%r!IGjOx~8YN-I`PPQ%qZ^j_ZYa}LUlaAToi(}XYI0NpN_ z2Wxfc#ln2jo2Ha7XFnoAfu;M!3q6zZwLZ*q6~&ED3?Gna78X43VDoNEcUjWyWZ7Dq zEspHzzUp@P^}d`#_g^WcTxs-xuv|+UOV{YB<>$&FV(H;euf1LFxIdhXKY5R^C=+UW zPI_z#yV{MoA3_iq&{VkgWYHfu+~ik=B`!%?wudA&xvyKUTa=IxkW>wW-9}I~OD(4g zIJ?QXE@xW;P}%fp@2q%mM|$&W1s{~P19tZ%xG+W!?qu?|XT<(VdMB4zJ&FCJ)*Wz| zpMT&~GDAa*1|onwXy;dN5;~8WEPFRwNgB41KyfE$%wYEiF1x1?)UJ%pvF;lrUJh|v zR<*NlGfMjYM%_zJc;UjvZEC}AUX+lT6%&CAT&S&GcXg73Juc5J7p|wv;N)phVnvG# z8K_l}#^D@L9kK^!3);USh?(~Sk<1dYo4bh<;Lg^dX?SPH_`rwb{vKoyYkt*#cj%vU|Zo+2gUJMVDg_#a0PX5x7BXM6sxqdnO z?>iLFWC<}bTTY+FES;&SX-jn3x@6WKPk;2;t=Ie2Bms-kS`ns22e*DG zJ29C5mBn=?h@AF)Y$nUH6Q(@PK*#fhm&C9|jm=5kq@&Sp0RO5J5@J3X zvK@87x$S_5!YKP$o+!wAH-!%vDwSro36V^BS|xUyo5+Qa4)+k?Hyn1=USAfLxlSG48oxiietuOU64y*k@>#ZBI~tzk@tHXtYEVt-7KWeNsaDXv z0$Nx+i}RwFo-LDdn{m=)&T!Z#!_25=yltmr9zyC`I+K;)64@8lI{Hpz!)Z8(8slEj z`I;BS4R!8NH?`j*IA`uxzy`8oS7jq@>e@2upzT_0@2n^c>Buu=%L;MwLG4&8E_mt_ zweZ?-8EAL4)$xQ2M6t43Hcu4;a+rHbFQhQZoRW3&;Ww76Y6Lm@V|jX{EO&Nt8o!Ey zN}bMwaS61Qj;F|ce0)6#qaU86hDbF>xs0TV&P2-czE`5l9-*9DeNI9)kjm1Q3b}FX zI2R70Nd^Lcu-o?q_cmmuBO;j>m2r=DPmZ@4eRPpGKlAV5l2?gPXwaGP!zILkRpD)I zR3XoWiSfb@Z$k(xzn!PK>?}S^T_uk-(0g-OEucVk5kvj4@saL7QAiQo{mQja{T^8^ zm^d!sw@KHfNZQypEt5x;c&NuXIb}HWtyX=`m=+Zf)M}A=^b>N7yUHhx^0qKD#p(>z z0+uRRuGhe$a<<8R7ppW>bKMKp(R<|sIVkcxPqEHrDp_sP?-CJK!c94hm>rJ%-C8qi z)g1t?24i*bL_M>Ci_|nh-~imt&2TwXbY!s6WnO!k>6ZuRB`(FE^vU}@^4pp6h2OYr z65Z8J_90u))#~!!IxGgB8X| zZ?HHP+}cBgSU0C2qfD>)#B}AF1B^2TEDP5DQNv;{r&IIaWK;rQb)}u6c9k^aPJDgM z(m_G_hJfYLbf&|iYVHfSN%V;mR~Fqqk_bLbOixNdI!>M!ehws<6J{HQbM{|{7Z!Ep z$XQtV4W%`{w+gJjvX|IWbb~TU2UKlkXkAf@n;;rje-hMjfF?1gG1H100!=`v3-ZzH z&CL@eZl{e{y)*wU<2IJ$E79Mh#z^{CH48$SE}#r=c060A^Z9prO$O=fsqpiT3@*$l z*&is>CUTj*FUVI zK~wlJP5Ajq-O>o$P%W?jkr-EEzd=diSFP~;Jf_w-TOnihj0Gmq32zu+bDv(8S7K*4 zm%s4hfNa#hvQ~_gu0E(Z#Nd)GfW0(Gy2&*y432M&q#}%wChKN&SYfp+w3@q7WdQm< zwD%srL^N2R^+n-KoAbYK0O0F8h#BcuT>Lf{_D=#o3m8C*U(*h__1VKOC-!7(IH4JW zPCs?KQO(`tlon3ocU;V8Mg9t-#|IS$&q~Ggv;|Gxe0pI*Xrt}E2}w4Ofth41GJLC7 z9{Ak3rWuI_{PVc7txQg%4Zwb!N1VSa40lAnQ(#2gpTrLTATGI$Wc_3HdsBT{x$cl! zmwMf1e67IHb;SicNS@{TiFq@1YP3UCZMS zN@mPye9vNVE=M(4ndy@W*iNu|!BZ$sh)H{W_aa!mo&ko*)>7!w7clc*CvwoXTd^`e zCcwkIC#wT!;^Smyo}>4Zm&xc~qWq%pH5KB9uJ;OPHq!Ska()d3+WtkCH#tyRZG2|M zF&QPYp#jvMG^d8<4a*ElvmF(^_;R9)BLrh@>(@x?wI&`~+u?21Kq}(?Pa{p7Rx_mq zC1(tQZyG(0_!T&~YOyutOp>twxC(l$nSy^+J3)wX(WM@p?}pPisCU*L7=U+Utj+xz zV9JtIg6huC(YU)FT0)HD$MsLnJGROHPPHi|y#-{pavJ~J$0Bx%hSQn*0~Hri(YsAv zqwOByNY{2?-Mmly?aPUGg0T~&dJLc_FJy+C{H z<(qssk%>K-7e3;ulVVbpp6M`-h`!Y3Q+c#c`3C1=QkhHIY|7Fnke8k$?ahNrn$ZQ* z(6qhH;^tHsu9b_AF0I^p=?}j#-_n(;p-T=k*$FLkVIjp2*$~Go(p%_qw}kD=m`<*O zzlvYXAH=0A?0UtTkg7p#jFe}~ux1A}x_5Q6ZXHj#xjS>}k#!vqmI8O{U#lOFa;fy)!y&y7pk;f{WBxH>ne8{3;@>&7M$Xzn*RF zSA-Ckjs>e_uke{+l3#)U)P7PK4wPOp@MZ zpgoA#i+-|DZ_Wnr%YA1bX4@|w^c{UqvmZtA5)AQpof7Y5?8_F$H^7LYaj*N!t6cY3 zL3{=C1)p0^3BxAhvA52)-O_tmaAi?c!i__DX(cp*5Sc;H(b*0o@g7{a?Xkt65QW!~ zv1MAg_TtDczDuO_;>5^J(dCIOKDnAhj`onfG%F?gVjFl!f43u(c=*07#PD@GLYXi8 z1HxR4wds#S^o){d*Ibw%d#yWSwoEJ-_;F&?P>K`NpYAePGW_y8>tA}nXNYJV5z0zR z^%*LXRxl~Vu44T|urIxb0~$eps35XGF3H{;cOhoO0C(k5fc^jqXR1nDQ$yzpl&z(T zvMIhJWzDpB(2lhP`F6}Mh+i01w{F*V;)nbHd&rp;Fo`Y%)$TkvA&e_G_nKasi(Y)d zV`5XcMbLyZ_~4UOn00_+zund9G-YI~gXKDU=^&0J11pa$i$ZTwPv2dH&oz=VjdRxd zVNVfCd|vVRb3uB-={>8Q&`Gp7ICCzK-dA^A0Gauang@Tcy7bAYx>D01AI$sA7Dayi zC2y{fy)?`agd!$!*G_o$?P1JbS=@XQOn2)hblkAC@t$sgew?C~FdXbl-bdTX zM@CH4&+tW%6>1Kd@a<~XKDkB2Zoe-ReL{fCEnHBuqvv5 z-F7B%E6=PHVL$R6j1FIhi21jHiU$=}VtuE6b);>~#fXbrS+!r$^^3+b8mM8LH)ElK>{dxuUhfp)5G6QB77`KpKpb9l(Kndngqp8W z;uId5t}Lse9r^O;+(muBYELI@oIV%BG-Z|HG6e8t#3je5zS?w zddYRknUb{UG0V^TH>a_5BDqbS!ey`{d`g$@y@oRBeZ#t$+rcA4%nxmx*fjbSroLke^Z4tpFQWK{0gq1@b6K*>N%m_kyIGYC1A1DzJ8 z+dIDdSkrapEl#;&weXYl5k|0E9}9au9?tc`$pd~3azNq3ke(EX5mdi9qnY++W);$f zK1An6H_|+I39&GkPs(#|Vf{(O*+33ryC%y`n0eJ;hx1)O9L`U_hcE>=(|d8LN&<;g z$NXmsXZXO26KTVTEFd+&#hj4eeD@_FajO@_l@^|^&43saAV&S0ubY*qs@CE=Sv5~U z$X7a4sHpD2GWFNE3zpWsU4%XOL}bIY8F#m+(!${yKzL?K<|h{F5pMh~tesBquN2JG zf~4eW5zEu`T&3-34C3SH@92?DimQE;(Da6(Cd36wSd1Dxa3mF-E_~^YouFlp8zve0 z_ovuJ!9D22ksog!!GkLY=G?MB1Rd2M__3eDwWiAP^+fpUA|@FX&7xI?G50GqpZV;< z-uN}AGVreMDoXPvjB<^~jk3XZx3aXf*$0vafq1j2Byb4( zRI*_@A4F>;`Fm*^{zBn#9Ca1g5p6K{0>66{hLWf7MRJstmxC_siC>8hlzn18LbjXl z&kxK2fWGI?Wd#qj-k8%wzgJ(q{B=W=GThXSh*kxIH9*K`)K&=KBr?*=zjd9+`tS&X zKnt*znD?tF=q+%`nmn{V#_&~UUsyED*S2rRF{rj$%vbfEzJi4_T8@9=x zfz)^1>mIo|l@s_g8rE-N1a&0du>5dCfss-W-!;xSb`p0{=XaYAbQw|;$z$@E8U08$ zt3&6LQ|xLlElZ0xx7Y#^wvOHo#5a!ZhMJ6iS+w36Ae0Y{Gag8`27bJ9ec%w_j~qz% zUhS1+uDk9^_r0f6u`}*rUudoZFnvh%rr6MjXM&T6l;oyKP=aKwQjMZn-Ya{<8P9*5 ze-$$Ib4obhcsB0LmJJ^lx=)&Ij&sBed$>d*Hbuu{9uQ4x6w!6=#p9%)!OuN6es?KZ zaDGh+a5@>Bov`wBdYLb%#M9YwofI3-_b-1v!dm*aJ)&=y2B6+LN0QD>-V+3lDqxx# zmM)Jo&!%ROlnr($2R|jeJ|GIuFV768#PcaUiFs;HA%|A+tq0V$q~6+TyXpCK+iXP5 z#VR>-kGJHO^|#D|spC{aZlYRtsq=y**c5_{yUt2+yboFtLw6o5sSV)g+YDEsc+uwsirUa zfbVHcPD}{wOyCU&+`N6SNq27gqPf&>*In**opH6vLy}(Axwy43$sxnUskl6#Utqj^ zdaf@|%%nd3IC&}tT^;rP`(s{52^;=xp_-{~?1(1FU7NidzxW|<2xjZ5P3X)Iv1sGj zGrK-vT}w&1_a*9RI_VS(2+D}HpCmEO6*^MWqBEm*hwqo8X(Xz0)@ z4Sc1>jtG$IHENS+Z-|Iy=u&(|$efUB(3wlat}HVXHE_J!mGxN+9F)H#S&+o`m+HIC zsC+VcFz7|;P`K(qt5>(mR$cZrx0q-nt}OErju!`X$Law)Maa=HUyvLixm2|owU_9R^oy)XiaLQ;Y;e+SV@skWpH(J)=`hUdKh-> z+Gz2DZ%mtlL#`R_C~p${&Ya@zPf&AWTNUf0ehnnnOOeGO;Wstnr@J0^A723TB~S$< z7PZCkJ!tTV?3nxI$^ntO0*-IoQSb#p7NBG%a+?y*)z}>iU0SfT%1P4os|PbiXHCmK z;Z-@_p3{?GPPA`;q<53Gmm zeS3`6I1|2~o=I9|L*WkFH3KhqwoLObM|i*Ve{Mv>USf@1P4hY1&Dhp$`Qg<#UUM6# zbvlhAL@cPA;WjqVH0?AVqoCRta6^KE7j7aYmA zc~A~Sys=*AHE5jRR=i$Efs^kfNF7FaAo6f*39>x>UX>x_J*zsjHyH`;2)koHUSBT_cI>hLF9`x5+AU5e;6ZbM&Bd&NaUdG!iZgh53Ti1_O) zFz^4j_|a`82>KFi`t=RFK#3jFR0LW)}DO1}P}6gS!R6#qAlz+kk1Z+NWOuNtFP!oC|EL&?5d z5Uh6yurpJgow5LdTLNz+7qc#Im2x|m{Q&VOK&vc}Z)WZ_cEANfuE9@Fq3sK7WdMJ{ z%o*_i1DKI3xy?SL;?On4 zoUq>B#K=!W{_br!v$>=9#FoM%#^%qj3jcY{k;?UFRnSCeuw%885ZF5Mf20T`|1xEx z3n}pBFFT@~X7@rM89_QG{(Yv|zE)*p01bTR|A@UP;0QVUgZh`zUt-lulW+_bBgQ16 zWpGfwvr~mSwt3ZMLKPVoaR~$tczjNB^&(>&W@!uz&4FC%8hB+Cq!BL=JZCZ1vTW-G z(5il?7cDl@iQ?l3TYZWqZJ0tBOxkv00`huUcY6Yeqd3Cx9!HlzpawYJ@b+*%s&)IW zxWD_nlQLr!av4%aI&M2!4CeE)?Pg4)q!4dp`@6MZStz`DkpnFH6XsG>a*8T&ytxU1 OpXn)c%m)+ahyMrtM{nZ* literal 0 HcmV?d00001 diff --git a/moveit_ros/planning/planning_components_tools/images/collision_speed_100_meshes_collision.png b/moveit_ros/planning/planning_components_tools/images/collision_speed_100_meshes_collision.png new file mode 100644 index 0000000000000000000000000000000000000000..bc5d8b61fcec8c7b885f7b1c0a2dc28e3f3d6f2f GIT binary patch literal 419183 zcmd3NhdZ0?`+lriwN(@~YqzM{#E8{WqkW6oRaI&eJ9a5rv#q_gTCG)k#Hw1gV$@Ee zW+(|7q@?&g-oBsX`2Gn$$AKpt$@ARTeU0-vuk%hYHP&aKD&W>C^mp^ zO_~e9Uz)e|oq=DJFzD_3G{9FV&Et6B^<_WZ2Y&x}J@E^4@O1(?dwO{|U57dPIyrg5 zT)g~tC_2?aAb!wo9j*I8dE4#qH`nKE7l~*DFBXk%+98eRt1^uw>OpU^-clpKpxU?= zd6k!pjFmI-3b%XzRlAy~2kQb|KI8%XOdNYcmX;2coLcPeVvfYFwno3=Rb-yMgMngOv%3Szv}_oNb>yuy+@hMw21A0?-vEp`Tg&D zl~Oax_`lDM$peXv(f++hOe!0QO69+6W?%B6|8o=2|ED)$<|UH-YoA@tLT*Rw-WC1} z{E-fD8X8R+G65>ruUtA}%orwtcOYodsn=G2*zCEf$o^fW@6XHMSQS8{1H!Dn(s^iiT$ihy7p`tuDP+>q-=ASs?N zL4Z7J7aWwW32cJet>51af!}%e%)iq$?sM!v(3YUhe@q_A&%edm`o377t5zh6D{;e) zjs?Q1g%jH-)(cYC!R>KH2So0nZ@e0z{Micq%=~a)NJyvf1#_5;3jRz2*u!6@e>XV~ z+W6|ad!k}dKbIi8sR5aIQTlUE+Oo9hMB(qITGPUhjReEx^aw2?dQEzq9dbH^5u@*g zW!jP+JOQO?p6|ve801Z9=E?L-l4Op*uq>POYJy()pF8A#K4>f;xkR^rOFN~3Wz9-c zjO?9EAzr+wkM7a6qByRRYS)sZK9R?^g1FKLf9hV+WRPoSzY4rnMEu69!(Vhv7gAAt zOt%E3VWCT0RC-=UOG6XGx4K3FJijmdE?Wk$SISO)JL(!!0wWipeI>?)D9tXS4F1XPyhn=-v}T_;m;r9xXC?qFzSB5K z))#{)HAzDYWe`j4mAwnhbjN!TRZ~PC4 z7r&IcUm_D8ybVQzIJK^DF17!uavHb={T_4l2HbZY%nbU`(~z32l=l8C6f304EDu=F zrf+wh`9xz|n@Y?0HSIRh=+5-Q_jtg=!Ax~oV0+Yx8w5;#_c{rfm?=9DZ?JK;g z_$uIz*rN%fNDoa+CUZ@s`)#4JT(Vb5J&EiWsd~a33aGoUbH_bdEvmR(homCpR2bG7 z8EL9XMA9&yMMWkGN0Z2Ot}<6#(A%cg+;s&Vu>_+uc%cL0&;L!2%EA@bE48YBqK-Hs zBQ>|mt!9rDRhxf9z(!0V4=I~3EgIW6Gf9r(!W%U@lKg~vd(Tz{t&j~ep-*Y9Mn*#k zE$_t>kcXSc(*lt-b8!ZjUdd3ndf_M8d*r3+HHdZ4p@U|9C*IdIWVk-^a?||9 z0s+dRW=;yzI)FmY0mZwU>*)oLrPgCDyvizHz%&8?@4JL*aChLC$V* zAk(nu^ud+Aso8iR2UgVUB+3zNu5pnzNcPQ`L2mXIC}uP!>E#4GSJ5TTHCR9S;R?kc=x089>!dm8t#Ze69DLLR*G}E=bM2iO^CIeqD z{!Jm>n|ghaO#cLLEzJsOE3j{1MZ{iNzPoIL`fKRka9=tR{EuG)gmOj2js- zDy#7grZ+t@5&^|7>J#Bx0(1swZ3@?-RK|N2PajdiwY;4Ld}UwVLWn=+kII>=_8*zd zmjkcN=};mn(ctKc`|258VFWc=)_BS6polx5hzs%di30C5_lGV%yt?WT{Wmz4D&%LF z+afOZ(!z+j$jZNSZ@2TW`Hp2v>`W_x9n9sv{I3pAL&3Yi3E|#O} zUeD(4E4b&(b;nplq<3|BR5VwaT9d%w;Ib+i`<)iVD+w^JM$Qwl*w35Y$UK+I4CUM_ zpbcc9d8hTW2LZzhIeemc#NY02w{uphUSb#XBOvnk%$y2zz%uUdV!kL5+XMuDS_YYk zbl8(xW_ELAPMS^rOp4~ITF^Xu-J-N}=A*Ibqd6llq?ZIz$e5$?QzelQ_RFERY-JNiC_HO8)r8ELS% zWDfR3&~ERvqm3y-%79YjYMo6Gmo;v+N$rbDxi3J`t%8+JHEpO6?qS1KnOrHzLQda} zk}XcOM7*dle0NKrie~@Rm1*2O#v7s&q^h$kNO74~*==|7)p#pQjQ=^}E&HXyuuGx!lzq8$Wo1f$y76#e5see1w+C3P-koe5f{+6`P z=xG%Riu&N1P0V>d7ol<|4gXH6kW5}^Uf1389a-cn3BG?gvku%1LM85jJ2r?Ps7JbZ zU?M16e}jPV4-Rxbx3hLzq`_qo?~{*rzBBFD3CHB|{Jwqr9QByCYJq|J>_TarhlmNN ze$#Xkx^R&1b@oMp8AR`8JutaIC1ZElBtpnOixiELzEtHsLH4js{W(|my&r_dP~N3S zH1hZL)IO?JloNHB=%uB3MwyII$ufCYy}TI${prTN7{a1IZ}o{+_0sZU zn~T(TY1gP|z~^6+l93>eaGzq+pXzP$72#}mkAEWKLVmds-`cvAj)%h*t~R?dOqn(R zX_6MQxmX+*T$cAd!1$s4VN^9aM|iGa{&v1rgsE+EL3$fW7=H*aj-gK~H}ETT*#X&* z^ol7l8lq30hO9x1F7*twMbJqrjfRi#_`~t7wL#fUHD#J>0xCfkXF(j5e4Vw7YAmab%t#^|_r&`iOG66iHfdM3Pk5ub=+({r75GRhd8= zX#vq!B7FvgHt#Koipb0wT$Y^24Ie1wws`i2o3d{Uvd?b2Kia}sZ?sKl5hm4UXp}wv z)<@A)#JKJE9C@~m=>dbs+N&oOtj)U&;5_=KlLU{7Kl?9Wr})zz+iQO&qPN&~!de4A zQw*YDqFrPae|qRV|9ofoXIlMU>C?!`SDaM60b zfY#As-mc@Ryy3oyqZJeBP1sTN6>a`2*{A-(_Z@h0B^{X3GKZWW*?z0gQCB!qynJhB z2Okq6GQTr}7qUuZ_VB&KE%1T%F9M9IN%s_JWxwphYltnd{xAaBz9la(c4ngi<^@%C z?6?-JN=2@7=eTrqCSX3mXMZ25U6bnILYiSPC%3>&5!(SvO44WOp?E2sv6bbGM&Hik zU6}UGPA>8lRY!Mwlpee?EK5Vyx6g;nN8L%l#&ZTF67fOz+ZAL+OHm7Ye&R&#_tWLj zj56tJXKSsAVW&*Xg!cVW!h@_$+D3M}b*DXpkj=ds?~l7yF7aK>n_va$VIR$y)Ys(y z6+h{p6Rq!2TyfqcP^06eUMT&wjXc=r{7JQ0%2d ze6mcla@Vwe<1fWg%4fDSvaPD!I6mnYFwjP*1GUOH8~LJ2EEiWh^6>LPb(8m!s`NvW zw}B!9GTIPVDj8ABCb9R`FF7osr6`rAO7bu@s;Boc8FcB?hu;0H&3t~ zaD0k8Yg7wOwQaRgD|UblRUTj6$JwURh2!gh2uOIYC{(qdas5>M886-6e8x=_ZS4FL zmPt_l*|DJ|<$YYZxpGRqh{k498pcF zh%+ZY%*2h4bGs9tAt8mEOQ+JD4vRsx4t+HT!!#0MT5lJ2mdVA&zTq!kc=oaSQp_(0 z(^E4kbZG4(8@K#I`SR1YyHo%4bxmDdT$U|%mWdUw{QhUZGRLTz#p!;inlu42!E$~x0VrP-Pt5x zFFFd-2e9Q17t^qH{Ch(FmOf!OD43i3$2s|3yKA!#r| zBil0(JNlEE`i%4HeyZJ-Z96Y$Y1j}I%s46myL8$cP7nr$t9?$@5c{ti{|NPf9TV-j zk-tq?pvHp#nX;JMpQRBx3Af9vYC{JO(T8nhb2gQ3=S%<0T9X-S<-?T_gyY8GR;<*G zM4R%q9$QHv=DzAApB-$XlKt^};qQFqh`EhD?nm_n9$E*N9*f;z*X{w5Y26+h>cl)b zcy{X;hOf6$%gnThox(NoU<#9e>>%M)4SmZ9j=K@{$(Vb{yw_Z1ALrFEsm#7M|CueO z;k^&i$q4!#MnoOtIy=6x+4;y!)r10XrMK5%Y8NzQ+uOo8GCOx_TXrqHnev~Ae>V_P z11}$^*!PciE^3{Gh>Tv6X_ME%UcXXS5Y3wlstBLgr@mM%2>Wb^=#!K9iWwxKw|s^X zZ3gislEsC6yuG0--!6nipl*`EPVZAe2R;_7eU3h7SNb&-w*_6OpVV8~a;5QxIN`D# zrF3fQ!%Er|;ECpX8=*c8ztP9Lyn_pdU}A%tywG4JysFJNkm{!Ib*dZ_1(>rfDCq;2 zr%3Ci1Mas;gH2a^0H6`JxG>6&6^%N1jxlp@`hD;cAIgvkg(XWQJ*Y8=O`W9tn_S2r zR8~V3YHJUldoQCh>W54@`peUKV4ZI)Xrg|<2)t`&{B-dHyknPv{t3Qi5UngZ>YtMo zJDBlNXKgV>uB2B?I?hC{kJ!hF=E7|$4QThN2_9-~?6x;!;piO!o(RH65zxbW73eIj zEvuTqs-M=s_|HIx-%va1PNBEiI=#w1rJo{skK1`@T@0Y3C84_PF57^Wl zf*a}a%Iv;GwF4C4{#w}P#{dXcWqjk*9Q;j^R!%NMbx0ppv^p%~)-$iwa%^v>OF}lF ziMVP{lgBFC)RLpRhh8b%$kT8|xv?WA)vES)uVq)FQYSC`HFct&F8B*{y#T)CEyX#? zSIH6o?N`9#A?J9yi0_jL-DRUHC^izrgq8* z%GaJvp6OFM8znUVo{=I4Nrj?gDLNz4`8B?)|9)XB-%+wOxIit<0E${p`acWT!#@u+ z+6lJJs-A{q>Jn{k0fx00^xT_V80wyZcN{ETw}GF@1K$mE)bhe%t={9+X_++u4zkNe zgjKPvln_sCk=vqMx?WB5m%MZaLV520X0%hlhi=hZop<&lW!=@;lRg-uyqDi*k|jP@ z1+=eY{&7q4z7fNl&FymVF8u1wZ5m~jf635P$47=90ycC;zVL4l=|uCoI;5nHFSg(- z8_hp{nmZ{WpvYKh%dQ5#Io)?8m!gyPfG?1yHJqpA97LLS|4aUDVw>HEApSa^n`0-7 zB-@E$;^63q08}_f=cBzJs{1K*&@yEENuFHc4ZL!FHikK%yNxQ=Od=-lf47IqQANKO z_hPb&y6wpn3mc+eR{PBDa1ZpDDan1`lIkr&$o)_E1&)p&vN64P8YDNQG2Oun<&n&@ zmqW2Nt{v8PQeJ`IaHSXmW+w!%9NEtY=V`FQ$%H4aUaafr2$x||fBOMWQf#Ure7sU| zLXh9rSfmE&fJ@qU*bpcl)v{c5$Eh_hf-jGp%emHG1~kgHe-l%Gmw9dM!E~#* zN6Jm2+MsiRYLvQD1y?HKXRK2N^M0ivTmSgcGGoDStjUjgCMRe+^kV(u z#As}=nJ?lxjQD!twasTH&tSYV$P&9oo&8#wa%bNt(B9`HLVV=GqQ~p@7qAG#N%JOx z=OqkTXX}qCNZ*jud`B+doBJ>(PvJN1dH=veQ6kQtE7D-wxT zvMFL;AqTl$`q_$GKh)1T1}KlC3o1b;(w)2(i-DIcPBE97Fx@lKcbc(l`vLhJuNTZd z*(|mgw_#u|0~s;qTrXe7nHt;~m5XI@Q(>#upX^J*MYJ=JMd84W#Co*Na$B?3!6E74 zk!l9LL)r=QfN&#z?#PHbKXp<+2u1xbwVPPcFwHzA6tBb-NBCz!_lg}#YNq_h#SwzsivT^_INt8k>-xf5*zZC9C8uPe~ScKzMH#*?F zwn;^Bt{J4g#(trwGJHF@XPza2_>{C*rzE|j4<@ddSbJmrrig1ZY9+8VH%JuuvK(gR z3kx)vTjhL0FhKF1+usNoY|HPGi-E;p9>`Y{QA}}c&@B3$7kAz2J}-HXr++)0Um6)M z{VqIXl>M;zFn6H^?#YJw(1LB`LmPTpKw|s2x#O$!0mg?SwwJ0E`#6Qi``x@+;!OVo zQg*{~IPt;9T2aQ0_f42FS(94shhJ5-*_fSe{d7rrsgpGSW|?1+mTpzq6Nfp&Xu_Su zmt!#GO#wUJViEG{)ppnVoc;|w4R>dATHKf5s$k+*bb?bXJ-qrIS{fE;U-?gF%lfTh zb(;{krzB|T770UjG-^K>rkPDMNjDKqjU8=eg6gMhv3kSDu}mG$DmL^P~(durJ>i^Lc_6}mq! zB#+HQybOUy+4GjKw_qjiw!jNkOLtt$fWq)o5-6-(5)Ut4wWj?kpKLBfSkkMoWDIVz z%Eng*5*yFHs$4)87tZKcKZb#tP-V`WJ#wJeIeSe~PQ2 zO(39Qmj^>HwC<$7L`@ZJKo(;cGy=FIZ@WnsMJ;HAbMK{B=ug20aa-Zi<3-(aY`hws?1S`B%JWC+rV+r387FjR?9iws~Dcq$L zr>_|#wWAS7{!_f%j7B->e|MXj|Ku}rvKo+`fWIa-3dXM0J5$ch_y!~53MVimzv||8 z>u+jRrzhpF+p(prL@2Ulr(l71pHk`z8loq{V|;Lv82Y#XR?ih|dcPGFXZb4pe*9&| z_9mr{?b5o1$Yx!kv8h`%KtTTUm!kUx+8C6I4ggucrU66wqghtTP!Hq<2$mZinln}w zbb(2m;H9^h*chpU)^G*S!byCnQ48}UUu@Y2JCoKzlqdSY{awzb&1r~!bQ>}BolOhL z(SlF4r%*(Bi~UIvMT=7YKRj+Xl}^T(=Q?eP!LtcF{~TVjkEdtjIp*6mx!(<}+gIi9 z;Eoq0x%9$kWXp0Vn4iDY&v8me?AJr3u9nqU3@pnD8t>|3$HLgOf8CG`yoF8)pa<`< zk@*BP2FJoZbz9AYpCTmvP2|4IuwgBCTCN9TUF2)Q>jhEjy3Kq~3V)+W5^=MTc-i0E zdA1>$fd$T3&+ASY3V{zSfBipx^QTm_G3c)3D5NQ2TX3-Ny%Bf0h^_$7XaeD~;E%%; z?zpz2<0pGm`JL0jZlcoGGcB9|>32$;hM4%y00^wS=W3HhD^_S^+;w$BNnC1A?nXEz zwEg5Tz@Zc9*IwXTMiTjB@(ljgQla;J#3VO6FaRD?UwU31DuH|Im)&-V`JD9%rT#W% zN_iFJnleqd#kBLrAZ}_kw!IL87j04{qUfI!NJvotX9!(+C#tWLHkp7Ms$xaJZ>cXX zS|e}BJ7Ch>;aN!ap$0EC4CsfE4TxDPuJ;9JC5!0Z8wfMhSoUvpvDBt}Iw0y8EHi0T zi(yDz{uk}o@WmDl^O07pnDUlWo0Aj|qh_eMR7OXqE9LPr0!;tKdkq8YtZyHKdk)RN zZmYOL`vQNY6o^DNjo#!*-p)?AyaE5Ar2iQ=D8QF9&Gpq<`?cF$2_MmzJmdcsts_Sn z{aoBD)97hWut=-zPH=EV&cJaOmCfZg`0vr+-rmy|tGMXwn`6BKWvG=qev4ZQr3WjL zy}6zzi(4jl?8h84Dw%z){<>Il{9To9D;Ptq0LwK>=J9uMJMr$D#Gxw1@yvp|YwT8W zsQ!$$`Ie1%82ENJZJ7+=holotYtdrN=UVWG5I zk<5oH_`pyk>a|SV0fd3)Taf_-MM zg$+$CZM*2%lZJygvK=v`JJk>7J(1Wqj4A>jF8$}q58NsQ%BX2LoEpZ0@ZQ+2*lzkJ{7^c zU-KOAwo>4H8&B{qYW$xT!0PF{Tyg#Kw6GgD9vPVgtH-{@YjqxK&v@f;SVpfnxDx7c zrd~As!uXL`;dW|%6M!Ov`t@moi(>xH>IW>}aR(&9?bv8?wD1|!14F1fZ>3E3{Pb*u zPGH2gm1Wj2?CuUyojAAv?(`W)(d-_Z`@d3NEl?_4FDU;W?*o}O6^gAdKL zl6Y9wELYZ?2<}ZL9E_#7=r zzYSxoB@ZV4rm}F?xq#VeRlL}eSK-9RdO&na~ zG?w(x37yWwQ-3wu5)RziOmC77ud}c!ux(yEiVLJ00i|;b;Kxg}4+bxBv9eMV7Ddu{!e}23tg)eP^ z>F_OVGj^_U2TbsKmdVv7Vj}Pi_~O17K*m_&|MQ_ctTR_zeI~x)m(-JO-z5z@nMLwd+LU#g*;~0i0$R*3M)I%9Y5#4xS#8I4#}u4!wZeplO5%4+ z^DeC`!yGu7&nUzM zC_lVwqbz(RF2sE^%xMjN6`BH7 z;)y)8o{t47EkcNJWY}Fh=8Xb4o_ysVgS>gOPa0s7FIibp__sCXO6vQ(ex!oZ1UfSt z(zx4$-xE}c_(U7pKRCXYu%ad~v+=tHOrZ68INo}64c~&(n9;OuV%jeNl5FhXBum-& zouhGKfo4nKvGuy+1bH)t0DX$M0=P`(7t(V3CxZrP(Hp8DU?9|lo9=IZRxB0E4`Sx% zM}~Uh4n88R>2o#E+#H<_0>B@8o0lU_E^zDvfN7HYc^#B^l$F?Sj)nES!6MoNK+BRo z9Z1Jpse!)*dAfGf1xiW>+E&wB+$t}#>JNzjHv;1SdOWsYAWhi^LB5)Ia!0`^#WDt- zjQV|nP`_BiO;**kuThaP7M)BtE;OozcmR>)`vGOwRsJVByc>ASoNZz7dcl^%QwY_& zlQ(!}W%g`M`ZrfXE!MX+7L&&F)8LrUfK0p87sSm_kA9RU1&Cq$&*5kcTnd^- ztjC-n5Kpg^l4<>`Xo?z%nF0UX05%$5ci2 zy-w<;>fkDS6ms5Vo_9CFJHZ;UTpy$1RnO^CY>VN>$(&0g$KjDXv<`Zg0yHLiH}Ebq zM(>x9Mmz1Zxo}l(=9!?{ySM!8!*_ng061FPEh*;{j-*5@(9fj340PzJ6k`mCREC5N zD5Yg)@W&9Bd#IDS^=Zh=$AHlmzKfzUVw4(}Sl=0!&(hn26hfU;JFx+xogb*|8Y&GBz}Fe#Le;fm{30Zr_>14 z_4;hDftndjvPm&xZOVC2`$mp``Xm!@;GHsdBap2QEeFLw&0FaAZ+rM z9OAgK8*La6xEyMgKzie?G&FR!ImC&+VwBMofrfs8=)X{FN5k1sgsZS~XZ5Msw-r9m zy(!s3)5*oHVO#bd4umt5-y$O1Z+nD_IJ0P|iAvw;eAP*udZcL+Bydd$I|kANu``SI z`U8PO|1W}hThaAjh}U{MR*?0*-Uwg$h34;m0y@R@?J`6Zc)j%dVestlN``%n zNhH{%Xs`8oOKKA>-Z4M!cKXCZWstOJ6R8~=JMKFCR!~17a}_`O`s?hX)S|@!g6~}q zTBMmG%WZgmSy8IwBtxOAZs4LtR(I*Jn6$oS^XiwL+zL?B_}rE z!f{-{=lyiU7O|%4x$&f!)PESRoC_5n^ZDtHLo%b}fCq`>G!9*;d1~c^_frTT5kZ(V zV{c9@$jh~M+S_8(J#u5KZN-LfO*`89O?qgw0!<=w^IZjE=o@+fw4})MA_l#ce1q8a zDn`6(O}OC(-ql^B8Z%>@i` z&tWSwnLIszAla9|8SnUSK54-^$<^PEm@W`Q4sH+OTXG``){R@ZB+5DlGvQ3Y+T%tl zSlmR(I;q2?w-vv%yEYzejJlN6qg$|tJV1}&`WJve9-(1#X{pz5ZrR*&cv27-?23^2 z<~0flblRO@+hu*d96C?3B8Fy6+LZtdV>nsLoj33`JbpduIvmO4Cx5RG!Y6^v0;6ES z9s7Gd!&V6$jhV!tuJ61bwVcm*<|cpNv++hYOq&{Lec(#EyYhwan!u{*-|paWz&03M zE5#!9{QWyya+L{@7TXu7E18%FH=}5YHZ(UjW5{A3UIW2i`tViCb6^@CO0Al$cOMWi zuyhHq?n2tcB}l1kIs&B?-~(1rkn`&P_zugg!qC#Kx$0*0+SngTqOBn+ z@Z?|3+%Yoxl}jb{dGxTv`5P|aioB^QMLKoA@X=ZC^I;a>tl_+< z)1oZaJ5BqTQV0@K*r&#LysYRnC4LEi)SW5yup2@=9&@{mGfjTY>>K)*^?=o;yE{}i zK7S~nIY#$OdA%l*M$|cH>-#V_ku-|ikQb=)3p^8Cpih!ieJ-idQcEIhZM>f}s;YXV z+eR*dBF#2$g^A-P9L`WBmgzc1WtzYkUc_ z<1$hFG_;U**1l^91PpUDY>ZY}$ot<`KvuLwfS-%}%OiY&Pz=k4q(BhG&jw!@9Vv%T^*H6O|hw4@*bGp>jf$#V#12SrC8L! zLtZ2^Si03ewDrscY@46iZ-2C4!R!~k9Mn+1T9D$u<{}UHW$1Y&eYBn(6+W;Wwj{~% z`Jmidzt(GL0=_PpUJZGb{xS2HcnpWsu20Eu7$uGWv2!0A^uhAEHhNfBaXSY?CP&It2v1~i{$gzIHP{T-$m6sBYX<7_ zy-&A2i6~envF%5JB-`=&Af|oXRf=ac5LvDh^8C72Va~3d@aWmJaK&j#IosB?2vRNW zyUFhy5fyY@b*l=HjhWT|^fd_w{{W4xG zdRlB#$C<87$@7JBzVVy`@U>u#|#YFPP9BdbaE*zzL$>p zll_~wO;!^Z>M;RnN=bXKs-Juj$W6;Uvuq^)$@a83rwk0De&=N8gM0X1E{71nt~G6~Lmjp|6O!Bn%a_D2E9!3~-<5dda%CQU}ofyb%EW*Wc; zu@i@Xbb-aM!$xp-2-`sjzMs$o2^8VlHDd;4ASC#xH>DSo3>V?FP?~*>nd;CJ{?VfX zHkABW(38RV2H)H8%_Ui38i5bIe*wV-Ku+Cl9AET$Xfg;kf4GVSup7-b61`Zxk*9ldKEc;V56FzPg@*Jgw6kMHcLs>;UcqV1XPN`VGs zv!J}^P3s3$@owV>8;a7254c@hv8%T}=0)Le#>Hc#aoBlKWTpK_-Wu}=zuIcVC%<-6 zy22Vh2YdKCh0Rp!rF^63S?fjpw9Wuq%JG8-pf0KsOIV5`Nyq9klnT$eMeex1u^-R~ zIlX%=Ac)0eltc0hT}dpB)&h;+w_m2n81*1LLDox8-1HfChpSdO^eN;j!YUipohi|J@bjMlEe2_YriOJ7CK^X zKk_Cxv1a=7*SwugL6#b?WtmA2uKsT1BEwU#-mPw|pi8Y_X4} zjE#UMqp%O-{gi--u0y_UK)`W{H?sxnTESoMy=+tZe$nRO(kl`zz)~pN(Z&5E)Jw?b zk;X7o0h2Afmp@T-Y-l0A;>zP3)xzs9t2r$~czxQr;rQEeqCG=S_<1;l#FMP=2jqYH z^HLl8b zVj>UbLBn>Qm=cz3H~`9?CtWA-iyr^go5KG9mW#5>a`r^;r9I~o8_0?IS?)^wBZ!9i z*5Bc-QLvCfks`j+Iz%yF=|`zQKk8%Y94MgWlgg@s*_ZvVxqps1LF)zBIsn=@$oY-O$cnJ26M zuJswV6w+}%+PC1tQG`iP~ zb=NI{bK}CUhzhK(Y5}bTIf&Dq(-D^-zfpWa@4ZHYK^{UGDOURJiN!_ZW{RNA^h^+$ z+UdrM-{iNC(C@OuHn!%6N_W}T|C!_~&sbF~9|Y#?Emz7ou-y?kDGwJ;dqS7I(}Q;E z{@{jO)TU%CS>}`z<>=$MbH<;3MQH21OcVEN!UuN)y!)3M{uhoZx*D?Xz`bfZYxkdS zb8yXszE_^Jw(9^}ELCl#UC5d-7|4zSYCUE92l#^s*LKrE{1cVT=+dHL-K4Q~vd7Ea zovFLh0rf0Dr9%&?-0@>~ML#^73c9p(#4P(tipN-|_o0$#h?%;)eW7RuLwm z68t)=rxBX>!eTSk&sY_j#3sIdd(Z7hz!2`ZzqA^^B)#uP&yLNf&c3NQ1d)DSscNEx zx^K{q1(YVeS>9u#WJ@DdGvFo=fN^j zc%-^?L?xGSLm~b4Cy(uxkEU(o842D~|Cl<)exax6G4ydzXf+-VNRfiHraTi^W&U`} zR%5?u*Ylm(L5<>~%+~{{%8qhv#)m5ub-M6S@v5rE8pTdC-CI1hYuXZi&`~-dm8-qm zR%?8gMBr}Wmrk!QzkeyFwGdNPjMk0bg3Pr3C;-Q_D!!uQdSGq)^@9fKsk^7gBPAYV zqdnnOEk`u0#U*+Ppj;PBq*4pn5{~klN>DHSn(%T&$+xaO^XS5PN1y?OSFc+rjEA;9 z^}PDn32z=+p0=sMhq+vGc$@=6ddNo_16q#uK3~gU53^J#(E6YxeeOy965KPOR*YF8 zD2cyFmYHek)#gWvrY;C&;0+sGee8w>Dil zvsKpH@b>pz=6et2ZFEDW6u#jc#sNY`u_jWDv&wYYZmD45YT)(#{W&a&0ZwNI_2l$QL>7I;V2vpkFj?P)#?Q0KQ#OE}O6!^sKi`~>;>roPocd>s zd%Zp5tCO7D`!}C?|7m{x^{r%U>Xn3tH@42ff{`Z@}b<0B)xeCm~*VZ;m+PoYQyR^#?PyLS<(DXdnb~gR ztTt(V@KQAH30HpHLyahkz1WqgQP=gmS}k&G=eX7}v<6!TNSD=&MrjZS#^`m$E3@Mv3?r+w$);BT)Jpn+ShY z;!hpNyVpv@bH;>{VuO~%!s}PxOzLI;BV>XY3|YC+DExkIL}F1{dRU`rE2JuTZKn;% zE*dIgwzYwR1S+!LLqc}e@WE4@y64-{mxzIZfR^j(!W1puY{`=V9J3%>5^lD57<)JY z&pfQ(fn@etG_R&dX#B|yJ5;7vrcs6gHU3xX=P-u_hUUKJWKWg`#RjnAr zf9?N#TJp8^XU8pR#PfOUMf-KzJQN8`Ed3AnHIwZWm(_~-*Xs-<%P9SfS;P_F8PF9g|(=gbI>+bE92ha1FU5j2|#~uI; zt#U98KfpoQD5GTVsy-TtC~TQ??D;hT&j`R7rd0R8u6!u$jD`@chIOuvO;omDVHQ}` z0WkZ3c6WO$9IuT|Ajq+?Wz#f)4DgNq7Yf|eW&7PTV-5A?Xq0&Z@}n5}!?i=3PnIzq5^N~QAscg(^@fe`eBy&i zMUz9gu@_ZVl-2r}U ztE#1XP5t{svwZ#rbS#}4Y_?WFdh;Ccjxo%4;c5DJQoK^!ssHks6IvO?T!U()YY>@p-cmM-VF&N)+;*Zp;RHoi3 z!XHfrYnBY9-5O51CVxM8M>*0(U=Uy$T-nd#cT_IT_86sfk*nJ64aW5_8Ot(DDu6LQ z+{*>OlnU*}&FTxn0-EZuqdQCyS3aA5kdQ94#3Mr9L8?lZ#fh8PJ7)ejL;4ez}G> z|7D_BHTPX9L9)5j^>JC@Vd5)z^9h&k@z)pel+iFU9qrXpK)SCl@(=Sq$lh2-)OvcOI^4%hkba)6K>xr?-#am|^Mj>!`S>ZS5mKyNsGnaJa(;cZl2(WyGptEyu+Z6{Vu$TnN{ zETq2^zcY9cS(2N)-3MI(ozW6JQ(ZwO6!#R+Xm==3Yio|`6adSDXC6dYlKRIH!Ux>v55w#A9I>6h$tBg(-AfN_;NdI z6N-2>ndGDOK=SvMz=f(6e5%U26$<7oQgI@jQuARIIfkCZuS=f{7VKq;6TxSFz26iY zaNJhdwFa{{$Qp6-Za}XH5r&OV@dqNQr8x!IA-upL4sD<-SPjY+Xnaf1E859u?3g*} z2d|v$!20$^0T}wb3qRUAdOC-tswtrAF;Zh^{;*eX3(VC708E{C7k?cyG}Kio zYGvCDw?tUr8H;`t(BooTalOjO5Px|;pM@74erIkiinisUeMdOH_N4TPn&p@`2KS{# z*>1WTC0+V{YmNNEK)*4XOOt`!ip6^<#V2!g8OHp6L?xwqm}6|OG^kr%kuQDiVdf*< zT#`G`iiw{)=rea|FYp!bt)hjO2kguSnqp~)r2=YSL;x8=^j?P9-z?@}|V?j}1Li`T>2UjP*bj#=lY z#SsA7;Xn1Yd*lN{p_`Ft!Kt+ufur4@#^B) z+P?i%*ib;d$V^xab(zh|@G<82YD5^Sqoj^=$lNY8HXMX~EReVL<7(c!wvc|XQ?2=B zw{9k&54-yq_hSOurH3B5Ae6!Js&`4|QoQCQ7czn*Itm{-a^!i%3$Vk@&5w{=Umr#K z3SfuU(QP>l6d%Ta7eCBzpYj~w>|W-Clw|FJVE7VrZc_{&(#)Xo$gPn9EpdkIeEPnS z4A^8TN%8=+WTIM_d?$3(bil*HJ+r(B)gBjWb;+OJJb8PiF911OZ@YHZu$vivL9i9+ z47(3#j+@_3L_dlAA-b%3$U_7XWI_2uS<+cRtlRxJ2#!y$dPN@8⪼EjAX1Js|CdA zfQ(WW$k%xkdeG?Y-1w?Jz=h6SWzR{22=ub=R<3#Yj7Xqb;d~5%aR6$UHU?Dq2h9BC zKcPZ=(i7NPUj~3&gA%Cw&3D7QWH#xYx=n?$hd_tAjiuZA+mSoKSuh8DgOg}Ik~Ogc zuX|84(455qC+mRG47$~AtXs{JnO50rKrqJ_CEt@AYziqHlNR3Y)6J-X$fmPkh6duGF$;0Cw>efo#1FQ6_U;CM*8DcW*3SzO*e!}i4`Vsv@U-QkZWCd&J7t1W z01vhTlVk;XrMxq*%OeWZDq=~sCjdWfY?v=_K(=`$OP^zM^EEGPu|3PVq34Zw^5}5` z)sqPPefAji{AVV=-fs>Cutl=*!Up$h#rX{p!1-iEw@4WBsWG-x%%D@5g;({DOeS5{^clJk9=zM5iW5x`n5cia*u%0 zo@+?`lXglv@2s@wpcHX2+FD_gN4PEM!}>K;^AX4`@0VK(`RLmk>h`d{!phHJ0rrX{ zE(O_7_^{|`P`d!?Z<>jag*TTvO=8-#malKQTS9e}jQBNibMMO_5Uf7Vamkg@r>FZ& zrKT9_P2f;J4X}06L2|gvw*H^B@PplcC^KN@XAK%~B!OD&5us3xlP}Qyp5>%{!jbFb1x{U3U-*LqwxiP&PAQ3Bc-MF*-gb`Jf ze4Sc%F>beCKL5hjQvZJ`*o-9OvFH{F_Fv$YOX^1J&`}DX6T)C)80{; zi*Hf)(U3Fr$LxOB-2d#p z=i}E1mXWy%{F?(^?!Zc^onE6K?+8OktFVFGT zCkghsGUn-`{4qEj6E##ffu+NX?*2z_SG>jIUCyK~#bxQDhRb+{)Dl!-G`a;X@N{%G z95w~$0ZUL;l#IpxlLm-gy2?f1CGN}Z*c_fXxD6)i59$hqn1S4Cql0dp#G30@-d3Szy`12?M`%^eFx>-kbnIa|F2hL5j)b znxDto70u(%RvZEfY%je!;u;uUaG|E&Y3%f> zIE3t7Fxdl9oIT}!p**b(2S=i5VwB_k93FZ#z_gpqP~AjmuHq^W693$ndfe*89|cfI zu_~Y0^e53jeFcEU_D;|(m&x*Iiq-LYiPsMy*rQxq2crP0= zDExz5F1Ttf2M{NkdDX>GI4+hy?gm6r@e&NN+<6x+M4z*ax^DDT1?C9)c3U>N%8 zL4j#^Q=ERMw(Bex!e49z(THGLmb)CE-mD(p-luKc+uio48+YW`>odIVuUV8t4g>@? z0b+#X?^Gi3z|(vt`Mw&%OS(g%Y=_A`;ComS)?IJj?)%$HasM3p(8Xs+uukyLbynDV*rZ*V#ODx!>y#*Fn9oKDTAdI*WITRg z>olOT5Iyc;s4JNX4@uExkF^A^!2iRVF~!lxgLSYwqVBv;eeD1GB|j)Q0!QB>nw^dD zt#p06HP2*(9|+m4P6qFgv|xAzbhR3y#$+*nr>2d`_UlkvZ96VF&b2PNtbN>SaRs`Q zF>f~9&DR0vZ$9UU@`bJ9#iYDMAeOxFGi#0+1yH+#)&D}Xj@?u@3-6K=s9b>Cae)+G zLMVrw2QJ1zYb8vSlvXRG&Jbts14ba!gJHf8xeW3q-ybH4Pw%=ma-ZH-dP4sXjU zfAOl5jWKpY@81?Y$*F&Kk=U3ZbyXS<*MOJ|zqZAN!Z>#2Xsv8YJra>S1LHi9!Nt6a zo@gd@V`JeVphMd?B+8}uVy5O}3VdLXbI!Xov^WcEV9%T7UeyOkML8lzZ+_$0-$w2A zx!5rG5rGkp3;s5s3)U}1IdiY7QtJU1>|F;BQn)hW|NY3k4=4jJ^EGHP4|}S)K zpHOS{-PVjK%T!72i#{{{Z_CjN>5gP^WGv%tUu=W!!7DkcQ59g{wF^sJa-i+Ffs?09 z-lZQL_@xN%$2&A7EHfk#4*Cc7W&&29yFRzV_~R7DFTXqzfem=4 z$7>V*H&^h1erCihd;5$ARwS|^r8s-7w}<&PMn(FYk2J7!5cPw!S8Lj!42}BNe`+M- z%^cs!{t8~!rIOtPcL6unQXD%)Nga7{-$?Iz-^`W8Ve~)>ufzKY&hG`AfNz8$zhG52 zOL1;Wbc_e~c}T3s$@ut+tuCUlyC~3}w>>O^1z(*!z<;!Sy5lq%x(4J_uF?Zr+}T$x z#-_a*Tdr^ipR-FY%L?zO4lT`Sq5Il?&^CXxx}wg3(?Feb*EFPWi+r!VeV1eKxYemv zl6>I{Cn=WRiQz$rQWjlu2^Pc4l1JDH2Q~WJWg=j^XSRu{Sl}DULAn*_xtZWj6xb3m z6LSKJklL2js)e)QaIAEQ5&RP>xIF4!tCE-%UbC zet`8)75?rLaZ`7(G+St0c4~NCRRUS*^K!a3U@;PkX^M1U4^d*sF-omhdlXvb|kugXJWB#385lN1!dAbc-&a`DA9llkL) z%#U-1byk)GEK9l41;d4A`8XH|AS5F~#h|goYP(PnOJ~=;bIb~T+TxZOr~H~tZqBQq zjm%5F4LI%M+erkP%s%8i`|e{v06VeqND=_0B8Osivu?aFI3d6St4SRa%ZMA?=b#<| z12T0?#US8%JEL8L*}kq4FPXw)_T3%}j@&vSyl}{G3D5L>i(h-R3a>6MaZ5qb%j7Hh zWtkO}M$&ugEd>9?Z3&BkxGTB%r}?{?4Sb*pov5qufVJ`Sl*K=Ia@5QzF&&B9IX_2` zSK%#*t3cfJc`vl;TYX(eex7y|Z2s*GO72@;-cZTsh5CI1{8iAxZPY#W#LndT)9aj* zs73jG1t#zc)TB@UKD1u|aY`zOmnG?v$Q`yZJnh*mF9# zBO_eUY<#*@NQ30vo-WJWaiD+jgW}AyR}Q$mf40ReTA3&`%9^baU-yAB@g5Tbj*d$y z^h-I{z)OkSmc#DIyU!z=F=5tRB3r&Ug0~H`A@xF~Um}W$lWib~ro$nv<{bgdlm<#% zl>3YD`%M|$!gxcWM$V^kJm(TEY2CrZeixLD0kf^hziobpa4kpnXZ_ zS#XH9yS%S2pX)xiyz8}%hEIAv)$LVPql6KtxA2M&JVKhRLY?;3{jLWF zVp4?sC+gZ(9Aeq&AB#(>iJBYg&q%0EWTtVASuTh|p9lA-1n(IM3SV_)sH#rV0F*R5 zWl3)jZ&q%si0-u2kJkKL$9e}W%wFDp>g{T>GN{=tP~z2&5c+*Kgu1fotbNq>ZDhAl z|3iN{!O2bWRAl@1Z;8Ad+!k!54ZWNdG1Ib}7Bb-U=`c=pX3@`}J&l31sa1Jtv+*Iuh6Lbhkvx%5u zdET3`#Gl19Z6@ioO8?HY2~uccFfq-Z?=|0_41~qLjPyj}l{|Un#R`3kVK}b3(i=YW z>55tYLH`W5Z1`5~ql23exGAu6Am?dxtxNLFoV4M-Txst& zaW&7%_22~R`~}VTm*FsMduZpNisPgsrIja4b+f;cY;4&HSTj1a0*^A)mvyF*d*bAH z6hF@|6q5&of@#8UX_mZ4y=A9I%a46A5#)XaM@C2rsnly*iS}1xV4U zwo{%XCAbDC9xKVCjW_YUx*xXLqay`A{Sch&T>MeLW~eCpA_e27ZJEr$pH;kfy*Xf^ z#i`*_{yS3!Mba4sEMt!q}(uu0!t-xikI2h z9{*K_kr_YO!AeoHlwNm=xsmralNik zsBXd=IyyS~`0uV_f{BC$hp}BXq=Q?T74K16GPZ^Z@<$C@jOZ^v14Z2tU?F zahP8~6KbgSFpci!^2HJwNnd{bK03xH%QQ6k02>O?mB8`NYP40&jLby zhLSax$#uCz{&T!OuV1|m{1GWuhZwpSzFQ?s#p!bv=Z>aoCp#Noy3}1M$X9(?Q$Bfo zr5rWhXdBsZOhr0EhsUo_&a<)AAm~6HwQ%YcRV`a)g~vtzF&x3|`MLLyjPwH@!?3cA zsI%%pep%|5< z6*srU_4v_`Y~T?JZ3=PDn1rv*yDpk5kmg^b?329_uAH#Rf2!GU zM9hwSOyd2fLsnh{U#2?~WBNhk?4K04t557hG@AYIG$G9Fdf%J$TC}E}= zXxzVH1Pu&ld8xx}2gH{n7Qsle1GbE+)3DFuk?*Pizid#|AZcv)HRkX(lD7a#RnScu+=Sc-dv2`7WD|XyVOmPp zu*0^4*D`NFxe*gui2QUDy;&T&vJ~d5T?*E2k6ayOu-$D*hDUa6D1sp#u!ySs>nzGe#4wYufokW!Xwym zrGAC3FL|wgDg>kf8Ths+t#sq}J1&HC@v1NyDH z(gw3LD3j5P=iZTj7h}Vw;pwjnxL;u4X8^MW(Sbu!7XsOo-UR(!j|Pf>KhQ z4o(uAp7Lm9>-iLwJuL>1$LN~7y?28tXy_eZy5gQi(q)e15@VXtt8$*a`#>am0eIS< zX4pBe0<5b_2X!-)0a_$JyPZ6Ce(}Lzla61*dL%}N7--h~q-BOmUPA=MAqtrl{{+?R z8s&;II{Z6I^f^;UV|7c_$8z5u89$RF`bHwhc(sYZnxVCeZK_@E#I25CPYsaelEO)Tz%dn^%ZYULseFfj;gR%81)I45mos?EqM$o zStq%@LW&dZ8F9;&;KKp-vuS>fMtkXjcS-)95}_10Ww6K(_>#rKjKbU5Df5rVqr}%p zqPijmI*chx%y*xc0yimH{77qxFYIDP^SroUWrCq33eLTGpR;x=1jGe9bQ_@q_0`qh z<=s@J!t3))ak{1%A;ljjCOUPT&OB5ac)x~jGmsYwOXe7UELvKSW(~*f?(}1vB2mgy z$UR=UK^S9_pad&k(UB(-McQ4dnDO>Ez`5W3u!Zacd5P z;f*bkXf3XOP@CEJ8rHWs{@pk_&qbj)*O;7~yQpVPUGha(DyTPbxBJGW%FULYqODM$ z!$u!jo1NzM#>e%|=4WOHb8|ydcmMIq!a5y+@NYYPsNt+=u)r7l`=HsvFN@;qTl<=B z0YYEZJixXUw=(|MC2bf%mlfPAi|6$^m-R;TI#`Ai_I^vHkQRh}rGH`kuyPIhmR!no zN47|y8+5t7uLpqf=6wU2%Ge8CT{Q2cRmz{XgNQRNeS8yy-7P2}|tj*{-* z-zmRB{N`hCNzMRElIN=1Kqy^XobRp6VD*^?C4>qmf@?uP7DT^$AiO90x%QmV#r~KW zhU1bL0_qC}wpf=X4XPxHP)O*c58v`}vH8rOuD(JSczwZLVY|M?T)P$!Q2kiE`ExBz zyh(cEdHJ(W5-n%DZtDSr0*dEP!SsU|Xgn}T(c6g@P@BV;e02w#-C|DDx@{zLA?%C+ zPjXnj4B{V`sJZO>bAPzG`FVzPc?ruQ293eOm%ZDn=f=Q`DkcP*PC0bhWXrBLi)qOD z4@5|Jn5vtMM)fX;nW(#_owaI`I*TGELOADfMwsk^;l(2Srk)6oLDfdpaEa~8VPB|T zX)B;586(6V&W_jXMU;^@p6@CF zzf>|tGI0Ygb=HXG-ffR7->WJ95N3DbAI==z546$iBArjT%hwIV@hPL!!JS^5>MIid zub}*wQ2O8a$ekZ{xQ!~~C>)Nn_<7`KrXlIlhK55lOJAl8G(U$br=j?bp6AbO-4ryUT0)bq5Z$Ziej|*@#lFn|5U&KgGpx-#$8AdP{&gTTqR?wRyjt3CRgc zUme#`3cDXncxm@$E`O!oqGM;}mH)ydLUY^|v_qf^9odcl?wykXV zzhJymMb|zR(IJ#+vXXW zQyF84ms%rx1Uu0N2c~F5hW(wX>42t4PVg3fu5>bMTl*P-Zl^Cq zisKC~im%~$d7F6VW#OcvxpH-px!&CFBAf6>@V{YYwmC&3O_$*PrYF0={kQFya2OXG zb`x`c9lN>f^}P{OZ9P2J4-=SB^X3M_`B~pE4~Z>ttas2cUZ31-g+eErjUG18QqD}# z_Gfv525u4ZQz%@>LO_E?oL4p!-7K+pL7W=PSn!%9vO>b4A097^RZXiV6N~Ikn>yM3 z#8qI5R@d@o^O)I=CzWyU!|$HIKWXnI_+yG-5O}Z>HnM_KQo-;~gf>EqntG~HvVQ-Nzyu~R z&M)RBc5DC0Hr1GyuQfqkX=E-^Fzus9fvc>4N%&#aQvbmO`=U`L*axsm(grbflCcLB z%jL5lR$9@9eZ>=e&IpHxi?M{HQK!kr!kV%?R#i<}ySsxF1b^O@mjp4@A5p+Nr) z`bOTZLaQeUZZA!!StGr^IqW&lRCtGA<>;ZrQ=Kbteb$Z&u*&fgi>cf{6p-LSDQUs4 z3JA$=e&GO*MyergO8(arIM9S>F!RTMM%0|7Pjsu@CMdqci_`pk!XJRHEj%sIVmwXM znHkjj4v3&eLUkdNAOkE!9vs$(-)wj^NQ|HJH+osmj9Z$b*W-+v)U>Lvs{|8Qa2P5z zz{%6-*eGs+1Tt{eQuM}`u#>1Mjjbhr9iAcmm^Psod&*6-@?|8bG+ z84OhhZnWI+>q3gy&9(N5^~-!IBfMD@SvSpeM7_>yR&a0|i5v~qS#i|D^cU%cqI}Wg zuaNf%fp&@mJYowvJIxSXBx6Q7<_8mm=}-42E;`2vueQ=l??@Lw`9+Nt>K}Ugz7wfW zf2)s?UXPJodY8gD1mUkfoTkFi{2lB+ zq;mi;g2Lf$m){|GJ+Aq=8l|~aU)=H+z+us4gKZo?&zUwhyRGfjLu@@U=fv4HziZs} zaFpkUUtG$4Ylhk+>Kf8V@(kv^GHtc{$lxz1yhe>>iho{sU9@Hjc?FYJj3{w0G2`g5 z#$mrsw(qGC>QF>#05=%@HVh~B-)OXO>WBBXtPO5i3+3oa{4N}Y94uI2f@t#tHJY=|VQ_ye1ObVMEwT#TBIHI0e@8Os3>vh0kE(WFK6_#Q>Vs0FXm3hqmR;ZgxB zPVAHLu!k=#l*8^YD5rDwi3o#=_c&rpW}yx1C)|sD8CqaPjJ;_&fm^FxGbukV&eKU( z{mBuA@G<6Cu)*Y29+j7YbU8K9tQhx#(eEA<371{5-Ir08%S*VW9$pwXJet{lOhO@u z=mi$b)zjnn-<^>nJKCrWuJAkrvti}w|Ku*|XUx*=_QRf@<%F$~gmC6LntN4?r=v3d z8+T#en6SuRoD0=@e8cPJr61Px7b><|N{^^&(yk*iD)*1LdDxUmNl&nUDQfFga54PV zzxO0NEY65kH+Xhf_CAF^S$@_x;TX!(j~BJ1COL@Sn_BEY$Ry~G4~M7+ylNhUqX$&) ze!<&DU3GroDI*E4feiS6EubyZ%m-KsWbFx3EHOW5YD*^>O>>M|)lRilNvmkm9&f>6 z>sRe*F%c#~EL35bOrXoFX13?Ean+0Gp(^UIw`g$+FXAb#kay+0Txozo zd&l3+eH2>H-fVbwno4A-iMm?(T?%i6`VEkz!+()SoAe>aNTQNO3&;0>sbZ))(KjlO zL$EY7<(7bx?@(De3@oykjDV)k+dU6@l?E)Tu4J9f5# zfz7oCAfC0CM+L2WR!DgT^L~1tu{^-eND!4{hK5#N<4dHs62`G1eE-z7@Ib z7z*FrK5B{!{~&-peuc+C_j8UvnkPUR{8ZxGQjfaC`+jbTU-YHgwUkt8x{58Uj$*aH z9d@h=2ZIk0->e8nd_x`qBvJmyJ^#kni1;k_N$MAxmy_LX>$9AC77_SV5nhY3*)*-n z$lzQPACryDk?-c%LfxDrr&`G7BaDIH<_{SYsEW6|8?1LlW-nA9|#*y_V`Z zcpMjR)Y)H#AEG-Qig;+DY#4Ace)OBw7p22PDEGl5Grpp9NnE+=-(Y9G~b?@_h=%M@%ZYtwaw3-cf-zeQ4?-s zsS{cjSccIYO7i<0HpDL7>jExzgbamX1$}&GmHv~}R`9eSjfgeC>$%fqsj#AJmCNhT z2#4a6$8PIqc{+U|T?5*lz0bBOC06d`c%*38{{=2iUP=7C7X$Ck`zc2)>qNKTg?9I# zNqD}&AI*W=*icp{Px(0{7@=5ySEk@=!v2uW)+eRQwJR>6@B0INpC?8lYsG|-@V^dE zkauG?_02N@5^iVQJo~QCx^&!{6BE@Z}H zr1P*jE!{bJv_ACR)kEy9S#Y7g_bSL6=2C^qoGL=QiRH%t3VC@SYbL^_1PNrm8KQ#$ zG_I=Q9?B7yKE&tx$n&uKiK4}ju>=Prs;hzS>MIzx1J1H*IE47j7!<(Ny?M)^0M*E* zn7~HBevbRl{b5O^IKjsZzi20+@$6qD zxss}J^+VE}`v2vX9FX-r7w8-B;4d;eCiS8c>X@aKBP}ewT`>CaAomHOg*H^8Vc&~9 zFdVcTk9!MBb?p7soq1|&oc2hhI){%(ie zBdoWR9Oc`3A=jdFiL(0`2h~&j;bUYBV!@YT6)#v|fx?l6?^pJBSEmxa+Tp@=luay< zxm42+5p;buR~eA_<-Mha_uV)hpCk=;kvKQJF^y=^kFh`Jgf#tS@H=Y5ck+ajlQ-Gf z5i<(%kP{q8&B}|vI!xznVPm-=ObmGb$N0zpR%6~hSP-69xV0JmvMyN0bv>w^70a48 zS+&#rY>0YT-RMfu-)o0f<LA-+~fk~%6uU!4NZB3>Zswr8r}97K0d*h z5x`Smh51t!xUy<+vbz|5Z3O9wldyi3=qjh0jy_aYWlk@z!bIN8!Bt-RO67}(Pb#%A z>_)+RJ%SlitYW)&FNyI5c9Pao((kO4E)c9H;5#zNhvPdxYl3K;`b4}GRvPwh>Q+8u zRxI!>D}BX8sNFNOw?u9>L%ub?jXZou60=93*SL#+ESM=zsF>ntv&L;?%`+>!tqZy# zZf=Qw&`>5ZbK+jQz0Mk)_p6WI*%s=9b{0tl=0PjW^6ei|i&Z>3s*+{k<-pU16w?1B z!Vq=kPArYhgT{~#1PyMRQ2gOc^qO)i?EZq(UM6LY^p#pWIfDL#ATS*+{lET0z-tRw zb6K>rDS%-(he8;07VdUHk{7~V6GRJ@^Ldefvxm@nh5l<@FkElrc!xiH;J#mUnX!qN z!d_ZpuLGik<+AV&sWkodg!DXo?0HSeT$WT(VzYp4;X;W3Z<2)V)7Q4XVJoma+$`kn zaL319EM(JTzOxn9J`YANnaq;PO6m^A&*$NSZ`zTk|2fPQue@4^>>Gx>`InQk_}QNm zn&t>&;2=yR*N-AddbM-~YCz?E=^|05@|<>sT6vr*KwA2$({KYyeS(akIF0bdX=Q{^ zUhw-OWJS?8dp8r=A<1_CMpyh;((v-Ds=wp!HAON)!*_mY%55}frLkbMs>Hq<2gK{s z+~ph^_@uQK;JuLBr(!1TMGDmvUuZ>wbwUdFJN83xnU?(A5tg5&4H|Q9f>7M;2_NzC z0&bo}Be~}1)xGuLm*aKbQOydbzv({TrtYECa`fXECcY^9sS4rJc(Fa{X-d)12d$`J z-2qmiWj5RYCA;c~Hf#n3{-QDunT$){u#@QfYu9bwWe{rw3=Fx#A={`m){71Vzgxow z9&IZXyO%()D+jF`Si#zrJ=@;$-knOYrXf3E7c)~HE)}xsC@p=~#I1;o&|3d}9lHtY zXq$KIUL8Tr7&gk#Av?A#tz>|@Nk{To9?2E*NdjNy4pW(`vcf(~L7-zP@a~FYVD9`Y zp10u@+EUyznm@)RJY0vQri)A8((IYy3i84R75&h#gxfiPH=N_a+@YNCq227hP0`UKo zsKplAQPVW1wFixEj6dzJ$ho@r|NiuX*ic6HIS`slKpIlU%`5;XFv+R^W4t#|!GGf4 z9lZp?0w`a;a(PU82;CVN%(xfSk1s1LJGkbKyF`rSm;Bg^c`gOSf&8Z-*H5 zKK%b_!v|>l4RQ&Slx}WO!@$1YXng#xAW5DiTce}j%Yb5SnR`iFIhA+-7+$RBB-~!cNEZd|AMPhKV%%+C=yR_s;i2N|>KhGg-EEaVn3LAGBX6xA zvMyBmd})uc4iF{}D%R~{e_r5AUl)K1x+(TfqW&mJBV*bCwQs6g*R2ZhY#Z=WBldw-uiP9W=l z9;nB#ov7x4`mzU`D9egMG#}^jStIb|b?`oi1i~%fjJ2{u4LA`(1XZ?$@AA+9w0U7T z4bIyqELEw9n$Xb3htr~NN<2q1BgcTcz7j`&_fFF#dYx|%toD$df{83H{grD(<5$Vr z4f)U(X}J>xXxLLdoH%&{OL~2c7d-n(m_GzxV(i&_euR4@j#G@9)ys)JQed}~6$uhm z_=c=F2Fkt+#xE~!X$Q-2QR-E>)S-rF1>A&Ij)#Mq|5c#=h*32}oEf-An>iRm1z}IG z=id5^RV9OrKFBv`3S*xUQOj^2*jM}sdcOm!|yGe)NU%>vtJ=OcTgwz%KTN5v`WLib` zaf4HE7?^(i@Jd+4tnN~%Ka|0CB`55x13(;*dS@A7_Ce^-jf<^1HICtIrYo28I+bfv z%%NFo>Z;rP&uyKF%%M&AS)7MrI2kHP9zuWx2(vk=^WOQPfY^Jx>;4xXld$UOVi;^? zM?_qRG$>L>naqV1a@A=slUX5CzVmuDq3x?F{yd!f)mqLS9Xzu`M2q|G_Q7xgAXs_& zs02>!A$^a?c*0iobO%4Wd1h%iDhi;e{ukeCk=>0p8YMgwxt9*>VW1YJDU^|}knl%+ z^c(tr)Y~oHh~}QV`&Xf|v;;L=Fj?PhuluiSWd09n zdM&IQ3D5sH?l=0O0>6r+ZD3igOTIJD&iHpkJ=fn$N>Q@o#bda%^&JdR9UULZ_*TI) zQ{nu=RlC$C$F(eXp=Z~z&{eOfj(OSMn~T05;cq-;9Jw*Yl~k}VNU9CYElile(3m_7 zjKK^=7HnL#n(yolf77^CEz{)eU9qUesB`C0(^LGmR{>2c6^edW1@}+9{=5RIZPyhX zT9p8y*wHR2G?$qoD#tBbi+s-74FIJ9Dx`i3qX$gMCvVoU((9rh4PLbS^7+NdeIS&` zZHA0NMI}=C1wTpiF%?ScZZBAuEyZ4oOoJp?(CJtUl;Y)#Q8MHXSoPbp{2SYlxlf&h z-hK#6T(g}20Pil&)o9ivRfJM2ZMh`5%A}J$?nVksr^BGbnv*sW+ONLabzUiDQ)_=~ z^{7iu)p~?-UhKPHFRG z)dK9IA|Fpiv^X8oY1sbK?Ma^yV3~sre$Nlb7KQ8~r<0OKbm5oVrm4smn0GOM#C3}ZMCeB7=*8`NM z_*Y43)u~8j6D`5zH`|)N1rXWFh>ouwdQ)9CGL!Aj_Rzu=i|H=xqu3^!B-bM$eO|f- z&g9H^#~{}2NDgvZffwW#@vYCaiwm^@5K|VTl9@Aw+H@-e(qf;)C@I7|T#8<)?r~m1 zID3KUguIP!QP4-g-5}dIm@;EtAG3!JucjDe3@QGFx2E80hD?WFlCifb{CS=}Y$ANA z|2G6+9{RRo_JR=Q86yiFiyo?%U4sktYz73a!~s(-ms!kUHD6nc)(4x`7UAIakmJKa zIVGdph^l1;9&zZ4>~z^&nco_Xou)&L$wh|wfBJ@>39|VtvBZ%{7{jC_PKs+J9azR7 zzQs?IU^`5kZV>UZ&|mPXJrQqdP4Pn?i(s1Nj+mwsT&YX?{+HY7sFZ#@&M?;rY6oYP5Kc zR$`AJvc0-_RJ$}89meDwsOpFtc>)P`A$Iw%MNkLU4O|nKBnN4u6iN!KmIucUMi4Z3!DY%ahQPX6fd*H6Q5JJ)Y!* z9lCZ0Cr5)Zc^CJKGOhS^*TW9^tUvYH1)x=&pSg)HdBw9`7GZufqXAT|F}w$L(@oj9 zxsB$G_#v3!Z1QhlYOu(3r}-wS42Bc6h`9ADj0v1U*}p<8(sm?^vQx>4P2{*Q5hiV& zd{23Sb{^XxzR+U_wPC$m)O64%bTD9Kt;owM_g860Q{=)ZytV#teG*zqXlCbwC#%|z z+0bge%%T7*#e{vt6+C=dlvL_p5u|$|+C}becl$n&fpF;kjQFg55jaeRE8Hl6O} zFBqs@&!X4{Ty_JExmk2^Kh(&r-2vwx1Iz^R%AHx+=!yV1kcuw&pQ?hBc``;^j8c*y zoDQP~uQA0nJ5!6XhD|u_$T|O_AC1fmeZxo{98Gz2N} zW{`oS4@c6h#Gbk`DL~9|fSvG%K10F2L9_spf_7UZrdj^moKuyN8LPAPKP~=QO%fxN z?LmcLfeqv0wrBpKA-_%4k*B*^Jyz>2A~6hSSLhgXVNp#>n8|H2RoOiGu*QAYC)F$) z({hm|j8QyqBr)KxHFpgTQk(j8VA2N+M?XCRA=adwS7p=+TMgD8>wFIn)b36nyv}b( z>yUc2KQ;a)+UFHxN-uB(oK&2WT0+rr!XHucJK+Z(5i3pge8bt6>9EsBgN3R;cfV*k`x*7%$L6+7Nly1NgUEryuww`X1{l&3llNX%zS5DZ zc!o1L@D@fwoyS>##C#bJ9_(<)$Qgy~DhpdNNQd=^n~|fPn*N=cO+Y{mj$h;?|E8Zz zqBE~NHgiw+w!L+x;2#wEj3L$0YW&O$$|@y^`@vqKtIN9N{w^V)yD+R-H4XW$b^NIG znhKPox{UnZ(`}j#6IV+Tnw^}ukrUh_11pEFDWWFz`gLT`2F=h zP`#9Y5FFrv%9HRp3J(o>X*N?V_lJ!#$Kk|xDtGhU;m8=?zhUna5F%xr&QOlF_^|Gr z6mPxlbJjK2BmQ3&VDsH2ihSq+qe?j;7gg5#Ggp}AluUHL3Hc8-6xB8|##m5b#hFR% zf9UKV0&5a8?IL8!`p;b|$*|Dni7Q6)huEgEF1nB#fr;{fmbklqPG_BCv;9aZ%TNr0 zrTu%^4j_xRhBz|!&Kf-=K?#DUJ&iv^LJQcSD3mSrKB4)J8gFP zg>^~AN7_ynFW$9!`0qb|U1jWY{3Qc)PLZJ%QCwL*nqy-S&L>GzWyp`C;a^zb>{KDI z&qFwp7pjrX5onwV&D)=fQH(BNHMuD}iU6>bp`Fz9X^cG_x8w~cj(LB>V~-za&9c;n z7)O`wfTCmSuieo7+~QyPaF4A5uN^l#GaNiT=z40Tc<}Qir9AG@|7`(*MQivK?(ft- z47DcN5jAqs8QujRU=lT})a@;O@?t04rc1N@?rnP_kz9EsROr1(6!r8q5GWCoD;{5< zs_nR`{F&`&&PjkwWW2Px;!k-WP>3ghB>3q<FWS0x z*x;J59#RbFyn0Z71^Ftf3E`(qy7o&2DvE_EEdKIJ24WTrH4xQs!~vnU`E*t3)?WPh zgxo_b?(1NR`hOWhH?mMbe%EGauyN1oe_TS4wQ>D}Sp#m*vI+RL>bG(qRG*Wk2!6( zC1&N(>_!dh)6g^jipHT30Ik`F+C7$mwStH}M$_di_XS0hTG{b7fBuYnb<5xBO!j>F57znXc zD0>+@l=5X#rESbZbTM3W^j?y(+l)=X5t00wE*BBKQ{$oK2moCA4%=Bwq5 z;8eGi>NK5a1)_cU$gj{`^sYy(Lf+*qT0)zxWzq9n39Q?b*Ap`9wLkJtj270ou<`>f z$@|Ia*@_nzo5axy(dC*9WEeq$OcEmb5;>X$DOh7bhWR*k?mfO9nW?fCSz{_f(aHCj z7a^XhUh$G7Ku3manc_ezwp&v4khpOKtH#!wTvt2`Oo~H~6g7Vz{3oTl zqv!f}45(y^#PPTcohh={bjGv~Cpq05sL1`HeHj`a!RX$A+7BvdodxL7A(mHZ3mowS znvH$bI)DCV+Le_RR<+Ev75jueytzX}7yphadB~eaH^Md|Gc7lQ42$)Xo!>x<{vl{ssw`-9Uu*pV*x+?#8d6k5!k%n@dAG{xZe?g3!ja=eOtesOw{w<7_3; ziNU-sdFc#{54&qDCc z{mek<()4OrdFqn497wK5s{CEXR%T=4?Go8L-Ls~JqZ_%v zy89JGC4zOr#Cz554C_;GM}l{Sm*U&Ro^?r)>YEKfpc?dEO5FlVuq|vIW?P!d7ds64 z>m3=4y@^qi_GgN7(Gs!6k?~XpuGRcb&TJr)-idTMYW_I>H!GgTIWwP|B=)> z$h5){>_fK#`y^yg&BwCQIub%Tw$p=_q(3$U)Qh2B__$j&*;e^eMg;+kMgc$k!%*dP zC3$~l;pC%f(TQmQ)Zo?*vh)Akrzse~5F&Sil5ro*zQYM9~cB0 zvCLlYH|!9Bp3fdTS5>EJ%~zU%OB+v#>vlB}JzIIfL zR%e6c5;Iw&pFO~dcD(jzmzwb#ZNAODN!b8;tPcAs*F>q%MYy25r zb`vZnjNzpL{#-_WoAfH5y8vMjBtstG9ebQ!gnz;-|A0N@hI@1sF_D9T`cIp_gAD~J zYvP@2c`1x}eEjgXIIU1hV7AaiIcsU3cQ4$Bq9lWzJGTOWxIZ#ng?Zo0tOk5fXR zFzF5@8sBmE;3l>5s(P?(B=|uaKd(YakFKhZ1L~j``*UsWUYkGZ3CrRJ9CQTM*WP{S zz^a(kUiTF6%!etWYvbME*K?$~CnK0IqZXqs2oeM_Box82kAs%;`;-n97jv9V>SA#p%>hU3?X4K^++^wRc)U6B zcY)$K`#ANhJCdv~^sfXOMWAW&N;x8-#B~JDx{cX#Xkk-BwJHR*2Vkv6AmS_sgU*NU69-zdVOZzwzz0Rl5_7tRH01UfQA5qnbtb0`pDQnr zex0~pM1i&R619GOrWuz?rtD9Ip>3y>QEr5l4a62yN}X%34I2grVg0FXg2GWL=Vgi& zA+XDx?Y2gTpiyLXO)!K)KE>>4ryn`HvM!L3a9O=(Z|N!z*#Z(SHih-fdMWxg!jwIkz(

0 zB5cGcfiw(BvMfYU!7Y&sQC3YvfB$M4sV+-s4Krbl+!R23KPa50lu^w9I@oX-=#1%4 zfM_Mzvmsl>MosKg2LWGlTkqGB3I9bG;rMzvSW1NzFAZuU*+ zuqYa5NQ2Id1E<%~NFduU>tW`Mgc89e@HjY2eQr3NpLls2sxq)#_}(DaaOSK4WmL&{ z68^kqJP%JRgXw(mrM?V9*@cU`2EHRjch6WE*=OBeV8He0q)_GVwf)uE#>`j(1GnJ^d7!>-h%#cZ=@W?GWI`eHAN$O1pa%Eh2~jJMp`DTK!isTugAr6 z3E_Ng`Rb&T8a%Rzrqb*3{a%764i@7z4jzb4jk{$f#rt8z_KR^-!$LMus|-{o6~FVb z{F9H72~KU&oNozGsat8gji;^4kM3Fnjo~yKq0e8e$uBw2;IX6J_Y{I90?>jq3r~a5 zr`VBXp}bwC(>Ej}E5{hv_*rxLVWz{1=+GD9D-OH0{S8POM&#E!nU1CEvyaAh*zG+X!%E8Zy#;_}LJ6 z`pFX)NRM?n{n7dgGby--*fC9CG(7u#`vk7A9k+oVV>58JK&6CMcQAjjijm z25QlK4%S`WkY^;ybGAW<9LjN)or05#W5{eYDuC+=a3*27G9nU{A@;>Lab^bO`8Wjq zW3)iWw-=WXknmwgVkflC9k^(NAUts_Ihzz-kZ4O@a68i9;K2BVa~=ILUm`b6`I{3t zkKSdpucraB%l18j*ir20eKasHnC1-qEf*Zk$@*iIL5||0f`n3b>Q}Lt2ZC7K>eaZeHe z{L~)MyB`3#5{J17WP4nAUi#MJcD%;i+a0&SMrZZ-RT39p+=`WiQ;kT09q7oV0~Q76WkQImB^c(Z(yw*w)&bR@ zy53;t#iu*rQx@!y*uVRLQna)NQj7MiDvwQmNalZ*LNq7ZQ1U6v6=GK~+UB%-XOnK) zg^*REf?oGVC6@EY-;>4jALXa7|KlLFLa4uC>rStme&eBL&rmN-vx$$>nHHcvlGL~y zCy9$DT3g??FMUh6-FdW;o_b&1d(qS)Z`>Z&(h_#;w5V@Jw9}HO?w;v>$!6Zjn50j& z>T8>oeD2|m8r~btw(b&PADuvJp;3) z-|g|<3olCj7-;HC7Q|gpe-sTBC``mm(d$-{_Mk^kEqN^R6)OR2-xAD;1Uvk3PwW@Q zz#imoCi2hflW$xR1(7gmAR{mMEmSVsXUVB3j6?5d;Rj#a@;L$5vS4BF6f`n`0w%6mJUr>KQX^ihw116XEz);@$zwQt>TR-78j*&2arK$G^ z8vJ5Gl1^}5n%a7gEwL&@9);B?!}{v=Ugg=Q%F)>@aCV7v@PJVxL!>K-+rTA6AvYEk zK+l@^hlCx#uaWA) zXghBDYem+DzA0bX>;RT~&=%X{SX5e_ch4!taAHuO)1y)qs+?{|RN3%WF|@ zj2bxGQ`_T*$8r^olua0t+&uTbnj8YRO>glp40h@AHyT|JjT-eEskh1CqtJN|E=UgH zipH*R1j%XPx2ca;NRUZKJ#0WPdc{fbBV&^i+An*pVx+F)0OQW?NNs1ah)ayl;_!sG z=Is%UjFW)9O!~;6+1xi!!h0)(Uksn>%x%_`N6-v>l?k4C&-WR&ZJfoG`t~U(h36+O zU9+tq%9D`%KSs-20(!2S0VI5KNvIOeNij`CFYxfC3u-UO*1r*8tre1%tkGSKoa)3* zbDM4E=EmIy|7MSnZ0{TvsHnoIz!c#o8S-5c?N1f?XPL7|Gw7uv4Y!HDhhtvS6^WPL zVi)w()KsPIw4IdDvf))p21-$XG+HeSM?x-&(5aBA5te)!EBw7gu_pMAV-J|fME7n5 zt*>lSp%fU3D>-oX!=deV14iiZpWbl-SpChZqWQLInGmgQTEA&-T_BM;!Nn zJ3oaV{*hTA8zP7qE0A>6mCI|8u&J9kF9D_%D4jc};bTB$q%D5r|GQOZUTop;kWdEX z6JB=pbU`*I=BY^kqWV$W%(1+ulS!5N*~m?p+VAk4_CK{$uE;gw(~50!9EPY!sIXtc zd{b^6xjh7=!T**2>$;f@01;~BE^@xEH}3i+Lv=ep%t`!zkDTPm7jD)vM$tM^CBKKo~ur-fWzIh3hZ!phTYTEP241V&Sx;C_BB z9LzO>v^H$WUxwEGqOy$?qk1p5z7E-@O}7P`U59@ZKRzzX9&s5mylm1y@~}m^xxO}G zRO|xVuzM}G)g6(W8>RT4{u`OGlM>$7xJX~qEq(4YDTYgH18}FEd2U5hvPP2uneY#B z?+0!Lc3UrMC=6RJ(9^Sg2(2J;h*cK$^#}Hy`4IJ5o9{Rl! zf88(y~Y)xb*o+|54Q~qn;pPcg_ag{OFK}Q5+K17IhP# z$(8u~Z0qipf^ATbYXJY>aU&Fj;&~plPIG1}ecR>E@ox0{YFi;5-ZUyCAGvls15XX^ zY>`2MC)_H1TB2?eLUWpoSa+484ZX2Rpm~4tcnXTm`|{AjtoA;u?a+tQeEdqg`OvVs ztv9e_|FcE(Sp#fEc4#bq&}qrMpFp*yQ8+!5VzUnqMDoo zJ>Cgsc~j@0X0rdcVc2St#8?IrvN|J#lc9&Fm3-&%R1`u|R+17~mjC1^hvB}$#m_&I z?opET*H3!0TzZ^Y!<|`6L_5jm;0cKnSufF`C;-_}+467!>d|lX-fhW5qqx%`MQC-0sVOnM!$Lo3%8W z5P`SQH5@KacGciFn#Mn>3qdN)=J7jwFcax+&LcFqcv%p4) z!snDIw6g6boIB@vAWd67=2y>hS!Xvebtv}!jWAX{0(wDMnSYbl$VnXSal*&7_5a;zC_U3bRYg$X9K7N7MsD5wcYWI#0ray~6ujum*k}^dif+$v&y=5hZw~5= zIvvf=sjfg- zm8UwSTr3>H9v-H9Ovg(gVR}B^K|zwtekWQQxGXNJ0EGEPyd!jF^UrnPI?dtp*&XA? zXT)s^*yFdlu=7KGkDg7b^3NeBc??w|eTwc%Ax25XX<4Ui>XVSL7TpfxK{RQ23q=9O zxhE}{=HA-=n!k#s>F7~}(Rbg_-BYlni|};gx*3dDYrzPK6K@0T{Cn%R= zn^0okDds{!7aTAB*7pwxFae(39g>(;4hNL7JK`9dfQQVC-hb9h^@?4SBStu2`{qsw z(n4xNo(weHe}2Egpr9;knk`l?f-x=zAU;VnVJ!R?Cqu|wC?Wni1Pbn}iA31Ove0J< zEjeHfx*EfbC@&+yo_6Wbkb;Fx?~^3i#PZ!1$ZzfuVO;aH@OSCsGO@w7`KjSqA@!Hr zVv%@XAI?=34QVMPxsMxP+(Hh<6IWTbgff)En49Cnb((Z!>eV*esMrD4T$nSAp}7|d zr0h-0)zW_9+yVR%7;&Zx@(XybW`4nwdO!XtIvlml5PKf(GF3Z832`w&S4^3OD%0Qp zWqLI5a$%KAE*zG`L0kbUJ$5j$*GX5!ft`{I?+@Xt_2_VOl2VU>uNyi-D(|3Qs$@Km z_j~kSZ(Zbp!)PI8Br7A2vwtl3nt$wR|EN2~Ujr{30l$D{$vy?Og{LR_2vNLU^~Eye z`~I5>{?i=fO_8um`S66B*AtfGcDWq%zsP`>Up14)7|Kcj);@#QwzF8>xahN2|EJ#0 zPgEz^_e5ln`|JA!eaRbHktrtGIaSpp7DVBlqu);xBckr|Yameq0rS%goiXvTds7O& za|}gDqqXZcvR0?wU-fq{{GP#2)z6sVD6IYcHih4*X@(FQbQEr(0NjYc(>hcs$`uy$ zkxXcF`Riz@h*LdB@BN860sP{MZzCRloEcagp6|ScbgGdC?&{*=N-V&TGqV9PAn!#B zS$jQ3$S7gA#bPnAFg!c!cl-)pD!~0|DiU80Y#4IM7ZI@yY9%NwIeHa(r-WQIE9#1K9dkha7q50NU?nQyUzb%PTVZ#WNIPONU53uKliC zV!lB9b+bE{9%?j=f0M>z+8(C(cmxYjoCYr;w`8jqw6jDC!Hgb%ptVkq#qyUIduSuA z0UV#YF^V9$S}a!vQb0up7nq?Wk5erxiqzqQDwxB24~^bG3Aa&`_3d1dYTq@k1Ty<; z|7`F<{_$o?W1yX z*}FluRSe53zA6bZjX;ou;$H&hdmB~9fubGj-Vl+Gpz+aM{i@^ND*hJ?kog}&NUBTU zz!;#ZMM37}H}68{ViIl!DMvF;#F9U=QNt{BW&fCvYdA?1^p}63i^wTMnyK`zKnZzH z)-bVicY*pAwjGiQ|4?=nv#1m$W$E0#kX6;uBy9WSz+cyS(*6adJffgKp*AiAdB3`P zUmN_R|34Q1pY>8Eb=2n8Rz&W_>h~k5P11@QvHk%a4(Gx!FO5gN=+pXQY$F zpJ7i9fVHXX1*Z4?fXY<6thyN385+X6zEtiPC;U!}!Hf1OWqYl*00O7=#zlkbnKth7 z;Dkmcerha!#Rf)O?^Fkl5=4mYxA;AbU&~KBWN;)gAOCWFNg7#}x^C5hv1UM7MU&v) zWTOT>o=5LI+G+Wye?@cRve{c)$Xl~71e_4@_JJYFyzV!Fs59KICW`OO5bT4x@z*do z+VOPwxU*`Dr zWIERxv$K^iH&`oWuyHip4DkMi&uQtuyo~v(M;r}#ydVfTgn14PtZ~%r?nQCP1J#2+yk9GOI)7{pishLKCN6F+M!91l|6XQR z%NLAjgxFfQkr)|}@k31Jt!r2wzTF3w1s{h?STdYOh1U;N`OS+Kj$_-?$m7CG!c;2M z3${j=+Be=;W#Yiz(1)8HqtCnhy$MJW5eJ4T;J4!%sD+uVxY7o}j=>y$;GPKS3a0SQ zDX}|sWfUTt6MT~{8Cu)Gn2N8F07TfM*zQ9ieaH5AvmlDeksX+01xir?WuI%XpRTvb zr>G@uvUcr4uwFCx$T7M#-83pIn_7FfMn<9p*NeN>S&6Z^^7t16_iT3K^DuG9-JHyQeoVcoyAc!`nIk83!QIZ1f#=?MPPH zVY#03*y^B7S^j;?S4G|8PwMTSsSxouI3TV1MxG{Rx>VZn5BZJ2|F{Y(_HNd_|M%Xe z9O^8F)EVxGE6jI&1jeFVVfb0}HsR($_pc1ax9>(qZg;Kx_4nGvt9l2j2&nOi^5wv4 zzE4Nn-#f`sze=*W$vbhZ_lZe@CzUHLCb};q<`lpo;&xZH#V$WNl@|b#`_Lwkl%c*} z*p?t~zdg@NJ+Sv#U#)bCn*Bd%U@`iHciOYNYRcVQFcX4dv6RK>QfPI5J~tW93ZpRP z;4Ws1XWfO#{!e(13HPQ?&K$+Bpv`%2vZPt7_wCyK9Pk-2;(o1SE915f{3~2!ylL!9 zY=0&KXYXuyy)ET2++S4w`VxvjHjrKO4<4HtGaDO)qjB!2{_UG$-8LvXf5=fSlMv`V z`_wPA`>#ZNmd1Nl;TzMGrI|1TutDp`La%1j-co%w*K;{yaXzOK2`rZ$j1}}=Gz9szo>&o6A&IlH)7r+(mF}}qT!VZjTJ_Els#6K3z_X2x|GYoKDSD{K|6X`CjPSnOo;~@K z7j^6zb}m*p&xQ;W^U0WE1i7{9M2<{~A$kBzOpb`iSV}1#;_bytnp6;<3$D7V9AG4= zOI$k^-3oavL%|?-YXxFH0U)^!m+tIM)@UOQfS`2>J~hU%!kN-nMh2Ia-*9tDO+Kja zSNC-KY+WssEbLi2I1XbwS8nPZK3i2aI>xMV)RzC9pHQ~g!TP(E&K-wkaFoN3e{v_sEBd)xI zgu^)*b#fjr)(0FS_%SYc6;9cCIfUpx6Zp|mb1$+e{gjdq%{ppxIMJIj@S>poS=W>v zi*fFtaLmt_)n?aWFiqksDWJCZRmZe#6Zu!?wc~9_uq(2Gm+ZJye~Tlj*mvA=Eu>jf zACsSS(MGO8HgBD3>$t45l0*?-?L|wbEU$A+97sxVdU}n64v5&zI+^?m^s*)Um=GG{Ml8DlsCDisi~2N`TMxJLT@elPy3-s$4*V-{11DR4DqVf| z7Xm%b-EhQkw(#pDEU;DPs$D+GPc;EY97>7?Dw|=hsJ~-5^tBD;Psy-s`%v#!=9TYc z!6aTzwEndNA%ch$qMlb*(kynz0mV^`82NC0+d{%cU6!nv$8=&1Qz*dd$}{9ZQocg9 z$zd^gi|!(dA;q@sTWi{2Rt_3PP-C7I0|L8#ol{J7d;kf>yF)#1lmGv{&dNX-Eb6w9HbdIai52keR-&}XgF`Nd-6&;0-P;(zv%?! za83B`%CN^ahE1!Vykp-oA|ohFB!M^RUCw@3uILih**x-fucMuRhuQ5YDvNz+D3%(6 ziyzW6M0+o`H{k>6NjVu5pAb`M-(}A9UR&)Mw>1-5HGczXcF~eYrS$9&x*U?)?c7pw zyrvF37R<7T2{E$rysOdI3`oLW5pKKr2~^UyzDR?#OHDlF_mWxT)%ogf)pef&5C^}< zsMX!};00bxbU);fyVZ`HUr#|zKhtM)ABJ^jaaDe)5{yZO>Wk?>n@gxeO#C+o(`Ae? zbWnT~YV~{;=wP^3u^>rR8cg_=Nsoh@bQZvc80mp%E4h6UmIce!3ul3?F`-RyoP^^l zd_XJucTZmP?(>shPex|UQtbENQG&ldaX-BTt5-3Fp(ba9Ggc@PtxY<5sCj8@)=-1P zwGwXW5W6dB=<$D1)1JuETtfgr?e`G7j?4gP=Bt{byPLFy_|z5&s7n!04=1hZH-RZU zo=!XEhAYKK8mYI=c9E(O@j+Bw6hSF^ucOr`vbjF|*A z!UYqKxixMNOG`PnodPsLxCY54sS3w+4N+SO%U2I7m0HCe1Y${BvDcF7{VL-$TD^M0 zBGNsI`R2SDd$Hl;9CpurqdE|;hI^_?k#5Ylm3I(^@ zba%z?x4>=A#Qt%>S`g0R&A-~FUH`@Y>w2n4!ijw@&SPsb4@oIhX#4SjXVaa%z@t#4 zqicXo4oH;#{+JsxHkh9&)CAK$3=wgnHEoww_*krHsZR&7GCs-qA3k1)p=rH2_XE55Pdt zoI*#7r!g0gbyq@5OfkkLVYVhGXDZX_Ptx=6uk6g;x*FY`6G-1De1~u6!D%2o7I0cr zHI8&2FZ^-eIC!77_7=?;&ott$r$O7{^;qxhNIfHk{Uw{af=u@j7xNbr{xpQV&ROu?>m8eYo3 zq^gHp>e+Q_dYlvRE;vO5xZE|w=8MWsk!5Ki{hKb9?~zLah}sqwCih9zsli3=5Y>a^ z$9yKblKP4y0_Uk*l-n%om`B+`2c>qJo`?$YmVEv1uJ3SP{)!1he7pI2F>>F;Z`lcN z3mhYOw(0*`D{^o#KlX3v21;NYx5N89zCqB|4^aVqlLE{LKNQC923WCKgvVXPtCh%7^2`Y4Z0w*G;3!jIt(RE0C zJ0_Wc7y7BB6qXf>iK*;O2Q>w4xNJx7h60G9!wr*aW?9tQ;+hlG-~@(a=q>loCWOif5y&f1^sm5F|G0I_#iHUiF*2z40i%uzOj8s8FgT zT9Gl$?gaQ12!1x{6Mz@x>P)Tj%Bpe@l}gAnWWr`E;(t>Ozhq5beLAc`CRp=lE#AvB zexA(g_HR@4T)|GAC;UwYY@$IQ&kk1vm;z3}dgj|(b+;vj-TW+??i zI=DXP*XvUgt0L8V%{aMl!oNYJ;sR#MG+&|a6F@t)FN{IAqnd=?e))Bb`6Gml8#Ax3 zvmORmKUI~b?Z-Tdccw>;6xru zYS^5z5A@yPCa422?8uT5&v6MGozvk`EyhwMYfUI;b1H}-7i&Y1@MB4YV3T<)4;UZE zM4U%_hV!iigsYSJgnu2HMk)s+GKsAe#O>{*OW81!)xzSAqatXT^F znpVLN`yiR&y2K)k5rV*Qa@48H3vM7H!sS+c8GA@c`vyhQ+a9u9XgGYcEt;?b!`1|L zYkQLhu=8U{`u_L&g|c~j)@q&$&7t%+fGh*!QS8{ic6|v? zqFp=ooG&tRFm^y#mb^IA$U?r!2T z)qkRWXW`6sKk`#xx^Ni5o1enUsucO`D#68)q)-P}6N%zcOk-|`n+S#$s$YcG7_4HN z)A&Ufcgu-iV3j1QBLgELXUW2RtiLP;aow_pu@xSJK~ofNRvHq$|2gT=Hg%w$RCT7GvW} zgndV3EqZW`*orxM0oXyixd?N@BukV$svdujU?MubiQFcXA60_^Y>cLJzPa!LZ_-|x z7Vpzfp&S-|7f*E3p6xoDUmr>}CCGEWDI4K=Ymr^hBG+;hfg?V~zTaKtE9|aGpF%16 zsD_zvHsl3Iv4VP66mANrZ!2y&nc=(GFdo3Q3H^Uyrh31T$0zbyt!uSaVIKZOFzPpE zY#$6;%A+I>@t#jD@Q^3+n5byZi;Mtq^yz!gy&IJ{5E02NG+yHD(vm@a0Y5OWRf`3# zU+d-ewjjNUYIJn-TG&DkY?k{^USZBcx!p@C-Y0w*H@{{0?${D=dff0(40HGl2mC<`i64T$ZG*9wqT^@uiZS_o2T|%Ev~cM3ntUFiYnoA~85c397l?KJqK+>N8J{9~tY`OYSKaJt=$EV5W4 zJWv{egu9Lno?C&F6?OivQ!qU4mbX~;3vIHG1hB{3Db1s*z}T8(4LD-4i-rK`x*CZe z8D1rL?h20*z4%KrnN}%xs%hH;f2s}XGZDyYalowEhuL0>1b-NMGQLiC#>zo{-=|zbW~eWgf|b z9!WcD4paX;nwyuEcymye3~UeLj@pPJh1fO3JSi@ewbEKHOkj6{vk^DS`>ffE08aA} z!>oGUwtZ*B?8IU4dVO{4>u}~>rKs=#L>f-!uJa7$-*Twj!kAPyfSKq@eZ`~U@2qAg zTS#1!Dpzw-Q10_n=&7HkGV>ogpS_QGgtqN(0O^d$2P-tLJioU5*l8bkV;;mQe!C>1 z_^FzJU=6>UikT(57(Joe zsqCMAI7`beV6mzH{xK?^s&Q1cW%PVGlvD8(zxfo8miP34BN{!W}&kY7zwZ9pi9R(qzf~Hd#OTFS;Bs;#fBm!e+_l1qTvvj{JW%-U`5F`ML zq==8%9?aNtAy6Y`swB%dLUA$K-c5uX#7GQ`{zwOZ65;_-M|r!y%7rqoc~nZyKFC-I zP-k%wduOr70rd!H@F{FyX=*waLz(<}8|S zTg+9hd|kQT6Tinfq>#Wmg>%?#8PI0vibs%*BM#W}C$g;1bFIS-Qy~5v+m(k`X#PiE z?}>sy13W1FUsah}!xeG_5XhZLB<*g(N%U@oiQ$^eiru{1yw-hTMePv}9{V%7ot^od z7MiA&2rL{#iDxLuN9=ClR!@VDv~gJ=GpUG7D(mHxj#Az0*680}W^u#1;F+orN6qdRgNLSR~&chto=7y!un(%BgZ~ zP;4owwQn`?FSv;5Td0zTCDh=MI7cNboHnUQO;@+`4x;jOk(!T*&S?mBzeFkQY%_8oSLHb=G74 z6=y7At@mvDrS91<_XNZ=6k*yGYze-}x-ZLgV!y#O(xef(Pxv*0)NBApX!UB#W?&}3 zEvWvpOt&oGK01uwVr7Ca&$$rToSzPwZ$IucXz1g0QcMYTB?2-|)^Zgj1|DQ47Wv!g zXPAT`9V7tjUYw=p1T(aMU}Yr-Bj=fL6#Cae!>M)E-`jZsk zR9ho+gaq+HJG6G5)%9{u=$J)Vk^S~7 zMgei(`lF;I^|C`cCFV6cH6%LiYJAY7ao4Hs+E08IgX0!cWAL z3LcfcVzRe>`Ezoatuck5(e(=vsEUrk_gSgqMG5Mao}@_~OArA15LEX0IVOhTV@UIP zE4x*$$xGT%RT3UTatKGMKUHnXPiQZF>$b zYQ}Le%{scea@}=3qO8~LdIw|$;I!`xwQcVKx%~s9_p+&@<@!+Z3F0Q(GP&m4AdxQq z_jsRL@Kk`mpWZ?DG|A#@xN)5(3flG-a4ZOc@jOPpwtkLAw#LEVa*suqAX&Y66=X!A z1uE4{d6Mo?bt zDaMoyrFilMe3z}ruF@6R{?;2yOqqA5auCM|9OVf}i%8%Jt4bB>OTDf@^JT0O7+2Ns+LQz68$Q?TZwbJkv`ZmK|wfn?mr z$T04vTgC2smItqQ=j+&npvd;=)Y)@=r$LDc|N4&x=x?ihx01tFf!2WiLu=`N=951- zWkweoyzKPy9-$k*NGEF9N0kdzWG1AfEk4S>q!kKBn*F{qARszp8T$R!ZT)P#goGfi z?iC#q9%vAEGcfvXFe{vxdOaFgs-e zr9PHevNX2-UHuV31ujK+ya)L1c}XwPOVi6BY$%d4)1`qoX#6qgf?pCetE()+yaSVU zA%J!op&d3!;9~wUejTh50f=szu*7Kec9wNg5&eiK&56zY#FX9q)w`AF>J_E;4O;E} z@t0!pmd$ZsRqRnApQf(@TzhGIn_YJsLX8{8z_(J{U?YVc|5c*xNNC1%WkJ->2s)_3 zFj@BVSKpqNss09tat9MyHJBOs^gs?tp%1!%{~kQl4=fdgqb*(roh&3OGvTm1GK4=Q z24f<=pQ|T|jew}S@CFz7iECK`nCI}<>^}ae#n>Xv(^}ij2Ul5TBpTDXs<^c*@@ElO z4iJo5b|Uq4##SPNibQzF$c6Limh$&fy)I%3`S;0RGVNF@zv2t-Yq0UZFQ`A5@v9Kd znh?3p*~RcpT<5L6vFwq< z+sFF>{~RI}TQUo&iHwTr{cRMzEM@t5E^suptuQ7@JWgVn4r^bHT}?FkgJOo&ROCxt&Z8{>f>&}J(`oL5ifIhA=RFEkqcnH#FE9a-L^bq;+-Vbs zg+;+85CO0a-}%RGjS*LnH~F$(l^J?i=SgTgs>x>TSo<|Pe6%bZ?1;})e=vm0Bo*XJ zOn;U%()ofPJmWdwv(^K1%@_t>P*>=IEpt-Zu_)wrKmdH=MSQj3M z>FUPrW^gmh!2ao9q!f#81P{VCq}cIQOL8* zrrz?Gt{`n=&P4N%9$)gUE6#(21IcZb`8PeBW$!P={|xf-{FKFeCau2lD=lQI`sBuR z{rd!j#W7#!4@&uzv>J%@bJqElF+#m!rcY&j*z=*+f{4tIiLFJnzcY z3_O_5lYn*LZ-M3kEg7}cO7z@YmfALS0g6uc5{9ei;{KG)aKr*0qV6MalCgm-?3w(i zYo!g^MS+55s6JWq*IQIC827%4uj8bDgZm+4iWqXxYG!+Z7OW)(w99~fAZGa@=_&wG zqjt5Qaer3_`dgTbwQvjjlMvEEV0rjjrw{>R@S`*dS?wUgROg!z+&dH^<)o$(WRm$C z!w7F(r|qK+I24(nq+Bp$mznrUdl-({Cc?gEYFB#Kx`?$s8EQ55AwKtf#S|`1Gt_na zZa&7QjVMOVjiItzT!_C9PwYo$zmH00p;3j^)bn>>uM-~tZyOnTS!(RmhO(Nf{5rl# z2s^YRls^X`)_{aiZ#9w&F#aO*&(rBGtl z0Q#(Zt%yk@UPTzyUL;W77i_J3|Mk*znP=yt2&cy(_c| zyq~v`3Er|{$kbaGK#%ELAN|RhR(W9LuiJgP%X^`nb&-wmznUZ6hlHAU{5((TdbuS8 z5mBC7@^_(O7?^~8wR2&Iq+Le(DL>}f%&?2)ZQhUxhJhJ2JQnoCK8&DO6jIDG{TN=^#ku>2V1L?S@lwAP?qQjS0b|ZqXD|=Y6jws)aG^_%jsK zA-7}VVP5M-2kLi+`K#cdZ>PdnX?1vP^ZvNPY%hs(d@pkS3qwBpvo8URd;B*D!9#ad z%-Wmb-;1aLDZ!BRDuu)OKI)X3qK|Q5w~Dq8sS7_lC^^~!0+Z35PcU=ha)&L*0!6;4 zki2y>!Xl6?Muc{RAQ1tGI#}fs)U4od*!)89;FZ71N)Rde&|3LH4+qehw5DGpY38+N{mod_t%7~q zc}FQMb+VFD%?nLt`w@W+T}ytIv{_%W{geFvXgUkFxSFP0V6?(R--NpKIr-Q67m z!3pk=KyVq{J-E9=a37qrpZB}YUzj~zU0rprwIVM5)xEmyCT9i{K_?2BiN&p&lRIBJ z{|MS2#EvHhEM`lRFP6LE@*LB6Bru$JTC3h@%h53+h>vwAgUS?+z>OwFASu3GScSsz3rCTF485mrOxgklJ_ZTcAR8kw>a89q@3bCm4`P#>{_NWMl`J zG?qQDfco^=P(@8BrYSCe-AICKUfo32Zg3Q^3kREKvZ-H)7&kUWBdB?4vz3n>ZZe$ zVQ35{t06$nWs<=7t(hI?wW=0j*xs-5_V`SM+*X9`62~&+FcpB$$rSQ}y5WCOA z)cZsiek}z%*ThkbnEnW1LAGtUPz(@2euV=-MAW`pu}`}X67V*F8(5&=b{Ez#GH zhi*zOk`gY7QKEfW_8gjbS=`(Loakjy7&}Rg_FO7<_Bq?c4q`cuZyRc|8QN|#wkQz_ zPb*O8!(tqz>%9_t?{fScc0-PGHF zTkFmq|A5p6at#Bp?_vmWLfq*77m5k@5sH4B8WWcrr^{Splzy6T_TgL^2j@`0X@B+e z08+DasSlYI(JKdc7~?iND!&fnq$ERR<{j7AWZP$7d`0M)NzQZ3z zk#1D=&lRePg?#CP9U^Fx%kax5z%PY{ z2---@fVc2FV@Yw?AS2oY;6gG6MK5HwRX2ybJF@OcvhE|fzrkB~f{gfI+)G)ok*Odt zF?%KYx_1MB)>|u5gL^j&e+L#HECJ1^?{->#9M4t6=jEV zC*PL&D`F|tz{?R9vid5D9tjz=1cUEIa2!0A^taQrx*2Xh#B&-B;5AXQK1K`&<9=}I zn$z4IUcAq+d9FCxZ$0_lR0OE8@(Hv6a8*Os`W7%!0l>N+K}KY6@VH?R&;YX)vhmmZ z{E9<+;g=qPBMRp_L0{ERji;vIQr{^|38V$id@~ob>w3AnX*OAPl%AfF7R|#+;q|aT zK12mBe?x6x_8In&2}w*wKiJQ)IQ}SnTe`c+&2U^@8&O%zFV*l^(YxqWmoN z)tZ>=u-iyyqI^FJJIdVJHN>>lB6|^fsS7=W{iBf z@&&C{#v-A4U(8j+^5cn)`&xTmry&R-!jm66bIT08>wN6JyMWab`E}~>W=07(Ct)&U z5)W>FRio+W|GnxZvvD{=UO$mN<&VlIDCWl{)KTI?-w=yUk*Y*CSg9~?l~Xou=;Pm; zlQvFuat8Lp-!s0Vl9x;flYc}gRYVAWueJ2?;i3>Z$&i{q9X>g>H6o)D+TT?l>VORR z42Tv39z9IZ4rw{@jrnFmv{YFT5MrGUOV$l}##}pU}Qbbnj8Z(C=dN;N0^2@ ze)`dF=&hCcoR(?4bjrk|P<*brIS^r^aHAz8&_}Slq=Q2|`L;m5f9V)ML4FiI3|y$f z($we;{xzuqmA~#H{E%uF1rR+Z1p(UjwiX%=?y?H-T8B*%2|2WxMivTH1;n`1H$>o{ zaq7P8_7;5|A2vZ2cF1-;XwyW^C73f61kQ`O$8QS6SBUJ9t!gm_cGWKIb+0OQ9JmN5 z5$(tf8^*~q@?7E~JDC3hK=9ADNPfNPj53QkuvX(C?dJgDr%};ffqcmb`lovLHXk!? z5?lYJs*w9wqDHprzp^>;6H$&Ja}YSV1aSOFs;9E>NlZlWsG17_S7#7z;J<}l;#1P7 zd{D>b9z3CNU;bv9uBSmUJ|13BRlrv|sG^L$>Utn2Z+4A3^u4jY-oj>n)?x~#pKFR9 zJ;KZ_S5{!NzKt9-ohS z{?|2!1k1f_(&e%*jEHLlN8q_wk9!yD3}U3Xpv(o>M8sSk#nKO094RgRGsw|7o!@w5B=}xPQ`J- z1d#(Nh(GQohzG(xkIx8g=GS%)9F( zaIQl*t9?<{W8B3tO|nnx6U z-P%WMdMQRoM+B61QahT7JWNg^$-UyOuf^Pc?h?z#qiS)4p;(1u2w`3430#h}fRLFDX zg*yTWpnp6R#tvY9D!PL}#Wcf)SwbJJN=Dn$34p0VD+t=Vq2q`nIv$|8jusvChN9s6 zHp7x&5;pV^Be_oi%mJvm$^NYQ)#EeQ@liqGcR7tRL8s0rJGD1G?UpnDZvlrd zH;qme=RhzvMgk`g+_2Llco~ewpqnwsx+%0ns&{;#U%>*gZ8=^>TXmn}fx;|pEt8Uf z$B3Eflj7w}=1p+C*Z=0@%szPf`vQ99WhIfn({a%(!vQ5*8YVZ`U`=-TCxR1hm>X0d z>#txGvzGKK0Yn|Cl?60R37qG0S<bzMro!@3CbLM? zH87Me)arR&6v~_`&~Z;S){z3oZCnqfr&^^8CquiXz5KcEt<~UryquBM7KDtkQy&3k z^84E(ctWR`o%0j`O^0^zRkNt-blO&@GQBV6D5ysyYZz}Wd1~;j8Trx|{cDzlm%Dlj z6#>k&isLftruM)#DYUz|pfQJPATEs&W&$)aHGzNZEVB}YxMJ`jXzm!r_=R`no1ES~F1 zEE-(IVv948c`sgo>HQMNhx77~EhdzxOn2bwcR&!H#4sSbjhO(Gb7w2F2ZtE$@Qd7M z(z2pmZF}RNk!L7dA;RkDHJs#$c=4-0rFVrjAK@O-h2CYVImISF*AV#|{R^-{&+_G8 z{FP=qPl!o~dujGFMaie->&H09ZNU^r|7V2$tsCH1W|RUJ4;^u2nd2CZf9MkGBIym@Bbg580jB$(MeWHp;-w0ttOZd)T7rbDqU&VlkAI=>a;#9X$~Qbi`AIVk z=Q=GsC2nXmO+1#Z9YM)%O!4#)FGvhqgbU0%3kw!`P^Djgo?ORyB!{LtU4*1sZD3RQ znKmrF(HzyK{(C!fOCENg6*5ml3C(F+tMb1VU1yf;Km;r+Z`B9audlpXl5f738o4Q8 zSwO>hV2o4rpCM2mi%e({0rb#8fxJEQ&5p$|a=yxSwXA9nG#d;&X)xNp$q#u)R(Npg zxad(X1P5rOIDN11yq?80Ij7<6cdE{~e>?sOdoUVf{7=Z!Mp&?oj#vif!cpq*!MQI7 zlN?PteV}~m$htaa>C4eMff|M1LG7=ZC+_2MI6^Ij+S6eGvDGP zv%Y2rmI?NP%_?XF#b|UgNHcZo?{MED$1a4_x+}Yy0U|RUlg&X4nke7a^Z=T97CS>Y zuODdfKGm?8T|Gbw<)8Hgg&+@|KX}%Cn}XRxBWClMK_FUQ*?>IP{5R9&*ksD8cXhJ4 z@99u8j<^)eurAXMMYWN6;@pv_mF1weLc>%`4(g{_n>B z9x?N!P_Xg616Le9qLC{DlFuAYd&lS-q>hsnTig(~++#M+W1IUAD(}w_Z#(1Cvlg_A zeb*ZUM92E`!w4Z$omh^H0)$1+i9Q3m*UFd=jKouKYE$PnLGohmFc8y$6$xWTX_Z6p z=TWN+TY7L9rLmTTZgMD!JasIR^WfrfuK)JEjb`(CHHAkH-q1Uc5r&W;b2J91%q?unH4qTMUn~@ zj9dSGxzW-mj!X2Zk2#ejoJ)qxxQ-;ck9&}d z@{_dOXz6+e5^boQcWtA@Ug?Jft(hfU%@37!5fc3d-ap(L;6gnj^rMI6k-7(>%{-YV zLs$amK(Gh`0@$}3$~oVo_*CV#haqKyzO7-JB3M%+g<%6!`lQwoM_&OZ9tQ% zH=g0y2BJ2*SZ@5zKqi^$b>zQG7yi-G0j!x!B|iR%wdJe$borg*C!cc$ulm_BZ0g6O z{4MHK^le;F!>#UoCRl%D&@o9JccaN&Q zk#6D8v5mdXf2SXTMyfR(L9{73!_U@cG&+X~$3z@GRa3VAmAnPG;^f~2$=PD@N1D^Z ztJ1%RMg`4~vS5Co;#ii+wmS%0MMB7W{D(Apvo2}n9FvGlLH37q|1J7w|B zaSCJnM;z*U@-TjO?RDYBsIwb~V{}%ecX^%@lJ>#rk5@hlPt-o{A19LN`NC#1mri?2 z{LZ`lYVqszUTZ2rYJHcWltKyB$4Q$qx?-v5UbmDOz#LH)8e9YhSrHSipHuXKe&edn zPC4X;{mb&-ZYyvTMLqC^(ZdIS?t@QsooTI;OojlwQknxyY?MCVhP{8z=gda1uZo19sJRX%NLc^g5N>ZF;n>Ie5GpbhHaX?!S%=GBn? zQ4>Y_yCeZ^?3!>)BWCREvj!8^BL5tV_ELdrj=j0~7;N%@xsD_8v3P*&fv*p7a<3jG zuLAih7;E@q&C9iGgP-O{S9qVnx*tS|?lzkGMkT2m68_`NiW`-n_Wd`d#s^rwY;|D6 z-Ha`Jrd4W1r#}BXVZYw9cpaiM;p^xBh8$^O)66d$ng`-*yjsqB|~BKk!f`z4c4#DPU{sxNg)izqmCuLL-u4nwJ& z*kn!+BS!gQWj5D0mdsxcgTHH}gY20qNI|vr*t8|=Aj1i4FjHIy{Y`N@x?7n&tKJVQ zMng87#eBXt%REfVXhznqx17U-H9JRjxKFx%i6pX^Y8|dQpK%YO@%?k(6 zm>&B{kz?)&=cb){jJr*J^uH0`;@zQQ`7>Snb`0I8KR!{(*rSs;8j2obDmQE?4(HeU zTUpWnF|P@WY9!j>p=RhP3?4JWRUJ$7zbWzZ9|%*W_-udBm1cR79=8}~q3{&mJD^+Q zgO5eVA;ug5Lkn(L+M@xdH78EnvL>qSj_*d9{?|kJQ~^^6U3u!kd6wPk*mD)AnPFnM zKI|B^25+leLw>pE2DmD{xsx`%aN#<3uZ@H$CBqhwx!gZO$*(L)WZ&`&KeWjw_d52o zTypqDClGH>Q=u?3G`l#E7i@7`zBmm&9}0Okl9GKlnKrE=dTlUuo?*;&c7p8*Di;Ia z#a(#>i~Cga7E^9X1l(4j-pbmy-^FB;WhF)eUzTiwIPSBUpIjap`|) zv@n;bfVKj9QxN@LinPi6oHi(ko!>mv35~xh2E()ATc6X@vG~UD)sE^cj`_<`3YJynl^ZpH& zogA;}7kBoqE^}oJINdCsM!N%F1 z=e$Y9-mHuX%lIoT>Ob{G5Q?S2e>+EJ^eENo)0zs4O!6Bv`M9b+2=iPzv#(4VY0&+R z#y3h=WtV4X1=>uC^)7`zkWULDjZmoa-6{W{1ppTr470KrHwb*{NZEAdOj1PZsmycj ziwSE#D0+H2)HNp?FJE)o6R&iO;OZIcBSYoLdKn2u;kPnt6-0FF?6=VWof92bZ*7}< zsaOcsgf?YY_~(CMRZp#oWqY49Ab;~-hH=*3XbTufsB2u=!`%s5iAZfqDF9F+ddBwp_dGiZo89SjJxCA}DVRN_A2{Cm~% zeHeJMTW6N_6Oq~2aE63NScE3@RI(2p1j!b_u%zNcpw|A)px3MV@=NzjzVM=x{Z4`> zv=8Xg!a`69f!ww%BGX$d7U_+KByRu<=7uB!^LvX4nE}c`xl?3TU8NiuG!yZG?OsTQ zh=R?^y^%h5vkE{_v?SU2S-w*X9;&BnP;TcLYT_s3=+_rw2b6Bk$jov=_OvWOtMsEh ztHi0{(ISo^hcW&Eo=}ch(R;{8M{A@Fr;}z>&KZYO?FrgK*vKjI3_oj<9^VL8mu^GZG(hZ{OP0j{cD&pTuAq0J{wdwjKm(JZd3vYJXz>0 z%3ed)dBihQ6X%sfTZZ}Zm4|Ec$S*@)-1I*!L%nkaOlQmL-o9a+kk*Q#Z6Pf$4f^DB zoc5_JyG#K2SIzJ?=%hZyf&EbVpM`_2`W7@KG-*l0BLAq+jrIi9{!a1Fr8Q$m`(&Wc z2`H7({CnwHNB=u&qo>aj{;4BmBwMM`U9EXUJY9~)IZUJM+B{xftjofZSHo78a>+v2 zhJq|K&yYQ5s$hTMcZuPK&LX$!*>6A^`{jKpO+#98wLSmN7MA4;Uc3gL&m3d_)sva| zZPNTWjF-TWgpCN4gt$1y2T4VbJaB=TW#H?(92ZHY(( zAIfL%{PLGQiI9`&Q}=O$U1-tvA*Y}0hp(9Wd~YLx$pbAgC3t0E7_QaOIHK!2qHAb4 zFnNK7-Vfdzo?~I{tU<=wN>P6)Km;4|oSwy3(|Jz}!k`PH=R^Lxo@R}PNe}<8X{1cI&bs1L(WX@Ch zV{9(N?W@PuS-xn&OPHBy3YFg#`x* z(tGYr86Z~gn?v~Ou)oh)|D9EQusz*>QV}|kKuSQfod-5+C~BWV28AuclB9Ti@GiPQ zdG#G@Tz-<&e6y;DuNrK@v&`aAoM1FZI+`rQHj26d;B@%!eMugI z8r9k^b_ZV?S6S~OMS~%Q|B|8@sj7xn2S?dtAf~IbgdD^0whzfRFzMh^s!=af9-wH2~N?JM$;Qf2L7UXs!M+{Uak}`wZ<92kU&GW8F<`G)^h(X6$Cg7cp?E>%$ z+4Bh9frP@u<)5Ca2;(6cVp)k@~`;0)KT_KwDzVa%m?U)snJSHFRJgRGo1!H`ew6Z z80L+5q~I?^r-&X^+BYDv?}7hC-ddYNk%&%MuTS4GbL{PEXGH z?sF~ot@NHQJAbF) zb0EwfPR;UjRFVJPSS~IPxiwV~Q{wt4$*;>_MNijsp}xUJ7Wj6^IxVI-IR2-L2R&PX zr3t&JA}u+(Fo9F@;2&&}CR1n@J`{|6L=SRncjd1YhY+ zGNPYo1ns!=GV)xT(KYjeSM$ElzW`5&TLc7vX^VPli7Ls4ypY9T{1u@UoQGzaN12CHdcJCu!DF^LEOJa1W2>${Mz%wkFHTvTDzHsPaABo4~W84NAW?i9d2@Z{sj?NUW6A~ z`AaVDPgnh`;4nhLd^FpN_N(u_?^PkIiSwqc1_%=n91wsdL2Uri5aVtB%jl7Sq6wc* z#mgV~V*4JkZ^d^*SZ(>|=6+VhO`py!e?un0X0#EsNxKVgj2HwO?6WQ@qjG?Pb-;ye za3%@1{0Vo&V@rM6NWi-$q{+P3>Df96lyLDjx6lmVy43N!$}|DX?Z`l~gwHV`YdJRr zXVI)vGrVyC*Egvnd>n8%tKfqt7j3qybtf2_$vxEC`psLBig#SUuJ)n3x>x91uW=o1 zr4o~WT-WH@1BbVL&)kuF-`WmEtTB}&fukx!OQsovs6xe*490z}t$cDSTxT%0JDlQ# zWS@%$9$h24-{wUD#m(Q(=4>P0MPo6B{1$o3st6x{fe)=ij+R>BmCeO!XT!DOTWBVX zB)i1z3|&r*9H!l#$im=DY(+_mrD3GWaAJn)J4y!7DK^wMgoQex3POzmly&lv16H$3_aL=tUKx z_yoZ)zjDT`A*TBCtLYV+~~@`Q(=012$J8C z8P_Y$HDQM4-4(SHn=!nYl!Vt)3$IeJ9tk(+)U_@NOLkcd_-R`4CGh9a9M<7T2|IQx zV&!%0R~bd*Ui0|G*p9S+Qn!E}ZOn0x@K4-dp;v6Jf1jVzyVdNXPk_sMQSlZqhXzce ze?ZwSB;X*rvM>H<}P9RhuiEwynReU zf1?H`;bjIF5LFfbM~vAakK|5X-*}ZnJst&AIo0fq%#Ld<0JI&-2BmLjp0+tW#LEID zrwIqBr31g<#TmapD$-?E$s6eEWsonu{yETUo0z~t72G#{Sx>5u5_9`UpRV3fD&W!{ zWA=}cOpjtSD*Bv;F{px+-6B13d;U!oma|Ne`Y`4n2<(~&YBCSOOef1Ki4={R`{kSm z1l=%3tZ@v*fZmXZ!-nBS!0WWL1Vb#wLz@1!UkF?S2`R_FL@wbg!D1}MU4dC=KO{K3 zos(2*8={-JK3tRCXO@JIax+!YEHGEnhwRMUF{gAVpv?u3<2hT2*;uUPrvEd|8VJF> zyKu#y?uFl`Qf4evC%Y|afPTm;_ef>|eF~ih{D-3;4mo=O@iBrV{I-LibzoVR;+Q;! zTQG*PK1&Y$r@#tciNCaNU)vR5Hpc7g287M@)Ha-mVw3~(o8A6WwV@R6>ygrg!z70c zo=lcq=v+H0s7(tbGke==(u$qt#S1#xiuv5Um$Jn9I3aL&M@E6tyK8dV-W5>q23(8H zuhc~b!`R+acFs9#LNmDiM z5OGnp==@R6#?JpqrSGaA52*kE)APd`e_%hr{ogt5zS=%#D+k03g7M=v@{3satneq@ z*RDb9p~i3qvsO|YwS@rdnO6eF?r#1SitI?6(dWj29OuuTz{uzi z(cEJa&`5ug;h`mEbPpQnqlr1`2)2i-xzkH5Cc3~-rD_d{D=wEH1P2|_!%qtB&+M|F z2k%T5#V3Jjc|0`b@dk+P1SI6Hk|=Vdf5#ZHBU3)+4S#eV9tQ5G8e@D(gR)6xB%Q6B zn}n6PTp^2X!7s$+VI-g*F9|4%Ssbp9@wq3LFAYOl!l$|ZW?K8D>P#}s70n`T6s*(% z{`3G=di=y~*Gl53+YPeymHDP}IJ@E-d)50db;D!9;{dU>xY7e2#tFWDCGutmi_*H#qm|p) zX;_H}m$ji_hoL;5L$e`^LtoOZERaQ5 z0t(-^x?T!INI7CDgb==x2G4xsU+oXS4ENrn<3~oSCVri`J(G|s-apK#Ssq{!g0y8i zt~mUC^<%IG`w-f_h%p^<+xw<3u{sfJQrI^&!07Ge-P!x%4|*D zNPqo;FlQggPz-ULypO=ue{#SNOEb^YPhM|K-fVs^7F&xVzV>@k>hSFkw9&p&%N1Pd zR9OxhQ5t1nr+J^qKc|L@KK6pHlc_PmlP#satmGmkC=4klneDXrXmT_lz;OdK zWphSu>M)2i#gwXKIbn5JcReo`9*lB9SOB!Nz~D#Q1orcB9!xxR@SeCEN!0r`MoOFb zyzGTD8YtsP!Z7r#NzY!iBf_8VBR&VE-X6L732%{H7yb4VHpOiyN&!@w0rQnF8Jrb7 z2fhkM3M^&Q7uHl`eW~-mysfPG)qV!?sqn*>D>4j_Na92ow<|{amUDRyP}j5+6Pl_a zhZ`#H-Sep91bUsf^rx;kF9TB)5iDvMe2AR*GJu1CbtMT6%Q9Pvo?i;6qe*NI=><0% z(`4&b(va_|^myVU$2&a0Hoj3SYIHq74Nchww2XQiJCbJ7#xE=VonPJ1IZLJC6mVJw4-qhKJ z*wRb=p}e8-7BU%N4wGXd$PE-WUGUT4d?YZguVcxZiMKA@R`QyiuwvHF{V2qxK_Y^E z4*uCFM4t08jE7%c$46n=iF-|@5Ly1!?NKi#nL2Y|GA5>M@kr%)`b%{VU*v0+)qSOT zmPgA|_LAk)nV=laqc{mU2skzlkz%b_)>+TczrdL?)V9H0Q(ALgD)NcX4w{3}F7P(O z)c$qGH{5odWpZQKjX%2nr7RN1Bs$2&xx zzMOnJBSD&Ch7gd4nxMpHs?1Z}fb_4exV}D3QsbBW=rX9!Q<$(=bS;AUH*0K!8`6ih z=CmTk_C#>|umyf|Cf*XyMA+73tIb4f|s)MslEhmywFRjf60$ z#_T7ZEjRDH!F_b12t(hz6BCt8ZYa_cL-%EahbxnoLi*ifrg@Em&=$FJE(6p(q`=)v zlxc2r?Sp}xMU6@RLUG_e8*1^1(ub@1=j(r(b=^zX+lxjxxr~ze=CMailW_%J--MRg z?73}((ILteK%I!x;i0W==GZ*HEJk+);*+t<&w~(??tnOi5u+3(ZQOWbnbHkzC@lTi zlrcr>aqK*Bhl>~&7l`^0nz&14lih|C5_w{8W~8k!AnY=np1`|wb~KYbuf>2H0GgG@r$)#UNEcSDGWD@laGgxiAU`l(4&iK9-Q;gTx*HEnEs^u zQk6zoDGb3Qz7k%<$k5q35Ga2{z`@ql^%O$~uy0Z)d7|lqXMW#io_&!cej{7!vDz=R z)YiM(l-;GwoblFu`xDZJb75qkT>SdX1z0n41ixpA#RmXS1_wfYMT?tnVWU5RJs%J{ zVf_*C68hjVxP+q-s5fuRrY$4U$o>-obBTK_+UJxUDBhGixeD3a8-w1aIC_aA%LlHC z?x5#C>SJVrCiXYH=D3NNx6!YJ1j!O!WXBwFf|8A$C6q3u1;KIZWalAzb0FD`&)fgV zkg>{{3&vo{U|hJ^Fx|Y97uJiZ{#MybJ~-sVHX|$8HjaqW{gZIb4&CV*&EyA^gbac8 z(f8se^0&=6{4z+5mv+U@0g_2>m?8^l&+_C~q~a6k?3iwU#2S|D=ZTuihckHxakLDv z1lU8nxVxc0e~xI%;cxT*mL-3nOf!_iCz63sDW*eH30EO4{o&@C_vIW$sZC;4`H5x` zZWP{N2-XW(fg;ADy06Z|LeZ{I;?s9UiqHYrFD`y z4MuHV?cA|?R@|we3G#{s>Z)keB}}IKx1DkNK01Pm$n&^W-+TS(xW9c1O;Ns9mH}zY zRU1fyZ`k0_WpD|E^OqrK5I*sz4LE&mf!s@mJG1LLNvVoV z9(-&KYV;F>xLXdfo>wyo&qtkLRarFlO$i^<-(Pv!svUo3RNcL^f?OH>l7`oHc_N>& zeRlbUcZ&@2R%1RNEvXsJ7rX*m)KrEG!Afr!L-&_5j~h!nI4GZFBNSSRD|p9Ez%mIBORc z^U*#orZ^KLgvU~11zYyrqCABq2n5n(C3+QbLh&yaO>hon%# zvMjk_juMKyi&%nJc-U2ncm_8C3okqau!E##YU2^syhf37<__AtESa4Du+-f#}~ZKkY2qeI?48Ev^tO7+3byXM*` z*rl1o8W;7vJC2ExfgzS3$Bcz|`8|JaWFuMn`bl<~n6Fv9D`5henc|UL>o(AuSh_e$sd}{p;^=VDzFX<+k@2MIrMw z%niQS3VOZKY=zpK@jZla3zpMV*v^6di-*X+t_$#sE@qrw6jf*bH!x_^L9OWO1tqD; zj+Z!E%GV?zi~rmwV+t-=&Nz^3ktBIwInhT1H59oh)fy@;aqGI~t1b{pBOA^+#3xEZ zdfCRzevj7D=C)&9?UkU}Ghm%RDI^T5;0@$=Dod5#mdOz&)*%0(G|wWnsKlcQ#7Z8A z$JI2lY>6+d-vhH7r%Pckp{J$Pif9luhr80Hb)t8PBP^9F>&9n8V3g#bi-;bot#mRA zZeYSjBfJWbQ1X3KJLTAN>dUkUrw_V+f{%yp zD{wIg>Wo;?nUXS@1;Q0GmFU)a@qb(C)OC3OlksSAAi{3U(FN^m=VKey;U*Wp;5YN23z+&zMW2_;)yk+`?)_VrQ2hMIcreIy zjm=4k`VbMjGRB4f^|;yl&uwdy;e){!WF{eW_sHj|EbxQ`v+Xnzkt_KaV%+Vd$yot# zZ*aYE7vM#>q(KI4L$jH(Wfq8ySDv@{e?`g?Rj$oOSwYX;f;LkDL$)0B8F^4o{rZ5& z_CI0Pe80sp7v*#NM5L#ti+>5m3Y(>+xb8fPa{b+^RG7w_^L{^9(N&GKbqj?3qC)qN z-guIA=4rApDq$~}#RA>;gVfqDr@}A4&&Pf)RC*FT9s2wQ0V}fsUoo80fOQtD;AmWd zm=XGFa<=_hs(&PkVPS|dV8<~$Y-$%f2~8i(=k{CJjEHhL2PLY4y^(!^UBxKy!FUYU z&13tIT4e>E($#4f#iQ@}DL`ZCECw+n0xHfMthh}OjOtKLANYo5g*D|+vr>8R)g(D# zU+hTCGfYckS6mw(X+Iu0<mE~A95`I$nuk*RiHl{Xxy#PJO*mM z>{x75_Fofi<~Uu2?2d=%e&RuviPvyc)3;tdw}QjZ-ru8=J$G0en)Fc(R1SYh`eS|8 z)`3GElBkGju(hfjy)3 zqo^vGJY|Bl$uAg@$i~z4E(>C2d7as}DX=*O-}eC+g#XV1^nM-rGez&^-|L!VUH;?t z_JQ{3UYRQ3dM|mBGgNyROkD3j3=di7E6ehH?M8=+@wh8+Affrju}d_}a=X`tJ_yx1 zc&Vl-Sm$wAFNo75%KMnD{oR~tDthYz$1iME88vb9gTa-3v{zDMy41_zzW=xGQw#I- z(VZwF0X&T4-e$fu;6whaMZ50NEe?1eKhl!>s#t+$@biqB+Z>j4S!<*;Z~>pKzrUtq ziGZ3-9pph~Yb^UQDh3qJPVl}6TIJd7sS?B1SoD+n3r`sumtG6MgWQ;Si;~ilaq@i$ z*X|d$?~2hNWsU;}*Oy_$Hj*D3#Xp*7Hyh1z>rGh*fY=tmc-quyb@}-YPsk(|%MBva z2aMtxIWWug<|^xDTLuRWtnJ_1O=Ws#DfG z@~?GzKqzLhw9v>_PSoWS$G5EA5Nvm8LQzJ(|1Rv8-f-3SmQ1)aqJOk+mdjx!Og~*S6l4!sf!=}0X3t( z9cSA?ESwn|B_$4hp_6Vu+bKEmu`}4#hX4DjjY-{f=+zaHotm&BWtz4d8|DKdJ49~m ze<}Sako`8~UJM+*z-NHMA4SQ3{pq`Wy7FV;5+LPRXy8DG$UC#5dbgow62~&D}Wk&!|jv;tT3ry|o;$&NLC2n-s7YnD3&HkJR+q?ED zrtOcMyDQs5WnwhP$L&{1GUL>t0y9f+vr` z6yr%@7%DRQS_&tv7IJb%iE7$1o~5JE5s2yZbO&_@E_e@rw-Vv@urSvxUcmV$)TmvT zQ5rGOuAAb7YvP{dsX1Jz7+{j{q_&#-q)yJb_kbC2_)+Vh&MrJu0)8{{A9Nm0fkiQ4 zALkS}Dv%<0ck1~-F2>K^82PaEW3~Qv-I}FBlX7WJTskwI-gjIHAd@N*TcaAugGtXWh)mlTi9 zr1CyCAi_x0&SK)lc&0*p1=!}nT5JL-D?Lu@<<5L0JeQ6|F9#P_yo%$UH4g<-C$*}2 z%6b~=gEGNO4-yV?5S64!ePSt~c44-&&xc-t2X)Ns&wj?))n>oD28+^|A!u;`HV|H{ z@#j#qo!hcPG3nyA*ui)De@V^MAth?A_9X2Zq)?fDTO<-8y;{)jj#giVgY5DSfK+?v z$7XbaVB3m{j!LXHhAYKff| zw@=m#9`u~aNfHwYg3x^SIDxgnK)!O77GAbx;0RV&bsgpw;2gAQl>8QVX@M-%?TZ4N z5SKwrUzh_^{$pYJCQom4IPC|GQ~rL3;*s)C4uwZCcZX}P-BVD6s7(=-%1`1aA#*H> zK*L#Xg6^Mz2M4$WycVx@v)}%^XaCUY z2l<)NqiNO=_tCPCoJ7T5Dr`aa+lS=OKboMM}4+Sp&4^LZb>GvES0H6;x@N z4UCw#jm`S{I=-a0?EHr)>WQ#eVemb${^M5r>xJ=;fG>VJa?NS4Z(m>EEnbq&EyqTj zIHKiGy8q7YUio)uv~8{Yga9rWTRB)x#1Sk=?VeLVz`!(FgTp&>kOLE2%5_Wp7dkCx zGMPBuPGBX$!Vw)skR@5F=Kl9Xj@uJLcd`r9BMae*u6m@c2Ux*!zFTe`E~E-oSs~5V z;5Cn|XETX91w4A;>;GEls;IIGh96Z9l&0EGwhy~S+DI?IZ{Tmx&d!u5K(ASH0i4>)vrXV zrTPdV$Lg{=i`^q7>N*0+vEd{KpJaM09(FP5@5{V&X`;U-uiJ*e`KI+0T!>t;+Vms0 zLuA-Mp72)mse-|UKmDgR}-eSX6X>jVoKs~dRo)6%`2Mf#3cROb{gM(|CPkEt0nAb zQ#(cFQP^TZ#Iq+UXxvqu3kgRW@kJ~5KU3Jp%Y^7OFaAt{Hlg5B6GN%@XaABn5|64* z8}yVL9@^PK87w_DszU^p!{pT56qhXLTfQ&$DOLL}^=t_3@)6-1naB!HpOyPmNj*{M z52ZC98pSvswHugT*{F9Mhh;Bk$m-$i)lp!(NOdP!zsUlf8sLFa&puXkLygN&7OGkk z_N_R^o*B8pvd@~3<27z?&yX!c>tBP=%^u@qVaVGjz-K{kmlqY7t32z{~ypS@fB}8t|BCl$*Mwys%*3$(#IZ6SMiU( zs#Fc+Wq#zV3=KkvaxHxLxDY=Rb~j$Ui*!Dj!PDj5AnMnQ*Dv}=nMX!7np$^ksNY`Hf4Y-hS_ASiF(7G+1i5z>Ub}9YAjR zQis{;6!sHD5NHP7&f-=?KpPMINX!&`yk<-|F~RWClbGF)1_;V!D!fA|5W8K2O;s z3lp@l!0j5A>FnSNQH1a1jjioHL^+cCKc-^srn!0RNwH&DAlbi=FvNsTMmKmk?E3E@`7c96 zq_r6=jtjlxDN<_huAAkV+)prD%^8v2_d;IeTcN-%cT6D3!qHy=VQ%THH)25@Fzza@ zc13)eQm}B^8W5XrbD$lDb!whNZHni2mY@^F01*uF2HTBx`&m$oL zf2SBQo3wRb9_blc@|b>6)D+1yDggcZ`!8rXx}DA{Ga522CLvwTzRX@IYk7MAHGcn7 zP7#I-#0nVS$D0fTx zY$Yo8R23ZQrion6wI3lH2~Wkuz?aTn)_M#Ygzo5Of1w5I*Aj3G6K5IW{+?VnSfJJL zf;Zcc>9c%F)B78690_M6i=&W$eRq8_#@g%cU}C1CHkyGeJT50-`*lHtztHtjiJ!!%wQq{;u^YKQ@Ss5FJ^-m4g06&}Kv zi$MY4c-!XH7NyJSX$!*|2JfLK@-)c7)`~x*RWpnsu9_>%*EF+!@>|_YoFO#T!OCV8ZJN>sQz3`;P zLn{z=&^M4ZHsB>*!ivs%=rjqGOwf@)X5Xo2&~-z5wG z61VR%{nO%e_{@1J+`@J?UVTuGs*)}xIecNO^@XS#OTsZL9wxC)I77lpU$e-5q&rlPt6qD~Nd zZO{eOg_G0|@uql@s-AJ#@SIsgyYhf^*+^^#rjft4|Qsm6MZD&eB?1oDvRM5IQ*&7>f18CceZ|2dm(zMOAP2EzI!H$r?{m-ySauyQW=jA2!?el4urWuP==KGBJ`!z?8^F!(9_?8}}9!mcd|F$bQ zP!W6OG&czXBSJ%A!_Xu1a|$cU0$!0eZy#d8Q+7qe?Hm1lE(~f1gPR*-hM_sn7`q|T zyKRBk8s+09VSq;txq4#yZ%`&R1h7n!jldT>nI+SGiRFYijIV z;^GOdZ;U#eh6gg#Hia|liImX0e%(75>G=>lo{YBoRagWXOX}Q8Q1?5*si)obvV?TH zSs{=ahtO;D7|b_Vkj&*=1Di#+QhD~*zWn`IsuMJSPXF}sYR4))4-GsXt0Xjbt(5U z)02CdIq0?}h0b|GQ4=A>t><>d&*@Gr6G}N*@Fl6eQD-c7@XIN`H7>FH*1pWV>F2_0 z^B}drqc4p$O%t?m-m96$8$Vx?%r^irydFfqL;FJ??eRFVo)kLqdd6VJnd)#x8+_K! zb1>~c9>xmUCYG$(hf$OOS)JCM&6lkKc!-_Nb*NR(e|F3YL5TaM!5e1$p*SQeg(y)6 zXi)n%%4yE5%^Qm1lQQ~x9DC&{pOh@S-B^?YtZVg3oYV1h-G1kvFLWFRbVpv3j@btM zq<3zfB6&ep?&`M<@tkFCb3CGdm`De5?$n-4>xHBe}JWS7dG+(D% zZCWK-L38$0j{MXmjwsOu{j7F$kprf79LwjjTz#^8ib^l5C5hIw7`B@5$u)eq zPK1X4I+{`X1XlHCzlzyJzV;7eO*iVe`Sc=>IOx_ES=```W@e_Qp>@)a;LbC7zCS*# zB?0hsimb=ZWx2(0K1cMTB7hqV2|MTh7iVarsiK=w za4yk#eeL_~qwZcE#h@xebB`F@&OgOva*I3%o0mn3T3r6uxOJwjJ7m8iXDi|A?Za24 zj2fk1c|r9(&7-XQAvhq_@gX6flr*I!`rhl`jOA85SX9ZKpX(*+x-mDYeh~ z%CG!(8~O8uigz0O_?7hdKr+oi%OHb`GQ7Q>%oVQ;Rs?V(1^?(3QS2I!??MBhmXnr< z?bsMI6czaKD03VTMJzH6Qs)`!_l|95?18#@E!o9PkNwW|yBUfWGp8(0Hr(I)mOUo` zfd7f28z)iZ8mW2TeL?=5AbVOYwr{XM(x*0h7FFp~4>aqY!dX&>UrvN-H-BmB2Vt|q zwknjAwYswfN0Xk!dTGN-VnYu~p%8jIN)E$Bk){DHVq8 zMI$*otbt6frKcNs(*A6!?`pMoZ7!W>dj$Ntqr;x1o)y}Hr25w~3OUrkTgs}DAMizHRfvv(?mRsrYSEO-|5NMiX7xVn~e}Bvb%OkORF;M!3WRz?? zi_LQRieNG<8vVGC7$`&gJF7&6>uw}V4cRSz3din;sie8lkN3C}0CBW`qrFk~-zP+T z6Yr+q97x^8co1uA{>!a32gdZ0C3zrHSE))92*} z_k`>-=E@Jj!9h}}PLO{Y{>-}JQzjduKM_ICm|Ete{#XIc^zIf0@7Gx#x3n-RyUeSO z6oF+J{>vY@rvoeIzzK(_zkeeX2RDt#x4%ZAJ5PkQ9yea;e3W@Zys2E(4*^$1hInr% zZ`KGFfp4sIJm|TL^?hvMsrrs1b)ccfD0LA)C6@WY@hjp`n8jr4$kBr24IR&Ql#Na9 zvkIA@jy3k>HNI0`Qr=dC-o4UAh1R;LH&SW3vE<4zu*FAyz8HrqdD zLgvTnGN`DNE5Zl5*2C|LcvC7fH`HbC#dSnJpOo?5o^tayWx>`?N!RZ6>k#}r$aFS{DGpnjJr*%Z*`Hm8 z4qe2ug!ijqB4A2}=oBg7YBnPg*9*!b*gcK;tzfAG+ z`<6tAu5XQ<{DB*$|3@94rsgfi1lo)UTPSV`r(LJL3C?C!p$r*E&z9HEmiF=MVkOU- zm`GrM#F6s~B2=9S(f$bq0}}j3r{Q+mD+h#bivoc7S$!*)pD#g?cspmjz8DJ27)zy? zVWfKmtoxGmRk}=zvoogN47~Yn_v13nQ{vssn?F#CqLD8%B0?L-rYCAY)qM=$v%^Aa zx_63woN=?;(Yi7s+s_1v_J{r;qT_R7@U*+WWr*PAw}*LBSjJ?WXLgQ|jZ+1wfKU0c zQ(OXMhhKVghJVYIz8&J>?eV8`z1R6*Mf9zk#)SONBrT2h!C{waVi)}k4y^2=z2q`U ziE7XLri}4>Xr;;GSGPlE#lNc2;ehZ?zwk~Ymo|_r3%af!E%mTK{+$P1DSi-w(P<4o%_2nmEscq&N-bG5 z2R?vM_-I>=vv+^?K+$&XujEBC1qr21BSMB)0~*x0ni0*3H0rS<&8@){m!zuUfPXFQ zXW!zycVcrR#>)5p;$Em z_rt(9^jJ{4gJ9Vx(;wXT7-o>-1Yrs22FU$%vPrpt@3h)M3dZH{a0WJyyBG%zZ-^(s zFj)&(tk%#V{SDjxz;OaxIGXtoJcsFvXV#$ZG5HQ)ui?}#kXM*;16;`FA|HbHj^=DU zxun{D{D`=|83hIz^~`~;am!ti?+v${LeRJ3%agE!Rj0y*>F|9d9lANpJqhZ3w2EmB zJUj$wIB&zkTgRHYI6KI<{sjuA_8GnP!!sn!eOpiMK zIodt(qcElgd^WTbZm}2A(2V(5JwqzgDvvpD)k1ON4oI<0T%%hxfvM`3uYuy1dFcIE#nSRJ@SuAYs} zT?oiL9z4y`+2-H&@Uvua?NuXEY|aWb)Fn?;II}obwZxwb_?91jxIV4_UF2`ZB&LP^ zn}so+;&OUz;k}*A2(A0Vt*v( z*E{Orw{hE6*!RzT(#B(n1hYf;gg+t2k9GZ0qD!%Frb`p9n%MGJ7Qsqc0h^W`Y|=!v$TjRd%9AB z4Q0843Q%U=QAv-hPqy)9om(Xxra*fT4ttS=NIw?JJ_C-} zu&~>yB*(f9o)Jj@!ej%8s$YuiSlFpKMB$r>iGjtYx-F{ zEFQWY$zvyZu(SxskYp(d?1RWM(xIsC9S9vpNnCUjq1s>&2ZS&dJd^!_i=4)8ULYiu zev11ytjj4o)x^lEmUbg7i}4r9X0@b17HVfW)`Q;A9+Y}p<~y`&zjF*u7HfB4ORi%C zQ*(JZNZL0W!KbHA@p}wnb?>LSeJwD1;ZecnJ{D9u0U=u*8#+n`5UeudyRD_JXFvKp zit7XtT)APb(++73gxv7Joj1jk%i=FFPU<(R7C_|K7~Dq0zQBM z;|Vfk^?|~^&v$m9q%|rA!&326jhZ=Z?kYJ!I`wdj}DLI^Qj69Luj|8oISH`KGQ%^Ubk zak04coKMyXtW9`Ow-NiN+UP}*NInJX5uZ){{OF52b`^MipoTEi?A7>^!QC|6B=9V3 z^YHVWj4--pOyi5{IHb(9481(+F%$LkgHUP^|G8~nYe1Sa+h7*&z?WD=rPOQPSi^ad znE#fs>wLamcO~hm1)t~wGUukEUF92se#tQFs-W?G(a6+lQ)?fOslmYuYKsDv=3RA& zt$QW!-jplvlw(u*oxH=Nr0O4f7p3o;Sksz9M?bTS7^O=c4bd;!IJTTSLgStGD1g=s zC68|C7eBLN88ho))+K_%Rq&4be{=+2K#K7K#Y50KonuOatzkorO@A3xG567kv&F0z z4F|?fP=)$*k1{A@CEelSXUc7Jqi`2boaQ9TRov90cp83oq3}vk2viit zLG)bG9T2kXUvJS9GBol1Dq-@Q?aA;S?Clo3nl8BSyl$b$+&dh4=3|5z`EO9u4GdJW zGFm)`fG1;Gk!Ddq5Gmx9)wc><(JWH?bpWRSh?<%)M|#@I!NWdKpkWUds{}XA znJLy(Cpp;i+Eg}Ks9$%G) zTe^qH#VH-cHw9n!%g3ZKf>f5P$~%QIyE`@7kLHcmotCX?o_dDIz~zK8GsyyYxaf2`hwGRmLP==%QKWrd{Z#>=d4Fq<+bWZ`c5 z)rnAFM#`EAws&mA=O$MLD^Z+Ri&YnuurLpn6<2XV4v6|ZRPwFFKzahR9}sFhe7})| zmqNnU(FdB0)!sbdSfNqj7<)DLr$(juYBB_{D-%$a=dCFHv$GhJ-IM)Du$pe8Odz#Z zc+SjU=iM^DLZ;i(^JsD^mdjoEveLTW-RB&a5#1+mJdJLOhmY{KNoFSL56?m zq5sYcIqhf7>j>-jLLefeEV_%(xrN|<5oz7Nf$}n52-8q+xVvK0G9G@JRN_x>czpmw z5C27AmR#4NxP97B5==6CdxDCyg8iKmGJB8>M**cH);Hq2NABh&-X#!dB?uAUD!^o(+3nS9QPEu{4y{rsGZq$XGa56PyPwl5Htzm|y z>Bvdsd_D^U8*NXaVI)!2N#GAuKu59fM&+eKvsW5r{gNfnNWrg1Ra>ZKT&Q19M64td zh@%1SzoJbf)tr|mJFPf91VdFj zS2AQ=I0zn!EoSOOE@=8IV7p{XF9>q6MkGpc=}B*<52fGcvqRt3ofZeEkXllP=<6&6 zsYd9VzQ+_xTgI0dl@k3tZpX_<6h2%VM3|e$CGa1N1oqbxmA0M+(~bco%Isei+e72; z&;d#+5;Z_Y9ofi|dlyEo!|P`&^qE4N3yoQS@VykG;2r~#^p>8iQ720=;^1LX?_m_5 z{+1dyu%Zfo7aotiPwwOAn^$vnTMlYSTt#(@^M?!a7lVPZmdG~mez}zoV~H*ArbQOobKt4zdv(ycwHhu*g)P}3A;d>?kSDW?_KJjFLdk| zu%);U$toKXez;5o?QiDqPrtP~1p-((4miKfh`u*J^MrRL!bR zk+a0y#%A)xsu(o+0)Q{t$^=LNyT-^ZbM+Lwy044)?J30NUM-v-!zAl>R@2ntn^ zK9L~@Am8AK+loL!{@Zr83&UiR%;Wn$+ zXK*OAIatq=pxOxhZKTp@gM+a?zqmvj%KyLT|zX`}66H54V)3a>9?UbgzaAcgCTqyW!B|JsV4r z&6Hr&9k=<9l4dgjL#|hDI6W0OlLEwXnb-yiaKWUmY|*eaT*D({2Ub54>KPj^nBWAi zD3TZ7Lab;S>voQbzDHgb=UUE+S1aYM#*bQ1w)2I~;0k?s32=P9(lK8Rx&A)-2dn?BRbT}pFZB=8{O*B@N9fWlt73$b=JFcgy|=TE@B;gv@}VV#0E2?3CII*R z0}fZ>W4=3FSleW)(D*1*zmdC{s8c`KjMm);!Ld`6{)o#q2f=1oVhVsIxIdF}A-gRc zNCK12IUofDt!sqq%y0^TgEy{{^+q{;>{jGXpk*h2z@y{$hAxf&B`j;`(Uj}mzjEk-3)O(zGHf#&P$}a|ZZ@~&lXS}2n zK@KGlewDTgDRcG*O1Ek7I^}PLb}E;CuM!@dYLhzJhXFExR=jSPn0yo5r;#fIdN3Hj zwh!0{)z>P<=e10*78l<yb18i2jWhqs?0Ps7JR3?U#umpzBHB_!E0Q08O1n{~~EUWAy*QHTwg z?kJ>~)k*1Vhlve_641idRI(-+md+q?P;p*Q^T8|k`F`*jK#fYiee{;`HMyCZ8vM)y zpYRHJWTnAspy6reCHY0Ewt4q=+G)y;8&CkmRi{pnkPPb#{qCIM^3;E%;Vg5ymSWzN zp#}YB&L9WyxTlk>jbbrvTplux9To0tF6{N7rggCRHz#a8K1Hpjz#ivvJrM3Jpn0-Y~dBt3$ zq>OkUi^^3bYBgcwH*iAQaH0YgkXH!vOa8X5c6a9(Xn@KuwzH_NpOP?xLhD7Ii8}s7 zd&Y2hQ6M88HjT-@;X}+QuQwlf1i(o;k)AyD{Mj;N6^DVIU0wN=r>({#_%2ob*%oeJ zoO=;VO~u)o#j~Vollc_jf>Q!8U{l!r^#wt(wnN{FSoeJ-0P|Bfp^vFY=bc-d#YN ze|rTCS-=U%a|Y2GU#M`Rl*&X(L;PQXK`}E2I64=_A@khUCj6v7-wmrNfW}&{Q)%n@ zl;{{fe9gvZa0RnvrdL>x>DP%um{_|bE`gg}Xr>19%V;;ln2(Cc{2NnFD1Z`5NxY;f zV`(W@gw_b#ka?@9#r9VrsK&l($LV}B2m z*izB;j!~LpuPgC4_PLek4s5y=5nbyc!pHD)F?Atv$|2uWJRny}09&3WxbF1|!iSerl%!!{|0wBK0)t8UaJDa$3^A zuS2AmZ`Onh!jTxBkr_|9HR#NY9-cDAQ$%$VByOOK+`QoD!GCaP@1KdG`w>jW~wBf1DFF}A@{^Jy!B&&RLOZ?PJ zqJQsWiAZJq0~z6wpZ+Y=l@NKlr^j`OD@|rPrKbb0uuF!AEv5_A zFsUzar&N)$!tiH1BS?=Ta6|4r1Y87#xR0Uyw^0G;B@zSQKjJwtiL+HvA%u92hTg5& z9@C-3jH7Y`j_~~_&HmG8h|9nvQ}pixR>WrY()WTMRAd+HJGA48js~HeDGLJmlVV*g z5mkmzPo>@_yon(4k(lD9y~)9R>L3r8&@Kij7C1zsk4X7m++ zE(^}7${S?E+FK>E&pDRGz>Rvcg}IuS#+K7^43BGi!u>*QLG=dm1qm+%(5u+Pbvs zU2G_=z%0o9%~h3iV>Dd-Nwn=5{g3eKPgcMD|MvvN6vvY)R@hDV0zbbG24*RJOJcaf z`%D8O!}?Lg$osZK;*edYbfhB9xYw|>bKcqs@Sa7pi&c_b7C{_ zD!5c34gkEC8L**m+R;U0u%GPmQ_QNo>x91;4WM_nxhP_E$aORjIs*OIOGsk>6*RE# zbNkfzOf|y)el{AOk9i(Cm)tdha{&v_=d>bz^0&6w4~EL&Ce+d2bOiAdaUZiNEnk0D zFlT~6H4hweimnF;bkZcV+yiga_=$ZUXK?1a02<67uq>=!aVapx?uv(pl~a^skDmKt zGe23hUKvRjf;Ij+M+Ok)Y~Q8)qC~JI8!p{ZISJy_!DL;LFT#vR28HY9GpD%nzDK`P z1il4q(2LKeMhRl2m5?-97+ar0v{~PObNgdXL!p^uB-qs8S#L4`v;EU+C8w6X?`hie`HU|MFB}4~NX~RVEm5tvZ zuA$IS_)k;^N~I402Qtacf|oFRY&MNrk7CAc6u$AgYRKuG9`+F>+@(GR@QA8)WMpcU z*V2IdJVta_H`F-LkNCzpm(XV_U3pWfOhFWyvTKw)arNUbgm2t>1SyQFvuoj2!nfqj z^K3Qm=TBHy7%UVg4m;s83}n|&0+VJ&CBO8Md#B!CM5@JfBXT~O)vpM=aU?S0wIN2~ z-g@V=f7m|Aarqs%u&44Aw>^CRuUnq>XtjfRkDhXwv78m+Mf9<>42Z>%v4!TpZw^W2 zaz@J^Hg1)`3+<1F?(k{~*&LYS5HFmG9QF7~?(mWs-vFL^#*>nG4O##I^Vy4> z`u}LSDz0n1Y8(<5bsjVAC9sahfIs%)cRqv-(muzw#fScYExJ-Xep)B4#3Zk}+pYFE zCW(W7jLAScBB+X8$%MyR4bFBq^*Wt@E@duo-@xRjt{-w$YV5xQ2czJWd1d&; zq7hxaAQE(EZx|eZ1Q65b@6R9EjY+D*t(&89ZB(Thf|#8v`?7c>QziixkCJx)PgF1-g2EW79f-qCm_J=MA22v&KDrnJljl76jD0W#G(;Bo)B*0D*16CRoaABnw!fF>+6#9jZGSzr?z23c8@0j0Oe2 zzXC+*4zeR1{^Q5>!-kgvwxEbi+QF62d~q^T(h&y_YP$c*JxkbjEu@Y_H)D{1SJ4?_ zyS6X@2-pCY;Mvw^IYWmi>s;)6o;4l-YsM_FPlH!VSMWInLkIsX<2nl1Ks;+wJQ)+( zgvKxDrr!fNCqwXZAXmhaaGj=m^4IUB!TmM+F;(HaBH(8buKOF)lw=s>2Wp{}SV&pt z*28`4^5V;@%G{eE>f81b#U>2E_Pcv$PWGsJDb9BC$@PfGg$Q1_8U4zceEmx&sq1Ti zTu!POjDU@F@M|hx9^@fus5~pmG<%F!m=7-G-s{)$UlTZ`mpIsSJuO+Kqeo=CY5%lK zbHa@X5ZKw!HyY{8$us|iaW_p6WVfVV2nz^`TZ<|AeVaO({%+j$A#~eBX$NbrQZc4L6*C zL-p(St_W3#$Q?fDzqk10-AB}o63(0pK77OXQUv0|lWC=Hi`SrK)N#qbPyF@AImvQ6 zAD(7E;5YCj0>sho>XqH(Pn|~{lw_e`uu`-EQG0#zD_5GsX2wn^p{8WK>VbB|(zJ19 zL0pQn-!Cd@g0zjLdnkMn2 zHl+Cw7;q$>H)o)qHjV~Qd`j{K9}6*>V7*b~ndmJo^)z^pJTXl)yzGph^oE-z#zl5ej+k4_(WRDO#AOc9n%klY1f`^JT>byYpWqL4YLJsF`D7O)i8#Kit9 z$RFVmUhLu5-au&F5;2Ab%Bc})j_i0qpZ<@An{x6GV*S5ihWHV1?t{~A-YD^kwM7^{ zd3hPCCzb@5kUoVbtXc_5%vZ_H1aZ^t(?0t^yheJ*o;9qQICfD-EV*%qX9Svk5e!xg zq~0czqs5PmW$u&J1LW}Unf*TbdZAa~D7}EUeh$5V#*QGw8_<owQ0 zRh^e0T2f6PsOSBe zKH5uSTH2>WrP|m3BtQjzRML!ESdnxCQX$uB8I3Omd((v|Ty7L-08Ni{IOtvgcWMd>*VCbqW)iLJh|&vgTcLo%S9 z%_cC>lYUkT+2va2Qd;1rGe{LC3^cmJu;GUj{h4UaEbtN;$tA$k_ary3DcmgDPY3rZ zSs;d-I&Wpzy(=+JH+5jhwPh*wrcHe{NoGj^E78%R`35d@9#nqGyaBlD`-74%~A~C{a}kJOu0eaL_xej1Ty|4{W^7Q5jGft zaQ!87doekdkZN*r>5%NOub>qWCWCz7yPY36z3rju&$&$gQHiRaRoIXFvKx#GLZWd! zsmw(yoH(!A%$kc_xVLc$qu5c)VP(o*xiCmCbHUZVp7t_@P#HrE-`LYP!**Qu$B9p; z$$`$ZhLOz{k=K%AY9NbWSEx5GwEqZNKZsH`m4T|V=liIwaLi8Jkz=@XQO{j5x_JQn4G_-E=(*uKncpSWc?w=)K z$$I9u5{!A7qX)#1O0n2&KfY-Ow18Jg)JBsz5jF!a`!7UP2Ea_xRLwk$1HQjB zPX9P|y4FHJcm7;t2+~>C3F)?`b*CEKU5|1~G|dbT*dip*ZFz4)l`%B}@_L>fzqM^z zuIZux3*Y%W>eHk~@udx}j%*yGwp z{=;WtY`{bk2T6$OI1&{_Z7&?`HW{uym&_ALE2MFG9IwkbN}bq60bZnlkQ#j~bMnU{ zODw3Tfe0Oo&_D2yTR~LVDAz7L^`vi2pT}2h04WRr|M@09n2MRp zjmcNaneRS429{D)6Vsd1{2(=ZduHiv4}JOP!v}pOjyP9gCiD$ehMnUlzGy;AhdSDV z3JF9DXk9sXeS;Lso6 zw2`J1bn+r9mco%ir8ZWme3c*T9!^2_-P(iCFg^oZ`o2I>F^uW?zy4cLGkmz|pC2VS zwIW^41Ef3Rh&=|EL_O|r3D1@%o1nI$NzY+sAtf&L* zVkMLT!$HX#cL^c(>@t(F-E%38hdxSjW+n13Pp^O84iSScsDK_ycP4t&fA&+5JDkEn zmg%+@!~R@F!-9i4(2A@w=774Blij?7xor9#0f(p^tcwom z`F7uutIYV3tLUuY211ih(&sT`aaLjRH{Po5U}8MwL#V!G=XzuXzoUH|P1S-QTmphb zED?!2qf%O4w9BRb8!46qXJ;t znj$w6Q4+S>MclH}^^apLrmUnT7B+~@zgNJYcOH99cY&E38L!5D9WtOwr+9lD`H43E zZe}InE6+j7uWn$o3G_b!S#T~@&uZLN!k(b6kVt?$9^S6Z#QBiw9@i%fMyNnI4D1&6 z54ge*R-N4o(3nS`m{r#V0`DXlg4*;0Pgjd<(5v_qPH+I-?%k7J^@|Ai^->_RAVaxzeAV&0SWzhBWJ| z422HmX?HQK$EUL?@xnxGm-cZ11-P#p@1{#jW(y|{LPU8KMeqN>+t>FmwUDH|8}71e zn(Hr;P~=%`6XsTS^jA;!C_wBcU?Ih+gN98zD0ouj?uBSP94qZdkjf0F3%SGJ#}$mp zgWU?piiqMvBT4-^tWC}YXY_{AaLv77U6kpm9$VrU^y%?TjkZJV8F~IK|d-JuVW1Thw!qzR;i`jrV*3W zNS(QGHD6#j&^L)XI16R^e=Yz5Jt>ejLy3u2uJZk#{q~J{CD*p4QA;KlcR_#zf$@fg zB9)QE1L@kx{ip#8r#-@{Bk9CS&S1cpkM6<&KvV>vI2>>Kw7we!YQMi5hS##?{eyK-1SN`8u+PK+rYHv{0ZzyQteAryo z`ug9VT;w~p{ALCpsk-meiHgzqr~5=x@_EhHTPUFP%I{bm(2C! z!VWeK`>y=m4|#|*_G9mjDJ1$qgnBa9vK%(}W$dQ1ry3<_v-Ek5og@ffac$zW3SOZJ zp3mG#>%YMri@m5xg90D5oDj8V_S!^g*Ua6|LkmqEyM={v0JlH_Z?|&rKPAdS&Y^$t z&3p4h!N(NCh3w4-0&D7O=ciM^8iRSP~Y8J*Ev~ zf+in49L<~);kM18C^#VH(uAAlnVU3oDM%<7zNqA6ym)DVGNOCtF|i&H zvZh`{m2X@`p6TpGwC0?%ImXR-=C!XSsPg~Li(gDAS;fqmn7s+F%DbVBQ0KmQ@)Pc; z(cO7pl+J!+PWm5t9L>RUUPhYn^Y<73TR;Vr<@aFtb(HLTXZ`!QRj>zLp9t*eJ01M! zg;K~hmIByE>l!U|w89siIdbXRAoiMn_GWu8ihMZ*9UE7LKDo&5f5J6%I00wSg?2O%t=>kILnxSFyEwODOxSr$ zUGOt(^K6Jk{W6}BnBvUc2Y6!?%)lPl-^BAlUqo4myh7o2g2SpVbo`gR?KmzH_hxtRMoz4mz-h`ETi4F{jkIn-wWUSeFQ#ZApb6(XX&j% z3y_cl5D;YjJ)@n=H!SFpE*yon+a|Oj(S#z@kYj$Wk3QP)PfZx*m6p1&OSyH$*vGYz4eo#+E#V;``q zU+2nhKq4bdA6B~mp+5b@qnl%;9s8a*QKJ@RGVb|BRI24=-r4#Y61YB=_-M~x>mt))$J}9t` z)45PA0;Pu7K-s#-gAmeKqAVBm1iGuGcELb^o1pqDH+uoHVbebTh<5f6{Gh$u_%X|=i z_wNEQdmCC3cj(%JbtWijtzrvbk>Eny1XbiIh;H^66kF4a2(-hUqNY)~ynTbuhdZxE zEyxdPXRZ%lKNu8tn-&PG#|3F)>EBNXU( zpO(2h`3_n&EV}JcA-spRAiIl%J8j+&i`_nxd3*-PU!iR>m+PgO7*oKX`;oTIn1{T_xQs-tRp$)`khOFan;?DFeqC5pVQJ|M6(2bzhI%110#2;Y%$M z^sQ@hn|Cr+76dqS0~uwe+Xr~=LWzl1RR|B>h6KgMHP-$Cm5voq`01+iNV)LW;Zx za5Aw-jYOAvI|ICjB}g_Cg#+I;U$0Vg$w7xCYH_vcHGwm!&dg&?$g#x%O{S0 z*M@R5n^FgUhd)(if_b1s@5t~>45S}YrBZjgY_yYb)sRAjo&C;lro5Pi8*CgwJZ0Op zx+uU1*R6IZ567-z7XLh!B!CM=_L^P#ODmdY-}tZ8`xZE_UZ4!_^)Xmc&@R{oKJ*I| zc&J$IB&B2wpZO2&Evz?T%3GGsR-_^co+%#h0-?vDC%L^s`KouObj!V18%I9Z>TcWB zVtlH|?dQdL{srW+1ueKhC}XOmI=u97>=I4npV&!n0i@tC2Lk9F|`HGT;mP-RS6kgF?W-T3e;ZLdQb(?oDm}6Jph=totr%rBl z3krVs{QNjg6nFvdSNmg8O`u@=o_Z^yp5G|3VXtoy)dr2c%IAkSSD{{qpOK!RJ1CFM zw;~Cq-#kd~?u3|^>ELbu?=Xz3lA>`HWC-uOe(zMiEfol@qbu-p9#opM(0nFFICk|A zVr#*}+@`^L9BmDvZ+?v$@TIU-4NErr*nZm+E)GGrLNcMFqU}yJn|f~B*?Xnr01W7n zr^8>2ann0l;HEPbES-tQf0>d~2Se$;CTp|4tiYGpVv@6PnCR^@9{iB$npF3xy?wtb zDkR_`{d^T5Qe$MUP5qT-FcX&H?@0xe-ZyeiUom2fcB|jAYUfzPl+8V@7k|J>jam8q zP|m``{YgB~KNfxqU}*3?R&dq8g(#VvaEc1Q1{~@M#rbx=SoGz4iFiS+?Xm0sBX~*& z@0}0`w)Jm02pk4h=XM!v3p#X|l-rfgyfX&f&_U;ZO`JRsvJEOjzhS^#-34;BfFRPi z`w*;#^QI!A7&z(y+>{hzK8xtXxnlI{T2j`@f+i2LlqS;cMr zYCMSScQ1Ef)kWMDRuNVeBKaLEQ4>SdK#FFs4gWXW;}Px)l-F=X=iWfi#w6t!MpWHY z;09|gSmM}(Q`?X4-5ezH*A?m>v8y$vBllt$q` zt7Raerst$>S4qp82@6*N4t&wXR`P<|g8+;GN<@n7jh2%Sz>2GFbjGg)@jbLq%#LXN z-3-b%hGI-0b%1~VJFC`3-GG3VcokFW0U-mG9#YBk1I39%;a{G#1CcS1HReoVq(nB{ zt`F2Zm3knt?n{cPh;zi)+PlPC*yDq1Lb}q~Gq=kB&~%n@? zrMm~|?vxn1ODV}A1f@$s8tF#qe(&eQ`wQkc=DuQId#`n_=8~kjY0WX7Q`YV)Ffy4$ zn1t&$?v90!TC8RVVZqM#!wXf%Oj#<^ zzjv0R651X}GHc;nJ-DK%y_FHtYcOugj9B)$h^K>7Byz$+-xf}L&)ipGCE6en+ic3K zmntO!L~DmIZ>175&=EHQP-Y{1Mn?E9`nnJv@CEhJBd9zCOlKYXaXrk6Q_dmAC?<#M zp&A7Nh~rh9EW<15bY3MJ9r&GK57aQ4mH3_A8~_oknxCJRgi-ndUYP3n2BDmni%((+ zMEssEWEWXK9lo*%J1<2-&d0vM>ariO$#y|r@K6c9{!2#5oOa?%R!nAi%T81-2@jD9 zD5_Lk*k}j>Ko*?R%*=FH`*9dT9gC-0?driVR>`tBeC^y*Fnb}pyNOE<^?-^~P0Qz8 zBo~D;VPkyheI#U79;308W3nFDfOqu3S8(oN^J~uI{j3j_pV4#B@vyfhxEkS?&53+4;SIdN(q ztns=taiugg7-GsCXZNqeJbQILV$j^DTCR~z9G>OT6C}H0|FMo+LQSupZ?OZU%chS6 z4VdA4dg&4CujI!JOrE!Xt}2JEe%?kPjlu zuZcH7H9&ew@onPwV$X+h`s6&pwS3|d@Pc=Qq5qTdlk|)Rme3JxQ>ehiJ zmhnkL#$*OMRqZZ7ZYXFt<7Awk;)(-xTH6@h442+})(UdN1g^5TYPnfqy&vZmGc{i; zMY7BvZ}0C9&?P@Z{)z8m`~(VOkF(-pcsYcz4KBn;-9wqr3N-E%5&wjWdv06S4}vsP z%ZwvGN(E?@q&uZ;enQwQfmzy0?S6{}IB_(0g#G8*Q|#B(fJU~|$$-0Tx`(OyWO_sa zCX#x;kgO?^slS*GjaQ*6tVw*(3x6P-H$rOmin4i~zQsoTb-ZDs_?NJ>ud3{*#gJO( zAn10BgN3sR3Q$iTOuC#Dt`1$hfHhYhk=oY}_aD11F#tYv9#CFdpUr-Cexc9+7My6$ z*AjHm)&XxMD|D{Hl0>2ob$JOX4eZ23^Yo73C&ELp4=IN+rE|$MxK_05u1xo_fLj6W zOxXKYtT~t}Duhf*5=9qNz6=ZD5E=4p3c-SykXcERAs&ciXem{4p@uGD3@tcULMNTM zlHYFH7J>HJxHkoE^EqvEfQ#x-WEx>p;`kcNQ#a8_v1tE7 zB%nUSzn9DYxn%!csERM&b)5ejjwp1*_nA!desG$60^ii*UVAE$BdK8I2G|hDt(-~% zwTM9RUUeQniV4(^2@VdhqpI5AP&5Qryj$@$1kBFen-z46CWYDqLb96(Osj^l8#gz` zMO+JQ<0$c$_k7{Hb_54r_$@mm8VuQ5wJ17y2oE&)&74bgs##qI502!!E91IY2U`L? zUjPP5t)+V?=)8u>hx#XDW*6Lctn}?S0^)+VJAJpE^2o`|LyXxLtN3eiL@Sj-e%|D^^*ft5?{H$wbt)A`?@|-Ak7R9NLC?HzSWRMP=fFzb3 zYl2+eFFfIvvSGtPC5!`m&DEahnKp!pAZpzLNto)? zOF+)Va=_Go7EOf#6heIQ4NsT4`Rejrbx)}b{2dX{VWT(J-SP&1dcC!Sn=4~_$nz^u zzTw5_qSfSMO%=k%njGxuQ7NqUfR;;56zGX`C)VGLwa##>1};E%Az(o15QN+@w|Ih8`SG zJ&KFL!qa$a7*qXLxkqtJq-HQdASa`!jrf1@tWI`jxUs4dVt8>vpHSooy+>JIF}3I9 zrQrNMbbkcRjh}qV5gx=6&KPs!r1h5JD%Nsj6pYlL!?}Ws?8Q;#x^tm?sZ7Bei>s^b z`+!W&M@YgWNPtEshABhBSL!czc(?)Iwh2|^qb*n@mIsMeo3}B zcn{PC=vd2?jV*eQCz(%y#YP=QJpCaTOH?aOrPm7q zdX?W%KEWoEMqPWH&VCFRNjkR~cP{m#Jm;zp#(WY$6C{NGq)lz~%2Qmu>m5-dQ9SK- zmJ>~`2EsCLn=eg75$5`9lGpmjrWKLlUGL^eXYPZMA4=7stM9=7n5tISyk{TSvf#1= zFvcugwBmFu=!AD&TRC$OHgIG<`=98MM#Lb|+L$xQv+^VTI`#o_|GY(axat;|@J*1wib7BsrfHS_Qj}a< zRd!s+=}RX?S@4$5h8%KZkqX`XH_Z-c5>QCMJt5c+IwE!^U#6W!)&ILr0~>%=jjan} z3&?$I9S$fY+>;`5lu~n?$47y;7|ERX)-)A?MA1l9HD`M*(ZCP*yhoX z$)1CEKsMH1e=f%HCji2%H%19m4V4f}NoZV8EYc^3&A3-=BNTb9a)$zmx`>U*5;bS{ zPaLR`s5lO!zBCxZEdR*xzEb(!bzwmjQrhg{x-OB9-wESK5B#8S@1`T3=8O4`d_5#3 zT16R86;vQ(eCih*5Z3do8_BnhxaD!^tu=;5Y=a^KR@0PGXfrK~t68u_LHNmpO>;pm zKh|6I`6rTp+W2-t5p1Em3OY5v?QE{N;JF=3?RaRcmVvf47{@T`%Oo8zgAP4FGlT)l zR@scdzRu=zcY|x0t*MzH;==;=)l|~r)NWehYXoTHcHdj%NxP!!B{un0?uIfgfd|VYpvo1x&B`b#to$_r=`8~NG9)_lZ_7~e=CL) z0llSsZ!WB#+FGxTBH`WxJ3(Q$e!f;uxzeCdsQ7No))(2pyOBZ4EX0K$e1J<#h*RCy z3&66a|7I&0l#t5YWuDOhm}~oJseqo4_rL1cGtW76Bm(3N6j2aPslfZv<%>AsUXP`P z2MThJ8Th`I!`x=YlfT%S^cm{Y#dtB`qpdclM9;{;$1jA3DR;*b6|1T>v|2t6AX2VK zCzMK_yIKugpQQD7*XOlmuUQfc@$omvE3`IY=QiS3GeaD{TERBBdq40EQ}R?)@E0sq z71sRL`y+lm{%A1@bt3Jqt3+CoDOOIx!V_;?P%Q% zZ(Hz)z7>S~7<0=iNQ#&WoB?=Ax~KnUu<5UJYE<`L!@+|@Gbx8mtJ;uusIwf%| zfjjqbia=ei%lDUtY->gQD`EAkM!$Z1`+pb z#N|xNk;AhMdSU9re|M5nM22XX#RuFhG74m$X0@-nC|7cV! zH4(}AqkMRvJ~B`#zcCa`jdz^`kmQM1i}l6@EnEjfV<8H=&MeKzCV)OKKWuK|N^$_< z>QgM?MnHMS&Xr<6{DR^Yp2#(4@q_gYt;#F&MzZPdWT1zIKG+EGsSE~Lw z+aj8($!!A)zmaooy55T}xQF7iG9bh1$gM6#a}5l=YNI!-Vmoz$)?T_13PT>=GC~@ZEF=zgqgF*MM z!Y^<&e^#!tUVNiH(*?_T#kV1<2N6Nx6Lf)|N=1xD4=-dS{0+lMpZG}dt_MDn--i6- zyi4nAeCcjQ2T(;nhzv@j*w2^T{sa9N`z=oS^HMLZ3F%(6VFd?_py#9wtoTb(Z{sWWFb4O%|ZFQ^o6Ij%Ou_fDf1D z=-zjylK}jibX>HMuVMU(n_}`h%P&JNtr9aTG>ZXH;_q_N*kgFesXKqnD-BKd{DrAR zd9AP%r5_B@#H9Fhwx9li@<`4S74aKWLDC2Fh=94sP|##%k3ZgNM;MC+ge8sl@2t#h z37a7e@sgtU(}%)V&n7RngGuL{y9>5`iHZ*hJ=m0&pi9xvz1Q47?1yXC2X`%q_fU~b z@s}Op@joF|%S+DmS`J~wQ{!tT-z)VOH(j-djf>|?)a$%0-a@l=yRf3B##gGuT${FR z$;4jnXIVg3((7U%aPx&DCO~YgH4;Rl!)d;laHoV_KhP6iVzVw z*_NUu$sW3X(8^!(`lC9Wa7g!`!JhfZuOG%%zqLYBdlguw9l5ZmGJ+aaS$D`x7U-+P zRd7Aua%-oqg(1vT)P_uOzk#hwo>*PvN&C{|Ke(>jSRb|NK(If&9NC>+@;oOyo;=^L zFMZY;X+XNFO>QO{T=Ux!KON{CfT0%T0;fFMq@LMH-J_*#bn=Sy3^s_|aDlL7Ma0`A zOyM!)E-j{{cutwMq#QuD0-u+*)I(YlHg$di;f@}3MlH`-HHKP?#=#llg9=PZHa`eyszyCz(gLF#glEutc1%j?b|ToUr&?3G4n=kE4GP(9 zK4BMC^(KM!w0BsZB~vMdHQ;}6gL-eo`A=SM&JKQ-4+I2;4efhL)i8mWR`nqr`vcvn zlgEfm9XXB|7tF>moPaD^=St~9``&s1d$I$_Qz6_WU%+IcXK!XR(zpa|K)7w>@(_!dH6iC51n+>B~ho-+H1l3 zbhjEXbshlq`(EIT1u!J=|7s zNprQg<3Uk`b&f-Z1x5y1~11UQ9tVLldM0*M#4 zD&Zn(gDO2Tw*4G0-3d6V-wS5vOlbaAjRxV!K>eM$nS=1<>o?(g83UIdzlcwSy^0M= z$69hyA$o{rUd=0@9(;U1L&@p6Yg zM+vNQcBeD*6r_w3%O2LLY|_K=zICV2;3#V?A}$}D^=$MAU<)sgm$@ucyLxTmOi9aw zpp`zTZwRVczTgEB2Xin^i#@yL6X1og_jG}5Mj#viwoU;*XM4#6b7!CDGXm5?e$O9E zWK;xT4FZkv0^X_b&kZ3anDeKOo!Fh#gq$hqr$Sg$=0fP(?{%Le;LnN@{+aEner2=c z!r;J|c^D^*Z(f2VOD@ZlkzZG@yc@z1gFc7=oA+8!0|>H5dBy>@Mom>^ zwmdtMV*g^5B&Ay?hWlk75^y(*yl(F`n-r!f^@7ox<#^}O5#cfq^{)4{5J-8bZWZb{{ZZ>9JEc?F z*tgsb8*BF>hP%T4bJ)ry%2QJJFJ0Z==Wk}-f22>_Hq>OZ{oHr0DOwH|k&6Ebk}sU3 z*J*6ZOuzgQTZ8)jVaAc*vh~P+FPFJWjja(WZ6{=Ok%~&GSqZcrk9-=baI0K~T4t=b zB^Z)7e$d$EzGYW^8giwWJUMZ2%SCXXEY<3c0QLZ*Xu!JGK6=n-GjPlg3!g-x1bO$a z{;@<u$+#O~#{-{< zp(0(8R>7A2W6e4x1Ezw3DvR|dx@d+^o! zy7&h1F%r>M#GM}Z3I*a}gnN#7@inkq?wNqjh_N*S6g{O7_~Tr}o(Q?NL_gBp&}ly3 z^$z&cQGS-B5d@Z|S|!N(v!>vum&l!jo@-WiSb0&btV|-dfpLWAc)BfsD`7 z*BO3(=Emuk3t?h!h%nZ0Lgy8iu(s+&g+BYfPygeUWwPREqh>MW8Sr+%n5#yDr^Vi- zK_5M6DT3Q}W_1}lhL;t@a*#_dlpJr8y$!Gz zh|z~lZ5f76y6vorxMuw=;H@Nwox%uuh`~P3M{q}~%*-^&J17>=f5~VC$Yg9U-)XJ_ z%(M%kpkBqmr5DbXng&2DFeAJ4aIDXOR!AzWrkHXW`cCn;4t!8x@i#$y?n{2&fB5;7 zuBEqeCCfwGrDL)im#}oHOk`tSYST9emx6dKyiM+KcetQ^5&0E4Dj53Jnh7J9O93C0 zeAcpvhu)vE%KW@d?Bsb`JRpAL6!@Crai60g-N&V#22l{_1^v+RoK_bQzFwjWu_8vt zyQ_X*yezMIr&)Xcu{MrcWl`|&AY{6uOW>LQi1DDjuyT&alsFkAa*Xm;LeZ#0!ez+Z zKLBlEWP%DZDTMklN*%@3vsP;w^vpoMZo~-=g*kkp?q2Ph{}`|usjbB{2m9`bcZI5X z2?zFe^}L`#Ht>H_ky2Uzg7{v)(F`EQf+vVVpxP{ETvF7JbD|vB~6CG1fck~vq z_E1|^uV=3fN#sV44=-~JAn4N`&y{dtfm#s7pu}#$5MxY4$MJy4ZS`w@@J`>i-Q}Vt zPYajsq=e@Lx<3L9^l6`R?Hc7a8MDH$uz z`$HNW_x>H3JGjxDlrrPM7X~p7TDR6R;bwFJ|1_Z5(R9)ZsPBWUFMuhlFY26ey3_Zf zIYc(r!+pqqUY|*50joGwEfoQR#s@z}MFvm`n zg6A-It2)jBqs(qF1=8-{{Y&tK+14OZMJ(nZxPAsM7^ayRxBO_k#AUc2RUI10{botE zUjsX%qfv8NZw6aw)W)Hpp{L>FpUL4n;0_rw@~|x80*HmxXL^jSH+8GarAck>n7g9P zhyn*lz*JzuWl9Z!obe{{Kf#ZWAHm3*mhTJsD(BRN(#)(lC9pHfQwC{5DWs(FCg}Ho z+XOR=A~MUfwPC3-h-Mc+}(Im_ksa=L+dFUr1-SSIrFI5}s4eC{ZN5`cq7habYCzeShMdpVka{W`=O2 zB+I$w;;#>FKMsiR!;OL~pg4LapqEN)J9$1s1G);C(uYD-ddq&fQtU2Mv0EKPPZ4(ec}Jk?bv%@&3Ok4jp0GU2 zwk@9i-Mtk->-Pw-QJ=5JygqPx;ZilK8J6>W zhgO*3ZBhJJlxG>d^w3MZqEzfJK>~4&Rg6E8jg~ON@oIcRlt3Jh+Q~XBxA(-M0>n72 za6rFe=iSh64-~$5gh&!aOoT^pL4rEZebU3|O_R$|=L%K(k7E_hah96V73zD(VH5-mPMei&L9P$!qaSd`YB{R*$Vyh^p)BPc2 zjYUT#u4bRYe*IxM;inDDstXYizpAN9j~ki5n5a@)Z+u16Ki-)>vogOB$VGR5Fr+&v zmayWc)X@8yCt=s;8;ukxG2Sj%b_=zB&5+<}AKv-GHR`dMi&an?kvu28i3mv3 zh{gC*w;`cr$#X9B{^2p99oS*tkU2fsG}!y;Xz7ABT4To8cc7Nkvb*bOW9O_k`_G~W zPipo%p=ZyB6lj7(fX4HpZ))flrJL1yvEzkcew|Z$`;|j2he13|t~D5&(kK>}fy=SNi!}hkOuP{?1zDHq(*KJ)8?=tK&Jc z{fwb)jGC%2kcWQ+kLM4w4A^ZYqWdVhZ$s)-vq?N`)^pTj=0u=n`X=rtKDosm+@yI% zt#0Rzltm-E1%XVi0dNY{BcD>my*CQZT$f;Q6Ms=-b;RMsBMz2eZ9fFw>^`KSi;Cx7 z;tQ3|xx#|*YqJ?1mevLpZO;PwpX#dDiIR{DYAFa<5L;8)W|thHecJuCkm=#ZP!l7g z@IJW*X%+c2$GFvUNP<&MZs;Q4m|u3wEq2hsi#YetgifVt$GRrpr7Gut2JKd% zazLl$W_^lE1*%+3>G;oYt*9h0e(Aj4O3X%h3`=cA!?9m@-H4x=TE9~}$omdHw1g=5 zTt(s_kP+tM|BSC`;kgw=B!f%9!55FqL0$uevTD)XE_y=zroN7et+!^- z2W9*q)+{bTK^M|wsg&~bmdN29sAD4V9~Jl42r3(W2VXY?xpW{m-&I~9RMLHh$AOH0 zy%FQ$knVtna7}6Yhk&0oZos5Fbk@Us0CK`YvOyMLz&QI`795R?c_kErN~D@8p+LR_ z{<{*1rIJ_!;H7*j7|f)D_R%%9oTePsbRCGkr-fI7C)0xOo^R6!VMo0jD)6&&~d1hJO-86Y&_E!j!9MV@EiG!Q0howyr3! zmmal~WPrYMFUR(I@m#zrEmprfY7MV@#)+g=9+R+R2GE-PLN?Cx@|o^(-yk-W z=`to-&!Es>W)%wAKFq-v{~BgP8I^$;*??0ID_3E&!T8U#1tf!u_KV=FA%ZS*VN4DEOf?=jLPdU#05D&)f}Ak})pGWU z1F0)F#MCg74ObfoRH!(fQ2e|3?Bc-e@AfmtV*{9|kl@bR(ZfXb3jd(#ESWSA0)jmy zQ!is``wwEi@-n%D1iB(x$Q-_$*2-)p+PRRcp=F_F3SSv(54qXvwAHJ-pdB&-D#he} zg=hIZg9CD=>abc1P400!D>j8^`l^q6!;KuyqAvAkjivq?+`QOU1!vtIX9K{KE~qtO zlGDYyq!+|x+RiXKY;OF^sSP$hETb2XW<{7B`4t{wgz@b+P43`-Z8)?J{O0Gn6`|Lr zDVx8eL|T0vZ2PWR26`YQ@3WNF-Z}8*K~TLg0*L5_1A2bNt&GZ(QQsd462Y<>1I{3E4I!NHs~ z%U!|dJIP;)w>_svg=>Mf>_83@{?~X&jnBkKZB3jL#Pa+{MAAfV2SEoZ6hlU~ni!lL znY(1{Sv4)qUfn+6Hce*-(yivq=pE+s@gvcY%l~0f0#k2G#OR-EdSvHhVI!?WYM)@j zoE}$_8eij(nT+`MpUNs;>+-$?v~?@qy0>i?3s zH?PJua50?6=>AZdgQeug*{+3xc7<`xOvCz!WMAD=5BJg}-2QT@zqjruxf&I)Aon+$ zW`UVt?Hk83(Mq*L4164iSIJb}#qr6t1S6}C)_;NVeH=(N2mTl%;(_*5i3evPEYyeR3fqe_bvr7%*-Z@%B`qsT z-t@3^?^2Mqa*<;u9H^MAkSL$1c_K%Po000-lbxu(ss1}3y6c5$>Qv%;4o6eP`|`oE z^m4t9__tnr^p_N0_3MGEvaKclPp^E1D)N-9*vUGQk`2nI5QvB4Aq5C6- zH}jm`W^0tN`JPhn%g^#Z_rc{2V8kWt+X2&PL>FEAQ??@S(X)oHL8YD{N{NjQ4tFB@7CuhJ!&xUq}lh^NZhfb6M zL}3mY!jBQO7_vu9fGHD?#fP~6aJ)Jm1rbvP`r~W&>DVy}2a+&jsz@*2j7G8BQO|RN z#8ZOAm5x@@nWLIQ4ZL7nA~HPO8aJl+lDgELl4zn=bL`W(hXggTCX1$Pg(Cy(5pnw7 zD-P?d=|LG^U@beq(wX=@<|_j#>XrMuv~ODiD&V^b7=a68*?z1cvv6U0xN?ce0AirU z(QJr*S?~i|l>>zhFzP${5|KLfBV{2|2bU{yCla@v`0MMg8&la!yNLWe+}O|_30V1_ zj0wWP$CUjM{g|kE)g+q2oI+^>t$pUxVR*!R01xyc{=cFI*d$OQD1a8O`Nun_>rUxK zuc^k5H4&srtXTay(i}L$b?-1K*OO!ZtjR};Ue(_KI4=;qdq$am5Vw{)rFL{q@nQb) zEEHz=hMU}ZyXbB9b0a=Jpo3rKH<0-9H&WpS9-E600{~@S6kQbI#Rt_b%3RWEg-+rz zsN81GgtR8Cj|MUIwJ=S0-^Nk2#Qu31gPbvTYi>wqA+osDK%f0wxNhdahvdDo7`z7G zGn<+zRr1}~8V!5eVk;~R2ftDFDv>Ije;j!v(R^({YxKH!Ga9`F*?Mc!dzpL=^L4@C zg7tmIc9$~K-zucNNi2~|^G+%YYt*1Bob_kxq_QWs`7aU7XN&}UN*0W9AkHw(p#US} zaOkcs3YaP5NeUeb8;}b406hMa>}bpIz)04lsADPEg*Yg&EpPf`Es zFEg_>GkH0DB1!{v56|9ASn#&IVm~@OV+S%GCUjw>et5oRKY9HT>e*Zp;68D;g%Egak3LUtl%6yiyw`BamV+t5``u#Slf6$F zgQ>Ssjv~)N3R@n9;*HYnV<=DfzOrsY-c^4&IxR^m9L&fACKNFkSBZNC$Ha@Kb%4pC zHy)4{;l&~S6$PP3k5jmxdoMH!@(u}>nBa~Lue+m|ZGZe_f$$skOwvB%+;eaq#Sv4C z?bXb?BM;vdkT^%OG@eo2wsAc`Olvh9jSkGk z1jczBTIn%?(ni%@Ss7NnA@~Z7W5@@=AHDWqxx)3&1Sj+>!4E7m(kBl}qxTc-i{>aF%xNneqGM8|lD$J}=mjQlE9z*;L13NYJT|9!=Uy%e9@ zJQU6^*mWEMTW|nBTA?hwOff~mk1>#dgq7#wO%;9>iZ_?sf@j9N<-!ZZR%Xnq&~5y# z6yKI-=eTMoK%NF1vsP?xF9gK+1U~`89o|uothDu|e~VaIoo57XI?3TB8uF<&mycXgP3&ce*33DEP`~3r>IIcjqK?Yn z$ka5Cw)*D1yfHHEg~!;5yee%G_RJi?psD1_70h#=66UP^d*ak?nDS?81+RJi)Cc%s zbf#fD2{1Fj%Rql76q@ubxi|wR1%E0@73%&wis5(eR+h%{;EcNi#YkMu8+M`6fWJb79&#j@onN5 zE2@I<-QV~1lygb*We4e;FumF@iCp>k!!iF5Yx+mCwb6*EI77eF@ViKFqJuW&<7@Qc zl6kTlTleNADwOgvL_)jR-$KnaT5a#7nkl66^prG55m(WTJc^wiKeK-TUw5Lp(KC)p zBBt{Av|@AFm3yYyxEIAk$H7j3KOiWZH?3*`inAVvT7H`5 zp%(?AwTvWy+NZXn=#275a#;$KZ3L;Dc;-IqLH^fGggJPQ@K?(~Nc6H)$d}L`zFv6= z3N7HKeQII^VBn`KYOaTuLiZ{A7QLDdvC~iY4QOG1L9HpdK`g_M@PHU*HhFpsEWE^*hoP-2js{*b~zhHcIf|;jhyFF9iFnhuKFbA>JKy)19 z0#c}_2I5Zx-ImKcELivmMd4!G)jF|&Nrm^DEi@E0kcc?^8yx3y4)m<`XBr$p7y&!E zownXz^oxzUxHa>ZoRfbR_hGU{3?dC;>XOfmkF)EK8!?Z_T=ubc04o(m+*w4G03hY% zuoa~42x%R@X1FA^n*h>Z%B3CX95|8SF~7`N;1To7hj5`Zeg>8PS`=y5owi*~&I22% zT#foeS2C1*CNOPsYN`*qJ3G}>tZ_xHJ*~?|K{|hBY0&EeJn;-5(y9^E0At9p#xkF_%ordDAku(}2}y=#ZcAeH zvN01vGqet>k+|Japc27}96p*MrGNN)}Wg3=@iPf|W zRR{eb^#UP}JKrE&!=E*!;wzOruH1LTa6UFJ4&ruPzi%sqtTQ7hkf<{wUqde#Qwib{ z`YGoSm_!gjilKi{3iy!+@eai1!Y?h;8?`%Ys4gdE{V)_oTr-5uMM4VrWJf@`R8Wpb zbIOCVl*X0b_xDVqt;!g#Ycamsv{Y}xJ?-bi1sZ;Q_Hk6q)@`D6D*An1gs4M&l0a9x z=h}^fwL6k+Zbb5x5Qwlo$H?zrnYf8?c+-D9Zj$Fy;kEguV>kW0c1t41*sO*bCmF=) z7262nqjEq%IRTww(+AvXbi)fazhw6@-~o(nY5#wiOOSws5?h(2~vcB zK1$q|Z(7ck^Kvx!bglY5Z79K%&6lL2pDDY8IC2L7{6zD^H@yoM#U=DZ&bb26XIKH@ zjN&bx8CRCo#Q~_3Tr&?17)dSOa2dYEsVGE`rH}{6_c=3?i!s%4A2L_t4vskvRS|ko za${Ou9AcD>l=y25wOO*vxnMtUmVdlw`N;tKZbhE{C7jx`Tybi6rsA(!Wet~9o;Ktx zOZ+vbR1J3DW|h3XIWcb!-Kfq`@+Qe0RfQGw4@7}m^O5>b%kHbxbl3I49YPh*@(mxP zB#X60pDowUrS;zc1)*N{6c!<54Od%Tq~O$d%A zc1RIjgkL0Io-%KRQ9-7Z&27`}g$Sa{2Cv_JDw{Y22lj5$c)-oMG9MbF)u1UqP%+hi@ptYCIB z#kq(@ws1{Ff+teOCkOc^bkF#CAE-zmR+G2Rntp&2fEF%j_EadfX(NMAE(LlR#V3M~ zRojbfgQpzbDSEv{i_fJjW_Ho_>KvI&Eg)aEV4wcNFp&BSeR6;Hb!S5^m$ch>ZM!cJ zP$=KW1pd<5ToqEQIG>=O>j30aHI^3MTP{He(IN=()(YVtBGT4AxAaScx*rj;Nhg)4 z($JgvqtB|kERlxxI9*gc#tkdX<9}fmhXofb$c?>x#N4h1Usj`TyTVTqk)d$x{B4v^ zo$@Y?ih#e6o?t=JmQC@R)1fl!bIX87fsg1>Tq95OR}Ti0*^Rve!6BE-36a5CAvddnqci$T$>! z_=#!S?34?EY2UHuLa>a4*cC;+shV)4KlfNq>R-_9ChB zHAU9VI3|{hI%y%1nY50NMohoV8Zy=i=Q2)HNd_8jdeyts8BBy3OU%r5!;b-G z>7T^jFiV1bc3!gAB^dwsE02rb*nW_FTsHz3a9Z|=nq5tDryc_(q{GtWvI$ZY38njl z^7s+H>akBz-A#Gh$fV%GC>#~D@QuH5;LSJ97;Bo^#Z7Go0Uro-XYhI>!tXk5Yj)pm zMxqpqMl86ydUj#&i<1Vd?+-0sbhY5IUDM3D&T!bopc;;2w1c$#BQzE4zrNL4!);M| zfN&xD26Y?Ov+bRmR_%`%x4%E+vtJq5d;wp5@;<1dwWKT#d*H<^dMk`j$SLgJmsvZ2 zSNJ&~BM#6hZIZ-%*;UeDfgfK;n|6b|;}pfo93{4_J`1?-7PxC-pCxA>oxD=`1&Mx3&wpHoB%%?!?k-`e}io3E0R&x(Iwv(Rxd0pUv z5V`89s`SbXhe?62H7A@6RKn^X0%UYkEFLNwHKDd2C>Nfx*J!#G=W7g-0t43lkSig$ zrH8O~Dk`p9&VAr)iv7b$FbnhwdL@5n%m$stGF;o4GwFb#GiJ-bG&yQGdIg}@fXeS6 ztUYBgDQZGg+^k9L02v3KrJr~Sj8KLW7~IsRLz0L{%lw0Ic9UA_YlxO)GJ$tfE0<-^ z%*fOP77#_eEf7KGO%p(wK=o!Qaovi$LfrM8kqDP@Wwu_YMmLPz;N%Ukx`{gEK`g)K zP#>vt2dhsGXG|*rkKsds`sNedWJ!aqGo3~uk&w51)m6R{GA%{UgzcLk{%njN7r)uh z2K{@(2cHo$A9sm9^E2F#fXG23nznCyk&3I~;u+O5Vuv#ehX-nx}!dY1r7*HW|>=j{A!s#O8>BC%C(yy zS1Ww1tWGNBw7v>e=T)r|KkzqL&~`a@3Dgyt#>=eRm#*Y79p5AaIsGK*N&rN+@`uFC zM}JXJ#J>Cce#%{^&~XBp&VhIN#TWs;Ag3HKsddN@$-ibpr!`6ukzVTL-@prij!TPEk>W(mfx$xy5tvM|Dt>JXO!-U##%!0CFe2s4i&wo6hXHxBi%I0S8h4BoKUe zcH+7U1j?j?dZW2w@dwW_TPV+D-!ggfRPys3yd(%7mGJXL&f{hb+i2oPfmm(g8qsLC zrtk7?{&sd+0Nb+Vaz&KOjW>=SH{x45+G!+G*m#V?yv#AdfAX)cI=SM=MeW)kt%uXF4ck(jvl&SkoJbM^y)q3S#IQbQNgxY-tOR;&#Yc8@kab%3|LC`NS$F zJ;*N55X(=z1aejM3~~pVFcm;3KeRqr&`GNSzPgPpITZDeUR3aNCE)t$?`g!n&c303 z$Wcu!mU}L|mL#&TH4-?f?do-lALn#5{d+Fp+Q){ASig{e@|>Q`^z0tto%K={Ula!4 zFa=T={C-;`$}=?+N`)K#>2)h0R#S5-ATV|lqr1O9q4L-8Rfnq0URYsCi&=u)V2IRt z50@7a0n}6Xx=^I6K7r6J4U|l?Lnl6^fy4zT^4^<^A0%Ou))5gZd15Ne8Sf8arl*fH zF8_;*2dKuEG?wD(`(@HnbJwTX@(XMimW!Q(+NmVrga<-F8n>~_OJ&Nr?uhEeeK*d; z$KlDA#o9O=-K)l9{%MR(PqPrZclDS}#qgbqV-4TdTnio!XV4kX3wlCQRP1-nv=OXF z&A$*E3)bGU2ZwWE;H6S%UD#B3B**TVZ~hZ6q5L^3wlVxMT~FROfny<_#c?45S;=Tk ze0xxQyY#_Fua+UzuXi@Ub_tX1I_IJPRf%%fAdC4n6F%Y*8DIQ0ndHIZEXDjBEMN-W znjDG9d7c$baIORI&)TKfRCYr+LB)W&~3FP5Dt>8n-8g)}#QDd!z@d%W-! zNAQ(ehA)A1981?Z&eSqjsV>-A;n)?Zs2G(zg&^Up6M-m!q?4!Y6iXbKn6-8>i~KEJ zXxq1E!Acs1B4?1M8cQ6_JYDGq@)$&g?OFzb-TD;a6x3#iX$$x)pK-*&VJU{Tt9vL} zIIAVbHBy9BW9gi+WZWdOw|_Z&H#*Q8J%v()cV>tqKd(Y1nua!Rwfud+I&QftETrY0 z$K`vmW9dh5Y1{S>nDaufFP8xH+e#8{SKucYyGt6*7}2tKUFN=$=TPbd`7?tE&h@7y zEuxP_=bcy9bP8ia@?}l;(~y2_~5Jc2ujHRyxO0umC=7~>r%@0F17kGW74_3as%+w+<( zmF;7a;er)}g@7q^E3>uX9&7(7F2k<8k9ipA8{H6p_L0qDP_!*oCp(q4RNqH%2 zL+R~5qz<;JR(Wq!qDKf>SazN?Zni(23!0LE^BW@Y3|-DfdTHM{;-L*J{Qrx9La47D4dGG!1Ir+zr98UJxXJ>YHCeiH}(sE)` zB8`@F1`jX~^4r4r&}qH+OB7jS(OQX9Dw+7A#6>onpT|`&2Aqv4@L^}W(J^wA)Ak3R z4m0^I8MQl@K!WjKdZ6CNs_Gvkyv`bDpR9Nh;?QZ5(E$W3_XaM;ghSIt7y0hplJ}Tq zm+0OE{_YVYlSK6gHKH5|-$V>}Y|_VN3%RBngNyo{ajo7@bS7wLIx44tfFIo4n48$D zy0YT0Z1!amHqpOn;>l01j23e=R74vw^uOBC7daMt_o^N*w9S%uYADwV!$d5ZwIi-a zrjV9$wNxmyb{L~)*=DhXQ>927G{)B!ODmO1E0vTyI=d#y3h64y{a^&lcTZPSNXTIU z@PZeb`{faQ#=|xmO&$y;&7(zHc+(lAph*9?ER{?W0^JqiXTA zrzWW7H~3N(K6cV^@}(*$9AeYetdhCr935J=E@|s>qh6d0n=veF;~%DEf;XF18DXC0 zu79k%4bStINDB7UPdDlxe9^*SNn0f5YDkEmxcT|!hP70VYnmw*+NL#GoI(seNwL8; z?n}qE^(nPij|~i5W~^d05!q2^ve`Z7slP&`-xGhxJ2R!=BuIVxi|sjrwN3yIRzs%! zp3SzknA=ier_!NIxJa;{Cx^v)4~`DhRA%$1kaV{%)xP=jKu%0n#7=obhWllpvMdVN z(q(hwm$l08=?!o)KCf|+=E9+)=GyUD_?!3e7Anu5Cb{uRQXN2mpc^vgyXl*rG?tq( zXa4z?o<|BIM&hc2zr!6;PEv*CX^J~t`&0^N9d@T1({$I3=2Kw&J4ZsbL#a(owo1D` zFuOmJ{es31ix2MmG%tNqJa)**Xhx*%=bW#g&%aHHY+ynao+rF1(sW>iccyj;M<36< zZsvsU7*8n}JU?V!7Y8@z2JsVjq^LEAAq}W0f=t%8MLAC2q?9mHyTzEClQ!Ar4l!9e%(hzR6d7;8&Kk98|c9~c@4X6_|vixxgmt`;c`DDk56XA29csgpGt z_mZe>bHO!@wdtKA*beG{6e5z$31JV`gLx3XQ2P*LWQZu@(iJct&S^$2gHyazT(790 zrkNX;!|#F4?~6F?kGPl^`ok9%6@vfUiJ4hhxTmlI;trpzr~B5S_A(^;^+noD1ghAp z?)CUE`Tk9(rXg*sqk1f)B@aK=xo3+IUe1vRq45lK`ccxw6J>s8f%)X$g+Ob;1B*}F9$Yle-g}6CAc33Z_1KEXqiQQ14Tb6{!&W%+P-;_ zwY5zAPMq$$;X2o!{=^8?$@y4(E&k(A|6ER*H)Xc@R&z!{ty}NB_!afsAHOH=c1-7X z12muzXxikg_}GfT9lzvKX|2rKWQo@=>bw5%gz!gEcQ5_~A}W80Bc|!m;L0v>ma6Lo zWRBI67&x_D2z|L`;nW<{f3zVKhy7VyMoa))-(m5?*<>V@jEn6@i=8%d|NRy5N1ZJI zT|#ocw&uf%%85xiM~_FQrcm({TH$)0DE~vx}A!&forn( zM<$hW^fH5-E5N1c=lPw`x)_Ea%gA=$s?_TU-<3n`>>MRQD-ar;xpia)`OBZ~Go)W3 zoVFZv>_>i{H6n<{l2zbklr*@G=#}s%WS4IOKH7ZuS|Hp+x^}JfkV_4kdojB#q zKfopcF#K_?5LTuxSRj`WD&4@{6z=sb5xR5bQDMdCCQ&c)z+r^)nlxXd+?f!0B~$uwG42zugxgmN?KFxmN3l-zSfJk$o$x#x&BO*S{^{@$(p(8& zXqMg1W;fiU)cV?`Q-5N)A$~alcAg9;{4)pDq|`VsdC3=OuJ2i38VYVyKaf?G8JG}$ zu(ksCSf|}-IKv&f?FtZ*$X3Vv3N3AJ(E#eSg*`;u=)2To!4n2(_@O4)1MNi?4 z3^u#`4VcEJCGiki3k1SWcL%S|ehiC4o9!!&+g{9n*mOYuCmUc)^GC%vnFJA_euL4J zPS-&DOgS0B%fe9fn;A^-&Zhj!W4zD^Nc2OJE_n+y^{-yie%%eDGonxYZ~mW+-_Jcf zWanob_lPb7%NGjvHfn1d&@7x-eHS=trCjk`RSW#2EGKmSF)7|Zx9S^KL*d~BjpV1c z_8%eWUdE)9WrE6kZ&A+~R>X_MM;YcDF&yfJ-Q}WKE}l{X4DZ`)Pyko-oWR6KcHG}L zDb>jQ6-N#1U$N-W_Wf#JCcO!6I_C@GVm5q{WxsCocF*w8tA=nv<=kdrRzU@T1NS8d;5EH2|AmpbhU*I2!lof6&Bpht}EB%y}N%~ zb*;b)Ks1eM7w%*zj8+sSNkr(7ZJybQCrHp1Ve^B$m=WNNBEIv~UkpyPs}zSWoY;b9 z6+PZ@W0V>*Wzn`@oP01=!^~{76Xe_Pq+z5f<5DIT*D-`QA@UPEv;P0JD zh4&?-mVK6*UJuqu|Cs3|rQf@ciL;yn#_r`huDS@!Km)J9sZ3<_*y=p;&4`5aV(A$a zrPOq-neJ;)?dz>AZSx9uKO?%f$e{wkRI#=8LP2RuG<&RF`arXScoo(ajZ|Yozu?{= z67AktdJ(2gHOwLwY73D~y&l)@burMS9RV~4Faym#B5kT*ypGZroPoOH(9$MywFVxu zHJI+n#mZcjYb&PdFKR}7yPkjg`D8zZPerQkIvc9+!ceu4+m}Ufo3%5ht>&K@<-Y&< zcLqF{`zEt0lvo1?iYW{PRKKjtj;s8BCyJ^(_G=Em*X6*Oh1X@SMpJEF!FwMXfG zo7lBpKv#0e=Ul{~5faAy6r=%g8qXbqsR{HQ$c+{nl6S_QhX1$H-W5pQ7rcDE zPiCO0QY(vypd4#x$)bVvrCT_}<)_LTQ7rIc06$I;kqJ9r4s{Xsl56zfh3Yi*M}}kO z*Y{|@&olCvo0&J=Hd-UdMr`j6d3?DHaVcgoE0Hs!@C5An{Q_pq+PP%Ej2^>mxh#hA zvN}>x1Aa+4`vvWl!p)j}cm<57u7M=2%fAkkM>b;IQbdt^Q@~WQ(pilt^VnAUbxvry z7<-yV+!_4|66*PpTtG;W4Xa2itjEoZxnQVQlzlwmIEL+lYu*g zEU-|aej_D%q{#ulG;rNvr80Y~7g~t&3}ErC-TRyAG>dLgk#=>i9d>!XJ^#X z-Eb-lA}BiKnAZ9A#q;7OMB;&O^wNO=mq!BHGu|b#`l; zQc%v2SQIfb9;G`deZ+4=G$1CO?0l#}z%Q~EeaOnI$fD`reJXptOKL&|dyNV< z{18dxSF0-Of)SsKQGq8=#h%ZWtO#xRT|Bm2Sv<~rNPz9V@g!U{s>%TKo@(v;y-(DI z@E}rB$eZ2VwgAXOKX|BCiK7}G>o&iuG|Acua608hWyvd~t!JAFwP z=jk2czrt-v&oHZBs^~(#M_jI8_LyL-)!5HFl#^#hvb>Z<{qb=F>Kn`(PQg5y0$;uW z{5;4)+s=GF5jW3B_YGw4*vj}C*t$gz`1S-45m`}dszd9AHB!AQR^R zQxkBY=?=6jhFK)vRoUcg`EwH?2h!L#(CM7ss7Gx+8vgUTB@mh7bjG$gdHaU{ga4zt z^Op%$U(*O!A`7k`)IX~yTiIBOdvM2fVYD2vPH*sNR1fwxNw$T3X(PIx3x zkDX1dIMF_ruvs-q0tK;@1F%F)qtB13bXT|Pq><0xx_bJSJgDrlFs>6DTTNf!ti$0^ zyzoslQ1W3*oXJ;M#9DJ^wM)XMAjG9&uM8WCM*`ZQO#>)oz!X_|93pX(Cnk1rBzMy4 zs?a31-S7K(TJ%yL-o~Cpz9ADS(F&fez)BzZ{g8@~K90FbL^qIc(=2jCR;ZM&47id_ zIjz)97GJh=G;Qia9^x`)RL&ILoJqKCO2irN$bH(mY#{f#5-feH^7t)w!~1K8`e|e8 zm3{d!c_dMbMB>Q>{YGd}>{Y|{#wwQfy*ql44jBpY1$T*Z?N;BAKZ zfN%CH;Gpo!fy>QR$=+sW)O;@qef~#3nA1w7gQMPbYCJ!=;W_Vk=n@V3AmqC{qP?+_ zty3#Ah8*>q!^gD!9M@-!1{r-NW~zk~8f@r>eK@>Brtm%xEsE9Je#=wG?8GevTU z{6Nn%Gc%|Vq#E`F2(=bsjvBo9c+MXwQSRT1IHde^YKn)v%GKiYE#YI0WXV?Itsf9m zkK&qy172@LruY-F{J^8!3h_{2-%imr+^xm|OFG&_Bo~oo!NDORizsXX5rsA~n6uxo z;rrcUQ!%pg98LBFg(tT$)BCHf@4SD0*v=98?yq#wq^eg#ZhK(o?V4VB*Z_tSUT~ZX zYD3PfEZiKsMtY07A?X@3O1-s_BmLrBW=r|H7Sjy|8%zXrhMy!nzMr4f6%rv zu!SHp9C@tE1d8t7g;GFtE2++(&)|ln!ixp`qJyW#>L2`~!4i*0Q{)mhLzEYu5H%8* z12ov39P&p0usdobVruVll9~?|OP|y6W=giR1IEHP>aiSuM)VR?*pBtr-}>kQz>IbU zZ<&1SLrmFU5bwGTYGpqK7+Ed5Ra3_e!7Og|32~ucJkLC%Sn3NM7rffcnHt8A4%X0v zZ#dNK9!C&(6^{4z#t{fTyT-2D>RKwckOPZmkFXk5!k%E=nuULd`83ISr69om1ud$e zjn*{c=$uPGlC2X%Y%_?ZPFI7F{2aNy^ns>jk?bxo{^>_2xA^m`)&s%Y zkW;q-KH%tWLx@+Mn>paQjH{&%VJ`4#Jc&Bt-bJI5j%zY62Odk5xmPJ&SkmCPEv2IG=l=_ru`L4a?bW&(}(3{jh3aZ2}VH>t+=)xuqxdr#L=2QR3en zKLjn4JxN>QcrN)!Yhs)0>8R-1VgnWfxtH*!J`J=tXQj5S439X^RP|9Zys5^dkD8$d zKnNBlm^M5>Eb^wVCv`PL!qPDAGTcB_RZJx=oRp-bY-s>`NFDkBd+gyg)}I{Q(r>06 z4f@T2SVOCKYb+qxZ~PLgGOXUN;;4H*emsaIaBT?2=)t#)c>9k}VU@uj9Y>ens_el) z_PQ+cmM0#>HeUH5@m0kHdvm07bd=OgAQTEG)eWOs;7kj{iY?B_e$o^o^U1-x_@0#3 zJ;H)qof(KsU%!2JjndC7mfE6wdCbGuFMKd0d=7h$6AuTT4(I>r9o5kO{!i#hSN`H< z?I4oS(3vKNgeKT+Jm_T8QqOj@m@T6*>zMa3kk4G9rMU=2y>arf_F(3Rvw?slzXoA&agE&Q#9cljJ&G z{aOFB67}|Wy4M^iu?23Uuk?@CbdVU}6=u8p$^XS11mVLvenViKH*iQqHQ|0$nQGDS z`+!g|!Ngo(@o9!Lo+%w!1K$AE0+D`oG$y(_=$~jhFeQ)1h#t~kjQc}IQ^&SgkM@+8 zNRgw%W?&w=27p*Dyts0v!GZ1yYx+}Fs9DDDSj}@{#O*I`lnk8AA30X@DVCV2BQVgp(Jz;8&#m`#; z{yX&ancw4lV`{3doR*+^s9i`U)f9K8x^szS%ohTb1_BjHvBmd0bT5{^fzKvvSnfX= zGK*buDNiiw(X}flf(&tz*kgBa#PpK|l|I$sPDo{ov>8*Hi0$80Ue=&g)xx?_Qc5`8 z3i|OU0ntaqW`{qU&EVO2E>cPb3OW`&DkzB0NW#15@h<+_T4xAsd%Pg;Ctb(Jg*x3l zRR*}^|5HG7eebR!@R;WPc+V-_u~FPR1u7j}S}3v$UVOu5_aT&@VML_-&V7Krox0C3 zK;yf?zdL|_Cd#=;ej(UI$ike>Kw-p`jS8LQ4+EOxW+9RUCUEzb_}ibgM;OAUja)H% zxkB1b$%!nS99^*d$mzDSe84W>{%M&`^?Y;ifaud{qpKl}96jM^xa(x5Z@Jdj$R=sL z5BtxzYO!pa*tWxa48T{hYAiCJnAhXmg?NMz_;+G=@sQYrYW7LpLRSOG{qte%GJcxNaT+NVNaghKT_7G{x7OSGoIFiwjZW^P*p+g(?r~?vaQO=pX^D#AV8_9Y z*~=#0m|}dBuIX*Zod3&X<>0aL6mVQuEP6fLgcXK?v9q;5WZ;C#^;K_~E>#+a5(}H) zK+WlGW}H~FoYOFBMek>&i{YLuG>9_CEg49;aAiresU@GL=}=>Cmj=nK=?SoZ`Gq2< zXnTwd02hH)zieX@@pAKC#yyC@uxQEWtHgRzmxf+hBG`@(9>Am&PI#fuv72y@!`jZQ zNxaJD0XpQ#l)q>ape2uM22D%xK@n!OV}bMstG=Rh6+#&5jP-Ns&U#%<+B7|kfQ{Sr z984dULQ6Nx*@d_KXh} z*tNY+5+~4-Ms?VZUK0J}bLko~KspGN;S;D8NTd>2T&-O74Gje%Eh{Voyx|M=@n~!W zR4G{37gr{2cG`g@%g#egA>9s@)f$ksAs1_Q=k3s4=V*xiy2Vf#@(w1ZeD`7j^j(g zuL!B__pyZ+!Rs3K<~Ql+e=Kq137Pk_H#w|f0_0RZM;s05N=3TN=2z=$&vs9FOZ^>6 zJExK&aXW9Azm_;=1FGY!dY=gy-hNZ053_jsf*0;<3U8rjAk-H3HV&O2A8u7xuf*Ul zAn3mPGVYl`fVV#$qhUhp+e;0R1YLtcwf($p8WKxNo}tcMN6NGQb zFh5O>?frw$DZF0jvwpd{-t7Et%g#rq3)d6^y0no zzNImCH2qa*Iz2enf`*lG*x9yFiS1|Z3dnNf=ha%O2FdlGL0utyQ`;iuxOjUjqzeqW zAlu_DxI*?1p>Gf;!_n%&k(AA~{mzqZ5f7vw!Mj z7r=R#3-LE;1Fr&A)x!m)aM+uqcXE4jr*;0w#8*>COiLUF6xNOGY)W}wz6EQUc9aN%*zDCz(a#;1cgi#$w3<>T9#qsRdca%Q8s*D}rs7I;u%jv!=wMyjPGOp}SGZc!;us~d&@?$>?C{~INPM%is z4l5L?E)Csb7lXpQcLvQa_g@6PI**OTrtCzAko~|Cyy6iEU0A)N!Q|{IhiwiCI@4&4^!ti~yfor zbZSg;V%_71U!SWo#;?TNu4TD!O%u**R(NlW(P}YdSRXwmYCt?)Mk6=Gm8_;yE9`A_ z_=5zpJr1=jW|&_KUJGjA4A<9UOQPMsgQ^Wysqjx9Bdfxp6l2o4NqQ*KGdW)l`eG0# z95V)s0H=6s<{96t67~`utNQ9QzJ;l3ZC|3ZYLOM?XCPIOeffs7H^iZXLbp-3HHnu2gpqIsCbY z3A))CcMp2-mEXspK9e(QhDm>o)qzqmyDbE>9Y{IMuG{N}{lce|-L&Fd6VmYvrM?D7 zZLr0-+PAKYUeIN0vBw_AXeoggOS;EW2 z(IBNL^E-V3RwP#;C%{5Iy)mxiby)V8@-#RfGe! z=#NpM(i8JN>%$b_R%-$4pLB_2`8Dg#tTjP;z{!iJD~uSes+0jfp#zP?mbg$SdSiyl zUNXIit@VZNl1JP3Bia2tDHZXh!9o_PT&K_c$k`U*gX<9c`QFm7fnW(_{{4t`_+o0k zNZG3Y>BkldR{ulkjaP{C9yY0jy_I7?@{8!^`r%SOo~hQ{>iFQf3=r#E=?3nIx}UG} z+SxhuZ#oXGXaON^!Re>{pJ1q127K*Ffxpgf^-g%6{9%q8lXd=PeLQ^eLOkh{FZg?s zKf2iG2&7f+D$zvuQ{n-&884i0FYS=svkI_^yHW&w&}ETC$OhMTRr@flxZwKx-}L^b zUtmwu?5YhzkI2`3DUcYBsC)0z_3TO?waoHNZVLT@tFN5xlRtAYgak#-K5(`(w$ZRtzBT1km$z7b zh~6&o{f07K#$^c&iZ9*znc5{mx1Wtrz%WU}fw%8OoFkZ*9Ii-Ius(VV>8k!>yR%lDrr@LA+~qY75F^~X0K3=8Nh) z6Iaua8;%7jD+Pqfw(X6xfu4~srL!Ee1=4(SwX1dXPAK)pJP3YT4%CxTGK-pvj$9e0 zS^Q*2{X*vWmsYrl4HY$>C)-PmD$tyQ2cu4fP-uc6D0(5*~)<1oViKZ!>4`M2G=TpQc~uc`Fyr*v+$a zfZ2f*cI@z{X6`FqTt~f-Vi=>o4nu4>^?OOq0He2SQ7MLdJ zt}pkQept1Kb^3o3E(zUG6N5tMy>Et{l2?XgILx?tp>-Iu0nm~KtNkhZ-+HcDvD>{n zI0MjJAp`mYRXAX#fg=LIo3DI;zo6WUftn6AWB&wAcnDNI1SOhQC3w0B(~rOZr6ht5 zh~-cpT<{fMPE;IUdALP8S(wFc=Mi0 zsDqSm>4!u4h|7IoAr4kLlx10g_i-SU;KKu(3uYiqr}^6<)4$u7SfiGCImUcTGi~D> z^uB#Yy(1>p(m3yEo;2jGANXi(j^@h6uYId&Le_wl7jtik-XhVGo2O;)*+{y6M5kCr zz?-Y^W$5=uy<$jCwzi7_F%vNL&zF!D+-xQL#)X;rhVSOE5&3Z4jf!FBj0K%X(Oh0s zVlKYGTE_TWfg4ob>YHa%s3ro+PngqxCEb_S#EF@XGv>DExlT5kQA6)Cp^S4TXd{yL zP~96MhAvbKEq6V1x7I{S_v??7^Rh5piZ^hf^RTC&Rq>+IPAz9S^1L_Pdnf>K5;9my;R0CG&n(@(d_<`l&4>1~9VQ+{Iy6uRnU55#gdgxcM$Z#gCnU|y4(pMk*nKf#Q_fQ4 z7TzAD0^)0)p>CFF{Tg*%?(ZJ?M3QDxG#f!IOX2(H5@h-v^vTl?yt#FwzE)|AIMVWl z?yZ!^|tQ3_xj5#@6PVK|IqUoP+^uFOX7WU1eYe4 zKB9jc_Z;mWjLd=E6w|_o&doqDQ_T(sm_T~~<%P(v_!W2jXo5nb?RZe45wKcl%6xtC z?}IJtLpboo$5)$Et^vXMJ9Lfya~FAEWSST6uSCfBef;$tg<-X6Xlx$PSxQzw`GH&Zq9rT@NfQ_DnR?9lInr8(iA zyfxb%Q$-O4JP;SK@=0t}W;UQOZjU~)Q>yZa+AD4FDOfKUJ3@vF2flTeGb))d7dx1m zlC|dY2YF_l!=F#zqLojw0fl^Ih1CU5uj2DxEkttnoa$UqOPgQ})Ad4nx}J02t9z+= z>h$S{*ku7ZquH?-Ac5Uu0SnwYMt?xm4m(8tGu3_peFHx%ZlXP#wp%n6qw)hAcL$6$ z15SY+c76klr;Nxj+YacDST=kT4GA)Kv0VI&nW7&R{qanyw!Y%_xQ_D};b#dyZ(P3t zHKFtR*^^@1ypHF?Hv&Je5b9iS7;a)}Xjl*y7Z-9m{jJYHDx^q1^n^V)FixDnCwOrA zngg*fyvW~$WHF}XY8_-$J|wHmq~l^1chVC+hYWzOU%pN{WVMeD-m=HqZIwQXg4>c3sp=l?pOBRpC zAI8oh$M9^>{lQ1k_i>@)x~ii0NFnclrJ*Y&h9Ta=?WwrjuMd5phrS}o>nqTac3ejEVdRZNA`jq)VPy&zK6t{}f zzJl6{BBym)KTh-OE9^Gk!=g*che@V|dKG<~5fqILb9!JCLE;Z_pv4duW-rtM+<;J( zc}5~kwKEVTruEF}HE_!A?9~2j;N&E36npuQ6o-E#ZTa2b!iePBI0agy^QCt)!=HKDQW<#=iI2LXN(6=&l-x0Ar4UVr!hj_=`O?2!n z_*rZAwJ5r14<9jBy%WR&M`-T#HMqtB7iwB$(B^6aZLTIbS^h%8827qqqOLPkX4EY= z5y*KB<9R-9J~k@6zUJE9mcybb@*$t1)?Ktk;t`O-)trP{qTA}pf^_pxtBhIm zzp|*F@{aHeQjjtI$b*{pK7FG$MhFg=VrD!E<7`Tc8=mGK#XVGYda?$M>}i$y`i#0Pv7dcH zm1#4NveVBRrlu&#cbI6DmyFXDUc|s~CK_f^z&%Df(r zp@H`GUrf>azCYHNgao?B?FJxp$rpR6K%TnU2nX%XUJ$4oHG!`M_}s{?;bur~4_WF_ zKL%(Drh_GR#mtE`{-qt!J|;JRTt{zA)9HC-017uPS5ic3@&X)gB<)?oBQ}S*~@kYh_7{c`W zrL)P^yNdsGf?cYm!7dl|h_(Y1%7)Y;4j~lHf*h>AHs$?ZmrnrHt!|M)JJsNl;~2cw zH#xX@`~atf0~-6R?N-1*ovjE74e<8XNOyFXA8%k_Qp8}?3mFSz_9$YJa`*X*Q|(Of z6Q8Wv3@-$?khxW>#8cu%|FSY)TvXqF;;gfW20)S@e;+m{LVdigBY~K2ZMfVeU!I26- zH|^i(0DS8uX$Cna>zr5gsij%d#(U#9XzH-;!$=itA+z{le(*t7=3lp1>Xi_mPY<1d z^PuRRaq=gO+erKU{s=Yfw1!W(@n+=Hv*i8#YUuY4_|TtLhdDT)Dy}&T`?)VMMK*1p zwV^0XD)=QAvTF;=acA|`(QJE_%C@dy@#j=#yCpA zJOg%yAj|QsF#^#{LZ@_>xf8mVHm**;ye8e>a`rjZSHoOHP8Dkr_12D`omyGV zS~%3ccz(HnI#s;7%#n#jLycgMWwF;0@9)G^Eu)5wvroNv>%p}!ZZSCE3;)vSg*-iY z9_9B{zQkL&O%tUvBa*lgq^hm%K1tc$Z(*C%KJasSw&eHTKYS48&#TB+q~mj?cQ($* z+i9Un3G!&Ri548Y4prkN3cO0u*a@ao!o>;T1P8(b7p%7LHN|2LRKc+fE1B<#-38d6 zJt`;W9SpoA5}mkDx(~{o`0|XswLb`fr-8aZlpdxdeze)*lTMrFo{qM`5r9;#=d2oi zXfB#6HF5~#al%4lH8c6^!M$y8QShCaB&lgeC4ndVdk8aX0)`9mbBmewdy$*#d!=6X zX!4Maw)8Z0vvf^X{{d+qKlGl*-i8@VZQC~wVcNDU`1;3IzLN_{_C48&8ppZ;`QR4H z?6W=wy>J)X_gMYsVa{q3v}j><;4ux@`yJI!7ggF!d4Cmet3f-W=SawUdbYhT(46z@ zs*K4m-Q|ZSw0}EHh(?b>5aL&6+@CR@zwhbdf_~=8F{XqJJ>M*hLl+m&YqLcsqJB&s zqMXp#&szU#JMzX&y1*?;)R|Fny=Q#GVYTCAULaVDPS6N-=SBJ z*RNpUis7Cxk3FWZ(Bv>!PFN156z~5UOrSx27R%YbTM*ed@AUY_1&ZoTouJ;Ihj(q) z?LysYm$!_~T;1V{p>`)&{egCjVk2KBBpS4>M0zuzJfO)%8CxZ=(UjsKbmg62>W`XQ8&(dHQp_vm#XZ-;~d);^F z&od8UKv-)-WijG)G^_Tiu*IuD@)im>y5}4XvWZ*Qvd9|dW-fHMN3#{~lEnI)JLcmE zJbJ{P{_gzQD(hzVuUI38jg>(sS+!IevX9s z5(ns+xmWk%y6rL2&*FKXuvUPoiA)F&g;%MiP8>8>q@3iG6*F+3)ppKOzj`X0&Dc_p zOgb1T+!eyQ0<0g<)=#%$rt5=Uidc=n0a=85lr7s6YxL~ zq=cq4de%Q-{9ernx~>omXlk>Yvj1h`IfvO})+1U>gSZaMlzrjK!pA9kU$hZ9T{=5d zgq^kiGC~Dq;5YGoB1j{**N^)R>t~<&$-(63KQ_1SF?*@t$P%L5R?B=w)7hhZEzz>s zS>`V&-8uNkfRw7O@@;ApM%-|E4&VB_ZL+~$4_BCX*)`6I8O)>^sbaN$Wt|we>Or0r zV+%xR1fh^o7GJ{^Xpx2BvqOEVKJ!$-Y%iiDz0`q~K+6M@D& zocG3}ZPkJ5W@h>BpiY#w#!B|4E0TNnHaOo6*dSz1fYrE)p%rPgvjHn0`VTewt8n;y zh*cOB?VrhdNXXDP-Z81cfy{ykbmPyL)L+|h-T&@7Ubk_hmPNZexQf(1!2vEvF&3-g z-`nes4=aUO+*xnb3Idh9>LP`T@^FH`mAu~=j5nj;ZZWs$*Q;3sYzV=yFE+xbM^NzWrj%0P*TngcIoVC&=&HWg@_kHqB z#q9}Mq%PTB#g2Z?t0HW~5A7$! z?DS;--Mzjj*5h9|-ce+`;`3XapY$TmzHdTd`u|R?s?9V}mk)pAl5kw6!5%-P`Zd8W zwu`D!XF9@S5cV9H>;Ol??7G*%kG0RQC+f+`ictv6s7@~)(9!i`;%961D>3jEy1e*k zHGcuZe6a-1rv=y%0#JS%Wf*x4Cb+A*lAh+j9|@}Bwz#nYn*H-j3cJ)hxI38UmtF45 zs}6dg#NqR3Rhl4TE%tx>0C&GjY|~Iv@g?!K>35&Nc=0BS!CEyLT3TiEnH7}$s>>eY zv2b3{j0pWVa$*IS>9V`Q%y$d2OYpf}n(RY^sKPS;;ebz;lzbZy=H#PKg~(U*X5nSc zV9U!)z@|UO5KfTEJf$4KRd|uHhuXGjZIUa|Dk(??=XP8QBDrlq*OcPnY);tV$!6uj zjj8A32Gwkt$fU<)z_TF2GVTrMH4653@Htc{6$Um5{0amS_*)<~eT!|AYlLVL8lP2+ z7y#?dU>wqHoK5Exz{DihL{@wXv;)Kl@D7 z<+tJwh(WY(YBVN|HiWERo2O=?5x;#jR-yfyuD#aC3*?Bo24 zm*LpX5$Fx?Hpwl^!xi9%76-Iv(5=$g-9j`q%q7o8KGHsAfpX&9G1uippt=SqdNPcH z&ZOz!fEpvPO9FOrFf<3i_dQ@m>FBFFa5~G;VYlMK7e($umxAKkEG=I~wq%##+{x(%9N7pORK>3vfJ*Og1{myD2PM|dKIL{RK8250wW9D=Qc$AFI5#t{# zBA&(`NAw97H17eqynCYuQzIl3fnu27cz^q<;gniYoLCgg3n8CY{)!Iu>P@2DnqDvt z1sMwke(e2~a37MMfr&)8Mh#%;4=BO7h)48Mnb%JHG-JwQ6ZTPp$Fl&&FJa0Hc_{kQ zL3>^kdAE}MDjOf5(yEd&iwSmt>Q8-wxo#IG>$R7%susS~E!uL+2dA5d1Dawm!7_I2 zalC;hb0R_$k&k++Xw*BcI?=K!gdj^}ypBig5F(C)3S|q};#{V{_S6|>;lH$6`8gI8 z0w4tBhZSXco5e{Q^X0MVWH{i|7fQyVWnRyUX+d@lO)dQsnU>xpHRNs|0S=?N&tLh& zpRySeG+WH0scxV_nNY|A$BhWD%|-Y`c2X246!flnccYZ4ChHP={wepT&GmTQZT!s> z*tU45TtZDi)Wd?QsX*QwO9;i+|3si5^?Ag`?foK zU6${hW{_iNor_sg=(mf zYs@fqcfX5j`Tu1|Lj((Hd#cxKo#1V=WT@v0USVP7V;x+)P|<06g6?sIpr%ZZX{b({ zZ#d33wY^<{D`-)QdTMD;MVo4)8RBg}T%hE?<)$>;&K=ME5nARa%zj0e*gGrCa(?Iu z$I6TQkVV={3rBDiZTB`!mB8+k!Nn|l>keg1PjX@5eWnLgd`b~6cK)XbMWOwWFh2b4 z=II-OW=U`TqE5!@e%tEYUJ2?du+tD< zZyfJR1^qrh8v38UB2tI^Q^$Ke6LuUDvK+EYm(Eq3$Wzje*UE4muO{UDSl)OR16J&T znOIn|Mr=W}LAof!bcY99crn%i4$Sgju=_iv1)B6(GO}rZR>f zkWOtq1jW~O=62?@w$>~b?Kc369>6`}E*|2A1q9&IokZU08>VFwV|OIMHcq0J<~)8D zk}3-$7fgWxU7p`!LOJWt%*)Qmx)!_yKGT|YT^MS;nI1p10RQr6!RF zw5MT@wb=u+nCpAc6O7WKr%&EVf@h*tKcOPz?SH@vV^xsLy*K_jFl~Mn7RPc94wdG% z8f6-Pvl(!}`e9LsPmnIqCBGX_&+nd2vmt6Ei|DfM8f#O+yj1`H(ezb8adus^!5xCT zySux4@BqQxB}fP!+=IKjySoN=cZb1AaG&$M->Ls*uBe)tV(qx(R{A%&m!#y%S?|M6-t2SlHMN~Fv03AUHNOMcSiQ76-?TOQgWrbXAj1&ONfpMxcU)0 zLkRJKg)vUlBIwyEehseFn+^AwDj|_iC%!$}U9~3*W6EA%N}%e`{uvIn&G3)B=^X<) zvB`AC>T@CSdi9twX=A@E*Yv;O#ENdWX!uw*UeErd%)t0mo{Oe<59-aP^aQ<%BVT~JhTyq!S z%O8k0B==QiGDzjP8qUklrn_My3<5zbZg<3v#KXxAA?+|to`k?;DezyWFpRB#58DWl zl+*8>cQ+?-JSAX()`X2SIP$b}XHh7KA z1RAz#cHr+&TpvH&@y8a?fS_dLyemG>hdtD!WtoBY;J7zUSOwIdjFMYgVz=3TpbGC; z2EObWS~QR|v8dT8^{}Lwc8S zD$<1p0tAnUCxi$r-k<$%krm2VGZN38Q%(*Aeg^TGNKl#-Qxve;$2B+6l_4KR?@C_X zpq6Aa%jo2^uAj^~`j1^N2G6?57XIY@&q(Gs1kxbHe6}3d|3;9m59O9Ins~lh^JIo6 zz!#h`f;}R_g*Jl*Hi}}}5L!2yNgx3iPR%`^> zuipL`Bg_T0tAA(@%k|ZP>qP{>tNm|;prwZxZ-NuB!;$a5?1V-O2{6GcvCX^{lXwjY zeg81NweL=1pLattpjQtM*E;5XxVv8fBMh2!-Uw+Et_C)=Kfm`<>mdMhXa#`l8`y=z z<_eB}ah$#_ART>}xQ^nPScDnIb2;R~H6nL4z+Edx=KDki!bHv?h>j)1wn{%E$Qtb3 zkzJP$z#DzlG)(YBUS430u$cL;%`J}!a0Mm+05V+0;4pKAyZ9m2$?R_g#4kiX4@75? z*ACsRmv{}_`wfBTx*JDC8`c1Kh>pS35O{y1Pxl_RKpADvci0_S0sX`Z$3Q17lzWcr zAwoRNa&*YwfX{m1o#mf}P45a1MErFhcG%zf)IvFZ4CKyH)%H^!Sp%ip6)vbbHMj|t zH@Dk}FTW7ds^k`7Ii;ja|I>vx8(Y8kjQKQbJf4=iAM4Dzo$9R5T zI@@1A38fE1eX6+}XqLC>KIN^itw;Ox`44*EFEt@ZMf{iVq=KiY*^G_hUaE+%JnVlN z+zgTe5!L(nP5`&$-yGGD0lh{AwtVBR z$`u=2OFjPd3Z>RfCbJ`N<-48wcU!{`+mp9rBgX>pitl1?%Wq$>Jk+uLKW}7vyHq)TDx<2wh+rxD-aM{5ZeeA=n(v}3Xy=ZZsja!iWGZdq^JO)I4;5B-eJuELy* z^Y>EUpGH4z>Og&p@Og@MJQ;2OItc6Q+XG^%QOtvsAm9f%HAz5Kf>HcdsK5EAcr(|R zoTe->TX~&iJDG`8KS5*?1PPl3E#W6%Mi4mPI&{mZ{~Oge)k+^j{LYQ?S*hV^4X^p! zjv*^8eTWM`B_Hp0-tUKJ597(Q1KKW2!18by@25{!`C?X-R8I>)vC_C_1Zah_a~c?c*k5H5sJ=R zvh;(Bp3lIKw(Dput)K6oV<#)s zi{Lcr)sLE!FZ`IH%8xND9VeBRg)J$Q##x{r%LH}4NV==c!z2qb;`68GOZ{NUzTnpn zgeFe9b9k)TN>UvEbh0{THu+_S0=eJKK_Ij43tNA2urUu=4}HmY?ZQj$_-Dj&rL9$} z^CP)5JVX8p=5L$7BLvww7dW}36U)$1ERs2k5?9dguCF=YyB8QZZBa+lq7U1t#+ikOJO(DH zgHqXU6FDQ5s!=fAZV6oD#L=jJJcsOz*iTHu3yC$eNo)(_z7cuG5=f>$ME9Ksu4;;_ zL`;VJEFV`D(IroBjC#6{4NJ6c0x2yA6{|*(Rq18@%3Ra2S zC4^iwxK|qTZ&5n$v6V{QPLTskAZz$I^5!%l8S14=O$10w+Ft`&>>P26zao$X z9-VP`Da(4e*5Z@%x6Vc`I6L>j@O>Z5r#E5D%um9PI(sZgs z9c|C11l5+$iY%5qgCgiRPu|X@C;inMdY%-c$TU-kASRd^?-&*JCQsfhNbTkdz*4!# zxxAb-9;ydvmJF^2AVk`Yo&82>#GQ7x z29R?E#vRt_)N#qWa_!#~CANN~Afvle{WWnz+T^*g8WRyq)W?}GwEOK5URYAI0di8h z-A`U-21}SAs$EIEZUhk3K|P__S1jGJ#LRrAe_!wqh{L@;JUdv+kvyDf`n(PC{qK9W z&9JW2v|(su>}D2vZK~Z8Ozu>kSxMJK2&W6E@U$!99%^T@{ux1LC3kf)+-}?5iAlzL z=+X0v$%D8h+aPOGBvQy-XK)POE!m{Iv{p*u1ImPa$v_q=^bt-=xnT!cyOuqEhci(ytkGG~IcNx&7>J(+rnd83@Q`}#^ZUj%o z4fI1EC#9F`1e5c{Hd?mVaJ%!D<#PId79%P!B%kjg_eThzubMv)8ay0!JZ|~HWu#l{{nLg~LWqJX8)SAR@HAy`7 zJlScfxx0x0UHvOj)Fa`KktX-frKE#&R_kCC)kRcq8$5du$*H{a^IdK3c7y$aVoY?M zB0W736Pa-N;%u3^1-@Ltl`f5V1mvV>w&Hk^?~J?n5itX2ENnd~ChyFA%O%ASwel=& z+OE2^Dw6fASdZ>L(WLS|jQtxZi(N)R;KpA=jw98TYH&Dw*f_m@R51{yiPWCceMR%e ztu4j%!s{m3yYyPz0wJjR({*rI^wC!d_69=`GwQpyrbA0Km6x=(+LHCGk>K@o4)24) z`HWG*$MM)hvA@4aYzQo&7zq`H*(HI&8qU8Ozkmc}cu z^7hb9c4Q zThmerxvP;TP?Wg1Vc@^!KRAmq`p4qARx3~)31ac5_7vS4BZ1jTuhC<99L8Hy;{%3_ z{>2N+U;ZX=QJD9xk^Q$uflD~gQ$;^#&681wKr28yTk6K8L8*@xkzt#_>tsX}5Q;$a zlYZBeM1?nZc-_GS3z;p+3T55-G&U*0tTfgHNBM_m=d2|c!}d_>o~d}_+TlrkVbB$S z1|ZTxxGoQs;WI>NEI9+~i=P+KRMfoOZPK#9q>)*vMj9f5{G?&GWEV)Hy}#P6UG`H| z>|IZ76S4Ic{87{~^pZruQGM{XnkK@KyB;(nglnT~WFJ-f_0`^0%KA#x zgI7{j>GhGE3quTck1c1~L*OLYaGW;67OO{khna%Kf<+hS-ezbOTqcB^HK}@Y{=3uN zOnRGulS`ckqv=TwQB(H$)zszzXsCuS(`Vz?qf$G(KN@qyCON%{@ zq!Q0^oKs(^4BL(mCaiEy?Gqvr%=k*m+_?a72DqS-wOXO~WWuHPg%9%A2-Fx0oXsF? zDDTKode_OA=%65_q8eu3&dz zO>$z@(4qTW1;-`2V~({|>mDy*I*AYLhUw;};61l!~p{GjXM>hjH`+cECk|SL?i(m_xHG@mf-zcOcBaIT* zHqPa$%dUD96Qid!m~q}@&*Rb@e3j8wjS}hugG)HuBu55xcTL?AU0$uV`EPQMm6Re3 z?K9K9OsBpd){gk`NdmU2-HhcSD@pj(eoCU(20eIes3XSf#D4vgc9EZz#Dx zIcd)5p3~{Vt1MRpFJs={aPVvKZb^bxG=doL=`rGDD8nXeWoCuvi+?JAABhlAw-^#% z@-Dr(<2n#V-ME+b*ybZU+Ln)<0{8IW8Gc-h&c+4@j<0WSS?z6g23EJwa`(cOBlRZ;QFe^6Hb&T_HXLdu03eHpbr1)GWLU zCMyFgGZsZuDSw!)8(A;Bsbhk?!p`(6&N@w#f`u@a~d!B&Ld z#gDEA9+8B)t_1pBDdk82%oV z8W8OFfcg*Ts1QxP>2^_WixH=?B0zEbQCNfB2@k?hvd)R3>4W~FkZ&Cr0DA=v1fW25 zw+ffj9?Um~LY4fz?Y~?@USjSVz8uOlk=iVDwuXtZy%#PpxH?w2gc@`19`*{2_3Uhh zHJhVX!GLo4MYZL0p4x%>Nj)VxL+2jY*Tn7m#m<}A{)}4v}eopYt zdM#6YWpucMF+fkp8;JS**ro__hI`aaodS2rgFaBVUOU@%z9W2rhNc~Ei+6$o#h@Oo zq<)WWOY2k?@L%Zn-PCds>T91eIor|z`jqZ4toDZ5rn$%ndx7tnB(~(13>u%!*86(a zOJ1k0md)IGvL;+`#kLKBgbHr6z1kPI-Zf&1^I=EHX4dmztc^d79c{fBvmsxkHCOL= zB3|Oh8E8Mt>~H5FXLyOz;;{GE|JsHUp6sSsw%@PGx$?Mb%Uqz@PGmT*NhdgtmIud+ zOvn7La7aZa23AKMl9cxPZbqGMi-y-Ja?*NZ?gDHXv##w{`~7{+Mg2coLd~95U>$Gi zs~E10!gwtsqkIR=$);dh(&CQZNEsoYHa}h`$_v`VlKHbdn(x=ryAF(QZ|^o){pc-V z73~o=CdWAXZP6sTeK1$=TG8_%3)JJB$mj~CK$c?Di%rUp->(>*n=zyC*;UJcDjl1WT#`GV1aIr!PRgsqAR>vr3PJyld}sl<+anrl#q1Jd)Q2m`9>FL^ZxZZk`80KTtiJL2 zajJJ!DszD>a5$CFx}!zwC^=^YWBi(_vSF4_`+y~>$)J(D!b2#UG2|7U)AP(e8LOD=kF_u@gbKGk4Y;|JOpf{P(Ly_ zX|51(yGzjIdj-+lVRhZ`82*jM#imkK_p5WZ-RV5^Y!~{t@}Dys_7p4CVIbp=2MBg^4C*4-%@K^=8EJ_7DAhe&FUQHfJU*T-SRId8 z0qJTc2Z@AF9xqB;BHa3}sJh3!k8ZKU{!Yl(*C&2Cl5u4(zZoQO$B!N=v#XIld8uh{ z?EbDIlU+RUn>;S%L8lgfe|LY;en7Vn{}-W1nJLl_ol2QeP4m#q0~`Y`QuH4Z1DNvH zCqDQ(hTN%)n3X-f6~|=Q%Jd-*>lHYwsaYak86lZC&LlbJ3q}g|veS08wfS0HcMZS2 z=Zl|HBkOQ>^ow3X8I}W+pF7=R-E`jvm&jfGj#XxmCnZ?_>YL8gUiD`*WATH#IrVT? z=>ZPV4cZ?UjPO)%`awXm~uP6$rVeOH8Z#8;8`IXtl z(-i(WDARb{6*A)3I*4ft^@9);;?OVx)JXA4@@K68@7ySlAxa^9j2z@f3tQ4MHBVO< zyKM>`H27sGH$_=aE&%qZo~4FYpv%mOG1@`}v1m+q@!Ex^NXK(4Qa+ev_-}Rn#lO29 zO}K8lxO*){ScU{h0C4<=D(q^{cqm?h-;k0}P`**uKMlQB_G0-BCrT)O>p5&7b);mq z4rZ7I>al(Z|D&H|*bvI~OT0PxpU?8K9q0|EFL?ymL2|JtJ3FN}DOvGqEQvY3ewkHX zctqZ{txeBPFW^utkYjeXO`ijKcg4hDO3cB=VXBzPvvQoo^vxxU#nSwu={3@xRDz zNUMrGC$s#EChwRVbnJUDm=ELLjFbLuB(Bp^a{ag9{pW1aI!t*L5*6?&*ZUw;Ek!=k zz{1{NE1BW`cG673@f;-OU)7q)&DYiRjRr|Cl@CXXor^XS|8;279i!U)5igr3V*=H2m~)L4sIrnZfS53SK3jzPy$J9 z(G>w~Nws2aMEjd3dDt!xC2oBr$!#FAqCVj>%LcJeYM2a=xp}T_L9WJseeQRvD{wr< zxcOh_bkDA97oK01k?;0dSxu+YY)zqc!vYp}OMg0``=;h3+x}GX#&TjYH87cb{So_- zFS1M$^)Tp=k!yo$z_^iZC3)4l!KCM>hd(v@H`RA@aIjxQO>-sc>LloZ?`o5j`uy}j zN7*M@7&e3v3Htui&c8W90WW78f`0&iQw+=cVBJK3w*dPt=?i!T!MYX~IzTGdEn?z{ zc@y>DmcOn@nvUhhBy6XCl}Iqf))*P8w_$SeZW6PR1YYu`q<7!H(x%@bMO==0zWcgQ z@?&-<#NEztFP)3(ls*iv=Pk})R1l8Jr#_RWY3DE?@Ed(Rc`j$?4sMp-cbJFK0 z?9?LN1tW5^7{oMl6zYMz0KX}VODu$$nuS?$rFrv!I&XbF_bDhMDJiT};=e4xZkl7q z#QAeY%^m&XpRV!i#M~FudhzLaZ;2uq`E!Evg>_$zU&?JFts*rEu-)A51l!8E`$#iTIJI~=V0=`;$^be@kE6g{_>AnHfM z65XWVZrVx8#G9$MmH+Y|A2GWBm-`TwvY^1wmP~Jd!>0(_edCLL>}UG>>`^oQ`2#6nz3_`QC}2HbeUOT7N(%ZGSDOtNo?MZoR#u zS!~j`7*HJ(N60ekprGxI_os z?yg6{llkohW-MiysmRpxiST-Kt`)f9>s#DeynMzIM=6M|{{Fv#H28uK8thB=l%$A= z5}Tj2LIPoeKS!2~KsI6RVaKbWD>FxTR3xMcQqFcev&+^AXhV3pGT)~^ zI~UcSNZ{yR&hnYeFawIR3@w~L6b-DvqdAZqw;F>m4L#J2hhJ&<(2TYWdw+(p{P8p9 z%A;evTSy%olaa3k=MC_AKA68gy5A>{g!Bv*w48EUs1;)kxPIoO&x`-i-#}|#5-^Q0 zT+o{GWv$Pf3~yzumYvO&JryLhBO-ojHx;+)t&(yTy~{E)unFdC#ywhOr<(vpnFu~ zl#mP11+>?x=NdU@QWqOiN2h0faQeqAUpr{!@lypM64hhXkhS9K_=F4@8%Rmub;Mgo z$=Po;A`7>c#Z(iSV8?OSPY)&;1pX&fOsD{_7ehOsd8n^rnwCY{A$GZFY~7Xin$B-2 z!d5s;N{`JC4q9rgHu-6Qz;Io|c3XoFV3HT38jV$hJm0pMud}n20^cb~UfYEfR?5Gy zo%=V`G=SQ92ByW~Oz5yJPEU^JIR5T4yJQ&$DnbMDncIWgX9w}0Op|gEvzTi-9Qn+sP1>AwbgvdHywbQFN|c}7}cWodhQe!yX+=T%f%Tb#~=k^X&&>}n*fQz=j*z}D=D z*8LP+m&^ny*u_wm0gA_NSYjBX(3H$f{=2^#dYDf9Q5zv??XC{F#A@b>7GpRU2POQU z6C_s=>}TQTVo3o-(`lckq<>fF7O$Mn&3~(q6sKaxonoRaOy&aoams5_kwrVLzwW_x zsHDoUhFJL3U8NF#eSUqN2e`5WR3jmxc7WgCruuXU;$?^Q>fFKL}#KpRv%W!>EKl%#ojup%NTB3r!uuw z4Y5XUH$B>`k{wtOj;<6$(!{1kWUma}%GH5O|VxC~CL-QCa_U1lWRwGRocu)*UUUzf}#bkvbq zl|gfVZYZ=`6)ye&RN1au-gN92sm0%9s@z+0Y44}Hh^^&hOE3QVrMEh?6Smrp)w`^( zLng>1`1}__^Z4dtx4O4jh4&}yCrg?G99{a4V=Q~2>YYWoQG)d@vorbkYd9+4rv*0> z)eWoZv`O&nTb|x8Jx(w;ta3-VY_{yxV6WK48}_$hO7<8S#9|I?>9)7pm?g6bKR@#gj>hU&^c=j;_0ci%mg!c)4lHE~6~sJ?*A`Vr0=-JxHWq5tTGIXSpz++Zm+4e7tfp^mc?w?j!!B%eZptpL*fRXN_upqRhBjYUEv`(83=-WL3?C>Oau0*?9d94g)>EnPMbTplFL~!m z80JdY(i!>aQV`1>Ay}* zs|x!=&#IRu$#79}URPZQ&GumA)F*wJ#RE~%xrAjsk%&>NsH$j5Tm!>EFDf}AF_DeeTNmk zVM6gecb7Vd^x1w~na4ZRR&~`*l6e1s9Uz@f72igHAKsRtI6F?@J?C0gF6{?vTaY<3ySyVp;Y!NX=6=J1 zcNL%0RGsk=ZzEM?_?YT@#BzP2)5)|+yCt4oNo(`w3H9uoFmM$}kfk<%&!>q$w;fO7 zr8Yu`4{xya0&oZgoamg*_$dsEXS_XPe|=af74S_6rcyvTv4SPIkPjl6F|77kzWv1T z$^1?j_V$;)Io-KUFOi!k2`k*pIJh=kupoyv{DKiLnmtxxruq+X^}$<})YXG88@v$^PaExj*4I%UMEG)=w6&ZmCvTK3l%2)oET{M-DWP!ZIi|g_A3?0G;X|2`H(u-2mveh@Hk9mqCz45{O%km7$_2WEiWKViIs>bWwKm5A@ z;kE+L16ieu)M&fUv$c1bNa^ni5x`X!SMU%|P;94I_4Y81+B=JP$dbK|wp{12rYokR zP~PHy_nirjgz%699~(mZ7p>6S>-(+xB+#=$TZvbNHyX~(RS*D85bLU@WA&`y@}T#s zhAT3^2z=|?#>B;IwXcqAg}%d;q-)j3GlQ#WS+slz4XlG=kj~bEJsS?Pz^kAVcZuQl zdv|a=El78k%2U6mKKkptwVz`0^pc29;ZsF{ivq8BYjTzE<*gmxiU9C6<`vXje78H( z(b{T9-*Yqyw!o&X3<0BK>z%Pbbl(eIs{~bs$8R1}%DZYLh=u@kl6d28Vd40wnL3>+ zaJG4t>%G3ST{#)j8B43{P1 z9YPgk$3S1%Y@WYz$8?@)iGJ&H#~iGVOw&;(i6gB0W7F<=W@WoJYaxV1tb)gqBT4E~ zj^93W{E|Zwew#BV!&r7)sS+>|zNSTmmK1cLS17L@uq6Ko+4{3}k+CNIr`*^YmuDMS z8ogQ_$!uLUp_9rkr==S6#h4=Aa&AuZ+7681|9ZfMD#*n9ILH45e7?Y{vyIAj-$TDy z7;$U;(S2CkJ|flXKJaB!>F4)H;HK2PL(I%p#)kZzq(J#T#ib#cyAtsYU1qB?tft_IkSrmCrl+POXtAo zxTf4BpjR-4JSOp`8AXrfLCMK7+}4|;Qb0vC8<*P&kjLA!e<2!5$4@1F8C8y0%7xyV z4kdS~i6D0|Y_F6sF$(yo7ul-yDEFA)4*OHia!e7inQbv9h5akVN z8{oOkx-qb@6y>N~jHFa((&+U%>w4&XyW-IB_yW9tU;N*=?mN${VPdv5x?O2Ol%UH^ z+2c4h7H`M016_Zk?EQObbFv5?%CkoUku}#e^I6)qzLQBe=bdA9KAk@HEO8^{a+*_) z1U#*vfdI&4J%+oS6ZBVhfaug@993~n6fbd3tnd~2F~k0o(&c1*)k-m^xAHyG?!z?eIk9uyiWZbU(avXF;KI zB;Z<`V%Qy)Oqmt@i^4b8TGqP zf%0JC!+9v0XSB2s=RNsO#aT4jY)^4htbMx0uPcSA{TTsYviE4)RQFEtL&>`8yWc9j zE?DIpB$c4CxNW3?|KQYFvCL6!g``LR4)^-E&{i1eC#xnlJBlk5oqSvVZ0~FemT}L- z7CjsHrHv(8TFWV8MByo0W7X2{=iUK7&cs`6I!tLt=24nPydu!}5!_Db-KzZ0I6isD zz(l&L?&&rv$w)8MC^UITXU)qY%R!xX_pTQ8Y!uhe6T*~kM^nZ&LQQ{vl(k6gOns?~ zcBO(V3Buwn0}o73%9x>a0`xBud8%|}&zeRU9prNg!S@VE)rEWdd#YZF5m&<#>oKr>4euAexfa&#%h^Dpe>2 zi#G2zH7Yk>8ZMZ1@AwGLj)WTi;wbr=NA55EsZJMOH2>Df1vu-j@m12TZID;GWX$Kb zzx9T0wXA9E4AwjwXeuNln{TU3>1@RO(FmSxT{h^xg?|G})s zBiL)TXoOJZZtuu}9!LOi)<3Q?R2nV*T!1lLhLpsWPD!z2gLUY6oO{JCL?}=U?h@3iBPfQ47rwBN96jpkdeU?)+ zT~^LwBpP>OmPB6`ZLhICE>%wXNTr^voZORt)rAZy1i3yE8VEePtv=nsF8d)lt!WTr zsxS1+fo%kNHFkOQBMDPW{{06%?b(on=ps+&0NmbMNq| zh3Ewyf24f`*pQWmj#+Gnu)vNbi=j?BqKJ3ls5G2x7&Gy!Ts1(m zsIAy^c5oIUAldd7{%*ELNk_Iie1rV+4*-yyx7+FU*Y2d}^G}Tq36k_K{kikm)Fs8K?CdAD413Zv%?~ z)+L1=^2%y2E5)3+dXMz^x}#V87Rs@-Q3>VR#$IP+QMhsTnrURikYG5w3h37Vsp3Y# zzaCmQo@SmV!B0Y7@+05s*dEiZ8yRl19qn3}>y{+RG>}mz7T>ju_-8uK1nz!&0)H!s zNLXD%{>pK##^e?d{wHtQ;1{?`<<-t%(tswU8zyoEx4yPv4G>kcy#;mPm>6oSjScxF z=11(!Ivgb91~<0fEmN;rJM8@&F0TfaFZ*{La9>&(4Q$!4Qo=8|bhD9!RA;UX7CxEx@A%NhxNgixNy;tj>SCeD| z3XK0Xzlk;z5S+X-HnstSRl{ifC37e{?PkGZGG?L{W;Zxb?UdhDe;IxcJn&T?@`v-=XuHUa|6lXv#kBwp%WzI_#ZG279Q7d2LkI=w> zH%nz69v=UWh2G3WfaO91m53E2OP{LqEbz6@6%?h#W#wLeH=m51(*v&R?1qyCQ4!_) zmWuJJ;v#=4?;I3@b}~z61~SJHYb})1vAr4TS+rLv6RBdKJ=lX3`|nHwGI}j=hcsF` zRBO)kRNbYR8XXw{mJGUeb>BlkKlAAr76&&8w%C)9mge(ktsF4WV_9mf4D;tX&Io$s+Y8&fK@#{_ z1l&-Cj-SWEe+tul`%Tx)vH~fC3@?D~j4HqYuf^UXe5{^|^?(Jqq@L(XpB6|y)N;!o zmGV>F0-Jx`i|SfElv-)E@U78!_kTU~Y5K_6Y!>pWrR0Tx1XKX%a|kb80F+|fQxN1j zS-;rF*JynKQ*!0#1d-ee)^VsQbVqo$w=tggx>3xB^ zs**`y2Tv{dkOOBB&K3mfe)YP$(%vBqC4VxWPD{Tt8cs;w(KRu7xls)P+N(dmn4-4N zEoyZ;nGxZTtg%tlu!l0)k4Z4iV%ar^Q^FpDWuG17c@gpOCW>`;xS~jCMJQ}Ma8;X6 zy5(_%|Fj(H!vmbI!U`j|#bzx1wyLIrxE{SSpy{t7TMI9mUcG|YD#pWk?>GKw`+oVD zNE|Fq0133>2LW0vUGl12xK_Ay`*)C-$0yIYghB#_UJ8IVz__A3IKUbDPgY+QB~We1?o7pa z(AY0CS!TfxKF%!a4a5}d#e%~txfAiYT(Xj|7$coRD9yIT1d78LYMkot!$E0f*yxVI zaw?`%I&Cu!C}PqR6-PQ3!%r$S74YR*>$k#ljMrZwsIJ9hojHl_#AXc8JloJZf@9*A zSM(y`&wrt6h;m7v!RC6QCDqI{BTg7~CQNEmc0X89g}vs1g3Q}5b)GM=-D^4%+DH$C z3$Y|55Y4qKzSB?1v>j2AxrMl zu?7xRDt2n?^g9{ENqS7z@uHhy#FxO|k5U|agc?Tc);v18y8ZO}Pk!?brOL0ygi{Jx zS-cckm2fiJ<%w-T7s=G|2~oA9hAXa_R*KWf?Z+~c*V#$VM8VA4BC;!K`b5#d^L^1; z$DH4^*w=4jMVl$~5^WqZh*=Xwo%s_t z0_d|!M+UPmxy*X%{;%0iZY^?& zGjPcpjKIM9CEPmnNQ+bBvHm%kfxOB6{+XsTxmsOojNKmaC#4Q%e!;(eTZnz zQfe_8enrA7%qxiC-A8^F)qli25SUT#ls)k0VtXJ+GBOwzGPZPo*>ZBz;&}(9)YZ0} z`cw~MgC7vHv<2OIQp}fSmz1q0s$#oviBOzLzyFJUw$_UAQuP;qi~(lEjD?T~`@Rzq zC`rl`3s6)SRI< zX^i`G#bw)XTx+G=06b8qVWl%R-mKC}94DAvWKW?gpe16U zE=?l)rzjyM+$rHW(hFy$9=7A8WZ~3Q-G_&xZ3jzmmjLB(7Jgn9--Sw^U@e*d^dCu+-*GXY&9GWsrn)t@uC04IHFZxl_`O{?H zId{y{fjDC>2XVdSOFM?8FZ~SOehz4Ge&A0>gyVWkaHn9O+|eddZ}rxg8Z0y~Em=kz ztRL8H5BNdXgs!A9-FQ>Wd^d~nxJf7*cf5_RrkeERZuf)}V!)6Mhw2FRB_V@F!XHJr z?*9I<-E92r5}#w%>ZqwRGb7t*a!x=@8fM9u30E*OeDFB@Rz952Qq*vgoz_;m$g0LT zu$ue1?anaa4poIyl7j&Hsw+%rGD9A0E&Bgx`UbbizxVHK+qJpP*2c}7YqM?Jwrj&? z+cq|1!e(!pj7^)L_vib&p1)wOYv#;-&ix|QUm2$den1{3Mnz#47CJHBF)>PfC<#xs*=32NJrflB`Do42fAu8%!BiTQiw%e~KHoK;})OmHv!q7Vle zXyNM4bL#2KPlb=r@9UB4X^x+pA6FjGnoh_Kk1Ekc!sBSZeY{|Cqys2b^T*>u^Ys)m zm^_WB+5hBC@pq~$7$C@<#8j$LZK9GfauB10+?85S@l@rylsx89;w#Y|^JNX+4yWHOoNZQO+@dKTmllaNCrQ~G6jc8|qE@+2-SUK$iZvBg;fr&)C)mPOf ztZS1PmyG&xl1oIqcHu4WV2#p|Bo15Rv`hL=rm_tw-|S@Q&si%vO<2o({{DU7>6|VP z#qLqRF85o4VvH|2YTlj~ea@P+Fr)r-?}UI$nA%7Jk~@(TDAja}vk_mOl1pG_O5WT z_Co4>#43FhdsfKVfcnjCpNn`4)g3J?^kTA_DU3GBfztJHsq<5^_Qi$r9d~rYk?+(? zIow}M+Z7Pz8*}I{#mzr!nKO9`tM*g;f%i5lp^-Z-#p2wA|Rg<9Pdl)*1!Dy z_BKUo6%n}BiQ|wv15JT?#5Lcv9{>IRqqcA2H>Z@1#MpI_X$k40wI@x3z*^+AZIx|L z6dOOSFE~Xu5vE1Mou!RO3P-&KQAv{uN)nS2ny3JF2vrz9sxxl=iFP^Jw=Ohex{_zm z=ThDbCkGFFsLd`vC5Po6G_--PsbO;Ow0zxWqfP|%;}n5QzGaQ59634L{j|& zpE-TlC9a2mgQXU-FKEx=?`v2zr6!nlDn1BR=(1MnwC{|V2Uaanww12bPlV$aH!R2g z+7ZNL#}P8MeG!_KH$f{ETnt(k zw8mX{65Sq7$LU(cCAi_=^Uc^XcvA3l%SV|4AcZyldXLFl1^1xf(Uv!~4b@DZJF;#5 z`@JaTV1%QrL2JN2K$!=N3tH!~8qxCRS_0G!T%KR#$AUxEyNFZFW0y&@0_Yo?$%9X)~oU+kz(6Y#@_|1n>% zXXl~LCrKX zja&XPRt^%}sF7*i*im`*Kg(CBp<(Aogchj#N^l} zlqKkjCEvE_+dazQ&n)8bD=o!8Q&=bSxBZIk^2BpwZ32I+hw__S>|3PE%Z2u1tv|MV z>M5NhU;)m+=GpR%@Ba0a;2GcSu(73An#YZyO?4kFhoIfA&7Z?Srds+j_1WpCoGLo4d1 z>o8=`=%I%+WB?I1-h>y9G)_G$+5>BUe(~BFF#40!v>h`^>fMxQ%#P220if#@V9NJTU)qP8VlkJ0WRnMbMW+ zE=V_NjyVO6(qsW79%DzyVdaNOcmeUQoTfQC)`G1%ssuAXI_$7mjaf*7%il!ZDjFj% zsU`aj&DE%;0xt#3-Wto5V1s!~@h!S2idB^3FYBKS;&G$%c^V$HBn-j=7somkCiZ6t zQ}>uh#4fyruep5`CXydlpT7iNc$a_CF|=d94`u*NLmg#tr#2S$Q*YzT_D77jeB6=g zE8<-)PL9+5N)H5W_4-O=p8ZRu_q@zDOcNS5lme~QhuttQt47Hn}(D_ zoio}4FEeY|5bVL_Mr8=JGzTVo1{&g@hScVKR-dusVEPZ$?C?7`|9F#0Cy*3^AKFrG z!}Kz)=+%0PGP6F#hKMp!A8gJY3(re7yBuudd@T*g{o3ha!&)MNq+) za0qia8+NE!kD}r8Ij5K5bJ`86+803HB*bqvs>-&Poyw7CFZlb6L|o2`paH!k!cWwp zBlig&+otk#J~tHYhc4q91#g<9QpRkm{W$F_7XNrrB$TymtD&`qu(QsLh@wb-t@P{` zMtDlA90TAFYxLY0*V-df)gHOy9gmV>foN!}dp5h=?)5!q7xcNSpfsqgc9&0AAwg2M zI{8DmSO8D@-7{^wr!)<^K6taO{9dQmvaRw)So*-%C+{T^WZ!#vW52#n!&dg=&Q8Yp zuYO4BGZyg_F+msQyf}ghq3Mj46LLqSUQ38n0~tk3XOM~lZA}2 zBcygR;OX2d@A|g`SZsnttF2aF?3WB5rbxpua2{}hYL7d>W&d0}F>EpIo~aO$yibSk z^3U5NAK6?Zrcj$G8UIh7)1c9qukuor%YGlRbtiKP9sfxlwH|sq9~`*derhNALI>FQ zpUKbFfXIyASu&SlrRH7lR;8y=j^+L);1g>6P3)ceCsqH2?yCp*63#3;<+~@>Nen}E z+c$k7_en-q#VF`sYT`Ig* zetp4n;T-2!DC?PrO;Jx5+Pa3szUMz@EZY2hgw6g|sLLi3Xa3#D%7cCyKbvbSe-Rv| zAtt!15mw5XaXRx;0fcnYsD#7uv1W8D@-uIj1a4B!PNx)qe1J_`qLW$G`kv5|O@1N@9#S2+o4 zt;(eu)s5Xf*>CU<=m%Gwc!^nV?EMNGIb*f6ZCdZO<~`WkPA3X1<46_v&!4HldVgL& z&U>xXsMownJq*v2br(}wJ>4~x>O-^gV4+heG+f~sp!7t8VnwHx8yQlT=NqXfi5bYr z)nXY^(wWz(X>`lnOQ@0{fLtL-C~62SN)HSe3%F42F3Ym2^7bl_PLoS?-2Ky-?w+KX zg}b|_yOO4-pZ~KuOEPA1R`ONv%ht;lda8&3|KDcJkM8`_qBnSZ34F?7d{d^B5tT^R z;y4a8wsgFVrV1PB4C(TI=Mb_VZ}*=3}SSwFL;*0)g|8~`n=;p=BdoDJ-8g%mSjiE$L#J{Ej5;&tH<5IrC9|DUyiA;?!&7ivFFoMo6FlD zS4y=LxM1q%!rS!#6|jRm!P;5)f~Yt7sqe|_;2;y);?D|Qk0vRNKr;IT$J%sc=!(_H zehDY_)wcN5-*m}#bWGT@Vm^Cx4cR<7blM6&Zv5hY+!kcmq7NINg6`qAU)n7wwiD*x zO^Bs_1wH1HonF=adT~Jiv5!m81_>kT#T~|ZRq(i$_XT||Uc7hHn-}sXkQyU{_v9G- zV<#wZFtXKZ;@MR>=$W^@>86UTCni!Fk8E&q0@uo&nb%>`OtjIV*Qsysq1CW6;?mH= zbw8Ql_wS@!%XOvXhs>;e*Tuot*_mIy?HM0riyJn34?NiV=4mRU?Y{caz7vNtOt-{>Z0J*2nn0r%q+DoK}Qw#@TM7C2b`;MHa_@nq5F4+ zw!WHcYBQkbi%lR~oob(96fbh*bqPpVuh>5(t4y9m_ zrc`}anJ`=UDRGt7hvnI?eF2EalGs{w$3mdLEzA>2!)EaIOAS-~!HWC~LZkov#v|Nj zst8ZClaR#1w_SZ-r>Horf{ey1gnJB~+|HU%Q7D$q0yRQ6NRW3fe>^glInumV;$QoQ-~g=M}fX91_B&s#zcS+!~kl7HF>Lcx{oPAK_Zt_G{m~MQ*xaB+JAqz35TJ~J2Lo_ zH<*W(_-9WJ{D%V{sC95Yc7H^bWA%`>$6R1#;`Xf&fqQ7M^NP_#9)wI=rjo>hal-X- zQTX6e@GBVu{WI$Wp)W{x9)rF>Z_Qydq}zu@Z1d1i-RpSDY47U#ugcZyhM7 z!on6N^qtvX45y2pANHJ@yy{_3XgUq|mAg_|KEuk{hfhmT|2B zfGhlaWYp1dcaqfr5|th>+&zS#7F##GPrzEpLfou(+hlPRB(eWcRr{`?o=NS>)For5 zC1K$it@xJ)a`W&@h+8T{MJ>x};Dre>cj0k6suOa;(sbN& zCq|6}ql~|S^k6V~{={B76qIOSd}h@A0KdTMk3QPuwUNW2?b|05hJ-G@njTMMi)bxK z3Uxv>ul$kFrRT<%C|Kd|iF$ukQT!oKK7SY`yDNk(ODEPe@?L&{BP%G@gH5#^g=v7) zjF)Kguy3>}bR;j ze9}nvD@gl;pmNWM)ElGrXh~5$50rUqz&$@qxBn6$i5>#M+EWp9yZ4NE?(rsgR*`d1 z=A^fv$kdh>z2OW8g8gG;cE*KKy21|+-d`s3g?w1c&yqYeqnZ7QFQ+)NxfWQMc(Joh zw*E1kC{GcuYWMmR3iP9aym9_X9e!qw7Oy9~F@IxklQMz%!gZ6?Kv%BA(H(Lq1+KUL zk?rYMYG#K(WB=PWok~~7e*>-;)jY}+MRu;C;ZJ1}g?FZ<21RUGCX=Gnn6cA=cHQ~tGC(WH!5_{Is;c_Uq zJgWxb#@@(a4*zp-61pT*u*ufO$r$3S0@ijNFgo*dhvn>g4N;@2od>w!Q;1$La|7u3 z9+%YhP!x~T3*_L4{X^&y@&?z=tX3RIQ!t7}YSQADpsOOEOc!DqoZ2+g1ti*-hY<~) zN1LZ8Ngo9DG>J;Pn>tx7MJYp(CY6>n(`qb_%nxR~Uz!B}6klrGwHFkV&Ko|gSr%_mx~)i*BgzhR@UG)Jqucrs-_-z)SB^hae)DFCptU9|2;Kru%VQ z!m&-*wMo=uCsNd?Pu64qP}bs5Cf*nF0fE8MRlTTIL~~n4F~7Mm28$t&C^;bP-&WSC z4htnl91{W>yz^c9-tGqjQ)fx;s$Kko$WEm1komTB(W0um9eDS3oz?PU(IK4RKc9;R zS`=pV;n0Pb;OLQ~JzA}E&!bNPqT~zJMe-Tx87A_dm9JdjY%QxecrYc&MzAUWs`Ztb zdAdzhqH?Y!GFT_jmQ+XW>)VZh+qX#18DArtS;;h%|3+jmyU|~d!5}0JDdPZF?QRKU zy8&7x$YFD3Ga)akpIHN0S;3i~y)*2(jJ72iThq*OVWWP#F@HliK7U=vY<*33@nx4m zfUt&WNXj6$mhanoGJ&^Fn}iW1Ds8~~Yhh+BBH2Uul{0I4k^eLDO1Ar%DVye#cL{zAagT>K9C5~ENCrpB6AomoY0^&y@8O;$kzx_S0eZ1q}2 zkza(1s#*$*_ydb};1Zi+M|-(C$MI*G6w!vd`B+nOHZ^?(T`cjbC&u}j;|_k z5cQXTWbP4)sxea(Lk3$Ri)IjHdBqkKjd&D})dUa_QaqrhnvFW(PIq+RZxB-0#O%xA zSg#wbSWG7Lhg8iG%kdQdY*xC)ot)ssvBxkike&Zq)wy+k`PO{X@K{Mkw6h{gR6_8j zx4rx0B24pku7QY;+c#!nr<$l}31RfhH2A2kQ;16JNQ*SJr#Ajc#Le;dFg;`Nkrz|f1Zt8l zaeglwz=;?};;R?ZFg@dN2xF2%8zHi($X!=3aebU&vG^EeLYW8zk&<@q6jC>0HGeEIg)l;v^~LkI z({&>*EZg7Z2$raD9GjmDzVef2My7>TN$I;#Ky$zeNsBQ|H`KLUK}5hM>k+fv_^EKz zLun1+CH^d94`l%TQ=Zg$#YoK_x>zT7Ewo85_L6eo{v) zs_%KuN;H{%>L(#aIOOTC*L2sCX8t$(MEFZ1&KXQYeM;i?ZA~cYDwYKW3fD*xLClLL z%y-nhw}#o=H@B4$)8X}JEuh#t_Rm^kTiTu^xZCFSX$KYTSjfK};7nU%&s&6T<4Y}S z;0_6$QEnozf3v)LoghtdB7tw-W|H3-2HlfNXi^?YW3oN)tXrUsOlS;IUu?nrnfgSV zf+#=FFzv-zp?S)bI{QH!5uC6lR=*zlfh+R35d_Dzvof7cU)7(h;vf9uMY1iI;L9NO zXa$B7!@%%QGE#IHqbcevwkoNYkB-XW4&3x;>vQLZT( zfg}3B}Tq4Qhh(gDhN`LeA zB{c6LGbVu3+mP(uNk429o zqJDD&uZW|oEsP(zzs8E%H|+Jip2Pzbs*^BG;>|=Xif`0IgI!Z)v<2CbcQ++0jv3`x z5yc*@(Lk}b+L*jrKnUSOrxMXripZX6tRuX@P@0vE;nI*$)>0^iPW zw5PLgee)MHo$TNfF4?GzfApy6rd5qS10EDWDrHmCQk7cNrz3rT8ak2(O%n=iyApm} zpiHHuAaVO<+8~dR9I2@j)yrd-@sf?&#Hn{(c4=kmwa(DO%I!lh=nOfk;(M(?X{f#|i~z`?_( z1K;FX#&%EPcB-sa659l0$q>I<0gPBzT&Au}7$kXbv~zsAT&V86muYt>+rf~>H-mr_ zqNNvYSOy#vH@fhPAI~nWXt}gpnvr=hF*OEMzj`;>X|0{-n7W=jcdRxr>zRp$$>A-yOlZP%ra((@J?FaFp4NTPU>MR;gJuW1-KfJw`ZpzGt zKVA3lq5E(Dhibf2ACv8n1_}peSj3{S3y+S}2HZ}Gt8m`CG#_Svz}&ab0L9zs6NybNKYR~=#B zhjV>@M_`0W34fXW@`ORfw}r<8&E`XJSwJHN|4Y>9 z^7o%pUxEsPTM3%^V=u{A@Orl#Ox^9>?*tINp-lgZWWhK`40>-S5CYy+mth|{6|6}E z@csaQl3^fDo@@We$nXZ;Xw$)0EKj7n*~iA)?krA`QXCL3|LW5m=$~mbZ+sRNXC~*7 z!wDxYfTe?S`6Ex(@A9Ifq92tCZr$LC``bP(>1>Uue)?n_;OhXf|5KP&du1JGG*f^t$YAs?4A@^=;c6)RL8eC!*!Xgr|Oy#OLm_ zrkRXKVe||1_CNjS#W%I69dvpyv-IxAyI0}L6p%xag(HvnW)|6n5Gh8y&)0ECTBk+hR9|0T-4GqgK!klb@^+iw$r{t8+~b5=up&zPK8 z*OF;2tZ2JBqxdGX!JfFye*b2SEF%$C3?j#3c1vDwy$|M*AtTt0X0zBK;FpPbjZ?nr zPqqAt+1Q;XJ8wJ{YYgc@>?9abh~3@E*+)`?rBKcLxyzRM`iI8O-snqPeTfZB8>L0I z4_@$mU}b*yvh7vHU=9~hSH*`eNbqI$;OT*on5#sFN4r);fnGyH!{OPN;Xf2k7>E(z z*~pehZ6^F$ZN)~5LQRbL%zAGuPf$bMP-wj8k4+e*M(Cnk^H$59^E}7E(^pEpq9WbQ zz04dM)Y;t)Gny6*5{Lg?FW=8{^D|RIp zJ?f#KP=veJ&E4iWt~`0c|5?HS!XqPv)gXSY&2Qp|=vl1jG+@|EpWiaHX{eobNjILs zT6(H<6$lJ#&Z=iM3pUw5nlxHZze1RClzL5Tb*4+4K#6$l(E)c*1WP^k+GPyRJM{6Z zF`cKMIKt<0!~VGHf7@*z%${;v=*J>54Skn4`L;$aOIHJ<=DAOFf{N_q%P~ zW=0jdqymnCsh#S6H}UG*Doab1hwjA^8f8ycWcUOulM;*RVIgi9YdEl?o3|byj!YZo z2ftOIDwpxets|xwK)DWuH5v^FOgwkmc8!h(DCY?IKDY-O0;35Id83V8h#l1g^MUx1 zmV#m~J5DmuQ;V4ARIOudGdYfVfGpL&d8rlNkWQwu748^UBQ`Hb21Dc0AJ3nf5C068 zq8)V8w-d^5Z(;Aj&zIP43I~dZa%CBZ>r0H^*&46U4)&*cgB*B)ME?;fb;rlLneKl4 zCA#TmmY2f`?WgS_&%+Dtz;t6MEcmZK*B5y|bErS17`K7GWbs#yu9q4ygj5*dYEJgYwNg6uLpJ|d^qOFyyMPo*A_oTg6#kz>0@6kV8sAh6{> zqalUuba>*`8T>N+`}iBo)-RO!zVdSXssi6_xGs5Z2O-DclYdqCMrRMq{`n_8_#51# zUa_w+$39`Qu2_lve{NFv0m$B$>E~~Svd@c%d$j~y1q=7xAFBEHlIYsVBc1&trPE(~ z5Jy@KyJ&rjt0iFBJI2sqjm((3gbRzCog5z}$r)_P;XykWgF+WjcxtE^xuLhYJvI6@ z-($;q0lrmo=m_pl??$NaCh4VfT@?oM3pK^D;kdjFO>#tUq5@Zl>9;Y5&Lck&;PO#K zRUCY6r!AVoEc-a+Dhn_{ig&b;fwDZ@L^`#8OUV6Y9Q7Z5TAw7^$i|2BA;+fer3mHz zhhSPibc|54BElg=8+3n$$K1|Exg}G&85zRfViG6`OmXO8+8P#r%Ia@Q-PY-R%7gaf zAE)R1GbYenus`fv7QQvSIX1ng3{ywxvhR7soeEa}3Z;Y<2;2+?nrAz3WHf#5pNP?p zVDWMZVCFs2pf?uy)2(Fl_ys5%DG4SE@!Up+M=pl>(%t$8vY~qMeZwQIF1>G7GI=7f#LYLej_W*cqeb6| z8-AuUIZw!<#l|ickvK294^0V3?8(xb!c{{DK|SpA)JQXt2s)n^Y%d2#fW5uU_AqmV z1BhISUHJq&afWHP5 z9{Xz4`fcNL<(zoA5q3EXdZ<6zze~-!y%YT{0%Xwkyl+Lwjdqby``E}KqVshH`*p^S0+Dy==1I% z99UHQUTakNSC37I@N}lK_BW7isr967*~pN3!@3`J5_22V85i+2tagnB z0<5T}&4SpIuy-$=0&X)qJ#^tI%PbvEqlny(uDgq{dxIk}E=v5>VQTH#M6Yd%r@l*w z6te7BiL;u2Iw-1CP6_-CAZ>*F@3f2o6%UphnNNj;Zh4ml?`EV?gEkZ1fyOsLmfaH> z(#t1`h73EDWjIv>#erFs#XRiyTuiiS>$X6s!%U(d`B~>+c}Umo;l)Hn<1^2;ziV3e zerdxp>)#5M&SXMyx(dM@ld9rO?{p9-6qmzT=O~TGu)AdXxCCAVLR&n4ha*)@$GhrZcC! zW`a6R`?kxqMxhDvz{Xv?@wJzDd!!n%b-ZrWwJ^N^K93F8q6~e{V3W8s_aPtXWdP3^ z=Q`U_(VrYjH@JMt6s&|bbXrde)msLQIj4Vaa{9Wc@88cGvjGK$tLJa$h6715a$lO& zr=akUJ!G5BKo&gZ^B6D2&%;OrpR2rir+82fKINUVy0^m({yqPjM_cXr*=`CBYv8nj zzqIbhE(ty19ft$uc>=R`peSD`ZC)LGwdfR zZ@3XQh(Ck^Bm)({gP4QM38J2a@2b#j=S{K<(WZZso%zQB+?uiOruf$!K(}vEDb0%#l`d(zS&8lyFKOj zBoK)?agWpJI|z>J7=Kb(9&jM---Nq_kTky8a)u_@h4>WOx|$6w`;3cNoC(H`CLt>0{a?dC*wY|> zPHw4ZBAR4KYYH+%cQP3LN1qXHelCI&L~ni`Qi>O(Z&wU)t|H|ue#Ej-hdaIEqCG^Q z{n8qBm5Lta;yg2GmQV`D4tH~BU4)tVirDTo)DGU&<8zPkmzB~K8XIaHrW87H&hTou z#kZHMawKPVcMPnXIUT}kt4+E`4k}oFtgfenH4{UaGFsVDmtIA>y=rIs6wet&JV|-G zGVvEGw!oz*Wb~1n10#R_-%~%tnTn&W%uA)8BAIRvZ*q@pkQH-?`{d-ob#cWNrrq=E zDyyq-2zv+NoXCBu8VCQzE>dgE;d>*r&C2dvY8b{S%}JGaD6l`L&09+dekA z`2_X-U1}tRY406t7x7n^QD>Xq##MQoFI@=Rz_$zo@$Th_7h3#b3WR)Q6@W~#64xd~ zCVyU^;n6{^U3m6W z6)_jWH9&X1&_upi3^Jb6(?S1i$joU2A3Idn2eR*+kAz0Lyu2W*vH_o%k=yp6&#G2=yln5$Q^B<#OF)1R#u zSav_qC;)RO=mYsdJ1C&GjNP?66SaVUpd>Rj`lm~85ofuVbKNNeqO70@D}-tQ%V5o* zLH1qBw3$6JLVYCL7wBc;njTpk$gce3qMIYQszmj9-#&O-5R^#+qI%q!9C5c$=_QeI zDKsr-cyhvh)uMij9(I$5amUzT2Mi8Z>O<`I^$Q1Dlar2rV zx3g;8BABTqcD!_0$gc!VB$T8#?_Z*&%)#yH?Eil)fFA1V$k~EHZCq&L$q8*;d)@v) zvKmF0(PSo69#oE_yRook(d_{X!qP4eNeSLaTYvblTtoI{QQ=muz%0ik#=ITR{JM>p zD>rp7OoSyx8G8|!YPdY}E>{A^8pt7$KI4qq+Jz&P1>;P8ePWA zIiDj*&rD1li>i}g6RbCkmR4M@W3cTIE`48%x zw#UviZ!b;ttqBsOv85yQ5ZBhn;$utT#M6Q54>QBMakeXmIDxWqPRfI0pf8nyUj7`9 zMsHfja8RVZI5zf{*%=ojD}kZ~15hS=XY4>sg85f(4zaxUg`eWfS#k$8IlS!<_*ERg)(F0wN^jSd>fYj=Lz(H}eawOj*NbGSlOWbL3DEZ*gRdHq32}uWNY_#{{fmq|QzU*F^!hM_JhZov=9B^#iOsh5-*^w zzpBufTKTH+Sz9w9_W{jqDu~}wwA%KXSolsB8bDQJ@mE9{*d*zKQ$?@sf}W0`?g&xi zCDBpWMf+z*V;p!*2{tZxAdFm_&DN**Rxu)+Z8evH_N@{Jq&?tjp0t@43aw4VqIJ*z z5WJlu7GU|9&4_$xy`3NhJVbii+0yVWF%fP_a(MdnCo=muTZb&P{p{ZOJN9@N9>mqv z)xT(424tKr--6&>N5d>!oVEcGOcn)gGOZK;k=}V|=v{mw{l($~gRNsv^`OpHy8;>GRq=-l`NfVcII($!9W`zDl`QdpVhA=}+G7S#9wVGHlpZ2Q&3lGjIY$Em zoPI;GOcHy$PTK(Pr(h=)VRCi`p9Kr<9j>y!^=a6M%CZP?vQ(xcR6(aT2145j$(Lnp z7AOo)P>0B_lJB1nZ=n!eAF@cLARA4XXK^7b^Tq4W=7@-f?lGi*<%P8A!Z-hX(6<@h zc0R;)Q4Cy7x|m;OnTnLe88;OSbCY2)hR40nRg@(cWD((~+E@UQ`pJl8`rw~^rPnTn zerUwHDL2e)la7f zCddaVGxaD5%B;k<+hwDINM=Pf%^7Thfr?=m9jS!?1!cQ#Vnawj3+YRT6#aJtw&{RU z(t;ohBD=pedgLPj^BvzWYnS+o5CoY}L%YUk6ajqOBh)$+eXKI%?cr30%WUO9=a$3% z-h(7xIVm3<$!a>P8s~gKW&Gs`AUBHl@hQcRwgUFhQvVw>ENpAMf zNFw!|0N_g@dNU|)u&HxlWLYOf>HgN!`M_}$Lf&Y^9dlb4n2&M+B^(ndaX)TOREc4~uD-ntPtoH$6)0YZ+#-_YtlYS$=MN72pj~o$lJ*0=;elFQR z=X~_CcY^pC0vS@m4td;TprEEjC<3^P@UZmj%JD4HvVp}!rNY3z{BFFCfC(7z0qJV$X&>Xyl;4Yac`q* zzb$Wu!tqy$KFy1s!nAz*)4x~j(gig$chE)SuBuL6YjQ0F?IxU;?^yp{T-^}(%ff&x zq-3{u?zhYCEeb{5ut@YsE!A>jYYHUPMw5&^n)7RY|DRg?- z?o%Mnf9n!T`@Lh7%oQFr9YC2whT~jLN>y2Jh12yRvJP;<$+>qwvu?-hT8_YhG1yO#c0J z*|q(Fq&s|~u#6<(%5AMqd)hp*%a{WDbA*;z`>P?JVuH|7VyrCVMfrliuosKyVI>l! z1e10_!}OMt#~%_`W#6WVBj2A^Zvn{7SJ^*mw}&Yd=_h1Qq{EHoxz)ZBl@SW&9b~z% zXQ2fDgDN10x_otIFITXtqqRMYq}E50Nz|>0qd0Ls=@(cl5j`LK+dM7AYvaX+PSKsG&AO`#=VJSCnFP1JPNbm~$8RswD3zbeO;KZ&v$zHz+TLhZnZ7I2} z!NG70e&Y(^k}vEe0@k4O-@LZ{6z>{Z|A;Tz<6b!JHl$pOqGv8?IY%y<@5Mf1{X5V* z0nl&l{^uPwMyk=X(C=fv9_Us)QiwK@&iKy!Ow$#6Eo#tG*plH%yFC=P z8MkQg=byi_f-)lHb5DQ+_hPwq-Wskv!7{VtXIgGL1#(T4 z2LwRtjMjLjZq^cqjU_p{cc}OmsY#8aLtpB~@xXDH!@o#6zEyUYe(J#+@?IR=JqxvK z+7HzEP(ofZtPMHE+13@Z^D~2P7U9UA81q{Anogu_UZlcVnH8HwMtD_b7LMH zd|y+El%^rQPo5EQ<4|yiu}9IOOR@;ChcB+>GMSg5kUkiNd!*4F-=1ohtrMZww;ZHH zBzCW%Bzr;flE3i3^6Dui6jp>WB+zI(2P%3O?-zF*# z)muk(LQRndET2q`_04T20OR0+O^_Z;^|6cK(ccpblIApBeYk34egYcvt#f4j0;yB3P#>w;{X=6yL-bm7YSXVp_*`Y{G_xcBT59Ck{1l;`hks9Z2GA?oiQ8UPkXn<%BZl1GDi9|wO_(`obz1v z74W@j)eQMpc}+HujTN&B5Y1&n{rf;U?BC$t+)=jXBL_W`KaoQ(GL+gu zGJg5&#gORaguV;~=#hMmAsLM1iM9A!UBKJZ3d3U1_Pg6AtydN;Jh~0EOeLR)CCY`_ zemHKx7r3;AwB$AoyGR(z6FJrI=o3mZl(H5RwAq^;t|Beh)di~~0KB=ZVDLv3y6d{c zPc}UL^vkgG_147Iiq~GGuExrJI7x1@-?V7DK8qPOewt`lUpKi10jZ z%MyI2YR~Pmm44<_S6HA}%06{O+l{yf zh<`(?A?1~rmW<1qK|F46KQAB7JMw8xE^y_Le_?GhW>BMRp!4mujngAx zZVEV z)7Np0H^xb)Kw-{7`M@>fSRWb;)YxvD#yOOJNauD|&5TkLH@ zpklP-vi^;?-uOu~olu6g#b8QyYou{RArB*ICwP4EGj`>=6!QwYJ~gDeRPT7*tv^qm z4OwJ`TtLMYy;2)hh zhe44Z2Uou<2xUAM8iDr(OmS~#XS7XgOeKSbK}T!rce#u!35-$2Xzyv@opL})Ue2Za zwW@rYJe>8w-sUP0r8}}HZDA+S`Qc8x$LSq*O+1butg*!yiFvP25VKq9dHVNgj=Ak; z+!DjLNKNC9vfQh9793|dQ4%ODY>fPYpY3zGXGLuR*A)Kud!OBuB83ZoSrNn)G$5MJ z>odcHddVM*6gjawI(`jy3WxBswy0yD6~~Hb9=qp3Jrwg$VdPC=BP>(l5jonKi5g@X zx|k=xdXU4^OjF1JK9uX(7X(9u$5NO@Iya_S=8RfBbn8v_ATkAs)!NG!l>!2{q(GLfdO%l52RYcL_^^N`NC4hdh64iV2gV*y zK8aOqK~Z*iJFE4}H}1PC`9YCu!Q3;AI9h`2gckHB1V{TzGl-I~Q5=h@_BdG{8wmzN zyiXJ?nynQllwqeU8u=iU(3m0RL6oFN%Id!Jtf|`oCbR`y9ejTi6ARwRH!}UntED61 z)-LN1f?UU7P3O6@Wv3U{T?<{3=HV|1hqvy5$S>|}$DZ5tcgwv&yyv2EKMdt%#|I2)TA z-+Avj-=F-PoSEtAr@Fej3M5c_Qyl)Y)O+$4i@tfKd)6xj*Jm-4ai{Yjbg=X9H@};b zlzF+!0rF&fsa4zNoF!-<0HI$=y)pGZoOqA>J=o{>9x2XvlD6KqU^EXYw70InQalGU zPrvu)hNXRmEzZ&L(cii&%Hod+dL~o>&Zrkc*bLbb*8p@Va4-+#N5NH7Dgy=8i7oJg zyExf65gKZ??Zz?i=Adra;*Vml$rl z%i<3F2${KGnvh*2JHMX?iK{HDSXm0Et&00Q=xkvRy^jZ!48~~7g@az(Rz;GHPrX3S zZ`_kJzu34--7RO(P=wtCM4vkt2OPiWQNL+*eM^Q(J5EzBhduXH9@&b9ggu$}4ZV`b z?WqX=#U<%RCF(1X_91lH#91Rz4R!h~qLx=r>LxZ)0?CZuJ=<=SgUHZ{00F-zto88o zxKBUO{k9wIG@E2|O8C3w_mZ0IM&5sB^dfF7!Gg8NFwFNE@Pm`9Z(z_oN#qnsi2 z;xq-|68NQ=h&>`1UA%ukMzC>yjG9#=^%%{2{orMR3wDHZ+BspZnp@d@g`9FFFFITo1dUr|iGV~M0dfxkrx z8>aarhznAJ?Ea!yAG5`^Ru`Pb0~G|)Olbmh*J^0dk9$NACn@@qGBkLfb+pX&KMpr; z<d z-?j~xqq>r7<>i6**bIWVJSa#7qoXDdX+Af56d<2OUU<-oyz7$fSOtyr7g^w21)}MQ zux%`I36-=^x3YdU`Ax8%97hrQIhHO z6ygH6xB#>5=te623*QSK3S?7ZL%?0bj&ccduwlh%c$`c~nwogGBSh=Z8ko}ZFhM!& z`eLF%jSUTOVBe5@<&~67eSEq=k2_Im96!(GKSwa8^F*5>CXG;?gvaD7M``%-PsDZM z6++tK9|3DxZ`rn5AE)X$FDIFHL}Z<_-;5YKGnb2C%OOH_$!d0=u&F>i>zp3)IBC#h z!m!z8>quY6uzyHMG~|W9Z#wuTJz0)9td1%H{KD|HMrunq2q{VMamdo<&=9G`X(mw9 zb_VEx9Xr+!XuI7_wRP4RbCJqEM~sv_6$oDw2tdukyqQj?mQPV(N>RrZmkt9(9kB!e(X2U00bh?dW?B|tO(hJ?a~yQ;V3kd-@{U#DUgF_0CA^7++}I2P&Lu* zCBD@lC+Nu;5wqZJ3Nc0*={K~(fRvQf_9UdqtO#L5vaxjPFbCw(iaZfY6&Hhq035kW zp9fi}uV_J_qx-$evpIou2EBDV`hY~?gc>TuUx?qNHAVOX1f)6lO2}MyQQHQQ`tE1#pHV&k+<4+Pn!3Kc z^@{2RKv4+A`;8Lc7xma#uD&WyP9))AuyLDU;OBxKkQPT6_F6@w2x_J}QFe zauRV1ww+%l{H{6erS=q>0o7S*d7ue$Sj=qr6Fc%!QnH74Q~zpuK8q>_jPTo$;|(0y?yGbVO~thBI8* zBsMH-yQRfTI_zyq>^_W`3mg!aZTB+87$B1F zbljr*a@giGPmNv|9_({{y0$Ft9Z3gZx8^DdJ3tA|0%ucbM+&ru zcx_uouYrXF%wT>V1inOsPn)C66l3JAG$H^MkYd`Il3C*ZS^|c>x&c zt~BmyAAj${tC^_qJ{6@i|6<}YT-00=-sQ;r%odJHIvMK&aMER z<+$>p@Ozs&X4JJCb!49#?!jY0)n(XN0znQ;!0;WlmWG zb=5Jv39z^Oy$gA@Pb2u8{`Qd3LVz)_POuwjP)egls4dw=xE-agEQ!ybG4-90SHlY_ zf%<_^^B=l{2vN?;??9I>qA!zmx>yK4Kogrs*FKL9s;*t)P2i(20J{J=rhakTwtxGo z)%mxcF)`L}+ekEs>saC{XZ2U2n65grXLz&6$N@BIZ~KxM{WHMa>;!k;NFtT@*Nkl& za|kFCjHE{%p)wrcLr+4#Y%w#LU$6t}M;M0{m6 zMUBJPQK2IW5PcTqm@8qZuMho48)HfuNuzhD_)FrG$Um!=hDx^5(!Fvq> z6i##^xHbIx)yXJRKGKEnghQm!+r>MU8~?1yZ$VrcXluQ%3wHGBMbhsIe&Nb_A!Y9- zm@2dJtQ!mqwi5L_yb85H>Pjgf8kWTYuONH}vxsX|3}|BK)i`Mr2raGTkt{z_9aw2b z1D;96OrroIx~^n!_a4z|qe1O#ie)}mc+#5elo||t3V%LUDa>4{y%&bKMQC;NFsG9HaWpjo2jtMWw_)sw6iMjkC`d zbeG%8y;Epv1Lh3pCMI=OE-ki_t|HxGHIRAn%P}y0v$PgjW@~VgtXk@;DE{eq)2g=% zzDo7mqU1~g0yQLjF!pSZ=HDi)xDx#34Eg*fezC?!EEiWIWb(v-$Zmd$s6X{!CFd z%19uvr%w;4>F@d*Z^X|C1uP%XvA)p~;DU_$F&Pe;+(1~@iAvZAx(#|Rn@yov=Z95T zm=gumCH7Kuv6`DV%mNkn@wQZBEV zpO%L5X?TxLDhwhHo4u6I&RLS%4dX!!W@8S25hA*`HR`ge`ZrY=&`H%%N@PiP!7`o5 zt1+@%lhY7iaCHChR4JkGn-Fusc3kX9-~Y4#|MFmf2M%!@N*j?W=Z}=Em$dFYSI74&A%#k9N1uexZIix2T;VO^|a8MZ5DQRjn zI}a&hg25VUEJ8dwY44DZpLFG0y6@C`I@=9kw-|Jtm1BzHs<`Zh-0`nloDVZ^E0t6~ zryd{2fWr@+m*+mf=#5T0&F;+j?(e(8gjnKE1IZXopRM%A;}kC{pdIBFRNCjOV#Qfe zw?TceMs@LyWpu2r{jIywj|Hg@kC)OyhzH=)nMssIJ-P#+0|lG5q$YO+DLK0 z#cq+Z!Q)3ovMm`Bz9O1qK6(nbEeaetT91(teGNlq|hMQ z^%wRExkBt~8=FiQ!pt;w9jiY_CmFXVu}4EuJqU;i#$-dBoMx*)#Y4fNk=n4x_-x4F zaXY$#w(Kr2T4Hz%V7K%v1E**!l*t8UTUbv;$N`&D)ytx_8hlF}KBzYAK$wT9xOO7c z@k~#l)@c2UM!sTU^`H*ACnyp;&~w>8um^@R1WIyQm{ocga6#cKm~y9Xx)P|(hQ~)- z(or&)pr%VficRfD!)wggbJn5EtlhC;5d)x64rf%FT$+1xL4EXGLhqT%5#=+yG1M4- zc^544Jp1OP)R_UwAFTNr;dQwLF3l#SbFQ&*y8bMka;yFPDXOV-bNr=y$^tj}XV?N`u){N!B%HiPG1n>%1sME3Z4$z9GZ*sKrBbS7#L z3uoW6mRgYmf^uq(ZM(zW@9ldZ@?pRZ7X;8#q4ExF_=CE%4Ma-7JAA_}0f~i?W{qH) z{9vY3W8Di2SqPpv{XHRbg)We&MhrhHHJy)k@9t;jG>zn0*FgUt^pld% z`zWnmi))>i&u(beh0Fv|?cO%{wY3^o&or~;1=JTG)~GYiu#YrnNYJ!#mp0GDR(+50 zda&Oik7LX6L7Es{Toje&fP6oxBU(&QC1M;`mExmUA-Z=gsXsc zSU-y24(Ftty6+(QFbKA2Xlgn?U1+a8_0U4ooBHyz-=Z->;U+viugjK!P31azF#lUH zVvnFFZSsS>M~{b{g@Vf?zO3TE^~1Eg+gsmjo;^Irs0nw2yFtIXuuaXBrs@fORMMPL z=aAhz_vKIx0UQnTjFw!5FroeosELat$?BWAzBIaT<23%EGexObtRfhO)d-ynhK$jG zOa0vhBO$`f?|iY_&aVO`m+jKTu+dy6Q6E<(Fhv@t=kA%FxeQspHvnT3&@`QdP~>lb zhq-W0yYG!uTTj+bNcztvu+me}Jo&Ps^TFgo)1AfUZQz@GF&e08sX*03uhnhu?<9|LAk)(J#QgN-n{ zdwi71&~4I7>L&We{xKoFEYqrE-066J3LKf1JsxGrY(oreO}76AqbrJ+99CnATpv~; zq%H*#QXR!aHcw_qgJz_Vv$>9{V?-d~F3cU^+UN86U*L#w@&Xr#ui(_Jftr&+zaE*R zX4il?VJ!x_B{X(yQ#z#)x>-q8-7IN+t)8#F+@J^tE$8bG&wqVR)PV38SDP8ITUA`sn@I>BBlfC9& zADjtwM-2l2a6a=E1$xF05#wf$E(6~o+yZ5~rfORCAke|Phc88myyKM1y*!A76>%ID zn#Nl}0BxcYfv%h$ZX3VgtHVF8RCDLQQTDASwx0{eDbI~{#1Q%v45pq=#RPl>@p?In zAnzhv{!Q|CXFd8(Je+a>ic{OGfM+JG|50dU=?r>~-^HGf3a_Aph_+n^N1f(V4-(dt zszurugu_%Gfq3g_A^`XAGqug!plzXVtTJ0hRgoV5*792PH2+DbrwY{#;3(t->Zv={ zixsuVS{esKK3rkyJi4M!V!49MU4_yhIMNlf0k)MtF9O3!XwA7^=F}c^D!^j#E&VA& zb|u4t0b~>eN-s%tW}&~V6jT}B)d#6Tw#P3mZ*g&yzsh*&Ea}?sqBX(5`b)u%O5h=a zZ(7j26~edY_N-9o5j$hK-4C%0*1dk$28pldXH6Z4^|F^_ywA&kCFI)I1w8Qb2F!_s zy9ENKISs#--Q_=qur9m^KxQ2aN|fB!|9zZXnnpjgk_te02y_e~j*9>ZK$w};X~@Fq z4A_`P7ntFJ4Y6G79MR%5Vp^Ni*O&xNJVPljS0LwqkXc93UfPHD^>aI!aQx0_pV!iU zU-z)hbn+IIOg8wF8(*qGt!tkcv=N;XY!{KC(IpG^A$6Vl* zEavwsL;~0Q&?XRi<|JIxkS{mbrv@aCD8 zsrtD3$5=P|UOv#9fS%ZiFD-C5=9kOl8=+%4+<+=%HqeL=#$nAOS;ZZOMm4Vr^_TA6 z3+!@Il1h2#56C?0uufTY?Bv|jj5v&znX^r|oluWgE&4_b8hDyxKSrKTr^;W4`?tBF_1zYoE1amJ{={I8TTQI^~ zp+y{tiL+@53)%}Ag1SWPB4rca{{#JKdny3}C8r)ymc0)gOK;|gC9DpDS0yR_OM>cy z?Muh@t8{wE`>YaVEN~HG!lnqatTy6Uce}-xrMN4@WhUzWb(5&m)*-PkyoV*?`K)DH#1Oij%gRS80Gevh1lC3}K8*$W^dUlOG?jISS^D)r+M=^hVyRZMCb z#~;p%@A^T!U#50j`t7L(Su2pmp42WJR9hga1y!^^oasjPh(`OcjwQDFkT8rIJYFy3 zzYowr(_liNeC6 z;e-ua{8=@aviV@KHTDv^zz^;CV?5l$V0+Do2oqA0N(Ch^qCUzC1)~8#1^Bbf()>%XYe8I++CpT zJw#M2tFKkg%i|5Rc=N;{v)6nD`bF;8b_Kmgi5$SifjNIOB7QC|pP7=`8+?1<5l!|- z=KbL_j!Y_Eqri*bB)IzMt`Nt6DUKKvpU3^Zpzb<3xc}rYo&Ft^DO+%fIf%GKP^n0@b=}iVOIQ zvf-VAEGlyLO!<0GuNIwQd2M+Mn##6wZamy~9HJhD2FA*5O)~p3FXszrR>?47DDI$; zMYoI-18u-DI>?wgoxYT0Y9NIC78aHGXzA1vJ#2UCC4H_DUq{D$-d#b;g0itX_)&7* z=xnr1>urlec1M*wz_53DYByaIg zy4^-VTa!1V#sg8CQ}5}+Gig)Azxe=)BES)ePZGH4(4j)z6O*I#FCKVurx%gwi)UM5fZ1aaZ3Ss#w(rf7b2QPewH7Sf6}! zy^P=SGTv@AB9_m>Mv56`J7^a3U$HJi9MnjS_2YDvJ}R6!KiArsI!mtP=U;uJFW?_L zwa}US>R!oLioU#Dyt?q6*ZJ_;K1*D(?Z@xEZk)Li5)`-I1?K8$oE3*cL)lX_$xwZ9nC04R_3FLn z@3-lr|GeEgjO9OwF`YUk)b2+9QOe>-OnO;rjgM@zj_)v1>ek+TEGI14?&Vym{cCWJ z$79slZuJNo)3Z3;?IB4qoKvCrw{U@sh(JZL7k)L3=1FhuB*?9RRzM}n&jqyjuvD%@ zFQpgROzT4(<4@?X_I4xQ1m&hO_&DVLygueXf0(9;fs~Y= zaz*_+r!w)e<@mxKjk292^(vlaQa)y9E1EFjgTQ7Ka%!GJ0)#!jKW85ijxyWVRgHwK|9AJBrn+u<5c>i7RFD z5FvX;V8?MVE@&>-)COP576E?WJY=fEpSX6e9@9GoWmYtOW_n&r6kpA<{&2|7CRyV%!eBN=$$bnHpYDk1-KBc>4@x4L`cU#el@9_`HY8QUl-=)s=4Pbh zgyVDGtr7~4O7gQ8# z)fS$X5>{+NwKL~P6=9N|?l1p&#_?*WAQan;mhO6ET**nSp6)YMa~4(~AzZ3OKe}Q6 zDBercuL+j$EQ`)t$h1>XzCnO$ifX&dVQlzs?J%D z`Fq~&C@JT{U(4fOP)*jX^Esp(iA7npCTa2yja8>{OTp7p*%8gc+-JQ7*7$HY1pZ1a z(4>WrfA=C&a*c>I8~xrgYvHM{yQR)%Qtr;+At4lp`vuKSG%b=)`R9ZcK^VtxcIVRiiOPOayN&y}`gr<`%nl7z)trKk` zSKyGoQzd7${02qkTP&PhTSj&sMf$k2!=qlk1Dni9fz*;YlWI2bivMpy0A>7(H`^6o zM|I5K7V@L6HBV+XdKC!JK`29JbDjgsXYGOA`{hWQX)36ydW2%zj*OYsN!92@O}G2H zcIST;_5T~<2IYiJZ#dS1H{7rxUVQa^=2`2x+5Z-2mI*9f$yXjNIVMAw0m9~Pg@Bs~ z#tb2{s4+S!=o}J{|G6C{IQ&#l9N!xGF|y4zGLr95@w^Bfg!>T8nU&P3tBUg(9N)HQ zeA%>r?XF2jMC!^ecJzT)HQ{`yf!s)!$)5j*L88`6(y zzY6m?XmLp2`Ib-cDN4a6K=y^uDuFNUe8&(idywkqcHHP zvcF~j?gPFK4=G=K>&3=WG5)PFUjgwFZZ1@3696=`(9irM89_K!P^p1aiAY8CH@w6L z^;re#Hmo=?mA3s@ggm|kqq8}}Iar665dJHJ8EkMCkX6eQ9Jci7^+zHh8^08&7=N0} zTm?rPETMP)yhQc$rX)(ab)O^2*n$sPcRdf4K!a-EL7F7T3}-$WiAOaU9n|(n zXf=DiVLw>;XXVc<8ej0{`POW(O0I>vFBsr3tUhx|P{;av^TEgAa*#PgV#gXoAhbd1 zPP^^ME)vZ*Als+0E3nc&y})^?=`l$e_x&ZNwMU~ z;5N@BrDWhqhAuM4<`7MR@L+bX>>;2NM|ri`Vu^Udr*S46m1Z~ZU#DG=y@D@HSCZ;rankv6R> z@W+bEQA?H;2r6}K^$#C)NmFC&L$?lGBHsQDQMPWEW>wz-@5{lQW;_1~i9P>u@XcGQ zQ2zX6wMr03lUU@S@?!VtuO0jM6Q*&8bSBwZ2v|Yv2XMZp?RBsj@TGD-6 z{kfoe)#zAw1Z601fluh(h7LN@)%O zP6vD;^_MNN!!VTinpswi|hAqvVvXU(Q?sKm)Ttx zLLQ;#(JH;-w1!11HO~HzzVb7s&j+E(S|sA}M(eX{+yi89XAGc%y<}i?QPgKLAvSig zOY$>)TS~tF$z}`X5P1DcXG35>qaynd6W4TDA#b>BS;iw)`E$SBJ!jFjoF(qtFD5T` zz)H!xbmr70Ud}3aGl~7;do^|DAl@D75w=VmTnoJ^u1#c8pVScVOlWx^;~9ssQ*He= z|HJ?)1C0!S!|ypMj6Xnqo)vA&kNB0$uhW-*U3A@zptmkU>-QkQYl|YnSMcGR4Ww5q zWBXd+7sX8>Q7)n5$X2P}8NoKRs`>=@^vZpj0@9#9$#&6`ZdFh_JU&# z{m3)ukzI@11$t~={;I<#Kq2BE%Jtla?wgo!dH1PztZ@Z7pWoVYel;Pu*+(gi1pLa` zAC$mmmyJ_GGi+l0$@=_Mnb*CQ!F6-qYlFLVkG+|^hP)R>e7X(#s~bYusvse)(nQNH zhG!{pnN`G^zx0XaKdniHQP+K#=EG$9Uq8dKdBnGz&5c#J@{73YO2@!bBqhJzO#C-K zE_0`y>W*VAGS3heG{-tQIT;fh&&FvmlGB>NTEMM7fMUn$9oD)d$sPQZ$(eWU@_ECv zY_gKZz!P15j}2}+dGV$-mLIQH}fA-z3B3y1u|npBxgiqso8fp1{b zo*cIt1HDH-QbnF^Wi{}ZAm78{HJ`mPvDSEnk;o3kkPjHCeNfTc!7uAu$AQTra<(Vh zxi&2mp#m&PojbeI7>^$13Z=@Yc#IN549fY$La3v{v{bcB4u+^$$Z49J|LoT~bCE{C zP)K-zn$bEb5<__Xpw{xFxlwW@N9I?#;do*MFrx_V>ovNc82B)w_GxCoB9z@V>+#KsCYEC zlOMm;Vj7(L-W68hQ@DwUfSmpBBG3#+z=vUz!kTY7sde3K%NpGdW{@k+N&RL&nyI0o zUD&(_gWxA|*8%qd=E1FCI12IpOxIP>md(5sdxT<6Y^=ND*Pd&8He>R2*2TvRMhO8O zGk@hw5ZJ{qvnsjB?8(0$U!BdG1n5`ks5&xYOs5ek2)hw#Ov3A{0%9Qr)v}&7E6d3> z1~!xJ%3E)-;G2J#7|n4Zo}Jb^ShT)oOLz0DS!2^O0};P!erEd9)-xG-MInxMLW-*_<)+KT`ny<$_s zj>*-M_~S+LKqwLlh)l@Z^EtQL6DX;wSSDhBGlkJ+i@uOu*m_9IDr>Twz4`6vPi#8V z=d~?Dn&EYgq@!5u(^_3}wI_EJ;E=bZCyQ5LC2H=8B#~xL`o=1t{AAE+JeiVEFiK53 z?;0y>JV>0BHfJ~UuvjPt8^s9yt64a@VZm8`03mA>=TYRVYQyY)zi@gtuKtdj^w~v;8gtG!UrZ%ooMeQz``^)6OLtG#ptv8DXpnIVBOG$gJG9&ng|5=U+(xD z$#{0S<9W=3D?uwwQ|pD%1*^2MyhWB(`MYQ@aUCPQwLjVS9HR~QL`&@*Mn7&c{vn8H zdO3Kho1YM*vdAbs`kq4K2F-eHN(^UaHZ^JemnDuLX(*H?K{$-tz} zyNcD@a%2_nA(H!z(IOdpzkDCXAc^O`Mu)epa5|)Td~A+(Z|X^S&6hqp;=SH3TF7NR zQ?f8cH8XojJyWR`<-iSoX6I%E&d5G%f8URVf`#hjuaF8WUFhv}ta$WVas}6Zql$|q zldn{H(t(p?oxJu}ppFlfiN6rBgz;c%c5dSyE@a(ZoWWY2+~e>6v;eq`#}|2SVO%3f zE{+rPuQ62;v&5In)i_u5--GkM!6vPHI?jvDo{yzdURrJ(UJ>oT^D;z;X*Dq#z2K(# zlzgcqaL+EhE&3gs9Y?1N&tPtbq=MDOdR@YyGh(*&Ix(MumXKx(eYMY@O3oK zQBfqndYErc$D%Em3>Eckw)kR7?pN_!ykqaPkJWqbInDj~L@7s@yv6xgLy&=Bdk7CT zioib*b1qrrg>`hcskybs)=QP5b+R}^5LQlBUt|bHftMQ*+TcXHcq!;tWgUONtzzPy zBL}Pw(Q^wq$Hfl7EJZGdDQg#$(bMwt{>v5=;fqMdM-NMMJ+HuC&B6@S^>j|xr*^@- z$X+Ap??iv%L~>kIb$LoCk<+DfZO>BLr6%|4q7n&F@$6q|wdEsX0>73(z0Zhz%W=_N zy64TR{-eD9t)p|N%=xwxZRF_x8vh4>DFX%0{C@3Mm;qq zyB6;SZ>nf@ZqF+Q>F+qvP`}9yUl)qIzKWT^OGP6snq=&I%LqYNC4D*UsSkWfo#FXC zPcQhYJ3`BB0hk!wp-Tc)033lA?-MA(iW$ zI+5E;i8B%+Eg>{gAi6O-a0{rfT$$n$jW7Bwo$l+~o(c;OkBoWo0!jP&o^AYQEqsRm zH9LJx1G&b2U&;gae3pmBcW85VDF?@Iw* zmd!9$t0IMPro6Sz>@~$y-?&k`2+&|in)mAtr1Wrh_#kZjap01}eZcE<{Y^)Os&yxg zee;n(a3r+w>Pw}PKOqoYa)k6Q*LgMdTJ0-H*c-!gT;wDe@eV?A+-N1MloZiMa320^ z$EZSs-mF@i8vz^~yLcfWA1-&xU7x4-ty9(_Y8!V4_hHV`y@r;aF#x`k?oH4khE0$| zaRUD~4c!BF(DC0tWRd`}#D@aR-C7^r%?gRh^sGwYr9Yp!IP}`=dKIxiHTepK=5sc| zIPatD90FHt~N-Sz1c}OgG}~6wOgDLNduXo zMjiR$&be28RZOPv{~`7!EhEUB6a1KB!4xQG``EVD2rx|rw*9qwmk4>Vw~j)^>OYyy zDPFY2#8!j|MYHjnYBR65{N5evz5q$;31}o64^_nw8Wv#OhE&Y)Tp@4$Tin*~#Y+YOddd z5@w6*Fjb^oBymnda+}tzY8M@<{tSK3IyB3(X_IA4S2aa%=Np(*)F5?J!a*s{L z)sUVomm!ihCTa#{ur7DXVA8RzqukgEo=#^UcmuSa{FYejNJ64AjfqhGzl1)CUsK}T z%O&1yFAXx+P=-M9JVRd4Rk;})mO($a%*of?ccGxB!5r7SPJCnx_Zd6d=^1g4M2lt^ ze(#Pju21BlKvtRyi?*n#A8w*e)K309|6R8f?7z5EbjNOiyl`)vjw`kA&<>iGuD#xN zq*m39-^=x_9)8Da--Jlt3~AX{!QZFp!;nJxeK?_N?rIY#lVhtr^Mrz3N?KIj&V@$h?Y(_W)*d_{6RX3B4s@k$IWi zZy0~|_?#)q)aH^VMfcFG5T$w7e~%zCzZ?YXLZUQ~^HP!EI1~ACB9um|F?o8xt797;7iZ55k+ytxTm|W7ncjEog;N>s~Ut1_&!AbUHGm4di#9$m*sJvL1k-} zTNxZI?O2F+x*xnD=;fWq7Zjmb%k8@L;9%wJar^n&yLBFRfh*(NErHroQe5u-kTWA` zOX>&(e8erY{lxrZ?>_i^wLuNfqMI*41Ja>Q0n5b$kRgNrlGqd|fq!z+$#mFwwK#XQr;ZAQ#FZ&yJ z1{dKfR&J@b_4jh?nC3mZWtLqUm+2tC;r)S^%um#PgHsjRoF)|0OqP+!13%zlR>#5) z7rKzlrZ_1U}++)<%}F^`|q z#T~HGz9CLf*PHEBt9=06=Lw~P+vxvUyiIs$M|C}WHmgeKDHhm zSp2!&BBUe6NqNG2dj%h(4JlBL`47C-vH5#X;m|%nA8NO+o{*FyU8V81!Lpt?1q<^B zx{TC#6FJuhchohi*-*IIZ1U#8SI8!0g@2LkIh!oL@9qxq2{g>$!ARXzJ_7%GS^+f>_t_N z{zOiB?fB5Jo$dK8wfW_kgA&(}{5Q)#etq#X@w8^aD$Ml}a-oYd|CLv6aMUZx3(fZY zgPhH%Z=)DnfVb<=KEYpy$;=@zEbg_RwUT*#D2uvjxo0fC@5l~k3!ns?J&+Q?ltO)v8b!P8(Mp818te7TsbzU1n3&>HBT zKJlepc=H6Yc%3PkL0(sk5q45fw5o<9P$fr@(l*-oL@9H;uku_nqz^L@;c}X;Hra9; zz^r(s#VA;pV)HfutJ(T~`NH-0&+LYlD$Y}5Y}m*@)EL!Y=JnTaMjjx@&|tv;!B=pb z3PTUbTCh_j6~|DxN)IWfj_6L*of}7@Y0q zk|cCrvoq}kTX{XD6H|qkl*(&1?7(+C<&8ul63+pNvB%wm)*-8|<2}=KzH0H~oJFS@VDSJu+cvKjibx>;geVPl03qC689vEl>ggUE0n-%ZHR;L$K+$)ke^cy{Q;G}IA zq+BL<4~{S@_)hmJlK_V=Nq{(RkXeQ{D1L^N{{`8GR>myM zbYw`$*T|@EaIWW;%yqI{1#r=~QrvF7i1vL)3=kkK`#enn{!=2j9Zn69%R4I1(v+No zUDaolA(<*7QFRZRv4*8AQLGp%HoT?fl{hMU?zMYxiFkT{2d7Kr5axjxoK|SA$7;1- zbnndRal%Pdx$`Vo)qKNXJ3@bg;DP1#*=0B_wti@gQ|1IQQF0`DQKeUlLcFnQ^K+BD zy_dzM>?$A&iw}6J$Gn$gHY{Sg@5S7??R9kIb6Itj8=DC^51E4!p?oq^unGs{qJrN& z(?8xo6tx^6W2}f|cDOzg0`fedN%0L3mfYaPAiI-4lGOmm3x3rY?Qo{$S)Sqjj73g< z2nlkj5-4warKx8BB=7bJOFXr@!!(Bi5!bo~6yf|NLw#CYPctmCJKvS%v6)`^3=x6y zp>Hk?I$Gv`&*7mZ_W4gI^H3)?DU!C4n~q7U68*kcr|{4-s-{*7uy9hckY7<;a9mJT zzU)3Gz9X$>bxZlpmKMW#`;x?sxdd}RCq9%M`mKE&k+0vwt-m#XQ2H8qmD|rBH?G=s z?wmOAcwl7uu>Dd;{ZUSM5aJ>HWQ9XZg&7-3K>gE?-h{Fg@QD`w`9aokyNP5qzHZuToR~8!fwe4TfNhI7uJgZtcI1oRC;VY3F|zSRT{bTq?{;Z zr(|&EBzo1~sP9|xl#|> z9i6og>DRo0UpTyXD8oOO4)pJJ3?C;DJwi$xOMljSmGwGSd49eIqF!am8PV@R0?sMA zlAc`cL0O2}?5BCxtczKNR(kn?lcgqI-rLHk=je4%K9Ro+k@ae>i0W$0ftX4{LCVZ2 zklvE#=nr!Q)YPYBEO9H)ih(^=&vM(zYm7dEH|%c{_HevNvsK}eE`A(mSqP1ud21an z9cVoOT2N=$8tt;#c7Q9P$kfXdmybf%2J|y0Eg1SlhylE%U+we%2Gr}IPk$^CK8~KT z3*k*@ihm9F{*VD^Y23Lqoj}e`n^GTT$ec$PF8R%n{+qQtoE6v7?y)r6SuUfh5s&Ud;StBRnSd~f6!QZT4_6WgBuYh*DZ_n4Y z8ve%)eBm69#){Q>T?IDRxL;f383oYG1fI-dQFgG4H;F7fR zC8hH;OxukVBw&ZcT-Ai`Wlf5Coxc_z39nO{*b+-J`1qB2v{UiN;({b5kuNcNYrB*Q zU1g4U`7FIN956o>KRxGH7_IpT!a0>)s`6hwr$3axMl=Zyug6XE+jyRnk6Q^XC+{z* z1;w6}gi^}hQm@kNozci|cla$#j^ugEc?mZ??J5$dcC}Iz%dI`K4;Og&SH%#%%i|<4 zqDi|IR`;26b8gvyez!)OoXgX6mzvGAJ$0cL)+I+OfB1af)Uv1Mkw88E|Fr-|&hKe9 zxGC!Tx) zy{KbOnK3z!JYgcD-bd1*Z=G9p=|0u%zgO)tdu|>!V*|Hg#&CU^^ucXIYSIy4uiA9J zww0(Sjc4Oy=puHDkDd!daPaPf8? zJfaQ5C%LdUPnaZGk52RvS>GO!{Jrh8S*xt6+Am$n;Db)T%lqNOdqdKmgd-i64la?t zBaJa_sLyW*{DzO;@>%=8*YH<;%;P5I*O$?=I$A+fS@f)ime#0#qDu3xn-ssh$n^J5 zBU0gnAaZvPv)FMK-II!>hr~PH?z5GcC;O)F!r9$jb7MDjiR8d99Ow~wdiz&DRaLI; z31lA1klvrB{^dG)T1V40#=n1@{Qdd<350~e4pjg15BBQMqj7Cmps=sqWHMlpgoM&N zO4tiF-LH0gLB|1N9<9@-bc8Q>=#Gb(oSLFsCq>|~5ZD3krmGAJCoKG?PxpM6+4}5F{hg0w@;4_&#QWM{Sig7JDxLrEM?QoxW}uh! zSiKgJ*1mgjqV*YsndBnsTvxXAwok)0+(D!Xa$=##n&Q}#t zPZImgt1}etRYtDOrRK;sj=FJwnkt)2GS;dR@|=8qme*7tmTY&HjT4%P`$0!HT@fVKF=Scm z?Gs<;(ajxoY#{~alu7>0%J)JbXg}K~dpwli$=UPNF7MwU-0n)kL-O} z*8am9)_zO%S`Ue}v+O9P!9X>`P9-}+6Fsv#*h=mzsy^ee(Prj0;{%9Tjh|cn}%LMg+!gmy~&)F33FXC_d2n0^m zVdKLal)kluyXxXMe7a9{DV@Bzzvod8_Ztf@XCLnow&G&Fu9MeIUOSntUU!-a6a7^>_JWeUODQi(&H?nS-bcv$GVg=g^5p9WNqk~`YBq(vqDXr7 zp)8%pI`O`xca_N9nTwMlt!EX_h~_PUAE3wtdJMcAG|UFosVe3^)gmb92KWu1#%CHxSCYLqi@W4fbc@VQ?R`_y zmm8yu`?bL+<9&1Ci6@^xOKTXDhVp@l0^gB)dn9L%vgdk2{w~76cf#r2sW`FZy$?eO zdhUVTMdj-|O52nBCI0(uAN#zG`;r^4oBBsnm@_7NTF0%sw7$`z`RTBHcCd;}7f4OU z?|WeE0LAo;;q5WP?g=>bk4;x&T)LQYC3^oGEekIW{K5gLr8V@lj@R_4KUQb*U6Tky zQ2khy`p4=VJ#eh=LLVLBE7#WHNVS!_gep?p?#2G%>~uL`EowX*)pA z>F9Z#_VaDl{=*uwA<4cbOYY8`lDOxU)Lmo*T1KPuT!*#aQ{4d${%;4BXu(MQIUj_pH?#^@Ub1P?0!Jva~1la0=&1)rCT6g6V&vYUTyuB+L@jzFedG z-CDvlE+Xg6zwLE@Ns`XmaXSRED*9TrMMvm||E*hdiVrhy zw|&}QY4^8biz?TSn54#2s%$JNVHeww@WE{Z%h7*()u#J+FQ8odt`gbfS+tCX*YIdP z)uQ#ct-jAP^Aj`iyrAi!B^UZaw!Q2%6oXf$p|xqCu_x@Fzqwy558C)xav<91%^kUyEH zMJrz4eWpwHzASoCXR)sLI@h4Xt4R|O=f;@reEHsqcZ0ZlZC+=^Jsl? zpC~IF)t(DY#6~s(C1s0h+!0_+S$(B6=ih&RtCM>8_xIJmMZrA*&CfN-JeUCkytc>s zXV#hizG;wPU$BRNpNY4~zBWtlo*YKe08gm~qTol+RRlR`2Lw)l-|}&*E?(8cyU?ru z6Q)2*YshxbSWlq-_Vm6Bh0e4903ZNKL_t)vBKl5%HEv<%OuA>g6z(a2fXy#$QhuZi z0WH#`ismROM$u7zC#0^P;6R3Xc@}CGX*!JC@PYlhj9K zJz0ysMcNMi`$YF70j+0ScxSwL{p62KVosZJprQ3>NX`dd*LktS^!HDzzd;i;X!PC( zdg?k?Qf*Y(SKN0^eurBLyD>0~kQ}pamGSCg{t+7hhDUxQ&)mdF z>b@I{9O3sq8hV5JJ#~z-fjMI$4CO#C1BqVH(F^M4P5_Ok8dUyc1=*C8-de(%wUo>~ zuX>}4oIua$_#K}MKcxoa+4s#-{FY*zMHgB~vVHabQGfEjHfrH=L_`O(WVZ=L*zkz2lGWz^%H}zEGp_7YWnL@X*;e zWGUQNz$h9>U&;2R(MZHl+`T<{i>2bVL4PndB+|y50f135wq&}6CVH;# ziCXnxb?N326Klfi&xFI4zUG7VOgf#27ayK2+?Ny61b>eEZopYrnGw0%qSoOX*dmSmqwN$3{2IEo3-cKWHEq%i5E# zk&?=Xm9($%mlro|OO6NIk#it!`_}iKzNGklMOuH=?}d?jV~)c82~zH% z*L;G8BJ2mJ501vcJp?!J(wJ|s_WT;oDJS+zfZlm85EQ?&NcT*a?vvfv2SNst!v}9o zl0FRE5qBwNpBIj%Y4P$-#2Xlq?-8ykY}@A9XP;FB-A=R^Nf6rDm0f@_Z(xp@y^MIG zk9hRhN78kH-}SN2*`(*vaqHAe?s-12-d?roe!VAYFMel{-0_^cZ)Fv4SpW38GISMC zcx!?5(KKtnxz_ue61Vc1_sx*nm%^Mhk%7cnb~yi&iE^|6@^8#DcXkeoTUYA7TLL~f zevQViy@gAW(u?vt%Chjfg6boj($MnZ#L!feL`-Nt(PZV9SGIr0ok;o)lke>wcQS4o z_Pq4Y68KR2xH=&tlAgUki;x0m$>Fy2Z6m4sWn$zAzjx3`HE7)1P%T!3zqClu3Mjs%h&64YWi(ZR(D=}GJhYEqOW`hgwvvh!_;-|;z|JHl9Qob=r(ijyVsR-QI(npy)4!6}{@O3<~m zGW^pEDpRrcyKDWbg<-#egnRG3_iCJmBU~X`b1gO}Hu1V1^}nk3f=|iwKU_8YFJ`fq zY}Ws1y%&TEf7inYZcUQD+l;rAGH#s-kWOc??JnJJcNBOY;ksjCVS&nerN8WRNG3hG z-aKN055~BmT6iWi>PCXJC&+cA42#Oowvg~7S%+wz?!xYDV5Cg zY97renzTOOLLiv?z?^FJYJ$7ra{hhix3p+ODw1A!WkHQ=o}}@3gOy)jiSOmP*X77A zW|;|QuEo!=Com%5BSeVK1(CZiCrjT^QgtJd%snLN!W3#*7d-z1J7m$};v_ve01e56 zUkORKUSW|rlA&|DgY&FIW!e3vkrdIJ^>0^u==p|Ky z9<&04p)#skK~n@hB4|MKnI>z$yM}qtB!8bWKGX9$dQn$KbC!tPuT?%?iCd8!{+%94 zk0fzXUG*RVOv>A1DSLPg_B{_t{o04tXnd?e;T;7sM>4Gb#sFbYT<7_3&yzl!#+tS; z#*NDB@j0r=Ln=s2A)q+b$#speAij+HR4$g{$Y)Pyc z3u#MLuV>4id_By#XI_<|@IV3SN{oV`%-p(aAHV5uA?86hKz39uyWjQIayMY?U1Q3X zKPA|oo@XpIhA;)Q#y(^qS+txq^h5+()v^@R%s9b1{^~ zE-m97Da4>ZdlB8wqT`Jwzaw00EG#TgtyR^%3Hl}Bu$9mzmaW??G)uKljkhP{@94b@ zJD~1&zT6_0?~}a`O6i5b@Az~tbg||wGKVubs}8AsDU1n2^*1*JUfpB$w^l(w{w;YX zzk3qRPzgVG-KF+ujrvD>epXQ?T}sL9`)4WKUx*cL&BvN#PiAR;wW;O?0cbgmyQc5D z+F$?6h7kcD;Zk83Q%0}OEsS0XD>ceV;hBYKi-!!3W(1k$iGOkCDiFu&p??C*y%a4?4 ze5^s^V-2SMV2aM$m6L1XZ3U1}`j!$>O3ZO{D8Y}96`IlsL(o01$WG?L4DG*bW6XzU zZ=NFI1SkTX$dXG(EqgVO)-x?+TaubfVHQlZQYbx?s5*a75&7Kfaui=xRQ=DtP-FR5 z)VNrFcbW9TbdQWXJ?Hhn4r1~i5%r)GfUPD??z*coB0{hfEVAq`q3If@{AUTAfD_ya zz=6~uj%V(oqqSLES;G<*FBM;)@L+-FUpM>5Mh%QvRg*5es&8(y-5ia9*9pz3CXL$~ z=tZ68*PGa1wYQdSgimwdnxpto5p&#(&;5ySpCG+IjkDlT|Es#%-4awcs?5#q?cnp} z67OqO=RNEmHCQ_snbQq3{TWS~N4Q#u4CCUtZ8D)164<^>dISm13yzY%&lzZ0P1Ttv z?z_=`QIK6rY4{z#PqH3o$Vn_G=KuDpjo0zW-JQc6GZ87FOia@nSnAmA7usz8=_Ux6 z`>8psX$y2^WLo=DjmF0tTUA1d0EA{J&nr-Kh0co|R(@@T`JbET@NXXG%#WYJy34{d zJ&Yye8vF`ZjwAseHogrK@LSD=`}oxZ!9XAyQQHO6Eb&Giw&brzx(t> z4;W{!w{GZv<4}U*w|um;M$k~BNfa#5_o?ilp3#xEa;Nk*Jyj3ugjqaS#)eYqlWB|z zgXU*@lhkBAx{)=o7j15{MpE}{g=F3OtxVR35pLj;vOzFBBUG8L;2d_aW-PRvrpRU| zK%`X$-N+m2rWy3EC7}tt4UdIiS|Dfyl;2TS+4h`?^kqq9d2jejI#xX_EBhu_hWums`5VQldl!k8TagX!B4thP$y8*e^ z=a9an^erWfvH=}+tmczV*8iXNI5=5)hiau3v=l*)`=vX9a)9rIkL@s<9!l1WZ!IdP zeM_m9G&3B7HK9JU7X}mD0DPEUnIY%qu=b@mbjKm2D_OX3fuJ3*$tF+#^;0U#p3?C* zeY_11cf|!Cj^APM?|ztP1!VB(&0x#z^o@6vBOVJkSP#{O%W@xGoIX8M!OW=F(*gsX$m&1qw3VNw`F_5`zVMm2D0gq3MVOZhb^XYK{vp+eYBK@Ii-x|L`onsA`aMh zUhJ^>XPau=dfS9*b-L+N|4Lo?%nbS5bN}lc2w42UA^_$0l*t~;lD{uc-~_Dx@ha&< zX{=cb`>f6CZ?95*SDD*ucd(zH+3Cx9&0Q)1A7#KtxKfDm?;V%j=etb)@MJte*K*-w z7}bghdh_unOFzG~?ZJkTb76^Caw7wk1@~=V^=zyvYg%Jokp!cloQxxbr0}3U^Q9n^ zqlOb*Z{1VtTIx;nZ_4AWI5^KZTS$8Lm06@C$*tv>8{6C2hMNI5lOz1KB;1#Z%3KBa zu&YW^Qd%sT7FmIwSJx0GeR6Z5D{@`=M1{tu8l;Y<$Q;QiiF;njr1g@HNGUgREvr%a zSS5Bn-}gW7qxh;~-wi&xCV_srIc%v*A4=MzeUqkc+tg4yYfil0Ub5-F)Qt&o>r*`g zs7TUXerFkP-9s1xXW4-QjJ)I05de%gS%5dbz@z3#6t~afUu>&*pdhxb8g4Xh*M?o}~EpBDrHZ6%IR0ip&dXs|O}cd1T-wB|dVJ{%kH#696GumV(O(WY zrbB(YPIs(}Ic*}5tom-?f1{B<9H-RIc8 zV^{Sz9hULltZnj@Z+?kGcOB;Gm8aA>?MR%{4&V75Zv!BeO5HHT`$}=M_HZBJ`oOkr zo;~$!T+c0Ci6m5iMHnQzLxz&{U1{{9j*(NDW*PKaegd(99E6J5ZTg%GE4&w_RI*M= ziDqcHbr*EVoyf8IxlKA>>M;AhSyhW}D0N=z*%rbOvDZvA=9S)EV(iUhr1qsiK;y9n zi$A;A`|uKG|J5wgleAB@seiQ2)IXWRoHSYdz#^GfX86|0Z$p<#(m#0J0hfL4jR^P% zdkx~iI}gZ)Y>>S-tFm$lk{e6&LS<8_d5`1ca^=(^ZKq+>~Y;(e1O zdoZIOlkq^g!a5fm0CFdD5QIi@znQ2@AEwF~*cLgt8@Cx4MFY*!aH}rvIhW}loyMzq z_$?njrT4PrcCV(~t9dx94&`^1DZintp4g9II+-XlTKWgsz%DyS*40vtuqYme5jCM;S{w+Olc zhy5cIrD{zxJu?FmmRFXs3=`8dM-z){h-SIT#>@uhjEP>*v6pQ&|7^22p&pQV=HHU1 z_;4{U>408vIU?p!rpv9ktbbyi^n9B77uEGy+$ipdsU=m+n5<1t5J-n$%3HHz!E=u*8Y8Xq-u8&M{GQfwX#mJ>B!B!T`VN z<25{-C5QCkG(s2T@6Gq_TOm;wnxOe)lkW3f%5PR(O`^~1tU8?evHzdF_l~kOKkGX` zzc<|+D|U6BneG{lW_m_(2B8R&Fj+`|1O{RDm_6V_uuU)++jw_vF9z?jcriX^fjxl1 zLIfd1SdfqeA_#@iXr#%ht2$M#y7|q&{p0th8>+f`rn{%Rb)PXV>d{p~FQopKx&r5U4N(y305t^OmWR*;%RjoDZ1&>;8}yY- z-COsRY<)ezIIj9OnlmOw%}_u*+pjTG7lV;X-s8+(VsZpY+ae@t!)r&I+zg}cUQ)F< zvY)b6pstrl-Cv%^C>yw!Tu$mI$yrkt>3YW`M|gN~Quv!0a9?RWSAUM;Jw*l3b)lrx zcBp`M1kmHU>@k3D2)d7US^vd#^hKS@eU+r$S`7HKqL#E@3wi%v z>;;iw!^hZf7&DU_I-YTY-}v1P+8=6jgGQQ+$wdI&4q5)OWt!jI zWcz*FXaB$hUq&%sY;0_# zYjbQOm5IWu3t01(T8{@3XWhYh!T}wQA3aX3cxwXn;NC#}jpB0H4ErbpiT*i#>@ic| zK8=G$Y#bM}>G5q~2Lu~IdSYfsmLDuDwW|r5so`$Astb!mIjKV;WtV|OgkqRkk4d&% zwEjfT8DoGLoF^(*GcCvJKhsZqU<#)T2u)CVRb@zZmu2UN)VQP5N(P|MeeWEDX9ifa z7Wrd&oaY>7K0kwHkGEUfj{(o;^9%+9^7%adem?=|g+gJdqeC3iaX5_|A;kUn-!ISI zc@Fz=oBBKJ3Y16f65IH4NRkIqyRh%yd#D^ zyXnYoJ_OOva(I&+J0{B@Vz0b(gB!YPGwkp9N$aDa7a)7eMOpeXslNdzy}E>SCB?NO zOZBPL{iTlBWG?FnpWQIR{u@ph-gLsvSL1jk@%+s53*w(>OgdZX-|( zj05x$srp`h(FxUj68Aox|ABc1&kmHteiWRK{GJ%b#rI{doqKr>vtTk<8Bl&jStaXS ziQo0Hmu;$Vts*ta%v)#3E#=adX<<6Y5Rk~wV<~yIjy*PjlY^)ic}6ZDIw7(*%$$dA zsOG86ZT^8gjW;&Zx^q(y^a6w_xb)MPXnseN!fl1*H4UC0;B9(}ZPtBIZBGaoF13VpreA zJY*t;M6c@f|E@oIBOOKc?NzkAh8(Ch+F5rLm^dGBZ0Q))eDy%je zyiXE#RYFvlY9C`AwlErsjEKnq`zf2S7b*~x7Zmj(rlVS8>F0FxM~A`t6u3`gQnXqv z&YnA)l$q)k)e1Rq_Xg?uqK?r}3NoXt%H4#S>5m>qtWJ&BWl#N#NmBYH3XvB|T;^($ z0Cg>=C72*7NwrjBp!>xxLKm$4+#1zytWvzQNbYbBYth16_t^dTF0J1mNmx#pTcng! z{y~M#pLAIK;Y9?3-XlHU?7!u*67(yb`!2`6Pr>^%UWyP)uUeA5T92R~q`l7#&~#0) zL%QneivaIA5AzOFZSIdMDW_2}hBxd+nkh>_xSnq2hmMV8)rRDMnN4O~TbeLg(WV$z zxiW_XW!9t>G_XN0>IB^YcSR)_;!QR}O)$k*6v2nN&8gJ=dPJ7thP?rItyTwA9b>z# zCfM!m?OPgfUui5KUB+6nFb|o^&!H6{rA%wd%X+%-W+bv%fWH3U)>Y!FB&fWmg3&P0 zDq3PPr`Od+K$OI=)E%gQYn_GnE+_zDkg(vu5FumnL@&BNZV zhh}JO{^}-2|J~7~4&B@I*!ZOl5OCuEKB3(EZHaryW#=z;7<_VYMF4$L#3d<{%&?}L zlksr#E}i*KQfsbPl>y_<$9Cv{qCfrs0k8O;S7P0xSfIMBgM=~_1}g&!Ckm8A2?X>+ z54jN%SRtL~JBS%UtGtW%m`CrgdOY>?lSp5xzv%%a?)!z~Iy|%f491*+xnN>0C?$z8 zZ_sd_m1y>)e$R<%V}>n7t! zuj+*TkePSRu=RUeLrJiiZ=b=aW%fX!Dg|8s<@E&kEdBUWI=2K8Z^NVgi8jU6BBgo> zxh<*c;|Yu@aG!?bC?m$@3(IPM8%fV)C>2PL_RadDPOut`0p0u=B|kT621ZSleF3Q{ z93;KbWvITNdO<0(VlbY)#RARH66yUAX0WuBl+#s`(cN%yS6v2A4`}~EdmM;oqV_GS zKHa_GD)YF^`qg+(18dg8dEVg-{Wo6Mz2N1*J51}+r*UnQuiy2g97e2_F}l> zHWM&Dp=ImV(ahP+gfiI1Je0F^mh|wui_^?bC9qe1z!FrI{L` z9y0rmS(^>{HQy-iFbvzz(MIdRQ+4A{+v$=vtNB@XiC zx0dOCq&xh$2~m85a(^{y1|n3B^_s36?tRAx30hsF{;IkH=V9kS-=)1`wA>`SL7CKg>Si^6yc)4l z+rT;Opx1QFipi1YQM9~9_fuUCcaFmSP^qNzVyRT3{Q5FZ%}Kt~UD3gQ#^Iv4Nb$}h zXt1uWDS-aP9@10o23t?JC>P4;LTCHwZTinf164q|RDRL#Tf^$k7gAZ{c@CS{|}>0a>T~<{<=qx9_2TcQ=smIXk`t#Bw1Zs<^1Y-juemF zkU)LaUS-GGflXB3<%&k}z9RmvPvM>dQc9c^Cn>iBz#;`NQQ41q$i!Jzf8V)(GN%ZM zSjiL_eDy!srzd;3U+ZGsW8t24nY}Q)u8y`ejbqYygeh>p6(g6+k;~;c&T#-%F0Ck| zQAcIF<689KQh;&TK(Fbl#3b5Q$F=Bj6kl6J7=rwfd;+B7h_4m*P>(&Kgq{KEBY<5q zRnkD{0x4u#1`~qRB-WgTwO~=czs&r%&eMIk3j$VuW;K3N01o}cAp=lzu0DGaptJ+n=~e2IxeSiWg+IiYfc7sfLSxuV2x#~ znqtut`T2+xU05Ok12JnpWHJ#0^4M`Q%aDta|1!O9(#tujKPO;F9vhn^Nk^G45h~ef z`_paZ-kzzw&E1j1J?A0{(GxpL0uQLRY(;~@`2vTkhpy>|e51$J0^HXst9dIq!oQes z1DCaztzj;j7&C_QW9TTxHD=JYqNadn1jxN@4|B#$81(*QeKvkY0bm(Ow30^QWd+PR zGikk&Od&`B03ZNKL_t)h*CRGQa&*`78h+1DBT34?x+h(SV@O~{t0+-jSUQJ;%}X=5HT(Fy{Q)2P%d2usjw z^{~&{co#i79W^iR>}&&Iyvktk*8WhE%G>mCS6!?nOZ5(p)>v=TqxCl}I)9|fJW6jW zQGa7yC5b}GrJuQ^63??5VJqZV^Vo~(Z@)TNrCaMNHBD7hN}6as@H;-;N4tshGJrX& zyk!Q@sdjO#4`w8*(fg~sr9$p#E-m>OC|2EBa}c3mzCBN^a$^AX3l}f2wX>D(T_RJy z=6jlmlAv@~31JAfKfF!%FBHfSLU8)`PAl`;sPr#Rx~>22I_*DfGyenglT!ZgFKfmtXeqMpF=fCH9PX6*qwQ1jrMbEJ0qab~( zHa(0MZX~ncpuWEwzq~>Dt>esK>CGjYZ*LAg*4y>ai^@nX9E9pVH%jK&N`0eQTKYV3 z*-TtUBjiw`Kn4-h9c0+_fMFKgmCB^gNo&#*)_jz057Zo%WzGp8cQ`jbpTdwK*w}m; zfw;=oj62yLs5Iaz#3-w9v$E`0>}swBY={l~ipQE*MK3CM<8^I~U^n2>&rWVbI`>cJ zREGjc#zyapilHvwUryIuVB>=ubUxG>KKEli%(_YOt|D4qEt>gP<B`}IU_NBiZ|ecUKNNBio8HMcS>x*oCT@t!|Y(Fmz79Li9wXF;^K%8Xhz zA_2`=6LZGo_TRW2By=9?AU(<2{}r_?{pyNZ|4kS?F`#^^jQL1r|8f(=<=FS^ygfQL z;f1%1o5rmXhTe#3R^v8jF#yn*2(}YOTxO0ld`@y76Rs1VKB*UM9Pvh9%ls@dEKmR% z8d69?5u%}5!R7AFsWvqsI4cgujDc~~z(1#Cu1R}|h&*%F9SVmEtd>@9XsP?P0NikM zO2)HbJ)Yp=iS24Q!r#yl0d%2>r1juv9d_my+%0!9c#eJ-h8lzZ1s`wGOX|*KK>^Kp zQI@(SG(t-EmZ*MxmG+0!#&qYyJ1o9;F};4~R{An!aT#HBNx^tr0walld^AtgzN|*~ z&%3F_Jrli;_vn4RHxYQ2-dsYHS|URYZDqy=4EKNaWcs=?pGgAFGt4=G0^@-jsA~lz z()OSqDE2Hikc+{2Y$6va2C}Mwcpj^NYn{>^rF4^%`8$s#GUEs{-+}1&IIKkr8+*co zu6<0}h-6vzY(fpL{f6U8fj3+}G1;wl*`C?PoHbP)W=X@y8)$hAd)Zb>kCCW+O@-R) zA|p7|7>!x;+>spSyqT~~-7j=m|M~T?L|5T;1uFMfFy~D)Q?(APyrn|W4H!H#;ArFM z3;vv1ofemZOXy8~m?<+9IJXv6o&4fITTIt}7*D(d#XE`=f3%oL?_0mGzuZ$nFOxf( zQvf~YD0Go9g91U}Sb@bKUsN@^p++cF(hvhgs6bBhJDLjg1cJ^-l~Q8){4!@=cIK+S zwJC7F^9Wo|Uji zkpS}Kz=WTN_M_3JIX1P5vT(`ZkDnuAos#Db)nXU-baWneaB!IYwppwhOM#3m;~1*} z3IgE?{AC}nrs_DiSGGB1T_36Y6mVai7@go^NB9RHeBCVz)|1i?N$SxsisYgN&s+DB z-$$AC_;_49X+LhG7j!g3Bj^U`QYX~I1kegY0rk0e&VhjT2ixra_AbRci&S1+88?0t zndcqte;(gv&EvA>a#20{(oZgN@!wxOI4dln`;jh{w^UU7vPf1NIDzUc5oPnoUTgMq zi=?R8I&|re$kSt{JTi>KBWbH99XTXNk6HM5?n>UPFNeYTB&E?vBwa2v7%UGcoGzrF z#XkAY(I}T+e7*8PV$udAR=~Vami<})@5_a2y0mB8IP(r>!BjHvnktjfN*aSF2jk#e zz})xGQNFjFd{?~Hc_ZuaKfO-wiuACiOQ<8q@hx1s6EOJM36z@{L3f;fz;ymn7 z``VgvSl{xJQWc40^S^I0|3A;GBwg&_Ze)_f0cpn=E!~sHo-QI(>y<>cbb|=AdzrO9 z-p<6OtFBsa3n!H4!;zmlg1hdLo6S*v?E0)*H$q%#-8(R?d%p$4&~i$q5<3|uj5mxi zIirSdn$*(GcpM`De2f_yz0GVcg^V_;Q3nYrW#Wt~g-lqtI8#j!Etr0q!A3IFR;qAY z0sow@HsvVe7rm?#_ChpW!+yj@e^94e>(bh7y`W9O6mZ`dk=@{DrSP{3ShuB}$}D4f zcnmN{*zr*yE~OkcbsLGW7vf%YmDJse7Ghf{kZuU1B?+|~_KdmPkTIMcq)fRdhx?qzEg@qzK4iEPg)gapYke#kIs|ltz+3gk=HjI0 zZxoR4@tIr;dHairy$tBenYi#rfCcYs>biKODhYuu|6i^Ig_{Zf$J57GGba z`am_QOBcFoz2}^Bl!C0G(GZOnC2enYTI>e9%C*@NkRvgG8mtV+zgpF$nzJT;%cuK= zE`v|2zenwxYD(S?DND8&tA0n3#+w@SpY5~zySudiyiHrSseN^g{P8@Rrz!cn8={wW zcI+Ko>9XdmvCvtdR=&nSU4T(D$Q@FOhQJA^ygFLDGm1N52+*N?p8^Vvw={;wmjG|g z9#B3VnxPSfO5*Nqc_=>zFE|ulRUm&jk7jA)7INgjBad^zQC*s(D(5KtNP%z=;%>U^ zzJHhA<2`ambBR$LMC*5!yvw#mj@u8fsBCz(tn$`L-U;R^@KVYd4>T}rPlamh?w-7m+~ zz~07bQ^5Vw$6j^oH?zrH;tdgH$44dZzz|Dquh^>gGt(Ji5?~X$V$i%x9@e5okPnhx zr=b?6j;)%iO*d)=i~nqq)t_4(P893~#P>3b%d*%pyDT#ccnHP2ij>|`qW7_0^8O}9 zCMhALAZQ1wy<#^czmQL#!;P}|lA#QKJclK<<|Fd<1f<8V?VT`X%Li%Ad13(Pr|)m5 zwmnq-8rGaO%$|?VjnT3E@%-3#Kxl&QpLMCfv7Wqsd&NdGG;XxG62^ObTbZfla%rhi zv)RP9?dzUo>lhujX17#LX4yn5YDpVIEwAM5n}4Lt46}*m`5&C8d?saj+;x}MUn{Bi zZNGY3^7nQ(U0Qz?vF0P-y!l;C%J-I&gn4Kyqr<_I0|w6w2%V5)){8TR16bKuQL=d} zYMYd)H|_j+w26BIcw3%QZA9Q4p!IjBEQ|%3p{h!CkEvb6lEKm^$>~m>}iFL?ISaPH)LAm4C?;v!;#H<#eaJ+zF z8F=d+;ckdCaL5@qR=M3lfP1-IVSavL47g80z>AO3d32KDSRCDMhvP?&1E@ag5kT&L zrcdaF6pj@T2sAWuhjSE86qMnzr$Dt{)G6Lxr1Wn}1ie5d?R1f>@iBugNzGvsHa%X? zGkgB*dYcdmUaBn1b~%Z!<*Yh%KG(tijEz30Q~i9Eig9hm|E(5N>HMt|2F5UWPBLAL zfa*!NS#n&8{TJxH7R2FU_Y;YI1mH0u7C$~7T_cgB=~18d==uTyCIohX zp3@PUP_4pSY2vZ< za2}sG^Y$5XM{;cc`u6x|GQxo87IOsc03ikLwoB=CCAIk5Qq|VGO13Y8biT-bCQ1Ey z46=RI;w6!?JD~#Tw%Sx=YtG}pNy3`P?=u4Z_^O9&w%yXCc`nSzQZYr*IExY46bPjbGGzM5T3!`qR4hc2Y8(1r9h@4>aLlHQSL%@;$ctj;$ z0Hk8a*Z=c63;%RM8No%#we{a!R|=GDa!5kyD@xS9EMnd@HP(WDK=<=qc7A&&NdVn- z`mUiQ#tVs7tED}&G~At>IVTE7*4L>a>E{?#snzaKF8`vJMb zIf|!><4-V}oX;F9o>u1SJ?=|tffj05w_AA6dPpSUMo1__%$J*l zger5cN&e=;pP;y@Cd2uy8>jutG_Gz;GMPJd>U3fK`OKdt81M)bc1=HJB z$UQ43+lxU@LI;1-$FKWJ>K=D)7-V{(mV|P6Hw5$VnrHn3>%(As1Y{GWjQ%KKA7!zV z`t(o^N#SH5Dc)YB_`ei!*Ihafb?E=?aJwpi!mA2|c8Go6ChVvhap$~4;bZ|(5DEAu z0IAEQBSZqw1t~a>*>Epq(nBXCXse#*iL5=-=R5}ILrI1#qpnAZkSGZ`w7Jj5Xc9C7 z&a(z_=A4Oi<@kL1BI2c}@xEMSHx=>teX+IdlcJ+_*gCO=wP2B}=MV*fR@N{oh8hof zfw%3^{&1V_|CfUE>NixWy}p)!b9ck#+>f23`t?kl7L-$e_u($9|0(MKJ2Dn$ zzh{=xy(Izf zU+OD>9s%wCvwa4CpEBuV;CvroK56|go-R`S*F^?T4WN`xZ!bEL0NFc(NG$?H~d3wXdsTHchpGmLW)0>*YS{s*K2*ObHLrx4^UxJp%&dDmUV3~~ z5s{6|U8hgonf!j@_z6m-(n}7!Urg*(%MK~aeo%#stGTpi+6~ zypB?Q#Go&3OO}lZEO`vf$K2j0gl1_3y8+I52TNFJx|SM9MNB$I#02XSvu38w=a&Q9 zF?byT@i9nFKzt+}j{&mZ@xfC7$hqj?eBMdlYx1@wy}#=v&v&$Xlv=GusaRt9W6P?} zt`s1=s2{pc!fFpOT0utA9x-Y7;J1Cko?`N(Cx@8xu>TaMxTaUt0kQ3vv;O#Cj&jkyjk0_o^$e`8&-<`Oy&ca+&1sz%vbv=Bnjd8otc&#sP-O96A=H%Ix5YAqCYRb6uT3tc*& zR1CKJl*`=0980rHSM{@MwOXowdm(C{6t!0rX4LL0`Yu|BEe1ld<-5PX%PzY^*Gbs( z{n+ve$1dBhEe7kRO3t}!F8L+(o%JvJ;|%oG2$@NM#-28D@=lP}+Qt!;E4XVe){-^+ zK1Px-W5?rP!_gDKtecE{%wH05qXYL*`R|Pezz-S;P;a%ka7ZN^?Jq$JV3`5Sg`TY;hicT>O9`>1SlF5@+3#EFKX+ z(W73;A4yGm8t-mUI9WhCQuQ7U6zc~{>TaGk@t*SNp6g=FkrD+8+4IASQOJwm)a5uW|G>$3JGB z>?O=L(*>bcsWID|rTy79jkh!s;H_B^Qx=Y}&>2}eE@|C)KSm! zD#Z}6yt15p7Ee9%T{iYcjr~~mlXu?6tTo3m{RBOuN6Xx0Ex(FhQtY{A zX^5iwu6MuCW&OXbCu3yshZiZoLbXqG*Ijo1YM1s0+f=`?%EEgVfav(gcU3QN82Z~+ zzP3W`%W4=6gRrj>JS8U7v)>7L$-Kw$lOS*cdR~vJRlQ2@?BA2H0DjQO0`+CNjB(gd z{m?_zk}WKU=&p`2V<;jEhC#X@0poEAYADs37`GTr*mD`F-$IeBkwOZ3&-CbgxPx)r z;P~<5>O1f6|GKAf^To7f+zSsQX-_s9*#&g3#Wo(KvE(bNM-f6GlOji(U;~$wV#S5R%)P_oWQEhJQc&&lGZm0F_Lba8m5!KujB4*N46Ni7n z$EX>sR#%m)`&8=wB^@`6BYgCIPT1xw;7%~A`r+`uK1}b)-the~P`gYN-&iDY0-Vn| z7!5j(>WwOyWn>^au|bIP0_`?+HZfJs>Yo%N*M;Np)hDoH!A{@d%cKF}H- zd(qM}|NZln?pEa%VPAO~^q=hGulcxJF8xpTuL*0OjkC*VF_sLKc*~{E?W4(ht7+k0 zbjKbq5R^`p_GQVlKkqAhAHMM95nKvmW_;e!>M_L}c4N!muv|41YEN z0o2}AqxE|&+)Ii@KajcGsIqWme-(Gsn8yBbrNCWENw3$VP$)1M4A3<78IJ<+(XyUf z9SqcJHO|z|BtSjvgzWz5E`?VV$Q{lR4np*jj$YBzI(1D;6>4Tks17rdza*`Sq@elTO^SCEk&aB{dWnQq19!{CeZs{&ZDLdn2A>{~ zKbcqe=Sb>rF;z0ae$1wLUy+@`4vXgEm42?%xJBb7b%fW||DYYHaWDyhWyX#*Yti{! zhvwUwliBkT<~$pf`zrK5*2g{XVrf}zJ(+}Wh+RWb60dQKg5O=oLmi^3g+hJ{|$dBS=eJ+CC4!W1mz7iky`>?dp*t4-!N7wCO? z4|ma}{0(KEv7W+RcUk$N6+ra^Z@i^}wP@k4xt#rhv*W;AM6CJiY8W+xaG-#>yYABc zR2Q>h(tom#x9nY0)*Qfb9JH#2Q8$!IAOhz>FQD_0&hUINVptZl<@Il=@0qwisEmCm z_5$Y7t@I_{riYY~!VRweXEaG=lr^yQqjs(n#?Q`2c360?QcjGq=M(1LA>WYXU_@-_ zVysqu8kdi~81Uh9X6X0(%*@Or`_}F4?P+Z+BS1Y^Cg=q0{?V@LoqjY=Xot$ty)2O> ziJn)6ig7tuqCSZMx+HB=8n+ik(0{&9_hVf|UU2fRlT_r?1a2C8$CtWv-pJN^B#w;P z1#=IO4imunqy<*8?>R#tKDzNH#yTsV0S}XL0^$UP_ocQ>ptjhU0oW!k0=V+FG05-}JDK zTNqVi_=L;`)7$oxEEn)z^|XQo-&bA;y>dLOWNb>hLRIUPVCtm+VU(G z1`F)&?ji$;9_VUru_6{do`z!39ZB1l{KJHV$eg5JYk&{sVmVO@lq|(!cUxVMpa&Yg zdJpn~l}DEoX71_brxJNz;k5-Cry7ZiywC)Fzt7I{&X9z*`)6IMuTG_|LK8`gHS2H+ z1ia0WUVHn+0Z6`EEnHk$wHEfgO}>#=;Iyczp5{eOfpbgXcYPlJ*kiOm)J`5N5S;l} zXIMIYgyQ)kXP&(u@0`cteTOK1bqV)AhYMThS%2TUGEf^)j#a)+0ds4{B5(qNod9>u z#as7~wpyRIKd`M>^V8RkHIKu#m5f!hqS6&pwN`Um<4n2PFq6+h80N}P#t%Xk8Ku8N90+^ed64WpA@<@gDW+ z2-LtGOCzUAfY^v-h`f*6IYiP~p+!u(jGQFn`(?Vh-uktzWZ^vYBZt&EhFW}!cNKBI zsLZ8=At;6#il zk<`8TrXu~1_a|O^5+KjNBA>`ftvL$<(5p%c?(g^*b%XxXePkeUpHRTTeR4wmY34oc z2hQWD^eZ@94uf#OVe1H*si7Gf^?U=NK}D}1Lm&r5Q6#iOx;tH@C6Qi8yWIgPS-G%` zbOB%T%(j{mhejGs{+v0-AWP)zx{O zKkiU_V{P)^lkvHB^!G1?`tvwe-BkvM2AK0EM#I3U8EAD?dZ1Ty)jDn0qx1337%;EB zt496Jbp{Iq9{%h@tp1E@k@m80zni6Z9LB0yR6ko~=8;)?y&j!2YQg{1BcB9d{s-qN z-%}>+hWJ}Pp&t_L2KXC3!FIslxdHZL_BChCvvGF$EJ7FPWqsTnZZrf8J~?G*OQ*+R}9SeVc&;-4pHqTJ#e0I)L1`p;(+h=3L=Cs498h?7(N#LA64+|e0Yb& z?;26sWs?lCA>3Z?JM+IKTP(2fBHz%>71s)YUoQ6AEGe0_qPhV}Bts zkvpbZnQ8;o`AC>z+z4*OG}gXg)BDRFvrDtgRA#WSz_|Qtn8q|-Xk3YO{zk5QH!vnF zm~o8A-7dQbPuN%rL|Q8@CofVHS@dXg9|LoBsu;>>6K1Y8vQZ6ogCPlPR?3e6qio;@ zzLMd}>3?$W-b+jmyV1bA%TvQI)?tkGJNja*ol&*vLk*a7Q6E zlAE;o%tW>$g0~T|vYP6R?v*^Wx<+6q_B_mmiPXLK(Vlu867mn`6XlDwXekyh>S?Z* z^~6}s+w~AawXSkj9l~=V-WR;RnWKzztX9zoJ7F@PoH&|-^Mmy9-tF(wbvo!(15Fcj zy&h7BmHtX1&0E=8#^3f6ckRUZ%?*{Q-5@0B1vJej&H3gKz#5t-GYOEPzoQUC^X^^_ z`{s;l8HpI4$E-OE>7*CU9m)W_Df@;iT+WrxVJ%tY>v{A>1slyWxzJ!QFl>gZ`2jV1u^ z{;_W(e>~4>cnR5t9e10HoeOyD9^Sf#yXMk=yiX%&aQgJ=>-L#Aj-%8Uw_6E&K4cCz zVNIX-y^IdUK)v~%CWX_5vHM3Kb0xq$_S!(IwY>9@PEvnwKvj+{%S`8 z)H^MfA6kY0C3$y3=Bb~pV zW8FxcNu!PAW&4Ob7jWlZRDI$C*>rx47isJ%iKpdzL}q$O^4!%V$z$@ zteffeBG9iDHG)>49G&CGj=ru?$?L?Tz*%*^q}ZovT%EXDj_~HLNp6tCDL5E)Lm3^8 zv_s5pE{8UndoVwHMGWeNE+9~VT`OvcBZ5E=k_@;o1@qrCpD^G_E45hi7D)Vo4*~R_ z?&GXExSw-XqRkXZOT^IL-_cAnG-Wa)B|$q#Oe0_Qiq~-W-FIJcAMZ&psKely0mpywIM#xyDPR8n<>cBY{_6>>1&g)cT%+~7ts!N?)90R|dS8{f zcg?Y3uOS`DY5Pt>N3zsC%x(VdbYzE}?lwU`V5hi!-PaP^wh@{bc5gQX-H&(2&MiDx zz;BJ9#K}1%w%@l6K=a$16iy8T^($$=2E+T0{$qU#cNA1>Ln-kusMm9~4dQaST;f!X z$rvz?NrB{8{J0~z^q5fHCvrmhAP=ffSm4G+B(N!^W%tGgwTJ#}Gk z*Fy%9-s7q#x$^}l$*}819Rz4qZK%(w2n3-MCM>*q$ua?_F{>mH$p#=!+I{xnPxIN& zeVP-;P5@k1<{rnPnL`viMOGhQCEv|s&YS2JRhRB}Bj!Bnbsjp3MX$Lv!fwb+bB3I? z&*nLq%0~VMljh3o|Dn5w6+c2#?76q;QT_VrP|sO+)lCwSU&_(;+iWguVl76jc||ei zMolGPw33F%3;bOlt)j8;Z3{_Vf9JCugf3Y4hpDtUx0vJ5zdS_$={}9GY=DefYsvKc z2~hcl3bohQu$oq404F`kqunp!UUKnwecVefM>r+;29}UM*HR^YSya+ifm<;Q~g}Ab%{cS|UelbN8_> z!Dhhm6I0fF8n;$l@0M{!EI$YKUWBtU%n`w}R;+;jsAls7Mf@-x=eoL9oM>~nsJ#<0>Cm_5OD#I&J;y&so&ld*D zJSICYgfhfzMoJc;*mVS|o;~uE7@AK=H^kDD3SfolMj&yFcO_@fp9SCqm#;;SqgIZ3 z!9LMKThq`MG;G60w{_$|61GDMUV$7r2q9O>L23VMx}iIqEFs$LBZo_T;?4MIi9&S$ znDLuHl?v&C!tI5j>+e~j^d%CG>#*%?`aIYeUJBN9O+O163-osj&82YqWl+rN)sE2q~z3W0mUbt5`D@VLw!ivj=h@ zao1f{Yu*d6S8V#9?GqjgXytZkZSE#xKOXm!ShuU)zHxJVlicZ?V$UNG*!#=gWYCOA zzk>oA?`}}Mv#2~RfSvd4Oni?1U-#+LM|``WcxUm7fq8NpnJWrIB(Bg%n9Y>1T+PUu z(vB*QL@1J<_hQolc7J=9=G&Xt=j<_~xFIi&Y(j2SJs|ejlU(DN0`RLJ*Qq*Iv?s{~jReRbKriX3oGq5gM<4+dXb`3te%@%| zcGq240O!)pB|n?SjUQLy2w!@(ME9^_!`w@%w>K>)+*wFu;J^@TKIHQ-8W|~e@eM_K zkM|fnH$d8w^4(=ZJH#j&SgH%SH5>^^!jP9^w#cZW2PI;Cz%ppTMYln;yAp4(F0Hq%?@bnmd^B&g|^90XQ#Z z*tX5CwM&j1)*%b6tYI{a6r4wLZY{4VV>nHLQ+Lh9x!|z)og)d?Y}r7R2+E(x)1rkC z3Ygd4P{W$F2zmiQJ5a9wgAi}iBiIcHIsx9A$Kd$^)!iz0oVsJ;Yaa#S*+u-p2VVzV z9hSY-Qm3MYqNkoG1i$6uuBM5<3GeFB}s%AZ6twJvxSFOPCSd3(%q978ME0Ou55^I{NK~CMbyuQqF zahzJ^I=4q0oo8U|eOYtXzASqDd-)%fll#qCbNDmOg2+jq-Dn&@IdQYbpgy2<8VjcY9qBO5Pru!}UJV z8(U)m3SU-80J(Vg9&5XR(p@D=ca>iY_{|wNGjOjJl^Sgt)0oDUMRxwqcCXmp-cI|VPXgn6g+Qb|)762? zJ%G4HT5_(&_@XN^$D&PA5_0@!Z&OKYjk-bMWPvWJ*^LlFG3ODBKEzbT*A|&<`Q_e2 z1PJUe+LRwCv%0#WYnuS@cYDDv2!10~B6cz^EIv|D=t1r>RsY1GO>L^E&>fkF(++>Jd8`2=rMU zZ_C46GI5`A$5c7l=kqRjsy^6K#&1Fw<8#UcupYW0M!`^~a&D*|Zz$6?P)P)>tbqr@ z5UgyhaExQuAEakD+@nlfbpOQvna7WY-|^9#s)pZMn$TzY;M4al7v^CT*_0SF2Eib} z>v=ezahU(+d9=K$L622YLKnE}F2WR49;l38Op>$-0+cEt>hD`RQ$o6uprdNcr7a1a z5P#cO?bBL5-nz%&xdGLkDz`7*K7Rcd5c{$0vC3w@vKWA^?JcakEM<~s3Y?41*g0AC zTKsz~Q|~{i0ABH~B4Z^LLLD#uZ$4 zE^F~Ny|k68c5si0D-Ic{e)dWPM8}5=Ye&1!M8DcA$|GcyVNXMfL(;SO=fZ6T%sCT6Fy~BUASvEmKQ3KK>zRhG~U^u`K~5|vjh5{>JN=Sgp#=HIUW5rU4fom zmvB2Ie{Y`94wd8bUI0>p{e(^a<#{wyBisp7P@9#tXX`eBpc7!un5y;LB_DI%92>h~ zH5CM!t5eousoo0MzsW+7`HHc z4p8-~oIiV>t>4^Y_IqZjys|>^?xJGQb&;BGMSbUUheuk{#cHZ-9WtcO#J*zELsyvv z;wjeL-%@OO*irT8&UuIa7yDPtnqO&LDa-!3&po6PJ2MuT3W$0;8IOaJ@9N-h2Q3xTbNAPn#K{23SY*3OV^qNl1tPO48U<$yek;W9bPh%Q4Uc3+>zE>C%nVbDK zbbB#9S>}6>a8^^PnzQ177J+bp5O&|UJGLnYSo{}@RPL`NJ(Oi2lTDMm>LNplbKYU_ z+yFx8eXKWhjoLTXC>$>&lGvah5Dr4zO&4#&!>X%Hw-6#RuA0c~N7wY0J&cCIN^ONx z_EhRVjjI<2>EjJ4V{($)FwPsSEG^>~eex&r^< zcZ1LiaaJ6q4*7JtLA6(GRl_R;`ht#@({P`2(U0g9ju+618k(U15P)WDiscSMN_Um$ zX+8XNQJbbcr0d?6N8wmu;(ak&1*EK#gwR*6>HtAEz^EH&1x*2`_@PnHa?Mb$X=P)D z)49{v$FydY4WA%$H=E`db3V~9D++JhQ?leqiNqK)9I$=fHLKx$3Bz19G43>&`R*A? z_mmJNL9Uon?K5?yra&7>hK#ILGLT~+UHVc<*d68QF9XGz2mJtl(F~DA9XDt;yTJzMb5P_p0+tFDp$PH`0^%yFT_KYF;XruHkn#BV&*M zhcZNyk;*4h9}NL@_9ix*6-P1X5@!85rN{`O>Q*VcW%XSGibj!c*u^jVgjz_u)#lvh z3fiVdQ)^B^)HJR*roep~)428#@6}_s_WjgMFLX>96Bp@&$|a?$<%`2vQLIk_x>2Be zH0s}42h?KUczXj-hAWM?HZT_yGrsoU*Vui3`dD|(MFtY*oWtOm0Wy$u{z_8V-v^)8|yDBe~?yhEu~T7TFY8yEQp^KAc*ZE9arqjILg`Y)}gV@OK+ zpX?8ht$8TEx~N*HeX2+GZB->Fk2kdjAi%lkP`ax`;Y5MXxyY4rbUuk0@wI^NS{Lh7 zH2wnh++jB)*bGwT3aR&m-}cdRO7;`>LxOz3Xh=_;D;*v@w2+TV>Rdz&?%f$$qRSQC3idrI<)_=&D^`@_ITo}{Z4M4s4dkH_XzZo zN+xNRma^t?pZP2Zj1*Kb4$h^oB<|rLOzP03qvY(tPJrL`6V}{ab?JYh&mGk}t|@DN zrIFpFKk~&#$laY&?Ga6(yc)zXppNHzdBFp%_VY2 zbE$lN!$#Tl|-4}v&STn zqr1+~Yr5lTnxtO0jDZO$9#qRtL*DY!`JhBzAze#a;5 zDL;e!b{;7X(z0e6(>Pd6f%`P3apiFkAbvG^o=*y666@_=NO9uGg;6!w z`pvB|*&2Y?8kYOXKqk!K*!4-J?!CY7;ot4^|Fiez!Lpp^dEf8r>F(KQ&VKH`0ry^9 z0t7&UAV7$uAc~Yq3pY_t?n4Lv?z&| zWJ%m4lN3k++yGD{aPP%(&)v^5=ghwJboZA(zTT(jEO)`(`l*_lnck+mr@O!Rd7pQg zY0YFUB_2@RZsNTc6uwcQK!NUjm-dM^`Ga}vu1(>Y0>c+ozvKQl`bZ>$Zw@HDr=Un~ z%ZVGOzFp{jH6Aw*u3+%0>PIYup!1au^`EJ${B%d6_-Y{(_@@1C%(-Hq$3dpfv&68iX(eOUcb%vsUV2d zF^(FH-x|k`HP}}XAgSV+zfKBEfD2_dL`q88ttfbN&NRNj66DdT%Vd9_Uc@xD?! zg}_?3uvRUqPkW*N0?hqpe4OyFd9S6HbW~9?Ira$o*hO02=bj?u_NI-~aK;D{5EK{TI$#rOoS$IdJf%hs9G&jguUFOPNLF}mBRG1G{=VjQ3%Rs%7H>2 z4PS>aEI#QBD8I`i>;Frc-fKOE7l-uzq{rM}nG2&A90`5G0&xNgg~IJSQU0|MdH3(J zePTbFR4pE7omr}G6>Jrip`FgLv9=y;rrYUo@!|^R!zQ!ESvG3xL9}1+k`h;QL#L_# zJcu|$ns$?ABXj&`xc$h8dxi%c)56jrwB2u34ZjiStG}@teA)WV^xmf1VfL41RX*Ay zrGSW=`^=rg=O7r2JKyeb^?$lLL7Z;}v;W~Ng@Xm8rIZh8$3y3%iej>DQQ@fqqnAh9 zhq}AUz;QYbmu4^Va4)@$PKJ9EcN6j6D=Li&l}3e5uEXY$O&EFxZvw;b4jIZJg{KM= zMya1~UU=5K7d#``SF;6z<{vky|7;z-tScv%rkq!!t0O9pR?zYqrDsd@Uz}{i1O)e! z)wC!-Tn<#sVy(z{Wjr-B9*Bj&7tR!L`i@#BG9Ofhcym@c1)Fx0W&QRWy%R;WWMZEB zkgA>|)bn#9pF9WFhDGV&QW#O-ZM*n^;`Yzm2LQPr<$=6@ z^#LaeC+;fMd>YKtrb>}03hYfU?IfX`dtbt9SAeEzs!b^}Eb0oeB62=rMBNW#XOsEA zI(Zf*CjC_w((l_cA}ffGBA=GY$*$3)E)oXl0E`rHaTw zU+XIeqzhfy<&tqpIq-G3y6*}XU${7Ve7^ywfAMs%?-TR4`z}yXLYaV`;SM5=xKHcl zJgtv&h75NT?p7i`4f<|K`Rp&LF!bUhMG8lh7u(#IXaSNI#yTxb+=r_|pH8sRhFI1<9)Z|UMAjqMb)WNeY;A#-{zuu5x4J7s?xyV+k=UFLrT?m+xsCM>faj$7oM01G99Zgl1~={;Ze2lLDMe4%(Ai5Za0a1-HH-y=!r=m=^%2&&Fu&Gqy14T$M%|#1RZ&$0Q3o6P;p9LvAFOy@hD z7~zqws%3($LfYMt7tZcTm2WCf8zz>rd2_YKI<#}3eK!cU-r1zTp>pefne3RjxYo{BNr5u6?pH-nh!y3=uRnobznzGc(z7~Y;=Qa#{D z*HT)mO+hr=I<2kR#Agw?w^2mg4`FZo%La{qsm9U<4OPtV(Q>f7YrC=QRJszU>rg&Z z#@@6Uzd7EvxZ!RHxQ}Uu*|p|121f=M%Lci64r6Sf*{Y~sTIyWV$)5)6x`nxH#-kKK zMI-p7GX!*B?$Z5Am-^4wgR~c|s4?n~$Q$>@wU*(*#%_pvrBY>SX?cRUXQVyDjpBYG z;?tm?_-7|V67C!A5lA;>6q+_LMZ(~aVF9`3S5(e=wCEeCaM-9)?&l8z)IM3G^@Ri} z@KF7^Duv?(%movDR!3Tr&X+r}{jx4w6b=-SuEg%x^nTcz-0!qQlale)nBvhQg(nLP zUmi{mb@%h-gzAsJb$p9`WWCXEB{JfD>)@Zi$Lo*NYPIN%dq_*tXf+gJ0Tve2`Obeh zkI)3hqJgn!P&!qj|E4;}t=~%_&WSM2?$^5U&~70@H94}J?-=M`=u>^Xir@AzHWl4Cyh6!ekAX{&zb?!d*~uyNWBgL4Ba>lMx{oVnA7w`b%Z@ruKlLKS}8O)7*x zj%&nK;wkHjAx`1>grH3Q!AHDiBvaiw&>Kx1bj#M*G43Q$bb3Im6$l;eK(hOcJ>7+DKceRIDx^N14<`Lp;EtH`_~K^ z?g3=^?HO(b?sp(ucE$O8vs^9GL(ROMkUUPQ_-`edzI3|<<g;LSahtCjyeLpHkN{R8Xp{jMu@SczM81{b$P^$W~ur$1l1l?C13t}=_ zKuo@Tr7{+&_`2ReS^e3nYJcNZMD+{$B=>*#?IFXLhLgpwn2ZugVt92(>39ia)}Zj70>kf5 zA?e`;!3F!O&E@@M-uE_o2YU1m_Cp27GqR%!P~mWa;Y;eCNUA`)LWHme zBsND#&^-dGn>q`>ynvJvyKQ4PZIA@{>_P}-_qMC9ZWzZcoBTqTFqvM}ZC33DI0 zqv@mtb?#ze?Fpg|>T~<|+ti+^DFW_=u=n5SQ+baU>YGsGCJg^wgy7z<$o()!b)ibT z(q`@dTw{1{h&ytrf4Z*7mMvNM#6qB&yFC}XsZ?{P?J!;+GkkMM`Ffet^QYl9XGGi% zv|26Dz??S`1wsESyHwBdQbt7jI36oKwV2%fK377Vg1|FGGeKL`K{n3}i%nchFb=)hfkP#nhR8ks=@% zh_=3>@=c?jVhBT6L6P|A;9mbsozkfi`NR3RN7)=RxFYmv>v$O*|6%f>U8l6i#%OYevc#oW@+Q(~? z+w}g2(n)oXNmnv>dk`#CkE-id{!rbR<;D??4Md@AqBlqDd{B772MzvoWNU)IL?C{e?E8OFNur|Jfi3 zB4Uzo`l^MbALgDRLxzmF->b-o_{~9Y{Z=aoCBGHz&3s5CGoP8EbgG0z<#G)#4jEn? zl0W3NV_P!S0PT}=8#=FaSo=q7;b4{+qw9*!!mFd<-BF16ywS@ev=3>ak_AD*IEf61x^z{jY4Jo+5%r5hL|!0V@Umqg}-k;%u({@xrxSosKb+OhgtZNpfl@dp0j@*-~ zc@!&`Rwz7KKo~-GY49;k`4&+6P$~GFTEH+##{HLn`=_*Dn;K0bRUt@!-Dv)|J8~JX zju8l4>9YU80roBLyS;|CjPFBCEsx}&`{ILiWmw(bL;g^n`d_Lm6`e4uu=#JAoc!F$ z;CHse;OzmW<0Vx`V+x7SkRgLYM%?cX_&nm^!9&5cjELVn^r+fBO?A288S&1r7qs0r#*&xMttSlh z2%+>1(((t(6Rp#dPB#fV6wXn>15nB`jhI~ z&L7NU_w97`-g7d@fn<~)-9ECKR2-w>=z4_2B+hU3XR6`6$hQ1f@l>MUxu(*AG$XU* zr=j-s8iSu0AT7z@M+2260rY>=r*NncsOG)bdMY((=H7Q;j7B5SAipoqXf4t%bPD`? zy8n$nbH6+nPruNJU&l$pE%!#ugUanrwCA1o?t0_9&K9ba}+=PtFDe9YT_Bty>s#iMEJ|NCopz6TPAb^9Bs4 zkrGjpAw!03krDUXgCr48@(BOA3>kO070@9hJ`3$q`cMLLR*`)awh zVd3^&)_-T6+6QV}`R}h#`Kb!sKkiO!C#IxJ75-j2Q3}Yp{~j#wy{GQ~UJgk>_eV-a z*JpJK?=3L=-tI%)g(287ZhckvjCg0b9ayigQ!EvgTAm{`%)QTn5$ieqnU+Bj- zA|u2!5Y zu{#H97Umvc{=z)#53Ms^AEV7{%F!WVygFvQF{W_7z?u5JSD}3xhQlF+#|mJ8-Lhlz zCJMWGpUWRE)BAcaHhrQxdo-dgI)9qNKIM1)`#Vt2{nR+u8_VOn%v*208GAo*;vpbz zV6UyM0yk$|M|038D!2y=5l_L9%5m1FP5*44>ieb?h)r{H5I@JYr)oG|hw<7tbdF@m zkl~ggBksGyPDDK867LN3i9#?QVup3nAWSj-YM-uw3o{>?0Y??5k&Z;VlJVx4_KR%> zZw!#Gj1lq9zf%`~_lw<$Z3jlS8+x_-U+**fq1m8FoKMnI$@y+V+@rslSZ?@ILPh`H zHY$38q#MT!Mu$eUuePbloxR{0@y>9k(4m7dZ=e-4MR@%X_h_LwkFW>Ix}Ngf*v3GQ zsmM`dJu<`Y+8DEjBB5R=d#p8<8&}t*E6FeC>ClNE&zC!aL!tUpRSHK6*eyFG0!SLY z2J_YV+oGb2ycYs1m~=*LL(Y=~2%(%4XsVyQxAQ8cCjw)}z!^B(I|UwK=x^)ne|taO z`7WEaO-7eS48AwOIBIYrcY=KV-d4?{d42$2`2F9dxKw1kHpYI#p13ciloO%p{(Ts$ z5tFUKkQNeKplNnr~Y$wTK_K513Kk*aAAC5jAm$*7RuDe zGw3-zBi#4Q#Pg-_-U7q#DrcIxWFky~UJoC2T3tgkHC{jaI>(M43+~HX0-sn5 zfiY`LogX9WzIy(UD%R;g=LACac-LTbWu*F@i?E$#%;_=3(V8JdrBOllCEaUXO0^OP ziwAdJ=`KK;AqDB-g^fl--JeS)q@1{v(G*U>_TOaI`dPM@|Xx9!y=i1T_K?Joz8?x z4luenQqO}Ojwf(v2xz%2mbDus{24N2xM^(nZ1Iui6ok8nxbIY`4KgBrw<1o^Cxr4a z^u6|p8pYE^%zdUB#2q!*jvHf!mxi>y-kMOyQ@6W=$}=?tR)zz1=i431kCu_PRPEk; zgMG?zOiZ7mn5SXz?E&S-%IH;{;)xqi!D(?wfU4v7mFE&y=s*exXLQ;hKPPn6RYbMM&A17B&vfd)tM^H-03|Du@?fqQ;uJ&)uQlt0e9%q|4yHY zF!cod`K1q+6iH8s?3mi`d^@!~IzKwiQ_5qI1krZVl61e;4G3`|U*JZNG^}@0*P=Tgs3j!=1!#)OL91 ziF;Nt{r*FopnHQw?UP}+x}QIe1LMXRIg+e>R#jQ={E>IjrK!lVub#mz#I6RjSPp!O zvORcdKQOc5JC{V~DUZl(4Uc{+UIJkQPvk*z6$?(o$ zF9iH%owwb77yY=7J+OBq=TQ`n6cEpQksA>wTP0C+%qEn;6Rtip!e>~^D{_SV$P}jajT)!>9x6P_^k9w^o;yCLOzH{}jL(oEErU_1AqZA7h-o~MK z32|u0%i=s-_q6KVQ3)4T)+(^+^)+joHfIBiArJ8lK2wz$>G1XAVnR3 zNXwy2yB7-6*x>Zmf$$fHrB6r`;l6RTrk=ij68f`x^|!VS@g>lCRHtmV@gy$hJeg}vn zf%yFA#Vz&ciIrIRWGVJ!at4U6TFgu^#xIZReqF8rpD%9ux7**lp~^1++Cd|IkQqp8=Re#i>ZxrLWb!T4zO#fbjG$+ZF$fa;Autx?+4+f(3>T)w8q*0D0WinLZGlY zSCM9gn{hL#;*Bx6A%P7?E64@-ynfdA^X|2x)726Tzyi@rtY41<0{L`nQ{fVf(1z~r zzx$b7q<+=9hKUsC0dqGUi`!4ROoUZ%qK7B}tu=qpkZBt;D9QKQ2)AE8?pm&klRc1Z zi;%fdexPz#Ohbf`t_DHuA!v_yx)dEX>(13*O`_NZ_KxhcGAYl-c*G!MJunmgvD;6g zxv>B5wDcLGvUzfX3nI`4Uep;f*ZRJ|hW0`rVNu8;Ms2t(`G~gDoUri$#HaR!2Dt@- zBqPRG-B98vyDj9o6%|(xT|v%A0Mt&+!a90L|GL)ro7Tc{S=5;OjH?R#rrxv@+&aQ; zuA(~tLWcrMAjp$!Lo9#Q!@Xjnm;5D@QRWX5Jr2*)%9CB#Bb-^vY1xhr$0;$}bMvID z+oeL$GYJ^5cJ{lKN@dx)LY6m)jIXh_r7}k2)vYAU`J{+YWhUsu3J$_1Wmp8-si5D< zxlXM$oh)w1_kO-z2;B~23SP?*yuW{B?BtS(=ubAo(IZG0Qf?q3IQY8M-;@EU%lN}3 ze*2GZeFNPOZ9CL`8I_OAD+O^d$H|9h3mVYaJ^q3#wjmi+DO(usbFW9L)x%1Zi(^LX6@;V>n6(03go3e|sEVCA>?6)H$?JDBDf+$zc6R*V zow>*pMtB(%9ICW%eI8V9mr++MMAgUAe}54V9e;o8wTtw5WHh;LBfu%wr>}}&bkzD= z@$fBnyUnk9<2g1O%g%J$#=+&Lzei1&ZwL}BF(faY@-mcULNvM6=svdG`ByIN^cG>N zv1>WJ)Zc6PQZC2W;Q(@}JTW=EhJ?ZIl~;x0FWoUBydeuV9u`T>a1%*Sswjie=~HC; zF#chiHXR0k?d2uP?@$=hyx{?~5WSc=%QVNMB!k60_KqI%Ta2UK?0aUT*zPP=H1 z1nFkcP*}vtGan))!q;}Txtw3IGC|dsr_q!MvWSP%hG_dFgfE-py-u=g|I7+P_T8ne zM7YAYel;2JbSNnELovnLcYuV%r9+gL@m}X%(#BB0B^zFju!226C>fk9>Rn%{CHumc z5E)>*^SA_Grm+lt59&5w;fHXkP?b4=e=coU&@*txJp@{m{8Sptb7z1i0YP|TZ z?udK2fjKNOF_Cae8h7t?!TMp%!@Y3gbkW8R#qU7~my~5x9Rh@_z(K8aX1ge}*_mYuAFv3JxD?^+GcFY|3lfS=X=5#Of1z<}@#O(^6`}VN@ zpT*LxZScc;Up%yR3^X_>2zL3+k8v=1y{Y=&+lH0*3b4jnwY^`r)?j>YhjiAn-SNha zpqnwFNjaTaTMs#SGdR!hMA#)m>i9coKmlQ*PeejKaBJIp{(@sUAlRrc`!c*Ykw8T4 zm#vP(>oCkg_~sL%3iX+sW_E|_xo<;(1=CP(qx$$n7+37vl3#^Av}2ei9QnMp=V)RR z2cHav_aOyy=3LF(`5L2_!v-jP9Z!N;&$i+vQ#+D3?0cT0QHU~Az?M#P_|214 zE_d+1^EoY07PGZFzCy3_Nwef~58NG>_kpHj-L+(#Jmn53OnV1ht4#veG^C>1-&j9R zZqtV4O$^06LQp?YC;ae;s9HmP8`ZdBDEu zCMDPIv8b^EL8bY$I7BTFu@J-B^Jae8@qDwFPGra*;i`Y~_Y_^ZBYEDl)8kZ?ss4sQ zG%nx1%i019`Rzc2yVPp~gl>K!mKhZb+*=8qaiZ55)B@Gwuy# z5BQF^QK7xSb`G=BS)S1;SLJU}I}mzQr)wD11({PEX$D9Y0>#JpsZz1L>4m>VpO;lk zI$%!ngPTY%O3QJfoks*QzJt|{u&c++bo)g}35u@Rv>lNd=Nz0^Ml*F5hYQ$9e z^kew9-hDOl9lL(EruzYAoP&BN?iyJvKFAhn9&x^VUBU+iyF1gCWz@hZHNVt;-Hiv# z+g*j=MipRTch5N+3aEhD1;6~i7C^SOIvaa7_Ro0JVVO`QSW;$^`<=D&07P>l&%p=I z1@3AGLqpYR7ZQj_(Ef&~7@ZQc!-~DcxgT^*ra4auxzx}!v@Z_*NT*haaooD(JRB$N zyHw8T>ZqgTMygV?S4|ZNYxC+cRdlZq{RVq3rx=C|(ACmy@#hJ@9^D|3r}|Ba-(?{^ zyF3eJS+p<>=NHulm1G_B2EE@bhxgJf^kCW4ZjbPt!yP?;H%sc<<%h!0LCHJrj5wA;Dj)OA|K; zuf*0_hvn{W%Yo|I4Uk4s8WkT&tpMwl9EW?klC51H?6qDj>--^ZJ$?hW zCr+{NFAV}NMNYeFest;_I8oFHm&BAacPU~E^UnosjQ%^RvYR$W%p&V%t=Wd(?}

PackagecjboURYCtlgn@eS?HugB5CjdI;s(gJ&AC)|78#_cKE6Tb3;+v zY&K<2dGJy?rcfu;aH;t(0sylZu+8y$B-E0;3TlcG+_EY3ny9SWKN!_LYLPvWy(Zlu zdrGh-my&ZxM;m5rmo4B9azv<>2dzeIyqr@%y?f$0le^>XR~wpIIwp=~EGCmwqDdps z9?YIA?IA1%ia+w%*Xf9Gn)R^z=fZtqnXtF@1y+t>S1tWc&uT?W+>>b+Gw zBuF!PRqUUPPh!$gWGOVkmvi`f$2$}JVRv1wg)-O|y&UQ+A z(*+!7Th>+23HipEh-*5Jk=CAl3%NCNdBY|a_ogn1FTcXi!slucoud??>9c=JnhaV6 zflvG)b<65eNP@dw*mnMCAZ$`FdJ8u;evsGWnW5c(c`hBs8;zlL>w3@kC>E(Cqb%rc?FRQIUXxR+dG1KkzU>LKv6rIqSrQIjeqWy#%O`wXVU9 z{pt5ThTl32`885#A3n6gzWv8kw(C1zX|~6 zG^S@~Yxb3syAhQx)q3Iz(#LxV5oPMyjXgSSKKUX3gYharc;JHUrz`brEqh~uJUlJVJqYMmuyz^DW5#T)YTPz)l<*F0Fg*$#2oaMK5NP*M~0=+iQ!9+ zHlp8ElTVH%OXDl>FldP@5mcKa>e`TQDLGU?+@GCdly%+H%{3rsRQ;7D>x-Sv$OPTE00?=B!B_O`X;#*1)q#TXWe$ zlt^ctW1Kg05ixjmNpj;P$Tw-fi2}sPVABWt%AHxl+YgmIKav2JKzbc90gRrygQN_9 zpBkTz9WeY9{yb^a;es(pXrJKPrBs&f0&{M7XBSZmU|f_T@^RTwDwpVLn|$r z^H@Fnsj#)l!J#~mm(4B=(__e-=Snnhim9*_dOI!U%2m?V(eb^Xnf7%n1rcUFq5r4# z*)j(*0Q`?q1(^ze92ER0bjst%xD&qaSw(tte#Y-B&Ny2&;dK zC)1E2nY-#j=wv5-)^j)vI`bOr;V-Bwz?^8lcpTj-RISs6{yz{sb&EvaOH9ftQ>i;8tw;jS$InXjZYBTB@296yHS_P9M8-${i~2x z%hg2wIdvt=SRvLnr)DF$A${&7+!J|7Y}M0Y!|MC@BQN6Pz)%o9R=9@j+NfE>#LTQB z)JS5v*dP-}h>ztQ(hY>ndX2$HyD{XZ8uQbGg=cXJ*iBazQnw5+jzr)$l69O)Lc4aA zn=hvf-{d`0Xz6AdOcC7iaapQuo%^D_&qk!l`Wl-WknFnA4BhkAEPuLl|2|EPx$0(i zcwM4d+(Gh+a%Zjq3n1sheF%LpD7%f=0Z1hBz14OrWBw^8k> zg;H+#Ga5fJMgvaDQC2^V(_-oVil}Lve>f4FP*5ScF)e&!zDy+<7#x(i>~db~eO$?9 z{|!K;aqfAva#1dkF@Akb0RwMJd4ew`lAg3+$PErsFX#Xc2N~||9~+*R3#=#CdbyUX zgkE{)0a-+K>e|nQomrjp|)o>)+H7$JEpmR{V%*ju{^K&!@c&^9oz@94oZm zhKmiI-G_YM-h7)49#CATPnJ5&`MZyhjKai7JZ{_8ad=2VjCBZ8Um)#eUU;u81t8s7 z70R&jY`_#(qo&l@&N;+=%qe>*5(&vE7D+-i)KnF5aI+qczpZDoiNtpf+s&@axZJys zwss!6go*~HFVe(FOd!qv%U%xVzmrd=5XBcjDAEG@GKv_`*4AldCuhJ938H;xqcHup zG-gkfw4!HI%uw;>5(dC5wz9LWJrdy4dT-}=*U$)=K4+vERJ6^nRu})0e7WCxr2eo` zsxgDd2FWhR!tWvN=Kh9JB*>ixbPcui6)Mk$U&$oyZwdW*x42!2$BBun4%A>vtZV#N z+g!t(Ba^P>8kpAq&3EK(9-cVh{!Qqu-_(f@we|2OnHV}_Pme1;VFa&Qv5frRUjoBU zA9$)f{{x4$@O8lkT9ROMTw0o;o-pl75YfNaoS%>Tgx*0iMM|%d_J$Q!FwL4As0^yv z-LQZ%Vk;JVo_k7_bIT;k7da>{*tCn$VZotQ3IZ}#zQYYeFF2H2fIhyolGn$%(jEM!VW z7TWKdd{0{J@R8oQAaysF#t?D4nQqq?3B2cjl#C8~koV=NraN<^Qk6$$vKC$g(pivR zwQ@NrquC`VUVTD;)En@koifXiJetUt5gsinOQ)jx!&94BO1>z|W3)CMS|W*9u=PH+ zSJ4!k2~Tp#CJZv)5O!4^C3(HQVz!By{%5(EZIuq^$xwboq_lm5w0~Jm%?auu`xCxR zMbwh}cu!g>8(oTETIquoy!R!HooaLb-kYdV^cO_Ub#+C}X=oT=hLMQH;R^}gsm-N0 z)=)&rv|i64&^LbNME~zsxWI^pv=bhL7M?QubxH^@av$tlKf?1A?=WNL*3qEMgdZSg z_M2xEr~CE(!k|Unbi=#>;P;4C-iwbKtfCt})pUY2dPC8^CfnwPj(zc5O)!E;1E((%2HCcDrz+5G~(YzD(vR5zPBAjMU;tY9@bbRyHST; zeb^fhbn-EidtGqjGs}UTE5u@6^Nx4ztGR5^Pn3*C0XIGhOs1Nfo2yK_-J+16u0;0~ z?e0;fOPM&_dAFVw}=cNumZk}CVY_9 z?ea0b0luZsLcSqq=!|c`xqqv%z<@gb=^J6&+2V|+R->;X1@kg50T?`UTjT}Z@7{kK zNWcY?a7tj(W{2tsEMC2F{Aei5X!pR4y(9x^gDDBy0;puA4JLbv#=fypEg(4c4b$^Y z0tiV*TJ^p=>d=ThaE&d$S*&#~8!>1J)gO@k4;~3N+fB(mSoLvRc zUPYt1gu#E1?uR6HYYvDF8xQpB@l1@L!44Wm*WE8JsXQB4lTt163FscA6Q@-Dz2@mAvz$bz8utax+)p*X zcN}3Fzw3b_#Y{0BF{@~Qf!?C&3ZW7yN{N+PT zb(+bBMVw7Gzo@J;Li;0E`x~_RMt-Z|!*e8BT(hjEhhYkZG1jck^vAw)P`3XKXdH8J zn_q&!K)YglMEq>uh3czKhxK39Rz{Wp1rmFq`=UpIA(|J4 zlEv-rKgP#ZU5i3&4s;K~!qscXFOO&`02L39L3W$p3b(}+gIIjT(f=j(E~oq`dvZwJ zOki-csn;!TDxT0>?0Q2TO6h*1y}(kzogke~2kALyIMpMIK7R_#%=tqJDr9(9LrCZQ zHpLxp6!}n?;n60%Yu*u1lcdGk&44KDjpa`n$rmtXCZPzyYxSSiCD>d9%Ha){cLI$~ z(;P!jA(8tOIthwJ1BAz~Mb4e`TG?HHsqFhYWX@96b!WD`S)`H$a|EXk-;thKWlB@e z#lX5gchak=kALIt3p#(RFnk#ntj?~azX)T4+N3>A^%u3(crek~yDGP!=ILw32bdVE zj$ij+?=nl~Y;nxE?(QV_Dq&E2mSM#yHe3{z(v_SDb6hEJuWnRd(yA21r9KJTo<8{r zGh`xw{JtpT_o0xm*@ZgHz$b;-((@N5xsK$2|MVu0+*EY|H+w^u8*Qv>W9rqzODdv? z4Ht6s{K20>;RCzJD4#eM`nm!UQbHO-oSTU4tFc`LjK}6s0bzXU66@(x)W_0&DqGt; zQ_4PU!jSrKp`0H5(B7x!-VS(}PYj;$%kBo%{|j+qH8i7_CDWBSn}LC7J?_SEMjs7PQcB^~PXPDR zSN;chYA3y9VA~1JkZ@=*XTmccu4w^J)9e+68NXYimWY1riL`TRStW)$>cUe+Qv(FO z*TBtP%Kjhe&8DeSR++tkEXUf+9j_M)ubs$$sv$SYV~r-5EfysTogF&~j*M49i7kKP zXRe-Y(WM` zKY`e1AMU45MtMIV35499%-&z3EQqH+ALKAP&3%~PjY~>sNO{QhZR+f zpZ@yA^_h%}F8?Ucy^Blt^9OCXAdm_2VfcP=uOl@0cYTMI@(uDTP-ua0^WIyzs$9?3 zN?#a-3zU&%w6}?h;sjNqRqHjURUo~Va8QlqT6CHK)W4*JyRYAoU>f7DoLoMxO_Bqj zSL2N#)ehV@ARGkIC$5ew(H?DOw75`>^fA=1gn*asnsL~6uXd*0RPZm#FjEGc&T84L zMY(xuonzX)m@#*z8Yg3Cxx7ojf7ClvN*3AVs2lwT{? zt^WZy)5ZDcWVhsE+1}4H{IWBN{ErOU4nB1M@jS&!az@nBt8!ck`i=q({LhRh%&K7~DReH27RM5bw$ zP42@@44fIm^u1#8q$AV9Hc9GH$|w~=4T88WqRbQLACY|Ok~M|TLazBh7j zHe4kOPV)^d6WDGx?gR%d?2(etFwUqhxlS&#RB06}W&fNgj3z)9qC;J`OI<%KnYI;a zgjnrGn-B4L%CB8GePazz-1D4H-Tt(5dWGCv9Ti8c~VTQ6Zh z96p-7>QW21pEDkp<;uK#jC1RZY6W6M$PD@@@&B38OJB&8^XJAA?>&&iq`;5}b7p*r zyrmhA%V)dt&AHnInHL$%eZ%KQ1w*+qw|HGjN3UjZivBPmDlPE^zImWc2Horq#9D80 zJ#Kn#IWqpfmbxs}H8!^L^5S{v4gLzUYa!?`Owz8+?5qbVFbaH+@#n^`w>=ixK}+kn zi@!@{3@{=L<#tZ6X5U|rrG#bdNBlSUXft*cTl%}pCqqyq&4zcikPdk;m&&ILi5VP; zQ9wc_85M28A8O8$pYf}qOaYL7$ECiH`FS{ji;5!;DouNU5%#0?1Br{UV4x#dY=S!y zgEVD#D$_3xT?`n^hZ`}NjAGfXcOQtbW65_1C!tfmp*?CfDfm_=aC%e)NVUiy`Z>~K zv4;R^i)D9`DZs;?jO}2S5+@8os-=WOLqaP1`##?-kP4_LcHje4IiC1m9ElsceShlr z%9Q4+);xpRPGwWjbAg)com4_7zNT_V?&S>bRwSKw4Ly8qKFyL^lU>a($XNmqbl2oTpUZ>HxI4&Ahc3((vU`UN78E9uuYRijU z>zL7U8?dgtn6GKmUHRcWg1*g8g}li%d&zEkI#i&DbfI`#Bi#Nld*PRe41jEh)N+_EG`?Ozrv@j;aHB?3MT3@!MAy;R zaq(wC?E4+U1~UG1X3S>Tyt{^jY}UHLQzdWP47B%^uaSM9@rIwX)W&{~Bk(1srJrX4 zkED$jEI%2TSj88|64~pWTTVZ0lgqyx;OnavL{}?85#@Ku$NyP_?T6G44Z~cARhc(N zG5FKg40zn=`2Ai^jTW37ER$_A4IYmDKx@xE>@mI($-?Jv9nGn)FT-kv-v+T?LI}b7 z8D+9}bAjP3GVP0Uot3){hMk^NftHB?hfi|^CZ_Gyor;%lsi4BLeNe9?F6D_)h_-e$ z-e^e{13JHw)6_^c-UNHipEn7@Vf&ewbjdaEX+F_CR5DaFHq*3LC(?{AeCrbDMk zaN=`Yd++Y3m?dF^KYt)utE>qu!M#GY0-e3cSvFDEW`EUd)(E!x+P7_h&aOT;rff5F zg-__zo9^!Jpw)Tuh~qV&puV2j*#C+fg;=2ZLiNQ~S6?4trGD9tNHi$VuP3R(_S|^O zs;l9`4YX$!1igV`QDHz7Lt;Mac#KaJ(ldrvY3mRm#=oej65CP%QSceQO{%(XR;*)Y zD=A+4FBZLXm(6!%!))f+nj<55c}6uh1O_Cdu-FsVf_*+-c79#K@ezRZ%I5?D>{nO# zZVg+<+mdIe$z_eI_r_7T>yO7oPm(<((ywZj-+mtgtRK50YrC!2t54MkS3z_ICcZ0E z)P%gFh99sx^ug`ZcflPA_8nX%HXJYa;LFL{?6^wnJGK@*PSqcxPBrIOtx_mLZXzi} z{5}zWN3&`7soW*DJZCH(3kU?q;+XEBzCC-cuc~UScv{3sIXA7(pl|dyXj-I@nVxYB zfIgMOLR$GwxXd}kEEbmpLdF7Kg2c6Eo-^j>`7BSvjqjgVz6c`_z6Y}e{*GNrzzLqI zYZDrs*@X}BtUd22GeOp30P}?>;sy5+H|lKaNk14J5d+(ztegQyZ{46|X_?2-IRfGz z#5JYdr>u3XujmrH$O>PX($zmgw(s>HrQFUM9H&6${H>FG=Ss9KIM?U4%=f@PU9$w2 zKgjqyBiAn{B3??J^J{jS9>eh!_Uzvmb>no#d9sv7a}V&~F(igXfOPpRW-N@d`&#!1 z$Gx5`xU3XP6GP$Ye=QSH<4-{28C9zoG>+4iE?2*rkQfH19-ZHt60Cs#In_KI9T`Me4K|$vVR`L)&8X zor8{Nj|*XA;Hnmc9(H{X0-p?;N7w7+<*!cco4?2a5&0oFKLOuKj!=&eW>euJ$U%Cm zZR@r8$)MM}?{Om#pvT)h)j+ zw$^ctJ69`#L!f6o$S~2|`_KIIxDDm-6F24Q214V zb8fDx#HCk!FAB;iiFzxmtL|&bb8;^0C-9W0)SmLc8v&H3F#WMew3x>A=}E~ZH65*~ z;CQ6J)#e^teIUjP|#$S1>Ac=Z`nNW?I-f2!0# z6clG7;U{6h>$J0Db5uZ7q&(nB@qG-AAQB1w_D^#yO@Bg1|BV=XJo#YvO<}6Wnwljq=98D2fI` z_oK8pTflS5Coiyupo0dlKAG+Lr+DxkRh=i}v4{Jc^2?W0!dOT|<<-3sg;fMhG}qA{ zS@CDQC&=Je&WcG5J-|?~m?W1^Do7%_zN<}UYZ?C{L=N>@K98rOHPa2ZdJe1&r6gYq zcK%1|^a0)RlgN#Xf5a9{gyUYV!8LK=lRWt8j@LZ5PLp9X5ijv?AbWGvL%m1|l@0F3 z;Uqr@m8M{I8DkmEi_@%WTy^C&tob<4-+mN~a#AU;#2Z5{FF2u4F2MT6w+MxYD09Q_ zi(Jv7tLQmOO|&U51uUR6yZB>(C8}EH(=GS)807Fz;gO|Xq#U;gqbd45u&|;4r1~Tv z=!2BOl>xmX)eFlfnjua)ArTocSQ>X;;ljN!K+pq-U!fs=9hU|3oL#hY=)MlTKA8T= z835eaIi6g$e|WpKck=bMEzt)WqcbDv)GlWi7FMd#frzVoWoy~T%|erEO`rW(S`bOx z<^#M!#7Cb?*#j59eRPUA7i({hwCl$ZaJa-SAP2Nj-H{WBCDe@ajh zFSQNfe%Uw2DIIi;4pC218_J$>h5uUz`qs4{cd^z1)YMSKMxY1Ym;z zWcTifa?$jX94xSMyxrg`>{Pzc%*iKLGf)g4y zv(RBeni@=^e#Fpx^Oo%@b9UElu_K>KrZ$6cz1{pB1Mx-52Hj!uC#uXGi2?=g93?vPKJuSP2QS^#@KU2`GEU z6I~jQesX$r=m_x7K}2)u6Y&3Z#ZZ6rV&OK&!WO9@EYig5RLE<^mnM!vdPU0+Rf#m z|Edvw-sp5uaJjTuS7OS3fd8x)2fwbADxYmrKxvPegr0&q77L?oFG^|_HU0S<21=$C zMqSP?cwM8omO^~C=Xt;&e9ak#0lgR_i)ui8w)$&_8hNxVjM-^$e|umdOX0Jv%ea!< zZ%e_(gOE3vl=eD20+UfDsO0=312`sHaIUee8~kJ&@fF=eI^Vrw`O-r^C-MjmCh{7q zQhMTHxZ1&7FQy-kGjxu2@;jU^PE{|ns%dzmZPEdn>ogh2bw$(hN@OMO**A}nLvcvL zj>}*CcbX`!AGfFVCyM5_mf6K4^LEgq@c|9@zw(ETYkTc1!a{6nHvSuWp;Pa zVgs!vw730oiH{Hy%Sr9AM!wX9ZLeK{HW7<2ssi^mPHlaQ6S{jD!N@-58+rfNPzV$1fN56c~tbw5r@u6wVeKcL1 zE|mW7L-iu~N#fiKNf;LxM2T`>Y1aM$eAZyXv2D@sQlCqo_slPqO}-n(RJr-M4i26J z{^z~~B6i096rkI5)SYEs8s|kzGHow$^PQT@I{N?`Cp>5dx5lwIO3X#lxQwElxLfmE z_s+^+V387h@U4z;_rChG&c9M-W}5`pkT8HJA^2t`S$2z5e8^szim~L;n@;2L5yMs< zm^!Z!=?O{HKg<$DmoCI4&LH!2<-!^5q8L6L2pq&8^!GD#_lfP&MG(jGuD%PdlUX(ed-7V`4&RK0&)_V_j%tp<`jU2$%?13C_JZ`p*w zpyesW$we+(D?$#L#PVcV}XeQGK(j@aeZyeQE!^r2{3bE%ImIPNw z+%DV&2QD=i;2hA7k`C5pYdfrGEb7>%Ixsy=&pO0%65izB5g?F-H2dc}?-L6lnb`BA z)1dh>{sW{yUOvh?>#gPTT_KCbA+9Wy>Gc=_Z|@|gGOrRb6qsSjA9%(x+KWv6V7hR; zu-^Kq2KfgKN5e5W>I5XWN2~Ba-Eoq(%b+Ejz^69V>^ktx)SkLbTVtQIuC$M?`xm3N zKi0{0&N+pBZc6=rkjQnH2B9rW5+3$YD@XFmTg$%mYW-+Dk`XOvEmPbm&BTMiVM3^3 z@_BbZh-sz4uV%cwi}#)%OD{b3A!^*JK2;&dZMiY)=qx%c7GAzM>BonOkPqA8aI?_; z>^JUWuwpO0YNdr-M$pIAK#k+P1nZPf)H2=s-h7dwO~(CTiR~n~&2aI6v3~e4Lh8T6 z)Ee)g8l-N;&?i2%g}HPKrNX4?`yyvVneT00;VgBVt*AqjlL$|MEkrQkMK9M`5@A2A z87p@(zEv&=NfoC&1MriBgM&NoBR72ir_0a-KC+0S4Wo@Bcjh*qF;|z9d~YYnhpV-u ztB|HoDqiJHELnb$78AuZ)e^SKTUX}9LVSp}3v-aqNelad0!+_MSYkt1QWxLG* zg9B8miy~-qbPpBJRKXlcCu@|P`hl*?;wKwqvJjvvE`S_&O1HN&atty~bM7A8R2Xqk zBhBl(%Oa#IyvygiFOi^Eo%bz)q?0!a=GO-lm#pONECs0ge2>J-^{mijaPXSN^r4S< zWYF0)1wi78&h5L={)Fhrv?$HQ!eVZBz4GmBCGKgDrw$&`PdeJl76T76(e$(JBt5V= z>_G$w59U15x&d>g9>g;GEne7+k~c^40+u-<>!pf^cVYuM4E)NPU#`vT=I!3~+s!|Y zFogOdu@0h_7w1j*tAwbdbVFbEI@AM07o3&sOsh#fnvO-*-y46nPwYns;wN_UWRY7! zcW_0uqz>%EH~!V(-})`mKsU7JrXu(ZcLPJo$dEVxERkq`LLVY{u-4nd8`OD~^+6m$QlYn!%UM8l+Fg$~ z48F$Mds6c>z>Tc+n?+&oE&J$>)f|l_VIM48a5|BhcDwPjBpK+vjp_kDvv~8qd0)hg zkbi2rYu8&v*fS?%w@M4LE;;pmcqJqyp=7iMUs{k3-Q2JR-Gq}O<;^WCY#>HTp!$Ox zUt)0Lpc4lgRIb&WG}|Z|lpg$rm&2IXJKZSoyp^>KfY<`fHc2JtP3occ<^sQx4ebNw zfpV-nUgnB}h_Bz+_UpzOmn~jzm+GcTMKnYuBM2#LZgy|;P*1gxLKm5@|IYU+%aaqf z-zqiSJ$N8m^Z;bj<+4`-eL~8WvKv;xT)n(-;-})>a>c-|nVo)T2&PGX9t(9Y8F{F9 zX+X00UkiMEUmnn+ao^!ggkZ7s=e8^ZsD=<+rH1~RaI!y(EzP|0s)rqM4r~%Dg~aW6^51ol(gxq|HdL(*xkFUd zrkZ%uFu0FzGXySl^8!Td%WhLOx6491Sj1*iaBBeigE3(9+UTKZDO(M#&Oh6DEYV@? z5#j6Zt;cNDF!^!T`l#{3voTJ5N6x>7q`E8HYQBZn#cb{FQ)l4i^8*$P47BF-5W-w| z_9dNpSDNiOhOa>#D`WZ@XThr#OPG^Oe*6@8R{zC^sm#P;vg*dp)@Gh!3o7fzqw-9q zO-PHoyC+$1Kbe&GZIT3D6hfE&T{ClI0)8(06K0}2HKBlgB2fp^qkP=ab;+T9m zNsNOIs5JrgAu!!2TLgc^=)~J6vt$i2y4HY5Q&-<@Uw&(v?AP^RXlQ7FHIV%BFNj{U zR=yWk6LR5x!gE34D|EO09~uEFjVHoflu0Q zoWRGaTCxbToRgLdzwZZbUx`u{zrky5#HU6)o8wZA?^_b|zVYCfz(7ul8b$rt053)A z%G=75zUI)B!-lRUTPB+ohfOSpNq~nO;3rM|6?*p5_ArPe8pd`Pp<~)0C^nD^M;u>5 z^zcNzn?+_B%<(=n-2Jck%ax1cieUrGh1PEeZXSN+^RA#=Qu_%s8P|HRiyF(tJA#-w zMO)LX?~_{cUHWs4t)`Ps$$rSl3Ia%Q8zgyUn5OTW9e+TRd@`30%MRB`+g0_M$e>~c z_>D2)jI^jzU?|>>C&79~lt4(u{GAuxdJ1{G)4hc8qz)a@LjuE{87AUU)ye%H+Ns|r z=_i~O>?s4N=xcTmN^xn>N$7d|EU*7M@P}5Q}bBjDDp%C2|61n z<0wu$QH)m0Jp|w&)>@fQmd`wP`DXoFU>kk^UU-5~Vj=+#xu6x5^`H&Mwt9L;SQn+T zt1w}2B3RHg(P1HgjuFO7>pjH&zW{o$T-XcH z_xxAymwqu?HX7u^r(;uXB^zku&+bCw298~C(Y=!Q1xw%GR>AS;#t5Opa-92m$3jb# z!qAH&&;N4lOWu{i@8jd+7uja5MYwd*!vj*QRFTv4otx3PDGyN(%ZFO@*v&}W7K9g$ z?$&z*zO8m!1%5XW)Cb9TVw7GwOpCl#g}_vaH{JgP7}X;c>oOI&bjJv_a|SLsLr8}} zBR$F>c6Z-!|;Vk#QZI*&;fgzSMu5i!7w0+BIOO zC#nK~!IChUgR0MVNf-bJ3G9Gvlmf2#c!9wqj{*LsapX)n^a*QELiHkHgXjji{SRN# zZA6k$H;Z46gcY`fTuPwvQ(B@@Y#70?zx_#&Qz0Kx2_ZayhS-{?jA*n?y7)J}#HsY~(Q2VASPuI$zBF@1%}> zQ(GdlRfhrla;*un!dAheJ)Kr%e?gXXr14|D@$ubD*rm6*IRZqQo!6|o3btAZis0S$ zLY?9;T`oVF14`Xv1km;8n&*RB6VEBN4xMN32l7XM-PL0Q9vWaEyAet9AW=%JsPGGU z|Iv5j=MRzt*+^J|A>6n4^K7~y){A2ZHuL#-e};c0v3~D=mGq5ZB=5dAHd#^ps}Tmi z#KW^)nmipbpH( ztg!N;Q@x2LxWo^5EHV|d1z@XAI-KxNYf;q(z!+!~PDn^_!P$c5(oigG44a7|opTT+>JH%+b^b zFj-(2c)O`QmI7ekGruBIjA5U9Ij_Dxaz4S%9}F~ckRUGVy%7CBN3qrqn7iLbbl5)c z4>pO|=jO;OIJtHlN^UhFM^jSSj$yKz?5*l9*;1)C68LUm5^GMJ2u#suy1B-nFe{j@n<;I0-WOj>5H z*60tTNm5-TxFr1w;1Fvck3Y18HmmM?tqk4BM!T`iU}Du0ye-1 z{&3=|wrw=FZ5!Wy-kEP^|4gzo=iFH9(z4N}jVE_!yhq8$jFXv;JrQdb@i*i)QY>uGd#SBF|ezP?uWA^=FU!+RXS)tx+7gvPsU) zr}5b&-TxN3SKW>$+!VO$Fa2qI6?UjWC~RcFrJSq!(>A>75d4UsauDZ9_KO{7|7L_o z#>QfJ4tRbWW#hip^3h(P@@(y3ll_6bCmywNFLG<}{)5YCa$;NM!u{|)3*iB*#u^*O zibDq7BR4c2%9bCYjv5`2@Eroz#j{}2=Do*Npxaf~%*>{JSJNJOKNv2CYD$anr#2)4 z?y~VR>;x?}U9WEFWP+DBT(ftIzXl6TLwgM+gTs{Me(i+4G~y~Wu--$7ry$b}ys zYb+Oh0>mFeJJ>|^+iro$s>eX_b38Jb|LVP`G+br(p~-^~A8Bk%W^|+D>zk9C5#r)qvsN%3+6D~e zG~(wS5(%cm%YA;ALVl|%L(^hU(}<*Y#H8TISLKs6@C08_ebngMxA2Dqy`?WMn0nzq zub)?@J^HzcKhm6z5`5F;-?|sXg3@KQDwi&5PCBD|ZPQVb;pcB$vG<8EV$aEg&WR;F z^MjiAq%SAjtJn~_b)ePRuoasMqY`RTe=1oMxwE5+fDqsy%cNexN9@8vO8I2ziM&S! zg^=OK_{;@}0SBTA={a*&o+RKFw+yjz*!keO-W*BWK3E^K}%yLtunp5f_yCte?^&q{j220FPaljHzlS&mp^0!HU?7YF zb(Ad8mx)Mr+ zKw`}61*?+rJxte<#BFZnW^O`@3y1CsZV%jejfvb74-+$mnqu9kz#e~Og&O&}< zfckbp_pgCBU2JY?{U=_8;hO$m_g#_L@-7Sb17;-844qKxadjy`b!E|K>AIIB7D&1o zKZ)}wW&>;tA+zC76SFiCy=J?KB|$vHgI%i%%glyJ+SO&R*?a@ut^{}3$DWPd$egM- z)X)eZVrESEH1OoX4aleYAGe7slF@|NirH3265qd>|yWsfweQKxhC!rXg8#9zf|kindmI2*C=Gp5l}fDsMj*_Uc8=fx=* z*^xTosM&r04Kh-hYMRUrod@LNh<~s>xY}lYEr@GbbW5YmZP9M7T87jMg+=3ua_2nr z9bRyolTU3(hOZq=?iDg92}TqB?KFn>#UM4bnshu8Z)I=3bF>P-4SZ_#DPwn>-S$K! z`1GUTgfE-yS&#l-uE|{06DTz7yG=@CoT}wjG+Jq`)lMenCdHBpLiDSt!7ZjK*PM!& zlDg|=_2e^*pq2wylg1U7x4Q+f&EEXg&LEHxB2SVzY2e{Okoh!bY;(CO-1auX`?$~9 z8FG=%74pWp7B<-V&aNP(NPlg`9;Rf{;>-LO6$;EZxg&R@7|`{w)xv18B8R(h*ZNQ- zr0H4Fu%dajXu$zYn*FudXiYpY;3ApB9Hjh!kdFxfc^We~6GPMhaCG2W{G$iJN(csp zQK4!MxbrccS8ZVm3Jb5UN11l|LNNaOUPdRP@ zHtYf3|6rE8eE&hZ9P}JPF~d>*0gYdoadM!Jvuk87rQX#pLOT(Q$XcCne{dQ z9eK%fuNHT|@{hOEb6t0+@S+OGt~$&@G@?Ip%-3xCTDTBnB;!-GsdQRvf+4lf?B#Z`pRSO zcQzNtm*^OXvym1HDKPV)da$`k`XwJqO2O61*h4;s15OZ^<1r@4iq#pQef5ouBwFP0 zq2fcpkIxK8X>w!g0L(~L|X-UVZ$#SaZMQS>+6P8h&)sF5lOj5$E)~BTz@A?|J`V^ z9xV-})rPS^-GFH$e{_9;HXhl6GrOa5^|RZ8wqH?;fzE+(JGO^*kayPe9PgQ$LOq3t z$%4qj5Yfd6f;ym8MkcxIAjX-T&-*$69QOd>2zBU}4fE)_cI1E>lq~}Vw3Uw?ktDFF zL8N?>qBuc^4m>7ocqSaFE=T!^HlqxUS?cuL-7v&Zu-(M(|A=??e+dMvTr^==XYhQI zqS0LVl>dXWi8>$k$WnsF1W`tV4+*9O5iK0}bIa+-CYZjgL2Wy_7wa^@^cVGnK)dEJ zxAFwHiod`Ms1rwH@}Z{(Z1lA_^bred4t~WqgchlmF1EXKTC6~mLX8_KS0I3Zf6(K3 zCUXEFJK;etr`3Z(G=#9zxCUId+vL=;Ev5i?H`@WAH;WO( zaHImOdib^sPe>IX4(mjMu>P2Oc&_#p1qxKsGkLcm{=JpP0aHn0Q9e{X|N%Pm9 zI?nh+2K-cQ_q{>N9g|V8nKriQ~X^22LO-`o$u}^As zaZltIQ_Bfp7)9JYYqi9T{Or2I;-7V6^=EWW#itB(l&yo)b4^UqKJ_Loz{6Cb=)KOw zDkI%6m+~7t>!^@-5_bJQ1eyl$Dcz_;csdM}XDn=NVfvmmFEYkT?66-u?nuaT&V?FR zx-?zg#i@$S2-N3vdPP0vUE?$eWH>>x>FnS~x9VMbUq>9auTEB+(GUk>WJTWjMAknG zpami6B|b*~1+cKgu@!*DK@96gvQ>w8Gvr(_BwCfq7turHDlkS88Y9Cx8TR`8m~h6C zny+Ttcs{NdevOPmoyVvoJh~>16+OdRvP%66!PLA_3)UX|p-!?j=MLp>ac^?636{Dr z50G1HR%(6-12{lY0oXbmv=R%HAz*QWe1T}2tt~wQ0S}xombRDk)?&PsiiQ5-cx55j zq{b8g^_p&y{fbd|k4cSvzC-@R)JTVo-%naGj`W1xEfRI=FF-;VM`ckmUJ^&pT7fWH zQ&m$_bKcRiC7LFRd5P_@Ydu1>Uv%YFFA;wZ2AcF;g%>NNz!e0~^p#pl+a4Tbw3zpU z;+2MNvY4k6?ZfL64zCR_&K(#Gt(`YJBtJ#y5tEV5oG!M zDFt^!E8x7arCF%-VByL@hc=~RlEhKc%t^AuQQ-y0C8m`J&!hsljfa0n4!_G=KnLk^ zFhG}Z=7fLeMi{5G>M$dVLrgp1ic?uzQbc+VBioE%3jD1FbfJsemoL)!jgawd7aODy8St&4dis+8WI=iycUU| zfV)X>(YZW}E0_fO>_~}qW%QR}v)<8iDoVq9!zVVoA6tLA()kexbCDerO zCoY3b)UUhTx!ucssJr!mHLZ&YHIIyNS9t7^=ba>>BAEy? zah{u^vEiS+O#0sD;Vn=&=(1(~RMp`pR7KZz`=U%vE`NcodQlG^$i+vA2{E@c~42 z>WWt!yDhjuzS-HNs3KKuZD9|N!7mJPO2pZDAs_f;Z^UQ&Mae0HB=?$Ll*{}5dwMiL za+FF1(TBBnVfe_$58)e_ai~SQnk#-X_X;l^%@YOl@trfQlCVRMfa7A?5i-N33WPyh zU5$h`jy^|9P|J5K!=)=PA3lV!eh(;nZEDQj8ZkO{QU;V>RN<~n$T+2=Wybda#MU3Q zdIuBfML%d=cB26*Y_-+pt`APVnE;u%8L6_<3te@>FTQryt*GLPUOQt|CGLWHPD|V8 zk@_z)hd>1Hc^d}jngeRQ*=H+XXJpjPbW|bTmBafLv>?)bMy;MvruJa$3~xDzW#Q@j zwN~eyIW?X3^8uSLC3CbTb0w$#6;dm6K-)t`M)r7WEErnpu}25;!PfCW6TDLvocsDw znCbPiW5VxpYYR=|{xLdo6#-Opw|8f>#6Q`qWC=H#ZaBr!KifeQcPe1k_aCd!WOd~W zb+}aJfRJDXW}&1b>x~HE8I;M1YM@D)oDS3H^U-{CMxgd8p-$GMc_9{YeOY;-ar5(2{Zp0oAoiflP={a>0fV^ zE3Y5P_$_Gsw$Z4)pCwB?!);Gs8_O~*G(ah&NG3&t0kbOorgn!wO{1l=$_BRxE#yD2!HA)PUUt0QslL)s0^ z0U|d?$X*I#xF_E0BF|hPZE!D3Rry}){*p!foT&hM;kli`I28!iNoGh7`JK6lPp1PE7MmCsK^b zH#oB!jt^^1Z;ynF)duKdWWhbKx-tSEyQbKk!25!%bT{7Lk^lhLcwK8oZEg=0)A-%73gyyc@Tqg8GClm}Er?S-2&LG}Zn~HT<<+G>2mWJ`E%?01LC>lN4eY5U zbmPcL_&Vm_ZO9j)hmV=N1dMvAaZDySFz#jQU=bc2+{6#ApOHU_oPUvpT>k2#uS@Nf z6a4xk1Enrb-PO&!M)o3RP|LFs^`A#TFvVDtK!NDwQQAMkOL}rjuq6cw8!1wvW3JE_ zh94Z!XwVHKHl!)OR!8cLOv0mAM3)Ddf+(P-J>m_&DHJp((Z@tj2PX1~Gqc~Z(5d13 zCKv-rzR#S;Bw-{LfgYejKYIwxr$n{qQ=i-Cu(xTP)m|$lZ(^DmR2xw2PRzso%GvFO z0W$+m@75$rVRwdIM@=xr{etI}V2RFb25U5+E z0?gQ9>7XrI5KL4|8Nxk*57cKf&$5w{(#l%kR4gzqxZ3M{Wz7PgzcF6JTmu1uCE9jZ z0?biHlW|8RvU`KkBH@#=*jhp(_4*s=)5@X0+ViXV_tZuyCa z(jeh>()9k7r(>xPWnnv#m^=1>R+f`A4%~4?pGes>uG^qL4MB8^SwhT3m#_(s?%Bn# zsF_dp-^E_11Pjiv$+Aw-auvpnn9G1P6|I!0F*ms)(u^_10iF{U0#)SqkyPtiLd2OK z&h;}88tv)gf-`6K8Mrog+k12bKS69>M&}+nEW5ni{+f1%_>P{Vfl4GE?NJG#?U-^r zh3d6UB@hx4LdYzGp@(BJ;u7nlYxL|fg2R~Fr5iS!abP^zzTzyPVbD}AHjiTkAPo(c zxocL^_a5b0aoZS~k5Is2bn&9Dq0tQ}f`FPB3k&OV(IC>&Exm$$lu_u(&K$RoQi%nF zoy-gA(5HD)%<3ERnh3VEs5DN8%}$*JfQ>zp3~~q&quyB&)``EQ)C>m`Z!66c^2R|5 zomUXhKKu~k#hmbh0V<`sj+GdH9FZMx5wZz1aTJ~*WcOayVDTc)Z&Gh*BBTCO+@U}9 zsLkozi-P7NQ{C5%g@agSOWu*MeAXz7TRg8?e;_b_N|H4{ zP-K;2W`l%A^Z~tF7|mjT)bY*)wy#JW0Aa=#Ss{Mjjtuh&X%vS7@+GK!teWw*(>|M0 z%$E-(rrKLBzN2q*d16kT8U$6d9ZJ%Kbj!N~{>OHELJ~L|-E=B&N1wORh+%qQ zR}C0n`vvxWX}i2Dcg#TV5@Z5@8haXoH%9IGXAFf92LeZgW!vtml0kmWpmb}8EbsVH zO}Z!ewDyR&x#N}yO-7jB?@8Aruz9m8pFTl^mHG(#oYA9VXNI^A$%Yml!7O*aCh9=L zZhG|v)-qM_MrzoBr}L-9KsQbIX{Pk+x3}!%ZkeSqhjFgii-u*`dHDI{SDP;ah`muh zL8yM3jDLWF!2s|ef09|}6b2cr3b}BLJyDIavJ8nNoT`z(#QQE>gj%7JZypmA*jI^u z`4Zc4ofO&!`{=#2u0+NcU33qlM&{MQ0KmPJ9|w|9p?rEP0*MuZR2wQVGm|@F4c^1-Wbzu@|!;)l)*TY^YXu8<7%X zCoPQK%_fl&H1Rd}MzugYqFDTcnU&SAx|+q~IySR!XTt>@xGrAqFaA`VB(=M+RG1bj zSuvj99PG^IN>(j3+3m4rJl!{fY!uSUSFlc>-1sLe z>seMa_}zbhktnjBH&2HK0p=y3kssQ#!@wk1-qf&VyElYx6Nx)WQ;-lu=GXBUF#_C`F*#A{^QeqpAr;l1ub9Q`Ddsk;Xkp>1O!N@Es zbKJINXy!uX(S7*uf#&6;6ZCEw3Q^gQ^mS+uTLdp2(mH9P7==r&l)U> z=Z_q8HLxftDHY09uZrb8;eql;_a_#ti&{{ld}feo3Tro&%=(v4E=siOH@CHM)8yS< zIK68R9#GEfp4fJ(RwsL8P%d}z7sHLT-}guJTn@AU%%8bd&{2c{;>|!2(*BH#<$8qJ z*w`sHr23yoXHM>-H(DAR(N^af2i-S(r)#a)fLG%2d7-o8=x*7htiMKH=2WO$dl27v z-A8*qdxd8VPB|f!xhG06?m$uwgT`Jy=gQVl0e)(E!SAKQeXT+7KZ39U?6y$^<2VH^ z>#v7)buwKDgJZ63(o5@&|#5P#iqMCyW14|;OH9y%Kn z{#uu@H%hb(uaL-|13_Q`s&vpk?0O1@JRn2nVfu|Yq!jw&UYHL@IsbJa?;j+|+#6{R zHI9;5I;-oKy7Q7Y$|mG!X_h|rnK+JbH%uXm zw91DY;U3Ir6(v3Q)NxzwJt)v8%jH}2LSQJEQB>&<9dz536Vuzf()BM9g=ZN!J^pdI zBF{Ych{Q+-Pr(>BKx@FF|6R>6SJ&p~{pXc7#DhyF7bkC#o)FS=*1Qcq_v$gX{3H9~ zvDLrfbciv7V!Zj;t3wTbXr6Cdf_uGQe;Ys9YhgF9Nrso292saN#F-&LGnvb&6#r1D zT*$|4$YhC!r~W~bJ+d{oQ4%qGd;oZWusSYbbU=t%F79Y83WJo3c{a?kaj-YR#ME!Ha&lm0x+ciW*45e2!lRqA}KuK=8HbrZ_!GP{cD1(A_r1yxJ{2m;k$0y<-(OibYzoSbbGk4or}|6juz|R z$F0M7U5S@J?Q`|>ggmh>F;EpEN1((z?>!OucN9CoHYds)M$JX5?!gjeAA=IG6c2>%R5`EMDKO&Ltu*AhfirPh?maA_#1+0`y9+}A-A^Mn?8z3wAp)WEb*s&rU#{0%hw{nx`<=!%CRx# zq)(Y2;jTU)wf9N7COR6bXb^7FvA%LJ~&ioKRDD?yZ~`ok|9Qc zF_12kwQ}|aQIwvEi3}D(M)qo6Hb_s`#pTfxW&&)J_-u<{Gz@j7;3dz-Dx@hL@rm9G z8QtH8+nruH@$5oHMU1x=S0NFLeTn1o zwksXFMZdn{I;6!)0>cdG`*0IzKjUC{Azkj9pQG#yGap|V02BZco-P!Kw#)6&p#v?A z8!L_GIB8x3(_uHKOp_@g8vlY2a>$K|UnqnF-!9s*_M$W z|G8#8ZBwXDs734Wx?6_**!8bo8YG3TUpum}9lYeLFm;i?w2Sf_@DvGZ97y%_IwCTy zW=SReN1_Y0p?srS;5@FI_*Du=Rw!`V3=&_WepLsLQ(^dI-4i5shQJ(&2wbdRh|DzG zw{n~I;BY+pNYhtNW*`fj=L?tO5HW74;=gjG30o8SE^lW>7AGP`O zenaX|*c|Fu0@HGQTE-kj|J(G2oBc;uMFaLSgoM_@@SB-IuFRuEp%#^DTl_e0^H(o= zQ=zAKOeJ0N!JrA{!^xu|{?uMELx`u;YnlSha@)_|5IC?6nuw2|6oyiik6Msd8h}q) zh(-$|0Mb;I4}tkv(iXfq&biTN{XSv+_>Hle}y5U-#0`ZLKy3F?Q;cwDTcP2GO+6xY~|7Sr)VKoI^}BC|`%Rh#c5(2}mHM z=szs;vY*Kd4FF6sKVA>AP4WJd2bqGLL}9ipsfxb|e>sn0fSD=i$y0u(G9QwKHewb4 z6@scC-_&@VzS;7(On~|FG4W=E;Nq=b&3fd=@Pjex6tBmAz16D33H}P|_$9wA`d3$| zx$2YSh&$SqEBh>gh@LUH;-p|is^)Irk8o0(uH-27cs+wXQkg7sGWQ3qB*yn>KJ>8W zB2G?OjEup|&K!<(rN4kQWby`pZOb+~PAXb2$8I)c=c+>$kWTvsq_-lshmc7=O^BZ^ zwPF*uet4Or1R2?%^_gHwsB36-ujXsGXrb5x$4DQ=;qQkEqHMWER~P8HsjJB9KjNe>DsIOFrsaH-B&>S)fP4;;@FFBd`*w zJZu{fsNjc*T23sLuhf4%7+CmJ{PgQOjUPQMtB7j6=CJO=WA&F7yd(yU7n~(E{+ArSsg8EWUOgEI7z`3mZ+Vvz zV6w@ute@rAA=Z14*7Y5$HT0$+Dv&R2{^z^>k~of)QBC$gwe->rkPzsX>Fia51e!?*9O4wgAJq_V<(@0|piE?Rr|JQ=5bv~;AU2EK^As-OUb%wIMAktYIDtG5S0Vz{Wet5tE#5VB@7Po9BF+TKL2Xm#IXLzHp` zNwlbHh|-lt1Vh0sMUtObLFqN*S>G5`|6dDWil-wjG57svEvcf$oq3~cF4PT0deH9i?HeKvc->a``-*rE^5z5V4BNjmC*|vVX2h+R z4vRucvq?D8X`BiLY9ChW1=|?R!;dzqC|B>EJuQ7!S>>N4{fsxjCIBC{L>T3QMSQ$! zefc%vnQj{aF#t-3DgugH3sy}if3|l`3KuP@_ z^E~>{5II+f_@IR9IYQ;~BSICZ8qma{HB6urPHdSFRn?(n?;oJ6x$_c|@UwtO5-m8b zl!dMQRpaziS6A~>R?R+(@;Y|h*P&-)pIUx&=7Ce(%5*XxWVVh(qhfpam~Q)wbPIugAqyc>^v>?5#dohNkh3&LpV~r0p!nsH_mIv=Kq_oKh?;5gbGxH-s#djeb z=LV~{tiXPzyrrA1PR_7mllK6c$cPK+SNpbB| zmMi>huLMrAGRmD*!nfFD{dc&g6-tcpzq0RdcRst%p)w*+zxQ`OV-xuNVgW_ISeP4W zI-&8_-{6AAmB(*g7Y;H~!uLA02M#v~_Jmk+LNcM0K>{%g{k(rK*Y8m4$xhc>@j;hc zDBkMxJAx0Z_5cYrDfqz`^yiPVWiC`L^|9-x9XXo*<6~N$bYuw$3BWtRrunQFXx?f9 zbGEB#P)$P+?(2#;l21yIUo)S^Vg)^DVnO{4hv_8*qZuOa8=Id5yNOKf&F2!-m`H?D zYOuaYf*85tdEa0^Uc=P07sB1(31~;cuX_c2A9ntvciMmyn6SW{fFHZL+HjU(jv_Sa z{yFZ!yq407?7j4D3Ov*hBC^a_At_Z{CWvi0E5<5GEecC|=+qkHN=!&7-mE$Bp9pIW zTLhYc*J^F-N(^9Ljj5z#^m1TzAhWARqnWfXt7;&QQ9?7j8jTDcZgX#cFYUf9LJWxtxVz|ZljTn(t^6#Trn1>yP)snHejM})WV9gq;yiA`%NC#4xO`#=5}@s5~_wLkQ4ZqJ>`a^9I> z^8VJ)kN`~|`|g6TXO_wk660rAT zDHy7T37=yfJm>E{$B$a)bth!2zQc34YtaXbE^PS4x+6}Yf%~09lUmpUv;UDIA|Z9# z5}0zuPaf!LVD&JIm8-^9j4kol{~M76Id%6KuCN!EHNh`4jP{I-{QZ41dr?cGc@F(4 zthBQK76|{03nA$)L1zFTT(43_u^--avS;#4LZ@?n!R^BleXHg&1 zcmgb6b2InR{+sAg{)B#gP`47Rle(=R(>i7HVcw?vGfsfAzZ7Va+ITSVUn+AJ85%g8 z%e`WcZMZG8ug1u93X#zpyUPoZE$lw4ct_BQn|b$tMw#G8cKG$}(#fWGJ`=L5VoM8v zJ!8w7b-QZs_Lo|&y(mb9tQ!L?XyB$Y`LPET3@4F#LHR#gg1;#S0OyEYA>`yu)(H=w zj)DKgIF6FLOZg6LiID}ncK5t9dPgk~A4{&l2i(7E^9Le=wkM;ypO(RYe9V~05~}N( zf(74V1tCC~s}KSXhpIq+F1}nEm+wWVExc8C|KwZx2cp4FE;AxV0q)Cr--C~IEjDowmcCcz;iPzUtGVW^;m4oKc*c{R9R5xwg;Uf=64kYHFipf+x_F z?^GjA#WyW>P*Zpn4}p?jyV=z7U%nY6eRS@_OsLJ%5Bf+LdiQy-t3jsjWI7wSP!llN z6s=Xxdvvk($+D@#>qb08Q$IYNjm*$NF1UC3DS6XGRS3Z199cI}c>Q5(N_^oXQYbl} zKtz0cbLDm!8(As0MWENeWV++&apFe58e%;d9t#_05Zi3p5$_pm~8E3}c3L3%f$gVgr2cO4M z>yb*=V>u8z{d-ouXh95nK(Ep{IkYcnST2`c7@21Lgn&DSEhB@2fs;b;pVkxTsH@fE zJjcWl@&S$8e=@iB2!I*m!p;%`UDFFGfPFz5OV74foNR35ND(#ll^O~gk!Pb5_HJ%? zBtDL%@pk6M0g|FzIkuc8IcRJfyOOcth&IZ9kp`AI)4x+4W) zdzo#ncO|)yJNG|@R`8e_cL#&^!dL)vBcS$fsfhcGz@hAO2P`^Z6KLG%&>|h4+Lnv@ zvyuETd>3&PQ%^hmy5k;>`}LT`9C<*iInbTgAdHyz-v|DyHu<8x6is5=V5Ol}YkpCR z9Cb;-jjHYs`p15ePiM`o(1GbVDyN-R>oRqt5W$PiAlU z`F*9|`%@L24AucD=cQ3PI@B?bpB2fZI( zNCGw|S}%I!J+7&C!~(u#DtA3S)f)c?)|&37NUabKb_4CU0zFnY&PpB2>gj2j2UFNq z$H&{n&M;Nx>aCk48?Y6&ufHU`8LGz;{D3J1=LAx-%P9|5-)nII!y_-Ydcn z7XLjZ(IbrQaWU+mU=kpa3+a@OuW*cZ00z6OMd(A@*>)b2&;nm0tYA`CCZe{Tk2S9S zk2?>Do)>Y!c`C2+&re<7Dv#mJho_S#Fqzff3&|ZcHp|K)p?Z@KJsM{oZjb_71m3g_ z2&0Is3saV4>$S*^jsodyRo5#dUNloQ8YSo*Chg}LI5QxPIGQYN+5V(7vDw2h+RmIX zi0})3K@`vdVP4RTT);JjY1JI?D3#~cn1w}8Mv{1&YxA@udgb)(bsYOEgJ?;+vEc*2 zn{AoIKJXLn4L5&a+r&laeyL7lEa(%?*;~i5KBUF>7IFTav-#XgZM?0y5FeH>k`~(C zq=$pv#zas5J*%A6Szmz(ho1yITN2yK^QZ=PVoZ#^D+PjhmTv`g+cqA&uZ_K9R*fCL z@Y10&Xm=*`H9@xe{b6om7|N0=q-3v5+kM%z%7x$kUBb;YiHK9v&<93SvLM%-TZ5*90ETJsp~&a2pRH zso2O$ARIZ!^qd;q0T8MB*+)lVC_Mnoo9v7NU=G3{mO$&m2`<>$OVk#VtGW&=7$6Hn zYGM`tqM;p{GrojL=_Ipxo&TP%8L_i@d(?P&>YS0#fdqi7Bsd0-p-dm#fBVtTy)T7V z-A=N9lyX$di^*j2hW;V;0=R2OS%W6k+7Qjtlj_el$UxZ(=(qmsqY_86wJWWg4!D^Y zan9XZ8Ww<4eQZ*Q&TL3_AN2{%&)Gjt(Zb9FHtTi9kPC*;3fdSAGZM>n(w^+RMX&GS z{gEicE&R3(^t~qg<)O9e!_)fO($rMZ&j!5SsBsTeJ9hvjH^{P*9Iw<~M!?IiK;hFq zBTj9z`@Kk%RJjA-3avakuNZ&7OZ(*cy-rakP z1aO)KFen1B3yyow_vX0bu+fCm||z>EgRqVcpFOuZy_Pxw`vs&L*0{@DRZTu&x^(XrlvFTo!A!F_Gnl?WN zla=$jEyO>XU~B~1T)f&jtm6%SW*z|~6mu5a&BmF&pA(@yPv2oc%pNblyrZHb;|E(?1y?{%5>sJa~#?UR%}w9lkX%IA}-%1X8r>g>R9YuWJR>uRxF? z3NsjutJq{*QQQ9d-lPgbhCmU?ZMNABoya9sl&lzoX_Zq?H!C9Rx z?H3VH!9t|?InFQId1_)mCE}QD@CmPTHmRmr; zVh&If2`D8^Bp>|emAwX8B9%!`>P;euN?rH`A7S z%p*7UP8qBi>6%tUM6V0j@V7kyLLg{QCnDO{#+_!W8vjTz_yA! z7WQO-(YCnkW?EC0qVV`l&3@QJDDaDx5HdtjS)d=AwyXVzz(e>dk%6~EWp8%K;v{k1W4f$|^ZZHCT??_D@KM8#YSDN(S3 z#quyft^TKY117|f_=gYB%kv41<2V-HQ9bHOe!F1rNl>=y-2CiAJtDJ zI|}a>vRx)?)#?`J7~O^1UvOzBjYMy(zOLq5nmc-$^)^?sXu<9NA$Hi6*pJx=)cP-B zr-adxi0m4qLXC@z@!V>-l4uY~2xN?4eX&%!c#Xoio(4-`wF3qMU`&|DSy@xxe_l_(LLv&0{cMpA99mKw8MXPs=(&(Y z`Kx+3F9-eU!kPF$@Hk32h;P=U7f{%qw>NC+HYe410ymS~L6IOw`#4=SBAOX^@l zGMM$z;6R&&oT5ls_4g&3U+ox_xO+Dn8H-JNWoP*gF|?z^Es5Bl&ei6M@=ig#R;$JR zF`$fu`4RhNgqRKzO8t41qA4O2o+C40#b~L~fY)sY5@AXOR3VV(d+Uz}nn-j1!|P$} zQ{kat2%>nF6151IE%%)u@|PnHFu0Kvw&7JbxK z?tLTITiw~K`N(YNjEvnanGoy5PsVLlQ<>MED{YxGE1oMyD*mOL0thS`(7rj#EyUGF9Xm~I_=ecvoMcR%e{o5QmMpbCpW#fE$*Io z`_S8ZofR?@-0jVk^zcY5=f8X{QAoQ)7;9>SCg@Duw7dytx{BC^lhE^ReO;JN3<(N zdEB1TTE`{Rjw}4_fMx81#?V6>Ci~ma;;+$i-7?k+L%S~_`yLo)^&HSJFuW03QpLXV zvtk(?UzhA5u7Gz!lDk>f{{$$r!IGOjg28_iNk+9H0EeqwD5nRob~zOBbbY`e^RiFL z3vc(=>h@b9S+{su7(>D$a~#;m8HKD)HggZJ*TD_2IL^dT%J(i?#da15-W^j{tCCi- zWJc(f9aNT8$ifmmq9 zRHEkK$%BY0f>@L%9>OuJ6osHGHc~J`U50R>ROjYr-A@L-=OJgCxem|$2{5(kOSb5r zD#OtPNAnD4dq0a=*fb1x0cK{OAGCqMA!FCAdL7MUJh9LF_e@wCVM?*6!aD!km`pV_~hgg7n~S*D7Mjtvwf1 z37r~liz(tRXd#(CT~CgZ-IpWCuR4bqcK)7pe98WFm#Uc3M@!iSQ=bL$uYUF*Rb@`ub)P!BT{bY9@D&d8c;w@lqp^v5p&ex z@5!?tyJ!!={F|(k#qYF(eiK5PSgYrf2_=_%LyQmOn?Xyq!8gY@JBUNZlg=`&?4}#o zt_pLPT7!{y6RiAR3dD9hSX80o8q5)9P|HQrqK#xjxZj>^%oqIShi81aVh+5Cfv?St zKN&FLKmD%NNS9510QUg9IO=aQn&0ZgZE*5+W-> zm(~h*IZLX8Am}QAA=aHK{z-&zdrI}fhadZY8;)Fw^sAG!*KG;X(=*i_j}fqA73BwF zXxXITwJr#V{M7A<#ZpgOppP(BcZ}VzMOX~u4&_HzJt-pMRj!I_7##EAw&3fq{HL8) zPZ=8Ur1^w6<~VN3`D`+_4zMwTza_^hZTJ_6wJ-4h8?826&%I54kQZQ_YCmsb1cC}$ z*XCWxiZ+@nN@HUs^K%A+v*$(iNxgM;TiYYinyaW>O9kwVaZPc%>-zK<`6?@9>FdtG>Xk1nL?Z zf(^piK^BuBUVeQI7wgUHEWL&GJ{o%9y~ekx}UWWhRY=v4dCOXi~8F7-9xE2G*tK4E=qLo zWJ_`0=b0HBEE5bnp_ZAJP+H%*Aa1Qde6hozt{}hyr&>8jivLe{^_89^szZc=h*r?z zclp}uSCR4*%&3Ud5k3qb*|YDas`8L5z4o*L3d6nhg4tO0Cmy!av$klphtQTgN)L_? z`EM{`%@+eX{?+Fln6;M%fv7@-G3M>Zk)cgO>nmLLdkoK~Pg;AI+`BT|f=pHi946_* z8<}G7uC8&!N^geyo#Q4^i~(*qQu0nl!U;lltReSy_~trlgDYG!LQm5FN7GqGMfts7 ze}Ex|7Le{Qp;FSFB3%L^NJ@8i4${&sHFS4(C?z$tNQ1=C-TB=9p7mdAUhrlXi($@v z&bjty?`=k8@JZq^S11^t^!D*`p zdk;{bod&Xn>XNnEeSkK>I?|RydMVWbolP+e>5aunb)xYd?7f_LuQ&9atz@5Fl&r79 z$2uyh3VDSGmbjtWG+W1znp!1f&jQ4;B7z47msp%j9&Aug^(YSuo19?+)amugrod_i zOdDu_ms!)4_I$YB{Ge#I&soy6lU~+co}p4#&^L%46ivLyG9G6enTmjvW~;|wsn3nF zu}WwY7r|vL-p~4wbSY|tucah#fwyON(3y!Ac})%A%n{&xz=(T^9?%#P<{{Q2{?e-~G8 zRR;Zkv1={>x~bIE6%aB8C`iES1QLh-MUeqfs353D@%Y-zE_be9;gr=wAB#cE&mM__ z20y_64JhH)MT;-ZfLXxKX@2;YiCh#TZHTl_)iUQb%GYsFoLG{~Q(MP3e$`O3Lp?Yy z7E;EM@V4ues(36$uy?ljcf8BxZ9PJbn;=77`i-PWDn|Bkc@O$jK8Ft z@Q2hG*3t0YcOhLIzXTbM7mfJqV3#dp1&Q)UF-W@{$Hepk?w@5O8LUZcA?|x9mF-{v zj`HvMo~29&dLP=w-H%)Q*$%q5S2rkM123u)9V6M$oAA1D;OQ=j(E2r6zxTy`0f#hm zQX_8PgWpVV8olf6{I`h=S+P|koV$N*Bpi&3bQAm%LRuGz0epC8i)#t_V}EQnSNTPrV=) zIE)a=d==%7G9&QElBa={Z-xgY?)n=eB0B^huu9~5{E~B{8ibN7Mx>}|zTk9UE?ZcG^dD1}h;_c$Y17G}aAib= zVE=#T>AT=5LE&sU23PrJPmvh+_DV_b_T_)s%%z%AKu*lr85enUsFt$rZ?yU#ZJo~_ z$d)(0Fl8NSjGs5S5KeJaGtKf6z@SmDPY%5d-mcuvCCp&)nCp@y_v`oh#vQe4-?k@D z2qY||OPwjpcLvO-IBrw@N|#=h$RWS+M>@41Fln3PMqV|qXnh?}+(ptT2FfChseb2| zdIyBlOl*KI1rxe8C@hzLnb&K7Hke#oXtEQA!$eVNrKaqKsCJT%aJn+_0a-4_bQ~QG zZDMsbRyAOc=JGDgz6*6QmMSDmt|yUzf7KaFp^yuA`OH@-OEk#2H0tTeAE@`f$sqZj zCRRLuerXLeENKwU7`qE&YD^C)I4o3$$i`Cl`(2CYq#~{bauU}4w&2z4p*GbJyHtYS zWneMOiheBIYQ{NeZiu>fyel8#>QMxE?QOLp@yr}ywf`-Zvkrdsul=Q-xd7Xr3Xizxqs(mcbntv*d{5`c3rMXl#tHF6>3^ zW07))3-Sqcs#(GNwlEtu6>x?0a+8G&y4%f0iPxqNI0hF>A>KfncGm@UA8OHh%&^;}uksalc(*@Ye~F zfTem{ECPb)#OOFZ3W8q;@SN6zxx{ORj`XmB|8O2|MgoxEq4ZLXKOfK4ukz4Izmm)- ztpI&}l2USS_UYF&l@=Vixi4XelgL?E`$ASQO8XH_@G)VCAsGJ^5!zc(ZmgRgzF?fbm!I?kyPlrZq8%RV&Hg>QC8=({hxno6u2qZQed+t_)ECmz<@d?N z&F*EfDhx~;e>}3W)an8?6g6@;_kPDau2@JQ6BZ_g@Txaw3SDfpNlls8NRGTe3dli8 zO8oE~-EVvY_vkL~&d$v2J6djc+8#~AQo)w3XicsMx@&VFE$8;2xOu&AE5%i+NW>E! z8rkqEY6UbB!q32?UtteEY+@}aT)VAl5WStr!(p--`}DI!eg7s#L_u1cD2b6Gsu5`V_4n5+4C+8E9?D%va*-J=^L$cEJZ_qm?xeKtY@-~ryV zP%KpF4oxsgMU{c=tx&`a+QH^1$d)P%f6MY4k>TAi3c&!;Gyv`-X#1 z@}U@SbY%67$&u=IUNglHhJgW-VHRA}(6j1Mo3sv8MMg8O9tYVaFU@CSYl^x}a~NHo>L$YmbSltf5mjVubPsTQh}W6RX;EK7JbpR24+g9AFZZoPr}$)nLC6+%8@ z50#Cm;J%$jSl|6FZcSVzDRcD^yy9<4E@e(hPp17rD$pcPg8^Zk(5Pu5*&N5I42wG=as zo!p;yd`o#)4Im2{`85;zClo<++I+CBNLmgV+K()ehkJYZ^*jl&NBCG$=X9q${J%Yi zi>_JgkT+a$>K1_QO;HhPWQ_3$;9i3+y1JfDxV4`L(_Fmc&DkxZy2_KW6;T8l5WzV zpdeAgUL0b~_`TFI?dCp5fKme3cvUek&5um4QjuPB@`zv$e%Q+bJ_1c4#|x}4l9*Uh z-a!5UAe;920STD~hsZf2-0anC`)A23Wyd0>=fcaMSZTfV32srm8s4xmFek`GF}QLG z7?1M`Q~yZcKYrB_jpR%x{fEw|?vm%}on;|T$hh`vtRoeYN?BdGq^@SxJMRG62hysn5HG|^QU1Rbr6c}#8 zR?y9Wu12lTmYG(x9?Dhw?(~Z_Ls#u?B$bWCYsjZvo@=Hx(rSvjtk zG^ps`!5JTkb+eEvP?oMs5yWRk#-=R9)Ys2_SocJeFJOX>bW}=H_!gYZD-iIGdMdGm zO5=K{PnbC_z(CAsO#ia^iz7SI7cAhfwVq-DwMck3r@vLJRL991wOK|@3d?+5Wqc(% zht+WoYI=;dE1@p`NkFWCcNcN<>%|mGfvg~}x8PTJ>c?Y&qR20<4*h&h%HPC`r5*IT zR1(~g*F-oep)m{}z*G5kRT+t1ggG82k2m+Wd=#%;Vv!HhBjBI+41|_3{kgfCr%kJ! zJ}3VTgz4HpX-Z{NtWeu|6m{w6wGI-Ra(3PYC^z zsa|Ejy{er1R5zv2a5sI*`?dqq%!+5`HfQuWW@AU)Hcbg_-j*164Ve-E_OX(={S4#j zu)DCDhPHzJ1RFjq|7aJQ8r=jlPY+c5#BYKgvu=q_+pik`2>eAgB9gj9U56xf=fOfW zjq9F2K04a42P04v2j3s+@*ezYL$MgoTsb-Q6_38A8b5apSbxO8aK?^{*I>q!km=7q zzt0p7{uM4-EGEzRD4@-pe+Uc~M9%(f)EE<^VFJf61DR6IzP&R7F8|PL;45!k8ZMI` zxL?&xZBY!*{al$lYyV|1ChIASJdOw%x!wKv_W@V)k6b!RKXv(5c{g4zp>L)27GG|i z{?6(7u$fyFeRxH5Qt#ji#Nf>gB?5Xy|J9-J+l_IGW{1@nKnt-I;9R;TWei2pY>d`# zci+9QFsMcW9d$nYOJHn1GQ?E6Q8u0AJ z9Lho*Dd2b2pfF&GVu@TwfKMApF4d`OkRfd!pn`PRAYm`dUSdc6>5F4u`t%a?yK5Fk z%z#5e6XDu+FkZ&uWY;@5)kyY6>yr<;EL+4_ouxb@qG1ait0Wf1@dJ4C3OO zC<_YU+F(7Fo^VqOCZW3$F;+=i$rxCA-idlid{^sEUBZvaDvRVc?J{IwGid#W7{BMF z`qdyNPKj?`+;Zys4Oxmww~09$ro_jAI?kkHth$WFcDp{8(_ii$GZL{N2Ynsarp%PD zi&@Sh_N}Q~YPZg)X(Z-qoz35VcC);?vdB`@82aRB|CITyH%6YwKo{71+69n2&*?>O zH@dlTJtc0}xxQt;&U>DH{IwjXk+X!3aLgUPc;8q~p{>dix8m87`{z)e;wYUaR?gsn zeHOi~4C%(cBgak?xvF(S+L$5mAX!J76KnpljL#c2!>Dw8qbVG`ug(bHZ##@$>ltf! z51KmVZe?ymVH&B68!~NJN=sG#&pes=>wmxX6o6|68S-A{q;m1J5Sb>wpf`~OV&wg$ zsx#$@y2^g-&hzg0lZM<-VjJDd;U$$(#ls=;H-=S47j^(=85wdkS3nZU{s?yJ;zeXU z&H4Y8OKAQi>Zy!J%!-rpYv{{Y`YM4n&YwjZUlpg7Q?gR>(=S~QJ?q5%FrwUw_vLh2 z6Neapu(3DEQ;#8k5x|k|Upt&Y%rs#EfJF^D0qD<%H_36)79ZRmoR(9YrdtUJ;>3DU z@jjz*JgQs$R5zhUvKtGAkVUN$oGiR~npMy)8UO8O8H@gM?ub{U#qCpn=^!dOf_%S> z(O)eJ-l#L42^Hvub0QIa8{spjdN^>q{O2V~AeB z5)mVQ3$AM0djnWk83-cO!evUerH55$vnVA*sAUUVzG(^t1y3FmL`YkT$;k{88Bp81 zx?&qXJRtDvjsJMfn*r$VrveaEmf8zEt)bCky*ew$WS8lG2d-+}Cf9tEe5^C(@|y8q z+<}nLH&>zP#ztO%Y7+6fuw0AVgcFbVe*9z@-LyFH^QVZ6EJ_Xs9VdS`^uN`I-(JZd zv1+%SkgB)e6L3DXKa8CJIvUFJZ>p**i(P0dYE=Qt&d-~my^5vC$^1KNvhL?PqY*Yq zy&AZhX=8j?Zu81NH@vnvR??f`mVzh+9}@W65^GCZ=cD5dU7Z1|uw&`yBIEsvwn*xT zI17>|2TP;kH>b6raiIf2C~Pfr#eqqY6DpCif>OYY84-ijj?$2kfv7FD-6*Actyzl% zg$L?^F%%AzEcq<?w6wr9~+4j>iqf!MKmdm(CyVq1`cR-3a^f*TjR#673$8-dn3g zjun2LX!i;f2FhkH3{>+D*gWIdl9dBCQ)P@CYQJ=@{2Ugqx-bukc8`5rHTk7qJ^XI0 z(+Brad^R>P6&uVOI5fF7bab}Le!OhPe-ernxNS@L*nELLpDNmfd`xnHw3uscG@?z0 zRkuDU)J83>R5H5bU+HeG>TXCpFFulY<*D3H5#Y($A?{p8`~9N4dR(=Mj&9s%Ir7V_ z_shrDBUTAk;&)tW((I|zM$v=y&R3ZrAwSvcSE)^eaoUnoea%zE*0&`4NKdYCC+W9>vDbri z83e%Wy=ujy(Ex_RN<}gPYK5%bdgZz}D0~mW|NLaT$X7RCfu`je4GQLJIX?$RthS54 zx%L1ZpYf!ih;|}-$dtAafEr~D4GqIfohW5THZ#VGx?=M4^TP)>6cjq%;;^vO7DBhU z5$ZqDcd79SzX@JT$BD$0@zpH7yj!>%ef6>IWGy!&h5fXMc^{OJFYo#BBXg%yRp8mD z5mGA3kT_%gu-@xM%E1WD((kpuk_@3e%|i|6Hma+#m&_~tQce$DZYa4i8j6Tw7^BeL#rQ01=(5_2~$x z@i;m<`n!mAYM%qArTfcj6bV`J<>qG)5LRv^ek}9w$SfY{+l6OkxToS5;O$O~!z>hU z#{a)vXq%@Z%6P1lGFPxU-^=>R%*&L0yoasba+EJ_DLz=#7`?(v-F0|=V_C-~UG}85 zy_Zg6U=bw{(4@+ro4)ph#&T?@>|Uz5RQI_e&)4`olM`%hB$K@XRs3ObJEAKcnTmtDnjf}dlb z3r37?39Vj4l`Zp8!d^L?3H(iT`e-ptAmrou>w_dTC8!tUq+~AF_O60$mgCc;uM=UZ ztwNyQYt9pHly&T*t`|YLrIZwPjO$u@MmhJKnrAPS#*`)72znjJWsEhcRLu?1{vuFQ zp4|n>1!ObJA1=Kl-duGYuU>!Ibo=j{pS72yZ!_};ua+r@THE0}w1eDFSOw9yh^=>c zACY%*&&R3Q9=M~9LE2VWl?sae`}fd}#tHRxR2d%-ZM!&@w!~GZjwVgD@oA7P!Jx$5 zdrmU*B>FLA^+ojzlj}L|K#7&Pe)ZBM*u@!v^#zR71ulRT9wl5n#NVk}JPn+$;uAgZ zY+;Edq@P?KHQyAPu-rNq{Yz@~JS%06?-Gel7S?*uppVN`LOYYmg=XLbBF^WZJG!z+ zNUR+WD6~}JraEq41zwmsB62zDixL>0{*L%_av|wyQrVo}c$;-2XtC8ew{~60)0CKq zJnR?wg7NRt6HtOdO@cw(sEl<}TN7v)@pez{Z(%k?2DfKFX9#0XJbH6b)l>nT6Qc+khT;ueyZTK>B zYhquyFPSz8f>PB(69(hCe}T6euvc;IMn?T4_Q>wCQnn5m&?SK}-Lr)F+sd*K+tpga zL6|4yCj&3D@Gs1Z^%i&lbelU!;mj8eOn9@$H%g0DbLXw;;?)XQ)5jdYL-0iiL{xv) z`TkD(nDwXqT48hyPHI#7^ls`3SeIK{Td=9C$RDso#zgr8z&{RTGDu+%MOurc9_|me zx@hEHa;33e8MKnLdmMk=_j-km``5z%3g@Q1`%$2(?0-M_P=?H$H5C^ zvYB+L?l`e6ve^iPQNwBWu84(uup2DMNx?+Q__y_pngZtgjZ6W(4zIzORvilRNc9Y^NXJ3&p&$ZD}%dI zw5E=~XMRkKdchC!aStG#f8Ij5>^?BnWiPq}KX%r-HsiQ2r$BrD{-kFLWokQCFKMwy zrUbVMtf3)Gs7z_y#YkkOZASg^Ae2MNVij_Tgs|USXF_;JVke!U$lKkg!re&Yr*U0$ zI4mSF%fHXu6VyISF@~KpSToy{JSY}&>lLPwDWvxqN!7Q_`jiEtC`b)0Qcy3|MGl?H zFEf>WP+1GFPc;RnD}fxy?IQh|jZ`GE2#}`uZr!$_Q9VTZii=Hvi3kuRRpH8 z@{xe%s5yO)pH7sU&aL?0fp^?rs$?N=BRFAj+O4M3LoJPL71cK<1Fw&c z6uj+LXGtI?B8lh+1WfP_Qi*MyX(z$QSheEPSar%)_-~WWsn=t-(GmsZ3_W%ef7t79 z4_%FVx-zVUy;AN-CZs5oIUKWLk#V;~sQ-HlKjz%~L>ZnT16d1a zEGF=4z%6}ixmxgZt<%%ZiwyXN{~m3%Vphq-;JgQ>B)}^|8hNqFu6J}vB;DryrpxUw z!FNoFKlaX?OI8iMX2pDu>Ys_ods{lP&@AXQja%M@;r?5J<)Q`avkGJ=C@5`N2ot@9 zf8u?7e7xG&`RCprYZLeeUialEA>*sLbLF#TH&?xQCa$*F_f;!+s@_0U_0JK*(+z>M ziFJG+cDKFjir03r7KNpJIKCCABe57!9T#KxtymsG51pm#VEJ=};p6>mOmKp_!dB+_pj9Ww z>JLN#)t@m78VUYGf)W2A_99UP$h-ElrVA^PN!$!Wq(1g$Df$W^f2Ji2VIg9Zh z7T)g47q}Nph%8EXAHcgboq-)osr374b#nX?OuCL^cic8GIkwv9uLtwn2gP3PyYJ+l zZWKC%0|7pit^3aDhtyt&;nu~>ZpH$hZ({22ETzLGNwi?cEPMo5)KS44C92;GXY<;s zZnKQ$wJl2h}AH4m(8tOK3pkMKL zdZ@FLHk3sW^x_v2{`*lPj?w|i|CE1wJ@DWwJ;~p}O^lTB@vW_cT{B4eT*$07%4ad{ zOn`7Nj@Mxf*83Hk7tS&nt1Rll^Y5dIAx8a2IYb*}+GhfD7 z=;_U#HRF6wBefq% zVG|U9Q)gMuZ5Ko5(lPcMO4J}DVh5W@S9&sH)pIUI};D@}Zz zNazM=nHz+|Rsx)@sOVL##3}fs6g2OEAiC%C`(tcv7g${MSk{3bAJ-@II7(%yKgfs9 z6o-X;>ZygU9jz~~bFi7?Y_JEK1at#^2vQN56Tzi0x$`iA#}}(j$|0AJJJ>a!Wi`EODI!*wtapI z`eZ0xZzy*)$m*etdmFp?!K>kG$hE!N-coNm_ZuC3({9t%`He}?Q%B(yvXE56e$oLXb_29xhMb0v&dqZA?O8nTBkjM$2mV?j`+<)&`qWIBb%oihmp*y-JH^k z+|k1)aE*6E+4$Vgdt!Q>cAPYDXn>dYrMPO|nKYxX-}M4O!7QamHh1i?(Y<|t&%=1k zBoO;XXV9><-;zHn~e?d1uj&7;d6a-sP-++O=C<=T&Z}Pl!6NNMBwbUOD=^*&7>@e z0Jd*@56==J6w3MYvt;2&^ZX&*rfOH`oYK>rp!eTVkUG}H&QAGKnNE$p>WiZze%0bY zpiR{e;Kp{klOG%0bm>9!tlP0pvJxI2&*`cDx6JSUt*~(9)|S~nrddP7B3_ECz;&(Z zoa>{*<7qy#Guil4EW#p=Xx?n z=5^fN>r(`cKrT5ZVx=m2C$iT8>xHNlkH@@)MftnQTfzsv5yGz_FHi_fAmI-#Bsm#P zIw_9?mH|f#E|@u~|MX9e@GQ>x^9f%DpJ_W}u{cGoUga7f?w{#MDq(8E3&DZHet~*jig}hGmCq46V@DmMr)F zI7C|=b5<`ZC8*9Pys6Sfo*ndN*oicr-0ei~L*KBbX9G{A7JsBE0;??HNzUoH-bTS! zciQ?fIT>B9zeddq$x}Kcwu(OcoRv*;wc5D!EJ6o{&=d?`xrtw09{3(NVsW}3^N=rd zw-S>ChYBe3l z6Y>Vh!J_89HeuXD%wdV%Uzu40w_@4D;Gg&*PhMmwVG&Zu_RAr<&b7~bk5n=E?7XqY zlU~joF8b`T#m5?@L}#}hDGkht@?+KsCLw=^1Vn&8_xpPJ0C*+#@G7aF0+Ni3IL!L#HHy%<4jmy3q~e2z87doSNal_)h4f zgs51y535_bzPv1bzSLs5vPx7bhSg|=~Z^L_{)&~1QBIkoEYP2UfN<9U>Z z%l?SR2m);9-&7csL`mz}u{3=#z_P6DSj(kYI(>K8Vv8^Xgie}womzt_r<~GcA>;tA zJt3+P^8#}=cb`63V)1Z$(>w&xQnm!ros2$QDiPI-~;2Rg;DOCP<^ikT8?sw*Q=WO0)V}M?B z;6b7cKt=vLj3vBR$Q)0qob$~4`0CN+wb7(#QOAn|QHF2VM4W%9`xy7_0ZmrarwSwR zN)h_lYBK&BDyDxv?_yk_eC1o;^dSzO%AJbnp7hzi_$|Mh*96IFBn4f}z=`M?3&k)D zy3_QHWybi(gM^iLDwAF!`{W)H^zNgy$DG-KZ(8bC#POmNal(rvd}^b$#X_|Wv1@&? zBYuFJo~|net_|UC4b6g!!M1WOZX`plJT+&;A_Y~-sMxidwbhh*(NBi?CBDU9_dLoa zh0~(D>@;VyEU3*vpQqqIytYm;Q^Xd`jCk+D{9oOz>huyX%<^R(3*-iKP#Lu@vi!M> zMm~6|gOkZ7x17=ZiddJ%Hg|&Zol-FL;k6M`${AyxikSlz}Tf*yNtErHEHVOjzgD%HdU zqT)+6-w=fzdchL%gsW5znwWmO-g2hTz+%~0S&dv>iGXX-uB(vi_?f@VTzrtHi%z1X zL-XQ7yeuM!QWP8mX>oB>8ab(fjMPaDlL+A{d@%bmcx6X5D07&%={O?>`xV#YQ`6yG z)1a*`ROiYRG(S(qtA~Mv5vWi|-~|7AZ|iPO4ytiMzySo>dcb!%;Jgv<7uoOZT#g+OH8mguC6!B|Lc#$$9+baEu%QNN#o<8~mU1B*em2Ukv! z;+)&SF|kWJFPJBeeEBFo z74QZ0c>jf`cK5~WF9YluAQ5XFT!vd`CM@((j1c(`3P&XfVv+P=C4_@hiuaHPM_d2?o_S>`# z3)1fYDbiyz$K0iMerBu`sd zdtI(U1+ht&X_S4`52I6@xrN77gf$2)tw5JOmRfD>xSrUkCyasVjX3qa1gFh#T1OS9 z9g&sAQFWEmIX&uelS_F5)y4l_KmL!gL;L5jaA=-9vj^S^;Af?p3ZD$ue zm?4JGKR)!}7Qm%}X?mPOA>b8N1H!)e`NN8{#R7c|N;wJ;k{F7HK|hUmKMSu{sPGLUTiK3$b*?SxdJX?XxE@^Z}xXR zdc$w6kaswZ9;i0H2VDF1RP`E7K6-^RR>H=|6`MF0u}7#I9~ke~>k-%zsmk3A>)+?igmvJ;=e(=VX5pyp`~?XMrlN0qX5SB5#(deWw^n znZ;4i(#@4Xf^LGG5`$VmPSuZi+v``N1J$ZTCid(ov?-C2O^h8WVcKtq(N?g16<@ni zZB`9Ru4U=09{M4G0zwz(8)BLcC|%{^6)M0q1P|2?{mQ0@mO&3+BlPMdqlr54H03U@ zx5w1w*t<(#O}nR$OfQ(ez7}^B7rWWTD;owa{n#wmuSh~=v7<|g$i3C(_RYRKP&)jc zP#g0w82{_wQHb3spEPS>UG=8SDw><4qrXFoQg3g|hC7E}`*bE!E6%!04b+km# z9Z;<`W0JMcqtpQF@oP$=@j!DZL z^$#ym0US?nZZ;RQh>-vSpriDFvh{#m5Wa@nVqlJ}XrL7xlWj<-4#ro+1;3$$_e^^x zC^7NM6;F_S%ko$p)&W|e|I6}MSNOLh1I2Z8vdD)H`@bvizUx*JmnDDGV*0LC%|Bx| zOv&OKU=EMsX3k!59CN8(&fqm3MoKPlG&YtJ9{Sc#UywvfM)u=q#Qm>J_OnQpm?P+2 z(7PumTJV(NzX4+ZcmR9DWBbtqIcOH|dCq5ra2^dAu{4hYqPgoa{M#IW@ZjVU!lVNm zc#P}^%0h)}oRx(z*1QFWHXCD(w~G~ze(?=jCral!u8aV$!@YYN`!`s)02ivNx3^R z{lklo)#oFzXZ|p)UF;vy+u0YU)x)8r&JajL4q>m8jCzpExi7E)Slga03X=zy(%j>o zhkVt-7M8xlOepBU+gwE`?~!euTS7Vu8va#JM6n)PN&=to?+d0sIC)(Jq*z{-n zkb%HH31m1Kdbj(1HPQ}vUIH{6sAAO0uGkj&QvJGkL-7S5Ip=(;mvrOOPfD1 z)vncMC9)Wue+Ff8nD5A-khrQ38M9t@E!)3m9wkI3Buo@=x#Ize!@5QUb1M#SeY-5i&aDfha)11Yj$MC2oJ<-+ z$qyXFW_i$Kge#*`sAJC0tQ{L_~02lrqAtJT;R-8oZcQ-aJt^dRasU2 z@X~v=trxCV(VEh9ZzRG)eBPR8PkMpaw&iyGYLEPVT5vX7H0N3N?!yBjZ-q~AOP-gR zE|7GsQkt2(!<1xBz!Lo@>wO`YgZLkrmvl|G^OzDJhdot57sibkElB(CRyyTMW32`p z5(5MvyYi-2=5hWOM62LI$+gJlD~aKFeILX> zhKc4e9MS1KGC^>C`eaVK{%&iXRo-cSG#e(7*gjM8h1Xa zS##kA6Ljn@_5RUvD+edUo*qCIWdkmUc!M|gmE5N zRBIJ=tre}<{TV24yh!D88gIqC?9x{x@I#Rr9{jRl^8$o`y)= z8w&W6qOiW!lfvkSb+4hO+ZCHcBwV8O#Q!JT@YMv!74zJ$?taP5%?;chZES2>5M4f+ z*(B=BxdnA8LfPSlyX(J6mcKosUzymt0{w2K;)ruEwrRL2*FVxVS8bvUX}b3PHBPte z4nJ~+X#+M{AdT@9($1!hCUW`bO292-CDGo}f-!zx)6(*fJ+y8to3~U~22IRG{axSn z-@K-<$jhIxn0fLPsY*uC%cF_x=ea4`C^X5lBa{H-e!1dl0JPWGtN4r)vta~e3z__9 zEv3S^;dviJBTlV;b#e_{IDxoo0Am6A^T2oa_ncyUXvOQ6&>`hZgkx(D5bq$9pSyLv ztAqw1ycQ~A(Lv~V^oc`jqiNh!i}Wl+m{gd-W9ez6G1jm|4Io%`T{zaCM7Y_ZfAdDb zzU9d~Q^*YyFW?-*|6r>cnnVB`qw#zn#Z0HjG=~|18@QgwvfICcKe5{(RbzX1G$0*y z_4cawZmDS6s`3UX@cg_0{~@R!8`gD-1M0CcxSzJpv=IQ#b5_5X6m+sUw;3ErFougE z!3jN9vsnBlM<0dsq*i~3@v1s)4Fn2iM{+tk#4h|=g~{GxZ9zF@Cz@$NJb_ocDlZ2Z zeYVHI2R*>;5WQbXLWS>8m?t$g6-EJiTv`xBZDZwy;Pk^Xew!o+-WoDb0^q&5a^hkF z9c`og9QjoHBNP#iP`qN)(L62-MHkK`EwVRao*bl(p?{b&z(Nz%H$D`JDKAe`K^uT& z*saGvOH5sIkrpipYj<*$&TLmZjWHkfu>5)iQHR(w4vFbfs6Vgccxx;I|y zxx*Yl%Yi<9ydY@c98ON>ADQ+UnGSrsg{craJkdBo6Y5%OubIH})Ykp`qPe9*<0wo0 zrd75?VI(BuOQ2L2*@FS6kwEaaOVY#IbfrL1s&l#f8G83_y;gatc1|jr7uDhersx4pR1`tcFUHuNb%}j@UO{@H4MP=m|J3E~6^76Zm7e0pqxay@u>0?Fy zO(zw5uJB!hZP54TM1;Jr{d)En{q4bw!~#)xk3gqtIBiO1!p})hY?Mrj;yS_nfs=>mTNR^~ z!?ZUzMz}l9&3zq;s>T<*`LFz{z~2N(@Z(aj7py$8RrTwRJKqOh{eN_XWKe*1{06Q_ zi~UR4=Qql~U2z_>z@I9?+MFs4K0i40EEEBl$)V^;u}O4=z-;}UH-XEsmJ_0IJVi~~ zBecf5`A30*Eo-1U3AE70`1-SZevBpjvj_K2_)i+Bez1%&dj*U!b#v@t6wRCbt;Zo=%64#FD~ z)9vH$w&7G{okAfPlwU}ugYipvZFXcWQY;hZ`G6a8{k+<5`*g>1p#gJZOWX$K95@*x zVU&l-?AZ7RTWYW?M1t=93F5l^#*&hrWkS#PO)*jZ%vF*C?lu1iz7%+EsUx;sn}cte zy7{uNhVv!EfFd^2n#kc=Wp#i}NtbhyZ5H>$w#SE&^g2Bl20L|Zm@!-sx^2s>FH8s} z0OcKrO9Fl)p!4f3un1f$UD$p2_p`=ff&Ff6&hKAu}-lu1*(vC;>nzaWuf*?mBqXTgb$*nzQ+Y$Lj-YlMCJD>qC?G>cm3Tc(JjVd{XiNhFYR{P_1M1RTvWCX z{4OJk7k#Yn+jQeP9nz*Ptt_VeDcgutt@eycKCtxx{QbsY?8X3v!;FfQy0jc%A>j!I z7UF9fVEifIx`Pkg9Dxe!7)rsfiw@mXup~!vNxSoqTQ9k{w5-t2jo+*bzuva$032kl zI(u&u(D3Mi8{vulZ%fOtZ`&nP#U3m?1OeF(#Q)s7@p3zTj$37zt$&*QgaMYiuqcKt zLuDg`!xJe++z%7LOTG*8zg)NiEL$H~iBJC}d4p=&+Q{#+gxf%jTdGWncLGX|DpwKR zPc};zEX`{(Rf%OC4(l^P+RMZ>x+ zL?yDTlx%2O=}@kGsrV)vfJH?9cI;sin`uSxA6Ge*O!~a+gfr!nj$~aCxT8)_Covse z%D0fdkk*#-r!po`=&NWkL|}YkXzOFq#(#a`C`k9zNbor~Ts<&L@vycWJ1uYSrfG8Z zClFGBgG&CVqrl$^SY!Q6b)V3NEPi2)nY6e%^PgEN8f|T`?DH$pXOhNrnOGzHEQKc5 zO?d3EQPi=0?6BhJXvc1v zL|SV1OP z9UBCP;DfxJ3!F~~P%SaEU7#uwDIAK~yASFap4h^*yD5cxktmo-z_m;6b>m&xvi-k~ zb0AxNdV0D|A2@LVhpJM|vR<8m*yK4uWyxrH{T|)-%lIeDU4Fv<$bDc7$`bb2bGts64+8FT zK>CtjmTYL)q@@6W6c3=UT$2Dt%6}eD)yEmtQc4sY^i|)Tw?NkQKMPEs$trO8{okeV zY7;_~O5(I=*9^d0;u{g`Q@c5uBfv97Mpm}5p%dtu9?F+Jotb6^3}aD%4r&DK>VAG+ zn)|~L0X{x}T_N5!cT6yA>pp7R2Z8WJPC&RcVo@O>s9C{Pi+>V_OfR&JWBMNi^ym8L z?w;7fg^Qi7#5Af354&7DEszCrT$Yld-1{XRg8na4&XKnz&~2*3?`cI-BEJBDe9cYm zdkn>%L<5OUIW5>}!lT+B^O25!*fB0G|Dg`fgkm&-X%Lb#VBs)-+eK$WiyqK zA6&@jQ+GWGDw@53;`2x5C{d8r*IJTs^z9g!|H;Cqy^?X5iV3>eI3%aW{tzG%)IPtB zC6XRDMGLlx`QP=H`m>`5K!c~^8n6RyW^E6FX)qB zQ{gP5QjCm+GcZ#~kKTY}heI|hgp}bJ-vQhas}|a~kS0+?@ovRVO0IisbW~dCri2{I z_(%!njrhGw+~?*!0U1TsdNMcj3#08WeqWp~@kk_a;LI_G3a2%PZ6M*E)Rh~A0+`+e zp8)r0bdcb=kH!v4^QLqR!C{Ey6mnOC<88$h8KtGi~xg;t*KANFRW}1&WI(T)} zE!h2A`|V!}Q>QETIAz5bcDpu>3j$#Us4C!;+S_A;Zdw9R%p4|k)C6m=3pag$ z{+rT(atV6Wnl7gGYplj74ZIKQNuR03R)HAHzw7ILmDz{t035$-y6O!M+oiH~djFeL z|7WR2e$DV!4ng6s*1xMN@->-2@f1L{*@Ta6ZqieF?dF4po)Ky@J1;LW!fp~NHtN^I zx~DndX>6&mBw)Mwow{E3npLh;v&Sq>^!cqXz|W^;Wb`auEk@-&-0-c_QQeD=HK-%| z-WEFLWQlF2S@ULoXe-ON&Sj8}zh-$0SKgY8z~s0fWSE=P zG-si~4KJHttO-zlrAwI4mb{b!lhax6tB{Zv%+b6}lNoyd?Iw!!b^lbMz# zov#WxSu~#u4-1n*TN*HNq6t{6v*Hmie#0v%fKK-Wq2)czlC7kc2?1*8fw zV<$uY0Aw4)-K&@boez14#J7$jAD1XCK4;OUO^i~yXrIlQXgus(nE#&_;7_Fl9j4>p zLfisVfw~PB)_P z9nS`yqJPu(wi=s)z?-wqie z{;S*P2s$^L=;&2!yLP`&HpIy#wk;#I&+ctk@bx;1xWvwQGyD5qVlCdj|Jz^SQ0Gv& z)7G5!u*e;llfJlr(Sp3o?de7?r|^|=I!R&ivr=)I3^gQ$yJqLxLP3{1VL9cTkAIKH z-Xx>1FvS;joD)6Dbtxz6{Jq%5VdmyIe|v4weEeQFmmcYPuNm3fKc1br;CG;9g9);Yq3~qsZZjcsz0aK0NetwbsB88|Ffc$KrFcy~`ShFW0k5+C zay}N(6|vysT>1|y4(~A$($jB;PBh%X%6{Enb8j-r(XTHf+P|)VJlgty{MPl)In$$& zzavhDs&cv^L?aS|6pdxvNB^()z?mtVuNq*tkbl-!rNw64oA`*wkAjP^V=0O>f!0gJ zu&6k0FWBUv%g-84MM~O5b>B_3YIy9WJzyePMyi){Gy&!-uy`|k^7Zl(oD2iqj2#`K z?Q!60vgIvzp;TQdG^3pp5tAb?!NJDf+q4=U9tIcGD?)USWABynGqiZ0oLs(0j?SmY zJFRVJvCl?;$&t=wb$$sIm&aLXI7Ea1_-X~e<0?dWY$9J6y#HRM4rdsmBq5l%xe@6% z+5^75pX|3oaS-8buv_@owg~=h@MhJk2xEw&JZw1v#25*L4-NC64Fbmxml&vmK`2y$ zL36;Z1A=XZs;^dmWnv>B=;#^rl_oMsnV~=q4>&r(wgl91JU#evegx;jFy7ea^Hj?@dyE)12OkI0ZQ(G(G;UO|cluD{V9?_lRH!#h0(0(^-g z$Gj&OoV=?%BpHVO!|lT1L!C_Q6s8f=(El!*C=fQ)Yz1{q`G5asIMcK-DvVN?!eD8D z_&N9die20*;V$m^+-gt&m zxft3n9KM?dbL{T(|CXd8TOzUNeh!`}Qb32Z$SrXtoFdvzv_KG~Z{Ff5_^h>A`<``b z%Xkv;dy$*l^?%1VBCOEYR*atuSOoidTo$8i?TvjX;^B9`Hu>MS4(;b;T4#TClwW@I zyz|0FYZqB|g^kNVwl;+Q(vGQAnO|C@Zr2POh^i=O?)|~91;h=} ze@rrbNh8i>W$Jhx44P_(6;a{;`Ut$l2pJT_v%elq@)HeUC9BjjL~+XgCd}Bx?;gG8%Gut zoJIcRS#&+YJ-RL)gNR3npiGKLFn4b7cd;T53CLd!evajL878f|_$ zC;Fv-thP-*r^bBH>vER(#hyI3{AWVG1VS`)MARU%E+pwf3D?5b@|JR&DueZhh_-|T zUETHoh>Wx2-FaylAV+d?b=)o88K$Ci-&hAWUm05Ojl0JQ>|m30wY?}f@Q5nB^uhY( z&r0L2fIV3MW-@NF#$$|jT(VB^Uch@ouh~GhUhfdHsqh>Yt3=)lG_Pv=CBgJg%H-ik zS;)eS%@X8BJ;>gwcew%5Zom>1{#FsHJ_I|iQ}T>Z!*X@Zb1NWfR@{a?qaRZ!7%G2M zi0Ddn7FN5NQ3!PL7S(K^9kI-P!K7AOH&lZwuPxDxtk%QWh8eIbZumgCUwiPZTH{7T zo)nEeJKwXG@rrS{Tn+V^H0!6md*!Wp?306snYc-$50?ST-W@T2I++u_DG^#I7PM1r zi1Y_u)&}ZrLSVGv8<2*#QIRDNmxDqfxr}r!cbp%1vz4|MI%KC4fN*h;O?s ztf8q9ZlK8s0UJSqX%(4TO8;Yk3UzMEH;a~yPxoYdmcuMN4wvl`MO&^hY==t(HaGxk}ev3-kC z(GaQF)kiLtJmF@PX+m*FAulaW#Bc_TMdS93Fa#$}m(1HwoavySh?JC6Tt!7CJUAfD z!rEx>%(_+95G9g2&s&nnU*jjVurkyuJJsw^t2hLme^9d`qP(&Novr;G|6h+-`mOOo zgQiO(GX}7@fHg%gSipUA35N3=lz@=H_2X41+tn67Hb@D8RtXjZkF`*+`P2DfiB+qc zFf5;cW?~|0{Td|2DtIjKWuoU|L#VB(L+hTB|1Wm}Y1HC6QP=$IQCiK>@rjQi9pthEI=%ny-@s zG>8HSV_p0P0~@458h(&sgjAQ;j{-lz;LQja;P4%p9|!NQ4SV!V-o+Y zI`948wXR^tTa4edPT=H@JTBSxyTU!IlZYr3W(z3cs7y^&C4KYOa3{tT5g%bD$V^fy z(iA>JwK67awoRH03av;*`uYBtp`I(T3Ffl`^UrYOR6s1h(Ld|(YIXJtjQ_-WN?Xn* z%Q|~`<1hki^rTKjyL5P?VH9uTSjh}yoqbHBlwziWxzZrZ2eDDC3iq}B@F*z~lHL`~ zE@m>G@u&&(@F#~Cz>2p6B@~ikxX_Wmdn$ZY^XO+h8!zng;-jvbUC)u(T*;KXe5oeI zQx@};qvpP=0+xMN;O9Of%JrTy?C9-LK*>S!GfH)<7uioUFJ&x*c2 zn8db_a1+*#W|M98ph3j_p~?>*vD!xS9j1LnU{0rhMlN1{A^Y;m&+YSRU^&mxw|5?6 zaoZsSE)$qo%5w>{k=B#YbddEmW-NDvyLcE{Gt_gx)A_8A)MjE8Ze8cc-$UbL_#S*^ z1DIOo#WZCfeu{XrtG}Y|PD9)g?3>M=;0g6#>gAbyW;Wa6_8RDcwEhb5tD*G^4OkQ0 zFlNy}3G}I=F~yg#g$4^!umP8GaB#?DPMaSqZGXiiQ8A%>Sl&QU-kNh1f)zNzu zwC5Vp!pg1UR#p;_v#sH;S~X|axD@7n)GRkmjT62=CG-gsN9?}ZHuFTZm$}8uwxx0- z$NLCsh6!q$aQ>Y!;m7NZe_JlS3SwkrWOmT`fwc4?myp%y6(Ku7Vh#?znX8&V{FK+e z`yR}F;DnNrlZV(X%z_shP&MCv`(nQ3X4ua)CLHXxKRvjySYsxar=%60s)ruu5^K(h zHo={0KE^}5g!9gfi5~}}SF2pB&GDL@@;`FKz4)*{@$W_UidoCXa4tMXP$Mm z%0hWnm>ywf%IP;!8p?Kg?wT7O!VVsFwzOccsbg|5(aB2`hlu!3Ht?QvlA`WzllPXi z0vXZ|TY|*L@1?hWpY?@rd{Am&|BAJi)qZ0qv+Bc0j+f!SnM^m^;%S4DUz22dzdfpt zkoRB*FV1u?A z;Mt63^GDLzP<=B^&Cva)gv4srUcQ`camkFP2o>DogZE7W=jrjN9PIqW^Lr1z)DXrR z<|Q4wO7-_BCpgfTfvYJ&@g)F41IIA^{*-;AP=Kn3FVAEu@+Cb25fLjHe_O;ylf1_R zmoZw7oB*wcRYjrG4B)0>DenF#=ol+7a4^!fXi;#ZsgWZF`N}50aqTbAQTr;JJ*p$g z(|4raaFVk0Gx;?V*}LC8(-rzJhTj~*sBzILkSa?JTWSuiy-QbF3|Q@65F@{564#y^ z{+I5_FL9*yKte&j(LK$$)Xnj_= zm2^=IX58o#-vIT{miP5o`m8R#0NrH*M;PD)nscUibrh;Y3+98EF(W58zJ5HfS|(YO zM{}cEJ7!9(dcRX*(VJWLzVG&8|DRrn)HgHG4eF+jns+GA&(B?@*?c5>%Zf;tWo=>*tmzQ45_n6BUN3T!19Foi$Qf z1Q8JtW8)Id*G*f~`cOyV|E@Ozw+`cGX}^rYc8t-*t-hPxBX{|Sm6^r;fQ&$qvdXor z4UD%JxY)y$z#D0mSEj@BZ|+;?Op(+}<25;q5291j+hv%hpGoJd>T4RBd2h4ebD2ZR znF0(^T`v~5`A^sJY{9Rf?04_U4YKaqT&(}yWhvk;y9x0*amsYZvNPiGJ`x#_Cy(_* z=~*BmyPkzuc5=U#&^>|))VgqCI~iq z(qt_c{hs_uOKKqoS|}9N5u%pn6Q2Eg@!xYOhklHGC(CvD?&4zfnm}H zKnBdC2Yt4xP5HO{zHs{RvFAtTC4_btVEV^`N-(U?zY+H)B9((?Q0l%S?-MCVe>va% zz}ZPG06Bl~qOu-)y>?ipp;XVBm(|*)5lSk4`@HIH5XP(w-kRw-*@uqVzN)8@D!J)s z26j2SXRHdvK{WNglb_TnXx7+Q+4*s}HuhZSiR;}72R5@%bd>Wnvh^ppvpg=e+FGn| zWN&ZHb$QV3;y8#s6xjhsn{(EBMRmVYBe!5t2Vr?~hk5wRL$u_dH{Zx{R}-9LxzZKQ zBmAyO3*L6BwU(@{?A5FFu1z%*nrSSwlj&3b)SR&5&(6y^a1u)Fgv9bb$_O+ zpQQ6z257I`nWGSoB9*rm0R1Bc*ls^bjG}=u^7g0y<;%o3ZP)8j_*q8&4;QoGPTTZ_ zTw9zxk3m+>7DW0*s#h_sOB6P2&I!HradcFDC!LqEGbVgzYqcP`$OU}c+rc=Pn0e|I zL+FUyj&VOhizs9y_3ia)44)h1JT%mE(SHT!WEF<9Dt3R$w(&=SO{&=d*$nfz#fNoVV5ve$7Q56xsI#emE@1Zp+8`|BYanvbD~qfdZaK)7PTaf zbI*J1nqDny5WZku?L&&1C?U}U{AN%`(!p5YH16%42?OS2?53RYfZUF*4o>j=i zv&Bd%nsV7+&Z!&U=2ELYKX73-oOVdae(q$#Y$n5; zM{B+zaWrjM7I~FHH(k`)(8E7)T=85EOI$8fW5M7en}~}(2v!MIMd5umr~0w-=W$n0 z^z0ZSRED!IHYK_p#nVXoTA@&s&i_8pJqiKH4c$7#wE#x08yGY|w(&u43WoQo(pqmR@ILeTHubU}Yai(Kx{4b`o!5u8L*Hmu1 zmjIrd@yfl@?8l3Ai}(8392_xjVQc@C5kEgsNXV4W8E6QlKtiMjPdD&g29jsi5q~T* zi>#uzXf94tIGvn&QgFayZvZ#?_wpiHwvw4vvf#=il|wS7)kEa%-hRd0P3F9d=_;c* zqnl)-Zm@VW{5z6o77=M0AsBn&H+iPIka(FnXnxY@f7f_D+ba7dsBtBTPuSd?)N*_$0t-aZpoH$0ze8DHTSW7r8n1q z4FgbUKG<2)lrsFRFV1`>I!9yoqKiBNaemEnxORI;M>f`loGMG@aU~HF&|$PMb(fUu zP-$O&^Yuz}!IHY2C+4z-_m|B^IfD7l0fUw6Q`NwS`$`Voo(%oP8D+=c94YRR*zezT z1f8Aj8EoYLQJZucA5_~zwT)o#u9Jhgs&({iD%8ml#Y8|QIy zXpXi8-W|S}R6oIgR5yQs*MZc`k5%vb1p)H+Vv}SYndo$R%UAA)!w@m|b zJxPz(R*$C0Rrm9+b$m?KUR5-DLQaOEgc^QAw5eIGRx(k3z`C?_BBo69d&ljK$Yx&|f|HznG*vd3GMaZ$2r~mgHo- zx&b4-9)msBR?0Vmn6M);`Ror~z_p1S&^ukjUA8})`eJBOjrTq!dY*4FR&75dxpV56 ziAzOXjYWnS*-nFi0JPbZ_c)&H=Y&60Izf`Ha?k>lAiIZ$KNlCX%)8#yWJ~wE8p`oz zCW~igy#Y=z&h_A~HYV{@Ij{t-WqQZMj&SPY>p6}vBcTG79*5=88=^_s28ljSejk*0pg52x(; zT_|U=zzg&b=jJTLzProA-hqJ##X>YOf|5?oU)7eQ&;MPl0Lwzbt%MQqM~6AA-e%@g z_0Aqgc`Wn13Ro=y8aQieF9utiUrPmk4VmthSxx zqOW52pXVx~bT`);Zur2q1qmuPSfaK1;elt3@fB$zS4`OQCQ`?(iZiN@>A&caSF?I^ zYB-i+r~m3zTwGizgk?!i0}L`Yg4IJ%nO;G)!e)A@gL3R_(L4nopNQR>1>VK1YQ>E@J0 z=e9Bz>Il(~?OW^UlP;<=vbr{zS{Kgu|bSgBV@SBRzqrW$2&e+yq*G!<56UdhRlMWZ?u|FGp=K6oPND$j5s4kD;*g zu1v{~Z%A2*2~1LwY|yVQx+ddIlZmjodIQE-`%O9ezHt0aLp%3nMO;6JQq&k=E^bwm z1VcL9gawo3?D&4^?uZcO`QjbG$mJbWqHqOJ94SaXXQR=F;e*<|e*gQqC(;j8f+`CQ zusnGsDD^J`i#V;YFY__e+QaTLG8`qIN7k9;cI1}Ve;d)^rIVC~QpumKqSrS25Mz(# zf;(KFBV%mm5Uj~QvR5k3;F_=UD1oO~N`L7#qXt^NCew?!C?+gb5ALmZiFxD@##q@^ z1Ejihp)3We*jw};{YyG75Q?j)miI})$}t`h=p5Q-OHcXRv z##&`CLK^Xq_v?f|A0|wM%#%Wtf!DF0(i@YE*H)Y<`OAx2U=eEov{kYDUa_QgHH5#H;YeIy6@QezbjQV=X;^U9xE4w}*7fSJzD z&Q47i|LXX0&{r(Xo<3eaqP;+_CbiWLUtwoyNgb{#r*uZDsWd!40TiO3$8zeuaM z4^$`;6_qR|WC?yeiK0%XA-YN(Fv|ZW^UH_!*3e%YF~*r8(9RaB$~r2NHSc*etbQsH z?8d`_D-!R7`IMene=4bmZ-Q^140=Y4-~ms>a-ZaQ@jVhWqy^RZ;~$KPlUtEUb8gC? zVczdRXvonRKX}PDtwiyZ}N~ z2c^_cIl`yMgKQY%3tV<;rZLdLm^a+-KW1HuA79L>zdbUq++W`uOvGO-Rpd>#TabU5 z7>H{Q2LWGcOcJa*tuC3a1Vtoj!r1ReDq-E3&+9^PgD}#NKQLmA;NmgpNlMxLLfQO# z^(~>DyVcf{M^_JL_f`>oU-+Z}1!92Rctl&zie$AM_+!=hq2;0n=CHt-n{iq)SO78N zzt0u-(M;p2f5xKF{L^GQ=d$#p9yOI9d5w5}=icK-D)Diey!kc%CwKE_Qp?;({8oM+ zT6b^?gON3ggjB?^3|y2T-Y&m3hQup$NjqU6zuJEc)^!NEYFr4 z2~J`h;@?fu=BExS2&BP&A+q&C1fhw2+AWI)X(cJ5?gt*6l>v_*C#u`mMs~OR)$zqv zS99}lh=w!!QXr4Jg-(di)Zz15fv z@;pWJQY`Vs*t^%_ByfwTHwz2sc`C#VFi70bG`0_frk`L9_{2on2cFbzhBbk4ScsE3 zF2`ftuSnA#igh=uhI-!<9?*YTy0XJ0%g$H98@8+*bo{ z7V{jl=eI`S2<^qqJaiNV-8q)%T_X(#y4lP?A+^fHHhdL7>v&^M8;5lqNyg94ME*r* zcQvG{x0y#Cr-bIO)Kma?5S&cq_tWci2L4JQb8)q-)M^} z%F2>6ZugJJPok{Nn_nOvGu&AJ-k%&{K7v@V_c}s5_+MW?ib0!56Cs8$V;1JFgMFA8 zCTP2eii#R{GfYvVE8-%K8eBod#_AO>{2hNYIw?2BZj(_Q<{b|8B+V-S-hv*W<GOIyqMF~oe;x7K3u%|IA(cqdI zCj*b)GLQm^+lp5IwYu11O(xTgqS04RlL)wgPNYbW9aX|KLo^luxGB&`7Z;B$7@N!$ zBab#;64R&o?>C%Xxo9CD&dihkmW|)q>AG2fNB9&v1xgNJ zfyb0=E=|^55pogN8%561xU_E_$C~834}wF=x&f}J@sF~p!uoFmjz)jgcG^L_n)NPG zf_YW1RiS^Q{_tN4^}~1lCfjoMplod-_mH}#61)QztJt)Xc1R*un@?KUT_rlz0D~J7Wd_{^jq=ryTFEodyL< z1%?8~K56XM*e&2w<9jRLR3lcu;!A@hpu^F8@PjMMlDOELSV-)LLp9uk;3M>j2Liqg z%4oywu*hkB4ViWKsTl4)c{AY^sqb5=yQY zy8o@5e+WM{1sSGz&)xl*gD(bo*Ds|^`VVmkyTbyQnhSLw3BXN6-rrrSQH-Emz0?RDFHTA3*J0*$~H6_d%B!d~$9jDF=sRU6L@m1imgDI~96-;Ktc<*1go4^4rnK z?J;`1FbZ~w#b9m2B_JU1cx>K57(Vb`M>F_zvsEsb^5tD$v>&=y$zt&Or9e0H`<^a) zVm#YYb@c60`gO(rb~<-=QLZKUmB`#2x>f4%L(s3N#7R0C+L z_AaFDVXFQ1t*b1Q6sLs9Z8V=U0+EJV(s$Cc_d6+(6C}Jkdf1RR5DGW0X#)kX#5+%TUTT)UAJo;d50WTHN%=NU7G!zY6D@EY&=?L3W^k{aa)ER6AXm$l|+qgZNJq zv@v3TD6|HhavNvW0CyJ*qnn1WCgxPZZt{2kNjC8<0&+93f@IAeL4uEshQQ5%-<=D$Oc##K4pmWDQP^DuPssC zU?(U?VA2||Sh5Ge%E3c?f{GyJWGx2AJOuhb#3h#oQfDjWCBkU-_qu-Ukkh&_3kw=WZr9~t7(*iLcbx8gX zCp$@{Y$%%vu_8hrBhqK^*h+u}PZRCR0qB{$o!SnSt-&Qx)dczY2&BqsB_sKGH$`Yw*Xy}v} z_O2SZ*)tQBIgld%D^ljMnFbh@waW#Ee!4he+BgBsGNARWu2KWX>c8DRkWNR&#tPq^ zX=q&@udsCs1jN=_yS^t}lhS&zy}ivYDjEyWvla5BM;9Rt>ds5JBGP4v7 z-Y?!7Q^c%AxNDH6^*#xguN|N(RHdlGu1-gvOp(|A8*pAwd7ptuZd@(!zY(yav(so< zib8fZOL9Sq-sEsiwIx%}Py5+12Ymc#%@3$*&%>Wfp6I}G-U~k|>)Fg5oI z?34GEXc!h5ZzXz3BGS1rVj;} znJ!Fe3`3-_Mu9z?DK$BYRY}d)?<45$-mf+3ULQ%Gh_|`aDDdiy)dj7Ya_72x3b2`^ zH)FDrwn3&kn2Po^beg0hes4JpVjy`!T#Ryh$+yR^)Xum2ww1OqNg6$CGfB51VfkkZwBa&qfX#?=L}Zbd z=8|T-v8R>4y~gA#3(C9Rf|sR9uQvENoD`YFt}ZX%vZpD&d?8LtWQks1G;U)!^P-EE zUEb8$dg%j1z3DC1-;~uUjkU%dUnswCHfztN(totuxbTu+aSzUcr|AS8{@TX)o3F-@ z_fUMP4uh9p>loM!1?5bbq}nc?=Va1*EA{{Leu6X%VgXE(nlys=#7|?n1-+>Buq2Ed zml;p?#j#$+J8BettkHK;6@!}76p-zm9ijQv@qDR3@(Yc`M4iP>Dn`_4zRG)VGdj$Qf9__8{nP!X?9<9Lc#xE7afXA zWrKsq)7}-3dQd1mAizaRN9l7S0W_Z*Q0{h&|4W+Hn>X|4KTA%x1YSacpd)1UzkaHI z@Z^Pb=g7&Vsy_XHvc;*cx$d+smK};<=-Mk3BciZ^N_ zVBJsN*K)Bhw4J<2n=^T7;=kTsplKsXFfYL&9(9+O^~+fe!BOpI%PNG&h>^Y637N6p zo4Vck$o3qJXeXkwjr_zX=+`s?5 zChdC$TxaQ*nB!fU9aI%57GfHC4|dS;{fd^coL2H1_bnuE$~|d{gB$$g)qq(%LMQ&Q zuU_7}(3ibA0&5RXa?V<~-+ z-Ua* z!_Q?tnpckIc^MIXra;?$L!mqcb?2`OL3SDML++5UU#Z6G&f)tRn4X&P?=kJD1YO)R zW$&R$UPN;p^k%>uND_7}6dOp5|{R`P4pT{U+ItnqQm&BAu62R4OnBp+6fDz(G}RZXhG{g5n-W z75dgdR0|U7G%9NC^>3nAz55=WWql1B)hC;31o&M4%gBrRzz;9 z?TYzo`VaXxgBYWWeE`K_D`~(qd{Ik1O`sw_$6e^r{rD7 zOJ3!O2bf*qPpdd5v4*YHAI~hG5mPm|w{1{d-_CTh+Bh0H{Uiz`x387Vr-p7@iJm

t0?1K<(kEcu3HT8J>Sw>` z*oy_05){Gz0a(Oa#}2psceX1XBJ4&iOS>(VitTS+CW2?MLkYV9NCEzeA)^ajOQ{8M z!qsjRsy?s;Rv)BqFmlEFla4u!iLxB+BL#w)#v@rkRehW^^nd}7oS5UtfN`f z{E&~YM9^wjpEe=F^7mf}h1`gv4d?r+nl94#U!h+HqF~A4Oc7TN5jk$JAkkCrJy;o&<@)%Vn}^wZw0H&) zlBDT`GuP$7wC-q-wCY?I%UmwIci8r`roo?unK=K~k*+XR9Afjdt@LLe>8zc%WBL{q z+U&U;-@t*z#R<>}nbs&mc|R}gmHsH{Z%pPYWzMPlw>=VJPth!5XFX!els4-nGIuPG zk8C`&KGizy=~r8X@7FWUdFHcaOw&?Qy{dX+mAO*`#v~z8%J7_tp9(4)t4`QOTy>WE zocA%Y-)^OTrWan)oy7hHgsi9*bOpVlwC|1s#oyvd6y#_msbU-?0Lu9 zcepA)*EGrbc~D|Duyh!hql;vPoF6AXJ!dK3O4Zkh?cA`S{8UT5-e}6C|Gv3AC@_}{ zm${{Yv-7)*tx=d63*SovYrjch3$YY4lZjnRNI3~tdUiHgF8<4$ot&_Po!JNi~T+ODsl!LZCBnizWcN6i0x$eH1*+~j@eHOW{d z9}|d#Z3-mA1o$~gELV9A0o8On;xo{~4s^+SwQ<1H73nlHj#7ll#Td;4UsW3_6!_7a zi>Rb*K||N^R(W4Nq7_}1qCeqVe@VsEAuc=4PVpEV-ovc~?9q#$j}RmnFi6eP(zBAj zJBTLZn;hb)W=LGqU^6dWo+kJoEwkY+ z+}TS(&Ip6>X_a>yk1BehJ{v!Be`>G79&nzzsf=eT4Fm9rJ3mgmWCmbRz?jY`k_kF{ zwE#S?q7Q#nHUd#7)x%_!0sSpuK zoKg7(%3St^_L+?7sML&H#pw@vcI_fJ*!Fs`yZ=sD5mG}%kP8Sy5X)Kk|3m@1QI;Qv zlR_HfE`nagUMtSVds%Lc_9jQxEN83H;|$c$X2F-ni?D;fo{2O?6dlPoI2_!BqqM5X zwNs7uoq}Y{clwlh3ahJIS6|0eqW)N|N!&5by0}p&`_rh!{T0hR^2l?_ECl>2<1q*P z1A)dS!ws=@ATFfyQw@-xDUN;gY)U;$dXG1v&jz~3PcCiGb?y^nm`!&u$%oP@qWLH(}$w4(;Uw0DN!=G zr+j`k_>%Rm&{cYSH((Wx1vGa@6Gv$@m|>E3|1IO7u6Y?KxcB?P0}UhGbmHm+V!3|J zp%ar_njxnP%Bd^f_4f22czJvO8>1#B=%Ca)1}Vm7ij$Om@|5(|e7<~~!^ zHbXMn>7%z8ZeQJtEQFvv3A3yX-KLoD<|JuTb{!E}KbAxG_KbO53}ru%du&Z*Wx#B@ zd+u0$bIphU-hUpXprY#U>kBb=*OK|fv!ac(XnpOSR(e4Ea>cry{)9eHC4bra&G+g8 zp_)f)tUYkAtOG;WPY`PW$q8V1;o##5O}ttP$3%=DQTxb4od;*-VQlwt<*75AI)d(0 zUmb_HyD(HcnL$>eJv1hy$YoPZfBte~_Qu;a0kjgRyd1bH5(U4{=4$IcYw`&7PZjjv zMbOQE*Bvf2qDbCMJvk0-)LjW2JW!!H4%u`f+&j8g*1ZnLnWnJF$RD}ewi1h~^6X@Z zkXzU8IHSl@m$3C9J@&h5r3*Y)Z*bXBCKvJ26ddpIbrHC`>wNNF^w@||mv{iJTSz

2&Eu$lie zFKEuApQSI_{m<`46c(Iv)jL*LBpGnXEKD0>E@#p( zghz9u9_Dtl{TQ#Y=X0px+(SnllbQWua-3rtZ~39xhWG8tD=4nOe3r||)A7w}#F$XS zWUD%RztVr~r^0Y$CAHSel$ff|bQ~GDY@%cZMeU{CIhSW%JW^6FMjGsY*BY<`-9{&? zKYcjdN7ykJ5J4dfG~tb&Q?Od&rl2bEzRS5Q79h>UJTD+YYJ?OP-@!QkzWiO6S)e5H zLxJ~P!VvQvAzNn#{vVUKcfV^*rY=tlX-7m{oREK{9nMPIlp*~BVYbvV@xrZ|{0S8IpAZb*za7dO<|>2vN}co|0GAjI1;GHxfj1d)p) zHWfk`1-Jt~O#6IWO^m$(hVIm=kZ+<|MrVHWxN1^$akji!gc#ZEUUq5;3_ZPpNGTkIBUcl2KoR-5!9E2^lt*G;{h zuP81?snJm^8*~6xH_%0&9I#N!3~s}<$k&zP02gIxX*q6vvtIkp%mb)z=AFwOzTRI4 z$OXDkvle%aff;iZkv!4>xFH*(n_Q|+7!F&S>J=p*SvK2xZCx)nB=dE#xlf89o6p-thCtZPI&t?kui5lCh2?{XKrY-ZbK# z*8OqFKRn2r@I-WBd#MdS&3%DyKdZ8K_e-gEO~6JcHRs2l(BD4`0&eq9C)R&jvsb6g zdY)##VUICcTflf!c=vJUoaiG6Ar77J3`X}4hVCB5!!DCzhBT3y%UxDEV&|8i*GT|* z^vz2oXen#652F{Gt7QbqQ%b#lJiBsLmI2@JDRb=>GgFDN^)R6xTf=zjA#8{IA=HoF z6fF=T<=F9kQCPE@;-35TLFX{1H$fUP=iCD{!x?7~# z53_fulUm2+v_H6DES24*f?&xy`@-p<;+;ZRI;aP>&^3-;Yu?ELu%ZQe*vEy2k^Gl= z$}jp*r6-E@iuI9Z5SbZQvL~CKnE~!Jv_IU0l(R9{c^xgf1d@FKMl+AgIV%oc-Y{V5 zoFia}4>N`nM0Xow3PPkN=;f`lg1L}A77F9tlSOd2IYRr6SK`c44G^XmUWcv?;Hg#3 zd~1Mm6cE-(U}Fx6FsN1r*%R%S|0UYHI^lV*wCs7ke0QVofED00Rb2M?5(PQw_!)!@ zqUbpxVkbS~@kHJ=yu3r~kWPQk?#{Pvgmj234R2w2{{LvYs-U*IXpOrSC{o{|Ifdf${*2UcJ^`P8ja5hxkUDOS z6c8xUzd8W5jvV6Jx)-v{%c!KqA14QY`vEwQo+!?o7_Ix1%^cz3T6pog@)WtguiyLm zo<}oLFycrA0Oy7#usxwblPY8SN)F&r7V<)*?G^LisDj7SD1g~RA~XdrFUuLTXqc<+ zgb3PrOCJy6lyUqm1>qo(rnV^q{$BupgcogWIhz|}tLH~WRJ}pdRk?=0!QU|1@zdU} ztplNJIZN22&CbSMx3TtG+fJcfhqW9aEV1<)X%p9{c%Mwsh@jb0o{W5@KY$9S%kE9m znuTD$exVO`HKo{d#6yXJyzwCn?j}73yHYTBp*<6tGb)3gLToQ(Pd*Y`Ocx66Mo_T5 zJcmGKQaI&!J0gtU%INTfyJGCKJH>c1p87_V)GkX!cBY!{Og+G6Js9gJ&0do_Z0utn zjQ1r`5F_b*ofhQPVu@mI5Ja^ToFe7aPA$Z=nwz-%NaM_)U|AzR?y9yoY!G-Dm#GA0vWf5 zICM@|J+*3s}V7ZvDf$(1Ka3 z5kD`$rMRv5S`<*cUm8KE;;y3+>BKGX4UEG=bnIK`MN~K00j(5QQ-wpnO#(@v-QC~t z&N<tFF~%(ew_!{1_3-Y{b{Fz^*?^(_wNtwV8EX>S`$G6bLyd2 zQCvE24VYxQF8b$A+U52(|cdMzgCYL6e~*FyxXms(EHw&kl#QfDGP=$g#6{Xc*7PoLigJr6l_oiTs8-E&j@#!t@ z?s=XsUN;k)qL76mV36g*+F6VKWeeT0riB7D1RBA={TgLj@38$x9N;z;lsuzCt4Ib1 zzqq^enX6L|fJbq85?z%ZogeQVi%5i?P}VZSH#(SGuCDuh^#wyBX1xk1)gQcI(x-UVFjiBveF1r{TpP+3S_DWDeK{m$v$x$fbG$Q#5DhjQ z8n+=l_dre&-P^7OZa}sA@sP;1n2$Y%BG7>5CVbJ1l;J19^#^Mz~k zAhd}x?m`T%YCiVqk} zdxK&BTa5lQ^OE52?9mUW1_=cF*|a*v3N&-n->M(6WGn6hhCBdD5vBay`v}R68)xm5 z-+zY{ZcER}83Jti&OkbaDFBn;8@TJX>x4u)yRqe3|r zu=8eTkn7@tRuszIB6#O9=)gk;N(gES@uJ`}N&cR;6{!Mh=pDsF{w2dX8B|+IFvUn+ z<>PU>K8D3-WQ;tYZ74%tC#Y$twucnGI+JS)AjZJkKr<0L9?^{dHUg{GYd?SvM^+a{wURE?%yuLHYeo zK%H|#)hFbE>V^LNQy>l;klzj&cy5LZk`o_8=|7ZHXCcQMzSq_U`}e^f2KiaKn^iBK zSDH*tOY(MZlrAH;AO99t5W{X(YKG@MZwR2~1s74Z^ZdM1Cjt)8jcQyo>b)@@(456MjrqFoz3D3b|2pw$~$XY)Y)D<+;EN{_*hM zxfebu_&M$TX67Zpq}$l!E?noI!x%-g6co;o(A|a^KJ4SzjUUG#Z1^xAmxP3$AjWeW z6tctOZT5!^{7>I80584-g+_128+YwFb*FaaV#0Gt-H=T;V@d6`4smep^L3ZA+-<(Zm~2}j{%fkA zj>#8V#MlT`?W#A!Vd{dshVrpguDkzp-j{d=`m%G%@ew-P2x&0j5*qLDlFF#@9E--6;lf{&ngMAJp7wxqS%-d~Hi&C|7U zoe*08qAfbR*1!z>Gm#C^27*io5_Fc`Nr0w2cr*EiwWLbLULACIIw-|F1M_)nkc}>s zeJPOMmu{KUw;LzIN&*Fbf4jf@spJRK>2cZ*_%pg#|4Zx333##wT<2!Mdi5XvQDV)m zfK%J^K-qw`GhOs4M~OBc=lOImjT4I1Y`cc(Y6?2tcZwWJAYIoZT9n_82#6``yR*@q zZ%@F9kT>rwkJ0$(Gp1B>T#}*75+n{jREt^3J0`&xIBVI2k|5(Bu4B)^t49mo(}LDV zGRB3mn|Es`GdFl5<&Np8%)f-%Zex8SKdRfTopG%WSFR!IolxW)*0xR#L+J4;9_nFf02&mr5|?jMkp@6*$Elh?qVTE4dB&}!7muJ7o`_Mea0@BP?FY>227Drh_e zCgk6D>-SWgv{z{oSWw4)G_31oU+P8glK?L8Sv}Z5l9?cq1LOT>3+pw=C41@&{iDpd4mkE zPi`du(K1Y^Ux6^ul4+#@Yu>WAykC7v=%)L%mIYe+d*qzHKWpveFhOd89u=-)dwmu; zmi^VpK^XW>>wrhxzMbHl-r5a+g z>)@0uM7IDD^zlEgE1oCAR-lz3;7q2HS!}{f+LT=U4Tjy=ufw+={LImWf!s?pQi>42 zmK9>#Zq>ae{yKcSP0v<=-gY3d8}9tCu@csuBrg-X*+=0nw|$i_U5m}O)&}eGJQ|uK z@+~qFk%5ne*VvBRxZ70Bs5I)sFNDEwf1Pw=a~pOxF&8B?ZVcCt9hHO}F+>h=#@z6? zIiM%3osv2^@R5vaw+y7FlZ9rUZ+r73&*uvfaaCTa|E-{`V87aM-=jv%B#KV|($Yx= zI=rOWQONV|03SAPN?{-4o&;?Cgg`LMr#!WhdntUQu)AQMiR%eb7GHMqPNt|a*N=TU zmG)k=qWmH2>7z&`T>u7{~`X2$>P9^KYK{+ z`QGe&85B!Odo_teO0Z0L-Ny$vvM-y)v8xZ|8b@;y}s=5WsqUS#g1hg%B;8sWq-R+Aqfck7T#rx6<4bQW$q@7 zkplBq`)_AQ)ouqbyz!E2+05s`CQOLS0dny5MWFS2z{Jg+`0*tfB@?=*k-q-mI2$M$ zFgXHb%we8v={!(Sfr^Sc2QBrQdo=?1F^VLH8km6(y%p@&zXI3vbQ%!52AGUY>*-~a z>kq_k(rdhd0hI%o%K^TEeMnRPn)m?C9+6yP9rQTqm^VxmScXADP7gCeaaMdKlEgk8 zob%3g;%Ju)#cym7*qin-<}~@cq_M_sP}ZV&c8;E(Px3rsN;q zx4SjwrlucvcAfK~jjFUPrs;rG=NXgZ9OP1`)>Lsm;J?i>%hz~5jY4kbd{Kh5S+wEW zdPoVz_ltKQ7dz*zaTpI1Yj_#^2O)$$WbDTJbSiBj=tj&4ReB3M-vRAy<4CJLFyDMK z%4|bqV+C3m!g;=@boE9MgkHE`ieLFH`t+B}Cc@T=#4i6H-v@vfQ>eN;+GE-Svj#H2 zIyk)dePH4+gL)MKeH53yKfr{UKV@q#suz#pjoVFJY>1f8o-7&qaDy*`*6f+%>Dxo zLW#DxPg(|)6rUp!JMHQvE*MZ zVowCF!exgkEZ≺BIT|XO}y9@QEZjp7BV@7O%UgJOv;(N7(#szXHr*Pc@i_>|Zji z*ji}BR01bFS7c8qVs;*Gob5QKYhtJPa6EXRyjT9a^`4(pT;!zDXq17fl6jYS{Zz^4 z1Rh;PU9qchd4Hs#pcBM~G9lH(*@|)!2K7LFc+Np>&Xixl>4t<7aguW&cZ|GU!TVsvXSe8447u zSVx93-}BO9PHN4Xm00r~C^vP6Ca=v&{oG9@`G)p@S-|{3sW*#jwj&STaL-r=p_N5E z5C=lDtxnv8iBhU);rkWpBde&xty}fyRlBG6ej#SMo;ZJY;8zoEyyaY2V>sa0g z8z7MdVcOt!RURc9Oyw#%*om0^_vx+7Z*mD5kZuspZ;pR{kbH-j%|#p`4Nq7YF;3YL z7zrw`fThGbb3L9NskLhJ2Tktp^p60Fn9rRJ|U77cc* zD+It0*u(|Fk@GEXm~!>;RvY ztUvj!3H4a2(5aq09tCYCZ$e(yc;|x*WJmFPF2ch;;)zp>`|5l(C`Ggis#%u_tX)5K zxj9e)qO);&4T11%|J>^8YDrv$P&M#<;&mN!6u`!3q^A=a-ghkTJPXGO-(V@d%#x{m zeZ1cATE!A)XvIlHx_q4_q;J8n{ub=U*`*iihLg3fx>p_{CQ47^_ZR2F^PP3)#tr-L z*6}SiBs^Wj5yqNxLI?CExXWTiVcX$HduIOis`5j9+(6niCaGZA+I*V@4sNDFW#syK z@uejgiWF`u+El6*fz%YKKVdia@x3e92e+I{mg1|huSRhK(zwjr(YelyLp$r=kq>L4 zo-UvHa<5J1SjJ7=-IE>tkM$-dCaCYHuLXgKxSHdHX-J~I{rWzDeVe_%y$(o6^G1_M zQ@S98LRV`>Sv0061ux_U6jaD2-yy;lNDJkbuy2HC2rC8IG>>g;=+A+Lj7-}0_V)gD z*LVj2<&46&=itfjXj6^937<&$>EmlA#WY}?f-mSW)Sf~^|KWWdE?t;wf-@VxT#wQ5 zWaSYilrY=Z$DmdbC_KzE!+C5L%NnLnjb^o;PjmpLaDwqQ2#AxL4d67xQH%3OTWrhTGw@4RHA0RX%&v04gH`>i5y5TEDuWt%lKN9 z2Kwm!9FQsN>M1ySyrU`zzA~jH@5Y}+(Zx!raj*UsapL_ws2y*>2F%$t_44NT^t@ZM z=MtR6F%`OS`Q2x|p1br<+G+K~b5zsMEjZih8Wk^-U}J&2XK8%qgW8)ze?4@;C{S0V zM%~{P)s>e8+U!~nQPJyq3zyiff`~mmVbV5N+0{<<0F(*oCi^{hszb=Cun9koY!=csCcVX2~ zuXm!}Z1%ye({dKf#rP>d#+u8Te!bZk6(@y_c^Bzi*^&ClE*c0}4>7b6-EZj;yq%5L zvZ-|xOY$5QK1wGxgVk>Jh;nVW$u1Z+RX?5CPF%ZCjEz_}XXlKf;&KWa5uqVe)zl=6 z?yZ>>O$vSalhgE&icko;7&6)pjSwkQmSEN_r=oyDc}9)Q$^&g!)(Pc3 zE2o}GjENm=9#OFTY(3cN+4__1zUXSpG*c~yb31*L{3EpbYNCU#W7Ku&(FSo<3sh*# zq>Q|p+Uq9Wm6V`aDOETt-P>rf#I3#De*EeCTFd0x+9x1d&I!9bPh!`%+mN!&oU!8= z!kqc*qPf-|ec1WolI-1C_~{rl6^8W?#8iwO4tocsIFYn2E{eq)YCih2PVcaoYTdJWldOQXg z9{lWq#1;6N`*Uz^-8*0sJlvfcZpc3c($j$eFX89aX?Dx3j2!{RoJyoyUzZ?^0Vp15 zF6CAE=3GJf%P*-HhU%-U>t<=4kW< za!N{E9qEwKm#d5p7ipNm1+Hxp5N$-tgwGx@`8HpMTJD~0Pu>DODyJA!r}EcIZ6%Bo zp-XnpVtm1P=^=7XH+A^`Nr)`wKe5h(cuL3JwbU6X>bzx9;_~l`!S^G z=?SWgRP>}88EJCQaN?xxQ%(N+lClv6HVDm)D})}}>lhw?!Q9ud+A?oC%ky0wcy4KJ zbta}WaTdr{?1#(b?pGkM3Fa$fx8n>X*p8_UqJxCQ&InD9Uy)&zAgx?c1jTq0Yp`gWkb zXxm5C?zb!XGdEUY(audp**0qn{m?Y}UqDZWiMKbO$$TNbtW8zsL?=RxD7LsG48f!_ z*c0r}E3Pd0NalhD|ChM8`IH)SGohkLS6UZWVep=Ek!DbI6VDl$hcIgV4`hK1t+vX2IGKt2nc66_jm0H(88)A%7w4py&Du z1e%l&*)NSXebXeKg3W=yD_n6!P8u{oO=Y*6;%hCcN(wKFK@;T;xDtP^1^ z&e+XbB?;;2FkcNyM7Mm~{LYuB?u-wkn_hgS#V7EFo{ysJkKI!{RIT}cVZ1$;F^(5w zT0IH#rAmz*9ly?>aiji7QW%y7r`^tQGu05@u%iKW<_k8>l9m+5(zEZjmP$O37jY;b zO#%lC3tE%u%*p)~t}^GAMD~qR;ai5YXEw6$87h8R?r#Ur>u|s!8yH^&HpfFLGDJpnG25RbTkU2LAH@*8&uYU<&AsHlA~lgGa}bbX-yoh)?QP z*Qj|(ih?`XpS)jpf9C!!BMU~~jOZL$mZ=H}?;Mx%P+v9KDZ`b$A7$G1qw8db>J~0@ zZSYPg!~5V)RaJ47V$3idbuylxmdt|;2v6WW5RN*UGSH~cvZ6HVMv`5#o+i~MUjOPh zWFs^`9$#5nP7iC}PHJp;d!{)OdFH$(zW%n)CXBd!Z&KVtPc}Jw6vbZBIF(_+qI*ur zBZ=1=b2dGO<1+TK&{g=wiIx@EDMpjt%TTzT1LK5`IyJni805cEKg5Ui`Mp%OGCjx6 z)qq{!Gr!$VhlwS{YTQG&o42NMtD{uvIER89@dw}S+=EEV)GS8$F)9)R?y;p|B{Aza zm-obaPhPJ(vU4Vtj>cxoE1uKuj6D^RteIhu#l_p>juw=MjWvo?EsZ%Nuv+M3K+!&={?<Jz zV-AgNXfw2Qn)f_f%rv*Eef!-OcbC!q!ae6{CU0C1?2;WwPvAVtr~D| zi1Ct~)xc*rm^wXmKHV2BZWPk1+Ajs?5pF+V;Aw`K7R5)>PdS5+)j%M{_d}C+Iu#7Y z9psaO7=O`4&g5WfL7chD``%#9ZYj5~LY!MHq-R}~D>wR88%Nr3w$M{z5dka1&1~~KxypR_cA z7TQN{4eKw&|B)OiM17T;C9RYM_fy#+L{3C&pVRzvl@2?GL~yM$xr2V-J16MQ*6;F5 zKq$9yrb>Lx`eEj!TqgVm4v73x2)a&O7yYfUD1z_jnBMY0L-;SZ$mn9p`fFOhS&3e3 z+0{)n+X&Zx1QTp0~A9>)$@EuvMX9@>*npk>lwqr#?*ORQ^A^PI<`KE*nr`2P&j9j2CdG=h; zaH(riVc6d*9*@bM1m6I>&VNwdCSQ0Ld8HOq^S89I7AXglGA5)vB)>Bij}G1(;N1@h zK;I^2(ow*?^N2tnRF~>b0Bnrr`>Ra_td7@kzjr-E=FLv9>|gCD-EmVXHZs#YShm77 z^KqT?M(GwO0vcMP*cLX6st>eS>KYhf%m;<6`Ny;oBm=+Lp4+_D)@PToKfym>KY4C$ zM-6$VWEszit2yO6219m1IPk{kYyV+P(T@KtCyHLX#W)#Z@QwjS?x zC_LPjU;n;bqb(sm(7obfi8S02LroJeYlP14libvm+GV_VrW2RG#_%N8a-@-7Z>8T0 z^aL=V?Qb|)oxH_tMl*GKT;=Bc>pdrd%Dd5$_# zyfdzE^2@*tm*`*RnOOqeKYF2GaApoKOy{AC?xCrFBOKjGUacz1ceFU8`QmLLtGaxY zX}w=o5%pqYta>%s4ju9wN=G=n|BTi#>Y=hmO8(jyu;NvlYOtF%ar?f36*0f5pQLm2 zo|u}1ru=5zUDMVxHSxq>iHK3>7aM=Sw`U&kR2rB8sAC}B%xr9Q86R~6O?-SZ3_3mP z+uKKh1PRr9Z4sdORp5}V$()33l|uytnwjxP6f)A)H8nAT9(FEUv`LjDILkD8fC;A~ z6_Xe`S?GT;g0dAQ{CHV#uBJajU9Vt_7r9j~+7X$4BgY7189<}nJkbhjsNT;|*%yHL z+WBXTllukxCRbifM)BY{X7{e|{$|=M3+$}Xe#~aR-kp6s5dX#Wcu;Q+-QIEzFWQr+ zarM!m=g#72lYdw%7N@z;g#SB28IIU}J8Pl{7c!elIYCOHWO6n#B9o;UMwX)yG{mw| z4rS(9G52=Vyoa)_H_v&roz8MiaQHzIs-imy_38B^)eY}i8p<+0@7wtB<@p7} z2^$s{Ed1C{u_w{&%6Awt%QXtEky*Ya3SLqyZptbdCTLt_J+|nq4d&iS{Rn~MSa(rs zJ_?-+Upy=?L!Ke)cnupRj;Z~Yxs@&BiNt#ZZK7k99?|1<{{peF5#%eaz4D%lu`v(_ zf(tZ`W4iHkTx-N|TsEON1|7-IPkf9owkHz~bWSa>5NOQJmgS-lG-=t>DYIK8-q%^fw_G>kiYsV(wetQsX z7d@wv7%{hrb6)pAvqS?KCku5qYW+A8gc|aA=tZ)Mimb71?3eB7d7EZk7*mvR@e1N( z!;Avw3>mI;6Lc%?Hef3q^$pq@LinieZzu4pE(D4$Rjz-1vcZOLoHCL!%=~v^MODTW zUmW09%aED=3=x;59H;!%=N$&K^hY1Vk-axLq4@nJm+vL+P30E}Jk3@Gp`3sA+H7Gs zlDp&&ofL(}10AOvmT3#dk<#8)=qq_nFj>bbPY;xQ{#bE0d>h6`kF&hq1-v@Uhtx8x zTW)oScBq`MCh2U)W(6HHAoz{wF^T1%honmo+*_%)Vo_%bJziw!EbBKV6Sw;+614rf zlNOfV={<6!ge*&(iF3eDhsf)L7w@pn0}Oq8_Kv z=hG(LYIWTo9tR2v3N+g*Tr5aP)S%!KN3p;=KLP;2)2h?}JPg1iQzb>jXkCWj3=nT; zz4Rikki=!u@{$b~g=S!B#n@9HJ$6ESF&eTdmt$!`_X(sT2f&8lr+oAD;OG@JTo#x& zduCc+5Ye%`2W?E=X|*o$THQ4Z1iMC?9lCnL^=qiechmWB$)cA8c>TIkO|KT&EN};G zG;#E*c{hbL8J_1*3zKh#Up_SjAJv~!TPyCJ7jaZC4YmD@_pu2f{T0^1y!7fuAB)qM9(d*#- zgM;=^>-e6F*l?4aIE&fhBGAS%QApS!NJ!y&hgP*r*_Tk7Ag_O2Z%3K9hGb`gU%N)i zf&W{*c9GWEtbdhUO#;ywr~hZORS~s5{|R`s;P&=u6545qEd-GY`UIR??Ab6pXiJNg(re(h`u1=qEkI+81pZDXK(L=(7;H%<;@nR#+LwnE_YUKy4!| zI6Fs2wQ5ZqbI-}hOo~zf2MS_L9jD2kv><3nO8C;V6VnB`KBusPtKKHT+SGfMNVT{g3|jRJrzKw=+!!A$?EZE>t147qkM3GfB8cD} z1xPttbw;+z;BKMkRSv9E^^%~eci(#X7JWZDEY;~gs-Oma`BgyOWnff#=oYU!cjyKn z<>F$+4F{HTWNIRz9R~82@mQ3_;p{c)>#J$Em9;$?AGg=q6x{T-ewlI$8H@ay6-0gk zT?!lGv(-unIV{6~P`gO>%(^~u|5=%g$7Gktkl4mkpK3>5Xg2==N@_>*Pmf1mUu;`~p{l4NPF9$%22fu2q*@w)c*bVjfhzoJ_pXRONdxGMnHH+8F)Q_m)dBLTW+ zo4^vqeqJK{wJj?yuLC(F)uZDfOF#=rXVf5>p*5qJB~%7J-M#dH;YA^avjMfp?>QsA(e3lQdDau>qtgyMbz++&5q8{&ME{?5 zn16N(3 zy69*i7kc}a=VHO+SW$PE)2uFlYp6@`Yc$5puF>A+H%!S~LL2It_V9$DP4BWOaUQ`( z>#8qEJg_aX_LnEEAF;j=UN-V$HhA=8y3>Sh6TAH|-w4|JnS*URm`8sj_kD}gZ_1ND zZl6kFX{&Q6_?}Gqm=me2r%5)zh}gjRU(WHg!o1yuIj1X(2XgMJYkG~GK5MG8!Ix19 z%mF9haD;kJqGh%hso<8o*tSR=|HQ*#%Z+5SJkVaOIBOr>Sm`l=E6P1?AOFnS`xIzk zc~%O+Rk(!{xJSh|`IL6=BSbtb)7w6pVLMN*29h_<*&MQy8+K06-?}(Zku3_}(eM35 z2|u=r&;~0=$h9OlgS-M-L)ATXW(XV!YgS2gOmG+ z7L)rTLywo}F>8&rXv19|jzJyb(rp;0Selq&mzk%?dDts0BhsxH>PE6?(o#A)k04wYoPW}1t*&YCPRY8V`W8E5)2ywl`*%staSx#L zEq#3jto014fljj=piwjnh_Hot%LJMLEnFq~tZJ2uD=RC~cN`d1ab}aZuOdX!UsheG zueG(cd+%S9*t4t!f^VGrouJ~jTru^?p0HRe{%ui@Q6&DE?1=nBAUJ8Q?lW*&2A(%u z@b?MZ@fRTu+>R?JJ0%V=kP~F_%0=?6O+WmUe3VHSOfaK+p z-N-QYztBh*HJ5clnPt`eaa00R8jei+I=sI$J`9oz@eoQs5&6GV`neQmt+CFFsh^bz zvfb(H-Lh!>`SEQaR{g7LGRl}+2@nQP960Q(p{uYgNEW@Hi*KkeEn92Qk0&iG9`p)NT8ewZf?!>@D-m zEY}bXl+fpRL+--+nbG3Ntw zggS0A>xR1=ZjblM&GK6x0n0TRH?sKXL3)fO#DyMBvwd{V-E582(Hhs=pq~Xr`QIwny_%ckk03A@|zQ=_)C&4{@^_8(>--`WRpNuv+9f!cAR>+ zL4Y`=VC-O6IW=%67&kSB@-Zngt;Vw^l@e`KM-w~8p^(6d0Zu%%7q-qGe$0$VFj``b zj#xAUs7E>>7Q{Z)%Qh`|6H6WkB}JPaPGm=M<>jHPQ>}l3KD`BI-h^` zly1ZI2>^WuKbW8lrB}5(pf>t90BIMyS@v?SuQkOq9is4F&||Z+-=r`POkgOmzlB2? z60?aApF<26;;%Y;D|g-BRmmh}4S%Zorqbj2BfxJc1o`WAb-`-{oPsz=^Nz6%KFpy0 zP?QH&gK%{s=n$K3`*{vwPUs$lv#GUD^RBLTC%pigH4Ti8doJk$=Utrbm_V?U5~YOO zpJ30P>+Jus@hNksf$8TPB~cU5p5zC%w6$e|rXDDX%3enB4d@{kVnS=RU9KtmXVh&7 zu74j1xycUO0Z4i8GlTQ~pSiPCFtLO7|C)jh?vRzry10rgwq){xQQVP_wHD84XhBI~)=xX!c&~PfD)Te4V_e5?7zjt`8H=|~KaR{s zYukdbVIfH~uu@-^bW`+lcV~Z!Cn|h5W-_?WFzmA@agFnsa|HM9MOtQ0{k{DgL^w7fe{`1#EG z1I=$DrMRNe`MS=!ia!UGF6J6@vZzxD7d+NV>|;i7?2M-#+r@zJ}{0`-+^s zz_mr4vD2+>v=;YoXXZN2z4#?|zx$lCN4#UNN5cNwpd3hD0QwdHYS5!I8%ilVv_>kw zp*`Ox{i9dMVMpc#o*@6UTM`sS^c~C22t(0XU`62X!aV;f!d8)!Fzp)dH(p4c;mqz~ zp%P9r)zb2^nt6t%rY5$#N494!y7lsuDyNoFgs1IHJqA;F^y=27Dl&(!PDj#~PvU#}D!Io5)+HGGb)- z^GLwl87D0J5P}bQd2f7#T-x>fi)_Sr(R;WH54}mV73rW4Erf3SjN@%;?G)T7B(d>w zvHuEw({3_+R|8!!3BJ{Ma)hEV^vKADJUmfTQ@obs68sR}5>TGFeTHIbe9|=j2y|33 zU*z6!L?A*G)nn_orYRXmcCnlalLV1oJm)qyg*I6ot2N8HGn>{=2f@t2;>987&&cEp zRn-VP9t4zjzC~!6Tw7$?2Igjnc$*E(v7|y{K;j})@3_&M3-kDY)lb9D#K4c0=&{*s zi(l)rSlM>J<@-ULorB}|oF2PE<9f4hOmlPdg5~Nn-x;aJ{TC4tpd$Nw!|B`Wvkq`? zx183;1~uy7Ws&i_MH-VAP&)2-*N!EMme6epo*nz zhZ&h(2KN3|O~+!*O6QXW4uo-7BokBnkh!BjlZWr0*QB6t0v0TvdwF6ZXMk(JO0#`f6jxY`4y(`Zl>0v%W>&v^)}LEoTUTq?iiB4xy^lnpMT|=ExBG zLpUCh2ALI1217mhDf1mIyc`i<2RE0!_3;tuIf2!IbdQACx@ +mD`8dp96qh1*)q zkG{f@kudNRg$_w&MAJR~c%DPRBK)l;q0qlg;Z{K-GViYZQ@hr`k)QgMQ$^D>Uw#uo zcfK~u|7^{!VMG1Zi1W|+c*?@8<;(RXcOADxpIR(b=pq@i{qhvDy-!$|a&LOQVfnX( zxUwFxJ<4ut!{WD5eweM1LISTHO5XNyf#-l4>>d(50c0P9)!BQFI0@Bh zGa!bPGmaFL&IKny>;ErHYOS6;%(zb2_*BR?$;hb0?HVK7OR(I@BKtB(n-M{XZ&~-E zE8ER1&RclhF9O=9bUq(YFjOv|{GV=x9gbi6!Jh!0%qOc%z-_yxwzjdo{pb0! z%vLq>*d+0Bw)pDg#)~9iD!u-mLAkW|j#j&aj>MwZozgh%r z+yhmU-%Awo)-4H4F;MK33bij=^Prx~9~v=X-iaPK=V*gdp%Iy*h%b1y34SHNo?r3| z+j7fx8~zoYk@o@_(jDD1Xfcj@r$e-U$yKNKZoUY@l`Ji3r)BO=U@$)cIyv~QhoLXD z=YSzt@xP3kKa^yAAxQ}Ul)#$$pAAf!;qgcdGzO%D-~{s$)b<-yYo=#qeRp+DTwGjS zaGreoo@Jb6SlZVfTaX64Z)zF>e3jsb%-N%&#gV-r5Nc39-Ne3;s3a`2J3yxSf$2&A zuYC>WW^#Ifm+J%n561OUa3Ib{9TlFIZj{@T&0(ic*DTwJySK+8{$bljCk6h#e2dHI znurT$4Almk7JJXyXTjyZ;kdeGbN=ckkO3JOATGxm(xxA#OV^=g5aSF@m0xg7sm z?PlG*lp_||T*!@4mhXGF1wQ29b=m$Zb82fppR_JEEiv1#~1`(l`EvK z^J&#;HTc3qOuRaye>_eMA(vjy&@A*+WKk}rYIyeC@B7c3b547|H|=2E^YQ$@7GPxT zEybfD%1XG?4yU0{VjG;~j6(90<*0uL;3lUPW0IJ%M_YJ{{tkC`5)hHCboh*xm3rKYA-I1%$%m;#3Hp^>MxwEGV7(KY&ld{z@K9~hD+1oAd*}oxUgN2+(pOj9yvzBAPw8{NkxCjR1VU~+XvvrDtx$gTv0EAhg5lgO9vT7y z$0xsB^WpM#U}$Vje68QHV7Wok;2ru}42PsV6~+4^6m4|iYv00+Qg>W+0RFiwf|a}8 zh}QSC{5SQ?nt7Y(@=jH@XUpdD2XB!OONHVaqw+CY3lbi0>!EYYI7r~fyp67k+qLfAQW_#A?{HLh`!~u~7k-F6$ioc>Ry zC%AW@6{+33D{W)p%Q0ktnx^0l^iMw;ugvlr9$oPA6yDZf3F*hgP4k2>u;P#E0u*=$ z_vFysSL_hsrRc?&X|A%Kf?u8g=0r9vfjd^)dwFami}Pp7XHRQ%k-($&(zd-HdP8LA z0>`iE#sH2)@}}e}BISD4wl`t!94umOoe)>c7OJ=okA67y49>X#^SxXb?i^N8yKCtea zP6#+}c?(gNA&#_s-*=o=?y0U4znA0LV#Rdq8(>}i_lqxt#r@P{>+9EqDI7+@HPFB? z_?XJ>w+}a|4LdK7G(y+m7bT``oarM#1z9KQ^PNLL9@zwIP9)1@H#cGr&-Fp>t?sp< z5qX{x%cLWEEizs^=vQ>)H$gt-ssEwr8pHDb!+qAWR?D_+FWWX&%XW*)Hn(isUVN5q zJh^4p`Tfti&daXrdGUMmy}R!ZQOV12HQ(35bigVRBYGia<-eI1K+?7;BY)KV&x!Mvk7hu3$W!|M}-9-0DA zTTECQ)4zr1o~iw~`2wK9?IR$UO2G6+%)c^?PJj_#({<45TKff?d$J-2hk_5b1f~DXP<9wB>X_FOs?t`{6XL4 zaSuPMG46W~6s4q0SQ3u@GGL~G&|tZnn5^Y?t>p@8VEoi+dDC|f&DpzJ^f+MVf;I&; zpXNC8t|b6#CLKUwzF2HDcCtevT1Tkz`~y=Gofk4}3JTm%_*8hC{vy|vu+w<3tb}=Z z$r}fCR+CNtBdr>;y4P`002F-ZyuA+dxG_p>4wdSAF_K$yfKRerVI<9R~ zgY6lp9`Hif#1YfKxqC1nln(CS`Q&C3k0d_`4m$bls|Yef1+!9uy^(+a{E^aB92$_n zz1=^QS(s0UrDJDOP{{7K-01;z4B|rIsYJ}6gYq#x47hv0{U}+^2_WHN4}9D~b44Ex zvV;H0WQmztutvfDj;2B?Z1E@0?ojB<&G@53q{^=AP2XK#yv!K6Q#N`^@%~Tz7138O z`7z<`a9*ZgI<$aU)3i6=&5j-6=RiUimMKVec(}??wq5mz23eAVugl zildjrEXmPL=U$wE*W*q^xmINkpDXoDF`%AL@3}3ms;<7;4nd=y##i44y`p4*0)+Fe zt*qcC8UQCX1=v)tT*C~MCp6YWIJyHYZa{}uwx4i5f^pu|qztJ_92Dq=*YyxN0+qh_m-+9;Nv0rIxLioLEJ@@c0?cAlfdS;=mVPu>XVyaZ~QNEt_H(&h&)M67+U<7 zteCBQ7)};vLd~=(xb=?(()4Jrv#sh`|KIWlQ#gkKR&nFe3~qTBMH#M?RlS}`1-5TK(T>nN2NrgQ4oQ7DKu$kOF zAE3T-l^I}^Z-*C}B7Z;xI#*l0y#*>Ne%RHtr$NKl{0D%PYv>pW%NDAFk7mF+x_lC) zb0u@_U7AKU%Q%1>y+GH}wL-SrSbN+rPYP@kG4)X)C&=`A4RsMCKzMe8OHPxr9w6oZ z*bVPPH;nzG;Kl3n2dwaH(rx_h*wj9Dp-p1lJbH)vh?f8bfdGE@A?=vDlYF;B~8Y={v2XPikN{?0Rephab+t{&w^-M@UEiN zAF*XkDuj;GPF{6XQwe2JJYorU`VQ)XH|ujREDCMfQwX9~=9j0Z47fDW9ZOg}WBb?u zr881CjrFO$c-oF^QUNfmJNr4UHWPm5ySVUSx(# zEpb}@d7)FwMR4a`DigJ_Kqh6WF6<}I&Hmo0Qb2>kr$m3dWr2e7b%XGGp%Q1F)CY5j z>kcB7L&oR9tc61^^mP4f*9cU^L9s;s9>fZgZ!>M3Vo$Yrzs{|)@h0k#c1G5kABt}% zFcK;cJ=Oz;@_?55ykbI^U#~Txm#|_9W$m8)xsu!R$`*9w0_E2FCc^i!<37HN;qWo%QgE*Z4VA1-c(vi9&afFM`OW})-(;AChhqc~^hj(I<$3A$VkA2L4mh_Fu(fbq0 z+mSS%|1+n&y!_cpV??*_gBo)>R&4Nh&QOP1!1HZoZN0L*{PV^md#wHFHX9lqu#n*9 zf!TzFav{zDovTM@xqn=vGt$L>T}i2OVSp+HXu2KV*?MNuWcp!B*&1f{rxAmmWNLj+ z1a?t|0f>)c`G-s&W4M(J(FW&KBp0os%KR@?P_TA`}r&FH4P+K zM6cwk{HsqF=OG$E;9vTnIo6IUc!uvL7gl*M3WCf(^4I?sryT% z&Yn{}Gx$>*afqGona@HL*K;m@(m%KHf4M#=G^X?R816AX_lCah(6n~1i|QO5&U6RB z0Z1?c?S`J5Cnr=W#UD+V6k~%DFZ0C15XD82wiNT9`tOfgkVkPC=g<0m{5~3COBe zXHJ*yi_L8O)?h+yqm5hu2+P~w_*Rvh%9RL`%i0)C9DKmP%yMeP)1(->ev2NfAeNrc zmX0JzNBruM28i5!QL`;m4jrQHrjbvtT%}n+LcxGfMu3ty=HdFo8_dt_$u?cJDGmE2 za5jp&a#o)SsBvjwbCcTh(dqYU4Qw$X*`Hz34LBEmTgc(yKztoSr0>E}n$yGdfB^ZHRiQh5gV~E%kq*nFy{T(buRdd*uaz-+Wkt7R zgINqc0R#qgJ#977U~jih!{;|)F?3b|f$aUdY^u{HJzp(6C1_Wa0r$=muqwDLJEqg> zWa}S?$e`<`FUd<$5l*Y~oDB&|ko!0GjW-)~D%BY{_DvCI1i?&hWi^Bt5V=goIaxNH zw~m|#n;X>YmiS-kg{gbP^oC20qX%cA2*^UR(-04WEd+NKUeq7>sLCD8ox(~%F5EF3 z6Ah2Ir6N=DZ_Dw;&rloiuerT9o?ag>TL3sfDB!_3EC*CCeQ;lTjSBF(pJ1P!o&vTN zmjz92HsJFGN%&{QFO|1g=%`#>UB_Gj8WCMwC1&F3TS9GM`j>hAwBf}?+_(3Y!slr% zXn!lKZ~$9`_T0+N^B9?-*I_rEzjHj}8wvR*?WyCqN>K*h$GP$~M0|wnOZ|7CVkUj^ z^laRB%Glq;Ce!pw(ZV--~C9d1ADN zhjTY)?qh{qimuOn?0JA8&RMoc%mQ^D%1XNKPXvx|atB)jy<9O}n8%jsP_< zXByQUvx?EKi$h<#LMRlBs8325vVlSR!tRWavk;@?2wW21{5r)>?U<~#xCM|M16ffW z<6EoWRZ{yGA@~w_H6zN1Fx;_8;)SRcA# z4eJoOLqa3Z1b@Rqe0rZ|&b!5i$Re9bhEbp#G_`u(CZ29taO9 z4$`WZs>4BVrl_c`n&n&nb*B>j%Jo##?S|LFr_Qi$U~(GYS_l6A`uP#KEU5A=t=(pm zqr8{SA#ABNCBp(o&l7lc6%uUq4!hCG-(Ia&FFGNoyQ$EuXZtfXET_+cQHUk8RV#$9 z;jfYY4ucBcjY*RlA`xtaHqGANY)Nz9+I4gIXo!q>Pz$WnR-I>ks!}2R=W^aqr$Z&) z@`zHbSq3bOiF~(qdlDOrwq!pICICTO21|+q8(cCArW;vrUp}8OhpMpxK_xow(X}Fv z?r;(Ck4uj_QVcjJ4(+&!KU0D~@Z^QSBbAR?C4j#v&6;f)Pvzxn?8xUYWB!W| z!mSyDTc>*>d*%}}Zt;sfxOO{VHUoEGHGw^KjTF zf<0YV{PrdgO|hV%M-#jf-*8V6bY z)Af%;`FPAhXaS|99Qvp5@xvd4X%wG|iZEahwn{fH=Ms}jE(d%3WtAG>mH~t%>Y{_w z5FG|_uR;WPvd7>VzgoxwW@wwQ9+Ik(=rG8;m`=|(qzDMXL9|hr+<|5RdCIRlBx+J& z*QcZ$=`*IZKWmK}v!TO(U6^Mcw^dWJ{_HKa8;4g-Dce8&;4;aJ1z9Q~ScPrZq0bK_ z#3S_I<&E~+YA&R+q#ki*Bkt82tDYqEdfNTmn-O#dq^ANHane={ z^F(Q&_7FH%vGmGc1d)L`RSw8ulPkt7R*Q^`L^x{w?T%MO!|CX}hpkD**Fu2@HAyDLM63AOdQ2Inx)TX=Yz8i+N$T99%%AYA(}&wet_p|Slc z>m}fyLTg~m_WWi0^xtLkL*?JKl~&MfTP>i^8zzXBKsOJ@XmpLXCVOjscOtj*2wJ(c z_~VX3h5@QecV zoM`RR;Ddk9b!si;QWoqoHSf}CFkRMfdK`r;Wl^!3oYoztv~*5l2T5 zqe1?*a9aMn4`^t|IDSDk;wKikE-o!D0=|^;^70e}Q!Q-f%8QzhrDL2d`z25j;)omb zGtj9e0x*|XPsUF;P#Q*Uv9nlb!>TaC62G|cp#B9l2}YHlG2u1rR-|<(KqOi6eo>Rq zTc75CFj#YUw(WP6k*!`U{tQD8Hk5-Kxxx%W1wN)r4pxjGMqfaOoD6fMD+Ke;X&pxh zs?+oBapG;vcfFBlpgM>0%kI%kAutbuNlgHFq3iA35&U);7}(I8nu-HkaR^=k_{%51 zy!Q6?A+j>ac69Rc91~q!K=NxUXSg){;4cUQYE@}K%TFm90{Wb}DM@1WQ_@?gP zWk`GxEN7nmx+B{n(jnAfPmY;`KX~WGXg^lYQR-3kf15?8nS_8NoPS}57|+?R@xdN* zyIZ{b%JZfEi3H@02#3gR%5kJ#IN3MBzkO>5bSAlte~h!W8LzZN!D7X3M*jXd54P!> zjY7Y_hH`yN%vK3junK4qPD3}#kjJVRvzU+mnki$id;c0RoTuFFj#Ozf>~P0R!nfIS zJ&pu?<9Z0QQa?UBc;jaoJ_fhAg+BYSLIrB^8u3+3%cWzoNQ(6v^eMKn2IcqgiU1$V zW5JRf$O95a^UvogqU$EE1w#+JcFdbv3LHmzp%XgPY3;A~Sm!smk=f6YbUEK5SP}3E zwX1X;*rWqIg;5&zrVXK6_IvE;$pw}(wh+x}q$Qz>ERBni7|-?Oa^rE|fzT8SP@X@X zqiwO8tYC#HpM`s!oDLd(4&L^K^UC`^w>T1iyhmA2N5$%_oJjt2Dg)RFmESGzxJcJ+ zZue5wSgk|$KxslHEJA7nVWR#>&^8530Bz#2YV9K2e(SOI%?WjTl8^TjYI`K1R{w9` z4*9wgan+lbo7=ooD$v^Ye$eZbH|#!{^XAbHT^8k@8w;0eR}n6r-6Hl&c^8zDIpvd$ zPjCd#-0yQE%dRYh^aP5(~xw}E5CySuQ5@I~~l@2tQK0jPlBq3)J~$|}Gw ze3pr7z~j%ocFW4MqrMyQam=msQ~JKM2OrD?6dFP1J4DhTkSftWHEPkPNd26A~y|F)H2jhljSki z?XMa+q+a9zsX)*VwSz}y$)=@R8FShg)0trORfp2|-VC=+oj1>JKlLK-jx<%7%vwWP%?wp;w9A zCJ_Hlwp;w|1c^i`#YW3*{Dl#IxP`FVfQhBnUjAFEs4Z?_gpT!TJ zM<>K`B#fHuPh#Pe42*j|OQRoMQc0NLsnO#min1Uap~4|{I3O-3f8$;~=eD!6 z1NN5b9KK{;XSWk^psmM!!$Pn|gC1B+fF%Z4OvHaBFdAy}ZE^GPfX3sjxj05vT7^Hn zxkX-gfa5Ze9v7npuja#518E>z6$AZ-c87=U@a{o%z~!b-v25$6L1X$0zMLejxA*|D z37v3><0qH6;V!C-s0VxK+N3&$l+eT?7#)m#=ty%!+j{lq?^RFx@d|23I2nTM-dY%K zVDokZ)UBIxE)C&!S-e=uM#MU6d#S!*Rp)jW)JuLxJkxJU@`tm_@tcmFb0ePqejsJU zjwujJU0t21aK19k2B$<%bF+IGS+7~(@mUw%?bmWKdzHbBQ?<4l(Pac2-uOT-*IwSy z)zL*8zYehjMVxE+8(DDFk0bTmYJ=0>P{AV`(nXPfJ=7|Jn%#F0|bn7@lHqeec@l&NTZNA!Ky zXON-1sLDo&<|2_}=m*zMyIwCTv460vL%=6dRm`mHZ!$Hz{sCCF8x`Y zPezTg!$E!OH=rR0f(VNB<#pnuj>7k;UE2(`5l1$*6b*a%&}qJRJbABMnKBGrMbdV_DP1;USFr$%2GOpGYac`&l8BSZ_M)=O#bJwWB5L{#_gX z;GYu!Z>BoXW6ZNaWL(bAt3^V>5kUHa?*o=rT5%@SR0SWX)jyza@JQJr0s!NFYol7{x96BeQKfum7+NV(IQqoapaeCRhyrAz1kOu*a z`B>(iU_4NweA@9}hsV0}DA9lYK2U*scZbA2R${g|ZokhB?Q#RQw zQ^9r~AZNk>htEsSZE#0-FW82ki$d{!p8oU5#bV-WWQ38SLI4+tuTzkb!X?>61Y437 zBH*)hve{h9%km%`A@+MTMqYZq9vOOG&BO3kS>_Gzw*> z9kOvx*VpW^Uqiye!YJbns8--i z^|{5rE}L64t_ntut zcj$NLUkKZsYqk%)QLz6euRLk8@a{KwKBehA(Xh0#{QC9l$V>OuLqJRNphwV=lXs^- zKrYumsbXiY-v;!8?6+RJ1v~2cLM{VZ=_{$Ja8g2wGTaaq5jsqA@S!jd1;um{34x~% z2#u4(SbVT?p^ESXEr~QTTy0WWVzAhHc-zq$)fP%XxJYikLhLJy#o|A-Iu*-RO#JO* zmGE}Gh>fT@c5Q45MZHO2m+wxG_3Sx#SZYp7P9oRNzcn61@T@yQo8n#nK=TVCML2Tf z4>kSs-x~NjC^8u#eEVyj3r5GtXvEzR5R&~T9RbAOz=aEx?=>BQ70o zG#$;ao_kg78HY++bbwEZjV0@6o0x{(EKUJ}OrqW{{T=1-l(wv85aT!@4M33bg?;Tt zm)HAm#wh16)qS{det&;!OytR2_hOA0tmQ+C>>|~;Xf{c}EnL<}Db|RP@7#z@Rwnz0 z_i-2C%gLEaYB=$gK<89hE8kZzh8e=XdH7HKD%)Q)8IRv)3#nu zj4O1x2j7Vpljzc<#ybe@J)CtQnCYzd{AsCPi*3B@Dp>Ota4ftJM)omW^nNb~M^1m8 z-&7f7YM&>mCeU)f-)Te!Q;}4EGWbci4Dk*o?(hsp>!)QrmySoOj8Bfe(tlU zNOURefhk>=@~PD5atPZ=(JTi5=1<2`aW1po^p=usVmeQqg##j|KX zer9O)Fvg?4-?O~`I-jR2t|8z@EBVHNPiYy}!S(@z=u{CFJ!Dkza&Qd=D zWt_}W;^&)rmRF_6JgL@QEeF7E{pApW)1%q{euiY2P!)Xmu^)TDtT5?zfZWyXcZ|39 zlwY9jVWNc`Mb@m|Y8NpON&@{>LtEnayCQ#qMslO?bg`vw(Mn!!C*wiepvR$&jA_?g zRk$~*M}aPUG;#!O30zOcfOCOIYT$<&^f==O4Tua=R*jE6`DWwit6N#Bt*~>1qhj9? zhzd8m_sg=M%JHc$bBdlMlT#mR{{~bkI`OhD)!k-c zCU1$zo}`qYG;JjsRW5o zhu~!ce9hoQ_BcdD95V!Rq3wE~dmMv&dbhxUtr4yWXq40+_UAP=L=f|JPaPPKf(+NS z!$)*pLiT~WU{||V!M=h6nZwys)v8x$C>(Q1BVRAL+tetyfF!WZ^U{;5RDL^)6Qzvr zr6uvII$lPQVK^}&lf`|5hdig>LEZK?k&tq+T}Gwt74-AEON3i=TNH&TRL`2j*Pt z#g}HdzR~ZA9{iw*$D9hiw17B#0A|^F3`Umit;<3g+fM-FVEF|G!a|RN;u;iZgdxgd$^Z({=T|`;($#kV|SIIc5i&5 z=C=whyK1$|{t<>!zR~XF@f=t_FSqrkB{CNa>UKkz=7k0$j`ZQ0P-*j*Yn{YP^jIuq zU;YE%jevE=TdUl`)udxS8_)6VOPbn?JY=GU(kKqQX1dCMd&D@t=&@5 z{md;s^Y^$HHkm)h2_*Y@@JNy)-FPdS3O-~!dY!F$H4Pd1`$xxxa)QihAz~`6A;W*- zK7F!%^S28nA|ar*_jjv_c!+M;*~4p%LO*7=8$6BY`%GGVQT>21T5p8Py?T(JnJ%;eU z?AxE6Y{??LV@b;`jxbk$G0+e}nWbk?5GMt@*9=OoFv(DFWvFe%Eny#m5W4zm| zNjIzcZv#Qa4k8;P)$P2N@S~nbwfEmPv+LN|9@T}}(*SKD1%E~_n+UDC4MUqe6;kVU zKh6!o@I+L(`R1+P(5EVB4I`spPscxg?gAsK3?b49;V6*k(EiHFAi{~u0(_rq{DIp0 zqvS^doC4qj6Bmn>%4(lQ&W8gJ2HHXREF5U>2(lKa1iMpxKeix}{F<gkLm~e;{RA3!^T&dSZt_#(?z^l1K(R_}r^HfY3X3Y>L?SilT?+Qj!nya)SD+r$-+Ib_8Dunag7n_f z?pr@%)7d#uJCFj0sZ4rf?*Moe!q4KOjq_O)sE+PDj6@)_C^ZT7@8@9fj|dsjd=0Mo zZfN*NPICFq*b0#Fv^nP1;O=Idh&b{1Up~>5xOr*$p zzfRX&lb2PJ^MkZR+tv9(@Gm z4>$iRVH=Vw%E`8_9n6=j1JM=$TN+T)buCxZ^K1(Q)+R^o&xjf#nBM;?+;bZfUQ4XY zl%-qg4HU5!`3Fe>QTiG3kIcd1iaSUwta**`#h5xDO6 z`RLOPEqqX-k?g>B=?@b4cle5wwd%e&i|kLghM%9KvG^#Q`$rAexGi; z@;z%LI@y$KhAL(emr|~P;DdMiz zfJ7lY(2SaEhQ&~Q;3Lp4?qmJRRVij-DXm`q20t#k%2H#Oe|l}ZA?DqDa{HLDBv&$kvsPnLSl>TWTu2{dGVNkWKZHtYbB#{v6U3 zcWzHV$naVSjTEgfq}KD*uX?`{CH($uto!S{;}W@>=iDSe;>{NFVbU#p_7n)WSu1kW z9}5aRSGWSdYVg(8t|U3r8KABR*T1b%H*IiNR{-%yKtBm9axtnz%M^lfEaFa&4M<83 z&&fJ`TSoMyoV&f6Z=u$mnP0`g!8>6@njStRe)Z@p{DFML}o*RJuTd@H4wBO zpYNF2yB831grw*EDKJ904HJ_QwxVh^VM@bVfsd7lD`CtcCd!{Voj|pEDd?4})*B`N zY0_Gu`p}fMN0(qY77hoSxAkVq-K!I2nn=T&OTcecs{SM`{)-B!zk?~Hww&AwBQUYJ zCW|X916sCd7^pz*oq|ipq{aKYTG3yixq0Pk?2*r~f?H7I;TOZU#TZSfVQTjNF{)?QXBlR9_0f8)T(p3e&TWkYV7mUqX!6al(sH9Vpa7~38<;Dgqj{?kC z(mU3#9FhU31yl68j+U=L!>PL=#TR&_pQq2~dI-~~)~)@|$GHOPZ?5az$WB1=?oBt! zAFiWje0PBlKi~`;9+nzlwE$vifxP+urdG}6GxJ5yi$ft#K{FJm3ECh!=muaLlu8FF z;sui=JKUF><&TGKM%F#B0kK1J+;1`d9Dybx6MOquYvBhz=%(hm;n3Dk-~JU-q)H4+ zZG1LDu7S>%y-&@O8~#^_%j)@~mU7DwdK;;T0qz^xGrvwR3)N6!XwiIDh}rZ9)xU^7 zp+3t#lrlC8$fJ=YyV_em?!9E)an?Ym_Z;v?Y)hibLntje`}uZFVA1qYCCwjZ3&qeA z1;L3h?X#VeR>SDl{TbV4gcdkL|9=P|A_0>;aJh9C6{+1xVIYC9HfYWJC8EU@>NZ^E z4&OT9dw3Z>-NM&TIq5aD7*%Eez_Y1x{oR3Pt{Ci*7Vp;J1w{tRsnM*I8g+7genG%JVDCDF_DHqRaAE~U4P!kVXcSsVSzk;%dk(iXIcKc;F8KPq1~Xm zwS81x2yNQAPvzf(~?J$Gn-jI*UEz`2d!?!zl+MQqz)i>p+^U^%{+vINm z1>aycFRmK>@vL4CYNba-m5{(6q1J_i(S{?5m|9i$&roJ%-6i(}jL&;0a@wirD?=-s z$C%CJa@r|J{>3~Exz5OxXtg^zOgm*gf;S_Xs%gFmPNUtt=lembVLF-b`f{>P+z^O& zEDPT@t3}&*PRVS=HNQP7WY1O?yX_$%oRVEM$(t(whz%i7=rSWmc~9#aGF>XfB3AmJryzGFsS1%75#129CXbd#Do2L^rAzEgj3XDzVqpV)nrfdI{fjbc7 z;(t0kR__tC5hsBL`<^HDBSv~YOqwz-gutO|=9a=g@4wRq0+=vFs@PYLd+|VWPT;L{ zpan$3|K2gBb8P47V-^YvK>+joU-AZ!Y7ZFy0L|SlZ;1vyzKRV;G5@?_^q=N6`{d7t zwM&2`i80s6%-=kh=0OAz3aOTn)%_T|QoCw*vtURhq-wCAP@&5*cz_CaF)>smFc>o! z1xCqO4M9WbPLQ5a7or9RgrN~Sq!@mJtdfEhbb5LaikR>KBQE~4Pxkt6SV}_ZDap#E&!fv9hQp4 z7smbRC*#N@vy~pN&e=DJaQ?Z+kIl>&1Px?J{%i5JDAA;&ox%P(EaXuBWlE`^t#~C= zCFXTDpLyo-r&*wamv2MP+eYJ|&Zo{#lII93ssm0wWTIg^t*JT&?$^W0>y@eW0z;@@ zl+0g?=5SdE;MuvMiCG|HQjD~JKV=^N+1BkUvblbI!`})TOXN>qbj+IlvKJ>EVu5mk z??Q+S{ik-0y*kEpfXYSXat_JUQRpvuX9ibXF@{NvwRSz`VfM$9ciLuiim-5HydCbw z713|eR5>i~A3VpZ?cL&U>UW%f6*ns61o zI}uSf_5A(vy>QKeTf&N4;^TY%tY;eBuk*316k}~_2g-7yJd5iZ+0+g)sS~`ARM8)< zrv}mKI8)BYJ)GPc9DMnovnvuar)jMMye6Oc#X2~S@$UVcUv}lUW_V^o(QjY;v4oN2 zROF#Gg!o7tbxF5$O{9DU$>uiCd*?NE`Vd~`pEU+t^2aK3f31z=uI%L$*@8igW#K7XR}mvcP=dcjefZgFPJJ#O*7}L; z5BO&P#v99{n#M=ZQZFLO&Z$86F59su^P%UVJKUROG-GXXqct8sh1CE^1w$|Y=K+1F z5@a}1Z>EbM#D*EJxpmDSs1*{~+mc1I5L>*@M2o8w=m=$&WWEW12A4eX+NHA7R)g2( z2>WmPp=T+lG}m14`;w6_6U3!A$Tvaldf7Z4A01B$Ls{q!J8lmzp?FOwiX2epAPoNH zRVpbdiTT}X1#%B)(25jkc8h9uik896e&^ z4@hEe{)-y}*ggDDllBXK0!wRF>q|>Z<{pCyWiLrZU|2ZOjw_fUg<OL6%dWP!L{_$~)ltamM9o_dpv$YcUIr7<2yXam7 z-Va+-%bxc(0Q2!1<`fJhgSeBLC>d@jW>f@0Q_Jfhgj|F$jjtgz4aBjlZmr) zsZT&_RGT$n9EeE$_jGaN*M;2kBJqtSkuC?A{jY%NWiGK1I5V}=S32t4_cNw}MW*yS z*y8U5Pug+LQQg&2Tb|@AlR?Y2VeVQG7OxT`>)f<`mVnWpFn%deW77UM z6ssZZ<3W~v&&?mBfGJHK%Yx%)lcuFS&7~u**41o7EkQjh2&pjmy{mhAImhkAuWGBM z5F5E>p6nlSikw8%xqKbpA+Ne%p&N9OyTM|8O@K}6Zf8_R!gqr8ky+MLa$F4~tsnUz z#UC9*R*XIXA~I?5HT5A{^T@k)i=vl7`7(=mqgIEjGotUYt+zPE?ehGpLZByk&Lz_S zjOf*9H;)96sFF#|LRbck6vqDW7^*wABclD7%Tm0@uBqbr>q7VxYxzfajZE$sn&&3_a z5??S~teD}SG>q>0HX{hSiLuE;56=I1$_EQwddFiltxoc%(Ham}#(X*c>mlH#2Yt&6 zIidtLZi#te$8qGre9o~5hEiy2f^iT(=&)50u9|AsMp#8&zJ>4-MR0%@wyOSM=zTw3 zcN#>YQ{kBoze(HUe4H|1L`*N0fjJ)&pr^sdUMk3cY+tQDHPFKhL!s~0K zE-a<@-LZK`#&%SKtwb7ubjx#=#ecQ`oIf$EbKp}uRHn>)m(RZ>w}kIbFkVFMBHnhPdlPxG@gyc$IGP@%td^sw0Gb)9>sA*%4L3O$RTg>#Blh=dxVSTN9k;E`~S zVs?{J|Jnb;yJovC+hMKqhfo=9$~=!Ob2CR%zA`(~>=ge+!DkWNPt1~1`+h_{`;I^D zR~l0F?32`rW6)J-alWt@NJ%yfsg5$09kqd8xd1j$ z_ijpsDo`WLUy~JB^Sw%iKCP!ZzY$nG{44F%gDw>KEIv2NkR1njt1wsGc3{pqW^iNe zAF3-(zqdBJIbOzHNDi#`W*53uX9?OR2tRPRJ~qAwDar>`z^OeuR`;Cq%R2ZFB)i5z z%wS-9d`6EexO#%yMM^~S9@U)WgXE=x$3Xb46^ek9Y}8qqmPhzCL^i5s;yi7=iPvEF zJ(KgZUuCQB14FZ(YoasU%bZ2k&JH%8}ShU*=V=07I+%r>mcp$D!KYz?L5l* z8h}@XRtsOg#a0W>i|@6m{E5{4J-+!c>sWr|hsnwbjdo^-xvi5QTgd6}y}6_Sy`Fjn za6VmxCXxx|i$Sk-?n>)4xgeDit&8}NF4Es#-p&wzjbV6~0s{{y;_LnH@M|)9qO{w$ z#?+dIHn%gy@>3U`DQo*}MfaRj^JcvZlKozxP_oZh1hZ(j1jG@ssgtYEQgyEdZni0R zUP>w4Dq!0 z#uWAxWQ`Od+3v?gr>eP6_j#O0Su?D(ik4vgHPeePoZs9%_4_7n1POQ0o9CNhMMa&o z>XWHEM#WScb>ZTM5KiJ3b)UJYkUQx95%ers`s7{|8oEUD9S|C#!JA+IwPXa3yLAwi zl$NR&=Y0Rg^JbJ^D{Q^y(0+icelp74xit^)nfK>~lcY`lguI>+xFGih(@(!bx9Y~l zbPLwt?m)j&iqFaBh~+NvY{jRt?6xcK+Tyn#jtHSlrzip)WPK8dclN!ah=*;PRB zi_8KyyxP{mjko&l1d!}>iofuXiF*Od{KVnSACEFZ^J0W{O6IURJXHy77ON$QEaY-%F94@u^$~kLR)A z-(QavAw90^9m|dcV#yn7 zNtN%g6)KXHZ4ot(u#C&Oq zQ;*cIP4&n{&|>wPTJiVZ>!H2d5aAcJOZBt15c2U*dM1WYll$sM8#U=R=mewAYHr$2 zxT=p~{)Ntp@2|rnCa6|IrS@jvKamuSor~gVaFwK% zjAHCeBpVjdI#7gYbMw(Qx15+FvJ##xX+_Me*{^1WTdUPB3gyyHhRCr*JE@~mAfp3P?=Ngmy2&+W3hO5L_w~y4is~Hq(hg?$i2~uB7 zgMTHOsEs^3iJi>lvBGYK+@D;R?7Qy!zJR)(>56KJ9X?OUiw$semri!<=*jFw!sW8) z2HUUbngxiLvAk)aAXI$bQ+q(K<^Tk6e3D?6!uWwL=pFA1`*8(7_FlAgjvD-sakiP z$~#4DMIQ*oUwbIe`)9}63-F+u&T9gX3ux~ZtL>o&XpHLKmcuDapVCr2AThSZ0?R?G zrAsMa$E>Ujds9rvjfEG& z5oJu}du*v=bDVvSvMWoSJ_6Qi00Qj1bx=G{R2e!ahn(pG(C zW$9CGXn}CshlU9Wxl;q~=rs{A5y4_iR-}6Jv!M;a0O^i%uBBcsf7f}60Mm^n^omcC zDVqb1#9?Nxi!(c46z=?b-tr~lJzk=u-s7WoL!mn$YNT+?Z#e8)(~=*}17Q&w$rq_A zzy}9kUL92}U&4`W;#pkl?LyP?_hoL0XVvIFJazkbuu_YH64YPe0K%mQ-KQa%e=_1L zg+Gz$?P?*1R3+Kk?}fG*|JE!VO--WH&_B*b1GR+4DN{< zrl`Ucj<11%qXZZGfZAdw{^JNJYZen|zZEJP+~$%8`a7S1B9Q?=r2~jO0Vq`v#9?|R zhMw`)b4y!QnOnvfC=$v9`7f{nIoLb+N4^|vN4HD{;t#;uZwPUwx=V7}1eEDE! z-H*8_A322Awi<~7M@SSFle6_1q>-eN=gV?A|6xa-L^#1FV(suVD~+fkT@V#-=dI;J ze+CW_jC3S+92^J2Rx>je>Web?SbgUO>$i;94^L=1s<7M;_HW6K*+owZb17QUAsM_H zL4p^RUydZ}s-FjToy@1;b|WoI;LqB$TuD|Y14#9WWs=Qvo6>EsEs~0eAj~0@-h%}d zXDjZ8<)h0ZK%a;|tfGw^ZA2HThRre+PMX&EGJj>AexR_5+|%93aaN!V#H!SHwKVv- zsQkx8cbnS6(Eijx)^U3~5j|hvIw7fd8E2|%2X6mH&nfZ-sFhS+KCUsqHSVSxxoO^w zA;t@+$~T_fqGyY#@@j+;`z%)&GU7^B+T9@Ih2Cg4q(>xcm#xk@d@a*vai}aPuf~Z@ zDJ1VH`+XA37<`o+OF-=0+NDC zr*wBWNOyOGbaxBV-3`*+-QC@A(cOK4^tbQl8}AQr40LdAR~&0jYi}8@`>|zefBq-Z zgp-@T(;S?Nm`RZk=SeG2Fu`@dk5>ibZfjeTO!u_)G#a}|(_xbmo!^Vs9bgNR>i7(Y zVT9z9^KFd|Q!2Y&u@l-;#IuiHfoLPBo}%KEk@I`cd=Z^V*%{O;1fqQ^IVQMUp$)n68pvlxPQocW_U<^*Vbh~Jlkau+ zVsc)&N`a9?|p`IoikUd_A4!t-UOt6O=-Y~pCA4Gw`OAqut|MP5Ioh2j0Q4lfqC;EqN zwBKx)8m@Q&xY7?$k021&*hw^2P!(t#3bEkBd>-z4Rl?DbR-Aw*MByj)ch+ndErmb4IT^AckH+|dgaMj zzyq=#1Juns`>LZfELdl?y@Vx$kJ;z5EzO2lFQtC$f7F$Teq2%t61ZU)Rw#U_`ZF+@ zdXWIqSIQUn6*X@Q96>OlJmFpa>u#giDDRkQ$X5U^6!NGCHoTY*m4tg#CYf%_8!Oe_DNtQ&>o`Od}HEQ&(@TXp=SZoWsCF`0om2BKqPVn%dT!&n$Y2 zSjrrYl=bVz&oyk~+~#KSCsot0IM@&^7{eBz;3H4OW<{ZP#9%*QGh#;6IMmu&JF4b8sme&`yyUN5N*DuTMOiPy*uNp31# z{G@5A>;ho*_(@5hF`3R@V+F)Mk3DBEl*$)@?Cbefx4_~z-171upjP>e`A5NJ`VXV& zVsM%>Mj3t5Nb1Z*=6-Iwf26Sq-kv!#m0^pyVxKB`A|8^LFAMb;PCT)CXx!gNEb0c` z&k^{dNf8ZKGv&sraxe`x=|Qh-&@#`Pg$=c_ELciORR5qj?{OoO*_2d z`*EM&VMIRNpRdOVo}_O>%ZPZfsVQvH_7-^;KS(4S@X)IV&1i)LcTS;J@mE!_1^t%f zqb8%gaQ5k^)A+T#J>u4zGI6f@g+$COvHSESK2!*+cjG__`QA2Z=gG8#G!ZhJY@_=b z=(61dB;mdG)nUn6|Q}!9)yK*3K%`wC*5Gdmi*@lE5$aHXM#rU zHW+JY0=Yw439^SKT-dQjzUq_$JA$vjmwBMlViklDjoxVGli}HXt%bN&U}kKZsir5R zE}$7qfP&_H)d}6n=}yN!fZ7gl-f($6(0)QW-@88{0}^z3^Q)o}Lv=dz#bbvqs-oE* zOQ&2wgx3u>ur(UY`yKw(BL^H)jITBMl1}^no)^AQeUef7yyiTofd$Ospg(2CLFEC_*# zJ;7~`c!7Ne#=amQPDZ@6>oY@X$YwNpEm5~}K=|SJjDIQr{(aTwJ1$h&xr`HREBje} zUKZxP5SseJ;RMH<9&h>9Yo!tst2??hw|ugLi<8@tUM#4nWJ#}q0@&7FHB48AY6FaW zo{F|84yRvAukLv60*h1DO?W`)23+!JfhD1sfn44lA}{kqS)r`$56H}0H)+UbntB>V zeo9G|(g+|jpNboUz|t#x)7iL!vwRBF?Q`Mer<^SFlMU&m(UL1K3a_%EHilTfQmp%m zF=+-pJ-kHSgg~30eZCT>mxrqsQ#%6KJR6 z!L9%7RI+u7s%dem*=|ejtoBE%hyh z!E*N&kCe?cf~actV}Xt0?qYfJ9Bj(|7^YsLt@SB^<%z1dReQR7LkLL3Aj?CP+_a!3 zbH&ypssmRmCXXWo#$_TSKX z)s7~%G#jD)kHT{4%(wpffe5B@zb2`Z^f?28ILnENM8v%*8PkQ#T=clDv?FJGoQGZ@ z*&&QKOiNEs5@Z0>Y;cj)m6c3sLA_SP6}CW9(712;;)>0#`V(!GotFT zP!$C3HY`GhjmO(!;%Gz*UuCh*qOmWhe~Q4&AYd2i2siJ-h!&J+MD2kXI7~@*2k~~) z6_1s>sQ&6Wr)K9X69*>g;p)UGZ)?2Xz^4xr5%yb??T+Ys%ku>mx4bLmmQs`DonQ;j zoQGqqyiQA#zrY!8tjUcH!qaS? zDZb$$0QhGzy^<`^Hq^CIi?$8LSLL`K7)Fy~+~?C3sV$1IMIi%~Zt0jYtU1)$%bLki zy}Rk4FZMSd492RDsjbA~hYdFB>Wb}ex_PdmZ*km{Ug@I6q)uC+xN+f|buwEtgVyL< zk7?=Yc{*yunXNigq9{n+Ya}Y^vY#f*wVQ1Zs^3qmk*wx*bji9qCSb}cw1^R`9wm?d zW}vxQEROwj0i-c%)6z-ah=Kl*M{y2#{S^X}L>(LcPcX5sL{isM9EETPZJIe^0oMp{ zg>BEH{Lzpx!H%2Jca z@V5b;Q%J*9`9lyu0n+{>B*6^``K-29gd5CHBcEx0&xRk>&l(1n&R75ZxaD?+pb}MC z6E?1>x73Nf9Fl<%E@fw4P=?bdrAJ}5iV?&-U(u@*0zjEy;%4fA80~lt7q+J-9zrGu zOO=$cRmz$9JV52xibq33Lq)FJED41D9HN1RhkyH37YG2ZW#aF3fzIot>05&$=-WmZ zkFZ z{IeO``K!lL6>83Ck@gLCwyQHRw|F$(&Q1H3*;V@%a?w2}SEGPEC8Q z(HslZdoigyj5{@4`$#0w6A1k^hosYPukACL=ojLz(65In z@@{04$K5+N7H-inW<_(43ZC?McmQ9!J^lOaYgt8)X3}gLX2SYKm+W)=+T-Fc-&(HS zsD`kXgCtYS#WfCRdiTvKkN~+?Z+3T$SHSz=8{}vH30F;w+6k55TelP{-M=eN{Y}yn z4~6AY{{Uj|zujO(mukO)Dp886dQpnZUf;FV$i5pS&;Rz+p6ndR~u&~_X z+^kntyZZYHr6wxJZQn&G5IU#fcY~oqX+Es}tZ7^An>hwvp9y|>#8jyEXq0tQZZyZ3 z+m!9uhrqWa#N#FVVNyMfvc_StWC-i#lDBlm<^M0Dj_GAY##DwNF455$|K-Xg-XsqX z20CeK#~7dgedkGZPM`}LsjX>D>Ccc!poA7z55l(qE#JIjxy*aEx5Pz% zwqW2Wtl`Q!qpPCQuc%t%UeJP*Q!^=oUPSW5F)6}VPWm|*Uy_7$RK+9QsFu?n1!2T2 zF{QPuY0B9q+?sv7FhvKh=dZ{o{TiFK*%@d)$p2Qpr7tqw9FMv`Wwk;!-k&mVPgD?W z=8U|puWf?kd);d-KIGO+uaAzyE6ufNyZzGs@<~_%)_(A;N6q z-b9ndGy{?eG*qb3aWHAO^WE?1yXt7qz9%dHYVJciPNy>8R0ji9>623#p83ThG@R=*-BBYY0n%xfm5V1& zU(b|ybN_0y4an?PcR69^0UOgP`@4}fXFH1g@U_~HOm z7U%LG!!IH|Not&4w1tRiri?1CiU&EKN?(|JGSIKd;O4NeytiK1k@cI?ZVh`(X?)He zT|>RFf`Mc!PESpPr}mpA^s8E-*wJRd9+#70@I--8L1%o3HbKn*u-FCC1X?kTBNL|f z;bWs*nW(xSj#;D2awJAVJ4;Y!Zq>?OHc`*wXD48OHS^&YSA`Fatf&``)GlM3IK?FP zUA+n{L1Ich-GaLVl({5UfUW8gLVZNO2g#XI%Q_8nK|7+tlDMUIxSx~#-!)?hp>HYx zsH^dMv~|5)=Za@Rsj02)4H0-XbUj}&TpQDqpSG~(L$K-Jb$Br6p4MsZtX$hX0|VWn zF)0Q2#^wxv;i^8Sn5dATjS(aN+RojBd%n||+xbdwm(L5D@bv(O{Pqif*A>Pv6M(8A z2OjUN*mvSW6Cn1=G92UN`QiJV^YJRktB4}HV`PP}2Tu|HkMXqYnHY0q@o`nJUN?U8 z7t^t)p8&d~&iF}Xrm-O7m?@z|vbJLDg#giZ{m&2a+Cvh#O$a|!?;P9WeI28@-|-Lm zRY6kbTmzIMRW=$k7a2C>gm3%kp{mXXfU-PY|5wrN?2{&?ezmiCB)KqcNvA7oo8=w@ z$M%cijsTtg>9Snm(Vx-n(tPw}_2kvI18@FkATwgY06*iTKQ+6ys6@QtNrYi>jhwo? z4%=E=5w?j^O5V?6mks%sApOheiCd0Z9>(3byQ= zM`1|Y9mL%x&+9|CgHd{u6N0lZOCn@MM~FrgO zS7h=%cg{kEDiZ5`IrUTKAes*y^pGNw^ZeqsR{Z^bM8+gB?WUc`)8WFeIDoT{eQucs%anmUHL zq(1`7Q+A1MH%u$TRgS{+_Gm4wNcgQWiDL^m$>&)v3t-80P617_?CbgWT zBC7P|4<_v=RsyS%a5#TuC$uhw16q!Xu`hc=4NOP;cxzPx^GB77AQyKpOtGI5Qz$IH zFQBa{3jP}elYAjqHbZvP#R_%pR-cxRbjVLVrj$*!-(e1&J|$t4f4n=LElivs{O#eg zXwv!IZy~T_|I3T85RM>kjMpOcCQ#tPc-?+4ipqSFQ=uwg+a8Mtsi*bQm=VIfpZXG@ zLm;K*AdJ%Ok8GroT8afBH|7|q7It{3#Uv%IA)Akk zWT_0D2zZJbn83p4K55L?JUWy}brlMaLzm)x3bQv8ICVOlz=pP*&g3#)GjMc_2PjkQ z>k4poPOvR>msgwC zFOr8F^-v^myz(U@pW4qgA>!O!tzJI}lJQ&I-&iL=w*H zyZw^ucWGG!{z_G2H|c7iHkxdNU8Ux}*FxU8-xtT!?KP`nv-i(X5U^=);D4FfB_qOz zox`V~8cgn3?o)H18A3m8e>KXUxqGMqM5 zzkLSTaS(N^T6JWcg|S)j`XuE2LemSdwYjiq=P3amFHozjF6&L>|eixl8Yk$+kFTIhj;$_@vT zF6mM{z>I-D7%DZ-ct{5kMrqr*XE|AIp~^R1wVl!X?x_3qu0u<3zVMi*0r$zVGhSUG-IK-d2W>_R zl}CanjbvjCGeo!f054|1S&B zouCaP$-S^afTK&4kf`A$)y$JmJ*6J^;I>wclj){7p+iXq2_4Xa=037^HJGBqoXppZ zkL_=$W7w0r7TG#EWI0ourqFZWYbfr&S@htm6z@_*$+VCpMHD{~&0^?u-Fz=y<8qp5V6GBROS2aDYy_hqQYGI78=P&eEZh# zBdWQ*jbv50aGJF<+wwj@^(Dx_lrp70!H3*n`Ig8 z2-n>-M=v|@9}I?@3|cVpTc8hlm$|Sm1(vA*Y(v*VALx*ZNPtxvQ{&*qNQuky!814v zh3@MkJZazeM`IJBw44mTS(~TER2rSkCJ}of!4_V@!ZqxIRl5^O#P45~qET6Dm$?>P zry5|Ix0O0tCEn^?>Z2wklP>@A-T9xjm$ms{ZBMn_KcN0;1NnfSy;i7`#v-_nfa8m3 zLV=%^?4yI<^~0}T{<|~3(%$%n3r4=v=5El1XA1pN<2Wt$GZhiufuhK-rP{JXKQ8}8 z=DQ;LzALp*f1;i+)V(b3sZ;&Sx z?YZuxLs|JX{XF~o4LrSI!Bql-eu*2H*ae=*9z`4vf3gayB^}3+oLin#MtG#bJzl{TML|&Bs z^bDgtRs#Cg`fE{%%Q`$}28)e(qvuZmgLyH}&;EJ>8qg`4+)gIkM^iG%Q(TpN$ux94 zuVpoEJC_2Eul+Q$C#sNEH#A>A`@C+9=56V3C#+`oX1O8A2ZeKn;w#(AF!(#bpHM^h zv!5H_ObIo2FWXp)2cEArMrhl%4lYnEZ?`O;750H#8ludW+B^9T=fwLIz+rO5(rEW5 z&fJ(z07VCM!MtF30ZA{qYKF zNeODjZr&@=j>lcGA;-PVm$$dB?khWwirT?VJ#Htw5XaJrLquWN-XVPsxhq`k`S}V1 zW$XS;l$fY#GYFsE4Gbg#f^C7*y>sVO_dA%nA4#@B*>=QpOg^Tjf1XBEUbUjfIQo?o_j+$24Os^8-I5SH6xbFw@aYvPNGyNech3%I*E)=$ z?9DSkEdE^Xz~&+mtijwTFKE=ZY(=5l|=UqOPn;PNBKegB&g1~D#6&vxn9-fce`76-pU=P6#9nDVKJVc zxFJs=2%d85O7Odj^UKeZ9`(_G@>4I_(8}+qj;w(V#DE^M#Z$K6WJY&88%9zOX}GeP z@79L=4GhRM=62P)uGHPZWYvf7#g|8)-MjLZ z?}O4vSn4!6sjMkO(J4h3M%Q+VZ#krSaa=O*D95Pbjgq>rD5i`Ou{i(edqTh+@jl5h z4y{JdC7+jL4=6JCyU;!5AJ+s#o1cE*7cdyCleiR_dG-b?Bn& zk;q_f*g(cf(VSH|CnZa0GJ^S(2ZS=KIrHo)Pz_mroQg$mF#^eQR4}%$9@i&UgRs0` z7=Ls#O4AJLaKp)#!kyjkSg6SA>+1pYiucxZY10|Ad)Jkn_IbWES zl@W14S&FFD$Kww}ZgTIf;VWl&`Ai~iZ!GlT6(wd-J^`QH1b@2#so9xknIR^QSik5? z)-FF9dm%@jY$$$nYYI|?pO!_8zb>mGeVJD;x1hxSQJkANJBTzzUbJpm;@tdocyRWEfRNC_dkAXea{SgM&uU6=;I)nAYD#3>kRJ_8Fu3Fxi{v`-A zlhb4OeTr~K#HL44{NZ>6h9D#h(npL?QMHz2J%RQB%H&H7fBOSnl!O~hAzr+qi{ zUu$fMz)+@$4UMR*AA*MEx7<2&}4xqFAQ6poz$e>W8bF@zmYi5aBpL*Arp!(ix1ggZ4s zJm>}7;Up&w>%9qw;tiD>59ODnnU=!8c%Gf9y1VNIO;$9B;$M4PqJz_0E1r2+zL@{q zX}AD2#|(G9W2NXhQ6&g4hGE9fU4D6|A~aW02Du-d!I8!z{y)n_s-JpK$Oj%i7>X)?|ceeduKT zpBF)PN6lL=dgKx%7bd2Qc*_&6ZBP0*d*(*9#1Z9O_I901qx1&OWcYo6Nph4?5T zBdZgTRHI{$C!Y=KcJiv8eQLg;@nRFvqpMVx&%J4-7!dcY{vFuwQ%lv8*sy(hd`R<- zn2Yw+*;trv;NQq|{QIRIDZ?;5qDlY#65jt>8_ocpPw+5-m-}lUK%6;v-#~t;Tq7&yQpCX^w9HG{Zp&*i{hQAtkVZJQCzDil>Sdu z8Go46vjPW5l3;7BFDHRil6XF02?I?Pm<7W_ol$I@6lY79ve7rhD$&o*w^z(h11ZH9 zn~fQb4>-)p4TQEV4*f8DAJ&a0xad`yqq~aa$rVs?C+FpSRwo)Gp@VIzs{`V?cQkKk zk)Mz#f;FeZ!XbuK>}no>zKfATfgZtn2~%a?i0|J#IH zlr96n`a5zh?)7@U#-u^UDb`y07CYw-CFj-q;yA|aRA|^XeDsuWxUVwUBRZ8H!3A+) z;i_E5&ks&)+G{Wl{_AuI%l!+QFmRM`>$|Q(zP)RVZAlA|+jE zG1go(ZziXKkN~8x2B#f1Gv>9{HD7DtY#LJ`z_ew-9s`IuX3ZH>1^F8YE&jx&->(x zZd-EsU|Kx$P_`#y6hll#$}2Mc{!_`?DfhCDGAM*{`~6`5vHKm6eQki3=WUyTJtCK2 z)l~pokmp80{TXFzbl0iYnwa22+O#(upf7F)ars-=e%Jp_+JWxhVK3m4gH{1odMNz{ zh`W7Ezxp(PamA+QqrV)o05O$b7D`Kv#22`k#|)j}I$xdwXcmZ#_I8=K8O(kRk%J%d zCfB9~d6&%b)L%;h_V3Pa>}|Fq_s~HJ@~CWt7qx5zn%HRd+K8m2J>?3_T*Oq(Jr0)I z`Vb6QY~KH^D8shzu7lNx9}5Y9_XA)&-y+gY;4s#@@r0c9e#c(oKc0EWKarf9TDU;H z7`5`GxxJ8n;!3p$SM9}hTL2|MOIy>?F|ilJKNcnuqVc-atikITv~)_HrS>_kVHy$} zo(}51a>B^_pCJ&fI4q7o? z%B9=}Fh8K+eps*#xn5qDFbBKvJnA6AfUQm|-o?nJtdb4x2=AzLb&PJ4B^TLq?(4fq zT7Ti50b-`i|K=ZEEazN}w`;TMo(8^1YVXdlKV&{q)Ay_1)BeKiUTZPD^?rHH3};Sf zRmZ2wc%b6p<$Nis5FvzOYmsCo9$^RvL2&q&PN)GC+6YzdNa&rzuj|pw~xI&kY-;uy;8B= zWx=lgYP--S%R=d&-z2SC-ON3OKAh!MedRQ9xR*G@&FI)LS9&G#gr~i~3KDgL@~e(v zIYXlpJLzOB)ui#O`~6tWw~lYiA6_NCgjJB`9%_>qs?&Fx`u4rPEOUB`JxOD|BH}=k z+HGz$@7Q&*$k*eG*mscVY{DahD5frGc2C@%!3mq6S*CNnw0>Me9!OJiL&Z@t0HF@l z*d`;w`|hRbjgL)yK2NuY?M{XEiY5!;jCyp%d7YC+mdI@AwD}%2HW1j((yn4hu5BPI z73h1~*LBSuOZ6Vil#(Z>W#+dsFhPnKZ;GyGmCbeDXUxqTzPMM<%~!t#xopqdWO*Y` z_*V1%{Bmv=P08O^VH}B`I{n_g*6lBlE#6FRgyH1~1V*oq6*h_svdTfUPCmh!y#|TM z&(PdiC~S7mwqF{3DO}SN5}Vdl@okOx;^+vWKBh7u z73ho+WL%N4VSu&H13==GEm|V> zh#&mTklGkLZbev;CA073bv{M|(5qaNgAKodr?Y%z)JQ9(DkAVR=1JJZ@)AmlM;cy~ zoS?Zq5x1H8yE?oaY|yx&Ry>`xsk8M)hzr5_OiPi_hn0`kV^c|I;m=2!jkt?q5AlRr zj!1OdASph6nL~_`hOLCAfijJM!`{D>Ak5b#g}f2@C%9K?(Z8aSU1ol^^S>r=sW4N) zt5XUnR(9kdJsmXHxH<#gd7fH1z~WX42?rV++E4X5*;l9*E{lzEwLc;k+yZ@bN=GG= zzkPFDaL@81fPuVl%{Wjl=5%Fsx=mJ`j&r585cpX;X?LYHdCoRE_vJUeeez$!I5U}HyVQ`PKWx}jE@@!mM@x%*?Z zs5`?62xDjzmayC3-0yaElIL5^bsX%&$MD-kCrf!`0=1nKTeiUa{+^D{!6L&c*MD>| zr47valR%e zyyF-o)YGE3n9$&*GcXI+^N~&Vg(7#++AO((wZg5oTQl>R@f@6KrcBrFk9c9n>`@t= zz!jj^`P4I%g84_fg(Ql|>yDuJCqz}-`<@-)bSnC&!@l)Er=F&!r1UE9_dB;|Bto^w z@I*Go1G9Q>XrFz`(h}~gA~6aQRdzWujFK_%S85J52#uG0obr#BHiTcrt;a=QMUe8X z%Ux|l(T03fpDA5LbuV`&JtXwMxRAPn+NrpX=o>qKwRNN;Sju=-G?BzE=k$Jy=Iyub zd{X-+&^&_Q?eWR)xk})`oc9eH03 zF+pNjYctU3>Ng*~`em7W@N7v zJ;B$kShz@F?7Xi7focWW)pybvBfQyKG$~If zpFqlzqfo70_6K=e)Ko&IMrWQqa`#z=N1WWl=vqq87qO*sLsWw=j=W|AWS>yipT z*Q}-Q+V__ivoBgPQ>0lH$j4nx*Ika9usevr(^F($ksj*$q)%Al_)G9e8egoLZ8rAC z(x)kD3Y(T;eiykGzx7*2MPsW{kxsYt3z1qSqS< zl6-3x+n)4zq-7ig$c~qe)~u>c8L?(;n@`B%u56p7%NM1@)*1}7@rEndGVZ;609J*S zRXHn1dPQXgP_c8po)ZIT4o=5?cnZ19?UUjhx#g7PWDQ-7?UmX5lfuT)g%Q#{7jBTH z)SZ?A>gwGcY@BGd*cSwOU;B(82Fs(`LVxF}Ii=3B0G%Hy`*@JdkxBJ~QrqpM|Jb&D z5hRkBEewhO5G_0sd-Q(W)~S{TK8b@4ORq@c(he^n+optk%k2F8Q}+dBsWUzW(hE^o zes7#fxC`!E&(5De3aW?pG|cXbH7s~!UPlIw@jr9k-igC6K^^+KM-PD#)=ry7m?Y#g zId66|j{!t8!(YB?cT=0cMYo)OjAn0kODDdi((2A>Phr?-HMbNrr1^vwA62)xW`4ia z!dl44gK}BbjkdbChG+G;%LtbAIr|3vJREQ&XP>Mrq8qSnYDEtvImlcRgxJ586*v8n zwI0-LSWpXJm>bJ0PV)JxTJJ{cAE4wMRhc>mOBVcDWLIqhc)>Z^KRO|-*+Rd<_kjV_ zoZHgIeFke=f!_yS<=qns%a%X_q#3e;7`x%Ln0?e)sn9uPK#Mcs;lN0yy+s1`8dV`) zR?fRkLs%~iMH>ZWcUi^Q@$o|eq$N@oh&!^=P)1D1Xs`*bb|gG~jP|!-vTj1{Kn6JV zL;a4{+k`kO@3Q~~NtvD&nTCqu{n($cugm|+V3JNcW}2Jq@f-PaD4&0}#CfqR#}<7O zX0fylP#w)y{rI)XL?lv*r{5=xLUyp6i0!w*`G(>1%q;2mBo#Ach2vhIt;E%Y{E@pV zk2&R&fVEDUFeFNZ02)y!<6Zi(w$+j+v=9~BRj^dbbm-%yOrCN7-$Nj80HWD1^cnxd{>_D+mpScJWRAvUC7Ru6# zT`Od3kzrc+t>({jSq9JklQk1~-`*;2KdDL|2~Uir4-`-{4C$$3V6aR0e_cljmA%E` zx<%exu5@z`^~MlRJlCv{kjwo+FD%FN+^h&`o;QB$uEKEJU4OTpK(fs}9MyodLI1l^G-p;w1&zT2da{{D;RtA7~)2sbBpe(?tjeDY9kGcucy-348ObNxwE1M|x5iM0-KnYuGAGe`qQ&56&{N(duW4 zo7phsRwsr%Q`&L*VW15~181m~BKhFrLi=P^hYHC>5wm$kDAhYn+#)BHX99Omt%C17 z-vo0sIPFZ}xoc0_8j_4=@q<4({uqy%tVp^Ve08=b>t%ss`Jp$MH~-r~ICTHn+-VG7 zpp8`Bm2Oh(6Po>fp!n)zt1Xm@#6A(H)Ij?YY=v}?Jvdl(;Q^x{YHt%ho(N>7AWBMCSS0(y-IILq^ZC~CQq8x;xXj}@ zcTPW~tjTz9iN156x{wZ6`IVs7Dl&9l`*1)1*(VuqeLESIqFi)sTi42K!bLSA$H6dn zq05O`UE%ZzF-R$s#BJVtb&-zday2Rg_T$TZ;?Nvh-=~@z%7ff$#gE>j=Trm>>h0St z=lv>Tc=pN=bSD2kDlqu*Q8l>Pj6OR}iakQ-926%z0S9nytsJaI4QfGlA<{$!79 zd#VxD&NOSu&yI(OaxAc-UYMVKeb5yG_dvqZz|g*G@O7{sqmZof4a zXGU+=^<+lh_0I^{?E&Mt$*=!^(-$nFNDOHa4uR5Ohr0=1K&mRlf8S z(gjkZX@sCk;#L{{@XvO$Se4;Hdy+{S7;*v*_XjAbLSQYU76dh4%3 z%fG#Hr7BeQRVN(-nxT%sMVI8t)81&YwI8#UIym!)jBJNVbfo10|ZUk|#Qe@wLmsx|>7 z)Y@aC#k+38;Zepy)p({$s}oy5dKbBwHX`DMB_(zh(xL&wUk9teTYA+iUY*ZrR#Sit zTQmDns<3Zi0kcsHdHQZ{ZMzMn4em&bRxYAA4ZV0;N94vkZ7H{(~3b5T8${95d7X;)H8*?p;BFtNfZ*|JziI~dKq4a{R5 zjj9emIOwz@;N^pZJ`?woii~|vszqH3V*a5QkuzU!YU?yk*IZ*hO^`lmCZ!KNW)!XhR<+` z@LyL=vZe`ne{zCdwn)w6c4>08`F6g-G!6S5uRy7kQ88ybZ`&vtCjx0)qz_hX@6;z> z{NNOE(W>medo=|8&F?Y03hLx8>{uJUU~BQOlX=Kc1`alIw2XHjo;}-}E|9M;7HW%* zF3!~x&vw~|Pin7vG+a$X^p+ZLTo{1(zAHB|^bDtnnT4)^Gl3jxiQf0M4CRiB&~6F0 zbesPnnMum2kXB$7VxNo54PziRKg}ac*55ZU{96k~nv%zC0|mAu4kRTZBgye0rAbLk zF+}Gv^U`cs55zG}0qR%5od5DcUjYGzh+9#_kFdP)$?w%KV0sL@j1j@lz-;Vu1(ab} z#i`)#6PZ$XE%i66kCvYYg0My^m~YyhufMANiq+Uxox<{}-Ih+kQ8>p?mdjL-jVi;v zUjR}gU5&J>r%bOSBTg>GH@krWD+e`>C3_s$Vd%C;R|}t3vGalZ%VR|^r>GOcY_$&% zZ?7=VH(?VDZ|K}Jc+a(HVM!pI;BF$g*N)TLahDc_J+x0VeO9HqUJgv7=GZhAXPuFi z8XenP7kWBMSZ%6em@zn0uO_`s5~T2hi9epI-<{IywPDsXtjm4r0MHHb-q%66vgNb? zN7FU`<@taAWw*F$*|u%lw!Q3H*fN)G+qUhyy2G+-TlV*Qe;$wT5B&po*Ez3q&Qle> zU7k`WHhc-Wb(XpSjnc8&|Lt4>UK9#uev4)>(p*j!dW*7d(<$U;?0X;dc63yI4djj- z>bZ0!xNTl%PW@6@eyP>>aa4QBXLk*KO5%yR#utOgO}#ChM5Y z(^d#ZD|IhYyIcsumCD#%UsQe3pKo~LI__5W*Bi{?fS@?w_6Kwx;6<*kt_Gw8qD|`- zK0X-$ldwfm{gf-r^b`5LAC;BVoTy?R2bh8i>bR)z*t+rkYokU|cyKIZtD-Qb7(xeW zJ!Zetsiz5o`|Pv6iq9odKh(sK)-(e5L(SZEG))2#lrdZF5Y4wuBP$hkO70R+)}zbI94&g6mI$k3U(Fe688O0HO9&~e8%yZc$0GoO)5XX#Q-x(p zO46}DNjcy!;K6LHTk3DA8|jYP9jGi4-|O;peABCM?sYaysgk3)0%qWlrm2CfJDr!0RaeXv zgGf$BF|NwtOoZjOdNL8_nY3h@+_Ed zBdUwlMVLx2KK*(+?=L4`+BEb@iX05cgEnnSgDwRt*}h*7`izQ)Skn$=7CAo)ahs~9 zhsN&C`r~x2as9>fQx6OIoTN~!qrFiq3B1iRezIIi@+p1!9wDG=&Z3pdynWuVhO!_(qxX<3;+H_*0L?69kq?-7i&ee4!o1dG_At{KAU_ zxD(0z-Uzxri&egd;NIlZ7jV-4b6(&jXsy9q8nIguE;7T<;fMk!xC4(B1lT8^Qb3y7 z10g&i03Q;tkGHjr1f1_{4ZFB(@xMn^&$P39CrX2zF+ZCF+Q*Bk<=T4u(pfpY@nxT^ zpO_N_!@Nfd7#?i`NVf8YB@yMsrXa@=1-9o5Z(gelMRQ~#LNo#-o9>1JIpMFf0ZoJ@ z$(y{)_)AGNow7bS4Q8FX*dxBKRD|8K@xYV9vAx-!;r8d=*LJ;PnL^?m-GzCSX?+^m zx_lJyH^KlAzzPjO2|+ zWr)rV62?BQ4b8!4vZqH`f-UH4IfAy`Ah5;ko(+59&Pt=qZ z#6v+#-xZ==wREB6A?u$pTYOS?8+1KzTpz13#bv)(kBIh zVNM3o0nqZd@E*X(Q=2PjJ!{eI<}HHlbo}&B4EoLeQ_q7pa}Atfm#FIIr4;0R3wF_V zLbO7=`qdHo81${RoJ;(#jpMHll<2osh znYlNe7IJ+Mk^X@Jm%tS~F7Y=^jnnXDtfhIhoBfKW@M%3f-=i`db28EN$Oa!;ij97{p^R&ci*Mk zQoo|>mLS~OsXejoRv9`h5fe^NuK0saa?l3E|J!(Na;@r8q+%c;^6@*{&sWb zq@(oj`J!Flo4W7_2TSDSWi3^*CpEH*N@71o7Rjy+q9G8Dj zR|gqF?^DJ0>|B>6+n`J<=kSe)sX56XbIwgqXD5o+Tco8k&Z#9)ZUpMba!xc=EU7j( zaUDKLkKWW0_HHcLN~~R0x|UCp(AMZF_`+uMhVLel!qXV)nf+NK*|DY$NOn@*^24SC z(dj>_)c5Lc_?RGstccvD{cxGjadiRGiKFG(Z@|)ok5v-llcSm)L6i z-hF{k!j8QtdLG|fn==cD_WKR{3?NGOC%(q%ARxbR0j5qZZEamWy}~W0V~Lku!qTGS z4d}CVnqQK0ihqotLEsu=mZ%rc+M*d`Oa79m+#oeJjM(xd8)+5-#YZGaN8IBkO%1o`L9umb@bz%? zi?j!D7VB`l&)RmsaTe4j>CN%MCpt8=9DE}k8ltnSqP;d#WBDtHIfhDcWW<{7u1PcnTzgw7VDE!g+IC1h&cLrgeNaup75VsXx z3aY#+q-T;(@V*Nu+5TT1%nOy0A#al_a~!CPWo)BuV|Df)#G(j*AMu7GhzH`d%S-T~ zb9-*g!PNZ0@#n804HhOr=6wQl?owY{PPkc4d~j9i-=StdRt)xn@eHvSVhIA z11+VJCcFRHK^ zqps(;sg>BZe#H>`Cq*z}Zgv*10isEpN}f8zDnCoh2(KAv@94z_`tf2knI;tvGo(9v zW)*n{j-fj`&PCUE{_9S+b1n_&t0Ol{2jQ6Tn3PfY5D`fY;Jtwz$>c;EWBnX-u20bM z=2?nq*QMoL2Oo?LZTsHwrF)TNOD?r&vg%INBgR=YQf?(_$WS8EFaT3+Y@NAr{Xy4X zHYxMmV)_0%a<+Z_d&)0|M& z5$l*sj3XG}a@w`%on7##)B9N-eRcCr49C{Hg?nrM_$8j9y4^BvBZFFfob5gV9J+Ye zQ3w8h;!Or9cJ`{$Mx1@S<*eXJ=*D{f6V{=;(hX3Gfy^`8`~X zxDdn<2~GlfdR-%DCIzt4)kHkIw{QIQk1c*vEv(4VzG)lswRo~`D-`}jiVNE{2 z86mE6V)q(3e;a^-GgkZGVhiNZ)D?xuMF#$A0vFt!ez^+dwhTQAkrT01j-;9Wx8SDX zr9$#Hi@GgIRFXrvshgHh{2M8+!sKOf_P>$0$_i<~S&;a@a#Z~6wMn^#Ps7Y**N>Eo zaLue6k0f*hLHmFK8nX+0j z-(V?&(eyC5r${92P{%e5{Es2!v-QyE*{Dpy_WF|Qp#erB&qukk_vRmde31DWTq)Zp zCzZ`;pOY-Ax``I98R(P7YARC6@3j!-sNdxy{18vtm#%DbwJ-z|5;rNJlY12o?`a^k z3Zjaq3Nq)X=X%_+4%7_HA~0g`iFUGo28Zn~$zt``Sp!xq&No647u>eDWw?^uHdj~4 z^5=0{UnM~yF1upA!!t2X;mI|L0E(`Az!;2RA~iQ zx76ffBpk79UFx3JESn`U!Iwk%DLGc7B|}V+TI!y^BC4yWp|_MkP77>e;rzY1CR7hj z)rqgXE%imCr=y?ZtOtFHoPwg_A;6q2w?nMa;^7YNs5lamqhLDj=BU;onydyPJ3yWk zg-rrxQGiP%@x!K_aVu%LmeE}r^XJseR!sP53 z_-BlRejgdwS-1&L>7>i;7dPQC$~2?QRR++4YmRa4dCGZ`{#&51;o0jyt*Q}VYX$gD zBO@ama|FxmdUPpjjG@Q&lx-44%O{3WT`;^P#SR!CG?LyW01=ByF>&b<1j2q^XwrvoG7-(hAd~ z#@3oyuBSy41e4%hM@0|iYCmn9YqNM0Qj=H3eHn_9B1#7zq0evKJ^wCXR9$vvyHmrt zkI*j=aEYcw;!C^ni4Ruar~?LhhI2gBu8{xIF@yzOfivJ`q+GHwMfS*RU7q(Plp zh*0AYvP~u1vN*%*pXlvqCGA6(qtnvLTl#tOeUmp{xLQpEaW8D~`|;~K)fEW)lN5-~jws#Xr`fv)Q&er1 zZ8epr=ZMvxi%(rvW$5;0XY+pbX>+#BjpA#s@9wI%sjz!xqT?C|Gqj{^gC zD53YPzKLu!1<@~pI3a9Na#u1Lm2L5KbyiZdFnBU;-|;j!9LoM?1QHNTnQb<<&(l=E zf&n%7if4|OP;yc*8gj`n+?7n+IUx`Lt@Nsxrn=}-G5kZ#oQmX5EJX3@|Kyh~bM#cikzt^M z^1pjCEH;c*>V!>^vL!{$e3>)|QiK6)0$XxeMZL-7yTFLW4_D`^K>>ta4rP3}ykl;z zP(v+feuGLGKD=eBNv$Pg+-B)_*0#uVid6A{G7oR>TS$y$(xdXC1JB#r=_(^>`()!9 zCA1RcEHMclqPzQML;}u(sgQllT3_G?Q6n8v%wr(^3oBMSSKfl{@=Y5y3+Oufw?U96 z?O8`$aQOW-fOa{?{l0LZPwc=!sy<(|x5u|{$8_Qgs_pHRd%15#L^jkTeU&T&0QMr$`DwrrYJ40f>@;M&!lY6-+bU9BxWMcD(Gel zhq^iE!nprN!5!yo3{q25GMF>DcV?bj* zdll4b);6^{S)aDNXQ$`V4g(EUSOji)`L7_nk#*|z$ zO_!Q;1jdXT&6r&K{%w+~fZETe&fh=q+oxUHRg9}FbfJ&=LWn3oxTPJ_l0^RriAi2- z1rtC1B`#y-N|l-n*rATwCm;dc)pu<%0n|IAI%(jp2zZD9I)l~KRhu?&m}zdsjan9? zgM+Dhqg-;a@52gTBY9Fuikw>~juLx##WI z8B%kMq-Y3+hSFno$rwl{aucA|Py0vK`aM#PNR^9&DKyd`)%{0l6xBhV@DCIok@u&V zUv9b5nvUORM6%JZ6~Fg|&<*iGZ&-^u3hVn-VC_yFvJFUyaG#VGHVtAgJ5@K_M{?TL z$*23Gu5Z$2NT+zFMOa9AI0seC4Es^x3M79vS?71KOI!J7ZLlG|q0mg-%XsVxtWWZ` z%p21!o{giM7XN|P@b<(+|J0$ zXx>B-oVV2%9G6<`@8>4~R~Jh?1e-?82Hv4xZL@5bePyDP767Al)tt5)@}TnJw%Q6ry>^Q1?UMs^G};s}jdSyn&P z6bZVglKI^C&nWh=#Fk?tV9}8WgiD|csV-&1DASv%6B9KKbB!|ezLm8U>ST2g_%O%p zI9lo>4CgonS@j(41xoeo05P*WZ>iTuNXG9W8*NwNNDH?bKe})&;YzwgI{u#vpyZSd zjR52@jr_+c*dN~=5_km#&D2s{cZX$Kq(Ml_U9b#elEtP1lDk9(kjgK+x9jQhEtnZY6Xae7v+hphONt7Pp?NPe zE!F{5X9^T{C61P63W`37RX`PU#6`$_i6W7w?5EM8l z*n@C_H~$ldFfv*>-kR8wcQV_3F}Ye2{Ec3_>arIahBp13$y%9Iuk3;A+ovAlbTVT5 z`Qx@E|M#ndras)iem6Z&oye4^+$ftZK_98Q8O+Uue76dn*}1u<{{Gqc&i+uJ|x=`NksCbr_Z{1Ooq9x;T7ooJ*9?>~`>;O!i@%WA)IVbqF zT-H(7ka|50IV~Jn8KuJOY&q>rp;mFxhCBV?D8s|Lg5TrJlTd$+=bm0zPiWEsJS#a5 z@ZI7{4XQ(()Hb6bU96KrYirVjjAC+eicY?YjJjmgMcdQ0mTJeZjM}v5JqG6q(!*FU zm(p|!Nzl7+Q7D3M1g_qhKYsX_Y>e}Qmj62h7Art>#^q-1o+kmbuD|2(e z?Goh+4vwy_t}1Bx!B|7J0cVN;OzLUon`1<`Z6~!5y01&j^py@?+O*<#^~muc_spInfq;36^m?hKA|v z+IB|I#-Y9TRj+xiO{wt+TTq(p%tNm55dv2Axl3Js_8lj{1S?0l#X}m0?cB8VJxHw;t~wjsH-+h!J6sru8PrYI1x^wDYq{RxBc-@RC|~>Lhg;@ z4dBm*f$Bkqf_HZMPGqRsld~-^Hrfl|wk+Blt1p&(BUC%AvbMd+t4E2!g|%LNt21h( zdU6Ld02l8t0?7_X1%r|R{1gYX2VURYB#4zFpAoIopzC0>2i!6lk*$w%gtfm1Qnk$A z$sfyPnOS`WwiG~PR0O`nfI+{L%dslUknV?7Azm*l(%9N(p}G~16L{xo+G29Epj@dG z(Ur@l8DtIH<)|@48n9(?I?BblnTzh!KU_$U~FECIL{Fqs8(ZUD3c;G0doy-H2%3T{irr&|Bys=2f$NM*N_ z5x+6JMuN;zHq>#+-C|-w*kw{FQ=;i@@!B+I1U2?of3HJaRm;86wP35*2yEmB8WVb5 zZpNQ^WGSx3EZXJgCMKZ)(L(xP)Zo!{C)mfIVR2DP>nNa(cj)~4u>TONWmgKPs+pgD-KxtPQ`-yQ=c}4tqACqrQ<<(W%_+7gBMZ zQRr){xz!F2NKkBTHKFSfuO>S_s&m?ItTyD?rtW-+a9l~{(-<3W*VO1}sLY3=8P^qj zaO(ebA^n+}_f~{*o;BYYyLSN=ZT8Bp>H7ju3FS>sNo#Q$j!SE7J*IQ#eS|()(Hu29 z%`QzpK{PJ7A+eXj)64-hN~bIF2CphdGuVL9S1CgTV1swW?ibfZV9=EvYw@ps+ z;EZt_hmL!uU*l0U5+^9s35U8Dx&F43*E#fBfQHb-K@Uq3zTg&X8X+N>4QB88&8n?7F6%wH)|3}}2TmFhUf#&2mHW_^u zRmmhLclo6xD^J6WD?M5wek}%E1D+ta+nhRD!ZIJX=<~ccO!>)^nyUk*x;pUTmXY~= z7Jg*`)`cHGU;s`+RaMo0wy1w`RQ%5jSd_h|uW~F;|0H1~HAz(3LsV|NMxbNKI3;7T zE!DFU=1nM``S*%jD*qUd$C{#9ucKrrWEpH^DD5qA_0RcLCor}Va3CwB!v=@QLNKgB_hf|3Ii5@BS$MPXFTKUg`DR@aWjsY0VvO_E#Dcqb4-N z;GH{Q@CA|zO8WH705gr#SZpx_kt^?O)T15Yyg6#QY-cM=I3Ri5bK6MvJ}HjM?Txn` zD>YY7&i-fSQ*6ifcKweXLz7^bqacr5VEXrbJwxSk0-Ib3x@86D2L7(0@IubmkqULs z2uPE%!uUf3W+hV)@fVYAWk8y0jR;NG8J3F`Oh9bbM{GX-z-&q0XujOg?d^KsiA;G& zJ&o-Cl*YgB#Lsq!bvg%Kjx)!fkeF#HmzOPDoKl7&=hnJ>b z3NPd83m*5Ud8|lGsF6l&V3_`!i{J7|Ho{ng zYwD(Xbd|+=2sC(fmrRKDU866BxC949N0NN}F`o{PJnE}szFW|=;{^MnXTt%!X_R^L z-_U=OlP|iCzUK1YBJ&@4&Y!xs@6Yn{ba=DP* zISneR8?AEqF0W|z-F#th_ zj(}kWo82O$KUH#1sw6f$JEbm$X)WumMo!iV7-#I2or4v#@8V90&@s+iz?6Gsv z@-#YVatmHnY)rdQQ|Ww;0JEynlj+54P%o-ZDYjVrO04m<#omUq&s*zUwapaQCNOJ- z9M!)*nI>C>vHsp{p=Yn9$0kiY{bsd{{6+0pTD6;w(fEd~dBP@1J-V#i*wFei!Ns}R z9B$yRvGDAFqK1vEM4|j2@Jk#pP!5R5M?cGI<@YmTUj-Q6q+u5MKYOOP`;e7|8TS-V z{uVt z+B(K)As;>NbrTtNYoJk^JmkaCE#t--=zLdzGFxg@a}dS^Ge!00u#42SfgJ(#w*qv| zf#kjcoT5RSOm-<>$5P`f-oq6%WIp0gy_Cu2LRLPS5h1&crlEh6sBQSx#$+b1$!heZ zyVXK;u&k&oV;14^-YuN#5nTP!Cs1G5p0iRx%^GM&7>}-_M%uCU?C%Ojm224& zhRYjS3ckCPI0Bd}H*Y?v+L{6G3)p~)KPQ}jnYm8AtFW}+J0i14nyhCbb1R9BnLOD^ zl_GK|-5U+F_Gf$9EY$7I{bUo4yo9U1eFW?a(|Wj|ZG@WaKM!eXmv8_YJ7Fe}Bs&#e zw{BAro1lPgXYu|$c8eZ2Y)|HdOZ$ZJPz?|&(wMlZlmZ7LkJg zNq4im;C4eFtrG!9kU^#KNAM_jbU)>)PkoVA(oK?@)i0iaS>KC5&%RHM?}jRA(Qs%h3Y64r!b@5g@zX^dN+Zx$}+d7(NQzbMH9WXf3Sw z;>*c4SCX4JJEMyidZqzYIu$uGT7+T>lF@kcR~|F8G331nmN*{tF#<8R=KOW`Y@Cq>`)Jn7KZ!#EkH15+ z2#D)6@T+2IQBY6@O>c133A~!;dedD~7{Gb<$3gZ2i0@19ywVLhz*WK5_h(gY=l}lx zyDk7ZF9l1s|Cx(lbsRHYJ)dc#b)x*@$yNDe783?IJa>1#z`Ff7w~NPRkLMGP z@oVKrUJrMCl10}!zc4o$JK00^+2n?A&|BwAO%fU;X_z#_;2H}q^LGY>urWA?(S6If zxtJ2DZ4+qRU~7j_;N%K#g$#?rCBu#z!`*ETdw@=YaC$1(Q{!86rq|Ti=o%ftAGF!d z?Ia;-rTK6kSXas4=kd>Cln5IS?;-H|;pOGcdyId3qC`HPo!hJat8x=yLZffq%tz)B zUa2e)1Mf$Ai+xHZ@k8uqgBa0Bv0yF^%JOrlJ``*nYQ#PHFN)fBcFGfV0e|lyZ1>m= z&)A!^Ik&f!bLex~Y@JGEx!glRq9>!baXK;hjSsIZBw-&2=ZPsoN*tw4!{|jq^{kV0 z3uDjN>vlLQ15vDD%b1@@3r7+4m~484=pLF|4mFw5TQcY|riExY`jSfvgf+1mcFF<% z5G!Ss5!V=S;Yz6F2f87i!aOq+gF;R``c|pd36y4|rO#i@Oo%zGLhx#X{_>?*(|e~0 zs>>6xJWS<$JfUt&3%tSzN7^o1Y^g5kNpeFecS;jK_=izz*hs+N?C>d{>+4DQp^NgX zgz}e<%3(pLOkv-@U*=}cZjbTO@x}3_i8YDLBULYHKluLDSQyT?&One^?KSW$mr7mt&(dV<#65X%9eI30B%I66Lly08|uUtaR+IXA4h z{1@o9e?+LI=D67$xVmD;36KCszwsOZR5U9d0ycjBsg)}2(>vd0@a6I+k%si0UAxsR zgS)!M^=%7#XL?oLz3`c{9Yf30KEuKX*Z3^T@SCCpJPmwSk%~id4M=WwrLXirbHZ_n z@m<|^im%E10Qt^!!d}D$(pXlfQkN|Mg#1~nDK`^r3O%Hf$!4Di`!iR(&jclR4Jv5$ zkUP{kzCAP0=?aM8H6%wQQ$r=KGImK9XMPY2jEf3UIUt>-ZEWZm$XFWVR{lw)sQbeJ zvs&QJ&D`<_#3_|wmwa;zq5ZmPUYZKmwaYNCbJvCU5(UAJi+Fu%WkJxj52Eiq%pd>P z+T%C+-}Wk=?}EQekZ``e{<>@&H*<3fotcT%cxq+0o^?H(T!C^qmw(5Y+ww+jJ09ajGsdYrXfr<9&3n*nv?ez4--$MO|1nj`j1dow;`F)wz2KauH&lY{McN9XO)S%k z=>2#dE1oJ2MHH^wafRn$jo0amGg~L8oQIHFD+iAZ>;Mft|A$rhj|WDIdnG*Qc$S7% zE?Mp|vSo6H(2Ud>qQeNI9?KE<#Kf92gsh7ufK4doy;nm#?mHS!pN6xXxIXT%$JdCIMU)yPa3HO zkJ^sbw>C;%B{FuShb(@>n-YAz{8+Qv)yG-PIgTh+>eN~=RKTMlX}I7MTltfc*zq|h zDzvHY2)!^H-v#-l;vw7YhS;&RHemZ2=)s|SO+O{nvY8t{CIZ~9jtDk~8O^F6`)%yq zgP2Rh{;jyO;mQ597w`P2&FZxJxcX+rtmnt6Of zXQy0+#@^mYw7~tExro@{*Z|emy>-qeBmPa`+p~xGXc5icQrTt??Nj<-?`i=ZbpKj8~hTzrqMbJ9iQSv_`MZe~q3F_z4GEb^X10 z&0pU~kxospS0Fp!y5G1=rWM(7l}vi!uES?RuvST<_d6&`cqy7)%DZ0Oey75~9HtOc zniY6^wp;`q*e-u`hd18vfBt)og3(y(a=N&`Y@*aAn=m6Q}#CwHdGsq!E`1kpj6-%bc4?oh~(#Ca(F7$!8#%5_h^xZ(2zc#P zu6T8mz`Y}sr-LxRB&J6paP5CroWi?F5kB>5T&i9aZyKb0-<75NfLpZ7brMh@{dk{n z#$S2yc4XAZ()%UH_;UW0IrA|(P>-~=HW;;F{{CuNBvx^olRSa$rUIFw5pB%;hh2X; zi+cWTD#qK+WtktWCpDJjeA~1B5%&N8#` z8p|K-FoTx~ZGkIk+aC98(g?lR>HwPrVebN>6-YR0(fe6#-2N?CtztNb-|I1U=P?$l zWxmd!V{nSO51C>ZO!eDyqLwCVx|ylSb9WG?I60fw?I(Z`hLHSL1i%qdG{D^uXsNOY z3if^Bw0F83=aoZ#0XM#9yp(9S#urN;lZf0wUE#P=7Z2o2h2}j=tiNHxo-0NU491F% zDZ}~y&_oAuFE-=-C0EqT@y!V}!^-gSyzu!HBQ}9nknAqah|l*2hV%qSQmp-Qr!~;( z>{t@E!_GHG8qhk}PdD^-*5>Ari7(2+-p;gSNi3@FurpPLRbELCYZ`Nbvz*IAoG@K$ z;)WIwe7=C`>5*lxO#Qx3QXNaLAlD}8fb;fkd_%{(sR)ODDxrF5_`P_!w95six4ik- z2R-}t5S|pmZfPmWn6TO+(mc2*hetEyrd4F`f-&aG3Nc*6S38Ak|o%*$mSFyGYWkt1v# zA%V(baC2tu7mB|*9OssD1P#rYvVsFPU8q}9 z*Kg_{(k_Xu;4{B{2JimucgV*WO<_ub&xJn;DhM*k7ppocJ;?I@iHr`lt60pU$^ z2L_1O2q%?NlN9m1+X;)oVnCj}cdBJgT{C3qcwOp^_iVFf?rd^^Hm=gOwl!r(+QG8N zs~>GI_RiA7c$<#2E-^C9eo500nZ#bOa}QX zG%7S$-w4K*rTfslXvWZOyS9DMpi8eh0#U*NKYW&RFFr#59q!Oie5yG;&OOG4O>Zbj*o@FRG}*ii4Qs^&+1){fy*f>JfAPEI3eA3kNg60|;B65&d5SLI*@U$v%WAu7 zxAogLwRp=wI8Gk%Dp>XuJYkvG%Y7g-dRMOYfwXiaF3e>Ii`=5U_g=xSQ@Fj?juHDD z6PI)~jg_DQQ$jz2{x4SSP z@QKtI?wh@lF&q15EL7Qc@zY>9Nwu@*kgmYWd%oYrRo2NU#2gM>Hd1U+L@r72g)GcQ zn6Q(q!Q(o{;nu{$w~3`>1Q=5Wu@}?%32l5K9+}W;%n5U;nMK!6?@nUnrO&_(RQCgG zS=_cHkZEe6u-u<_2gC_l*JN9#or_}WOF=F{K930qO}PomamzzDK^&G%-sW8s)#Mp$-*OBT!igKNshEKZG4bcL;KSkr|syl{tUw6D;z^ACtNdOrnt8A%ORU zL~UogeWgiRtr5`s}=;@gHG!CL{x$J{wr`;1AWANX0^ z$>KjKre3CrvmhYm0{j6XKtzerFj*AOvg&X*abIOW0h&b8v&vo<l;mJcRZU^REfs%L7kK4qg(l2jqjjzDiA*C7%Gj09xVWImVY!i*Sz8BxC_8ur z=^LiRN2Nq+RE0_|?q7!Cdv%k={0MNj9<~ujzcFEtV<(ouCdW^r%H@l0n>`>bT%JQy zx98hL`rInT)ySAkrJDNBc6G`L&KAZ<8QkibLDSdaG3}-&sZX1W3v$vwyfn2$qB7De zgYD+|e=fi%$SUT{ZT`*5_1yIerO;JubFCum1>${x~ExbFtjtNbJHPGf=-Pxfb9 zxWgNmu~n!ug-D0@%p_)7SCyI}3JyvQ8hj=@E>j8Z!Y}H&i3`jh*&%Kuetr*44-MU! z>yBd63G+`|s4^$-T~23#|CoHjx=2ehC*||;@8r{ntW8Z>Ij>cvaDM_`2j(uI^yG^f zs!KgZ;?$vuL} zk-kc5pA8F_Z+e+^-lpodHaw>>J1q3`+`1ILK;JDC6oJ;_ZBUO`1736m;tF`nbEJII)= ze$i`K;=oLjim?GN62_6b`NzEJ-)EFWkWND)w$ zoqMmnJ2q}O5=6?D7UYNR-$r1py7%pW2;k!8!rdhJ_QnoM&&Vs0h@^MZ?oex=w7cQb z_iA0+STF7XNnFVaBwk}=nMH-8k7edVVLe0%3}99>)ipM>x_qCGZRsApGWA7a2$WI5UdS{0Y}hl~_FUX@HH%_)ia zIRdkE(#Rm0q1m2WGO5{Zv8v8@6LpzU^ln z7s4qGdE8%Pzr^MRcNg@W8`|2!gSM@2rz762Uy2D3Fjj>dlx_?a!^qd3KkHw-PRBi= zUYE=^Z_Gbe*QKSPY+J;&ffl$n6D^jBc2iy96KZoX$GXppzxQbS?O#Xxz4j`=dYx+q zbWWBkjO9tcMU><0&XpKqcMeYhrv_!m>+;#I{qiI=KGAHdZ87*k7LGhl)mgDQ(91%) zdmhmp(W;iWA79;YR_pP(<%8UERtpuSvhFr8IG&}8HNI4Jar8C}{0r(+_(%}~6Yo%c z;tEgFG+|9w1J*NIF~w(WGFNLeC9;9YDu4bR?!o%cXno&erztOQB7+;%XVvrN!Oj*B z&S-eb@y_7WwTNL~fQD{Mi+SebJ{VAO|zt4Tr<41 z!6fpDKdTA9mHKTwBR8kbkRlh6p=BQ*g(w91!qk(tACm}x$FqIeYx=2XY zP)DhL@~g>PDjr{rX+cKt4<>f#SoLMbWW-!SAS42gBd*(y!*o5fa=CJ3IHvGLQgm?r z2%<&vCYMe2%afeIPkZtaXyyR|`5fbTtv!cFss+*%Oz3BKjE&VW!(o&i5fE%pt5*I% zyYM%I6Z;nBf?SU?_K9DNR&w=$3GL#|jUIhlb=C`G?}im`b3ZuaZSH#i8Ezt7-<@}f zAkW=aZW$XGx@7m71qG0Yum)Bd_QF$S`KMP=PY2qHsN9yIKIxtZ6sVZ+5D@Xe397K* zB45#7P+I!zbePWiJmg)nb#oR!tteW(4r3YLySoeM)Z{c)aye=wXD3MFDeezASdjCOQQ_ks+ z^wvtAMrn`ZaP+}ds7hM{6+CDobIRIZkJSP+`l@a^(vqy}aQ-d3WhESKO2Mu_$(QfU zA(^l8Mnwg3&%`L9Ql~pOr%^=i{w6`s1Jdjat(+h3%6A?9pA)x`yx>4X5iCJ89UH>G!N~@6e#%m)oeHG+)7f-L=RR zq4-cCE#G%93&@TyMPhr$l&zOeBhAEwqEeF4Fym`6eNRuR>&j=9wB1oM=zBUia$k=I z$iVOl-Trpq?k_A(qG99;7^LcaGi2vhIVwav%=)B6*3NDE)fyMRM*dUei8 zIiRSU(3|R-+Fj#+w%uI=S9RB4^&L4h&X`$v`QJ=?;$0pjwo9(%SAv&&U_>X)^5_h| zNKg?6OYG(S#q{%gZGBirICwQm=zzQZP~p=yYv)o((f*}Y2@%?@Ac-e1DbWMIZ0zn< z=r9<)5g-_orCr!}xRNH_^MV4?IfFNbSnv*;BHMZh%OS!XQXkbSB{RjS`=g>i;t{h(l`(&Q-+<5Yh=Nn3m)%z6kT4AO5 z@gW{;L+WsR;Q#QK0nR>KSq@&>icbth;PsdHI`^kzl~5-^jaFaa!`ULhIXEoHNV=mX z^30|QL7hiKnavRzi=DxbS(Qe>Ya=-n?b^6vzOFnlG}hBDQN5UrqDl^q!X(-vV(l&% z_h~1lYV}nd-?zn+S(%Dj`g>-pIA2ttHqV^fl}>}6siHs3UDuS|SY^G#=;}1-J~GVP zlDpEt&e?JK^-mlNtB=|^)PVqV;wFTLpvPkI#~1@*fB)G$Kb!2k7YFOrTEAC73d@y- z$f;PQVFMCbtJSq~J@n>hqh#?l%Y4{Kf;rhUki9FDDBoi8_>v|fhicz_07&Qk-O|pv zmhz=9=+*J!qyE9q&$~?U8MOE#Ec!SaI_v*vy2_}i+OCa)gmj5?H-dt6Hvt@| zT$(Z^hI=+y1oI)km91@@ZfZ0~VJ}d-%4gc%*`BE1UAr{ezLG6%sj_ZrJUPEJ#>&-< zsBd>q*wq!L=JxlC-{aIfy>l%nC4ODlo=w6g)7wFQP# zpKr6Af$!asM|u>*Y;3?)^^g@S?SJp7G#r>?8{}GF3Ocb4LC-6z_lM|x*=m!=kIA!# zK1IFbm>VXy9rdc&Rmu;Y?_v6@=KDMRN1o(sx;CwmG|b%U&3}4Dr?ojg;nNXdDyMsI zbds@d=59}nV%UxMSC$cANU-4wriiVSG}xx#OxC zm_oTT9KD*Kxmq_L*B;iK96)M@)mdrg*>!cHG>h;}?d1PZKe-X(q84DNtLhaan|JdM zm&QDuo}R{RpQ3k!oEIwAulF(X9-p1kqp~Yr+B_tj+Pf}2_I+44CR?vF&#Iw%LFK0e zHp;JsLfvMu3|s1xveC0HU&F_ocoTk85bg@Z4h8(%UUCu8_s>vU5~kT=-71c-S#$L@!lp=Y?+B^DjemxD6(RF@x#5r*m+J;whs!QbBbGR_P)9 zke`0M9&S@Ibr=H*L!Zg8(+_0&Op2nsDw$*j*UZT>gcc&u-C+A_> z&G-bo$_n`YQ|2DlGG{X#eQH~%d3Jq0V?DYje%{{cCnvUM=8Cm7HKF^SHumJ7#9u(& z4m>8#&NH)|*{P|yI;<+J$&87z%~f(h(DHYNO;0kwLJZc)*t^%t8Z6dr;=R!1A;0cQ z=gJ>ViUBrFcYxj<%PmMQ6fX9Bra^wIl4Sdp=nDx3VU!e*7XG`J@_%UU=;`U{oAQ^A zyv+|K$9XmB&I5F7|?nE&HPsLCPe0n+Bh2Se|+?>bdM z>peU#t8YjI=RfAwwewS{K?Q1VIVeChoOoC$&5WCmnAzR0@SLq(OPohdk8gSIVFWd= zrM|cR?&N2ABlQ48Z%$|%jobv^{w`))C+<;W-Z5F{aq?ym^9fAG;x-a(HA=O(wDjd} z0rVT%JOoztMI7ZdJ9U(%kd+lA$of;+ig)yGH4Rm$D51~+p;9!$OC#l_z5EDwRt0Hy+zz#XPQ7(N>M&JI#Z)+}!#E3k1v$?0SqFxfF8n#ON5Jt8c-5KA?Ii5{{jD)GNrHJMJLgNkqC#FO4uC-nNKi?NAHi0C027@tQO@VRr6fk z#Hi0w4;$8%Vx}&Q%xoBgBa@Fav`5GWTpgZ4>3a!(| zbZo4uY=|e7tb}GG!LpZ&uZT(8QOO#)=dh}B{PPuS@NsWcBr`liy073lD0~h4)x8h( zFX!rWDPqNkCnhj}Xc@fs^b;=0V&m1dO)W`wy;zn`dRRQ1A!7MCMw8ajBk`00a9I~w zyPt|TMz*)Xl>d6SVu73-0D~*ooJVC#38h;5$Iq~xDleVCaV7I;T;dQrN$&W}B06ao z4~c2_Jd4^8M)Gm%d3|`Jdf7^S@E*}qw}4=Kfjb8gkxJF5ne2Q!7&VCA=QEV1hYq?d z>GtyUZX!#NAmpd~@FN~QDt%Ip^}S?v>LZ$3^~k9M_gdNy^qQ!LSFfyfIYD*dy70Rc zWx_!DL@YZO^8%c9bn2P>={{`?+)LY`hrd*Ny~u!ygLAL>@;-5O;+ub|;%sPw;6@{I z>l_~;&M7j(z)Y-#U7F$=ZY+~UWdl#}ZIY6EB;TLemzd^`H##Fc;l0%xMZpw(dz z9!TEqgfFz%wW5;*g!?|HdSA?XOM>ZZzhNBo^@T5AL#ouow+L6DN}3(R6S`~5ZRh`n z4q8)*mKs2wo}m42bu2j$7-fr`{e&rl3{F?}DT21j%51eVnCmW7i1*f=2y`SUN=6^8 zJITrD@;V_9X*|iVMCgEh0dQLU&hXBXZqZS|J~yhB0U{UV%~4u)`E~utit|=}32-$8 zW=a3Gb+Riz0IF{c(4Gew%5-f23k?_yAk6|2q>{N)d*4H_MxK(D2*R@nU>|=jS@AIFm5yMwGyIZWL;3pksD_C3cYviNF!0~pCjGmf{;KW3eu6Ok#*ym_;- z-$ZDG9x8?1JBcp8nh8qc0FOOy@oH^MQym}x&p0tT?EdoQS{en7;O(lL@vv zsWq9oMGT@VK3%`FKSE=pDNv?_sQ%_t)vBv3+|-ejg`_>O1Qw%uJ1 zr_~WQMEARXQvPf(i$D@PdIw0Y0rYNnf-g&Kk1Ug{Du?RLOD$nofiKR!p*fH9OzU;h zyBF_$_<=bSBNxf}b&dV);$!a0aR?ZrO#Oi{fy$jJVNBMdHIQ&JmrzzVS5>M)N#X}q zkmG=xtt)x1pC9~YA~~P@)+q52FX=w&#RtI;>kyF#tJJxeiZs#L)qm-Gfn|qL+3NZ9q2qsINlQqs;KVQYAM%R}&*7Q9DJhUMO{1l5QV~$*0+$D2X^g?87 z&Vm!;SvzxU8gH-w={CVT4~CoeqFLumlB$-ef>w=P=k)mNrTDO@#6);Ri8YrE`L!!g zRi2?o?0Ir-h0K#4l6S2Q%Z72!O@s~^fm!D6lPZ%NfljCgcoM#|R|fm66W-wqNHdY= zDjBK*c)-QL3Oz#cLY)Iy+J5)YegiqIyE{`;Kf2UNSh_zzBsTiRA?)zmsC;Pi@uveb z&GB!EwdMiO$?z|jiQxvwo=?+`2Wc6?o)71BUp4u?n`m)*s5bZ}R@KKi9QR6btar2o z>v`x_6rOk+LfuJ3EbFSxZ!K|s+Jfhfx-@?`X1;u+Ezsd(a^`s_rzvdYRJIf3);3;V z-C0I=Y+c2S^iPzXw;aav53RZ2HnrZbnLwFGO=jeiWS~sF6LdXJxh!q?!bi92`+H6v z{9~kM*SpS=!`*UI=gy8~ud$Z^O)ut-1c@yV436N`1)mCmjE4VBDS$c>NRP3zn_$>3 zR>%Z$4G1{}tU+uaB6y<3@g(UE(<)u|x9t3XE!~{d0UCS0A~$bMHF{yenQiTnl1Zo*GRN zz)|Gw<3ddHw;yWISfs#9GVIXzW;&wqCBfm381SVyz5O)0?#GhtzFVV2N17JN?U96{ z&$h1jSQUOIa{>|cIIh-OxHrpf)(xG-lCN_@&Ko?^cN;EqOpZ%mI6hMt-1k#XDN8so z%VfbWI`unlne07hv`?XT=TbSE_D&b8l2w(uDt7sKB6uY4T?^A9%5sR*EmtkRuQI-^ z@nm=>;Caxn68U+D53w~h?OL@)JLsw;ph$yvQ9^$CC6-n0^y2vD<_Wgd1*B{=~r6HuRxP#yEs@36kfkTE^vYP$7!(Q%IE7~Qev zTEP;-It``(#sxCfOoStC#@KL<1;5XLnMq!j@@Vs!aQNh{$1xI+Gp_X~;7=eshS&<^{0qdY$aM* z+3GM8eKWWLN)Q}R&n#k+j0A&f<;K0A>&S$)%8V=KXEzlI7&bdN@|0#dDgg(wt-Msd z3UkB=u=8Ql^~wki?w@Vn3EFZP0t_D_A|eJxPFt%OON-*WmckosnHS1DfxuH(L*h|I zxG(pR%4EIiT;1^Q@$pF&lxzqg`}vqBy7Z*Vc$zp$>gctF+(JD)p&dti?Nbxd3Nq!v z(+UPVShY$W3t#l%g8d1!_|k7X@9&iP(Dhkym*2cpK+otI$$sXYTlcJzJ~FNIpE_m> z_^9ui>%A!j&^jy1GUKkxtr@&)=f6~k$dSFJFQ%?f+1Hv6sLU6^Jwa53p5JS(0`y}Tl2wI z!c$#iLfEu_@K}TKQ@D`ScGiiB2#G@GoTD5S!&UwiJ151cOwd;*_bVm1z45W>5o%}@ zBnAeBr%y2@&@gd#~1;Z`xvga-pk>u&-N56PGTRP@`u+vZ-< zqq8d9)1YP!*5cPA%N{qG*)Y7V=$#Y5#QSTgYSXP>LYXaV$n@gIzLoRX_{5s7xukmq z0-o7S^2WrnozVm*XyMD@@~vItH@DKpCvmzXPH;hc+H=*X3D%+kSirn$d@IAL03SPk zj#)ysM@Itpg!-&*uF*Y6wJ9FYf87o;7vdNsu(C4TLpAC-v^=f?OK&(s)5JVQz?;N( ze6JwZnfc>OO#B@RHb0}zO~=vlA&%t;I|`+$u6tPB#0EUV+t+r?IHqcKK7ICI`mNmm zbT2}C0DP8^JEB}W}fpej6mF(jXbBmew`3a3%qHKTPj0O`v2b`nu7J+Y)c~a@IMsrJX z8Nu;$h#;_=)T^Yj24C*7CwA!>z6Sfw1AU{i?1+Ev*y@Ik+0Nj?$75;BJTh4>+J^Um zMa&kzao7?NzJL~nckKM{zXn749oIC$(Vuy#Oc;QBuj*+VpD+8U<}ttA@~v;m3F(j5 z1HPvvk4+6Wr)~2$?3}U5l@X_z01K$A5gTy41n4FWZw|SMoSi?Vjng%1`5Fz^?+(;0 zQ@m}p?@R4`Nh!!8Z%oBzxbdMk_ggpj)@vFCOSfY7)?dI1miNg+H|XOWwKd75XR~AW z^=T;gwFI-1)c7BGA}b&(C&t=Tj^QD=n4F>zrIiY${DG|c!Pa^O!y=Jc^*({)5D8cu z)ed|r)-@cbuuS~*{I`gf!Zw;cqA!j&Y%nBk2Kk9k5p9(FX*uS%*gZ-1}b ze6LlNgOVUN{5duVyCEWw*j)B#-$uCFScY=kpvsLH#8~RDN!p32{yJ~vGqq#>d#vQJ z;4b7lO~-ABRjH@f-?_yj&~kXs!`s3_9DsXOgO7DX&E-YhpjByHnx{zhZ)V~~)I=`o zu=eJd%_d{pm&;GzFi+~UlA!SW%e1At-O3dBmE|n8DO*lKYx7C)gFgHVNqB}F&W#UJ zXwlJw_`CE9sQE}Dvqp>4E|dFd#eb7qH7XflV3m@#hD3*4a?~99(`ctM(D~$8zgtU? zLoKLnhnQ)(SEra$Qdl725k!Ej@#DEvcG`bF9*4Ssn;jb`ZIx}7)}tEN=nk9sj%bq1 z@;qMq!o#W6!blm_%pA1UhAFSvV-MB8I_nv0AH;fF*KSY%lMZJMTs)}tW~~UzpG}L< z`_o?up+dJM_Mbm3xTz|BBVCBjX_%dH>os*E9V4-SZ7z2*P6btV2ulIZ?Sy+Jp zOU&SNO7pX|X#Vscw#WANJNBTz&y}qC{M1DHQ-3O3hGdle+Q04Es~>dyHHH#2C7e7@ zPO7}l(`EWE=dQ#oZp2vRZ47(%hg!&$mPUc z?@>&J7D05c?HOP}yU6d+o>5(Jxr^Vfso3c6^_g@1w7KK}I1nZ>l)ftAUUqa$B*c;j zDzuGPCMLG;p>%g=t5B=`KNp~Ne`%7)bhH1<;793!`YU=tbh=O81CLMsQJVYw!y0|I z5&XQP!{3BsMm|*U%yIIb!7mg8^?{ygGC>D5pUB`eA&oXqS;eC#k0B9VD5+8_RW4+1 zwON3~D_i+Wj^%gOwP%-Qi1C5vOJM~cO~T55DyJMMT z;(17=?J7#Z1kmw{EbPZhpQen2|OD z{obj(H|wp)2!83Rpr#(~(bp0|?p{``m^zb2yQOvuSo>ZVKy?^Vl|v*er0KNDR#|hM zOM9fRC4A36LVakE#TfF|j8a=6{PJulJib7?Nr@awvTC?u!M zke)Yl*2VV?%g}}UCSW-l`@4YTp-#3U%6^~d?hUYA<2IBMx}TN6`fLsEEA3ttVxd%?os`~Ki|i~YUdW+RrTPgloe16U6+mBgC)d} zx2lweE`Ny{99Y{Z4)$HA zJH{l6stZT&TIvXMK;Z9c62D{{UC}&N`@o5=bBIl7i3noh^Ur}vxe|}9SVc@-f zYihkg4sr{@Ca%hP>^1*^Fz@FhZ)*OEg#wx+jN})Y2F*`dwNf5luxF+|i+pQYlR7xO zPZ2{lh59$iE~3Q6(V&S*L8NxF%`uU8kUhEle8SrXwK7O1e!r-@J`zT?)At;Dyr@rH z=EvT&iIQ0gj|g?f52r4lSn|k>(H^}<{2ZkLUa{JK2WEvvx%$I#CreM?v#ZXd1NcmxZs%-)o`3D@vRANn11x>Xs?jUNlvPapHLoEm`)#^A08jM zeRAdpWkeQG9#E;mzhjRLdtL&~vLE)%@MSHn+ZT~!1l(x;u2z&j>!d6f=+BmPa#Z27a1P1e+~n#;p6LyNfpp$yAFMHe!6Eno6~rs#rhF1H&WXvhCqb^kE98z+^{9n;j#HxSO^M2xidE~x@6K}qf`g7(<2w!au=jxqD7OIT z0jy~x_M(A7c{o4Gms~J7M0@z-7tM%P{%yRuKjhZ)h+hF4JuzbJg4BcoJL@JA7js}c zi}oLpikP4OlqE#oBvt;q?zH?kO`|p(cXZ7J&y9e6dYh(OO2;$*j;jmZBMS<-1}tOj zn(v$kq}+`BF%S9iv7ura@56uHfAdBP&h-MZSfU0;d~M|#Yq43V2O6(3X@SMZ!q65a z%ZMJV%!D#+EF^8xILd!tK-^`J)1jre$q|fq#sUtHy73|re=SXg`6uZz+(=xYs!$=_ zRiuzxy>x5sh09@2)-wgfuRndjnfufv3(UhxV=4s#F=Nm({ue(fZmDJsogjK-chyp1 zCHzB)pXz&VzA?ixiTEzetgQ&d(M-E;vjvVXyyP?$FDGiOPba7d{6O5aX=LTl@n03d%8=2s@wN(bXI zgoIu2fPL}6LJIQZ;LBr!pL)`-ikffj^^r1>6|141>rmO*=fFu74#nD)jorN$`CH*e zd!wnfxCj*`(8p1Ykqj95MD6R7P`Yr}aAtOU{J-*0$~d(5HMhpr6}1D~NCYiUsL6*< z3g&fSp@~zwTwwe=$}XwDt>s+qniXme+*@SNFERgnKY}O<;8AGE?&2m4htGCrRn;qo zL(HCtDq=4=xpU5$^z#D+wFbVUnTU2;Gtz2(F=ao=n~po?`F4&^Jpb`i>4WIavutf4 zkg1~wqM14?mB>M!I*f1eWActB-%uW*DB8B~#Dh$7hU{smNl1D`Es!gB7+pE=45uAg z2Vj2>)j612b)}F*%ka`tlOI^#i)qF*3aZctL})XsL=Q&IT{9@HTyKhTrMD@~agg48 zFAUZA1s4dPxwwP86@KkHM!nDbzExDu9vQ4~ujI`;@8%ia{(xb#J6_ij({naK?K1y&i) z5qJD>Zd8gD93L)s;T@Njn=p=A0dtCi1Yk}0_}oc(oR)%033$T-?mOa_a)mg%PD>@1 z{YGCBDJB%y%?ATEm_ZOo?cANsyxX(g%F0z}Tje>l;^fg^P!`;R6fDKq)PA<18pIhi zy+gbDw;w%mR}hR1AX$6s!h`dJO=D+c0&>O&8!O8(mEB0j$}gfn00CKt@VfDqFREAn zb-iK6ny&btZFd%B&%B-!^3KfqK&`>p>xytzT=RHsSIVEa{#M0F8_dyKmp0)@A!|Pb zC%L5K6C8-;7y|h$Nj$+`S&%c3gs}ZO2k(`Q5B>Sq9kt7fMqlOMcY~nq32Fnl<2T)3 zzpUa(_ZbpyowydI^b92AbY+Dm{@d13qAhSPVA`Tf2t2rtF5^DQ0lV>Hxxh+V9cb$8!JcJm%m^Mt z<-WB`ff|^PD0amB>dEze#L6I@HOYUi= zcQZ~gIpb&F3DQ|;p*21#g$DgU;hc|EGXo85tGq?#9e^WUM;|v2460(ICH|v~B zbCleTTJpiP2ydSKvZpM7hSdlJ4-1pmA5M>dwC3b3sCuXJK*Y6hVRrZtd%7H+T6wOv=M32sf6$ zxC68o0)F6~%o^b4UVKi}xv(oN2aZ9%dCHrl^bOE4f5i>?=B4(MmmVZ6|zEZw!FwUz( zpi%QB3{9Ujj}%U!A0)ert-X3OcH$*6fD3T#gS8X39}h2l-Bac|9xBT?W?)7!OYY-1 zHb{|N($~U;?9H%!A0}>QT-HAqz1H$H3l?ALSSP8)`RLXZh}<8qEv*^>i@o01E3Ha1 zRTt8Dy9E)*$&QF@|73(PBn+km@^O3R%Ws3WB8bK+pEV|KG(A9fo1CfuOGlw+KmY9u za+;^W$@nPxCDLOj==>))`xE8YG(Nq9($^Vx2Os=&y%w#2v!9hQwv5W8nv((%C8!A2 zA^0=H9xi!T~0$4@(%@bE!z0g~3~9`gcDZW+BvOQXWc^_{H2h!3~7juO7B za)X%wiU<1>Rq7?w?<=-$@CtM{xnF1xs_Dg$x!@&~Ht|R$zSN@gXHm&g=GJzmtIyKe zdzJ7*naJ|{()#SZ)RV@{-SeRaP~%<0X&+H7wU|k6d}AW|_IR=Wq|UhTMJ0!PXYo0a z^~4?iQt3pjOH~&cU;oE8Ma}Q^P9dF`m(hu-T)c0u9lz2vn`$IZ*Moa4-GkJh9cEj2 zOha-AQqYcj2+V{-wAN@Fgs05*GC78Fzh3MEilyJ>;(HNSI}pKvW{nl^!pmhO8k^Dk zQk7yrumpTz0PUyzKr624(~0Kjz2Wdyo#S>qc!X3f5%THuvHp3QW^H}pSH&+sZi{=X z*j@Fr58khKG1KXC3FK_z_-aMp-%HSx=v01x-eOwYZ?$>u{dBdGD3-R-s5B#7H<0$d z_E3&o$7q*rz;tn4T@gbdy1?4WPSF2(Mo7-zDr`gr-|l(ylQI@bO=~h+qWrcD4ajmJ zquQQ?GJ8OSOtGS>;-hnVCCSQ;&`w%|?pJd8W_ z(5A`V+xt6$FZscr|N0nAg#Tc0lmFfjbvVD|6KD4Gw2*>iDT~4|C6S~Al`O4Zx`nNq z6P|sFfG@$PtvF+WEA^RQD293eOkyf`=CL;uGK_WMJJu{!EbvrAyOpZV*PkljdroQl z{Ca0y$=xv{S0*V3n~-$BnH}@e50AvinD1Z)mm1rja)&wL9EVtm3{xREeO*7&>(nZ` z;wRnf(&*22*FPZgh8;YHZaZBpVosC%ZlgT9`;}o0&;)=mrW>zJzzN2xF%48uG%^ zy-x6-u(*#y0o4IN`37iDt|M%dxa%T4CEZ$nXHXHDu5^EAAo64~e#kq^$qzEVQ()@# zsA!h-Ts(FSOa0b!rMELbjC1pR-^iynS?odjc^p~z-G9wS^nexo#@$lJQeP=wXh*>W zZw{aOJNy-Z!NcT8S78Gsazh$FEigsVQGhZR6vmp*d3K~vAvE%EYJbyHB<~$(iJAPg zJUQ}b5EYWY(kw|3zB$pxq^GqL+WWExBZkNe_*Vp4%8|rJqaW%G7OckwSuj&sN%BYz zGVGgiD-#v}4h5V1ROqK%jp6viKkSn^x5mhWRC%giEsadi4(F!Mv7?UKC&l~lS8(>ufJ%_r-C#2_HgH&JEA&5d8% zJFC%G2c^Ngo5x9y0S~$N?~k?mrbnAP9@6+1DY{^OCZC1fm~{~{+~~1_@yw z*~}Bwd_|J1JyU7Mz@A#!2qr9VDKbVXotKhgXADlF4hjpD!Rd`6KEmLb@p4_Es;*3p z$9p}d_<2(3Y*59{ZPv?8c49{iITU*nsPJ>80r?T2yVz_Sy9SPPsZaiyje+oOl-kyn zd1n>G(f=~>*1z?8ho<-OyI+$H-_4`P09QFjyLMsfYIr({_yCmes1x;0e3z$YqE{GN z{|sFj^J@$j-gvkNcGs3=uwpje6&@KWMLs%Ou(p=o-g4uAA5u_035^(!YMI*Ov(TIs z{-A4E&p+uO40ucJ^%KXdEWu)N1_dLL;}a83htsqZ8NA{^3zDc&KX=GN2-#`9a;Ro% zUAO=EyM;!_+!UhbE0~g#_}aec3R{hd=ogImy#e12%ut-#=hx}3$=0_EpKF`p$E;{m?T7*X$;Hk;mW7lp5D4MZO+ zK4(iH_Ia6P>LrTH`_%|j2G6gpG8KFk|7*>SIABC7>jcMS@M_qnDtE}zt@FhvLBp-> zxHFo4;yDWtZ#(f+W|>!>I5>Q#l3T^3zJ!(BUs)P8Gp<|wGYt}U-a7_fuy{D|f`0)t zSfa0sYiql=^R^jaWwPMn(72t_OAS6x4c0eJAwSGc7mztVsN^KWuzurE~zww}8 zW^S%RI8k4A-~8>!b4C;9NmduOa_sqNs=a)2qO&0QeNbmG&04HV7jEg^nGzvupOiSl z`vZEl=p3c%%6&8|>}O%eTf^Sz7}jlg4vnZ(X1 z&uI9&%srWT`{xbzcoSd&tA#5Hm#MqtF0>rDnOc2eM=N}a%(J!oo>tAzS;Z;l3yu2l zvvBSAQ8temyso{kviCH5{F&AF=IO_Z9}Txr3+;H>TvMz!e{q*fY_I z7NI32Dy^1eG^w%I$<)QWbOy&`iB=Yj#(isw+J0yvs@~ULNnc)S&3gE(BByO&^!dhf zIkTgd=H;US1QN00fc;LihWO{tt;g6ST(uGVPcyKN^#EH8tTgy|q%Q7C3-a%+l9$h^ z0m&bca5_!-r*3QMb@b)?dTwHC@VC9H9u;)HN$X2cfhdZRJdNbqkmHrTr3_;zj(?YR zdPM?X%GfI}zJJvjp^#qop1{|H zLCwZ%i4q(KqSElKd%y86{nY`Ng7afhqBV?A3BX||*j5eqtj*7p+Itm;=q~5mACu?f z$ib^GQ!5mPciE?8tLnm56>g?wS=l7VO)INS;NB$!HDi(op<~N9PP>1M`#aIjkWm|N zn3h>fW!8S&DQIbBSaX53ya~zX4W#()32%@D=x8v>fk7c9M@c#SQQ0%h4T4Uo)8(wo z2#x-1S|Ra;?i=6w-^QVIK?zoh6(%{x_(h}XBuR0gzMZwcP$tTj;d>C$!oHfUSKX!4 zmOAopX|Hyefn{8>l0HsE?huYA+5 zoFCB+?qJ6^w~I&iqHs#W?1qy2{hn!2u#GqN9!Id|=O5#$*tKqZpEsdeU8J z=tw+%8F^{l>w_=XOScsJftfI5XaDR-TD7+PmVTMVcLOJiEvD;^g@5{2!Nr*-Rs7$= zdrv)o?aZS|jYFTh-sO#PpBmhEZ9BQWK`XL?vX8p85^iSiQb!gs^ zDjz;QrIpqci-q;HcX(6fpA{xHOXX_*7{&unEk}sWHKU}EA1510 zSKQ%RH)~!QqPSEW3JP)+(z%&y=C9;m;`mqaH7IUPko;=c#|Ke<8jKTcQ*UnE zP*}cyx#FP<$k)HN_&f8?@1O{tHt%+FLEBa0l9PYmX@4>6e{zON+c{sH=@s08ok%5o zO-(Vl-cE2%kUC9~u?oE0o0^%KG2p$w_7dD9_yEVbxy8o{Y*<~|bj#+5cAZiU@IEhC zeIa^>7a!+DQgw=~y4GLnvV$AG8jO8`Ufz~##bGgct^eKWd7>0#5Z;6$39 zXPFqZ%c}OGo;XaUyRHLTg9C?9@lvVQcZYgpuSg$5T!o(4dRj1N-=TZNiz9k7uD193 zBmnjI7fiKpBz}uu=P+IVNx-VPv5vjz^O#cNRjNXF(OXTia z7)hw=+ZI$|zkt*IHk&7MSs>lwlK)$r%jWGyE4n{Ak-Z{6gPU+m`AE6w<3olkZ-4c2 zJ?FjNk5_$MaMjr-LJ+Hv-^GvQz6`3UHMf;mXP$OYip2EE!_Mqke#v$SDR;!(w;wNl zIl)%oOBpwH!b$LzjS-KxCpo~6y;T*=-li1O1Gg}1RHe~4tJL|mf2KX4M+4Ty3d;5C zCM}I8o7%2``Cw6nw5q9IV11%|>m=ZX3^YVFtk1T9UMfQuI5^P9D2)<&zS zwKW<&9<&*FDNW)Bs8W%QG8Lb%!ejff$E8gq;1+P-saX4Cta7O;t`UM6_-5so?Xn-S z`%1Y&>FQN0Dw+-4OV;ZX~lu{VKKOZrzz5bQA_o&9{ru=-dsg!4>sHK5>KPwV; zBOFQnx*ktvw?5*c7JhXUTA;thVuEF18ASr3X{L&altY%>?fAfRISE7a${-;s@ zmq|z6WIucIu+2*nz=!Ssxd6M&Hk$dXfO&083ol?^D<>z11a4kpjf8>b=}Qlmoc~%i zHvxjM7P?K(u${X1?Rv$^r8OP(kB@QC#(hG6k^ zRR}6vnPAJ$Qur{K4j2(3ZqA3;f4Ryc{O~$!bd>Tc>}^lRy5zaw?zdh)r{~_~Ywupp z{pk~`oYgQg8`zf0c#uUg!vG2OZBT3!JqIohdupuV?+{8#6v9oEAVM7Q8$^z#Kob(` zmA54DT+ue3i6%f2&fRI-tdn(hedpDn_e816zWQ3>+$UH4IY{rgV@D4ALVJo_HwO58&M6K%b-n!n}vUMvsjOQ#_L_&Agg4PyS z2A7uHFI8J~yuDp23OWR3(YV7CVwC$pQS?#1epYK3@pV?X?X{nWk^OCr#QxrW&iidv z*G)((zT9Q+FJK=!RE*upyOMdkXDaeVpCnCTfI$yxA`n$%lP7P@l%$UYYE#;t1lehPD-YZpk#)d_iY6iox&UT8N#mpM{upH!7zwhio8-%C|_95#`qM+u#D7pl z3r}t&nQz&%eSbKq!KTGmvVU3Fi*2*q$wlWPTi!hI=L>JF=z*({jM&?eFQ7-FSxf#R znrtUC>+7oHb2Cgf!4AvSEYc`uY#7u)O4(fQ4dyU6GD@c@!fx`iJ=q(s#m)2MJU#*4 z_+&I@VqfVY*nP7lpcGMiI);PdTEh_R4ZgAW=o6=4U86|hJbbN^O4_kTXMTA<-yXbi zy^|y@5guFx9*{JA8@%$KXOhl}_bd+|OtYLWJ64Wg;7o2yqnW7o)aLLIIa zLs1yXKQA>$*M%c<>Z~)}tir;&0EQ;2f^|J6og5M&e7OE?17!Ix|kE^Q! zNEjptIm;E7{T%v#mhCBZ!}cu0DZFz~Y-NbXHq!Rbt4WHkzJ=VpwhIkOV}CBG!FmxW zewkoQ@;6Jp6f`xVV9a8eezcN{yXKAmgJR?qT^vm>NXUE;K>XLU=$G9A9;?F8!%dJ> zNf!O%_W;8kyyK_DMD=m$(t{nA!=2le8lmIaNj-42?**1{)trLWVtis)1Zg6 zXrQca8;C*$yudEM|6U*+oK{}2P-T89(IZ%4XOhSfnsbg(n1w1{g5dpjOf&uxHv09| zL*ir;c_*~xV-`2N3q?x|4Jc$QNxea!dnJ$N=WyRo>XVbL&)y+Sz=pw@?Q4L>-DmS} zfl&qyPeC`j;^OalYvy2vT<*FUFI>8e&L@)%sSm^)2y1Y6rUbrP^eUa5Bp;ID8EMb_ zUtulKsy3RULnYnD6Obp)_?giOQIkR+8S;6@j6q8Z9#7@(EW{ZmndDlDBIDs z2CSJtHzQubN4E@DCSwH|QR#6VXXZ{}#Op?V8504Chdg6Yt4Z~;dB~4UrXiy!!KGA)h$xtwVCYE)5tb+a>XJQA4pC(v{BFzk zI#*Ygb&i@G`>LbdT3#UYm?>G<&QFPbC$_t)HrR`~{|=}6G}qjk$ zwGvYrRt?^blgBo-sV-{H*QLID+sFw){)uHWngM&FdcU8tXbXr$xHmq5C{jjX?3c#= zpd7`|Zzy!bzL;;?HGe^g8M3)`keP7(l0Yl$ui}@I6Jucs96qJ^zlX2Xp=g=sA7GJO zD`nr*jaJ5#&9G3*1Fq_nd7fSU51I$svEyG&1_QBjy$BkSpMy?xLrNMCD)l0VijLU!;0<{{T8MkZJev`EPjDzRRv2igl5-m^j-ZUS}m7U&D=?&au}8 zJpto}t`t?vVRyQPe}0tjloTS4wO;8pTmWg`7>z6uf$RIeQ6IXa2iM;NA8Pk^W}%KW zzVssx&CfQR_VlCCHKmj2Kgmr5y4Pjp{jqttjQDY8KIOcrP?)_!pUFR?P8<7;d=7`yclg@8 zS&H|59G*FG+o7?D(HLgNzP~Mi2y6<51&D4?0cGy8NrNw>7WFfisLW5 zcLL)_f1pYz!JIW`j5vNQjvwU(JU5;9S;u%zl{J@j&Nc-;!{5Zuo zop_+qTGx)0^DgaZ6&cjJaBksWDZkIgS6jaOXOjGn&i7l_pvUxaUpjODBNn9BaZyui z_gxXmWEAbTaz6C}`U`HKC-m&Y#xO6xj;*Iu4)K_Wua;{gMx81JD35au5a}6mNzIjw zJ`Z*w1&}bxSkw=;hr|5ilmA+eZ#CZk-C3l^TV|ICxW{PIN{*>|Yx#F$A_2I{v4Ta% z8)>=dMmpk7tS$k(_F!-A=$B+%lv_8Mu@HjbrfV8;MVwp5f4&J4b#r{fN%AryQ=!mw zn4tf0Z9~O}YI0L(ve>}VN=U^cmFS`?cJ;=ylSZNqU(KEBdq&z!1#_0vmn-2{XE?wG znG8S>nkuNXBpSLXkV{m{Sy)-nKE=q`;dNz>#R?sf=tq@xCNdkV0;2O7BQRNiF1(|F zSE2e=efBF$w$Ed{1tEH6i6Lv6OfDFeZ<04(`*zFY<-{|H^+0HT4V*#`{OelpV9Kvm z{>)l1k`{h^_B<`?NMM+Jy{A8(p8O&Z&_ZQDj;+qP{qjosL4Y}>Z2#@;;l?|;toE@xk4&+cCL%v|63%y-nZ z(|@aF8U+1}l@foZ35OQS-GPf$ZxXXDFP4mjTt(6J(!VAO$@9=>qHiX5Daw$^hcY*D z#>mL?aZd;BlQ8e}PmR2CT6?bzbtT!l1wnvKUrm=odSUisYp(3O9t3z;VkP%b%VUxY zFQT&LR0h*S(OnTmB8DlJm2gx)WFoQUKGD>NI?;400YyClm?iPLWnz_TTO3# zP6trOQ`!3$t#=m!4gqrutW`te9UaT(U{O8xVma<26B5Z5>pxB(enoj@wA55qIz0Ai z=J@Se;``hR-3P_BTKvIq`O1;!-WLARfoIn<72z2r6!W3|ty19c)X=qkDeaZ;j;?Ct zULgZgSe`P0iBtSSfKi=`8q^D=M&F*s1E%e2ahKxozt}Wc7`Xt6tOUm+a2C`p9mO9l zG{&h2f`rcbV?@>^*XMHlWjKH>r4Qez3}HUkf@pKq|K!_W=CLbL@zIie)IyyBmyen#rTNU39x zG_XvywJ=7?_{%~HX5>F2B7IQN34O()s!fQit-)4pQnc+_LyvR37^4ILt7H?6m1}=FCN-LY2#HpJ~f!nh&_JvuENFpCAF=(3Xu_X~nIFtzByn zP6d)rlsMjA>x+$Q{12-NH`@B|bbo`+p6k)+)Rt=Y=et+pZ7)RD$;F+Y4iRi8Is({zrg;U?E;rsRyHE1qj9v;}MWk#l~?T5rBbHHuUW@eVTI_P07h<$p^|9(44!AZu)?80o0{G^;IqHsR<>{Y*mfjq`C4nOA zKnG-gKX;&lTgGg%RVh;|U1j@Q8AU+~tHS5|=<@!Umhe!4Fzlpp^K*pZ(2<7zDxFbC z=gXiwhT5ZvZfUzH1yEIvnEhVVJqhZ&{C3f8GXHW1!PTxqQAgO+fna94wVG1s3v=!* zT3gRyvOi~KvC}NEvR+`B&&>psM@2Cb)Le$f0%wW|M(<9sEur`Cchk1 zn&{2jemn|%52~^k@>!g{;0U}u3ZM51;M6(q3uP(*00)oHE7_`pFEe-Db&L;mG$-qf zIrcp9-%?Tn8P!*wT3l|3zc6$?6XzV;TH1EcVJ7$bE^uSe>+K}vzL0iG%2 z|F0uG*Yh*=(Pj!TT$C{sA)l~}Jt&T3-M{zOJtJq6#M>F2i>3}<14_j=yuJpPF-`DG z>@>%TzfKJn*XNS1z$Wcl&>iyr-OIltZ8=G3~C z=G+#&;^QMF6D};uWmV-RL*Cw$1l2id(|K4fV5DUo#200)2AggYvW*WsaM-C!?5ajUt-Rlj#pQ3-}tdsc0d#^8&yL|Jp3*p=Sg7e9-?fL2L zafj!=vvSs&FllDlylzs@C77(9nS)PyclPvlaQ8FSnjYaP$H%9oZ9f) zZ~1-jlulczlG@B~_q0cAOHd?F?~UH|%pB_9-9VTsTXFt5m@oZv&aWxfeGI1+Fw8QKGlh)Y z?n_hOr5UgMJ;si+tOnl)q3TC`w1{`{wxn&piL&fir*Y7Wz z6Y-40=Np@tfXyg|qdFz{d>+=+(>p5Y{$ymx;T;9;YEVM1zS(21+})4YexbwNx13ty z{ghZfX9c-`wy*C%-S42pVQjTWn}v7BJ7Q4?U1=#q2*-=JQoP6p^o`%GgPoCs&wY|AOe!PzDWNq$f>@_A^%C_a z=qw(t@gU&Z07L(g`t}T$n}^~so~T_tdR~vkio_*Uq$rd-JGzk|Wb!SeY!+DObDZ^K zmTE(V(3z9vS-y|iCa?19Q4_~!WT}24JKMVxlm+BH>{!U-sjNuodrEKb41;*=?Xx6$ z&!)w$CN2AodW%@b>x;5%X|Wssco0!mBm^BpTMEB^Zk>jFUGnCM;|HUVH>}J$2Eb4 z0;?9itf877T=W~lPjL708@K+|PR;AHa&prghD&At*cj$L__6xn_mAM#2Mp_;kG^M|(osEmG)8(x2k ze*~+2yzV{{5%C&-1#}um^UU8NXE=0YcTcYV|=iIpcfzs+((rI|;Yr%qqLehL? zM5k5f>PyL`lR(`_jz#(#rJP0NOu1<48+Gznj^gb4CHwjLIj}Pf98hOkJ|5RO67LB> zy97s9?bk;QxgWIu?KuGo3@Wnk2>2$3hG66M4!RgrWE|T*ER9;RU7O}sW?F+yGMs=* z^tRWT6#nPg{#m;#*;d%_6Dan2FpK4-<-yUe>(mT>)8VB4GiP(4ESJwcC7k~SvHLl^ zJHpJW+^sbq8?tMEUSM#6Nc4zgbR9&@uyPKdQx?;1!p~3`9ohXRR7bAFM6N z;JOd@t6R>}$sn|ZqS2hbbQf)42?-FIA5-R1shlP2O$(R?(S!i?i>RAK;$5~umR8P| zAIIDJX}yIP5fdKp5~4k391nqMq=K8yz0#Sd>z^D2hist!LR8!=Rkr=9R?>piwkT|b zKSEe7T=)(9_m0mUule${lf^bsanv%qy+?t0NG{@iMSyrkujeN(h>YM)9DwJsfV`cY zL8>aEh$+nWgDudn0v;^w4b{-bzYb1#G4HX+O2d++eAk*z(}Qi$E@&#GQlEaQ%GF7R zYs2+tSsM`(U3Up=dP0`OS!<%=?BvXn9V00X>0)Paw@1RAfG6{9fcAE39{8 zvAH{r!*o#deAb)!F(gH!TC{k1%j#Yv@llM_@e6$8c9pG`ZRwS#)mteFu{t5i9!byj zbMV8j%urIE$UOX=V_!6ttjb;dOB^=bu?u{B2J(BuuqWxFdh|4s_xht}09)O41CjlYkV>k{;-{ZGDEo!9(bx5+h5pbaj7?Ty>7RjRpD z@P8#37KM%QKQ|!hv0L%^m+OOg9xylzS{?S7_PxgP?y4z1SX*^A_xy)|f1oe$UbFuu zI!$AQ&k1JNKe+zS$cUf$RNF}XY74i>TbeNc!zA-cKO`HQisubcd?LPjIQ_d>LZHbJ z2rGsQ{8Um)-@fivhK4Zzo{wxJB|2qs28MemC{Wuy9~fA92>7M_$0c7>##-o$ZWRo& zmU#5kK3)hJW2)>AKd{Z^QC4ORJeUfBHrO4AWeO{_#empXt!9xnecCD$iu|IbO)Guv z$@S5CN)YenPrh>2HBRfd$r$oG(lGwu0f%$%olcfsobNzISoO0vK&vDyMQK!hl1s($ zM^Z~+)w%<;)tP)bfT#Xrlbqa6RXy;KPXGW1LNxM>1c*Jfl_0N53$=Wz=$11I-{sOHl)OzWk^zMKfS>!h01@Kv-C}6*EPW*Q!04O;mr36k5t?Gx6BHcbv*1cb$tr2SK{`Gzv>GEmZySpyeI~-QB)c&&Cy@?q}uI7mh6tW7#>wW49-#)`{C&X26i) zw=`K0VIE+T`~Lp5X|rV2;-5FdCq6O7KMv%ZGvdn79Kx z3au^=sYNrE%-*IP9;y30#ysL@>1rw>vGlDHdooEa^DbdDmn&qj6j&v9XAE!Hn*j{^ zJ`ee)zJ-IQgD=N^a|q&KpIcp@uUleVi_6K0i)cyFGS3qGSe(G&cxU`~WzlOFg@-0Pk(ghub47{>PiY}!acJ4rPQ3gz7QBnUXj5ypF+`Ccyzs{BDbb_9 zq!o`|T+lvv`Hpj+HrWD_91|>fKVvOzv<`bT4terh@bm|mFx0ylak~Br5$dtfOi$Zt zpXXLDzpXJY0TEO+5Q$xFpC#8fwYDPXi3SI5H z(j-?fD`@U>;mL~YjB|CuVnxuN-Mm@<7AVY_ZO*$2*gu6&j&Vo6ATLsMfMp5zq>Y7S z0mrM)&1T8UI!ljhQoJ@WyUtPedsSiCKQb)DXX=YZzGGz8zL4C`-`>oQD4B0^_gVPI z%FM;E8_r`wYjavA_pn$Wuc%UI58ME0es?^rM2Y%^)9+nwVrmNbbA~lr&)z!lKx-f* zPhUu-w5sqq6gSo$I$&-v12*>QDvlxW9_4M)Pd$2p-cFfS}-u`7aet#x@6%?|AMYwaoc@3g{Ad*+l>!;A)X!` zMzvOA83Q`aG??NuPNr{K;)w{83y^N^x|8uEt~z#f#3zx=HsYf_yk>KrvSsU2Fz{XX zR9&qtks79t=CyWz7utg4+;%Q98#JQx>DVy0*on_nw%J0Dy<19@Q}z}fl3H0ruIun$ zv*MJPQ9a=m4bh}XN4jPpL#WC$#=1mJRD-A&YZwU+m5A%ovM8Xv|K@mI<;=( zw30KV7JBCl4~0=*Cj(CnZ8U&?-=+@>fD8RW#UC(XZ#xPhr|c*Q_@LtfZD!BIZ&I{r zNYL>&++tm2wIvmNkBe(;wila;-}cj4?Ztr0Di2=1KjEJp z3}=Lx9L}(LPlelPxKccxE}kluqZo7ObY+sAD#D@u)+IV_J=VJ`eG$#xC{R7?r>c8> zB$PCf51XNQxOjAaCwoW^_XJ&y(BX1x;?o{eFC0gFczP=-^x+qo}{^^$V(I?0ns>yc$JHK@@+m!0`Oafe1Rxndl2nUuVV~@256A{8% zZ%T;I{G_>%85c^lr2Jy9HbN5E%S`d4uwUT0{|CtB|5*S$sroBVd_ZCn35R$7w*#k< z;;NhY@L%4`U$s@{Q!o0!us!lKfv-6&^SFUyV?CBzERDg=-^~;w{=Qm`k|N8yn0L{& zA?Af_KG^H2o=g@sj!PC?@#`(gV1gQ_BV}>#1G#jLTe8tU0JzFsL_Tk4@7;suFTF2#dFaDrhMPV%s{WMh&LdQ2eQ4U*OkFMHXr{bwRdo<8P%e_#DbVi9{& z{G5Fik51N?UD+(yM$vdDKp)*s`B`r8Clq-wg2x&@6*O7o(SH9ZN$Ky7WxSU6IGYhQ z|Iq{Mr8m6kV9C3-f?Ju2&)m@swS)vSIRmNwM5*m<;fSG(_}tqA!ly$RmLiSYmQi*wwgh7KZq5prTjw(vO@l3fuPFac}v& zd@8V9Lj+7#W~Oua#wXpkW4P@1T>0KJx?OcV+hzlZkAOa~f#A~%ePjGWQydck_rqjO z*~zoAblk@hQ>B>9CPL1b299oUARZx~)OjY|lD{t=>B055N=HFN&f*O=zWU;yXJw=Y zJ}Nf$-UqPtCeNEt3?px^xhXtD9_6ki1d_+PBu^eqbhxC)#}oV2sWo~m6MKa?3TQ87 zrRG4_orT^0%C5jQUKE|o2$}r!pbT9RhBH7&mtB;{2m=m|Bks${(}DbP{#I(yHFf^6 z>#O@;EnISNNxck+ijmQ1%sF2 zzf$FJTOudsUiA9N?Y!6a{+@ z3~1Gd9l#SoY?`-Kg5DNwKS6su3yxLB=z#8UuYY2UrJqr}d)1;wV{`X5*(CtR8pERt#4AKWXaRZiV1&;$Tg(<@_66JIYDBTC zM)vv;f9h_^p@I_{Y>Ebc2rM|9z&lOL~&@cR|MP zB?YhuRsS4yOD)gEKJp@;Ru~1i_V3M$73-x7yWd|TaP~qhLjj1-WtdlUA>Afp4r%+-?o|DH;Glz7${9?1w2L@hJy?} zu#Lc0P6o;8xfy^X;-p~u|zrUdih~eS)2)uVz?M8aO+&Ls}$TJFmPY#a9D^~ zAq8#Gh!8|+At#d!8i_A+_!`E4Oq{&&Xx`LkN?c>OL=x{P2?D5$Dw=?+Fb0e2RKc|X zzSTucRyaX{g~67|94I<4)aUiBcy|01aW=kbn%18Dbhh>_nf?eVX+oKt{E_6mI-ZbI z#otugR$>K^$iQ{l0-QJ}Mj$@SP=<;;coaoe zWAjNXT&Yvf9DwlVRnIQuCB4S&>d2S>@$d`LMARVMnW0$E_W8-LuCTb9rFosBW|#1+ zOs|pD^s-*0+GMG~AHk95cKx#qat7-(i#L{ti*4-8<#vsV_dPN`x#O-Uk|3aP6+peB zVSMY=ly*;s$_f}VhC3sus33JJz{V&%iq%Fpdi`Fa`8rE>hzgB@1Zl@8I*}~{59Fg= zJtwNp2GJ`Ec;__;gnJBE^sd%`#cUI{+@05c@hb7=b+KA!?>9UC7|d;}t;1>rKxt!K z>*61~?NYJ!F3edDfC)#K_XYp!O5WlI9GL%-5&M+#)0u~9e9lRkNsSzVrA!B!Ju6Z8 zCA8<+M+?=BwGm(U9!f+22<2n<5hE&3`wrc2zQh8pW@X#Iidb$*Xs}9O#>7{6rsj)Y zbyo~~0n!5a?e!Q*NZ!+L={2^`=HpS^5!i}DVz!w2U<$jYmSY;ZA4ws4Q1>x$dP}1d z<|DY122+_4^P&l`aH??lcAvZ;j)^#6t%^(7(9UHxp= z7^bZxi=Ju*q{i@vNrbBd6A!yp?S?cKvyBXb;k)P6PDYSHwr9+E~I!zn2G&QlL=Gr&u0p`qNGiBXKS9_T6y4)f4-|K%R ziVNvS$tx5Qkk8WRvdM}N{j2fxWmpI@h@OgZADtg(I&;H5>gA3j;q~FV#z@Qwb_XV) zg3(>_H^zI4!PzF!b|s7v ze$6s+f)7T-|GD@QH`3aEru;e2z51c)#Br#ao}M$nX|3p2EPK+blvP{zrH z3p?i}iYfMY*tbb#TH+Fb0GddSX1SjzKw+#@3Kb`#rj}%Uo$(gyvnB-nnJgW`t>DOoiRUZ)bc>X&2z3mf3@OS&Au@6FdOKF+D3PxZh-y2&e)%f|KgY@Igp zb%cd)-$PRi;TGgvIOXQ%9{1b^F7KUtZ8}_~IG4x0!=Y=*mmLt3>{ULEifY$+6A6v& z#~~x{LmTml?ip&p3l2u)f+FW{USxeOqI?BdBjn2tyBCkP<#Wf|gVE=2wM)7MQYhEIF!V)^9FzL!zImB@aZYnbW%=8u zzsq^f2##Al)9Je|efbW00zZ3$&3hA0jg7A;bT$@24Y++9GeI!BIm0J-(I(h!-+Ix# zVJ3oTHRb|(nt!`C)>O%viKkz@?|xrV8D2IZhUMMqgr9wCHkU zO$i227aU0k`Q;f+`Lg})@_OW|fPcioF`nw7#DU2E0@ywJR7>s(8x9X=8w6P+#fk?z zn@{Dj$QleI?K&RLi~Fva`BZqOHe7)1toPlx;CgaH`YzPp^D2*fmDXFClc%p!v`E+D zWlD<;CUJ;@%aR2_bo~&yv%&{u#CKKh*F5**#3S+TjWU@kHeYOPNe#R2nT`5Nz+0~o zpL~C&%Y9$v9d{n*SsC3LD!sJ@-rlP5HUrmRct)R09+E;0-B^Ec^%tx$s>6i<9!|Th z=$vsqa>}35_}96o6)O(|lb#17NX63L zPlJnGGlto&i%P%HPBifn(U_zN#zycLITY5`hs%z&g0f)P$zo)T9||g1@v#RA7(OSM zi<*D4SGnK5aiHAihuBAY7A!@FBb(u;5xE=7KP-r6^G3cNcmDMH3;N6E6Qjibg!jvZ ziLUmjs-=kSHzPf1>?x_^O-spAbnPDph!@O)Fp<#B)ouNU{|&qWC<5&__X`AWl(z1!o*W)8 zH0|E5)(| z6kXuBQgOdh^rSKW(=O+)bX+nx(q?J@tRqj`!4_5haLH`G zi@ohTaMvajCO$m%ohZy*-+o-vCak6^T`%OE-CqHZuAo4Vs-e@gXvD7xI(llu%SK7& zr=c509WsbcgPF?;GIS`7_weYkF8(vfDAfBClxmXD80b85*1`npXb(>M#+g6WV7`c4 z`Sp#~*mKEIHf=$I1iY9b^iVNhrXQfrf*}+f!C%z2c9yx%=BbrZXZqR>l{|U)irh+u zI0loGh!6IWO=Dm9&AYALBJ%WKLuLN4z$UtwFJd*G?s!Q}lduWs)I95oMY*FVS1l(R zgq^vb@n%OQtrcO)WHs>u-ZGXV$gv9w}9FfSPqBlr{Q5?+!5_aDu;iZ9!;!hL^ zkbZ%GIem9xfv5Esl?x&A@*>;};3z-|W9Io-*f9R~V9rybKO$4+jo8}Xd{U<{cqTw2 zv=EAKpH>TH!=ikj#k?<{Yzly=78@?|hkgNr?qG+b=EF~}?3BagWj6kUl7LB|Z<*kY z`}McH0S$HnfCBYp!a%7caAieCwlaQOoL05`wDfkcj|~*nMNkpQ!~XKJ@7Y*GCbMb0 z+n_ooR8naFCQbJQi3YakD0RWkJQh}Ny#+s`H%r;6+E*!C-=I+IK66?c*YX?74(fDy zO+M1oYaJ*{BTe5{Y-9l6(H!y%#&WP^@+Y5yJkqz92-!}{wg3S(!XZ!2zt3QsiMVl zzK$eK1ww#uqgEvJ3(#;^rcLo!U*B?E9C;NL@<+3Pl$-iFh2ny2iim9AR+Ovt$HU$6 zE|Y;S#0&hJuf+yIX^zOOIi|Z&jAW>at;yS68yt%Bxm%^ylr~zo$=v zIAE{Uo@Z4}W6OOFee9MHtzTS=ZNDarzk6U`<`0o-nA5VH*&cZvlrED?j%9YLBNE%I z_ALDRHVV}#qIK`WEQ%DdAO7!~xU21^Ulg)91FzqB-ygznc=$mX)CFdnc1j(c5zDt0 zeB~Q(FYZTF8%l%?2};Z9Qa&H*1XXiy4O=^{cM2)05(<(Fh9Q4)NY8A@&kmnbNAbpx z?!hna*yT5crq5iu7L=Hdt~1qIagYz89y;RGXG(qQh4=U2#0}5B7msZJqHv}x`0cpj zGCd+VG+nDf_k>bJ+{su;@vi;nm>l^l1>p(Avu*CXisx;~Ey=WT5OlRU@5h3QqY80V zjzm9AQI@_f+DCs+Rb;nmR zA>|XlRjv&t=fdnry*G=WT-Anbmdl^j2>13GKRJ$P8@wj)g5tcEU*&SA?(obyuu@7W z)SoUB{`Toh)$=RO=)?7Kh!jE=Z3_BQ(C)#Ah~yp71^D395o)J{^{% z=`AcCZuIK&B%!-DR%|O7xYy4$+s}`EZYlBKyGN8_@b!+o`rck3CEs$*If@M)qz0lf z1?F>&r_#pMh}gbj4LAice$&cBdM-Eld97;y9vRn%x#+X-x8VqoY3mw9C6V;!m1QW8 zM?A2;hR~xr#&>1TmpoN2e%#dXPFe1=`fU&EKMASS?YwB;-A_GQ@UMxYV28S+{od48 z>25SD?|`;jEP6>M&lWh(3+)sO?N;s4s%1Qu+bAEuh@Vh?lZ<^0elQgKSelIT+Nfa2 zf02;OV5RD=iP3Ff!dPSr4Y-IcZ;(m*+I&P@|J8i5mlEe*?`&}Sav{7}uxG;COm$eK zUeutEjTAA;Mr6Yh0EyV8_C4cZ5+L)NUc=dXa0)ZXGB*&=u6J<-GQ_KpVtS{l>B-%3>a<_a)pX~-ri zz}ohzyz&o&@^^;Oe-et9y*^y=O3W7iUB0HH&xgOBnBGo#w++v1RYWcgo6m^h9^|WB zttQaTpI9YYkRwK(fnjvqqz9V(N(HFbik|Y;a+^bvtBkU4cC>YcP!Q*b;tn zBZ|!!3L`S~En`YT|3CY_h+U|OE_ZdLdx-oh|LW?!D5aFA6D_O~QE^G?)0fOUlSgyZ zNTOLs)Gy=_f{FTuVWb{Bh0xFRggD3y$A$;7X}^cn-FkZ-=~Y7bU{58>PrPT(wocev zOmvA)%44blf)L0tOviGs$8W3xix37!;pUVZ>VlY2u-fD3Cud|{H~P9UPZWu??|6q< z3ChG@A2w=J=5LK{i4GMMJBNnahLAWeJR*c3j$tTTe!g}j67e?ea> zIYV27fs#X`81vU{R+u_2e0)rHa-`F!#ZDuy7sFF+3N3T9d2Mc!J^^u8uoXUGI z!Z4YJy)suwM>OCvufmoRje_P|f+?|7KgLkY$6?`Eha; zvpHY{ngn+rzgp}p=>UDly!^5AkD=EY(9tJ|caQ_)JDoOMr7t>jf=kY6z>q1DE37F_ za|*4NLL*}7*!Qk;W4ymm_W?x&nrj=8XqyPLoft&3eR0fh-{ZXE#QRG##CE>dY2jB< zY}QJPDeSoeeoOSLdMT@^=1l7-MgZ=Ov7WfAROrhwz{hvJNFR+4qYt_Db%HVDcYJ-r z_(I$4EC0-QJ$|1w7Vi^ZrryOs+jUhdG^R*sH z%@ItadTfwVUe;!&VeL0 z+@ifB**$NtPNR6g{CK^&eNH07P}k`K?1Ry^L$5P;)c);VMRxbz6977;iCA#q{QIa4 zA;dTWO&CFN0K8w`eKr{&EbO&Mv+{ScNhuWEd3?VsE&Jskg6{OCP+2ci-)at|<2}zc zSkf*A`ypBQQ^CbblGICxAOw?CJx+D)86-6!!uWgE*Vs51@M44Af4x?*Va0YaZ!?KE zquNj-^rnnfpR1i-T>#0zLEt`b!zXl;#iz{v!{J}WskcZGzfQ+b-Yy+jHhb|NqXn%H zw-&TMub~MK;7JRkXhfr5wT+K4)FkjE8iN8oV?H1|OT6xLujXHcVfF(8*=j0piriCX zo=)#`FO4^?b~{cOKe{ZLzcc^sUNC#Cnf%3RU$hw?Hiddr-UPdbU zF3(5pP7CJ(-%)Y4GjZeGNiZ3R()BW%k>JCtvxK$ znF$>hpc8=FnBkPVwK~DV6K#yF2%sG5tJASF@)um*e#z~tz+EIgD*E1>zNsan6d3G$ z=ie%C{*Y@yxp2^CI?~@zm%AkKx6etx&dLY+&v&y&GjcRO*bQ(9Ei-7{Y#3Lq(l&R2ouAxOMo6DeNCU( zNyMWg!n25mi&Eh76!lDM*R_?7e<@%q0kcTRyVSSwRg=d1p{sNm zX)=`DNow%oPeq{?lhCqCA@SJ^V%b!zYETO&m(HzF3DeqHP8xnuk9IWrgG8A92-N>h zcE2;`H*kQEdBj-nr?I(Hy!L4g#_-U9N%0ybJmH6=O% zXv9H!0K}^!xJiY^fw9B$Zj9w7+YJ%=p%`FT;Ljl&kb;+QPXrc^cc$?M*6j05S-GcO z7%^l4!D(t94`^rb!Fy8y!bIq`zGCNx)}MJx2&LvnP1$aDuZYs)tBHI$>lX73Fr{q) z*##y-!aN6WF!Ra$C(-!O?$$g%1?QPjw1+VY9^9he{d1`WQ0}4wH_1hKKa&e0>udMR zUi_vh98~+7^RRXn`R*}XigVJ9&JL+6t3$68B3>Zuc>yTeG#r9Y7aUJKW57*pNq@3l z$phX4h5FnJBSMQ*TXTa%RLn&u@0-sl9hTV-{Pliww8ld17kX4)P3 zPzTVRXF8Te%u9XXB=HuVdB~zJD8@2uFp_BWhm ztj7m5a;z+N4fBOyMq*kir(7S8GcmmFr*PGKQr_(X4{s#v)jn=BTk_KYT&r@}e$Uzb zml8k>AfFu0TwgnM551K1HsZGUE^WDpT)rl4N#v)hw@x!^IU; zPyPwhTbOm`_Uo6}E-5%uYvMMHuYMT_>n`{MmCq1LRmD}`Ulj9}Ha-ibib^H7dVxT}4C_5R zJDcfXMcN(>UvS_F>eLqIk3%Uumf1PqH5VAW!bb;-3|E>Tk(<0zY;ybVvG?rg<=tb- z)|)I;n3Ai_BT=Q@Kg!KS#c`ahMK3F~0|9h6nH8m5%`7Zl0+sQjxs?uo%}^B|yS(=d z>#Lo4zI+^Vrf1EB)_3Jzm3dr|x(W3yo*73xxA^uwXW9dO|EpSW=GnMpzDV0OSxptp z*8@=7T}48X1#-pIBXPtPk&NO_ zz|!axw1v!rstSi`Rx{bkX;#f6(j;=X%C;P?b#IH&82Q*bDUt>n$8EDbzthzO7FkIe zm*&?FknizHv&D;`B8&O~Bxj5O2DlM_JqAh`e$XIKw5kAmWr%2PZsX0@cmSDaeq(vg ziT}`f){Z+q%uI3ZUK{lEl=XdJS}}3nKJ54Nn4dYzS>CtiRG-J{s^*|NXB3@#3R>)3 zwL{)OG?)m3=**kKbebEZUG5U%YeRjx;LEJrMif z{muRYh1BlWzX-Yub9trBQlm`vBb}h%sji~xiB8U2qQrIRv_-#PLqtI8OLi@e=NAB|SpuBA@9aeXWLh&gx{2=c5SQ zk$t60EKuDYYN6>dq-fp&!zs!HwDFa{bQ#(8vrW(2Yov;ZDV!`nczE zvY#@f)BfAF_LbmoR@n7(R7`lvV-NiP2+F>akLL`u14CPjbv5zPw1SdOX6Ye{EF*lF z;m#X>r=7cm?R8-lUKjxeaDCv>Oc`kTDa$Ibx$MKDXf$&W3Dqj0HM0!|Rtd z6q40#Xt>S}?544*8f$HIzBeT>UiuH5iCD?9+HZ5kpexa_KI$m5IX4`gNsFMcy$HjU z8r#oTEY#CiMSodJqRa5#7-;{?N44F^=erQwm^m$kCQ(`V_@MO&!Q8Z z<};oe8M_6knW)C+%o1GVcv_MYw$1zX!kIn&V|QGnRtoF;!a9qA;`~Nx`D}JMmTbql z5V=#ND5l{pS?>%^&5Z>~=kAxaExNiTjE^)%ip87!ZSohKklcLORAVioeJ{-LCY;cd zX6SIGl!-a_-)W#gOLjkh;{&r)&>~q1=moi`T^bkE*q`+hZtnKZcY#^v9Nhb&yX2=l z|244rEPpevzD**834S*rvff%4-l{~VEJr!A_C>m=@BRJVRTIb8&JMq`W}o*muk&ZW zwbuf_7EYU43TPA=P{;*ge?iLw{gm{l2?P2xM;rHhzCo;;{`%M(SA6;5MFJ=qAyI}+ zLB);&D1Qv<&sJd|lBkw3KN&kufKF&7OZ_|In$M6tz%U(lJ~{Ka!F(fUARf7Ra^PRV zvX8kB?9~hfMnugX6{RG|_a?o6^}skVw6@UD&!-lT{$7NX|9Wk(B74HLVRIX3-fxOC96#R$zx@rR!%LKH zoTgD)r(i>eHJOF324w}zZZi}fN+h9O$~9`3E8@>DOG9=y2yjO_EcTRA*BtqJa@IgL zbETI~QT8iG+y343{kly!SGGh>z52bWg}36KaFt}04Xce;?A!$$6qIit%sUqFjq5GidO1kmmE7F8Ik&n};Vu6N{J|nJlVM%)(t^P|k>Wwmf1g>9dIX zlal5pNz7TVY^(%=o%O8Iexj&n1?rlzAo&Xlbo&_UeD1n4E0r=$L@xz&$#dH9m0SlE+Xm?Ece- z0k9;vznUID7Wl`u_2Qh#|8|$5p8v;yN)IXa6Tx>AI>wTxQXGdS;}c8D5r>{Zuwuv- zZwtiw36{LY(gRS}ea=N0?4AACoNrUK-dvf0DuuT-y7LkQ^nc*Q`#KsTMKIgZt~BYiXs8IM`$DscVlP)fm1&h!~c;Ley)So3wb&(^9m;|LP` z+a_d=5L-Gna6Zx#?l@4xXfI-^`BwYWm}}8#lmAtfRx~AoeVD(Uytz0@IF}P)g^%9akOkKoIyc)V^q@Ffpp0|DMyh-)@iCOjqfYPZ(2q%#Bl`;A2W@U z2jUR22C6TQWGuVW=beN7@0g(CmHw&cKO>YCx|t$AuSAf1c8fr658XeFC#20zDOJI* zXkQYHMScw!fq)RvUBJ=DM(mon#JVU6tGa4fUH$o{rqhrhsg~>1Pogq4Q1vyxtTIES zC~pChB=Ld~4EQJ{$@iD|rHL*Hm_Hzv>IXr&o7022-JEXo$@)4(ch*?a?x{PueZY#g zw)8@K*UO@KzOSKgKU_e6l2$}*_{F{6@Z@12gh+bEy{PrLfVp0z^7i~i-CvlgPKr2u zr|_1n)vas5#+y4?IXNZTl#ye=aO|wecen4QWvi&W5P9srdnq18{7>(m0zKd}5Yli& zNDBaPBBwcXc|?Sm9(?`Rs#eVM=v3fbC%iuvxM$sQKCiGv)Ch=Obs>N4U5X8Cqg8%V zs2TfyG%q?_4IlLc_qG2pfj5ETKI7VVe}Co_Vjvs|AKhH@(`na%q`t{?N$$Iwm>Ekk9nMQ&^;Xf+%{E!6Y-5q*EaOiXOJYd}^PiPuWT9nw0KL z-r9V0;#-7~>pZ8F-p!zJQ%$VL<}wj_H@x>iegu8-R_2ST30`xSf9!Jb*TmE{v@(gz zSdM$Rnw@mdPq!4fCR{o zr%s3xIXd{mzs3eOaaI!#1?8fT%n{61pF;rsZJSH%I=#0a+`!Q7Iz{9BOcxgy82&;t zMoq-UyuytF$rop3Cxj2rNHzWK`1-3v{@FGCX|L)_HPn+ytd+9%X^+82DGh~ zoam1}`Z0Id?35K3{B#a;0A_vRxm}_7{=f;VjF>zj?C?-RJVh^GGY=gC9c4ZB)*RPzxWcO(Suy8_JbE9zO# z^8Ufx_L04K2GHk4r{!fpz6BAWezh2C4md~Duh@oTA>v-yV+kD$>DfUQvLqF_s{N^fonV67` z^?>`B{iHcm8Tg8@u{#s(A)f7G2@3tX5EgK7S{Rc@P@9jO{;fr==R;N{o9HAYb`HKF*3qG9ZXXzE9~L8&aYvHg=azXHby8indO!< zv@nI6QzrdlkUBaP-KptLpmOP&J-5BK1f2sB3lm;m-63{VM?J2PWj02#C$icLat0k> zS!vm-sQcF`dzgKjm-IVxMh8gDIiiJ(AHL$@%4!;_DZ=@`(VqDT`j5j1)q?i&i7P+U zRq^Sl6Vb~n;>Q?LSDhTYDA}(>>Dz=Ed3i$Ir=m5TzoKrdioWbJzuOW3AM0`#`yQMd za=5>><>DL4aU{nYRPA$gG$Xe7*Leu~`Q#gRUeyVy)kbSe6vz+e7(12HaJ6S$g4dWS zb>yBbj{O0uRW{rd+_A5lO7ND?3g8(IViI|LCTJw_xgK)6kpmA~>bPt2_n5Gv%PtKr z5X(KK61xaKftmvnW1r?Aa;E7Bqc-h&QmVsOW5pwDFh9f)M`Krifv=BM%SCbgiMqHV+@)Y5S zfc0?Xok6*w)U$TY9N4yn;r->?MG|`x$OA@2fcu;GVBA%#vf_@8L!KHN#v5FP-R~$5nM(%arRV>R^Lh z@)uo@3Xhk@b4NxJ{YK!FGm8mx7^zgo7&sa%f$^HeAP2hP6^mhRL)}X*JJ{FXTRKrP zpewg7kKmY6h$tlaxM;x{ggjJKfTwHQq`wH{Gn}o-m&3dtua&b$0c9anO^vZ5CJp3p zO^p%jUGZ%O^H=q!aUk#T8O^u0prEbL=Zm+EP>Yk~7bMSCn12of(GiE(wwD?ifHq1pxu+1_9}A7`j7g=|*Xg?p8p$8QN$VipS>Wy>X7u3}-2C;5ct99v{B0Ue&_>&K) zA$r9B6A@`#uGjV8SEcUFjMZDdnM87bUvtn&#^ZMT@slPwPu7vS*O$xaO{@CZ5Wi-ae^AMA z9BY~A-N<&Ic+#hQWE#w2JG81L4>MhG?%LvdY9N?-JhD#c255Y^3iOsU-9;+;GPki7X_%iNNeW%!FD3}!Sp)D$ z$}vh}posn-ORVOK*Q(=zM;dDF#Ww;_A4p=^qmswtU5Yjmob#xM&8s}I>ie5hA%T^N z;_20`amF|8eBhy%o`&b_@%{;B_n;)ia8>N+AOF!Xr(duQNAOnouvI{1R)$u#2UYJ* zc)7Tursa=i2-=Q}5uM^1J{&RmPxTrtf-rAoD&`t!lPe?M=Y%*@R~lA)REPU=?;>Fp&e=+aTo z&rsnp%WW6c^@1iou%&Zbw&F)Fj@+%L1#}6ViHUPJb~SPD96#e<1OsmPurVByoazmA zn!BGk6@@%wKwJUYVSUx*#HrZ0fduhYM{{%JhwmW^3(arl@B0`Vg1_JuW*oWGy^v6u zR)!vW9lTcMENelsR^>$bj^1nDmyg5)+j;)$jj@hXoq4_kO8dh&jV=tt7dP15bd1T& zk7Y>*%!iw@V40H=smkEnqN)}XYlpLw-N!J;tcW1GL_g;}eB-_NMh$kB!`#_0@!D)# z+=}FOcam^8)JJ=da+yaGBOFe&6Ll98cT{CxaK4ju1OAQlTWd2wKk<5@|4H)FWP}3k zj-9pgS_AhAnKF0~b=U67n0!{?r6Qvsc(+wh!L52?1*FLW;N#pBO_;)=7a>y*?f4H0 z3aLfo+Z}Q7)Hu*KSh;2&hNV9bnLPev5^|We8^{a|qy}`;-7SMbF5Q71tIci7L()P{Nv z`rYyeWO}6gtCXdk`?D$FF*@a_X17W~mYXxVx_>3@X&BdxHq?BOh-8N!W7)n#YiPb;5t#zA zhI78RcIDn~``LzN_Gx>_KOu63pJXdJMJP(e$4=N^dr3k1kO5YtCMs=tu!0 z${O`Qq;m=L(P*58&ZgMWR8g`QG{9D%TQ73Ta%stKy6lqj2eNA!h1Yr@>cGYdBZ+R~ zV*i4=rP_0zD24|~uCg^PN@VYH=(wff5EusB9B_T;K1`SFJ<3Usc~)83IGnfMJZB5{ zx?`$$fX23uR4H)r9uOHNR(M`ug7OKMeNI*n&I?vRE)dk6;<0XVW0{<(bgfy{G``U2 zFtWVH89V@>%}29_$rtr3yyS@PG8o}VjTO`Cxkk5eW_gL2Wa7$uR1n!X;77Tfxz^BW zuEfWI_xT*t%lIB|#|!Cr5xmuD#aJS|WS-(hOM_9tUF}?N+gi1YbH_gkSRpftlhkj) zyvGDyH1RgQQ>T4g?+$*YxiYAx-D&tqOmGw-gW7?*vpd*=MfVV`mK^P z-{)Tawl#KtZwhpV{!{#`3UsuMq&Yr~;kro6$;fLLf5ENBd=XCx?fr{vUl1dFZ_7ek zNT(<+@@O@`?X3QzxRhX$RmbqFo1aJ&BXWegI>Bq#JZ=0T9s|MtC2>975H9UF|1%3R^)rI9as7~^q~1pF0kIw{>I@SJ4zO?`I7&#iEto|r}}blBFG7Jmb~n-;e8;rqOeg3AxexD_aBV! zlO~c4tlRFIx1@LeLD$-|rBNzIPm=X^MAS0T1I?}TMd;;$#kU{Q&85ai-|MZi+#36P zI;9Cl+nAj}PS3RRj(B)!_ex|6SgzDZAdqOhyYUMF!1Q`9zG%fSzN!3U<@jDS?e;f$ibHB3$|0d(OS~I-2K9C^8>@?aY08ic6*e+e zE7Hmwm?qLS^Zzgg5c-kFCxp6Et4h+dYSEC{>D-VZRjZTlaK^T#AWd*@Ze0c8LA%D| z+l3%N$S^Z6@=$c)wYq`yvr%_c%h6gq_k8?fbRur!!9k%%IF8u@eAPJLA-Dv^aa2sz zQZva&Q{E&XP);Vw{Or(Sk{n~MQia$o&)@7xonpZ>$2Xs5X5 zT~IvaptTHZs&g*xlI~RnzsVmOQ*|>4+Y5 zYmdBAS)@}O3d1yRn}MDBM2*{sjy~^umAbHAut>ZlN_T{UPaR%8#BF~?-?Snq^q@ZK z5;ldG(tm6uei21Cm4FAMf1{cW+Q%?~IHP`jiCFgZYM3m0t6HMjFv7_@f#d1f*78ec z)hk`F)a2qzH=~QeqFmWj&|XGlwTCesy%2yUZIFh8Oms?rF0nXrLTqShBBH18RykM{ zgI#opa3V^ip?fFpq&#&vpt|8EB+>=W@iuY^g;u_@ccTHXGbl$?X@1- z=snY2(@i9nF=t2T04PI$Q0g@EFB>FRcfsJ>MxXo=G#4=`*sk23R)N>8=Q<2uybHZa<8w#brvpK3XpUNfPysVAWu2z{>>V71t2ps*R zie+npxtN4fpDT}X99c^t&SWr+x1QyT2G7@`?%>phE{0yF!?GL_ktzwV6RV1e27*CwmNeQ0WG)(rN|XOOwQv4^8___>&Xhm%5aic7_cX+96|Xw$XKwM znO$!WqL|HV*&BOOE<>PVb?_aR-*qw4Kb?;?td7{ER*>W3=P8lsjRlyO5| z6rX6W6Qf>3Q0 zr z8f1m}jV$@GpziQ)b@mazeOYzY<foKaO=JwTVI!HGR4ypomm#H;GGD}`V;j7aH4~XdwoqHthyX2yXH-BpX zW<-r&S*O1|2)0D2j?+b(20d0?<_T(8xy9Dp$q_#e-i;%sHyjk-te?$Mxuie8mrf0B zi7x6l7nIPYd0Hjn@6XSX^rHa!ADY)5Kh>F*p#3Fm!X{m8L;1kC>)R0AorqM5$x3*V z_?8WFi&UihQLVZ2tML>VWm+%Cb_;U(x-tt7LyDl34Xe~3&ygVQ?he zQq?mT-klh{yX@t7Y7y^ct+(v-%&_D>u_= zA_{9KT|A-+Y$j`C8lAJi7Z#~cMRJ?y6L-ZK^`gE5qM3dWI(9IzBM?&j$)_8ityI^&{}xIFrKc zW71>-qj8JTf$5#!6#IoAQx^wUG)NzM7U{hko*-g zc5W>U9! zV4gsM0AiZrRf+mnsx-bn@S#`4)2koe@$PSw0i#Dpscg=~COVPj>!v0%9_W6Sdz3ON zBh1f~kaDhp1C!0g1)wG$y{SI6W&sc54W*nQ*%*cc>KPAIsuGuxz!J$3#LmeTPjs6( zAKO^Uzc_>er0WGs#=lpe7niH3b9CKT8wbbAa@2|op#|QSu`#og<|ZZ8^vHVTJh zlAo9JT1D=QJAWg!9F{fa+)iWr7WCh9eG`jmZ9u#s7I}E3j9Fb3WKrakvZfujI-O_# zy;1zuqumzxewzTR^SedS&kmK{X#y|_jEWyM=!GY+hR)?Mzn=dB8FUl{xGA)wX{i>L zV*7uncda;1JnClF={GgPM{_(>J^jExzV}F))DhV5HQGlCOhBZ%9NmA3>Z-rlnvU8~ z2AOJ(XTiKtqRAaN!I$F3oRy@=kY4hy^Ehv>ld7MX$m=Kg;ttw&VQWMJo|Z(TWGiCVrp@K&4Lu((otS;9cy^} zx=&=UFEMjBBkpGge#8BZS&t3o=06yC!_?OJu8X{TdQ8*P z{WkrfBl1tf5n58?tR7YxTRA@?(|0SFbgKz-YrMWH4(N`DDmh%n*ieJOFC%V*a(d8s}%NNU<4;J=@d~Sp`?E<@Q5*=DHeXc!7F(}eeQyRdml@k3nHA!% zH-A2uAQC#>4IpY#wO#Gy2SIy;^KO&_N#K2T+knrM16e3&7I~}FCAl=aY6TWQDSfcCSu}8Jk8Z#|u z4SY#%N`^JzBsPE5P&2C&jJQP$GVU4Im!;GuX-US&^rn(-#%eSS&p#Nye}v9{omuoS z9R=r!>(!x~J^K?C7w5ibJQFH%Qhhob<|>%MkZ#7n%L=Ww(Zp5-f392H!e<^sH$w$& zrUee!ka6$)b-?Fd`|jTS?IB)VhF6a>q9gJ?D(`?!kCU~At%YE#{aWS1X=9GTm{S~- zZhLF@3H7NToP=PJ&@eO^~@h1(+&%s+o2g+HIB@z>XtsAw@776ngCqhD`$^gHQi~;;D zCfgz+szUK;Z{)MYazex<-dg3ay=#MvzojS+@cdS@XM_kP4W9Pf!9e`M70Od$`vURp zE0!9ZzvyApK`Y!fj)sO{p-dCYc7OE2v|kI&Bad{lq@(w6p+Mc<(cR(h!;9O8aeP-T z(ZGv_FA}MO{62<%__$8=!q&mTtiByXUBKk}NaEHPfqc%7-C)t(z<$9-?7F?ZK68-! z(u(P&6%Fzz3#e)V%0aS08tr(EP#CwBN&gn)96f^Rz))vL34EeJ{+|^ zpAQKEyL28f9UNNlY(tqlIwB_h*ef_+J~94F-3hH>nm@zpZd7BK{P5!#LoAi~ibTYx z*X?mf*7)f)3U@q-gOO@>HAJftq3)w&a(N9fxx2)X2z)^1TfWNL_n*>KDPJ-vQbcZgKEWj4RZav~wp?Q>JBFgN7N zf>;Ke+W!4kA}NggmL<$GJXa#Bn-Gbei@o4ud%JV{$LO+?F^QOxTKQ1Pbhx1-nmzR88i)!Mj`D4A^E)zcUkS;cc>_iR|qVM z=a9~sojQ8$due@vo5SHC*%H}p7Ti};8)xM2LcSe@?g?+*GQH&QZ6Q{I)6a-|bF5tZY3Fvi`P*dzEl`bvd35*y}uUlZc2A>_OTw z$~$(=&%QLDmE~@`o$2ip(U7p1h_W2q9XA3xxzBu~Y0Ox-leKn)&zS$-zx>Ih%B|8e zp6V9wp&r+}OZHVD;0HR$>d}y`-r!MTzZyq3movH+pt`xS@UW181*Ei)FXy+L*uKblwTCE@h}jf{TiryL7)@U|=eS*eESruN>H*2d-l;qbp-=-M=Kk6SRW+lF0T-fL@@ za_L{5ue_JPu+Ea{Riw@2Ww2~tPV?{+(7F*xXcFAV;DcEk?OTlC^xH9Ob1It>$J3x# z37G|agVKJPv(cns>Ua6y9Hd=bU}2;4cli=kT*dze~!-9%uQmp5lFY z{)fxn^duIaRzjaMARs;TwGYv1O8C*3Et6y_*k{$LHbq2HCJGfR^+HcJ4?S z7fs`Wt*Ny&y3b0>45#2HArLm&A?U4|^r;no)bB-pqTFX0nwhzeLtZ?CA9}sBq-O@`^1$ZEowDS8mTCQQ8O8-U)oSq|NCfXXyf#=}yM$&! zt)&jS^OZ2tm=zoOe)wLEO&NaIXWzEa;5Rk)ombim%`}o2qX@lg8FBwzv3J_1+RI^Y zgJ9(G6Ghsq-=7tBlGdW>P`#E;u4SzCPwSuwJJzwLPhP(U{v(+e9WY&(Z|@IHj5|)g zAOk@q)3U{~JVY1oFI0V+P(Gk7HPQb>fw)rB;1(ibm1=4#O19CG+=) zXpM_KE@Ol}-p!wG4kqHL(DF=>P^H&a2Dz7n{oSC0b@$l%<(}K39;2rQUVIDsR1oRq z@?qQ3bZ%9+3%ScM^ek!F1dAJ$C;s>+`$5=f`iHrZ^gTQnTfa6u(VIwG3O0`U^_=1~ znTr7Om=*^JH3(w~d!{XB#gOymw#9Ihh7X%=$nkRCfws|+J-%?@7KHeWt?G7V*8S@* z=s$oA5$^aqVE?)1-)PYH?*rC0lXxdXRW0TUqtC(>! z!%fqzRoKG`&8s$x7=H#*(Sx577y~RvCFz=tdU(6;3?X}Qd&ZY1tOq|iATHg|8ng%& zunXXb%+^xFdj0VidH|*5Zz2wS6saG7!g5NEYp_Q9eQ>+%nuVW-cldC1{C&v(cnEs@ z$ybjzJ+BhtdH9r8rPPa1nHqDyz1MCMyVPNoLzK1OK-@noyhuy_aBoJWWBF)BVXzsQv83DH*~3IZ>`#-X-;_}&uM_I_ z|30EG)%tZ0EChJEi^H<(vx6_L=hTEXBBWbnYt9g4H*?41+5y{0t2(&+xx${tKNGjd z^LjyW=jI^$Mn3Sq*23bE;AnYPV2u@RE_j*G-kXjyKKX-~)k7>w7%)xzpkT{FH-40= zQaF+^n&l}{Hd#%BUql)h)b(;dK@ZC5@cn*#Ogl+$WZ8226yr2U*t8Q?U1s~#OVt`U z*wxgy5&I{F(u17&1uOT2Fiuwy_Z5_E%(SOS)g8Zjr)DcX@Ax&@+BCglJlVOWx6^dq ziJzjgiHi2o@lIMIpL1JpQrlx4}Dsjp!6djxFL5F)v<$GLdFvw0iR;YNK^x9)u0 zgf`*<+G@5Hp${`4N#4Ydf9bON_@30JGZ|&u*2$rI3;&&eOwB)#Ju|ZkdF)PdER92# zfzyOvTXb)KZ(_#{Z;Voa@wZPk^DLA|8a{ShE_zjoAH&>*_@*;Ko+b0femWwDc#M}p zv2Nux{*agP+9(#}U6Id~+kQ^H{GZY%Yd^-%U|;^A26I!Wb|EoG2FkCau&>>ub1o-N zA{b52Xm=Rj5H8{yIUZj2(SKo{GenZC@ZGd9Jas#DmhODO4`(@h8`ezze_swxBn5G*@1f_alC0Tm1kKm;r#}Y0mr<=7$EUMIo3Q-(F#2*>D^MKRp@*-j>y2_Wwm+_Uc;Z&FGZ$7X9>R%WqGil== z^I;2BPVlX`z%`URKoGj!#)Thw#?5;231z1qcbfZhmSx`ZD1#)aE_FU#^iin9WQ5Fd zRCJdLk>^?&(x2W>=@IKQ{p(O=M830Z&H|$;U*y0fR@eIPKYt-gV!If5T*#yhuz`K8F{NmhPL{|yg^8% zAWeFJ=L=M?c`@BoBED>)4>ZX z?B*Zd>?6rVR9r4)m!!j{>M5R*nRxsYwA84%e#tI#MHKTq7d>FN*J~kiarpwZWFp{u z{6=Cgy)f#eaUeupN-(s?8QcwDv1G=20Om*0oFh2q_Z~~7J7}TGp)&_$2derO<0mUy z#XA9votQ+cvRQ-j5HgilQQLlyvRzwY5c0A*XVQN%L-}(Y!{nfN4{0Gqh$ZdJvjC-X z(hvh2(r(C=$f4yLYW*doOWct>8d2X?v)~NkHv9q}u|Rgm`Bv96<}!bl^tBbVU$7}- zI-?0wOIDf?!Wr=yyQ;pc(o*m*$(Z;|V?4U~z41L5B>4O_8L9odbhG(88l#|m=Kpj| zK}Voba#U0lR7UnYPM}YHM10GXy|+%*wPT#T=JK+&n$l!87_ysfsnlp_S)ay|=QNmf zDI0ObZ}x#_)`7{MwC_N;jM@_%RuaA|zW63M3#Lml%Eea|c`yc&NuOgNBPCXfaGGdG z_I>fEJ9Haa*pdpMP%k}Z@`T({;w#_M8eENUhniZf?BHKZuxUt~>b=W5`o8)0njGzX zLMmlcfhi>Sp@Fw}O@u0$`5m+U6kd$wzkHVz?*q10(BAn=MX7)-S_4;lKzTx>9w~i; z*4@-rYvc1L{iohRn#sJ+TKL?xYI7&VPhKUgT@NG4S$m>Osdg`GP^{_ZfRyWCIhsb9 zZ$GK_SHZL~wP@x|`?9|;CEK$(gOAvPJjg9wx$~lm3!HR`knitW{|EBW@c`=jfD2`7 zNqtNT*<~SQc6|L(e4cB5qHx(eIrJ`L|G#dzjk)l>Q@#S+tf!1AW-7Ms15a8EX>P1> zpBB`8RaeQiH^Q_`7W0hmBD405PWu<%OCP_x5r5VR75oEh0ZLgrNy4qIF_qL6p@mTT zC<;gd=-{a<{D1Y#Ct7~H!?u;*HTfH;s`!aO9Y?Pmdp4oQf9+OiB^ir;i-7;SG8PrW zDoP6$oNp!Qjr}djhTbQPaHWzv>Rc#)8kW=a2>eIJa#Kt{SMqwz)43u1dpQ8>Z(XL^)n@s>@^4DVO&k;FZ9qQ%D1$e zNXd&V18e*?x7%oIoPFW!SV1^}QB*YOXJV+LQcKejn%?uYXiHA4?24QM?AFeL-#?c} zl+*yUO&5J!;bZkE&F0q=gV6Cd`&HQbQt{ba%4cZ>esR*?!5q?ppeIXJbH|?7Ye7%7 zS&=8zbJ24Yd|_4Zc*eX{oJbd_tCp8C4b1?&xSqEqMjhqnoc{bevJzAH3>2M%lA$i$ z-P@#sHIGJ(iDQGpF)g6U5`R~WP+1`N59HA=ckpZnCSvwi!TP4ISnI{2fLg2{s<2?iTv4MCwI`VPbqj|Mb;ZZOaNC?vAl|eoGgs|`Z#TYA##kkr9P)Ht6N1>MDlpUWo?1UH>sHsl32=Mxoe;V9)*U>3?dMBVI;{Q5rER-bW&iu@vb{Cr;W zjd?{=2Dnh}>PW0$JU9Ea8`Jjh1!|hOe+OpHkCKGezx%wg-xdXo$QhK(P;U5@61fRSak{vwYvgg|^eK^D)RPwK!<_~3F=10!y|#{_*GRFU;;the z7n;)tF0725QJYjvOpHug9@Gt5Qcy1BlBRO7OI%^uj%h*&zj|0nSgNGS;iH6$TCBk+ z$Igfs!un;B#LvFAM@F7644tdFZC4!>`wriGB`wWPdO$}~zeP&KcgE_de2+mO$cV3J znBQC-4v?%eT~(%C%6YiI=wSKk-rtKRRuqRItVS{$f51Do;-p39VpZEXcPULU-)(Vv z25BYLqK%IbE`+M86Prxn?S&;uhNgS}Snp}XosxGEMey^-z!_`5>L%?YH^bQo(#9Pj zsfekUlDh#*4SL7g>9uWp@2U(gM*;G^9Oa@f+9amZp&U<$I8)^@XA0(qiPi_NdU2DU zK0*B=?WdJ3pJedJ){7qzMOa#`9Hcu^*#_R&5lrx>~aoFxTWtl>^$e6_62bq zs8^4+;(wy6(%2mVWAWuEfZ}_u)428XG30NId}Z-tJ@`S%ns&`MDx7f=pE#fX(p`k% zK-t@75O?!8@s#MGLJi^<*3{|MyM&b1rlZY*B|P%pY!QprM;rpP4*TT}fDX4?Vng1p z{?rURC4f5aV7j0N$&lTF^`iy;D~8@%^B|TIBLDazwh6e5faZ-vj$BX_Oe%s3^reYC zmI@RZ!IwI)*-9z)Z;!i!^L&PJM1&8r+AkJ$o9{q(L!y0|IXDvxJ6|Ik0BmmBwS7Q!{Z} zh7r_0Hqk)H{5;XZo_4PZIFwMv={fh?z2Bt9*a=3v#Zowidc}?v6z(2%= z60S{WB2^?96x~@(7CjINlPKZz?#r+8vGQ{kC!tE>*&ApB@7GYwvm|t2c4ecv8I3PU zkSTC;793JI`h>JZ5A?3i{J^L$4I`~K?- zCeAWAvjHTXjS+huN_^7qv3GNJ3oh@2)`=6ma)#rRCVVaVzHGbQ1xQc@)8RxLz0+_*Uy&)3kF*OSHr%1S=3PvNkdqR1d*E*~pQ>d|o;_xnj%Ke2bMB3lvS zw8{TOx>WMZQ33v~ZY>v`mc|bUr4Str0;L7N`H8S<+jJ4-;oxNxmn^^Oor~?RZx4LY|iA=NK3BySnss@#>uPCsnJ+o{5y1@2{dn&>#u5pO zl6N>U>FU{X`LWjOG?_9Rir{TY!o_kFkN*By(#ErQC2>eYgY6_|E~oQJNA5e zDp3ZIk=~kv>&@Wpv5{W1m}wKn6hUkXV0##-m|c=(0fmN!Pvz=43~X!xMEa~TQ!+j}$w3$m*@ z#z4Jpg`O_=`-U@b^1B~64xQl@3ml~lLmxEsNv=w@F-jHk<}retK@~rwSy)TOwZW1> zWXwik1`qYoO%c)BH~pa_leWNq>%KheuKaBOW*L}V$DcOcr^nRB|MB(2f>t^Y^G~l( zfSkq8Tyv@{hF(@a*;(SZ1pk?LdDK{wYrh zbhO;Mr}x;?=3diyAqr*&XtTGxdFAw+;5XoFw4W~F-~a^rd#F?g$qn46C-gpuvhHM~ zAykNjjFA>T!B0lc#Ct>sOND#Ay@>5Z&G3;%_Ey;fol|1{3QSsb{M=NcH}Yz>RPG3G~?bimGJb4ZTH}; zM$N?C;g|%f^J9$fRXeQ{0MJ6uQ zbv4(Z<|k~vETcL)Q(C|+7*v{xf!f@b2uXlUM=oY>JVtyju?%#)wM<6j`Q~h)A0F5= zgWB&?Z67bhiZ=ycVHLhebE$rhM1fto)jDhq&z|AzbsI%TP5J<61bX$k1H<7@LLRu2 zasrM4OYuE*x9t(ffj3k){)R1Cjk+93!&^&?(^Md(nGmn;*N!Y*cMqa5a=|s*DZt8a zzK@MvT^^NPT)WH)IE8_?ON{Ryxk@T`A4mfvC`$og3S?lh1vCW#Qk1`SLuCuiUcbpf z=;7uhsyVmmUk+64s2Q^T2|6|p2KB=$7n%Y6fU)Qgo# z+EKVHJg`b_6QZncezAywd6)cZrvV+8zgJk*lgVB z2Z9)rx)Q>4wx~NKtC5&rN3vCi*!n=vL~clXm%K{b*@dx8^w4s`*E7HI*Hn<1lq3;1 z9AeC^gGZfEN7dhJNHClr(So8JIP)EIyWscb-IHbVksV*?gZ=Bk=ZmONM|SvGvP!MZ z2y{#Ai?LXiI`p4OlQ^@>=cxdx7s6q5dL0rhHIzy*hTeK>uuigpB$@vPc5f5{R{4x7u1e>Yx+a( zw7(e3r{edS*9=fD(;b)BPo@1SM@1U+O}ezPbYjH2g-;h@-$& zx~H#I^6qCiNphNa{u(#>%-`3gW?MAxR3`awhQiLC$^fF`o76n?B0&I?>3C%`PqfZ_ zOJT^Wqm_K;7^Mp+ElegOB_w_i0m-fJj!Z0F>EviP{Kqv(dQj-f2J5QlX%Jb}S4*+; z`r*`%uo^Ij5omfY+%1xz#AD|>`2*ruW@?Us+x3^itG=#;fF1D^y~yO3_qeGNvxQoz zuEDVRf^=Y)#VDs_F|vEp$e<+c@5yKxS0WZmSPu*Q-xdBVM3_0$^5{!jC9`b~l1I-r z8ql+T$UXUWr$vk?`C3C&TpvnA5()GR)O>A)TrFE}rLBRCM`h?S$I8_tk1u1p z4C=2-&gZtyS2c29MC{bC!d!y@2`3pQiWSU+syesAhPhok$bEygNN^2B9_Ohnm>>Cq zF@@J-Dwd+HbsIuNmK{sp2i)e7N6lm?aNA;+5DQ6duP!Fcz6dN%L_+_;;>Nt#><&)q z47s57b))Z1hGZO-sQWu5-{q+2nclJ!A4!xobm=?FAF8e-m5JZiIfGr(8HSMd$ZBvD zxzr1<-r1!(-RlO-4p~e}2M(z4Pv$#`{mYNnBQ-EAq4^Ek^SY0R>}~ke4-HMkFp#CH zyB~e5itR{T$o|u_O4^vF>nLp!^F`VeED|00i@c8AwdUUZI!b2MA~CQf&4(I%$sOK) z{+j7|dE<4>)3N9$m64um2iuONBeZFt3X^l6)P$Yl1cmyr_ zFSibLZ*;Cbg^H%JFxdTUSx6#$ib)kRBVn&!U`j-7`w~X06Utpj@&JwGx|(?cvT#Gy zv?#AH)g4uMpPkdNGG#3`TJ?+;de&`DZ#3_o2#QxpX6v>=WL*Q6LgUX-+zKy{^LsRB z%gWI*_td=$NeS!iz{clJVvfPgL=x3zMOV-p6y>0+Of&g|K5-9RezC5V}HuF{)EqJMPHi!;3qv%ZW!<=St^x%>^5p(mc;{G?yF^iX@}aOY`M3W{oEf zYZy+*U+0|8NSN0^!Ww^?0z>Z5HSkc5Dxk3s~|lq%mXP+rDiqp z{Dk%#G}e*x)}9=W1G4|qzqldbX@Co8xyMuK6Gqcr1%5;2ddS45DHMY)97S*o!WJoT`sDeKa)0csZihrZMq{Pqy8Zoh`d9;Q6UWx0VJg36gJ(wCH}&nfK9@z1}+> zt=I6drc+fx>kAcp{&EXd?YyAeJ4bbg%dYvL6`dEAdE053HWI2nzdsP;Cc(a`#N7$O zLmAmw@MS0NzCEz{k1YiH1^QlA$poZ8_L626?K;ZPJp>NddhDVd*z>Q83@LK&i3*hf zezuQJ%>O|+LONH-wK!eXqUFU~23MtTSeb;;B5WeErDPbu>V+7|p$Fu>Nfp~S4+T-? zEWi*Gr*ur}=bV9gP%=j$QYF>5c`AD~$BDT_To^raW+vL946HSb+bOd1@wDjUBF3be z+kDO|uO?bgz9C7$wV#yDFAI3)6*65AY9y10m+uwM7p@r>>lLj(=Ixng^tH=NFDCatBHAjKZy) zJ=<@1@MmLNMtI;eih>5a<&ke4599PubCF;kmC;9!ODC<8cPf{8TzjSMH3J$eZm(&x zFtbj42dUCsO&%=*U9G!DeRG@;K4WocZn%vSCy_`1Hpc5St|W*CRIyp2fVvpAf>$V2 zBbJbj;bxQ|v$FMV*Ej#Gx;u7LJHRa-%(l#@0bZ!_&1#QeIFTZBb_Ni5A!c89>AxgareUCP zVOO7qQ${wlJ*cFr6FYma?U_8TRmFFZvHm$f`$cL^17a4}Fy5rWUbwR6a8{H9>4E;` zi~0~TSWYRr$d4$Gu_L8wPG!-NpWzhB9wC{uCkfFQ*m!2CHHlkboYB+f;#kDplo{(l z9F{ZR;WTt`%OLv+$=AKBJRKw0F~BFgD<`U@1P`4zY|RmjKFkQ@`)`FWJ zXqwS?Yt`@Q*~@xesZc+ayVp2Y%P$l4Y66^#pbq8%kKpec&l8EKRE+S!D=E!jmi+V zrtn}}=s^EzN?`$6zFi=I^u;7Z*4{$5`Bw$+l>E8u)BzVm_bVhil<#5c5;2&WYnT{CiLHEd8YKn zD&#+=ko@IODn%rhzTp>1tp}WCjJ<=r6nw{gQoKm2x#v=%YEq6*T7|!8dFquUHJPJY zR`nDLCgI1r>$@iHZu}2TR~1xO(=89~?oM#`;O->2yN2NI?hxDq z1h?Ss?(R--cR#?vx##<lw(S2r>QZ0)>6lm34vIkDa z#*G0iVqNwxpp0*x08mtFLx?r>wdaL_?B+$HKI+Lr(C20IMWnF{{o22N9t*g&3u0wxAUC{GY4L=>PyV}b;+M~vaVw!L4D(xNo`Vuwj&ss+Ki~dEP!wZ zbqrO`S?+ZYL#@Jd-%OpYsC2gc0ylNR&Tp%mF|{*5+|9b9%(q1eps`Z+H;aC7Qir(T z`oSg}`q(iOTPzRvn{DO7rU0M^Z(f@YP$md+V^)_Qt;v#`#F&u2`4N35Q6DGr*wp( ztAwzY;Q;eM_{(?MUQ2|FPGd)P#9J$s&r^7me4`U%g- z#D2=YPHyK0v73v27R1e;X5wYJ><4BwobI)E*K(t~n)PYQe{48$LijUVLiGXi7DYQ`@kCG3Ji#6lSwfOdsC`${t>_x^(I z>E%nejAiK);;7!Xe&ROb6fA1!Q}ID!cK8?_C^&y0;Dz?Z^4&k0L|p>;1|l#x6>rnT zUkLY>GVXSiR)($^-KMwX#qcX36X^2YUAdS1Ml@jE3*zxalBjdT!SToUWSO`)7%W42|Xx-1-)2YQ8|~50@aX!>r*IA$z{GqB1;CD$x_|X^?H~c`Dms;gAS||mTCVax zUCCaANt$?;v|BYw`S}@W4RR8GhV@w}8b-3gUoVEs4!_2@&-yf{8FVYHR^PS|As59b zMt46PvQ7GJG)o^!A->nf-7${mkus)Yv!C)AS~%%UgZA^U(!iam0MK@TjXG89pJjTF zTD}GZKWPi=E1Cr}e_kFyvJ5!O zB%$0_I2UCum}N8EDAwz#+IBtai>^(3Gdp(_2$ZCYwhc1KjQ6Gd&tr7Oj{~_4ux5I1 zsONzK^{oMFHgmT~$@6!%2Hdf*H%mvr&VJzrC^FXvJ4JQF?V=jo4@jiv(-G^k=`zbe zh}Bh93Wi+1AJ$=){w54-0hZ%RQZijeRJ371I!p{a8<`~%fA49BF0`ULlCz&Mf2bSK z2uUMY-+Yf@!^2(Y5Oep=Zyi&#O6KdZ2+|LU zi^hN81g0~Cf&yVDRi>9tVCvdWR!z4$;J9lr)#m!2X&&h4=-#T)0(J(3hKOIHhUPJT z{MLs(0950rOax|yd`^0K4Y$<@sO2oFyZ^~VfL2l;)-<8FFX}Cjv0fsy4k5gM1XnC1 z$_KP%D`55%36Fg3rilbmR+NNwdu|V6-s{gM6$}Wk^B*uE#yVux(b#aAR2x$nV z029c;t~{E)IX@p6Pg=BuT#*1&KyCNdx66jxC=}iW)D09oaC6`+8a#Qnu|)zZRqsn2 zIrw3_4|pI>*zySvO9Hx8DYevRxb7Oxe7*_Zx93btfm2(3ukAc7-vg=^KQbl=&Ut&6 zuNPI?E1*3^ub+{@nw?by4G<#MOB-fi>$<^`(2VU!T>n;TFj##sO#QFlSE2f`I{jPu ze9%SXnSV$09Vi8Bj!?9-YTY!sMsoBmz^V|;->e194{ywb@4Pvk{~q;PpAg-2x9j}T z6WSjH>AP*wAAjcum*hv3bPf7E@^9>!987QibR}9MD>r?7Z(y$&ekJ|3TiwnJ`hJ}B z4wb7MrPvYk5dsx;B<}(~ScbvA2!?B(^b9q_Bd9F!cn=Pp1z|Wx#YBjVaCD2e_I`gf zzGXbWa+Yow$xSaDa7V>(6V`sjfzu=pXoShY-9#g2e$9A6trN@KJA40?F0VuEl^^cxCz z(^f}-f1jDJ5P+qKazHgIT|D5K@(X#c!cpZhgo{O(ff@~6+A*t_dzF}%DITUkgr^8$ zEt|XWbQ&pL{wj09a*f!vV&HQVSAl|+>*>BpS<);x9sY4;?b$rTo@X5)@A#6_zIEO{ z24s~KB^Z=PnDdo=4D!0L+APBD?d^;h$>Ls5_{w(6YXQ=gAi;$QX0*X}-yWhUAUf#& z1++qmO9HJzw;_9E*+NdzFHC;gOf3*h<#=7qQ9F_A;;|Vk_(6{)_A)a>{{E-_WEW7f zT~_ES4>&E^IB4c0H^7Zq$jmSAn9N!@a8rvo;s-k+IXyRD+7>n~2Dfd8YR=>DtHa~O zp+GMEfOOt!hEx8XhT89nUhp`Pi;xZXTDe$j9OB3u2^lSuH?u<_0 zXx0PT(SLSyIy^VmY&i<7FR+TBfQ2_z!;*WgE-Lil$nEgc1K8zLthNQWP$=lBy#^ln zGpE|0WalSuQgBNiS{a&DL^h|}9 z2o>t0!~i|k_~lyOqG3ru`#t0fIokIlseXGtAnq+^PIbrgGZA1s=%Q^Gb$G;B=S`O# z2Y&>)7ltr`kj{usseiSm(2Cq7L^bG9YyIRS3@~0(?Q%7K;oQsA!uU+eaq|yFv?s|< zTF5u(#y<-j!6Vht=I^CvlI=7YI0rxA9gh9s9mn*D!hwtO@mgKg%lUbS?_~d;GydjTt1Dp43 zEp)->ouiO|`_22-t`9@oBbL27Lr!J>Qo3`FOO^bRScaddBNArcsM|cyZH;{`h$g+v zcdZPaH!x9bho3(*3!A9t4q12BStZblLzTMRp=1e?SrrmtZofyx+Af`$EvGwahGp(f&jc-WGv(pnfeFT|vkwT*<)=);Wm z5+jT`h?Uy=&I$Srb=?^;q&$<}JA+&2E^R-%CJI^3;BYFoN)T# z$kSo#-=?TXAp~|{yYuHLe?6jxD8%ObR_Drt8#$pyYHBK=oCEmk*Yoy{AFPcHaJ;>f z%Bng1uKUP<+X&<8P10H&y=RJ{{;2Leq!-T;QW3%6;l^Xg<|$HiYo--&$?*H*1Jl}} zBLCIGz<&}AF-^4d{ZkCF9>IJV-q*IL-3VpaQzS~D_ZSBZDePnop|QYTPyHtFbvH72 z++~%y4DiaDb$HvAe#(BofZPELbwnoyIzm%nCJNOTO_I`{PnzVRqTqNYchp4NKcvj> z_!)1Yj|w`$r%#?Qs-sNqy$3g$d()=_%eEVMcu5k`np0Ca~7ZU5~|Hrxgk}3 zChQ&MIOTeNGSCW9Z$fy_JQy3*FThQP9`{9f%PJTK`Kr=hgPp`nPIU%|u|Q6_VuYDV37G7( z?th948c-r5b(kaxUl&ujwsos*ZiJP_4-YsW%8De?3XC)qeq-*N+ZK${5_x0*7;m#) zOB|x4l24fle#IgVjKy!?J}r3fuoLzIrOJ@E&D?Y#h{cNdQw2}jwNXm5LFYfy57UMy^St*2OY8jiSRE6xPC!-#C$lwnDPZq;A z$C>AG1{Z=xKi#`72F2aItJ{JSyzjg$ol354k7HEpAdYXH=Gy37X z6%y*(;IY)ntLe!qjMV>S-`1^yep04j`Fb4OwWajPPr+!7z27^0_!yOepf;Xa9t~OC zesZSUVI4)&{2J^cCkH;?;eUw_FuDaHY@ahzA3CYfOn4#p)i>lB7{0Mn+&by;{T1D; zF*UJ+_){TvdNlZ^8I<+Bz^UVqO#gLr132~7#g|s>M}dXmcRH+rono;Uv6}L#8T(rv zo#26ISyTo-l4SqbP{ptHxRD6iVuWJ3uCWBaG#Y$@gml)`@vel5$_RbmZEVdP@>r19G2ys8cRQHsM@!@(dw+ zivv8Z-k^s4L1DQDqPOzw_0vq^&UQ0X?_(x=&pMAo`wY-jo3P$9BeLF|EVo|hadQ+X z&dYJdiDUR|EFNw%arXI0?UHi^%yh5V$gF5$I8j$Ct#cXyqQ^Rria28Gko&?D!=n!?-yZ9tU4bkC%n4}ns`D0zV%7F2VcPy-bD^QuN*SG7!F!; z^}FEZK;=V^>%c7WXhH=mMyFKN_;`1zq)pXM;fMH(lZlY>U=M{2ee@peyX)7*&wPK= z<~(XgIAYVrV@40#nXMr~TrN|3{TOVEBzqQ$O>tOv6Z+n!^OU5OmRlxDbrgEzc6erm zGaejlT^4n>v=RH>1nK;^U~x=&0>r#SK30&Y{$00a2Qwp^DJyT8)JDh&ONG9as7U$$ zS^!s|b$#E?@aNYYZn_6=`~F*Pq7kMdo%0+MBQO_~ht;^w9gY{?(GJ`7PhUQ^)}IsJ z3-d+hWtG1Xrd%!ECaWlITldJOad7l1X9%}wdcMR-AcSNjY3spfir zlTin^zx_qa_9q$$cv8(HCj|#Mgr+@pACbO3?>gb5$BL>O3wP8nm+dpYhN~Qd8ooqr zZ$wh1v0PbK{*9f!BF{EH`*|*$K9CoXi+WiazE?&8%Zkd@&J7+Sh$vVRDg|_*eU5w3 zj=XOL^<9ZcEOjx3ar>WV6~=>NP00C+o2)s)z%?pms4r7AOwu;(@DjlB3{6SuD<9-2fHeToFREvj3FokPa+qaPTA;kY}1lxLok(cffH`l``Q zYH-{(U&V~1Mb;g47|&dI_ZM+lxGlU&$z{*rRT)JucddVJo0kxUN;+^!M0aSpq`BX( zAg`W8p7KFJ<+{>kY;-pilAXQkJ`S^yk}yRI##ur36?P`jK79B-Po$PWf8r39e*uOm zbju0Zk%Uz?yR_2BW^1Y$;T#7^l7jbs1~n)!@9xNHAVP$!?XXC2TB81K+OW;IEv#k6 zNL=nK>63y`QGsrC!SY05UC*-UwBOan!Ma5boIl>!Yop8jow)tEt`lQ^pd>XJ#qZ=z|m==Ytw=PXmN4;V;V#t#g!@S~IehQ4aLtWsHFR>IDJ0*^*+x z4vkPjg!e)}TariFmvV%CC#XX*ItdZ#1lP#WplS|zLko)2Ussv3^If22LNs)bn_Ob# zQD`;viq!5m+tRCiC?)?SiI&44h| zfcdK{Vq?kAhYg7)yz9#aCpYqD;8L@(!z!B*d&rpP`^Y(d7yp0K7kN{YQ4s^Bin7w-(4%JwIlulzoMIQr@qQ^Y7P8+TA=DRR zy-l%+yA&13bHW2r7^Q7}_!1=q9>z17T`qhP1Br<$zboUs5ae{r8)&u#f0S9aS(PpR zomP8)&ZQf@z?RD$GEy00tOd%JXqktp zQmt&WF^(~LnnbtVfq{3%FDENCbv1{+tR1Gc+Btt8H)1R-*DP7=_HewTGCJ2~(Xy8# zZ%h2z4`dZWd;W!dTprR7!eM(`c5Jut>8y!z(}MGV0E{~@nH_>OYu~{B=BM0R@>@p# z0Gxb$7L>3aJN;ibdsoc_9rAz~(?5c*K%DNYMoi4ChgK_(jW z7yy=w=#n(F{uQU%W@^sXWGRbjVJ#?9qDynz-dPV}1B~8lhXCAJ4*%J}Ak8otzvsAV zPvAEC5spGZclDf!*^h5b0N`Exzrlypko*1(F1_q-fyFH7z+!cb?ooP+KY87GdlK63 zV%^m0-w}eceV@VwBeKx&%6@r&h}jfHKW+TjhHGzT#S;)=ki}B&@B9uE@@9Mjs8i}# zbZd>N)5h-Pe{@b$9jI)2pr+cxoIq0FnB#Ws3xR3SO9?wNM@DF&yj~(TWM!hz%UZ5QVncP zJeBC@x*t9sGB`Pwor))h(b}n?Fn5vNdx0({k{v-@WY)T6w(_fs#0IHEo4Rzx5+uH{ z!@mR@=Y)2~`ka5!+=f22;oxl$%(qSTG$N3DA$dG0pb)q}lSl_EFaAe0M}52U660J} zK;$Pcu!mB_W#HxdR`>_WIo-Q4!kWnp4p>9_^nuA^PW7>%u5HV&1%Z^B z$vZ*IBzveMWP)nuH@4_hEa`ur=_HF@8#)Fu_=IA03z#sLdSf6WVJ(LLmj?VL@wYUqhn$0vH%L2hq^c4oQPEBQR&0-3r%n5*khN`D_GbGw&|A{Tt0lCqRF&IX zpv(GkTwSIl!O=%(*jjt%vytMn{zb8C>o(*ZeTMr0D)lVP;k`&j*6 zi9c*}i(}=?+O*T2g!8i>;&1MoUkn7h%s$0)eM%9{D@4>`8t@wjbVU~<(jo$p?t8Y- z#&M_d0CX1I z&b?~v5$+iLRJ0 zui+^i!h8b$DamPiBqr~gedT)E0u8Wby@g%v5_kh?5nwsnE{F}rD7reSbmiNIE(5AF z{VSGQ?UdNOUhbjQvgCgSgQ$xY>6`a>SM4<($f;ONoq!m&!Y==zqpIv!X+15ns08H( zm~@ZqH~0+nYCO(FP-D;6_2DGn=xoi9^y4DjZNtmize>fP-7YHpnC34khi>~4QjR^_ zdh6io6ICd{hf*CJKv$g`B zXW|Qejaoz*e8KxdhlB5Ub-`z8U0rZZ?F)Loc9wb!#cXZ-PED$wf{oDlPUzvp>H@jq z9ojLtvZXSRYSdQG87rH~GmHSDa??{cEm=Cx?I1}KuKLia$rw^j41AF}GiH&v{gH_E z5Xy_a=0}(U1LG`pG=W!*+D9DzVT9nd-F0`TlCcd{KA1L(&ywY4@#4Y9D7j1h-2mE` z^b{@tC`kTGJ}*ZlpUau&Q2^#(tEbbWBmQVgA_>Pz09r_R%wjimOMn0BGz1BXZI~wc zmTGd|WXhEHms|VyfB*#J4RgLxf-&^U8H9~AOlPqHnCcHQE!u#Hh0$mS4c9LoLw>T`xsgRk_YUIwZtOH>8yh9j0=halg7Z5f+KEIJnr3kD6*F;Q;aMzN1Qp_9uT_D0STZlt969 zpSX1xp6F5!eiHpoXAH@`C`DjNrZQSZf(=JeJ#>MSMuau?`}G3^g7$M7qX`T`XyN1j zSEly^%pFL)j|!w+i|b74d3wmp;b9LVy7B*8JCh4&V9qVnGiUdI|7%T8 z4XP>?xVEMbu(b+`?r)-y(pC4W)#{KS>BT$bKRyyxzn!6CxRQGz=N72bGThy|sHvX} zzR<=7Spku`4+;G<=yR!E1XSZVhx0^6yI#T39oz6uHm3XjrzFMXXsvD5{l3fcSwy84 z8nQ{yz$k(Pm)x@K-Y1XboqplelvkVm_xjKpR$3TVLZAF7YaS1?9yN02&$qh)3Nd^i zFASX^1oQgN=4h`mFGvKm6m!_#L$tpOZnGb@MDHc9tshD?X22P4u#Hs2oC{@44hi*2 zw)^I#D{q57T;OdH#J(IH;u}xcA z3k^MKUMr7OwM?j~X>V+11{D&-ejL%|_XUMoS)82-O^p7U7lFD~lW7!%&zib;>`Fe- z@5kN8qHFNNuXYc8203L^Mz(JAyjM%SVxH7Iu2kdg&~E^My!ousS^l@{zB-ast;>5L zXCt3W`CzCIhyOLLSoMTs#MOb1FhsaX-khcA??ch6#rxIF;n^OVLXuRa_|F^ywny;iMnm1|OF2;f-MX`^keh+Y2oG zmxMHP`yXitMMBVzCvYz+xGxwVpPHnC_L5-EHISC6kzTs^ z7qJ*7qmNcC=(7%cx&#is@#p{eXcOFZxv7&^qBqaK6-IVmbr(lyY#_XE;6IyiyI6TF zY+IIes(`nmoBCHvT=P}-^<&)+4Xb~tvk@=wGV%J-`hJ1Q`S$bUyDRXq#mszCBe^5c zh(yeaP8k@AzoUR$d`yz8u25!B%?NM*%- z3aZV9jenWk-T|9P{v+%yHoN?ZJ{0aIMzJ+3XLQ(z>ug-RLx)+KYCW}5-uf6xo~Q6=S-@8qKU5_`IA+0oYiZ+q9@H_F|qp2<3^pCvji%bon)^o*93l@<1>6+U0v-R z9)|r^jQKkaB&M-s@a6PRd4AItBt%fdnW#`7PzLK8Wc(Lt9kw{jh_7HWEH6h>#m@t0 zv&N0`AvTAk&)v52Q?bR{%{ zFsQuT(xDjwmM#KE_7CcUm#(J*8JI*suf=Z^ zOjfx3Re!5xa@A1WEX#WSI1$~G7-KB614}rbDT^h~u{lt;F#IT%W_)ElAtozjJ2Ksx zH0Mu=6DnScS3=6Vz>?JF_HnXV9g2M;i^x#Q=k~Udun{cOz>X*-}pXXR$1RrW9nvmlekxh zcUt;HG=1{I!67uhmwZrvt{nJv5uRaknNs*`|Ce{P3r1?J^mz$VFzbbOW2h=e#KKS$;whwoX;2x);~(+(GXP+ zehJVSIU=)_K3kkQS%st(dG&WP!mrzesceR6VK4>gFX)_Vd;pI}~$~v{C`IB~MYbp3UR6pGZno%eG6W8b-Ad`+INePM-x4#Z1|J(tqpF8ZX+0}j3Ee@ zK5H5~oMlpq!v_+wGd_iv9dBI<$rtrsDOVmA$y(I!j=F0I*@>RoH^;L%J+W@zBoej0 zU63yzn=KM43VX8ZO{wcIh}~M^uwlx zrH2ijpAkZF%T3y1N?!K7aW{U$-prlQh=JSRtaFdG;T9C+ZcfqTXp+@95@i06Yt({= z=j;99xLR@SE$u+RP3glfoRec$SvLg5Z83$aL&SDKQ(-?-rlV_wo`?qFUmYy?uZf|T z`1AAV3T`3s6(`AyuKg=(^fIk_9nVn+hqb}r*c1=9&&T^>#DS8C1hnyuS+@%H`xKYe z1Tn({8xw=*8iP~@!8PG8RHI@1=WgIg85b=OaSI7IH@D;AxH%Ug60gtW6^X{;R}G1Z z%{@|GM()NAF6HdI4+UKPO(BgLtyB0fv#8dqPp*w5?<4&?3WspxK|Vg$9YU2KM+{)E z<-0N4wI`O&i@5B{yjNL5B{}f@4QI4IW~oVoEh4BC`kMp-wySLK%~?W6o6qa7$;}P8 z8f0?j?`*8{kK9lRf}~2S6Ak2simeg*2g>S>YF*}7tQfDrhUtsn966d=zBy!C6LPD| zk>y_VZkWvoUuF$U@*QgdKRH|fBK>4<%?>SgG9{99SU}j0&J|A@HH<+On(R}A4|3O) z*tLb2Qd3|8zgb)vlu^EVm~B(0^EPCa>)Xi);YE6O+S+cXduOT#==v?3eXDDSJDt-n zYG%IlE?{8&tHTJsuAUvh{Snit7%HsE6o_D_ZAR z^frfL94)m_Wm5{?&EahhHYK2!vgn23rRYwFJuQmVXNK55Llaa{i^Rs7=vvIyZ{Ky!u~Y8qY)tyQ`GyZqPHio^Ug=Kds0B2xb-Um=vg+ zLhn*JQjT4kd|Ac0wI&CN{K{>7kk5p_peRBnP9GGo81lr%SWvGmRA;z%J-Kbh*x}XS zT?1$u_3Rxn3%SJcUiaE40LHuTNMgqhO;3>Np56DmQi?~fMwbx}!&Omoqc=d*ZsEX3 zH9~mmgmY)~%lUW;xgU@}fr5=L9{oOm)CrtQoRxyJW{L#Y2*)%&FGx9%z=s?a$?h$D z9S8(f7Serv2L>vJH`fqmUnG&>{0{*P+gft8C+zp#3li@n4?Bi)WV7r~!`M6^Y^cOm zRt_gc0gHL|F&}W(&zN9;!-Pl!8B8k5zo#}eS?{Z}oPGBQ+41Y1&j)Z0-#W!v1S;ts z13xvs@Ym3~ZBek6bfcT7wH2=V3Kxp*DFAXYpgwL&m3!L8!M0D zC>m-|D;~PUXu_soU&2~SF_-YL8~8+yb}$>A(tka=?e9Z7_1vTIs^g` z`<_CoOJAl+Eu{T-3GU{3g3|+V6!GJ&0}FgMph*HO6xaoBB&~)n3I}~jp;2rEH7s1QTm@ zAk+0!3o6cXqTk7niqCg_y03?Oi=yPvH<&xmp!WwLwNVTZ>bf)??hQrrdR*yO*B`dp z?P58LFlV+pkP?e{6Q@<{LjqiTX2bl|kKePUTaijE*vvTg8K}(zQMf)7DalW=mE3FZ zBfj%Kz?FdM@?Zf+lPX>9*Q%z`rWm{!@?_}1v6<>w2O5+63(HCTRqLkO*7S-K9|)ML z-i1O`gH=DGsK0?@$c=+(D)-$h5@vN6=&lr4;8!K$2+~P`z&j+6e5f=>SCQ(`@TIrz z;bf05UHOw6I;V)*Cz8P@NyU`3Xo>L*d#jUSf!{4aZx5(ffN2b`I8Tzsxc&FWYY#}@ zPmvoY>#GCJJUjX(DNvQ~SPt+jDs*jwHfPqFaB&>Pi2my6GU_${m8PxE-jCmm^p0;$ zq|WSNbHic?d&n8tYCFnXNh8wYZ5AzUU~AU0DJ+<^_KANpk$JvUmjuUiLvNw zBKMzb8`Gz&FE2)?W`y7-sXUU!yxdu@+Eyk8Z_K}CC!CA>jl>BOS@^3YMD{abWAbzg zVF5gdR`ot?cK+=5G}-9wynQ8_{lv5XftTtEOKKtYmI?1v&w@++4D8a%v(gpC zBpKKhgSV^xTVbK0=t(jhg0sft=tS1p*=1Y`ku?0rippD6XS>iIQx0!t!aJJ}yv18Q z6VqoZTH}puJ5d7&Q^HDp)+P521j$dKSW@^Hr_+giBrSo{B^*kLG%QfmDWXF8;yUWe zs^92(l*8wzkLURc5)?l&lhJHEtjy>Y-H}}qZZDU+%pRu4?S_D=mDXnFv>JFy1iM=F zr3Gd7etm})Pv|;pRM-qz^r8&1j%%nF3Pd>ScjQi3dF~p@-`LB;85GcMVRrK{4$5c| zdUB22RX^^rTbdU2L3MtrDu^Ki7`@&@!M?K6U%tDh7$4TvVCMRh*aO;~A|VT( zZCb}IIM7Kffw|`z2uT6sHIG)h`o@HZl0N)D2$3!y$V0-G+~;d0h@aMIv^OzGQ)JM< zW2>De$eMv*Mq;N*iuD5fzik0y|(Eab7Lv7gevxXi;M z&}|xsKUbMs9&_OZ8fT-mh*{aKsN0k%GRLnmO$UaDLHj%i_RzB5GhieT2W=m z^O3HXuv9o^v7x=}2tZkud|lkuQ=&a(N6sY>?@@}-PvgwxEK~4x8CKDytG+l^@sgQ| zr*LZ%;vqlH`z1fQu$CA)HKlN|-4oFKcsP1XIPCtbooT|+7B1wA7GB1n@~Y7eOQ9Rbapo>BfTWa60kP0zhhw;4EPFCQ4VfLroKH3kNU ziad~I|5J`=rJ9-i)co&D^hd4&#reo~Z}~8Pc{mqdu?Q4iD7fXX(IxR*I?BJ5F`<(R z2~nJLZAQ3Kw$#P<9gY0nLjPY205x&M!ElL4B|v+~MN0LeH-j6t?m>#}Yve%%Ek!v~Fb6B|cpz|9#)Xe9erY%EGCLwvFG z!6-X8a#CJWD9PO}wZtIVm0sRRs#hq2F8JPtJaB+GCg(7`5uBTr@C}p7Aoz04_inw& z>|1-|jM|)^De3o?->G>}f$r)dO%y2)Iq%kAmD=1(Bo??D1?+e;Q5T^YXmv<+c#PQl z)Q1M5i-+I#Nm zLmnOW@}F-IPUCwlWFXbt#%cFIedPUJzqDL^Bz-X(SF8l#b3!(T{9sS)NHnK{tJd;; zN*wy6JU=D%cf$6!;(mmn$l(W?LHVm1+}>emd7yQTcx%6v4P{_Et=Fme8+dI@37xhR zu+`i~Kj5c8Azl!OK0VB=BttV=l(wC&YqK$5ILM??M8nOEG#f|CxetxyS zbGJc4y}KtV2hI;Rn1yx1kr~ex9X1Gj+JO@OL4kkO%PMYBh%$;>CNjfsiogPd*YMc8 z%!#=j6YMej1L!;{L^~acTU(4+GAmI&n$q)RQ)~qypCgR~%1oE~AGwtxgh>xmGj;`c z&0vZx_zvy&7v}H(v)uQ=qS$cI!q~RYrkC6VUqR97Xt7Ud@5#GzKJQ?#x%a|91Jfs< z!#H8o9~i)3?ONcxCKzug=A;y#tspBG7bp=ED1lkFF^(AZ)c$AFP+i0y*($K4n%FH; zZ98HZnwS?tio}wre%(0zGw0eYb9=Y{6?ez>&}Q~HsJmdmTH8^bb@f*$TL2$ADmEkn z8Ratl>)rupsnV0k-$vR+jMnxZOx=%+@p(CKags+IqH!mfBI#j`d#WGGa|;_EB|*z{ zSr4V>)>%bx2ZA>1mNonK)R+$xcYAbWI0Vf$ zE-h=vGITw$9c^ma{U)ej0t3+k$?V*(*c{=TkQcM}50n>dy6=d;q<0z%{iN)DJAhv% zI5d|v0kB{^K)nzwWn_!)Gu+)zbJq<~$JyBEdVSg(VKD^)GnM1$YBej_-fP*8$|rK# z!6Hqk9|Sf>)J+1#)+Cv{PgQ~)- zC3>uueYxT*fN?8_J~nCJ6oPQg*+^X%R|J$3&me)VQc;{;u~kp?MR}=AP#}1h&PBT+ zRYBI@c=xd8-<#CWl6|(cpxh9r@!4CUCje>B-g8UxEJ#_(0w#Q3 zC?{%5z4p0DVKE>Nb-jtQ`l|3)8Vu7Y>PP43h*nGixNY z;Is0ffH0glofM&>!gK>$^0VMp?wFEt6v9u`oN`$L#8#K1-utZKnsko=SI@=>- z-p^GzjBjAtA@~$;Je)*%VPNwgBT}c(@tA80jLztT;t~;-w6A3e*GY+6j@8H!#%s%m z?|=0P-M_QDC(gSpF6K5>wgO!zBEqToZ>VSxFlZ}44~}} z@jT{IaN3qD0+WMT+8oLG{mw9cnzB#`Fol8sU1UzlNV4rp;`GCE-jGT9>y+i4wIZrN zA;nT%5#oqoW1x8DSi8I7+eJrK3QJ0#ms+&uK9}#g>>}NYF4s}#1>x#*cwn9-g+~lU zaSK>rV_cSrXJNxs)mm2)@P15qxPcHl)DvrOOFDDrfRs$Woz;1U=)7;0)n z`N@M)5Gu92MAUh;;MJ?VR`M2L!_*Uc!;$T`&B&nXaHF4Hy5M^82bL|tp)M#b`_i(+ z5JAuNk#k6tu4Ly0uXqNd5$4*=NbVzXG06bP{;hXwyv*G7Q0J72^Ks!E%?YxG;sR(l z9jV(Z3PLhlQ9fa_yfzZ`g#n3uxDr_J^;0MQvX5+20>P=i!2#LLV>-9rZ3 zAtychMcQLwx8}@3(c!97DD?YJGoGcB#uY)0Bm$+4s%wtOELxq9gxUby$<>ekG00N! zwrC&4+2dfY1kabIHTX~jmfR11S0kOY0uf8oTAn1fQW^}$TjILz6po<Vo4{n{-@RMPz&ngL-8_qXV1S zulX}TgDs&NvIsu0<*++Y+LzQKV`lya0xAPY4Z8StR5kk z@y$2Rhkdqy$KDQsfq9!&z^384Y2m9jDN#00YrwwVFg~ABaB1~x=%zka|?Eh;JAz?udNAW|t! z>|M#gX^?bX4O}Wx4sV;VQ5I3;F{0JuCjp#=QDCBe_b6q~bIn*Ngtkv`bg7xx2O$}9 zY-pi+jz8Y}LT2pIU>z(o_2Rmi#uMc6V0Lv$`-TZygPg~OO*&csHxQTZB_3pA)ZkTZ zojOfI6^BcYhEAix8;PD5_HCYqn_2WGRC54PhqC+O+))P@@+e)<)C5W+iiu~;M{1V!PHt@M1^2*57 zq|5Y+w);sJSbR8MnT2&v+!6c_-Wzvg{9(_mWXjQ$Fz8Xh_*((hAH+`~ahy)2wMVGP zLC~wsbs(Ej@oHJC_`1G{z%I3P00_Zg&tgMNRw1(FueCsFLROvw-1uKe1~ue~fmvP& z^lAwi&)i=STO7%*w(&(3-}fQ*0ir_H;zHFle+~7XGGJ1se?P%Op|!d+Xx!__@~aAe z5#k1q=$mhFUDiq4?&WlZ-5MLLwDAUwd2oL|WTlN} zO2X^#HHosr6keOa8`#XQwcTGbsLfj3;4yg0Yna}R@7r>*l*wj$yw`gDI%yM6X65UO zypK7bObqY<&I>lkU0(vs#>E~%3yn;h@r!L&MYxG~LDVT`Qqgt>12{jTO*u^BdVa9x z7E6j!j~f~KfB%F*?3Pv(zwc^QD71q&{IGD*W;+RiaD3_>Oq=jDyr8OQ_BS%s<^HV5 zlpLg>wP5@WTLKmrmiHO9xCD1YDRX15q7XHk#+TSfDnJh}mr*j|r6^IHQa~uk0VxOp z6qnI3F;Gw5_l=z=VB}ue&*N(+Yf0IO3;La%RPL1XyB*z`m*G9J4}qMdpCxi|qs_7u z(i%I8ddaS?tBCR6G;f7}CC2B`TkSKD%I%pIC(d%=>WRVR_L4MtDgRNX=ZVbNiZU9m z9m&~w4Ge_Ypn~US{mad>HO}uau=zY3#_F2TR1?PTtp0Uy{tbxRU8X>D* z&wiJP&&p#|R&@6=IQj^aM z8sd>4n|AU=mk5hs#!jVWGP?ZFSbyit`eBQ;?>|Vs(8teYj*F-d_ebCIF&yOE3A{Nimt{BSuC!6+Gn-)(gCuBG#S6NxrOEy{f{yiw-ZOTvt+MBmU5* zP;pazn>2+xFI&ebUZ-6*6dnu=6o^T-&-ILk_BFiA{CPhrdWMzQlv6n(R0_A0m3PgC z$b)!7asdNd@e4yrc^-@TS@g#?z8o$6;r{W^%)>`Uf^l8(d9%&aKu4j^9Jv;fc zX-qnX55=t38csxg(4S6h43~OO1wLc37Tt0yS$!QLid%jww{`&9WuILdH)Etb!t`Vp zlV#iD7kUB+Uz|u%RKXmNM=y%I{ftzbUbsB)7JN8JB$dw(ME>9_q)?#G_Vg`}VkSmA z`y;tm()1mu0rSxt-W4H{UaAvmfMD#l8;q4b|iV{ZExE-w$j(N-|=s zSk!fesozD*=g_qY;0+r7(~|9XtEvL^AKCeRv3<-j&l_$X^PD5Q#3DsTr2XovWVWYu zh=UN0MXA(|k{A1{j^?9sRRD#hSr;F2L$|2^-%_{6o&-m_}fy=Ly2;Z)6n;SHuE>Tb>x4mN!I_r*!g zaO3ItZL*oLV0&x1Caa?W6e!#7 z;JN5AOAc*CjaVDyDVJju+7>o3QYXe7$q~k)Ls}mh%?x?h<4n#l(as9<9)gjlKUpvZ<}6u=(2g_}ji3fh*Lv7d z*URGKcMm>%EChr^*qvYNPx>hxojzm&{;8ZkAr)HYpc%^S$zC%vNQC!i=#yYO3q{h~ zyvhX$1_?FRe8<}E$?=x^%Dg*yEXwYMEml!RN?IPB#4|oqjl0;OgS!0buKmE5H1A&r(QkcD@#{*B%RbEDTXYpVs-d^u9M12YEhY5CsfU zyD614&D(5EcrGhAcnlWYpH~=B9CKS6qqk(^j{wS?i{fc$O7WVWC zyf_*K-DL*J!9i$DLe9Xn*g-QIo33P2SXo}d&>KUJ2nn{4rg6pdv=VMIxWGSY$6ZUKf|j{NF|(cOgN!J+sk;)ZMn5YLJ;1j)}QBkV8P z;TrGJcK9S$J))l?ZZPxxc{24sm)`%;_srCcv4{l;Q(Sm z+7F_V3*AQqk3M+Vka62#1uh`wQ12Y+!Tx8cfIX^4Im+usS$7g^Og5o}C{;mQ*J!LD z>T24bbbE6mI;R$GL?cl@g(}rbr9!%x~e>?B#v$3~l{vnE(D(D9%o& z8Cs{)H)as7xhfmD`RDT)l1v_FN?Zkg%1zKME$i!%kwFH=;wFal_fG!jeL)SN)i0&nZb1zg9By#c1N5J=W zGTFd+mmx8?lyfzP^0@sRsquBwmcQrRWq(3q_JVX+AitUzU*;5P%+m4HsoKdJcEq0! z^Jk31#na!!xi<3i%l&l;Ofx6+?C_I>D@HfgSBz0m;KjXjwS#4C_J`ZX1dVU(TDHY; zxRIlFM5qB7-1HOT>KCGv03Kq?WHLD$qoka5(iGwb$9Ey+{}te9=tFwji5$A3J@1!K zMzt!6MKFnXDNCDaea{jeau<0n^h=0si3Ji7fvG4z@ z_pGGct6lSD1hKgl9kly6$`H{0#$`OzYps8C7u=eYwT;rHenNXbUecXLbbjAGR=f}E z7q6QY1^R)3@(Tg`wNsVY42a#B7;pi0+Hwvu+%|_K{|Y@w;sGJ|L~x-iM4wV<^uue!h+eu&6#-xEWD1JJYso|Z{nILdOvcR!e(6@ueZufO!%+5 z$b-EBuGgbkbyYrW`(&*ie&x%u=K5r0C}LV^p^EQ>?u+975_!2aDvb}^VKv+%1O17! zVYtCJFbn`;#x!w4F=LF#KA!bc*2b-QB!0s0Tgvn-=EyV3sV)N;Rtikwf$Qobn3B~O zB+M?084XG}u$^{wr<{@D*jsH~ugA|q8{wn^py7#B*z4Rc)0YJrFyv~}V1HPjQ!=cX zzeN!6u2)7#EpipoNBYRt^dYS8Z4o4*A5G9vSC?QVl`u;W)_qE+63&>#=YNK~`Fsry z)2jiq|I1@!WW%aFR!}%tS zS>y+n(hK|2nOTi1>&5f9$vSe8|6R`nfjkCFc$J}#e)9Y3WWWd{)4})esjU|=87rP5 zmYVaT3P(L%6(oRN20~{va(RHwp)Lbe5UFf@d@gExMyCt-;bMy@KdQdW2Np)t=w58R z#kkqBZ=H%ibc_=SM4j8vlzq+#Q>leWKh|U$hM@cXt6O-Au3^U>R%bn zCOPkWZ?;O-=^B7cAF%(&yjC2wHK zI<+ZRJM~D77m1xV;@UEzG2+wSvRHG)FifMDZ}P&jP6xKPrJ}z7cGuXO()&ddnSvPQ zAa0Y58Jtd6j+z%}Q`X~imu>TM{~}2Fw_#e0O=nC~8=iI(p#>{biV#_}N@d0aMqUZH zovUHM(`$g9uAC}&gVBiM*eKR&KJR;;NLe$M82rrjC>(m<%KsZTr_2)*C$Vq{Asy*G z!CxZogc`WfcVNI>)v|d_#N8HcL3!aK`)k~B4e5P63CjgoST^pv)6paZ&A*hm&kbDBSKpL`kH9{OB5n#)HtN%%N^ym{io*y%Hiyv* zOG9B_4?vNS=H+#+ko)d~3#8|&+aZ{#dkH+6%V3_dJl~AZZ*N0&HF`_VRIUeqd6ROQ zlG}xEHnfi4D8a+ph^a{(c4BA7^>q*!5&g^ARl2VS$E*8~8QzTy0cS+E)yinI$M};e`cbF0ygTgp zr45nZwXvrjdBtBtNI7$ZYrTM3!&fZ9_TB9--xS|kE{HSq{X*}}9m2nm8`v~H z9keW`(hQlySucj}Ti)GaKEwJYLzzS5ah)|~?(?|3jZb?F-T?%Q`wLzIE5Ay-x%r1I z{JDU9NoyvCHT~W*5vkL^%FS)Qn}0hEX^i7Yn?Uj#E{R>BO&C>aJgwiojy(Ol3qXmb zLNJrMoF+1$_yP3%Z0HsU1F#J*5$K_Pqf%ECN3i((YO*XG<@}ZRa$GW~k!@XwNAnmL z*MD~}Es`g{)m-glmWP0tUj_G2a<+&v=9?=B->8VeAzI~|BmEaZUmJ~6OYZl4R=NInGZMP&yDE~ z>Jt*6;4a+fRT=8v5?Dd8&tiL!z~iJl!?tlj_YWVru9gnqw2M z8Re+|KE4$D>D&`A3+0z$7?J0CHV%i$69`wk#*wxmT}(+qKLWKE5dI}EBn%HSs)|-F z#=*WIjjE1cvdXghc(skR=Z16BI-YWpN76b;M^!;{xw@k8NnRK!lOQzi`y1Qg>iea) z0NM5(5zynUXem;m1X=sZo|=2hY#PHi$q-uh+BQjQYfwlLqqupLeBhnvxl!E4qGZ+!C~kk!CXSSdgE;o= zDSA?ty7jRtq{|uegD#93a?JjlH|%KBz5(24p!U0H0x;4uDJ=TaUob=TYFO24hbZ7E zNlmy9(hi}s5U*!XKT~{sz4Qe}KAP}5jj+d73sRmI^aF5xr$h-7so$z6EAjX#{5heI zlFu9n>^cj_+ff1{-xknNV>nLPUK^YoS|AtMp-bl@7(R@vNUC12@$43r>A#K^xVs%> zulqD@{hXN}{FUuHo@y-QTH$!j2@Q$!Ub94%SBm$`SY+cEDJh;0CTq&Ht3cw)!~3C5 zBjO}>0AMaOf$D~3JC|U`mG%8@GSz_j$*a+~~7FCa1 z!O3t~?k^D~)=#P>OCnE3%!k#tO`r*N41ReXdp{NSECdjLV&dtWYbRPoECNjJmjNrw zFef*lg+pyw9`xOKHdf&N+03SWejvK(hP9Rl+u&AAHa5%0Rc|nfXlygJE`Dkd+mL}q z1CUuKtx;Z)EIN5#*BAOcIa{&o$y!i1-!q`ij$kHW_o3ub1E2>l{D8mACyKeg@F#-2 zor<$jjW4Cd4*H-?IbTHCQ7zVWCZbx-int>eE&JGH7F&M0R_&g8m-k0f}I0quk8%W26tkX;w!*sQ{NaH5>*80s;c;j#krd zV_0paxJZ7l$b?HWw|e#9r>rxdtby%-n&HJ=q0Jr<1DK|L(;YE8aj}BFLw-U-ik?pU zox7SHAZ)Acrp0;HKY;}~WRDW*f{-GU`ZvJ`mbLd1rh}0_SD|MWkf}#+2XCoQM5JFw zECs|4U6jf`E#I&#;>J*fVv`D`4}BR?tI%q?k8=65 zM21@r}%5_BqWcxk(5;o0E^QMch|Lg*Da25f8 z+sdjZ>M(Dd6WR zI3I+ZKQ^$qc1;891e11iaa}EdWdLCNquMZ+;)y&BrjwhY5vL3ZE?TSb@SYh;`F?5a z6BW5xIgeg}L>Ki5FzC~!InBZOrP<@2-ePvx@J5V<8S(}ytJ?ABKd@eLq;=xz@9I|o zlO%5+Py*VxSZaoDDTB8$dnveX_n%cnBXM&HeM}oj{R^;FgvS@#<)x8FKEe++#hiCy zrV0Fz25ji<3u+9wQzd*L1VS2R8{PPg17|e^HDrUn%cmHx#rKc2xKN<9Ml*=|JVs?A zHDuzO;DN|Oh#5aX^>PU-w!2X$3I=QBtCFl%7>QLDJXA8-KuaY8LeNhN5uW|Y9=zh) zjWfm9!Gs$;Cc@Sxmo6C`*&M3gX2?Z+sZ)1}2q^m~xzV_L3hFRsMPuN>+U3HYoY^+< zx^+lHlGrWae;VHdXp94CU{WjD(gX?K~ZPHS# z_Ef|#cTXNuGstjf=%JIZRJVX(b=RuHbT(VaC;Qian)7PjZLHt@8?EVL;k_b#_8!*i z<*)+=mQhJu*@J5zA3YarYqnsAm&3Qch*Z6gRPAl#<+T=C!(AMmU&wt4bFY3T=}Hp> z)*VwSo5E3`NBl8uFSWAOv3PL0;MJZw#I9jMotMYU-pOlNfdSF_2bT~@v|7p*7v`9+ zR2IDkw3V@yf3XP1ANnz-pMnRGplqg~T#v-^#TCEh=^dg9Mg*fA!C$rcg-$B26l223 z1=4a5fJ3pXk9d#6(R_NsAHPKZ{iNv0?jL|P!E8BiI5*H)cKY(o?+$ur_4=9QMM$(X z=V$(vH*zhTnqM7??zg??EWthahbv)5G96?zN`&^{p+IGN8z(2^s?LY360I_m5h1!1 zBX~TG>bCai*bI^bzn^^9&4Zsv&Q3k2@?=)y67P_CUrljpDZzH$UP<=BHeFv@OZpgv z)L+T++qWMr9~&krChpb`dan#ns6YZWntI;Xo(vfeqE4CuC%fbsPokqK`_^x>FQ*>k z&9*lL$L$t>h*Yu2&9=!xWEG?W=Fr`_9jBJmjw+8FWh>91~;&z)LY!`U^( z%~pWF&UR(cxER^^^xxc&B4gv=$0N2(yrE;mmqbPfsMx_L7bY&mOqb*Dm;sqCWcjcx zY;06yOD}il#Ep%-RHy45J_w&m=ZvgqbQR9d4yAAdnj_a&MCJlo(A(^3?ij?+_yg;< zb03unW>OL=AuZaxdBPuo$_D4gm(wl3gS5PDbqpS4u~2rN|Eg<64tmdDmdPFF;aEUd zUYkYST8}y>%uH76aiBPal*PBgSSFgRzE9HVNmPQBK-49o3VGVEnU)|YKL33LJ!9Lv z+kt8Lvw1O{Fa{*s*FN5-S01`!!5MLLHs$rFj*QsL3D-K3Kw(@?l{baOh;On&azj zN5^4wmnDL<{qz08!DGhuvV+lR=k3e$%fTngpk^YP)i;T**c#bdjzuF=Vq zZP$gmnjFXLixzskRz;t5t&1*43sV*vFWD`kqaO4=Wg7c$S!hJFgwS}CLtmh6W-FEN z{EI#!^wS~pzL?Vpojy1iN7QXoF2`2C3?2QB5~)Z2J3zq{r{sz_rg3}Pg|fm;alZP8bellcl`ODx*ksFrNUw< zx#|2=jcbC`q*su(`uW>ld(o365oVyr ziqxUK^&CFo=Qo`qfv+2)HXG&V4npHB6l05vIeGIoWX&A{5&c->;M=I^35BfVLM|~Z zfoFz44AxLS>E~+3(?iaQq!9i%jP(x|__IcX<+o(cV{>3H;6i@m_42ay*9d9>Vx#MY@tIs&%}_IJ2U4a1AqWb zc-&(c`DbjKapEs))N|><*J!9c9U#9o*=DTtdqGTbjHx_?hdnbrw4H!a*=np97+Lye9u_4x*vxeUC#t zMb*}N$L*9YFUHz>rqp)i9%Ltg1W;Z#OH<9Rfl1wta?xD$MSA<;Lq+J#VvhzfT0cm{ z=EZM~+v+gXzZ1@UcuJ&xV!t!1g34JE@$ijuB8s>->s5v^HYh}#zSova!7XL$6PSLl zYs2#K^Cwifz`rWm)6X&lcjPAfP>;cIt;P-Of^&7WCAXD!6;=?HqFfW4H}po;s{^L& zqXl$lAV(hp?eex9aTO*G|9-toy%;ZgE)=_!d2q}h1dE8i3D(pIdfa=t(IaWKgSmaU zcDsDh4m9K|J2}^ix!GSfgMNgfVT2JV4x?uyV1DR+jeow?V(-$GX(p1iL8dK!shFYT zwL4o;bA->bQa1M~WWSc#J+|Iha4ceA29qq;jbsKGoL)X_%|>r%^E~EH63;{scP)$W zUZZR?$lP&|22A|g7dq&aiW9hK{CsF9V1p&+^DtY@7L7ggwOO-spzH`jKU~WK`4~De zi;kVxt?SI8Lqcg3ztyotzS{Y&nayV&T78CN#%|yr$K-3Da@B~>N!97BYk`b3#Abb z5;+>P1rMf39LL=rDC8C)l3W&v;`?Q6`Ku^ddNEi>{ED`Bdy<{nnoj4GEoYKXo?!W8 z#iP3Q;X|XT|LaRcL}jWUP~e(Veh-Bss82>Ls7%ET*d+738EO4PmI3wFf+5*bV$(n~-hoC;xw zxJ51-auIyt;F==*P7(!%4)MIFL}K@Hk^2KnnJ;hG$Q#Of5DG2A1tTcJbl%`&cv+^? z*P;6hq5M;i>b_M-ptps^QsdhnvJtMD_y% z?%7Pmx3L9Pl;1u=t4L}u>s;Orx4CAqK%VcC*U;Z1bI*m*J$#6ksOofiuyrSTw`GPb zPk(~%$tcIfG+th~5H?6byt#&#oyt<)GYYwnd(EySys1q%hHP<;oBYbW!nNVVs$hGK zA5Uv>U`00nPM77X1h(j3Jr49(W9lqU^yyMJsc5ONU)w5*R_rEz}U zJ#Be>>#(wDtHZ~7wSIC!GNX{gnD^ANtET%C!CWH$`Tk}-B*z09^^7oxY!3Fd-OQj< zBJ%017xAEyY{f1ur;NttJ0>Y%s?5g8!e4a5lI48r6mVgCl_UB^De6?6Aq)5=kRm@9S+-SD#{X1%fLa+vULp!U|?WI)K%TWHb0uIAH@YSL>|}7B>otY zL!t!xpA+~a$L-|)w#~j&35$%J=ygKuB7=t&oX)ImvV}xrnSN?E$Wms(FX$F*+$Fv_ z{7t@2!7IhUzP4z79KCm+Y72QEdgkAJO9YRPrnrs~L12WL%dsgF^s73JUa_4PUEHgdn3dp`3O7 z%|Nw5aWrQ8ij&c;{IT4OmcTk4)3 z9k5cZ9xHvP`|ib}$M;)rETfl1q=`y;b&!Z9B`KbjNQ)@R<4P1}W0f451hblN%O<+q)E%C%6Q%{ul zbjYQlMukERQK3N^J+4i#k0um}rX-*}mi8%^AJ(g$3h2t|fEFbM!WG(S%{VOask(dy z1zOfw%UgoWD?39!{Or$FhPX;+&TTdon26J-yD)u_TkQ|MiaI>Cj=D8p%Eq5rd0hx^ zeAlks?x(Oe`~KG&`yPykvMK$U(ajj#{1ytZx(8t&oX{fLcM;ny)Q<)j0{8p81UXdn z_WlUB0THj7Jepiv2Hx20GVGFvb!#-KR@p6$sEf|iD0-NU9GA?Y);|yZ))dV=jNPb2 zklXD0UccmM4$2Lc=c6)mcm@Ps6pw;rpiYPKC0``(T=^rF;usX+ z)%JR3UlT8fU;~+$beh7S4{(6H=Bi4oYTU?==FwmXu5{!=enD0CzShWXa&>W#2&+Yt z46}t~+GG)39VUAr2LU=Z2%l1c3w3|gn`ofgvpbq5e^yl$9tfB4pN@FV!yvQ9)k2B< zN_cMhhLs1HyOJcy{phS?)6DuJMqP@|E(@T@z!FGCQ%b9ktLU`tTxVqx^#%3ODCRS7 zhK{~Zf2{}59R{>LW@ct9-lp?EzrJ##{X>S-FectJVXPbsXfkCZUoB46B|Vk{)}NDf zcTfE~5TRemliQl+L~O$?iEDEd(GEaUer3@&T@6^$SgyGM)H`@m-?!ROJZ-Sd6K>K& zN9G1E_v$eE@FLms%gGO~?u7fDOQv^B%+6n0bi~MaIoB`BLg)uso2%=?au9twe={4AuIM7bxp()}Yw4ev??k~nTe(FtAH=U?wMe8XMxqfH3fmP%~91BPVp=|>s-%Hh-q!_R}ox9OLj z%9L=8*?av4q;D(O%)RdRFoI?<59qWgC_Cs4es= zIWX|GGd`6_rBijt(2EHcbBH#%Mo$GmV-4Rflr7RFNkgNb0v=Y zOr_an;k~0#4O8<%yZbQTd&sAs4J*AjWh;^HG|e6cz#`i-Euf(?j2e!|gbq4tkwWyb zo~rH>vO^b^72Pm+T&-^2--@1E;E6GlFgvzJRAo~9xj7L61v<9`D;JOd+y2Tk(LXw& z#w8}%wG)rsMwMY>yEc!cUSNZh-njv&qut#r5 z_)@}8=O){}A^AGJc*|hF*Gp|n&lfF)S9>CgEnTkxVHK8K*dGMkqm!vLl)j&leV+AJ zhgo)Lz^V*Ou&~}!!lkXBo6@71My_8#T7XIZ%9YBSs{E&+C`ed@b8JHv0x4!TWM#*X zKJ%lvJ@q^4g}=nro{BmU-)g{In`c8vkyvQUx^~=?U(b1&o}CUs*B8l=F{|J1$cbXS zob5zFv-&yy8MlS&h9{=D;{5*Cm}Cajj$eTM7f0ccC7z*oLG~rV1A8}ejssn6q_T0@ zT5sK7Zch*45_ovsB&2M(NIk4z44C2IP!@~`5qu>f3`ucBV!EzSy($EHfsR|j(e)_c zXQF9URboUkGyAQ|OH{E^QjRlNTnc)~ zoI^b2qF)hvon8E@ER3=j3El#B-DfSd(XESL0Ra%#(&hYEHwUs_tf{EM&Gqg#p&A@p zzJxQH(s@hl`K^Xw-283Pj&J@#AoS4un{SiB;H*eX`~^u@EujOvaq>RNxF1xqZCH&j zmPhwF=Bb?*PbYAD!9j!=yFRB1uwM2QS3J3}9L&%@)$Ei8p (iOSbb%j|6T%X;5 z_1X{V!|)SNU%zVAN5tLi-`6MV0~X5F6PwHbwCZ%Cn}ci3>{1W|Dvo)?O7e-n1wn`h zqUrGk`l8j zGKks0ZwZ<8KW9lOdxjNEK;1iRbi99_dN=qmLEm@pcJy$G(MCg^j)nLO3v2w83k`sb zmAN?6E{Ai1OQp8oMP^aO_esbgP}y17R>D{Q#tqMxG@j)B&L0CML%!u*!W*ANE1C@c zWPc)@YFNO;EJ$*%s9OhwUUhWHEiI+Lm7=B=L`dzx!8&mx`VZM)Wko8tgj77>Z`;RA z1i1aE`eH_%tn$cDS6w{2NI$w*oYEYR7Cn0XeIKz0jPtzLm* ziPSFW^2L910C6>4Rwg;R5qyY$h~(Q==={Xw(^>8}BD&Xo_PKkh zbMNqT;F1uM@{r}VZ;xL0mnLT$F%s(h{Q+RIF0G7$y}kg;LfJsSp=Lo(>WZIONf}WF zAeT(uKVAeGDzO_==(DPf{+{pO39R|(2&jz8t#|x#agloi zn#51;xTM&?QBaAk?Ks*N)-XuCowf8vk3`vo0`;h_JWU1pK$pUsxPQLi#><}}lS5=* zF1%x}?-l<~@b{?+XLssW$iG)r)|LqZ)7%`a3@I)J^jO~~jN3}zeVD7c`Ey^4`#+s~ z_eNJy9G&~_T*NTiN~I-7f}}mF$`EZ>V5=Q4rI9}IPIgTa0xDqcBFM;YVaoE{U8ub)_aJA#_5R|#fl`_?C9E1@cmh{yhe{?HHiKr1AYE| z4=}dhz#;c9|6X8F-^vQ4-imEqit-qQ^|t@q;9KR@2@&mbMh9yo$-Dbd=>+0%rr}Kc zZc_?Sx0dX=0XJC=U@eAW_JbeZQ*;zYq}nL4eyaM*L}dxQp$1GWoYGsNj*tVz+YLWw}Ffu*eXBlQatvxDAm5kbs&xU!WV1>r>G4?qmco z=aG?CaU3N$)7B;&VnhpGT_B(dwV=?w>PwW$>MkIPHyi1bdo9ofpy{3>@$f9pXOLBc zo^Z|E{O^kB{%sym{ikOqNu6zF2n52juXCdesoUIanguM^z&{`$!nr6b!JBV6WdHj* zHSs_701CPHGQkkU0-o4k5f^;xcOMbl@&)>O9QOZ{De`6*(HYq6tq=TnC#EJ*LjRhQ zeX)Q-v~du-3|W9){`(p?@T)SQCj>aaG5W6sQHbF5Zoi3tO%39|u0_Vhg#3wQBLKuC z{jUcHI>6dGFx}Z~8jYYk{r3`AYCvRHPRFyA-G3Sxc<8@KWF7cccRgL0iPQfZF?C=j zU3m$|s?cw8ae&WE|JE_$4|Nr$)(oD6uEVw5V{qnVn#->V_r0NI+JnyBI KA(fIQ!T%4HNZL&R literal 0 HcmV?d00001 diff --git a/moveit_ros/planning/planning_components_tools/images/collision_speed_100_meshes_no_collision.png b/moveit_ros/planning/planning_components_tools/images/collision_speed_100_meshes_no_collision.png new file mode 100644 index 0000000000000000000000000000000000000000..d2f99f939abbf5abe7b0f2ccfd8889336842e260 GIT binary patch literal 359517 zcmX_n1yCGK7cIep!v{fvyIZiu-QC?ixDz~h2rTXz+}&M+yE_DTg6qPY{IA~Is)b^i zo$0>a*UmW|p{yu{iiD2@1qFpFBQ3591qIUryx|bwflmzWb^ijtU|mIJ)DeJ}FM|0G z;P20F654KRj+SnoCN36GRt}E#7EG>YE*2IJuGWrjmoVLeP*9{$GU6iY-!jiSy<#bk zbq=0hUVrkDU$$SAw@YZHJgdryvX=aZ5GEmw$I69_pR8jEZVcQYLR+gP^010B<1Xm6 zTa2;@BQjZX=h{@Qjh{1G6B~%)=wvjaQ*jcYhFkT2aABa%=guEp=w*3sR`4 zFHgoqoMQ2Z63ud`#4#!;E-o%85Br?im-3<|ZvR(UEUW}ZX6C<(dXB`m2i6>(=5B6L z;Ut1%*Vk+dHdI|*5N`AnZ4#nGlZTi)e6XZzH3E~Y_;oe~rA=Wo=LyJx@lZu|-V+_5;$&f%+Fnqf=Vj@wzS`@07l+XD1YfS4g$SBX|C6e`9 zz%9+JhlJLEjO@15k`TXsk z*PCSVmKTCay@0-csZHs(K4X1>#^lugX+x|0_+tZY3tz*_+Q$DTk>b)^3pQFA^^E)k74x%3s2Fk7A4I zZn7=l-#V}y`L#aGTsR^dL{}W+x}_#sHcvCx6lwiO@-*x@Heq#pxAM9n@lo>YbHqfx z|Fh053h%?o+x?FJFJLb~LIhRvR~#FVD~=@{7*rg#RMtFa{dLCAXjcEs-eN)c`!!uJ zZ-ZY5k|<6gI&V^cS|0c(??htG-GyYc=XF zKqAt+gn==8r&S8F6ct-JZSdZC8vx_?vD6YR;%e`LX!d!4j(TVnx^qJ-Evva{AG9v# z()}qho1e|WMMnQzPgq|E*mV<~b;Wd}o<8v1TA|e*ww#b3I3Lo3Em_u${!Lj5aEs#p z0uDb3w+jD*G?vY|T{o7RqAVB!EH|2^<_ym+>-Kz5G!7JCLS#J!7AL@~@WS?PPTu}a z5t&zBcCMQLZrFX*u9Y-Fa5aHqR0F39%Fu6x`9lhhEMk zTA&=^1?vlVBI?3pEv^*mOVGn9bTCfuhU60BI$!NpRW(i{uu7L}>He1OE_?pc{);GF z|9cK$LrW(?1M)iGZDP+x^oEDunjGiYe!StH^)tLczvkRj14;X3F=1;)LJlYX|8Acb z|KfArelFRR&I_{&x9RvSy1g0E?(UZ_$aHS~_4&X5ed|o>y~QJd^J4^B!t5=(&w}R~ z*zORz|DIeMdE2M>67Ir7963*&UV=VpwMj{iMU|s73N3o}mTx0Ix?;m-^2;;$v1~#j&T`3IyUT z#LSAaXf@V@u)cw?aJ=~cFP@8e`8TAD8JUrEk?qpr&HRTe0n=%N?JnGw7i_#j1EUEM z%q+OjH=SEsj*R1EmpzDhdv|cSM8lY?NS8H`6W>N$9UlZ<&F?o`F7w+xNSs0enlI<;#B!11i(orDC3E zP%wkFqdT^&ADa25O;poGOk;s>piRPE$ONv%}{*It2L!{p9(L9|G_D1iVUP8fdBW&%SzPOwOqAS|b+viFA2?va%Kc`)K6Na@< zuEh*+4%Kjycl6#3DxUuwXd17;&)>#7>`4!V?#+&tDdHL64g>>?nd9wrNKrX#m4|IR z%=z;Li}!qt<@Ax1$?1##(A10t8LVqK+Rfw;G0gVj1VP9BD8t7(aHp_PdpEFU*?$}X z&l7RlhSrK)-4t!_Y7fV0N*5`}8I6m{;UBreqM^e9JT*`Q_KoP{liNivUQ;j!*9m zPZcj`XN1LyS?~>~Ym*09yxz6>pFgd}JV9!<_#$8zWRq;Qs{TH7--!7hhId82-_iJI zijxQ0uOW~j^ka;vlGKNpu(GlBJr2Mcy{>ih)SNR@$yQ8Me9%=@~ zUpwdXd?pME#_CaNQ4~6Rz@O;E$eN$AzR&Q(Zdt87H)_J_nr=N>R(uom%SgXC@Zu$Y1wI(-rnh#uYbdz){BKuX>pfWqaOXcE^nTnmWIvQ2?e}wg`1Y{ z^cNMYYp<*bLN3wcL{<4!C~GrGRb*og&WUQ0Zio}}F&rJ9zO zX{{k&h0B^lXe}UU!>`_Kcl=9Zla%uLRzRk z)TkOETj))?)>hXQ)Vv)BIbhuHW`E|-sbrxn#01ai@(4YX#LSAGtVbNXP;O451&^Xpqg6#ZQrnA2lkbZ}@ZWBB2bD$EN^J`9rvC@L80%GJwo|b_TlO1R9`1ZxS>6oo;BmRn=)run?v3%8T(i4OH z`#i@n``5-+=xE65mr<6KQiBqew=p(RBvvxJ5dEy~pH8shl4z)j(;Gj1oCAi2Xi_sX z^R(BEO-i_u+Rj4h;$B`-oeImTV6_&PzJEHZ_PRV?{5SKk45m~Q{7Sy0^ z;U2v?-)3W%ix(dD|^3J48g3Rse+8{xtw8>cs-k6e!* zudwEAIN$CayD1pimXbOY_2TGqN3NrO6w6-_G)=nfGkMJ)}vs4U7a54}hhml?@$#qs8R(#dbtYVtw1=J?nvfcS5c#}XHm zzJWvrTVOEw(JOBV8!1$K8ErPtF#ju!><^-bsUT1OSJwLaaA0{m{NG-qA3P0km2aF- z`}ZCruzYv7e1vjd&Wb(lGxD}(6gv$+8W(o--TD-L36~F{(A`;s{SOHtNsob11mc}_ zhNV+?Ij-v66>>|((A_#q=cdq`dnNYCMfWJkn`GVbhcSp+$d=99(n0lbq_0Yo( zVb%c%SIcM5&{0uQ+uUd)=08oHgBKPwAcoT@l);A4uNM#g0`+o^v@S<%(%?@+Yf?Q;qnR!A8J^Irlwv=-e3G2gHPM$Pu)i~lEOMv^+i&l zO(I%`H1SUAK4GRW%&;YnzdEu@Q-;zYpL8h%5_prbB_+X=-%IT*z6dQ z>N@m!e2aOb5RTXqdSvPLoMNszZDXuvtna8LL|%Oy`-C6{CxNJDFsLFVWib->NEK3p z!}esBAZ--y!%-Y!`%AP)mL)Ouecy}v?HG18p2k%4T#I!OnTrP26>j47Azi?%3; zcVw;-n_sWByC*qE>(}qL3s$NsAtK|2tU8fmBSRZ5)~~n_Hf2YfH3w?KjiWrqRS3 zMknsAG^s)}*<=89z)EhNT1c$U^m(QOdlik1iSS<6A)rIIRFXX zT5DB*8XX6MJCn05ZSPMReQL(>B01ZpXGe-dO(ZHea7Zq(?TpT%-S_T;1V6S&0SAUS zf>G_G(J*y~3l`AE`n!MTZ5oFN6DjS-;j)5f@1*Ai-V3dbZ+hrTcQMloeqlee6j!0! z46LB03rEr&B>5q?AUVJ785FjSme_#ys%oKAH9o!m>?XuTm{&xj&~a(js?W=jTqXjP zkkySOEgN{CA@Su~Yi(97`@$#mO!3~|6eHk#{ppa{FHyFz@AZ}6$mf{qZHNRi(?aRy zY!aCBl;Vk&+2(vN$-qzVVr`%FWBp=N&|C->ULHRIL5%ei+J~Bpikr}J+<8dF0VLz= zC|jbm(yX>aEO#d9%VY)SR^9%dWR=4*0$sPV<(t`!c{h*TwNw16elgRnIwbixk0h_ia^jl)3J)RB97iZTXkmTle zQh#;BXavmu_nYKI=c(SP|NWFl9(|*b?cDpUp3bEv71DTwAuiENx-}MFhdZN zx(GGJRKr!(Su{$0oBEP6w6<{w(*$SLL?3=7#QT7o=U&P;wl)EbLjGaHrY+kiJ-B@4 zmN(;NqMe7*oUaM98|)k0moH|H^jREcoGdthZU_%4tR%;!v<~mCAm3DGF?7PNO`V-()pOZjP79Ajn=cA>QWL-Bt*P*-^$_}1_EuW zE7H^cSx(mG$D}8FO~N(W{+E*euDuC1*~2K8FF|oTUsEns>BILl*#iMUiaKABe`0)m zU^f&hEL_8;9wW#IvS9R^LqWM4x}TM|ONzPD1P=f~vSXE4)$w;C^Nph9VhU-Jh7GQ2 z(bq;>C3Yr!xYZQPFSSZeQB7h4*tU>zpO^W^T+(Vu@G1Ow!O{pp=HH!QKhxxgTrLQ#6JjuV0{*}|{U!4uw1su(AV zfr@lHy9$!OkDfuPW;9!1&!AC}Y)yUZkmT`&M*WC^IK3gxVzDMjBjcLQ%HuJg;#E4BM zY*EhpEpVJaX=RA3!A^=_pDbR0MzwmWe{EUzM{c&cEK#i#V-J$HhllMMpx84M;cH3n zvaGK;ANnopRT9;GFhmuqCd37YMryL3z>xT!7j;8Ih^OYDAU!;rl8nFwP5kXy#Ki+m zC)rrcn|~r0T@dytjU~5BA@tar(?sTp4i2*3x&I zMRQ+~LZMgeaMqgA($o7`QEVFVkGo}WO8+R42~cT5I7zHVMV@6-rN}`KEhqWnw6z&N z{Kc>x%2eYc`7)TUh>tQFO;I5;B!pD1)WH^d!vzDLzCiMezOehb+K9hkRW>@)`9aFE zUFt|u;ON`e6z7z`;MffWO8K8__##qg(=l@ z99Cr#G8I+ZHS_Q7zzm?O9(QgwPbX0=0NrD5d+@xB7`@|6Hxx$JS1|e_-G`IdPDvC| zAsyu1bG5}!9D~yP{SP`y{oBjw`fFMS;fv>xHsnUboRrSLPWnEh&PHBZ)c8O?=JgjE06oQ6JfX%v6uPS=91ClZ=<&A!?$vvqCr{vbDPGl-}2Xc z$bPgl)TK2_tNLcNu^fR+x_ItWqy*I9(9p=fOB5Jvvc`ojaElQk9;8kmYJ#VoTo|92 zVbpa=QAmj*2E{)G^J~#9lY)QB%-1&)UC}T03Hc_oI*XufN?8-xo~uIkMEe`J6g`M?m_HR{$fO*xDN?#1MPg=V+Mo0kw z?U(}qyP%??qg_ts>EcHy6A}^%R7)S5vPOfAe6J+l#x5a(q6q$f!~K8EST29U;A^oR zi-gML@k>_mBM zpQB8Y{V-snUi5WvM*~3w1NO0t#8C{xE&JDMG%3Pg{uok(lBH8d7M7c*L&2%)&No~2 zZcU4Oqj6J!5wHoJNlzd!fHysCy48<(#dw5^ zNtm($s^m9L@cnEpPwb$PcNxRG`OtWvYC3g;mwq}G46&K9p;kay`=wgG{t$HSpuc6u z1w@s(s!Ksug#F>T7bMvp$oefgg##=|L(ZBO}KrC)vJy>D}4cv2WHI zwSY48+SiyZ{Z!7tpk36PACpOyGsVuq5n{jg2?>sIj66$m=)%_L#FpwzvTzc_Gq{sbaec0q>K@&zc(8-B@EPm3c9 z9qJ*uZ8ObHnozc6EkkC^S+-0wF^pbI(pSRN2oid-RY#%J&`NSnaEPzj`+f z6)PsZWFJr}Q&v(=wgmHCss=Pp?%SsPp(hmR@c9?JzHTx@HeU$f{8lz|JkcK7VDyYjcw9AoruDqCRe zc&=EcP!-5sCwBnV;J5Z7m4p)Knxj-OQdRcSF>*%C2?wb2n}Ww*ojmHnziT5gzg z33fXF{H8g-&^SVK5o#-H<{sWO68NY)cWMpTrV*tNWo<)PQ-dxS#hmbZN1j*ytzTxp z>ASAF?8v-O4*82T&av!C%4y~Al0yB@BTlI{gh!N$Auf#VuAe3fd59m(OA_4q@VM}?v$Ins$k?q>$4MLG!5_P&x3==2 zkqAcZa)+4wuqcgbls=P;=#NW9?#I}A*oEDJoCKWXtcEgUq-ap&`j^OE5!(NsGsmp>hdjvaaC3DC$Xb!w`-2x%J46{4ml=8F37|Lj!k1l4EuL zu(a(%=0}f;hF6puVn0hN4GSBM*#>Md1~UKE;jhN7jndIsJF%7%^8(X3#2uqb} z%qcrp!W*|M#WU*w%Wi{4$Yi%gTsKdM^$8;e`NYYY!h|4*{X)50XNb7zYXuiVq>Z^W z2R#CEqwt2stC`@P=R9Ie7&kkb77W|=?#XW4}qs}lR(?>oM56O2~|cLS;NsmEoyPI z%!C_W>+O(19x{-K*#bX01ILxU(o=(iEsmYU^j@5KG6h-Yw{-#kxg?@?XM=-|3JQ$l zP(OC5)oIH_&>+tjQZBGU#$i8zT103@7#u2lzwl)#XVD820>kB(66vfGvMu za^n0btffBJ<&&H*Iuh|22I-2d0>jZvTVlXuws9=$1{N`h72eTs*-3y zVfgCVRRA2w-H&FeII#xC9h~@_es#~)jP>jGI39B=+4yabpOQ>#%P+1Sd*k3)a({Ti zrtxwtu31!82C&%9??^&kw;wYzDs?N2(Yyb)nV*n{t-uroN`D}BO-2N4z^$u~mrJK1 z4NBi6mbowxr2qQYgfX3&=w-1YOXxQdwzh{f=#nvNTr=^DD5FY~$WmYiU6BxL(bV`8 z0+4mnv(s8gRZTjYBLYwm$wTH7F*f5U0OZIoNcWexgX^Rvqg(TE zMl{6D?<2s-@Z~vkE!ZR><1+ufol^p;!%xU7lKH*Y*Dhz%=|!!D z!F)gLuvxM1Nag5SeNXGWm2?%Z=!HoWhwa}GU6l@dLT;}bc#l6d-5u(lXp{F_BD$(Q z!slYPmzvQHzK?LfwM`Lhy~C@$+aaM)d4vLvbsUO`+kHr`_U`+0ozE5M>KH%|aQg?C zebWypae}$3<}!g6dI(Z*%3kHflZnBxK|_jyy?fJ?EJ6>MxLxv99a*8& z%4r`hy;9>IoQxQInQ&%)V`O5gZE6}kJ7WY(!YsM#7#R)S@H5#o z8@twgDp5T@3h5Y+8U)Hl_I)vZ=Be=bc(HeEqy|qmQK}lPO)ZsBJcywmK!$Dmlk4UO3Wz z^*MjTNTK-`T$QELOuqj;V$Nb88|IJm!jXn5n7=pzXC{2vMs$RMCIn^TJl6NSn>71Z zY^PYK995vy*T0mSo2q8w(57SxCC`w>H9ynuDi$TFspYpv`x7d2BS z6}BF@h+!Xgo&{h3>RdlTSDm_C?VMgg;Gg;hQTiWWPEP27uBD}l0J(90e}B1gf&9Tk zrCmeG1usmU9@{1YzmAqq?1WPG=%=MJ(ZHp`Lpk$>TqRpfUo&n`$b7{7Ii$77yX*Ff zBbD_UhN9x_-o_P1`GS%4ZdZ-l6>X({bO9XV05EPND+Nep&oaHQLkBwV1H8VVg6MLu z-wU=HNF?-shvQt$`w0VFk8fZlzYCaRQ9{e!VXEcgwBS!z=EVuW%-rVzVg~=bf5x(A zDQ8TP(_Oy!A_J27U0%{tWEtVIcQi;frb19fru}}#ySv4yXv!;M*7@I5AF2T-v$=&| zXxd1c&uZA9a`3Eq#LtRtgL}N^X$;L?RjQHPD2aNM^m@$Go?dy%1$A7-|c*}m*+>mVlOGHFOkug!*);2oE(hWQ9ia6yHniwycA84T? zO{b%)n`c-(lvg=pZHy=Bt4i~{S#A2@V5l|k*}eeD7u@&m+&n4eR31Xpr5Fk!i_Oz$ zs>qIt_J0DuD|N~_Y`tXb2sx0T)_})_g^K?YJ5G+FFv@CGvs^=e>9;ds5NXd1zunpe!$;3% zLn)sgb#V@IYkxn%iyYLYKE*5i!dm42QkaH1U-H~@fW}X-F;|?0fHFrqK2J~3dX5hklz5P%)ew6q4JL7tU4K{ zyTiP=_|@f_$YRENO|p85A4B#rtaKvvz(rXlmrb~u$kv(K-FL*}BG7>mc`S6P-L4JB&;e0nji4$VqCE3|<%qK6v@JeRlxk zbX#I`yFIEAl~i>FqZ%H5(VrIpTG`cP;J=*~6E03bu7u0M2-?49dn~iq2CxcaFR!%y z{Yglk-=6k*Mmsp8ocLGQLQbVxKojv6*KHgbE9s3ewTGu#u`1#7lbGz$+fY*U;Xg0| zVK|`J#l4WG_0bQP=hYE?SDrf)k=U4naJR1y&cSaNHL5N*8dp$S&lI0pvahG?s4!Y` zPLbnxo>AE;VAho=!4um}9NlG)k1E$+_oAg&7D2dMGJdWHiKMs50_I}zyh*D2{dqj6 zvv&SlAC$i7-&B{f^(rPkoq`bYFm9C(l3-T9HMHJJsrx3oxOz&blR zP$xtftWN}8kggzqK(1+M7@z*@Kx{KJW(?$f4{f zvAipv+HlJ%UNwuSnf&XHPx8Fo&rLEC?&jp56ZBL4vKDgm2dmcbWMBd|Xez>{t=}AA zR6S14UvaHD{G>QAtuX+_kTDPf54xuF{o9a>Pe=*s*niRGRj$|gDPA<@wBZx!<;K9j6Rjn^G!ceYvuw+A zhoIj1&ZdNj{BTNyq1iadLjB7iDB0V?v#V^#l0b6a$+jOXE!CCqH#Ez~5B)h<03k@b zLLxtqJ_!lHBCHX)hMae5N@=~kstwkgZ`Rikm-TLL08n-4!4|hhIJ{-!XE?F1)0i=x z@_V%jo+^Mhd+l1pDzhTfSD-br@Lvy8*xA|9=B`EC!7ItQv&q-dg_L>aJYI8MINV*9 zslQj@Bk%nQV2xshOABWkj*$g=tRy8Z2cKwws=e8xP%`P>I>1VK`#wt}C;zrrfK==f za?)vlj_~t?iAJM+f#D*wR(=Gx&;7aq7#9B%An?oq8m(5E)zC`fyE%i9JLORMP*u2g zaf{ImoQKJQ^XS9X07J8r9L3*=Ws;-646C;^^Ue@sI6c94zwaiun38O8N@@io`Vkw6 z;EBwB?sVxPwmMF!d@K9TY56-QUxcx*u)1lj1{I$G)tuJZu~ofGSwwbZ`ePWzl&9nI zvF5rG&$Xdzas67xSeA%ijibA!k5DvR*CiJ0hh&S|ni`C$4!D1?;_K5*^WDxxSjz^} zhf~g1?bOW{=Y)unkw^awIJ@?iPLgCC+|NXxSMEK?VDG%MNc@-3Zy0i3k~I0m2s|;{ zoRg&4zdmt+a2+le_V3437KVUn!F(vMV7rD;P+DS93Z z^TixBVp8i=F8q4|Em?NFwR-la`H+-C{|$E` zbIzKqGuyx_{elE{zP#{tWA7PW1S7Hzkbh&qR#a872?%Vcgc00DHye3^B1Rt=OesFJ z*VG8p?i0aPWZ40d@+P1mX)Ncz8`y;vaayyV$s2p{yME%wV#gYuYPjon6bYa1bZOOS zA{=co=gH1uSbIzMbaDcve324(g6=)JtQhR(yHWT|pHt~{GW->Q6+}zbcgBqTigusa z(Ji%~2QYstVar5OJ6RHTWDq~HHn6!>2MB_TSRzn4=78R4X zdrdubj=lZF^$SEG7E3tF1I%wA{-{JCm+YISiRON{`c@I?F58M}IZFv~|5_%AeaTeK zscNpK>UumS7v;C4-GRDaotX9xy4e#-ECy6tnv<$j3p4keFX6d7!Obgj@-Q|jwsx;0 zlD_SUm@h38&pFlXf?v=^I$tBZ{Lu0Uy{cRPhcaFMJS8Zkm7}}z%elLAAKiEH^`TDq zkl0eTV56P95J${d*ipT?ehzEr<|j^{aoXa~0(Yi3J~?hj=RiSzxK_uAJ&Pl{DqTJr z!>3I9>D!)N#bg8D%(1>9cYXCYqAi?vD{ZOV(lY|?~kV^uddf5D_&yI z8^!K$Sixf{xin|wQv1G2KzTI#j&b!eAEcEdJ&^R7aP|y+6(XPwWaag+Y?HMNX(fQ( z)lSgN^u~wlPwZ0NS)AJ1*}Ke!C3R=}4X^M~OZd6-IGMr>BW9JBhbp;FjBFQ9_bgd4BG+`i1db z^RRba(A3OQ{-k=oHfE(@<2L(EVGHg2GGC<3yCox*p{099Ql&?ogW1~M9ee6Vx^Ui~ zhl4}BRQ=}=d|J_GzSgxQhGyjDuuFDcu0?0R0m~0-_Ua6>XD?k-<9h?i5C4fO($Pbn{xO%IS=MEEoD_jP7Iv69>{fV!H-L_4YP% z#Fn8wdkgP9IUA*;ak8L+?Hz%*(9ic4=geullp<}^M7UbhNE2EQq)9*i?Cn*;GB_Lj~0UsaJ5w1}uDEkco?A5$Y)@7FT zJZ_`}J{VqAw#uM=@MZHsFgZC{cPPrIK9)mFiI?XBg&uR;a+A)QyTP^swax($XpbVbAO;n916Vf=|-(~cB@@Icwz@$i<5u*kqVIKcsmEoUPS zzF-S39tI%T?M?3Uj;2_U?sn-xFE|oxIY`r2Yihrtj}hgqdrgyhGD|%bvfa0Uk5f64 z30e*fkkGM@sGn;Iy@XoymORbP6B`>BT3+uDCNhBX&h==TOgM?Id8!*IX~tBsQH>xc zT0Fk5dv6`TUVhEG8N)RrYl|VUA!^w02?}ohSB2fVc+|LbdKdk@@`q{d@bk0xYpU5$ zbfbT(2*cg0Anhk4{%a?-Bg6J}l@XxqZk`y9Haa4DcI|#55QOS6@_S>^tLEfg1gwv1?*2d)-0zbIfiaHGzmBs@i~}5JEvK5Xnmi|Nk%Z$p|6$p=y<3&llwnW zqSGXCH^EBwDNEs~F{Ou+@y5qpMP#i@4|8eaCuXy#JlPf6i(Zl!yH4RPa(&^PM`2*+k#f!gGntOZjhN2J8Jz%=<76U`W z1ER}sAEBx)H&cAtu|q`zrI0d0m@$JCEXF)B+KQvI4y4)TPvlqx=e(~KAg!ej1=2)H zwfAY92xRYDdjQ#Ut!FX*C835e{>G*KrjH|?*uuj@@nHiEZxx~enC|=0=O#Z{4&9+o zSr()A`{5UyxIXiPpSSjopn#eju2N#6tc zEeFL(OAO0Q(bCLqR|MXIFQNox8s5bxD@^c!lTvjJ4WIa0pq_lQO>2LAO@|fve)%#n zuuWKK13{a5y^j#LD^$;mo!V_ zA=h_xDOPGmiZ!A1|FHTv1RJ~lyP*j!*XNFnsNOZhVysr$vHY3a{gwk=@Gk~3@{R9t zXj8%7k$=OpYnDv-G;*nb?$1O_1~V1^!YH2wiRBM|~U8rTHJEw$g~Y z47R2JiP;xMVp1u0TEq;v1r4}C`>YR2m)%;J(0yU><>V@L$K%{3*8L_N)Cb{VA zgKpGiLQ4l;dHT%%bO!y}d%V%X_GymgLrql1V|%VT0NwSE&mbfKGzFArYqCE+XlgAW zW}23c_w|uC;~d20np@a|Rb`lP>e5g^4(ru9Q zfcxAWC=mOc)jnQ<95*QKLb3#E+r_1huNTIcK%h_3^>Y(`OJ+)6csL86?O6<#z5J_K zN;|47O=*Z_YOZrMcM>cYTS6NOct-#qC-tU`J@7xGdC&rmfC0!T5o5uUn^eAPSJAXP zb<U zj=>#iOSuQXP95=9G#JccuME*|+>e~l{jX>IM&CZP^I50nm

%?GlB$Q92*Y&)S{8ln?oLt>-nas%Aicxebu;J(GRgeidCyjOv}uS+E7|@-(_W( z|C>D|_ls;T?PgLp}FnD0rzM*v@;nf=R`5dgzS^JeGplVoY_ z2F~}OLNkf{BUss2L-YwmG5Zzo*0!Tc#{_n z-itS|u)_TMFq@1ZewGPj8}59xJ{*AOO^p>y(hSN6DsGJ*>K`$w{mr3m=Yyc=4>d>} z{usc$q8cTA>DKKXPhaNAhztGCB@}X5c^c-jhe>6l30|0`&m{n22X{T135KZ!W1qM5 z;zYb1WiEVc&AT@Sdm0MB94$8~0NO~PwMmqva%t6Pw5wmSJKK5gLND1|U@um3emdb~ z9JBmJ8Lgyh)uL=8NaP|zjh_wV12wddp>?4I;$(FZV7 zQ&U5o%`rl6-(K-4m^<79jUI1|maiPU^ae`Deiuz+$D{ksHIt(}j!nHvSgo+Fs~l-= zl?8>2zpw%9MG^xF%^?ibaYd~ea)zrqqtB8W*G-{;N zL@$}xe$;lbTVlegp(j9+xdYN;_6--i3Kh8|6lg%UQmeE~S)4$GI5K0mL3>735uVyq zYMHPK{Ra^cRx6{aALo`npNHgjRr`5^PjJi52}Di==)yKZ1!^%SiMWEiKI~YL=UyqDyrbbWm7tni(G%h>^`Cpi%4*(slzd$7^rM( z>59CiEN|54^B!&i0m&RuC=l!3W5bd>4j`0k0126e4kIIDO-IN0?X4SQERYrX2sOv* zYV0$71*hUDr0|F}p=o6aqJPMG_p?TYe!)oL@yQ+{aO@B%RnME@kKkYjjm_*1ecLfm z+TkvXS!mMeuCz!~r!SM@K!GNt>0N&0{tZUu`bp4IHMY8{>zYxlZD_cuIp2i5s#__E z0SoA~G&ME<`sDyB2#1Zf(90J>K98uYD`#LNJkC3?P)pTDhvh~P`kR{@z}*eKkEu3O zee)M)YhpfK!)?AlWLuiMh1f1t-4t5AD`rVsv*?AbNg`4aE5`CB{frTlMkpuCC7Nwn zQ7)Y*BDHtF^tA(9vheupcW$|_MHQ_rJ3cJ6IqyD4-+XNRd+y&*vj9auU;25QvQJaB z6ou*u{cz=R03M*q1spv2<+aO@Z$Mk^+lm5!4RHBeW{RfzJ zUORRef?d?6|D-P=fIC1}B6dmeQ!P8V5z1$Ia8%Fc%0?JcrhB%OUFp0x{onNygo*Dg z%*CALyw=GYC^FevQ4VT&Bb1-VFmnmwAw`TP6qJc%i>aaedc$G7A4CvxaqA3?wBD@@ z0cdw*p1lB{&RiiL*?VdqELczOc?||_{^NNm6J(76eI(=9&u+{Jael1k&JZ3K%Ruf3 zo;E>x$LOw?E+wG11=u?7XfX%v32mm7(MK>mrZgoq_BV5#VOH4OY|vjA{wWFsX#M7@ zLEJPPt;1@djY-YQF#GBp*SzUnZx(iba$>{Z?&0DRzrSw*oP-HSBQmwL3y>xwC*X-HKVf z6FWQ5ce+XW;V~{xqZZqfU&k)Sm{-ES@Q$VB7db(QQX!sv{k{wOhK30QJs}1BeLc3%EiT;hd$fo zTKpk#%uaQdz||V=O4MnuA{~X2n884x+S;s;!u837kb_G8E)+F2)|*Y@d~D)(d&miH zx%$6GsviyzC`@JsV?aBuzFRMtpIa*O;-XB}bE7BV|9H7?B40QWxez zY~Q;YU|_bZMj&17hxQ4;w*iqzfl>itCo9?mlCA8mH!Av)dmQSBjagC+KARzl6nwmR zCrE!&G*`G*+uYqf>E>|i=EAc(18|TK4B$+}-X7&|)Bo0*oC!Y;IRjeK+7j^k>OE=E zeVo|LzK7J(4gaR^jRLXv5m}$IDL(w6kqyHC( z*uVSSc$&%W6`wy8t+u|r4RjytE&fCWdviHYj6gsJIQS_gJ>l*Drv~Hq(g^2~KbpKQaa*^xWZGnEv z)CSVQAsy@8SW+|O9gq+evaPcj`($S1QpHRv5*gJO*-Eh^oE(;T-el3`pn)YS_1`}} z_Nw+QAK{rP&#@3X{k^H%!l&--0FyyveE-BMKiu#5Z&LEo15UGL)}HsrElt zOA2^gOb{15t@?7QZS5>~(HB;lv|nd^<)KU{_5$b7W2MiGzRdy=w^@xxwU_XVGd1D3 z3ReG@o$TrH{?BYy?D~+a@=C+wB7_nlE#N5RUJO<3pt_^$QD|yzgk1?~T@9d?z($R* zljg!F2yrM`H)5gGn~HbB+7_*v$zpaC%&`KT$12A!xj4r^eP(8+g&(M?@#Z+34RF_* zcC%MKmbltFvTDFT#9e)Oo!Qa~l<`bG9HPzHBx=K=Lj+>| z{ftb^Z0dNHUS1KVrQ(f?^lL)EIgT)kFKoO;WrTK-6(-lO$Si+x1FjB<)?l(+1J)$k zEOf4HD>TdT!bAWz|25-qp^Lm#Vg2&sP)rVK#)Q2dYOT(ZH>l%rK(}Gb2bUjEQR9OM zmOm48?# z&ooV%nvBUdXR>YE=FO9h$(U@rZZ>bWZBORp$?yE%Tdxm&s)%%@fyl9 zU-opbU|Wnul3JBbP&mRE2_UnH-U*lXgFim1Er)24d8tPujQU9Yo-L71rBQ(=A!bP; z=32`P|HcrMv5S?H>|i%p9|di5yTSAXCxem*)j^>Ut`C2QefblnY)`P?mLm~(Oljqc zne@imlmON}s^!SkK$56g-l3}WK7~3H{`muZQneb+>!Ok=H9QuSsfcmRa^?F}?D7H< zo>6%Nm%nGFJYNNw&j`3ZqC#eHouzQg+2dEclgFBh-`M{56sdkMwO-UhkEKNeqZn1C+(bmLxP-BGc+P_YPf|>3IA3!T73}E@;e-x&_NEVjT$br>IO8j2Ml}2;|BxU&9bL;uB4U>N-Hj zP{UFEGa)_%)yCIhal_tp(Qy179O~Y{Ji2_e+!?lJR~bS*iMgOzloWCjbAP(Gm7=kq z0YjYY3UPhLNrL;BG^LL7KHc&YKZaZ=u=!|*!E?EQy)N&7e65dNvW6=0ABAGC2F&ON zk~PU}Qia#4G~%-~q6O_YG<_)V;RPN&eLzy6uVv2lm5N*g{o*TZovoXFIN%p0(C4bVnG*-@CuI*G+WI|s*khg^ z-&efmh^1zqJ1!p%wA@_W?tCW3FV4mZm%l`Q#`++X8PxrS7%nwsA5QBYTWq?}RK}IF zf~g5V{)|{GL{JG~lu|AZHMu9&DvVeVsxZvNk=|eLEMQ~cLYGfs>Q+f_LoG?o<6ZqIB@F}k z@9OV45P9)W@iK@I`v>ubUKD8>ZS$l~)5V==oFtQ5PR%@+0xa-M7E zr=e#!m2+fBo{EoxG-16e=wpvL__qaTn2Z!04~co-cuW?=(qwAQgTd9PC|B~b=_xLt zpKsroX?0lBSQb}PCYd{i*UVrtta|C@FOmoqBg-(}*T=9q5`%g3RzhUW)kxT}ARIX6`)xAxnW zi`8mw@2BYR7GWFUbAVj{7`L8wCm0{O}&?TU9#$rjF#)OAe35 zDk72^_}2WCsi(nSecWm_8hov}0Eu3<9~p;vNUBp#3Q5?ugEb{ri%~6$p>AEQsJn9E z-!ghi08&n~#ixgu*sB{j!^{mK9u$6uUB3L9Z#Q`PRjV$XNLF z@1AjMJQKe=h#+KqK~yMWLhHYe`Ih?h8y!1OWBcJZnA~XmTE@pqEK(nDqex4!7xsD% z>}`L`vO8y=Fu#UdxyT}=5pkfFz|t!fE~P8jW~0%@(Be6^5P)5pp1BE+5}!l~ESzRkt=8d1i{L7YYBzO;j=7h7Uiz2| z=Z>e3ovBmC09ydy9_%l#?i^iQ#JcpD@=8rlHwM1?e|O}pduB_-lebjKV2Is5`*?2+ zx#*=&Py#SoaFN+$CfBbuH!kNR<|zS=r!4P>4GAqxUE#U9CJ`u_ga$hd#!P_}?OYS! z=bTn7$`(NtRTai7?2Z^tSF_wHi%hdpbHvZtGIJYH=+c+eMvHR$>F!+QS~AR3vS(U& zM)0QOTGm-YgC#dN#Fn3-t6}hYJTuG6%Dmglf0VBq`1uPf3f}KAKn@8pDA@*aOxC&A zU6wXD_M>o2qMD)fH3GdTqCQn(R1MJ{k8|D<9jjzh|LNS_&5<6lEete~en^Q8D6G-V z^=ngj)clr5Ls9yE&OcE}DYqhu0bM><-kfCv!Hh*+HLsTf(vNIo3i4FxNi16#f+#f~ z7PYc|1n5$HaH)-a1qsOck_(==8bY-B)>u*|BMdFFGyl*s2d!# zwR z4hnnG{(`mG5iCodv{g#o+x~4k>+a}vA~vGBjIiiJrAm5%@GIUYTA~U=qNbI{SArmn zp!3*kh#Anj)R)4fQH^zgy4XgORnS|@CWQV7(T~~K&;xk)CR-C6n4#YUWD%sDPYcz% zZHz-PL?xQ^2j>CtYv)#X=F1X}d3c81kP}qE4;Rq19ZXL z9)vtRJhxtaK;=OyzG9=0s)V4Qap4tIfFcsI(S9s28z zy}zSI%(?o62^*(k>IPw|{pvVi+emd)FxLl9TzwPxhPsfN&*_!GpR%^f9$e_Zzxm{V zVH6(Fs41Pv#54XjOD|dKlP`YglobHxnWr5V_@m;PD~d+d+LnwyYg0o{;%=r`pNhVA z=J8}HV2OS?Ks!w7^Ezye^ZGh{eNNTv1}5+jcv&9eIW}TNY!F^Wqi!Unf&`7i(n^1x zQU?D(JogLe&hE;*<(}fcH8?k!&>C$*T$W+)arO6`A%cz{>4{-00clo-6v16)itYCx z%n9&C-^71{Z+P>OiPJF=v~k+Uq9$Zg_Gk&zpILd+SaWA`pU*BeVodGlQ`06XY*Xjv zArRH6;cR!G@L^+N5o%s5S^xp=Hb5D<)&$=dTU0~=xY+FNW9CW}fD%%|KpkR1M@K(u zlgAJP0MWBEo6bq`0-=JL+@({;t6q&Wr(2`4+-=->eg5V{Khys*f98EZln z&b5Nl!rii4yBLz!1U z2)ZF&{OW2q*PFOqzuuaoJPTWQ^U!sejRZZM;q0{Su(y#ab0~nrB+Bub<^L-~#-Tch zBVpC6V28c3fZ>dc{T>{RNWMQb%4y&hTWfk^ub{-OE-Vgqkujzm79>TwQ}NLGd@M6= zz~ZRT4i5SLktCS3O*tR4u@>z0d5_i>H=PVIpk_f(J!g8se-q`CunsP1!{-6vhCtG; zUd2(nk}acE$Q>V^8L+*2TmfcNoZrQ3a)j(5tiA^*; zmlvOrLJB?R4>?juL|A?*Ga}=c#x`1LNs{nPxB}3&!7WQU<&JCWT9;S_Wpjv19jb2Y zp!#>ki)w%CJ0kqfht3DcW#5Ajqh1^5tlo}%$NO)VK|IVq$$~&F*mmr|T?n;grycT? z`6#jXAuZV_I1@P>pSfyw_{EV+b}KfC9?M8yGAMbJ21a4FCWVLwk!t)jOGVRDA*C1E zx5bQ*h=88(ci`xM{T&$_*e|Ea(*<*AFL3Nm2JA%X3`MgS&T59g)u)*)#(M+_i&6(2 zqu6ZoRwAO{RB}^X?!1@5nepgoM^rvgJl)s!l@Zo<>HplG$rkt4t1?Mja1aASl_fRM zY@jX0keo(E=`8K)Y%i~ABw?*2@)e(j>C(kI3;eszjvgd_en0uQW)C$wGQBsiY>Vy_H{Ey>C))y>x(bB+~41~G}MOAohntSBA1FuG=5oaR`}Zc zw&&Oy<3Z^&plU`sL)VtxA2ll#quN72+1&RvDozb42@&5}Uze%Q%RYJU=@$|26yOT8 z)4U@viDIcjUg@7nT?fi#;i{OpMWdJb3WC0htydX(zOen?5~Jc2i%kI`x~1MQ813_u zCZbmIvXDdls_L&$oO>h=(vVlv{U(h@rbL@xl!#q#q^wE1t@qcV>-1np*!pm38y{bv z4^jAz!n?i?*_(H(@0gp_4YPJZ8rM^6D3k5xl@>988mX*a`22?vd{p{Jm;zcI5CtaO zogZ}(-EZU7VD=wx;|#C(#U^OOhV!8GudXr@GR{Za**|HzD!tQwqGuzkWsrPa1dHu# zLTw`RMTO351e=2}lD|Z_e*4czX_xd>`GfYPVcYy9~PQ5Y7b!0DK{+IIH$O!SF#9#Bm>f& zQPNv%uF1$CP%Oj{Wv#GtOdSf^lC^7jPZW{YUoP4Pc01l4C}8zx)9A?@a1#e!S8LCU zKqk>^rZ5+j^pFe=rbrPA)%4}9nxGouhP?|-1$7Z8GgJ_zB_o?v1Fq@@Eif@uRx)5= zV;?(nrA z;F1e4#H^iP{}8zO;h~c)iv%<6k_Bp%hUg#erx=*q+oz1=p_I@OZ1pe>-`{Pf31=0T!@JT1Z@wJ~{{HE8 z+q$_8p(hYZsT%9Jm`a&Ejncv>ZhFy)MG@t~jq=lb$m-==NR0XnBUy8=VzSx9?7Sdr z;Bq*8`bUUzjAylP?&q6g)PfUBiXb^Ql&N_dYsW2Da*W>?NBNUZc{ID(?(jWYYSz|O z=_nty!PG?vtW^l*2#bN%m@u(}Y{}GqH?K4zHtxh<=J6;KMq&&&ny}I`HkGZZqgvvt?*) zRsZ3~7p;ec_(MuqJM)C^lWv^}05rC^w6wCk+|7*mUjY%l^bp08!z)YYQwziG_YO+} zdIf+sa@ISn&v~UMdixuC6ANB%je}8l;U=(II~a~5^S;s9>SXVMz*H>@aBo-`(e-AYi>jF|mRMFsh&Q*A~`sBLC+Pmoh=fwi|-2Hnd^G9+>TYdq1- z_&pG`ugV_!GPw(l+ON7yfd*etETA2!)caWj@0ZqC`BxM_rIQn&m`ODYA2e!eohL<6 z;NlALbbBn4xSDtxoYF)dC(6Hma*6j1oMEQMidg=_4j6Zz8_E~cR+kqiD+7Z1LN{CP z%>@2prXiWy3BagA{v?KQS<#v`mF^={isq-xn|3=GOudROrZ>a62}ihL&&Y6Vj%w3X zI1Ab!tj{qH^bH7AI;T!It&WQ}%MY+M%ZUkw4&?Y?4P0Qi$@=lyVzLTIdj?m{`+SoW zl|#w?GIJsQ5X`jT`S~A}Paz$wDSIOqLR>sC{AyH){=$r;wKUmzQ(2R}C@|G%b|&Rm z%qHFD9IJTQGibb-QPT*hiW8T<2z@@}3Z2+e|3nxL&a->vS%^_TGFHUSSP89f)BX58 z-uZSimp~PloRVzh`n@d^-N2gCmV!;x`}GGuInWb6l)kR@niAqH+ z`j}BP=*$TMQJ!s{MTRksY;rECa(luBTd5I+)(J?mR=D@IB8{5uZvoP=qvuFNyb zA2?(j>XkEeiYTMifD&o>{(s%Y1P6V+eA^tVQo86TKZ++qKNj)Zl*w(V3+)XirJrm3 zg`|4VaM^{I6L(LeXRI)cjvw`<=j($|uy zQyg>{Pc5SP@l|2e)hPTU5m9xOWL;JVg;pTiY5#fDkVfq1C7ze18j=&gn!ig@Rn%AD zF*Llj46IjAksv@>ulBAuI3Rot&J|<>Z`Ms!R-e|iRKqZhEqznguI41fHY8+|YC^yD zAOKuhDeG}U_0!OU8!$7+a3&*)i_6f?IvS%FRz+ta7Q&)phVtnRRw^ecr73nBTsI+2 zWJDh(X2V`nTZrg-pB}MZ?H|(KLvI?oW{Mr$0!BqJQ51b^#y7hBICT8l1~!QtMtKC!jNqfgK~mktvLNq!f0W_q2IIkAtcYiE3Al942DY^M9X=rIa}B$`ZSa2hxQ z@AulfIgdvAn%}BA`=4Jm3c2fEKV#BzGK4I_m(>nIAi4R3AWzALZ*i$3a6J=*8!5W3 zL|bMjS2T=#KJb0aHS}NP=C0dTh!qd>imh6sxm4T%<(c@;`mNBUZKuP@+7|bHRSOyl zArxIbA4qSf;?i7c^gqGt4E<_h^p-=^x+R4Sry@$3lF zO{~;eKY7(jif@7YQJ#cD=EZ4pBCop7kUO18rY!#;4##hn9*B_c#BN8MCman$c8NBh zD(%AhlXn5f^4~*|2IR7UP%i7bS3i3dKZC4xzjwQ~tz+YyCU$a%5|Ts?WnUrotpRN(^n3x4G(8{Xwt&Hf%suL}n4)+v@cFp*!-$~gX2 z1`Ww+lqnpi%0!Ik_nehjI07> z*LM@dJVU*pbv`}UB!1WUBe;?>5t7y%ITDwRK9dIb%#ZiLm68_x%{9eUiW`0~Gl0v= zI1vejmY&X9Eq^Tn5zpn5ha_4RO{DXz9NZrilhN&bzqmlHZgzu=_-FBEG6SU9gb065 z`0vT%Z>#j;6S+6RUkO%>A{os6T-cFWoVt#UCjn_OsoNZUm@=q@ZO(~>FXzXQko?~| z!J6BaG10P?Rxq9n<7>Ulu;x#ej^SHBzUZ1n?#v|dVa>XisbsD9oW%uLlpwj-q#ASo;ns z`)2^inHsS&7lP3=Fd!oLx+E*;Gv!cmdLYv|$aO(P-}8~UdHVV!R4r=&P6};8F9F$5 zW4022;0v{I9Z?P|1M}@f8wm2$U@qzEfza02szQCp+vWLLx>M!_0ZSU||5g++-6sHg zVW}cA34F$7;T`tf2|kptOXOJVr1qc}dQN4ZH7|PW;Xd>|$EWM66}}@_@Qx)zk#3c< z9eve!;@tZA)0#o>_#&RPsYJJf$h|;F5HbFSgt|9n(#oc4)udib3@KdG%AE|gG zj1lcCk?-sInD2Hy-dQ{+{DiAhuBz~60$MkxsiG#0;XI<4qd7dJvnuUpjLMf}^ zyO^KoBlt!1GYK@X-N2DBTgb94r&t}uf1H?N6QaiZyS|m90>2bz`j|uSv_oW&`)-;u z`U{g)C)m5SM7M$_Qhqv5-1dE_olDc=!AM>pg+-}ZFwx?81*?d&=)%fN#DGC!` zijL|+8e2M&G4J9p<$D&Kt&h&~$=X9UyMeyYKQ^-kt$e6op_{Z>P~oK)+EedwJLA2H zYxqNpKI#u0dEpvN!+E6J2>JCGH3kP_DOz}KHL_zWOH4V60qcl$(s>Ki^q-Zt*(a!5 zcsD<9zhB9)0E#D^kbtWHjA9sKfP7~UYx~#y;+cJVakOTz+??o=i?g1D^uoOb=LcGy z#k#7l{|xO4j%d=*rxacceaI5G_ZzB(&&>mn_pUcwsKqmdjE68F~Q1 z*<52#E%unpT*mWlso+VSR*g341lY#@z;rOD|GyUC_kaj8U+?1L!9QJmlPJ9Y)a_03 zs*=$2yLCfk@cD-46s2uFUk##kjX8t*`vAyJ2~S-ZWi)-!t4+dzM3Gi9(QrabbR+EI zMy~_AWQfMY2M2uz>UF)`8tvTsdNyF)?&&I2eYsDt&FsP_Z5C<&vHE+OvY{i} zQ8}kjCZ&a_e04AXw|Ob|n(V*?Mt;=Z;9aVz0#eN%*Qh9p_Fz5fgJ_+k(DPw?^>+0Y znz@rE$$U{cN-=3Am(D$1eyIRuJ~o*+0UJ1ZSj&3cC(-npawN&h>A6A|zQ(Qi;xqAI zf3ic-#3If{MM?ALK}<^hKV&Was^oSlz?1739ClN|ypNFAX!htF5F$t{4u?&I3+4bR zP~{`t`&vXDR5ff%iS9xMhqM#yto5ePWH9UwPmkMC*X-efhQa=Ho=9sOHy76kU?HA3 z*!FtNZCf>>P?t`7n4XwT{+LgVOH!s~xga!TiK79AK)ic;<~YM^59mAikiW?UVtFbH5D6RT#N~9RTElG{fSjxR2 zO4B`=g1-)U#0X1sQ=Fqh0@@EqBLXKZ6Szz+lF#b@2}e1m1jv$4Y+2KO)+gI?TkC4) z`#wwn$OF5^^uk#z{73Qn;<4ynF&E?6ZsD_*!)Ih73pc)6KZ7nyS)RYt|A$8CT~M&T$51Lu zdTyPQIQM)(?(9B%+ja6jvEUbL)h$+6Ns!2SnlJe`Zow~-mm6OYnh;l%*t(lR4sV1L zPyS!X4CKIZEPk{a_PKH*SMpBn5Dprr9%jb8kdlol2q$4M_uZqo$pY(UzBmdRQib09 zXFZ%OXjq0L0;9Taribrx!gs?g=iHOBD8Sno*IiF_OYqCG)rBG9j}>>%PtU%1q?)O^ zg02(`o0W8q-~LF%*eF}Aqgh6=*nQoNlOSw;gvx71v1~z)O2Fpt> zHS8=T`Y|F}GFG9xTJWkHk_yeAzMw@M6m4kd?6bX*e)P+nZeXo#g1YYnt16=%y=WZ@ zu06zU@dYAM?5Gbv3215bKM;Hd#Vi7Q|Tv4j*Q>A?Jp?4?cUqZo(lIc zBwE}BmrOkJ#>45EXys==zNx^-6cxqk4-;_f9_e>y*E$U_*aZJ_z$^321Qo-ujB>B3 zE@~Nt^X_|i{!AZ8qYHiL&XD12BZPMBPWy}aT~f-LxR$V{CdKCdtqX#>22r1x)^GvE zEG3GyV%9!9@{JczE`+Mm{6Zw3RYCtMRVqmnh-$`x43;s~4%?i4h(~BoIx>T5Voxn_J2f1A(Ts}*Ett8w9?9$zJ^!n~Q zpMl7(|A2^pyVzY}C{SZ{*H1dcR4rX^r(Kf(Kc9F1(7C$)ig4jH1!(6^;!vS7hDFuj zMIFH~u#z-DkP*4mU)>n$2!{=RBb=J{_RuxAV^^NDv$LAURwCH||@zbvI>6OHe53&DLi{Gr2W^drzi<@+^z zAP9D1$ZJ6UsHNWMELg-119-++H;zjADs2m7wtXF?Y4^AIu;cfLRcqApc3) zHsuo6*{@$m!>cREbOcO-j6}Nnw$dic#i_4^ee+E6BX8JDydNVen86CbW&e2P!^sp=*BJo=ErLN)Ju z#5>pHwTMSjpA@573MmL-?ib9OSS`WX>mpPA7cZ(WZfW7?!M;V+qu#xkV>9!pd${_H zvB0!gk%p51vyYr~{OW@I(9XE;H3rVM8x>p*9FDfsVmP>aEU*5{Z8(6{p2=r_mNdN-l6Me0obyS4I$V zVLbj!z(o@7X0qTbsX^}M<&+^_>A3B|qI>h8o<7jz{DtK6L8jE%z>!+k?iH!{gyCCP znua=n#8{Sw#~teSwhK#3$bgI`&`;b|8(*F#FqZojV)eOcit?&ToJpz3njc*&!Ze-dktYjP!wooD`>fBpZZ@bfCBO@JH&I;iZh zm|KWA>^2Dn=NiWP%F$4YrCU=%R4K@gYez93+znGE$Motr3Xrq0sdbwsco<|)g|d8>bjYC5tF0hV<*VYV@UaPR;Iaz_|F0@LK;wOSkf|0G~F zfk~j(2Rk+7HN0M`JmpXzRskawBep@FTi<#%aasRQwQ{!~9K<#vkU<@<`tSmEihPXWh~ zEzMqlXmjga;fl@g(pKN2xs&GDPE}gl%qX;hytI?o%l3a4r2Bu&it_DXZ~c}_?y|7< zk&{`9k&55PtSUUdSg+zXJdMEIh~z12)m3zz&pmpE?$(lCSfz|SD13j6lN#6h;iu43 ztZZGKGXM2*?Ve*z*3SkP4XASQ%W5xwaC9ZVt{9!A<_*^C<=u+js@ZI6KfC_(2Q$)4 z!=V)+N;F^(evYl2OtWvtAY?FPT5S;6f`Nal=NBI!c4zwlKiKdQCg^l00NmZX2Zim% zG)v>6T5YC6b`}iwQA_o`WjzufMB7-*LvZAnTw^22U9ka-U5V8Xm8KG%Xdt2J?6p4a z24+0U^eUP$w-ZKcdMs4}TXKgSC$%kOnq%+UPq+~nliCr9FcGGY?_d~K{JTT%qLRG{Ne z)6#yJ%=~^e^?anJ=DA}HL238a@#qJ$0;*A5o{q0JVos2$a!|_qqfPx z!NK8EAldg}%TEAsngQ$0-+pujLoKZRpIny-JTD2TYfsOoE=Btvkv|UxaS)=%P9509!h$Jl`b%~8 zqOty)LmQjewe73x7x=>Um*Cw*D!drLG3sgtSE|887cO?Jh!GE1D;{EID)>CkXKYjp zF;`C}|2HNe4MMxY^?f>Bsvk1Ac0-T%6fV=xe=$s3eYXC0=xvsf@gL+2vGDw7;a&6_ zlg*f__}$oGA+GpW>b9z&18vJmQ=MW8osk~{19klDGYHvhU@*P04wInaT1f7Kc}9+* zLR;&Pw{!N5_2I%c7gA_^IR1Q45m>p387c4f4+S90u$EfD4%tBQ(>X5*N0w*4ii58R zDT1-L0@-Y8i^?>|eLeVf?!Rs9@3ks+6!`RfD(h>CX1$ny|67$r`aC8LiAyED>dSMo z(_3%ey(UtDxuGmh;-*3VLlWWU1E2Im;`Au)L1A60gcoZ26XVUoQq`_JZo=8ZeOLzg zutZ_ue-Zwfj#e|5AI-~G&XyQ>}_2(T~>P8jGtJU(D&rjIm$Ud zxAd%cYgm(6#gWlY@~_546~%kWVI9RB$H;QDW_Qgijam1Kn8L9*CxR4qfenfKYRGEG zfenSE%W1(}m7k;AS!TO>Ka<_t(!D}n&)HRo(ni(R-^%#!6SGR-F5h|>fmi*^49|L! zv;_mdMW1BWcl-DeGxO_oiL-kv{mA_ut2DT&DwK;2VlC)(4@G=0DxD__+NzCC zZ=VfGj~BM{MN<_xbl-wth;*qUa(f=BJ@*DU-Gn9luBB1978e6UmB|3fjMj zlX)SmGEexF6CTY#pr#K6__CDNO!1T4QeuP}gQBLAG?w{O_Be@)I^rUMT_|zx>z%8- z5QEb)G_&tSh(&njLZ=f#}m-oTYoN>Vo7G(*e&Dg^dz41Pvv5pd}FHR${Y zd^gu*>y%iK`BkAj906L&pe&@Vk}Md1-l_P`Ut3D$RZj)8S-Gsk?0i@w&~}x62{1QZ zReXq*h0hT8H z)|bY^$t)8qt&K10$OMCRU)+U)rsbNny}WPAE^FHAgfT*#SG~ZKT}uVJm5yisAo$s- z9krjbdh6%`xv2<*Kj5$x;AJ!I08armrp=2aMF$l`m5ZI#1Ped!vLG11r@sT>B+qQ^MZ$iNh`vby@B)D;adY%1Ffat?lp0A1qY zChTk<SjU47>$ax5z$I-;8%;;msP!8_Yx7$u zy1pMBb`6%r+>zy~4JKvgV{@2|7B9G5+P>YLLz2kYo1zV;P^cso8)2Yu_WC1gdU#DPE#) zh3Mr7;oBGfh0yZc@)0ED7zesSvqsI-mPh^CZ^ff3XPi9bRmy(~V{yuli*lNNjZ6ku z()Ch*vs8d4e=>Hbo6Qtu=@j38VfwpnZV>6G8 zC&h306P$g^H{*4aJzBfJ(t<`s{%j^YCyf0O%gMh}T5c7X;_je`v<$>ELl^D0?EiG_ zk@c=+yax&c>&zh=}-uM6{c7dr7VTahiRe+Q1amB;c zO(&E0@x=Nm%3htGfnF#dv>YHLXRZB%uBe~WhFFQM@j}bZK~6$>;Up=Dw6Ru{>6B|l zEdHTk0^a^UVKq8{S4gY>SX70C@3)oD-SD2+VklekZT5wdPelZmiScCFDc5O_H(QH^ zp7fA3?S{8DWRmAliC1}iZBJ6}x3?R9knGHOC)3zN@bchiUKohoWNp7n}3EyV7ErmSggep9p znm%}^yUQu>byke^mt1_1gihv*jh$T`kQ_N6gMDSJki`5!Tgb!=6x;}D6gqKHhlvfA zMKW-E4)ZP8G|bDRoE!v->}%Tq;{*tOV}3>&_aLy$Q#U0Mkgl%;#ocZNT_5Pxs?d;M=`=2PlRZxUZv88_xN{V{IstP-qF49EUy zx?8`l3{vj;*W#NeV-aYHtf<4+_!(F7oS9p}CG{n?js-vrciSUdwtgPJxuldSD3Amx zLgTnpxVgG9mScB(``_?s5599xx2(63&J$#B7FCdrN&0aa9hIKJmqueGM3`R;I3xmE z%k#BlL#+>+&&w?hJx)_T@5y)4zV*gdR=_#C-qqFBqgFL>3I-nV6axuBGqZuz*eFP) zt7&v{lh|%Gr>xD)Vw+x$saj9u3t>j2Vu_uF&IUnQSBn1g%z4tViz`rpS>YxZfBI4+a-LNtw}OGOrPl8=I-oBvGTTI|-L;he!z22Xy`H*c z>+^=iyg&cDe0#5Gp6wdBRN#MCAA*;ZKbWW@;XSvG1$L;vicUR zx)tDvj~b%XmdVX7_0*RAr>3=4s7x_emzIpm9wZ66F)F#HN|1 znWqnSa^Gh@WX!_zPLU}6gTm5sagVxQ@f+vAaze|D=#%$>m77z|@=?mwaJh~DT_d*H z$IEmoB@=la{>KK2#g?Hs#kpx$j=df42#>q|?QHn|o{E=9)O+WStjo6)pGJRs=HUs1 zHfqnJmYHvpv;f(ci>kQr0YLK>7k3$ybt7Z1l4WAI$aeQBkFfaM7sa%{-eJrO^p&#Y zjy!{WJ7{`QF#YUmdeSi|)btpCi}{`u?(A*)o-J;jN^TbVO6trK$n$_fOJmVDbxYN0 z7kpliiwrwb7}K{_`evF0&>@;XXen^B;&RL4v{&izL3D~e-^+xpC$yI7T=~oSuDKq3 z4(Py*&naE2TUB-TRTgQR8lI|zWqag{+BFW2LV{<5{eUpCBBR`C;+%3{_%S)I+lJ32 zWYNvZ>=$gl`J!NNTWb}J&NrHPhqJ0b_}W~eq!dhD|Ih+1V@A$hfH;wxzkSH%tFlAD zBLL5aXezh02h~7BG(ymV>f4qT4xag=*+60%S2TH~b4bP;=KVfnl)Ag1s9JTVL z13a@{o+;mJ;o19)I$GR%sVaNms&bi8n0ZkPjx67@2)ktka5 z_{2n>hV(f5#aN6rhFF6(LsN4zpnTc}RF**!!tU_#>cSO6iujr6$csdU8@5iJKKwa` z!+K~|a(95qI_am`o;F^n)CSj4a(e?V1FJ}*_S|T?vR!0)n|$=lbynOnDB{d>d>xxP zHFXh$YZGI4(@4p?g?NQD{$VBIUx>oiCE0VlUTgVi+IKgF!gc+pir-vTv*A1q1?<>O zOf{wqfN0px6Y5@GY!S`Un*h*YRt{t&Z+B+%+1O)1iU_r(3a&-P7TUC|3{^<{2VB}-@)}{tE`->stM3#CLr~J)m-?Bk3v|uYc9&}T!C4DV+zQ36}7eqJ@)xj21rbSggsBl-e<8Tnsz|q) zzVy0jDrfGkv*~}LpGcs`-o4W%IYLj?>W6luSn_&{afxjATv|T#g!_;7VY=8>NRaRF z2YRuE`NY6-X)6C`tOf$00Cj$uDs77qbZN36RDdiRc!3rWUkUytAyY$qN|(d#GVZg9vpk*AO5#ykRcWTLG|eloDA%W6oDZaA&04Q7Z}4Y) z7BN=E)u*qz3cWvc_3k$Ao2{}}D5+s@UkK|cR_fK_{q&olQuO?uI;tk`M8PPHqDf-F z|L978JY83$98@7+MY!B_5gpck(S~PumJ8?;*8DjLdS2shYakzkevq_B>n*DZiNO6# zqSHhMD9>L{WujW2%}7~dQWJkCst$*t|H*cRmxum^w}r_ZAD_8{@TuL1?n<(qn5z!u zd%W!HXri(WQqLsOg)SBvThxhy8qIp8XO+2aNXYr17Lyb!=C$dB4!}!kj$tj<1yizC zP@G;*rEgFndsfrbZoFn}SnI?Zz$)`Zy(Rcme5Pv-$0DTL_L(AcXB$q^u^v=D;hSE! z1CHoPjGdTZyl7qe2(GSy~RwJjr;l?R&W zc(3!>SYwSzhpsM$kUr35!j?92q4EnJPXy)`#wHRm7JS3qk>})Y=WVzE3aqTl-cV@r z6x2xaJ(Hh%ic6KSyma1Drr^olMf7oEHY-#>q6&-ZR~L~a=)vbV-J37PI+Sv|-wI^v zVjrvSNnzpYuQ(PUYCbSZ_#1_A=1CT(qqy`(M20qvt=x?WeQhnQ;lA#lJL;v^=C*xW zk!JCdo%qFUcC#?{s3-mWX?;KmE6hhnvWuA4Et2PLgKHXI?QX9@vWp>%WCZsriDx|9 zVm~`vlFyAq)ku{6Ro{2!A6-L7Vq{4h8ooa0F!rbm9y`z|e7b}zo7^8-aJ{sL9`mi4 zJa5?;Y!rQ0O=u?qB0PZCH4=#N0OI$Gt>UY3V=i@!pZc#KJ2lN6p>`*R{_42P3BfetJW$Sm*pnMbM4T~Nw zO3`P$T0>ckVsGPh#G!r(3zn<~pt)lrSLWtnvTBQeF@oC_j$(%1cN*!6J&tiTl(c{c?%YGl=k$umvu*cJbx( zMME-x*9$73`C`Va&TV6j0a7k{xpsW5ftza1F>Tb_a(THj!A4bw2ADTCxsVUXCWqx( zp61jjH#q{hGMXN~QFdu@&YKJVchI&$cp9|sxrP9n1tMW)59hr?JQkKLIEk` z_7Q7zeK)+9kOaXhNEjReUJ{=Q@x#ay2=+OFdt85Dss=(pf0z{=UM+mU zL}k11F>l&A*BfRHZ6N?^)vNazjI2?Miv`#%RF~%uk!$Em96I+DeY`L$bCrB*z;GAH zs$;}AVNwpXxv!UN^P724C%^{C`X>MBn~e5*Rj{b?yJ^;a&R0t4F#-MJhXq1q$%rCS zAZg_I*b3la(=swlK%fX98VDx@n1iqjW!p9vmR-x%s-=@{TWf3C#;KOwvTe6&VfFm}&+~cS^|Eu$ zeP8!=U0<2_Kp8J->lIz$408fNmM^&*De#x(*lTl()1EJRxD*s%X2in$zA0U9sSWfp ze?^FZeo5c;JL#&jla(|d96+o|t3r9Ef5A<}*FMqY>l1cVh6ez=P|WtGO+I^h0d^su z>F=qA*yn|Hc6qj4*7q$Nc8!AFisnCyG*_I0O=i+>9g$=ti|}~=5RpmY`RfL@sWg$) zFRXv&ULC2P50ik7-ND9DF=NyJG|AL#AOE6D0YXYRo4N3&qN4B7;P3C;$I4ISz(gNjG{xU-NqQ8!Xv5ueDr#w z$hUdW)mCymv5o$qlJz>MxXzooTTFep#b@`H+#$}wo00W$P5PxM3)1?z>md6mR*o4H?z#fm#b*YiHM#lGGV%It__IS zsT_QL^;fefOS8?90}`1~rBgltIc6c0fTRVGT>?f(fP%T(1Q2e=$hm!U_6ag9bub4S zEStXUFSfN!0C#8p)QhY-C1s7`A>BZK9;sD2?&s`|{(Q_QeJvqQTFakN7bxhPzfA%+ zmR%Y-W8k$2zL)E`6l}DtIZc@eDBl>%Dn%;AtmDJxRR28c0Fi{HV{ex55#oHnT%bAE z&IQ=y0AIch$70d|;Dh@$8@G73azIOmuazmDD<5(xWYy7AC07|lwyg;$SfPsUGJG%b7V9zp5 zJ!r)yJgf^wVRvl0ZAsE!Ovya|*-G4*DJHzAoR15=Tn1jnNwODqW`0B=#?UyZfF4punuo@bqVE5rr zGbWcHLPJN9aDxuNE_F55p4M>x4*G1RqvQ1`eAtgV(;FRqSyNLnRim4qZ5p_B^#XOs ziPK&xNN5FPPA_xSO#Ye~@sVqx?y|uwE&gBdnqxWY6#B4k;P;T#Qr-exl5z5v(+zu> z5Pi(48Dt%PD=;nfz@dU&Sqep4E{=OeSjbMW`6$At+E)b|{j}+c*&p+K+#Q!k=?VRlm6yaSRa*{<>maMo)RkyM?2|#x96t zR!QFtZ}}5VSvIPdlWAG=z3@<7<5AUd)`cY|ezXV}ut;Ifh<;(Je21wrWeI3G|fi=bpLM0p+G z2l=YpP=>|7km3-~bF}cn4{#*a9wbj6`4&`u?|TdY>MF;O{@j<&_@~n0ViF-#$WAQS>Jr3a2b>a6wocbjq$g_V|ve5nH>Mx?FcR5J8U}EWfGuyz9F_sHx)Vz{7X(* zl>)sq#rOL|UC^)9si4$FwA%czU2|$8w>5t)b{m}|h6`nuRJrcy51XLK_!l#Du=tB^ zrh?cuvqZCs@>B+KNy^joB7`HPj!deNw}!YLNkImJ8iB_WDu=7QNxhE3CbSj3K!w`& zZyB2IkRA|&Wl;ztt8Y(PdiR|OnZ*@v*ulh^S2l+)Wta|pvO&iCcL?1XH z+nd`sXg&95#DyAFu+wZRJSHXRH&OouW}gz}-GLrvV~Z#=ErvjxG}g}|2hOVbPW z7Q4(lAm*n^nAL~s8fyn1bB*wfSG93nPBHe{8gS5oCc zulb!5sj6X4{+{qgp=rPMguRS*Oe7%V5$~|A!8rP|()g3OrtDp%>arG}LISu6X*ID` zBcKi0VKs!;vQNy6p3AB^SA7-F`DWC0X9v1t>!_FAh4rnv3j#%RU+!7D_1xP60--H} zKwWn|-n)QUi(y?Wib#(Qicb|M=er{Ro=I&a2dX5I^z^;!&77>KkW-j>80B?+TT@tc zw2;cUOyjJKSYc@X_|7_s3*=E`IV0SHD)CDvniL|CF6o)SHW4efz=4V>g&9$t#)cyW z^qxGb(v=Hy^LQo_}s-)9vtW*57m_J2}0!Zcxxbq{ootUgA@LsI5r-5UWqEUl#L3AD!;u z+D!JjS!c_nR@}t0Bf5xgA%}@`oSsa=Z`e`lst!oCe%=8szIdX^+>^)-m-kR<8pQ|q z_)?RwIxOmC)a_nu_TyNw7?90$e&1pz{ogM8SRgGZv|XAqttf*6hN17ZxU}uAMelO` z9AbE(6#?JwG&fb3P-^u+t0AEkvIqHr;_PaHf`lgJf*%+=j z-CO%vkKn8*Jx{t$vf@>PkNdY=cy;>Pp3_?nkYkBZ*L*A<_+z9+vc-=A(sFH5fZ?>L zO32HGhtew5^!`2m5+ee%7Qn*wm;I^kQo*_|r`nuPyz>!f9ipzkmQ4CIiLz*gr~?y5 zASLii(S6CAA(Z=CM8SP+4=uVrH^rGlAzl5|3~LJKbI7!-?b1+3iN?-|duKnQtwifQTCPp_x+w2HkefPfm`g=7iLU&{G}oP%S#GMOe^4 zJWF?XJm7~62%Z1=05XdDhORKcKp(vv1x_!6)_7NTiq%r(aN9nI4yx8lA7~z~11Nwx z2pH%9ooDj}vC!aDfmJ3A_fO!A=_6dMt#ZV`|3M;?3lCj06P7rO#$Ppcc_eGRx%w*- z`HBoEUP8VwhQw1swMuOL)qWa9?jNV>qQyv$X0~J#wj^_0%`H+L+gO5w@FGqiT~<6QhexghC3E%Y}HYTQ)X)U8D5pSm`x-cmcR&bhzeD zD1-U{dYvy6N6cNV;PpNuXj@r7M(H-9l6yxKVCIM@^ky$Uci6D%tIEEP#;{xj2W=wQ zzX&X$MH$k{ZeL()x5f+w!u98ASkwL4gt0Wp&?^NM{ur`w-fav8Zyg$N5fl?5ay8aeHM z%B`oTCm~iC3B+^aG%$|;LxeCw09lL7xSRR{T9_oIICB&f8>otf-E!w)tq3cG647-V z0tM{fjtXAtJtb>&DuMK8fXgHj42;z%X_~23u(t%N~(uc77H+iBeVq0 z5g`S|@ZTYUSJy{eyqh&r)e@Hi*F#cId?Ahs_&B7{qjdw~yiC%n_e-HdZdCm$c#ERS zxcHx|58zi-0t&W}7RC~tFu}XA_0xdg!yPT?*qBZ4CHV%+DTQTh_QQ#=u;KE^q^O2Q zH7xe4OPi)JgaQJKfBoQjL1WV-f>^dPsaFj6hlFNvB$SjKLn-wHQR$4vbiXq`_e$&@ zp#t<9T0M(cJ$v>L&WNMV>0mfioO~ow^u%wcbr_Xre%0Vwb)k6tP zvECJ17n_Wa`ur<2_*B4OJ30Lo9n@VWtUM}_k}hDKINi`_pNr?)(0e#w`e#=3_l9^~ zx(of$8O-KC;YZn>FB#%`5u%o%&$!PB zUINp?z<*OA?@aD{tB~M-2RDjULpU1IvD{eb=K)sH`-Z58qtk$~^DkenskoQ-y&-Xz zOg3v+m#-a6G*>3u2w&?bL|ePQpVayA#DDZ@N38C)f} zD}VUEa9~DRHeBSN*dm~BDtZdE>@JQ}h3ww|xB5|cSHHJc!LGCX$52k-7HP(Cpwg=A zkfp%61_Ds|KHrkpjsW_9VrgJK3^)S;5j=|9$`&(u(?6*Q z>8{lC)Gv4r(RVdV$mO6zJEkQAdg8rl5|MiaxHyS-Jt12|{C%#XKek9=Sg@HepT6RK zV%Fue=9z%cn%;I{vV!v_lDorj9}ap+LYtgR%AJOS=P#teS6t{k>Str+?4a33u`;IK zDAG#D*=0g%2noqzS3eLcKbYPfwDFqFOVYERw4lJiS)Ou%Sp$g_wS78&QIirxL`CR< zyg_OS`GM1+vEJ6k2)e8uLPjz>OYi;$lR(=(VTDLSRX(th^iv)R18`$2m-qFh9HX}! z^rlbQuOqWKd*2{_4TfIdv>bS;2i(fQhB5y>T1WaXQhH<;%gHMxOh|y3qs>y>rPS)1 zx9p1U2aQh`t$Q>7Zk7;zU{w4dj-YUWV*}**d;`5;W|HReaxXaJ*Uz5;<@jaT3EeTW ziStGOg|mrk2#sTGy9-lTjSViVu#h4Uf))onT8C&B*bwPwZvod~c14?geTg+31KjJ+8i z((rMC+UPwWy8~`)KfM^C`AkuvGf?g7>*tS+Hlr|-r_(7 z)*>01nVjO{iNh2@MlI2t5vlYaOMag~>;$6Rlh@e*c96=)kCtD)Jbc#`>dpu5X^~$8 zipN-(K7X;Ur(>WM4ghP5u-awKAsw5}Y{2ak%fbLFEDdfclimiQQK#y7o#ox9ZFau~ z)zGd&>{Z00~CVSb0uo^T{ z^D!y@o02GdArfZWULBT{ET62PQGY<({JTD7A1iJxmh*pWsA@}rZ_mvofeDglx`un0 zdj7wb$3?B{C+JZJrUf+P--J|~WUjs^8TqBHvp9`rm`*bpC-@6v`N#G&HZumm^={o9 z-Pw=859ZEQX8F-I+Ki%2+p6~z+@3h+8Lf~RdHzmklz|(M7<)a?W+|`YGEGf zYmZ2mxcrjuq~uKtwfp1#cI~s77|iGmhQYhP5nt30%jNKxZID&lCqc(>hAqPkP$u|# z&W^YUhpfy<5qbs4+)2v35zu!S1I>sOE47zb5;lEu^yK38`F!Lt=fyoI>-{g z^4*a)AW5j@r-b*69?ts|Ger%UkMUKOLW8Cs>JEap01Qm{Y`@3zRIOmcy>ZPxq1m>; z>up08_78%?%yI9Cq9i}q;suhiY*T78bx~j6+23T@UdzYZOOQ8o=|46*YjSa*d>U-l zsExc;AXMxwgdPlO$2%cPV>i*t953M?fnTIsboIPDu354pyoQTt>}i=CPd4uIfdahM zda2rbN(l!Ue7YEc6O$3(w{C1Kd!HVjm$MDW;vmBnjR()%|@x&0VMyRd|PJ8;* z|KONSW7ndZMG~6t>>UHtszpS6PQb5-3gtI)2pNWy&^@Rsh*Qo=9db5~jv7*|HU`4X zLz2MIIQG3P8lx5~#*4|E@*tbqZO9s&gf=S6zv#6q^|bj}EB-q=Aw@gIeLpeibq`3A z(M)h$Yx(T1?G*{iCcw|+&)mdP&5bA+^p2`SruB9=QxGN|Ti|mL|X+-^z@paSL zw3H5xq8uA6z>9-NrEebk`WN;B|ibj^R$%OIM%n zRm7qVAI>s@;-@mYgx3eG{|F(4r^QdhO5w06EK%R_qxT*g_ACx8@d$lkF0Ba6X=IjB zH3VJpXx-BC3`A>xw4yxVZd-Ow|DE9E%f)6J!Jf%U3j81w3-w)7N_~E8A!M;>CPtOr(W>l192=9=l!fnJDd1aKfhH!kk&gRS-1mGsWRSv?@np<$k~$iV(a7 zjG};Fd3-DZ`-sz$@RB!fDWNPjKkoKs36j{HBJ+%M!SMqe9SA#P(>nSd5W$5u6i5sE1oI!s5{z}1K(URcAk}@;L z%#8Ust`vR&O_PJHSRulvdsjXyiGKlTrzl*HEKrK?VvR55|6(FU7pyMw&m`GzI|^r& z(o$G!f&$4N-ai8%`4=m286e73UBV?whUJTchoryU9Zy-w;33hK_D zttN-j9&9#Rtk8YH+Y^Y6O5=`wlb-jc_l0JihZwB*kp}r9ET5Lk%clQt@1_++jboU7!D870fT-NX@aO)`I;*RInJ05 z&WMn1ZTtUPY5Wpzc??O`?fhP9kLVn%k}tnj3^Hp)kQXz)lvg>$M%5DIZyCzGDHMd3 z>qN!BbN9f*(B-$kSh~J=!ml?>gxM6HR}mk&;j|A39>e8hz#(Ar zZijj)(v{GwxW8F-JhhWZ3Wb-^XMAf?Ia>UX%2XdV`1m_DdN2^xeY~OGaxhlQjz!jj zIjzCAu!n@~h1k5*Kxwm0w0Ij4>4yxTp2m>=wML&R(K_Gue-z0YTAS?u17>sY_&Kcn z@x&_MMOTT}t|j8Zh(trUU&jr_l6k%E$NcWnm5#B0$BKoO1Ki%=4KAd86RR0FP0$v5 zWM)hEeBK*XAd=Un&-78yPC2D1wbbeA_d_rr4r4LRlG?TyR|B{Wyo zAf6=nf&1cDQ-oXVXDCN6WBD@8K0xmdD}{(U0Jh8FK!xe3`Qh#Ywdw8T$X5F4r_}GK zL|KlX&}kXSf~5YB(sMjY>G&{75!$C)hE~pS*LX*t4rcS!mm7GAS%qE5zw5=$I zI$@w4nk2tH%e#HLsE;==-YVcDcf|&IGxd4TZ zVXfh|q9dvVpWhIvfTi?&d|Prz(gy^$lR|Xa{TqKn8IV8t8TUYbe1vQiDy)%c5>O-t zw0j?#O|RO%oq!+rri^pwuQe!7nh;cVxUX`DRe^!Mn7|7uO)rz`RL+Ra;g~dy02Jmg zHAej=Se8mF6OZ^+^@z)WiZ^cUpxV`S>F*bq$#((Rd8Sb9lL1=Kp6_W=#2lPcHq43=|v5o^q|582>P3ww#R^>xVD5ErpEj8pfdGP$ixhRLxYu_@9Szq66889E z_s2$!@P8pce8>&zTq{`yzj|>%s&cP&{{_F4wV-8ZAsmHx8i3~t9&}S1By~1U#q|Vh z@0<{R7v{=6vUiM}2if9UBM_EjIsem-oo9U5`+hSnqaQo z-SoRk2J457Ay%SRf9+cS=wARD1l6{`$OrwA7j9@>S&HL)GM0&^QeyNpK1x_5s#~+= z0VG%~4iH|lLEiYw3uE7lpC5~{&^GRjn|hY}m)R4F>(oIgL_+LeK=5f-%;Q^*Pr+RF zg+GgL-g%y(@^8By?s&7baW2o;LMB1LJ=(!V#PDI4B}DmOXg1>~BOZYT}?f!8#upQvbPzs?RAbOhO&RT8gSc zFLh;v(RbDdIb$I2UJ`vGi&6yNF!#j$6!>y3pj38?rwi4dVzh;ICLE*jXP6hEZADYO zm5F5D_uGK58&m-*wNAx`&u zUB6e6)+@-GCzeJRi}ziiK~-=lsHovIaL2Twyb?2DVDZA=8?%xBbC`CKu@155*+cu7 zZyHY<3b%B$XqWY?-oK+t82YyK!WytF>Vmg!AN161&HI_x=9^Cm{4dQ9R%XmSy$rO& zKMj0fKu#3p+6dBKw-gw^cD|rG1~rA^3N~(Z#2!rp=2V-0o1rND-7c@?OI|%Ls0;(v zQLe18PFnM4Ui0*4kPq-7MLNsWQo9N zIRb6+FfN&xaiPENMR-LpnFiXfXl(A~n^boBYNVrp>+@RDifQ2-R<#_%KN+Na7g`jf zltEfptzN$H=AdT2ruIJTq&#-`l#Uwd-#qJpqPmVn8QRF42k!b!(n^K?$M@|&>taca z;8T=oU|`*RFJJ$DI9=gbmy=k@^{XOGzexkMeN^D5)-bj!01`gbh`lO@cDVVf&M{Qi z(GioKosBipoiN*79ee1Rs_X`3+YvGJNbFkZrSam4HW*GleJApJ==rL8Z^VU6@m7;a z@@y0z*&7#Z3L*|F2OAxD3jk0WaNFD1+Y$MtV9%hK)Zh@%jRf1AeCr7!+WuvylwfW} zNL?x;QiMjy?f<2Dpbc}aV_ao+Zq{>Valcxa3t_@xSvF!c$48l!h^fYt(1B}YDz{2= zGxN)M#2BiA@|c6At}(k^3V1p}^NGtKY&@AljiEX9}+WF|me@%`&e=W#l(bI!< zKBl%gG@I2ZsP!_Q88? zjwAT1zg7LMne5>=o}Ml6%cmeQr%U6!=6fBHwvP|J7-qu>x=OyV<)Y?Ls3R2P0}QmO zY}uiw9*VqPZI#>?>NDW6)vk#nQ2sM+oI4B&>&#~|WeXfzxOfaMEDEF5kFB(A)vu`2 zv(??T@c3L=mUQB}Bg)&3{!|<7$m+_7k~aaeY-<)^4en<>z3ih#m~vGof<$Bo5=VsN zu9@JMQmCb`kqaJ{ec;Eu%z4TwXwa{sFRN1Xu@clN;8itLdyIJ~RQP7@o!Z=H4mPq?dQdBHms6MUNq z^}hBvXQuiHY|v(qbC-$!?){SP)uCVQ(PvvAw3#1XGhR6$je{OiTk5hd1j=**=2i^o zcIp0{)q&3upjqNft)s-lSxNZ<-G(+Te5D`CdIJVq^}B%&nE|*Ito^T02sI&n^Il{? zD%BiVQKUjb(_a^KJ)i)sl}x}q_2W#){@{bIF~=|aatSTo6znS`S5bz?M<8(N=bZIQ ziN*Bk-aRa>728DG{?5AEbRe@XmljcC_rg^5(nqwt+yyKhhHP8Gs&BVsi`!)`Q!F zFIHTD?JX{gc7_TRtUt+bJ%EP^Wj1t#Dx-{VK@{uZ68|@k^EXo$6``Ky13h^LVl{{=>d;YFR)kikHbs+?;lr#tE&ej zYNuU9RkauthCBEMPxyaArq0j|VPRnnOhS7POXnGiA;=>K?sdzE*9p5rL)tLwtp=9` z?^|Q9!i5{Zd%-3) zn=}+zTHQmT+Of{+7(%`2-qB+5%2bc9$f};WY@c*F(lap~THI6mXAfQVdWdW-#vjT- zU~%8iWPLp0uh-;mx5kn#V!Xox(83820)e6!)uNH8@H?mH|8b-QC0euv?kumKA@1WKs(O148 z6Ik7A_wHf&nQpBWKq;OJLnt&F1oEywnS^23c$wnT)_Ca|*0KNpP&4F!^-L!fS>CdW#J3K_QP)!)&oA+8w-iFb21anDU;=n2|+{#LqnIptK9Smj~9xyhe+3 z)Oyz6M;b~Xwu!sXO6RTpM5TN}xT{0gyUq-7wofB6s-URtugYHj`)_AyzGGbKO#q=e zHwi10Srj z|75~qQFn?@s5;T22N|d1?ahR58N|RXi*4cwkAce2L&`{X9+Y5&v*o7LQSvJ8(%D2I zp#qR5BuA^u3QdxaCc-ygm$!d(EmjZ%-^s**1Xmj_xP!A?v`t){*}~2$n)?{Fh{fcy zdE62zE<|T!0F~Ofo5&(k`C8lK^cIegwHAtax-hJd04q-xfu{sX!FqenTHN|7z^t(m z$p<2Q1=v6=1t`C^)XF>dT9T}pZmRJ``5{Ui4WbW`lSu#gEqP}+d1mn2$B06{aed$F zJ5L$6hW2qZahiE9h6;*VATmh9(d6? zD>5Sxsn2N@#{RQ|Sey|Z93zb+x*=7_1qgT!@HzI;kN*CpI}|kUzm`)At0U_VTKLF1 zGkS*+U3sH|DOb^|c@UfrW(SKcF1e8z_4pHMJ*r4EJJ`ygB|-OvU)%vU098+IE=Ztb ziGMz1o5CWnlb|Mfx6895Gje{nV+a+lZiKEouuHAcJ}24k!3(aAautE~XwQ>v$R&q? zb~QV5rC~h1z7-F_^D6lA-)Gye9a4+#MRa_7vmUFGR8g@P*M$M`ynW-WLlX`*d~wcQ znqeie%g{6zT}YoEiPc}_bE*976#hXlj`hy~EMjAn3v)nK*qjv72YHS=)9LNot6QmS z1C!MIy$OAEO*lhkX{O-nJ5~hXWOKee+6W?2=&quEyOYOq!AL#((8|o>NwG`@vPIhC zs@QBNH!!s;tLgqrnoYv6$_=}0ZIx>f9=cjScvl@d2-+KT14G>p=)&8}Hg)0iweVrD zWI(&y$w0x(pvDKu6OSJa*6L;_fp3s-PX<%5S!s?a?n{%N zN*S@IelUzfw9Ns8dqR%MHiRSEQ`$aFprcN;Q;`7LkGOW0M}}l+fn52qoo~Wjj!A-c zCC}Hc@>XtY>3!WVmz`Q*Z#_(@84pog;@}zi3RDa$-0BOb<5sfFtDjt!ZV8f(m1A?r zD~(L_i(X{0v9Z$GtCAABghoFD;RX`$UZT#5psxU)SadWAWpTsK>}^o+DP=a@hv7>xW#ba zHXn-nDA?%*RfGGj8q>n)V||Mu$+Dy4?!NrxuQp*Qe3>lwA0TK_#;MZIW!avbNbd=_ zStR1GFv^Cp%dZ2fX<`%UG`}bT<)&V)ap=;m>vc;gLv{Qo+S0bbdK)lv-VV5xybca@ zrGe3NNVBT4KZ4jMnFQO?pQM-uhs;nbNz_BRPo9Gc+K>mhq;x@n*cU(6+FpCz)p=gFCQ5v~v39Hx`Q7Vp zKkBM~ZG&|g4SkOZy6CG$95h45^{>z2V5EQ#X1Pi2c>h&R{A(KR8-6vC*7JuPfAm;F z{>l=W=TuxZ+?h8GlBrnqbA7{vgR}R*0M}finSUAt#-7Q-`Joyd+a~T_rc= z#6u&5MNp9=<`a(z{r~DvhTUc^n}Yp7Y;Hzm^F>{N2?0y|4>^}dh0kE)Y z^Uf`RYf{j8%vcsAgoO~ThU#w2bmtnLr&ZTR?d|n%s|E%&>(wxoVN+k`&4>J~H`8;z zo8loQnEKN5?rOBuwFGp?-9})%{KE~uHggTM1TyH18+}BSN#C|l z0VUD~%2wO1f#Xa0KYz5vrts6@O1+m?;+RC-KI+^Lq2QEKFVOq;ty_W@N)-2 z7T3(fI=oJF``9*i);|iAmG$;sXGKDkD78GpLMs0%U_DgDPc@SLt$hx#XL%fU4hs4a zrZWdZKK3<%PPR3w2jVeykoWABp*JR|Agnx;L0P&mv#8ZURg{nw6dTtp(jlvcu@{AW z$cd2N`;d^cco(>y(e$@ zPoGNO8hzRjkA>kv1IgZM$^zAsR^pR~zKU2hb9ZUj?iFtR%>kNcPYod}lIzQ+h<)Px zHoLpN=Y5}j#h$jR`7gJ*C`oh2@E1-Gfu#6626G#2FtgsOz_iXtDjbQy(@$jg--5um z<;;d~+X$$Xzaqwg+Cv3-Ol|ucMQjD*g_(|;zSTJMef+{NQF&Q;E>EbqKgVNNYRiq1 z>U3EWdzrcsn4OGl5Lva4?q5x+%mdx#ziXj#S2)%T-#SUokEN{YEE|~8pk5>^vK6JC zlJ!v**++d+O6DZYvbVT4B_yT0`Y`_8~ZLESS} zrFNZ^v;K7c=9cvpyHv|i`ibN{}teV9^seQ;g+UPiLm1VL;dDLxfr0 zznFEx&$nt%)1!WGxEMV=Lr!IuLImQ-0JFNtz(a-ya)7;=Q+YE_#>}Tu0`5Gf79c6| z?}#fWqjHQ`e(r7MxO=pzyGchoQRTcp(`uk&LusGjd%qMh2wH`XUA~8DRlgM8(PQ~G z7SgRfRP5MGrB(dP56k9jSu32z^9L$n{KB%jHVZZ!K_kIP5Z7SEUt6qp7{NLKCbq%) zDtlxp%F*<3Zpc!?aM6m)B>4;A)SeK1#GvaQPfvH+YP5M_Qo7=}`&xE4r!djI-*5Ps zB%CLG`9}K4qj4F1W4a5J_+C19-~dfO|I9? z$(8!}$slj{q$~Y9r~i6&?1p(fT;-Ih3SqlwHS>_xj6WJA^-lmT?l zJ!=w2BxGsg268Vqd=PN7X#uc+GfP8NPXsVbvvwb;4_|}NKc~{d#F}x??w#KqY0xW$ z>StcbhyGGfs%v9p4untJoW2XPO$=h6oH1Kq-R6Vz?n~eC>|afmKB{2k3?g{4{^;{v z*%2H>W$0?9TyvaW`MIFy5YowV#w$wn7xoxg&qAFFAI*Z_1`0@9I;-~Wil}yX0>2?0 z*MN1}d*V3v$w&HvHjh%(5At>nFm3K^+#Yv2AKjHN7;~!W*qYgb!4kD+QGBI+PZC%X zrFs<;7JU3uFW-2r+&?AN=|3|r;%xi3H-1nM1AiXAfe$T;?57!p- zU=!4`=eD9=HWezSHQ9~#zChfb^WEIs90~|d57`s&xfvwb@!gCX?8cW2E+Fn=5{gl1Bf_mrvJj8-9!OaI5G_uT|KJk^!=c7a%M;C=uIE1h zpzg7{bzZRL%M}RP@vmTWyEXv5q{YAblfQj0{8s)eV0JNRS?VN+adZsrcsk=awvk{O zL;$*F=_QE4z~_|xu5QF+()zz=JshelAh;T__n4k*-w8cDF_8b7^}3>nuCTG}wQQAgg9m1(eUyY&nU~)Qn~)k>u}*9?2D{*M zBCVFa>iJ_VsEJz_+&vzdXQ^Co_DRRaNyI95V&3?fQ=t`Iodwt417aXm^?+$)I($>XEZmKvc1Qw5utOIzS)k z2)Scd#62 zyU3{=l=t4}{-rQ75+^&@tESGSq(+@0BVv5Aj5M_1(*)Y!XSFQfb*s+fy$LsuTOmL+W8UxAmpFb7o=l#{#+>O{Kqqz+jj;IG{*knps#t1FSL%sC5U- z7AwmMQt1IZuoschl57)xpOASUaU*6kO;hiN+zAYATP$1GkqD;Lf5M>sL=TWk0J7@R zHmjd&fd}^CgHVVeXmpno`w;(Y6|5#&X+!Ieh%{=%DB5v*A#BRYtGpw!KWq=`@1I4V zjN%*H&VsrUT$yF&6M(`-NDE_NqKw}T6H$i)NH2=kgko9=`V!oAfn+94a%${}7~e?% z9ER)E#pZWVEOPjd2M6@AAPMlUUo69>#y(wo&T?KC#`6&Lu5hLJH~K&d-%^3 z*53cm1;`qnsc8nPnznL!?&No)6TV?pTR5~KbC?~Aw!2^rk>DTxA(i@W-9}a+B}yrc z?p2%FCMo=IQ$_mpPcp4_>I&|tmJzx}2*3yy$d)){$}e$<$g(9h*zxQ?*FAsSNrYBy z-xL8Dc9bh?1Qg}ewcS9cf0ocrdXYg`kgv2ApO?GLCY_kVRJ;nL$p8WBOV8Y~UXrr3 z%zHw<=y7Lym`##ffeDq0YsAd9E8=?8l#mdxEB>Q&*|#daiQU75LvAo~a+mTFZa9xc~ZhCQ>X6 z(8K#W-9Eie4Ox~h>&_3KE*q>v^HAEmK7MX=4Ec(Can%fjdjbj^8vVJ^cl#9PR-sSM zD16ZWQEo32jq(J(>~;v&RBHToSiW?kpJ0XJ`Cb{vXl6I{B zs@PjYzbk$mPH6cre;(p;*)%apZej01Fi-2ov6kvhE;@P}qsh<LSpyph^x zF&o*+kG#`O@SpBH?=!sF79(lZhN2DkCNNXgHxm^TF!+tnXR;1i4;D1CKn&r-PWYgi)0w7Y?G|XcfaCYv30-g!1BDURPjJQ@rc6PkV?RIx~!WUxqP7+U!6g0QE z)upe03->xH7h%bG>!h7{e-L3a?h3Qaw_N(h+LHba`dikp4>o;S=qdDS9bL?5VFe}8 zZbjFRk(y=Z-n+R_TAtq3^<-83_YJSKH+~Z(i`y#|i+^`^Hri01wTEy8Hj#3&;CDVzk_8S2ANrEMol zYih-ex&8BM){ckqwA!?k-4FuG3}F&>AQPU26ZT=a+7_9QStN^M0cP zUXBdaSJi1VCivsQ{6Uu-fN*8#7y~&hJ}FlL$(8V1pQF+$b4(7*Ku-O2O*4GFX8 z;e>cdj0YYBG!Ps=Q~?w`GZpxa^S#$)2s}~dxc0NGN|XDShpaUw4z}^JG~>AZ!w2uv zTVm10tewv8g--9k{ZfU)15OnSnci+F_no<_p1W)C=!)90e-S4WJQIfQ%yT9FtFjnE ze+IlmU&wpRmN?RcifgAWF=3-VN>^VU3Ue&~B} zVM#Z^%b>%zS<*aRGM=|KDxw2NJ$hEQaW5Rck*zU*nN`{TSbYo( zj5pHX3U)<@c17rhBqWQm;!PW~CQ050Z+JpUFs`d>TmO2^r1{bB4-@T0upm*i^;C9N6RhqAlK}u^KeCZQC{`wr#Two2E$`r?D}yCbn%(Y^O;Y>(2i^ z&%Iw^=Dhptv-eu-hk#9l#*w>Lk+7IDz$SmI^3=8aR4Q^u4{_2IF4~WtksGFX8C?lp z(NznfDG`5KC0qH+(=uv78S2%u`;N*K&hC;IsEVI}dkrfOEw~cgDWl; zj#a)4pTGnqW>LA^O%m}Wkf`K^o1iH63*^|VeIkuoG~j_=`dAdW`Emm*aYPOO1im@$ znu~y#h}PfuZIko~*3qDw;3l%dqjj&MVekrPMY%IHYaOiOG#S)RsE6)U+Nu zdaC({AV%mzi3^I|j<0{^T-+L-e&Z%R%tEx?Hs-tzfP~?@Zmp2xQWIXkEe3&N4hqN9 z1^J?OcT>_Z3F^9BwpKid*PGo(vcN41ah48ym}2%$YOCcoN5t1>Yc}@jPsRA}p-@YW z)E>?U!R%l-I^vp0C`JD2yFZ9%N>CCPQL(7qSh8 zG+!{mB(|?1v0b-orH+t;@E9sCB=&qJ(xKyvmU0x3#SgZD#6I58QdzqwLz}yothb+| zin@-x(ZGE!p^nrHr^lqQa=q$WSH|~2{KnWF5cr|ksy3enXC{#YgLDiaj)+2DdAygg z&)=W?$s2Q#jhESTun5s99$C{t{IE0c`hsR|0MiN~myo8|(}0TF;6;gBWMwTyom>ar z970)6`x4%vw`c|^LcBi#71Ke1D~0Y%kkh5%FKcHLUGGl3zHF!)RkcGV_+ELxoLn#d zMTfs9HU+7Vm=6Iu+zdf^e*0yeO640Lc5PdA6dTeW&jaTlS+s{tMbsog8~jZ(#|r<6 zV2ft|R`c9SsFbUhmWR!NSw|j>Nb){5sjt^vJP6P$ppA!488WBlRgCI30v0F>HR z42^d=l3x2yokd~Tep`yTPaXY_ihGe^2HzM(b$c+A=!EswlhKC%nEqnTKwhmUvI&p+ zQT9`eY#fSx3|9-0dQSO|Gzw>k4_rn$xnl&|aO+1D-)1#f{w|zNuPE)5r{t4SL^j}~ z0zf?Q&_-~ttp(@Hc(?%+cMPq?W`#+V8pU@05$xe=0(uWPa}@=)Fb>CO9|?vXvG(g4 zVthqlf#LJHCPIrPm1uIk@R*aWyfP4!`GhN0rzth>Qj#4Y9b~Y>BQ!EW_u11Sx7_Tt ze@|ZhR!xsvE94)A&UdoW<(TggrbgwYVFo*)MC~Xg-=fQc7~LJQWmEz|z7zGJus$BA zmD=7ml$ad~x%eInl1jH2gD3b78iqQ4I?CkFrrsI7qI;4J-20bfGadpE z?=9rnZA%@>Q(O#3Rh9Q$4f?G%XCME8t^n05nkN|jJp5DRZlb-RH+*c3)R{DKqh!9G z(8urRuZ`mQ)2Czt;*A*jcS^`u@yb0W_w%^lKi%6ArUWqFwE0(%K(;`=tzsYx3y;qp z4KEg~nN>7ApvRxBegolvW~!G9C*R|DI!#HfZ&Bn=aqe|6b#+=j;1YG^v7}2Ne^tvN+<#-TI6Wpnk0tT< zIzEIbK@L)BB0g)1Q|sj>6OrI2Iq=5|IA z+Vm}cTKMOZ0`gKQY@vBiMkD$Y1_CvnpkU8Cs zPXsX{%5UsuRMg_BSHo}Q$audpzPmCe)0=YTL5zF8KeYz7t9(Bl97;pf64p$|B{{WC z4E;iuZbB)WMbM__@tr}8X)qdvd!Bi?UQ*6fFJ7GkN6JCA94sfjs3sKy3yoMwIZ0H z)#-0Iy+dYv4uVV&3n;Lx;Gnq{v#O`_Sc1{k5Jbd=9-mfh#$$5VRObJORoi+nZ^dE}^P#S$iv{V#TljU?JBONBOt`94_hL&ozopKb5;v*^>0oEq z&~GcHFbvo9Vj}85f<=fnzTZtOLK#Z@(31JF7O{K95G)#=(97Sa2j?|nsUCFRP79mB zX?@v2R_JJr@%iGfeARgJ>w_Z#&V#W0YTi_u0Oq!a0(B3zb6=mU!Z$rA-7q#Yf9}5T zbg^X=I)~)xJ6gAUuK+iZlwJT-q3N#%_d~~=zl%d`AiPfyV^D;5JM$`-3;|slqmM28 z>II?Sl}fWqMZHSf4d{Dv1q7UW!)l1 zua9%o@0~v40e(%tb|RS9m^1C&5B=NLyyKmoY z(gSvT9Y~P*$Md(3pYg?Eis_j-U=QQVyB`>50cQgU+t9T?A{Z*n!QjJlJ1H#m$lkY2qP?0pKM+?3qc7-~+%jj}G^7lT5>wWU>EK`<^GZRAVR-*=Gj5`P9gzcgS`TS>QAFePXQ1yi}flyJGu}cC6OM5%G7Q48r7$SoVMySP1>- zcT~p`S+x0Qj_mCe9g|++(yP_mrj}K6UP$KBjWqo77xSl4E3IK5#I8^Cu|&1>ao1+) z<^$b&F%0H=R7ERzQy@IpOi4pySb=rqHyDKrZIseOwMt+i3`nK5nXZGMNQLccluZy! zmOQIq-oFG#3|fip)exk=4p2=XVil_^R`Wm|% zk##QAiK(fg^lc^&#-0i!^T7~q9iJI)K<{6+dtR?jxNk?|N0VJ2O-o zMh-uMEk>ib7wFWP^^0uO#Y_sS&#k($Y;gP)uM%mA2d+-f$3}fido01C;5tt5vW2En z|L(&Evh1K5zZB@64hlF=z;6XuEo~~M=y%H5Ul7VsqC+d%uyOG0;ah|`;c%kdMb~A%S5`owaPe#v7ZsLK9|yi5ZKH8nS_du*F1P+ zX35T@P;v<W7D6Y>pnh=eEoU77{w7 zUcrRMBpi=7V&$l5(A@OwFvV9a^n=Fnv<%;o&gaJ5GK&L{TRL$!1x~E{ix4NpziUrA zCz*N3ZJpG4d22?H2RV9f3;fuJZpmbMZuiQH7fctNZtzl*B0x|54X%{tU(V(2eLQ_y zxqx3yIyjbie;ZUuiLMHzsa$d>;mPnFn|Om)`rSr)rojiu3v~IjM(E%eXiG_*4avjx zGgeSQ?O(BB#*x@=V^ODCG?AT$gtTmt-VHLZ(m--gbCP~KBe=h+y(}Q0`&Oh#HXSYdqJ}ip%qeoVgYX zH!DIf|JnA{6O7JhXlMu@^3XO4bHi}s;+oK-AzLIU(!I5>Fh2NVNOZ*jLjc|9jvW|D z(F_9iZoJHFN*%3;?|CiJbDgyK6fQ@w%whI5WpndXWM*v-#As~ZRt?>ibse}zxVLfu zf=r~85S>xLjjekl#9Tg^=l1R4$^gnRxO+xNdzVTx(gs;^MYAmPOrv6@KHMQ1CrB(1 z{TxVSmW+ZZvfMmf*E133Xe364XJ0ICE-Ps~+G%*a0<`rrV?@6^a*w}d&dMtQP~PgL z8HCn_*Mwcq-KdB9mLjk*?c)}}0!gq$OTBbTvhz#xo~)~Kw$ zrjPhNNJ$yTKUXv9SPTCr?dtK&hk5Q;CLLNsCQ0ji~>Pe zDV!aqGw<=P=!FFl0ABqH|Eet+$fTdWQi9VjVDUYFq@#8~#63XyWp|~Qin^VhVM}K; z0Ty4r&EzVMhBkkqGipqbflpC|pdR!E1IXipcK|(uAnyPw2p(|d3mnB*m*-=ev47nE zMmdEsm2rfygqr2MrD zF5-t$@n=zpE&4x03}lp1eFh``o3iA@0ggNvJwF*R3i_>EjZlj>oYQ z*?9Q?-acs9aq<;Fa}oNVxkWMUgA?z;QxiUkJmPkv$ux*QDcpMgnEZJB^NDj6=XkFgq~0KWU9p-I;Jl>!Ibm*8#8_NVVfiTQL8{oEt1#~5oYuV@ z6CV{)b%wFk`6%?^N2VX!LV6ZOkeszK#qhcyYY&jA(RX$l_9}6>!DG6fqx;6GeEdy& zn{5}_P)E(WKAZmDLcx`@Rynf}S(ZM^3KZu=Z?)}~wxbiJWvr*(luz;AKU@(`p_OW3 z_VYC_L;h6}BNh#_e1lJnn#u>@kR>IK7PWKCE}#9Ax1LPaIItGccNPHtEJ~^vp;ncw zqkC4l123Yley51_U2QCfb{2^sBVRqp13P58^h1}_V@d0~#uA++MR`1`Ao<2on=UO= zrCy2v@t*_@RN->DhQj2HBD4V!Ys!RwqRZ2$KYE?TW>6NOR%2$Al&e0S5tyk`b0Z6e ze5&bgj$?PIy-gED>E^qM^V|B%7Ki^e4%fA&<=CSQSU3h$UhXNbNOSN0PNh4xb&LOg z$F}kC`E#Hcu_KML0L^cVqs9i8g4tIw&X33QTl0gE79K6H9=$u2#ER=F{VvO*98Nb; zXHF`3`em=DzQt+I+aOL|S>~U})C}1Sn0&#nFh^6Jzy2eZY=}d*w+m*O1QAd*ihv8A zB>|9x`*T&%_>WqtHywKdbwLGeFx}4?wI)~i`y=9-)^?oo$((rB7P8tjCx4>EqO2f0 z-oDCAn2O#X>%_I02UpDVZ?`NP!)?h|%$j}{5!vzQMf79Q8h;vlF(iua_bkDX>I)Cm zR*e?4=T%mHHh1DV_w~@No%5WwQ?H3<)p)5*D5p)R9PObNw))&@FKw9gqe1kg!L!{6 zLejbwDxia~K!m0hB})^NPv}eJ)GC&a1A@F?vtNnv1KbP3k)-T3r}R^jhAmmg)gnj~ z=o30yJS1obw*EnY>6Z19BM2~mU@SW;E2K7rNw0uN4GW3_ZH0w@2-L+9;mlN-=a4kd zD|)5-43!9~aUV7wdK9~;)o>(z%GNz1W#r0kzV5;t^bYoRx%r)$xYXT@GiJRnm!60$ zS>5vTd`I+1InzBC`f4$)8tkI3${c67-ZfJ9%nlqpMfOd|L>_gy^k1WZE>A*92X^5r zAzs30-7&aJ*6VEzw1N8q#w^Z!KxN&{Do>jn$8IA(ShPe9!niJ&9i)|`WiUIo;}I%R zv?iUnwDJ{3O;vl!j-2MeJSUT9vCv;N7I|Km%0ck?+7;>*i?(0ZDw~!TzFCw#ut1CN zGn&&geOG6v%VFg3wf}Z#k7p&x3`do#m#3xwQlz|sb|L~E6tXg3+$)mTEU(HZEWq`Y z_OvKo;y?T})+Pb75!J?Uq6_pEhp+~bDE<}L3Z9O$qU~k}{mP zXdKXU`wJ4bq?i*EOql^K;Tc8d%F0? z*F^O#4s^8Xx(k#J!m$10?^Yf~d1wUu(HLf1tIzh(f1O59>TVbn=kw~;r#U|gZ`ngm z>2myu`sJquAop>PzghMvxB{gHW%b1Z5C&=ZDl0KT8m@@siRuE!r7#L?3YV0UhMjz3 z3|c4O)mdp;-{?>+R&rW;%vTeQe4cpx5JDoS|%%)eR%)(zoG-?06<*okUnWwXw7iNSAfeUEke35brSoywZEGdv8fou5@Rj%R+JgV zqi?;2EFEEzuy?>jAScdl_UX;EhqCVCsn__)hP&@oo=U#fcS4mM6mjgALf*KX*?Rv1 zyc7(mFFTG{I;wp(D(XO16*%hHsH>V_FV^u_0bhB@gQYV&fq8gqtQi2g90WR zCGCGd(S-T*`NrDXo}jkxFOfL zNn3P&?wK!v!z?z_!qa<;HNo7FwxZYc_kEXgGI*eV-M$GTw1n@v|JqFJIDVK@9AMk- zRQ?-=#B#=CyT~TBDu$XU2+Izi5##4ET(;P1T3|G{g}Q&O)YZ5!La%YX zc)TQf(Sa6Y}W{6HqT5!e2aSiYNAv05i#oYy*gqpQxT#smwEe9HS00JGRRVSSxI`Pzg4N%d+tl~ zMTCjQfKeg9i_NWn$I}n=TRPV7-8nXA&CNGkS1Uj7pB77ZFty?UWgYc>{UNUaW9_b^ zjEXwF2y$i3c$s{k5#PWue8v5)pi$$=ySNS&_slY34Y`Rw4sPx))ENPnprKh zP$cl<3|W6^t@ppO0*%xJW|LQf?}nmJ#z7%&vcja z<|8UeGg?+wjdlw7Nl%k+ud~=!v!Wz#L$L6b5}YD7f-$N*jE0EP>+%8<06gS5kCGx@ zv=K(VnaHYtb4to9&q8LA{N_XwzfoSdmcY&9 zv@hk0oRSsP+A=D>Q;XTnmY&IfK4n;3)+!G!Q#cwYu701dIPGQC8WjGqZ*R{|O&~ME zJZkTdlJbnS*rzyL|0?0u4&uqcTzBZ?A`0v>kuCVA;$i*S*UTFCFU#4`EFv6llihaI9`<8W8 zYa|`RT`IbSs8(1pIhz6;(dUo)8m)CT)I2*RuXS1-*5hrl6{M5khM2BAt>ju_4k8q{ zTyJ_I-8|PyHeflbthbft0+gy|MVq=P#FpqwND?F3@B@Wvaw{7`>-)0s%X$7Mfxb8; zUfJlC_?CFZhC%>3Wlz`m5FH^33uDnA>9*i99Ro4BhkEC&i4HOo*3)(tCBX!A>IMm$DA*}KW$nMpj4S52Z__J z6VPY3O7~03Ih(>ka^$sjbo6v}<(OMa>`InQ!5|0-UZSq2Cw37*M=woySs5EVS)rrp z#aSmvi{w}kNh6oMvF|2kAFHy4DW3C>CjP%l-KN@0tmD)`<%?NP+#L*&7zJ+GK#nNP zvX@b0Ke>8Q=(c~SaW(hcS4|}eK#c|}&@+-4^sg#tCmdm+4os#hu{XF_iv?^|@zILJ z_JnR~$xv3FUILSdc{Xf(a^J~btxS3ZwXX7y6?4Sx3Dxk=T`XYQ5()MbbBjvCDk2$S ztedj3jE}K|TTg*WojqQcV4l^oJ0SgAt_D@xUwa@u9lY7s;kF$MFim3gB{;@*0x-F)#{f z?QrZN@NQ*-vD%WYDzSu_7=xo4N*$i*HsJCpcwAOtbbI`BQR%5=Q$gemj5uRY*}!O> zfc?z8Y|xLR5c8b5Rm_d3_7@)!{KnzX-o&7}#una6t*t?-#x`bbrj*qjZkHWy;ZQ}W zV%`)D2|r4@&ergT-7+3UnXCo-=tcmxrFIQDuO5PT7*`o3>ZV*`E9a@x3<^RvItEgo zxE$9HHhYJf$60hqX$RY;rj7WkX&5ZAWI_fNG9x%&XfHP8Z zm`>v_2<1Dw6Z{J&PByp z+ojj?Vq7jQ&xu=Tdu{D+5Q4(_=;%TC@%>AXLh;;=$;emDfim9|&tOjU&jOJdD<$26 zs&R=v>eZzDYB0QDBs%;s4GjXULz&`)Hts}_q#RueXSe$32ssx|YDYTqTeFd3p4&0q zgOQ`6>ZwauRbc(ANFuWDSq`uhb}7V)_^6qvvY4;fhyl=iBa6LeV&8B*&0{Qt?US_+ z4Wodd=cU6QpOM9-YA!uL@JKkQD1MiXxlSlnr4cq>JIUiA$`Q47b0V?^;BK{e>kUV@ z10z#W29#BdI4082y{+gcG=AXANwYk=8Q`dL@cMWFLGa{=`Sy4El^50YedYAr062yp z3O=3_9yG?|l0@`li?b{4q@%JW||TS+(shFipXNqcE^SD^7gguzpP z441t6^7PXRXfm3Tb|ugjtI%4lts?vpzjvv7eN0rXXAVMN z3`iXRc;qNvPD9X#s;?0_IumawU)ZrN(-I}eW=HYXs>rj%Y@LP^`K^6KpBYAQp;_O4>qMF>91#nH$>@+T2v@f>gLCjDHEspHp8Ca6jRC2>(kOWNY z=J$nGtfl+q!{eZYnGv|AfquvI)a`M7K--xBeBBUFVxna%ET57iMzywUXR5_e(-k z21kIg8?DJ8byvpn|9Zu+vK0Ft6`@W+gh$DewYZQ+LK@^wFRmQ?sXJGr$ho&Ti6Eha zQB*X=a~>hJk-i?8n1_pgKERXncL`G3B&xK+(6uuwj;(~7m`Z=v{Wad3Xb5LCE-P6?s9;_NEIuhe(yW(1bXK4m!(C8>K{&x82?0To{>w#`HjQ> zm~+@S5p2-5o$CrNY-tT=H0VCUBRh7ve02mpin2%`q=lSt95koQI>t`f2vqVh^8VAO zE|nU$#b}bH;w9`zd~>3e;#HT6P}}#(rRER7zsH!6zi_6)^q3OTqURxq)bJA^?^XE* zX#QIquf6=6nV;h1MALHZnJ)PA)AAxryznjFqJ2thD?d^MG$c`d*i8d716^(U%a2`x zWw%g*NOj;gBrnh|gTKN}!V!yGd%kf2lvUL<32a_9?&!KIjYsZhYa*uwSu4+LP!LM1 z+Xs;})6k|RZ8J{a9(xLun+xKhxDW8u(W*MQmin@rp5TO=A}Z2{U@?$6Q!}yFtfzW4 zBN|Qv4s>*&XYiU3IA8-pTGAS5b%OIGjtVL3DImGpMaqY8)vq_$aqoMDS|pX*RRQV9 zB?4Uo$bKo1H~0sbW244X@8gcJkwulPr&S}D&&VLseyUWOpdz4x$U?!rd$DCzYm2@& zh@=b$xRUuFcu~!-HnjVw;ceE*y32tEz=j#lq2%xxlY;PG0=sb=$aUaaM&oD3skOh9 z#_Lpd5laCnV_pW8UQ2Vy2UJ7~aF`pj37mZ?G`sduj1;$cG7Q|UtaT-$JcQa%DiEIe!))5F2e>I%-23KBG zk#Ee7IOH}&M`HB5pv;T=lA>W~onBR2XvO|>6X)$=Fkmnoi*v}rqGBb?U*nWYyu>n6 zuu&}hoA2sL0*mp!S(d}5(@f}6<0&f-n<)*%X}dGSX06d9m`huU;pr_V!xiBe42Fyp!0%dqZTaB-?q9BD}x@CM&cqxpZJ!+z(PK`0IsD&Zo<42-Bm^+eg#=4Yjb9Rd=7%>#N6wG<3pw=LWEHc*pX8nJ znK(uWQd&a~+1)*;gVj0pyz7v=W`{HP=@-R@+lFhwS6c3lB*(E;k&e+P2a}gIE;WWa z`Y&){jU+y7crz!SWm?YNp0o5Nd??Khig`@yJ%F7*qvkdR>-M#e9x=A5k^C;er%dYq zW(TLNl_k?PZ>F(**|6a(c!3^0b6W=HZx2iJ7T?-WHG-x2n^zdZ^Ipd|Y(#t`FLl}- zJ6Icf{t9WW|9kv=(kGOEYFr>qkk+RN`AOLg(gDbvnt|ZI*&0E2#l^)Y{=w~m0sS*5 zhXX_#E}n>o&|^Mni=TE9619Z1NWsoD%&oYz)IJuOH)I0_w;Gt_!q_+*qM{#(r+{N! zG~^W6%9(-dDxb&^`n{e5*?L9Q2r1PH zQUu)d69)9s*uukrp$?jpQ;7Lu`bRDTPVu-YtD?j;Q_Ty3&j^{EB^p4iBWoJrM746! z&9*3hhmxiSc_*Jap#9&8Z?1w9SU(;csz2n2AyfS(9bC9Z(IxFpgm^+At1}tCArV5@oLP9Va9P1xuNWAn) z&3K$2y~1f$@XyOe<=6ekg`CW}nYh{6!`*p)HSb1CZn}56H1x9mnxcIpod+k8rK>F- zc)hnJ1t@_o#{{f*oP6=5!)lA?00(NJHPActFLW}vH{30IM`h7#o0lZn+O(y{G(qk` zTculP7~$moJD-+`AyJ#3(2Q`GJrd01&|#$1lYtHDUyGYS@&s-LziHx?pQmZYDz9Ez zUlPGdPOBj0?kyWHfbv%?3DQWjH632^{?WIj5#}C7lP3iTW&nb945|t1C8GvGraNpX zr5})kIYMTK_;1A`KDgB9K=9{|D@1-uZWJA~RN^XdpEAB8e)P*f1sO(Pyzn)Z<3g40 z!22Vc;$aFhBvd+5B%XjxV*D7#7RbGvc{)QA|Ekv2oaIazpj3y$Y`w)9k0Povj##>U4dZDYd-^3c~aFtBoR z;&$HSJ)L6b<()3qEY;KFA3ApogTO5(CYUlxIPsLcWmMw5S6#%aA2ZWyxgw%TwoTi$DM+0IOb{UB)m}Y!TXja z;IL^1**lY+$~*!{0}9i{8SF*^8Cq?Xkqs*VQ=pD&#x>^{Jmw2P`$%m?`b-#wR)|^10m7$pcORNBQ?||r^nN_RGpkg}$h+363_htP^!@(UC zq2|v2L{vZE{hEB?mASA*BgKoDma(*@!Y)mMj2x3C`ti@lpSSCFW`V-ypq(*1D8-`y zn~SR)V^j_{SjO#1s)$HbW_EkQCENU^AM7czW_BeRAV>e_$$&H2L|tYj^!dxOxY)I0 z0%Nyl#1d%}-_v^%FkG6_#voNfBREg#sd?P9Ysu`4w;4^xjkc_fADj2PhLr<1>mxi3iQr4)20;2%5T3M0p!Ho1Vl(HP zB`k2uRg%6lnhBrZhAf_M3)3A%W@dZ6h< zGR60g{tGn%EsMCEy-7n%UM4( ze}497kwTIibt*WFt7u*wr&YZc$Yj5S`vd1kssg@KNaenSgU_BH#QeF#b#3k@|vvtfesze>sgEjH1%GCX&Vfm8{ zpG<1hK6V89Yrd?$9Fr^&KSqum4-^UvBiJ{A*27?tir`11eS{ z>_*`!6*a*>zbplS0GPh0JG*4y=G6jLS=+AAI(3x5OU`|xnw*nyQvDI_mc_&rRIFlM zl`4VIH;HbB#IU6a1(n@>yzf7o+TE;l?TPVFp!jhX6nY`HT0$J#Iz$BGrf7~XK6!mA zwc}|%!6WYTP6x@4pw;)2$|%J#n`~LTN8BCul@^8~zAyU`6H3Se8I#gek|!w~P^KcerR&*}g5vmYfjL`cCmHym%Qq`=L% z3y=Gc9mU)wG3FBPc4ceE2040sv?xeHv(ig1iLe6-fLmMr3fOwMCbJ+D>o$2|XPyoQ zgNjVLj#R7Sw3oNwXCg9y{}qJB(skcdLmp0s5Sk^--;lEO@NK-&6|_AIyQ=e^!@vMd3*LcsrTGWlgBo z)|hMgKx|kFQ3Zn^MRGJ-qdn>B8cAGBbD0V&$UNCKUUS3Rn}B>zl~v!^NR7q5>~vyV z^vm#bK0%_oxFX=i;h{`*$~VuH2p`hjH^%Nxr3Xzc|4VDrEeFjYfEQ`G^4H4VVyOGK zpB)IAuu6Abn}wbx^r|A}1+#DdL@e09zHU%`*MI(5`3%Ot<7uCkS4+@hws~dFHRT== zW^rHiA){<6sL)G#Mo@A?2U=r4C5M;FH1osKHvhf}BoCY2RRNC4pu=LcRh=KSZt2~0 zFyT*T9VI4P9fqXXDRU>F$G?es&`-uNRKAGa|yQhQ0)u2Kzg5g62J2qyjm43PZUzQvhBAfsco{K`?_RcF9Y)jU$T^K7 zwqi?|W>~A#DR;$GCZ4r9KW8zu5~IL*wr_yFF}!}#4<#^ZY)vtLANmhe_KQj%&~ID^ zAW@y8*0(~ha zZGE#L?#U449AwgKefFg}J>)9TUR@>z5%|MtJcD3H@ga3VCdo+iIls z!;;DjLHuj&X|=nfwY^?2eZFBeMC;HD-Sm^fNtW*>do#rVu1O(QzgVVX-n^+5ND!x} zWmKJ?&w3W_%vTRhyQ8Q_b?ahOWI5e2UgRHwZC36n`oNC+EhVH4TBK2A!~E|f*#);j zh)Z^-=$hvAGBrdt$SHee>gPudaB65dHB#=c7&_XNKxhkD6nA z%8vIb>uLZp98w;OZj?Sh+lzdqBgccd zjnc0p#*r9V`}svUHy1v~eU_8YSA(GFynTEi&~@ru$U4ERGtvz5$5%o#N@V^vrML25 zt*_)V9COR!V#eS8?|fVjlj^jKXa{5hyuK5SDmyc#joYPbn@6kIyOcV8FG)8KkBArq zw^7efah;-qnwz87FfKD0S{_BpsT5tU%YA zT5@0U)IRC3d>ChiK{b;Jx%{Co%4r*-TUL)N zYKt2p0(mbHEKSXe$3bOIIxQ_&S8d}`LB|9&{n*bV&jG@nE*?i%2(`(HhwzzEm!%&1cjz@8t)a$C ztIx5_9TqI$S)7U^Rb^76cai6HX36PR9}`74qgaTR)~an11IA32sGg3=8Cqr))H}w9r?U z@*5p`*bNT_4&sixY`~%oa{tmmC;GSNvz@u8Mi}_Qdld{;Ts)vJY1v3v*ifk&^`o-D z{SB&-qV@7Bj*0yp_aXtTOs>p?N4zhHg*@Xvu$wwBZQ3D@(n?tD4xQHSP@lp6&hy<$ zKEB~SQ0%Hk+5%H!2(_l%cm5iLx;jU-mKyal$^T~r^lT^ez7HGqUnG$zCc=Wz*ZO%t zW};U_zv!LOxClJHB1O>mX73(`L7HhEoI0;e?a~yqdcA^*KLj$pJ&x(N0943aH=6)X zoB7fMr$$WRhzK$*GG$$BB(BU1_jG9;Jl@}zuC=}? z$lJemLwW#TFv()GHhJh$Zu4p4Z9S@7jVZ%}L93fnE%UpWRi4I)e`u)=RTkp(q1%>}#7WEJev&EDA3@v`h4z>l$Y_{J^Fn&S z!4o3D+K}(-0&ikzx2ah1tQ zhO}q~3a)&W=dxkvg?QUnxz7+q$K%BBDO`bnT{D#S@%qSqTmR;uYTw_yD%&DD9W8P} zh|f^Hi{DJy^DwnFsEaJTS2k9t9c^CYq7uL;=P!=-HC3@_B)eu*9#}u?I|fl%z$3{(y=TbCe|W??vj<1Wl=FAuHpxpTCWw zTWx{+SJeVgb{~c(@gq)i6~1m}3a&12o^Zr)@1IT-8u?LQ%Bq;!sDl=ec_m$`IZC&O zcsC#Yv?pWlu?$B939>ZN%tVWB4iyw!q2Rt$AS%#FG)Bs}^5ebLm^;+ZL;z)dU}P41 z(%IQ?m90-X*Sr6H8o#re(z@J^v+yBV<7EBw*SO{HIx6q)X$taWbs}xZTYS(wL7_Ob z#I8KFy{Tz%Xh=qXJTqLK+WSBvU#!#kXw@cn(n*wjKptK9K6yJQ;Ih}tsBMF#(>Ol3 zdwO%z*goNZ&q3@E(Qth3c3;F1+W8+p!G1OrYiny6Dqa@x=r)W5!u&sLwosg7f zqnRxZ@2jz~>0d}7FA;DT2!9@;1QSptjO0ze9Od=X0Dd?w8xk$!pdThUIEe}d>50@U zwHh7nro@yx$@nYiDbu#QbrSmvnc8hX1jW7GutQ=`-b!Eok)G{Usgwy@ars>amiJG9 ztQF#frE!w@E?4wN3#h>Dy~-1Yj_D-R~%yz87+9*k%lk8{cz?ykEy(f#a@T z?3AylsJNT+?7iW`p!#=yD_E^TTl}O?P19LfU-uP9)?@X8EE^$#KCP}h63*vw_Y`m! zP>1!={4Nxqqe)H+d1BoV({bXS_s8T4;zJR8RSI}JCijI7x#Z_p`dF-6AU&;-#ven`%hhVlM=(zM4}HhmvVR8 zW&PalTZ2_`cPE(fK`zi`nsank#a^d(M&$$b@2*qC?bNonXWo0ky4zR(N^UBCETM=I zZIk<`2zrixI0>wlY!M_kitNZZ5I$4)_d?uIk8ZdG;0A0CwV$$kMrp+|M4Gwww&&Sv z4kKKeojToR1AfxR@CVo~#8^z|6ap%-v+=ih++VMErRDw70;(jWdQkk!)QyN?dK;P)9&xf;+)0xTCcHz{_=(3e`vY}_qw02+1P2&IE~Rb zjcq55Z8f&-GJf&IiHW%{58@rD3e~O}l$Lkeo`v}-d?79~!ak1xg)g|#!+l=+XzqR2JwvUT zOL&`!Y_V&lemGVknAv_}%%BNCcA=~=K=%8@I#n_`R-b&DwIuZ3*?>?iuKK1{u|s{l zw@)=Jg);#p>{JYVP^a;nzr>0UqI;Pwa5by1E58y6Upiv(QTlRQA$?ieE`cKnR2~?F zm!3VLb}F3rqxdP8SR-r)FK*35r23w}d#QV5%my*C>Sr&O&%dV| z-oj?6{60t8T{xd1cu*E!#`ePq{w^cu68()YvAvis)}{$m^Gr+0<9=A;v%cg!ymiaw4nrws%3)nYz{6|O08oVeoa&sg1>t;9wh3vg_Dd7H+qCp5E zX=MN3s#(hB9foOFaVR^Je$9NRqiz0|#y;B`s=p}t)8sm+9Ui8hWKLbAt+3%ukZJBk zBM{Im23YwP*-&mV+)6W-L}xjn95BI*)Lj9e0f8| zYI*LjeT4IV)ssa?+)WBNc~o`O&;t#vKQZ&YEA}>MrDB&I9$i65d8%p3m_F}ygpnl?7&*XnRjZM9 z)?etj5U7(?xKnkyS-y2T#8drm@vOw`oQvFMZ(;rl%;KL{GFteS!Qg9-Rpd-sY0@}K zpYtZV6emd~tb928y7ojMh+UHYbe@d4$IEoaCvxoZ#v2+*QbC&f5o$`>ud-WD4q)9PPFjYwc5=h# z?Wj5Z<80*{*+DLWGJUoL5Px_XPjD=iN%@h}?pV}&0FJnzK<*$OVo zJ3hcEo7`}8GiV+8!bv>3kC@6-10lySssd_rAPg%WphR?oc=Ssh4aVv~B03LSeX8`! z=xKY}WSV`|l{-Vtq&Al-0cR=f|ETc$F~^QYg|*}pQl&_VWPVP+dtw`X(q8dJ0qgc* zw{Ct6PxzRUOy2g z7Oye|`b)!}zkP^>#3mJ>@9n-cyvq+cJe+9(;w4hzqc59wA;MA(3t|myMRavj%e00S zW%)QR_x#k|?6~h#HNhPR7)e9tBmXcHrDi=_3Re|>1PB>!8WrmB%rF-?C_q@xC_~%f zZe>q3?}!8ml#;UvuP~S#aduGwD{rHGeUt%F7%BdrPAYnvI_r2eOt z8U5JEHF*=fd4Jghx-{!rZ~~uX``fYjt3cg7R>L!>HbteGmlF#gAtln1JbL zUS0+%E}9w`)8nPFn#nk^%dROnG`xQrnru*i5IrBc4iS-&zI6wtJ^D_OpsmRT>x2v$ zGmo0&mr)!qTx(O$=FM_}_n9^REo_M5=TpwNOxeW5?we$$VpHd*6w=jB{7_!Rv?!`# zjnvx9COkgAyan63)~fL8(^j~|PU#|dTiaw3_y_dOQy@l5K1?LPXJ?0O!3t}RZH6rd zKm{otzZQE!;3evmtClUDy6|NG#i_omg!y%LxGzq$*2_U)!we6HypQHTHe_&*zybA! zlB?c6@G|(qChQWagD+j)`y157B^7k_@bKW|oE5SsZ$e1r`3OQTv@8Jb9|ZUNtjd_0`k3puI4t%&{8a#4hh{jBPb# z@mY$$7;5|UKc#bNm9-5G6hx=OecHQD!%<-DH$)c|YD6+q|DbM4YtC3Bj&Z(-pJ(4! zv_T%smx61#Z(JxG8_mMcP=(MxX(&5~yvE;`vog*GA4u?M6W3m(kX{9JU?gE!Vs67` zTH}KNDNs}9T2dA7^?hq=>z$9)Tq_Kk0^?gr92d2!|E^Jpek?T_Ugg#Oa*}fexajs& ztQu9ujOsfiqB)|=(b-xFT95-;U*p$iZjk#XluP?pBHugf*0s*=t7)+8SLYdM2#NI` z0V5TFAG}N;+GgIHg<}+kj`0gt&KIllszup!O-i`U(6(P|H zvBxmEvbwRp{uMAQe)l6DtDknwE-t1-?w2~X`iz)Xv0w#H3gnW~P%3>n*MZk0uAW<5 zm6~7P;>Tg4h>l~u-n;8NfoTb;;Ll7Uk#j`KpGnJHT=zp#tXyaTf4sK(JlAJ11@H{& z-d`{IX7OhKWs8ItGx~x`Ngjc1F|1gUvJZsP;|s1TA{Ah$`}-v)8|d{47#B*Tl#l%W zCzIbNTFf}N8>&97EY5i%0bM1;&w)z(wV6RfU(4ufrMJ=AF>rE-cf89T`!yT|5+u2k zp-ba&(puW`dbhrO;6mim01RZ7wiM6Q@wy1i<=-?RHVE=XH5!uIG-haci^c#OK#HOY z-0W&`4pUnrfVP7#p)+Kqc}IM{Tl3n=v(T@c6qEjR>55j)PhxnIw=gi^Uz>JezR0NU zrA2e@Oh+zm3O7{|PYXN*&j9Z8vZ1pB1INVEGkt%5|IUlWNP(I1Z!`A6iSrq3JB5)^ z0&%%wiH$+8TaZV=b)%nO2cv+n_vg;q@)M@ol`Fpys6u1)?}48f--#E^PxHjI&UiGn&sApmyW z4u^sbkl@vkd`#gFq3j7MgS&Xn6`IhSwyY2Ov%DVW|Gmo%n^dX8sp=prHtm}X#TG9? z^j&Rz@mO+AP%hzj^X?>y`9-#H`tG`FmYIi>x3VfwD(wbVNu*PKUk#1INwXgRfzwpd z@-rTKB&Pp{yL&AlkZmL2e z-Lbd3!}Y8&7X6H3k`1(gQ4(~sKco0%jy&9MKiBPHkJ$B!xrd_Ml>~!G3w!!C^*GFb zoPedX*9D{Ju4}#%pYbmgz8GRrc(id^HRANmSt~d{UbolXS=#kZgxI_@09gK)W*&;) z1_ts_slX9)otLp~DL?scDZ**#FqWvBkCAM?!4d2`Z}xN+Z-EvwKV{>)Mxo48y-Flu zwL4ZNI8r&jGL(8rcX%qlL)3iy8vPlcCH}7fNG6x~8NFBk7lrAhLJnW6eRCs_BkvwqZuc* zwh?UmARgnH=S0x3=pOd{w74`zWaP`V>E1U1yD5(^C@{z^^%rW zM}TDW1^X^JTVAlOY{aNZLWF93fp3H8O@cW1N|juNxUc-ISUUKHd`~%r)tSbBLL)yb zzZ@e)l%Fy!qFoLtIf#xdfwuMLQpU~C9xkBKK-c7>JG1mA5>W(^4Nq|6%kg)5%C?9M zo%vdus9ItSEp#JJxkD>yuxq(=q&v3c#n~rW!F@P=%xC#Jd6zIr{dbP!$5r zcQPE^QPbqsDOB<(U#|ck>fwSs&l9lR4UhNT+@#Q?RJc)s&1v5LQfdwG(ZhBWq~Xn& z{#28WVYQe$wsBSj4JViLeowQa#&feW_)g#E#6M5WKQNt`q8=@oC`_)VOFD-rq11G< zV>7aEcP9*N9YZ9;(Y{*m4N-l~DC}jw$*4C{hAYMoa}Tq5zs7F3%qo6#iTg;hDxBD~ ziv37RU#Ftg?=B@97Jlcmb2dO=3g3qU9pbifCgZnkLL7tt(M z)Im;aVyCY|`O4)*|2U45y_fXduQmMdH3F1f3%@BNO2Go%843I>wums=7PfBnFO6~n zSJQ6@&3!YPF4OLb(F)gTNmPl0%NrY$-25eJJ7yxv_PM_)JQ5@u=1fmdQeJFN3&Gb{ zGo%l`9HEyqTee;EtIMbsM=f(%KbG;M=PdO6ss3+P5xJ5VnMj($cnHj*_Qk4 z*Kj^XVuF=c5dyZKSK!1Zh;O+fOZX7;s|PO^9V)9Jm+0j+(M1xY*fdCOD0;v6IL(8V zL<=-({J-i>%8P+5t#J>ktVXPoh6fQ?)AZVcEqMh!yCATZxDSdLf+apD6^>jjyPXv_ zA!_5QSdf}nPl8};ABi7!G6#QepM|h#A&A%C``bJ2EGZN{>;YR#*;?0ELoHO^GcgY@ zdPB!YU@0y0>Y%h`L5cP^V4Q&#Bv6U@Ao6qlhWt({_pJy)=8S?R)Ay}~-;)gbi@uKG z)wA%6;CGw}KCK^AG(ykWKij|toR4o}KlzbI_BcqJ?Y1fm+3lt5mqlR{vlmfkmdMd+ zUtilj%|itqa>u$tqm6Yu&0$>1x;v{+`dtFeEDX!RRm{Ix=v@Nax1F9-nF>=~D(q`8 za^OExwCp=^pNCmzP_zN+B3%YLj?ITuM=>PEd#9-QLRINUPvDN2UdG(n~d~k#1ic1T=!2z*E|T ztHyD@FSqdf(_6p|)X(vX2v@O25bG&bm0uJJ$1+d-fRBHUleLUX&419w2nNEXJ6VjT)6I``>sar>QU7mpzl+G13?IcD)M1`+MHkMGWW-u z-`M_f?<*xz`D|E!AX&5YwB)M3@E-B2advy<^NX$h`kDf6VYHgo8~Ci#^S9}W-sW?w zy?@<7j#lzQqnn0%Wuv>b@SWytZzAAhY120pjmKsF$*3bHZfZFihuPVT56+}EOgtc_ z4vvqdPdl28gcOYgq3A+lTHC99!|HN;l#w{@;}@g84t%{ z((mxN+<1NW?e4y0N@YZ9)G5i@v_4Lv+4-F(cry3rT%C}R-!1D~FG)@v zHso-v1^?6)f^~{dBeG$_wo}7Q;=Oxzi2kAA*7)pKwmSXH9y?9t8q)&pGB%R4pOv66 zwmi1{dLQ$`>jShQgV?gfx_#XvrT4AaYmatw9i<3(XFgV8IY*8z$47xS&pgHj=604p zuneM=WVW?dk&2k}XbFt9BVe$EtO@}OfWLjK3RMub*GL3c{fgZU)%>u(<*72Db(J!8 zh=y(&Q`II=${-`)p;zR85lX8Tm31Gypp<@sHHT%>Uz7`65!t=&nflT`$ntY51F!wq0hPO`$|W^_ zyqYMf*uN9Tetb)wzZvJq7Od{QbSuubu{KRiW8hI}xGPFP2u|P4$WMM2;UrkP;<&Wj zsd+M8k9IL1-$>anfxFpDHG?+FW8E+3*plKcPgy)Y>o#y;qrcQ($cya5a5F z!LEN6yvX!bzoq{c#_iE>0C8_^Xhczdo<8wY|kf~dCW>jFh zdiuMWPCl4&{M72Q?D%$9#DfnSspDrH;3yZ(7;Di{p1-Ebg+Gm$0M3SzAU9XHU_L{1 z^K*vd{w*dfw*P?mAeuzq_B+w5QyvX^P`K65K57%zBUK?=8RRs^AVu>-Cy##2<+UIb zdo6c^YHfsSkiaNvy#h@MKT2lT-0M3ff9}cB-X{3q03ym8jZ-~~&offLX3T)^>F`gh z^^>IU88<<@XV+x1=ojO1EMyFw-|8{x&wy&t0Kx2R3Bg3?hds4Q-g@fNa zpRtO=*pFtymIHlqpUq8``Yc_UVOYG4`Y;uDMV6Wg1_H%>K@mioO|yOC>e`1nYqP5U z&%)=Es}xD6Eaf6~GA5qA2RtH~iu<;qh%5Efe6;QwNB;SO8y~dRJx8Z)L`#ANHGx|e z0q;baz3C9w!QV4AILg}=g4q|42n`ak zEpo!~_Rc2t##l+kd?O&Bt=U7|gy%s(Eh2m-c}zl_xw^YA@Z1J5jSsygoaP2ZRv7d!KRo!CwshEx;a} z@KL=kPvJQpd`1(tKCpH1h)?2XJgIev%g@I0sfrnM<@S9%H)*Tup^6>f-omi8&5wOW z3Ri$zOfx9PV6oAgb(1}qbr)>|zy2ARwaYznIHk7F20aq(3cbg;c90XXZnE?j`#VOs z@G7v$iR1oy>WbUt3NZ?|Re;=b`V3-Qcy#LB;K1j3PX38!nUP3=@UUicCd#pps&VY| zhqc%0x7KOI_dkZtV`0?yyNJaQJ{4f%7>WP>IlJ(080)#upv!uFgQ3jZT65d8jH=V` zbl7eK5*{MF;W$g)31CWb;%b?R}gb^nqCmj~6 z7>Bqvk90W8YUWD0CeFVs=a@#%*phxb@Ku5qGvGo2g6_W%g76C(8m>%_#ve3LIiDhTr6t4qpAnP~MBKSOSzH~P~;Jv94r%Ufh z8V9vQN<7B6ZKf7?TVIw1!o+a0NmWxk(FySSw~0Q|>1DM+VXMk(lyyqjyFUeo0pUkH zkM6Z1QT>8`9*@3!zC^d+&Yyal?q{2Xg|F0lupa?~Nih-5XwVQqWbZH19L^QD0%W^D zJ=c~`j(<@05-EBgeVgnzN>0@AvI+1EbsnIYB~(-nW%4aLn)79elocZi9=-Xq(Gqsg zy*oF0$u5AMqN%NY&=-2k_me-AGc$OtBu2%tRs@N6{6`pM{u-S2k*i3_B@_)Mr10Mo zSvuep|IN=1bzA3G3v?GgRQWn-z&)kp_}lD#YqeYb>?A!iE@9*unxX}%x_V@yJIbkB z*G3MI1L>G4Dg%v=P%sFr6|B3BFsFc%T1O^+ES9Zo1;Zap8#w23m2#&|@5glj+J}nwCLe(4;SVPe7ow7+GhT<s4Copp z-1SOuvuLJ^sxsE^OX|Fb2mDTj$H6F6mnNUnKI~1O!*wwwF;L`ntmk-_Ys#cgSl<=` zvz*=eIlEVP=>lpfO@vw_ZVn#C2F6Zx8@>Qy&mmv%L7&e>!Qr`8;pj}Es zc>SMt4|#|>*r){!_2Z2-@$<5X*C|&$zN!9tdfg@VMAHBOi8S45XMoZvzVkzTP?s42 z=;4+&)RxzkY9f1jBLOhh% z#pBrmjDgm8-wPA|YxoLdeS7@spE`PumE}3a?5h4FSX*~pa?RSy#Iviimkv8zRd5lk zOAHM?1QkMF8xs0umZGzRJ8otGea$NK1IqWn{NOqp3ah`d?{1vm-3{4^iOTdndaI=8 zFGjsB0VNho63@ypLYY+_?`w*dBJ)#UK#akP;ZqYS(xsM=7e+p`2x4Imjg=F%Tv^Gu zKq$3?+1C)JGJln9;1kvrTqiuZlo0EO@M~Vp^-gN4GQ)xvddAUzjbII-E`J<;Pf1C6 zo=yEQUE2L$^;}+-jqT8AvBi09@l`7+_fyEeC~b2Qm|0n~vC6F_;5a^L3Hu{*&eTr- z@mB^}`U8|>#t2B8Djx+bJ4*jOd^_^3Dz%2f+8sA9iH351`W1iWA+d zCJ;~EeP`0AILa4(oQaGkh;{lxmE4et=xpmbmVp8>NOEC$*N37I)<*rO?{8=tpOBEy zX+J}XKy6VjQ~RrY7|Q0&U^CzX_54IF2#ZZ2i4jx?0aTWj8)uEXQ><~nCHfEa+#}_G z`TlNF)L9|SLW_4lzi21YgJwN3&}ytD*m!C#vtRV?ziTqcHjL%U##}lU|K^^$E>2rC z*J0}_7Y2U_TttrTpRwd4H(n!7Td=_gecF7a#?>W-{?DA*JsXi`$nGLpCKPZOOQa~W ztoV@^TZ3Ugu&kCjK6)oY^xcg6K{;tJ*XZ4#hSldXelQcC3(H(rC(FL@_YSC)T3W?7 z_TUf)S<^s|zmK6gOW;!{+;x^@D`kB1S&WX$vq*E%i2quCjEehT=oO*{7jXksBLwnJ z$w2qzx!u5ND}!!>k?h9K7c#jV> zzd6$7N6Ket2mUM?P?k2^;0x|PnR{`F+PJ(I%8!Wlkx!I4xkO>mqUx`0`8B~%GeTc$ zow1(l`%1sQd5jOat!-m>cMtDAL&=rstp|_dl!TJatp06Mk8h~1a0s3$o|&0FVpqimUMIFDHJ6Z+`c=gi>L)TBC_EU1ES=dMFMz9*`#FLrKw-kR52xX+>RNTL-Mnh z=MD$?VH2J6;3R=ZHr7zRy7KO7)O54N5QZ{vBhU3S zZq05&2@SB7n32m>X}MxqS1>(jXW7JQLVqXDl`0TCsMCnIuS>{8H1EuGHp{wjWEe78 zUD7uiW)Q{Uf;XSV!T|jGg*Q>oj_I$p#dc9gV`Hne&j!+6AH`aGD#jrR>W-JOpI!W5;(S6C8h8Cyozn`F=`Amt(g~vwm%6VmqD*H%Z%JB2!iU%QcvNI@*?_+{ zxyf2nJQ|o&Lr%wN=|L!^ZYD&osx{iOMTtR;g68YsBIyzyd zkkvzB-n$Y$TL3RE{0ix-HUphJWmWsC3zVSSuEc>)?M&V}URCbPtht5%>w`PEsyo%R zBSA02xD;X3qNW-d8!d%3vBKhxjMYmdR5I+r3z6Dofd%UpCZX6BrJA;Xlod{wmivV} z@<@AgW^7rqL{YcP(Z(9bU8j9ik`C4&RyvJgn&tdbwx661}k8I^W{| z>1_SAuVwSGN`IK)JxeN!rrgE1f)--+O&8~n?W(+^*o2pbGUCvvD) zp*@ej^kXb6`;edw@5|2i*gEEP$mm|xjFKYDNVFi5=}rN-IMllW{n#WY;C(c@S#Hsk zS@*?Wfmx6Kq@*kJG@?_8>8P$VoU`ltM^E6tXObMhKh;Fvkye#hSQtm(YiSo-E&>T4Mr#T-tWPfiq77j=OU_ru zF$f%-8nYBQ7^Vh0#1rYDaaf(WQrbCbFS-Vjc|`L2^~eYNrsH?N&y-k5m-^EIi!;(g zecMEUHJ_G@e@!|*=I7!|3(Axy4n^(1BsTPUvtauYr!5DJD;`yI01c$PI7~3)-7^r8fn**ZYFA;CSGUwcjl#a zP(XIvxt!TyNxh4k#p#({!Z{sPYsZGMm-p@3>U+y~HyQ@~W`7g!$I~&=9-TWm4(HuN zPR)16iL5Wd{wu9b&l^+}N2?%BJhV5L{gJxpmDncBAzPK96(5}>v*HNCWBs10qJhC= z6l{0wa_3Jj_AO||(QYMI*nEA3`RRM4H9wfD&AFd-WGp|J()ipL*$tbSZ@x7dfSWyG z*}FH?!Od#7{w~~PT=;T?Yj>UGAjy}=P0>kM;#KJ9*SbK)AGUIQ} z6LUVYn8`FdrN%0^?YA>u*YVF(RH}MQ3a*Lt6nJkB@O*RkBR^x&?egfEnT$ln70EG4 zl0uKD2dTJ+bDCsn!9f@4Xg}wZt%qr&QuGh~GChwTRPMj)uUfET!$;|3rhtbuA%pRT zn(E_k{ZuOMoyYd$*o9}_?@vh_gZfkRtJr68uin^;r8N(|;2CepU5ps(z{S~R(m#&f zJKrL6nv$HjjkMl6iPv;Kr9n-5qwRmwhD^7n$+cH2`dKxQ_S=SXS<-bSvAmN(QUDe~ zO<8t1fVX@WLwRZ~T9O{}nM zPykreH5zEQ&aqoPI%t)65qdPqF#4jqN4VUFT)M!sKK z;J&_8n0~Rb{_{s=`uQAn;*ry`t0n@UhNs*6{)WQ{N$i`;4rMKFn<)Otr!4`EI)irx z!cU-JkHFpI(S*mSlQJu8?;b|#pT%D$+8gCO4|jRlHggrkyaQExw49{e2RrSlt(GH~ zlJ`eERWGXL(iVQlczsF|J`)6WAM7|i`)hFwZN6H?4SZ(YnQSey==j7$psFUX9?x$} zU#vWERG#pBHn8Ww?p&^EVaThAkCq#kknpd2f|~EE)ovbP-k2DaMZ4A9s$DPad>GS@ zc!Gh9frn0<)O@KbWs}N=;&rM@>}#_>F++(Mocr)qCqYx z`V9!S35Q5!$V}W-lGMWHc0J*8X3|M#Vva^;xDbNK(v%64>sl3$h+w> zS)T(g4LZNg>+KJmkq`hLDjaMp)FG5dpe0h<(l$T8xRBd_;OE;9bmFl(MUffrywah} zHM+K#c*^*Qe;C-K${u4xBgr90^%uA23?6@SwZgF3D&a^Twd*R)8IKYZ8@!_LvH8}S zI8*6$Mp`0T5;*2db}SSuqSRbVJtHV&_c>cKc^liaddAe1cgX zYoiRw2+jLDnrK^{#`JF?PheIF`HKn0W6WbsD*F8Q0geXy z%mF>OA2*0j#e>xCY!{Z(&zE1Fk#B4h4+t`T{hU}`X>n^Zw7z{Wt@=VCra>lwBZ2(k z!TdcXWb@1H3lmT4-ul@#k(1*|to|~5)P}*GVmjHn6v8Dk-rGRH0cqSWFU}pZPz=e{W_#di&ejZjAeQJbX134Xdex~fy?}I6CNe# zmay+%Ya>(v(W;{4ro+@#xK;gWtnmn(AV1EZ+Uf9^#6^s&{k{-eT>KM|YK;Fg;kAOZ zHr%dm!8BRfLV|)dn7%c6M{%v!FL|da8KM}}hYmfF)zsc;1Hc1pe}{yVv5If`e~(oQ zh7e>mTgG^fGsW>fD&klVr@abb;s^skr!ji4s_OcwnU?n%LXR|znz}j%)^X!xT-eOF z=d6DPS4+TQZBc`Lyg1Pf_3_&EHsu|ROW2sVc!rDS1DiiN9v2lhWFEU1Q~h=@;E!^T z=fLk6-@o&_Afg!|fEMZhO!rl2x&)yjDV%qI@PwOVYgBfJLIrrEF$ul>+#mK1bsr%Y zz)%pdShO6S|6u=baZJOucSSQU?dxx^v-O~oI}NQ((mOyCwas%j68Z|2VwS?1kTof_ zgKMevej{#u5B`iJNsq5`Vf;{h9Da7eyMpeztb(3;dZ%F9^eL>I*~t;wQLQ#)J%62y zJL|^IK&_>dda9+`BJs=5UZ(xg2i_T~_BAI6o9HGIm1V-?UzWGHRxKA$ZJSng!kavl z77rvG|GC){B!A=3$=r>Y3c9-2UR~oa*Xng`ot(9Z@VdJbF!$mq7F?p{&dnneKb46x zIj%c|{st|)(MGapi}z0@Xiu*1>~XwrA|OZ>{5Jc+t4Mh-eEc1^!K0)~#Ik+m^LR$r z*tLY5%lBE5Cs|SaORJ-eKgI7_O+PPU25ch4>%^u`ZU)YoVwno9U0d{K-Sxk23212p$PuRt=&qZ*;dQ(VM#a#$g|LF;$=bW$)^4{@BHaFYE5I zv$EYCtF!A2BWiEUJ*&TaOrbhBj>+Bp1VPYU{~x@{TM;gIlmYVjXr@Q99}peTu0MlD z-DV)wS;58uzmah=fqbTAj(ChUrgUYev_^`dFI&Qz&x9+48D!$bO|=Dx2vIa&wlFmO zt`UO;@oURDw4Rz^R2D<$_c;b>1gS+3G0O|ijQavARQ=Tb^|BZI>qdbnqx28r2Ujw6 z`3djmHu(_9%EaJRs}J6JmVG6zpwgT}Om8#*E*Zd3Dk@GaRO%8Mw;vSdncggbwpKTf zX{)e48w;h5PbgZf3=D{zPRta7_IS9ZOZRBn8}=4gL|9%K!CNWFL8HQ>g_&s2bP-sY zVcCD&NBV22&H=fSn#9t{ueo8IU~jVz^ECEx0lT};qnp6nt{;Oi+5CvjKRkG2TJNKn zJPjUfd2l=6HD`0qv{E5pxw{N<9W$wcu>^MeAU>@!&`I}yl&sOPq_o`b4zw8E+<4*Q z$*^$F)n~mmRNXJOI2=Ptn#E3E+1qe%Mv^t{+Nv7NaL>=&e9I4%y7oNySR_nV!D7%L z{#Nhzi;HSmdoze*3A%NKj#Yv)E`Erjo(WHY&-C^tA79JHVC5hle1`j5%#y7x zk7ecLh9i}Xp;h$Ay28aCIhXJR8`(Rw6|1_s(*AvEaYF|2eP=ykQzz;eqZwJ=dHc+h z$p6`dZF9cpG5xyEG?kS>&;+D&4N^G-x{i2$J!(786xgeDH!q2xIfub$8^0;awYF^0 z$G~ccP0S(g`)23iUM&~2l*!4`?B6crl&5CMzk(e*+i{mJ5p<$4_0h-u9_eRR;JHVK zjPD**s?=k|T6O)~o0`_(na2JitH_LiK!nON_4MSiCLElcj)ZXc2m^iI>?(@wl@g~a z4DZi~&dEXLO!J<_*G7U4NNw!DB3%-Da~i2%!Ce84Jr7-Xh0N5M>Yn0tZU{^-UQkqZ zW*7X1ry9+lP@Le*VKJ>LjQQvWy-A^Gk}Y<8Is80>x7WhxK@EAYyV|o-DZ;E>8}*Xl ze1Ywm_|-9MRe9AGp9dD_xQVax`}*bpzKrzGyfJdv;BIK294mXT0Yi4Fy)9^fSlblf z{*XU3gF&tK+5L&wDfQ7O6E{@|@zzBn#Sboh7?``{7V+Lsxx@!I3i?FNzmmA6CJ!~~ zY9FAR-O5%#P(}Bke+wt}5Lrpo3STuJHVx4!(UmY}h>uo-CbB0Gl9F}~oMU;=PF7`& zj)5>h1fMm|=t3EG!-xW@_VC!3wqLBu!wIGX+JVUU8_q})IcfkL^u14xYbF`qd}O8b z8`IK1$ZZA{mi;%HN~;9B(%q#CD2Itt+%b;qP#=o1DKIq2@MFV$Zc}v|Qvl%iZ}aIS z8T^Vk-1N2ECiA>MQ&i8JfDX!~FlLsbYI#n#f{x9ZP7(u)rf(^9{<4Bx zm){5{0<-@BE9yxu-iCsUHP&c>aBXH{g@IrC&;62+uqY^vq&dm8Wx6}y3Py`Mn6c#0 zyce{1Sw7d%UcTZ(z(Ng0KtCli5NYd9z$mJB0oyv_jR{K!C7}=<-i80&-59mf7(EL< z&2tmE$WX%;7CGFlpj)*>u*t-&@2i*aXWqZkIyB{_ludJH7BD(+Z5zYc&J*ssot>k= zb1TV)O3XUJ=v?wpVs*T5S<*60p_Hh>si5!Z>;fN@8P;4ouXB*F+OOO4%-2-=DE|#5 z*|THC#z~H_P|tp@wYhMey|~Zd0fml^F#lCZ_Q+Q>cz?#X@Eucj;Jw>od)z|Ex)uAr zDAtNb1Pg-;qA>6S`EYSE=9MnR>lB;CF5o1y@{agymyK|?>i6DAh|Bj-?umMw@v74{ z%wl95M%`ss59vgUTc*Fdt!3Tk_jl%}pI?Wkq=HN!8r!XwT{91OGgimt3r;5=cpV3oL z)>Nv~_>_Up&3VsqJ-Da!Dl~>GYURSdfgiNk!$z>Aj7T}Cy!7fsNTN zuy2y`i_CGRCQVsD_A>r>5h9-Dq6zt`ka%RHM+}xaiLYDlg)=DtUd=1cJiEYw)jrZ% zGpdsnz2a>;cvEQvIkb4gKe?~4-@CV;?LB&Jb{uB-wK7BH89CB?4GZ`PR-x;!#5BGm zI;wjjk1`pF3F__NeGp;Y53 z&HyE|g$I~7cnV**R9x=QxoKxd1)V% z{g_T~3yyT}0Fozn<#y3I=CPn|q6yt-UxK@ic~@-M>J}Lm-52q4@Me7qQy?!vrDLcP z2I-J<-cqp;f6Q_mAMZpgB*xKDMIkw-C4O|(eC4^l%6wt%sDGAmE&{-?)2;=u=ArmobLY z&&wMG-y(~i^hwV;k`?HO4n$@Ex4d0uu_KP-tQVD95t3^|jf`RXC?JrU zJxHPN+fN8T%)?NI*-xSG{30`SlrS?!P#D%zTP;T1*@N~-ava$*Cn^^=A(y)@!A}EG zI4vI6kPQvapXll7op%079~>N%t5*V{T=lPw0%my-JL<078C+lX_ym73F9$#3AEg3G((HQ%OLhD0`D z+6a4oRx?XKzd#*3?jMCSTeHPcS^Tfi{o4Y*2DwIJj$$chHbn$~?X0#p|KK>w_kZuC z5B-*1`Q}q5HUoCb-D(QhdI&h$##l?wKYbsSZ&4X+X74+3dPQCuSoZqnOlp5zzUmPAviy8lEBc};b3aK79HsFN^Ivjiy7M| zkmmhu89x3p^=<=XGEHnjk|ourBPyn6IH6_p9>#FcZK8jwXyV#0KMrx=;&C2 z`j}aJHnz)cfMdmm_wTV^T@)H*yBWC^@Di+Q z+=({*kcr93`nI-;I3uIpyMAEeY|8h-neX}^rkS8t3c7d#M58;%5w371959Oj=-$}f zX@a})KAWdx(hE`?_uij?q2I9^k9uLiF;v^q<34C_8i?-0k`VZ%l~P<21uaIRX~n2_ z59-h?Sg=aZHD>r-QeJIh-a*9Uv`57^W#Z;`K?x+1qwnsGsN4YpIxGXAcZ%Q%7usU* z((XM#czKMA2^d}C>)B$0<&s4OsOb?gi8qV!@piK-^$tDf`sSVO?5iVxOBNdb7K7%7 zYt+<5+~UV;Y(CgH6bBq#eqt}|tN`1_7~g?Gcl&71D~UJ~c=+IWH*b(V$GTiEUS*kF zJ5Qie8q5$BppfAuUg_0uYSi(oKKgzTCRcG93GLVWlB(tpY3p~ixij~^Tc^^!f0`)N zZe^mzkQEsWA0ltZD^ind)9L0^EZgGQvm=nHw6#zf(~sHkcqgPRMPCF-^d+5hs+~rC*oL>gDxCht&|Rp5r5HR22qp!+^bXElh1hHBNJKRo+_ku>s*2X;ouWM~cuZb<+)hR%$M5;GTfHgA`FUkI$zHaE|tf89II zr`I~v8x24pQ_a$~FY%Hq^#i9Zb8<I_z^Y29W0Lrf4(w=l1i`Iv0ChmAwV4dsQ>Btb_?D@l`=Rx4r@n(<4s*%Uw z{lws`{fGVZT#q<{DOFvvEnz~`sh4&|bkKys0#y*wXbZGWPCiwHR{=13^Lg&mS^4GH+l~{>l|h zJ2{&znQ2Uc$k6pDP)^+!TkkQOXFI#cRKO=RC}wm`>Ykw)omi7;QZx^NFdtjR-11%V zj#V!@EfUAKcO(WxTcyQ(oU=Vd1L&HyjSb?w)+U^`f5ee$F3<(ht%I`?_R42eJfeuA zlEtkErurN3;YZeN?(sTWhpF^{Y=Wy(%EJ(lgH^IWILsFaOW=oRwT?wN@&8dwmo9I?w}~ zL~je>QM#P{PR56oacX#N&xGF-6mkw`b2F$0Y0pfKh@ab*bT|LamcQ#_NDC)}bqc_z zsn;L8>LLyEdqQh5Y)#RhPBE?eiBR}ctHf+T=f-7IL5ImLbpFslx)_gzKzI8@T|{Ke zRl@k+z=L=QgZ?za$J@iRbQ1?ST84(US3nZcmH`#HDknEB*I>DU za@yr@Qd3H}`uVl1Zfs>B2psQET)t5P7Rp_%v_>Xg`sDe(S=}(nGv@E_pSSkD&bYpL zUjNi9Zsk6&m?9F{pb}T*8H=sJz9#B+9oj}V0kNOPMF_)vhu&XQ7Su7r>fF8`U5`Qv-idow!y-5O z{(4_O5Ntk1OJJLF*f|pGXRL#D(W$aP1m+%Jk-CFhE`VQP$lTIIc4uk1GYaL?JC1cv z=Z$hURqCns#rjhS6z>PB{9G@O6}o0%R}bX+5}N?Q>3{*U7BzWmVy*8j$2q_StZP-^ zR7{OsyPbnNIcl;g2EI%ZXgO^!!yYs*%>P0>mLI+9%D=ehaJ~GkBQWka^hdvixJHhr z8%w@l<*Yg$a{r{es^TYZU(iZ2`G`bE;N4z}9nc3EfR_TyH@|WNz*&PWY$X(kSNFt7 zPgYcceR-(G7)1y}yv6;6EUgmy1gll8lEe{;?xB8nd} zAYsfv=TTnVcbzVsxI@}YQ6t6D-(MO_*&RW$l@MLhlI{BI>XYZLv_KH)6j&FU#VsZf zq4I&9T;A+L-~=RL!iCKXWal%E9U99upRb7R7?F*$vW<5&2#)6eV3^`d(CSPoUzv44|Vu%nz%Lb?3b8E20VfL^6 z^imJ6a%0rFU9F9d<2cl6HEi1^2!aVG9~Q7TKX0qU=-Q-6J(t&eRn{IgPdPEpR`TH17~k(bP^-xzb!OC_4krWr@C z7JunN)cs`5@Z6hM#WH4ddz0;C8*kaeI;t}%m3oCN%E$&8Zo}1#9_SvTMP!hTGSr5` z3M|~iF0Fctoj2?-vpqxQ(aN)SoYT>@yR3_K)3a2i=ql@E2M8??kMda*lFM|YRER7+VjHH%k=7aULJXu8=WXQgx{;hSYuc+$t zm@|&vibwl%ZQ37cvnJL!^iK~_zpHM5k~vgIm>z&C9Rt7OxdP}Cf^FNT21Zyq;~9yF zbC=HPn#9KQ2XaTFkx3lY-%`h4@KK4 zi0E8|U9;)_VOO8~JgIc}CvxzWf0vCnH1xU2jWC2O5Q!k(jH#ZgA_jUA%sLspnI2K^ zY05r}YfS2U!KzpW;8t$0(EViB^!zIpWTSHdZ!rQh$fu&{t-SzOuFCS7H-D?lrjhSFT4<*{hyJ7(DV zZ7r*w{VtshwJSPFA{;Hv7rwx$IjC7hbS^S({?B)E>ctcXZ0|FRlP6En>vgY5j*Oy_ zRv0w3>PR?xEfak;y6WppU>?`9AG zkUviPrwTi>304Dm7qow?4d0O)gh_q9SW+k9rPOWal%?m%wV&2W#+{Guu=CL!R{rsd zW^+|Zsb*U&e8&R)Q+-+=Yw0z&t><97q1}|7SzVXwJa%Jw{`^&mXZ@@{mn?6T>Xr|! zw8eI`joq-ZkJ{8{>R@a6*67>_wjn$oYL`$2SwCZVZbAqNs?#W7iSHQe3)!8k6L z_SgPI8zi)4o7wN2#hrC=8jikci`rarIWuB{(G^(^fCK%!m0Frv%-LP5W-*8d^wU0T z{WThy2D`rWuWP{`#Sz=-Ho_GQ4r_ykuo8l&4F!bAnc5tTi9iD-Vda0@mI1xqvd8F= z5!F{!$BVs^#8N1s8MXZJ=wd{+osFG`_W-xo>i=1Lo9;7guCFq%6|nisS6Q@_cHMYy z1Emz}KfBK2-(J*UEQ8*IJ+tlH|2SuylVu_&YN#hzId-#1m!r$rtGbm;cq%l^Z5iY) zfA2Ewk7(Ir;T(AQpC2~YXz}AMde{wHn@_pIFwJG4TO>`KrmhpU{IPsD@eX(uJ?Wn}G)WGlfx=`(nEKyV~r^EWn0HWF4o zxI*ySfWe;*4EP^Bm`iR;z4grRLc;Fk=h`Xt&#PLvi!R%n+Z^()zb|epYEc(;tg6mH z7pTC6+$BxktL4x3uWwImQ+ZQGD;1<}Pld!dy-&Z#I0Q-o`L!^( zZ-D=DpXzNo(H5VLi5`#4_@`3E#;`pj>Y?Sx8_B9I}h9Sv%!qVVCUinPofbHEsuGSK0R|v=k zdRW~2@y&m~$>{t@8$@0st_E~Betv@uwZV~}Jc2Xl7&B0*>pV=2u2NbV;aCphl%T)d z$G&7!z4ZL32Vd;r`~I$;xyn++kHvuQeeTY>hA^@-o8V9Y-v;gfrU{?wtyTTK6lyci zGIHhK1HFlB26A)Ie~bpXQWGl0I;Z-oD&DF`@BSXi#YC&AW^JuB^ew`(2j)`Gc3kf` z@w}uJ{xU3YOw3ky3#)3exxJ~`ykh~iY7Hy240v_tG+3*@qHZqAte-JDJ0f{(N(xtA zL!=@DyZ8w#y7jsN5XKQj9cDAgJkY;FP2awD$r_K?;mTV#Vw1F*6@xg^1C+|!`*sSblP1FVWg*3KrF*K2{= z^3%bIah+xHTrrvMY7KOisN7u911!0@RBlXE^zj4Twk7Lklh;m$uD|W4W?$3nZ71na zc}0b|9dpyax=Cjj-LcbO5%GG=%m-#@|JSj5;pKmsTjZC{WpSS^{_rBzTeL})N|m{e zin?!{Jh9*l-Q%>V+s$3Qll6qN=g)HdrsG$Ybv_%>I@LmUBykdJU`C!;Dne<1HGE{K zS?VQ^bTei6;E>Vz5!sy?@r4*N7(6kcepelI}*mGI_};Amj3F9UL|@wFg@)aP3W z(pHL7b9BP0;gHQ{bZ5F8lGhJ#&0%Ls*SD5xsQn}UF7*Nc8MrGh&G$8lE>8AKM#1p@ zDM#ee=^1=+pxIiWO`G6#!C3V$we7@%MdBmV;C|-r8OMSHs&8mcPX!h%;K3?|Xkb*UvO~NqTQ==>(y#$4&na56U>6&A8tBTX>!T?sQDE zywd~u%Hc!&{{6qlR%=VMa}^8!kWb@w?MRySGX_tNKVWz=)U{`M$ev{QavoQe{;wHsl6sq*&{&amYPHZ>vZDXdiFYGZN&YP~UPb zBastc&K`K$*k_upooL5nV`L)EyhC&`qHsSf?m_sMp~&dFDm zLGJfdnKda)!SeUn_|V4H_fz`nyh9%6|IvAt{?3v)@BN3y0D1oV<|k`!V`bC;aws)O zpJL{U*jm}bc5Oyq88LX(U|eHcir$~~xct9g-v9IO5gWg#GuI10uz)k`;8Y#+xs)vR za?WLB@s$`kl2{c>yT@7@RGvL|mWBBRR##V_*84x52+jqxvKDHTgRR04BA30(L}CRN zwqui}8Szsw$z}o>%va}`zkeQk-X?ifg1_S9t>|_+R#TfrsYH#hq2k;Yi>6X5BL60z z(UT*spa^rYU$O9(JUZt(*DYBqC&O?t?FkCQy4k-qOZ)$67aIe*&#k|#CtZKVA3vA7 zMmArs$6}>Qi8pi$pVAwH(k5FdaNj!VTpm3YP5dthx_&cT&sg}M7RaK^F#Be^d@L+# znN8v*@4Z|kO^W+sD2EKQ;becqoB^(8Rl+1R&-tm3m);l)M=2%sdR@1{2typlnHY1X z9h8gLNGUma@+4W7nf|x!cAJ%z6;o%wy1L5d)~2Z!n`njM3N5pjT9M<!?o0#Qbi z?$^4eBx!mLn{wsXEWsi70Ci>c#xlmFe5}_As zHaMyz8M6gzOKZ$k=BTbcb2fL~UN?&i5==`Mm8!9fI6vm1hO=@@MYnR1`m>F`q1{To zWi1gzfUWc>pB05sMM#W9M%ik@nYmc}l3^A??f=$Be77Jx8Jfi|U|3s$Fbg9Ty2*CF zpQ2db02;5v?8-5-t0ILxW3w4;BCAr@yjuEvhs&X<(JYLk3Se9GJuCXMP5|4&iaiIm z#p9LNRJihMR|t*=Shh80FN*6zX*Xq*&H|_F>G*R`Pj1&S+4+aZMi*tar2n-Nds}}0 zeIkBJ_x-HBqNcOHQcI|csNP=H4z!6h^{<9Kk0es0{f{2tvE0^+rp;p0Opw^gux8(EV$BL_JvS-Z4GgKGoaD2^ttIjQfHq6U`?=DuZ4 z1Ki@PPzrC>Lw4kZlzk#8qS)n@oOV)#t$XSgV=V$L1>56wZqpsJ{CQGi@aF@Teq;%! z;h-`tr;nbBCP4A%D;$^G$=RkA@w50uGtew17$0$x@Jd)a>yVVjK`&134wrG zQX@#l%yWJ8{4OTv=H_TL8u-4C=Xpk&ykBB%+F&Y8bs55FG(suGg$ozB{r20yi6s;5Uoe_|ENz}wYlx| zZO@x4^+8O=er#`11_r$7+7}ha{&ax(W@}T=c>_syC7bMH4hH0Qey2m@oee5CSM<2} zUJY_5Vw&kKLKMGprfzR}$^_7@y{d-yQyx2iyu+2By8?eFpUZP#TdcRFkmi_+=Um&x z3N3mwJ;XUdZT%pTbiHBN+;eA*fn>Ipk*(?Sj__DWCbjy}uG!3g&pcZn-h%gPKNFGZ z`P->E%>S)$0k?Kwo9|9og28`+-@iFR#-2SPSo@M|fL^Qby( zfw*`bi^qd_T2ZUjUt9n;&SJLuTeLDrFX6 z`QTZA+vfY4`n_F4E^~sIFyHnKEF^l9pjTCPL^1r-;R*dvftp0I_^Ot>0?V zN?Xi)&kWU@b^9S<>(?3SvOPX#!v^Qzw|Kk&!OBwx*jQoiL>uL`bg9Iiv9-}wTe9=n z9o%Cs-in92?BdnE-E3`Ppi;15B|CP*-sS+;Tk@#CzE1aRT^e^bblqcdVdM*Ofne@$ z%(3&|cg!SLZt(qd!d-O76Jn_a&FI`{Jb~`D&(e3c@())~O0o2Jmkdzqe|-#)7k*$t zpSxI&d*9~U$w-oHCk7Bq%ahmLb^L};a7%!D$R#`(PB`l#b>igo$4+zQzg(gHkv6+x z@A-W)1Z@AtHkEI#P^}^vyIimtJJDB&R#le z0Iurk`$@%0gW8*PkHoze^1km=xmY1No?y?~8axe@Rx;#C7L}C@eWsaQq2B< zi)gLHvKEryrb9Xyw;bCQ`QuI_@8g@hA}gh|gf&-ySYh6l#?>u>L{+f(gNqvY=1i@( z(n`jP^G##H001BWNkl^0yEI zd)^+Of1q3G(y#W7WA;fd#*oy@X~YPHJz{5fz@u33U)pKg!^+`1@LH?~MR@is0W++Z_wzt_FsK{vE9yk@<}d(f}x)pP@mO#^1|f(5Jv zENE;nHrVd=)Of^_CCjqLDh)MP4v|C5ai??MkMEpw&y9@C$|^~gWb&((E2AQB+_>Yp zXMcNt`};@->`e@XqXiTW6@c#r+8=CBlq%=e1A0#xN9K$wCk&2D?~(Pn@0!ER+h$NW zR>0DGmavRvEd25U+`601*_cJGAyp}XdS6f?zvuQajE1PK)^_w9*p0ECxfZlyFMK3K z_-KgW<^Y8Qr0TKDHfDcx7M=S#aHolN<(xcN3dahV{*h^{|M@!2wdJ|CJWBpqMe5?w z`-dLV6{-VImJ9?O&*PL$G?>(r{Pyagb25uu&I|RIn3G>Tgw^1 zq_h%1d%g`;U?@IzZ4S$=atmyb)g>y{R?Ca!V6DKdd(?$fb_m=)5mGNhxv$*~)r5?remD}e-L34t^o5p$rxZumrq;n#*p+6kt=cN!cVm76OWv#QwJabT;! zgq$n(9_V5H!|M_DR{mC^@3;C()-?7HV>kbcvx(`)TgHjZV{d^EIx&Z(=Z@`1&HIUq786 zE97tG;I0^G*qO3InFOk_vT(Qnd^4y;(;9*5XemPUwMc_p0igk|N{Xe!RKMr$b*UYc z&ycJpNY)eBfsK{I3N2Fsywrm?c=jeHLI|X3iqU9^}+@Q?cl&xB~Ll2YTjR9fuDGmjU8q1Ex@C$6pT7P3W9diCW@(Axg{Ht;3_FU3Pe z;4MJ=Z?*ur=5J){Fk?Gb*95vp|Kuoo5A<;H=PzRR-_D|RgEYRh0V|R&%PgCRS%R7njp1Fbv{Tv>BjpiQyf=`|miH+*5RgA9C`|h-h-kBas zHbCJ{n~UY{iB#Ah?a#-|ztg)C$16i6%~k z!v!Q83F4JFV}9i_e8dr63aJO9Vg!AFgCvWcMh7?v6d&Ii$nDj7NZTZn5JIwVvQA9b zlS%oQigYD~UAMt(rdv2Lklf6h^I%yP(xEh#a>$)eyP2AEYB_jYR>ts}hEqBALxNk# zGP|`uTLb%HhQjee)=tRipQ~J!bKqP}SS9eywIOn{_e3gSbDBh3siltv4>`Rb2{;W0 zozHXt0c?D51C4hyXo7cZIWs>om{K)}^&aS9@!gB4-d#oMRV74EMCjhzrJ6%kiwmLm zzxAgrz0Wy7at%@Wu1d~a3ZQM%GU~0sAdq{ADz$==C-)!j>%TSsRa3W=GHvihB7lgl zTeJVIDNhyy0oG)cKoX=N5GgVH%fGR#kKx=8&FO8Ldc#y6;F=*OKiDVj+s0pSXrQ&- zN1Q1Ku^q#jv4GLm%RSqn%z3WobHz(V#D^t|8)wNY5z9W*cAmvm2`pjJeOiBYoyG&{ zU~Ly|aG!ujzurgGigso=!jOHha;`QN2^^^0QORnqIWT?Kw7xf^#RyC=j4qC{i5nLf zJvD+O9P~cZ)8zq5X6{y8IRnuxel!~_=)7a{JTzQl_84VOd2kHmCbY&zE7F%Q9^AXCi9-jqlPyjy}M zFDr!=SOp7>LIdU1>r1u>Az<4!T-Vj?^l&&t5Ck}M=nxu>2I}=XrlzJ)tyW=K7Hr$j z_R|QnH4x_<<#HLFP6t8=1VKPOAh)=F_w6<6abbX5o6$2VCe>2;Rb>=!FXGbwcL@u> zyZ~Vfgy%zYJ}+2s8Z=GtW;}>QAX!e(T5X|RERRh=b1|X(Lx{Y` z2ECcR)*$ydfn4?$hK;}3*t#Cqcy@0xZIiJbtE+(S{JZ8c|E_r~zGo40@0>&F#uCYr znITz@xt_=HIFmCohoM{Of}Lg8Js$m24AX$hbdDcAG{Wei5el~!5F89hExHh3bbdq~ zMpP@IQzaCR7Jz>SbU)Qa;g$mGZ>$sH^o3q_zT`YoSVH^8q#G$pmrC&O*)8LEw&Pmr zZ-M6l;P$3_xYE0V=wbxk;ag66y&F74k3@(NOlLg&!;#PirnM_PL$oNJA zu+jMu`k&p}*;YivvNj6r>SG|8Z7Z;J3mPk=USi==2%d7R{uR|kto+dmDsQNu zaI}ERZ50hlqvZ%2pV>g?Z#$^oT}APD5#`&;=s(iO${(*FT8ZE<_*s3TMJ-*-npOR` zorXL&!LdLa^yu@&faTv>&XzMDx)as`0G$tYF#A)p`nasTZ$cF3g7sXIt}Dc0B4 zF^BD0Tz!u*kn#ldAlLW4I^G-^fJ6pJnZ@AGk=wF6<7U!74wP=9`+oXO)A}BcRw4q$ zOA)|<1+gDfVjA~6{f~eEx*w&!z|3Qq{^4o3Q!e5qDY2E~a=a4ffGjvoAhj7Dr)`Wb zN7+8gaYZXfUAe*S4Qj zT9&0vqRf7_j4>ohf;3GLMG=G$ICkt9PMkP_`T2R2N+py^CD^u|tHFKmd*7Q~FS6II zVsIH_`n@QMbbF!g5;UhyoWkR09w#Sr&bM>D=o%>f^B@E*t6b7yht{x1OhVT1mHk+g* z7z=%kZwGR}hnU>L;R5hh>UCSvuiH(TtvBC!KF(V zcf9=94d6yWg!REXT934-4s+b~cYOIrwgI-r0OP;Zr;F}}G9W;vQ%1d?22fmJcy5>h z+i?a>zNZGMw38C6Ql?3xum#dErNIZ(D;Fn#;_0nHZhsHC^NK`Dkwa+)l<=$-hMucs zwHRqXh39z~oE_wVLha2pavGM~szI|YSz5WDngx(72v2z)fEtdYip4P5To@%^tAD%- z_OBR<$BST)lXs)uHaE#s?5#1W)~v0;{%XT3$u24Bw5luSF$XJ=)1Y0o;ZD1V*JA>8 z5NQ8J8*S0XA{Mdd-|xZji6J_F+W|mztmT)Lvl_&CAO0R6oqIZH?rCPsFM!e0BQn2I zGM1fy+evaIWe{WhEpElAzNw1NM>=_cXGHaGsy8hjBZh17-HWKav;rRToH^CZkIm#5 zTazv15eFY}a*34R69n1d+8K3VH%R_13Igdcg%w*EJ~+hsU#{z9hGJXw1Kcn@({VL$ zI}TnA=2A=Pf!w6QJ??7uc0Qbcfw%(fEPUU`@bMvn!vV3lN#A1e#@A`MNdPx=27E zTFHQ9bRojv2{LD@zN!kZ>1hTKkhSKV*t}Ujhm-simavG%4Gn+;jW;%EJ<9}uqh&dc zi>w}f@VPOGk&#r>|Hs&Cp1=0CZQ&)x|C<41{oa7i5A*08S!GK*!2^JmT4>oVh>Ad^ zTLI@>1G!?c2-~({S<(d@)Yn!4Q2;qm${54`{rhqF@L?=0ETCSmqgt(EdU`s?-0EZ} zF!A;6x8JTAUCufB{XXXB=eIr%plkHt{)0Go>0HK?PdIif?_0slkI$g|hBEq(_ECFP z4VV7kOK{)e!miqI>I7`aCtTdNp6XcqV?*y=WRiOJb^B6ojfICO!5~>4)70Eui7CqZ7i-4}M z1zg8P*Y5%@P&&H{a(5czJ#+Q5kui+EN~|plh&b{Z9x8Vdxr7m@?8_pCmw61R0LglS z@R5*ORXSEyp5#;OO3SlQIKSKX_PL5;v3N}hwXe%`vTJp5Y2^~qc1ng7*Af2BR72^_ zdsO$v7@WGJ>tjtYI}UD>jRb=;gY7{x#^hlFNH+-Nsv0piU>VEFf$m;1e%gx3U7uPf z7dKUN8;QKRl>8nwNZRu@O1G5|E`=!FQp#-a6;QA#Lu7^+(!^HR$R^o@$g-q0dSYpb zv?&u-eM>dK6QEWpV@R*{2Q4&zw29IwX*?xaVl)@{ zEkU}PqVn1bIv?+3pV^AV-DW=R?entQN4YQFyobVx0s&kGRNqWaj@~|!$$N8NZqgmC zM3{T$93?4)z{>Aut%XeKtLk^F!LCf2D5k2}%sc+FBFjEMl=nm=5MBs3Kgb30Kor2( zGkWXyx64;;!{WwgVniYW^B9VAMWDCMdR~BWpmG2!Pp=S*>oVP@hj~)YD^1yy5=3OP zAl;`L0#sM8dIL)qmv=eV*Vi$REkKNUsWYZoSpYOKlH-6L8`OO!fvw7DhgU*sWN)0K zxu2eczsJv7JTa!n2?2qV=_$-%9*$i09DF}y%`UJ3%Ctej}OJ;MPh_sJ59%d zOG7CRL@f!jg+O#Mf`Gv2-qGYqtJ?QT*W5U`72|JYpB1Tq-L%R0iUG-DLSsf-z#3VY z@n+DpnIc0W|K#GCvc0$5ax0dXm!6U2+4p^%JbV&s;Tk$K9fYTi zt{6gWzAwhckoMoTfxC79a#|+5(erv>KHG7L+-^|hdgcPUosd(cGK|3^1H^k`Z9=75sEL@=NGv8rr7B$v&xDzO zgvh!V6fP7He14a`eJu-VQ7~n=+nn(@Tkap9FbrYLHK(~n~o-6_k+I>#R3$(eA zeE2FSS+idvQXuZc7@Qs;df0GS-fki;moeG6w3osz+2D+mQ)7FWP{p`AkEB_OaN-`GZ^3>_F)AUhW8BNOgoy5RdU19O(p6p%PbA3I5^mlj)!f_ z79N8%li~@I-EnYjE~U?dfM#%Y&%~4(k#nM0^?F+GYoYX;B|VAOGC>2-s+(O)4;6p~ zofAt~d0;EIL)WST8elTPbYg&|1Q@JjFfr)ql=Md;B)NlPt2frtVr_YMD`YziU7ovW#j{{+Z>h4c}b;61uetL zNQn12`ip&H(Z+!%aP|9RQfvoyRdPlin@QitEQXn5GjMy)tk?AO5kU~-SzN`gOBNSN z1`Yw_Ox^TYV*@OqYsPX(?qmfPtfFLb+aY{+YZ)kW80?CjB|+r4sAcmo?nxP{)sBEb z<+cj&f71KjK2I~WrqjCMLf@Mw#VM3g!Q<9rSQ3l$J*uW(UgKIKtpK%^zepITW8zMe zEZUiJ^yi94iuCzc(f7q`F}ZI`S7iutRE8h}!TgGm+fP&OtN(5+S2{^;w}ha2)TZCW ze03hxQWb?lK?7RbwzZsCfl_nbim4SsAc`XRzK=?!f?~0V`T2S5*|P^TGc&sXZK+g( z<2c$p%Can+KYxDHxmEIJB~4D#R0C;cT;+LJuhZVQcOTZ0HHd*mwYhOlja$9{(L?jc zng|y|H1BF6ZKvpet&iS=JwO1JZ>k_^QN6Q0ZKJig%i!+D&e|6pKYjwY-+l+eaD-m3 ziz7#lKBL-R-}AA@+k+BHIE%BnYx-*SxDX(Jle;MKRyQI&2g(KEs}=4^7VY1uU2;|{sf_YTm#xSe`%H#Wzlp1Hj*_TU}!5I+?o zek#V`3j;95P<~yR%*b3oB-BP!qyl_}qkm7Ix*jk&uVxIXm7=m%L2%zLxP7kTTI+9t z=Lq1&l^E;8b+j(Gzz00*hdCC7mlBol&bn~wv?+Gk&Ona=Y5|Z;z?ejB1#Xj0g8pOu z$u(J%K(0CIVZ>r-^OHzOaA+a164)ghrCUmHrXA`fnF_4>tKb!mN;j(?41*ASiR7c; z03u{{laoRK!TtbAFG2BDMHEgHu>70Ln%SKC=TpS+fRQUQ18gQ41l42IBIjrGc&mx6 zWzqY(HdYxQ;$a8roo=OoMhl!p!J6>EdJUe7#ON=u44bM@2816fMhj6 z_rWgu5A;!fSs7~|TmvgHnjovzL)%>=z~qM+pj8BFpH)PcBh3Hoe6}ugog)9RbN7;Q zj)(+Sem8SY&M~&C{?=f0dB_pBGfnxp=}VPWasKRixO>TwHeQS&Tmk=^e1!LnfrB0F ztKN?tGH6>KM-%8Wpk$SH=8X9Q3*YxK&89JYdYIMXIwVKCIFnY6TQEx-Zy$YV~EY|7(6*Z{B(>1dk$dV4f{~7R^hrX z9LLcNspB}hw$`$&oI*yuPSX^YWx@A-xUNekWW}QHc{@8h3(xaVDwW`Q9(>;i=Ujta z0J^5u491uS!PRONoO47`gyC?gf%Hz}#M}v-X`KP@@+=W&gqpaw-q%9;JIW~EREAx$ zvHEALsJyO%V1GdMw*wA)U_+Dzc;)&OS-bI1BPJP0ojG%Q+xA6UeQv9$qKboi58~YA zbJq`YPY}puZ)4c_z{bR7&DWK?D(L=QcWaQVVhdK+eD>=;x&Rjb(*m5DgVqOHsN7zG zTXVq$2TK`j*Vd+ly+b{Su0ZL^u3Y(A!|a(0A4u0yct=Q47o8>bQ1=sE^jHt|H`nR8 z9%n4BUAKuu8qhPASq#;cDhg+IrOD4_Txq${sOf<;R#2w znjl?E(ehf@KYswz#c4eC)RXA*K1m$70K&|zRi zn0nO|ylF3Egk;81{a=W@@su{fQm=WIJW$)AnNOn*P1WbZrG2v;OHTxCX2bF<@F7QH zNo}rVCq*hydUXji-#4SLE0G8kju#M~3(@++7VMIZWH|xv@lDL}q;s=fRNq{kc#TKP z5f*=a5eI(#0D52P7e+sRMuZS=2GL) zY&LQF^yz#pZeM`=Jm((Xy|=4b`s(dfoc)Qjz`ID9!Q8A>1oMoVo|1)by^Hy1f=pQ@ z={ES2L36FHZ7#bQrP^r-jDdvH=BavUfdubmpPz)scC05{l6 zPQ$o41Hwr+(Z3gx1DpnMw#rbk|(QWq9{TT1Sk{=C>D$GJP(yh z1*K96)oN8Ux9aao=B&mv&N&`__~9G?S4^#9aErwvy4|jB6J*u}?<9QR$Nu_$T)>5^ zf?Noo_mv(>FD+s3&+kQYB_Z=a&w^dE!G|2y$kOe`c4IfL7PGT+TAI9_#JOCq;1#d< z763apw5nBWILHp-{FU=r%Re)=0;`Z9_ih5Yg`)-FZPHD5o6mJV)WO`l=D=*01E^cZ z7C@KY%I^7#|IbD2|JD5{oGjqVuUx_2|9dZyKMufM4$HN`A{vX@2io8(9F^q7`|Nza zLiNlYjYc@PbdD5)Wee^>7xCE`{tZ5?5|Kj5T0-Mq#YVIq!D%?uvBCzB0RD!L;?uiq z`g0xET7L^XivTx5gtg%s)>hZR+k9)L5pE4b;EH{;yX=a6h9B>zpaiJrO7=C&jk6NHTrl{ZvSJW+(#^iVilz!Iauv-0u^ zffOY{ zX5TzZpHptklUf6bvA-$iNV*;~TY_Er+hUgP+pDG7G9h4D7D<&8Vs!b4gGKaxPT4`i z6Yz)}2OY;j_rqNXAu#ji87*_RsutC&3W34n1D$*ruh|8VV_g@2eREx$8O3A8iGWx+ zz@Gov^Z9FTi~8Qmn=0A$t(KM7POj!w0o+K?K7<0iy>%i90H#V)Xsx%X#?Yc`EqOXY z_0}rpe|jE+GXu2$uAMU{yQ=g`EjKB)0KaDasd|rD49yEo;6>a4L9w{KM|+yZwE~)m z2v6uloxk5lcp-$p*GIGzq4s?>EWUdY`+s#mwY?Ff2CzH}wr@jJ1?&NttaUDSP<5&k zzn@1OozHa;UI;NfJ%l&o!Kpb&!xVO4V{~bR`ki&egBZa=pzCA9#Sj~x-oWhJXDRV= z_W+#_b@7IR4q?5zh9~CEWan4oRhg7hNl00%%Q#7sby4@AJ*(` z7>4jXPrr_e5%>m2t#b92J+)E|{b3)iPD`J=Ng%iPahYhdrHp$c1^SdAu`2{{({74= zhg_(TO_nAMxRZ&tBi|e=4EI>4P{3Sit!!=fNxnXV!reIEdF{DgkMD zu*y^(r~0Zp)8vscwJ~*7r~Tb{=5gf6(QB^11)e2pGP|7WL!ak|x)Ng~it-Ps%neUy^>~Q0I8mQh; zgYxLRW8x`JWDw*g8{frH&H3QR&12**x)wLzD^14Y8JN)l^~Jl zarwVfYar$7JaPH)^~z(``&v(%OU?e&EQ%+J#Iz)v*;^4xd2(lKd9n)M@zE$ZurgXf zd_E??l?p7qZwZyxRZuuoK;?B68qn__>2tCvwgSHDZ`-^-(S4Z#g_jplTYK^M(rh-G zXnnPXu)rfD?<^{!|;oCx*KJ>D<4b zL*=##X5TieWyZy$MRbtAZtu9|6`23pJnnz=KAbs!8W2<$uE(u8PdB#T#Kr>Jtr-lu z1rHJcwoHDda;#4!*y2pL3e(bMd5LQlm?5j}H}6w5wvFGr#(TwK5%axyG~Ff&vjupb zr@^h~c?g0aCoxul7l3A6^|nZ0>Nt)D$-eKSP$+1o*0yaF3I$k}g+igQ$^1sK!dI?b z$*tE6&N-&1rxC}oZab8wR5u*Y##k+^LQDy)MyrU|W9$v~f_FIrJD6Cz-KaFM)?d^8 z=eKfJRv_1#ql5@tKy~f2Yc`ybgW&RR&Dm~jAJ3#E(DfKv9XWCoOPwWrW$k{99v$t- zbP5a@-7~@lHZb)X339(5*!b^bAb0F@OTV#%>RnY-@2aBn;Vqu|Tg5i4Zudi7bU-Z+ zhshA#k90MgD}>Nyqy7OOOVJYM&&|ImJuF@bp{}I^`~YA97uGLe@W_Dt3K%eABls`z zVf!|mDQcNk4p-V zN>($L13MPgtf_iivp=c{Zq-lHNzi+wx7lQ5Ct-kFQ<6OU&RHt}%q*7m%T0NjwGEPnBC5VM$AX)KwLr$!0vWUF(1cU>t+na>(YX<-FRNzj z0`kcR7+_klx@zAYX!9EldL)yl-KxPd_F1k|0XxPSESDHv?Px85uvN6EUL|I5XI(76 zdl}_FDC-AE>BbT;0=!py7+xA`Fb4t5|GRlC{nw?;ZF+~{Eo1{+@BUsM*p9DeD{!cm zHQB0!CiWtc#%9tfSs57dkl5Sg3Ykl7B~Ml{U7g0ts;uo*=RXCyAMc`P^-z9o8RZ+w zaBD8QpXz2NLD&N9?j%gsR_&u14A|4y1FT)&>R(9V`#ze}P4ph^!I^Pj1r~V9^@Ic| zF`WHJ`{dS{0<%9ni`jR~(tVs5piCxBEZ>4talofJ(oTxbc z8{lpDc=ThB5V)1ww(@)OI#R5w0LrhYL{hLnfB^OQbsPB!mu7m6T6+zQ9igSo%2-Y- zOl+CZlxmFJvw%U54SGCNs+cZ$CMe+B+-PEdv>)Yi8KqJQuIp-sRRP*Up#b0ab56;o zTQWehWX`#kFDoWMeNNTks#;qmdGLY=-b`h_pwlrG349aIrh7-i@uJ(P(0Jb`Jmek6(D!*0~e5_vqeZ zsQYz%?9q?r7n%TU&DX9X)Tku1Dn<(9Pav=zJ)fPh9@x z%h>z#d*M#GRAy9mfC>;C3D8Ztn0|cvI=*~2RH$nw@B)lRBb*oK_4pV*9>S?PsPrqC z3Z~#CyTJDA8P`*P3tT6_jU4J}+jsl$kNfC+Z2Qbr@Uj5)SJvr2CSxY+FPWdO<azdme$-1VOBLft2V#Q+)ciPRbi_^p_jU|fh z0&C^9jCo7PHbhMzn8>Zi*+YE>3r7li(%8Mfi{;;1#-aBef>n@gD??`fVgcMDme^vJ z9(Yx6SDr{ovUE?jxRo8(1iA(n3|Tu6P7+D&x-3C09LWkRfa6%O}(^2&^Y9g!F8%uL735Eq|@@sKvo)t3PBUr@bEbPv?h?XLK&YUR+ zR@p*yDT3Q{F#=sfGe159f1km0sPEVT(Dj<02Bw2E161#-=FT$&P&iu10HZK|01TfZ z2i}$UuAqx7Lsh=H4Bq1!+>XnW#Yno!?huRno{wg|iItU=Y%(jiPox6fPj->MGKDwi zq4|SNgqK5f|5sNoA~xMq6CdW2*xIqbnNfO03F#9rLT9NhVw}b0eU4-!LE)5S?owdt zbyH~nRU7TUZlnJCI=qGl-LIB`SlomZ6J{;0U9quLSc0$w+`xrhu)!*1NM#qOw%RIK zq$cqh)d!7D#EOTD!0UnTC%bUw9fS{t=pE~!baN?pO#;BTAAdCf;C}x;;FZAK19K2U zVEE(^-M^RDsgWq`eUjL|p6KP7Q~;&dl;H2D`^%g4=zg@Y>6*}P>Pl53jMhZb7*8?0 zDzT*iuyBM<;IWjX(N5h#(n;V64-4xHn004SpQ)o#spOc}LV=iE1#p3^#?~~KQZ0MN z&8HOW>$GPt4l;I%R-t;xtr{3s~A}`qA1dUm~)OW3^mY=q6n?^7OHnt z(fu@?kI@4ocqcqKbq99YMsPTQNCZZs5k~VPL~9Y2!X@y8W6xj@seR0QSY9vwjXqew zF#5^}s&@O*f4GF^zidJb1cHqKl~x6AXBXV=#`dvq-$Kqj>bZg0tAp= zPA5OJ#@-v=GX$6fxkrm&KftJG{C40N$V$DgHzokx@z!#kdpl@;cN5->hu*_I6pj>N zr#9k+7;9MD1-Z{csAT2MwVTmyw*gS;2^gv2R7=L)`1-~5)ZYTv0dPCKgI28t=ct2p zF-8Al{cV@8eNT=0jH!u>l}%(U-^wOBim~O{L`Ab&a>6t^H$wX(?Hy0*b74%zOy;)o zJ$Xs8Vq0?HmS=5#?7&9h6$K2xFw9Nj0O-&ACw%lj+t;QmSKl-Sx8cHRIDk#PAvgZl z26~?$b0V+dX@=EeR?e)2Sp@t9$b3qDA7{icG+l#Nq;I^R$+oq5v+cGv+umH8?KYd6 zYtv@iHQBap*JgL;ckj99%pWjk-ub@I_xbReYzjaLD^6J#Q2>yhoEHd@rT)G;-xuI! zVculA3_b1X2kZQ4qriRzU`wxaoE(2t^HaKg3o=nft}yn#Z&Qn+l9E^|G^uC~gJdwS z8EfL=#DoS~TL$nGT`)JyJY9w=Btc0+1Z#e~6Iz^;^E^qxSTBkS>H_OH>|6y%9X_)D zthobb4M*c;COcfIVQu7pQpVrE*!ZPX?OaM)8e2ht!|dNjRs@{OGZT{qJSo{06$dqL zOxX$UVKg+59wv~0NS!R*{>Vx`-_d(I0pJEyyth4(974=}E85cqtrOc#dw~^^!kZYA z_?_2zoU&UH*g5vO{%!?+IOb3|6qs$UFm8JGE+nnbC%(g94zUT6v>Sv12hX*$7By#F z6)3g!9g>{!42J#dUMVoWZUdp}dA|x<7d1yk8zn-DwPZz2qE))QB zR2RH{dDPtG9tRErTQ3C7Kek&Cd$txBsym`az zYLvRMjrUp0a$;AH`;9f{+lFi+02e>bn@ooZ?{l9<(PK|bv$LHD%PK4!=hPZ4Q<$f{ z3hts^Sp$m{fu$>Z8ZnBaWZIykwknu9Q8ddSx`C$**A8mPif+bu3x^z^n~_WbUg?|q z5|vVM_|P4+6tkU^TgCi`6oT`zn3u;TbOR*Tw_UUuOmuSelRZ5z^@x}E6dNemW34DH zLyC0~BSqi|&G))&Qc&ip<5bluo=?DPcq?}s&6bh`K@pySK(~!R)XmF(>!Y4vEDZ|LDz8`IKQ-zK9cB_5!V|hz}YOPNHwgc8>H;2ya*tc6`vm~KS zQ4S_3P=jrMd0}9Uhzv7rj}t?K@RG!ZI;V%X0Hs0Tui~z=iZ_Nqg+ulBf9s)TTV2`9 ztljEh0wPtird3|#mJZyyT7*B6>6s+YKo)Odwa_nK-;{e0rifa~`q&se!h>Je=qT;< zT;yb~MYU!?GpJF3Ka5r@(t(rj?T5Qr0sD(W@=2*gclG!JyFd5F^5_C{y$G@0a?a6_ z;W}RR9TW1<&(46oYJ5VAeF2f%j22%(TW@LhG$Ln!4Xq#eROcElvM8$#R52ZrT>zEw z@w6o1J0hLb@Kv3ntw9+kN7WhIC!gbS1?VA1@>&{K6D3h8E#9|fHt8??2@hW76b0)X zjDEIOsmITPwU~@*K%spoh_vqjWz&erkE2(wwPL?uwrp~o>%(EXZ0D+&+O|jg%2#F* z#=}G+7=FS^rO(laBg(Y*S343)JLnHQpxx#~6~C`OopWh?P*|p1e*eP32%XPXv2#-R z2&!?beh@RpG~>GRfAuMO2XJ6j)Xih!i(uy$-QmhXF$bXa=u7BWC<&hfoW&RZsN+Ma z`WCw&5lmT{pK9K^$QO^zGarAr4mU*=GIcr;i@dLTQe@zBFnHV+V7|ob)~VN&eEfjW zE0?RS>z%*ZGVFgH{|=vS$oISnCfo$=~sY8o=saFnlL$phW@O3a4=W~+WuYKs%jp{ zaqC_SHTm;G2v224A@W_=qdtSo*&p?#s7Ue7l`@rhDs1VH$+9J_Pdphf`0 z2o8<3Y2EMKMSc7m1(7wLODpBl;Yi&4;NMUY-3g|+UPc>3P_gu4z`e&uhEWQRxMZq7J z)3T3X!!OlrNUN4?v@-2$W);lHZdryXk5iZ2v<e{KYWHr;jH#`ujr93;}&tcl8^)edj>Noh*cc0dR*C0UQpJU5+de{RX3fMRNFkW zDiV~qw1@|}iGirx!)~?hMP6q45_b0}tOm>NBEG;gbMvrP9ArN{>#My5H~-hLPyt@b z2d>TG{gI^vGn;ur%Suy}GR)@hBtbi=p?Gy<2+g=?>qMl)1>rg2w`2Ndeq4<7V8z1` zVI4@cQUh`_(I`#$N(>7s!2p641McCssmp)+T360s%(V=h{p1`xmA)VI5RbX?EWIAm zj)q>rS^tN2r>UJFt!hezodVV0#XefH0v#yr?}#6aLcOqJJYmExUDE?&_8>@a0`Gr3 z-bk`GzXz#=QVQnEO`n~aO1{Ub<_}4qG|Pl*8jEX&Gb#`M^atLg{e?gi*@*=snk0Gj z=rUhr!*y#vVy~UTC*O&mZaw=bn|Eg2M;-eaeE)F?5D7Ws-FhFga&m?k*OpKXhkOHq zIqzRdz$DwXj2=#W^?`o6>oSqbvPyM;CL?=i*Qr(+t5_?G^Q7bR&4K*VZykkqO6feR zvG-qJdS*nHyvi+tRXmo>2tN$I2<*~qX%+3;&`WfXh!vPxHN9w-U)$g9aOr{R(o3ZK zJY3pj_ib}hy*#fq56;2S0cxAki(_imiA7P&jqIHwm3k;)F3EI$-;+HMX@h_`nz1Gm+`b%)GX5#!}a_3-jflktA~@d&<=p z5ZK_^4V!Z@Y)H+#>)_^lSECd(BPTW{W9B$Y*TF1ClI@Y3OkxCl?7dffK=|v|^@u4{ zEG9zoyb@6!MM#?fAu)14SK#X|{_RnJepx55tl@%hNwq7AZm$Wsq_u9XL#@h0(hV%w z%yfk45JN%434Vj6##g3}r!F;@N=B^R4#Yr3_eI5Ps7<2HF-@r$Ax_1P8}Yt-RY*}X zOaVHP^_2bdB-?KyfYCZM#bW{$d$AVvVWlX7vjF4p^u{Qjo20_;T?tSU3<51_167^7 zF;Q3x_AF9uJlILJMC(LrdwR#VX6nc+GSD^+6tC<*%YCupaA!U<950*OH7T$0eiZr0 z@|JIrq<$U4ktWBHV#2&%u>qSI2me_raC3G;*VyJs37&^NA^{O;Iw}BX)QI`j(#2-C zd}&DwFw_Qf+uh$>cQTq@2e2kisCB@Ub(>>>_QJIuoL?O3Z-kcmDs#sr;pCAdbkQ%u zhis%!_=C_mfnvbjLOk#r419T*1eXFk<5iTIx)4LIXB>&dFiX2tr`R)6yy=ARmqJaF z!HzvL<~v)!Mzz#MI1+z;dbEG_WhUI2CqCVx@ejJ5-#dVj=kYxr@B)%_+VL3eO2)pqwqv>^W#-ICI#M;5qKWbl{d!vsH1q0(FX0qQ1bCP1%Bd1whYIJ1 z-aWtr6MF?BpCdx+Wr*1hV80eA#!3n$I$9WXeE!bdsdq6 zU6rpkA^|}h_jZ%p*}nVU0P&KpAEFJ=fdbvgwjs19 z00`d1jKG2UUgQvlV-eoqD+d^brz>A^spA`Qbg+I1QP-{kUsSJpvfb*|xT*$sGq&Ae z6HI^4TI)ZiL0ISL^=;p>>3;PeC8P^aHF05fQ||nW#EM08&TL32p@xg^bd3@Q8VWsz zo5(GjQe1UQ4&a_h%}a(u>mKh)p9y7`VXDL@pb|}zE6Ip1ebKX%9brq&Rjd;mO_Ju1 zmk3NiMQ80ECrdS^dR}lcla&dv4d9S>yeytHn)+-EOMq$CpoZ}9J;z=v)ADit#tx}cQTLx~OEet4BB3Wf>m_F2y`NuOl!)a(Z0rwjLC$@MBw)zN4 zO6}^-zy)#JyCBEOrEg)_z$1h4hs9B;{pCXT&r?yprgbnK$abhfrM>T3nkY))bfYn@ z+3I6^@6dUbh@U{f;&%O}?t0S4jJmV6P0>OHP%*r7IQ%AGp25REyajpVW4|zWPe=w1 z)6}tMy!&!Wu}ml;-d`4?`3>q7LVpZJiNCazzqrKj5-)0uNa9=&)4@V7I-8`&wgqf# zlG#lfF&w4S(YEHKh)5da*G8fNoX}7$1ZPpbcNB%O<0|I6wZgcsh8UU}4l=;Fk5YNw zE|)HB0Zq-&604~8K)bXFcesi;Gae?8?~;_;g8bs2s=lC()3~*FOK#OAmhP^VFm72V zv*z<&lmhg55RMf<9jd5<~6uZUVtKx#;-qV|1Krl|C~MnbKi)~uu!l_ zTm08tNk@_!xj0Asamv^SL8rU&6RGNO1j|I(4^&i#sv*usk_ zpS$8^ovJo9>r6&{Mr@$x-<@##{F2rvxH&tpAmEkw)O)Yj(d;(9&ng)5;;0m$iM_nl zZsTP;Wq)&`T|u&mxd)2)kcBo+i5*1|&tFj%pEL?tJ^6Z8Q(p(U^4 zG7?M1P%@swt{?)~uiIsu{wgXIiRh!e+LS6UXJ~MX(tr_irgs>94_U3=x2f@bpEbN? z1 zCR{=4wRLBriM4y7bZ^7g=!kF}jn+y@Ka;tVeiuw3FXfe`j2E5F*jK^L0k^^8glIM2(jh zcK#oU8X25SeAt~tvNzr@{{Cw}PJPg|F88OEy z!LvT_;H}|eZFOVaY1mTXx}r*d!ybnrYuk~K31Leu*9kN1gG4{l5LPAcY!v`0fqUZ9 zE|2@qE7~$y1`}+1{bEgJT``Ddn-XG!|L*P?19n3J0Rx!dIXd%28}nt+dvD3Sa!$=g zs6GFNqK3eEmQ?FrNhqdDLwOI2ly<5N{R-0omb!@)sz@&_XOL=wG!%xYrLv$5)ZZ&# zlu0$EV}=4M%IX)w3~hKPdBGjrCUJsICj@Nxo8Y(BPA_ytb#?xRoylbrZ?ul4Cine^ z1*+g(bnPV}E~Goy&9LYZea||7&Bo=96Apo))e4a0)`n zs+ae6M_=?%s3r*tz9CSyBMQC}(~R>&h`Il*UIUaDQ~qcfnj(C~cxAWn35Abb;5w_J zb*I4#G35TYK)u9Pbu(NOsA#($Giu@ktrFq-K0J2~gXeebq^^9VY_M>1Ev$K)j0cNU zFx@!6Bj6?VtaU2Pk*8Mp!R~yJTgTFW?Y_Nby%nkWq`Uf+K+Q6qn{3|H{PaSV22@ZW zuS^;b+YFJ8r0uJrpa)|C^Pf;@pSeXRy1s?lnqG91TDlG@+FVh2BCQbOUGhciv)onZ zw@&jLu^`sp>l=uKm|)X(uK22AsaM+B7%YK(9@iTzpDoG!+3wlo!V2b!8I$RC4ewEy zI39_(Xr<1^=z_%7%Or*52F!XV%!StqD@(I11{F(5Gip2rEvU;hQyV;*I|%n%VRlse9Le({`FD==39xv=b~jja zX$qe);I3cevYIR5_gYz27Gugtk*f9>ec`xaLuLLPg*YyGQfHy~T7LvtbFnQ%Bp0bq znJCFRzsPG)X!;5lYS!t7dbnEdqsSG$h1r5d+m}n@7uUJxMG=hn+wersmGDFXZNnk3 zs^_ZU9Rh@E`Xn!}tsTG~4ogGt+m8Fhb6IN)P$R5wDEb^8yw85=2_#~q>b z1Y`}$d>S%J2TQLKP-AEes*NebW%QrMm4uDNG>R+Tu*}6UMHKK}9z7X7>``WT_OZj~ zY1)QN7{NA(SD`lgil5902UpOC1=e1i#px{HI(y$ejp+Scg;_aUYepMaZ4bUCrxMyI zl2zHcPxHURGFx|<=%P;4)?gF0-|uE(IOV=nBL-`HY<(ZlRX-oHNPSwf3Z*fCl zOB)Fprx>csmraKu^E3Lb66773|JMTiKFWI>$rdtbwSMrk|5gA| zNGO|bF9xxUO$auMKe<1H1)JeghOJ%ED;^K4%|_f@ArK9fIHguP<(2E8e6h``N;kw5 zlb%!E<*ntovd>2j8EHk>U~e{psUD1}|Aua_`+(MVI63 zt%)h>)s1>9Ek{WmyqXYom^EYZ>{$@`rZ^%I1NXu|hUQTf$av65(cxcSUmQ~PD@@eT)6b_qBPO2whcnEI}Lu0OhMjBu=)YSe0fMY%e({`Ef##?pQz+ z%F1A?ah`=no$Qc8cV;uWe@~n&I3Z%tcb8{ICtZvemd|==xlZToj`T3~_#WfVjXMJS z+hV`uLH5m2=JnE;WyE#!f4)gcJkI=9P+6|$nq3U;Y(nCjsaHIovuoTM{y+M&LZ~zH zZV-zc%YgtTZw&p-S19-^8WHN9Yt2Wg>-a~v&Eh8PGy{{ww69?h1#ou&&uv4(gID}t zC^?mDL7C{5?kMFXAUIID5`>NJP`<9w1HO)cFZnH7m%SOyVCByaryyhdSRLo^L_cl1RP4Us#FGMTrv|E;^5uXtw~i=1Pr{({VPYVWB-WAd-=V6BEbC_RNc#g z1s~71^*=KpzO&)Yfx>g7(~iUdmBQryjgjico@V;hr_3bP185X{i`IRdPz)jqzl{9L zadm@@!oik#gX&b+1|*$i&TK$}eIRdi8`Be#<}x-*nBY~YuZ+yow-jo7Y;fS9DBkwT z-Or>S+Jo2Y4o*{g6kH!pkms;k*(1tA53h055KKe}wlUQ<&S0sKc+`P3V?b0yX4W3$Ym9$7jUNL|+q!@px(Aw3=1uYcfdb9Qw6 zP41D%kWiU%3ciHYhgdVIsLU9K6nQg^E)j_qT`l?u=;(uAzud5}!(Pvh8uWprKS9|2 z%6_En%(lnWLMax_WpVKaodbw9tJ%UBX2ZMii-xZ-woe=C#wZKGk?Y=7t42WSF67g@ z?^Z(@KW)$DQdb%yL#ZPE9j1tk{v!`soU-O>>48iq`o>mtzpv%C2ZgU9zdB;9%SBtb z=J8|JK<$U}1ofu*A7W^$A=qC*5wq}^@OL`^yl>5!=+Ra-oP>6wKXTrg@4qqxU5u z!3FhBQEnqdGPPZH@{@;OlWStBx-;t_i+2G;G#|#lmlKChI`u!tTp7;wr$KgL9Vpmi zE~3HOntk+ByN)ef1)%vJJ(einyS)1Sb4~%TB&)9Fl-gpL za>PD;q>*$s!BnAJveqP{@%=1D;E#ES24Et)euU;w1+I9X)V6rrH#X=M)sB}(O|$HH z(menxd}{E|$K^PWDPE%8z?z)RHO1|N*lQ5zA+kM{l@ha+6M$?7Ofgr z_r2E7kkfxRo#KeE0f0Ard@Y~b_19F)+0zV{C+aTd{4`w^NbQysZ5Ta2uBD6KQV8zp zu;UAWQW6(D*`oeB)cIRd1+pKIUKBYHR@qS$dG#N9#Svms8cFl|$qDz2 zK_GR>MSz}0@G3^pCRJt0*@1lwFgpY$?GV#k@kkgv-@`Dh%On}aHBznFpS<6Zo)!iB zv+(-n=0jy<{~ok~ieQTmMYJS#W$oG(>_#r=h>C>HZiPAGPLs>*p!YpVWC{~tny8mu zi6n#cRe#ykHFld9xe8F%HtcZn^OakeIk(&1cU*-7Gi>p4bk4X?mF?zz!c~ zZN+I&^`Cnb)w5an;trSRj6kv>ElcS-h^B-_=&AMJuC^wW)5055r+&>4%|tbi%OX(0 z#bV%fQCT?fII4V5i?!0X5G6gK#2r)!gx%&v1a%^tysmF34lI2;%<80 z#Ruc~D=(9&5Ko9NxP9vT9xS2O`>i@wS7G=Gw*o~6R^EMBsHsi;3N0YM{@5MiPKYZ& z9{o_Z^-Cy+AgtYxu*qVCti{o14(Ktu?!0Xhvdj5)OugE8=oF3v+_Q`Z1%D_9EfFA{ z2SGctIGoZ!sk4}I0H2K?Czv8hke*ce{ikV6YNfbz0SyM48MQwpn@Bj62-@G7m@1S7 zYy*%c^On!yQ0LLQ^Og4k`~vB>3j@$8=+zG8=+gem-jDxOJUmSLfh&E=v8gE5b&1KC zM1a1n&REXGU$ODPh&^d(r%)z@E_KwBgAh&wvgNCuntfHT;D7s4$=6F=YBS#9L2hoA zM^))D(yTznOvf}=%#E;iaeOn8e@f_;bBtIx2zWkz{t?Xz1!X&dyF~N=O)a)T%al;9 zIP@kltZF(HU6N_gC^t{9xzuD6ft<>j=R&SAn<5y1H4{l(G&COwHkeV(o%p>t^!2{? zp@Z>?msKJ-E55EyXZCDKdI-yUFd@lIhlbds3{_KENF;LPo7yOcd^!kQTNit|?Bdsa zXY(_`s$&y=3pV~53z8&Ttdlx1*HQ_mBh;nw#)umAdAzF_>6F}A0h>h^@pEq&OkGOFYkL; z3L}mQK_+E9&?vD^wG5AawZzMdvY1fKYqrKC;c}I|Y-2Ty;wd>ir zF^0r-hVnZ8Fby3zhr6vuZ%?r#pbyVAjAN%tjY=T#w%!g=4ifP!I@EBlY{97JBUu|Bfo7s>ls&AvXEV+`DdX&eqCc z%Hp0;ry;*Q&n#z_mAvm^R>UoyK?Rg$KN0}?8>3+!DV3^+z&sJ>c#%;1Yw+X{%(3tY z`Zw?Q_(fZ7j8@Yog5#s+b+Grgk3Sxn+J0O>u(I4Lnj2o#qF1OApNw7=-9Zed+ZZM3 z!l!&JU_5*tz1JKv@J>fr(+47j$CnvMMwtAWR~_pP8Jgpw*IU_o^X(t`eMk_-Yj6B= zRJ_CO+iw@$+XFFb&z%1lb2|qx(-7muecnqLs%lb;@N~;wm$h|x1Qb$G<&}oI$kh@C zwG;KEH+;C!pJ{z(-mrjlGOIp=>L>xGr_!}RV-P&klJel>`sm~TV+l2w-l=|?$JGQX zZ>+VY`N)!UCC^XE{A~+4PkE(NQ>1P{+Fj&3G|$hTl*f1>>FhEmaM z6J5ixQ##sx*E3|H2BZ5kha8^z_wf2nFBsiRGt_t#>#{ZNlKVzlongwe^a^S%*aT^nMBI8(~nKs6; zP6}0WlKOX3e?aN#D-Pa7BpiQEc+x7w@pwA>cIMug-=|rB_`laraHliRBwnJZj3-GX z1GET={NAsdb`*y92MjagU|QdXy+!KZEt&Y<$gP6;#>3W>i=Bq#=n!oY8NNjChVfQTof3vskLZR;~~T z;v%t7&k4p!IsoDQiz6BUer} zu{^6LLOBU@!t9ql*oaguf8hD82^scn{-hv~C_tV=LU-T8S}zMM_Lswg`*gJRB;2=K zyB*86-5$q7v>OuW##kTxIqyS>NN_i3@VxI{fIfvprEQu7XNpx0p*S2a@%2Bvz4ObgjCDdyaN)W3!>?={Bm z(5JmvkLR_-HrVbY{dS#8ly({f1m}DbBE|x9@0wH)0=Yr1@bmT(k{VsOc67tf9mxe*(P*hoCQpoC zbVSAGqgcT9hSH}c@qhoCQNQ5@6gvn5<|1nMiiqzRpFQigDkG#v;ry(E zTNm+Kfxj4_We2#}&!!V<10iF~5S+R`inwz(?7B+WZr2`qYb#5-pl^H~h5)*N*ek({G)D zWZ++(i+c`ILmdwXWH5wWWK7IjY8v|A2Nd zUT3sVncwEo2>oiw*jaYlnBTw10$|rlX!JD`mbmJ_jJhWGMA5u6pP^v-bR$ti zm&}{oDtHvwcuz_a4bT{<7q~GvQ=eW{iDGOT^g$kCS2zm2YKf{Ky}IvsQ9N_tM!(1<0X8sZzJFi`J;aNH37gjXfWE8*e&) zg18S>FlUpX8+ezl-f=|8lfih!-5E}3ImETs;M(*XL`QHdDtXf>+{a_uXXm!*a7LB? zc6z*iLJI*wJ2>!EyKGl_%(s;j_hvK})KgjCgkT~U9N;sha(wh&LagUAtzQ+|T2u&9 z_ETO~p1g2m3p`WO*;kVi1v^|h&gDwv@+jnSD&5G5TMW@yL8iM$ALpOo0-bEEUZY=bKc4Ydo=zFXKPg-Og;E?v8|F zdb1$_@&3e+Z$+q;B|u28kNZ3YKnM_qyKblqit>%~ixGDLgJ(>vHuK4iU`1+@&JLvr z?ItorKZMBj_|tj$`eET}OD_A=FOqVo@{Tca-m%@4@BTA_8gwa2xU*8&BjZpTvK06{_B+t@5k8bDrl%bnaU!IMlfrluVgTbPm zR;e=Sd3H(Y$)k^x6E?C>-O{sv9jgN7juiGX`j&RDcgg{PpdJ8-1l&0fxn=A9H|@7) zYh>35@{eDV_>G!Nh6?EvyUJFTw$n({p1Fw-Yj21S?CrtNY>nasHLqJ2MQ%9X z5!rXFFWxp z{q@M1=3R<*Vw+AdDx@zKFI@;^guhSby zQD%um-!NU|7Sy)u>RTg)G`JKm7TKQC`B#yDr{oHs<#Q}T z5*N-g%=CYszsiGg<+ew6*7wA@6G0lRh41h5kY+Ky+2$*WM+F`wX%KqoT5J8I%*=h> z)A_DzD0aa`$DbpdLuSbrtHkPbtg-IcpwRV(U#hM%OE z{botcwWY80S0jWC9tv3R@Tzx{tqsmw>vtN(!uF=LgCR^OJNhQV(2dt&rBva4NRRk^dh4`M?BUQm4<^-9><`{BV z$z0K7JeavRFXF7c5ae+#_<;)m&6+|AdxchF%*azITlUn$%G84-P z*FBOa!*iO#-WI-ARs`*C7cc-K{s*kZB|^i*Xq~kJ*c_7b*mFSt(A$I`&XzN?5~nx4 zNGhwXX{W_1UksrZHkFxq-M9}0C*|o;a2XLSt5!eDze9K%A3Mm4E;(^mq4oe@zZz6H zpvh?Wrs7qI`(WQq*N+r`&x6muH2==4CyfstHI%?ZzftX`{CUEMfCvrIuw7bKH*Ab} zP2$|Yf%j6Nuq9d?*K42C6*zy(@>9@|l$3P%v(1_ZCSy={02q9n-!d55 zUIrXy!jm{lV{=)^RlN5Qm<#jBp2K}nAtpwm>HHi1f%;McYJv zn+jo~(~@Ebs{9;rB#HxUb(9+FM(4P1dVc+N(){@v{ulh9M}?;*v)q@M*9IZLX3QwowZ^JiLj`4xkrD!JL1+{#R2=2yAO7xx zVxO0tZ!tj>0=TjO7$2;}_=bWb)WY3wlgtph$jn@t+TQ zk*ak!B!+D50w%I#0`MQ20^mk;4Bb=@T6=h7HCY$q0Ke&IKpjcY3cU%>ufoTPtLNs_ zB-w~7(1)r&-s-!!qHdJGgNNq=qnHteR*codSOm3mXr{9<3%cn!+JbE(?e9y8A#b;u z*{Sx-?~u~&hw~Nobl5D^#CNp$71LZ=eN~yMs7T`P4x%HQv163xr(#&SslYlSmA{(M z)gN`jT;MwJ=K_z&hALE%q9RvQ5rwL=Q9mua?QpGi+A>=E>1^IxfnY7Hq~+IGxDz+HF(K^UUHi z1{P%b0+RtYU!nb0f7s+Z+rzSV2|!+D!dh>V7jyeFz?XLBMIIp8P;-Sp48&jXGH9l+(Bq>dp}*PudQs_wo|T^z37tu2tripP2R>C7BECY0$n}A zEgK+tfRI#Qg=JtxWz`d)K?*dKM+?9AjF7xHCC=6=8K|^fF_NMSHD^?F0xwR_s7K`@~ou!hNucjtRZyB!3ag4Y_VKg^4mzJ#niSO1ksQRMX zfHGP8?-^PADq2a4#_$2xy!K_N977L95tXV2nG0jPEyG+#U6M1=agRP>JRF(43`w( zLqcO{`{(FxhAW-ikV%&3STJqTBHrD zlVg*RjsGLe1qctAU7*53#oftvk={q+$axK?@A-ntlM+akNuSw=;tZQMq6AN`>wBDP zlb2+&PCE?H6QTPZrQt4uamPA)VknKqyOe@RZKz1|`>$!T#Dc%;aNuV{9GHvp><>Qj zteL2^m;-M{uT-k;-ejt@V*_VV*P1@TV@3!r_==*JQwyx0f57L3?RO7#=;}YdoBa(5 zFqI`_4+%F>{xT=Q#%ys|pZLQs2__$-3N)jg5kCbEc^X|1yxlKBmcrklJ9{5D7}m-+ z6`83YT^1vBNQFRUuGrI8{i*o|*UOB+&`LIB%zn z`XRFKiY#TRBByE*Kxx-#D5>!ht@2(q-W~GVXl-_MSed9?D7`k+cBgCsyr>RKhNlDc zFkYkHOj5}zkCyP87&DA?nM3+dE#3><@=eqc*DIgSL_uEOX^BK9Lj&MbR<(-~m`X8hnWLc|Y_1hC5I75a~MAy3TDGYkLXGMNcCOBDt>J43&pk8D6 zpl4?Al}L4M^on4U@e=(?r<}y+7g&eiY=XyWC5_DhZ3qoG7QAOq!eum>rqyAA{)%!Tw&3$&CS+h$l+4KzHQ%uTtN$eFi$RERLV(~VRzuLO z?XX?_!csiZ@UD0htyxuV=&%HM9||v97lqwfLDLZp0AV{`>>?|?jXPmx7QwUy5;Jq-gGkpRnhQ zl!k<}T-P-EeR$}sn>RWI~s#xw_Vs8OyO-AF~HnRa-d6di3GWKBZEZy%hp z(Yk1{^c?5ddW)%Dlbk2KOhO_sUo)5l1n+zLE<4rAhO$jqBZQiRhykC;a;}4{@`qgv z$qN?NBOlA$%UC|3A~;f8#w2(jXRbCIHhuHRTquKuPpMd57;si17&htO#z*jQ>sl@bgLxw;jRB*JN*~Xjn~6FQ7->$*vG06t1T@i3GP|zHq=Z9($7RBO3)9taf! zxbR*(_H~u}F(*2n!ZrcBEtcjSe29rAPMZlFwZ9(lAPhA_wHAgCS0*)f4T>>He zF4Y%Ik2z3{XUh7m6sM4kj_I0;9FP(~Am-?ZEFkMi?%)P{1 zn(}X|o_sZBDdW6Dgnqj*{t7ED&M6SyO^Wo-+nuAv zTW5fPBya?UPQJP6Ak_(0JygK~@wLFB{A!#OTAKGzxeW!-MRo}-NUomaI<2n+k7X1l znKUGGP&bDH#h7DEo)oZH4&^qsag&mhI3Z_hpWd_ojsU~*2c2CGSe+1B;ghnmVVYq> zMiou<=%pTJZkhhr$>W#txA3B0i~LFZB9~;r=tNhw`Awy&9TdOP6k~m1UE9zRY}Y88 zbddOqz*I4t_%RsKdbM|csHo=S#}YH){;9fgX1@W0EJLpbjjIK~=0o66-z=IlLwvb#FVd*?~TG+K#54Vc3MRYwDx zG<)Y$1;$ho_QKUHLLf?pS0#er9#BqkS6?*f=OCoa8~@=bJAraarH*Tf0OZd;W)4$jd`IHbT)(00@ek!!=!m^h{F`vAvh%84^`r^ z#?_AjE#K7<2?t>is+qbsv&3Jknev`8$YCDQ@b<`H9}^#VOuAoMZ0!;3ZpX^E5~nuz zNCYl>c+W*jAgtO#i*tSINeo@YBK2$S_|bg zl8&?xpltkx!wQa7n<4%7Z;+gvJJ6fitUtXc_iDaWlNPo^b<;9SZnO1Z)M7^tfs7su zn(}?HvPHILI=g&&<2Itsv;6CXZ?LyDDXj^1-r+sJbI;RRpF9>?8Z=?5MvvoNhoy-c zBh0iMWrvr4CKQx=U&hJ=p}cYCtnSiq`WT8lx3R&vqfS3t!QP`TnkgPT2nN44M9p$i zd0k>yITe9Y5VnJZ@`JoI5dFvMILurbYKtlw)Qph)@0WbAs_1h2`0|CRHC~mF6YVJs z5er0F{T7A>xw!x+4Z>T4n~E37BH=%2NvCB9ys}8j)Z60Z=b%iGGK$EM#FEAi)XcP( zB*lwvYy)o$-S=0i=7et3r~F@Ch&g4&&Tbh2{=P{N1&bZfS4PZ&+^y=o+_P!>LIpKu zT+UMJUtJQN-)$y5v4N{MV`D5+eY&m#yNpe*P;A~Rmj-oV#f)FACj{aOy zNSA4Mk}-^w`%ymPF=r?zTt#LK76%M{Sl~B0z5q@Akxc@ro(gFoy}V{!W2n;&)HQCB zdC&+Tz|@WEF<4`dw;&?{LbnBy0@2}51tg-Ht5)Y8zYZOdzccju*W(D3NuzL7nqs7+LN^PP>S0v zqhy`qgyuHV;Vu*5@@+&|PX8_X+;*-8z36i%45&2yQI{EEB^~^iea%%^Y*4DtNW%Ml zyukMty*uc$zYV+Z@*$j1wuQ}nIeo0^Gah~|V%ttvoWEv7DdIyj_Ths)pBqln`K35ik(rf~!YHRJN3l3pHbp3>U6f|N2 z;oDvp(jYMs9vI1kwH3@xzkAy~9s=y>*baPUN-_Tyw2Hjn&%5dX_9d<|TI6SpC^Z3A zW<~dA)7f;85vBDFL$wSGs+Oc5_ZhB3o!}Yc_1*Ef7U9O)QmQQDX(Gzm6nQ&5O?@gP zxFtO{_Si&^e5SbH1R{R!4w+YL?Bh5u>HG2?q|uUy=mE6Ws*O`^Q&sZt8_A=sknC4k zt&dSY3x_m_#pCR43|_W_f}gz;FvyoAc^?vF_ha^tu`l6#7=X8!?XI#!?xlPw7URVU zks=XSqBdH6aA;rs&&cmI*~jIZr;uL22;OvJINuTbTV*wjMC?$^;qw&MUu? z8$&|4brIpVf~D;&N*#|^CnWtAlFZ-!O*R3pew+FTYJUWz6-YbNwvH(mWBnfi@Wt&Ffft&s zTT!*m`}yX`F&OL3fCzFXk?hFmga-H!gQy`1W$e{M2$Lvbi&_M*l zXsJsUN>o%R>EM3MvmIZwT@DWUAMa-H<<$DwbEpn2XD;*GW2501pVv;mn-DcT{KX&W z-paaRZU4?wJ3gRu)gZalW|#W+Gn>_*nFWXT>J7w-Jmfs3Yu~ONG3zA6S`@i@upCiV zdvAGYaQfKMbF7(5Hsv=^R9toH@i5C{U-{X!W7TLBH)3aWg715{KEKJ7`1$iT>brX%Yn07{NgYp) zRjVIXSn=de0&^d2#=6>~{v-KbTV%d`=yDJpX^L3E7M$d|MMk}L@`Pi!Qk)=uVsF|F z+}ZUhdaV;gy^VqBic$tA1?}FRSf8b>29@6QnFrcidK>CB&eNa6iX_(F>)Ze6TAO(H zs!3ZMo7kcM^|apA0BrcAbNdW4(&yMa3=|4ALv@%~X)5!_TXewpRV8I`2vH7zaq3%My%Xc|W`vV^rp(jBiy8pR~}5i?Si!+MXpM zkphNejV|jk9!!B`)FHkhN00Y^bs2yBsro+HeX?)xoCw;6jdV`KuRUE38IxIAZR*xuahTaipu@gEu5Bl2?q5tY#VZTb>h$8K$m@*^+HU9yPhO^f0< zBx>{T$N=HELYLDGzAe#OyR{*GfCf)AP!^j;kcMZ3Xqo7;%6sQ zh#J*eW#p^!AzS0^26$$=6OyZ|pfZ~bd&RR={Hy93|D=7O%Fb=_Twp4|%$_}6;&cQz zDlee-7i0ibR!_@FK;2fZN3*oOOe-5~>_g2JHxXv{(pS5qBGgL)G-VF4bDS^LxM(;5 z50p6KtK;!6za3Q}`d;XQc7^tOqt|Z@wvT;~@gYV{YB z*$mHEMOmCQ+MmkPcEQ;~%q;psDqjRKj|!_uy<~G)xQ!P~%ByhLPNx3wy_5JN@98%B zb!-_q2Mr$%rL|FvmzRP@W}0W9R$$LccnKD*#m*@|R}Ar8reYx)77~+UFhtoE;~S#AFz8xOyS-dU_z&F4k;U<0 zXYBh}6Up5(a}$d@aKKy<6f&l`6lza=Yi$>;NKSw{%sg$qs~bNJuKnqX`N{q<>r?3F z?^B+3f7+a<-ie=$qL8=a1U7U7LcPTmWHd|`G zUyEJe!H|waSwT(&dBoCVnOO5Bv8l-Khy%tt^Z0S6Si=)%LD%nmx`L7V6Z+NFuH-4& zjOvt%_hYGSR>GXu;$OZ}CM#03(s zW=v5IP&Q=5ciMP(YLH8W7R?5lfEGXF7HBKypE-Fgpbi+_dmpC)c)D@ce{+Xas3s+; zQE`y;*QCp5o|6rPW3Qo9w3Xo_F)%H|u=UVOxPBIAL)M=kV5SR=`Bz_F8+9r^Pd#a* zIf*)Yvn?-2X0aS5PQ5=f0{>>~4*MOD*P+0Vi_`A^Xe7hja(fo3YFwkYsYPxWo^m-? zer0~Ijp<)E2Yocw4=9BoO_IoXxrlmVqp=4eoUw01;xJ$g_P>Cf=3aq6%aQ}foDKbY z#t!OS7E@nwPM_LnlH}U~KEC7qX6C?N*yp+(S(|=#ceylE%{S?6KHfA@ohN=~2FBG_ zy@KWB=%82f?9K;kJ0EMPd9o@%4Vdq&AL8}VC{~L14cZafb&by~U;x?swIg+n`{#@9 z1F%hfp&cW`BC6kig)|6vitvKqK5y`R;^05;8oAROhrzg3YuIS=a&&O4RiOKW`&nRT zx~$-RO=}glc`D?mcfWt{9}%=&_JnyrRz2gClYc)*$k*gH?HOk^YB51oQz9d7;pQQY zw_Z$u4ebdA{ldrV*d_hvHC$c+9TkyE{uh5f#yPh-1yrfbl{M?qRgD|>o!l!_yJ-7-LD>)IE-Y-IE)k(j@GZ29#?JA3-{6D78&)5w2%_A5#n?k-H>ym6Pm-BY_;WTY3nNtM9jQ zOSIJ;@Z^GxhLx$BdM^d`I-kXv2Va6W7`&n)eCP44sm%VBd31cf3`DVos(U|sJU7f^ z%GRfnLY96U%4C<}T&7Ywsd}O%W)@+vvi9baDqS_IM<#Qw#)Q)ILi-vdZDFX*@_XXt z*cY!HF1!`mA-WYp!>st-3TK8DlEUa9}VON zyr70v`(?;dN2d7-B30PTx4DOv%r6-Akcd=lxjw!G9=5IR&ps0C@E07+f&ffDe$>kS z(Js>pO$MB*Rl3WE^q!Vq*Tea~c;teHEh@nMjq}|Yt7by5%Y)z67ltiqcZsK9Ix)T8 z(;7H$Cwj3uMDhS%@A27X8Rawpu*LIipX^`@_qT@C+ayqA9_d zW`%_``gaS;Vxg($n%~}%_$q}gaf;D>)t*OrIPtgR3UO3K!wzy7k{>3nlw7lz$>IR- z&do$*CqGS}fL!|EM)U3Lo}EARhkt#;9vkldntKRBHSyaS}@y7{BP6m zkbc&)9%0zS_xJ7ib*Uji`$1OCC|^D*5e{)s%e9gE>ck&oF{nLu*0H94piH8m=0(G& zm-Gl8m|Ta-fV#c?U*AAb8VW2>e*!LkG3&IYFI};?8SPG@q*aXIL)l!G0jke!mM70``3v85mVB%*JH@{3%Z(UX=2cWYdtJEK=i26mhXvqp zcf99Su`31Ud%A%zfr^oDT11f0i3SD~0eO4c|FUnl^v>g;;+5?8Z%X)BN?E9%*%)I?Uk4NZNP135OjWg5O=ybefw7r2~noBlD24q}XN{~2%RsMMrS6yZhk4Z*=!M;G& zV*EaI^ZGdKrb72+)6w_i3hLX-qx{=s%+jjT_-0L$g?7mwjx`99x}PY`iY)@ToHDsL z&#%UYliexm5}{@0rfNyqmjBeGt+nrx6}+sm+|Qu2757h7k~Xb7jJcj*@dd2rg;wMB zmg>OsLFB0;q7taWrcTbYOr|jYpYBFn2z4p zR5N3z*xg24K^C^t417N{Q!2W9-K2XYw|Co%z}tcdFFWek9*k>xB5N_*cY>)dpJf+Q z(0@QZ()j#KHQl`-D(?6b3P_Xt&t_RE(5bPn3S%1(Tz&Vv9okc-x7QxhETGbXpP z%C#jT4H^?X?ky%v_4!M+10j_qXQ)R`IH`Nc5|8!Y`u2N6A;*6Kr9G8O#n< zej`n2tyufA?2a+&w3kBS{*iUr6v?_;U1RHqSIS??*Ygeqc&52!um4Pu?D~>nnw-3b0 z?z1c9GxREFrZNmB!2V35whIE3$6CV1J>}`}3y_HD$GN}wJqM!@peE%}*WfUzpTK=9 z-_0QqOvC5l)!EluHUeJs@r-GLc|}oc1;3kCCkE1BycVCj{zz%1WyDqj5To;{CKkrj zL2if!wpvK75#E19LV$&Tx~n60=QQ#^ouo1wx|^5{j;Zx=^B*<@$B^MfD?BQ22R$=2 zcH)3PTNjTorS?ewJ<}N!Lp5^ugc;?4mYa}L*^;EW-&ed=pKri?FYdR>Ub+Y& zA&_gT%fv!+~OB( zAg^U$mJ$Aje78osH&PX~BSUA~rqfT^cknmYB~ajB2nv=!K})C?5kVKq+_wR)OiiBD z#}PntXDq(aR+CXJjaleD0$cY;Ifco_SV~XtWkpW!3cgld= zLhMiqobzmuJ%H_Jq&1WY|`Y^(q=vR<9vhsZB zlSiqT1~8@+ZgkEB4<-QisFPzG4KMxEDqdp&y=j*L(yl2mMqtvbgOoR2wqA}8ZI>7x zXL>*Y%0N9m8B{~Lr9R@=YN@B?lKg|!Tq@PYBi1v2b#1S0m^3X+Ucxe-(Fijd_ltg| z(TlQ5mEUlc!f?}rEr^)$Y(c`b{Gw_7^XP;$GxVd zaU#k;a%*GUX-{;8Y-nT<<5+w{WAi$7c#(E6MZ6g>K};=)0nMpuR-Ro zW=U&Aipw8mgzd=1YG$-D;u4>WE?#^pKy{hy-#Ugk%i@BVt<8ot#{@IPYExyBz~X++ z!lqa`d^h!lj)W)|#9a$Z&{_3G%~d}B>Dz%~8zvb*h>>Z6;IRNPv5xeUgR>b z_ua%GXY=;a?R@1e{yV~)0|OrfGN78&kEMD3k!AP!GGJEsn{_U$avO_*Iy8^j5De(L zh3ix%=Vr)z>2VEqTpKIo6cOJq1zh_Q7s(T@?5!Ey8U#vn3p72oxyhL_&1giC6>58E zNet-x&clv-MQfdA^;r&q>5ey~Qua%@qn>a~5Eh)VHH$)AMw7j0Sw#(lvOiKdm#FTq zEWu459Wj~gMk^2+&sNaUu<()Tb%n&&m{1JBVshbx9E&#d=5+z)wR(d7GC*{7lMZZ* z6&-%xPh~O6d9|rMjbiVDXnFxr&1$6M>~XnKd`ya`xM{iw={IgdaTz|$By<7#V_Ph$ zf@}YN{ZSH%_L|8;>W@&!AUZ`tUwD>?D zPoV8|S7&T!Q)m)d`PR~o>h&wD6vkUF4wx-8>%)eIS2Gr+`aaC|*i%gb&J@|Di|y;& z)MYamHE}~p?4wd>LHAVad!?!ILvONx;%YMIqRWufdYaT!sZ!tlq-XeLB-;(Zif^ck zJ}fYgR?+rlHyLZlA9u57eL8{$D=MTab8M|0b^C@lnoj6m3;n{b_t}!H$;FU{Lnds{ zjMSj>ptc%~+3jAl>@80(uvyD(GzDNV)(m40yYQ7$TaiTY3O=-*V8Lx;KOGQ&5aV}1 zn)Ub(8W2xyL6nHzfd70@qFy*dB6yv=N-9%vG#g8cLN`2AJR&P~*kemhyZS=^LzHC) z|KbvL${A!d?Gf>yCmtacVzEnD=e_hh@H_GC2qUdM&l$aB%y$HfRC&*av>o;GZgm^p zINkdTBY`0aCn3I8PHh;+f;Z*tjZ7%83KrISrNOqTU@b?P!f{Ji45&5y4qU=7Eee%0 zZ&e&5KB2phe}|l^-Tu`EdZx>3s{wb%inTIqL$^`;Slr!2AC`FV8~S9u_{reTdUm}# z7BUSfb-k*jV461a+YnG>&;Vr{7V`e~Htte(OlTp*7Bn0USSRp>I3%v5*+_H~LHLZ_ z)yVyBQC9fIP(;8jtu;gYxa5E!;dZ?z^V|(O&4saG!%u+CghIWIN3AFqWw5#gwN+>PCN5 z>?6D0}qDB|CclO#MTJPZuqw?%rS`du%i)wJx>xB2n_BLVyE3#v;p@%Pg#+`xU%GMjH&pi!p9ybV zqjlHXUY&1#^sG8{^HekI=uh&A+Sp>rJR(b2OK_@40IxO0J!ZqQ#J{i?V&xp-Q!DB` z(qNN^S^mUrQOSI6QbPzntE zMO*nIH&f|bnfZ?^6!R^c!rlKVJBQR5|kGVxkz z>`$*_)g!UE|J!IMmsGTMD&*0v+}a53-6SuV>zeD~%2r6`U2+38-Hi7|TCea%+l&1cPo^!YClrMFFoe-K5dw;AZa%96`vURUO9^;Z)`?5^%#It;@Nk5K5 z<>@CPA{-*H;DfzqlP+r;J`$5P)Hd`*f&R-44E~Yh1`EtXZUh}lS%?7W8YK~j%m~Gi z&4hAHjJ&~iVGjxs>e`lE(#gU4fV+9uv&=3JNnh~M$h|1hK|6x4SXjeYAe(tycOu1S z^_H5z4N5`mO6GznQu!8AF*Q|)Cd}$nk=`pLrlUJ>LkZ98nt5d?j1c*liW}K1``zC0Lgyq#eK042@_R(=eTN$9+=~q%&nz+F%^Q}`BV?E|htUNGhJd(f!3zoA{T@4B*IpDll*H8K*;@){!ISXxv{8PRu_)$v;0_{ymxRIFu97Dd&BDXdF@;Pt#pj+XwUw`4MK^~AzMHC4!n$~in z^UMASjL2jZuHB&hz2%xj+C1{zRCr8=*kWQFelz@;&Lt*qX@Q^Vu}rAfB794ZqPQ?Y z&d(>Y_ERsfXlDB>4mb;A zA|UXp@EZ!K`%E}3gx56hIg7AVkPvzECI1o*#8|}$g>1FH&|jmfDEd+mI?BKx0fLiaWgI7^kJI!xnNS2Ffv~orlfB2n%i+Bwzs|*MW!)Q{H?ng!wj(I0W(qLQHld`<5O-D%0h%Am^ zCkdC`8zi@8W? z!#pd!QQuTe%HeNv>)!bGa6kG-RW!Lpaq_oMeI`r=ZyXXi78Sv7=1B89c@gGBzm}0V z`^j`5CpGo@4|D5>d)s=b#(a3PplR=lwP=oPtp{X(gop}c_!rfZHg%B}t95FyAii|D zGL1$Cn|1e^=#KhEEe%KA40y$qEmAb6=w7_fGIY+1d888o#AxfwNv?%pr1o=rMqs8t z#;ehF+HIX_8JOun!z^rW_|t$!(%+V~TiORVs}k?;&&u*fhnr-<$3q_z@^^i_BzMhj zIatdsC<|74ewnCaOzIFj+m%4StQaF|k$TLYmuxry(Xy#3`E*VB<-HnWVn#aG zE>p2T@lYzdi!NaP?xPmbsN zk(=SZ0u3Q@|IU&0RF>}wKR3B*Q@k1_ZD8FQM444-MhF|cA+GND&XBJzZIC<5qg^9}QSTB-Izf55|}CoKtmfb9>) z^dgLC_&9rVeA8tOiHKwKtb4(&@IIbG)6=tBQD*$2IQVJG-!|F#j3HR@bA4hfFQvW!DF3_pufPI&bvzM(yJJ3}f8)$uSWA!ss&V zDgK2b*fwzEw!C$(Qe^yD~8xL zQ$`_7N=^l9EEtXBTUEvcc1)b@?Z~8t$))nk103Nkf_?<53@8Jn?41(5VJi42PqvVm zM5;Mnm$nK|M?N30@!)OcQzW`IIwO-fGlNkXaO!GDbr` z(sAn{e_Z5wx4jM2`!zjhmm*&~Is#88Ym!ErOsmVkT|Ci)WLyB2QhjLWu?EG!p;d>w zmcYzscnbRbq_3l$ACEx3n*A$U+aI$!@qq@8@WT*l;lu3rZ7t@%u`l-LXp8JX<)$Or zf9|Y##VV7`QT7Mb9^s2WP^m`h{eDq*lYHIar&+Gy4z;#ayUWNz-Jt@DjV>f@IXY%Gcr5ZYa5LE$#y8Gr`aLi+Wv2lbWaZSG0=#IA|m_C@dSuba=c zEHju580aR#JN}`)sjuoUvF7X|j89CHlPq+B!wN_`iISY9q1d(^wkcj z*>yy`vhit#@~f3V8<4kMX?!abtmxUO7%cs_6yJ`b-4wGM;HL`dBW@0X75F#o zn?|@L-iDUHQPC$%x|LMr%^4*_GWN`y`c>ZUwTsB(nJmWmoFiY*BW(~p8|AOT zMH8|{(TillulIiDdwcyssl-bP5~$Hq)TBPc+wCjA59MjX#4OM;Wp}pvLFc-wx352V z%e?~o;}0(Z-6lLlX8&*HvCn#oNxR=0lqKudgX^{mhVnUrEyAO}+nZ%Kv`-_7j7|Vi zF;+0nb0#?;Gvfa6Hnv_r_y+PBcq4q+Jlat+ zr<;IUA0SMES%l3L`Qz;HgW&z>T{uu{$y8 zbTgJ|EuwSu9x3t^l(Y7;w!O;$@07|{(-ln=LKERojcKC^oh7YRk@Yge=_=s5GU5|I zF2x)t!19u$={5xn&wIo+sPlX53I7Gr0OIRW-%&GrQ)wfH?&2cbX<3O6?1)hvYGnlj z$LVJiqyq?5_FsTcjCF9DcU^RQ1ri|fw@(6nSZ;HU@}D--m+rdG#J-RZ!o-0v8P4yX zRI1uwtbCwE9ukL))sQhy*f~eH(iy!cP!I#*7y^(tiW!3^E3#&yqrCSLR)++SO`cPV zYMx8fQ@!N?iDo7*Tx+3t&h&PB;4Kbxn`F*3hP?#)u&IGP1rKfkMD`5(>_7@n<%0)oI46|D5t(2%hU_4#>>FY!^QtF|C;YDTMaGl_1-+C8mGGk z+oEes|IfFko~2dgu1SP+%RtWUUf@P%R2d*ZZl*iiP;3W#_f|jm2&(k$ke`}a$bUawWES)!~V+?ku{yu^Y_E0Cxt3hac^jR3eP0~VB z?wd{0EgcDvl?oWyt#FtM&8ZL7&XZ^-MV*=`L<|eVFQca{rU>2(0QzLySX?t z_(sIzy{IkVccR?j`_>sxemJ+RYIUCxQRFqxynz@J=+#N(F$<-GS&m2=$uQu0t1;1Q zWnZ8Z@sBAc7fXpGiTronL>o8TW3PxHhl{X4%8FYv0sSEt4zaN`dmCi9moj~s9k;!N zZ9muXowp69D+wObI!rlp6{a)_&rE@QcX|4VNB$I6V4%vr?HFrhfgGVLg@kXZOY90p z9*}HoL!df?eZtX(R?IqBU0WO27ly{bX`W?+Y{VE;)|EUuQe<7)swY^+03DrT%o40$ zyW1zjACLD*&nvFW^2`#$JaSDS@!>J$Acm6aTf8>nUF6kDHH zL(?LB)zlM|iN6O+EHwaj=@zvu33c8lThbkkt(Le;Rfivy2hqo- z^ug`Q*x@b%p{H)i7}P}AoOG)F6pf>z--rSz^Pr(0!KQ#BckM9sJ;qwXpaA5se}Fh4 z%r)(w8*7E$lU`$#h`hwX#U<1nF!;bNoywGrw63dHuh8W9*Ol9_qHUg$n%Om~zi=xs zvGyE#nmw}Qj7iRDe^IkhzpDO;ENf`=`vHrtFVuWLd(cJBmL3QLA?kuG00%$90i!|J zc8|d~w9hjs=-u+=IB96N`h>DV(EpUo@axTl{2Y+9;;Tpm=XU{` ze0$}+Yl}v+H+mPmNqx#DuzNk0YP~ahjfGQ_?L%}@bg%d#8Mqcb32OvG3MnIha9%2~ zt3o!?KR%CS)_h~C4kK6-|F}axBOu!FMAOY#4#bC7bExPp)hV4^5Weg)yxYET$FPGw zdQ<%#l}=CL^`ZBN@^CqMiDD+t1JfN(@1*YY0Oe<1^edx0VtPs_lV1=`B76+=@vdes z3iRkD-@mzA-EI!DJbK1#hkPn!sS->55-0fvFGXF#J8P}V3OZo@2c~f?lT)&w!z^wu z4>V!|Kh*!J4J&!uBd>|_#2T=T*n^%!hC!V3Oo(Y~Pa!vghe8**lEyg8(8?PpA=7>Q z5pKm6c${8>D;#RyUAYQ>4-=l851mOqav=2ve~(Q9g-?QYS#WA9fsdpmL%8Zj&Q0TJ zrdGTzetdwLSroJ-0iKnoe*7i`czcb>sxh)rRU_t4{6JuMkDayMZcoVLolgPzo4(kK zw+g*{#wJ(4B-b3@P>vqFj@(L^CIk(iAn9VS&2n_sadixBjjy`ZAxbT6#;Gu3)Qn`H zFviVAprOkk&>{A}Gg9gyVKh+Qc1i?9Ptx!e`|FZyYlLsBiq{KG%c^Pb+8TH&Q!jru zttR4X$3fGW&wF8+z1W>YwRwSr&5v6vKRrDy0e$Vzfjebm6mFH9Qw)r?oNQUc1)?qHhE z+?$;BJ*#@E{`0f7s=Fcy~azN8?514^A(BbFtj8{};p_`WYnJW8M!*wFVs?hwu}(;q`Zh>=tm zntF`AhF4B|?lHOk{)WoQ-VgVzH}V|#9bUmbmar#JbG|qi1LRG^U<|di2;_e*Ii~p? z5EiWC{5RgN%atpr$zkr5Uqs8swEdCxAl4$scNR9p63s{*e_WoM{M!P}ua>fweV$)I zr2wW@pi-qTyGB`y(a6Ild}F<%pUsK!X4j&-|(Qp3?nI;C-QnYd{ zCG+Nz*qyc2J$T0*$C%q5IVEiAbxc0dPSs0bj7Q>j&M&ok}kp(pE8$Z>Jf#4Jv2el1g+xO)+QQ?&o4Iu37?6zix$RI$O213!2jp1JB z^urzaZ3pSLS4$;BgOtb5zBo`@TR%j0`_aLY)X@1aU#nwRi+xQ|-~%S{_~rnnCz+S^ zdh$3m(3Pjt zrh)WWm2KkxwE*zl5v#B`!FeYxu~;NaoLla`JO^AhRj_o49APE*YU~*_bxoo2sGY_{ zHeEKGI(-X&nx=hRIMVuiajycj~Br#(aZ=@UlpRQ-5SetI z<9%+8xx3=722}W5$2<``PKZ+9g;y5!lx$xiXx0Zx7o*Q5i)1gCLm&t>AwAK{cTx>M zR~T`Z;w7g^1UR2=j7+p*b@laJzZ1P&k8=1vF50-97R_rBhf_4&ZZydGjItx)wXf5N z()jnAP_~zb;vYHi(A*o-Abi%xL(yxW6QP(X^-J@SN$6bmmbG0g`v$PJ(MBf_%x2(L zh@hiC`K-Hz$qT08w`IMFwSU=S2(*t@+b#2gQUQ4N)N<$O@d}Jzbs)%!v2*aF=dDn#MP$5K! z|0X@aqBDxND=NN6Dq@6$T8U|f0ROzuuhqDs@3_5Qg&+%pCq-j53aYO|x6G+1g*in> zU^zaNfT&Z`}b8xf?51I0g!?QYG1)oxp0<=6kgMrhtffbD}-_DC{eTP!t_hkLuhhj|XFS z9LhE@rVqjq&u)n#*X0IP zy9dt`zyQYcwq(u6Ot1uLM3SHW;V1|VNp1yKjCPVrH{PAKN)+Ot&!#;a%aDYK7y_wN zfs0Bi5PowwwLAQa5`A-L3vqsh6bIXKRVVa_nZzJHop7GJ^@T*hXLGUI|6*r_O3+Y#CIO{e z@FL#JIiQ5=+x5yfBOr2l1M26cs=qDCc~>r@BhmTo=lrhHJ-gktOu~Kd2Z&CXL$F(O}x{fEhu18+Hw_nP;G;5@t+QS`O7@Y zos40pl#zGMWX>aexV1KwBYqmh#3Ov1>t=}(b*WEyoq8ePjMKqwxKiO~8}q*wyJ z4l7PKlRnR4k@=oozhTVGaOz$%NfzgDl1YD-ud&Vt+!i@lQalYN-VHuMUyeXJ}+F=CyRJitzqN$Gy#Mo9zh5ZeLzK-5tj#D>!sD)kO64r0_|w2 z;cC=!nBA@LCLA)P5E0o*_qL&6RNbXu#@ww1B&msXLwdOghE<6TZBm;#7alRt7F+e% zDK+0e~|LrX@G+49qdA>Ng?C8NduS?~Gx01^COM3G`dV-d| zeoxxpKhwpW)txQb0vXQ@raw&`nyEUmA0es{=f4wz0+ArbVPOQDhd!-9CVek4I*Fkx zJzVTziu2++vfp|A(+M|!(pu;6>^U1;bn7OnoglJ!BN!D*nTbkUMkIjIEo5SPdj$XY z{tK!GYH!7*?Z+0jDsx3=})(X9P_Z4Qb6byo>ou>6tKV%60nn z-h$7Th=HCGTI0P6hlhsv@sH0f%k7Ni+uo~OYti%F&w#dc+a2CtN%;M=ziGyV%Pf93 zEVm*$vMDIK5u%!ZXOh>GeV5smjs&hI&AEoOmuy}b$Dt^Q8)06w%l4GkWGpD7l_h-v z9f_wRcHcABFPqEI6uBgg+d20=`lxK^0N9%uq zgv}9VTV zc9I(b{G{s!abHi~BO0$s|57b;h-M)&Wa%6U0K$`%H%~irin{Hzpm{upXP$^k zTV@xN8jR>f_KD~nQF)hM|9uWdKk{ely7td?PHCmolFiRA^*DkVZNKlcy zjER+q^Yi6Pg!I-G4e2*_7-AmJiH7F;l~W__1Fhi85L6HIYI(BQ+S|=@1ER{`B8)ED zvt=y8x3<-sWxI{|Z(M&6cJ5Rau9$z19ln`Kz5pC-a5rG+1kfNPa=pN0UriGObxR+05+K1PFE z%^!PCO#&{uQ~Ji3p5luIg#$+fJ&XZJG4^n#Sk(x0DPZAvSI20Tl1w;shx5fQlLtp> zvxUx<$>RH&7k`n*hR21p7zy^RK$&`QTZemmvgt5jfoK{6>LXL=sO|KX^3{&W9%Eb%JKjuW1BY-i`&#d&bid{^o(K zwv8Y>1gD?OXnbQ?&s%;9Uw%-Mr~)7bFdk59A#IKGp%;;lvgaqO?w^iY`S?Rs2-`%% zsbajOP=awv^q9_q5h582EaPLp#Mv4EI4t<&qb@`t7PUkNdCz5v+s|vB+$gy)DCs5k zpJE6tAH=8oJ=ZxG^>1BW1j1@tgXmjBm{TXJH43;m1@;i+)1Z1Ofmj#%$+hH(f_oXf zq=+men8|LHm^5?>{h%@^15X7S9{AZtq0W#U5=kBuxP`ose?L-@!p)--*5I<3i@x$w zSF9LG?LepLBxV;b>Uyff>=TVvr{lJ3djD4i9Aem3?V@H#NRy<${WFm#l-FFvJ6T9m zOl3SgOJ-1w%-Di`1w-}$tSl1>7 zj~qH8VK-lYf5qRh=keK%7rbAvdBD4t0+n%iUS+n}4-udy$Xq{e5Bu-rMmOSsv97gN zUkP{5CID`E<10KwokJeSyr@eDZ|9VgV>quI-HGfV68@1(Ryr}m`&diS`w3sNpBIAa zCM!&vHG``!uX|OOf0hnSEv-M}Fu%X23;t{lTUZaD-KRCgpCkF}A7b#>c#&4VH6d>| zENhIFhn`v~yDr4RM$Gv-Dr3?%6&y@~1;cnCHA69osFEf{2x&}|!USr@k8AcK1AZ9G z$)1xitZF{CM{2O?2ig4QbJ-q8(?_8^g&O41n>boX3NC=8)7oLHiG-fudoSrM2Lho@ zw~)Z^L`O#$?kG!GQD7eK;K$8sEfnjux_)8ETuDZ7pL@ zi6kvlfS%A6l>|H$XA9=uLRH|}VUS>g-R0RCacJN6QtSOJwX5UBpp2?;Ia2!~2eQJ>h@!M}m1=ZqXh*im0;7FdFW#oXc zW2p>Q@6Tp$6?r_z6fz=+J~2PP9dCUJ3(PF!JVccL`C~Buqs7l_we>i6(ZXu3acb#e zur!~Z6_S1(u~m|=Iqd&vIt#8S+qMf2GjumYNs1sL4bqL2bl1?`-Q5yW(%ndRm(txG z(j5v22)y^RzV-ctS#w{t&%KXri|m*eg87}!%xwM2v%_+qcZU4vFM8nCHb4@ zw3OBe9eS7Xk9i({i>bnmkx#W|KfitIuoMh-8P87@yKZg}+y&8EuOLRclOQj=kx|K5 zsIT8-oYYecnBxprv%%sG9j$SQ{ov#^o$=6dSg*kWL-ZVO$xH5 zwPU6Zl>9>0%IedQU?%!qXjrD)yL|J(cl>7b@m0nI7Xsn&tUyG~d*xHmPIp)QBeWv^ zOyIWc(u`_QOvZA)DIaysikp7(;qN?>uBwwc4A7=ziG|3)?-HKKhL^PS%NhHa#>B7_ z=(E2MrO*wv(MPLrH7VmW1TpAa%D!gXKi!{8&qpiqDz^%Nu5j&^{_JglckNQwWMDQe zrvO6s(?Z{AbD&4ZK75s`a=P2q*snuIdDQ|8^T3dSG_JsLrTNo)rS=91le(n*ioxGl zjI=N!viBTl5E*`AMBC@zgc%N26(TGkK>i8A@~n4of_)@9ItbE!cCpI>6?$*~x~4|N z!cuB07Td)3uwSL?hWg>2l~R<6d#Zc*~N4&{7+?2@y=u&JMT~fPpq@T-jdZ z9@P5tfA3=w`dhh#>gZv*UEZG5vtHg zEtV)j7p-cN$JQ63CTUPE;@fZ;{FsM0{{@hsAI$liKD@_Ar6-K>Tp2TPT7j?ne&MID zW?w-+)Zwg(@D~U6cRnFQ9j)Lk)*EQvBd&@xp?J{D3XV4*wEg>q{gXe2-cS>fqYTr@ z(LJ#55RoBoG0O{X<}1em!u$)}4gCO2am89}ytri;MXr0%n%Tu?!yaO;3WK6R5tr&) zI#_WM9LEfF+&(>TRh0F6_@a|QBxSfO%@gZVWD~tbM9ihlo+7`KG`*n(&8{#D?xs)p zADD>~V+#?)MR^^KtzeMeHDdJa(|GcIGQ(6$Zh*diqC54o(`T0>>G7!X*tu}&lMnAJ zgcLMbSW~+w+)*t~Np|LnAyWqO84A!wyz>+YY=Z?mC^vhcA7xOAz7g+bjV)@Drk!53 zwqxW@SxWo&khY()t5UO+?TKD3{uvUJ7{epwv~XEP1~lfy9<=bNmwZ|XDni9siLlm$W6<%c!302FpAN zA&t*e<`33ppM!&+sJ66buk{J+7&^uM`U!~$F6CMo_3~!b$#~20hxL_HuTqd^Q!sQ- zwwJU;CaG^5e_z|&XX4M_2@ztgfW?T{S}Y_KM@zeA)`c|L(Bj6IQdnN9cwz<4TWQX0_u9T~m&bPV7ceuu)IbDf0Bh@FmhpwF2oi}F~@0j^jqxJa00niu{5zuDX zk0}yT3OWoTJfZtHSAg%JSN_pu$<1k_X=&%{&>J`o;(rmojoO(a_N;Jw)=xzt>-#{n z->({G6@`viwe8S!`P;PW9)QKpv#U6As4h{2(oYjlzbib36e7>NYE{L}^t@CuwwJl7E;5^2Y8s+gJaMI8N#$%goXduP;1;D-@0c^dU z!w3-MWXnL>s5-EgoSH=H$iFydyBszZ#GGKbbf?Gq;YLkDlP{UwrMeeB0ZYvd$(59f z;Wo$S^eb~X`)N2R+#+IEp>MxEAL9o*pT&8x1*g6L&j>-A0eWIqMsw$GV_2zL(%aev zLz+^585hStEK`etSy!q`32lFKR$E_F2*+hw-ynrU(P@)t6+K@-c6uW_r6x+ku-{^Kou}E3%BLqS zR?6_;qj5bKCbJKQGL<~=Q7V$V9Ce2?p)5AgTNmAOHOI@Y*Jv@S@)6%^3okde@@&K% zsivUYs@v<$Ol?@Eo9XUSSH~}=i%x~pgn7SAWk>KQvI|~D+EaU^cuXY$4f$pqBsSL= z#2GC@c@IbL=S_koByDrcLVDRiA(Qf#=9T?@XYG@c>ar~T$ubh4J|y~GjLB)T`^are z2eEnXu~OCqx`83c?fHHWH_VAPxS5lXTKlw|fdA(|fz zoJr#0OPxN&$#}QP-2uM!D9?XMyR;lAd#HvWZvb|`_@&r#c-#ynW_VsLX?MH|omU2% zR@(;Pe!I?uwVdH75QDpryd#^b)8_EC)?Xy3-N+taj+c~bgt=qrzSkpMW<>z$yv7MO ziaf`%z+0asC)gI6B1X@Ked%QRMs14DofaaGJ;6NioLtz-KRfSNWGQ>N`u8DgXrxB~ z1-aMonvIWN#V!P%=xWl-(fGr%af_ti|2)51}*yyv_OuKv;+R=D1s**s0PpVb@~lz;(XG>YDnK z_?;Ljj_Cq}T-$^4?XLiq^zkf71NeJM7I-4gH0rp7k$uy}(9MLOkWuL0jj&p^_nDbh zQyY_QBfdknh&wT32g`Q{K9-jb0MnAQZMaYEl9-puCo|*kZjibeVz0_jh4feoUORbh zHa3IilDNSKsiq&{cJxC`Ht6zE>XhIn$H;8ql}YKKb?CVC0CKYo&+uTb?cU`Y?cCAI z%2v4xK85)89d&;b_RE}5fjxYob)W~YKH7?`?ATp-nVY=s;!oMhY+-3P1+u96GE;x& zJTA*}CJcB=fgqX~X^;~0%xCRQMvN;1+vzGca)=(~QeRp!@%%TtfNEgh{&^dWllUad zcg-HdID6m5ak#h1Dxr-7AB{5kkbjtu!lgONAkB5UIFMDt5}@`%}&a(PEWe7V3U z@vUcZ&)2Ok3hY3i5VY4Fl#}06_2C4aNnpD?$}p zdXm?cIMmosU#GyT9)NB~TB_96l$euQAEdG2`c-trVgnZj2d9LCf-FbdR7*t^1BULQ0&#H_)RTtuUF^qb;uhrH#$Erk*%4ik1|;N6T|ILHJ8`)9}7W z&eg~xH^ve>-?1@pW|3Y{L$MtbuQ&4X^OGQB0wp000=mx5K5_iSPHc7LU1Vs<^9Xnee(b4v1& z>%Q&eJlo>dn>@k^YSC^1Q^WAY@Hi^@f({tv zrTHx}E&2yAFD)hX(F)PN@7AU=H<*v76cBxOF`^`q;2oOW5xE6bw4yp2A~&nPu61EI z*6%5-Jegv3Dc-R@U0Up%xxmz!ZdEe?CAQ4wXnSzRf0Fps^iq9z+N^c}KXx!JG#JI;j*j48}=Np$u+ z9Ueh;TMo{O*sQR-K|X1|+UV2@9(C&x^m6agx>a2a+Y?Uv{m`F>A~VT-vtvh}=iPg` z@fg149|H8Ql7(BNV@9k7i(CjD)fpMlvP@Yn=aB;&!j9NfmqO4^6Xk@i>;mR6WjG zi!-Js z=qw}c2FDJA6-bcotZ%F1t%Htgc{*(H=Pi6Zl>G{2$Q;>{9*8>f*fwSM)`|wo(ByvpfHo4}* z04v7mDkdbdx;POrvmxkBA=W(?4wL{fAhw-wJQ(nEOR7$q z1ex{PM-MXgo0Q$}$S17nHUXKwmsbeMY89zR~7dkn%kXtEK) z@VVJU;~14~fEA`qC_g8GFUhKa*KAh`s)p}swxdA~PVPB~P7<6> zDy)k$-&ztp?Ighd^9D{0rYn>QXav2Z=sOz74OY~vk%=b8t6rNETw}x>h`AK76E0Op zLNb+KT`e&?L=o1cw>&sui%jXuN47AEyJC%FuQxj9ggV)Tg!do%d5!bxy}f(`EdVb0 z7B@L6to31Zr1q=EXMZOG)}V`T^u0Q-)!uwQmG4M+rynXm-*_#$qj@4Ibrwb%bG8YH zwLp+CVb3pa08HsBav)%OXjR-M?Pbym@KKFNxA7-DMbPr?Gx0|C6h0ZSWfu6#5f3b3 z%si=m;9@zV)C6l!mc4RvKz~Rv3B2ur%%t3F-WQJkCy!K~yS=W-tQgqr3=ul+@cga8tNN09 zO02sQpoG*I*^`7BME0F)h8$<#!ZMkyC?i!7z8NpDn&UL^DQmUjuhfO}j(OU5njG2! z19&=UmI>xIT+>qGbg+7Jncx^K^O1=`zA4G-(Dd( zx*Ic=@k_T=Hlq#v#ulE=SFRtKAJ}BshjfHa`u|=4deW;!0ib6T7FtA9^OZh2T7-5b zjyH)Bu39-%zNAuaLjB%q0}^UIy`ed-=Ym< zD%*%^IeR-5>rTPxO@W9vJ`Hu#x8yJc{$Nhc52zu%An>W-| zoCxv?7HD%JXMg?q_om&s)t}7y3nYP+4D_?!7!!d~U5SqcN$ES~M=4qLhwrWn5lBbC z3poHq?(+idp8#o}bIUF$MW3s#sI++sSDmw{rJ#xE6?2a<3F zS6AX`V3zgTC$#AQVRG~3uUN%cZ+GrW#J=HdeXEmFp|c!9Fxp?nmkZIe&$%vn^=(g- zA^6$OalFW>VAdp~-zJ%9{+Wo*TQff%bBlsR3HRJDj`OZd5aDKdx3h1$e*K>DwMhcm zf9}=TTv|UoX#pBu7aFnMsI5JSex$D(g#I4%I;%O8=4wZE&UJ7AoZsghvtLeo zo$&$)e3G}#?>o(~NCWDFW?xmr_DfLV*4*I(CXg;2L(j1{qmeFg3a<2uX&f93M8K*$ z=8=wq5*|%xJ^O{8b~q`dj3c3`2t~L*&cUtr@0f}$ysDzZVzC^`{4!~M^J3`PGLK5W zh4gdsvVF3!RA8QE>8s{WFUoprDpH+OoN%aEJO4li!PibYO@Tx5YZJ&L?Qu8Q3;c*) zVicT7)@*jjm+DJ%SA?#lfC}2&VI8CE-^ZSF$PB+PLTa!E^1rp_OpO!{@ip{KDzq!T z=S-L&UNp?K3E)}>H7ZgQRNr8g17q}K%fw)Qo=oTCx&HW}?JY>ep&aQxh2_!kltw3d zyK_DWXtF+$+!oCFzE)yGeL4sK4#vo?LBAw$6zeK*Kf4M)lQE+7Zqo`S5 zu`1{`_IA%}j$AkBpS$SC5^E=uItG0OZ8Im1`h9`vyD(jtx(qA`8ea0UH~Dx| z9-B_~9PtE-vo17GZG32$!DuK71Nx4Af;qVLw)d+?5q^J4ykSqoKiWmE)I_ENKWqyh z?Dj{Y(UUV60cDoIARS}022myV-A$#&P7Lqb2E$EgqgN7BNp)|VD{j%Pl=o9SY|r|0wWTHrPd!|9 zFGHtzmmMz~Mak>FsvLqApSHe;!fl20IAY5Qld38<^l41rtjmSyX;?&}W9+d*lzIBS zvW8}6XX;WvX`WY{FMfw{1a8!_0j51`m5hKX0rX7v=02UkGEypQX+Ta0FAa}mk+A%1 zSWI&{DD@Iq5RxIh=eHpKyziFr_fM2;Jm(NZBs%yXdT6mLTmH_mKBxL3qE?};2s~Ii zj;w8dF2QE!MXPOzJ#oBB6ti-(fDXZyc#t|$yGfoa&-ySKB^;)|Yiv)16W@2OWmo-A zACa^5clGFJk`h33abEneG{nRE#mc!L);cjb+kcny5$ldY^?+t%Sb_Z^+d#T)pnkFw zvbpbQq3=3V9DV`G4-JR@a8;G3wX3AvsXg%A`g4+r_2$h-7X}GF z5bdd{Bt7tYAvYpgmr6_ssL3*>~Hw5 zpVnnk&2N6lw)CTjsR60TbBdeo%QIa`w-M@vC{baA6e|g0^b(<}kQgte)MnT(1d>bf zAV&^~M%?1j;MOTNZ(|9U;}~M8OKASx0?9a%yC-Hf>)i3kBwNI&<+{X2Vr~`u{TFe+ z3TA}bE0ZW^=le1Jv!`PxN3=k`gyAB{R9nyo+;vncpiKbT;^zvvi5v$5UlXTRu*-VT zYcdv~-My^PnRnYDE`E{ZYP+;GehxtyCdbQu9MW<8x>XCE-Dn)erIpnKvpDG+_k)>C-W!R6EZwgIdS*^EEVAD^s5knoPm_- za1v;KNR#z>i=?mlBVRT~STeWEN_Ru?C?#d2GmD`x1zpa(1FdP)vh2XmmF-ZY^0WKv zr;Ac&wJ|zLbWw;RZx*GqYZP~*x;M#ZC^p+@%-$COiS#b!O#iUhUn+Zw@3XCU$VpO^3T#UJ;h`Ed65-7^K- z+xMAl%Ww5p9MO@b16Rm)r=BPznP%2!CEBJI_V+BWwY!$?9@2caiTe4V`^ zfxtE}^Vb8T@ue2QXEPj#5X{H+K9L;^!(XaW2&5^kxNQ(8GXqW#)LBGJAB_nyn@V7q z_~aqfOKz)@syRA+Sbi<2%2qCOuxkG(guudLQ`qGY5NGXVhDKn{R#8qcU*B&}OI+8v z>Lvj~FIe`uO1!4_i6_Os`t>~afzxb$WM{U-M7w!!q7GNwD z@|m*0u@Cdym|5C+w5g6j!TZqjdmr^^bo|03UND>#;1V@K*%h4U_tP@JbuA2;G&F~p z_#p9+? zjc?9MFaky*{5ZXG3jEFrP@mJXn+CQ;l9z zVh^LsH(S2^gOgFz2v)&eKM{;KsqAbm@bCC1H^zJ8Bkg!1oCQu;uK~UX&88LX-m4LL zl^yCnW|1i`GqoTS0geRCnkB%uF@5X?J8!^diGa}!4CdOw4$e7hEo{MUTT4G4DHIUf zs<0ZjJC}kuE&~ymjzNj(sudrq!LKOr&Mp@w$C@bdoI<>1iq+~3*y%V0{>g#s?xCeC zbG$l$Y?S+U=jp zwQ80^)azgq+Q^#XYCUzRh4}nn!r3@e0pih&qjZl+5E*?o(_6fM_Q2s1K{@%W`En)b zB!jIGhHaL-Ti=KjJt?^@&8w`^^w@`=w?NMdScQiXRC3Gjot>wxMl)t_;wDS}b+Hg8 zx~c5m7n>CSaEiV!e2AlKzjxmeJo=f5XB`0a*g#W*c2<@2??aQa<_7!0`AH_soiP#~fMc*S0?9<~t-=PZ>0fH;(Pn5YZ}RCMmxg zYSHHpQ@2iOvaLC<;Wx<4Kv4IQT4!$7#5qNwFDMUUu~B%O73@5kjW4}T1{s?oc(qqw!_9O zllpF7p`iu>VWz3$^;71}J`QMrKOuzp6rxqy1F@S6`;FL+H4g~RRkvafmJ!bne#3{) z>z)I#wdiO|h5>@+=H}Dq?lZ@(Kn}5_q(m)~s(hGP}nGwllq3{#aS~%^Z&C9g->)&;4mRYKH?NiME0I140LQ8S#yRR zj8wYrSs3f~5snv>tNlfdKm~fIiU#pNff(R{0zzQv!FEGPm;Y`fQU+q!{-+?ULs-tj zF)HoPnV8--w1XBDPQmP;?|6j4Lm{S5+8ijpoE8~AK4Qi;>N@lk4DY_aKx-?!b<^!; zecp@)v1Nv>B5t1?zFWw3FFk6-4AsVU8wc<8pCf(bmD*LTMf$X8+v=fK|<^ZDtith-1ySV&Tw0e{SoP%O{0kyj%k%Lo1DZ$qyPk!&6Dt>nUw zE?}9#6-FtBQfDhn=2$$dr9w+K*%5G%zrXgJf@q@aU3#fXn+O8$TPq6p1RrbElKoVa z@$>U+YphhrF9OAv#gtz=`}Q3y@*$hD#Tg`D?o~P~Oj8q|-n!K6n>!@@_WIZT97Y?% z7=qVNwA(ZCB7{k{FNEeSyq9J??KgJKgshMbav>hCD1S_m?d|o8>oe@1jNXksuy@s}nr6q4&#P&uB63<(nB!lFWu1b z!<-f;(T)T#3|C*v)_(D#n9&F=lXh!R4iyreTA~(C6aMQfHPqw?=tgozi$OqE zq|}K5!J+4p6!_YuuY#Mv6)W!L;VS3hn3F9We$-ztcb6{UROC~&T{?^y_bt*sg}TTq z9W*i~mfU|SCxpU!^V`JBG_CZub;$s%czbCPYPoVD3ge0DG}DO`%JR7+tvfqDE47Lu zw|)|J3)QVv@*(G0sFtD5P>|m@%`T#4ZF%btKPpjw23Lq@S$c`^Ann}=0guf|K88l! z`Fqujo}B?J-V?7n3tJ!|Mn|kic}$MlZa))watVdD=Dm!|vo5TjIIj_IiOtznbSFFe z?ovfqrQ;)Cm;IDKk0Sx@G5v;j%6taFk?`CvrV#I#l~U3|%IT)OHP)diTDJ{|sLDe7LC`13c~V=^o>~{HMw= zdEFX(ag`cDHtx7naL`YFu5Dl|6aO5NC7xclHB-Mk!p+SmC~INpa>s}j2dw5X9bT;E9RN;F+4=2G_;Jo zuM9fkZu(q3$b^l}U|r#zJ#v6}2-d=g(bA(k=J`W$3WzQ6+yGuIZ|j=G^dIuE1M+8E z=zT)Wq#*g9*SZ>TBXBo3Wz`RORnA?+v9Nnv)>OjG8e(*{2hycj>mR?L%=5M(*o>Cm zO~t;-xvRbE`2Zq=PGT7Kj7jbE=;zp@`4|DAVwK&z;|x|kh1Fr>agfcAm%n=L?$V=dx`@WZvcf)W>Mi};IWWr77m^cA z2odp&yJ)$1*U{TTOX+b!pt@|V3;gV4%q*(vAwjaG-BCWhJyU16c|-Mgft6dO83BVL zyB#b7X32>e>#WFB-~zS?Myg4stUjYt)~u`d!+tCo^7cZy=8{#&03c|d>V$G@5707B z{K+Q@HIIkC8K2vehTBn|g$E*41RRBKe;HtIW58xF=_ckkzDrgBi-75K=#5XdmlnKtnznaouf_q?KQhDCG}h%LGC=;VpG4^8F8McIoxMW z1*t@GIYJn|*2%1?-^=^}PdfTcCp_@>DXtck21yRL}x3_H^40B<}us^$B_T?}*mP znTN_aRQsEd@(zzlv1SSoISmwa079Rt(T^9R-|8FqJc*Gd&OgDS3+Ly; zWIL_cKzm=C*w#Wf`>O>`yYTJkpHK@id`u$zZBmo*qu+Y&U7+C4AdB3PDn$pQ3pTbiQNp9sMVv?5HQdG%OcEb!Z#+re0#7hPvr&aA$sk+ZSI*BLr( zuejUR>8tpH+K~3mxi<_e9h?Vz7)V`ZNINJ{rgwpjc&!VQ=U?vu_Nr6|MVQECH<0V| zdrMAlj#A6e{RLbL+yv$$ph~t4J4xb=!i!S7KBV+yjIdJ|-J=%;qM?@~C~#u_Got|T z1f#j%MQo9i?E$-C9Ck`i=gs5(dnX3a0h1d&%s#8B2|cAxy(*|X75>}%77bhl>_L3N zw&xko>}#%ybuAf-iw|(?vW)9mAj93Z#%l{>cGix@W(hOqh~qW{NPF&4a92>i=a-A9 za!vXCSnhB`66%^aM8YD;wuG!)cW|HTf&rukQoufCs=u|Bz)|%STRx(*s5h2*HdFwh zAVYD6F<9W2v(|nHuzXg2tkC~>XKO4nCVZ%BZiY>@jE*fpu)S(iy`jo?Q{24ZIhk=M zB5i~77jbBCfRw(wGqOLQc9x;=N(IQIm?zdItGB=wm?A&rxfT^6s-xhKSEOh3swO#h zR&S#yC~NV+fuU3i(?0qE{Jh+O+T7}Q+{W_9k={*mat4n0Z*2EPHrn*D^bGuo7HS>r|w zvOX6tM>W4eKJGV>TY#lGb}*9!-nO{*n@n6;V4yaK5wpsk!L9d~@>x_$j_{DCkmI=} zyl4$R_o<(aDsCI{(bIgA5m~b$y#}>A0GjNwC8}EB*MANp2}&g$CMr)RrO&_i@p;;l zZ=$k)R0du%Ik@Ol4j>F(a4f_by_;qX+}s*!YDME-h<5}3c`c7WIbZnkG2K))STRZFaep4tceTLdpaMv_ zU<`0qcF$?q?x;DkWCW&O+{C5-R0lzAlqdY87s;m+xE6siQ0FY8T$=v`ryn7XR#@av zlsd)T_t?kGQ%*YYm9EgtU%~uEWj&^nr4Q%0WT#Pr-tVetI9|{dF^ncNBQ*;Cxodir z(!cmU)vVF4Yl+-*=#O{##et)6Kgs4nRq|AV6<)LAcjm+q=$}09hZZweli@ROd_-^k z=Pq0?b{1{i1g?+Y0SZ~vRbi(hd*5zyg4*Qk4xTtv5+e0eNmSA&1Alry%Z9ubP(qY^ zt#AfDU@bhS3kIk<_uxFShaX+?UIu2sJzi4xPZo-BN zrOY)Mf7{lGb9+gQUzNA>4U%JJ;9JUS{6PWl;Gk*$}q=)BV{VmV^e&`(&+5rE1|pCvZ@Su35kh((M4%j^#H~| z0y}dIr+t55yMyVp93>%pQV?BId0^}n!uGU-BxNrIb90x~R=f_J zOm>2|DHkuoD17FbCQ|9YSSfN)-!6mzUMW~`jr`QT<2{_^KmJ2LB=-ra`8cH`!a(Pj zOIZs!&Sp6H<><1-WxFH700FV$Q2EQq(6F;0rl@cte_JJixow_W+mWg(S{K#;s^DwzN!dXc*3M*qg-`HIk2z zKpev`La|Y>>+gmFylFP2ePCb8$Ez@pikGqPd4VQ?P+h9vUAXUFX!|KB@BW;^_ykw% zmsu+iZ#$Ye6G$uRPgmAXsuqBQHcAez+(x&%E^BhpCGUR3!26v$iap$;Ak+5rj<|?J zKl?WrP)Y*QH9KnL2kL*f*C<5+t~Vt}MnY5#n&J#185R^3yeKtbL!hMCbMKzc&-3zq zEhF)&RoiZKw)xLnfBPyEvJMgU0KL@L=&(hJP>R7PrAJ+>?^wnDD)P!PZ&Bsz@^Qr6{M7kfkPi8e5&7uxA3rAFI1HVT3MJ*(UB{atl*+ur*6@^t6BF?(Yyfk17KG+`9U9u#652C9q&Rgere)Xo}Eces|IFSH=o5HjU^i z_!+qt&OV_wo&Pde5nxFJjqt-UWkiu1 zA7c6a^|069{m_u@0;Z?GroDF0v2LuBz!VMMF?*z`FjfarD(ptGu%xkbMnlE? z6Y*vd3+Pwy~E)=G#^Q}MW{Kf{~Db7 zI#!R04fp5ik@UpzfE;j1nSUIJi-e2<87I_raPM{lZ6w!ooE4L=p$<%8ktTjp1;b_k zmf+7zb%Z5X@HI8$IaX*ii5hXL{`h2M(n;mBd38y~nKk8-vDT9xSl8w9bA1czQI5bv zT@om8-%wBEH1}Dt!DL&b4!%eqzLg*=*c)I5EEwFXUp#4juPe;$VLt zjc~D%gDjH~+oax3?vdc!j&=dV0vGQ4*M|EXQ=n92NomOwf&aXRt!ub)1b9rqnu+1Wmu zkeosF{YsdkK?#i^dS%P$G&ykw>|ELbVuWP$lk3c z5^%d<(Q6e)8tPX-D8SHH3dBKLeVs>q{mg>20aphlCflAK<16rncg#~-Hta*5x`mN9 z-z52<%bb=!8)10z0O0rGAZJsnyhhuBz>3<(@vpi?V}4JQK1CR7nh0&wsM&8SBexF> zN?}Bxg?9=IJhyD*aA%rT3&RaXb2Urp0f(j9sF)y2>0fO%<EDdM!7+{9^2}d+AkZ!*Ifc z=*g-%oLAEPiM)@%%^D$ApJ_HMA*$UmIZ*!#9twnq-6GNcxQGFVPAyX>St**hF)^I> zQyv1mzgO=_IldU!WRBJ7bK0kWq0T-uxO`|QaK^$w+h0CEUzSaM-6%b=#E@cqjCA^M zS%l!{&#hUpHxySmxma!P5K3T-++wp<^??R1y5@cVb0->NxaC_hz$^Q!No%o4eT{Oc zQ8}&@U}&}~#`s(sIrzXeznG{ECc-IIb+%MD40{!_NyJ7{-*3RXoBi=LUgM4nl?MKU z_q`%q{O>-|DuZ1Krs;e2_0V3wD;~cy)8|W0lk)4T)e^aaiMZwPvwfjKY#mbWY(l<2 zwReAW-1b;0bmm2h&Q$6-VG@@T(35!S0?N8{g;6vzT+WXl$n}w+l2QScFwSp}fA}Uq zTC8+$(jZj(20n^jY5td-qjF|yhAKY?rFph@rHdkzi?S~oh{Yq8eKvc4y%3h#ZC zre9(|Jo#HW$Pt*iIyl5#Umwwb%sgnEUyZI%(k!VXVHT>AFlG)lA6SW5Qu=m|U^D11 zW%t@P_f`G3pW$cf10%d0S>!`?Og_W2=^xZ|UYM>Frz#U`u3}933c-Xi!ab1}Ch+bp zFW!t#*Ek_1_kl=Qo4xLb{N&3q+);#Z0|?l>0Ooa16w|79^HOi9T~+#6DM}=zZ#!qw zxfEWZ;jV*gxyMxQL=|)XJ>KtLW4{hbn%OWZVGf_+0{H_(Bw(KcW0B=o>`Om^PSuxoKG?v9#cl{OPOOLfc zM-LP>p>?%$_-got;CV9Y&+;A3J3eG^s1fqkeO+B0oXbe^+d|rGAq2cHY>gC_Cbvr( zGDd&=ewVF_omAgt0T?1&N=pQQb?->wfl`J_DCZpzoz*t0f+CZU0L5v=7r@(1G*{(Z zA>+Y$Vr#4qFUk(Q=S?&gJ1>BIB^+7zDzUJBs85#4AWWEp!t#9NAzELx$d%uT%}3TF z%C4=Kkw}nJAUb>Cwi3oQ8tr>O1~6_2TT40Xw#xjGT=1$$#rgWyOC{R46-O+07RO=m z;os$adJ<{noXKyByZrd$y>+F&bNh>XqwVZN3No_30l0s(f)P;GLj|T(5BJ&SlGy8^ z!MbCI#K+FeUY$nMs63;#uA4Mo>;;2;MQZM76!BcB&QI4(QgAs*%(FD`dw?5nO{(P{&L?J!| zbCv+h59UwvfB?;U0vXfpeGixXv!G zHwTuU%iDYbH8Dl@8g;2h?GHSL06nu)Ns+lx)PVJSOPniFVEO5n~{C< zUQHpuGeh8jwNwEfS!!;*pfOae-|5fM%;7TF>vYm1U-2Z7gwZ8oRz_d&<@f8o*Z51O z_Go~zsJ&sglKo5#$e_(r7D+?q8_q+gQs;9;Ia-f0-q(8ih6839$<&ZqQ2Q4|%N$1r z%6^e&z&-Y{qUujk=`S93+zO00L++IR(z|L&_6N)y+W15(Vo@OjWQAqw#9h-5cK+XTr1oN>{iGEBq9e1wvkrwzJHk=Ao8UXJ^mk49O?tI{Q(?YH)EIScc_Mld zdDD9*eI3rlg_L1vWBfQV2g$P#Dc4_@4vx#QkI^)_5|5@4ZFA4)5-tu7rW7fh2`sS! za<=sNsAWs1fATRY=>p$Eaew8&5NWPZV0Wb!JL^ER$*qw>={-;(Hsl3t9=L!pAK-i^K&>8%I3ut~ z%G_)6$J!^1J2YnN6P5Z!-&KcH5N-vM)g+{BCoj=eNEeZ08Sq?M#rImg*Ro(vOrp09 z-J?9?09t{!a;-=solX1RKbPOfioUaYtwXn6l;Ibl&mXj5`+|q~D;5fg9vzM+kT1;` zx^p5WBfIdsfaPw5VHDpnz9K2Dkw~Mbna~9j%)aSk2@zg!CO89&SLqj$T^fK**qI+_ zPIjI52vSw$+EEhIk~5m9_x<~+OAFMS+20-oiZ^gx5k>^m^#4#fXc!dcVtsmUXmi1x z_jU6p^Vx+i)HP|nDJuRA13oSQaP)Dzkp#gWr!;Rsak{%DcHy-X)e*Ninx73g?N~3T z3fbMSEt{rBXSvx1F=YK}Q@mRUZ*|RCjVFsW+VZI{7XVL{&*;x}{=?~R9tywls+CzJ zq;E}aNO$DsCwHM2zL?Zqe^2&|GdB}A7xxABYaHn2a!s`(>39;2OEA3OYo3h ziSD0}b+jITI3)i&P|!LnLYtJkVKiHBMjh*Yl8e?;0P@v%x1R>87Ps*HcpX)%BIP%$ zWcU7F7#VjjQ+O-E03WQ6e|P8KftMcF$5oVol^3to25{@QG~O2sgzKfxuv6VUwTeD}`0?q%&%>$T=oYl!c_-x@O|lM!N|24@<$1QM7;o`%GafeGw(87>2_A3MT{%m%VU za`j9J8nip{j*MiT)yCKLnxU26Ngt#3pahbQ@}3IBk*Vm>N7l{ZGCoL-Xc?jNWnsVD z$(>umARDj3f$j2PT<|z|>VnSag zRnQJx{agweqbBLLTcfp~JMCjdAI;<{qhpW3udOMj9GzFvdA~DyZy2>2VJ!l`jAUDI zN0GN$4j5-_q~JWWZhCAJXz7?~ zRFXtzu)!_>fU(?KNd3wSO^9Qtb6gzF_;NOiAJ{lZ*Ep}7AQiXg5o4tz#iZELf=*|@T8U%Ab;HrJ-z&6};wuFdww&G+~BpSd43(@YP#>Z9{M=XLUrw?HtX zeq4ig)Q7TDeJoM^8jy#bUqB+@$437A>c$ZZ^`lQyf`BsuDO#a(0^sRAg3y8PPd%PS z6N3@^09-eMJbzp!8~< z1ZxK{>|g^m@e0C3if$ z45?7pY7Gpt|HjQRW@1hx1sr+%mw*2J30+@6EPz4W_{kP_3#s!lU-Me~63JGg19PMR zpV6*_yis#!2{d3_u#;XW+7%~=(bxbO=V{CFF`+LvxJmesaCc-^2QXXX%Ng9DZ&kLf zVj?gI=-fksEHCkJB*MNJH82xHEs;;KAXFd&-|I;?G$p;xE2bJ$+a`U%5&S9C=<1tE zw@YwcWc8JzOl3)}Y<>3CkpJ)YG00#ny_Y)Ph4=2LT@6eis5`n-!{`KQU^*D{RZ1?N z;5|+B4v|!WU7`&y>{rv&&G@DjH~=x{>IE!&p5-F<&e|vk4gk(Y{yk3em=cagBSZpq zA_jPCP6TI>Itwd+(-9GZttNZH1Rq}Jh- zr?LQXwM}JDx`dwKmtQX49t+Pv z3N*fNEphW9f;aI4TnQ6on#F3&URn1TjMTw83jkXl1RFKH#gFl5zFHX1sX=WcJWqge z@KgHtUiuhNRvw?}EY4J|B%U8;R>2v2SKhE^!os*R6SIoZOwBR8Ey_{G^2PmPp}W!P zq>A#?GIXKReP{&pU(pzK?cBC`9 zT(v0Z7{}%oQ z#SC{=s8WE|Idgj18+gL+RJ>djAFG699j_FV-# zgI{;%J(|yYlbai718qO?3T^}HrjRcwx9YAiM6Pe)AI|B~85otIBXmwl@%T?#i6b13 z*iyuYIr&~|Qh2D@YsQ`{eznez#>=CQ#-tUl80ykiNlnRVhz5_J&{qK>z=k4;yt<~n zSOh@zHD}UEvUNKf8A< zLw#5g=lJV?2WLR5YO9o|P|?OH6N1LAU06a>yc{|BBBX$sBI%raN-$O~76-8_JnhJp zc-s4)v_N{vB(*A(e9hXvsD39NH6tyLUD>^EDV-m15j_Z*dL2Zu|ES};4uXqn-qIT& zG6WXmROwBsBH8{NkhBB0u&lW(>{f@=Z^?Kri`q{3w@|THKRwY6_gDx+w|vFwtBdw5 zn1_WHUy%6oRh$LD^VfG;G$zlO@Zxp>j}vs^0I_=QNq*CNf8f7|xtDnujv||6-p!N>XjOVRk5nA?=X>#!O2`&W6EQABtO^hh7gc zxec*I#T}=;(n!n1NaAspEkBKLptrs6tNi3u#;ji>G!2Zmwhp2Ih<)uQ7(^HDkq}-* zrtWMoiMde4cgB3LyNg})3X|Y;B3O(Zt2lZ{`xPhWrP4+`n~-LX%7bqHjDm5^-1MO9 zYno>9%?~8dya1M$JgfoBf((+hC!0sUli_Vkh~tIn&)e6h{cd{tsB&e_*taw;8B$vd2#$~&olU&U^oJ@=;g`Vj3;E_qo(kUj6xskV10tYR7voWXd+)1 zXh#rT8aga;n)_GsTKF#*ZNq)EsxD+ITF|cKX_0Oz#6iULgC8z2Um)!JKGyFLoFf~q z6LCBUd~5HI#gB+NYU|Cs=`N?Ot>M?{-{G8!epTIdNFKw|#7^lAj1vo3xEtO*%|Wcz zUIAzu6{`PWopwqZkG;9pTikq~Ii_3+E8p#M8MX{$AJ0ynIL3duG{Oy=a-t2}P7WC= z{4s30KZNHCC5OsSLt9Y9jJHY@teDl~e+>5k!Aad--PrlKpTk(z;5`apmGx`N_X-Fi z0W>8$lmu&N&Ax?h1T~YENt&2@kvkTs?@qr|&`P+dhqbf+3+OjkqM4d$m&VdB7mE9Q zkGr|K{Z`ve-P@%s7wp~gyR57*o<7fiTTkeDb=jZ$6UelQ zLm)c)^ZTll5dCR&ZG^}CXcvi+wt@OSd2 z#Ku2M!+-vTqk=KROK)~1a^2OC)oRk+k=CH;pqfusxV9P0OCu}7D0+v7;ZO#WwJe8Y zNGVK!;@Roj{deJ=>|<5X?j4M3fl!7A%G&Z82ri8`h0x+Ss6LvkltHRsQQcn#F>(M_ z?-tKfGddUyHh)ZE>!cSxh9#|QXkqA2hC-dSo>G7p-f7$QsTWs6#;An@O4?`MEs52s z!#JI1-tsY!2LgJ-AHmUMS4GlK(uA#e&O;7|CgJNYJ*0Iy$OWqKHdFb1)Jei&mCHq$ z7qs&_;ikgL`k807 z;UrK*2nfu3NAi2CZ7WyVnBtP(?4=#9-0kNZ=I>in^`s!{%KDW&xn4tVO)d00A_*D< zi){!$kwIUp$-j!46`#uK;{ z5YN2k36C~!+5p)9+k3aj*jCq5Vy%hxC!gPeMal&nBqSO9+6Wu|i2kO>U!RAWQk^9o z@hJE4JzR%(V1Y~Q3(F)rxBA9!-!u{o+^B?SIKV_tRH>L-yJH-y6yuKxlzjKoOFez6 z88vnmadCANG_)H~esAC}OZ-3Os2cx!wzEi0=AK zeldM_5QM^wRual&JxM5Chx*hVfc9w-t=3OTbzks%6^$Xxv8* zMSA%$pYw8yQhaHo0>RcGdhAD*X=!Dhj|uztqj?3Vvk*tK)9A|Llj?0nd0ulr<+wB& zWdxoZ&qf~!Oorj@qWf}TF6UKP%9^1$#GRQxPx4y>!s^iLHWJmm=x>E#I3%haxYiqq9=87rgn*REjSbFk4}-&{vzU zuD)R*-xlDyMIz1=2X5u_%)V-14>89Pf-*g@OJ93q1^;~*L8@2#74k#4u3zv0Tx`HY z4XoK^U9jO_AqA{Kw|Id>PCmXUi;=`WHgsjb14ITqjEoN<=TvVW7KumF@Qe$Od z&%*A_b!ozhxvLZAr4jXt5PgY7uy}*s7>NG>d$NCJ0M3{;p2=YuqQiKYM?v_>Rbf=m zsTBLE{2C(gnu3NqPve`kOS|!SgNdd`r;eqPR)?7q?D)ebRDgDR;or@(9<#0xi~g|a zuNtOhNvkR^CLt|F;2zrs-E^$gC@VbZ*boMi2AeFk>NOo7Z*APvoTl#!}P!yTJn;)Poai>`hsrrkZH;zPY*iSeQW-j@{JKq~yZ zbFph1L|rYVy|m}sU^C~JLX=EF-RwTl^mdgI!tJW8DqYBAkvJ_zUe=IF=x=s~4q9IU2GC$l&H>~kBJIhLf1ZmLAmJuEZDg04ZG@!G-ti7YyiFc{J#;w<@C)gETzDye9C~(7 zGj1=1tZfmusT@gEa33coa^9|=abT2a)_EhlG17IsHjfE9QU1Rdz`KID1ibNOoxYb) z4yd(SStlS0nNPlGem?sq@ahnK;5)=?PY*{&-8NHAG}J%M4m!Zsg-f)0eHrGwKV$?R z-)`Jd9(7WC|A30ea?09B`OXLecD(R)SM?#Q+#7~lYE#ii$=G2&-VWWS5;%{|bRsMR ziGc_aO1szoY9d~0PqNG259>96g+o1m;Q@d67rp48T1$J4h)Z)EJjA7y(9L^x?ccRl z2&srbEw%kKNBkjDtn!NwlNAywMHm!E?qO8h7P!4)$`gB_0#hq)KjgyZ-Kk4M&g^@# zjEko0`m3#`D$>Dnmf7^Wz{k}zdSD3uY{#%E?+gTPl_?}@>Yi9~m|iAT+oPSTx2VmC#xS+rjBVk%;Sp~+;S z+M2`$x~3GO>X33E@!~u2Pkg6{iJ&0a0HJ>2F%DhaI;P-H# zF#ZF}Tq8@U4VD3&A2E^&?IAO|4HW_Js^Al8L^H-xI+g6gR`lG9U3}k&uebThBkZz$KTJEx71#-{pV6Va{;+$_TK##|>j%#{0odmjpejfqz1XuP zq6%9*xNO*Rs*G1ju+v{(E)1s68oX@?neQVIlZ(ltGBEd-_Fs3BK;o<|Ph)3iJpHuj z7qYDa1C)6};6IY=TTe%BbI2)z$E7O=kjB>NWYa_Ml|l&1YYC5ly^x=oh2?U|wP{0f3yQmpK+P}1`Xg0)SG?l5_&C}hq>3;}hsP32@W#t>K`BdAG=-VQvUtKLAAM#aZrNR}E zI%bN&s!!(}Cqegh+Mc4TDdR7a--Y!Mi&gAq6p1>ch5b|(h^}wNL_VXS(&nxQH8JvK}J@eprP+|5dRot_DgNOJ3Uws$2^s_*`FuJ^w}HKOt8=7OTD zrHSgHLpSa=Z!Z5f4z+On9Y=I^1>_AV7x>VPh0~(iQdgN!CUcrt3$$?uQi6b0f@|d7 zadt}zpI7){_0AIcWmXC|4v)kdVc=5${v0%CU$i)vNl*=HFJb}C&CjQ=l8lLSoq8^D z&%xuMSw)`0V_H(!pUF;2L+JPBr^m|VPzxQVQ#hB=b?PrpC!>qW=8e^hg;if~){qoG zUvziw|7<4t@7}y=icRJRcDG1npq%#iuTvUP5>%W}?bm>AfD~+tHR6VF?H&jPPy+T9 z_c($xZ|?X*SvY2&*V7*npHcUUHD37O0F?IQ#p@>zQpQ@!38-@PLf7hIqs|`)A-uO} zm(jfMJ#a+IkWHUL+5c{>p93Rxl`gWA5HGURVs{l-h3sT5-ekm>6(POI5=*GNNMfyU zP2c+f)6MXRU9xq@_mAw#n68os#+MqZT*~LE#Fhe!NL0TDXHD|jt52mYZ-F?x4;naW zYh%FZQr%GDc6za84X*bxvj#;qj+>DrBUq=iCi^WZ8xcmpj$S^W2Au%M28KHaXGG)H zb$OXFq*;qeV%bYU0qzC@AEKXQuzA@eZf_*1RD)6-Hzk^dtnjGM%NXE;+vY|+wfh}+ zB$~drn?}{2{ad2m3Io3E^}JhLlT9loRmW8=V9~TY{b8@@)m^nob1C^3OC>loCsY5? zlvI6HD7lab`mJ_C=`bw$Hdhb9RZ(95>*j5q;CbYvp<8s z1)Jht?m44v#2%eGEyni~rGYS$1U%)ktVVqY)%6&_FC@clq1TQv-|oM82x{FBaTlL? zha#+;J6dDPeARI&Ph^k?uui^HEiBuWp@L68l4y$0T_cpq<%Iibub7^Tn0PMC{XHXD ziH&0)Ua2>J^RV>u1ph6X+tsP#sR}cQjpw64s8(z`EOv=A2iAr6t=xQaUtkn?T1sG% zMMtZjeS9U(#Aam+6RfPlH8yy^@~X*%$#+}I-5%%*uK(smp=sd~)D_uAiVj|Pf~^2w z^wq^|+M^s&V&h)Tsedi3QT^Jj0dHsp$Y*+Kc&;L4*MtPyL4O7!1K2=^#@rnADm8b1 zTTu4BuTE317kYkH$LDY@d$^k4m01k0ObpMw^pO#fmtCoL2V-q&k_l~{W6aO)Y9*eg zplviH7gI1>tCDdPYo3BynkPC;*+I9^w7F!y$4`vO_pLwDGt*Z~SBVnpWK_cRD!3ap za&P)I#2vT=>$&4#P{Z#b`XV8^*elM|jRj-gFE@H{c6tZ*X>^u#;6)us9aEtHQXTCR ziHq95NXGbfV#LcC`78I%^*`?UAKtIPg8!)MAq65(;C&PXfA;Y_rY)eW%(4b!k>sq% zBk0Salep%SDt4at@qNn!xn*8a!BIZC=|WYXd;}2zk35^r0PF1+>P7QM`(KcD>ldx- z7vnG6vk-lu5V_@)LfCzJ9Q(UCLdL%al{nrk<`=}4q+;D26w{HPIufBMn#4APFB8E{ z&ZP`0GSSl>Q@EIa)@HTDn+?TTde!*dk928tpN~XXg*FpzxmV}Xq zww8SVQe68oup|7t;Ybxq!aq^)+!ri$6U`8OC4ylSo+uC{MIXD&zp{|?vDtLO#x`Gs zqL0@jH5R-b0W?qJtT2rmk{5_e75+aFz`N79&?S{-ZZ(U>Q*Ad0#|o{JA$I2MzL#L& zYj||=nNaY1WiVNk+4e%U<_sz+`M+EA(FgT)#g-gNk4yaWv!JihWd8bPC@5X*_iBNH z)f&3a_23M`wZ~@6`luOY)Pt;y?;RbnzkjRL*46?|2jnB}9)5Q+gk~C!GACEuSB0U1 z&U0_sLSvSHv?Lr7B%alFNe^ zw~S~TCqjwrT`xV=I!h27hEuIxdv zsG1tUyMMo#2-o`D91;$ReU(2toBHMyXUx*1~;sg>|$lCV-5C9`~ z7~)_&kGb|}ZyiR~sYsz<$^`uc7t<6^)C2TIAfhB%dOhRUw*$V=y1VW({Or^zN8q5Y z+wI)VCt1I}6mATeXgC7sBzvck-`xB!nmnCX)A&{rP&UJ#Zp53#sxU_jxPHoCn%Y1{ zfp{T)5}d>44Tmm#XJ17*xk89mI*~>^j8{nPOlHymTtSM_$a)>a4+JNX=T>ijp|~I| zMYg1tFH$KC8VWhx(gWR&wNBGk2k~ZO)49Lt4@#Qr7<9A2Hs#hSP)xGGWowO5Uk=)C ztxt*95v8luiRp2k-(oI_EJk{3gty!EyTd!QTxAhbt+@e%q3NYcNn&BV;^!s%3WQc& zGsXHlQrd zRxr#XK*!dkTfq~UxviET?J3y2`NaPF*ELu9}b+#mo;Y6 zl$J46qz=a*5r}uLKS$HH)?#h@Hj1u(YedUUo^%$fsPc_~Jk=qknA)@< zn}d2~_0WIjrqx-VMQABVkP?z&up>LS1Bepb6vQFqIl zMjZW%e71ysGbd1<`x#v6B84@|Hh(c5nH|#CNv&y@xfmn z%WDoYR>}KD|{ZhP^Z)xAm@UJb=np?aj3fW&Vm|4znC6LeIG|$@aCwd`XaL8TwQ345JVQiaUQU-qPUm_gKxS|zIqZbD!KsM3?J&q9s1`q zJp&@IE77%~;VWi`Hq2W)Ko&OFP+$+`w8p^w+l|H-%#QAyWa@Qtxc;!dS~)!~YN?_R zG=q~@TbSKWrytk34wgB!Kb$$}Jx`QRV&b{g2vJ;$(7H3*jpu{&o6NR#y1M3HA~|EX zf8N}7R;@;^wj@aHMK^93WZI!FmBDVLS4c42`d^^iXFut}xGRk)g04U-&*WN$fqUru z+nuw0zzF$(NIX4t?9UYh-Bo_NMGV1}5IR7KEHL1ct@LLAEM=MU9WEK#x9Y?A;A)gS z)dpw)9aGRzD^0n=)6Z#}j>;%WHxZ90&g}ks=wU&Bj0LW%_y`7(SF_Y@ti;Ry<;|BTKwmbA%4A1%PdkR`SiXUE@`z}tP1nn#RrYt)0QT#?z z2AdeP;V}wb!6^_6qaP#(D(mIHKZ+P>RajP@7bHVH1LQ~AnWN=U08vvRPpGMjVn~%<&AS(N9M0;C$vIZr#HhBM1CB%SQ$8R~sdX@ABh-k##zBWN zKBV%Zu3D}08sRF7sF{&IZ&SfdgK2!F@f~LRwoD+Ms2xzG1OvknO*P_h4eZOC1_`@GYUnQBAL8zn`n5b_q zl(`Q}@U6YZP(N$a*$x+odUhVdyOF#hPvtDOxc_qj3>7i;2lbbGGjRES_Fwr|iYnkE zP&OrE)0eMS@{2Fp_=lBWQ9mwh1aIu6zm4#qGs;BM{u7VbX8LV0gyA0-ukuFIT{`&< zn1^P*T~(A1PI90e*7eGb8=ez@->4*ZsJY0xcc6_Rl| z+T_o2P@L_h8Tg#<% zOV@udv}&IP*Z)_q^^XOfY%58J)Opy-HNefWLnt9(ShhvYauhFh*wY?RQ@o z?tULCJty!R4K?02RBAomAZT6`FR;3=Ls}X9cnJR^lD~(nplnZ{H$&H0UpJo@EdFWo zS>h5+>)=0u5pDAAj-=KO*|6x3B$md)bZ)a+HmhY#d$2IXyL)ZQf?_$A;t=rJC-k%CtPuLmU*-e2&55Hf#Z`{+u;ZIL~IB_x&W-*H8 zVd3!TyZd8lz84NAaXQ~3k@49;R#vb+LGShFMl$O)NA=*%r7qS88>!=Yp~Ggs+WLlF z$vr46Rzgdn^QEXjn?n(9LB$`QVX^Y&$s$-;Fy|XKMr}#Is>n7>7#a^+8b#aD0>(4_ z5ppZ668Q?=<@Si)Y>2Ed?W?$8>9ygOn1v9P@5SxdDZG(BBod#2l>f3_p$bu&m)V}ZFg`o~hkWVrH3PrJ_)r5>u~LffMrdP0x${@Jg~J^})O zF)d4cv7xsG2YET3#{^NypmmUd+axwSJo)QOyJkl>%{O#}nQvJP9eIJw5<7aKR57q{ zSB#%!8(^KyYPjM%_3}ls(r$0+7v&kG3Cg8_CYP zeRV!l(C7k_ZrJM7l1+qPZ-1llefVk?PNYD~J!re$`2F-O_?i_H#V7&Rdp$$R3z3*z zKk-pPmv;Fx&vv-UnWY^7aS%UoCXBQ}my9{_2T=V%a09ywyYh4^<)FCRIFJL%Zv)M_ ziwj2L^C1@OC}@QJcN5kl9IcmwXP1V7_s@L5_$A_ulBmc5bQyhEHlRdQdDVL5VsWpl zd(iR~vM3@Sq>vBReUi_3c4_6!QxWcZ7F`pUz)$R*D>s%#On&KbJaU>T-<`cK_Y3O3RLb_KrrwN%9BNO4i6 z(;w;r{ZZSI@c5Dh8#~RmRjzWr{m4j98+k_y;b*YhsriK-{NoRiPt2dJ7Cw2UE(UX@ zKFNYczhH?tm{nuGtin4o2~vpTR1OB_IOL#BJ}V0$RxI1#d;@9wxdgwY>gk!uonaRn?(P`-?vSyaq{(h+XUzb9imtGA6C-aq@6L>3phLnkfyrJfud!XrR=$hCQ4`iEJHDhjQ1l@`>Cfk+ZJcvV zIR0i%nKPH>&|^4J?=q?$q=`G+2Kg9|dk1rj}MrmwIZ`CyE31Qp>QzC$||Wg0F@dW^Yt61RR(d# zPQZ-Zi0Kf5D5**e`EP?!dl@B^=BKcA&pbc{!X{$cDW_)R9j_R!R5PuI<{iIRZh~2A ziaf4~86H$W%Mf+g{1fp8tz}-QLa zi;s`5IO!~hsta!Qu~?#Jg}Z>#+aeoV@#5Swul2kypMPibERn5l6s{FjfOyVrs6Bjq zPuo=WBu%nb70RGIKW)WCfTt`$|HH82b@7^4<-`~9vh-3$=4%$@rOb~owa|C4DVetPQ^F-T5*Z)IqDh*P z$OJoFc*;67qxE^q4TuTKqIqf~vjbmf6qhq*{My2bg2owl5aliQ5%ybniSZd@{q zeC{bq5H1B3gO}rUqp_%nBxbcr|R7mEcjmK>xwbNoVz;8H9sJ$>OIo;gMVt&%RgIv*PCRd`&I$9 zN&b{VF7Z_@nL_KF9bC`ER}N)5oE?9P)VR(rULLk7TN;W@uR1!t(78!L!CA~v=FS@_ zDKS@(M$_lDY#jaRstr3y_y(aDd~iF_c82T&Ddal8q0&pj!OIIfz|q*kMA!qN5?KN_f_wOff8U9)t!kbYRuuG90-E)vsMJAFCga?%XX0{;b=nb4>`VPFhTh>qTB_2Z?a4rum%j?o~Y5$GQQw$H3u2cSJ_V&G?8E=krSbUiv+iu-d#6anOM?RUj0QCC-OqgfJC#(?R64DfM&TQ$5-IC&)UAt`4t4 zb}+PtF(4RvD&qT7pa7b@hbt3=j%@sW_i}Z%b|aakgFk4UcXuPyFK}OzJac3=#3eh8 z&c3LQvW*q|Wdd^pKYcY5xMDN%q{78R!vV?{MVxG$O#S>28wpEqP`^wg76tGdl5vDa zuyyYc__>%BHKt#C-iL1jK%}5?nQLE&+ib*JMQrsrTlo&DI3kO=;LjQR8x_CrtXs0l~J_5i9O}E9-&Z_r|=6I~sc|-Jb36o-pGdl@G zu}=%V*t}DLt?KStdvz}kd{_dwiVn02uFGB1nTGN_TuYOX1wmM)%8OwKu{e)pH_~~3 z(7%x_6+CFtGyfHZjMYtk3@mHYK0R2h;00!x#;MqAKY7XAvVYROW@>OLK~^?l!(HVC zC;vj#_B&0`)F2qo=ivkn>!j@kk2q8`FDC>?fbK93PR>yC z@7bj+$YxCQWn(-jQc%yCw6{K`45=@F+$UIpDax{`kBG@_?sD&A5a3qWdIT_`+kRwp z^z^_0bjG=rb1P@$=9#PGf(=U*b5B|cgIFqJ0PtwRN0bM9S_{w$?n^TQ4$l%AA{U5icZo6F=~CRM9AjRy9WMj2><2BT?CQ){5LQ-u|$Pl@Vv9a(0?$SJK-FCq$Xs zcEi8_C3BiGh(CUy@z?IiI$mwT51uocYSQUMP0a+9WWBJ>6C8YX%lLmUz*0!GqCH|| zq|Q>~{iET@^xdBU6us9-8EJrwVd`X^xI>y>2(|G%5iF}98*zxA7h{i^Nq`k^4RKtCJ2UT(pjsKv$C+4l8_M;$S#(1Zuyti*=>WbrTR_(v zk7I`R2ZCD!zk=e&(HK?#<@WxZJaw#GhcnZj(VJLamR_EvD9)Ak zq#ZgosXV5l|4$tHoSMgvt1fNiMViK$M%6CZ>(+FS3NxJMv?QLM*8m1xmc&+Oq>$`H0Ea4n2p zcx2}27>~zwG^K7*VNAuzA|gQ$74t@rf=M}7+GjDX0|CKs#V4nNhc1?|;=g(Z+*Q0n zkawyvBL(dw zxq1E_1>x*M#ixdXM8u7fX7kR!?q?-8o`06kg`OwW%Z4&cuoWYKue6ct$d!YM0& zYW0bp@wk9Xy}O%*x}*SJVQedC?l;3&P){rVyfRk&EQ)>jp`2!ir++qaNF$_ldWVLP zJ`}v%8M&db!2lKp)*(;nODb+|?X45?lekT;;y3Y6c@{%zc;T0V z8HK_u@VZtr7u(y~(6|ON1KGiLu}TR)?0InuPMa`tQjCr=8W;!iIcMM8b%jdGQFDIk zggD_Yx#%~wpb7#EF2KFqC%v1W?=L&n+uvghI1B3bOIs9ig2V`8UhHg&1d;2M6L$n+ zQXJ{Pq9ij9V%!{e-J8%RXn@BC!^a%SsNfX3xp-|Qo>PGTla{96c)C?~_7uWO1y$<~ z-cE8cUb2DL+H!P85y{IYC&%!A6Qo9T^=E1VHQ;{>xrZy_d-}uM7q{`A3sb42Errxc_mva=9-1$#(ugkN5(I-AONo7d4GaER1r3 z$_1mpA@gFR_Lk(FA{aw|6`CWPUImyz8#Ik_GvbYh6?WpLB2h}wu{GxnJ>NQ&(p7a| z6u5Z17k$ZF&836wy+`VNx+N9VF1f)-JuGqZ^sGe)nqjqxT$n#?v6nt~X%9NHP`AqAnPxw=)O=ZMjpx)Gsr|aE!R5~k z;iyQ8f5!3H6u0U#dAL?I;(0|Yk&5Vz2Hi?x`)X`)BwBJOQ&(ly7icDgi-(?ktW~;H z954q1(nzKg#-tL!wY*jP&`TwPA6c?#{V#M1*aZHab$fQl;Y79(Q`^ylga%f8623mA zR9N;^pJgh2{{XJ60J2#aL?!SIJZCl?dp}*!)o?fSqS#I6w6g?t4$W7qKmGzQrK+b0 z+ZyIf!b?^A8mmDaFdCw}sU>luhqy#_ARV_&%p=4W5;;7$X`zTA>qQZ}RjnltVtO;u zJELZ8zw|6}SMMl+sr3FED`pDUHvLmj1>P3Q3d7eH|37}1{}0s?1fjGhI$JQWLQ~ic zua)nFT~J@cJw_n2cyGAT(f35d8G3|cqC`lCOmWw}J)Y}ax$4ePbv_Jl$@siIJ>kZe z*Q8v{0UZZq1^TaRaVV={;U32wXlE2EY#hnAiCn`V75(;6UT|ohacb@*+*b>t1HCG4)!Cn-3bZ@#*K6?@iS2;~vsdA>UqvJ@* zg8_jhrc86VUp{+tB%i1Ebbi7t;0PELhv>``h)-zzh#+NBzXx0z%`zLWmZsR$W$Z8i-#j zWKop60#Q!uF?$4N1SMKblqga#5@0#l73AhQ$UUOu(XQJYxkZ9HP zr`7cghUM9YHRzz^{OeEs7$~S~qdDj@tkj;@OoWw%R4B&DuXz<~Bh zXB=qEr7}n&m;Kd6h3H*C{|rp0#~5;nX8el7cNd4^t=Z@3pgKMxSW$`Y$Rj$0}w zzfVFw2r<>#0bw+WRlRTlN_3T;8c)Y#azoS)9fdkv{6^8NxmD>ay&YZ=oL_(&7#^VlE zwm8s8>`ZET<-83EZc>MX<4*>^bF@d~C@pD+?T7L|A?oS4G*IcGR&`Q`7xaEZPQeMT3X^#m$u5o3!OOtcoyCLkWy9dTQs+Q88863pP`zr`vJ zZ5SqCb-rTUITF6KzFZ0`lQUGj5)gX^<}+3YXYkIVyly_z4<#o$XFMeFzV+U=VF$WbA8|N6o`@=v?4hY+~K# zuwJb#cZvHK7{a;6j>wJ!>U^mPEy8K3E2rNGO)umlGaIB4MOK<161c{C{b6Rqa50spFJSA%q`Fa}TO(l)S`l2ls` zja0(R@o}%M;Yq*1@H>abS_Z8jeR?2+;`q%ijK-6p&=t%-Z3yRGx|!G$;R^1-KLm&I zh;=0?2n&Qo5cQ=}TV*!_2I|z?@bZm$w$fMg%I?MEM#d?hGYhdHYPhE$hxHRFhxAWT z6bc*wo8D!(G2n_DBV#nyB(minCYY>2{AnK+e)?6l%Pdy>is?(HpN)v;)u|#Xsx_0O zUO!APA0%Go> z-%K;J*X$qZB~JF0&80}(JAz1fGk#Qjg9F-;iBB8TT@Xjlm8 zuXK~CF0Xr-1^}D{AqN7BGpZ2p4FXFaXF`)7@-@e@i^=Cyl%qsWGBKx3n~SWZ&-sbA zg8g^%~%O`(2xb)?K7fwnjnWIs@cxJfu1_7&@}lol2wXJ?sz33 z;!AQ;Xcilz!&%FM;!vKV1&CcjE}lOiG@osQnqWCV=f6g1Mo9@@75LdnbxyY)g5bot z)PZF8R|la+ey3rP)&C`|aSa+Bk!_)s^dz_8U4H?Nq#dXpalbFPn%?dnlMyG`3}mb& zkG|-$?pqY=N!$W*e&z8e)m?}RO>tV(9BVPjo>f&X*Qs&DOWjLRqF<$$ZFAto?h_y6 zkoupms}^pV*qWD*FW5p|4(>A~K-pnJ3J!CjCtvI)LxRZ)G)blTWoAYYVZ`$IRvtbn z3zDlsDAw#C{muO->f%qzr1>=g-9sF(|7>!&ZmQk2tZl5++VM^4RKrI}(i|>=<{E(z z(jsRFj+nqh*3o>B%9>{cjvMO;1rIJzcqIQX2oVmNZzLWPng7QzUe)5MjuPv=&Z73) z=C=c6S@_L>)iE#W-WGpiBjFEG|3C=VWhIXa(sO>cZ?)wz)mm8KdqoP}c^*n?G8!8) ztu$nD@I>I~n}?t9>-E5H`}q`0xIUWXft@&$?h=m*Sc}5S9}HB~e9sbcskZ50KgwA5I_KKUau!;0R3eqcXX4$@S zUFTJ#k_f^s1>9^-ggWoaLnay1*INN%Pu2H98t&5P-(~#XWE+RztBr=q?;bC>@Z~Rx zwxY9x!!C}X8*-JfDj36%@34=8_=6M(rf6n`*72yE&O1Sm)-tKdnl`DNF%lZMVA*U1 zOz^WG!z2=Z##^AqAn(gU z`p2U-OG8-6=AHXt{=sY#Su0;@m34T})Ooeyo6|J}`8o>Vp2ENcX5Wx=Prsf(x|{Iq zUhG+OIVsyV3~zeKJx5c9gTt}#^zO|NeAu~RT76uJW+58p?r|tA7qpaJO%&~>gsM;* zc6{YfOXlejPa-eaz7L=>snvLFr&4I;5hy5p0zxp>WwtB{P ze#aCd5ej_ONJ9L9vg!1mqwb+Kcj4Kh_e2Sz>i3~rJVmgK0E zdF6xhuGjOQT?A+SPivBNU4@g+{jo%zV`%OOVv4Z+-JwgtGB8r_?g=Ri6baj6zwo>% zBe14$(H4hR$t5xQn#d!S{b4aoXnBBZ(EfgpN>L6WESYOcP5FxjOL>_qDxZ;;IUkFD{6dEAFh>~OXd~5!46i^`( z&(`aNB>MqwA0IK7QB?iA_H@Q}K`{Czfm8yWxYWaWcFrf+uf1h4Cl5lK%d(2wrQ}*n zS^e?I_+g=YP#zg1yM&lxMs((h0frkAUS&FhzPpem+ZD25z{BuQS;ESV9YB^x0Dp=U zys~DL5~@%_af|IFiLagWn%sEJ!JXYaZO%AKKqTgTvP3B}(}xW6Gfa!;Byh;w$e4nj z`t$i0F3cVoO5_wNLT0)Vnyr>Q)StU z6#9B3qQ`Rh#)aGY`TCd11yTJc$%R3e^n?eqJVIodK_LC6n%XfufK!IOoXQKPqzG^nLy6#q zQ~Jolac>duApUrR@6i%)9;$kd8f2x>^ic_la*?*2k>2X3jusbCGN^ec4h}vf?A#~# zV@E3%7?9AGFnHm`<4b8H1V`lPQya)h!&ndKA{Al5RYSPjN5zfElr3OPlkb#dsS6w` z1`FvlcIXp^8$;*Rlu`~&AzqZ`-s&mJHdR$@_i=F#wD59Ubudc%&ZejA3D~_|U;5S2 zfn2sZO?;0G=zf4}07v_ZiDR45iETw4!>XYMRCi8Y4*##`a~ApDvIA+O!V+ll8;y~+ zzYlWmlou8s%z+O1WIXgL$i_Blpg~27L6oI~dH-*`txJt1v7w^!)8g2*60z}72_*&t ztbmkq^F~L;(@Emurw|Pp-rl(!K`mq+ALtGnxfv&Xm`(gQqOYWVRp)ORTTP3-phVG4 zKifRAR{m17I#YM?58kx*T@F4bvr7DY0we<;7cqb0yX?kFBweJ5<0arnY{6=j%u8P7 z=YSON+T{864)PjTz7p$G@E`@92W+tJjP|-hSQ}cQd9d~}beL87etX0@5<%@DPq>Mr zHse{Px5V0`UR{A%Y`lBD+6!Lbz=2G~T;iRv&2-KYuEopKpn2#U z1hb@!ON*2gs?gQp5W7ad_ExKVuK$$Iy0idiHFU!=P zW(WNy-gPhM%^FXABv`zNGb)uh-JF&&=B(zBb-DrqUbw%gq8oGMv!pMn z`KEn~_pyaH%{ZN3D7@cn3H!#ucv{jHb&#_q=PQk{pI@f60?Gz`7#Fv@TpqlnC_iet z$f?(NoY%b5we5$f{LWLDjalpxktwZGm|a)eOv^q85#?Yd#p4g@a(!IQ z-P0T>GOUJyYb7(aE&XnTKWniN;mRm`GJF!~wd6P5wFo~Gow&_H!uxpCy%?b(2wYq) z=EZ$CF4Q>}xJ`1qkF9UEypf!Ap$?T)B+@|AZ1e|n%6i@r(hxLJYAMiu6>Je!tC^QY zSH#>#7H{c6JGB_RC5vGRcH-=PBx9jfv^yGxdqf>m!V6?+r|aq|!+pGaVR+cZ$$I~5 zkv`k@$#3FI33wZ?LGDW0Qf|53K=`4O?D2INT1bFilzj#lI_?|_Y-cAyNy9G$G+ZrQ zuBPJ-CdVmf+Glwh)ieazU4=0S0^h3>tks6_a8|GpoeeDHL=~DZ#ZvpQ;$!}D%GKLQ zNtHUG6%>M>q6zn_$}iTTmZDYG0_rFWvEhDjQAdUA-GM5~;I`!l&QSk68?mUQ2{nT8 zcH=FLQ!%2MsgI^{B2HNy1P}L*n8w;xw!MXVqLq2!Uph~4oHVVL$YC`#CXgr5*II(u zd86B=_Zra(g1g|+lSEcc1Z`q}czcvgp(U~SNN&hIcoG{5KjbD2rgFt9r4U&fjEWUN zO6|B*8!%`0%&vvpwq+4k_xcX0GOwxrR7jhrL&tz%C<(!jex?TA6VheRObt~$w5&ZN z$8K#aJa5i^LH{~)=qI6e90q9%aymMI`|4E9@!Op;Kea}DJGTGFfCI6wkd@EJumDkh z_%oZ%FN_~MaJWX?H;e@|#U~v3oN#5s@tj3pXJZ*H-2OVpmXTAhVn|z|SdBq+XA1D} zgldEqA6twvUVrkhOx2wezdNj+(2nSc0D3ZuZ78mt_*(yP&oac4H&TzChqypf zSJq5p$eg1co950?2ZUSR{+_;QA~dopmqG)uPNyhk@C#%#BrFnz+zaKTlR-d)lEK(b z=at-lCBmw-Z;Lq7RD)2qxeJun@NV4yemmV*L(tKZ=km6+vq3k?Uy?aRbSUfk9=V=L z!4Y@^BL&IK&z9b;Lt%L<54&q{GJf41Cj$Ow`dbZ9Fsu@_p`%rXp$T!H-2^$0&+;bC4slN=UyMdm0G?R5JnIR_%uPfeJ$1+ zz;iCZmC^=E|1I#6f#1_DNbXJXpukXrvk@#yyq zS))oi>-S0T=Z~w_n|+IroK)W3YS=y#H4z_^J0ec}!#Gfu`JKUYAM}BW{x4(SCWB4H zaz^B~AmPl*@fLqQNd1`rL4q}*RzNy%4TRA9WN4|;E8=8P*ck09BK1N!n~781i>J_e zYSM};!(J&kCu|Up-5W#>@AJF!Yue$f4W$OJ(yqa%^m=e9yD{AhKIKpgUW9h+_<*$) zbMSVAk%dUX*hqCRo8Z^3-K*MXb7?eBSQbNcSNYt9L@-*8h?+1lft+n`4autJ*v@ER z?gM4b%%-4;(r?4nL=>af-7n*1ddC_0J@sGT^)Z=uG_W_faPEY9TnS;F45wtCx*9A( z)W=y?(qEk1%?K^B7{X3LkK!V+#ITd;Ijlh3r_=@hp1tHB`9tl`#M@=EXu(5NLT?5j zkqo#EuGr8Yqfn`{!go<1b1L)H{nS3r5Xm<&%&-R^({@i}4-bzZw3=r_Tcd}7hy8~v z6N>uL**|KMaT^c08>3tkfgLsv9>IQr!hXCglPuY#$oF!D!(iAtKpN!0Y2*A|n@k;Z zHKU!k-vOZsS+zA4)k9eQtNGui)@{G&MmC5HT3#z}Cn$3CqopMHh}<^bW`cOyfh46U z?2OgK@lA5&WyJBy|LEy^{fG5cPAV{o8?XFqEZ1vSY*(>K%=}+=COLQqMf@b&rlK%U zBpPw0fNA9`9n+}tG;Qwpu`BlDRqx}%W0%CtWoGau-p;QNa!>pG=x-5f&zrEDXl0#- zDgH#89DmNg$!Wo@J=Vbb!mZC|D{r}4TX(xez;a$*aOK0Y_=PTY+Kv zUoOIYuoL%QZvpU-BaHqldNS!sUDsBMwdx8k4EB&Vf@ z8}6j`4}kgY1zp&9bX#s`_{cYuFz(8D>a#(}6QB$(>+O5&%kDCAHp(ddY~VoLSbrAS1RZKX#p~=mqhI92qeh8(IS3fy&@c^Yz0;Kq)ffm{@VE|CS0!cw_B*q zM`39SsUPwZ1#q22-s0BuPDV27^89+kx%&)Z-K;8s*RnC2i0&-X(x9IzEo|;mt2c*t z%jAm7HqPpPn`61ssnGqhC(joLQuNObJBov{9XTM-H5PQnbQfEIjdX|oeyL#6%o_yH zB&G@BwnrL{qN&awN0Vg+!Cx%AYzabF@D~Qwm98CZ&@FhwueC)kW`H-u_b)6ZMewK^ z;*}o(FJX=a>4p5BTUns+;R|Dp+MXz`D+OX*+3w=L<-A+r~7B_%vXbA9j2&bcAL5#Vhy{0h8An^Um9=l7d6PX`ixQlAB?Ele|e zs5j@M(XeEu1G*8Ps=Wyd$UEJ%$q*xWgK3Ai1YZfq99xw1?O2&ouq=(Z6<85m?y6Mxn`z=styM z+ICj4+`6b$d+D%lL)KLsb7frQG%TT-I?z#TI9vV4~JLy7d@=srU@=DM)G5QmszPHtqBd;9{mW27u_&d8sqk^i7=+X{@X zn|c_V+i%|3mHGNZEV^z}J#C1oEqa^PzWT41HYzVd%1ChOF#pm68}prL*W)!Fp70y< zlMT@{irKJ;Hg#0-^ZuH65dS%i=@iS%pi2!Ug`518Or&FmEN|@#GYvmsS#XG)J;a)+!R% zxgly-BF{TBA@6rW7cH&OYmQ5-1h{C$;Z9zs1DlzxCm%J)HE3bbnfu47 z?cQtrNUiJagu91taS;>b<_-P42Y2&to*S1IA{rxZ#YXWCDiFR-nTRm1C<^DKsr)J1 z9RY(U05$a%Q-vT&Sa{dJhIN<%_JK9n`M_vlWL>xCyGUY}4y(gsTgGXA1QhiU?RSQZ z^=Kr#bm@z_cN0#j(uw+=dT?tF%y=R`j@_@@h0&sChz`qN6oX4*S@-zF4cmXj>~Hs=XK_9=BF;CGGbeAirSHU{3v-_ zu4a>A=$GjZfNuUh)mLRIrvbK&{}%XsBB=u{MwUH$+^zP$-ZQy-5#4f!6B*AwRFa2ZyH^*fZ+H>^nDUi^LlE~^lCc}?Q$o2Pz zYP-TvE|J5_ndkBNO*g!`3Q-Y1s%OCiqbe5GY%rCDi{GhJ5eI8mTZ)s-`=kIhG<888 zzDtU!qB~ufmVmpS)a0LdD(vRWWf?vnyRMsLO?3{=A2Z$ZrDL;~8TPz^#cY+B6$an>AVj6$?Ygxs`$8$Z+`E#8QROKri~lHWy6qhHJrnv z1uMoea=&y*(F!gb9}dqdFMR&N1B4jNegW2+=Db)|%A^7$gaR5hvl=XE`pgv4BHo=t zC0PIa>c5#*f2&)2&v~MF(N}M%GX#-BXsAzZ6S~Bs@i)JPmwc#2g!SXKOvwJD6C}`< z*~aSpCiWY|?rkN-B6%_1ZMU=~h{HT$inC!M?fw);mCAU18{IeY>7Q!uTd8nbWXqfS zh&J#-7l4g^u}`R)_7Op0=8f3Ge}R*#$zaEENx0*9{|vS>^s+fd28{zq@Yb5$yX#{% zv>~Z3Z3V_7>y1{NPohY0W)>7d#%jqZhVD?6Br=)yoInow)e*=Tj!D}Gfq1LHm6jl* zjX)lv)IJJG_x=@aNFHJzA%}7As`1_81oTiEloPe887=7!8gGI2+(&Q*N;is<5CocX zNT=N=oK#dDi+sot3jL;#5MvLJ%vM%bxU^wsw8Q=Ut@_w|iy<;;nK@cPC+w-E*9))+ zjQzcI+}YbNa`uBn>LEi>K|Q}#bx`3xy?L^GNMcC??S8)Wt|z_K>pS}+^OZ+;*6X$T zqKN)Fn4&eL^?F1ZB$51M^UFRtsWDi0DUBJ{i!6%HQbeL&5)-IEUTsH7ahJ;Wa1~yX zGe}X*#}eDFoncvM$Z2?o#&+@{SyJfp-L(?{ti!8aHjAiFvq#a-|E>Nw(*qv`qU__F=nZ#qL9PlK(XP-D$Jvd5V zNAX}cKD#T`ocCp5=`!v+xB1{hS^YN{q7rWl;7d}nly4{Rd+9UPsc|@q09eF?_=83H zJ^R#J_=#YA4ikKf`-U%uGqc=z6NTYhNvc_wwe)Wf7%4=Il(Vz-mIcztN0CXTeaj+= zP?LnF?=o=G48Ufr3SJqcvks$LhHY&=HsbZ{m16Aiz%Kxl|raz!|aB>|;mDXDab$^-C$9%M1wy}H3 zhw?0r7~(l8M&|DbNIK#?&1JyH*k9JHliMp*%!FBTGe)HhZhGL=AEbT@`S(Hs?cOGV zd^ynZZG8(!U8oJnEMX`A$VY|&wJ+Q!-|Mjn#^y7WHKB9y$cu9sPw~8>% z?(t{l8WEKM7&(KMnW?tP!v<10y9oJEa|pH37sg_BaI}H2OCn-9-hC8_P&x6U&}oS= z?GnUFv9Xk-((9ZHse%&)-*B5`X#Qlt_mfv6O1WGRPNo;elk2Oq8sYK!{*W~nC|dB` zJO&Xq3sFYuJ33;a2)dam{Jr1FJ(u^SvCQz$X58^~nGix|Wo4CVvwttQ4u@)fWxZr0 z97A*CFD~|Y@J(F@zoVn0)`>J#34Yg|xcUPsriw%~)T7F@X35d=(&+$I--8c*N3gaMW9;$P8}goT1V0y;F2iH=EeyQz-I z-US~dgIH&P&gi00yvTe(&k|~Au4uA8`I%JPo<6S4>iRY1^C2odUR{6jWY-sQw_r^r zi(f`zFSP^)=!cVp-qU_<(CrzT347cKOu|i5*Frrn+j);ZD7P)7UFDVBIl@)XxV5I0 zsj~qTr1H@efuDRma>U_yXG0sKKUh?=H9~Qh6d#8Q>$e9Ki4zM{7G#D#ggZ1VmwhkE zzzogqVVH(6hnK1Go{bW_l4`Y%I}@y41&t@m``zj4sWW%FEv^XqtEi|HTq?mCQaQ469IxTQ-h$fhIY5;OLGZGjh!BXzhQ$9LBO4kOrJN;*J0 z7SVpt;g?kuy%!dEMt2s|nlPp|mW-A)qO{_`b2;db%g6uP^B+W|7M9Mo7;+uayW`_% z>evj2nkb{7)O_FiohYdBnDf6^5#>s?5bLZtlgmUGG)v9??Nt>4A0qQL#`WFn_z*ke z)rodBPu_7XaaY_?PRmhgKZf`BxYUQu82^Gj&W;+6ccYf1)VgQ(;#PkI*V<0 zC{WM*_O_L2_nr^^296uq{=gVkO=auOJT@#`c`HKq(4cIXWy!Z>a4GmEZS5Ne9O$%s z>aHjU6lXNP>^~_e^#Tmv=Wof&JbOK{I?Z;Mq_vkPtx~=P)=!}i99X%AzJ}5W+H9W` zqnX<@r-umis?&Gi0fKOWCe@d#nKt9qWSU1zu?_pBL7z9&~gf z{tEq@j**xS;xZ_Hs0iOnQDRc`c;#&tZRsfDG_rp^=sK&<7HQ%QY zPrpXVf~#a_(I_|vv~;a?0_rXtwHm~u`H!kLEPVplg}ror0!qJ}?JqXD9Wj>HUS@yh zGf-R&Rny}^BZb7Wd7i%MD|hplg#L!oPYXm`*TX&)#5IMqMGk3wky^gHiRlBoz~fJt zM}kecdRNLl3kY-=Yy`AwQTY$=f7JO43|ZO=I~d?Aicte%I5INDh2GyCN{5&IPsy;Z zZ)u7dqejT>oL(A2h7f)iZdViSLIQ2Ro<-}J90p!mRL-bar_#~eEG)qHeOl9~A!JO{ z{<5`;wF5fr;9NcY`jrv4H>*ijv~>M}BI_-4G#NgCHP!6q0euH&q>hmGY?%0dCzejw zmOVTxWJX0YMUvAU6j26;IJ~iVa!Sgs@@H_npC(N=4Z!1Ajl|*pH%H&LQCGbs)~c;I zK1bhl_s_u>n7{nNO%^LGEj+wr`5D1f&WXv?Pf(d1=d;b0$c}F@$5w~_6sK7ps|?J> zbKYwwMBr786H(uH=ex?|6Lz$RdSfNFGNxaB)IJ=!1?&f8yk;K0_W4nOb|Gh8&{grX zHNmj1UBO=ITcL__nzJF8)!$TJ>KMn=9biVN)r@3%gDvzfIZ$i(;^hyc+Fr=arhU;F zjRkJ|6kQiiau#n>mFd!RC?-tU>r1b0CfbjIRMRi_$xbK7%qmn4uj@z}_IkC7f=>xi zl|fx2Vw1QSN-iAIO}K}y{hqG1)X-)wjEJSAiT5#bN>%hFc-Pn0mccqYg-j^@TzEL< zrai36K2dW=dwpEgaEO=`oje2L<^};>lP;Jcz-C80R46|!{T*ve7*h;S{!1a{BQ=?* zH`=K;X;vwKrqWM?Uuo80BhU~SUtD%T!U}kO76$ch4Jd#>*Vl+V5WT#QX4t?AiHxbV zKS%6(6aN6Q3A!ikkG|a%FrXW~=?*$JMRm}yDb?)h$UUL5{$VlKVEVE%TXUQh5 zoUKd}hO(Ek*bX{!i%8Ny$lqMlQ^l87VqPqT&~+{G6bdG8SYi~ShAO$77hTe>?*SL2 z5;qRI^<*N{9`ilQcw@vK%rFP{2h1LoxZD-wZ%==j*7d9_cqS`wt&=scUijMHhB!Jf zkfT0fV9~j*{0m}koUDET2BN;lTt(aqBIKs7a6x{6Elol;6^pQvp9jBCh zxa0z@{mzu(y8LihiOOhi8K?FQTaJv2@u~LLq}!1Og0)VVW#P~0&!d&F^#eufOsia} zGjTEhH48d&O@>2mPePMkQ+57?T|I&&#tOaY-&@&34hwPGr+g53-nH;W2JQ1#DR1e6 zEm&n;>_J{sHW&6P0IKrOeUkr(#56aPyMZmYQ_aKfIpceDUK(piDVnprEOyky{Dyxo zj7)FPOoYU@2Ji|6c8le|Y>7%ORDDuVHwF)G3b+l&h%cJny?fKT``vrI4WfSpk)O;b za|d|RTq?BNu2V6F*M$M-r5t-uJ)07vv1f%OHcoQ8bfc)%QnrLIju#{$no zXNu;3bDXz#Gp7E&Nd}_$UjBH^?)lcA;IsW;jK6>X>OHFj`{T)FdhK_K;>(2=F{$h5 zJTu3@tTboNdP(g_J9bG>qZv~KoScFnapHqxmwn4dVgi)TMhYa=Y~t-v(p+oRqIG@zq0VAXmn z%!frZnVCWT@;_I&TOFrX$~AYQuw;0b&a7I$h%Zd=m%-bzlq|DE|2p${MDrlned36> zVxs1)i4oDTeY3|$@6+uC@Z4ZeBtlyfb-Of#r(A)~eKF6TL9(W5C_&gS5HN{$URRy9 zy5Nnd_8<{E2I_ynF&KTJT|`!AU$Y4^D0wQBD1jcZjZPbVSTdSvW^wyDX^i^tFFZkq zt~9m13bD!jC*7WaW5nWPNXzeCG!e=xo!_<#z@utrqY&ttS zo2bpsTY`XCX~B9zDN29sZY$_c>vn>p#PuVAm+BLtg{rYQZ2-U9zYA=)jkaVNg8bV05Vf&2LWwruGi92)Y-grc*uSw}LJI4l69lJg;@mEp~7O^;* zd{)#y#Rhqdg}3ZUXV3ix%Y+w7SJ15yj$RFI<}Wy7ScbdT@R|z}E?ZPjU7Xxf%qijH zZo!d|qBmof>_j|wT-yiavHX6j*Ix4bEqz#+RMt^aV36EO*LorpLt{a}A3X%W*dAfQ z1aj^UB0_9%H`C9&O`-qAbCLSa))jFM7Squ{V3V8j>d6>|J-;I$NIsqliRT15|LA z$d>#FhuE#0?T)0(p>C^EvzA?c_V1<9QE+hf0#*7RuOiDC!wj{tO;NV)jD0^*6j6l* zC&atbPpo4yO$T?!)%nDRhRtn_y+C=Nxcn&C;Co>OeR|e@>Khs=b+IBT<6+Ud^?kT9 z{7Xrquq8`Pt2Rkwx>u8}D9et3<2&U~ysD{m5AS@(E5uVk`kLhu9B2y17(g@dShW!2oTkm7^ zzZR0YUDPCvepjB)oJw`_2A6)6qf+VtuM+WAFzB)yDf4s6cT67MHf(OkOucIpcLa7~ zm1`0!icUn1ss5oZqQtSn&ieHFoGbUJDuukcieMl7j*w2)U%eE=5<+R+rSD7n#7N_e zZ4F@#+*lF7(wDh%O!V+TM7yCD_AL$3G6hC7p3G5B`bTujk55vL6uw&XM(&&U>czIy zE{PeHzrb*IRW`)JVBh2yyVXy{uX zJ&E|5$kx^CY*m;@-v1fX>Jwt7_2|}$*nK?9<}5 z_slvWx|wajiqf|Z^DbGVPZzSc!8h~p1>?kLvC4MlTUpKzsqAU2M%aI(5+?%!A_oP} z@iRd_$tYN=>ZW-Fa++09sUV z(hVSh5+lFw3)qYa3rUaJ4g&S#!C6y^yVkU3X`h5VwdNA9Ua(P@XrnRw6`F(rh`CsC z1h<;avk6la*X!87Qhx4vOS}1`z0senNJt^8Hl<90G<(DQ zQc~0UE=Uj?@!lNbwVH-yiKYq}%7s6sv!8eLSwA`Ue#h2GVDMjKP%QiTSyO!?G%BTc zy9e{5tDh4yVk*z#ZbIi2E24*SBq^LU!sh5yD4y}y?;Wvk9rg8LnVH0O7$BZ2TfenC z)c4nY=}-NroM_L15?B1K^!@~k2m&!w{CBDQ&*rhsqf}iK5w^rfzjNEASVwB zERb8z2VY!sIF@q-4C4qm5D3jbWCL@Fo9UyG1fGd>EqQO(LDBV6aZoDEP4^8G@`%2dPir5I!|$V}D@Z)N*DW@jaxL#Eg1b&7X5iz> z-UqLu*EM#gD5l_1uyb8=CTE6#d;e{aDs1_NW@0Oyu!FIatTiI3lQ*#8Z{1&9>`|Np z%v6;UBUk-TqMU@(zAGA_pvdXC76|a9lfa{%S;3-IbvDB+TOlTkQRJ4Xg7^!xnYb4$ zUzQ`A8E;IKcBH7d-Z!}$94j7jtb)8 zjKs<~a!R%T1I#%WOhv3cBz3-uf8+w|J+MPqVa@=4L=I)mIHk5wsRMIvgrul1jyRrJ z3cmcX?nzZkCmsi2vrIT~H`yx8hrP^qR^C?k%u0;P2i?;VeqsE~oTe-=e+Cw&`1HSB zxU$RE;`SC1TwM)YI3|jzB6O_uOvT!tC^+5Z9oa}eA=u&aG1-oM)R;!qH!zYu8M>JC zEuv%u-Q=AWDkKG^(xv|M&^TvTA&N?U!_usl#AF z;$e*^C)^Xq(?>H+IF$FM9a6DrI)RV?Tqzi5 zN{)Ma4Y+`8V_1Jq%(gs$uWW#{AmckuVZ~2MJ=RzK(dD}Dcv@FCt=88ZTRAIPuj{yw{l^t04KYk_Z~WzF zPrO|8-C6FenqAt<$x?=aOP=9*j8(X+;Kb?*5_S2FC*HUBVT>shRboKc#%B8uCdD?5 zSjUFBLAiaJj&-&J;D)|RXgr;0>E}gkoov3f4zKIrmh1L?egn`4iN<_FlDry<$krUQ zZXSj2N)V2v+e?Mqc9Y{+UNCQzm9(-D_#|Y{#96vrtLzl``JJ-Y5dZBX=DUO0=APm! zp(MHxYhh(7A$mGnrOK&!H$i0G2cp~51-Wv3^R}7|X~pnTiO|$Gr{LS-7gI4zhAt?- zvLw)$Ce*z=tuW=W$*5``y+&eo<<(0QCo>T*D%C)%K@mO#n_`DI9fBhPC*K97w3T(l ze9xhF&DjU-CJY`G+{50CixyCcIH?)@LDPwNUh=TMxaF@z|1_ew@1{e`MoUMsxlaU_ z3@)T0{)O?&D(Jw9s=8^Ihy{6b&p=BC$#@~+{3$GTC$@CvF_Di1uTt+ zbz%8ReUA<~QLv=&!oEc%6zWz|n|{80b$J7U%!A_4Exo;4+`J!R_2ben#162^!n3IttbWT4d5! zp7Un3U*KcP(lYgYM+@kSysn3me?r_N#w`v0_%p{v{(mi^h}bV&K@_lqqPvD#@hD8J zdoOCi(_FDa6`yK6nq3>tPx7l*{vm$LlK#uE1IfMW`jh%jbO-vwFBy$U6?&S8yXG`l zFX5rG5^|&Ko7oq0CD4UThO3>`XZ1f*Nb-W-NmG$Nbqz}!~Fw^{3)1|RMUC4N{v z8x3oS=!E%uIomL{#I=7@bUU2_=SkkAcm30irw+x?BPo zqdl9-KX#%K&1v%-!&A`U*f@L=Cfapm2|0a|045Pn69qiEX+CyZ$-VpqK5y3Trj9^^ zD-Q`y$q_(gAe&&2sX9rjjSkZgC;B1dLc|! zVxTAxCwDw+ji0=OTl`mzuDxz1{h%#8-mN&SSpcHxHM_2RoYH&J6ENuc93ln=2*75S zz(-r=vY?^sEEyom&mY(Dv0+59$xyCKOX1sBrTelew31mVoBKEuH986?{qSZaVbZlj zykhJ(@9rv$o|z`g6L;*+ENlj#}A+-6H5sP80BeIXd= znSZeEwMmc&T!Ti`w(4Z_9GUE1_lvy}vl>w~|48OFf`3o#C6Dtd^*UK{$yvp$Yhs?h zEw3;9lj_iWccGN1y2*0TA(3E-C&-&s&5*{)`6b-jl>?Bu4EYci>S{OktlQElAPOyr zMLjW|_53&|^E~a)`roL0`<0BJ_@fNf_}FNl*|?PyJo9_Son*n-RMKXm;On+|oA-ZW zHjF3UJNT6v=c)1QV;T>F_zJXDO!PmGO~C)_=?<=WNaUU5yEjC+HqVOtdpw*8c4p)( z9!)Z`Co8Gvr>aJFelbA3bqJbbqy5MU>J_prxY&bfL=k;I_I->?!oIiXpy5{W$TQo_ z5hi66y)83JqP(HcAfEm^I0Eoh+>di;VRVMlj|uYJO{|Vpgp*CupoB_pK|9mxTb@^U zW9fX1yvY#r_5ly+K|4n;8)E%cCt2Cv?u2^~i&^){G@EkDH5(@+cGVz{Vc~2q(})<& zjCDHpuaJl8(}99yR=L?8GE8@OMa7-)2Y@}!&SsfK{O^xG&lcER+;L0U%rB455}yT1XDl~qq2J#4C<#)5=<1V7w9DR9SBCfHVf9^OLuDS~aviYqA) z`a4_MbyCFD)D7#F2Jeg$ID|@ps>E9&YY6qUMBHW6Ix-X5Mj*`K#v{SlhsVJ)#TXJe=3;j_bpP{cyNeF zT(CNMr>S0wfKB3hg4cJUV;z3H9t#faX%baYG0mqcwx4A4`m>MYqn9w%&bb>`z~#0a zaF2?=bJB?P=q9nMfD%& zBVe5FpF1il6}VYZycyJ%LQMF%m;|1wuvN$poD1vJTGPJx@IpQtQXG%yO(}b>NI3Gk z&Ykj}IL^gE>ct!0X?d7o$-Dx;__M~=8WvPB`OU7D&ks1Cn9rgCzl}EH2KI962AFbP zzRGuhYK4GOFOut>@6kkN{G-#;0+~*QXgFG`4o{ZagMDF`KYYbvFL)<#E|h1uH_MV4 zm~x|RF$yhprX2;`+#YE9tFeYPV6*aWW}EPs(!$QhO%8BHa`wqG?lX{lWG03@1Xqps z(uT48>Q~H1ZQ9POnW{ID|IwARb|ScccRQ(rb6Jhs%JKPuvbyZjl=(R$MJm5cb=3>) zf$41Bt~qPbboIl?3S8OSd-9TGdaUICbozH)0=mKe@VTwf&y;ys{C`)fom37N5M0l9 z{{NthzUAONZ=w^d;>_@1xHP#SVqoCL^BxDbR2sJZ(^u+qZjMfirWOziBTB!RwfZrw zld07vJJ_j?bbv$|`-j=>pDby0pw%x~5j}X?Z7;O$el`!d3&~a8NbhaZ$E2z@+ht=) z%`i@`>#FQ&&T>|ZEfhRTW>m95@JZ2>?ZAFUr`-F2>?8wi+3M@r;oW3!GLeF_`@0Ox zzqQ!r`}aN|(A|h*MwCkHxFZ&=M97zGs&9!ja5>-go%p;mbLP3w`o_l4ih=Q|$q*?F z6lu}RrwY~;HnJy@kDQzJGU2MSAVM+VMQncNMtetnyGx~m(EQx={IiDlO3Y7-^&tQ_ zRezMY&Qw7^Jz-)O>T^v~erX3;R?a_yT+h#nSjX-R*BwzYIhrm^(O- zsEOaU=;gE+R&N(NEHl7yW5Is~*{nJd!}Uj)ma|N}tkNI}z>sB$hR0}Nk5}qe+7Vp@}U~Ux2-4dDzkvZVX9JS4j1ROq~+a8`-I$qZ`Hm1buA}5>RemNC&`DJx-S|@Yl z^mTR}MQ0e$>?`63dRq;45ls?DW$K(-q4h%5`;S~o0+wWhwj-V2nB?p9fMv=+UbBH< z1l?Dy;4(Bz9Nlj}aP*_tRfZUuRMR{>J=-ZOcZ|ub`|ddZ5gDaWVeuL+~bQzo`g5zXJKyO{7E`AliTo?gA(s z*#ne#KEqlutSAkN*p2dj7c}NjOOs+xNBJU&U(gj^w^fbLEEiN2A2@rAE98GeBp@)) zJ0Z9B!^*=7w_5U3Uk%u+&jjB5*fkiVv z6`Tjbs679KYv=Tyc^JoFc>9Rp{5{wmWlhlQqQX0kT>-X~JC21M5pR=x`O)FY@PN zz2%B%AcJo)SSzC)@RWUbnohd2E1JxPE(3B)vtezRBTc~NVE1Z&UD4}wE3P9{BSL)V zeL1~t!U(1uS|ep&4htHK)Lj^?leqkbt731D_21xg)r=A?U;DD;#P=2*7ctTufEJDg zSE)DC&P(Xy{=&`$)B3G75^%?of<~Rc@vJB&29+s-;(a?HR9G11#bY>LSCu=3C}h|k9llEgMz90YN5V5B9@qz zT-(E&fX_yOHmf_H`lx_e)lkhd6H2G?8MNo;$McbWGEe-E9$>YFuOpJ$+t3R7h%f(3 z>e7r*+A8L3fhgdQ=*GMK%gMZ_rLF_Z{f%=-*xV*cErnw24U@AUa4NttqJkh|7E<=3 zuu+T8^PzX2zx%`Yzl0xzm1f{(3&;V0L@pCOkdd=gNd-&b1|b_qP2Fy9+Cur2!qoNP z(?Ic$LPJH0$dE?;72AIdQ)b^Ut2T&0QY8oj>)vwa+=M)sq+t0|)YV zjFgtA&}I@?Y3HY!<+9*{m~E)siA39Y@2O;-)L|b^T8KY!vZMsE&*;#sno8mGo@0$s z5tbuMww)ci6mZ-%gO6j{%M*cIC}U%g8}SJ77oeDYk22XiIW@rxN|$X}kaMd-8vn2yNkm&Pd+5o7!XSeLgp$jC zJPid(0neR)Jv!ZEz`k z(UP;}@ic+9j@*8YD9w_-e;M46^-H0jLIf}!!>opJDfW2GOWU`>87s6Dh&H_v{CCke zw)yvUil@ZyvwPlGm10R*#({v&GsGR!FDk->ypod8IklY~mxa)Tn2|3x+UXo7IQdf@G_cIIslO_&0Q30M`n?3T0zgv(TK^at-#o;Pd-&{QA=t<8 zjqQy5Y(%E%#WX2r_c%QUy#JPpVs!(CzM;8))(DfYb-}f$#lZ9Z_zaam{=K}ogdVl1v6yFLRw`1>6dgXXi>i)JQagVpawXY}E z)X}=8pif;-FFMpdmEjE+e}yrP;BJYjh$b?fxaQ&T4!tqwiE>B>z#iF_AWs&bKCgsE!$xiVoJWnqc~sgxDZsLML%eY_O}JCG2d?BWnh*~ z@eiNMhY$V#OXid*ggCdLt~6U1q=A}C?PUPtsDSW4el&)W{cCKQS|A?Cp()EvHtkJF zKeg|NF(sRFmax#?_|}hMX{=~}V=5cF2M>yRPvP^tiH-E#(VWr$D$tsDhlx-;RMWn- zZlxM&>$Rg3u7Yk@otx`5_b)JU3#kc(b$OY(PyZ7{Pg&Os>)5fp>R=!~pkn6QtI9H8 zNDBHL+`i^>`YA{acjko}(3F~)xM3S(xJpuhyqF0H2wb;+1|2vy&p6f_D&7+?z#rXy z4q8}?-+mnMGAXlS+5VuJECbo@h$))a&7ZYUA?;UJ`q8s{FP7Q%D}lOG2Q#}T7mw-; zcNGT0|FO@+Z7cs)%l@iwNF&S=uZ4|6E?vF-d$>8H6afFkiZH)YUVBW#0bv~1hNY#09}519$ia%Jm^{b_PY>UD$F|`D+H0Srvs)9e?+?hq z!{*->NIKOY-RG4GM{l=4Zgrb+m=d zLQ^yOo8c#Xn0>xrhi>vb(12^LEwg_e1MG!~W2u)JfDt~@fq}xA0;UYIcLIIW9L=!x zpE%wwT(8EDW=zu>541rfQ@X^_Z>~33L_@+wb8y`@AcrJ7u-|((@sv(`R^F*wZcG`_ zuGrl2JCAY411X*SlqSd6=)6Lz`pT5eeM{@TWdDwYYKWw1r}vNCr-=!G+d){5+0hWM zTMKcGh4qz@oHp}=!4%{P+$o`eZen#47d%?>JL9}=8P#RHV)3lTVdi+hgewiNdQ0gb z{OtQ5TweBxV5UAyEh{GDc~s!U<~2Yc{O;SZzr4#k>OgOLzO)!NRlURxCx;op{Ji#- zuz`go!_K>)gwa-0jGsvTecOW$&syYR20A&sp7G4I(L>=0x929tP6kId{--(j_vP!+y%+F7=tJKC<8~M* z&QomB_XuCuSSP0bmU4{*^yU|``oA|-p%BWNaO0J!k!|2>nvy@Xp}+|nytj2<=>(L- z&ib#C_+U=Q)k+_o(O$|w`LBf=m@D*D*=I^#*#xAi_Z__OoZQzlkInoU>UTwMb_#G8 zWDb;YGW|1SBlI=lHV|SzVZVN;0tqVyM{MM`6^57;&h)1^y4O?f!`;Bo`&3V!dLH!< zf56?I3xOY{tc9#3I_-$Mf|1I1l0XRe;4&OiIMNk}zn;|cj5(PmoR>TYa$%@c>s5~D zA@gmSX@_laHkK90S|<|p`3C|IR7CI|DtN(%9DdFD!tRDgqHrX{2ym4|Fmq{qXataF zl%REL_rEuER!Ny1f#Z7j!Rqy0vy=YYuA!hwulAQUKOK0G)k+34Y7cvd%9FakQIL?B zU#pL)M*d~XY-jx1&DE4sH9cxKH$+UZ7Sn6ZoI@LrBXW)!lQHeO@Y!Ha&hlH?h94z8 z$t+6=kVuge;L8DfzA28j83`{#|5F<{;A5;PMVhfq6w(Q+yOq}~+1|&yISRM4+_v#a z7D){IQb1wOdN{X^ru18uh$u4jEc&e>3PsRkwMr#wN_1E-3A%JxAw=499KeKo;)U85 zP+ICpBq@~!2`6J7%xDlbrmgiU{<*#O_&1Yb((B)w&3flNc?+9o+m_f1GPqi*QGmG&g16TT=CA|U*_6D;1U0=j2lumU))b!Pd{EqY!fp6reqYkT+ zV{%^le);QDn?V+fxm6b$FA4ey5h7Q^C6Z2iR~>y5`C%dE+T9@CLT(2@xG`!3lqTt}4hMGZNP)*t(Odon1s;03iswh>GmJ8CK#Z~Y6^BWym|-E;KEs2V z83NLy#ihEm6zE&RBP0YGhk+l0dd|P|0LuQ-QeZaeIRO!%LWXh6js;dTsUBR6v;P?r zw(peCvGK^97`1_Bcp!z(e?qdJF!xah z4Txa);3U0>Vj1i-MajjdIFTR&1)h|7GjQLKX;^rY{wFG|c1U_ua0tKlLRm6@M0S-E zUyoGwU`&836UPP=Lk9EW%$0U5i603Giq4L6y~gAA??fO=IWmZKWR`y}^xlk37=#0K zU<T;|B{PgGRbcMRk@ zWCuLh_0vy(U*ck2GF`~TgO;O&>-T0Tdw)P61#EsyZF+}G$8VWuWg9NvrK%^Ix3(6Q z6?(6ZTeZ)i1fxR*PD92268dbh!do5yKW(hQ)EbJ32FBf}@%Ywks*MAqgIEKFk6w-v z+%7>d@pCZ!;)KQU*#7BtHyHyzRehncwY8LY%kM-Q7at#=82{Sa|MJzhi;U@uBgW)r z>VQt86p5@e&{kF)eZml?d|$ek_)myuTL^6(y{~jvU1-NKNLA3!W@xcU@uVL*9<-3~ zxowNmHIQrBW+?NtfaUQ)-s%w5eib^>?UKfmV`Xb9&_}>YG-&cGvBk#}Ip^05M+CD> z8?8Qq_;o8b5}h>YD`r6%rOuBF_~HCIzK7*xRDoXZiAYhaCgf;#(&a>6l$o~t* zX2JGHt1fv{eA2U0yl`v7=s`_fb8DMesk5w-vCX;7_05xSS!<$3CjZayxvJPG9C&yB z)}0LrYfpIZww%Uy3z7!E#l&ts)M(nTqiX61X`U5RqYK^lsjIjbT=5FnsF(}z`YUhk ztEg`(RV_lMc8 zL;SWdkhjmM7e5rwl&Kx`GD(EKgc1hNiXK-9S>&`{+aBDw8zt;cUPxeA=p_sBlA=b* zcv;GJzYB=83tRUY`be4s3GHMY-DrqPHXnvdb)BZ#{Cjou@iOG3GUZTNe+d1B=P#=n z6MD1N>xu3y>_u*aANoWL|97P*yK^rhGol8uwodzZOi+W-%na|h(@(v>wlu%&lWp3L zZ53<$DkAI1k%fma!a{+fC3DeJK>4odDxD?GUKtw;hq?3(=qrEp1=!RDn?yB+(8|+% zm69J=iv(SzXxaW6dAZK@pY%yBH+u9Fk4Gl8;T;A}8M_OG zh&4x^xsghsB$k%rPDkAs9B2~u1K~-ivRRcXG_(v2O-=H?QM9!E&}a(H8`(_1QJJ@DZQ--0l2e??5gMwa|N z6==r?t{F&0=1M zP7T}#{7)i;i+6+SYf2Lvl@&^O0kC4$ljJ-6-*c<}4oZLN26_bh<0vJiisk_D@8q6m z!Xlxefm1_612`eu@?LLLrZenjpN1s%I50Z=&uLfXI?dznKO`G9zxZ$bewauxrB(PI z%20*jWb2%_ezuSegO8DB3=@XFn=PN;r}WcKHV$aT2Jy$K~k7$eO89_^il3V-WcdLwH)eZZRe z=7h?M;6zbYO^X12d+TTv)Vok|nYWv#hhgtTpi*0s&e!)Iqq z4LZ@zE#rV)(sD8bzgTvY(W)2~M-%tYWnwdJ;JtWMwBUx(d()vXydTQqhhyd{t*u;# zla0%;qL%lXOBvIf%Fp%9I7mV$#%^1W=2I4fi9Ulyr!p4UCZU%w4n)epo`n^`p%`LI zR^GAccE1S#<2o%{NM4`1!aMQQ3Bvdy0}`DHDpDmdO$=X~Mu z?Fhm{FngdYZW@~((p4?y5p!J%vlG7{B@q}mITpHw?a9WcgcoqOcsC+L%{CQa6z05{ zr9(Ri1~gbH14YUVoVT}M&A=l@RV%>$J5p|0DNB38DeS0uuMW~^N|EM=uaQ|#B>PbJ z451nB;)vafbN$hID*IYEM>t%ZQ>dOcac~p@XSLfAkd$T4OfVgSU@LCA_n_jvY=FZe zhS7nLA5(IEUFCIpk^Y=Bi+l2g;tB1Q6hIV|MoqzY6pa^iSAd^%eoRmL-0Vq{ zc;;_G^BA}gf+gGi+!(Q!9(@Jwa?!2_xjN%y3Pf7;EbQ%3hnybry-t&T&&0jS8u*brkERw&G}9B;l?Tf*Q88|6IAkmh24bKnc5 zJggAdi0n%^Q4TIIP`?>(oNI)*()N`}EE6P|Z!hrwD~$E5i-_0AGyQ^+h~ytW*%~}8 z&hdcoPiXZZoMuJy-BhlFxR!QJ(ZS8k+>iv>p?;s;SYyzN>02+@$l4y!S7NBWM=X0< zSp{jKtAzcTGT}#l&a(%m_Xv$A#mH#Y$gcOnq2=&;TJEh}_{A~Hz96g1D|6H3GUz<{ zMYkCa0oq%Q(+?{B;n@9)oN7HVcdF5M`Jwnb~$%7G{xD{&Q z5_A(Yt;`@MyYPyP&UD?07zr!3)^&Q&;f93I33#o=J9T!S!X_@(Z{!=j?&VbtjG3n(eO{ejW!0| zL@|C1T?Dh%haBO^uTu8d&thUeSXXl#L}f?%S^H?ZU2hYt`Z3=s_=s zr%Z3NpW!$C0>+8o(saAd$Gp~D;j2ypQRg~6G_m+h={vZPX?g-v#?!8&zRmH>gOKQs$R1o)** zFlR47=I;`%O#?xLSoGdD6%c~}?X z#PWbuS{q&!cR~}BJgK%0mjZuHg4eXaiyJ&e$%+n&c1<1vA8C9qzZG7;6Q2GQKe0j@ zPy;H@3T_T`JE__XgdV@+x=(Dh=bfV47cmD%iS*kE#@VcS3!o&oGMt<%{EBzH5E+s;MqtLV#2^2&uW6|y#{Lu}1c?(Cf80c?{O-$2)EJnUn2geB7#SIX z`vjoWb^w+`{2JSL1~o5rb#pJTw1I(vAIo7di?{9a{EngM9$8$!3W+{=eP6z-Nw#ao zO&!v=ip!jT_LFTDNQ>nd;s5pLcIDHle{;-}wD7|P({u7+$%k*|L2|m;0Sfh-NNeul=q( zi{!U4Bw%mKZ%?iQrLKv%PF4y}ittTU=*YR3CgW&hkrO+>WweJutM+{n>J`6aGp2lm z@(d1mnwGql(wiCmDV)F@IL=VqhSgaaz!pl?w59yRZseJ3Z)BYX^Y}XVIg#dTK`JrO zV)g99q1;4T?SKfToclZe)e?kuG!^*o^oGo6x64kf&S_<>*7;!SoL<`vy+Qt(nY1ED z9EOVo&hohRRKZDZG@Et_yTHwg8l8Y=4-H0-OU_zSsq%WP3VBYxo9*8f!v?I zyURvzE?N$?YaAkzGVm}6hGIkS|JMJtbgd&$o*DP+-t{jm_y|nN z8r*!Ao7w9fvw*PtU8lK&z9+j=s zaSv2_xjGSrr#p0}0=PNwH7=Yi(w}+5!qa?Gx@LHiZ&7SBJc;iR9J|vk(tgWSU-WZF zz2%Y{*G_2JN;BOq!jbxC0OitXseVz?1ph9Yy$+Yew01>6(h$2uHKOm7GGqAw)6((w zz*{9Ho6|gs9Nc)I?6F{xQYh_ID^;K?oB~yJ_V-m_v{mGBg+&CjSR0!ZW?>oQ|J6!-MtB8j-q8YCF#zC9I)I_+K6gA@BFj`M@>rh>s52k00wX)r8+v1%f^2 z#j@vT208iGOm`JQDgTsH)7szPU6Hmnak+0ChQHn)(6=WsJz5E_NW4|c_#M0m7(7z< z*iY1dyG{J+T)3mxPye9ve(!l^uA7x_kRUl`1GCY|%uQ7vh+UR_XV$xW)mo{E0JE9` zGE&}T@85QYDr{6@WX0)e(K-@eJc#k1@(}yXcv+d@?#Wpcgx0w`2D2{$MSF4R7#Y+vlD^+FCm~|7@8RQs;?egRGFCE53E=AiVoVXr@c;PqyAlN;OUXDN*I$ zKh7-J@l{H&Vs-(EbV^#9h`PFkr6sV#N9jOscwct3AIm#LE;a#<7<7sgW$(Kq<;};= zM@SU()r#ICq4!BZFYBs*(e>d6Zat=f(9qN@S#qz^+jVyQenvO7GPTXQPu6w)47XcF zV)?Iai2g%8^{WGK-}V;YPi|ZGTbHkDf*R&8-bEOZIoF|Z9>3Zcaa;2KRmN&Q=2LSA zOx_ZYODEW!pCqhNO2*9Ef!qc}_-Z+`N#W6m#7gMugK!L1L<#4rvzh?X${6W)K83or zXw1s*kf3zto4!nw+ti^VkW0(EuW0MI|5*-C2V#hE!MM$E(6QDPaLY8`yAG* z&s(8`GMm^9QR4WM^9Z}5D}!~%pTPG5hLd7V;<^l_f#t@Z&OJNN(dPT0et(oQv5sb2 zDgW!Ch!92m860-j6Qu`oWr7tLf8Svm>#84TA0v7S1)N=zGU;Z`wFdST6j(ljWLIxL zp29%pTPF*jC?x9UD9>#B`?7arSkN&xuCK3)mn!qqljxLw-Wd1_rr|HewZ8Rl1L|-ob2prMF~;xh+olhfXFDY#=Iu%G)2uXLvI)aY>cy z*I|21Y=hDEZz5p!V=F|7Ox#h=77}pYJ5ncYkM1o4nNkP_&MFrdoiv|Ts@b>(rl~2% zBSq#)k>{O~%W-ciVKO{BOk#e66uAslJ_hWM2ryaOmrg96_HT*;uHzwaMh&uOoxqR1AlLvQE zKY`FLJRT$7H2jfYfBj`CM%J3rSu;T26)U+Kget+LFU>ACnM|Vs|K3}q=PoWJz*sBh z{I!<8QWrF=P+F+A9*GRO)WFY;V(|km00@0DG3tT5f5><%%gZqikE;Kl~_9K2>^L(mt#3%7g|3LR$NKq$8Q7AB4a0LRg#bGjzFN^96E3MYg zV2R&atv~HztN)FSF~N_G|9i!UVkL9&mRKZ&&1G)k3fzT>vmw$c-Y6YgUzh46-d^o- zJS8~_7cD83p@_>_JC|Vg+M(j2YQ268^|ak14QP|#dYDc|VCG_iwgr)kZCcnI%B8aoS z@z0qVrX_kpiTr9|#LRU0*Qhqyo`#ah@FpJdk82NKmxPkBveOGt!*?8d@y4hbuweyT zo2UIwHc{6W|AHkgBOe&AL@~=yznR}kj@jPOj*|Hqf#I@jc*&c7ytH)3wv(z+BA{A~ z$_mDS$!?y~hE$CSr|O{+Z<|1}LSxtNmMJ1Vzj*sMajUJqieUo9M=hTS?o7S{Fq^En zTh=Vu-8t#&Byj1iDmIOL*uF=~4W2c<=q_8rxU1_Hyg7Q;@oNa+buXt6No{4s%$JiP z6#KM)j$1h+?nvFg3c)e>VS3*bh%JUQ`TYF6@f`1sl|-4IQ^xJeChnQC{^`EEA=xeY z@8H5TvUlIZRJ8x&Q=$1}@?`lw2RHPYu#PPBw6tq=_Sq$T({`RYrzg{Av5wae3aaOy zmunCNq~Pf%cu$km_Z90qC*rMJjLyH^YiIy6IkwluuXyfY!}Kw<|AS5OZ5eCinQDSI z-HKj*(k=YHMc{3BvlZq*uouA_;d*|D4ThnfaY!{u zyvfOR`Ihe!F^?EJjB=)Z)7n7e`O%v)nhl)v!|jwHX#?NtDc;-=onoTXl0HxNvC)P- z;D_BK9iICqmGH8)D7}$E;qOmky87!dwfe4T_pxHcHFgn``EwJER5{J?s%G|jRvGoQ zLEnApjYI;OfE^Xro{(TJSv)((m+3uMxi47L<~UM+;ehM9j&cHXIFXu$CRY`AKjHMkN-`C^A(H4VwjOMrrV&7nOrt9yZ5MdQpW7JQJhw zir7>3xtz1@MLnC3xNqizWZ^bBUzU91r|BHVih7KH`Pht zZ8K0-mWKU!{1e?pq7$*34+8VX2*xlrZ}W%{)>fV>|4{Q;*|tYahWGsBN_2>H5s?{Q zl1em6Y|^7oRt#JIHbVgKzfJa%KbdRD+rQG3ZN9&xiT)w{tAjSG!E7X=xw4+x+cgzJ z=RMmTqW-spA2aJ(r=GZ;oor4=m~kSPf=9Ue=9_{?T_sgZZ#?e6i^bV)-^J)q*xG9D{(A4A9$Xf* z<|dv0c^S6NVgI6M?W#JInYECg(^Zw(rFbv$9T64M_QN;&PIi{8o6pQdj!+pPOlVR^ z@nes}wyOijUh#C7S$e(#!dGM{h0UzOb8V=inMj6Y)vqg>tye&Jxt3NzBvAgo(y5oA zv?Q04yPb6CYWG+1U~u`)>E`GrkpfC$7(_TH%fLvTdZH5(?e}y7>Bsbdj zzkJ5iuC~cpR@N`Kjrg_hW(Bp=4_E2&dySp5|9Qt(k}XrUGP~!ifC6p+bv|)z;*H*} zw`hK_n@DZ7>qyp~nr^8O-Bp5CS2g$dtXk^H*&W025T0&gOoy^ui7Vjl9M3_&)5Eb~ zEgn_Qb26?>|C19<<0si%TfAU1|C3+@TDqPdwIatjCiQ#PF}Af!m7z5=f?PJ!uW9_AnxOdyCI zr=;oO1$MbSytly7e4?>PCMy5lysWGISINMOG;!65v^eOf$pGoGP6!1?7ZD~Jjf3j? zQ_ibM@|**|{6zzIyktDyS2&9qG5ON*k9WRkNrLi^8LvIPBfs3JXPtxxWP2#t((cJH zQIne&R7FDLLt||m!u?G$-tQ#&U!7IPyPPXPK3lYjGUEB{RC&2NZP*y7Bd3lV_D|p5 z^{jM1VN?oME)F_`3$m)kU`F&8POY(~NKYuG(QRw`lwMZIP^=+JWlPQZnWrB}rj`FI z!mc-5Fq4)Q8L;*GZg@7!4JLxARWOz2x-K_0R3k%Xg2xXN z_T0I=Gx1<{b@cDhzw7JW7_z7k!2<&h_xc*jY=w}$SZ2XYo?NkUY?V7)gxg+1z&R*) zjuMf8cxQbKr{cn3qa!|1!cN3oxCx~)eH8lOiH%mZlj|eblE-H*xVY6zLfc$%j3a9+ zYviJk?)Y*#w|9|Hp`xSMB$X*O<5MoZ&&mh`qZVxApL5@UpGImM-^?2rm#mRAT)WbC z3DWx)hDJif=W#XaKjRw^sV>|loa_tEtvU!>G$V%-E$WSe`}Mj=G`NS|sg)76Y!Ku{ z$p5LE#}?>p`UGEVjHb60cV7eHHhJ*~7vmoma7L`nV@j;^F&hs9D(_M675DDPrEAX( z=&+c3S4)3Lu#c=_q`>Nl6BG#Q0M|V|3|Q&1SB)Bxup_ny5c=K20r>sZnm_~EyxEo& z?rOHJ>165?HtFqjai>*Crf`L}>4287V2Pyv97a%gEp?ez=gSkJ~Qo+x6|FD6HR?8pR zJLNBrb~v^->>)S&{4%m=!e*+b1>Q%Y(|W1w`5Y&W0Vny$c^ku9>l(?dVM5{aHO+{3 z0v;@M)v@%#;=OrZ1vGeLplpQ1msT-rJ@L|uzM3ftkXu5fO%$q01&C2Nna^hJ9uE&^ zl|r8fiS$DkvWPve*G#kxJC@v%x$7=g~AR{ZM|5>Rjp!CxqiR?GNaH%6X(6PNo?S3~n``h$pxv{)L7 z03+K~D;5j~qa^YCvvMn{nx3bR68{~sw#xep2Gi39M`%u%KQm+~&xq%3 zIOmN|5MjMy7up5SehI%-BzK5%7OI#DKM5cq+AE&_ zd;})iN1>=3*^&-lZH5lk?}_J3?6@y)fpv5uGQ*!C&l$BM3?3!{h0sL+8;7 zfFg&yw-UDx7e0g#zNE6t3YE7t22%&3K3nfN8xn+-O(A(H3g0VYcx(Q}yqhc}7^8KI zX@LHlC!xE74xvBSrF?xuNRHB}7>utGJ4wPCObLuvxzZQpQPeW5ky=k7^R@&rAYT)Z z9P`fpwxJf2{x#>q+bRzlj z{}x(9uT|@lZoCC$Wc`^tpZ3ZR<+jYr$6o1rJxc*DW3UHar5YcM3x@DY)R9Y??}aYwfk1@kkTAXk^=a7D$k+CQXy3w+KJ5sdy9 zBYTU#u*$Jq0nxJ(VjhQwtb$~Wb{P4JC+P_ZeOrc1)E-Nizumv_uke{Y&`PmbZ1BJS za2V_9_)0Su&&7=%lm%5OBU`)Xs9aad7Z^HbWwdFR7j0cHuo8bSW)N@%1K`cY?M=N; z=|itFF3pHpd?cU4G3DNlunaa#ce}u1RpM(asB|&0Ua!%P$JInp&TVq9VW8Aw6bfpf z5vB4RkP)i$;C`Imk}G^~`sWCkRJkyf zl90+%A^{(d9ABlj4GVx?)bZ0Qh!!O6A~q1HTnwXXSvyy$(7+He!jt%&##fGVX({(r zzC+KlpfN}w*R*16Z*MPCng`7t(2W4O?Oz0HEGvtew)|*qZObS{+z^))_BW;)4=^N} zbyD!Mb5_eu@Zci)5JMQ)XiyZoZaHKm;@2jlhzmF8)A8nMG};X&q&iGEO5ht*L)yO-4Pu zchY|lJ$+GWaWxBaHoCWvSL<2tX(_9;S&>dJ)hx&bRckP{V{_kL{4|8 z;&*+)8i=46FuA^g$FK2PgWLy&5CB5=z#TrZFGwXw$sIqgw;CVzWC66f+(EIjy{Y!i zI~<`pX|5Z(i_zO|)iYfv!nX>2+wZIa?Pkrd>m5Y4I<(?l`-Dn*seoqL)5AEH$`A8b z__=BfPtbh$(L;s#p^cqt!r_Izr+z=-?S$!^cCC2~z+`9Mk?hYYe>&Dw+eICo9Rfp* zsX`h%7G>G2{*lVZke8sH!rj-;cl7!XrE(0w<*xJMPdbl{Oo3g}7u@kN9?1bjiwvKb z=;$~(8f_gN6EHaDtL3EQ)0HuM>Ud%6x<|#CW3gJZU2*>>RzsHL5{)uIjr8Y~2NM%> z{`B5AHN_`)ID&{<5dPE%9<*^^$%iKYN;QipXYYWn+uV5y%uHdJ=~b}0w*VZDBiCof z#~H)5pIfM=O@latx7yqT6fIc$lmt3;3ys-$$fH#;SB$741F@gfy8xp!Fi@xY9$X3W zfYoE4PgpKEQ?~{F14Z(ZC<^1(y~E1F4v<1W^LlnzSs!qJB*bj`dJoXJQcx%AmMyUi zqvrWpf+q%SJ;tZ>JKTg6dU7omf!-qmFtqo`VDpSnrqZ=WM_`~XU-1u!d3wzTALWj>is|)M4&hsCge%IOlBnbW7qB$&Z zCCLSOzGoWiQC?LhbhFrh;go}Ffvcw`2#dWy@yPkJ5Nt=Wx04o#MQ4==5hN%sOl!-GM(%#oOl9!<}nIQ!5O<4bgcVgQh{1o4UJn^4_K{#xRiPT2y0K?5yKpJ1d1bpM6qgM2Dg zO2ir(08N@4jcAP)Rh$&c{Pri^xITg{+nz!`6j3}p8;4%|*i0P>*CtJpB^!?g1WG;n zaVm>tt(BZzHz8=wKh8&*pNvU3Fv8B9YhDP;U|_~S9{3FAfbNlDUxx71mCTVo;7aL0 zTl9d^V^3^Vqoj!5F`Ot>ic(9s5gM~ez5HlCsl zWXWU)`{0wPT7c=t)_~agl*G{*i@<|3mPS599KLHe;kA$`R3vawm6?b1;%;HOdXV$S z%03LBQ!O#2FUOARMs+m!{^F{XOHkT<6GbAe5TvzQk(qaY9F|_sk&&V`o3`F63RrT`liX$z>E_V5gcOr?IU;P@; zYTSXUA99(4@Px{~g-*S|G$+oi`o~JYbL(yM9-ZJnJ8HCjK1!MRG^w|vemL`mkT=%(uGP3L1}bVy;wMr)vme!3?ME;de1 zU;TQRRoiA9fW5~vrd8Hsp8d=W`i<&Yfzt95SLwMJWo)9IJ$UQp+J+=mlHH8GZMTyb zK#4p5+l@sWb-yxeY^bmJ@NTLUgaa>*T&G|BU^1Yhd{3cjI`v;y^)|gP?z?Z&QAmK7 zSc|d&_epFTjP*G@jgzkGJD&ErK)zxnl z;!9}gtwIyURA{u<#H22({W^6Bt&o#>m~UDLe~(oaT}Cr6k8#l~dz{^1qw!4#xS9m~1<|IJ>y#mg8VNd}<%dC<875N$ zknQQEZqfTAps8w?1b;T)uMd&YrDZBL4Z(D3qqf5-=MlaAm2tY3v$@0WL|KjmIhzzfu7TFP$vE87D zfzXJ@WFWy`sWD9fwel=y`T97Dc3GCUoxS9*{l(1mnaxawEKkfVcl6a@F6wLq+~5$Q zKq9@fz;B@gpVH->H9KdXbwy&iPI`&CU?4*eS)XmI>YIJ`SdJ%?y&jezD4MHH>3xHM z3pJKBa$fO_v+_q9AjC=gHhO*AWu*ayh-M$BqoYX=4t`ZVliPN}EK>l8CIGGSNN67JFnUfl7Kd z!)AxXS(zOPz#nBRPB6VGy3>;s`#rfqv z&Nv1kgF~|oApTBQk>)+HHVB{EuBWvpsxBbGi4^#(C08)43hW*`eVWuWm{3i3EvUh;*r7;&Kad^rJ%VMk;>KC%!wWA0#)tU|95r z3M+)+NJwVyv*Gy@;u)}i8DS_pi^3Y2HvwDPfHergL1zS0BO$U#fE|p;p z^i>l1Q~3I?(-o?HEK*t9ueb*<@J>lDw?8By5*5{sd^F;(@5W|1-dny1wT8nV-@4o; zcT`CXPB_7a#7>A!jv2t8slWj=4w~@uI*5oq+U$mH~}S z=v@Fx_#hGg44cq>qCk!H3oo~~_8Lk2gmJ_7Z$wBe`pT$Fqz+%Os>-6PRF$#R)QPnw z*-#Z&v%>1wzJP>jGFzN|$E!r;@QT~->8a9?@WJJcSx-F3whV-95(rR! z+{5^_G>TpJ1W?6(B8y`3U9M@XU4z5+dYP-0a9<(ZN2pVhWQs1OOimlj;%|Bg-NV;G zwCQ%$kc27)xAvrZ(}SD@*~z70p25;L9x9#W8e;^F2_x-P!A}Ch$5S$NTL3m`{rg=v z0_azqm7FcM#;)4kcHe7N{^3;}#%uxAW|-VoQ{yB&GMcnTTdJdxkZ-Rw66)vcxL&v} zdQ<$*U?e?$ex>DCNoIKm>>h+~3VN2qI-&mteAQ{()V?*fZVqLH-8bu*wMX)6)Wr2G zS4&YiIsC71O=b?}I_U>vhCU&*TJC?RQ zLps6>7N~7P34r+;NIa3ziQ5C^PJ330?r&hDvpokVsFuX>FY}?d|>W$a1*Mm=0j3_Bc$;g-~qIt+U2;AiO zFpvd05h@K>@&*&6S)_3rWBonBKwLwTP?EW_O;WMqg0d^Nd&wcXO1Hk7aPa;aJ7Fqn zp^mCq@Y%w~UwgJ%&R9o&ihDVv0J#cHg~qYupf>}5WX-t08e*`fi)3t2XRK?`4TXSM z=2&2@;GvUXWyw7CeXCIWZ3>qZ9iw2oQBREQYOPbvC0&3vCTn?xwH{Tcj(IeK-AkkB zHXj8BsU3o^*teR7v<))Vn;rVNMFb%uGFa8kjM+*Naooo=?51rLUFc+=9w_81I?mZQ zuP8$As8><19HQrKHpi{nLj|Y&qI&dUsNC04(H28Nzl1cwEgM5kJ{H*8Xs^vwa}3U1 zBNnn!qzO3LQe(a7&}M$|hkbFo%^e{O?EYpY2CeJ8LF4N}0Qd~D>xH{JxHcs<#%>gg z-s29A3#sbd*o3-8k!w4^W7zQg_Yc`7C(LhKLQZ*7PA2d=;?a&1yCsvVRHSe zAQcd=^s@KRr8(TrAo^Z*^R~HcW7xp|>YqWJ0+arnivKehZb4B^npoC8Ywz>)n{)Df z9AM@S1Qi)`;}rTi4cS=X#t44G5b*!>n?Wj|g_f(~0<*K*bCdl31nacI z^`E8h&B~k8Pk<~3sH0I+!KJNUb&BSR5t_{}XkYCqYN1}ga7(eamk4d2B7MhgE&um| z)Z#D}#}{w5&!q{c$;L5=SDi9|%_x+qq5K`T=^zNzPd0vS1Mi4y7F8xMq=`beK@6t& z5!XW{dd%O}Ana4`4Sqah8?DYE*=sZUr*cBsI`pJ%w%}h$(+Bf|D+mjwd z)YrFf7ON-A-D`d}Ax2`Z4<|LJH>2Ex>S+ysd9_XOzy^JpkyTRArAt$@0--W_mGTM9 z_FN%3GRz7*pc=kLKOF}sR{0@eDa9lC$-|+F^{5$JsTCiDoh}S?Uz7SJo78x$oNG!n z`;o;5-HmXphR{UVtHiG@eq|o@R|m`n{0ktIBvli)J+%8%yW6TM)u&ac#k_&kYynDQ z+^W#y>{UKvdaS@Nwnmu^z%)h3YUk+qFdMVNxZ&=`DC^=uvd)mhK-F~Js;{m-qssG; zAI(#kCvOd-j~{Tfb!nPebH#*aIk0^f#b4XTM$v{F1U#!^_tPhN!z96D(aBN+utW-~ z@~c$z8JF2{XQQlrjl}j0R7*b#&dWK31j8k8Q)PPa2KG_dAYHT*4hL{NUsnEmOKUkl zs?h_k<;Z#}`4!Y1V&-_;-rE6g`WqJ-M*Y#Thr{xw^R~-ic_0w_Ue0$*{`r8b^a4&0I+l3JRV-_zC)DMEHbw$y1hoS z={`nwpkpUPj3>}2o7}J+leBsmIO);Fbs*qjN_4$Hq`EA#H$D>$C;{b1^!XM2qgw33 z+8F64rGFZQkf<&dDbVGbESq1kAE!HOCT=SZAyO;LsX5C_7d}3swSWH+G&!+vj_&As zCag4YIK+6MbtdFqFO3o?`(vRl4bHYPu39DX84|vu=YN?%mS2rq@^e5v%7!o80?7k> zeKadb%hxh{eDR}F;XPBAG}b`!;bw7=QwA-WCGGF0vO*_2NvWn@$8T!?I@T6Wy|Qq% z|W8A{F_IO()ax74i|zNP5nntW49|{l|y8hLROBO2?et@n0Dq zew<}NFL`fNm}@gV%f`7B-#$;~Kt~y*%2nCH<~+~PjF8IanPGvz>qdV%dcJ$5-Ko5k z`h;h8Klhdy<=+5spxg50eUl?L!bNR^9J#8|H7?GGpKKB!hZt|Sr=e&vURL-k%l_YE zV6NE+NpU7D(aMD2tl0TY1jcI%^Z~oWK~IRNe%2wCw$b+_Y!=R5_kw(0+JA*E&7uG@ z1mvGt$KDd{IdM*XrzUB6^J9yYQA!AYZ|O_4N%wiW(G6(X`CW!C|6c1{{{RetLf~CU zmcCrovmM@grTR1Rg`uoNZa}-(Bad5uvF!>w{9y^HFnL^7LY^CX^u$>t z7eb+G70U1}XTo`R9%C{?=Woeq0#$r1mxbC_)O^E48Wek=orr(!oct*98|pS}HxOTf^ej=sR=i8^Ahmy0?A z#$&=}DLeJ52hXtq+icA{?dfm&uh(62!_Kfnmt`&bq;@NvBgFWk=j!JR9sj#i@nBGDq*RLX6-2d20qAoSq=b}n*tga~A_@;B@C`DNw+q-YD$)}|I^=);{@upv= zcGyQA&V3@fGgt~qVf-Qn)jz}*4YLIiCsX{Yd$W3EJ@a@e4q1E2@YSs9ToEmlYxo8L zJ-ZDWw7nn6TB6$|Z9Y^eA8XeOUA!NW^^sVa$8<<A_)T*RRs)c(08$MeJ#`lOOHfFEW=p?E?+G z?-@eV9__-{=~2!>1;k`(!mmjn{erQlgx$7x&Poo6|Am4ZYsz4n;wyeN)Ar007Sg?` zM(jmP1Co&fPT70&tI3w(^ItrK{0}Q)0zd!2l@{(K>Bk(zm=LtN=Q`k|=U`wYD)kUI z(x-=etzj9Mh2-3;m38H9M=77Z#`^ycO3qgWg2p-(sWpHdyAFf92G@aizIX4fnqO0Oew}^RTD!Yf_xbly3OYC%AO_(e<4pebA8!2H z+Ao?M)xg<%j*33`*wIZO|8_E>k~yxg`t*%|7s|3Pvkg{93|brF4X2HTE&pPhTkmRm z%0*jU)hZ5iIGyLlPfD)fe-}(f&2j9Cd?x(E^~HsSzeX+GoYAlkI+kJstnvN?cq2Ri zaPkJS`Z}t__alfTZ8&-^M2A=;AMg#}0WXYNHLI~;NkjU#pK?aiURG$HByV%mcmdre zKq~r^KSgD~gw5`2`@-HihgY-2!IDniNaBiYIP)b0id>pwUi_O_;LVuq>PXD|-enEx z6s$V7gfYsnOr$SzD;seMU8i;ht1&?ZRHP3po% zzvU_hhqmUNXGZ5Y=|d7}Y;a>Jo*QkKzv`4-bbH1wqmrDg0emv2WTzhM$7+0oKv4b* zu1oBO3%E>rn*#OYx@vc2JDZ=QNOGWDtc#4n#}l0NplppL4=T9ubsDo~Xc+<|3_2x~G(IN$E4*?tu^d+hsK~6Fa5z zr=loH2hbS4IX5bJ_uI>$JwPsBu8kC>X`RLdC(ZvOBFRMpHgp+>pf;|fsXn8PJGWRl zIP4mZc+C_;CnN7%ibuPHt{aJJ(DnBA-xkaNB zr?6Q#2x}-O63F-d=&M057i7!wgKo@?Ep_?)FYyFa57e#DBysUSZ>siJBZr+c*yIh^V7YW$a(<0WM=8TuRO_O~o zzw-8nJcR{vn&g|;C_wcMy|(g{|4JX45e%tvG;ezJJzf0R+azweU@eX54-Vw?;(wlH zN41^LZ(JY;xNon&(DZolu{#!M+7NcPpkbCdOp@9EnG2Y+{3vK<;do=b*Q_PoZt|j* zIW9Db_ugW)lS4_0yS&Pp9VwSPal>pnxQ=c<#fB!9hc4aFP`19kY{PC(!dp`&^|Vdu zMm6W3C--KH3<(MU_?9w?N*$*hJBvQ%=FXHim%%S~gtzx56qNh_-~FQs37@&ZLcxQ2 z{M zPQ=|(w(v5c#2(htELm=4XN14j=j2$W&02F4{Yqg&U{~$4`m9Pw3R|A=uN$m+fr}qG zM2iwKR6xPa9O5|$amE%6tEd;jU8^$wWLo(*Iq64v$er0qgpPb!&q?X6%N;?%kd5s1 zCyb(X9@#q?S0FKj^j%RxfxnguMw(Fe6vYjv1LOS+^U8ywcpQZ2h=`{b5ulM+I_}|y z<0U(i#DP9$>f;hTzn(z31xO4f=9}))*W*@iYjnVt`Q|Y&uG%nOGNplUG;H4K4u`$g zBl}WvU7xpjwQbv>j(ycb&1Aa11U^1q%P||&wi4vj@5fmJXBfs5L4BzK!)pTaEm2;_>RzxVPcsrL)B;`zz$F z2AxZx+7gtNJ3=7vZ~Yoy*wpC>uc?LLyy*>#TIypJ-3b)|6;&pR&qjVuPANVTi?Hn6 zhQ;5uMYLg}LD~+>K+u;(DHzj9k=Sy;*yuMWymq#EISgZ)@tlH!_p`cn+)okvN&WFO zaqv*mw(e$b zTfd@s|9gkKOL_H$NX_CsOjWp6p!!XuZ%ZJquvX0DKVIA3Pi4&taIWXoSFIDb+0FBw zhd15M?-Yeu{)h^{)G6--3faCA@_J=JqM^b;Y|+WYkqcOHN5DF>k5Uu-C6S=wtG&S~ z+JcbKnF@Ct*T^~m`5tym8Q8ekU{iDSM)k7GbU33mZ*4Vuv*~3Er_ZGTJE;yV<6`lS zx8bt8`8Z|gT`gVHjlPu%Ue>YC0S~fF18H*X!>egM-}On$ctJ|Rj`7f|g{SOy7mr#h zD)u_A8xQ}`gchBFJ#q~fRx=rH4RoQ~DqvdY$a4hUPB6AUIT*k4Nna^^(Qdi*1Xa{e z00$tpl_J290sa?y=459SO*IF99YMo8K;-(`(!lMSbsB?K6?rxAKNuvCg-N02sdS5i z`BUU00$DBUguOQMUuZIgt6tRO{t@5Rqgl#ppL?2~OjJ^YClk0;UiOY*h$ z7=|J?`FL7v;mamUr5iej!TG~Y)zr$3u7jUDE=x&&kXvGSQt>M^aIS}~tw@Z)t*&pw zPD8FQ*tr~vF(b+O~pv>#E6nZ4aQCriiu84+$}GBcll zsI0Qs_1+dxqiX$i!8#7aTU+XKCXms0`|}o^wu>s6OVIAo)JmRr~WYCBtNFEs@GI zn(rJ^?vj1-A*NFedNAu~DGB$0 zUo%NJ9e1oFV)ENKEj(vQhe`~fNc>&k>-8CIbovG5@CP-ZtVB1Oc-Y#a=i;#Qu(+th zGMRS%Ep3$CzY~k&k_0(yj6`s9rnsi4N2|xJc2N#1?;g=MlS1GgbbLwG>DP`qI|Kng zI9OT=u?1goGD|At|Fi%c4*>y3)*B<&I=P70N|nM5t`5Wy`}qlhjWsi?0$%xX9Lncp|_=M5PdD5()$zj6_o6Op*a zAhwm*rL*RkLzm$Ead0r?Pc4jZu>JG-%1?c=cOsZ|K?Sve1>Eejv;5!qq4Q9cdFTC1 zj4v4HFBd<5{4_PW`8q4^Ea#J5oHZ8yXDs~hh}Sl(YyJHEiQ%P9(UaxX+8MTrPA~== zBRtcBCajtcSDAvepUd&So^xC{lQ6W@zY74u_b#}+rE_3*fBE_cZpA7*nF7iFP2MNG z7$c3(Ex}qGv?S=3Va@F)FAyDEUmt)#U8|Bf8)2zneFaZ2DDo0DH-&#=9CyFzv@%CE zQ9F~BIGblhs!rSZ)eF6RXfLE2nUAC&Whfg2c*6j6k$K!mO}%f9Xm`==Wn4LUr92dZ zij`PiCc6!1%~LXweVKhrt&nfawtMMwc*ob1v;(p~L^OT}Y_32KUk%cpMWsqd7uIf> zZ*PO8Oe%+kHob$`)yu%sL*8>@>Tm`~kG$TR**2(uk7*4ETIaQ8Z`2b3@w&cUE&p;_ zeXyllU9S3}^|SBQVwGawBTuOLBiAiOYaF|qrCjgjyaZ^Ezx;JU;MA|`+DjhSsnjad zFVs4$O=y;5EfcY^uY@{Ud^( zz*%4ny<}d%>L^9`vy$}p`0d=E_b^PIc&8fE)j{px$>GL8JlNpJZL}q@JM*Z_0L$X? zDHgtYDoa62v#PYIZLzy~bm(I}aH=}#ZNa_aJOrgfllZvG|3%#nycdfSSJ|kj%P8qo zlwuqu&mJ|>M@L7Av<3MZS(|$?`L@WMbrG^9Tr*!UJ)C@cK)CuYPIUHkcQ^f9R%WF8 zqy^I6W4jXMjpjBh(+-|IRrJt8GARG5odUNUXNPbF*&dRcvgDvSS`?7@JKL(XU-L`- zue{=`TjpiRsOP>0oa)QU#|U3{jJs3jGl=%t>pe{SrExTTV?pLRBTF}(y%booKop&t z!grc*|#a&kEo!o=&W#ZS07f z+vho~hLb{U^sz&(nPA z#Z}Ovj@H)U>3s1!Z$1JzXDSIgr8=IoT6hie^q57)xFM9jrCIvFb&MmsY`eDf^*2E> zYQMi0^4zSKFeE0AZIc8yf?EflCnuCV)WQZyP*?gZVU>;7t%_1o=d@%f!e=_%e>(hJ z7y9-loO}rdeGF$X5AfxglijD>V`k0d0zOR`eOdz%-$JY$h8#~`25me}u@O~$41$8d z4KbQK`(3V4gK7jL7783Hy5WKH!J!iRVnsk)2S4Y_!3BKNs+YXwiU;lMR0`1eHHuVPn__Rveu1C z-ty@tqW3cg&kd$T z5N(mxk~X^a+VpqsPG-P2WfS6kw&#$uKdtJ33d{TewOQ)#yVrSaJCg9~eFBe_u zS|tv0bo;vbtM}w&Ri}_`Rc5Ja+z5^TXQTFj=W_njF7wX`A;g|BanlbGn~LOx#y8c~ z4iAgW+tm*ebaY*fp&Yh_5i6R%L>Vj(s-pJH4`z8543PKVhD6NbS%fcQuyXgPTUlE> z+@6$Y@ITSZ3Bm?)+x`|7it=6bx4WOz4srWD!+-lW4{rtHq(3)+4PA&HPd(%KNgM8cz);psO8;(gLd~-jjoxxEV_mz>~Xq=<4f>U+aQ_L=C78JYa(6` zs}py3Kdx0=3IPElOXJ=zSnCB}Bu8WY4xx^b(KVv*ikF!YMrp?hSv8a6#caudl1i^qGtVOs3pi%1F zm{~uG=Z!TwtY!q5x}wyVuETrMI)sKhAK3PdpS1a^k0?!nrAH9+RMGjEO1-3X&SJI)uijn2w{5FdB))XXag$eP87B5!ZY&rf!) zpo&<9aW|D|vA1|qKO-P%^ggXRPX4=|)stZw%MZL$9p7JP{8u<>VxM}o=?)x%Oem2{ zdwYc&LxmWqV{DYYad>M&IN;*H2f!$Dd>3b0m}WLwoL6J<<@?`BGH9$q;3037d_BT5 zeVgBj#I`A?q9ml@SixWG9w}HW+xNlZeQj|x6LAL8@;C?}n4vHIn-C#o&Z2w&YzjVZrSEYF0E-mMVJ|EoKVWkj=k*>{*(* zOp7LAy+8jc2;rmXzQerawhx+KbzXXA3(~D1c-ni&YBc+Aal{)%DGxn&qE654#Rw0K zsBOEQeuc|KxZ?KWCE6;q_p-tn;g#|9b(NDPX`-Gz#EdcUm`?Mvac}ZS+bTZ=A+4>} zFcEZ?Z1W$JfyKNxI;Dvd?^F(K@0LwuJaa&ejPGt(7D|0;$}C}qIL!~90bwHhPU6c6 zdMh18JlgUGO8XO;`O5T#NEaGFHUfB6;WDj}FuIo~TLFC7hw|yN zwCo#71uiLGV?^2CNWGZaNy!?s3*YgH2jX0_R9hEipJ}k zs?6vv8`^iY4QGC4y|zE~JHM5C6eYx}3RuEcFkp!Sk7`c1Ffps`TeU7vgi+67|EwO2+)Pdd~LIU-6>Na{hv2M4fDzLl7PF~(l3>* zid{|u%C80L)G>A^OSLdLR*-wtu+_d5g6A4BbopKN~lW;*_3wsBJ!9l#UwVUe5!B#_TME{-FV zRzU$OPI~^;0-|Z&X7o&0tNbY=vbDAK!Y~Hs%k>PoZx}o*&Lv)QK~TMQX@}}kW}lsQ zhS-EPjl7#uRVi9nve&A(_L!$E-()**I({c6;uIAZ(_w$a{#fR6Z#fagACl}`!&<{= zW{dTQq9gyNImrYgm`oU*7F2gUW7!CpdRBm2qFw{Do8C*ufCZzc-k+`8TY%?^Tywb) zx5L-D&htVzxKiYorEjM^)3W3!RU&AVJ=xDWSWv5Z4^mP~e;wJ_z8RsyVRJUvhFhr! zz6%fe3X1VH%bM^#MM>^MREMRPC%hV`E(>OActtx>vTISqh3^za)xN*EZ>mKlh}Ugx z#*hD3Mu0GRw{+WNk*IY&c~={~JaT9j!+a3hA>?}F^umO$?Zo0x;)yugekE}R4dy3P z8pfl1+O>m{|5rBBMO>^QgM&poiRM0~l82?3?RrZCZXv>R5eM0)MxBNVR=BlJ$K8}e z9#m+Z1+(sL`O34jM$*3!ZPDeJ8)OibM4X(L?h_;ZI39{kHC_ubK$o!aRh!pGPI2Fh z5-Hh*AmCmhc^rhR1#U}xf$g}{)p4#)Tc1>~6@_*SSYWZkRg`fkJGSquYK!_`%69uS z34bPQP1QLOGiX^qJlXr3PL4wvtEq2g0vNn9M&tU4~+Ej~cd2NH!L6s7o4IVI_ z0)=s}_e)~%D%OEm<`J6m;W>cPPsn zF5t1I?^z4PgNC3jzui>YQeY0NRt%0`X?sUwvd8~eYs&_}V~hBq$9de*2%W_5qj zA-Nl+V?Juiwtg-pmP>KJk>KZi>nFA*nLg3wi{9>lY6)qE>>9m?sHo4Ik9{b)K>J4j zbd67kCFb6HU*8WBIl3r4-x*WVy{DSU@=jbc;QfH!}EQg)|e>ElpPGu^wBlarc`jz7n3>PZ}`!?L~3!Jo`zTIS3l zIX_ZS`|J)LU_OGUe(8Ls8n}RmxX$8N6bcwnK>*V%;3We8>?epG_u!h&{|LOGq$hM2 zeV>AZRd1ka?A*s;hvDJLDdPe8%-Ia?0aKRYt zYqBQpyi-u+QeEzBDBoj>?N4j81J4)UATUkk-0p*{p07|%eJxd)%6uz1NLz>fJDJcG zbHvI>I&>FukqiWlIkXEw&Z##v)`6csR*CO=f5TjpPG+u!LI!z^F&;2BO-p)l^$t+s z5oHatL-##uAG#lB8T7m@+a2iQ`r zk(!Dv%ZaYgE!C|(0e>;pdyGso-h{3~_8+T(%u^_+?B-sre!ec%;aZPENclMniafRM|l%*s;L9nVw6MOOF>&4tTAFsdC3b6Op{s$p89z|l5 zeChJyQMD-g35FDdE2M~fBV$(Iard$oU?;Ht0X1K+r-#$hI9C@c(oy9MpPEY~6j>a! z2{;K7U?;AUB=g5KvP4tNNFFGE4n`#V=Pjt>{*?Tb@dR_61Irf!;l9FxId__JHv;P? z0Us2&L?(|*DgLH4qa#*`A$v{_@)%$&h|-@|>O+mDeKeVrAFCggU)+NJMjSLheRhB6 ztyFwKXywdgfpqZvZDmkk#IA*lfSy7dvfCBDPMNZQJg3dUb21fVivVxczsN1n zb=3dnH#^tBKC-j<3(*Gfr*K7~pHGht`-9b!2d@4~05{X7tYL}?1C7+W*g%ia3q z*~h~pSR}U_I67%Xv!}ZI8Hb;K@4IL4cz&F`2Gs6o(Pv2r^R;D%wrMG+`TF{Y5zciqE-D74cojkXC@Rngwu+XCZSXSbIz{RgZ6sR z-rh&YExAFa#11?vhFRsmzlU#s`#GGjX#ZQwH%=Aqpf}~Fg;?MpCM}pF>y=T}&y^+> zY46DbGv()LqWRTtKZ%GI3)~Or)`Z}~J!-*DqPF!sv1{8ZlAUe@kp3Z;MTXa?Ak!F> zy8P;uf8~9uiR1uHKWk#$F;$V*Ur6P!{&@8Hs6^UsAM>)_U!-+&zFX^V>xIjLH2{ zRfO_+q)kHWhj#^J3P^R7T;X|6GSYu7{aI1M@(^d)XTccoZ-Wic#V`y$8*1PJeGbA2 z*eJF!0|W5D5&0;=DE-w&1QEPQC)%Mfstpva1t^|EYRDG123n;c$q)SpOS!0oT*4GN z)(sjv`T#~+?IPDDrDN`H^&#{h@2hwpUVJ}hLk&nsx0BFo=js&U&uA)1fF(#`;@>xApxK zOp(NBMV={xy<7Q4QY1Pt>AoX_B5DP<6#r8$68V?Hq`d+``<>E%b&?S|NT1*cp~6jb zW@}ovQ;ewlf%!9C6v5Ubi(w1JTELXso540~Ph-2w&FP9FZ>-4vUQ;$->AZmHcOI<> zVr<+%U|F+Hc8UnSd#p8n6zAZCP3QZ)#|m$(ki!aYr&D(FMo`=2Tn{^^`rOA1 zJU!Fjdv4xloxm?fw)1x!6(;M6q`(;s3uVtBwY^;%C2n=yIe1YZ6ru0#80y#kBNj)g zKBC|U>Q$cj$v5ZK$x_PeY^i(7nYG-wX#;Fmr(-X(GbcWneo%qZ-zy0`CS+BqMlGyi zmP!7)nN$f;ijODJ(MX8zA$gnp`Xba>`68eA?CqZ9Ym>SXJ(s|-9IC=cva)WDbwlxZ z>|OB%q!;7@UauZV%OE#CAoS2X+az=zENg_zWpv748Vb(1?qvcOXJCT)rC7^uKOy}b z&VfDOcO@b1@<9_PI1@t07t{tQfsp3s%PChq2Im?s`#|BktA)MbI z%GXOR!uqB_)0QyKwdhY(RI5IX!*6YS8X-Kkv%@NTX; zz8w2DS#V^DEk?-gK&hgE<>{c>C@Wzt;sT&(SX=2O^@pVrshAsqW7Inf?6SSAGP4O+ z4}1?^Sw3L7Qsq3&%DS5N?uVPJ!<5F59nDZHC`4PI=)*mGGC-BxRHIwi*LLldWYeLUS7U(3bqbEvQ&P1QmSu> zZUcVo*&q?bMw0T{3pp;ksPAJwPDwY>m9w5z;#?A`h3#1(sk(D{R-1FA}}DX2_lQl2UhsZ|zR=V<%o zsHX=CY%n#G#@}mJj3GSqW#_0^*77z-R7kf?I?pETkU0=YmI-PX+SS#tv|596LRU7y zo+-$$ju|Bca`%^LJBi*JLYvm_Io^`#qFz!#)!G^aAbF2w|VX^934(6x2A-|#xd1pXB8dj13xQTW3i>8|JAKdUITKpyw-=V_$D&AMT=+u z8yvf+4|)T}Jxvgq-ZM69he!pRn&mf_kz3%jPr)&Qg%X6GPC)+V`>u>BoyrZp`x zYw7S#)tw@sZe9Ly|26z<>U2tw*(Vx2lXE$ab0U@w*zc}g5qA!(`B4x^0%J-EEuj%tT`OMGji6gU;{x`87z3)4?Na#gV>u|KrEzEJH**!lIE zYGEqK6X811r**N!*Eu}1HDk>`*-ivpsT}tw=`eBgY@CHe8Wjhep#;C&&VM8}ua(rd zHikL(_g4JJ-?UCi?(3bh4D^Qoe&)UUOqKX2nv|+0t&%M0C^kJI3rP#;0!NSbFSl9S z^8kBrz7qJTwdS<%au}PA<unt-?=nN0{HmeY3X>3c6Lgpa&@Qi1)^7oZ>_w2 z+v-}zzU~Buo!ze|lb7cn5863CxTc*K56m`%CPQ zE_3^NRdaUX-xFJrF4vX6+Nvobr%-R6JcpI;fRc-0Bs)5agxY1?59RTxm@Z|PofaJQ zwvQ*h^37@ap$@fySo>~|I2XKe0aLp$0qk5n=L}E6f&>{s4p%RAZek zF2^@AvhuOr%r!Q*KZ9$w0|VHYhx`i`2pE^+wD75uJRJhH(CGa667b@YT?p>zFVt)4 zzrOpmP3U@^YK{7TT7U!F+v$(-YiC>pqgtcpRX^v55(EwCV@=xY$5lbn<9JVTM&ea{ z%dVMAa=WsGnHs&GT{ zD*-}u%Sw*dB_z4K?0@R&cpwA_po@Rw!H^Xd7wt)%jc$!pty#bmIsU~Z`)skz2xMzQ zSqght(RE;$%klqIiKm&+CB5v(@@nYyHd!9FN&kRwY2|B~z7kCbV#7aj_D2fTwcOiSUvScpYEj|zB*sof6XkUYik z6V5rIB8#`pwYnbau3a&m8(3J-shHHeZ0vAgC9;#%Xoxk-%U7iW%+UE0yzuV?%*m_J zyiiiagS-DumDhUl;<5c^Z_xEu+1c+-2U9U7+^w<+5Jvm)v-nlH$5l9s6y+uXE``8& zum!}$qYIE!vZ7q@MFaPi)e}N3h}K;DjZ^Hk)J@B>eZzeZ`L1j71+#~hmlXwc?M0j5 zOG%p)C1I+dLnnz}dQ`#LEvfe1`iXjmUcnTt-9=)AP=PugghOQ;4{RWBMwtC{azeEa-U%w2` z@pMt)}iP2i?TTub#RVcET#teHd1o?D6eS!}C7YzzH-@fD$*Gsb5i`ZacetfsfPSzZ?U2wz@L*Ixr z45%~L?|_B(N^$Ai<>lqgljV8~y%o#GwxMv86G^2B;-MDwGGG9@-L?9wtQz(!YTQ}J zqm#Jm7&Dk`c6N5v@tQcx`$B`9T+E6i#?UwJ*r#3k_)e2uaJk?ymmZ*H?!bpLkDaP* zwsVpHj)=wgqD%C1-vt69gN-(^EYGu4h&L@(zeHd0gtoktjj{h5rs}VE)7)*NI{0Ad z=e7x3FROPxC^!vaZBcB*)vh+Bojp%PBh?bAQ_G)U^zaNlH@ zCc$#uLM2Z~x}tTq3C>xK=rr?yZ~T3!Wb#KNpcnK$qnhYs@^MTi@z?$t=pkW42xFCT zP$M-S?YUCfRjZ!qPnjS1Z1y2PO3}usJPHMYb8{v7M8!4;U35p)9L{&1gj{-_MD;X~ z^L3ZHUJxT5LQDLIZ+H)IkbPa_8?a@V;a@MjX0%&u8v`7}W?$^)Pi+O2Dpegf$P2{q)gCaIhvB7S>H^qPKsE zOa00718K%@&jiP1%nlhkb07V*6q$;TA#r>IOi1ah z)-tnh1%7vV;!pL~^yj4;8S)FFbW{@tW^(PBFuTbUe!$2kC4|K1yDYL?sG*3e`26Zl zt#P|G1n5b8lhQblA5^ls6bklFFo7TuG8HMTT~{Nc05x6r_lwT%m%}u(Z4fIkWSV{V z4HjmCx!OJ3b_{DEa);kA^{X`Te^^!?u!YmYIB=ZxLx9dHlIM~jKpmI{GGr)cYf{{2vC@-gqMIB(c6o&&u8#dH`>&^s?yIECRrq~1 z$-#4eCK8mD{xw|r=C0GGjWlKqin*&gYO#dOx%Q6u#e&3%)<+BDsYbSjASavmoN2Xn#3oB~WjTVSa!6Trt{h96Bp0t2TJ7;WvTIhKHRvmfa#Xu?b ze0pXMb10f<-}~H?WZu#lO?ZqPDN#zoB&j25)Wn}uA|O5PT!vGa9VtsHD|xPy8UlnM zgFv8;`>lwBwu$z2*qO3Dky+xz!j9?Punhs#j^vx9efPuKv?A_;Gu#L zkU>ev?|==dI~4a0-FQBF5Z((IWSxGmnQgh*wXR|$p%%%K=&A;!ecq1>bnX(Pt-Wp> z!|vAE8JURE0GIx5pw#Gc=g+_>cry`}BRJ=yl`XO@W?|LPYtJeT{I#;76S*;R&$t&$ zlGhHoR=XNpwKHnQY9=G*wENLoeE0rAz(I5Dv)d?f9U~G!t|ngydxlbqJzZjzvW@|A z0fk0a#~6nFP7+<)wq3rm?&Ov7xb<>(it3IkgQYoD``ZJ$mB}??-OiBh`8<-h(PU(lND&rVJg@Q`-~QR7BMMBYm5CHo@MXLuq0F>Z%pD zQm$N}xwVg*r`jbL2Mo_OllJGxk3lpH7FyP){?L-T9Px0dlW5Yz*9;eT3Q}6Tg^Jfyz&dp&%ZIXZH3xn5(*ky?s_=6Yk4xBy9 zT&h^UM&So0ZwZUh#(|VkDlCu>C3{sRmQcRRr4wGF87walJfW}Z18ORN)pG9AcK(h3 zX71?nf2F2N{ZUrNl+zSM(0MvoE#^Xp7>w)KJZf<4ZW3Ty<#xoJ7TX%YOQsW<0bENL zw*G*Z`LVuG4NlD5PhROLazh+cZz-Bpq(j>~Bv;X1)e;BcP-)@bvDJd=y|!3ldxJYT z1(vOn?;iRi_*J;N?>$x4W~;M)vK)Wrc$aUQ(@KvrqIGpUrWGNFGfYG<2fdxRsX<$| zJVEK^-a)?=`I9Gnt{B}4W(CqV0S(H-tovp4>X(IfF9Bb*=_dBz>w3z8cW5?U|G|u;BLA}Plxwid_`8YO7>vXywA9jT#~ug-Uee_5 ztkhyxj+H%UdpH#&?-UfO3;EGY1iE@e-?o)w%^_4c(YBZf7P_pO(hWbf=KhXk9C3Fh z`<}{+RC3q2cwBfc+CHnA9ZmZ}va+!URrcdCujHjpAr+qc6B@42=BE6ro!J^aHqAZX zqgz+I$3*X2)H_ifDW@#J?i&g?^>KO6a((6C;7ADOV{So`2s4E1W3|)4TUWc0L(&*8 zVxo~8(0I=!?S@5;boBJR(A2q7(QYGD}X<}hvVdZV@U^RSI_mKr<@tFF_JdLy9 zqgGO5abZkCoJcN^v@APWtXVm8eAD7~Vc1q}y|Pp^o?j?_0T-=XJTz!T;M7&NqV-uE zRa4Jap+%3WTccXNWu}l)%C0*&5fJL|qD5Rq%n>*UTYLAs7Fa0kofwFXz`GF zB0UdR%o0`+ip-1m9qf^nkBksq z*EFPNE@WA>kK(b6Vm9J+MFhA(S7*Y}9y`ctJ0;uY7wtCxBSQq z9rx`@eTiHw+%nBt1o*d0ER*L`8EK#f>08~;e>Q$atE0+wvJNrO;%wqb(MgIxIEMS$ z1CNC1dA_$tUB$~s=1drJxXM2M3+q@Q;)Maw6Dw}I0Vc0fatY{d^7Lob=Vd|ND5g-2 zy}i8>WqM#R`cCC7S);U&m(j51H+IL|Weed1Qsp!jrfw_9(@Khjq)A@@}xG8&-SZkPRtOMd$nHHUAjXwJS zwrrHb7OSYJD5y^C=CnGAlWo-rMnHJwU4>~{X#4kKN&zelN%A!>y&F-2LJs3p3nj>S!Lsni({0lMXU1-&%B2I@j zR~I?**A{#liYkUlAo^!!dlGQL1uVMlCBx?PY1A#$5jxuO|-?hfmBAm!!p4+iZD}Cs#uz!3DBhi<>lpSgs#EV89iO?M2Iq)aL{GiWTUvJ6S0qI zc-;u+9UGvMP-?WhDx9xV$8&v^GC`Dp4_@F^MD7Y9|DT^OciYBmd|XA zCelaJ_)Gv~zaj6ZZDF$MD5WEKCPXTQ;~Ofu^|MVtsUW}DSzuXZVKv;sb)5h$)3J1K zWz2Jg>!;SBb}q{Mc)MzSB#U2<3S41ad!#|ONQO9EO^}xLGetLPgV0s-H%2L| zO@@sCPsCO!b5y^!qMAKfH9C6Kz+kE^{0S9Fat+AW}qg z&h%@KPJXGg%;23zUb1FaPC(*eGQysuf@n5$PO6I$nzs?^0$n>9V0Q47A-_V2Lc7qj|@Q*T1wfYaLChWV$*hTv8pwzJ2eN06o#^`Vz5ve7-Q0 zN$AYYCWbMir1@ea7KL5}apC2x#zV^u^wt3RKdkxg<=7J^^@>7svgyOZPO;Q?=}(zz zybZ58nF6kV(@^^0Pk*Y%D=_&_CGaon)`d(|Upa(8Uu^;2h^oQpueFq=S>>itqWohN z=I&j|cEetU-z5>2>z6WtK0v_lIq7(iP#v2~JyIWdst1`MytRX!#i+QG+~9W8eyN^h z<_88{Yz8fcg9x6V(-KHkC(?O80~J(m#OheWJQOrTC`D6wNbp~Cg|`LSg+c)7#K<`_ zn$^hHo(y_91^PrFrxchf33b%Ub+$#)e;(V{(*rhy5+tLyLqy_k@kzWwnvUs9e)WM| zlHUNF8DKtvhtzd+c3bzej7&{G4B1zyNAOm0C}K(^@+bA@Wk3Z>@g}m&u*G(+-2Y2P zg0c%qOAWU=?$5CA@nQfI#(%6sfa0(DqlDox!90i)t1Vl@3X=*0K}qeB`uW~^W{&ee-7e~!0otl zvf;zb3*LKrEK&ex5ffZqiw?!d zz~u0e)Qep=er~%?X115q-TJrA%~!BBF~7IP7W;JDxU5?lVlyMnNg$A@9_2?i&%VrQ z=}D|&SO4Ry1&J|)^Z58!yTdJ3P0u?qo<{D|{CredOa73+%1>H%FYOv-MY#eWRH|CY4&xSoXH+GF1LBk3S5;vJNPUWr4cV)7MzsOrIDb#8_TXr!qHM@UIXQnBSbuBK&-j zsu?e^xryFY8>-Gc_3|sPZM&cPFkgHRl)sWdeq;=Jq@wCBid-{Oe^~W9`z%p95a!Bw zAJ>V2{j$Mqo^q}_=C`yNqsEVIdb34e!W_SHaJ8;1R-2uTy0r;5JIjDi^=Zp1o9o|g z<%Un^w}{#d_&Z+e!&JEz47b=+LNy#qfJ(W#4k#QOvEWWo-Oit%Bnp{`h&G zpwB}bMSF6`COmbUGw`_l)1^)>s{TtwEFRvWddDU_?J9j|KqV?ubFLndOKP07I{Y;I zCGhkLM#iWx3_tYm-GNu#>5!`-Q5yHR-K|ZH^z45fieoi8oUC*(Ss2Ni@`7id6+#>y z%6`{Nq~FfmBL0x59Ilk((q+(>RXlPhNtj3k6veYZJ*VI4^R&cx=^IGbmVLdx3_r(i zcl)!y?AbNQO4h~-B$*HANAAMg*mIdW2jaR1JLx6d%Hustrq=SetvfKMhqENP|d|^b7ItrfUp`Azgn@5 zx1emg+GraS4@<#|Y$0L#8@Zo;(w^-Ly*ZEFR*N_iYOG%+bwR0uc-Q=inIr2$Cxq__J6B z%nUAhe$m6BY6%YkU+T8wKY`>EI5M@QY{3s;A*AaXf2Q{HWj5@RDVi_PX6Wfb>0=xM z%r;Kj`vyjjoc&NBO#IM$mRujch@2pA`K|;hfl|tuc8SQbkAf8KA{(0JCQDR17%O`G zmo%j{BzS^}R>I6g^0pOYCI&ZARG1ti{yOjDoqZ*=ypsw;&?4VRPLK{eYbnyd!rI`H zTl`pMuJBsU#a(X257A}x@;fzH#xj#>-5==!7hVL6IKAJa=CDK_Q<lxE-Sa^eZp#LO}H?V5V|; zzO`U7>X~|bdz;%OrUI>-&A?bv74wm;6FfDrrY=XC-a53?hQXEC8D^rU;+qD*6y${e(N4`giJf5%BV#E| zX<5zr8B)##@pdP~mK1uKFc$Sq*-f76Ped+iA_gu|h9fI*GubIeb6 zkhfZ-V%HpDwO=wmh96ffl4O?N6im2U<=T`Swt7~SLD8xg-;b&q8Ju|Icmv|7RAsFP z!8p60h4U(#AqBTRIzV@!m=pNTi7$tiIP;m?EMVhyO!fzXL6q#+;E~gtzxa6YqkA(~ z1r~28FVS=t6~omV8!IAH)_m9d^RZGLhE)9Wy54_`t(~lXit3$tILk`!dz?}MGdp#* z*Yc2{IOk`f4bKE9YY#9c@)*Qr1K1r<+$e%`MqG3x_3OT!!(M#087CF^V$3&ZUoc!I zoS#_v_sDv7S20awUTc}=rYAxX6?zZN{6n@IFm)GmsS5{o-s2U^7eN>i^_GW^Qsut} z!)_M-k$n#b8(TdJA!X*p17!<)724a&6b4}$&`G|+0Afif=*>Z7G*Z#Jep$p>m1a>Q zXSXuJF9+3B?2>XJrJ6$5JZ|4AeFfAQdI0i)|Du$@$GvX1-@yb#wL++?tNTPA`fDCB z{89RLR2`j7XoSv&6;9T3ZMONKg5G? zD*ZMOI(AizzNV>sR4D8y8&WDQFL?o1_H#e$m^~n^=yFVZ(aS*9*Ii`n;zcPkpW|OG z`upm9X}<-@(0>s>i@cnHyu5=?+VUe+FzJb2D0bI@6g6d6RVpL+m|bxhod(d@-)Oyw z+T6m*^Zcaa^pffBmRvc6N~@!m7S~LxX5%khxN6TQJlJ_l?Kgt}RF53i$O|c)R=$Nn zeq&NF=$}cw*LS1KgNEEd=tf-Sk(S7CHQ^I+vL_wa1cBT4&d^P=tfJon$9vtUU*sT? zW<}nF70$Lx5@Zw?>Iow4`v?9FV13SHwtVVzAqZtVhjf;#*Avqr^Fp>!2< zIcs#;+KI1!L&v^qIKiue#{Ij4Qgz&*6G==w6hl!J6u&4VOUa&D-LfE2JL(*= z(%*}}lqb+gVP+OlU8UC6S+0$an&8BbJCslcaH79djQ#v_Yieq$n6|cU{tJ0~8K2Js zRuv_Qx>a#ccbS!DD5zL4pB>FrNyt#f@Fo*eR%#-j{oxOZV$`7_rs&btDSa*G)6=hR zQ9b#09tKtAg)BO!EF~N5gQ4pKs?=n+H)-Z&q6&hJ8lrnA(uurv|SWqE506> z6}ydo^D)Czd&U-a>{43w5vQaB``zk7a*3NW_ZmD+SBc2^I`-AU6)7#R+A5D&f`RNB zlCx1aYt(oVonzWfTxAz`*3JJNE$ewZBQA{p3Y7xNgvczHLj6kbzhOi_9BG>EZ~#+E{XJo1Jf5QU z@AYccTHJtPGw&M+xvJPgw9-5qE4dy6c>}VWSpQ&)DcAQ3C)_=3)w3(a9=c0?-YwhB z)*zG&P8Zx`euV$%OOFxj$odp?uobtM(So%{c0DAbIz|#d!t5iD=ZF3o4LA%2oO=Bo zEdSrq{)4QNpfBD^Bv7)?bYbi*^pE5vD^lpwnjW0A2b$;xi4T762HZ7K(BWfe{`NnT zE@kuAF3fa-`Ix$_ms)cKm;oHS@HMs0H*Az7K;V*d)23x%MKteZ{?fziepw4xG-Sl5 z9}veTh|M&IrzgM0_HNrK^8T{$Bm5*jreoT-%o!r zlG8*0S{Y;RtQobVpm#^PF0s4r9w=+LO?@jUia^!MJ1`$kkwM0egZ4QIMh~T8nSgl_ z=i6>FaB1Qlu-C{Z;1VzND2{aC9_T$aTds@WTLDxcx%}K2J~qZv6&=PVh=6Lm0Bi2O zXzlVeq#bv4Q}Y`?>Gm=jZ;2tRiS2lha0A1 z(x-Yn)7~khW*i>k^!fq*mFyYg`EO+n?$T;0eLxZA4`D3sozY4i@pw<8Kcx@mVpe{w z?)DP*Qt((sb?lWl6a8EV^+DD0JWrWSo|swB)c1q#06zOb(kd$QS@J=8i58Ac0pwSR z()a1OSlKsg*VHwtdfgtbrAfVpSx^Ohm2WLc5x?&p-yNrG8dq-h1C)WA zk6po}eUE-CD=WwnjZc6v9pLJLn+D1b)QZO2Ani0pG*iL4gIi2cRxu%ExkZBaX+a4a zS1yBW*TIs3L3OvVCb_(O)%1d+mH8f}Gu0>?(5}1`1;pe73Sw0pmEH%}TY(m^Y=GP2 z80Wn*R?3FcL%k>WHo6pr8i-BZtOVS9Ww7rKE&A1a8ykUJAKI}4W?^nSHjLVU$657x z1z{krW?ilLkn2n;G&bRl-s|?e19JN|3isfO@7q%px8gfvR}{R%VoXSh98iR}hW%#( zSThA)51J-g6AYjiagp$4ZCq%`0*_ah5Ptf7D@lAg7qVRb#ucGWwDWZI<(^NqddGtz zE8hm$#=j{9?t-_Jz+O=aE$|=JaUm}*hxAEG=5ENP$o+Jerg@|(HXtE(^4}7LNdkOb zTE0<&L)83WyEHKrGnw|uffWQlORX9+H-~8Xd8i^Da1LfgJnnzQEabg2@Hn~yz)Du{ zrbG^E#vL_d_pq*6?DM_=hWVMpbc{SjlQBzW(x5RbJ(Yv3ZWpR?$jU054_!YyCRs-X z#Y=E`iLQo3huo&Zn4VBRKy{%%q)<5>h?R{bZojj1T(=b;hR#{SpPZf1MWB$teA_JW z=!`YNotm93J8McwGOQZ~dfD3^cYGFgw=SGx>E?^%Tpqd_7_<{HBAsMf12UzPtVQ^SL$Q z{RA}vc2K6RQ>(8M`JX&hFS_&3o6qmp(;<}OS9kT-N4cr)|F#7e1y8OnF2FCq8AJj} z;bcff6T!ff)l`yX*$I(lFfEAH#u|zKk%Tfz(};P@nW|tO6=0=EKv(=Mg%AEJ<0P7( zZ`?51?Rm1Xo>%4bQd>Ro>iuzWSR#o4cL&BKhr`hNgz4rsmP)<9pBb*{(hx89>?+xXfcsUif3R74b!#vVHQ7wKhL^}GJhK0X^!;p(Q9G<&RXB`RMv%1aSU z(N>P7wbnjPB?33i*@IZW_xbcg&cnq(7maK!#IPNEWQZFrhnd}O$ABig&I~Jhp|4B= zv!}kQbRc|-7-!gGZb#a4erOfo-c**^OOt>8@zr`QSdf z5-m~!PS8Y<`1W#4<09u>piY`0%LB{2v~`U7e~zW-SzOGpGX}%8s?P zubv8>sH>~4-i!L07kJ)56Ab~dY$hh)rPkz=Y~YkX3q`U@3$Z=Fo#!8WNH6cZkmj85 zawuY)A78U06;pC8ZsW#R_Nn0_T{6necs@vDU_{iE%!g zh9vY@S_dAPYS#AR%F*qg)6i?KtjF9bVQ65Gsh#Wx;GFxvSYW7QjTyG z1*$&imSIMh^!ZGLv=;R1b7cNn%7Ix{{pE(K|M`5*^P(Kz_#g=*Dl{gl3w1fVabtLU zbG++^A^owAmJy+VWP}@Rb-^h|DJ?l)dYUk&U0jlIZa~1t7&OobTt#x|Aoy%bHE3Vt%{3B1(Cp>^ZFs+K1qbVn z|5q?8&)|UZ;|kp6ne7@H4D}Me@u3gYvXr*4zFt^Rn>JZzK?2^xVv8Iqa)1L2QxYv{9}$OX58%-#^zO!9 zH|)SE14zJ3-+g&^d#0wiMA_3dzw5xr(I!!fc5Y+u@yntEqFUfkqlw;eP5=ZU?6sXl zqdZr}`nv7kz?T{+r=BB21jc?TT9MKP71E_jBf++d7<};4=`cWU9>2*jW!riWkEv!g zt6d*cYhCpXAJt5%_|RD?{ZcBQD|4q&T8~>WUah@Br!QiS&tgeKD#xH}ZiOkSA~lR+ zz1y7@v8LUBKQqx7OIZytAwvAL0zZa;1VvZ&0k)uB3Rp-G9=ig=8-$MV;2|RDD#Mri zK#JiS5+w(wPo~jRUU`Qf?~$@^o3{tD$vzt5pMg6F?AYH0i0q%Q1DyDB>2%5L-DK7Z)Aiyb-^6LM7ZZ?_tC;3b&-5fy#K$=zo$j+LsczTVye=IwgRklH&mo%u;W>Aqe zH9rgRs~{}R5vLl#JG0gb%iywbq`&hQ0@;AW_Nd^BWYq^$k!V8=q4QV;^L>7HeOC=*d zcxq98UE>KL=c5U%si`Tr8yO%GX9@{ZQWR4XknIpjv)u6FBGorG1|N*ZUnglyDviB$ z##T}}$kp@yfFVr+pMFn$Cq^0+QN?6YR2Oh;(TzO7jXN;*_QuCy(*H3Eezm{9(01iP zc+e&e*bZHruGx}>+ufA%7i{f=Y|N3;zsh5wQhX_YFm|`4@=ZxSh-$C=fzdfRNEn)w zO^s69rd80YVQ!080QRf|xOefV2C1ulF|MEd)(SuM?HcTJ)C;m<1*_ViJMVBxqV10y zuhq!@+@+@en1EWi#4PiFPa#d3qH2QKvsMa9VFh_5P@I2v%m!`~WW3XlBO@WiRrmSx zd#Q;m6#Ucf6R=wD>wX?Pk#iR5)(^)AdS2-CQ@_%X8CbsM(1b_)WOfr@h#4IutUzSk zyKtUAUd#qr^Bo+|>;?c>^M>fZjBS9W?OPDSuH#E^|_bzo=-wWVuQTn%k)KU`n=;{42h*u!;*h;BeVXIFY_~ghWmj>N zRERX^ZE{*WJW=6&m=7+LL8PpLZ?1*OQiH;Q_z^HRwf{|BvJG{>-GEY-+`J{H`zXGB zk_9%B%`E%GOR*5~qE-X?Y{Bf=zxT&+=J%W5C;_;#Nr9*kK!($$M;Kn3g-&E4TvjQW zEIPPcdV0Do!eCJXG4bKQZCpsvPGAr224>)O_2lPzm<_0YIt#iK22)) zCm#nDhmk>d11rHVMXS7*q$sYG{O}@yKRCk|)DrY%MKB?@mvk6w`;R?p)ETMm zSKu%e`(GTZ&2Ems3m|J28PnQIke;$46h)Q(4lyXwL>Gwc^ftwfd-7++O*d9QFIB)g zoh?CLYXc_^(IaM`VhYq18IJDH`?2Vq$Vz|ELi?0Y(-l74VGM3M)rrE(S@+qmS91_V z6=nggE8e}~1FnxAGSY+d3bxe~>=!5cN#ThTU)02sao=0g$Jzb$4A8_C{;wcA_7GMx zFxmu%Qc`M@nNhG(p=sPRyRRXiUy+*6*nBBXr%vgj)vC7b`4I0CY&El~m0sIfnHd?Qj(od;Vc1xslr6Bv-{-+@>626i+UE3o z{fVUGiMNL*;voJgmV$J$H@CEQ*we3J!G1SRt#1m>J+eALkmY+GPZ>@+zuhB)~9V=L){|l z;iqK>T@PjzcqarK!u(~gYgY6mglK+K)0a7~V0w*Hp&8P9FvxFZ$Fq0$XM@)J<)($< zy+e!-!d_{FS~*c_J9Rb`|EHHUuK4uF5Chl8FwC*PO|4DJwy|S7I}uz4y$k$Qr0m?g z3dkb~ht@Vf2x9FiJ(Z*Vah*6p)>z3!X=jqt%hJ+PnPquuB*xU!@g8OU{2{?SrlNqi ziYV`;wqheNqnf?d{Q&|=HtZ`2W1a>KY0gi{L4j0v@6885$RCA7%bzS@~@;y&W}7W{t`f!eF?&AK)C#%V!6 z(ky~myS?u#os80K!?NAm-tH~!?eE?dn<_T0ztx6(g#;F41tw+ujM6BcIAE6SHl?bT z#ZaMqR+xKpS-H96?@ng7|6azJ5kA@sIU^0L(3yrdf#k6ZZP7N8?SHMHamOy5G`($j zQ+Tnd&?nmN1iKb(PLEM}43PzLzjU@SL4t;;$I@MSNL-DXvRIJu=5XAqKeo=U{EET8 z&>-lN_RzE}Gt>!7d_O3M`2sA)Jbx#@8WXYwLPV~bW;B)Zcr+IjeoF|)d}fL8Pz-rL z>xTsO)!g@XV=zWvNo@g*T#;m5k|G)i$ZkM1IHq9;uXS^Dg0`z~L`Iq@(D z>|;;uz<58!Pn^$$^D3!*Xd(M{CXdz)QZ6px2_*XfUJ0LT2^Jdd=pZu_jkDG#bn0rt z7{@qZEc=<=1JtpWv4Oe}yEhA=EzQ7#!hg?=-sd%h*PY}S+|&}<1&TfHcr zpWs+azt8)38PIAxNtCgU|4IRzM{JXau%q%P<30L{Cx~S$l-3!^{oC!k#+gd{|G}QCLixZ**mDToc zrT6y4kvm-{+JFF-4UC$|rL$t*RGC4u(C2i`y-ax;9OHv!sI~sLOcJ zJl}Tkh6!iO?-C~vU42f(VhRMpX8em)hD!@aI( zz!NP|E?A*BxtnbH;N9+5qwM_+bHwSpNZpEb)fnOrX0MoT#`V6P$43zi-3HL~1+Is% zu>aeWeN!li-eLuo@82XJ@MNnj8u&YBzia1)xixxHG#^2;8$}AlQ0sCC2_-mDGP-w-rO4{6y*0Z4^n$#1Lt~By%T60CL?! zACpGCIq5}2aZl$8enlbD2&59WRv_v0w3qIfNK-SQZ+@^v3uHVSe$>=)H_Bnmz zTn}#rYFe9?V9cH>$Ni5{y?GXyV3BXGjnB(3FMNEtaZg=S3s}r&5TEazQvWEc(0+eqx z=rtF#U}Dc-B#)NVVt!p*t!wD>4GdgGpH2lza(KkWspeb0I>R`kXd3m`Y9f`6RMWGt z3Id`JXPE|^$xR1O8w9nztA?&esJ{DSfs6>na{JT<&`G8%8=R~BgW&_qC(5x2Q$}h# zLVf~`vEZ;=6PBu{JY0czj)ZG~Eiy;l*}MuEL9e!Kat(@3ef$R83@om{g|=&PCz17S z&B(M&CJ+xABdbC!BowqG?G0QPA+eu6MM9^jeXnLHKaKReypk)}`0_7}_xY)ZM4ImD z_4YZzU1Sf|wrI=)`N^T`1w%g7qZ@6i@(ts;aYy&ELdBk|LIbF=|2qhg zoqgcFwTV1Hc{X!^%;le%OMkV>#dRWM9J;6I*${L# z!XO5yh-KFe$ry%)$UI$-(T%{a96K`JmMHVS&JOOh0vj{*!B~)X-bv>nh!9C#E>obc zde;5}aq zyy>S)2j>(VvZU#>ep|XIO{tfhq+DGIs3H2-;IfhtKtbx4X#KK6h!mUa zsDmEX)COpj`uB5-^fS*CafF2j@!mD`6M1;z>6qu{<8eXf7P$2tw^53?0rr2SV1A zkZmg@b|o(LUlK1I7D>ln*iBosD#!X9$>tjJ8JR=?ejBEvp z*448aMSWfiL7$4fT(mHOztzQ0N8NDmq&QirJ7HD-;z97B@TveaE+`BBNZ&~i?wZi~ z{Q}mzeMlo|86fg_u04$v8h3_()0Jxs)hH^_P&U!=q@V#R?;FHjF}$B{giYne<56!W z0;$RQH4sdhlhb9^yDS3*=*qosVOei=z1o271VdA6v>WrPXc}e;Dnl(SP6v zYzJ&eha+7+f`)yT)1s|-O&ORhPN>m6j6$oYNDY+do%uQ+uBFhvvcAnp=M1fRfLQ;9 zS98(CnPhgp7WJtAeihmZqBpnhJga>AYlo|>_`ZkPoJURRU`lwHoGsSDd}IJD8Ql=E zG*(kP>SwVG)}3O9*##w>4YyZvr{ti2<3XXq~+Fh!K} z?EEVw+OSPhl~fkpdDb!Sq~Cp;|G4;V%`D!`&+iKmKjvxvetn6_5m6A0r(iSz+8n%Cs9fkP09b(NL3aZoD|7!tY7g;k$ z7Z%WRgaaOkwk>gCICGqVNmEc_OY^m~#Qv(3GXz9q5svmB9xf5JhZFt?JZF9$i$9+8 z#$RfU-irnM{gX^2@R=k#0*E1f0BLoT3`-=eURW=OhOpxl&ioR1o`%LK=(|}bdoj8$ zl0g{RNvrb|-VO5`l5%1-ZUH5E&HFEydkwgKHCxKB-#im#=hT1e{;5KZ1+fvp03(X2* z!o);8aQ_tcib(r&V*)z+kGXu@@ujldw9g*e^8_HNUgu#aG-=(1-8U;R@c<_ zvsHUyUUbw>u5GH?2bl-T%P0;&gEa{Y=utf%K8W1@&>$ZCeJ2k5^^hI#^4N2s%j}l| zEg}8aD5%}Uzt-qN1sHgfjbD0m!|R?|X7D+!8<3=)*@%nIw0Uul0y$@0Rg35$cLZjK zeJnh_?UWwy1nV3XG{8$8tw`PWIO92EN@nEc)n3;5_G)ckmT&B-!srmnxE{AXcQ0~a zJ+k%P`;pY1lgAx&e{z8EzAEp)?mmt#90D*qQ6x_sULSOU0*wg58p`?{AO2DrLQNTK zpe>yWlBNt4m(B#P{mxD!oP0tR z>O4w%6;qT{sGdtlUZHzw*h*%|7l&t_cLy0Um+FRA4_0m=u~^a@d{-{0`BK9^r@zY65Pp($pmq*p_K zTM}XJqvjnv3D zI+(#FBF=SM@{S1r7th{?yEqWdLhHf2-^I?YRFI&@<$@0tf_>?wkIhvWk9%@a?hr(5 z?Z8S{iR4oqDPorTQUt?lfak`iKSuZLzd5hwkQ-((OXDG(CpL%5nQX8@<}a*oHDqyb zTWBlkoJFM8sSV)>BJ?Ht_(3c4s9*o~upRNlIUjG~S^dNZ76b1HA5xgAUH~7llICnC zair+NY<#)w0%L_+@wRR2aKVzlr25C_`O!-jQ<5^`Y|{veO+WeU?vvkN?3FVBJ+hZj=;u(?*^bqGw-5APUCSr%4SGB9dbXT@Fcb*fDE z48lkk>QvBe;<;M3+TQVZ^ZKbwm7Xpgipli5+1vO1QgzuRINm!N5r%9{=Z7qQ*}KR1 z%H^YQJ)N|u`-`j#+5BWZhV2R-uUnz!O`@)U3y7X}1phgIEBD@~zypzKnkB#JN>_o= z*MW}46)Tprt4pgYBt(FF&V;C`zKSvig{0_kd`r;CTbWS`m8bUH)YT7zql`s4Q$cn# zRpiY(Nlhg-7!wMhRYcr^+w-#qM|#(Ay%aFzMdNZ(p$jhz)gTO2zw;Z3{Nx~ojG=-I zGa@BPi5ssYQW!mal-shttAw21tIgF8X)qQ6HaI&?AA%lC)2pw?bI&5MJMzSQBp`YQGg_$9~2_D zQfhdr+f=~H87ildkrfrhFXF~z>XK*+@8R!PM*RhvG*0%W1}r&0cWZi&ONSDHOu6SH zz_8Bo$5K}{xQA3vI>ZYk-v)`@pno3>xai6(Y7Pp3=tk~Figu&toIyob6x{g{xe3op zKK^BDmQ4evGR8p!D3A7>f>RuovTv>LNSq?Ir_tr-xWGILB2D)xwLxa74t+PXLd5sp zT^4_Rzm%ixGedFWqQm|ugKoZZ)VSyULG))?nLR0u|bgUFdt^P@yDqJ8NjoEHM61@(R2@$D9| z@g_n1`m2%wYjvQoHZ&hSCcUQy=rq}r1Cj{^q3b^ea~lmyW$x69O#O_pKrqxw?>2ft z#xds1S3-08%rzVv)n*cV3f;&#?pGE=RvTjlm}14bv{Gxu<$?Zk2H`Y|={G^PpY)T- zKMVVeSpw}h_uGAE#bG+x_@_YAw&t9QfCiL8w2ZJQ?Rxak#fvSS;_f=5wBpGDz?>(8 zNdvE60VG*ZPtTD)1mLAYfSu07>v!z84{0<|<2J!7A&f8yK?Zx+eM}E&r5Ah&)U?Tnlh@G3!c#K2 z6|oa-e=gda3KyYSGuh&12O~AdE1umWr7*9!4HW!c(A%>pc(eCzW{F2`lp|Vr0}JU+ zKa6qa5zzoU;Y~s(58|RkEH4X2D)0jvzf^?$$=HfnEF&41WbSnXc^;ISF{gpHul9zb zl0iea#A}&P^_-#3Yk>=D1+5t)0q468zwYo>5a1B%A7)pb&Q_H)_B1_8lKbu9;^;8%&nxQK#S{@Rqd$YZ(JeF%Sro5PaB73lw zbvW>b()!+~dmpLOK0s8z4dq48joXm2njS>BiKe+k7D(*`jQjW;8ble2$KH_ohryoZ zn1ssK5UK`Mz)tvy{s}FF2>qfkj72jiV8 zJ9qm^pB5}&w#n;yJk=(A#TZ(VnUR-2a(twSDGB~4&vfG@lD$Gl!Np4}MgvFm#pSEN z6~z1J(&(>z-q44GOF3t7a)q);r89=w3XujZJLyfHn8B9&Vc8h@8LFYlhBg`M@95?v zQw7Rrooh4G0liVL=Zr^VBmhNpdO=PMD+f^r8m;AsDOwtA{M)> zn1U>HlbVT+5kyA76%R3}Due*^89)4TGJNWWrZco+IVOg|VcItDHm%%sNg30o5oTD4 zJ}->&p)vac#zzN)4a;>QoMlP4(~|U)6N{3rol9Wy|H)<+KD+`%{y*HY37T)x7RI(; zL;ltWJ*ECg-zpl{>>;263QObmZ;vg8W=HDt>QMzrSQCQe7RdD-CVd`f^%VmAb$LF} z#8e85^XcP@>fIc}Ih;}nDy|3urD0mz(<&}D!X16OoN#*>3t6s!S#}X4uJqm@=;Ip>C~$-f z-nomsYYwf)kCxs;{gi76?Q zBmriyeyU2qpf1Ujhi~RWJezP zj9JE6KQ;M|&F7}4*;aJ*vxr>EyCli5A2gZW?CR<7 zfu};xljZ=M4#_YJ{I6;2v6Zz@CuVqJuwutzC!swN&gBkKaMCR;YdPolPh2wPw5BL4 z0VY_T18~SolvY3h0dOc1I201h5;Xc;k+g9T_Ht$-En6P9Qo&b7f5hx%FJ#AU{xtCw z9>KkY8a8M_cWNKX#`*qSzN|FRSep_bXp^8AFkpd$SbNA#E+lSNLB#T+kLdmL(+ofV zr)oKLslOA#2m6;aqaG3YS-p5r{cUfZ@r$F9miv_s^jt#G1bW$pb0UkE{cjwaaA+VI zW~IfwZIS6>k!#8egC_8BdSYBv_}5?ttF{`|Y@)%WjWM@T706tcxnw`*_w`>$60fwP zO|SD2bKp5VFpTJKM(eMib-}1ro-zrjI+dC;tyV6h{e!z|#FR2Ag-zJ%$j#kBk%2d+ zkxya_LT25w!E6su z773HNi5AbCu*YvBR8y*$WN;9nCXqDJpZn@55wi`t(uL1ok~`v4%s7-qX0~U=9~O*C^vXuY(+MZMo`MO`cBRaz(Vesjr|WA^H?I#F?f((Q<1O- z_27@TOhDP-@9sk~Z>ma1EhXdg;w;x=<0DyU$!0<_Z=qne8j(LVPWzrCWA_fwo{aw7 zxC-0jYw|1ATTeyTHMo`HNNF%Cn{?+swrt$(>-iayCy+4}z&V;wO|6kz17$bNma^Ac zJz8ziCg-GQ9%k&)Li=UB1?qt|J7^QB5Sz4%a7Bn-%o~8%|9VpSOdi}!nb5=qKMk`n zt{NBWkvRUk|BIcY%;O~VyWoC2jpeBV4?aaUh@5yHs*AVpb`;(A%6FmvpHY6e-;@-9 zo4ook4SW|jpTtYagD}Maj!^0kKnNhbEcH9pdgPY7`=Mzy!J*h4RpsLYxLWv?FC(+C zgibde8{pl0a+|UgLuBnjlResrhc$;h($dqfBVB=DY5ta9%%^}-2zdUV)OjS-eAj+` z=4=+CZYR7-wScL=Ro2vI`=hhc0r&vgU&NBA#VxppO>xxhbs_(t=TMv^zW>=G=oy<)gQa1`Fm{7AhGi_a}M4Ja`>uj9Y}Lo4up zw#egpjMNGOLCgF+z;z}>#3=LcHS)_%l1#PTvG{KCJR+Q5N;|+33K&U}pqvR0&X%hp z543qu?q+XZG<$kVeKQ^NcklALT@>rLR%CiO`)!)uRn<)yLY_DjZAl)KR5^P0eA3>8tkG%M;P#x)QZaq^6hmX8(n?RA({{ApGa-M|O$rEyQ9`u5# zUtMq$IM?SiwbI9VvZ>gHS7X!97HE|!qarU_=Ovlcelf4Y6ZFph^HvVc!*2SL}k%;Qtp4e}V@4T#!OX5V)Z#iWvPcx_Q%cMb@1&;J+qif=$Yc zrl#HFZ;C$eaSE76e*pblJ2UxveNs3E)r!?*28xMHWdOHtCA<(`_jL2QRJSH68`oS# z4ALjg`!esPz!putt1(x_xmML0Nl$Liwn(eMVa6lvTes9oAmbCmWuSQhEe9C?eR+S3gbCe@~0*e7CQ`RK`< zTLBKx(d)6lKRO3VVX(_^py_vnwxzo82Lpz`RF;H%J z@7x6BbeD;cGy!&9vm)27f@_cKaE6F;p8>BjhTp&Rq|!nkzf(_PPy-Jfpr>=wSMe=eDR;hHp$xQm4R%$faQ<_pF6)nX10hE_K+P*Lv% zLksgd`}YSwZif$5hYrq0FbpcGZk!1FL`<)o_)h}_xPp#y*~#r)r$qfaetxb^DmZpz zuznY?eU?(ghKy=@6c?mC0^=Ll5CbH}pu?vz@WPb72H_1rbDE*gLDGvAZvQ-SyAwA) z?Pn$NAMlkyI4)(O5~`?tgw0$be2Z@xz0m<*H55l2=`%9(;;(b<>UlpYOajFG9h#Nc z`DpPQ(ZRX%eP?h(8)@Ha{wE)kVOQkkr9D!_Abv8zC~Np286cy$uwS&3gK64oj?R|l zBnsEk)ZBX&N}@#D1bPWfa1ln*1LAgFNykA{5l9ji1=HZR$ympehxTI^tlyoER%E8% z6MIR%yFE9YJ-#|3w%LRD&7An-oVUVI7Yv~biI5$&#!m?JS0>*nZJ?Cc|Bi=%9VepG z>_D|0*+>e_IKEX?y2mR(fCt_=M^tPVXJlrsLq{jKNT7;x*1=xctJj~$(WH*+$db~q z`~`iL3Sj>8x}2Z1Vf(iQm!GF?VLL=Sz^BT&z{aUAzXSOW?%bl1k1qHQJW_J*T_jym ziW9a%MP zYZrC}8&kTx;8JV2Y1FLgjNHr5`Y%A;aYw3{<2g;E7$HR#TPY(ziGAJ)d3-D)+kQ4i zFpT^d3cRlm+nW9TSrLEQ0{3iby~I`_CR{D<$^l5Cw zbk^?65E)aLJ;aqKqmP z(D=V9@H@8k>W1A7aIWSoTsXYY7r{E&o{mAl2NL47R=sQ))hYsQ6HXFcORA#?XkR3@ z8DldPc(>&eyD}qVJ{n5Q_IvY^FdK*l^*It9%BqpmuQA91$Gt7@oVD$%#(omc)1xuf z8IFAW9=$v-!)Adlq_Y}7G1A$&b<`asmhJE; z+3N(pX<#+}u~cML6(j(se3D%{;t-sa$0Q%QAHgqrXW&9>Oa&ZyPZ3^iAho-L# zin4v*UY3^b4nexRVd?Jf?gr@w32EtW5Tv`iLqfVcB$bqY@8|cs}vg+glOZ zSsRs-ry1r@C0k`D(;?B|ainHM%(?KcPXMU4AG=@K&8cO80Ch`-pB84apC(*VFE|2w zB(~nK&O1RiSvuT(vwtrp=EUF>(#WOo+K0_49X7{-ZWs><_&}(FEp9cR_skb1H4Dfq zeDl+L-1jp8op~?u;Pa2jgVHa2gfl@PmC`aN`A9*%q!}&tZmXs+! zOq{w-;xF%S834$v=URkoZ^*e^gnuFjR;xM{v?l0HT{u<}dYUH3@SW(fi}2+LG_I$n zPi@il0lk~R^i_MQ@Q%~Q5alT)UT}=f-)T(GcRV9J0gIRxJTNBaX`KdTfpW{4&XvD? zMBC->O2)^DgmAIj%(1mjHAugbrdd>V+;AHc2G?1(2%EYHQ3eTNNy z#G+t>OqVA)I5?W`)a6#2OH-)ud_Dr;MoWRq`-bJsMNYK+;==duv+pxW$=62pGxpH- z!6h>HkAM7lE*GEl`A}iP1QsIjg?Yblmx|iks}RHPwxa{!3Y}755hgWggnFovUy!YM9TsR4~>)2yNS5;_a#v1e@v(czPl8x83-6MyYPgpG^`kk z{PsU3o&9?(hSvIX_M}A0yXg1*g8c=koj5>s-Imv>AG(w=HCB+bjdZW2R);{K zr}{%(E&4z&^wu|eW73qQMWpQbL0Jt8IyFA2XRc6DZ`~lNiSLJ_KbNz) zhD`CJUw0j9UNXWwP{)A4t(KM+my5OL)`L8MU}gUk@Sd0dDbAeOi(xuXXcF_h1zcVH zQGdpSq_w~^!@OZlOH@IPVO*WFH0y!e-Zk3@yRT;l7t$?3%Md`Ig$XkQM|s>_0GVT% z%|5xX8#K?N=01E|g(>(VmYuSZa`&MLD;Hwl*mE}Wv6y;z$(N8j$PsJKUez(66&ogF z=o9Ci$ts4T^2~oW1%TpgU+;9&-iLq;uD-xG$P{>LLdmeRm?yWX{bx`I((q3MoaMDO zL)}7$o;R;|f?EaaCE>nuw0<aeS&HG9wK%KNF zFg`hy+f%A-u)thgLcS;R5k8%@_&+L%TcdnTCZZ`7P)-6R#(>D+KuzuWXVT%9ko7WT z9dP^KRs0KQXYu9?Rh^DBv$KR&^lscjk02yK0AC*e;R*+>f4+arqrP(!PW&izwl0p5OqJ7VtYp}xNUZ(50M*X8|G=CZRN$$t!}lgR&wF?ctKEyL6;3*)SB z6KrWU#6tr;hP6O{!YjP8L8_p9f;qeh)at()*H=BX_F>UXZddq%qCIh8n+DL(9N`@m z;(C0=P=>&X9@Z%YdB_Y&}L1HC#@Pcvb=p^5rCUsWWN&Q;^7W7xeq z`}lKXITYb+*~M#M^n(F0|7V1rtL^z~iyaVXLzE42Z3FgbfN4cNL$V^NKRv&|uy6Vj zmR*J2wDpJ3W9j=?%#S=4=)|N?5gMzJIZGWVP&Rl4 zXu_79VaFPYtLHW2s)dUidmM|m-VI2wvBKGUv9ZVPzyYD{xJ4#d?>m~@5IkL|=Hd^D zF>t10*c8O=&#!Oz2U`*+;qirh62eoGNBkY=nkLnP(#L@gs7vCw1L# zqw`3^P(Ua7FwQ(Bi8ZbTe(*9lID5Q&PlIt1Rq=vQLppNI_BgPBANiBsUvm-fTg7s! zP)hC@ugE(fIP`MxqDV#tjA>7;<6Cc^%$@D?OBZ#tOJI}kV4`bGZd!RVQ~1o|Plz{g zO+)&xQ0>Yn74Op`^G^TZ`JW{7@7>}5_KjW|$9-82;7#Dw^rHkHNXGv&KwdO8Z(lP# z9|Gt*f=ruiN1H6_gVE7=5`f3#`fe5+z@j)0GkbXc16d`Eybn;G0sB!A?SKg@`jLXc zT6S55jp`?P;8Y&Y9FjIG%GNP*q^%n1Ix#LI@~oQnCYlXiT zBOuv0R)c#!-0-jdODD>B>ZkKIzHz$jlUqLW=Ky+EPUwWnOLVVpw&Gl0nPMIU`wRt| z;mpSY{6!bHva}AX#Nl0(Q_IhulUtk~oFsL4zv z`UQP9Lo8Dz(Lj!VkWS1FbH?ylllkm_`n#&TOJZpu;GXQ$**Ne817t-AShDw5u$S;= z2%q%;ztuEF*?G78ZJogA{;p1LUlab1ERs8beyh>&>QF)SnF_ zYtF`Qh~?$w!5eNSldI~Qny_cUF*Nb;5xM~8O+bhYAfJFr&|n$16k8M>fpy&8;6|`k z=%|VwzKC6&c+?d|WwOEQO&Hbgk-Z&G+f(Pr!7;i0u(^*o5e*L7l(D(1>m@Cxoh;zi z_59ocP`samM1a-3n%D21@oe#uHG&(>O(#KT);hC4WzEt~a#-s=g$Aj#h;tj~2iZBA zQ#c1Zi!t3e;uUPIaHQDljRaK&A9t81TNX}rrebPm(P!+FWM5c2J|si10WdY_#hXkK z6*a=DB2*RQ0uTCRy7w$b<{x+3%HuW5=c5jvJ^A6~%KV>h#l3-qhRb`WjXQ5;XoL5< zgcpfvYH`QZ#g95k(E2__`qEXWLutAjaFZ0R#NP-|=5AuL6(py0PD2S*N=pdc!i9yFI0^V@B*sU8srFmMwj6 znc~1yd<4$aZ9$rp;XdO$zZGLsF z{_jvx=db6=(*t@Q6-U>ki(;p5`4Atyp!AJ_ zuR~Qkc!L$$_38F^*5^zjg}-`+Z$MzZVVR0;)`tq>tJ+DFEFjyXzOfNnj!tk!oOpEX z#sDOb>dogjX*tPmqp7LM+RN@jVo6O!B>nf!g~ys%F-@P!)#dED2XqYRk}NG@v$M0s zY62=W*aS0+}fh*<#^oMdN#PV ze0du+pU4dO8|?a82_z^p*sM+L{2n}KZiq#740$S~0ni%|V~i!?6sH)ZJ0><%BR`pv zME}qT2*N3N_)Q8td-cBS;{!o7XYl&w@e{45VSka?Y zP;>WLh+Hu*FR$M`p}y)pj#|n#alk8J2+v%>KMb) zx#*>Wu}J|IOoynD4{M|_)7bDPj}kkEy(w1`%Z-zkjX>;5F*ndCPWdT=7Y)t$!Ji`a$H!o2akK6SL&CO2JH zkY$E?jEP}V7a7L?QiVif4}CgW?IyBHG+dVS=9O?0Zhu_U|1E@MtOHU-9Egc!)6}<1 z?V1D~KsvJao@NY37mmN<|6xk@@kt4j!k$Y-_}%jS)5_ZVVbfW0K5+<Q6JI=~9gZXBKo;0&5;Z)pJWgvWD&6sznU@6pQL158JLZ`}=dj89%~DK)XFO zH5ChLO;NMSf&<=6C#e{M!h97U<4uS5AnUAM^X%E(?1d)~PZ=g2iMksCPYQUY0srM6 zLA2#sW4tM5_;ds24VVHz|BNh73)mrl$L+V>2e#)ky%QTxZ;ujKGpinf{s2s}H$JrsNN-mC)fq_Y-0yL@z7YM(zi1p-aHtW0UKI=t2EZFxx>0Y{tM2(Is*R@Y}C7H^WLHtRL7!0&XnDu8x4B zE%c;M?Z=1XeSY{m)E6+f0oZ!Vm9oh=-KZ|rRQAqCo(uVT0wb>6@gixlrES={Y1DXZ z?)X1j*Z~S1J8Um6ky=@KCQ1vFC;9Gyi)^?8{OBA1Cmj7%?{!J3;XOO7DdAl37bTAif242HeEyr#-*BUh%n*ysI)i^{&iZyy zCFyb&vEriq+x{b}_~9&LnE5lGJ#TG`Ci1&Ylz*|{zv4Gai-(_&C1JnGvLe{5qyB#S z&X>f434vPh!Feh%f->+i{I#FSGF5la0}I%48-&}jRm!kwg%YZ9-t7fub6Z&{6A?2cS;BgRg}e{V-v_1?nvCrSy*DAm zqoMs=$k4J$J3xC#=;88He6RT>2tR}j+jFX}JysXI3?(XjY z9q?bjFyqNY;>i&J#ERLVEqK=jsE^(80cu~cm|}TV4m}#8ix74gTIub;Tc2cy+*jK( zIYt5ARGe;=Yt_Th!P(i|!vhD{v;!tRTF=hx_rJ~oTD^30dwV2Nd`Nw!VCM2RzcQ~v z!-iVsT#BKm&ur=1`s+rrrI~aO3)ymCwFNoC2k#jlo%e8$f_n@-eRx&GDRt_jqE~q* zJ1g~^VG^|r8hUdbvqZg$LN@Dys1H^(RFid@|LLFmdEgYvx{Q@KgpMNaleu#W-N`K` z_Mg`2w~|3#HfZ(TbtpF(bjY$PxB`g30zY(*>TqwJWaV!(2e$eJ>#HD)cxt0K4cSS< z)+y4uD>93+c%jBm%h0SqrBUYtikWy)p@{!np`W|c<~R~_}3GXXx4d(chOq}G&3qED}x;AF|w=UX>! z(T1{o;G_-mS6s`l*LW-&)KGMw(+AFiTqKmhD+~vCzC+Iv`#YvhGNIc6{@XPA);wte z2*{rq>q4GuoZEW;D)dI9F2W!F)j8ipN?I?K6^R)}hME0n&VTj`n|a@Jgtor%gS#RA zcjQmUSd4+L-(HR7qvF(`R(-!KjUoyxae|p zEC>1Ta7Ozl95TS@IWz}rj= zqx(&Q1!rJ)?gEU*;@V3x8oT^EbH-EsRwvjk2=Lv_tH=>piBZ{zQ6+yeSvzgZ&8+`_ zYxpLXbI&6+(d6V3p8?Z~Z=2@g=9)rYRhHfG&Lx#{+HnsLlGDfWWcQtkcW+Bh0s8PF zI^HUhE$zH1b6)%q%?eBb9wC2+%@| zoA7AJqy@Sgvv5zhx3*7BDCSwNX!A&eP0AcjzNWVGCUC6yv4!=04VI2ApC*6MSJ2CA zruvPLvg|oNOq2C`UHM_OFBH+!HDdR=Ut3r(vTVh8KVH6G{+HSW0dLswi}4Z-RAtx* zZuE_e@~He90mnu|6+0B%q@so{f_C*XCX$n-{6On8&EN?P4FXx$Z(B+$`n!mS8B<|R z4_YHz68adv)_y6AzRB{*rI`y@%7UJnO4+{U#aydmZxW=Xt*HewXMgf<=KSa_-E+lJ zV|Afl)x}@i2G3Kvod1bY-+Lp|jgdT;v2ngNee1EZdEV7E^Vz^+hJm@wL3LjvKC#>t zR^FHOkB*DB!;pYvwhy9)DVWn=j@@)&#Q0@?5cmjY2;L=RwL;-_Mz^;O` z@&6$jqe^H((zqjX9Ohi|*gn=mb=iNL@ZQJ!!3*!#;vb8;a8V?Jt^LnE&))$B?G;Q` z(2~Ss=S0TaU)zJr@%|iE)#n*5#+Pewl`KQ9h706hs6l@6{D80(rC!_)Av3X`7xm|H z9kG9yQ2U0f>jw|q_xEARb=Hob^Dl~?ZScZ*aBO-p`MRUZuUh_gqG*u*nC*Eyz?|tM z7Th^8J}G|irnpk7aWpTjuT(3^`1ZBAnrlLk6MsCB23ZFQm(Rv+_SF#Uk>;jO#9CZ` zeM_D`bGE*o4X~$^Be>k2sOqF~nXsn5&T*oY+0rMjUU=fnHZi&>%^bP`QTeQjSdads z5vH_>*-Rf{6y^c7$|;{Hdcmes@th41mWrAoDl4T8UNEZtPMRKD75n(@L7uPS_}fIu z@>${fMbZ3kTH9-28w6B5^&K5g4gD9im^K>4Yh+Z#JtYH@ET=nck?yU~m3AR~3&{-o zAwcv=YN#*{RJx_J&VBQ!`S&vefNisWvG=%F9u8ws@J~X)j3?G+(2U zW3^Zg4m$ctU{6d%e%T39yRW8i0ZbW3&eODd#xtHf?Q8C@zAxN5D4>v#91SYPcsAOf ziPskSPmkzSjD}IzCvad_0jV|uHRLhWUkBM0N_ky)xk^OJBCo0TzOT2dwhQ0SW=A~^ z;tzM;IPkWRNHa$2?uhhP4zkPNTyTJ_^|^ohaNc8u4Ne4#tngwJjV6}=z&-QkAa3EF zkgZO?jQ2l+%mKWn`92K+)F)iTC;lyA*F0&m4Hbwh^W0`EQVX#*n)T(BQplvP|0;zC zO3_D}pcI$;1O$ch8!SOG3N}1EJZvH&L5lhN_J#CPu+>IL3dLO_7#~=3MNcd=hM-Xa zT6zDy7%{|kDA>w2idh0$6#>+e&(M(6yV=@7E3EQ`OE%Vf323|~a45gSg$o)U z{bnox_FIbx?h8jVg!~qeZ#!&t{)j8`sgKDF%8Fgb9Ty)z*F3Z#tsi7{)w8h?t-oIS zPvE@Q%sDbon^zaaX>zfh=xzx_&(_!%LXIm*$U)qJ0np|_UdZUTnQknuil`^8g2so< zf9+QEbz2`!iU99KyOI#uXu=b{aRmxvX-gLf5g1}Xm zdWbS;*28SbGS2}WMsC6(i`uQ77%1ZqoKX+fn#iC!f&B#wJ^nW0T6Hj+W(m*_fZ4wM zB+sce21vDnAcPn&M}GZ^h#ez&c+2>pJn@|>csk46^6KofjTv2tPq&eD<>J7^#0R_P zEdFWximzYk%gW0Bma+)B{anXiESK`gjS-doyzhi;KBcmsXk`}+RZ1TkpVCM0CTb;m zYuE4SrjHP^XxuJnV#Y6iIZb?he4QS^4A~d;D3T-RsW=h)@=pc%=b{W9LFp%~JqzRH z78X)ho7i9$n1xU$-~#l}+D>(}lKDy5NYxfh_OGsbvT^3Q8TkA7bM0BzI%LoKpf;46 zq3tKcZa5TUyFHMT$#Nuiw+323CnmWi-zUj*N7&Qq{}IIipCU{p)b!h|uE#Y?pc%$d zO6D8KO<~WvPU6=TmOe_QfVPj?$=VpI&Uzcs7&VdW%oIGyoIYkn(T|U?pZPD+sVq4J zs5h()Z!fQ)xD|=rN8L^M2M45acXI#MJ$_ZxcweTEm4)ti10W9wZkvT=pTOO*Rm=S2 z4Jv<-a7u1kKpY%)Pcz7UM1xw;BxSw~o_~)v0^w^#D);Ln&jA!#R3}?_oqHT;z~y&S zH%8xPr^BSwDoZg=bAcq`m%LD^6UbRT?#uUpV5p-a#A2Ji_m19Ifo<8Pymq0tH`mn3 zA2pPSeQ|^s%x^a_>oh2!9COrj0wR;XC+F(omrYBB*KRtqq()~;71D;JXsYKSg$^SR zA3Q3D^h%y=MS~NAf?>>aMiEl~;{NSq5kAB+w$FK&Amog<-?kk+l3xY+z}#=!D6s*@ z_&Ji1)L6o@$(Co~z30Ubsat(q*Lsx)DSf;FErgsplekm0MuE*i-`7@FR9@SRpVyZE zyKviTD&1TcrIRw`$KUklIW-e%_Ic$x7_ZXVB$5!*d^#`-fC&b0{kd4h_CL_Q$#!s_ zb;N5`hR5KsnECh+44Ywe)G#zMAi5p;$(f))F7%ghIyC4jgg4~VnN7{j#k*%FD}eu0 zRjqL+8*nbUew^t8K%HG)KQf&80A$Migzt3NlG<=1b9_M%mLJY?yunk2!OGbfbps4J z9v8Z{?zH;D5d!VZZ(yXq=TuFMcVa6u-~F+7*{wxn<_9e z2u^X6GYX(e9cyc~)6W3v60f0GtTkz|r9nLE@TfaZ?fR@p)J-{X_9GNw6+@~__MZ-o zZ{dw-lwaP$d-#7J3k$*n-%3tRh1>;K1T|PJMm-C@S|_^_iargl6TdDB7d6{tf8MtO zw`!YHd$hZ08|b_bhI%DLx=?=^vDL(+PVD506ekmcxoU#nI|bP`;spB>p@WkR6SYo-&T~{p4q{n2jxC|4i*0Z3{@eq zCW4UgSWrD;T_TDUJj#W6<7$`fbTW}DPI0JJFQO>|f+FOkB*!M8qqIl=rUhn0`0VP$ ztjZ1^*X!l=lWbZ|tQsQbl6VuTW8rINi*MlX04mmz*(XXjbyO{Ao3e@xK-~NfoSg&zAD*?ay)_$wtyP(dR1+)osaxgF; zfoR~(UCN1_x0Ll*pTOPUeWBmwrO~>(Bs(b>zyTl@b;(5sZdF?B#F^vzuCCb9QmPHt zQIhq(>dz=#$l|g-rG5a#gW0#%Qcap};)yRIM@ci!nL4Fwbx9cU-xV0xTN`XwttE~= z8){#>Jm{1PdxW_o*Bo7D3ycGDJ^n&=u7Cec8dVYhcjdkQSS>-|wql1pg~3CZhT()~ z`{l2b>67iDxCLj^xQU|;?etB!hsaN0D)f|(0U-8>NEMA)jJ9vmk64uwTndCaC<|9=uh-9{I)AMyv`|D z_x)w=+4m*kAKR*uZK87|3f6x}KZd~^vDzp=oAuEWq6Z|^P2xzMAQp^W7N;x>LB7uu zKy3(3{9-b59mM@YJ!^b1_?}h!Hk0c|@<*7)r4!3N`$9+!jpJZDP5Sp6W@a`DTgy-B zY`V^w;9srjtoQM3D~;T1%FT0TOtAr1_&JoQ)31N{rUJZCho56?M3y2RJd`CR`9_J` zygFeV_!d5WK}?d>#W|g`y!3=wzs)KyY>DI28s#w%e^7jb0nVIU0VzWcBjgLX-!6DZ zEqK~mfVR6fWum7`)Tg2M6s_hLQoVD|PBE<*@KkI!h;Ur@zF)O={rATdJ7yTFfL639 z=!4BDgKX#bjI`3)6?K{pYLja zNrJ&!s~tvtxH7fBDQ&OIrJ6-zbQjW!+sB(3Ri9aNkEEwc*yJIzFXk&X!58Gpa5aE~ z9`*Tx<|_)LkVcW@=0~t}PsDHJ_GfpOnRe}foW*f^pgU+0AIC5Xog~_X|ENHL4?Q^U zmN4`0__qrgh2rp#i7*^a;nfwNyw=eT|-krHt%=7l6@O)sj*P8eqoLkpXX?_C$$p6Uj3J0C$Q^D)9W0T0X)ZYt9-d2G>qz@|I;vv3LMX^mn1+eHNne43MoSb!^*6%wg8!IML zhZ#8s;9hV}-P8KGyo^x?@S>(A1%he^w`*a1omos?HuqC#GVo>C!utKT22X3N)UeSJ zR+L^(uqT`u2Wg4yV1?cz1gU!*!P!A=U;kc_O8nn?*{hm3=Hqsw9 zk$d8j2hQW-U%;>P0S+%ua1+}`Qcm9t@+6HD>opSmVZYGOf@QS^wmL2smCc+q!c7rI zU9pMm@!^1lA8sQd711DxgW-z`hA3nrZ6Z$x(d;l(Z0VbBs1afWmCf`Bre*2K>IFAp zT)>S;;NB1kAlARtB!G#x(q(ozH75CgHip3DC?7%aTWJFa_1*mGxfQ#>5(HZTcX3@Y}0XN9RkjEPrPMsC8K zuP7(|78DO>lP0yFFX)+zn{-#C^w)0Yu`_hl@Hh}mi-T&^F--gJ>35MZcmHAXJYyEm z13|j4XE>tlyXg$;*zam@V+@mm#q5%tLB##UoRgS$S5?{M^@Qg7mzc@Zc1(@x_jM0! zQpN&rXAGbQOgpipT_fXQi<=UL1%{`T^YLKZ4{8VJn_wHB0r0xLIxF^TLh27UbLTrA zBb%Z0mWJT|VPiAVQ-XFM7dBcOfoJ$c5V!un;=kqEyE*5%4Ggbel$ zYwf^>)(nc0V$yeOHyFys3LI!j#u9NLqz!H`-lw8~3L2(~bc2j1Ju5%JhZwhbLbdK+ zk~!VDi;olzlZNPT9is<81qsa`_1o?>Zr9l7xC0@7 z8uU0oT%_Rhd1KpIO(zhv4!@!@zO&X6#~Z)Pi#ot6A)F7;o(Q2PeFbu0t&d-lxa7Ih z-D?hpuKm7|b0dIlNjG*>243&%#hm)w<&^Vij+ zUuq>YmT)2T4<+(QB-IFn%LR{wdw~iJwL+*ENJFX$0c}|-AfCxK#Heqp3#7nEQ~x=T z?iDq8MuZ>lf;h8LosIlmE9sNIdYaw^FzV|W15Rh%BuYFo`l1&_%az6ohUdC$_Rr{T z0j$tYBJ40uSDvYZ{9T2i>-tN^4MJ`6YPT~(_cBZMVen_4zR8ydIk_1~qmgvKq~FnN0J(m}a}eQtX`tTP zeMTq}p48if92zg!O$ap<0%c&hoGPgMme$^exnq|lwOk1EDblputpSSA`Jg2tpECcL zH{Q)5kjvinFE1$e$w`{)M}XFAAg1FWf`%O&TVvO-DKXct72<3i{fvTP4@Jr(8hy1q z#xuMAi)6>DfY_SiAh{!WWU$xtG_YKJ$i=_v61%CNb!? zj4C`*8?hKUBo@wSK&Gv4wydH2Vct|fuXzD5M-{Cigd$1UV{}+|?x3J{my5e|!Mbpz zAMkg`lP}|QfQU|nmte0@CGRXCYArKbi%yVqG~rsW7WfCJbS-pu3HJ7T(7OKO5fwG8u>a@sG3nWxF-`K$SccPMxe;yVKbTIJcTZM zNXDD={MdG?vOXcuHO+M8-eKIr`w7WXYBe}8AJt*@vSkl9M8Ex6dwgp;$buB8NBSwU zA@R46!Yzic&8rQ)u5uD@uBen<%VK$JeX55Vo9`_Ok)Dxshr;1&;Xjf65XPxH* z2WVt@eQ(Rzqmmrp&5_#;PLHVE?FJLsYpL8BLVlcwr^e*F5;H&;dhzJQ)7H_zdclV| zVeU1-F6b~oTES1N8_jR@2^1l~w)d*fj;dhQD+Zr23`C8xZkzqeur84+s;d71s?b;% zc$_tRTMX9s$nWZJ6$vs~H!zvV^3@&fK~eZn(7O?-%B4ngNXxdF6p;(&Y)zivedM!1 zeOn3Qjc^z7XQryX2X|hxp{!Inng!nPpraPQt>*&3gTa0m)I&K^?`d)@4sup?e^%2@ z??Lgi1s;+}FC>Xyk@n%>T$agknBnS2UscxFX42pUORYs7sO`#SH_s(?H5vgHXs-Q4 z*#mNR{YUhsNTt}LYNfWUX0`t1vF__zKmg}xw$A%M{?FQxY_D z!y?q~s<3wCs;NFOA+kh<>2XN6D0AISIO~nqF$X6;pENM_wvTOplh#4nCH4fWw}&Vq zjAr{sc~QU^hXoC+;}FG^7O`2$=a)x!UjKjSpq(XsXZ0B5KKo?HzX4>~`q?WR3Sm2o zeLYxhn_q{2$|YUNQH^h^8#)A-A%Rq0w7@M(O{w&d*p%v21tAVBm*;kv4Y$)x?Hw)j zHjgVe#6CFNFBvC@!#mOBsdSUXj!d;7u5j;|mVXRw3|8)RA`((ja@&$cxDf&9rj3BY zOI|5c?rT(^<8dQN%9g&^rHqp0?lr0M>bk}I%Y22PE<#9gWP;J;^1o=!Zos=W1Os3| z1FS>p%0l6G&Czb{g0qwitGcUUM5Fa?a%m=pY zheBJz>qkMtxRGgi*DwY@MFL=@YwopkcUPBuXAQ$0ZdXQqN@}*`j#JF4E;T0?pV*n` zJuEum3SPfgr}IOy_%zWOys;+`{rlzAGG$^&@yL8m4Q39a*+Y^{y89sXA@1n5i5-1>G}J!maeW+N1xo%vDxC#*V}`z&uGW< z$lA(8DM{WNVYHbW6J4zd+AcAw zXD4H%1Ls8w-^C834x=xtaY0bd#SSVNX_Y;&{F(`8A*fZD zH-FvU={r<*oT3KJ4|u8nK>W>i)K9=PbLNg~0{v=#;4L}g?xndQfnDlK$7@S67Zy{< zp1kYdC}NYob)2jYHORtglg$_6ea6Rvd$JGkHon~nm>YbVnHel6G?B-qvYgI{;!gf* zo8yBuzI8crWW^FH0EG-?HoT<~%I748#PuhU!Nfj=mRm+PRZ%bCFx2e(0e8lT%iAxX z9k9#6ECvW)$0o7c-PwOXd)H@6k-?G_uJspA08!f};(}F={6jS}^_CEn4P*rfKIn?O zJnkre)A2~GS8QP_oy@APc&2J|6xo7GnW%FN5yrRfwapW%W$@f_SN{X;hC1Xp6E$PnL`#u>6u>12#=7a? z8!djEt3g~z@Kxs*VYDBR`BUlKL9<6O8LTZ=A-eZc6M@>n^u{t zjL-B?H5QhZ2WX(f7RJMgB$TxEr0$LuM>Kpr*;Y%$&p=g;;0z#TJ-PE;CsA@%86Tc} z^$Z~U51iD?#z&XI~ag(T_b#Qhx6JhrO?wGcDw(n%X3gbt8| zBwMbw;6J55u~H)s?@jo$v00H{Qd@w(H+|rHlW%Wfv|kcCe5NmdpGLhL=mZ&# ztb`=dB|JHM)j;E5aT+`kkT_&PMHXl^RSn{fYkEYqVNhNU&pMLq+C?FW1VNc{z+Aak zy;W+G!!^mGyxml!dhZ&-rd+Yi!*G@$8efHz>IJQVA@2%*ePi25(GIpq`P>dt-$6!J zYK5XoKo5ALKhIxW2%VaD@S| zYrp9*qgf0__)qqp9%et1{^jMm?R&zu4$ct$xHoRy72X-3rQvH+V?+Onts)^NYgf`G z)eTSM0t?5fugIImk;tQ0yR2Hf?D}^P)>2QsT4`^kUQ#D5T+Z?ZvOf6SlhEAE#aR2P z%vskZ8!BhLr+D{D40@zDh}hhjw0I?do^&b5gX9o?^1qO5okm=y3=7LKU%QRM1~`Jv z?Lc;@)(6YJ3l_{aCLxW119)oF_)KV_r+Pf-?oWD*=rH71JeKX4Ki7%pw7!(%eykae zB4V=|wVu1Wopa!Vx3ua6J%NR-a zu9AqlPLlV?H<0^@1TT6q`@Rjr@2}xXVgRR2VP1kcJ*?);rdbb&xzYG0jG18-Qe$Bk z`Rp8WD=abPp}2SnVQeV&_Lb>D*Y;JYoWK?K+Js8UPHrUa zxw{k_-bOO$Hxd>FJLQT6bi1iMUvarvcVhSZCB22f!0?4zDMSH*Z~(ZcD3gr5QN3Lf zD1?B#_Kn8l_GAu<)eT)PbltbRg~ihTrxMmh0m|ka;z}A~0pUJe$a)c~`>&F>^-!sH zS8gVU1LvR6A61@z5}L6@sImgvO%ms^$B>YlHfbW`6~21*kQ#NJSR6kb`>>#Q9X z)hqG!p*cnCGWBFsuN*-aG;4Lojt9<@>iUD`1TN2xR2sdNau_PV6n;ocDgFxA`6qVQ ze4WUE$m;AO(TuRGB?D^3*`FQy$lDSY3~#eQMAx_Zwcbkn*vW1C7QPH)NlfECP?M?X zOXWvCpJj9YcMt?rPZs9IDe!+(76oiEDd zWV+KoKVb7O?s&QmFuvPnkHa?d{Z>P#$|Uyp3cK(y8TJk4pIlwKCigh(E|k_aeV^k4 zgW>0%+5X!Z#o`+TpIGcI4akH1X0EPMZsET&ijjOGGQqq)Y2win&rZOf(kz8i?TOT4 zY3!*w)8aV1GjUesu~O88jA-;zLdcv0zvf#D%eWTx$b$2lYBXEJf7t{ZTb;dHJzv-W ztV3fWH6YorM_-eEzxZX{@U#!w0A!MmKh?`!NVbe|0`^QBoLG`rahW>azM?C zq<@DVRJmB7^rC_+`wfKoOnnhD#|ldW>5Xqul&#x+`=o228X*EBC*>faGe6!-?nqzO)~p+fckxBNYRBZz_&Jcd(e{A9g@9$NYFwiYYU zpa#b~F4uP6n!0oE3T}n=S$!9%pPUyFimP`flwVyThixfhfpew_Xu;f&(#IIki?U%5 z+n_$eMb4;E7@S4Rbxb_(kfj-op95rz}^I<@5OyraX*HK_W@AKnAzk%@UrOC zv(tu?Fgk9L*vyXaJ1*{nQTocdTEj~Ia$%;PolNP~WuAZO4lc;(X;vN`owlpJ0951G z8*^E_+3!egxosZyObpO40360>XfSa4M4u_X#64%jH4D4|4eFV?yi~o)#!Vxj>vP2( zJoB-8+2@bs%8cDt`rw9*vNEND#!MXf9vOnH z38c0_W+rE?aI5Q#ZLp|$oVxesOia1!ty!;Hy|66DbY)BU@;dt3G>vrW%$HJ`4ZED$ z#Yll}h3Ua!99%1rj;UN9pvXh@c7j2BR#l|w5bZ3ibB)eW17CDdh{0JKto?x6We~ay zO2nrX#e3-?Fp&ERXHg=n*SI*e_B8Eq>0;UMRJ**stae1wUu{s@9b2f;vTgJ7Jo-)K zoAk{$>Wo3w`!s-t@uWALU~iRTms!iS56)A4xm!=DZm_o_rvOD2;n{OvuR9 zH-F7cL6BJUV~Ar*GFrA_pfJ&o!ElsXz?u%Ge zygsZT%Mv@AWQy$$P>@Yd7wJHDA@4#D)x@7Fk{wYa{l2yCl;ua{D2c!f_xqSJxL7)V zpdJm?kAcG>W|+cZ9pGtPh^6nA;}a4FTP~p5|FRMxnFJsH6lR$`w+gbzK{t2k6V*LB zo**0FyMGSgBt!wai$OWH*0Rzs07mf?;Si)zOt^Rl^b#t~?b#1VoH9e{+622tLIXVB zvjd7$f5G2Lg;7qEsBYLA!VS;-)1udKZn@nCytT~F%W_$@iV@`j){>dGdKo{81-VglVNRR z`tdo){h_)Gz9S+GvL7M?4^jJHV*AR3>)l!r^1>taSC$i_@l1PDnd-IoPUn+r+wxe; zDkC%~DJo)rE!32;C7@01X7+99y|C+}7caVf@S44e+dq$_s8is0gxJmCo#T zA?92@2s7huu&WtxL9wAqIxX*6PpXM6yY0$r_@hK|H!pL){_2Z29V(t=nG9luF=QW>yeoW;acMxstJihLz@8VKh3Y*>ji1w!4}G=o>Dq1+`Pmv6+Osln6& zjL&R!Av4gR4&$?5^`F{G}ZX$3a}mhD_v;9kH6wrgMO>|0XEbK0&D; zg&St5v!W5%;~RPq2nXj-Aad`)pxxzWt^N8vH)ySQMZ4^)uC!&)Y9yCqDVwO289xph zR3IGT)z2wwPXb(R0>z&f#>I5M<3)bto|T!oG?w<35Bh9`Xf2{^QkuCs=7N-!!u}v0 zSgewqK;A9|WyySr@3t$ycW3y!z@8V#CMyHOfhhX9JV%8CtX{BEsd$y6ZX_T)q>pe@ zwBn|$(_hfMbX;QZy;($EdcX3(=YWe14JjwUs3bxJ^JSHZ4dNtMLc7L5;CWL7y$fm* z=RCF#i=F~B0laYs>TXx68lj_<0TiDBLf{QX_)l-vp3G;nPe^}N3BpA;9J6qs#SMB`LX@~p0Ot2``Mp+in3OqVb6#rYIxJ`=R zs3$(R^%H1It*TDNm*LBNzf(x7@ei!SBoCsfO(C{__dp@)?SVotIVDq49dR5OqrWLP z`hcw&?*MjcpTTe*o=}M9vBf|EO z4SLDXjEIDI+;$17kUY)RoCmBPef{Ta;3m%PQg1jbqIZgtjC6s;qexi3bkQu~s+A@3 zWhLtC>a1vTI%t8EsR7o?DT3YbR9`Fwn)?K?BCT~9G3lu7XA?MF34{lHQHeyh&zXVuD)){ z#N=|rGd{T!)!E5xGF5ebXudt@b;g+Zj|M|`Wfa;&Hh93`*bfBS(DeE!7^)IFt z=Pupv43nTs;lpjhNLJZkRRpwkLH?r8j5w>{Q&)?Mrcn$YZHnBw@`lo{m524a5tN!h z4JpgZ9tqRHVQUdGEF!Y_bsR3Ed7f=9|dkKFvpzAm4k4=RqA*jN|=yNnLM*BIll zp9_g`OSjMhmhjGo2E?GH^g|W=n<+|K7O$0l1bM3#r0nqq8}^Mx`4!l-SK!(evJp^6 zV%tLeEXEx4fIWL?wNEYk`g`2+sGT@K&f`nc@&+kT4|dBP>VXrzd(;O6yh=z}Vhu(n z??WmDIfFxEqL(9n;e#SW!?odbL^>CvFhri)tT}~~8&x?P8u$48<Wt9EG&%SJkFyK!!UesPnN6f1m<7#vl9~lH((v)TZsBv8i_U zE$NzI-@nn3ACI#VtuiB|R`Ma_02e`^0ohQw`(gt(4-u8%XTdT1MW?%46e1ef zzNECn1f?G#Sms*66$|+T;DrIUgSdOg=elgdy@`e1GO@2L zq&HX2;AaSwJmCcRIl8|nT&4}A-?hbsiD@<>jTEyYUCEh_?Bg!9F_$m;VFS|qv@K_H zc3MAL2F+ivGUYHU>x0)}!P6HDFxi)*NxwsuHfOK^n5&bcC#t9E)Yqc-U&A69a#tL7 zE8(D>kjtEVn`A&}<&rs!VgQG!9Eh0IY+)Wc+sFQVvjgWaqDKJ?&&widccsZNQ;` zQ9K+!N1F9t#U`;Lhv+?Iz;=s&MDL#`^j#xs8ml8@BvTrJ+z2_Nwwi>x`)pRC1OqR)2+o-K^f4iWi= zZLJ_MeSA&6^eDn_CeukTMLH}RL=79WF_~mIiBegmG|TGGR@kL#zOy<&Kbm`2)U*%p zDIjKF)TdFA`6=B;jb|N=EBw%pgCYs$;;9^hc}sW0pfDPh~F+hyr*3_QNsmk6ek7PeZz*-ysvV8yL zm{pn>Vq^eK$7~fY+1<_~&@F8(-6Xk5j)~X;X7{nm@ENc!1T{ukjV0 zw;IGltWU{2?1zMf(nfeEfdXL{{ftQi&f#2|>PNCegFG!{BMJiwoD$m&F#f)Pd8y}l zMqsOjabYNXeJ63(jkA7a=`7N2>jb&X*VX<^p9Ig3XYX%3VbJGwg(bzHKCLG@Of&bS zqLi*>7V0fX0a-uQw?+)nvz-ri9SZs_j0*DUOtcas0gvco0jX`tOr-!EjRH`OmR`*W zurAr2V>?TG^YPVYB3p2L2nW;Go0R}tf>d6|hqvbPa>4sA#GIJ0oNvz-V40_d_iWyE4Ossrw1S>qq z917HS9^#0g7ci%NBD7J+m2ulLQh@&I@+-zqIX%Ue{LltQ zA84gyaU!jx_=aw=4(>YxQCRCjRF?&1<|ZqvV_Pe`uP>8WC5h9fw@qZX0-@uFF+9lJ zr_<00+_ygKv50cCCO{=wv_e3ee`|Oa4P&8(2g=BQ+R+` zdMfc8#yDL{Dk4bB^0)`5P~jaEIo$W-*S#OWJ(TzGjVaLbVjI(KZKNaIRD3<%KW0I+ z4Vu^BFl#Qv4-6~E_dSj`_zzzcOfJddQFx6CT(r6`QWKvMSN}-IPcFrC1_acrCPTT} z+m;vz@&Qh`kF%?wNPxctkdMtC)Khm$ETcjL;s?eYH^9t=eM*)BRbk>i*83 z;{FBcJ{gJI*N(6b9MP}N8RM!en*O8XWAw!8`r5#IlN%nXhDPqutp4K(H!I(!=-&=$ z)ot|x4=U+&{q@5V3lEE>D9l}C2p13NCKTvq8$mTrNZ7#uN6?5%JHm>21wmauP>Q}H z3J7oa=9maXubFJe^2Y|Vnv68%d1rZHZMR}I-c$B|x^>72w8pS#kJA5RdfjV363i~$ z0h5X|c=W$1Pb$y|R$1rEPNfAWN9+P#6M*Mdg?dgr$rWjl9Xcn`JiZ2^T<6&=Q2YBL z`?9Y&#mxFgqKn1s0{~mpy*UGbRBzGO@P9I#dV`mqcmO^rO;57|MUyrKeT1OTLICo! zw_rKSSfXiWGxj0%G_*h+K&hSJLBbrV5di{32cw<|Jq>OwCZg_ z{DLKFafS?4IS3krAaDX~86qp?&T`_S$4k)}+Ipz4NiI~Vv703;2spl{I2ZXf#dp(? z&0jSFcMX0E;a?htL57D|st^Pi$0rpF9o7nE2G#D$QzcKdu7jbPbA8CJXXf7@H)N&E z*F7hFAvnd;Gw3KD6mp)5bVIl1@ryJ=Plz}QV{Mn1NHOzx!I61@i{yH0;1tkW|sK+k@XjhyqUws@%@UFpqrk6Sp=AsgBv5ip zV_>wp(u7Jk%19d}k|!|ZeKHIcKPR^^_bfq2(=DS${RKZ6<=-k^WSjS&cCus)1sTf->KdTuN&A+`P2< z6hR+;#!&f)vzeF*7v747zPI9u+MoBXBkArOnC;Fv@FcgZtna25o9 z$<}7B?EkzSfBrag$AJURKpiZX_8b#y4h1zkdz|nKL0BV-&Me4Am(BI0*msFiMoGNBY1O0?dLBHvZs|S3aruHzw@tlww54>1%G$YzKQPS zl{>F4EU?4t$>~ibZPJbpEJ&wXvUrsXKwyJoG!NwEr%w!Bms_X}pfr@BH_;^X>lPtV zn4ZUI(!*oxZ$Zh2L-?iV-+=PUk|)NDas%Ij9s&TzrYaLn1Re|2)SJX1WvmfEhE0 zznycw*T0T+3qspYi8kO9>m!fn$YX*3pOCw3p4gU1Z;)1#M(@#qYg*^zr;aE&#@Go- zgn(u7KO@xZ*)qDgKe`AX8F6*1676>rvT{sR<~soQIEW082+U6eocBOqj5vLIhlyd_ zFM_eA)TtUIRw6buSZ~d^9E4X@M!1rA$Vt#C8gvl9{LiTPQe;L@u#8uugOnUIOgSgM zzAdQAtBhwfC%VG`TpC-x{QO>rU8!;UpzlU&S-YZ#$hdR(h{IgUM2YtZR+1P}OL0&K zQwRPC^OzGXI*+ie(AeK`Ou1doj6wi@Uw4pMeMIs8_J^QQU&91Uer+uSkSgxd9zz{v zq_YwzlJY|^A$V(lcMw&qQHu*f49FFiEDo}0@)nEJd8Lm5(Atgs|CDXb%@9_B20-zv z>0?HqGSY;+Wh9x;6b_f_vW`hc2?$=oXOF5AogrD^@|P_~D*mn>j&iuUVQ~x1E*Z)l z(<_V(=)|t{Ckc4nux=TLG;13a!ya3%2y@UxCoOw~jNhk6t-XaBQ)ndCxSgyMDQgT` zsomJeX^vPlhdDoOllL+USJTLRCozVmbxlNwWcIj7Idx=iWeM6xl-B{8bdYb?Hj&uZG$+ zyf1JYwE7Z<6uNkuFL{%OuPNBU>h!hIt3&4GUJmFg#K)CjFnP#)z*b7a7JOyTa&Jhu z#j0ca;X!R25MA5u31=_A52~IFM#LZN_r7%LTDLg1CiV*a7SV z&9U7K%%U0ZfKQkj&tb<@yQ%-D{W?wo(zm3vaN0X9Pn$^U8efhyK|;0MXF)3S#65g9 zB)k;mbosY-wcD_lI;xMdpjS-lZ=lDMHvGGvg`e&Ucp)^}JT(34D9 zED(GhZWp7R{b$;BE;=0z5V#uIpDrsPvS9Oo^9+b->|l%E4dpiuosNEVA`hDINuW$x z&52FO{SirW$u?khr*Bg1&n@S@;9(PDOKy>X)diUWJFNkO3&wxrQ*xVoGeK4haFqQ` zfH?XM4(1r9cEqXCFPy`)aHZOqAHRI-SwI%>RtmSo=%}+E3m3Syh(-5+r37VXD2PbW zs-^u)L!E1}w9mjq|Bov8Y%ASa#OE^JH608XDeuKru*=!$+q$ae>cy>7`MOSBR+QW; z<`w?k=7rz7^$r9KglcDBLX5-?&}CC;oiX(_(!*7Ndk@XXi6bK2vz*v&M)JfA>r{cWm55xj_YH1O9IHz z-NdAt3HuH2`?L8%^gy^Dd(;9!EIwOF`20a@zAkuql{_QE;zBzDABaIU{eYFBsRlKC$cSZEC4*tMSWyD;-<&g!fn_eH8m9|;1dY{RL$?RkH9%r^e zc=R}7V1gRC)XtX4y@jx+ndU~FChXdluG!cs3R6>8Z(?o_Nq0#HT(JQYI}j^-=Ee)i zEGC+1VZfH9@WKm$NX}M>CbTsG&|kWAuc@u2VXM0Q-bnNb6xhVusB_6NsT+k=_Va&71g0XZ@QPr zO4Y}#d&nWS__mfrGO40FUFcKtRi3yhn7ME!9BGi7i^OALUN#@jcTY)~bNi9s={9y% zuJdtO+MwG2oz65N{C-^rvmPa<<8CO6aY=idBq?9wA71>Z7xe&3IgtLDx(VJrU5JQJIM+U9IUND%LB-2inq zyV#O2Ri~WB8nI$fTb(d zAH9a-6CK#_Bn|5uq1ZlFkGkBwKM0}N?kcH-N&|aLq)N!+!G&Jah=#YRZo+5Wx(@Ac zl~sgXvKKNlUF&@8LWqInSOC8V=w>eZODyR(2gfJdFTnbhAlE|ipYTddCH$lW_N%#m zgrPs|J~b>?c$?>$RyAVaCaOzsL3($8}-=YMNvm;VE){mo^~#~9v~?JZ=O;CHFM zPAtd`Xl*jptwi~vU<0tr^wKPiZ^n6+9I8n&kkmQr5YVul!Q2krelO@a8GR+he!d89 z1g!74Z>r=XR9(sF1<~*@M+(mspo1}sba#jfa|a)2%KF&s_R6IaQi}|MwfbFfHur1| zTrky(!UUZoE0_WpxbeqsEi!^?HBeR#D3YAwRPyW3(9~9aE1_>E&8X+l1>AE^a(FT# zo@q6(wsfPhfHWuI1EA*btf-}!&=|p){ipn!E}X}21mDY&y|hgES`#{_N!I*s=~zAx zuohBE8Z_iF>H)*az)eH#Kow5)@YS-XvUh}4jGh|mne@=wBScTWTavn(1X@<5ly<<2>_9NHiP%{{A#{X z@mz%E5==tpg*VAlIk07vW~68~LBdW!hHp#t{WV!ITA~2{E%>K8zmKcu1uvCVShk>; zS~aFNhPXo|#=ZqGNlT&q2Hsuyz)cse87p<{oHa?6*80+X3p04zf(5|=w4xWYEmx+`Uj9SkG~j|i>Xydjlk8z`k~f#Gk!d*^vu)a9XGD)&CN2xL z$fCqY@3fg`O4GtHb#`0ecED!}>~`VT_-vrF32fVFYRUJ;)*>p52FS?v?#z;LkU1Jp zrxk(d2tMRPWX5+;!cDIjpDH#G!-;0dNYsRxwJ6xNq6#Wz6NA1WoHsV9Z8T>lN~MZJnI3Ex?P{Z(t$3`nx_0!LxAKu5GvppVvR7rZU#z^URwO>hz=0> zZ0aVVCzMD-c1}~K((e#Qj%lB@wBQbWO74xF>5|~)d8@B(wtGDHpZC5KfO6!X>ON%s ze$a?>aII>zG4lZ(W6FDUkhh=2SyDSH(mWe0*9)RRL(6J6Yw_E6d}4 zxISS7CzoE|P2vLkIF9EqdGDnF-(wCFCuHUA8Vgk|y&G?r4ts{?0tpNbGWJ9R`6ld+ zBe2sc?uOpc|4e*4WQ2%db`1d(Yj6d8GAE4Sk3U!up%9pv|5^I|_5;!C;ZHGZ9#0(} z;KFcv7b1{)`^EX6+zy2KPY60nq3|`At*p(`KXvfEK<_Pbu}T6>+c4sAPwlcd*))jO zjZmn!3dCZwKAww`?iNn9q~t+CyMZ%?b!V z$*vHqS-+;iEkK43IFi$#?rAjBfaMealrkY57t}SmU+Z1TIxdeXHw9#2GdA#<(y&on*Z;)V4OjztO=^9N(V+V; zDBK<|egw;(m8c=U!WR}?$sHTCHCW2Lda5_S7Pd7XRmdqJNM%bVWgK$_%*mdG6(1Ks zrJ)PUUn4hAgJXqjVOZH%vS`nkPvswf_{C%le}@h$NfYXdQQP&0s~y$52&SX>^}_d1 z@hOdo(zoJhhZv{_0sC3I?UiY*?$(`qCK^W*e;oK1Xi;ZwwCRUOvb zhovAs6hP_L_VKjoNb9tD#p&GQf%T~Py^_EzC*8~}b*U>~b|w{8j#L5LFX%rNR20dN zEb>2s5iBwSU*L?TtIC5{{c&~!<~Bf7L}jJ5euvwi4NtTGBqMaH1og%I{0GZ8*S$7_ z1~~X!>T{5dC6yPi0HSgKMmh~QM={8pJbkx)q z$q<~vN@u60e*9bB`KfaK-DTO|`g}{;GoPl~vfrBT?1Qi)~e?Jaz^;G|h*oekIaVRy0x`@#fqM8bFS zgdxa%kp{~kx;0;_>pAtyd-kSdsxO$mK0+(<6fWK;d-c)Md(mhHGgXK9RgTqzY z!8ME5xg7R7LqeyCl+H_~;FOflRX)6!sB%(8Pd%n0f|9azrZI%WmO^HHA=#~G+4vZo zZtDFVVHsOXqm)bak4Y(`40c@YM6B?-`X)(Z@y#vQ53C`u-!}H6e$Il%91Qm%{ArFP z`uzoL#AgulW3cz6O11i?J)E;gN95to&KVx~qsFy8BeH%OFj+0bvGjdi`=@e$-LcD* zM?)*PxXvKQE2v6*ayUy9vnTg!ctY{2TsQ@IrWsndk}$>=(vH_!@C`>k+MWL`0SUIw z_#NAfv67}zwh!F;)8>8+Gl|7g-1kGMH?J$X6NxTyy_eru&Ae=Yt1)*+v2KO*9vL|Ve^x4*;@$m=E=tJF4OfS$5=o}lCBy)Bih(jNA<<|{GUEjJVA`)g!TiDnuihMbgPwjl zx4bFv$5h+-E%r?+yEssG_TfkMK~t$yn4Rb9B5nG}y&8Ga-Lm zVQH8rpy+P(3h$$2S)){$&JYL*!y!+u`BPQR)3v-D8a!_W74!I=4GafeA+bkoPDw`| zS9f6&bQU~;p-n-{cIPtB+aWfUC8>we%)#>(mR;5KaVGIS54uVlvVK)R)V!a-p^aZatKSYy z%}yz|H7C&bJft7N?&2=|+eHLg>BV4P^zLPn>UB8(So#HPlqw(BjqTfe{j;?h_OJ7$ z$)2W^iYFT0a!lgO7|h|AuZ}7hX)&AHhozKJXAd9Jn6&;$z|+DUPtIgR)4AvcN%g{2DlbHA_{wj#F7=Go&)xEv7pDw~ytC${W3E16cDHyw&tt;}N&ZAW3# zMUfZ@yUu(n*v#6j3W5w)=Bsc&8|w!W1$XfT(!X_f@|aMm|4@|2ZVt*FC);$q6iC~6 zn{Oxo|5|`DVG6498*Imb>(*6CB0Pbe21J+i({M9x%%jXuDcDrm7$fcgPGW1CDEsly zi6Z(ET2V~{)xvYG{Tr(F^q6pkVZRaoK`jQGIB~3^0%662zbiMIIN|Qb6CQU3In3a0AfwuLTNbS!=ERWmZqr>w#E{+L zrol)t{!aJsu7wYh`SWXW`Zch@L4_8ay2X4GqhYakH-gubZ3hRGeOsgY5<^8hTP3r| z9Hda=%xfea8D4J?rM4Qwi#3_@J>F=h`GvG4Xg;2gbEocIYJ`#!v6U=UP{(=`(rO0; zk9UebK+>64MWRF&aYZ$uNU>c-axz_JouXQ0PG^;gyu)MGhUZI&cT81bRU@Q?ZnBCn z-&FIm%Y4^fZ zo$&#aO}9ZjfE4ROdRlw`JTX@Qqw4cs>#z8RgK1jARX=aVtdckO$pxGi5aUz3jWA9e z6g$Xq9@=l&#dOCi^=D@s1ho~Y9z~+rP{VrBM^C47c8H$a5=@Az1bj)G?BlU@t+x(M zFX`FiFD^Ifsu%WATOK#(R~0*?g$wNgsf=uO($Qq!$4n&Le zh^7<5@6mm8`S08oo1A#rVwhT4w9=~DD%Hu~AYF7wRl37y!r?k7$F8SiF=iGE^-p5P z_kMwnloaY}AOzjMP6$9}q-V%SjkEdG&O%XAm4mEp@HTxLnDsOhXu~F6IO2TR$mfx@ zrJz()AQxzNuK%9nhTl;|rShucXt0D{q=S1Rzhp?)`UU)o=W;&JfA1~~?sEpva5PM< z=4Ep2;L@?f`=L?$84bQ4?cL&+JS9(@bi80^+xc51-wbYxdn5RgJjLYxeSai8uO!AN z3ib#TEcXdIcYP25xOSn>Xcl(s2wRO?=PiTW3h59Px+fH`ep<_&3|bTapBFbLeYf zOrD}!gAQsevh7#pw#GnBuaojA?xUkT-CQRRCmePQewb(tAVE67< z|28OC@lr+7TL|12kNxwUD_O!0T*j?1B~F&7MPt>X_6Q!=>;%JBebN&DH&#k_;SnN` z2o%G)Ek9b62ugimf2?Kgh)&v0G*TD6RM)BgAs>t~k&g>jT-xO=rs}Q(xjCE&sJUKb z#zN>iB+R>G#+H|GuunOjPyP7_Rq!U9e!wA-x^P*BwV7Aq)Yi9SxGDGC`JMC2B6(yK zm)$I6;*u6eg3Ox%q`wkmRhi7OtJF$zCq@P*Tt<7jy_tqkz~5~X#1o?Vt6kx($)0kD zSL9FBffIfpWjI@Oab`=_Hw|sb%{;m~1Mtm|l7nQgnw7Gu;Xi%PGus530k)~bU%sfW z3+hx|(*4m&jfLp68e1wyE4%lwytFm+YX~sDBxEUNWdwtg0|)fF zsy@!J-Bqr@5j#0OReICSpNgUnB~G7o2EtAFXAjTji}Crr67qJ70}7Nrl?iFcQ?<0_ z>~jk+14o#phn4+?tQu5yjhy!nV38&h_DurqH`TjovtKDiVO|Z@nl2BqqO{EsLK6C5 zg@s>!o8Lp<<9tOF!Z;lnWqtK7{joUqt4jCy)H+FB=4Ski%cVO!JlJBIrXX*<50B#A z`FVZBnRk*v*^2I%E6j^_83<=59@tx2vm&FN3}UO2YbnY#RDshb$EYd`H406S0iiP+3o)e<_*v=EWKLlRrT>Y`5; z1D@#7ILZk}eDTEs+E4XyLYob1qz07IM&%p&w1w~*63bJCx<T`3~6+)}hv5--tki%8>b^=(Y8Y|lE zsbZqpSI=YN8||Hg7moFf|8^wIS)$Nf4pDhr5^zD6kepZbC2Tf9XQITEwLYfb;NGaQ zm}&}d_>6%qa!3`w=%UM>g_w0s87mp$U#rx)WwEVOPPnv=W48w_D8keSi@&AJponnY zuCZIQdinRYd)`@v4^xEr{2A{1v428Uyzp;~HkwWSAJ1(KNsR(;lY;GZF6@Fk%ER?E z1SAl^_iKA8%We&mFJ+j>JGF13?yQ?MVlG&{`~?u{huk>fLFPJZEz=gJ=-#PA72lda ze@Os;sVfP+Qj<4U8kpEFO^9ue^qfR8OUr(>Z>;*#4eZgHza$z65k`|G)Ibkdymq1@ zdh1rubtQ_O%!9TH@g;|q<4{3Y2;psaOfG8Gw9~bT3;(2slNR`frDlfKWHnKD zw~1e}F;E(C1#vhgLWmC2o^{Awx^ABCA`#*!cm0ySG}J{%gRc3Cci%8{AXAVzG(~KN zMGTl62>1gzOyO;kdV{H>bY*es3Al}*tuy)oTpP5OG0OjXR(Bq<#f`iX1dFsU|| z1P=|2ztO6zRajA=A#Uc?Yr4pPp;}WQ254xoJ**y~X7Q?oeK4!IXzjFup<%kX={Q#$ zjXa$Tqc*<>@$b`#0UxS1;@k}8eef@r?oMtCxr)CS2tV#Otg9ccj zdPF+|eqobS*_J5Y?gKyH>=vI66NBzL$yKOsb>uWG$c7njvQz3BDV_<>o$ox6sy_G< zeoD}~4Gz&#yd@Q2?%8{XcBAy}Q<1?3#|EzY-Ckzq<@6M6g5!_w7fKPzKIz$YLoy?0 z;v`=*7<3etjQPY|@+n@!mo)HVNrSr}f^Q9V18E?bwXJssUjWLo>B@WSg947(JgwLVTuDv0H4Twb# zvjiyf<_Mqw^L2z&(I-=EM_v9CYNxC`Dke`pYSEZ&$kq$vk+fZn=c+$5-`398&93Mz zA=oQCkK}y1A0xgfmK!PI{1 zkN4Lz?n3SaIM5T& zAQj1miIPAnGnPi)W>cGQ9Tt7tq^=)-lS|7&MNO44j4vd5xl7m$dPFu|Ra|bPtf$H= zDK@zsK(th1VYTl1LklPAdiC_?#~uE+yG&h8t!~5jEel}Z$pk;=see7pNT7LPelN3h#0w9y--HkeEVFHXWjGUQ zLsj)w3%ge^U(R=ci%~e1jNFD7MVF>)JWfxARKfupb!TiV5)Vh1^*mBMajK6QQ#lfi zir-bG_FVTuu3f#OUW4R)7AIE~eL1RObXrVsAgQL;phOS@mglW?qB4FCqC2j zacbYj&J$K{mNoyHB@$0LBvI;JT}{(~C?D0(kq{#`Y01YIg=MmA+oOc+q^>bjjv7mL zIl*5vQjoWwhkhKGWV+a){*ewBu;Xb#q^)F}$NvOMqsnHozy+7SDi!At?(ilQ(iutR zXx8oQ8_PW$>?fL(An|W6NOM=X3ddbG>NtTOhG*}Z`Tic*E1i2xqsW7+f^0oq;%2_-$MdR) z=*R5EQYG7;7T9IOQ)Jkz-M_FMpt-jm33)?nwtKnC#+te!a_Z#;m=!&IpwQ0*{N-S#yWpkGKa( zk3*;WM8V@`pFB!Guqww&lV$o9p|-fU!02c`lW`8@>?$KtavvEfHjWay`3~Hhs%X23 zM1wO^xzUUp`KC56e;Ole#ATqM2wGK4h^?XZAe5 zUOivNi9W^&fSLQDdpO6=(Ef2zU4L>ko(yQwaC4!6qKB_GrQ=x4s#dM#XMAQ_sFCKx zlHa2OBZY)T(6sN2OJaW;75$oK2Hpp(J>|j?zRfUD#>%9Nr1P%_%J~$Mdu{rG!xgjVOF`DpEXW%%1ygM|M%Sw?ka0sV=i(} zQ$~{UP}EG!iy!m^zrXk1koo;iqw$gGqKFUH?AvG*hSnn@YdTTe`e|CRY*RId*BcM5 ze^eW#e^~9F(@X}LUV(!X3o9G>&KNd9b}PuLD^Fx=DI(mhAyf|G>C+)uzshI1*{S{C ztO})oZR5(LiB`fZ)XQr zPhZfjQ2Z9ln2N2*eb)otd>dQzN@3^c7JbJ+^77#ggJF%h0cuR89x}@(D7h}YQiS81 zc|o?6`f;+i<0|yEz_DeYc68)&_4Gls7SG`r`*d*;^VAbl8Q-VuhhCZNT;O9sPUCV} z?YGe0TKwu+M}zj4%mpu29qvtb7_n2f@G)CZgstG&3R$57-Ls8HeIJ<~e~m@G%3v4g z#h;MZ4M+I`@RAlUT336Q&MtUztJt@4hKB{8aST3@ihMrtALC7AaG)E*MlsPXys0W( z_b$P1`)&=2VX7nzE5W>WDK?;zZzif1DOK+*#x5gjJ4lgB$-r1cv2 z`5%{`!E%3$36=0XGt6c z1;2vKZm5Q-v#^0^2hmA~(bM^QjKU=%ec#@d`tSH4)6W*o(axdJG6>K#W(U77dv~Qt zHSoHXr5Na_k_CmrCF7E`-x3+*2gJkW(e5bA!>j@L9%ze1w1(#T^Vij2O(~`LP-Gp| z)6vqk+HDSSGlOXXqivNqgf0$lL|Eooc6`GKAvI5{AXmScBmA_@#Ea9~))Y~|^tK%zPkD#U=a$5{5-$yvusiW1PABl?fJziy+N?f<-$XHjb)$!vm!rw!6n{XQ=*FEJj5U0;RP>^SG|aaEIP8|dp3 z^T(@Ky%_!x@GvmwF7^)ZkNN1I-T=>XMLDW11( zh;Yrn$s`dAUo7^inqW(wg1M7FtPi8a*($O5PXDk0wnFmKH^q13J4*6jweI9yAa(j(L zTiHc*W*mzu>HPg*3E<4In7Cwz$>b8HbhCOao`~;@C*9*UgPCVKll^9Ic`QlQH(SV# z21!&Sf8Gna4qf~Vj_s*X>+PzGGy-`M^Nz`9)&4qsJOW-_tqt|9EDsi}=t$BduoMtn zSwfz5VxbqsSKE?SRA;=|GBKhN1HTMr|bw=?F-mbWcMJqSjP zO?B0VaZ*5qyj35+J$y)we6a z#DrRA3qqRP=YU%4q>kUxG{vV;>MRWSA#_EigWWX=t^{n}B*)hj9Y8N-FJ(eRJk%XM z8xHu85ydo1=yY^-d9N!9+MYT6zQY6^a$99dc5a`n{WLWy7`uL!r>Slu@N)hqoe^&^pTrE-G z@@sjOudyN!OCd2~;TPQ0HHi?!9(#4nCu9xwMRNXl%bBvJ+L&blk|n8VNdLM{t?XI0fw_QgIW`Mcn!Io* zkexx(-c69G*e2`b7MgYND%e+f7@RhG@Dfl4I3)oH71@!;jeu*4BO|iLhMOuY0>i|4 z-aPxrxIM8HrW?_++GS-yL~QS7!4DvBc>rIfO-$>$o|}Eb@vW_z@gw35M#1R2MWvfD z%R4>lV9#b#P;Q3VdB+9HZKnA<*knVfa z32hAH(t48do+tCSu}-wK2u$Lg0L`Mgj~1pz!qzI2Gc-%)o1cMCg_;w56-YWTI^f}2 z?5W~V3*qO0Fv1kJQN4$Xp0+C8!mb1W?AzG2p}Hn5#nUKQdiOoidFh*IG1b6r>Sd1Z zyt&C5>Z+yU0`VL-zqnRA^Ck$G-c@v95?(5^wPR0-;xkc;gFl#mt$ofV1c(OtE;|!x zr^Qq8@YQPpPW0QHg2&3VtWOlaA~(<$-X2>X#MWCAd%?G+lcvwj^7xp&Ja7P8!ltx< zZaSLgw{7Pr#q~+*Lr(Ze5%?A^0`4LmH>0lbyVqz?u8?h4CXbFQf3S18I6y1^Q^4=9 z0B=Vb{kTl8eG1f%9{TaajBb;(84Uu!!d-yL0f;6lx(Zl%pQyHN@wBRdm2N$B#HosH z&P`811MKKi}Axd*Da-G|fo*k&(2Xiw?A)R5sh-eUL+F#aGRuq6W`@82Xt1 zT$rYasS3B%Wt7<}rSjXD!3SWI5F4?Lh=FH6@5p}B8o1U%XL^G!9vE(#59jw2o zN5u|1<2{@SU4j1a%La+#7PzdkUyu8>Lk@X=xATezM0$6DuC?gpWlVr3?ms%h0MXKS z#sm}-&3yJZzE=hvu{TaL4I@r98c|m#j+1OJuQD!g%SLa1m42`)s&85C`3VO;B#6B{ znp^mrh2JBZ|1DCy?I+Csa#f=V6-9WfGCpFhiNJ86uNb;j$lnUWZIHc z)!$!WdIlIjABj;Nm!%>me{az}hc+*&2!X~wZ)1kZqXI44MO_RpzW*ZZ)9wxwhQ$$) zy8_=SymLnJ{XZ=O1W6i23Sc}?ARt5lJ?p<7rSe4*7%+rEfaeFW@L!{Zhwz3kU{A4} zYV~;o8JywyhT;w!dZ>D}C=>}~Nv0v1D|gv~A5w2?@GYbxSp~iU>5!Qe)RtpTg##`2 z7kcjOdYDxuR8kK#MW+7y;b7&TlQc2j;KgMp#L2j7o5 zXvBd>l(&RfgE`sJ3i0D}b+Ze`8PAUa*2Q_{nTJ8)Jv%)F` zkwdor8$xvaQ$vFOc2yDIE01^l+Y>>b`}dZXmYMMS zFPy-|2k(GNq1x}P#)dRF3IKUyQ+>bRofzamaR~bh?I+40zC!-kFjK-Q)6baDM}^Sct3sk*@W2xHO!bK@F!xQAIdF% zn}{Ei@up2I$&K`RL!{&~L%pO$(Og0F->%`s5Y|4z2V}%Dy>E7A# z|8P7#ImwkJW`Mm-I`RK-pxAzhCot$q#{V zmtKdd>OSm}d-q^uE|^Lj6cs9TP+lh>f=D&rnZ`z*=J@wY?;@d{2m5&pSv(wKe@n9b z2am3Uo?79&7^X{uQQJ6J6dt)LARE?~UpshtfHG>7R;zAqXO3c?l?mGI%J`I%svR{k zKEcn6&pTEvXjbQB8mt)P@?HW3$@p=&CmnMZy~`N|v=y~mqGsX?sNp+M-3JOhJe4v% z$Y0!6vkB@U1J#8tu82CS9{!zZ`{%!}!+=x^%>{B*4f`Pfr)M{XB`mZ}kJee6Z>y%h zmbr6vIA5+;*tFCzD18Zmw79)RWNW5Zv4BGv`diW=PIl0cw4I#+hrXRdx1Z&xKLoEJ9mPDO z4fQ}E4U0W1FMEaAsA=m#VC+8- zv=dThzp46$abk1QCqfmtI?fHdp&p=cv~x9>ZL3%`ajWoMrL{F^ZocJ@hj`rf1?JKE zhS}Ta1$Xc&+~|fzp_il@<=3K#KJ)Q@JttaSrEZ=3EVlbyoEBaA7M<&3pF%K5JdZW9!!c4KAyuT6GgP5Y#JCC{KtOQcxhyH%7#YanXh<@^k_Q52yGi}@iv z(Oi~`n(#U1A+FQZ;eZ79Q@)%f!K(jddx$G{bRYQ;v65S8fY0n1D~S%H1-~ExaD98NJmb#Wj_%=V4>qabj+nsawW!Zrii)KmE^2v0IR--PEjcsbM>5mX z|Aqa&_+9&q_;PS4AASZbLTDj|+k5CZ9HLsWD!b~FN2BrOdB0&D@f~R#<+(Acm}XjZ zN;n~V`oUDO{CAmJNE32P8UUA)#eM(%B3F~Kmb0NSYyq9|C+{bcoa_){kmkUeh)n4A%w zJZ2PbvGXte8CqQ(#b8O5SZ|6@-;EYMUemXxNhI7J{)p79%9`fp6#1geU}>{v8l}vV z?QyT~J|EDmvE{xVQ42$AWVijiRH-jCk^ISeLS5|Zh7BJ%NPp@5iyo4Q06vm<1dzdMa zoy2c(B?ygrhShn;0{9hP+&pM-BKTORRCIoUK+SfCq-2RmK;CIuh)rcDfb0wS-7O>O zqcPsrUaVzpt1CzuqX8$Irb;k{?g|c*0neLZknNE;8Ug?1hkPZ ze+vziDjum?PK)R=acK#LH|v#vaQ)}ZbsA9TN~M3j>J5%X4JqmpR;&@<2`SkgiftMqQ|jM;{XZRqBlK$ zz(MC?|H<>>t_Ve>GuV$$Q`=&}mS(NgYw@1?$|4PIdq0V7Xwc>3lq;ri#f^rL%y3 zz$GDXwt&}Cb_zLwqWg*Pe&Q2EW$9BRyfp)v(8U(c40HddBBXNYSVfgk9#FXuL*zWw z6duFNQQi#hF6g;CM)LqdM~WT)ZAT$sFSXnb`IpEoVRkErmCh4swZ%P9pQ&G#n5wU+ zeSuLmhG%s&-e$W(tkjd)$nDFvPN#D$K^4!flFEg_IHWC4;3R9C@A#IcBWQZ8wFtGcD_s*TiQL7z#bYT;cjahIW$(}|R{TMu87aL~|m@^&VV`3NR{ z3qDgI9B+vl0lmJwJ#0);3p~M(RxzlwRt60t7GR|+X*)3hD)fbXMde@^gc zqU{Z&*B$cDiNeP7>brcRR><1b*?3;4ydW11LBb}YGlwB)E_J~?D+B*t;tCKbnSCxw zCTnG4ntk`-#oeNc`=?Eks~Oq-EU(;N8kV)V{E+=H?b{qN1%205Zoi@Eva0#PuLh@M zp%*9^2yS2hQWERZ-JLibQz4@$s204P|30ob~inO6uN|f5MO&7LxuP0;Vljsl2RQIMD+m&s@0r?Kgv8yr~WZv-(s58FR zvMYw&RTc;( zDmZse*;Fr6<0;}{Cju$XMR?5gt=qG4v68rRrR+*VL(`Qjp<7ad>ffjlZsI%po59`B zyDb_ZlYcu%fZO96^1mJ2i1lZW@lGi7!h$jjM~0sc!mDOvXQ>}J7qcrcb+G-R#1el= zZa*)|!y0}N^8zx{*e>UBIgjuH+0$%t@S`}wwRvq7$PEE69)S*jNV1BN#t07FU>HLj z;nQ`XvK0f!r4gjvKzPz1S3Z2?D3V`kQ~j(_RxjYT|Gn$B^b2dOi=ZF@uabu}giX(% z4-aB@HA-||S(K-rPiSgaw#MR>h8nio*(z21?j+y7FeXv8kcauthcxc$LDkK~EblQ& z|4_Rn|HP_fF_g4%d-1U9{JHBLzf1mU&+L~*i2xHr(Qt*i^>J$<+)eYZm>>6RAk&;* zd^TMj$$V9bGQ#%$h2y;I&nPH*PBcd_m8x{@r)Eq$vIfsHM;rFQyUVDlT91&V8=MRl z(WT~U@r<9qRxLI;492*i34~p%Dm8KWu<-@N$8{rA3HI6f4q5@PJ$&B&Af^tCi$1tLBljmmR>QH?T;Lm%-U|LPrqIy&3V3*L6NWuti=+Pu5Z0LmE4;oEV4KhtLzp z9%xIMW#kTTo%3$Cb7D}Bw+~IYkD36sYB(3!TZ}(iu*)SkaO;k%SZ2|6{Uv)NVhZM7 zKqokLmt3co(AO=H;yc}WRfKmI<>e&~5gTo}ix3Gg2=oOuf~TH30VkhQ6jETXolhT(qyFg!t+G2;nd(Qe3Q_ zOJeO;BcK4kB|bM1uy!Khb0FF{S_Ts6Ppi%j<_2klXn}W;xku*;Fvra#JjxkhMnkE} zCZR|SbpzoA9}6vMAa5#~q)~c?Ouh}K9quRq_#gvx9&XkJbC=PVPJ#Za7vNTUUQ`$P zQK-)=zC#|h+v)sa6n!0Kmd|9hv1Wb;RaXCzJAMl^NiHh#hAUhsa*0cq8-K>G|KwlE z2B~-m2UHWY`r4*jAN3N3&e(Etce?4)I!6EHeAdnmeFIT6>zT6Ovg)eaijI-m&JBous%-V#;GHoytNZ5fZzDSkaYJ>zrPJvOt@5u@`7wPQc39E=sNauJIb+gmBq;2E=pxXC1*LzKG z8W6ubXoNRcyF%m>>^MZGwPfZuzHlHYD1@Z=Q`m2kZ^KH>bvc0rL@f?sph%J2*akiU zQB&v#S~A3h(O8IjUXI)JiG^pZ^Z1Ci-7oBrDDUsWK~zg0Ixq9xx9KJ2$v+kE-1OB{ zx3GjuOYt7SUhz5Tn-yO57a(*_Q5mMMao&7Gi5$Utt;`G~;nrmwsUBVCd`SIDjBbX1eX4>3+y5pv@;b;Xh!J&qr=^Y5Cx{Bpv%*c)heOH8tB0 zrGPEjztm;|h%o{Yv!a(HVsXkzJIn@}T0;R4!^bvBoqA@3f0Ns+Y`*O*@%py-;lWQ^ zjD=FhnVN&IsL514wET0P|3iJ%0%~44iB4_b%egrV5ehYfuB=y=W%6OnTbY2zh3lk? zA8{D&Ttx_rDv7iYbwN;DRo@_&QX5W|`KlGZKo^f94{=6VImnto`0APrP#iMDp*H@f zKuHr?5xY*#ubpij*TxcB3|B~$^+&ss>GTk^qvaVo>rm!gOC2hmC(n_iZ&e%)0kH zJ+Zyhdg<1}LTU~N6jem&N8JqD-3eDw=nt)F=wX#;piI6_%#l@a^ldXhxQ;%RTTD=G1%fB01VN=(v!6#b;eEPktd5Z|E^>N1bPdtO_n5rsa z$7YH{_92$F{!TQY^XpJeFD~mp%t#8Qo848#JN#!liaziq*PVd^%Tu&;?-4406+kB8 z8CapRLjlm)A(;S0C$#1(cC)n&=M=TetE<1peB*O+sZ8_Sph*57mtDtstV{}i+$hrA zHHPmpr}B2xeuiYy?@7wazE+qIsbiy`R1b&$PEIPex59PO$p9^*T;iDm-Wf36?x6>% zvk2{tFcXQz!NmDQ6xd=G%f8<&{%cd6TEL2b8yQubg_KLCS?Fq~u83Gih;rmC7)zwO zwzik)b(_hMurbwR9(w+0IEx#No|C@6?rH6Om@6K*K~)uk~>^KyL^8Z-OhW{}Co z9^lxWvr@{cmpRA@(j@0*GHZ4K1mszSz-56H3!v8FYPQ=?u2lC5n;3+cIGA)_%A{Sf zlCtt&G8u!%5QF5ynvP1uB;oJ>C`0@l|F0`Akoa#0)su-QO3LT#1NTPWH&63WTPeN;&^>*<9u@-BLVw{CME$*_FbRB;Gzk+n7cv>(jE+N#FlbRI0IV!tD>4Vl zRegBqu6kMeG@*|M_>DN6N{Oj1Sfsxbk+%rjsPaU6JG~WV`@pt+S!Ppn9avKEai$a3 zg6qLwe}oNUq*>L>aZn4$Av7G<@^FngcW(ujeCEwAWx}_#ixcFVEH%$YP)yc0@{pza z!b8@yFpm}bK(t@WpOV?w=Jj6OfQ?xD z4zBwN^tWKOQdL8c6i`lPv7n~}=QJYx(DN&?&VUQv_^bx5H9$u4JVc6iWG$ZDHY7-1 zY6=04m$a5TO*_5S@Mr%2=|6il*#ThkUP`7+&Zgmx822kM!A%7eETC#98SlN*(zp`a z-4naocwt@lnw^s7(MHy|Uk*=-6pi#{d*RhGq0kn^+kQin)LmMh@=m`1L)^ZvPtUDn zRlBDcZ&aP=Z5L(t^0<^~C98?_zl!3m)r-sCGJEpuvFx=3$5tke#+!37CrRDmW9%0R z@0Y_Iwq>HoFKbua&C{HbwYgq@^44`op$$Hs7tzv5{B^QML70KUKe$cEM5VV%Av3a- zO9}>TB7ha1c*_)A|!3P=iqd{ab8XBiZiZh{u;o6^fM~TRydKg z?}o53la32zV&i1^@)mGUW$~LryJP>WEm(hZmrrUB`;#9TtWaA*-*$rYfC%Uod?(=l zJzQJNdyc0g!}#0bjq;Ef(R4(r7q0O@Q;v~(XU?=lQUc6*=IoG*K;PY;qbj(|uf3YOF|9-1mfiyw2~WobF%pV^5QY=O2>$HdM5YPB(6_|jb+ z%2_ijfnA|cZ#fMKe?s%s_DGHL)K{s?CV!u7{>64}?av2?Sp*@o;dACJou+M{*?Xa< zOo5YAIds=A{k|1I$`Auy1FL93@kW(jLeAB$azv4{WKDuAH;}I2YQHNmHZ}$)%KRjnVCf0UFXHUX<~FN6 z5Io;_ z!~$P9!M_z3m2epP-`#t88`t)AXwFr|vdOxcTKATgkiv!dn2bN18G6Do)VgTY)(T6u zsZmRq3A&$DIpdvbFfbP~X(3SQ8dr~sRQY)-@b~SqMB^@+G5$s}N-W;R9Y#slK*o&n z10tqld{)1fqoq%!Fre5s_p^iLFDiRig_okyRqkGMIBN4PKpm_Sq;l8^(&%0ISWzBX zkS}h;FZuO^y$4`=i)zVn{O_bxZ3=u>9wKf`ZKu~oh3AE5%OOixVewQg=cR3H66Nr) z8+bQ+nU&ZNBg~aEvdz1jl*bDHBLvEQW!_ZV_AX z_r3>&k{DUisfjy0NY79_nKTi)4oyly zd?%b{U2C*xp820#PwiP1!&}cw$2OOKyp#Ll5!)zwxrPIu6aX2#jCo;cI{&~4J=~a> z_`8ue(ZH~r1`jJnujIkpb}bJH9@RCqKGeT3%efh8a<#wR^U5o@4SC0?1PQdrCgz;3 z?udQHtzEs1T-|+|Noi|8$Yd6H;e2a3Y|VleSo51bdkuL+_PaCi!#v2(g9D}SO2jbe zY8gIL7nXcjWkv}u{|7NE$jM(HEK37?*;?!)8<`e%MktdvsoB}Y@F}H>CqAbp#Ik<& zoEf(-ZwDRx%VnPR7T{Y`1N4R?$1Ge#XyA{voZHfwuntTd?#ljtH?UNuUNk>zY>;okO7v27 zo-8SJkBrc)tun*!mKE1Rb@N=>VDBITi5Hp*0mPQNQ-28v6oh`j|aN5Dj`?&K_>ECObkaCP7NgjX99*#xPvxBXVeWGY^R&RiYwK9A=S$JrC&{ zV><&wz>TEqwrsZ4=&OCba^#Y=cyC&@KfMgjRUqy<%foi9k1zC^Dd^X7o&z;mANCwIC0#6*2PaPP`KavqASrF@!S-LHf;d4TMccGPK+^evmaZ#W+bt_1Q+MYnfSgMj%&fkfNEI0O8BOJcR&7U4w4;1$I+dsc;P3zmT(ICO`j>=bQxJJU0)539oMbYo; z)33kE5CEoZ&;6Vg&p2$4fW^nhPy9t$nsmDQPjKlR0krT=w>{G^{tI;UU2Od%eS4SDCkecaYDJPSXmtJcsmF1 z-lWtS)XN%RNv-5bOM{CbFJF`0C7tp_jFx$^Yx046vQ7EWk<|+B^lVFk4(xccesKB7 z)rQiG7F-;vjv~c3dDZt8_cwxVe}s447w6UuwPVjKIu4@Z_Y~KCe={QY@d56Ak^+Kw z9fj-rmCsjif=^KNOiVybV~M>jDr`^X$jE;-Ha7oB|2KaFp}(BRv1lBNPr@Ex@Bp8Q z>O87CGWX>*Spkr=uqLY7l}rgP6us=(T`yC8yo_{tyYR;64MjWcVQLubV7naY8rqeveV8qQs^i@cog8!!lkmPRuFf80PAFhVl z4l?zXr*_wie*gKl6_$>Jr@8@jPJe}!o7?0CD3$o>jL$WquQ-~y{r&S>GcFH}pH?1gYtg%UhrRY27yV1{k!3KAv(3KE0QVl!mmdbQj{DhIm1`whK-hmHfV8xHU zYu1!q#vX(U>a`7vz`X)WnxDz|l(AQisvzx6pp#DT9MN(Hp`rjGl5c)0(YSTxXBmJ) z+Rdxy|M0mB|4AeMGoz0pZR15CAlxeS^Z0fD4ZQTp_SBaX5HNo4@I`=3) z;0m5#9fY8tu|1+cK{9)Q0xxWe%Go9lO)!t-lWT3rBgB4yz6{+32d0W zyypI>BGj}Ij46}j5*U>1B87aZ{ zOIedu`_+fV#dsdY*!lCsq_55?T8qV*T1S&t%G?T?h{zZ1DdWEc-9#BskIbC$Mtnal z>MXhimSAIWiJ2qfSo(}jwrZfI>ai%aMRL^h3W8g8jysaH)U=Y)(9&?d`*ki}9XGIa z)YaWJ)HLp17D~|LnDpB{YT6&p9{?uc;)3B~Yf<3C=5L34bmqpI@H=6+kfNU%uFZ7s zvJ>+Khhrmw-|245Rq#;Pk3>b9q~X1r-+ZpWZVo1|E)D!M@$H(8j{R)X2Z341ckwmU z7rITNF+^M?B=rIPd|&9AAnH5+4BOs$m*P0Xo`tR~#jNCY{k(i1w&y9>m7%yhRc?~O{Nw3KWF?xx_Fx=k`-*tm$Gx2CM159pwyuK zf^;lW&Q#L)mD}m+`iqS8{`i#t9|eF7k9dj$kLzBu1!s1^^riesiF!laa+R(wClF>PU+PR`1P_{G0Gyg zMD2OX7V*HO2;IM4Po0(Gua>o?5lTKLVMpG7K9~5+Q^rnFurBcyLLT8%4k-badh@*< zRuXuc6dY;u+t*9fS22EgaMOAOBf5006!<9MLOL|*{%_;R1uw~cwq^}-v@2?7E&ux> z`px5cr0D*wxUq50_M^d0!7W91Wsj4;OL7w$4%%-+5`}V~1>JeM|E!t)qZ(f5E>jJ) z`X<|5;#;D!1NGeMu4JyCiOBVC8gT~_ofWn-IVH*do`vP8Mlm9HQHO#+gk zcg(%zhZV1MXryh7orDuxd%aN`=j17W-tl+WfQ?JsQzWXH2$OkEggBYy*kH_s;B?4x z#g;ndA-nvQ{PQ7bTCxt*?61(q`B+oSPKmN6lPY`2r0~}KOMhOD&P@@2*OO5WCX4$Q zj(`ro^ki4A$Ww49khSepQ71g0)k8I6H4Ptemv7zF)5Uxp*||_qfu>kr#Lx<#2*vgZ zXEAR5I1dY7Tx3Vyc$jbTwuLnOz3aDaA=tzFKEg}W(b#|Pzf$nnO4er)A|-hpvGEYm zevVlckqz+CkI$k8qO^hg@H{Gdn2rlOH(U>)_D0OaQ0Hg)cs$2w<(1k@|a5wMC>3q zTk&yjU?*^?iHc=tX@PNHaKc;3Ko9dl_xBCT&njezVTSI_9tfy7-Ak0WThhNUY+9tP zOR%HK*m@4s$D4)75)e&WGq09xX;IPl3Dt#@apl=M2fJ|zxFh42@qr*P->g5Z5|&3% zM^I2NO-)BxWQ%F^+=@gggSLlFeEmBvc?J5AmCLI{$w-b=VVT5=Qfp=1S@jEbo6J?5 zVrw3>Fp$7Z-I%XWO;-8R0D(Z=pHGdxIMOV`dvc*wmFBg2@eHs_bwd+QZbPT`|bbkw*d$mXiP3ZF#aNx7VM;Wi#C z>>b>8%E=T1o+mz65y1k2*AUSHo{x3=53K+G2^HqTcPxR~%RFBTOhKSC@wFFDxz21^ z-nMr+(iV`{BLgwyC2Z+Fr@u21aLVjoZr|HO#p1hoV>-;rcuuv@+5T!~b!MlN*wy3H zPef@aeVYJHE!mI+k@&k4@!fT#k6A0*HVb^$#oM~FQbiV1n_~0kyWS;V^*P-yA20yU z+K=-0d*!Wk>gGvGT2F#~G<1flxGqYCkKE4MQe?`tuEY2??oikMKMpDoH`UUB0nFJx zE$_pW7;@?5a(xbD&5hhqtU!m!d>2SxK%l1gXDC?NbKfJs z`Fru`0VY~YV%%FWy*P&=Cn*6%$F69EoA%55Fi&$XKOB9g`AgP1y=WWu=pFrTAO#;y zB$Kk>(Z9d>V zu)@?1Yve&G7sb+FG`ZA(vONtV3GLcA<}F(~u>DOKx;-xLiZWuw=X{SF%_CVivj4v) zO{}>P-VTH6Zu947z5|d80%2r31nlicYm5s;03i*g#E-Z7sJFy_fhd7SLTCIOsEVC= zJ!#O0dp_Q6 z9h#ss{jqlBA2WHG;%reysk4~Se1e3#UlxQQLv=LV_?|x*eDw#4d5d;ZR&bwF*F}`r z3LT?lbZ29{-fTAP%tL4nh_ZEdc7BQ1&|6#vgTYOL=Sf0|GK}AIspy=@9O>aSbyQ-) zTMcU(rAjo(?DweyfAj32c<&Rv7sdc_FCnEv74?(_8Y#nsV*|Oi(7#OG1i)#JzR~^% zS1erTcCmR)kO6b?+7DhG@}8ZMr_%TNWNXvA$sI;zKgtdZ{%HHmy<^%l7&GX&5FO`e zxFmJ!E4sdBM2(j-egl9-d3m`2Q&zkYPo-Aii*8>&RZdAyAwh9B)lqRoL-OY{t1Jo! zj4fm@h_(;uxFtO|t=G1Q9O1ol1gYZJ421LO2o?A30|eUGLS@PzTskN(nV!_b%P%N7 zKH*I#HPqQr>z^fc7Eqx7a|P)31~an z_60|HmoKbm)W7#Aj{Q+LXvyPt^~uO9)%#vMcsec;X8MOtn<#D;LmJ4KH}M2S_>d!)`}u0bXjXeoO^>|u zr#M-vUu0$eo6w`i^mF#gUh&V>@}lfh*s7L0BRkYRFi1;ojng`<$M)dQaI61uC#rUq ziJW^dayL@czn5aVe-2|gNvAEkRU{&Bns%0mZwNL#3+K4a<9RO>+=g__a&gFVp_)LG z(tON?x&1@daQ_Xozs?+L|0tbpD_gC$0r{y^9jF>ihhw*b`#B^Xu(4CPyl!#U><+l+ z!SdZ9-AWvSgQQUQV|{+wdWulr6tQh}w%uPngmCp<-TV{^xXyq7S{PnU{w|>h^ zM_O+g=3h|4UXke5)$jI}#n83Qq=X4IWOfH%+b08l)R#NPnG|-vI$;81bbbbkmgzI# zWXcnbN!f+iAXcaYl6c{#dsbkt)#p$>dq}?JIM@elXAUUMtD~Q;cgX@?h&}I7IML1X z*7~m1!h1rF@+4+1tM4sQRDZT1?E8hGxUGXF2&*K1#Y-4xsC~s4#)g;2~gln)0J#~;>z1*D!NN5Psn7M}c~A$HX7B0B_Xe)R5a z9pqu1D75r#z)S#diT0)4HKOoi>&o_R?x>G?vSmZd`&OKWILXhYi(%9t<{$rE(OaMseICO^qT9bLhk$A@86#O^B|DlD`s zNQ5&$Z4R)uLJg8}Afa8;gT(&z;f{9D{ue;bYm!V?Ic@sP5JNa%a6Uy?Cv)|{<;Tf`q!jy2DG zYWr&ZU6E`CWTXu97bJGoL(o-o(|Ko{q(nAhpgTxv)MOH8@N!n5#dqCgs#emZzV=UV zbgsl)dA-H;Jm5IuQ#YbN3>lDjOmK~emo)c&si$K|4xJPpkVC&ttyQ)R8!XCB~B*GSy>gHAeW@W|a%v*^ zQ92A92jH!2M{PETJ4ZyL;z0IfQ#dz`-d>|4AtTbXHl)W%y`f@NY~YO8sDl&HI{Xcy zULS;eLf*3FrK5q0Us<`j#1kx7AHF2B$(OPwiH8sp;>(E*5W|7A8R#v>!y#s_UIYQ% z#|}FaK|AW7tN$&?S2-Sw0atC zYL7IxY;I$$s48{i+u?fY%HDfqA{gQ^jryXa-0!AKh&)ohaVHtG6T8&vnzE46doSo` z?ZB^Zp5bKpy@6V?Nq9++bEV1ZuDb-Hg+ki;Q|e16_f*ak+PthP91Dxmw}uc$BtS$3 z%3kn^NLT8+W|SYm6xnotot|X7Gk~5=YDoiU+;4IPWW&G@w&zljLUSo_p$EBXGp$~^ z4u+qVKZD67qrYor3{4-oEUm1xZr_`N8$DmIhUVSlk3TWEKzPX*eT$#$lT>nuzzKVYUI3&|62Pibgz7{mC7td~-I1lr= z^h^o@kLe<8?K4<5)FyCK!bq8ky#;0)v3cXpae@~IQR2^1(4wew4?#;<`dI0cPtXh@ zCDukmYH?=<%D_p)aA*2j)NwLg&WjZGNwN63B4Z=245^xdUI_-+5|#8;ksl$StP^dE zBix>Yz1$2j&vx_TX&2?-(qMnb-8ZBt^ihuZp?+-T8Lw&G$!4LIq)#m>Y74d?_{;Mz zqEBl}_?&M{4m%)}akgO;L~l7-Racu5=r zcb6im21K0jgQd0HH8q=b&^dx$LZrq0i(t2AG)UFRcX{>th>^Ev?~gG87&>#qm|-T8 z`;G;*2ehk&ege81H-9Gh!3Y~1X^{8#`J{kRGGNuljT11{3;inD(9o`FX-B88w^Gc% z{ZQC98Iu%$p77KG1G(~Xw>E#G|o2NX`XNLWBwhTJZc0aD1 zpz>@Zi`(znILVk)$3Y6gJyx=KJ@P_F$dpdhPQNyz=STHXU*tj}BN~E1ygvgb%8qDNzC&j^;OgaobVhwZN~#kQ3;)A6v7c4m~{9r2OILRyDy&&rbPns04wpO7fc3Ln-T#|v%{wlF`^LZLwgp0z38;&A z{-vUE5atNpW7lzdb*K0N^Tnbs*YnTFZdqR37K5&^E5DPA$8$4E zpVR5yJ{5)r%=>qs5dXM7+k4a{{T`640vgO`oUfpYYo$O4sz(wJQm_Z+I<&;^nE=f{ z9p3F;B z!PGcKAN7vi7hz`opVw_RA*DMRwt?pCg0V;(7m=ovxb?IW$M?#90UI_CFQE(P4pTz{ z0RtIVrKrGfz>cRI*)*1h`Wp|gdhf%X{^;q0y3BQO512Wl6q9H6u}=K=LYHPF%i8uBjXzh z3iR7Y@W>KsFODyc`3v$kh>7}5iz@h+ZQZcCI_>D!XMbeScF-PdAmD{3Ljz^$WP zYuQ_e(4G|7I%b0;c5o{M*jHv`#D+kDxA8Fb}AC}o2pD)H$ zRcGp^@IMwMzB^6hmb4@VC|hL8eezjq^Z$%1wc@`@q@=nnly)3z9lkA2M+DRq zOQqLm3}cf^SFWR+3#BJ ze{M=vPI;{sPfbx)C54ZS|MBU*F{gDS&5QhWwg(u_vG zwq^J>&-gFys9P)p4GMqO7~ci;W0hu~2Y0uG#f4_(8K@WB(IV#+BL%PtrGFgfUf2|# zv2hyMf!e@3GWENfzs1=>{bE*a<>LL6hAXsj;OYcfTeoV2Q$0OGf;DGGY9TKAqH&E> zhrZ;ZD_}m}K5Nvj_-d|T*P#7-ohOPJ`uTTdq7-?Wt{gauJU&Bbe1Ly_9428s#XGg= zJ22zJ+DTo7KevE^`eX{`QAeQ1O~i{@;N;dFxi0+7S(6dO<)yL(vogMPbQi))ia>_UN>(JKlpIW`bX|4&uZ&IVr-v;i#`E5l@ zRk|+q+IVwip!~Fe%@IC`;br?|HrvRb>jEbZx67R1z^1!~nylzc?Hu@e=Nakwb5)Le z^dWiL39lDJW_n&&s^@ycV35a_))>FB$Po{vqRTQ06%03Bv;OLnu=l`N=ElcLT6gsF zc-*ykmB=%Xlz8SoUU*H2na_lqg}a@3Wv*jV^vckYzW@xZQnr=M%FtH#&3V3On&eTK(ALm^wxuv3UCCA*R((yZ)|>L^I4f5d8Hyo}`H(OA zmkbBsCleELeRCXG1%ans2)EbZp}|4ewl_UyA*K^c7>1F6uxGKNgap_457H-bktg0U z98B`G=Bb>If_GIXUSaEpG_kDGKSV)u6JF62l>@HFDB99}o=d|1p+t)W#D@*NLp<4s zQ!Q0}qk_6dmw$f@xHAk6RTo;-=xH6wOh<)PajM?6w%zIVXXR|G-h1W(6}hh5@UcA; zeS^BWFQtmyy`@v)lAllfQnZlBOU#O@ftp%BHCdKYs_If{ z=zqWEJ^}Rj#?{q5pBM>E7M-y@7>PLZOJaSt7k)BEB(!uvI7m!gdih)MahH~-CyWt% ztHDJnYnWp^j`0(9GVPjDD+$DR>b4mAnA7%>JwQO#%)^J;BhHK$jkzXAJAJR^?ecaI z7(vo{pDuUQsQmgA{m>J<+b$TjTG&@S)6l{!W6d;oZ($*Mr`ol8zZ(^W?8d`e+94+{ z$#lr#c;_v%HFxt|EXOVGlJ~4zm-8$S&S;dmufjoU3eDRPPVX-axAVF-ebwglL#*ps z)arM^o5su`p1~Dh?QhqTbMRn@IX)>xBEtE8Zf~uVO2wA z#P&(|{LSv;KZbWYe8T^$ch+{iES3raupX zpcmYhBMP`0g29O7w0#}+w>W%kT&Oj{QxmVx+TZOYmE-xhF^nu`d)D04I{dy0J%8ys zn0=A4KXxm6|XH}1^#Hil*xXm^A;TVNnW=m^?B46zWN6tJhRmD8= zhI}`&@mMj9uzK{gw2VAG)0q6OB2wnF2p*cx&T}*{X&J?J|DP5B2;w|efBsT+j&=$& zY!_N73bSj`!Vw;R-UbupXD##~A#yu2n?{zSfE@ce@FB2%Q*OHGwD5cyJ9ykEe5tuCt4_a2qs6W81cE+qP{qNz>T2-MF!x#s0QX4|rz4tej3paV~1Xdn`-S z@*-1OvdqDfK8L?W^oGP6%0;f+#Sxva`!hB@JwrmvEvvu^Hga?+wd9p5>OIKB$EMn6 zmAT$^rXmV^`{Q51*APu~wbsR&s5X;|W(I1PEX3qULl3z+c}i+gkyxNl0)iju=;)9k zzNSQGLTc&jmuOTfGo;PB=K!cbtO2GO_m7{B$1lXE&a|)91d<>X>JK+~dTB z=PTDokoG5_UtaHuYXtb77z2Tn0Fb;KlYF#m!&n7MoFQ%Rvg2x~**{+p8@G-wCVuw?K|qOiehNR1r=I6w<|xJ zxIR*FClPat!=c2EaYDV)ACla}4$9T`UU;KIB>3f0=YVvy^Uu?-m=<`Zr&Omti#@86 zjlA76h8)08Mlw9ijLe*JfMNKUKKcYXYIQ!42q&mw%Nv752MYEvRBrdVO31ZwtiKBD zoUTW!Lr?v;aHG0FgXyd<$>M_A|eref3IT!OQUx*Qdtj>TtG^2OmVg?q6qjJ!cxsqmrGwtO42v`Gj z_qO-^#W=VAeZ7oh#C=16SpTXAHI&FKVrAOI-PNs>_xE4<*Az`PN60fwx~cbC zO9_K1?kl5ets9PD3h2f!<>fG_<n${pIC<{ou`A%?Y_*UqoZN9(fwtI) z1J0UaEM?C)xEkP+I>t-8Upszf+f)So19M;{dY(tV+p=4DLfnc!rSb0)!`9Ks_}(XevJokQYWAA|8$1x> zWzQSf5!+$s-*9CBa=JgSXc8~%c%Iik2R%$x#*b|oY8g~%l8>7^lJ+quMyiG4dpH$e zYFL(t;f!HAnuBxSrO)f=}7*}8?cgeTOV-PE9Y8RKk_kd zX{@bPS>rtT-yWzx?FImg=*qbJf>Cb_@ zuTXgVWFpP)lQ!VEHRL|XJbF!VvBLuOgGhLW|8D# zh9v6>@s<*^9ki}tmVelN7{mQcA=DFQWrHn)-FVJ(G!+-EOw)v0ruC!cl4aee_BK=47#=lez=>ju792|Iht~2(hk0qqg=55$nM0hj$8@Jq`ua zD`T7R?Z&&~s@|R;luc|h&H2Xkjo9U#Qb*zHvv!B~ma8{r>zw1j?EW^_BPabdMgQBR z)|&A$!4GR9;s!u9nSfrsczSc|z}Kl>a_u%K9j&f2%U5Ry*hm5$k;cB$EepF}IAm1B`mr^L%Pp*jFxCzXBI zlm<2pMTEMoPM#Y+KQUMBt3iVd7y!ZmjO!5aJ$sca0jkv4nrrp%CeTSf1<0sdE-^)t zu|ugAbXoxx`1Zo2R^L0)ZM`-iKb~MZ)0A7HS`VQPovk(CBm?$SLqXcWmLI45-F75|4Np?>DvQvLMI?(zI!O} zw&K#rjcqkLFK>b;=#Y)EH{|O;vNhwfdEi9Y;4ZDluG`<$j;MBbRhq-vt?R^{Qw1-jl#dZVP#SU4+p{c-($bGs(9tTtbs=0lSrM+@fNuq`Q zMQ=mx4|b|ejooWS!qOL^q@ADv7*CCZ9CV&MwZPcwxMnMmaaI!OvN$uCC8T2fV;dr? z=g;W7?qQt3+cFvn?vNbuY?l-_&iNm+%_P<653c-ue3r&)=iBg;P+Ye z*#C<`;ZBXLo%!{$eUCxhOIB1&_|N0Q8nki@!!FZ7A)i99GDq2K?Q(e(&p}HXwVr1F z`yBUg=Mz`4PX|tAKM{IMUF+=(}U-Q6d5haqH zy5obc-I)A+A|gsOWY9csnf5V*-qPI=Tk-}<(5cWWohu1J_?o5NdD(fZgwm zUZ-e)z0L~AL~>+rVbG{R2gqBlNxn-X_yCDcwY!q9zVl`nfhRB1A&I7|N$iPrQ`rR< z;ZM+D4SfaJ<_x!vz-#A-{kwpu7roR^}I2O5ErQ zSRWbMKi?DN*Av<0qf`@$)`_42R@MIJ+ttzZ=lL{Z_Z?L4l!B>PTNjqQC(yCyYSqPR zAEV=!T_T@BR5f&c)%)69oR~nP0>5YEWzT&}R zu(FrE0VwP&`yE}}Wj6oz6!x8P^i!Pwm*n}G@rMg?Fc~M$B^iWA?+mudL4ABV;Kh?L z1LRhu&>0k2^%*3!+79lVoUU<8ZU4GZX?>e$ek@v(u@dBO4DQl_q^%R>PJyuH4tC_*WXZfUDl4yo`^1Gg$7?8 z!${D)GES}b*Y0_-9{-0ayij9&okVIj%~@C8y}VD43- zd^0uO4Kg>Sg*Lus z2tRepx<4bG0-#R%QK3RJ=JIxlBTm~znXTdH3xQ%zg)s$tU9t(u&5_aN3H;HR-!a2T zgmEnih$+96D_z{=H`ms7*S9}8w%-pq+W-;@<26htQ@VwD_ep6M<-+E77vBw$eTV_d z(4-`o=uN`63AiVW5yJ5zY#L{6D-vZ#Q5T$6VU&q)Mca$5vep=bkW1^9hGjsOm~`Il z8^9wBA%$#3KOeSV3s@_?TSbft@J#3*1^Yn=Gb%p!amH|vmbODFb-GGk72uSU)Z?SF zI7<3^OSj}Bg=5JWSae|GV#$;O1~O#h#mQdwasNp-UH?fpgz#ASa?khxSfgnA45I5_ zqHbz!C{y`(#LALhrStyGccF6o*Djte;keB%eWt$kYh;!>FZthHJ4S9#svYn>rRQo( z<8+AZNXoo!2e#y_df3_9+=_mcvCf0N>$kgs?H}~GZd$Lrvgf(0A&$%s>4Mw5ioDAN z(&FfuG>*{cU%E=sHiroV-Xjw?+LrKgLSj{HHpL>MY&Iv>GwC5@}?N35VIIm-SuYc^WfC|KT8Y&iUd7-v@W;_^DUZGn|dlTjnryyq_Ra%Vu~ra zOauP7g|{`ZvR8&U+J{T}Q=jtIy4bkeRoRg zMNAyUUvQpzzWCO3Rju4!4H2N4urZeAQ(<8bT*ygV(_liK=X!ZBVzT)@npMc|z0HT))lv^C+2u}MiZp73 zamSLi>1%hTpx!iaFTy2`bn$uM@Mn7chQwbJfoO+#e(9@Of*?jDpx}&xi=rD7ud=bQ z_o43hu=mj6WxJD)K7)T}6#xq2FX5de$W6=T0<|G$aE+-tRiLy7t;r_)Ed78qg@Pap zE9ti!CNYU`H1OikU(wlxeBUF6x!rHa7VgqGWLm@Yv$>(Tl_12i#!~5m^W7As6PyXz ze?FovdhA&a2>jWruqR4-m?suy{g%5oh#{e_TL9zYNfR>T&sOeP)!9^twjRH7DvqCk zD%12~Te2o1@?J`9G){dn@|8bJs=jlme>&Lh_8e%Ba#fAahhLvc@d^^ z)|L1-LVN_*IzxL7h67}&%L0&0sX9fB@B!S*bhT+ZpAG)vTJ}olyzOiuD{-I9Grw?16`qPkO^>L&EWMBn1Ik( zY>hB@LiMkN%6}5XcCTKn&=IN&yZ)sfgl||F-v1py!ALdj!vWTq5D}1e(h>1O3k1nP zF+W&b{lWJqt_ywYSdXx4U>c$9m!_+eI>@2$ucu$tw)NG=b`ZQb^wTzO@D4j8I*L>b zA3CRY@&qQPM0u|v>>zN{;VeWPNFO-b^^q%@*OS6Kg(MLM|JPzMm_pokFv-&4+Hpgq z94_|tG@wZFAx0exn`-V(yeW z+ED5B{1IxX**r6*OM~V>lYhW2W{|I)+3pDA6Zf?8vAGGOuKK~;PCNYhm)Tre0sm?< ztYaa-T}Guo_*Qq^O>n-%Gl0=Mz_dEyo*dtHweVCeBgu7?`v66#@D(Zz+Se71;w+Id z_=9fTG_GXSp%whb7LrBiHF2knI7AiF|0KSXJ|kc?z1sO^ZRE$yN+#E zb3`0%v5L&ZnLKQn$l?+_Y9jlBpL~KYbjoe~Z-i+?^)EWDlh)}Y8RPCMbBExM6J zNc|i_`SInnM@Ldd8g$crnqUhJOz63EW9#W{N7H4WO&SHbRsSmlE#iSf&;V>wy6lm^$AJbuvOL;S$sE?e38;CJBMvXi?3?dZ!c&zb} ztKNLX&^+Bnh-PR-bw(@aj5=<1I$I?Ivld~zV78@~htxmFWJW*POf4u+o@S)z_o3r(W^7#*#=5<31Nco*i zjfoOx^Ypi#N2A**md`OtqX^T5C=uHa-&2($%6Hq!=&mp++#TNVO0wIT^iDKLfkFU| zOH)wtNeWYc9^2nf>c(B1rnbtcS_g2X%> z%Iru5P8oVSwdmi5-I9LLhfo84mB$wQ+m`)BBTnF5C#4JAzSGt<>>%+lZ*ZvJr>kqK z5BUSJ#0-~!>BWzDDKuX zCBt_<+h#tsnKW!itBca-{C#Tb_?soXgXZT>q>Qvhqkrw2lqTd>G$~oj<}^(|b8ccZ z#9pf~5V{B`a`)ixsd4%ZjQaXbsT(mhf~u(d!=Nq2Ri9ik%z`!Hn0xZ>F(+gfUh4c% z>3QqLW{bgK4tb}LjQTNIr!0nNnYN1`H3M@Wp<;lB;1b|nVQm6ddTisYS;5areyp--pWzKpNJ zg85F0fqk4Rn1xc%PGq?k1u1L*pGo0umu~zmVZC|N;<=u9(lV>nmZ2B@;xFKNT!g1| zTB-B=bJct^{p>aEv+!vVGgnO>o!NJ3>G7SrZ_YPWE$6WXimuyM89~8Qe+qPV_p@S@ zZd-fEnkzj_{?NwNt!K65VK!C8HTG!F3A^jJs<*2+IV>*n@z=$Q=W` z6-$vJmcCfy-xinEOigfCG=Yb7gdmO*0RNRjsS=OMlkQZ;o} zOkO@ek-==QP8$Y`M=zvs%dz)4s#j#FQ5QMX9SL<4fnj)NIuI&fc2|zLl!Ng&z|pqu za_WrqYUuO1)R~}EZlx_X^i)01PXu_z(rGOUcac(j@^Y`gjBMvnwj_#T{d#}9h35=w z{M3y~T{3x~)U4B>V`KX-T_~V4ZbJH&@@|ZtI6SCD0*|!Z=PANf(L!-5ey2jRPHW;e zlz|i;APt581d4fwffA5i!0j*0lGg$q7;A9^AY#}Gs)6n*EqSAP{d}`g(m@~$nA7Ot zu)H5C=_ikd1FlHSpyutvPG^F>U||s4sg}XZKUm;<-M(9WyW>aadCV0D$N@itOP(11 zew97)|HOmVdYvT)lvki`0Y2~U0`blz#)eHS5Rbm1%dzw;q|S;6K9Je*pF4lW zlSV@OAQi5ZVIptBfPf=uu#uBR@6Z5lTHd~gJVvfoKIi%DEKlo^{=N`J7Y{pfH_>%a z@J6X_W^qJ(y1X5+vVBvc#HEC|owzU&&96BZ82Z}3?#;^Ugp;tg_Jn73ck9%P9(iAV z4>l_xk&%2ehg|+fs%Z682|d`+;_R>kze*U^mH0+C-oq1v(`UI%rA7E^T_f@>e0+Hp5mj zmow06X!AAQgKJLvijd2{yWrYO_WxT4U-XdyGE*+NX`a;LyNmCf!hkKp`%Z-;MpcZi zDDvWy(RAx+zg#j-Pd)021b%#>N*jnAqAmZ=*iN3MAK1DLcapbOwadr+Gp1*0E&sX8 zWm?i^f+cR2NDP)$5x;P*MGx|rmERRoFr`fb>3jPeuw3J(|F6yXV__LjX3zZ5paV{) zk+Tq}?>Y)vvC&x;CxfI75}~{tr=uj~B=vww?z9hzFx{)_qH(pu8_89DG95=e$fP{n zVBth?ZFu{R(L1XU=;_H9(oT<6%0NbWyPg=@UT0uB3Pm8dyqxVV<>KQ9_9cNQHs`6z8xgLeI*k}j7`b*eWZgEq*pt>uD)5)w6uM&X zsdJj;wWLAE&^sS+IZ+%O99HKXOd1GPMHs-wA0IFXzW;7gseSrRP3OM7>3;H~E9rLy zX789{jZ$}m+>>#W|4WTa)#o6;!+eFRl{AG=f-cO<@;>p;z98_bEIRSQl3xBRmDb9i zO=fj>5%`gmQ5BGC+Lo5KmZkV_-lTTRXr$t5o&1X5V?Rd5{l(A&(Ofu$(;qhCd*OT7 zi(l6;x!7M(`kU>=9mFFy6qFq)7nG2(Y@<(i3fQGX68Oz&V1Vvn2?oLv|Q6s;3)P`zvC2VN>{;Vi(hZTlUB`zB>W&KWc180K2}D=m3UA1iovS+?9H02Tb=6dc6tIP)o&71Glr^m56fV*qS> z0sk4R1+QB)Eb-YNWtQr33s2Ms&2LU=uVJTjpbv8y&t!>HrXnVE+to;zIQNx8FTVsM zWFvYlH-ZbM4FVQWtT*g3Ch+_V3w}WWqg473K@h%Fiha~vM^i*Q*I#0ye#>{0 zK(8D1hGDR?*3D09Jd%3oigqe+RYW5rmkV@X5z#>bLL(=zkJZH;9x4dd+d{}qT-E^| z?Ow}}o$MR@VBE($^Ow^Ay)q|#Vu}`r&60;Sdcd&~{Ro#Lli>J1yw84HJC+210A98; z(F}BZX|$j93HOSH4|r2_u-U-xx$&!u3!2!A$-lSBYK8IG&(GU1=YfyLfG0DEjM8`4 zg?AiP94NzOS3<-IVmE`$nOb-eEpi0V#3{4R$k9{OD`Si3xe`9KApldW3GSr+8?eb9 zn_oZxe54dHb}oJ@$;AJ20ZbfM&>ZQ0LHQV0O->36%sI0N!~~Z%t9omO^v6^Pc(igv zqn|HH>Z&di2I4irO@CbDRU@^`(8&B{*G!qC5li;~FbQP;L@%vk-;oaLrdQAXyyCh9 zSN5(!KeXBqznD(-ZmBi3JX&WhNvd63jv-YI_Nm-(kH~i8x>eldoY zowSqGFq5aM2OM!fN!o6nh4itD)ojAHZ%x&~gsWP+Z(n=0qTY`RjkhT~!Uw15s)Q27UG&Y=l-c{oq9T zcM*uRg6Q8+M*k?>EuZ1NwV48KA4@N6P+@!l7#iX;MvrI07N_rh9+2`@7k>2$IQB!$ zojx_E8tz+CskW74=S4!u(fZ)%TmQzYfJbqnulkw+>0|}8kKQRyYE(%jy7083#q8X_ zCXVhf$##I{n1`5$6_w*9eb`zWAdYe%UGqY%v4ZAo#^tn!DMZDd@O0)wPNr#dI`O>= z-DX>P0Pt*njIZOpX$}K^=I2aH(C8}6V%xh0x=#{COViVGd|)w1EAP}(Glk!884w29 zVL36NdA=Hqlm1a7sp>eWxa$CaEu2!Y4I`zMM7;fO$frWaLBU4SXjH2OP0Wqn$!c-j-c)cIfCX1d+XZnC+&~jvWt{xF|no$=1*&C0G+*03Fgc-|opa`w|@@Cr^I=U;SC}UytaIm0A8hoCwCoNf?tS zuoNvF5FOeU@djdugK7~a7!s9xnvJli60=k>|2{OFxe0o2Pw;eX-Q=Ejw3!s9J#e*( zjEA^eXZC6_&B*fj$V*P2!7qma9_#<^AmeYZ!i4m-%0Ai=In$RjA8wvzM(dwmS&IRU znepq6ZrD`_t)HB=ir=CGrIK|M8h^*@+-ki{v203qz~|yCwRed*nf`R$5@tL-_jgOX z9MHF^tuanUBZc<)+DFuTMa8o5ViT56sKl!mgxMEddZ4T24R?zP+W2qPd2@bW;Q-p~ zXI=5HrYg}?l1FlJ#KvDFO_}B4pHq~cD7?2{)v5q{e;<#@>}wQ8j^Igjyl&a09C?du zfFsPwXzP}SjP!*c5IqXG$s(80VO}eSd_Mi|&lOVI2NV3H5SyX zFHbKqBJ`nGY&-m^JjL%S4O3U4*YyZ+OuJ>I`bESh{Ng)SVR-}Dj7k1g(f3@0j5B~F zPS^9hh@a!qWt5;<#iC7}hmSo?TaJY!M=dM$rbcA-aArXuc180Jgu0l+tYi!Eul)x6 zDkQ6b_R0PyMqpX7nDWH0ToQ-@f~d&5UYLFpY2IlZ@RI3W;HraUk7&8yDluarh(JpO z=1~ft9Z770%W?}Fb=!g5junr_C%MZ%y9@O&+h-csje@z|n z&>(z-Ew-r=t$cc}-@GMBW!po!_Q|~}mg#zwsBQq)j??1K>zN~u3vWvjIi| zr=aWtn*};~NS*Z2{24vwloF)C1v3A6D40nLSCe(Xx0^hB;_hV2Fw_~?vA4@t{Yn>k zyJ0iw>E_f4LNBq5>HH;+SMl-?k5;fArHtH@;PTJQT^1_% zzrTdKgP+avcck*{*wo}rPYdy)5B$fe3NtVx`gx{1j&w?`-T6~{Eai~NH8RRWP=I=v zgnyYj{=UH%`q?j&nIjxVEDEeB%|MiBrq0mP?2ljyl<3OHsIx6z)qcD@cIO%Dsj&Kn zWKear1v6V3RR?>x>K9p|Gvi_^#nlq&f61wJr|PWwX)q&+KKchidq##m z1*6C`;{G<`ib{u*tn|cL_SOGnk^H_;pj`k+_XeupYjDE1o7us})3-`AM|F);D(TIu zH@BV{@?%J?VfyI8gnQ^EH4o_@-3y7XW^M%RNAhE}bVc;G0GWPJi^B;AF{p>&&utRz z{ijS$nC-TzDnoNTm#ilDJ{YACqB$S6d0un*fzAH?xZzp?pmQFDzPv!<4I4W;UdddI z$-;gsttW>f!K!vf`N>gwNT50U{OFz4Cn_@;lNAqVevnWjmJVi3)1P%2Z~}o?jY{tV z$1QFpYs_MrJcPCM1g$z5{*6Vb_7rfk6Diq<)UFI@yI$*Re^C#Gw|2v9PmW8in_hFf zJR1ZE)r%nWlretvFFZ)@e-yOpz_2UAfV&IURa%fD9{`cCt-Ngzz&YyXyGg?Q z?~)+E@8UsnTlu`@xC`*j?UxDcg`1I~nP3R8BzWqC)4bub_p`;fT@)F&#ghFtlo;4% zVnV<=JMl+KS+d*&hwajq3b?AI=!G8m)nPf&0LNth#|;MnLaLyv=2Z) zSW(pZ`=Npy7;G)QQm1rzQ+j08D}UaYCon0|R_RH)#$yOpIV&{i3`94ou4vROvdUk@ zc}-L>ZRwj8Wweu8QLvOGXOHXKbOn|p%WQ@a7A@I5=^h3$3Ws9vrMu#FaEbNG?hjFb zUrh;>wVTY7EKKuh7h(gGR>3h>La?wX%^irc3#b$nahSwahxw(;Nzpm}Yr_bDnj??%UKjX@SclWfmyB#Tm9+-O>g-^NhjtW<@7N6dCz;ipPx_Roc9MY!2ymvhA^WjV4VG=PVBb|A2it~SyA`u;biH8vSPV|-ToMB?B z3V*%3y~9pp?dtQHrJ=sU7o#CPX!>XxMM=%Gt7 zq4X1}{^2%j?ONfYyb0$J751uUO1GE|&A%k~G^vq}a8z;K%)jy=6cs}rb&*baWT*-# zhM#ZAvKRR5uY^|(Z{_+JTeW6m)(F?hWNq=3m{d;dg};0Fv4GcD9-}T^{ANB{UM@Ob zCX+OnXvyT<)W&(oe{pgvN#0+Ya8%7B8U~X=|KI)SJOmPCMRRk3=>j~tqynXiy^k@u zDZqoAr4m8KDM?FZT0&?{4COuA6}UvHp&7k>fB!_Bf`r|WzRtb=qo5lJ-_=k`Hr9I! zs$gl6x?bY5sjZS9-DUyH`eYp3_g4w%xN=od@{4fM>VA)7Xm}T-HzicP=w>3)L$dDa znY^wP-3Ldhw5PT_@DJ|FMot!0z^MswsEtzAT-62^J{(Vu+40YQlXOBah%Eig$8>ri zH0-*JrUyH{QrY+T*scMOu)UQ{7Q@WI3i>#8i(3bTeXcS*b;m5c?L(+cE=jyXgJQix z$wVjK;+H9kgRn8xyo_(mEULu3FSu}3;&6O3LpX=oqf2dBicYgcdN5_{H_+;Y@I-Qp z64rO1jSbEQ&bBNm)w{X<)n(b=-CZ=s$)x6I96C->qr&12g34^_HAF!N^NwartSaW&37}%U29=UAcw;ev|_}x>a zaH1xiur6!5s~qh<_=||RStNrJMxIDC!r}B-(!DUn>I$|V@1P~5SZY=atW|Ux(gT{1d%nDz#tL94`Zi@Qz2YIn|Mu{i@!UIMLv8_ zp+#pvDtT+Pwkwk7)-Je|Wo^!D59d|ar6>ZW>HI1UqF95YQ(grP;!*PAAt>{~25w4C!$&r^&hVI{vC5i$8{8X3D<{ z%aI<>bqw2QfAB2#V4D_qPz}mNa~0%<>RocQGq2TsC&ZYQ!7-*JJCzu8i1zL;eyO2Y zs%O_0DkEfENH-^IIM1=L8MKIIcdYxMT<03jj_&pC4zu6vJgc^_78{^iN9sq$uB+-U z;vsICFtd{Gsn71tOPXKWuErb0MdD0mR#{gvBO$Nr3$|L3v}XsASL?phcH7*U=vkhA zKuSzs&9=PJ{4=qwIJZ=W44PGbBOc{JoJP2*@rmwX`DS!dmWsBzc?;7)oZ?H1YLeBB zNufOvyb-z?zeV`?8HjMj?M9jiHrVx)v>F)6bsyZH>PouUMJYNtaY}L;w8E{@26LRvc5UZzawrMji_Yh3jV`RU>v14mpEr6y=)mJYO{f zb}D=Zn!J?#zsHgo=VPp$xHjy2+>vIXYM$@cwdG}-{xHsNV@@MvnE&CIWQA7#u^AN+ z`aNY?{Wo9wLS5;gDaSbV!VjW*GT*Zb-Ri0}yc2W$6WM#noa)KytcvV}1i8=xkMQlA zI>pszT7iBP?6w@5l2KwsAKWW@B_7>=J+`_-w2ZVJF=MnUDD2h5gh9iHhNoD@Smw+z zToW(<@Hh9gu}T^zDaB=WKZK%!A9T^p%+`|X>J%~5H;DzspY1&B0rm0+zO-xx+eF6@ z@C3L-bOkoqIipwHDI8_R9EuveAjPm(!*bPjS#t;Ts#Ea`)5qd8x5!|KF(r$kWFP6_ z_F)x)uO}XN2gu%fQTvIkIb#WZyUxmON_Zho-#dFrDHYCKwI`cHU&12v$4AVYafG3DZkgf54k;E4(%jF z>vtv!aWF~yjamKt%mPHIFd0cOGS@I3kge%BY5E>>BSu28Pu8B%h(#3o5YQECFmK)2 zoRF|SQNm0akvVowkw}d#3%pu>8xpQ)vy0%c{JOE5mK94!$qKjPkyUOh?RV|%8<}gN0>|u! zUD{9wPUHp!8Qn32_92J+hkU~^OS1MgNzYYGg|HSv1XSeg1#!vCopGKk>6vJjUS2So zZ$pdorNG+;l#bwBpVwRa>*{Pq)9EoxG1cW3+#D+9fSGno7De9x>m_kM!D@7|78d*y(e$=T3 zdcgSbJm0Q&laVNti8(B}=_u-z?=Mo&G4z9Gd9-n=amM6SX_R=|w5v12URLE!nu?0A zcT8UqxP)R1)wZt0vRin;kLGCCL1W{C@Vxtsd zXWGErFWEdg?R_A~Jf%`?s*7O#$`RZ&egbVm=F56Urpt3v1!PTZo|~ei+_?4pvv??f zo%cofgNV|7uidiry`6|)$Vw&S7mOiwUNL1v5!QE#%OeV!{OC+kd)*Y;5h)Ay;X1;g z0*AZR!F&G8d4@;jtU3LXb3Is3(P$YHsuT<6`6n?OLD+;UNE8yR4rXP>%rTU*)tz!W z2N^rd1w`ulGnJ~?4ENd)P_Q6(9IO35kgH`BJ%e2hfsZdKJkr?40PZ4#UiFVOOVlUXtgTd@l?j2oyNmO znN5Amw;21SKHLd!kfuFj?U~YyuiAQ6mL?J($%JE>!@5ZaYiQEmTsgnjSkk&Oc-=K( z?DUCJr0+UQG`PTQe2E{!7W5;A3BE3s&iuJl-hbS84I(1r>?iWFe@LFT_&6yE=w;6&Vb&He|G(uP~XiESk9-KBS^fWh*MSodh)=f=DAg`v!QA zjB}JBIC&k~9vxTv1@SEb^wp#TQBwYH`P(d!2rFAB9KoW?Ln(esQ>CryUBr?Gd-36~ z(ZgD5Q|hCyVQnqb#tR71?Sb&*AD|`8Bg5P;#dnLY!k4awA}8$RG~23ucIm+eQX9$& zbMBR~0(^XAo>+({G3rWW2x3+Wb8C|O(fy6ZJ=I1k7-n+s48h#x?o$SwSuopAcNC<6_DEdfyK2T5|3mA`Ksur7<#Wki zOfv~hZLN~Obw+x+73`ceNb^u0Q=htqoE@SIH6Upycb0aMT#&a3WR|J{E+Y`gugY z1X|?T+&*#H58y4F_^nce!E7&(QbDQW>HaV{;i{uhpAFG#Q(Iw---N|q&eEonP1nsX ze9#BG)cN=LDeUG z{_(MdR=|z4=HS4aQRUi<$SF>vs!%c(`UMUkVFyeOGr>m}eH(C++Os&BFr^s$%97N- za4to3{NWI{q^naQAOGbjnU&ZS3`cNbWC7Y*-tRr4& zZg(-Ss9h25@Gz;a<7jWbyusFMm=CmdkTrxdg?LBdrZ^{T>F;?XQMWM*s$mD(tTggg zk^43{r4A$p%ZR9d!I`ZrZNW2k0C_~2sKrB;Nykk4+T}~{@rHMIwE~v6w|3qZe z^K3WGzdHom@#`aRfO#_DEPgZ5|8sYb*j`-;uUL$RmI6gf_i>JL2C~Rm45%NYsg!BX zM%i5GVl5w^hA(N9=CG(B@LVdUDc@|0TLselvOwDuO=*ivI1&=YlcW?)Ix))LgU@s6 zv_Z6b&b3aRZtqU0`Y&_nGE7FR9 zlgqq|`g>E^(-%^Vr;k~`NW79k_VO1w!tqN=mJPM9$pVLtug|supX!7Vz;l#fK*~Lh z8UX#cLk0zdUH-2IPqvtzRVB3UCcy4b&5(hiu+Z@Kbi<~-#k-Jb3@EPN*tlxe2Bn?NlX+H6%+%kh z$u)O`tz5d+n7Nn}d3l6xP%M*wxnoGW^)oNrEOysm;%3}c@{S!emh;oIls3%#jIPV3 z8gXl4{I&T7WDUyPlQHI)l=k3FeCv_w|Ca<_!B}ge7Gw93bQ$&Q7=y~`&B)m~f->*N zZhn~au!!uL1j|XLu*GBYk^u~92DChV5_V}QjkW4?Cu#lVu~lsSHKCt*@^_;G@=|hg zgsyVha-`EX%Iy!btqM-|R_dePPEl7JziGu%<`E(J;u|804Fqn#+xzOR07zIon*~|- zSU%Taz@2ygxyrB?WeQw$URbK$zx#Km=`5m37Y+4FocNz1i|CASR_g?ZrvjQThcK%w@v6iD3{oC3~56wNz(&vBaKD)$a%4DSs zQ7&mv%m3iA_*+a|Glm6M?%bPb{pvZqVjPtZN_w$NA|e$M8JE_tjwo`$BadxU+3NiK z!>Y|;3^U=K6hf$Dv=fHedej2Mz|>}|Sg5@DrJae*z+edo6a#b7w17*N|Bg`(s6L50 zs){s~1(DV|iNj-b!~Hk1C|_)Y3;qdpKW>~UAoREv;8vbnv2d2UHuQYyi& zyCvb&mgPDJkf0*t!5`Z|=kW)hit^Z&s5X}ARJtu$sLAP-V^3k3YsaA#w*iPN<5>&3_3LFkpTCe&LU&sjLb2eF4*Nt{P&236L-2!BNkkRrr}{PcA|c# zxUQ0~wtZrTe9`6Cd3I_85r;0a%u{l>E`PCw2xv~xLWMzq_h=aIrv-dOD1+OY1Fp05 z%<}7qRv|A3NDABIp|IAl#JNZPY_ z${{NSnDV=>X~r~B8zZp`shjGsuS^^KGEEcicCB&v zS=(y*g|C{Rj_vB$6lIZ(5n%)6k=Z5h`Evc=?Q1}8;LTg<<3Fmup~bxibxt2wl9C%w zPHT>WCOz32h~#Gwu<{5^>98Aftv0VcJ2S5N;I@5x3qNrpEoxiK_*SXnTKka^NT1e@M#q8FAaLl)b1sSl&&7;=GJ zT07y@uHX28_e`JnOu^k5e(d`u>AAn1=Uf&R#aaB(tNp<;4eK;^89WV=#-F&ARjvkj zFA=+Wxw~7h3}3}Fxss^pP*Pw*UBRv-;qZ6Mc_Oy#wZaBN4p@bk zih`~UejEE!t(tlofh*%Yk;TGKtYE z7I4}WplO4d7=X&#-|y>_m=WV?8qF;#9`PL&8wkWCWk|qQ_H%Y;@eY+Enc2e{qOr*l zFAuA8q1%9s@EtQE-+Rt_mX9F*`U~EdJn}pa|sFwX&b;5C2os1_hr`=Wo4g8l|~a2+YH}) z(C;T2Y`zdR#RATfZk8@~ACAAj6o}O?G!DM3Ht;$-@h{rL(_(p7#n59|3%0*;I=}z< zQ5^I0+D{`|qf0+mttbr(uB1d=%k=`kCV6kYAtrc$wvCFpe!oKWyQ?nLV39J|A{8(4 z#MaATDFYj##6nd-EC{*)Muf3Apk(@X_ZH!5xzDM01lN2IngPP<4(GskJR|8wO^J*j z(>1pFvt4l^?)Y6eY=<+$>}p@S&;afUshTHxq%o&^%h=azdPU@wJ_g5J)No{ufIoI3vFjQ&9@z^ zV>IY9d3pW89Szt9Iw4N(f(7^wh{rE^pv}3LX{BPes4qvPme0Z!ZOqS-7?_GHn(^X4 z7OMZYP6@C;^q?tYpgI#rCdZ8w%$71RMs{)Z`V0^1)Kjo3=@8ezD0zP6Oa_n?2=-+- zC33^udeBfsXh%rRFMsd=yybb&X^<{_>s#EISp>Gqm|)HE{H9XDD}@9$4U$`o=lMqu z-69sO+g1$GR7FYQw{w z;QS##-N&G|rIzE8Fxgrbz{G9gn;X?tSRwrCnEQN5-tOBK;VRQv{Sr+D>ucx_*q_vf zXin*CDqh5NIj2Rqk!Ngv8#W0zHih9!XQt)9a@jvE?%-5VdDxT|~wx#)^ua4e_DsP@}rGzO$or)L#Px~fI*{%rTadc>`avvF^`=u^vz<;OKM>!=1+z-tTtE>p#PnS62k-Ptus1O)TbynBXPIzJf`so-;YieIpk@ z^k#Z2@lT|Qpf^Uho7@_boL8O+Iv26|%@dinm2EniYAA>G^SObpzP^w~(sv$68t%B& zdEmb8I`~hF^6-82?xV4UXU_6px(anWwMH38=63i!FTkWg2P2Q9owz z!~-V$S}O7{W>kK+pgG)_5lPd@ZMSZ#Aruv%?9F0MD8zFYSWSQIu*F3JgkN!et782@ z;&HJEEB0KF3!CHE=8*yzx0e&giiw3)%9se2G&m)xJ!XTqaYno%*AG&57?Sn6ijNWR z&pU@j50Hbv#asI%XkzzNlB;t|*|{B-zvaJ!hAf}uQ`;5b9g_HG zpB`D8B_97BO&)!w&mMOT{$)Yk*}6koG#BsgJ2DrfciTiP6 zb@CSmj zVuLpzPet%}r}JQS78E;!D%zN;evBSVnFWjrp}Fw!j5x6PwDc_2*v@#VqMA!~40Vd* zkZNYk3mtiL&R$VNXh;`0wIv!iG-zxprDrlHl?$^bA21nLUoDHs_t)|(tzDNyu|JZv zoBN&Mdxmcl&wpThTZe3uuR+m4DiKr*LiMQlxek!9K2)6D|L?mRwtstq&i1-_6F}UH z4&nP;I^s8)&|Cped+xWoTD#+8PFvTawP_PeAALHcI~G~^%VV<3djTx%D22YOuO=T= zGeMyw9d1)HK?-A0_h5RB7^(}ma3N1yE5$*sgv`vOE*yb`(Ly{Q9$az2^wR(PfV!qO zhAy&^dy_9A;-K#UkLhK?kTsz!E5pS9YUE@}Hh$v#`?#FF#J!(X4KWdpi;PAbT^uvO z3{zhT-br+*d$*yz-Sekdwe=dXs?Rv$v=W1()cYO-JR}FN-n0kSbKIa~XkqvMi5}nd zB4x35Qf*#WS=5Y}xi8E^0>QjGy^+4DPAlSF@AzBXA58d?hL)ZNKqi+oWBEd=nN4u( zgSEb&o;{M7z$KgqeL#*=;j!=bV{fNUPQ7L49XlAg`Y7PrMfYOyeRgtueaY;qSCt)= zvEAD=&;K{cm1vKix_VuBVGNs( zWzS?ThRQFqF_f&5iLQtkJ;8-GE3OpQekQ(_>E%7rfk>1iNqI^u>p>I;z6nfX`vdmz zv=a-fgZez?Lq|jEVR3i!oY*#@na4S~1}gQ8tNu}cAbt8mIqV%|CMx331OQ{4M~<98 zw$!y;$l@bt`F+`jOUyZ8YCnV6axH=CS0Tj=PMe3$AM=uZ(p#+*=j2^7?0Wyn&B}`= z!XhJAQ&q^l{E;W-adHTUdyt2C4smaK0pruIUj1OYzy&<(!5=(W1AhC|2TCX#fJDXx zpwpedh2Pj!;siS@Wu-~e-Z1McFDr35b4JpLJE3p9VSRURJtx-!ZmTl?{mG zZ&A_`^pyIIaCdDocC2!wOMGhamYCnFOd=*#Oh;1)+rPzo()7&udL@43JeIJN<~>wn z!}8bpxy~q>@I)kAyziEI47XHxtS{0pnrm6*hE8=7f}|pR8e|)Bj8CzNAZFtduc&3Y z^-A^v8wj2Xqm zG~$PB7fI)08|F4lg367v%vFCfbVcQD=kw4AgQft7N`m!DjvvTJAdiY_B{-0N1Zz z#3^fH3N9oS{S|v2zgoK5^CXJy&)uJGiP);Ro6E(b?~BLU(e z@eiJ>YL1JfhG){JHlbUX#7Mt4#M1oFsmyQ6NJaTLvVu>{mhvd|w2dhoibFpPCr_!ViBkM#pMWs#sZ$TTv9hi%t`}LX z4}Ptbti?CFoVZk^+pCF*Llb|t!s^UJeIYu`7w11V^dOJ+t|eWn!voAsn~|lfSR+w- zCj@kiFN6Cn162ia%8-&`_!Q%#!jS}BH{=0oTM+hjpMh5t`Rp6MbRLgCv46@nEIvYg}JgE||C;^StU3fN@ z@<`oy2Oco_iaQn91WiMt6#zM{)%uu~RW-)Ess5BD=QO2OP{MEV?gj z5lLez{HAEWQS}-md1c5O&}DH#$Q$Ona`gN4w?0i1Vzo}^=)yIS+$p&Xb1lio3lcq& z2)&PTmN=O8j}HP1{qCYShusL(g=u%vm%|J~HfX4#1M> zU;e1cQE#}l6SnK`;oB3*JO~k<__j{{TaTSVC0PJH6mUE7&lJ60(*<^88eLU&c7}Mz zO!RK4c}a53HCZsY-g|$(0`zffNAnoGe0$vzmzcIzbGOl37JK zA37iyvWnlJr3o=h9r4(Q`U#I!a_7tw!@o}SuWG#63tQy0ezW*(v$6s7)e2}qI7Amw zY!{{8+J+0&W_M@aETTd+#|ifW_0bF%(qzgqlRbmqv4gUZj)W2RhK5EW9C6miU>6vH zG3$(IM?`F)dUKwW&UU|UXee;DnHnqPqkjciljagMHV+Gw4i$UY*5?FpQtVeABx37&W{fLIRTb0QzGlxf z`#3~j4B#l39bUYuMOaA`SApA=SS{TT@kJv1fy7HneOz`-Z(|5FGSg@ml8y-co}@1) z;^jrB{?y0{@oS}Q#}B9&7DWFk1(0f=sWagC*$aWZm;XWo{0u)|88Z!K$YY}33n2_E zHByG-3g6t)Ugqv?sl~g{Y4m4kox@&FHPm0bu+xhu`-=NZDJB}hQ4xbP3oeyZxNJ}N zC}yV^BPXK(ON3QQna0AES*81NUG$v&GQO>$61+%Ej9vFO3J=aU|ApyZSyyMc_oB%) z1$vpUZx(QmN}Nixm*F_|6cgZ4YKpiPEAs=mPBU%;FZ0Omor-O9`d*z{fq)f;22QF6 z!$H+S+?g1?q0kgLj_AJC!E_krBg>t+?YOeCW-oJ`zx`5FtckNU$^{i^8V_qy6NS6_ z)huQ__TtfP$UQ25-%fd)_g*%NfQoy<4VNJlTb`t+18p)7SIHcvq+a^sbY+it^i_1B zTsLrC23eHc0>VF+`*^l;t7XhJyz#^55d3j!7v^VoVdW%s+}QTv<`Lhaj5Ucg!k@Ok zd=?e=3Z!A*knQKgyh+0dYB15Au&VE)I$KRt*NR>x5V7-21nk3_+(1aW5*MR8pE%4%FGxdHuI6(cP2+`6EW#@82;z*R#!&{Z zDvUHcCt+Ld5V<1Z?LdoZ!JuZ}`Hk**+P?#3#TgxF$juSbcX>&7L13-uEuW-EB3Jdx zZwDhpX-mO)M-=1u-8g1B3K(!4!eupcq7rj{y^dLD-s7|vp9qSw0iI<2Y!+aPY!ZgD zQpoHua#ka4rqt>%rc~=cu!LNb_!&v$(C{y5lH`3^M7M`;@KKyN?epeQ8gAL5>)~}W z8h*LgJ2acaeN4O7~1)tAu!j#<9hy$ISmg^C|%VG~CaGyg+1DVi%n&e)10;XeMs&Cn1}W%Q z05E?yTdVNZ2|fdXmLu;9)irD9V2b;vj*2Txt(IU?!-_h+nN!Eb7kVrJiH@QL4;cJ< zn7_smP>xj6b2d4hPM0A7f_rm)Xm=R{Z1Pip<8H z0raMmzUN56=UP@4#&3V9);O_5Yy{B`2@X6V;uR2cG5FaiJy4~P=5tu9ijcm z5>8sctQhxmA_EXVNjFxd`m)AvG_Bno9a>2B5F7F-&Zc+`#wekTo9;1(8R`9dn4S5k zPGBNAI6J!kuCs6c{O*6MWAsp*B7VC5S*+9UGotQ)<+Ge8ePR}-0}A!rEV>Jij}Jez zr_IlpKUx*N=!E_2h0bm(k@R+@{ST9ut=Drf5UOZX4La<5x$;QaUMPuYI|1%mj~44p z&ddxr9=p>FRXWq4qv3e7_(>1ShcJQX&>){}Lu2P}vdY{cc3Gd&0>cpU_D=0``U;d7 z17L6DSbCAoXspqzQy!cjoOSuT;;I38QnkU)1R~Po=*i=ixY2lb_RGS|-SZXCm6a7> zPug#S)4O-RI-EJ*#+cVg$s>Me%(sIt-B){j7Y0oPS?k|_I6}&n zDea|41tlkjC<|LI|DL}pBuF@*l27a=WQWgi0O2G9-R)urAEWr@H~;4PC>>yb!Ij8n z6)U)Zpp;#SIE892N}gfD2ihU+X5E^7=%Hdsju!)3>|q+Q7MYRb!b)oHVKi<)mA5`d(_|<|Z z_(h++&>Cbb;k^w=cJ^NRrg&}}Ul8h~8@7HsLA=`&y#A3s5her9q#m2J^Q6|pPhc&Z zZrr%-q@sF7ErX68U3LLg#e61qbQ;nMj*}-}efoH;$F-)Y_^AbWqV zcNmgg@IuJ8yLJ(a&_wU!;;W!fD`b{KQotq5pE_GD`VyfGg zjU;UL@r?k0b-S&T`hQ)J_imY(!i2G_zfDP;{8!k_KgBXGwpNS1WYxusX00s&60+>$ zo+`oOFkw_)KQ#nMcx0)HkL!QPX(65F+{u2ysW;Ujzk zT+{nyDnO^w%sphqqNzCO(-!|LWCMvaxxBljqTXfYLM)ASq)w?=IuH{}vbTz%0?#I8 zRc**7vowd(ksZiM_WHQ>Z9Qsjt5htR%w_gN8rN2gBcgq5oxs8GwT=)e=vg<5zUX1A zLl3NBx}8;;-3W5F@F}uCRD|SSi7oIIIS$B6;`)A()S`U6KSv$9V|%w29!(7I-BC4Vl>D0V8d6~}L>>p@?n z069>A9~J@aKb>_Jl%WN&AEg7e(T&zkYWL0@&{qEyq_&*|>w1IW!bPQnC$GnX%A(`4cFL1ztGarUGFEz@*G)~w})xoy6O@?gfIeJ zkfkZC^jvJ)Z)^*lq-xS;g9RIMsTa(cASUccQv1@?YiMaC7opxNq}%wB^%vY^A4|_K z^j_Zn7dLDkLRj+Q@eWGWT~(vosDNQ&tw7ZFMf#bLN2*HuA+R=EvhVFJ4tYJTPAm@M z;fp1aYVn2h|7ih0TP82erv883xcqX4wl%>4E^MbHw2`2rnT3a}0jN!PKUN@@m(_AS zqyfE`4SY3PDy<#EwC0ti_s>_`ZjI#quB~hJR}4#tsc&;J{3i{!p4uP#vgs-jf3f++C!T3E1OvlS=ie~@rxbo)%XzC%{6LGK11UogdPvSS7(gRa@qL^ zC{W@Z>#0akJCSwCMDKd9fbYc`x`-J=CIKMmf=B@y!~m|<{_)thRJUDj?BwSmBJCNq$1)^N%0hX=dgyblej+2F-TMk96?t-wM z%=w(aisG0;b|;x^9&zpV)*FjHhQyV`_LvnVV$tTQWeM@5^y3Mq6r}&+r~gUztxSN+ zdB}(Dp=+&LNCK{BOxi*~F({Ts0Uhe=*b30L_sruyuGdi? z%4D87YjZH9wM=k9^7HH|`W%{b28r3}9O_N2jh1nZY>vtigNt*(L92k{b3IId^{M~^ zkpO_7Z=UR%T;z}VS_}nmd*y}bw2_=5n?M=A?%hD>vrh-!wZXK{Gq_83ZQw2;KtZ^! zV6B;-ecHOhq=Bo=<>9w=(+>VFk+YDc97Oh~4Ov5)1i$fcR=D`&P~X@)I7<&u5NsRYPV0%apX)#ckD9TzW#BR#)ejbeuG0m*4Ged z4QYw3YZo#jS+GBT{(F-j*9}EP4QS7i5D~0JtG$_0t-iiY)7*aI=Z;utmHhk;j1c*9YAJCj9yye2_;fRq-qqj`8G;MwqNQ0wUp`tDAB%3 z%^8(vb}Y2qy;Iib>`DWaoz9Nl)QW@)rgXxZ8Rm}9Trj52OMD9ccnY=MngIj*_RXkq z?--FFYNJnOFat!=D|G@oX>scC5q#5MmmIsW?{&nWim!mtZ5=+&zkG;yhD)Hp#q{&g z7LsRbEdyE7u0rg*>7SM}2+{^_!zU$4PdIKpIZtB|;Ha8mB2TUH1WWnki|QR}2crUm zzXmpd-z87J)Kt%XT(hlxx_f?NQ1IbXy8lT3RbiP5OCBW>_?(on)O@M|D2{GrvsJ%7 zvv6l4tp|6v<_;z|3(Yx36lFFzJjPuXBB%zu$4bAWIRhBbOUfyzkjBu)CL3t6yZH*1 zXbOj5kX?>|^x~g@Z8!T-ekQT{KZvX&LJ*~in9WrJFPSRc2^BZyukG&=tCjwbg`zFU z_5TWBM_Fe`m?^Qcac1BD={Q*8G7|>aar4Yq3Ftr-?k{T8Piu0CC}K&PCl|k9TZD61 zeY{Ta!Y-tv5%o^M2R@_Bp@>P$cr4%~U`k%8`P1n58K+tL>U7o>rsPDpgTr;dRx1zxmsouM5~Xu6_dUD;&8 z^-b!|QD|frPyS;WzCR99j3IDmZ_C@$y#@+YK$D*tGx53#Yp!WbChe-<##ga3n~34Z zs^F@X#y+-h-;BGZIDgI^vz9(oDA8@Ke~ht*J9I~4c+|qh{efCi#>pCwl>q=B`ZEaw zUQ>gtMze9&)U(N*%Ff!`lsue94O*8$ZVMYUiz-8cItqpT#OcGhMl_VRzwd357qqPB z#u;Xq@|Q{K*xhzIn9r*lJS(&wm*_wLt^X%YqljFNit8c;`jrp*FTo?jth24r%8Wdv zLt*n8O{Fl5L#%{@?#@V+*;t$1)bpX=dw-&I*OvEEt+JVB0APeyiD`gaE&co%AEPR0 zoo%7eMhPA*ik)J%WM+fRT!|p?NtE2E$Edl5iHD71%}&5XcXZ0nruLbIGi|HdCelc8qJhQi^1cb zTxobj|L=^O01GKF;a;{rf+2O}Fek-B0Y9b2vfTX;j*sQ=wqoE>6A{+fIFYgll_chO zMhM_GcIfMsJvv5pY|J}$UR?DPsMDUf32{KU#KHDTgc)wTqqwSwO$rTb9y`3Uqh&>y z&M_F4q=;9aJ*32ur+ang#G9HBSF6^%|C5~F;Fc{ z1ef!Rqxb%=`V$)}hxT%Ur&h~-TzR+WXShL^i?_UVs~CVFCbHO{ z(KI(HkyO2bl~r8AF2{MnXFTtw6YWkx;plwmw59(v^-1%C)G_1tkZcZ@c5DO~2)9Wd zo~88h6c{KaCWy#f8aN&=NpZPdGNYpI4fM6P(`yR{W*<;g&s1k{xI{MoYO$3K!`#c` z-S+B~m`>e(!Hmgi&uib~09kcaXgXsC;;lsD7>XQ;oPTQ{7Rv`Y^-dram?ND-%(Gd{ z?{PU!jqBsEb^JSAn1X%1U+49F*d%V(+>RD*)uK}p3q=yFIjO(S{&vt{97L)1j!_X_ zT5JNf^p%q0TnS3xU;=H`)*985u|QOGep61U29TuGk=_s&Qe$~p{=KxlqS{Tegf_}% znX!thMV*Dq03PFf(d$(xI&pnf)cP(tdWCJqBHbacDJe}!LPpic-nbr7et(hk6bY~t zR9k!}HAK^b@+1PJ{+voCicImDeD-az~ z_?zr=_HvES0z>KO&yD;akso~eJGZSInjfo$B` zazq$7ne?S^8Naa=L@s@qPxbXr@@2S7wUJ8TKXstfCjc=X{%BpGN+$-nM6XL=W)U#H zod0>@K6Co~C7wuaTZz&Tm!jpu^!rAj*m4o){K@aiLEDF2~dje8> z)!Z>|ML_b8zCJDWkWp2BS!ER&lF6jf={53NcWYE4imkMGQ@L%W0j=8aHH+>NQnvHp ze!PR!tX4|V_}431sUjHQZTD`Er?syvtI?x3LssK#G1g^yOXVv`E(z9OA`4E`2>zku zy!Npy_yPIV$A(AM1w_~mX=9o=b|9E!C#HYLpH{f&_^wcLc@#UEqL`nd^CnqCrBA;M zz%j@?sI|bdp1Q-T$o0`*CWi{ZvpV^Trb$RIY-F=3hqxjESXd^X^9#D~ec$&fRd1Ut zMTA9USi?y<6NTdBg>@JqLXsu|(C00im{~`KKd5{?_F|81$D~xNoJ83Z=Q0GKSvcN4 zS3Ku1N9y0&u3WFYH7i?HOfHnQH+UB|=9sN4J=iA2gyf6$6zatz8h`i2!Viah!D@y` zV<7xXK>9a;e>hf84U7g0jC72FMq*wj4q`%^c^;0F)YKzwv$%b+lI(Ff@}g>q;Bt+a zsh+F7{h^}->CgD1PKO%RUYF-3=jZLzaK1!mr`v-Y)jm2pIwC>y<>GFEfwlET0rHw> zRgdp-T~pV!V7Qm!L?`(#vjopP#tTH&;sYJz2dHtyh5Gv3qgwqNT8NK6Y)`>*3nN>H|@*Apb1xuy+M^=!Utuw$|-QO+(}G z`H0{6%!W2P2vONOCV$x{z`9|ye$@KW?JZ~**7zs@`sNk+Vg`}=%bC3&e0jZFydH)% zfh`A963yE(SbKLZH>cf^M77*Pu1?`Yc*x)O{kY4bb6;2A0Nk1n#ERleW)|uUE~a;z zIr-5^{xkuboLf8W!O_e+2g{qHJ<*QEPG@WkoWbOd%2*#8$%orEP34uM2j6ZgvqzbX z>F@`{rt3rA_=|omIg&YD`ZMXpwbSW%O<357&tl)vp>9hM5kNeXZglrwJoTe+C zHI@d9TT|Z4+S`R@SR(StvIC_M@vLe8?b&7-qdhl`gr#@q-Uy;itb%p) zaBBg(;Dogzrk?GnukX4iIF>iE<9RcYTUc`*Wx@{ZltM+b0uqZ;5(XKPLgB<$u+l8=OUZ~nsJrJ{wsZY8$bG*snD}3D-V$0-Js6brv za9dbRFBd#Y;Tjtk`4I@Wo0T|WMAk!*URB>i)?bdi=JN%=Dd#H1rB)V|Jz{lm>g`?G zCdS;$xK)03IwyZd81Hg3axDNN3l#DC9Ky!Z)12Fg16cZH;K?Jt2Q#n;cy~SCt$gQ4 zpwsq9c4;^CgwVi+20CEQ6E4@>pUR)b#`T&)0!)zP1ck-vFl6CfD8}$M&zQpM>CUAV zJXQOP&C-pE3?r=Iny|) zZ@0Sv+Yosa;;W+g-k^}Sb`kyGM^D<~bfT{5`S6NhT18#h6y8>ehcLW2~uDm1qI%MBM8C{S{lxG5aSi~UkxN&@z1VH+4Q1k zYI2HFT;QINcq`~tjibkaTYat#;mEL!B2_a~nQ5RbRKB`1ocN8x?Y}R*EIeR0E>?aN zDI1C-INrNt0$$MKOPA5~bKg->R;f~@`m9(hEXx~v1>2=S=iQ^d_;r}ALb9=60f@74 zMH5u3k;_l~7W)q+hXQ@aTQO$ddBVv?z$diFJ~-@dXstji?8VJi(dw^0k#&ZjGqu#a zs}I!fs0jn6q&0kYS2koSS^h@|)1#?b_-uUPgT8+&Q(e6;FWTHAjT|V0J8n!-Pe_UT zu`9n-T8gu>pCRk@+3IwgjaH%v&oaErD`KV9`AjtYv&7WUnJp|3*FN=QYBn&|SWV*tKQCb;Y7{M zJLApTi@9jEysrK45c+axSSrOnYFG~kIup;9g5Iwl)IYf!7S`=ug&AdO#R1(|t}lkbu&`0_Ue+_GCw2S}U{D!W{RvmF#wOMDXU1 zhwD%h@PhTx;@@q`7*0gNbUct*f^$-#n61wYtq(@lu>pU zNOG#azTQF;Ox=r=B|U1k2_nSl^dAx@NCW}#+yYujG)@?6=KlCE;|haNHj7Ldrs2iJ zH>|oydkk$Mr?V+}-%0a)uD)oV>t#=5J%%e#E!P+&wWExIF`>O|+u*^%@WK%fgM!;2 zQNE#d^wiH=Hm7{#Q7?3vp&srXWb5DS*z1Cl6I!os85~N?|hFXW88dsF!|b4OWJxr z_gUR(pf4&)Ac;0o% zI|?i2V#91*oANdH>pKH6TKsT|Fizqj%0-(nqN%M(EXKlu>9dRrYzW*7%Eff9sd>TB zh4In$p^3>Czcc<-YON&e6iJ*ZrD#(xej^`0Yl&S4112~7ZoU7V$^4`mnDFpM2};R( zw#~?uCvKe3?7!S6d7|kwlf(Iu|CRcWYnj&WM)K}$l0rhfl73Blb7{KtR00Y(O(8Cy z5-qi#Yhav*28GPhpN}u=&l#@c;HMaF@L5v;U^M|8#t7k{w=X9+tU99Uw-l3W|7(AM zkU7S3sm33|bpR4tL9D(%(G-e|d>%9FAUqu1h)c*!z=?JDVbofNO%~Y6jb~A94Q+I7 zzthe)nIC8+5dbUTyfMbi<9F(jqP&4Q?LKN}Pm_9OD6gpLX@!1}Y*dBp|JT;5|7=f` ztT~idM6n#CFmh_T)bdM7G>KG;&A5!pR9_`2x-|qYK@TOlFMK^Y`E2)D@pd`ixgeUA zz1arIt*1`f3nZY7PJjWN#u2}Q^zw68*uiwIZ<68yS0K-YnI?Y?;M2NNmC7J6hq7^E zH+Utm#rU3h1jmQB-H1CYU@^aTKbr2f4yDF}G1s!pK;4{OmP2VJDhT6L9CkIh{;pf{ zBla!-Qp*XhwKH4kexj9Coei#a;sxtSHXF2P1PlrPdrR!qSnLB>Q&KFq;w5*H`SI%# z2&9c?#qUW93$f$wP18>d#L<6{c^1BH(rvbaa89MIub?ZHimGSGLC&LXCtSmd zut?U{W#9G6kuNh|O7SZO1I?csdkD?9<0%{MKpr4dB$#LXX~rlo1Xfo^U^d6UyBjXe@`c{q5IxeFqro}^OtH#9T;?0 zAF@H_q2;TDvqtnQcxHQ^(@!eeknJi9c!0P_ac7> zVp%9C`brfH1%P>IE%9rSL&gAB|KTIkJXK)T40^=OZk@6{qEObkV3V6^U^xv;O8mec z;&=0JhIzuQ$A_SL)(JQ#pGw11a}q^YHaANcQZ#usmd$pE|!@r(qtt@18nLWpY~5 zzo6$}R=Gq(-NWzdSfPhcA9|9M^`_kj6El*fBzMbFm)(1XgHxs{KJGsl(;~#HF}lA& zh-Y)@idRR3S{B@)uLb{5mT0uJ@QuiTf}oNkcdI7^HKouNVwn=ES~by~eaSSj3YoH8osR+wlz^aDx z&t&_*iTlmb8rFG}ENPQaHKFS|trNYYV2(4mmzR6C>dmzNG?y4&2P8*i*D6wW54KOPl3{l1>P^YaLb-)s?6jV&Uhg6@Ll<#q4s-@jE)INg3$NW&T*Iee0CEy<9c% z37OIRx!NL&rWy}ihv%Mly?uy=QVYepgmM7WdxxAYtTJ#fZ|t5m`FHjGkq0Dm-@lAi zGjwjmeJ6X*X%D0(tW|yg{KijiB}o|Z8vX-{$cG|iYJaUban3+Wu)xb zIG$QgTm)Iy{&k+3NcL*fG*qFkrYmSJm%s+QFWx4>*yuP?zw?;QR?Tak?3t^7`9Ced z5iL%Hh92j$MlcCgK-O`6SMzyM#?sj-#}>vJl_^;_bR<_5qrTEwfwfGl%D{UmhlE@b zS-UJjrN*bxudw1L%gKPiIhWCqm8#F@MSrwuE|rh-@Dm(Yia~R3(!HR47X_JV>>TTD z7p*(Bgedq{X;-P5$OsHN6o{Ta&(Ij;e3Slv`$(=MWE-;Po%8vh9y#s41tu3239GA+ zBLK&>At|k$^R1~|&YuW$qGO?CZzad`PD)kkPF5|(~NNx<}8gv)@N3t zqpF@rWRiulh>Nq&dGd=hUeB8lr$;N)zen0TMy@$ri)?lheEzX}zKQ5~+-#2&X{N{> zpa?C%Wg8%0@c~2w8D@@(vSc+&hcNnG5i&sKD|w}~kdcnvxnazINL{UJWF@HA{nHUt z!O=ySHPG3&&uB4NMYtbOzo_zD4j1~PT)`ojf_xA>OU0t{u8^p-%zUhLC}bSHN3js| zX#J;9%zdF}AzOE>L^2*OwoG8NNBt-z?k=~1rX!tQn8Ue<$M%KGdJvm3K2&M9>!BunNKEXA}Cb8WP3^o)?T zRRiaG1l?taB$t%*hBbg0;@aM`UG_uCGQ52ttt118OwPNnChT=OIY@4Cwk3jYhgLDL zLWUZ5iD)xS+>;VxO4Gqo|C-Rw@)kWKBLj-)pNwa*s0kzVBnCv0>8jT}npWPpcYO|0 zC!TbO6kdo{oIv0(gnBY^7eW1X?>DF1&QCY#UlFzN?djI`{B@=1>ZYAq4D{y%2UB)t z7gl>(1qzn8N;@>8qeCgq9SrJAFY+&+KaOG-t`;Ivxm*@`U`B=O1nbbEPuw*lael2R z=SAli8R}2bDa1tt`_YU@Y1VFdI3SA^jayR1r@@l7q&%n@-yi4r4}|st^DPZN zZRX&CNb4@9?U2>mhNoJ>XD_tIRFl{P%B;PF9XTP62>^4|9I94QX{99lphg zZ$PJj0d|v4v}X|JUax#BuaamIZQ+co(oDGZ#El%KcD&IXLOz@krY6ctgML4rKn5k- z+Lt?rPaa<%r)a8OpF>RS`V>Hw;_6_{K1-FfAuOKv+>;v7Iu$u7ARB1SWoW$Ap9^ig znjpFk6xlc1w@AB>hIkoCg=*q~Q6305FO6vv2-rwZa#PG-j0paT&&{XCafmAgHPH9G0&J@Uk2p+^UINQwR6W z)I92i;{}9dhEbs1IC$p6kG|G=JUMg|QcdNJ3wwlQs#=^2l#jD%Ejxr(q|-36lhaM^ zJ>)JdVlM$MiLk4FDAC%EmbYBp=HP+4KAo1KrivvgsC8pR(@Dng@ad2a50mvDz-UJe zZM*pbZcJWy86f#j;_nmp9-{9owi6rHH`J{t^$blzbK@{h-cWWor)1;*P%pw62Tdzl z8|nF?i1DAr2gG(+)|Ky0NY~>U+zikxtN=T*^U+lC@bpCm0+W%Hy*g>|g0(6{(7$YALH)H~gfI zSqdXhBrx94svtY{?VgQU|G-?GrF)gIPeW9Nvq(&B6$ zy&YuJE>iV2RU^(~x-uckKc+*GG=HxBC=D?;%7J%K#%-~C34l+kul;{CePvi&T@USB zN{c%T1&X`7TXA=HDDLjo;_mM5Zi7>xXmNLUr^w*k)9>Eq17IpkhsZFOF|-NtQoZKg0v@C|NL z9K}2Z_w%(Yl;yU!*l}sB>kQp!ygUu>*1S#zl9A&`s7CWof-6O}Xi8Z12z01cxE?)k z1Xg(ds3fwRI0|6@8L^#4!Oq4oWc1hbytKu~MQ|3f3$KU2AQjHPSjp*#~g!uM-HTT`a`X?7#RM# z^_gCOi%)E8$}QkAa|Elpl+LdRuV>qmL9vR0ARmQIZ^DPzF0?$A_ ziyv!Fo+%aQjwhcI4ncb)AH4c1bWsh7r&uU+6=Q^g=e0{$n$TvrwdBT5sf0_0sfK8w zMpX~##wfyUU+?Q}mO&8wmcyFhf!CiZvTGN?82)e2%Uq=Rd-r?w3|PAOax~x(KVcVg z8bCfzJ2$GuGFq|>?S$kx*3K*+k3S1|9nyHMWA~?i1M=#y>$1w z^&jwAm<5%zcUiqDOg7%cx_{~B4O-IGJP-H@oVrBqrZK%P>9VB51aAq$q!A1Kay&)r zU}ggut+e&_@9#$IB;i_AQ5>WhsTqh0NXM_0)M5~Qs^3MLsfoK+uW^~{nOWI_G7^HC z2^~#MHXk|Y2yhSf`7)O&uHB;iM{9>|lnivhGz!ZpL!5(W?<;T9u;wx2{)U+KZG^dn zk2Z}q{VEF`C5a}G|3-pGa?SZ9+NwRe*oA3P6HPb^m7SPf-^q^`aQRT<%k(w)$#GD- znR-q;IXJWBtp2$d2GpuEfL9jwhJ(NF+_kQ-uv0O*q)*D)A&A%g&Z z)7WW(jYvm#LD@%UVrw%@-#0gwBBa#Jlt!XkLDUQk!A{a4zSK)SWM~8O1D^RN25?w6 zCLlakz3+8--HNKu&oVTJ>MCtRTQ>ya^e6KVjDAepD&`Ye%|KF5fE#hC{GyMTd+?*u zQn-5j+aKvRq{V)%(yZTfI*#~a9*GFY@;vc2`ek`VWC5))i2#^17-5Db!loxhznbh8+}s-f$2y}k{K8t$hi*mWlt zz9IkW=4t3Ci|MJicn+Rfn|Fg%3%1=&9M%P6Ru2$$F78}}aC-vX*jN$m z0^K;HRZ(J>D!Fq7_w9I5@c|-+2Z0t^=Ml=UYAh=!%OPTfa{0t zQwgH3gCGp`qivXeIP04W!PRW87B2t8P#z*8E>6<;Hm=%XYY%4E2I2H6nre0cv0)Al z(8<~u@m@Ad`@K(w(lI(IB#z?~%3&-EGEIikqinErRmgyh%Z=+YTm=^Tii2YD&9u^O z6_W=XLwD)8jzkNU#Tu-YV>RA(;DPkfgVf~a(!Swp7NMvRQ&?7UbWYm9i;{d=?fc)q zGc4xvxtly>*cq*nHT7gNO6V)$I{egauqnb{U5v-d2I#d@=IXC<>Fb^q z+)+|!=BA$d=|A@*FQSo92p?xTBabvgyD9?_O;bmIR~CYpeft)tyNW)%qeSEYg;Di+ zJ^)!QZ|-N%-b>1v2EyI=VJ4Rz4B1(|d)%H?+B1gPbSng*ME94T3kuah$NTrxM?rQ4 zAnJ_w%#bb1jqNChC^cav96 zqSjVNO1oPkl8GJ?PE>wtxPC4Q&pQ9;GdaPW^BI75AcXh04<^F&@2P+M={*>TI?*$J zGA*zf*rIPnPnVkkdd8*adxAg%oGrcEs|hbQ!C6r@=!pVj?GN*Hoi-!qN_jyA-W`#v zj>pjW33oxFWT-!TF8U!`XMI`+t2bORH)RIE1)~yrm+czjEh1vqvsC0*uhjAq-(6)V zRJ^e;n;1+RCoNyrD!hQM6q)N>LMz%$znODJX7k!#jUuNKQ#%A;+$7y-sfW~Y=V{@! zXVL+OPz4auCoMw3tLp*E+&ca47n0LccRKE_+MFXTm|Cb28*6A5Y zCIuVtZPT1L&)Y? z%Kh$RZkGAWEAy`47Y$;{^pb1okND|Zfi_9;XA~y=QO?eXX{VZa)xp67yV0xtt2Vl; z|G`x=f`M6QgFJL{VXA0k!7Q$}!t)e0!CpF*9rPmRhBFzun&G7f3%KJe26~nDOrvM~ zRt`D_mdp=Nc|P3-hna}}?wSM`otRtMS6_UYNKQ?&boeAVkNnOi%QPRs3mlOWKRNBY zrP_0M1P*83Tso9_28vG7k!5M~zsFPKklntyqFkEBUL3Z=5-&Y>@%M2euOxK{t`uL( z7SgZ|Pe%TjPL78Q$m~_Z_OtqVuy4lx*`lx^LRR*(GC5ZKSV0wHG}Nqs>^N;!OQ6l7 zG*N_8NGH1?$5@5eiew_t&ml@q&ns*2-Q~(e{1DA{OvTRL(X8X?vjMkj1F6dHz!$+B zc^EgF)lNtG*@Z07=I~!YZRSd3W0vW4D0X-cMx&!!;JSZ<4#!tU4Gi`uKYLaAjVu-9 z$PE&N>`!>i_C`k6ONC@$_!Yq=idNJ#7ZWuVtFEj$uaG_I#kpau6B}-viBK_O(PAng zPXjs-$Q7(O55M5Z80qx0cM--TmEV|pjA^eWq$rgLFJa5)TPK$D>xpZMMv9gV{myYL zNWhfw1Qd%K87;f>J;SDUXtD77jx(R8dZQl6yq3!!xiqTP&$(u4}wbp{ssI>zyIn0P1G@qIe>DQf=DLG}tXzo2>_#+jdcnqzs&uTr|cOK^)z zmReDV1i}S6jim_%z(qTaX{y*TVjBIVlCqh0%~#DpSak}hCzep**b8`AxU{zVD~j$? zp_Q%E?A&S`qosbZzmLIpGgfSDYufGeDCSM?BkVYdW?REMr0X6tTUi=DGhBv0iYZ%2%N+o7%;BaJswMo&5GO%EI>o*SRF-=+;#K!fg)Gz*uby$mLaM>X79v!&X z^392r_(as>hTtL~(nhlHGI;Il`rg@$R{KP#PD-x-n@S6LzyPU)3j1wQ_%u~JVS+xvP_4k(#6Ip?A0SA+8IDSk; z5Js=Z{A3Jch`IYRRtMdSi=7@rUfEzO-*&62W2Al)uE_VB_7x|gsxZ!YPhlrCi)5Wd zl%`kexz+NmYUSqa4RwT*h7T$rSK!k6EYm`Nh!On}{vsbnI+zr{W7OQS%E9}R1AK2k zt->!%Nrp*G_2mq4>HK`=6w=!07aFoj1Z$HSJcQ+aOkll?dy>@0y+8G8gz*@wtGwa5L4LI- zkT;wiPqG2jT02?p!(MkeBxygg6DjZw+R@$T$&V>-TfCtCeq4SPNbdsZ7@q@s>(e>` zw?GxH`@^uz#QRMakCFWf#1FKUPFxOk`z}L&FB*47Cb!GLPdA5M5MJyEp0fIR^Enb+ zYE4qLHXpJHe)XX@^wM(gnGwrYhGzT(Tu~bPpP>frqn6*tGYWJX@fStPA{;--B~R%m zNnlp{H{gadNIFd4Q)<||s#Zd>^OxkyTDV8Xcd8l~3b|LgWww^BQwO%oDm$vx5tv2~ zW(ibXPv)qBud4F65%V!d)`6Zb2Jhme1MvW+tVVb|y+~F!Z1H>6Eb<#`cYPC$5aXHS z;S)ZTV4Wz0xgW5~gB^?`5hAHeY^H0 z(w($P!5HDSk3p(l=*#Z8&))oj%aBZ%D;@&{CY20&8I; zm%7_*-^9&jEh2x#gE#$7js0eTViUzOp@DUy9MHzO;GqkyMuk>>76CHdSi7r|b z^fMmYus=&DJKUE-Xo@qFrS`Hu9gF(dhh_?JVvkZzta+qt{;=(d64v2imk;yIs=e!` zjUkj3^X1y+cGbSygA_PVOx)RUfz{#XZinSfER1w?P<-r{hpoNnx(JatOBVJ_0{ow` z*E|=?e37JDaSBo}tN+j!Q92&ro2VoV;8-%6y1S=}8_ur{!D;MPw#qP&pq8c3=)7+! zwj9OTZJ?KxwE({EB30j}j^z8B*;(l}(dfCTp;7z|L3pfjMNIc)z ziq~d7>Asu#n?)u<&F+Di&edk3wp_mPOsLg(VsFE_*p z@i_3SvTpFjor`|MZ~oq@1pzcpH+u=d>c)(Uh# zcX1UU{y?j!==s;{o-gAY;*S^9Xh|jJfwJ!d!z*z7~n7NR=bnX%i}{OImUq<do$S3Xjp13jRzpT%+pLkmIKQouwJh+D?l>ZMCl@ zDr9ItaGLu-kRPAJiDS_3skoHTATDy2x@JzNZlbmkqhDAZuF> zZsHZT!(TQ@tSB7cdFk@KBj%hgx9da>AwTc>4z7@qeS~@2SU=RAId39qV_~IX7}#!$ zK)^5so)f7(irtAy_8fkUk9Q&!;Db`>)R37cGMnqx$zgW2^5c4*nfHAXd6#pDysg^! zb|tLKAX~@n+aq}#_qb^(t1p4Ci?DrtxY>?ic?T*vKPN4~}coV@U4d>5Y zbZzr-Ud_a4@PgAF$G@YYVBQ3Ov0 zo^}KN&CoFL$av7!?1vFMVW-)-zo0IGE^-t$(Mz$TMqK-O(qnR;Q}>>cN7lpi(O)ElIdj3!iV?{bxPE4a5_++db)xP)=9-5T=fvqgzd;{D z-TS%6Ufo>*I^bSH_$!4+a&AQX%tQQlJNh!KkP6)pVf+R{!{_%6vrJpi`x!|!wvL%e zm|P{{`SDCcm5S|P35gUB>)jm}5RB22gAL-{smZi0SmM)96XqnTr==6QaehCx2@~<` z_y+|KP>R<7c~r{3Glbr0JgcZt%Y071~}OeC-pn)Yjp zb{`~d$IsV}coY;+xLKwc@$1{*gnaS0_Q~@d5PdbZqM#~t5ZzkZDwhZzj{U+eFx(m9ETvQn4is zKKF0X>%#Gc_GXE*Asem{bt7B$69?i0FZ#T4SqK#2jy1DZ0&pAU86Gagz~%lu-z*>4 z4gQ62bbcr4t_;+?SuKual9O{26)7YyM8CZ^3SX`8=G8HXRWLp%vqw4tSjYptvH!iG zbKL}gTQ;hnAej#%nGYvfTXNi52K1p_UeqD&sQ`MoN8uX`h=8& z$F1mxBbbXhT4d)AK_jE>URlFqnoWF~C9{`bw`ak1t?wS_UKav4W8?s@G^n4Z6*PqV zsm=EN8y>(Iq^DKYiN99?D1#H&JR!d=zH~Ah?!K?3>1Nm7Abl9u$h4@&d7Xg8w+y0e z^@H#na@#-e3&2Yb&mh+BtAV<*ex!)D5LJss#&}(>l3=Av=@?%7*Rn=WiMwLOENj__ zsI!CHWqr%bUlq$cANO=3t)8!}=+k!fJKaJWGONf34}4aL+mf05oTFYkEr0IOXL8W& zf(N7AhsxA~wbgaI9I=jN1YChUma$~OIIw1O|E29lx5qWxB`$-_n+n@URb?9}Ri9ic zNkY;t4ObMT?$NV88ABBA(L*)~(S0W+Q{VmwjQ^2KSPzQBk-i9}s)z8DJWLR(P2c$q z_M`X|(T>biRg}^{ZJcC$B&%GvdPceJBqt1X5s#MH3RixUqU*;&jjb#E3K4zJLg~M*?J%Ut7acpV5dc>qznTaYa7@w zKP(t3sQJq!Iz;56#Bu!0C zLnc9{NX2hF(sDgIOty6;gL_T64G}1r~mzajS2&cb9VC zxBDT4PEp^c9(o5w@$#X@YdIVlTY(g~sn(Kjk!e?v*-UbboeaZNJRbw~|8oHpR)IDj zv;N-d=zkRjhqmYHOX-=r6gPM`5+0-=Q0(VMz(mjTXaEy+`Gu|<2`qDk_w>IN08Z0= zet`FI+M`B@NAh|SkeSsG3wa{oKtGiFyr>A=_)H{zRiw0=C@vtIi6kwVrKVsN&T<9aRcrFLE&X$hA^+kW?Z z@2@#A&8cDaISSKqf{1F$enL~pJTnFW!e;gTHep|uUwUlI4&Plhu-UElh}x`~fA{@j zCfm};*01M4E1aVcHap&=>?1sGXP+8kzpAIjC!i?I+#?v95TRtON%cbqsuJ@dZRJ7hFsydS^oyDg zy?C~A$px6G`ms&MDO2vglm25FJf4&GbC+8!>n@qrimvbg2mA8Q$gjeaHt&nIPcja< zP@0hq1N9<+g?8LCynP~Ejt6X1RfHHX0oZds9C92Q@exlD+U?0S5X!_pCmO}Y^umIJ^Hc4 zFI83(DLY2wf9=_jv5D6sT}y+DH_6luMu=Ibk^tKe_%Tu5oJgdRacKD=Vg%NP^{6U? z)6&XqfE=sa=`j6Pt<9%yVGo|>m-8=GcsC(DmfX?T^;NjJ)V!4sf&^$JO!lSl{VXhN zAPngbD2efUTVY;(x5%9EpJLwq@a2w!u)k^8c6gvqRbCt6dnK!0yA)e{69x8e-_8Ze z##E-C8$u){6w2)Anhl5NdGn>u&W$FYw&thBou%z@@P;TQ?s7bvfd#s)XjDz~=vzxg zxwNbO!$F*m|0^h4aKY#q8`yQTxe&flP9fd9|DXHFfF&0&OpO9oxQfqevLlEZ3<^m&&8_R1tV z(AU(k%cEF@+R=R^y=S{54S`C9P43vurh_(e2&Vh~_yM19(ReS`Ap;q(PLQdj0gK-{ z9ddCd^|G1S-12v{Bc_>0X8K{sihELC`SH0_(v(LDGa4DW>b!mxS)){QkDmce$3R+Q zZ{I!Cx!k|5`t6p`C$;4_umTDlaD%HK-FcneyP-+P-LbNyGe51PjYLCV;-Fk+nLiaC zyuxRf55jl)rIc?S7%m3qBv3zZP1Eb})}veh9@hTH1Svf@dTrbY zxWj-V0p{@f0zW;I<=SQh^0 z`^~#T68J6bM#|FWP2MF5@=Lbq(HqCq#8JKZQ+e|f5i8JGWi;3QVIFyS@P^eGve{e}i{ElHqR!|6$yE<}g~`xt2&050Zx^h?)yC@|G-NO*n_%hVKLS zjkM^@a=i0l*4WJ6C?#*k3jH|!?Y>=#ZQ)nJ9sdx+#Qt}nIe00GQY$Ilkz!;lE%Ddd zI=42bwuBEFNa17O6kVUaot-*lAVA)if9@xCj{ml1(LT2NL2|O%e+!G38?&P?|Fjz` zE8{~Il(hpxfocx)F-TtC{9b_@WR@eVgTQjD@fVE)nLlRJq*xy+3a=QS&)x?GwsnYs zP0i+WKoU3gelAcw33kw)$d_io@%TfaZPAbe$GePoxm}#2=0u39tUP!)1Q>R~=`?h& z-qT2N1G}Pwz%N1Dsy~bGGpB3WnA%(4x;8~CSX9GD9w#wAFb#+n zmaL^1F*B1-SLav_%4!9=FOa?%id!e*P$gzt|8xIDyXB2C*nxzY$JNOvoIY%|B_y-H zraHr9-N+$AX&N7&m$_0ba2ny*>SmaGD8RwjEAR`APKdZh^wx^8&6&I$aeh29? z0E><{_6kd9jZrhe+R>)25!_Ej@Dt!A1$D8tqi<(1yEa|RN5#l3DNJL`3!d)sAJ)HT zK9jd)KVNhAxjO~KC$1e-%Bkkbj+^O!R4ar2H1eV71gQBJ=-Nu*%N@J!sP8m zmPG6=CMl`b&?&27sm@E1`U6s~yPcKsnDhh|vllVKO?-x_?q;nTkqr~t`9_=9&aOq- zUe!iVA1YOIMsct-bHbPXE9+ekPJ^gprmPzBOH}U)bEi3NOZnS!ET^DodkILdw4|pt z#&uc1@}`Eo4WvMkr1-Ehq~N2i6gl7#Uvn{oiqxLX&|CgvINBivU&Ldmy`sK${!k%k zrhJ4Y#EH4Q4Dw1v<`MA_v-uUH8>``8hYKz^4r*Q%W+Gh-=k1sI#j|r$O_L&NX3|U3 z_w##P&WkleXWzv2F1?Ul;(UyZzZEsY&`|4(2bV0YZG_|I89|)Fjr76ysjG;)#OF$Q z5(K%9FZ${S0|Vsh8VWmH$kx`Jgx9pqt%LwZv$K>(2*EkMHeq2%KH+!?B);&(&bGjv z-~eAU;3OcRcH_^yUe^$}J^n6IbfT1GpqNLekYb<@6AVKdk)t2+ zRXn1HgxN`yn5ii)WLp```s7=xuC4r5gKf;4_AAz-Fj81NoBwD%%A-z=Oam6nj48jz z=b1+}hP6w1|2iK~QleD5U)^iTTjVI+k|@_bfh(Q4U*!xh*@)>r!Vb-+;InWLt(qG- z;qNAjV>o-hvPB(9Nl1Pi?W=7RY1$@Bc{*jomJV!UU3HXNfwR)ehiO-Kh-Bg+mSsE_ zK2A;)Ox-NB{L34ywW+6&_K8b^C^Nx>pMy*sZjCpJV?y|TAD;g59h@H zX8X@NN}rZk(H7{Sz2OjECP^fa6-VH8;XD2{m!El3j_l=x`6lnwGsWVIi>2nEC=f_3 zW!Eo6Pt^wz7X6d31`|vbvqd7RQF!vM|ILM*WS$yOBw*P7jL0~fapSX9m>?9?tJP>a zD$|Lh9JaNiq_f%^RTpurX5d^-YBRaSCSzJgblwb6 zDQX2_-1Rzq>u>aRqR)}1%D-YIWW2XH<&gOvUThO?$ZY6mN+7{ah5*MBcT`mJcL9Bg za&9B*$SR+*CE4xB@=qg6G1N@r1LLDh_nOh$Eua3*v@clZS;zJT#b>WZ6p60fcV+tY z&f|UbImj-o@hC3^bD}`Rf%==vr(<>L_QahWxA2V!3%`)b|m?bl;#gsA$l_{9s+d1TI5S;4P0XyT=BXu^dNE#w9#)8-67OknWa+W*X)pTgE z8D36#3@Ci$Q#d`Woo*w^;W?j65$#=zqSf7YkXy#>pw9JJi%>B;w}vrF@DG#qxm{@u z0-{S{T%AnRK4^perS*;Gkx|ePyx+;L7=i7)SvYyhJWKEQU%KYVoDNbT^aj5WL^4Hc zu&JVI{!T=vY>EDAW`~5yEb1x-V@}?4Is@%C{LSU~uECw94I5oPzZsCG{VTlZ)yqnR z>8Vt8wi(*XxP!ra`S@}z+3{=gve7M-$K5OBYShQt(hVtfFSCCh%zZ!i@Dpiqa^&B( zB~7#j7M^`UR(m}U)k@c?1}W2s7jBdK4S*m4Fe zhSCs{>SpO*9BoBaa0ffDoIJ0B*LNzwE9JCb@w?{?quY9}>5UU{L=8VUrXnM)Zw5YX zUQJ^s`!0;r*hHj(vw;laCvs0#*cJ~bpU|8_^lf+l^Yl9GWs`ediu_mIq*h9J8Lw({ z(=PTndFNyfb|YoeGS#BWo@np6b$R=bl8{-c0PmzlZln*|Rf!1jS@Q}EYU=3hE@bP3tW8j>YHZV`VP&dIjcZNH@x9!`I?Csdu(i-bXt`ImzP zVInFZ%Dv7|r&`c|zg>@C<}5zLV$TTV{9N^3?V1&dydhP~BmcvFw7+97nT*qmm&AwC ze>8x;LEOBQSmpcO~dmD@oedeY0A{=8n zmw7TfJP!*$fPeD9=1(k=Wxu9-%QdTL{SOzw$2Z?6dt%?K6kKTgc#qZG_DJx#nZYcg zG~@mg^Q++?WR=#^an1g>+6zLTv8cH{5qbR$b8nmiVEndrm|XRH3Fb`Py7t4ogTEwc zow_MPB#&yzRd6h$FQ`#Wo=suu+(kxJ&PdBn{(7yF1!HN;hQJsJ7eyrP z0jzHEY1<64KUa2Pvu*%=-DA=NI7i0wDq!wi>FYknIb#bjujTVdGOMH=nr=eqgQP)& zt8e5lI6BW(1&e3f`9Y4vDuf(~gLlu`@%bh#&U z$7%8K5j-~a?L zKpL~25aR?D5~QQ8;Hv&fAvulW3sjT2_Mf8HB%8E+fPkcoxbY>8?BK7Wh|_$;&Y1ys zr1!@D#b&$A=?#5Wjd2bNO@dBi1gMr8fi+7kv>HzbdNg6}`Gdi8k-qev1mJBMwQO*L zu#}Q^31m$I>lJSdPO@?9Hqs;WTe*L=UdqL^8JjJ-m#TzIcC=;kw6z;z?t<7K6>0yn z9gl;9t%>{Xkt^Lagukw5=nh4N7FlX9?4}^FcvccMWbGZm*DPdgzr`g|D)`+V0>e7$ z>e7C_y$S5P=q}g{b)_8zh8>xSc7hUXO3A=huX|?YEwh4)No)71fBzdpTzJ3WIWOku zzU8j@H@E<3Kl_sBa+IA+_y2Ptm_^rP_ooMJ9{>S2?)iFBVb-9J!Y$_1?=GA`mjC?7 z00-bj3$3C5Wt+ldXX`$#PKj=)SRuId}`3S=92PfXhQ-a`k`NdsjQi$ic$8}d3BO?8n zR%SymK)<3K0FI;3#RS$)C4Jx)EBNBL0{nNb@8S0=-CWG^TRlTX{_swe$CtvS0<30& zrDxAIrMO9$@)~)_+9xjmH(^WDmXnLu(0?}{vQ?GEjET2(rcGS@y|O6LXihHBgRRNf z#}2#7Mul|BI8X!H5sylOV%hebR%@)b z|8rSXc>XUcKAQB}m*+yS~C}^jka<6=be#%B?6+TG{S$mAuPumaqt7 zZ;g9m9I{?}r-Qn*OgxFYy*a&Et!v!q?I5Vj>=T-&c{uF7yMI=qTSOf{384}sdP6Rj z|CVM1PFNJ!j0MlN zAA<>m)!qKR*eDFjQCOiM+W7buC&!*pA?=O1xNR#$rE;AQwD;jiMLJygVi(nZ3Xt}uOw-Z5Wph9mmx4OR*$d5mH zA9a4l4ru()b$KU_w&rLCw^mKTf|Vqo-Dl8*0e+1ZC^1hv9m5|><=*)!`tSYu-61Gw zt$pEumJqYzQ{N5owKgI(mnY0u8`^v>v0~%new9G+=TqBEJ6(sP%~PG99SM~3J4TWD zmVKNe3xYipFN|Xt{9!g^?nF)_qIL%tylLh0F)cNrCcaLmBupqKb zzPrrM<(L@;mdZZG7XH{;zd6oJrwEJV63x#2(P>@e()cA>kU1^M0oho|J+{?CueTvC z`!0n^$L-z|cF9R6mUm{?ChceoveRTMl|pn-7w)Dg5>nZW9gXE^01<4gdF4mk# z&20+MbTC)&L>&66qW+}K^0*a|do)YGb&>uOys$)l_yd7Pt;mt*M2A<66k+N?IWX8j zo2VTxzC}5TL)EXW@5vn8c130Jq8~U@S)4zeJY$tOj9*e_HYhMl0^U)R@9cW_LnZr= zb2*LFkkfE%0xhUU{b+vkZQ*_>3U0-O&WqRL6B}y#veUVh6XU^^)y4q|VG&8}-Cnjh zj@YI6WJwtkcFg(YeV@n82lqBQosFL4W@KC&)|76+9LK2|aCx#7`T!QRkOy=EnUSk4 z7WiOC)dqU8((9<8q-TS&E7k5eswMAIgq$;q=SnMLN_|KH8In5IYVr(U- zUifx-x~J~svI(#?Qx7eD>g(@dXe{29@cajWQI$IJr)5UB4skU$oa=N>$(52kaQk^a z|Gnc<#u6c4LD4!nGalGvSGG*+nlI-+cQN!X|B@HIZMfb2N&5HOT8UwM2%D^wce(FH zDaQaGt2M5;-VoB*Ada&&!NPj?YT?~Jj^$+sb%*cTuw9FPLX0+yoM_C@Mrn3Za6}O` zLI4pTyi`Knf^L7hbJZ^-8_NU+t0!yUosR1i0R0oUhgqe*%DtoZ%L|i2vZN(PHkP%a zx|h$Ho_oB(Ms9w)SBlT+;3WkdY;bsU+&G6lXg#hedk6FD6-#`k*e%DrI8E3_%+-YV zoL|FW|E90xb)XnioVP*D{izCj;;iKa5F?aE>L4dX3B2?_K3{af(*{kWX2g|Yg%7V& z2KarGN|wxki;rRAe|87Jt_s3(;jCgxTD%c;I{wnRdquKzRF7)u3DIKR8t z7JMJ#OH8cB)sdH8g4=hw949SX8=H`tp;_vJY z7@d^gV7ypNeLGF3h(6&^_>szw80~U$SiCAB@Yg!ZFpz1{{8c^qyR|gkWpUbGv3|1r z8g_=uBE7!!M%xc$y5r7m2>`L1 z*LeN6_op_T|;E^bMyLI zngD;D_`J9!T2V?TVVUzuq>O0hBTVH@r-^1GF!G64C7xiHP2kQogWQ4 zgIZ1&`ql;T5q&bB7W{p)`$8ua!#@6hE&#j;afh0HU$|sXz0%r+dwc&lK>0qmZ<%I~ zR&lB_QcJn7Y$|x|YN_tPjfKC&_LftbSQvsBcg_;5EcRP8N+E5p5(oPT0Y4OMA8B1n zi@1iXBMq|MKr_kWVJS`N9WP=g9#UT_Tzlt6d<9Mhe{f**R-?umh zP_U2n&H?vHo*=x`-y8h>@JB#?WMLm!#pq{I{L(GChOx4UZfrUD1lp45)e(Y2=}HwM z=7hkl{R;XW=TEKh++iD_(mVAZV)i-Hc^n^wgKM7DPSC@}o~+NUyy)R7zUnw`MBK`- z#jbl52Bc&o^ka@{#@{4o4;NYzO}LQL8GY$@{aoREA_|p%cmB7=WwKE*F0sslDjAM` ziG3_7+I&c<4uY;BnH?C97E@JFi~N`ZXgEvec{fKouM&p^etb}uh%NmW-#u}?FCu{7 zUT>?~6F3?#An`>H9HniV$(&7QeOi3_tu(e{ptra2?i72HTdO@=PDnQaYGeIebW|M% zSOt3L&_oNeAZc)D&l4(tspBZ}>$5MA7j=rohJp!|DvvLN&gxV5$9H1%~; z;6OsCK@xe(Ht3J>)jieRKzdec^Ss^iUM1&aOe5yGqG*MU!}Xt+eqs}P1S2RP#gdb} zx)d{mvR~?v49IJcd4UPM$kTd2fUlG33?(RjL$D3z~fCe}>xP^g} znTtHMDOH(T3iF%@cILlL$nil(&?W-&O3P;_nHo6>v&LckcGAG;ohMxLX<{GRuQqkz zZ|uC5%ti-ISh0oX?s!-^j-<&P;?;o(^1?4fCF<4T|6;iZ^oAz{f9=yM$#8Xh3yfXf z^r!Kso(y!%+2-ciKP~xx!{LEF1=(%6xp;C@n<0Hqvy2YdAdr@(CYR$KICiuur(>wM zOd4>Hvk!WUm^WhWfAnU|q*vr@$rh*kGjs!S+IFDU1&D&Dp>Zm$LR=GpS8KRR#b)B}AFQ=k zeCZ|8pKl~_uLLo%9{Dti?8+cfecKlK6p;K&{6p!9zpK*C)rH7Gg>yoM&kxLJF(+s4 zl?u5|wB0=$IX2v9&S{;qv|>61QO8mx<5gsXjBt=JuhWn8(cSXV^27wYqN4({wicfs zgfy@LSGNgda7qlqhwKws>>0{!Rc~&DpMZI)q^)1${Ova~K9x+p*=2t33BwRr7gtgC z@9_V+pVL9&@M#;3x(b^K8>!0c(MXN=s(wc-8M{>nkjw`o;fWky-)rWuXmcML=&O^*o zYRkBm;Hn&xdA;yiWw+M62|6R#e?)>>%!Rk$K^P4KS9UYbV0_ml$L=TX9T=ux>9nmw z%?EgZUftCMRO#z17x_>LC0-^;m9rO>4PrW6^3N;n;WdNI!aI8%ms-~m{=~xD@RI{Sr zo5Fw_@lou{r>k60yC~^6#^bt30qH=UF!%-rA4q{j;(7CKuV*CxlM^&!=4a)-^IB3K z)clDT;_beJbuZ4%aP$3944CZ;AO-PmDnNxkBQNQFytk7|~O=eX91z*HfoPlf|E>rYqvv4O*IlhKLZqXlt~5a1|jl^(Tx$geD{;X`jAc#JBcyorRh;iJ2R!)?#mF++8NP zMZ%(PdI6JVR~egI@Bw7&>z=(rYj?Mznp&){4n+#-G&a{B&UFM<`d)Ioxl#Qzu0-k4 z`r}!`A{gH_)PDAIfI#VjN;K=m#~vHSP|h?qv4c%!>cvos7$#EIvPy8SR3B!xw@LBwi!JVGZ@=8n&p%)_Yi*W+S zGHQjbMDDDst?LRsS67tfTJF!4GJ=@;+|A3TETGhm_-xJ8fNQRdcyP#A@%x&V>vOoc z*2r0Ks>3q$6Ef4YMrJbyVk?F5)#>SDI@(qk=6kfvyPBsJJ~!^{&rC2g>s;>MUS^E@ z?PC~Mf*)Y8qhJm{`ze+=O%v%#OB-=(k>nSi3eKEbQjl&6*;uD1mqo?+*Lqo-vzr*4 zbQ7R30DY+^5<{7DJvLsyYj-FH>bQIkahA04&8QoG){jbC4(Si)lzgTrbZhlix3SR( z7|U=NaJph|D;PI6)zyu1;~j5vbwYYp^rc-G|7VzVGcJ#Lo9_3ZB?wJ;dV3&oRd76F zB#Xn}MtS}4fpIL)r*vC&5jWbWRR0tU3Pv3$7A_1?NT+~SL( z?L~rnYPH~X32Sqm1?8YtjV+oYuM+7Q_zh~33cFO5CTo?*h1$~^BvP7U7+;t#X;wx)!O z+CHK#(P$4nbqEM-&8bE>I?jn_a_fS6xO61zea$H)SeOHZu!g%tmJU(mLr>S%MYC~D zlWJUPn|-MBiN|e3vTw#R`muzk1*l!$-Z#0~CtTY^A1oW|u1`yQ_3PR= z3v8(AnsYn-sD4t3e;Sa>T%lV|KK}KE+=c)W#ri6|tWF*_2Yy#3&g9MVT1Q%sOZn_{ zPR8nt>O-ugJD+6cfki{y07pvL z-Bm!%7wNh*u&{72R)T;(w%2yQ%@Is6XG}uz1@CD&w=q10e@wVONUaAC9(!5}n|yUi zIGbQkz_(yf*g&&r3#T(qZ51SReu6C8mTXekTj{pj!TG&R-4IiUazx7NuF3x< zkiL;u6!@vufZabeGl{Ljh5RMpn^bO}@2LO}`&#t<%$ z*xsY`&i?5u;%L04lU~ekg%mlmjGzLH^yA-0B&H5tnUz4X6iR@fInBe=UzXU_8p;xf z)tq4UTdcFlxNH05bmU|>a?nul)o+MWk$)2nmySkzqt?kKL*_eR$S{#V{b`%)*OAxf zV(Rg~LgLx-y|r8_Aosr-k3Foo9|SC8M{~b4qG+nT7}>K7_6H7usaDk9GJm9LPJFE> zSfH`3>2I7^K!;i(#Q<0}T7Y9Uq*U!fdi8lk>C&%hbqjvJKbJd;{XM>V&*0=4-iWo# z+k^`avy(BJ!xMH@t<}f7!GzKT6yJmPiI4c%7X;cD>f;*S5 z%g}U;tldUjcRDESc20TzmP5tPUWWbw-?L;R-x|o_fy2G`)bg0IgL31W5{mu*q6`MfF} ztYCei>_%h*Ul)(h5S2kx!DUQ_(fH)na!MJFAQOgyMP;Z>JT}iC|s017Q7&Vhio@p1$|PU)J>dO()^vye;<(stwsk;GOd|yZ+{uyRy!>2)ah8g{RA%xnB>?(Is%U8Z(zgId#f~lbku#%J<0P)I7Gxn*?zyzz$@ly3 z#{|tX=Zk`fM^#Z@`J$0BbvC5uZ@%QAK%~wt1Z#!Cem#P9)^48wkOy-tHmOB{<#L7( zYwA~k*izD4ccMe=(B33_PUUu2ny$Ow%vkWXm-N3T8L;5;=UgyNjJv7=+tjnEI|6%ay0Lu zD%*nmp>PDb>9vk`5zaikP%@OH@Fgy4M-O zoB8-GWk$w0c4J-w;Fw;n)6H1iRt3DZR?*%09*~2=YCvg~N*)KuFg%<{INe1mhEmG%U*gE&AiX%kV|C+lZgs|CzWG=Znse!WT)Ufd8q3(m zoq1OlpBqHu0G*_v&v3~`4F7|H>oNCs+(3*B<7 z?OsnU6PM_kt(g6~jtSpPG9J2a%%3j>0WG8Z(6YddGFOcQ&CH8UupwNW-+h`faTb7m zxSJ#m^IR&p$@G1mn`T^o`c@4XlXK7aQ9y9~fP!G^0!=+w#P)K(F3kf1zX>Y3*%UQL z__iV*C7j*R!3%wcMbOyR6LLA*5wrdV(gy)O04U1SBYz?RjSdUyF~r{p<;(Z z+B{I4WUr;JP&A9uRI!VA>gIQfRr@LW2_N{7E`6*x>2YQ%W?4kx$DpAs-5;_mDMSbVf;+-V{i8m^CoVA+`Amv5a1L zBI~X~`riN8&8w*k2Sg9xKkR;gd}DmamSdT!&@89OByp+G(zNjH;_eiLdNB8ChcT|!9BY09x8sJ8{*X^jPd8S#_@}uM$nu`9 zwx9n{LyIPEtr8i%UBD0D$HR<4#lE0O%qIX@;%;6RU7Yv?nbRiHNCIeY0(fX|kQnUf z`mzpOA#1TJlVMi_oT%lNxf;23^OUn&y1llD3oKFSq7I(L52&vHy-sm=s!d|V(+bZ@ z3b=iSvQE^KfVcd#X`@m~r$+=%M{@h=8>YUowuyo}i-f{jNau{@N&><+&w`*avn`4K z`G4k#71%yM@?Fq>=J9g_u*)ue{jDe;TkQPYG_*W4OyHV}^VJ1>+dY-tp#9xFPcor> zzO1&QKcJJX%HH%JAr1u?D^)Gt-hZ}3@gtNf@RBe3)Ca2xz{ho+C;zp&bO(8lSeh+O zA*Z;Kiv%|!T%I8lxUQ^^d;3WjA1HZgt^I@o{PHeLgAR(ar!28HWo!Id=$_c@(`}~A zZH|KF5bEBb>6Fy(kW-N4j@jC%O?XrsY;Ci@NGH=6zq@|Cmi=#%QdJcgQ#Tts^Xo$& zv;op&mjVc%?NL*`eZ&yL_)Irwon!el@hn;s0?%sie#*X?xYFpuQyi@Gq)QLUds6?~ zk~4hnzjLt$oV$+So@qvHjGh3v&0e;7<>9a)+|Lq7nPql~u!<>3OsTr|sV;7(m(D471n`A^|7`{_xc7*N>|0__c;i$tB}$EIHNZ+M0ndCGukn zDZRjB#>=5g2YGbqkGl-SZ=X$INC3U~NH^46Z_iG*|V;GVxz<=y-BamVPM zlFwb(Vex7S2e0T^+U)S00m>Dm0r95l`7oEuYMRQMaW;JKi~<}5!}ouLG2ZX9ru!iR zfUY6n#)6MrDl9(4k+h0sUFlfw*S=|3b4s-kva~0!K89Lm|L8Ov0?*mWuR-;X1k z|1Dk&=n#-^6nNebw1%H^QSpIO*))US?J%B>brKg`6W^hlp@1#*`h>&iv3?n}A^v|K zr~6QZB1Lo?>W|Me#!J)w`I8$HS&Qn;+*!GA@p|m=BkK;e?i7j12PUe7Kn2m?DvU9= zilsi79xsDaqp0J{f14=(Kh6I3q1mXuK?qc`H}g-4Jsa_>o6U0_RSu17(GCi%;JYoF z1Wp3;OKfQkTq*ZrnYU_YM5IrJM7IrH?Eencu~UM+ZRGX&{+FW)AX?{a8ag^P^4*zw zc&4f<0oLiXNgrtW!WVOcd#E%*TRGWBi5&gJ9w0yIeE1+_4FUQu4{+tsC+S(77j-m1yaF=z27e4AXEWJ``7gY05I z8%z1nD%viFUk@ao{b zVVF*xq|THCPe@A6junoS9rAtZH-#?(eoOtPJ!Xsz@BPcfgsoqO@RTX*D1{eu{cF?o zvU4&3w6^_mr8ZTN{RvCVN=nNrvfTV^zfj4d{-!8FVxHvsLCk7>I7M1KdqnJc1_b*> z$DaW~BYv4?-cb2t_5>MS6&J+fCtsxZn2;csPNX$F70e9ULJN!Cv8V(5i?X!=gbylg zoQ2_oyXg{e{=RPN8Tq8Hi~VsIzy3bD<(x0q1~&-%rYOe`+gvEdiUQ=mPrND!E%GRG z7>WojdshV5IwRPi3*8;ZRUKT=UhaIli@1YSLC;CKO9(v~Z8q;jP z^MYG7jAy4glsLJeKN9zh-{c|q*)M%{2bPi6`*`-VHBy4VrXoy0gn4wNNh_6Szn~g0 zY@p$C5s*86cMJw%BRwx@i5%}WiW{FG(VTeb!DqRq}H<{ z>=0+`nGr2xr(u=nV-k`(03?JyZoFtXP~U&`O3l9i^$O#fUATX4T|Sy)85E(C`1=x9VWNW;c3kVUG1I3>7P5m(Gz~@b|w^ z`mt?>u%vCSAKIyXDRu-i*#v!mHW`C3Yx=X#Ocz!3Jb?uxdrJ!917T$y?* zmy_kzb>4q|W(H_&*83=&oJY2KitN;Bu)S`9-2NHS&*HIQc!pPr7|3a9tgp1x-$}5T z;*V$7LsG{+&rxzT<5XQf*UWH<}dqMI}vg{ef% zO-&UU|M{|TDu{;f#1xKeM0of4&H3)MjjD+>+n%Jg;Zuz4+u5 zmL1|ugfM$*Hx^SmEw&pv8Fjd0h?|sMnUZT9Y+ zg7R~OY3fs)g5wrjZ}Jbn`Z3m44BP9DCb7+aIGXn~gVt!#DG7$4td6+=-qhCzRdK5X zM@L=WPXNw2wC422n2SxF(?=Y{J&zUuqDpl94)1Tbh zBFd9DX>1hNMD?pP~2h*l}c*WK5+jgbhehqqd`rz zEmq|u+~;P%L7E&Ps5@~{BqzQ<{x*Y;6Sj&gF~^=$`GnC;LCb;bnqlZ5EVl>EVeKb5 zQpr1?d)L%4xo`W%MBo0v3(|B41KhXgF&e@a&O}LE`VvsCIRsefex_JSJ@dg)ol@`@ zq=$pz{D*d;A%z|KJ-Oe)X99?64SgY0vWXOC_jX*lK-yIK+IskGLWQasfAYmJW=Bh) zOg0C7{-QgF+8hf0CM0yV>lZnuC9=iTN+>``E971{2I0%4x(18ye+N%H)wEVU$#z*7UEew9Z_77=?&YNy3)qVA$9%RZAkOe}bya8T=__ z!s5fiOvqS?W5|sZv!^?zpb$3a1-Y6^PK3&tf{!W`DcdW4Y&Lm^ofyh3b=g5k&NnlN zIspPUjxHse31%twqw;t@fLjJ-a)eW_8vJ#1G}{m$fDw6P0Qj+3mz;251{TJx?1pq^ zU-~S(99LRCcU#+#8XTqnGlD`qf{>iH8i4-^pj!+D2I@rx&@1->)v~U!V=z}Ba44l* zi@r25yAhv+D-8(D&5g}t>YGEx&%Up7IvjzFyoPb0ds

R=|G!#*m>pe#GR2%D<;%I&*Eb_h+1xj!g>YKUSSas$tqO%Qr2v5j ztqO^-WZ*3Q)29zwSz@t~+4NJ3!!{7*tNkmCMqH4LIu^fXr9BSZiMdjqueFJMwQ^KM z2i&zZtGe*7fW4P?Y+j&tK)SOk{QcDTo3vO;&WgU~T;osI0TyMwk#{mmo@QxAmmu}hSC@PDht zUH(*7R#shK&l!5ed%xmZq*UUSOAqIL@vwq)wymTYn`54QHOmW-1x?pYLFjzjp`tns zk_vdx>zChli{=lzV}I5RE%w$x-9rE<+n0FMQ9!9_?wx@mx!PY`NJTY_rz)G^K!ABTyhLvR0XP-5wTC<141a5(#hAai z1c$?bUH$}~wfydZueD~`eFD6o?oMwEz5Sc^xem8fK~0soL>NlXwdP+aw|PhmF9;ZM zRrg9$a!G{DD!o}{pzp9rY_>$bq0)ZsDdjPhHyr5DPmJcQ0(g;rCu8Qp2-%DbMSp-z-tl8mRsxyjFW6e>IivPruQAiJ* z3&)bCG_reN{xcxe3^Qoz?YfjVYl%>y`622{a$p0>%D)%fjs47iE@S_+=#Qw4$>s@T z-j6KVHMb}MH{lcJ2)+>cu@W9+H7bqeNZ7l=MNzbbp&p|IkC+q~cdB&QSohi<#M_BYWr zo*JI(BTB1pYtJ|mUUs=R(~x(mvPC|@KF!zLTtT(&bKCCQHvV?HemcBLD!L`ZliD-& zy#A}NcbwQ-ME$qX&i0NB@G=&ELJ6Ba&d0uVQy9xqvnU=VhMz+R`_v$2)?HEbKO(*M<02WM-X2Ekh zBXnFV`_pvi$%+B>aG>wk=e4Tl<)C|TwjKLwEjAO9ZU>w0WEEkv&zJ`tEP|0pV^-WP zgP|G;btPikXk{#6wm!XUTbA!85%550f6~(qw_?+m(>y#};Ll7)WDmigD54w2wGte1 zpK6pAOCOpV%Z4QyHM>Stb9kG-c#S2%W)ZO)$q_jDL7_A5%d}sUQvleAoptp=mos=M z+~5WtZ!oP#L`s+FDCs%s>u$nOBa4%KV!pnB{hYpe8P zP+!;RZcm!LN}te4wp{8PbYA4f61tKv7VK!=W;1LiH~2IBCz|q+mEWp$CYaMd>sCox zBkyN~DKuKDoh&Vru2vv##+w)Gj#tr-i|N2_U}T6d@In1N$Xdfl9J{9` zGov+%mrSzf^bhGW!SZQk-q;Sh@P!Kh{F$70kIlg8%7^QK%WZE+7AMfL)-aBAx*y!!{vwXtw%EJ*wENxk#aaq)5fHwZ8KD)&wOQ>{PVkLX;rt ze}+z)l_Msnqy@zJI=!sBtoecd7Yjf0R9pZNn>z20S#fNbQ?}*1y`7dxA#Bw%caGL% zqv@EI9A59SXu%~()v}O(iftndwKO-PxBOQaiAa23yyrA=s$X1@SH6>PH$vkP;RgQ( z=G?dRHKcCxk0T*(92RQyG-@dqu2I9X1@SK@JNUer!aa6SH~&6`=0bpNuG!d7#NE=6 zbffHRb3CZlQ3nWG2|!b*!$;^<0o%6e%M>pFbl+({w+C1RGEa_MJ2mw%p*4jtO`L715o(=;X z(O&yNzFlc;PbdQN!#emA6I=HuNoxbsU`XXO`JKhFusw6(6=E3g99GtrUMMYlxL*>G zC_RfM-?q^8oJ(*Ls!~7;P9o7zrlf-_**+*0hq&3<*;9m$`VBt}E#xYI3fR{7`sCud zlymXs{l)H!lrvxMy&vA2=O$`u^?Y(~N8P{?Ukzosgr{o|)cZ~60p>KfSXE2La%hlY z45mH*w&GWSw^XP_oObqtTbiV>2rCTd{8(%JXj*;*@&^_^uIOM5-Dw9xk$|ER0LbGe zF2GA=L6TUc2ymbHaD8#d4P3VIPU!QUQrf=BglynIsS&4)t&mk44dsMQf$5G^Z?Lr4 z9i=WUuUPWz_L68dXjku-0uT{)EOCbj}oI2d%TZ+jclJeCqhYf9D$1x=$tW zX%MuXDx4@^74JNn^H#obm1Qi6guULg_b=L{T+X}PU_vYH{wf;Uwrf;nNMsuFLYId`?^$5(C4oRTbOCI-X z$tp}0r;S2pr8{i@noQ*~)_G`d+(IXx83f@7KoY3ya{Ulh0)jGm83rC#@H|<;`LQH# z$0ruAKh##i0{}BNdEm!$Q{6|~gyzu*^8MDR7{tC~4p(db{B66X+r5}i4L4~0yk7Nb zJl+w#M!gCnYUszwm`2C)HhgxizPFMIwW*nB9jjV_#SZJKs+`%=9e&P*_#Z!Q)y%sZ z7T5~3i9e@N%fIi8;eG(5rsHr_Y3C|}kBf$lHoC9AGS7;08k2P8xEpF};li?Yvs!S|ZP*F8-STKe5{|sWU zUK*$>ZVsg=J8!nh#{g=-HUPm9?S$Gq-~3Uy?x8K|%$EC4XIY%NJ7qF_k5SBe&Mkp^JjdkJn zhwJ?U5oA`oR?qWH!7h`49XC|S7Iw3RP>epyvm#>svVr}wTNH4gZPw-xXCNQDEbyXn zU(g|Q*>L%3HJo(1Le1f_eiUZdJIiY7{F1LX!K(XGqsd~Qj3#2v6qOwQH+;Q^a)N`} z@~1f))8UNfTB+yO2^kzj36e60lL#3mz=`tkBU>MKk(PVDb3X200ZC-z7fYY1b*d;& z7MXJ-(KXnTcel8Wa|mZ6Zs7d#9+z2(Fw2A$JzGs7&hY$>_=7b4uy<7v<sb?SvOGRbYVq8QFZY3R;l>W93*O&! zGkQF#+S|u~=9!5pDFupD*Xj4A8_&GQ$HxKgrrreR{K*m~;|-wxpI3TFdX5>%80{+@ z2h+yGmT-T8yE`+JwBw)xuQg`5x55%HPO(hNeJ(jG-5s}GBwutCeM+e7Ed;7{6)g7m z_N-U+`R*I&-R@h~^QKQjOG9{%x{2L6{`z*( z`(e^1r`H}n8^4c&^~o*kaH{IFvRUpc+No)5#g7t7zjO3@j>+zibjYcr`q{pp=C$Hl zuh>&H!B^rD3gO=b*+3=rn_9-4vic|dTFMf2VPmM_)kntu-5_&%94x$yS3L6eFUx9q z#N2l7m_V7InT&%j=L9>ck;=9KL(UJ&YC)TtzI^6_%KCOlf+!&Oa@WJWpT7ANcl@yL zz7j0)T*sQ1ATi@n1w^+Np zokubAM+)U@ZUm#hUO{6o2CDCNuL7kt)Q^vyx=WgiI5et?m#90Of=CNJ;0riATzddH zWIWbe^WmEvER$LreLE}7-329n?_B_*bD!`654Oc(&>Hr<{~Wk{t4gC=Tno;`f%g$>P-+Q4JJz-VsGb*DxiOWQ04GF`bQAVTI}e; za)PgndLl)oiz9XFj^tD7+ajTC`xe1u;c&|qX4U~XqgtuMxk9J>Tm56nU76m!1GOFb{yHqCRiXx zaVI(;ZN=)7Jlt1ndoo*uyC;uNQ9yd5A{FxwmkxpP@h#`9sRM=m@on?VI5Bj97k(I| zL*A_HH-wS=`Y!bgvbC1DqB(x`72_T@R#9s?_zM3vt@9qv$GKyNHTMfT>Dj8Sxdu<9 zJm9_AlH!gof^uJDOF`!XHj}olo}f6VhT#^LXDt6}QVv;mD;aAk&p#2I!Fc}_v8kA$ zbgvXT+x=R`rUpE1kbYM#XnN+0P=jFcED?KKUNP1pK{nEJ2 zUA{1@o+uc0in?0I>=Qv`3YB5Rpy*L9aNA#8>S^M><@&+FisREPn%NDDSRQ&EVxnh5 zh#%**x$mLs80=m#Jj)|H4hI1ccy3M!s9?nvama8zYf~(l6W0Ws^Mx-~BSnE>VsudXDJkSF#ekh6xSORD+h&)8z6(K5*M+bvD0zHJl+@Xyz0uTC~VvD#pjfKpg?4Y;U` z57ug9(xkpIRi4+colnJ+#8JZw>(1EyXKa%m6oW$Lf9#BeKXjfs#4exAm4bGbKrfs=XH^ zwQC{u2OR|`=swW`R_)F8SZ9Ip))|cX)v@+t0%6#AxUa=w4Cxj1Z`cGll3U-}6#2)f zk8I0I3EeK2ft#H@Fr=bOi=zt6>8lvv=!pOR)zB~9Ggfi6=gY2?$D~yVmDhzDcFb~! zv1D@X&HRZlVcOsyqe&ig%vKHz3S#Hz4|Wg8vc`=lMU-lR^tOlf>tZ)#|EwDe4ESnb zNRuzePF6%<2+}P}7qzg5!4k@H4|1jFi;mgc5J#GNdxiwa?@gK)3VoRM%L5%&75oaP68|c ze;LbWA|aR){D%Mpg?z*)9;nL2A`&>uBuM>G$ag=~-!_4|xq{@!_hk(l-}qYlN%pzU(-lX)9i*r)ih9x(C{`!W{pMAKU zxKKH#B1qZ0$ZUkPl_3CBdiVl!PBb)f-3!9_&QX&i_t=s@6@ah&PMFG|pL}X4+q}E2 zm`i$oSYar0+8)M9eL60H9)nc$EaFr};`)*YK`jLA{a9P0;6eZt9(dqeeVG(Cc)Sb` zDF?i?AI>D~i0;qz->?UP?NTQsV={L$&|m0q#QZZRE^x&cdwMPDrS8a>)%gyQ;g(3= z6eD|W=6v6lq{=3W1z=sgFP|H^KICD2vGe@9%guOs6kh1d2K}?pI&NKlQhCnk>vzj@ z{Q9_X2S=6n*y{Rr{}?A(QosGMykY~kUp}7KNs}v*f#L6?WJk6N5M#fRuY3ojX7Xe0 zcZxWAM-%u~#TkTZp8Ap5S?0N0{wt+K8HGLX;(If^SG=V|iD_pil^p-45r$9vvACwamL9~;=46o45n>@ElHX0~(VNl%Jc`*Vi}!UW1MP3`s=&t)9Ck3I- z!R#*PBYUY|)AJSUEo02w#6-%-zO93OJ6a%U8r^IwZivplGUS|s^8RJ9R_D^b{+zjG#W5gdI$-}B4~6Ntf$P4{Bk0y zpsf(ECF-J9pj%Gco0nY-K(SQ+Gp}eW1FW2QRCz(}F=glV;(c298lq3Ls`ynVs}of+ z*8>g+rp0trz%f+RN@?5lUwcz;`<*HHq(ha~x&ziW$sH|>4}3SL=nY%;Osj@({MmyI zFt)65_(jO6NEGkyr)d(QU!6SGJKjCpTAB-t@>5o`*-`TLqG!ySuF7dF94#ugSz6aL zjZx{ndq7d1HwXd!1rZ^yK)h4u1_-|+g4)(?3ZlTlY_Y)W<5rNC_2kv&3(++Z z35TYBjy2{NR}t-lC?_=|y%zXHt97+?MlZJp-B2w8F1X`XVS&O0JXc2gfdkwZbl zpZbZUF#tY~9<>KlM@%sif(C1!m^r3j2&2vb!Dp1eI%4H+AaQt-&vo+ro;fNc6aPD7 z+zaBgoDh#RUp)N$9$3{u__w@hqEfI6(DKXB9shLpdqz$&Sh~bW4NIHPgYNMp2?T*| zR3TsNRiO69ldQ|Rtnr%|(pf7np1_8*DT(a`PugqXK7Btfi;q+VnZrRp!LaxX<<=kY zh$N}v4i~T6*=YIqKnNlIYxF|5|B+P?`JYesftfc3-ihf9&@q<`8Z>7z8VmGGHr?TG z&+Z!~12QalY-GlKw$#J}*LvN8842rm%q-*8;jDI(-`9)=SW}}Zw~8`p6y)`A;DaiO zoDz6+vl5O*N~?Kt8g1+#6*ayXBaN9;n%s_W0iJ;ZF_YFLDex(sa`Uw*jwhg>*O z-BEV4(+n%AaZ}r5a76P^Kk~gg1#$3<8yQTT zcDY=|RSTR=!4v(L(QzvUhXi_`(01X7d(!|M`lQ=(V@|b(_7|ezZ|DW2%wfIq+;X-7 z(9J}ek83`z#`Ve1FW-TDdf@c;GNpnIq)Va5jdTL0Z0eAMyo2Zy6gyF9uY$fup84->o!hqFzncV)LqLa@W|twr!ioUPK=HpxkuF-kbbwaSK6Vd8XXUh2;e3cX&LxCF)!M9~t_58KvJ;LeTH*W^1~XBTgwP za}r5n%{WjRH#1P+CBQ`dH-@;*qgJwE5wlt2o>Jewfnv$y09V&Q9zQ0Ynfhnp{fbQ|9A{4&Gv%~+e9tVkSmmJ}w z-cJ`3nHrE|*1t&3sXk)iac;o_?(cRhp?tK#HBnsW<3ti26HZyp;tKZy4u_y($^t6=>MJc zVy1ji_VQ`^jV>}XNEi&K-~hE+ocNbrTaNT13FYR%8p#|Rm{Tn{74OJ2ug+x@l;ag^ zS+~5zsG_@o(+9`*)whASp>@iBsnFJXsE0wjCpgZP2ziG#6EO*vpUI|oNANm|@Efoj zYbY5(uFDYRfrbjvfb{JwuLElm6}G*1ptBTXxqnSkWD*3U)gaRZ*26@erhf$y=Lc25 zd@(%o-HXD;Cm|ly#U%2LTnT9myaEeDCW7{6w}>4!s9i3L;f2?O%!EF}`ekS50~}V; zcMl`ar^QoqA$n4Q?$66_E$%m-B=N1`?Ptk2q9Dtj(|ws7feo|8$&HNpmde~Ln^NAtD<*EI5s)%%fR)@hPK#)1 zTL?!r6Wn}0HUv)-Zyx+{jL1Fag5IuuVj*T4rN`I;I&r7R(vfiA|gdf#$^ z%U2Wm zy)c+jtDlqNA_BzPNVnnJxOmLpCmo~laI$DkP$Po=^cK}XBR-9dK|+*y;T;vZM{w9yOr%LsQ`)kEa^03 zNQjCd00sj7U4ezcuXB08iU6HRvYr0$oS$OnZH-u5nG85yTG>}xo5aYCG0c<{92;0W zKnoM-wfUQ9-oQnbdjqEha%@a$J-AUaTd``f%*dwXnm%lJlS{J#qy)zcRG4FT&2B0e*Yts`1W2#s2=m zJSjR_eUU_5Bm|sbN6#}0h3H1Guo$+ku*=LnH&*c@*Oe zz@<$U!48w4K$m#L(D0v*{o@l<5X#_fu(NRfG*R-@Bef5K%c-*^Ep3d$dqQ1S$_d;c&crd>G9@*j{!ITXX{gs9WE><+hG7LN{bA z456BNik&yxJ@y50+jz@)woYA?79|J7b&3{}7sLBW&!3<(4ZLr3rcAMz-0fFSt6Y(y zT*>U^#imcxe(pi&HwG|7yq~zzHcD7CLwH1JFtQzdOIg3?CZIT}TE0wgw-9283JL60eB@3|V?e!oG zzlfR+>E@ffv3b&Qn&OW0-k;!k=<_%*KOMBMkDhctF7R(aW!F^4h3yrGP?ciP7Vh7F z9z_t6IxBPi=V!=4KaLIo<}6jldW&mHV?9)WWDZ0Y%5P1z;%#}i8Vt&yKkGXSIpz4hzg-6ZTxv+K*F zk2ygJ5OX1!U=J;i9FwFa_s{m5j;?{xr%A6-Ym^@-PeFBnZ~Ho!t}7lPp+o3XrT4wW z^uTi>pk}j&*dsKV@F?;5tuQv9x5LJvL0_0H4t+G1f_4#Z!rkd=*3D=ZluZbMj{f5F zCpsZ4`G=JfaH=CTFOtow2f?UMAeE^Otfg_;<>+={2bIbzwiu5FA$`lZ(0m4r| zFhm&{r;Ih4CSd#Lhkf2kGbpE(lgK?sE&LgRuh((ZhDbDZPV)#FOY@#E>QXP`!JjpAJ*MU#ycR+^twJae7u8{*sRt1^jr`If^U<2k;$h>r zov!!-2L1h?VTJ!NQD_u0JhwCF88`gM1u>_m7Uot~+nqi#RkgLDCCb{q<;N;&YJGQi zcVhT>Hjt4RBKe1*2U@nDV}*WIdbS`r%?fkFIj~2kkbid_r1RQ37Pd|!z7cdThw>a9 zw?z6r=5q?cGm~)aig#a{<>cTDft0?>Pg|rz>c8g7a{+-^m)mvzxWS1pCJ}B$%#uv$ zLer+@CcWD&*YK;{#@@=QpA8eor{Rddqfn5NVgnl^x(rNXGbvdoFY|kenHT6MF8;d( ze=AXQR<^3fO6Z?iQI~)h6YYGw-rsDdd+ms3SETTkkc0ptt!LrzPsz=vo>XlPC)d-_ z4Rqj{B67lUogSU(jUkn61-cp_ajsu{Eujm&06lm{z-hz3-!NyF4WU{=z!=jv+HzZA z&jvI&LR6q=OM@Y-^Y;mfZA$U(su`%)Tbsv9YOwlOz~|Rss{G-nq>!Z{EgCI5p5VhU zfQHZ5x~cNlYr17F1|kP-&%aoNQi7BOZGw8V(e>%dF`PgVUS_Q@9O>Cf8d_ad7krK z&U5c`)QiV|qW=8;LQ^p;s@BZvMqSh9$-d=X zK_miMo)NcNCJ4s@u;4HbrYC#vg|o{KW?A?2A7T&Mx(4X~0_#Q1-MS7l%?Ja*o4~Nm z9__~`_H96Y5{tV@)XRl`<2UZnbaA_0l~wY^m9gE??b*Z&S@-jv>$LrQXK&)a?|X9e zTY}v+)>ia~Dt@|Ct(i2T?3beLuYzJac8}NFtPQl&xAYzwPfVg5v+6-}XQ*4z^{%VF)vsp~tn%$z$D}^FFpA_cAmNL!*YN zHu1*2vbCQxeMI-fPIrwY@eKU05L;w67!}^`Vf9*(?r&MKM(Zq&(D=xkWyMy^MQT2r@|?9cGqmIFSaPJ=!KB>avoj|vBe)Y{qE?V&j8YA^n0l?kNQ3K zcj?k2sk|*UAS#_u`DWeRzILPE!DR22GHyTQ`@t;G*=h2bwDrkur_l*e3@GGb0`(Q4 z@Zho$O7?0ZLGjMmQdI=I-65SoS1Em4_%DYb4*H8VC6Z#p!Nb!5F6k#c9wBNhD_-AG z39wT&yxbD{R;JQiOfn%iTg>P&#EeiP=s8NuO<2Hp9yx=C_yYF)$%>e#gU=hByRLK_ zx>}(;!F?|7)Q1LR&2myw==`^E=&u$cY=1+pmgd8h-MZd}Ta&!if6F^0yZg!@wF^D1 zXP(;dr-y~r%U3?@J-O4o*>k5{G9}_v>3z-CycLn|PfK3^5$Kr{VB+MoUFgUqFHdO{ z@~OeeJbHPxVfYty8z0{3g&A`VPaA|G+JZ$NWC9~jlkBwpcvvOh?1fg5UrR)JWyCI} zp>fO)uGOGN=q>ohs^X%(Axo1}g+1}f!j>g|HNOVf0-YLC6RKQ-T&Y)tCP-qkQ=$D) z)$`Bp3GT@#^RzZF9J1WHH5v5h`l-LNIa1@KB((f}V!7NxLIjXV%n)hUG4p|{Mf#7m zoUkRmT#S0-LB}DZ%*B}%yC@tx4ioabj{2y*__ER62O3*4uvH}?VwPQf5R6;F^z_G- zJE+nt#jKs?@=R7zva0h;gTfEGwJRo0TfkgW1OzE@i2zli5&`oJ_VTm3YW-W4?Th8B zN6oW4!9$2;hU0Ly-G}E1H)OebT=`E=Uhi8mgdB~WDqE3VT?z1sye(U7%8LNr*jsgK zpG!rCw6@OMX>Cw7=4p;uR5#>MVo@6+PWlRCltl7@Q+4>!CU>2JSJ0%lF@H~_R@?6IIa_FP2|o|t)<*Ep(H-wImEI&HNZ&lN&}Q)bu=N?Y@${J zb&uzM5dHp-2M2|TOI&61t;y&!52rmvd|5T^ED3%_MJbCHl4R$lGdvADPpINy@?M@! zMPVz}dm*>}QIu=3Ug9v7b?JIDyL)0507k z=E=+W^a@kT_Br1Rm%_JA(It{q7ej8r&+)}|eF*;P0F5R<%iiy0DP34$fzWyvmUtI5 zs4ArT-`;X_Gf=g3ZL(j6@@{ZW<>q$=-0DIUV-mKx;Qi6YHQ=+-`OqJ2n>ssbdpo{A zKFDAiRu%l_otcTPt9kyVT=C`H1Ve;M$zj_5o31uy`o+9(|KoLA$z%b%oC&{GFiF_5 z{my#hU#ru8LXg)1_#6;SX$h?w^Vq$f7bdQ!dCp%rGG@a5Xpq;8hRvuWG!gMSOL?i; zcg}Vx>V$a~4mjFXI--YlcKfBv2CYA}JaToAA$09_YYU$PJ*)ljp8OE*{s6HELHfnF zBD8^f7AQf7qnq+3=H0NJ?s8~&`Eg71mp#!@p&OSPFT@S>QvKU#5eXwI22+~|o4|a; z#uqAhqD<)o5P2IQO~zWz-k5M#hMnF`%B8*X^yLEIaWa;h3f`8erjnT+MH+U+`< z<3K3%l+1@NN7Fb)RBS$~JZ9O!`uT2MSVx@as>Io zO1sa^5Tvn#sZNk416{wjg6CX7lfjUNpUI3~zlUqAoTSTID|0=OvD`XWnttp^>N{Kw z&$12pB)tE|IA)~l(v!lN3Ay7%*Y8)StWZz4QAHC?vc4~?u*xMDpEFh#2*4}FS*%MR zS{<;-F$51e}?B1O#r z<6C8y(tPQ0C5uO!IU?4&avPgvg31uFFJ6~}{DrcGes>#~NV?1Ta`Q;h(?;Z_3 zFU-kV#KFno$6~~p^bGi4hb#QjraTOSP8pLu4|2pk>Kp<7sXhhOkY`|L)r!ios_`du zJY~@hbRIE^$~$Zp6;FQy309>Hv+bEd^&PjUFnQ?sCpcYwuUWkf=&GMjyb1T@%b8y= zeXDddlCLv8uIKw@x*_Dvl2!|$!zY+yl5>DpV3)pUV~er{65p|!AKj{`uHW}cJNj%8 zef?}RTleEalm*~<&S9oJr~gc ztuw~YhFCZn7xuo++JUrk(a_`}wO-edx{x=M_aS^T?VhY&+uH60>}Htar|-*=pEjrA zybDsvrIrZpR`Wi6uS^A_PMvSX#h;IBWixd*m=~lw!U-W4tqSE&ZD= z^U777h~BZ)T5h6V7;kH>Znu~CTCb*J+mF2;IV7$#ntjp;w1FTm5cG^Ftd8gf26pAc z6XRh85tTqwp3<3I}y{!KrxF7VrSfe@1{9SF95pVl3T}ktXqE}^Fzz$LrBNwZq!Q%9UFMGxN)CSEJxYx?2G6(J zYcEfqN1%)125F0K7uK1plJ3%d3@eITs5rl~-~Egz@+;2SFxEE|?H+1M`5F^utV?wm zSLoB@wgK07RhD~NaQ~G}i6`~+3y6H}BckNvf5LqW=g!@!XvxfRW0zx+s2|L7_TpU3 zude&-aVyQ@B?9$eEvWVdMzC<~PuBkLLcelk^6P-1W3Eyn3?wxsg5^kne&W!TW&{Wk zlj7s5HF~8-_;G!z_no%uk<_?^g$kZZ%I4j9&tN{Y=u^Yg>z64^pz=DK)zI&J@Y@3$ zDT1f&vp;Q(FfEWqUxwcq_~t2PzP?O#RBWqlQX5~<1sK5KgaO_H4B+4IL%9Zf%q3@Q zb%T(k5iGA^jc|NAz`4y|kh3F}BqXlXQ1#l6g#MgNnbEwU8|E6N!raM;f~VZYzMia+ zHa4?NTr?V#i^ec5h|#eJ@lHtd15LrC+U7FJ1f2M2q4epGwAUVzAL=QEF=OgSL2L@& z^~Dh3Ux{Tok5?WX%=(W;9K{;0BM&SxB3jDM* z>4w32H{0t0JcwO%x~!LFak?_5=k~mXHg9a~$5QFu%@mG7Y=U?l`N_E%9_L7>9^#;z zP^{Zls#77D8tqLex8NR}`&GRZGq);7^sM zZ98$|g#4T5G=`XmZhMP(rvYMR{WhQyeYcOn`jtDex%12sAmD++jJ2^^juHO@l)Fdw literal 0 HcmV?d00001 diff --git a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp index 3afa3dfdd5..36a75ca4bb 100644 --- a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp +++ b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp @@ -45,9 +45,14 @@ #include static const std::string ROBOT_DESCRIPTION = "robot_description"; + +/** \brief Factor to compute the maximum number of trials random clutter generation. */ static const int MAX_SEARCH_FACTOR_CLUTTER = 3; + +/** \brief Factor to compute the maximum number of trials for random state generation. */ static const int MAX_SEARCH_FACTOR_STATES = 30; +/** \brief Defines a random robot state. */ enum class RobotStateSelector { IN_COLLISION, @@ -55,12 +60,14 @@ enum class RobotStateSelector RANDOM, }; +/** \brief Enumerates the available collision detectors. */ enum class CollisionDetector { FCL, BULLET, }; +/** \brief Enumerates the different types of collision objects. */ enum class CollisionObjectType { MESH, @@ -68,7 +75,7 @@ enum class CollisionObjectType }; /** \brief Clutters the world of the planning scene with random objects in a certain area around the origin. All added - * objects are not in collision. + * objects are not in collision with the robot. * * \param planning_scene The planning scene * \param num_objects The number of objects to be cluttered @@ -188,8 +195,11 @@ void runCollisionDetection(unsigned int trials, const planning_scene::PlanningSc if (!only_self) { req.contacts = true; - req.max_contacts = 999; - req.max_contacts_per_pair = 99; + req.max_contacts = 99; + req.max_contacts_per_pair = 10; + // If distance is turned on it will slow down the collision checking a lot. Try reducing the + // number of contacts consequently. + // req.distance = true; } ros::WallTime start = ros::WallTime::now(); @@ -286,7 +296,7 @@ int main(int argc, char** argv) ros::Publisher planning_scene_diff_publisher = node_handle.advertise("planning_scene", 1); - unsigned int trials = 5000; + unsigned int trials = 10000; ros::AsyncSpinner spinner(1); spinner.start(); @@ -331,6 +341,7 @@ int main(int argc, char** argv) runCollisionDetection(trials, planning_scene, sampled_states, CollisionDetector::BULLET, false); runCollisionDetection(trials, planning_scene, sampled_states, CollisionDetector::FCL, false); + // bring the robot into a position which collides with the world clutter double joint_2 = 1.5; current_state.setJointPositions("panda_joint2", &joint_2); current_state.update(); @@ -342,9 +353,15 @@ int main(int argc, char** argv) runCollisionDetection(trials, planning_scene, sampled_states_2, CollisionDetector::BULLET, false); runCollisionDetection(trials, planning_scene, sampled_states_2, CollisionDetector::FCL, false); - moveit_msgs::PlanningScene planning_scene_msg; - planning_scene->getPlanningSceneMsg(planning_scene_msg); - planning_scene_diff_publisher.publish(planning_scene_msg); + bool visualize; + node_handle.getParam("/compare_collision_checking_speed/visualization", visualize); + if (visualize) + { + // publishes the planning scene to visualize in rviz if possible + moveit_msgs::PlanningScene planning_scene_msg; + planning_scene->getPlanningSceneMsg(planning_scene_msg); + planning_scene_diff_publisher.publish(planning_scene_msg); + } } else { From a6c173f3fbd6a3433908c370b0178ef8c6e9d6e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Mon, 13 Jan 2020 17:42:10 +0100 Subject: [PATCH 153/612] ikfast: implement Translation*AxisAngle4D IK type (#1823) --- .../templates/ikfast61_moveit_plugin_template.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index b90eb5faf7..775d9dc104 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -596,15 +596,6 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector 0 ? &vfree[0] : nullptr, solutions); return solutions.GetNumSolutions(); - case IKP_TranslationXAxisAngle4D: - case IKP_TranslationYAxisAngle4D: - case IKP_TranslationZAxisAngle4D: - // For *TranslationXAxisAngle4D*, *TranslationYAxisAngle4D*, *TranslationZAxisAngle4D* - end effector origin - // reaches desired 3D translation, manipulator direction makes a specific angle with x/y/z-axis (defined in the - // manipulator base link’s coordinate system) - ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet."); - return 0; - case IKP_TranslationLocalGlobal6D: // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end // effector coordinate system. @@ -618,6 +609,10 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector 0 ? &vfree[0] : nullptr, solutions); return solutions.GetNumSolutions(); + case IKP_TranslationYAxisAngle4D: case IKP_TranslationYAxisAngleXNorm4D: // For **TranslationYAxisAngleXNorm4D** - end effector origin reaches desired 3D translation, manipulator // direction needs to be orthogonal to x axis and be rotated at a certain angle starting from the y axis (defined @@ -635,6 +631,7 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector 0 ? &vfree[0] : nullptr, solutions); return solutions.GetNumSolutions(); + case IKP_TranslationZAxisAngle4D: case IKP_TranslationZAxisAngleYNorm4D: // For **TranslationZAxisAngleYNorm4D** - end effector origin reaches desired 3D translation, manipulator // direction needs to be orthogonal to y axis and be rotated at a certain angle starting from the z axis (defined From a2b7b02f7a7157f37659fe3a708dd9c87fc84d65 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 15 Jan 2020 09:09:30 +0100 Subject: [PATCH 154/612] Fix RobotTrajectory's copy constructor (#1834) Fixup for 01ff0358b58ef14aeedb1ceee98887c3c49f8911 (#1760) --- moveit_core/robot_trajectory/src/robot_trajectory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 5f75bf5b62..70e837ca9d 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -56,9 +56,9 @@ RobotTrajectory::RobotTrajectory(const robot_model::RobotModelConstPtr& robot_mo RobotTrajectory::RobotTrajectory(const RobotTrajectory& other, bool deepcopy) { *this = other; // default assignment operator performs a shallow copy - this->waypoints_.clear(); if (deepcopy) { + this->waypoints_.clear(); for (const auto& waypoint : other.waypoints_) { this->waypoints_.emplace_back(std::make_shared(*waypoint)); From 06a9e4beac1e94d862c8286a22561529cb03d638 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 16 Jan 2020 09:32:39 +0100 Subject: [PATCH 155/612] moveit_jog_arm: fix dependencies + catkin_lint issues --- moveit_experimental/moveit_jog_arm/CMakeLists.txt | 8 +++++++- moveit_experimental/moveit_jog_arm/package.xml | 4 ++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index 58daac76d8..8130c87c6a 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -18,6 +18,9 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs moveit_ros_planning_interface rosparam_shortcuts + sensor_msgs + std_msgs + trajectory_msgs ) find_package(Eigen3 REQUIRED) @@ -31,6 +34,9 @@ catkin_package( geometry_msgs moveit_ros_planning_interface rosparam_shortcuts + sensor_msgs + std_msgs + trajectory_msgs DEPENDS EIGEN3 ) @@ -78,10 +84,10 @@ target_link_libraries(cpp_interface_example add_executable(jog_server src/collision_check_thread.cpp - src/jog_server.cpp src/jog_calcs.cpp src/jog_interface_base.cpp src/jog_ros_interface.cpp + src/jog_server.cpp src/low_pass_filter.cpp ) add_dependencies(jog_server ${catkin_EXPORTED_TARGETS}) diff --git a/moveit_experimental/moveit_jog_arm/package.xml b/moveit_experimental/moveit_jog_arm/package.xml index 9926ba24e7..d57541434a 100644 --- a/moveit_experimental/moveit_jog_arm/package.xml +++ b/moveit_experimental/moveit_jog_arm/package.xml @@ -21,10 +21,14 @@ control_msgs geometry_msgs + sensor_msgs + std_msgs + trajectory_msgs moveit_ros_planning_interface rosparam_shortcuts joy_teleop + spacenav_node rostest ros_pytest From 8f75792ba52237646fa560ee3b26c617e4ab8ea5 Mon Sep 17 00:00:00 2001 From: Jens P Date: Fri, 17 Jan 2020 15:56:09 +0100 Subject: [PATCH 156/612] Adapt cmake for Bullet (#1744) * cmake for using Bullet only if correct version available * Fixup * PR review fixup * Rewrite with own FindBullet.cmake * Use pkg-config * Fix travis failure for melodic * Fix moveit_ros_planning dependency on Bullet --- .travis.yml | 1 + moveit_core/CMakeLists.txt | 29 +++++++++++++++---- moveit_core/CMakeModules/FindBullet.cmake | 10 +++++++ .../planning_components_tools/CMakeLists.txt | 6 ++-- 4 files changed, 38 insertions(+), 8 deletions(-) create mode 100644 moveit_core/CMakeModules/FindBullet.cmake diff --git a/.travis.yml b/.travis.yml index c51f7602da..6ed584a5ff 100644 --- a/.travis.yml +++ b/.travis.yml @@ -25,6 +25,7 @@ env: - TEST="clang-format catkin_lint" - TEST=code-coverage - ROS_DISTRO=melodic + - ROS_DISTRO=kinetic matrix: # Add a separate config to the matrix, using clang as compiler include: diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 57f19a86be..5f1ff12c85 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -21,8 +21,21 @@ endif() find_package(Boost REQUIRED system filesystem date_time thread iostreams) find_package(Eigen3 REQUIRED) - # TODO: Move collision detection into separate packages -find_package(Bullet REQUIRED) + +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") + +# TODO: Move collision detection into separate packages +find_package(Bullet 2.87) + +# TODO(j-petit): Version check can be dropped when Xenial reaches end-of-life +if(BULLET_FOUND) + set(BULLET_ENABLE "BULLET") + set(BULLET_LIB "moveit_collision_detection_bullet") + set(BULLET_INC "collision_detection_bullet/include") + message(STATUS "Compiling with Bullet") +else() + message(STATUS "Version of Bullet too old or not available: disabling Bullet collision detection plugin. Try using Ubuntu 18.04 or later.") +endif() find_package(PkgConfig REQUIRED) pkg_check_modules(LIBFCL_PC REQUIRED fcl) @@ -30,6 +43,7 @@ pkg_check_modules(LIBFCL_PC REQUIRED fcl) find_path(LIBFCL_INCLUDE_DIRS fcl/config.h HINTS ${LIBFCL_PC_INCLUDE_DIR} ${LIBFCL_PC_INCLUDE_DIRS}) find_library(LIBFCL_LIBRARIES fcl HINTS ${LIBFCL_PC_LIBRARY_DIRS}) + find_package(octomap REQUIRED) find_package(urdfdom REQUIRED) find_package(urdfdom_headers REQUIRED) @@ -71,7 +85,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS backtrace/include collision_detection/include collision_detection_fcl/include - collision_detection_bullet/include + ${BULLET_INC} constraint_samplers/include controller_manager/include distance_field/include @@ -108,7 +122,7 @@ catkin_package( moveit_planning_interface moveit_collision_detection moveit_collision_detection_fcl - moveit_collision_detection_bullet + ${BULLET_LIB} moveit_kinematic_constraints moveit_planning_scene moveit_constraint_samplers @@ -145,7 +159,7 @@ catkin_package( OCTOMAP urdfdom urdfdom_headers - BULLET + ${BULLET_ENABLE} ) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} @@ -190,7 +204,6 @@ add_subdirectory(robot_state) add_subdirectory(robot_trajectory) add_subdirectory(collision_detection) add_subdirectory(collision_detection_fcl) -add_subdirectory(collision_detection_bullet) add_subdirectory(kinematic_constraints) add_subdirectory(planning_scene) add_subdirectory(constraint_samplers) @@ -201,3 +214,7 @@ add_subdirectory(distance_field) add_subdirectory(collision_distance_field) add_subdirectory(kinematics_metrics) add_subdirectory(dynamics_solver) + +if(BULLET_ENABLE) + add_subdirectory(collision_detection_bullet) +endif() diff --git a/moveit_core/CMakeModules/FindBullet.cmake b/moveit_core/CMakeModules/FindBullet.cmake new file mode 100644 index 0000000000..1dff096d89 --- /dev/null +++ b/moveit_core/CMakeModules/FindBullet.cmake @@ -0,0 +1,10 @@ +include(FindPackageHandleStandardArgs) +find_package(PkgConfig) + +if(PKGCONFIG_FOUND) + pkg_check_modules(BULLET bullet) +endif() + +find_package_handle_standard_args(Bullet + REQUIRED_VARS BULLET_LIBRARIES BULLET_INCLUDE_DIRS + VERSION_VAR BULLET_VERSION) diff --git a/moveit_ros/planning/planning_components_tools/CMakeLists.txt b/moveit_ros/planning/planning_components_tools/CMakeLists.txt index 77f51dacd2..4faa041dfb 100644 --- a/moveit_ros/planning/planning_components_tools/CMakeLists.txt +++ b/moveit_ros/planning/planning_components_tools/CMakeLists.txt @@ -14,8 +14,10 @@ target_link_libraries(moveit_visualize_robot_collision_volume moveit_planning_sc add_executable(moveit_evaluate_collision_checking_speed src/evaluate_collision_checking_speed.cpp) target_link_libraries(moveit_evaluate_collision_checking_speed moveit_planning_scene_monitor ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_executable(moveit_compare_collision_checking_speed_fcl_bullet src/compare_collision_speed_checking_fcl_bullet.cpp) -target_link_libraries(moveit_compare_collision_checking_speed_fcl_bullet moveit_planning_scene_monitor ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +if("${catkin_LIBRARIES}" MATCHES "moveit_collision_detection_bullet") + add_executable(moveit_compare_collision_checking_speed_fcl_bullet src/compare_collision_speed_checking_fcl_bullet.cpp) + target_link_libraries(moveit_compare_collision_checking_speed_fcl_bullet moveit_planning_scene_monitor ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +endif() add_executable(moveit_kinematics_speed_and_validity_evaluator src/kinematics_speed_and_validity_evaluator.cpp) target_link_libraries(moveit_kinematics_speed_and_validity_evaluator moveit_robot_model_loader ${catkin_LIBRARIES} ${Boost_LIBRARIES}) From 384c231b143b73ada9a8ee0eb11cf22435309ef3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 20 Jan 2020 20:08:41 +0100 Subject: [PATCH 157/612] Travis: temporarily disable ros-shadow-fixed test See https://github.com/ros/rosdistro/pull/23469#issuecomment-576397569 for details. --- .travis.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 6ed584a5ff..8c83fc2ead 100644 --- a/.travis.yml +++ b/.travis.yml @@ -31,8 +31,7 @@ matrix: # Add a separate config to the matrix, using clang as compiler include: # add a config to the matrix using clang as compiler and also test ikfast plugin creation - compiler: clang - env: ROS_REPO=ros-shadow-fixed - TEST=clang-tidy-fix + env: TEST=clang-tidy-fix BEFORE_DOCKER_SCRIPT="source moveit_kinematics/test/test_ikfast_plugins.sh" CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-overloaded-virtual" From 546d6e052df1a92c0f3805f3c079c80fdc9916d0 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Mon, 20 Jan 2020 21:28:21 +0100 Subject: [PATCH 158/612] Fix command frame transform computation (#1842) * Fix JogArm command frame handling * Add comments to clarify frame transforms --- .../moveit_jog_arm/src/jog_calcs.cpp | 41 ++++++++++++------- 1 file changed, 27 insertions(+), 14 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index c5f6648a47..bf38f99f78 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -142,7 +142,10 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) original_joint_state_ = joint_state_; // Get the transform from MoveIt planning frame to jogging command frame - tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); + // We solve (planning_frame -> base -> robot_link_command_frame) + // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) + tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); mutex.lock(); shared_variables.tf_moveit_to_cmd_frame = tf_moveit_to_cmd_frame_; mutex.unlock(); @@ -266,19 +269,29 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& } // Transform the command to the MoveGroup planning frame - Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z); - Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z); - translation_vector = tf_moveit_to_cmd_frame_.linear() * translation_vector; - angular_vector = tf_moveit_to_cmd_frame_.linear() * angular_vector; - - // Put these components back into a TwistStamped - cmd.header.frame_id = parameters_.planning_frame; - cmd.twist.linear.x = translation_vector(0); - cmd.twist.linear.y = translation_vector(1); - cmd.twist.linear.z = translation_vector(2); - cmd.twist.angular.x = angular_vector(0); - cmd.twist.angular.y = angular_vector(1); - cmd.twist.angular.z = angular_vector(2); + if (cmd.header.frame_id != parameters_.planning_frame) + { + Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z); + Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z); + + // We solve (planning_frame -> base -> cmd.header.frame_id) + // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id) + const auto tf_planning_to_cmd_frame = + kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + kinematic_state_->getGlobalLinkTransform(cmd.header.frame_id); + + translation_vector = tf_planning_to_cmd_frame.linear() * translation_vector; + angular_vector = tf_planning_to_cmd_frame.linear() * angular_vector; + + // Put these components back into a TwistStamped + cmd.header.frame_id = parameters_.planning_frame; + cmd.twist.linear.x = translation_vector(0); + cmd.twist.linear.y = translation_vector(1); + cmd.twist.linear.z = translation_vector(2); + cmd.twist.angular.x = angular_vector(0); + cmd.twist.angular.y = angular_vector(1); + cmd.twist.angular.z = angular_vector(2); + } const Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); From 1e1f0fe2ea6178c0c07260721384e2ce1d672a62 Mon Sep 17 00:00:00 2001 From: llach Date: Mon, 13 Jan 2020 16:36:31 +0100 Subject: [PATCH 159/612] forward controller names to TrajectoryExecutionManager - ExecutableTrajectory has a new member controller_names_ - PlanExecution includes that list when pushing trajctory parts to the TrajectoryExecutionManager --- .../include/moveit/plan_execution/plan_representation.h | 9 +++++++-- .../planning/plan_execution/src/plan_execution.cpp | 2 +- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h index 809cb259c7..724414768f 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h @@ -52,8 +52,12 @@ struct ExecutableTrajectory { } - ExecutableTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, const std::string& description) - : trajectory_(trajectory), description_(description), trajectory_monitoring_(true) + ExecutableTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, const std::string& description, + std::vector controller_names = {}) + : trajectory_(trajectory) + , description_(description) + , trajectory_monitoring_(true) + , controller_names_(std::move(controller_names)) { } @@ -62,6 +66,7 @@ struct ExecutableTrajectory bool trajectory_monitoring_; collision_detection::AllowedCollisionMatrixConstPtr allowed_collision_matrix_; boost::function effect_on_success_; + std::vector controller_names_; }; /// A generic representation on what a computed motion plan looks like diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index f669e03af8..b282a34a9d 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -370,7 +370,7 @@ moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(E // convert to message, pass along moveit_msgs::RobotTrajectory msg; plan.plan_components_[i].trajectory_->getRobotTrajectoryMsg(msg); - if (!trajectory_execution_manager_->push(msg)) + if (!trajectory_execution_manager_->push(msg, plan.plan_components_[i].controller_names_)) { trajectory_execution_manager_->clear(); ROS_ERROR_STREAM_NAMED("plan_execution", "Apparently trajectory initialization failed"); From 590a1d2b1d21feea4c5c05026e888d159315a9fe Mon Sep 17 00:00:00 2001 From: llach Date: Mon, 13 Jan 2020 16:44:15 +0100 Subject: [PATCH 160/612] read "default" parameter in MoveitControllerManagers When reading controller configurations, "default" is ignored as opposed to what is currently written in the documentation. MoveIt{Fake|Simple}ControllerManager now maintains map of controller name and ControllerState where "default" is initialized as "false" or whatever is given in the configuration. MoveIt{Fake|Simple}ControllerManager::getControllerState() now returns the state that is held in that map instead of a default one. --- .../src/moveit_fake_controller_manager.cpp | 17 +++++++++-------- .../src/moveit_simple_controller_manager.cpp | 17 +++++++++-------- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp index f1fb8a7db4..64c28f82cc 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp @@ -115,6 +115,12 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont controllers_[name].reset(new InterpolatingController(name, joints, pub_)); else ROS_ERROR_STREAM("Unknown fake controller type: " << type); + + moveit_controller_manager::MoveItControllerManager::ControllerState state; + state.default_ = controller_list[i].hasMember("default") ? (bool)controller_list[i]["default"] : false; + state.active_ = true; + + controller_states_[name] = state; } catch (...) { @@ -258,16 +264,10 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont } } - /* - * Controllers are all active and default. - */ moveit_controller_manager::MoveItControllerManager::ControllerState - getControllerState(const std::string& /*name*/) override + getControllerState(const std::string& name) override { - moveit_controller_manager::MoveItControllerManager::ControllerState state; - state.active_ = true; - state.default_ = true; - return state; + return controller_states_[name]; } /* Cannot switch our controllers */ @@ -281,6 +281,7 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont ros::NodeHandle node_handle_; ros::Publisher pub_; std::map controllers_; + std::map controller_states_; }; } // end namespace moveit_fake_controller_manager diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp index 46c8a507f1..83d8a6f7b3 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp @@ -145,6 +145,12 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo continue; } + moveit_controller_manager::MoveItControllerManager::ControllerState state; + state.default_ = controller_list[i].hasMember("default") ? (bool)controller_list[i]["default"] : false; + state.active_ = true; + + controller_states_[name] = state; + /* add list of joints, used by controller manager and MoveIt */ for (int j = 0; j < controller_list[i]["joints"].size(); ++j) new_handle->addJoint(std::string(controller_list[i]["joints"][j])); @@ -220,16 +226,10 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo } } - /* - * Controllers are all active and default -- that's what makes this thing simple. - */ moveit_controller_manager::MoveItControllerManager::ControllerState - getControllerState(const std::string& /* name */) override + getControllerState(const std::string& name) override { - moveit_controller_manager::MoveItControllerManager::ControllerState state; - state.active_ = true; - state.default_ = true; - return state; + return controller_states_[name]; } /* Cannot switch our controllers */ @@ -242,6 +242,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo protected: ros::NodeHandle node_handle_; std::map controllers_; + std::map controller_states_; }; } // end namespace moveit_simple_controller_manager From 3a57141b48435de0a6026f9fc3496165c72e19e6 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Tue, 21 Jan 2020 09:51:36 -0500 Subject: [PATCH 161/612] Reduce console output warnings (#1845) * Reduce console output warnings * jog_arm: cleanup PSM setup * clang-format * fix cut-and-paste error Co-authored-by: Robert Haschke --- .../src/collision_check_thread.cpp | 18 ++++++++---------- .../ompl/ompl_interface/src/ompl_interface.cpp | 4 ++-- .../src/occupancy_map_monitor.cpp | 3 ++- .../moveit_cpp/src/moveit_cpp.cpp | 2 +- 4 files changed, 13 insertions(+), 14 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index de07e0a817..0335e88d8d 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -53,20 +53,18 @@ CollisionCheckThread::CollisionCheckThread(const moveit_jog_arm::JogArmParameter } planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(model_loader_ptr)); - planning_scene_monitor_->startSceneMonitor(); - planning_scene_monitor_->startStateMonitor(); - - if (planning_scene_monitor_->getPlanningScene()) - { - planning_scene_monitor_->startSceneMonitor("/planning_scene"); - planning_scene_monitor_->startWorldGeometryMonitor(); - planning_scene_monitor_->startStateMonitor(); - } - else + if (!planning_scene_monitor_->getPlanningScene()) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); exit(EXIT_FAILURE); } + + planning_scene_monitor_->startSceneMonitor(); + planning_scene_monitor_->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + planning_scene_monitor_->startStateMonitor(); } void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index 8f68566bd2..dc4ffc5371 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -51,7 +51,7 @@ ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstP , use_constraints_approximations_(true) , simplify_solutions_(true) { - ROS_INFO("Initializing OMPL interface using ROS parameters"); + ROS_DEBUG("Initializing OMPL interface using ROS parameters"); loadPlannerConfigurations(); loadConstraintSamplers(); } @@ -66,7 +66,7 @@ ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstP , use_constraints_approximations_(true) , simplify_solutions_(true) { - ROS_INFO("Initializing OMPL interface using specified configuration"); + ROS_DEBUG("Initializing OMPL interface using specified configuration"); setPlannerConfigurations(pconfig); loadConstraintSamplers(); } diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index 284f429bb7..482b317963 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -90,7 +90,8 @@ void OccupancyMapMonitor::initialize() ROS_WARN("No target frame specified for Octomap. No transforms will be applied to received data."); if (!tf_buffer_ && !map_frame_.empty()) - ROS_WARN("Target frame specified but no TF instance specified. No transforms will be applied to received data."); + ROS_WARN_STREAM("Target frame \"" << map_frame_ << "\" specified but no TF instance (buffer) specified. " + "No transforms will be applied to received data."); tree_.reset(new OccMapTree(map_resolution_)); tree_const_ = tree_; diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index 3c0da8bf5f..a53c5d5f80 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -94,7 +94,7 @@ MoveItCpp::MoveItCpp(const Options& options, const ros::NodeHandle& /*unused*/, trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager( robot_model_, planning_scene_monitor_->getStateMonitor())); - ROS_INFO_NAMED(LOGNAME, "MoveItCpp running"); + ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp running"); } MoveItCpp::MoveItCpp(MoveItCpp&& other) From 92032b5167f78f12550ff3d48d3b75afca29461f Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 22 Jan 2020 06:33:24 -0500 Subject: [PATCH 162/612] Move attribution below license file, standardize with MoveIt (#1847) --- .../moveit_jog_arm/src/collision_check_thread.cpp | 11 ++++++----- moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp | 11 ++++++----- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 11 ++++++----- .../moveit_jog_arm/src/jog_interface_base.cpp | 11 ++++++----- .../moveit_jog_arm/src/jog_ros_interface.cpp | 11 ++++++----- moveit_experimental/moveit_jog_arm/src/jog_server.cpp | 11 ++++++----- .../moveit_jog_arm/src/low_pass_filter.cpp | 11 ++++++----- 7 files changed, 42 insertions(+), 35 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 0335e88d8d..27f22d8137 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -1,9 +1,4 @@ /******************************************************************************* - * Title : collision_check_thread.cpp - * Project : moveit_jog_arm - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -36,6 +31,12 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ +/* Title : collision_check_thread.cpp + * Project : moveit_jog_arm + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + */ + #include namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index bf38f99f78..ff73fc3fd0 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -1,9 +1,4 @@ /******************************************************************************* - * Title : jog_calcs.cpp - * Project : moveit_jog_arm - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -36,6 +31,12 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ +/* Title : jog_calcs.cpp + * Project : moveit_jog_arm + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + */ + #include namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp index b964b61e2d..4da4f70ba9 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp @@ -1,9 +1,4 @@ /******************************************************************************* - * Title : jog_cpp_interface.cpp - * Project : moveit_jog_arm - * Created : 11/20/2019 - * Author : Andy Zelenak - * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -36,6 +31,12 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ +/* Title : jog_cpp_interface.cpp + * Project : moveit_jog_arm + * Created : 11/20/2019 + * Author : Andy Zelenak + */ + #include "moveit_jog_arm/jog_cpp_interface.h" namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index 1238a4043f..2708788b46 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -1,9 +1,4 @@ /******************************************************************************* - * Title : jog_interface_base.cpp - * Project : moveit_jog_arm - * Created : 3/9/2017 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -36,6 +31,12 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ +/* Title : jog_interface_base.cpp + * Project : moveit_jog_arm + * Created : 3/9/2017 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + */ + #include "moveit_jog_arm/jog_interface_base.h" namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp index bf001ab3e6..a6fc001d78 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp @@ -1,9 +1,4 @@ /******************************************************************************* - * Title : jog_ros_interface.cpp - * Project : moveit_jog_arm - * Created : 3/9/2017 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -36,6 +31,12 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ +/* Title : jog_ros_interface.cpp + * Project : moveit_jog_arm + * Created : 3/9/2017 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + */ + // Server node for arm jogging with MoveIt. #include diff --git a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp index 9202c3dd00..ee49feee62 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp @@ -1,9 +1,4 @@ /******************************************************************************* - * Title : jog_server.cpp - * Project : moveit_jog_arm - * Created : 12/31/2018 - * Author : Andy Zelenak - * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -36,6 +31,12 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ +/* Title : jog_server.cpp + * Project : moveit_jog_arm + * Created : 12/31/2018 + * Author : Andy Zelenak + */ + #include int main(int argc, char** argv) diff --git a/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp b/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp index 5071d010c5..8170474b40 100644 --- a/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp +++ b/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp @@ -1,9 +1,4 @@ /******************************************************************************* - * Title : low_pass_filter.cpp - * Project : moveit_jog_arm - * Created : 1/11/2019 - * Author : Andy Zelenak - * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -36,6 +31,12 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ +/* Title : low_pass_filter.cpp + * Project : moveit_jog_arm + * Created : 1/11/2019 + * Author : Andy Zelenak + */ + #include namespace moveit_jog_arm From 27e02032277e2b900558b69defbc2976d7c52c1e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 22 Jan 2020 14:02:43 +0100 Subject: [PATCH 163/612] Revert "Travis: temporarily disable ros-shadow-fixed test" (#1851) This reverts commit 384c231b143b73ada9a8ee0eb11cf22435309ef3. --- .travis.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 8c83fc2ead..6ed584a5ff 100644 --- a/.travis.yml +++ b/.travis.yml @@ -31,7 +31,8 @@ matrix: # Add a separate config to the matrix, using clang as compiler include: # add a config to the matrix using clang as compiler and also test ikfast plugin creation - compiler: clang - env: TEST=clang-tidy-fix + env: ROS_REPO=ros-shadow-fixed + TEST=clang-tidy-fix BEFORE_DOCKER_SCRIPT="source moveit_kinematics/test/test_ikfast_plugins.sh" CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-overloaded-virtual" From cd16a503350848df545735d404abda8ad186cf49 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 22 Jan 2020 13:23:44 -0500 Subject: [PATCH 164/612] [jog_arm] Pass planning scene monitor into cpp interface (#1849) * [jog_arm] Pass planning scene monitor into cpp interface - Separate LOGNAME into each file - Add unit test * Add PR feedback --- .../moveit_jog_arm/CMakeLists.txt | 22 ++-- .../moveit_jog_arm/collision_check_thread.h | 11 +- .../include/moveit_jog_arm/jog_arm_data.h | 1 - .../moveit_jog_arm/jog_cpp_interface.h | 4 +- .../moveit_jog_arm/jog_interface_base.h | 5 +- .../moveit_jog_arm/jog_ros_interface.h | 2 +- .../src/collision_check_thread.cpp | 42 +++----- .../cpp_interface_example.cpp | 22 +++- .../moveit_jog_arm/src/jog_calcs.cpp | 2 + .../moveit_jog_arm/src/jog_cpp_interface.cpp | 11 +- .../moveit_jog_arm/src/jog_interface_base.cpp | 10 +- .../moveit_jog_arm/src/jog_ros_interface.cpp | 18 +++- .../moveit_jog_arm/src/jog_server.cpp | 4 +- .../moveit_jog_arm/src/low_pass_filter.cpp | 3 + .../test/jog_cpp_interface_test.cpp | 101 ++++++++++++++++++ .../test/jog_cpp_interface_test.test | 12 +++ .../test/launch/jog_arm_integration_test.test | 9 +- 17 files changed, 223 insertions(+), 56 deletions(-) create mode 100644 moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp create mode 100644 moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index 8130c87c6a..439ea9ee85 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -7,8 +7,6 @@ set(CMAKE_CXX_EXTENSIONS OFF) set(LIBRARY_NAME jog_arm_cpp_api) -set(LIBRARY_NAME jog_arm_cpp_api) - if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() @@ -66,10 +64,7 @@ target_link_libraries(${LIBRARY_NAME} # An example of using the C++ library add_executable(cpp_interface_example - src/collision_check_thread.cpp src/cpp_interface_example/cpp_interface_example.cpp - src/jog_calcs.cpp - src/low_pass_filter.cpp ) add_dependencies(cpp_interface_example ${catkin_EXPORTED_TARGETS}) target_link_libraries(cpp_interface_example @@ -125,12 +120,21 @@ install(DIRECTORY include/${PROJECT_NAME}/ ## TESTING ## ############# -############# -## TESTING ## -############# - if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) find_package(ros_pytest REQUIRED) + + # jog_server add_rostest(test/launch/jog_arm_integration_test.test) + + # jog_cpp_interface + add_rostest_gtest(jog_cpp_interface_test + test/jog_cpp_interface_test.test + test/jog_cpp_interface_test.cpp + ) + target_link_libraries(jog_cpp_interface_test + ${LIBRARY_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ) endif() diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index 73ddd1780c..b413501cd7 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -49,8 +49,16 @@ namespace moveit_jog_arm class CollisionCheckThread { public: + /** \brief Constructor + * \param parameters: common settings of jog_arm + * \param planning_scene_monitor: PSM should already have scene monitor and state monitor started when passed into + * this class + */ CollisionCheckThread(const moveit_jog_arm::JogArmParameters& parameters, - const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + + // Get thread-safe read only lock of planning scene + planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; void startMainLoop(moveit_jog_arm::JogArmShared& shared_variables, std::mutex& mutex); @@ -62,6 +70,7 @@ class CollisionCheckThread const moveit_jog_arm::JogArmParameters parameters_; + // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; }; } diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index c96af15726..9a890df1ec 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -48,7 +48,6 @@ namespace moveit_jog_arm { -static const std::string LOGNAME = "jog_server"; static const double WHILE_LOOP_WAIT = 0.001; // Variables to share between threads, and their mutexes diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h index 4b174a04f4..e9a070e178 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h @@ -47,10 +47,10 @@ namespace moveit_jog_arm * Class JogCppApi - This class should be instantiated in a new thread * See cpp_interface_example.cpp */ -class JogCppApi : JogInterfaceBase +class JogCppApi : protected JogInterfaceBase { public: - JogCppApi(); + JogCppApi(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); ~JogCppApi(); diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h index ed186be292..32cbc66893 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h @@ -55,6 +55,8 @@ namespace moveit_jog_arm class JogInterfaceBase { public: + JogInterfaceBase(); + void jointsCB(const sensor_msgs::JointStateConstPtr& msg); // Jogging calculation thread @@ -68,7 +70,8 @@ class JogInterfaceBase protected: bool readParameters(ros::NodeHandle& n); - robot_model_loader::RobotModelLoaderPtr model_loader_ptr_; + // Pointer to the collision environment + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; // Store the parameters that were read from ROS server JogArmParameters ros_parameters_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h index f840fc3a99..7afe5029f5 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h @@ -47,7 +47,7 @@ namespace moveit_jog_arm /** * Class JogROSInterface - Instantiated in main(). Handles ROS subs & pubs and creates the worker threads. */ -class JogROSInterface : JogInterfaceBase +class JogROSInterface : protected JogInterfaceBase { public: JogROSInterface(); diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 27f22d8137..2b7f3f644f 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -39,33 +39,21 @@ #include +static const std::string LOGNAME = "collision_check_thread"; + namespace moveit_jog_arm { // Constructor for the class that handles collision checking -CollisionCheckThread::CollisionCheckThread(const moveit_jog_arm::JogArmParameters& parameters, - const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) - : parameters_(parameters) +CollisionCheckThread::CollisionCheckThread( + const moveit_jog_arm::JogArmParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : parameters_(parameters), planning_scene_monitor_(planning_scene_monitor) { - // MoveIt Setup - while (ros::ok() && !model_loader_ptr) - { - ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); - ros::Duration(WHILE_LOOP_WAIT).sleep(); - } - - planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(model_loader_ptr)); - if (!planning_scene_monitor_->getPlanningScene()) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } +} - planning_scene_monitor_->startSceneMonitor(); - planning_scene_monitor_->startWorldGeometryMonitor( - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, - false /* skip octomap monitor */); - planning_scene_monitor_->startStateMonitor(); +planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPlanningSceneRO() const +{ + return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); } void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) @@ -73,15 +61,16 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables, std::mu // Reset loop termination flag stop_requested_ = false; - // Init collision request and current state + // Init collision request collision_detection::CollisionRequest collision_request; collision_request.group_name = parameters_.move_group_name; collision_request.distance = true; collision_detection::CollisionResult collision_result; - robot_state::RobotState& current_state = planning_scene_monitor_->getPlanningScene()->getCurrentStateNonConst(); - double velocity_scale_coefficient = -log(0.001) / parameters_.collision_proximity_threshold; + // Copy the planning scene's version of current state into new memory + robot_state::RobotState current_state(getLockedPlanningSceneRO()->getCurrentState()); + double velocity_scale_coefficient = -log(0.001) / parameters_.collision_proximity_threshold; ros::Rate collision_rate(parameters_.collision_check_rate); ///////////////////////////////////////////////// @@ -98,7 +87,8 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables, std::mu collision_result.clear(); current_state.updateCollisionBodyTransforms(); - planning_scene_monitor_->getPlanningScene()->checkCollision(collision_request, collision_result, current_state); + // Do thread-safe collsion collision checking + getLockedPlanningSceneRO()->checkCollision(collision_request, collision_result, current_state); // Scale robot velocity according to collision proximity and user-defined thresholds. // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index d51e5c9dcc..5ccd8cedb2 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -38,6 +38,8 @@ #include +static const std::string LOGNAME = "cpp_interface_example"; + /** * Instantiate the C++ jogging interface. * Send some Cartesian commands, then some joint commands. @@ -45,10 +47,26 @@ */ int main(int argc, char** argv) { - ros::init(argc, argv, moveit_jog_arm::LOGNAME); + ros::init(argc, argv, LOGNAME); + + // Load the planning scene monitor + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; + planning_scene_monitor = std::make_shared("robot_description"); + if (!planning_scene_monitor->getPlanningScene()) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); + exit(EXIT_FAILURE); + } + + planning_scene_monitor->startSceneMonitor(); + planning_scene_monitor->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + planning_scene_monitor->startStateMonitor(); // Run the jogging C++ interface in a new thread to ensure a constant outgoing message rate. - moveit_jog_arm::JogCppApi jog_interface; + moveit_jog_arm::JogCppApi jog_interface(planning_scene_monitor); std::thread jogging_thread([&]() { jog_interface.startMainLoop(); }); // Make a Cartesian velocity message diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index ff73fc3fd0..af403d9834 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -39,6 +39,8 @@ #include +static const std::string LOGNAME = "jog_calcs"; + namespace moveit_jog_arm { // Constructor for the class that handles jogging calculations diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp index 4da4f70ba9..71f6aeb39c 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp @@ -39,16 +39,19 @@ #include "moveit_jog_arm/jog_cpp_interface.h" +// TODO(davetcoleman): rename JogCppApi to JogCppInterface to match file name + +static const std::string LOGNAME = "jog_cpp_interface"; + namespace moveit_jog_arm { -JogCppApi::JogCppApi() +JogCppApi::JogCppApi(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) { + planning_scene_monitor_ = planning_scene_monitor; + // Read ROS parameters, typically from YAML file if (!readParameters(nh_)) exit(EXIT_FAILURE); - - // Load the robot model. This is used by the worker threads. - model_loader_ptr_ = std::shared_ptr(new robot_model_loader::RobotModelLoader); } JogCppApi::~JogCppApi() diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index 2708788b46..676f9597a5 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -39,8 +39,14 @@ #include "moveit_jog_arm/jog_interface_base.h" +static const std::string LOGNAME = "jog_interface_base"; + namespace moveit_jog_arm { +JogInterfaceBase::JogInterfaceBase() +{ +} + // Read ROS parameters, typically from YAML file bool JogInterfaceBase::readParameters(ros::NodeHandle& n) { @@ -192,7 +198,7 @@ void JogInterfaceBase::jointsCB(const sensor_msgs::JointStateConstPtr& msg) bool JogInterfaceBase::startJogCalcThread() { if (!jog_calcs_) - jog_calcs_.reset(new JogCalcs(ros_parameters_, model_loader_ptr_)); + jog_calcs_.reset(new JogCalcs(ros_parameters_, planning_scene_monitor_->getRobotModelLoader())); jog_calc_thread_.reset( new std::thread([&]() { jog_calcs_->startMainLoop(shared_variables_, shared_variables_mutex_); })); @@ -220,7 +226,7 @@ bool JogInterfaceBase::stopJogCalcThread() bool JogInterfaceBase::startCollisionCheckThread() { if (!collision_checker_) - collision_checker_.reset(new CollisionCheckThread(ros_parameters_, model_loader_ptr_)); + collision_checker_.reset(new CollisionCheckThread(ros_parameters_, planning_scene_monitor_)); collision_check_thread_.reset( new std::thread([&]() { collision_checker_->startMainLoop(shared_variables_, shared_variables_mutex_); })); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp index a6fc001d78..9caaeaf802 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp @@ -41,6 +41,8 @@ #include +static const std::string LOGNAME = "jog_ros_interface"; + namespace moveit_jog_arm { ///////////////////////////////////////////////////////////////////////////////// @@ -58,8 +60,20 @@ JogROSInterface::JogROSInterface() if (!readParameters(nh)) exit(EXIT_FAILURE); - // Load the robot model. This is used by the worker threads. - model_loader_ptr_ = std::shared_ptr(new robot_model_loader::RobotModelLoader); + // Load the planning scene monitor + planning_scene_monitor_ = std::make_shared("robot_description"); + if (!planning_scene_monitor_->getPlanningScene()) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); + exit(EXIT_FAILURE); + } + + planning_scene_monitor_->startSceneMonitor(); + planning_scene_monitor_->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + planning_scene_monitor_->startStateMonitor(); // Crunch the numbers in this thread startJogCalcThread(); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp index ee49feee62..f258211f45 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp @@ -39,9 +39,11 @@ #include +static const std::string LOGNAME = "jog_server"; + int main(int argc, char** argv) { - ros::init(argc, argv, moveit_jog_arm::LOGNAME); + ros::init(argc, argv, LOGNAME); moveit_jog_arm::JogROSInterface ros_interface; diff --git a/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp b/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp index 8170474b40..536d75deeb 100644 --- a/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp +++ b/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp @@ -38,6 +38,9 @@ */ #include +#include + +static const std::string LOGNAME = "low_pass_filter"; namespace moveit_jog_arm { diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp new file mode 100644 index 0000000000..b1cb6b9a80 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp @@ -0,0 +1,101 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik LLC + * 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 PickNik LLC 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. + *********************************************************************/ + +/* Author: Dave Coleman + Desc: Stub test for the C++ interface to JogArm +*/ + +// C++ +#include + +// ROS +#include + +// Testing +#include + +// Main class +#include + +static const std::string LOGNAME = "jog_cpp_interface_test"; + +namespace moveit_jog_arm +{ +class TestJogCppInterface : public ::testing::Test +{ +public: + void SetUp() override + { + nh_.reset(new ros::NodeHandle("~")); + + // Load the planning scene monitor + planning_scene_monitor_ = std::make_shared("robot_description"); + planning_scene_monitor_->startSceneMonitor(); + planning_scene_monitor_->startStateMonitor(); + } + void TearDown() override + { + } + +protected: + std::unique_ptr nh_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; +}; // class TestJogCppInterface + +TEST_F(TestJogCppInterface, InitTest) +{ + moveit_jog_arm::JogCppApi jog_cpp_interface(planning_scene_monitor_); + ros::Duration(2).sleep(); +} + +// TODO(davetcoleman): due to many blocking checks for ROS messages, and +// an abundance of threads, unit tests are not currently feasible for the +// cpp interface. This should be addressed in future re-factors + +} // namespace moveit_jog_arm + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "jog_cpp_interface_test_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + int result = RUN_ALL_TESTS(); + + spinner.stop(); + ros::shutdown(); + return result; +} diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test new file mode 100644 index 0000000000..e0fc2aaa85 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_integration_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_integration_test.test index 4cd7ccb4c2..fbe629e8b6 100644 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_integration_test.test +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_integration_test.test @@ -2,12 +2,13 @@ - - + + + + From 2a855950318d06631a8d439f8aa8ed3ca090f98d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 22 Jan 2020 11:40:42 -0700 Subject: [PATCH 165/612] [jog_arm] Remove scale/joint parameter (#1838) * Remove the velocity check with the scale/joint parameter Add a unit test for direct joint jogging commands Add a check that publish_period > 0 * Clang format --- .../config/ur_simulated_config.yaml | 8 ++-- .../include/moveit_jog_arm/jog_arm_data.h | 32 ++++++++++--- .../include/moveit_jog_arm/jog_calcs.h | 9 ++-- .../moveit_jog_arm/src/jog_calcs.cpp | 24 ++-------- .../moveit_jog_arm/src/jog_interface_base.cpp | 6 +++ .../test/arm_setup/config/jog_settings.yaml | 8 ++-- .../integration/test_jog_arm_integration.py | 48 ++++++++++++++----- 7 files changed, 86 insertions(+), 49 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml index 67a383643e..396d8a2741 100644 --- a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml +++ b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml @@ -7,9 +7,11 @@ use_gazebo: true # Whether the robot is started in a Gazebo simulation environme robot_link_command_frame: world # commands must be given in the frame of a robot link. Usually either the base or end effector command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: - linear: 0.6 # Max linear velocity. Meters per publish_period. Units is [m/s]. Only used if command_in_type=="unitless" - rotational: 0.3 # Max angular velocity. Rads per publish_period. Units is [rad/s]. Only used if command_in_type=="unitless" - joint: 0.3 # Max joint angular/linear velocity. Rads or Meters per publish period. Units is [rad/s] or [m/s]. + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.6 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 0.01 low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less. ## Properties of outgoing commands diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 9a890df1ec..0e21bd74ac 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -86,12 +86,32 @@ struct JogArmShared // ROS params to be read. See the yaml file in /config for a description of each. struct JogArmParameters { - std::string move_group_name, joint_topic, cartesian_command_in_topic, robot_link_command_frame, command_out_topic, - planning_frame, warning_topic, joint_command_in_topic, command_in_type, command_out_type; - double linear_scale, rotational_scale, joint_scale, lower_singularity_threshold, hard_stop_singularity_threshold, - collision_proximity_threshold, low_pass_filter_coeff, publish_period, incoming_command_timeout, - joint_limit_margin, collision_check_rate; + std::string move_group_name; + std::string joint_topic; + std::string cartesian_command_in_topic; + std::string robot_link_command_frame; + std::string command_out_topic; + std::string planning_frame; + std::string warning_topic; + std::string joint_command_in_topic; + std::string command_in_type; + std::string command_out_type; + double linear_scale; + double rotational_scale; + double joint_scale; + double lower_singularity_threshold; + double hard_stop_singularity_threshold; + double collision_proximity_threshold; + double low_pass_filter_coeff; + double publish_period; + double incoming_command_timeout; + double joint_limit_margin; + double collision_check_rate; int num_outgoing_halt_msgs_to_publish; - bool use_gazebo, check_collisions, publish_joint_positions, publish_joint_velocities, publish_joint_accelerations; + bool use_gazebo; + bool check_collisions; + bool publish_joint_positions; + bool publish_joint_velocities; + bool publish_joint_accelerations; }; } diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 058aec32d9..b98775e0bb 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -77,9 +77,6 @@ class JogCalcs bool addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const; - // Scale the delta theta to match joint velocity limits. Uniform scaling - void enforceJointVelocityLimits(Eigen::VectorXd& calculated_joint_velocity); - // Reset the data stored in low-pass filters so the trajectory won't jump when jogging is resumed. void resetVelocityFilters(); @@ -89,7 +86,8 @@ class JogCalcs void publishWarning(bool active) const; - bool checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory_>& new_joint_traj); + // Scale the delta theta to match joint velocity limits + bool checkIfJointsWithinSRDFBounds(trajectory_msgs::JointTrajectory& new_joint_traj); // Possibly calculate a velocity scaling factor, due to proximity of // singularity and direction of motion @@ -127,7 +125,8 @@ class JogCalcs // For jacobian calculations Eigen::MatrixXd jacobian_, pseudo_inverse_, matrix_s_; Eigen::JacobiSVD svd_; - Eigen::VectorXd delta_theta_; + // Use ArrayXd type to enable more coefficient-wise operations + Eigen::ArrayXd delta_theta_; Eigen::Isometry3d tf_moveit_to_cmd_frame_; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index af403d9834..440aab95df 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -305,8 +305,6 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose(); delta_theta_ = pseudo_inverse_ * delta_x; - enforceJointVelocityLimits(delta_theta_); - if (!addJointIncrements(joint_state_, delta_theta_)) return false; @@ -322,7 +320,7 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& applyVelocityScaling(shared_variables, mutex, outgoing_command_, delta_theta_, decelerateForSingularity(delta_x, svd_)); - if (!checkIfJointsWithinURDFBounds(outgoing_command_)) + if (!checkIfJointsWithinSRDFBounds(outgoing_command_)) { suddenHalt(outgoing_command_); publishWarning(true); @@ -352,7 +350,7 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& /* } // Apply user-defined scaling - const Eigen::VectorXd delta = scaleJointCommand(cmd); + Eigen::ArrayXd delta = scaleJointCommand(cmd); kinematic_state_->setVariableValues(joint_state_); original_joint_state_ = joint_state_; @@ -371,8 +369,7 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& /* outgoing_command_ = composeOutgoingMessage(joint_state_); - // check if new joint state is valid - if (!checkIfJointsWithinURDFBounds(outgoing_command_)) + if (!checkIfJointsWithinSRDFBounds(outgoing_command_)) { suddenHalt(outgoing_command_); publishWarning(true); @@ -557,19 +554,7 @@ double JogCalcs::decelerateForSingularity(const Eigen::VectorXd& commanded_veloc return velocity_scale; } -void JogCalcs::enforceJointVelocityLimits(Eigen::VectorXd& calculated_joint_change) -{ - double maximum_joint_change = calculated_joint_change.cwiseAbs().maxCoeff(); - if (maximum_joint_change > parameters_.joint_scale * parameters_.publish_period) - { - // Scale the entire joint velocity vector so that each joint velocity is below min, and the output movement is - // scaled uniformly to match expected motion - calculated_joint_change = - calculated_joint_change * parameters_.joint_scale * parameters_.publish_period / maximum_joint_change; - } -} - -bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& new_joint_traj) +bool JogCalcs::checkIfJointsWithinSRDFBounds(trajectory_msgs::JointTrajectory& new_joint_traj) { bool halting = false; @@ -622,7 +607,6 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n } } } - return !halting; } diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index 676f9597a5..77c5f0f583 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -108,6 +108,12 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) rosparam_shortcuts::shutdownIfError(parameter_ns, error); // Input checking + if (ros_parameters_.publish_period <= 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'publish_period' should be " + "greater than zero. Check yaml file."); + return false; + } if (ros_parameters_.num_outgoing_halt_msgs_to_publish < 0) { ROS_WARN_NAMED(LOGNAME, diff --git a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml index dc2294d889..6907b090f7 100644 --- a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml +++ b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml @@ -7,9 +7,11 @@ use_gazebo: false # Whether the robot is started in a Gazebo simulation environm robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: - linear: 0.003 # Max linear velocity. Meters per publish_period. Units is [m/s]. Only used if command_in_type=="unitless" - rotational: 0.006 # Max angular velocity. Rads per publish_period. Units is [rad/s]. Only used if command_in_type=="unitless" - joint: 0.01 # Max joint angular/linear velocity. Rads or Meters per publish period. Units is [rad/s] or [m/s]. + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.003 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.006 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 0.01 low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less. ## Properties of outgoing commands diff --git a/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py b/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py index 833cad8ea4..fe1a0323a1 100755 --- a/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py +++ b/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py @@ -29,11 +29,11 @@ class JointJogCmd(object): def __init__(self): self._pub = rospy.Publisher(JOINT_JOG_COMMAND_TOPIC, JointJog, queue_size=1) - def send_cmd(self, joint_pos): + def send_joint_velocity_cmd(self, joint_pos): jj = JointJog() jj.header.stamp = rospy.Time.now() jj.joint_names = ['joint_{}'.format(i) for i in range(len(joint_pos))] - jj.deltas = list(map(float, joint_pos)) + jj.velocities = list(map(float, joint_pos)) self._pub.publish(jj) @@ -51,11 +51,10 @@ def send_cmd(self, linear, angular): self._pub.publish(ts) -def test_jog_arm_generates_joint_trajectory_when_joint_jog_command_is_received(node): +def test_jog_arm_trajectory_generated_when_jog_command_is_received(node): sub = rospy.Subscriber( - COMMAND_OUT_TOPIC, JointTrajectory, lambda x: received.append(x) + COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) - joint_cmd = JointJogCmd() cartesian_cmd = CartesianJogCmd() time.sleep(ROS_SETTLE_TIME_S) # wait for pub/subs to settle time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for jog_arm server to init @@ -72,17 +71,42 @@ def test_jog_arm_generates_joint_trajectory_when_joint_jog_command_is_received(n # This nonzero command should produce jogging output # A subscriber in a different thread fills `received` - test_duration = 1 - publish_period = 0.01 # 'publish_period' from config file + TEST_DURATION = 1 + PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from config file cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) received = [] - rospy.sleep(test_duration) - # test_duration/publish_period is the expected number of messages in this duration. + rospy.sleep(TEST_DURATION) + # TEST_DURATION/PUBLISH_PERIOD is the expected number of messages in this duration. + # Allow a small +/- window due to rounding/timing errors + assert len(received) >= TEST_DURATION/PUBLISH_PERIOD - 5 + assert len(received) <= TEST_DURATION/PUBLISH_PERIOD + 5 + + +def test_jog_arm_joint_command(node): + # Test sending a joint command + + sub = rospy.Subscriber( + COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) + ) + + joint_cmd = JointJogCmd() + time.sleep(ROS_SETTLE_TIME_S) # wait for pub/subs to settle + time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for jog_arm server to init + + received = [] + TEST_DURATION = 1 + PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from config file + JOINT_VELOCITY_LIMIT = 1 + velocities = [0.1] + joint_cmd.send_joint_velocity_cmd(velocities) + rospy.sleep(TEST_DURATION) + # TEST_DURATION/PUBLISH_PERIOD is the expected number of messages in this duration. # Allow a small +/- window due to rounding/timing errors - assert len(received) >= test_duration/publish_period - 5 - assert len(received) <= test_duration/publish_period + 5 + assert len(received) >= TEST_DURATION/PUBLISH_PERIOD - 5 + assert len(received) <= TEST_DURATION/PUBLISH_PERIOD + 5 if __name__ == '__main__': node = node() - test_jog_arm_generates_joint_trajectory_when_joint_jog_command_is_received(node) + test_jog_arm_trajectory_generated_when_jog_command_is_received(node) + test_jog_arm_joint_command(node) From 75104b0d49a755f635af0867b5668dcbceae8fc9 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 22 Jan 2020 11:41:57 -0700 Subject: [PATCH 166/612] jog_arm: prevent a crash at velocity limit (#1837) * Prevent crash when a velocity limit is exceeded * Add another condition. * Move points.size() check to top of function * Move traj size check to top of function. * Move traj size check again * Clang tidy --- .../moveit_jog_arm/src/jog_calcs.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 440aab95df..3523900ba3 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -558,6 +558,12 @@ bool JogCalcs::checkIfJointsWithinSRDFBounds(trajectory_msgs::JointTrajectory& n { bool halting = false; + if (!new_joint_traj.points.empty()) + { + ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "Empty trajectory passed into checkIfJointsWithinURDFBounds()."); + return true; // technically an empty trajectory is still within bounds + } + for (auto joint : joint_model_group_->getJointModels()) { if (!kinematic_state_->satisfiesVelocityBounds(joint)) @@ -565,14 +571,19 @@ bool JogCalcs::checkIfJointsWithinSRDFBounds(trajectory_msgs::JointTrajectory& n ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, ros::this_node::getName() << " " << joint->getName() << " " << " close to a " " velocity limit. Enforcing limit."); - kinematic_state_->enforceVelocityBounds(joint); for (std::size_t c = 0; c < new_joint_traj.joint_names.size(); ++c) { if (new_joint_traj.joint_names[c] == joint->getName()) { - new_joint_traj.points[0].velocities[c] = kinematic_state_->getJointVelocities(joint)[0]; - break; + // TODO(andyz): This is caused by publishing in position mode -- which does not initialize the velocity + // members. + // TODO(andyz): Also need to adjust the joint positions that would be published. + if (new_joint_traj.points[0].velocities.size() > c + 1) + { + new_joint_traj.points[0].velocities[c] = *(kinematic_state_->getJointVelocities(joint)); + break; + } } } } From 577a38e41100877874caa5dadbb08fec65e8aad4 Mon Sep 17 00:00:00 2001 From: adornbush <50332900+adornbush@users.noreply.github.com> Date: Wed, 22 Jan 2020 13:47:18 -0500 Subject: [PATCH 167/612] add license headers to bfs3d source files (#1804) Co-authored-by: aurone --- .../core/sbpl_interface/src/bfs3d/BFS_3D.cpp | 34 +++++++++++++++++++ .../core/sbpl_interface/src/bfs3d/Search.cpp | 34 +++++++++++++++++++ 2 files changed, 68 insertions(+) diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp index 9fc8f84eb6..29f81140c1 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp @@ -1,3 +1,37 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Maxim Likhachev + * 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 Maxim Likhachev 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 namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/Search.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/Search.cpp index b446977e8f..9acff406cf 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/Search.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/Search.cpp @@ -1,3 +1,37 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Maxim Likhachev + * 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 Maxim Likhachev 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 From 1389df26358746d123306915fe1751616d09dc34 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Tue, 21 Jan 2020 14:25:27 -0500 Subject: [PATCH 168/612] [jog_arm] Improve formatting in comments --- .../include/moveit_jog_arm/collision_check_thread.h | 6 +++--- .../moveit_jog_arm/test/jog_cpp_interface_test.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index b413501cd7..cd6d98f9c2 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -51,13 +51,13 @@ class CollisionCheckThread public: /** \brief Constructor * \param parameters: common settings of jog_arm - * \param planning_scene_monitor: PSM should already have scene monitor and state monitor started when passed into - * this class + * \param planning_scene_monitor: PSM should have scene monitor and state monitor + * already started when passed into this class */ CollisionCheckThread(const moveit_jog_arm::JogArmParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - // Get thread-safe read only lock of planning scene + // Get thread-safe read-only lock of planning scene planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; void startMainLoop(moveit_jog_arm::JogArmShared& shared_variables, std::mutex& mutex); diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp index b1cb6b9a80..5247c5091b 100644 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp @@ -76,7 +76,7 @@ class TestJogCppInterface : public ::testing::Test TEST_F(TestJogCppInterface, InitTest) { moveit_jog_arm::JogCppApi jog_cpp_interface(planning_scene_monitor_); - ros::Duration(2).sleep(); + ros::Duration(1).sleep(); // Give the started thread some time to run } // TODO(davetcoleman): due to many blocking checks for ROS messages, and From 38fa392ae4eb694d2c7a04ed970978456e83482f Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 22 Jan 2020 17:06:25 -0500 Subject: [PATCH 169/612] Improve RobotState documentation (#1846) --- .../include/moveit/robot_state/robot_state.h | 138 +++++++++--------- moveit_core/robot_state/src/robot_state.cpp | 10 +- 2 files changed, 78 insertions(+), 70 deletions(-) diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 43789a908d..ecd5a6c3d0 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -48,6 +48,13 @@ #include +/* Terminology + * Model Frame: RobotModel's root frame == PlanningScene's planning frame + If the SRDF defines a virtual, non-fixed (e.g. floating) joint, this is the parent of this virtual joint. + Otherwise, it is the root link of the URDF model. + * Dirty Link Transforms: a caching tool for reducing the frequency of calculating forward kinematics +*/ + namespace moveit { namespace core @@ -572,8 +579,7 @@ class RobotState */ /** \brief Given positions for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const std::string& joint_group_name, const double* gstate) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -582,8 +588,7 @@ as the new values that correspond to the group */ } /** \brief Given positions for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const std::string& joint_group_name, const std::vector& gstate) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -592,21 +597,18 @@ as the new values that correspond to the group */ } /** \brief Given positions for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const JointModelGroup* group, const std::vector& gstate) { setJointGroupPositions(group, &gstate[0]); } /** \brief Given positions for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const JointModelGroup* group, const double* gstate); /** \brief Given positions for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -615,8 +617,7 @@ as the new values that correspond to the group */ } /** \brief Given positions for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values); /** \brief For a given group, copy the position values of the variables that make up the group into another location, @@ -678,8 +679,7 @@ as the new values that correspond to the group */ */ /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -688,8 +688,7 @@ as the new values that correspond to the group */ } /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupVelocities(const std::string& joint_group_name, const std::vector& gstate) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -698,21 +697,18 @@ as the new values that correspond to the group */ } /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupVelocities(const JointModelGroup* group, const std::vector& gstate) { setJointGroupVelocities(group, &gstate[0]); } /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupVelocities(const JointModelGroup* group, const double* gstate); /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -721,8 +717,7 @@ as the new values that correspond to the group */ } /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values); /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, @@ -784,8 +779,7 @@ as the new values that correspond to the group */ */ /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including -values of mimic joints), set those -as the new values that correspond to the group */ + * values of mimic joints), set those as the new values that correspond to the group */ void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -794,8 +788,7 @@ as the new values that correspond to the group */ } /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including -values of mimic joints), set those -as the new values that correspond to the group */ + * values of mimic joints), set those as the new values that correspond to the group */ void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector& gstate) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -804,21 +797,18 @@ as the new values that correspond to the group */ } /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including -values of mimic joints), set those -as the new values that correspond to the group */ + * values of mimic joints), set those as the new values that correspond to the group */ void setJointGroupAccelerations(const JointModelGroup* group, const std::vector& gstate) { setJointGroupAccelerations(group, &gstate[0]); } /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including -values of mimic joints), set those -as the new values that correspond to the group */ + * values of mimic joints), set those as the new values that correspond to the group */ void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate); /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including -values of mimic joints), set those -as the new values that correspond to the group */ + * values of mimic joints), set those as the new values that correspond to the group */ void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -827,8 +817,7 @@ as the new values that correspond to the group */ } /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including -values of mimic joints), set those -as the new values that correspond to the group */ + * values of mimic joints), set those as the new values that correspond to the group */ void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values); /** \brief For a given group, copy the acceleration values of the variables that make up the group into another @@ -1311,6 +1300,12 @@ as the new values that correspond to the group */ /** \brief Update the state after setting a particular link to the input global transform pose.*/ void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false); + /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel. + * This is typically the root link of the URDF unless a virtual joint is present. + * Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed. + * A related, more comprehensive function is |getFrameTransform|, which additionally to link frames + * also searches for attached object frames and their subframes. + */ const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) { return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); @@ -1322,6 +1317,27 @@ as the new values that correspond to the group */ return global_link_transforms_[link->getLinkIndex()]; } + const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const + { + return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); + } + + const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const + { + BOOST_VERIFY(checkLinkTransforms()); + return global_link_transforms_[link->getLinkIndex()]; + } + + /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel. + * This is typically the root link of the URDF unless a virtual joint is present. + * Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed. + * + * As opposed to the visual links in |getGlobalLinkTransform|, this function returns + * the collision link transform used for collision checking. + * + * @param link_name: name of link to lookup + * @param index: specify which collision body to lookup, if more than one exists + */ const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) { return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); @@ -1333,6 +1349,17 @@ as the new values that correspond to the group */ return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; } + const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const + { + return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); + } + + const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const + { + BOOST_VERIFY(checkCollisionTransforms()); + return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; + } + const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) { return getJointTransform(robot_model_->getJointModel(joint_name)); @@ -1350,28 +1377,6 @@ as the new values that correspond to the group */ return variable_joint_transforms_[idx]; } - const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const - { - return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); - } - - const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const - { - BOOST_VERIFY(checkLinkTransforms()); - return global_link_transforms_[link->getLinkIndex()]; - } - - const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const - { - return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); - } - - const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const - { - BOOST_VERIFY(checkCollisionTransforms()); - return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; - } - const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const { return getJointTransform(robot_model_->getJointModel(joint_name)); @@ -1638,24 +1643,24 @@ as the new values that correspond to the group */ return *rng_; } - /** \brief Get the transformation matrix from the model frame to the frame identified by \e frame_id + /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * * If frame_id was not found, \e frame_found is set to false and an identity transform is returned */ const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr); - /** \brief Get the transformation matrix from the model frame to the frame identified by \e frame_id + /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * * If frame_id was not found, \e frame_found is set to false and an identity transform is returned */ const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr) const; - /** \brief Get the transformation matrix from the model frame to the frame identified by \e frame_id + /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * * If this frame is attached to a robot link, the link pointer is returned in \e robot_link. * If frame_id was not found, \e frame_found is set to false and an identity transform is returned */ const Eigen::Isometry3d& getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link, bool& frame_found) const; - /** \brief Check if a transformation matrix from the model frame to frame \e frame_id is known */ + /** \brief Check if a transformation matrix from the model frame (root of model) to frame \e frame_id is known */ bool knowsFrameTransform(const std::string& frame_id) const; /** @brief Get a MarkerArray that fully describes the robot markers for a given robot. @@ -1812,9 +1817,12 @@ as the new values that correspond to the group */ const JointModel* dirty_link_transforms_; const JointModel* dirty_collision_body_transforms_; - Eigen::Isometry3d* variable_joint_transforms_; // this points to an element in transforms_, so it is aligned - Eigen::Isometry3d* global_link_transforms_; // this points to an element in transforms_, so it is aligned - Eigen::Isometry3d* global_collision_body_transforms_; // this points to an element in transforms_, so it is aligned + // All the following transform variables point into aligned memory in memory_ + // They are updated lazily, based on the flags in dirty_joint_transforms_ + // resp. the pointers dirty_link_transforms_ and dirty_collision_body_transforms_ + Eigen::Isometry3d* variable_joint_transforms_; ///< Local transforms of all joints + Eigen::Isometry3d* global_link_transforms_; ///< Transforms from model frame to link frame for each link + Eigen::Isometry3d* global_collision_body_transforms_; ///< Transforms from model frame to collision bodies unsigned char* dirty_joint_transforms_; /** \brief All attached bodies that are part of this state, indexed by their name */ diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 6fea83bf05..4c4c90b4d2 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -631,21 +631,21 @@ void RobotState::updateLinkTransformsInternal(const JointModel* start) if (parent) // root JointModel will not have a parent { int idx_parent = parent->getLinkIndex(); - if (link->parentJointIsFixed()) + if (link->parentJointIsFixed()) // fixed joint global_link_transforms_[idx_link].affine().noalias() = global_link_transforms_[idx_parent].affine() * link->getJointOriginTransform().matrix(); - else + else // non-fixed joint { - if (link->jointOriginTransformIsIdentity()) + if (link->jointOriginTransformIsIdentity()) // Link has identity transform global_link_transforms_[idx_link].affine().noalias() = global_link_transforms_[idx_parent].affine() * getJointTransform(link->getParentJointModel()).matrix(); - else + else // Link has non-identity transform global_link_transforms_[idx_link].affine().noalias() = global_link_transforms_[idx_parent].affine() * link->getJointOriginTransform().matrix() * getJointTransform(link->getParentJointModel()).matrix(); } } - else + else // is the origin / root / 'model frame' { if (link->jointOriginTransformIsIdentity()) global_link_transforms_[idx_link] = getJointTransform(link->getParentJointModel()); From 5cebef9e4584e19b4970ada1b48d8c61fe4de06e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sandro=20Magalh=C3=A3es?= Date: Wed, 22 Jan 2020 22:43:32 +0000 Subject: [PATCH 170/612] Add support for pos_vel_controllers and pos_vel_acc_controllers (#1806) * Add support for pos_vel_controllers and pos_vel_acc_controllers Added the support for the new controllores derived from JointTrajectoryControllers. Just adds the acknowledge for this controller in the moveit ros control plugin interface * Clang-format correct --- .../moveit_ros_control_interface_plugins.xml | 6 ++++++ .../src/widgets/controller_edit_widget.cpp | 5 +++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml b/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml index 2bbf76e67e..3bac259dd2 100644 --- a/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml +++ b/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml @@ -9,5 +9,11 @@ + + + + + + diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp index 5840fe2894..f86d0f7d5a 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp @@ -195,11 +195,12 @@ void ControllerEditWidget::loadControllersTypesComboBox() return; has_loaded_ = true; - const std::array default_types = { + const std::array default_types = { "effort_controllers/JointTrajectoryController", "effort_controllers/JointPositionController", "effort_controllers/JointVelocityController", "effort_controllers/JointEffortController", "position_controllers/JointPositionController", "position_controllers/JointTrajectoryController", - "velocity_controllers/JointTrajectoryController", "velocity_controllers/JointVelocityController" + "velocity_controllers/JointTrajectoryController", "velocity_controllers/JointVelocityController", + "pos_vel_controllers/JointTrajectoryController", "pos_vel_acc_controllers/JointTrajectoryController" }; // Remove all old items From a85738b7a1f1913bf97f99e138dfc8d22b0d3dad Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 23 Jan 2020 08:50:37 +0100 Subject: [PATCH 171/612] Move get_planning_scene service into PSM for reusability (#1854) --- .../get_planning_scene_service_capability.cpp | 16 +--------- .../get_planning_scene_service_capability.h | 7 ---- .../planning_scene_monitor.h | 31 ++++++++++++++---- .../src/planning_scene_monitor.cpp | 32 +++++++++++++++++-- 4 files changed, 56 insertions(+), 30 deletions(-) diff --git a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp index 89ccb8d817..5bdba89f0b 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp @@ -45,23 +45,9 @@ MoveGroupGetPlanningSceneService::MoveGroupGetPlanningSceneService() : MoveGroup void MoveGroupGetPlanningSceneService::initialize() { - get_scene_service_ = root_node_handle_.advertiseService( - GET_PLANNING_SCENE_SERVICE_NAME, &MoveGroupGetPlanningSceneService::getPlanningSceneService, this); + context_->planning_scene_monitor_->providePlanningSceneService(GET_PLANNING_SCENE_SERVICE_NAME); } -bool MoveGroupGetPlanningSceneService::getPlanningSceneService(moveit_msgs::GetPlanningScene::Request& req, - moveit_msgs::GetPlanningScene::Response& res) -{ - if (req.components.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS) - context_->planning_scene_monitor_->updateFrameTransforms(); - planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_); - - moveit_msgs::PlanningSceneComponents all_components; - all_components.components = UINT_MAX; // Return all scene components if nothing is specified. - ps->getPlanningSceneMsg(res.scene, req.components.components ? req.components : all_components); - - return true; -} } // namespace move_group #include diff --git a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h index c1cbfde2a7..9ce1b3bb19 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h @@ -37,7 +37,6 @@ #pragma once #include -#include namespace move_group { @@ -47,11 +46,5 @@ class MoveGroupGetPlanningSceneService : public MoveGroupCapability MoveGroupGetPlanningSceneService(); void initialize() override; - -private: - bool getPlanningSceneService(moveit_msgs::GetPlanningScene::Request& req, - moveit_msgs::GetPlanningScene::Response& res); - - ros::ServiceServer get_scene_service_; }; } diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index 49db508c9f..d79b575eb9 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -324,19 +325,29 @@ class PlanningSceneMonitor : private boost::noncopyable return 0.0; } - /** @brief Start the scene monitor + /** @brief Start the scene monitor (ROS topic-based) * @param scene_topic The name of the planning scene topic */ void startSceneMonitor(const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC); - /** @brief Request planning scene state using a service call - * @param service_name The name of the service to use for requesting the - * planning scene. This must be a service of type - * moveit_msgs::GetPlanningScene and is usually called - * "/get_planning_scene". + /** @brief Request a full planning scene state using a service call + * Be careful not to use this in conjunction with providePlanningSceneService(), + * as it will create a pointless feedback loop. + * @param service_name The name of the service to use for requesting the planning scene. + * This must be a service of type moveit_msgs::GetPlanningScene and is usually called + * "/get_planning_scene". */ bool requestPlanningSceneState(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE); + /** @brief Create an optional service for getting the complete planning scene + * This is useful for satisfying the Rviz PlanningScene display's need for a service + * without having to use a move_group node. + * Be careful not to use this in conjunction with requestPlanningSceneState(), + * as it will create a pointless feedback loop. + * @param service_name The topic to provide the service + */ + void providePlanningSceneService(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE); + /** @brief Stop the scene monitor*/ void stopSceneMonitor(); @@ -495,6 +506,10 @@ class PlanningSceneMonitor : private boost::noncopyable ros::Subscriber attached_collision_object_subscriber_; ros::Subscriber collision_object_subscriber_; + // provide an optional service to get the full planning scene state + // this is used by MoveGroup and related application nodes + ros::ServiceServer get_scene_service_; + // include a octomap monitor std::unique_ptr octomap_monitor_; @@ -535,6 +550,10 @@ class PlanningSceneMonitor : private boost::noncopyable // Callback for a new planning scene msg void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene); + // Callback for requesting the full planning scene via service + bool getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req, + moveit_msgs::GetPlanningScene::Response& res); + // Lock for state_update_pending_ and dt_state_update_ boost::mutex state_pending_mutex_; diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 21d4ed1e43..2381e06035 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -467,6 +467,11 @@ void PlanningSceneMonitor::triggerSceneUpdateEvent(SceneUpdateType update_type) bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_name) { + if (get_scene_service_.getService() == service_name) + { + ROS_FATAL_STREAM_NAMED(LOGNAME, "requestPlanningSceneState() to self-provided service '" << service_name << "'"); + throw std::runtime_error("requestPlanningSceneState() to self-provided service: " + service_name); + } // use global namespace for service ros::ServiceClient client = ros::NodeHandle().serviceClient(service_name); // all scene components are returned if none are specified @@ -485,13 +490,36 @@ bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_ } else { - ROS_INFO_NAMED(LOGNAME, "Failed to call service %s, have you launched move_group? at %s:%d", service_name.c_str(), - __FILE__, __LINE__); + ROS_INFO_NAMED( + LOGNAME, "Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?", + service_name.c_str()); return false; } return true; } +void PlanningSceneMonitor::providePlanningSceneService(const std::string& service_name) +{ + // Load the service + get_scene_service_ = + root_nh_.advertiseService(service_name, &PlanningSceneMonitor::getPlanningSceneServiceCallback, this); +} + +bool PlanningSceneMonitor::getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req, + moveit_msgs::GetPlanningScene::Response& res) +{ + if (req.components.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS) + updateFrameTransforms(); + + moveit_msgs::PlanningSceneComponents all_components; + all_components.components = UINT_MAX; // Return all scene components if nothing is specified. + + boost::unique_lock ulock(scene_update_mutex_); + scene_->getPlanningSceneMsg(res.scene, req.components.components ? req.components : all_components); + + return true; +} + void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene) { newPlanningSceneMessage(*scene); From bebed3ab6016c0b1d25504c5ebff647df2849607 Mon Sep 17 00:00:00 2001 From: Jens P Date: Thu, 23 Jan 2020 10:30:27 +0100 Subject: [PATCH 172/612] Rename PR2-related collision test files (#1856) --- .../{test_collision_common.h => test_collision_common_pr2.h} | 0 moveit_core/collision_detection_bullet/CMakeLists.txt | 2 +- ...on_detection.cpp => test_bullet_collision_detection_pr2.cpp} | 2 +- moveit_core/collision_detection_fcl/CMakeLists.txt | 2 +- .../include/moveit/collision_detection_fcl/collision_common.h | 2 +- moveit_core/collision_detection_fcl/src/collision_common.cpp | 2 +- ...ision_detection.cpp => test_fcl_collision_detection_pr2.cpp} | 2 +- 7 files changed, 6 insertions(+), 6 deletions(-) rename moveit_core/collision_detection/include/moveit/collision_detection/{test_collision_common.h => test_collision_common_pr2.h} (100%) rename moveit_core/collision_detection_bullet/test/{test_bullet_collision_detection.cpp => test_bullet_collision_detection_pr2.cpp} (97%) rename moveit_core/collision_detection_fcl/test/{test_fcl_collision_detection.cpp => test_fcl_collision_detection_pr2.cpp} (97%) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h similarity index 100% rename from moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h rename to moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index 4b4ccf6547..799fead00d 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -31,7 +31,7 @@ if(CATKIN_ENABLE_TESTING) find_package(moveit_resources REQUIRED) include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_bullet_collision_detection test/test_bullet_collision_detection.cpp) + catkin_add_gtest(test_bullet_collision_detection test/test_bullet_collision_detection_pr2.cpp) target_link_libraries(test_bullet_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) catkin_add_gtest(test_bullet_collision_detection_panda test/test_bullet_collision_detection_panda.cpp) diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp similarity index 97% rename from moveit_core/collision_detection_bullet/test/test_bullet_collision_detection.cpp rename to moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp index 3f22bab5f0..e522c6d67e 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp @@ -35,7 +35,7 @@ /* Author: Jens Petit */ #include -#include +#include INSTANTIATE_TYPED_TEST_CASE_P(BulletCollisionCheck, CollisionDetectorTest, collision_detection::CollisionDetectorAllocatorBullet); diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index fbeefa76fd..3889763ac4 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -27,7 +27,7 @@ if(CATKIN_ENABLE_TESTING) find_package(moveit_resources REQUIRED) include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection.cpp) + catkin_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection_pr2.cpp) target_link_libraries(test_fcl_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) catkin_add_gtest(test_fcl_collision_detection_panda test/test_fcl_collision_detection_panda.cpp) diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index 9df7168c69..c5b89cca28 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan, Jens Petit */ +/* Author: Ioan Sucan */ #pragma once diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index 7a936b2771..0cf3446237 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan, Jia Pan, Jens Petit */ +/* Author: Ioan Sucan, Jia Pan */ #include #include diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp similarity index 97% rename from moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp rename to moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp index 5dffe19a8b..efe312cf3b 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp @@ -35,7 +35,7 @@ /* Author: Jens Petit */ #include -#include +#include INSTANTIATE_TYPED_TEST_CASE_P(FCLCollisionCheck, CollisionDetectorTest, collision_detection::CollisionDetectorAllocatorFCL); From e582953c8c7a92f19674479f9986fd45d44d4f92 Mon Sep 17 00:00:00 2001 From: Luca Rinelli Date: Thu, 23 Jan 2020 11:17:42 +0100 Subject: [PATCH 173/612] Fix typo in cmake file (#1857) --- moveit_ros/planning_interface/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 8a907efba1..0eb0f40575 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -31,7 +31,7 @@ find_package(PythonLibs "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" REQUIR find_package(Boost REQUIRED) if(Boost_VERSION LESS 106700) set(BOOST_PYTHON_COMPONENT python) -elseif() +else() set(BOOST_PYTHON_COMPONENT python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}) endif() From d7b05fa1a2073a57d0fdea76472fecd40e7613d9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 23 Jan 2020 11:39:49 +0100 Subject: [PATCH 174/612] getFrameInfo(): Avoid double search for link name (#1853) --- moveit_core/robot_state/src/robot_state.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 4c4c90b4d2..519df14dce 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1019,11 +1019,9 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c frame_found = true; return IDENTITY_TRANSFORM; } - if (robot_model_->hasLinkModel(frame_id)) + if ((robot_link = robot_model_->getLinkModel(frame_id, &frame_found))) { - robot_link = robot_model_->getLinkModel(frame_id); BOOST_VERIFY(checkLinkTransforms()); - frame_found = true; return global_link_transforms_[robot_link->getLinkIndex()]; } robot_link = nullptr; From 4f996c71a1734e82ddab5e49b188a2d5cc20ea7d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 23 Jan 2020 07:10:01 -0700 Subject: [PATCH 175/612] [jog_arm] Various small fixes (#1859) - Fix logic error, testing for empty trajectory - Restore the integration test timeout - Rename checkIfJointsWithingSRDFBounds -> enforceSRDFJointBounds --- .../moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h | 2 +- moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp | 8 ++++---- .../test/integration/test_jog_arm_integration.py | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index b98775e0bb..4c91199557 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -87,7 +87,7 @@ class JogCalcs void publishWarning(bool active) const; // Scale the delta theta to match joint velocity limits - bool checkIfJointsWithinSRDFBounds(trajectory_msgs::JointTrajectory& new_joint_traj); + bool enforceSRDFJointBounds(trajectory_msgs::JointTrajectory& new_joint_traj); // Possibly calculate a velocity scaling factor, due to proximity of // singularity and direction of motion diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 3523900ba3..ec550da754 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -320,7 +320,7 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& applyVelocityScaling(shared_variables, mutex, outgoing_command_, delta_theta_, decelerateForSingularity(delta_x, svd_)); - if (!checkIfJointsWithinSRDFBounds(outgoing_command_)) + if (!enforceSRDFJointBounds(outgoing_command_)) { suddenHalt(outgoing_command_); publishWarning(true); @@ -369,7 +369,7 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& /* outgoing_command_ = composeOutgoingMessage(joint_state_); - if (!checkIfJointsWithinSRDFBounds(outgoing_command_)) + if (!enforceSRDFJointBounds(outgoing_command_)) { suddenHalt(outgoing_command_); publishWarning(true); @@ -554,11 +554,11 @@ double JogCalcs::decelerateForSingularity(const Eigen::VectorXd& commanded_veloc return velocity_scale; } -bool JogCalcs::checkIfJointsWithinSRDFBounds(trajectory_msgs::JointTrajectory& new_joint_traj) +bool JogCalcs::enforceSRDFJointBounds(trajectory_msgs::JointTrajectory& new_joint_traj) { bool halting = false; - if (!new_joint_traj.points.empty()) + if (new_joint_traj.points.empty()) { ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "Empty trajectory passed into checkIfJointsWithinURDFBounds()."); return true; // technically an empty trajectory is still within bounds diff --git a/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py b/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py index fe1a0323a1..3c901e0b64 100755 --- a/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py +++ b/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py @@ -11,8 +11,8 @@ # This can be run as part of a pytest, or like a normal ROS executable: # rosrun moveit_jog_arm test_jog_arm_integration.py -JOG_ARM_SETTLE_TIME_S = 3 -ROS_SETTLE_TIME_S = 3 +JOG_ARM_SETTLE_TIME_S = 10 +ROS_SETTLE_TIME_S = 10 JOINT_JOG_COMMAND_TOPIC = 'jog_server/joint_delta_jog_cmds' CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' From 2395183dccc373eda9da9cf7ee5295d601413ceb Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Fri, 31 Jan 2020 08:06:11 +0100 Subject: [PATCH 176/612] Adapt message serialization for Python 3 (#1870) * Replaced has_key with in statement In Python 3, the method `has_key` was removed from dictionaries. It is replaced with an `key in dict_instance` statement. * Replaced StringIO with BytesIO for python msg serialization * Use py_bindings_tools::ByteString as byte-based serialization buffer on C++ side Python 3 interprets strings as unicode, but we want the serialization buffers to be bytes. --- .../bin/moveit_commander_cmdline.py | 2 +- .../src/moveit_commander/conversions.py | 4 +- .../src/moveit_commander/interpreter.py | 12 +- .../src/moveit_commander/robot.py | 4 +- .../src/wrap_python_move_group.cpp | 51 ++++----- .../wrap_python_planning_scene_interface.cpp | 8 +- .../moveit/py_bindings_tools/serialize_msg.h | 103 ++++++++++++++---- .../src/wrap_python_robot_interface.cpp | 26 ++--- .../moveitjoy_module.py | 2 +- 9 files changed, 139 insertions(+), 73 deletions(-) diff --git a/moveit_commander/bin/moveit_commander_cmdline.py b/moveit_commander/bin/moveit_commander_cmdline.py index 61a37e8898..821b4ffed5 100755 --- a/moveit_commander/bin/moveit_commander_cmdline.py +++ b/moveit_commander/bin/moveit_commander_cmdline.py @@ -45,7 +45,7 @@ def complete(self, text, state): prefix = "" if len(cmds) > 0: prefix = cmds[0] - if not self.options.has_key(prefix): + if not prefix in self.options: prefix = "" if state == 0: diff --git a/moveit_commander/src/moveit_commander/conversions.py b/moveit_commander/src/moveit_commander/conversions.py index 71c2dfe147..f9129ffd8f 100644 --- a/moveit_commander/src/moveit_commander/conversions.py +++ b/moveit_commander/src/moveit_commander/conversions.py @@ -36,8 +36,8 @@ # Try Python 2.7 behaviour first from StringIO import StringIO except ImportError: - # Use Python 3.x behaviour as fallback - from io import StringIO + # Use Python 3.x behaviour as fallback and choose the non-unicode version + from io import BytesIO as StringIO from moveit_commander import MoveItCommanderException from geometry_msgs.msg import Pose, PoseStamped, Transform diff --git a/moveit_commander/src/moveit_commander/interpreter.py b/moveit_commander/src/moveit_commander/interpreter.py index 02cc10e4d4..c9088de375 100644 --- a/moveit_commander/src/moveit_commander/interpreter.py +++ b/moveit_commander/src/moveit_commander/interpreter.py @@ -103,7 +103,7 @@ def execute_generic_command(self, cmd): clist[1] = self._prev_group_name if len(clist[1]) == 0: return (MoveGroupInfoLevel.DEBUG, "OK") - if self._gdict.has_key(clist[1]): + if clist[1] in self._gdict: self._prev_group_name = self._group_name self._group_name = clist[1] return (MoveGroupInfoLevel.DEBUG, "OK") @@ -267,7 +267,7 @@ def execute_group_command(self, g, cmdorig): assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd) if assign_match: known = g.get_remembered_joint_values() - if known.has_key(assign_match.group(2)): + if assign_match.group(2) in known: g.remember_joint_values(assign_match.group(1), known[assign_match.group(2)]) return (MoveGroupInfoLevel.SUCCESS, assign_match.group(1) + " is now the same as " + assign_match.group(2)) else: @@ -286,7 +286,7 @@ def execute_group_command(self, g, cmdorig): component_match = re.match(r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd) if component_match: known = g.get_remembered_joint_values() - if known.has_key(component_match.group(1)): + if component_match.group(1) in known: try: val = known[component_match.group(1)] val[int(component_match.group(2))] = float(component_match.group(3)) @@ -303,7 +303,7 @@ def execute_group_command(self, g, cmdorig): # if this is an unknown one-word command, it is probably a variable if len(clist) == 1: known = g.get_remembered_joint_values() - if known.has_key(cmd): + if cmd in known: return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[cmd]]) + "]") else: return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") @@ -381,7 +381,7 @@ def execute_group_command(self, g, cmdorig): return (MoveGroupInfoLevel.SUCCESS, "Forgot joint values under the name " + clist[1]) elif clist[0] == "show": known = g.get_remembered_joint_values() - if known.has_key(clist[1]): + if clist[1] in known: return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[clist[1]]]) + "]") else: return (MoveGroupInfoLevel.WARN, "Joint values for " + clist[1] + " are not known") @@ -423,7 +423,7 @@ def execute_group_command(self, g, cmdorig): return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") if len(clist) == 3: - if clist[0] == "go" and self.GO_DIRS.has_key(clist[1]): + if clist[0] == "go" and clist[1] in self.GO_DIRS: self._last_plan = None try: offset = float(clist[2]) diff --git a/moveit_commander/src/moveit_commander/robot.py b/moveit_commander/src/moveit_commander/robot.py index 6336600aca..137fe4b4b4 100644 --- a/moveit_commander/src/moveit_commander/robot.py +++ b/moveit_commander/src/moveit_commander/robot.py @@ -261,7 +261,7 @@ def get_group(self, name): @param name str: Name of movegroup @rtype: moveit_commander.MoveGroupCommander """ - if not self._groups.has_key(name): + if not name in self._groups: if not self.has_group(name): raise MoveItCommanderException("There is no group named %s" % name) self._groups[name] = MoveGroupCommander(name, self._robot_description, self._ns) @@ -279,7 +279,7 @@ def get_default_owner_group(self, joint_name): Get the name of the smallest group (fewest joints) that includes the joint name specified as argument. """ - if not self._joint_owner_groups.has_key(joint_name): + if not joint_name in self._joint_owner_groups: group = None for g in self.get_group_names(): if joint_name in self.get_joint_names(g): diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 50646a8777..f12bd4be97 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -94,21 +94,23 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return setJointValueTarget(v); } - bool setJointValueTargetFromPosePython(const std::string& pose_str, const std::string& eef, bool approx) + bool setJointValueTargetFromPosePython(const py_bindings_tools::ByteString& pose_str, const std::string& eef, + bool approx) { geometry_msgs::Pose pose_msg; py_bindings_tools::deserializeMsg(pose_str, pose_msg); return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef); } - bool setJointValueTargetFromPoseStampedPython(const std::string& pose_str, const std::string& eef, bool approx) + bool setJointValueTargetFromPoseStampedPython(const py_bindings_tools::ByteString& pose_str, const std::string& eef, + bool approx) { geometry_msgs::PoseStamped pose_msg; py_bindings_tools::deserializeMsg(pose_str, pose_msg); return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef); } - bool setJointValueTargetFromJointStatePython(const std::string& js_str) + bool setJointValueTargetFromJointStatePython(const py_bindings_tools::ByteString& js_str) { sensor_msgs::JointState js_msg; py_bindings_tools::deserializeMsg(js_str, js_msg); @@ -125,7 +127,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return l; } - std::string getJointValueTarget() + py_bindings_tools::ByteString getJointValueTarget() { moveit_msgs::RobotState msg; const robot_state::RobotState state = moveit::planning_interface::MoveGroupInterface::getTargetRobotState(); @@ -143,7 +145,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return getPlanningFrame().c_str(); } - std::string getInterfaceDescriptionPython() + py_bindings_tools::ByteString getInterfaceDescriptionPython() { moveit_msgs::PlannerInterfaceDescription msg; getInterfaceDescription(msg); @@ -260,7 +262,8 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return place(object_name, msg, plan_only) == MoveItErrorCode::SUCCESS; } - bool placeLocation(const std::string& object_name, const std::string& location_str, bool plan_only = false) + bool placeLocation(const std::string& object_name, const py_bindings_tools::ByteString& location_str, + bool plan_only = false) { std::vector locations(1); py_bindings_tools::deserializeMsg(location_str, locations[0]); @@ -313,7 +316,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return output; } - void setStartStatePython(const std::string& msg_str) + void setStartStatePython(const py_bindings_tools::ByteString& msg_str) { moveit_msgs::RobotState msg; py_bindings_tools::deserializeMsg(msg_str, msg); @@ -326,7 +329,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer convertListToArrayOfPoses(poses, msg); return setPoseTargets(msg, end_effector_link); } - std::string getPoseTargetPython(const std::string& end_effector_link) + py_bindings_tools::ByteString getPoseTargetPython(const std::string& end_effector_link) { geometry_msgs::PoseStamped pose = moveit::planning_interface::MoveGroupInterface::getPoseTarget(end_effector_link); return py_bindings_tools::serializeMsg(pose); @@ -406,14 +409,14 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return attachObject(object_name, link_name, py_bindings_tools::stringFromList(touch_links)); } - bool executePython(const std::string& plan_str) + bool executePython(const py_bindings_tools::ByteString& plan_str) { MoveGroupInterface::Plan plan; py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); return execute(plan) == MoveItErrorCode::SUCCESS; } - bool asyncExecutePython(const std::string& plan_str) + bool asyncExecutePython(const py_bindings_tools::ByteString& plan_str) { MoveGroupInterface::Plan plan; py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); @@ -436,7 +439,8 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer } bp::tuple computeCartesianPathConstrainedPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions, const std::string& path_constraints_str) + bool avoid_collisions, + const py_bindings_tools::ByteString& path_constraints_str) { moveit_msgs::Constraints path_constraints; py_bindings_tools::deserializeMsg(path_constraints_str, path_constraints); @@ -454,7 +458,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return bp::make_tuple(py_bindings_tools::serializeMsg(trajectory), fraction); } - int pickGrasp(const std::string& object, const std::string& grasp_str, bool plan_only = false) + int pickGrasp(const std::string& object, const py_bindings_tools::ByteString& grasp_str, bool plan_only = false) { moveit_msgs::Grasp grasp; py_bindings_tools::deserializeMsg(grasp_str, grasp); @@ -466,27 +470,27 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer int l = bp::len(grasp_list); std::vector grasps(l); for (int i = 0; i < l; ++i) - py_bindings_tools::deserializeMsg(bp::extract(grasp_list[i]), grasps[i]); + py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(grasp_list[i]), grasps[i]); return pick(object, std::move(grasps), plan_only).val; } - void setPathConstraintsFromMsg(const std::string& constraints_str) + void setPathConstraintsFromMsg(const py_bindings_tools::ByteString& constraints_str) { moveit_msgs::Constraints constraints_msg; py_bindings_tools::deserializeMsg(constraints_str, constraints_msg); setPathConstraints(constraints_msg); } - std::string getPathConstraintsPython() + py_bindings_tools::ByteString getPathConstraintsPython() { moveit_msgs::Constraints constraints_msg(getPathConstraints()); - std::string constraints_str = py_bindings_tools::serializeMsg(constraints_msg); - return constraints_str; + return py_bindings_tools::serializeMsg(constraints_msg); } - std::string retimeTrajectory(const std::string& ref_state_str, const std::string& traj_str, - double velocity_scaling_factor, double acceleration_scaling_factor, - const std::string& algorithm) + py_bindings_tools::ByteString retimeTrajectory(const py_bindings_tools::ByteString& ref_state_str, + const py_bindings_tools::ByteString& traj_str, + double velocity_scaling_factor, double acceleration_scaling_factor, + const std::string& algorithm) { // Convert reference state message to object moveit_msgs::RobotState ref_state_msg; @@ -524,15 +528,12 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer // Convert the retimed trajectory back into a message traj_obj.getRobotTrajectoryMsg(traj_msg); - std::string traj_str = py_bindings_tools::serializeMsg(traj_msg); - - // Return it. - return traj_str; + return py_bindings_tools::serializeMsg(traj_msg); } else { ROS_ERROR("Unable to convert RobotState message to RobotState instance."); - return ""; + return py_bindings_tools::ByteString(""); } } diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp index e1e825b0b0..e0cd8f7818 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp @@ -74,7 +74,7 @@ class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitial bp::dict getObjectPosesPython(const bp::list& object_ids) { std::map ops = getObjectPoses(py_bindings_tools::stringFromList(object_ids)); - std::map ser_ops; + std::map ser_ops; for (std::map::const_iterator it = ops.begin(); it != ops.end(); ++it) ser_ops[it->first] = py_bindings_tools::serializeMsg(it->second); @@ -85,7 +85,7 @@ class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitial { std::map objs = getObjects(py_bindings_tools::stringFromList(object_ids)); - std::map ser_objs; + std::map ser_objs; for (std::map::const_iterator it = objs.begin(); it != objs.end(); ++it) ser_objs[it->first] = py_bindings_tools::serializeMsg(it->second); @@ -96,7 +96,7 @@ class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitial { std::map aobjs = getAttachedObjects(py_bindings_tools::stringFromList(object_ids)); - std::map ser_aobjs; + std::map ser_aobjs; for (std::map::const_iterator it = aobjs.begin(); it != aobjs.end(); ++it) ser_aobjs[it->first] = py_bindings_tools::serializeMsg(it->second); @@ -104,7 +104,7 @@ class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitial return py_bindings_tools::dictFromType(ser_aobjs); } - bool applyPlanningScenePython(const std::string& ps_str) + bool applyPlanningScenePython(const py_bindings_tools::ByteString& ps_str) { moveit_msgs::PlanningScene ps_msg; py_bindings_tools::deserializeMsg(ps_str, ps_msg); diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h index 5b9a9a306f..9ca48182db 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h @@ -37,36 +37,101 @@ #pragma once #include +#include +#include +#include +#include +#include namespace moveit { namespace py_bindings_tools { -/** \brief Convert a ROS message to a string */ -template -std::string serializeMsg(const T& msg) +/** \brief C++ Wrapper class for Python 3 \c Bytes Object */ +class ByteString : public boost::python::object { - // we use the fact char is same size as uint8_t; - assert(sizeof(uint8_t) == sizeof(char)); - std::size_t size = ros::serialization::serializationLength(msg); - std::string result(size, '\0'); - if (size) +public: + // constructors for bp::handle and friends + BOOST_PYTHON_FORWARD_OBJECT_CONSTRUCTORS(ByteString, boost::python::object) + ByteString() : boost::python::object(boost::python::handle<>(PyBytes_FromString(""))) + { + } + explicit ByteString(const char* s) : boost::python::object(boost::python::handle<>(PyBytes_FromString(s))) + { + } + explicit ByteString(const std::string& s) + : boost::python::object(boost::python::handle<>(PyBytes_FromStringAndSize(s.c_str(), s.size()))) + { + } + // bp::list[] returns a proxy which has to be converted to an object first + template + explicit ByteString(const boost::python::api::proxy& proxy) : boost::python::object(proxy) + { + } + /** \brief Serializes a ROS message into a Python Bytes object + * The second template parameter ensures that this overload is only chosen with a ROS message argument + */ + template ::value, int>::type = 0> + explicit ByteString(const T& msg) + : boost::python::object( + boost::python::handle<>(PyBytes_FromStringAndSize(nullptr, ros::serialization::serializationLength(msg)))) { - // we convert the message into a string because that is easy to sent back & forth with Python - // This is fine since C0x because &string[0] is guaranteed to point to a contiguous block of memory - ros::serialization::OStream stream_arg(reinterpret_cast(&result[0]), size); + ros::serialization::OStream stream_arg(reinterpret_cast(PyBytes_AS_STRING(ptr())), + PyBytes_GET_SIZE(ptr())); ros::serialization::serialize(stream_arg, msg); } - return result; -} -/** \brief Convert a string to a ROS message */ + /** \brief Convert content to a ROS message */ + template + void deserialize(T& msg) const + { + static_assert(sizeof(uint8_t) == sizeof(char), "ros/python buffer layout mismatch"); + char* buf = PyBytes_AsString(ptr()); + // buf == NULL on error + if (!buf) + { + throw std::runtime_error("Underlying python object is not a Bytes/String instance"); + } + // unfortunately no constructor with const uint8_t + ros::serialization::IStream stream_arg(reinterpret_cast(buf), PyBytes_GET_SIZE(ptr())); + ros::serialization::deserialize(stream_arg, msg); + } +}; + +/** \brief Convert a ROS message to a Python Bytestring */ template -void deserializeMsg(const std::string& data, T& msg) +ByteString serializeMsg(const T& msg) { - assert(sizeof(uint8_t) == sizeof(char)); - ros::serialization::IStream stream_arg(const_cast(reinterpret_cast(&data[0])), data.size()); - ros::serialization::deserialize(stream_arg, msg); -} + return ByteString(msg); } + +/** \brief Convert a Python Bytestring to a ROS message */ +template +void deserializeMsg(const ByteString& data, T& msg) +{ + data.deserialize(msg); } + +} // namespace py_bindings_tools +} // namespace moveit + +namespace boost +{ +namespace python +{ +namespace converter +{ +// only accept Python 3 Bytes / Python 2 String instance when used as C++ function parameter +template <> +struct object_manager_traits +#if PY_VERSION_HEX >= 0x03000000 + : pytype_object_manager_traits<&PyBytes_Type, moveit::py_bindings_tools::ByteString> +#else + : pytype_object_manager_traits<&PyString_Type, moveit::py_bindings_tools::ByteString> +#endif + +{ +}; +} // namespace converter +} // namespace python +} // namespace boost diff --git a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp index daa49825da..be238104f4 100644 --- a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp +++ b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp @@ -223,10 +223,10 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return true; } - std::string getCurrentState() + py_bindings_tools::ByteString getCurrentState() { if (!ensureCurrentState()) - return ""; + return py_bindings_tools::ByteString(""); robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); moveit_msgs::RobotState msg; robot_state::robotStateToRobotStateMsg(*s, msg); @@ -244,7 +244,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return boost::python::make_tuple(parent_group.first, parent_group.second); } - std::string getRobotMarkersPythonDictList(bp::dict& values, bp::list& links) + py_bindings_tools::ByteString getRobotMarkersPythonDictList(bp::dict& values, bp::list& links) { robot_state::RobotStatePtr state; if (ensureCurrentState()) @@ -273,13 +273,13 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return py_bindings_tools::serializeMsg(msg); } - std::string getRobotMarkersPythonDict(bp::dict& values) + py_bindings_tools::ByteString getRobotMarkersPythonDict(bp::dict& values) { bp::list links = py_bindings_tools::listFromString(robot_model_->getLinkModelNames()); return getRobotMarkersPythonDictList(values, links); } - std::string getRobotMarkersFromMsg(const std::string& state_str) + py_bindings_tools::ByteString getRobotMarkersFromMsg(const py_bindings_tools::ByteString& state_str) { moveit_msgs::RobotState state_msg; robot_state::RobotState state(robot_model_); @@ -292,10 +292,10 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return py_bindings_tools::serializeMsg(msg); } - std::string getRobotMarkers() + py_bindings_tools::ByteString getRobotMarkers() { if (!ensureCurrentState()) - return ""; + return py_bindings_tools::ByteString(); robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); visualization_msgs::MarkerArray msg; s->getRobotMarkers(msg, s->getRobotModel()->getLinkModelNames()); @@ -303,10 +303,10 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return py_bindings_tools::serializeMsg(msg); } - std::string getRobotMarkersPythonList(const bp::list& links) + py_bindings_tools::ByteString getRobotMarkersPythonList(const bp::list& links) { if (!ensureCurrentState()) - return ""; + return py_bindings_tools::ByteString(""); robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); visualization_msgs::MarkerArray msg; s->getRobotMarkers(msg, py_bindings_tools::stringFromList(links)); @@ -314,10 +314,10 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return py_bindings_tools::serializeMsg(msg); } - std::string getRobotMarkersGroup(const std::string& group) + py_bindings_tools::ByteString getRobotMarkersGroup(const std::string& group) { if (!ensureCurrentState()) - return ""; + return py_bindings_tools::ByteString(""); robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); visualization_msgs::MarkerArray msg; @@ -329,11 +329,11 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return py_bindings_tools::serializeMsg(msg); } - std::string getRobotMarkersGroupPythonDict(const std::string& group, bp::dict& values) + py_bindings_tools::ByteString getRobotMarkersGroupPythonDict(const std::string& group, bp::dict& values) { const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg) - return ""; + return py_bindings_tools::ByteString(""); bp::list links = py_bindings_tools::listFromString(jmg->getLinkModelNames()); return getRobotMarkersPythonDictList(values, links); } diff --git a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py index 3868b0e3ad..5bf253c3ab 100644 --- a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py +++ b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py @@ -453,7 +453,7 @@ def waitForInitialPose(self, next_topic, timeout=None): self.marker_lock.acquire() self.initialize_poses = True topic_suffix = next_topic.split("/")[-1] - if self.initial_poses.has_key(topic_suffix): + if topic_suffix in self.initial_poses: self.pre_pose = PoseStamped(pose=self.initial_poses[topic_suffix]) self.initialize_poses = False return True From 1ec63725e998a377edb743bd33819fc9e304dab7 Mon Sep 17 00:00:00 2001 From: Dale Koenig Date: Tue, 4 Feb 2020 05:22:41 +0900 Subject: [PATCH 177/612] Fix segfault in totg (#1861) Ensure acos argument in valid range --- .../src/time_optimal_trajectory_generation.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 19d1c38cab..26b1cd815a 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -110,7 +110,6 @@ class CircularPathSegment : public PathSegment const Eigen::VectorXd start_direction = (intersection - start).normalized(); const Eigen::VectorXd end_direction = (end - intersection).normalized(); - // check if directions are divergent if ((start_direction - end_direction).norm() < 0.000001) { length_ = 0.0; @@ -122,7 +121,7 @@ class CircularPathSegment : public PathSegment } // directions must be different at this point so angle is always non-zero - const double angle = acos(start_direction.dot(end_direction)); + const double angle = acos(std::max(-1.0, start_direction.dot(end_direction))); const double start_distance = (start - intersection).norm(); const double end_distance = (end - intersection).norm(); From e43a0a0bc8ccc63a78b385275d721a9234ab260c Mon Sep 17 00:00:00 2001 From: RMonica Date: Wed, 5 Feb 2020 15:19:03 +0100 Subject: [PATCH 178/612] Fix pruning of enclosed nodes when rendering octomap in RViz (#1685) --- .../src/octomap_render.cpp | 31 ++++++++++++++----- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp index 00851951eb..07d5868a90 100755 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp @@ -166,6 +166,8 @@ void OcTreeRender::octreeDecoding(const std::shared_ptr& size_t point_count = 0; { + int step_size = 1 << (octree->getTreeDepth() - octree_depth_); // for pruning of occluded voxels + // traverse all leafs in the tree: for (octomap::OcTree::iterator it = octree->begin(octree_depth_), end = octree->end(); it != end; ++it) { @@ -178,20 +180,35 @@ void OcTreeRender::octreeDecoding(const std::shared_ptr& bool all_neighbors_found = true; octomap::OcTreeKey key; - octomap::OcTreeKey n_key = it.getKey(); + octomap::OcTreeKey n_key = it.getIndexKey(); // key of the maximum-depth voxel at the current voxel corner + + // determine indices of potentially neighboring voxels for depths < maximum tree depth + // +/-1 at maximum depth, -1 and +depth_difference on other depths + int diff_base = 1 << (octree->getTreeDepth() - it.getDepth()); + int diff[2] = { -1, diff_base }; - for (key[2] = n_key[2] - 1; all_neighbors_found && key[2] <= n_key[2] + 1; ++key[2]) + // cells with adjacent faces can occlude a voxel, iterate over the cases x,y,z (idxCase) and +/- (diff) + for (unsigned int idx_case = 0; idx_case < 3; ++idx_case) { - for (key[1] = n_key[1] - 1; all_neighbors_found && key[1] <= n_key[1] + 1; ++key[1]) + int idx_0 = idx_case % 3; + int idx_1 = (idx_case + 1) % 3; + int idx_2 = (idx_case + 2) % 3; + + for (int i = 0; all_neighbors_found && i < 2; ++i) { - for (key[0] = n_key[0] - 1; all_neighbors_found && key[0] <= n_key[0] + 1; ++key[0]) + key[idx_0] = n_key[idx_0] + diff[i]; + // if rendering is restricted to treeDepth < maximum tree depth inner nodes with distance step_size can + // already occlude a voxel + for (key[idx_1] = n_key[idx_1] + diff[0] + 1; all_neighbors_found && key[idx_1] < n_key[idx_1] + diff[1]; + key[idx_1] += step_size) { - if (key != n_key) + for (key[idx_2] = n_key[idx_2] + diff[0] + 1; all_neighbors_found && key[idx_2] < n_key[idx_2] + diff[1]; + key[idx_2] += step_size) { - octomap::OcTreeNode* node = octree->search(key); + octomap::OcTreeNode* node = octree->search(key, octree_depth_); // the left part evaluates to 1 for free voxels and 2 for occupied voxels - if (!(node && (((int)octree->isNodeOccupied(node)) + 1) & render_mode_mask)) + if (!(node && ((((int)octree->isNodeOccupied(node)) + 1) & render_mode_mask))) { // we do not have a neighbor => break! all_neighbors_found = false; From 49892146c95659c4230a3372c36ba89598fdee0a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 5 Feb 2020 07:27:12 -0700 Subject: [PATCH 179/612] [jog_arm] Rework the halt msg functionality (#1868) * Rework the halt msg functionality * Add a test for halt messages from the jogger * Clang format and lint --- .../moveit_jog_arm/CMakeLists.txt | 7 +- .../include/moveit_jog_arm/jog_calcs.h | 10 ++- .../moveit_jog_arm/src/jog_calcs.cpp | 17 +++-- .../arm_setup/config/singular_position.yaml | 10 +++ .../test/launch/jog_arm_halt_msg_test.test | 39 +++++++++++ ...t.test => jog_arm_msg_reception_test.test} | 4 +- .../test_halt_msg/test_jog_arm_halt_msg.py | 66 +++++++++++++++++++ .../test_jog_arm_msg_reception.py} | 5 +- 8 files changed, 145 insertions(+), 13 deletions(-) create mode 100644 moveit_experimental/moveit_jog_arm/test/arm_setup/config/singular_position.yaml create mode 100644 moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test rename moveit_experimental/moveit_jog_arm/test/launch/{jog_arm_integration_test.test => jog_arm_msg_reception_test.test} (89%) create mode 100755 moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py rename moveit_experimental/moveit_jog_arm/test/{integration/test_jog_arm_integration.py => test_msg_reception/test_jog_arm_msg_reception.py} (95%) diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index 439ea9ee85..8bff931ad1 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -124,8 +124,11 @@ if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) find_package(ros_pytest REQUIRED) - # jog_server - add_rostest(test/launch/jog_arm_integration_test.test) + # Python integration test of basic message reception/publication + add_rostest(test/launch/jog_arm_msg_reception_test.test) + + # Python integration test that a halt message is published at a singularity + add_rostest(test/launch/jog_arm_halt_msg_test.test) # jog_cpp_interface add_rostest_gtest(jog_cpp_interface_test diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 4c91199557..7a65867b31 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -94,7 +94,15 @@ class JogCalcs double decelerateForSingularity(const Eigen::VectorXd& commanded_velocity, const Eigen::JacobiSVD& svd); - // Apply velocity scaling for proximity of collisions and singularities + /** + * Slow motion down if close to singularity or collision. + * @param shared_variables data shared between threads, tells how close we are to collision + * @param mutex locks shared data + * @param new_joint_traj store the motion in the first waypoint of this trajectory + * @param delta_theta motion command, used in calculating new_joint_tray + * @param singularity_scale tells how close we are to a singularity + * @return false if very close to collision or singularity + */ bool applyVelocityScaling(JogArmShared& shared_variables, std::mutex& mutex, trajectory_msgs::JointTrajectory& new_joint_traj, const Eigen::VectorXd& delta_theta, double singularity_scale); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index ec550da754..bcb8675fbe 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -316,11 +316,15 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& outgoing_command_ = composeOutgoingMessage(joint_state_); - // If close to a collision or a singularity, decelerate - applyVelocityScaling(shared_variables, mutex, outgoing_command_, delta_theta_, - decelerateForSingularity(delta_x, svd_)); - - if (!enforceSRDFJointBounds(outgoing_command_)) + // If close to a collision or a singularity, decelerate and publish a warning + if (!applyVelocityScaling(shared_variables, mutex, outgoing_command_, delta_theta_, + decelerateForSingularity(delta_x, svd_))) + { + suddenHalt(outgoing_command_); + publishWarning(true); + } + // If a joint limit would be exceeded, halt and publish a warning + else if (!enforceSRDFJointBounds(outgoing_command_)) { suddenHalt(outgoing_command_); publishWarning(true); @@ -483,7 +487,8 @@ bool JogCalcs::applyVelocityScaling(JogArmShared& shared_variables, std::mutex& new_joint_traj.points[0].velocities[i] *= singularity_scale * collision_scale; } - return true; + // Heuristic: flag that we are stuck if velocity scaling is < X% + return collision_scale * singularity_scale >= 0.1; } // Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion diff --git a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/singular_position.yaml b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/singular_position.yaml new file mode 100644 index 0000000000..582482f8b2 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/singular_position.yaml @@ -0,0 +1,10 @@ +zeros: + panda_joint1: 0 + panda_joint2: 0 + panda_joint3: 0 + panda_joint4: 0 + panda_joint5: 0 + panda_joint6: 1.57 + panda_joint7: 1.57 +publish_default_positions: True + diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test new file mode 100644 index 0000000000..5f25dfdd33 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_integration_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test similarity index 89% rename from moveit_experimental/moveit_jog_arm/test/launch/jog_arm_integration_test.test rename to moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test index fbe629e8b6..343bd87c49 100644 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_integration_test.test +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test @@ -34,6 +34,6 @@ - - + + diff --git a/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py b/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py new file mode 100755 index 0000000000..741c18a0b4 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python +import time + +import pytest +import rospy +from geometry_msgs.msg import TwistStamped + +from control_msgs.msg import JointJog +from std_msgs.msg import Bool + +# The robot starts at a singular position (see config file). +# Listen for a halt message from the jogger. +# This can be run as part of a pytest, or like a normal ROS executable: +# rosrun moveit_jog_arm test_jog_arm_integration.py + +JOG_ARM_SETTLE_TIME_S = 10 +ROS_SETTLE_TIME_S = 10 + +CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' + +# jog_arm should publish 'true' here if it halts +HALT_TOPIC = 'jog_server/halted' + + +@pytest.fixture +def node(): + return rospy.init_node('pytest', anonymous=True) + + +class CartesianJogCmd(object): + def __init__(self): + self._pub = rospy.Publisher( + CARTESIAN_JOG_COMMAND_TOPIC, TwistStamped, queue_size=1 + ) + + def send_cmd(self, linear, angular): + ts = TwistStamped() + ts.header.stamp = rospy.Time.now() + ts.twist.linear.x, ts.twist.linear.y, ts.twist.linear.z = linear + ts.twist.angular.x, ts.twist.angular.y, ts.twist.angular.z = angular + self._pub.publish(ts) + + +def test_jog_arm_halt_msg(node): + sub = rospy.Subscriber( + HALT_TOPIC, Bool, lambda msg: received.append(msg) + ) + cartesian_cmd = CartesianJogCmd() + time.sleep(ROS_SETTLE_TIME_S) # wait for pub/subs to settle + time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for jog_arm server to init + + # This nonzero command should produce jogging output + # A subscriber in a different thread fills `received` + TEST_DURATION = 1 + cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) + received = [] + rospy.sleep(TEST_DURATION) + + # Check the received messages + assert len(received) > 1 + assert received[-1].data == True + + +if __name__ == '__main__': + node = node() + test_jog_arm_halt_msg(node) diff --git a/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py b/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py similarity index 95% rename from moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py rename to moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py index 3c901e0b64..e5cea2f0d4 100755 --- a/moveit_experimental/moveit_jog_arm/test/integration/test_jog_arm_integration.py +++ b/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py @@ -8,6 +8,7 @@ from control_msgs.msg import JointJog from trajectory_msgs.msg import JointTrajectory +# Test that the jogger publishes controller commands when it receives Cartesian or joint commands. # This can be run as part of a pytest, or like a normal ROS executable: # rosrun moveit_jog_arm test_jog_arm_integration.py @@ -51,7 +52,7 @@ def send_cmd(self, linear, angular): self._pub.publish(ts) -def test_jog_arm_trajectory_generated_when_jog_command_is_received(node): +def test_jog_arm_cartesian_command(node): sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) @@ -108,5 +109,5 @@ def test_jog_arm_joint_command(node): if __name__ == '__main__': node = node() - test_jog_arm_trajectory_generated_when_jog_command_is_received(node) + test_jog_arm_cartesian_command(node) test_jog_arm_joint_command(node) From cf5899e87d2190aae5354e7b6d10284cf8cfa054 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 5 Feb 2020 09:47:37 -0700 Subject: [PATCH 180/612] Improve pull request template with hyperlinks (#1879) --- .github/PULL_REQUEST_TEMPLATE.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index e653cc1ef4..fbd1d34b45 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -5,8 +5,8 @@ Please explain the changes you made, including a reference to the related issue ### Checklist - [ ] **Required by CI**: Code is auto formatted using [clang-format](http://moveit.ros.org/documentation/contributing/code) - [ ] Extend the tutorials / documentation [reference](http://moveit.ros.org/documentation/contributing/) -- [ ] Document API changes relevant to the user in the MIGRATION.md notes -- [ ] Create tests, which fail without this PR [reference](https://ros-planning.github.io/moveit_tutorials/) +- [ ] Document API changes relevant to the user in the [MIGRATION.md](https://github.com/ros-planning/moveit/blob/master/MIGRATION.md) notes +- [ ] Create tests, which fail without this PR [reference](https://ros-planning.github.io/moveit_tutorials/doc/tests/tests_tutorial.html) - [ ] Include a screenshot if changing a GUI - [ ] While waiting for someone to review your request, please help review [another open pull request](https://github.com/ros-planning/moveit/pulls) to support the maintainers From b36d5144f4257090470dc167212ccbcde905b82c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 5 Feb 2020 17:48:20 +0100 Subject: [PATCH 181/612] Fix spurious warning message (# IK attempts) (#1876) --- .../kinematics_plugin_loader/src/kinematics_plugin_loader.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index 6756b71f90..2f1e24d15b 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -346,7 +346,8 @@ robot_model::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const s // TODO: Remove in future release (deprecated in PR #1288, Jan-2019, Melodic) std::string ksolver_attempts_param_name; - if (nh.searchParam(base_param_name + "/kinematics_solver_attempts", ksolver_attempts_param_name)) + if (nh.searchParam(base_param_name + "/kinematics_solver_attempts", ksolver_attempts_param_name) && + nh.hasParam(ksolver_attempts_param_name)) { ROS_WARN_ONCE_NAMED(LOGNAME, "Kinematics solver doesn't support #attempts anymore, but only a timeout.\n" "Please remove the parameter '%s' from your configuration.", From 75b79108fd0587d6388a4b10cb960dc8092bde69 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 5 Feb 2020 12:04:28 -0600 Subject: [PATCH 182/612] Always provide the end effector transform, not just while robot is jogging Delete a redundant transform variable Add an isInitialized() function Add a method to temporarily pause message publication Use ros::Rate instead of constant sleeps in while loops Check for an uninitialized transform --- .../include/moveit_jog_arm/jog_arm_data.h | 2 - .../include/moveit_jog_arm/jog_calcs.h | 38 ++- .../moveit_jog_arm/jog_cpp_interface.h | 11 +- .../moveit_jog_arm/src/jog_calcs.cpp | 309 ++++++++---------- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 20 +- .../test_jog_arm_msg_reception.py | 6 +- 6 files changed, 186 insertions(+), 200 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 0e21bd74ac..a37818d44c 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -48,8 +48,6 @@ namespace moveit_jog_arm { -static const double WHILE_LOOP_WAIT = 0.001; - // Variables to share between threads, and their mutexes struct JogArmShared { diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 7a65867b31..54304dd177 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -56,11 +56,24 @@ class JogCalcs void stopMainLoop(); + void haltOutgoingJogCmds(); + + /** \brief Check if the robot state is being updated and the end effector transform is known + * @return true if initialized properly + */ + bool isInitialized(); + protected: ros::NodeHandle nh_; // Loop termination flag - std::atomic stop_requested_; + std::atomic stop_jog_loop_requested_; + + // Flag that outgoing commands to the robot should not be published + std::atomic halt_outgoing_jog_cmds_; + + // Flag that robot state is up to date, end effector transform is known + std::atomic is_initialized_; sensor_msgs::JointState incoming_joints_; @@ -69,7 +82,7 @@ class JogCalcs bool jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables); // Parse the incoming joint msg for the joints of our MoveGroup - bool updateJoints(); + bool updateJoints(std::mutex& mutex, const JogArmShared& shared_variables); Eigen::VectorXd scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const; @@ -77,9 +90,6 @@ class JogCalcs bool addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const; - // Reset the data stored in low-pass filters so the trajectory won't jump when jogging is resumed. - void resetVelocityFilters(); - // Suddenly halt for a joint limit or other critical issue. // Is handled differently for position vs. velocity control. void suddenHalt(trajectory_msgs::JointTrajectory& joint_traj); @@ -98,20 +108,23 @@ class JogCalcs * Slow motion down if close to singularity or collision. * @param shared_variables data shared between threads, tells how close we are to collision * @param mutex locks shared data - * @param new_joint_traj store the motion in the first waypoint of this trajectory * @param delta_theta motion command, used in calculating new_joint_tray * @param singularity_scale tells how close we are to a singularity * @return false if very close to collision or singularity */ - bool applyVelocityScaling(JogArmShared& shared_variables, std::mutex& mutex, - trajectory_msgs::JointTrajectory& new_joint_traj, const Eigen::VectorXd& delta_theta, + void applyVelocityScaling(const JogArmShared& shared_variables, std::mutex& mutex, Eigen::ArrayXd& delta_theta, double singularity_scale); - trajectory_msgs::JointTrajectory composeOutgoingMessage(sensor_msgs::JointState& joint_state) const; + trajectory_msgs::JointTrajectory composeJointTrajMessage(sensor_msgs::JointState& joint_state) const; - void lowPassFilterVelocities(const Eigen::VectorXd& joint_vel); + void lowPassFilterPositions(sensor_msgs::JointState& joint_state); - void lowPassFilterPositions(); + void calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta); + + /** \brief Convert joint deltas to an outgoing JointTrajectory command. + * This happens for joint commands and Cartesian commands. + */ + bool convertDeltasToOutgoingCmd(); void insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& trajectory, int count) const; @@ -123,7 +136,6 @@ class JogCalcs std::map joint_state_name_map_; trajectory_msgs::JointTrajectory outgoing_command_; - std::vector velocity_filters_; std::vector position_filters_; ros::Publisher warning_pub_; @@ -141,5 +153,7 @@ class JogCalcs const int gazebo_redundant_message_count_ = 30; uint num_joints_; + + ros::Rate default_sleep_rate_; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h index e9a070e178..0fb1568ce9 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h @@ -69,9 +69,14 @@ class JogCppApi : protected JogInterfaceBase // May eliminate the need to create your own joint_state subscriber. sensor_msgs::JointState getJointState(); - // Get planning link transform. - // The transform from the MoveIt planning frame to robot_link_command_frame - Eigen::Isometry3d getCommandFrameTransform(); + /** + * Get the MoveIt planning link transform. + * The transform from the MoveIt planning frame to robot_link_command_frame + * + * @param transform the transform that will be calculated + * @return true if a valid transform was available + */ + bool getCommandFrameTransform(Eigen::Isometry3d& transform); private: ros::NodeHandle nh_; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index bcb8675fbe..63b12042da 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -45,7 +45,7 @@ namespace moveit_jog_arm { // Constructor for the class that handles jogging calculations JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) - : parameters_(parameters) + : parameters_(parameters), default_sleep_rate_(1000) { // Publish collision status warning_pub_ = nh_.advertise(parameters_.warning_topic, 1); @@ -54,7 +54,7 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader: while (ros::ok() && !model_loader_ptr) { ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); - ros::Duration(WHILE_LOOP_WAIT).sleep(); + default_sleep_rate_.sleep(); } const robot_model::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); kinematic_state_ = std::make_shared(kinematic_model); @@ -65,8 +65,10 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader: void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) { - // Reset loop termination flag - stop_requested_ = false; + // Reset flags + stop_jog_loop_requested_ = false; + halt_outgoing_jog_cmds_ = false; + is_initialized_ = false; // Wait for initial messages ROS_INFO_NAMED(LOGNAME, "jog_calcs_thread: Waiting for first joint msg."); @@ -87,33 +89,56 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) // Low-pass filters for the joint positions & velocities for (size_t i = 0; i < num_joints_; ++i) { - velocity_filters_.emplace_back(parameters_.low_pass_filter_coeff); position_filters_.emplace_back(parameters_.low_pass_filter_coeff); } // Initialize the position filters to initial robot joints - while (!updateJoints() && ros::ok() && !stop_requested_) + while (!updateJoints(mutex, shared_variables) && ros::ok()) { + if (stop_jog_loop_requested_) + return; + mutex.lock(); incoming_joints_ = shared_variables.joints; mutex.unlock(); - ros::Duration(WHILE_LOOP_WAIT).sleep(); + default_sleep_rate_.sleep(); } - for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(joint_state_.position[i]); + + is_initialized_ = true; // Wait for the first jogging cmd. // Store it in a class member for further calcs, then free up the shared variable again. geometry_msgs::TwistStamped cartesian_deltas; control_msgs::JointJog joint_deltas; - while (ros::ok() && !stop_requested_ && (cartesian_deltas.header.stamp == ros::Time(0.)) && - (joint_deltas.header.stamp == ros::Time(0.))) + while (ros::ok() && (cartesian_deltas.header.stamp == ros::Time(0.)) && (joint_deltas.header.stamp == ros::Time(0.))) { - ros::Duration(WHILE_LOOP_WAIT).sleep(); + if (stop_jog_loop_requested_) + return; + + default_sleep_rate_.sleep(); + // Ensure the low-pass filter matches reality + for (std::size_t i = 0; i < num_joints_; ++i) + position_filters_[i].reset(joint_state_.position[i]); + + // Check for a new command mutex.lock(); cartesian_deltas = shared_variables.command_deltas; joint_deltas = shared_variables.joint_command_deltas; + incoming_joints_ = shared_variables.joints; + mutex.unlock(); + + kinematic_state_->setVariableValues(joint_state_); + + // Always update the end-effector transform in case the getCommandFrameTransform() method is being used + // Get the transform from MoveIt planning frame to jogging command frame + // We solve (planning_frame -> base -> robot_link_command_frame) + // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) + tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); + + mutex.lock(); + shared_variables.tf_moveit_to_cmd_frame = tf_moveit_to_cmd_frame_; mutex.unlock(); } @@ -124,25 +149,16 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) ros::Rate loop_rate(1. / parameters_.publish_period); // Now do jogging calcs - while (ros::ok() && !stop_requested_) + while (ros::ok() && !stop_jog_loop_requested_) { - // Flag that incoming commands are all zero. May be used to skip calculations/publication - mutex.lock(); - bool zero_cartesian_cmd_flag = shared_variables.zero_cartesian_cmd_flag; - bool zero_joint_cmd_flag = shared_variables.zero_joint_cmd_flag; - mutex.unlock(); - - if (zero_cartesian_cmd_flag && zero_joint_cmd_flag) - // Reset low-pass filters - resetVelocityFilters(); - - // Pull data from the shared variables. - mutex.lock(); - incoming_joints_ = shared_variables.joints; - mutex.unlock(); - + // Always update the joints and end-effector transform for 2 reasons: + // 1) in case the getCommandFrameTransform() method is being used + // 2) so the low-pass filters are up to date and don't cause a jump + while (!updateJoints(mutex, shared_variables) && ros::ok()) + { + default_sleep_rate_.sleep(); + } kinematic_state_->setVariableValues(joint_state_); - original_joint_state_ = joint_state_; // Get the transform from MoveIt planning frame to jogging command frame // We solve (planning_frame -> base -> robot_link_command_frame) @@ -153,57 +169,61 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) shared_variables.tf_moveit_to_cmd_frame = tf_moveit_to_cmd_frame_; mutex.unlock(); - // Initialize the position filters to initial robot joints - while (!updateJoints() && ros::ok() && !stop_requested_) + // If paused, just keep the low-pass filters up to date with current joints so a jump doesn't occur when + // restarting + if (halt_outgoing_jog_cmds_) { - mutex.lock(); - incoming_joints_ = shared_variables.joints; - mutex.unlock(); - ros::Duration(WHILE_LOOP_WAIT).sleep(); + for (std::size_t i = 0; i < num_joints_; ++i) + position_filters_[i].reset(joint_state_.position[i]); } - - // Prioritize cartesian jogging above joint jogging - if (!zero_cartesian_cmd_flag) + // Do jogging calculations only if the robot should move, for efficiency + else { + // Flag that incoming commands are all zero. May be used to skip calculations/publication mutex.lock(); - cartesian_deltas = shared_variables.command_deltas; + bool zero_cartesian_cmd_flag = shared_variables.zero_cartesian_cmd_flag; + bool zero_joint_cmd_flag = shared_variables.zero_joint_cmd_flag; mutex.unlock(); - if (!cartesianJogCalcs(cartesian_deltas, shared_variables, mutex)) - continue; - } - else if (!zero_joint_cmd_flag) - { - mutex.lock(); - joint_deltas = shared_variables.joint_command_deltas; - mutex.unlock(); + // Prioritize cartesian jogging above joint jogging + if (!zero_cartesian_cmd_flag) + { + mutex.lock(); + cartesian_deltas = shared_variables.command_deltas; + mutex.unlock(); - if (!jointJogCalcs(joint_deltas, shared_variables)) - continue; - } - else - { - original_joint_state_ = joint_state_; - outgoing_command_ = composeOutgoingMessage(joint_state_); - } + if (!cartesianJogCalcs(cartesian_deltas, shared_variables, mutex)) + continue; + } + else if (!zero_joint_cmd_flag) + { + mutex.lock(); + joint_deltas = shared_variables.joint_command_deltas; + mutex.unlock(); - // Halt if the command is stale or inputs are all zero, or commands were zero - mutex.lock(); - bool stale_command = shared_variables.command_is_stale; - mutex.unlock(); + if (!jointJogCalcs(joint_deltas, shared_variables)) + continue; + } + else + { + outgoing_command_ = composeJointTrajMessage(joint_state_); + } - if (stale_command || (zero_cartesian_cmd_flag && zero_joint_cmd_flag)) - { - suddenHalt(outgoing_command_); - zero_cartesian_cmd_flag = true; - zero_joint_cmd_flag = true; - } + // Halt if the command is stale or inputs are all zero, or commands were zero + mutex.lock(); + bool stale_command = shared_variables.command_is_stale; + mutex.unlock(); - bool valid_nonzero_command = !zero_cartesian_cmd_flag || !zero_joint_cmd_flag; + if (stale_command || (zero_cartesian_cmd_flag && zero_joint_cmd_flag)) + { + suddenHalt(outgoing_command_); + zero_cartesian_cmd_flag = true; + zero_joint_cmd_flag = true; + } - // Send the newest target joints - if (!outgoing_command_.joint_names.empty()) - { + bool valid_nonzero_command = !zero_cartesian_cmd_flag || !zero_joint_cmd_flag; + + // Send the newest target joints mutex.lock(); // If everything normal, share the new traj to be published if (valid_nonzero_command) @@ -217,8 +237,6 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) (zero_velocity_count > parameters_.num_outgoing_halt_msgs_to_publish)) { shared_variables.ok_to_publish = false; - // Reset the velocity filters so robot doesn't jump when commands resume - resetVelocityFilters(); } // The command is invalid but we are publishing num_outgoing_halt_msgs_to_publish else @@ -233,7 +251,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) if (zero_cartesian_cmd_flag && zero_joint_cmd_flag) { // Avoid overflow - if (zero_velocity_count < INT_MAX) + if (zero_velocity_count < std::numeric_limits::max()) ++zero_velocity_count; } else @@ -246,7 +264,17 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) void JogCalcs::stopMainLoop() { - stop_requested_ = true; + stop_jog_loop_requested_ = true; +} + +void JogCalcs::haltOutgoingJogCmds() +{ + halt_outgoing_jog_cmds_ = true; +} + +bool JogCalcs::isInitialized() +{ + return is_initialized_; } // Perform the jogging calculations @@ -277,14 +305,8 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z); Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z); - // We solve (planning_frame -> base -> cmd.header.frame_id) - // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id) - const auto tf_planning_to_cmd_frame = - kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * - kinematic_state_->getGlobalLinkTransform(cmd.header.frame_id); - - translation_vector = tf_planning_to_cmd_frame.linear() * translation_vector; - angular_vector = tf_planning_to_cmd_frame.linear() * angular_vector; + translation_vector = tf_moveit_to_cmd_frame_.linear() * translation_vector; + angular_vector = tf_moveit_to_cmd_frame_.linear() * angular_vector; // Put these components back into a TwistStamped cmd.header.frame_id = parameters_.planning_frame; @@ -305,40 +327,10 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose(); delta_theta_ = pseudo_inverse_ * delta_x; - if (!addJointIncrements(joint_state_, delta_theta_)) - return false; - - // Include a velocity estimate for velocity-controlled robots - Eigen::VectorXd joint_vel(delta_theta_ / parameters_.publish_period); - - lowPassFilterVelocities(joint_vel); - lowPassFilterPositions(); - - outgoing_command_ = composeOutgoingMessage(joint_state_); - - // If close to a collision or a singularity, decelerate and publish a warning - if (!applyVelocityScaling(shared_variables, mutex, outgoing_command_, delta_theta_, - decelerateForSingularity(delta_x, svd_))) - { - suddenHalt(outgoing_command_); - publishWarning(true); - } - // If a joint limit would be exceeded, halt and publish a warning - else if (!enforceSRDFJointBounds(outgoing_command_)) - { - suddenHalt(outgoing_command_); - publishWarning(true); - } - else - publishWarning(false); - - // If using Gazebo simulator, insert redundant points - if (parameters_.use_gazebo) - { - insertRedundantPointsIntoTrajectory(outgoing_command_, gazebo_redundant_message_count_); - } + // If close to a collision or a singularity, decelerate + applyVelocityScaling(shared_variables, mutex, delta_theta_, decelerateForSingularity(delta_x, svd_)); - return true; + return convertDeltasToOutgoingCmd(); } bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& /*shared_variables*/) @@ -354,24 +346,24 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& /* } // Apply user-defined scaling - Eigen::ArrayXd delta = scaleJointCommand(cmd); + delta_theta_ = scaleJointCommand(cmd); kinematic_state_->setVariableValues(joint_state_); - original_joint_state_ = joint_state_; - if (!addJointIncrements(joint_state_, delta)) - return false; + return convertDeltasToOutgoingCmd(); +} - // Include a velocity estimate for velocity-controlled robots - Eigen::VectorXd joint_vel(delta / parameters_.publish_period); +bool JogCalcs::convertDeltasToOutgoingCmd() +{ + if (!addJointIncrements(joint_state_, delta_theta_)) + return false; - lowPassFilterVelocities(joint_vel); - lowPassFilterPositions(); + lowPassFilterPositions(joint_state_); - // update joint state with new values - kinematic_state_->setVariableValues(joint_state_); + // Calculate joint velocities here so that positions are filtered and SRDF bounds still get checked + calculateJointVelocities(joint_state_, delta_theta_); - outgoing_command_ = composeOutgoingMessage(joint_state_); + outgoing_command_ = composeJointTrajMessage(joint_state_); if (!enforceSRDFJointBounds(outgoing_command_)) { @@ -406,38 +398,23 @@ void JogCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTraject } } -void JogCalcs::lowPassFilterPositions() +void JogCalcs::lowPassFilterPositions(sensor_msgs::JointState& joint_state) { - for (size_t i = 0; i < num_joints_; ++i) + for (size_t i = 0; i < position_filters_.size(); ++i) { - joint_state_.position[i] = position_filters_[i].filter(joint_state_.position[i]); - - // Check for nan's - if (std::isnan(joint_state_.position[i])) - { - joint_state_.position[i] = original_joint_state_.position[i]; - joint_state_.velocity[i] = 0.; - } + joint_state.position[i] = position_filters_[i].filter(joint_state.position[i]); } } -void JogCalcs::lowPassFilterVelocities(const Eigen::VectorXd& joint_vel) +void JogCalcs::calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta) { - for (size_t i = 0; i < num_joints_; ++i) + for (int i = 0; i < delta_theta.size(); ++i) { - joint_state_.velocity[i] = velocity_filters_[i].filter(joint_vel[static_cast(i)]); - - // Check for nan's - if (std::isnan(joint_state_.velocity[static_cast(i)])) - { - joint_state_.position[i] = original_joint_state_.position[i]; - joint_state_.velocity[i] = 0.; - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "nan in velocity filter"); - } + joint_state.velocity[i] = delta_theta[i] / parameters_.publish_period; } } -trajectory_msgs::JointTrajectory JogCalcs::composeOutgoingMessage(sensor_msgs::JointState& joint_state) const +trajectory_msgs::JointTrajectory JogCalcs::composeJointTrajMessage(sensor_msgs::JointState& joint_state) const { trajectory_msgs::JointTrajectory new_joint_traj; new_joint_traj.header.frame_id = parameters_.planning_frame; @@ -465,30 +442,14 @@ trajectory_msgs::JointTrajectory JogCalcs::composeOutgoingMessage(sensor_msgs::J // Apply velocity scaling for proximity of collisions and singularities. // Scale for collisions is read from a shared variable. -// Key equation: new_velocity = collision_scale*singularity_scale*previous_velocity -bool JogCalcs::applyVelocityScaling(JogArmShared& shared_variables, std::mutex& mutex, - trajectory_msgs::JointTrajectory& new_joint_traj, - const Eigen::VectorXd& delta_theta, double singularity_scale) +void JogCalcs::applyVelocityScaling(const JogArmShared& shared_variables, std::mutex& mutex, + Eigen::ArrayXd& delta_theta, double singularity_scale) { mutex.lock(); double collision_scale = shared_variables.collision_velocity_scale; mutex.unlock(); - for (size_t i = 0; i < num_joints_; ++i) - { - if (parameters_.publish_joint_positions) - { - // If close to a singularity or joint limit, undo any change to the joint angles - new_joint_traj.points[0].positions[i] = - new_joint_traj.points[0].positions[i] - - (1. - singularity_scale * collision_scale) * delta_theta[static_cast(i)]; - } - if (parameters_.publish_joint_velocities) - new_joint_traj.points[0].velocities[i] *= singularity_scale * collision_scale; - } - - // Heuristic: flag that we are stuck if velocity scaling is < X% - return collision_scale * singularity_scale >= 0.1; + delta_theta = collision_scale * singularity_scale * delta_theta; } // Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion @@ -649,16 +610,13 @@ void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_traj) } } -// Reset the data stored in filters so the trajectory won't jump when jogging is resumed. -void JogCalcs::resetVelocityFilters() -{ - for (std::size_t i = 0; i < num_joints_; ++i) - velocity_filters_[i].reset(0); // Zero velocity -} - // Parse the incoming joint msg for the joints of our MoveGroup -bool JogCalcs::updateJoints() +bool JogCalcs::updateJoints(std::mutex& mutex, const JogArmShared& shared_variables) { + mutex.lock(); + incoming_joints_ = shared_variables.joints; + mutex.unlock(); + // Check that the msg contains enough joints if (incoming_joints_.name.size() < num_joints_) return false; @@ -680,6 +638,9 @@ bool JogCalcs::updateJoints() joint_state_.position[c] = incoming_joints_.position[m]; } + // Cache the original joints in case they need to be reset + original_joint_state_ = joint_state_; + return true; } diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp index 71f6aeb39c..bb6e15ad12 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp @@ -146,7 +146,6 @@ void JogCppApi::startMainLoop() main_rate.sleep(); } - ROS_ERROR("Stopping main loop"); stopJogCalcThread(); stopCollisionCheckThread(); } @@ -213,12 +212,21 @@ sensor_msgs::JointState JogCppApi::getJointState() return current_joints; } -Eigen::Isometry3d JogCppApi::getCommandFrameTransform() +bool JogCppApi::getCommandFrameTransform(Eigen::Isometry3d& transform) { - shared_variables_mutex_.lock(); - Eigen::Isometry3d tf_moveit_to_cmd_frame = shared_variables_.tf_moveit_to_cmd_frame; - shared_variables_mutex_.unlock(); + if (jog_calcs_->isInitialized()) + { + shared_variables_mutex_.lock(); + transform = shared_variables_.tf_moveit_to_cmd_frame; + shared_variables_mutex_.unlock(); + + // All zeros means the transform wasn't initialized, so return false + if (transform.matrix().isZero(0)) + return false; + } + else + return false; - return tf_moveit_to_cmd_frame; + return true; } } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py b/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py index e5cea2f0d4..c80109ffa2 100755 --- a/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py +++ b/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py @@ -68,7 +68,7 @@ def test_jog_arm_cartesian_command(node): cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) received = [] rospy.sleep(1) - assert len(received) <= 2 # 2 is 'num_outgoing_halt_msgs_to_publish' in the config file + assert len(received) <= 4 # 'num_outgoing_halt_msgs_to_publish' in the config file # This nonzero command should produce jogging output # A subscriber in a different thread fills `received` @@ -103,8 +103,8 @@ def test_jog_arm_joint_command(node): rospy.sleep(TEST_DURATION) # TEST_DURATION/PUBLISH_PERIOD is the expected number of messages in this duration. # Allow a small +/- window due to rounding/timing errors - assert len(received) >= TEST_DURATION/PUBLISH_PERIOD - 5 - assert len(received) <= TEST_DURATION/PUBLISH_PERIOD + 5 + assert len(received) >= TEST_DURATION/PUBLISH_PERIOD - 20 + assert len(received) <= TEST_DURATION/PUBLISH_PERIOD + 20 if __name__ == '__main__': From 83728716e0c754e841fa629d477006ed6e8c8434 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 5 Feb 2020 13:00:34 -0600 Subject: [PATCH 183/612] Fix the publishWarning() functionality --- .../include/moveit_jog_arm/jog_calcs.h | 5 ++++- .../moveit_jog_arm/src/jog_calcs.cpp | 20 ++++++++++++------- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 54304dd177..d6d7cf5e54 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -112,7 +112,7 @@ class JogCalcs * @param singularity_scale tells how close we are to a singularity * @return false if very close to collision or singularity */ - void applyVelocityScaling(const JogArmShared& shared_variables, std::mutex& mutex, Eigen::ArrayXd& delta_theta, + bool applyVelocityScaling(const JogArmShared& shared_variables, std::mutex& mutex, Eigen::ArrayXd& delta_theta, double singularity_scale); trajectory_msgs::JointTrajectory composeJointTrajMessage(sensor_msgs::JointState& joint_state) const; @@ -140,6 +140,9 @@ class JogCalcs ros::Publisher warning_pub_; + // Flag that a warning should be published + bool has_warning_ = false; + JogArmParameters parameters_; // For jacobian calculations diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 63b12042da..4913864ceb 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -328,7 +328,11 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& delta_theta_ = pseudo_inverse_ * delta_x; // If close to a collision or a singularity, decelerate - applyVelocityScaling(shared_variables, mutex, delta_theta_, decelerateForSingularity(delta_x, svd_)); + if (!applyVelocityScaling(shared_variables, mutex, delta_theta_, decelerateForSingularity(delta_x, svd_))) + { + has_warning_ = true; + suddenHalt(outgoing_command_); + } return convertDeltasToOutgoingCmd(); } @@ -368,13 +372,12 @@ bool JogCalcs::convertDeltasToOutgoingCmd() if (!enforceSRDFJointBounds(outgoing_command_)) { suddenHalt(outgoing_command_); - publishWarning(true); - } - else - { - publishWarning(false); + has_warning_ = true; } + publishWarning(has_warning_); + has_warning_ = false; + // done with calculations if (parameters_.use_gazebo) { @@ -442,7 +445,7 @@ trajectory_msgs::JointTrajectory JogCalcs::composeJointTrajMessage(sensor_msgs:: // Apply velocity scaling for proximity of collisions and singularities. // Scale for collisions is read from a shared variable. -void JogCalcs::applyVelocityScaling(const JogArmShared& shared_variables, std::mutex& mutex, +bool JogCalcs::applyVelocityScaling(const JogArmShared& shared_variables, std::mutex& mutex, Eigen::ArrayXd& delta_theta, double singularity_scale) { mutex.lock(); @@ -450,6 +453,9 @@ void JogCalcs::applyVelocityScaling(const JogArmShared& shared_variables, std::m mutex.unlock(); delta_theta = collision_scale * singularity_scale * delta_theta; + + // Heuristic: flag that we are stuck if velocity scaling is < X% + return collision_scale * singularity_scale >= 0.1; } // Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion From f6308f405d40a61b1df0126a9bbcc5e238b8efaa Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 5 Feb 2020 19:57:08 +0100 Subject: [PATCH 184/612] Fixup: Use CMAKE_CXX_STANDARD to enforce c++14 for portability (#1607) set(CMAKE_CXX_STANDARD_REQUIRED ON) --- moveit/CMakeLists.txt | 2 +- moveit_core/CMakeLists.txt | 1 + moveit_experimental/moveit_jog_arm/CMakeLists.txt | 2 +- moveit_kinematics/CMakeLists.txt | 1 + .../ikfast_kinematics_plugin/templates/CMakeLists.txt | 1 + moveit_planners/chomp/chomp_interface/CMakeLists.txt | 1 + moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt | 1 + .../chomp/chomp_optimizer_adapter/CMakeLists.txt | 1 + moveit_planners/moveit_planners/CMakeLists.txt | 2 +- moveit_planners/ompl/CMakeLists.txt | 1 + moveit_planners/trajopt/CMakeLists.txt | 6 ++++-- .../moveit_fake_controller_manager/CMakeLists.txt | 1 + moveit_plugins/moveit_plugins/CMakeLists.txt | 2 +- moveit_plugins/moveit_ros_control_interface/CMakeLists.txt | 1 + .../moveit_simple_controller_manager/CMakeLists.txt | 1 + moveit_ros/benchmarks/CMakeLists.txt | 1 + moveit_ros/manipulation/CMakeLists.txt | 1 + moveit_ros/move_group/CMakeLists.txt | 1 + moveit_ros/moveit_ros/CMakeLists.txt | 2 +- moveit_ros/occupancy_map_monitor/CMakeLists.txt | 1 + moveit_ros/perception/CMakeLists.txt | 1 + moveit_ros/planning/CMakeLists.txt | 1 + moveit_ros/planning_interface/CMakeLists.txt | 1 + moveit_ros/robot_interaction/CMakeLists.txt | 1 + moveit_ros/visualization/CMakeLists.txt | 1 + moveit_ros/warehouse/CMakeLists.txt | 1 + moveit_runtime/CMakeLists.txt | 2 +- moveit_setup_assistant/CMakeLists.txt | 1 + 28 files changed, 31 insertions(+), 8 deletions(-) diff --git a/moveit/CMakeLists.txt b/moveit/CMakeLists.txt index 9d8f3239d4..9acc0a9b9e 100644 --- a/moveit/CMakeLists.txt +++ b/moveit/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 5f1ff12c85..4dd4e6be27 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_core) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) # Warnings diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index 8bff931ad1..f83181ee73 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_jog_arm) set(CMAKE_CXX_STANDARD 14) diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index 9383ecfb6c..b422c10827 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/moveit_kinematics/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_kinematics) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt index e4020ed280..f1ca31f341 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(_PACKAGE_NAME_) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) find_package(catkin REQUIRED COMPONENTS diff --git a/moveit_planners/chomp/chomp_interface/CMakeLists.txt b/moveit_planners/chomp/chomp_interface/CMakeLists.txt index a811145448..1c64f232e5 100644 --- a/moveit_planners/chomp/chomp_interface/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_interface/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_planners_chomp) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) # find catkin in isolation so that CATKIN_ENABLE_TESTING is defined diff --git a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt index d02a10255a..cd3e7a29b0 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(chomp_motion_planner) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) find_package(catkin REQUIRED COMPONENTS diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt index 0e69e70585..fd4f0bdd5a 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_chomp_optimizer_adapter) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) find_package(catkin REQUIRED COMPONENTS diff --git a/moveit_planners/moveit_planners/CMakeLists.txt b/moveit_planners/moveit_planners/CMakeLists.txt index e6e2b34540..753bb7b4d9 100644 --- a/moveit_planners/moveit_planners/CMakeLists.txt +++ b/moveit_planners/moveit_planners/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_planners) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_planners/ompl/CMakeLists.txt b/moveit_planners/ompl/CMakeLists.txt index 677f51f1d4..9edbe70b8d 100644 --- a/moveit_planners/ompl/CMakeLists.txt +++ b/moveit_planners/ompl/CMakeLists.txt @@ -3,6 +3,7 @@ project(moveit_planners_ompl) # At least C++11 required for OMPL set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) diff --git a/moveit_planners/trajopt/CMakeLists.txt b/moveit_planners/trajopt/CMakeLists.txt index f93e000d34..ad66fd900c 100644 --- a/moveit_planners/trajopt/CMakeLists.txt +++ b/moveit_planners/trajopt/CMakeLists.txt @@ -1,7 +1,9 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_planners_trajopt) -add_compile_options(-std=c++14) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) diff --git a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt index 7a24177c77..7102c1ec77 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_fake_controller_manager) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) diff --git a/moveit_plugins/moveit_plugins/CMakeLists.txt b/moveit_plugins/moveit_plugins/CMakeLists.txt index 03c972ba5f..9afdc1d9d8 100644 --- a/moveit_plugins/moveit_plugins/CMakeLists.txt +++ b/moveit_plugins/moveit_plugins/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_plugins) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt index c590d3de6d..5b310439ed 100644 --- a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt +++ b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_control_interface) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) find_package(catkin REQUIRED COMPONENTS diff --git a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt index 48c02ea8b5..db6dec30d5 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_simple_controller_manager) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt index b3cd004a94..1a84d206de 100644 --- a/moveit_ros/benchmarks/CMakeLists.txt +++ b/moveit_ros/benchmarks/CMakeLists.txt @@ -4,6 +4,7 @@ project(moveit_ros_benchmarks) set(MOVEIT_LIB_NAME moveit_ros_benchmarks) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) diff --git a/moveit_ros/manipulation/CMakeLists.txt b/moveit_ros/manipulation/CMakeLists.txt index 7173cbb2cc..efab60effe 100644 --- a/moveit_ros/manipulation/CMakeLists.txt +++ b/moveit_ros/manipulation/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_manipulation) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index a3f4015b0d..5775f3b07b 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_move_group) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) diff --git a/moveit_ros/moveit_ros/CMakeLists.txt b/moveit_ros/moveit_ros/CMakeLists.txt index 75cf534418..5425d0c2e5 100644 --- a/moveit_ros/moveit_ros/CMakeLists.txt +++ b/moveit_ros/moveit_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_ros/occupancy_map_monitor/CMakeLists.txt b/moveit_ros/occupancy_map_monitor/CMakeLists.txt index bdb0bae0f1..290a0525ed 100644 --- a/moveit_ros/occupancy_map_monitor/CMakeLists.txt +++ b/moveit_ros/occupancy_map_monitor/CMakeLists.txt @@ -3,6 +3,7 @@ project(moveit_ros_occupancy_map_monitor) set(MOVEIT_LIB_NAME ${PROJECT_NAME}) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) diff --git a/moveit_ros/perception/CMakeLists.txt b/moveit_ros/perception/CMakeLists.txt index 9e203bf937..fe26aadc4b 100644 --- a/moveit_ros/perception/CMakeLists.txt +++ b/moveit_ros/perception/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_perception) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) option(WITH_OPENGL "Build the parts that depend on OpenGL" ON) diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index b3833b5447..c61c9cb6f6 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_planning) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 0eb0f40575..57942ccdf8 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_planning_interface) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) diff --git a/moveit_ros/robot_interaction/CMakeLists.txt b/moveit_ros/robot_interaction/CMakeLists.txt index d18735a809..1c05eee701 100644 --- a/moveit_ros/robot_interaction/CMakeLists.txt +++ b/moveit_ros/robot_interaction/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_robot_interaction) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) diff --git a/moveit_ros/visualization/CMakeLists.txt b/moveit_ros/visualization/CMakeLists.txt index ba60500eb6..572ef94865 100644 --- a/moveit_ros/visualization/CMakeLists.txt +++ b/moveit_ros/visualization/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_visualization) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") diff --git a/moveit_ros/warehouse/CMakeLists.txt b/moveit_ros/warehouse/CMakeLists.txt index 9efe441920..7441199a93 100644 --- a/moveit_ros/warehouse/CMakeLists.txt +++ b/moveit_ros/warehouse/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_warehouse) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) diff --git a/moveit_runtime/CMakeLists.txt b/moveit_runtime/CMakeLists.txt index 18700d6157..0224cde26c 100644 --- a/moveit_runtime/CMakeLists.txt +++ b/moveit_runtime/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_runtime) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/CMakeLists.txt index e70d73f585..5fe318ad31 100644 --- a/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_setup_assistant) set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") From 45346d60b9534a32f6479378954caa031567133f Mon Sep 17 00:00:00 2001 From: Sean Yen Date: Fri, 7 Feb 2020 01:29:18 -0800 Subject: [PATCH 185/612] Fix various build issues on Windows (#1880) --- moveit_core/CMakeLists.txt | 16 +++++++++++----- moveit_core/kinematics_base/CMakeLists.txt | 9 ++++++--- moveit_kinematics/test/CMakeLists.txt | 4 +++- moveit_ros/perception/mesh_filter/CMakeLists.txt | 2 +- 4 files changed, 21 insertions(+), 10 deletions(-) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 4dd4e6be27..f8b6bf2984 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -5,10 +5,12 @@ set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -# Warnings -add_compile_options(-Wall -Wextra - -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual - -Wno-unused-parameter -Wno-unused-function) +if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") + # Enable warnings + add_compile_options(-Wall -Wextra + -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual + -Wno-unused-parameter -Wno-unused-function) +endif() if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") # This too often has false-positives add_compile_options(-Wno-maybe-uninitialized) @@ -20,7 +22,11 @@ if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() -find_package(Boost REQUIRED system filesystem date_time thread iostreams) +# boost::iostreams on Windows depends on boost::zlib +if(WIN32) + set(EXTRA_BOOST_COMPONENTS zlib) +endif() +find_package(Boost REQUIRED system filesystem date_time thread iostreams ${EXTRA_BOOST_COMPONENTS}) find_package(Eigen3 REQUIRED) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") diff --git a/moveit_core/kinematics_base/CMakeLists.txt b/moveit_core/kinematics_base/CMakeLists.txt index 2ec7a736f4..5e0931e401 100644 --- a/moveit_core/kinematics_base/CMakeLists.txt +++ b/moveit_core/kinematics_base/CMakeLists.txt @@ -2,9 +2,12 @@ set(MOVEIT_LIB_NAME moveit_kinematics_base) add_library(${MOVEIT_LIB_NAME} src/kinematics_base.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -# Avoid warnings about deprecated members of KinematicsBase when building KinematicsBase itself. -# TODO: remove when deperecated variables (tip_frame_, search_discretization_) are finally removed. -target_compile_options(${MOVEIT_LIB_NAME} PRIVATE -Wno-deprecated-declarations) + +if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") + # Avoid warnings about deprecated members of KinematicsBase when building KinematicsBase itself. + # TODO: remove when deperecated variables (tip_frame_, search_discretization_) are finally removed. + target_compile_options(${MOVEIT_LIB_NAME} PRIVATE -Wno-deprecated-declarations) +endif() # This line is needed to ensure that messages are done being built before this is built add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) diff --git a/moveit_kinematics/test/CMakeLists.txt b/moveit_kinematics/test/CMakeLists.txt index be739327c6..dd05543291 100644 --- a/moveit_kinematics/test/CMakeLists.txt +++ b/moveit_kinematics/test/CMakeLists.txt @@ -8,7 +8,9 @@ if(CATKIN_ENABLE_TESTING) catkin_add_executable_with_gtest(test_kinematics_plugin test_kinematics_plugin.cpp) target_link_libraries(test_kinematics_plugin ${catkin_LIBRARIES} ${moveit_ros_planning_LIBRARIES} ${xmlrpcpp_LIBRARIES}) - target_compile_options(test_kinematics_plugin PRIVATE -Wno-deprecated-declarations) + if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") + target_compile_options(test_kinematics_plugin PRIVATE -Wno-deprecated-declarations) + endif() set(DEPS DEPENDENCIES test_kinematics_plugin) set(ARGS ARGS ik_plugin:=kdl_kinematics_plugin/KDLKinematicsPlugin) diff --git a/moveit_ros/perception/mesh_filter/CMakeLists.txt b/moveit_ros/perception/mesh_filter/CMakeLists.txt index f08902a474..8d8a00743b 100644 --- a/moveit_ros/perception/mesh_filter/CMakeLists.txt +++ b/moveit_ros/perception/mesh_filter/CMakeLists.txt @@ -9,7 +9,7 @@ add_library(${MOVEIT_LIB_NAME} ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${gl_LIBS} glut GLEW) +target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${gl_LIBS} GLUT::GLUT ${GLEW_LIBRARIES}) if (CATKIN_ENABLE_TESTING) #catkin_lint: ignore_once env_var From 500a236ffe551aac09b11779206209bf17a06048 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 7 Feb 2020 18:23:53 +0100 Subject: [PATCH 186/612] CurrentStateMonitor: Initialize velocity/effort with unset dynamics --- .../planning_scene_monitor/src/current_state_monitor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index 0f4dbe8f20..41d5cbc2bc 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -385,7 +385,7 @@ void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const senso { // update joint velocities if (joint_state->name.size() == joint_state->velocity.size() && - robot_state_.getJointVelocities(jm)[0] != joint_state->velocity[i]) + (!robot_state_.hasVelocities() || robot_state_.getJointVelocities(jm)[0] != joint_state->velocity[i])) { update = true; robot_state_.setJointVelocities(jm, &(joint_state->velocity[i])); @@ -393,7 +393,7 @@ void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const senso // update joint efforts if (joint_state->name.size() == joint_state->effort.size() && - robot_state_.getJointEffort(jm)[0] != joint_state->effort[i]) + (!robot_state_.hasEffort() || robot_state_.getJointEffort(jm)[0] != joint_state->effort[i])) { update = true; robot_state_.setJointEfforts(jm, &(joint_state->effort[i])); From ecec26d1b57aa3b8afc22044cd0837e5a13afe95 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 7 Feb 2020 23:02:39 +0100 Subject: [PATCH 187/612] fix RobotState::copyFrom() Velocity and (acceleration or effort) each contribute one set of variables only. --- moveit_core/robot_state/src/robot_state.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 519df14dce..21fea88a78 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -144,9 +144,9 @@ void RobotState::copyFrom(const RobotState& other) if (dirty_link_transforms_ == robot_model_->getRootJoint()) { // everything is dirty; no point in copying transforms; copy positions, potentially velocity & acceleration - memcpy(position_, other.position_, robot_model_->getVariableCount() * sizeof(double) * - (1 + ((has_velocity_ || has_acceleration_ || has_effort_) ? 1 : 0) + - ((has_acceleration_ || has_effort_) ? 1 : 0))); + memcpy(position_, other.position_, + robot_model_->getVariableCount() * sizeof(double) * + (1 + (has_velocity_ ? 1 : 0) + ((has_acceleration_ || has_effort_) ? 1 : 0))); // and just initialize transforms initTransforms(); } From bd85e7d78e3992b8671d3a1814bcd7eaee2ac076 Mon Sep 17 00:00:00 2001 From: Dale Koenig Date: Thu, 13 Feb 2020 18:31:50 +0900 Subject: [PATCH 188/612] Add error message on failure to initialize occupancy map monitor (#1873) Error when no sensors could be found. --- .../occupancy_map_monitor/src/occupancy_map_monitor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index 482b317963..0cd6e75434 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -174,6 +174,8 @@ void OccupancyMapMonitor::initialize() ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str()); } } + else + ROS_ERROR("Failed to find 3D sensor plugin parameters for octomap generation"); /* advertise a service for loading octomaps from disk */ save_map_srv_ = nh_.advertiseService("save_map", &OccupancyMapMonitor::saveMapCallback, this); @@ -189,8 +191,8 @@ void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater) if (map_updaters_.size() > 1) { mesh_handles_.resize(map_updaters_.size()); - if (map_updaters_.size() == - 2) // when we had one updater only, we passed direcly the transform cache callback to that updater + // when we had one updater only, we passed direcly the transform cache callback to that updater + if (map_updaters_.size() == 2) { map_updaters_[0]->setTransformCacheCallback( boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 0, _1, _2, _3)); From b7767de73aeb728e0e7d1d1eb08aa5686e586f73 Mon Sep 17 00:00:00 2001 From: Dale Koenig Date: Thu, 13 Feb 2020 21:16:41 +0900 Subject: [PATCH 189/612] NAMED logging for moveit_ros_perception (#1897) --- .../src/occupancy_map_monitor.cpp | 51 +++++++++++-------- .../src/occupancy_map_server.cpp | 6 ++- .../src/occupancy_map_updater.cpp | 4 +- .../src/depth_image_octomap_updater.cpp | 34 +++++++------ .../src/lazy_free_space_updater.cpp | 31 ++++++----- .../src/shape_mask.cpp | 14 ++--- .../src/pointcloud_octomap_updater.cpp | 21 ++++---- .../semantic_world/src/semantic_world.cpp | 29 ++++++----- 8 files changed, 107 insertions(+), 83 deletions(-) diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index 0cd6e75434..c87ee47189 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -43,6 +43,8 @@ namespace occupancy_map_monitor { +static const std::string LOGNAME = "occupancy_map_monitor"; + OccupancyMapMonitor::OccupancyMapMonitor(double map_resolution) : map_resolution_(map_resolution), debug_info_(false), mesh_handle_count_(0), nh_("~"), active_(false) { @@ -77,21 +79,26 @@ void OccupancyMapMonitor::initialize() { /* load params from param server */ if (map_resolution_ <= std::numeric_limits::epsilon()) + { if (!nh_.getParam("octomap_resolution", map_resolution_)) { map_resolution_ = 0.1; - ROS_WARN("Resolution not specified for Octomap. Assuming resolution = %g instead", map_resolution_); + ROS_WARN_NAMED(LOGNAME, "Resolution not specified for Octomap. Assuming resolution = %g instead", + map_resolution_); } - ROS_DEBUG("Using resolution = %lf m for building octomap", map_resolution_); + } + ROS_DEBUG_NAMED(LOGNAME, "Using resolution = %lf m for building octomap", map_resolution_); if (map_frame_.empty()) if (!nh_.getParam("octomap_frame", map_frame_)) if (tf_buffer_) - ROS_WARN("No target frame specified for Octomap. No transforms will be applied to received data."); + ROS_WARN_NAMED(LOGNAME, "No target frame specified for Octomap. " + "No transforms will be applied to received data."); if (!tf_buffer_ && !map_frame_.empty()) - ROS_WARN_STREAM("Target frame \"" << map_frame_ << "\" specified but no TF instance (buffer) specified. " - "No transforms will be applied to received data."); + ROS_WARN_STREAM_NAMED(LOGNAME, "Target frame \"" << map_frame_ + << "\" specified but no TF instance (buffer) specified. " + "No transforms will be applied to received data."); tree_.reset(new OccMapTree(map_resolution_)); tree_const_ = tree_; @@ -106,20 +113,20 @@ void OccupancyMapMonitor::initialize() { if (sensor_list[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("Params for octomap updater %d not a struct; ignoring.", i); + ROS_ERROR_NAMED(LOGNAME, "Params for octomap updater %d not a struct; ignoring.", i); continue; } if (!sensor_list[i].hasMember("sensor_plugin")) { - ROS_ERROR("No sensor plugin specified for octomap updater %d; ignoring.", i); + ROS_ERROR_NAMED(LOGNAME, "No sensor plugin specified for octomap updater %d; ignoring.", i); continue; } std::string sensor_plugin = std::string(sensor_list[i]["sensor_plugin"]); if (sensor_plugin.empty() || sensor_plugin[0] == '~') { - ROS_INFO("Skipping octomap updater plugin '%s'", sensor_plugin.c_str()); + ROS_INFO_STREAM_NAMED(LOGNAME, "Skipping octomap updater plugin '" << sensor_plugin << "'"); continue; } @@ -132,7 +139,7 @@ void OccupancyMapMonitor::initialize() } catch (pluginlib::PluginlibException& ex) { - ROS_FATAL_STREAM("Exception while creating octomap updater plugin loader " << ex.what()); + ROS_FATAL_STREAM_NAMED(LOGNAME, "Exception while creating octomap updater plugin loader " << ex.what()); } } @@ -144,22 +151,22 @@ void OccupancyMapMonitor::initialize() } catch (pluginlib::PluginlibException& ex) { - ROS_ERROR_STREAM("Exception while loading octomap updater '" << sensor_plugin << "': " << ex.what() - << std::endl); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Exception while loading octomap updater '" + << sensor_plugin << "': " << ex.what() << std::endl); } if (up) { /* pass the params struct directly in to the updater */ if (!up->setParams(sensor_list[i])) { - ROS_ERROR("Failed to configure updater of type %s", up->getType().c_str()); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to configure updater of type " << up->getType()); continue; } if (!up->initialize()) { - ROS_ERROR("Unable to initialize map updater of type %s (plugin %s)", up->getType().c_str(), - sensor_plugin.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Unable to initialize map updater of type %s (plugin %s)", up->getType().c_str(), + sensor_plugin.c_str()); continue; } @@ -167,15 +174,15 @@ void OccupancyMapMonitor::initialize() } } else - ROS_ERROR("List of sensors must be an array!"); + ROS_ERROR_NAMED(LOGNAME, "List of sensors must be an array!"); } catch (XmlRpc::XmlRpcException& ex) { - ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str()); + ROS_ERROR_STREAM_NAMED(LOGNAME, "XmlRpc Exception: " << ex.getMessage()); } } else - ROS_ERROR("Failed to find 3D sensor plugin parameters for octomap generation"); + ROS_ERROR_NAMED(LOGNAME, "Failed to find 3D sensor plugin parameters for octomap generation"); /* advertise a service for loading octomaps from disk */ save_map_srv_ = nh_.advertiseService("save_map", &OccupancyMapMonitor::saveMapCallback, this); @@ -207,7 +214,7 @@ void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater) updater->setTransformCacheCallback(transform_cache_callback_); } else - ROS_ERROR("NULL updater was specified"); + ROS_ERROR_NAMED(LOGNAME, "NULL updater was specified"); } void OccupancyMapMonitor::publishDebugInformation(bool flag) @@ -283,7 +290,7 @@ bool OccupancyMapMonitor::getShapeTransformCache(std::size_t index, const std::s std::map::const_iterator jt = mesh_handles_[index].find(it.first); if (jt == mesh_handles_[index].end()) { - ROS_ERROR_THROTTLE(1, "Incorrect mapping of mesh handles"); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Incorrect mapping of mesh handles"); return false; } else @@ -301,7 +308,7 @@ bool OccupancyMapMonitor::getShapeTransformCache(std::size_t index, const std::s bool OccupancyMapMonitor::saveMapCallback(moveit_msgs::SaveMap::Request& request, moveit_msgs::SaveMap::Response& response) { - ROS_INFO("Writing map to %s", request.filename.c_str()); + ROS_INFO_STREAM_NAMED(LOGNAME, "Writing map to " << request.filename); tree_->lockRead(); try { @@ -318,7 +325,7 @@ bool OccupancyMapMonitor::saveMapCallback(moveit_msgs::SaveMap::Request& request bool OccupancyMapMonitor::loadMapCallback(moveit_msgs::LoadMap::Request& request, moveit_msgs::LoadMap::Response& response) { - ROS_INFO("Reading map from %s", request.filename.c_str()); + ROS_INFO_STREAM_NAMED(LOGNAME, "Reading map from " << request.filename); /* load the octree from disk */ tree_->lockWrite(); @@ -328,7 +335,7 @@ bool OccupancyMapMonitor::loadMapCallback(moveit_msgs::LoadMap::Request& request } catch (...) { - ROS_ERROR("Failed to load map from file"); + ROS_ERROR_NAMED(LOGNAME, "Failed to load map from file"); response.success = false; } tree_->unlockWrite(); diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp index 4453583fc1..b91b591d07 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp @@ -40,6 +40,8 @@ #include #include +static const std::string LOGNAME = "occupancy_map_server"; + static void publishOctomap(ros::Publisher* octree_binary_pub, occupancy_map_monitor::OccupancyMapMonitor* server) { octomap_msgs::Octomap map; @@ -51,11 +53,11 @@ static void publishOctomap(ros::Publisher* octree_binary_pub, occupancy_map_moni try { if (!octomap_msgs::binaryMapToMsgData(*server->getOcTreePtr(), map.data)) - ROS_ERROR_THROTTLE(1, "Could not generate OctoMap message"); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Could not generate OctoMap message"); } catch (...) { - ROS_ERROR_THROTTLE(1, "Exception thrown while generating OctoMap message"); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Exception thrown while generating OctoMap message"); } server->getOcTreePtr()->unlockRead(); diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp index bc7a78e095..0e44f7d587 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp @@ -39,6 +39,8 @@ namespace occupancy_map_monitor { +static const std::string LOGNAME = "occupancy_map_monitor"; + OccupancyMapUpdater::OccupancyMapUpdater(const std::string& type) : type_(type) { } @@ -75,7 +77,7 @@ bool OccupancyMapUpdater::updateTransformCache(const std::string& target_frame, return transform_provider_callback_(target_frame, target_time, transform_cache_); else { - ROS_WARN_THROTTLE(1, "No callback provided for updating the transform cache for octomap updaters"); + ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "No callback provided for updating the transform cache for octomap updaters"); return false; } } diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index 9d1840a118..c2da776a0c 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -48,6 +48,8 @@ namespace occupancy_map_monitor { +static const std::string LOGNAME = "depth_image_octomap_updater"; + DepthImageOctomapUpdater::DepthImageOctomapUpdater() : OccupancyMapUpdater("DepthImageUpdater") , nh_("~") @@ -106,7 +108,7 @@ bool DepthImageOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params) } catch (XmlRpc::XmlRpcException& ex) { - ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str()); + ROS_ERROR_STREAM_NAMED(LOGNAME, "XmlRpc Exception: " << ex.getMessage()); return false; } @@ -171,7 +173,7 @@ mesh_filter::MeshHandle DepthImageOctomapUpdater::excludeShape(const shapes::Sha } } else - ROS_ERROR("Mesh filter not yet initialized!"); + ROS_ERROR_NAMED(LOGNAME, "Mesh filter not yet initialized!"); return h; } @@ -186,7 +188,7 @@ bool DepthImageOctomapUpdater::getShapeTransform(mesh_filter::MeshHandle h, Eige ShapeTransformCache::const_iterator it = transform_cache_.find(h); if (it == transform_cache_.end()) { - ROS_ERROR("Internal error. Mesh filter handle %u not found", h); + ROS_ERROR_NAMED(LOGNAME, "Internal error. Mesh filter handle %u not found", h); return false; } transform = it->second; @@ -211,8 +213,8 @@ static const bool HOST_IS_BIG_ENDIAN = host_is_big_endian(); void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::CameraInfoConstPtr& info_msg) { - ROS_DEBUG("Received a new depth image message (frame = '%s', encoding='%s')", depth_msg->header.frame_id.c_str(), - depth_msg->encoding.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "Received a new depth image message (frame = '%s', encoding='%s')", + depth_msg->header.frame_id.c_str(), depth_msg->encoding.c_str()); ros::WallTime start = ros::WallTime::now(); if (max_update_rate_ > 0) @@ -289,11 +291,12 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP { failed_tf_++; if (failed_tf_ > good_tf_) - ROS_WARN_THROTTLE(1, "More than half of the image messages discared due to TF being unavailable (%u%%). " - "Transform error of sensor data: %s; quitting callback.", - (100 * failed_tf_) / (good_tf_ + failed_tf_), err.c_str()); + ROS_WARN_THROTTLE_NAMED(1, LOGNAME, + "More than half of the image messages discared due to TF being unavailable (%u%%). " + "Transform error of sensor data: %s; quitting callback.", + (100 * failed_tf_) / (good_tf_ + failed_tf_), err.c_str()); else - ROS_DEBUG_THROTTLE(1, "Transform error of sensor data: %s; quitting callback", err.c_str()); + ROS_DEBUG_THROTTLE_NAMED(1, LOGNAME, "Transform error of sensor data: %s; quitting callback", err.c_str()); if (failed_tf_ > MAX_TF_COUNTER) { const unsigned int div = MAX_TF_COUNTER / 10; @@ -309,12 +312,12 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP if (!updateTransformCache(depth_msg->header.frame_id, depth_msg->header.stamp)) { - ROS_ERROR_THROTTLE(1, "Transform cache was not updated. Self-filtering may fail."); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform cache was not updated. Self-filtering may fail."); return; } if (depth_msg->is_bigendian && !HOST_IS_BIG_ENDIAN) - ROS_ERROR_THROTTLE(1, "endian problem: received image data does not match host"); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "endian problem: received image data does not match host"); const int w = depth_msg->width; const int h = depth_msg->height; @@ -331,7 +334,8 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP { if (depth_msg->encoding != sensor_msgs::image_encodings::TYPE_32FC1) { - ROS_ERROR_THROTTLE(1, "Unexpected encoding type: '%s'. Ignoring input.", depth_msg->encoding.c_str()); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Unexpected encoding type: '%s'. Ignoring input.", + depth_msg->encoding.c_str()); return; } mesh_filter_->filter(&depth_msg->data[0], GL_FLOAT); @@ -523,7 +527,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP catch (...) { tree_->unlockRead(); - ROS_ERROR("Internal error while parsing depth data"); + ROS_ERROR_NAMED(LOGNAME, "Internal error while parsing depth data"); return; } tree_->unlockRead(); @@ -542,7 +546,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP } catch (...) { - ROS_ERROR("Internal error while updating octree"); + ROS_ERROR_NAMED(LOGNAME, "Internal error while updating octree"); } tree_->unlockWrite(); tree_->triggerUpdateCallback(); @@ -550,6 +554,6 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP // at this point we still have not freed the space free_space_updater_->pushLazyUpdate(occupied_cells_ptr, model_cells_ptr, sensor_origin); - ROS_DEBUG("Processed depth image in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); + ROS_DEBUG_NAMED(LOGNAME, "Processed depth image in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); } } // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp index 5b9cde2bc5..cc033993ae 100644 --- a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp +++ b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp @@ -39,13 +39,14 @@ namespace occupancy_map_monitor { +static const std::string LOGNAME = "lazy_free_space_updater"; + LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const OccMapTreePtr& tree, unsigned int max_batch_size) : tree_(tree) , running_(true) , max_batch_size_(max_batch_size) - , max_sensor_delta_(1e-3) - , // 1mm - process_occupied_cells_set_(nullptr) + , max_sensor_delta_(1e-3) // 1mm + , process_occupied_cells_set_(nullptr) , process_model_cells_set_(nullptr) , update_thread_(boost::bind(&LazyFreeSpaceUpdater::lazyUpdateThread, this)) , process_thread_(boost::bind(&LazyFreeSpaceUpdater::processThread, this)) @@ -70,8 +71,8 @@ LazyFreeSpaceUpdater::~LazyFreeSpaceUpdater() void LazyFreeSpaceUpdater::pushLazyUpdate(octomap::KeySet* occupied_cells, octomap::KeySet* model_cells, const octomap::point3d& sensor_origin) { - ROS_DEBUG("Pushing %lu occupied cells and %lu model cells for lazy updating...", - (long unsigned int)occupied_cells->size(), (long unsigned int)model_cells->size()); + ROS_DEBUG_NAMED(LOGNAME, "Pushing %lu occupied cells and %lu model cells for lazy updating...", + (long unsigned int)occupied_cells->size(), (long unsigned int)model_cells->size()); boost::mutex::scoped_lock _(update_cell_sets_lock_); occupied_cells_sets_.push_back(occupied_cells); model_cells_sets_.push_back(model_cells); @@ -95,7 +96,7 @@ void LazyFreeSpaceUpdater::pushBatchToProcess(OcTreeKeyCountMap* occupied_cells, } else { - ROS_WARN("Previous batch update did not complete. Ignoring set of cells to be freed."); + ROS_WARN_NAMED(LOGNAME, "Previous batch update did not complete. Ignoring set of cells to be freed."); delete occupied_cells; delete model_cells; } @@ -121,9 +122,9 @@ void LazyFreeSpaceUpdater::processThread() if (!running_) break; - ROS_DEBUG("Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells", - (long unsigned int)process_occupied_cells_set_->size(), - (long unsigned int)process_model_cells_set_->size()); + ROS_DEBUG_NAMED( + LOGNAME, "Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells", + (long unsigned int)process_occupied_cells_set_->size(), (long unsigned int)process_model_cells_set_->size()); ros::WallTime start = ros::WallTime::now(); tree_->lockRead(); @@ -162,7 +163,8 @@ void LazyFreeSpaceUpdater::processThread() free_cells1.erase(it); free_cells2.erase(it); } - ROS_DEBUG("Marking %lu cells as free...", (long unsigned int)(free_cells1.size() + free_cells2.size())); + ROS_DEBUG_NAMED(LOGNAME, "Marking %lu cells as free...", + (long unsigned int)(free_cells1.size() + free_cells2.size())); tree_->lockWrite(); @@ -180,12 +182,12 @@ void LazyFreeSpaceUpdater::processThread() } catch (...) { - ROS_ERROR("Internal error while updating octree"); + ROS_ERROR_NAMED(LOGNAME, "Internal error while updating octree"); } tree_->unlockWrite(); tree_->triggerUpdateCallback(); - ROS_DEBUG("Marked free cells in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); + ROS_DEBUG_NAMED(LOGNAME, "Marked free cells in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); delete process_occupied_cells_set_; process_occupied_cells_set_ = nullptr; @@ -229,7 +231,8 @@ void LazyFreeSpaceUpdater::lazyUpdateThread() { if ((sensor_origins_.front() - sensor_origin).norm() > max_sensor_delta_) { - ROS_DEBUG("Pushing %u sets of occupied/model cells to free cells update thread (origin changed)", batch_size); + ROS_DEBUG_NAMED(LOGNAME, "Pushing %u sets of occupied/model cells to free cells update thread (origin changed)", + batch_size); pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin); batch_size = 0; break; @@ -250,7 +253,7 @@ void LazyFreeSpaceUpdater::lazyUpdateThread() if (batch_size >= max_batch_size_) { - ROS_DEBUG("Pushing %u sets of occupied/model cells to free cells update thread", batch_size); + ROS_DEBUG_NAMED(LOGNAME, "Pushing %u sets of occupied/model cells to free cells update thread", batch_size); pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin); occupied_cells_set = nullptr; batch_size = 0; diff --git a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp index 6013d0ffc5..1afaeee43c 100644 --- a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp +++ b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp @@ -39,6 +39,8 @@ #include #include +static const std::string LOGNAME = "shape_mask"; + point_containment_filter::ShapeMask::ShapeMask(const TransformCallback& transform_callback) : transform_callback_(transform_callback), next_handle_(1), min_handle_(1) { @@ -76,7 +78,7 @@ point_containment_filter::ShapeHandle point_containment_filter::ShapeMask::addSh ss.handle = next_handle_; std::pair::iterator, bool> insert_op = bodies_.insert(ss); if (!insert_op.second) - ROS_ERROR("Internal error in management of bodies in ShapeMask. This is a serious error."); + ROS_ERROR_NAMED(LOGNAME, "Internal error in management of bodies in ShapeMask. This is a serious error."); used_handles_[next_handle_] = insert_op.first; } else @@ -107,7 +109,7 @@ void point_containment_filter::ShapeMask::removeShape(ShapeHandle handle) min_handle_ = handle; } else - ROS_ERROR("Unable to remove shape handle %u", handle); + ROS_ERROR_NAMED(LOGNAME, "Unable to remove shape handle %u", handle); } void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::PointCloud2& data_in, @@ -131,11 +133,11 @@ void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::Poi if (!transform_callback_(it->handle, tmp)) { if (!it->body) - ROS_ERROR_STREAM_NAMED("shape_mask", "Missing transform for shape with handle " << it->handle - << " without a body"); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Missing transform for shape with handle " << it->handle + << " without a body"); else - ROS_ERROR_STREAM_NAMED("shape_mask", "Missing transform for shape " << it->body->getType() << " with handle " - << it->handle); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Missing transform for shape " << it->body->getType() << " with handle " + << it->handle); } else { diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index 1fd300ed75..2c92ae43b9 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -47,6 +47,7 @@ namespace occupancy_map_monitor { +static const std::string LOGNAME = "occupancy_map_monitor"; PointCloudOctomapUpdater::PointCloudOctomapUpdater() : OccupancyMapUpdater("PointCloudUpdater") , private_nh_("~") @@ -84,7 +85,7 @@ bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params) } catch (XmlRpc::XmlRpcException& ex) { - ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str()); + ROS_ERROR_STREAM_NAMED(LOGNAME, "XmlRpc Exception: " << ex.getMessage()); return false; } @@ -113,13 +114,13 @@ void PointCloudOctomapUpdater::start() point_cloud_filter_ = new tf2_ros::MessageFilter(*point_cloud_subscriber_, *tf_buffer_, monitor_->getMapFrame(), 5, root_nh_); point_cloud_filter_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1)); - ROS_INFO("Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), - point_cloud_filter_->getTargetFramesString().c_str()); + ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), + point_cloud_filter_->getTargetFramesString().c_str()); } else { point_cloud_subscriber_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1)); - ROS_INFO("Listening to '%s'", point_cloud_topic_.c_str()); + ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", point_cloud_topic_.c_str()); } } @@ -142,7 +143,7 @@ ShapeHandle PointCloudOctomapUpdater::excludeShape(const shapes::ShapeConstPtr& if (shape_mask_) h = shape_mask_->addShape(shape, scale_, padding_); else - ROS_ERROR("Shape filter not yet initialized!"); + ROS_ERROR_NAMED(LOGNAME, "Shape filter not yet initialized!"); return h; } @@ -169,7 +170,7 @@ void PointCloudOctomapUpdater::updateMask(const sensor_msgs::PointCloud2& /*clou void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) { - ROS_DEBUG("Received a new point cloud message"); + ROS_DEBUG_NAMED(LOGNAME, "Received a new point cloud message"); ros::WallTime start = ros::WallTime::now(); if (max_update_rate_ > 0) @@ -199,7 +200,7 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2:: } catch (tf2::TransformException& ex) { - ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << "; quitting callback"); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Transform error of sensor data: " << ex.what() << "; quitting callback"); return; } } @@ -214,7 +215,7 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2:: if (!updateTransformCache(cloud_msg->header.frame_id, cloud_msg->header.stamp)) { - ROS_ERROR_THROTTLE(1, "Transform cache was not updated. Self-filtering may fail."); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform cache was not updated. Self-filtering may fail."); return; } @@ -346,10 +347,10 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2:: } catch (...) { - ROS_ERROR("Internal error while updating octree"); + ROS_ERROR_NAMED(LOGNAME, "Internal error while updating octree"); } tree_->unlockWrite(); - ROS_DEBUG("Processed point cloud in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); + ROS_DEBUG_NAMED(LOGNAME, "Processed point cloud in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); tree_->triggerUpdateCallback(); if (filtered_cloud) diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp index 5407d287b7..dec19c206b 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -54,6 +54,8 @@ namespace moveit { namespace semantic_world { +static const std::string LOGNAME = "semantic_world"; + SemanticWorld::SemanticWorld(const planning_scene::PlanningSceneConstPtr& planning_scene) : planning_scene_(planning_scene) { @@ -66,7 +68,7 @@ SemanticWorld::SemanticWorld(const planning_scene::PlanningSceneConstPtr& planni visualization_msgs::MarkerArray SemanticWorld::getPlaceLocationsMarker(const std::vector& poses) const { - ROS_DEBUG("Visualizing: %d place poses", (int)poses.size()); + ROS_DEBUG_NAMED(LOGNAME, "Visualizing: %d place poses", (int)poses.size()); visualization_msgs::MarkerArray marker; for (std::size_t i = 0; i < poses.size(); ++i) { @@ -224,7 +226,7 @@ SemanticWorld::generatePlacePoses(const std::string& table_name, const shapes::S } std::vector place_poses; - ROS_ERROR("Did not find table %s to place on", table_name.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Did not find table %s to place on", table_name.c_str()); return place_poses; } @@ -347,7 +349,7 @@ std::vector SemanticWorld::generatePlacePoses(const unsigned int num_x = fabs(x_max - x_min) / resolution + 1; unsigned int num_y = fabs(y_max - y_min) / resolution + 1; - ROS_DEBUG("Num points for possible place operations: %d %d", num_x, num_y); + ROS_DEBUG_NAMED(LOGNAME, "Num points for possible place operations: %d %d", num_x, num_y); std::vector > contours; std::vector hierarchy; @@ -434,7 +436,7 @@ bool SemanticWorld::isInsideTableContour(const geometry_msgs::Pose& pose, const // Assuming Z axis points upwards for the table if (point.z() < -fabs(min_vertical_offset)) { - ROS_ERROR("Object is not above table"); + ROS_ERROR_NAMED(LOGNAME, "Object is not above table"); return false; } @@ -442,7 +444,7 @@ bool SemanticWorld::isInsideTableContour(const geometry_msgs::Pose& pose, const int point_y = (point.y() - y_min) * scale_factor; cv::Point2f point2f(point_x, point_y); double result = cv::pointPolygonTest(contours[0], point2f, true); - ROS_DEBUG("table distance: %f", result); + ROS_DEBUG_NAMED(LOGNAME, "table distance: %f", result); return (int)result >= (int)(min_distance_from_edge * scale_factor); } @@ -453,7 +455,7 @@ std::string SemanticWorld::findObjectTable(const geometry_msgs::Pose& pose, doub std::map::const_iterator it; for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it) { - ROS_DEBUG("Testing table: %s", it->first.c_str()); + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Testing table: " << it->first); if (isInsideTableContour(pose, it->second, min_distance_from_edge, min_vertical_offset)) return it->first; } @@ -463,12 +465,12 @@ std::string SemanticWorld::findObjectTable(const geometry_msgs::Pose& pose, doub void SemanticWorld::tableCallback(const object_recognition_msgs::TableArrayPtr& msg) { table_array_ = *msg; - ROS_INFO("Table callback with %d tables", (int)table_array_.tables.size()); + ROS_INFO_NAMED(LOGNAME, "Table callback with %d tables", (int)table_array_.tables.size()); transformTableArray(table_array_); // Callback on an update if (table_callback_) { - ROS_INFO("Calling table callback"); + ROS_INFO_NAMED(LOGNAME, "Calling table callback"); table_callback_(); } } @@ -480,8 +482,8 @@ void SemanticWorld::transformTableArray(object_recognition_msgs::TableArray& tab std::string original_frame = table.header.frame_id; if (table.convex_hull.empty()) continue; - ROS_INFO_STREAM("Original pose: " << table.pose.position.x << "," << table.pose.position.y << "," - << table.pose.position.z); + ROS_INFO_STREAM_NAMED(LOGNAME, "Original pose: " << table.pose.position.x << "," << table.pose.position.y << "," + << table.pose.position.z); std::string error_text; const Eigen::Isometry3d& original_transform = planning_scene_->getTransforms().getTransform(original_frame); Eigen::Isometry3d original_pose; @@ -489,9 +491,10 @@ void SemanticWorld::transformTableArray(object_recognition_msgs::TableArray& tab original_pose = original_transform * original_pose; table.pose = tf2::toMsg(original_pose); table.header.frame_id = planning_scene_->getTransforms().getTargetFrame(); - ROS_INFO_STREAM("Successfully transformed table array from " << original_frame << "to " << table.header.frame_id); - ROS_INFO_STREAM("Transformed pose: " << table.pose.position.x << "," << table.pose.position.y << "," - << table.pose.position.z); + ROS_INFO_STREAM_NAMED(LOGNAME, "Successfully transformed table array from " << original_frame << "to " + << table.header.frame_id); + ROS_INFO_STREAM_NAMED(LOGNAME, "Transformed pose: " << table.pose.position.x << "," << table.pose.position.y << "," + << table.pose.position.z); } } From 5e218e73b2e9ddaf9bdb470c030bb008aba8e480 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 13 Feb 2020 11:41:32 +0100 Subject: [PATCH 190/612] Revert #1772 Even after reverting, I cannot reproduce the issue mentioned in #1772 anymore. Reverting fixes https://github.com/ros-planning/moveit/pull/1894#issuecomment-585417830. --- .../src/motion_planning_display.cpp | 2 +- .../src/motion_planning_frame_planning.cpp | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 2d6285fd22..06d40e9fe8 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -1143,7 +1143,7 @@ void MotionPlanningDisplay::onRobotModelLoaded() query_robot_start_->load(*getRobotModel()->getURDF()); query_robot_goal_->load(*getRobotModel()->getURDF()); - // initialize previous state to current state + // initialize previous state, start state, and goal state to current state previous_state_ = std::make_shared(getPlanningSceneRO()->getCurrentState()); query_start_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "start", *previous_state_, planning_scene_monitor_->getTFClient())); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 1e34a25338..55006806df 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -172,9 +172,6 @@ void MotionPlanningFrame::computePlanButtonClicked() ui_->result_label->setText("Planning..."); configureForPlanning(); - // move_group node uses an empty start state to refer to the most recent current state - if (ui_->start_state_combo_box->currentText() == "") - move_group_->setStartStateToCurrentState(); planning_display_->rememberPreviousStartState(); bool success = (ui_->use_cartesian_path->isEnabled() && ui_->use_cartesian_path->checkState()) ? computeCartesianPlan() : From 49e09969c4dfc0be1c331315159ba62f7aeebf8a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Thu, 13 Feb 2020 21:10:00 +0100 Subject: [PATCH 191/612] Remove object from collision world only once (#1900) --- moveit_core/planning_scene/src/planning_scene.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index ed8a94dac9..ad8b363960 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1453,8 +1453,6 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache // Extract the shapes from the world shapes = obj_in_world->shapes_; poses = obj_in_world->shape_poses_; - // Remove the object from the collision world - world_->removeObject(object.object.id); // Transform shape poses to the link frame const Eigen::Isometry3d& inv_transform = robot_state_->getGlobalLinkTransform(link_model).inverse(); From 15cb1a2dcc0ef61f87966d42c7f7520c9f1b6f89 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 14 Feb 2020 04:35:36 -0700 Subject: [PATCH 192/612] Add warnings to moveit_jog_arm low pass filter (#1872) --- .../include/moveit_jog_arm/low_pass_filter.h | 17 +++++--- .../moveit_jog_arm/src/low_pass_filter.cpp | 41 +++++++++++++++---- 2 files changed, 45 insertions(+), 13 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h index 1c753cf11e..6d5d6ba2b0 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h @@ -38,6 +38,8 @@ #pragma once +#include + namespace moveit_jog_arm { /** @@ -49,16 +51,19 @@ namespace moveit_jog_arm class LowPassFilter { public: + // Larger filter_coeff-> more smoothing of jog commands, but more lag. + // Rough plot, with cutoff frequency on the y-axis: + // https://www.wolframalpha.com/input/?i=plot+arccot(c) explicit LowPassFilter(double low_pass_filter_coeff); double filter(double new_measurement); void reset(double data); private: - double previous_measurements_[2] = { 0., 0. }; - double previous_filtered_measurement_ = 0.; - // Larger filter_coeff-> more smoothing of jog commands, but more lag. - // Rough plot, with cutoff frequency on the y-axis: - // https://www.wolframalpha.com/input/?i=plot+arccot(c) - double filter_coeff_ = 10.; + static constexpr std::size_t FILTER_LENGTH = 2; + double previous_measurements_[FILTER_LENGTH]; + double previous_filtered_measurement_; + // Scale and feedback term are calculated from supplied filter coefficient + const double scale_term_; + const double feedback_term_; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp b/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp index 536d75deeb..4c74829bbc 100644 --- a/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp +++ b/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp @@ -38,15 +38,42 @@ */ #include +#include #include - -static const std::string LOGNAME = "low_pass_filter"; +#include namespace moveit_jog_arm { +namespace +{ +constexpr char LOGNAME[] = "low_pass_filter"; +constexpr double EPSILON = 1e-9; +} + LowPassFilter::LowPassFilter(double low_pass_filter_coeff) + : previous_measurements_{ 0., 0. } + , previous_filtered_measurement_(0.) + , scale_term_(1. / (1. + low_pass_filter_coeff)) + , feedback_term_(1. - low_pass_filter_coeff) { - filter_coeff_ = low_pass_filter_coeff; + // guarantee this doesn't change because the logic below depends on this length implicity + static_assert(LowPassFilter::FILTER_LENGTH == 2, "moveit_jog_arm::LowPassFilter::FILTER_LENGTH should be 2"); + + ROS_ASSERT_MSG(!std::isinf(feedback_term_), "%s: outputs from filter will be inf because feedback term is inf", + LOGNAME); + ROS_ASSERT_MSG(!std::isinf(scale_term_), "%s: outputs from filter will be inf because denominator of scale is 0", + LOGNAME); + ROS_ASSERT_MSG(low_pass_filter_coeff >= 1., "%s: Filter coefficient < 1. makes the lowpass filter unstable", LOGNAME); + + if (std::abs(feedback_term_) < EPSILON) + { + ROS_WARN_STREAM_NAMED( + LOGNAME, "Filter coefficient value of " + << low_pass_filter_coeff + << " resulted in feedback term of 0. " + " This results in a window averaging Finite Impulse Response (FIR) filter with a gain of " + << scale_term_ * LowPassFilter::FILTER_LENGTH); + } } void LowPassFilter::reset(double data) @@ -63,12 +90,12 @@ double LowPassFilter::filter(double new_measurement) previous_measurements_[1] = previous_measurements_[0]; previous_measurements_[0] = new_measurement; - double new_filtered_msrmt = (1. / (1. + filter_coeff_)) * (previous_measurements_[1] + previous_measurements_[0] - - (-filter_coeff_ + 1.) * previous_filtered_measurement_); + double new_filtered_measurement = scale_term_ * (previous_measurements_[1] + previous_measurements_[0] - + feedback_term_ * previous_filtered_measurement_); // Store the new filtered measurement - previous_filtered_measurement_ = new_filtered_msrmt; + previous_filtered_measurement_ = new_filtered_measurement; - return new_filtered_msrmt; + return new_filtered_measurement; } } // namespace moveit_jog_arm From 2de8471b8ffa8ae2be907c3d820b4144b87c2f4b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 15 Feb 2020 10:17:12 +0100 Subject: [PATCH 193/612] replace panda_moveit_config -> moveit_resources/panda_moveit_config --- moveit_planners/trajopt/test/trajectory_test.test | 2 +- moveit_ros/benchmarks/examples/demo_panda.launch | 6 +++--- .../benchmarks/examples/demo_panda_all_planners.launch | 10 +++++----- .../examples/demo_panda_all_planners_obstacles.launch | 10 +++++----- .../examples/demo_panda_predefined_poses.launch | 10 +++++----- .../planning/launch/collision_checker_compare.launch | 6 +++--- .../planning_interface/test/moveit_cpp_test.test | 8 ++++---- .../launch/run_benchmark_trajopt.launch | 8 ++++---- 8 files changed, 30 insertions(+), 30 deletions(-) diff --git a/moveit_planners/trajopt/test/trajectory_test.test b/moveit_planners/trajopt/test/trajectory_test.test index 36314b49b5..347b97a1d9 100644 --- a/moveit_planners/trajopt/test/trajectory_test.test +++ b/moveit_planners/trajopt/test/trajectory_test.test @@ -3,7 +3,7 @@ - + diff --git a/moveit_ros/benchmarks/examples/demo_panda.launch b/moveit_ros/benchmarks/examples/demo_panda.launch index 66742658e5..07ab802d6f 100644 --- a/moveit_ros/benchmarks/examples/demo_panda.launch +++ b/moveit_ros/benchmarks/examples/demo_panda.launch @@ -3,17 +3,17 @@ - + - + - + diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch b/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch index 21b2ad58a8..adbbed130f 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch @@ -3,24 +3,24 @@ - + - + - + - + - + diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch b/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch index 763d16e86a..1ae08adebf 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch @@ -3,24 +3,24 @@ - + - + - + - + - + diff --git a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch index 657fad0a6f..91fb0448c5 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch +++ b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch @@ -3,24 +3,24 @@ - + - + - + - + - + diff --git a/moveit_ros/planning/launch/collision_checker_compare.launch b/moveit_ros/planning/launch/collision_checker_compare.launch index 3be9255091..5208ade9cd 100644 --- a/moveit_ros/planning/launch/collision_checker_compare.launch +++ b/moveit_ros/planning/launch/collision_checker_compare.launch @@ -4,13 +4,13 @@ - + - + - + diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.test b/moveit_ros/planning_interface/test/moveit_cpp_test.test index 2e3d327b6a..b6e5f894ef 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.test +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.test @@ -3,18 +3,18 @@ - + - - + - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch index 7185681292..d411006e39 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch @@ -4,19 +4,19 @@ - + - + - - + + From 362a9ef110b104616a16c609133884355d5f8004 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 15 Feb 2020 10:24:11 +0100 Subject: [PATCH 194/612] fix MSA template: use GENERATED_PACKAGE_NAME placeholder --- .../launch/trajopt_planning_pipeline.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml index cf5735429d..3bba189470 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml @@ -17,6 +17,6 @@ - + From a8a3353f1bf611e189826bd572ba1cfb279de489 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 15 Feb 2020 10:26:53 +0100 Subject: [PATCH 195/612] moveit.rosinstall: add panda_moveit_config panda_moveit_config is a dependency of moveit_tutorials. If not listed here as well, panda_moveit_config will be installed from debian packages via rosdep, which in turn will pull in many MoveIt packages as well, which is usually not desired. --- moveit.rosinstall | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/moveit.rosinstall b/moveit.rosinstall index cca53db4e4..fc82f3ab95 100644 --- a/moveit.rosinstall +++ b/moveit.rosinstall @@ -24,3 +24,7 @@ local-name: moveit_tutorials uri: https://github.com/ros-planning/moveit_tutorials.git version: master +- git: + local-name: panda_moveit_config + uri: https://github.com/ros-planning/panda_moveit_config.git + version: melodic-devel From cfd1f499d7f24afe31bed034c249c17de34e52a1 Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Sat, 15 Feb 2020 21:05:31 +0100 Subject: [PATCH 196/612] occupancy_map_monitor: add missing dependencies (#1901) --- moveit_ros/occupancy_map_monitor/CMakeLists.txt | 4 ++++ moveit_ros/occupancy_map_monitor/package.xml | 2 ++ 2 files changed, 6 insertions(+) diff --git a/moveit_ros/occupancy_map_monitor/CMakeLists.txt b/moveit_ros/occupancy_map_monitor/CMakeLists.txt index 290a0525ed..686ef96e38 100644 --- a/moveit_ros/occupancy_map_monitor/CMakeLists.txt +++ b/moveit_ros/occupancy_map_monitor/CMakeLists.txt @@ -19,7 +19,9 @@ endif(APPLE) find_package(catkin REQUIRED COMPONENTS moveit_core moveit_msgs + geometric_shapes pluginlib + tf2_ros ) find_package(Eigen3 REQUIRED) @@ -33,6 +35,8 @@ catkin_package( CATKIN_DEPENDS moveit_core moveit_msgs + geometric_shapes + tf2_ros DEPENDS EIGEN3 OCTOMAP diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml index 28c302748d..8ff09a2966 100644 --- a/moveit_ros/occupancy_map_monitor/package.xml +++ b/moveit_ros/occupancy_map_monitor/package.xml @@ -23,6 +23,8 @@ moveit_msgs octomap pluginlib + tf2_ros + geometric_shapes eigen From 7193a4ea2eb97bbe0b9cb159bb1b7539e9baccd5 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 18 Feb 2020 17:48:39 +0100 Subject: [PATCH 197/612] Fix segfault with uninitialized JogArm thread (#1882) --- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp index bb6e15ad12..0ad2876c94 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp @@ -214,19 +214,13 @@ sensor_msgs::JointState JogCppApi::getJointState() bool JogCppApi::getCommandFrameTransform(Eigen::Isometry3d& transform) { - if (jog_calcs_->isInitialized()) - { - shared_variables_mutex_.lock(); - transform = shared_variables_.tf_moveit_to_cmd_frame; - shared_variables_mutex_.unlock(); - - // All zeros means the transform wasn't initialized, so return false - if (transform.matrix().isZero(0)) - return false; - } - else + if (!jog_calcs_ || !jog_calcs_->isInitialized()) return false; - return true; + const std::lock_guard lock(shared_variables_mutex_); + transform = shared_variables_.tf_moveit_to_cmd_frame; + + // All zeros means the transform wasn't initialized, so return false + return !transform.matrix().isZero(0); } } // namespace moveit_jog_arm From 26863598bdff131cf9dbae71f97a1b775753f47f Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 18 Feb 2020 17:23:49 -0700 Subject: [PATCH 198/612] [jog_arm] a ROS service to enable task redundancy (#1855) --- .../moveit_jog_arm/CMakeLists.txt | 5 +++ .../include/moveit_jog_arm/jog_arm_data.h | 4 ++ .../include/moveit_jog_arm/jog_calcs.h | 9 ++++ .../moveit_jog_arm/jog_interface_base.h | 6 +++ .../moveit_jog_arm/package.xml | 1 + .../moveit_jog_arm/src/jog_calcs.cpp | 32 ++++++++++++++- .../moveit_jog_arm/src/jog_interface_base.cpp | 18 +++++++- .../moveit_jog_arm/src/jog_ros_interface.cpp | 5 +++ .../arm_setup/config/initial_position.yaml | 1 + .../arm_setup/config/singular_position.yaml | 1 + ...jog_arm_drift_dimensions_service_test.test | 39 ++++++++++++++++++ .../test_drift_dimensions_service.py | 41 +++++++++++++++++++ .../test_halt_msg/test_jog_arm_halt_msg.py | 6 ++- .../test_jog_arm_msg_reception.py | 19 +++++---- 14 files changed, 175 insertions(+), 12 deletions(-) create mode 100644 moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test create mode 100755 moveit_experimental/moveit_jog_arm/test/test_drift_dimensions_service/test_drift_dimensions_service.py diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index f83181ee73..1e05203e71 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -14,6 +14,7 @@ endif() find_package(catkin REQUIRED COMPONENTS control_msgs geometry_msgs + moveit_msgs moveit_ros_planning_interface rosparam_shortcuts sensor_msgs @@ -30,6 +31,7 @@ catkin_package( CATKIN_DEPENDS control_msgs geometry_msgs + moveit_msgs moveit_ros_planning_interface rosparam_shortcuts sensor_msgs @@ -130,6 +132,9 @@ if(CATKIN_ENABLE_TESTING) # Python integration test that a halt message is published at a singularity add_rostest(test/launch/jog_arm_halt_msg_test.test) + # Python integration test of a service to enable drift in some dimensions + add_rostest(test/launch/jog_arm_drift_dimensions_service_test.test) + # jog_cpp_interface add_rostest_gtest(jog_cpp_interface_test test/jog_cpp_interface_test.test diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index a37818d44c..24718c6413 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -79,6 +79,10 @@ struct JogArmShared // The transform from the MoveIt planning frame to robot_link_command_frame Eigen::Isometry3d tf_moveit_to_cmd_frame; + + // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw] + std::atomic_bool drift_dimensions[6] = { ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), + ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false) }; }; // ROS params to be read. See the yaml file in /config for a description of each. diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index d6d7cf5e54..f25548e752 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -128,6 +128,15 @@ class JogCalcs void insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& trajectory, int count) const; + /** + * Remove the Jacobian row and the delta-x element of one Cartesian dimension, to take advantage of task redundancy + * + * @param matrix The Jacobian matrix. + * @param delta_x Vector of Cartesian delta commands, should be the same size as matrix.rows() + * @param row_to_remove Dimension that will be allowed to drift, e.g. row_to_remove = 2 allows z-translation drift. + */ + void removeDimension(Eigen::MatrixXd& matrix, Eigen::VectorXd& delta_x, unsigned int row_to_remove); + const robot_state::JointModelGroup* joint_model_group_; robot_state::RobotStatePtr kinematic_state_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h index 32cbc66893..6e486e737d 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h @@ -42,6 +42,7 @@ #include "jog_calcs.h" #include "low_pass_filter.h" #include +#include #include #include #include @@ -59,6 +60,11 @@ class JogInterfaceBase void jointsCB(const sensor_msgs::JointStateConstPtr& msg); + // Service callback for changing drift dimensions, + // e.g. to allow the wrist joint to rotate + bool changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, + moveit_msgs::ChangeDriftDimensions::Response& res); + // Jogging calculation thread bool startJogCalcThread(); bool stopJogCalcThread(); diff --git a/moveit_experimental/moveit_jog_arm/package.xml b/moveit_experimental/moveit_jog_arm/package.xml index d57541434a..3673512fc4 100644 --- a/moveit_experimental/moveit_jog_arm/package.xml +++ b/moveit_experimental/moveit_jog_arm/package.xml @@ -24,6 +24,7 @@ sensor_msgs std_msgs trajectory_msgs + moveit_msgs moveit_ros_planning_interface rosparam_shortcuts diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 4913864ceb..b4f71243c8 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -318,13 +318,27 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& cmd.twist.angular.z = angular_vector(2); } - const Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); + Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); // Convert from cartesian commands to joint commands jacobian_ = kinematic_state_->getJacobian(joint_model_group_); + + // May allow some dimensions to drift, based on shared_variables.drift_dimensions + // i.e. take advantage of task redundancy. + // Remove the Jacobian rows corresponding to True in the vector shared_variables.drift_dimensions + // Work backwards through the 6-vector so indices don't get out of order + for (auto dimension = jacobian_.rows(); dimension >= 0; --dimension) + { + if (shared_variables.drift_dimensions[dimension] && jacobian_.rows() > 1) + { + removeDimension(jacobian_, delta_x, dimension); + } + } + svd_ = Eigen::JacobiSVD(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV); matrix_s_ = svd_.singularValues().asDiagonal(); pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose(); + delta_theta_ = pseudo_inverse_ * delta_x; // If close to a collision or a singularity, decelerate @@ -734,4 +748,20 @@ bool JogCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen:: return true; } + +void JogCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta_x, unsigned int row_to_remove) +{ + unsigned int num_rows = jacobian.rows() - 1; + unsigned int num_cols = jacobian.cols(); + + if (row_to_remove < num_rows) + { + jacobian.block(row_to_remove, 0, num_rows - row_to_remove, num_cols) = + jacobian.block(row_to_remove + 1, 0, num_rows - row_to_remove, num_cols); + delta_x.segment(row_to_remove, num_rows - row_to_remove) = + delta_x.segment(row_to_remove + 1, num_rows - row_to_remove); + } + jacobian.conservativeResize(num_rows, num_cols); + delta_x.conservativeResize(num_rows); +} } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index 77c5f0f583..6cfc579973 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -195,9 +195,23 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) // Listen to joint angles. Store them in a shared variable. void JogInterfaceBase::jointsCB(const sensor_msgs::JointStateConstPtr& msg) { - shared_variables_mutex_.lock(); + const std::lock_guard lock(shared_variables_mutex_); shared_variables_.joints = *msg; - shared_variables_mutex_.unlock(); +} + +bool JogInterfaceBase::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, + moveit_msgs::ChangeDriftDimensions::Response& res) +{ + // These are std::atomic's, they are threadsafe without a mutex lock + shared_variables_.drift_dimensions[0] = req.drift_x_translation; + shared_variables_.drift_dimensions[1] = req.drift_y_translation; + shared_variables_.drift_dimensions[2] = req.drift_z_translation; + shared_variables_.drift_dimensions[3] = req.drift_x_rotation; + shared_variables_.drift_dimensions[4] = req.drift_y_rotation; + shared_variables_.drift_dimensions[5] = req.drift_z_rotation; + + res.success = true; + return true; } // A separate thread for the heavy jogging calculations. diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp index 9caaeaf802..72144692fe 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp @@ -90,6 +90,11 @@ JogROSInterface::JogROSInterface() ros::Subscriber joints_sub = nh.subscribe(ros_parameters_.joint_topic, 1, &JogInterfaceBase::jointsCB, dynamic_cast(this)); + // ROS Server for allowing drift in some dimensions + ros::ServiceServer drift_dimensions_server = + nh.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions", + &JogInterfaceBase::changeDriftDimensions, dynamic_cast(this)); + // Publish freshly-calculated joints to the robot. // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). ros::Publisher outgoing_cmd_pub; diff --git a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/initial_position.yaml b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/initial_position.yaml index a296398243..7a86cc0aa7 100644 --- a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/initial_position.yaml +++ b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/initial_position.yaml @@ -1,3 +1,4 @@ +# This is a good initial position, not near joint limits or singularity zeros: panda_joint1: 0 panda_joint2: -0.785 diff --git a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/singular_position.yaml b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/singular_position.yaml index 582482f8b2..5ce15e651a 100644 --- a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/singular_position.yaml +++ b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/singular_position.yaml @@ -1,3 +1,4 @@ +# This position is at a singularity zeros: panda_joint1: 0 panda_joint2: 0 diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test new file mode 100644 index 0000000000..22b081b990 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_experimental/moveit_jog_arm/test/test_drift_dimensions_service/test_drift_dimensions_service.py b/moveit_experimental/moveit_jog_arm/test/test_drift_dimensions_service/test_drift_dimensions_service.py new file mode 100755 index 0000000000..b711668710 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/test/test_drift_dimensions_service/test_drift_dimensions_service.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python +import time + +import pytest +import rospy + +from moveit_msgs.srv import ChangeDriftDimensions +from geometry_msgs.msg import Transform +from geometry_msgs.msg import TwistStamped +from std_msgs.msg import Bool +from trajectory_msgs.msg import JointTrajectory + +# Send a service call to allow drift in all but the y-dimension. +# In other words, only the y-dimension will be controlled exactly. +# Check that the service returns and the jog node continues to publish commands to the robot. + +JOG_ARM_SETTLE_TIME_S = 10 +ROS_SETTLE_TIME_S = 10 + +CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' + +COMMAND_OUT_TOPIC = 'jog_server/command' + + +@pytest.fixture +def node(): + return rospy.init_node('pytest', anonymous=True) + + +def test_drift_dimensions_service(node): + # Make the service call to allow drift in all dimensions except y-translation + drift_service = rospy.ServiceProxy('jog_server/change_drift_dimensions', ChangeDriftDimensions) + # the transform is an identity matrix, not used for now + drift_response = drift_service(True, False, True, True, True, True, Transform()) + # Check for successful response + assert drift_response.success == True + + +if __name__ == '__main__': + node = node() + test_drift_dimensions_service(node) diff --git a/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py b/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py index 741c18a0b4..882d12edfb 100755 --- a/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py +++ b/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py @@ -52,9 +52,11 @@ def test_jog_arm_halt_msg(node): # This nonzero command should produce jogging output # A subscriber in a different thread fills `received` TEST_DURATION = 1 - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) received = [] - rospy.sleep(TEST_DURATION) + start_time = rospy.get_rostime() + while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION: + cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) + time.sleep(0.1) # Check the received messages assert len(received) > 1 diff --git a/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py b/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py index c80109ffa2..1aff26396b 100755 --- a/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py +++ b/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py @@ -73,10 +73,13 @@ def test_jog_arm_cartesian_command(node): # This nonzero command should produce jogging output # A subscriber in a different thread fills `received` TEST_DURATION = 1 - PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from config file - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) + PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file received = [] - rospy.sleep(TEST_DURATION) + + start_time = rospy.get_rostime() + while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION: + cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) + time.sleep(0.1) # TEST_DURATION/PUBLISH_PERIOD is the expected number of messages in this duration. # Allow a small +/- window due to rounding/timing errors assert len(received) >= TEST_DURATION/PUBLISH_PERIOD - 5 @@ -96,11 +99,13 @@ def test_jog_arm_joint_command(node): received = [] TEST_DURATION = 1 - PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from config file - JOINT_VELOCITY_LIMIT = 1 + PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file velocities = [0.1] - joint_cmd.send_joint_velocity_cmd(velocities) - rospy.sleep(TEST_DURATION) + + start_time = rospy.get_rostime() + while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION: + joint_cmd.send_joint_velocity_cmd(velocities) + time.sleep(0.1) # TEST_DURATION/PUBLISH_PERIOD is the expected number of messages in this duration. # Allow a small +/- window due to rounding/timing errors assert len(received) >= TEST_DURATION/PUBLISH_PERIOD - 20 From 193cbea741b781f1658e7360cef5eac861b2e545 Mon Sep 17 00:00:00 2001 From: Aris Synodinos Date: Wed, 19 Feb 2020 02:22:35 +0100 Subject: [PATCH 199/612] Installs an empty plugin description xml file if bullet is not found (#1898) --- moveit_core/CMakeLists.txt | 2 ++ moveit_core/collision_detection_bullet/empty_description.xml | 2 ++ 2 files changed, 4 insertions(+) create mode 100644 moveit_core/collision_detection_bullet/empty_description.xml diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index f8b6bf2984..cd6c1ab095 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -224,4 +224,6 @@ add_subdirectory(dynamics_solver) if(BULLET_ENABLE) add_subdirectory(collision_detection_bullet) +else() + install(FILES collision_detection_bullet/empty_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} RENAME collision_detector_bullet_description.xml) endif() diff --git a/moveit_core/collision_detection_bullet/empty_description.xml b/moveit_core/collision_detection_bullet/empty_description.xml new file mode 100644 index 0000000000..3ef0fa6791 --- /dev/null +++ b/moveit_core/collision_detection_bullet/empty_description.xml @@ -0,0 +1,2 @@ + + From 0abd09585d93b631637a121a286db74440b538c0 Mon Sep 17 00:00:00 2001 From: Bhavam Vidyarthi Date: Wed, 19 Feb 2020 06:54:09 +0530 Subject: [PATCH 200/612] changes made to ModelBasedStateSpacePtr() calls (#1895) --- .../joint_space/joint_model_state_space_factory.cpp | 2 +- .../work_space/pose_model_state_space_factory.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp index 6943b91b91..222909cc49 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp @@ -52,5 +52,5 @@ int ompl_interface::JointModelStateSpaceFactory::canRepresentProblem( ompl_interface::ModelBasedStateSpacePtr ompl_interface::JointModelStateSpaceFactory::allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const { - return ModelBasedStateSpacePtr(new JointModelStateSpace(space_spec)); + return std::make_shared(space_spec); } diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp index 80dd48b35d..93200757cd 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp @@ -86,5 +86,5 @@ int ompl_interface::PoseModelStateSpaceFactory::canRepresentProblem( ompl_interface::ModelBasedStateSpacePtr ompl_interface::PoseModelStateSpaceFactory::allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const { - return ModelBasedStateSpacePtr(new PoseModelStateSpace(space_spec)); + return std::make_shared(space_spec); } From 6eaa75d690614d6dbc53e9ad557bf3d8de2f8ad9 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 28 Feb 2020 05:59:13 -0700 Subject: [PATCH 201/612] JogArm C++ API fixes (#1911) * Rename JogCppApi->JogCppInterface to match filename * Use public inheritance of JogInterfaceBase * Fix crash due to different Jacobian dimensions * Clang format, update unit test * Fix dim mismatch in decelerateForSingularity * Revert 'static' for several variables --- .../include/moveit_jog_arm/jog_calcs.h | 8 ++-- .../moveit_jog_arm/jog_cpp_interface.h | 8 ++-- .../moveit_jog_arm/jog_interface_base.h | 19 ++++++-- .../cpp_interface_example.cpp | 2 +- .../moveit_jog_arm/src/jog_calcs.cpp | 43 +++++++++---------- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 18 ++++---- .../test/jog_cpp_interface_test.cpp | 2 +- 7 files changed, 53 insertions(+), 47 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index f25548e752..0774a03c07 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -101,8 +101,9 @@ class JogCalcs // Possibly calculate a velocity scaling factor, due to proximity of // singularity and direction of motion - double decelerateForSingularity(const Eigen::VectorXd& commanded_velocity, - const Eigen::JacobiSVD& svd); + double velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, + const Eigen::JacobiSVD& svd, + const Eigen::MatrixXd& jacobian, const Eigen::MatrixXd& pseudo_inverse); /** * Slow motion down if close to singularity or collision. @@ -154,9 +155,6 @@ class JogCalcs JogArmParameters parameters_; - // For jacobian calculations - Eigen::MatrixXd jacobian_, pseudo_inverse_, matrix_s_; - Eigen::JacobiSVD svd_; // Use ArrayXd type to enable more coefficient-wise operations Eigen::ArrayXd delta_theta_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h index 0fb1568ce9..0541d96620 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h @@ -44,15 +44,15 @@ namespace moveit_jog_arm { /** -* Class JogCppApi - This class should be instantiated in a new thread +* Class JogCppInterface - This class should be instantiated in a new thread * See cpp_interface_example.cpp */ -class JogCppApi : protected JogInterfaceBase +class JogCppInterface : public JogInterfaceBase { public: - JogCppApi(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + JogCppInterface(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - ~JogCppApi(); + ~JogCppInterface(); void startMainLoop(); diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h index 6e486e737d..7afe1d7460 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h @@ -58,19 +58,30 @@ class JogInterfaceBase public: JogInterfaceBase(); + /** \brief Update the joints of the robot */ void jointsCB(const sensor_msgs::JointStateConstPtr& msg); - // Service callback for changing drift dimensions, - // e.g. to allow the wrist joint to rotate + /** + * Allow drift in certain dimensions. For example, may allow the wrist to rotate freely. + * This can help avoid singularities. + * + * @param request the service request + * @param response the service response + * @return true if the adjustment was made + */ bool changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, moveit_msgs::ChangeDriftDimensions::Response& res); - // Jogging calculation thread + /** \brief Start the main calculation thread */ bool startJogCalcThread(); + + /** \brief Stop the main calculation thread */ bool stopJogCalcThread(); - // Collision checking thread + /** \brief Start collision checking */ bool startCollisionCheckThread(); + + /** \brief Stop collision checking */ bool stopCollisionCheckThread(); protected: diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index 5ccd8cedb2..ae1b77c1be 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -66,7 +66,7 @@ int main(int argc, char** argv) planning_scene_monitor->startStateMonitor(); // Run the jogging C++ interface in a new thread to ensure a constant outgoing message rate. - moveit_jog_arm::JogCppApi jog_interface(planning_scene_monitor); + moveit_jog_arm::JogCppInterface jog_interface(planning_scene_monitor); std::thread jogging_thread([&]() { jog_interface.startMainLoop(); }); // Make a Cartesian velocity message diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index b4f71243c8..f324d6af78 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -321,28 +321,30 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); // Convert from cartesian commands to joint commands - jacobian_ = kinematic_state_->getJacobian(joint_model_group_); + Eigen::MatrixXd jacobian = kinematic_state_->getJacobian(joint_model_group_); // May allow some dimensions to drift, based on shared_variables.drift_dimensions // i.e. take advantage of task redundancy. // Remove the Jacobian rows corresponding to True in the vector shared_variables.drift_dimensions // Work backwards through the 6-vector so indices don't get out of order - for (auto dimension = jacobian_.rows(); dimension >= 0; --dimension) + for (auto dimension = jacobian.rows(); dimension >= 0; --dimension) { - if (shared_variables.drift_dimensions[dimension] && jacobian_.rows() > 1) + if (shared_variables.drift_dimensions[dimension] && jacobian.rows() > 1) { - removeDimension(jacobian_, delta_x, dimension); + removeDimension(jacobian, delta_x, dimension); } } - svd_ = Eigen::JacobiSVD(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV); - matrix_s_ = svd_.singularValues().asDiagonal(); - pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose(); + Eigen::JacobiSVD svd = + Eigen::JacobiSVD(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal(); + Eigen::MatrixXd pseudo_inverse = svd.matrixV() * matrix_s.inverse() * svd.matrixU().transpose(); - delta_theta_ = pseudo_inverse_ * delta_x; + delta_theta_ = pseudo_inverse * delta_x; // If close to a collision or a singularity, decelerate - if (!applyVelocityScaling(shared_variables, mutex, delta_theta_, decelerateForSingularity(delta_x, svd_))) + if (!applyVelocityScaling(shared_variables, mutex, delta_theta_, + velocityScalingFactorForSingularity(delta_x, svd, jacobian, pseudo_inverse))) { has_warning_ = true; suddenHalt(outgoing_command_); @@ -473,16 +475,19 @@ bool JogCalcs::applyVelocityScaling(const JogArmShared& shared_variables, std::m } // Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion -double JogCalcs::decelerateForSingularity(const Eigen::VectorXd& commanded_velocity, - const Eigen::JacobiSVD& svd) +double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, + const Eigen::JacobiSVD& svd, + const Eigen::MatrixXd& jacobian, + const Eigen::MatrixXd& pseudo_inverse) { double velocity_scale = 1; + std::size_t num_dimensions = jacobian.rows(); // Find the direction away from nearest singularity. // The last column of U from the SVD of the Jacobian points directly toward or away from the singularity. // The sign can flip at any time, so we have to do some extra checking. // Look ahead to see if the Jacobian's condition will decrease. - Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(5); + Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(num_dimensions - 1); double ini_condition = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); @@ -490,29 +495,23 @@ double JogCalcs::decelerateForSingularity(const Eigen::VectorXd& commanded_veloc // "Resolving the Sign Ambiguity in the Singular Value Decomposition". // Look ahead to see if the Jacobian's condition will decrease in this // direction. Start with a scaled version of the singular vector - Eigen::VectorXd delta_x(6); + Eigen::VectorXd delta_x(num_dimensions); double scale = 100; delta_x = vector_toward_singularity / scale; // Calculate a small change in joints Eigen::VectorXd new_theta; kinematic_state_->copyJointGroupPositions(joint_model_group_, new_theta); - new_theta += pseudo_inverse_ * delta_x; + new_theta += pseudo_inverse * delta_x; kinematic_state_->setJointGroupPositions(joint_model_group_, new_theta); - jacobian_ = kinematic_state_->getJacobian(joint_model_group_); - Eigen::JacobiSVD new_svd = Eigen::JacobiSVD(jacobian_); + Eigen::JacobiSVD new_svd(jacobian); double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1); // If new_condition < ini_condition, the singular vector does point towards a // singularity. Otherwise, flip its direction. if (ini_condition >= new_condition) { - vector_toward_singularity[0] *= -1; - vector_toward_singularity[1] *= -1; - vector_toward_singularity[2] *= -1; - vector_toward_singularity[3] *= -1; - vector_toward_singularity[4] *= -1; - vector_toward_singularity[5] *= -1; + vector_toward_singularity *= -1; } // If this dot product is positive, we're moving toward singularity ==> decelerate diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp index 0ad2876c94..1b9475044b 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp @@ -39,13 +39,11 @@ #include "moveit_jog_arm/jog_cpp_interface.h" -// TODO(davetcoleman): rename JogCppApi to JogCppInterface to match file name - static const std::string LOGNAME = "jog_cpp_interface"; namespace moveit_jog_arm { -JogCppApi::JogCppApi(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) +JogCppInterface::JogCppInterface(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) { planning_scene_monitor_ = planning_scene_monitor; @@ -54,12 +52,12 @@ JogCppApi::JogCppApi(const planning_scene_monitor::PlanningSceneMonitorPtr& plan exit(EXIT_FAILURE); } -JogCppApi::~JogCppApi() +JogCppInterface::~JogCppInterface() { stopMainLoop(); } -void JogCppApi::startMainLoop() +void JogCppInterface::startMainLoop() { // Reset loop termination flag stop_requested_ = false; @@ -150,12 +148,12 @@ void JogCppApi::startMainLoop() stopCollisionCheckThread(); } -void JogCppApi::stopMainLoop() +void JogCppInterface::stopMainLoop() { stop_requested_ = true; } -void JogCppApi::provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command) +void JogCppInterface::provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command) { shared_variables_mutex_.lock(); @@ -183,7 +181,7 @@ void JogCppApi::provideTwistStampedCommand(const geometry_msgs::TwistStamped& ve shared_variables_mutex_.unlock(); }; -void JogCppApi::provideJointCommand(const control_msgs::JointJog& joint_command) +void JogCppInterface::provideJointCommand(const control_msgs::JointJog& joint_command) { shared_variables_mutex_.lock(); shared_variables_.joint_command_deltas = joint_command; @@ -203,7 +201,7 @@ void JogCppApi::provideJointCommand(const control_msgs::JointJog& joint_command) shared_variables_mutex_.unlock(); } -sensor_msgs::JointState JogCppApi::getJointState() +sensor_msgs::JointState JogCppInterface::getJointState() { shared_variables_mutex_.lock(); sensor_msgs::JointState current_joints = shared_variables_.joints; @@ -212,7 +210,7 @@ sensor_msgs::JointState JogCppApi::getJointState() return current_joints; } -bool JogCppApi::getCommandFrameTransform(Eigen::Isometry3d& transform) +bool JogCppInterface::getCommandFrameTransform(Eigen::Isometry3d& transform) { if (!jog_calcs_ || !jog_calcs_->isInitialized()) return false; diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp index 5247c5091b..2b18f7e083 100644 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp @@ -75,7 +75,7 @@ class TestJogCppInterface : public ::testing::Test TEST_F(TestJogCppInterface, InitTest) { - moveit_jog_arm::JogCppApi jog_cpp_interface(planning_scene_monitor_); + moveit_jog_arm::JogCppInterface jog_cpp_interface(planning_scene_monitor_); ros::Duration(1).sleep(); // Give the started thread some time to run } From 99610351429f5d48f53e7ab850ad54ad0f4142d9 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 28 Feb 2020 06:25:08 -0700 Subject: [PATCH 202/612] codecov yaml (#1917) --- codecov.yaml | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 codecov.yaml diff --git a/codecov.yaml b/codecov.yaml new file mode 100644 index 0000000000..9cf0fb0e1e --- /dev/null +++ b/codecov.yaml @@ -0,0 +1,13 @@ +coverage: + status: + project: + default: + target: auto + # allow coverage to drop by this amount and still post success + threshold: 0.5% + base: auto + branches: + - master + if_not_found: success + if_ci_failed: error + only_pulls: false From e8a4072727abe4c704d009f05baa764bcf106b33 Mon Sep 17 00:00:00 2001 From: Aris Synodinos Date: Sat, 29 Feb 2020 01:29:54 +0100 Subject: [PATCH 203/612] Scoping the namespace of the template specialization to overcome gcc-5 bug 56480 (#1919) --- .../src/planning_context_manager.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index fa2e88a308..89809dfcff 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -192,34 +192,35 @@ ompl_interface::MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob:: return nullptr; }; // TODO: remove when ROS Melodic and older are no longer supported +// namespace is scoped instead of global because of GCC bug 56480 #if OMPL_VERSION_VALUE >= 1005000 +namespace ompl_interface +{ template <> inline ompl::base::Planner* -ompl_interface::MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) +MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) { return new og::PRM(data); }; template <> inline ompl::base::Planner* -ompl_interface::MultiQueryPlannerAllocator::allocatePersistentPlanner( - const ob::PlannerData& data) +MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) { return new og::PRMstar(data); }; template <> inline ompl::base::Planner* -ompl_interface::MultiQueryPlannerAllocator::allocatePersistentPlanner( - const ob::PlannerData& data) +MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) { return new og::LazyPRM(data); }; template <> inline ompl::base::Planner* -ompl_interface::MultiQueryPlannerAllocator::allocatePersistentPlanner( - const ob::PlannerData& data) +MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) { return new og::LazyPRMstar(data); }; +} #endif ompl_interface::PlanningContextManager::PlanningContextManager(robot_model::RobotModelConstPtr robot_model, From ba4b60e079fd14a61c50ef34c156eee6d63e58f7 Mon Sep 17 00:00:00 2001 From: Ayush Garg Date: Tue, 3 Mar 2020 14:27:44 -0500 Subject: [PATCH 204/612] Replace namespaces robot_state and robot_model with moveit::core (#1924) --- .../allvalid/collision_env_allvalid.h | 28 ++-- .../collision_detection/collision_common.h | 4 +- .../collision_detector_allocator.h | 8 +- .../collision_detection/collision_env.h | 38 ++--- .../test_collision_common_panda.h | 8 +- .../test_collision_common_pr2.h | 24 +-- .../src/allvalid/collision_env_allvalid.cpp | 24 +-- .../collision_detection/src/collision_env.cpp | 16 +- .../test/test_all_valid.cpp | 4 +- .../collision_env_bullet.h | 34 ++-- .../src/collision_env_bullet.cpp | 40 ++--- ...t_bullet_continuous_collision_checking.cpp | 16 +- .../collision_common.h | 25 +-- .../collision_env_fcl.h | 34 ++-- .../src/collision_common.cpp | 32 ++-- .../src/collision_env_fcl.cpp | 44 ++--- .../test/test_fcl_env.cpp | 16 +- .../collision_common_distance_field.h | 6 +- .../collision_env_distance_field.h | 38 ++--- .../collision_env_hybrid.h | 32 ++-- .../src/collision_common_distance_field.cpp | 4 +- .../src/collision_env_distance_field.cpp | 32 ++-- .../src/collision_env_hybrid.cpp | 32 ++-- .../test/test_collision_distance_field.cpp | 24 +-- .../constraint_samplers/constraint_sampler.h | 22 +-- .../constraint_sampler_tools.h | 4 +- .../default_constraint_samplers.h | 18 +-- .../union_constraint_sampler.h | 4 +- .../src/constraint_sampler_manager.cpp | 8 +- .../src/constraint_sampler_tools.cpp | 10 +- .../src/default_constraint_samplers.cpp | 36 ++--- .../src/union_constraint_sampler.cpp | 4 +- .../test/test_constraint_samplers.cpp | 56 +++---- .../moveit/dynamics_solver/dynamics_solver.h | 12 +- .../dynamics_solver/src/dynamics_solver.cpp | 6 +- .../kinematic_constraint.h | 72 ++++----- .../moveit/kinematic_constraints/utils.h | 10 +- .../src/kinematic_constraint.cpp | 59 +++---- .../kinematic_constraints/src/utils.cpp | 10 +- .../test/test_constraints.cpp | 44 ++--- .../kinematics_metrics/kinematics_metrics.h | 26 +-- .../src/kinematics_metrics.cpp | 48 +++--- .../planning_interface/planning_interface.h | 2 +- .../src/planning_interface.cpp | 2 +- .../src/planning_response.cpp | 4 +- .../moveit/planning_scene/planning_scene.h | 152 +++++++++--------- .../planning_scene/src/planning_scene.cpp | 125 +++++++------- .../test/test_multi_threaded.cpp | 16 +- .../test/test_planning_scene.cpp | 2 +- moveit_core/robot_model/src/robot_model.cpp | 6 +- .../src/cartesian_interpolator.cpp | 4 +- moveit_core/robot_state/src/robot_state.cpp | 22 +-- .../test/robot_state_benchmark.cpp | 4 +- .../robot_state/test/robot_state_test.cpp | 2 +- moveit_core/robot_state/test/test_aabb.cpp | 14 +- .../test/test_cartesian_interpolator.cpp | 22 +-- .../robot_trajectory/robot_trajectory.h | 54 +++---- .../robot_trajectory/src/robot_trajectory.cpp | 42 ++--- .../test/test_robot_trajectory.cpp | 20 +-- .../src/iterative_spline_parameterization.cpp | 10 +- .../src/iterative_time_parameterization.cpp | 32 ++-- .../time_optimal_trajectory_generation.cpp | 12 +- .../test/test_time_parameterization.cpp | 10 +- .../kinematics_cache/kinematics_cache.h | 12 +- .../kinematics_cache/src/kinematics_cache.cpp | 6 +- .../kinematics_constraint_aware.h | 14 +- .../kinematics_request_response.h | 6 +- .../src/kinematics_constraint_aware.cpp | 18 +-- .../include/moveit_jog_arm/jog_calcs.h | 4 +- .../src/collision_check_thread.cpp | 2 +- .../moveit_jog_arm/src/jog_calcs.cpp | 4 +- .../kdl_kinematics_plugin.h | 4 +- .../src/kdl_kinematics_plugin.cpp | 6 +- .../lma_kinematics_plugin.h | 6 +- .../src/lma_kinematics_plugin.cpp | 4 +- .../srv_kinematics_plugin.h | 4 +- .../src/srv_kinematics_plugin.cpp | 2 +- moveit_kinematics/test/benchmark_ik.cpp | 6 +- .../test/test_kinematics_plugin.cpp | 24 +-- .../chomp_interface/chomp_planning_context.h | 3 +- .../src/chomp_planning_context.cpp | 2 +- .../chomp_interface/src/chomp_plugin.cpp | 2 +- .../src/chomp_planner.cpp | 12 +- .../src/chomp_trajectory.cpp | 10 +- .../detail/constrained_goal_sampler.h | 8 +- .../detail/constrained_sampler.h | 2 +- .../detail/constrained_valid_state_sampler.h | 2 +- .../detail/projection_evaluators.h | 2 +- .../detail/threadsafe_state_storage.h | 10 +- .../model_based_planning_context.h | 10 +- .../moveit/ompl_interface/ompl_interface.h | 6 +- .../joint_model_state_space_factory.h | 2 +- .../model_based_state_space.h | 28 ++-- .../model_based_state_space_factory.h | 2 +- .../work_space/pose_model_state_space.h | 8 +- .../pose_model_state_space_factory.h | 2 +- .../ompl_interface/planning_context_manager.h | 6 +- .../src/detail/constrained_goal_sampler.cpp | 10 +- .../src/detail/constraints_library.cpp | 8 +- .../src/detail/projection_evaluators.cpp | 2 +- .../src/detail/state_validity_checker.cpp | 12 +- .../src/detail/threadsafe_state_storage.cpp | 12 +- .../src/model_based_planning_context.cpp | 4 +- .../ompl_interface/src/ompl_interface.cpp | 6 +- .../src/ompl_planner_manager.cpp | 10 +- .../joint_model_state_space_factory.cpp | 2 +- .../model_based_state_space.cpp | 24 +-- .../work_space/pose_model_state_space.cpp | 6 +- .../pose_model_state_space_factory.cpp | 8 +- .../src/planning_context_manager.cpp | 4 +- .../ompl_interface/test/test_state_space.cpp | 6 +- .../trajopt_planning_context.h | 2 +- .../trajopt/src/problem_description.cpp | 12 +- .../trajopt/src/trajopt_interface.cpp | 8 +- .../trajopt/src/trajopt_planner_manager.cpp | 2 +- .../trajopt/src/trajopt_planning_context.cpp | 4 +- .../trajopt/test/trajectory_test.cpp | 10 +- .../src/moveit_fake_controller_manager.cpp | 2 +- .../benchmarks/src/benchmark_execution.cpp | 12 +- .../moveit/pick_place/manipulation_plan.h | 10 +- .../include/moveit/pick_place/pick_place.h | 2 +- .../pick_place/reachable_valid_pose_filter.h | 2 +- .../src/approach_and_translate_stage.cpp | 38 ++--- .../manipulation/pick_place/src/pick.cpp | 6 +- .../pick_place/src/pick_place.cpp | 6 +- .../manipulation/pick_place/src/place.cpp | 25 +-- .../pick_place/src/plan_stage.cpp | 3 +- .../src/reachable_valid_pose_filter.cpp | 10 +- .../cartesian_path_service_capability.cpp | 24 +-- .../kinematics_service_capability.cpp | 24 +-- .../kinematics_service_capability.h | 8 +- .../move_action_capability.cpp | 2 +- .../state_validation_service_capability.cpp | 4 +- .../tf_publisher_capability.cpp | 6 +- .../move_group/src/move_group_capability.cpp | 4 +- .../src/depth_self_filter_nodelet.cpp | 4 +- .../mesh_filter/src/transform_provider.cpp | 2 +- .../kinematics_plugin_loader.h | 4 +- .../src/kinematics_plugin_loader.cpp | 14 +- ...re_collision_speed_checking_fcl_bullet.cpp | 16 +- .../src/evaluate_collision_checking_speed.cpp | 8 +- .../src/evaluate_state_operations_speed.cpp | 10 +- ...inematics_speed_and_validity_evaluator.cpp | 4 +- .../planning_pipeline/planning_pipeline.h | 8 +- .../src/planning_pipeline.cpp | 8 +- .../src/fix_start_state_bounds.cpp | 26 +-- .../src/fix_start_state_collision.cpp | 10 +- .../src/fix_start_state_path_constraints.cpp | 6 +- .../current_state_monitor.h | 16 +- .../planning_scene_monitor.h | 18 +-- .../trajectory_monitor.h | 2 +- .../src/current_state_monitor.cpp | 22 +-- .../src/planning_scene_monitor.cpp | 26 +-- .../src/trajectory_monitor.cpp | 2 +- .../robot_model_loader/robot_model_loader.h | 4 +- .../src/robot_model_loader.cpp | 21 +-- .../trajectory_execution_manager.h | 6 +- .../src/trajectory_execution_manager.cpp | 28 ++-- .../common_objects.h | 14 +- .../src/common_objects.cpp | 14 +- .../move_group_interface.h | 14 +- .../src/move_group_interface.cpp | 68 ++++---- .../src/wrap_python_move_group.cpp | 8 +- .../include/moveit/moveit_cpp/moveit_cpp.h | 8 +- .../moveit/moveit_cpp/planning_component.h | 8 +- .../moveit_cpp/src/moveit_cpp.cpp | 8 +- .../moveit_cpp/src/planning_component.cpp | 14 +- .../src/planning_scene_interface.cpp | 2 +- .../src/wrap_python_robot_interface.cpp | 44 ++--- .../test/moveit_cpp_test.cpp | 8 +- .../moveit/robot_interaction/interaction.h | 6 +- .../robot_interaction/interaction_handler.h | 12 +- .../robot_interaction/kinematic_options.h | 4 +- .../robot_interaction/kinematic_options_map.h | 2 +- .../robot_interaction/locked_robot_state.h | 12 +- .../robot_interaction/robot_interaction.h | 8 +- .../src/interaction_handler.cpp | 10 +- .../src/kinematic_options.cpp | 6 +- .../src/kinematic_options_map.cpp | 2 +- .../src/locked_robot_state.cpp | 16 +- .../src/robot_interaction.cpp | 36 ++--- .../test/locked_robot_state_test.cpp | 18 +-- .../motion_planning_display.h | 18 +-- .../motion_planning_frame.h | 4 +- .../src/motion_planning_display.cpp | 51 +++--- .../src/motion_planning_frame.cpp | 8 +- .../motion_planning_frame_manipulation.cpp | 4 +- .../src/motion_planning_frame_objects.cpp | 25 +-- .../src/motion_planning_frame_planning.cpp | 46 +++--- .../src/motion_planning_frame_states.cpp | 12 +- .../planning_scene_display.h | 2 +- .../src/planning_scene_display.cpp | 12 +- .../robot_state_display.h | 6 +- .../src/robot_state_display.cpp | 10 +- .../planning_link_updater.h | 6 +- .../robot_state_visualization.h | 10 +- .../trajectory_visualization.h | 6 +- .../src/planning_link_updater.cpp | 2 +- .../src/planning_scene_render.cpp | 4 +- .../src/robot_state_visualization.cpp | 12 +- .../src/trajectory_visualization.cpp | 4 +- .../trajectory_display.h | 4 +- .../src/trajectory_display.cpp | 2 +- .../warehouse/src/import_from_text.cpp | 4 +- .../warehouse/src/initialize_demo_db.cpp | 4 +- .../warehouse/warehouse/src/save_as_text.cpp | 6 +- .../tools/moveit_config_data.h | 6 +- .../src/tools/compute_default_collisions.cpp | 18 +-- .../src/tools/moveit_config_data.cpp | 50 +++--- .../src/widgets/end_effectors_widget.cpp | 6 +- .../src/widgets/kinematic_chain_widget.cpp | 10 +- .../src/widgets/kinematic_chain_widget.h | 4 +- .../src/widgets/passive_joints_widget.cpp | 4 +- .../src/widgets/planning_groups_widget.cpp | 10 +- .../src/widgets/robot_poses_widget.cpp | 15 +- .../src/widgets/robot_poses_widget.h | 6 +- .../src/widgets/ros_controllers_widget.cpp | 12 +- .../src/widgets/setup_assistant_widget.cpp | 9 +- .../src/widgets/virtual_joints_widget.cpp | 4 +- 219 files changed, 1593 insertions(+), 1578 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h index 024a59496b..2f0e91c88f 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h @@ -46,31 +46,31 @@ namespace collision_detection class CollisionEnvAllValid : public CollisionEnv { public: - CollisionEnvAllValid(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); - CollisionEnvAllValid(const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, double padding = 0.0, + CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); + CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, double padding = 0.0, double scale = 1.0); CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world); void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const moveit::core::RobotState& state) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const AllowedCollisionMatrix& acm) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - virtual double distanceRobot(const robot_state::RobotState& state) const; - virtual double distanceRobot(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; + virtual double distanceRobot(const moveit::core::RobotState& state) const; + virtual double distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; void distanceRobot(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override; + const moveit::core::RobotState& state) const override; void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + const moveit::core::RobotState& state) const override; + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override; + const moveit::core::RobotState& state) const override; }; } diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 96f3343d0b..38a3fef260 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -256,7 +256,7 @@ struct DistanceRequest } /// Compute \e active_components_only_ based on \e req_ - void enableGroup(const robot_model::RobotModelConstPtr& robot_model) + void enableGroup(const moveit::core::RobotModelConstPtr& robot_model) { if (robot_model->hasJointModelGroup(group_name)) active_components_only = &robot_model->getJointModelGroup(group_name)->getUpdatedLinkModelsSet(); @@ -282,7 +282,7 @@ struct DistanceRequest std::string group_name; /// The set of active components to check - const std::set* active_components_only; + const std::set* active_components_only; /// The allowed collision matrix used to filter checks const AllowedCollisionMatrix* acm; diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h index cbfb39a161..89624d9e09 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h @@ -56,7 +56,7 @@ class CollisionDetectorAllocator /** create a new CollisionWorld for checking collisions with the supplied world. */ virtual CollisionEnvPtr allocateEnv(const WorldPtr& world, - const robot_model::RobotModelConstPtr& robot_model) const = 0; + const moveit::core::RobotModelConstPtr& robot_model) const = 0; /** create a new CollisionWorld by copying an existing CollisionWorld of the same type.s * The world must be either the same world as used by \orig or a copy of that world which has not yet been modified. @@ -64,7 +64,7 @@ class CollisionDetectorAllocator virtual CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const = 0; /** create a new CollisionEnv given a robot_model with a new empty world */ - virtual CollisionEnvPtr allocateEnv(const robot_model::RobotModelConstPtr& robot_model) const = 0; + virtual CollisionEnvPtr allocateEnv(const moveit::core::RobotModelConstPtr& robot_model) const = 0; }; /** \brief Template class to make it easy to create an allocator for a specific CollisionWorld/CollisionRobot pair. */ @@ -77,7 +77,7 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator return CollisionDetectorAllocatorType::NAME; } - CollisionEnvPtr allocateEnv(const WorldPtr& world, const robot_model::RobotModelConstPtr& robot_model) const override + CollisionEnvPtr allocateEnv(const WorldPtr& world, const moveit::core::RobotModelConstPtr& robot_model) const override { return CollisionEnvPtr(new CollisionEnvType(robot_model, world)); } @@ -87,7 +87,7 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator return CollisionEnvPtr(new CollisionEnvType(dynamic_cast(*orig), world)); } - CollisionEnvPtr allocateEnv(const robot_model::RobotModelConstPtr& robot_model) const override + CollisionEnvPtr allocateEnv(const moveit::core::RobotModelConstPtr& robot_model) const override { return CollisionEnvPtr(new CollisionEnvType(robot_model)); } diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h index c634ca8597..7d42a73e3f 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h @@ -58,14 +58,14 @@ class CollisionEnv * @param padding The padding to use for all objects/links on the robot * @scale scale A common scaling to use for all objects/links on the robot */ - CollisionEnv(const robot_model::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); + CollisionEnv(const moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); /** @brief Constructor * @param model A robot model to construct the collision robot from * @param padding The padding to use for all objects/links on the robot * @scale scale A common scaling to use for all objects/links on the robot */ - CollisionEnv(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, + CollisionEnv(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, double scale = 1.0); /** \brief Copy constructor */ @@ -82,7 +82,7 @@ class CollisionEnv * @param res A CollisionResult object that encapsulates the collision result * @param state The kinematic state for which checks are being made */ virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const = 0; + const moveit::core::RobotState& state) const = 0; /** \brief Check for self collision. Allowed collisions specified by the allowed collision matrix are * taken into account. @@ -91,7 +91,7 @@ class CollisionEnv * @param state The kinematic state for which checks are being made * @param acm The allowed collision matrix. */ virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; /** \brief Check whether the robot model is in collision with itself or the world at a particular state. * Any collision between any pair of links is checked for, NO collisions are ignored. @@ -99,7 +99,7 @@ class CollisionEnv * @param res A CollisionResult object that encapsulates the collision result * @param state The kinematic state for which checks are being made */ virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const; + const moveit::core::RobotState& state) const; /** \brief Check whether the robot model is in collision with itself or the world at a particular state. * Allowed collisions specified by the allowed collision matrix are taken into account. @@ -107,7 +107,7 @@ class CollisionEnv * @param res A CollisionResult object that encapsulates the collision result * @param state The kinematic state for which checks are being made * @param acm The allowed collision matrix. */ - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; /** \brief Check whether the robot model is in collision with the world. Any collisions between a robot link @@ -118,7 +118,7 @@ class CollisionEnv * @param state The kinematic state for which checks are being made */ virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const = 0; + const moveit::core::RobotState& state) const = 0; /** \brief Check whether the robot model is in collision with the world. * Allowed collisions are ignored. Self collisions are not checked. @@ -128,7 +128,7 @@ class CollisionEnv * @param state The kinematic state for which checks are being made * @param acm The allowed collision matrix.*/ virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot * states). @@ -140,7 +140,7 @@ class CollisionEnv * @param state2 The kinematic state at the end of the segment for which checks are being made * @param acm The allowed collision matrix.*/ virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const = 0; /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot @@ -153,18 +153,18 @@ class CollisionEnv * @param state2 The kinematic state at the end of the segment for which checks are being made * @param acm The allowed collision matrix.*/ virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const = 0; + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const = 0; /** \brief The distance to self-collision given the robot is at state \e state. @param req A DistanceRequest object that encapsulates the distance request @param res A DistanceResult object that encapsulates the distance result @param state The state of this robot to consider */ virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const = 0; + const moveit::core::RobotState& state) const = 0; /** \brief The distance to self-collision given the robot is at state \e state. */ - inline double distanceSelf(const robot_state::RobotState& state) const + inline double distanceSelf(const moveit::core::RobotState& state) const { DistanceRequest req; DistanceResult res; @@ -176,7 +176,7 @@ class CollisionEnv /** \brief The distance to self-collision given the robot is at state \e state, ignoring the distances between links that are allowed to always collide (as specified by \e acm) */ - inline double distanceSelf(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const + inline double distanceSelf(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { DistanceRequest req; DistanceResult res; @@ -193,13 +193,13 @@ class CollisionEnv * @param robot The robot to check distance for * @param state The state for the robot to check distances from */ virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const = 0; + const moveit::core::RobotState& state) const = 0; /** \brief Compute the shortest distance between a robot and the world * @param robot The robot to check distance for * @param state The state for the robot to check distances from * @param verbose Output debug information about distance checks */ - inline double distanceRobot(const robot_state::RobotState& state, bool verbose = false) const + inline double distanceRobot(const moveit::core::RobotState& state, bool verbose = false) const { DistanceRequest req; DistanceResult res; @@ -217,7 +217,7 @@ class CollisionEnv * @param acm Using an allowed collision matrix has the effect of ignoring distances from links that are always * allowed to be in collision. * @param verbose Output debug information about distance checks */ - inline double distanceRobot(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, + inline double distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, bool verbose = false) const { DistanceRequest req; @@ -252,7 +252,7 @@ class CollisionEnv typedef World::ObjectConstPtr ObjectConstPtr; /** @brief The kinematic model corresponding to this collision model*/ - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -312,7 +312,7 @@ class CollisionEnv virtual void updatedPaddingOrScaling(const std::vector& links); /** @brief The kinematic model corresponding to this collision model*/ - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; /** @brief The internally maintained map (from link names to padding)*/ std::map link_padding_; diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 42f356a4d8..86535974cb 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -51,7 +51,7 @@ #include /** \brief Brings the panda robot in user defined home position */ -inline void setToHome(robot_state::RobotState& panda_state) +inline void setToHome(moveit::core::RobotState& panda_state) { panda_state.setToDefaultValues(); double joint2 = -0.785; @@ -90,7 +90,7 @@ class CollisionDetectorPandaTest : public testing::Test cenv_ = value_->allocateEnv(robot_model_); - robot_state_.reset(new robot_state::RobotState(robot_model_)); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); setToHome(*robot_state_); } @@ -100,13 +100,13 @@ class CollisionDetectorPandaTest : public testing::Test bool robot_model_ok_; - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; collision_detection::CollisionEnvPtr cenv_; collision_detection::AllowedCollisionMatrixPtr acm_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotStatePtr robot_state_; }; TYPED_TEST_CASE_P(CollisionDetectorPandaTest); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h index d62c1e8872..8a3eb61e73 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h @@ -78,7 +78,7 @@ class CollisionDetectorTest : public ::testing::Test bool robot_model_ok_; - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; collision_detection::CollisionEnvPtr cenv_; @@ -96,7 +96,7 @@ TYPED_TEST_P(CollisionDetectorTest, InitOK) TYPED_TEST_P(CollisionDetectorTest, DefaultNotInCollision) { - robot_state::RobotState robot_state(this->robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -115,7 +115,7 @@ TYPED_TEST_P(CollisionDetectorTest, LinksInCollision) // req.contacts = true; // req.max_contacts = 100; - robot_state::RobotState robot_state(this->robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -154,7 +154,7 @@ TYPED_TEST_P(CollisionDetectorTest, ContactReporting) req.contacts = true; req.max_contacts = 1; - robot_state::RobotState robot_state(this->robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -208,7 +208,7 @@ TYPED_TEST_P(CollisionDetectorTest, ContactPositions) req.contacts = true; req.max_contacts = 1; - robot_state::RobotState robot_state(this->robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -278,7 +278,7 @@ TYPED_TEST_P(CollisionDetectorTest, AttachedBodyTester) this->acm_.reset(new collision_detection::AllowedCollisionMatrix(this->robot_model_->getLinkModelNames(), true)); - robot_state::RobotState robot_state(this->robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -341,7 +341,7 @@ TYPED_TEST_P(CollisionDetectorTest, AttachedBodyTester) TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) { - robot_state::RobotState robot_state(this->robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -403,7 +403,7 @@ TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) this->cenv_->getWorld()->addToObject("kinect", shape, pos1); - robot_state::RobotState robot_state(this->robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -419,8 +419,8 @@ TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) collision_detection::CollisionEnv::ObjectConstPtr object = this->cenv_->getWorld()->getObject("kinect"); this->cenv_->getWorld()->removeObject("kinect"); - robot_state::RobotState robot_state1(this->robot_model_); - robot_state::RobotState robot_state2(this->robot_model_); + moveit::core::RobotState robot_state1(this->robot_model_); + moveit::core::RobotState robot_state2(this->robot_model_); robot_state1.setToDefaultValues(); robot_state2.setToDefaultValues(); robot_state1.update(); @@ -475,7 +475,7 @@ TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed) TYPED_TEST_P(CollisionDetectorTest, MoveMesh) { - robot_state::RobotState robot_state1(this->robot_model_); + moveit::core::RobotState robot_state1(this->robot_model_); robot_state1.setToDefaultValues(); robot_state1.update(); @@ -499,7 +499,7 @@ TYPED_TEST_P(CollisionDetectorTest, MoveMesh) TYPED_TEST_P(CollisionDetectorTest, TestChangingShapeSize) { - robot_state::RobotState robot_state1(this->robot_model_); + moveit::core::RobotState robot_state1(this->robot_model_); robot_state1.setToDefaultValues(); robot_state1.update(); diff --git a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp index bf84110b23..9f6c842b5a 100644 --- a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp +++ b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp @@ -39,13 +39,13 @@ const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME("ALL_VALID"); -collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const robot_model::RobotModelConstPtr& robot_model, +collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, double padding, double scale) : CollisionEnv(robot_model, padding, scale) { } -collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const robot_model::RobotModelConstPtr& robot_model, +collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, double padding, double scale) : CollisionEnv(robot_model, world, padding, scale) { @@ -57,7 +57,7 @@ collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const CollisionE } void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { res.collision = false; if (req.verbose) @@ -65,7 +65,7 @@ void collision_detection::CollisionEnvAllValid::checkRobotCollision(const Collis } void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { res.collision = false; @@ -74,8 +74,8 @@ void collision_detection::CollisionEnvAllValid::checkRobotCollision(const Collis } void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const { res.collision = false; if (req.verbose) @@ -83,8 +83,8 @@ void collision_detection::CollisionEnvAllValid::checkRobotCollision(const Collis } void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const { res.collision = false; @@ -99,19 +99,19 @@ void collision_detection::CollisionEnvAllValid::distanceRobot(const collision_de res.collision = false; } -double collision_detection::CollisionEnvAllValid::distanceRobot(const robot_state::RobotState& state) const +double collision_detection::CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& state) const { return 0.0; } -double collision_detection::CollisionEnvAllValid::distanceRobot(const robot_state::RobotState& state, +double collision_detection::CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { return 0.0; } void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { res.collision = false; if (req.verbose) @@ -119,7 +119,7 @@ void collision_detection::CollisionEnvAllValid::checkSelfCollision(const Collisi } void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { res.collision = false; diff --git a/moveit_core/collision_detection/src/collision_env.cpp b/moveit_core/collision_detection/src/collision_env.cpp index 131b25676c..701ca6f354 100644 --- a/moveit_core/collision_detection/src/collision_env.cpp +++ b/moveit_core/collision_detection/src/collision_env.cpp @@ -69,7 +69,7 @@ static inline bool validatePadding(double padding) namespace collision_detection { -CollisionEnv::CollisionEnv(const robot_model::RobotModelConstPtr& model, double padding, double scale) +CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, double padding, double scale) : robot_model_(model), world_(new World()), world_const_(world_) { if (!validateScale(scale)) @@ -77,7 +77,7 @@ CollisionEnv::CollisionEnv(const robot_model::RobotModelConstPtr& model, double if (!validatePadding(padding)) padding = 0.0; - const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); for (auto link : links) { link_padding_[link->getName()] = padding; @@ -85,7 +85,7 @@ CollisionEnv::CollisionEnv(const robot_model::RobotModelConstPtr& model, double } } -CollisionEnv::CollisionEnv(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding, +CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding, double scale) : robot_model_(model), world_(world), world_const_(world_) { @@ -94,7 +94,7 @@ CollisionEnv::CollisionEnv(const robot_model::RobotModelConstPtr& model, const W if (!validatePadding(padding)) padding = 0.0; - const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); for (auto link : links) { link_padding_[link->getName()] = padding; @@ -113,7 +113,7 @@ void CollisionEnv::setPadding(double padding) if (!validatePadding(padding)) return; std::vector u; - const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); for (auto link : links) { if (getLinkPadding(link->getName()) != padding) @@ -129,7 +129,7 @@ void CollisionEnv::setScale(double scale) if (!validateScale(scale)) return; std::vector u; - const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); for (auto link : links) { if (getLinkScale(link->getName()) != scale) @@ -289,7 +289,7 @@ void CollisionEnv::setWorld(const WorldPtr& world) } void CollisionEnv::checkCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { checkSelfCollision(req, res, state); if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) @@ -297,7 +297,7 @@ void CollisionEnv::checkCollision(const CollisionRequest& req, CollisionResult& } void CollisionEnv::checkCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { checkSelfCollision(req, res, state, acm); if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) diff --git a/moveit_core/collision_detection/test/test_all_valid.cpp b/moveit_core/collision_detection/test/test_all_valid.cpp index 435f473980..94c0bc36d7 100644 --- a/moveit_core/collision_detection/test/test_all_valid.cpp +++ b/moveit_core/collision_detection/test/test_all_valid.cpp @@ -47,8 +47,8 @@ TEST(AllValid, Instantiate) urdf_model.reset(new urdf::ModelInterface()); srdf::ModelConstSharedPtr srdf_model; srdf_model.reset(new srdf::Model()); - robot_model::RobotModelConstPtr robot_model; - robot_model.reset(new robot_model::RobotModel(urdf_model, srdf_model)); + moveit::core::RobotModelConstPtr robot_model; + robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model)); CollisionEnvAllValid env(robot_model); } diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h index 31a0a9e070..bfaee98429 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h @@ -48,9 +48,9 @@ class CollisionEnvBullet : public CollisionEnv public: CollisionEnvBullet() = delete; - CollisionEnvBullet(const robot_model::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); + CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); - CollisionEnvBullet(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, + CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, double scale = 1.0); CollisionEnvBullet(const CollisionEnvBullet& other, const WorldPtr& world); @@ -60,38 +60,38 @@ class CollisionEnvBullet : public CollisionEnv CollisionEnvBullet(CollisionEnvBullet&) = delete; virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; + const moveit::core::RobotState& state) const override; virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; + const moveit::core::RobotState& state) const override; virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const override; + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override; + const moveit::core::RobotState& state) const override; virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override; + const moveit::core::RobotState& state) const override; void setWorld(const WorldPtr& world) override; protected: /** \brief Updates the poses of the objects in the manager according to given robot state */ - void updateTransformsFromState(const robot_state::RobotState& state, + void updateTransformsFromState(const moveit::core::RobotState& state, const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const; /** \brief Updates the collision objects saved in the manager to reflect a new padding or scaling of the robot links @@ -99,19 +99,19 @@ class CollisionEnvBullet : public CollisionEnv void updatedPaddingOrScaling(const std::vector& links) override; /** \brief All of the attached objects in the robot state are wrapped into bullet collision objects */ - void addAttachedOjects(const robot_state::RobotState& state, + void addAttachedOjects(const moveit::core::RobotState& state, std::vector& cows) const; /** \brief Bundles the different checkSelfCollision functions into a single function */ - void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const; + void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; void checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, const AllowedCollisionMatrix* acm) const; void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; /** \brief Construts a bullet collision object out of a robot link */ void addLinkAsCollisionObject(const urdf::LinkSharedPtr& link); diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp index f42bc09b6e..079943d03b 100644 --- a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp @@ -46,7 +46,7 @@ namespace collision_detection const std::string CollisionDetectorAllocatorBullet::NAME("Bullet"); const double MAX_DISTANCE_MARGIN = 99; -CollisionEnvBullet::CollisionEnvBullet(const robot_model::RobotModelConstPtr& model, double padding, double scale) +CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, double padding, double scale) : CollisionEnv(model, padding, scale) { // request notifications about changes to new world @@ -58,7 +58,7 @@ CollisionEnvBullet::CollisionEnvBullet(const robot_model::RobotModelConstPtr& mo } } -CollisionEnvBullet::CollisionEnvBullet(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, +CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding, double scale) : CollisionEnv(model, world, padding, scale) { @@ -93,20 +93,20 @@ CollisionEnvBullet::~CollisionEnvBullet() } void CollisionEnvBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { checkSelfCollisionHelper(req, res, state, nullptr); } void CollisionEnvBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { checkSelfCollisionHelper(req, res, state, &acm); } void CollisionEnvBullet::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const { std::vector cows; @@ -139,35 +139,35 @@ void CollisionEnvBullet::checkSelfCollisionHelper(const CollisionRequest& req, C } void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { checkRobotCollisionHelper(req, res, state, nullptr); } void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { checkRobotCollisionHelper(req, res, state, &acm); } void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const { checkRobotCollisionHelperCCD(req, res, state1, state2, nullptr); } void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const { checkRobotCollisionHelperCCD(req, res, state1, state2, &acm); } void CollisionEnvBullet::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const { if (req.distance) @@ -195,8 +195,8 @@ void CollisionEnvBullet::checkRobotCollisionHelper(const CollisionRequest& req, } void CollisionEnvBullet::checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix* acm) const { std::vector attached_cows; @@ -225,13 +225,13 @@ void CollisionEnvBullet::checkRobotCollisionHelperCCD(const CollisionRequest& re } void CollisionEnvBullet::distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { ROS_INFO_NAMED("collision_detection.bullet", "Distance to self not implemented yet."); } void CollisionEnvBullet::distanceRobot(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { ROS_INFO_NAMED("collision_detection.bullet", "Distance to self not implemented yet."); } @@ -313,13 +313,13 @@ void CollisionEnvBullet::notifyObjectChange(const ObjectConstPtr& obj, World::Ac } void CollisionEnvBullet::addAttachedOjects( - const robot_state::RobotState& state, + const moveit::core::RobotState& state, std::vector& cows) const { - std::vector attached_bodies; + std::vector attached_bodies; state.getAttachedBodies(attached_bodies); - for (const robot_state::AttachedBody*& body : attached_bodies) + for (const moveit::core::AttachedBody*& body : attached_bodies) { const EigenSTL::vector_Isometry3d& attached_body_transform = body->getGlobalCollisionBodyTransforms(); @@ -357,7 +357,7 @@ void CollisionEnvBullet::updatedPaddingOrScaling(const std::vector& } void CollisionEnvBullet::updateTransformsFromState( - const robot_state::RobotState& state, const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const + const moveit::core::RobotState& state, const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const { // updating link positions with the current robot state for (const std::string& link : active_) diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp index 4dd5115d2b..3e9d99cef7 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp @@ -54,7 +54,7 @@ namespace cb = collision_detection_bullet; /** \brief Brings the panda robot in user defined home position */ -inline void setToHome(robot_state::RobotState& panda_state) +inline void setToHome(moveit::core::RobotState& panda_state) { panda_state.setToDefaultValues(); double joint2 = -0.785; @@ -88,7 +88,7 @@ class BulletCollisionDetectionTester : public testing::Test cenv_.reset(new collision_detection::CollisionEnvBullet(robot_model_)); - robot_state_.reset(new robot_state::RobotState(robot_model_)); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); setToHome(*robot_state_); } @@ -100,13 +100,13 @@ class BulletCollisionDetectionTester : public testing::Test protected: bool robot_model_ok_; - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; collision_detection::CollisionEnvPtr cenv_; collision_detection::AllowedCollisionMatrixPtr acm_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotStatePtr robot_state_; }; void addCollisionObjects(cb::BulletCastBVHManager& checker) @@ -231,8 +231,8 @@ TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf) collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - robot_state::RobotState state1(robot_model_); - robot_state::RobotState state2(robot_model_); + moveit::core::RobotState state1(robot_model_); + moveit::core::RobotState state2(robot_model_); setToHome(state1); double joint2 = 0.15; @@ -274,8 +274,8 @@ TEST_F(BulletCollisionDetectionTester, ContinuousCollisionWorld) req.max_contacts = 10; collision_detection::CollisionResult res; - robot_state::RobotState state1(robot_model_); - robot_state::RobotState state2(robot_model_); + moveit::core::RobotState state1(robot_model_); + moveit::core::RobotState state2(robot_model_); setToHome(state1); state1.update(); diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index c5b89cca28..66beae404d 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -62,13 +62,14 @@ MOVEIT_STRUCT_FORWARD(CollisionGeometryData); struct CollisionGeometryData { /** \brief Constructor for a robot link collision geometry object. */ - CollisionGeometryData(const robot_model::LinkModel* link, int index) : type(BodyTypes::ROBOT_LINK), shape_index(index) + CollisionGeometryData(const moveit::core::LinkModel* link, int index) + : type(BodyTypes::ROBOT_LINK), shape_index(index) { ptr.link = link; } /** \brief Constructor for a new collision geometry object which is attached to the robot. */ - CollisionGeometryData(const robot_state::AttachedBody* ab, int index) + CollisionGeometryData(const moveit::core::AttachedBody* ab, int index) : type(BodyTypes::ROBOT_ATTACHED), shape_index(index) { ptr.ab = ab; @@ -128,8 +129,8 @@ struct CollisionGeometryData /** \brief Points to the type of body which contains the geometry. */ union { - const robot_model::LinkModel* link; - const robot_state::AttachedBody* ab; + const moveit::core::LinkModel* link; + const moveit::core::AttachedBody* ab; const World::Object* obj; const void* raw; } ptr; @@ -152,7 +153,7 @@ struct CollisionData } /** \brief Compute \e active_components_only_ based on the joint group specified in \e req_ */ - void enableGroup(const robot_model::RobotModelConstPtr& robot_model); + void enableGroup(const moveit::core::RobotModelConstPtr& robot_model); /** \brief The collision request passed by the user */ const CollisionRequest* req_; @@ -161,7 +162,7 @@ struct CollisionData * are considered for collision. * * If the pointer is NULL, all collisions are considered. */ - const std::set* active_components_only_; + const std::set* active_components_only_; /** \brief The user-specified response location. */ CollisionResult* res_; @@ -203,14 +204,14 @@ struct FCLGeometry } /** \brief Constructor for a robot link. */ - FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const robot_model::LinkModel* link, int shape_index) + FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const moveit::core::LinkModel* link, int shape_index) : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(link, shape_index)) { collision_geometry_->setUserData(collision_geometry_data_.get()); } /** \brief Constructor for an attached body. */ - FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const robot_state::AttachedBody* ab, int shape_index) + FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const moveit::core::AttachedBody* ab, int shape_index) : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(ab, shape_index)) { collision_geometry_->setUserData(collision_geometry_data_.get()); @@ -285,11 +286,11 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& min_dist); /** \brief Create new FCLGeometry object out of robot link model. */ -FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link, +FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::LinkModel* link, int shape_index); /** \brief Create new FCLGeometry object out of attached body. */ -FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab, +FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::AttachedBody* ab, int shape_index); /** \brief Create new FCLGeometry object out of a world object. @@ -299,11 +300,11 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, /** \brief Create new scaled and / or padded FCLGeometry object out of robot link model. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, - const robot_model::LinkModel* link, int shape_index); + const moveit::core::LinkModel* link, int shape_index); /** \brief Create new scaled and / or padded FCLGeometry object out of an attached body. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, - const robot_state::AttachedBody* ab, int shape_index); + const moveit::core::AttachedBody* ab, int shape_index); /** \brief Create new scaled and / or padded FCLGeometry object out of an world object. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h index 8906f8155f..5c9674cf32 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h @@ -55,9 +55,9 @@ class CollisionEnvFCL : public CollisionEnv public: CollisionEnvFCL() = delete; - CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); + CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); - CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, + CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, double scale = 1.0); CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& world); @@ -65,32 +65,32 @@ class CollisionEnvFCL : public CollisionEnv ~CollisionEnvFCL() override; virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; + const moveit::core::RobotState& state) const override; virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; + const moveit::core::RobotState& state) const override; virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const override; + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override; + const moveit::core::RobotState& state) const override; virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override; + const moveit::core::RobotState& state) const override; void setWorld(const WorldPtr& world) override; @@ -105,12 +105,12 @@ class CollisionEnvFCL : public CollisionEnv void updatedPaddingOrScaling(const std::vector& links) override; /** \brief Bundles the different checkSelfCollision functions into a single function */ - void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const; + void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; /** \brief Bundles the different checkRobotCollision functions into a single function */ void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; /** \brief Construct an FCL collision object from MoveIt's World::Object. */ void constructFCLObjectWorld(const World::Object* obj, FCLObject& fcl_obj) const; @@ -128,11 +128,11 @@ class CollisionEnvFCL : public CollisionEnv * * \param state The current robot state * \param fcl_obj The newly filled object */ - void constructFCLObjectRobot(const robot_state::RobotState& state, FCLObject& fcl_obj) const; + void constructFCLObjectRobot(const moveit::core::RobotState& state, FCLObject& fcl_obj) const; /** \brief Prepares for the collision check through constructing an FCL collision object out of the current robot * state and specifying a broadphase collision manager of FCL where the constructed object is registered to. */ - void allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const; + void allocSelfCollisionBroadPhase(const moveit::core::RobotState& state, FCLManager& manager) const; /** \brief Converts all shapes which make up an atttached body into a vector of FCLGeometryConstPtr. * @@ -141,7 +141,7 @@ class CollisionEnvFCL : public CollisionEnv * \param ab Pointer to the attached body * \param geoms Output vector of geometries */ - void getAttachedBodyObjects(const robot_state::AttachedBody* ab, std::vector& geoms) const; + void getAttachedBodyObjects(const moveit::core::AttachedBody* ab, std::vector& geoms) const; /** \brief Vector of shared pointers to the FCL geometry for the objects in fcl_objs_. */ std::vector robot_geoms_; diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index 0cf3446237..a569479207 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -67,11 +67,11 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi // If active components are specified if (cdata->active_components_only_) { - const robot_model::LinkModel* l1 = + const moveit::core::LinkModel* l1 = cd1->type == BodyTypes::ROBOT_LINK ? cd1->ptr.link : (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : nullptr); - const robot_model::LinkModel* l2 = + const moveit::core::LinkModel* l2 = cd2->type == BodyTypes::ROBOT_LINK ? cd2->ptr.link : (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : nullptr); @@ -414,11 +414,11 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void // If active components are specified if (cdata->req->active_components_only) { - const robot_model::LinkModel* l1 = + const moveit::core::LinkModel* l1 = cd1->type == BodyTypes::ROBOT_LINK ? cd1->ptr.link : (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : nullptr); - const robot_model::LinkModel* l2 = + const moveit::core::LinkModel* l2 = cd2->type == BodyTypes::ROBOT_LINK ? cd2->ptr.link : (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : nullptr); @@ -707,7 +707,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, // attached objects could have previously been World::Object; we try to move them // from their old cache to the new one, if possible. the code is not pretty, but should help // when we attach/detach objects that are in the world - if (IfSameType::value == 1) + if (IfSameType::value == 1) { // get the cache that corresponds to objects; maybe this attached object used to be a world object FCLShapeCache& othercache = GetShapeCache(); @@ -743,7 +743,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, if (IfSameType::value == 1) { // get the cache that corresponds to objects; maybe this attached object used to be a world object - FCLShapeCache& othercache = GetShapeCache(); + FCLShapeCache& othercache = GetShapeCache(); // attached bodies could be just moved from the environment. auto cache_it = othercache.map_.find(wptr); @@ -852,16 +852,16 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, return FCLGeometryConstPtr(); } -FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link, +FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::LinkModel* link, int shape_index) { - return createCollisionGeometry(shape, link, shape_index); + return createCollisionGeometry(shape, link, shape_index); } -FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab, +FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::AttachedBody* ab, int shape_index) { - return createCollisionGeometry(shape, ab, shape_index); + return createCollisionGeometry(shape, ab, shape_index); } FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj) @@ -887,15 +887,15 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, } FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, - const robot_model::LinkModel* link, int shape_index) + const moveit::core::LinkModel* link, int shape_index) { - return createCollisionGeometry(shape, scale, padding, link, shape_index); + return createCollisionGeometry(shape, scale, padding, link, shape_index); } FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, - const robot_state::AttachedBody* ab, int shape_index) + const moveit::core::AttachedBody* ab, int shape_index) { - return createCollisionGeometry(shape, scale, padding, ab, shape_index); + return createCollisionGeometry(shape, scale, padding, ab, shape_index); } FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, @@ -910,13 +910,13 @@ void cleanCollisionGeometryCache() { cache1.bumpUseCount(true); } - FCLShapeCache& cache2 = GetShapeCache(); + FCLShapeCache& cache2 = GetShapeCache(); { cache2.bumpUseCount(true); } } -void CollisionData::enableGroup(const robot_model::RobotModelConstPtr& robot_model) +void CollisionData::enableGroup(const moveit::core::RobotModelConstPtr& robot_model) { if (robot_model->hasJointModelGroup(req_->group_name)) active_components_only_ = &robot_model->getJointModelGroup(req_->group_name)->getUpdatedLinkModelsSet(); diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index 20ef171fbc..f44f4b7d56 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -48,10 +48,10 @@ namespace collision_detection { const std::string CollisionDetectorAllocatorFCL::NAME("FCL"); -CollisionEnvFCL::CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, double padding, double scale) +CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, double padding, double scale) : CollisionEnv(model, padding, scale) { - const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); std::size_t index; robot_geoms_.resize(robot_model_->getLinkGeometryCount()); robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount()); @@ -86,11 +86,11 @@ CollisionEnvFCL::CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, d observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); } -CollisionEnvFCL::CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding, +CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding, double scale) : CollisionEnv(model, world, padding, scale) { - const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); std::size_t index; robot_geoms_.resize(robot_model_->getLinkGeometryCount()); robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount()); @@ -148,7 +148,7 @@ CollisionEnvFCL::CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& w observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); } -void CollisionEnvFCL::getAttachedBodyObjects(const robot_state::AttachedBody* ab, +void CollisionEnvFCL::getAttachedBodyObjects(const moveit::core::AttachedBody* ab, std::vector& geoms) const { const std::vector& shapes = ab->getShapes(); @@ -174,7 +174,7 @@ void CollisionEnvFCL::constructFCLObjectWorld(const World::Object* obj, FCLObjec } } -void CollisionEnvFCL::constructFCLObjectRobot(const robot_state::RobotState& state, FCLObject& fcl_obj) const +void CollisionEnvFCL::constructFCLObjectRobot(const moveit::core::RobotState& state, FCLObject& fcl_obj) const { fcl_obj.collision_objects_.reserve(robot_geoms_.size()); fcl::Transform3d fcl_tf; @@ -191,8 +191,8 @@ void CollisionEnvFCL::constructFCLObjectRobot(const robot_state::RobotState& sta fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(coll_obj)); } - // TODO: Implement a method for caching fcl::CollisionObject's for robot_state::AttachedBody's - std::vector ab; + // TODO: Implement a method for caching fcl::CollisionObject's for moveit::core::AttachedBody's + std::vector ab; state.getAttachedBodies(ab); for (auto& body : ab) { @@ -212,7 +212,7 @@ void CollisionEnvFCL::constructFCLObjectRobot(const robot_state::RobotState& sta } } -void CollisionEnvFCL::allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const +void CollisionEnvFCL::allocSelfCollisionBroadPhase(const moveit::core::RobotState& state, FCLManager& manager) const { auto m = new fcl::DynamicAABBTreeCollisionManagerd(); // m->tree_init_level = 2; @@ -223,19 +223,19 @@ void CollisionEnvFCL::allocSelfCollisionBroadPhase(const robot_state::RobotState } void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { checkSelfCollisionHelper(req, res, state, nullptr); } void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { checkSelfCollisionHelper(req, res, state, &acm); } void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const { FCLManager manager; @@ -257,33 +257,35 @@ void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, Coll } void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { checkRobotCollisionHelper(req, res, state, nullptr); } void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const { checkRobotCollisionHelper(req, res, state, &acm); } void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const { ROS_ERROR_NAMED("collision_detection.bullet", "Continuous collision not implemented"); } void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const { ROS_ERROR_NAMED("collision_detection.fcl", "Not implemented"); } void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const { FCLObject fcl_obj; @@ -308,7 +310,7 @@ void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, Col } void CollisionEnvFCL::distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { FCLManager manager; allocSelfCollisionBroadPhase(state, manager); @@ -318,7 +320,7 @@ void CollisionEnvFCL::distanceSelf(const DistanceRequest& req, DistanceResult& r } void CollisionEnvFCL::distanceRobot(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { FCLObject fcl_obj; constructFCLObjectRobot(state, fcl_obj); @@ -411,7 +413,7 @@ void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector& li std::size_t index; for (const auto& link : links) { - const robot_model::LinkModel* lmodel = robot_model_->getLinkModel(link); + const moveit::core::LinkModel* lmodel = robot_model_->getLinkModel(link); if (lmodel) { for (std::size_t j = 0; j < lmodel->getShapes().size(); ++j) diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp index 64f65b64c3..a2803e223d 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp @@ -50,7 +50,7 @@ #include /** \brief Brings the panda robot in user defined home position */ -inline void setToHome(robot_state::RobotState& panda_state) +inline void setToHome(moveit::core::RobotState& panda_state) { panda_state.setToDefaultValues(); double joint2 = -0.785; @@ -90,7 +90,7 @@ class CollisionDetectionEnvTest : public testing::Test c_env_.reset(new collision_detection::CollisionEnvFCL(robot_model_)); - robot_state_.reset(new robot_state::RobotState(robot_model_)); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); setToHome(*robot_state_); } @@ -102,13 +102,13 @@ class CollisionDetectionEnvTest : public testing::Test protected: bool robot_model_ok_; - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; collision_detection::CollisionEnvPtr c_env_; collision_detection::AllowedCollisionMatrixPtr acm_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotStatePtr robot_state_; }; /** \brief Correct setup testing. */ @@ -230,8 +230,8 @@ TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionSelf) collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - robot_state::RobotState state1(robot_model_); - robot_state::RobotState state2(robot_model_); + moveit::core::RobotState state1(robot_model_); + moveit::core::RobotState state2(robot_model_); setToHome(state1); double joint2 = 0.15; @@ -276,8 +276,8 @@ TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionWorld) req.max_contacts = 10; collision_detection::CollisionResult res; - robot_state::RobotState state1(robot_model_); - robot_state::RobotState state2(robot_model_); + moveit::core::RobotState state1(robot_model_); + moveit::core::RobotState state2(robot_model_); setToHome(state1); state1.update(); diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h index cfb6fe750d..70cc449dfc 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h @@ -114,7 +114,7 @@ struct DistanceFieldCacheEntry /** for checking collisions between this group and other objects */ std::string group_name_; /** RobotState that this cache entry represents */ - robot_state::RobotStatePtr state_; + moveit::core::RobotStatePtr state_; /** list of indices into the state_values_ vector. One index for each joint * variable which is NOT in the group or a child of the group. In other * words, variables which should not change if only joints in the group move. @@ -170,10 +170,10 @@ BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeCons PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const collision_detection::World::Object& obj, double resolution); -PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const robot_state::AttachedBody* att, +PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const moveit::core::AttachedBody* att, double resolution); -PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const robot_state::AttachedBody* att, +PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const moveit::core::AttachedBody* att, double resolution); void getBodySphereVisualizationMarkers(const GroupStateRepresentationPtr& gsr, const std::string& reference_frame, diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h index 2a5e4a62b6..5d7725faf3 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h @@ -60,7 +60,7 @@ class CollisionEnvDistanceField : public CollisionEnv public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - CollisionEnvDistanceField(const robot_model::RobotModelConstPtr& robot_model, + CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr& robot_model, const std::map>& link_body_decompositions = std::map>(), double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, @@ -71,7 +71,7 @@ class CollisionEnvDistanceField : public CollisionEnv double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, double scale = 1.0); - CollisionEnvDistanceField(const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, + CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, const std::map>& link_body_decompositions = std::map>(), double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, @@ -117,7 +117,7 @@ class CollisionEnvDistanceField : public CollisionEnv } void distanceSelf(const DistanceRequest& /* req */, DistanceResult& /* res */, - const robot_state::RobotState& /* state */) const override + const moveit::core::RobotState& /* state */) const override { ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); } @@ -145,46 +145,46 @@ class CollisionEnvDistanceField : public CollisionEnv ~CollisionEnvDistanceField() override; void checkCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; + const moveit::core::RobotState& state) const override; - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; - void checkCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; + const moveit::core::RobotState& state) const override; virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const override; + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; - virtual double distanceRobot(const robot_state::RobotState& state, bool verbose = false) const + virtual double distanceRobot(const moveit::core::RobotState& state, bool verbose = false) const { (void)state; (void)verbose; return 0.0; } - virtual double distanceRobot(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, + virtual double distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, bool verbose = false) const { (void)state; @@ -194,7 +194,7 @@ class CollisionEnvDistanceField : public CollisionEnv } void distanceRobot(const DistanceRequest& /* req */, DistanceResult& /* res */, - const robot_state::RobotState& /* state */) const override + const moveit::core::RobotState& /* state */) const override { ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); } @@ -211,10 +211,10 @@ class CollisionEnvDistanceField : public CollisionEnv return last_gsr_; } - void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; - void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; protected: diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h index 6acf5da2ae..eb98812a43 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h @@ -52,7 +52,7 @@ class CollisionEnvHybrid : public collision_detection::CollisionEnvFCL public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - CollisionEnvHybrid(const robot_model::RobotModelConstPtr& robot_model, + CollisionEnvHybrid(const moveit::core::RobotModelConstPtr& robot_model, const std::map>& link_body_decompositions = std::map>(), double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, @@ -62,7 +62,7 @@ class CollisionEnvHybrid : public collision_detection::CollisionEnvFCL double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, double scale = 1.0); - CollisionEnvHybrid(const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, + CollisionEnvHybrid(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, const std::map>& link_body_decompositions = std::map>(), double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, @@ -89,18 +89,18 @@ class CollisionEnvHybrid : public collision_detection::CollisionEnvFCL void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& state) const; + const moveit::core::RobotState& state) const; void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const robot_state::RobotState& state, + collision_detection::CollisionResult& res, const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const robot_state::RobotState& state, + collision_detection::CollisionResult& res, const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix& acm) const; void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const robot_state::RobotState& state, + collision_detection::CollisionResult& res, const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; const CollisionEnvDistanceFieldConstPtr getCollisionRobotDistanceField() const @@ -109,37 +109,37 @@ class CollisionEnvHybrid : public collision_detection::CollisionEnvFCL } void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const; + const moveit::core::RobotState& state) const; void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const; + const moveit::core::RobotState& state) const; void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; void setWorld(const WorldPtr& world) override; - void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; - void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, + void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; const CollisionEnvDistanceFieldConstPtr getCollisionWorldDistanceField() const diff --git a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp index 83e8dc4065..0baa38a20b 100644 --- a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp @@ -100,7 +100,7 @@ PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const return ret; } -PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const robot_state::AttachedBody* att, +PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const moveit::core::AttachedBody* att, double resolution) { PosedBodySphereDecompositionVectorPtr ret(new PosedBodySphereDecompositionVector()); @@ -114,7 +114,7 @@ PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const r return ret; } -PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const robot_state::AttachedBody* att, +PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const moveit::core::AttachedBody* att, double resolution) { PosedBodyPointDecompositionVectorPtr ret(new PosedBodyPointDecompositionVector()); diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index 6ddac01a3f..d65dd91342 100644 --- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -53,7 +53,7 @@ const double EPSILON = 0.001f; const std::string collision_detection::CollisionDetectorAllocatorDistanceField::NAME("DISTANCE_FIELD"); CollisionEnvDistanceField::CollisionEnvDistanceField( - const robot_model::RobotModelConstPtr& robot_model, + const moveit::core::RobotModelConstPtr& robot_model, const std::map>& link_body_decompositions, double size_x, double size_y, double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double padding, double scale) @@ -71,7 +71,7 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( } CollisionEnvDistanceField::CollisionEnvDistanceField( - const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, + const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, const std::map>& link_body_decompositions, double size_x, double size_y, double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double padding, double scale) @@ -1388,14 +1388,14 @@ bool CollisionEnvDistanceField::compareCacheEntryToAllowedCollisionMatrix( // } void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { GroupStateRepresentationPtr gsr; checkCollision(req, res, state, gsr); } void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const { if (!gsr) @@ -1420,7 +1420,7 @@ void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, Coll } void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { GroupStateRepresentationPtr gsr; @@ -1428,7 +1428,7 @@ void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, Coll } void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const { if (!gsr) @@ -1453,14 +1453,14 @@ void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, Coll } void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { GroupStateRepresentationPtr gsr; checkRobotCollision(req, res, state, gsr); } void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const { distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_; @@ -1479,7 +1479,7 @@ void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, } void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { GroupStateRepresentationPtr gsr; @@ -1487,7 +1487,7 @@ void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, } void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const { @@ -1508,22 +1508,22 @@ void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, } void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const { ROS_ERROR_NAMED("collision_detection.distance", "Continuous collision checking not implemented"); } void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const { ROS_ERROR_NAMED("collision_detection.distance", "Continuous collision checking not implemented"); } void CollisionEnvDistanceField::getCollisionGradients(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const { @@ -1546,7 +1546,7 @@ void CollisionEnvDistanceField::getCollisionGradients(const CollisionRequest& re } void CollisionEnvDistanceField::getAllCollisions(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const { diff --git a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp index 7a367c012f..256ad6a9a7 100644 --- a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp @@ -43,7 +43,7 @@ namespace collision_detection const std::string collision_detection::CollisionDetectorAllocatorHybrid::NAME("HYBRID"); CollisionEnvHybrid::CollisionEnvHybrid( - const robot_model::RobotModelConstPtr& robot_model, + const moveit::core::RobotModelConstPtr& robot_model, const std::map>& link_body_decompositions, double size_x, double size_y, double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double padding, double scale) @@ -55,7 +55,7 @@ CollisionEnvHybrid::CollisionEnvHybrid( } CollisionEnvHybrid::CollisionEnvHybrid( - const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, + const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, const std::map>& link_body_decompositions, double size_x, double size_y, double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double padding, double scale) @@ -74,14 +74,14 @@ CollisionEnvHybrid::CollisionEnvHybrid(const CollisionEnvHybrid& other, const Wo void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { cenv_distance_->checkSelfCollision(req, res, state); } void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const { cenv_distance_->checkSelfCollision(req, res, state, gsr); @@ -89,7 +89,7 @@ void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detecti void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix& acm) const { cenv_distance_->checkSelfCollision(req, res, state, acm); @@ -97,7 +97,7 @@ void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detecti void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const { @@ -105,27 +105,27 @@ void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detecti } void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { cenv_distance_->checkCollision(req, res, state); } void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const { cenv_distance_->checkCollision(req, res, state, gsr); } void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { cenv_distance_->checkCollision(req, res, state, acm); } void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const { @@ -133,27 +133,27 @@ void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req } void CollisionEnvHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const + const moveit::core::RobotState& state) const { cenv_distance_->checkRobotCollision(req, res, state); } void CollisionEnvHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const { cenv_distance_->checkRobotCollision(req, res, state, gsr); } void CollisionEnvHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { cenv_distance_->checkRobotCollision(req, res, state, acm); } void CollisionEnvHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const { @@ -170,14 +170,14 @@ void CollisionEnvHybrid::setWorld(const WorldPtr& world) } void CollisionEnvHybrid::getCollisionGradients(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const { cenv_distance_->getCollisionGradients(req, res, state, acm, gsr); } void CollisionEnvHybrid::getAllCollisions(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const { cenv_distance_->getAllCollisions(req, res, state, acm, gsr); diff --git a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp index c744150492..56b0bc553c 100644 --- a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp +++ b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp @@ -80,7 +80,7 @@ class DistanceFieldCollisionDetectionTester : public testing::Test srdf_ok_ = srdf_model_->initFile(*urdf_model_, ros::package::getPath("moveit_resources") + "/pr2_description/srdf/robot.xml"); - robot_model_.reset(new robot_model::RobotModel(urdf_model_, srdf_model_)); + robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_model_)); acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); @@ -99,10 +99,10 @@ class DistanceFieldCollisionDetectionTester : public testing::Test urdf::ModelInterfaceSharedPtr urdf_model_; srdf::ModelSharedPtr srdf_model_; - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; - robot_state::TransformsPtr ftf_; - robot_state::TransformsConstPtr ftf_const_; + moveit::core::TransformsPtr ftf_; + moveit::core::TransformsConstPtr ftf_const_; collision_detection::CollisionEnvPtr cenv_; @@ -111,7 +111,7 @@ class DistanceFieldCollisionDetectionTester : public testing::Test TEST_F(DistanceFieldCollisionDetectionTester, DefaultNotInCollision) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -126,7 +126,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, DefaultNotInCollision) TEST_F(DistanceFieldCollisionDetectionTester, ChangeTorsoPosition) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -154,7 +154,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, LinksInCollision) // req.max_contacts = 100; req.group_name = "whole_body"; - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); Eigen::Isometry3d offset = Eigen::Isometry3d::Identity(); @@ -186,7 +186,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactReporting) req.max_contacts = 1; req.group_name = "whole_body"; - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); Eigen::Isometry3d offset = Eigen::Isometry3d::Identity(); @@ -235,7 +235,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactPositions) req.max_contacts = 1; req.group_name = "whole_body"; - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); @@ -299,7 +299,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -326,7 +326,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) poses.push_back(Eigen::Isometry3d::Identity()); std::set touch_links; trajectory_msgs::JointTrajectory empty_state; - robot_state::AttachedBody* attached_body = new robot_state::AttachedBody( + moveit::core::AttachedBody* attached_body = new moveit::core::AttachedBody( robot_state.getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state); robot_state.attachBody(attached_body); @@ -341,7 +341,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) touch_links.insert("r_gripper_palm_link"); shapes[0].reset(new shapes::Box(.1, .1, .1)); - robot_state::AttachedBody* attached_body_1 = new robot_state::AttachedBody( + moveit::core::AttachedBody* attached_body_1 = new moveit::core::AttachedBody( robot_state.getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state); robot_state.attachBody(attached_body_1); diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h index f86cc069fa..6b2074b390 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h @@ -104,7 +104,7 @@ class ConstraintSampler * * @return The joint model group */ - const robot_model::JointModelGroup* getJointModelGroup() const + const moveit::core::JointModelGroup* getJointModelGroup() const { return jmg_; } @@ -140,7 +140,7 @@ class ConstraintSampler * \brief Gets the callback used to determine state validity during sampling. The sampler will attempt * to satisfy this constraint if possible, but there is no guarantee. */ - const robot_state::GroupStateValidityCallbackFn& getGroupStateValidityCallback() const + const moveit::core::GroupStateValidityCallbackFn& getGroupStateValidityCallback() const { return group_state_validity_callback_; } @@ -151,7 +151,7 @@ class ConstraintSampler * * @param callback The callback to set */ - void setGroupStateValidityCallback(const robot_state::GroupStateValidityCallbackFn& callback) + void setGroupStateValidityCallback(const moveit::core::GroupStateValidityCallbackFn& callback) { group_state_validity_callback_ = callback; } @@ -166,7 +166,7 @@ class ConstraintSampler * * @return True if a sample was successfully taken, false otherwise */ - bool sample(robot_state::RobotState& state) + bool sample(moveit::core::RobotState& state) { return sample(state, state, DEFAULT_MAX_SAMPLING_ATTEMPTS); } @@ -182,7 +182,7 @@ class ConstraintSampler * * @return True if a sample was successfully taken, false otherwise */ - bool sample(robot_state::RobotState& state, unsigned int max_attempts) + bool sample(moveit::core::RobotState& state, unsigned int max_attempts) { return sample(state, state, max_attempts); } @@ -197,7 +197,7 @@ class ConstraintSampler * * @return True if a sample was successfully taken, false otherwise */ - bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state) + bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state) { return sample(state, reference_state, DEFAULT_MAX_SAMPLING_ATTEMPTS); } @@ -212,7 +212,7 @@ class ConstraintSampler * * @return True if a sample was successfully projected, false otherwise */ - bool project(robot_state::RobotState& state) + bool project(moveit::core::RobotState& state) { return project(state, DEFAULT_MAX_SAMPLING_ATTEMPTS); } @@ -228,7 +228,7 @@ class ConstraintSampler * * @return True if a sample was successfully taken, false otherwise */ - virtual bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state, + virtual bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts) = 0; /** @@ -242,7 +242,7 @@ class ConstraintSampler * * @return True if a sample was successfully projected, false otherwise */ - virtual bool project(robot_state::RobotState& state, unsigned int max_attempts) = 0; + virtual bool project(moveit::core::RobotState& state, unsigned int max_attempts) = 0; /** * \brief Returns whether or not the constraint sampler is valid or not. @@ -287,11 +287,11 @@ class ConstraintSampler /// Holds the planning scene planning_scene::PlanningSceneConstPtr scene_; /// Holds the joint model group associated with this constraint - const robot_model::JointModelGroup* const jmg_; + const moveit::core::JointModelGroup* const jmg_; /// Holds the set of frames that must exist in the reference state to allow samples to be drawn std::vector frame_depends_; /// Holds the callback for state validity - robot_state::GroupStateValidityCallbackFn group_state_validity_callback_; + moveit::core::GroupStateValidityCallbackFn group_state_validity_callback_; bool verbose_; ///< True if verbosity is on }; } diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h index f222aea111..1bc811c4e3 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h @@ -41,7 +41,7 @@ namespace constraint_samplers { -void visualizeDistribution(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state, +void visualizeDistribution(const ConstraintSamplerPtr& sampler, const moveit::core::RobotState& reference_state, const std::string& link_name, unsigned int sample_count, visualization_msgs::MarkerArray& markers); @@ -49,7 +49,7 @@ void visualizeDistribution(const moveit_msgs::Constraints& constr, const plannin const std::string& group, const std::string& link_name, unsigned int sample_count, visualization_msgs::MarkerArray& markers); -double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state); +double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const moveit::core::RobotState& reference_state); double countSamplesPerSecond(const moveit_msgs::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, const std::string& group); diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h index 399e3cc7ef..ffc4b5ea8d 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h @@ -116,9 +116,9 @@ class JointConstraintSampler : public ConstraintSampler */ bool configure(const std::vector& jc); - bool sample(robot_state::RobotState& state, const robot_state::RobotState& ks, unsigned int max_attempts) override; + bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& ks, unsigned int max_attempts) override; - bool project(robot_state::RobotState& state, unsigned int max_attempts) override; + bool project(moveit::core::RobotState& state, unsigned int max_attempts) override; /** * \brief Gets the number of constrained joints - joints that have an @@ -195,7 +195,7 @@ class JointConstraintSampler : public ConstraintSampler std::vector bounds_; /**< \brief The bounds for any joint with bounds that are more restrictive than the joint limits */ - std::vector unbounded_; /**< \brief The joints that are not bounded except by joint + std::vector unbounded_; /**< \brief The joints that are not bounded except by joint limits */ std::vector uindex_; /**< \brief The index of the unbounded joints in the joint state vector */ std::vector values_; /**< \brief Values associated with this group to avoid continuously reallocating */ @@ -445,10 +445,10 @@ class IKConstraintSampler : public ConstraintSampler * * @return True if a valid sample pose was produced and valid IK found for that pose. Otherwise false. */ - bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state, + bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts) override; - bool project(robot_state::RobotState& state, unsigned int max_attempts) override; + bool project(moveit::core::RobotState& state, unsigned int max_attempts) override; /** * \brief Returns a pose that falls within the constraint regions. * @@ -471,7 +471,7 @@ class IKConstraintSampler : public ConstraintSampler * * @return True if a sample was successfully produced, otherwise false */ - bool samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const robot_state::RobotState& ks, + bool samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const moveit::core::RobotState& ks, unsigned int max_attempts); /** @@ -508,10 +508,10 @@ class IKConstraintSampler : public ConstraintSampler */ bool callIK(const geometry_msgs::Pose& ik_query, const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, double timeout, - robot_state::RobotState& state, bool use_as_seed); - bool sampleHelper(robot_state::RobotState& state, const robot_state::RobotState& reference_state, + moveit::core::RobotState& state, bool use_as_seed); + bool sampleHelper(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts, bool project); - bool validate(robot_state::RobotState& state) const; + bool validate(moveit::core::RobotState& state) const; random_numbers::RandomNumberGenerator random_number_generator_; /**< \brief Random generator used by the sampler */ IKSamplingPose sampling_pose_; /**< \brief Holder for the pose used for sampling */ diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h index 7b00590491..52b696df30 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h @@ -148,10 +148,10 @@ class UnionConstraintSampler : public ConstraintSampler * * @return True if all invidual samplers return true */ - bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state, + bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts) override; - bool project(robot_state::RobotState& state, unsigned int max_attempts) override; + bool project(moveit::core::RobotState& state, unsigned int max_attempts) override; /** * \brief Get the name of the constraint sampler, for debugging purposes diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp index 274c6be1da..5b82d70ecf 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp @@ -57,7 +57,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni const std::string& group_name, const moveit_msgs::Constraints& constr) { - const robot_model::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group_name); if (!jmg) return constraint_samplers::ConstraintSamplerPtr(); std::stringstream ss; @@ -135,8 +135,8 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni samplers.push_back(joint_sampler); // read the ik allocators, if any - const robot_model::JointModelGroup::KinematicsSolver& ik_alloc = jmg->getGroupKinematics().first; - const robot_model::JointModelGroup::KinematicsSolverMap& ik_subgroup_alloc = jmg->getGroupKinematics().second; + const moveit::core::JointModelGroup::KinematicsSolver& ik_alloc = jmg->getGroupKinematics().first; + const moveit::core::JointModelGroup::KinematicsSolverMap& ik_subgroup_alloc = jmg->getGroupKinematics().second; // if we have a means of computing complete states for the group using IK, then we try to see if any IK constraints // should be used @@ -297,7 +297,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni bool some_sampler_valid = false; std::set used_p, used_o; - for (robot_model::JointModelGroup::KinematicsSolverMap::const_iterator it = ik_subgroup_alloc.begin(); + for (moveit::core::JointModelGroup::KinematicsSolverMap::const_iterator it = ik_subgroup_alloc.begin(); it != ik_subgroup_alloc.end(); ++it) { // construct a sub-set of constraints that operate on the sub-group for which we have an IK allocator diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index 70cd7af200..5a3e059bc4 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -55,14 +55,14 @@ double constraint_samplers::countSamplesPerSecond(const moveit_msgs::Constraints } double constraint_samplers::countSamplesPerSecond(const ConstraintSamplerPtr& sampler, - const robot_state::RobotState& reference_state) + const moveit::core::RobotState& reference_state) { if (!sampler) { ROS_ERROR_NAMED("constraint_samplers", "No sampler specified for counting samples per second"); return 0.0; } - robot_state::RobotState ks(reference_state); + moveit::core::RobotState ks(reference_state); unsigned long int valid = 0; unsigned long int total = 0; ros::WallTime end = ros::WallTime::now() + ros::WallDuration(1.0); @@ -80,7 +80,7 @@ double constraint_samplers::countSamplesPerSecond(const ConstraintSamplerPtr& sa } void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& sampler, - const robot_state::RobotState& reference_state, + const moveit::core::RobotState& reference_state, const std::string& link_name, unsigned int sample_count, visualization_msgs::MarkerArray& markers) { @@ -89,10 +89,10 @@ void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& samp ROS_ERROR_NAMED("constraint_samplers", "No sampler specified for visualizing distribution of samples"); return; } - const robot_state::LinkModel* lm = reference_state.getLinkModel(link_name); + const moveit::core::LinkModel* lm = reference_state.getLinkModel(link_name); if (!lm) return; - robot_state::RobotState ks(reference_state); + moveit::core::RobotState ks(reference_state); std_msgs::ColorRGBA color; color.r = 1.0f; color.g = 0.0f; diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 2de8cdbad7..c8dbb1f45e 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -74,13 +74,13 @@ bool JointConstraintSampler::configure(const std::vectorhasJointModel(jm->getName())) continue; some_valid_constraint = true; - const robot_model::VariableBounds& joint_bounds = jm->getVariableBounds(joint_constraint.getJointVariableName()); + const moveit::core::VariableBounds& joint_bounds = jm->getVariableBounds(joint_constraint.getJointVariableName()); JointInfo ji; std::map::iterator it = bound_data.find(joint_constraint.getJointVariableName()); if (it != bound_data.end()) @@ -120,8 +120,8 @@ bool JointConstraintSampler::configure(const std::vector& joints = jmg_->getJointModels(); - for (const robot_model::JointModel* joint : joints) + const std::vector& joints = jmg_->getJointModels(); + for (const moveit::core::JointModel* joint : joints) if (bound_data.find(joint->getName()) == bound_data.end() && joint->getVariableCount() > 0 && joint->getMimic() == nullptr) { @@ -148,8 +148,8 @@ bool JointConstraintSampler::configure(const std::vectorgetBaseFrame(); - transform_ik_ = !robot_state::Transforms::sameFrame(ik_frame_, jmg_->getParentModel().getModelFrame()); + transform_ik_ = !moveit::core::Transforms::sameFrame(ik_frame_, jmg_->getParentModel().getModelFrame()); if (!ik_frame_.empty() && ik_frame_[0] == '/') ik_frame_.erase(ik_frame_.begin()); if (transform_ik_) @@ -385,7 +385,7 @@ bool IKConstraintSampler::loadIKSolver() if (!wrong_link && sampling_pose_.orientation_constraint_) { const moveit::core::LinkModel* lm = sampling_pose_.orientation_constraint_->getLinkModel(); - if (!robot_state::Transforms::sameFrame(kb_->getTipFrame(), lm->getName())) + if (!moveit::core::Transforms::sameFrame(kb_->getTipFrame(), lm->getName())) { wrong_link = true; const moveit::core::LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms(); @@ -413,7 +413,7 @@ bool IKConstraintSampler::loadIKSolver() return true; } -bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const robot_state::RobotState& ks, +bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const moveit::core::RobotState& ks, unsigned int max_attempts) { if (ks.dirtyLinkTransforms()) @@ -459,7 +459,7 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q else { // do FK for rand state - robot_state::RobotState temp_state(ks); + moveit::core::RobotState temp_state(ks); temp_state.setToRandomPositions(jmg_); pos = temp_state.getGlobalLinkTransform(sampling_pose_.orientation_constraint_->getLinkModel()).translation(); } @@ -509,8 +509,8 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q namespace { -void samplingIkCallbackFnAdapter(robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, - const robot_state::GroupStateValidityCallbackFn& constraint, +void samplingIkCallbackFnAdapter(moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, + const moveit::core::GroupStateValidityCallbackFn& constraint, const geometry_msgs::Pose& /*unused*/, const std::vector& ik_sol, moveit_msgs::MoveItErrorCodes& error_code) { @@ -525,13 +525,13 @@ void samplingIkCallbackFnAdapter(robot_state::RobotState* state, const robot_mod } } // namespace -bool IKConstraintSampler::sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state, +bool IKConstraintSampler::sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts) { return sampleHelper(state, reference_state, max_attempts, false); } -bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const robot_state::RobotState& reference_state, +bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts, bool project) { if (!is_valid_) @@ -592,12 +592,12 @@ bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const rob return false; } -bool IKConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts) +bool IKConstraintSampler::project(moveit::core::RobotState& state, unsigned int max_attempts) { return sampleHelper(state, state, max_attempts, true); } -bool IKConstraintSampler::validate(robot_state::RobotState& state) const +bool IKConstraintSampler::validate(moveit::core::RobotState& state) const { state.update(); return (!sampling_pose_.orientation_constraint_ || @@ -608,7 +608,7 @@ bool IKConstraintSampler::validate(robot_state::RobotState& state) const bool IKConstraintSampler::callIK(const geometry_msgs::Pose& ik_query, const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, - double timeout, robot_state::RobotState& state, bool use_as_seed) + double timeout, moveit::core::RobotState& state, bool use_as_seed) { const std::vector& ik_joint_bijection = jmg_->getKinematicsSolverJointBijection(); std::vector seed(ik_joint_bijection.size(), 0.0); diff --git a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp index 3eec66d7c6..ec0e753408 100644 --- a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp @@ -123,7 +123,7 @@ UnionConstraintSampler::UnionConstraintSampler(const planning_scene::PlanningSce } } -bool UnionConstraintSampler::sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state, +bool UnionConstraintSampler::sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts) { state = reference_state; @@ -147,7 +147,7 @@ bool UnionConstraintSampler::sample(robot_state::RobotState& state, const robot_ return true; } -bool UnionConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts) +bool UnionConstraintSampler::project(moveit::core::RobotState& state, unsigned int max_attempts) { for (ConstraintSamplerPtr& sampler : samplers_) { diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp index bd0d7b956f..cc579534a7 100644 --- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp @@ -56,14 +56,14 @@ class LoadPlanningModelsPr2 : public testing::Test { protected: - kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const robot_model::JointModelGroup* jmg) + kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const moveit::core::JointModelGroup* jmg) { { return pr2_kinematics_plugin_right_arm_; } } - kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const robot_model::JointModelGroup* jmg) + kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const moveit::core::JointModelGroup* jmg) { { return pr2_kinematics_plugin_left_arm_; @@ -85,7 +85,7 @@ class LoadPlanningModelsPr2 : public testing::Test func_right_arm_ = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, _1); func_left_arm_ = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, _1); - std::map allocators; + std::map allocators; allocators["right_arm"] = func_right_arm_; allocators["left_arm"] = func_left_arm_; allocators["whole_body"] = func_right_arm_; @@ -101,20 +101,20 @@ class LoadPlanningModelsPr2 : public testing::Test } protected: - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; planning_scene::PlanningScenePtr ps_; pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_right_arm_; pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_left_arm_; - robot_model::SolverAllocatorFn func_right_arm_; - robot_model::SolverAllocatorFn func_left_arm_; + moveit::core::SolverAllocatorFn func_right_arm_; + moveit::core::SolverAllocatorFn func_left_arm_; }; TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); kinematic_constraints::JointConstraint jc1(robot_model_); @@ -253,7 +253,7 @@ TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple) TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerSimple) { - robot_state::Transforms& tf = ps_->getTransformsNonConst(); + moveit::core::Transforms& tf = ps_->getTransformsNonConst(); kinematic_constraints::PositionConstraint pc(robot_model_); moveit_msgs::PositionConstraint pcm; @@ -316,14 +316,14 @@ TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerSimple) TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSampler) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); - robot_state::Transforms& tf = ps_->getTransformsNonConst(); + moveit::core::Transforms& tf = ps_->getTransformsNonConst(); kinematic_constraints::OrientationConstraint oc(robot_model_); moveit_msgs::OrientationConstraint ocm; @@ -359,14 +359,14 @@ TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSampler) TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerValid) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); - robot_state::Transforms& tf = ps_->getTransformsNonConst(); + moveit::core::Transforms& tf = ps_->getTransformsNonConst(); kinematic_constraints::PositionConstraint pc(robot_model_); moveit_msgs::PositionConstraint pcm; @@ -438,15 +438,15 @@ TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerValid) TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); - robot_state::Transforms& tf = ps_->getTransformsNonConst(); + moveit::core::Transforms& tf = ps_->getTransformsNonConst(); kinematic_constraints::JointConstraint jc1(robot_model_); @@ -606,10 +606,10 @@ TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler) TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); @@ -692,7 +692,7 @@ TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager) TEST_F(LoadPlanningModelsPr2, JointVersusPoseConstraintSamplerManager) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); @@ -861,10 +861,10 @@ TEST_F(LoadPlanningModelsPr2, JointVersusPoseConstraintSamplerManager) TEST_F(LoadPlanningModelsPr2, MixedJointAndIkSamplerManager) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); @@ -937,10 +937,10 @@ TEST_F(LoadPlanningModelsPr2, MixedJointAndIkSamplerManager) TEST_F(LoadPlanningModelsPr2, SubgroupJointConstraintsSamplerManager) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); @@ -1078,7 +1078,7 @@ TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler) ocm.weight = 1.0; c.orientation_constraints.push_back(ocm); - robot_state::Transforms& tf = ps_->getTransformsNonConst(); + moveit::core::Transforms& tf = ps_->getTransformsNonConst(); constraint_samplers::ConstraintSamplerPtr s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "arms", c); EXPECT_TRUE(static_cast(s)); @@ -1089,10 +1089,10 @@ TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler) kinematic_constraints::KinematicConstraintSet kset(robot_model_); kset.add(c, tf); - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); diff --git a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h index 1f579d0c6f..c8402b4031 100644 --- a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h +++ b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h @@ -66,7 +66,7 @@ class DynamicsSolver * @param group_name The name of the group to compute stuff for * @return False if initialization failed */ - DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, + DynamicsSolver(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, const geometry_msgs::Vector3& gravity_vector); /** @@ -124,12 +124,12 @@ class DynamicsSolver * @brief Get the kinematic model * @return kinematic model */ - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } - const robot_model::JointModelGroup* getGroup() const + const moveit::core::JointModelGroup* getGroup() const { return joint_model_group_; } @@ -138,10 +138,10 @@ class DynamicsSolver std::shared_ptr chain_id_solver_; // KDL chain inverse dynamics KDL::Chain kdl_chain_; // KDL chain - robot_model::RobotModelConstPtr robot_model_; - const robot_model::JointModelGroup* joint_model_group_; + moveit::core::RobotModelConstPtr robot_model_; + const moveit::core::JointModelGroup* joint_model_group_; - robot_state::RobotStatePtr state_; // robot state + moveit::core::RobotStatePtr state_; // robot state std::string base_name_, tip_name_; // base name, tip name unsigned int num_joints_, num_segments_; // number of joints in group, number of segments in group diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index 9a48b0ed2a..20cf844b39 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -64,7 +64,7 @@ inline geometry_msgs::Vector3 transformVector(const Eigen::Isometry3d& transform } } // namespace -DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, +DynamicsSolver::DynamicsSolver(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, const geometry_msgs::Vector3& gravity_vector) { robot_model_ = robot_model; @@ -88,7 +88,7 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode return; } - const robot_model::JointModel* joint = joint_model_group_->getJointRoots()[0]; + const moveit::core::JointModel* joint = joint_model_group_->getJointRoots()[0]; if (!joint->getParentLinkModel()) { ROS_ERROR_NAMED("dynamics_solver", "Group '%s' does not have a parent link", group_name.c_str()); @@ -120,7 +120,7 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode num_joints_ = kdl_chain_.getNrOfJoints(); num_segments_ = kdl_chain_.getNrOfSegments(); - state_.reset(new robot_state::RobotState(robot_model_)); + state_.reset(new moveit::core::RobotState(robot_model_)); state_->setToDefaultValues(); const std::vector& joint_model_names = joint_model_group_->getJointModelNames(); diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index 12804a01b1..cf28da8858 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -92,7 +92,7 @@ class KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - KinematicConstraint(const robot_model::RobotModelConstPtr& model); + KinematicConstraint(const moveit::core::RobotModelConstPtr& model); virtual ~KinematicConstraint(); /** \brief Clear the stored constraint */ @@ -106,7 +106,7 @@ class KinematicConstraint * * @return */ - virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const = 0; + virtual ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const = 0; /** \brief This function returns true if this constraint is configured and able to decide whether states do meet the @@ -166,14 +166,14 @@ class KinematicConstraint * * @return The kinematic model associated with this constraint */ - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } protected: - ConstraintType type_; /**< \brief The type of the constraint */ - robot_model::RobotModelConstPtr robot_model_; /**< \brief The kinematic model associated with this constraint */ + ConstraintType type_; /**< \brief The type of the constraint */ + moveit::core::RobotModelConstPtr robot_model_; /**< \brief The kinematic model associated with this constraint */ double constraint_weight_; /**< \brief The weight of a constraint is a multiplicative factor associated to the distance computed by the decide() function */ }; @@ -207,7 +207,7 @@ class JointConstraint : public KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - JointConstraint(const robot_model::RobotModelConstPtr& model) + JointConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), joint_model_(NULL), joint_variable_index_(-1) { type_ = JOINT_CONSTRAINT; @@ -245,7 +245,7 @@ class JointConstraint : public KinematicConstraint */ bool equal(const KinematicConstraint& other, double margin) const override; - ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; bool enabled() const override; void clear() override; void print(std::ostream& out = std::cout) const override; @@ -255,7 +255,7 @@ class JointConstraint : public KinematicConstraint * * @return The relevant joint model if enabled, and otherwise NULL */ - const robot_model::JointModel* getJointModel() const + const moveit::core::JointModel* getJointModel() const { return joint_model_; } @@ -273,7 +273,7 @@ class JointConstraint : public KinematicConstraint } /** - * \brief Gets the joint variable name, as known to the robot_model::RobotModel + * \brief Gets the joint variable name, as known to the moveit::core::RobotModel * * This will include the local variable name if a variable of a multi-DOF joint is constrained. * @@ -323,10 +323,10 @@ class JointConstraint : public KinematicConstraint } protected: - const robot_model::JointModel* joint_model_; /**< \brief The joint from the kinematic model for this constraint */ - bool joint_is_continuous_; /**< \brief Whether or not the joint is continuous */ - std::string local_variable_name_; /**< \brief The local variable name for a multi DOF joint, if any */ - std::string joint_variable_name_; /**< \brief The joint variable name */ + const moveit::core::JointModel* joint_model_; /**< \brief The joint from the kinematic model for this constraint */ + bool joint_is_continuous_; /**< \brief Whether or not the joint is continuous */ + std::string local_variable_name_; /**< \brief The local variable name for a multi DOF joint, if any */ + std::string joint_variable_name_; /**< \brief The joint variable name */ int joint_variable_index_; /**< \brief The index of the joint variable name in the full robot state */ double joint_position_, joint_tolerance_above_, joint_tolerance_below_; /**< \brief Position and tolerance values*/ }; @@ -355,7 +355,7 @@ class OrientationConstraint : public KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - OrientationConstraint(const robot_model::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL) + OrientationConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL) { type_ = ORIENTATION_CONSTRAINT; } @@ -373,7 +373,7 @@ class OrientationConstraint : public KinematicConstraint * * @return True if constraint can be configured from oc */ - bool configure(const moveit_msgs::OrientationConstraint& oc, const robot_state::Transforms& tf); + bool configure(const moveit_msgs::OrientationConstraint& oc, const moveit::core::Transforms& tf); /** * \brief Check if two orientation constraints are the same. @@ -394,7 +394,7 @@ class OrientationConstraint : public KinematicConstraint bool equal(const KinematicConstraint& other, double margin) const override; void clear() override; - ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; bool enabled() const override; void print(std::ostream& out = std::cout) const override; @@ -404,7 +404,7 @@ class OrientationConstraint : public KinematicConstraint * * @return Returns the current link model */ - const robot_model::LinkModel* getLinkModel() const + const moveit::core::LinkModel* getLinkModel() const { return link_model_; } @@ -475,7 +475,7 @@ class OrientationConstraint : public KinematicConstraint } protected: - const robot_model::LinkModel* link_model_; /**< \brief The target link model */ + const moveit::core::LinkModel* link_model_; /**< \brief The target link model */ Eigen::Matrix3d desired_rotation_matrix_; /**< \brief The desired rotation matrix in the tf frame */ Eigen::Matrix3d desired_rotation_matrix_inv_; /**< \brief The inverse of the desired rotation matrix, precomputed for * efficiency @@ -512,7 +512,7 @@ class PositionConstraint : public KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - PositionConstraint(const robot_model::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL) + PositionConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL) { type_ = POSITION_CONSTRAINT; } @@ -533,7 +533,7 @@ class PositionConstraint : public KinematicConstraint * * @return True if constraint can be configured from pc */ - bool configure(const moveit_msgs::PositionConstraint& pc, const robot_state::Transforms& tf); + bool configure(const moveit_msgs::PositionConstraint& pc, const moveit::core::Transforms& tf); /** * \brief Check if two constraints are the same. For position @@ -561,7 +561,7 @@ class PositionConstraint : public KinematicConstraint bool equal(const KinematicConstraint& other, double margin) const override; void clear() override; - ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; bool enabled() const override; void print(std::ostream& out = std::cout) const override; @@ -571,7 +571,7 @@ class PositionConstraint : public KinematicConstraint * * @return The link model */ - const robot_model::LinkModel* getLinkModel() const + const moveit::core::LinkModel* getLinkModel() const { return link_model_; } @@ -644,7 +644,7 @@ class PositionConstraint : public KinematicConstraint EigenSTL::vector_Isometry3d constraint_region_pose_; /**< \brief The constraint region pose vector */ bool mobile_frame_; /**< \brief Whether or not a mobile frame is employed*/ std::string constraint_frame_id_; /**< \brief The constraint frame id */ - const robot_model::LinkModel* link_model_; /**< \brief The link model constraint subject */ + const moveit::core::LinkModel* link_model_; /**< \brief The link model constraint subject */ }; MOVEIT_CLASS_FORWARD(VisibilityConstraint); @@ -757,7 +757,7 @@ class VisibilityConstraint : public KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - VisibilityConstraint(const robot_model::RobotModelConstPtr& model); + VisibilityConstraint(const moveit::core::RobotModelConstPtr& model); /** * \brief Configure the constraint based on a @@ -771,7 +771,7 @@ class VisibilityConstraint : public KinematicConstraint * * @return True if constraint can be configured from vc */ - bool configure(const moveit_msgs::VisibilityConstraint& vc, const robot_state::Transforms& tf); + bool configure(const moveit_msgs::VisibilityConstraint& vc, const moveit::core::Transforms& tf); /** * \brief Check if two constraints are the same. @@ -800,7 +800,7 @@ class VisibilityConstraint : public KinematicConstraint * * @return The shape associated with the cone */ - shapes::Mesh* getVisibilityCone(const robot_state::RobotState& state) const; + shapes::Mesh* getVisibilityCone(const moveit::core::RobotState& state) const; /** * \brief Adds markers associated with the visibility cone, sensor @@ -813,10 +813,10 @@ class VisibilityConstraint : public KinematicConstraint * @param [in] state The state from which to produce the markers * @param [out] markers The marker array to which the markers will be added */ - void getMarkers(const robot_state::RobotState& state, visualization_msgs::MarkerArray& markers) const; + void getMarkers(const moveit::core::RobotState& state, visualization_msgs::MarkerArray& markers) const; bool enabled() const override; - ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; void print(std::ostream& out = std::cout) const override; protected: @@ -868,7 +868,7 @@ class KinematicConstraintSet * * @param [in] model The kinematic model used for constraint evaluation */ - KinematicConstraintSet(const robot_model::RobotModelConstPtr& model) : robot_model_(model) + KinematicConstraintSet(const moveit::core::RobotModelConstPtr& model) : robot_model_(model) { } @@ -890,7 +890,7 @@ class KinematicConstraintSet * KinematicConstraintSet can still be used even if the addition * returns false. */ - bool add(const moveit_msgs::Constraints& c, const robot_state::Transforms& tf); + bool add(const moveit_msgs::Constraints& c, const moveit::core::Transforms& tf); /** * \brief Add a vector of joint constraints @@ -908,7 +908,7 @@ class KinematicConstraintSet * * @return Will return true only if all constraints are valid, and false otherwise */ - bool add(const std::vector& pc, const robot_state::Transforms& tf); + bool add(const std::vector& pc, const moveit::core::Transforms& tf); /** * \brief Add a vector of orientation constraints @@ -917,7 +917,7 @@ class KinematicConstraintSet * * @return Will return true only if all constraints are valid, and false otherwise */ - bool add(const std::vector& oc, const robot_state::Transforms& tf); + bool add(const std::vector& oc, const moveit::core::Transforms& tf); /** * \brief Add a vector of visibility constraints @@ -926,7 +926,7 @@ class KinematicConstraintSet * * @return Will return true only if all constraints are valid, and false otherwise */ - bool add(const std::vector& vc, const robot_state::Transforms& tf); + bool add(const std::vector& vc, const moveit::core::Transforms& tf); /** * \brief Determines whether all constraints are satisfied by state, @@ -939,7 +939,7 @@ class KinematicConstraintSet * report satisfied only if all constraints are satisfied, and with * a distance that is the sum of all individual distances. */ - ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const; /** * @@ -959,7 +959,7 @@ class KinematicConstraintSet * report satisfied only if all constraints are satisfied, and with * a distance that is the sum of all individual distances. */ - ConstraintEvaluationResult decide(const robot_state::RobotState& state, + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, std::vector& results, bool verbose = false) const; /** @@ -1053,7 +1053,7 @@ class KinematicConstraintSet } protected: - robot_model::RobotModelConstPtr robot_model_; /**< \brief The kinematic model used for by the Set */ + moveit::core::RobotModelConstPtr robot_model_; /**< \brief The kinematic model used for by the Set */ std::vector kinematic_constraints_; /**< \brief Shared pointers to all the member constraints */ diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h index 54080fbacb..3396520631 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h @@ -83,8 +83,8 @@ std::size_t countIndividualConstraints(const moveit_msgs::Constraints& constr); * * @return A full constraint message containing all the joint constraints */ -moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state, - const robot_model::JointModelGroup* jmg, double tolerance_below, +moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg, double tolerance_below, double tolerance_above); /** @@ -98,8 +98,8 @@ moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& * * @return A full constraint message containing all the joint constraints */ -moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state, - const robot_model::JointModelGroup* jmg, +moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg, double tolerance = std::numeric_limits::epsilon()); /** @@ -213,5 +213,5 @@ bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::Constraints& * @param [in] state The RobotState used to resolve frames. * @param [in] constraints The constraint to resolve. */ -bool resolveConstraintFrames(const robot_state::RobotState& state, moveit_msgs::Constraints& constraints); +bool resolveConstraintFrames(const moveit::core::RobotState& state, moveit_msgs::Constraints& constraints); } diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 884a1dca51..9ed1c08125 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -57,7 +57,7 @@ static double normalizeAngle(double angle) return v; } -KinematicConstraint::KinematicConstraint(const robot_model::RobotModelConstPtr& model) +KinematicConstraint::KinematicConstraint(const moveit::core::RobotModelConstPtr& model) : type_(UNKNOWN_CONSTRAINT), robot_model_(model), constraint_weight_(std::numeric_limits::epsilon()) { } @@ -140,13 +140,14 @@ bool JointConstraint::configure(const moveit_msgs::JointConstraint& jc) // check if we have to wrap angles when computing distances joint_is_continuous_ = false; - if (joint_model_->getType() == robot_model::JointModel::REVOLUTE) + if (joint_model_->getType() == moveit::core::JointModel::REVOLUTE) { - const robot_model::RevoluteJointModel* rjoint = static_cast(joint_model_); + const moveit::core::RevoluteJointModel* rjoint = + static_cast(joint_model_); if (rjoint->isContinuous()) joint_is_continuous_ = true; } - else if (joint_model_->getType() == robot_model::JointModel::PLANAR) + else if (joint_model_->getType() == moveit::core::JointModel::PLANAR) { if (local_variable_name_ == "theta") joint_is_continuous_ = true; @@ -159,7 +160,7 @@ bool JointConstraint::configure(const moveit_msgs::JointConstraint& jc) else { joint_position_ = jc.position; - const robot_model::VariableBounds& bounds = joint_model_->getVariableBounds(joint_variable_name_); + const moveit::core::VariableBounds& bounds = joint_model_->getVariableBounds(joint_variable_name_); if (bounds.min_position_ > joint_position_ + joint_tolerance_above_) { @@ -204,7 +205,7 @@ bool JointConstraint::equal(const KinematicConstraint& other, double margin) con return false; } -ConstraintEvaluationResult JointConstraint::decide(const robot_state::RobotState& state, bool verbose) const +ConstraintEvaluationResult JointConstraint::decide(const moveit::core::RobotState& state, bool verbose) const { if (!joint_model_) return ConstraintEvaluationResult(true, 0.0); @@ -268,7 +269,7 @@ void JointConstraint::print(std::ostream& out) const out << "No constraint" << std::endl; } -bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, const robot_state::Transforms& tf) +bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, const moveit::core::Transforms& tf) { // clearing before we configure to get rid of any old data clear(); @@ -378,7 +379,7 @@ bool PositionConstraint::equal(const KinematicConstraint& other, double margin) return false; const PositionConstraint& o = static_cast(other); - if (link_model_ == o.link_model_ && robot_state::Transforms::sameFrame(constraint_frame_id_, o.constraint_frame_id_)) + if (link_model_ == o.link_model_ && moveit::core::Transforms::sameFrame(constraint_frame_id_, o.constraint_frame_id_)) { if ((offset_ - o.offset_).norm() > margin) return false; @@ -429,7 +430,7 @@ static inline ConstraintEvaluationResult finishPositionConstraintDecision(const return ConstraintEvaluationResult(result, weight * sqrt(dx * dx + dy * dy + dz * dz)); } -ConstraintEvaluationResult PositionConstraint::decide(const robot_state::RobotState& state, bool verbose) const +ConstraintEvaluationResult PositionConstraint::decide(const moveit::core::RobotState& state, bool verbose) const { if (!link_model_ || constraint_region_.empty()) return ConstraintEvaluationResult(true, 0.0); @@ -489,7 +490,7 @@ bool PositionConstraint::enabled() const return link_model_ && !constraint_region_.empty(); } -bool OrientationConstraint::configure(const moveit_msgs::OrientationConstraint& oc, const robot_state::Transforms& tf) +bool OrientationConstraint::configure(const moveit_msgs::OrientationConstraint& oc, const moveit::core::Transforms& tf) { // clearing out any old data clear(); @@ -562,7 +563,7 @@ bool OrientationConstraint::equal(const KinematicConstraint& other, double margi const OrientationConstraint& o = static_cast(other); if (o.link_model_ == link_model_ && - robot_state::Transforms::sameFrame(desired_rotation_frame_id_, o.desired_rotation_frame_id_)) + moveit::core::Transforms::sameFrame(desired_rotation_frame_id_, o.desired_rotation_frame_id_)) { if (!desired_rotation_matrix_.isApprox(o.desired_rotation_matrix_)) return false; @@ -588,7 +589,7 @@ bool OrientationConstraint::enabled() const return link_model_; } -ConstraintEvaluationResult OrientationConstraint::decide(const robot_state::RobotState& state, bool verbose) const +ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::RobotState& state, bool verbose) const { if (!link_model_) return ConstraintEvaluationResult(true, 0.0); @@ -642,7 +643,7 @@ void OrientationConstraint::print(std::ostream& out) const out << "No constraint" << std::endl; } -VisibilityConstraint::VisibilityConstraint(const robot_model::RobotModelConstPtr& model) +VisibilityConstraint::VisibilityConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), collision_env_(new collision_detection::CollisionEnvFCL(model)) { type_ = VISIBILITY_CONSTRAINT; @@ -664,7 +665,7 @@ void VisibilityConstraint::clear() max_range_angle_ = 0.0; } -bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc, const robot_state::Transforms& tf) +bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc, const moveit::core::Transforms& tf) { clear(); target_radius_ = fabs(vc.target_radius); @@ -746,8 +747,8 @@ bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin return false; const VisibilityConstraint& o = static_cast(other); - if (robot_state::Transforms::sameFrame(target_frame_id_, o.target_frame_id_) && - robot_state::Transforms::sameFrame(sensor_frame_id_, o.sensor_frame_id_) && cone_sides_ == o.cone_sides_ && + if (moveit::core::Transforms::sameFrame(target_frame_id_, o.target_frame_id_) && + moveit::core::Transforms::sameFrame(sensor_frame_id_, o.sensor_frame_id_) && cone_sides_ == o.cone_sides_ && sensor_view_direction_ == o.sensor_view_direction_) { if (fabs(max_view_angle_ - o.max_view_angle_) > margin || fabs(target_radius_ - o.target_radius_) > margin) @@ -772,7 +773,7 @@ bool VisibilityConstraint::enabled() const return target_radius_ > std::numeric_limits::epsilon(); } -shapes::Mesh* VisibilityConstraint::getVisibilityCone(const robot_state::RobotState& state) const +shapes::Mesh* VisibilityConstraint::getVisibilityCone(const moveit::core::RobotState& state) const { // the current pose of the sensor @@ -846,7 +847,7 @@ shapes::Mesh* VisibilityConstraint::getVisibilityCone(const robot_state::RobotSt return m; } -void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, +void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state, visualization_msgs::MarkerArray& markers) const { shapes::Mesh* m = getVisibilityCone(state); @@ -918,7 +919,7 @@ void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, markers.markers.push_back(mka); } -ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::RobotState& state, bool verbose) const +ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::RobotState& state, bool verbose) const { if (target_radius_ <= std::numeric_limits::epsilon()) return ConstraintEvaluationResult(true, 0.0); @@ -1015,16 +1016,16 @@ bool VisibilityConstraint::decideContact(const collision_detection::Contact& con return true; if (contact.body_type_1 == collision_detection::BodyTypes::ROBOT_LINK && contact.body_type_2 == collision_detection::BodyTypes::WORLD_OBJECT && - (robot_state::Transforms::sameFrame(contact.body_name_1, sensor_frame_id_) || - robot_state::Transforms::sameFrame(contact.body_name_1, target_frame_id_))) + (moveit::core::Transforms::sameFrame(contact.body_name_1, sensor_frame_id_) || + moveit::core::Transforms::sameFrame(contact.body_name_1, target_frame_id_))) { ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target"); return true; } if (contact.body_type_2 == collision_detection::BodyTypes::ROBOT_LINK && contact.body_type_1 == collision_detection::BodyTypes::WORLD_OBJECT && - (robot_state::Transforms::sameFrame(contact.body_name_2, sensor_frame_id_) || - robot_state::Transforms::sameFrame(contact.body_name_2, target_frame_id_))) + (moveit::core::Transforms::sameFrame(contact.body_name_2, sensor_frame_id_) || + moveit::core::Transforms::sameFrame(contact.body_name_2, target_frame_id_))) { ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target"); return true; @@ -1070,7 +1071,7 @@ bool KinematicConstraintSet::add(const std::vector } bool KinematicConstraintSet::add(const std::vector& pc, - const robot_state::Transforms& tf) + const moveit::core::Transforms& tf) { bool result = true; for (const moveit_msgs::PositionConstraint& position_constraint : pc) @@ -1086,7 +1087,7 @@ bool KinematicConstraintSet::add(const std::vector& oc, - const robot_state::Transforms& tf) + const moveit::core::Transforms& tf) { bool result = true; for (const moveit_msgs::OrientationConstraint& orientation_constraint : oc) @@ -1102,7 +1103,7 @@ bool KinematicConstraintSet::add(const std::vector& vc, - const robot_state::Transforms& tf) + const moveit::core::Transforms& tf) { bool result = true; for (const moveit_msgs::VisibilityConstraint& visibility_constraint : vc) @@ -1117,7 +1118,7 @@ bool KinematicConstraintSet::add(const std::vector& results, bool verbose) const { diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 59beecec6c..6125ae954c 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -126,14 +126,14 @@ std::size_t countIndividualConstraints(const moveit_msgs::Constraints& constr) constr.visibility_constraints.size() + constr.joint_constraints.size(); } -moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state, - const robot_model::JointModelGroup* jmg, double tolerance) +moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg, double tolerance) { return constructGoalConstraints(state, jmg, tolerance, tolerance); } -moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state, - const robot_model::JointModelGroup* jmg, double tolerance_below, +moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg, double tolerance_below, double tolerance_above) { moveit_msgs::Constraints goal; @@ -529,7 +529,7 @@ bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::Constraints& } } // namespace kinematic_constraints -bool kinematic_constraints::resolveConstraintFrames(const robot_state::RobotState& state, +bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotState& state, moveit_msgs::Constraints& constraints) { for (auto& c : constraints.position_constraints) diff --git a/moveit_core/kinematic_constraints/test/test_constraints.cpp b/moveit_core/kinematic_constraints/test/test_constraints.cpp index 4c6cd14eba..b16e9505ae 100644 --- a/moveit_core/kinematic_constraints/test/test_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_constraints.cpp @@ -54,14 +54,14 @@ class LoadPlanningModelsPr2 : public testing::Test } protected: - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; }; TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::JointConstraint jc(robot_model_); moveit_msgs::JointConstraint jcm; @@ -174,10 +174,10 @@ TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple) TEST_F(LoadPlanningModelsPr2, JointConstraintsCont) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::JointConstraint jc(robot_model_); moveit_msgs::JointConstraint jcm; @@ -305,7 +305,7 @@ TEST_F(LoadPlanningModelsPr2, JointConstraintsCont) TEST_F(LoadPlanningModelsPr2, JointConstraintsMultiDOF) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); kinematic_constraints::JointConstraint jc(robot_model_); @@ -356,10 +356,10 @@ TEST_F(LoadPlanningModelsPr2, JointConstraintsMultiDOF) TEST_F(LoadPlanningModelsPr2, PositionConstraintsFixed) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(true); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::PositionConstraint pc(robot_model_); moveit_msgs::PositionConstraint pcm; @@ -432,9 +432,9 @@ TEST_F(LoadPlanningModelsPr2, PositionConstraintsFixed) TEST_F(LoadPlanningModelsPr2, PositionConstraintsMobile) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); robot_state.update(); kinematic_constraints::PositionConstraint pc(robot_model_); @@ -509,9 +509,9 @@ TEST_F(LoadPlanningModelsPr2, PositionConstraintsMobile) TEST_F(LoadPlanningModelsPr2, PositionConstraintsEquality) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::PositionConstraint pc(robot_model_); kinematic_constraints::PositionConstraint pc2(robot_model_); @@ -595,10 +595,10 @@ TEST_F(LoadPlanningModelsPr2, PositionConstraintsEquality) TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::OrientationConstraint oc(robot_model_); @@ -656,10 +656,10 @@ TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple) TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::VisibilityConstraint vc(robot_model_); moveit_msgs::VisibilityConstraint vcm; @@ -707,10 +707,10 @@ TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple) TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsPR2) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::VisibilityConstraint vc(robot_model_); moveit_msgs::VisibilityConstraint vcm; @@ -781,9 +781,9 @@ TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsPR2) TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSet) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::KinematicConstraintSet kcs(robot_model_); EXPECT_TRUE(kcs.empty()); @@ -847,9 +847,9 @@ TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSet) TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSetEquality) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::KinematicConstraintSet kcs(robot_model_); kinematic_constraints::KinematicConstraintSet kcs2(robot_model_); diff --git a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h index da5c213e49..27e111fdf1 100644 --- a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h +++ b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h @@ -52,7 +52,7 @@ class KinematicsMetrics { public: /** \brief Construct a KinematicsMetricss from a RobotModel */ - KinematicsMetrics(const robot_model::RobotModelConstPtr& robot_model) + KinematicsMetrics(const moveit::core::RobotModelConstPtr& robot_model) : robot_model_(robot_model), penalty_multiplier_(0.0) { } @@ -64,7 +64,7 @@ class KinematicsMetrics * @param manipulability_index The computed manipulability = sqrt(det(JJ^T)) * @return False if the group was not found */ - bool getManipulabilityIndex(const robot_state::RobotState& state, const std::string& group_name, + bool getManipulabilityIndex(const moveit::core::RobotState& state, const std::string& group_name, double& manipulability_index, bool translation = false) const; /** @@ -74,8 +74,8 @@ class KinematicsMetrics * @param manipulability_index The computed manipulability = sqrt(det(JJ^T)) * @return False if the group was not found */ - bool getManipulabilityIndex(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group, double& manipulability_index, + bool getManipulabilityIndex(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group, double& manipulability_index, bool translation = false) const; /** @@ -86,7 +86,7 @@ class KinematicsMetrics * @param eigen_vectors The eigen vectors for the translation part of JJ^T * @return False if the group was not found */ - bool getManipulabilityEllipsoid(const robot_state::RobotState& state, const std::string& group_name, + bool getManipulabilityEllipsoid(const moveit::core::RobotState& state, const std::string& group_name, Eigen::MatrixXcd& eigen_values, Eigen::MatrixXcd& eigen_vectors) const; /** @@ -97,9 +97,9 @@ class KinematicsMetrics * @param eigen_vectors The eigen vectors for the translation part of JJ^T * @return False if the group was not found */ - bool getManipulabilityEllipsoid(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group, Eigen::MatrixXcd& eigen_values, - Eigen::MatrixXcd& eigen_vectors) const; + bool getManipulabilityEllipsoid(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group, + Eigen::MatrixXcd& eigen_values, Eigen::MatrixXcd& eigen_vectors) const; /** * @brief Get the manipulability = sigma_min/sigma_max @@ -110,7 +110,7 @@ class KinematicsMetrics * @param condition_number Condition number for JJ^T * @return False if the group was not found */ - bool getManipulability(const robot_state::RobotState& state, const std::string& group_name, double& condition_number, + bool getManipulability(const moveit::core::RobotState& state, const std::string& group_name, double& condition_number, bool translation = false) const; /** @@ -122,7 +122,7 @@ class KinematicsMetrics * @param condition_number Condition number for JJ^T * @return False if the group was not found */ - bool getManipulability(const robot_state::RobotState& state, const robot_model::JointModelGroup* joint_model_group, + bool getManipulability(const moveit::core::RobotState& state, const moveit::core::JointModelGroup* joint_model_group, double& condition_number, bool translation = false) const; void setPenaltyMultiplier(double multiplier) @@ -136,7 +136,7 @@ class KinematicsMetrics } protected: - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; private: /** @@ -151,8 +151,8 @@ class KinematicsMetrics * Ohio State University, 1986, for more details. * @return multiplier that is multiplied with every manipulability measure computed here */ - double getJointLimitsPenalty(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group) const; + double getJointLimitsPenalty(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group) const; double penalty_multiplier_; }; diff --git a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp index 6776a1e475..1ab07c8a2c 100644 --- a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp +++ b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp @@ -41,25 +41,25 @@ namespace kinematics_metrics { -double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group) const +double KinematicsMetrics::getJointLimitsPenalty(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group) const { if (fabs(penalty_multiplier_) <= boost::math::tools::epsilon()) return 1.0; double joint_limits_multiplier(1.0); - const std::vector& joint_model_vector = joint_model_group->getJointModels(); - for (const robot_model::JointModel* joint_model : joint_model_vector) + const std::vector& joint_model_vector = joint_model_group->getJointModels(); + for (const moveit::core::JointModel* joint_model : joint_model_vector) { - if (joint_model->getType() == robot_model::JointModel::REVOLUTE) + if (joint_model->getType() == moveit::core::JointModel::REVOLUTE) { - const robot_model::RevoluteJointModel* revolute_model = - static_cast(joint_model); + const moveit::core::RevoluteJointModel* revolute_model = + static_cast(joint_model); if (revolute_model->isContinuous()) continue; } - if (joint_model->getType() == robot_model::JointModel::PLANAR) + if (joint_model->getType() == moveit::core::JointModel::PLANAR) { - const robot_model::JointModel::Bounds& bounds = joint_model->getVariableBounds(); + const moveit::core::JointModel::Bounds& bounds = joint_model->getVariableBounds(); if (bounds[0].min_position_ == -std::numeric_limits::max() || bounds[0].max_position_ == std::numeric_limits::max() || bounds[1].min_position_ == -std::numeric_limits::max() || @@ -68,13 +68,13 @@ double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState& s bounds[2].max_position_ == boost::math::constants::pi()) continue; } - if (joint_model->getType() == robot_model::JointModel::FLOATING) + if (joint_model->getType() == moveit::core::JointModel::FLOATING) { // Joint limits are not well-defined for floating joints continue; } const double* joint_values = state.getJointPositions(joint_model); - const robot_model::JointModel::Bounds& bounds = joint_model->getVariableBounds(); + const moveit::core::JointModel::Bounds& bounds = joint_model->getVariableBounds(); std::vector lower_bounds, upper_bounds; for (const moveit::core::VariableBounds& bound : bounds) { @@ -91,18 +91,18 @@ double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState& s return (1.0 - exp(-penalty_multiplier_ * joint_limits_multiplier)); } -bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& state, const std::string& group_name, +bool KinematicsMetrics::getManipulabilityIndex(const moveit::core::RobotState& state, const std::string& group_name, double& manipulability_index, bool translation) const { - const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); if (joint_model_group) return getManipulabilityIndex(state, joint_model_group, manipulability_index, translation); else return false; } -bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group, +bool KinematicsMetrics::getManipulabilityIndex(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group, double& manipulability_index, bool translation) const { // state.getJacobian() only works for chain groups. @@ -162,19 +162,19 @@ bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& st return true; } -bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState& state, const std::string& group_name, +bool KinematicsMetrics::getManipulabilityEllipsoid(const moveit::core::RobotState& state, const std::string& group_name, Eigen::MatrixXcd& eigen_values, Eigen::MatrixXcd& eigen_vectors) const { - const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); if (joint_model_group) return getManipulabilityEllipsoid(state, joint_model_group, eigen_values, eigen_vectors); else return false; } -bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group, +bool KinematicsMetrics::getManipulabilityEllipsoid(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group, Eigen::MatrixXcd& eigen_values, Eigen::MatrixXcd& eigen_vectors) const { @@ -192,19 +192,19 @@ bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState return true; } -bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state, const std::string& group_name, +bool KinematicsMetrics::getManipulability(const moveit::core::RobotState& state, const std::string& group_name, double& manipulability, bool translation) const { - const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); if (joint_model_group) return getManipulability(state, joint_model_group, manipulability, translation); else return false; } -bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group, double& manipulability, - bool translation) const +bool KinematicsMetrics::getManipulability(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group, + double& manipulability, bool translation) const { // state.getJacobian() only works for chain groups. if (!joint_model_group->isChain()) diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index 9cf2f206f4..4c583a922c 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -163,7 +163,7 @@ class PlannerManager /// It is assumed that motion plans will be computed for the robot described by \e model and that any exposed ROS /// functionality /// or required ROS parameters are namespaced by \e ns - virtual bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns); + virtual bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns); /// Get \brief a short string that identifies the planning interface virtual std::string getDescription() const; diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp index 6fedbb22a2..0f3b2ac243 100644 --- a/moveit_core/planning_interface/src/planning_interface.cpp +++ b/moveit_core/planning_interface/src/planning_interface.cpp @@ -91,7 +91,7 @@ void PlanningContext::setMotionPlanRequest(const MotionPlanRequest& request) request_.num_planning_attempts = std::max(1, request_.num_planning_attempts); } -bool PlannerManager::initialize(const robot_model::RobotModelConstPtr& /*unused*/, const std::string& /*unused*/) +bool PlannerManager::initialize(const moveit::core::RobotModelConstPtr& /*unused*/, const std::string& /*unused*/) { return true; } diff --git a/moveit_core/planning_interface/src/planning_response.cpp b/moveit_core/planning_interface/src/planning_response.cpp index fad5558909..7824cd75a3 100644 --- a/moveit_core/planning_interface/src/planning_response.cpp +++ b/moveit_core/planning_interface/src/planning_response.cpp @@ -43,7 +43,7 @@ void planning_interface::MotionPlanResponse::getMessage(moveit_msgs::MotionPlanR msg.planning_time = planning_time_; if (trajectory_ && !trajectory_->empty()) { - robot_state::robotStateToRobotStateMsg(trajectory_->getFirstWayPoint(), msg.trajectory_start); + moveit::core::robotStateToRobotStateMsg(trajectory_->getFirstWayPoint(), msg.trajectory_start); trajectory_->getRobotTrajectoryMsg(msg.trajectory); msg.group_name = trajectory_->getGroupName(); } @@ -66,7 +66,7 @@ void planning_interface::MotionPlanDetailedResponse::getMessage(moveit_msgs::Mot if (first) { first = false; - robot_state::robotStateToRobotStateMsg(trajectory_[i]->getFirstWayPoint(), msg.trajectory_start); + moveit::core::robotStateToRobotStateMsg(trajectory_[i]->getFirstWayPoint(), msg.trajectory_start); msg.group_name = trajectory_[i]->getGroupName(); } msg.trajectory.resize(msg.trajectory.size() + 1); diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index acc43ab92c..0c48ababfb 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -65,14 +65,15 @@ MOVEIT_CLASS_FORWARD(PlanningScene); respecting constraints and collision avoidance). The first argument is the state to check the feasibility for, the second one is whether the check should be verbose or not. */ -typedef boost::function StateFeasibilityFn; +typedef boost::function StateFeasibilityFn; /** \brief This is the function signature for additional feasibility checks to be imposed on motions segments between states (in addition to respecting constraints and collision avoidance). The order of the arguments matters: the notion of feasibility is to be checked for motion segments that start at the first state and end at the second state. The third argument indicates whether the check should be verbose or not. */ -typedef boost::function MotionFeasibilityFn; +typedef boost::function + MotionFeasibilityFn; /** \brief A map from object names (e.g., attached bodies, collision objects) to their colors */ typedef std::map ObjectColorMap; @@ -88,7 +89,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from public: /** \brief construct using an existing RobotModel */ PlanningScene( - const robot_model::RobotModelConstPtr& robot_model, + const moveit::core::RobotModelConstPtr& robot_model, const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World())); /** \brief construct using a urdf and srdf. @@ -137,23 +138,23 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from } /** \brief Get the kinematic model for which the planning scene is maintained */ - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { // the kinematic model does not change return robot_model_; } /** \brief Get the state at which the robot is assumed to be. */ - const robot_state::RobotState& getCurrentState() const + const moveit::core::RobotState& getCurrentState() const { // if we have an updated state, return it; otherwise, return the parent one return robot_state_ ? *robot_state_ : parent_->getCurrentState(); } /** \brief Get the state at which the robot is assumed to be. */ - robot_state::RobotState& getCurrentStateNonConst(); + moveit::core::RobotState& getCurrentStateNonConst(); /** \brief Get a copy of the current state with components overwritten by the state message \e update */ - robot_state::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState& update) const; + moveit::core::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState& update) const; /** * \name Reasoning about frames @@ -168,7 +169,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from } /** \brief Get the set of fixed transforms from known frames to the planning frame */ - const robot_state::Transforms& getTransforms() const + const moveit::core::Transforms& getTransforms() const { if (scene_transforms_ || !parent_) { @@ -181,10 +182,10 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Get the set of fixed transforms from known frames to the planning frame. This variant is non-const and also * updates the current state */ - const robot_state::Transforms& getTransforms(); + const moveit::core::Transforms& getTransforms(); /** \brief Get the set of fixed transforms from known frames to the planning frame */ - robot_state::Transforms& getTransformsNonConst(); + moveit::core::Transforms& getTransformsNonConst(); /** \brief Get the transform corresponding to the frame \e id. This will be known if \e id is a link name, an attached body id or a collision object. @@ -204,17 +205,17 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be successful or not. This function also updates the link transforms of \e state. */ - const Eigen::Isometry3d& getFrameTransform(robot_state::RobotState& state, const std::string& id) const + const Eigen::Isometry3d& getFrameTransform(moveit::core::RobotState& state, const std::string& id) const { state.updateLinkTransforms(); - return getFrameTransform(static_cast(state), id); + return getFrameTransform(static_cast(state), id); } /** \brief Get the transform corresponding to the frame \e id. This will be known if \e id is a link name, an attached body id or a collision object. Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be successful or not. */ - const Eigen::Isometry3d& getFrameTransform(const robot_state::RobotState& state, const std::string& id) const; + const Eigen::Isometry3d& getFrameTransform(const moveit::core::RobotState& state, const std::string& id) const; /** \brief Check if a transform to the frame \e id is known. This will be known if \e id is a link name, an attached * body id or a collision object */ @@ -222,7 +223,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check if a transform to the frame \e id is known. This will be known if \e id is a link name, an attached * body id or a collision object */ - bool knowsFrameTransform(const robot_state::RobotState& state, const std::string& id) const; + bool knowsFrameTransform(const moveit::core::RobotState& state, const std::string& id) const; /**@}*/ @@ -356,16 +357,16 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from specified, collision checking is done for that group only. The link transforms for \e state are updated before the collision check. */ - bool isStateColliding(robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const + bool isStateColliding(moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const { state.updateCollisionBodyTransforms(); - return isStateColliding(static_cast(state), group, verbose); + return isStateColliding(static_cast(state), group, verbose); } /** \brief Check if a given state is in collision (with the environment or self collision) If a group name is specified, collision checking is done for that group only. It is expected that the link transforms of \e state are up to date. */ - bool isStateColliding(const robot_state::RobotState& state, const std::string& group = "", + bool isStateColliding(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is in collision (with the environment or self collision) @@ -386,33 +387,33 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check whether a specified state (\e robot_state) is in collision. This variant of the function takes a non-const \e robot_state and calls updateCollisionBodyTransforms() on it. */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - robot_state::RobotState& robot_state) const + moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - checkCollision(req, res, static_cast(robot_state)); + checkCollision(req, res, static_cast(robot_state)); } /** \brief Check whether a specified state (\e robot_state) is in collision. The collision transforms of \e * robot_state are * expected to be up to date. */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state) const; + const moveit::core::RobotState& robot_state) const; /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given allowed collision matrix (\e acm). This variant of the function takes a non-const \e robot_state and updates its link transforms if needed. */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - robot_state::RobotState& robot_state, + moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - checkCollision(req, res, static_cast(robot_state), acm); + checkCollision(req, res, static_cast(robot_state), acm); } /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given allowed collision matrix (\e acm). */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Check whether the current state is in collision, @@ -433,7 +434,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from but use a collision_detection::CollisionRobot instance that has no padding. */ void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state) const + const moveit::core::RobotState& robot_state) const { checkCollisionUnpadded(req, res, robot_state, getAllowedCollisionMatrix()); } @@ -442,10 +443,10 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from but use a collision_detection::CollisionRobot instance that has no padding. Update the link transforms of \e robot_state if needed. */ void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, robot_state::RobotState& robot_state) const + collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - checkCollisionUnpadded(req, res, static_cast(robot_state), + checkCollisionUnpadded(req, res, static_cast(robot_state), getAllowedCollisionMatrix()); } @@ -453,17 +454,17 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding. This variant of the function takes a non-const \e robot_state and calls updates the link transforms if needed. */ void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, robot_state::RobotState& robot_state, + collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - checkCollisionUnpadded(req, res, static_cast(robot_state), acm); + checkCollisionUnpadded(req, res, static_cast(robot_state), acm); } /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding. */ void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const robot_state::RobotState& robot_state, + collision_detection::CollisionResult& res, const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Check whether the current state is in self collision */ @@ -478,15 +479,16 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check whether a specified state (\e robot_state) is in self collision */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - robot_state::RobotState& robot_state) const + moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - checkSelfCollision(req, res, static_cast(robot_state), getAllowedCollisionMatrix()); + checkSelfCollision(req, res, static_cast(robot_state), + getAllowedCollisionMatrix()); } /** \brief Check whether a specified state (\e robot_state) is in self collision */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state) const + const moveit::core::RobotState& robot_state) const { // do self-collision checking with the unpadded version of the robot getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix()); @@ -495,17 +497,17 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given allowed collision matrix (\e acm). The link transforms of \e robot_state are updated if needed. */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - robot_state::RobotState& robot_state, + moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - checkSelfCollision(req, res, static_cast(robot_state), acm); + checkSelfCollision(req, res, static_cast(robot_state), acm); } /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given allowed collision matrix (\e acm) */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { // do self-collision checking with the unpadded version of the robot @@ -523,30 +525,30 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. Update the link transforms for \e robot_state if needed. */ - void getCollidingLinks(std::vector& links, robot_state::RobotState& robot_state) const + void getCollidingLinks(std::vector& links, moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - getCollidingLinks(links, static_cast(robot_state), getAllowedCollisionMatrix()); + getCollidingLinks(links, static_cast(robot_state), getAllowedCollisionMatrix()); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state */ - void getCollidingLinks(std::vector& links, const robot_state::RobotState& robot_state) const + void getCollidingLinks(std::vector& links, const moveit::core::RobotState& robot_state) const { getCollidingLinks(links, robot_state, getAllowedCollisionMatrix()); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the allowed collision matrix (\e acm) */ - void getCollidingLinks(std::vector& links, robot_state::RobotState& robot_state, + void getCollidingLinks(std::vector& links, moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - getCollidingLinks(links, static_cast(robot_state), acm); + getCollidingLinks(links, static_cast(robot_state), acm); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the allowed collision matrix (\e acm) */ - void getCollidingLinks(std::vector& links, const robot_state::RobotState& robot_state, + void getCollidingLinks(std::vector& links, const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Get the names of the links that are involved in collisions for the current state. @@ -561,7 +563,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Get the names of the links that are involved in collisions for the state \e robot_state */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - const robot_state::RobotState& robot_state) const + const moveit::core::RobotState& robot_state) const { getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix()); } @@ -569,26 +571,26 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. Update the link transforms for \e robot_state if needed. */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - robot_state::RobotState& robot_state) const + moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - getCollidingPairs(contacts, static_cast(robot_state), getAllowedCollisionMatrix()); + getCollidingPairs(contacts, static_cast(robot_state), getAllowedCollisionMatrix()); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - robot_state::RobotState& robot_state, + moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - getCollidingPairs(contacts, static_cast(robot_state), acm); + getCollidingPairs(contacts, static_cast(robot_state), acm); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the allowed collision matrix (\e acm) */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const; /**@}*/ @@ -601,31 +603,31 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring * self-collisions) */ - double distanceToCollision(robot_state::RobotState& robot_state) const + double distanceToCollision(moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - return distanceToCollision(static_cast(robot_state)); + return distanceToCollision(static_cast(robot_state)); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring * self-collisions) */ - double distanceToCollision(const robot_state::RobotState& robot_state) const + double distanceToCollision(const moveit::core::RobotState& robot_state) const { return getCollisionEnv()->distanceRobot(robot_state, getAllowedCollisionMatrix()); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring * self-collisions), if the robot has no padding */ - double distanceToCollisionUnpadded(robot_state::RobotState& robot_state) const + double distanceToCollisionUnpadded(moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - return distanceToCollisionUnpadded(static_cast(robot_state)); + return distanceToCollisionUnpadded(static_cast(robot_state)); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring * self-collisions), if the robot has no padding */ - double distanceToCollisionUnpadded(const robot_state::RobotState& robot_state) const + double distanceToCollisionUnpadded(const moveit::core::RobotState& robot_state) const { return getCollisionEnvUnpadded()->distanceRobot(robot_state, getAllowedCollisionMatrix()); } @@ -633,17 +635,17 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring * self-collisions * and elements that are allowed to collide. */ - double distanceToCollision(robot_state::RobotState& robot_state, + double distanceToCollision(moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - return distanceToCollision(static_cast(robot_state), acm); + return distanceToCollision(static_cast(robot_state), acm); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring * self-collisions * and elements that are allowed to collide. */ - double distanceToCollision(const robot_state::RobotState& robot_state, + double distanceToCollision(const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { return getCollisionEnv()->distanceRobot(robot_state, acm); @@ -652,17 +654,17 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring * self-collisions * and elements that are allowed to collide, if the robot has no padding. */ - double distanceToCollisionUnpadded(robot_state::RobotState& robot_state, + double distanceToCollisionUnpadded(moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - return distanceToCollisionUnpadded(static_cast(robot_state), acm); + return distanceToCollisionUnpadded(static_cast(robot_state), acm); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring * self-collisions * and elements that always allowed to collide, if the robot has no padding. */ - double distanceToCollisionUnpadded(const robot_state::RobotState& robot_state, + double distanceToCollisionUnpadded(const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { return getCollisionEnvUnpadded()->distanceRobot(robot_state, acm); @@ -752,10 +754,10 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from void setCurrentState(const moveit_msgs::RobotState& state); /** \brief Set the current robot state */ - void setCurrentState(const robot_state::RobotState& state); + void setCurrentState(const moveit::core::RobotState& state); /** \brief Set the callback to be triggered when changes are made to the current scene state */ - void setAttachedBodyUpdateCallback(const robot_state::AttachedBodyCallback& callback); + void setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback& callback); /** \brief Set the callback to be triggered when changes are made to the current scene world */ void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn& callback); @@ -823,14 +825,14 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check if a given state is feasible, in accordance to the feasibility predicate specified by * setStateFeasibilityPredicate(). Returns true if no feasibility predicate was specified. */ - bool isStateFeasible(const robot_state::RobotState& state, bool verbose = false) const; + bool isStateFeasible(const moveit::core::RobotState& state, bool verbose = false) const; /** \brief Check if a given state satisfies a set of constraints */ bool isStateConstrained(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr, bool verbose = false) const; /** \brief Check if a given state satisfies a set of constraints */ - bool isStateConstrained(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr, + bool isStateConstrained(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr, bool verbose = false) const; /** \brief Check if a given state satisfies a set of constraints */ @@ -838,14 +840,14 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const; /** \brief Check if a given state satisfies a set of constraints */ - bool isStateConstrained(const robot_state::RobotState& state, + bool isStateConstrained(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions and feasibility */ bool isStateValid(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions and feasibility */ - bool isStateValid(const robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const; + bool isStateValid(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user * specified validity conditions hold as well */ @@ -854,12 +856,12 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user * specified validity conditions hold as well */ - bool isStateValid(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr, + bool isStateValid(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user * specified validity conditions hold as well */ - bool isStateValid(const robot_state::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, + bool isStateValid(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility) */ @@ -928,12 +930,12 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from double overlap_fraction = 0.9) const; /** \brief Get the top \e max_costs cost sources for a specified state. The resulting costs are stored in \e costs */ - void getCostSources(const robot_state::RobotState& state, std::size_t max_costs, + void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, std::set& costs) const; /** \brief Get the top \e max_costs cost sources for a specified state, but only for group \e group_name. The * resulting costs are stored in \e costs */ - void getCostSources(const robot_state::RobotState& state, std::size_t max_costs, const std::string& group_name, + void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, const std::string& group_name, std::set& costs) const; /** \brief Outputs debug information about the planning scene contents */ @@ -964,8 +966,8 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from void initialize(); /* helper function to create a RobotModel from a urdf/srdf. */ - static robot_model::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, - const srdf::ModelConstSharedPtr& srdf_model); + static moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, + const srdf::ModelConstSharedPtr& srdf_model); /* Helper functions for processing collision objects */ bool processCollisionObjectAdd(const moveit_msgs::CollisionObject& object); @@ -1012,16 +1014,16 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from PlanningSceneConstPtr parent_; // Null unless this is a diff scene - robot_model::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent) + moveit::core::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent) - robot_state::RobotStatePtr robot_state_; // if NULL use parent's + moveit::core::RobotStatePtr robot_state_; // if NULL use parent's // Called when changes are made to attached bodies - robot_state::AttachedBodyCallback current_state_attached_body_callback_; + moveit::core::AttachedBodyCallback current_state_attached_body_callback_; // This variable is not necessarily used by child planning scenes // This Transforms class is actually a SceneTransform class - robot_state::TransformsPtr scene_transforms_; // if NULL use parent's + moveit::core::TransformsPtr scene_transforms_; // if NULL use parent's collision_detection::WorldPtr world_; // never NULL, never shared with parent/child collision_detection::WorldConstPtr world_const_; // copy of world_ diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index ad8b363960..5f6c75d38e 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -56,7 +56,7 @@ const std::string PlanningScene::DEFAULT_SCENE_NAME = "(noname)"; const std::string LOGNAME = "planning_scene"; -class SceneTransforms : public robot_state::Transforms +class SceneTransforms : public moveit::core::Transforms { public: SceneTransforms(const PlanningScene* scene) : Transforms(scene->getRobotModel()->getModelFrame()), scene_(scene) @@ -110,7 +110,7 @@ bool PlanningScene::isEmpty(const moveit_msgs::PlanningSceneWorld& msg) return moveit::core::isEmpty(msg); } -PlanningScene::PlanningScene(const robot_model::RobotModelConstPtr& robot_model, +PlanningScene::PlanningScene(const moveit::core::RobotModelConstPtr& robot_model, const collision_detection::WorldPtr& world) : robot_model_(robot_model), world_(world), world_const_(world) { @@ -146,7 +146,7 @@ void PlanningScene::initialize() scene_transforms_.reset(new SceneTransforms(this)); - robot_state_.reset(new robot_state::RobotState(robot_model_)); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); robot_state_->setToDefaultValues(); robot_state_->update(); @@ -164,12 +164,12 @@ void PlanningScene::initialize() } /* return NULL on failure */ -robot_model::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, - const srdf::ModelConstSharedPtr& srdf_model) +moveit::core::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, + const srdf::ModelConstSharedPtr& srdf_model) { - robot_model::RobotModelPtr robot_model(new robot_model::RobotModel(urdf_model, srdf_model)); + moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model, srdf_model)); if (!robot_model || !robot_model->getRootJoint()) - return robot_model::RobotModelPtr(); + return moveit::core::RobotModelPtr(); return robot_model; } @@ -484,7 +484,7 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest& void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state) const + const moveit::core::RobotState& robot_state) const { // check collision with the world using the padded version getCollisionEnv()->checkRobotCollision(req, res, robot_state, getAllowedCollisionMatrix()); @@ -507,7 +507,7 @@ void PlanningScene::checkSelfCollision(const collision_detection::CollisionReque void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { // check collision with the world using the padded version @@ -529,7 +529,7 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { // check collision with the world using the unpadded version @@ -551,7 +551,7 @@ void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::Cont } void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { collision_detection::CollisionRequest req; @@ -571,7 +571,7 @@ void PlanningScene::getCollidingLinks(std::vector& links) getCollidingLinks(links, getCurrentState(), getAllowedCollisionMatrix()); } -void PlanningScene::getCollidingLinks(std::vector& links, const robot_state::RobotState& robot_state, +void PlanningScene::getCollidingLinks(std::vector& links, const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { collision_detection::CollisionResult::ContactMap contacts; @@ -593,25 +593,25 @@ const collision_detection::CollisionEnvPtr& PlanningScene::getCollisionEnvNonCon return active_collision_->cenv_; } -robot_state::RobotState& PlanningScene::getCurrentStateNonConst() +moveit::core::RobotState& PlanningScene::getCurrentStateNonConst() { if (!robot_state_) { - robot_state_.reset(new robot_state::RobotState(parent_->getCurrentState())); + robot_state_.reset(new moveit::core::RobotState(parent_->getCurrentState())); robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } robot_state_->update(); return *robot_state_; } -robot_state::RobotStatePtr PlanningScene::getCurrentStateUpdated(const moveit_msgs::RobotState& update) const +moveit::core::RobotStatePtr PlanningScene::getCurrentStateUpdated(const moveit_msgs::RobotState& update) const { - robot_state::RobotStatePtr state(new robot_state::RobotState(getCurrentState())); - robot_state::robotStateMsgToRobotState(getTransforms(), update, *state); + moveit::core::RobotStatePtr state(new moveit::core::RobotState(getCurrentState())); + moveit::core::robotStateMsgToRobotState(getTransforms(), update, *state); return state; } -void PlanningScene::setAttachedBodyUpdateCallback(const robot_state::AttachedBodyCallback& callback) +void PlanningScene::setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback& callback) { current_state_attached_body_callback_ = callback; if (robot_state_) @@ -634,14 +634,14 @@ collision_detection::AllowedCollisionMatrix& PlanningScene::getAllowedCollisionM return *acm_; } -const robot_state::Transforms& PlanningScene::getTransforms() +const moveit::core::Transforms& PlanningScene::getTransforms() { // Trigger an update of the robot transforms getCurrentStateNonConst().update(); return static_cast(this)->getTransforms(); } -robot_state::Transforms& PlanningScene::getTransformsNonConst() +moveit::core::Transforms& PlanningScene::getTransformsNonConst() { // Trigger an update of the robot transforms getCurrentStateNonConst().update(); @@ -667,7 +667,7 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene_ms scene_msg.fixed_frame_transforms.clear(); if (robot_state_) - robot_state::robotStateToRobotStateMsg(*robot_state_, scene_msg.robot_state); + moveit::core::robotStateToRobotStateMsg(*robot_state_, scene_msg.robot_state); else { scene_msg.robot_state = moveit_msgs::RobotState(); @@ -876,7 +876,7 @@ void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg) c scene_msg.robot_model_name = getRobotModel()->getName(); getTransforms().copyTransforms(scene_msg.fixed_frame_transforms); - robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state); + moveit::core::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state); getAllowedCollisionMatrix().getMessage(scene_msg.allowed_collision_matrix); getCollisionEnv()->getPadding(scene_msg.link_padding); getCollisionEnv()->getScale(scene_msg.link_scale); @@ -905,7 +905,7 @@ void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg, if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS) { - robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, true); + moveit::core::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, true); for (moveit_msgs::AttachedCollisionObject& attached_collision_object : scene_msg.robot_state.attached_collision_objects) { @@ -917,7 +917,7 @@ void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg, } else if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE) { - robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, false); + moveit::core::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, false); } if (comp.components & moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX) @@ -1089,13 +1089,13 @@ void PlanningScene::setCurrentState(const moveit_msgs::RobotState& state) { if (!robot_state_) { - robot_state_.reset(new robot_state::RobotState(parent_->getCurrentState())); + robot_state_.reset(new moveit::core::RobotState(parent_->getCurrentState())); robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } - robot_state::robotStateMsgToRobotState(getTransforms(), state_no_attached, *robot_state_); + moveit::core::robotStateMsgToRobotState(getTransforms(), state_no_attached, *robot_state_); } else - robot_state::robotStateMsgToRobotState(*scene_transforms_, state_no_attached, *robot_state_); + moveit::core::robotStateMsgToRobotState(*scene_transforms_, state_no_attached, *robot_state_); for (std::size_t i = 0; i < state.attached_collision_objects.size(); ++i) { @@ -1110,7 +1110,7 @@ void PlanningScene::setCurrentState(const moveit_msgs::RobotState& state) } } -void PlanningScene::setCurrentState(const robot_state::RobotState& state) +void PlanningScene::setCurrentState(const moveit::core::RobotState& state) { getCurrentStateNonConst() = state; } @@ -1129,7 +1129,7 @@ void PlanningScene::decoupleParent() if (!robot_state_) { - robot_state_.reset(new robot_state::RobotState(parent_->getCurrentState())); + robot_state_.reset(new moveit::core::RobotState(parent_->getCurrentState())); robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } @@ -1388,7 +1388,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache if (!robot_state_) // there must be a parent in this case { - robot_state_.reset(new robot_state::RobotState(parent_->getCurrentState())); + robot_state_.reset(new moveit::core::RobotState(parent_->getCurrentState())); robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } robot_state_->update(); @@ -1430,7 +1430,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache return false; } - const robot_model::LinkModel* link_model = getRobotModel()->getLinkModel(object.link_name); + const moveit::core::LinkModel* link_model = getRobotModel()->getLinkModel(object.link_name); if (link_model) { // items to build the attached object from (filled from existing world object or message) @@ -1578,7 +1578,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache } else // APPEND: augment to existing attached object { - const robot_state::AttachedBody* ab = robot_state_->getAttachedBody(object.object.id); + const moveit::core::AttachedBody* ab = robot_state_->getAttachedBody(object.object.id); shapes.insert(shapes.end(), ab->getShapes().begin(), ab->getShapes().end()); poses.insert(poses.end(), ab->getFixedTransforms().begin(), ab->getFixedTransforms().end()); subframe_poses.insert(ab->getSubframeTransforms().begin(), ab->getSubframeTransforms().end()); @@ -1603,21 +1603,21 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache else if (object.object.operation == moveit_msgs::CollisionObject::REMOVE) // == DETACH { // STEP 1: Get info about the object from the RobotState - std::vector attached_bodies; + std::vector attached_bodies; if (object.link_name.empty()) { if (object.object.id.empty()) robot_state_->getAttachedBodies(attached_bodies); else { - const robot_state::AttachedBody* body = robot_state_->getAttachedBody(object.object.id); + const moveit::core::AttachedBody* body = robot_state_->getAttachedBody(object.object.id); if (body) attached_bodies.push_back(body); } } else { - const robot_model::LinkModel* link_model = getRobotModel()->getLinkModel(object.link_name); + const moveit::core::LinkModel* link_model = getRobotModel()->getLinkModel(object.link_name); if (link_model) { // If no specific object id is given, then we remove all objects attached to the link_name. @@ -1627,7 +1627,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache } else // A specific object id will be removed. { - const robot_state::AttachedBody* body = robot_state_->getAttachedBody(object.object.id); + const moveit::core::AttachedBody* body = robot_state_->getAttachedBody(object.object.id); if (body) attached_bodies.push_back(body); } @@ -1867,7 +1867,7 @@ const Eigen::Isometry3d& PlanningScene::getFrameTransform(const std::string& fra return getFrameTransform(getCurrentState(), frame_id); } -const Eigen::Isometry3d& PlanningScene::getFrameTransform(const robot_state::RobotState& state, +const Eigen::Isometry3d& PlanningScene::getFrameTransform(const moveit::core::RobotState& state, const std::string& frame_id) const { if (!frame_id.empty() && frame_id[0] == '/') @@ -1890,7 +1890,7 @@ bool PlanningScene::knowsFrameTransform(const std::string& frame_id) const return knowsFrameTransform(getCurrentState(), frame_id); } -bool PlanningScene::knowsFrameTransform(const robot_state::RobotState& state, const std::string& frame_id) const +bool PlanningScene::knowsFrameTransform(const moveit::core::RobotState& state, const std::string& frame_id) const { if (!frame_id.empty() && frame_id[0] == '/') return knowsFrameTransform(frame_id.substr(1)); @@ -2003,8 +2003,8 @@ void PlanningScene::removeObjectColor(const std::string& object_id) bool PlanningScene::isStateColliding(const moveit_msgs::RobotState& state, const std::string& group, bool verbose) const { - robot_state::RobotState s(getCurrentState()); - robot_state::robotStateMsgToRobotState(getTransforms(), state, s); + moveit::core::RobotState s(getCurrentState()); + moveit::core::robotStateMsgToRobotState(getTransforms(), state, s); return isStateColliding(s, group, verbose); } @@ -2016,7 +2016,8 @@ bool PlanningScene::isStateColliding(const std::string& group, bool verbose) return isStateColliding(getCurrentState(), group, verbose); } -bool PlanningScene::isStateColliding(const robot_state::RobotState& state, const std::string& group, bool verbose) const +bool PlanningScene::isStateColliding(const moveit::core::RobotState& state, const std::string& group, + bool verbose) const { collision_detection::CollisionRequest req; req.verbose = verbose; @@ -2030,14 +2031,14 @@ bool PlanningScene::isStateFeasible(const moveit_msgs::RobotState& state, bool v { if (state_feasibility_) { - robot_state::RobotState s(getCurrentState()); - robot_state::robotStateMsgToRobotState(getTransforms(), state, s); + moveit::core::RobotState s(getCurrentState()); + moveit::core::robotStateMsgToRobotState(getTransforms(), state, s); return state_feasibility_(s, verbose); } return true; } -bool PlanningScene::isStateFeasible(const robot_state::RobotState& state, bool verbose) const +bool PlanningScene::isStateFeasible(const moveit::core::RobotState& state, bool verbose) const { if (state_feasibility_) return state_feasibility_(state, verbose); @@ -2047,12 +2048,12 @@ bool PlanningScene::isStateFeasible(const robot_state::RobotState& state, bool v bool PlanningScene::isStateConstrained(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr, bool verbose) const { - robot_state::RobotState s(getCurrentState()); - robot_state::robotStateMsgToRobotState(getTransforms(), state, s); + moveit::core::RobotState s(getCurrentState()); + moveit::core::robotStateMsgToRobotState(getTransforms(), state, s); return isStateConstrained(s, constr, verbose); } -bool PlanningScene::isStateConstrained(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr, +bool PlanningScene::isStateConstrained(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr, bool verbose) const { kinematic_constraints::KinematicConstraintSetPtr ks( @@ -2067,18 +2068,18 @@ bool PlanningScene::isStateConstrained(const robot_state::RobotState& state, con bool PlanningScene::isStateConstrained(const moveit_msgs::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, bool verbose) const { - robot_state::RobotState s(getCurrentState()); - robot_state::robotStateMsgToRobotState(getTransforms(), state, s); + moveit::core::RobotState s(getCurrentState()); + moveit::core::robotStateMsgToRobotState(getTransforms(), state, s); return isStateConstrained(s, constr, verbose); } -bool PlanningScene::isStateConstrained(const robot_state::RobotState& state, +bool PlanningScene::isStateConstrained(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, bool verbose) const { return constr.decide(state, verbose).satisfied; } -bool PlanningScene::isStateValid(const robot_state::RobotState& state, const std::string& group, bool verbose) const +bool PlanningScene::isStateValid(const moveit::core::RobotState& state, const std::string& group, bool verbose) const { static const moveit_msgs::Constraints EMP_CONSTRAINTS; return isStateValid(state, EMP_CONSTRAINTS, group, verbose); @@ -2093,12 +2094,12 @@ bool PlanningScene::isStateValid(const moveit_msgs::RobotState& state, const std bool PlanningScene::isStateValid(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr, const std::string& group, bool verbose) const { - robot_state::RobotState s(getCurrentState()); - robot_state::robotStateMsgToRobotState(getTransforms(), state, s); + moveit::core::RobotState s(getCurrentState()); + moveit::core::robotStateMsgToRobotState(getTransforms(), state, s); return isStateValid(s, constr, group, verbose); } -bool PlanningScene::isStateValid(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr, +bool PlanningScene::isStateValid(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr, const std::string& group, bool verbose) const { if (isStateColliding(state, group, verbose)) @@ -2108,7 +2109,7 @@ bool PlanningScene::isStateValid(const robot_state::RobotState& state, const mov return isStateConstrained(state, constr, verbose); } -bool PlanningScene::isStateValid(const robot_state::RobotState& state, +bool PlanningScene::isStateValid(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, const std::string& group, bool verbose) const { @@ -2154,8 +2155,8 @@ bool PlanningScene::isPathValid(const moveit_msgs::RobotState& start_state, bool verbose, std::vector* invalid_index) const { robot_trajectory::RobotTrajectory t(getRobotModel(), group); - robot_state::RobotState start(getCurrentState()); - robot_state::robotStateMsgToRobotState(getTransforms(), start_state, start); + moveit::core::RobotState start(getCurrentState()); + moveit::core::robotStateMsgToRobotState(getTransforms(), start_state, start); t.setRobotTrajectoryMsg(start, trajectory); return isPathValid(t, path_constraints, goal_constraints, group, verbose, invalid_index); } @@ -2173,7 +2174,7 @@ bool PlanningScene::isPathValid(const robot_trajectory::RobotTrajectory& traject std::size_t n_wp = trajectory.getWayPointCount(); for (std::size_t i = 0; i < n_wp; ++i) { - const robot_state::RobotState& st = trajectory.getWayPoint(i); + const moveit::core::RobotState& st = trajectory.getWayPoint(i); bool this_state_valid = true; if (isStateColliding(st, group, verbose)) @@ -2282,13 +2283,13 @@ void PlanningScene::getCostSources(const robot_trajectory::RobotTrajectory& traj collision_detection::removeOverlapping(costs, overlap_fraction); } -void PlanningScene::getCostSources(const robot_state::RobotState& state, std::size_t max_costs, +void PlanningScene::getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, std::set& costs) const { getCostSources(state, max_costs, std::string(), costs); } -void PlanningScene::getCostSources(const robot_state::RobotState& state, std::size_t max_costs, +void PlanningScene::getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, const std::string& group_name, std::set& costs) const { @@ -2304,7 +2305,7 @@ void PlanningScene::getCostSources(const robot_state::RobotState& state, std::si void PlanningScene::printKnownObjects(std::ostream& out) const { const std::vector& objects = getWorld()->getObjectIds(); - std::vector attached_bodies; + std::vector attached_bodies; getCurrentState().getAttachedBodies(attached_bodies); // Output @@ -2317,7 +2318,7 @@ void PlanningScene::printKnownObjects(std::ostream& out) const } out << " - Attached Bodies:\n"; - for (const robot_state::AttachedBody* attached_body : attached_bodies) + for (const moveit::core::AttachedBody* attached_body : attached_bodies) { out << "\t- " << attached_body->getName() << "\n"; } diff --git a/moveit_core/planning_scene/test/test_multi_threaded.cpp b/moveit_core/planning_scene/test/test_multi_threaded.cpp index eb9d1d1bb0..2cad1d799d 100644 --- a/moveit_core/planning_scene/test/test_multi_threaded.cpp +++ b/moveit_core/planning_scene/test/test_multi_threaded.cpp @@ -49,7 +49,7 @@ const int TRIALS = 1000; const int THREADS = 2; bool runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene* scene, - const robot_state::RobotState* state) + const moveit::core::RobotState* state) { collision_detection::CollisionRequest req; collision_detection::CollisionResult res; @@ -62,7 +62,7 @@ bool runCollisionDetection(unsigned int id, unsigned int trials, const planning_ } void runCollisionDetectionAssert(unsigned int id, unsigned int trials, const planning_scene::PlanningScene* scene, - const robot_state::RobotState* state, bool expected_result) + const moveit::core::RobotState* state, bool expected_result) { ASSERT_EQ(expected_result, runCollisionDetection(id, trials, scene, state)); } @@ -75,7 +75,7 @@ class CollisionDetectorThreadedTest : public testing::Test robot_model_ = moveit::core::loadTestingRobotModel("panda"); ASSERT_TRUE(static_cast(robot_model_)); - robot_state_.reset(new robot_state::RobotState(robot_model_)); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); planning_scene_.reset(new planning_scene::PlanningScene(robot_model_)); } @@ -85,13 +85,13 @@ class CollisionDetectorThreadedTest : public testing::Test bool robot_model_ok_; - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; collision_detection::CollisionEnvPtr cenv_; collision_detection::AllowedCollisionMatrixPtr acm_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotStatePtr robot_state_; planning_scene::PlanningScenePtr planning_scene_; }; @@ -99,17 +99,17 @@ class CollisionDetectorThreadedTest : public testing::Test /** \brief Tests the FCL collision detector in multiple threads. */ TEST_F(CollisionDetectorThreadedTest, FCLThreaded) { - std::vector states; + std::vector states; std::vector threads; std::vector collisions; for (unsigned int i = 0; i < THREADS; ++i) { - robot_state::RobotState* state = new robot_state::RobotState(planning_scene_->getRobotModel()); + moveit::core::RobotState* state = new moveit::core::RobotState(planning_scene_->getRobotModel()); collision_detection::CollisionRequest req; state->setToRandomPositions(); state->update(); - states.push_back(robot_state::RobotStatePtr(state)); + states.push_back(moveit::core::RobotStatePtr(state)); collisions.push_back(runCollisionDetection(0, 1, planning_scene_.get(), state)); } diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 9d93148edb..6a3b52875e 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -162,7 +162,7 @@ TEST(PlanningScene, isStateValid) loadRobotModels(urdf_model, srdf_model); planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); - robot_state::RobotState current_state = ps->getCurrentState(); + moveit::core::RobotState current_state = ps->getCurrentState(); if (ps->isStateColliding(current_state, "left_arm")) { EXPECT_FALSE(ps->isStateValid(current_state, "left_arm")); diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 2abd64c912..ab17d7f32a 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -1149,10 +1149,10 @@ const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* { if (!link) return link; - const robot_model::LinkModel* parent_link = link->getParentLinkModel(); - const robot_model::JointModel* joint = link->getParentJointModel(); + const moveit::core::LinkModel* parent_link = link->getParentLinkModel(); + const moveit::core::JointModel* joint = link->getParentJointModel(); - while (parent_link && joint->getType() == robot_model::JointModel::FIXED) + while (parent_link && joint->getType() == moveit::core::JointModel::FIXED) { link = parent_link; joint = link->getParentJointModel(); diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index aff7d5bdf5..46d17753b5 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -143,7 +143,7 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons } traj.clear(); - traj.push_back(RobotStatePtr(new robot_state::RobotState(*start_state))); + traj.push_back(RobotStatePtr(new moveit::core::RobotState(*start_state))); double last_valid_percentage = 0.0; for (std::size_t i = 1; i <= steps; ++i) @@ -156,7 +156,7 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons // Explicitly use a single IK attempt only: We want a smooth trajectory. // Random seeding (of additional attempts) would probably create IK jumps. if (start_state->setFromIK(group, pose, link->getName(), consistency_limits, 0.0, validCallback, options)) - traj.push_back(RobotStatePtr(new robot_state::RobotState(*start_state))); + traj.push_back(RobotStatePtr(new moveit::core::RobotState(*start_state))); else break; diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 21fea88a78..5324d65d8a 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1200,8 +1200,8 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link return false; } - const robot_model::JointModel* root_joint_model = group->getJointModels()[0]; // group->getJointRoots()[0]; - const robot_model::LinkModel* root_link_model = root_joint_model->getParentLinkModel(); + const moveit::core::JointModel* root_joint_model = group->getJointModels()[0]; // group->getJointRoots()[0]; + const moveit::core::LinkModel* root_link_model = root_joint_model->getParentLinkModel(); Eigen::Isometry3d reference_transform = root_link_model ? getGlobalLinkTransform(root_link_model).inverse() : Eigen::Isometry3d::Identity(); int rows = use_quaternion_representation ? 7 : 6; @@ -1239,21 +1239,21 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link continue; } unsigned int joint_index = group->getVariableGroupIndex(pjm->getName()); - if (pjm->getType() == robot_model::JointModel::REVOLUTE) + if (pjm->getType() == moveit::core::JointModel::REVOLUTE) { joint_transform = reference_transform * getGlobalLinkTransform(link); - joint_axis = joint_transform.rotation() * static_cast(pjm)->getAxis(); + joint_axis = joint_transform.rotation() * static_cast(pjm)->getAxis(); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation()); jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis; } - else if (pjm->getType() == robot_model::JointModel::PRISMATIC) + else if (pjm->getType() == moveit::core::JointModel::PRISMATIC) { joint_transform = reference_transform * getGlobalLinkTransform(link); - joint_axis = joint_transform.rotation() * static_cast(pjm)->getAxis(); + joint_axis = joint_transform.rotation() * static_cast(pjm)->getAxis(); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis; } - else if (pjm->getType() == robot_model::JointModel::PLANAR) + else if (pjm->getType() == moveit::core::JointModel::PLANAR) { joint_transform = reference_transform * getGlobalLinkTransform(link); joint_axis = joint_transform * Eigen::Vector3d(1.0, 0.0, 0.0); @@ -1595,13 +1595,13 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is } if (pose_frame != solver_tip_frame) { - const robot_model::LinkModel* link_model = getLinkModel(pose_frame); + const moveit::core::LinkModel* link_model = getLinkModel(pose_frame); if (!link_model) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Pose frame '" << pose_frame << "' does not exist."); return false; } - const robot_model::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); + const moveit::core::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); for (const std::pair& fixed_link : fixed_links) if (Transforms::sameFrame(fixed_link.first->getName(), solver_tip_frame)) { @@ -1806,10 +1806,10 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: } if (pose_frame != solver_tip_frame) { - const robot_model::LinkModel* link_model = getLinkModel(pose_frame); + const moveit::core::LinkModel* link_model = getLinkModel(pose_frame); if (!link_model) return false; - const robot_model::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); + const moveit::core::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); for (const std::pair& fixed_link : fixed_links) if (fixed_link.first->getName() == solver_tip_frame) { diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index 95ae2c1f62..871c0cc524 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -95,9 +95,9 @@ class Timing : public testing::Test TEST_F(Timing, stateUpdate) { - robot_model::RobotModelPtr model = moveit::core::loadTestingRobotModel("pr2_description"); + moveit::core::RobotModelPtr model = moveit::core::loadTestingRobotModel("pr2_description"); ASSERT_TRUE(bool(model)); - robot_state::RobotState state(model); + moveit::core::RobotState state(model); ScopedTimer t("RobotState updates: "); for (unsigned i = 0; i < 1e5; ++i) { diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 2a1fb259fc..358d0cb64c 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -535,7 +535,7 @@ TEST_F(OneRobot, FK) TEST_F(OneRobot, testPrintCurrentPositionWithJointLimits) { moveit::core::RobotState state(robot_model_); - const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e"); + const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e"); ASSERT_TRUE(joint_model_group); state.setToDefaultValues(); diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index fa058a2c93..1c0cf06f6f 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -61,15 +61,15 @@ class TestAABB : public testing::Test protected: void SetUp() override{}; - robot_state::RobotState loadModel(const std::string& robot_name) + moveit::core::RobotState loadModel(const std::string& robot_name) { - robot_model::RobotModelPtr model = moveit::core::loadTestingRobotModel(robot_name); + moveit::core::RobotModelPtr model = moveit::core::loadTestingRobotModel(robot_name); return loadModel(model); } - robot_state::RobotState loadModel(const robot_model::RobotModelPtr& model) + moveit::core::RobotState loadModel(const moveit::core::RobotModelPtr& model) { - robot_state::RobotState robot_state = robot_state::RobotState(model); + moveit::core::RobotState robot_state = moveit::core::RobotState(model); robot_state.setToDefaultValues(); robot_state.update(true); @@ -85,7 +85,7 @@ TEST_F(TestAABB, TestPR2) { // Contains a link with mesh geometry that is not centered - robot_state::RobotState pr2_state = this->loadModel("pr2"); + moveit::core::RobotState pr2_state = this->loadModel("pr2"); const Eigen::Vector3d& extents_base_footprint = pr2_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin(); // values taken from moveit_resources/pr2_description/urdf/robot.xml @@ -288,7 +288,7 @@ TEST_F(TestAABB, TestSimple) builder.addGroup({}, { "world_joint" }, "base"); ASSERT_TRUE(builder.isValid()); - robot_state::RobotState simple_state = loadModel(builder.build()); + moveit::core::RobotState simple_state = loadModel(builder.build()); std::vector simple_aabb; simple_state.computeAABB(simple_aabb); @@ -323,7 +323,7 @@ TEST_F(TestAABB, TestComplex) builder.addGroup({}, { "world_joint" }, "base"); ASSERT_TRUE(builder.isValid()); - robot_state::RobotState complex_state = this->loadModel(builder.build()); + moveit::core::RobotState complex_state = this->loadModel(builder.build()); EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[0], 0.1, 1e-4); EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[1], 1.0, 1e-4); diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index bba9b70656..5c84fce441 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -241,21 +241,21 @@ class OneRobot : public testing::Test moveit::core::RobotModelConstPtr robot_model_; }; -std::size_t generateTestTraj(std::vector>& traj, +std::size_t generateTestTraj(std::vector>& traj, const moveit::core::RobotModelConstPtr& robot_model_, const moveit::core::JointModelGroup* joint_model_group) { traj.clear(); - std::shared_ptr robot_state(new robot_state::RobotState(robot_model_)); + std::shared_ptr robot_state(new moveit::core::RobotState(robot_model_)); robot_state->setToDefaultValues(); double ja, jc; // 3 waypoints with default joints for (std::size_t traj_ix = 0; traj_ix < 3; ++traj_ix) { - // robot_state.reset(new robot_state::RobotState(*robot_state)); - traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state))); + // robot_state.reset(new moveit::core::RobotState(*robot_state)); + traj.push_back(moveit::core::RobotStatePtr(new moveit::core::RobotState(*robot_state))); } ja = robot_state->getVariablePosition("panda_joint0"); @@ -266,20 +266,20 @@ std::size_t generateTestTraj(std::vectorsetVariablePosition("panda_joint0", ja); jc = jc - 0.01; robot_state->setVariablePosition("panda_joint1", jc); - traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state))); + traj.push_back(moveit::core::RobotStatePtr(new moveit::core::RobotState(*robot_state))); // 5th waypoint with a large jump of 1.01 in first revolute joint ja = ja + 1.01; robot_state->setVariablePosition("panda_joint0", ja); - traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state))); + traj.push_back(moveit::core::RobotStatePtr(new moveit::core::RobotState(*robot_state))); // 6th waypoint with a large jump of 1.01 in first prismatic joint jc = jc + 1.01; robot_state->setVariablePosition("panda_joint1", jc); - traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state))); + traj.push_back(moveit::core::RobotStatePtr(new moveit::core::RobotState(*robot_state))); // 7th waypoint with no jump - traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state))); + traj.push_back(moveit::core::RobotStatePtr(new moveit::core::RobotState(*robot_state))); return traj.size(); } @@ -287,7 +287,7 @@ std::size_t generateTestTraj(std::vectorgetJointModelGroup("arm"); - std::vector> traj; + std::vector> traj; // The full trajectory should be of length 7 const std::size_t expected_full_traj_len = 7; @@ -302,7 +302,7 @@ TEST_F(OneRobot, testGenerateTrajectory) TEST_F(OneRobot, checkAbsoluteJointSpaceJump) { const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("arm"); - std::vector> traj; + std::vector> traj; // A revolute joint jumps 1.01 at the 5th waypoint and a prismatic joint jumps 1.01 at the 6th waypoint const std::size_t expected_revolute_jump_traj_len = 4; @@ -353,7 +353,7 @@ TEST_F(OneRobot, checkAbsoluteJointSpaceJump) TEST_F(OneRobot, checkRelativeJointSpaceJump) { const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("arm"); - std::vector> traj; + std::vector> traj; // The first large jump of 1.01 occurs at the 5th waypoint so the test should trim the trajectory to length 4 const std::size_t expected_relative_jump_traj_len = 4; diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index ae8865e85b..80b4539211 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -51,9 +51,9 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory); class RobotTrajectory { public: - RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const std::string& group); + RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group); - RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const robot_model::JointModelGroup* group); + RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const moveit::core::JointModelGroup* group); /** Assignment operator, performing a shallow copy, i.e. copying waypoints by pointer */ RobotTrajectory& operator=(const RobotTrajectory&) = default; @@ -64,12 +64,12 @@ class RobotTrajectory */ RobotTrajectory(const RobotTrajectory& other, bool deepcopy = false); - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } - const robot_model::JointModelGroup* getGroup() const + const moveit::core::JointModelGroup* getGroup() const { return group_; } @@ -83,32 +83,32 @@ class RobotTrajectory return waypoints_.size(); } - const robot_state::RobotState& getWayPoint(std::size_t index) const + const moveit::core::RobotState& getWayPoint(std::size_t index) const { return *waypoints_[index]; } - const robot_state::RobotState& getLastWayPoint() const + const moveit::core::RobotState& getLastWayPoint() const { return *waypoints_.back(); } - const robot_state::RobotState& getFirstWayPoint() const + const moveit::core::RobotState& getFirstWayPoint() const { return *waypoints_.front(); } - robot_state::RobotStatePtr& getWayPointPtr(std::size_t index) + moveit::core::RobotStatePtr& getWayPointPtr(std::size_t index) { return waypoints_[index]; } - robot_state::RobotStatePtr& getLastWayPointPtr() + moveit::core::RobotStatePtr& getLastWayPointPtr() { return waypoints_.back(); } - robot_state::RobotStatePtr& getFirstWayPointPtr() + moveit::core::RobotStatePtr& getFirstWayPointPtr() { return waypoints_.front(); } @@ -151,9 +151,9 @@ class RobotTrajectory * \param state - current robot state * \param dt - duration from previous */ - void addSuffixWayPoint(const robot_state::RobotState& state, double dt) + void addSuffixWayPoint(const moveit::core::RobotState& state, double dt) { - addSuffixWayPoint(robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt); + addSuffixWayPoint(moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt); } /** @@ -161,31 +161,31 @@ class RobotTrajectory * \param state - current robot state * \param dt - duration from previous */ - void addSuffixWayPoint(const robot_state::RobotStatePtr& state, double dt) + void addSuffixWayPoint(const moveit::core::RobotStatePtr& state, double dt) { state->update(); waypoints_.push_back(state); duration_from_previous_.push_back(dt); } - void addPrefixWayPoint(const robot_state::RobotState& state, double dt) + void addPrefixWayPoint(const moveit::core::RobotState& state, double dt) { - addPrefixWayPoint(robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt); + addPrefixWayPoint(moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt); } - void addPrefixWayPoint(const robot_state::RobotStatePtr& state, double dt) + void addPrefixWayPoint(const moveit::core::RobotStatePtr& state, double dt) { state->update(); waypoints_.push_front(state); duration_from_previous_.push_front(dt); } - void insertWayPoint(std::size_t index, const robot_state::RobotState& state, double dt) + void insertWayPoint(std::size_t index, const moveit::core::RobotState& state, double dt) { - insertWayPoint(index, robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt); + insertWayPoint(index, moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt); } - void insertWayPoint(std::size_t index, const robot_state::RobotStatePtr& state, double dt) + void insertWayPoint(std::size_t index, const moveit::core::RobotStatePtr& state, double dt) { state->update(); waypoints_.insert(waypoints_.begin() + index, state); @@ -221,7 +221,7 @@ class RobotTrajectory point in the trajectory to be constructed internally is obtained by copying the reference state and overwriting the content from a trajectory point in \e trajectory. */ - void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, + void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const trajectory_msgs::JointTrajectory& trajectory); /** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required @@ -230,7 +230,7 @@ class RobotTrajectory point in the trajectory to be constructed internally is obtained by copying the reference state and overwriting the content from a trajectory point in \e trajectory. */ - void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, + void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const moveit_msgs::RobotTrajectory& trajectory); /** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required @@ -240,13 +240,13 @@ class RobotTrajectory using \e state. Each point in the trajectory to be constructed internally is obtained by copying the reference state and overwriting the content from a trajectory point in \e trajectory. */ - void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, const moveit_msgs::RobotState& state, + void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const moveit_msgs::RobotState& state, const moveit_msgs::RobotTrajectory& trajectory); void reverse(); void unwind(); - void unwind(const robot_state::RobotState& state); + void unwind(const moveit::core::RobotState& state); /** @brief Finds the waypoint indicies before and after a duration from start. * @param The duration from start. @@ -263,12 +263,12 @@ class RobotTrajectory * @param The resulting robot state. * @return True if state is valid, false otherwise (trajectory is empty). */ - bool getStateAtDurationFromStart(const double request_duration, robot_state::RobotStatePtr& output_state) const; + bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const; private: - robot_model::RobotModelConstPtr robot_model_; - const robot_model::JointModelGroup* group_; - std::deque waypoints_; + moveit::core::RobotModelConstPtr robot_model_; + const moveit::core::JointModelGroup* group_; + std::deque waypoints_; std::deque duration_from_previous_; }; } diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 70e837ca9d..475f973374 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -42,13 +42,13 @@ namespace robot_trajectory { -RobotTrajectory::RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const std::string& group) +RobotTrajectory::RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group) : robot_model_(robot_model), group_(group.empty() ? nullptr : robot_model->getJointModelGroup(group)) { } -RobotTrajectory::RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, - const robot_model::JointModelGroup* group) +RobotTrajectory::RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, + const moveit::core::JointModelGroup* group) : robot_model_(robot_model), group_(group) { } @@ -118,7 +118,7 @@ void RobotTrajectory::append(const RobotTrajectory& source, double dt, size_t st void RobotTrajectory::reverse() { std::reverse(waypoints_.begin(), waypoints_.end()); - for (robot_state::RobotStatePtr& waypoint : waypoints_) + for (moveit::core::RobotStatePtr& waypoint : waypoints_) { // reversing the trajectory implies inverting the velocity profile waypoint->invertVelocity(); @@ -136,7 +136,7 @@ void RobotTrajectory::unwind() if (waypoints_.empty()) return; - const std::vector& cont_joints = + const std::vector& cont_joints = group_ ? group_->getContinuousJointModels() : robot_model_->getContinuousJointModels(); for (const moveit::core::JointModel* cont_joint : cont_joints) @@ -166,12 +166,12 @@ void RobotTrajectory::unwind() waypoint->update(); } -void RobotTrajectory::unwind(const robot_state::RobotState& state) +void RobotTrajectory::unwind(const moveit::core::RobotState& state) { if (waypoints_.empty()) return; - const std::vector& cont_joints = + const std::vector& cont_joints = group_ ? group_->getContinuousJointModels() : robot_model_->getContinuousJointModels(); for (const moveit::core::JointModel* cont_joint : cont_joints) @@ -223,11 +223,11 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajec trajectory = moveit_msgs::RobotTrajectory(); if (waypoints_.empty()) return; - const std::vector& jnt = + const std::vector& jnt = group_ ? group_->getActiveJointModels() : robot_model_->getActiveJointModels(); - std::vector onedof; - std::vector mdof; + std::vector onedof; + std::vector mdof; trajectory.joint_trajectory.joint_names.clear(); trajectory.multi_dof_joint_trajectory.joint_names.clear(); @@ -306,7 +306,7 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajec geometry_msgs::TransformStamped ts = tf2::eigenToTransform(waypoints_[i]->getJointTransform(mdof[j])); trajectory.multi_dof_joint_trajectory.points[i].transforms[j] = ts.transform; // TODO: currently only checking for planar multi DOF joints / need to add check for floating - if (waypoints_[i]->hasVelocities() && (mdof[j]->getType() == robot_model::JointModel::JointType::PLANAR)) + if (waypoints_[i]->hasVelocities() && (mdof[j]->getType() == moveit::core::JointModel::JointType::PLANAR)) { const std::vector names = mdof[j]->getVariableNames(); const double* velocities = waypoints_[i]->getJointVelocities(mdof[j]); @@ -343,11 +343,11 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajec } } -void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, +void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const trajectory_msgs::JointTrajectory& trajectory) { // make a copy just in case the next clear() removes the memory for the reference passed in - const robot_state::RobotState& copy = reference_state; + const moveit::core::RobotState& copy = reference_state; clear(); std::size_t state_count = trajectory.points.size(); ros::Time last_time_stamp = trajectory.header.stamp; @@ -356,7 +356,7 @@ void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& refer for (std::size_t i = 0; i < state_count; ++i) { this_time_stamp = trajectory.header.stamp + trajectory.points[i].time_from_start; - robot_state::RobotStatePtr st(new robot_state::RobotState(copy)); + moveit::core::RobotStatePtr st(new moveit::core::RobotState(copy)); st->setVariablePositions(trajectory.joint_names, trajectory.points[i].positions); if (!trajectory.points[i].velocities.empty()) st->setVariableVelocities(trajectory.joint_names, trajectory.points[i].velocities); @@ -369,11 +369,11 @@ void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& refer } } -void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, +void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const moveit_msgs::RobotTrajectory& trajectory) { // make a copy just in case the next clear() removes the memory for the reference passed in - const robot_state::RobotState& copy = reference_state; + const moveit::core::RobotState& copy = reference_state; clear(); std::size_t state_count = @@ -385,7 +385,7 @@ void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& refer for (std::size_t i = 0; i < state_count; ++i) { - robot_state::RobotStatePtr st(new robot_state::RobotState(copy)); + moveit::core::RobotStatePtr st(new moveit::core::RobotState(copy)); if (trajectory.joint_trajectory.points.size() > i) { st->setVariablePositions(trajectory.joint_trajectory.joint_names, @@ -417,12 +417,12 @@ void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& refer } } -void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, +void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const moveit_msgs::RobotState& state, const moveit_msgs::RobotTrajectory& trajectory) { - robot_state::RobotState st(reference_state); - robot_state::robotStateMsgToRobotState(state, st); + moveit::core::RobotState st(reference_state); + moveit::core::robotStateMsgToRobotState(state, st); setRobotTrajectoryMsg(st, trajectory); } @@ -476,7 +476,7 @@ double RobotTrajectory::getWaypointDurationFromStart(std::size_t index) const } bool RobotTrajectory::getStateAtDurationFromStart(const double request_duration, - robot_state::RobotStatePtr& output_state) const + moveit::core::RobotStatePtr& output_state) const { // If there are no waypoints we can't do anything if (getWayPointCount() == 0) diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index ba57080bbd..208b16e813 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -44,7 +44,7 @@ class RobotTrajectoryTestFixture : public testing::Test { protected: moveit::core::RobotModelConstPtr robot_model_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotStatePtr robot_state_; const std::string robot_model_name_ = "panda"; const std::string arm_jmg_name_ = "panda_arm"; @@ -52,7 +52,7 @@ class RobotTrajectoryTestFixture : public testing::Test void SetUp() override { robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_); - robot_state_ = std::make_shared(robot_model_); + robot_state_ = std::make_shared(robot_model_); robot_state_->setToDefaultValues(); robot_state_->update(); } @@ -99,7 +99,7 @@ class RobotTrajectoryTestFixture : public testing::Test // Get the first waypoint by POINTER, modify it, and check that the value WAS updated in trajectory /////////////////////////// // Get the first waypoint by shared pointer - robot_state::RobotStatePtr trajectory_first_waypoint = trajectory->getWayPointPtr(0); + moveit::core::RobotStatePtr trajectory_first_waypoint = trajectory->getWayPointPtr(0); // Get the first waypoint joint values std::vector trajectory_first_state; trajectory_first_waypoint->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state); @@ -109,7 +109,7 @@ class RobotTrajectoryTestFixture : public testing::Test trajectory_first_waypoint->setJointGroupPositions(arm_jmg_name_, trajectory_first_state); // Check that the trajectory's first waypoint was updated - robot_state::RobotStatePtr trajectory_first_waypoint_after_update = trajectory->getWayPointPtr(0); + moveit::core::RobotStatePtr trajectory_first_waypoint_after_update = trajectory->getWayPointPtr(0); std::vector trajectory_first_state_after_update; trajectory_first_waypoint_after_update->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); EXPECT_EQ(trajectory_first_state[0], trajectory_first_state_after_update[0]); @@ -121,7 +121,7 @@ class RobotTrajectoryTestFixture : public testing::Test // Get the first waypoint by VALUE, modify it, and check that the value WAS NOT updated in trajectory /////////////////////////// // Get the first waypoint by shared pointer - robot_state::RobotState trajectory_first_waypoint = trajectory->getWayPoint(0); + moveit::core::RobotState trajectory_first_waypoint = trajectory->getWayPoint(0); // Get the first waypoint joint values std::vector trajectory_first_state; trajectory_first_waypoint.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state); @@ -131,7 +131,7 @@ class RobotTrajectoryTestFixture : public testing::Test trajectory_first_waypoint.setJointGroupPositions(arm_jmg_name_, trajectory_first_state); // Check that the trajectory's first waypoint was updated - robot_state::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0); + moveit::core::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0); std::vector trajectory_first_state_after_update; trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); EXPECT_NE(trajectory_first_state[0], trajectory_first_state_after_update[0]); @@ -164,12 +164,12 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryShallowCopy) modifyFirstWaypointPtrAndCheckTrajectory(trajectory); // Check that modifying the waypoint also modified the trajectory - robot_state::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0); + moveit::core::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0); std::vector trajectory_first_state_after_update; trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); // Get the first waypoint in the modified trajectory_copy - robot_state::RobotState trajectory_copy_first_waypoint_after_update = trajectory_copy->getWayPoint(0); + moveit::core::RobotState trajectory_copy_first_waypoint_after_update = trajectory_copy->getWayPoint(0); std::vector trajectory_copy_first_state_after_update; trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_copy_first_state_after_update); @@ -190,12 +190,12 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDeepCopy) modifyFirstWaypointPtrAndCheckTrajectory(trajectory); // Check that modifying the waypoint also modified the trajectory - robot_state::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0); + moveit::core::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0); std::vector trajectory_first_state_after_update; trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); // Get the first waypoint in the modified trajectory_copy - robot_state::RobotState trajectory_copy_first_waypoint_after_update = trajectory_copy->getWayPoint(0); + moveit::core::RobotState trajectory_copy_first_waypoint_after_update = trajectory_copy->getWayPoint(0); std::vector trajectory_copy_first_state_after_update; trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_copy_first_state_after_update); diff --git a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp index 1f7816b860..38938a01d2 100644 --- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp @@ -87,14 +87,14 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT if (trajectory.empty()) return true; - const robot_model::JointModelGroup* group = trajectory.getGroup(); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization", "It looks like the planner did not set " "the group the plan was computed for"); return false; } - const robot_model::RobotModel& rmodel = group->getParentModel(); + const moveit::core::RobotModel& rmodel = group->getParentModel(); const std::vector& idx = group->getVariableIndexList(); const std::vector& vars = group->getVariableNames(); double velocity_scaling_factor = 1.0; @@ -134,8 +134,8 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT // (required to force acceleration to specified values at endpoints) if (trajectory.getWayPointCount() >= 2) { - robot_state::RobotState point = trajectory.getWayPoint(1); - robot_state::RobotStatePtr p0, p1; + moveit::core::RobotState point = trajectory.getWayPoint(1); + moveit::core::RobotStatePtr p0, p1; // 2nd point is 90% of p0, and 10% of p1 p0 = trajectory.getWayPointPtr(0); @@ -197,7 +197,7 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT t2[j].accelerations_[num_points - 1] = t2[j].final_acceleration_; // Set bounds based on model, or default limits - const robot_model::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); + const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); t2[j].max_velocity_ = VLIMIT; t2[j].min_velocity_ = -VLIMIT; if (bounds.velocity_bounded_) diff --git a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp index 1b86b56918..e739158fcb 100644 --- a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp @@ -102,10 +102,10 @@ void IterativeParabolicTimeParameterization::applyVelocityConstraints(robot_traj std::vector& time_diff, const double max_velocity_scaling_factor) const { - const robot_model::JointModelGroup* group = rob_trajectory.getGroup(); + const moveit::core::JointModelGroup* group = rob_trajectory.getGroup(); const std::vector& vars = group->getVariableNames(); const std::vector& idx = group->getVariableIndexList(); - const robot_model::RobotModel& rmodel = group->getParentModel(); + const moveit::core::RobotModel& rmodel = group->getParentModel(); const int num_points = rob_trajectory.getWayPointCount(); double velocity_scaling_factor = 1.0; @@ -123,13 +123,13 @@ void IterativeParabolicTimeParameterization::applyVelocityConstraints(robot_traj for (int i = 0; i < num_points - 1; ++i) { - const robot_state::RobotStatePtr& curr_waypoint = rob_trajectory.getWayPointPtr(i); - const robot_state::RobotStatePtr& next_waypoint = rob_trajectory.getWayPointPtr(i + 1); + const moveit::core::RobotStatePtr& curr_waypoint = rob_trajectory.getWayPointPtr(i); + const moveit::core::RobotStatePtr& next_waypoint = rob_trajectory.getWayPointPtr(i + 1); for (std::size_t j = 0; j < vars.size(); ++j) { double v_max = DEFAULT_VEL_MAX; - const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]); + const moveit::core::VariableBounds& b = rmodel.getVariableBounds(vars[j]); if (b.velocity_bounded_) v_max = std::min(fabs(b.max_velocity_ * velocity_scaling_factor), fabs(b.min_velocity_ * velocity_scaling_factor)); @@ -195,11 +195,11 @@ void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const s double time_sum = 0.0; - robot_state::RobotStatePtr prev_waypoint; - robot_state::RobotStatePtr curr_waypoint; - robot_state::RobotStatePtr next_waypoint; + moveit::core::RobotStatePtr prev_waypoint; + moveit::core::RobotStatePtr curr_waypoint; + moveit::core::RobotStatePtr next_waypoint; - const robot_model::JointModelGroup* group = rob_trajectory.getGroup(); + const moveit::core::JointModelGroup* group = rob_trajectory.getGroup(); const std::vector& vars = group->getVariableNames(); const std::vector& idx = group->getVariableIndexList(); @@ -301,14 +301,14 @@ void IterativeParabolicTimeParameterization::applyAccelerationConstraints( robot_trajectory::RobotTrajectory& rob_trajectory, std::vector& time_diff, const double max_acceleration_scaling_factor) const { - robot_state::RobotStatePtr prev_waypoint; - robot_state::RobotStatePtr curr_waypoint; - robot_state::RobotStatePtr next_waypoint; + moveit::core::RobotStatePtr prev_waypoint; + moveit::core::RobotStatePtr curr_waypoint; + moveit::core::RobotStatePtr next_waypoint; - const robot_model::JointModelGroup* group = rob_trajectory.getGroup(); + const moveit::core::JointModelGroup* group = rob_trajectory.getGroup(); const std::vector& vars = group->getVariableNames(); const std::vector& idx = group->getVariableIndexList(); - const robot_model::RobotModel& rmodel = group->getParentModel(); + const moveit::core::RobotModel& rmodel = group->getParentModel(); const int num_points = rob_trajectory.getWayPointCount(); const unsigned int num_joints = group->getVariableCount(); @@ -363,7 +363,7 @@ void IterativeParabolicTimeParameterization::applyAccelerationConstraints( // Get acceleration limits double a_max = DEFAULT_ACCEL_MAX; - const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]); + const moveit::core::VariableBounds& b = rmodel.getVariableBounds(vars[j]); if (b.acceleration_bounded_) a_max = std::min(fabs(b.max_acceleration_ * acceleration_scaling_factor), fabs(b.min_acceleration_ * acceleration_scaling_factor)); @@ -464,7 +464,7 @@ bool IterativeParabolicTimeParameterization::computeTimeStamps(robot_trajectory: if (trajectory.empty()) return true; - const robot_model::JointModelGroup* group = trajectory.getGroup(); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { ROS_ERROR_NAMED("trajectory_processing.iterative_time_parameterization", "It looks like the planner did not set " diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 26b1cd815a..d92e2a11db 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -880,7 +880,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT if (trajectory.empty()) return true; - const robot_model::JointModelGroup* group = trajectory.getGroup(); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { ROS_ERROR_NAMED(LOGNAME, "It looks like the planner did not set the group the plan was computed for"); @@ -926,7 +926,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT // This is pretty much copied from IterativeParabolicTimeParameterization::applyVelocityConstraints const std::vector& vars = group->getVariableNames(); const std::vector& idx = group->getVariableIndexList(); - const robot_model::RobotModel& rmodel = group->getParentModel(); + const moveit::core::RobotModel& rmodel = group->getParentModel(); const unsigned num_joints = group->getVariableCount(); const unsigned num_points = trajectory.getWayPointCount(); @@ -935,7 +935,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT Eigen::VectorXd max_acceleration(num_joints); for (size_t j = 0; j < num_joints; ++j) { - const robot_model::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); + const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); // Limits need to be non-zero, otherwise we never exit max_velocity[j] = 1.0; @@ -959,7 +959,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT std::list points; for (size_t p = 0; p < num_points; ++p) { - robot_state::RobotStatePtr waypoint = trajectory.getWayPointPtr(p); + moveit::core::RobotStatePtr waypoint = trajectory.getWayPointPtr(p); Eigen::VectorXd new_point(num_joints); bool diverse_point = (p == 0); @@ -978,7 +978,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT if (points.size() == 1) { ROS_WARN_NAMED(LOGNAME, "Trajectory is not being parameterized since it only contains a single distinct waypoint."); - robot_state::RobotState waypoint = robot_state::RobotState(trajectory.getWayPoint(0)); + moveit::core::RobotState waypoint = moveit::core::RobotState(trajectory.getWayPoint(0)); trajectory.clear(); trajectory.addSuffixWayPoint(waypoint, 0.0); return true; @@ -996,7 +996,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt_); // Resample and fill in trajectory - robot_state::RobotState waypoint = robot_state::RobotState(trajectory.getWayPoint(0)); + moveit::core::RobotState waypoint = moveit::core::RobotState(trajectory.getWayPoint(0)); trajectory.clear(); double last_t = 0; for (size_t sample = 0; sample <= sample_count; ++sample) diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp index a27a834141..d3cba00ecb 100644 --- a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp @@ -53,7 +53,7 @@ int initRepeatedPointTrajectory(robot_trajectory::RobotTrajectory& trajectory) const int num = 3; unsigned i; - const robot_model::JointModelGroup* group = trajectory.getGroup(); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { ROS_ERROR_NAMED("trajectory_processing", "Need to set the group"); @@ -83,7 +83,7 @@ int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory, double const double max = 2.0; unsigned i; - const robot_model::JointModelGroup* group = trajectory.getGroup(); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { ROS_ERROR_NAMED("trajectory_processing", "Need to set the group"); @@ -109,7 +109,7 @@ int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory, double void printTrajectory(robot_trajectory::RobotTrajectory& trajectory) { - const robot_model::JointModelGroup* group = trajectory.getGroup(); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); const std::vector& idx = group->getVariableIndexList(); unsigned int count = trajectory.getWayPointCount(); @@ -118,13 +118,13 @@ void printTrajectory(robot_trajectory::RobotTrajectory& trajectory) std::cout << " Trajectory Points" << std::endl; for (unsigned i = 0; i < count; i++) { - robot_state::RobotStatePtr point = trajectory.getWayPointPtr(i); + moveit::core::RobotStatePtr point = trajectory.getWayPointPtr(i); printf(" waypoint %2d time %6.2f pos %6.2f vel %6.2f acc %6.2f ", i, trajectory.getWayPointDurationFromStart(i), point->getVariablePosition(idx[0]), point->getVariableVelocity(idx[0]), point->getVariableAcceleration(idx[0])); if (i > 0) { - robot_state::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1); + moveit::core::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1); printf("jrk %6.2f", (point->getVariableAcceleration(idx[0]) - prev->getVariableAcceleration(idx[0])) / (trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1))); diff --git a/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h b/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h index 233ad5f27c..5a5f72579d 100644 --- a/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h +++ b/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h @@ -102,7 +102,7 @@ class KinematicsCache * @param opt Parameters needed for defining the cache workspace * @return False if any error occured during initialization */ - bool initialize(kinematics::KinematicsBaseConstPtr& solver, const robot_model::RobotModelConstPtr& kinematic_model, + bool initialize(kinematics::KinematicsBaseConstPtr& solver, const moveit::core::RobotModelConstPtr& kinematic_model, const KinematicsCache::Options& opt); /** @brief Return the instance of the kinematics solver */ @@ -112,7 +112,7 @@ class KinematicsCache } /** @brief Return the instance of the kinematics model */ - const robot_model::RobotModelConstPtr& getModelInstance() const + const moveit::core::RobotModelConstPtr& getModelInstance() const { return kinematic_model_; } @@ -168,11 +168,11 @@ class KinematicsCache kinematics::KinematicsBaseConstPtr kinematics_solver_; /** An instance of the kinematics solver */ - robot_model::RobotModelConstPtr kinematic_model_; /** An instance of the kinematic model */ - robot_state::RobotStatePtr kinematic_state_; /** An instance of the kinematic state */ + moveit::core::RobotModelConstPtr kinematic_model_; /** An instance of the kinematic model */ + moveit::core::RobotStatePtr kinematic_state_; /** An instance of the kinematic state */ - const robot_model::JointModelGroup* joint_model_group_; /** Joint model group associated with this cache */ - std::shared_ptr joint_state_group_; /** Joint state corresponding to cache */ + const moveit::core::JointModelGroup* joint_model_group_; /** Joint model group associated with this cache */ + std::shared_ptr joint_state_group_; /** Joint state corresponding to cache */ // mutable std::vector solution_local_; /** Local pre-allocated storage */ diff --git a/moveit_experimental/kinematics_cache/v2/kinematics_cache/src/kinematics_cache.cpp b/moveit_experimental/kinematics_cache/v2/kinematics_cache/src/kinematics_cache.cpp index 127160ce31..a332d31b2b 100644 --- a/moveit_experimental/kinematics_cache/v2/kinematics_cache/src/kinematics_cache.cpp +++ b/moveit_experimental/kinematics_cache/v2/kinematics_cache/src/kinematics_cache.cpp @@ -46,15 +46,15 @@ KinematicsCache::KinematicsCache() : min_squared_distance_(1e6), max_squared_dis } bool KinematicsCache::initialize(kinematics::KinematicsBaseConstPtr& kinematics_solver, - const robot_model::RobotModelConstPtr& kinematic_model, + const moveit::core::RobotModelConstPtr& kinematic_model, const KinematicsCache::Options& opt) { options_ = opt; kinematics_solver_ = kinematics_solver; kinematic_model_ = kinematic_model; joint_model_group_ = kinematic_model_->getJointModelGroup(kinematics_solver_->getGroupName()); - kinematic_state_.reset(new robot_state::RobotState(kinematic_model)); - joint_state_group_.reset(new robot_state::JointStateGroup(kinematic_state_.get(), joint_model_group_)); + kinematic_state_.reset(new moveit::core::RobotState(kinematic_model)); + joint_state_group_.reset(new moveit::core::JointStateGroup(kinematic_state_.get(), joint_model_group_)); setup(opt); return true; diff --git a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h index 4200f2761e..5117918b15 100644 --- a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h +++ b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h @@ -74,7 +74,7 @@ class KinematicsConstraintAware * @param group_name The name of the group to configure this solver for * @return False if any error occurs */ - KinematicsConstraintAware(const robot_model::RobotModelConstPtr& kinematic_model, const std::string& group_name); + KinematicsConstraintAware(const moveit::core::RobotModelConstPtr& kinematic_model, const std::string& group_name); /** @brief Solve the planning problem * @param planning_scene A const reference to the planning scene @@ -101,14 +101,14 @@ class KinematicsConstraintAware return group_name_; } - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return kinematic_model_; } private: EigenSTL::vector_Isometry3d transformPoses(const planning_scene::PlanningSceneConstPtr& planning_scene, - const robot_state::RobotState& kinematic_state, + const moveit::core::RobotState& kinematic_state, const std::vector& poses, const std::string& target_frame) const; @@ -118,20 +118,20 @@ class KinematicsConstraintAware kinematics_constraint_aware::KinematicsResponse& kinematics_response) const; geometry_msgs::Pose getTipFramePose(const planning_scene::PlanningSceneConstPtr& planning_scene, - const robot_state::RobotState& kinematic_state, const geometry_msgs::Pose& pose, + const moveit::core::RobotState& kinematic_state, const geometry_msgs::Pose& pose, const std::string& link_name, unsigned int sub_group_index) const; bool validityCallbackFn(const planning_scene::PlanningSceneConstPtr& planning_scene, const kinematics_constraint_aware::KinematicsRequest& request, kinematics_constraint_aware::KinematicsResponse& response, - robot_state::JointStateGroup* joint_state_group, + moveit::core::JointStateGroup* joint_state_group, const std::vector& joint_group_variable_values) const; std::vector sub_groups_names_; - robot_model::RobotModelConstPtr kinematic_model_; + moveit::core::RobotModelConstPtr kinematic_model_; - const robot_model::JointModelGroup* joint_model_group_; + const moveit::core::JointModelGroup* joint_model_group_; std::string group_name_; diff --git a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h index b54fd335e3..87be80bcf5 100644 --- a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h +++ b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h @@ -68,7 +68,7 @@ class KinematicsRequest std::vector ik_link_names_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotStatePtr robot_state_; kinematic_constraints::KinematicConstraintSetPtr constraints_; @@ -78,7 +78,7 @@ class KinematicsRequest bool check_for_collisions_; - robot_state::StateValidityCallbackFn constraint_callback_; + moveit::core::StateValidityCallbackFn constraint_callback_; }; /** @@ -93,7 +93,7 @@ class KinematicsResponse virtual ~KinematicsResponse(){}; - robot_state::RobotStatePtr solution_; + moveit::core::RobotStatePtr solution_; std::vector constraint_eval_results_; diff --git a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp index a1e45998b4..5e25a7def2 100644 --- a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp +++ b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp @@ -45,7 +45,7 @@ namespace kinematics_constraint_aware { -KinematicsConstraintAware::KinematicsConstraintAware(const robot_model::RobotModelConstPtr& kinematic_model, +KinematicsConstraintAware::KinematicsConstraintAware(const moveit::core::RobotModelConstPtr& kinematic_model, const std::string& group_name) { if (!kinematic_model->hasJointModelGroup(group_name)) @@ -118,7 +118,7 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt if (!response.solution_) { - response.solution_.reset(new robot_state::RobotState(planning_scene->getCurrentState())); + response.solution_.reset(new moveit::core::RobotState(planning_scene->getCurrentState())); } ros::WallTime start_time = ros::WallTime::now(); @@ -129,7 +129,7 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt } // Setup the seed and the values for all other joints in the robot - robot_state::RobotState kinematic_state = *request.robot_state_; + moveit::core::RobotState kinematic_state = *request.robot_state_; std::vector ik_link_names = request.ik_link_names_; // Transform request to tip frame if necessary @@ -161,7 +161,7 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt EigenSTL::vector_Isometry3d goals = transformPoses(planning_scene, kinematic_state, request.pose_stamped_vector_, kinematic_model_->getModelFrame()); - robot_state::StateValidityCallbackFn constraint_callback_fn = + moveit::core::StateValidityCallbackFn constraint_callback_fn = boost::bind(&KinematicsConstraintAware::validityCallbackFn, this, planning_scene, request, response, _1, _2); bool result = false; @@ -198,7 +198,7 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt bool KinematicsConstraintAware::validityCallbackFn(const planning_scene::PlanningSceneConstPtr& planning_scene, const kinematics_constraint_aware::KinematicsRequest& request, kinematics_constraint_aware::KinematicsResponse& response, - robot_state::JointStateGroup* joint_state_group, + moveit::core::JointStateGroup* joint_state_group, const std::vector& joint_group_variable_values) const { joint_state_group->setVariableValues(joint_group_variable_values); @@ -321,7 +321,7 @@ bool KinematicsConstraintAware::convertServiceRequest( else kinematics_request.pose_stamped_vector_ = request.ik_request.pose_stamped_vector; - kinematics_request.robot_state_.reset(new robot_state::RobotState(planning_scene->getCurrentState())); + kinematics_request.robot_state_.reset(new moveit::core::RobotState(planning_scene->getCurrentState())); kinematics_request.robot_state_->setStateValues(request.ik_request.robot_state.joint_state); kinematics_request.constraints_.reset( new kinematic_constraints::KinematicConstraintSet(kinematic_model_, planning_scene->getTransforms())); @@ -330,13 +330,13 @@ bool KinematicsConstraintAware::convertServiceRequest( kinematics_request.group_name_ = request.ik_request.group_name; kinematics_request.check_for_collisions_ = true; - kinematics_response.solution_.reset(new robot_state::RobotState(planning_scene->getCurrentState())); + kinematics_response.solution_.reset(new moveit::core::RobotState(planning_scene->getCurrentState())); return true; } EigenSTL::vector_Isometry3d KinematicsConstraintAware::transformPoses( - const planning_scene::PlanningSceneConstPtr& planning_scene, const robot_state::RobotState& kinematic_state, + const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit::core::RobotState& kinematic_state, const std::vector& poses, const std::string& target_frame) const { Eigen::Isometry3d eigen_pose, eigen_pose_2; @@ -358,7 +358,7 @@ EigenSTL::vector_Isometry3d KinematicsConstraintAware::transformPoses( } geometry_msgs::Pose KinematicsConstraintAware::getTipFramePose( - const planning_scene::PlanningSceneConstPtr& planning_scene, const robot_state::RobotState& kinematic_state, + const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit::core::RobotState& kinematic_state, const geometry_msgs::Pose& pose, const std::string& link_name, unsigned int sub_group_index) const { geometry_msgs::Pose result; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 0774a03c07..5bbf55cfad 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -138,9 +138,9 @@ class JogCalcs */ void removeDimension(Eigen::MatrixXd& matrix, Eigen::VectorXd& delta_x, unsigned int row_to_remove); - const robot_state::JointModelGroup* joint_model_group_; + const moveit::core::JointModelGroup* joint_model_group_; - robot_state::RobotStatePtr kinematic_state_; + moveit::core::RobotStatePtr kinematic_state_; sensor_msgs::JointState joint_state_, original_joint_state_; std::map joint_state_name_map_; diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 2b7f3f644f..80059cd559 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -68,7 +68,7 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables, std::mu collision_detection::CollisionResult collision_result; // Copy the planning scene's version of current state into new memory - robot_state::RobotState current_state(getLockedPlanningSceneRO()->getCurrentState()); + moveit::core::RobotState current_state(getLockedPlanningSceneRO()->getCurrentState()); double velocity_scale_coefficient = -log(0.001) / parameters_.collision_proximity_threshold; ros::Rate collision_rate(parameters_.collision_check_rate); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index f324d6af78..4ec6da3c0c 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -56,8 +56,8 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader: ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); default_sleep_rate_.sleep(); } - const robot_model::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); - kinematic_state_ = std::make_shared(kinematic_model); + const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); + kinematic_state_ = std::make_shared(kinematic_model); kinematic_state_->setToDefaultValues(); joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h index 61865c482b..70146c5afa 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h @@ -160,8 +160,8 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase unsigned int dimension_; ///< Dimension of the group moveit_msgs::KinematicSolverInfo solver_info_; ///< Stores information for the inverse kinematics solver - const robot_model::JointModelGroup* joint_model_group_; - robot_state::RobotStatePtr state_; + const moveit::core::JointModelGroup* joint_model_group_; + moveit::core::RobotStatePtr state_; KDL::Chain kdl_chain_; std::unique_ptr fk_solver_; std::vector mimic_joints_; diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp index 9ba7d0e4aa..62d7a89337 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp @@ -214,7 +214,7 @@ bool KDLKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model unsigned int joint_counter = 0; for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i) { - const robot_model::JointModel* jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName()); + const moveit::core::JointModel* jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName()); // first check whether it belongs to the set of active joints in the group if (jm->getMimic() == nullptr && jm->getVariableCount() > 0) @@ -244,7 +244,7 @@ bool KDLKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model { if (!mimic_joint.active) { - const robot_model::JointModel* joint_model = + const moveit::core::JointModel* joint_model = joint_model_group_->getJointModel(mimic_joint.joint_name)->getMimic(); for (JointMimic& mimic_joint_recal : mimic_joints_) { @@ -257,7 +257,7 @@ bool KDLKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model } // Setup the joint state groups that we need - state_.reset(new robot_state::RobotState(robot_model_)); + state_.reset(new moveit::core::RobotState(robot_model_)); fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h index d9c3ab754b..ba81d148ce 100644 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h +++ b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h @@ -146,11 +146,11 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase unsigned int dimension_; ///< Dimension of the group moveit_msgs::KinematicSolverInfo solver_info_; ///< Stores information for the inverse kinematics solver - const robot_model::JointModelGroup* joint_model_group_; - robot_state::RobotStatePtr state_; + const moveit::core::JointModelGroup* joint_model_group_; + moveit::core::RobotStatePtr state_; KDL::Chain kdl_chain_; std::unique_ptr fk_solver_; - std::vector joints_; + std::vector joints_; std::vector joint_names_; int max_solver_iterations_; diff --git a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp index 0dc4f4496f..dd0fd88e9f 100644 --- a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp +++ b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp @@ -110,7 +110,7 @@ bool LMAKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model return false; } - for (const robot_model::JointModel* jm : joint_model_group_->getJointModels()) + for (const moveit::core::JointModel* jm : joint_model_group_->getJointModels()) { if (jm->getType() == moveit::core::JointModel::REVOLUTE || jm->getType() == moveit::core::JointModel::PRISMATIC) { @@ -133,7 +133,7 @@ bool LMAKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model ROS_INFO_NAMED("lma", "Using position only ik"); // Setup the joint state groups that we need - state_.reset(new robot_state::RobotState(robot_model_)); + state_.reset(new moveit::core::RobotState(robot_model_)); fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); diff --git a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h index 3f23c70150..2adad1d763 100644 --- a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h +++ b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h @@ -143,9 +143,9 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase unsigned int dimension_; /** Dimension of the group */ - const robot_model::JointModelGroup* joint_model_group_; + const moveit::core::JointModelGroup* joint_model_group_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotStatePtr robot_state_; int num_possible_redundant_joints_; diff --git a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp index e876e79ca7..4c7695a471 100644 --- a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp +++ b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp @@ -109,7 +109,7 @@ bool SrvKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model lookupParam("kinematics_solver_service_name", ik_service_name, std::string("solve_ik")); // Setup the joint state groups that we need - robot_state_.reset(new robot_state::RobotState(robot_model_)); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); robot_state_->setToDefaultValues(); // Create the ROS service client diff --git a/moveit_kinematics/test/benchmark_ik.cpp b/moveit_kinematics/test/benchmark_ik.cpp index 4e334dba6a..72d7b8c78d 100644 --- a/moveit_kinematics/test/benchmark_ik.cpp +++ b/moveit_kinematics/test/benchmark_ik.cpp @@ -77,14 +77,14 @@ int main(int argc, char* argv[]) spinner.start(); robot_model_loader::RobotModelLoader robot_model_loader; - const robot_model::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); + const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); planning_scene::PlanningScene planning_scene(kinematic_model); - robot_state::RobotState& kinematic_state = planning_scene.getCurrentStateNonConst(); + moveit::core::RobotState& kinematic_state = planning_scene.getCurrentStateNonConst(); collision_detection::CollisionRequest collision_request; collision_detection::CollisionResult collision_result; std::chrono::duration ik_time(0); std::chrono::time_point start; - std::vector groups; + std::vector groups; std::vector end_effectors; if (group == "all") diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index 2554516f5a..7ca176cd0b 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -77,7 +77,7 @@ class SharedData friend class KinematicsTest; typedef pluginlib::ClassLoader KinematicsLoader; - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; std::unique_ptr kinematics_loader_; std::string root_link_; std::string tip_link_; @@ -104,7 +104,7 @@ class SharedData ROS_INFO_STREAM("Loading robot model from " << ros::this_node::getNamespace() << "/" << ROBOT_DESCRIPTION_PARAM); // load robot model rdf_loader::RDFLoader rdf_loader(ROBOT_DESCRIPTION_PARAM); - robot_model_ = std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF()); + robot_model_ = std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF()); ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; // init ClassLoader @@ -280,8 +280,8 @@ class KinematicsTest : public ::testing::Test } public: - robot_model::RobotModelPtr robot_model_; - robot_model::JointModelGroup* jmg_; + moveit::core::RobotModelPtr robot_model_; + moveit::core::JointModelGroup* jmg_; kinematics::KinematicsBasePtr kinematics_solver_; random_numbers::RandomNumberGenerator rng_{ 42 }; std::string root_link_; @@ -307,7 +307,7 @@ TEST_F(KinematicsTest, getFK) { std::vector joints(kinematics_solver_->getJointNames().size(), 0.0); const std::vector& tip_frames = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); for (unsigned int i = 0; i < num_fk_tests_; ++i) @@ -331,7 +331,7 @@ TEST_F(KinematicsTest, randomWalkIK) { std::vector seed, goal, solution; const std::vector& tip_frames = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); if (!seed_.empty()) @@ -466,7 +466,7 @@ TEST_F(KinematicsTest, unitIK) std::vector seed, sol; const std::vector& tip_frames = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); // initial joint pose from seed_ or defaults @@ -539,7 +539,7 @@ TEST_F(KinematicsTest, searchIK) moveit_msgs::MoveItErrorCodes error_code; solution.resize(kinematics_solver_->getJointNames().size(), 0.0); const std::vector& fk_names = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); unsigned int success = 0; @@ -573,7 +573,7 @@ TEST_F(KinematicsTest, searchIKWithCallback) moveit_msgs::MoveItErrorCodes error_code; solution.resize(kinematics_solver_->getJointNames().size(), 0.0); const std::vector& fk_names = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); unsigned int success = 0; @@ -613,7 +613,7 @@ TEST_F(KinematicsTest, getIK) moveit_msgs::MoveItErrorCodes error_code; solution.resize(kinematics_solver_->getJointNames().size(), 0.0); const std::vector& fk_names = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); for (unsigned int i = 0; i < num_ik_tests_; ++i) @@ -642,7 +642,7 @@ TEST_F(KinematicsTest, getIKMultipleSolutions) kinematics::KinematicsResult result; const std::vector& fk_names = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); unsigned int success = 0; @@ -685,7 +685,7 @@ TEST_F(KinematicsTest, getNearestIKSolution) std::vector seed, fk_values, solution; moveit_msgs::MoveItErrorCodes error_code; const std::vector& fk_names = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); for (unsigned int i = 0; i < num_nearest_ik_tests_; ++i) diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h index 2dee290b9b..893d69ff62 100644 --- a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h @@ -52,7 +52,8 @@ class CHOMPPlanningContext : public planning_interface::PlanningContext void clear() override; bool terminate() override; - CHOMPPlanningContext(const std::string& name, const std::string& group, const robot_model::RobotModelConstPtr& model); + CHOMPPlanningContext(const std::string& name, const std::string& group, + const moveit::core::RobotModelConstPtr& model); ~CHOMPPlanningContext() override = default; diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp index 35ddbc8a63..eec0b815d2 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp @@ -41,7 +41,7 @@ namespace chomp_interface { CHOMPPlanningContext::CHOMPPlanningContext(const std::string& name, const std::string& group, - const robot_model::RobotModelConstPtr& model) + const moveit::core::RobotModelConstPtr& model) : planning_interface::PlanningContext(name, group), robot_model_(model) { chomp_interface_ = CHOMPInterfacePtr(new CHOMPInterface()); diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp index 6a8288b78e..e73789f733 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp @@ -50,7 +50,7 @@ class CHOMPPlannerManager : public planning_interface::PlannerManager { } - bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& /*ns*/) override + bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& /*ns*/) override { for (const std::string& group : model->getJointModelGroupNames()) { diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp index 1a46e85fb3..990c11e62b 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp @@ -56,8 +56,8 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s } // get the specified start state - robot_state::RobotState start_state = planning_scene->getCurrentState(); - robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); + moveit::core::RobotState start_state = planning_scene->getCurrentState(); + moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); if (!start_state.satisfiesBounds()) { @@ -85,7 +85,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s } const size_t goal_index = trajectory.getNumPoints() - 1; - robot_state::RobotState goal_state(start_state); + moveit::core::RobotState goal_state(start_state); for (const moveit_msgs::JointConstraint& joint_constraint : req.goal_constraints[0].joint_constraints) goal_state.setVariablePosition(joint_constraint.joint_name, joint_constraint.position); if (!goal_state.satisfiesBounds()) @@ -223,9 +223,9 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s for (size_t i = 0; i < trajectory.getNumPoints(); i++) { const Eigen::MatrixXd::RowXpr source = trajectory.getTrajectoryPoint(i); - auto state = std::make_shared(start_state); + auto state = std::make_shared(start_state); size_t joint_index = 0; - for (const robot_state::JointModel* jm : result->getGroup()->getActiveJointModels()) + for (const moveit::core::JointModel* jm : result->getGroup()->getActiveJointModels()) { assert(jm->getVariableCount() == 1); state->setVariablePosition(jm->getFirstVariableIndex(), source[joint_index++]); @@ -254,7 +254,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s // check that final state is within goal tolerances kinematic_constraints::JointConstraint jc(planning_scene->getRobotModel()); - const robot_state::RobotState& last_state = result->getLastWayPoint(); + const moveit::core::RobotState& last_state = result->getLastWayPoint(); for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints[0].joint_constraints) { if (!jc.configure(constraint) || !jc.decide(last_state).satisfied) diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp index e1f56aa0e7..bb2e0f012a 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp @@ -197,8 +197,8 @@ bool ChompTrajectory::fillInFromTrajectory(const robot_trajectory::RobotTrajecto const size_t max_output_index = getNumPoints() - 1; const size_t max_input_index = trajectory.getWayPointCount() - 1; - const robot_model::JointModelGroup* group = trajectory.getGroup(); - robot_state::RobotState interpolated(trajectory.getRobotModel()); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); + moveit::core::RobotState interpolated(trajectory.getRobotModel()); for (size_t i = 0; i <= max_output_index; i++) { double fraction = static_cast(i * max_input_index) / max_output_index; @@ -211,14 +211,14 @@ bool ChompTrajectory::fillInFromTrajectory(const robot_trajectory::RobotTrajecto return true; } -void ChompTrajectory::assignCHOMPTrajectoryPointFromRobotState(const robot_state::RobotState& source, +void ChompTrajectory::assignCHOMPTrajectoryPointFromRobotState(const moveit::core::RobotState& source, size_t chomp_trajectory_point_index, - const robot_model::JointModelGroup* group) + const moveit::core::JointModelGroup* group) { Eigen::MatrixXd::RowXpr target = getTrajectoryPoint(chomp_trajectory_point_index); assert(group->getActiveJointModels().size() == static_cast(target.cols())); size_t joint_index = 0; - for (const robot_state::JointModel* jm : group->getActiveJointModels()) + for (const moveit::core::JointModel* jm : group->getActiveJointModels()) { assert(jm->getVariableCount() == 1); target[joint_index++] = source.getVariablePosition(jm->getFirstVariableIndex()); diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h index d054ab8e15..3eff4f9787 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h @@ -57,16 +57,16 @@ class ConstrainedGoalSampler : public ompl::base::GoalLazySamples private: bool sampleUsingConstraintSampler(const ompl::base::GoalLazySamples* gls, ompl::base::State* new_goal); - bool stateValidityCallback(ompl::base::State* new_goal, robot_state::RobotState const* state, - const robot_model::JointModelGroup*, const double*, bool verbose = false) const; - bool checkStateValidity(ompl::base::State* new_goal, const robot_state::RobotState& state, + bool stateValidityCallback(ompl::base::State* new_goal, moveit::core::RobotState const* state, + const moveit::core::JointModelGroup*, const double*, bool verbose = false) const; + bool checkStateValidity(ompl::base::State* new_goal, const moveit::core::RobotState& state, bool verbose = false) const; const ModelBasedPlanningContext* planning_context_; kinematic_constraints::KinematicConstraintSetPtr kinematic_constraint_set_; constraint_samplers::ConstraintSamplerPtr constraint_sampler_; ompl::base::StateSamplerPtr default_sampler_; - robot_state::RobotState work_state_; + moveit::core::RobotState work_state_; unsigned int invalid_sampled_constraints_; bool warned_invalid_samples_; unsigned int verbose_display_; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h index cb72a22811..b2195591bb 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h @@ -71,7 +71,7 @@ class ConstrainedSampler : public ompl::base::StateSampler const ModelBasedPlanningContext* planning_context_; ompl::base::StateSamplerPtr default_; constraint_samplers::ConstraintSamplerPtr constraint_sampler_; - robot_state::RobotState work_state_; + moveit::core::RobotState work_state_; unsigned int constrained_success_; unsigned int constrained_failure_; double inv_dim_; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h index 61e9b0d15d..e0b76d6309 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h @@ -64,7 +64,7 @@ class ValidConstrainedSampler : public ompl::base::ValidStateSampler kinematic_constraints::KinematicConstraintSetPtr kinematic_constraint_set_; constraint_samplers::ConstraintSamplerPtr constraint_sampler_; ompl::base::StateSamplerPtr default_sampler_; - robot_state::RobotState work_state_; + moveit::core::RobotState work_state_; double inv_dim_; ompl::RNG rng_; }; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h index 92784cfe37..0e40f3da4d 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h @@ -64,7 +64,7 @@ class ProjectionEvaluatorLinkPose : public ompl::base::ProjectionEvaluator private: const ModelBasedPlanningContext* planning_context_; - const robot_model::LinkModel* link_; + const moveit::core::LinkModel* link_; TSStateStorage tss_; }; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h index a276ede06e..e896b23e3e 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h @@ -45,15 +45,15 @@ namespace ompl_interface class TSStateStorage { public: - TSStateStorage(const robot_model::RobotModelPtr& robot_model); - TSStateStorage(const robot_state::RobotState& start_state); + TSStateStorage(const moveit::core::RobotModelPtr& robot_model); + TSStateStorage(const moveit::core::RobotState& start_state); ~TSStateStorage(); - robot_state::RobotState* getStateStorage() const; + moveit::core::RobotState* getStateStorage() const; private: - robot_state::RobotState start_state_; - mutable std::map thread_states_; + moveit::core::RobotState start_state_; + mutable std::map thread_states_; mutable std::mutex lock_; }; } diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index 65acf98d8f..0ed6002e8e 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -102,17 +102,17 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext spec_.config_ = config; } - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return spec_.state_space_->getRobotModel(); } - const robot_model::JointModelGroup* getJointModelGroup() const + const moveit::core::JointModelGroup* getJointModelGroup() const { return spec_.state_space_->getJointModelGroup(); } - const robot_state::RobotState& getCompleteInitialRobotState() const + const moveit::core::RobotState& getCompleteInitialRobotState() const { return complete_initial_robot_state_; } @@ -234,7 +234,7 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext void setPlanningVolume(const moveit_msgs::WorkspaceParameters& wparams); - void setCompleteInitialState(const robot_state::RobotState& complete_initial_robot_state); + void setCompleteInitialState(const moveit::core::RobotState& complete_initial_robot_state); bool setGoalConstraints(const std::vector& goal_constraints, const moveit_msgs::Constraints& path_constraints, moveit_msgs::MoveItErrorCodes* error); @@ -368,7 +368,7 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext ModelBasedPlanningContextSpecification spec_; - robot_state::RobotState complete_initial_robot_state_; + moveit::core::RobotState complete_initial_robot_state_; /// the OMPL planning context; this contains the problem definition and the planner used og::SimpleSetupPtr ompl_simple_setup_; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h index e856b1af14..caf92fab42 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h @@ -56,13 +56,13 @@ class OMPLInterface public: /** \brief Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the specified * NodeHandle */ - OMPLInterface(const robot_model::RobotModelConstPtr& robot_model, const ros::NodeHandle& nh = ros::NodeHandle("~")); + OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const ros::NodeHandle& nh = ros::NodeHandle("~")); /** \brief Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the specified NodeHandle. However, planner configurations are used as specified in \e pconfig instead of reading them from the ROS parameter server */ - OMPLInterface(const robot_model::RobotModelConstPtr& robot_model, + OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const planning_interface::PlannerConfigurationMap& pconfig, const ros::NodeHandle& nh = ros::NodeHandle("~")); @@ -150,7 +150,7 @@ class OMPLInterface ros::NodeHandle nh_; /// The ROS node handle /** \brief The kinematic model for which motion plans are computed */ - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h index 246f8a48e1..10cac453eb 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h @@ -46,7 +46,7 @@ class JointModelStateSpaceFactory : public ModelBasedStateSpaceFactory JointModelStateSpaceFactory(); int canRepresentProblem(const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const robot_model::RobotModelConstPtr& robot_model) const override; + const moveit::core::RobotModelConstPtr& robot_model) const override; protected: ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h index dbad573239..b4543f047e 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h @@ -51,22 +51,22 @@ typedef std::functiongetJointModelGroup(group_name)) { if (!joint_model_group_) throw std::runtime_error("Group '" + group_name + "' was not found"); } - robot_model::RobotModelConstPtr robot_model_; - const robot_model::JointModelGroup* joint_model_group_; - robot_model::JointBoundsVector joint_bounds_; + moveit::core::RobotModelConstPtr robot_model_; + const moveit::core::JointModelGroup* joint_model_group_; + moveit::core::JointBoundsVector joint_bounds_; }; OMPL_CLASS_FORWARD(ModelBasedStateSpace); @@ -202,12 +202,12 @@ class ModelBasedStateSpace : public ompl::base::StateSpace virtual const std::string& getParameterizationType() const = 0; - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return spec_.robot_model_; } - const robot_model::JointModelGroup* getJointModelGroup() const + const moveit::core::JointModelGroup* getJointModelGroup() const { return spec_.joint_model_group_; } @@ -228,18 +228,18 @@ class ModelBasedStateSpace : public ompl::base::StateSpace /// Set the planning volume for the possible SE2 and/or SE3 components of the state space virtual void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ); - const robot_model::JointBoundsVector& getJointsBounds() const + const moveit::core::JointBoundsVector& getJointsBounds() const { return spec_.joint_bounds_; } /// Copy the data from an OMPL state to a set of joint states. // The joint states \b must be specified in the same order as the joint models in the constructor - virtual void copyToRobotState(robot_state::RobotState& rstate, const ompl::base::State* state) const; + virtual void copyToRobotState(moveit::core::RobotState& rstate, const ompl::base::State* state) const; /// Copy the data from a set of joint states to an OMPL state. // The joint states \b must be specified in the same order as the joint models in the constructor - virtual void copyToOMPLState(ompl::base::State* state, const robot_state::RobotState& rstate) const; + virtual void copyToOMPLState(ompl::base::State* state, const moveit::core::RobotState& rstate) const; /** * \brief Copy a single joint's values (which might have multiple variables) from a MoveIt robot_state to an OMPL @@ -251,7 +251,7 @@ class ModelBasedStateSpace : public ompl::base::StateSpace * cache this index) * e.g. ompl_state_joint_index = joint_model_group_->getVariableGroupIndex("virtual_joint"); */ - virtual void copyJointToOMPLState(ompl::base::State* state, const robot_state::RobotState& robot_state, + virtual void copyJointToOMPLState(ompl::base::State* state, const moveit::core::RobotState& robot_state, const moveit::core::JointModel* joint_model, int ompl_state_joint_index) const; double getTagSnapToSegment() const; @@ -259,8 +259,8 @@ class ModelBasedStateSpace : public ompl::base::StateSpace protected: ModelBasedStateSpaceSpecification spec_; - std::vector joint_bounds_storage_; - std::vector joint_model_vector_; + std::vector joint_bounds_storage_; + std::vector joint_model_vector_; unsigned int variable_count_; size_t state_values_size_; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h index 0bd363d9b2..2ed0f90eb4 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h @@ -67,7 +67,7 @@ class ModelBasedStateSpaceFactory request \e req for group \e group. The group \e group must always be specified and takes precedence over \e req.group_name, which may be different */ virtual int canRepresentProblem(const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const robot_model::RobotModelConstPtr& robot_model) const = 0; + const moveit::core::RobotModelConstPtr& robot_model) const = 0; protected: virtual ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const = 0; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h index b38d5f782b..5bb996aebd 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h @@ -107,7 +107,7 @@ class PoseModelStateSpace : public ModelBasedStateSpace bool computeStateK(ompl::base::State* state) const; void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ) override; - void copyToOMPLState(ompl::base::State* state, const robot_state::RobotState& rstate) const override; + void copyToOMPLState(ompl::base::State* state, const moveit::core::RobotState& rstate) const override; void sanityChecks() const override; const std::string& getParameterizationType() const override @@ -118,8 +118,8 @@ class PoseModelStateSpace : public ModelBasedStateSpace private: struct PoseComponent { - PoseComponent(const robot_model::JointModelGroup* subgroup, - const robot_model::JointModelGroup::KinematicsSolver& k); + PoseComponent(const moveit::core::JointModelGroup* subgroup, + const moveit::core::JointModelGroup::KinematicsSolver& k); bool computeStateFK(StateType* full_state, unsigned int idx) const; bool computeStateIK(StateType* full_state, unsigned int idx) const; @@ -129,7 +129,7 @@ class PoseModelStateSpace : public ModelBasedStateSpace return subgroup_->getName() < o.subgroup_->getName(); } - const robot_model::JointModelGroup* subgroup_; + const moveit::core::JointModelGroup* subgroup_; kinematics::KinematicsBasePtr kinematics_solver_; std::vector bijection_; ompl::base::StateSpacePtr state_space_; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h index 11f45ad066..c5b1676ceb 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h @@ -46,7 +46,7 @@ class PoseModelStateSpaceFactory : public ModelBasedStateSpaceFactory PoseModelStateSpaceFactory(); int canRepresentProblem(const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const robot_model::RobotModelConstPtr& robot_model) const override; + const moveit::core::RobotModelConstPtr& robot_model) const override; protected: ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index 8bff59c4dd..14e1eeb0ea 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -80,7 +80,7 @@ class MultiQueryPlannerAllocator class PlanningContextManager { public: - PlanningContextManager(robot_model::RobotModelConstPtr robot_model, + PlanningContextManager(moveit::core::RobotModelConstPtr robot_model, constraint_samplers::ConstraintSamplerManagerPtr csm); ~PlanningContextManager(); @@ -165,7 +165,7 @@ class PlanningContextManager minimum_waypoint_count_ = mwc; } - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -219,7 +219,7 @@ class PlanningContextManager const moveit_msgs::MotionPlanRequest& req) const; /** \brief The kinematic model for which motion plans are computed */ - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp index 04a7fc83e3..ce417e99c2 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp @@ -63,7 +63,7 @@ ompl_interface::ConstrainedGoalSampler::ConstrainedGoalSampler(const ModelBasedP } bool ompl_interface::ConstrainedGoalSampler::checkStateValidity(ob::State* new_goal, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, bool verbose) const { planning_context_->getOMPLStateSpace()->copyToOMPLState(new_goal, state); @@ -71,12 +71,12 @@ bool ompl_interface::ConstrainedGoalSampler::checkStateValidity(ob::State* new_g } bool ompl_interface::ConstrainedGoalSampler::stateValidityCallback(ob::State* new_goal, - robot_state::RobotState const* state, - const robot_model::JointModelGroup* jmg, + moveit::core::RobotState const* state, + const moveit::core::JointModelGroup* jmg, const double* jpos, bool verbose) const { // we copy the state to not change the seed state - robot_state::RobotState solution_state(*state); + moveit::core::RobotState solution_state(*state); solution_state.setJointGroupPositions(jmg, jpos); solution_state.update(); return checkStateValidity(new_goal, solution_state, verbose); @@ -116,7 +116,7 @@ bool ompl_interface::ConstrainedGoalSampler::sampleUsingConstraintSampler(const if (constraint_sampler_) { // makes the constraint sampler also perform a validity callback - robot_state::GroupStateValidityCallbackFn gsvcf = + moveit::core::GroupStateValidityCallbackFn gsvcf = std::bind(&ompl_interface::ConstrainedGoalSampler::stateValidityCallback, this, new_goal, std::placeholders::_1, // pointer to state std::placeholders::_2, // const* joint model group diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index 8e99b06dac..96c6f7c92a 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -230,7 +230,7 @@ ompl_interface::ConstraintApproximation::getStateSamplerAllocator(const moveit_m void ompl_interface::ConstraintApproximation::visualizeDistribution(const std::string &link_name, unsigned int count, visualization_msgs::MarkerArray &arr) const { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); ompl::RNG rng; @@ -459,10 +459,10 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra // construct a sampler for the sampling constraints kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel()); - robot_state::Transforms no_transforms(pcontext->getRobotModel()->getModelFrame()); + moveit::core::Transforms no_transforms(pcontext->getRobotModel()->getModelFrame()); kset.add(constr_hard, no_transforms); - const robot_state::RobotState& default_state = pcontext->getCompleteInitialRobotState(); + const moveit::core::RobotState& default_state = pcontext->getCompleteInitialRobotState(); unsigned int attempts = 0; @@ -473,7 +473,7 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra // construct the constrained states - robot_state::RobotState robot_state(default_state); + moveit::core::RobotState robot_state(default_state); const constraint_samplers::ConstraintSamplerManagerPtr& csmng = pcontext->getConstraintSamplerManager(); ConstrainedSampler* constrained_sampler = nullptr; if (csmng) diff --git a/moveit_planners/ompl/ompl_interface/src/detail/projection_evaluators.cpp b/moveit_planners/ompl/ompl_interface/src/detail/projection_evaluators.cpp index 53fd8a6498..cb938df902 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/projection_evaluators.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/projection_evaluators.cpp @@ -65,7 +65,7 @@ void ompl_interface::ProjectionEvaluatorLinkPose::defaultCellSizes() void ompl_interface::ProjectionEvaluatorLinkPose::project(const ompl::base::State* state, OMPLProjection projection) const { - robot_state::RobotState* s = tss_.getStateStorage(); + moveit::core::RobotState* s = tss_.getStateStorage(); planning_context_->getOMPLStateSpace()->copyToRobotState(*s, state); const Eigen::Vector3d& o = s->getGlobalLinkTransform(link_).translation(); diff --git a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp index 5deb25fce8..6205ce4dfd 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp @@ -86,7 +86,7 @@ double ompl_interface::StateValidityChecker::cost(const ompl::base::State* state { double cost = 0.0; - robot_state::RobotState* robot_state = tss_.getStateStorage(); + moveit::core::RobotState* robot_state = tss_.getStateStorage(); planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); // Calculates cost from a summation of distance to obstacles times the size of the obstacle @@ -101,7 +101,7 @@ double ompl_interface::StateValidityChecker::cost(const ompl::base::State* state double ompl_interface::StateValidityChecker::clearance(const ompl::base::State* state) const { - robot_state::RobotState* robot_state = tss_.getStateStorage(); + moveit::core::RobotState* robot_state = tss_.getStateStorage(); planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); collision_detection::CollisionResult res; @@ -120,7 +120,7 @@ bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base: } // convert ompl state to MoveIt robot state - robot_state::RobotState* robot_state = tss_.getStateStorage(); + moveit::core::RobotState* robot_state = tss_.getStateStorage(); planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); // check path constraints @@ -149,7 +149,7 @@ bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base: return false; } - robot_state::RobotState* robot_state = tss_.getStateStorage(); + moveit::core::RobotState* robot_state = tss_.getStateStorage(); planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); // check path constraints @@ -192,7 +192,7 @@ bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::St return false; } - robot_state::RobotState* robot_state = tss_.getStateStorage(); + moveit::core::RobotState* robot_state = tss_.getStateStorage(); planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); // check path constraints @@ -243,7 +243,7 @@ bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::St return false; } - robot_state::RobotState* robot_state = tss_.getStateStorage(); + moveit::core::RobotState* robot_state = tss_.getStateStorage(); planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); // check path constraints diff --git a/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp b/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp index d829852774..e0cfa8245e 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp @@ -36,13 +36,13 @@ #include -ompl_interface::TSStateStorage::TSStateStorage(const robot_model::RobotModelPtr& robot_model) +ompl_interface::TSStateStorage::TSStateStorage(const moveit::core::RobotModelPtr& robot_model) : start_state_(robot_model) { start_state_.setToDefaultValues(); } -ompl_interface::TSStateStorage::TSStateStorage(const robot_state::RobotState& start_state) : start_state_(start_state) +ompl_interface::TSStateStorage::TSStateStorage(const moveit::core::RobotState& start_state) : start_state_(start_state) { } @@ -52,15 +52,15 @@ ompl_interface::TSStateStorage::~TSStateStorage() delete thread_state.second; } -robot_state::RobotState* ompl_interface::TSStateStorage::getStateStorage() const +moveit::core::RobotState* ompl_interface::TSStateStorage::getStateStorage() const { - robot_state::RobotState* st = nullptr; + moveit::core::RobotState* st = nullptr; std::unique_lock slock(lock_); /// \todo use Thread Local Storage? - std::map::const_iterator it = + std::map::const_iterator it = thread_states_.find(std::this_thread::get_id()); if (it == thread_states_.end()) { - st = new robot_state::RobotState(start_state_); + st = new moveit::core::RobotState(start_state_); thread_states_[std::this_thread::get_id()] = st; } else diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index bd64bfadcd..5cee993b16 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -420,7 +420,7 @@ void ompl_interface::ModelBasedPlanningContext::interpolateSolution() void ompl_interface::ModelBasedPlanningContext::convertPath(const ompl::geometric::PathGeometric& pg, robot_trajectory::RobotTrajectory& traj) const { - robot_state::RobotState ks = complete_initial_robot_state_; + moveit::core::RobotState ks = complete_initial_robot_state_; for (std::size_t i = 0; i < pg.getStateCount(); ++i) { spec_.state_space_->copyToRobotState(ks, pg.getState(i)); @@ -528,7 +528,7 @@ ompl::base::PlannerTerminationCondition ompl_interface::ModelBasedPlanningContex } void ompl_interface::ModelBasedPlanningContext::setCompleteInitialState( - const robot_state::RobotState& complete_initial_robot_state) + const moveit::core::RobotState& complete_initial_robot_state) { complete_initial_robot_state_ = complete_initial_robot_state; complete_initial_robot_state_.update(); diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index dc4ffc5371..f01b70e8d7 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -42,7 +42,7 @@ #include #include -ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& robot_model, +ompl_interface::OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const ros::NodeHandle& nh) : nh_(nh) , robot_model_(robot_model) @@ -56,7 +56,7 @@ ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstP loadConstraintSamplers(); } -ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& robot_model, +ompl_interface::OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const planning_interface::PlannerConfigurationMap& pconfig, const ros::NodeHandle& nh) : nh_(nh) @@ -78,7 +78,7 @@ void ompl_interface::OMPLInterface::setPlannerConfigurations(const planning_inte planning_interface::PlannerConfigurationMap pconfig2 = pconfig; // construct default configurations for planning groups that don't have configs already passed in - for (const robot_model::JointModelGroup* group : robot_model_->getJointModelGroups()) + for (const moveit::core::JointModelGroup* group : robot_model_->getJointModelGroups()) { if (pconfig.find(group->getName()) == pconfig.end()) { diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index 2c8c67fe40..8d284feb38 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -102,7 +102,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager ompl::msg::useOutputHandler(output_handler_.get()); } - bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns) override + bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) override { if (!ns.empty()) nh_ = ros::NodeHandle(ns); @@ -177,7 +177,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager ROS_INFO_STREAM("Displaying states for context " << pc->getName()); const og::SimpleSetup &ss = pc->getOMPLSimpleSetup(); ob::ValidStateSamplerPtr vss = ss.getSpaceInformation()->allocValidStateSampler(); - robot_state::RobotState robot_state = pc->getPlanningScene()->getCurrentState(); + moveit::core::RobotState robot_state = pc->getPlanningScene()->getCurrentState(); ob::ScopedState<> rstate1(ss.getStateSpace()); ob::ScopedState<> rstate2(ss.getStateSpace()); ros::WallDuration wait(2); @@ -191,7 +191,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager pc->getOMPLStateSpace()->copyToRobotState(robot_state, rstate1.get()); robot_state.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms(); moveit_msgs::DisplayRobotState state_msg; - robot_state::robotStateToRobotStateMsg(robot_state, state_msg.state); + moveit::core::robotStateToRobotStateMsg(robot_state, state_msg.state); pub_valid_states_.publish(state_msg); n = (n + 1) % 2; if (n == 0) @@ -208,7 +208,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager msg.model_id = pc->getRobotModel()->getName(); msg.trajectory.resize(1); traj.getRobotTrajectoryMsg(msg.trajectory[0]); - robot_state::robotStateToRobotStateMsg(traj.getFirstWayPoint(), msg.trajectory_start); + moveit::core::robotStateToRobotStateMsg(traj.getFirstWayPoint(), msg.trajectory_start); pub_valid_traj_.publish(msg); } rstate2 = rstate1; @@ -224,7 +224,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager { ompl::base::PlannerData pd(pc->getOMPLSimpleSetup()->getSpaceInformation()); pc->getOMPLSimpleSetup()->getPlannerData(pd); - robot_state::RobotState robot_state = planning_scene->getCurrentState(); + moveit::core::RobotState robot_state = planning_scene->getCurrentState(); visualization_msgs::MarkerArray arr; std_msgs::ColorRGBA color; color.r = 1.0f; diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp index 222909cc49..2c5952640c 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp @@ -44,7 +44,7 @@ ompl_interface::JointModelStateSpaceFactory::JointModelStateSpaceFactory() : Mod int ompl_interface::JointModelStateSpaceFactory::canRepresentProblem( const std::string& /*group*/, const moveit_msgs::MotionPlanRequest& /*req*/, - const robot_model::RobotModelConstPtr& /*robot_model*/) const + const moveit::core::RobotModelConstPtr& /*robot_model*/) const { return 100; } diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp index 847ffb9f12..5e359da1e4 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp @@ -140,7 +140,7 @@ void ompl_interface::ModelBasedStateSpace::deserialize(ompl::base::State* state, unsigned int ompl_interface::ModelBasedStateSpace::getDimension() const { unsigned int d = 0; - for (const robot_model::JointModel* i : joint_model_vector_) + for (const moveit::core::JointModel* i : joint_model_vector_) d += i->getStateSpaceDimension(); return d; } @@ -153,7 +153,7 @@ double ompl_interface::ModelBasedStateSpace::getMaximumExtent() const double ompl_interface::ModelBasedStateSpace::getMeasure() const { double m = 1.0; - for (const robot_model::JointModel::Bounds* bounds : spec_.joint_bounds_) + for (const moveit::core::JointModel::Bounds* bounds : spec_.joint_bounds_) { for (const moveit::core::VariableBounds& bound : *bounds) { @@ -227,14 +227,14 @@ void ompl_interface::ModelBasedStateSpace::setPlanningVolume(double minX, double double minZ, double maxZ) { for (std::size_t i = 0; i < joint_model_vector_.size(); ++i) - if (joint_model_vector_[i]->getType() == robot_model::JointModel::PLANAR) + if (joint_model_vector_[i]->getType() == moveit::core::JointModel::PLANAR) { joint_bounds_storage_[i][0].min_position_ = minX; joint_bounds_storage_[i][0].max_position_ = maxX; joint_bounds_storage_[i][1].min_position_ = minY; joint_bounds_storage_[i][1].max_position_ = maxY; } - else if (joint_model_vector_[i]->getType() == robot_model::JointModel::FLOATING) + else if (joint_model_vector_[i]->getType() == moveit::core::JointModel::FLOATING) { joint_bounds_storage_[i][0].min_position_ = minX; joint_bounds_storage_[i][0].max_position_ = maxX; @@ -250,8 +250,8 @@ ompl::base::StateSamplerPtr ompl_interface::ModelBasedStateSpace::allocDefaultSt class DefaultStateSampler : public ompl::base::StateSampler { public: - DefaultStateSampler(const ompl::base::StateSpace* space, const robot_model::JointModelGroup* group, - const robot_model::JointBoundsVector* joint_bounds) + DefaultStateSampler(const ompl::base::StateSpace* space, const moveit::core::JointModelGroup* group, + const moveit::core::JointBoundsVector* joint_bounds) : ompl::base::StateSampler(space), joint_model_group_(group), joint_bounds_(joint_bounds) { } @@ -276,8 +276,8 @@ ompl::base::StateSamplerPtr ompl_interface::ModelBasedStateSpace::allocDefaultSt protected: random_numbers::RandomNumberGenerator moveit_rng_; - const robot_model::JointModelGroup* joint_model_group_; - const robot_model::JointBoundsVector* joint_bounds_; + const moveit::core::JointModelGroup* joint_model_group_; + const moveit::core::JointBoundsVector* joint_bounds_; }; return ompl::base::StateSamplerPtr(static_cast( @@ -291,7 +291,7 @@ void ompl_interface::ModelBasedStateSpace::printSettings(std::ostream& out) cons void ompl_interface::ModelBasedStateSpace::printState(const ompl::base::State* state, std::ostream& out) const { - for (const robot_model::JointModel* j : joint_model_vector_) + for (const moveit::core::JointModel* j : joint_model_vector_) { out << j->getName() << " = "; const int idx = spec_.joint_model_group_->getVariableGroupIndex(j->getName()); @@ -315,7 +315,7 @@ void ompl_interface::ModelBasedStateSpace::printState(const ompl::base::State* s out << "Tag: " << state->as()->tag << std::endl; } -void ompl_interface::ModelBasedStateSpace::copyToRobotState(robot_state::RobotState& rstate, +void ompl_interface::ModelBasedStateSpace::copyToRobotState(moveit::core::RobotState& rstate, const ompl::base::State* state) const { rstate.setJointGroupPositions(spec_.joint_model_group_, state->as()->values); @@ -323,7 +323,7 @@ void ompl_interface::ModelBasedStateSpace::copyToRobotState(robot_state::RobotSt } void ompl_interface::ModelBasedStateSpace::copyToOMPLState(ompl::base::State* state, - const robot_state::RobotState& rstate) const + const moveit::core::RobotState& rstate) const { rstate.copyJointGroupPositions(spec_.joint_model_group_, state->as()->values); // clear any cached info (such as validity known or not) @@ -331,7 +331,7 @@ void ompl_interface::ModelBasedStateSpace::copyToOMPLState(ompl::base::State* st } void ompl_interface::ModelBasedStateSpace::copyJointToOMPLState(ompl::base::State* state, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const moveit::core::JointModel* joint_model, int ompl_state_joint_index) const { diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index 7433f35ec0..97b27e0884 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp @@ -51,7 +51,7 @@ ompl_interface::PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSp poses_.emplace_back(spec.joint_model_group_, spec.joint_model_group_->getGroupKinematics().first); else if (!spec.joint_model_group_->getGroupKinematics().second.empty()) { - const robot_model::JointModelGroup::KinematicsSolverMap& m = spec.joint_model_group_->getGroupKinematics().second; + const moveit::core::JointModelGroup::KinematicsSolverMap& m = spec.joint_model_group_->getGroupKinematics().second; for (const auto& it : m) poses_.emplace_back(it.first, it.second); } @@ -177,7 +177,7 @@ void ompl_interface::PoseModelStateSpace::setPlanningVolume(double minX, double } ompl_interface::PoseModelStateSpace::PoseComponent::PoseComponent( - const robot_model::JointModelGroup* subgroup, const robot_model::JointModelGroup::KinematicsSolver& k) + const moveit::core::JointModelGroup* subgroup, const moveit::core::JointModelGroup::KinematicsSolver& k) : subgroup_(subgroup), kinematics_solver_(k.allocator_(subgroup)), bijection_(k.bijection_) { state_space_.reset(new ompl::base::SE3StateSpace()); @@ -338,7 +338,7 @@ ompl::base::StateSamplerPtr ompl_interface::PoseModelStateSpace::allocDefaultSta } void ompl_interface::PoseModelStateSpace::copyToOMPLState(ompl::base::State* state, - const robot_state::RobotState& rstate) const + const moveit::core::RobotState& rstate) const { ModelBasedStateSpace::copyToOMPLState(state, rstate); state->as()->setJointsComputed(true); diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp index 93200757cd..5d912db8f2 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp @@ -44,13 +44,13 @@ ompl_interface::PoseModelStateSpaceFactory::PoseModelStateSpaceFactory() : Model int ompl_interface::PoseModelStateSpaceFactory::canRepresentProblem( const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const robot_model::RobotModelConstPtr& robot_model) const + const moveit::core::RobotModelConstPtr& robot_model) const { - const robot_model::JointModelGroup* jmg = robot_model->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); if (jmg) { - const std::pair& - slv = jmg->getGroupKinematics(); + const std::pair& slv = jmg->getGroupKinematics(); bool ik = false; // check that we have a direct means to compute IK if (slv.first) diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 89809dfcff..6e868a5b5e 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -223,7 +223,7 @@ MultiQueryPlannerAllocator::allocatePersistentPlannerclear(); - robot_state::RobotStatePtr start_state = planning_scene->getCurrentStateUpdated(req.start_state); + moveit::core::RobotStatePtr start_state = planning_scene->getCurrentStateUpdated(req.start_state); // Setup the context context->setPlanningScene(planning_scene); diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp index b704a83abd..90bbcc1d38 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp @@ -76,7 +76,7 @@ class LoadPlanningModelsPr2 : public testing::Test } protected: - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; urdf::ModelInterfaceSharedPtr urdf_model_; srdf::ModelSharedPtr srdf_model_; bool urdf_ok_; @@ -146,13 +146,13 @@ TEST_F(LoadPlanningModelsPr2, StateSpaceCopy) } EXPECT_TRUE(passed); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToRandomPositions(); EXPECT_TRUE(robot_state.distance(robot_state) < 1e-12); ompl::base::State* state = ss.allocState(); for (int i = 0; i < 10; ++i) { - robot_state::RobotState robot_state2(robot_state); + moveit::core::RobotState robot_state2(robot_state); EXPECT_TRUE(robot_state.distance(robot_state2) < 1e-12); ss.copyToOMPLState(state, robot_state); robot_state.setToRandomPositions(robot_state.getRobotModel()->getJointModelGroup(ss.getJointModelGroupName())); diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h index 12676f57e7..c3d6390164 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h +++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h @@ -13,7 +13,7 @@ class TrajOptPlanningContext : public planning_interface::PlanningContext { public: TrajOptPlanningContext(const std::string& name, const std::string& group, - const robot_model::RobotModelConstPtr& model); + const moveit::core::RobotModelConstPtr& model); ~TrajOptPlanningContext() override { } diff --git a/moveit_planners/trajopt/src/problem_description.cpp b/moveit_planners/trajopt/src/problem_description.cpp index 7cd678282c..59c5de2198 100644 --- a/moveit_planners/trajopt/src/problem_description.cpp +++ b/moveit_planners/trajopt/src/problem_description.cpp @@ -82,9 +82,9 @@ TrajOptProblem::TrajOptProblem(const ProblemInfo& problem_info) , planning_scene_(problem_info.planning_scene) , planning_group_(problem_info.planning_group_name) { - robot_model::RobotModelConstPtr robot_model = planning_scene_->getRobotModel(); - robot_state::RobotState current_state = planning_scene_->getCurrentState(); - const robot_state::JointModelGroup* joint_model_group = current_state.getJointModelGroup(planning_group_); + moveit::core::RobotModelConstPtr robot_model = planning_scene_->getRobotModel(); + moveit::core::RobotState current_state = planning_scene_->getCurrentState(); + const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup(planning_group_); moveit::core::JointBoundsVector bounds = joint_model_group->getActiveJointModelsBounds(); dof_ = joint_model_group->getActiveJointModelNames().size(); // or bounds.size(); @@ -181,10 +181,10 @@ TrajOptProblemPtr ConstructProblem(const ProblemInfo& pci) TrajOptProblemPtr prob(new TrajOptProblem(pci)); // Generate initial trajectory and check its size - robot_model::RobotModelConstPtr robot_model = pci.planning_scene->getRobotModel(); - robot_state::RobotState current_state = pci.planning_scene->getCurrentState(); + moveit::core::RobotModelConstPtr robot_model = pci.planning_scene->getRobotModel(); + moveit::core::RobotState current_state = pci.planning_scene->getCurrentState(); - const robot_state::JointModelGroup* joint_model_group = current_state.getJointModelGroup(pci.planning_group_name); + const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup(pci.planning_group_name); int n_dof = prob->GetNumDOF(); std::vector current_joint_values; diff --git a/moveit_planners/trajopt/src/trajopt_interface.cpp b/moveit_planners/trajopt/src/trajopt_interface.cpp index 4f76c1add2..7a5e95b71f 100644 --- a/moveit_planners/trajopt/src/trajopt_interface.cpp +++ b/moveit_planners/trajopt/src/trajopt_interface.cpp @@ -83,13 +83,13 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni ROS_INFO(" ======================================= Extract current state information"); ros::WallTime start_time = ros::WallTime::now(); - robot_model::RobotModelConstPtr robot_model = planning_scene->getRobotModel(); + moveit::core::RobotModelConstPtr robot_model = planning_scene->getRobotModel(); bool robot_model_ok = static_cast(robot_model); if (!robot_model_ok) ROS_ERROR_STREAM_NAMED(name_, "robot model is not loaded properly"); - robot_state::RobotStatePtr current_state(new robot_state::RobotState(robot_model)); + moveit::core::RobotStatePtr current_state(new moveit::core::RobotState(robot_model)); *current_state = planning_scene->getCurrentState(); - const robot_state::JointModelGroup* joint_model_group = current_state->getJointModelGroup(req.group_name); + const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(req.group_name); if (joint_model_group == nullptr) ROS_ERROR_STREAM_NAMED(name_, "joint model group is empty"); std::vector group_joint_names = joint_model_group->getActiveJointModelNames(); @@ -303,7 +303,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni ROS_INFO(" ======================================= check if final state is within goal tolerances"); kinematic_constraints::JointConstraint joint_cnt(planning_scene->getRobotModel()); - robot_state::RobotState last_state(*current_state); + moveit::core::RobotState last_state(*current_state); last_state.setJointGroupPositions(req.group_name, res.trajectory[0].joint_trajectory.points.back().positions); for (int jn = 0; jn < res.trajectory[0].joint_trajectory.points.back().positions.size(); ++jn) diff --git a/moveit_planners/trajopt/src/trajopt_planner_manager.cpp b/moveit_planners/trajopt/src/trajopt_planner_manager.cpp index 760c58f823..dda52869f5 100644 --- a/moveit_planners/trajopt/src/trajopt_planner_manager.cpp +++ b/moveit_planners/trajopt/src/trajopt_planner_manager.cpp @@ -53,7 +53,7 @@ class TrajOptPlannerManager : public planning_interface::PlannerManager { } - bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns) override + bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) override { ROS_INFO(" ======================================= initialize gets called"); diff --git a/moveit_planners/trajopt/src/trajopt_planning_context.cpp b/moveit_planners/trajopt/src/trajopt_planning_context.cpp index 34b1e4c307..28594f0465 100644 --- a/moveit_planners/trajopt/src/trajopt_planning_context.cpp +++ b/moveit_planners/trajopt/src/trajopt_planning_context.cpp @@ -13,7 +13,7 @@ namespace trajopt_interface { TrajOptPlanningContext::TrajOptPlanningContext(const std::string& context_name, const std::string& group_name, - const robot_model::RobotModelConstPtr& model) + const moveit::core::RobotModelConstPtr& model) : planning_interface::PlanningContext(context_name, group_name), robot_model_(model) { ROS_INFO(" ======================================= TrajOptPlanningContext is constructed"); @@ -32,7 +32,7 @@ bool TrajOptPlanningContext::solve(planning_interface::MotionPlanDetailedRespons robot_trajectory::RobotTrajectoryPtr(new robot_trajectory::RobotTrajectory(robot_model_, getGroupName())); moveit::core::RobotState start_state(robot_model_); - robot_state::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); + moveit::core::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); res.trajectory_[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]); res.description_.push_back("plan"); diff --git a/moveit_planners/trajopt/test/trajectory_test.cpp b/moveit_planners/trajopt/test/trajectory_test.cpp index 3494ba2d9e..49dd079500 100644 --- a/moveit_planners/trajopt/test/trajectory_test.cpp +++ b/moveit_planners/trajopt/test/trajectory_test.cpp @@ -56,7 +56,7 @@ class TrajectoryTest : public ::testing::Test } protected: - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; std::vector group_joint_names_; const std::string PLANNING_GROUP = "panda_arm"; const double GOAL_TOLERANCE = 0.1; @@ -91,10 +91,10 @@ TEST_F(TrajectoryTest, goalTolerance) const std::string NODE_NAME = "trajectory_test"; // Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group - robot_state::RobotStatePtr current_state(new robot_state::RobotState(robot_model_)); + moveit::core::RobotStatePtr current_state(new moveit::core::RobotState(robot_model_)); current_state->setToDefaultValues(); - const robot_state::JointModelGroup* joint_model_group = current_state->getJointModelGroup(PLANNING_GROUP); + const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(PLANNING_GROUP); EXPECT_NE(joint_model_group, nullptr); const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); EXPECT_EQ(joint_names.size(), 7); @@ -108,7 +108,7 @@ TEST_F(TrajectoryTest, goalTolerance) // Set start state // ====================================================================================== std::vector start_joint_values = { 0.4, 0.3, 0.5, -0.55, 0.88, 1.0, -0.075 }; - robot_state::RobotStatePtr start_state(new robot_state::RobotState(robot_model_)); + moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(robot_model_)); start_state->setJointGroupPositions(joint_model_group, start_joint_values); start_state->update(); @@ -119,7 +119,7 @@ TEST_F(TrajectoryTest, goalTolerance) // Set the goal state and joints tolerance // ======================================================================================== - robot_state::RobotStatePtr goal_state(new robot_state::RobotState(robot_model_)); + moveit::core::RobotStatePtr goal_state(new moveit::core::RobotState(robot_model_)); std::vector goal_joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1 }; goal_state->setJointGroupPositions(joint_model_group, goal_joint_values); goal_state->update(); diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp index 64c28f82cc..0e6f5cae64 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp @@ -141,7 +141,7 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont } robot_model_loader::RobotModelLoader robot_model_loader(ROBOT_DESCRIPTION); - const robot_model::RobotModelPtr& robot_model = robot_model_loader.getModel(); + const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel(); moveit::core::RobotState robot_state(robot_model); typedef std::map JointPoseMap; JointPoseMap joints; diff --git a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp index b9ca44eb74..1a412ae7b8 100644 --- a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp +++ b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp @@ -883,8 +883,8 @@ void moveit_benchmarks::BenchmarkExecution::collectMetrics(RunData& rundata, namespace { -bool isIKSolutionCollisionFree(const planning_scene::PlanningScene* scene, robot_state::RobotState* state, - const robot_model::JointModelGroup* group, const double* ik_solution, bool* reachable) +bool isIKSolutionCollisionFree(const planning_scene::PlanningScene* scene, moveit::core::RobotState* state, + const moveit::core::JointModelGroup* group, const double* ik_solution, bool* reachable) { state->setJointGroupPositions(group, ik_solution); state->update(); @@ -1270,8 +1270,8 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR ik_pose.orientation.z = req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation.z; ik_pose.orientation.w = req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation.w; - robot_state::RobotState robot_state(planning_scene_->getCurrentState()); - robot_state::robotStateMsgToRobotState(req.motion_plan_request.start_state, robot_state); + moveit::core::RobotState robot_state(planning_scene_->getCurrentState()); + moveit::core::robotStateMsgToRobotState(req.motion_plan_request.start_state, robot_state); // Compute IK ROS_INFO_STREAM("Processing goal " << req.motion_plan_request.goal_constraints[0].name << " ..."); @@ -1358,8 +1358,8 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR ik_pose.orientation.w = req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation.w; - robot_state::RobotState robot_state(planning_scene_->getCurrentState()); - robot_state::robotStateMsgToRobotState(req.motion_plan_request.start_state, robot_state); + moveit::core::RobotState robot_state(planning_scene_->getCurrentState()); + moveit::core::robotStateMsgToRobotState(req.motion_plan_request.start_state, robot_state); // Compute IK ROS_INFO_STREAM("Processing trajectory waypoint " diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h index a8415864cf..e4df2f2c01 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h @@ -63,9 +63,9 @@ struct ManipulationPlanSharedData { } - const robot_model::JointModelGroup* planning_group_; - const robot_model::JointModelGroup* end_effector_group_; - const robot_model::LinkModel* ik_link_; + const moveit::core::JointModelGroup* planning_group_; + const moveit::core::JointModelGroup* end_effector_group_; + const moveit::core::LinkModel* ik_link_; unsigned int max_goal_sampling_attempts_; @@ -125,9 +125,9 @@ struct ManipulationPlan // Allows for the sampling of a kineamtic state for a particular group of a robot constraint_samplers::ConstraintSamplerPtr goal_sampler_; - std::vector possible_goal_states_; + std::vector possible_goal_states_; - robot_state::RobotStatePtr approach_state_; + moveit::core::RobotStatePtr approach_state_; // The sequence of trajectories produced for execution std::vector trajectories_; diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h index 722dd3f79c..1448112f13 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h @@ -131,7 +131,7 @@ class PickPlace : private boost::noncopyable, public std::enable_shared_from_thi return planning_pipeline_; } - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return planning_pipeline_->getRobotModel(); } diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h index 802c5dafe0..09125623fe 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h @@ -52,7 +52,7 @@ class ReachableAndValidPoseFilter : public ManipulationStage bool evaluate(const ManipulationPlanPtr& plan) const override; private: - bool isEndEffectorFree(const ManipulationPlanPtr& plan, robot_state::RobotState& token_state) const; + bool isEndEffectorFree(const ManipulationPlanPtr& plan, moveit::core::RobotState& token_state) const; planning_scene::PlanningSceneConstPtr planning_scene_; collision_detection::AllowedCollisionMatrixConstPtr collision_matrix_; diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp index df25737485..c05e817149 100644 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp @@ -58,8 +58,8 @@ namespace { bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene, const collision_detection::AllowedCollisionMatrix* collision_matrix, bool verbose, - const trajectory_msgs::JointTrajectory* grasp_posture, robot_state::RobotState* state, - const robot_state::JointModelGroup* group, const double* joint_group_variable_values) + const trajectory_msgs::JointTrajectory* grasp_posture, moveit::core::RobotState* state, + const moveit::core::JointModelGroup* group, const double* joint_group_variable_values) { state->setJointGroupPositions(group, joint_group_variable_values); @@ -90,11 +90,11 @@ bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene, return planning_scene->isStateFeasible(*state); } -bool samplePossibleGoalStates(const ManipulationPlanPtr& plan, const robot_state::RobotState& reference_state, +bool samplePossibleGoalStates(const ManipulationPlanPtr& plan, const moveit::core::RobotState& reference_state, double min_distance, unsigned int attempts) { // initialize with scene state - robot_state::RobotStatePtr token_state(new robot_state::RobotState(reference_state)); + moveit::core::RobotStatePtr token_state(new moveit::core::RobotState(reference_state)); for (unsigned int j = 0; j < attempts; ++j) { double min_d = std::numeric_limits::infinity(); @@ -157,8 +157,8 @@ void addGripperTrajectory(const ManipulationPlanPtr& plan, // Check if a "closed" end effector configuration was specified if (!plan->retreat_posture_.joint_names.empty()) { - robot_state::RobotStatePtr ee_closed_state( - new robot_state::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint())); + moveit::core::RobotStatePtr ee_closed_state( + new moveit::core::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint())); robot_trajectory::RobotTrajectoryPtr ee_closed_traj(new robot_trajectory::RobotTrajectory( ee_closed_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName())); @@ -195,7 +195,7 @@ void addGripperTrajectory(const ManipulationPlanPtr& plan, bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const { - const robot_model::JointModelGroup* jmg = plan->shared_data_->planning_group_; + const moveit::core::JointModelGroup* jmg = plan->shared_data_->planning_group_; // compute what the maximum distance reported between any two states in the planning group could be, and keep 1% of // that; // this is the minimum distance between sampled goal states @@ -208,10 +208,10 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // if translation vectors are specified in the frame of the ik link name, then we assume the frame is local; // otherwise, the frame is global - bool approach_direction_is_global_frame = !robot_state::Transforms::sameFrame( + bool approach_direction_is_global_frame = !moveit::core::Transforms::sameFrame( plan->approach_.direction.header.frame_id, plan->shared_data_->ik_link_->getName()); - bool retreat_direction_is_global_frame = !robot_state::Transforms::sameFrame(plan->retreat_.direction.header.frame_id, - plan->shared_data_->ik_link_->getName()); + bool retreat_direction_is_global_frame = !moveit::core::Transforms::sameFrame( + plan->retreat_.direction.header.frame_id, plan->shared_data_->ik_link_->getName()); // transform the input vectors in accordance to frame specified in the header; if (approach_direction_is_global_frame) @@ -222,7 +222,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).rotation() * retreat_direction; // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping - robot_state::GroupStateValidityCallbackFn approach_valid_callback = + moveit::core::GroupStateValidityCallbackFn approach_valid_callback = boost::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, &plan->approach_posture_, _1, _2, _3); plan->goal_sampler_->setVerbose(verbose_); @@ -236,8 +236,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const if (plan->shared_data_->minimize_object_distance_) { static const double MAX_CLOSE_UP_DIST = 1.0; - robot_state::RobotStatePtr close_up_state(new robot_state::RobotState(*plan->possible_goal_states_[i])); - std::vector close_up_states; + moveit::core::RobotStatePtr close_up_state(new moveit::core::RobotState(*plan->possible_goal_states_[i])); + std::vector close_up_states; double d_close_up = moveit::core::CartesianInterpolator::computeCartesianPath( close_up_state.get(), plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_, approach_direction, approach_direction_is_global_frame, MAX_CLOSE_UP_DIST, @@ -248,9 +248,9 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const } // try to compute a straight line path that arrives at the goal using the specified approach direction - robot_state::RobotStatePtr first_approach_state(new robot_state::RobotState(*plan->possible_goal_states_[i])); + moveit::core::RobotStatePtr first_approach_state(new moveit::core::RobotState(*plan->possible_goal_states_[i])); - std::vector approach_states; + std::vector approach_states; double d_approach = moveit::core::CartesianInterpolator::computeCartesianPath( first_approach_state.get(), plan->shared_data_->planning_group_, approach_states, plan->shared_data_->ik_link_, -approach_direction, approach_direction_is_global_frame, @@ -273,14 +273,14 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // state validity checking during the retreat after the grasp must ensure the gripper posture is that of the // actual grasp - robot_state::GroupStateValidityCallbackFn retreat_valid_callback = + moveit::core::GroupStateValidityCallbackFn retreat_valid_callback = boost::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_, &plan->retreat_posture_, _1, _2, _3); // try to compute a straight line path that moves from the goal in a desired direction - robot_state::RobotStatePtr last_retreat_state( - new robot_state::RobotState(planning_scene_after_approach->getCurrentState())); - std::vector retreat_states; + moveit::core::RobotStatePtr last_retreat_state( + new moveit::core::RobotState(planning_scene_after_approach->getCurrentState())); + std::vector retreat_states; double d_retreat = moveit::core::CartesianInterpolator::computeCartesianPath( last_retreat_state.get(), plan->shared_data_->planning_group_, retreat_states, plan->shared_data_->ik_link_, retreat_direction, retreat_direction_is_global_frame, diff --git a/moveit_ros/manipulation/pick_place/src/pick.cpp b/moveit_ros/manipulation/pick_place/src/pick.cpp index f2e868ae79..def95e4c2d 100644 --- a/moveit_ros/manipulation/pick_place/src/pick.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick.cpp @@ -73,7 +73,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, std::string end_effector = goal.end_effector; if (end_effector.empty() && !planning_group.empty()) { - const robot_model::JointModelGroup* jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group); + const moveit::core::JointModelGroup* jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group); if (!jmg) { error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; @@ -91,7 +91,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, } else if (!end_effector.empty() && planning_group.empty()) { - const robot_model::JointModelGroup* jmg = planning_scene->getRobotModel()->getEndEffector(end_effector); + const moveit::core::JointModelGroup* jmg = planning_scene->getRobotModel()->getEndEffector(end_effector); if (!jmg) { error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; @@ -109,7 +109,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, ROS_INFO_STREAM_NAMED("manipulation", "Assuming the planning group for end effector '" << end_effector << "' is '" << planning_group << "'"); } - const robot_model::JointModelGroup* eef = + const moveit::core::JointModelGroup* eef = end_effector.empty() ? nullptr : planning_scene->getRobotModel()->getEndEffector(end_effector); if (!eef) { diff --git a/moveit_ros/manipulation/pick_place/src/pick_place.cpp b/moveit_ros/manipulation/pick_place/src/pick_place.cpp index 0c7ceb41d7..8571f24d47 100644 --- a/moveit_ros/manipulation/pick_place/src/pick_place.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick_place.cpp @@ -125,7 +125,7 @@ void PickPlace::visualizePlan(const ManipulationPlanPtr& plan) const continue; if (first) { - robot_state::robotStateToRobotStateMsg(traj.trajectory_->getFirstWayPoint(), dtraj.trajectory_start); + moveit::core::robotStateToRobotStateMsg(traj.trajectory_->getFirstWayPoint(), dtraj.trajectory_start); first = false; } dtraj.trajectory.resize(dtraj.trajectory.size() + 1); @@ -178,14 +178,14 @@ void PickPlace::visualizeGrasps(const std::vector& plans) c if (plans.empty()) return; - robot_state::RobotState state(getRobotModel()); + moveit::core::RobotState state(getRobotModel()); state.setToDefaultValues(); static std::vector colors(setupDefaultGraspColors()); visualization_msgs::MarkerArray ma; for (const ManipulationPlanPtr& plan : plans) { - const robot_model::JointModelGroup* jmg = plan->shared_data_->end_effector_group_; + const moveit::core::JointModelGroup* jmg = plan->shared_data_->end_effector_group_; if (jmg) { unsigned int type = std::min(plan->processing_stage_, colors.size() - 1); diff --git a/moveit_ros/manipulation/pick_place/src/place.cpp b/moveit_ros/manipulation/pick_place/src/place.cpp index ae7e1f04cf..2bf053a06c 100644 --- a/moveit_ros/manipulation/pick_place/src/place.cpp +++ b/moveit_ros/manipulation/pick_place/src/place.cpp @@ -52,7 +52,7 @@ PlacePlan::PlacePlan(const PickPlaceConstPtr& pick_place) : PickPlacePlanBase(pi namespace { bool transformToEndEffectorGoal(const geometry_msgs::PoseStamped& goal_pose, - const robot_state::AttachedBody* attached_body, geometry_msgs::PoseStamped& place_pose) + const moveit::core::AttachedBody* attached_body, geometry_msgs::PoseStamped& place_pose) { const EigenSTL::vector_Isometry3d& fixed_transforms = attached_body->getFixedTransforms(); if (fixed_transforms.empty()) @@ -72,8 +72,8 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene double timeout = goal.allowed_planning_time; ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout); std::string attached_object_name = goal.attached_object_name; - const robot_model::JointModelGroup* jmg = nullptr; - const robot_model::JointModelGroup* eef = nullptr; + const moveit::core::JointModelGroup* jmg = nullptr; + const moveit::core::JointModelGroup* eef = nullptr; // if the group specified is actually an end-effector, we use it as such if (planning_scene->getRobotModel()->hasEndEffector(goal.group_name)) @@ -113,8 +113,8 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene // check to see if there is an end effector that has attached objects associaded, so we can complete the place for (const std::string& eef_name : eef_names) { - std::vector attached_bodies; - const robot_model::JointModelGroup* eg = planning_scene->getRobotModel()->getEndEffector(eef_name); + std::vector attached_bodies; + const moveit::core::JointModelGroup* eg = planning_scene->getRobotModel()->getEndEffector(eef_name); if (eg) { // see if there are objects attached to links in the eef @@ -122,11 +122,11 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene // is is often possible that the objects are attached to the same link that the eef itself is attached, // so we check for attached bodies there as well - const robot_model::LinkModel* attached_link_model = + const moveit::core::LinkModel* attached_link_model = planning_scene->getRobotModel()->getLinkModel(eg->getEndEffectorParentGroup().second); if (attached_link_model) { - std::vector attached_bodies2; + std::vector attached_bodies2; planning_scene->getCurrentState().getAttachedBodies(attached_bodies2, attached_link_model); attached_bodies.insert(attached_bodies.end(), attached_bodies2.begin(), attached_bodies2.end()); } @@ -172,14 +172,15 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene // if we know the attached object, but not the eef, we can try to identify that if (!attached_object_name.empty() && !eef) { - const robot_state::AttachedBody* attached_body = + const moveit::core::AttachedBody* attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name); if (attached_body) { // get the robot model link this attached body is associated to - const robot_model::LinkModel* link = attached_body->getAttachedLink(); + const moveit::core::LinkModel* link = attached_body->getAttachedLink(); // check to see if there is a unique end effector containing the link - const std::vector& eefs = planning_scene->getRobotModel()->getEndEffectors(); + const std::vector& eefs = + planning_scene->getRobotModel()->getEndEffectors(); for (const moveit::core::JointModelGroup* end_effector : eefs) if (end_effector->hasLinkModel(link->getName())) { @@ -224,7 +225,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene { // in the first try, look for objects attached to the eef, if the eef is known; // otherwise, look for attached bodies in the planning group itself - std::vector attached_bodies; + std::vector attached_bodies; planning_scene->getCurrentState().getAttachedBodies(attached_bodies, loop_count == 0 ? eef : jmg); loop_count++; @@ -240,7 +241,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene attached_object_name = attached_bodies[0]->getName(); } - const robot_state::AttachedBody* attached_body = + const moveit::core::AttachedBody* attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name); if (!attached_body) { diff --git a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp b/moveit_ros/manipulation/pick_place/src/plan_stage.cpp index 1d81f2e779..3646c94a0f 100644 --- a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/plan_stage.cpp @@ -77,7 +77,8 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const // We have a valid motion plan, now apply pre-approach end effector posture (open gripper) if applicable if (!plan->approach_posture_.joint_names.empty()) { - robot_state::RobotStatePtr pre_approach_state(new robot_state::RobotState(res.trajectory_->getLastWayPoint())); + moveit::core::RobotStatePtr pre_approach_state( + new moveit::core::RobotState(res.trajectory_->getLastWayPoint())); robot_trajectory::RobotTrajectoryPtr pre_approach_traj(new robot_trajectory::RobotTrajectory( pre_approach_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName())); pre_approach_traj->setRobotTrajectoryMsg(*pre_approach_state, plan->approach_posture_); diff --git a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp index 36221b884e..986c05602e 100644 --- a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp +++ b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp @@ -55,8 +55,8 @@ namespace { bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene, const collision_detection::AllowedCollisionMatrix* collision_matrix, bool verbose, - const pick_place::ManipulationPlan* manipulation_plan, robot_state::RobotState* state, - const robot_model::JointModelGroup* group, const double* joint_group_variable_values) + const pick_place::ManipulationPlan* manipulation_plan, moveit::core::RobotState* state, + const moveit::core::JointModelGroup* group, const double* joint_group_variable_values) { collision_detection::CollisionRequest req; req.verbose = verbose; @@ -90,7 +90,7 @@ bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene, } // namespace bool pick_place::ReachableAndValidPoseFilter::isEndEffectorFree(const ManipulationPlanPtr& plan, - robot_state::RobotState& token_state) const + moveit::core::RobotState& token_state) const { tf2::fromMsg(plan->goal_pose_.pose, plan->transformed_goal_pose_); plan->transformed_goal_pose_ = @@ -107,14 +107,14 @@ bool pick_place::ReachableAndValidPoseFilter::isEndEffectorFree(const Manipulati bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr& plan) const { // initialize with scene state - robot_state::RobotStatePtr token_state(new robot_state::RobotState(planning_scene_->getCurrentState())); + moveit::core::RobotStatePtr token_state(new moveit::core::RobotState(planning_scene_->getCurrentState())); if (isEndEffectorFree(plan, *token_state)) { // update the goal pose message if anything has changed; this is because the name of the frame in the input goal // pose // can be that of objects in the collision world but most components are unaware of those transforms, // so we convert to a frame that is certainly known - if (!robot_state::Transforms::sameFrame(planning_scene_->getPlanningFrame(), plan->goal_pose_.header.frame_id)) + if (!moveit::core::Transforms::sameFrame(planning_scene_->getPlanningFrame(), plan->goal_pose_.header.frame_id)) { plan->goal_pose_.pose = tf2::toMsg(plan->transformed_goal_pose_); plan->goal_pose_.header.frame_id = planning_scene_->getPlanningFrame(); diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index a2f467b1d9..321a5ad3dc 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -49,8 +49,8 @@ namespace { bool isStateValid(const planning_scene::PlanningScene* planning_scene, - const kinematic_constraints::KinematicConstraintSet* constraint_set, robot_state::RobotState* state, - const robot_state::JointModelGroup* group, const double* ik_solution) + const kinematic_constraints::KinematicConstraintSet* constraint_set, moveit::core::RobotState* state, + const moveit::core::JointModelGroup* group, const double* ik_solution) { state->setJointGroupPositions(group, ik_solution); state->update(); @@ -80,10 +80,10 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath ROS_INFO_NAMED(getName(), "Received request to compute Cartesian path"); context_->planning_scene_monitor_->updateFrameTransforms(); - robot_state::RobotState start_state = + moveit::core::RobotState start_state = planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState(); - robot_state::robotStateMsgToRobotState(req.start_state, start_state); - if (const robot_model::JointModelGroup* jmg = start_state.getJointModelGroup(req.group_name)) + moveit::core::robotStateMsgToRobotState(req.start_state, start_state); + if (const moveit::core::JointModelGroup* jmg = start_state.getJointModelGroup(req.group_name)) { std::string link_name = req.link_name; if (link_name.empty() && !jmg->getLinkModelNames().empty()) @@ -93,8 +93,8 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath EigenSTL::vector_Isometry3d waypoints(req.waypoints.size()); const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame(); bool no_transform = req.header.frame_id.empty() || - robot_state::Transforms::sameFrame(req.header.frame_id, default_frame) || - robot_state::Transforms::sameFrame(req.header.frame_id, link_name); + moveit::core::Transforms::sameFrame(req.header.frame_id, default_frame) || + moveit::core::Transforms::sameFrame(req.header.frame_id, link_name); for (std::size_t i = 0; i < req.waypoints.size(); ++i) { @@ -128,7 +128,7 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath { if (!waypoints.empty()) { - robot_state::GroupStateValidityCallbackFn constraint_fn; + moveit::core::GroupStateValidityCallbackFn constraint_fn; std::unique_ptr ls; std::unique_ptr kset; if (req.avoid_collisions || !moveit::core::isEmpty(req.path_constraints)) @@ -141,16 +141,16 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath req.avoid_collisions ? static_cast(*ls).get() : nullptr, kset->empty() ? nullptr : kset.get(), _1, _2, _3); } - bool global_frame = !robot_state::Transforms::sameFrame(link_name, req.header.frame_id); + bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req.header.frame_id); ROS_INFO_NAMED(getName(), "Attempting to follow %u waypoints for link '%s' using a step of %lf m " "and jump threshold %lf (in %s reference frame)", (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, req.jump_threshold, global_frame ? "global" : "link"); - std::vector traj; + std::vector traj; res.fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &start_state, jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame, moveit::core::MaxEEFStep(req.max_step), moveit::core::JumpThreshold(req.jump_threshold), constraint_fn); - robot_state::robotStateToRobotStateMsg(start_state, res.start_state); + moveit::core::robotStateToRobotStateMsg(start_state, res.start_state); robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req.group_name); for (const moveit::core::RobotStatePtr& traj_state : traj) @@ -169,7 +169,7 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath moveit_msgs::DisplayTrajectory disp; disp.model_id = context_->planning_scene_monitor_->getRobotModel()->getName(); disp.trajectory.resize(1, res.solution); - robot_state::robotStateToRobotStateMsg(rt.getFirstWayPoint(), disp.trajectory_start); + moveit::core::robotStateToRobotStateMsg(rt.getFirstWayPoint(), disp.trajectory_start); display_path_.publish(disp); } } diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index b1e86fa6b4..849ea5251a 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -58,7 +58,7 @@ namespace { bool isIKSolutionValid(const planning_scene::PlanningScene* planning_scene, const kinematic_constraints::KinematicConstraintSet* constraint_set, - robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, + moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, const double* ik_solution) { state->setJointGroupPositions(jmg, ik_solution); @@ -69,13 +69,13 @@ bool isIKSolutionValid(const planning_scene::PlanningScene* planning_scene, } // namespace void MoveGroupKinematicsService::computeIK(moveit_msgs::PositionIKRequest& req, moveit_msgs::RobotState& solution, - moveit_msgs::MoveItErrorCodes& error_code, robot_state::RobotState& rs, - const robot_state::GroupStateValidityCallbackFn& constraint) const + moveit_msgs::MoveItErrorCodes& error_code, moveit::core::RobotState& rs, + const moveit::core::GroupStateValidityCallbackFn& constraint) const { - const robot_state::JointModelGroup* jmg = rs.getJointModelGroup(req.group_name); + const moveit::core::JointModelGroup* jmg = rs.getJointModelGroup(req.group_name); if (jmg) { - robot_state::robotStateMsgToRobotState(req.robot_state, rs); + moveit::core::robotStateMsgToRobotState(req.robot_state, rs); const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame(); if (req.pose_stamped_vector.empty() || req.pose_stamped_vector.size() == 1) @@ -96,7 +96,7 @@ void MoveGroupKinematicsService::computeIK(moveit_msgs::PositionIKRequest& req, if (result_ik) { - robot_state::robotStateToRobotStateMsg(rs, solution, false); + moveit::core::robotStateToRobotStateMsg(rs, solution, false); error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; } else @@ -129,7 +129,7 @@ void MoveGroupKinematicsService::computeIK(moveit_msgs::PositionIKRequest& req, { if (rs.setFromIK(jmg, req_poses, req.ik_link_names, req.timeout.toSec(), constraint)) { - robot_state::robotStateToRobotStateMsg(rs, solution, false); + moveit::core::robotStateToRobotStateMsg(rs, solution, false); error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; } else @@ -152,7 +152,7 @@ bool MoveGroupKinematicsService::computeIKService(moveit_msgs::GetPositionIK::Re { planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_); kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel()); - robot_state::RobotState rs = ls->getCurrentState(); + moveit::core::RobotState rs = ls->getCurrentState(); kset.add(req.ik_request.constraints, ls->getTransforms()); computeIK(req.ik_request, res.solution, res.error_code, rs, boost::bind(&isIKSolutionValid, req.ik_request.avoid_collisions ? @@ -163,7 +163,7 @@ bool MoveGroupKinematicsService::computeIKService(moveit_msgs::GetPositionIK::Re else { // compute unconstrained IK, no lock to planning scene maintained - robot_state::RobotState rs = + moveit::core::RobotState rs = planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState(); computeIK(req.ik_request, res.solution, res.error_code, rs); } @@ -185,13 +185,13 @@ bool MoveGroupKinematicsService::computeFKService(moveit_msgs::GetPositionFK::Re const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame(); bool do_transform = !req.header.frame_id.empty() && - !robot_state::Transforms::sameFrame(req.header.frame_id, default_frame) && + !moveit::core::Transforms::sameFrame(req.header.frame_id, default_frame) && context_->planning_scene_monitor_->getTFClient(); bool tf_problem = false; - robot_state::RobotState rs = + moveit::core::RobotState rs = planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState(); - robot_state::robotStateMsgToRobotState(req.robot_state, rs); + moveit::core::robotStateMsgToRobotState(req.robot_state, rs); for (std::size_t i = 0; i < req.fk_link_names.size(); ++i) if (rs.getRobotModel()->hasLinkModel(req.fk_link_names[i])) { diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h index 30e300ef47..3195f04150 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h @@ -53,10 +53,10 @@ class MoveGroupKinematicsService : public MoveGroupCapability bool computeIKService(moveit_msgs::GetPositionIK::Request& req, moveit_msgs::GetPositionIK::Response& res); bool computeFKService(moveit_msgs::GetPositionFK::Request& req, moveit_msgs::GetPositionFK::Response& res); - void computeIK( - moveit_msgs::PositionIKRequest& req, moveit_msgs::RobotState& solution, moveit_msgs::MoveItErrorCodes& error_code, - robot_state::RobotState& rs, - const robot_state::GroupStateValidityCallbackFn& constraint = robot_state::GroupStateValidityCallbackFn()) const; + void computeIK(moveit_msgs::PositionIKRequest& req, moveit_msgs::RobotState& solution, + moveit_msgs::MoveItErrorCodes& error_code, moveit::core::RobotState& rs, + const moveit::core::GroupStateValidityCallbackFn& constraint = + moveit::core::GroupStateValidityCallbackFn()) const; ros::ServiceServer fk_service_; ros::ServiceServer ik_service_; diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index 26c619b7d8..22d8777d0a 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -106,7 +106,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const moveit_msgs::M if (moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) { planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); - const robot_state::RobotState& current_state = lscene->getCurrentState(); + const moveit::core::RobotState& current_state = lscene->getCurrentState(); // check to see if the desired constraints are already met for (std::size_t i = 0; i < goal->request.goal_constraints.size(); ++i) diff --git a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp index c333f4a9a7..385d715847 100644 --- a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp @@ -56,8 +56,8 @@ bool MoveGroupStateValidationService::computeService(moveit_msgs::GetStateValidi moveit_msgs::GetStateValidity::Response& res) { planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_); - robot_state::RobotState rs = ls->getCurrentState(); - robot_state::robotStateMsgToRobotState(req.robot_state, rs); + moveit::core::RobotState rs = ls->getCurrentState(); + moveit::core::robotStateMsgToRobotState(req.robot_state, rs); res.valid = true; diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp index bb666dda04..b48eea2e81 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -98,10 +98,10 @@ void TfPublisher::publishPlanningSceneFrames() publishSubframes(broadcaster, subframes, object_frame, stamp); } - const robot_state::RobotState& rs = locked_planning_scene->getCurrentState(); - std::vector attached_collision_objects; + const moveit::core::RobotState& rs = locked_planning_scene->getCurrentState(); + std::vector attached_collision_objects; rs.getAttachedBodies(attached_collision_objects); - for (const robot_state::AttachedBody* attached_body : attached_collision_objects) + for (const moveit::core::AttachedBody* attached_body : attached_collision_objects) { std::string object_frame = prefix_ + attached_body->getName(); transform = tf2::eigenToTransform(attached_body->getFixedTransforms()[0]); diff --git a/moveit_ros/move_group/src/move_group_capability.cpp b/moveit_ros/move_group/src/move_group_capability.cpp index 2aa6a9cfce..48ee60e0b1 100644 --- a/moveit_ros/move_group/src/move_group_capability.cpp +++ b/moveit_ros/move_group/src/move_group_capability.cpp @@ -57,7 +57,7 @@ void move_group::MoveGroupCapability::convertToMsg(const std::vectorempty()) { - robot_state::robotStateToRobotStateMsg(trajectory[i].trajectory_->getFirstWayPoint(), first_state_msg); + moveit::core::robotStateToRobotStateMsg(trajectory[i].trajectory_->getFirstWayPoint(), first_state_msg); first = false; } trajectory[i].trajectory_->getRobotTrajectoryMsg(trajectory_msg[i]); @@ -72,7 +72,7 @@ void move_group::MoveGroupCapability::convertToMsg(const robot_trajectory::Robot { if (trajectory && !trajectory->empty()) { - robot_state::robotStateToRobotStateMsg(trajectory->getFirstWayPoint(), first_state_msg); + moveit::core::robotStateToRobotStateMsg(trajectory->getFirstWayPoint(), first_state_msg); trajectory->getRobotTrajectoryMsg(trajectory_msg); } } diff --git a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp index 511e2cca6d..c195c6ea03 100644 --- a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp +++ b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp @@ -176,8 +176,8 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d void mesh_filter::DepthSelfFiltering::addMeshes(MeshFilter& mesh_filter) { robot_model_loader::RobotModelLoader robotModelLoader("robot_description"); - robot_model::RobotModelConstPtr robotModel = robotModelLoader.getModel(); - const vector& links = robotModel->getLinkModelsWithCollisionGeometry(); + moveit::core::RobotModelConstPtr robotModel = robotModelLoader.getModel(); + const vector& links = robotModel->getLinkModelsWithCollisionGeometry(); for (size_t i = 0; i < links.size(); ++i) { shapes::ShapeConstPtr shape = links[i]->getShape(); diff --git a/moveit_ros/perception/mesh_filter/src/transform_provider.cpp b/moveit_ros/perception/mesh_filter/src/transform_provider.cpp index cc5e8bda85..527776a207 100644 --- a/moveit_ros/perception/mesh_filter/src/transform_provider.cpp +++ b/moveit_ros/perception/mesh_filter/src/transform_provider.cpp @@ -129,7 +129,7 @@ void TransformProvider::setUpdateInterval(unsigned long usecs) void TransformProvider::updateTransforms() { static tf2::Stamped input_transform, output_transform; - static robot_state::RobotStatePtr robot_state; + static moveit::core::RobotStatePtr robot_state; robot_state = psm_->getStateMonitor()->getCurrentState(); try { diff --git a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h index 07c6545208..809599bf79 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h +++ b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h @@ -79,11 +79,11 @@ class KinematicsPluginLoader /** \brief Get a function pointer that allocates and initializes a kinematics solver. If not previously called, this * function reads the SRDF and calls the variant below. */ - robot_model::SolverAllocatorFn getLoaderFunction(); + moveit::core::SolverAllocatorFn getLoaderFunction(); /** \brief Get a function pointer that allocates and initializes a kinematics solver. If not previously called, this * function reads ROS parameters for the groups defined in the SRDF. */ - robot_model::SolverAllocatorFn getLoaderFunction(const srdf::ModelSharedPtr& srdf_model); + moveit::core::SolverAllocatorFn getLoaderFunction(const srdf::ModelSharedPtr& srdf_model); /** \brief Get the groups for which the function pointer returned by getLoaderFunction() can allocate a solver */ const std::vector& getKnownGroups() const diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index 2f1e24d15b..286896c521 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -84,7 +84,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl * \param jmg - joint model group pointer * \return tips - list of valid links in a planning group to plan for */ - std::vector chooseTipFrames(const robot_model::JointModelGroup* jmg) + std::vector chooseTipFrames(const moveit::core::JointModelGroup* jmg) { std::vector tips; std::map >::const_iterator ik_it = @@ -124,7 +124,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl return tips; } - kinematics::KinematicsBasePtr allocKinematicsSolver(const robot_model::JointModelGroup* jmg) + kinematics::KinematicsBasePtr allocKinematicsSolver(const moveit::core::JointModelGroup* jmg) { kinematics::KinematicsBasePtr result; if (!kinematics_loader_) @@ -137,7 +137,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl ROS_ERROR_NAMED(LOGNAME, "Specified group is NULL. Cannot allocate kinematics solver."); return result; } - const std::vector& links = jmg->getLinkModels(); + const std::vector& links = jmg->getLinkModels(); if (links.empty()) { ROS_ERROR_NAMED(LOGNAME, "No links specified for group '%s'. Cannot allocate kinematics solver.", @@ -211,7 +211,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl // cache solver between two consecutive calls // first call in RobotModelLoader::loadKinematicsSolvers() is just to check suitability for jmg // second call in JointModelGroup::setSolverAllocators() is to actually retrieve the instance for use - kinematics::KinematicsBasePtr allocKinematicsSolverWithCache(const robot_model::JointModelGroup* jmg) + kinematics::KinematicsBasePtr allocKinematicsSolverWithCache(const moveit::core::JointModelGroup* jmg) { boost::mutex::scoped_lock slock(cache_lock_); kinematics::KinematicsBasePtr& cached = instances_[jmg]; @@ -239,7 +239,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl std::map > iksolver_to_tip_links_; // a map between each ik solver and a vector // of custom-specified tip link(s) std::shared_ptr > kinematics_loader_; - std::map instances_; + std::map instances_; boost::mutex lock_; boost::mutex cache_lock_; }; @@ -252,7 +252,7 @@ void KinematicsPluginLoader::status() const ROS_INFO_NAMED(LOGNAME, "Loader function was never required"); } -robot_model::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction() +moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction() { moveit::tools::Profiler::ScopedStart prof_start; moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction"); @@ -265,7 +265,7 @@ robot_model::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction() return getLoaderFunction(rml.getSRDF()); } -robot_model::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const srdf::ModelSharedPtr& srdf_model) +moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const srdf::ModelSharedPtr& srdf_model) { moveit::tools::Profiler::ScopedStart prof_start; moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction(SRDF)"); diff --git a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp index 36a75ca4bb..a913b5e976 100644 --- a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp +++ b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp @@ -92,7 +92,7 @@ void clutterWorld(const planning_scene::PlanningScenePtr& planning_scene, const planning_scene->getRobotModel()->getLinkModelNames(), true) }; // set the robot state to home position - robot_state::RobotState& current_state{ planning_scene->getCurrentStateNonConst() }; + moveit::core::RobotState& current_state{ planning_scene->getCurrentStateNonConst() }; collision_detection::CollisionRequest req; current_state.setToDefaultValues(current_state.getJointModelGroup("panda_arm"), "home"); current_state.update(); @@ -170,7 +170,7 @@ void clutterWorld(const planning_scene::PlanningScenePtr& planning_scene, const * \param CollisionDetector The type of collision detector * \param only_self Flag for only self collision check performed */ void runCollisionDetection(unsigned int trials, const planning_scene::PlanningScenePtr& scene, - const std::vector& states, const CollisionDetector col_detector, + const std::vector& states, const CollisionDetector col_detector, bool only_self, bool distance = false) { collision_detection::AllowedCollisionMatrix acm{ collision_detection::AllowedCollisionMatrix( @@ -247,9 +247,9 @@ void runCollisionDetection(unsigned int trials, const planning_scene::PlanningSc * \param scene The planning scene * \param robot_states Result vector */ void findStates(const RobotStateSelector desired_states, unsigned int num_states, - const planning_scene::PlanningScenePtr& scene, std::vector& robot_states) + const planning_scene::PlanningScenePtr& scene, std::vector& robot_states) { - robot_state::RobotState& current_state{ scene->getCurrentStateNonConst() }; + moveit::core::RobotState& current_state{ scene->getCurrentStateNonConst() }; collision_detection::CollisionRequest req; size_t i{ 0 }; @@ -290,7 +290,7 @@ void findStates(const RobotStateSelector desired_states, unsigned int num_states int main(int argc, char** argv) { - robot_model::RobotModelPtr robot_model; + moveit::core::RobotModelPtr robot_model; ros::init(argc, argv, "compare_collision_checking_speed"); ros::NodeHandle node_handle; @@ -322,11 +322,11 @@ int main(int argc, char** argv) { ros::Duration(0.5).sleep(); - robot_state::RobotState& current_state{ planning_scene->getCurrentStateNonConst() }; + moveit::core::RobotState& current_state{ planning_scene->getCurrentStateNonConst() }; current_state.setToDefaultValues(current_state.getJointModelGroup("panda_arm"), "home"); current_state.update(); - std::vector sampled_states; + std::vector sampled_states; sampled_states.push_back(current_state); ROS_INFO("Starting benchmark: Robot in empty world, only self collision check"); @@ -346,7 +346,7 @@ int main(int argc, char** argv) current_state.setJointPositions("panda_joint2", &joint_2); current_state.update(); - std::vector sampled_states_2; + std::vector sampled_states_2; sampled_states_2.push_back(current_state); ROS_INFO("Starting benchmark: Robot in cluttered world, in collision with world"); diff --git a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp index c990a30e68..b8530d349e 100644 --- a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp +++ b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp @@ -41,7 +41,7 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; void runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene* scene, - const robot_state::RobotState* state) + const moveit::core::RobotState* state) { ROS_INFO("Starting thread %u", id); collision_detection::CollisionRequest req; @@ -95,12 +95,12 @@ int main(int argc, char** argv) else ros::Duration(0.5).sleep(); - std::vector states; + std::vector states; ROS_INFO("Sampling %u valid states...", nthreads); for (unsigned int i = 0; i < nthreads; ++i) { // sample a valid state - robot_state::RobotState* state = new robot_state::RobotState(psm.getPlanningScene()->getRobotModel()); + moveit::core::RobotState* state = new moveit::core::RobotState(psm.getPlanningScene()->getRobotModel()); collision_detection::CollisionRequest req; do { @@ -111,7 +111,7 @@ int main(int argc, char** argv) if (!res.collision) break; } while (true); - states.push_back(robot_state::RobotStatePtr(state)); + states.push_back(moveit::core::RobotStatePtr(state)); } std::vector threads; diff --git a/moveit_ros/planning/planning_components_tools/src/evaluate_state_operations_speed.cpp b/moveit_ros/planning/planning_components_tools/src/evaluate_state_operations_speed.cpp index d4ab597dcc..d38519c3c7 100644 --- a/moveit_ros/planning/planning_components_tools/src/evaluate_state_operations_speed.cpp +++ b/moveit_ros/planning/planning_components_tools/src/evaluate_state_operations_speed.cpp @@ -51,11 +51,11 @@ int main(int argc, char** argv) robot_model_loader::RobotModelLoader rml(ROBOT_DESCRIPTION); ros::Duration(0.5).sleep(); - robot_model::RobotModelConstPtr robot_model = rml.getModel(); + moveit::core::RobotModelConstPtr robot_model = rml.getModel(); if (robot_model) { static const int N = 10000; - robot_state::RobotState state(robot_model); + moveit::core::RobotState state(robot_model); printf("Evaluating model '%s' using %d trials for each test\n", robot_model->getName().c_str(), N); @@ -80,12 +80,12 @@ int main(int argc, char** argv) moveit::tools::Profiler::End("FK Random"); } - std::vector copies(N, (robot_state::RobotState*)nullptr); + std::vector copies(N, (moveit::core::RobotState*)nullptr); printf("Evaluating Copy State ...\n"); for (int i = 0; i < N; ++i) { moveit::tools::Profiler::Begin("Copy State"); - copies[i] = new robot_state::RobotState(state); + copies[i] = new moveit::core::RobotState(state); moveit::tools::Profiler::End("Copy State"); } @@ -101,7 +101,7 @@ int main(int argc, char** argv) for (const std::string& group : groups) { printf("\n"); - const robot_model::JointModelGroup* jmg = robot_model->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); printf("%s: Evaluating FK Random ...\n", group.c_str()); std::string pname = group + ":FK Random"; diff --git a/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp b/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp index 117136f163..d0267a2850 100644 --- a/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp +++ b/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp @@ -56,14 +56,14 @@ int main(int argc, char** argv) std::string group = argv[1]; ROS_INFO_STREAM("Evaluating IK for " << group); - const robot_model::JointModelGroup* jmg = rml.getModel()->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = rml.getModel()->getJointModelGroup(group); if (jmg) { const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); if (solver) { const std::string& tip = solver->getTipFrame(); - robot_state::RobotState state(rml.getModel()); + moveit::core::RobotState state(rml.getModel()); state.setToDefaultValues(); ROS_INFO_STREAM("Tip Frame: " << solver->getTipFrame()); diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index 313f770999..27b14c78dc 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -75,7 +75,7 @@ class PlanningPipeline \param adapter_plugins_param_name The name of the ROS parameter under which the names of the request adapter plugins are specified (plugin names separated by space; order matters) */ - PlanningPipeline(const robot_model::RobotModelConstPtr& model, const ros::NodeHandle& nh = ros::NodeHandle("~"), + PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const ros::NodeHandle& nh = ros::NodeHandle("~"), const std::string& planning_plugin_param_name = "planning_plugin", const std::string& adapter_plugins_param_name = "request_adapters"); @@ -85,7 +85,7 @@ class PlanningPipeline \param planning_plugin_name The name of the planning plugin to load \param adapter_plugins_names The names of the planning request adapter plugins to load */ - PlanningPipeline(const robot_model::RobotModelConstPtr& model, const ros::NodeHandle& nh, + PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const ros::NodeHandle& nh, const std::string& planning_plugin_name, const std::vector& adapter_plugin_names); /** \brief Pass a flag telling the pipeline whether or not to publish the computed motion plans on DISPLAY_PATH_TOPIC. @@ -160,7 +160,7 @@ class PlanningPipeline } /** \brief Get the robot model that this pipeline is using */ - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -187,7 +187,7 @@ class PlanningPipeline std::unique_ptr adapter_chain_; std::vector adapter_plugin_names_; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; /// Flag indicating whether the reported plans should be checked once again, by the planning pipeline itself bool check_solution_paths_; diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 6bee28fe7e..3188880f46 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -48,7 +48,7 @@ const std::string planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC = "dis const std::string planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC = "motion_plan_request"; const std::string planning_pipeline::PlanningPipeline::MOTION_CONTACTS_TOPIC = "display_contacts"; -planning_pipeline::PlanningPipeline::PlanningPipeline(const robot_model::RobotModelConstPtr& model, +planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const ros::NodeHandle& nh, const std::string& planner_plugin_param_name, const std::string& adapter_plugins_param_name) @@ -70,7 +70,7 @@ planning_pipeline::PlanningPipeline::PlanningPipeline(const robot_model::RobotMo configure(); } -planning_pipeline::PlanningPipeline::PlanningPipeline(const robot_model::RobotModelConstPtr& model, +planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const ros::NodeHandle& nh, const std::string& planner_plugin_name, const std::vector& adapter_plugin_names) : nh_(nh), planner_plugin_name_(planner_plugin_name), adapter_plugin_names_(adapter_plugin_names), robot_model_(model) @@ -298,7 +298,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla for (std::size_t it : index) { // check validity with verbose on - const robot_state::RobotState& robot_state = res.trajectory_->getWayPoint(it); + const moveit::core::RobotState& robot_state = res.trajectory_->getWayPoint(it); planning_scene->isStateValid(robot_state, req.path_constraints, req.group_name, true); // compute the contacts if any @@ -338,7 +338,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla disp.model_id = robot_model_->getName(); disp.trajectory.resize(1); res.trajectory_->getRobotTrajectoryMsg(disp.trajectory[0]); - robot_state::robotStateToRobotStateMsg(res.trajectory_->getFirstWayPoint(), disp.trajectory_start); + moveit::core::robotStateToRobotStateMsg(res.trajectory_->getFirstWayPoint(), disp.trajectory_start); display_path_publisher_.publish(disp); } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp index ea3f09f0ad..d3e4fb1617 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp @@ -84,10 +84,10 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap ROS_DEBUG("Running '%s'", getDescription().c_str()); // get the specified start state - robot_state::RobotState start_state = planning_scene->getCurrentState(); - robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); + moveit::core::RobotState start_state = planning_scene->getCurrentState(); + moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); - const std::vector& jmodels = + const std::vector& jmodels = planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ? planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() : planning_scene->getRobotModel()->getJointModels(); @@ -101,9 +101,9 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap // how many times the joint was wrapped. Because of this, we remember the offsets for continuous // joints, and we un-do them when the plan comes from the planner - if (jm->getType() == robot_model::JointModel::REVOLUTE) + if (jm->getType() == moveit::core::JointModel::REVOLUTE) { - if (static_cast(jm)->isContinuous()) + if (static_cast(jm)->isContinuous()) { double initial = start_state.getJointPositions(jm)[0]; start_state.enforceBounds(jm); @@ -114,11 +114,11 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } else // Normalize yaw; no offset needs to be remembered - if (jm->getType() == robot_model::JointModel::PLANAR) + if (jm->getType() == moveit::core::JointModel::PLANAR) { const double* p = start_state.getJointPositions(jm); double copy[3] = { p[0], p[1], p[2] }; - if (static_cast(jm)->normalizeRotation(copy)) + if (static_cast(jm)->normalizeRotation(copy)) { start_state.setJointPositions(jm, copy); change_req = true; @@ -126,11 +126,11 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } else // Normalize quaternions - if (jm->getType() == robot_model::JointModel::FLOATING) + if (jm->getType() == moveit::core::JointModel::FLOATING) { const double* p = start_state.getJointPositions(jm); double copy[7] = { p[0], p[1], p[2], p[3], p[4], p[5], p[6] }; - if (static_cast(jm)->normalizeRotation(copy)) + if (static_cast(jm)->normalizeRotation(copy)) { start_state.setJointPositions(jm, copy); change_req = true; @@ -139,7 +139,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } // pointer to a prefix state we could possibly add, if we detect we have to make changes - robot_state::RobotStatePtr prefix_state; + moveit::core::RobotStatePtr prefix_state; for (const moveit::core::JointModel* jmodel : jmodels) { if (!start_state.satisfiesBounds(jmodel)) @@ -147,7 +147,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap if (start_state.satisfiesBounds(jmodel, bounds_dist_)) { if (!prefix_state) - prefix_state.reset(new robot_state::RobotState(start_state)); + prefix_state.reset(new moveit::core::RobotState(start_state)); start_state.enforceBounds(jmodel); change_req = true; ROS_INFO("Starting state is just outside bounds (joint '%s'). Assuming within bounds.", @@ -161,7 +161,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap const double* p = start_state.getJointPositions(jmodel); for (std::size_t k = 0; k < jmodel->getVariableCount(); ++k) joint_values << p[k] << " "; - const robot_model::JointModel::Bounds& b = jmodel->getVariableBounds(); + const moveit::core::JointModel::Bounds& b = jmodel->getVariableBounds(); for (const moveit::core::VariableBounds& variable_bounds : b) { joint_bounds_low << variable_bounds.min_position_ << " "; @@ -181,7 +181,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap if (change_req) { planning_interface::MotionPlanRequest req2 = req; - robot_state::robotStateToRobotStateMsg(start_state, req2.start_state, false); + moveit::core::robotStateToRobotStateMsg(start_state, req2.start_state, false); solved = planner(planning_scene, req2, res); } else diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp index 02768c02e8..8a3c72431e 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp @@ -99,8 +99,8 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA ROS_DEBUG("Running '%s'", getDescription().c_str()); // get the specified start state - robot_state::RobotState start_state = planning_scene->getCurrentState(); - robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); + moveit::core::RobotState start_state = planning_scene->getCurrentState(); + moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); collision_detection::CollisionRequest creq; creq.group_name = req.group_name; @@ -119,10 +119,10 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA else ROS_INFO_STREAM("Start state appears to be in collision with respect to group " << creq.group_name); - robot_state::RobotStatePtr prefix_state(new robot_state::RobotState(start_state)); + moveit::core::RobotStatePtr prefix_state(new moveit::core::RobotState(start_state)); random_numbers::RandomNumberGenerator& rng = prefix_state->getRandomNumberGenerator(); - const std::vector& jmodels = + const std::vector& jmodels = planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ? planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() : planning_scene->getRobotModel()->getJointModels(); @@ -151,7 +151,7 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA if (found) { planning_interface::MotionPlanRequest req2 = req; - robot_state::robotStateToRobotStateMsg(start_state, req2.start_state); + moveit::core::robotStateToRobotStateMsg(start_state, req2.start_state); bool solved = planner(planning_scene, req2, res); if (solved && !res.trajectory_->empty()) { diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp index 80a6f058ce..c7cdd212bf 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp @@ -66,8 +66,8 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe ROS_DEBUG("Running '%s'", getDescription().c_str()); // get the specified start state - robot_state::RobotState start_state = planning_scene->getCurrentState(); - robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); + moveit::core::RobotState start_state = planning_scene->getCurrentState(); + moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); // if the start state is otherwise valid but does not meet path constraints if (planning_scene->isStateValid(start_state, req.group_name) && @@ -95,7 +95,7 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe ROS_INFO("Planned to path constraints. Resuming original planning request."); // extract the last state of the computed motion plan and set it as the new start state - robot_state::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state); + moveit::core::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state); bool solved2 = planner(planning_scene, req3, res); res.planning_time_ += res2.planning_time_; diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h index 806db00daf..ab63281a5e 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h @@ -60,7 +60,7 @@ class CurrentStateMonitor * @param robot_model The current kinematic model to build on * @param tf_buffer A pointer to the tf2_ros Buffer to use */ - CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, + CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer); /** @brief Constructor. @@ -68,7 +68,7 @@ class CurrentStateMonitor * @param tf_buffer A pointer to the tf2_ros Buffer to use * @param nh A ros::NodeHandle to pass node specific options */ - CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, + CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer, const ros::NodeHandle& nh); ~CurrentStateMonitor(); @@ -86,7 +86,7 @@ class CurrentStateMonitor bool isActive() const; /** @brief Get the RobotModel for which we are monitoring state */ - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -120,17 +120,17 @@ class CurrentStateMonitor /** @brief Get the current state * @return Returns the current state */ - robot_state::RobotStatePtr getCurrentState() const; + moveit::core::RobotStatePtr getCurrentState() const; /** @brief Set the state \e upd to the current state maintained by this class. */ - void setToCurrentState(robot_state::RobotState& upd) const; + void setToCurrentState(moveit::core::RobotState& upd) const; /** @brief Get the time stamp for the current state */ ros::Time getCurrentStateTime() const; /** @brief Get the current state and its time stamp * @return Returns a pair of the current state and its time stamp */ - std::pair getCurrentStateAndTime() const; + std::pair getCurrentStateAndTime() const; /** @brief Get the current state values as a map from joint names to joint state values * @return Returns the map from joint names to joint state values*/ @@ -193,8 +193,8 @@ class CurrentStateMonitor ros::NodeHandle nh_; std::shared_ptr tf_buffer_; - robot_model::RobotModelConstPtr robot_model_; - robot_state::RobotState robot_state_; + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotState robot_state_; std::map joint_time_; bool state_monitor_started_; bool copy_dynamics_; // Copy velocity and effort from joint_state diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index d79b575eb9..ca9fe70b3a 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -170,7 +170,7 @@ class PlanningSceneMonitor : private boost::noncopyable return rm_loader_; } - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -437,7 +437,7 @@ class PlanningSceneMonitor : private boost::noncopyable void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj); /** @brief Callback for a change for an attached object of the current state of the planning scene */ - void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body, bool just_attached); + void currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody* attached_body, bool just_attached); /** @brief Callback for a change in the world maintained by the planning scene */ void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& object, @@ -453,8 +453,8 @@ class PlanningSceneMonitor : private boost::noncopyable void excludeAttachedBodiesFromOctree(); void includeAttachedBodiesInOctree(); - void excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body); - void includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body); + void excludeAttachedBodyFromOctree(const moveit::core::AttachedBody* attached_body); + void includeAttachedBodyInOctree(const moveit::core::AttachedBody* attached_body); bool getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time, occupancy_map_monitor::ShapeTransformCache& cache) const; @@ -516,10 +516,10 @@ class PlanningSceneMonitor : private boost::noncopyable // include a current state monitor CurrentStateMonitorPtr current_state_monitor_; - typedef std::map > > LinkShapeHandles; - typedef std::map > > AttachedBodyShapeHandles; typedef std::map > > @@ -580,7 +580,7 @@ class PlanningSceneMonitor : private boost::noncopyable ros::WallTime last_robot_state_update_wall_time_; robot_model_loader::RobotModelLoaderPtr rm_loader_; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; collision_detection::CollisionPluginLoader collision_loader_; @@ -596,7 +596,7 @@ class PlanningSceneMonitor : private boost::noncopyable * "operator->" functions. Therefore you will often see code like this: * @code * planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); - * robot_model::RobotModelConstPtr model = ls->getRobotModel(); + * moveit::core::RobotModelConstPtr model = ls->getRobotModel(); * @endcode * The function "getRobotModel()" is a member of PlanningScene and not @@ -688,7 +688,7 @@ class LockedPlanningSceneRO * "operator->" functions. Therefore you will often see code like this: * @code * planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor); - * robot_model::RobotModelConstPtr model = ls->getRobotModel(); + * moveit::core::RobotModelConstPtr model = ls->getRobotModel(); * @endcode * The function "getRobotModel()" is a member of PlanningScene and not diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h index f7638161a7..aff77b560e 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h @@ -44,7 +44,7 @@ namespace planning_scene_monitor { -typedef boost::function +typedef boost::function TrajectoryStateAddedCallback; MOVEIT_CLASS_FORWARD(TrajectoryMonitor); diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index 41d5cbc2bc..d9262aedbb 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -41,13 +41,13 @@ #include -planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, +planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer) : CurrentStateMonitor(robot_model, tf_buffer, ros::NodeHandle()) { } -planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, +planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer, const ros::NodeHandle& nh) : nh_(nh) @@ -66,11 +66,11 @@ planning_scene_monitor::CurrentStateMonitor::~CurrentStateMonitor() stopStateMonitor(); } -robot_state::RobotStatePtr planning_scene_monitor::CurrentStateMonitor::getCurrentState() const +moveit::core::RobotStatePtr planning_scene_monitor::CurrentStateMonitor::getCurrentState() const { boost::mutex::scoped_lock slock(state_update_lock_); - robot_state::RobotState* result = new robot_state::RobotState(robot_state_); - return robot_state::RobotStatePtr(result); + moveit::core::RobotState* result = new moveit::core::RobotState(robot_state_); + return moveit::core::RobotStatePtr(result); } ros::Time planning_scene_monitor::CurrentStateMonitor::getCurrentStateTime() const @@ -79,12 +79,12 @@ ros::Time planning_scene_monitor::CurrentStateMonitor::getCurrentStateTime() con return current_state_time_; } -std::pair +std::pair planning_scene_monitor::CurrentStateMonitor::getCurrentStateAndTime() const { boost::mutex::scoped_lock slock(state_update_lock_); - robot_state::RobotState* result = new robot_state::RobotState(robot_state_); - return std::make_pair(robot_state::RobotStatePtr(result), current_state_time_); + moveit::core::RobotState* result = new moveit::core::RobotState(robot_state_); + return std::make_pair(moveit::core::RobotStatePtr(result), current_state_time_); } std::map planning_scene_monitor::CurrentStateMonitor::getCurrentStateValues() const @@ -98,7 +98,7 @@ std::map planning_scene_monitor::CurrentStateMonitor::getCu return m; } -void planning_scene_monitor::CurrentStateMonitor::setToCurrentState(robot_state::RobotState& upd) const +void planning_scene_monitor::CurrentStateMonitor::setToCurrentState(moveit::core::RobotState& upd) const { boost::mutex::scoped_lock slock(state_update_lock_); const double* pos = robot_state_.getVariablePositions(); @@ -369,7 +369,7 @@ void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const senso if (static_cast(jm)->isContinuous()) continue; - const robot_model::VariableBounds& b = + const moveit::core::VariableBounds& b = jm->getVariableBounds()[0]; // only one variable in the joint, so we get its bounds // if the read variable is 'almost' within bounds (up to error_ difference), then consider it to be within @@ -448,7 +448,7 @@ void planning_scene_monitor::CurrentStateMonitor::tfCallback() joint_time_[joint] = latest_common_time; std::vector new_values(joint->getStateSpaceDimension()); - const robot_model::LinkModel* link = joint->getChildLinkModel(); + const moveit::core::LinkModel* link = joint->getChildLinkModel(); if (link->jointOriginTransformIsIdentity()) joint->computeVariablePositions(tf2::transformToEigen(transf), new_values.data()); else diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 2381e06035..cc0ebeb38f 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -162,7 +162,7 @@ PlanningSceneMonitor::~PlanningSceneMonitor() if (scene_) { scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); - scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback()); + scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback()); } stopPublishingPlanningScene(); stopStateMonitor(); @@ -266,7 +266,7 @@ void PlanningSceneMonitor::monitorDiffs(bool flag) boost::unique_lock ulock(scene_update_mutex_); if (scene_) { - scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback()); + scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback()); scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); scene_->decoupleParent(); parent_scene_ = scene_; @@ -376,7 +376,7 @@ void PlanningSceneMonitor::scenePublishingThread() // update while we are // potentially changing // attached bodies - scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback()); + scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback()); scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); scene_->pushDiffs(parent_scene_); scene_->clearDiffs(); @@ -568,7 +568,7 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningSc if (!scene.is_diff && parent_scene_) { // the scene is now decoupled from the parent, since we just reset it - scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback()); + scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback()); scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); parent_scene_ = scene_; scene_ = parent_scene_->diff(); @@ -673,7 +673,7 @@ void PlanningSceneMonitor::excludeRobotLinksFromOctree() boost::recursive_mutex::scoped_lock _(shape_handles_lock_); includeRobotLinksInOctree(); - const std::vector& links = getRobotModel()->getLinkModelsWithCollisionGeometry(); + const std::vector& links = getRobotModel()->getLinkModelsWithCollisionGeometry(); ros::WallTime start = ros::WallTime::now(); bool warned = false; for (const moveit::core::LinkModel* link : links) @@ -708,7 +708,7 @@ void PlanningSceneMonitor::includeRobotLinksInOctree() boost::recursive_mutex::scoped_lock _(shape_handles_lock_); - for (std::pair>>& link_shape_handle : link_shape_handles_) for (std::pair& it : link_shape_handle.second) @@ -724,7 +724,7 @@ void PlanningSceneMonitor::includeAttachedBodiesInOctree() boost::recursive_mutex::scoped_lock _(shape_handles_lock_); // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid) - for (std::pair>>& attached_body_shape_handle : attached_body_shape_handles_) for (std::pair& it : attached_body_shape_handle.second) @@ -738,7 +738,7 @@ void PlanningSceneMonitor::excludeAttachedBodiesFromOctree() includeAttachedBodiesInOctree(); // add attached objects again - std::vector ab; + std::vector ab; scene_->getCurrentState().getAttachedBodies(ab); for (const moveit::core::AttachedBody* body : ab) excludeAttachedBodyFromOctree(body); @@ -770,7 +770,7 @@ void PlanningSceneMonitor::excludeWorldObjectsFromOctree() excludeWorldObjectFromOctree(it.second); } -void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body) +void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const moveit::core::AttachedBody* attached_body) { if (!octomap_monitor_) return; @@ -792,7 +792,7 @@ void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const robot_state::Atta ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str()); } -void PlanningSceneMonitor::includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body) +void PlanningSceneMonitor::includeAttachedBodyInOctree(const moveit::core::AttachedBody* attached_body) { if (!octomap_monitor_) return; @@ -849,7 +849,7 @@ void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection: } } -void PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body, +void PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody* attached_body, bool just_attached) { if (!octomap_monitor_) @@ -997,7 +997,7 @@ bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_fram { boost::recursive_mutex::scoped_lock _(shape_handles_lock_); - for (const std::pair>>& link_shape_handle : link_shape_handles_) { @@ -1009,7 +1009,7 @@ bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_fram cache[link_shape_handle.second[j].first] = ttr * link_shape_handle.first->getCollisionOriginTransforms()[link_shape_handle.second[j].second]; } - for (const std::pair>>& attached_body_shape_handle : attached_body_shape_handles_) { diff --git a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp index 3631760f0c..157020c9c4 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp @@ -113,7 +113,7 @@ void planning_scene_monitor::TrajectoryMonitor::recordStates() while (record_states_thread_) { rate.sleep(); - std::pair state = current_state_monitor_->getCurrentStateAndTime(); + std::pair state = current_state_monitor_->getCurrentStateAndTime(); if (trajectory_.empty()) { trajectory_.addSuffixWayPoint(state.first, 0.0); diff --git a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h index c39d9f1056..5307ae2b3e 100644 --- a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h +++ b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h @@ -84,7 +84,7 @@ class RobotModelLoader ~RobotModelLoader(); /** @brief Get the constructed planning_models::RobotModel */ - const robot_model::RobotModelPtr& getModel() const + const moveit::core::RobotModelPtr& getModel() const { return model_; } @@ -128,7 +128,7 @@ class RobotModelLoader private: void configure(const Options& opt); - robot_model::RobotModelPtr model_; + moveit::core::RobotModelPtr model_; rdf_loader::RDFLoaderPtr rdf_loader_; kinematics_plugin_loader::KinematicsPluginLoaderPtr kinematics_loader_; }; diff --git a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp index 1ba8e75359..d5bc34ca5d 100644 --- a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp +++ b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp @@ -67,15 +67,15 @@ RobotModelLoader::~RobotModelLoader() namespace { -bool canSpecifyPosition(const robot_model::JointModel* jmodel, const unsigned int index) +bool canSpecifyPosition(const moveit::core::JointModel* jmodel, const unsigned int index) { bool ok = false; - if (jmodel->getType() == robot_model::JointModel::PLANAR && index == 2) + if (jmodel->getType() == moveit::core::JointModel::PLANAR && index == 2) ROS_ERROR("Cannot specify position limits for orientation of planar joint '%s'", jmodel->getName().c_str()); - else if (jmodel->getType() == robot_model::JointModel::FLOATING && index > 2) + else if (jmodel->getType() == moveit::core::JointModel::FLOATING && index > 2) ROS_ERROR("Cannot specify position limits for orientation of floating joint '%s'", jmodel->getName().c_str()); - else if (jmodel->getType() == robot_model::JointModel::REVOLUTE && - static_cast(jmodel)->isContinuous()) + else if (jmodel->getType() == moveit::core::JointModel::REVOLUTE && + static_cast(jmodel)->isContinuous()) ROS_ERROR("Cannot specify position limits for continuous joint '%s'", jmodel->getName().c_str()); else ok = true; @@ -97,7 +97,7 @@ void RobotModelLoader::configure(const Options& opt) { const srdf::ModelSharedPtr& srdf = rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); - model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf)); + model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); } if (model_ && !rdf_loader_->getRobotDescription().empty()) @@ -177,7 +177,8 @@ void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::Kin else kinematics_loader_.reset( new kinematics_plugin_loader::KinematicsPluginLoader(rdf_loader_->getRobotDescription())); - robot_model::SolverAllocatorFn kinematics_allocator = kinematics_loader_->getLoaderFunction(rdf_loader_->getSRDF()); + moveit::core::SolverAllocatorFn kinematics_allocator = + kinematics_loader_->getLoaderFunction(rdf_loader_->getSRDF()); const std::vector& groups = kinematics_loader_->getKnownGroups(); std::stringstream ss; std::copy(groups.begin(), groups.end(), std::ostream_iterator(ss, " ")); @@ -185,14 +186,14 @@ void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::Kin if (groups.empty() && !model_->getJointModelGroups().empty()) ROS_WARN("No kinematics plugins defined. Fill and load kinematics.yaml!"); - std::map imap; + std::map imap; for (const std::string& group : groups) { // Check if a group in kinematics.yaml exists in the srdf if (!model_->hasJointModelGroup(group)) continue; - const robot_model::JointModelGroup* jmg = model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = model_->getJointModelGroup(group); kinematics::KinematicsBasePtr solver = kinematics_allocator(jmg); if (solver) @@ -221,7 +222,7 @@ void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::Kin { if (!model_->hasJointModelGroup(it.first)) continue; - robot_model::JointModelGroup* jmg = model_->getJointModelGroup(it.first); + moveit::core::JointModelGroup* jmg = model_->getJointModelGroup(it.first); jmg->setDefaultIKTimeout(it.second); } } diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index 44091052bd..7a143dabf2 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -81,11 +81,11 @@ class TrajectoryExecutionManager }; /// Load the controller manager plugin, start listening for events on a topic. - TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& robot_model, + TrajectoryExecutionManager(const moveit::core::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm); /// Load the controller manager plugin, start listening for events on a topic. - TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& robot_model, + TrajectoryExecutionManager(const moveit::core::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers); /// Destructor. Cancels all running trajectories (if any) @@ -303,7 +303,7 @@ class TrajectoryExecutionManager // Name of this class for logging const std::string name_ = "trajectory_execution_manager"; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; planning_scene_monitor::CurrentStateMonitorPtr csm_; ros::NodeHandle node_handle_; ros::NodeHandle root_node_handle_; diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 71d180cefe..bf83b39e7e 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -78,7 +78,7 @@ class TrajectoryExecutionManager::DynamicReconfigureImpl dynamic_reconfigure::Server dynamic_reconfigure_server_; }; -TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& robot_model, +TrajectoryExecutionManager::TrajectoryExecutionManager(const moveit::core::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm) : robot_model_(robot_model), csm_(csm), node_handle_("~") { @@ -88,7 +88,7 @@ TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotM initialize(); } -TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& robot_model, +TrajectoryExecutionManager::TrajectoryExecutionManager(const moveit::core::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers) : robot_model_(robot_model), csm_(csm), node_handle_("~"), manage_controllers_(manage_controllers) @@ -806,10 +806,10 @@ bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::RobotTr std::set actuated_joints_single; for (const std::string& joint_name : trajectory.joint_trajectory.joint_names) { - const robot_model::JointModel* jm = robot_model_->getJointModel(joint_name); + const moveit::core::JointModel* jm = robot_model_->getJointModel(joint_name); if (jm) { - if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == robot_model::JointModel::FIXED) + if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == moveit::core::JointModel::FIXED) continue; actuated_joints_single.insert(jm->getName()); } @@ -936,7 +936,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont ROS_DEBUG_NAMED(name_, "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_); - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; if (!csm_->waitForCurrentState(ros::Time::now()) || !(current_state = csm_->getCurrentState())) { ROS_WARN_NAMED(name_, "Failed to validate trajectory: couldn't receive full current joint state within 1s"); @@ -959,7 +959,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont for (std::size_t i = 0, end = joint_names.size(); i < end; ++i) { - const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]); + const moveit::core::JointModel* jm = current_state->getJointModel(joint_names[i]); if (!jm) { ROS_ERROR_STREAM_NAMED(name_, "Unknown joint in trajectory: " << joint_names[i]); @@ -995,7 +995,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont for (std::size_t i = 0, end = joint_names.size(); i < end; ++i) { - const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]); + const moveit::core::JointModel* jm = current_state->getJointModel(joint_names[i]); if (!jm) { ROS_ERROR_STREAM_NAMED(name_, "Unknown joint in trajectory: " << joint_names[i]); @@ -1036,8 +1036,8 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context, std::set actuated_joints; auto is_actuated = [this](const std::string& joint_name) -> bool { - const robot_model::JointModel* jm = robot_model_->getJointModel(joint_name); - return (jm && !jm->isPassive() && !jm->getMimic() && jm->getType() != robot_model::JointModel::FIXED); + const moveit::core::JointModel* jm = robot_model_->getJointModel(joint_name); + return (jm && !jm->isPassive() && !jm->getMimic() && jm->getType() != moveit::core::JointModel::FIXED); }; for (const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names) if (is_actuated(joint_name)) @@ -1538,7 +1538,7 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon ros::WallTime start = ros::WallTime::now(); double time_remaining = wait_time; - robot_state::RobotStatePtr prev_state, cur_state; + moveit::core::RobotStatePtr prev_state, cur_state; prev_state = csm_->getCurrentState(); prev_state->enforceBounds(); @@ -1563,7 +1563,7 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon for (std::size_t i = 0; i < n && !moved; ++i) { - const robot_model::JointModel* jm = cur_state->getJointModel(joint_names[i]); + const moveit::core::JointModel* jm = cur_state->getJointModel(joint_names[i]); if (!jm) continue; // joint vanished from robot state (shouldn't happen), but we don't care @@ -1611,7 +1611,7 @@ moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::getLastEx bool TrajectoryExecutionManager::ensureActiveControllersForGroup(const std::string& group) { - const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group); if (joint_model_group) return ensureActiveControllersForJoints(joint_model_group->getJointModelNames()); else @@ -1628,10 +1628,10 @@ bool TrajectoryExecutionManager::ensureActiveControllersForJoints(const std::vec std::set jset; for (const std::string& joint : joints) { - const robot_model::JointModel* jm = robot_model_->getJointModel(joint); + const moveit::core::JointModel* jm = robot_model_->getJointModel(joint); if (jm) { - if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == robot_model::JointModel::FIXED) + if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == moveit::core::JointModel::FIXED) continue; jset.insert(joint); } diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h index 10248a0f96..e6e62f60ca 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h +++ b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h @@ -44,10 +44,10 @@ namespace planning_interface { std::shared_ptr getSharedTF(); -robot_model::RobotModelConstPtr getSharedRobotModel(const std::string& robot_description); +moveit::core::RobotModelConstPtr getSharedRobotModel(const std::string& robot_description); /** - @brief getSharedStateMonitor is a simpler version of getSharedStateMonitor(const robot_model::RobotModelConstPtr + @brief getSharedStateMonitor is a simpler version of getSharedStateMonitor(const moveit::core::RobotModelConstPtr &robot_model, const std::shared_ptr& tf_buffer, ros::NodeHandle nh = ros::NodeHandle() ). It calls this function using the default constructed ros::NodeHandle @@ -55,8 +55,8 @@ robot_model::RobotModelConstPtr getSharedRobotModel(const std::string& robot_des @param tf_buffer @return */ -planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model, - const std::shared_ptr& tf_buffer); +planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor( + const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer); /** @brief getSharedStateMonitor @@ -66,9 +66,9 @@ planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot @param nh A ros::NodeHandle to pass node specific configurations, such as callbacks queues. @return */ -planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model, - const std::shared_ptr& tf_buffer, - const ros::NodeHandle& nh); +planning_scene_monitor::CurrentStateMonitorPtr +getSharedStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, + const std::shared_ptr& tf_buffer, const ros::NodeHandle& nh); } // namespace planning interface } // namespace moveit diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp b/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp index 2c44c9c0d4..684bd1851f 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp +++ b/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp @@ -58,7 +58,7 @@ struct SharedStorage boost::mutex lock_; std::weak_ptr tf_buffer_; - std::map models_; + std::map models_; std::map state_monitors_; }; @@ -113,31 +113,31 @@ std::shared_ptr getSharedTF() return buffer; } -robot_model::RobotModelConstPtr getSharedRobotModel(const std::string& robot_description) +moveit::core::RobotModelConstPtr getSharedRobotModel(const std::string& robot_description) { SharedStorage& s = getSharedStorage(); boost::mutex::scoped_lock slock(s.lock_); - auto it = s.models_.insert(std::make_pair(robot_description, robot_model::RobotModelWeakPtr())).first; - robot_model::RobotModelPtr model = it->second.lock(); + auto it = s.models_.insert(std::make_pair(robot_description, moveit::core::RobotModelWeakPtr())).first; + moveit::core::RobotModelPtr model = it->second.lock(); if (!model) { RobotModelLoader::Options opt(robot_description); opt.load_kinematics_solvers_ = true; RobotModelLoaderPtr loader(new RobotModelLoader(opt)); // create an aliasing shared_ptr - model = robot_model::RobotModelPtr(loader, loader->getModel().get()); + model = moveit::core::RobotModelPtr(loader, loader->getModel().get()); it->second = model; } return model; } -CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model, +CurrentStateMonitorPtr getSharedStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer) { return getSharedStateMonitor(robot_model, tf_buffer, ros::NodeHandle()); } -CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model, +CurrentStateMonitorPtr getSharedStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer, const ros::NodeHandle& nh) { diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index e7d2b3bdcc..e792c49487 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -118,7 +118,7 @@ class MoveGroupInterface std::string robot_description_; /// Optionally, an instance of the RobotModel to use can be also specified - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; ros::NodeHandle node_handle_; }; @@ -187,7 +187,7 @@ class MoveGroupInterface const std::vector& getNamedTargets() const; /** \brief Get the RobotModel object. */ - robot_model::RobotModelConstPtr getRobotModel() const; + moveit::core::RobotModelConstPtr getRobotModel() const; /** \brief Get the ROS node handle of this instance operates on */ const ros::NodeHandle& getNodeHandle() const; @@ -303,7 +303,7 @@ class MoveGroupInterface /** \brief If a different start state should be considered instead of the current state of the robot, this function * sets that state */ - void setStartState(const robot_state::RobotState& start_state); + void setStartState(const moveit::core::RobotState& start_state); /** \brief Set the starting state for planning to be that reported by the robot's joint state publication */ void setStartStateToCurrentState(); @@ -386,7 +386,7 @@ class MoveGroupInterface If these values are out of bounds then false is returned BUT THE VALUES ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const robot_state::RobotState& robot_state); + bool setJointValueTarget(const moveit::core::RobotState& robot_state); /** \brief Set the JointValueTarget and use it for future planning requests. @@ -516,7 +516,7 @@ class MoveGroupInterface void getJointValueTarget(std::vector& group_variable_values) const; /// Get the currently set joint state goal, replaced by private getTargetRobotState() - [[deprecated]] const robot_state::RobotState& getJointValueTarget() const; + [[deprecated]] const moveit::core::RobotState& getJointValueTarget() const; /**@}*/ @@ -906,7 +906,7 @@ class MoveGroupInterface std::vector getCurrentJointValues() const; /** \brief Get the current state of the robot within the duration specified by wait. */ - robot_state::RobotStatePtr getCurrentState(double wait = 1) const; + moveit::core::RobotStatePtr getCurrentState(double wait = 1) const; /** \brief Get the pose for the end-effector \e end_effector_link. If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is @@ -994,7 +994,7 @@ class MoveGroupInterface protected: /** return the full RobotState of the joint-space target, only for internal use */ - const robot_state::RobotState& getTargetRobotState() const; + const moveit::core::RobotState& getTargetRobotState() const; private: std::map > remembered_joint_values_; diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 6069606410..35a9d39352 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -113,7 +113,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_); - joint_state_target_.reset(new robot_state::RobotState(getRobotModel())); + joint_state_target_.reset(new moveit::core::RobotState(getRobotModel())); joint_state_target_->setToDefaultValues(); active_target_ = JOINT; can_look_ = false; @@ -248,12 +248,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return opt_; } - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } - const robot_model::JointModelGroup* getJointModelGroup() const + const moveit::core::JointModelGroup* getJointModelGroup() const { return joint_model_group_; } @@ -345,19 +345,19 @@ class MoveGroupInterface::MoveGroupInterfaceImpl max_acceleration_scaling_factor_ = max_acceleration_scaling_factor; } - robot_state::RobotState& getTargetRobotState() + moveit::core::RobotState& getTargetRobotState() { return *joint_state_target_; } - const robot_state::RobotState& getTargetRobotState() const + const moveit::core::RobotState& getTargetRobotState() const { return *joint_state_target_; } - void setStartState(const robot_state::RobotState& start_state) + void setStartState(const moveit::core::RobotState& start_state) { - considered_start_state_.reset(new robot_state::RobotState(start_state)); + considered_start_state_.reset(new moveit::core::RobotState(start_state)); } void setStartStateToCurrentState() @@ -365,13 +365,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl considered_start_state_.reset(); } - robot_state::RobotStatePtr getStartState() + moveit::core::RobotStatePtr getStartState() { if (considered_start_state_) return considered_start_state_; else { - robot_state::RobotStatePtr s; + moveit::core::RobotStatePtr s; getCurrentState(s); return s; } @@ -560,7 +560,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return true; } - bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds = 1.0) + bool getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds = 1.0) { if (!current_state_monitor_) { @@ -842,7 +842,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl moveit_msgs::GetCartesianPath::Response res; if (considered_start_state_) - robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state); + moveit::core::robotStateToRobotStateMsg(*considered_start_state_, req.start_state); else req.start_state.is_diff = true; @@ -1009,7 +1009,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.workspace_parameters = workspace_parameters_; if (considered_start_state_) - robot_state::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); + moveit::core::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); else request.start_state.is_diff = true; @@ -1226,7 +1226,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl Options opt_; ros::NodeHandle node_handle_; std::shared_ptr tf_buffer_; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; std::unique_ptr > move_action_client_; std::unique_ptr > execute_action_client_; @@ -1234,7 +1234,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl std::unique_ptr > place_action_client_; // general planning params - robot_state::RobotStatePtr considered_start_state_; + moveit::core::RobotStatePtr considered_start_state_; moveit_msgs::WorkspaceParameters workspace_parameters_; double allowed_planning_time_; std::string planner_id_; @@ -1249,8 +1249,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl double replan_delay_; // joint state goal - robot_state::RobotStatePtr joint_state_target_; - const robot_model::JointModelGroup* joint_model_group_; + moveit::core::RobotStatePtr joint_state_target_; + const moveit::core::JointModelGroup* joint_model_group_; // pose goal; // for each link we have a set of possible goal locations; @@ -1340,7 +1340,7 @@ const std::vector& MoveGroupInterface::getNamedTargets() const return impl_->getJointModelGroup()->getDefaultStateNames(); } -robot_model::RobotModelConstPtr MoveGroupInterface::getRobotModel() const +moveit::core::RobotModelConstPtr MoveGroupInterface::getRobotModel() const { return impl_->getRobotModel(); } @@ -1501,13 +1501,13 @@ void MoveGroupInterface::stop() void MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state) { - robot_state::RobotStatePtr rs; + moveit::core::RobotStatePtr rs; impl_->getCurrentState(rs); - robot_state::robotStateMsgToRobotState(start_state, *rs); + moveit::core::robotStateMsgToRobotState(start_state, *rs); setStartState(*rs); } -void MoveGroupInterface::setStartState(const robot_state::RobotState& start_state) +void MoveGroupInterface::setStartState(const moveit::core::RobotState& start_state) { impl_->setStartState(start_state); } @@ -1623,7 +1623,7 @@ bool MoveGroupInterface::setJointValueTarget(const std::vector& var return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance()); } -bool MoveGroupInterface::setJointValueTarget(const robot_state::RobotState& rstate) +bool MoveGroupInterface::setJointValueTarget(const moveit::core::RobotState& rstate) { impl_->setTargetType(JOINT); impl_->getTargetRobotState() = rstate; @@ -1639,7 +1639,7 @@ bool MoveGroupInterface::setJointValueTarget(const std::string& joint_name, doub bool MoveGroupInterface::setJointValueTarget(const std::string& joint_name, const std::vector& values) { impl_->setTargetType(JOINT); - const robot_model::JointModel* jm = impl_->getJointModelGroup()->getJointModel(joint_name); + const moveit::core::JointModel* jm = impl_->getJointModelGroup()->getJointModel(joint_name); if (jm && jm->getVariableCount() == values.size()) { impl_->getTargetRobotState().setJointPositions(jm, values); @@ -1691,12 +1691,12 @@ bool MoveGroupInterface::setApproximateJointValueTarget(const Eigen::Isometry3d& return setApproximateJointValueTarget(msg, end_effector_link); } -const robot_state::RobotState& MoveGroupInterface::getJointValueTarget() const +const moveit::core::RobotState& MoveGroupInterface::getJointValueTarget() const { return impl_->getTargetRobotState(); } -const robot_state::RobotState& MoveGroupInterface::getTargetRobotState() const +const moveit::core::RobotState& MoveGroupInterface::getTargetRobotState() const { return impl_->getTargetRobotState(); } @@ -1722,7 +1722,7 @@ bool MoveGroupInterface::setEndEffectorLink(const std::string& link_name) bool MoveGroupInterface::setEndEffector(const std::string& eef_name) { - const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name); + const moveit::core::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name); if (jmg) return setEndEffectorLink(jmg->getEndEffectorParentGroup().second); return false; @@ -1965,7 +1965,7 @@ bool MoveGroupInterface::startStateMonitor(double wait) std::vector MoveGroupInterface::getCurrentJointValues() const { - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; std::vector values; if (impl_->getCurrentState(current_state)) current_state->copyJointGroupPositions(getName(), values); @@ -1988,11 +1988,11 @@ geometry_msgs::PoseStamped MoveGroupInterface::getRandomPose(const std::string& ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); else { - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; if (impl_->getCurrentState(current_state)) { current_state->setToRandomPositions(impl_->getJointModelGroup()); - const robot_model::LinkModel* lm = current_state->getLinkModel(eef); + const moveit::core::LinkModel* lm = current_state->getLinkModel(eef); if (lm) pose = current_state->getGlobalLinkTransform(lm); } @@ -2013,10 +2013,10 @@ geometry_msgs::PoseStamped MoveGroupInterface::getCurrentPose(const std::string& ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); else { - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; if (impl_->getCurrentState(current_state)) { - const robot_model::LinkModel* lm = current_state->getLinkModel(eef); + const moveit::core::LinkModel* lm = current_state->getLinkModel(eef); if (lm) pose = current_state->getGlobalLinkTransform(lm); } @@ -2036,10 +2036,10 @@ std::vector MoveGroupInterface::getCurrentRPY(const std::string& end_eff ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); else { - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; if (impl_->getCurrentState(current_state)) { - const robot_model::LinkModel* lm = current_state->getLinkModel(eef); + const moveit::core::LinkModel* lm = current_state->getLinkModel(eef); if (lm) { result.resize(3); @@ -2070,9 +2070,9 @@ unsigned int MoveGroupInterface::getVariableCount() const return impl_->getJointModelGroup()->getVariableCount(); } -robot_state::RobotStatePtr MoveGroupInterface::getCurrentState(double wait) const +moveit::core::RobotStatePtr MoveGroupInterface::getCurrentState(double wait) const { - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; impl_->getCurrentState(current_state, wait); return current_state; } diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index f12bd4be97..94639c6f9c 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -130,7 +130,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer py_bindings_tools::ByteString getJointValueTarget() { moveit_msgs::RobotState msg; - const robot_state::RobotState state = moveit::planning_interface::MoveGroupInterface::getTargetRobotState(); + const moveit::core::RobotState state = moveit::planning_interface::MoveGroupInterface::getTargetRobotState(); moveit::core::robotStateToRobotStateMsg(state, msg); return py_bindings_tools::serializeMsg(msg); } @@ -306,10 +306,10 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer bp::dict getCurrentStateBoundedPython() { - robot_state::RobotStatePtr current = getCurrentState(); + moveit::core::RobotStatePtr current = getCurrentState(); current->enforceBounds(); moveit_msgs::RobotState rsmv; - robot_state::robotStateToRobotStateMsg(*current, rsmv); + moveit::core::robotStateToRobotStateMsg(*current, rsmv); bp::dict output; for (size_t x = 0; x < rsmv.joint_state.name.size(); ++x) output[rsmv.joint_state.name[x]] = rsmv.joint_state.position[x]; @@ -549,7 +549,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer if (ref.size() != 3) throw std::invalid_argument("reference point needs to have 3 elements, got " + std::to_string(ref.size())); - robot_state::RobotState state(getRobotModel()); + moveit::core::RobotState state(getRobotModel()); state.setToDefaultValues(); auto group = state.getJointModelGroup(getName()); state.setJointGroupPositions(group, v); diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index affeceb1a7..72eb43cb2a 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -127,18 +127,18 @@ class MoveItCpp ~MoveItCpp(); /** \brief Get the RobotModel object. */ - robot_model::RobotModelConstPtr getRobotModel() const; + moveit::core::RobotModelConstPtr getRobotModel() const; /** \brief Get the ROS node handle of this instance operates on */ const ros::NodeHandle& getNodeHandle() const; /** \brief Get the current state queried from the current state monitor \param wait_seconds the time in seconds for the state monitor to wait for a robot state. */ - bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds); + bool getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds); /** \brief Get the current state queried from the current state monitor \param wait_seconds the time in seconds for the state monitor to wait for a robot state. */ - robot_state::RobotStatePtr getCurrentState(double wait_seconds = 0.0); + moveit::core::RobotStatePtr getCurrentState(double wait_seconds = 0.0); /** \brief Get all loaded planning pipeline instances mapped to their reference names */ const std::map& getPlanningPipelines() const; @@ -169,7 +169,7 @@ class MoveItCpp private: // Core properties and instances ros::NodeHandle node_handle_; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; // Planning diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 51032ee3ec..cc650fbae4 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -151,10 +151,10 @@ class PlanningComponent void unsetWorkspace(); /** \brief Get the considered start state if defined, otherwise get the current state */ - robot_state::RobotStatePtr getStartState(); + moveit::core::RobotStatePtr getStartState(); /** \brief Set the robot state that should be considered as start state for planning */ - bool setStartState(const robot_state::RobotState& start_state); + bool setStartState(const moveit::core::RobotState& start_state); /** \brief Set the named robot state that should be considered as start state for planning */ bool setStartState(const std::string& named_state); @@ -164,7 +164,7 @@ class PlanningComponent /** \brief Set the goal constraints used for planning */ bool setGoal(const std::vector& goal_constraints); /** \brief Set the goal constraints generated from a target state */ - bool setGoal(const robot_state::RobotState& goal_state); + bool setGoal(const moveit::core::RobotState& goal_state); /** \brief Set the goal constraints generated from target pose and robot link */ bool setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name); /** \brief Set the goal constraints generated from a named target state */ @@ -195,7 +195,7 @@ class PlanningComponent // Planning std::set planning_pipeline_names_; // The start state used in the planning motion request - robot_state::RobotStatePtr considered_start_state_; + moveit::core::RobotStatePtr considered_start_state_; std::vector current_goal_constraints_; PlanRequestParameters plan_request_parameters_; moveit_msgs::WorkspaceParameters workspace_parameters_; diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index a53c5d5f80..941aab5613 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -203,7 +203,7 @@ bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options) return true; } -robot_model::RobotModelConstPtr MoveItCpp::getRobotModel() const +moveit::core::RobotModelConstPtr MoveItCpp::getRobotModel() const { return robot_model_; } @@ -213,7 +213,7 @@ const ros::NodeHandle& MoveItCpp::getNodeHandle() const return node_handle_; } -bool MoveItCpp::getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds) +bool MoveItCpp::getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds) { if (wait_seconds > 0.0 && !planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), wait_seconds)) @@ -228,9 +228,9 @@ bool MoveItCpp::getCurrentState(robot_state::RobotStatePtr& current_state, doubl return true; } -robot_state::RobotStatePtr MoveItCpp::getCurrentState(double wait) +moveit::core::RobotStatePtr MoveItCpp::getCurrentState(double wait) { - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; getCurrentState(current_state, wait); return current_state; } diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp index ea57f7a833..68a60dc2b9 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -219,19 +219,19 @@ PlanningComponent::PlanSolution PlanningComponent::plan() return plan(default_parameters); } -bool PlanningComponent::setStartState(const robot_state::RobotState& start_state) +bool PlanningComponent::setStartState(const moveit::core::RobotState& start_state) { - considered_start_state_.reset(new robot_state::RobotState(start_state)); + considered_start_state_.reset(new moveit::core::RobotState(start_state)); return true; } -robot_state::RobotStatePtr PlanningComponent::getStartState() +moveit::core::RobotStatePtr PlanningComponent::getStartState() { if (considered_start_state_) return considered_start_state_; else { - robot_state::RobotStatePtr s; + moveit::core::RobotStatePtr s; moveit_cpp_->getCurrentState(s, 1.0); return s; } @@ -245,7 +245,7 @@ bool PlanningComponent::setStartState(const std::string& start_state_name) ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", start_state_name.c_str()); return false; } - robot_state::RobotState start_state(moveit_cpp_->getRobotModel()); + moveit::core::RobotState start_state(moveit_cpp_->getRobotModel()); start_state.setToDefaultValues(joint_model_group_, start_state_name); return setStartState(start_state); } @@ -287,7 +287,7 @@ bool PlanningComponent::setGoal(const std::vector& goa return true; } -bool PlanningComponent::setGoal(const robot_state::RobotState& goal_state) +bool PlanningComponent::setGoal(const moveit::core::RobotState& goal_state) { current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_) }; return true; @@ -307,7 +307,7 @@ bool PlanningComponent::setGoal(const std::string& goal_state_name) ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", goal_state_name.c_str()); return false; } - robot_state::RobotState goal_state(moveit_cpp_->getRobotModel()); + moveit::core::RobotState goal_state(moveit_cpp_->getRobotModel()); goal_state.setToDefaultValues(joint_model_group_, goal_state_name); return setGoal(goal_state); } diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index aaec69288f..704372d6ec 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -267,7 +267,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl ros::ServiceClient planning_scene_service_; ros::ServiceClient apply_planning_scene_service_; ros::Publisher planning_scene_diff_publisher_; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; }; PlanningSceneInterface::PlanningSceneInterface(const std::string& ns) diff --git a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp index be238104f4..f21e58127c 100644 --- a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp +++ b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp @@ -77,7 +77,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list getGroupJointNames(const std::string& group) const { - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (jmg) return py_bindings_tools::listFromString(jmg->getJointModelNames()); else @@ -86,7 +86,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list getGroupJointTips(const std::string& group) const { - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (jmg) { std::vector tips; @@ -104,7 +104,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list getGroupLinkNames(const std::string& group) const { - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (jmg) return py_bindings_tools::listFromString(jmg->getLinkModelNames()); else @@ -119,7 +119,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list getJointLimits(const std::string& name) const { bp::list result; - const robot_model::JointModel* jm = robot_model_->getJointModel(name); + const moveit::core::JointModel* jm = robot_model_->getJointModel(name); if (jm) { const std::vector& lim = jm->getVariableBoundsMsg(); @@ -144,8 +144,8 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list l; if (!ensureCurrentState()) return l; - robot_state::RobotStatePtr state = current_state_monitor_->getCurrentState(); - const robot_model::LinkModel* lm = state->getLinkModel(name); + moveit::core::RobotStatePtr state = current_state_monitor_->getCurrentState(); + const moveit::core::LinkModel* lm = state->getLinkModel(name); if (lm) { const Eigen::Isometry3d& t = state->getGlobalLinkTransform(lm); @@ -166,7 +166,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list getDefaultStateNames(const std::string& group) { bp::list l; - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (jmg) { for (auto& known_state : jmg->getDefaultStateNames()) @@ -182,8 +182,8 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list l; if (!ensureCurrentState()) return l; - robot_state::RobotStatePtr state = current_state_monitor_->getCurrentState(); - const robot_model::JointModel* jm = state->getJointModel(name); + moveit::core::RobotStatePtr state = current_state_monitor_->getCurrentState(); + const moveit::core::JointModel* jm = state->getJointModel(name); if (jm) { const double* pos = state->getJointPositions(jm); @@ -197,7 +197,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::dict getJointValues(const std::string& group, const std::string& named_state) { - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg) return boost::python::dict(); std::map values; @@ -227,9 +227,9 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer { if (!ensureCurrentState()) return py_bindings_tools::ByteString(""); - robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); + moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); moveit_msgs::RobotState msg; - robot_state::robotStateToRobotStateMsg(*s, msg); + moveit::core::robotStateToRobotStateMsg(*s, msg); return py_bindings_tools::serializeMsg(msg); } @@ -237,7 +237,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer { // name of the group that is parent to this end-effector group; // Second: the link this in the parent group that this group attaches to - const robot_state::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg) return boost::python::make_tuple("", ""); std::pair parent_group = jmg->getEndEffectorParentGroup(); @@ -246,14 +246,14 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer py_bindings_tools::ByteString getRobotMarkersPythonDictList(bp::dict& values, bp::list& links) { - robot_state::RobotStatePtr state; + moveit::core::RobotStatePtr state; if (ensureCurrentState()) { state = current_state_monitor_->getCurrentState(); } else { - state.reset(new robot_state::RobotState(robot_model_)); + state.reset(new moveit::core::RobotState(robot_model_)); } bp::list k = values.keys(); @@ -282,7 +282,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer py_bindings_tools::ByteString getRobotMarkersFromMsg(const py_bindings_tools::ByteString& state_str) { moveit_msgs::RobotState state_msg; - robot_state::RobotState state(robot_model_); + moveit::core::RobotState state(robot_model_); py_bindings_tools::deserializeMsg(state_str, state_msg); moveit::core::robotStateMsgToRobotState(state_msg, state); @@ -296,7 +296,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer { if (!ensureCurrentState()) return py_bindings_tools::ByteString(); - robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); + moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); visualization_msgs::MarkerArray msg; s->getRobotMarkers(msg, s->getRobotModel()->getLinkModelNames()); @@ -307,7 +307,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer { if (!ensureCurrentState()) return py_bindings_tools::ByteString(""); - robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); + moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); visualization_msgs::MarkerArray msg; s->getRobotMarkers(msg, py_bindings_tools::stringFromList(links)); @@ -318,8 +318,8 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer { if (!ensureCurrentState()) return py_bindings_tools::ByteString(""); - robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); visualization_msgs::MarkerArray msg; if (jmg) { @@ -331,7 +331,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer py_bindings_tools::ByteString getRobotMarkersGroupPythonDict(const std::string& group, bp::dict& values) { - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg) return py_bindings_tools::ByteString(""); bp::list links = py_bindings_tools::listFromString(jmg->getLinkModelNames()); @@ -363,7 +363,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer } private: - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; ros::NodeHandle nh_; }; diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp index 675d7c761b..d1430075e0 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp @@ -84,7 +84,7 @@ class MoveItCppTest : public ::testing::Test ros::NodeHandle nh_; MoveItCppPtr moveit_cpp_ptr; PlanningComponentPtr planning_component_ptr; - robot_model::RobotModelConstPtr robot_model_ptr; + moveit::core::RobotModelConstPtr robot_model_ptr; const moveit::core::JointModelGroup* jmg_ptr; const std::string PLANNING_GROUP = "panda_arm"; geometry_msgs::PoseStamped target_pose1; @@ -97,7 +97,7 @@ TEST_F(MoveItCppTest, GetCurrentStateTest) { ros::Duration(1.0).sleep(); // Otherwise joint_states will result in an invalid robot state auto robot_model = moveit_cpp_ptr->getRobotModel(); - auto robot_state = std::make_shared(robot_model); + auto robot_state = std::make_shared(robot_model); EXPECT_TRUE(moveit_cpp_ptr->getCurrentState(robot_state, 0.0)); // Make sure the Panda robot is in "ready" state which is loaded from fake_controller.yaml std::vector joints_vals; @@ -135,7 +135,7 @@ TEST_F(MoveItCppTest, TestSetGoalFromPoseStamped) ASSERT_TRUE(static_cast(planning_component_ptr->plan())); } -// Test setting the plan start state using robot_state::RobotState +// Test setting the plan start state using moveit::core::RobotState TEST_F(MoveItCppTest, TestSetStartStateFromRobotState) { auto start_state = *(moveit_cpp_ptr->getCurrentState()); @@ -147,7 +147,7 @@ TEST_F(MoveItCppTest, TestSetStartStateFromRobotState) ASSERT_TRUE(static_cast(planning_component_ptr->plan())); } -// Test settting the goal of the plan using a robot_state::RobotState +// Test settting the goal of the plan using a moveit::core::RobotState TEST_F(MoveItCppTest, TestSetGoalFromRobotState) { auto target_state = *(moveit_cpp_ptr->getCurrentState()); diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h index b0027cf0f2..3dc50a10ea 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h @@ -84,7 +84,7 @@ enum InteractionStyle /// that will be used to control the interaction. /// @returns true if the function succeeds, false if the function was not able /// to fill in \e marker. -typedef boost::function +typedef boost::function InteractiveMarkerConstructorFn; /// Type of function for processing marker feedback. @@ -98,7 +98,7 @@ typedef boost::function ProcessFeedbackFn; @@ -111,7 +111,7 @@ typedef boost::function InteractiveMarkerUpdateFn; +typedef boost::function InteractiveMarkerUpdateFn; /// Representation of a generic interaction. /// Displays one interactive marker. diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h index 3ea644e86a..e6a07c1d83 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h @@ -77,7 +77,7 @@ class InteractionHandler : public LockedRobotState public: // Use this constructor if you have an initial RobotState already. InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name, - const robot_state::RobotState& initial_robot_state, + const moveit::core::RobotState& initial_robot_state, const std::shared_ptr& tf_buffer = std::shared_ptr()); // Use this constructor to start with a default state. @@ -85,10 +85,10 @@ class InteractionHandler : public LockedRobotState const std::shared_ptr& tf_buffer = std::shared_ptr()); // DEPRECATED. - InteractionHandler(const std::string& name, const robot_state::RobotState& initial_robot_state, + InteractionHandler(const std::string& name, const moveit::core::RobotState& initial_robot_state, const std::shared_ptr& tf_buffer = std::shared_ptr()); // DEPRECATED. - InteractionHandler(const std::string& name, const robot_model::RobotModelConstPtr& model, + InteractionHandler(const std::string& name, const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& tf_buffer = std::shared_ptr()); ~InteractionHandler() override @@ -230,18 +230,18 @@ class InteractionHandler : public LockedRobotState // Update RobotState using a generic interaction feedback message. // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. - void updateStateGeneric(robot_state::RobotState* state, const GenericInteraction* g, + void updateStateGeneric(moveit::core::RobotState* state, const GenericInteraction* g, const visualization_msgs::InteractiveMarkerFeedbackConstPtr* feedback, StateChangeCallbackFn* callback); // Update RobotState for a new pose of an eef. // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. - void updateStateEndEffector(robot_state::RobotState* state, const EndEffectorInteraction* eef, + void updateStateEndEffector(moveit::core::RobotState* state, const EndEffectorInteraction* eef, const geometry_msgs::Pose* pose, StateChangeCallbackFn* callback); // Update RobotState for a new joint position. // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. - void updateStateJoint(robot_state::RobotState* state, const JointInteraction* vj, const geometry_msgs::Pose* pose, + void updateStateJoint(moveit::core::RobotState* state, const JointInteraction* vj, const geometry_msgs::Pose* pose, StateChangeCallbackFn* callback); // Set the error state for \e name. diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h index 8a0248a60a..0ba6699d47 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h @@ -70,7 +70,7 @@ struct KinematicOptions /// @param tip link that will be posed /// @param pose desired pose of tip link /// @param result true if IK succeeded. - bool setStateFromIK(robot_state::RobotState& state, const std::string& group, const std::string& tip, + bool setStateFromIK(moveit::core::RobotState& state, const std::string& group, const std::string& tip, const geometry_msgs::Pose& pose) const; /// Copy a subset of source to this. @@ -82,7 +82,7 @@ struct KinematicOptions double timeout_seconds_; /// This is called to determine if the state is valid - robot_state::GroupStateValidityCallbackFn state_validity_callback_; + moveit::core::GroupStateValidityCallbackFn state_validity_callback_; /// other options kinematics::KinematicsQueryOptions options_; diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h index f9c6a986b9..f0e6d807e1 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h @@ -63,7 +63,7 @@ class KinematicOptionsMap /// @param tip link that will be posed /// @param pose desired pose of tip link /// @param result true if IK succeeded. - bool setStateFromIK(robot_state::RobotState& state, const std::string& key, const std::string& group, + bool setStateFromIK(moveit::core::RobotState& state, const std::string& key, const std::string& group, const std::string& tip, const geometry_msgs::Pose& pose) const; /// Get the options to use for a particular key. diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h index fd6b3f3616..488b49057e 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h @@ -60,8 +60,8 @@ MOVEIT_CLASS_FORWARD(LockedRobotState); class LockedRobotState { public: - LockedRobotState(const robot_state::RobotState& state); - LockedRobotState(const robot_model::RobotModelPtr& model); + LockedRobotState(const moveit::core::RobotState& state); + LockedRobotState(const moveit::core::RobotModelPtr& model); virtual ~LockedRobotState(); @@ -72,13 +72,13 @@ class LockedRobotState // it. // // The transforms in the returned state will always be up to date. - robot_state::RobotStateConstPtr getState() const; + moveit::core::RobotStateConstPtr getState() const; /// Set the state to the new value. - void setState(const robot_state::RobotState& state); + void setState(const moveit::core::RobotState& state); // This is a function that can modify the maintained state. - typedef boost::function ModifyStateFunction; + typedef boost::function ModifyStateFunction; // Modify the state. // @@ -105,6 +105,6 @@ class LockedRobotState // The state maintained by this class. // When a modify function is being called this is NULL. // PROTECTED BY state_lock_ - robot_state::RobotStatePtr state_; + moveit::core::RobotStatePtr state_; }; } diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h index 494386af0f..3fc649d93e 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h @@ -76,7 +76,7 @@ class RobotInteraction /// The topic name on which the internal Interactive Marker Server operates static const std::string INTERACTIVE_MARKER_TOPIC; - RobotInteraction(const robot_model::RobotModelConstPtr& robot_model, const std::string& ns = ""); + RobotInteraction(const moveit::core::RobotModelConstPtr& robot_model, const std::string& ns = ""); virtual ~RobotInteraction(); const std::string& getServerTopic(void) const @@ -140,7 +140,7 @@ class RobotInteraction // is needed to actually remove the markers from the display. void clearInteractiveMarkers(); - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -180,7 +180,7 @@ class RobotInteraction // return the diameter of the sphere that certainly can enclose the AABB of the links in this group double computeGroupMarkerSize(const std::string& group); void computeMarkerPose(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef, - const robot_state::RobotState& robot_state, geometry_msgs::Pose& pose, + const moveit::core::RobotState& robot_state, geometry_msgs::Pose& pose, geometry_msgs::Pose& control_to_eef_tf) const; void addEndEffectorMarkers(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef, @@ -199,7 +199,7 @@ class RobotInteraction boost::condition_variable new_feedback_condition_; std::map feedback_map_; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; std::vector active_eef_; std::vector active_vj_; diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index 3c8492fda8..1b76f7a196 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -56,7 +56,7 @@ namespace robot_interaction { InteractionHandler::InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name, - const robot_state::RobotState& initial_robot_state, + const moveit::core::RobotState& initial_robot_state, const std::shared_ptr& tf_buffer) : LockedRobotState(initial_robot_state) , name_(fixName(name)) @@ -279,7 +279,7 @@ void InteractionHandler::handleJoint(const JointInteraction& vj, } // MUST hold state_lock_ when calling this! -void InteractionHandler::updateStateGeneric(robot_state::RobotState* state, const GenericInteraction* g, +void InteractionHandler::updateStateGeneric(moveit::core::RobotState* state, const GenericInteraction* g, const visualization_msgs::InteractiveMarkerFeedbackConstPtr* feedback, StateChangeCallbackFn* callback) { @@ -290,7 +290,7 @@ void InteractionHandler::updateStateGeneric(robot_state::RobotState* state, cons } // MUST hold state_lock_ when calling this! -void InteractionHandler::updateStateEndEffector(robot_state::RobotState* state, const EndEffectorInteraction* eef, +void InteractionHandler::updateStateEndEffector(moveit::core::RobotState* state, const EndEffectorInteraction* eef, const geometry_msgs::Pose* pose, StateChangeCallbackFn* callback) { // This is called with state_lock_ held, so no additional locking needed to @@ -304,13 +304,13 @@ void InteractionHandler::updateStateEndEffector(robot_state::RobotState* state, } // MUST hold state_lock_ when calling this! -void InteractionHandler::updateStateJoint(robot_state::RobotState* state, const JointInteraction* vj, +void InteractionHandler::updateStateJoint(moveit::core::RobotState* state, const JointInteraction* vj, const geometry_msgs::Pose* feedback_pose, StateChangeCallbackFn* callback) { Eigen::Isometry3d pose; tf2::fromMsg(*feedback_pose, pose); - if (!vj->parent_frame.empty() && !robot_state::Transforms::sameFrame(vj->parent_frame, planning_frame_)) + if (!vj->parent_frame.empty() && !moveit::core::Transforms::sameFrame(vj->parent_frame, planning_frame_)) pose = state->getGlobalLinkTransform(vj->parent_frame).inverse() * pose; state->setJointPositions(vj->joint_name, pose); diff --git a/moveit_ros/robot_interaction/src/kinematic_options.cpp b/moveit_ros/robot_interaction/src/kinematic_options.cpp index d55440e60f..67a39eeefc 100644 --- a/moveit_ros/robot_interaction/src/kinematic_options.cpp +++ b/moveit_ros/robot_interaction/src/kinematic_options.cpp @@ -44,10 +44,10 @@ robot_interaction::KinematicOptions::KinematicOptions() : timeout_seconds_(0.0) // This is intended to be called as a ModifyStateFunction to modify the state // maintained by a LockedRobotState in place. -bool robot_interaction::KinematicOptions::setStateFromIK(robot_state::RobotState& state, const std::string& group, +bool robot_interaction::KinematicOptions::setStateFromIK(moveit::core::RobotState& state, const std::string& group, const std::string& tip, const geometry_msgs::Pose& pose) const { - const robot_model::JointModelGroup* jmg = state.getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group); if (!jmg) { ROS_ERROR("No getJointModelGroup('%s') found", group.c_str()); @@ -72,7 +72,7 @@ void robot_interaction::KinematicOptions::setOptions(const KinematicOptions& sou // robot_interaction::KinematicOptions except options_ #define O_FIELDS(F) \ F(double, timeout_seconds_, TIMEOUT) \ - F(robot_state::GroupStateValidityCallbackFn, state_validity_callback_, STATE_VALIDITY_CALLBACK) + F(moveit::core::GroupStateValidityCallbackFn, state_validity_callback_, STATE_VALIDITY_CALLBACK) // This needs to represent all the fields in // kinematics::KinematicsQueryOptions diff --git a/moveit_ros/robot_interaction/src/kinematic_options_map.cpp b/moveit_ros/robot_interaction/src/kinematic_options_map.cpp index 4487e92a45..c3460c0d8a 100644 --- a/moveit_ros/robot_interaction/src/kinematic_options_map.cpp +++ b/moveit_ros/robot_interaction/src/kinematic_options_map.cpp @@ -131,7 +131,7 @@ void robot_interaction::KinematicOptionsMap::merge(const KinematicOptionsMap& ot // This is intended to be called as a ModifyStateFunction to modify the state // maintained by a LockedRobotState in place. -bool robot_interaction::KinematicOptionsMap::setStateFromIK(robot_state::RobotState& state, const std::string& key, +bool robot_interaction::KinematicOptionsMap::setStateFromIK(moveit::core::RobotState& state, const std::string& key, const std::string& group, const std::string& tip, const geometry_msgs::Pose& pose) const { diff --git a/moveit_ros/robot_interaction/src/locked_robot_state.cpp b/moveit_ros/robot_interaction/src/locked_robot_state.cpp index 4ea148e6aa..2576df198f 100644 --- a/moveit_ros/robot_interaction/src/locked_robot_state.cpp +++ b/moveit_ros/robot_interaction/src/locked_robot_state.cpp @@ -38,14 +38,14 @@ #include -robot_interaction::LockedRobotState::LockedRobotState(const robot_state::RobotState& state) - : state_(new robot_state::RobotState(state)) +robot_interaction::LockedRobotState::LockedRobotState(const moveit::core::RobotState& state) + : state_(new moveit::core::RobotState(state)) { state_->update(); } -robot_interaction::LockedRobotState::LockedRobotState(const robot_model::RobotModelPtr& model) - : state_(new robot_state::RobotState(model)) +robot_interaction::LockedRobotState::LockedRobotState(const moveit::core::RobotModelPtr& model) + : state_(new moveit::core::RobotState(model)) { state_->setToDefaultValues(); state_->update(); @@ -53,13 +53,13 @@ robot_interaction::LockedRobotState::LockedRobotState(const robot_model::RobotMo robot_interaction::LockedRobotState::~LockedRobotState() = default; -robot_state::RobotStateConstPtr robot_interaction::LockedRobotState::getState() const +moveit::core::RobotStateConstPtr robot_interaction::LockedRobotState::getState() const { boost::mutex::scoped_lock lock(state_lock_); return state_; } -void robot_interaction::LockedRobotState::setState(const robot_state::RobotState& state) +void robot_interaction::LockedRobotState::setState(const moveit::core::RobotState& state) { { boost::mutex::scoped_lock lock(state_lock_); @@ -69,7 +69,7 @@ void robot_interaction::LockedRobotState::setState(const robot_state::RobotState if (state_.unique()) *state_ = state; else - state_.reset(new robot_state::RobotState(state)); + state_.reset(new moveit::core::RobotState(state)); state_->update(); } @@ -84,7 +84,7 @@ void robot_interaction::LockedRobotState::modifyState(const ModifyStateFunction& // If someone else has a reference to the state, then make a copy. // The old state is orphaned (does not change, but is now out of date). if (!state_.unique()) - state_.reset(new robot_state::RobotState(*state_)); + state_.reset(new moveit::core::RobotState(*state_)); modify(state_.get()); state_->update(); diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 698b4ddd3b..9483f9c4cb 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -62,7 +62,7 @@ static const float END_EFFECTOR_REACHABLE_COLOR[4] = { 0.2, 1.0, 0.2, 1.0 }; const std::string RobotInteraction::INTERACTIVE_MARKER_TOPIC = "robot_interaction_interactive_marker_topic"; -RobotInteraction::RobotInteraction(const robot_model::RobotModelConstPtr& robot_model, const std::string& ns) +RobotInteraction::RobotInteraction(const moveit::core::RobotModelConstPtr& robot_model, const std::string& ns) : robot_model_(robot_model), kinematic_options_map_(new KinematicOptionsMap) { topic_ = ns.empty() ? INTERACTIVE_MARKER_TOPIC : ns + "/" + INTERACTIVE_MARKER_TOPIC; @@ -115,7 +115,7 @@ void RobotInteraction::addActiveComponent(const InteractiveMarkerConstructorFn& static const double DEFAULT_SCALE = 0.25; double RobotInteraction::computeLinkMarkerSize(const std::string& link) { - const robot_model::LinkModel* lm = robot_model_->getLinkModel(link); + const moveit::core::LinkModel* lm = robot_model_->getLinkModel(link); double size = 0; while (lm) @@ -131,7 +131,7 @@ double RobotInteraction::computeLinkMarkerSize(const std::string& link) // process kinematic chain upwards (but only following fixed joints) // to find a link with some non-empty shape (to ignore virtual links) - if (lm->getParentJointModel()->getType() == robot_model::JointModel::FIXED) + if (lm->getParentJointModel()->getType() == moveit::core::JointModel::FIXED) lm = lm->getParentLinkModel(); else lm = nullptr; @@ -147,7 +147,7 @@ double RobotInteraction::computeGroupMarkerSize(const std::string& group) { if (group.empty()) return DEFAULT_SCALE; - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg) return 0.0; @@ -159,7 +159,7 @@ double RobotInteraction::computeGroupMarkerSize(const std::string& group) double size = 0; for (const std::string& link : links) { - const robot_model::LinkModel* lm = robot_model_->getLinkModel(link); + const moveit::core::LinkModel* lm = robot_model_->getLinkModel(link); if (!lm) continue; Eigen::Vector3d ext = lm->getShapeExtentsAtOrigin(); @@ -191,7 +191,7 @@ void RobotInteraction::decideActiveJoints(const std::string& group) ROS_DEBUG_NAMED("robot_interaction", "Deciding active joints for group '%s'", group.c_str()); const srdf::ModelConstSharedPtr& srdf = robot_model_->getSRDF(); - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg || !srdf) return; @@ -199,7 +199,7 @@ void RobotInteraction::decideActiveJoints(const std::string& group) std::set used; if (jmg->hasJointModel(robot_model_->getRootJointName())) { - robot_state::RobotState default_state(robot_model_); + moveit::core::RobotState default_state(robot_model_); default_state.setToDefaultValues(); std::vector aabb; default_state.computeAABB(aabb); @@ -228,11 +228,11 @@ void RobotInteraction::decideActiveJoints(const std::string& group) } } - const std::vector& joints = jmg->getJointModels(); - for (const robot_model::JointModel* joint : joints) + const std::vector& joints = jmg->getJointModels(); + for (const moveit::core::JointModel* joint : joints) { - if ((joint->getType() == robot_model::JointModel::PLANAR || - joint->getType() == robot_model::JointModel::FLOATING) && + if ((joint->getType() == moveit::core::JointModel::PLANAR || + joint->getType() == moveit::core::JointModel::FLOATING) && used.find(joint->getName()) == used.end()) { JointInteraction v; @@ -240,7 +240,7 @@ void RobotInteraction::decideActiveJoints(const std::string& group) if (joint->getParentLinkModel()) v.parent_frame = joint->getParentLinkModel()->getName(); v.joint_name = joint->getName(); - if (joint->getType() == robot_model::JointModel::PLANAR) + if (joint->getType() == moveit::core::JointModel::PLANAR) v.dof = 3; else v.dof = 6; @@ -267,7 +267,7 @@ void RobotInteraction::decideActiveEndEffectors(const std::string& group, Intera ROS_DEBUG_NAMED("robot_interaction", "Deciding active end-effectors for group '%s'", group.c_str()); const srdf::ModelConstSharedPtr& srdf = robot_model_->getSRDF(); - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg || !srdf) { @@ -276,7 +276,7 @@ void RobotInteraction::decideActiveEndEffectors(const std::string& group, Intera } const std::vector& eefs = srdf->getEndEffectors(); - const std::pair& + const std::pair& smap = jmg->getGroupKinematics(); // if we have an IK solver for the selected group, we check if there are any end effectors attached to this group @@ -400,7 +400,7 @@ void RobotInteraction::addEndEffectorMarkers(const InteractionHandlerPtr& handle marker_color.b = color[2]; marker_color.a = color[3]; - robot_state::RobotStateConstPtr rstate = handler->getState(); + moveit::core::RobotStateConstPtr rstate = handler->getState(); const std::vector& link_names = rstate->getJointModelGroup(eef.eef_group)->getLinkModelNames(); visualization_msgs::MarkerArray marker_array; rstate->getRobotMarkers(marker_array, link_names, marker_color, eef.eef_group, ros::Duration()); @@ -450,7 +450,7 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle std::vector ims; { boost::unique_lock ulock(marker_access_lock_); - robot_state::RobotStateConstPtr s = handler->getState(); + moveit::core::RobotStateConstPtr s = handler->getState(); for (std::size_t i = 0; i < active_generic_.size(); ++i) { @@ -584,7 +584,7 @@ void RobotInteraction::toggleMoveInteractiveMarkerTopic(bool enable) } void RobotInteraction::computeMarkerPose(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef, - const robot_state::RobotState& robot_state, geometry_msgs::Pose& pose, + const moveit::core::RobotState& robot_state, geometry_msgs::Pose& pose, geometry_msgs::Pose& control_to_eef_tf) const { // Need to allow for control pose offsets @@ -619,7 +619,7 @@ void RobotInteraction::updateInteractiveMarkers(const InteractionHandlerPtr& han { boost::unique_lock ulock(marker_access_lock_); - robot_state::RobotStateConstPtr s = handler->getState(); + moveit::core::RobotStateConstPtr s = handler->getState(); root_link = s->getRobotModel()->getModelFrame(); for (const EndEffectorInteraction& eef : active_eef_) diff --git a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp index fd31cd1f7b..dc820d047f 100644 --- a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp +++ b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp @@ -256,7 +256,7 @@ TEST(LockedRobotState, URDF_sanity) class Super1 : public robot_interaction::LockedRobotState { public: - Super1(const robot_model::RobotModelPtr& model) : LockedRobotState(model), cnt_(0) + Super1(const moveit::core::RobotModelPtr& model) : LockedRobotState(model), cnt_(0) { } @@ -268,7 +268,7 @@ class Super1 : public robot_interaction::LockedRobotState int cnt_; }; -static void modify1(robot_state::RobotState* state) +static void modify1(moveit::core::RobotState* state) { state->setVariablePosition(JOINT_A, 0.00006); } @@ -281,7 +281,7 @@ TEST(LockedRobotState, robotStateChanged) EXPECT_EQ(ls1.cnt_, 0); - robot_state::RobotState cp1(*ls1.getState()); + moveit::core::RobotState cp1(*ls1.getState()); cp1.setVariablePosition(JOINT_A, 0.00001); cp1.setVariablePosition(JOINT_C, 0.00002); cp1.setVariablePosition(JOINT_F, 0.00003); @@ -319,7 +319,7 @@ class MyInfo private: // helper function for modifyThreadFunc - void modifyFunc(robot_state::RobotState* state, double val); + void modifyFunc(moveit::core::RobotState* state, double val); // Checks state for validity and self-consistancy. void checkState(robot_interaction::LockedRobotState& locked_state); @@ -335,9 +335,9 @@ class MyInfo // Check the state. It should always be valid. void MyInfo::checkState(robot_interaction::LockedRobotState& locked_state) { - robot_state::RobotStateConstPtr s = locked_state.getState(); + moveit::core::RobotStateConstPtr s = locked_state.getState(); - robot_state::RobotState cp1(*s); + moveit::core::RobotState cp1(*s); // take some time cnt_lock_.lock(); @@ -350,7 +350,7 @@ void MyInfo::checkState(robot_interaction::LockedRobotState& locked_state) // check mim_f == joint_f EXPECT_EQ(s->getVariablePositions()[MIM_F], s->getVariablePositions()[JOINT_F] * 1.5 + 0.1); - robot_state::RobotState cp2(*s); + moveit::core::RobotState cp2(*s); EXPECT_NE(cp1.getVariablePositions(), cp2.getVariablePositions()); EXPECT_NE(cp1.getVariablePositions(), s->getVariablePositions()); @@ -394,7 +394,7 @@ void MyInfo::setThreadFunc(robot_interaction::LockedRobotState* locked_state, in for (int loops = 0; loops < 100; ++loops) { val += 0.0001; - robot_state::RobotState cp1(*locked_state->getState()); + moveit::core::RobotState cp1(*locked_state->getState()); cp1.setVariablePosition(JOINT_A, val + 0.00001); cp1.setVariablePosition(JOINT_C, val + 0.00002); @@ -415,7 +415,7 @@ void MyInfo::setThreadFunc(robot_interaction::LockedRobotState* locked_state, in } // modify the state in place. Used by MyInfo::modifyThreadFunc() -void MyInfo::modifyFunc(robot_state::RobotState* state, double val) +void MyInfo::modifyFunc(moveit::core::RobotState* state, double val) { state->setVariablePosition(JOINT_A, val + 0.00001); state->setVariablePosition(JOINT_C, val + 0.00002); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index 1df45780c5..9561868774 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -96,17 +96,17 @@ class MotionPlanningDisplay : public PlanningSceneDisplay void setName(const QString& name) override; - robot_state::RobotStateConstPtr getQueryStartState() const + moveit::core::RobotStateConstPtr getQueryStartState() const { return query_start_state_->getState(); } - robot_state::RobotStateConstPtr getQueryGoalState() const + moveit::core::RobotStateConstPtr getQueryGoalState() const { return query_goal_state_->getState(); } - const robot_state::RobotState& getPreviousState() const + const moveit::core::RobotState& getPreviousState() const { return *previous_state_; } @@ -131,8 +131,8 @@ class MotionPlanningDisplay : public PlanningSceneDisplay trajectory_visual_->dropTrajectory(); } - void setQueryStartState(const robot_state::RobotState& start); - void setQueryGoalState(const robot_state::RobotState& goal); + void setQueryStartState(const moveit::core::RobotState& start); + void setQueryGoalState(const moveit::core::RobotState& goal); void updateQueryStartState(); void updateQueryGoalState(); @@ -216,14 +216,14 @@ private Q_SLOTS: void scheduleDrawQueryStartState(robot_interaction::InteractionHandler* handler, bool error_state_changed); void scheduleDrawQueryGoalState(robot_interaction::InteractionHandler* handler, bool error_state_changed); - bool isIKSolutionCollisionFree(robot_state::RobotState* state, const robot_state::JointModelGroup* group, + bool isIKSolutionCollisionFree(moveit::core::RobotState* state, const moveit::core::JointModelGroup* group, const double* ik_solution) const; void computeMetrics(bool start, const std::string& group, double payload); void computeMetricsInternal(std::map& metrics, const robot_interaction::EndEffectorInteraction& eef, - const robot_state::RobotState& state, double payload); - void updateStateExceptModified(robot_state::RobotState& dest, const robot_state::RobotState& src); + const moveit::core::RobotState& state, double payload); + void updateStateExceptModified(moveit::core::RobotState& dest, const moveit::core::RobotState& src); void updateBackgroundJobProgressBar(); void backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent event, const std::string& jobname); @@ -264,7 +264,7 @@ private Q_SLOTS: std::map status_links_start_; std::map status_links_goal_; /// remember previous start state (updated before starting execution) - robot_state::RobotStatePtr previous_state_; + moveit::core::RobotStatePtr previous_state_; /// Hold the names of the groups for which the query states have been updated (and should not be altered when new info /// is received from the planning scene) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 94d4c5c171..e228fcec58 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -235,7 +235,7 @@ private Q_SLOTS: void populateConstraintsList(const std::vector& constr); void configureForPlanning(); void configureWorkspace(); - void updateQueryStateHelper(robot_state::RobotState& state, const std::string& v); + void updateQueryStateHelper(moveit::core::RobotState& state, const std::string& v); void fillStateSelectionOptions(); void fillPlanningGroupOptions(); void startStateTextChangedExec(const std::string& start_state); @@ -266,7 +266,7 @@ private Q_SLOTS: void checkPlanningSceneTreeEnabledButtons(); // States tab - void saveRobotStateButtonClicked(const robot_state::RobotState& state); + void saveRobotStateButtonClicked(const moveit::core::RobotState& state); void populateRobotStatesList(); // Pick and place diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 06d40e9fe8..2ce01bcd6f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -488,7 +488,7 @@ void MotionPlanningDisplay::computeMetrics(bool start, const std::string& group, return; boost::mutex::scoped_lock slock(update_metrics_lock_); - robot_state::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState(); + moveit::core::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState(); for (const robot_interaction::EndEffectorInteraction& ee : eef) if (ee.parent_group == group) computeMetricsInternal(computed_metrics_[std::make_pair(start, group)], ee, *state, payload); @@ -496,7 +496,7 @@ void MotionPlanningDisplay::computeMetrics(bool start, const std::string& group, void MotionPlanningDisplay::computeMetricsInternal(std::map& metrics, const robot_interaction::EndEffectorInteraction& ee, - const robot_state::RobotState& state, double payload) + const moveit::core::RobotState& state, double payload) { metrics.clear(); dynamics_solver::DynamicsSolverPtr ds; @@ -564,7 +564,7 @@ void MotionPlanningDisplay::displayMetrics(bool start) if (eef.empty()) return; - robot_state::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState(); + moveit::core::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState(); for (const robot_interaction::EndEffectorInteraction& ee : eef) { @@ -591,8 +591,8 @@ void MotionPlanningDisplay::displayMetrics(bool start) } } - const robot_state::LinkModel* lm = nullptr; - const robot_model::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(ee.parent_group); + const moveit::core::LinkModel* lm = nullptr; + const moveit::core::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(ee.parent_group); if (jmg) if (!jmg->getLinkModelNames().empty()) lm = state->getLinkModel(jmg->getLinkModelNames().back()); @@ -620,7 +620,7 @@ void MotionPlanningDisplay::drawQueryStartState() { if (isEnabled()) { - robot_state::RobotStateConstPtr state = getQueryStartState(); + moveit::core::RobotStateConstPtr state = getQueryStartState(); // update link poses query_robot_start_->update(state); @@ -645,11 +645,11 @@ void MotionPlanningDisplay::drawQueryStartState() } if (!getCurrentPlanningGroup().empty()) { - const robot_model::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup()); + const moveit::core::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup()); if (jmg) { std::vector outside_bounds; - const std::vector& jmodels = jmg->getActiveJointModels(); + const std::vector& jmodels = jmg->getActiveJointModels(); for (const moveit::core::JointModel* jmodel : jmodels) if (!state->satisfiesBounds(jmodel, jmodel->getMaximumExtent() * 1e-2)) { @@ -741,7 +741,7 @@ void MotionPlanningDisplay::drawQueryGoalState() { if (isEnabled()) { - robot_state::RobotStateConstPtr state = getQueryGoalState(); + moveit::core::RobotStateConstPtr state = getQueryGoalState(); // update link poses query_robot_goal_->update(state); @@ -767,10 +767,10 @@ void MotionPlanningDisplay::drawQueryGoalState() if (!getCurrentPlanningGroup().empty()) { - const robot_model::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup()); + const moveit::core::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup()); if (jmg) { - const std::vector& jmodels = jmg->getActiveJointModels(); + const std::vector& jmodels = jmg->getActiveJointModels(); std::vector outside_bounds; for (const moveit::core::JointModel* jmodel : jmodels) if (!state->satisfiesBounds(jmodel, jmodel->getMaximumExtent() * 1e-2)) @@ -944,13 +944,13 @@ void MotionPlanningDisplay::rememberPreviousStartState() *previous_state_ = *query_start_state_->getState(); } -void MotionPlanningDisplay::setQueryStartState(const robot_state::RobotState& start) +void MotionPlanningDisplay::setQueryStartState(const moveit::core::RobotState& start) { query_start_state_->setState(start); updateQueryStartState(); } -void MotionPlanningDisplay::setQueryGoalState(const robot_state::RobotState& goal) +void MotionPlanningDisplay::setQueryGoalState(const moveit::core::RobotState& goal) { query_goal_state_->setState(goal); updateQueryGoalState(); @@ -968,8 +968,8 @@ void MotionPlanningDisplay::useApproximateIK(bool flag) } } -bool MotionPlanningDisplay::isIKSolutionCollisionFree(robot_state::RobotState* state, - const robot_model::JointModelGroup* group, +bool MotionPlanningDisplay::isIKSolutionCollisionFree(moveit::core::RobotState* state, + const moveit::core::JointModelGroup* group, const double* ik_solution) const { if (frame_->ui_->collision_aware_ik->isChecked() && planning_scene_monitor_) @@ -1061,13 +1061,13 @@ std::string MotionPlanningDisplay::getCurrentPlanningGroup() const void MotionPlanningDisplay::setQueryStateHelper(bool use_start_state, const std::string& state_name) { - robot_state::RobotState state = use_start_state ? *getQueryStartState() : *getQueryGoalState(); + moveit::core::RobotState state = use_start_state ? *getQueryStartState() : *getQueryGoalState(); std::string v = "<" + state_name + ">"; if (v == "") { - if (const robot_state::JointModelGroup* jmg = state.getJointModelGroup(getCurrentPlanningGroup())) + if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(getCurrentPlanningGroup())) state.setToRandomPositions(jmg); } else if (v == "") @@ -1087,7 +1087,7 @@ void MotionPlanningDisplay::setQueryStateHelper(bool use_start_state, const std: else { // maybe it is a named state - if (const robot_model::JointModelGroup* jmg = state.getJointModelGroup(getCurrentPlanningGroup())) + if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(getCurrentPlanningGroup())) state.setToDefaultValues(jmg, state_name); } @@ -1144,7 +1144,7 @@ void MotionPlanningDisplay::onRobotModelLoaded() query_robot_goal_->load(*getRobotModel()->getURDF()); // initialize previous state, start state, and goal state to current state - previous_state_ = std::make_shared(getPlanningSceneRO()->getCurrentState()); + previous_state_ = std::make_shared(getPlanningSceneRO()->getCurrentState()); query_start_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "start", *previous_state_, planning_scene_monitor_->getTFClient())); query_goal_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "goal", *previous_state_, @@ -1188,12 +1188,13 @@ void MotionPlanningDisplay::onRobotModelLoaded() addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedPlanningGroup, this)); } -void MotionPlanningDisplay::updateStateExceptModified(robot_state::RobotState& dest, const robot_state::RobotState& src) +void MotionPlanningDisplay::updateStateExceptModified(moveit::core::RobotState& dest, + const moveit::core::RobotState& src) { - robot_state::RobotState src_copy = src; + moveit::core::RobotState src_copy = src; for (const std::string& modified_group : modified_groups_) { - const robot_model::JointModelGroup* jmg = dest.getJointModelGroup(modified_group); + const moveit::core::JointModelGroup* jmg = dest.getJointModelGroup(modified_group); if (jmg) { std::vector values_to_keep; @@ -1210,19 +1211,19 @@ void MotionPlanningDisplay::onSceneMonitorReceivedUpdate( planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) { PlanningSceneDisplay::onSceneMonitorReceivedUpdate(update_type); - robot_state::RobotState current_state = getPlanningSceneRO()->getCurrentState(); + moveit::core::RobotState current_state = getPlanningSceneRO()->getCurrentState(); std::string group = planning_group_property_->getStdString(); if (query_start_state_ && query_start_state_property_->getBool() && !group.empty()) { - robot_state::RobotState start = *getQueryStartState(); + moveit::core::RobotState start = *getQueryStartState(); updateStateExceptModified(start, current_state); setQueryStartState(start); } if (query_goal_state_ && query_goal_state_property_->getBool() && !group.empty()) { - robot_state::RobotState goal = *getQueryGoalState(); + moveit::core::RobotState goal = *getQueryGoalState(); updateStateExceptModified(goal, current_state); setQueryGoalState(goal); } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 18e9ad7922..c81aee0e15 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -261,7 +261,7 @@ void MotionPlanningFrame::fillPlanningGroupOptions() const QSignalBlocker planning_group_blocker(ui_->planning_group_combo_box); ui_->planning_group_combo_box->clear(); - const robot_model::RobotModelConstPtr& kmodel = planning_display_->getRobotModel(); + const moveit::core::RobotModelConstPtr& kmodel = planning_display_->getRobotModel(); for (const std::string& group_name : kmodel->getJointModelGroupNames()) ui_->planning_group_combo_box->addItem(QString::fromStdString(group_name)); } @@ -276,11 +276,11 @@ void MotionPlanningFrame::fillStateSelectionOptions() if (!planning_display_->getPlanningSceneMonitor()) return; - const robot_model::RobotModelConstPtr& robot_model = planning_display_->getRobotModel(); + const moveit::core::RobotModelConstPtr& robot_model = planning_display_->getRobotModel(); std::string group = planning_display_->getCurrentPlanningGroup(); if (group.empty()) return; - const robot_model::JointModelGroup* jmg = robot_model->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); if (jmg) { ui_->start_state_combo_box->addItem(QString("")); @@ -321,7 +321,7 @@ void MotionPlanningFrame::changePlanningGroupHelper() planning_display_->addMainLoopJob( boost::bind(&MotionPlanningFrame::populateConstraintsList, this, std::vector())); - const robot_model::RobotModelConstPtr& robot_model = planning_display_->getRobotModel(); + const moveit::core::RobotModelConstPtr& robot_model = planning_display_->getRobotModel(); std::string group = planning_display_->getCurrentPlanningGroup(); planning_display_->addMainLoopJob( boost::bind(&MotionPlanningParamWidget::setGroupName, ui_->planner_param_treeview, group)); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp index 4159cc4ff0..bf4b79055b 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp @@ -301,14 +301,14 @@ void MotionPlanningFrame::placeObjectButtonClicked() ui_->pick_button->setEnabled(false); ui_->place_button->setEnabled(false); - std::vector attached_bodies; + std::vector attached_bodies; const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (!ps) { ROS_ERROR("No planning scene"); return; } - const robot_model::JointModelGroup* jmg = ps->getCurrentState().getJointModelGroup(group_name); + const moveit::core::JointModelGroup* jmg = ps->getCurrentState().getJointModelGroup(group_name); if (jmg) ps->getCurrentState().getAttachedBodies(attached_bodies, jmg); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 7f865d531b..e22be44612 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -179,7 +179,7 @@ static QString decideStatusText(const collision_detection::CollisionEnv::ObjectC return status_text; } -static QString decideStatusText(const robot_state::AttachedBody* attached_body) +static QString decideStatusText(const moveit::core::AttachedBody* attached_body) { QString status_text = "'" + QString::fromStdString(attached_body->getName()) + "' is attached to '" + QString::fromStdString(attached_body->getAttachedLinkName()) + "'"; @@ -280,7 +280,7 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() // if it is an attached object scene_marker_.reset(); const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); - const robot_state::AttachedBody* attached_body = + const moveit::core::AttachedBody* attached_body = ps->getCurrentState().getAttachedBody(sel[0]->text().toStdString()); if (attached_body) ui_->object_status->setText(decideStatusText(attached_body)); @@ -641,12 +641,13 @@ void MotionPlanningFrame::computeLoadQueryButtonClicked() if (got_q) { - robot_state::RobotStatePtr start_state(new robot_state::RobotState(*planning_display_->getQueryStartState())); - robot_state::robotStateMsgToRobotState(planning_display_->getPlanningSceneRO()->getTransforms(), - mp->start_state, *start_state); + moveit::core::RobotStatePtr start_state( + new moveit::core::RobotState(*planning_display_->getQueryStartState())); + moveit::core::robotStateMsgToRobotState(planning_display_->getPlanningSceneRO()->getTransforms(), + mp->start_state, *start_state); planning_display_->setQueryStartState(*start_state); - robot_state::RobotStatePtr goal_state(new robot_state::RobotState(*planning_display_->getQueryGoalState())); + moveit::core::RobotStatePtr goal_state(new moveit::core::RobotState(*planning_display_->getQueryGoalState())); for (const moveit_msgs::Constraints& goal_constraint : mp->goal_constraints) if (!goal_constraint.joint_constraints.empty()) { @@ -775,12 +776,12 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item) { // rename attached body planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); - robot_state::RobotState& cs = ps->getCurrentStateNonConst(); - const robot_state::AttachedBody* ab = cs.getAttachedBody(known_collision_objects_[item->type()].first); + moveit::core::RobotState& cs = ps->getCurrentStateNonConst(); + const moveit::core::AttachedBody* ab = cs.getAttachedBody(known_collision_objects_[item->type()].first); if (ab) { known_collision_objects_[item->type()].first = item_text; - robot_state::AttachedBody* new_ab = new robot_state::AttachedBody( + moveit::core::AttachedBody* new_ab = new moveit::core::AttachedBody( ab->getAttachedLink(), known_collision_objects_[item->type()].first, ab->getShapes(), ab->getFixedTransforms(), ab->getTouchLinks(), ab->getDetachPosture(), ab->getSubframeTransforms()); cs.clearAttachedBody(ab->getName()); @@ -818,7 +819,7 @@ void MotionPlanningFrame::attachDetachCollisionObject(QListWidgetItem* item) else // we need to detach an attached object { const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); - const robot_state::AttachedBody* attached_body = ps->getCurrentState().getAttachedBody(data.first); + const moveit::core::AttachedBody* attached_body = ps->getCurrentState().getAttachedBody(data.first); if (attached_body) { aco.link_name = attached_body->getAttachedLinkName(); @@ -876,8 +877,8 @@ void MotionPlanningFrame::populateCollisionObjectsList() known_collision_objects_.push_back(std::make_pair(collision_object_names[i], false)); } - const robot_state::RobotState& cs = ps->getCurrentState(); - std::vector attached_bodies; + const moveit::core::RobotState& cs = ps->getCurrentState(); + std::vector attached_bodies; cs.getAttachedBodies(attached_bodies); for (std::size_t i = 0; i < attached_bodies.size(); ++i) { diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 55006806df..9b2efcff0b 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -114,10 +114,10 @@ bool MotionPlanningFrame::computeCartesianPlan() { ros::WallTime start = ros::WallTime::now(); // get goal pose - robot_state::RobotState goal = *planning_display_->getQueryGoalState(); + moveit::core::RobotState goal = *planning_display_->getQueryGoalState(); std::vector waypoints; const std::string& link_name = move_group_->getEndEffectorLink(); - const robot_model::LinkModel* link = move_group_->getRobotModel()->getLinkModel(link_name); + const moveit::core::LinkModel* link = move_group_->getRobotModel()->getLinkModel(link_name); if (!link) { ROS_ERROR_STREAM("Failed to determine unique end-effector link: " << link_name); @@ -261,7 +261,7 @@ void MotionPlanningFrame::startStateTextChanged(const QString& start_state) void MotionPlanningFrame::startStateTextChangedExec(const std::string& start_state) { - robot_state::RobotState start = *planning_display_->getQueryStartState(); + moveit::core::RobotState start = *planning_display_->getQueryStartState(); updateQueryStateHelper(start, start_state); planning_display_->setQueryStartState(start); } @@ -275,7 +275,7 @@ void MotionPlanningFrame::goalStateTextChanged(const QString& goal_state) void MotionPlanningFrame::goalStateTextChangedExec(const std::string& goal_state) { - robot_state::RobotState goal = *planning_display_->getQueryGoalState(); + moveit::core::RobotState goal = *planning_display_->getQueryGoalState(); updateQueryStateHelper(goal, goal_state); planning_display_->setQueryGoalState(goal); } @@ -285,12 +285,12 @@ void MotionPlanningFrame::planningGroupTextChanged(const QString& planning_group planning_display_->changePlanningGroup(planning_group.toStdString()); } -void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState& state, const std::string& v) +void MotionPlanningFrame::updateQueryStateHelper(moveit::core::RobotState& state, const std::string& v) { if (v == "") { configureWorkspace(); - if (const robot_model::JointModelGroup* jmg = + if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) state.setToRandomPositions(jmg); return; @@ -300,7 +300,7 @@ void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState& state, { configureWorkspace(); - if (const robot_model::JointModelGroup* jmg = + if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) { // Loop until a collision free state is found @@ -358,7 +358,7 @@ void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState& state, } // maybe it is a named state - if (const robot_model::JointModelGroup* jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) + if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) state.setToDefaultValues(jmg, v); } @@ -426,15 +426,15 @@ void MotionPlanningFrame::constructPlanningRequest(moveit_msgs::MotionPlanReques mreq.allowed_planning_time = ui_->planning_time->value(); mreq.max_velocity_scaling_factor = ui_->velocity_scaling_factor->value(); mreq.max_acceleration_scaling_factor = ui_->acceleration_scaling_factor->value(); - robot_state::robotStateToRobotStateMsg(*planning_display_->getQueryStartState(), mreq.start_state); + moveit::core::robotStateToRobotStateMsg(*planning_display_->getQueryStartState(), mreq.start_state); mreq.workspace_parameters.min_corner.x = ui_->wcenter_x->value() - ui_->wsize_x->value() / 2.0; mreq.workspace_parameters.min_corner.y = ui_->wcenter_y->value() - ui_->wsize_y->value() / 2.0; mreq.workspace_parameters.min_corner.z = ui_->wcenter_z->value() - ui_->wsize_z->value() / 2.0; mreq.workspace_parameters.max_corner.x = ui_->wcenter_x->value() + ui_->wsize_x->value() / 2.0; mreq.workspace_parameters.max_corner.y = ui_->wcenter_y->value() + ui_->wsize_y->value() / 2.0; mreq.workspace_parameters.max_corner.z = ui_->wcenter_z->value() + ui_->wsize_z->value() / 2.0; - robot_state::RobotStateConstPtr s = planning_display_->getQueryGoalState(); - const robot_state::JointModelGroup* jmg = s->getJointModelGroup(mreq.group_name); + moveit::core::RobotStateConstPtr s = planning_display_->getQueryGoalState(); + const moveit::core::JointModelGroup* jmg = s->getJointModelGroup(mreq.group_name); if (jmg) { mreq.goal_constraints.resize(1); @@ -444,10 +444,10 @@ void MotionPlanningFrame::constructPlanningRequest(moveit_msgs::MotionPlanReques void MotionPlanningFrame::configureWorkspace() { - robot_model::VariableBounds bx, by, bz; + moveit::core::VariableBounds bx, by, bz; bx.position_bounded_ = by.position_bounded_ = bz.position_bounded_ = true; - robot_model::JointModel::Bounds b(3); + moveit::core::JointModel::Bounds b(3); bx.min_position_ = ui_->wcenter_x->value() - ui_->wsize_x->value() / 2.0; bx.max_position_ = ui_->wcenter_x->value() + ui_->wsize_x->value() / 2.0; by.min_position_ = ui_->wcenter_y->value() - ui_->wsize_y->value() / 2.0; @@ -462,15 +462,15 @@ void MotionPlanningFrame::configureWorkspace() // get non-const access to the robot_model and update planar & floating joints as indicated by the workspace settings if (psm && psm->getRobotModelLoader() && psm->getRobotModelLoader()->getModel()) { - const robot_model::RobotModelPtr& robot_model = psm->getRobotModelLoader()->getModel(); - const std::vector& jm = robot_model->getJointModels(); + const moveit::core::RobotModelPtr& robot_model = psm->getRobotModelLoader()->getModel(); + const std::vector& jm = robot_model->getJointModels(); for (moveit::core::JointModel* joint : jm) - if (joint->getType() == robot_model::JointModel::PLANAR) + if (joint->getType() == moveit::core::JointModel::PLANAR) { joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[0], bx); joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[1], by); } - else if (joint->getType() == robot_model::JointModel::FLOATING) + else if (joint->getType() == moveit::core::JointModel::FLOATING) { joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[0], bx); joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[1], by); @@ -515,7 +515,7 @@ void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::EmptyCo const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (ps) { - robot_state::RobotState state = ps->getCurrentState(); + moveit::core::RobotState state = ps->getCurrentState(); planning_display_->setQueryStartState(state); } } @@ -529,7 +529,7 @@ void MotionPlanningFrame::remoteUpdateGoalStateCallback(const std_msgs::EmptyCon const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (ps) { - robot_state::RobotState state = ps->getCurrentState(); + moveit::core::RobotState state = ps->getCurrentState(); planning_display_->setQueryGoalState(state); } } @@ -546,8 +546,8 @@ void MotionPlanningFrame::remoteUpdateCustomStartStateCallback(const moveit_msgs const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (ps) { - robot_state::RobotStatePtr state(new robot_state::RobotState(ps->getCurrentState())); - robot_state::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state); + moveit::core::RobotStatePtr state(new moveit::core::RobotState(ps->getCurrentState())); + moveit::core::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state); planning_display_->setQueryStartState(*state); } } @@ -564,8 +564,8 @@ void MotionPlanningFrame::remoteUpdateCustomGoalStateCallback(const moveit_msgs: const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (ps) { - robot_state::RobotStatePtr state(new robot_state::RobotState(ps->getCurrentState())); - robot_state::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state); + moveit::core::RobotStatePtr state(new moveit::core::RobotState(ps->getCurrentState())); + moveit::core::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state); planning_display_->setQueryGoalState(*state); } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp index 3e3982bb47..584f2329f1 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp @@ -119,7 +119,7 @@ void MotionPlanningFrame::loadStoredStates(const std::string& pattern) populateRobotStatesList(); } -void MotionPlanningFrame::saveRobotStateButtonClicked(const robot_state::RobotState& state) +void MotionPlanningFrame::saveRobotStateButtonClicked(const moveit::core::RobotState& state) { bool ok = false; @@ -144,7 +144,7 @@ void MotionPlanningFrame::saveRobotStateButtonClicked(const robot_state::RobotSt { // Store the current start state moveit_msgs::RobotState msg; - robot_state::robotStateToRobotStateMsg(state, msg); + moveit::core::robotStateToRobotStateMsg(state, msg); robot_states_.insert(RobotStatePair(name, msg)); // Save to the database if connected @@ -188,8 +188,8 @@ void MotionPlanningFrame::setAsStartStateButtonClicked() if (item) { - robot_state::RobotState robot_state(*planning_display_->getQueryStartState()); - robot_state::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state); + moveit::core::RobotState robot_state(*planning_display_->getQueryStartState()); + moveit::core::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state); planning_display_->setQueryStartState(robot_state); } } @@ -200,8 +200,8 @@ void MotionPlanningFrame::setAsGoalStateButtonClicked() if (item) { - robot_state::RobotState robot_state(*planning_display_->getQueryGoalState()); - robot_state::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state); + moveit::core::RobotState robot_state(*planning_display_->getQueryGoalState()); + moveit::core::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state); planning_display_->setQueryGoalState(robot_state); } } diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index 06e30bef58..3b24a461c0 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -102,7 +102,7 @@ class PlanningSceneDisplay : public rviz::Display void clearJobs(); const std::string getMoveGroupNS() const; - const robot_model::RobotModelConstPtr& getRobotModel() const; + const moveit::core::RobotModelConstPtr& getRobotModel() const; /// wait for robot state more recent than t bool waitForCurrentRobotState(const ros::Time& t = ros::Time::now()); diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 33af096a85..d0305bedac 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -268,13 +268,13 @@ const std::string PlanningSceneDisplay::getMoveGroupNS() const return move_group_ns_property_->getStdString(); } -const robot_model::RobotModelConstPtr& PlanningSceneDisplay::getRobotModel() const +const moveit::core::RobotModelConstPtr& PlanningSceneDisplay::getRobotModel() const { if (planning_scene_monitor_) return planning_scene_monitor_->getRobotModel(); else { - static robot_model::RobotModelConstPtr empty; + static moveit::core::RobotModelConstPtr empty; return empty; } } @@ -416,7 +416,7 @@ void PlanningSceneDisplay::setGroupColor(rviz::Robot* robot, const std::string& { if (getRobotModel()) { - const robot_model::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name); if (jmg) { const std::vector& links = jmg->getLinkModelNamesWithCollisionGeometry(); @@ -440,7 +440,7 @@ void PlanningSceneDisplay::unsetGroupColor(rviz::Robot* robot, const std::string { if (getRobotModel()) { - const robot_model::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name); if (jmg) { const std::vector& links = jmg->getLinkModelNamesWithCollisionGeometry(); @@ -542,9 +542,9 @@ void PlanningSceneDisplay::onRobotModelLoaded() if (planning_scene_robot_) { planning_scene_robot_->load(*getRobotModel()->getURDF()); - robot_state::RobotState* rs = new robot_state::RobotState(ps->getCurrentState()); + moveit::core::RobotState* rs = new moveit::core::RobotState(ps->getCurrentState()); rs->update(); - planning_scene_robot_->update(robot_state::RobotStateConstPtr(rs)); + planning_scene_robot_->update(moveit::core::RobotStateConstPtr(rs)); } bool old_state = scene_name_property_->blockSignals(true); diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h index e4d0465d6d..632692dde0 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h +++ b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h @@ -75,7 +75,7 @@ class RobotStateDisplay : public rviz::Display void update(float wall_dt, float ros_dt) override; void reset() override; - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -130,8 +130,8 @@ private Q_SLOTS: RobotStateVisualizationPtr robot_; rdf_loader::RDFLoaderPtr rdf_loader_; - robot_model::RobotModelConstPtr robot_model_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; std::map highlights_; bool update_state_; bool load_robot_model_; // for delayed robot initialization diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 24d739ec9c..20dd6f1f93 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -306,12 +306,12 @@ void RobotStateDisplay::newRobotStateCallback(const moveit_msgs::DisplayRobotSta if (!robot_model_) return; if (!robot_state_) - robot_state_.reset(new robot_state::RobotState(robot_model_)); - // possibly use TF to construct a robot_state::Transforms object to pass in to the conversion function? + robot_state_.reset(new moveit::core::RobotState(robot_model_)); + // possibly use TF to construct a moveit::core::Transforms object to pass in to the conversion function? try { if (!moveit::core::isEmpty(state_msg->state)) - robot_state::robotStateMsgToRobotState(state_msg->state, *robot_state_); + moveit::core::robotStateMsgToRobotState(state_msg->state, *robot_state_); setRobotHighlights(state_msg->highlight_links); } catch (const moveit::Exception& e) @@ -381,9 +381,9 @@ void RobotStateDisplay::loadRobotModel() { const srdf::ModelSharedPtr& srdf = rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); - robot_model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf)); + robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); robot_->load(*robot_model_->getURDF()); - robot_state_.reset(new robot_state::RobotState(robot_model_)); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); robot_state_->setToDefaultValues(); bool old_state = root_link_name_property_->blockSignals(true); root_link_name_property_->setStdString(getRobotModel()->getRootLinkName()); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h index b1d1358a28..3f16dffc55 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h @@ -45,11 +45,11 @@ DIAGNOSTIC_POP namespace moveit_rviz_plugin { -/** \brief Update the links of an rviz::Robot using a robot_state::RobotState */ +/** \brief Update the links of an rviz::Robot using a moveit::core::RobotState */ class PlanningLinkUpdater : public rviz::LinkUpdater { public: - PlanningLinkUpdater(const robot_state::RobotStateConstPtr& state) : kinematic_state_(state) + PlanningLinkUpdater(const moveit::core::RobotStateConstPtr& state) : kinematic_state_(state) { } @@ -58,6 +58,6 @@ class PlanningLinkUpdater : public rviz::LinkUpdater Ogre::Quaternion& collision_orientation) const override; private: - robot_state::RobotStateConstPtr kinematic_state_; + moveit::core::RobotStateConstPtr kinematic_state_; }; } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index 52b81c9f07..58b258df24 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -50,7 +50,7 @@ namespace moveit_rviz_plugin MOVEIT_CLASS_FORWARD(RenderShapes); MOVEIT_CLASS_FORWARD(RobotStateVisualization); -/** \brief Update the links of an rviz::Robot using a robot_state::RobotState */ +/** \brief Update the links of an rviz::Robot using a moveit::core::RobotState */ class RobotStateVisualization { public: @@ -65,10 +65,10 @@ class RobotStateVisualization void load(const urdf::ModelInterface& descr, bool visual = true, bool collision = true); void clear(); - void update(const robot_state::RobotStateConstPtr& kinematic_state); - void update(const robot_state::RobotStateConstPtr& kinematic_state, + void update(const moveit::core::RobotStateConstPtr& kinematic_state); + void update(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color); - void update(const robot_state::RobotStateConstPtr& kinematic_state, + void update(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color, const std::map& color_map); void setDefaultAttachedObjectColor(const std_msgs::ColorRGBA& default_attached_object_color); @@ -101,7 +101,7 @@ class RobotStateVisualization void setAlpha(float alpha); private: - void updateHelper(const robot_state::RobotStateConstPtr& kinematic_state, + void updateHelper(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color, const std::map* color_map); rviz::Robot robot_; diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h index 025fbc0c82..0b6979701d 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h @@ -89,7 +89,7 @@ class TrajectoryVisualization : public QObject virtual void reset(); void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context, const ros::NodeHandle& update_nh); - void onRobotModelLoaded(const robot_model::RobotModelConstPtr& robot_model); + void onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model); void onEnable(); void onDisable(); void setName(const QString& name); @@ -143,8 +143,8 @@ private Q_SLOTS: float current_state_time_; boost::mutex update_trajectory_message_; - robot_model::RobotModelConstPtr robot_model_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; // Pointers from parent display taht we save rviz::Display* display_; // the parent display that this class populates diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp index 89baece404..0ad0668b06 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp @@ -44,7 +44,7 @@ bool moveit_rviz_plugin::PlanningLinkUpdater::getLinkTransforms(const std::strin Ogre::Vector3& collision_position, Ogre::Quaternion& collision_orientation) const { - const robot_model::LinkModel* link_model = kinematic_state_->getLinkModel(link_name); + const moveit::core::LinkModel* link_model = kinematic_state_->getLinkModel(link_name); if (!link_model) { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp index fdfe93fc59..05210fe078 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp @@ -74,7 +74,7 @@ void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningScen if (scene_robot_) { - robot_state::RobotState* rs = new robot_state::RobotState(scene->getCurrentState()); + moveit::core::RobotState* rs = new moveit::core::RobotState(scene->getCurrentState()); rs->update(); std_msgs::ColorRGBA color; @@ -84,7 +84,7 @@ void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningScen color.a = 1.0f; planning_scene::ObjectColorMap color_map; scene->getKnownObjectColors(color_map); - scene_robot_->update(robot_state::RobotStateConstPtr(rs), color, color_map); + scene_robot_->update(moveit::core::RobotStateConstPtr(rs), color, color_map); } const std::vector& ids = scene->getWorld()->getObjectIds(); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp index 6460d1f083..f68ed950e3 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp @@ -86,34 +86,34 @@ void RobotStateVisualization::updateAttachedObjectColors(const std_msgs::ColorRG robot_.getAlpha()); } -void RobotStateVisualization::update(const robot_state::RobotStateConstPtr& kinematic_state) +void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kinematic_state) { updateHelper(kinematic_state, default_attached_object_color_, nullptr); } -void RobotStateVisualization::update(const robot_state::RobotStateConstPtr& kinematic_state, +void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color) { updateHelper(kinematic_state, default_attached_object_color, nullptr); } -void RobotStateVisualization::update(const robot_state::RobotStateConstPtr& kinematic_state, +void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color, const std::map& color_map) { updateHelper(kinematic_state, default_attached_object_color, &color_map); } -void RobotStateVisualization::updateHelper(const robot_state::RobotStateConstPtr& kinematic_state, +void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color, const std::map* color_map) { robot_.update(PlanningLinkUpdater(kinematic_state)); render_shapes_->clear(); - std::vector attached_bodies; + std::vector attached_bodies; kinematic_state->getAttachedBodies(attached_bodies); - for (const robot_state::AttachedBody* attached_body : attached_bodies) + for (const moveit::core::AttachedBody* attached_body : attached_bodies) { std_msgs::ColorRGBA color = default_attached_object_color; float alpha = robot_.getAlpha(); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 3d5b4c2acc..04d946e4a6 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -164,7 +164,7 @@ void TrajectoryVisualization::setName(const QString& name) trajectory_slider_dock_panel_->setWindowTitle(name + " - Slider"); } -void TrajectoryVisualization::onRobotModelLoaded(const robot_model::RobotModelConstPtr& robot_model) +void TrajectoryVisualization::onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model) { robot_model_ = robot_model; @@ -176,7 +176,7 @@ void TrajectoryVisualization::onRobotModelLoaded(const robot_model::RobotModelCo } // Load robot state - robot_state_.reset(new robot_state::RobotState(robot_model_)); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); robot_state_->setToDefaultValues(); // Load rviz robot diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h index d4c0ab28a8..e0cbf22da6 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h +++ b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h @@ -86,8 +86,8 @@ private Q_SLOTS: // Load robot model rdf_loader::RDFLoaderPtr rdf_loader_; - robot_model::RobotModelConstPtr robot_model_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; bool load_robot_model_; // for delayed robot initialization // Properties diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp index 971c31f87e..2128e624e6 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -76,7 +76,7 @@ void TrajectoryDisplay::loadRobotModel() const srdf::ModelSharedPtr& srdf = rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); - robot_model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf)); + robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); // Send to child class trajectory_visual_->onRobotModelLoaded(robot_model_); diff --git a/moveit_ros/warehouse/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/warehouse/src/import_from_text.cpp index d1e6229831..944d5e90f4 100755 --- a/moveit_ros/warehouse/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/warehouse/src/import_from_text.cpp @@ -83,10 +83,10 @@ void parseStart(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* } if (!v.empty()) { - robot_state::RobotState st = psm->getPlanningScene()->getCurrentState(); + moveit::core::RobotState st = psm->getPlanningScene()->getCurrentState(); st.setVariablePositions(v); moveit_msgs::RobotState msg; - robot_state::robotStateToRobotStateMsg(st, msg); + moveit::core::robotStateToRobotStateMsg(st, msg); ROS_INFO("Parsed start state '%s'", name.c_str()); rs->addRobotState(msg, name); } diff --git a/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp b/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp index 595cae59c1..e357870b33 100644 --- a/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp +++ b/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp @@ -101,14 +101,14 @@ int main(int argc, char** argv) ROS_INFO("Added default scene: '%s'", psmsg.name.c_str()); moveit_msgs::RobotState rsmsg; - robot_state::robotStateToRobotStateMsg(psm.getPlanningScene()->getCurrentState(), rsmsg); + moveit::core::robotStateToRobotStateMsg(psm.getPlanningScene()->getCurrentState(), rsmsg); rs.addRobotState(rsmsg, "default"); ROS_INFO("Added default state"); const std::vector& gnames = psm.getRobotModel()->getJointModelGroupNames(); for (const std::string& gname : gnames) { - const robot_model::JointModelGroup* jmg = psm.getRobotModel()->getJointModelGroup(gname); + const moveit::core::JointModelGroup* jmg = psm.getRobotModel()->getJointModelGroup(gname); if (!jmg->isChain()) continue; const std::vector& lnames = jmg->getLinkModelNames(); diff --git a/moveit_ros/warehouse/warehouse/src/save_as_text.cpp b/moveit_ros/warehouse/warehouse/src/save_as_text.cpp index 7926c60edc..1154fbd7f8 100644 --- a/moveit_ros/warehouse/warehouse/src/save_as_text.cpp +++ b/moveit_ros/warehouse/warehouse/src/save_as_text.cpp @@ -123,7 +123,7 @@ int main(int argc, char** argv) fout.close(); std::vector robot_state_names; - robot_model::RobotModelConstPtr km = psm.getRobotModel(); + moveit::core::RobotModelConstPtr km = psm.getRobotModel(); // Get start states for scene std::stringstream rsregex; rsregex << ".*" << scene_name << ".*"; @@ -150,8 +150,8 @@ int main(int argc, char** argv) qfout << robot_state_name << std::endl; moveit_warehouse::RobotStateWithMetadata robot_state; rss.getRobotState(robot_state, robot_state_name); - robot_state::RobotState ks(km); - robot_state::robotStateMsgToRobotState(*robot_state, ks, false); + moveit::core::RobotState ks(km); + moveit::core::robotStateMsgToRobotState(*robot_state, ks, false); ks.printStateInfo(qfout); qfout << "." << std::endl; } diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 0e42bac586..c027abde9e 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -280,7 +280,7 @@ class MoveItConfigData void setRobotModel(const moveit::core::RobotModelPtr& robot_model); /// Provide a shared kinematic model loader - robot_model::RobotModelConstPtr getRobotModel(); + moveit::core::RobotModelConstPtr getRobotModel(); /// Update the Kinematic Model with latest SRDF modifications void updateRobotModel(); @@ -499,7 +499,7 @@ class MoveItConfigData */ struct joint_model_compare { - bool operator()(const robot_model::JointModel* jm1, const robot_model::JointModel* jm2) const + bool operator()(const moveit::core::JointModel* jm1, const moveit::core::JointModel* jm2) const { return jm1->getName() < jm2->getName(); } @@ -514,7 +514,7 @@ class MoveItConfigData std::vector > sensors_plugin_config_parameter_list_; /// Shared kinematic model - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; /// ROS Controllers config data std::vector ros_controllers_config_; diff --git a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp index 05dc0d8808..5d534420a2 100644 --- a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp +++ b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp @@ -84,7 +84,7 @@ struct ThreadComputation }; // LinkGraph defines a Link's model and a set of unique links it connects -typedef std::map > LinkGraph; +typedef std::map > LinkGraph; // ****************************************************************************************** // Static Prototypes @@ -106,14 +106,14 @@ static bool setLinkPair(const std::string& linkA, const std::string& linkB, cons * \param link The root link to begin a breadth first search on * \param link_graph A representation of all bi-direcitonal joint connections between links in robot_description */ -static void computeConnectionGraph(const robot_model::LinkModel* link, LinkGraph& link_graph); +static void computeConnectionGraph(const moveit::core::LinkModel* link, LinkGraph& link_graph); /** * \brief Recursively build the adj list of link connections * \param link The root link to begin a breadth first search on * \param link_graph A representation of all bi-direcitonal joint connections between links in robot_description */ -static void computeConnectionGraphRec(const robot_model::LinkModel* link, LinkGraph& link_graph); +static void computeConnectionGraphRec(const moveit::core::LinkModel* link, LinkGraph& link_graph); /** * \brief Disable collision checking for adjacent links, or adjacent with no geometry links between them @@ -339,7 +339,7 @@ void computeLinkPairs(const planning_scene::PlanningScene& scene, LinkPairMap& l // ****************************************************************************************** // Build the robot links connection graph and then check for links with no geomotry // ****************************************************************************************** -void computeConnectionGraph(const robot_model::LinkModel* start_link, LinkGraph& link_graph) +void computeConnectionGraph(const moveit::core::LinkModel* start_link, LinkGraph& link_graph) { link_graph.clear(); // make sure the edges structure is clear @@ -358,7 +358,7 @@ void computeConnectionGraph(const robot_model::LinkModel* start_link, LinkGraph& if (edge_it->first->getShapes().empty()) // link in adjList "link_graph" does not have shape, remove! { // Temporary list for connected links - std::vector temp_list; + std::vector temp_list; // Copy link's parent and child links to temp_list for (const moveit::core::LinkModel* adj_it : edge_it->second) @@ -390,14 +390,14 @@ void computeConnectionGraph(const robot_model::LinkModel* start_link, LinkGraph& // ****************************************************************************************** // Recursively build the adj list of link connections // ****************************************************************************************** -void computeConnectionGraphRec(const robot_model::LinkModel* start_link, LinkGraph& link_graph) +void computeConnectionGraphRec(const moveit::core::LinkModel* start_link, LinkGraph& link_graph) { if (start_link) // check that the link is a valid pointer { // Loop through every link attached to start_link for (std::size_t i = 0; i < start_link->getChildJointModels().size(); ++i) { - const robot_model::LinkModel* next = start_link->getChildJointModels()[i]->getChildLinkModel(); + const moveit::core::LinkModel* next = start_link->getChildJointModels()[i]->getChildLinkModel(); // Bi-directional connection link_graph[next].insert(start_link); @@ -422,7 +422,7 @@ unsigned int disableAdjacentLinks(planning_scene::PlanningScene& scene, LinkGrap for (LinkGraph::const_iterator link_graph_it = link_graph.begin(); link_graph_it != link_graph.end(); ++link_graph_it) { // disable all connected links to current link by looping through them - for (std::set::const_iterator adj_it = link_graph_it->second.begin(); + for (std::set::const_iterator adj_it = link_graph_it->second.begin(); adj_it != link_graph_it->second.end(); ++adj_it) { // ROS_INFO("Disabled %s to %s", link_graph_it->first->getName().c_str(), (*adj_it)->getName().c_str() ); @@ -613,7 +613,7 @@ void disableNeverInCollisionThread(ThreadComputation tc) const unsigned int progress_interval = tc.num_trials_ / 20; // show progress update every 5% // Create a new kinematic state for this thread to work on - robot_state::RobotState robot_state(tc.scene_.getRobotModel()); + moveit::core::RobotState robot_state(tc.scene_.getRobotModel()); // Do a large number of tests for (unsigned int i = 0; i < tc.num_trials_; ++i) diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index d925af3807..00b9d7d1ee 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -82,7 +82,7 @@ MoveItConfigData::~MoveItConfigData() = default; // ****************************************************************************************** // Load a robot model // ****************************************************************************************** -void MoveItConfigData::setRobotModel(const robot_model::RobotModelPtr& robot_model) +void MoveItConfigData::setRobotModel(const moveit::core::RobotModelPtr& robot_model) { robot_model_ = robot_model; } @@ -90,12 +90,12 @@ void MoveItConfigData::setRobotModel(const robot_model::RobotModelPtr& robot_mod // ****************************************************************************************** // Provide a kinematic model. Load a new one if necessary // ****************************************************************************************** -robot_model::RobotModelConstPtr MoveItConfigData::getRobotModel() +moveit::core::RobotModelConstPtr MoveItConfigData::getRobotModel() { if (!robot_model_) { // Initialize with a URDF Model Interface and a SRDF Model - robot_model_.reset(new robot_model::RobotModel(urdf_model_, srdf_->srdf_model_)); + robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_->srdf_model_)); } return robot_model_; @@ -112,7 +112,7 @@ void MoveItConfigData::updateRobotModel() srdf_->updateSRDFModel(*urdf_model_); // Create new kin model - robot_model_.reset(new robot_model::RobotModel(urdf_model_, srdf_->srdf_model_)); + robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_->srdf_model_)); // Reset the planning scene planning_scene_.reset(); @@ -511,18 +511,18 @@ bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path) for (srdf::Model::Group& group : srdf_->groups_) { // Get list of associated joints - const robot_model::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); + const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); emitter << YAML::BeginMap; - const std::vector& joint_models = joint_model_group->getActiveJointModels(); + const std::vector& joint_models = joint_model_group->getActiveJointModels(); emitter << YAML::Key << "name"; emitter << YAML::Value << "fake_" + group.name_ + "_controller"; emitter << YAML::Key << "joints"; emitter << YAML::Value << YAML::BeginSeq; // Iterate through the joints - for (const robot_model::JointModel* joint : joint_models) + for (const moveit::core::JointModel* joint : joint_models) { - if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == robot_model::JointModel::FIXED) + if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED) continue; emitter << joint->getName(); } @@ -841,12 +841,12 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) for (srdf::Model::Group& group : srdf_->groups_) { // Get list of associated joints - const robot_model::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); - const std::vector& joint_models = joint_model_group->getActiveJointModels(); + const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); + const std::vector& joint_models = joint_model_group->getActiveJointModels(); // Iterate through the joints and push into group_joints vector. - for (const robot_model::JointModel* joint : joint_models) + for (const moveit::core::JointModel* joint : joint_models) { - if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == robot_model::JointModel::FIXED) + if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED) continue; else group_joints.push_back(joint->getName()); @@ -891,7 +891,7 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) emitter << YAML::Key << "hardware_interface" << YAML::Value << YAML::BeginMap; { // Get list of all joints for the robot - const std::vector& joint_models = getRobotModel()->getJointModels(); + const std::vector& joint_models = getRobotModel()->getJointModels(); emitter << YAML::Key << "joints"; { @@ -899,11 +899,11 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) { emitter << YAML::Value << YAML::BeginSeq; // Iterate through the joints - for (std::vector::const_iterator joint_it = joint_models.begin(); + for (std::vector::const_iterator joint_it = joint_models.begin(); joint_it < joint_models.end(); ++joint_it) { if ((*joint_it)->isPassive() || (*joint_it)->getMimic() != nullptr || - (*joint_it)->getType() == robot_model::JointModel::FIXED) + (*joint_it)->getType() == moveit::core::JointModel::FIXED) continue; else emitter << (*joint_it)->getName(); @@ -1058,15 +1058,15 @@ bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) emitter << YAML::Value << YAML::BeginMap; // Union all the joints in groups. Uses a custom comparator to allow the joints to be sorted by name - std::set joints; + std::set joints; // Loop through groups for (srdf::Model::Group& group : srdf_->groups_) { // Get list of associated joints - const robot_model::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); + const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); - const std::vector& joint_models = joint_model_group->getJointModels(); + const std::vector& joint_models = joint_model_group->getJointModels(); // Iterate through the joints for (const moveit::core::JointModel* joint_model : joint_models) @@ -1083,7 +1083,7 @@ bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) emitter << YAML::Key << joint->getName(); emitter << YAML::Value << YAML::BeginMap; - const robot_model::VariableBounds& b = joint->getVariableBounds()[0]; + const moveit::core::VariableBounds& b = joint->getVariableBounds()[0]; // Output property emitter << YAML::Key << "has_velocity_limits"; @@ -1193,14 +1193,14 @@ std::string MoveItConfigData::decideProjectionJoints(const std::string& planning std::string joint_pair = ""; // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = getRobotModel(); + const moveit::core::RobotModelConstPtr& model = getRobotModel(); // Error check if (!model->hasJointModelGroup(planning_group)) return joint_pair; // Get the joint model group - const robot_model::JointModelGroup* group = model->getJointModelGroup(planning_group); + const moveit::core::JointModelGroup* group = model->getJointModelGroup(planning_group); // get vector of joint names const std::vector& joints = group->getJointModelNames(); @@ -1459,13 +1459,13 @@ bool MoveItConfigData::addDefaultControllers() { ROSControlConfig group_controller; // Get list of associated joints - const robot_model::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group_it.name_); - const std::vector& joint_models = joint_model_group->getActiveJointModels(); + const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group_it.name_); + const std::vector& joint_models = joint_model_group->getActiveJointModels(); // Iterate through the joints - for (const robot_model::JointModel* joint : joint_models) + for (const moveit::core::JointModel* joint : joint_models) { - if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == robot_model::JointModel::FIXED) + if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED) continue; group_controller.joints_.push_back(joint->getName()); } diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp index 1e31472d13..1ea9a6774c 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp @@ -381,10 +381,10 @@ void EndEffectorsWidget::loadParentComboBox() parent_name_field_->clear(); // Get all links in robot model - std::vector link_models = config_data_->getRobotModel()->getLinkModels(); + std::vector link_models = config_data_->getRobotModel()->getLinkModels(); // Add all links to combo box - for (std::vector::const_iterator link_it = link_models.begin(); + for (std::vector::const_iterator link_it = link_models.begin(); link_it < link_models.end(); ++link_it) { parent_name_field_->addItem((*link_it)->getName().c_str()); @@ -515,7 +515,7 @@ void EndEffectorsWidget::doneEditing() return; } - const robot_model::JointModelGroup* jmg = + const moveit::core::JointModelGroup* jmg = config_data_->getRobotModel()->getJointModelGroup(group_name_field_->currentText().toStdString()); /* if (jmg->hasLinkModel(parent_name_field_->currentText().toStdString())) diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp index 926cbb18f4..a53189872a 100644 --- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp +++ b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp @@ -146,10 +146,10 @@ void KinematicChainWidget::setAvailable() return; // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel(); + const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); // Get the root joint - const robot_model::JointModel* root_joint = model->getRootJoint(); + const moveit::core::JointModel* root_joint = model->getRootJoint(); addLinktoTreeRecursive(root_joint->getChildLinkModel(), nullptr); @@ -160,8 +160,8 @@ void KinematicChainWidget::setAvailable() // ****************************************************************************************** // // ****************************************************************************************** -void KinematicChainWidget::addLinktoTreeRecursive(const robot_model::LinkModel* link, - const robot_model::LinkModel* parent) +void KinematicChainWidget::addLinktoTreeRecursive(const moveit::core::LinkModel* link, + const moveit::core::LinkModel* parent) { // Create new tree item QTreeWidgetItem* new_item = new QTreeWidgetItem(link_tree_); @@ -191,7 +191,7 @@ void KinematicChainWidget::addLinktoTreeRecursive(const robot_model::LinkModel* // ****************************************************************************************** // // ****************************************************************************************** -bool KinematicChainWidget::addLinkChildRecursive(QTreeWidgetItem* parent, const robot_model::LinkModel* link, +bool KinematicChainWidget::addLinkChildRecursive(QTreeWidgetItem* parent, const moveit::core::LinkModel* link, const std::string& parent_name) { if (parent->text(0).toStdString() == parent_name) diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h index 4c5764cdfd..e7a3589803 100644 --- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h +++ b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h @@ -68,9 +68,9 @@ class KinematicChainWidget : public QWidget /// Set the link field with previous value void setSelected(const std::string& base_link, const std::string& tip_link); - void addLinktoTreeRecursive(const robot_model::LinkModel* link, const robot_model::LinkModel* parent); + void addLinktoTreeRecursive(const moveit::core::LinkModel* link, const moveit::core::LinkModel* parent); - bool addLinkChildRecursive(QTreeWidgetItem* parent, const robot_model::LinkModel* link, + bool addLinkChildRecursive(QTreeWidgetItem* parent, const moveit::core::LinkModel* link, const std::string& parent_name); // ****************************************************************************************** diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp b/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp index f7aa81693d..9048f45f10 100644 --- a/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp @@ -84,7 +84,7 @@ void PassiveJointsWidget::focusGiven() joints_widget_->clearContents(); // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel(); + const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); // Get the names of the all joints const std::vector& joints = model->getJointModelNames(); @@ -133,7 +133,7 @@ void PassiveJointsWidget::previewSelectedJoints(std::vector joints) for (const std::string& joint : joints) { - const robot_model::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); + const moveit::core::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); // Check that a joint model was found if (!joint_model) diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index 195bf6395b..2fc01a69be 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -318,7 +318,7 @@ void PlanningGroupsWidget::loadGroupsTreeRecursive(srdf::Model::Group& group_it, group->addChild(joints); // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel(); + const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); // Loop through all aval. joints for (std::vector::const_iterator joint_it = group_it.joints_.begin(); joint_it != group_it.joints_.end(); @@ -329,7 +329,7 @@ void PlanningGroupsWidget::loadGroupsTreeRecursive(srdf::Model::Group& group_it, std::string joint_name; // Get the type of joint this is - const robot_model::JointModel* jm = model->getJointModel(*joint_it); + const moveit::core::JointModel* jm = model->getJointModel(*joint_it); if (jm) // check if joint model was found { joint_name = *joint_it + " - " + jm->getTypeName(); @@ -520,7 +520,7 @@ void PlanningGroupsWidget::editSelected() void PlanningGroupsWidget::loadJointsScreen(srdf::Model::Group* this_group) { // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel(); + const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); // Get the names of the all joints const std::vector& joints = model->getJointModelNames(); @@ -552,7 +552,7 @@ void PlanningGroupsWidget::loadJointsScreen(srdf::Model::Group* this_group) void PlanningGroupsWidget::loadLinksScreen(srdf::Model::Group* this_group) { // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel(); + const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); // Get the names of the all links const std::vector& links = model->getLinkModelNames(); @@ -1428,7 +1428,7 @@ void PlanningGroupsWidget::previewSelectedJoints(std::vector joints for (const std::string& joint : joints) { - const robot_model::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); + const moveit::core::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); // Check that a joint model was found if (!joint_model) diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index c83e5e9a45..2bb46d6051 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -357,10 +357,10 @@ void RobotPosesWidget::showPose(srdf::Model::GroupState* pose) void RobotPosesWidget::showDefaultPose() { // Get list of all joints for the robot - std::vector joint_models = config_data_->getRobotModel()->getJointModels(); + std::vector joint_models = config_data_->getRobotModel()->getJointModels(); // Iterate through the joints - for (std::vector::const_iterator joint_it = joint_models.begin(); + for (std::vector::const_iterator joint_it = joint_models.begin(); joint_it < joint_models.end(); ++joint_it) { // Check that this joint only represents 1 variable. @@ -508,12 +508,13 @@ void RobotPosesWidget::loadJointSliders(const QString& selected) joint_list_widget_->setMinimumSize(50, 50); // w, h // Get list of associated joints - const robot_model::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* joint_model_group = + config_data_->getRobotModel()->getJointModelGroup(group_name); joint_models_ = joint_model_group->getJointModels(); // Iterate through the joints int num_joints = 0; - for (const robot_model::JointModel* joint_model : joint_models_) + for (const moveit::core::JointModel* joint_model : joint_models_) { if (joint_model->getVariableCount() != 1 || // only consider 1-variable joints joint_model->isPassive() || // ignore passive @@ -676,7 +677,7 @@ void RobotPosesWidget::doneEditing() searched_data->joint_values_.clear(); // Iterate through the current group's joints and add to SRDF - for (std::vector::const_iterator joint_it = joint_models_.begin(); + for (std::vector::const_iterator joint_it = joint_models_.begin(); joint_it < joint_models_.end(); ++joint_it) { // Check that this joint only represents 1 variable. @@ -808,7 +809,7 @@ void RobotPosesWidget::publishJoints() // Create a planning scene message moveit_msgs::DisplayRobotState msg; - robot_state::robotStateToRobotStateMsg(config_data_->getPlanningScene()->getCurrentState(), msg.state); + moveit::core::robotStateToRobotStateMsg(config_data_->getPlanningScene()->getCurrentState(), msg.state); // Publish! pub_robot_state_.publish(msg); @@ -840,7 +841,7 @@ void RobotPosesWidget::publishJoints() // ****************************************************************************************** // Simple widget for adjusting joints of a robot // ****************************************************************************************** -SliderWidget::SliderWidget(QWidget* parent, const robot_model::JointModel* joint_model, double init_value) +SliderWidget::SliderWidget(QWidget* parent, const moveit::core::JointModel* joint_model, double init_value) : QWidget(parent), joint_model_(joint_model) { // Create layouts diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.h b/moveit_setup_assistant/src/widgets/robot_poses_widget.h index bdf6aa3e2f..a73a035378 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.h +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.h @@ -156,7 +156,7 @@ private Q_SLOTS: std::map joint_state_map_; /// The joints currently in the selected planning group - std::vector joint_models_; + std::vector joint_models_; /// Remember the publisher for quick publishing later ros::Publisher pub_robot_state_; @@ -237,7 +237,7 @@ class SliderWidget : public QWidget * @param parent - parent QWidget * @param joint_model_ - a ptr reference to the joint this widget represents */ - SliderWidget(QWidget* parent, const robot_model::JointModel* joint_model, double init_value); + SliderWidget(QWidget* parent, const moveit::core::JointModel* joint_model, double init_value); /** * Deconstructor @@ -279,7 +279,7 @@ private Q_SLOTS: // ****************************************************************************************** // Ptr to the joint's data - const robot_model::JointModel* joint_model_; + const moveit::core::JointModel* joint_model_; // Max & min position double max_position_; diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index 9922043012..aa13f54ab4 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -283,7 +283,7 @@ void ROSControllersWidget::focusGiven() void ROSControllersWidget::loadJointsScreen(moveit_setup_assistant::ROSControlConfig* this_controller) { // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel(); + const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); // Get the names of the all joints const std::vector& joints = model->getJointModelNames(); @@ -473,7 +473,7 @@ void ROSControllersWidget::previewSelectedJoints(std::vector joints for (const std::string& joint : joints) { - const robot_model::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); + const moveit::core::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); // Check that a joint model was found if (!joint_model) @@ -608,14 +608,14 @@ void ROSControllersWidget::saveJointsGroupsScreen() for (int i = 0; i < joint_groups_widget_->selected_data_table_->rowCount(); ++i) { // Get list of associated joints - const robot_model::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup( + const moveit::core::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup( joint_groups_widget_->selected_data_table_->item(i, 0)->text().toStdString()); - const std::vector& joint_models = joint_model_group->getActiveJointModels(); + const std::vector& joint_models = joint_model_group->getActiveJointModels(); // Iterate through the joints - for (const robot_model::JointModel* joint : joint_models) + for (const moveit::core::JointModel* joint : joint_models) { - if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == robot_model::JointModel::FIXED) + if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED) continue; searched_controller->joints_.push_back(joint->getName()); } diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index fee2c0455b..8059a09ff8 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -420,7 +420,7 @@ void SetupAssistantWidget::loadRviz() // ****************************************************************************************** void SetupAssistantWidget::highlightLink(const std::string& link_name, const QColor& color) { - const robot_model::LinkModel* lm = config_data_->getRobotModel()->getLinkModel(link_name); + const moveit::core::LinkModel* lm = config_data_->getRobotModel()->getLinkModel(link_name); if (!lm->getShapes().empty()) // skip links with no geometry robot_state_display_->setLinkColor(link_name, color); } @@ -434,12 +434,13 @@ void SetupAssistantWidget::highlightGroup(const std::string& group_name) if (!config_data_->getRobotModel()->hasJointModelGroup(group_name)) return; - const robot_model::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* joint_model_group = + config_data_->getRobotModel()->getJointModelGroup(group_name); if (joint_model_group) { - const std::vector& link_models = joint_model_group->getLinkModels(); + const std::vector& link_models = joint_model_group->getLinkModels(); // Iterate through the links - for (std::vector::const_iterator link_it = link_models.begin(); + for (std::vector::const_iterator link_it = link_models.begin(); link_it < link_models.end(); ++link_it) highlightLink((*link_it)->getName(), QColor(255, 0, 0)); } diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp index 1080649b48..a988648038 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp @@ -359,10 +359,10 @@ void VirtualJointsWidget::loadChildLinksComboBox() child_link_field_->clear(); // Get all links in robot model - std::vector link_models = config_data_->getRobotModel()->getLinkModels(); + std::vector link_models = config_data_->getRobotModel()->getLinkModels(); // Add all links to combo box - for (std::vector::const_iterator link_it = link_models.begin(); + for (std::vector::const_iterator link_it = link_models.begin(); link_it < link_models.end(); ++link_it) { child_link_field_->addItem((*link_it)->getName().c_str()); From ece0b7b44da2cfcf178e557557bc9d2693d17d5f Mon Sep 17 00:00:00 2001 From: Ayush Garg Date: Wed, 4 Mar 2020 01:54:15 -0500 Subject: [PATCH 205/612] Improve documentation for setJointPositions() (#1921) --- .../robot_state/include/moveit/robot_state/robot_state.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index ecd5a6c3d0..00004d0247 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -487,7 +487,9 @@ class RobotState /** @} */ - /** \name Getting and setting joint positions, velocities, accelerations and effort + /** \name Getting and setting joint positions, velocities, accelerations and effort for a single joint + * The joint might be multi-DOF, i.e. require more than one variable to set. + * See setVariablePositions(), setVariableVelocities(), setVariableEffort() to handle multiple joints. * @{ */ void setJointPositions(const std::string& joint_name, const double* position) From cc60d023cc1b835b2b3456598e2420c046dd33ef Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 5 Mar 2020 20:46:17 +0100 Subject: [PATCH 206/612] MSA: robot poses should be display on selection, not on click (#1930) Moving through the list with cursor keys does not work without this patch. --- moveit_setup_assistant/src/widgets/robot_poses_widget.cpp | 4 ++-- moveit_setup_assistant/src/widgets/robot_poses_widget.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index 2bb46d6051..1e779c5f9e 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -122,7 +122,7 @@ QWidget* RobotPosesWidget::createContentsWidget() data_table_->setSortingEnabled(true); data_table_->setSelectionBehavior(QAbstractItemView::SelectRows); connect(data_table_, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(editDoubleClicked(int, int))); - connect(data_table_, SIGNAL(cellClicked(int, int)), this, SLOT(previewClicked(int, int))); + connect(data_table_, SIGNAL(currentCellChanged(int, int, int, int)), this, SLOT(previewClicked(int, int, int, int))); layout->addWidget(data_table_); // Set header labels @@ -317,7 +317,7 @@ void RobotPosesWidget::editDoubleClicked(int /*row*/, int /*column*/) // ****************************************************************************************** // Preview whatever element is selected // ****************************************************************************************** -void RobotPosesWidget::previewClicked(int row, int /*column*/) +void RobotPosesWidget::previewClicked(int row, int /*column*/, int /*previous_row*/, int /*previous_column*/) { const std::string& name = data_table_->item(row, 0)->text().toStdString(); const std::string& group = data_table_->item(row, 1)->text().toStdString(); diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.h b/moveit_setup_assistant/src/widgets/robot_poses_widget.h index a73a035378..d1dee54b10 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.h +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.h @@ -110,7 +110,7 @@ private Q_SLOTS: void editDoubleClicked(int row, int column); /// Preview whatever element is selected - void previewClicked(int row, int column); + void previewClicked(int row, int column, int previous_row, int previous_column); /// Delete currently editing ite void deleteSelected(); From 223e3a1d35cc10f1412a7d6d9b622ab164253ded Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 6 Mar 2020 00:11:14 -0700 Subject: [PATCH 207/612] MSA: Restore display of current directory (#1932) This reverts #1618. --- moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 8059a09ff8..61fd9145c9 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -48,7 +48,6 @@ #include #include #include -#include #include // for loading all avail kinematic planners // Rviz DIAGNOSTIC_PUSH @@ -118,7 +117,7 @@ SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, const boost::program } else { - start_screen_widget_->stack_path_->setPath(QDir::currentPath()); + start_screen_widget_->stack_path_->setPath(QString(getenv("PWD"))); } // Add Navigation Buttons (but do not load widgets yet except start screen) From fd213f43f0451ee54e128fb421f53dbef480fc75 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Fri, 6 Mar 2020 16:14:13 +0900 Subject: [PATCH 208/612] MSA: Add initial poses to fake_controllers.yaml (#1892) --- .../src/tools/moveit_config_data.cpp | 42 +++++++++++++++++++ .../src/widgets/robot_poses_widget.cpp | 3 +- 2 files changed, 44 insertions(+), 1 deletion(-) diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 00b9d7d1ee..8675a67ac4 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -529,7 +529,49 @@ bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path) emitter << YAML::EndSeq; emitter << YAML::EndMap; } + emitter << YAML::EndSeq; + + // Add an initial pose for each group + emitter << YAML::Key << "initial" << YAML::Comment("Define initial robot poses."); + + bool poses_found = false; + std::string default_group_name; + for (const srdf::Model::Group& group : srdf_->groups_) + { + if (default_group_name.empty()) + default_group_name = group.name_; + for (const srdf::Model::GroupState& group_state : srdf_->group_states_) + { + if (group.name_ == group_state.group_) + { + if (!poses_found) + { + poses_found = true; + emitter << YAML::Value << YAML::BeginSeq; + } + emitter << YAML::BeginMap; + emitter << YAML::Key << "group"; + emitter << YAML::Value << group.name_; + emitter << YAML::Key << "pose"; + emitter << YAML::Value << group_state.name_; + emitter << YAML::EndMap; + break; + } + } + } + if (poses_found) + emitter << YAML::EndSeq; + else + { + // Add commented lines to show how the feature can be used + if (default_group_name.empty()) + default_group_name = "group"; + emitter << YAML::Newline; + emitter << YAML::Comment(" - group: " + default_group_name) << YAML::Newline; + emitter << YAML::Comment(" pose: home") << YAML::Newline; + } + emitter << YAML::EndMap; std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc); diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index 1e779c5f9e..4f70d8904c 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -64,7 +64,8 @@ RobotPosesWidget::RobotPosesWidget(QWidget* parent, const MoveItConfigDataPtr& c HeaderWidget* header = new HeaderWidget( "Define Robot Poses", "Create poses for the robot. Poses are defined as sets of joint values for " - "particular planning groups. This is useful for things like home position.", + "particular planning groups. This is useful for things like home position." + "The first pose for each robot will be its initial pose in simulation.", this); layout->addWidget(header); From f03d66f9c254a13b540ff7487cfaab2ab10c6e8c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Fri, 6 Mar 2020 15:25:12 +0100 Subject: [PATCH 209/612] MSA: use matching group/state name for default controller state (#1936) The old logic just picked the first group, but this not necessarily matched the picked group state. --- .../tools/moveit_config_data.h | 7 +------ .../src/tools/moveit_config_data.cpp | 21 ++++++------------- 2 files changed, 7 insertions(+), 21 deletions(-) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index c027abde9e..c9bbedb26c 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -481,15 +481,10 @@ class MoveItConfigData */ std::vector > getSensorPluginConfig(); - /** - * \brief Helper function to get the default start state group for moveit_sim_hw_interface - */ - std::string getDefaultStartStateGroup(); - /** * \brief Helper function to get the default start pose for moveit_sim_hw_interface */ - std::string getDefaultStartPose(); + srdf::Model::GroupState getDefaultStartPose(); /** * \brief Custom std::set comparator, used for sorting the joint_limits.yaml file into alphabetical order diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 8675a67ac4..5fa78dc73a 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -846,24 +846,15 @@ void MoveItConfigData::outputFollowJointTrajectoryYAML(YAML::Emitter& emitter, } } -// ****************************************************************************************** -// Helper function to get the default start state group for moveit_sim_hw_interface -// ****************************************************************************************** -std::string MoveItConfigData::getDefaultStartStateGroup() -{ - if (!srdf_->srdf_model_->getGroups().empty()) - return srdf_->srdf_model_->getGroups()[0].name_; - return "todo_no_group_selected"; -} - // ****************************************************************************************** // Helper function to get the default start pose for moveit_sim_hw_interface // ****************************************************************************************** -std::string MoveItConfigData::getDefaultStartPose() +srdf::Model::GroupState MoveItConfigData::getDefaultStartPose() { if (!srdf_->group_states_.empty()) - return srdf_->group_states_[0].name_; - return "todo_no_pose_selected"; + return srdf_->group_states_[0]; + else + return srdf::Model::GroupState{.name_ = "todo_state_name", .group_ = "todo_group_name", .joint_values_ = {} }; } // ****************************************************************************************** @@ -908,11 +899,11 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) { // Use the first planning group if initial joint_model_group was not set, else write a default value emitter << YAML::Key << "joint_model_group"; - emitter << YAML::Value << getDefaultStartStateGroup(); + emitter << YAML::Value << getDefaultStartPose().group_; // Use the first robot pose if initial joint_model_group_pose was not set, else write a default value emitter << YAML::Key << "joint_model_group_pose"; - emitter << YAML::Value << getDefaultStartPose(); + emitter << YAML::Value << getDefaultStartPose().name_; emitter << YAML::EndMap; } From 2169a32b45b340a507921bd9143314a684e96b5f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Fri, 6 Mar 2020 15:27:58 +0100 Subject: [PATCH 210/612] List missing joints in group states (#1935) --- moveit_core/robot_model/src/robot_model.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index ab17d7f32a..fc477150cc 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -355,7 +355,18 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) group_state.name_.c_str(), jt->first.c_str(), jmg->getName().c_str()); } if (!remaining_joints.empty()) - ROS_WARN_NAMED(LOGNAME, "Group state '%s' doesn't specify all group joints.", group_state.name_.c_str()); + { + std::stringstream missing; + missing << (*remaining_joints.begin())->getName(); + for (auto j = ++remaining_joints.begin(); j != remaining_joints.end(); j++) + { + missing << ", " << (*j)->getName(); + } + ROS_WARN_STREAM_NAMED(LOGNAME, "Group state '" << group_state.name_ + << "' doesn't specify all group joints in group '" + << group_state.group_ << "'. " << missing.str() << " " + << (remaining_joints.size() > 1 ? "are" : "is") << " missing."); + } if (!state.empty()) jmg->addDefaultState(group_state.name_, state); } From 643bc5716397b0ce22735b334049b21df2c9a73f Mon Sep 17 00:00:00 2001 From: Dale Koenig Date: Sat, 7 Mar 2020 19:15:58 +0900 Subject: [PATCH 211/612] Wait for get_planning_scene in background (#1934) --- .../planning_scene_rviz_plugin/src/planning_scene_display.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index d0305bedac..4833af109c 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -370,7 +370,8 @@ void PlanningSceneDisplay::changedPlanningSceneTopic() std::string service_name = planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE; if (!getMoveGroupNS().empty()) service_name = ros::names::append(getMoveGroupNS(), service_name); - planning_scene_monitor_->requestPlanningSceneState(service_name); + auto bg_func = [=]() { planning_scene_monitor_->requestPlanningSceneState(service_name); }; + addBackgroundJob(bg_func, "planning_scene_monitor_->requestPlanningSceneState"); } } From 6d99f7b80da0a567bcaddc3cd8583312d336988e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 8 Mar 2020 19:31:00 +0100 Subject: [PATCH 212/612] onRobotModelLoaded() always runs in main GUI thread --- .../motion_planning_rviz_plugin/src/motion_planning_display.cpp | 2 +- .../planning_scene_rviz_plugin/src/planning_scene_display.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 2ce01bcd6f..ada868845a 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -1185,7 +1185,7 @@ void MotionPlanningDisplay::onRobotModelLoaded() if (frame_) frame_->fillPlanningGroupOptions(); - addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedPlanningGroup, this)); + changedPlanningGroup(); } void MotionPlanningDisplay::updateStateExceptModified(moveit::core::RobotState& dest, diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 4833af109c..ef5832503c 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -533,6 +533,7 @@ void PlanningSceneDisplay::loadRobotModel() model_is_loading_ = false; } +// This should always run in the main GUI thread! void PlanningSceneDisplay::onRobotModelLoaded() { changedPlanningSceneTopic(); From 7753624650e53ff5e2f82cb4b722d85232e8bab3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 8 Mar 2020 19:57:17 +0100 Subject: [PATCH 213/612] Provide feedback on success of requestPlanningSceneState() on success: update current start/goal states if necessary on failure: issue warning in display status --- .../motion_planning_display.h | 1 + .../motion_planning_frame.h | 1 + .../src/motion_planning_display.cpp | 4 ++++ .../src/motion_planning_frame_planning.cpp | 12 ++++++++++++ .../planning_scene_display.h | 2 ++ .../src/planning_scene_display.cpp | 13 +++++++++++-- 6 files changed, 31 insertions(+), 2 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index 9561868774..ef8f3accd2 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -196,6 +196,7 @@ private Q_SLOTS: }; void onRobotModelLoaded() override; + void onNewPlanningSceneState() override; void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override; void updateInternal(float wall_dt, float ros_dt) override; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index e228fcec58..a3ebe489b2 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -166,6 +166,7 @@ private Q_SLOTS: void allowLookingToggled(bool checked); void allowExternalProgramCommunication(bool enable); void pathConstraintsIndexChanged(int index); + void onNewPlanningSceneState(); void startStateTextChanged(const QString& start_state); void goalStateTextChanged(const QString& goal_state); void planningGroupTextChanged(const QString& planning_group); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index ada868845a..84eae7c372 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -1187,6 +1187,10 @@ void MotionPlanningDisplay::onRobotModelLoaded() frame_->fillPlanningGroupOptions(); changedPlanningGroup(); } +void MotionPlanningDisplay::onNewPlanningSceneState() +{ + frame_->onNewPlanningSceneState(); +} void MotionPlanningDisplay::updateStateExceptModified(moveit::core::RobotState& dest, const moveit::core::RobotState& src) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 9b2efcff0b..8f8ab01772 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -251,6 +251,18 @@ void MotionPlanningFrame::onFinishedExecution(bool success) goalStateTextChanged(ui_->goal_state_combo_box->currentText()); } +void MotionPlanningFrame::onNewPlanningSceneState() +{ + moveit::core::RobotState current(planning_display_->getPlanningSceneRO()->getCurrentState()); + if (ui_->start_state_combo_box->currentText() == "") + { + planning_display_->setQueryStartState(current); + planning_display_->rememberPreviousStartState(); + } + if (ui_->goal_state_combo_box->currentText() == "") + planning_display_->setQueryGoalState(current); +} + void MotionPlanningFrame::startStateTextChanged(const QString& start_state) { // use background job: fetching the current state might take up to a second diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index 3b24a461c0..d6ecd6848b 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -149,6 +149,8 @@ protected Q_SLOTS: /// This is an event called by loadRobotModel() in the MainLoop; do not call directly virtual void onRobotModelLoaded(); + /// This is called upon successful retrieval of the (initial) planning scene state + virtual void onNewPlanningSceneState(); /** * \brief Set the scene node's position, given the target frame and the planning frame diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index ef5832503c..7701558a58 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -370,8 +370,13 @@ void PlanningSceneDisplay::changedPlanningSceneTopic() std::string service_name = planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE; if (!getMoveGroupNS().empty()) service_name = ros::names::append(getMoveGroupNS(), service_name); - auto bg_func = [=]() { planning_scene_monitor_->requestPlanningSceneState(service_name); }; - addBackgroundJob(bg_func, "planning_scene_monitor_->requestPlanningSceneState"); + auto bg_func = [=]() { + if (planning_scene_monitor_->requestPlanningSceneState(service_name)) + addMainLoopJob(boost::bind(&PlanningSceneDisplay::onNewPlanningSceneState, this)); + else + setStatus(rviz::StatusProperty::Warn, "PlanningScene", "Requesting initial scene failed"); + }; + addBackgroundJob(bg_func, "requestPlanningSceneState"); } } @@ -554,6 +559,10 @@ void PlanningSceneDisplay::onRobotModelLoaded() scene_name_property_->blockSignals(old_state); } +void PlanningSceneDisplay::onNewPlanningSceneState() +{ +} + void PlanningSceneDisplay::sceneMonitorReceivedUpdate( planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) { From 17a472848ec252b33425a03182884131e33c6234 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 8 Mar 2020 20:27:22 +0100 Subject: [PATCH 214/612] Fix segfault in RobotStateVisualization: remove processEvents() (#1941) --- .../rviz_plugin_render_tools/src/robot_state_visualization.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp index f68ed950e3..99528fb88b 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp @@ -66,7 +66,6 @@ void RobotStateVisualization::load(const urdf::ModelInterface& descr, bool visua robot_.setVisualVisible(visual_visible_); robot_.setCollisionVisible(collision_visible_); robot_.setVisible(visible_); - QApplication::processEvents(); } void RobotStateVisualization::clear() From ab3b5a858be329706be8763a6c65d8a1c38e992c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Mon, 9 Mar 2020 09:02:10 +0100 Subject: [PATCH 215/612] Fix issue in unpadded collision checking (#1899) * Add (failing) PlanningScene test: added objects in diff scene are not respected in unpadded collision checking * Unconditionally allocate unpadded collision environments to fix the issue --- .../planning_scene/src/planning_scene.cpp | 25 ++++-------- .../test/test_planning_scene.cpp | 39 +++++++++++++++++-- 2 files changed, 42 insertions(+), 22 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 5f6c75d38e..fa65396f97 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -204,10 +204,8 @@ PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(pare detector->cenv_ = detector->alloc_->allocateEnv(parent_detector->cenv_, world_); detector->cenv_const_ = detector->cenv_; - // leave these empty and use parent collision_env_ unless/until a non-const one - // is requested (e.g. to modify link padding or scale) - detector->cenv_unpadded_.reset(); - detector->cenv_unpadded_const_.reset(); + detector->cenv_unpadded_ = detector->alloc_->allocateEnv(parent_detector->cenv_unpadded_, world_); + detector->cenv_unpadded_const_ = detector->cenv_unpadded_; } setActiveCollisionDetector(parent_->getActiveCollisionDetectorName()); } @@ -286,12 +284,8 @@ void PlanningScene::addCollisionDetector(const collision_detection::CollisionDet detector->copyPadding(*active_collision_); } - // If we don't have a parent, allocate unpadded versions, otherwise use the parent's cenv_unpadded_. - if (!detector->parent_) - { - detector->cenv_unpadded_ = detector->alloc_->allocateEnv(world_, getRobotModel()); - detector->cenv_unpadded_const_ = detector->cenv_unpadded_; - } + detector->cenv_unpadded_ = detector->alloc_->allocateEnv(world_, getRobotModel()); + detector->cenv_unpadded_const_ = detector->cenv_unpadded_; } void PlanningScene::setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator, @@ -393,11 +387,11 @@ void PlanningScene::clearDiffs() if (it.second->parent_) { - it.second->cenv_unpadded_.reset(); - it.second->cenv_unpadded_const_.reset(); - it.second->cenv_ = it.second->alloc_->allocateEnv(it.second->parent_->cenv_, world_); it.second->cenv_const_ = it.second->cenv_; + + it.second->cenv_unpadded_ = it.second->alloc_->allocateEnv(it.second->parent_->cenv_unpadded_, world_); + it.second->cenv_unpadded_const_ = it.second->cenv_unpadded_; } else { @@ -1138,11 +1132,6 @@ void PlanningScene::decoupleParent() for (std::pair& it : collision_) { - if (!it.second->cenv_unpadded_) - { - it.second->cenv_unpadded_ = it.second->alloc_->allocateEnv(it.second->parent_->getCollisionEnvUnpadded(), world_); - it.second->cenv_unpadded_const_ = it.second->cenv_unpadded_; - } it.second->parent_.reset(); } world_diff_.reset(); diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 6a3b52875e..37cddc3296 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -100,32 +100,57 @@ TEST(PlanningScene, LoadRestoreDiff) planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); collision_detection::World& world = *ps->getWorldNonConst(); + + /* add one object to ps's world */ Eigen::Isometry3d id = Eigen::Isometry3d::Identity(); world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id); + /* ps can be written to and set from message */ moveit_msgs::PlanningScene ps_msg; ps_msg.robot_state.is_diff = true; EXPECT_TRUE(moveit::core::isEmpty(ps_msg)); ps->getPlanningSceneMsg(ps_msg); ps->setPlanningSceneMsg(ps_msg); - EXPECT_FALSE(moveit::core::isEmpty(ps_msg)); + EXPECT_EQ(ps_msg.world.collision_objects.size(), 1u); + EXPECT_EQ(ps_msg.world.collision_objects[0].id, "sphere"); EXPECT_TRUE(world.hasObject("sphere")); + /* test diff scene on top of ps */ planning_scene::PlanningScenePtr next = ps->diff(); + /* world is inherited from ps */ EXPECT_TRUE(next->getWorld()->hasObject("sphere")); - next->getWorldNonConst()->addToObject("sphere2", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id); + + /* object in overlay is only added in overlay */ + next->getWorldNonConst()->addToObject("sphere_in_next_only", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id); EXPECT_EQ(next->getWorld()->size(), 2u); EXPECT_EQ(ps->getWorld()->size(), 1u); + + /* the worlds used for collision detection contain one and two objects, respectively */ + EXPECT_EQ(ps->getCollisionEnv()->getWorld()->size(), 1u); + EXPECT_EQ(ps->getCollisionEnvUnpadded()->getWorld()->size(), 1u); + + EXPECT_EQ(next->getCollisionEnv()->getWorld()->size(), 2u); + EXPECT_EQ(next->getCollisionEnvUnpadded()->getWorld()->size(), 2u); + + /* maintained diff contains only overlay object */ next->getPlanningSceneDiffMsg(ps_msg); EXPECT_EQ(ps_msg.world.collision_objects.size(), 1u); + + /* copy ps to next and apply diff */ next->decoupleParent(); moveit_msgs::PlanningScene ps_msg2; + + /* diff is empty now */ next->getPlanningSceneDiffMsg(ps_msg2); EXPECT_EQ(ps_msg2.world.collision_objects.size(), 0u); + + /* next's world contains both objects */ next->getPlanningSceneMsg(ps_msg); EXPECT_EQ(ps_msg.world.collision_objects.size(), 2u); ps->setPlanningSceneMsg(ps_msg); EXPECT_EQ(ps->getWorld()->size(), 2u); + EXPECT_EQ(ps->getCollisionEnv()->getWorld()->size(), 2u); + EXPECT_EQ(ps->getCollisionEnvUnpadded()->getWorld()->size(), 2u); } TEST(PlanningScene, MakeAttachedDiff) @@ -136,21 +161,27 @@ TEST(PlanningScene, MakeAttachedDiff) planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); + /* add a single object to ps's world */ collision_detection::World& world = *ps->getWorldNonConst(); Eigen::Isometry3d id = Eigen::Isometry3d::Identity(); world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id); + /* attach object in diff */ planning_scene::PlanningScenePtr attached_object_diff_scene = ps->diff(); moveit_msgs::AttachedCollisionObject att_obj; att_obj.link_name = "r_wrist_roll_link"; att_obj.object.operation = moveit_msgs::CollisionObject::ADD; att_obj.object.id = "sphere"; + attached_object_diff_scene->processAttachedCollisionObjectMsg(att_obj); + + /* object is not in world anymore */ + EXPECT_EQ(attached_object_diff_scene->getWorld()->size(), 0u); + /* it became part of the robot state though */ + EXPECT_TRUE(attached_object_diff_scene->getCurrentState().hasAttachedBody("sphere")); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - - attached_object_diff_scene->processAttachedCollisionObjectMsg(att_obj); attached_object_diff_scene->checkCollision(req, res); ps->checkCollision(req, res); } From 5ae179fc5aae15b0bc92bf5298d080202e91090a Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Mon, 9 Mar 2020 17:27:12 +0900 Subject: [PATCH 216/612] Add default velocity/acceleration scaling factors (#1890) * Add default velocity/acceleration scaling factors * allow for scaling factors of 0.0 to specify defaults * Add documentation and migration notes --- MIGRATION.md | 5 +++ .../src/moveit_commander/move_group.py | 6 ++-- .../move_group_interface.h | 12 +++---- .../src/move_group_interface.cpp | 36 +++++++++++++++---- .../src/motion_planning_frame.cpp | 7 ++++ .../src/tools/moveit_config_data.cpp | 27 +++++++++----- 6 files changed, 71 insertions(+), 22 deletions(-) diff --git a/MIGRATION.md b/MIGRATION.md index 2c2da8af98..846f4b28f1 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,6 +3,11 @@ API changes in MoveIt releases ## ROS Noetic (upcoming changes in master) +- Planned trajectories are *slow* by default. + The default values of the `velocity_scaling_factor` and `acceleration_scaling_factor` can now be customized and default to 0.1 instead of 1.0. + The values can be changed by setting the parameters `default_acceleration_scaling_factor` and `default_velocity_scaling_factor` in the `joint_limits.yaml` of your robot's `moveit_config` package. + Consider setting them to values between 0.2 and 0.6, to allow for significant speedup/slowdown of your application. + Additionally, you can always change the factors per request with the corresponding methods in the [move_group_interface](http://docs.ros.org/melodic/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroupInterface.html#a3e2bd2edccca8aa49a6bec9d039d5bf3), the [MoveGroupCommander](http://docs.ros.org/melodic/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html#a7706effa66a0847496de477cf219a562) or in the Rviz interface. ([1890](https://github.com/ros-planning/moveit/pull/1890)) - Extended the return value of `MoveitCommander.MoveGroup.plan()` from `trajectory` to a tuple of `(success, trajectory, planning_time, error_code)` to better match the C++ MoveGroupInterface ([790](https://github.com/ros-planning/moveit/pull/790/)) - `moveit_rviz.launch`, generated by MSA, provides an argument `rviz_config` to configure the rviz config to be used. The old boolean config argument was dropped. ([1397](https://github.com/ros-planning/moveit/pull/1397)) - Moved the example package `moveit_controller_manager_example` into [moveit_tutorials](https://github.com/ros-planning/moveit_tutorials) diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index 0799e77c50..8290e880ea 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/moveit_commander/src/moveit_commander/move_group.py @@ -468,14 +468,16 @@ def set_workspace(self, ws): raise MoveItCommanderException("Expected 0, 4 or 6 values in list specifying workspace") def set_max_velocity_scaling_factor(self, value): - """ Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,1]. """ + """ Set a scaling factor to reduce the maximum joint velocities. Allowed values are in (0,1]. + The default value is set in the joint_limits.yaml of the moveit_config package. """ if value > 0 and value <= 1: self._g.set_max_velocity_scaling_factor(value) else: raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor") def set_max_acceleration_scaling_factor(self, value): - """ Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]. """ + """ Set a scaling factor to reduce the maximum joint accelerations. Allowed values are in (0,1]. + The default value is set in the joint_limits.yaml of the moveit_config package. """ if value > 0 and value <= 1: self._g.set_max_acceleration_scaling_factor(value) else: diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index e792c49487..0bacff0bc6 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -246,16 +246,16 @@ class MoveGroupInterface /** \brief Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,1]. The maximum joint velocity specified - in the robot model is multiplied by the factor. If outside valid range - (importantly, this includes it being set to 0.0), the factor is set to a - default value of 1.0 internally (i.e. maximum joint velocity) */ + in the robot model is multiplied by the factor. If the value is 0, it is set to + the default value, which is defined in joint_limits.yaml of the moveit_config. + If the value is greater than 1, it is set to 1.0. */ void setMaxVelocityScalingFactor(double max_velocity_scaling_factor); /** \brief Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]. The maximum joint acceleration specified - in the robot model is multiplied by the factor. If outside valid range - (importantly, this includes it being set to 0.0), the factor is set to a - default value of 1.0 internally (i.e. maximum joint acceleration) */ + in the robot model is multiplied by the factor. If the value is 0, it is set to + the default value, which is defined in joint_limits.yaml of the moveit_config. + If the value is greater than 1, it is set to 1.0. */ void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor); /** \brief Get the number of seconds set by setPlanningTime() */ diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 35a9d39352..9f7d75fac7 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -124,8 +124,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl goal_orientation_tolerance_ = 1e-3; // ~0.1 deg allowed_planning_time_ = 5.0; num_planning_attempts_ = 1; - max_velocity_scaling_factor_ = 1.0; - max_acceleration_scaling_factor_ = 1.0; + node_handle_.param("robot_description_planning/joint_limits/default_velocity_scaling_factor", + max_velocity_scaling_factor_, 0.1); + node_handle_.param("robot_description_planning/joint_limits/default_acceleration_scaling_factor", + max_acceleration_scaling_factor_, 0.1); initializing_constraints_ = false; if (joint_model_group_->isChain()) @@ -335,14 +337,36 @@ class MoveGroupInterface::MoveGroupInterfaceImpl num_planning_attempts_ = num_planning_attempts; } - void setMaxVelocityScalingFactor(double max_velocity_scaling_factor) + void setMaxVelocityScalingFactor(double value) { - max_velocity_scaling_factor_ = max_velocity_scaling_factor; + setMaxScalingFactor(max_velocity_scaling_factor_, value, "velocity_scaling_factor", 0.1); } - void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) + void setMaxAccelerationScalingFactor(double value) { - max_acceleration_scaling_factor_ = max_acceleration_scaling_factor; + setMaxScalingFactor(max_acceleration_scaling_factor_, value, "acceleration_scaling_factor", 0.1); + } + + void setMaxScalingFactor(double& variable, const double target_value, const char* factor_name, double fallback_value) + { + if (target_value > 1.0) + { + ROS_WARN_NAMED("move_group_interface", "Limiting max_%s (%.2f) to 1.0.", factor_name, target_value); + variable = 1.0; + } + else if (target_value <= 0.0) + { + node_handle_.param(std::string("robot_description_planning/default_") + factor_name, variable, + fallback_value); + if (target_value < 0.0) + { + ROS_WARN_NAMED("move_group_interface", "max_%s < 0.0! Setting to default: %.2f.", factor_name, variable); + } + } + else + { + variable = target_value; + } } moveit::core::RobotState& getTargetRobotState() diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index c81aee0e15..1d466c186a 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -73,6 +73,13 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: connect(planning_display_, SIGNAL(queryStartStateChanged()), joints_tab_, SLOT(queryStartStateChanged())); connect(planning_display_, SIGNAL(queryGoalStateChanged()), joints_tab_, SLOT(queryGoalStateChanged())); + // Set initial velocity and acceleration scaling factors from ROS parameters + double factor; + nh_.param("robot_description_planning/default_velocity_scaling_factor", factor, 0.1); + ui_->velocity_scaling_factor->setValue(factor); + nh_.param("robot_description_planning/default_acceleration_scaling_factor", factor, 0.1); + ui_->acceleration_scaling_factor->setValue(factor); + // connect bottons to actions; each action usually registers the function pointer for the actual computation, // to keep the GUI more responsive (using the background job processing) connect(ui_->plan_button, SIGNAL(clicked()), this, SLOT(planButtonClicked())); diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 5fa78dc73a..c791a31d8c 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -1085,8 +1085,27 @@ bool MoveItConfigData::output3DSensorPluginYAML(const std::string& file_path) bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) { YAML::Emitter emitter; + emitter << YAML::Comment("joint_limits.yaml allows the dynamics properties specified in the URDF " + "to be overwritten or augmented as needed"); + emitter << YAML::Newline; + emitter << YAML::BeginMap; + emitter << YAML::Comment("For beginners, we downscale velocity and acceleration limits.") << YAML::Newline; + emitter << YAML::Comment("You can always specify higher scaling factors (<= 1.0) in your motion requests."); + emitter << YAML::Comment("Increase the values below to 1.0 to always move at maximum speed."); + emitter << YAML::Key << "default_velocity_scaling_factor"; + emitter << YAML::Value << "0.1"; + + emitter << YAML::Key << "default_acceleration_scaling_factor"; + emitter << YAML::Value << "0.1"; + + emitter << YAML::Newline << YAML::Newline; + emitter << YAML::Comment("Specific joint properties can be changed with the keys " + "[max_position, min_position, max_velocity, max_acceleration]") + << YAML::Newline; + emitter << YAML::Comment("Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]"); + emitter << YAML::Key << "joint_limits"; emitter << YAML::Value << YAML::BeginMap; @@ -1151,14 +1170,6 @@ bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) ROS_ERROR_STREAM("Unable to open file for writing " << file_path); return false; } - // Add documentation into joint_limits.yaml - output_stream << "# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or " - "augmented as needed" - << std::endl; - output_stream << "# Specific joint properties can be changed with the keys [max_position, min_position, " - "max_velocity, max_acceleration]" - << std::endl; - output_stream << "# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]" << std::endl; output_stream << emitter.c_str(); output_stream.close(); From 29a6425b1a0965d65735a4b81c7fe4267c64d75e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 4 Mar 2020 15:04:20 +0100 Subject: [PATCH 217/612] Allow to filter for joint when creating a RobotTrajectory message (#1927) --- .../moveit/robot_trajectory/robot_trajectory.h | 3 ++- .../robot_trajectory/src/robot_trajectory.cpp | 15 ++++++++++++--- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 80b4539211..69ff8e0eca 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -213,7 +213,8 @@ class RobotTrajectory double getAverageSegmentDuration() const; - void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory) const; + void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory, + const std::vector& joint_filter = std::vector()) const; /** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required to contain the values diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 475f973374..4fa428de01 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -218,12 +218,13 @@ void RobotTrajectory::clear() duration_from_previous_.clear(); } -void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory) const +void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory, + const std::vector& joint_filter) const { trajectory = moveit_msgs::RobotTrajectory(); if (waypoints_.empty()) return; - const std::vector& jnt = + const std::vector& jnts = group_ ? group_->getActiveJointModels() : robot_model_->getActiveJointModels(); std::vector onedof; @@ -231,7 +232,13 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajec trajectory.joint_trajectory.joint_names.clear(); trajectory.multi_dof_joint_trajectory.joint_names.clear(); - for (const moveit::core::JointModel* active_joint : jnt) + for (const moveit::core::JointModel* active_joint : jnts) + { + // only consider joints listed in joint_filter + if (!joint_filter.empty() && + std::find(joint_filter.begin(), joint_filter.end(), active_joint->getName()) == joint_filter.end()) + continue; + if (active_joint->getVariableCount() == 1) { trajectory.joint_trajectory.joint_names.push_back(active_joint->getName()); @@ -242,6 +249,8 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajec trajectory.multi_dof_joint_trajectory.joint_names.push_back(active_joint->getName()); mdof.push_back(active_joint); } + } + if (!onedof.empty()) { trajectory.joint_trajectory.header.frame_id = robot_model_->getModelFrame(); From 5327e419719a033a8b6b8d04c4d501e6937ce262 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 9 Mar 2020 08:32:52 -0600 Subject: [PATCH 218/612] [jog_arm] SRDF velocity and acceleration limit enforcement (#1863) * Loosen timing-based test tolerances * Rename and comment joint_state variables better * Add a placeholder test for vel/accel limits * Remove useless update of kinematic_state_ * Fix suddenHalt() * Improve Python integration tests --- .../moveit_jog_arm/CMakeLists.txt | 3 + .../include/moveit_jog_arm/jog_calcs.h | 16 +- .../moveit_jog_arm/src/jog_calcs.cpp | 162 ++++++++++++------ .../test/arm_setup/config/jog_settings.yaml | 2 +- .../test/launch/common_test_setup.launch | 34 ++++ ...jog_arm_drift_dimensions_service_test.test | 32 +--- .../test/launch/jog_arm_halt_msg_test.test | 32 +--- .../launch/jog_arm_msg_reception_test.test | 32 +--- .../launch/jog_arm_vel_accel_limits_test.test | 15 ++ .../test_halt_msg/test_jog_arm_halt_msg.py | 8 +- .../test_jog_arm_msg_reception.py | 25 ++- .../test_vel_accel_limits.py | 80 +++++++++ 12 files changed, 287 insertions(+), 154 deletions(-) create mode 100644 moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch create mode 100644 moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test create mode 100755 moveit_experimental/moveit_jog_arm/test/test_vel_accel_limits/test_vel_accel_limits.py diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index 1e05203e71..fc0c787bde 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -135,6 +135,9 @@ if(CATKIN_ENABLE_TESTING) # Python integration test of a service to enable drift in some dimensions add_rostest(test/launch/jog_arm_drift_dimensions_service_test.test) + # Python integration test of velocity and acceleration limits + add_rostest(test/launch/jog_arm_vel_accel_limits_test.test) + # jog_cpp_interface add_rostest_gtest(jog_cpp_interface_test test/jog_cpp_interface_test.test diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 5bbf55cfad..027eeeed6b 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -75,8 +75,6 @@ class JogCalcs // Flag that robot state is up to date, end effector transform is known std::atomic is_initialized_; - sensor_msgs::JointState incoming_joints_; - bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables, std::mutex& mutex); bool jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables); @@ -93,11 +91,15 @@ class JogCalcs // Suddenly halt for a joint limit or other critical issue. // Is handled differently for position vs. velocity control. void suddenHalt(trajectory_msgs::JointTrajectory& joint_traj); + void suddenHalt(Eigen::ArrayXd& delta_theta); void publishWarning(bool active) const; - // Scale the delta theta to match joint velocity limits - bool enforceSRDFJointBounds(trajectory_msgs::JointTrajectory& new_joint_traj); + // Scale the delta theta to match joint velocity/acceleration limits + void enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta); + + // Avoid overshooting joint limits + bool enforceSRDFPositionLimits(trajectory_msgs::JointTrajectory& new_joint_traj); // Possibly calculate a velocity scaling factor, due to proximity of // singularity and direction of motion @@ -142,7 +144,10 @@ class JogCalcs moveit::core::RobotStatePtr kinematic_state_; - sensor_msgs::JointState joint_state_, original_joint_state_; + // incoming_joint_state_ is the incoming message. It may contain passive joints or other joints we don't care about. + // internal_joint_state_ is used in jog calculations. It shouldn't be relied on to be accurate. + // original_joint_state_ is the same as incoming_joint_state_ except it only contains the joints jog_arm acts on. + sensor_msgs::JointState incoming_joint_state_, internal_joint_state_, original_joint_state_; std::map joint_state_name_map_; trajectory_msgs::JointTrajectory outgoing_command_; @@ -157,6 +162,7 @@ class JogCalcs // Use ArrayXd type to enable more coefficient-wise operations Eigen::ArrayXd delta_theta_; + Eigen::ArrayXd prev_joint_velocity_; Eigen::Isometry3d tf_moveit_to_cmd_frame_; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 4ec6da3c0c..ca8de0b921 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -61,6 +61,7 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader: kinematic_state_->setToDefaultValues(); joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); + prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getVariableCount()); } void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) @@ -75,15 +76,15 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) ros::topic::waitForMessage(parameters_.joint_topic); ROS_INFO_NAMED(LOGNAME, "jog_calcs_thread: Received first joint msg."); - joint_state_.name = joint_model_group_->getVariableNames(); - num_joints_ = joint_state_.name.size(); - joint_state_.position.resize(num_joints_); - joint_state_.velocity.resize(num_joints_); - joint_state_.effort.resize(num_joints_); + internal_joint_state_.name = joint_model_group_->getVariableNames(); + num_joints_ = internal_joint_state_.name.size(); + internal_joint_state_.position.resize(num_joints_); + internal_joint_state_.velocity.resize(num_joints_); + internal_joint_state_.effort.resize(num_joints_); // A map for the indices of incoming joint commands - for (std::size_t i = 0; i < joint_state_.name.size(); ++i) + for (std::size_t i = 0; i < internal_joint_state_.name.size(); ++i) { - joint_state_name_map_[joint_state_.name[i]] = i; + joint_state_name_map_[internal_joint_state_.name[i]] = i; } // Low-pass filters for the joint positions & velocities @@ -99,7 +100,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) return; mutex.lock(); - incoming_joints_ = shared_variables.joints; + incoming_joint_state_ = shared_variables.joints; mutex.unlock(); default_sleep_rate_.sleep(); } @@ -119,16 +120,16 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) // Ensure the low-pass filter matches reality for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(joint_state_.position[i]); + position_filters_[i].reset(internal_joint_state_.position[i]); // Check for a new command mutex.lock(); cartesian_deltas = shared_variables.command_deltas; joint_deltas = shared_variables.joint_command_deltas; - incoming_joints_ = shared_variables.joints; + incoming_joint_state_ = shared_variables.joints; mutex.unlock(); - kinematic_state_->setVariableValues(joint_state_); + kinematic_state_->setVariableValues(internal_joint_state_); // Always update the end-effector transform in case the getCommandFrameTransform() method is being used // Get the transform from MoveIt planning frame to jogging command frame @@ -158,11 +159,11 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) { default_sleep_rate_.sleep(); } - kinematic_state_->setVariableValues(joint_state_); // Get the transform from MoveIt planning frame to jogging command frame // We solve (planning_frame -> base -> robot_link_command_frame) // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) + kinematic_state_->setVariableValues(incoming_joint_state_); tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); mutex.lock(); @@ -174,7 +175,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) if (halt_outgoing_jog_cmds_) { for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(joint_state_.position[i]); + position_filters_[i].reset(internal_joint_state_.position[i]); } // Do jogging calculations only if the robot should move, for efficiency else @@ -206,7 +207,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) } else { - outgoing_command_ = composeJointTrajMessage(joint_state_); + outgoing_command_ = composeJointTrajMessage(internal_joint_state_); } // Halt if the command is stale or inputs are all zero, or commands were zero @@ -342,14 +343,21 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& delta_theta_ = pseudo_inverse * delta_x; + enforceSRDFAccelVelLimits(delta_theta_); + + if (!addJointIncrements(internal_joint_state_, delta_theta_)) + return false; + // If close to a collision or a singularity, decelerate if (!applyVelocityScaling(shared_variables, mutex, delta_theta_, velocityScalingFactorForSingularity(delta_x, svd, jacobian, pseudo_inverse))) { has_warning_ = true; - suddenHalt(outgoing_command_); + suddenHalt(delta_theta_); } + prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; + return convertDeltasToOutgoingCmd(); } @@ -358,7 +366,7 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& /* // Check for nan's or |delta|>1 in the incoming command for (double velocity : cmd.velocities) { - if (std::isnan(velocity) || (fabs(velocity) > 1)) + if (std::isnan(velocity)) { ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "nan in incoming command. Skipping this datapoint."); return false; @@ -368,24 +376,28 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& /* // Apply user-defined scaling delta_theta_ = scaleJointCommand(cmd); - kinematic_state_->setVariableValues(joint_state_); + enforceSRDFAccelVelLimits(delta_theta_); + + kinematic_state_->setVariableValues(internal_joint_state_); + + prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; return convertDeltasToOutgoingCmd(); } bool JogCalcs::convertDeltasToOutgoingCmd() { - if (!addJointIncrements(joint_state_, delta_theta_)) + if (!addJointIncrements(internal_joint_state_, delta_theta_)) return false; - lowPassFilterPositions(joint_state_); + lowPassFilterPositions(internal_joint_state_); // Calculate joint velocities here so that positions are filtered and SRDF bounds still get checked - calculateJointVelocities(joint_state_, delta_theta_); + calculateJointVelocities(internal_joint_state_, delta_theta_); - outgoing_command_ = composeJointTrajMessage(joint_state_); + outgoing_command_ = composeJointTrajMessage(internal_joint_state_); - if (!enforceSRDFJointBounds(outgoing_command_)) + if (!enforceSRDFPositionLimits(outgoing_command_)) { suddenHalt(outgoing_command_); has_warning_ = true; @@ -539,40 +551,77 @@ double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& comm return velocity_scale; } -bool JogCalcs::enforceSRDFJointBounds(trajectory_msgs::JointTrajectory& new_joint_traj) +void JogCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta) { - bool halting = false; - - if (new_joint_traj.points.empty()) - { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "Empty trajectory passed into checkIfJointsWithinURDFBounds()."); - return true; // technically an empty trajectory is still within bounds - } + Eigen::ArrayXd velocity = delta_theta / parameters_.publish_period; + std::size_t joint_delta_index = 0; for (auto joint : joint_model_group_->getJointModels()) { - if (!kinematic_state_->satisfiesVelocityBounds(joint)) + // Some joints do not have bounds defined + if (kinematic_state_->getJointModel(joint->getName())->hasVariable(joint->getName())) { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, ros::this_node::getName() << " " << joint->getName() << " " - << " close to a " - " velocity limit. Enforcing limit."); - kinematic_state_->enforceVelocityBounds(joint); - for (std::size_t c = 0; c < new_joint_traj.joint_names.size(); ++c) + auto bounds = kinematic_state_->getJointModel(joint->getName())->getVariableBounds(joint->getName()); + + // Apply acceleration bounds + // accel = (vel - vel_prev) / delta_t = ((delta_theta / delta_t) - vel_prev) / delta_t + // --> delta_theta = (accel * delta_t _ + vel_prev) * delta_t + Eigen::ArrayXd acceleration = (velocity - prev_joint_velocity_) / parameters_.publish_period; + if ((bounds.min_acceleration_ != 0) && (acceleration(joint_delta_index) < bounds.min_acceleration_)) + { + double relative_change = + ((bounds.min_acceleration_ * parameters_.publish_period + prev_joint_velocity_(joint_delta_index)) * + parameters_.publish_period) / + delta_theta(joint_delta_index); + // Avoid nan + if (fabs(relative_change) < 1) + delta_theta = relative_change * delta_theta; + } + else if ((bounds.max_acceleration_ != 0) && (acceleration(joint_delta_index) > bounds.max_acceleration_)) + { + double relative_change = + ((bounds.max_acceleration_ * parameters_.publish_period + prev_joint_velocity_(joint_delta_index)) * + parameters_.publish_period) / + delta_theta(joint_delta_index); + // Avoid nan + if (fabs(relative_change) < 1) + delta_theta = relative_change * delta_theta; + } + + velocity = delta_theta / parameters_.publish_period; + // Apply velocity bounds + // delta_theta = joint_velocity * delta_t + if ((bounds.min_velocity_ != 0) && (velocity(joint_delta_index) < bounds.min_velocity_)) { - if (new_joint_traj.joint_names[c] == joint->getName()) + double relative_change = (bounds.min_velocity_ * parameters_.publish_period) / delta_theta(joint_delta_index); + // Avoid nan + if (fabs(relative_change) < 1) { - // TODO(andyz): This is caused by publishing in position mode -- which does not initialize the velocity - // members. - // TODO(andyz): Also need to adjust the joint positions that would be published. - if (new_joint_traj.points[0].velocities.size() > c + 1) - { - new_joint_traj.points[0].velocities[c] = *(kinematic_state_->getJointVelocities(joint)); - break; - } + delta_theta = relative_change * delta_theta; + velocity = relative_change * velocity; } } + else if ((bounds.max_velocity_ != 0) && (velocity(joint_delta_index) > bounds.max_velocity_)) + { + double relative_change = (bounds.max_velocity_ * parameters_.publish_period) / delta_theta(joint_delta_index); + // Avoid nan + if (fabs(relative_change) < 1) + { + delta_theta = relative_change * delta_theta; + velocity = relative_change * velocity; + } + } + ++joint_delta_index; } + } +} + +bool JogCalcs::enforceSRDFPositionLimits(trajectory_msgs::JointTrajectory& new_joint_traj) +{ + bool halting = false; + for (auto joint : joint_model_group_->getJointModels()) + { // Halt if we're past a joint margin and joint velocity is moving even farther past double joint_angle = 0; for (std::size_t c = 0; c < original_joint_state_.name.size(); ++c) @@ -613,6 +662,13 @@ void JogCalcs::publishWarning(bool active) const warning_pub_.publish(status); } +// Suddenly halt for a joint limit or other critical issue. +// Is handled differently for position vs. velocity control. +void JogCalcs::suddenHalt(Eigen::ArrayXd& delta_theta) +{ + delta_theta = Eigen::ArrayXd::Zero(delta_theta.rows()); +} + // Suddenly halt for a joint limit or other critical issue. // Is handled differently for position vs. velocity control. void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_traj) @@ -633,32 +689,32 @@ void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_traj) bool JogCalcs::updateJoints(std::mutex& mutex, const JogArmShared& shared_variables) { mutex.lock(); - incoming_joints_ = shared_variables.joints; + incoming_joint_state_ = shared_variables.joints; mutex.unlock(); // Check that the msg contains enough joints - if (incoming_joints_.name.size() < num_joints_) + if (incoming_joint_state_.name.size() < num_joints_) return false; // Store joints in a member variable - for (std::size_t m = 0; m < incoming_joints_.name.size(); ++m) + for (std::size_t m = 0; m < incoming_joint_state_.name.size(); ++m) { std::size_t c; try { - c = joint_state_name_map_.at(incoming_joints_.name[m]); + c = joint_state_name_map_.at(incoming_joint_state_.name[m]); } catch (const std::out_of_range& e) { - ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joints_.name[m]); + ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_.name[m]); continue; } - joint_state_.position[c] = incoming_joints_.position[m]; + internal_joint_state_.position[c] = incoming_joint_state_.position[m]; } // Cache the original joints in case they need to be reset - original_joint_state_ = joint_state_; + original_joint_state_ = internal_joint_state_; return true; } @@ -712,7 +768,7 @@ Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& comman } catch (const std::out_of_range& e) { - ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joints_.name[m]); + ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_.name[m]); continue; } // Apply user-defined scaling if inputs are unitless [-1:1] diff --git a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml index 6907b090f7..c6ecb8998b 100644 --- a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml +++ b/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml @@ -25,7 +25,7 @@ command_out_type: trajectory_msgs/JointTrajectory # What to publish? Can save some bandwidth as most robots only require positions or velocities publish_joint_positions: true -publish_joint_velocities: true +publish_joint_velocities: false publish_joint_accelerations: false ## MoveIt properties diff --git a/moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch b/moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch new file mode 100644 index 0000000000..d734a2746e --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test index 22b081b990..7bc276f34b 100644 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test @@ -2,38 +2,14 @@ - - - - - - - - - - - - - + + + - + - - - - - - - - - - - diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test index 5f25dfdd33..0243368fcd 100644 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test @@ -2,38 +2,16 @@ - - - - - - - - - - - - - + + + - - - - - - - - - - - + diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test index 343bd87c49..b35c6393f1 100644 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test @@ -2,38 +2,14 @@ - - - - - - - - - - - - - + + + - + - - - - - - - - - - - diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test new file mode 100644 index 0000000000..42fc118672 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py b/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py index 882d12edfb..11e75997f7 100755 --- a/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py +++ b/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py @@ -42,6 +42,7 @@ def send_cmd(self, linear, angular): def test_jog_arm_halt_msg(node): + received = [] sub = rospy.Subscriber( HALT_TOPIC, Bool, lambda msg: received.append(msg) ) @@ -52,15 +53,16 @@ def test_jog_arm_halt_msg(node): # This nonzero command should produce jogging output # A subscriber in a different thread fills `received` TEST_DURATION = 1 - received = [] start_time = rospy.get_rostime() + received = [] while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION: - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) + cartesian_cmd.send_cmd([1, 1, 1], [0, 0, 1]) time.sleep(0.1) # Check the received messages + # A non-zero value signifies a warning assert len(received) > 1 - assert received[-1].data == True + assert received[-1].data != 0 if __name__ == '__main__': diff --git a/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py b/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py index 1aff26396b..7ec28b8298 100755 --- a/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py +++ b/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py @@ -53,12 +53,12 @@ def send_cmd(self, linear, angular): def test_jog_arm_cartesian_command(node): + received = [] sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) cartesian_cmd = CartesianJogCmd() time.sleep(ROS_SETTLE_TIME_S) # wait for pub/subs to settle - time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for jog_arm server to init # Repeated zero-commands should produce no output, other than a few halt messages # A subscriber in a different thread fills 'received' @@ -74,35 +74,41 @@ def test_jog_arm_cartesian_command(node): # A subscriber in a different thread fills `received` TEST_DURATION = 1 PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file - received = [] + + # Send a command to start the jogger + cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) start_time = rospy.get_rostime() + received = [] while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION: cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) time.sleep(0.1) # TEST_DURATION/PUBLISH_PERIOD is the expected number of messages in this duration. # Allow a small +/- window due to rounding/timing errors - assert len(received) >= TEST_DURATION/PUBLISH_PERIOD - 5 - assert len(received) <= TEST_DURATION/PUBLISH_PERIOD + 5 + assert len(received) >= TEST_DURATION/PUBLISH_PERIOD - 20 + assert len(received) <= TEST_DURATION/PUBLISH_PERIOD + 20 def test_jog_arm_joint_command(node): # Test sending a joint command + received = [] sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) joint_cmd = JointJogCmd() time.sleep(ROS_SETTLE_TIME_S) # wait for pub/subs to settle - time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for jog_arm server to init - received = [] TEST_DURATION = 1 PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file velocities = [0.1] + # Send a command to start the jogger + joint_cmd.send_joint_velocity_cmd(velocities) + start_time = rospy.get_rostime() + received = [] while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION: joint_cmd.send_joint_velocity_cmd(velocities) time.sleep(0.1) @@ -113,6 +119,7 @@ def test_jog_arm_joint_command(node): if __name__ == '__main__': - node = node() - test_jog_arm_cartesian_command(node) - test_jog_arm_joint_command(node) + node = node() + time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for jog_arm server to init + test_jog_arm_cartesian_command(node) + test_jog_arm_joint_command(node) diff --git a/moveit_experimental/moveit_jog_arm/test/test_vel_accel_limits/test_vel_accel_limits.py b/moveit_experimental/moveit_jog_arm/test/test_vel_accel_limits/test_vel_accel_limits.py new file mode 100755 index 0000000000..b6ca05dd45 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/test/test_vel_accel_limits/test_vel_accel_limits.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python +import time + +import pytest +import rospy + +from control_msgs.msg import JointJog +from trajectory_msgs.msg import JointTrajectory + +# Test that commands that are too fast are caught and flagged +# This can be run as part of a pytest, or like a normal ROS executable: +# rosrun moveit_jog_arm test_vel_accel_limits.py + +JOG_ARM_SETTLE_TIME_S = 10 +ROS_SETTLE_TIME_S = 10 + +JOINT_JOG_COMMAND_TOPIC = 'jog_server/joint_delta_jog_cmds' + +COMMAND_OUT_TOPIC = 'jog_server/command' + + +@pytest.fixture +def node(): + return rospy.init_node('pytest', anonymous=True) + + +class JointJogCmd(object): + def __init__(self): + self._pub = rospy.Publisher(JOINT_JOG_COMMAND_TOPIC, JointJog, queue_size=1) + + def send_joint_velocity_cmd(self, joint_pos): + jj = JointJog() + jj.header.stamp = rospy.Time.now() + jj.joint_names = ['joint_{}'.format(i) for i in range(len(joint_pos))] + jj.velocities = list(map(float, joint_pos)) + self._pub.publish(jj) + + +def test_vel_limit(node): + # Test sending a joint command + + received = [] + sub = rospy.Subscriber( + COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) + ) + + joint_cmd = JointJogCmd() + time.sleep(ROS_SETTLE_TIME_S) # wait for pub/subs to settle + + TEST_DURATION = 1 + PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file + + # Panda arm limit, from joint_limits.yaml + VELOCITY_LIMIT = rospy.get_param("/robot_description_planning/joint_limits/panda_joint1/max_velocity") + # Send a velocity command that exceeds the limit + velocities = [10 * VELOCITY_LIMIT] + + # Send a command to start the jogger + joint_cmd.send_joint_velocity_cmd(velocities) + + start_time = rospy.get_rostime() + received = [] + while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION: + joint_cmd.send_joint_velocity_cmd(velocities) + time.sleep(0.1) + + # Period of outgoing commands from the jogger, from yaml + JOGGER_COMMAND_PERIOD = rospy.get_param("/jog_server/publish_period") + + # Should be no velocities greater than the limit + assert len(received) > 2 + for msg_idx in range(1, len(received)): + velocity = \ + (received[msg_idx].points[0].positions[0] - received[msg_idx - 1].points[0].positions[0]) / JOGGER_COMMAND_PERIOD + assert abs(velocity) <= VELOCITY_LIMIT + +if __name__ == '__main__': + node = node() + time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for jog_arm server to init + test_vel_limit(node) From caf5ef191c72b2d4fc0f11f21c36fb20b3d271c3 Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Wed, 11 Mar 2020 12:24:56 +0100 Subject: [PATCH 219/612] Release Python GIL for C++ calls (#1947) --- .../src/wrap_python_move_group.cpp | 12 ++ .../moveit/py_bindings_tools/gil_releaser.h | 111 ++++++++++++++++++ .../src/wrap_python_robot_interface.cpp | 3 + 3 files changed, 126 insertions(+) create mode 100644 moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 94639c6f9c..7b2e293c77 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -57,6 +58,8 @@ namespace bp = boost::python; +using moveit::py_bindings_tools::GILReleaser; + namespace moveit { namespace planning_interface @@ -396,6 +399,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer bool movePython() { + GILReleaser gr; return move() == MoveItErrorCode::SUCCESS; } @@ -413,6 +417,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer { MoveGroupInterface::Plan plan; py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); + GILReleaser gr; return execute(plan) == MoveItErrorCode::SUCCESS; } @@ -425,8 +430,10 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer bp::tuple planPython() { + GILReleaser gr; MoveGroupInterface::Plan plan; moveit_msgs::MoveItErrorCodes res = MoveGroupInterface::plan(plan); + gr.reacquire(); return bp::make_tuple(py_bindings_tools::serializeMsg(res), py_bindings_tools::serializeMsg(plan.trajectory_), plan.planning_time_); } @@ -453,8 +460,10 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer std::vector poses; convertListToArrayOfPoses(waypoints, poses); moveit_msgs::RobotTrajectory trajectory; + GILReleaser gr; double fraction = computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions); + gr.reacquire(); return bp::make_tuple(py_bindings_tools::serializeMsg(trajectory), fraction); } @@ -501,6 +510,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer // Convert trajectory message to object moveit_msgs::RobotTrajectory traj_msg; py_bindings_tools::deserializeMsg(traj_str, traj_msg); + GILReleaser gr; robot_trajectory::RobotTrajectory traj_obj(getRobotModel(), getName()); traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg); @@ -523,11 +533,13 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer else { ROS_ERROR_STREAM_NAMED("move_group_py", "Unknown time parameterization algorithm: " << algorithm); + gr.reacquire(); return py_bindings_tools::serializeMsg(moveit_msgs::RobotTrajectory()); } // Convert the retimed trajectory back into a message traj_obj.getRobotTrajectoryMsg(traj_msg); + gr.reacquire(); return py_bindings_tools::serializeMsg(traj_msg); } else diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h new file mode 100644 index 0000000000..697a0a446e --- /dev/null +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h @@ -0,0 +1,111 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, RWTH Aachen University + * 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 RWTH Aachen University 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. + *********************************************************************/ + +/* Author: Bjarne von Horn */ + +#pragma once + +#include +#include + +namespace moveit +{ +namespace py_bindings_tools +{ +/** \brief RAII Helper to release the Global Interpreter Lock (GIL) + * + * Use this helper class to release the GIL before doing long computations + * or blocking calls. Note that without the GIL Python-related functions must not be called. + * So, before releasing the GIL all \c boost::python variables have to be converted to e.g. \c std::vector. + * Before converting the result back to e.g. a moveit::py_bindings_tools::ByteString instance, the GIL has to be + * reacquired. + */ +class GILReleaser +{ + PyThreadState* m_thread_state; + +public: + /** \brief Release the GIL on construction */ + GILReleaser() noexcept + { + m_thread_state = nullptr; + release(); + } + /** \brief Reacquire the GIL on destruction */ + ~GILReleaser() noexcept + { + reacquire(); + } + + GILReleaser(const GILReleaser&) = delete; + GILReleaser(GILReleaser&& other) noexcept + { + m_thread_state = other.m_thread_state; + other.m_thread_state = nullptr; + } + + GILReleaser& operator=(const GILReleaser&) = delete; + GILReleaser& operator=(GILReleaser&& other) noexcept + { + GILReleaser copy(std::move(other)); + this->swap(copy); + return *this; + } + + void swap(GILReleaser& other) noexcept + { + std::swap(other.m_thread_state, m_thread_state); + } + + /** \brief Reacquire the GIL, noop if already acquired */ + void reacquire() noexcept + { + if (m_thread_state) + { + PyEval_RestoreThread(m_thread_state); + m_thread_state = nullptr; + } + } + /** \brief Release the GIL (again), noop if already released */ + void release() noexcept + { + if (!m_thread_state) + { + m_thread_state = PyEval_SaveThread(); + } + } +}; + +} // namespace py_bindings_tools +} // namespace moveit diff --git a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp index f21e58127c..893a8c9bb7 100644 --- a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp +++ b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -49,6 +50,7 @@ /** @cond IGNORE */ namespace bp = boost::python; +using moveit::py_bindings_tools::GILReleaser; namespace moveit { @@ -216,6 +218,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer // if needed, start the monitor and wait up to 1 second for a full robot state if (!current_state_monitor_->isActive()) { + GILReleaser gr; current_state_monitor_->startStateMonitor(); if (!current_state_monitor_->waitForCompleteState(wait)) ROS_WARN("Joint values for monitored state are requested but the full state is not known"); From ad0529f7805101f212ff645c43f51b4b7661f60a Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 12 Mar 2020 10:36:15 +0100 Subject: [PATCH 220/612] Add a pathway to avoid PSI blocking for services It is still impossible to create a PSI if no move_group is running, but this allows users to detect the failure to create an instance. Might be useful for people who want to run the same node with and without a `move_group` node. --- .../planning_scene_interface.h | 7 ++++++- .../src/planning_scene_interface.cpp | 20 ++++++++++++++----- 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index 9dc25385e0..004ecade7f 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h @@ -52,7 +52,12 @@ MOVEIT_CLASS_FORWARD(PlanningSceneInterface); class PlanningSceneInterface { public: - explicit PlanningSceneInterface(const std::string& ns = ""); + /** + \param ns. Namespace in which all MoveIt related topics and services are discovered + \param wait. Wait for services if they are not announced in ROS. + If this is false, the constructor throws std::runtime_error instead. + */ + explicit PlanningSceneInterface(const std::string& ns = "", bool wait = true); ~PlanningSceneInterface(); /** diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index 704372d6ec..3a86a62f22 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -50,7 +50,7 @@ static const std::string LOGNAME = "planning_scene_interface"; class PlanningSceneInterface::PlanningSceneInterfaceImpl { public: - explicit PlanningSceneInterfaceImpl(const std::string& ns = "") + explicit PlanningSceneInterfaceImpl(const std::string& ns = "", bool wait = true) { node_handle_ = ros::NodeHandle(ns); planning_scene_diff_publisher_ = node_handle_.advertise("planning_scene", 1); @@ -59,8 +59,18 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl apply_planning_scene_service_ = node_handle_.serviceClient(move_group::APPLY_PLANNING_SCENE_SERVICE_NAME); - waitForService(planning_scene_service_); - waitForService(apply_planning_scene_service_); + if (wait) + { + waitForService(planning_scene_service_); + waitForService(apply_planning_scene_service_); + } + else + { + if (!planning_scene_service_.exists() || !apply_planning_scene_service_.exists()) + { + throw std::runtime_error("ROS services not available"); + } + } } std::vector getKnownObjectNames(bool with_type) @@ -270,9 +280,9 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl moveit::core::RobotModelConstPtr robot_model_; }; -PlanningSceneInterface::PlanningSceneInterface(const std::string& ns) +PlanningSceneInterface::PlanningSceneInterface(const std::string& ns, bool wait) { - impl_ = new PlanningSceneInterfaceImpl(ns); + impl_ = new PlanningSceneInterfaceImpl(ns, wait); } PlanningSceneInterface::~PlanningSceneInterface() From acaebbf5a9f5172e280b3cf54404ecaab13df6a4 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 12 Mar 2020 10:39:23 +0100 Subject: [PATCH 221/612] remove PSI from rviz display The display maintains a PSM listening to move_group's PSM. Using an additional PSI (connecting to the move_group via services) is bad design and the use of it - to poll via ros-services - is even worse. Implemented the same functionality through the local PSM. Note that the second argument of updateDetectedObjectsList was never used. --- .../motion_planning_frame.h | 4 +- .../src/motion_planning_frame.cpp | 8 ---- .../motion_planning_frame_manipulation.cpp | 42 +++++++++++++------ 3 files changed, 31 insertions(+), 23 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index a3ebe489b2..ac707bf294 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -43,7 +43,6 @@ #ifndef Q_MOC_RUN #include #include -#include #include #include #include @@ -129,7 +128,6 @@ class MotionPlanningFrame : public QWidget MotionPlanningFrameJointsWidget* joints_tab_; moveit::planning_interface::MoveGroupInterfacePtr move_group_; - moveit::planning_interface::PlanningSceneInterfacePtr planning_scene_interface_; moveit::semantic_world::SemanticWorldPtr semantic_world_; moveit::planning_interface::MoveGroupInterface::PlanPtr current_plan_; @@ -272,7 +270,7 @@ private Q_SLOTS: // Pick and place void processDetectedObjects(); - void updateDetectedObjectsList(const std::vector& object_ids, const std::vector& objects); + void updateDetectedObjectsList(const std::vector& object_ids); void publishTables(); void updateSupportSurfacesList(); ros::Publisher object_recognition_trigger_publisher_; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 1d466c186a..ddaac366c3 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -181,14 +181,6 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: object_recognition_client_.reset(); } } - try - { - planning_scene_interface_.reset(new moveit::planning_interface::PlanningSceneInterface()); - } - catch (std::exception& ex) - { - ROS_ERROR("%s", ex.what()); - } try { diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp index bf4b79055b..c130bde929 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp @@ -41,6 +41,8 @@ #include #include +#include + #include "ui_motion_planning_rviz_plugin_frame.h" namespace moveit_rviz_plugin @@ -68,7 +70,7 @@ void MotionPlanningFrame::processDetectedObjects() { pick_object_name_.clear(); - std::vector objects, object_ids; + std::vector object_ids; double min_x = ui_->roi_center_x->value() - ui_->roi_size_x->value() / 2.0; double min_y = ui_->roi_center_y->value() - ui_->roi_size_y->value() / 2.0; double min_z = ui_->roi_center_z->value() - ui_->roi_size_z->value() / 2.0; @@ -80,13 +82,31 @@ void MotionPlanningFrame::processDetectedObjects() ros::Time start_time = ros::Time::now(); while (object_ids.empty() && ros::Time::now() - start_time <= ros::Duration(3.0)) { - object_ids = - planning_scene_interface_->getKnownObjectNamesInROI(min_x, min_y, min_z, max_x, max_y, max_z, true, objects); + // collect all objects in region of interest + { + const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); + const collision_detection::WorldConstPtr& world = ps->getWorld(); + object_ids.clear(); + for (const auto& object : *world) + { + if (!object.second->shape_poses_.empty()) + { + const auto& position = object.second->shape_poses_[0].translation(); + if (position.x() >= min_x && position.x() <= max_x && position.y() >= min_y && position.y() <= max_y && + position.z() >= min_z && position.z() <= max_z) + { + object_ids.push_back(object.first); + } + } + } + if (!object_ids.empty()) + break; + } ros::Duration(0.5).sleep(); } ROS_DEBUG("Found %d objects", (int)object_ids.size()); - updateDetectedObjectsList(object_ids, objects); + updateDetectedObjectsList(object_ids); } void MotionPlanningFrame::selectedDetectedObjectChanged() @@ -153,8 +173,7 @@ void MotionPlanningFrame::listenDetectedObjects(const object_recognition_msgs::R planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::processDetectedObjects, this)); } -void MotionPlanningFrame::updateDetectedObjectsList(const std::vector& object_ids, - const std::vector& /*objects*/) +void MotionPlanningFrame::updateDetectedObjectsList(const std::vector& object_ids) { ui_->detected_objects_list->setUpdatesEnabled(false); bool old_state = ui_->detected_objects_list->blockSignals(true); @@ -265,13 +284,12 @@ void MotionPlanningFrame::pickObjectButtonClicked() { if (semantic_world_) { - std::vector object_names; - object_names.push_back(pick_object_name_[group_name]); - std::map object_poses = planning_scene_interface_->getObjectPoses(object_names); - if (object_poses.find(pick_object_name_[group_name]) != object_poses.end()) + const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); + if (ps->getWorld()->hasObject(pick_object_name_[group_name])) { - ROS_DEBUG("Finding current table for object: %s", pick_object_name_[group_name].c_str()); - support_surface_name_ = semantic_world_->findObjectTable(object_poses[pick_object_name_[group_name]]); + geometry_msgs::Pose object_pose(tf2::toMsg(ps->getWorld()->getTransform(pick_object_name_[group_name]))); + ROS_DEBUG_STREAM("Finding current table for object: " << pick_object_name_[group_name]); + support_surface_name_ = semantic_world_->findObjectTable(object_pose); } else support_surface_name_.clear(); From 703b837549214b3d12525c931bb7568b1a85060e Mon Sep 17 00:00:00 2001 From: edetleon <44973981+edetleon@users.noreply.github.com> Date: Fri, 13 Mar 2020 10:33:14 +0100 Subject: [PATCH 222/612] Implementation of parameter TranslationXY2D IKFast (#1949) I needed to use the parameter TranslationXY2D in IKFast, so I added 2 lines of code. Related issue : #1931 * limit new implementation to IKP_TranslationXY2D Co-authored-by: Robert Haschke --- .../templates/ikfast61_moveit_plugin_template.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index 775d9dc104..c2745fe788 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -602,9 +602,12 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + case IKP_Rotation3D: case IKP_Lookat3D: - case IKP_TranslationXY2D: case IKP_TranslationXYOrientation3D: ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet."); return 0; From 8b2a08145d74250a1d4dea4c3d9b50ecc5020282 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Fri, 13 Mar 2020 13:05:05 +0300 Subject: [PATCH 223/612] Fix versioning (#1948) --- moveit_experimental/moveit_jog_arm/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_experimental/moveit_jog_arm/package.xml b/moveit_experimental/moveit_jog_arm/package.xml index 3673512fc4..931be71bac 100644 --- a/moveit_experimental/moveit_jog_arm/package.xml +++ b/moveit_experimental/moveit_jog_arm/package.xml @@ -1,7 +1,7 @@ moveit_jog_arm - 0.0.3 + 1.0.1 Provides real-time manipulator Cartesian jogging. From 97e3e9bfac616251062fbb60599390120afb42f2 Mon Sep 17 00:00:00 2001 From: Dale Koenig Date: Fri, 13 Mar 2020 20:11:53 +0900 Subject: [PATCH 224/612] Do not override empty URDF link collision geometry (#1952) * Do not override empty collision geometry * minor improvements Co-authored-by: Robert Haschke --- MIGRATION.md | 3 ++- moveit_core/robot_model/src/robot_model.cpp | 27 +++++++++++++-------- 2 files changed, 19 insertions(+), 11 deletions(-) diff --git a/MIGRATION.md b/MIGRATION.md index 846f4b28f1..99b8e20aa8 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,6 +3,7 @@ API changes in MoveIt releases ## ROS Noetic (upcoming changes in master) +- RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link. - Planned trajectories are *slow* by default. The default values of the `velocity_scaling_factor` and `acceleration_scaling_factor` can now be customized and default to 0.1 instead of 1.0. The values can be changed by setting the parameters `default_acceleration_scaling_factor` and `default_velocity_scaling_factor` in the `joint_limits.yaml` of your robot's `moveit_config` package. @@ -12,7 +13,7 @@ API changes in MoveIt releases - `moveit_rviz.launch`, generated by MSA, provides an argument `rviz_config` to configure the rviz config to be used. The old boolean config argument was dropped. ([1397](https://github.com/ros-planning/moveit/pull/1397)) - Moved the example package `moveit_controller_manager_example` into [moveit_tutorials](https://github.com/ros-planning/moveit_tutorials) - Requests to `get_planning_scene` service without explicitly setting "components" now return full scene -- `moveit_ros_plannning` no longer depends on `moveit_ros_perception` +- `moveit_ros_planning` no longer depends on `moveit_ros_perception` - `CollisionRobot` and `CollisionWorld` are combined into a single `CollisionEnv` class. This applies for all derived collision checkers as `FCL`, `ALL_VALID`, `HYBRID` and `DISTANCE_FIELD`. Consequently, `getCollisionRobot[Unpadded] / getCollisionWorld` functions are replaced through a `getCollisionEnv` in the planning scene and return the new combined environment. This unified collision environment provides the union of all member functions of `CollisionRobot` and `CollisionWorld`. Note that calling `checkRobotCollision` of the `CollisionEnv` does not take a `CollisionRobot` as an argument anymore as it is implicitly contained in the `CollisionEnv`. - `RobotTrajectory` provides a copy constructor that allows a shallow (default) and deep copy of waypoints diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index fc477150cc..5f06e8d626 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -990,6 +990,7 @@ LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) EigenSTL::vector_Isometry3d poses; for (const urdf::CollisionSharedPtr& col : col_array) + { if (col && col->geometry) { shapes::ShapeConstPtr s = constructShape(col->geometry.get()); @@ -999,21 +1000,27 @@ LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) poses.push_back(urdfPose2Isometry3d(col->origin)); } } + } + + // Should we warn that old (melodic) behaviour has changed, not copying visual to collision geometries anymore? + bool warn_about_missing_collision = false; if (shapes.empty()) { - const std::vector& vis_array = urdf_link->visual_array.empty() ? - std::vector(1, urdf_link->visual) : + const auto& vis_array = urdf_link->visual_array.empty() ? std::vector{ urdf_link->visual } : urdf_link->visual_array; for (const urdf::VisualSharedPtr& vis : vis_array) + { if (vis && vis->geometry) - { - shapes::ShapeConstPtr s = constructShape(vis->geometry.get()); - if (s) - { - shapes.push_back(s); - poses.push_back(urdfPose2Isometry3d(vis->origin)); - } - } + warn_about_missing_collision = true; + } + } + if (warn_about_missing_collision) + { + ROS_WARN_STREAM_NAMED(LOGNAME + ".empty_collision_geometry", + "Link " << urdf_link->name + << " has visual geometry but no collision geometry. " + "Collision geometry will be left empty. " + "Fix your URDF file by explicitly specifying collision geometry."); } new_link_model->setGeometry(shapes, poses); From e34a07d8970b7f39cdc374d8f3a88c77328a4956 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 13 Mar 2020 09:54:17 -0600 Subject: [PATCH 225/612] jog_arm: use wait_for_service() to fix flaky tests (#1946) --- .../moveit_jog_arm/CMakeLists.txt | 12 +-- .../arm_setup/launch/panda_move_group.launch | 74 ------------------- .../config/initial_position.yaml | 0 .../{arm_setup => }/config/jog_settings.yaml | 0 .../config/singular_position.yaml | 0 .../test/jog_cpp_interface_test.test | 2 +- .../test/launch/common_test_setup.launch | 12 +-- ...jog_arm_drift_dimensions_service_test.test | 4 +- .../test/launch/jog_arm_halt_msg_test.test | 6 +- .../launch/jog_arm_msg_reception_test.test | 4 +- .../launch/jog_arm_vel_accel_limits_test.test | 4 +- .../test/python_tests/__init__.py | 0 .../test_drift_dimensions_service.py | 20 +++-- .../halt_msg}/test_jog_arm_halt_msg.py | 15 ++-- .../test_jog_arm_msg_reception.py | 20 +++-- .../moveit_jog_arm/test/python_tests/util.py | 10 +++ .../test_vel_accel_limits.py | 16 ++-- 17 files changed, 73 insertions(+), 126 deletions(-) delete mode 100644 moveit_experimental/moveit_jog_arm/test/arm_setup/launch/panda_move_group.launch rename moveit_experimental/moveit_jog_arm/test/{arm_setup => }/config/initial_position.yaml (100%) rename moveit_experimental/moveit_jog_arm/test/{arm_setup => }/config/jog_settings.yaml (100%) rename moveit_experimental/moveit_jog_arm/test/{arm_setup => }/config/singular_position.yaml (100%) create mode 100644 moveit_experimental/moveit_jog_arm/test/python_tests/__init__.py rename moveit_experimental/moveit_jog_arm/test/{test_drift_dimensions_service => python_tests/drift_dimensions_service}/test_drift_dimensions_service.py (65%) rename moveit_experimental/moveit_jog_arm/test/{test_halt_msg => python_tests/halt_msg}/test_jog_arm_halt_msg.py (85%) rename moveit_experimental/moveit_jog_arm/test/{test_msg_reception => python_tests/msg_reception}/test_jog_arm_msg_reception.py (90%) create mode 100644 moveit_experimental/moveit_jog_arm/test/python_tests/util.py rename moveit_experimental/moveit_jog_arm/test/{test_vel_accel_limits => python_tests/vel_accel_limits}/test_vel_accel_limits.py (84%) diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index fc0c787bde..d8e01a7ec2 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -126,17 +126,11 @@ if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) find_package(ros_pytest REQUIRED) - # Python integration test of basic message reception/publication - add_rostest(test/launch/jog_arm_msg_reception_test.test) - - # Python integration test that a halt message is published at a singularity + # Python integration tests add_rostest(test/launch/jog_arm_halt_msg_test.test) - - # Python integration test of a service to enable drift in some dimensions - add_rostest(test/launch/jog_arm_drift_dimensions_service_test.test) - - # Python integration test of velocity and acceleration limits + add_rostest(test/launch/jog_arm_msg_reception_test.test) add_rostest(test/launch/jog_arm_vel_accel_limits_test.test) + add_rostest(test/launch/jog_arm_drift_dimensions_service_test.test) # jog_cpp_interface add_rostest_gtest(jog_cpp_interface_test diff --git a/moveit_experimental/moveit_jog_arm/test/arm_setup/launch/panda_move_group.launch b/moveit_experimental/moveit_jog_arm/test/arm_setup/launch/panda_move_group.launch deleted file mode 100644 index d7bc6b9055..0000000000 --- a/moveit_experimental/moveit_jog_arm/test/arm_setup/launch/panda_move_group.launch +++ /dev/null @@ -1,74 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/initial_position.yaml b/moveit_experimental/moveit_jog_arm/test/config/initial_position.yaml similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/arm_setup/config/initial_position.yaml rename to moveit_experimental/moveit_jog_arm/test/config/initial_position.yaml diff --git a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml b/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/arm_setup/config/jog_settings.yaml rename to moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml diff --git a/moveit_experimental/moveit_jog_arm/test/arm_setup/config/singular_position.yaml b/moveit_experimental/moveit_jog_arm/test/config/singular_position.yaml similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/arm_setup/config/singular_position.yaml rename to moveit_experimental/moveit_jog_arm/test/config/singular_position.yaml diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test index e0fc2aaa85..428d8482e1 100644 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test @@ -7,6 +7,6 @@ - + diff --git a/moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch b/moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch index d734a2746e..f406c10bdb 100644 --- a/moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch +++ b/moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch @@ -8,26 +8,18 @@ - - - - - - + - + diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test index 7bc276f34b..33669dc061 100644 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test @@ -7,9 +7,9 @@ - + - + diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test index 0243368fcd..0ff461bb8c 100644 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test @@ -7,11 +7,9 @@ - + - diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test index b35c6393f1..4761fb2ffb 100644 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test @@ -7,9 +7,9 @@ - + - + diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test index 42fc118672..6a35a19e08 100644 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test +++ b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test @@ -7,9 +7,9 @@ - + - + diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/__init__.py b/moveit_experimental/moveit_jog_arm/test/python_tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_experimental/moveit_jog_arm/test/test_drift_dimensions_service/test_drift_dimensions_service.py b/moveit_experimental/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py similarity index 65% rename from moveit_experimental/moveit_jog_arm/test/test_drift_dimensions_service/test_drift_dimensions_service.py rename to moveit_experimental/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py index b711668710..d28b5c5a6d 100755 --- a/moveit_experimental/moveit_jog_arm/test/test_drift_dimensions_service/test_drift_dimensions_service.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py @@ -10,17 +10,21 @@ from std_msgs.msg import Bool from trajectory_msgs.msg import JointTrajectory +# Import common Python test utilities +from os import sys, path +sys.path.append(path.dirname(path.dirname(path.abspath(__file__)))) +import util + # Send a service call to allow drift in all but the y-dimension. # In other words, only the y-dimension will be controlled exactly. # Check that the service returns and the jog node continues to publish commands to the robot. -JOG_ARM_SETTLE_TIME_S = 10 -ROS_SETTLE_TIME_S = 10 - CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' COMMAND_OUT_TOPIC = 'jog_server/command' +SERVICE_NAME = 'jog_server/change_drift_dimensions' + @pytest.fixture def node(): @@ -28,9 +32,13 @@ def node(): def test_drift_dimensions_service(node): - # Make the service call to allow drift in all dimensions except y-translation - drift_service = rospy.ServiceProxy('jog_server/change_drift_dimensions', ChangeDriftDimensions) - # the transform is an identity matrix, not used for now + assert util.wait_for_jogger_initialization(SERVICE_NAME) + + # Service to change drift dimensions + drift_service = rospy.ServiceProxy(SERVICE_NAME, ChangeDriftDimensions) + + # Service call to allow drift in all dimensions except y-translation + # The transform is an identity matrix, not used for now drift_response = drift_service(True, False, True, True, True, True, Transform()) # Check for successful response assert drift_response.success == True diff --git a/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py b/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py similarity index 85% rename from moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py rename to moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py index 11e75997f7..05f2910700 100755 --- a/moveit_experimental/moveit_jog_arm/test/test_halt_msg/test_jog_arm_halt_msg.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py @@ -8,19 +8,24 @@ from control_msgs.msg import JointJog from std_msgs.msg import Bool +# Import common Python test utilities +from os import sys, path +sys.path.append(path.dirname(path.dirname(path.abspath(__file__)))) +import util + # The robot starts at a singular position (see config file). # Listen for a halt message from the jogger. # This can be run as part of a pytest, or like a normal ROS executable: # rosrun moveit_jog_arm test_jog_arm_integration.py -JOG_ARM_SETTLE_TIME_S = 10 -ROS_SETTLE_TIME_S = 10 - CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' # jog_arm should publish 'true' here if it halts HALT_TOPIC = 'jog_server/halted' +# Check if jogger is initialized with this service +SERVICE_NAME = 'jog_server/change_drift_dimensions' + @pytest.fixture def node(): @@ -42,13 +47,13 @@ def send_cmd(self, linear, angular): def test_jog_arm_halt_msg(node): + assert util.wait_for_jogger_initialization(SERVICE_NAME) + received = [] sub = rospy.Subscriber( HALT_TOPIC, Bool, lambda msg: received.append(msg) ) cartesian_cmd = CartesianJogCmd() - time.sleep(ROS_SETTLE_TIME_S) # wait for pub/subs to settle - time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for jog_arm server to init # This nonzero command should produce jogging output # A subscriber in a different thread fills `received` diff --git a/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py b/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py similarity index 90% rename from moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py rename to moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py index 7ec28b8298..9147559d43 100755 --- a/moveit_experimental/moveit_jog_arm/test/test_msg_reception/test_jog_arm_msg_reception.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py @@ -8,18 +8,23 @@ from control_msgs.msg import JointJog from trajectory_msgs.msg import JointTrajectory +# Import common Python test utilities +from os import sys, path +sys.path.append(path.dirname(path.dirname(path.abspath(__file__)))) +import util + # Test that the jogger publishes controller commands when it receives Cartesian or joint commands. # This can be run as part of a pytest, or like a normal ROS executable: # rosrun moveit_jog_arm test_jog_arm_integration.py -JOG_ARM_SETTLE_TIME_S = 10 -ROS_SETTLE_TIME_S = 10 - JOINT_JOG_COMMAND_TOPIC = 'jog_server/joint_delta_jog_cmds' CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' COMMAND_OUT_TOPIC = 'jog_server/command' +# Check if jogger is initialized with this service +SERVICE_NAME = 'jog_server/change_drift_dimensions' + @pytest.fixture def node(): @@ -53,12 +58,15 @@ def send_cmd(self, linear, angular): def test_jog_arm_cartesian_command(node): + # Test sending a cartesian velocity command + + assert util.wait_for_jogger_initialization(SERVICE_NAME) + received = [] sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) cartesian_cmd = CartesianJogCmd() - time.sleep(ROS_SETTLE_TIME_S) # wait for pub/subs to settle # Repeated zero-commands should produce no output, other than a few halt messages # A subscriber in a different thread fills 'received' @@ -92,13 +100,13 @@ def test_jog_arm_cartesian_command(node): def test_jog_arm_joint_command(node): # Test sending a joint command + assert util.wait_for_jogger_initialization(SERVICE_NAME) + received = [] sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) - joint_cmd = JointJogCmd() - time.sleep(ROS_SETTLE_TIME_S) # wait for pub/subs to settle TEST_DURATION = 1 PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/util.py b/moveit_experimental/moveit_jog_arm/test/python_tests/util.py new file mode 100644 index 0000000000..0ee323b8c7 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/util.py @@ -0,0 +1,10 @@ +import rospy + +def wait_for_jogger_initialization(service_name): + try: + rospy.wait_for_service(service_name, timeout=15) + except rospy.ROSException as exc: + rospy.logerr("The jogger service " + service_name + " is not available: " + str(exc)) + return False + + return True diff --git a/moveit_experimental/moveit_jog_arm/test/test_vel_accel_limits/test_vel_accel_limits.py b/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py similarity index 84% rename from moveit_experimental/moveit_jog_arm/test/test_vel_accel_limits/test_vel_accel_limits.py rename to moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py index b6ca05dd45..2baf6a594a 100755 --- a/moveit_experimental/moveit_jog_arm/test/test_vel_accel_limits/test_vel_accel_limits.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py @@ -7,17 +7,22 @@ from control_msgs.msg import JointJog from trajectory_msgs.msg import JointTrajectory +# Import common Python test utilities +from os import sys, path +sys.path.append(path.dirname(path.dirname(path.abspath(__file__)))) +import util + # Test that commands that are too fast are caught and flagged # This can be run as part of a pytest, or like a normal ROS executable: # rosrun moveit_jog_arm test_vel_accel_limits.py -JOG_ARM_SETTLE_TIME_S = 10 -ROS_SETTLE_TIME_S = 10 - JOINT_JOG_COMMAND_TOPIC = 'jog_server/joint_delta_jog_cmds' COMMAND_OUT_TOPIC = 'jog_server/command' +# Check if jogger is initialized with this service +SERVICE_NAME = 'jog_server/change_drift_dimensions' + @pytest.fixture def node(): @@ -39,13 +44,14 @@ def send_joint_velocity_cmd(self, joint_pos): def test_vel_limit(node): # Test sending a joint command + assert util.wait_for_jogger_initialization(SERVICE_NAME) + received = [] sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) joint_cmd = JointJogCmd() - time.sleep(ROS_SETTLE_TIME_S) # wait for pub/subs to settle TEST_DURATION = 1 PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file @@ -76,5 +82,5 @@ def test_vel_limit(node): if __name__ == '__main__': node = node() - time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for jog_arm server to init test_vel_limit(node) + # TODO(andyz): add an acceleration limit test (the Panda joint_limits.yaml doesn't define acceleration limits) From b171a0194ab1b50ba8ad1fa046afebeab8471e46 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 15 Mar 2020 18:15:05 +0100 Subject: [PATCH 226/612] KDL IK: constrain wiggled joints to limits (#1953) --- .../kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp index 62d7a89337..a0bafdd4af 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp @@ -428,8 +428,8 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, con KDL::Frame f; KDL::Twist delta_twist; KDL::JntArray delta_q(q_out.rows()), q_backup(q_out.rows()); - Eigen::ArrayXd extra_joint_weights; - extra_joint_weights.setOnes(joint_weights.rows()); + Eigen::ArrayXd extra_joint_weights(joint_weights.rows()); + extra_joint_weights.setOnes(); q_out = q_init; ROS_DEBUG_STREAM_NAMED("kdl", "Input: " << q_init); @@ -478,12 +478,14 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, con delta_q_norm); if (delta_q_norm < epsilon_) // stuck in singularity { - if (step_size < 0.005) // cannot reach target + if (step_size < epsilon_) // cannot reach target break; // wiggle joints last_delta_twist_norm = DBL_MAX; delta_q.data.setRandom(); delta_q.data *= std::min(0.1, delta_twist_norm); + clipToJointLimits(q_out, delta_q, extra_joint_weights); + extra_joint_weights.setOnes(); } KDL::Add(q_out, delta_q, q_out); From a26ffa96967b4bd1638eed44882c9a959ee9b7ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Mon, 16 Mar 2020 14:34:25 +0100 Subject: [PATCH 227/612] mgi: add execution methods for moveit_msgs::RobotTrajectory (#1955) This eliminates the common question how to execute a planned Cartesian trajectory and provides a useful API either way. --- .../move_group_interface.h | 10 ++++++++-- .../src/move_group_interface.cpp | 18 ++++++++++++++---- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 0bacff0bc6..0d5a49c90e 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -726,12 +726,18 @@ class MoveGroupInterface target. No execution is performed. The resulting plan is stored in \e plan*/ MoveItErrorCode plan(Plan& plan); - /** \brief Given a \e plan, execute it without waiting for completion. Return true on success. */ + /** \brief Given a \e plan, execute it without waiting for completion. */ MoveItErrorCode asyncExecute(const Plan& plan); - /** \brief Given a \e plan, execute it while waiting for completion. Return true on success. */ + /** \brief Given a \e robot trajectory, execute it without waiting for completion. */ + MoveItErrorCode asyncExecute(const moveit_msgs::RobotTrajectory& trajectory); + + /** \brief Given a \e plan, execute it while waiting for completion. */ MoveItErrorCode execute(const Plan& plan); + /** \brief Given a \e robot trajectory, execute it while waiting for completion. */ + MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory); + /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the waypoints is that specified by setPoseReferenceFrame(). No more than \e jump_threshold diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 9f7d75fac7..8c66ab78bb 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -825,7 +825,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } } - MoveItErrorCode execute(const Plan& plan, bool wait) + MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait) { if (!execute_action_client_->isServerConnected()) { @@ -833,7 +833,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } moveit_msgs::ExecuteTrajectoryGoal goal; - goal.trajectory = plan.trajectory_; + goal.trajectory = trajectory; execute_action_client_->sendGoal(goal); if (!wait) @@ -1438,12 +1438,22 @@ MoveItErrorCode MoveGroupInterface::move() MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan) { - return impl_->execute(plan, false); + return impl_->execute(plan.trajectory_, false); +} + +MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::RobotTrajectory& trajectory) +{ + return impl_->execute(trajectory, false); } MoveItErrorCode MoveGroupInterface::execute(const Plan& plan) { - return impl_->execute(plan, true); + return impl_->execute(plan.trajectory_, true); +} + +MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::RobotTrajectory& trajectory) +{ + return impl_->execute(trajectory, true); } MoveItErrorCode MoveGroupInterface::plan(Plan& plan) From 6d3b899042295579f04d5215d16e00cc4d7d7799 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 16 Mar 2020 18:38:46 -0600 Subject: [PATCH 228/612] [jog_arm] Publish more detailed warnings (#1915) --- .../config/ur_simulated_config.yaml | 2 +- .../moveit_jog_arm/collision_check_thread.h | 2 +- .../include/moveit_jog_arm/jog_arm_data.h | 22 +++- .../include/moveit_jog_arm/jog_calcs.h | 63 +++++++---- .../moveit_jog_arm/jog_cpp_interface.h | 23 +++- .../moveit_jog_arm/jog_interface_base.h | 1 - .../include/moveit_jog_arm/status_codes.h | 61 ++++++++++ .../src/collision_check_thread.cpp | 10 +- .../cpp_interface_example.cpp | 10 +- .../moveit_jog_arm/src/jog_calcs.cpp | 107 +++++++++++------- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 24 ++-- .../moveit_jog_arm/src/jog_interface_base.cpp | 20 +++- .../moveit_jog_arm/src/jog_ros_interface.cpp | 12 +- .../test/config/jog_settings.yaml | 2 +- .../halt_msg/test_jog_arm_halt_msg.py | 17 +-- 15 files changed, 262 insertions(+), 114 deletions(-) create mode 100644 moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h diff --git a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml index 396d8a2741..05a964a0c3 100644 --- a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml +++ b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml @@ -47,7 +47,7 @@ joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving cartesian_command_in_topic: jog_server/delta_jog_cmds # Topic for incoming Cartesian twist commands joint_command_in_topic: jog_server/joint_delta_jog_cmds # Topic for incoming joint angle commands joint_topic: joint_states -warning_topic: jog_server/halted # Publish boolean warnings to this topic +status_topic: jog_server/status # Publish status to this topic command_out_topic: joint_group_position_controller/command # Publish outgoing commands here ## Collision checking for the entire robot body diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index cd6d98f9c2..f5da13f822 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -60,7 +60,7 @@ class CollisionCheckThread // Get thread-safe read-only lock of planning scene planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; - void startMainLoop(moveit_jog_arm::JogArmShared& shared_variables, std::mutex& mutex); + void startMainLoop(moveit_jog_arm::JogArmShared& shared_variables); void stopMainLoop(); diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 24718c6413..31d8e6a374 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -38,18 +38,27 @@ #pragma once -#include +// System +#include +#include + +// Eigen #include + +// ROS +#include #include -#include #include -#include #include +// moveit_jog_arm +#include "status_codes.h" + namespace moveit_jog_arm { // Variables to share between threads, and their mutexes -struct JogArmShared +// Inherit from a mutex so the struct can be locked/unlocked easily +struct JogArmShared : public std::mutex { geometry_msgs::TwistStamped command_deltas; @@ -83,6 +92,9 @@ struct JogArmShared // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw] std::atomic_bool drift_dimensions[6] = { ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false) }; + + // Status of the jogger. 0 for no warning. The meaning of nonzero values can be seen in status_codes.h + std::atomic status; }; // ROS params to be read. See the yaml file in /config for a description of each. @@ -94,7 +106,7 @@ struct JogArmParameters std::string robot_link_command_frame; std::string command_out_topic; std::string planning_frame; - std::string warning_topic; + std::string status_topic; std::string joint_command_in_topic; std::string command_in_type; std::string command_out_type; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 027eeeed6b..f4465b3463 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -38,12 +38,18 @@ #pragma once +// System #include -#include "jog_arm_data.h" -#include "low_pass_filter.h" + +// ROS #include #include -#include +#include + +// moveit_jog_arm +#include "jog_arm_data.h" +#include "low_pass_filter.h" +#include "status_codes.h" namespace moveit_jog_arm { @@ -52,7 +58,7 @@ class JogCalcs public: JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); - void startMainLoop(JogArmShared& shared_variables, std::mutex& mutex); + void startMainLoop(JogArmShared& shared_variables); void stopMainLoop(); @@ -75,34 +81,48 @@ class JogCalcs // Flag that robot state is up to date, end effector transform is known std::atomic is_initialized_; - bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables, std::mutex& mutex); + /** \brief Do jogging calculations for Cartesian twist commands. */ + bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables); + /** \brief Do jogging calculations for direct commands to a joint. */ bool jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables); - // Parse the incoming joint msg for the joints of our MoveGroup - bool updateJoints(std::mutex& mutex, const JogArmShared& shared_variables); + /** \brief Update the stashed status so it can be retrieved asynchronously */ + void updateCachedStatus(JogArmShared& shared_variables); + /** \brief Parse the incoming joint msg for the joints of our MoveGroup */ + bool updateJoints(JogArmShared& shared_variables); + + /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. + * Also, multiply by timestep to calculate a position change. + */ Eigen::VectorXd scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const; + /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. + * Also, multiply by timestep to calculate a position change. + */ Eigen::VectorXd scaleJointCommand(const control_msgs::JointJog& command) const; bool addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const; - // Suddenly halt for a joint limit or other critical issue. - // Is handled differently for position vs. velocity control. + /** \brief Suddenly halt for a joint limit or other critical issue. + * Is handled differently for position vs. velocity control. + */ void suddenHalt(trajectory_msgs::JointTrajectory& joint_traj); void suddenHalt(Eigen::ArrayXd& delta_theta); - void publishWarning(bool active) const; + /** \brief Publish the status of the jogger to a ROS topic */ + void publishStatus() const; - // Scale the delta theta to match joint velocity/acceleration limits + /** \brief Scale the delta theta to match joint velocity/acceleration limits */ void enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta); - // Avoid overshooting joint limits + /** \brief Avoid overshooting joint limits */ bool enforceSRDFPositionLimits(trajectory_msgs::JointTrajectory& new_joint_traj); - // Possibly calculate a velocity scaling factor, due to proximity of - // singularity and direction of motion + /** \brief Possibly calculate a velocity scaling factor, due to proximity of + * singularity and direction of motion + */ double velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, const Eigen::JacobiSVD& svd, const Eigen::MatrixXd& jacobian, const Eigen::MatrixXd& pseudo_inverse); @@ -110,18 +130,19 @@ class JogCalcs /** * Slow motion down if close to singularity or collision. * @param shared_variables data shared between threads, tells how close we are to collision - * @param mutex locks shared data * @param delta_theta motion command, used in calculating new_joint_tray * @param singularity_scale tells how close we are to a singularity * @return false if very close to collision or singularity */ - bool applyVelocityScaling(const JogArmShared& shared_variables, std::mutex& mutex, Eigen::ArrayXd& delta_theta, - double singularity_scale); + bool applyVelocityScaling(JogArmShared& shared_variables, Eigen::ArrayXd& delta_theta, double singularity_scale); + /** \brief Compose the outgoing JointTrajectory message */ trajectory_msgs::JointTrajectory composeJointTrajMessage(sensor_msgs::JointState& joint_state) const; + /** \brief Smooth position commands with a lowpass filter */ void lowPassFilterPositions(sensor_msgs::JointState& joint_state); + /** \brief Convert an incremental position command to joint velocity message */ void calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta); /** \brief Convert joint deltas to an outgoing JointTrajectory command. @@ -129,6 +150,9 @@ class JogCalcs */ bool convertDeltasToOutgoingCmd(); + /** \brief Gazebo simulations have very strict message timestamp requirements. + * Satisfy Gazebo by stuffing multiple messages into one. + */ void insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& trajectory, int count) const; /** @@ -153,10 +177,9 @@ class JogCalcs std::vector position_filters_; - ros::Publisher warning_pub_; + ros::Publisher status_pub_; - // Flag that a warning should be published - bool has_warning_ = false; + StatusCode status_ = NO_WARNING; JogArmParameters parameters_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h index 0541d96620..533c311d08 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h @@ -40,6 +40,7 @@ #include #include "jog_interface_base.h" +#include namespace moveit_jog_arm { @@ -58,15 +59,20 @@ class JogCppInterface : public JogInterfaceBase void stopMainLoop(); - // Provide a Cartesian velocity command to the jogger. - // The units are determined by settings in the yaml file. + /** \brief Provide a Cartesian velocity command to the jogger. + * The units are determined by settings in the yaml file. + */ void provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command); - // Send joint position(s) commands + /** \brief Send joint position(s) commands */ void provideJointCommand(const control_msgs::JointJog& joint_command); - // Returns the most recent JointState that the jogger has received. - // May eliminate the need to create your own joint_state subscriber. + /** + * Returns the most recent JointState that the jogger has received. + * May eliminate the need to create your own joint_state subscriber. + * + * @return the most recent joints known to the jogger + */ sensor_msgs::JointState getJointState(); /** @@ -78,6 +84,13 @@ class JogCppInterface : public JogInterfaceBase */ bool getCommandFrameTransform(Eigen::Isometry3d& transform); + /** + * Get the status of the jogger. + * + * @return 0 for no warning. The meaning of nonzero values can be seen in status_codes.h + */ + StatusCode getJoggerStatus(); + private: ros::NodeHandle nh_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h index 7afe1d7460..741c1babbe 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h @@ -95,7 +95,6 @@ class JogInterfaceBase // Share data between threads JogArmShared shared_variables_; - std::mutex shared_variables_mutex_; // Jog calcs std::unique_ptr jog_calcs_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h new file mode 100644 index 0000000000..37cc4228df --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h @@ -0,0 +1,61 @@ +/******************************************************************************* + * Title : status_codes.h + * Project : moveit_jog_arm + * Created : 2/25/2019 + * Author : Andy Zelenak + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 the copyright holder 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 HOLDER 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 + +namespace moveit_jog_arm +{ +enum StatusCode : int8_t +{ + NO_WARNING = 0, + DECELERATE_FOR_SINGULARITY = 1, + HALT_FOR_SINGULARITY = 2, + COLLISION = 3, + JOINT_BOUND = 4 +}; + +const std::unordered_map + JOG_ARM_STATUS_CODE_MAP({ { NO_WARNING, "No warnings" }, + { DECELERATE_FOR_SINGULARITY, "Close to a singularity, decelerating" }, + { HALT_FOR_SINGULARITY, "Very close to a singularity, halting" }, + { COLLISION, "Close to a collision, halting." }, + { JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }); +} // end namespace trackjoint diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 80059cd559..18cc0ed3b3 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -56,7 +56,7 @@ planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPla return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); } -void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) +void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) { // Reset loop termination flag stop_requested_ = false; @@ -78,9 +78,9 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables, std::mu ///////////////////////////////////////////////// while (ros::ok() && !stop_requested_) { - mutex.lock(); + shared_variables.lock(); sensor_msgs::JointState jts = shared_variables.joints; - mutex.unlock(); + shared_variables.unlock(); for (std::size_t i = 0; i < jts.position.size(); ++i) current_state.setJointPositions(jts.name[i], &jts.position[i]); @@ -108,9 +108,9 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables, std::mu exp(velocity_scale_coefficient * (collision_result.distance - parameters_.collision_proximity_threshold)); } - mutex.lock(); + shared_variables.lock(); shared_variables.collision_velocity_scale = velocity_scale; - mutex.unlock(); + shared_variables.unlock(); collision_rate.sleep(); } diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index ae1b77c1be..987bf449cb 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -36,7 +36,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -#include +#include "moveit_jog_arm/jog_cpp_interface.h" +#include "moveit_jog_arm/status_codes.h" static const std::string LOGNAME = "cpp_interface_example"; @@ -109,7 +110,12 @@ int main(int argc, char** argv) // Retrieve the current joint state from the jogger sensor_msgs::JointState current_joint_state = jog_interface.getJointState(); - ROS_INFO_STREAM(current_joint_state); + ROS_INFO_STREAM_NAMED(LOGNAME, "Current joint state:"); + ROS_INFO_STREAM_NAMED(LOGNAME, current_joint_state); + + // Retrieve the current status of the jogger + moveit_jog_arm::StatusCode status = jog_interface.getJoggerStatus(); + ROS_INFO_STREAM_NAMED(LOGNAME, "Jogger status:\n" << status); jog_interface.stopMainLoop(); jogging_thread.join(); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index ca8de0b921..081727d655 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -47,8 +47,8 @@ namespace moveit_jog_arm JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) : parameters_(parameters), default_sleep_rate_(1000) { - // Publish collision status - warning_pub_ = nh_.advertise(parameters_.warning_topic, 1); + // Publish jogger status + status_pub_ = nh_.advertise(parameters_.status_topic, 1); // MoveIt Setup while (ros::ok() && !model_loader_ptr) @@ -64,7 +64,7 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader: prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getVariableCount()); } -void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) +void JogCalcs::startMainLoop(JogArmShared& shared_variables) { // Reset flags stop_jog_loop_requested_ = false; @@ -94,14 +94,14 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) } // Initialize the position filters to initial robot joints - while (!updateJoints(mutex, shared_variables) && ros::ok()) + while (!updateJoints(shared_variables) && ros::ok()) { if (stop_jog_loop_requested_) return; - mutex.lock(); + shared_variables.lock(); incoming_joint_state_ = shared_variables.joints; - mutex.unlock(); + shared_variables.unlock(); default_sleep_rate_.sleep(); } @@ -123,11 +123,11 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) position_filters_[i].reset(internal_joint_state_.position[i]); // Check for a new command - mutex.lock(); + shared_variables.lock(); cartesian_deltas = shared_variables.command_deltas; joint_deltas = shared_variables.joint_command_deltas; incoming_joint_state_ = shared_variables.joints; - mutex.unlock(); + shared_variables.unlock(); kinematic_state_->setVariableValues(internal_joint_state_); @@ -138,9 +138,9 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); - mutex.lock(); + shared_variables.lock(); shared_variables.tf_moveit_to_cmd_frame = tf_moveit_to_cmd_frame_; - mutex.unlock(); + shared_variables.unlock(); } // Track the number of cycles during which motion has not occurred. @@ -155,7 +155,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) // Always update the joints and end-effector transform for 2 reasons: // 1) in case the getCommandFrameTransform() method is being used // 2) so the low-pass filters are up to date and don't cause a jump - while (!updateJoints(mutex, shared_variables) && ros::ok()) + while (!updateJoints(shared_variables) && ros::ok()) { default_sleep_rate_.sleep(); } @@ -166,9 +166,9 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) kinematic_state_->setVariableValues(incoming_joint_state_); tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); - mutex.lock(); + shared_variables.lock(); shared_variables.tf_moveit_to_cmd_frame = tf_moveit_to_cmd_frame_; - mutex.unlock(); + shared_variables.unlock(); // If paused, just keep the low-pass filters up to date with current joints so a jump doesn't occur when // restarting @@ -181,26 +181,26 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) else { // Flag that incoming commands are all zero. May be used to skip calculations/publication - mutex.lock(); + shared_variables.lock(); bool zero_cartesian_cmd_flag = shared_variables.zero_cartesian_cmd_flag; bool zero_joint_cmd_flag = shared_variables.zero_joint_cmd_flag; - mutex.unlock(); + shared_variables.unlock(); // Prioritize cartesian jogging above joint jogging if (!zero_cartesian_cmd_flag) { - mutex.lock(); + shared_variables.lock(); cartesian_deltas = shared_variables.command_deltas; - mutex.unlock(); + shared_variables.unlock(); - if (!cartesianJogCalcs(cartesian_deltas, shared_variables, mutex)) + if (!cartesianJogCalcs(cartesian_deltas, shared_variables)) continue; } else if (!zero_joint_cmd_flag) { - mutex.lock(); + shared_variables.lock(); joint_deltas = shared_variables.joint_command_deltas; - mutex.unlock(); + shared_variables.unlock(); if (!jointJogCalcs(joint_deltas, shared_variables)) continue; @@ -211,9 +211,9 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) } // Halt if the command is stale or inputs are all zero, or commands were zero - mutex.lock(); + shared_variables.lock(); bool stale_command = shared_variables.command_is_stale; - mutex.unlock(); + shared_variables.unlock(); if (stale_command || (zero_cartesian_cmd_flag && zero_joint_cmd_flag)) { @@ -225,7 +225,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) bool valid_nonzero_command = !zero_cartesian_cmd_flag || !zero_joint_cmd_flag; // Send the newest target joints - mutex.lock(); + shared_variables.lock(); // If everything normal, share the new traj to be published if (valid_nonzero_command) { @@ -245,7 +245,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables, std::mutex& mutex) shared_variables.outgoing_command = outgoing_command_; shared_variables.ok_to_publish = true; } - mutex.unlock(); + shared_variables.unlock(); // Store last zero-velocity message flag to prevent superfluous warnings. // Cartesian and joint commands must both be zero. @@ -279,7 +279,7 @@ bool JogCalcs::isInitialized() } // Perform the jogging calculations -bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables, std::mutex& mutex) +bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables) { // Check for nan's in the incoming command if (std::isnan(cmd.twist.linear.x) || std::isnan(cmd.twist.linear.y) || std::isnan(cmd.twist.linear.z) || @@ -349,19 +349,22 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& return false; // If close to a collision or a singularity, decelerate - if (!applyVelocityScaling(shared_variables, mutex, delta_theta_, + if (!applyVelocityScaling(shared_variables, delta_theta_, velocityScalingFactorForSingularity(delta_x, svd, jacobian, pseudo_inverse))) { - has_warning_ = true; suddenHalt(delta_theta_); } prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; + publishStatus(); + // Cache the status so it can be retrieved asynchronously + updateCachedStatus(shared_variables); + return convertDeltasToOutgoingCmd(); } -bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& /*shared_variables*/) +bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables) { // Check for nan's or |delta|>1 in the incoming command for (double velocity : cmd.velocities) @@ -382,9 +385,19 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& /* prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; + publishStatus(); + // Cache the status so it can be retrieved asynchronously + updateCachedStatus(shared_variables); + return convertDeltasToOutgoingCmd(); } +void JogCalcs::updateCachedStatus(JogArmShared& shared_variables) +{ + shared_variables.status = status_; + status_ = NO_WARNING; +} + bool JogCalcs::convertDeltasToOutgoingCmd() { if (!addJointIncrements(internal_joint_state_, delta_theta_)) @@ -400,12 +413,9 @@ bool JogCalcs::convertDeltasToOutgoingCmd() if (!enforceSRDFPositionLimits(outgoing_command_)) { suddenHalt(outgoing_command_); - has_warning_ = true; + status_ = JOINT_BOUND; } - publishWarning(has_warning_); - has_warning_ = false; - // done with calculations if (parameters_.use_gazebo) { @@ -473,12 +483,18 @@ trajectory_msgs::JointTrajectory JogCalcs::composeJointTrajMessage(sensor_msgs:: // Apply velocity scaling for proximity of collisions and singularities. // Scale for collisions is read from a shared variable. -bool JogCalcs::applyVelocityScaling(const JogArmShared& shared_variables, std::mutex& mutex, - Eigen::ArrayXd& delta_theta, double singularity_scale) +bool JogCalcs::applyVelocityScaling(JogArmShared& shared_variables, Eigen::ArrayXd& delta_theta, + double singularity_scale) { - mutex.lock(); + shared_variables.lock(); double collision_scale = shared_variables.collision_velocity_scale; - mutex.unlock(); + shared_variables.unlock(); + + if (collision_scale < 1) + { + status_ = COLLISION; + ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); + } delta_theta = collision_scale * singularity_scale * delta_theta; @@ -538,13 +554,16 @@ double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& comm velocity_scale = 1. - (ini_condition - parameters_.lower_singularity_threshold) / (parameters_.hard_stop_singularity_threshold - parameters_.lower_singularity_threshold); + status_ = DECELERATE_FOR_SINGULARITY; + ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); } // Very close to singularity, so halt. else if (ini_condition > parameters_.hard_stop_singularity_threshold) { velocity_scale = 0; - ROS_WARN_THROTTLE_NAMED(2, LOGNAME, "Close to a singularity. Halting."); + status_ = HALT_FOR_SINGULARITY; + ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); } } @@ -655,11 +674,11 @@ bool JogCalcs::enforceSRDFPositionLimits(trajectory_msgs::JointTrajectory& new_j return !halting; } -void JogCalcs::publishWarning(bool active) const +void JogCalcs::publishStatus() const { - std_msgs::Bool status; - status.data = static_cast(active); - warning_pub_.publish(status); + std_msgs::Int8 status_msg; + status_msg.data = status_; + status_pub_.publish(status_msg); } // Suddenly halt for a joint limit or other critical issue. @@ -686,11 +705,11 @@ void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_traj) } // Parse the incoming joint msg for the joints of our MoveGroup -bool JogCalcs::updateJoints(std::mutex& mutex, const JogArmShared& shared_variables) +bool JogCalcs::updateJoints(JogArmShared& shared_variables) { - mutex.lock(); + shared_variables.lock(); incoming_joint_state_ = shared_variables.joints; - mutex.unlock(); + shared_variables.unlock(); // Check that the msg contains enough joints if (incoming_joint_state_.name.size() < num_joints_) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp index 1b9475044b..0414d2a9ef 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp @@ -95,7 +95,7 @@ void JogCppInterface::startMainLoop() { ros::spinOnce(); - shared_variables_mutex_.lock(); + shared_variables_.lock(); trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; // Check if incoming commands are stale @@ -139,7 +139,7 @@ void JogCppInterface::startMainLoop() ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); } - shared_variables_mutex_.unlock(); + shared_variables_.unlock(); main_rate.sleep(); } @@ -155,7 +155,7 @@ void JogCppInterface::stopMainLoop() void JogCppInterface::provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command) { - shared_variables_mutex_.lock(); + shared_variables_.lock(); shared_variables_.command_deltas.twist = velocity_command.twist; shared_variables_.command_deltas.header = velocity_command.header; @@ -178,12 +178,12 @@ void JogCppInterface::provideTwistStampedCommand(const geometry_msgs::TwistStamp { shared_variables_.latest_nonzero_cmd_stamp = velocity_command.header.stamp; } - shared_variables_mutex_.unlock(); + shared_variables_.unlock(); }; void JogCppInterface::provideJointCommand(const control_msgs::JointJog& joint_command) { - shared_variables_mutex_.lock(); + shared_variables_.lock(); shared_variables_.joint_command_deltas = joint_command; // Check if joint inputs is all zeros. Flag it if so to skip calculations/publication @@ -198,14 +198,14 @@ void JogCppInterface::provideJointCommand(const control_msgs::JointJog& joint_co { shared_variables_.latest_nonzero_cmd_stamp = joint_command.header.stamp; } - shared_variables_mutex_.unlock(); + shared_variables_.unlock(); } sensor_msgs::JointState JogCppInterface::getJointState() { - shared_variables_mutex_.lock(); + shared_variables_.lock(); sensor_msgs::JointState current_joints = shared_variables_.joints; - shared_variables_mutex_.unlock(); + shared_variables_.unlock(); return current_joints; } @@ -215,10 +215,16 @@ bool JogCppInterface::getCommandFrameTransform(Eigen::Isometry3d& transform) if (!jog_calcs_ || !jog_calcs_->isInitialized()) return false; - const std::lock_guard lock(shared_variables_mutex_); + shared_variables_.lock(); transform = shared_variables_.tf_moveit_to_cmd_frame; + shared_variables_.unlock(); // All zeros means the transform wasn't initialized, so return false return !transform.matrix().isZero(0); } + +StatusCode JogCppInterface::getJoggerStatus() +{ + return shared_variables_.status; +} } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index 6cfc579973..1c550b5998 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -94,7 +94,6 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) error += !rosparam_shortcuts::get("", n, parameter_ns + "/planning_frame", ros_parameters_.planning_frame); error += !rosparam_shortcuts::get("", n, parameter_ns + "/use_gazebo", ros_parameters_.use_gazebo); error += !rosparam_shortcuts::get("", n, parameter_ns + "/check_collisions", ros_parameters_.check_collisions); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/warning_topic", ros_parameters_.warning_topic); error += !rosparam_shortcuts::get("", n, parameter_ns + "/joint_limit_margin", ros_parameters_.joint_limit_margin); error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_out_topic", ros_parameters_.command_out_topic); error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_out_type", ros_parameters_.command_out_type); @@ -105,6 +104,16 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_accelerations", ros_parameters_.publish_joint_accelerations); + // This parameter name was changed recently. + // Try retrieving from the correct name. If it fails, then try the deprecated name. + // TODO(andyz): remove this deprecation warning in ROS Noetic + if (!rosparam_shortcuts::get("", n, parameter_ns + "/status_topic", ros_parameters_.status_topic)) + { + ROS_WARN_NAMED(LOGNAME, "'status_topic' parameter is missing. Recently renamed from 'warning_topic'. Please update " + "the jogging yaml file."); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/warning_topic", ros_parameters_.status_topic); + } + rosparam_shortcuts::shutdownIfError(parameter_ns, error); // Input checking @@ -195,8 +204,9 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) // Listen to joint angles. Store them in a shared variable. void JogInterfaceBase::jointsCB(const sensor_msgs::JointStateConstPtr& msg) { - const std::lock_guard lock(shared_variables_mutex_); + shared_variables_.lock(); shared_variables_.joints = *msg; + shared_variables_.unlock(); } bool JogInterfaceBase::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, @@ -220,8 +230,7 @@ bool JogInterfaceBase::startJogCalcThread() if (!jog_calcs_) jog_calcs_.reset(new JogCalcs(ros_parameters_, planning_scene_monitor_->getRobotModelLoader())); - jog_calc_thread_.reset( - new std::thread([&]() { jog_calcs_->startMainLoop(shared_variables_, shared_variables_mutex_); })); + jog_calc_thread_.reset(new std::thread([&]() { jog_calcs_->startMainLoop(shared_variables_); })); return true; } @@ -248,8 +257,7 @@ bool JogInterfaceBase::startCollisionCheckThread() if (!collision_checker_) collision_checker_.reset(new CollisionCheckThread(ros_parameters_, planning_scene_monitor_)); - collision_check_thread_.reset( - new std::thread([&]() { collision_checker_->startMainLoop(shared_variables_, shared_variables_mutex_); })); + collision_check_thread_.reset(new std::thread([&]() { collision_checker_->startMainLoop(shared_variables_); })); return true; } diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp index 72144692fe..00d7fa2760 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp @@ -117,7 +117,7 @@ JogROSInterface::JogROSInterface() { ros::spinOnce(); - shared_variables_mutex_.lock(); + shared_variables_.lock(); trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; // Check for stale cmds @@ -162,7 +162,7 @@ JogROSInterface::JogROSInterface() ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); } - shared_variables_mutex_.unlock(); + shared_variables_.unlock(); main_rate.sleep(); } @@ -174,7 +174,7 @@ JogROSInterface::JogROSInterface() // Listen to cartesian delta commands. Store them in a shared variable. void JogROSInterface::deltaCartesianCmdCB(const geometry_msgs::TwistStampedConstPtr& msg) { - shared_variables_mutex_.lock(); + shared_variables_.lock(); // Copy everything but the frame name. The frame name is set by yaml file at startup. // (so it isn't copied over and over) @@ -199,13 +199,13 @@ void JogROSInterface::deltaCartesianCmdCB(const geometry_msgs::TwistStampedConst { shared_variables_.latest_nonzero_cmd_stamp = msg->header.stamp; } - shared_variables_mutex_.unlock(); + shared_variables_.unlock(); } // Listen to joint delta commands. Store them in a shared variable. void JogROSInterface::deltaJointCmdCB(const control_msgs::JointJogConstPtr& msg) { - shared_variables_mutex_.lock(); + shared_variables_.lock(); shared_variables_.joint_command_deltas = *msg; // Check if joint inputs is all zeros. Flag it if so to skip calculations/publication @@ -220,6 +220,6 @@ void JogROSInterface::deltaJointCmdCB(const control_msgs::JointJogConstPtr& msg) { shared_variables_.latest_nonzero_cmd_stamp = msg->header.stamp; } - shared_variables_mutex_.unlock(); + shared_variables_.unlock(); } } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml b/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml index c6ecb8998b..76b8cd892c 100644 --- a/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml +++ b/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml @@ -47,7 +47,7 @@ joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving cartesian_command_in_topic: jog_server/delta_jog_cmds # Topic for incoming Cartesian twist commands joint_command_in_topic: jog_server/joint_delta_jog_cmds # Topic for incoming joint angle commands joint_topic: joint_states -warning_topic: jog_server/halted # Publish boolean warnings to this topic +status_topic: jog_server/status # Publish status to this topic command_out_topic: jog_server/command # Publish outgoing commands here ## Collision checking for the entire robot body diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py b/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py index 05f2910700..bb083877b5 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py @@ -6,7 +6,7 @@ from geometry_msgs.msg import TwistStamped from control_msgs.msg import JointJog -from std_msgs.msg import Bool +from std_msgs.msg import Int8 # Import common Python test utilities from os import sys, path @@ -14,14 +14,15 @@ import util # The robot starts at a singular position (see config file). -# Listen for a halt message from the jogger. +# The jogger should halt and publish a warning. +# Listen for a warning message from the jogger. # This can be run as part of a pytest, or like a normal ROS executable: -# rosrun moveit_jog_arm test_jog_arm_integration.py +# rosrun moveit_jog_arm test_jog_arm_halt_msg.py CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' -# jog_arm should publish 'true' here if it halts -HALT_TOPIC = 'jog_server/halted' +# jog_arm should publish a nonzero warning code here +HALT_TOPIC = 'jog_server/status' # Check if jogger is initialized with this service SERVICE_NAME = 'jog_server/change_drift_dimensions' @@ -51,7 +52,7 @@ def test_jog_arm_halt_msg(node): received = [] sub = rospy.Subscriber( - HALT_TOPIC, Bool, lambda msg: received.append(msg) + HALT_TOPIC, Int8, lambda msg: received.append(msg) ) cartesian_cmd = CartesianJogCmd() @@ -66,8 +67,8 @@ def test_jog_arm_halt_msg(node): # Check the received messages # A non-zero value signifies a warning - assert len(received) > 1 - assert received[-1].data != 0 + assert len(received) > 3 + assert (received[-1].data != 0) or (received[-2].data != 0) or (received[-3].data != 0) if __name__ == '__main__': From eb839d370e9b2bd6a46c98c75acc71041824c0f9 Mon Sep 17 00:00:00 2001 From: Ryodo Tanaka Date: Fri, 20 Mar 2020 18:12:53 +0900 Subject: [PATCH 229/612] Fix service call to utilize original name space (#1959) --- .../src/moveit_commander/planning_scene_interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_commander/src/moveit_commander/planning_scene_interface.py b/moveit_commander/src/moveit_commander/planning_scene_interface.py index 43c6769cd2..cbc3c72021 100644 --- a/moveit_commander/src/moveit_commander/planning_scene_interface.py +++ b/moveit_commander/src/moveit_commander/planning_scene_interface.py @@ -64,7 +64,7 @@ def __init__(self, ns='', synchronous=False, service_timeout=5.0): self._pub_aco = rospy.Publisher(ns + '/attached_collision_object', AttachedCollisionObject, queue_size=100) self.__synchronous = synchronous if self.__synchronous: - self._apply_planning_scene_diff = rospy.ServiceProxy('apply_planning_scene', ApplyPlanningScene) + self._apply_planning_scene_diff = rospy.ServiceProxy(ns + '/apply_planning_scene', ApplyPlanningScene) self._apply_planning_scene_diff.wait_for_service(service_timeout) def __submit(self, collision_object, attach=False): From 918a4e9df5fa72ba593fb550bc6a0eb1b8620358 Mon Sep 17 00:00:00 2001 From: Jonas Wittmann Date: Sat, 21 Mar 2020 14:25:53 +0100 Subject: [PATCH 230/612] Fix Condition for adding current DistanceResultData to DistanceMap --- moveit_core/collision_detection_fcl/src/collision_common.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index a569479207..b90d7f0c53 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -610,7 +610,7 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void } else if (cdata->req->type == DistanceRequestType::SINGLE) { - if (it->second[0].distance < dist_result.distance) + if (dist_result.distance < it->second[0].distance) it->second[0] = dist_result; } else if (cdata->req->type == DistanceRequestType::LIMITED) From d99295730b748b1f03fef3d9d91094d6c4e5959e Mon Sep 17 00:00:00 2001 From: v4hn Date: Sun, 22 Mar 2020 07:03:51 +0100 Subject: [PATCH 231/612] collision detection: add test for SINGLE distance request This logic is/was broken in collision_detection_fcl. bullet does not even implement it, so factorize DistanceRequest distance tests in their own suite --- .../test_collision_common_panda.h | 48 +++++++++++++++++++ .../test_fcl_collision_detection_panda.cpp | 3 ++ 2 files changed, 51 insertions(+) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 86535974cb..4a42c65924 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -258,5 +258,53 @@ TYPED_TEST_P(CollisionDetectorPandaTest, DistanceWorld) EXPECT_NEAR(res.distance, 0.029, 0.01); } +template +class DistanceCheckPandaTest : public CollisionDetectorPandaTest +{ +}; + +TYPED_TEST_CASE_P(DistanceCheckPandaTest); + +TYPED_TEST_P(DistanceCheckPandaTest, DistanceSingle) +{ + std::set active_components{ this->robot_model_->getLinkModel("panda_hand") }; + collision_detection::DistanceRequest req; + req.type = collision_detection::DistanceRequestTypes::SINGLE; + req.active_components_only = &active_components; + req.enable_signed_distance = true; + + random_numbers::RandomNumberGenerator rng(0x47110815); + double min_distance = std::numeric_limits::max(); + for (int i = 0; i < 10; ++i) + { + collision_detection::DistanceResult res; + + shapes::ShapeConstPtr shape(new shapes::Cylinder(rng.uniform01(), rng.uniform01())); + Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; + pose.translation() = + Eigen::Vector3d(rng.uniformReal(0.1, 2.0), rng.uniformReal(0.1, 2.0), rng.uniformReal(1.2, 1.7)); + double quat[4]; + rng.quaternion(quat); + pose.linear() = Eigen::Quaterniond(quat[0], quat[1], quat[2], quat[3]).toRotationMatrix(); + + this->cenv_->getWorld()->addToObject("collection", shape, pose); + this->cenv_->getWorld()->removeObject("object"); + this->cenv_->getWorld()->addToObject("object", shape, pose); + + this->cenv_->distanceRobot(req, res, *this->robot_state_); + auto& distances1 = res.distances[std::pair("collection", "panda_hand")]; + auto& distances2 = res.distances[std::pair("object", "panda_hand")]; + ASSERT_EQ(distances1.size(), 1u) << "no distance reported for collection/panda_hand"; + ASSERT_EQ(distances2.size(), 1u) << "no distance reported for object/panda_hand"; + + double collection_distance = distances1[0].distance; + min_distance = std::min(min_distance, distances2[0].distance); + ASSERT_NEAR(collection_distance, min_distance, 1e-5) + << "distance to collection is greater than distance to minimum of individual objects after " << i << " rounds"; + } +} + REGISTER_TYPED_TEST_CASE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision, RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, DistanceSelf, DistanceWorld); + +REGISTER_TYPED_TEST_CASE_P(DistanceCheckPandaTest, DistanceSingle); diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp index b83b267171..1561841df9 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp @@ -40,6 +40,9 @@ INSTANTIATE_TYPED_TEST_CASE_P(FCLCollisionCheckPanda, CollisionDetectorPandaTest, collision_detection::CollisionDetectorAllocatorFCL); +INSTANTIATE_TYPED_TEST_CASE_P(FCLDistanceCheckPanda, DistanceCheckPandaTest, + collision_detection::CollisionDetectorAllocatorFCL); + int main(int argc, char* argv[]) { testing::InitGoogleTest(&argc, argv); From 0407e868ba5311ac3d9351105f0ded11660aa493 Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Tue, 24 Mar 2020 17:16:31 +0100 Subject: [PATCH 232/612] export moveit_py_bindings_tools library (#1970) Co-authored-by: Bjar Ne --- moveit_ros/planning_interface/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 57942ccdf8..522839f505 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -63,6 +63,7 @@ catkin_package( moveit_planning_scene_interface moveit_move_group_interface moveit_cpp + moveit_py_bindings_tools INCLUDE_DIRS ${THIS_PACKAGE_INCLUDE_DIRS} CATKIN_DEPENDS From 8bfbc214e1fe6f51563cf1b2f8a67804950876e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 25 Mar 2020 18:03:48 +0100 Subject: [PATCH 233/612] MP display: change internal shortcut Ctrl+R to Ctrl+I (#1967) Ctrl+R is used by RViz to rename displays. Overlaying the sequence semi-breaks the behavior and the rename button gets selected, but not pressed. This is an internal shortcut used in case the interactive markers of the display fail to update correctly. This is seldomly (never?) used, so remapping does not hurt anyone. To the best of my knowledge Ctrl+I is not used in standard RViz. --- .../motion_planning_rviz_plugin/src/motion_planning_display.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 84eae7c372..4e8ab72d7c 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -258,7 +258,7 @@ void MotionPlanningDisplay::onInitialize() if (context_ && context_->getWindowManager() && context_->getWindowManager()->getParentWindow()) { QShortcut* im_reset_shortcut = - new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_R), context_->getWindowManager()->getParentWindow()); + new QShortcut(QKeySequence("Ctrl+I"), context_->getWindowManager()->getParentWindow()); connect(im_reset_shortcut, SIGNAL(activated()), this, SLOT(resetInteractiveMarkers())); } } From 8946d266b5e72488688931f7db7268ce31226cfd Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 26 Mar 2020 16:41:32 -0500 Subject: [PATCH 234/612] [jog_arm] Add a binary collision check (#1978) --- .../config/ur_simulated_config.yaml | 3 +- .../include/moveit_jog_arm/jog_calcs.h | 3 +- .../include/moveit_jog_arm/status_codes.h | 10 +++--- .../src/collision_check_thread.cpp | 32 ++++++++++++++----- .../moveit_jog_arm/src/jog_calcs.cpp | 20 +++++++----- .../test/config/jog_settings.yaml | 1 - 6 files changed, 44 insertions(+), 25 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml index 05a964a0c3..f978ea5817 100644 --- a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml +++ b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml @@ -52,6 +52,5 @@ command_out_topic: joint_group_position_controller/command # Publish outgoing co ## Collision checking for the entire robot body check_collisions: true # Check collisions? -collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. +collision_check_rate: 10 # [Hz] Collision-checking can easily bog down a CPU if done too often. collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m] -hard_stop_collision_proximity_threshold: 0.0005 # Stop when a collision is this far [m] diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index f4465b3463..916a8aa9ff 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -132,9 +132,8 @@ class JogCalcs * @param shared_variables data shared between threads, tells how close we are to collision * @param delta_theta motion command, used in calculating new_joint_tray * @param singularity_scale tells how close we are to a singularity - * @return false if very close to collision or singularity */ - bool applyVelocityScaling(JogArmShared& shared_variables, Eigen::ArrayXd& delta_theta, double singularity_scale); + void applyVelocityScaling(JogArmShared& shared_variables, Eigen::ArrayXd& delta_theta, double singularity_scale); /** \brief Compose the outgoing JointTrajectory message */ trajectory_msgs::JointTrajectory composeJointTrajMessage(sensor_msgs::JointState& joint_state) const; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h index 37cc4228df..1c6d264696 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h @@ -48,14 +48,16 @@ enum StatusCode : int8_t NO_WARNING = 0, DECELERATE_FOR_SINGULARITY = 1, HALT_FOR_SINGULARITY = 2, - COLLISION = 3, - JOINT_BOUND = 4 + DECELERATE_FOR_COLLISION = 3, + HALT_FOR_COLLISION = 4, + JOINT_BOUND = 5 }; const std::unordered_map JOG_ARM_STATUS_CODE_MAP({ { NO_WARNING, "No warnings" }, { DECELERATE_FOR_SINGULARITY, "Close to a singularity, decelerating" }, - { HALT_FOR_SINGULARITY, "Very close to a singularity, halting" }, - { COLLISION, "Close to a collision, halting." }, + { HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, + { DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, + { HALT_FOR_COLLISION, "Collision detected, emergency stop" }, { JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }); } // end namespace trackjoint diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 18cc0ed3b3..67783e0b79 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -40,6 +40,7 @@ #include static const std::string LOGNAME = "collision_check_thread"; +static const double MIN_RECOMMENDED_COLLISION_RATE = 10; namespace moveit_jog_arm { @@ -49,6 +50,8 @@ CollisionCheckThread::CollisionCheckThread( const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : parameters_(parameters), planning_scene_monitor_(planning_scene_monitor) { + if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE) + ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Collision check rate is low, increase it in yaml file if CPU allows"); } planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPlanningSceneRO() const @@ -64,7 +67,6 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) // Init collision request collision_detection::CollisionRequest collision_request; collision_request.group_name = parameters_.move_group_name; - collision_request.distance = true; collision_detection::CollisionResult collision_result; // Copy the planning scene's version of current state into new memory @@ -73,6 +75,13 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) double velocity_scale_coefficient = -log(0.001) / parameters_.collision_proximity_threshold; ros::Rate collision_rate(parameters_.collision_check_rate); + double collision_distance = 0; + + // Scale robot velocity according to collision proximity and user-defined thresholds. + // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. + // Proximity decreasing --> decelerate + double velocity_scale = 1; + ///////////////////////////////////////////////// // Spin while checking collisions ///////////////////////////////////////////////// @@ -87,25 +96,32 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) collision_result.clear(); current_state.updateCollisionBodyTransforms(); - // Do thread-safe collsion collision checking + // Do a thread-safe distance-based collision detection + collision_request.distance = true; + getLockedPlanningSceneRO()->checkCollision(collision_request, collision_result, current_state); + collision_distance = collision_result.distance; + + // Do a binary collision detection (helps with strange edge cases like being in collision initially) + collision_request.distance = false; getLockedPlanningSceneRO()->checkCollision(collision_request, collision_result, current_state); - // Scale robot velocity according to collision proximity and user-defined thresholds. - // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. - // Proximity decreasing --> decelerate - double velocity_scale = 1; + // If we're definitely in collision, stop immediately + if (collision_result.collision) + { + velocity_scale = 0; + } // If we are far from a collision, velocity_scale should be 1. // If we are very close to a collision, velocity_scale should be ~zero. // When collision_proximity_threshold is breached, start decelerating exponentially. - if (collision_result.distance < parameters_.collision_proximity_threshold) + else if (collision_distance < parameters_.collision_proximity_threshold) { // velocity_scale = e ^ k * (collision_distance - threshold) // k = - ln(0.001) / collision_proximity_threshold // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. // velocity_scale should equal 0.001 when collision_distance is at zero. velocity_scale = - exp(velocity_scale_coefficient * (collision_result.distance - parameters_.collision_proximity_threshold)); + exp(velocity_scale_coefficient * (collision_distance - parameters_.collision_proximity_threshold)); } shared_variables.lock(); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 081727d655..ba562e8458 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -349,9 +349,11 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& return false; // If close to a collision or a singularity, decelerate - if (!applyVelocityScaling(shared_variables, delta_theta_, - velocityScalingFactorForSingularity(delta_x, svd, jacobian, pseudo_inverse))) + applyVelocityScaling(shared_variables, delta_theta_, + velocityScalingFactorForSingularity(delta_x, svd, jacobian, pseudo_inverse)); + if (status_ == HALT_FOR_COLLISION) { + ROS_ERROR_STREAM_THROTTLE_NAMED(5, LOGNAME, "Halting for collision!"); suddenHalt(delta_theta_); } @@ -400,6 +402,7 @@ void JogCalcs::updateCachedStatus(JogArmShared& shared_variables) bool JogCalcs::convertDeltasToOutgoingCmd() { + internal_joint_state_ = original_joint_state_; if (!addJointIncrements(internal_joint_state_, delta_theta_)) return false; @@ -483,23 +486,24 @@ trajectory_msgs::JointTrajectory JogCalcs::composeJointTrajMessage(sensor_msgs:: // Apply velocity scaling for proximity of collisions and singularities. // Scale for collisions is read from a shared variable. -bool JogCalcs::applyVelocityScaling(JogArmShared& shared_variables, Eigen::ArrayXd& delta_theta, +void JogCalcs::applyVelocityScaling(JogArmShared& shared_variables, Eigen::ArrayXd& delta_theta, double singularity_scale) { shared_variables.lock(); double collision_scale = shared_variables.collision_velocity_scale; shared_variables.unlock(); - if (collision_scale < 1) + if (collision_scale > 0 && collision_scale < 1) { - status_ = COLLISION; + status_ = DECELERATE_FOR_COLLISION; ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); } + else if (collision_scale == 0) + { + status_ = HALT_FOR_COLLISION; + } delta_theta = collision_scale * singularity_scale * delta_theta; - - // Heuristic: flag that we are stuck if velocity scaling is < X% - return collision_scale * singularity_scale >= 0.1; } // Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion diff --git a/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml b/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml index 76b8cd892c..036126f453 100644 --- a/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml +++ b/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml @@ -54,4 +54,3 @@ command_out_topic: jog_server/command # Publish outgoing commands here check_collisions: true # Check collisions? collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] -hard_stop_collision_proximity_threshold: 0.0005 # Stop when a collision is this far [m] From a672455de72a2829797dd192a9f33acaedd1a594 Mon Sep 17 00:00:00 2001 From: Jonathan Binney Date: Fri, 27 Mar 2020 00:59:19 -0700 Subject: [PATCH 235/612] Remove @jonbinney as a codeowner (#1981) --- .github/CODEOWNERS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index b8bf3f5483..f67b645235 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -55,7 +55,7 @@ /moveit_core/planning_request_adapter/ @rhaschke /moveit_core/planning_scene/ @rhaschke /moveit_core/profiler/ @tylerjw -/moveit_core/robot_model/ @jonbinney +/moveit_core/robot_model/ @tylerjw /moveit_core/robot_state/ @rhaschke @mlautman /moveit_core/robot_trajectory/ @mlautman /moveit_core/sensor_manager/ @mlautman @@ -79,7 +79,7 @@ /moveit_ros/robot_interaction/ @mikeferguson @rhaschke /moveit_ros/warehouse/ @mikeferguson @dg-shadow /moveit_ros/move_group/ @rhaschke @IanTheEngineer -/moveit_ros/visualization/ @rhaschke @jonbinney @RoboticsYY +/moveit_ros/visualization/ @rhaschke @tylerjw @RoboticsYY /moveit_ros/planning/collision_plugin_loader/ @j-petit /moveit_ros/planning/constraint_sampler_manager_loader/ @nbbrooks From 6601d299d5c728c65fd6909c0459f70cc2c61f11 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 27 Mar 2020 12:39:41 +0100 Subject: [PATCH 236/612] remove unused angles.h includes (#1985) --- .../constraint_samplers/test/pr2_arm_kinematics_plugin.h | 2 -- .../src/time_optimal_trajectory_generation.cpp | 1 - .../core/sbpl_interface/include/sbpl_interface/sbpl_params.h | 1 - 3 files changed, 4 deletions(-) diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index a3cca94e0c..da5be7edc3 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -42,8 +42,6 @@ #include #include -#include - #include #include #include diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index d92e2a11db..4bba94268c 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -39,7 +39,6 @@ #include #include #include -#include #include #include #include diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_params.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_params.h index 56c5a53f51..4c9ac11009 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_params.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_params.h @@ -39,7 +39,6 @@ #include #include #include -#include #include #include From 490622f024bc11170e53cd88cdd13e0cb2e7f1f0 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 27 Mar 2020 14:22:58 +0100 Subject: [PATCH 237/612] Fix errors: catkin_lint 1.6.7 (#1987) --- moveit_kinematics/CMakeLists.txt | 3 ++- moveit_planners/ompl/CMakeLists.txt | 2 ++ .../moveit_fake_controller_manager/CMakeLists.txt | 1 + .../moveit_ros_control_interface/CMakeLists.txt | 3 ++- .../moveit_simple_controller_manager/CMakeLists.txt | 12 +++++++++--- moveit_ros/manipulation/CMakeLists.txt | 3 +++ moveit_ros/move_group/CMakeLists.txt | 3 +++ moveit_ros/perception/CMakeLists.txt | 1 + moveit_ros/planning/CMakeLists.txt | 5 ++++- moveit_ros/planning_interface/CMakeLists.txt | 5 +++-- moveit_ros/robot_interaction/CMakeLists.txt | 3 ++- moveit_ros/visualization/CMakeLists.txt | 1 + moveit_ros/warehouse/CMakeLists.txt | 4 +++- moveit_setup_assistant/CMakeLists.txt | 1 + 14 files changed, 37 insertions(+), 10 deletions(-) diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index b422c10827..67f843f1ce 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/moveit_kinematics/CMakeLists.txt @@ -32,8 +32,9 @@ catkin_package( INCLUDE_DIRS ${THIS_PACKAGE_INCLUDE_DIRS} CATKIN_DEPENDS - pluginlib moveit_core + pluginlib + roscpp DEPENDS EIGEN3 ) diff --git a/moveit_planners/ompl/CMakeLists.txt b/moveit_planners/ompl/CMakeLists.txt index 9edbe70b8d..20577b7fac 100644 --- a/moveit_planners/ompl/CMakeLists.txt +++ b/moveit_planners/ompl/CMakeLists.txt @@ -31,7 +31,9 @@ catkin_package( INCLUDE_DIRS ompl_interface/include CATKIN_DEPENDS + dynamic_reconfigure moveit_core + roscpp DEPENDS OMPL ) diff --git a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt index 7102c1ec77..95636d536f 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt @@ -25,6 +25,7 @@ catkin_package( CATKIN_DEPENDS moveit_core moveit_ros_planning + roscpp ) add_library(${PROJECT_NAME} diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt index 5b310439ed..90a00ea567 100644 --- a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt +++ b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt @@ -22,8 +22,9 @@ catkin_package( ${PROJECT_NAME}_plugin ${PROJECT_NAME}_trajectory_plugin CATKIN_DEPENDS - moveit_core + actionlib controller_manager_msgs + moveit_core trajectory_msgs DEPENDS Boost ) diff --git a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt index db6dec30d5..caaa598307 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt @@ -22,9 +22,15 @@ find_package(catkin COMPONENTS include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) catkin_package( - LIBRARIES ${PROJECT_NAME} - INCLUDE_DIRS include - CATKIN_DEPENDS moveit_core actionlib control_msgs + LIBRARIES + ${PROJECT_NAME} + INCLUDE_DIRS + include + CATKIN_DEPENDS + actionlib + control_msgs + moveit_core + roscpp ) add_library(${PROJECT_NAME} diff --git a/moveit_ros/manipulation/CMakeLists.txt b/moveit_ros/manipulation/CMakeLists.txt index efab60effe..1288e6caba 100644 --- a/moveit_ros/manipulation/CMakeLists.txt +++ b/moveit_ros/manipulation/CMakeLists.txt @@ -33,9 +33,12 @@ catkin_package( LIBRARIES moveit_pick_place_planner CATKIN_DEPENDS + actionlib + dynamic_reconfigure moveit_core moveit_msgs moveit_ros_planning + roscpp DEPENDS EIGEN3 ) diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index 5775f3b07b..d74c89ea65 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -28,8 +28,11 @@ catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS + actionlib moveit_core moveit_ros_planning + roscpp + std_srvs tf2_geometry_msgs ) diff --git a/moveit_ros/perception/CMakeLists.txt b/moveit_ros/perception/CMakeLists.txt index fe26aadc4b..0456a0299f 100644 --- a/moveit_ros/perception/CMakeLists.txt +++ b/moveit_ros/perception/CMakeLists.txt @@ -67,6 +67,7 @@ catkin_package( moveit_msgs moveit_ros_occupancy_map_monitor object_recognition_msgs + roscpp sensor_msgs tf2_geometry_msgs DEPENDS diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index c61c9cb6f6..6383fe717c 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -63,10 +63,13 @@ catkin_package( INCLUDE_DIRS ${THIS_PACKAGE_INCLUDE_DIRS} CATKIN_DEPENDS - pluginlib + actionlib + dynamic_reconfigure moveit_core moveit_ros_occupancy_map_monitor moveit_msgs + pluginlib + roscpp tf2_msgs tf2_geometry_msgs DEPENDS diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 522839f505..8fed49b108 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -68,12 +68,13 @@ catkin_package( ${THIS_PACKAGE_INCLUDE_DIRS} CATKIN_DEPENDS actionlib + geometry_msgs + moveit_msgs moveit_ros_planning moveit_ros_warehouse moveit_ros_manipulation moveit_ros_move_group - geometry_msgs - moveit_msgs + roscpp tf2_geometry_msgs DEPENDS EIGEN3 diff --git a/moveit_ros/robot_interaction/CMakeLists.txt b/moveit_ros/robot_interaction/CMakeLists.txt index 1c05eee701..0380aef8db 100644 --- a/moveit_ros/robot_interaction/CMakeLists.txt +++ b/moveit_ros/robot_interaction/CMakeLists.txt @@ -28,8 +28,9 @@ catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS - moveit_ros_planning interactive_markers + moveit_ros_planning + roscpp tf2_geometry_msgs ) diff --git a/moveit_ros/visualization/CMakeLists.txt b/moveit_ros/visualization/CMakeLists.txt index 572ef94865..16b505344c 100644 --- a/moveit_ros/visualization/CMakeLists.txt +++ b/moveit_ros/visualization/CMakeLists.txt @@ -80,6 +80,7 @@ catkin_package( moveit_ros_planning_interface moveit_ros_robot_interaction object_recognition_msgs + roscpp DEPENDS EIGEN3 ) diff --git a/moveit_ros/warehouse/CMakeLists.txt b/moveit_ros/warehouse/CMakeLists.txt index 7441199a93..ec829566b3 100644 --- a/moveit_ros/warehouse/CMakeLists.txt +++ b/moveit_ros/warehouse/CMakeLists.txt @@ -27,7 +27,9 @@ catkin_package( warehouse/include CATKIN_DEPENDS moveit_ros_planning - warehouse_ros) + warehouse_ros + roscpp +) include_directories(warehouse/include) include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) diff --git a/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/CMakeLists.txt index 5fe318ad31..6f9b85ef44 100644 --- a/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/CMakeLists.txt @@ -56,6 +56,7 @@ catkin_package( CATKIN_DEPENDS moveit_ros_planning moveit_ros_visualization + roscpp ) # Header files that need Qt Moc pre-processing for use with Qt signals, etc: From 13bf03e93800d35d31f6f9951857e2cdc98c8b69 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 27 Mar 2020 17:04:06 +0100 Subject: [PATCH 238/612] Remove duplicate collision check in JogArm (#1986) --- .../src/collision_check_thread.cpp | 20 +++++++------------ 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 67783e0b79..ae4709ee70 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -67,6 +67,7 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) // Init collision request collision_detection::CollisionRequest collision_request; collision_request.group_name = parameters_.move_group_name; + collision_request.distance = true; // enable distance-based collision checking collision_detection::CollisionResult collision_result; // Copy the planning scene's version of current state into new memory @@ -75,8 +76,6 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) double velocity_scale_coefficient = -log(0.001) / parameters_.collision_proximity_threshold; ros::Rate collision_rate(parameters_.collision_check_rate); - double collision_distance = 0; - // Scale robot velocity according to collision proximity and user-defined thresholds. // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. // Proximity decreasing --> decelerate @@ -96,13 +95,8 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) collision_result.clear(); current_state.updateCollisionBodyTransforms(); - // Do a thread-safe distance-based collision detection - collision_request.distance = true; - getLockedPlanningSceneRO()->checkCollision(collision_request, collision_result, current_state); - collision_distance = collision_result.distance; - // Do a binary collision detection (helps with strange edge cases like being in collision initially) - collision_request.distance = false; + // Do a thread-safe distance-based collision detection getLockedPlanningSceneRO()->checkCollision(collision_request, collision_result, current_state); // If we're definitely in collision, stop immediately @@ -114,14 +108,14 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) // If we are far from a collision, velocity_scale should be 1. // If we are very close to a collision, velocity_scale should be ~zero. // When collision_proximity_threshold is breached, start decelerating exponentially. - else if (collision_distance < parameters_.collision_proximity_threshold) + else if (collision_result.distance < parameters_.collision_proximity_threshold) { - // velocity_scale = e ^ k * (collision_distance - threshold) + // velocity_scale = e ^ k * (collision_result.distance - threshold) // k = - ln(0.001) / collision_proximity_threshold - // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. - // velocity_scale should equal 0.001 when collision_distance is at zero. + // velocity_scale should equal one when collision_result.distance is at collision_proximity_threshold. + // velocity_scale should equal 0.001 when collision_result.distance is at zero. velocity_scale = - exp(velocity_scale_coefficient * (collision_distance - parameters_.collision_proximity_threshold)); + exp(velocity_scale_coefficient * (collision_result.distance - parameters_.collision_proximity_threshold)); } shared_variables.lock(); From 768908596f58d9435191849a0b485456456ebd7a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 29 Mar 2020 11:32:50 +0200 Subject: [PATCH 239/612] Note changes in rviz planning panel requiring saving (#1991) Forward configChanged() signal from planning panel to PropertyTreeWidget. --- .../motion_planning_frame.h | 1 + .../src/motion_planning_display.cpp | 24 +++++++++--------- .../src/motion_planning_frame.cpp | 25 +++++++++++++++++++ 3 files changed, 38 insertions(+), 12 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index ac707bf294..26ac2b6a3a 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -143,6 +143,7 @@ class MotionPlanningFrame : public QWidget Q_SIGNALS: void planningFinished(); + void configChanged(); private Q_SLOTS: diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 4e8ab72d7c..28f7b3fbe4 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -230,6 +230,7 @@ void MotionPlanningDisplay::onInitialize() rviz::WindowManagerInterface* window_context = context_->getWindowManager(); frame_ = new MotionPlanningFrame(this, context_, window_context ? window_context->getParentWindow() : nullptr); + connect(frame_, SIGNAL(configChanged()), this->getModel(), SIGNAL(configChanged())); resetStatusTextColor(); addStatusText("Initialized."); @@ -1325,27 +1326,26 @@ void MotionPlanningDisplay::load(const rviz::Config& config) int attempts; if (config.mapGetInt("MoveIt_Planning_Attempts", &attempts)) frame_->ui_->planning_attempts->setValue(attempts); + if (config.mapGetFloat("Velocity_Scaling_Factor", &d)) + frame_->ui_->velocity_scaling_factor->setValue(d); + if (config.mapGetFloat("Acceleration_Scaling_Factor", &d)) + frame_->ui_->acceleration_scaling_factor->setValue(d); if (config.mapGetFloat("MoveIt_Goal_Tolerance", &d)) frame_->ui_->goal_tolerance->setValue(d); + bool b; + if (config.mapGetBool("MoveIt_Allow_Replanning", &b)) + frame_->ui_->allow_replanning->setChecked(b); + if (config.mapGetBool("MoveIt_Allow_Sensor_Positioning", &b)) + frame_->ui_->allow_looking->setChecked(b); + if (config.mapGetBool("MoveIt_Allow_External_Program", &b)) + frame_->ui_->allow_external_program->setChecked(b); if (config.mapGetBool("MoveIt_Use_Cartesian_Path", &b)) frame_->ui_->use_cartesian_path->setChecked(b); if (config.mapGetBool("MoveIt_Use_Constraint_Aware_IK", &b)) frame_->ui_->collision_aware_ik->setChecked(b); if (config.mapGetBool("MoveIt_Allow_Approximate_IK", &b)) frame_->ui_->approximate_ik->setChecked(b); - if (config.mapGetBool("MoveIt_Allow_External_Program", &b)) - frame_->ui_->allow_external_program->setChecked(b); - if (config.mapGetBool("MoveIt_Allow_Replanning", &b)) - frame_->ui_->allow_replanning->setChecked(b); - if (config.mapGetBool("MoveIt_Allow_Sensor_Positioning", &b)) - frame_->ui_->allow_looking->setChecked(b); - - float v; - if (config.mapGetFloat("Velocity_Scaling_Factor", &v)) - frame_->ui_->velocity_scaling_factor->setValue(v); - if (config.mapGetFloat("Acceleration_Scaling_Factor", &v)) - frame_->ui_->acceleration_scaling_factor->setValue(v); rviz::Config workspace = config.mapGetChild("MoveIt_Workspace"); rviz::Config ws_center = workspace.mapGetChild("Center"); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index ddaac366c3..0857a85bf1 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -149,6 +149,31 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: connect(ui_->tabWidget, SIGNAL(currentChanged(int)), this, SLOT(tabChanged(int))); + /* Notice changes to be safed in config file */ + connect(ui_->database_host, SIGNAL(textChanged(QString)), this, SIGNAL(configChanged())); + connect(ui_->database_port, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + + connect(ui_->goal_tolerance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + + connect(ui_->planning_time, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->planning_attempts, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->velocity_scaling_factor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->acceleration_scaling_factor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + + connect(ui_->allow_replanning, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(ui_->allow_looking, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(ui_->allow_external_program, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(ui_->use_cartesian_path, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(ui_->collision_aware_ik, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(ui_->approximate_ik, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + + connect(ui_->wcenter_x, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->wcenter_y, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->wcenter_z, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->wsize_x, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->wsize_y, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->wsize_z, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + QShortcut* copy_object_shortcut = new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_C), ui_->collision_objects_list); connect(copy_object_shortcut, SIGNAL(activated()), this, SLOT(copySelectedCollisionObject())); From 379acf91aa3630f320db77ba85b1ca294310b0bc Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sun, 29 Mar 2020 12:29:28 -0600 Subject: [PATCH 240/612] Integration test to defend subframe tutorial (#1757) * integration test to defend subframe tutorial * make AttachedBody track global transforms (additional to local ones) --- moveit_core/CMakeLists.txt | 1 + .../kinematic_constraints/src/utils.cpp | 35 ++-- moveit_core/package.xml | 1 + .../moveit/robot_state/attached_body.h | 49 +++-- moveit_core/robot_state/src/attached_body.cpp | 33 +++ moveit_core/robot_state/src/robot_state.cpp | 2 +- moveit_ros/planning_interface/package.xml | 1 + .../planning_interface/test/CMakeLists.txt | 5 + .../test/subframes_test.cpp | 198 ++++++++++++++++++ .../test/subframes_test.test | 29 +++ 10 files changed, 312 insertions(+), 42 deletions(-) create mode 100644 moveit_ros/planning_interface/test/subframes_test.cpp create mode 100644 moveit_ros/planning_interface/test/subframes_test.test diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index cd6c1ab095..e645a0d179 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -60,6 +60,7 @@ COMPONENTS tf2_eigen tf2_geometry_msgs eigen_stl_containers + eigen_conversions geometric_shapes geometry_msgs kdl_parser diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 6125ae954c..9b8813a1f7 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -39,6 +39,7 @@ #include #include #include +#include using namespace moveit::core; @@ -540,21 +541,19 @@ bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotSta if (!frame_found) return false; - // If the frame of the constraint is not part of the robot link model (but is an - // attached body or subframe instead), the constraint needs to be expressed in - // the frame of a robot link. + // If the frame of the constraint is not part of the robot link model (but an attached body or subframe), + // the constraint needs to be expressed in the frame of a robot link. if (c.link_name != robot_link->getName()) { - Eigen::Vector3d pos_in_link_frame, - pos_in_original_frame(c.target_point_offset.x, c.target_point_offset.y, c.target_point_offset.z); + Eigen::Isometry3d robot_link_to_link_name = state.getGlobalLinkTransform(robot_link).inverse() * transform; + Eigen::Vector3d offset_link_name(c.target_point_offset.x, c.target_point_offset.y, c.target_point_offset.z); + Eigen::Vector3d offset_robot_link = robot_link_to_link_name * offset_link_name; - pos_in_link_frame = transform * pos_in_original_frame; c.link_name = robot_link->getName(); - c.target_point_offset.x = pos_in_link_frame[0]; - c.target_point_offset.y = pos_in_link_frame[1]; - c.target_point_offset.z = pos_in_link_frame[2]; + tf::vectorEigenToMsg(offset_robot_link, c.target_point_offset); } } + for (auto& c : constraints.orientation_constraints) { bool frame_found; @@ -563,20 +562,16 @@ bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotSta if (!frame_found) return false; - // If the frame of the constraint is not part of the robot link model (but is an - // attached body or subframe instead), the constraint needs to be expressed in - // the frame of a robot link. + // If the frame of the constraint is not part of the robot link model (but an attached body or subframe), + // the constraint needs to be expressed in the frame of a robot link. if (c.link_name != robot_link->getName()) { - Eigen::Quaterniond q_body_to_link(transform.inverse().rotation()); - Eigen::Quaterniond q_target(c.orientation.w, c.orientation.x, c.orientation.y, c.orientation.z); - Eigen::Quaterniond q_in_link = q_body_to_link * q_target; - c.link_name = robot_link->getName(); - c.orientation.x = q_in_link.x(); - c.orientation.y = q_in_link.y(); - c.orientation.z = q_in_link.z(); - c.orientation.w = q_in_link.w(); + Eigen::Quaterniond link_name_to_robot_link(transform.linear().inverse() * + state.getGlobalLinkTransform(robot_link).linear()); + Eigen::Quaterniond quat_target; + tf::quaternionMsgToEigen(c.orientation, quat_target); + tf::quaternionEigenToMsg(quat_target * link_name_to_robot_link, c.orientation); } } return true; diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 2e540b1fc3..a8256a6e29 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -24,6 +24,7 @@ eigen bullet eigen_stl_containers + eigen_conversions libfcl-dev geometric_shapes geometry_msgs diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index bf5a4f5ec6..7086d00120 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -50,16 +50,16 @@ namespace core class AttachedBody; typedef boost::function AttachedBodyCallback; -/** @brief Object defining bodies that can be attached to robot - * links. This is useful when handling objects picked up by - * the robot. */ +/** @brief Object defining bodies that can be attached to robot links. + * + * This is useful when handling objects picked up by the robot. */ class AttachedBody { public: - /** \brief Construct an attached body for a specified \e link. The name of this body is \e id and it consists of \e - shapes that - attach to the link by the transforms \e attach_trans. The set of links that are allowed to be touched by this - object is specified by \e touch_links. */ + /** \brief Construct an attached body for a specified \e link. + * + * The name of this body is \e id and it consists of \e shapes that attach to the link by the transforms + * \e attach_trans. The set of links that are allowed to be touched by this object is specified by \e touch_links. */ AttachedBody(const LinkModel* link, const std::string& id, const std::vector& shapes, const EigenSTL::vector_Isometry3d& attach_trans, const std::set& touch_links, const trajectory_msgs::JointTrajectory& attach_posture, @@ -105,30 +105,41 @@ class AttachedBody return detach_posture_; } - /** \brief Get the fixed transforms (the transforms to the shapes associated with this body) */ + /** \brief Get the fixed transforms (the transforms to the shapes of this body, relative to the link) */ const EigenSTL::vector_Isometry3d& getFixedTransforms() const { return attach_trans_; } - /** \brief Get subframes of this object. */ + /** \brief Get subframes of this object (relative to the link) */ const moveit::core::FixedTransformsMap& getSubframeTransforms() const { return subframe_poses_; } - /** \brief Set all subframes of this object. */ + /** \brief Set all subframes of this object. + * + * Use these to define points of interest on the object to plan with + * (e.g. screwdriver/tip, kettle/spout, mug/base). + */ void setSubframeTransforms(const moveit::core::FixedTransformsMap& subframe_poses) { subframe_poses_ = subframe_poses; } - /** \brief Get the fixed transform to a named subframe on this body. + /** \brief Get the fixed transform to a named subframe on this body (relative to the robot link) + * * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). */ const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; + /** \brief Get the fixed transform to a named subframe on this body. + * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's + * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). */ + const Eigen::Isometry3d& getGlobalSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; + /** \brief Check whether a subframe of given @frame_name is present in this object. + * * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's * name is "screwdriver"). */ bool hasSubframeTransform(const std::string& frame_name) const; @@ -145,12 +156,8 @@ class AttachedBody /** \brief Set the scale for the shapes of this attached object */ void setScale(double scale); - /** \brief Recompute global_collision_body_transform given the transform of the parent link*/ - void computeTransform(const Eigen::Isometry3d& parent_link_global_transform) - { - for (std::size_t i = 0; i < global_collision_body_transforms_.size(); ++i) - global_collision_body_transforms_[i] = parent_link_global_transform * attach_trans_[i]; - } + /** \brief Recompute global_collision_body_transform given the transform of the parent link */ + void computeTransform(const Eigen::Isometry3d& parent_link_global_transform); private: /** \brief The link that owns this attached body */ @@ -175,11 +182,11 @@ class AttachedBody /** \brief The global transforms for these attached bodies (computed by forward kinematics) */ EigenSTL::vector_Isometry3d global_collision_body_transforms_; - /** \brief Transforms to subframes on the object. Transforms are relative to the link. - * Use these to define points of interest on the object to plan with - * (e.g. screwdriver/tip, kettle/spout, mug/base). - * */ + /** \brief Transforms to subframes on the object. Transforms are relative to the link. */ moveit::core::FixedTransformsMap subframe_poses_; + + /** \brief Transforms to subframes on the object, relative to the model frame. */ + moveit::core::FixedTransformsMap global_subframe_poses_; }; } } diff --git a/moveit_core/robot_state/src/attached_body.cpp b/moveit_core/robot_state/src/attached_body.cpp index b54e1fa1cf..51a403a7a2 100644 --- a/moveit_core/robot_state/src/attached_body.cpp +++ b/moveit_core/robot_state/src/attached_body.cpp @@ -54,6 +54,7 @@ AttachedBody::AttachedBody(const LinkModel* parent_link_model, const std::string , touch_links_(touch_links) , detach_posture_(detach_posture) , subframe_poses_(subframe_poses) + , global_subframe_poses_(subframe_poses) { global_collision_body_transforms_.resize(attach_trans.size()); for (Eigen::Isometry3d& global_collision_body_transform : global_collision_body_transforms_) @@ -79,6 +80,19 @@ void AttachedBody::setScale(double scale) } } +void AttachedBody::computeTransform(const Eigen::Isometry3d& parent_link_global_transform) +{ + // update collision body transforms + for (std::size_t i = 0; i < global_collision_body_transforms_.size(); ++i) + global_collision_body_transforms_[i] = parent_link_global_transform * attach_trans_[i]; + + // update subframe transforms + for (auto global = global_subframe_poses_.begin(), end = global_subframe_poses_.end(), + local = subframe_poses_.begin(); + global != end; ++global, ++local) + global->second = parent_link_global_transform * local->second; +} + void AttachedBody::setPadding(double padding) { for (shapes::ShapeConstPtr& shape : shapes_) @@ -114,11 +128,30 @@ const Eigen::Isometry3d& AttachedBody::getSubframeTransform(const std::string& f return IDENTITY_TRANSFORM; } +const Eigen::Isometry3d& AttachedBody::getGlobalSubframeTransform(const std::string& frame_name, bool* found) const +{ + if (boost::starts_with(frame_name, id_) && frame_name[id_.length()] == '/') + { + auto it = global_subframe_poses_.find(frame_name.substr(id_.length() + 1)); + if (it != global_subframe_poses_.end()) + { + if (found) + *found = true; + return it->second; + } + } + static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity(); + if (found) + *found = false; + return IDENTITY_TRANSFORM; +} + bool AttachedBody::hasSubframeTransform(const std::string& frame_name) const { bool found; getSubframeTransform(frame_name, &found); return found; } + } // namespace core } // namespace moveit diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 5324d65d8a..0e454a3e90 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1052,7 +1052,7 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c // Check if an AttachedBody has a subframe with name frame_id for (const std::pair& body : attached_body_map_) { - const auto& transform = body.second->getSubframeTransform(frame_id, &frame_found); + const auto& transform = body.second->getGlobalSubframeTransform(frame_id, &frame_found); if (frame_found) { robot_link = body.second->getAttachedLink(); diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index 5c57d296b1..e6cf739497 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -38,5 +38,6 @@ eigen moveit_resources + eigen_conversions rostest diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index eff482cec1..3bb83daaa3 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -1,6 +1,7 @@ if (CATKIN_ENABLE_TESTING) find_package(moveit_resources REQUIRED) find_package(rostest REQUIRED) + find_package(eigen_conversions REQUIRED) add_executable(test_cleanup test_cleanup.cpp) target_link_libraries(test_cleanup moveit_move_group_interface) @@ -8,6 +9,10 @@ if (CATKIN_ENABLE_TESTING) add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) + add_rostest_gtest(subframes_test subframes_test.test subframes_test.cpp) + target_link_libraries(subframes_test moveit_move_group_interface + ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) + add_rostest(python_move_group.test) add_rostest(python_move_group_ns.test) add_rostest(robot_state_update.test) diff --git a/moveit_ros/planning_interface/test/subframes_test.cpp b/moveit_ros/planning_interface/test/subframes_test.cpp new file mode 100644 index 0000000000..757fce094b --- /dev/null +++ b/moveit_ros/planning_interface/test/subframes_test.cpp @@ -0,0 +1,198 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2019, Felix von Drigalski, Jacob Aas, Tyler Weaver +* 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 OMRON SINIC X or PickNik Robotics 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. +*********************************************************************/ + +/* Author: Felix von Drigalski, Jacob Aas, Tyler Weaver */ + +/* This integration test is based on the tutorial for using subframes + * https://ros-planning.github.io/moveit_tutorials/doc/subframes/subframes_tutorial.html + */ + +// C++ +#include +#include + +// ROS +#include + +// The Testing Framework and Utils +#include + +// MoveIt +#include +#include +#include + +// TF2 +#include +#include + +constexpr double EPSILON = 1e-2; +constexpr double Z_OFFSET = 0.01; + +// Function copied from tutorial +// a small helper function to create our planning requests and move the robot. +bool moveToCartPose(const geometry_msgs::PoseStamped& pose, moveit::planning_interface::MoveGroupInterface& group, + const std::string& end_effector_link) +{ + group.clearPoseTargets(); + group.setEndEffectorLink(end_effector_link); + group.setStartStateToCurrentState(); + group.setPoseTarget(pose); + + moveit::planning_interface::MoveGroupInterface::Plan myplan; + if (group.plan(myplan) && group.execute(myplan)) + return true; + + ROS_WARN("Failed to perform motion."); + return false; +} + +// Function copied from tutorial +// This helper function creates two objects and publishes them to the PlanningScene: a box and a cylinder. +// The box spawns in front of the gripper, the cylinder at the tip of the gripper, as if it had been grasped. +void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface) +{ + const std::string log_name = "spawn_collision_objects"; + double z_offset_box = .25; // The z-axis points away from the gripper + double z_offset_cylinder = .1; + + // First, we start defining the CollisionObject as usual. + moveit_msgs::CollisionObject box; + box.id = "box"; + box.header.frame_id = "panda_hand"; + box.primitives.resize(1); + box.primitive_poses.resize(1); + box.primitives[0].type = box.primitives[0].BOX; + box.primitives[0].dimensions.resize(3); + box.primitives[0].dimensions[0] = 0.05; + box.primitives[0].dimensions[1] = 0.1; + box.primitives[0].dimensions[2] = 0.02; + box.primitive_poses[0].position.z = z_offset_box; + + // Then, we define the subframes of the CollisionObject. + box.subframe_names.resize(1); + box.subframe_poses.resize(1); + box.subframe_names[0] = "bottom"; + box.subframe_poses[0].position.y = -.05; + box.subframe_poses[0].position.z = 0.0 + z_offset_box; + tf2::Quaternion orientation; + orientation.setRPY(90.0 / 180.0 * M_PI, 0, 0); + box.subframe_poses[0].orientation = tf2::toMsg(orientation); + + // Next, define the cylinder + moveit_msgs::CollisionObject cylinder; + cylinder.id = "cylinder"; + cylinder.header.frame_id = "panda_hand"; + cylinder.primitives.resize(1); + cylinder.primitive_poses.resize(1); + cylinder.primitives[0].type = box.primitives[0].CYLINDER; + cylinder.primitives[0].dimensions.resize(2); + cylinder.primitives[0].dimensions[0] = 0.06; // height (along x) + cylinder.primitives[0].dimensions[1] = 0.005; // radius + cylinder.primitive_poses[0].position.x = 0.0; + cylinder.primitive_poses[0].position.y = 0.0; + cylinder.primitive_poses[0].position.z = 0.0 + z_offset_cylinder; + orientation.setRPY(0, 90.0 / 180.0 * M_PI, 0); + cylinder.primitive_poses[0].orientation = tf2::toMsg(orientation); + + cylinder.subframe_poses.resize(1); + cylinder.subframe_names.resize(1); + cylinder.subframe_names[0] = "tip"; + cylinder.subframe_poses[0].position.x = 0.03; + cylinder.subframe_poses[0].position.y = 0.0; + cylinder.subframe_poses[0].position.z = 0.0 + z_offset_cylinder; + orientation.setRPY(0, 90.0 / 180.0 * M_PI, 0); + cylinder.subframe_poses[0].orientation = tf2::toMsg(orientation); + + // Lastly, the objects are published to the PlanningScene. In this tutorial, we publish a box and a cylinder. + box.operation = moveit_msgs::CollisionObject::ADD; + cylinder.operation = moveit_msgs::CollisionObject::ADD; + planning_scene_interface.applyCollisionObjects({ box, cylinder }); +} + +TEST(TestPlanUsingSubframes, SubframesTests) +{ + const std::string log_name = "test_plan_using_subframes"; + ros::NodeHandle nh(log_name); + + auto planning_scene_monitor = std::make_shared("robot_description"); + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + moveit::planning_interface::MoveGroupInterface group("panda_arm"); + group.setPlanningTime(10.0); + + spawnCollisionObjects(planning_scene_interface); + moveit_msgs::AttachedCollisionObject att_coll_object; + att_coll_object.object.id = "cylinder"; + att_coll_object.link_name = "panda_hand"; + att_coll_object.object.operation = att_coll_object.object.ADD; + planning_scene_interface.applyAttachedCollisionObject(att_coll_object); + + tf2::Quaternion target_orientation; + target_orientation.setRPY(0, 180.0 / 180.0 * M_PI, 90.0 / 180.0 * M_PI); + geometry_msgs::PoseStamped target_pose_stamped; + target_pose_stamped.pose.orientation = tf2::toMsg(target_orientation); + target_pose_stamped.pose.position.z = Z_OFFSET; + + ROS_INFO_STREAM_NAMED(log_name, "Moving to bottom of box with cylinder tip"); + target_pose_stamped.header.frame_id = "box/bottom"; + ASSERT_TRUE(moveToCartPose(target_pose_stamped, group, "cylinder/tip")); + planning_scene_monitor->requestPlanningSceneState(); + { + planning_scene_monitor::LockedPlanningSceneRO planning_scene(planning_scene_monitor); + + // get the tip and box subframe locations in world + Eigen::Isometry3d eef = planning_scene->getFrameTransform("cylinder/tip"); + Eigen::Isometry3d box_subframe = planning_scene->getFrameTransform(target_pose_stamped.header.frame_id); + Eigen::Isometry3d target_pose; + tf::poseMsgToEigen(target_pose_stamped.pose, target_pose); + + // expect that they are identical + std::stringstream ss; + ss << "target frame: \n" << (box_subframe * target_pose).matrix() << "\ncylinder frame: \n" << eef.matrix(); + EXPECT_TRUE(eef.isApprox(box_subframe * target_pose, EPSILON)) << ss.str(); + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "moveit_test_plan_using_subframes"); + testing::InitGoogleTest(&argc, argv); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/moveit_ros/planning_interface/test/subframes_test.test b/moveit_ros/planning_interface/test/subframes_test.test new file mode 100644 index 0000000000..64d3bef441 --- /dev/null +++ b/moveit_ros/planning_interface/test/subframes_test.test @@ -0,0 +1,29 @@ + + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + From 5298b9ff4d05b5f96cf2831aafa003fb9bea265f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Mon, 30 Mar 2020 16:53:44 +0200 Subject: [PATCH 241/612] Modernize Travis config (#1999) ... to satisfy the new Travis config validator --- .travis.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 6ed584a5ff..c834e8d629 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,5 +1,5 @@ # This config file for Travis CI utilizes https://github.com/ros-planning/moveit_ci/ package. -sudo: required +os: linux dist: xenial # distro used by Travis, moveit_ci uses the docker image's distro services: - docker @@ -21,13 +21,13 @@ env: - TEST_BLACKLIST="moveit_ros_perception" - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" - WARNINGS_OK=false - matrix: + jobs: - TEST="clang-format catkin_lint" - TEST=code-coverage - ROS_DISTRO=melodic - ROS_DISTRO=kinetic -matrix: # Add a separate config to the matrix, using clang as compiler +jobs: # Add a separate config to the matrix, using clang as compiler include: # add a config to the matrix using clang as compiler and also test ikfast plugin creation - compiler: clang From fa6b72dade7ce181c41e7edaad04d2189dfc514e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Tue, 31 Mar 2020 11:35:39 +0200 Subject: [PATCH 242/612] temporarily skip ros-shadow-fixed (#2000) --- .travis.yml | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/.travis.yml b/.travis.yml index c834e8d629..c0e97e7143 100644 --- a/.travis.yml +++ b/.travis.yml @@ -21,20 +21,19 @@ env: - TEST_BLACKLIST="moveit_ros_perception" - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" - WARNINGS_OK=false - jobs: - - TEST="clang-format catkin_lint" - - TEST=code-coverage - - ROS_DISTRO=melodic - - ROS_DISTRO=kinetic -jobs: # Add a separate config to the matrix, using clang as compiler +jobs: include: - # add a config to the matrix using clang as compiler and also test ikfast plugin creation + - env: TEST="clang-format catkin_lint" + - env: TEST=code-coverage + - env: ROS_DISTRO=melodic + - env: ROS_DISTRO=kinetic - compiler: clang - env: ROS_REPO=ros-shadow-fixed - TEST=clang-tidy-fix + # test_ikfast_plugins takes ~10 minutes: include here to keep the main jobs shorter + env: TEST=clang-tidy-fix BEFORE_DOCKER_SCRIPT="source moveit_kinematics/test/test_ikfast_plugins.sh" CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-overloaded-virtual" +# ROS_REPO=ros-shadow-fixed before_script: - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci From a6c9852a9bb35633c7e695cd200ffd2b52bc75ad Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 31 Mar 2020 11:34:59 +0200 Subject: [PATCH 243/612] Travis: send emails to author/committer --- .travis.yml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index c0e97e7143..2fb7ce897f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -8,9 +8,8 @@ cache: ccache compiler: gcc notifications: - email: - recipients: - - 130s@2000.jukuin.keio.ac.jp + email: true + env: global: - MOVEIT_CI_TRAVIS_TIMEOUT=85 # Travis grants us 90 min, but we add a safety margin of 5 min From dc71f6de61fb57e2843e3f979e5592b330cb8028 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Tue, 31 Mar 2020 21:13:03 +0900 Subject: [PATCH 244/612] Fix TfPublisher subframe publishing --- .../src/default_capabilities/tf_publisher_capability.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp index b48eea2e81..c60b9ff1f6 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -95,7 +95,7 @@ void TfPublisher::publishPlanningSceneFrames() broadcaster.sendTransform(transform); const moveit::core::FixedTransformsMap& subframes = obj.second->subframe_poses_; - publishSubframes(broadcaster, subframes, object_frame, stamp); + publishSubframes(broadcaster, subframes, planning_frame, stamp); } const moveit::core::RobotState& rs = locked_planning_scene->getCurrentState(); From 60c78f29ee673b1bdadc12944f55cfb4aaa767ca Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Tue, 31 Mar 2020 21:58:40 +0900 Subject: [PATCH 245/612] fixup! Fix TfPublisher subframe publishing --- .../src/default_capabilities/tf_publisher_capability.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp index c60b9ff1f6..5c0de5d28c 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -57,13 +57,13 @@ TfPublisher::~TfPublisher() namespace { void publishSubframes(tf2_ros::TransformBroadcaster& broadcaster, const moveit::core::FixedTransformsMap& subframes, - const std::string& parent_frame, const ros::Time& stamp) + const std::string& parent_object, const std::string& parent_frame, const ros::Time& stamp) { geometry_msgs::TransformStamped transform; for (auto& subframe : subframes) { transform = tf2::eigenToTransform(subframe.second); - transform.child_frame_id = parent_frame + "/" + subframe.first; + transform.child_frame_id = parent_object + "/" + subframe.first; transform.header.stamp = stamp; transform.header.frame_id = parent_frame; broadcaster.sendTransform(transform); @@ -95,7 +95,7 @@ void TfPublisher::publishPlanningSceneFrames() broadcaster.sendTransform(transform); const moveit::core::FixedTransformsMap& subframes = obj.second->subframe_poses_; - publishSubframes(broadcaster, subframes, planning_frame, stamp); + publishSubframes(broadcaster, subframes, object_frame, planning_frame, stamp); } const moveit::core::RobotState& rs = locked_planning_scene->getCurrentState(); @@ -111,7 +111,7 @@ void TfPublisher::publishPlanningSceneFrames() broadcaster.sendTransform(transform); const moveit::core::FixedTransformsMap& subframes = attached_body->getSubframeTransforms(); - publishSubframes(broadcaster, subframes, object_frame, stamp); + publishSubframes(broadcaster, subframes, object_frame, object_frame, stamp); } } From ef16990f7bd11082fa3ff0bda5a8c91632dee531 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 26 Mar 2020 22:01:45 +0100 Subject: [PATCH 246/612] Wait for initial jog commands in main loop --- .../moveit_jog_arm/src/jog_calcs.cpp | 61 +++++++------------ 1 file changed, 21 insertions(+), 40 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index ba562e8458..04b293993a 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -107,49 +107,20 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) is_initialized_ = true; - // Wait for the first jogging cmd. - // Store it in a class member for further calcs, then free up the shared variable again. - geometry_msgs::TwistStamped cartesian_deltas; - control_msgs::JointJog joint_deltas; - while (ros::ok() && (cartesian_deltas.header.stamp == ros::Time(0.)) && (joint_deltas.header.stamp == ros::Time(0.))) - { - if (stop_jog_loop_requested_) - return; - - default_sleep_rate_.sleep(); - - // Ensure the low-pass filter matches reality - for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(internal_joint_state_.position[i]); - - // Check for a new command - shared_variables.lock(); - cartesian_deltas = shared_variables.command_deltas; - joint_deltas = shared_variables.joint_command_deltas; - incoming_joint_state_ = shared_variables.joints; - shared_variables.unlock(); - - kinematic_state_->setVariableValues(internal_joint_state_); - - // Always update the end-effector transform in case the getCommandFrameTransform() method is being used - // Get the transform from MoveIt planning frame to jogging command frame - // We solve (planning_frame -> base -> robot_link_command_frame) - // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) - tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * - kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); - - shared_variables.lock(); - shared_variables.tf_moveit_to_cmd_frame = tf_moveit_to_cmd_frame_; - shared_variables.unlock(); - } - // Track the number of cycles during which motion has not occurred. // Will avoid re-publishing zero velocities endlessly. int zero_velocity_count = 0; ros::Rate loop_rate(1. / parameters_.publish_period); - // Now do jogging calcs + // Flag for staying inactive while there are no incoming commands + bool wait_for_jog_commands = true; + + // Incoming command messages + geometry_msgs::TwistStamped cartesian_deltas; + control_msgs::JointJog joint_deltas; + + // Do jogging calcs while (ros::ok() && !stop_jog_loop_requested_) { // Always update the joints and end-effector transform for 2 reasons: @@ -170,9 +141,19 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) shared_variables.tf_moveit_to_cmd_frame = tf_moveit_to_cmd_frame_; shared_variables.unlock(); - // If paused, just keep the low-pass filters up to date with current joints so a jump doesn't occur when - // restarting - if (halt_outgoing_jog_cmds_) + // Check for an initial jog command + if (wait_for_jog_commands) + { + shared_variables.lock(); + // Check if there are any commands with valid timestamp + wait_for_jog_commands = shared_variables.command_deltas.header.stamp == ros::Time(0.) + && shared_variables.joint_command_deltas.header.stamp == ros::Time(0.); + shared_variables.unlock(); + } + + // If paused or while waiting for initial jog commands, just keep the low-pass filters up to date with current + // joints so a jump doesn't occur when restarting + if (wait_for_jog_commands || halt_outgoing_jog_cmds_) { for (std::size_t i = 0; i < num_joints_; ++i) position_filters_[i].reset(internal_joint_state_.position[i]); From d168cd9eac22e02a5892a746de656a99a9b18db6 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 26 Mar 2020 22:03:23 +0100 Subject: [PATCH 247/612] Remove redundant joint state increments --- moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 04b293993a..a400933103 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -326,9 +326,6 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& enforceSRDFAccelVelLimits(delta_theta_); - if (!addJointIncrements(internal_joint_state_, delta_theta_)) - return false; - // If close to a collision or a singularity, decelerate applyVelocityScaling(shared_variables, delta_theta_, velocityScalingFactorForSingularity(delta_x, svd, jacobian, pseudo_inverse)); From 2e4c08d52ecde2d861381ccaee7268e6a3c5f1ff Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 26 Mar 2020 22:16:14 +0100 Subject: [PATCH 248/612] Verify valid joints by filtering for active joint models only --- .../moveit_jog_arm/src/jog_calcs.cpp | 89 ++++++++++--------- 1 file changed, 49 insertions(+), 40 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index a400933103..bce531df2e 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -61,7 +61,7 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader: kinematic_state_->setToDefaultValues(); joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); - prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getVariableCount()); + prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getActiveJointModels().size()); } void JogCalcs::startMainLoop(JogArmShared& shared_variables) @@ -76,7 +76,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) ros::topic::waitForMessage(parameters_.joint_topic); ROS_INFO_NAMED(LOGNAME, "jog_calcs_thread: Received first joint msg."); - internal_joint_state_.name = joint_model_group_->getVariableNames(); + internal_joint_state_.name = joint_model_group_->getActiveJointModelNames(); num_joints_ = internal_joint_state_.name.size(); internal_joint_state_.position.resize(num_joints_); internal_joint_state_.velocity.resize(num_joints_); @@ -146,8 +146,8 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) { shared_variables.lock(); // Check if there are any commands with valid timestamp - wait_for_jog_commands = shared_variables.command_deltas.header.stamp == ros::Time(0.) - && shared_variables.joint_command_deltas.header.stamp == ros::Time(0.); + wait_for_jog_commands = shared_variables.command_deltas.header.stamp == ros::Time(0.) && + shared_variables.joint_command_deltas.header.stamp == ros::Time(0.); shared_variables.unlock(); } @@ -555,61 +555,70 @@ double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& comm void JogCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta) { Eigen::ArrayXd velocity = delta_theta / parameters_.publish_period; + const Eigen::ArrayXd acceleration = (velocity - prev_joint_velocity_) / parameters_.publish_period; std::size_t joint_delta_index = 0; - for (auto joint : joint_model_group_->getJointModels()) + for (auto joint : joint_model_group_->getActiveJointModels()) { // Some joints do not have bounds defined - if (kinematic_state_->getJointModel(joint->getName())->hasVariable(joint->getName())) + const auto bounds = joint->getVariableBounds(joint->getName()); + if (bounds.acceleration_bounded_) { - auto bounds = kinematic_state_->getJointModel(joint->getName())->getVariableBounds(joint->getName()); + bool clip_acceleration = false; + double acceleration_limit = 0.0; + if (acceleration(joint_delta_index) < bounds.min_acceleration_) + { + clip_acceleration = true; + acceleration_limit = bounds.min_acceleration_; + } + else if (acceleration(joint_delta_index) > bounds.max_acceleration_) + { + clip_acceleration = true; + acceleration_limit = bounds.max_acceleration_; + } // Apply acceleration bounds - // accel = (vel - vel_prev) / delta_t = ((delta_theta / delta_t) - vel_prev) / delta_t - // --> delta_theta = (accel * delta_t _ + vel_prev) * delta_t - Eigen::ArrayXd acceleration = (velocity - prev_joint_velocity_) / parameters_.publish_period; - if ((bounds.min_acceleration_ != 0) && (acceleration(joint_delta_index) < bounds.min_acceleration_)) + if (clip_acceleration) { - double relative_change = - ((bounds.min_acceleration_ * parameters_.publish_period + prev_joint_velocity_(joint_delta_index)) * + // accel = (vel - vel_prev) / delta_t = ((delta_theta / delta_t) - vel_prev) / delta_t + // --> delta_theta = (accel * delta_t _ + vel_prev) * delta_t + const double relative_change = + ((acceleration_limit * parameters_.publish_period + prev_joint_velocity_(joint_delta_index)) * parameters_.publish_period) / delta_theta(joint_delta_index); // Avoid nan if (fabs(relative_change) < 1) - delta_theta = relative_change * delta_theta; + delta_theta(joint_delta_index) = relative_change * delta_theta(joint_delta_index); } - else if ((bounds.max_acceleration_ != 0) && (acceleration(joint_delta_index) > bounds.max_acceleration_)) + } + + if (bounds.velocity_bounded_) + { + velocity(joint_delta_index) = delta_theta(joint_delta_index) / parameters_.publish_period; + + bool clip_velocity = false; + double velocity_limit = 0.0; + if (velocity(joint_delta_index) < bounds.min_velocity_) { - double relative_change = - ((bounds.max_acceleration_ * parameters_.publish_period + prev_joint_velocity_(joint_delta_index)) * - parameters_.publish_period) / - delta_theta(joint_delta_index); - // Avoid nan - if (fabs(relative_change) < 1) - delta_theta = relative_change * delta_theta; + clip_velocity = true; + velocity_limit = bounds.min_velocity_; } - - velocity = delta_theta / parameters_.publish_period; - // Apply velocity bounds - // delta_theta = joint_velocity * delta_t - if ((bounds.min_velocity_ != 0) && (velocity(joint_delta_index) < bounds.min_velocity_)) + else if (velocity(joint_delta_index) > bounds.max_velocity_) { - double relative_change = (bounds.min_velocity_ * parameters_.publish_period) / delta_theta(joint_delta_index); - // Avoid nan - if (fabs(relative_change) < 1) - { - delta_theta = relative_change * delta_theta; - velocity = relative_change * velocity; - } + clip_velocity = true; + velocity_limit = bounds.max_velocity_; } - else if ((bounds.max_velocity_ != 0) && (velocity(joint_delta_index) > bounds.max_velocity_)) + + // Apply velocity bounds + if (clip_velocity) { - double relative_change = (bounds.max_velocity_ * parameters_.publish_period) / delta_theta(joint_delta_index); + // delta_theta = joint_velocity * delta_t + const double relative_change = (velocity_limit * parameters_.publish_period) / delta_theta(joint_delta_index); // Avoid nan if (fabs(relative_change) < 1) { - delta_theta = relative_change * delta_theta; - velocity = relative_change * velocity; + delta_theta(joint_delta_index) = relative_change * delta_theta(joint_delta_index); + velocity(joint_delta_index) = relative_change * velocity(joint_delta_index); } } ++joint_delta_index; @@ -621,7 +630,7 @@ bool JogCalcs::enforceSRDFPositionLimits(trajectory_msgs::JointTrajectory& new_j { bool halting = false; - for (auto joint : joint_model_group_->getJointModels()) + for (auto joint : joint_model_group_->getActiveJointModels()) { // Halt if we're past a joint margin and joint velocity is moving even farther past double joint_angle = 0; @@ -707,7 +716,7 @@ bool JogCalcs::updateJoints(JogArmShared& shared_variables) } catch (const std::out_of_range& e) { - ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_.name[m]); + ROS_DEBUG_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_.name[m]); continue; } From 85be9f8f2709136b4bff59d315daa660e90dcc27 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 26 Mar 2020 22:35:28 +0100 Subject: [PATCH 249/612] Add pause/unpause flags for jog thread --- .../include/moveit_jog_arm/jog_calcs.h | 8 +- .../moveit_jog_arm/jog_cpp_interface.h | 7 ++ .../moveit_jog_arm/src/jog_calcs.cpp | 13 ++- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 96 +++++++++++-------- 4 files changed, 80 insertions(+), 44 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 916a8aa9ff..da328d0a36 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -62,7 +62,11 @@ class JogCalcs void stopMainLoop(); - void haltOutgoingJogCmds(); + /** \brief Stop publishing jog commands while keeping the main loop spinning and filters up to date */ + void pauseOutgoingJogCmds(); + + /** \brief Continue publishing jog commands */ + void unpauseOutgoingJogCmds(); /** \brief Check if the robot state is being updated and the end effector transform is known * @return true if initialized properly @@ -76,7 +80,7 @@ class JogCalcs std::atomic stop_jog_loop_requested_; // Flag that outgoing commands to the robot should not be published - std::atomic halt_outgoing_jog_cmds_; + std::atomic pause_outgoing_jog_cmds_; // Flag that robot state is up to date, end effector transform is known std::atomic is_initialized_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h index 533c311d08..edaec28a14 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h @@ -59,6 +59,12 @@ class JogCppInterface : public JogInterfaceBase void stopMainLoop(); + /** \brief Pause processing jog commands while keeping the main loop alive */ + void pause(); + + /** \brief Continue processing jog commands */ + void unpause(); + /** \brief Provide a Cartesian velocity command to the jogger. * The units are determined by settings in the yaml file. */ @@ -95,5 +101,6 @@ class JogCppInterface : public JogInterfaceBase ros::NodeHandle nh_; std::atomic stop_requested_; + std::atomic paused_; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index bce531df2e..24829c8169 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -68,7 +68,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) { // Reset flags stop_jog_loop_requested_ = false; - halt_outgoing_jog_cmds_ = false; + pause_outgoing_jog_cmds_ = false; is_initialized_ = false; // Wait for initial messages @@ -153,7 +153,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) // If paused or while waiting for initial jog commands, just keep the low-pass filters up to date with current // joints so a jump doesn't occur when restarting - if (wait_for_jog_commands || halt_outgoing_jog_cmds_) + if (wait_for_jog_commands || pause_outgoing_jog_cmds_) { for (std::size_t i = 0; i < num_joints_; ++i) position_filters_[i].reset(internal_joint_state_.position[i]); @@ -249,9 +249,14 @@ void JogCalcs::stopMainLoop() stop_jog_loop_requested_ = true; } -void JogCalcs::haltOutgoingJogCmds() +void JogCalcs::pauseOutgoingJogCmds() { - halt_outgoing_jog_cmds_ = true; + pause_outgoing_jog_cmds_ = true; +} + +void JogCalcs::unpauseOutgoingJogCmds() +{ + pause_outgoing_jog_cmds_ = false; } bool JogCalcs::isInitialized() diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp index 0414d2a9ef..2a5214f752 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp @@ -61,6 +61,7 @@ void JogCppInterface::startMainLoop() { // Reset loop termination flag stop_requested_ = false; + paused_ = false; // Crunch the numbers in this thread startJogCalcThread(); @@ -95,51 +96,54 @@ void JogCppInterface::startMainLoop() { ros::spinOnce(); - shared_variables_.lock(); - trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; - - // Check if incoming commands are stale - if ((ros::Time::now() - shared_variables_.latest_nonzero_cmd_stamp) < - ros::Duration(ros_parameters_.incoming_command_timeout)) - { - shared_variables_.command_is_stale = false; - } - else + if (!paused_) { - shared_variables_.command_is_stale = true; - } + shared_variables_.lock(); + trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; - // Publish the most recent trajectory, unless the jogging calculation thread tells not to - if (shared_variables_.ok_to_publish) - { - // Put the outgoing msg in the right format - // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") + // Check if incoming commands are stale + if ((ros::Time::now() - shared_variables_.latest_nonzero_cmd_stamp) < + ros::Duration(ros_parameters_.incoming_command_timeout)) { - outgoing_command.header.stamp = ros::Time::now(); - outgoing_cmd_pub.publish(outgoing_command); + shared_variables_.command_is_stale = false; } - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") + else { - std_msgs::Float64MultiArray joints; - if (ros_parameters_.publish_joint_positions) - joints.data = outgoing_command.points[0].positions; - else if (ros_parameters_.publish_joint_velocities) - joints.data = outgoing_command.points[0].velocities; - outgoing_cmd_pub.publish(joints); + shared_variables_.command_is_stale = true; + } + + // Publish the most recent trajectory, unless the jogging calculation thread tells not to + if (shared_variables_.ok_to_publish) + { + // Put the outgoing msg in the right format + // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). + if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") + { + outgoing_command.header.stamp = ros::Time::now(); + outgoing_cmd_pub.publish(outgoing_command); + } + else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") + { + std_msgs::Float64MultiArray joints; + if (ros_parameters_.publish_joint_positions) + joints.data = outgoing_command.points[0].positions; + else if (ros_parameters_.publish_joint_velocities) + joints.data = outgoing_command.points[0].velocities; + outgoing_cmd_pub.publish(joints); + } + } + else if (shared_variables_.command_is_stale) + { + ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " + "Try a larger 'incoming_command_timeout' parameter?"); + } + else + { + ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); } - } - else if (shared_variables_.command_is_stale) - { - ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " - "Try a larger 'incoming_command_timeout' parameter?"); - } - else - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); - } - shared_variables_.unlock(); + shared_variables_.unlock(); + } main_rate.sleep(); } @@ -153,6 +157,22 @@ void JogCppInterface::stopMainLoop() stop_requested_ = true; } +void JogCppInterface::pause() +{ + if (jog_calcs_) + jog_calcs_->pauseOutgoingJogCmds(); + + paused_ = true; +} + +void JogCppInterface::unpause() +{ + if (jog_calcs_) + jog_calcs_->unpauseOutgoingJogCmds(); + + paused_ = false; +} + void JogCppInterface::provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command) { shared_variables_.lock(); From 597dd5064726f3d75ab9e36d2d04353b135818af Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Wed, 1 Apr 2020 21:53:10 +0900 Subject: [PATCH 250/612] fixup! Fix TfPublisher subframe publishing --- .../src/default_capabilities/tf_publisher_capability.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp index 5c0de5d28c..f91b71c81f 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -111,7 +111,7 @@ void TfPublisher::publishPlanningSceneFrames() broadcaster.sendTransform(transform); const moveit::core::FixedTransformsMap& subframes = attached_body->getSubframeTransforms(); - publishSubframes(broadcaster, subframes, object_frame, object_frame, stamp); + publishSubframes(broadcaster, subframes, object_frame, attached_body->getAttachedLinkName(), stamp); } } From 2627d7eedd8394c10619a55f7f86647b838e2e58 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 31 Mar 2020 23:08:27 +0200 Subject: [PATCH 251/612] Pause/stop JogArm threads using shared atomic bool variables --- .../moveit_jog_arm/collision_check_thread.h | 5 -- .../include/moveit_jog_arm/jog_arm_data.h | 6 ++ .../include/moveit_jog_arm/jog_calcs.h | 14 ---- .../moveit_jog_arm/jog_cpp_interface.h | 10 +-- .../src/collision_check_thread.cpp | 79 +++++++++---------- .../moveit_jog_arm/src/jog_calcs.cpp | 23 +----- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 25 ++---- .../moveit_jog_arm/src/jog_interface_base.cpp | 6 -- .../moveit_jog_arm/src/jog_ros_interface.cpp | 2 + 9 files changed, 57 insertions(+), 113 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index f5da13f822..24f2c52b31 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -62,12 +62,7 @@ class CollisionCheckThread void startMainLoop(moveit_jog_arm::JogArmShared& shared_variables); - void stopMainLoop(); - private: - // Loop termination flag - std::atomic stop_requested_; - const moveit_jog_arm::JogArmParameters parameters_; // Pointer to the collision environment diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 31d8e6a374..62f1604aa2 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -95,6 +95,12 @@ struct JogArmShared : public std::mutex // Status of the jogger. 0 for no warning. The meaning of nonzero values can be seen in status_codes.h std::atomic status; + + // Pause/unpause jog threads - threads are not paused by default + std::atomic paused{ false }; + + // Stop jog loop threads - threads are not stopped by default + std::atomic stop_requested{ false }; }; // ROS params to be read. See the yaml file in /config for a description of each. diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index da328d0a36..049f7af452 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -60,14 +60,6 @@ class JogCalcs void startMainLoop(JogArmShared& shared_variables); - void stopMainLoop(); - - /** \brief Stop publishing jog commands while keeping the main loop spinning and filters up to date */ - void pauseOutgoingJogCmds(); - - /** \brief Continue publishing jog commands */ - void unpauseOutgoingJogCmds(); - /** \brief Check if the robot state is being updated and the end effector transform is known * @return true if initialized properly */ @@ -76,12 +68,6 @@ class JogCalcs protected: ros::NodeHandle nh_; - // Loop termination flag - std::atomic stop_jog_loop_requested_; - - // Flag that outgoing commands to the robot should not be published - std::atomic pause_outgoing_jog_cmds_; - // Flag that robot state is up to date, end effector transform is known std::atomic is_initialized_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h index edaec28a14..a6d35e077a 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h @@ -59,11 +59,8 @@ class JogCppInterface : public JogInterfaceBase void stopMainLoop(); - /** \brief Pause processing jog commands while keeping the main loop alive */ - void pause(); - - /** \brief Continue processing jog commands */ - void unpause(); + /** \brief Pause or unpause processing jog commands while keeping the main loop alive */ + void setPaused(bool paused); /** \brief Provide a Cartesian velocity command to the jogger. * The units are determined by settings in the yaml file. @@ -99,8 +96,5 @@ class JogCppInterface : public JogInterfaceBase private: ros::NodeHandle nh_; - - std::atomic stop_requested_; - std::atomic paused_; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index ae4709ee70..0657f13229 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -61,9 +61,6 @@ planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPla void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) { - // Reset loop termination flag - stop_requested_ = false; - // Init collision request collision_detection::CollisionRequest collision_request; collision_request.group_name = parameters_.move_group_name; @@ -84,50 +81,48 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) ///////////////////////////////////////////////// // Spin while checking collisions ///////////////////////////////////////////////// - while (ros::ok() && !stop_requested_) + while (ros::ok() && !shared_variables.stop_requested) { - shared_variables.lock(); - sensor_msgs::JointState jts = shared_variables.joints; - shared_variables.unlock(); - - for (std::size_t i = 0; i < jts.position.size(); ++i) - current_state.setJointPositions(jts.name[i], &jts.position[i]); - - collision_result.clear(); - current_state.updateCollisionBodyTransforms(); - - // Do a thread-safe distance-based collision detection - getLockedPlanningSceneRO()->checkCollision(collision_request, collision_result, current_state); - - // If we're definitely in collision, stop immediately - if (collision_result.collision) - { - velocity_scale = 0; - } - - // If we are far from a collision, velocity_scale should be 1. - // If we are very close to a collision, velocity_scale should be ~zero. - // When collision_proximity_threshold is breached, start decelerating exponentially. - else if (collision_result.distance < parameters_.collision_proximity_threshold) + if (!shared_variables.paused) { - // velocity_scale = e ^ k * (collision_result.distance - threshold) - // k = - ln(0.001) / collision_proximity_threshold - // velocity_scale should equal one when collision_result.distance is at collision_proximity_threshold. - // velocity_scale should equal 0.001 when collision_result.distance is at zero. - velocity_scale = - exp(velocity_scale_coefficient * (collision_result.distance - parameters_.collision_proximity_threshold)); + shared_variables.lock(); + sensor_msgs::JointState jts = shared_variables.joints; + shared_variables.unlock(); + + for (std::size_t i = 0; i < jts.position.size(); ++i) + current_state.setJointPositions(jts.name[i], &jts.position[i]); + + collision_result.clear(); + current_state.updateCollisionBodyTransforms(); + + // Do a thread-safe distance-based collision detection + getLockedPlanningSceneRO()->checkCollision(collision_request, collision_result, current_state); + + // If we're definitely in collision, stop immediately + if (collision_result.collision) + { + velocity_scale = 0; + } + + // If we are far from a collision, velocity_scale should be 1. + // If we are very close to a collision, velocity_scale should be ~zero. + // When collision_proximity_threshold is breached, start decelerating exponentially. + else if (collision_result.distance < parameters_.collision_proximity_threshold) + { + // velocity_scale = e ^ k * (collision_result.distance - threshold) + // k = - ln(0.001) / collision_proximity_threshold + // velocity_scale should equal one when collision_result.distance is at collision_proximity_threshold. + // velocity_scale should equal 0.001 when collision_result.distance is at zero. + velocity_scale = + exp(velocity_scale_coefficient * (collision_result.distance - parameters_.collision_proximity_threshold)); + } + + shared_variables.lock(); + shared_variables.collision_velocity_scale = velocity_scale; + shared_variables.unlock(); } - shared_variables.lock(); - shared_variables.collision_velocity_scale = velocity_scale; - shared_variables.unlock(); - collision_rate.sleep(); } } - -void CollisionCheckThread::stopMainLoop() -{ - stop_requested_ = true; -} } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 24829c8169..2e29282d9b 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -67,8 +67,6 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader: void JogCalcs::startMainLoop(JogArmShared& shared_variables) { // Reset flags - stop_jog_loop_requested_ = false; - pause_outgoing_jog_cmds_ = false; is_initialized_ = false; // Wait for initial messages @@ -96,7 +94,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) // Initialize the position filters to initial robot joints while (!updateJoints(shared_variables) && ros::ok()) { - if (stop_jog_loop_requested_) + if (shared_variables.stop_requested) return; shared_variables.lock(); @@ -121,7 +119,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) control_msgs::JointJog joint_deltas; // Do jogging calcs - while (ros::ok() && !stop_jog_loop_requested_) + while (ros::ok() && !shared_variables.stop_requested) { // Always update the joints and end-effector transform for 2 reasons: // 1) in case the getCommandFrameTransform() method is being used @@ -153,7 +151,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) // If paused or while waiting for initial jog commands, just keep the low-pass filters up to date with current // joints so a jump doesn't occur when restarting - if (wait_for_jog_commands || pause_outgoing_jog_cmds_) + if (wait_for_jog_commands || shared_variables.paused) { for (std::size_t i = 0; i < num_joints_; ++i) position_filters_[i].reset(internal_joint_state_.position[i]); @@ -244,21 +242,6 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) } } -void JogCalcs::stopMainLoop() -{ - stop_jog_loop_requested_ = true; -} - -void JogCalcs::pauseOutgoingJogCmds() -{ - pause_outgoing_jog_cmds_ = true; -} - -void JogCalcs::unpauseOutgoingJogCmds() -{ - pause_outgoing_jog_cmds_ = false; -} - bool JogCalcs::isInitialized() { return is_initialized_; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp index 2a5214f752..bfa0ea9832 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp @@ -60,8 +60,8 @@ JogCppInterface::~JogCppInterface() void JogCppInterface::startMainLoop() { // Reset loop termination flag - stop_requested_ = false; - paused_ = false; + shared_variables_.stop_requested = false; + shared_variables_.paused = false; // Crunch the numbers in this thread startJogCalcThread(); @@ -92,11 +92,11 @@ void JogCppInterface::startMainLoop() ros::Rate main_rate(1. / ros_parameters_.publish_period); - while (ros::ok() && !stop_requested_) + while (ros::ok() && !shared_variables_.stop_requested) { ros::spinOnce(); - if (!paused_) + if (!shared_variables_.paused) { shared_variables_.lock(); trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; @@ -154,23 +154,12 @@ void JogCppInterface::startMainLoop() void JogCppInterface::stopMainLoop() { - stop_requested_ = true; + shared_variables_.stop_requested = true; } -void JogCppInterface::pause() +void JogCppInterface::setPaused(bool paused) { - if (jog_calcs_) - jog_calcs_->pauseOutgoingJogCmds(); - - paused_ = true; -} - -void JogCppInterface::unpause() -{ - if (jog_calcs_) - jog_calcs_->unpauseOutgoingJogCmds(); - - paused_ = false; + shared_variables_.paused = paused; } void JogCppInterface::provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index 1c550b5998..d07b10981a 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -237,9 +237,6 @@ bool JogInterfaceBase::startJogCalcThread() bool JogInterfaceBase::stopJogCalcThread() { - if (jog_calcs_) - jog_calcs_->stopMainLoop(); - if (jog_calc_thread_) { if (jog_calc_thread_->joinable()) @@ -264,9 +261,6 @@ bool JogInterfaceBase::startCollisionCheckThread() bool JogInterfaceBase::stopCollisionCheckThread() { - if (collision_checker_) - collision_checker_->stopMainLoop(); - if (collision_check_thread_) { if (collision_check_thread_->joinable()) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp index 00d7fa2760..e2a228212e 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp @@ -167,6 +167,8 @@ JogROSInterface::JogROSInterface() main_rate.sleep(); } + // Stop JogArm threads + shared_variables_.stop_requested = true; stopJogCalcThread(); stopCollisionCheckThread(); } From d4ade526eb96c3a2a1c6394a7bc65282aa5762d3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 6 Apr 2020 10:50:12 +0200 Subject: [PATCH 252/612] Fix some clang tidy issues (#2004) Use default move constructor + assignment operators for MoveItCpp. --- moveit_core/collision_detection/src/world.cpp | 8 ++++---- .../test/pr2_arm_kinematics_plugin.cpp | 5 +---- .../planning_scene/src/planning_scene.cpp | 13 ++++++------ moveit_core/robot_state/src/conversions.cpp | 2 +- moveit_core/robot_state/src/robot_state.cpp | 4 ++-- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 11 ++-------- .../moveit_jog_arm/src/jog_ros_interface.cpp | 12 ++--------- .../cached_ik_kinematics_plugin.h | 2 +- .../src/ik_cache.cpp | 2 +- .../src/moveit_fake_controller_manager.cpp | 6 +++--- .../benchmarks/src/BenchmarkExecutor.cpp | 14 +++++++------ .../query_planners_service_capability.cpp | 6 +++--- .../mesh_filter/src/mesh_filter_base.cpp | 2 +- .../src/planning_pipeline.cpp | 2 +- .../include/moveit/moveit_cpp/moveit_cpp.h | 4 ++-- .../moveit/moveit_cpp/planning_component.h | 4 ++-- .../moveit_cpp/src/moveit_cpp.cpp | 19 ------------------ .../moveit_cpp/src/planning_component.cpp | 12 ----------- .../src/tools/moveit_config_data.cpp | 20 +++++++++---------- .../widgets/configuration_files_widget.cpp | 13 ++++++------ .../src/widgets/default_collisions_widget.cpp | 10 ++++------ .../src/widgets/end_effectors_widget.cpp | 11 +++++----- .../src/widgets/passive_joints_widget.cpp | 2 +- .../src/widgets/passive_joints_widget.h | 2 +- .../src/widgets/planning_groups_widget.cpp | 15 +++++++------- .../src/widgets/planning_groups_widget.h | 6 +++--- .../src/widgets/robot_poses_widget.cpp | 7 +++---- .../src/widgets/ros_controllers_widget.cpp | 12 +++++------ .../src/widgets/ros_controllers_widget.h | 4 ++-- .../src/widgets/virtual_joints_widget.cpp | 18 ++++++++--------- 30 files changed, 94 insertions(+), 154 deletions(-) diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index f2a404ec8f..8d270b7396 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -146,7 +146,7 @@ bool World::knowsTransform(const std::string& name) const return it->second->shape_poses_.size() == 1; else // Then objects' subframes { - for (const std::pair& object : objects_) + for (const std::pair& object : objects_) { // if "object name/" matches start of object_id, we found the matching object if (boost::starts_with(name, object.first) && name[object.first.length()] == '/') @@ -174,7 +174,7 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& fram return it->second->shape_poses_[0]; else // Search within subframes { - for (const std::pair& object : objects_) + for (const std::pair& object : objects_) { // if "object name/" matches start of object_id, we found the matching object if (boost::starts_with(name, object.first) && name[object.first.length()] == '/') @@ -312,8 +312,8 @@ void World::notifyAll(Action action) void World::notify(const ObjectConstPtr& obj, Action action) { - for (std::vector::const_iterator obs = observers_.begin(); obs != observers_.end(); ++obs) - (*obs)->callback_(obj, action); + for (Observer* observer : observers_) + observer->callback_(obj, action); } void World::notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index 6d55bbc2b8..097e8b5951 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -92,10 +92,7 @@ PR2ArmIKSolver::PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const st search_discretization_angle_ = search_discretization_angle; free_angle_ = free_angle; root_frame_name_ = root_frame_name; - if (!pr2_arm_ik_.init(robot_model, root_frame_name, tip_frame_name)) - active_ = false; - else - active_ = true; + active_ = pr2_arm_ik_.init(robot_model, root_frame_name, tip_frame_name); } void PR2ArmIKSolver::updateInternalDataStructures() diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index fa65396f97..28f70f32b4 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -423,13 +423,12 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene) // push colors and types for attached objects std::vector attached_objs; robot_state_->getAttachedBodies(attached_objs); - for (std::vector::const_iterator it = attached_objs.begin(); - it != attached_objs.end(); ++it) + for (const moveit::core::AttachedBody* attached_obj : attached_objs) { - if (hasObjectType((*it)->getName())) - scene->setObjectType((*it)->getName(), getObjectType((*it)->getName())); - if (hasObjectColor((*it)->getName())) - scene->setObjectColor((*it)->getName(), getObjectColor((*it)->getName())); + if (hasObjectType(attached_obj->getName())) + scene->setObjectType(attached_obj->getName(), getObjectType(attached_obj->getName())); + if (hasObjectColor(attached_obj->getName())) + scene->setObjectColor(attached_obj->getName(), getObjectColor(attached_obj->getName())); } } @@ -780,7 +779,7 @@ bool PlanningScene::getCollisionObjectMsg(moveit_msgs::CollisionObject& collisio if (hasObjectType(collision_obj.id)) collision_obj.type = getObjectType(collision_obj.id); } - for (auto frame_pair : obj->subframe_poses_) + for (const auto& frame_pair : obj->subframe_poses_) { collision_obj.subframe_names.push_back(frame_pair.first); geometry_msgs::Pose p; diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 9632e2d857..6bdcea49c3 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -218,7 +218,7 @@ static void _attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::A } aco.object.subframe_names.clear(); aco.object.subframe_poses.clear(); - for (auto frame_pair : attached_body.getSubframeTransforms()) + for (const auto& frame_pair : attached_body.getSubframeTransforms()) { aco.object.subframe_names.push_back(frame_pair.first); geometry_msgs::Pose pose; diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 0e454a3e90..11e2440759 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1050,7 +1050,7 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c } // Check if an AttachedBody has a subframe with name frame_id - for (const std::pair& body : attached_body_map_) + for (const std::pair& body : attached_body_map_) { const auto& transform = body.second->getGlobalSubframeTransform(frame_id, &frame_found); if (frame_found) @@ -1079,7 +1079,7 @@ bool RobotState::knowsFrameTransform(const std::string& frame_id) const return !it->second->getGlobalCollisionBodyTransforms().empty(); // Check if an AttachedBody has a subframe with name frame_id - for (const std::pair& body : attached_body_map_) + for (const std::pair& body : attached_body_map_) { if (body.second->hasSubframeTransform(frame_id)) return true; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp index bfa0ea9832..525232c2bf 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp @@ -102,15 +102,8 @@ void JogCppInterface::startMainLoop() trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; // Check if incoming commands are stale - if ((ros::Time::now() - shared_variables_.latest_nonzero_cmd_stamp) < - ros::Duration(ros_parameters_.incoming_command_timeout)) - { - shared_variables_.command_is_stale = false; - } - else - { - shared_variables_.command_is_stale = true; - } + shared_variables_.command_is_stale = (ros::Time::now() - shared_variables_.latest_nonzero_cmd_stamp) >= + ros::Duration(ros_parameters_.incoming_command_timeout); // Publish the most recent trajectory, unless the jogging calculation thread tells not to if (shared_variables_.ok_to_publish) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp index e2a228212e..9ead820ca2 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp @@ -121,16 +121,8 @@ JogROSInterface::JogROSInterface() trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; // Check for stale cmds - if ((ros::Time::now() - shared_variables_.latest_nonzero_cmd_stamp) < - ros::Duration(ros_parameters_.incoming_command_timeout)) - { - // Mark that incoming commands are not stale - shared_variables_.command_is_stale = false; - } - else - { - shared_variables_.command_is_stale = true; - } + shared_variables_.command_is_stale = ((ros::Time::now() - shared_variables_.latest_nonzero_cmd_stamp) >= + ros::Duration(ros_parameters_.incoming_command_timeout)); // Publish the most recent trajectory, unless the jogging calculation thread tells not to if (shared_variables_.ok_to_publish) diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h index 69d5a766a8..86687f3164 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h @@ -98,7 +98,7 @@ class IKCache const IKEntry& getBestApproximateIKSolution(const std::vector& poses) const; /** initialize cache, read from disk if found */ void initializeCache(const std::string& robot_id, const std::string& group_name, const std::string& cache_name, - const unsigned int num_joints, Options opts = Options()); + const unsigned int num_joints, const Options& opts = Options()); /** insert (pose,config) as an entry if it's different enough from the most similar cache entry diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp b/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp index 41c994bbf0..ff9c922934 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp +++ b/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp @@ -61,7 +61,7 @@ IKCache::~IKCache() } void IKCache::initializeCache(const std::string& robot_id, const std::string& group_name, const std::string& cache_name, - const unsigned int num_joints, Options opts) + const unsigned int num_joints, const Options& opts) { // read ROS parameters max_cache_size_ = opts.max_cache_size; diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp index 0e6f5cae64..d63a47d38f 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp @@ -195,10 +195,10 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont } // fill the joint state - for (JointPoseMap::const_iterator it = joints.begin(), end = joints.end(); it != end; ++it) + for (const auto& name_pos_pair : joints) { - js.name.push_back(it->first); - js.position.push_back(it->second); + js.name.push_back(name_pos_pair.first); + js.position.push_back(name_pos_pair.second); } return js; } diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 51d8cd07e5..3dd68f7b59 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -118,7 +118,7 @@ void BenchmarkExecutor::initialize(const std::vector& planning_pipe else { ROS_INFO("Available planning pipelines:"); - for (const std::pair& entry : planning_pipelines_) + for (const std::pair& entry : planning_pipelines_) ROS_INFO_STREAM("Pipeline: " << entry.first << ", Planner: " << entry.second->getPlannerPluginName()); } } @@ -245,7 +245,7 @@ bool BenchmarkExecutor::queriesAndPlannersCompatible( const std::vector& requests, const std::map>& /*planners*/) { // Make sure that the planner interfaces can service the desired queries - for (const std::pair& pipeline_entry : planning_pipelines_) + for (const std::pair& pipeline_entry : planning_pipelines_) { for (const BenchmarkRequest& request : requests) { @@ -499,10 +499,11 @@ bool BenchmarkExecutor::plannerConfigurationsExist( const std::map>& pipeline_configurations, const std::string& group_name) { // Make sure planner plugins exist - for (const std::pair>& pipeline_config_entry : pipeline_configurations) + for (const std::pair>& pipeline_config_entry : pipeline_configurations) { bool pipeline_exists = false; - for (const std::pair& pipeline_entry : planning_pipelines_) + for (const std::pair& pipeline_entry : + planning_pipelines_) { pipeline_exists = pipeline_entry.first == pipeline_config_entry.first; if (pipeline_exists) @@ -517,7 +518,7 @@ bool BenchmarkExecutor::plannerConfigurationsExist( } // Make sure planners exist within those pipelines - for (const std::pair>& entry : pipeline_configurations) + for (const std::pair>& entry : pipeline_configurations) { planning_interface::PlannerManagerPtr pm = planning_pipelines_[entry.first]->getPlannerManager(); const planning_interface::PlannerConfigurationMap& config_map = pm->getPlannerConfigurations(); @@ -531,7 +532,8 @@ bool BenchmarkExecutor::plannerConfigurationsExist( for (std::size_t i = 0; i < entry.second.size(); ++i) { bool planner_exists = false; - for (const std::pair& config_entry : config_map) + for (const std::pair& config_entry : + config_map) { std::string planner_name = group_name + "[" + entry.second[i] + "]"; planner_exists = (config_entry.second.group == group_name && config_entry.second.name == planner_name); diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp index 806b65d395..c2c92caf51 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp @@ -93,10 +93,10 @@ bool MoveGroupQueryPlannersService::getParams(moveit_msgs::GetPlannerParams::Req config.insert(it->second.config.begin(), it->second.config.end()); } - for (std::map::const_iterator it = config.begin(), end = config.end(); it != end; ++it) + for (const auto& key_value_pair : config) { - res.params.keys.push_back(it->first); - res.params.values.push_back(it->second); + res.params.keys.push_back(key_value_pair.first); + res.params.values.push_back(key_value_pair.second); } } return true; diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp index b8808d13c7..67a241c77c 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp @@ -324,7 +324,7 @@ void mesh_filter::MeshFilterBase::doFilter(const void* sensor_data, const int en glUniform3f(padding_coefficients_id, padding_coefficients[0], padding_coefficients[1], padding_coefficients[2]); Eigen::Isometry3d transform; - for (const std::pair& mesh : meshes_) + for (const std::pair& mesh : meshes_) if (transform_callback_(mesh.first, transform)) mesh.second->render(transform); diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 3188880f46..8f78cbf8b9 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -349,7 +349,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla bool stacked_constraints = false; if (req.path_constraints.position_constraints.size() > 1 || req.path_constraints.orientation_constraints.size() > 1) stacked_constraints = true; - for (auto constraint : req.goal_constraints) + for (const auto& constraint : req.goal_constraints) { if (constraint.position_constraints.size() > 1 || constraint.orientation_constraints.size() > 1) stacked_constraints = true; diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 72eb43cb2a..2f6e09caad 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -120,8 +120,8 @@ class MoveItCpp MoveItCpp(const MoveItCpp&) = delete; MoveItCpp& operator=(const MoveItCpp&) = delete; - MoveItCpp(MoveItCpp&& other); - MoveItCpp& operator=(MoveItCpp&& other); + MoveItCpp(MoveItCpp&& other) = default; + MoveItCpp& operator=(MoveItCpp&& other) = default; /** \brief Destructor */ ~MoveItCpp(); diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index cc650fbae4..c8bc15c6f3 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -126,8 +126,8 @@ class PlanningComponent PlanningComponent(const PlanningComponent&) = delete; PlanningComponent& operator=(const PlanningComponent&) = delete; - PlanningComponent(PlanningComponent&& other); - PlanningComponent& operator=(PlanningComponent&& other); + PlanningComponent(PlanningComponent&& other) = default; + PlanningComponent& operator=(PlanningComponent&& other) = delete; /** \brief Destructor */ ~PlanningComponent(); diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index 941aab5613..7f9b6700dc 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -97,31 +97,12 @@ MoveItCpp::MoveItCpp(const Options& options, const ros::NodeHandle& /*unused*/, ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp running"); } -MoveItCpp::MoveItCpp(MoveItCpp&& other) -{ - other.clearContents(); -} - MoveItCpp::~MoveItCpp() { ROS_INFO_NAMED(LOGNAME, "Deleting MoveItCpp"); clearContents(); } -MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) -{ - if (this != &other) - { - this->node_handle_ = other.node_handle_; - this->tf_buffer_ = other.tf_buffer_; - this->robot_model_ = other.robot_model_; - this->planning_scene_monitor_ = other.planning_scene_monitor_; - other.clearContents(); - } - - return *this; -} - bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options) { planning_scene_monitor_.reset( diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp index 68a60dc2b9..47f3ff56b3 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -94,18 +94,6 @@ PlanningComponent::~PlanningComponent() clearContents(); } -PlanningComponent& PlanningComponent::operator=(PlanningComponent&& other) -{ - if (this != &other) - { - this->considered_start_state_ = other.considered_start_state_; - this->workspace_parameters_ = other.workspace_parameters_; - this->last_plan_solution_ = other.last_plan_solution_; - other.clearContents(); - } - return *this; -} - const std::vector PlanningComponent::getNamedTargetStates() { if (joint_model_group_) diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index c791a31d8c..48678940ef 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -143,10 +143,9 @@ void MoveItConfigData::loadAllowedCollisionMatrix() allowed_collision_matrix_.clear(); // Update the allowed collision matrix, in case there has been a change - for (std::vector::const_iterator pair_it = srdf_->disabled_collisions_.begin(); - pair_it != srdf_->disabled_collisions_.end(); ++pair_it) + for (const auto& disabled_collision : srdf_->disabled_collisions_) { - allowed_collision_matrix_.setEntry(pair_it->link1_, pair_it->link2_, true); + allowed_collision_matrix_.setEntry(disabled_collision.link1_, disabled_collision.link2_, true); } } @@ -971,23 +970,22 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) // Writes Follow Joint Trajectory ROS controllers to ros_controller.yaml outputFollowJointTrajectoryYAML(emitter, ros_controllers_config_output); - for (std::vector::const_iterator controller_it = ros_controllers_config_output.begin(); - controller_it != ros_controllers_config_output.end(); ++controller_it) + for (const auto& controller : ros_controllers_config_output) { - emitter << YAML::Key << controller_it->name_; + emitter << YAML::Key << controller.name_; emitter << YAML::Value << YAML::BeginMap; emitter << YAML::Key << "type"; - emitter << YAML::Value << controller_it->type_; + emitter << YAML::Value << controller.type_; // Write joints emitter << YAML::Key << "joints"; { - if (controller_it->joints_.size() != 1) + if (controller.joints_.size() != 1) { emitter << YAML::Value << YAML::BeginSeq; // Iterate through the joints - for (const std::string& joint : controller_it->joints_) + for (const std::string& joint : controller.joints_) { emitter << joint; } @@ -996,7 +994,7 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) else { emitter << YAML::Value << YAML::BeginMap; - emitter << controller_it->joints_[0]; + emitter << controller.joints_[0]; emitter << YAML::EndMap; } } @@ -1005,7 +1003,7 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) emitter << YAML::Value << YAML::BeginMap; { // Iterate through the joints - for (const std::string& joint : controller_it->joints_) + for (const std::string& joint : controller.joints_) { emitter << YAML::Key << joint << YAML::Value << YAML::BeginMap; emitter << YAML::Key << "p"; diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 5356e9801b..f0e6b7e9d9 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -1022,24 +1022,23 @@ const std::string ConfigurationFilesWidget::getPackageName(std::string package_p bool ConfigurationFilesWidget::noGroupsEmpty() { // Loop through all groups - for (std::vector::const_iterator group_it = config_data_->srdf_->groups_.begin(); - group_it != config_data_->srdf_->groups_.end(); ++group_it) + for (const auto& group : config_data_->srdf_->groups_) { // Whenever 1 of the 4 component types are found, stop checking this group - if (!group_it->joints_.empty()) + if (!group.joints_.empty()) continue; - if (!group_it->links_.empty()) + if (!group.links_.empty()) continue; - if (!group_it->chains_.empty()) + if (!group.chains_.empty()) continue; - if (!group_it->subgroups_.empty()) + if (!group.subgroups_.empty()) continue; // This group has no contents, bad QMessageBox::warning( this, "Empty Group", QString("The planning group '") - .append(group_it->name_.c_str()) + .append(group.name_.c_str()) .append("' is empty and has no subcomponents associated with it (joints/links/chains/subgroups). You must " "edit or remove this planning group before this configuration package can be saved.")); return false; diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp index 33240ed4d6..c744036597 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp @@ -709,18 +709,16 @@ void DefaultCollisionsWidget::linkPairsFromSRDF() std::pair link_pair; // Loop through all disabled collisions in SRDF and update the comprehensive list that has already been created - for (std::vector::const_iterator collision_it = - config_data_->srdf_->disabled_collisions_.begin(); - collision_it != config_data_->srdf_->disabled_collisions_.end(); ++collision_it) + for (const auto& disabled_collision : config_data_->srdf_->disabled_collisions_) { // Set the link names - link_pair.first = collision_it->link1_; - link_pair.second = collision_it->link2_; + link_pair.first = disabled_collision.link1_; + link_pair.second = disabled_collision.link2_; if (link_pair.first >= link_pair.second) std::swap(link_pair.first, link_pair.second); // Set the link meta data - link_pair_data.reason = moveit_setup_assistant::disabledReasonFromString(collision_it->reason_); + link_pair_data.reason = moveit_setup_assistant::disabledReasonFromString(disabled_collision.reason_); link_pair_data.disable_check = true; // disable checking the collision btw the 2 links // Insert into map diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp index 1ea9a6774c..602469a32c 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp @@ -608,17 +608,16 @@ void EndEffectorsWidget::loadDataTable() // Loop through every end effector int row = 0; - for (std::vector::const_iterator data_it = config_data_->srdf_->end_effectors_.begin(); - data_it != config_data_->srdf_->end_effectors_.end(); ++data_it) + for (const auto& eef : config_data_->srdf_->end_effectors_) { // Create row elements - QTableWidgetItem* data_name = new QTableWidgetItem(data_it->name_.c_str()); + QTableWidgetItem* data_name = new QTableWidgetItem(eef.name_.c_str()); data_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* group_name = new QTableWidgetItem(data_it->component_group_.c_str()); + QTableWidgetItem* group_name = new QTableWidgetItem(eef.component_group_.c_str()); group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* parent_name = new QTableWidgetItem(data_it->parent_link_.c_str()); + QTableWidgetItem* parent_name = new QTableWidgetItem(eef.parent_link_.c_str()); group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* parent_group_name = new QTableWidgetItem(data_it->parent_group_.c_str()); + QTableWidgetItem* parent_group_name = new QTableWidgetItem(eef.parent_group_.c_str()); parent_group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); // Add to table diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp b/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp index 9048f45f10..f586963da9 100644 --- a/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp @@ -126,7 +126,7 @@ void PassiveJointsWidget::selectionUpdated() // ****************************************************************************************** // Called from Double List widget to highlight joints // ****************************************************************************************** -void PassiveJointsWidget::previewSelectedJoints(std::vector joints) +void PassiveJointsWidget::previewSelectedJoints(const std::vector& joints) { // Unhighlight all links Q_EMIT unhighlightAll(); diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.h b/moveit_setup_assistant/src/widgets/passive_joints_widget.h index 344b133d3a..3e6b94f439 100644 --- a/moveit_setup_assistant/src/widgets/passive_joints_widget.h +++ b/moveit_setup_assistant/src/widgets/passive_joints_widget.h @@ -86,7 +86,7 @@ private Q_SLOTS: void selectionUpdated(); /// Called from Double List widget to highlight joints - void previewSelectedJoints(std::vector joints); + void previewSelectedJoints(const std::vector& joints); private: // ****************************************************************************************** diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index 2fc01a69be..4379543ac9 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -1103,13 +1103,12 @@ bool PlanningGroupsWidget::saveGroupScreen() } // Check that the group name is unique - for (std::vector::const_iterator group_it = config_data_->srdf_->groups_.begin(); - group_it != config_data_->srdf_->groups_.end(); ++group_it) + for (const auto& group : config_data_->srdf_->groups_) { - if (group_it->name_.compare(group_name) == 0) // the names are the same + if (group.name_.compare(group_name) == 0) // the names are the same { // is this our existing group? check if group pointers are same - if (&(*group_it) != searched_group) + if (&group != searched_group) { QMessageBox::warning(this, "Error Saving", "A group already exists with that name!"); return false; @@ -1401,12 +1400,12 @@ void PlanningGroupsWidget::changeScreen(int index) // ****************************************************************************************** // Called from Double List widget to highlight a link // ****************************************************************************************** -void PlanningGroupsWidget::previewSelectedLink(std::vector links) +void PlanningGroupsWidget::previewSelectedLink(const std::vector& links) { // Unhighlight all links Q_EMIT unhighlightAll(); - for (std::string& link : links) + for (const std::string& link : links) { if (link.empty()) { @@ -1421,7 +1420,7 @@ void PlanningGroupsWidget::previewSelectedLink(std::vector links) // ****************************************************************************************** // Called from Double List widget to highlight joints // ****************************************************************************************** -void PlanningGroupsWidget::previewSelectedJoints(std::vector joints) +void PlanningGroupsWidget::previewSelectedJoints(const std::vector& joints) { // Unhighlight all links Q_EMIT unhighlightAll(); @@ -1452,7 +1451,7 @@ void PlanningGroupsWidget::previewSelectedJoints(std::vector joints // ****************************************************************************************** // Called from Double List widget to highlight a subgroup // ****************************************************************************************** -void PlanningGroupsWidget::previewSelectedSubgroup(std::vector groups) +void PlanningGroupsWidget::previewSelectedSubgroup(const std::vector& groups) { // Unhighlight all links Q_EMIT unhighlightAll(); diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.h b/moveit_setup_assistant/src/widgets/planning_groups_widget.h index 0d40c6cc9e..4c394c23ef 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.h +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.h @@ -128,14 +128,14 @@ private Q_SLOTS: void alterTree(const QString& link); /// Called from Double List widget to highlight a link - void previewSelectedLink(std::vector links); + void previewSelectedLink(const std::vector& links); /// Called from Double List widget to highlight a joint // void previewClickedJoint( std::string name ); - void previewSelectedJoints(std::vector joints); + void previewSelectedJoints(const std::vector& joints); /// Called from Double List widget to highlight a subgroup - void previewSelectedSubgroup(std::vector groups); + void previewSelectedSubgroup(const std::vector& groups); private: // ****************************************************************************************** diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index 4f70d8904c..e68b132194 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -739,13 +739,12 @@ void RobotPosesWidget::loadDataTable() // Loop through every pose int row = 0; - for (std::vector::const_iterator data_it = config_data_->srdf_->group_states_.begin(); - data_it != config_data_->srdf_->group_states_.end(); ++data_it) + for (const auto& group_state : config_data_->srdf_->group_states_) { // Create row elements - QTableWidgetItem* data_name = new QTableWidgetItem(data_it->name_.c_str()); + QTableWidgetItem* data_name = new QTableWidgetItem(group_state.name_.c_str()); data_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* group_name = new QTableWidgetItem(data_it->group_.c_str()); + QTableWidgetItem* group_name = new QTableWidgetItem(group_state.group_.c_str()); group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); // Add to table diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index aa13f54ab4..ff61771a14 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -466,7 +466,7 @@ void ROSControllersWidget::cancelEditing() // ****************************************************************************************** // Called from Double List widget to highlight joints // ****************************************************************************************** -void ROSControllersWidget::previewSelectedJoints(std::vector joints) +void ROSControllersWidget::previewSelectedJoints(const std::vector& joints) { // Unhighlight all links Q_EMIT unhighlightAll(); @@ -497,7 +497,7 @@ void ROSControllersWidget::previewSelectedJoints(std::vector joints // ****************************************************************************************** // Called from Double List widget to highlight a group // ****************************************************************************************** -void ROSControllersWidget::previewSelectedGroup(std::vector groups) +void ROSControllersWidget::previewSelectedGroup(const std::vector& groups) { // Unhighlight all links Q_EMIT unhighlightAll(); @@ -671,14 +671,12 @@ bool ROSControllersWidget::saveControllerScreen() } // Check that the controller name is unique - for (std::vector::const_iterator controller_it = - config_data_->getROSControllers().begin(); - controller_it != config_data_->getROSControllers().end(); ++controller_it) + for (const auto& controller : config_data_->getROSControllers()) { - if (controller_it->name_.compare(controller_name) == 0) // the names are the same + if (controller.name_.compare(controller_name) == 0) // the names are the same { // is this our existing controller? check if controller pointers are same - if (&(*controller_it) != searched_controller) + if (&controller != searched_controller) { QMessageBox::warning(this, "Error Saving", "A controller already exists with that name!"); return false; diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h index c1c05e609b..c78e291ceb 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h @@ -107,10 +107,10 @@ private Q_SLOTS: void editSelected(); /// Called from Double List widget to highlight a joint - void previewSelectedJoints(std::vector joints); + void previewSelectedJoints(const std::vector& joints); /// Called from Double List widget to highlight a group - void previewSelectedGroup(std::vector groups); + void previewSelectedGroup(const std::vector& groups); /// Called when an item is seleceted from the controllers tree void previewSelected(QTreeWidgetItem* selected_item, int column); diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp index a988648038..7203ee29fd 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp @@ -472,13 +472,12 @@ void VirtualJointsWidget::doneEditing() } // Check that the vjoint name is unique - for (std::vector::const_iterator data_it = config_data_->srdf_->virtual_joints_.begin(); - data_it != config_data_->srdf_->virtual_joints_.end(); ++data_it) + for (const auto& virtual_joint : config_data_->srdf_->virtual_joints_) { - if (data_it->name_.compare(vjoint_name) == 0) // the names are the same + if (virtual_joint.name_.compare(vjoint_name) == 0) // the names are the same { // is this our existing vjoint? check if vjoint pointers are same - if (&(*data_it) != searched_data) + if (&virtual_joint != searched_data) { QMessageBox::warning(this, "Error Saving", "A virtual joint already exists with that name!"); return; @@ -574,17 +573,16 @@ void VirtualJointsWidget::loadDataTable() // Loop through every virtual joint int row = 0; - for (std::vector::const_iterator data_it = config_data_->srdf_->virtual_joints_.begin(); - data_it != config_data_->srdf_->virtual_joints_.end(); ++data_it) + for (const auto& virtual_joint : config_data_->srdf_->virtual_joints_) { // Create row elements - QTableWidgetItem* data_name = new QTableWidgetItem(data_it->name_.c_str()); + QTableWidgetItem* data_name = new QTableWidgetItem(virtual_joint.name_.c_str()); data_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* child_link_name = new QTableWidgetItem(data_it->child_link_.c_str()); + QTableWidgetItem* child_link_name = new QTableWidgetItem(virtual_joint.child_link_.c_str()); child_link_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* parent_frame_name = new QTableWidgetItem(data_it->parent_frame_.c_str()); + QTableWidgetItem* parent_frame_name = new QTableWidgetItem(virtual_joint.parent_frame_.c_str()); parent_frame_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* type_name = new QTableWidgetItem(data_it->type_.c_str()); + QTableWidgetItem* type_name = new QTableWidgetItem(virtual_joint.type_.c_str()); type_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); // Add to table From 488055efd50d6c49f057719d80d34754861f234d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 6 Apr 2020 10:52:06 +0200 Subject: [PATCH 253/612] Travis: re-enable ros-shadow-fixed (#2003) --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 2fb7ce897f..f7cccd3d92 100644 --- a/.travis.yml +++ b/.travis.yml @@ -32,7 +32,7 @@ jobs: env: TEST=clang-tidy-fix BEFORE_DOCKER_SCRIPT="source moveit_kinematics/test/test_ikfast_plugins.sh" CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-overloaded-virtual" -# ROS_REPO=ros-shadow-fixed + ROS_REPO=ros-shadow-fixed before_script: - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci From d989220fb9256c9dbebe11d962ad4cd38ba8d0c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Mon, 6 Apr 2020 14:42:30 +0200 Subject: [PATCH 254/612] run shadow-fixed for main branches (#2012) --- .travis.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index f7cccd3d92..61cf02f9f9 100644 --- a/.travis.yml +++ b/.travis.yml @@ -32,7 +32,8 @@ jobs: env: TEST=clang-tidy-fix BEFORE_DOCKER_SCRIPT="source moveit_kinematics/test/test_ikfast_plugins.sh" CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-overloaded-virtual" - ROS_REPO=ros-shadow-fixed + - if: branch =~ /^(.*-devel|master)$/ + env: ROS_REPO=ros-shadow-fixed before_script: - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci From 18715e1c21e36f7df6dfae9ac14fe8577381e016 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 8 Apr 2020 10:22:03 -0500 Subject: [PATCH 255/612] [jog_arm] Fix valid command flags (#2013) * Rename the 'zero command flag' variables for readability * Reset flags when incoming commands timeout * Remove debug line, clang format --- .../include/moveit_jog_arm/jog_arm_data.h | 8 +++---- .../moveit_jog_arm/src/jog_calcs.cpp | 23 +++++++++++-------- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 20 ++++++++-------- .../moveit_jog_arm/src/jog_ros_interface.cpp | 20 ++++++++-------- 4 files changed, 38 insertions(+), 33 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 62f1604aa2..25b3fef816 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -68,11 +68,11 @@ struct JogArmShared : public std::mutex double collision_velocity_scale = 1; - // Indicates that an incoming Cartesian command is all zero velocities - bool zero_cartesian_cmd_flag = true; + // Flag a valid incoming Cartesian command having nonzero velocities + bool have_nonzero_cartesian_cmd = false; - // Indicates that an incoming joint angle command is all zero velocities - bool zero_joint_cmd_flag = true; + // Flag a valid incoming joint angle command having nonzero velocities + bool have_nonzero_joint_cmd = false; // Indicates that we have not received a new command in some time bool command_is_stale = false; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 2e29282d9b..61ca19dc40 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -161,12 +161,12 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) { // Flag that incoming commands are all zero. May be used to skip calculations/publication shared_variables.lock(); - bool zero_cartesian_cmd_flag = shared_variables.zero_cartesian_cmd_flag; - bool zero_joint_cmd_flag = shared_variables.zero_joint_cmd_flag; + bool have_nonzero_cartesian_cmd = shared_variables.have_nonzero_cartesian_cmd; + bool have_nonzero_joint_cmd = shared_variables.have_nonzero_joint_cmd; shared_variables.unlock(); // Prioritize cartesian jogging above joint jogging - if (!zero_cartesian_cmd_flag) + if (have_nonzero_cartesian_cmd) { shared_variables.lock(); cartesian_deltas = shared_variables.command_deltas; @@ -175,7 +175,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) if (!cartesianJogCalcs(cartesian_deltas, shared_variables)) continue; } - else if (!zero_joint_cmd_flag) + else if (have_nonzero_joint_cmd) { shared_variables.lock(); joint_deltas = shared_variables.joint_command_deltas; @@ -194,14 +194,19 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) bool stale_command = shared_variables.command_is_stale; shared_variables.unlock(); - if (stale_command || (zero_cartesian_cmd_flag && zero_joint_cmd_flag)) + if (stale_command || (!have_nonzero_cartesian_cmd && !have_nonzero_joint_cmd)) { suddenHalt(outgoing_command_); - zero_cartesian_cmd_flag = true; - zero_joint_cmd_flag = true; + have_nonzero_cartesian_cmd = false; + have_nonzero_joint_cmd = false; + // Reset the valid command flag so jogging stops until a new command arrives + shared_variables.lock(); + shared_variables.have_nonzero_cartesian_cmd = false; + shared_variables.have_nonzero_joint_cmd = false; + shared_variables.unlock(); } - bool valid_nonzero_command = !zero_cartesian_cmd_flag || !zero_joint_cmd_flag; + bool valid_nonzero_command = have_nonzero_cartesian_cmd || have_nonzero_joint_cmd; // Send the newest target joints shared_variables.lock(); @@ -228,7 +233,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) // Store last zero-velocity message flag to prevent superfluous warnings. // Cartesian and joint commands must both be zero. - if (zero_cartesian_cmd_flag && zero_joint_cmd_flag) + if (!have_nonzero_cartesian_cmd && !have_nonzero_joint_cmd) { // Avoid overflow if (zero_velocity_count < std::numeric_limits::max()) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp index 525232c2bf..47e23f5eb0 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp @@ -169,14 +169,14 @@ void JogCppInterface::provideTwistStampedCommand(const geometry_msgs::TwistStamp } // Check if input is all zeros. Flag it if so to skip calculations/publication after num_outgoing_halt_msgs_to_publish - shared_variables_.zero_cartesian_cmd_flag = shared_variables_.command_deltas.twist.linear.x == 0.0 && - shared_variables_.command_deltas.twist.linear.y == 0.0 && - shared_variables_.command_deltas.twist.linear.z == 0.0 && - shared_variables_.command_deltas.twist.angular.x == 0.0 && - shared_variables_.command_deltas.twist.angular.y == 0.0 && - shared_variables_.command_deltas.twist.angular.z == 0.0; - - if (!shared_variables_.zero_cartesian_cmd_flag) + shared_variables_.have_nonzero_cartesian_cmd = shared_variables_.command_deltas.twist.linear.x != 0.0 || + shared_variables_.command_deltas.twist.linear.y != 0.0 || + shared_variables_.command_deltas.twist.linear.z != 0.0 || + shared_variables_.command_deltas.twist.angular.x != 0.0 || + shared_variables_.command_deltas.twist.angular.y != 0.0 || + shared_variables_.command_deltas.twist.angular.z != 0.0; + + if (shared_variables_.have_nonzero_cartesian_cmd) { shared_variables_.latest_nonzero_cmd_stamp = velocity_command.header.stamp; } @@ -194,9 +194,9 @@ void JogCppInterface::provideJointCommand(const control_msgs::JointJog& joint_co { all_zeros &= (delta == 0.0); }; - shared_variables_.zero_joint_cmd_flag = all_zeros; + shared_variables_.have_nonzero_joint_cmd = !all_zeros; - if (!shared_variables_.zero_joint_cmd_flag) + if (shared_variables_.have_nonzero_joint_cmd) { shared_variables_.latest_nonzero_cmd_stamp = joint_command.header.stamp; } diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp index 9ead820ca2..d5ab9921ba 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp @@ -182,14 +182,14 @@ void JogROSInterface::deltaCartesianCmdCB(const geometry_msgs::TwistStampedConst } // Check if input is all zeros. Flag it if so to skip calculations/publication after num_outgoing_halt_msgs_to_publish - shared_variables_.zero_cartesian_cmd_flag = shared_variables_.command_deltas.twist.linear.x == 0.0 && - shared_variables_.command_deltas.twist.linear.y == 0.0 && - shared_variables_.command_deltas.twist.linear.z == 0.0 && - shared_variables_.command_deltas.twist.angular.x == 0.0 && - shared_variables_.command_deltas.twist.angular.y == 0.0 && - shared_variables_.command_deltas.twist.angular.z == 0.0; - - if (!shared_variables_.zero_cartesian_cmd_flag) + shared_variables_.have_nonzero_cartesian_cmd = shared_variables_.command_deltas.twist.linear.x != 0.0 || + shared_variables_.command_deltas.twist.linear.y != 0.0 || + shared_variables_.command_deltas.twist.linear.z != 0.0 || + shared_variables_.command_deltas.twist.angular.x != 0.0 || + shared_variables_.command_deltas.twist.angular.y != 0.0 || + shared_variables_.command_deltas.twist.angular.z != 0.0; + + if (shared_variables_.have_nonzero_cartesian_cmd) { shared_variables_.latest_nonzero_cmd_stamp = msg->header.stamp; } @@ -208,9 +208,9 @@ void JogROSInterface::deltaJointCmdCB(const control_msgs::JointJogConstPtr& msg) { all_zeros &= (delta == 0.0); }; - shared_variables_.zero_joint_cmd_flag = all_zeros; + shared_variables_.have_nonzero_joint_cmd = !all_zeros; - if (!shared_variables_.zero_joint_cmd_flag) + if (shared_variables_.have_nonzero_joint_cmd) { shared_variables_.latest_nonzero_cmd_stamp = msg->header.stamp; } From 9681f5064178a189e563055ee6d2cd72ac69a067 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 8 Apr 2020 10:07:33 -0600 Subject: [PATCH 256/612] add tests for move_group interface (#1995) * add tests for move_group tutorials * easier to reach joint positions for less flaky test * allow more time for planning * remove move to start pose * reduce epsilon to 1e-5 --- moveit_ros/planning_interface/package.xml | 1 + .../planning_interface/test/CMakeLists.txt | 3 + .../test/move_group_interface_cpp_test.cpp | 363 ++++++++++++++++++ .../test/move_group_interface_cpp_test.test | 29 ++ 4 files changed, 396 insertions(+) create mode 100644 moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp create mode 100644 moveit_ros/planning_interface/test/move_group_interface_cpp_test.test diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index e6cf739497..5b9a0eef07 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -40,4 +40,5 @@ moveit_resources eigen_conversions rostest + eigen_conversions diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 3bb83daaa3..7eb6543b1a 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -6,6 +6,9 @@ if (CATKIN_ENABLE_TESTING) add_executable(test_cleanup test_cleanup.cpp) target_link_libraries(test_cleanup moveit_move_group_interface) + add_rostest_gtest(move_group_interface_cpp_test move_group_interface_cpp_test.test move_group_interface_cpp_test.cpp) + target_link_libraries(move_group_interface_cpp_test moveit_move_group_interface ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) + add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp new file mode 100644 index 0000000000..7298f03d1a --- /dev/null +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -0,0 +1,363 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2020, Tyler Weaver +* 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 PickNik Robotics 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. +*********************************************************************/ + +/* Author: Tyler Weaver */ + +/* These integration tests are based on the tutorials for using move_group: + * https://ros-planning.github.io/moveit_tutorials/doc/move_group_interface/move_group_interface_tutorial.html + */ + +// C++ +#include +#include +#include + +// ROS +#include + +// The Testing Framework and Utils +#include + +// MoveIt +#include +#include + +// TF2 +#include +#include + +// 10um acuracy tested for position and orientation +constexpr double EPSILON = 1e-5; + +static const std::string PLANNING_GROUP = "panda_arm"; +constexpr double PLANNING_TIME_S = 30.0; +constexpr double MAX_VELOCITY_SCALE = 1.0; +constexpr double MAX_ACCELERATION_SCALE = 1.0; +constexpr double GOAL_TOLERANCE = 1e-6; + +class MoveGroupTestFixture : public ::testing::Test +{ +public: + void SetUp() override + { + nh_ = ros::NodeHandle("/move_group_interface_cpp_test"); + move_group_ = std::make_shared(PLANNING_GROUP); + + // set velocity and acceleration scaling factors (full speed) + move_group_->setMaxVelocityScalingFactor(MAX_VELOCITY_SCALE); + move_group_->setMaxAccelerationScalingFactor(MAX_ACCELERATION_SCALE); + + // allow more time for planning + move_group_->setPlanningTime(PLANNING_TIME_S); + + // set the tolerance for the goals to be smaller than epsilon + move_group_->setGoalTolerance(GOAL_TOLERANCE); + } + + void planAndMoveToPose(const geometry_msgs::Pose& pose) + { + SCOPED_TRACE("planAndMoveToPose"); + ASSERT_TRUE(move_group_->setJointValueTarget(pose)); + planAndMove(); + } + + void planAndMove() + { + SCOPED_TRACE("planAndMove"); + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + ASSERT_EQ(move_group_->plan(my_plan), moveit::planning_interface::MoveItErrorCode::SUCCESS); + ASSERT_EQ(move_group_->move(), moveit::planning_interface::MoveItErrorCode::SUCCESS); + } + + void testEigenPose(const Eigen::Isometry3d& expected, const Eigen::Isometry3d& actual) + { + SCOPED_TRACE("testEigenPose"); + std::stringstream ss; + ss << "expected: \n" << expected.matrix() << "\nactual: \n" << actual.matrix(); + EXPECT_TRUE(actual.isApprox(expected, EPSILON)) << ss.str(); + } + + void testPose(const Eigen::Isometry3d& expected_pose) + { + SCOPED_TRACE("testPose(const Eigen::Isometry3d&)"); + // get the pose of the end effector link after the movement + geometry_msgs::PoseStamped actual_pose_stamped = move_group_->getCurrentPose(); + Eigen::Isometry3d actual_pose; + tf::poseMsgToEigen(actual_pose_stamped.pose, actual_pose); + + // compare to planned pose + testEigenPose(expected_pose, actual_pose); + } + + void testPose(const geometry_msgs::Pose& expected_pose_msg) + { + SCOPED_TRACE("testPose(const geometry_msgs::Pose&)"); + Eigen::Isometry3d expected_pose; + tf::poseMsgToEigen(expected_pose_msg, expected_pose); + testPose(expected_pose); + } + + void testJointPositions(const std::vector& expected) + { + SCOPED_TRACE("testJointPositions"); + const robot_state::JointModelGroup* joint_model_group = + move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP); + std::vector actual; + move_group_->getCurrentState()->copyJointGroupPositions(joint_model_group, actual); + ASSERT_EQ(expected.size(), actual.size()); + for (size_t i = 0; i < actual.size(); ++i) + { + double delta = std::abs(expected[i] - actual[i]); + EXPECT_LT(delta, EPSILON) << "joint index: " << i << ", plan: " << expected[i] << ", result: " << actual[i]; + } + } + +protected: + ros::NodeHandle nh_; + moveit::planning_interface::MoveGroupInterfacePtr move_group_; + moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; +}; + +TEST_F(MoveGroupTestFixture, MoveToPoseTest) +{ + SCOPED_TRACE("MoveToPoseTest"); + + // Test setting target pose with eigen and with geometry_msgs + geometry_msgs::Pose target_pose; + target_pose.orientation.w = 1.0; + target_pose.position.x = 0.28; + target_pose.position.y = -0.2; + target_pose.position.z = 0.5; + + // convert to eigen + Eigen::Isometry3d eigen_target_pose; + tf::poseMsgToEigen(target_pose, eigen_target_pose); + + // set with eigen, get ros message representation + move_group_->setPoseTarget(eigen_target_pose); + geometry_msgs::PoseStamped set_target_pose = move_group_->getPoseTarget(); + Eigen::Isometry3d eigen_set_target_pose; + tf::poseMsgToEigen(set_target_pose.pose, eigen_set_target_pose); + + // expect that they are identical + testEigenPose(eigen_target_pose, eigen_set_target_pose); + + // plan and move + planAndMove(); + + // get the pose after the movement + testPose(eigen_target_pose); +} + +TEST_F(MoveGroupTestFixture, JointSpaceGoalTest) +{ + SCOPED_TRACE("JointSpaceGoalTest"); + + // Next get the current set of joint values for the group. + std::vector plan_joint_positions; + move_group_->getCurrentState()->copyJointGroupPositions( + move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP), plan_joint_positions); + + // Now, let's modify the joint positions. (radians) + ASSERT_EQ(plan_joint_positions.size(), std::size_t(7)); + plan_joint_positions = { 1.2, -1.0, -0.1, -2.4, 0.0, 1.5, 0.6 }; + move_group_->setJointValueTarget(plan_joint_positions); + + // plan and move + planAndMove(); + + // test that we moved to the expected joint positions + testJointPositions(plan_joint_positions); +} + +TEST_F(MoveGroupTestFixture, PathConstraintTest) +{ + SCOPED_TRACE("PathConstraintTest"); + + // set a custom start state + geometry_msgs::Pose start_pose; + start_pose.orientation.w = 1.0; + start_pose.position.x = 0.55; + start_pose.position.y = -0.05; + start_pose.position.z = 0.8; + planAndMoveToPose(start_pose); + + // create an orientation constraint + moveit_msgs::OrientationConstraint ocm; + ocm.link_name = move_group_->getEndEffectorLink(); + ocm.header.frame_id = move_group_->getPlanningFrame(); + ocm.orientation.w = 1.0; + ocm.absolute_x_axis_tolerance = 0.1; + ocm.absolute_y_axis_tolerance = 0.1; + ocm.absolute_z_axis_tolerance = 0.1; + ocm.weight = 1.0; + moveit_msgs::Constraints test_constraints; + test_constraints.orientation_constraints.push_back(ocm); + move_group_->setPathConstraints(test_constraints); + + // move to a custom target pose + geometry_msgs::Pose target_pose; + target_pose.orientation.w = 1.0; + target_pose.position.x = 0.28; + target_pose.position.y = -0.2; + target_pose.position.z = 0.5; + planAndMoveToPose(target_pose); + + // clear path constraints + move_group_->clearPathConstraints(); + + // get the pose after the movement + testPose(target_pose); +} + +TEST_F(MoveGroupTestFixture, CartPathTest) +{ + SCOPED_TRACE("CartPathTest"); + + // set a custom start state + geometry_msgs::Pose start_pose; + start_pose.orientation.w = 1.0; + start_pose.position.x = 0.55; + start_pose.position.y = -0.05; + start_pose.position.z = 0.8; + planAndMoveToPose(start_pose); + + std::vector waypoints; + waypoints.push_back(start_pose); + + geometry_msgs::Pose target_waypoint = start_pose; + target_waypoint.position.z -= 0.2; + waypoints.push_back(target_waypoint); // down + + target_waypoint.position.y -= 0.2; + waypoints.push_back(target_waypoint); // right + + target_waypoint.position.z += 0.2; + target_waypoint.position.y += 0.2; + target_waypoint.position.x -= 0.2; + waypoints.push_back(target_waypoint); // up and left + + moveit_msgs::RobotTrajectory trajectory; + const double jump_threshold = 0.0; + const double eef_step = 0.01; + move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); + + // Execute trajectory + EXPECT_EQ(move_group_->execute(trajectory), moveit::planning_interface::MoveItErrorCode::SUCCESS); + + // get the pose after the movement + testPose(target_waypoint); +} + +TEST_F(MoveGroupTestFixture, CollisionObjectsTest) +{ + SCOPED_TRACE("CollisionObjectsTest"); + + // set a custom start state + geometry_msgs::Pose start_pose; + start_pose.orientation.w = 1.0; + start_pose.position.x = 0.28; + start_pose.position.y = -0.2; + start_pose.position.z = 0.5; + planAndMoveToPose(start_pose); + + // Define a collision object ROS message. + moveit_msgs::CollisionObject collision_object; + collision_object.header.frame_id = move_group_->getPlanningFrame(); + + // The id of the object is used to identify it. + collision_object.id = "box1"; + + // Define a box to add to the world. + shape_msgs::SolidPrimitive primitive; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[0] = 0.4; + primitive.dimensions[1] = 0.1; + primitive.dimensions[2] = 0.1; + + // Define a pose for the box (specified relative to frame_id) + geometry_msgs::Pose box_pose; + box_pose.orientation.w = 1.0; + box_pose.position.x = 0.4; + box_pose.position.y = -0.2; + box_pose.position.z = 0.8; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + collision_object.operation = collision_object.ADD; + + std::vector collision_objects; + collision_objects.push_back(collision_object); + + // Now, let's add the collision object into the world + planning_scene_interface_.addCollisionObjects(collision_objects); + + // plan trajectory avoiding object + geometry_msgs::Pose target_pose; + target_pose.orientation.w = 0.0; + target_pose.position.x = 0.4; + target_pose.position.y = -0.4; + target_pose.position.z = 0.7; + planAndMoveToPose(target_pose); + + // get the pose after the movement + testPose(target_pose); + + // attach and detach collision object + EXPECT_TRUE(move_group_->attachObject(collision_object.id)); + EXPECT_EQ(planning_scene_interface_.getAttachedObjects().size(), std::size_t(1)); + EXPECT_TRUE(move_group_->detachObject(collision_object.id)); + EXPECT_EQ(planning_scene_interface_.getAttachedObjects().size(), std::size_t(0)); + + // remove object from world + std::vector object_ids; + object_ids.push_back(collision_object.id); + EXPECT_EQ(planning_scene_interface_.getObjects().size(), std::size_t(1)); + planning_scene_interface_.removeCollisionObjects(object_ids); + EXPECT_EQ(planning_scene_interface_.getObjects().size(), std::size_t(0)); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "move_group_interface_cpp_test"); + testing::InitGoogleTest(&argc, argv); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test new file mode 100644 index 0000000000..af9d7de7c7 --- /dev/null +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test @@ -0,0 +1,29 @@ + + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + From 47d3d616bc4f416c55958a57b4c04fbaeb11f4a0 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 10 Apr 2020 11:30:10 -0600 Subject: [PATCH 257/612] add use_rviz to demo.launch in setup_assistant (#2019) --- .../templates/moveit_config_pkg_template/launch/demo.launch | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch index 7cb5bebbf7..d7512b437a 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch @@ -20,6 +20,7 @@ This corresponds to moving around the real robot without the use of MoveIt. --> + @@ -48,7 +49,7 @@ - + From 3be547d1c3a4e2f475b5fb019bae43c05cda07c4 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Mon, 13 Apr 2020 09:46:27 -0600 Subject: [PATCH 258/612] [jog_arm] Split collision proximity threshold (#2008) * separate proximity threshold values for self-collisions and scene collisions * increase default value of scene collision proximity threshold * deprecate old parameters --- .../src/collision_common.cpp | 19 ++++---- .../config/ur_simulated_config.yaml | 3 +- .../include/moveit_jog_arm/jog_arm_data.h | 3 +- .../src/collision_check_thread.cpp | 45 ++++++++++++++----- .../moveit_jog_arm/src/jog_interface_base.cpp | 39 ++++++++++++++-- 5 files changed, 82 insertions(+), 27 deletions(-) diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index b90d7f0c53..862104d756 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -465,19 +465,16 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void cd1->getID().c_str(), cd2->getID().c_str()); } } - else + else if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED) { - if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED) + const std::set& tl = cd1->ptr.ab->getTouchLinks(); + if (tl.find(cd2->getID()) != tl.end()) { - const std::set& tl = cd1->ptr.ab->getTouchLinks(); - if (tl.find(cd2->getID()) != tl.end()) - { - always_allow_collision = true; - if (cdata->req->verbose) - ROS_DEBUG_NAMED("collision_detection.fcl", - "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.", - cd2->getID().c_str(), cd1->getID().c_str()); - } + always_allow_collision = true; + if (cdata->req->verbose) + ROS_DEBUG_NAMED("collision_detection.fcl", + "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.", + cd2->getID().c_str(), cd1->getID().c_str()); } } diff --git a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml index f978ea5817..8a42b955dd 100644 --- a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml +++ b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml @@ -53,4 +53,5 @@ command_out_topic: joint_group_position_controller/command # Publish outgoing co ## Collision checking for the entire robot body check_collisions: true # Check collisions? collision_check_rate: 10 # [Hz] Collision-checking can easily bog down a CPU if done too often. -collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m] +self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 25b3fef816..ec2f69794e 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -121,7 +121,8 @@ struct JogArmParameters double joint_scale; double lower_singularity_threshold; double hard_stop_singularity_threshold; - double collision_proximity_threshold; + double scene_collision_proximity_threshold; + double self_collision_proximity_threshold; double low_pass_filter_coeff; double publish_period; double incoming_command_timeout; diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 0657f13229..e2425be5d6 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -70,14 +70,20 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) // Copy the planning scene's version of current state into new memory moveit::core::RobotState current_state(getLockedPlanningSceneRO()->getCurrentState()); - double velocity_scale_coefficient = -log(0.001) / parameters_.collision_proximity_threshold; + const double self_velocity_scale_coefficient = -log(0.001) / parameters_.self_collision_proximity_threshold; + const double scene_velocity_scale_coefficient = -log(0.001) / parameters_.scene_collision_proximity_threshold; ros::Rate collision_rate(parameters_.collision_check_rate); + double self_collision_distance = 0; + double scene_collision_distance = 0; + bool collision_detected; + // Scale robot velocity according to collision proximity and user-defined thresholds. // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. // Proximity decreasing --> decelerate double velocity_scale = 1; + collision_detection::AllowedCollisionMatrix acm = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); ///////////////////////////////////////////////// // Spin while checking collisions ///////////////////////////////////////////////// @@ -92,29 +98,48 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) for (std::size_t i = 0; i < jts.position.size(); ++i) current_state.setJointPositions(jts.name[i], &jts.position[i]); - collision_result.clear(); current_state.updateCollisionBodyTransforms(); + collision_detected = false; // Do a thread-safe distance-based collision detection - getLockedPlanningSceneRO()->checkCollision(collision_request, collision_result, current_state); + collision_result.clear(); + getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request, collision_result, + current_state); + scene_collision_distance = collision_result.distance; + collision_detected |= collision_result.collision; + + collision_result.clear(); + getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision(collision_request, collision_result, + current_state, acm); + self_collision_distance = collision_result.distance; + collision_detected |= collision_result.collision; + velocity_scale = 1; // If we're definitely in collision, stop immediately - if (collision_result.collision) + if (collision_detected) { velocity_scale = 0; } // If we are far from a collision, velocity_scale should be 1. // If we are very close to a collision, velocity_scale should be ~zero. - // When collision_proximity_threshold is breached, start decelerating exponentially. - else if (collision_result.distance < parameters_.collision_proximity_threshold) + // When scene_collision_proximity_threshold is breached, start decelerating exponentially. + if (scene_collision_distance < parameters_.scene_collision_proximity_threshold) { - // velocity_scale = e ^ k * (collision_result.distance - threshold) + // velocity_scale = e ^ k * (collision_distance - threshold) // k = - ln(0.001) / collision_proximity_threshold - // velocity_scale should equal one when collision_result.distance is at collision_proximity_threshold. - // velocity_scale should equal 0.001 when collision_result.distance is at zero. + // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. + // velocity_scale should equal 0.001 when collision_distance is at zero. + velocity_scale = + std::min(velocity_scale, exp(scene_velocity_scale_coefficient * + (scene_collision_distance - parameters_.scene_collision_proximity_threshold))); + } + + if (self_collision_distance < parameters_.self_collision_proximity_threshold) + { velocity_scale = - exp(velocity_scale_coefficient * (collision_result.distance - parameters_.collision_proximity_threshold)); + std::min(velocity_scale, exp(self_velocity_scale_coefficient * + (self_collision_distance - parameters_.self_collision_proximity_threshold))); } shared_variables.lock(); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index d07b10981a..9b318d7b98 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -88,8 +88,28 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) ros_parameters_.lower_singularity_threshold); error += !rosparam_shortcuts::get("", n, parameter_ns + "/hard_stop_singularity_threshold", ros_parameters_.hard_stop_singularity_threshold); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/collision_proximity_threshold", - ros_parameters_.collision_proximity_threshold); + // parameter was removed, replaced with separate self- and scene-collision proximity thresholds; if old parameter + // exists, print warning and use old parameter for both new parameter values + // TODO(JStech): remove this deprecation warning in ROS Noetic + if (!rosparam_shortcuts::get("", n, parameter_ns + "/self_collision_proximity_threshold", + ros_parameters_.self_collision_proximity_threshold) && + !rosparam_shortcuts::get("", n, parameter_ns + "/scene_collision_proximity_threshold", + ros_parameters_.scene_collision_proximity_threshold)) + { + if (rosparam_shortcuts::get("", n, parameter_ns + "/collision_proximity_threshold", + ros_parameters_.self_collision_proximity_threshold)) + { + ROS_WARN_NAMED(LOGNAME, + "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate" + "'self_collision_proximity_threshold' and 'scene_collision_proximity_threshold' " + "parameters. Please update the jogging yaml file."); + ros_parameters_.scene_collision_proximity_threshold = ros_parameters_.self_collision_proximity_threshold; + } + else + { + error += 1; + } + } error += !rosparam_shortcuts::get("", n, parameter_ns + "/move_group_name", ros_parameters_.move_group_name); error += !rosparam_shortcuts::get("", n, parameter_ns + "/planning_frame", ros_parameters_.planning_frame); error += !rosparam_shortcuts::get("", n, parameter_ns + "/use_gazebo", ros_parameters_.use_gazebo); @@ -143,12 +163,23 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) "greater than zero. Check yaml file."); return false; } - if (ros_parameters_.collision_proximity_threshold < 0.) + if (ros_parameters_.self_collision_proximity_threshold < 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should be " + "greater than zero. Check yaml file."); + return false; + } + if (ros_parameters_.scene_collision_proximity_threshold < 0.) { - ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_proximity_threshold' should be " + ROS_WARN_NAMED(LOGNAME, "Parameter 'scene_collision_proximity_threshold' should be " "greater than zero. Check yaml file."); return false; } + if (ros_parameters_.scene_collision_proximity_threshold < ros_parameters_.self_collision_proximity_threshold) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should probably be less " + "than or equal to 'scene_collision_proximity_threshold'. Check yaml file."); + } if (ros_parameters_.low_pass_filter_coeff < 0.) { ROS_WARN_NAMED(LOGNAME, "Parameter 'low_pass_filter_coeff' should be " From 4d3062164bf03ebb74c57d5eba0e3e50438481ba Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Tue, 14 Apr 2020 11:48:49 -0600 Subject: [PATCH 259/612] [jog_arm] Fix param logic bug for self- and scene-collision proximity thresholds (#2022) --- .../moveit_jog_arm/src/jog_interface_base.cpp | 38 +++++++++++-------- .../test/config/jog_settings.yaml | 3 +- 2 files changed, 24 insertions(+), 17 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index 9b318d7b98..49f463002a 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -88,28 +88,34 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) ros_parameters_.lower_singularity_threshold); error += !rosparam_shortcuts::get("", n, parameter_ns + "/hard_stop_singularity_threshold", ros_parameters_.hard_stop_singularity_threshold); - // parameter was removed, replaced with separate self- and scene-collision proximity thresholds; if old parameter - // exists, print warning and use old parameter for both new parameter values - // TODO(JStech): remove this deprecation warning in ROS Noetic - if (!rosparam_shortcuts::get("", n, parameter_ns + "/self_collision_proximity_threshold", - ros_parameters_.self_collision_proximity_threshold) && - !rosparam_shortcuts::get("", n, parameter_ns + "/scene_collision_proximity_threshold", - ros_parameters_.scene_collision_proximity_threshold)) + // parameter was removed, replaced with separate self- and scene-collision proximity thresholds; the logic handling + // the different possible sets of defined parameters is somewhat complicated at this point + // TODO(JStech): remove this deprecation warning in ROS Noetic; simplify error case handling + bool have_self_collision_proximity_threshold = rosparam_shortcuts::get( + "", n, parameter_ns + "/self_collision_proximity_threshold", ros_parameters_.self_collision_proximity_threshold); + bool have_scene_collision_proximity_threshold = + rosparam_shortcuts::get("", n, parameter_ns + "/scene_collision_proximity_threshold", + ros_parameters_.scene_collision_proximity_threshold); + double collision_proximity_threshold; + if (n.hasParam(parameter_ns + "/collision_proximity_threshold") && + rosparam_shortcuts::get("", n, parameter_ns + "/collision_proximity_threshold", collision_proximity_threshold)) { - if (rosparam_shortcuts::get("", n, parameter_ns + "/collision_proximity_threshold", - ros_parameters_.self_collision_proximity_threshold)) + ROS_WARN_NAMED(LOGNAME, "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate" + "'self_collision_proximity_threshold' and 'scene_collision_proximity_threshold' " + "parameters. Please update the jogging yaml file."); + if (!have_self_collision_proximity_threshold) { - ROS_WARN_NAMED(LOGNAME, - "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate" - "'self_collision_proximity_threshold' and 'scene_collision_proximity_threshold' " - "parameters. Please update the jogging yaml file."); - ros_parameters_.scene_collision_proximity_threshold = ros_parameters_.self_collision_proximity_threshold; + ros_parameters_.self_collision_proximity_threshold = collision_proximity_threshold; + have_self_collision_proximity_threshold = true; } - else + if (!have_scene_collision_proximity_threshold) { - error += 1; + ros_parameters_.scene_collision_proximity_threshold = collision_proximity_threshold; + have_scene_collision_proximity_threshold = true; } } + error += !have_self_collision_proximity_threshold; + error += !have_scene_collision_proximity_threshold; error += !rosparam_shortcuts::get("", n, parameter_ns + "/move_group_name", ros_parameters_.move_group_name); error += !rosparam_shortcuts::get("", n, parameter_ns + "/planning_frame", ros_parameters_.planning_frame); error += !rosparam_shortcuts::get("", n, parameter_ns + "/use_gazebo", ros_parameters_.use_gazebo); diff --git a/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml b/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml index 036126f453..a26f72a2b3 100644 --- a/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml +++ b/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml @@ -53,4 +53,5 @@ command_out_topic: jog_server/command # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. -collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] +self_collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m] +scene_collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] From 173d6a6115d194019a3a5ada44e629d6ff5c7351 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Thu, 16 Apr 2020 22:38:41 +0200 Subject: [PATCH 260/612] collision world: check for empty shapes vector before access (#2026) --- moveit_core/collision_detection/src/world.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index 8d270b7396..fe49764cb7 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -143,7 +143,7 @@ bool World::knowsTransform(const std::string& name) const std::map::const_iterator it = objects_.find(name); if (it != objects_.end()) // only accept object name as frame if it is associated to a unique shape - return it->second->shape_poses_.size() == 1; + return !it->second->shape_poses_.empty(); else // Then objects' subframes { for (const std::pair& object : objects_) @@ -168,10 +168,15 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name) const const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& frame_found) const { + // assume found frame_found = true; + std::map::const_iterator it = objects_.find(name); if (it != objects_.end()) - return it->second->shape_poses_[0]; + { + if (!it->second->shape_poses_.empty()) + return it->second->shape_poses_[0]; + } else // Search within subframes { for (const std::pair& object : objects_) @@ -186,6 +191,7 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& fram } } + // we need a persisting isometry for the API static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity(); frame_found = false; return IDENTITY_TRANSFORM; From 9a9f77923628ba25d86c9fab56e32c82384ce152 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Sun, 19 Apr 2020 11:13:36 +0200 Subject: [PATCH 261/612] commander: python3 fix (#2030) --- moveit_commander/src/moveit_commander/interpreter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_commander/src/moveit_commander/interpreter.py b/moveit_commander/src/moveit_commander/interpreter.py index c9088de375..cd4777e626 100644 --- a/moveit_commander/src/moveit_commander/interpreter.py +++ b/moveit_commander/src/moveit_commander/interpreter.py @@ -587,7 +587,7 @@ def get_keywords(self): known_vars = self.get_active_group().get_remembered_joint_values().keys() known_constr = self.get_active_group().get_known_constraints() groups = self._robot.get_group_names() - return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + known_vars, + return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + list(known_vars), 'help':[], 'record':known_vars, 'show':known_vars, From 2a98624b26f7ad0be42c272e0787e538436bfedb Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Thu, 23 Apr 2020 13:08:15 +0200 Subject: [PATCH 262/612] Assistant: load additional file with kinematics parameters (#1997) add extra field kinematics_parameters_file add kinematics parameters files in launchfiles read in kinematics_parameter_files from planning_context.launch add button and file dialog to select file resolve choosen file to relative file move template cache invalidation to beginning of export - cache has to be cleaned before *each* export, not just the first Otherwise changes will not be included in the second run stop loading kinematic parameters in run_benchmark_ompl.launch - It's redundant there Remove support for manifest.xml - obsolete since ... indigo? --- .../tools/moveit_config_data.h | 18 +++++ .../src/tools/moveit_config_data.cpp | 80 +++++++++++++++++++ .../widgets/configuration_files_widget.cpp | 21 +++++ .../src/widgets/group_edit_widget.cpp | 40 ++++++++++ .../src/widgets/group_edit_widget.h | 4 + .../src/widgets/planning_groups_widget.cpp | 3 + .../src/widgets/start_screen_widget.cpp | 71 +++------------- .../launch/moveit_rviz.launch | 1 - .../launch/planning_context.launch | 1 + .../launch/run_benchmark_ompl.launch | 1 - 10 files changed, 178 insertions(+), 62 deletions(-) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index c9bbedb26c..d649cb6ad5 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -69,6 +69,7 @@ struct GroupMetaData std::string kinematics_solver_; // Name of kinematics plugin to use double kinematics_solver_search_resolution_; // resolution to use with solver double kinematics_solver_timeout_; // solver timeout + std::string kinematics_parameters_file_; // file for additional kinematics parameters std::string default_planner_; // Name of the default planner to use }; @@ -362,6 +363,13 @@ class MoveItConfigData */ bool inputKinematicsYAML(const std::string& file_path); + /** + * Input planning_context.launch for editing its values + * @param file_path path to planning_context.launch in the input package + * @return true if the file was read correctly + */ + bool inputPlanningContextLaunch(const std::string& file_path); + /** * Helper function for parsing ros_controllers.yaml file * @param YAML::Node - individual controller to be parsed @@ -396,6 +404,16 @@ class MoveItConfigData */ bool setPackagePath(const std::string& pkg_path); + /** + * determine the package name containing a given file path + * @param path to a file + * @param package_name holds the ros package name if found + * @param relative_filepath holds the relative path of the file to the package root + * @return whether the file belongs to a known package + */ + bool extractPackageNameFromPath(const std::string& path, std::string& package_name, + std::string& relative_filepath) const; + /** * Resolve path to .setup_assistant file * @param path resolved path diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 48678940ef..462b07b951 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -1360,6 +1360,46 @@ bool MoveItConfigData::inputKinematicsYAML(const std::string& file_path) return true; // file created successfully } +// ****************************************************************************************** +// Input planning_context.launch file +// ****************************************************************************************** +bool MoveItConfigData::inputPlanningContextLaunch(const std::string& file_path) +{ + TiXmlDocument launch_document(file_path); + if (!launch_document.LoadFile()) + { + ROS_ERROR_STREAM("Failed parsing " << file_path); + return false; + } + + // find the kinematics section + TiXmlHandle doc(&launch_document); + TiXmlElement* kinematics_group = doc.FirstChild("launch").FirstChild("group").ToElement(); + while (kinematics_group && kinematics_group->Attribute("ns") != std::string("$(arg robot_description)_kinematics")) + { + kinematics_group = kinematics_group->NextSiblingElement("group"); + } + if (!kinematics_group) + { + ROS_ERROR(" not found"); + return false; + } + + // iterate over all elements + // and if 'group' matches an existing group, copy the filename + for (TiXmlElement* kinematics_parameter_file = kinematics_group->FirstChildElement("rosparam"); + kinematics_parameter_file; kinematics_parameter_file = kinematics_parameter_file->NextSiblingElement("rosparam")) + { + const char* ns = kinematics_parameter_file->Attribute("ns"); + if (ns && (group_meta_data_.find(ns) != group_meta_data_.end())) + { + group_meta_data_[ns].kinematics_parameters_file_ = kinematics_parameter_file->Attribute("file"); + } + } + + return true; +} + // ****************************************************************************************** // Helper function for parsing an individual ROSController from ros_controllers yaml file // ****************************************************************************************** @@ -1550,6 +1590,46 @@ bool MoveItConfigData::setPackagePath(const std::string& pkg_path) return true; } +// ****************************************************************************************** +// Extract the package/stack name from an absolute file path +// Input: path +// Output: package name and relative path +// ****************************************************************************************** +bool MoveItConfigData::extractPackageNameFromPath(const std::string& path, std::string& package_name, + std::string& relative_filepath) const +{ + fs::path sub_path = path; // holds the directory less one folder + fs::path relative_path; // holds the path after the sub_path + + bool package_found = false; + + // truncate path step by step and check if it contains a package.xml + while (!sub_path.empty()) + { + ROS_DEBUG_STREAM("checking in " << sub_path.make_preferred().string()); + if (fs::is_regular_file(sub_path / "package.xml")) + { + ROS_DEBUG_STREAM("Found package.xml in " << sub_path.make_preferred().string()); + package_found = true; + relative_filepath = relative_path.string(); + package_name = sub_path.leaf().string(); + break; + } + relative_path = sub_path.leaf() / relative_path; + sub_path.remove_leaf(); + } + + // Assign data to moveit_config_data + if (!package_found) + { + // No package name found, we must be outside ROS + return false; + } + + ROS_DEBUG_STREAM("Package name for file \"" << path << "\" is \"" << package_name << "\""); + return true; +} + // ****************************************************************************************** // Resolve path to .setup_assistant file // ****************************************************************************************** diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index f0e6b7e9d9..fbb737ecd7 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -962,6 +962,9 @@ bool ConfigurationFilesWidget::generatePackage() absolute_path = config_data_->appendPaths(new_package_path, file->rel_path_); ROS_DEBUG_STREAM("Creating file " << absolute_path); + // Clear template strings in case export is run multiple times with changes in between + template_strings_.clear(); + // Run the generate function if (!file->gen_func_(absolute_path)) { @@ -1118,6 +1121,24 @@ void ConfigurationFilesWidget::loadTemplateStrings() addTemplateString("[ROS_CONTROLLERS]", controllers.str()); } + // Pair 10 - Add parameter files for the kinematics solvers that should be loaded + // in addition to kinematics.yaml by planning_context.launch + std::string kinematics_parameters_files_block; + for (const auto& groups : config_data_->group_meta_data_) + { + if (groups.second.kinematics_parameters_file_.empty()) + continue; + + // add a linebreak if we have more than one entry + if (!kinematics_parameters_files_block.empty()) + kinematics_parameters_files_block += "\n"; + + std::string line = " "; + kinematics_parameters_files_block += line; + } + addTemplateString("[KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK]", kinematics_parameters_files_block); + addTemplateString("[AUTHOR_NAME]", config_data_->author_name_); addTemplateString("[AUTHOR_EMAIL]", config_data_->author_email_); } diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp index ed1643ea86..f857926413 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -88,6 +89,20 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& con kinematics_timeout_field_->setMaximumWidth(400); form_layout->addRow("Kin. Search Timeout (sec):", kinematics_timeout_field_); + // file to load additional parameters from + kinematics_parameters_file_field_ = new QLineEdit(this); + kinematics_parameters_file_field_->setMaximumWidth(400); + QPushButton* kinematics_parameters_file_button = new QPushButton("...", this); + kinematics_parameters_file_button->setMaximumWidth(50); + connect(kinematics_parameters_file_button, SIGNAL(clicked()), this, SLOT(selectKinematicsFile())); + QBoxLayout* kinematics_parameters_file_layout = new QHBoxLayout(this); + kinematics_parameters_file_layout->addWidget(kinematics_parameters_file_field_); + kinematics_parameters_file_layout->addWidget(kinematics_parameters_file_button); + kinematics_parameters_file_layout->setContentsMargins(0, 0, 0, 0); + QWidget* container = new QWidget(this); + container->setLayout(kinematics_parameters_file_layout); + form_layout->addRow("Kin. parameters file:", container); + group1->setLayout(form_layout); // OMPL Planner form -------------------------------------------- @@ -258,6 +273,9 @@ void GroupEditWidget::setSelected(const std::string& group_name) kinematics_solver_field_->setCurrentIndex(index); } + kinematics_parameters_file_field_->setText( + config_data_->group_meta_data_[group_name].kinematics_parameters_file_.c_str()); + // Set default planner std::string default_planner = config_data_->group_meta_data_[group_name].default_planner_; @@ -338,4 +356,26 @@ void GroupEditWidget::loadKinematicPlannersComboBox() } } +void GroupEditWidget::selectKinematicsFile() +{ + QString filename = QFileDialog::getOpenFileName(this, "Select a parameter file", "", "YAML files (*.yaml)"); + + if (filename.isEmpty()) + { + return; + } + + std::string package_name; + std::string relative_filename; + bool package_found = + config_data_->extractPackageNameFromPath(filename.toStdString(), package_name, relative_filename); + + QString lookup_path = filename; + if (package_found) + { + lookup_path = QString("$(find %1)/%2").arg(package_name.c_str()).arg(relative_filename.c_str()); + } + kinematics_parameters_file_field_->setText(lookup_path); +} + } // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.h b/moveit_setup_assistant/src/widgets/group_edit_widget.h index 60a6a67b6e..bf62ee77fb 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.h +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.h @@ -75,6 +75,7 @@ class GroupEditWidget : public QWidget QComboBox* kinematics_solver_field_; QLineEdit* kinematics_resolution_field_; QLineEdit* kinematics_timeout_field_; + QLineEdit* kinematics_parameters_file_field_; QComboBox* default_planner_field_; QPushButton* btn_delete_; // this button is hidden for new groups QPushButton* btn_save_; // this button is hidden for new groups @@ -86,6 +87,9 @@ private Q_SLOTS: // Slot Event Functions // ****************************************************************************************** + /// Shows a file dialog to select an additional parameter file for kinematics + void selectKinematicsFile(); + Q_SIGNALS: // ****************************************************************************************** diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index 4379543ac9..e3da0c9ec4 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -1084,6 +1084,8 @@ bool PlanningGroupsWidget::saveGroupScreen() const std::string& default_planner = group_edit_widget_->default_planner_field_->currentText().toStdString(); const std::string& kinematics_resolution = group_edit_widget_->kinematics_resolution_field_->text().toStdString(); const std::string& kinematics_timeout = group_edit_widget_->kinematics_timeout_field_->text().toStdString(); + const std::string& kinematics_parameters_file = + group_edit_widget_->kinematics_parameters_file_field_->text().toStdString(); // Used for editing existing groups srdf::Model::Group* searched_group = nullptr; @@ -1234,6 +1236,7 @@ bool PlanningGroupsWidget::saveGroupScreen() config_data_->group_meta_data_[group_name].kinematics_solver_ = kinematics_solver; config_data_->group_meta_data_[group_name].kinematics_solver_search_resolution_ = kinematics_resolution_double; config_data_->group_meta_data_[group_name].kinematics_solver_timeout_ = kinematics_timeout_double; + config_data_->group_meta_data_[group_name].kinematics_parameters_file_ = kinematics_parameters_file; config_data_->group_meta_data_[group_name].default_planner_ = default_planner; config_data_->changes |= MoveItConfigData::GROUP_KINEMATICS; diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 5c22805d09..6910917c35 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -416,6 +416,12 @@ bool StartScreenWidget::loadExistingFiles() "at location ") .append(kinematics_yaml_path.make_preferred().string().c_str())); } + else + { + fs::path planning_context_launch_path = config_data_->config_pkg_path_; + planning_context_launch_path /= "launch/planning_context.launch"; + config_data_->inputPlanningContextLaunch(planning_context_launch_path.make_preferred().string()); + } // Load 3d_sensors config file load3DSensorsFile(); @@ -636,65 +642,10 @@ bool StartScreenWidget::setSRDFFile(const std::string& srdf_string) // ****************************************************************************************** bool StartScreenWidget::extractPackageNameFromPath() { - // Get the path to urdf, save filename - fs::path urdf_path = config_data_->urdf_path_; - fs::path urdf_directory = urdf_path; - urdf_directory.remove_filename(); - - fs::path sub_path; // holds the directory less one folder - fs::path relative_path; // holds the path after the sub_path - std::string package_name; // result - - // Paths for testing if files exist - fs::path package_path; - - std::vector path_parts; // holds each folder name in vector - - // Copy path into vector of parts - for (fs::path::iterator it = urdf_directory.begin(); it != urdf_directory.end(); ++it) - path_parts.push_back(it->string()); - - bool package_found = false; - - // reduce the generated directoy path's folder count by 1 each loop - for (int segment_length = path_parts.size(); segment_length > 0; --segment_length) - { - // Reset the sub_path - sub_path.clear(); + std::string relative_path; // holds the path after the sub_path + std::string package_name; // result - // Create a subpath based on the outer loops length - for (int segment_count = 0; segment_count < segment_length; ++segment_count) - { - sub_path /= path_parts[segment_count]; - - // decide if we should remember this directory name because it is topmost, in case it is the package/stack name - if (segment_count == segment_length - 1) - { - package_name = path_parts[segment_count]; - } - } - - // check if this directory has a package.xml - package_path = sub_path; - package_path /= "package.xml"; - ROS_DEBUG_STREAM("Checking for " << package_path.make_preferred().string()); - - // Check if the files exist - if (fs::is_regular_file(package_path) || fs::is_regular_file(sub_path / "manifest.xml")) - { - // now generate the relative path - for (std::size_t relative_count = segment_length; relative_count < path_parts.size(); ++relative_count) - relative_path /= path_parts[relative_count]; - - // add the URDF filename at end of relative path - relative_path /= urdf_path.filename(); - - // end the search - segment_length = 0; - package_found = true; - break; - } - } + bool package_found = config_data_->extractPackageNameFromPath(config_data_->urdf_path_, package_name, relative_path); // Assign data to moveit_config_data if (!package_found) @@ -706,7 +657,7 @@ bool StartScreenWidget::extractPackageNameFromPath() else { // Check that ROS can find the package - const std::string robot_desc_pkg_path = ros::package::getPath(config_data_->urdf_pkg_name_) + "/"; + const std::string robot_desc_pkg_path = ros::package::getPath(package_name); if (robot_desc_pkg_path.empty()) { @@ -718,7 +669,7 @@ bool StartScreenWidget::extractPackageNameFromPath() // Success config_data_->urdf_pkg_name_ = package_name; - config_data_->urdf_pkg_relative_path_ = relative_path.make_preferred().string(); + config_data_->urdf_pkg_relative_path_ = relative_path; } ROS_DEBUG_STREAM("URDF Package Name: " << config_data_->urdf_pkg_name_); diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch index 89078f2bc3..615b2f12a5 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch @@ -10,7 +10,6 @@ - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch index c2f9b0b613..54efbec4e7 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch @@ -19,6 +19,7 @@ +[KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK] diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch index f66253f1f3..f35d05fe05 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch @@ -15,7 +15,6 @@ - From ad88e18ae604e0533456c89eb0c6694bb0b63081 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 24 Apr 2020 06:11:56 -0500 Subject: [PATCH 263/612] [jog_arm] Keep updating joints, even while waiting for a valid command (#2027) * Keep updating joints, even while waiting for a valid command This prevents a potential jump in joint angles when jogging resumes * Simplify some logic, add a flowchart pdf * Delete pdf, clean up !stale_command usage --- .../moveit_jog_arm/src/jog_calcs.cpp | 76 +++++++++---------- 1 file changed, 37 insertions(+), 39 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 61ca19dc40..3d95fa2f1d 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -139,63 +139,63 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) shared_variables.tf_moveit_to_cmd_frame = tf_moveit_to_cmd_frame_; shared_variables.unlock(); - // Check for an initial jog command - if (wait_for_jog_commands) - { - shared_variables.lock(); - // Check if there are any commands with valid timestamp - wait_for_jog_commands = shared_variables.command_deltas.header.stamp == ros::Time(0.) && - shared_variables.joint_command_deltas.header.stamp == ros::Time(0.); - shared_variables.unlock(); - } - // If paused or while waiting for initial jog commands, just keep the low-pass filters up to date with current // joints so a jump doesn't occur when restarting if (wait_for_jog_commands || shared_variables.paused) { for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(internal_joint_state_.position[i]); + position_filters_[i].reset(original_joint_state_.position[i]); + + shared_variables.lock(); + // Check if there are any new commands with valid timestamp + wait_for_jog_commands = shared_variables.command_deltas.header.stamp == ros::Time(0.) && + shared_variables.joint_command_deltas.header.stamp == ros::Time(0.); + shared_variables.unlock(); } + // If not waiting for initial command, and not paused. // Do jogging calculations only if the robot should move, for efficiency else { - // Flag that incoming commands are all zero. May be used to skip calculations/publication + // Halt if the command is stale or inputs are all zero, or commands were zero shared_variables.lock(); bool have_nonzero_cartesian_cmd = shared_variables.have_nonzero_cartesian_cmd; bool have_nonzero_joint_cmd = shared_variables.have_nonzero_joint_cmd; + bool stale_command = shared_variables.command_is_stale; shared_variables.unlock(); - // Prioritize cartesian jogging above joint jogging - if (have_nonzero_cartesian_cmd) + bool valid_nonzero_command = false; + if (!stale_command) { - shared_variables.lock(); - cartesian_deltas = shared_variables.command_deltas; - shared_variables.unlock(); + // Prioritize cartesian jogging above joint jogging + if (have_nonzero_cartesian_cmd) + { + shared_variables.lock(); + cartesian_deltas = shared_variables.command_deltas; + shared_variables.unlock(); - if (!cartesianJogCalcs(cartesian_deltas, shared_variables)) - continue; - } - else if (have_nonzero_joint_cmd) - { - shared_variables.lock(); - joint_deltas = shared_variables.joint_command_deltas; - shared_variables.unlock(); + if (!cartesianJogCalcs(cartesian_deltas, shared_variables)) + continue; + } + else if (have_nonzero_joint_cmd) + { + shared_variables.lock(); + joint_deltas = shared_variables.joint_command_deltas; + shared_variables.unlock(); - if (!jointJogCalcs(joint_deltas, shared_variables)) - continue; - } - else - { - outgoing_command_ = composeJointTrajMessage(internal_joint_state_); - } + if (!jointJogCalcs(joint_deltas, shared_variables)) + continue; + } - // Halt if the command is stale or inputs are all zero, or commands were zero - shared_variables.lock(); - bool stale_command = shared_variables.command_is_stale; - shared_variables.unlock(); + valid_nonzero_command = have_nonzero_cartesian_cmd || have_nonzero_joint_cmd; + } - if (stale_command || (!have_nonzero_cartesian_cmd && !have_nonzero_joint_cmd)) + // If we should halt + if (!valid_nonzero_command) { + // Keep the joint position filters up-to-date with current joints + for (std::size_t i = 0; i < num_joints_; ++i) + position_filters_[i].reset(original_joint_state_.position[i]); + suddenHalt(outgoing_command_); have_nonzero_cartesian_cmd = false; have_nonzero_joint_cmd = false; @@ -206,8 +206,6 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) shared_variables.unlock(); } - bool valid_nonzero_command = have_nonzero_cartesian_cmd || have_nonzero_joint_cmd; - // Send the newest target joints shared_variables.lock(); // If everything normal, share the new traj to be published From 50b401960adcd4aef438882ed5e6331d618ff896 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Sun, 22 Mar 2020 04:14:00 +0100 Subject: [PATCH 264/612] Improve performance by calling Eigen::Transform::linear() instead of Eigen::Transform::rotation(). --- .../collision_common.h | 2 +- .../src/default_constraint_samplers.cpp | 12 +++---- .../distance_field/src/distance_field.cpp | 8 ++--- .../dynamics_solver/src/dynamics_solver.cpp | 2 +- .../src/kinematic_constraint.cpp | 27 ++++++++-------- .../kinematic_constraints/src/utils.cpp | 2 +- .../planning_scene/src/planning_scene.cpp | 2 +- moveit_core/robot_model/src/aabb.cpp | 2 +- .../robot_model/src/floating_joint_model.cpp | 2 +- moveit_core/robot_model/src/link_model.cpp | 4 +-- .../robot_model/src/planar_joint_model.cpp | 2 +- .../robot_model/src/revolute_joint_model.cpp | 2 +- .../src/cartesian_interpolator.cpp | 6 ++-- moveit_core/robot_state/src/robot_state.cpp | 10 +++--- .../robot_state/test/robot_state_test.cpp | 32 +++++++++---------- moveit_core/robot_state/test/test_aabb.cpp | 4 +-- .../include/moveit/transforms/transforms.h | 6 ++-- .../src/approach_and_translate_stage.cpp | 4 +-- ...inematics_speed_and_validity_evaluator.cpp | 2 +- .../src/trajectory_execution_manager.cpp | 2 +- .../src/wrap_python_robot_interface.cpp | 2 +- .../src/motion_planning_frame_objects.cpp | 4 +-- .../src/planning_link_updater.cpp | 2 +- .../src/render_shapes.cpp | 2 +- 24 files changed, 71 insertions(+), 72 deletions(-) diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index 66beae404d..7922a7bbdb 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -319,7 +319,7 @@ inline void transform2fcl(const Eigen::Isometry3d& b, fcl::Transform3d& f) #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) f = b.matrix(); #else - Eigen::Quaterniond q(b.rotation()); + Eigen::Quaterniond q(b.linear()); f.setTranslation(fcl::Vector3d(b.translation().x(), b.translation().y(), b.translation().z())); f.setQuatRotation(fcl::Quaternion3f(q.w(), q.x(), q.y(), q.z())); #endif diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index c8dbb1f45e..4f78d7edd8 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -479,16 +479,16 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q Eigen::Isometry3d diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ())); - Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.rotation()); - quat = Eigen::Quaterniond(reqr.rotation()); + Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear()); + quat = Eigen::Quaterniond(reqr.linear()); // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the // model if (sampling_pose_.orientation_constraint_->mobileReferenceFrame()) { const Eigen::Isometry3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame()); - Eigen::Isometry3d rt(t.rotation() * quat); - quat = Eigen::Quaterniond(rt.rotation()); + Eigen::Isometry3d rt(t.linear() * quat); + quat = Eigen::Quaterniond(rt.linear()); } } else @@ -565,7 +565,7 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq; point = ikq.translation(); - quat = Eigen::Quaterniond(ikq.rotation()); + quat = Eigen::Quaterniond(ikq.linear()); } if (need_eef_to_ik_tip_transform_) @@ -574,7 +574,7 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); ikq = ikq * eef_to_ik_tip_transform_; point = ikq.translation(); - quat = Eigen::Quaterniond(ikq.rotation()); + quat = Eigen::Quaterniond(ikq.linear()); } geometry_msgs::Pose ik_query; diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index 071777706b..0ec6cfecd2 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -172,10 +172,10 @@ void DistanceField::getGradientMarkers(double min_distance, double max_distance, // double angle = -gradient.angle(unitX); // Eigen::AngleAxisd rotation(angle, axis); - // marker.pose.orientation.x = rotation.rotation().x(); - // marker.pose.orientation.y = rotation.rotation().y(); - // marker.pose.orientation.z = rotation.rotation().z(); - // marker.pose.orientation.w = rotation.rotation().w(); + // marker.pose.orientation.x = rotation.linear().x(); + // marker.pose.orientation.y = rotation.linear().y(); + // marker.pose.orientation.z = rotation.linear().z(); + // marker.pose.orientation.w = rotation.linear().w(); marker.scale.x = getResolution(); marker.scale.y = getResolution(); diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index 20cf844b39..5462fd2ae5 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -53,7 +53,7 @@ inline geometry_msgs::Vector3 transformVector(const Eigen::Isometry3d& transform { Eigen::Vector3d p; p = Eigen::Vector3d(vector.x, vector.y, vector.z); - p = transform.rotation() * p; + p = transform.linear() * p; geometry_msgs::Vector3 result; result.x = p.x(); diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 9ed1c08125..3ca1d5c719 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -391,7 +391,7 @@ bool PositionConstraint::equal(const KinematicConstraint& other, double margin) for (std::size_t j = 0; j < o.constraint_region_.size(); ++j) { Eigen::Isometry3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[j]; - if (diff.translation().norm() < margin && diff.rotation().isIdentity(margin) && + if (diff.translation().norm() < margin && diff.linear().isIdentity(margin) && constraint_region_[i]->getType() == o.constraint_region_[j]->getType() && fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[j]->computeVolume()) < margin) { @@ -597,16 +597,15 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob Eigen::Vector3d xyz; if (mobile_frame_) { - Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).rotation() * desired_rotation_matrix_; - Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).rotation()); - xyz = diff.rotation().eulerAngles(0, 1, 2); + Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).linear() * desired_rotation_matrix_; + Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).linear()); + xyz = diff.linear().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints } else { - Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).rotation()); - xyz = - diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints + Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).linear()); + xyz = diff.linear().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints } xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi() - fabs(xyz(0))); @@ -618,7 +617,7 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob if (verbose) { - Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).rotation()); + Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).linear()); Eigen::Quaterniond q_des(desired_rotation_matrix_); ROS_INFO_NAMED("kinematic_constraints", "Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion " @@ -756,12 +755,12 @@ bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin Eigen::Isometry3d diff = sensor_pose_.inverse() * o.sensor_pose_; if (diff.translation().norm() > margin) return false; - if (!diff.rotation().isIdentity(margin)) + if (!diff.linear().isIdentity(margin)) return false; diff = target_pose_.inverse() * o.target_pose_; if (diff.translation().norm() > margin) return false; - if (!diff.rotation().isIdentity(margin)) + if (!diff.linear().isIdentity(margin)) return false; return true; } @@ -895,7 +894,7 @@ void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state, mka.scale.y = .15; mka.scale.z = 0.0; mka.points.resize(2); - Eigen::Vector3d d = tp.translation() + tp.rotation().col(2) * 0.5; + Eigen::Vector3d d = tp.translation() + tp.linear().col(2) * 0.5; mka.points[0].x = tp.translation().x(); mka.points[0].y = tp.translation().y(); mka.points[0].z = tp.translation().z(); @@ -908,7 +907,7 @@ void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state, mka.color.b = 1.0; mka.color.r = 0.0; - d = sp.translation() + sp.rotation().col(2 - sensor_view_direction_) * 0.5; + d = sp.translation() + sp.linear().col(2 - sensor_view_direction_) * 0.5; mka.points[0].x = sp.translation().x(); mka.points[0].y = sp.translation().y(); mka.points[0].z = sp.translation().z(); @@ -932,11 +931,11 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_; // necessary to do subtraction as SENSOR_Z is 0 and SENSOR_X is 2 - const Eigen::Vector3d& normal2 = sp.rotation().col(2 - sensor_view_direction_); + const Eigen::Vector3d& normal2 = sp.linear().col(2 - sensor_view_direction_); if (max_view_angle_ > 0.0) { - const Eigen::Vector3d& normal1 = tp.rotation().col(2) * -1.0; // along Z axis and inverted + const Eigen::Vector3d& normal1 = tp.linear().col(2) * -1.0; // along Z axis and inverted double dp = normal2.dot(normal1); double ang = acos(dp); if (dp < 0.0) diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 9b8813a1f7..c9cb35641e 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -567,7 +567,7 @@ bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotSta if (c.link_name != robot_link->getName()) { c.link_name = robot_link->getName(); - Eigen::Quaterniond link_name_to_robot_link(transform.linear().inverse() * + Eigen::Quaterniond link_name_to_robot_link(transform.linear().transpose() * state.getGlobalLinkTransform(robot_link).linear()); Eigen::Quaterniond quat_target; tf::quaternionMsgToEigen(c.orientation, quat_target); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 28f70f32b4..f340a5f144 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -966,7 +966,7 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const shapes::saveAsText(obj->shapes_[j].get(), out); out << obj->shape_poses_[j].translation().x() << " " << obj->shape_poses_[j].translation().y() << " " << obj->shape_poses_[j].translation().z() << std::endl; - Eigen::Quaterniond r(obj->shape_poses_[j].rotation()); + Eigen::Quaterniond r(obj->shape_poses_[j].linear()); out << r.x() << " " << r.y() << " " << r.z() << " " << r.w() << std::endl; if (hasObjectColor(id)) { diff --git a/moveit_core/robot_model/src/aabb.cpp b/moveit_core/robot_model/src/aabb.cpp index fa7955ed37..51b3523032 100644 --- a/moveit_core/robot_model/src/aabb.cpp +++ b/moveit_core/robot_model/src/aabb.cpp @@ -44,7 +44,7 @@ void moveit::core::AABB::extendWithTransformedBox(const Eigen::Isometry3d& trans // // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/ - const Eigen::Matrix3d& r = transform.rotation(); + const Eigen::Matrix3d& r = transform.linear(); const Eigen::Vector3d& t = transform.translation(); double x_range = 0.5 * (fabs(r(0, 0) * box[0]) + fabs(r(0, 1) * box[1]) + fabs(r(0, 2) * box[2])); diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 47ed6a87d2..b7d25080a2 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -225,7 +225,7 @@ void FloatingJointModel::computeVariablePositions(const Eigen::Isometry3d& trans joint_values[0] = transf.translation().x(); joint_values[1] = transf.translation().y(); joint_values[2] = transf.translation().z(); - Eigen::Quaterniond q(transf.rotation()); + Eigen::Quaterniond q(transf.linear()); joint_values[3] = q.x(); joint_values[4] = q.y(); joint_values[5] = q.z(); diff --git a/moveit_core/robot_model/src/link_model.cpp b/moveit_core/robot_model/src/link_model.cpp index a4435e425e..c75ab24e0a 100644 --- a/moveit_core/robot_model/src/link_model.cpp +++ b/moveit_core/robot_model/src/link_model.cpp @@ -61,7 +61,7 @@ void LinkModel::setJointOriginTransform(const Eigen::Isometry3d& transform) { joint_origin_transform_ = transform; joint_origin_transform_is_identity_ = - joint_origin_transform_.rotation().isIdentity() && + joint_origin_transform_.linear().isIdentity() && joint_origin_transform_.translation().norm() < std::numeric_limits::epsilon(); } @@ -83,7 +83,7 @@ void LinkModel::setGeometry(const std::vector& shapes, for (std::size_t i = 0; i < shapes_.size(); ++i) { collision_origin_transform_is_identity_[i] = - (collision_origin_transform_[i].rotation().isIdentity() && + (collision_origin_transform_[i].linear().isIdentity() && collision_origin_transform_[i].translation().norm() < std::numeric_limits::epsilon()) ? 1 : 0; diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp index 9be6a69bfa..f515be4c9f 100644 --- a/moveit_core/robot_model/src/planar_joint_model.cpp +++ b/moveit_core/robot_model/src/planar_joint_model.cpp @@ -224,7 +224,7 @@ void PlanarJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, joint_values[0] = transf.translation().x(); joint_values[1] = transf.translation().y(); - Eigen::Quaterniond q(transf.rotation()); + Eigen::Quaterniond q(transf.linear()); // taken from Bullet double s_squared = 1.0 - (q.w() * q.w()); if (s_squared < 10.0 * std::numeric_limits::epsilon()) diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index a7afaa9b91..c3783d48e1 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -268,7 +268,7 @@ void RevoluteJointModel::computeTransform(const double* joint_values, Eigen::Iso void RevoluteJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const { - Eigen::Quaterniond q(transf.rotation()); + Eigen::Quaterniond q(transf.linear()); q.normalize(); size_t max_idx; axis_.array().abs().maxCoeff(&max_idx); diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 46d17753b5..b2b38db361 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -62,7 +62,7 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons const Eigen::Isometry3d& start_pose = start_state->getGlobalLinkTransform(link); // the direction can be in the local reference frame (in which case we rotate it) - const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.rotation() * direction; + const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.linear() * direction; // The target pose is built by applying a translation to the start pose for the desired direction and distance Eigen::Isometry3d target_pose = start_pose; @@ -91,8 +91,8 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons // the target can be in the local reference frame (in which case we rotate it) Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; - Eigen::Quaterniond start_quaternion(start_pose.rotation()); - Eigen::Quaterniond target_quaternion(rotated_target.rotation()); + Eigen::Quaterniond start_quaternion(start_pose.linear()); + Eigen::Quaterniond target_quaternion(rotated_target.linear()); if (max_step.translation <= 0.0 && max_step.rotation <= 0.0) { diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 11e2440759..c8dda3b746 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1242,7 +1242,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link if (pjm->getType() == moveit::core::JointModel::REVOLUTE) { joint_transform = reference_transform * getGlobalLinkTransform(link); - joint_axis = joint_transform.rotation() * static_cast(pjm)->getAxis(); + joint_axis = joint_transform.linear() * static_cast(pjm)->getAxis(); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation()); jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis; @@ -1250,7 +1250,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link else if (pjm->getType() == moveit::core::JointModel::PRISMATIC) { joint_transform = reference_transform * getGlobalLinkTransform(link); - joint_axis = joint_transform.rotation() * static_cast(pjm)->getAxis(); + joint_axis = joint_transform.linear() * static_cast(pjm)->getAxis(); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis; } else if (pjm->getType() == moveit::core::JointModel::PLANAR) @@ -1279,7 +1279,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link // [x] [ w -z y ] [ omega_2 ] // [y] [ z w -x ] [ omega_3 ] // [z] [ -y x w ] - Eigen::Quaterniond q(link_transform.rotation()); + Eigen::Quaterniond q(link_transform.linear()); double w = q.w(), x = q.x(), y = q.y(), z = q.z(); Eigen::MatrixXd quaternion_update_matrix(4, 3); quaternion_update_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w; @@ -1836,7 +1836,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: for (std::size_t i = 0; i < transformed_poses.size(); ++i) { - Eigen::Quaterniond quat(transformed_poses[i].rotation()); + Eigen::Quaterniond quat(transformed_poses[i].linear()); Eigen::Vector3d point(transformed_poses[i].translation()); ik_queries[i].position.x = point.x(); ik_queries[i].position.y = point.y(); @@ -2101,7 +2101,7 @@ void RobotState::printStateInfo(std::ostream& out) const void RobotState::printTransform(const Eigen::Isometry3d& transform, std::ostream& out) const { - Eigen::Quaterniond q(transform.rotation()); + Eigen::Quaterniond q(transform.linear()); out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", " << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "]" << std::endl; diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 358d0cb64c..5c1d4f5559 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -450,28 +450,28 @@ TEST_F(OneRobot, FK) state.setVariablePositions(joint_values); EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(1, 1, 0)); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).x(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).y(), 1e-5); - EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).z(), 1e-5); - EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).w(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).x(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).y(), 1e-5); + EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).z(), 1e-5); + EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).w(), 1e-5); EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_a").translation(), Eigen::Vector3d(1, 1, 0)); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).x(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).y(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).z(), 1e-5); - EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).w(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).x(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).y(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).z(), 1e-5); + EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).w(), 1e-5); EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_b").translation(), Eigen::Vector3d(1, 1.5, 0)); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).x(), 1e-5); - EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).y(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).z(), 1e-5); - EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).w(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).x(), 1e-5); + EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).y(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).z(), 1e-5); + EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).w(), 1e-5); EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(1.08, 1.4, 0)); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).x(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).y(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).z(), 1e-5); - EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).w(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).x(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).y(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).z(), 1e-5); + EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).w(), 1e-5); EXPECT_TRUE(state.satisfiesBounds()); diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index 1c0cf06f6f..d41129ef28 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -211,7 +211,7 @@ TEST_F(TestAABB, TestPR2) msg.scale.z = extents[2]; msg.color.r = 0; msg.color.b = 1; - Eigen::Quaterniond q(transform.rotation()); + Eigen::Quaterniond q(transform.linear()); msg.pose.orientation.x = q.x(); msg.pose.orientation.y = q.y(); msg.pose.orientation.z = q.z(); @@ -257,7 +257,7 @@ TEST_F(TestAABB, TestPR2) msg.scale.z = extents[2]; msg.color.r = 0; msg.color.b = 1; - Eigen::Quaterniond q(transforms[i].rotation()); + Eigen::Quaterniond q(transforms[i].linear()); msg.pose.orientation.x = q.x(); msg.pose.orientation.y = q.y(); msg.pose.orientation.z = q.z(); diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 6833b606aa..8e72213aa6 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -137,7 +137,7 @@ class Transforms : private boost::noncopyable */ void transformVector3(const std::string& from_frame, const Eigen::Vector3d& v_in, Eigen::Vector3d& v_out) const { - v_out = getTransform(from_frame).rotation() * v_in; + v_out = getTransform(from_frame).linear() * v_in; } /** @@ -149,7 +149,7 @@ class Transforms : private boost::noncopyable void transformQuaternion(const std::string& from_frame, const Eigen::Quaterniond& q_in, Eigen::Quaterniond& q_out) const { - q_out = getTransform(from_frame).rotation() * q_in; + q_out = getTransform(from_frame).linear() * q_in; } /** @@ -160,7 +160,7 @@ class Transforms : private boost::noncopyable */ void transformRotationMatrix(const std::string& from_frame, const Eigen::Matrix3d& m_in, Eigen::Matrix3d& m_out) const { - m_out = getTransform(from_frame).rotation() * m_in; + m_out = getTransform(from_frame).linear() * m_in; } /** diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp index c05e817149..3fdde91816 100644 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp @@ -216,10 +216,10 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // transform the input vectors in accordance to frame specified in the header; if (approach_direction_is_global_frame) approach_direction = - planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).rotation() * approach_direction; + planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).linear() * approach_direction; if (retreat_direction_is_global_frame) retreat_direction = - planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).rotation() * retreat_direction; + planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).linear() * retreat_direction; // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping moveit::core::GroupStateValidityCallbackFn approach_valid_callback = diff --git a/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp b/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp index d0267a2850..4dc408396a 100644 --- a/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp +++ b/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp @@ -94,7 +94,7 @@ int main(int argc, char** argv) moveit::tools::Profiler::End("IK"); const Eigen::Isometry3d& pose_upd = state.getGlobalLinkTransform(tip); Eigen::Isometry3d diff = pose_upd * pose.inverse(); - double rot_err = (diff.rotation() - Eigen::Matrix3d::Identity()).norm(); + double rot_err = (diff.linear() - Eigen::Matrix3d::Identity()).norm(); double trans_err = diff.translation().norm(); moveit::tools::Profiler::Average("Rotation error", rot_err); moveit::tools::Profiler::Average("Translation error", trans_err); diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index bf83b39e7e..1db84cf7ac 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -1009,7 +1009,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont start_transform = tf2::transformToEigen(transforms[i]); Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation(); Eigen::AngleAxisd rotation; - rotation.fromRotationMatrix(cur_transform.rotation().transpose() * start_transform.rotation()); + rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear()); if ((offset.array() > allowed_start_tolerance_).any() || rotation.angle() > allowed_start_tolerance_) { ROS_ERROR_STREAM_NAMED(name_, "\nInvalid Trajectory: start point deviates from current robot state more than " diff --git a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp index 893a8c9bb7..35c1e047ad 100644 --- a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp +++ b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp @@ -155,7 +155,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer v[0] = t.translation().x(); v[1] = t.translation().y(); v[2] = t.translation().z(); - Eigen::Quaterniond q(t.rotation()); + Eigen::Quaterniond q(t.linear()); v[3] = q.x(); v[4] = q.y(); v[5] = q.z(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index e22be44612..8573ba7f51 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -238,7 +238,7 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() if (obj->shapes_.size() == 1) { obj_pose = obj->shape_poses_[0]; - Eigen::Vector3d xyz = obj_pose.rotation().eulerAngles(0, 1, 2); + Eigen::Vector3d xyz = obj_pose.linear().eulerAngles(0, 1, 2); update_scene_marker = true; // do the marker update outside locked scope to avoid deadlock bool old_state = ui_->object_x->blockSignals(true); @@ -322,7 +322,7 @@ void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position) // Update the interactive marker pose to match the manually introduced one if (update_marker_position && scene_marker_) { - Eigen::Quaterniond eq(p.rotation()); + Eigen::Quaterniond eq(p.linear()); scene_marker_->setPose(Ogre::Vector3(ui_->object_x->value(), ui_->object_y->value(), ui_->object_z->value()), Ogre::Quaternion(eq.w(), eq.x(), eq.y(), eq.z()), ""); } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp index 0ad0668b06..2455db97ca 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp @@ -52,7 +52,7 @@ bool moveit_rviz_plugin::PlanningLinkUpdater::getLinkTransforms(const std::strin } const Eigen::Vector3d& robot_visual_position = kinematic_state_->getGlobalLinkTransform(link_model).translation(); - Eigen::Quaterniond robot_visual_orientation(kinematic_state_->getGlobalLinkTransform(link_model).rotation()); + Eigen::Quaterniond robot_visual_orientation(kinematic_state_->getGlobalLinkTransform(link_model).linear()); visual_position = Ogre::Vector3(robot_visual_position.x(), robot_visual_position.y(), robot_visual_position.z()); visual_orientation = Ogre::Quaternion(robot_visual_orientation.w(), robot_visual_orientation.x(), robot_visual_orientation.y(), robot_visual_orientation.z()); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index 55ad73109e..19e06f150c 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -82,7 +82,7 @@ void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, co rviz::Shape* ogre_shape = nullptr; Eigen::Vector3d translation = p.translation(); Ogre::Vector3 position(translation.x(), translation.y(), translation.z()); - Eigen::Quaterniond q(p.rotation()); + Eigen::Quaterniond q(p.linear()); Ogre::Quaternion orientation(q.w(), q.x(), q.y(), q.z()); // we don't know how to render cones directly, but we can convert them to a mesh From 2dacfe990ec1bda8d78a0aa5954cb16bc0c17493 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 4 May 2020 10:27:50 -0600 Subject: [PATCH 265/612] [ci] configure travis to report success before code-coverage finishes (#2041) configure travis to report success before code-coverage finishes --- .travis.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.travis.yml b/.travis.yml index 61cf02f9f9..294c83851d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -22,6 +22,7 @@ env: - WARNINGS_OK=false jobs: + fast_finish: true include: - env: TEST="clang-format catkin_lint" - env: TEST=code-coverage @@ -34,6 +35,8 @@ jobs: CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-overloaded-virtual" - if: branch =~ /^(.*-devel|master)$/ env: ROS_REPO=ros-shadow-fixed + allow_failures: + - env: TEST=code-coverage before_script: - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci From 5553507f97f36d8c34678975b1888fb2cff02960 Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Mon, 4 May 2020 18:52:42 +0200 Subject: [PATCH 266/612] Fix mesh_filter test (#2044) Test passes when relaxing the acceptance region to 1e-6 --- .travis.yml | 2 -- moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 294c83851d..740788cb87 100644 --- a/.travis.yml +++ b/.travis.yml @@ -16,8 +16,6 @@ env: - ROS_DISTRO=melodic - ROS_REPO=ros - UPSTREAM_WORKSPACE=moveit.rosinstall - # moveit_ros_perception: mesh_filter_test fails due to broken Mesa OpenGL - - TEST_BLACKLIST="moveit_ros_perception" - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" - WARNINGS_OK=false diff --git a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp index a452eb14f3..0de32d2a15 100644 --- a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp +++ b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp @@ -231,7 +231,7 @@ void MeshFilterTest::test() float sensor_depth = sensor_data_[idx] * FilterTraits::ToMetricScale; if (fabs(sensor_depth - distance_ - shadow_) > epsilon_ && fabs(sensor_depth - distance_) > epsilon_) { - ASSERT_FLOAT_EQ(filtered_depth[idx], gt_depth[idx]); + ASSERT_NEAR(filtered_depth[idx], gt_depth[idx], 1e-6); ASSERT_EQ(filtered_labels[idx], gt_labels[idx]); } } From 7306d1df703d2baf0b98f7aac17e3879d6eb0549 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 4 May 2020 15:42:54 +0200 Subject: [PATCH 267/612] llvm-namespace-comment --- .../moveit/background_processing/background_processing.h | 4 ++-- moveit_core/backtrace/include/moveit/backtrace/backtrace.h | 2 +- .../collision_detection/allvalid/collision_env_allvalid.h | 2 +- .../include/moveit/collision_detection/collision_common.h | 4 ++-- .../collision_detection/collision_detector_allocator.h | 2 +- .../include/moveit/collision_detection/collision_env.h | 2 +- .../include/moveit/collision_detection/collision_matrix.h | 4 ++-- .../moveit/collision_detection/collision_octomap_filter.h | 2 +- .../include/moveit/collision_detection/collision_tools.h | 2 +- .../include/moveit/collision_detection/world.h | 2 +- .../include/moveit/collision_detection/world_diff.h | 2 +- .../collision_detection_bullet/collision_env_bullet.h | 2 +- .../moveit/collision_detection_fcl/collision_common.h | 2 +- .../moveit/collision_detection_fcl/collision_env_fcl.h | 2 +- .../include/moveit/collision_detection_fcl/fcl_compat.h | 2 +- .../collision_common_distance_field.h | 2 +- .../collision_distance_field_types.h | 2 +- .../collision_distance_field/collision_env_distance_field.h | 2 +- .../moveit/collision_distance_field/collision_env_hybrid.h | 2 +- .../include/moveit/constraint_samplers/constraint_sampler.h | 2 +- .../constraint_samplers/constraint_sampler_allocator.h | 2 +- .../moveit/constraint_samplers/constraint_sampler_manager.h | 2 +- .../moveit/constraint_samplers/constraint_sampler_tools.h | 2 +- .../constraint_samplers/default_constraint_samplers.h | 2 +- .../moveit/constraint_samplers/union_constraint_sampler.h | 2 +- moveit_core/constraint_samplers/test/pr2_arm_ik.h | 2 +- .../constraint_samplers/test/pr2_arm_kinematics_plugin.h | 2 +- .../include/moveit/controller_manager/controller_manager.h | 2 +- .../include/moveit/distance_field/find_internal_points.h | 2 +- .../moveit/distance_field/propagation_distance_field.h | 2 +- .../include/moveit/dynamics_solver/dynamics_solver.h | 2 +- .../exceptions/include/moveit/exceptions/exceptions.h | 2 +- .../moveit/kinematic_constraints/kinematic_constraint.h | 2 +- .../include/moveit/kinematic_constraints/utils.h | 2 +- .../include/moveit/kinematics_base/kinematics_base.h | 6 +++--- .../include/moveit/kinematics_metrics/kinematics_metrics.h | 2 +- .../include/moveit/planning_interface/planning_interface.h | 2 +- .../include/moveit/planning_interface/planning_response.h | 2 +- .../planning_request_adapter/planning_request_adapter.h | 2 +- .../include/moveit/planning_scene/planning_scene.h | 2 +- moveit_core/profiler/include/moveit/profiler/profiler.h | 4 ++-- moveit_core/robot_model/include/moveit/robot_model/aabb.h | 2 +- .../include/moveit/robot_model/fixed_joint_model.h | 4 ++-- .../include/moveit/robot_model/floating_joint_model.h | 4 ++-- .../robot_model/include/moveit/robot_model/joint_model.h | 4 ++-- .../include/moveit/robot_model/joint_model_group.h | 4 ++-- .../robot_model/include/moveit/robot_model/link_model.h | 4 ++-- .../include/moveit/robot_model/planar_joint_model.h | 4 ++-- .../include/moveit/robot_model/prismatic_joint_model.h | 4 ++-- .../include/moveit/robot_model/revolute_joint_model.h | 4 ++-- .../robot_model/include/moveit/robot_model/robot_model.h | 4 ++-- moveit_core/robot_model/src/order_robot_model_items.inc | 6 +++--- .../robot_state/include/moveit/robot_state/attached_body.h | 4 ++-- .../robot_state/include/moveit/robot_state/conversions.h | 4 ++-- .../robot_state/include/moveit/robot_state/robot_state.h | 4 ++-- .../include/moveit/robot_trajectory/robot_trajectory.h | 2 +- .../include/moveit/sensor_manager/sensor_manager.h | 2 +- .../iterative_spline_parameterization.h | 2 +- .../trajectory_processing/iterative_time_parameterization.h | 2 +- .../transforms/include/moveit/transforms/transforms.h | 4 ++-- moveit_core/utils/include/moveit/utils/lexical_casts.h | 4 ++-- moveit_core/utils/include/moveit/utils/message_checks.h | 4 ++-- .../utils/include/moveit/utils/robot_model_test_utils.h | 4 ++-- moveit_core/utils/include/moveit/utils/xmlrpc_casts.h | 4 ++-- .../include/moveit_jog_arm/collision_check_thread.h | 2 +- .../moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h | 2 +- .../moveit_jog_arm/include/moveit_jog_arm/status_codes.h | 2 +- .../cached_ik_kinematics_plugin-inl.h | 2 +- .../cached_ik_kinematics_plugin.h | 2 +- .../cached_ik_kinematics_plugin/detail/GreedyKCenters.h | 2 +- .../cached_ik_kinematics_plugin/detail/NearestNeighbors.h | 2 +- .../detail/NearestNeighborsGNAT.h | 2 +- .../kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp | 2 +- .../include/moveit/kdl_kinematics_plugin/joint_mimic.hpp | 2 +- .../moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h | 2 +- .../moveit/lma_kinematics_plugin/lma_kinematics_plugin.h | 2 +- .../moveit/srv_kinematics_plugin/srv_kinematics_plugin.h | 2 +- .../include/chomp_interface/chomp_interface.h | 2 +- .../include/chomp_motion_planner/chomp_optimizer.h | 2 +- .../include/chomp_motion_planner/chomp_planner.h | 2 +- .../include/chomp_motion_planner/chomp_trajectory.h | 2 +- .../include/chomp_motion_planner/multivariate_gaussian.h | 2 +- .../moveit/ompl_interface/detail/constrained_goal_sampler.h | 2 +- .../moveit/ompl_interface/detail/constrained_sampler.h | 2 +- .../ompl_interface/detail/constrained_valid_state_sampler.h | 2 +- .../moveit/ompl_interface/detail/constraints_library.h | 2 +- .../include/moveit/ompl_interface/detail/goal_union.h | 2 +- .../moveit/ompl_interface/detail/projection_evaluators.h | 2 +- .../moveit/ompl_interface/detail/state_validity_checker.h | 2 +- .../moveit/ompl_interface/detail/threadsafe_state_storage.h | 2 +- .../include/moveit/ompl_interface/ompl_interface.h | 2 +- .../parameterization/joint_space/joint_model_state_space.h | 2 +- .../joint_space/joint_model_state_space_factory.h | 2 +- .../parameterization/model_based_state_space_factory.h | 2 +- .../parameterization/work_space/pose_model_state_space.h | 2 +- .../work_space/pose_model_state_space_factory.h | 2 +- .../moveit/ompl_interface/planning_context_manager.h | 2 +- .../src/moveit_fake_controllers.h | 2 +- .../include/moveit/benchmarks/BenchmarkExecutor.h | 2 +- .../benchmarks/include/moveit/benchmarks/BenchmarkOptions.h | 2 +- .../src/pick_place_action_capability.h | 2 +- .../moveit/pick_place/approach_and_translate_stage.h | 2 +- .../include/moveit/pick_place/manipulation_pipeline.h | 2 +- .../include/moveit/pick_place/manipulation_plan.h | 2 +- .../include/moveit/pick_place/manipulation_stage.h | 2 +- .../pick_place/include/moveit/pick_place/pick_place.h | 2 +- .../include/moveit/pick_place/pick_place_params.h | 2 +- .../pick_place/include/moveit/pick_place/plan_stage.h | 2 +- .../include/moveit/pick_place/reachable_valid_pose_filter.h | 2 +- .../move_group/include/moveit/move_group/capability_names.h | 2 +- .../include/moveit/move_group/move_group_capability.h | 2 +- .../include/moveit/move_group/move_group_context.h | 2 +- .../apply_planning_scene_service_capability.h | 2 +- .../cartesian_path_service_capability.h | 2 +- .../default_capabilities/clear_octomap_service_capability.h | 2 +- .../default_capabilities/kinematics_service_capability.h | 2 +- .../src/default_capabilities/move_action_capability.h | 2 +- .../src/default_capabilities/plan_service_capability.h | 2 +- .../query_planners_service_capability.h | 2 +- .../state_validation_service_capability.h | 2 +- .../src/default_capabilities/tf_publisher_capability.h | 2 +- .../include/moveit/occupancy_map_monitor/occupancy_map.h | 2 +- .../moveit/occupancy_map_monitor/occupancy_map_monitor.h | 2 +- .../moveit/occupancy_map_monitor/occupancy_map_updater.h | 2 +- .../depth_image_octomap_updater.h | 2 +- .../lazy_free_space_updater/lazy_free_space_updater.h | 2 +- .../mesh_filter/include/moveit/mesh_filter/filter_job.h | 2 +- .../include/moveit/point_containment_filter/shape_mask.h | 2 +- .../pointcloud_octomap_updater/pointcloud_octomap_updater.h | 2 +- .../include/moveit/semantic_world/semantic_world.h | 4 ++-- .../constraint_sampler_manager_loader.h | 2 +- .../kinematics_plugin_loader/kinematics_plugin_loader.h | 2 +- .../include/moveit/plan_execution/plan_execution.h | 2 +- .../include/moveit/plan_execution/plan_representation.h | 2 +- .../include/moveit/plan_execution/plan_with_sensing.h | 2 +- .../include/moveit/planning_pipeline/planning_pipeline.h | 2 +- .../moveit/planning_scene_monitor/current_state_monitor.h | 2 +- .../moveit/planning_scene_monitor/planning_scene_monitor.h | 2 +- .../moveit/planning_scene_monitor/trajectory_monitor.h | 2 +- .../rdf_loader/include/moveit/rdf_loader/rdf_loader.h | 2 +- .../include/moveit/robot_model_loader/robot_model_loader.h | 2 +- .../trajectory_execution_manager.h | 2 +- .../test/test_moveit_controller_manager.h | 2 +- .../common_planning_interface_objects/common_objects.h | 2 +- .../planning_scene_interface/planning_scene_interface.h | 4 ++-- .../include/moveit/py_bindings_tools/py_conversions.h | 4 ++-- .../include/moveit/py_bindings_tools/roscpp_initializer.h | 4 ++-- .../include/moveit/robot_interaction/interaction.h | 4 ++-- .../include/moveit/robot_interaction/interaction_handler.h | 2 +- .../moveit/robot_interaction/interactive_marker_helpers.h | 2 +- .../include/moveit/robot_interaction/kinematic_options.h | 2 +- .../moveit/robot_interaction/kinematic_options_map.h | 2 +- .../include/moveit/robot_interaction/locked_robot_state.h | 2 +- .../include/moveit/robot_interaction/robot_interaction.h | 2 +- .../motion_planning_rviz_plugin/motion_planning_display.h | 2 +- .../motion_planning_rviz_plugin/motion_planning_frame.h | 2 +- .../motion_planning_frame_joints_widget.h | 2 +- .../motion_planning_param_widget.h | 2 +- .../planning_scene_rviz_plugin/planning_scene_display.h | 2 +- .../moveit/rviz_plugin_render_tools/octomap_render.h | 2 +- .../moveit/rviz_plugin_render_tools/planning_link_updater.h | 2 +- .../moveit/rviz_plugin_render_tools/planning_scene_render.h | 2 +- .../include/moveit/rviz_plugin_render_tools/render_shapes.h | 2 +- .../rviz_plugin_render_tools/robot_state_visualization.h | 2 +- .../rviz_plugin_render_tools/trajectory_visualization.h | 2 +- .../include/moveit/warehouse/constraints_storage.h | 2 +- .../include/moveit/warehouse/moveit_message_storage.h | 2 +- .../include/moveit/warehouse/planning_scene_storage.h | 2 +- .../include/moveit/warehouse/planning_scene_world_storage.h | 2 +- .../warehouse/include/moveit/warehouse/state_storage.h | 2 +- .../moveit/warehouse/trajectory_constraints_storage.h | 2 +- .../include/moveit/warehouse/warehouse_connector.h | 2 +- .../setup_assistant/tools/compute_default_collisions.h | 2 +- .../moveit/setup_assistant/tools/moveit_config_data.h | 2 +- moveit_setup_assistant/src/tools/rotated_header_view.h | 2 +- moveit_setup_assistant/src/widgets/controller_edit_widget.h | 2 +- .../src/widgets/default_collisions_widget.h | 2 +- moveit_setup_assistant/src/widgets/end_effectors_widget.h | 2 +- moveit_setup_assistant/src/widgets/group_edit_widget.h | 2 +- moveit_setup_assistant/src/widgets/header_widget.h | 2 +- moveit_setup_assistant/src/widgets/kinematic_chain_widget.h | 2 +- moveit_setup_assistant/src/widgets/navigation_widget.h | 2 +- moveit_setup_assistant/src/widgets/passive_joints_widget.h | 2 +- moveit_setup_assistant/src/widgets/planning_groups_widget.h | 2 +- moveit_setup_assistant/src/widgets/robot_poses_widget.h | 2 +- moveit_setup_assistant/src/widgets/setup_assistant_widget.h | 2 +- moveit_setup_assistant/src/widgets/start_screen_widget.h | 2 +- moveit_setup_assistant/src/widgets/virtual_joints_widget.h | 2 +- 188 files changed, 218 insertions(+), 218 deletions(-) diff --git a/moveit_core/background_processing/include/moveit/background_processing/background_processing.h b/moveit_core/background_processing/include/moveit/background_processing/background_processing.h index b87a82f997..3ecb98b526 100644 --- a/moveit_core/background_processing/include/moveit/background_processing/background_processing.h +++ b/moveit_core/background_processing/include/moveit/background_processing/background_processing.h @@ -110,5 +110,5 @@ class BackgroundProcessing : private boost::noncopyable void processingThread(); }; -} -} +} // namespace tools +} // namespace moveit diff --git a/moveit_core/backtrace/include/moveit/backtrace/backtrace.h b/moveit_core/backtrace/include/moveit/backtrace/backtrace.h index 224c0fe2dc..c00af92322 100644 --- a/moveit_core/backtrace/include/moveit/backtrace/backtrace.h +++ b/moveit_core/backtrace/include/moveit/backtrace/backtrace.h @@ -61,4 +61,4 @@ void get_backtrace(std::ostream& out) out << "Unable to get backtrace with the used compiler." << std::endl; } #endif -} +} // namespace moveit diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h index 2f0e91c88f..50b543d837 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h @@ -73,4 +73,4 @@ class CollisionEnvAllValid : public CollisionEnv void distanceSelf(const DistanceRequest& req, DistanceResult& res, const moveit::core::RobotState& state) const override; }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 38a3fef260..35ceecc200 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -64,7 +64,7 @@ enum Type /** \brief A body in the environment */ WORLD_OBJECT }; -} +} // namespace BodyTypes /** \brief The types of bodies that are considered for collision */ typedef BodyTypes::Type BodyType; @@ -392,4 +392,4 @@ struct DistanceResult distances.clear(); } }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h index 89624d9e09..f428066b42 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h @@ -98,4 +98,4 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator return CollisionDetectorAllocatorPtr(new CollisionDetectorAllocatorType()); } }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h index 7d42a73e3f..0b76908fac 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h @@ -324,4 +324,4 @@ class CollisionEnv WorldPtr world_; // The world always valid, never nullptr. WorldConstPtr world_const_; // always same as world_ }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index 1b9e36e36e..e83283688c 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -65,7 +65,7 @@ enum Type imply that the two bodies are in collision*/ CONDITIONAL }; -} +} // namespace AllowedCollision /** \brief Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type is * CONDITIONAL) */ @@ -276,4 +276,4 @@ class AllowedCollisionMatrix std::map default_entries_; std::map default_allowed_contacts_; }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h index ed36a81113..36069f95a5 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h @@ -60,4 +60,4 @@ namespace collision_detection int refineContactNormals(const World::ObjectConstPtr& object, CollisionResult& res, double cell_bbx_search_distance = 1.0, double allowed_angle_divergence = 0.0, bool estimate_depth = false, double iso_value = 0.5, double metaball_radius_multiple = 1.5); -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h index 8b06686148..bf7e2430bc 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h @@ -70,4 +70,4 @@ bool getSensorPositioning(geometry_msgs::Point& point, const std::set observers_; }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h index 84ae6a522c..071098d2f9 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h @@ -124,4 +124,4 @@ class WorldDiff /* used to unregister the notifier */ WorldWeakPtr world_; }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h index bfaee98429..d976448a2c 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h @@ -145,4 +145,4 @@ class CollisionEnvBullet : public CollisionEnv World::ObserverHandle observer_handle_; }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index 66beae404d..cde0730291 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -360,4 +360,4 @@ inline void fcl2costsource(const fcl::CostSourced& fcs, CostSource& cs) cs.cost = fcs.cost_density; } -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h index 5c9674cf32..a6c261ec39 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h @@ -160,4 +160,4 @@ class CollisionEnvFCL : public CollisionEnv World::ObserverHandle observer_handle_; }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h index 123fac3c08..7c0cc3334f 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h @@ -95,5 +95,5 @@ class OBBRSS; using OBBRSSd = fcl::OBBRSS; class DynamicAABBTreeCollisionManager; using DynamicAABBTreeCollisionManagerd = fcl::DynamicAABBTreeCollisionManager; -} +} // namespace fcl #endif diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h index 70cc449dfc..fe4567d561 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h @@ -178,4 +178,4 @@ PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const mov void getBodySphereVisualizationMarkers(const GroupStateRepresentationPtr& gsr, const std::string& reference_frame, visualization_msgs::MarkerArray& body_marker_array); -} +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index c9b7ae066c..627a0a790a 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -526,4 +526,4 @@ void getCollisionMarkers(const std::string& frame_id, const std::string& ns, con const std::vector& posed_decompositions, const std::vector& posed_vector_decompositions, const std::vector& gradients, visualization_msgs::MarkerArray& arr); -} +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h index 5d7725faf3..93cd63ef2f 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h @@ -306,4 +306,4 @@ class CollisionEnvDistanceField : public CollisionEnv GroupStateRepresentationPtr last_gsr_; World::ObserverHandle observer_handle_; }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h index eb98812a43..14d2a22a18 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h @@ -150,4 +150,4 @@ class CollisionEnvHybrid : public collision_detection::CollisionEnvFCL protected: CollisionEnvDistanceFieldPtr cenv_distance_; }; -}; +} // namespace collision_detection diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h index 6b2074b390..c7d24b3741 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h @@ -294,4 +294,4 @@ class ConstraintSampler moveit::core::GroupStateValidityCallbackFn group_state_validity_callback_; bool verbose_; ///< True if verbosity is on }; -} +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h index 89db5e830e..eb3d07fa07 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h @@ -60,4 +60,4 @@ class ConstraintSamplerAllocator virtual bool canService(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, const moveit_msgs::Constraints& constr) const = 0; }; -} +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h index 630e307c97..cfcd2f2ffe 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h @@ -142,4 +142,4 @@ class ConstraintSamplerManager std::vector sampler_alloc_; /**< \brief Holds the constraint sampler allocators, which will be tested in order */ }; -} +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h index 1bc811c4e3..54b392da54 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h @@ -53,4 +53,4 @@ double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const moveit:: double countSamplesPerSecond(const moveit_msgs::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, const std::string& group); -} +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h index ffc4b5ea8d..cdd5ae152d 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h @@ -524,4 +524,4 @@ class IKConstraintSampler : public ConstraintSampler frame of the end effector */ Eigen::Isometry3d eef_to_ik_tip_transform_; /**< \brief Holds the transformation from end effector to IK tip frame */ }; -} +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h index 52b696df30..9c46104e76 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h @@ -167,4 +167,4 @@ class UnionConstraintSampler : public ConstraintSampler protected: std::vector samplers_; /**< \brief Holder for sorted internal list of samplers*/ }; -} +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.h b/moveit_core/constraint_samplers/test/pr2_arm_ik.h index 46d4a7ef32..e27552a098 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.h @@ -188,4 +188,4 @@ class PR2ArmIK std::vector continuous_joint_; }; -} +} // namespace pr2_arm_kinematics diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index da5be7edc3..f03c56525c 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -257,4 +257,4 @@ class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase void jointSolutionCallback(const KDL::JntArray& jnt_array, const KDL::Frame& ik_pose, moveit_msgs::MoveItErrorCodes& error_code) const; }; -} +} // namespace pr2_arm_kinematics diff --git a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h index 7af922b3bc..18490b75d8 100644 --- a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h +++ b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h @@ -207,4 +207,4 @@ class MoveItControllerManager virtual bool switchControllers(const std::vector& activate, const std::vector& deactivate) = 0; }; -} +} // namespace moveit_controller_manager diff --git a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h index 27363466e1..f03a42d39f 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h +++ b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h @@ -52,4 +52,4 @@ namespace distance_field * vector. */ void findInternalPointsConvex(const bodies::Body& body, double resolution, EigenSTL::vector_Vector3d& points); -} +} // namespace distance_field diff --git a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h index a278a7f6d6..7dd4971421 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h @@ -609,4 +609,4 @@ inline double PropagationDistanceField::getDistance(const PropDistanceFieldVoxel { return sqrt_table_[object.distance_square_] - sqrt_table_[object.negative_distance_square_]; } -} +} // namespace distance_field diff --git a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h index c8402b4031..6e46a929c5 100644 --- a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h +++ b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h @@ -149,4 +149,4 @@ class DynamicsSolver double gravity_; // Norm of the gravity vector passed in initialize() }; -} +} // namespace dynamics_solver diff --git a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h index 34394e987c..1e4d28ddb9 100644 --- a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h +++ b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h @@ -54,4 +54,4 @@ class Exception : public std::runtime_error public: explicit Exception(const std::string& what_arg); }; -} +} // namespace moveit diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index cf28da8858..ad810d306f 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -1067,4 +1067,4 @@ class KinematicConstraintSet internal visibility constraints */ moveit_msgs::Constraints all_constraints_; /**< \brief Messages corresponding to all internal constraints */ }; -} +} // namespace kinematic_constraints diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h index 3396520631..1828077063 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h @@ -214,4 +214,4 @@ bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::Constraints& * @param [in] constraints The constraint to resolve. */ bool resolveConstraintFrames(const moveit::core::RobotState& state, moveit_msgs::Constraints& constraints); -} +} // namespace kinematic_constraints diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index a3dae858a2..7e2e47aaf5 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -75,7 +75,7 @@ enum DiscretizationMethod SOME_RANDOM_SAMPLED /**< the discretization for some redundant joint will be randomly generated. The unused redundant joints will be fixed at their current value. */ }; -} +} // namespace DiscretizationMethods typedef DiscretizationMethods::DiscretizationMethod DiscretizationMethod; /* @@ -97,7 +97,7 @@ enum KinematicError NO_SOLUTION /**< A valid joint solution that can reach this pose(s) could not be found */ }; -} +} // namespace KinematicErrors typedef KinematicErrors::KinematicError KinematicError; /** @@ -663,4 +663,4 @@ class KinematicsBase private: std::string removeSlash(const std::string& str) const; }; -}; +} // namespace kinematics diff --git a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h index 27e111fdf1..5a8f61c32a 100644 --- a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h +++ b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h @@ -156,4 +156,4 @@ class KinematicsMetrics double penalty_multiplier_; }; -} +} // namespace kinematics_metrics diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index 4c583a922c..5d785abc96 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -211,4 +211,4 @@ class PlannerManager PlannerConfigurationMap config_settings_; }; -} // planning_interface +} // namespace planning_interface diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h index e05209921d..6347a3d70f 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h @@ -66,4 +66,4 @@ struct MotionPlanDetailedResponse moveit_msgs::MoveItErrorCodes error_code_; }; -} // planning_interface +} // namespace planning_interface diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h index daf9426ba0..ab7f69b8c9 100644 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h @@ -119,4 +119,4 @@ class PlanningRequestAdapterChain private: std::vector adapters_; }; -} +} // namespace planning_request_adapter diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 0c48ababfb..ce2f166ac0 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -1044,4 +1044,4 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from // a map of object types std::unique_ptr object_types_; }; -} +} // namespace planning_scene diff --git a/moveit_core/profiler/include/moveit/profiler/profiler.h b/moveit_core/profiler/include/moveit/profiler/profiler.h index 554ec2b755..96a13881b2 100644 --- a/moveit_core/profiler/include/moveit/profiler/profiler.h +++ b/moveit_core/profiler/include/moveit/profiler/profiler.h @@ -310,8 +310,8 @@ class Profiler : private boost::noncopyable bool running_; bool printOnDestroy_; }; -} -} +} // namespace tools +} // namespace moveit #else diff --git a/moveit_core/robot_model/include/moveit/robot_model/aabb.h b/moveit_core/robot_model/include/moveit/robot_model/aabb.h index 5c1b84df13..997399675e 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/aabb.h +++ b/moveit_core/robot_model/include/moveit/robot_model/aabb.h @@ -50,4 +50,4 @@ class AABB : public Eigen::AlignedBox3d void extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box); }; } -} +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h index d10b1622a7..bf80d6a1a1 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h @@ -65,5 +65,5 @@ class FixedJointModel : public JointModel void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; }; -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h index 6d721d9bf5..73cd5b7a6c 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h @@ -88,5 +88,5 @@ class FloatingJointModel : public JointModel private: double angular_distance_weight_; }; -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index aa07321d73..3cb25d946c 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -518,5 +518,5 @@ class JointModel /** \brief Operator overload for printing variable bounds to a stream */ std::ostream& operator<<(std::ostream& out, const VariableBounds& b); -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index 108e1a5ab3..e2f1978f94 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -742,5 +742,5 @@ class JointModelGroup /** \brief The names of the default states specified for this group in the SRDF */ std::vector default_states_names_; }; -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.h b/moveit_core/robot_model/include/moveit/robot_model/link_model.h index d586dfded0..65cbf925e5 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/link_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.h @@ -280,5 +280,5 @@ class LinkModel /** \brief Index of the transform for this link in the full robot frame */ int link_index_; }; -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h index 3524250e41..747cc2c2ff 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h @@ -83,5 +83,5 @@ class PlanarJointModel : public JointModel private: double angular_distance_weight_; }; -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h index 87678c55a5..c3c5d0ac81 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h @@ -83,5 +83,5 @@ class PrismaticJointModel : public JointModel /** \brief The axis of the joint */ Eigen::Vector3d axis_; }; -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h index 76322c2b04..6d3b080408 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h @@ -94,5 +94,5 @@ class RevoluteJointModel : public JointModel private: double x2_, y2_, z2_, xy_, xz_, yz_; }; -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 835c675c04..8c8ffdaf3a 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -615,8 +615,8 @@ class RobotModel /** \brief Given a geometry spec from the URDF and a filename (for a mesh), construct the corresponding shape object*/ shapes::ShapePtr constructShape(const urdf::Geometry* geom); }; -} -} +} // namespace core +} // namespace moveit namespace robot_model = moveit::core; namespace robot_state = moveit::core; diff --git a/moveit_core/robot_model/src/order_robot_model_items.inc b/moveit_core/robot_model/src/order_robot_model_items.inc index 8c6a261773..c28073f078 100644 --- a/moveit_core/robot_model/src/order_robot_model_items.inc +++ b/moveit_core/robot_model/src/order_robot_model_items.inc @@ -65,6 +65,6 @@ struct OrderGroupsByName } }; -} -} -} \ No newline at end of file +} // namespace +} // namespace core +} // namespace moveit \ No newline at end of file diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index 7086d00120..cf93dc0c02 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -188,5 +188,5 @@ class AttachedBody /** \brief Transforms to subframes on the object, relative to the model frame. */ moveit::core::FixedTransformsMap global_subframe_poses_; }; -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_state/include/moveit/robot_state/conversions.h b/moveit_core/robot_state/include/moveit/robot_state/conversions.h index e5d1017d33..43968a2e83 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/conversions.h +++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.h @@ -140,5 +140,5 @@ void robotStateToStream(const RobotState& state, std::ostream& out, * \return true on success */ void streamToRobotState(RobotState& state, const std::string& line, const std::string& separator = ","); -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 00004d0247..cc1b6c5353 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1844,5 +1844,5 @@ class RobotState /** \brief Operator overload for printing variable bounds to a stream */ std::ostream& operator<<(std::ostream& out, const RobotState& s); -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 69ff8e0eca..6961c1c6c0 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -272,4 +272,4 @@ class RobotTrajectory std::deque waypoints_; std::deque duration_from_previous_; }; -} +} // namespace robot_trajectory diff --git a/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h b/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h index 222e703504..34dc28fb0e 100644 --- a/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h +++ b/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h @@ -101,4 +101,4 @@ class MoveItSensorManager virtual bool pointSensorTo(const std::string& name, const geometry_msgs::PointStamped& target, moveit_msgs::RobotTrajectory& sensor_trajectory) = 0; }; -} +} // namespace moveit_sensor_manager diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h index 67f4683047..9ecf66e823 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h @@ -83,4 +83,4 @@ class IterativeSplineParameterization bool add_points_; /// @brief If true, add two points to trajectory (first and last segments). /// If false, move the 2nd and 2nd-last points. }; -} +} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h index 64e3d2b5b9..a336c83b5b 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h @@ -67,4 +67,4 @@ class IterativeParabolicTimeParameterization double findT1(const double d1, const double d2, double t1, const double t2, const double a_max) const; double findT2(const double d1, const double d2, const double t1, double t2, const double a_max) const; }; -} +} // namespace trajectory_processing diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 6833b606aa..cd90bdff10 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -197,5 +197,5 @@ class Transforms : private boost::noncopyable std::string target_frame_; FixedTransformsMap transforms_map_; }; -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/include/moveit/utils/lexical_casts.h b/moveit_core/utils/include/moveit/utils/lexical_casts.h index 3dc0e84ab8..8b3496b028 100644 --- a/moveit_core/utils/include/moveit/utils/lexical_casts.h +++ b/moveit_core/utils/include/moveit/utils/lexical_casts.h @@ -65,5 +65,5 @@ double toDouble(const std::string& s); \throws std::runtime_exception if not a valid number */ float toFloat(const std::string& s); -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/include/moveit/utils/message_checks.h b/moveit_core/utils/include/moveit/utils/message_checks.h index 4ac3c0af8d..ffb12c8292 100644 --- a/moveit_core/utils/include/moveit/utils/message_checks.h +++ b/moveit_core/utils/include/moveit/utils/message_checks.h @@ -61,5 +61,5 @@ bool isEmpty(const moveit_msgs::RobotState& msg); /** \brief Check if a message specifies constraints */ bool isEmpty(const moveit_msgs::Constraints& msg); -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h index 652d7111b6..a22d6c635a 100644 --- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h @@ -193,5 +193,5 @@ class RobotModelBuilder /// Whether the current builder state is valid. If any 'add' method fails, this becomes false. bool is_valid_ = true; }; -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h b/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h index 2ffa863b3c..839256bfb2 100644 --- a/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h +++ b/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h @@ -60,5 +60,5 @@ bool isArray(XmlRpc::XmlRpcValue& v, size_t size = 0, const std::string& name = * @param name: if non-empty, print a warning message "name is not a struct with keys ..." */ bool isStruct(XmlRpc::XmlRpcValue& v, const std::vector& keys = {}, const std::string& name = ""); -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index 24f2c52b31..8fbf5f2d50 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -68,4 +68,4 @@ class CollisionCheckThread // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; }; -} +} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index ec2f69794e..1cb737d8f6 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -135,4 +135,4 @@ struct JogArmParameters bool publish_joint_velocities; bool publish_joint_accelerations; }; -} +} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h index 1c6d264696..85660cd50b 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h @@ -60,4 +60,4 @@ const std::unordered_map { DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, { HALT_FOR_COLLISION, "Collision detected, emergency stop" }, { JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }); -} // end namespace trackjoint +} // namespace moveit_jog_arm diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.h index 045b2190a0..82a88c20d9 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.h @@ -239,4 +239,4 @@ bool CachedMultiTipIKKinematicsPlugin::searchPositionIK( CachedIKKinematicsPlugin::cache_.updateCache(nearest, poses, solution); return solution_found; } -} +} // namespace cached_ik_kinematics_plugin diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h index 86687f3164..43e38e0dd4 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h @@ -344,6 +344,6 @@ class CachedMultiTipIKKinematicsPlugin : public CachedIKKinematicsPlugin // \brief Cache of removed elements. std::unordered_set removed_; }; -} +} // namespace cached_ik_kinematics_plugin diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp index 34292a9262..72cfd99a27 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp @@ -114,4 +114,4 @@ class ChainIkSolverVelMimicSVD : public ChainIkSolverVel Jacobian jac_; // full Jacobian Jacobian jac_reduced_; // reduced Jacobian with contributions of mimic joints mapped onto active DoFs }; -} +} // namespace KDL diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp index 45ad0a0c47..6d64b63edc 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp @@ -69,4 +69,4 @@ class JointMimic active = false; } }; -} +} // namespace kdl_kinematics_plugin diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h index 70146c5afa..16f381ed65 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h @@ -177,4 +177,4 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase * = 0.0: perform position-only IK */ double orientation_vs_position_weight_; }; -} +} // namespace kdl_kinematics_plugin diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h index ba81d148ce..a25d154d57 100644 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h +++ b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h @@ -162,4 +162,4 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase * = 0.0: perform position-only IK */ double orientation_vs_position_weight_; }; -} +} // namespace lma_kinematics_plugin diff --git a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h index 2adad1d763..4ae5dfb01e 100644 --- a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h +++ b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h @@ -151,4 +151,4 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase std::shared_ptr ik_service_client_; }; -} +} // namespace srv_kinematics_plugin diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h index 464da4ab92..e742d0455b 100644 --- a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h @@ -62,4 +62,4 @@ class CHOMPInterface : public chomp::ChompPlanner chomp::ChompParameters params_; }; -} +} // namespace chomp_interface diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h index dafda7f0b2..ea01bad078 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h @@ -216,4 +216,4 @@ class ChompOptimizer void computeJointProperties(int trajectoryPoint); bool isCurrentTrajectoryMeshToMeshCollisionFree() const; }; -} +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h index a4d85fb673..b47308edde 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h @@ -53,4 +53,4 @@ class ChompPlanner const planning_interface::MotionPlanRequest& req, const ChompParameters& params, planning_interface::MotionPlanDetailedResponse& res) const; }; -} +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h index 559237aa12..10bd896438 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h @@ -306,4 +306,4 @@ inline double ChompTrajectory::getDuration() const { return duration_; } -} +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h index a1935d640e..45b265474b 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h @@ -91,4 +91,4 @@ void MultivariateGaussian::sample(Eigen::MatrixBase& output) output(i) = gaussian_(); output = mean_ + covariance_cholesky_ * output; } -} +} // namespace chomp diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h index 3eff4f9787..a982a0215f 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h @@ -71,4 +71,4 @@ class ConstrainedGoalSampler : public ompl::base::GoalLazySamples bool warned_invalid_samples_; unsigned int verbose_display_; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h index b2195591bb..884b82e4e9 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h @@ -76,4 +76,4 @@ class ConstrainedSampler : public ompl::base::StateSampler unsigned int constrained_failure_; double inv_dim_; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h index e0b76d6309..7b73f60e0a 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h @@ -68,4 +68,4 @@ class ValidConstrainedSampler : public ompl::base::ValidStateSampler double inv_dim_; ompl::RNG rng_; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h index 4e93e0ff8b..46b97ba692 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h @@ -199,4 +199,4 @@ class ConstraintsLibrary ModelBasedPlanningContext* context_; std::map constraint_approximations_; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h index 00cf1d830f..a0daa1cf6b 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h @@ -81,4 +81,4 @@ class GoalSampleableRegionMux : public ompl::base::GoalSampleableRegion std::vector goals_; mutable unsigned int gindex_; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h index 0e40f3da4d..c612d22454 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h @@ -82,4 +82,4 @@ class ProjectionEvaluatorJointValue : public ompl::base::ProjectionEvaluator private: std::vector variables_; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h index 23e02f87fb..0b462b25f6 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h @@ -87,4 +87,4 @@ class StateValidityChecker : public ompl::base::StateValidityChecker collision_detection::CollisionRequest collision_request_with_cost_; bool verbose_; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h index e896b23e3e..e0a5e6dc7c 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h @@ -56,4 +56,4 @@ class TSStateStorage mutable std::map thread_states_; mutable std::mutex lock_; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h index caf92fab42..5677eea4a0 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h @@ -163,4 +163,4 @@ class OMPLInterface private: constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h index c82897307b..91d8867a10 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h @@ -52,4 +52,4 @@ class JointModelStateSpace : public ModelBasedStateSpace return PARAMETERIZATION_TYPE; } }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h index 10cac453eb..7386856fc6 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h @@ -51,4 +51,4 @@ class JointModelStateSpaceFactory : public ModelBasedStateSpaceFactory protected: ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h index 2ed0f90eb4..4b71b32e0f 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h @@ -73,4 +73,4 @@ class ModelBasedStateSpaceFactory virtual ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const = 0; std::string type_; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h index 5bb996aebd..b0668a6727 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h @@ -139,4 +139,4 @@ class PoseModelStateSpace : public ModelBasedStateSpace std::vector poses_; double jump_factor_; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h index c5b1676ceb..e6c34ebba9 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h @@ -51,4 +51,4 @@ class PoseModelStateSpaceFactory : public ModelBasedStateSpaceFactory protected: ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index 14e1eeb0ea..fc5c71b792 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -261,4 +261,4 @@ class PlanningContextManager MOVEIT_STRUCT_FORWARD(CachedContexts); CachedContextsPtr cached_contexts_; }; -} +} // namespace ompl_interface diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h index 89bf9f6f81..f33ed0656a 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h @@ -122,6 +122,6 @@ class InterpolatingController : public ThreadedController private: ros::WallRate rate_; }; -} +} // namespace moveit_fake_controller_manager #endif diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index e7b50d07bc..6aac9db366 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -227,4 +227,4 @@ class BenchmarkExecutor std::vector query_start_fns_; std::vector query_end_fns_; }; -} +} // namespace moveit_ros_benchmarks diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h index 92c7671529..88caedb796 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h @@ -135,4 +135,4 @@ class BenchmarkOptions moveit_msgs::WorkspaceParameters workspace_; }; -} +} // namespace moveit_ros_benchmarks diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h index 3ba4abdb2f..5987597bc9 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h +++ b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h @@ -99,4 +99,4 @@ class MoveGroupPickPlaceAction : public MoveGroupCapability ros::ServiceClient grasp_planning_service_; }; -} +} // namespace move_group diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h index fc1737229c..8c7014495f 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h @@ -60,4 +60,4 @@ class ApproachAndTranslateStage : public ManipulationStage double max_step_; double jump_factor_; }; -} +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h index 12b9dbf855..f341abf45b 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h @@ -115,4 +115,4 @@ class ManipulationPipeline bool stop_processing_; }; -} +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h index e4df2f2c01..2086af4b2d 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h @@ -141,4 +141,4 @@ struct ManipulationPlan // An id for this plan; this is usually the index of the Grasp / PlaceLocation in the input request std::size_t id_; }; -} +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h index 4b6e1f7b32..d71b930040 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h @@ -82,4 +82,4 @@ class ManipulationStage bool signal_stop_; bool verbose_; }; -} +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h index 1448112f13..ab26f50dae 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h @@ -163,4 +163,4 @@ class PickPlace : private boost::noncopyable, public std::enable_shared_from_thi constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_; }; -} +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h index 08ae38837f..32ef5d4277 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h @@ -50,4 +50,4 @@ struct PickPlaceParams // Get access to a global variable that contains the pick & place params. const PickPlaceParams& GetGlobalPickPlaceParams(); -} +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h index 4810e44edf..7b651e196d 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h @@ -56,4 +56,4 @@ class PlanStage : public ManipulationStage planning_scene::PlanningSceneConstPtr planning_scene_; planning_pipeline::PlanningPipelinePtr planning_pipeline_; }; -} +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h index 09125623fe..0e5ec7b0f3 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h @@ -58,4 +58,4 @@ class ReachableAndValidPoseFilter : public ManipulationStage collision_detection::AllowedCollisionMatrixConstPtr collision_matrix_; constraint_samplers::ConstraintSamplerManagerPtr constraints_sampler_manager_; }; -} +} // namespace pick_place diff --git a/moveit_ros/move_group/include/moveit/move_group/capability_names.h b/moveit_ros/move_group/include/moveit/move_group/capability_names.h index 53f36e07a5..2e5e64153d 100644 --- a/moveit_ros/move_group/include/moveit/move_group/capability_names.h +++ b/moveit_ros/move_group/include/moveit/move_group/capability_names.h @@ -62,4 +62,4 @@ static const std::string APPLY_PLANNING_SCENE_SERVICE_NAME = "apply_planning_scene"; // name of the service that applies a given planning scene static const std::string CLEAR_OCTOMAP_SERVICE_NAME = "clear_octomap"; // name of the service that can be used to clear the octomap -} +} // namespace move_group diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h b/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h index c34ef2363d..633b19be12 100644 --- a/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h +++ b/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h @@ -97,4 +97,4 @@ class MoveGroupCapability std::string capability_name_; MoveGroupContextPtr context_; }; -} +} // namespace move_group diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h index 7808a6f3ea..daeeb838a6 100644 --- a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h +++ b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h @@ -79,4 +79,4 @@ struct MoveGroupContext bool allow_trajectory_execution_; bool debug_; }; -} +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h index 9970cc8fef..858db10d64 100644 --- a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h @@ -57,4 +57,4 @@ class ApplyPlanningSceneService : public MoveGroupCapability ros::ServiceServer service_; }; -} +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h index e90efe670c..aa000aad07 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h @@ -55,4 +55,4 @@ class MoveGroupCartesianPathService : public MoveGroupCapability ros::Publisher display_path_; bool display_computed_paths_; }; -} +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h index 2c5e68b542..b43edafcf2 100644 --- a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h @@ -53,4 +53,4 @@ class ClearOctomapService : public MoveGroupCapability ros::ServiceServer service_; }; -} +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h index 3195f04150..34c2be26e9 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h @@ -61,4 +61,4 @@ class MoveGroupKinematicsService : public MoveGroupCapability ros::ServiceServer fk_service_; ros::ServiceServer ik_service_; }; -} +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.h b/moveit_ros/move_group/src/default_capabilities/move_action_capability.h index 56917e6a67..3f83999841 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.h @@ -69,4 +69,4 @@ class MoveGroupMoveAction : public MoveGroupCapability MoveGroupState move_state_; bool preempt_requested_; }; -} +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h index 6ca33b6791..5fb5bb5a0b 100644 --- a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h @@ -53,4 +53,4 @@ class MoveGroupPlanService : public MoveGroupCapability ros::ServiceServer plan_service_; }; -} +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h index 5d3a8b632f..da31e1ca46 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h @@ -61,4 +61,4 @@ class MoveGroupQueryPlannersService : public MoveGroupCapability ros::ServiceServer get_service_; ros::ServiceServer set_service_; }; -} +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h index 86dc6c8d52..c814974de1 100644 --- a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h @@ -53,4 +53,4 @@ class MoveGroupStateValidationService : public MoveGroupCapability ros::ServiceServer validity_service_; }; -} +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h index 2a15f4d283..9aab273170 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h @@ -56,4 +56,4 @@ class TfPublisher : public MoveGroupCapability std::thread thread_; bool keep_running_; }; -} +} // namespace move_group diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h index 88e8730d6d..cd09da30e8 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h @@ -119,4 +119,4 @@ class OccMapTree : public octomap::OcTree typedef std::shared_ptr OccMapTreePtr; typedef std::shared_ptr OccMapTreeConstPtr; -} +} // namespace occupancy_map_monitor diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h index ece0658606..32da5da893 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h @@ -158,4 +158,4 @@ class OccupancyMapMonitor bool active_; }; -} +} // namespace occupancy_map_monitor diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h index 07c2a0b0c8..edd896ddeb 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h @@ -110,4 +110,4 @@ class OccupancyMapUpdater static void readXmlParam(XmlRpc::XmlRpcValue& params, const std::string& param_name, double* value); static void readXmlParam(XmlRpc::XmlRpcValue& params, const std::string& param_name, unsigned int* value); }; -} +} // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h index 199c79204d..6779854111 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h +++ b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h @@ -105,4 +105,4 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater std::vector filtered_labels_; ros::WallTime last_depth_callback_start_; }; -} +} // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h b/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h index 270ad565f1..64e93612bd 100644 --- a/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h +++ b/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h @@ -87,4 +87,4 @@ class LazyFreeSpaceUpdater boost::thread update_thread_; boost::thread process_thread_; }; -} +} // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h index a817245040..f3331e809c 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h @@ -140,4 +140,4 @@ class FilterJob : public Job private: boost::function exec_; }; -} +} // namespace mesh_filter diff --git a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h index facd1d1319..d7248acded 100644 --- a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h +++ b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h @@ -130,4 +130,4 @@ class ShapeMask ShapeHandle min_handle_; std::map::iterator> used_handles_; }; -} +} // namespace point_containment_filter diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h index 5eba41256d..6053926ae0 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h @@ -103,4 +103,4 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater std::unique_ptr shape_mask_; std::vector mask_; }; -} +} // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h index b53e280328..102780606d 100644 --- a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h +++ b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h @@ -161,5 +161,5 @@ class SemanticWorld ros::Publisher planning_scene_diff_publisher_; }; -} -} +} // namespace semantic_world +} // namespace moveit diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h b/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h index 84246d1851..f7f0f45dbb 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h +++ b/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h @@ -60,4 +60,4 @@ class ConstraintSamplerManagerLoader MOVEIT_CLASS_FORWARD(Helper) HelperPtr impl_; }; -} +} // namespace constraint_sampler_manager_loader diff --git a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h index 809599bf79..f0377b80ba 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h +++ b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h @@ -113,4 +113,4 @@ class KinematicsPluginLoader std::string default_solver_plugin_; double default_solver_timeout_; }; -} +} // namespace kinematics_plugin_loader diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h index 7c93c012fb..274a3ca022 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h @@ -163,4 +163,4 @@ class PlanExecution class DynamicReconfigureImpl; DynamicReconfigureImpl* reconfigure_impl_; }; -} +} // namespace plan_execution diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h index 724414768f..fab59a19a1 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h @@ -86,4 +86,4 @@ struct ExecutableMotionPlan /// The signature of a function that can compute a motion plan typedef boost::function ExecutableMotionPlanComputationFn; -} +} // namespace plan_execution diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h index b0b468cd19..5741e2e223 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h @@ -133,4 +133,4 @@ class PlanWithSensing class DynamicReconfigureImpl; DynamicReconfigureImpl* reconfigure_impl_; }; -} +} // namespace plan_execution diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index 27b14c78dc..e7514f6702 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -195,4 +195,4 @@ class PlanningPipeline }; MOVEIT_CLASS_FORWARD(PlanningPipeline); -} +} // namespace planning_pipeline diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h index ab63281a5e..95292a3634 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h @@ -211,4 +211,4 @@ class CurrentStateMonitor }; MOVEIT_CLASS_FORWARD(CurrentStateMonitor); -} +} // namespace planning_scene_monitor diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index ca9fe70b3a..c7549056b5 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -719,4 +719,4 @@ class LockedPlanningSceneRW : public LockedPlanningSceneRO return planning_scene_monitor_->getPlanningScene(); } }; -} +} // namespace planning_scene_monitor diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h index aff77b560e..429d0163d8 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h @@ -105,4 +105,4 @@ class TrajectoryMonitor std::unique_ptr record_states_thread_; TrajectoryStateAddedCallback state_add_callback_; }; -} +} // namespace planning_scene_monitor diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h index bf506a96bf..d365ac8169 100644 --- a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h @@ -100,4 +100,4 @@ class RDFLoader srdf::ModelSharedPtr srdf_; urdf::ModelInterfaceSharedPtr urdf_; }; -} +} // namespace rdf_loader diff --git a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h index 5307ae2b3e..c3b136d2ea 100644 --- a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h +++ b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h @@ -132,4 +132,4 @@ class RobotModelLoader rdf_loader::RDFLoaderPtr rdf_loader_; kinematics_plugin_loader::KinematicsPluginLoaderPtr kinematics_loader_; }; -} +} // namespace robot_model_loader diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index 7a143dabf2..269422c203 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -361,4 +361,4 @@ class TrajectoryExecutionManager double execution_velocity_scaling_; bool wait_for_trajectory_completion_; }; -} +} // namespace trajectory_execution_manager diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h index 732457e801..7681fe51ad 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h @@ -159,4 +159,4 @@ class TestMoveItControllerManager : public moveit_controller_manager::MoveItCont std::map controllers_; std::map > controller_joints_; }; -} +} // namespace test_moveit_controller_manager diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h index e6e62f60ca..d457714c84 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h +++ b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h @@ -70,5 +70,5 @@ planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer, const ros::NodeHandle& nh); -} // namespace planning interface +} // namespace planning_interface } // namespace moveit diff --git a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index 004ecade7f..6d872eb116 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h @@ -148,5 +148,5 @@ class PlanningSceneInterface class PlanningSceneInterfaceImpl; PlanningSceneInterfaceImpl* impl_; }; -} -} +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h index 47825360ba..acbbde0d2b 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h @@ -92,5 +92,5 @@ boost::python::list listFromString(const std::vector& v) { return listFromType(v); } -} -} +} // namespace py_bindings_tools +} // namespace moveit diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h index 1effb467a5..060d84164c 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h @@ -70,5 +70,5 @@ void roscpp_init(boost::python::list& argv); void roscpp_init(); void roscpp_shutdown(); -} -} +} // namespace py_bindings_tools +} // namespace moveit diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h index 3dc50a10ea..fc1dafc8d2 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h @@ -75,7 +75,7 @@ enum InteractionStyle ORIENTATION_NOSPHERE = ORIENTATION_CIRCLES | ORIENTATION_EEF, SIX_DOF_NOSPHERE = POSITION_NOSPHERE | ORIENTATION_NOSPHERE }; -} +} // namespace InteractionStyle /// Type of function for constructing markers. /// This callback sets up the marker used for an interaction. @@ -173,4 +173,4 @@ struct JointInteraction /// The size of the connecting link (diameter of enclosing sphere) double size; }; -} +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h index e6a07c1d83..285b0c47a0 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h @@ -309,4 +309,4 @@ class InteractionHandler : public LockedRobotState // remove '_' characters from name static std::string fixName(std::string name); }; -} +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h index 16e821eab0..9404d5dc0b 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h @@ -67,4 +67,4 @@ void addPositionControl(visualization_msgs::InteractiveMarker& int_marker, bool void addViewPlaneControl(visualization_msgs::InteractiveMarker& int_marker, double radius, const std_msgs::ColorRGBA& color, bool position = true, bool orientation = true); -} +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h index 0ba6699d47..7207bd6152 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h @@ -87,4 +87,4 @@ struct KinematicOptions /// other options kinematics::KinematicsQueryOptions options_; }; -} +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h index f0e6d807e1..40ebd87d1b 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h @@ -102,4 +102,4 @@ class KinematicOptionsMap // PROTECTED BY lock_ M_options options_; }; -} +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h index 488b49057e..f10ddb99af 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h @@ -107,4 +107,4 @@ class LockedRobotState // PROTECTED BY state_lock_ moveit::core::RobotStatePtr state_; }; -} +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h index 3fc649d93e..7c458fab52 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h @@ -232,4 +232,4 @@ class RobotInteraction // options for doing IK KinematicOptionsMapPtr kinematic_options_map_; }; -} +} // namespace robot_interaction diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index ef8f3accd2..7dc3db6e20 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -75,7 +75,7 @@ class RosTopicProperty; class EditableEnumProperty; class ColorProperty; class MovableText; -} +} // namespace rviz namespace moveit_rviz_plugin { diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 26ac2b6a3a..eda9170512 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -371,4 +371,4 @@ void MotionPlanningFrame::waitForAction(const T& action, const ros::NodeHandle& else ROS_DEBUG("Connected to '%s'", name.c_str()); }; -} +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h index 27363dd63c..d50aa8a720 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h @@ -251,4 +251,4 @@ class JogSlider : public QSlider Q_SIGNALS: void triggered(double value); }; -} +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h index 0869a1a8ed..44fd36c20e 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h @@ -80,4 +80,4 @@ private Q_SLOTS: std::string group_name_; std::string planner_id_; }; -} +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index d6ecd6848b..7b917fe33d 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -60,7 +60,7 @@ class FloatProperty; class RosTopicProperty; class ColorProperty; class EnumProperty; -} +} // namespace rviz namespace moveit_rviz_plugin { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h index 1e086e2156..1bb8bf5daa 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h @@ -91,4 +91,4 @@ class OcTreeRender double colorFactor_; std::size_t octree_depth_; }; -} +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h index 3f16dffc55..bf9e86af54 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h @@ -60,4 +60,4 @@ class PlanningLinkUpdater : public rviz::LinkUpdater private: moveit::core::RobotStateConstPtr kinematic_state_; }; -} +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h index 987425752c..eb61b70521 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h @@ -86,4 +86,4 @@ class PlanningSceneRender RenderShapesPtr render_shapes_; RobotStateVisualizationPtr scene_robot_; }; -} +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h index a58697c244..9e3e9222fe 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h @@ -78,4 +78,4 @@ class RenderShapes std::vector > scene_shapes_; std::vector octree_voxel_grids_; }; -} +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index 58b258df24..754ff06a01 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -114,4 +114,4 @@ class RobotStateVisualization bool visual_visible_; bool collision_visible_; }; -} +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h index 0b6979701d..3c6d92307b 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h @@ -64,7 +64,7 @@ class RosTopicProperty; class EditableEnumProperty; class ColorProperty; class MovableText; -} +} // namespace rviz namespace moveit_rviz_plugin { diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h index 7e507bb257..cc08a95d79 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h @@ -82,4 +82,4 @@ class ConstraintsStorage : public MoveItMessageStorage ConstraintsCollection constraints_collection_; }; -} +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/moveit_message_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/moveit_message_storage.h index f6b6be0c3e..fbc6077e47 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/moveit_message_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/moveit_message_storage.h @@ -64,4 +64,4 @@ class MoveItMessageStorage /// \brief Load a database connection typename warehouse_ros::DatabaseConnection::Ptr loadDatabase(); -} +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h index 1878942ecd..e7899729ac 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h @@ -118,4 +118,4 @@ class PlanningSceneStorage : public MoveItMessageStorage MotionPlanRequestCollection motion_plan_request_collection_; RobotTrajectoryCollection robot_trajectory_collection_; }; -} +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_world_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_world_storage.h index fec6384e54..8e1b34af82 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_world_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_world_storage.h @@ -71,4 +71,4 @@ class PlanningSceneWorldStorage : public MoveItMessageStorage PlanningSceneWorldCollection planning_scene_world_collection_; }; -} +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h index ce592f9fa0..e008a5fbd2 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h @@ -77,4 +77,4 @@ class RobotStateStorage : public MoveItMessageStorage RobotStateCollection state_collection_; }; -} +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h index 7246618a41..7b2da57f5a 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h @@ -85,4 +85,4 @@ class TrajectoryConstraintsStorage : public MoveItMessageStorage TrajectoryConstraintsCollection constraints_collection_; }; -} +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/warehouse_connector.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/warehouse_connector.h index 3555097ccd..04e279a3ca 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/warehouse_connector.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/warehouse_connector.h @@ -53,4 +53,4 @@ class WarehouseConnector std::string dbexec_; int child_pid_; }; -} +} // namespace moveit_warehouse diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h index c19bc2e0f5..fc3ef90160 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h @@ -108,4 +108,4 @@ const std::string disabledReasonToString(DisabledReason reason); * \return reason as struct */ DisabledReason disabledReasonFromString(const std::string& reason); -} +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index d649cb6ad5..394eac0875 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -536,4 +536,4 @@ class MoveItConfigData planning_scene::PlanningScenePtr planning_scene_; }; -} // namespace +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/tools/rotated_header_view.h b/moveit_setup_assistant/src/tools/rotated_header_view.h index 5bf288d84c..04ec7897e9 100644 --- a/moveit_setup_assistant/src/tools/rotated_header_view.h +++ b/moveit_setup_assistant/src/tools/rotated_header_view.h @@ -48,4 +48,4 @@ class RotatedHeaderView : public QHeaderView QSize sectionSizeFromContents(int logicalIndex) const override; int sectionSizeHint(int logicalIndex) const; }; -} +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.h b/moveit_setup_assistant/src/widgets/controller_edit_widget.h index f7a2679164..1da3f602b2 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.h +++ b/moveit_setup_assistant/src/widgets/controller_edit_widget.h @@ -140,4 +140,4 @@ private Q_SLOTS: /// Contains all the configuration data for the setup assistant moveit_setup_assistant::MoveItConfigDataPtr config_data_; }; -} +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.h b/moveit_setup_assistant/src/widgets/default_collisions_widget.h index d69cedefd3..ccf814a70c 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.h +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.h @@ -255,4 +255,4 @@ class MonitorThread : public QThread unsigned int progress_; bool canceled_; }; -} +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.h b/moveit_setup_assistant/src/widgets/end_effectors_widget.h index a6be88ed6f..1073ec1866 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.h +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.h @@ -182,4 +182,4 @@ private Q_SLOTS: void edit(const std::string& name); }; -} // namespace +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.h b/moveit_setup_assistant/src/widgets/group_edit_widget.h index bf62ee77fb..a55fb2943e 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.h +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.h @@ -129,4 +129,4 @@ private Q_SLOTS: // Private Functions // ****************************************************************************************** }; -} +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/header_widget.h b/moveit_setup_assistant/src/widgets/header_widget.h index a5661beb83..3e82c51728 100644 --- a/moveit_setup_assistant/src/widgets/header_widget.h +++ b/moveit_setup_assistant/src/widgets/header_widget.h @@ -117,4 +117,4 @@ class LoadPathArgsWidget : public LoadPathWidget void setArgs(const QString& args); void setArgsEnabled(bool enabled = true); }; -} +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h index e7a3589803..150fe46de0 100644 --- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h +++ b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h @@ -133,4 +133,4 @@ private Q_SLOTS: // Private Functions // ****************************************************************************************** }; -} +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/navigation_widget.h b/moveit_setup_assistant/src/widgets/navigation_widget.h index 76084c869e..410be2deb8 100644 --- a/moveit_setup_assistant/src/widgets/navigation_widget.h +++ b/moveit_setup_assistant/src/widgets/navigation_widget.h @@ -82,4 +82,4 @@ class NavDelegate : public QStyledItemDelegate QSize sizeHint(const QStyleOptionViewItem& option, const QModelIndex& index) const override; void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override; }; -} +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.h b/moveit_setup_assistant/src/widgets/passive_joints_widget.h index 3e6b94f439..9f27cf15b5 100644 --- a/moveit_setup_assistant/src/widgets/passive_joints_widget.h +++ b/moveit_setup_assistant/src/widgets/passive_joints_widget.h @@ -100,4 +100,4 @@ private Q_SLOTS: std::string current_edit_vjoint_; }; -} // namespace +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.h b/moveit_setup_assistant/src/widgets/planning_groups_widget.h index 4c394c23ef..33fba55316 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.h +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.h @@ -204,7 +204,7 @@ private Q_SLOTS: /// Switch to current groups view void showMainScreen(); }; -} +} // namespace moveit_setup_assistant // ****************************************************************************************** // ****************************************************************************************** diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.h b/moveit_setup_assistant/src/widgets/robot_poses_widget.h index d1dee54b10..86be2084a1 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.h +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.h @@ -290,7 +290,7 @@ private Q_SLOTS: // ****************************************************************************************** }; -} // namespace +} // namespace moveit_setup_assistant // Declare std::string as metatype so we can use it in a signal Q_DECLARE_METATYPE(std::string) diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h index 51f07cf56f..20ab7cc114 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h @@ -235,4 +235,4 @@ private Q_SLOTS: // Private Functions // ****************************************************************************************** }; -} +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.h b/moveit_setup_assistant/src/widgets/start_screen_widget.h index d06dd6a6ce..c81def715e 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.h +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.h @@ -188,4 +188,4 @@ private Q_SLOTS: QPushButton* btn_exist_; QLabel* widget_instructions_; }; -} +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h b/moveit_setup_assistant/src/widgets/virtual_joints_widget.h index e3c6f5d4ec..0f48d7b2e2 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.h @@ -186,4 +186,4 @@ private Q_SLOTS: void edit(const std::string& name); }; -} // namespace +} // namespace moveit_setup_assistant From 972b8836f460bcdeed0c246a567615d1f9793363 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 4 May 2020 16:52:26 +0200 Subject: [PATCH 268/612] modernize-redundant-void-arg, modernize-use-nullptr, modernize-use-default, modernize-use-override --- .../moveit/collision_detection/world.h | 2 +- .../bullet_cast_bvh_manager.h | 2 +- .../bullet_discrete_bvh_manager.h | 2 +- .../bullet_integration/bullet_utils.h | 2 +- .../collision_env_bullet.h | 16 ++++----- .../collision_common.h | 4 +-- .../collision_detector_fcl_plugin_loader.h | 2 +- .../collision_env_fcl.h | 16 ++++----- .../collision_env_distance_field.h | 4 +-- .../propagation_distance_field.h | 6 ++-- .../moveit/distance_field/voxel_grid.h | 4 +-- .../kinematic_constraint.h | 6 ++-- .../moveit/kinematics_base/kinematics_base.h | 2 +- .../moveit/planning_scene/planning_scene.h | 16 ++++----- .../include/moveit/profiler/profiler.h | 34 +++++++++---------- .../include/moveit/robot_model/robot_model.h | 2 +- .../include/moveit/robot_state/robot_state.h | 4 +-- .../time_optimal_trajectory_generation.h | 2 +- .../include/chomp_motion_planner/chomp_cost.h | 8 ++--- .../model_based_state_space.h | 2 +- .../work_space/pose_model_state_space.h | 2 +- .../action_based_controller_handle.h | 2 +- .../moveit/pick_place/manipulation_plan.h | 6 ++-- .../tf_publisher_capability.h | 2 +- .../occupancy_map_monitor/occupancy_map.h | 2 +- .../point_containment_filter/shape_mask.h | 2 +- .../move_group_interface.h | 4 +-- .../robot_interaction/interaction_handler.h | 2 +- .../robot_interaction/robot_interaction.h | 2 +- .../motion_planning_frame.h | 2 +- .../motion_planning_frame_joints_widget.h | 4 +-- .../motion_planning_param_widget.h | 2 +- .../trajectory_panel.h | 2 +- .../trajectory_constraints_storage.h | 4 +-- .../src/tools/collision_linear_model.h | 4 +-- .../src/tools/collision_matrix_model.h | 2 +- .../src/tools/rotated_header_view.h | 2 +- .../src/widgets/default_collisions_widget.h | 2 +- .../src/widgets/double_list_widget.h | 2 +- .../src/widgets/navigation_widget.h | 4 +-- 40 files changed, 95 insertions(+), 95 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index 110fcf631c..836d900525 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -241,7 +241,7 @@ class World class ObserverHandle { public: - ObserverHandle() : observer_(NULL) + ObserverHandle() : observer_(nullptr) { } diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h index 45013d2113..69c856575b 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h @@ -50,7 +50,7 @@ class BulletCastBVHManager : public BulletBVHManager /** \brief Constructor */ BulletCastBVHManager() = default; - virtual ~BulletCastBVHManager() = default; + ~BulletCastBVHManager() override = default; /**@brief Clone the manager * diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h index eea9e17820..ec1a451814 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h @@ -50,7 +50,7 @@ class BulletDiscreteBVHManager : public BulletBVHManager /** \brief Constructor */ BulletDiscreteBVHManager() = default; - virtual ~BulletDiscreteBVHManager() = default; + ~BulletDiscreteBVHManager() override = default; /**@brief Clone the manager * diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h index 29f2434724..252f8c3e51 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h @@ -929,7 +929,7 @@ inline void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr& cow, struct BroadphaseFilterCallback : public btOverlapFilterCallback { - virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override + bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override { bool cull = !(proxy0->m_collisionFilterMask & proxy1->m_collisionFilterGroup); cull = cull || !(proxy1->m_collisionFilterMask & proxy0->m_collisionFilterGroup); diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h index d976448a2c..ba502f2e00 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h @@ -59,32 +59,32 @@ class CollisionEnvBullet : public CollisionEnv CollisionEnvBullet(CollisionEnvBullet&) = delete; - virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state) const override; - virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state) const override; - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, const moveit::core::RobotState& state2) const override; - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res, + void distanceSelf(const DistanceRequest& req, DistanceResult& res, const moveit::core::RobotState& state) const override; - virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, + void distanceRobot(const DistanceRequest& req, DistanceResult& res, const moveit::core::RobotState& state) const override; void setWorld(const WorldPtr& world) override; diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index cde0730291..6f33b9280c 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -139,12 +139,12 @@ struct CollisionGeometryData /** \brief Data structure which is passed to the collision callback function of the collision manager. */ struct CollisionData { - CollisionData() : req_(NULL), active_components_only_(NULL), res_(NULL), acm_(NULL), done_(false) + CollisionData() : req_(nullptr), active_components_only_(nullptr), res_(nullptr), acm_(nullptr), done_(false) { } CollisionData(const CollisionRequest* req, CollisionResult* res, const AllowedCollisionMatrix* acm) - : req_(req), active_components_only_(NULL), res_(res), acm_(acm), done_(false) + : req_(req), active_components_only_(nullptr), res_(res), acm_(acm), done_(false) { } diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h index 213b8ccc97..c18d059178 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h @@ -44,6 +44,6 @@ namespace collision_detection class CollisionDetectorFCLPluginLoader : public CollisionPlugin { public: - virtual bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const; + bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const override; }; } diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h index a6c261ec39..dbdfa05673 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h @@ -64,32 +64,32 @@ class CollisionEnvFCL : public CollisionEnv ~CollisionEnvFCL() override; - virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state) const override; - virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state) const override; - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, const moveit::core::RobotState& state2) const override; - virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res, + void distanceSelf(const DistanceRequest& req, DistanceResult& res, const moveit::core::RobotState& state) const override; - virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, + void distanceRobot(const DistanceRequest& req, DistanceResult& res, const moveit::core::RobotState& state) const override; void setWorld(const WorldPtr& world) override; diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h index 93cd63ef2f..3d1d81a55d 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h @@ -169,11 +169,11 @@ class CollisionEnvDistanceField : public CollisionEnv const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, const moveit::core::RobotState& state2) const override; diff --git a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h index 7dd4971421..e6b6d083a5 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h @@ -417,20 +417,20 @@ class PropagationDistanceField : public DistanceField dist = sqrt_table_[cell->distance_square_]; pos = cell->closest_point_; const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z()); - return ncell == cell ? NULL : ncell; + return ncell == cell ? nullptr : ncell; } if (cell->negative_distance_square_ > 0) { dist = -sqrt_table_[cell->negative_distance_square_]; pos = cell->closest_negative_point_; const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z()); - return ncell == cell ? NULL : ncell; + return ncell == cell ? nullptr : ncell; } dist = 0.0; pos.x() = x; pos.y() = y; pos.z() = z; - return NULL; + return nullptr; } /** diff --git a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h index 82a296cabf..da05bd437d 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h +++ b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h @@ -341,7 +341,7 @@ class VoxelGrid template VoxelGrid::VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z, T default_object) - : data_(NULL) + : data_(nullptr) { resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object); } @@ -368,7 +368,7 @@ void VoxelGrid::resize(double size_x, double size_y, double size_z, double re double origin_y, double origin_z, T default_object) { delete[] data_; - data_ = NULL; + data_ = nullptr; size_[DIM_X] = size_x; size_[DIM_Y] = size_y; diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index ad810d306f..7c5b1691e6 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -208,7 +208,7 @@ class JointConstraint : public KinematicConstraint * @param [in] model The kinematic model used for constraint evaluation */ JointConstraint(const moveit::core::RobotModelConstPtr& model) - : KinematicConstraint(model), joint_model_(NULL), joint_variable_index_(-1) + : KinematicConstraint(model), joint_model_(nullptr), joint_variable_index_(-1) { type_ = JOINT_CONSTRAINT; } @@ -355,7 +355,7 @@ class OrientationConstraint : public KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - OrientationConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL) + OrientationConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(nullptr) { type_ = ORIENTATION_CONSTRAINT; } @@ -512,7 +512,7 @@ class PositionConstraint : public KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - PositionConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL) + PositionConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(nullptr) { type_ = POSITION_CONSTRAINT; } diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 7e2e47aaf5..a77c930ffe 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -508,7 +508,7 @@ class KinematicsBase * supported. * \return True if the group is supported, false if not. */ - virtual bool supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out = NULL) const; + virtual bool supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out = nullptr) const; /** * @brief Set the search discretization value for all the redundant joints diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index ce2f166ac0..5a776494e3 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -867,14 +867,14 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility) */ bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory, const std::string& group = "", bool verbose = false, - std::vector* invalid_index = NULL) const; + std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the * passed in trajectory. */ bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = NULL) const; + bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the @@ -882,7 +882,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints, const std::string& group = "", bool verbose = false, - std::vector* invalid_index = NULL) const; + std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the @@ -890,7 +890,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, const std::vector& goal_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = NULL) const; + bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the @@ -898,7 +898,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, const std::vector& goal_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = NULL) const; + bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the @@ -906,17 +906,17 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints, const std::string& group = "", bool verbose = false, - std::vector* invalid_index = NULL) const; + std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = NULL) const; + bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility) */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = NULL) const; + bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Get the top \e max_costs cost sources for a specified trajectory. The resulting costs are stored in \e * costs */ diff --git a/moveit_core/profiler/include/moveit/profiler/profiler.h b/moveit_core/profiler/include/moveit/profiler/profiler.h index 96a13881b2..82b5901bce 100644 --- a/moveit_core/profiler/include/moveit/profiler/profiler.h +++ b/moveit_core/profiler/include/moveit/profiler/profiler.h @@ -83,7 +83,7 @@ class Profiler : private boost::noncopyable prof_.begin(name); } - ~ScopedBlock(void) + ~ScopedBlock() { prof_.end(name_); } @@ -105,7 +105,7 @@ class Profiler : private boost::noncopyable prof_.start(); } - ~ScopedStart(void) + ~ScopedStart() { if (!wasRunning_) prof_.stop(); @@ -117,7 +117,7 @@ class Profiler : private boost::noncopyable }; /** \brief Return an instance of the class */ - static Profiler& instance(void); + static Profiler& instance(); /** \brief Constructor. It is allowed to separately instantiate this class (not only as a singleton) */ @@ -128,38 +128,38 @@ class Profiler : private boost::noncopyable } /** \brief Destructor */ - ~Profiler(void) + ~Profiler() { if (printOnDestroy_ && !data_.empty()) status(); } /** \brief Start counting time */ - static void Start(void) + static void Start() { instance().start(); } /** \brief Stop counting time */ - static void Stop(void) + static void Stop() { instance().stop(); } /** \brief Clear counted time and events */ - static void Clear(void) + static void Clear() { instance().clear(); } /** \brief Start counting time */ - void start(void); + void start(); /** \brief Stop counting time */ - void stop(void); + void stop(); /** \brief Clear counted time and events */ - void clear(void); + void clear(); /** \brief Count a specific event for a number of times */ static void Event(const std::string& name, const unsigned int times = 1) @@ -212,23 +212,23 @@ class Profiler : private boost::noncopyable /** \brief Print the status of the profiled code chunks and events to the console (using msg::Console) */ - static void Console(void) + static void Console() { instance().console(); } /** \brief Print the status of the profiled code chunks and events to the console (using msg::Console) */ - void console(void); + void console(); /** \brief Check if the profiler is counting time or not */ - bool running(void) const + bool running() const { return running_; } /** \brief Check if the profiler is counting time or not */ - static bool Running(void) + static bool Running() { return instance().running(); } @@ -237,7 +237,7 @@ class Profiler : private boost::noncopyable /** \brief Information about time spent in a section of the code */ struct TimeInfo { - TimeInfo(void) + TimeInfo() : total(0, 0, 0, 0), shortest(boost::posix_time::pos_infin), longest(boost::posix_time::neg_infin), parts(0) { } @@ -258,13 +258,13 @@ class Profiler : private boost::noncopyable boost::posix_time::ptime start; /** \brief Begin counting time */ - void set(void) + void set() { start = boost::posix_time::microsec_clock::universal_time(); } /** \brief Add the counted time to the total time */ - void update(void) + void update() { const boost::posix_time::time_duration& dt = boost::posix_time::microsec_clock::universal_time() - start; if (dt > longest) diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 8c8ffdaf3a..3018008ea2 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -90,7 +90,7 @@ class RobotModel /** \brief Return true if the model is empty (has no root link, no joints) */ bool isEmpty() const { - return root_link_ == NULL; + return root_link_ == nullptr; } /** \brief Get the parsed URDF model */ diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index cc1b6c5353..fe49f1a760 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1734,7 +1734,7 @@ class RobotState { dirty_joint_transforms_[joint->getJointIndex()] = 1; dirty_link_transforms_ = - dirty_link_transforms_ == NULL ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint); + dirty_link_transforms_ == nullptr ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint); } void markDirtyJointTransforms(const JointModelGroup* group) @@ -1742,7 +1742,7 @@ class RobotState const std::vector& jm = group->getActiveJointModels(); for (std::size_t i = 0; i < jm.size(); ++i) dirty_joint_transforms_[jm[i]->getJointIndex()] = 1; - dirty_link_transforms_ = dirty_link_transforms_ == NULL ? + dirty_link_transforms_ = dirty_link_transforms_ == nullptr ? group->getCommonRoot() : robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot()); } diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index 3e8268a92c..b5b7c80fd4 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -95,7 +95,7 @@ class Trajectory Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration, double time_step = 0.001); - ~Trajectory(void); + ~Trajectory(); /** @brief Call this method after constructing the object to make sure the trajectory generation succeeded without errors. If this method returns diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h index a60b1e1c71..ac4659582e 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h @@ -53,13 +53,13 @@ class ChompCost virtual ~ChompCost(); template - void getDerivative(Eigen::MatrixXd::ColXpr joint_trajectory, Eigen::MatrixBase& derivative) const; + void getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory, Eigen::MatrixBase& derivative) const; const Eigen::MatrixXd& getQuadraticCostInverse() const; const Eigen::MatrixXd& getQuadraticCost() const; - double getCost(Eigen::MatrixXd::ColXpr joint_trajectory) const; + double getCost(const Eigen::MatrixXd::ColXpr& joint_trajectory) const; double getMaxQuadCostInvValue() const; @@ -75,7 +75,7 @@ class ChompCost }; template -void ChompCost::getDerivative(Eigen::MatrixXd::ColXpr joint_trajectory, Eigen::MatrixBase& derivative) const +void ChompCost::getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory, Eigen::MatrixBase& derivative) const { derivative = (quad_cost_full_ * (2.0 * joint_trajectory)); } @@ -90,7 +90,7 @@ inline const Eigen::MatrixXd& ChompCost::getQuadraticCost() const return quad_cost_; } -inline double ChompCost::getCost(Eigen::MatrixXd::ColXpr joint_trajectory) const +inline double ChompCost::getCost(const Eigen::MatrixXd::ColXpr& joint_trajectory) const { return joint_trajectory.dot(quad_cost_full_ * joint_trajectory); } diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h index b4543f047e..e334322a98 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h @@ -86,7 +86,7 @@ class ModelBasedStateSpace : public ompl::base::StateSpace IS_GOAL_STATE = 16 }; - StateType() : ompl::base::State(), values(NULL), tag(-1), flags(0), distance(0.0) + StateType() : ompl::base::State(), values(nullptr), tag(-1), flags(0), distance(0.0) { } diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h index b0668a6727..071a3f262d 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h @@ -55,7 +55,7 @@ class PoseModelStateSpace : public ModelBasedStateSpace POSE_COMPUTED = 512 }; - StateType() : ModelBasedStateSpace::StateType(), poses(NULL) + StateType() : ModelBasedStateSpace::StateType(), poses(nullptr) { flags |= JOINTS_COMPUTED; } diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h index 72218f3e2e..5e093e68a7 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h @@ -152,7 +152,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase protected: ros::NodeHandle nh_; - std::string getActionName(void) const + std::string getActionName() const { if (namespace_.empty()) return name_; diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h index 2086af4b2d..56dd3523a7 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h @@ -55,9 +55,9 @@ MOVEIT_STRUCT_FORWARD(ManipulationPlanSharedData); struct ManipulationPlanSharedData { ManipulationPlanSharedData() - : planning_group_(NULL) - , end_effector_group_(NULL) - , ik_link_(NULL) + : planning_group_(nullptr) + , end_effector_group_(nullptr) + , ik_link_(nullptr) , max_goal_sampling_attempts_(0) , minimize_object_distance_(false) { diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h index 9aab273170..7c01174408 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h @@ -45,7 +45,7 @@ class TfPublisher : public MoveGroupCapability { public: TfPublisher(); - ~TfPublisher(); + ~TfPublisher() override; void initialize() override; diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h index cd09da30e8..875f3cab22 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h @@ -100,7 +100,7 @@ class OccMapTree : public octomap::OcTree return WriteLock(tree_mutex_); } - void triggerUpdateCallback(void) + void triggerUpdateCallback() { if (update_callback_) update_callback_(); diff --git a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h index d7248acded..fdc238e891 100644 --- a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h +++ b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h @@ -95,7 +95,7 @@ class ShapeMask { SeeShape() { - body = NULL; + body = nullptr; } bodies::Body* body; diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 0d5a49c90e..ec8d70f0b4 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -749,7 +749,7 @@ class MoveGroupInterface Return -1.0 in case of error. */ double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true, - moveit_msgs::MoveItErrorCodes* error_code = NULL); + moveit_msgs::MoveItErrorCodes* error_code = nullptr); /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the @@ -766,7 +766,7 @@ class MoveGroupInterface double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions = true, - moveit_msgs::MoveItErrorCodes* error_code = NULL); + moveit_msgs::MoveItErrorCodes* error_code = nullptr); /** \brief Stop any trajectory execution, if one is active */ void stop(); diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h index 285b0c47a0..02b63a6e6c 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h @@ -215,7 +215,7 @@ class InteractionHandler : public LockedRobotState /** \brief Clear any error settings. * This makes the markers appear as if the state is no longer invalid. */ - void clearError(void); + void clearError(); protected: bool transformFeedbackPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback, diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h index 7c458fab52..e770293d55 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h @@ -79,7 +79,7 @@ class RobotInteraction RobotInteraction(const moveit::core::RobotModelConstPtr& robot_model, const std::string& ns = ""); virtual ~RobotInteraction(); - const std::string& getServerTopic(void) const + const std::string& getServerTopic() const { return topic_; } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index eda9170512..a858fe2105 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -104,7 +104,7 @@ class MotionPlanningFrame : public QWidget public: MotionPlanningFrame(const MotionPlanningFrame&) = delete; - MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent = 0); + MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent = nullptr); ~MotionPlanningFrame() override; void changePlanningGroup(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h index d50aa8a720..6cc3b42af3 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h @@ -124,7 +124,7 @@ class MotionPlanningFrameJointsWidget : public QWidget public: MotionPlanningFrameJointsWidget(const MotionPlanningFrameJointsWidget&) = delete; MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent = nullptr); - ~MotionPlanningFrameJointsWidget(); + ~MotionPlanningFrameJointsWidget() override; void changePlanningGroup(const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler, @@ -168,7 +168,7 @@ class ProgressBarDelegate : public QStyledItemDelegate VariableBoundsRole }; - ProgressBarDelegate(QWidget* parent = 0) : QStyledItemDelegate(parent) + ProgressBarDelegate(QWidget* parent = nullptr) : QStyledItemDelegate(parent) { } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h index 44fd36c20e..e423dc8272 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h @@ -58,7 +58,7 @@ class MotionPlanningParamWidget : public rviz::PropertyTreeWidget Q_OBJECT public: MotionPlanningParamWidget(const MotionPlanningParamWidget&) = delete; - MotionPlanningParamWidget(QWidget* parent = 0); + MotionPlanningParamWidget(QWidget* parent = nullptr); ~MotionPlanningParamWidget() override; void setMoveGroup(const moveit::planning_interface::MoveGroupInterfacePtr& mg); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h index c552f7ec84..73d2e6c01f 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h @@ -53,7 +53,7 @@ class TrajectoryPanel : public rviz::Panel Q_OBJECT public: - TrajectoryPanel(QWidget* parent = 0); + TrajectoryPanel(QWidget* parent = nullptr); ~TrajectoryPanel() override; diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h index 7b2da57f5a..c7e6964a23 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h @@ -78,10 +78,10 @@ class TrajectoryConstraintsStorage : public MoveItMessageStorage void removeTrajectoryConstraints(const std::string& name, const std::string& robot = "", const std::string& group = ""); - void reset(void); + void reset(); private: - void createCollections(void); + void createCollections(); TrajectoryConstraintsCollection constraints_collection_; }; diff --git a/moveit_setup_assistant/src/tools/collision_linear_model.h b/moveit_setup_assistant/src/tools/collision_linear_model.h index 9b702fcc73..dd3dd4006a 100644 --- a/moveit_setup_assistant/src/tools/collision_linear_model.h +++ b/moveit_setup_assistant/src/tools/collision_linear_model.h @@ -51,7 +51,7 @@ class CollisionLinearModel : public QAbstractProxyModel Q_OBJECT public: - CollisionLinearModel(CollisionMatrixModel* src, QObject* parent = NULL); + CollisionLinearModel(CollisionMatrixModel* src, QObject* parent = nullptr); ~CollisionLinearModel() override; // reimplement to return the model index in the proxy model that to the sourceIndex from the source model @@ -81,7 +81,7 @@ class SortFilterProxyModel : public QSortFilterProxyModel Q_OBJECT public: - SortFilterProxyModel(QObject* parent = 0); + SortFilterProxyModel(QObject* parent = nullptr); QVariant headerData(int section, Qt::Orientation orientation, int role) const override; void sort(int column, Qt::SortOrder order) override; void setShowAll(bool show_all); diff --git a/moveit_setup_assistant/src/tools/collision_matrix_model.h b/moveit_setup_assistant/src/tools/collision_matrix_model.h index ed5d67e894..9d4b9d2d78 100644 --- a/moveit_setup_assistant/src/tools/collision_matrix_model.h +++ b/moveit_setup_assistant/src/tools/collision_matrix_model.h @@ -49,7 +49,7 @@ class CollisionMatrixModel : public QAbstractTableModel Q_OBJECT public: CollisionMatrixModel(moveit_setup_assistant::LinkPairMap& pairs, const std::vector& names, - QObject* parent = NULL); + QObject* parent = nullptr); int rowCount(const QModelIndex& parent = QModelIndex()) const override; int columnCount(const QModelIndex& parent = QModelIndex()) const override; QVariant data(const QModelIndex& index, int role = Qt::DisplayRole) const override; diff --git a/moveit_setup_assistant/src/tools/rotated_header_view.h b/moveit_setup_assistant/src/tools/rotated_header_view.h index 04ec7897e9..41e371b441 100644 --- a/moveit_setup_assistant/src/tools/rotated_header_view.h +++ b/moveit_setup_assistant/src/tools/rotated_header_view.h @@ -43,7 +43,7 @@ namespace moveit_setup_assistant class RotatedHeaderView : public QHeaderView { public: - RotatedHeaderView(Qt::Orientation orientation, QWidget* parent = NULL); + RotatedHeaderView(Qt::Orientation orientation, QWidget* parent = nullptr); void paintSection(QPainter* painter, const QRect& rect, int logicalIndex) const override; QSize sectionSizeFromContents(int logicalIndex) const override; int sectionSizeHint(int logicalIndex) const; diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.h b/moveit_setup_assistant/src/widgets/default_collisions_widget.h index ccf814a70c..ade0c7f30b 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.h +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.h @@ -236,7 +236,7 @@ class MonitorThread : public QThread Q_OBJECT public: - MonitorThread(const boost::function& f, QProgressBar* progress_bar = NULL); + MonitorThread(const boost::function& f, QProgressBar* progress_bar = nullptr); void run() override; void cancel() { diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.h b/moveit_setup_assistant/src/widgets/double_list_widget.h index 1d983134aa..afcac603bc 100644 --- a/moveit_setup_assistant/src/widgets/double_list_widget.h +++ b/moveit_setup_assistant/src/widgets/double_list_widget.h @@ -68,7 +68,7 @@ class DoubleListWidget : public QWidget /// Set the right box void setSelected(const std::vector& items); - void clearContents(void); + void clearContents(); /// Convenience function for reusing set table code void setTable(const std::vector& items, QTableWidget* table); diff --git a/moveit_setup_assistant/src/widgets/navigation_widget.h b/moveit_setup_assistant/src/widgets/navigation_widget.h index 410be2deb8..2adb17ca4e 100644 --- a/moveit_setup_assistant/src/widgets/navigation_widget.h +++ b/moveit_setup_assistant/src/widgets/navigation_widget.h @@ -56,7 +56,7 @@ class NavigationWidget : public QListView { Q_OBJECT public: - explicit NavigationWidget(QWidget* parent = 0); + explicit NavigationWidget(QWidget* parent = nullptr); void setNavs(const QList& navs); void setEnabled(const int& index, bool enabled); @@ -77,7 +77,7 @@ class NavDelegate : public QStyledItemDelegate { Q_OBJECT public: - explicit NavDelegate(QObject* parent = 0); + explicit NavDelegate(QObject* parent = nullptr); QSize sizeHint(const QStyleOptionViewItem& option, const QModelIndex& index) const override; void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override; From 1d44aceec1d3dcb79c32c500eed41c22e5ba3c0f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 4 May 2020 17:37:48 +0200 Subject: [PATCH 269/612] modernize-loop-convert --- .../collision_distance_field_types.h | 6 +++--- .../moveit/kinematics_base/kinematics_base.h | 6 +++--- .../include/moveit/robot_state/robot_state.h | 20 +++++++++---------- .../gripper_controller_handle.h | 4 +--- .../test/test_moveit_controller_manager.h | 12 +++++------ 5 files changed, 22 insertions(+), 26 deletions(-) diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index 627a0a790a..37a5a664b8 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -450,10 +450,10 @@ class PosedBodyPointDecompositionVector EigenSTL::vector_Vector3d getCollisionPoints() const { EigenSTL::vector_Vector3d ret_points; - for (unsigned int i = 0; i < decomp_vector_.size(); i++) + for (const PosedBodyPointDecompositionPtr& decomp : decomp_vector_) { - ret_points.insert(ret_points.end(), decomp_vector_[i]->getCollisionPoints().begin(), - decomp_vector_[i]->getCollisionPoints().end()); + ret_points.insert(ret_points.end(), decomp->getCollisionPoints().begin(), + decomp->getCollisionPoints().end()); } return ret_points; } diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index a77c930ffe..c3ad491745 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -531,10 +531,10 @@ class KinematicsBase { redundant_joint_discretization_.clear(); redundant_joint_indices_.clear(); - for (std::map::const_iterator i = discretization.begin(); i != discretization.end(); i++) + for (const auto& pair : discretization) { - redundant_joint_discretization_.insert(*i); - redundant_joint_indices_.push_back(i->first); + redundant_joint_discretization_.insert(pair); + redundant_joint_indices_.push_back(pair.first); } } diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index fe49f1a760..a03c5bde7d 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1739,9 +1739,8 @@ class RobotState void markDirtyJointTransforms(const JointModelGroup* group) { - const std::vector& jm = group->getActiveJointModels(); - for (std::size_t i = 0; i < jm.size(); ++i) - dirty_joint_transforms_[jm[i]->getJointIndex()] = 1; + for (const JointModel* jm : group->getActiveJointModels()) + dirty_joint_transforms_[jm->getJointIndex()] = 1; dirty_link_transforms_ = dirty_link_transforms_ == nullptr ? group->getCommonRoot() : robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot()); @@ -1753,12 +1752,11 @@ class RobotState void updateMimicJoint(const JointModel* joint) { - const std::vector& mim = joint->getMimicRequests(); double v = position_[joint->getFirstVariableIndex()]; - for (std::size_t i = 0; i < mim.size(); ++i) + for (const JointModel* jm : joint->getMimicRequests()) { - position_[mim[i]->getFirstVariableIndex()] = mim[i]->getMimicFactor() * v + mim[i]->getMimicOffset(); - markDirtyJointTransforms(mim[i]); + position_[jm->getFirstVariableIndex()] = jm->getMimicFactor() * v + jm->getMimicOffset(); + markDirtyJointTransforms(jm); } } @@ -1766,15 +1764,15 @@ class RobotState /* use updateMimicJoints() instead, which also marks joints dirty */ [[deprecated]] void updateMimicJoint(const std::vector& mim) { - for (std::size_t i = 0; i < mim.size(); ++i) + for (const JointModel* jm : mim) { - const int fvi = mim[i]->getFirstVariableIndex(); + const int fvi = jm->getFirstVariableIndex(); position_[fvi] = - mim[i]->getMimicFactor() * position_[mim[i]->getMimic()->getFirstVariableIndex()] + mim[i]->getMimicOffset(); + jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); // Only mark joint transform dirty, but not the associated link transform // as this function is always used in combination of // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group); - dirty_joint_transforms_[mim[i]->getJointIndex()] = 1; + dirty_joint_transforms_[jm->getJointIndex()] = 1; } } diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h index c52b53fcca..aae275a2e2 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h @@ -117,10 +117,8 @@ class GripperControllerHandle : public ActionBasedControllerHandle& activate, const std::vector& deactivate) override { - for (std::size_t i = 0; i < deactivate.size(); ++i) + for (const std::string& controller : deactivate) { - controllers_[deactivate[i]] &= ~ACTIVE; - std::cout << "Deactivated controller " << deactivate[i] << std::endl; + controllers_[controller] &= ~ACTIVE; + std::cout << "Deactivated controller " << controller << std::endl; } - for (std::size_t i = 0; i < activate.size(); ++i) + for (const std::string& controller : activate) { - controllers_[activate[i]] |= ACTIVE; - std::cout << "Activated controller " << activate[i] << std::endl; + controllers_[controller] |= ACTIVE; + std::cout << "Activated controller " << controller << std::endl; } return true; } From e4c6bd32128c93506b872ea7f99051b07b07a169 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 4 May 2020 18:07:03 +0200 Subject: [PATCH 270/612] readability-identifier-naming --- .../moveit/collision_detection/world.h | 2 +- .../moveit/collision_detection/world_diff.h | 2 +- .../moveit/distance_field/distance_field.h | 6 +- .../propagation_distance_field.h | 4 +- .../moveit/distance_field/voxel_grid.h | 10 +- .../distance_field/src/distance_field.cpp | 6 +- .../src/propagation_distance_field.cpp | 2 +- .../kinematic_constraint.h | 2 +- .../include/moveit/profiler/profiler.h | 20 ++-- .../cached_ik_kinematics_plugin.h | 24 ++-- .../detail/GreedyKCenters.h | 14 +-- .../detail/NearestNeighborsGNAT.h | 112 +++++++++--------- .../chainiksolver_vel_mimic_svd.hpp | 1 + .../kdl_kinematics_plugin.h | 1 + .../chomp_motion_planner/chomp_trajectory.h | 4 +- .../detail/constrained_goal_sampler.h | 2 +- .../src/moveit_fake_controllers.h | 4 +- .../src/depth_image_octomap_updater.cpp | 8 +- .../moveit/mesh_filter/mesh_filter_base.h | 10 +- .../moveit/mesh_filter/stereo_camera_model.h | 2 +- .../mesh_filter/src/mesh_filter_base.cpp | 4 +- .../mesh_filter/test/mesh_filter_test.cpp | 16 +-- .../motion_planning_frame_joints_widget.h | 4 +- .../tools/moveit_config_data.h | 6 +- .../src/tools/collision_matrix_model.h | 2 +- .../src/tools/moveit_config_data.cpp | 6 +- .../src/widgets/default_collisions_widget.cpp | 12 +- .../src/widgets/default_collisions_widget.h | 4 +- .../src/widgets/group_edit_widget.cpp | 4 +- 29 files changed, 148 insertions(+), 146 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index 836d900525..c641529465 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -270,7 +270,7 @@ class World private: /** notify all observers of a change */ - void notify(const ObjectConstPtr&, Action); + void notify(const ObjectConstPtr& /*obj*/, Action /*action*/); /** send notification of change to all objects. */ void notifyAll(Action action); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h index 071098d2f9..4838758b24 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h @@ -113,7 +113,7 @@ class WorldDiff private: /** \brief Notification function */ - void notify(const World::ObjectConstPtr&, World::Action); + void notify(const World::ObjectConstPtr& /*obj*/, World::Action /*action*/); /** keep changes in a map so they can be coalesced */ std::map changes_; diff --git a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h index b8473a1239..56f0bc082e 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h @@ -68,9 +68,9 @@ namespace distance_field /// \brief The plane to visualize enum PlaneVisualizationType { - XYPlane, - XZPlane, - YZPlane + XY_PLANE, + XZ_PLANE, + YZ_PLANE }; MOVEIT_CLASS_FORWARD(DistanceField); diff --git a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h index e6b6d083a5..dcff47fcda 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h @@ -55,7 +55,7 @@ namespace distance_field * \brief Struct for sorting type Eigen::Vector3i for use in sorted * std containers. Sorts in z order, then y order, then x order. */ -struct compareEigen_Vector3i +struct CompareEigenVector3i { bool operator()(const Eigen::Vector3i& loc_1, const Eigen::Vector3i& loc_2) const { @@ -448,7 +448,7 @@ class PropagationDistanceField : public DistanceField private: /** Typedef for set of integer indices */ - typedef std::set> VoxelSet; + typedef std::set> VoxelSet; /** * \brief Initializes the field, resetting the voxel grid and * building a sqrt lookup table for efficiency based on diff --git a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h index da05bd437d..75dc96806f 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h +++ b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h @@ -461,12 +461,12 @@ inline int VoxelGrid::getNumCells(Dimension dim) const template inline const T& VoxelGrid::operator()(double x, double y, double z) const { - int cellX = getCellFromLocation(DIM_X, x); - int cellY = getCellFromLocation(DIM_Y, y); - int cellZ = getCellFromLocation(DIM_Z, z); - if (!isCellValid(cellX, cellY, cellZ)) + int cell_x = getCellFromLocation(DIM_X, x); + int cell_y = getCellFromLocation(DIM_Y, y); + int cell_z = getCellFromLocation(DIM_Z, z); + if (!isCellValid(cell_x, cell_y, cell_z)) return default_object_; - return getCell(cellX, cellY, cellZ); + return getCell(cell_x, cell_y, cell_z); } template diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index 071777706b..d150eb0612 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -358,7 +358,7 @@ void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length, switch (type) { - case XYPlane: + case XY_PLANE: min_z = height; max_z = height; @@ -367,7 +367,7 @@ void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length, min_y = -width / 2.0; max_y = width / 2.0; break; - case XZPlane: + case XZ_PLANE: min_y = height; max_y = height; @@ -376,7 +376,7 @@ void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length, min_z = -width / 2.0; max_z = width / 2.0; break; - case YZPlane: + case YZ_PLANE: min_x = height; max_x = height; diff --git a/moveit_core/distance_field/src/propagation_distance_field.cpp b/moveit_core/distance_field/src/propagation_distance_field.cpp index d656c22ff0..4bfa6fa1c3 100644 --- a/moveit_core/distance_field/src/propagation_distance_field.cpp +++ b/moveit_core/distance_field/src/propagation_distance_field.cpp @@ -148,7 +148,7 @@ void PropagationDistanceField::updatePointsInField(const EigenSTL::vector_Vector new_point_set.insert(voxel_loc); } } - compareEigen_Vector3i comp; + CompareEigenVector3i comp; EigenSTL::vector_Vector3i old_not_new; std::set_difference(old_point_set.begin(), old_point_set.end(), new_point_set.begin(), new_point_set.end(), diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index 7c5b1691e6..bb115456c8 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -144,7 +144,7 @@ class KinematicConstraint * * @param [in] out The file descriptor for printing */ - virtual void print(std::ostream& = std::cout) const + virtual void print(std::ostream& /*unused*/ = std::cout) const { } diff --git a/moveit_core/profiler/include/moveit/profiler/profiler.h b/moveit_core/profiler/include/moveit/profiler/profiler.h index 82b5901bce..1224016d07 100644 --- a/moveit_core/profiler/include/moveit/profiler/profiler.h +++ b/moveit_core/profiler/include/moveit/profiler/profiler.h @@ -135,19 +135,19 @@ class Profiler : private boost::noncopyable } /** \brief Start counting time */ - static void Start() + static void Start() // NOLINT(readability-identifier-naming) { instance().start(); } /** \brief Stop counting time */ - static void Stop() + static void Stop() // NOLINT(readability-identifier-naming) { instance().stop(); } /** \brief Clear counted time and events */ - static void Clear() + static void Clear() // NOLINT(readability-identifier-naming) { instance().clear(); } @@ -162,7 +162,7 @@ class Profiler : private boost::noncopyable void clear(); /** \brief Count a specific event for a number of times */ - static void Event(const std::string& name, const unsigned int times = 1) + static void Event(const std::string& name, const unsigned int times = 1) // NOLINT(readability-identifier-naming) { instance().event(name, times); } @@ -171,7 +171,7 @@ class Profiler : private boost::noncopyable void event(const std::string& name, const unsigned int times = 1); /** \brief Maintain the average of a specific value */ - static void Average(const std::string& name, const double value) + static void Average(const std::string& name, const double value) // NOLINT(readability-identifier-naming) { instance().average(name, value); } @@ -180,13 +180,13 @@ class Profiler : private boost::noncopyable void average(const std::string& name, const double value); /** \brief Begin counting time for a specific chunk of code */ - static void Begin(const std::string& name) + static void Begin(const std::string& name) // NOLINT(readability-identifier-naming) { instance().begin(name); } /** \brief Stop counting time for a specific chunk of code */ - static void End(const std::string& name) + static void End(const std::string& name) // NOLINT(readability-identifier-naming) { instance().end(name); } @@ -200,7 +200,7 @@ class Profiler : private boost::noncopyable /** \brief Print the status of the profiled code chunks and events. Optionally, computation done by different threads can be printed separately. */ - static void Status(std::ostream& out = std::cout, bool merge = true) + static void Status(std::ostream& out = std::cout, bool merge = true) // NOLINT(readability-identifier-naming) { instance().status(out, merge); } @@ -212,7 +212,7 @@ class Profiler : private boost::noncopyable /** \brief Print the status of the profiled code chunks and events to the console (using msg::Console) */ - static void Console() + static void Console() // NOLINT(readability-identifier-naming) { instance().console(); } @@ -228,7 +228,7 @@ class Profiler : private boost::noncopyable } /** \brief Check if the profiler is counting time or not */ - static bool Running() + static bool Running() // NOLINT(readability-identifier-naming) { return instance().running(); } diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h index 43e38e0dd4..c75e3938e9 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h @@ -177,27 +177,27 @@ class IKCacheMap : public std::unordered_map }; // Helper class to enable/disable initialize() methods with new/old API -// hasRobotModelAPI::value provides a true/false constexpr depending on KinematicsPlugin offers the new API +// HasRobotModelApi::value provides a true/false constexpr depending on KinematicsPlugin offers the new Api // This uses SFINAE magic: https://jguegant.github.io/blogs/tech/sfinae-introduction.html template -struct hasRobotModelAPI : std::false_type +struct HasRobotModelApi : std::false_type { }; template -struct hasRobotModelAPI().initialize( +struct HasRobotModelApi().initialize( std::declval(), std::string(), std::string(), std::vector(), 0.0))> : std::true_type { }; template -struct hasRobotDescAPI : std::false_type +struct HasRobotDescApi : std::false_type { }; template -struct hasRobotDescAPI().KinematicsPlugin::initialize( std::string(), std::string(), std::string(), std::vector(), 0.0))> : std::true_type @@ -265,7 +265,7 @@ class CachedIKKinematicsPlugin : public KinematicsPlugin availability of API in wrapped KinematicsPlugin class. However, as templates and virtual functions cannot be combined, we need helpers initializeImpl(). */ template - typename std::enable_if::value, bool>::type + typename std::enable_if::value, bool>::type initializeImpl(const moveit::core::RobotModel& robot_model, const std::string& group_name, const std::string& base_frame, const std::vector& tip_frames, double search_discretization) @@ -284,15 +284,15 @@ class CachedIKKinematicsPlugin : public KinematicsPlugin } template - typename std::enable_if::value, bool>::type - initializeImpl(const moveit::core::RobotModel&, const std::string&, const std::string&, - const std::vector&, double) + typename std::enable_if::value, bool>::type + initializeImpl(const moveit::core::RobotModel& /*unused*/, const std::string& /*unused*/, const std::string& /*unused*/, + const std::vector& /*unused*/, double /*unused*/) { return false; // API not supported } template - typename std::enable_if::value, bool>::type + typename std::enable_if::value, bool>::type initializeImpl(const std::string& robot_description, const std::string& group_name, const std::string& base_frame, const std::string& tip_frame, double search_discretization) { @@ -304,8 +304,8 @@ class CachedIKKinematicsPlugin : public KinematicsPlugin } template - typename std::enable_if::value, bool>::type - initializeImpl(const std::string&, const std::string&, const std::string&, const std::string&, double) + typename std::enable_if::value, bool>::type + initializeImpl(const std::string& /*unused*/, const std::string& /*unused*/, const std::string& /*unused*/, const std::string& /*unused*/, double /*unused*/) { return false; // API not supported } diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h index 5deab2dc97..1a311af4eb 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h @@ -84,7 +84,7 @@ class GreedyKCenters { // array containing the minimum distance between each data point // and the centers computed so far - std::vector minDist(data.size(), std::numeric_limits::infinity()); + std::vector min_dist(data.size(), std::numeric_limits::infinity()); centers.clear(); centers.reserve(k); @@ -96,20 +96,20 @@ class GreedyKCenters { unsigned ind = 0; const _T& center = data[centers[i - 1]]; - double maxDist = -std::numeric_limits::infinity(); + double max_dist = -std::numeric_limits::infinity(); for (unsigned j = 0; j < data.size(); ++j) { - if ((dists(j, i - 1) = distFun_(data[j], center)) < minDist[j]) - minDist[j] = dists(j, i - 1); + if ((dists(j, i - 1) = distFun_(data[j], center)) < min_dist[j]) + min_dist[j] = dists(j, i - 1); // the j-th center is the one furthest away from center 0,..,j-1 - if (minDist[j] > maxDist) + if (min_dist[j] > max_dist) { ind = j; - maxDist = minDist[j]; + max_dist = min_dist[j]; } } // no more centers available - if (maxDist < std::numeric_limits::epsilon()) + if (max_dist < std::numeric_limits::epsilon()) break; centers.push_back(ind); } diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h index 13f2482c67..4c4f180852 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h @@ -186,17 +186,17 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> { if (size_ == 0u) return false; - NearQueue nbhQueue; + NearQueue nbh_queue; // find data in tree - bool isPivot = nearestKInternal(data, 1, nbhQueue); - const _T* d = nbhQueue.top().first; + bool is_pivot = nearestKInternal(data, 1, nbh_queue); + const _T* d = nbh_queue.top().first; if (*d != data) return false; removed_.insert(d); size_--; // if we removed a pivot or if the capacity of removed elements // has been reached, we rebuild the entire GNAT - if (isPivot || removed_.size() >= removedCacheSize_) + if (is_pivot || removed_.size() >= removedCacheSize_) rebuildDataStructure(); return true; } @@ -205,10 +205,10 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> { if (size_) { - NearQueue nbhQueue; - nearestKInternal(data, 1, nbhQueue); - if (!nbhQueue.empty()) - return *nbhQueue.top().first; + NearQueue nbh_queue; + nearestKInternal(data, 1, nbh_queue); + if (!nbh_queue.empty()) + return *nbh_queue.top().first; } throw moveit::Exception("No elements found in nearest neighbors data structure"); } @@ -221,9 +221,9 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> return; if (size_) { - NearQueue nbhQueue; - nearestKInternal(data, k, nbhQueue); - postprocessNearest(nbhQueue, nbh); + NearQueue nbh_queue; + nearestKInternal(data, k, nbh_queue); + postprocessNearest(nbh_queue, nbh); } } @@ -233,9 +233,9 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> nbh.clear(); if (size_) { - NearQueue nbhQueue; - nearestRInternal(data, radius, nbhQueue); - postprocessNearest(nbhQueue, nbh); + NearQueue nbh_queue; + nearestRInternal(data, radius, nbh_queue); + postprocessNearest(nbh_queue, nbh); } } @@ -319,42 +319,42 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> // special case). bool nearestKInternal(const _T& data, std::size_t k, NearQueue& nbhQueue) const { - bool isPivot; + bool is_pivot; double dist; - NodeDist nodeDist; - NodeQueue nodeQueue; + NodeDist node_dist; + NodeQueue node_queue; dist = NearestNeighbors<_T>::distFun_(data, tree_->pivot_); - isPivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data, dist); - tree_->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot); - while (!nodeQueue.empty()) + is_pivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data, dist); + tree_->nearestK(*this, data, k, nbhQueue, node_queue, is_pivot); + while (!node_queue.empty()) { dist = nbhQueue.top().second; // note the difference with nearestRInternal - nodeDist = nodeQueue.top(); - nodeQueue.pop(); + node_dist = node_queue.top(); + node_queue.pop(); if (nbhQueue.size() == k && - (nodeDist.second > nodeDist.first->maxRadius_ + dist || nodeDist.second < nodeDist.first->minRadius_ - dist)) + (node_dist.second > node_dist.first->maxRadius_ + dist || node_dist.second < node_dist.first->minRadius_ - dist)) continue; - nodeDist.first->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot); + node_dist.first->nearestK(*this, data, k, nbhQueue, node_queue, is_pivot); } - return isPivot; + return is_pivot; } // \brief Return in nbhQueue the elements that are within distance radius of data. void nearestRInternal(const _T& data, double radius, NearQueue& nbhQueue) const { double dist = radius; // note the difference with nearestKInternal - NodeQueue nodeQueue; - NodeDist nodeDist; + NodeQueue node_queue; + NodeDist node_dist; tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_, NearestNeighbors<_T>::distFun_(data, tree_->pivot_)); - tree_->nearestR(*this, data, radius, nbhQueue, nodeQueue); - while (!nodeQueue.empty()) + tree_->nearestR(*this, data, radius, nbhQueue, node_queue); + while (!node_queue.empty()) { - nodeDist = nodeQueue.top(); - nodeQueue.pop(); - if (nodeDist.second > nodeDist.first->maxRadius_ + dist || nodeDist.second < nodeDist.first->minRadius_ - dist) + node_dist = node_queue.top(); + node_queue.pop(); + if (node_dist.second > node_dist.first->maxRadius_ + dist || node_dist.second < node_dist.first->minRadius_ - dist) continue; - nodeDist.first->nearestR(*this, data, radius, nbhQueue, nodeQueue); + node_dist.first->nearestR(*this, data, radius, nbhQueue, node_queue); } } // \brief Convert the internal data structure used for storing neighbors @@ -443,19 +443,19 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> else { std::vector dist(children_.size()); - double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_); - int minInd = 0; + double min_dist = dist[0] = gnat.distFun_(data, children_[0]->pivot_); + int min_ind = 0; for (unsigned int i = 1; i < children_.size(); ++i) - if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist) + if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < min_dist) { - minDist = dist[i]; - minInd = i; + min_dist = dist[i]; + min_ind = i; } for (unsigned int i = 0; i < children_.size(); ++i) - children_[i]->updateRange(minInd, dist[i]); - children_[minInd]->updateRadius(minDist); - children_[minInd]->add(gnat, data); + children_[i]->updateRange(min_ind, dist[i]); + children_[min_ind]->updateRadius(min_dist); + children_[min_ind]->add(gnat, data); } } // Return true iff the node needs to be split into child nodes. @@ -547,7 +547,7 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> { double dist; Node* child; - std::vector distToPivot(children_.size()); + std::vector dist_to_pivot(children_.size()); std::vector permutation(children_.size()); for (unsigned int i = 0; i < permutation.size(); ++i) permutation[i] = i; @@ -557,16 +557,16 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> if (permutation[i] >= 0) { child = children_[permutation[i]]; - distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_); - if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]])) + dist_to_pivot[permutation[i]] = gnat.distFun_(data, child->pivot_); + if (insertNeighborK(nbh, k, child->pivot_, data, dist_to_pivot[permutation[i]])) isPivot = true; if (nbh.size() == k) { dist = nbh.top().second; // note difference with nearestR for (unsigned int j = 0; j < children_.size(); ++j) if (permutation[j] >= 0 && i != j && - (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] || - distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]])) + (dist_to_pivot[permutation[i]] - dist > child->maxRange_[permutation[j]] || + dist_to_pivot[permutation[i]] + dist < child->minRange_[permutation[j]])) permutation[j] = -1; } } @@ -576,9 +576,9 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> if (permutation[i] >= 0) { child = children_[permutation[i]]; - if (nbh.size() < k || (distToPivot[permutation[i]] - dist <= child->maxRadius_ && - distToPivot[permutation[i]] + dist >= child->minRadius_)) - nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]])); + if (nbh.size() < k || (dist_to_pivot[permutation[i]] - dist <= child->maxRadius_ && + dist_to_pivot[permutation[i]] + dist >= child->minRadius_)) + nodeQueue.push(std::make_pair(child, dist_to_pivot[permutation[i]])); } } } @@ -601,7 +601,7 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> if (!children_.empty()) { Node* child; - std::vector distToPivot(children_.size()); + std::vector dist_to_pivot(children_.size()); std::vector permutation(children_.size()); for (unsigned int i = 0; i < permutation.size(); ++i) permutation[i] = i; @@ -611,11 +611,11 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> if (permutation[i] >= 0) { child = children_[permutation[i]]; - distToPivot[i] = gnat.distFun_(data, child->pivot_); - insertNeighborR(nbh, r, child->pivot_, distToPivot[i]); + dist_to_pivot[i] = gnat.distFun_(data, child->pivot_); + insertNeighborR(nbh, r, child->pivot_, dist_to_pivot[i]); for (unsigned int j = 0; j < children_.size(); ++j) - if (permutation[j] >= 0 && i != j && (distToPivot[i] - dist > child->maxRange_[permutation[j]] || - distToPivot[i] + dist < child->minRange_[permutation[j]])) + if (permutation[j] >= 0 && i != j && (dist_to_pivot[i] - dist > child->maxRange_[permutation[j]] || + dist_to_pivot[i] + dist < child->minRange_[permutation[j]])) permutation[j] = -1; } @@ -623,8 +623,8 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> if (permutation[i] >= 0) { child = children_[permutation[i]]; - if (distToPivot[i] - dist <= child->maxRadius_ && distToPivot[i] + dist >= child->minRadius_) - nodeQueue.push(std::make_pair(child, distToPivot[i])); + if (dist_to_pivot[i] - dist <= child->maxRadius_ && dist_to_pivot[i] + dist >= child->minRadius_) + nodeQueue.push(std::make_pair(child, dist_to_pivot[i])); } } } diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp index 72cfd99a27..782507bfed 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp @@ -79,6 +79,7 @@ class ChainIkSolverVelMimicSVD : public ChainIkSolverVel Eigen::Matrix::Constant(1.0)); } + // NOLINTNEXTLINE(readability-identifier-naming) /** Compute qdot_out = W_q * (W_x * J * W_q)^# * W_x * v_in * * where W_q and W_x are joint- and Cartesian weights respectively. diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h index 16f381ed65..977c3caf28 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h @@ -124,6 +124,7 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase protected: typedef Eigen::Matrix Twist; + // NOLINTNEXTLINE(readability-identifier-naming) /// Solve position IK given initial joint values int CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out, const unsigned int max_iter, const Eigen::VectorXd& joint_weights, diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h index 10bd896438..e197008a2c 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h @@ -294,11 +294,11 @@ template void ChompTrajectory::getJointVelocities(size_t traj_point, Eigen::MatrixBase& velocities) { velocities.setZero(); - double invTime = 1.0 / discretization_; + double inv_time = 1.0 / discretization_; for (int k = -DIFF_RULE_LENGTH / 2; k <= DIFF_RULE_LENGTH / 2; k++) { - velocities += (invTime * DIFF_RULES[0][k + DIFF_RULE_LENGTH / 2]) * trajectory_.row(traj_point + k).transpose(); + velocities += (inv_time * DIFF_RULES[0][k + DIFF_RULE_LENGTH / 2]) * trajectory_.row(traj_point + k).transpose(); } } diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h index a982a0215f..b794208d4a 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h @@ -58,7 +58,7 @@ class ConstrainedGoalSampler : public ompl::base::GoalLazySamples private: bool sampleUsingConstraintSampler(const ompl::base::GoalLazySamples* gls, ompl::base::State* new_goal); bool stateValidityCallback(ompl::base::State* new_goal, moveit::core::RobotState const* state, - const moveit::core::JointModelGroup*, const double*, bool verbose = false) const; + const moveit::core::JointModelGroup* /*jmg*/, const double* /*jpos*/, bool verbose = false) const; bool checkStateValidity(ompl::base::State* new_goal, const moveit::core::RobotState& state, bool verbose = false) const; diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h index f33ed0656a..35ae72ec86 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h @@ -70,7 +70,7 @@ class LastPointController : public BaseFakeController bool sendTrajectory(const moveit_msgs::RobotTrajectory& t) override; bool cancelExecution() override; - bool waitForExecution(const ros::Duration&) override; + bool waitForExecution(const ros::Duration& /*timeout*/) override; }; class ThreadedController : public BaseFakeController @@ -81,7 +81,7 @@ class ThreadedController : public BaseFakeController bool sendTrajectory(const moveit_msgs::RobotTrajectory& t) override; bool cancelExecution() override; - bool waitForExecution(const ros::Duration&) override; + bool waitForExecution(const ros::Duration& /*timeout*/) override; moveit_controller_manager::ExecutionStatus getLastExecutionStatus() override; protected: diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index c2da776a0c..b57d9e5418 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -473,7 +473,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP for (int x = skip_horizontal_pixels_; x < w_bound; ++x) { // not filtered - if (labels_row[x] == mesh_filter::MeshFilterBase::Background) + if (labels_row[x] == mesh_filter::MeshFilterBase::BACKGROUND) { float zz = (float)input_row[x] * 1e-3; // scale from mm to m float yy = y_cache_[y] * zz; @@ -483,7 +483,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); } // on far plane or a model point -> remove - else if (labels_row[x] >= mesh_filter::MeshFilterBase::FarClip) + else if (labels_row[x] >= mesh_filter::MeshFilterBase::FAR_CLIP) { float zz = input_row[x] * 1e-3; float yy = y_cache_[y] * zz; @@ -502,7 +502,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP for (int y = skip_vertical_pixels_; y < h_bound; ++y, labels_row += w, input_row += w) for (int x = skip_horizontal_pixels_; x < w_bound; ++x) { - if (labels_row[x] == mesh_filter::MeshFilterBase::Background) + if (labels_row[x] == mesh_filter::MeshFilterBase::BACKGROUND) { float zz = input_row[x]; float yy = y_cache_[y] * zz; @@ -511,7 +511,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(xx, yy, zz); occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); } - else if (labels_row[x] >= mesh_filter::MeshFilterBase::FarClip) + else if (labels_row[x] >= mesh_filter::MeshFilterBase::FAR_CLIP) { float zz = input_row[x]; float yy = y_cache_[y] * zz; diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h index dc8ee5f3f2..6fe3fb0745 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h @@ -69,11 +69,11 @@ class MeshFilterBase // this would allow me to do a single comparison instead of 3, in the code i write enum { - Background = 0, - Shadow = 1, - NearClip = 2, - FarClip = 3, - FirstLabel = 16 + BACKGROUND = 0, + SHADOW = 1, + NEAR_CLIP = 2, + FAR_CLIP = 3, + FIRST_LABEL = 16 }; public: diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h index d6dc185be1..5ffd8174d3 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h @@ -148,7 +148,7 @@ class StereoCameraModel : public SensorModel }; /** \brief predefined sensor model for OpenNI compatible devices (e.g., PrimeSense, Kinect, Asus Xtion) */ - static const StereoCameraModel::Parameters& REGISTERED_PSDK_PARAMS; + static const StereoCameraModel::Parameters& REGISTERED_PSDK_PARAMS; // NOLINT(readability-identifier-naming) /** \brief source code of the vertex shader used to render the meshes*/ static const std::string RENDER_VERTEX_SHADER_SOURCE; diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp index 67a241c77c..331dfe3780 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp @@ -59,8 +59,8 @@ mesh_filter::MeshFilterBase::MeshFilterBase(const TransformCallback& transform_c const std::string& filter_vertex_shader, const std::string& filter_fragment_shader) : sensor_parameters_(sensor_parameters.clone()) - , next_handle_(FirstLabel) // 0 and 1 are reserved! - , min_handle_(FirstLabel) + , next_handle_(FIRST_LABEL) // 0 and 1 are reserved! + , min_handle_(FIRST_LABEL) , stop_(false) , transform_callback_(transform_callback) , padding_scale_(1.0) diff --git a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp index a452eb14f3..2e09379c5e 100644 --- a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp +++ b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp @@ -251,11 +251,11 @@ void MeshFilterTest::getGroundTruth(unsigned int* labels, float* depth) co { depth[idx] = double(sensor_data_[idx]) * scale; if (depth[idx] < near_) - labels[idx] = MeshFilterBase::NearClip; + labels[idx] = MeshFilterBase::NEAR_CLIP; else if (depth[idx] >= far_) - labels[idx] = MeshFilterBase::FarClip; + labels[idx] = MeshFilterBase::FAR_CLIP; else - labels[idx] = MeshFilterBase::Background; + labels[idx] = MeshFilterBase::BACKGROUND; if (depth[idx] <= near_ || depth[idx] >= far_) depth[idx] = 0; @@ -272,21 +272,21 @@ void MeshFilterTest::getGroundTruth(unsigned int* labels, float* depth) co if (depth[idx] < near_) { - labels[idx] = MeshFilterBase::NearClip; + labels[idx] = MeshFilterBase::NEAR_CLIP; depth[idx] = 0; } else { double diff = depth[idx] - distance_; if (diff < 0 && depth[idx] < far_) - labels[idx] = MeshFilterBase::Background; + labels[idx] = MeshFilterBase::BACKGROUND; else if (diff > shadow_) - labels[idx] = MeshFilterBase::Shadow; + labels[idx] = MeshFilterBase::SHADOW; else if (depth[idx] >= far_) - labels[idx] = MeshFilterBase::FarClip; + labels[idx] = MeshFilterBase::FAR_CLIP; else { - labels[idx] = MeshFilterBase::FirstLabel; + labels[idx] = MeshFilterBase::FIRST_LABEL; depth[idx] = 0; } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h index 6cc3b42af3..88b9795788 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h @@ -164,8 +164,8 @@ class ProgressBarDelegate : public QStyledItemDelegate public: enum CustomRole { - JointTypeRole = Qt::UserRole, - VariableBoundsRole + JointTypeRole = Qt::UserRole, // NOLINT(readability-identifier-naming) + VariableBoundsRole // NOLINT(readability-identifier-naming) }; ProgressBarDelegate(QWidget* parent = nullptr) : QStyledItemDelegate(parent) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 394eac0875..f1a422e8b3 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -54,8 +54,8 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; static const std::string MOVEIT_ROBOT_STATE = "moveit_robot_state"; // Default kin solver values -static const double DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION_ = 0.005; -static const double DEFAULT_KIN_SOLVER_TIMEOUT_ = 0.005; +static const double DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION = 0.005; +static const double DEFAULT_KIN_SOLVER_TIMEOUT = 0.005; // ****************************************************************************************** // Structs @@ -510,7 +510,7 @@ class MoveItConfigData * \param jm2 - a pointer to the second joint model to compare * \return bool of alphabetical sorting comparison */ - struct joint_model_compare + struct JointModelCompare { bool operator()(const moveit::core::JointModel* jm1, const moveit::core::JointModel* jm2) const { diff --git a/moveit_setup_assistant/src/tools/collision_matrix_model.h b/moveit_setup_assistant/src/tools/collision_matrix_model.h index 9d4b9d2d78..8d88b68bf5 100644 --- a/moveit_setup_assistant/src/tools/collision_matrix_model.h +++ b/moveit_setup_assistant/src/tools/collision_matrix_model.h @@ -58,7 +58,7 @@ class CollisionMatrixModel : public QAbstractTableModel // for editing Qt::ItemFlags flags(const QModelIndex& index) const override; - bool setData(const QModelIndex&, const QVariant& value, int role) override; + bool setData(const QModelIndex& /*index*/, const QVariant& value, int role) override; void setEnabled(const QItemSelection& selection, bool value); void setEnabled(const QModelIndexList& indexes, bool value); diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 462b07b951..c1b7f324a5 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -1108,7 +1108,7 @@ bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) emitter << YAML::Value << YAML::BeginMap; // Union all the joints in groups. Uses a custom comparator to allow the joints to be sorted by name - std::set joints; + std::set joints; // Loop through groups for (srdf::Model::Group& group : srdf_->groups_) @@ -1344,8 +1344,8 @@ bool MoveItConfigData::inputKinematicsYAML(const std::string& file_path) parse(group, "kinematics_solver", meta_data.kinematics_solver_); parse(group, "kinematics_solver_search_resolution", meta_data.kinematics_solver_search_resolution_, - DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION_); - parse(group, "kinematics_solver_timeout", meta_data.kinematics_solver_timeout_, DEFAULT_KIN_SOLVER_TIMEOUT_); + DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION); + parse(group, "kinematics_solver_timeout", meta_data.kinematics_solver_timeout_, DEFAULT_KIN_SOLVER_TIMEOUT); // Assign meta data to vector group_meta_data_[group_name] = meta_data; diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp index c744036597..9f8bdeb1b3 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp @@ -187,12 +187,12 @@ DefaultCollisionsWidget::DefaultCollisionsWidget(QWidget* parent, const MoveItCo QRadioButton* radio_btn; radio_btn = new QRadioButton("linear view"); bottom_layout->addWidget(radio_btn); - view_mode_buttons_->addButton(radio_btn, LinearMode); + view_mode_buttons_->addButton(radio_btn, LINEAR_MODE); radio_btn->setChecked(true); radio_btn = new QRadioButton("matrix view"); bottom_layout->addWidget(radio_btn); - view_mode_buttons_->addButton(radio_btn, MatrixMode); + view_mode_buttons_->addButton(radio_btn, MATRIX_MODE); connect(view_mode_buttons_, SIGNAL(buttonClicked(int)), this, SLOT(loadCollisionTable())); // Revert Button @@ -282,7 +282,7 @@ void DefaultCollisionsWidget::loadCollisionTable() link_pairs_, config_data_->getPlanningScene()->getRobotModel()->getLinkModelNamesWithCollisionGeometry()); QAbstractItemModel* model; - if (view_mode_buttons_->checkedId() == MatrixMode) + if (view_mode_buttons_->checkedId() == MATRIX_MODE) { model = matrix_model; } @@ -311,7 +311,7 @@ void DefaultCollisionsWidget::loadCollisionTable() QHeaderView *horizontal_header, *vertical_header; // activate some model-specific settings - if (view_mode_buttons_->checkedId() == MatrixMode) + if (view_mode_buttons_->checkedId() == MATRIX_MODE) { connect(selection_model_, SIGNAL(currentChanged(QModelIndex, QModelIndex)), this, SLOT(previewSelectedMatrix(QModelIndex))); @@ -377,7 +377,7 @@ void DefaultCollisionsWidget::collisionsChanged(const QModelIndex& index) if (!index.isValid()) return; // Hm. For some reason, QTableView doesn't change selection if we click a checkbox - bool linear_mode = (view_mode_buttons_->checkedId() == LinearMode); + bool linear_mode = (view_mode_buttons_->checkedId() == LINEAR_MODE); const QItemSelection& selection = selection_model_->selection(); if ((linear_mode && !selection.contains(index)) || // in linear mode: index not in selection (!linear_mode && @@ -592,7 +592,7 @@ void DefaultCollisionsWidget::toggleSelection(QItemSelection selection) // set all selected items to inverse value of current item const QModelIndex& cur_idx = selection_model_->currentIndex(); - if (view_mode_buttons_->checkedId() == MatrixMode) + if (view_mode_buttons_->checkedId() == MATRIX_MODE) { QModelIndex input_index; if (cur_idx.flags() & Qt::ItemIsUserCheckable) diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.h b/moveit_setup_assistant/src/widgets/default_collisions_widget.h index ade0c7f30b..105dcb754c 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.h +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.h @@ -72,8 +72,8 @@ class DefaultCollisionsWidget : public SetupScreenWidget public: enum ViewMode { - MatrixMode = 0, - LinearMode = 1 + MATRIX_MODE = 0, + LINEAR_MODE = 1 }; // ****************************************************************************************** diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp index f857926413..3ecd04a16c 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp @@ -235,7 +235,7 @@ void GroupEditWidget::setSelected(const std::string& group_name) if (*resolution == 0) { // Set default value - *resolution = DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION_; + *resolution = DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION; } kinematics_resolution_field_->setText(QString::number(*resolution)); @@ -244,7 +244,7 @@ void GroupEditWidget::setSelected(const std::string& group_name) if (*timeout == 0) { // Set default value - *timeout = DEFAULT_KIN_SOLVER_TIMEOUT_; + *timeout = DEFAULT_KIN_SOLVER_TIMEOUT; } kinematics_timeout_field_->setText(QString::number(*timeout)); From e619b6ab849ae837c01a54ec715d4c604014621c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 4 May 2020 19:57:17 +0200 Subject: [PATCH 271/612] performance-* --- .../moveit/setup_assistant/tools/moveit_config_data.h | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index f1a422e8b3..d60629d514 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -43,6 +43,10 @@ #include // to share throughout app #include // for writing srdf data + +#include + + namespace moveit_setup_assistant { // ****************************************************************************************** @@ -143,15 +147,15 @@ class GenericParameter void setName(std::string name) { - name_ = name; + name_ = std::move(name); }; void setValue(std::string value) { - value_ = value; + value_ = std::move(value); }; void setComment(std::string comment) { - comment_ = comment; + comment_ = std::move(comment); }; std::string getName() { From 74bc601dce5e4f862652abf2a6026b1d8358d9ba Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 4 May 2020 20:22:35 +0200 Subject: [PATCH 272/612] clang-format --- .../collision_env_bullet.h | 28 ++++++++----------- .../collision_env_fcl.h | 28 ++++++++----------- .../collision_distance_field_types.h | 3 +- .../collision_env_distance_field.h | 10 +++---- .../kinematic_constraint.h | 3 +- .../include/moveit/robot_state/robot_state.h | 3 +- .../cached_ik_kinematics_plugin.h | 7 +++-- .../detail/NearestNeighborsGNAT.h | 7 +++-- .../include/chomp_motion_planner/chomp_cost.h | 3 +- .../detail/constrained_goal_sampler.h | 3 +- .../motion_planning_frame_joints_widget.h | 2 +- .../tools/moveit_config_data.h | 2 -- 12 files changed, 45 insertions(+), 54 deletions(-) diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h index ba502f2e00..e210322ebd 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h @@ -60,32 +60,28 @@ class CollisionEnvBullet : public CollisionEnv CollisionEnvBullet(CollisionEnvBullet&) = delete; void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const override; + const moveit::core::RobotState& state) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const override; + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const override; + const moveit::core::RobotState& state) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, - const AllowedCollisionMatrix& acm) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const override; + const moveit::core::RobotState& state) const override; void distanceRobot(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const override; + const moveit::core::RobotState& state) const override; void setWorld(const WorldPtr& world) override; diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h index dbdfa05673..278071b3ce 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h @@ -65,32 +65,28 @@ class CollisionEnvFCL : public CollisionEnv ~CollisionEnvFCL() override; void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const override; + const moveit::core::RobotState& state) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const override; + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const override; + const moveit::core::RobotState& state) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, - const AllowedCollisionMatrix& acm) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const override; + const moveit::core::RobotState& state) const override; void distanceRobot(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const override; + const moveit::core::RobotState& state) const override; void setWorld(const WorldPtr& world) override; diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index 37a5a664b8..f38c57aa24 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -452,8 +452,7 @@ class PosedBodyPointDecompositionVector EigenSTL::vector_Vector3d ret_points; for (const PosedBodyPointDecompositionPtr& decomp : decomp_vector_) { - ret_points.insert(ret_points.end(), decomp->getCollisionPoints().begin(), - decomp->getCollisionPoints().end()); + ret_points.insert(ret_points.end(), decomp->getCollisionPoints().begin(), decomp->getCollisionPoints().end()); } return ret_points; } diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h index 3d1d81a55d..a1f910d359 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h @@ -169,13 +169,11 @@ class CollisionEnvDistanceField : public CollisionEnv const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, - const AllowedCollisionMatrix& acm) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; virtual double distanceRobot(const moveit::core::RobotState& state, bool verbose = false) const { diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index bb115456c8..2eca8f9bc1 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -355,7 +355,8 @@ class OrientationConstraint : public KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - OrientationConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(nullptr) + OrientationConstraint(const moveit::core::RobotModelConstPtr& model) + : KinematicConstraint(model), link_model_(nullptr) { type_ = ORIENTATION_CONSTRAINT; } diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index a03c5bde7d..e8463964fb 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1767,8 +1767,7 @@ class RobotState for (const JointModel* jm : mim) { const int fvi = jm->getFirstVariableIndex(); - position_[fvi] = - jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); + position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); // Only mark joint transform dirty, but not the associated link transform // as this function is always used in combination of // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group); diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h index c75e3938e9..9038da5a55 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h @@ -285,8 +285,8 @@ class CachedIKKinematicsPlugin : public KinematicsPlugin template typename std::enable_if::value, bool>::type - initializeImpl(const moveit::core::RobotModel& /*unused*/, const std::string& /*unused*/, const std::string& /*unused*/, - const std::vector& /*unused*/, double /*unused*/) + initializeImpl(const moveit::core::RobotModel& /*unused*/, const std::string& /*unused*/, + const std::string& /*unused*/, const std::vector& /*unused*/, double /*unused*/) { return false; // API not supported } @@ -305,7 +305,8 @@ class CachedIKKinematicsPlugin : public KinematicsPlugin template typename std::enable_if::value, bool>::type - initializeImpl(const std::string& /*unused*/, const std::string& /*unused*/, const std::string& /*unused*/, const std::string& /*unused*/, double /*unused*/) + initializeImpl(const std::string& /*unused*/, const std::string& /*unused*/, const std::string& /*unused*/, + const std::string& /*unused*/, double /*unused*/) { return false; // API not supported } diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h index 4c4f180852..090c50057f 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h @@ -332,8 +332,8 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> dist = nbhQueue.top().second; // note the difference with nearestRInternal node_dist = node_queue.top(); node_queue.pop(); - if (nbhQueue.size() == k && - (node_dist.second > node_dist.first->maxRadius_ + dist || node_dist.second < node_dist.first->minRadius_ - dist)) + if (nbhQueue.size() == k && (node_dist.second > node_dist.first->maxRadius_ + dist || + node_dist.second < node_dist.first->minRadius_ - dist)) continue; node_dist.first->nearestK(*this, data, k, nbhQueue, node_queue, is_pivot); } @@ -352,7 +352,8 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> { node_dist = node_queue.top(); node_queue.pop(); - if (node_dist.second > node_dist.first->maxRadius_ + dist || node_dist.second < node_dist.first->minRadius_ - dist) + if (node_dist.second > node_dist.first->maxRadius_ + dist || + node_dist.second < node_dist.first->minRadius_ - dist) continue; node_dist.first->nearestR(*this, data, radius, nbhQueue, node_queue); } diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h index ac4659582e..7e76fbe1c6 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h @@ -75,7 +75,8 @@ class ChompCost }; template -void ChompCost::getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory, Eigen::MatrixBase& derivative) const +void ChompCost::getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory, + Eigen::MatrixBase& derivative) const { derivative = (quad_cost_full_ * (2.0 * joint_trajectory)); } diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h index b794208d4a..be711c991f 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h @@ -58,7 +58,8 @@ class ConstrainedGoalSampler : public ompl::base::GoalLazySamples private: bool sampleUsingConstraintSampler(const ompl::base::GoalLazySamples* gls, ompl::base::State* new_goal); bool stateValidityCallback(ompl::base::State* new_goal, moveit::core::RobotState const* state, - const moveit::core::JointModelGroup* /*jmg*/, const double* /*jpos*/, bool verbose = false) const; + const moveit::core::JointModelGroup* /*jmg*/, const double* /*jpos*/, + bool verbose = false) const; bool checkStateValidity(ompl::base::State* new_goal, const moveit::core::RobotState& state, bool verbose = false) const; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h index 88b9795788..8ae1258030 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h @@ -165,7 +165,7 @@ class ProgressBarDelegate : public QStyledItemDelegate enum CustomRole { JointTypeRole = Qt::UserRole, // NOLINT(readability-identifier-naming) - VariableBoundsRole // NOLINT(readability-identifier-naming) + VariableBoundsRole // NOLINT(readability-identifier-naming) }; ProgressBarDelegate(QWidget* parent = nullptr) : QStyledItemDelegate(parent) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index d60629d514..99e58e9ece 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -43,10 +43,8 @@ #include // to share throughout app #include // for writing srdf data - #include - namespace moveit_setup_assistant { // ****************************************************************************************** From 1bfa63f0aa8475d7d9343070592fc4ef15de9d81 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 5 May 2020 19:28:07 +0200 Subject: [PATCH 273/612] RS: add interfaces to zero and remove dynamics Sure, some of them will probably almost never be used, but I prefer full flexibility over missing an intuitive interface in the long run. Before this patch it was impossible to remove dynamics information from a state once it was added. --- .../include/moveit/robot_state/robot_state.h | 21 ++++++++++ moveit_core/robot_state/src/robot_state.cpp | 40 +++++++++++++++++++ 2 files changed, 61 insertions(+) diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index e8463964fb..4e6a41bcb4 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -246,6 +246,9 @@ class RobotState return velocity_; } + /** \brief Set all velocities to 0.0 */ + void zeroVelocities(); + /** \brief Given an array with velocity values for all variables, set those values as the velocities in this state */ void setVariableVelocities(const double* velocity) { @@ -303,6 +306,9 @@ class RobotState return velocity_[index]; } + /** \brief Remove velocities from this state (this differs from setting them to zero) */ + void dropVelocities(); + /** @} */ /** \name Getting and setting variable acceleration @@ -334,6 +340,9 @@ class RobotState return acceleration_; } + /** \brief Set all accelerations to 0.0 */ + void zeroAccelerations(); + /** \brief Given an array with acceleration values for all variables, set those values as the accelerations in this * state */ void setVariableAccelerations(const double* acceleration) @@ -396,6 +405,9 @@ class RobotState return acceleration_[index]; } + /** \brief Remove accelerations from this state (this differs from setting them to zero) */ + void dropAccelerations(); + /** @} */ /** \name Getting and setting variable effort @@ -426,6 +438,9 @@ class RobotState return effort_; } + /** \brief Set all effort values to 0.0 */ + void zeroEffort(); + /** \brief Given an array with effort values for all variables, set those values as the effort in this state */ void setVariableEffort(const double* effort) { @@ -482,6 +497,12 @@ class RobotState return effort_[index]; } + /** \brief Remove effort values from this state (this differs from setting them to zero) */ + void dropEffort(); + + /** \brief Reduce RobotState to kinematic information (remove velocity, acceleration and effort, if present) */ + void dropDynamics(); + /** \brief Invert velocity if present. */ void invertVelocity(); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 11e2440759..c0a4f5e214 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -232,6 +232,46 @@ void RobotState::markEffort() } } +void RobotState::zeroVelocities() +{ + has_velocity_ = false; + markVelocity(); +} + +void RobotState::zeroAccelerations() +{ + has_acceleration_ = false; + markAcceleration(); +} + +void RobotState::zeroEffort() +{ + has_effort_ = false; + markEffort(); +} + +void RobotState::dropVelocities() +{ + has_velocity_ = false; +} + +void RobotState::dropAccelerations() +{ + has_velocity_ = false; +} + +void RobotState::dropEffort() +{ + has_velocity_ = false; +} + +void RobotState::dropDynamics() +{ + dropVelocities(); + dropAccelerations(); + dropEffort(); +} + void RobotState::setToRandomPositions() { random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator(); From 12797f11773f1d105d689d0c2a5aff33bd02dc54 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 5 May 2020 19:31:27 +0200 Subject: [PATCH 274/612] totg: add a correct time parameterization for 1-waypoint trajectories .. as they might be generated by a planner when the goal is already reached. --- .../src/time_optimal_trajectory_generation.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 4bba94268c..076b1c482b 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -976,8 +976,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT // Return trajectory with only the first waypoint if there are not multiple diverse points if (points.size() == 1) { - ROS_WARN_NAMED(LOGNAME, "Trajectory is not being parameterized since it only contains a single distinct waypoint."); + ROS_DEBUG_NAMED(LOGNAME, + "Trajectory is parameterized with 0.0 dynamics since it only contains a single distinct waypoint."); moveit::core::RobotState waypoint = moveit::core::RobotState(trajectory.getWayPoint(0)); + waypoint.zeroVelocities(); + waypoint.zeroAccelerations(); trajectory.clear(); trajectory.addSuffixWayPoint(waypoint, 0.0); return true; From 7d7bd020cb8338fa19c8180220a1b0760ec3b39f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 6 May 2020 10:13:37 +0200 Subject: [PATCH 275/612] fix ordering of request adapters (#2053) Some auxiliary PRAs can create additional waypoints, so time parameterization must run *after* them on return of a successful plan, which means it has to be loaded *before* them in the PRA chain. Additionally, constraint names have to be resolved *before* fixing the input state w.r.t. them. Look up the source code and documentation if you don't believe it. --- .../launch/ompl_planning_pipeline.launch.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml index eb408ed545..9ce2a6e1f0 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml @@ -6,12 +6,12 @@ From 00f1ed9330a2a1dd93d2545c451ffba1bf23e4f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 6 May 2020 18:22:02 +0200 Subject: [PATCH 276/612] move NOLINT instructions to intended positions (#2058) --- .../kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp | 2 +- .../moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp index 782507bfed..ce8ad72bb4 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp @@ -79,11 +79,11 @@ class ChainIkSolverVelMimicSVD : public ChainIkSolverVel Eigen::Matrix::Constant(1.0)); } - // NOLINTNEXTLINE(readability-identifier-naming) /** Compute qdot_out = W_q * (W_x * J * W_q)^# * W_x * v_in * * where W_q and W_x are joint- and Cartesian weights respectively. * A smaller joint weight (< 1.0) will reduce the contribution of this joint to the solution. */ + // NOLINTNEXTLINE(readability-identifier-naming) int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out, const Eigen::VectorXd& joint_weights, const Eigen::Matrix& cartesian_weights); diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h index 977c3caf28..a94944a989 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h @@ -124,8 +124,8 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase protected: typedef Eigen::Matrix Twist; - // NOLINTNEXTLINE(readability-identifier-naming) /// Solve position IK given initial joint values + // NOLINTNEXTLINE(readability-identifier-naming) int CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out, const unsigned int max_iter, const Eigen::VectorXd& joint_weights, const Twist& cartesian_weights) const; From efa7fdca8a6999dbfcc28171b83deb67b258438a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Thu, 7 May 2020 13:05:59 +0200 Subject: [PATCH 277/612] Depend on geometric_shapes noetic-devel branch (#2059) --- moveit.rosinstall | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit.rosinstall b/moveit.rosinstall index fc82f3ab95..19c6e7546b 100644 --- a/moveit.rosinstall +++ b/moveit.rosinstall @@ -7,7 +7,7 @@ - git: local-name: geometric_shapes uri: https://github.com/ros-planning/geometric_shapes.git - version: melodic-devel + version: noetic-devel - git: local-name: moveit uri: https://github.com/ros-planning/moveit.git From aba69594485588fa0d0855f241b601807b1b83ae Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Fri, 8 May 2020 11:40:39 -0500 Subject: [PATCH 278/612] Jog arm dimensions (#1724) Added a service to change which directions jogging is allowed in --- moveit_experimental/moveit_jog_arm/README.md | 2 +- .../include/moveit_jog_arm/jog_arm_data.h | 4 ++++ .../moveit_jog_arm/jog_interface_base.h | 6 +++++ .../moveit_jog_arm/src/jog_calcs.cpp | 24 +++++++++++++++++-- .../moveit_jog_arm/src/jog_interface_base.cpp | 14 +++++++++++ .../moveit_jog_arm/src/jog_ros_interface.cpp | 4 ++++ 6 files changed, 51 insertions(+), 3 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/README.md b/moveit_experimental/moveit_jog_arm/README.md index 0543aed922..93c7c036d9 100644 --- a/moveit_experimental/moveit_jog_arm/README.md +++ b/moveit_experimental/moveit_jog_arm/README.md @@ -55,4 +55,4 @@ If you see a warning about "close to singularity", try changing the direction of Run tests from the jog\_arm folder: - catkin run_tests --no-deps --this + catkin run_tests --no-deps --this \ No newline at end of file diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 1cb737d8f6..220902b224 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -101,6 +101,10 @@ struct JogArmShared : public std::mutex // Stop jog loop threads - threads are not stopped by default std::atomic stop_requested{ false }; + + // The dimesions to control. In the command frame. [x, y, z, roll, pitch, yaw] + std::atomic_bool control_dimensions[6] = { ATOMIC_VAR_INIT(true), ATOMIC_VAR_INIT(true), ATOMIC_VAR_INIT(true), + ATOMIC_VAR_INIT(true), ATOMIC_VAR_INIT(true), ATOMIC_VAR_INIT(true) }; }; // ROS params to be read. See the yaml file in /config for a description of each. diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h index 741c1babbe..9dee171b76 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h @@ -43,6 +43,7 @@ #include "low_pass_filter.h" #include #include +#include #include #include #include @@ -73,6 +74,11 @@ class JogInterfaceBase moveit_msgs::ChangeDriftDimensions::Response& res); /** \brief Start the main calculation thread */ + // Service callback for changing jogging dimensions + bool changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, + moveit_msgs::ChangeControlDimensions::Response& res); + + // Jogging calculation thread bool startJogCalcThread(); /** \brief Stop the main calculation thread */ diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 3d95fa2f1d..bedaa6a1b8 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -272,14 +272,34 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& } } + // Set uncontrolled dimensions to 0 in command frame + if (!shared_variables.control_dimensions[0]) + cmd.twist.linear.x = 0; + if (!shared_variables.control_dimensions[1]) + cmd.twist.linear.y = 0; + if (!shared_variables.control_dimensions[2]) + cmd.twist.linear.z = 0; + if (!shared_variables.control_dimensions[3]) + cmd.twist.angular.x = 0; + if (!shared_variables.control_dimensions[4]) + cmd.twist.angular.y = 0; + if (!shared_variables.control_dimensions[5]) + cmd.twist.angular.z = 0; + // Transform the command to the MoveGroup planning frame if (cmd.header.frame_id != parameters_.planning_frame) { Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z); Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z); - translation_vector = tf_moveit_to_cmd_frame_.linear() * translation_vector; - angular_vector = tf_moveit_to_cmd_frame_.linear() * angular_vector; + // We solve (planning_frame -> base -> cmd.header.frame_id) + // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id) + const auto tf_planning_to_cmd_frame = + kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + kinematic_state_->getGlobalLinkTransform(cmd.header.frame_id); + + translation_vector = tf_planning_to_cmd_frame.linear() * translation_vector; + angular_vector = tf_planning_to_cmd_frame.linear() * angular_vector; // Put these components back into a TwistStamped cmd.header.frame_id = parameters_.planning_frame; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index 49f463002a..3cd342a100 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -261,6 +261,20 @@ bool JogInterfaceBase::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions: return true; } +bool JogInterfaceBase::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, + moveit_msgs::ChangeControlDimensions::Response& res) +{ + shared_variables_.control_dimensions[0] = req.control_x_translation; + shared_variables_.control_dimensions[1] = req.control_y_translation; + shared_variables_.control_dimensions[2] = req.control_z_translation; + shared_variables_.control_dimensions[3] = req.control_x_rotation; + shared_variables_.control_dimensions[4] = req.control_y_rotation; + shared_variables_.control_dimensions[5] = req.control_z_rotation; + + res.success = true; + return true; +} + // A separate thread for the heavy jogging calculations. bool JogInterfaceBase::startJogCalcThread() { diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp index d5ab9921ba..567e0b707a 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp @@ -94,6 +94,10 @@ JogROSInterface::JogROSInterface() ros::ServiceServer drift_dimensions_server = nh.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions", &JogInterfaceBase::changeDriftDimensions, dynamic_cast(this)); + // ROS Server for changing the control dimensions + ros::ServiceServer dims_server = + nh.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", + &JogInterfaceBase::changeControlDimensions, dynamic_cast(this)); // Publish freshly-calculated joints to the robot. // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). From dfb00da5917f357f56b54db5b0e3038a3180e10a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Fri, 8 May 2020 18:59:33 +0200 Subject: [PATCH 279/612] MP display: planning attempts are natural numbers (#2076) --- .../src/ui/motion_planning_rviz_plugin_frame.ui | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index a5f6f15f3a..2d644bcb72 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -603,7 +603,7 @@ - + 0 @@ -611,10 +611,10 @@ - 1000.000000000000000 + 1000 - 10.000000000000000 + 10 From e7962233fde5ae07f6f333efccf70e778178168d Mon Sep 17 00:00:00 2001 From: Yeshwanth Date: Thu, 7 May 2020 18:12:58 +0530 Subject: [PATCH 280/612] Wait and check for the grasp service Co-authored by v4hn --- .../move_group_interface/src/move_group_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 8c66ab78bb..0da1c24e4d 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -716,7 +716,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false) { - if (!plan_grasps_service_) + if (!plan_grasps_service_.exists()) { ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '" << GRASP_PLANNING_SERVICE_NAME From f34daa2689554c15e130f5dc8e00fbe8aa8f044b Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 11 May 2020 09:58:43 +0200 Subject: [PATCH 281/612] MGI: unify to LOGNAME logging --- .../src/move_group_interface.cpp | 122 +++++++++--------- 1 file changed, 63 insertions(+), 59 deletions(-) diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 0da1c24e4d..9774b58c5a 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -77,6 +77,8 @@ const std::string MoveGroupInterface::ROBOT_DESCRIPTION = const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps +const std::string LOGNAME = "move_group_interface"; + namespace { enum ActiveTargetType @@ -100,14 +102,14 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { std::string error = "Unable to construct robot model. Please make sure all needed information is on the " "parameter server."; - ROS_FATAL_STREAM_NAMED("move_group_interface", error); + ROS_FATAL_STREAM_NAMED(LOGNAME, error); throw std::runtime_error(error); } if (!getRobotModel()->hasJointModelGroup(opt.group_name_)) { std::string error = "Group '" + opt.group_name_ + "' was not found."; - ROS_FATAL_STREAM_NAMED("move_group_interface", error); + ROS_FATAL_STREAM_NAMED(LOGNAME, error); throw std::runtime_error(error); } @@ -174,14 +176,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl plan_grasps_service_ = node_handle_.serviceClient(GRASP_PLANNING_SERVICE_NAME); - ROS_INFO_STREAM_NAMED("move_group_interface", "Ready to take commands for planning group " << opt.group_name_ - << "."); + ROS_INFO_STREAM_NAMED(LOGNAME, "Ready to take commands for planning group " << opt.group_name_ << "."); } template void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) const { - ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "Waiting for move_group action server (%s)...", name.c_str()); // wait for the server (and spin as needed) if (timeout == ros::WallTime()) // wait forever @@ -197,8 +198,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else // in case of nodelets and specific callback queue implementations { - ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " - "handling."); + ROS_WARN_ONCE_NAMED(LOGNAME, "Non-default CallbackQueue: Waiting for external queue " + "handling."); } } } @@ -215,8 +216,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else // in case of nodelets and specific callback queue implementations { - ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " - "handling."); + ROS_WARN_ONCE_NAMED(LOGNAME, "Non-default CallbackQueue: Waiting for external queue " + "handling."); } } } @@ -230,7 +231,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_DEBUG_NAMED("move_group_interface", "Connected to '%s'", name.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "Connected to '%s'", name.c_str()); } } @@ -351,7 +352,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (target_value > 1.0) { - ROS_WARN_NAMED("move_group_interface", "Limiting max_%s (%.2f) to 1.0.", factor_name, target_value); + ROS_WARN_NAMED(LOGNAME, "Limiting max_%s (%.2f) to 1.0.", factor_name, target_value); variable = 1.0; } else if (target_value <= 0.0) @@ -360,7 +361,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl fallback_value); if (target_value < 0.0) { - ROS_WARN_NAMED("move_group_interface", "max_%s < 0.0! Setting to default: %.2f.", factor_name, variable); + ROS_WARN_NAMED(LOGNAME, "max_%s < 0.0! Setting to default: %.2f.", factor_name, variable); } } else @@ -443,7 +444,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_ERROR_NAMED("move_group_interface", "Unable to transform from frame '%s' to frame '%s'", frame.c_str(), + ROS_ERROR_NAMED(LOGNAME, "Unable to transform from frame '%s' to frame '%s'", frame.c_str(), getRobotModel()->getModelFrame().c_str()); return false; } @@ -492,7 +493,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; if (eef.empty()) { - ROS_ERROR_NAMED("move_group_interface", "No end-effector to set the pose for"); + ROS_ERROR_NAMED(LOGNAME, "No end-effector to set the pose for"); return false; } else @@ -524,7 +525,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // or return an error static const geometry_msgs::PoseStamped UNKNOWN; - ROS_ERROR_NAMED("move_group_interface", "Pose for end-effector '%s' not known.", eef.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Pose for end-effector '%s' not known.", eef.c_str()); return UNKNOWN; } @@ -539,7 +540,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // or return an error static const std::vector EMPTY; - ROS_ERROR_NAMED("move_group_interface", "Poses for end-effector '%s' are not known.", eef.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Poses for end-effector '%s' are not known.", eef.c_str()); return EMPTY; } @@ -572,7 +573,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!current_state_monitor_) { - ROS_ERROR_NAMED("move_group_interface", "Unable to monitor current robot state"); + ROS_ERROR_NAMED(LOGNAME, "Unable to monitor current robot state"); return false; } @@ -588,7 +589,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!current_state_monitor_) { - ROS_ERROR_NAMED("move_group_interface", "Unable to get current robot state"); + ROS_ERROR_NAMED(LOGNAME, "Unable to get current robot state"); return false; } @@ -598,7 +599,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds)) { - ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state"); + ROS_ERROR_NAMED(LOGNAME, "Failed to fetch current robot state"); return false; } @@ -628,8 +629,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl location.place_pose = pose; locations.push_back(location); } - ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations", - (unsigned int)locations.size()); + ROS_DEBUG_NAMED(LOGNAME, "Move group interface has %u place locations", (unsigned int)locations.size()); return locations; } @@ -637,20 +637,20 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!place_action_client_) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found"); + ROS_ERROR_STREAM_NAMED(LOGNAME, "place action client not found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } if (!place_action_client_->isServerConnected()) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected"); + ROS_WARN_STREAM_NAMED(LOGNAME, "place action server not connected"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } place_action_client_->sendGoal(goal); - ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size()); + ROS_DEBUG_NAMED(LOGNAME, "Sent place goal with %d locations", (int)goal.place_locations.size()); if (!place_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early"); + ROS_INFO_STREAM_NAMED(LOGNAME, "Place action returned early"); } if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { @@ -658,8 +658,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": " - << place_action_client_->getState().getText()); + ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << place_action_client_->getState().toString() << ": " + << place_action_client_->getState().getText()); return MoveItErrorCode(place_action_client_->getResult()->error_code); } } @@ -668,19 +668,19 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!pick_action_client_) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found"); + ROS_ERROR_STREAM_NAMED(LOGNAME, "pick action client not found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } if (!pick_action_client_->isServerConnected()) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected"); + ROS_WARN_STREAM_NAMED(LOGNAME, "pick action server not connected"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } pick_action_client_->sendGoal(goal); if (!pick_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early"); + ROS_INFO_STREAM_NAMED(LOGNAME, "Pickup action returned early"); } if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { @@ -688,8 +688,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": " - << pick_action_client_->getState().getText()); + ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << pick_action_client_->getState().toString() << ": " + << pick_action_client_->getState().getText()); return MoveItErrorCode(pick_action_client_->getResult()->error_code); } } @@ -706,8 +706,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (objects.empty()) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '" - << object << "', but the object could not be found"); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Asked for grasps for the object '" << object + << "', but the object could not be found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME); } @@ -718,10 +718,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!plan_grasps_service_.exists()) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '" - << GRASP_PLANNING_SERVICE_NAME - << "' is not available." - " This has to be implemented and started separately."); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Grasp planning service '" + << GRASP_PLANNING_SERVICE_NAME + << "' is not available." + " This has to be implemented and started separately."); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } @@ -732,11 +732,11 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.target = object; request.support_surfaces.push_back(support_surface_); - ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner..."); + ROS_DEBUG_NAMED(LOGNAME, "Calling grasp planner..."); if (!plan_grasps_service_.call(request, response) || response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { - ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick."); + ROS_ERROR_NAMED(LOGNAME, "Grasp planning failed. Unable to pick."); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } @@ -747,10 +747,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!move_action_client_) { + ROS_ERROR_STREAM_NAMED(LOGNAME, "move action client not found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } if (!move_action_client_->isServerConnected()) { + ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } @@ -765,7 +767,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl move_action_client_->sendGoal(goal); if (!move_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); + ROS_INFO_STREAM_NAMED(LOGNAME, "MoveGroup action returned early"); } if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { @@ -776,8 +778,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": " - << move_action_client_->getState().getText()); + ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << move_action_client_->getState().toString() << ": " + << move_action_client_->getState().getText()); return MoveItErrorCode(move_action_client_->getResult()->error_code); } } @@ -786,10 +788,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!move_action_client_) { + ROS_ERROR_STREAM_NAMED(LOGNAME, "move action client not found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } if (!move_action_client_->isServerConnected()) { + ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } @@ -810,7 +814,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!move_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); + ROS_INFO_STREAM_NAMED(LOGNAME, "MoveGroup action returned early"); } if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) @@ -819,8 +823,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString() - << ": " << move_action_client_->getState().getText()); + ROS_INFO_STREAM_NAMED(LOGNAME, move_action_client_->getState().toString() + << ": " << move_action_client_->getState().getText()); return MoveItErrorCode(move_action_client_->getResult()->error_code); } } @@ -843,7 +847,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!execute_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "ExecuteTrajectory action returned early"); + ROS_INFO_STREAM_NAMED(LOGNAME, "ExecuteTrajectory action returned early"); } if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) @@ -852,8 +856,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString() - << ": " << execute_action_client_->getState().getText()); + ROS_INFO_STREAM_NAMED(LOGNAME, execute_action_client_->getState().toString() + << ": " << execute_action_client_->getState().getText()); return MoveItErrorCode(execute_action_client_->getResult()->error_code); } } @@ -919,7 +923,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } if (l.empty()) { - ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str()); + ROS_ERROR_NAMED(LOGNAME, "No known link to attach object '%s' to", object.c_str()); return false; } moveit_msgs::AttachedCollisionObject aco; @@ -1002,13 +1006,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl void allowLooking(bool flag) { can_look_ = flag; - ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no"); + ROS_INFO_NAMED(LOGNAME, "Looking around: %s", can_look_ ? "yes" : "no"); } void allowReplanning(bool flag) { can_replan_ = flag; - ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no"); + ROS_INFO_NAMED(LOGNAME, "Replanning: %s", can_replan_ ? "yes" : "no"); } void setReplanningDelay(double delay) @@ -1071,7 +1075,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } } else - ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation"); + ROS_ERROR_NAMED(LOGNAME, "Unable to construct MotionPlanRequest representation"); if (path_constraints_) request.path_constraints = *path_constraints_; @@ -1242,7 +1246,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } catch (std::exception& ex) { - ROS_ERROR_NAMED("move_group_interface", "%s", ex.what()); + ROS_ERROR_NAMED(LOGNAME, "%s", ex.what()); } initializing_constraints_ = false; } @@ -1601,7 +1605,7 @@ bool MoveGroupInterface::setNamedTarget(const std::string& name) impl_->setTargetType(JOINT); return true; } - ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str()); + ROS_ERROR_NAMED(LOGNAME, "The requested named target '%s' does not exist", name.c_str()); return false; } } @@ -1830,7 +1834,7 @@ bool MoveGroupInterface::setPoseTargets(const std::vector MoveGroupInterface::getCurrentRPY(const std::string& end_eff std::vector result; const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; if (eef.empty()) - ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); + ROS_ERROR_NAMED(LOGNAME, "No end-effector specified"); else { moveit::core::RobotStatePtr current_state; From 3d6274ba3060e0808eaf95d9d0a9594cb13ab609 Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 11 May 2020 09:59:37 +0200 Subject: [PATCH 282/612] add aux check for shared-ptr Unify the checks, they exist for all other actions. --- .../move_group_interface/src/move_group_interface.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 9774b58c5a..8c73d4c5fc 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -831,8 +831,14 @@ class MoveGroupInterface::MoveGroupInterfaceImpl MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait) { + if (!execute_action_client_) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "execute action client not found"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } if (!execute_action_client_->isServerConnected()) { + ROS_WARN_STREAM_NAMED(LOGNAME, "execute action server not connected"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } From 2b38e26f737d79459e75edec9c7d3b28770765c0 Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 11 May 2020 10:02:02 +0200 Subject: [PATCH 283/612] MGI: use new COMMUNICATION_FAILURE error code --- .../src/move_group_interface.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 8c73d4c5fc..55a1a47bf5 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -643,7 +643,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!place_action_client_->isServerConnected()) { ROS_WARN_STREAM_NAMED(LOGNAME, "place action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); } place_action_client_->sendGoal(goal); @@ -674,7 +674,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!pick_action_client_->isServerConnected()) { ROS_WARN_STREAM_NAMED(LOGNAME, "pick action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); } pick_action_client_->sendGoal(goal); @@ -722,7 +722,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl << GRASP_PLANNING_SERVICE_NAME << "' is not available." " This has to be implemented and started separately."); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); } moveit_msgs::GraspPlanning::Request request; @@ -753,7 +753,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!move_action_client_->isServerConnected()) { ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); } moveit_msgs::MoveGroupGoal goal; @@ -794,7 +794,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!move_action_client_->isServerConnected()) { ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); } moveit_msgs::MoveGroupGoal goal; @@ -839,7 +839,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!execute_action_client_->isServerConnected()) { ROS_WARN_STREAM_NAMED(LOGNAME, "execute action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); } moveit_msgs::ExecuteTrajectoryGoal goal; From 8a61ed688983f5c1dfa321b322b85255a5d8fc4a Mon Sep 17 00:00:00 2001 From: TrippleBender Date: Mon, 11 May 2020 10:44:26 +0200 Subject: [PATCH 284/612] Added support for PS4 joystick (#2060) --- .../moveitjoy_module.py | 161 ++++++++++++++++++ 1 file changed, 161 insertions(+) diff --git a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py index 5bf253c3ab..1b2408fb34 100644 --- a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py +++ b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py @@ -322,6 +322,163 @@ def __init__(self, msg): self.right_analog_y = msg.axes[3] self.orig_msg = msg +class PS4Status(JoyStatus): + def __init__(self, msg): + JoyStatus.__init__(self) + #creating from sensor_msg/Joy + if msg.buttons[12] == 1: + self.center = True + else: + self.center = False + if msg.buttons[8] == 1: + self.select = True + else: + self.select = False + if msg.buttons[9] == 1: + self.start = True + else: + self.start = False + if msg.buttons[10] == 1: + self.L3 = True + else: + self.L3 = False + if msg.buttons[11] == 1: + self.R3 = True + else: + self.R3 = False + if msg.buttons[0] == 1: + self.square = True + else: + self.square = False + if msg.axes[10] < 0: + self.up = True + else: + self.up = False + if msg.axes[10] > 0: + self.down = True + else: + self.down = False + if msg.axes[9] < 0: + self.left = True + else: + self.left = False + if msg.axes[9] > 0: + self.right = True + else: + self.right = False + if msg.buttons[3] == 1: + self.triangle = True + else: + self.triangle = False + if msg.buttons[1] == 1: + self.cross = True + else: + self.cross = False + if msg.buttons[2] == 1: + self.circle = True + else: + self.circle = False + if msg.buttons[4] == 1: + self.L1 = True + else: + self.L1 = False + if msg.buttons[5] == 1: + self.R1 = True + else: + self.R1 = False + if msg.buttons[6] == 1: + self.L2 = True + else: + self.L2 = False + if msg.buttons[7] == 1: + self.R2 = True + else: + self.R2 = False + self.left_analog_x = msg.axes[0] + self.left_analog_y = msg.axes[1] + self.right_analog_x = msg.axes[5] + self.right_analog_y = msg.axes[2] + self.orig_msg = msg + +class PS4WiredStatus(JoyStatus): + def __init__(self, msg): + JoyStatus.__init__(self) + #creating from sensor_msg/Joy + if msg.buttons[10] == 1: + self.center = True + else: + self.center = False + if msg.buttons[8] == 1: + self.select = True + else: + self.select = False + if msg.buttons[9] == 1: + self.start = True + else: + self.start = False + if msg.buttons[11] == 1: + self.L3 = True + else: + self.L3 = False + if msg.buttons[12] == 1: + self.R3 = True + else: + self.R3 = False + if msg.buttons[3] == 1: + self.square = True + else: + self.square = False + if msg.axes[7] < 0: + self.up = True + else: + self.up = False + if msg.axes[7] > 0: + self.down = True + else: + self.down = False + if msg.axes[6] < 0: + self.left = True + else: + self.left = False + if msg.axes[6] > 0: + self.right = True + else: + self.right = False + if msg.buttons[2] == 1: + self.triangle = True + else: + self.triangle = False + if msg.buttons[0] == 1: + self.cross = True + else: + self.cross = False + if msg.buttons[1] == 1: + self.circle = True + else: + self.circle = False + if msg.buttons[4] == 1: + self.L1 = True + else: + self.L1 = False + if msg.buttons[5] == 1: + self.R1 = True + else: + self.R1 = False + if msg.buttons[6] == 1: + self.L2 = True + else: + self.L2 = False + if msg.buttons[7] == 1: + self.R2 = True + else: + self.R2 = False + self.left_analog_x = msg.axes[0] + self.left_analog_y = msg.axes[1] + self.right_analog_x = msg.axes[3] + self.right_analog_y = msg.axes[4] + self.orig_msg = msg + + class StatusHistory(): def __init__(self, max_length=10): self.max_length = max_length @@ -471,6 +628,10 @@ def joyCB(self, msg): status = XBoxStatus(msg) elif len(msg.axes) == 20 and len(msg.buttons) == 17: status = PS3Status(msg) + elif len(msg.axes) == 14 and len(msg.buttons) == 14: + status = PS4Status(msg) + elif len(msg.axes) == 8 and len(msg.buttons) == 13: + status = PS4WiredStatus(msg) else: raise Exception("Unknown joystick") self.run(status) From 50e3c21081c03a106ac1157d8a8687710bcc0398 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Tue, 12 May 2020 15:51:28 +0200 Subject: [PATCH 285/612] fix signal for planning_attempts (#2082) fixup for dfb00da5917f357f56b54db5b0e3038a3180e10a --- .../motion_planning_rviz_plugin/src/motion_planning_frame.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 0857a85bf1..3993bfd75f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -156,7 +156,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: connect(ui_->goal_tolerance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(ui_->planning_time, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); - connect(ui_->planning_attempts, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->planning_attempts, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); connect(ui_->velocity_scaling_factor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(ui_->acceleration_scaling_factor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); From 442c0fe6ebfef6a9701e05def44de53feb7ea216 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Fri, 15 May 2020 00:40:35 +0900 Subject: [PATCH 286/612] Check for empty quaternion message (#2089) --- moveit_core/planning_scene/src/planning_scene.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 28f70f32b4..81d2846a47 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1687,7 +1687,15 @@ void PlanningScene::poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Isomet { Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z); Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z); - quaternion.normalize(); + if ((quaternion.x() == 0) && (quaternion.y() == 0) && (quaternion.z() == 0) && (quaternion.w() == 0)) + { + ROS_WARN_NAMED(LOGNAME, "Empty quaternion found in pose message. Setting to neutral orientation."); + quaternion.setIdentity(); + } + else + { + quaternion.normalize(); + } out = translation * quaternion; } From 96e2e798d5d6de3770c002262cb9d0391ab967a1 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Tue, 19 May 2020 04:41:02 +0900 Subject: [PATCH 287/612] Display subframes in Rviz plugin (#2024) Display subframes in Rviz plugin --- .../src/motion_planning_frame_objects.cpp | 26 +++++++- .../ui/motion_planning_rviz_plugin_frame.ui | 64 ++++++++++--------- 2 files changed, 60 insertions(+), 30 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index e22be44612..ffdab45b7c 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -56,6 +56,21 @@ #include "ui_motion_planning_rviz_plugin_frame.h" +namespace +{ +QString subframe_poses_to_qstring(const moveit::core::FixedTransformsMap& subframes) +{ + QString status_text = "\nIt has the subframes '"; + for (auto subframe : subframes) + { + status_text += QString::fromStdString(subframe.first) + "', '"; + } + status_text.chop(3); + status_text += "."; + return status_text; +} +} // namespace + namespace moveit_rviz_plugin { void MotionPlanningFrame::importFileButtonClicked() @@ -175,6 +190,11 @@ static QString decideStatusText(const collision_detection::CollisionEnv::ObjectC for (const QString& shape_name : shape_names) status_text += " " + shape_name; } + status_text += "."; + } + if (!obj->subframe_poses_.empty()) + { + status_text += subframe_poses_to_qstring(obj->subframe_poses_); } return status_text; } @@ -182,7 +202,11 @@ static QString decideStatusText(const collision_detection::CollisionEnv::ObjectC static QString decideStatusText(const moveit::core::AttachedBody* attached_body) { QString status_text = "'" + QString::fromStdString(attached_body->getName()) + "' is attached to '" + - QString::fromStdString(attached_body->getAttachedLinkName()) + "'"; + QString::fromStdString(attached_body->getAttachedLinkName()) + "'."; + if (!attached_body->getSubframeTransforms().empty()) + { + status_text += subframe_poses_to_qstring(attached_body->getSubframeTransforms()); + } return status_text; } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 2d644bcb72..363fe0f478 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -938,37 +938,9 @@ - - - Object Status - - - - - - false - - - QFrame::StyledPanel - - - QFrame::Raised - - - Status - - - Qt::AutoText - - - - - - - - Manage Pose and Scale + Change Object Pose and Scale @@ -1130,6 +1102,40 @@ + + + + Object Status + + + + + + true + + + QFrame::StyledPanel + + + QFrame::Raised + + + Status + + + Qt::AutoText + + + true + + + Qt::TextBrowserInteraction + + + + + + From 091bdfb765e46ba399e6983f49a81c502b0bcb65 Mon Sep 17 00:00:00 2001 From: Jesse Li Date: Mon, 18 May 2020 12:48:05 -0700 Subject: [PATCH 288/612] Minor logic cleanup (#2091) Due to the check priority > 0, there is no time when best == state_space_factories_.end() while priority <= prev_priority, so best == state_space_factories_.end() is unnecessary. Then if you change the starting value of prev_priority then you don't need to separately check if priority > 0. --- .../src/planning_context_manager.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 6e868a5b5e..8fb562fabf 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -220,7 +220,7 @@ MultiQueryPlannerAllocator::allocatePersistentPlannersecond->canRepresentProblem(group, req, robot_model_); - if (priority > 0) - if (best == state_space_factories_.end() || priority > prev_priority) - { - best = it; - prev_priority = priority; - } + if (priority > prev_priority) + { + best = it; + prev_priority = priority; + } } if (best == state_space_factories_.end()) From 6427dfc1fbb8cdb71c477dff1f81936b7c72779f Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sun, 19 Apr 2020 01:06:21 -0600 Subject: [PATCH 289/612] move_group pick_place test --- .../planning_interface/test/CMakeLists.txt | 3 + .../test/move_group_pick_place_test.cpp | 290 ++++++++++++++++++ .../test/move_group_pick_place_test.test | 29 ++ 3 files changed, 322 insertions(+) create mode 100644 moveit_ros/planning_interface/test/move_group_pick_place_test.cpp create mode 100644 moveit_ros/planning_interface/test/move_group_pick_place_test.test diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 7eb6543b1a..8cb31d6004 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -9,6 +9,9 @@ if (CATKIN_ENABLE_TESTING) add_rostest_gtest(move_group_interface_cpp_test move_group_interface_cpp_test.test move_group_interface_cpp_test.cpp) target_link_libraries(move_group_interface_cpp_test moveit_move_group_interface ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) + add_rostest_gtest(move_group_pick_place_test move_group_pick_place_test.test move_group_pick_place_test.cpp) + target_link_libraries(move_group_pick_place_test moveit_move_group_interface ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) + add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) diff --git a/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp new file mode 100644 index 0000000000..a7dbffd210 --- /dev/null +++ b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp @@ -0,0 +1,290 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2020, Tyler Weaver +* 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 PickNik Robotics 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. +*********************************************************************/ + +/* Author: Tyler Weaver */ + +/* These integration tests are based on the tutorials for using move_group to do a pick and place: + * https://ros-planning.github.io/moveit_tutorials/doc/pick_place/pick_place_tutorial.html + */ + +// C++ +#include + +// ROS +#include + +// The Testing Framework and Utils +#include + +// MoveIt +#include +#include + +// TF2 +#include + +static const std::string PLANNING_GROUP = "panda_arm"; +constexpr double PLANNING_TIME_S = 45.0; +constexpr double MAX_VELOCITY_SCALE = 1.0; +constexpr double MAX_ACCELERATION_SCALE = 1.0; + +class PickPlaceTestFixture : public ::testing::Test +{ +public: + void SetUp() override + { + nh_ = ros::NodeHandle("/move_group_pick_place_test"); + move_group_ = std::make_shared(PLANNING_GROUP); + + // set velocity and acceleration scaling factors (full speed) + move_group_->setMaxVelocityScalingFactor(MAX_VELOCITY_SCALE); + move_group_->setMaxAccelerationScalingFactor(MAX_ACCELERATION_SCALE); + + // allow more time for planning + move_group_->setPlanningTime(PLANNING_TIME_S); + } + +protected: + ros::NodeHandle nh_; + moveit::planning_interface::MoveGroupInterfacePtr move_group_; + moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; +}; + +TEST_F(PickPlaceTestFixture, PickPlaceTest) +{ + // Create vector to hold 3 collision objects. + std::vector collision_objects; + collision_objects.resize(3); + + // Add the first table where the cube will originally be kept. + collision_objects[0].id = "table1"; + collision_objects[0].header.frame_id = "panda_link0"; + + /* Create identity rotation quaternion */ + tf2::Quaternion zero_orientation; + zero_orientation.setRPY(0, 0, 0); + geometry_msgs::Quaternion zero_orientation_msg = tf2::toMsg(zero_orientation); + + /* Define the primitive and its dimensions. */ + collision_objects[0].primitives.resize(1); + collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX; + collision_objects[0].primitives[0].dimensions.resize(3); + collision_objects[0].primitives[0].dimensions[0] = 0.2; + collision_objects[0].primitives[0].dimensions[1] = 0.4; + collision_objects[0].primitives[0].dimensions[2] = 0.4; + + /* Define the pose of the table. */ + collision_objects[0].primitive_poses.resize(1); + collision_objects[0].primitive_poses[0].position.x = 0.5; + collision_objects[0].primitive_poses[0].position.y = 0; + collision_objects[0].primitive_poses[0].position.z = 0.2; + collision_objects[0].primitive_poses[0].orientation = zero_orientation_msg; + + collision_objects[0].operation = collision_objects[0].ADD; + + // Add the second table where we will be placing the cube. + collision_objects[1].id = "table2"; + collision_objects[1].header.frame_id = "panda_link0"; + + /* Define the primitive and its dimensions. */ + collision_objects[1].primitives.resize(1); + collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX; + collision_objects[1].primitives[0].dimensions.resize(3); + collision_objects[1].primitives[0].dimensions[0] = 0.4; + collision_objects[1].primitives[0].dimensions[1] = 0.2; + collision_objects[1].primitives[0].dimensions[2] = 0.4; + + /* Define the pose of the table. */ + collision_objects[1].primitive_poses.resize(1); + collision_objects[1].primitive_poses[0].position.x = 0; + collision_objects[1].primitive_poses[0].position.y = 0.5; + collision_objects[1].primitive_poses[0].position.z = 0.2; + collision_objects[1].primitive_poses[0].orientation = zero_orientation_msg; + + collision_objects[1].operation = collision_objects[1].ADD; + + // Define the object that we will be manipulating + collision_objects[2].header.frame_id = "panda_link0"; + collision_objects[2].id = "object"; + + /* Define the primitive and its dimensions. */ + collision_objects[2].primitives.resize(1); + collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX; + collision_objects[2].primitives[0].dimensions.resize(3); + collision_objects[2].primitives[0].dimensions[0] = 0.02; + collision_objects[2].primitives[0].dimensions[1] = 0.02; + collision_objects[2].primitives[0].dimensions[2] = 0.2; + + /* Define the pose of the object. */ + collision_objects[2].primitive_poses.resize(1); + collision_objects[2].primitive_poses[0].position.x = 0.5; + collision_objects[2].primitive_poses[0].position.y = 0; + collision_objects[2].primitive_poses[0].position.z = 0.5; + collision_objects[2].primitive_poses[0].orientation = zero_orientation_msg; + + collision_objects[2].operation = collision_objects[2].ADD; + + planning_scene_interface_.applyCollisionObjects(collision_objects); + + // Create a vector of grasps to be attempted, currently only creating single grasp. + // This is essentially useful when using a grasp generator to generate and test multiple grasps. + std::vector grasps; + grasps.resize(1); + + // Setting grasp pose + // ++++++++++++++++++++++ + // This is the pose of panda_link8. |br| + // Make sure that when you set the grasp_pose, you are setting it to be the pose of the last link in + // your manipulator which in this case would be `"panda_link8"` You will have to compensate for the + // transform from `"panda_link8"` to the palm of the end effector. + grasps[0].grasp_pose.header.frame_id = "panda_link0"; + tf2::Quaternion grasp_orientation; + grasp_orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2); + grasps[0].grasp_pose.pose.orientation = tf2::toMsg(grasp_orientation); + grasps[0].grasp_pose.pose.position.x = 0.415; + grasps[0].grasp_pose.pose.position.y = 0; + grasps[0].grasp_pose.pose.position.z = 0.5; + + // Setting pre-grasp approach + // ++++++++++++++++++++++++++ + /* Defined with respect to frame_id */ + grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0"; + /* Direction is set as positive x axis */ + grasps[0].pre_grasp_approach.direction.vector.x = 1.0; + grasps[0].pre_grasp_approach.min_distance = 0.095; + grasps[0].pre_grasp_approach.desired_distance = 0.115; + + // Setting post-grasp retreat + // ++++++++++++++++++++++++++ + /* Defined with respect to frame_id */ + grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0"; + /* Direction is set as positive z axis */ + grasps[0].post_grasp_retreat.direction.vector.z = 1.0; + grasps[0].post_grasp_retreat.min_distance = 0.1; + grasps[0].post_grasp_retreat.desired_distance = 0.25; + + // Setting posture of eef before grasp + // +++++++++++++++++++++++++++++++++++ + /* Add both finger joints of panda robot. */ + grasps[0].pre_grasp_posture.joint_names.resize(2); + grasps[0].pre_grasp_posture.joint_names[0] = "panda_finger_joint1"; + grasps[0].pre_grasp_posture.joint_names[1] = "panda_finger_joint2"; + + /* Set them as open, wide enough for the object to fit. */ + grasps[0].pre_grasp_posture.points.resize(1); + grasps[0].pre_grasp_posture.points[0].positions.resize(2); + grasps[0].pre_grasp_posture.points[0].positions[0] = 0.04; + grasps[0].pre_grasp_posture.points[0].positions[1] = 0.04; + grasps[0].pre_grasp_posture.points[0].time_from_start = ros::Duration(0.5); + + // Setting posture of eef during grasp + // +++++++++++++++++++++++++++++++++++ + /* Add both finger joints of panda robot and set them as closed. */ + grasps[0].grasp_posture = grasps[0].pre_grasp_posture; + grasps[0].grasp_posture.points[0].positions[0] = 0.00; + grasps[0].grasp_posture.points[0].positions[1] = 0.00; + + // Set support surface as table1. + move_group_->setSupportSurfaceName("table1"); + // Call pick to pick up the object using the grasps given + ASSERT_EQ(move_group_->pick("object", grasps), moveit::planning_interface::MoveItErrorCode::SUCCESS); + + // Ideally, you would create a vector of place locations to be attempted although in this example, we only create + // a single place location. + std::vector place_location; + place_location.resize(1); + + // Setting place location pose + // +++++++++++++++++++++++++++ + place_location[0].place_pose.header.frame_id = "panda_link0"; + tf2::Quaternion place_orientation; + place_orientation.setRPY(0, 0, M_PI / 2); + place_location[0].place_pose.pose.orientation = tf2::toMsg(place_orientation); + + /* For place location, we set the value to the exact location of the center of the object. */ + place_location[0].place_pose.pose.position.x = 0; + place_location[0].place_pose.pose.position.y = 0.5; + place_location[0].place_pose.pose.position.z = 0.5; + + // Setting pre-place approach + // ++++++++++++++++++++++++++ + /* Defined with respect to frame_id */ + place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0"; + /* Direction is set as negative z axis */ + place_location[0].pre_place_approach.direction.vector.z = -1.0; + place_location[0].pre_place_approach.min_distance = 0.095; + place_location[0].pre_place_approach.desired_distance = 0.115; + + // Setting post-grasp retreat + // ++++++++++++++++++++++++++ + /* Defined with respect to frame_id */ + place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0"; + /* Direction is set as negative y axis */ + place_location[0].post_place_retreat.direction.vector.y = -1.0; + place_location[0].post_place_retreat.min_distance = 0.1; + place_location[0].post_place_retreat.desired_distance = 0.25; + + // Setting posture of eef after placing object + // +++++++++++++++++++++++++++++++++++++++++++ + /* Similar to the pick case */ + /* Add both finger joints of panda robot. */ + place_location[0].post_place_posture.joint_names.resize(2); + place_location[0].post_place_posture.joint_names[0] = "panda_finger_joint1"; + place_location[0].post_place_posture.joint_names[1] = "panda_finger_joint2"; + + /* Set them as open, wide enough for the object to fit. */ + place_location[0].post_place_posture.points.resize(1); + place_location[0].post_place_posture.points[0].positions.resize(2); + place_location[0].post_place_posture.points[0].positions[0] = 0.04; + place_location[0].post_place_posture.points[0].positions[1] = 0.04; + place_location[0].post_place_posture.points[0].time_from_start = ros::Duration(0.5); + + // Set support surface as table2. + move_group_->setSupportSurfaceName("table2"); + // Call place to place the object using the place locations given. + ASSERT_EQ(move_group_->place("object", place_location), moveit::planning_interface::MoveItErrorCode::SUCCESS); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "move_group_pick_place_test"); + testing::InitGoogleTest(&argc, argv); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/moveit_ros/planning_interface/test/move_group_pick_place_test.test b/moveit_ros/planning_interface/test/move_group_pick_place_test.test new file mode 100644 index 0000000000..17dd3c179b --- /dev/null +++ b/moveit_ros/planning_interface/test/move_group_pick_place_test.test @@ -0,0 +1,29 @@ + + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + From d3bd523ee0ec296656cf964c1b7af190e3bfe9f9 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 6 May 2020 03:53:27 -0600 Subject: [PATCH 290/612] change FloatingJointModel::getStateSpaceDimension return value to 7 --- moveit_core/robot_model/src/floating_joint_model.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 47ed6a87d2..bb0bb1b273 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -45,6 +45,12 @@ namespace moveit { namespace core { +namespace +{ +constexpr size_t STATE_SPACE_DIMENSION = 7; + +} // namespace + FloatingJointModel::FloatingJointModel(const std::string& name) : JointModel(name), angular_distance_weight_(1.0) { type_ = FLOATING; @@ -55,7 +61,7 @@ FloatingJointModel::FloatingJointModel(const std::string& name) : JointModel(nam local_variable_names_.push_back("rot_y"); local_variable_names_.push_back("rot_z"); local_variable_names_.push_back("rot_w"); - for (int i = 0; i < 7; ++i) + for (size_t i = 0; i < STATE_SPACE_DIMENSION; ++i) { variable_names_.push_back(name_ + "/" + local_variable_names_[i]); variable_index_map_[variable_names_.back()] = i; @@ -192,7 +198,7 @@ bool FloatingJointModel::normalizeRotation(double* values) const unsigned int FloatingJointModel::getStateSpaceDimension() const { - return 6; + return STATE_SPACE_DIMENSION; } bool FloatingJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const From 11cbc826d5ae001b0403193320923b1f7a3c3650 Mon Sep 17 00:00:00 2001 From: rfeistenauer <45563842+rfeistenauer@users.noreply.github.com> Date: Wed, 20 May 2020 16:07:01 +0200 Subject: [PATCH 291/612] [jog_arm] Fix crash on empty jog msgs (#2094) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * adds empty point msg to prevent invalid indexing * do not add effort it's not used internally and should be consistent. Co-authored-by: Michael Görner --- moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index bedaa6a1b8..f08a9473bb 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -78,7 +78,6 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) num_joints_ = internal_joint_state_.name.size(); internal_joint_state_.position.resize(num_joints_); internal_joint_state_.velocity.resize(num_joints_); - internal_joint_state_.effort.resize(num_joints_); // A map for the indices of incoming joint commands for (std::size_t i = 0; i < internal_joint_state_.name.size(); ++i) { @@ -694,6 +693,13 @@ void JogCalcs::suddenHalt(Eigen::ArrayXd& delta_theta) // Is handled differently for position vs. velocity control. void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_traj) { + if (joint_traj.points.empty()) + { + joint_traj.points.push_back(trajectory_msgs::JointTrajectoryPoint()); + joint_traj.points[0].positions.resize(num_joints_); + joint_traj.points[0].velocities.resize(num_joints_); + } + for (std::size_t i = 0; i < num_joints_; ++i) { // For position-controlled robots, can reset the joints to a known, good state From 26ec92850d4bae71bab1daf193981f66b0ad8beb Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Mon, 4 May 2020 16:30:17 +0200 Subject: [PATCH 292/612] robot_state is diff if only position changed --- .../planning_scene_monitor/src/planning_scene_monitor.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index cc0ebeb38f..f839528dc1 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -370,6 +370,11 @@ void PlanningSceneMonitor::scenePublishingThread() if (octomap_monitor_) lock = octomap_monitor_->getOcTreePtr()->reading(); scene_->getPlanningSceneDiffMsg(msg); + if (new_scene_update_ == UPDATE_STATE) + { + msg.robot_state.attached_collision_objects.clear(); + msg.robot_state.is_diff = true; + } } boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the // transform cache to From f43a3af5291a5f3d4f96d9233f8a0c354f23872d Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Mon, 4 May 2020 16:37:17 +0200 Subject: [PATCH 293/612] attach objects to the corresponding link instead of the robots root node, this will move them automatically --- .../src/robot_state_visualization.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp index 99528fb88b..7775e5bc6f 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp @@ -100,7 +100,7 @@ void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kin const std_msgs::ColorRGBA& default_attached_object_color, const std::map& color_map) { - updateHelper(kinematic_state, default_attached_object_color, &color_map); + updateHelper(kinematic_state, default_attached_object_color, &color_map); } void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPtr& kinematic_state, @@ -127,14 +127,20 @@ void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPt alpha = color.a = it->second.a; } } + rviz::RobotLink* link = robot_.getLink(attached_body->getAttachedLinkName()); + if (!link) + { + ROS_ERROR_STREAM("Link " << attached_body->getAttachedLinkName() << " not found in rviz::Robot"); + continue; + } rviz::Color rcolor(color.r, color.g, color.b); - const EigenSTL::vector_Isometry3d& ab_t = attached_body->getGlobalCollisionBodyTransforms(); + const EigenSTL::vector_Isometry3d& ab_t = attached_body->getFixedTransforms(); const std::vector& ab_shapes = attached_body->getShapes(); for (std::size_t j = 0; j < ab_shapes.size(); ++j) { - render_shapes_->renderShape(robot_.getVisualNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_, + render_shapes_->renderShape(link->getVisualNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_, octree_voxel_color_mode_, rcolor, alpha); - render_shapes_->renderShape(robot_.getCollisionNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_, + render_shapes_->renderShape(link->getCollisionNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_, octree_voxel_color_mode_, rcolor, alpha); } } From cf9d0659b9e636d8e4743e98e0ebe0f01aed20dd Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Mon, 4 May 2020 16:45:24 +0200 Subject: [PATCH 294/612] Don't rebuild the ogre scene if only the robot state changed --- .../planning_scene_display.h | 3 +++ .../src/planning_scene_display.cpp | 19 ++++++++++++++----- .../planning_scene_render.h | 2 ++ .../robot_state_visualization.h | 1 + .../src/planning_scene_render.cpp | 10 ++++++++++ .../src/robot_state_visualization.cpp | 12 +++++++++--- 6 files changed, 39 insertions(+), 8 deletions(-) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index 7b917fe33d..99f9ab8634 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -191,7 +191,10 @@ protected Q_SLOTS: RobotStateVisualizationPtr planning_scene_robot_; PlanningSceneRenderPtr planning_scene_render_; + // full update required bool planning_scene_needs_render_; + // or only the robot position (excluding attached object changes) + bool robot_state_needs_render_; float current_scene_time_; rviz::Property* scene_category_; diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 7701558a58..874940d83a 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -336,10 +336,17 @@ void PlanningSceneDisplay::renderPlanningScene() try { const planning_scene_monitor::LockedPlanningSceneRO& ps = getPlanningSceneRO(); - planning_scene_render_->renderPlanningScene( - ps, env_color, attached_color, static_cast(octree_render_property_->getOptionInt()), - static_cast(octree_coloring_property_->getOptionInt()), - scene_alpha_property_->getFloat()); + if (planning_scene_needs_render_) + { + planning_scene_render_->renderPlanningScene( + ps, env_color, attached_color, static_cast(octree_render_property_->getOptionInt()), + static_cast(octree_coloring_property_->getOptionInt()), + scene_alpha_property_->getFloat()); + } + else + { + planning_scene_render_->updateRobotPosition(ps); + } } catch (std::exception& ex) { @@ -633,12 +640,14 @@ void PlanningSceneDisplay::update(float wall_dt, float ros_dt) void PlanningSceneDisplay::updateInternal(float wall_dt, float /*ros_dt*/) { current_scene_time_ += wall_dt; - if (current_scene_time_ > scene_display_time_property_->getFloat() && planning_scene_render_ && + if ((current_scene_time_ > scene_display_time_property_->getFloat() && planning_scene_render_ && + robot_state_needs_render_) || planning_scene_needs_render_) { renderPlanningScene(); calculateOffsetPosition(); current_scene_time_ = 0.0f; + robot_state_needs_render_ = false; planning_scene_needs_render_ = false; } } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h index eb61b70521..ba49862429 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h @@ -75,6 +75,8 @@ class PlanningSceneRender return scene_robot_; } + void updateRobotPosition(const planning_scene::PlanningSceneConstPtr& scene); + void renderPlanningScene(const planning_scene::PlanningSceneConstPtr& scene, const rviz::Color& default_scene_color, const rviz::Color& default_attached_color, OctreeVoxelRenderMode voxel_render_mode, OctreeVoxelColorMode voxel_color_mode, float default_scene_alpha); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index 754ff06a01..0c04ade4e5 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -71,6 +71,7 @@ class RobotStateVisualization void update(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color, const std::map& color_map); + void updateKinematicState(const robot_state::RobotStateConstPtr& kinematic_state); void setDefaultAttachedObjectColor(const std_msgs::ColorRGBA& default_attached_object_color); /// update color of all attached object shapes void updateAttachedObjectColors(const std_msgs::ColorRGBA& attached_object_color); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp index 05210fe078..5537e0a340 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp @@ -56,6 +56,16 @@ PlanningSceneRender::~PlanningSceneRender() context_->getSceneManager()->destroySceneNode(planning_scene_geometry_node_); } +void PlanningSceneRender::updateRobotPosition(const planning_scene::PlanningSceneConstPtr& scene) +{ + if (scene_robot_) + { + robot_state::RobotStatePtr rs = std::make_shared(scene->getCurrentState()); + rs->update(); + scene_robot_->updateKinematicState(rs); + } +} + void PlanningSceneRender::clear() { render_shapes_->clear(); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp index 7775e5bc6f..2f4183daa2 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include namespace moveit_rviz_plugin @@ -100,7 +101,7 @@ void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kin const std_msgs::ColorRGBA& default_attached_object_color, const std::map& color_map) { - updateHelper(kinematic_state, default_attached_object_color, &color_map); + updateHelper(kinematic_state, default_attached_object_color, &color_map); } void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPtr& kinematic_state, @@ -130,8 +131,8 @@ void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPt rviz::RobotLink* link = robot_.getLink(attached_body->getAttachedLinkName()); if (!link) { - ROS_ERROR_STREAM("Link " << attached_body->getAttachedLinkName() << " not found in rviz::Robot"); - continue; + ROS_ERROR_STREAM("Link " << attached_body->getAttachedLinkName() << " not found in rviz::Robot"); + continue; } rviz::Color rcolor(color.r, color.g, color.b); const EigenSTL::vector_Isometry3d& ab_t = attached_body->getFixedTransforms(); @@ -149,6 +150,11 @@ void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPt robot_.setVisible(visible_); } +void RobotStateVisualization::updateKinematicState(const moveit::core::RobotStateConstPtr& kinematic_state) +{ + robot_.update(PlanningLinkUpdater(kinematic_state)); +} + void RobotStateVisualization::setVisible(bool visible) { visible_ = visible; From 1df820bff0ccee71c4904f4b532bc05284335702 Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Mon, 4 May 2020 16:48:21 +0200 Subject: [PATCH 295/612] accept joint state updates at ca 30hz --- .../planning_scene_monitor/src/planning_scene_monitor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index f839528dc1..d24e51bd72 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -240,7 +240,7 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc last_update_time_ = last_robot_motion_time_ = ros::Time::now(); last_robot_state_update_wall_time_ = ros::WallTime::now(); - dt_state_update_ = ros::WallDuration(0.1); + dt_state_update_ = ros::WallDuration(0.03); double temp_wait_time = 0.05; From 9554440f202cdead3ace84f0d019a40276e94575 Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Mon, 4 May 2020 16:50:59 +0200 Subject: [PATCH 296/612] try publishing with a fixed rate --- .../planning_scene_monitor/src/planning_scene_monitor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index d24e51bd72..e2d67803bc 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -410,7 +410,6 @@ void PlanningSceneMonitor::scenePublishingThread() } if (publish_msg) { - rate.reset(); planning_scene_publisher_.publish(msg); if (is_full) ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str()); From 2fc28767be58920fc74bdf57282b92424249a6b0 Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Mon, 4 May 2020 17:08:47 +0200 Subject: [PATCH 297/612] don't limit subscription of display --- .../planning_scene_rviz_plugin/src/planning_scene_display.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 874940d83a..5be08a27d2 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -115,8 +115,8 @@ PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool s octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR); scene_display_time_property_ = - new rviz::FloatProperty("Scene Display Time", 0.2f, "The amount of wall-time to wait in between rendering " - "updates to the planning scene (if any)", + new rviz::FloatProperty("Scene Display Time", 0.01f, "The amount of wall-time to wait in between rendering " + "updates to the planning scene (if any)", scene_category_, SLOT(changedSceneDisplayTime()), this); scene_display_time_property_->setMin(0.0001); From ea42a6814faa8ce70a80db3e6554766792d399ba Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Fri, 22 May 2020 09:32:20 +0200 Subject: [PATCH 298/612] Utilize new geometric_shapes functions to improve performance (#2038) Utilize createEmptyBodyFromShapeType(), setScaleDirty() and setPaddingDirty() --- .../distance_field/src/distance_field.cpp | 18 ++++++++----- .../src/kinematic_constraint.cpp | 27 +++++++++---------- .../src/shape_mask.cpp | 8 +++--- 3 files changed, 30 insertions(+), 23 deletions(-) diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index d150eb0612..5358a59052 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -208,8 +208,10 @@ bool DistanceField::getShapePoints(const shapes::Shape* shape, const Eigen::Isom } else { - bodies::Body* body = bodies::createBodyFromShape(shape); - body->setPose(pose); + bodies::Body* body = bodies::createEmptyBodyFromShapeType(shape->type); + body->setDimensionsDirty(shape); + body->setPoseDirty(pose); + body->updateInternalData(); findInternalPointsConvex(*body, resolution_, *points); delete body; } @@ -292,8 +294,10 @@ void DistanceField::moveShapeInField(const shapes::Shape* shape, const Eigen::Is ROS_WARN_NAMED("distance_field", "Move shape not supported for Octree"); return; } - bodies::Body* body = bodies::createBodyFromShape(shape); - body->setPose(old_pose); + bodies::Body* body = bodies::createEmptyBodyFromShapeType(shape->type); + body->setDimensionsDirty(shape); + body->setPoseDirty(old_pose); + body->updateInternalData(); EigenSTL::vector_Vector3d old_point_vec; findInternalPointsConvex(*body, resolution_, old_point_vec); body->setPose(new_pose); @@ -315,8 +319,10 @@ void DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_ void DistanceField::removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose) { - bodies::Body* body = bodies::createBodyFromShape(shape); - body->setPose(pose); + bodies::Body* body = bodies::createEmptyBodyFromShapeType(shape->type); + body->setDimensionsDirty(shape); + body->setPoseDirty(pose); + body->updateInternalData(); EigenSTL::vector_Vector3d point_vec; findInternalPointsConvex(*body, resolution_, point_vec); delete body; diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 9ed1c08125..e12e450351 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -315,17 +315,17 @@ bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, co ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses"); continue; } - constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get()))); Eigen::Isometry3d t; tf2::fromMsg(pc.constraint_region.primitive_poses[i], t); constraint_region_pose_.push_back(t); - if (mobile_frame_) - constraint_region_.back()->setPose(constraint_region_pose_.back()); - else - { + if (!mobile_frame_) tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back()); - constraint_region_.back()->setPose(constraint_region_pose_.back()); - } + + const bodies::BodyPtr body(bodies::createEmptyBodyFromShapeType(shape->type)); + body->setDimensionsDirty(shape.get()); + body->setPoseDirty(constraint_region_pose_.back()); + body->updateInternalData(); + constraint_region_.push_back(body); } else ROS_WARN_NAMED("kinematic_constraints", "Could not construct primitive shape %zu", i); @@ -342,17 +342,16 @@ bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, co ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses"); continue; } - constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get()))); Eigen::Isometry3d t; tf2::fromMsg(pc.constraint_region.mesh_poses[i], t); constraint_region_pose_.push_back(t); - if (mobile_frame_) - constraint_region_.back()->setPose(constraint_region_pose_.back()); - else - { + if (!mobile_frame_) tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back()); - constraint_region_.back()->setPose(constraint_region_pose_.back()); - } + const bodies::BodyPtr body(bodies::createEmptyBodyFromShapeType(shape->type)); + body->setDimensionsDirty(shape.get()); + body->setPoseDirty(constraint_region_pose_.back()); + body->updateInternalData(); + constraint_region_.push_back(body); } else { diff --git a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp index 1afaeee43c..5440274826 100644 --- a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp +++ b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp @@ -69,11 +69,13 @@ point_containment_filter::ShapeHandle point_containment_filter::ShapeMask::addSh { boost::mutex::scoped_lock _(shapes_lock_); SeeShape ss; - ss.body = bodies::createBodyFromShape(shape.get()); + ss.body = bodies::createEmptyBodyFromShapeType(shape->type); if (ss.body) { - ss.body->setScale(scale); - ss.body->setPadding(padding); + ss.body->setDimensionsDirty(shape.get()); + ss.body->setScaleDirty(scale); + ss.body->setPaddingDirty(padding); + ss.body->updateInternalData(); ss.volume = ss.body->computeVolume(); ss.handle = next_handle_; std::pair::iterator, bool> insert_op = bodies_.insert(ss); From 44e98d047dd2fd42be7850de1dbd3dde9d6d9a05 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Fri, 22 May 2020 20:36:14 +0100 Subject: [PATCH 299/612] [moveit_core] PropagationDistanceField: Replace eucDistSq with squaredNorm (#2101) This results in a speed-up of 25% for me for large sets of points. Before: 1248ms After: 956ms --- .../distance_field/propagation_distance_field.h | 10 ---------- .../src/propagation_distance_field.cpp | 12 ++---------- 2 files changed, 2 insertions(+), 20 deletions(-) diff --git a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h index dcff47fcda..4b3ad13cd9 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h @@ -539,16 +539,6 @@ class PropagationDistanceField : public DistanceField */ void print(const EigenSTL::vector_Vector3d& points); - /** - * \brief Computes squared distance between two 3D integer points - * - * @param point1 Point 1 for distance - * @param point2 Point 2 for distance - * - * @return Distance between points squared - */ - static int eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i point2); - bool propagate_negative_; /**< \brief Whether or not to propagate negative distances */ VoxelGrid::Ptr voxel_grid_; /**< \brief Actual container for distance data */ diff --git a/moveit_core/distance_field/src/propagation_distance_field.cpp b/moveit_core/distance_field/src/propagation_distance_field.cpp index 4bfa6fa1c3..0cb108cd7b 100644 --- a/moveit_core/distance_field/src/propagation_distance_field.cpp +++ b/moveit_core/distance_field/src/propagation_distance_field.cpp @@ -92,14 +92,6 @@ void PropagationDistanceField::initialize() reset(); } -int PropagationDistanceField::eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i point2) -{ - int dx = point1.x() - point2.x(); - int dy = point1.y() - point2.y(); - int dz = point1.z() - point2.z(); - return dx * dx + dy * dy + dz * dz; -} - void PropagationDistanceField::print(const VoxelSet& set) { ROS_DEBUG_NAMED("distance_field", "["); @@ -430,7 +422,7 @@ void PropagationDistanceField::propagatePositive() // the real update code: // calculate the neighbor's new distance based on my closest filled voxel: PropDistanceFieldVoxel* neighbor = &voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z()); - int new_distance_sq = eucDistSq(vptr->closest_point_, nloc); + int new_distance_sq = (vptr->closest_point_ - nloc).squaredNorm(); if (new_distance_sq > max_distance_sq_) continue; @@ -488,7 +480,7 @@ void PropagationDistanceField::propagateNegative() // the real update code: // calculate the neighbor's new distance based on my closest filled voxel: PropDistanceFieldVoxel* neighbor = &voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z()); - int new_distance_sq = eucDistSq(vptr->closest_negative_point_, nloc); + int new_distance_sq = (vptr->closest_negative_point_ - nloc).squaredNorm(); if (new_distance_sq > max_distance_sq_) continue; // std::cout << "Looking at " << nloc.x() << " " << nloc.y() << " " << nloc.z() << " " << new_distance_sq << " " From d15682bfd43c0142ff3280fc7992785cf4d1d113 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Thu, 21 May 2020 04:29:00 +0200 Subject: [PATCH 300/612] Added isometry checks. --- .../test_collision_common_pr2.h | 6 ++-- .../moveit/collision_detection/world.h | 6 ++-- moveit_core/collision_detection/src/world.cpp | 8 +++++ .../collision_common.h | 2 ++ .../default_constraint_samplers.h | 2 +- .../src/default_constraint_samplers.cpp | 30 +++++++++++-------- .../dynamics_solver/src/dynamics_solver.cpp | 13 ++++---- .../kinematic_constraint.h | 20 ++++++++----- .../src/kinematic_constraint.cpp | 23 ++++++++++++-- .../kinematic_constraints/src/utils.cpp | 1 + .../test/test_constraints.cpp | 4 +-- .../planning_scene/src/planning_scene.cpp | 1 + .../include/moveit/robot_model/joint_model.h | 6 ++-- .../include/moveit/robot_model/link_model.h | 11 +++++-- moveit_core/robot_model/src/aabb.cpp | 2 ++ .../robot_model/src/floating_joint_model.cpp | 7 +++-- moveit_core/robot_model/src/link_model.cpp | 3 ++ .../robot_model/src/planar_joint_model.cpp | 2 ++ .../robot_model/src/revolute_joint_model.cpp | 2 ++ .../moveit/robot_state/attached_body.h | 20 +++++++++---- .../include/moveit/robot_state/robot_state.h | 14 +++++++-- moveit_core/robot_state/src/attached_body.cpp | 15 ++++++++-- .../src/cartesian_interpolator.cpp | 11 +++++-- moveit_core/robot_state/src/robot_state.cpp | 19 +++++++----- .../include/moveit/transforms/transforms.h | 23 ++++++++------ moveit_core/transforms/src/transforms.cpp | 6 ++++ .../src/approach_and_translate_stage.cpp | 2 ++ ...inematics_speed_and_validity_evaluator.cpp | 4 ++- .../src/trajectory_execution_manager.cpp | 3 ++ .../src/wrap_python_robot_interface.cpp | 1 + .../src/motion_planning_frame_objects.cpp | 3 +- .../src/planning_link_updater.cpp | 1 + .../src/render_shapes.cpp | 2 ++ 33 files changed, 199 insertions(+), 74 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h index 8a3eb61e73..b8f3f8e22e 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h @@ -239,7 +239,8 @@ TYPED_TEST_P(CollisionDetectorTest, ContactPositions) } pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); - pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0)); + pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * + Eigen::Quaterniond(sqrt(1 - pow(0.258, 2)), 0.0, 0.258, 0.0)); // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); @@ -259,7 +260,8 @@ TYPED_TEST_P(CollisionDetectorTest, ContactPositions) } pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); - pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0)); + pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * + Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0).normalized()); // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index ad21d3a034..0225fd4cb8 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -151,12 +151,14 @@ class World /** \brief Get the transform to an object or subframe with given name. * If name does not exist, a std::runtime_error is thrown. - * A subframe name needs to be prefixed with the object's name separated by a slash. */ + * A subframe name needs to be prefixed with the object's name separated by a slash. + * The returned transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getTransform(const std::string& name) const; /** \brief Get the transform to an object or subframe with given name. * If name does not exist, returns an identity transform and sets frame_found to false. - * A subframe name needs to be prefixed with the object's name separated by a slash. */ + * A subframe name needs to be prefixed with the object's name separated by a slash. + * The returned transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getTransform(const std::string& name, bool& frame_found) const; /** \brief Add shapes to an object in the map. diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index fe49764cb7..383f298471 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -35,6 +35,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ #include +#include #include #include @@ -59,6 +60,7 @@ inline void World::addToObjectInternal(const ObjectPtr& obj, const shapes::Shape const Eigen::Isometry3d& pose) { obj->shapes_.push_back(shape); + ASSERT_ISOMETRY(pose) // unsanitized input, could contain a non-isometry obj->shape_poses_.push_back(pose); } @@ -208,6 +210,7 @@ bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeC if (it->second->shapes_[i] == shape) { ensureUnique(it->second); + ASSERT_ISOMETRY(pose) // unsanitized input, could contain a non-isometry it->second->shape_poses_[i] = pose; notify(it->second, MOVE_SHAPE); @@ -227,6 +230,7 @@ bool World::moveObject(const std::string& object_id, const Eigen::Isometry3d& tr ensureUnique(it->second); for (size_t i = 0, n = it->second->shapes_.size(); i < n; ++i) { + ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry it->second->shape_poses_[i] = transform * it->second->shape_poses_[i]; } notify(it->second, MOVE_SHAPE); @@ -286,6 +290,10 @@ bool World::setSubframesOfObject(const std::string& object_id, const moveit::cor { return false; } + for (const auto& t : subframe_poses) + { + ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry + } obj_pair->second->subframe_poses_ = subframe_poses; return true; } diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index 7922a7bbdb..be370daf07 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -40,6 +40,7 @@ #include #include #include +#include #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) #include @@ -316,6 +317,7 @@ void cleanCollisionGeometryCache(); /** \brief Transforms an Eigen Isometry3d to FCL coordinate transformation */ inline void transform2fcl(const Eigen::Isometry3d& b, fcl::Transform3d& f) { + ASSERT_ISOMETRY(b); #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) f = b.matrix(); #else diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h index ffc4b5ea8d..79ceeab154 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h @@ -465,7 +465,7 @@ class IKConstraintSampler : public ConstraintSampler * Otherwise, a random quaternion is produced. * * @param [out] pos The position component of the sample - * @param [out] quat The orientation component of the sample + * @param [out] quat The orientation component of the sample. It will always be a normalized quaternion. * @param [in] ks The reference state used for performing transforms * @param [in] max_attempts The maximum attempts to try to sample - applies only to the position constraint * diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 4f78d7edd8..4cdf1e1855 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -374,7 +374,7 @@ bool IKConstraintSampler::loadIKSolver() for (const std::pair& fixed_link : fixed_links) if (moveit::core::Transforms::sameFrame(fixed_link.first->getName(), kb_->getTipFrame())) { - eef_to_ik_tip_transform_ = fixed_link.second; + eef_to_ik_tip_transform_ = fixed_link.second; // valid isometry by contract need_eef_to_ik_tip_transform_ = true; wrong_link = false; break; @@ -392,7 +392,7 @@ bool IKConstraintSampler::loadIKSolver() for (const std::pair& fixed_link : fixed_links) if (moveit::core::Transforms::sameFrame(fixed_link.first->getName(), kb_->getTipFrame())) { - eef_to_ik_tip_transform_ = fixed_link.second; + eef_to_ik_tip_transform_ = fixed_link.second; // valid isometry by contract need_eef_to_ik_tip_transform_ = true; wrong_link = false; break; @@ -479,16 +479,21 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q Eigen::Isometry3d diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ())); + // diff is isometry by construction + // getDesiredRotationMatrix() returns a valid rotation matrix by contract + // reqr has thus to be a valid isometry Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear()); - quat = Eigen::Quaterniond(reqr.linear()); + quat = Eigen::Quaterniond(reqr.linear()); // reqr is isometry, so quat has to be normalized // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the // model if (sampling_pose_.orientation_constraint_->mobileReferenceFrame()) { + // getFrameTransform() returns a valid isometry by contract const Eigen::Isometry3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame()); + // rt is isometry by construction Eigen::Isometry3d rt(t.linear() * quat); - quat = Eigen::Quaterniond(rt.linear()); + quat = Eigen::Quaterniond(rt.linear()); // rt is isometry, so quat has to be normalized } } else @@ -496,7 +501,7 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q // sample a random orientation double q[4]; random_number_generator_.quaternion(q); - quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]); + quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]); // quat is normalized by contract } // if there is an offset, we need to undo the induced rotation in the sampled transform origin (point) @@ -549,7 +554,7 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo { // sample a point in the constraint region Eigen::Vector3d point; - Eigen::Quaterniond quat; + Eigen::Quaterniond quat; // quat is normalized by contract if (!samplePose(point, quat, reference_state, max_attempts)) { if (verbose_) @@ -562,19 +567,20 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo { // we need to convert this transform to the frame expected by the IK solver // both the planning frame and the frame for the IK are assumed to be robot links - Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); - ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq; + Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); // valid isometry by construction + // getFrameTransform() returns a valid isometry by contract + ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq; // valid isometry * valid isometry point = ikq.translation(); - quat = Eigen::Quaterniond(ikq.linear()); + quat = Eigen::Quaterniond(ikq.linear()); // ikq is isometry, so quat is normalized } if (need_eef_to_ik_tip_transform_) { // After sampling the pose needs to be transformed to the ik chain tip - Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); - ikq = ikq * eef_to_ik_tip_transform_; + Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); // valid isometry by construction + ikq = ikq * eef_to_ik_tip_transform_; // eef_to_ik_tip_transform_ is valid isometry (checked in loadIKSolver()) point = ikq.translation(); - quat = Eigen::Quaterniond(ikq.linear()); + quat = Eigen::Quaterniond(ikq.linear()); // ikq is isometry, so quat is normalized } geometry_msgs::Pose ik_query; diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index 5462fd2ae5..1126d29cdf 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -53,6 +53,7 @@ inline geometry_msgs::Vector3 transformVector(const Eigen::Isometry3d& transform { Eigen::Vector3d p; p = Eigen::Vector3d(vector.x, vector.y, vector.z); + // transform has to be a valid isometry; the caller is responsible for the check p = transform.linear() * p; geometry_msgs::Vector3 result; @@ -243,9 +244,9 @@ bool DynamicsSolver::getMaxPayload(const std::vector& joint_angles, doub } state_->setJointGroupPositions(joint_model_group_, joint_angles); - const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_); - const Eigen::Isometry3d& tip_frame = state_->getFrameTransform(tip_name_); - Eigen::Isometry3d transform = tip_frame.inverse() * base_frame; + const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_); // valid isometry by contract + const Eigen::Isometry3d& tip_frame = state_->getFrameTransform(tip_name_); // valid isometry by contract + Eigen::Isometry3d transform = tip_frame.inverse() * base_frame; // valid isometry by construction wrenches.back().force.z = 1.0; wrenches.back().force = transformVector(transform, wrenches.back().force); wrenches.back().torque = transformVector(transform, wrenches.back().torque); @@ -299,9 +300,9 @@ bool DynamicsSolver::getPayloadTorques(const std::vector& joint_angles, std::vector wrenches(num_segments_); state_->setJointGroupPositions(joint_model_group_, joint_angles); - const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_); - const Eigen::Isometry3d& tip_frame = state_->getFrameTransform(tip_name_); - Eigen::Isometry3d transform = tip_frame.inverse() * base_frame; + const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_); // valid isometry by contract + const Eigen::Isometry3d& tip_frame = state_->getFrameTransform(tip_name_); // valid isometry by contract + Eigen::Isometry3d transform = tip_frame.inverse() * base_frame; // valid isometry by construction wrenches.back().force.z = payload * gravity_; wrenches.back().force = transformVector(transform, wrenches.back().force); wrenches.back().torque = transformVector(transform, wrenches.back().torque); diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index cf28da8858..70cf8f0871 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -434,10 +434,13 @@ class OrientationConstraint : public KinematicConstraint /** * \brief The rotation target in the reference frame. * - * @return The target rotation + * @return The target rotation. + * + * The returned matrix is always a valid rotation matrix. */ const Eigen::Matrix3d& getDesiredRotationMatrix() const { + // validity of the rotation matrix is enforced in configure() return desired_rotation_matrix_; } @@ -476,10 +479,10 @@ class OrientationConstraint : public KinematicConstraint protected: const moveit::core::LinkModel* link_model_; /**< \brief The target link model */ - Eigen::Matrix3d desired_rotation_matrix_; /**< \brief The desired rotation matrix in the tf frame */ + Eigen::Matrix3d desired_rotation_matrix_; /**< \brief The desired rotation matrix in the tf frame. Guaranteed to + * be valid rotation matrix. */ Eigen::Matrix3d desired_rotation_matrix_inv_; /**< \brief The inverse of the desired rotation matrix, precomputed for - * efficiency - */ + * efficiency. Guaranteed to be valid rotation matrix. */ std::string desired_rotation_frame_id_; /**< \brief The target frame of the transform tree */ bool mobile_frame_; /**< \brief Whether or not the header frame is mobile or fixed */ double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, @@ -641,10 +644,11 @@ class PositionConstraint : public KinematicConstraint Eigen::Vector3d offset_; /**< \brief The target offset */ bool has_offset_; /**< \brief Whether the offset is substantially different than 0.0 */ std::vector constraint_region_; /**< \brief The constraint region vector */ - EigenSTL::vector_Isometry3d constraint_region_pose_; /**< \brief The constraint region pose vector */ - bool mobile_frame_; /**< \brief Whether or not a mobile frame is employed*/ - std::string constraint_frame_id_; /**< \brief The constraint frame id */ - const moveit::core::LinkModel* link_model_; /**< \brief The link model constraint subject */ + /** \brief The constraint region pose vector. All isometries are guaranteed to be valid. */ + EigenSTL::vector_Isometry3d constraint_region_pose_; + bool mobile_frame_; /**< \brief Whether or not a mobile frame is employed*/ + std::string constraint_frame_id_; /**< \brief The constraint frame id */ + const moveit::core::LinkModel* link_model_; /**< \brief The link model constraint subject */ }; MOVEIT_CLASS_FORWARD(VisibilityConstraint); diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 3ca1d5c719..8101a46a4d 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -318,6 +319,7 @@ bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, co constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get()))); Eigen::Isometry3d t; tf2::fromMsg(pc.constraint_region.primitive_poses[i], t); + ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry constraint_region_pose_.push_back(t); if (mobile_frame_) constraint_region_.back()->setPose(constraint_region_pose_.back()); @@ -345,6 +347,7 @@ bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, co constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get()))); Eigen::Isometry3d t; tf2::fromMsg(pc.constraint_region.mesh_poses[i], t); + ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry constraint_region_pose_.push_back(t); if (mobile_frame_) constraint_region_.back()->setPose(constraint_region_pose_.back()); @@ -390,6 +393,7 @@ bool PositionConstraint::equal(const KinematicConstraint& other, double margin) // need to check against all other regions for (std::size_t j = 0; j < o.constraint_region_.size(); ++j) { + // constraint_region_pose_ contain only valid isometries, so diff is also a valid isometry Eigen::Isometry3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[j]; if (diff.translation().norm() < margin && diff.linear().isIdentity(margin) && constraint_region_[i]->getType() == o.constraint_region_[j]->getType() && @@ -597,13 +601,16 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob Eigen::Vector3d xyz; if (mobile_frame_) { + // getFrameTransform() returns a valid isometry by contract Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).linear() * desired_rotation_matrix_; - Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).linear()); + // getGlobalLinkTransform() returns a valid isometry by contract + Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).linear()); // valid isometry xyz = diff.linear().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints } else { + // diff is valid isometry by construction Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).linear()); xyz = diff.linear().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints } @@ -695,6 +702,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc } tf2::fromMsg(vc.target_pose.pose, target_pose_); + ASSERT_ISOMETRY(target_pose_) // unsanitized input, could contain a non-isometry if (tf.isFixedFrame(vc.target_pose.header.frame_id)) { @@ -712,6 +720,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc } tf2::fromMsg(vc.sensor_pose.pose, sensor_pose_); + ASSERT_ISOMETRY(sensor_pose_) // unsanitized input, could contain a non-isometry if (tf.isFixedFrame(vc.sensor_pose.header.frame_id)) { @@ -752,12 +761,14 @@ bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin { if (fabs(max_view_angle_ - o.max_view_angle_) > margin || fabs(target_radius_ - o.target_radius_) > margin) return false; - Eigen::Isometry3d diff = sensor_pose_.inverse() * o.sensor_pose_; + // sensor_pose_ is valid isometry, checked in configure() + Eigen::Isometry3d diff = sensor_pose_.inverse() * o.sensor_pose_; // valid isometry if (diff.translation().norm() > margin) return false; if (!diff.linear().isIdentity(margin)) return false; - diff = target_pose_.inverse() * o.target_pose_; + // target_pose_ is valid isometry, checked in configure() + diff = target_pose_.inverse() * o.target_pose_; // valid isometry if (diff.translation().norm() > margin) return false; if (!diff.linear().isIdentity(margin)) @@ -875,8 +886,11 @@ void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state, markers.markers.push_back(mk); + // getFrameTransform() returns a valid isometry by contract + // sensor_pose_ is valid isometry (checked in configure()) const Eigen::Isometry3d& sp = mobile_sensor_frame_ ? state.getFrameTransform(sensor_frame_id_) * sensor_pose_ : sensor_pose_; + // target_pose_ is valid isometry (checked in configure()) const Eigen::Isometry3d& tp = mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_; @@ -925,8 +939,11 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo if (max_view_angle_ > 0.0 || max_range_angle_ > 0.0) { + // getFrameTransform() returns a valid isometry by contract + // sensor_pose_ is valid isometry (checked in configure()) const Eigen::Isometry3d& sp = mobile_sensor_frame_ ? state.getFrameTransform(sensor_frame_id_) * sensor_pose_ : sensor_pose_; + // target_pose_ is valid isometry (checked in configure()) const Eigen::Isometry3d& tp = mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_; diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index c9cb35641e..088fff83db 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -558,6 +558,7 @@ bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotSta { bool frame_found; const moveit::core::LinkModel* robot_link; + // getFrameInfo() returns a valid isometry by contract const Eigen::Isometry3d& transform = state.getFrameInfo(c.link_name, robot_link, frame_found); if (!frame_found) return false; diff --git a/moveit_core/kinematic_constraints/test/test_constraints.cpp b/moveit_core/kinematic_constraints/test/test_constraints.cpp index b16e9505ae..2ffe6df1ee 100644 --- a/moveit_core/kinematic_constraints/test/test_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_constraints.cpp @@ -694,13 +694,13 @@ TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple) // very slight angle, so still ok vcm.target_pose.pose.orientation.y = 0.03; - vcm.target_pose.pose.orientation.w = .9995; + vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2)); EXPECT_TRUE(vc.configure(vcm, tf)); EXPECT_TRUE(vc.decide(robot_state, true).satisfied); // a little bit more puts it over vcm.target_pose.pose.orientation.y = 0.06; - vcm.target_pose.pose.orientation.w = .9981; + vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2)); EXPECT_TRUE(vc.configure(vcm, tf)); EXPECT_FALSE(vc.decide(robot_state, true).satisfied); } diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index f340a5f144..be510d88f5 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -964,6 +964,7 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const for (std::size_t j = 0; j < obj->shapes_.size(); ++j) { shapes::saveAsText(obj->shapes_[j].get(), out); + // shape_poses_ is valid isometry by contract out << obj->shape_poses_[j].translation().x() << " " << obj->shape_poses_[j].translation().y() << " " << obj->shape_poses_[j].translation().z() << std::endl; Eigen::Quaterniond r(obj->shape_poses_[j].linear()); diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index aa07321d73..173590129d 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -443,10 +443,12 @@ class JointModel /** @name Computing transforms @{ */ - /** \brief Given the joint values for a joint, compute the corresponding transform */ + /** \brief Given the joint values for a joint, compute the corresponding transform. The computed transform is + * guaranteed to be a valid isometry. */ virtual void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const = 0; - /** \brief Given the transform generated by joint, compute the corresponding joint values */ + /** \brief Given the transform generated by joint, compute the corresponding joint values. Make sure the passed + * transform is a valid isometry. */ virtual void computeVariablePositions(const Eigen::Isometry3d& transform, double* joint_values) const = 0; /** @} */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.h b/moveit_core/robot_model/include/moveit/robot_model/link_model.h index d586dfded0..5d81a53fa8 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/link_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.h @@ -43,6 +43,7 @@ #include #include #include +#include namespace shapes { @@ -137,7 +138,8 @@ class LinkModel /** \brief When transforms are computed for this link, they are usually applied to the link's origin. The joint origin transform acts as an offset -- it is - pre-applied before any other transform */ + pre-applied before any other transform. The + transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getJointOriginTransform() const { return joint_origin_transform_; @@ -157,7 +159,8 @@ class LinkModel /** \brief In addition to the link transform, the geometry of a link that is used for collision checking may have - a different offset itself, with respect to the origin */ + a different offset itself, with respect to the origin. + The transform is guaranteed to be a valid isometry. */ const EigenSTL::vector_Isometry3d& getCollisionOriginTransforms() const { return collision_origin_transform_; @@ -191,7 +194,8 @@ class LinkModel return centered_bounding_box_offset_; } - /** \brief Get the set of links that are attached to this one via fixed transforms */ + /** \brief Get the set of links that are attached to this one via fixed transforms. The returned transforms are + * guaranteed to be valid isometries. */ const LinkTransformMap& getAssociatedFixedTransforms() const { return associated_fixed_transforms_; @@ -200,6 +204,7 @@ class LinkModel /** \brief Remember that \e link_model is attached to this link using a fixed transform */ void addAssociatedFixedTransform(const LinkModel* link_model, const Eigen::Isometry3d& transform) { + ASSERT_ISOMETRY(transform); // unsanitized input, could contain a non-isometry associated_fixed_transforms_[link_model] = transform; } diff --git a/moveit_core/robot_model/src/aabb.cpp b/moveit_core/robot_model/src/aabb.cpp index 51b3523032..1b115cfefc 100644 --- a/moveit_core/robot_model/src/aabb.cpp +++ b/moveit_core/robot_model/src/aabb.cpp @@ -35,6 +35,7 @@ /* Author: Martin Pecka */ #include +#include void moveit::core::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box) { @@ -44,6 +45,7 @@ void moveit::core::AABB::extendWithTransformedBox(const Eigen::Isometry3d& trans // // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/ + ASSERT_ISOMETRY(transform) // unsanitized input, could contain non-isometry const Eigen::Matrix3d& r = transform.linear(); const Eigen::Vector3d& t = transform.translation(); diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index b7d25080a2..15f0191f3b 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -36,6 +36,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -216,8 +217,9 @@ bool FloatingJointModel::enforcePositionBounds(double* values, const Bounds& bou void FloatingJointModel::computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const { - transf = Eigen::Isometry3d(Eigen::Translation3d(joint_values[0], joint_values[1], joint_values[2]) * - Eigen::Quaterniond(joint_values[6], joint_values[3], joint_values[4], joint_values[5])); + transf = Eigen::Isometry3d( + Eigen::Translation3d(joint_values[0], joint_values[1], joint_values[2]) * + Eigen::Quaterniond(joint_values[6], joint_values[3], joint_values[4], joint_values[5]).normalized()); } void FloatingJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const @@ -225,6 +227,7 @@ void FloatingJointModel::computeVariablePositions(const Eigen::Isometry3d& trans joint_values[0] = transf.translation().x(); joint_values[1] = transf.translation().y(); joint_values[2] = transf.translation().z(); + ASSERT_ISOMETRY(transf) // unsanitized input, could contain non-isometry Eigen::Quaterniond q(transf.linear()); joint_values[3] = q.x(); joint_values[4] = q.y(); diff --git a/moveit_core/robot_model/src/link_model.cpp b/moveit_core/robot_model/src/link_model.cpp index c75ab24e0a..76f45e3bb6 100644 --- a/moveit_core/robot_model/src/link_model.cpp +++ b/moveit_core/robot_model/src/link_model.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -59,6 +60,7 @@ LinkModel::~LinkModel() = default; void LinkModel::setJointOriginTransform(const Eigen::Isometry3d& transform) { + ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry joint_origin_transform_ = transform; joint_origin_transform_is_identity_ = joint_origin_transform_.linear().isIdentity() && @@ -82,6 +84,7 @@ void LinkModel::setGeometry(const std::vector& shapes, for (std::size_t i = 0; i < shapes_.size(); ++i) { + ASSERT_ISOMETRY(collision_origin_transform_[i]) // unsanitized input, could contain a non-isometry collision_origin_transform_is_identity_[i] = (collision_origin_transform_[i].linear().isIdentity() && collision_origin_transform_[i].translation().norm() < std::numeric_limits::epsilon()) ? diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp index f515be4c9f..2727ccd48e 100644 --- a/moveit_core/robot_model/src/planar_joint_model.cpp +++ b/moveit_core/robot_model/src/planar_joint_model.cpp @@ -36,6 +36,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -224,6 +225,7 @@ void PlanarJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, joint_values[0] = transf.translation().x(); joint_values[1] = transf.translation().y(); + ASSERT_ISOMETRY(transf) // unsanitized input, could contain a non-isometry Eigen::Quaterniond q(transf.linear()); // taken from Bullet double s_squared = 1.0 - (q.w() * q.w()); diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index c3783d48e1..4a87291f38 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -36,6 +36,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -268,6 +269,7 @@ void RevoluteJointModel::computeTransform(const double* joint_values, Eigen::Iso void RevoluteJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const { + ASSERT_ISOMETRY(transf) // unsanitized input, could contain a non-isometry Eigen::Quaterniond q(transf.linear()); q.normalize(); size_t max_idx; diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index 7086d00120..657c147dc7 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -38,6 +38,7 @@ #include #include +#include #include #include #include @@ -105,13 +106,15 @@ class AttachedBody return detach_posture_; } - /** \brief Get the fixed transforms (the transforms to the shapes of this body, relative to the link) */ + /** \brief Get the fixed transforms (the transforms to the shapes of this body, relative to the link). The returned + * transforms are guaranteed to be valid isometries. */ const EigenSTL::vector_Isometry3d& getFixedTransforms() const { return attach_trans_; } - /** \brief Get subframes of this object (relative to the link) */ + /** \brief Get subframes of this object (relative to the link). The returned transforms are guaranteed to be valid + * isometries. */ const moveit::core::FixedTransformsMap& getSubframeTransforms() const { return subframe_poses_; @@ -124,18 +127,24 @@ class AttachedBody */ void setSubframeTransforms(const moveit::core::FixedTransformsMap& subframe_poses) { + for (const auto& t : subframe_poses) + { + ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry + } subframe_poses_ = subframe_poses; } /** \brief Get the fixed transform to a named subframe on this body (relative to the robot link) * * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's - * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). */ + * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). + * The returned transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; /** \brief Get the fixed transform to a named subframe on this body. * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's - * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). */ + * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). + * The returned transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getGlobalSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; /** \brief Check whether a subframe of given @frame_name is present in this object. @@ -144,7 +153,8 @@ class AttachedBody * name is "screwdriver"). */ bool hasSubframeTransform(const std::string& frame_name) const; - /** \brief Get the global transforms for the collision bodies */ + /** \brief Get the global transforms for the collision bodies. The returned transforms are guaranteed to be valid + * isometries. */ const EigenSTL::vector_Isometry3d& getGlobalCollisionBodyTransforms() const { return global_collision_body_transforms_; diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 00004d0247..5731a6a4ff 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1307,6 +1307,8 @@ class RobotState * Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed. * A related, more comprehensive function is |getFrameTransform|, which additionally to link frames * also searches for attached object frames and their subframes. + * + * The returned transformation is always a valid isometry. */ const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) { @@ -1647,18 +1649,24 @@ class RobotState /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * - * If frame_id was not found, \e frame_found is set to false and an identity transform is returned */ + * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. + * + * The returned transformation is always a valid isometry. */ const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr); /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * - * If frame_id was not found, \e frame_found is set to false and an identity transform is returned */ + * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. + * + * The returned transformation is always a valid isometry. */ const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr) const; /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * * If this frame is attached to a robot link, the link pointer is returned in \e robot_link. - * If frame_id was not found, \e frame_found is set to false and an identity transform is returned */ + * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. + * + * The returned transformation is always a valid isometry. */ const Eigen::Isometry3d& getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link, bool& frame_found) const; diff --git a/moveit_core/robot_state/src/attached_body.cpp b/moveit_core/robot_state/src/attached_body.cpp index 51a403a7a2..ff3fbaa3fb 100644 --- a/moveit_core/robot_state/src/attached_body.cpp +++ b/moveit_core/robot_state/src/attached_body.cpp @@ -35,6 +35,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include @@ -56,6 +57,14 @@ AttachedBody::AttachedBody(const LinkModel* parent_link_model, const std::string , subframe_poses_(subframe_poses) , global_subframe_poses_(subframe_poses) { + for (const auto& t : attach_trans_) + { + ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry + } + for (const auto& t : subframe_poses_) + { + ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry + } global_collision_body_transforms_.resize(attach_trans.size()); for (Eigen::Isometry3d& global_collision_body_transform : global_collision_body_transforms_) global_collision_body_transform.setIdentity(); @@ -82,15 +91,17 @@ void AttachedBody::setScale(double scale) void AttachedBody::computeTransform(const Eigen::Isometry3d& parent_link_global_transform) { + ASSERT_ISOMETRY(parent_link_global_transform) // unsanitized input, could contain a non-isometry + // update collision body transforms for (std::size_t i = 0; i < global_collision_body_transforms_.size(); ++i) - global_collision_body_transforms_[i] = parent_link_global_transform * attach_trans_[i]; + global_collision_body_transforms_[i] = parent_link_global_transform * attach_trans_[i]; // valid isometry // update subframe transforms for (auto global = global_subframe_poses_.begin(), end = global_subframe_poses_.end(), local = subframe_poses_.begin(); global != end; ++global, ++local) - global->second = parent_link_global_transform * local->second; + global->second = parent_link_global_transform * local->second; // valid isometry } void AttachedBody::setPadding(double padding) diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index b2b38db361..5431b490af 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -37,6 +37,7 @@ /* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */ #include +#include namespace moveit { @@ -59,13 +60,14 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons const kinematics::KinematicsQueryOptions& options) { // this is the Cartesian pose we start from, and have to move in the direction indicated + // getGlobalLinkTransform() returns a valid isometry by contract const Eigen::Isometry3d& start_pose = start_state->getGlobalLinkTransform(link); // the direction can be in the local reference frame (in which case we rotate it) const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.linear() * direction; // The target pose is built by applying a translation to the start pose for the desired direction and distance - Eigen::Isometry3d target_pose = start_pose; + Eigen::Isometry3d target_pose = start_pose; // valid isometry target_pose.translation() += rotated_direction * distance; // call computeCartesianPath for the computed target pose in the global reference frame @@ -86,10 +88,13 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons start_state->enforceBounds(joint); // this is the Cartesian pose we start from, and we move in the direction indicated - Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link); + // getGlobalLinkTransform() returns a valid isometry by contract + Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link); // valid isometry + + ASSERT_ISOMETRY(target) // unsanitized input, could contain a non-isometry // the target can be in the local reference frame (in which case we rotate it) - Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; + Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; // valid isometry Eigen::Quaterniond start_quaternion(start_pose.linear()); Eigen::Quaterniond target_quaternion(rotated_target.linear()); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index c8dda3b746..da3750bfd5 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -1202,13 +1203,15 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link const moveit::core::JointModel* root_joint_model = group->getJointModels()[0]; // group->getJointRoots()[0]; const moveit::core::LinkModel* root_link_model = root_joint_model->getParentLinkModel(); + // getGlobalLinkTransform() returns a valid isometry by contract Eigen::Isometry3d reference_transform = root_link_model ? getGlobalLinkTransform(root_link_model).inverse() : Eigen::Isometry3d::Identity(); int rows = use_quaternion_representation ? 7 : 6; int columns = group->getVariableCount(); jacobian = Eigen::MatrixXd::Zero(rows, columns); - Eigen::Isometry3d link_transform = reference_transform * getGlobalLinkTransform(link); + // getGlobalLinkTransform() returns a valid isometry by contract + Eigen::Isometry3d link_transform = reference_transform * getGlobalLinkTransform(link); // valid isometry Eigen::Vector3d point_transform = link_transform * reference_point_position; /* @@ -1239,9 +1242,10 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link continue; } unsigned int joint_index = group->getVariableGroupIndex(pjm->getName()); + // getGlobalLinkTransform() returns a valid isometry by contract + joint_transform = reference_transform * getGlobalLinkTransform(link); // valid isometry if (pjm->getType() == moveit::core::JointModel::REVOLUTE) { - joint_transform = reference_transform * getGlobalLinkTransform(link); joint_axis = joint_transform.linear() * static_cast(pjm)->getAxis(); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation()); @@ -1249,13 +1253,11 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link } else if (pjm->getType() == moveit::core::JointModel::PRISMATIC) { - joint_transform = reference_transform * getGlobalLinkTransform(link); joint_axis = joint_transform.linear() * static_cast(pjm)->getAxis(); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis; } else if (pjm->getType() == moveit::core::JointModel::PLANAR) { - joint_transform = reference_transform * getGlobalLinkTransform(link); joint_axis = joint_transform * Eigen::Vector3d(1.0, 0.0, 0.0); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis; joint_axis = joint_transform * Eigen::Vector3d(0.0, 1.0, 0.0); @@ -1775,6 +1777,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: // Each each pose's tip frame naming for (std::size_t i = 0; i < poses_in.size(); ++i) { + ASSERT_ISOMETRY(transformed_poses[i]) // unsanitized input, could contain a non-isometry Eigen::Isometry3d& pose = transformed_poses[i]; std::string& pose_frame = pose_frames[i]; @@ -1795,26 +1798,27 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: if (hasAttachedBody(pose_frame)) { const AttachedBody* body = getAttachedBody(pose_frame); - const EigenSTL::vector_Isometry3d& ab_trans = body->getFixedTransforms(); + const EigenSTL::vector_Isometry3d& ab_trans = body->getFixedTransforms(); // valid isometries by contract if (ab_trans.size() != 1) { ROS_ERROR_NAMED(LOGNAME, "Cannot use an attached body with multiple geometries as a reference frame."); return false; } pose_frame = body->getAttachedLinkName(); - pose = pose * ab_trans[0].inverse(); + pose = pose * ab_trans[0].inverse(); // valid isometry } if (pose_frame != solver_tip_frame) { const moveit::core::LinkModel* link_model = getLinkModel(pose_frame); if (!link_model) return false; + // getAssociatedFixedTransforms() returns valid isometries by contract const moveit::core::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); for (const std::pair& fixed_link : fixed_links) if (fixed_link.first->getName() == solver_tip_frame) { pose_frame = solver_tip_frame; - pose = pose * fixed_link.second; + pose = pose * fixed_link.second; // valid isometry break; } } @@ -2101,6 +2105,7 @@ void RobotState::printStateInfo(std::ostream& out) const void RobotState::printTransform(const Eigen::Isometry3d& transform, std::ostream& out) const { + ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry Eigen::Quaterniond q(transform.linear()); out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", " << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 8e72213aa6..5fc6a81d9f 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -86,13 +86,14 @@ class Transforms : private boost::noncopyable /** * @brief Return all the transforms - * @return A map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the planning frame) + * @return A map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the planning frame). The + * transforms are guaranteed to be valid isometries. */ const FixedTransformsMap& getAllTransforms() const; /** * @brief Get a vector of all the transforms as ROS messages - * @param transforms The output transforms + * @param transforms The output transforms. They are guaranteed to be valid isometries. */ void copyTransforms(std::vector& transforms) const; @@ -137,40 +138,44 @@ class Transforms : private boost::noncopyable */ void transformVector3(const std::string& from_frame, const Eigen::Vector3d& v_in, Eigen::Vector3d& v_out) const { + // getTransform() returns a valid isometry by contract v_out = getTransform(from_frame).linear() * v_in; } /** * @brief Transform a quaternion in from_frame to the target_frame * @param from_frame The frame in which the input quaternion is specified - * @param v_in The input quaternion (in from_frame) - * @param v_out The resultant (transformed) quaternion + * @param v_in The input quaternion (in from_frame). Make sure the quaternion is normalized. + * @param v_out The resultant (transformed) quaternion. It is guaranteed to be a valid and normalized quaternion. */ void transformQuaternion(const std::string& from_frame, const Eigen::Quaterniond& q_in, Eigen::Quaterniond& q_out) const { + // getTransform() returns a valid isometry by contract q_out = getTransform(from_frame).linear() * q_in; } /** * @brief Transform a rotation matrix in from_frame to the target_frame * @param from_frame The frame in which the input rotation matrix is specified - * @param m_in The input rotation matrix (in from_frame) - * @param m_out The resultant (transformed) rotation matrix + * @param m_in The input rotation matrix (in from_frame). Make sure the matrix is a valid rotation matrix. + * @param m_out The resultant (transformed) rotation matrix. It is guaranteed to be a valid rotation matrix. */ void transformRotationMatrix(const std::string& from_frame, const Eigen::Matrix3d& m_in, Eigen::Matrix3d& m_out) const { + // getTransform() returns a valid isometry by contract m_out = getTransform(from_frame).linear() * m_in; } /** * @brief Transform a pose in from_frame to the target_frame * @param from_frame The frame in which the input pose is specified - * @param t_in The input pose (in from_frame) - * @param t_out The resultant (transformed) pose + * @param t_in The input pose (in from_frame). Make sure the pose is a valid isometry. + * @param t_out The resultant (transformed) pose. It is guaranteed to be a valid isometry. */ void transformPose(const std::string& from_frame, const Eigen::Isometry3d& t_in, Eigen::Isometry3d& t_out) const { + // getTransform() returns a valid isometry by contract t_out = getTransform(from_frame) * t_in; } /**@}*/ @@ -189,7 +194,7 @@ class Transforms : private boost::noncopyable /** * @brief Get transform for from_frame (w.r.t target frame) * @param from_frame The string id of the frame for which the transform is being computed - * @return The required transform + * @return The required transform. It is guaranteed to be a valid isometry. */ virtual const Eigen::Isometry3d& getTransform(const std::string& from_frame) const; diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index 190fb10d0d..005bc2e07f 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -35,6 +35,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -75,6 +76,10 @@ const FixedTransformsMap& Transforms::getAllTransforms() const void Transforms::setAllTransforms(const FixedTransformsMap& transforms) { + for (const auto& t : transforms) + { + ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry + } transforms_map_ = transforms; } @@ -114,6 +119,7 @@ bool Transforms::canTransform(const std::string& from_frame) const void Transforms::setTransform(const Eigen::Isometry3d& t, const std::string& from_frame) { + ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry if (from_frame.empty()) ROS_ERROR_NAMED("transforms", "Cannot record transform with empty name"); else diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp index 3fdde91816..4a667fd489 100644 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp @@ -215,9 +215,11 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // transform the input vectors in accordance to frame specified in the header; if (approach_direction_is_global_frame) + // getFrameTransform() returns a valid isometry by contract approach_direction = planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).linear() * approach_direction; if (retreat_direction_is_global_frame) + // getFrameTransform() returns a valid isometry by contract retreat_direction = planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).linear() * retreat_direction; diff --git a/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp b/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp index 4dc408396a..7030c918da 100644 --- a/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp +++ b/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp @@ -87,13 +87,15 @@ int main(int argc, char** argv) for (unsigned int i = 0; i < test_count; ++i) { state.setToRandomPositions(jmg); + // getGlobalLinkTransform() returns a valid isometry by contract Eigen::Isometry3d pose = state.getGlobalLinkTransform(tip); state.setToRandomPositions(jmg); moveit::tools::Profiler::Begin("IK"); state.setFromIK(jmg, pose); moveit::tools::Profiler::End("IK"); + // getGlobalLinkTransform() returns a valid isometry by contract const Eigen::Isometry3d& pose_upd = state.getGlobalLinkTransform(tip); - Eigen::Isometry3d diff = pose_upd * pose.inverse(); + Eigen::Isometry3d diff = pose_upd * pose.inverse(); // valid isometry double rot_err = (diff.linear() - Eigen::Matrix3d::Identity()).norm(); double trans_err = diff.translation().norm(); moveit::tools::Profiler::Average("Rotation error", rot_err); diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 1db84cf7ac..c07c592fb8 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -1005,8 +1006,10 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont // compute difference (offset vector and rotation angle) between current transform // and start transform in trajectory Eigen::Isometry3d cur_transform, start_transform; + // computeTransform() computes a valid isometry by contract jm->computeTransform(current_state->getJointPositions(jm), cur_transform); start_transform = tf2::transformToEigen(transforms[i]); + ASSERT_ISOMETRY(start_transform) // unsanitized input, could contain a non-isometry Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation(); Eigen::AngleAxisd rotation; rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear()); diff --git a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp index 35c1e047ad..f6e3d40061 100644 --- a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp +++ b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp @@ -150,6 +150,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer const moveit::core::LinkModel* lm = state->getLinkModel(name); if (lm) { + // getGlobalLinkTransform() returns a valid isometry by contract const Eigen::Isometry3d& t = state->getGlobalLinkTransform(lm); std::vector v(7); v[0] = t.translation().x(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 8573ba7f51..85cf1631e1 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -237,7 +237,7 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() if (obj->shapes_.size() == 1) { - obj_pose = obj->shape_poses_[0]; + obj_pose = obj->shape_poses_[0]; // valid isometry by contract Eigen::Vector3d xyz = obj_pose.linear().eulerAngles(0, 1, 2); update_scene_marker = true; // do the marker update outside locked scope to avoid deadlock @@ -322,6 +322,7 @@ void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position) // Update the interactive marker pose to match the manually introduced one if (update_marker_position && scene_marker_) { + // p is isometry by construction Eigen::Quaterniond eq(p.linear()); scene_marker_->setPose(Ogre::Vector3(ui_->object_x->value(), ui_->object_y->value(), ui_->object_z->value()), Ogre::Quaternion(eq.w(), eq.x(), eq.y(), eq.z()), ""); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp index 2455db97ca..26259978cb 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp @@ -51,6 +51,7 @@ bool moveit_rviz_plugin::PlanningLinkUpdater::getLinkTransforms(const std::strin return false; } + // getGlobalLinkTransform() returns a valid isometry by contract const Eigen::Vector3d& robot_visual_position = kinematic_state_->getGlobalLinkTransform(link_model).translation(); Eigen::Quaterniond robot_visual_orientation(kinematic_state_->getGlobalLinkTransform(link_model).linear()); visual_position = Ogre::Vector3(robot_visual_position.x(), robot_visual_position.y(), robot_visual_position.z()); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index 19e06f150c..f2ddd86aeb 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -82,6 +83,7 @@ void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, co rviz::Shape* ogre_shape = nullptr; Eigen::Vector3d translation = p.translation(); Ogre::Vector3 position(translation.x(), translation.y(), translation.z()); + ASSERT_ISOMETRY(p) // unsanitized input, could contain a non-isometry Eigen::Quaterniond q(p.linear()); Ogre::Quaternion orientation(q.w(), q.x(), q.y(), q.z()); From 7aa1699f84d38f2881156b0a7466d3d528228e5d Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Tue, 26 May 2020 11:09:47 -0600 Subject: [PATCH 301/612] Prevent collision checking segfault if octomap has NULL root pointer (#2104) --- .../collision_detection_fcl/src/collision_common.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index 862104d756..30aa876374 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -517,6 +517,14 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void } fcl_result.min_distance = dist_threshold; + // fcl::distance segfaults when given an octree with a null root pointer (using FCL 0.6.1) + if ((o1->getObjectType() == fcl::OT_OCTREE && + !std::static_pointer_cast(o1->collisionGeometry())->getRoot()) || + (o2->getObjectType() == fcl::OT_OCTREE && + !std::static_pointer_cast(o2->collisionGeometry())->getRoot())) + { + return false; + } double d = fcl::distance(o1, o2, fcl::DistanceRequestd(cdata->req->enable_nearest_points), fcl_result); // Check if either object is already in the map. If not add it or if present From 3f3ef1fa8f8a9856c24df97fcb3ff09a0aaec24c Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Tue, 26 May 2020 20:15:36 +0300 Subject: [PATCH 302/612] PlanningComponent: Load plan_request_params (#2033) Load plan_request_params and use them when calling plan(void). --- .../moveit/moveit_cpp/planning_component.h | 13 +++++++++++- .../moveit_cpp/src/moveit_cpp.cpp | 4 ++-- .../moveit_cpp/src/planning_component.cpp | 21 +++++++++++-------- .../planning_interface/test/moveit_cpp.yaml | 3 ++- 4 files changed, 28 insertions(+), 13 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index c8bc15c6f3..7a48c9446f 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -108,10 +108,21 @@ class PlanningComponent { std::string planner_id; std::string planning_pipeline; - size_t planning_attempts; + int planning_attempts; double planning_time; double max_velocity_scaling_factor; double max_acceleration_scaling_factor; + + void load(const ros::NodeHandle& nh) + { + std::string ns = "plan_request_params/"; + nh.param(ns + "planner_id", planner_id, std::string("")); + nh.param(ns + "planning_pipeline", planning_pipeline, std::string("")); + nh.param(ns + "planning_time", planning_attempts, 1); + nh.param(ns + "planning_attempts", planning_time, 5.0); + nh.param(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor, 1.0); + nh.param(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor, 1.0); + } }; /** \brief Constructor */ diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index 7f9b6700dc..8b5b494e7e 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -57,9 +57,9 @@ MoveItCpp::MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) - : tf_buffer_(tf_buffer) + : tf_buffer_(tf_buffer), node_handle_(nh) { if (!tf_buffer_) tf_buffer_.reset(new tf2_ros::Buffer()); diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp index 47f3ff56b3..102489b269 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -81,10 +81,19 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const MoveIt throw std::runtime_error(error); } planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name); + plan_request_parameters_.load(nh_); + ROS_DEBUG_STREAM_NAMED( + LOGNAME, "Plan request parameters loaded with --" + << " planning_pipeline: " << plan_request_parameters_.planning_pipeline << "," + << " planner_id: " << plan_request_parameters_.planner_id << "," + << " planning_time: " << plan_request_parameters_.planning_time << "," + << " planning_attempts: " << plan_request_parameters_.planning_attempts << "," + << " max_velocity_scaling_factor: " << plan_request_parameters_.max_velocity_scaling_factor << "," + << " max_acceleration_scaling_factor: " << plan_request_parameters_.max_acceleration_scaling_factor); } PlanningComponent::PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh) - : nh_(nh), moveit_cpp_(new MoveItCpp(nh)), group_name_(group_name) + : PlanningComponent(group_name, std::make_shared(nh)) { } @@ -138,6 +147,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet ::planning_interface::MotionPlanRequest req; req.group_name = group_name_; req.planner_id = parameters.planner_id; + req.num_planning_attempts = std::max(1, parameters.planning_attempts); req.allowed_planning_time = parameters.planning_time; req.max_velocity_scaling_factor = parameters.max_velocity_scaling_factor; req.max_acceleration_scaling_factor = parameters.max_acceleration_scaling_factor; @@ -197,14 +207,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet PlanningComponent::PlanSolution PlanningComponent::plan() { - PlanRequestParameters default_parameters; - default_parameters.planning_attempts = 1; - default_parameters.planning_time = 5.0; - default_parameters.max_velocity_scaling_factor = 1.0; - default_parameters.max_acceleration_scaling_factor = 1.0; - if (!planning_pipeline_names_.empty()) - default_parameters.planning_pipeline = *planning_pipeline_names_.begin(); - return plan(default_parameters); + return plan(plan_request_parameters_); } bool PlanningComponent::setStartState(const moveit::core::RobotState& start_state) diff --git a/moveit_ros/planning_interface/test/moveit_cpp.yaml b/moveit_ros/planning_interface/test/moveit_cpp.yaml index 910b85c700..6d622662c9 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp.yaml +++ b/moveit_ros/planning_interface/test/moveit_cpp.yaml @@ -11,7 +11,8 @@ planning_pipelines: pipeline_names: - ompl -default_planner_options: +plan_request_params: planning_attempts: 1 + planning_pipeline: ompl max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 \ No newline at end of file From acc94505b23cb28fbf075fea03ea00dc0148b97d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 28 May 2020 09:56:17 +0200 Subject: [PATCH 303/612] Fix getTransform() (#2113) Replace: - PlanningScene::getTransforms().getTransform() -> PlanningScene::getFrameTransform() - PlanningScene::getTransforms().canTransform() -> PlanningScene::knowsFrameTransform() getTransforms() essentially yields the SceneTransforms instance of the parent scene only. Accessing the corresponding frame transforms obviously yields the wrong results. --- .../include/moveit/planning_scene/planning_scene.h | 2 +- moveit_core/planning_scene/src/planning_scene.cpp | 14 +++++++------- .../semantic_world/src/semantic_world.cpp | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 5a776494e3..942cbd3b07 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -1022,7 +1022,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from moveit::core::AttachedBodyCallback current_state_attached_body_callback_; // This variable is not necessarily used by child planning scenes - // This Transforms class is actually a SceneTransform class + // This Transforms class is actually a SceneTransforms class moveit::core::TransformsPtr scene_transforms_; // if NULL use parent's collision_detection::WorldPtr world_; // never NULL, never shared with parent/child diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index bdfd3ed07f..c72decd3b9 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1285,7 +1285,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::Octomap& map) std::shared_ptr om(static_cast(octomap_msgs::msgToMap(map))); if (!map.header.frame_id.empty()) { - const Eigen::Isometry3d& t = getTransforms().getTransform(map.header.frame_id); + const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id); world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t); } else @@ -1321,7 +1321,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose& map) } std::shared_ptr om(static_cast(octomap_msgs::msgToMap(map.octomap))); - const Eigen::Isometry3d& t = getTransforms().getTransform(map.header.frame_id); + const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id); Eigen::Isometry3d p; tf2::fromMsg(map.origin, p); p = t * p; @@ -1493,7 +1493,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache if (object.object.header.frame_id != object.link_name) { const Eigen::Isometry3d& transform = robot_state_->getGlobalLinkTransform(link_model).inverse() * - getTransforms().getTransform(object.object.header.frame_id); + getFrameTransform(object.object.header.frame_id); for (Eigen::Isometry3d& pose : poses) pose = transform * pose; } @@ -1533,7 +1533,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache if (object.object.header.frame_id != object.link_name) { const Eigen::Isometry3d& transform = robot_state_->getGlobalLinkTransform(link_model).inverse() * - getTransforms().getTransform(object.object.header.frame_id); + getFrameTransform(object.object.header.frame_id); for (auto& subframe : subframe_poses) subframe.second = transform * subframe.second; } @@ -1727,7 +1727,7 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject return false; } - if (!getTransforms().canTransform(object.header.frame_id)) + if (!knowsFrameTransform(object.header.frame_id)) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Unknown frame: " << object.header.frame_id); return false; @@ -1737,7 +1737,7 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject if (object.operation == moveit_msgs::CollisionObject::ADD && world_->hasObject(object.id)) world_->removeObject(object.id); - const Eigen::Isometry3d& object_frame_transform = getTransforms().getTransform(object.header.frame_id); + const Eigen::Isometry3d& object_frame_transform = getFrameTransform(object.header.frame_id); for (std::size_t i = 0; i < object.primitives.size(); ++i) { @@ -1808,7 +1808,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::CollisionObjec ROS_WARN_NAMED(LOGNAME, "Move operation for object '%s' ignores the geometry specified in the message.", object.id.c_str()); - const Eigen::Isometry3d& t = getTransforms().getTransform(object.header.frame_id); + const Eigen::Isometry3d& t = getFrameTransform(object.header.frame_id); EigenSTL::vector_Isometry3d new_poses; for (const geometry_msgs::Pose& primitive_pose : object.primitive_poses) { diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp index dec19c206b..f1eb076184 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -485,7 +485,7 @@ void SemanticWorld::transformTableArray(object_recognition_msgs::TableArray& tab ROS_INFO_STREAM_NAMED(LOGNAME, "Original pose: " << table.pose.position.x << "," << table.pose.position.y << "," << table.pose.position.z); std::string error_text; - const Eigen::Isometry3d& original_transform = planning_scene_->getTransforms().getTransform(original_frame); + const Eigen::Isometry3d& original_transform = planning_scene_->getFrameTransform(original_frame); Eigen::Isometry3d original_pose; tf2::fromMsg(table.pose, original_pose); original_pose = original_transform * original_pose; From 81e64b9c8ce5672d770c8b4c21ed3896bd1ea400 Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Fri, 29 May 2020 14:19:43 +0200 Subject: [PATCH 304/612] [CMake] fix link to eigenpy (#2118) fix https://github.com/stack-of-tasks/eigenpy/issues/195 Since https://github.com/ros-planning/moveit/pull/1737, eigenpy is found using find_package, so we can get all required informations to find headers and link to shared objects with a target_link_libraries to the eigenpy::eigenpy imported CMake target. --- moveit_ros/planning_interface/CMakeLists.txt | 1 - .../planning_interface/move_group_interface/CMakeLists.txt | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 8fed49b108..3ba534ecf8 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -86,7 +86,6 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS} include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} - ${eigenpy_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS}) add_subdirectory(py_bindings_tools) diff --git a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt index 31536f08c9..d822174bf0 100644 --- a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt @@ -6,7 +6,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_object add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) add_library(${MOVEIT_LIB_NAME}_python src/wrap_python_move_group.cpp) -target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${eigenpy_LIBRARIES} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) +target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} eigenpy::eigenpy ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) add_dependencies(${MOVEIT_LIB_NAME}_python ${catkin_EXPORTED_TARGETS}) set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_move_group_interface PREFIX "") From 641fe842c9e40c77fba884487a61ef92dbf94b86 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Fri, 29 May 2020 18:56:15 +0300 Subject: [PATCH 305/612] Fix catkin_lint issues (#2120) catkin_lint became a little stricter in checking... --- moveit_ros/visualization/CMakeLists.txt | 1 + moveit_setup_assistant/package.xml | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/moveit_ros/visualization/CMakeLists.txt b/moveit_ros/visualization/CMakeLists.txt index 16b505344c..c1c71a5bbc 100644 --- a/moveit_ros/visualization/CMakeLists.txt +++ b/moveit_ros/visualization/CMakeLists.txt @@ -81,6 +81,7 @@ catkin_package( moveit_ros_robot_interaction object_recognition_msgs roscpp + rviz DEPENDS EIGEN3 ) diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml index 3a8581b32a..b72a15a2cd 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/package.xml @@ -22,18 +22,19 @@ qtbase5-dev libqt5-opengl-dev ompl + rviz moveit_core moveit_ros_planning moveit_ros_visualization rosconsole roscpp - rviz urdf yaml-cpp srdfdom xacro + rviz moveit_resources rosunit From 1518cbf71bf0c25232abba9640307929db5820e2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 31 May 2020 21:01:00 +0200 Subject: [PATCH 306/612] Bump version to upcoming Noetic release: 1.1.0 (#2123) --- moveit/package.xml | 2 +- moveit_commander/package.xml | 2 +- moveit_core/package.xml | 2 +- moveit_experimental/moveit_jog_arm/package.xml | 2 +- moveit_kinematics/package.xml | 2 +- moveit_planners/chomp/chomp_interface/package.xml | 2 +- moveit_planners/chomp/chomp_motion_planner/package.xml | 2 +- moveit_planners/chomp/chomp_optimizer_adapter/package.xml | 2 +- moveit_planners/moveit_planners/package.xml | 2 +- moveit_planners/ompl/package.xml | 2 +- moveit_plugins/moveit_fake_controller_manager/package.xml | 2 +- moveit_plugins/moveit_plugins/package.xml | 2 +- moveit_plugins/moveit_ros_control_interface/package.xml | 2 +- moveit_plugins/moveit_simple_controller_manager/package.xml | 2 +- moveit_ros/benchmarks/package.xml | 2 +- moveit_ros/manipulation/package.xml | 2 +- moveit_ros/move_group/package.xml | 2 +- moveit_ros/moveit_ros/package.xml | 2 +- moveit_ros/occupancy_map_monitor/package.xml | 2 +- moveit_ros/perception/package.xml | 2 +- moveit_ros/planning/package.xml | 2 +- moveit_ros/planning_interface/package.xml | 2 +- moveit_ros/robot_interaction/package.xml | 2 +- moveit_ros/visualization/package.xml | 2 +- moveit_ros/warehouse/package.xml | 2 +- moveit_runtime/package.xml | 2 +- moveit_setup_assistant/package.xml | 2 +- 27 files changed, 27 insertions(+), 27 deletions(-) diff --git a/moveit/package.xml b/moveit/package.xml index 5a50dbd315..ac350c8c1c 100644 --- a/moveit/package.xml +++ b/moveit/package.xml @@ -1,7 +1,7 @@ moveit - 1.0.1 + 1.1.0 Meta package that contains all essential package of MoveIt. Until Summer 2016 MoveIt had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See the detailed discussion for the merge of several repositories. Dave Coleman Michael Ferguson diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml index a962c6a2df..4d58b563ec 100644 --- a/moveit_commander/package.xml +++ b/moveit_commander/package.xml @@ -1,6 +1,6 @@ moveit_commander - 1.0.1 + 1.1.0 Python interfaces to MoveIt Ioan Sucan diff --git a/moveit_core/package.xml b/moveit_core/package.xml index a8256a6e29..26f94f3826 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -1,6 +1,6 @@ moveit_core - 1.0.1 + 1.1.0 Core libraries used by MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_experimental/moveit_jog_arm/package.xml b/moveit_experimental/moveit_jog_arm/package.xml index 931be71bac..0a9858261c 100644 --- a/moveit_experimental/moveit_jog_arm/package.xml +++ b/moveit_experimental/moveit_jog_arm/package.xml @@ -1,7 +1,7 @@ moveit_jog_arm - 1.0.1 + 1.1.0 Provides real-time manipulator Cartesian jogging. diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 64ddccb0b9..561d943079 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -1,7 +1,7 @@ moveit_kinematics - 1.0.1 + 1.1.0 Package for all inverse kinematics solvers in MoveIt Dave Coleman diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml index bc27808754..8a4291483f 100644 --- a/moveit_planners/chomp/chomp_interface/package.xml +++ b/moveit_planners/chomp/chomp_interface/package.xml @@ -2,7 +2,7 @@ moveit_planners_chomp The interface for using CHOMP within MoveIt - 1.0.1 + 1.1.0 Gil Jones Chittaranjan Srinivas Swaminathan diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml index 81c05a1bf6..1473bafc5a 100644 --- a/moveit_planners/chomp/chomp_motion_planner/package.xml +++ b/moveit_planners/chomp/chomp_motion_planner/package.xml @@ -2,7 +2,7 @@ chomp_motion_planner chomp_motion_planner - 1.0.1 + 1.1.0 Gil Jones Mrinal Kalakrishnan diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml index 4861c4e43c..d68ec3800f 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml +++ b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml @@ -3,7 +3,7 @@ moveit_chomp_optimizer_adapter MoveIt planning request adapter utilizing chomp for solution optimization - 1.0.1 + 1.1.0 Raghavender Sahdev Raghavender Sahdev MoveIt Release Team diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index a3ef9a50aa..bc7dc31873 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -1,6 +1,6 @@ moveit_planners - 1.0.1 + 1.1.0 Metapacakge that installs all available planners for MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index f7c87e00f2..1efad63a75 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -1,6 +1,6 @@ moveit_planners_ompl - 1.0.1 + 1.1.0 MoveIt interface to OMPL Ioan Sucan diff --git a/moveit_plugins/moveit_fake_controller_manager/package.xml b/moveit_plugins/moveit_fake_controller_manager/package.xml index d87a4a53bb..3c7dd57332 100644 --- a/moveit_plugins/moveit_fake_controller_manager/package.xml +++ b/moveit_plugins/moveit_fake_controller_manager/package.xml @@ -1,6 +1,6 @@ moveit_fake_controller_manager - 1.0.1 + 1.1.0 A fake controller manager plugin for MoveIt. Ioan Sucan diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml index c64dc69235..f6598bb080 100644 --- a/moveit_plugins/moveit_plugins/package.xml +++ b/moveit_plugins/moveit_plugins/package.xml @@ -1,6 +1,6 @@ moveit_plugins - 1.0.1 + 1.1.0 Metapackage for MoveIt plugins. Michael Ferguson diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml index 92bd877cdc..f3d1626a5c 100644 --- a/moveit_plugins/moveit_ros_control_interface/package.xml +++ b/moveit_plugins/moveit_ros_control_interface/package.xml @@ -1,7 +1,7 @@ moveit_ros_control_interface - 1.0.1 + 1.1.0 ros_control controller manager interface for MoveIt Mathias Lüdtke diff --git a/moveit_plugins/moveit_simple_controller_manager/package.xml b/moveit_plugins/moveit_simple_controller_manager/package.xml index a53dea01fc..af672e7e71 100644 --- a/moveit_plugins/moveit_simple_controller_manager/package.xml +++ b/moveit_plugins/moveit_simple_controller_manager/package.xml @@ -1,6 +1,6 @@ moveit_simple_controller_manager - 1.0.1 + 1.1.0 A generic, simple controller manager plugin for MoveIt. Michael Ferguson diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index acc1789072..d5d3ecb890 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -1,6 +1,6 @@ moveit_ros_benchmarks - 1.0.1 + 1.1.0 Enhanced tools for benchmarks in MoveIt diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml index 6b1c82dddc..49053da42f 100644 --- a/moveit_ros/manipulation/package.xml +++ b/moveit_ros/manipulation/package.xml @@ -1,6 +1,6 @@ moveit_ros_manipulation - 1.0.1 + 1.1.0 Components of MoveIt used for manipulation Ioan Sucan diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index 9c1130b9f1..39fdfe514a 100644 --- a/moveit_ros/move_group/package.xml +++ b/moveit_ros/move_group/package.xml @@ -1,6 +1,6 @@ moveit_ros_move_group - 1.0.1 + 1.1.0 The move_group node for MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index 60671d1f8f..3a2337f53b 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -1,6 +1,6 @@ moveit_ros - 1.0.1 + 1.1.0 Components of MoveIt that use ROS Ioan Sucan Sachin Chitta diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml index 8ff09a2966..f56023495d 100644 --- a/moveit_ros/occupancy_map_monitor/package.xml +++ b/moveit_ros/occupancy_map_monitor/package.xml @@ -1,7 +1,7 @@ moveit_ros_occupancy_map_monitor - 1.0.1 + 1.1.0 Components of MoveIt connecting to occupancy map Ioan Sucan diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml index 0d3a038102..19e72c6bd2 100644 --- a/moveit_ros/perception/package.xml +++ b/moveit_ros/perception/package.xml @@ -1,6 +1,6 @@ moveit_ros_perception - 1.0.1 + 1.1.0 Components of MoveIt connecting to perception Ioan Sucan diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index c5f426ebe1..1cf2bb163f 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -1,7 +1,7 @@ moveit_ros_planning - 1.0.1 + 1.1.0 Planning components of MoveIt that use ROS Ioan Sucan Sachin Chitta diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index 5b9a0eef07..90dc50db0b 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -1,6 +1,6 @@ moveit_ros_planning_interface - 1.0.1 + 1.1.0 Components of MoveIt that offer simpler interfaces to planning and execution Ioan Sucan diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml index 83116910cf..f18f2e4b1e 100644 --- a/moveit_ros/robot_interaction/package.xml +++ b/moveit_ros/robot_interaction/package.xml @@ -1,6 +1,6 @@ moveit_ros_robot_interaction - 1.0.1 + 1.1.0 Components of MoveIt that offer interaction via interactive markers Ioan Sucan diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index a0f4183906..40fcbd843f 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -1,6 +1,6 @@ moveit_ros_visualization - 1.0.1 + 1.1.0 Components of MoveIt that offer visualization Ioan Sucan diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml index 14512ec42f..b815fa139a 100644 --- a/moveit_ros/warehouse/package.xml +++ b/moveit_ros/warehouse/package.xml @@ -1,6 +1,6 @@ moveit_ros_warehouse - 1.0.1 + 1.1.0 Components of MoveIt connecting to MongoDB Ioan Sucan diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml index f331186799..bc8a465c1e 100644 --- a/moveit_runtime/package.xml +++ b/moveit_runtime/package.xml @@ -1,7 +1,7 @@ moveit_runtime - 1.0.1 + 1.1.0 moveit_runtime meta package contains MoveIt packages that are essential for its runtime (e.g. running MoveIt on robots). Isaac I. Y. Saito diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml index b72a15a2cd..9b73230f22 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/package.xml @@ -1,6 +1,6 @@ moveit_setup_assistant - 1.0.1 + 1.1.0 Generates a configuration package that makes it easy to use MoveIt From 4da86c65d3babc8f02c42aa2c6656cbddf295710 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Tue, 2 Jun 2020 09:50:29 -0600 Subject: [PATCH 307/612] Cleanup MoveIt 1 README (#2125) * Cleanup MoveIt 1 README * Update README.md Co-authored-by: Nathan Brooks * Update README.md Co-authored-by: Nathan Brooks * Update README.md Co-authored-by: Nathan Brooks Co-authored-by: Nathan Brooks --- README.md | 35 ++++++++++++++++++----------------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/README.md b/README.md index 0b330cc1bc..f371471aff 100644 --- a/README.md +++ b/README.md @@ -1,40 +1,41 @@ -MoveIt Logo +MoveIt Logo -The MoveIt Motion Planning Framework +**The MoveIt Motion Planning Framework** -Currently we support ROS Kinetic and Melodic for ROS 1. +*Easy-to-use open source robotics manipulation platform for developing commercial applications, prototyping designs, and benchmarking algorithms.* -- [Overview of MoveIt](http://moveit.ros.org) -- [Installation Instructions](http://moveit.ros.org/install/) -- [Documentation](http://moveit.ros.org/documentation/) -- [Get Involved](http://moveit.ros.org/documentation/contributing/) +- [Overview of MoveIt](https://moveit.ros.org) +- [Installation Instructions](https://moveit.ros.org/install/) +- [Documentation](https://moveit.ros.org/documentation/) +- [Get Involved](https://moveit.ros.org/about/get_involved/) ## Branches Policy -We develop all new 1.0 features on ``master``. The ``*-devel`` branches correspond to -released and stable versions of MoveIt for specific distributions of ROS. -Bug fixes occationally get backported to these released versions of MoveIt. -The next version of MoveIt 1.0 will be branched to ``noetic-devel`` around May 2020. +- We develop latest features on ``master``. +- The ``*-devel`` branches correspond to released and stable versions of MoveIt for specific distributions of ROS. +- Bug fixes occationally get backported to these released versions of MoveIt. +- The next version of MoveIt 1.0 will be branched to ``noetic-devel`` around June 2020. +- For MoveIt 2 development, see [moveit2](https://github.com/ros-planning/moveit2). -For MoveIt 2.0 development, see [moveit2](https://github.com/ros-planning/moveit2). +## MoveIt Status -## Continuous Integration Status +### Continuous Integration service | Kinetic | Melodic | Master ---------- | ------- | ------- | ------ Travis | [![Build Status](https://travis-ci.org/ros-planning/moveit.svg?branch=kinetic-devel)](https://travis-ci.org/ros-planning/moveit/branches) | [![Build Status](https://travis-ci.org/ros-planning/moveit.svg?branch=melodic-devel)](https://travis-ci.org/ros-planning/moveit/branches) | [![Build Status](https://travis-ci.org/ros-planning/moveit.svg?branch=master)](https://travis-ci.org/ros-planning/moveit/branches) | build farm | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__moveit__ubuntu_xenial_amd64)](http://build.ros.org/job/Kdev__moveit__ubuntu_xenial_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__moveit__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__moveit__ubuntu_bionic_amd64) | N/A | -## Licenses +### Licenses [![FOSSA Status](https://app.fossa.com/api/projects/git%2Bgithub.com%2Fros-planning%2Fmoveit.svg?type=shield)](https://app.fossa.com/projects/git%2Bgithub.com%2Fros-planning%2Fmoveit?ref=badge_shield) -## Docker Containers +### Docker Containers [![Docker Build](https://img.shields.io/docker/build/moveit/moveit.svg)](https://hub.docker.com/r/moveit/moveit/builds) [![Docker Automated build](https://img.shields.io/docker/automated/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit/) [![Docker Pulls](https://img.shields.io/docker/pulls/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit/) [![Docker Stars](https://img.shields.io/docker/stars/moveit/moveit.svg)](https://registry.hub.docker.com/moveit/moveit/) -## ROS Buildfarm +### ROS Buildfarm MoveIt Package | Kinetic Source | Kinetic Debian | Melodic Source | Melodic Debian --------------- | -------------- | -------------- | -------------- | -------------- @@ -68,4 +69,4 @@ moveit_simple_controller_manager | [![Build Status](http://build.ros.org/buildSt moveit_visual_tools | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_visual_tools__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_visual_tools__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary) chomp_motion_planner | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__chomp_motion_planner__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__chomp_motion_planner__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary) geometric_shapes | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__geometric_shapes__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__geometric_shapes__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__geometric_shapes__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__geometric_shapes__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary) -srdfdom | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__srdfdom__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__srdfdom__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__srdfdom__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__srdfdom__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary) \ No newline at end of file +srdfdom | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__srdfdom__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__srdfdom__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__srdfdom__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__srdfdom__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary) From 1ae43180a09b53252cf952a7ac83c87bd6097f74 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Tue, 2 Jun 2020 10:00:15 -0600 Subject: [PATCH 308/612] Remove support for Indigo's ros_control (#2128) Code states "This can be removed upon EOL Indigo", which MoveIt has EOL'd --- .../CMakeLists.txt | 5 ---- .../src/controller_manager_plugin.cpp | 27 ------------------- 2 files changed, 32 deletions(-) diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt index 90a00ea567..00988dcb65 100644 --- a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt +++ b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt @@ -29,11 +29,6 @@ catkin_package( DEPENDS Boost ) -# support indigo's ros_control - This can be removed upon EOL indigo -if("${controller_manager_msgs_VERSION}" VERSION_LESS "0.10.0") - add_definitions(-DMOVEIT_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) -endif() - include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) add_library(${PROJECT_NAME}_plugin diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp index 2a6e1ce8d8..e1e0232e5d 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp @@ -158,9 +158,6 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll } std::vector resources; -#if defined(MOVEIT_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) - resources = controller.resources; -#else // Collect claimed resources across different hardware interfaces for (const controller_manager_msgs::HardwareInterfaceResources& claimed_resource : controller.claimed_resources) { @@ -169,7 +166,6 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll resources.push_back(resource); } } -#endif moveit_controller_manager::MoveItControllerHandlePtr handle = alloc_it->second->alloc(name, resources); // allocate handle @@ -268,15 +264,11 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll ControllersMap::iterator it = managed_controllers_.find(name); if (it != managed_controllers_.end()) { -#if defined(MOVEIT_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) - joints = it->second.resources; -#else for (controller_manager_msgs::HardwareInterfaceResources& claimed_resource : it->second.claimed_resources) { std::vector& resources = claimed_resource.resources; joints.insert(joints.end(), resources.begin(), resources.end()); } -#endif } } @@ -319,12 +311,6 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll for (std::pair& active_controller : active_controllers_) { -#if defined(MOVEIT_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) - for (std::vector::iterator r = c->second.resources.begin(); r != c->second.resources.end(); ++r) - { - claimed_resources.insert(resources_bimap::value_type(c->second.name, *r)); - } -#else for (std::vector::iterator hir = active_controller.second.claimed_resources.begin(); hir != active_controller.second.claimed_resources.end(); ++hir) @@ -334,7 +320,6 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll claimed_resources.insert(resources_bimap::value_type(active_controller.second.name, resource)); } } -#endif } controller_manager_msgs::SwitchController srv; @@ -355,17 +340,6 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll if (c != managed_controllers_.end()) { // controller belongs to this manager srv.request.start_controllers.push_back(c->second.name); -#if defined(MOVEIT_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) - for (std::vector::iterator r = c->second.resources.begin(); r != c->second.resources.end(); ++r) - { // for all claimed resource - resources_bimap::right_const_iterator res = claimed_resources.right.find(*r); - if (res != claimed_resources.right.end()) - { // resource is claimed - srv.request.stop_controllers.push_back(res->second); // add claiming controller to stop list - claimed_resources.left.erase(res->second); // remove claimed resources - } - } -#else for (controller_manager_msgs::HardwareInterfaceResources& claimed_resource : c->second.claimed_resources) { for (const std::string& resource : claimed_resource.resources) @@ -378,7 +352,6 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll } } } -#endif } } srv.request.strictness = srv.request.STRICT; From 725a1f411a16b9703e034273faba40188002c11a Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Tue, 2 Jun 2020 12:38:24 -0600 Subject: [PATCH 309/612] Add temporary rosinstall for building Noetic (#2134) Includes all repos that are not yet available from Debian --- moveit_noetic.rosinstall | 75 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 moveit_noetic.rosinstall diff --git a/moveit_noetic.rosinstall b/moveit_noetic.rosinstall new file mode 100644 index 0000000000..532928a195 --- /dev/null +++ b/moveit_noetic.rosinstall @@ -0,0 +1,75 @@ +################################# +# Temporary Noetic ROS Install +# Remove all the extra source builds once they are released +################################# + +# This file is intended for users who want to build MoveIt from source. +# Used with wstool, users can download source of all packages of MoveIt. +- git: + local-name: moveit_msgs + uri: https://github.com/ros-planning/moveit_msgs.git + version: master +- git: + local-name: geometric_shapes + uri: https://github.com/ros-planning/geometric_shapes.git + version: noetic-devel +- git: + local-name: moveit + uri: https://github.com/ros-planning/moveit.git + version: master +- git: + local-name: rviz_visual_tools + uri: https://github.com/PickNikRobotics/rviz_visual_tools + version: master +- git: + local-name: moveit_visual_tools + uri: https://github.com/ros-planning/moveit_visual_tools.git + version: master +- git: + local-name: moveit_tutorials + uri: https://github.com/ros-planning/moveit_tutorials.git + version: master +- git: + local-name: panda_moveit_config + uri: https://github.com/ros-planning/panda_moveit_config.git + version: melodic-devel + +#################################### +# Remove the following once released +#################################### +- git: + local-name: rosparam_shortcuts + uri: https://github.com/PickNikRobotics/rosparam_shortcuts.git + version: melodic-devel +- git: + local-name: srdfdom + uri: https://github.com/ros-planning/srdfdom.git + version: melodic-devel +- git: + local-name: ompl + uri: https://github.com/ompl/ompl.git + version: master +- git: + local-name: ros_pytest + uri: https://github.com/machinekoder/ros_pytest.git + version: melodic-devel +- git: + local-name: joystick_drivers + uri: https://github.com/ros-drivers/joystick_drivers.git + version: master +- git: + local-name: teleop_tools + uri: https://github.com/ros-teleop/teleop_tools.git + version: kinetic-devel +- git: + local-name: moveit_resources + uri: https://github.com/ros-planning/moveit_resources.git + version: master +- git: + local-name: franka_ros + uri: https://github.com/frankaemika/franka_ros.git + version: kinetic-devel +- git: + local-name: graph_msgs + uri: https://github.com/davetcoleman/graph_msgs.git + version: jade-devel \ No newline at end of file From c6cc5669c3392c9ed6c426857c1a7db6e70a1ac2 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Tue, 2 Jun 2020 16:45:15 -0600 Subject: [PATCH 310/612] Add Noetic ROS Build Badges to README (#2136) * Update README * Add ROS Buildfarm badges for Noetic --- README.md | 75 +++++++++++++++------------ moveit/scripts/README.md | 12 ++++- moveit/scripts/create_readme_table.py | 7 +-- 3 files changed, 56 insertions(+), 38 deletions(-) diff --git a/README.md b/README.md index f371471aff..f10d46ecfd 100644 --- a/README.md +++ b/README.md @@ -37,36 +37,45 @@ build farm | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__mo ### ROS Buildfarm -MoveIt Package | Kinetic Source | Kinetic Debian | Melodic Source | Melodic Debian ---------------- | -------------- | -------------- | -------------- | -------------- -moveit | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit__ubuntu_bionic_amd64__binary) -moveit_chomp_optimizer_adapter | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary) -moveit_commander | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_commander__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_commander__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_commander__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_commander__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary) -moveit_core | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_core__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_core__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_core__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_core__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary) -moveit_experimental | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_experimental__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_experimental__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_experimental__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_experimental__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_experimental__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_experimental__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_experimental__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_experimental__ubuntu_bionic_amd64__binary) -moveit_fake_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary) -moveit_kinematics | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_kinematics__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_kinematics__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_kinematics__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_kinematics__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary) -moveit_msgs | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_msgs__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_msgs__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_msgs__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_msgs__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_msgs__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_msgs__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary) -moveit_planners | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary) -moveit_planners_chomp | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary) -moveit_planners_ompl | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary) -moveit_plugins | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_plugins__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_plugins__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_plugins__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_plugins__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary) -moveit_resources | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_resources__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_resources__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_resources__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_resources__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_resources__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_resources__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_resources__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_resources__ubuntu_bionic_amd64__binary) -moveit_ros | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary) -moveit_ros_benchmarks | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary) -moveit_ros_control_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary) -moveit_ros_manipulation | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary) -moveit_ros_move_group | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary) -moveit_ros_perception | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_perception__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_perception__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary) -moveit_ros_planning | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary) -moveit_ros_planning_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary) -moveit_ros_robot_interaction | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary) -moveit_ros_visualization | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary) -moveit_ros_warehouse | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary) -moveit_runtime | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_runtime__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_runtime__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_runtime__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_runtime__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary) -moveit_setup_assistant | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary) -moveit_simple_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary) -moveit_visual_tools | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_visual_tools__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_visual_tools__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary) -chomp_motion_planner | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__chomp_motion_planner__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__chomp_motion_planner__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary) -geometric_shapes | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__geometric_shapes__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__geometric_shapes__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__geometric_shapes__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__geometric_shapes__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary) -srdfdom | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__srdfdom__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__srdfdom__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__srdfdom__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__srdfdom__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary) +MoveIt Package | Kinetic Source | Kinetic Debian | Melodic Source | Melodic Debian | Noetic Source | Noetic Debian +-------------- | -------------- | -------------- | -------------- | -------------- | ------------- | ------------- +moveit | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit__ubuntu_focal_amd64__binary) +moveit_chomp_optimizer_adapter | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_chomp_optimizer_adapter__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_chomp_optimizer_adapter__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_chomp_optimizer_adapter__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_chomp_optimizer_adapter__ubuntu_focal_amd64__binary) +moveit_commander | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_commander__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_commander__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_commander__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_commander__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_commander__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_commander__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_commander__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_commander__ubuntu_focal_amd64__binary) +moveit_core | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_core__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_core__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_core__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_core__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_core__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_core__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_core__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_core__ubuntu_focal_amd64__binary) +moveit_fake_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_fake_controller_manager__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_fake_controller_manager__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_fake_controller_manager__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_fake_controller_manager__ubuntu_focal_amd64__binary) +moveit_grasps | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_grasps__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_grasps__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_grasps__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_grasps__ubuntu_focal_amd64__binary) +moveit_jog_arm | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_jog_arm__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_jog_arm__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_jog_arm__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_jog_arm__ubuntu_focal_amd64__binary) +moveit_kinematics | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_kinematics__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_kinematics__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_kinematics__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_kinematics__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_kinematics__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_kinematics__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_kinematics__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_kinematics__ubuntu_focal_amd64__binary) +moveit_msgs | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_msgs__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_msgs__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_msgs__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_msgs__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_msgs__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_msgs__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_msgs__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_msgs__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_msgs__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_msgs__ubuntu_focal_amd64__binary) +moveit_planners | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners__ubuntu_focal_amd64__binary) +moveit_planners_chomp | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners_chomp__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners_chomp__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners_chomp__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners_chomp__ubuntu_focal_amd64__binary) +moveit_planners_ompl | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners_ompl__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners_ompl__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners_ompl__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners_ompl__ubuntu_focal_amd64__binary) +moveit_plugins | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_plugins__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_plugins__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_plugins__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_plugins__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_plugins__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_plugins__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_plugins__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_plugins__ubuntu_focal_amd64__binary) +moveit_resources | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_resources__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_resources__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_resources__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_resources__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_resources__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_resources__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_resources__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_resources__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_resources__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_resources__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_resources__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_resources__ubuntu_focal_amd64__binary) +moveit_ros | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros__ubuntu_focal_amd64__binary) +moveit_ros_benchmarks | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_benchmarks__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_benchmarks__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_benchmarks__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_benchmarks__ubuntu_focal_amd64__binary) +moveit_ros_control_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_control_interface__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_control_interface__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_control_interface__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_control_interface__ubuntu_focal_amd64__binary) +moveit_ros_manipulation | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_manipulation__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_manipulation__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_manipulation__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_manipulation__ubuntu_focal_amd64__binary) +moveit_ros_move_group | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_move_group__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_move_group__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_move_group__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_move_group__ubuntu_focal_amd64__binary) +moveit_ros_occupancy_map_monitor | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_occupancy_map_monitor__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_occupancy_map_monitor__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_occupancy_map_monitor__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_occupancy_map_monitor__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_occupancy_map_monitor__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_occupancy_map_monitor__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_occupancy_map_monitor__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_occupancy_map_monitor__ubuntu_focal_amd64__binary) +moveit_ros_perception | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_perception__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_perception__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_perception__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_perception__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_perception__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_perception__ubuntu_focal_amd64__binary) +moveit_ros_planning | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_planning__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_planning__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_planning__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_planning__ubuntu_focal_amd64__binary) +moveit_ros_planning_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_planning_interface__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_planning_interface__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_planning_interface__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_planning_interface__ubuntu_focal_amd64__binary) +moveit_ros_robot_interaction | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_robot_interaction__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_robot_interaction__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_robot_interaction__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_robot_interaction__ubuntu_focal_amd64__binary) +moveit_ros_visualization | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_visualization__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_visualization__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_visualization__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_visualization__ubuntu_focal_amd64__binary) +moveit_ros_warehouse | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_warehouse__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_warehouse__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_warehouse__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_warehouse__ubuntu_focal_amd64__binary) +moveit_runtime | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_runtime__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_runtime__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_runtime__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_runtime__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_runtime__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_runtime__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary) +moveit_setup_assistant | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_setup_assistant__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_setup_assistant__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_setup_assistant__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_setup_assistant__ubuntu_focal_amd64__binary) +moveit_simple_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary) +moveit_task_constructor_capabilities | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_capabilities__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_capabilities__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_capabilities__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_capabilities__ubuntu_focal_amd64__binary) +moveit_task_constructor_core | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_core__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_core__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_core__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_core__ubuntu_focal_amd64__binary) +moveit_task_constructor_demo | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_demo__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_demo__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_demo__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_demo__ubuntu_focal_amd64__binary) +moveit_task_constructor_msgs | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_msgs__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_msgs__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_msgs__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_msgs__ubuntu_focal_amd64__binary) +moveit_task_constructor_visualization | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_visualization__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_visualization__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_visualization__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_visualization__ubuntu_focal_amd64__binary) +moveit_visual_tools | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_visual_tools__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_visual_tools__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_visual_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_visual_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_visual_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_visual_tools__ubuntu_focal_amd64__binary) +chomp_motion_planner | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__chomp_motion_planner__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__chomp_motion_planner__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__chomp_motion_planner__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__chomp_motion_planner__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__chomp_motion_planner__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__chomp_motion_planner__ubuntu_focal_amd64__binary) +geometric_shapes | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__geometric_shapes__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__geometric_shapes__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__geometric_shapes__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__geometric_shapes__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__geometric_shapes__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__geometric_shapes__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary) +rviz_marker_tools | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__rviz_marker_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__rviz_marker_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__rviz_marker_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__rviz_marker_tools__ubuntu_focal_amd64__binary) +rviz_visual_tools | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__rviz_visual_tools__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__rviz_visual_tools__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__rviz_visual_tools__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__rviz_visual_tools__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__rviz_visual_tools__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__rviz_visual_tools__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__rviz_visual_tools__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__rviz_visual_tools__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__rviz_visual_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__rviz_visual_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__rviz_visual_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__rviz_visual_tools__ubuntu_focal_amd64__binary) +srdfdom | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__srdfdom__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__srdfdom__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__srdfdom__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__srdfdom__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__srdfdom__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__srdfdom__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary) diff --git a/moveit/scripts/README.md b/moveit/scripts/README.md index e2da938586..e6fde97ef7 100644 --- a/moveit/scripts/README.md +++ b/moveit/scripts/README.md @@ -2,14 +2,22 @@ ## README Markdown Buildfarm Table Generator -This Python script will generate Github-style Markdown table with Jenkins badges for every package recursively from the current folder. It will order all packages alphabetically, but start with all MoveIt packages first. For generating an updated README for a new ROS distribution, first update the `create_readme_table.py` script and add/remove the current MoveIt-supported ROS distros and their corresponding Ubuntu distribution in the specified Python dictionary. Then, in the main MoveIt `README.md` file, remove the "## ROS Buildfarm" header and the following table. Finally, from your catkin workspace with every MoveIt package, run the `create_readme_table.py` script. For example: +This Python script will generate Github-style Markdown table with Jenkins badges for every package recursively from the current folder. +It will order all packages alphabetically, but start with all MoveIt packages first. +For generating an updated README for a new ROS distribution: +First update the `create_readme_table.py` script and add/remove the current MoveIt-supported ROS distros and their corresponding Ubuntu distribution in the specified Python dictionary. +Then, in the main MoveIt `README.md` file, remove the "## ROS Buildfarm" header and the following table. +Finally, from your catkin workspace with every MoveIt package, run the `create_readme_table.py` script. +For example: cd ~/ws_moveit/src python moveit/moveit/scripts/create_readme_table.py >> moveit/README.md ## HTML Table Generator -This python script will generate an html page with useful data about every package in the current folder the script is run in. Therefore, to summarize moveit, run within a catkin workspace. For example: +This python script will generate an html page with useful data about every package in the current folder the script is run in. +Therefore, to summarize moveit, run within a catkin workspace. +For example: cd ~/ws_moveit/src python moveit/moveit/scripts/create_maintainer_table.py diff --git a/moveit/scripts/create_readme_table.py b/moveit/scripts/create_readme_table.py index 9cbaed3e52..0b52973892 100644 --- a/moveit/scripts/create_readme_table.py +++ b/moveit/scripts/create_readme_table.py @@ -10,7 +10,7 @@ def create_header(ros_ubuntu_dict): ros_distros = sorted(ros_ubuntu_dict.keys()) - section_header = "## ROS Buildfarm\n" + section_header = "### ROS Buildfarm\n" header="MoveIt Package" header_lines = '-'*len(header) for ros in ros_distros: @@ -39,7 +39,8 @@ def create_line(package, ros_ubuntu_dict): for target in ['src', 'bin']: define_urls(target, params) response = requests.get(params['url']).status_code - if response < 400: # success + # we want to show a particular OS's badges to indicate they are not released / working yet + if response < 400 or ubuntu == "focal": # success line += ' | [![Build Status]({base_url}/buildStatus/icon?job={job})]({url})'.format(**params) else: # error line += ' | ' @@ -55,7 +56,7 @@ def create_moveit_buildfarm_table(): # combinations for supported distribitions. For instance, in Noetic, # remove {"indigo":"trusty"} and add {"noetic":"fbuntu"} with "fbuntu" # being whatever the 20.04 distro is named - supported_distro_ubuntu_dict = {"kinetic":"xenial", "melodic":"bionic"} + supported_distro_ubuntu_dict = {"kinetic":"xenial", "melodic":"bionic", "noetic":"focal"} all_packages = sorted([package.name for _, package in find_packages(os.getcwd()).items()]) moveit_packages = list() From 9bdce08b2eab287021835d3989d073019e5c4709 Mon Sep 17 00:00:00 2001 From: Jere Liukkonen Date: Wed, 3 Jun 2020 03:03:57 -0600 Subject: [PATCH 311/612] Fix memory leaks related to geometric shapes usage (#2138) * Fix memory leak with a shape object when malformed scene geometry file is loaded. * Fix memory leak by removing unnecessary and leaky call to clone() --- .../src/collision_distance_field_types.cpp | 2 +- moveit_core/planning_scene/src/planning_scene.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp index 72b49f7696..f9be5ccc4a 100644 --- a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp +++ b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp @@ -280,7 +280,7 @@ void collision_detection::BodyDecomposition::init(const std::vectorclone(), poses[i], padding); + bodies_.addBody(shapes[i].get(), poses[i], padding); } // collecting collision spheres diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index c72decd3b9..7d5dec02e9 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1018,8 +1018,8 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet in >> shape_count; for (std::size_t i = 0; i < shape_count && in.good() && !in.eof(); ++i) { - shapes::Shape* s = shapes::constructShapeFromText(in); - if (!s) + const auto shape = shapes::ShapeConstPtr(shapes::constructShapeFromText(in)); + if (!shape) { ROS_ERROR_NAMED(LOGNAME, "Failed to load shape from scene file"); return false; @@ -1041,12 +1041,12 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet ROS_ERROR_NAMED(LOGNAME, "Improperly formatted color in scene geometry file"); return false; } - if (s) + if (shape) { Eigen::Isometry3d pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz); // Transform pose by input pose offset pose = offset * pose; - world_->addToObject(ns, shapes::ShapePtr(s), pose); + world_->addToObject(ns, shape, pose); if (r > 0.0f || g > 0.0f || b > 0.0f || a > 0.0f) { std_msgs::ColorRGBA color; From edb0b0396a644ac9d22a6b8e3802b5217a57075a Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 3 Jun 2020 08:30:49 -0600 Subject: [PATCH 312/612] Switch README badges to travis-ci.com (#2139) --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index f10d46ecfd..18a0d11020 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,7 @@ service | Kinetic | Melodic | Master ---------- | ------- | ------- | ------ -Travis | [![Build Status](https://travis-ci.org/ros-planning/moveit.svg?branch=kinetic-devel)](https://travis-ci.org/ros-planning/moveit/branches) | [![Build Status](https://travis-ci.org/ros-planning/moveit.svg?branch=melodic-devel)](https://travis-ci.org/ros-planning/moveit/branches) | [![Build Status](https://travis-ci.org/ros-planning/moveit.svg?branch=master)](https://travis-ci.org/ros-planning/moveit/branches) | +Travis | [![Build Status](https://travis-ci.com/ros-planning/moveit.svg?branch=kinetic-devel)](https://travis-ci.com/ros-planning/moveit/branches) | [![Build Status](https://travis-ci.com/ros-planning/moveit.svg?branch=melodic-devel)](https://travis-ci.com/ros-planning/moveit/branches) | [![Build Status](https://travis-ci.com/ros-planning/moveit.svg?branch=master)](https://travis-ci.com/ros-planning/moveit/branches) | build farm | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__moveit__ubuntu_xenial_amd64)](http://build.ros.org/job/Kdev__moveit__ubuntu_xenial_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__moveit__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__moveit__ubuntu_bionic_amd64) | N/A | ### Licenses From 85ff93bd8724a5d8e15406c9a96680bff5858eca Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 3 Jun 2020 14:08:35 -0500 Subject: [PATCH 313/612] [jog_arm] time-based collision avoidance (#2100) --- .../config/ur_simulated_config.yaml | 10 +- .../moveit_jog_arm/collision_check_thread.h | 8 ++ .../include/moveit_jog_arm/jog_arm_data.h | 20 ++-- .../src/collision_check_thread.cpp | 89 +++++++++++++----- .../moveit_jog_arm/src/jog_calcs.cpp | 43 ++++++++- .../moveit_jog_arm/src/jog_interface_base.cpp | 93 ++++++++++++------- .../test/config/jog_settings.yaml | 8 ++ 7 files changed, 206 insertions(+), 65 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml index 8a42b955dd..74e896cfc9 100644 --- a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml +++ b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml @@ -52,6 +52,14 @@ command_out_topic: joint_group_position_controller/command # Publish outgoing co ## Collision checking for the entire robot body check_collisions: true # Check collisions? -collision_check_rate: 10 # [Hz] Collision-checking can easily bog down a CPU if done too often. +collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: stop_distance +# Parameters for "threshold_distance"-type collision checking self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index 8fbf5f2d50..447e945808 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -46,6 +46,12 @@ namespace moveit_jog_arm { +enum CollisionCheckType +{ + K_THRESHOLD_DISTANCE = 1, + K_STOP_DISTANCE = 2 +}; + class CollisionCheckThread { public: @@ -65,6 +71,8 @@ class CollisionCheckThread private: const moveit_jog_arm::JogArmParameters parameters_; + CollisionCheckType collision_check_type_; + // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; }; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 220902b224..4a4fb7b91d 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -56,7 +56,7 @@ namespace moveit_jog_arm { -// Variables to share between threads, and their mutexes +// Variables to share between threads // Inherit from a mutex so the struct can be locked/unlocked easily struct JogArmShared : public std::mutex { @@ -66,7 +66,11 @@ struct JogArmShared : public std::mutex sensor_msgs::JointState joints; - double collision_velocity_scale = 1; + // The collision check thread throttles robot velocity via this variable + std::atomic collision_velocity_scale{ 1 }; + + // The jog thread communicates the minimum stopping time to the collision check thread via this variable + std::atomic worst_case_stop_time{ std::numeric_limits::max() }; // Flag a valid incoming Cartesian command having nonzero velocities bool have_nonzero_cartesian_cmd = false; @@ -125,18 +129,22 @@ struct JogArmParameters double joint_scale; double lower_singularity_threshold; double hard_stop_singularity_threshold; - double scene_collision_proximity_threshold; - double self_collision_proximity_threshold; double low_pass_filter_coeff; double publish_period; double incoming_command_timeout; double joint_limit_margin; - double collision_check_rate; int num_outgoing_halt_msgs_to_publish; bool use_gazebo; - bool check_collisions; bool publish_joint_positions; bool publish_joint_velocities; bool publish_joint_accelerations; + // Collision checking + bool check_collisions; + std::string collision_check_type; + double collision_check_rate; + double scene_collision_proximity_threshold; + double self_collision_proximity_threshold; + double collision_distance_safety_factor; + double min_allowable_collision_distance; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index e2425be5d6..c3d47fdcc2 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -40,7 +40,8 @@ #include static const std::string LOGNAME = "collision_check_thread"; -static const double MIN_RECOMMENDED_COLLISION_RATE = 10; +static constexpr double MIN_RECOMMENDED_COLLISION_RATE = 10; +constexpr double EPSILON = 1e-6; // For very small numeric comparisons namespace moveit_jog_arm { @@ -78,11 +79,19 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) double scene_collision_distance = 0; bool collision_detected; - // Scale robot velocity according to collision proximity and user-defined thresholds. - // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. - // Proximity decreasing --> decelerate + // This variable scales the robot velocity when a collision is close double velocity_scale = 1; + collision_check_type_ = + (parameters_.collision_check_type == "threshold_distance" ? K_THRESHOLD_DISTANCE : K_STOP_DISTANCE); + + // Variables for stop-distance-based collision checking + double current_collision_distance = 0; + double derivative_of_collision_distance = 0; + double prev_collision_distance = 0; + double est_time_to_collision = 0; + double safety_factor = parameters_.collision_distance_safety_factor; + collision_detection::AllowedCollisionMatrix acm = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); ///////////////////////////////////////////////// // Spin while checking collisions @@ -115,36 +124,70 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) collision_detected |= collision_result.collision; velocity_scale = 1; + // If we're definitely in collision, stop immediately if (collision_detected) { velocity_scale = 0; } - - // If we are far from a collision, velocity_scale should be 1. - // If we are very close to a collision, velocity_scale should be ~zero. - // When scene_collision_proximity_threshold is breached, start decelerating exponentially. - if (scene_collision_distance < parameters_.scene_collision_proximity_threshold) + // If threshold distances were specified + else if (collision_check_type_ == K_THRESHOLD_DISTANCE) { - // velocity_scale = e ^ k * (collision_distance - threshold) - // k = - ln(0.001) / collision_proximity_threshold - // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. - // velocity_scale should equal 0.001 when collision_distance is at zero. - velocity_scale = - std::min(velocity_scale, exp(scene_velocity_scale_coefficient * - (scene_collision_distance - parameters_.scene_collision_proximity_threshold))); + // If we are far from a collision, velocity_scale should be 1. + // If we are very close to a collision, velocity_scale should be ~zero. + // When scene_collision_proximity_threshold is breached, start decelerating exponentially. + if (scene_collision_distance < parameters_.scene_collision_proximity_threshold) + { + // velocity_scale = e ^ k * (collision_distance - threshold) + // k = - ln(0.001) / collision_proximity_threshold + // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. + // velocity_scale should equal 0.001 when collision_distance is at zero. + velocity_scale = std::min(velocity_scale, + exp(scene_velocity_scale_coefficient * + (scene_collision_distance - parameters_.scene_collision_proximity_threshold))); + } + + if (self_collision_distance < parameters_.self_collision_proximity_threshold) + { + velocity_scale = + std::min(velocity_scale, exp(self_velocity_scale_coefficient * + (self_collision_distance - parameters_.self_collision_proximity_threshold))); + } } - - if (self_collision_distance < parameters_.self_collision_proximity_threshold) + // Else, we stop based on worst-case stopping distance + else { - velocity_scale = - std::min(velocity_scale, exp(self_velocity_scale_coefficient * - (self_collision_distance - parameters_.self_collision_proximity_threshold))); + // Retrieve the worst-case time to stop, based on current joint velocities + + // Calculate rate of change of distance to nearest collision + current_collision_distance = std::min(scene_collision_distance, self_collision_distance); + derivative_of_collision_distance = + (current_collision_distance - prev_collision_distance) / collision_rate.expectedCycleTime().toSec(); + + if (current_collision_distance < parameters_.min_allowable_collision_distance && + derivative_of_collision_distance <= 0) + { + velocity_scale = 0; + } + // Only bother doing calculations if we are moving toward the nearest collision + else if (derivative_of_collision_distance < -EPSILON) + { + // At the rate the collision distance is decreasing, how long until we collide? + est_time_to_collision = fabs(current_collision_distance / derivative_of_collision_distance); + + // halt if we can't stop fast enough (including the safety factor) + if (est_time_to_collision < (safety_factor * shared_variables.worst_case_stop_time)) + { + velocity_scale = 0; + } + } + + // Update for the next iteration + prev_collision_distance = current_collision_distance; } - shared_variables.lock(); + // Communicate a velocity-scaling factor back to the other threads shared_variables.collision_velocity_scale = velocity_scale; - shared_variables.unlock(); } collision_rate.sleep(); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index f08a9473bb..22ccdeffd0 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -477,9 +477,7 @@ trajectory_msgs::JointTrajectory JogCalcs::composeJointTrajMessage(sensor_msgs:: void JogCalcs::applyVelocityScaling(JogArmShared& shared_variables, Eigen::ArrayXd& delta_theta, double singularity_scale) { - shared_variables.lock(); double collision_scale = shared_variables.collision_velocity_scale; - shared_variables.unlock(); if (collision_scale > 0 && collision_scale < 1) { @@ -743,6 +741,47 @@ bool JogCalcs::updateJoints(JogArmShared& shared_variables) // Cache the original joints in case they need to be reset original_joint_state_ = internal_joint_state_; + // Calculate worst case joint stop time, for collision checking + std::string joint_name = ""; + shared_variables.worst_case_stop_time = std::numeric_limits::max(); + moveit::core::JointModel::Bounds kinematic_bounds; + double accel_limit = 0; + double joint_velocity = 0; + double worst_case_stop_time = 0; + for (size_t jt_state_idx = 0; jt_state_idx < incoming_joint_state_.velocity.size(); ++jt_state_idx) + { + joint_name = incoming_joint_state_.name[jt_state_idx]; + + // Get acceleration limit for this joint + for (auto joint_model : joint_model_group_->getActiveJointModels()) + { + if (joint_model->getName() == joint_name) + { + kinematic_bounds = joint_model->getVariableBounds(); + // Some joints do not have acceleration limits + if (kinematic_bounds[0].acceleration_bounded_) + { + // Be conservative when calculating overall acceleration limit from min and max limits + accel_limit = + std::min(fabs(kinematic_bounds[0].min_acceleration_), fabs(kinematic_bounds[0].max_acceleration_)); + } + else + { + ROS_WARN_STREAM_NAMED(LOGNAME, "An acceleration limit is not defined for this joint; minimum stop distance " + "should not be used for collision checking"); + } + break; + } + } + + // Get the current joint velocity + joint_velocity = incoming_joint_state_.velocity[jt_state_idx]; + + // Calculate worst case stop time + worst_case_stop_time = std::max(worst_case_stop_time, fabs(joint_velocity / accel_limit)); + } + shared_variables.worst_case_stop_time = worst_case_stop_time; + return true; } diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index 3cd342a100..3234d70853 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -88,15 +88,32 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) ros_parameters_.lower_singularity_threshold); error += !rosparam_shortcuts::get("", n, parameter_ns + "/hard_stop_singularity_threshold", ros_parameters_.hard_stop_singularity_threshold); - // parameter was removed, replaced with separate self- and scene-collision proximity thresholds; the logic handling - // the different possible sets of defined parameters is somewhat complicated at this point - // TODO(JStech): remove this deprecation warning in ROS Noetic; simplify error case handling + error += !rosparam_shortcuts::get("", n, parameter_ns + "/move_group_name", ros_parameters_.move_group_name); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/planning_frame", ros_parameters_.planning_frame); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/use_gazebo", ros_parameters_.use_gazebo); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/joint_limit_margin", ros_parameters_.joint_limit_margin); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_out_topic", ros_parameters_.command_out_topic); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_out_type", ros_parameters_.command_out_type); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_positions", + ros_parameters_.publish_joint_positions); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_velocities", + ros_parameters_.publish_joint_velocities); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_accelerations", + ros_parameters_.publish_joint_accelerations); + + // Parameters for collision checking + error += !rosparam_shortcuts::get("", n, parameter_ns + "/check_collisions", ros_parameters_.check_collisions); + error += + !rosparam_shortcuts::get("", n, parameter_ns + "/collision_check_type", ros_parameters_.collision_check_type); bool have_self_collision_proximity_threshold = rosparam_shortcuts::get( "", n, parameter_ns + "/self_collision_proximity_threshold", ros_parameters_.self_collision_proximity_threshold); bool have_scene_collision_proximity_threshold = rosparam_shortcuts::get("", n, parameter_ns + "/scene_collision_proximity_threshold", ros_parameters_.scene_collision_proximity_threshold); double collision_proximity_threshold; + // 'collision_proximity_threshold' parameter was removed, replaced with separate self- and scene-collision proximity + // thresholds + // TODO(JStech): remove this deprecation warning in ROS Noetic; simplify error case handling if (n.hasParam(parameter_ns + "/collision_proximity_threshold") && rosparam_shortcuts::get("", n, parameter_ns + "/collision_proximity_threshold", collision_proximity_threshold)) { @@ -116,19 +133,10 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) } error += !have_self_collision_proximity_threshold; error += !have_scene_collision_proximity_threshold; - error += !rosparam_shortcuts::get("", n, parameter_ns + "/move_group_name", ros_parameters_.move_group_name); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/planning_frame", ros_parameters_.planning_frame); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/use_gazebo", ros_parameters_.use_gazebo); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/check_collisions", ros_parameters_.check_collisions); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/joint_limit_margin", ros_parameters_.joint_limit_margin); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_out_topic", ros_parameters_.command_out_topic); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_out_type", ros_parameters_.command_out_type); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_positions", - ros_parameters_.publish_joint_positions); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_velocities", - ros_parameters_.publish_joint_velocities); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_accelerations", - ros_parameters_.publish_joint_accelerations); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/collision_distance_safety_factor", + ros_parameters_.collision_distance_safety_factor); + error += !rosparam_shortcuts::get("", n, parameter_ns + "/min_allowable_collision_distance", + ros_parameters_.min_allowable_collision_distance); // This parameter name was changed recently. // Try retrieving from the correct name. If it fails, then try the deprecated name. @@ -169,23 +177,6 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) "greater than zero. Check yaml file."); return false; } - if (ros_parameters_.self_collision_proximity_threshold < 0.) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should be " - "greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.scene_collision_proximity_threshold < 0.) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'scene_collision_proximity_threshold' should be " - "greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.scene_collision_proximity_threshold < ros_parameters_.self_collision_proximity_threshold) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should probably be less " - "than or equal to 'scene_collision_proximity_threshold'. Check yaml file."); - } if (ros_parameters_.low_pass_filter_coeff < 0.) { ROS_WARN_NAMED(LOGNAME, "Parameter 'low_pass_filter_coeff' should be " @@ -228,12 +219,48 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) "you must select positions OR velocities."); return false; } + // Collision checking + if (ros_parameters_.collision_check_type != "threshold_distance" && + ros_parameters_.collision_check_type != "stop_distance") + { + ROS_WARN_NAMED(LOGNAME, "collision_check_type must be 'threshold_distance' or 'stop_distance'"); + return false; + } + if (ros_parameters_.self_collision_proximity_threshold < 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should be " + "greater than zero. Check yaml file."); + return false; + } + if (ros_parameters_.scene_collision_proximity_threshold < 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'scene_collision_proximity_threshold' should be " + "greater than zero. Check yaml file."); + return false; + } + if (ros_parameters_.scene_collision_proximity_threshold < ros_parameters_.self_collision_proximity_threshold) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should probably be less " + "than or equal to 'scene_collision_proximity_threshold'. Check yaml file."); + } if (ros_parameters_.collision_check_rate < 0) { ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_check_rate' should be " "greater than zero. Check yaml file."); return false; } + if (ros_parameters_.collision_distance_safety_factor < 1) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_distance_safety_factor' should be " + "greater than or equal to 1. Check yaml file."); + return false; + } + if (ros_parameters_.min_allowable_collision_distance < 0) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'min_allowable_collision_distance' should be " + "greater than zero. Check yaml file."); + return false; + } return true; } diff --git a/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml b/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml index a26f72a2b3..67e2d93f69 100644 --- a/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml +++ b/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml @@ -53,5 +53,13 @@ command_out_topic: jog_server/command # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: stop_distance +# Parameters for "threshold_distance"-type collision checking self_collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m] scene_collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] From 8e2ca7fd7af41821a191a1daa5984fd08a7c0bee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 3 Jun 2020 22:01:24 +0200 Subject: [PATCH 314/612] Replace robot_model and robot_state namespaces with moveit::core (#2135) --- MIGRATION.md | 1 + .../planning_interface/test/move_group_interface_cpp_test.cpp | 2 +- .../moveit/rviz_plugin_render_tools/robot_state_visualization.h | 2 +- .../rviz_plugin_render_tools/src/planning_scene_render.cpp | 2 +- 4 files changed, 4 insertions(+), 3 deletions(-) diff --git a/MIGRATION.md b/MIGRATION.md index 99b8e20aa8..5b2462012c 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -16,6 +16,7 @@ API changes in MoveIt releases - `moveit_ros_planning` no longer depends on `moveit_ros_perception` - `CollisionRobot` and `CollisionWorld` are combined into a single `CollisionEnv` class. This applies for all derived collision checkers as `FCL`, `ALL_VALID`, `HYBRID` and `DISTANCE_FIELD`. Consequently, `getCollisionRobot[Unpadded] / getCollisionWorld` functions are replaced through a `getCollisionEnv` in the planning scene and return the new combined environment. This unified collision environment provides the union of all member functions of `CollisionRobot` and `CollisionWorld`. Note that calling `checkRobotCollision` of the `CollisionEnv` does not take a `CollisionRobot` as an argument anymore as it is implicitly contained in the `CollisionEnv`. - `RobotTrajectory` provides a copy constructor that allows a shallow (default) and deep copy of waypoints +- Replace the redundant namespaces `robot_state::` and `robot_model::` with the actual namespace `moveit::core::`. The additional namespaces will disappear in the future (ROS2). ## ROS Melodic diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp index 7298f03d1a..bb2666f24b 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -132,7 +132,7 @@ class MoveGroupTestFixture : public ::testing::Test void testJointPositions(const std::vector& expected) { SCOPED_TRACE("testJointPositions"); - const robot_state::JointModelGroup* joint_model_group = + const moveit::core::JointModelGroup* joint_model_group = move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP); std::vector actual; move_group_->getCurrentState()->copyJointGroupPositions(joint_model_group, actual); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index 0c04ade4e5..1f3d90c4b3 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -71,7 +71,7 @@ class RobotStateVisualization void update(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color, const std::map& color_map); - void updateKinematicState(const robot_state::RobotStateConstPtr& kinematic_state); + void updateKinematicState(const moveit::core::RobotStateConstPtr& kinematic_state); void setDefaultAttachedObjectColor(const std_msgs::ColorRGBA& default_attached_object_color); /// update color of all attached object shapes void updateAttachedObjectColors(const std_msgs::ColorRGBA& attached_object_color); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp index 5537e0a340..c24ca4adc2 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp @@ -60,7 +60,7 @@ void PlanningSceneRender::updateRobotPosition(const planning_scene::PlanningScen { if (scene_robot_) { - robot_state::RobotStatePtr rs = std::make_shared(scene->getCurrentState()); + auto rs = std::make_shared(scene->getCurrentState()); rs->update(); scene_robot_->updateKinematicState(rs); } From d03bb752e55e3e399541cb773cb389e6da5e27f8 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Fri, 5 Jun 2020 12:32:33 -0600 Subject: [PATCH 315/612] Added throttle to jogarm accel limit warning (#2141) * added throttle to jogarm accel limit warning * switched to ERROR_ONCE --- moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 22ccdeffd0..f97130b5d6 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -767,8 +767,9 @@ bool JogCalcs::updateJoints(JogArmShared& shared_variables) } else { - ROS_WARN_STREAM_NAMED(LOGNAME, "An acceleration limit is not defined for this joint; minimum stop distance " - "should not be used for collision checking"); + ROS_ERROR_STREAM_ONCE_NAMED(LOGNAME, "An acceleration limit is not defined for this joint, '" + << joint_model->getName() << "'; minimum stop distance should not " + "be used for collision checking"); } break; } From f52d7c6e4b0ef8633a517a611d9ff7a909950452 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 22 May 2020 14:58:40 -0600 Subject: [PATCH 316/612] simplify communication between threads in jog arm fixup! simplify communication between threads in jog arm --- .../moveit_jog_arm/CMakeLists.txt | 18 +- .../moveit_jog_arm/collision_check_thread.h | 25 +- .../{jog_interface_base.h => jog_arm.h} | 101 ++++-- .../include/moveit_jog_arm/jog_arm_data.h | 71 +++-- .../include/moveit_jog_arm/jog_calcs.h | 50 ++- .../moveit_jog_arm/jog_cpp_interface.h | 100 ------ .../moveit_jog_arm/jog_ros_interface.h | 60 ---- .../src/collision_check_thread.cpp | 32 +- .../cpp_interface_example.cpp | 19 +- .../{jog_interface_base.cpp => jog_arm.cpp} | 288 +++++++++++++----- .../moveit_jog_arm/src/jog_calcs.cpp | 161 ++++++---- .../moveit_jog_arm/src/jog_cpp_interface.cpp | 232 -------------- .../moveit_jog_arm/src/jog_ros_interface.cpp | 223 -------------- .../moveit_jog_arm/src/jog_server.cpp | 99 +++++- .../test/jog_cpp_interface_test.cpp | 6 +- .../test/jog_cpp_interface_test.test | 6 + 16 files changed, 650 insertions(+), 841 deletions(-) rename moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/{jog_interface_base.h => jog_arm.h} (60%) delete mode 100644 moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h delete mode 100644 moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h rename moveit_experimental/moveit_jog_arm/src/{jog_interface_base.cpp => jog_arm.cpp} (53%) delete mode 100644 moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp delete mode 100644 moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index d8e01a7ec2..b816413b7b 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -11,6 +11,8 @@ if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() +find_package(Boost REQUIRED) + find_package(catkin REQUIRED COMPONENTS control_msgs geometry_msgs @@ -45,6 +47,7 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIR} ) ######################################### @@ -54,14 +57,14 @@ include_directories( add_library(${LIBRARY_NAME} SHARED src/collision_check_thread.cpp src/jog_calcs.cpp - src/jog_cpp_interface.cpp - src/jog_interface_base.cpp + src/jog_arm.cpp src/low_pass_filter.cpp ) add_dependencies(${LIBRARY_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${LIBRARY_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} + ${Boost_LIBRARIES} ) # An example of using the C++ library @@ -72,6 +75,7 @@ add_dependencies(cpp_interface_example ${catkin_EXPORTED_TARGETS}) target_link_libraries(cpp_interface_example ${catkin_LIBRARIES} ${Eigen_LIBRARIES} + ${Boost_LIBRARIES} ${LIBRARY_NAME} ) @@ -82,13 +86,16 @@ target_link_libraries(cpp_interface_example add_executable(jog_server src/collision_check_thread.cpp src/jog_calcs.cpp - src/jog_interface_base.cpp - src/jog_ros_interface.cpp + src/jog_arm.cpp src/jog_server.cpp src/low_pass_filter.cpp ) add_dependencies(jog_server ${catkin_EXPORTED_TARGETS}) -target_link_libraries(jog_server ${catkin_LIBRARIES} ${Eigen_LIBRARIES}) +target_link_libraries(jog_server + ${catkin_LIBRARIES} + ${Eigen_LIBRARIES} + ${Boost_LIBRARIES} +) ################################################ ## An example of converting joystick commands ## @@ -140,6 +147,7 @@ if(CATKIN_ENABLE_TESTING) target_link_libraries(jog_cpp_interface_test ${LIBRARY_NAME} ${catkin_LIBRARIES} + ${Eigen_LIBRARIES} ${Boost_LIBRARIES} ) endif() diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index 447e945808..abef5e1abf 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -38,11 +38,14 @@ #pragma once -#include -#include "jog_arm_data.h" -#include "low_pass_filter.h" +#include + #include #include +#include + +#include "jog_arm_data.h" +#include "low_pass_filter.h" namespace moveit_jog_arm { @@ -63,17 +66,25 @@ class CollisionCheckThread CollisionCheckThread(const moveit_jog_arm::JogArmParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - // Get thread-safe read-only lock of planning scene - planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; - - void startMainLoop(moveit_jog_arm::JogArmShared& shared_variables); + /** \breif run function to be run in a thread */ + void run(moveit_jog_arm::JogArmShared& shared_variables); private: + planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; + void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); + const moveit_jog_arm::JogArmParameters parameters_; CollisionCheckType collision_check_type_; // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // ROS + ros::Subscriber joint_state_sub_; + + // Latest joint state, updated by ROS callback + mutable std::mutex joint_state_mutex_; + sensor_msgs::JointState latest_joint_state_; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h similarity index 60% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h rename to moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h index 9dee171b76..84730b5d0b 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h @@ -36,31 +36,46 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -#include "collision_check_thread.h" +#include +#include +#include + #include -#include "jog_arm_data.h" -#include "jog_calcs.h" -#include "low_pass_filter.h" + #include #include #include #include +#include #include #include +#include "collision_check_thread.h" +#include "jog_arm_data.h" +#include "jog_calcs.h" +#include "low_pass_filter.h" + namespace moveit_jog_arm { /** - * Class JogInterfaceBase - Base class for C++ interface and ROS node. + * Class JogArm - Base class for C++ interface and ROS node. * Handles ROS subs & pubs and creates the worker threads. */ -class JogInterfaceBase +class JogArm { public: - JogInterfaceBase(); + JogArm(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + + ~JogArm(); + + /** \brief start jog arm threads */ + void start(); - /** \brief Update the joints of the robot */ - void jointsCB(const sensor_msgs::JointStateConstPtr& msg); + /** \brief stop jog arm threads */ + void stop(); + + /** \brief Pause or unpause processing jog commands while keeping the threads alive */ + void setPaused(bool paused); /** * Allow drift in certain dimensions. For example, may allow the wrist to rotate freely. @@ -78,20 +93,46 @@ class JogInterfaceBase bool changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, moveit_msgs::ChangeControlDimensions::Response& res); - // Jogging calculation thread - bool startJogCalcThread(); + /** \brief Provide a Cartesian velocity command to the jogger. + * The units are determined by settings in the yaml file. + */ + void provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command); + void provideTwistStampedCommand(const geometry_msgs::TwistStampedConstPtr& msg); - /** \brief Stop the main calculation thread */ - bool stopJogCalcThread(); + /** \brief Send joint position(s) commands */ + void provideJointCommand(const control_msgs::JointJog& joint_command); + void provideJointCommand(const control_msgs::JointJogConstPtr& msg); - /** \brief Start collision checking */ - bool startCollisionCheckThread(); + /** + * Returns the most recent JointState that the jogger has received. + * May eliminate the need to create your own joint_state subscriber. + * + * @return the most recent joints known to the jogger + */ + sensor_msgs::JointState getJointState(); - /** \brief Stop collision checking */ - bool stopCollisionCheckThread(); + /** + * Get the MoveIt planning link transform. + * The transform from the MoveIt planning frame to robot_link_command_frame + * + * @param transform the transform that will be calculated + * @return true if a valid transform was available + */ + bool getCommandFrameTransform(Eigen::Isometry3d& transform); -protected: - bool readParameters(ros::NodeHandle& n); + /** + * Get the status of the jogger. + * + * @return 0 for no warning. The meaning of nonzero values can be seen in status_codes.h + */ + StatusCode getJoggerStatus() const; + +private: + bool readParameters(); + void run(); + bool startJogCalcThread(); + bool startCollisionCheckThread(); + void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; @@ -102,12 +143,30 @@ class JogInterfaceBase // Share data between threads JogArmShared shared_variables_; + // Queues for message passing between threads + std::shared_ptr command_deltas_queue_; + std::shared_ptr joint_command_deltas_queue_; + std::shared_ptr outgoing_command_queue_; + // Jog calcs std::unique_ptr jog_calcs_; - std::unique_ptr jog_calc_thread_; // Collision checks std::unique_ptr collision_checker_; - std::unique_ptr collision_check_thread_; + + // Threads + std::thread jog_server_thread_; + std::thread jog_calc_thread_; + std::thread collision_check_thread_; + + // ROS + ros::Subscriber joint_state_sub_; + ros::Publisher outgoing_cmd_pub_; + + // Latest state + mutable std::mutex latest_state_mutex_; + sensor_msgs::JointState latest_joint_state_; + ros::Time latest_command_stamp_; }; + } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 4a4fb7b91d..136a5b07f1 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -38,9 +38,8 @@ #pragma once -// System -#include -#include +// Boost +#include // Eigen #include @@ -48,7 +47,6 @@ // ROS #include #include -#include #include // moveit_jog_arm @@ -56,21 +54,41 @@ namespace moveit_jog_arm { -// Variables to share between threads -// Inherit from a mutex so the struct can be locked/unlocked easily -struct JogArmShared : public std::mutex +// Types for queues between threads +constexpr size_t QUEUE_SIZE = 10; +template +using MessageQueue = boost::lockfree::spsc_queue>; +using TwistedStampedQueue = MessageQueue; +using JointJogQueue = MessageQueue; +using JointTrajectoryQueue = MessageQueue; + +// Helper function for popping the latest value out of a queue +template +bool popLatest(MessageQueue& queue, T& output) { - geometry_msgs::TwistStamped command_deltas; - - control_msgs::JointJog joint_command_deltas; + size_t count = queue.read_available(); + if (count > 0) + { + // clear all but the latest from the queue + for (size_t i = 0; i < (count - 1); ++i) + { + queue.pop(); + } + // read the latest value + queue.pop(output); + } + return true; +} - sensor_msgs::JointState joints; - - // The collision check thread throttles robot velocity via this variable - std::atomic collision_velocity_scale{ 1 }; +// Variables to share between threads +// Be careful to not read-modify-write any of these because they are not +// protected by a mutex. +struct JogArmShared +{ + double collision_velocity_scale = 1; // The jog thread communicates the minimum stopping time to the collision check thread via this variable - std::atomic worst_case_stop_time{ std::numeric_limits::max() }; + double worst_case_stop_time = std::numeric_limits::max(); // Flag a valid incoming Cartesian command having nonzero velocities bool have_nonzero_cartesian_cmd = false; @@ -81,34 +99,23 @@ struct JogArmShared : public std::mutex // Indicates that we have not received a new command in some time bool command_is_stale = false; - // The new command which is calculated - trajectory_msgs::JointTrajectory outgoing_command; - - // Timestamp of incoming commands - ros::Time latest_nonzero_cmd_stamp = ros::Time(0.); - // Indicates no collision, etc, so outgoing commands can be sent bool ok_to_publish = false; - // The transform from the MoveIt planning frame to robot_link_command_frame - Eigen::Isometry3d tf_moveit_to_cmd_frame; - // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw] - std::atomic_bool drift_dimensions[6] = { ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), - ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false) }; + bool drift_dimensions[6] = { false, false, false, false, false, false }; // Status of the jogger. 0 for no warning. The meaning of nonzero values can be seen in status_codes.h - std::atomic status; + StatusCode status = NO_WARNING; // Pause/unpause jog threads - threads are not paused by default - std::atomic paused{ false }; + bool paused = false; // Stop jog loop threads - threads are not stopped by default - std::atomic stop_requested{ false }; + bool stop_requested = false; // The dimesions to control. In the command frame. [x, y, z, roll, pitch, yaw] - std::atomic_bool control_dimensions[6] = { ATOMIC_VAR_INIT(true), ATOMIC_VAR_INIT(true), ATOMIC_VAR_INIT(true), - ATOMIC_VAR_INIT(true), ATOMIC_VAR_INIT(true), ATOMIC_VAR_INIT(true) }; + bool control_dimensions[6] = { true, true, true, true, true, true }; }; // ROS params to be read. See the yaml file in /config for a description of each. @@ -116,12 +123,10 @@ struct JogArmParameters { std::string move_group_name; std::string joint_topic; - std::string cartesian_command_in_topic; std::string robot_link_command_frame; std::string command_out_topic; std::string planning_frame; std::string status_topic; - std::string joint_command_in_topic; std::string command_in_type; std::string command_out_type; double linear_scale; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 049f7af452..b1c6355d50 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -39,11 +39,12 @@ #pragma once // System -#include +#include // ROS #include #include +#include #include // moveit_jog_arm @@ -56,21 +57,29 @@ namespace moveit_jog_arm class JogCalcs { public: - JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); + JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr, + const std::shared_ptr& command_deltas_queue, + const std::shared_ptr& joint_command_deltas_queue, + const std::shared_ptr& outgoing_command_queue); - void startMainLoop(JogArmShared& shared_variables); + /** \breif Thread run method */ + void run(JogArmShared& shared_variables); /** \brief Check if the robot state is being updated and the end effector transform is known * @return true if initialized properly */ bool isInitialized(); -protected: - ros::NodeHandle nh_; - - // Flag that robot state is up to date, end effector transform is known - std::atomic is_initialized_; + /** + * Get the MoveIt planning link transform. + * The transform from the MoveIt planning frame to robot_link_command_frame + * + * @param transform the transform that will be calculated + * @return true if a valid transform was available + */ + bool getCommandFrameTransform(Eigen::Isometry3d& transform); +private: /** \brief Do jogging calculations for Cartesian twist commands. */ bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables); @@ -153,21 +162,40 @@ class JogCalcs */ void removeDimension(Eigen::MatrixXd& matrix, Eigen::VectorXd& delta_x, unsigned int row_to_remove); + /* \brief Callback for joint subsription */ + void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); + + // Flag that robot state is up to date, end effector transform is known + bool is_initialized_; + const moveit::core::JointModelGroup* joint_model_group_; moveit::core::RobotStatePtr kinematic_state_; + // Queues for message passing between threads + std::shared_ptr command_deltas_queue_; + std::shared_ptr joint_command_deltas_queue_; + std::shared_ptr outgoing_command_queue_; + + // latest_state_mutex_ is used to protect + // incoming_joint_state_ and tf_moveit_to_cmd_frame_ + mutable std::mutex latest_state_mutex_; + sensor_msgs::JointState incoming_joint_state_; + Eigen::Isometry3d tf_moveit_to_cmd_frame_; + // incoming_joint_state_ is the incoming message. It may contain passive joints or other joints we don't care about. // internal_joint_state_ is used in jog calculations. It shouldn't be relied on to be accurate. // original_joint_state_ is the same as incoming_joint_state_ except it only contains the joints jog_arm acts on. - sensor_msgs::JointState incoming_joint_state_, internal_joint_state_, original_joint_state_; + sensor_msgs::JointState internal_joint_state_, original_joint_state_; std::map joint_state_name_map_; + trajectory_msgs::JointTrajectory outgoing_command_; std::vector position_filters_; + // ROS + ros::Subscriber joint_state_sub_; ros::Publisher status_pub_; - StatusCode status_ = NO_WARNING; JogArmParameters parameters_; @@ -176,8 +204,6 @@ class JogCalcs Eigen::ArrayXd delta_theta_; Eigen::ArrayXd prev_joint_velocity_; - Eigen::Isometry3d tf_moveit_to_cmd_frame_; - const int gazebo_redundant_message_count_ = 30; uint num_joints_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h deleted file mode 100644 index a6d35e077a..0000000000 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h +++ /dev/null @@ -1,100 +0,0 @@ -/******************************************************************************* - * Title : jog_cpp_interface.h - * Project : moveit_jog_arm - * Created : 11/20/2019 - * Author : Andy Zelenak - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 the copyright holder 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 HOLDER 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 "jog_interface_base.h" -#include - -namespace moveit_jog_arm -{ -/** -* Class JogCppInterface - This class should be instantiated in a new thread -* See cpp_interface_example.cpp -*/ -class JogCppInterface : public JogInterfaceBase -{ -public: - JogCppInterface(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - - ~JogCppInterface(); - - void startMainLoop(); - - void stopMainLoop(); - - /** \brief Pause or unpause processing jog commands while keeping the main loop alive */ - void setPaused(bool paused); - - /** \brief Provide a Cartesian velocity command to the jogger. - * The units are determined by settings in the yaml file. - */ - void provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command); - - /** \brief Send joint position(s) commands */ - void provideJointCommand(const control_msgs::JointJog& joint_command); - - /** - * Returns the most recent JointState that the jogger has received. - * May eliminate the need to create your own joint_state subscriber. - * - * @return the most recent joints known to the jogger - */ - sensor_msgs::JointState getJointState(); - - /** - * Get the MoveIt planning link transform. - * The transform from the MoveIt planning frame to robot_link_command_frame - * - * @param transform the transform that will be calculated - * @return true if a valid transform was available - */ - bool getCommandFrameTransform(Eigen::Isometry3d& transform); - - /** - * Get the status of the jogger. - * - * @return 0 for no warning. The meaning of nonzero values can be seen in status_codes.h - */ - StatusCode getJoggerStatus(); - -private: - ros::NodeHandle nh_; -}; -} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h deleted file mode 100644 index 7afe5029f5..0000000000 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h +++ /dev/null @@ -1,60 +0,0 @@ -/******************************************************************************* - * Title : jog_ros_interface.h - * Project : moveit_jog_arm - * Created : 3/9/2017 - * Author : Brian O'Neil, Blake Anderson, Andy Zelenak - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 the copyright holder 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 HOLDER 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. -*******************************************************************************/ - -// Server node for arm jogging with MoveIt. - -#pragma once - -#include "jog_interface_base.h" - -namespace moveit_jog_arm -{ -/** - * Class JogROSInterface - Instantiated in main(). Handles ROS subs & pubs and creates the worker threads. - */ -class JogROSInterface : protected JogInterfaceBase -{ -public: - JogROSInterface(); - -private: - // ROS subscriber callbacks - void deltaCartesianCmdCB(const geometry_msgs::TwistStampedConstPtr& msg); - void deltaJointCmdCB(const control_msgs::JointJogConstPtr& msg); -}; -} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index c3d47fdcc2..3c4fcb8d18 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -51,8 +51,17 @@ CollisionCheckThread::CollisionCheckThread( const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : parameters_(parameters), planning_scene_monitor_(planning_scene_monitor) { + ros::NodeHandle nh; + if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE) ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Collision check rate is low, increase it in yaml file if CPU allows"); + + // subscribe to joints + joint_state_sub_ = nh.subscribe(parameters.joint_topic, 1, &CollisionCheckThread::jointStateCB, this); + + // Wait for incoming topics to appear + ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); + ros::topic::waitForMessage(parameters.joint_topic); } planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPlanningSceneRO() const @@ -60,7 +69,7 @@ planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPla return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); } -void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) +void CollisionCheckThread::run(JogArmShared& shared_variables) { // Init collision request collision_detection::CollisionRequest collision_request; @@ -93,19 +102,24 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) double safety_factor = parameters_.collision_distance_safety_factor; collision_detection::AllowedCollisionMatrix acm = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); + ///////////////////////////////////////////////// // Spin while checking collisions ///////////////////////////////////////////////// + sensor_msgs::JointState joint_state; + while (ros::ok() && !shared_variables.stop_requested) { if (!shared_variables.paused) { - shared_variables.lock(); - sensor_msgs::JointState jts = shared_variables.joints; - shared_variables.unlock(); + { + // Copy the latest joint state + const std::lock_guard lock(CollisionCheckThread); + joint_state = latest_joint_state_; + } - for (std::size_t i = 0; i < jts.position.size(); ++i) - current_state.setJointPositions(jts.name[i], &jts.position[i]); + for (std::size_t i = 0; i < joint_state.position.size(); ++i) + current_state.setJointPositions(joint_state.name[i], &joint_state.position[i]); current_state.updateCollisionBodyTransforms(); collision_detected = false; @@ -193,4 +207,10 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) collision_rate.sleep(); } } + +void CollisionCheckThread::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) +{ + const std::lock_guard lock(joint_state_mutex_); + latest_joint_state_ = *msg; +} } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index 987bf449cb..ad916a90be 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -36,7 +36,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -#include "moveit_jog_arm/jog_cpp_interface.h" +#include "moveit_jog_arm/jog_arm.h" #include "moveit_jog_arm/status_codes.h" static const std::string LOGNAME = "cpp_interface_example"; @@ -49,6 +49,8 @@ static const std::string LOGNAME = "cpp_interface_example"; int main(int argc, char** argv) { ros::init(argc, argv, LOGNAME); + ros::AsyncSpinner spinner(4); + spinner.start(); // Load the planning scene monitor planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; @@ -67,8 +69,8 @@ int main(int argc, char** argv) planning_scene_monitor->startStateMonitor(); // Run the jogging C++ interface in a new thread to ensure a constant outgoing message rate. - moveit_jog_arm::JogCppInterface jog_interface(planning_scene_monitor); - std::thread jogging_thread([&]() { jog_interface.startMainLoop(); }); + moveit_jog_arm::JogArm jog_arm(planning_scene_monitor); + jog_arm.start(); // Make a Cartesian velocity message geometry_msgs::TwistStamped velocity_msg; @@ -84,7 +86,7 @@ int main(int argc, char** argv) { ++num_commands; velocity_msg.header.stamp = ros::Time::now(); - jog_interface.provideTwistStampedCommand(velocity_msg); + jog_arm.provideTwistStampedCommand(velocity_msg); cmd_rate.sleep(); } @@ -104,20 +106,19 @@ int main(int argc, char** argv) { ++num_commands; base_joint_command.header.stamp = ros::Time::now(); - jog_interface.provideJointCommand(base_joint_command); + jog_arm.provideJointCommand(base_joint_command); cmd_rate.sleep(); } // Retrieve the current joint state from the jogger - sensor_msgs::JointState current_joint_state = jog_interface.getJointState(); + sensor_msgs::JointState current_joint_state = jog_arm.getJointState(); ROS_INFO_STREAM_NAMED(LOGNAME, "Current joint state:"); ROS_INFO_STREAM_NAMED(LOGNAME, current_joint_state); // Retrieve the current status of the jogger - moveit_jog_arm::StatusCode status = jog_interface.getJoggerStatus(); + moveit_jog_arm::StatusCode status = jog_arm.getJoggerStatus(); ROS_INFO_STREAM_NAMED(LOGNAME, "Jogger status:\n" << status); - jog_interface.stopMainLoop(); - jogging_thread.join(); + jog_arm.stop(); return 0; } diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp similarity index 53% rename from moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp rename to moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index 3234d70853..d124e31552 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -31,25 +31,51 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -/* Title : jog_interface_base.cpp +/* Title : jog_arm.cpp * Project : moveit_jog_arm * Created : 3/9/2017 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson */ -#include "moveit_jog_arm/jog_interface_base.h" +#include "moveit_jog_arm/jog_arm.h" -static const std::string LOGNAME = "jog_interface_base"; +static const std::string LOGNAME = "jog_arm"; namespace moveit_jog_arm { -JogInterfaceBase::JogInterfaceBase() +JogArm::JogArm(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : planning_scene_monitor_(planning_scene_monitor) { + ros::NodeHandle nh; + + // Read ROS parameters, typically from YAML file + if (!readParameters()) + exit(EXIT_FAILURE); + + // Create queues for message passing between threads + command_deltas_queue_ = std::make_shared(); + joint_command_deltas_queue_ = std::make_shared(); + outgoing_command_queue_ = std::make_shared(); + + // Publish freshly-calculated joints to the robot. + // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). + if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") + outgoing_cmd_pub_ = nh.advertise(ros_parameters_.command_out_topic, 1); + else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") + outgoing_cmd_pub_ = nh.advertise(ros_parameters_.command_out_topic, 1); + + // subscribe to joints + joint_state_sub_ = nh.subscribe(ros_parameters_.joint_topic, 1, &JogArm::jointStateCB, this); + + // Wait for incoming topics to appear + ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); + ros::topic::waitForMessage(ros_parameters_.joint_topic); } // Read ROS parameters, typically from YAML file -bool JogInterfaceBase::readParameters(ros::NodeHandle& n) +bool JogArm::readParameters() { + ros::NodeHandle nh; std::size_t error = 0; // Specified in the launch file. All other parameters will be read from this namespace. @@ -64,58 +90,54 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) return false; } - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_period", ros_parameters_.publish_period); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/publish_period", ros_parameters_.publish_period); error += - !rosparam_shortcuts::get("", n, parameter_ns + "/collision_check_rate", ros_parameters_.collision_check_rate); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/num_outgoing_halt_msgs_to_publish", + !rosparam_shortcuts::get("", nh, parameter_ns + "/collision_check_rate", ros_parameters_.collision_check_rate); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/num_outgoing_halt_msgs_to_publish", ros_parameters_.num_outgoing_halt_msgs_to_publish); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/scale/linear", ros_parameters_.linear_scale); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/scale/rotational", ros_parameters_.rotational_scale); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/scale/joint", ros_parameters_.joint_scale); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/scale/linear", ros_parameters_.linear_scale); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/scale/rotational", ros_parameters_.rotational_scale); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/scale/joint", ros_parameters_.joint_scale); error += - !rosparam_shortcuts::get("", n, parameter_ns + "/low_pass_filter_coeff", ros_parameters_.low_pass_filter_coeff); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/joint_topic", ros_parameters_.joint_topic); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_in_type", ros_parameters_.command_in_type); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/cartesian_command_in_topic", - ros_parameters_.cartesian_command_in_topic); - error += - !rosparam_shortcuts::get("", n, parameter_ns + "/joint_command_in_topic", ros_parameters_.joint_command_in_topic); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/robot_link_command_frame", + !rosparam_shortcuts::get("", nh, parameter_ns + "/low_pass_filter_coeff", ros_parameters_.low_pass_filter_coeff); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/joint_topic", ros_parameters_.joint_topic); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/command_in_type", ros_parameters_.command_in_type); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/robot_link_command_frame", ros_parameters_.robot_link_command_frame); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/incoming_command_timeout", + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/incoming_command_timeout", ros_parameters_.incoming_command_timeout); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/lower_singularity_threshold", + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/lower_singularity_threshold", ros_parameters_.lower_singularity_threshold); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/hard_stop_singularity_threshold", + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/hard_stop_singularity_threshold", ros_parameters_.hard_stop_singularity_threshold); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/move_group_name", ros_parameters_.move_group_name); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/planning_frame", ros_parameters_.planning_frame); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/use_gazebo", ros_parameters_.use_gazebo); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/joint_limit_margin", ros_parameters_.joint_limit_margin); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_out_topic", ros_parameters_.command_out_topic); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_out_type", ros_parameters_.command_out_type); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_positions", + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/move_group_name", ros_parameters_.move_group_name); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/planning_frame", ros_parameters_.planning_frame); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/use_gazebo", ros_parameters_.use_gazebo); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/joint_limit_margin", ros_parameters_.joint_limit_margin); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/command_out_topic", ros_parameters_.command_out_topic); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/command_out_type", ros_parameters_.command_out_type); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/publish_joint_positions", ros_parameters_.publish_joint_positions); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_velocities", + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/publish_joint_velocities", ros_parameters_.publish_joint_velocities); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_accelerations", + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/publish_joint_accelerations", ros_parameters_.publish_joint_accelerations); // Parameters for collision checking - error += !rosparam_shortcuts::get("", n, parameter_ns + "/check_collisions", ros_parameters_.check_collisions); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/check_collisions", ros_parameters_.check_collisions); error += - !rosparam_shortcuts::get("", n, parameter_ns + "/collision_check_type", ros_parameters_.collision_check_type); + !rosparam_shortcuts::get("", nh, parameter_ns + "/collision_check_type", ros_parameters_.collision_check_type); bool have_self_collision_proximity_threshold = rosparam_shortcuts::get( - "", n, parameter_ns + "/self_collision_proximity_threshold", ros_parameters_.self_collision_proximity_threshold); + "", nh, parameter_ns + "/self_collision_proximity_threshold", ros_parameters_.self_collision_proximity_threshold); bool have_scene_collision_proximity_threshold = - rosparam_shortcuts::get("", n, parameter_ns + "/scene_collision_proximity_threshold", + rosparam_shortcuts::get("", nh, parameter_ns + "/scene_collision_proximity_threshold", ros_parameters_.scene_collision_proximity_threshold); double collision_proximity_threshold; // 'collision_proximity_threshold' parameter was removed, replaced with separate self- and scene-collision proximity // thresholds // TODO(JStech): remove this deprecation warning in ROS Noetic; simplify error case handling - if (n.hasParam(parameter_ns + "/collision_proximity_threshold") && - rosparam_shortcuts::get("", n, parameter_ns + "/collision_proximity_threshold", collision_proximity_threshold)) + if (nh.hasParam(parameter_ns + "/collision_proximity_threshold") && + rosparam_shortcuts::get("", nh, parameter_ns + "/collision_proximity_threshold", collision_proximity_threshold)) { ROS_WARN_NAMED(LOGNAME, "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate" "'self_collision_proximity_threshold' and 'scene_collision_proximity_threshold' " @@ -133,19 +155,19 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) } error += !have_self_collision_proximity_threshold; error += !have_scene_collision_proximity_threshold; - error += !rosparam_shortcuts::get("", n, parameter_ns + "/collision_distance_safety_factor", + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/collision_distance_safety_factor", ros_parameters_.collision_distance_safety_factor); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/min_allowable_collision_distance", + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/min_allowable_collision_distance", ros_parameters_.min_allowable_collision_distance); // This parameter name was changed recently. // Try retrieving from the correct name. If it fails, then try the deprecated name. // TODO(andyz): remove this deprecation warning in ROS Noetic - if (!rosparam_shortcuts::get("", n, parameter_ns + "/status_topic", ros_parameters_.status_topic)) + if (!rosparam_shortcuts::get("", nh, parameter_ns + "/status_topic", ros_parameters_.status_topic)) { ROS_WARN_NAMED(LOGNAME, "'status_topic' parameter is missing. Recently renamed from 'warning_topic'. Please update " "the jogging yaml file."); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/warning_topic", ros_parameters_.status_topic); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/warning_topic", ros_parameters_.status_topic); } rosparam_shortcuts::shutdownIfError(parameter_ns, error); @@ -265,18 +287,106 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n) return true; } +void JogArm::run() +{ + ros::Rate main_rate(1. / ros_parameters_.publish_period); + trajectory_msgs::JointTrajectory outgoing_command; + + // Wait for low pass filters to stabilize + ROS_INFO_STREAM_NAMED(LOGNAME, "Waiting for low-pass filters to stabilize."); + ros::Duration(10 * ros_parameters_.publish_period).sleep(); + + while (ros::ok() && !shared_variables_.stop_requested) + { + // Check for stale cmds + { + const std::lock_guard lock(latest_state_mutex_); + shared_variables_.command_is_stale = + ((ros::Time::now() - latest_command_stamp_) >= ros::Duration(ros_parameters_.incoming_command_timeout)); + } + + // Read the latest outgoing command out of the queue + popLatest(*outgoing_command_queue_, outgoing_command); + + // Publish the most recent trajectory, unless the jogging calculation thread tells not to + if (shared_variables_.ok_to_publish) + { + // Put the outgoing msg in the right format + // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). + if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") + { + outgoing_command.header.stamp = ros::Time::now(); + outgoing_cmd_pub_.publish(outgoing_command); + } + else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") + { + std_msgs::Float64MultiArray joints; + if (ros_parameters_.publish_joint_positions) + joints.data = outgoing_command.points[0].positions; + else if (ros_parameters_.publish_joint_velocities) + joints.data = outgoing_command.points[0].velocities; + outgoing_cmd_pub_.publish(joints); + } + } + else if (shared_variables_.command_is_stale) + { + ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " + "Try a larger 'incoming_command_timeout' parameter?"); + } + else + { + ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); + } + + main_rate.sleep(); + } +} + +void JogArm::start() +{ + shared_variables_.stop_requested = false; + shared_variables_.paused = false; + + // Start the jog server main + jog_server_thread_ = std::thread([&]() { this->run(); }); + + // Crunch the numbers in this thread + startJogCalcThread(); + + // Check collisions in this thread + if (ros_parameters_.check_collisions) + startCollisionCheckThread(); +} + +void JogArm::stop() +{ + shared_variables_.stop_requested = true; + + if (jog_server_thread_.joinable()) + jog_server_thread_.join(); + + if (jog_calc_thread_.joinable()) + jog_calc_thread_.join(); + + if (collision_check_thread_.joinable()) + collision_check_thread_.join(); +} + +JogArm::~JogArm() +{ + stop(); +} + // Listen to joint angles. Store them in a shared variable. -void JogInterfaceBase::jointsCB(const sensor_msgs::JointStateConstPtr& msg) +void JogArm::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) { - shared_variables_.lock(); - shared_variables_.joints = *msg; - shared_variables_.unlock(); + const std::lock_guard lock(latest_state_mutex_); + latest_joint_state_ = *msg; } -bool JogInterfaceBase::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, - moveit_msgs::ChangeDriftDimensions::Response& res) +bool JogArm::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, + moveit_msgs::ChangeDriftDimensions::Response& res) { - // These are std::atomic's, they are threadsafe without a mutex lock shared_variables_.drift_dimensions[0] = req.drift_x_translation; shared_variables_.drift_dimensions[1] = req.drift_y_translation; shared_variables_.drift_dimensions[2] = req.drift_z_translation; @@ -288,8 +398,8 @@ bool JogInterfaceBase::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions: return true; } -bool JogInterfaceBase::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, - moveit_msgs::ChangeControlDimensions::Response& res) +bool JogArm::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, + moveit_msgs::ChangeControlDimensions::Response& res) { shared_variables_.control_dimensions[0] = req.control_x_translation; shared_variables_.control_dimensions[1] = req.control_y_translation; @@ -303,50 +413,80 @@ bool JogInterfaceBase::changeControlDimensions(moveit_msgs::ChangeControlDimensi } // A separate thread for the heavy jogging calculations. -bool JogInterfaceBase::startJogCalcThread() +bool JogArm::startJogCalcThread() { if (!jog_calcs_) - jog_calcs_.reset(new JogCalcs(ros_parameters_, planning_scene_monitor_->getRobotModelLoader())); + jog_calcs_ = + std::make_unique(ros_parameters_, planning_scene_monitor_->getRobotModelLoader(), + command_deltas_queue_, joint_command_deltas_queue_, outgoing_command_queue_); - jog_calc_thread_.reset(new std::thread([&]() { jog_calcs_->startMainLoop(shared_variables_); })); + jog_calc_thread_ = std::thread([&]() { jog_calcs_->run(shared_variables_); }); return true; } -bool JogInterfaceBase::stopJogCalcThread() +// A separate thread for collision checking. +bool JogArm::startCollisionCheckThread() { - if (jog_calc_thread_) - { - if (jog_calc_thread_->joinable()) - jog_calc_thread_->join(); + if (!collision_checker_) + collision_checker_ = std::make_unique(ros_parameters_, planning_scene_monitor_); - jog_calc_thread_.reset(); - } + collision_check_thread_ = std::thread([&]() { collision_checker_->run(shared_variables_); }); return true; } -// A separate thread for collision checking. -bool JogInterfaceBase::startCollisionCheckThread() +void JogArm::provideTwistStampedCommand(const geometry_msgs::TwistStamped& msg) { - if (!collision_checker_) - collision_checker_.reset(new CollisionCheckThread(ros_parameters_, planning_scene_monitor_)); - - collision_check_thread_.reset(new std::thread([&]() { collision_checker_->startMainLoop(shared_variables_); })); + command_deltas_queue_->push(msg); + if (msg.header.stamp != ros::Time(0.)) + { + const std::lock_guard lock(latest_state_mutex_); + latest_command_stamp_ = msg.header.stamp; + } +}; - return true; +void JogArm::provideTwistStampedCommand(const geometry_msgs::TwistStampedConstPtr& msg) +{ + provideTwistStampedCommand(*msg); } -bool JogInterfaceBase::stopCollisionCheckThread() +void JogArm::provideJointCommand(const control_msgs::JointJog& msg) { - if (collision_check_thread_) + joint_command_deltas_queue_->push(msg); + if (msg.header.stamp != ros::Time(0.)) { - if (collision_check_thread_->joinable()) - collision_check_thread_->join(); - - collision_check_thread_.reset(); + const std::lock_guard lock(latest_state_mutex_); + latest_command_stamp_ = msg.header.stamp; } +} - return true; +void JogArm::provideJointCommand(const control_msgs::JointJogConstPtr& msg) +{ + provideJointCommand(*msg); +} + +void JogArm::setPaused(bool paused) +{ + shared_variables_.paused = paused; +} + +sensor_msgs::JointState JogArm::getJointState() +{ + const std::lock_guard lock(latest_state_mutex_); + return latest_joint_state_; } + +bool JogArm::getCommandFrameTransform(Eigen::Isometry3d& transform) +{ + if (!jog_calcs_ || !jog_calcs_->isInitialized()) + return false; + return jog_calcs_->getCommandFrameTransform(transform); +} + +StatusCode JogArm::getJoggerStatus() const +{ + return shared_variables_.status; +} + } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index f97130b5d6..acbb153fea 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -43,12 +43,42 @@ static const std::string LOGNAME = "jog_calcs"; namespace moveit_jog_arm { +namespace +{ +// Helper function for detecting zeroed message +bool isNonZero(const geometry_msgs::TwistStamped& msg) +{ + return msg.twist.linear.x != 0.0 || msg.twist.linear.y != 0.0 || msg.twist.linear.z != 0.0 || + msg.twist.angular.x != 0.0 || msg.twist.angular.y != 0.0 || msg.twist.angular.z != 0.0; +} + +// Helper function for detecting zeroed message +bool isNonZero(const control_msgs::JointJog& msg) +{ + bool all_zeros = true; + for (double delta : msg.velocities) + { + all_zeros &= (delta == 0.0); + }; + return !all_zeros; +} +} // namespace + // Constructor for the class that handles jogging calculations -JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) - : parameters_(parameters), default_sleep_rate_(1000) +JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr, + const std::shared_ptr& command_deltas_queue, + const std::shared_ptr& joint_command_deltas_queue, + const std::shared_ptr& outgoing_command_queue) + : command_deltas_queue_(command_deltas_queue) + , joint_command_deltas_queue_(joint_command_deltas_queue) + , outgoing_command_queue_(outgoing_command_queue) + , parameters_(parameters) + , default_sleep_rate_(1000) { + ros::NodeHandle nh; + // Publish jogger status - status_pub_ = nh_.advertise(parameters_.status_topic, 1); + status_pub_ = nh.advertise(parameters_.status_topic, 1); // MoveIt Setup while (ros::ok() && !model_loader_ptr) @@ -62,9 +92,12 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader: joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getActiveJointModels().size()); + + // subscribe to joints + joint_state_sub_ = nh.subscribe(parameters.joint_topic, 1, &JogCalcs::jointStateCB, this); } -void JogCalcs::startMainLoop(JogArmShared& shared_variables) +void JogCalcs::run(JogArmShared& shared_variables) { // Reset flags is_initialized_ = false; @@ -95,10 +128,6 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) { if (shared_variables.stop_requested) return; - - shared_variables.lock(); - incoming_joint_state_ = shared_variables.joints; - shared_variables.unlock(); default_sleep_rate_.sleep(); } @@ -117,6 +146,11 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) geometry_msgs::TwistStamped cartesian_deltas; control_msgs::JointJog joint_deltas; + // Nonzero status flags + bool have_nonzero_cartesian_command = false; + bool have_nonzero_joint_command = false; + bool have_nonzero_command = false; + // Do jogging calcs while (ros::ok() && !shared_variables.stop_requested) { @@ -131,12 +165,40 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) // Get the transform from MoveIt planning frame to jogging command frame // We solve (planning_frame -> base -> robot_link_command_frame) // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) - kinematic_state_->setVariableValues(incoming_joint_state_); + { + const std::lock_guard lock(latest_state_mutex_); + kinematic_state_->setVariableValues(incoming_joint_state_); + } tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); - shared_variables.lock(); - shared_variables.tf_moveit_to_cmd_frame = tf_moveit_to_cmd_frame_; - shared_variables.unlock(); + + // Read the latest cartesian commands out of the queues + bool have_new_cartesian_command = popLatest(*command_deltas_queue_, cartesian_deltas); + bool have_new_joint_command = popLatest(*joint_command_deltas_queue_, joint_deltas); + + if (have_new_cartesian_command) + { + // Update nonzero flags for updated messages + have_nonzero_cartesian_command = isNonZero(cartesian_deltas); + + // Input frame determined by YAML file if not passed with message + if (cartesian_deltas.header.frame_id.empty()) + { + cartesian_deltas.header.frame_id = parameters_.robot_link_command_frame; + } + } + if (have_new_joint_command) + { + // Update nonzero flags for updated messages + have_nonzero_joint_command = isNonZero(joint_deltas); + + // Input frame determined by YAML file if not passed with message + if (cartesian_deltas.header.frame_id.empty()) + { + joint_deltas.header.frame_id = parameters_.robot_link_command_frame; + } + } + have_nonzero_command = have_nonzero_cartesian_command || have_nonzero_joint_command; // If paused or while waiting for initial jog commands, just keep the low-pass filters up to date with current // joints so a jump doesn't occur when restarting @@ -145,72 +207,47 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) for (std::size_t i = 0; i < num_joints_; ++i) position_filters_[i].reset(original_joint_state_.position[i]); - shared_variables.lock(); // Check if there are any new commands with valid timestamp - wait_for_jog_commands = shared_variables.command_deltas.header.stamp == ros::Time(0.) && - shared_variables.joint_command_deltas.header.stamp == ros::Time(0.); - shared_variables.unlock(); + wait_for_jog_commands = + cartesian_deltas.header.stamp == ros::Time(0.) && joint_deltas.header.stamp == ros::Time(0.); } // If not waiting for initial command, and not paused. // Do jogging calculations only if the robot should move, for efficiency else { - // Halt if the command is stale or inputs are all zero, or commands were zero - shared_variables.lock(); - bool have_nonzero_cartesian_cmd = shared_variables.have_nonzero_cartesian_cmd; - bool have_nonzero_joint_cmd = shared_variables.have_nonzero_joint_cmd; - bool stale_command = shared_variables.command_is_stale; - shared_variables.unlock(); - - bool valid_nonzero_command = false; - if (!stale_command) + if (!shared_variables.command_is_stale) { // Prioritize cartesian jogging above joint jogging - if (have_nonzero_cartesian_cmd) + if (have_nonzero_cartesian_command) { - shared_variables.lock(); - cartesian_deltas = shared_variables.command_deltas; - shared_variables.unlock(); - if (!cartesianJogCalcs(cartesian_deltas, shared_variables)) continue; } - else if (have_nonzero_joint_cmd) + else if (have_nonzero_joint_command) { - shared_variables.lock(); - joint_deltas = shared_variables.joint_command_deltas; - shared_variables.unlock(); - if (!jointJogCalcs(joint_deltas, shared_variables)) continue; } - - valid_nonzero_command = have_nonzero_cartesian_cmd || have_nonzero_joint_cmd; } // If we should halt - if (!valid_nonzero_command) + if (!have_nonzero_command) { // Keep the joint position filters up-to-date with current joints for (std::size_t i = 0; i < num_joints_; ++i) position_filters_[i].reset(original_joint_state_.position[i]); suddenHalt(outgoing_command_); - have_nonzero_cartesian_cmd = false; - have_nonzero_joint_cmd = false; - // Reset the valid command flag so jogging stops until a new command arrives - shared_variables.lock(); - shared_variables.have_nonzero_cartesian_cmd = false; - shared_variables.have_nonzero_joint_cmd = false; - shared_variables.unlock(); + have_nonzero_cartesian_command = false; + have_nonzero_joint_command = false; } // Send the newest target joints - shared_variables.lock(); // If everything normal, share the new traj to be published - if (valid_nonzero_command) + if (have_nonzero_command) { - shared_variables.outgoing_command = outgoing_command_; + outgoing_command_queue_->push(outgoing_command_); + // shared_variables.outgoing_command = outgoing_command_; shared_variables.ok_to_publish = true; } // Skip the jogging publication if all inputs have been zero for several cycles in a row. @@ -223,14 +260,14 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables) // The command is invalid but we are publishing num_outgoing_halt_msgs_to_publish else { - shared_variables.outgoing_command = outgoing_command_; + outgoing_command_queue_->push(outgoing_command_); + // shared_variables.outgoing_command = outgoing_command_; shared_variables.ok_to_publish = true; } - shared_variables.unlock(); // Store last zero-velocity message flag to prevent superfluous warnings. // Cartesian and joint commands must both be zero. - if (!have_nonzero_cartesian_cmd && !have_nonzero_joint_cmd) + if (!have_nonzero_cartesian_command && !have_nonzero_joint_command) { // Avoid overflow if (zero_velocity_count < std::numeric_limits::max()) @@ -713,9 +750,8 @@ void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_traj) // Parse the incoming joint msg for the joints of our MoveGroup bool JogCalcs::updateJoints(JogArmShared& shared_variables) { - shared_variables.lock(); - incoming_joint_state_ = shared_variables.joints; - shared_variables.unlock(); + // lock the latest state mutex for the joint states + const std::lock_guard lock(latest_state_mutex_); // Check that the msg contains enough joints if (incoming_joint_state_.name.size() < num_joints_) @@ -835,6 +871,7 @@ Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& comman } catch (const std::out_of_range& e) { + const std::lock_guard lock(latest_state_mutex_); ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_.name[m]); continue; } @@ -886,4 +923,20 @@ void JogCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta jacobian.conservativeResize(num_rows, num_cols); delta_x.conservativeResize(num_rows); } + +void JogCalcs::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) +{ + const std::lock_guard lock(latest_state_mutex_); + incoming_joint_state_ = *msg; +} + +bool JogCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) +{ + const std::lock_guard lock(latest_state_mutex_); + transform = tf_moveit_to_cmd_frame_; + + // All zeros means the transform wasn't initialized, so return false + return !transform.matrix().isZero(0); +} + } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp deleted file mode 100644 index 47e23f5eb0..0000000000 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ /dev/null @@ -1,232 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 the copyright holder 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 HOLDER 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. -*******************************************************************************/ - -/* Title : jog_cpp_interface.cpp - * Project : moveit_jog_arm - * Created : 11/20/2019 - * Author : Andy Zelenak - */ - -#include "moveit_jog_arm/jog_cpp_interface.h" - -static const std::string LOGNAME = "jog_cpp_interface"; - -namespace moveit_jog_arm -{ -JogCppInterface::JogCppInterface(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) -{ - planning_scene_monitor_ = planning_scene_monitor; - - // Read ROS parameters, typically from YAML file - if (!readParameters(nh_)) - exit(EXIT_FAILURE); -} - -JogCppInterface::~JogCppInterface() -{ - stopMainLoop(); -} - -void JogCppInterface::startMainLoop() -{ - // Reset loop termination flag - shared_variables_.stop_requested = false; - shared_variables_.paused = false; - - // Crunch the numbers in this thread - startJogCalcThread(); - - // Check collisions in this thread - if (ros_parameters_.check_collisions) - startCollisionCheckThread(); - - // ROS subscriptions. Share the data with the worker threads - ros::Subscriber joints_sub = - nh_.subscribe(ros_parameters_.joint_topic, 1, &JogInterfaceBase::jointsCB, dynamic_cast(this)); - - // Publish freshly-calculated joints to the robot. - // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - ros::Publisher outgoing_cmd_pub; - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - outgoing_cmd_pub = nh_.advertise(ros_parameters_.command_out_topic, 1); - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") - outgoing_cmd_pub = nh_.advertise(ros_parameters_.command_out_topic, 1); - - // Wait for incoming topics to appear - ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); - ros::topic::waitForMessage(ros_parameters_.joint_topic); - - // Wait for low pass filters to stabilize - ROS_INFO_STREAM_NAMED(LOGNAME, "Waiting for low-pass filters to stabilize."); - ros::Duration(10 * ros_parameters_.publish_period).sleep(); - - ros::Rate main_rate(1. / ros_parameters_.publish_period); - - while (ros::ok() && !shared_variables_.stop_requested) - { - ros::spinOnce(); - - if (!shared_variables_.paused) - { - shared_variables_.lock(); - trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; - - // Check if incoming commands are stale - shared_variables_.command_is_stale = (ros::Time::now() - shared_variables_.latest_nonzero_cmd_stamp) >= - ros::Duration(ros_parameters_.incoming_command_timeout); - - // Publish the most recent trajectory, unless the jogging calculation thread tells not to - if (shared_variables_.ok_to_publish) - { - // Put the outgoing msg in the right format - // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - { - outgoing_command.header.stamp = ros::Time::now(); - outgoing_cmd_pub.publish(outgoing_command); - } - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") - { - std_msgs::Float64MultiArray joints; - if (ros_parameters_.publish_joint_positions) - joints.data = outgoing_command.points[0].positions; - else if (ros_parameters_.publish_joint_velocities) - joints.data = outgoing_command.points[0].velocities; - outgoing_cmd_pub.publish(joints); - } - } - else if (shared_variables_.command_is_stale) - { - ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " - "Try a larger 'incoming_command_timeout' parameter?"); - } - else - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); - } - - shared_variables_.unlock(); - } - - main_rate.sleep(); - } - - stopJogCalcThread(); - stopCollisionCheckThread(); -} - -void JogCppInterface::stopMainLoop() -{ - shared_variables_.stop_requested = true; -} - -void JogCppInterface::setPaused(bool paused) -{ - shared_variables_.paused = paused; -} - -void JogCppInterface::provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command) -{ - shared_variables_.lock(); - - shared_variables_.command_deltas.twist = velocity_command.twist; - shared_variables_.command_deltas.header = velocity_command.header; - - // Input frame determined by YAML file if not passed with message - if (shared_variables_.command_deltas.header.frame_id.empty()) - { - shared_variables_.command_deltas.header.frame_id = ros_parameters_.robot_link_command_frame; - } - - // Check if input is all zeros. Flag it if so to skip calculations/publication after num_outgoing_halt_msgs_to_publish - shared_variables_.have_nonzero_cartesian_cmd = shared_variables_.command_deltas.twist.linear.x != 0.0 || - shared_variables_.command_deltas.twist.linear.y != 0.0 || - shared_variables_.command_deltas.twist.linear.z != 0.0 || - shared_variables_.command_deltas.twist.angular.x != 0.0 || - shared_variables_.command_deltas.twist.angular.y != 0.0 || - shared_variables_.command_deltas.twist.angular.z != 0.0; - - if (shared_variables_.have_nonzero_cartesian_cmd) - { - shared_variables_.latest_nonzero_cmd_stamp = velocity_command.header.stamp; - } - shared_variables_.unlock(); -}; - -void JogCppInterface::provideJointCommand(const control_msgs::JointJog& joint_command) -{ - shared_variables_.lock(); - shared_variables_.joint_command_deltas = joint_command; - - // Check if joint inputs is all zeros. Flag it if so to skip calculations/publication - bool all_zeros = true; - for (double delta : shared_variables_.joint_command_deltas.velocities) - { - all_zeros &= (delta == 0.0); - }; - shared_variables_.have_nonzero_joint_cmd = !all_zeros; - - if (shared_variables_.have_nonzero_joint_cmd) - { - shared_variables_.latest_nonzero_cmd_stamp = joint_command.header.stamp; - } - shared_variables_.unlock(); -} - -sensor_msgs::JointState JogCppInterface::getJointState() -{ - shared_variables_.lock(); - sensor_msgs::JointState current_joints = shared_variables_.joints; - shared_variables_.unlock(); - - return current_joints; -} - -bool JogCppInterface::getCommandFrameTransform(Eigen::Isometry3d& transform) -{ - if (!jog_calcs_ || !jog_calcs_->isInitialized()) - return false; - - shared_variables_.lock(); - transform = shared_variables_.tf_moveit_to_cmd_frame; - shared_variables_.unlock(); - - // All zeros means the transform wasn't initialized, so return false - return !transform.matrix().isZero(0); -} - -StatusCode JogCppInterface::getJoggerStatus() -{ - return shared_variables_.status; -} -} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp deleted file mode 100644 index 567e0b707a..0000000000 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ /dev/null @@ -1,223 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 the copyright holder 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 HOLDER 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. -*******************************************************************************/ - -/* Title : jog_ros_interface.cpp - * Project : moveit_jog_arm - * Created : 3/9/2017 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - */ - -// Server node for arm jogging with MoveIt. - -#include - -static const std::string LOGNAME = "jog_ros_interface"; - -namespace moveit_jog_arm -{ -///////////////////////////////////////////////////////////////////////////////// -// JogROSInterface handles ROS subscriptions and instantiates the worker threads. -// One worker thread does the jogging calculations. -// Another worker thread does collision checking. -///////////////////////////////////////////////////////////////////////////////// - -// Constructor for the main ROS interface node -JogROSInterface::JogROSInterface() -{ - ros::NodeHandle nh; - - // Read ROS parameters, typically from YAML file - if (!readParameters(nh)) - exit(EXIT_FAILURE); - - // Load the planning scene monitor - planning_scene_monitor_ = std::make_shared("robot_description"); - if (!planning_scene_monitor_->getPlanningScene()) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } - - planning_scene_monitor_->startSceneMonitor(); - planning_scene_monitor_->startWorldGeometryMonitor( - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, - false /* skip octomap monitor */); - planning_scene_monitor_->startStateMonitor(); - - // Crunch the numbers in this thread - startJogCalcThread(); - - // Check collisions in this thread - if (ros_parameters_.check_collisions) - startCollisionCheckThread(); - - // ROS subscriptions. Share the data with the worker threads - ros::Subscriber cmd_sub = - nh.subscribe(ros_parameters_.cartesian_command_in_topic, 1, &JogROSInterface::deltaCartesianCmdCB, this); - ros::Subscriber joint_jog_cmd_sub = - nh.subscribe(ros_parameters_.joint_command_in_topic, 1, &JogROSInterface::deltaJointCmdCB, this); - ros::Subscriber joints_sub = - nh.subscribe(ros_parameters_.joint_topic, 1, &JogInterfaceBase::jointsCB, dynamic_cast(this)); - - // ROS Server for allowing drift in some dimensions - ros::ServiceServer drift_dimensions_server = - nh.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions", - &JogInterfaceBase::changeDriftDimensions, dynamic_cast(this)); - // ROS Server for changing the control dimensions - ros::ServiceServer dims_server = - nh.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", - &JogInterfaceBase::changeControlDimensions, dynamic_cast(this)); - - // Publish freshly-calculated joints to the robot. - // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - ros::Publisher outgoing_cmd_pub; - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - outgoing_cmd_pub = nh.advertise(ros_parameters_.command_out_topic, 1); - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") - outgoing_cmd_pub = nh.advertise(ros_parameters_.command_out_topic, 1); - - // Wait for incoming topics to appear - ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); - ros::topic::waitForMessage(ros_parameters_.joint_topic); - - // Wait for low pass filters to stabilize - ROS_INFO_STREAM_NAMED(LOGNAME, "Waiting for low-pass filters to stabilize."); - ros::Duration(10 * ros_parameters_.publish_period).sleep(); - - ros::Rate main_rate(1. / ros_parameters_.publish_period); - - while (ros::ok()) - { - ros::spinOnce(); - - shared_variables_.lock(); - trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; - - // Check for stale cmds - shared_variables_.command_is_stale = ((ros::Time::now() - shared_variables_.latest_nonzero_cmd_stamp) >= - ros::Duration(ros_parameters_.incoming_command_timeout)); - - // Publish the most recent trajectory, unless the jogging calculation thread tells not to - if (shared_variables_.ok_to_publish) - { - // Put the outgoing msg in the right format - // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - { - outgoing_command.header.stamp = ros::Time::now(); - outgoing_cmd_pub.publish(outgoing_command); - } - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") - { - std_msgs::Float64MultiArray joints; - if (ros_parameters_.publish_joint_positions) - joints.data = outgoing_command.points[0].positions; - else if (ros_parameters_.publish_joint_velocities) - joints.data = outgoing_command.points[0].velocities; - outgoing_cmd_pub.publish(joints); - } - } - else if (shared_variables_.command_is_stale) - { - ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " - "Try a larger 'incoming_command_timeout' parameter?"); - } - else - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); - } - - shared_variables_.unlock(); - - main_rate.sleep(); - } - - // Stop JogArm threads - shared_variables_.stop_requested = true; - stopJogCalcThread(); - stopCollisionCheckThread(); -} - -// Listen to cartesian delta commands. Store them in a shared variable. -void JogROSInterface::deltaCartesianCmdCB(const geometry_msgs::TwistStampedConstPtr& msg) -{ - shared_variables_.lock(); - - // Copy everything but the frame name. The frame name is set by yaml file at startup. - // (so it isn't copied over and over) - shared_variables_.command_deltas.twist = msg->twist; - shared_variables_.command_deltas.header = msg->header; - - // Input frame determined by YAML file if not passed with message - if (shared_variables_.command_deltas.header.frame_id.empty()) - { - shared_variables_.command_deltas.header.frame_id = ros_parameters_.robot_link_command_frame; - } - - // Check if input is all zeros. Flag it if so to skip calculations/publication after num_outgoing_halt_msgs_to_publish - shared_variables_.have_nonzero_cartesian_cmd = shared_variables_.command_deltas.twist.linear.x != 0.0 || - shared_variables_.command_deltas.twist.linear.y != 0.0 || - shared_variables_.command_deltas.twist.linear.z != 0.0 || - shared_variables_.command_deltas.twist.angular.x != 0.0 || - shared_variables_.command_deltas.twist.angular.y != 0.0 || - shared_variables_.command_deltas.twist.angular.z != 0.0; - - if (shared_variables_.have_nonzero_cartesian_cmd) - { - shared_variables_.latest_nonzero_cmd_stamp = msg->header.stamp; - } - shared_variables_.unlock(); -} - -// Listen to joint delta commands. Store them in a shared variable. -void JogROSInterface::deltaJointCmdCB(const control_msgs::JointJogConstPtr& msg) -{ - shared_variables_.lock(); - shared_variables_.joint_command_deltas = *msg; - - // Check if joint inputs is all zeros. Flag it if so to skip calculations/publication - bool all_zeros = true; - for (double delta : shared_variables_.joint_command_deltas.velocities) - { - all_zeros &= (delta == 0.0); - }; - shared_variables_.have_nonzero_joint_cmd = !all_zeros; - - if (shared_variables_.have_nonzero_joint_cmd) - { - shared_variables_.latest_nonzero_cmd_stamp = msg->header.stamp; - } - shared_variables_.unlock(); -} -} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp index f258211f45..ffc7dc8b3c 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp @@ -37,15 +37,108 @@ * Author : Andy Zelenak */ -#include +#include -static const std::string LOGNAME = "jog_server"; +namespace +{ +constexpr char LOGNAME[] = "jog_server"; +constexpr char ROS_THREADS = 4; + +struct JogServerParameters +{ + bool read_error = true; + std::string cartesian_command_in_topic = ""; + std::string joint_command_in_topic = ""; +}; + +JogServerParameters readParameters() +{ + ros::NodeHandle nh; + JogServerParameters params; + + // Load parameters + std::string parameter_ns; + ros::param::get("~parameter_ns", parameter_ns); + + if (parameter_ns.empty()) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "A namespace must be specified in the launch file, like:"); + ROS_ERROR_STREAM_NAMED(LOGNAME, ""); + params.read_error = true; + } + else + { + std::size_t error = 0; + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/cartesian_command_in_topic", + params.cartesian_command_in_topic); + error += !rosparam_shortcuts::get("", nh, parameter_ns + "/joint_command_in_topic", params.joint_command_in_topic); + params.read_error = (error != 0); + } + return params; +} + +} // namespace int main(int argc, char** argv) { ros::init(argc, argv, LOGNAME); + ros::AsyncSpinner spinner(ROS_THREADS); + spinner.start(); + + ros::NodeHandle nh; + + const auto params = readParameters(); + if (params.read_error) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error reading parameters."); + exit(EXIT_FAILURE); + } + + // Load the planning scene monitor + auto planning_scene_monitor = std::make_shared("robot_description"); + if (!planning_scene_monitor->getPlanningScene()) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); + exit(EXIT_FAILURE); + } + + // Start the planning scene monitor + planning_scene_monitor->startSceneMonitor(); + planning_scene_monitor->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + planning_scene_monitor->startStateMonitor(); + + // Create the jog server + moveit_jog_arm::JogArm jog_arm(planning_scene_monitor); + + // ROS subscriptions. Share the data with the worker threads + auto cmd_sub = + nh.subscribe(params.cartesian_command_in_topic, 1, &moveit_jog_arm::JogArm::provideTwistStampedCommand, &jog_arm); + auto joint_jog_cmd_sub = + nh.subscribe(params.joint_command_in_topic, 1, &moveit_jog_arm::JogArm::provideJointCommand, &jog_arm); + + // ROS Server for allowing drift in some dimensions + auto drift_dimensions_server = + nh.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions", + &moveit_jog_arm::JogArm::changeDriftDimensions, &jog_arm); + + // ROS Server for changing the control dimensions + auto dims_server = + nh.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", + &moveit_jog_arm::JogArm::changeControlDimensions, &jog_arm); + + // Start the jog server (runs in the ros spinner) + jog_arm.start(); + + // Wait for ros to shutdown + ros::waitForShutdown(); - moveit_jog_arm::JogROSInterface ros_interface; + // Stop the jog server + jog_arm.stop(); return 0; } diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp index 2b18f7e083..fea4d4cf16 100644 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp @@ -46,7 +46,7 @@ #include // Main class -#include +#include static const std::string LOGNAME = "jog_cpp_interface_test"; @@ -75,8 +75,10 @@ class TestJogCppInterface : public ::testing::Test TEST_F(TestJogCppInterface, InitTest) { - moveit_jog_arm::JogCppInterface jog_cpp_interface(planning_scene_monitor_); + moveit_jog_arm::JogArm jog_arm(planning_scene_monitor_); + jog_arm.start(); ros::Duration(1).sleep(); // Give the started thread some time to run + jog_arm.stop(); } // TODO(davetcoleman): due to many blocking checks for ROS messages, and diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test index 428d8482e1..73d626f086 100644 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test @@ -5,6 +5,12 @@ + + + + + + From f21c7255fc7ff680adf5a23de21f55eb074d685f Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 26 May 2020 16:36:55 -0600 Subject: [PATCH 317/612] use ros timers instead of threads in jog_arm --- .../moveit_jog_arm/collision_check_thread.h | 48 ++- .../include/moveit_jog_arm/jog_arm.h | 18 +- .../include/moveit_jog_arm/jog_arm_data.h | 3 + .../include/moveit_jog_arm/jog_calcs.h | 53 +++- .../src/collision_check_thread.cpp | 249 +++++++-------- .../cpp_interface_example.cpp | 3 +- .../moveit_jog_arm/src/jog_arm.cpp | 299 ++++++++---------- .../moveit_jog_arm/src/jog_calcs.cpp | 281 ++++++++-------- .../moveit_jog_arm/src/jog_server.cpp | 4 +- .../test/jog_cpp_interface_test.cpp | 12 +- 10 files changed, 518 insertions(+), 452 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index abef5e1abf..b88b0d783d 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -63,28 +63,66 @@ class CollisionCheckThread * \param planning_scene_monitor: PSM should have scene monitor and state monitor * already started when passed into this class */ - CollisionCheckThread(const moveit_jog_arm::JogArmParameters& parameters, + CollisionCheckThread(ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, + JogArmShared& shared_variables, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - /** \breif run function to be run in a thread */ - void run(moveit_jog_arm::JogArmShared& shared_variables); + /** \breif start and stop the Thread */ + void start(); + void stop(); private: + void init(); + void run(const ros::TimerEvent& timer_event); planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); + // ROS node handle + ros::NodeHandle nh_; + + // Parameters const moveit_jog_arm::JogArmParameters parameters_; - CollisionCheckType collision_check_type_; + // Shared variables from JogArm + JogArmShared& shared_variables_; // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + // Robot state and collision matrix from planning scene + std::unique_ptr current_state_; + collision_detection::AllowedCollisionMatrix acm_; + + // Scale robot velocity according to collision proximity and user-defined thresholds. + // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. + // Proximity decreasing --> decelerate + CollisionCheckType collision_check_type_; + double velocity_scale_ = 1; + double self_collision_distance_ = 0; + double scene_collision_distance_ = 0; + bool collision_detected_ = false; + + // Variables for stop-distance-based collision checking + double current_collision_distance_ = 0; + double derivative_of_collision_distance_ = 0; + double prev_collision_distance_ = 0; + double est_time_to_collision_ = 0; + double safety_factor_ = 1000; + + const double self_velocity_scale_coefficient_; + const double scene_velocity_scale_coefficient_; + + // collision request + collision_detection::CollisionRequest collision_request_; + collision_detection::CollisionResult collision_result_; + // ROS + ros::Timer timer_; + ros::Duration period_; ros::Subscriber joint_state_sub_; // Latest joint state, updated by ROS callback mutable std::mutex joint_state_mutex_; - sensor_msgs::JointState latest_joint_state_; + sensor_msgs::JointStateConstPtr latest_joint_state_; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h index 84730b5d0b..c8c7cea178 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h @@ -38,7 +38,6 @@ #include #include -#include #include @@ -64,7 +63,7 @@ namespace moveit_jog_arm class JogArm { public: - JogArm(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); ~JogArm(); @@ -129,16 +128,16 @@ class JogArm private: bool readParameters(); - void run(); - bool startJogCalcThread(); - bool startCollisionCheckThread(); + void run(const ros::TimerEvent& timer_event); void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); + ros::NodeHandle nh_; + // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; // Store the parameters that were read from ROS server - JogArmParameters ros_parameters_; + JogArmParameters parameters_; // Share data between threads JogArmShared shared_variables_; @@ -154,12 +153,9 @@ class JogArm // Collision checks std::unique_ptr collision_checker_; - // Threads - std::thread jog_server_thread_; - std::thread jog_calc_thread_; - std::thread collision_check_thread_; - // ROS + ros::Timer timer_; + ros::Duration period_; ros::Subscriber joint_state_sub_; ros::Publisher outgoing_cmd_pub_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 136a5b07f1..be168098c4 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -123,10 +123,12 @@ struct JogArmParameters { std::string move_group_name; std::string joint_topic; + std::string cartesian_command_in_topic; std::string robot_link_command_frame; std::string command_out_topic; std::string planning_frame; std::string status_topic; + std::string joint_command_in_topic; std::string command_in_type; std::string command_out_type; double linear_scale; @@ -152,4 +154,5 @@ struct JogArmParameters double collision_distance_safety_factor; double min_allowable_collision_distance; }; + } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index b1c6355d50..8aa79cc836 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -57,13 +57,15 @@ namespace moveit_jog_arm class JogCalcs { public: - JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr, + JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, JogArmShared& shared_variables, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::shared_ptr& command_deltas_queue, const std::shared_ptr& joint_command_deltas_queue, const std::shared_ptr& outgoing_command_queue); - /** \breif Thread run method */ - void run(JogArmShared& shared_variables); + /** \breif Start and stop the timer (thread) */ + void start(); + void stop(); /** \brief Check if the robot state is being updated and the end effector transform is known * @return true if initialized properly @@ -80,6 +82,12 @@ class JogCalcs bool getCommandFrameTransform(Eigen::Isometry3d& transform); private: + /** \breif Init must be called once before run */ + void init(); + + /** \brief Timer method */ + void run(const ros::TimerEvent& timer_event); + /** \brief Do jogging calculations for Cartesian twist commands. */ bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables); @@ -165,8 +173,36 @@ class JogCalcs /* \brief Callback for joint subsription */ void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); + // ROS node handle + ros::NodeHandle nh_; + + // Ros parameters from JogArm + const JogArmParameters& parameters_; + + // Shared variables from JogArm + JogArmShared& shared_variables_; + + // Pointer to the collision environment + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + // Flag that robot state is up to date, end effector transform is known - bool is_initialized_; + bool is_initialized_ = false; + + // Track the number of cycles during which motion has not occurred. + // Will avoid re-publishing zero velocities endlessly. + int zero_velocity_count_ = 0; + + // Flag for staying inactive while there are no incoming commands + bool wait_for_jog_commands_ = true; + + // Nonzero status flags + bool have_nonzero_cartesian_command_ = false; + bool have_nonzero_joint_command_ = false; + bool have_nonzero_command_ = false; + + // Incoming command messages + geometry_msgs::TwistStamped cartesian_deltas_; + control_msgs::JointJog joint_deltas_; const moveit::core::JointModelGroup* joint_model_group_; @@ -180,10 +216,10 @@ class JogCalcs // latest_state_mutex_ is used to protect // incoming_joint_state_ and tf_moveit_to_cmd_frame_ mutable std::mutex latest_state_mutex_; + // incoming_joint_state_ is the incoming message. It may contain passive joints or other joints we don't care about. sensor_msgs::JointState incoming_joint_state_; Eigen::Isometry3d tf_moveit_to_cmd_frame_; - // incoming_joint_state_ is the incoming message. It may contain passive joints or other joints we don't care about. // internal_joint_state_ is used in jog calculations. It shouldn't be relied on to be accurate. // original_joint_state_ is the same as incoming_joint_state_ except it only contains the joints jog_arm acts on. sensor_msgs::JointState internal_joint_state_, original_joint_state_; @@ -194,12 +230,12 @@ class JogCalcs std::vector position_filters_; // ROS + ros::Timer timer_; + ros::Duration period_; ros::Subscriber joint_state_sub_; ros::Publisher status_pub_; StatusCode status_ = NO_WARNING; - JogArmParameters parameters_; - // Use ArrayXd type to enable more coefficient-wise operations Eigen::ArrayXd delta_theta_; Eigen::ArrayXd prev_joint_velocity_; @@ -208,6 +244,7 @@ class JogCalcs uint num_joints_; - ros::Rate default_sleep_rate_; + // Amount we sleep when waiting + ros::Rate default_sleep_rate_ = 1000; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 3c4fcb8d18..b04232f6ec 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -47,21 +47,29 @@ namespace moveit_jog_arm { // Constructor for the class that handles collision checking CollisionCheckThread::CollisionCheckThread( - const moveit_jog_arm::JogArmParameters& parameters, + ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, JogArmShared& shared_variables, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) - : parameters_(parameters), planning_scene_monitor_(planning_scene_monitor) + : nh_(nh) + , parameters_(parameters) + , shared_variables_(shared_variables) + , planning_scene_monitor_(planning_scene_monitor) + , self_velocity_scale_coefficient_(-log(0.001) / parameters.self_collision_proximity_threshold) + , scene_velocity_scale_coefficient_(-log(0.001) / parameters.scene_collision_proximity_threshold) + , period_(1. / parameters_.collision_check_rate) { - ros::NodeHandle nh; + // Init collision request + collision_request_.group_name = parameters_.move_group_name; + collision_request_.distance = true; // enable distance-based collision checking if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE) ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Collision check rate is low, increase it in yaml file if CPU allows"); // subscribe to joints - joint_state_sub_ = nh.subscribe(parameters.joint_topic, 1, &CollisionCheckThread::jointStateCB, this); + joint_state_sub_ = nh_.subscribe(parameters.joint_topic, 1, &CollisionCheckThread::jointStateCB, this); - // Wait for incoming topics to appear - ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); - ros::topic::waitForMessage(parameters.joint_topic); + collision_check_type_ = + (parameters_.collision_check_type == "threshold_distance" ? K_THRESHOLD_DISTANCE : K_STOP_DISTANCE); + safety_factor_ = parameters_.collision_distance_safety_factor; } planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPlanningSceneRO() const @@ -69,148 +77,137 @@ planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPla return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); } -void CollisionCheckThread::run(JogArmShared& shared_variables) +void CollisionCheckThread::init() { - // Init collision request - collision_detection::CollisionRequest collision_request; - collision_request.group_name = parameters_.move_group_name; - collision_request.distance = true; // enable distance-based collision checking - collision_detection::CollisionResult collision_result; - - // Copy the planning scene's version of current state into new memory - moveit::core::RobotState current_state(getLockedPlanningSceneRO()->getCurrentState()); - - const double self_velocity_scale_coefficient = -log(0.001) / parameters_.self_collision_proximity_threshold; - const double scene_velocity_scale_coefficient = -log(0.001) / parameters_.scene_collision_proximity_threshold; - ros::Rate collision_rate(parameters_.collision_check_rate); + // Wait for incoming topics to appear + ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); + ros::topic::waitForMessage(parameters_.joint_topic); - double self_collision_distance = 0; - double scene_collision_distance = 0; - bool collision_detected; + current_state_ = std::make_unique(getLockedPlanningSceneRO()->getCurrentState()); + acm_ = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); +} - // This variable scales the robot velocity when a collision is close - double velocity_scale = 1; +void CollisionCheckThread::start() +{ + init(); + timer_ = nh_.createTimer(period_, &CollisionCheckThread::run, this); +} - collision_check_type_ = - (parameters_.collision_check_type == "threshold_distance" ? K_THRESHOLD_DISTANCE : K_STOP_DISTANCE); +void CollisionCheckThread::stop() +{ + timer_.stop(); +} - // Variables for stop-distance-based collision checking - double current_collision_distance = 0; - double derivative_of_collision_distance = 0; - double prev_collision_distance = 0; - double est_time_to_collision = 0; - double safety_factor = parameters_.collision_distance_safety_factor; +void CollisionCheckThread::run(const ros::TimerEvent& timer_event) +{ + // Log last loop duration and warn if it was longer than period + if (timer_event.profile.last_duration.toSec() < period_.toSec()) + { + ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " (" + << period_.toSec() << ")"); + } + else + { + ROS_WARN_STREAM_THROTTLE_NAMED(1, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " > " + << period_.toSec()); + } - collision_detection::AllowedCollisionMatrix acm = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); + if (shared_variables_.paused) + { + return; + } - ///////////////////////////////////////////////// - // Spin while checking collisions - ///////////////////////////////////////////////// - sensor_msgs::JointState joint_state; + { + // Copy the latest joint state + const std::lock_guard lock(CollisionCheckThread); + for (std::size_t i = 0; i < latest_joint_state_->position.size(); ++i) + current_state_->setJointPositions(latest_joint_state_->name[i], &latest_joint_state_->position[i]); + } - while (ros::ok() && !shared_variables.stop_requested) + current_state_->updateCollisionBodyTransforms(); + collision_detected_ = false; + + // Do a thread-safe distance-based collision detection + collision_result_.clear(); + getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, + *current_state_); + scene_collision_distance_ = collision_result_.distance; + collision_detected_ |= collision_result_.collision; + + collision_result_.clear(); + getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision(collision_request_, collision_result_, + *current_state_, acm_); + self_collision_distance_ = collision_result_.distance; + collision_detected_ |= collision_result_.collision; + + velocity_scale_ = 1; + // If we're definitely in collision, stop immediately + if (collision_detected_) { - if (!shared_variables.paused) + velocity_scale_ = 0; + } + // If threshold distances were specified + else if (collision_check_type_ == K_THRESHOLD_DISTANCE) + { + // If we are far from a collision, velocity_scale should be 1. + // If we are very close to a collision, velocity_scale should be ~zero. + // When scene_collision_proximity_threshold is breached, start decelerating exponentially. + if (scene_collision_distance_ < parameters_.scene_collision_proximity_threshold) { - { - // Copy the latest joint state - const std::lock_guard lock(CollisionCheckThread); - joint_state = latest_joint_state_; - } - - for (std::size_t i = 0; i < joint_state.position.size(); ++i) - current_state.setJointPositions(joint_state.name[i], &joint_state.position[i]); - - current_state.updateCollisionBodyTransforms(); - collision_detected = false; + // velocity_scale = e ^ k * (collision_distance - threshold) + // k = - ln(0.001) / collision_proximity_threshold + // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. + // velocity_scale should equal 0.001 when collision_distance is at zero. + velocity_scale_ = + std::min(velocity_scale_, exp(scene_velocity_scale_coefficient_ * + (scene_collision_distance_ - parameters_.scene_collision_proximity_threshold))); + } - // Do a thread-safe distance-based collision detection - collision_result.clear(); - getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request, collision_result, - current_state); - scene_collision_distance = collision_result.distance; - collision_detected |= collision_result.collision; + if (self_collision_distance_ < parameters_.self_collision_proximity_threshold) + { + velocity_scale_ = + std::min(velocity_scale_, exp(self_velocity_scale_coefficient_ * + (self_collision_distance_ - parameters_.self_collision_proximity_threshold))); + } + } + // Else, we stop based on worst-case stopping distance + else + { + // Retrieve the worst-case time to stop, based on current joint velocities - collision_result.clear(); - getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision(collision_request, collision_result, - current_state, acm); - self_collision_distance = collision_result.distance; - collision_detected |= collision_result.collision; + // Calculate rate of change of distance to nearest collision + current_collision_distance_ = std::min(scene_collision_distance_, self_collision_distance_); + derivative_of_collision_distance_ = (current_collision_distance_ - prev_collision_distance_) / period_.toSec(); - velocity_scale = 1; + if (current_collision_distance_ < parameters_.min_allowable_collision_distance && + derivative_of_collision_distance_ <= 0) + { + velocity_scale_ = 0; + } + // Only bother doing calculations if we are moving toward the nearest collision + else if (derivative_of_collision_distance_ < -EPSILON) + { + // At the rate the collision distance is decreasing, how long until we collide? + est_time_to_collision_ = fabs(current_collision_distance_ / derivative_of_collision_distance_); - // If we're definitely in collision, stop immediately - if (collision_detected) + // halt if we can't stop fast enough (including the safety factor) + if (est_time_to_collision_ < (safety_factor_ * shared_variables_.worst_case_stop_time)) { - velocity_scale = 0; + velocity_scale_ = 0; } - // If threshold distances were specified - else if (collision_check_type_ == K_THRESHOLD_DISTANCE) - { - // If we are far from a collision, velocity_scale should be 1. - // If we are very close to a collision, velocity_scale should be ~zero. - // When scene_collision_proximity_threshold is breached, start decelerating exponentially. - if (scene_collision_distance < parameters_.scene_collision_proximity_threshold) - { - // velocity_scale = e ^ k * (collision_distance - threshold) - // k = - ln(0.001) / collision_proximity_threshold - // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. - // velocity_scale should equal 0.001 when collision_distance is at zero. - velocity_scale = std::min(velocity_scale, - exp(scene_velocity_scale_coefficient * - (scene_collision_distance - parameters_.scene_collision_proximity_threshold))); - } - - if (self_collision_distance < parameters_.self_collision_proximity_threshold) - { - velocity_scale = - std::min(velocity_scale, exp(self_velocity_scale_coefficient * - (self_collision_distance - parameters_.self_collision_proximity_threshold))); - } - } - // Else, we stop based on worst-case stopping distance - else - { - // Retrieve the worst-case time to stop, based on current joint velocities - - // Calculate rate of change of distance to nearest collision - current_collision_distance = std::min(scene_collision_distance, self_collision_distance); - derivative_of_collision_distance = - (current_collision_distance - prev_collision_distance) / collision_rate.expectedCycleTime().toSec(); - - if (current_collision_distance < parameters_.min_allowable_collision_distance && - derivative_of_collision_distance <= 0) - { - velocity_scale = 0; - } - // Only bother doing calculations if we are moving toward the nearest collision - else if (derivative_of_collision_distance < -EPSILON) - { - // At the rate the collision distance is decreasing, how long until we collide? - est_time_to_collision = fabs(current_collision_distance / derivative_of_collision_distance); - - // halt if we can't stop fast enough (including the safety factor) - if (est_time_to_collision < (safety_factor * shared_variables.worst_case_stop_time)) - { - velocity_scale = 0; - } - } - - // Update for the next iteration - prev_collision_distance = current_collision_distance; - } - - // Communicate a velocity-scaling factor back to the other threads - shared_variables.collision_velocity_scale = velocity_scale; } - collision_rate.sleep(); + // Update for the next iteration + prev_collision_distance_ = current_collision_distance_; } + + // Communicate a velocity-scaling factor back to the other threads + shared_variables_.collision_velocity_scale = velocity_scale_; } void CollisionCheckThread::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) { const std::lock_guard lock(joint_state_mutex_); - latest_joint_state_ = *msg; + latest_joint_state_ = msg; } } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index ad916a90be..53c1e971f9 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -49,6 +49,7 @@ static const std::string LOGNAME = "cpp_interface_example"; int main(int argc, char** argv) { ros::init(argc, argv, LOGNAME); + ros::NodeHandle nh; ros::AsyncSpinner spinner(4); spinner.start(); @@ -69,7 +70,7 @@ int main(int argc, char** argv) planning_scene_monitor->startStateMonitor(); // Run the jogging C++ interface in a new thread to ensure a constant outgoing message rate. - moveit_jog_arm::JogArm jog_arm(planning_scene_monitor); + moveit_jog_arm::JogArm jog_arm(nh, planning_scene_monitor); jog_arm.start(); // Make a Cartesian velocity message diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index d124e31552..29b604bdde 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -43,15 +43,16 @@ static const std::string LOGNAME = "jog_arm"; namespace moveit_jog_arm { -JogArm::JogArm(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) - : planning_scene_monitor_(planning_scene_monitor) +JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : nh_(nh), planning_scene_monitor_(planning_scene_monitor) { - ros::NodeHandle nh; - // Read ROS parameters, typically from YAML file if (!readParameters()) exit(EXIT_FAILURE); + // loop period + period_ = ros::Duration(parameters_.publish_period); + // Create queues for message passing between threads command_deltas_queue_ = std::make_shared(); joint_command_deltas_queue_ = std::make_shared(); @@ -59,23 +60,28 @@ JogArm::JogArm(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_s // Publish freshly-calculated joints to the robot. // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - outgoing_cmd_pub_ = nh.advertise(ros_parameters_.command_out_topic, 1); - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") - outgoing_cmd_pub_ = nh.advertise(ros_parameters_.command_out_topic, 1); + if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") + outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, 1); + else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") + outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, 1); // subscribe to joints - joint_state_sub_ = nh.subscribe(ros_parameters_.joint_topic, 1, &JogArm::jointStateCB, this); + joint_state_sub_ = nh_.subscribe(parameters_.joint_topic, 1, &JogArm::jointStateCB, this); // Wait for incoming topics to appear ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); - ros::topic::waitForMessage(ros_parameters_.joint_topic); + ros::topic::waitForMessage(parameters_.joint_topic); + + jog_calcs_ = std::make_unique(nh_, parameters_, shared_variables_, planning_scene_monitor_, + command_deltas_queue_, joint_command_deltas_queue_, outgoing_command_queue_); + + collision_checker_ = + std::make_unique(nh_, parameters_, shared_variables_, planning_scene_monitor_); } // Read ROS parameters, typically from YAML file bool JogArm::readParameters() { - ros::NodeHandle nh; std::size_t error = 0; // Specified in the launch file. All other parameters will be read from this namespace. @@ -90,143 +96,144 @@ bool JogArm::readParameters() return false; } - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/publish_period", ros_parameters_.publish_period); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_period", parameters_.publish_period); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_check_rate", parameters_.collision_check_rate); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/num_outgoing_halt_msgs_to_publish", + parameters_.num_outgoing_halt_msgs_to_publish); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/scale/linear", parameters_.linear_scale); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/scale/rotational", parameters_.rotational_scale); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/scale/joint", parameters_.joint_scale); + error += + !rosparam_shortcuts::get("", nh_, parameter_ns + "/low_pass_filter_coeff", parameters_.low_pass_filter_coeff); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/joint_topic", parameters_.joint_topic); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/command_in_type", parameters_.command_in_type); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/cartesian_command_in_topic", + parameters_.cartesian_command_in_topic); error += - !rosparam_shortcuts::get("", nh, parameter_ns + "/collision_check_rate", ros_parameters_.collision_check_rate); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/num_outgoing_halt_msgs_to_publish", - ros_parameters_.num_outgoing_halt_msgs_to_publish); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/scale/linear", ros_parameters_.linear_scale); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/scale/rotational", ros_parameters_.rotational_scale); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/scale/joint", ros_parameters_.joint_scale); + !rosparam_shortcuts::get("", nh_, parameter_ns + "/joint_command_in_topic", parameters_.joint_command_in_topic); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/robot_link_command_frame", + parameters_.robot_link_command_frame); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/incoming_command_timeout", + parameters_.incoming_command_timeout); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/lower_singularity_threshold", + parameters_.lower_singularity_threshold); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/hard_stop_singularity_threshold", + parameters_.hard_stop_singularity_threshold); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/move_group_name", parameters_.move_group_name); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/planning_frame", parameters_.planning_frame); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/use_gazebo", parameters_.use_gazebo); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/joint_limit_margin", parameters_.joint_limit_margin); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/command_out_topic", parameters_.command_out_topic); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/command_out_type", parameters_.command_out_type); error += - !rosparam_shortcuts::get("", nh, parameter_ns + "/low_pass_filter_coeff", ros_parameters_.low_pass_filter_coeff); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/joint_topic", ros_parameters_.joint_topic); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/command_in_type", ros_parameters_.command_in_type); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/robot_link_command_frame", - ros_parameters_.robot_link_command_frame); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/incoming_command_timeout", - ros_parameters_.incoming_command_timeout); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/lower_singularity_threshold", - ros_parameters_.lower_singularity_threshold); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/hard_stop_singularity_threshold", - ros_parameters_.hard_stop_singularity_threshold); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/move_group_name", ros_parameters_.move_group_name); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/planning_frame", ros_parameters_.planning_frame); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/use_gazebo", ros_parameters_.use_gazebo); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/joint_limit_margin", ros_parameters_.joint_limit_margin); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/command_out_topic", ros_parameters_.command_out_topic); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/command_out_type", ros_parameters_.command_out_type); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/publish_joint_positions", - ros_parameters_.publish_joint_positions); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/publish_joint_velocities", - ros_parameters_.publish_joint_velocities); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/publish_joint_accelerations", - ros_parameters_.publish_joint_accelerations); + !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_joint_positions", parameters_.publish_joint_positions); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_joint_velocities", + parameters_.publish_joint_velocities); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_joint_accelerations", + parameters_.publish_joint_accelerations); // Parameters for collision checking - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/check_collisions", ros_parameters_.check_collisions); - error += - !rosparam_shortcuts::get("", nh, parameter_ns + "/collision_check_type", ros_parameters_.collision_check_type); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/check_collisions", parameters_.check_collisions); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_check_type", parameters_.collision_check_type); bool have_self_collision_proximity_threshold = rosparam_shortcuts::get( - "", nh, parameter_ns + "/self_collision_proximity_threshold", ros_parameters_.self_collision_proximity_threshold); - bool have_scene_collision_proximity_threshold = - rosparam_shortcuts::get("", nh, parameter_ns + "/scene_collision_proximity_threshold", - ros_parameters_.scene_collision_proximity_threshold); + "", nh_, parameter_ns + "/self_collision_proximity_threshold", parameters_.self_collision_proximity_threshold); + bool have_scene_collision_proximity_threshold = rosparam_shortcuts::get( + "", nh_, parameter_ns + "/scene_collision_proximity_threshold", parameters_.scene_collision_proximity_threshold); double collision_proximity_threshold; // 'collision_proximity_threshold' parameter was removed, replaced with separate self- and scene-collision proximity // thresholds // TODO(JStech): remove this deprecation warning in ROS Noetic; simplify error case handling - if (nh.hasParam(parameter_ns + "/collision_proximity_threshold") && - rosparam_shortcuts::get("", nh, parameter_ns + "/collision_proximity_threshold", collision_proximity_threshold)) + if (nh_.hasParam(parameter_ns + "/collision_proximity_threshold") && + rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_proximity_threshold", collision_proximity_threshold)) { ROS_WARN_NAMED(LOGNAME, "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate" "'self_collision_proximity_threshold' and 'scene_collision_proximity_threshold' " "parameters. Please update the jogging yaml file."); if (!have_self_collision_proximity_threshold) { - ros_parameters_.self_collision_proximity_threshold = collision_proximity_threshold; + parameters_.self_collision_proximity_threshold = collision_proximity_threshold; have_self_collision_proximity_threshold = true; } if (!have_scene_collision_proximity_threshold) { - ros_parameters_.scene_collision_proximity_threshold = collision_proximity_threshold; + parameters_.scene_collision_proximity_threshold = collision_proximity_threshold; have_scene_collision_proximity_threshold = true; } } error += !have_self_collision_proximity_threshold; error += !have_scene_collision_proximity_threshold; - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/collision_distance_safety_factor", - ros_parameters_.collision_distance_safety_factor); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/min_allowable_collision_distance", - ros_parameters_.min_allowable_collision_distance); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_distance_safety_factor", + parameters_.collision_distance_safety_factor); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/min_allowable_collision_distance", + parameters_.min_allowable_collision_distance); // This parameter name was changed recently. // Try retrieving from the correct name. If it fails, then try the deprecated name. // TODO(andyz): remove this deprecation warning in ROS Noetic - if (!rosparam_shortcuts::get("", nh, parameter_ns + "/status_topic", ros_parameters_.status_topic)) + if (!rosparam_shortcuts::get("", nh_, parameter_ns + "/status_topic", parameters_.status_topic)) { ROS_WARN_NAMED(LOGNAME, "'status_topic' parameter is missing. Recently renamed from 'warning_topic'. Please update " "the jogging yaml file."); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/warning_topic", ros_parameters_.status_topic); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/warning_topic", parameters_.status_topic); } rosparam_shortcuts::shutdownIfError(parameter_ns, error); // Input checking - if (ros_parameters_.publish_period <= 0.) + if (parameters_.publish_period <= 0.) { ROS_WARN_NAMED(LOGNAME, "Parameter 'publish_period' should be " "greater than zero. Check yaml file."); return false; } - if (ros_parameters_.num_outgoing_halt_msgs_to_publish < 0) + if (parameters_.num_outgoing_halt_msgs_to_publish < 0) { ROS_WARN_NAMED(LOGNAME, "Parameter 'num_outgoing_halt_msgs_to_publish' should be greater than zero. Check yaml file."); return false; } - if (ros_parameters_.hard_stop_singularity_threshold < ros_parameters_.lower_singularity_threshold) + if (parameters_.hard_stop_singularity_threshold < parameters_.lower_singularity_threshold) { ROS_WARN_NAMED(LOGNAME, "Parameter 'hard_stop_singularity_threshold' " "should be greater than 'lower_singularity_threshold.' " "Check yaml file."); return false; } - if ((ros_parameters_.hard_stop_singularity_threshold < 0.) || (ros_parameters_.lower_singularity_threshold < 0.)) + if ((parameters_.hard_stop_singularity_threshold < 0.) || (parameters_.lower_singularity_threshold < 0.)) { ROS_WARN_NAMED(LOGNAME, "Parameters 'hard_stop_singularity_threshold' " "and 'lower_singularity_threshold' should be " "greater than zero. Check yaml file."); return false; } - if (ros_parameters_.low_pass_filter_coeff < 0.) + if (parameters_.low_pass_filter_coeff < 0.) { ROS_WARN_NAMED(LOGNAME, "Parameter 'low_pass_filter_coeff' should be " "greater than zero. Check yaml file."); return false; } - if (ros_parameters_.joint_limit_margin < 0.) + if (parameters_.joint_limit_margin < 0.) { ROS_WARN_NAMED(LOGNAME, "Parameter 'joint_limit_margin' should be " "greater than zero. Check yaml file."); return false; } - if (ros_parameters_.command_in_type != "unitless" && ros_parameters_.command_in_type != "speed_units") + if (parameters_.command_in_type != "unitless" && parameters_.command_in_type != "speed_units") { ROS_WARN_NAMED(LOGNAME, "command_in_type should be 'unitless' or " "'speed_units'. Check yaml file."); return false; } - if (ros_parameters_.command_out_type != "trajectory_msgs/JointTrajectory" && - ros_parameters_.command_out_type != "std_msgs/Float64MultiArray") + if (parameters_.command_out_type != "trajectory_msgs/JointTrajectory" && + parameters_.command_out_type != "std_msgs/Float64MultiArray") { ROS_WARN_NAMED(LOGNAME, "Parameter command_out_type should be " "'trajectory_msgs/JointTrajectory' or " "'std_msgs/Float64MultiArray'. Check yaml file."); return false; } - if (!ros_parameters_.publish_joint_positions && !ros_parameters_.publish_joint_velocities && - !ros_parameters_.publish_joint_accelerations) + if (!parameters_.publish_joint_positions && !parameters_.publish_joint_velocities && + !parameters_.publish_joint_accelerations) { ROS_WARN_NAMED(LOGNAME, "At least one of publish_joint_positions / " "publish_joint_velocities / " @@ -234,50 +241,49 @@ bool JogArm::readParameters() "yaml file."); return false; } - if ((ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") && ros_parameters_.publish_joint_positions && - ros_parameters_.publish_joint_velocities) + if ((parameters_.command_out_type == "std_msgs/Float64MultiArray") && parameters_.publish_joint_positions && + parameters_.publish_joint_velocities) { ROS_WARN_NAMED(LOGNAME, "When publishing a std_msgs/Float64MultiArray, " "you must select positions OR velocities."); return false; } // Collision checking - if (ros_parameters_.collision_check_type != "threshold_distance" && - ros_parameters_.collision_check_type != "stop_distance") + if (parameters_.collision_check_type != "threshold_distance" && parameters_.collision_check_type != "stop_distance") { ROS_WARN_NAMED(LOGNAME, "collision_check_type must be 'threshold_distance' or 'stop_distance'"); return false; } - if (ros_parameters_.self_collision_proximity_threshold < 0.) + if (parameters_.self_collision_proximity_threshold < 0.) { ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should be " "greater than zero. Check yaml file."); return false; } - if (ros_parameters_.scene_collision_proximity_threshold < 0.) + if (parameters_.scene_collision_proximity_threshold < 0.) { ROS_WARN_NAMED(LOGNAME, "Parameter 'scene_collision_proximity_threshold' should be " "greater than zero. Check yaml file."); return false; } - if (ros_parameters_.scene_collision_proximity_threshold < ros_parameters_.self_collision_proximity_threshold) + if (parameters_.scene_collision_proximity_threshold < parameters_.self_collision_proximity_threshold) { ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should probably be less " "than or equal to 'scene_collision_proximity_threshold'. Check yaml file."); } - if (ros_parameters_.collision_check_rate < 0) + if (parameters_.collision_check_rate < 0) { ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_check_rate' should be " "greater than zero. Check yaml file."); return false; } - if (ros_parameters_.collision_distance_safety_factor < 1) + if (parameters_.collision_distance_safety_factor < 1) { ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_distance_safety_factor' should be " "greater than or equal to 1. Check yaml file."); return false; } - if (ros_parameters_.min_allowable_collision_distance < 0) + if (parameters_.min_allowable_collision_distance < 0) { ROS_WARN_NAMED(LOGNAME, "Parameter 'min_allowable_collision_distance' should be " "greater than zero. Check yaml file."); @@ -287,58 +293,60 @@ bool JogArm::readParameters() return true; } -void JogArm::run() +void JogArm::run(const ros::TimerEvent& timer_event) { - ros::Rate main_rate(1. / ros_parameters_.publish_period); - trajectory_msgs::JointTrajectory outgoing_command; + // Log last loop duration and warn if it was longer than period + if (timer_event.profile.last_duration.toSec() < period_.toSec()) + { + ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " (" + << period_.toSec() << ")"); + } + else + { + ROS_WARN_STREAM_THROTTLE_NAMED(1, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " > " + << period_.toSec()); + } - // Wait for low pass filters to stabilize - ROS_INFO_STREAM_NAMED(LOGNAME, "Waiting for low-pass filters to stabilize."); - ros::Duration(10 * ros_parameters_.publish_period).sleep(); + trajectory_msgs::JointTrajectory outgoing_command; - while (ros::ok() && !shared_variables_.stop_requested) + // Check for stale cmds { - // Check for stale cmds - { - const std::lock_guard lock(latest_state_mutex_); - shared_variables_.command_is_stale = - ((ros::Time::now() - latest_command_stamp_) >= ros::Duration(ros_parameters_.incoming_command_timeout)); - } + const std::lock_guard lock(latest_state_mutex_); + shared_variables_.command_is_stale = + ((ros::Time::now() - latest_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); + } - // Read the latest outgoing command out of the queue - popLatest(*outgoing_command_queue_, outgoing_command); + // Read the latest outgoing command out of the queue + popLatest(*outgoing_command_queue_, outgoing_command); - // Publish the most recent trajectory, unless the jogging calculation thread tells not to - if (shared_variables_.ok_to_publish) - { - // Put the outgoing msg in the right format - // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - { - outgoing_command.header.stamp = ros::Time::now(); - outgoing_cmd_pub_.publish(outgoing_command); - } - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") - { - std_msgs::Float64MultiArray joints; - if (ros_parameters_.publish_joint_positions) - joints.data = outgoing_command.points[0].positions; - else if (ros_parameters_.publish_joint_velocities) - joints.data = outgoing_command.points[0].velocities; - outgoing_cmd_pub_.publish(joints); - } - } - else if (shared_variables_.command_is_stale) + // Publish the most recent trajectory, unless the jogging calculation thread tells not to + if (shared_variables_.ok_to_publish) + { + // Put the outgoing msg in the right format + // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). + if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") { - ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " - "Try a larger 'incoming_command_timeout' parameter?"); + outgoing_command.header.stamp = ros::Time::now(); + outgoing_cmd_pub_.publish(outgoing_command); } - else + else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") { - ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); + std_msgs::Float64MultiArray joints; + if (parameters_.publish_joint_positions) + joints.data = outgoing_command.points[0].positions; + else if (parameters_.publish_joint_velocities) + joints.data = outgoing_command.points[0].velocities; + outgoing_cmd_pub_.publish(joints); } - - main_rate.sleep(); + } + else if (shared_variables_.command_is_stale) + { + ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " + "Try a larger 'incoming_command_timeout' parameter?"); + } + else + { + ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); } } @@ -347,29 +355,24 @@ void JogArm::start() shared_variables_.stop_requested = false; shared_variables_.paused = false; - // Start the jog server main - jog_server_thread_ = std::thread([&]() { this->run(); }); - // Crunch the numbers in this thread - startJogCalcThread(); + jog_calcs_->start(); // Check collisions in this thread - if (ros_parameters_.check_collisions) - startCollisionCheckThread(); + if (parameters_.check_collisions) + collision_checker_->start(); + + // Start the jog server timer + timer_ = nh_.createTimer(period_, &JogArm::run, this); } void JogArm::stop() { shared_variables_.stop_requested = true; - if (jog_server_thread_.joinable()) - jog_server_thread_.join(); - - if (jog_calc_thread_.joinable()) - jog_calc_thread_.join(); - - if (collision_check_thread_.joinable()) - collision_check_thread_.join(); + timer_.stop(); + jog_calcs_->stop(); + collision_checker_->stop(); } JogArm::~JogArm() @@ -412,30 +415,6 @@ bool JogArm::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Reque return true; } -// A separate thread for the heavy jogging calculations. -bool JogArm::startJogCalcThread() -{ - if (!jog_calcs_) - jog_calcs_ = - std::make_unique(ros_parameters_, planning_scene_monitor_->getRobotModelLoader(), - command_deltas_queue_, joint_command_deltas_queue_, outgoing_command_queue_); - - jog_calc_thread_ = std::thread([&]() { jog_calcs_->run(shared_variables_); }); - - return true; -} - -// A separate thread for collision checking. -bool JogArm::startCollisionCheckThread() -{ - if (!collision_checker_) - collision_checker_ = std::make_unique(ros_parameters_, planning_scene_monitor_); - - collision_check_thread_ = std::thread([&]() { collision_checker_->run(shared_variables_); }); - - return true; -} - void JogArm::provideTwistStampedCommand(const geometry_msgs::TwistStamped& msg) { command_deltas_queue_->push(msg); @@ -479,7 +458,7 @@ sensor_msgs::JointState JogArm::getJointState() bool JogArm::getCommandFrameTransform(Eigen::Isometry3d& transform) { - if (!jog_calcs_ || !jog_calcs_->isInitialized()) + if (!jog_calcs_->isInitialized()) return false; return jog_calcs_->getCommandFrameTransform(transform); } diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index acbb153fea..46e123b367 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -65,22 +65,34 @@ bool isNonZero(const control_msgs::JointJog& msg) } // namespace // Constructor for the class that handles jogging calculations -JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr, +JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, JogArmShared& shared_variables, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::shared_ptr& command_deltas_queue, const std::shared_ptr& joint_command_deltas_queue, const std::shared_ptr& outgoing_command_queue) - : command_deltas_queue_(command_deltas_queue) + : nh_(nh) + , parameters_(parameters) + , shared_variables_(shared_variables) + , planning_scene_monitor_(planning_scene_monitor) + , command_deltas_queue_(command_deltas_queue) , joint_command_deltas_queue_(joint_command_deltas_queue) , outgoing_command_queue_(outgoing_command_queue) - , parameters_(parameters) - , default_sleep_rate_(1000) + , period_(parameters.publish_period) { - ros::NodeHandle nh; - // Publish jogger status - status_pub_ = nh.advertise(parameters_.status_topic, 1); + status_pub_ = nh_.advertise(parameters_.status_topic, 1); + + // subscribe to joints + joint_state_sub_ = nh_.subscribe(parameters.joint_topic, 1, &JogCalcs::jointStateCB, this); +} + +void JogCalcs::init() +{ + // Reset flags + is_initialized_ = false; // MoveIt Setup + const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr = planning_scene_monitor_->getRobotModelLoader(); while (ros::ok() && !model_loader_ptr) { ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); @@ -93,15 +105,6 @@ JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader: joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getActiveJointModels().size()); - // subscribe to joints - joint_state_sub_ = nh.subscribe(parameters.joint_topic, 1, &JogCalcs::jointStateCB, this); -} - -void JogCalcs::run(JogArmShared& shared_variables) -{ - // Reset flags - is_initialized_ = false; - // Wait for initial messages ROS_INFO_NAMED(LOGNAME, "jog_calcs_thread: Waiting for first joint msg."); ros::topic::waitForMessage(parameters_.joint_topic); @@ -124,160 +127,170 @@ void JogCalcs::run(JogArmShared& shared_variables) } // Initialize the position filters to initial robot joints - while (!updateJoints(shared_variables) && ros::ok()) + while (!updateJoints(shared_variables_) && ros::ok()) { - if (shared_variables.stop_requested) + if (shared_variables_.stop_requested) return; default_sleep_rate_.sleep(); } + // reset flags + wait_for_jog_commands_ = true; + have_nonzero_cartesian_command_ = false; + have_nonzero_joint_command_ = false; + have_nonzero_command_ = false; + + // initialization is finished is_initialized_ = true; +} - // Track the number of cycles during which motion has not occurred. - // Will avoid re-publishing zero velocities endlessly. - int zero_velocity_count = 0; +void JogCalcs::start() +{ + init(); - ros::Rate loop_rate(1. / parameters_.publish_period); + timer_ = nh_.createTimer(period_, &JogCalcs::run, this); +} - // Flag for staying inactive while there are no incoming commands - bool wait_for_jog_commands = true; +void JogCalcs::stop() +{ + timer_.stop(); +} - // Incoming command messages - geometry_msgs::TwistStamped cartesian_deltas; - control_msgs::JointJog joint_deltas; +void JogCalcs::run(const ros::TimerEvent& timer_event) +{ + // Log last loop duration and warn if it was longer than period + if (timer_event.profile.last_duration.toSec() < period_.toSec()) + { + ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " (" + << period_.toSec() << ")"); + } + else + { + ROS_WARN_STREAM_THROTTLE_NAMED(1, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " > " + << period_.toSec()); + } - // Nonzero status flags - bool have_nonzero_cartesian_command = false; - bool have_nonzero_joint_command = false; - bool have_nonzero_command = false; + // Always update the joints and end-effector transform for 2 reasons: + // 1) in case the getCommandFrameTransform() method is being used + // 2) so the low-pass filters are up to date and don't cause a jump + while (!updateJoints(shared_variables_) && ros::ok()) + { + default_sleep_rate_.sleep(); + } - // Do jogging calcs - while (ros::ok() && !shared_variables.stop_requested) + // Get the transform from MoveIt planning frame to jogging command frame + // We solve (planning_frame -> base -> robot_link_command_frame) + // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) { - // Always update the joints and end-effector transform for 2 reasons: - // 1) in case the getCommandFrameTransform() method is being used - // 2) so the low-pass filters are up to date and don't cause a jump - while (!updateJoints(shared_variables) && ros::ok()) + const std::lock_guard lock(latest_state_mutex_); + kinematic_state_->setVariableValues(incoming_joint_state_); + } + tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); + + // Read the latest cartesian commands out of the queues + bool have_new_cartesian_command = popLatest(*command_deltas_queue_, cartesian_deltas_); + bool have_new_joint_command = popLatest(*joint_command_deltas_queue_, joint_deltas_); + + if (have_new_cartesian_command) + { + // Update nonzero flags for updated messages + have_nonzero_cartesian_command_ = isNonZero(cartesian_deltas_); + + // Input frame determined by YAML file if not passed with message + if (cartesian_deltas_.header.frame_id.empty()) { - default_sleep_rate_.sleep(); + cartesian_deltas_.header.frame_id = parameters_.robot_link_command_frame; } + } + if (have_new_joint_command) + { + // Update nonzero flags for updated messages + have_nonzero_joint_command_ = isNonZero(joint_deltas_); - // Get the transform from MoveIt planning frame to jogging command frame - // We solve (planning_frame -> base -> robot_link_command_frame) - // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) + // Input frame determined by YAML file if not passed with message + if (cartesian_deltas_.header.frame_id.empty()) { - const std::lock_guard lock(latest_state_mutex_); - kinematic_state_->setVariableValues(incoming_joint_state_); + joint_deltas_.header.frame_id = parameters_.robot_link_command_frame; } - tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * - kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); + } + have_nonzero_command_ = have_nonzero_cartesian_command_ || have_nonzero_joint_command_; - // Read the latest cartesian commands out of the queues - bool have_new_cartesian_command = popLatest(*command_deltas_queue_, cartesian_deltas); - bool have_new_joint_command = popLatest(*joint_command_deltas_queue_, joint_deltas); + // If paused or while waiting for initial jog commands, just keep the low-pass filters up to date with current + // joints so a jump doesn't occur when restarting + if (wait_for_jog_commands_ || shared_variables_.paused) + { + for (std::size_t i = 0; i < num_joints_; ++i) + position_filters_[i].reset(original_joint_state_.position[i]); - if (have_new_cartesian_command) + // Check if there are any new commands with valid timestamp + wait_for_jog_commands_ = + cartesian_deltas_.header.stamp == ros::Time(0.) && joint_deltas_.header.stamp == ros::Time(0.); + } + // If not waiting for initial command, and not paused. + // Do jogging calculations only if the robot should move, for efficiency + else + { + if (!shared_variables_.command_is_stale) { - // Update nonzero flags for updated messages - have_nonzero_cartesian_command = isNonZero(cartesian_deltas); - - // Input frame determined by YAML file if not passed with message - if (cartesian_deltas.header.frame_id.empty()) + // Prioritize cartesian jogging above joint jogging + if (have_nonzero_cartesian_command_) { - cartesian_deltas.header.frame_id = parameters_.robot_link_command_frame; + if (!cartesianJogCalcs(cartesian_deltas_, shared_variables_)) + return; } - } - if (have_new_joint_command) - { - // Update nonzero flags for updated messages - have_nonzero_joint_command = isNonZero(joint_deltas); - - // Input frame determined by YAML file if not passed with message - if (cartesian_deltas.header.frame_id.empty()) + else if (have_nonzero_joint_command_) { - joint_deltas.header.frame_id = parameters_.robot_link_command_frame; + if (!jointJogCalcs(joint_deltas_, shared_variables_)) + return; } } - have_nonzero_command = have_nonzero_cartesian_command || have_nonzero_joint_command; - // If paused or while waiting for initial jog commands, just keep the low-pass filters up to date with current - // joints so a jump doesn't occur when restarting - if (wait_for_jog_commands || shared_variables.paused) + // If we should halt + if (!have_nonzero_command_) { + // Keep the joint position filters up-to-date with current joints for (std::size_t i = 0; i < num_joints_; ++i) position_filters_[i].reset(original_joint_state_.position[i]); - // Check if there are any new commands with valid timestamp - wait_for_jog_commands = - cartesian_deltas.header.stamp == ros::Time(0.) && joint_deltas.header.stamp == ros::Time(0.); + suddenHalt(outgoing_command_); + have_nonzero_cartesian_command_ = false; + have_nonzero_joint_command_ = false; } - // If not waiting for initial command, and not paused. - // Do jogging calculations only if the robot should move, for efficiency + + // Send the newest target joints + // If everything normal, share the new traj to be published + if (have_nonzero_command_) + { + outgoing_command_queue_->push(outgoing_command_); + // shared_variables.outgoing_command = outgoing_command_; + shared_variables_.ok_to_publish = true; + } + // Skip the jogging publication if all inputs have been zero for several cycles in a row. + // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. + else if ((parameters_.num_outgoing_halt_msgs_to_publish != 0) && + (zero_velocity_count_ > parameters_.num_outgoing_halt_msgs_to_publish)) + { + shared_variables_.ok_to_publish = false; + } + // The command is invalid but we are publishing num_outgoing_halt_msgs_to_publish else { - if (!shared_variables.command_is_stale) - { - // Prioritize cartesian jogging above joint jogging - if (have_nonzero_cartesian_command) - { - if (!cartesianJogCalcs(cartesian_deltas, shared_variables)) - continue; - } - else if (have_nonzero_joint_command) - { - if (!jointJogCalcs(joint_deltas, shared_variables)) - continue; - } - } - - // If we should halt - if (!have_nonzero_command) - { - // Keep the joint position filters up-to-date with current joints - for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(original_joint_state_.position[i]); - - suddenHalt(outgoing_command_); - have_nonzero_cartesian_command = false; - have_nonzero_joint_command = false; - } - - // Send the newest target joints - // If everything normal, share the new traj to be published - if (have_nonzero_command) - { - outgoing_command_queue_->push(outgoing_command_); - // shared_variables.outgoing_command = outgoing_command_; - shared_variables.ok_to_publish = true; - } - // Skip the jogging publication if all inputs have been zero for several cycles in a row. - // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. - else if ((parameters_.num_outgoing_halt_msgs_to_publish != 0) && - (zero_velocity_count > parameters_.num_outgoing_halt_msgs_to_publish)) - { - shared_variables.ok_to_publish = false; - } - // The command is invalid but we are publishing num_outgoing_halt_msgs_to_publish - else - { - outgoing_command_queue_->push(outgoing_command_); - // shared_variables.outgoing_command = outgoing_command_; - shared_variables.ok_to_publish = true; - } - - // Store last zero-velocity message flag to prevent superfluous warnings. - // Cartesian and joint commands must both be zero. - if (!have_nonzero_cartesian_command && !have_nonzero_joint_command) - { - // Avoid overflow - if (zero_velocity_count < std::numeric_limits::max()) - ++zero_velocity_count; - } - else - zero_velocity_count = 0; + outgoing_command_queue_->push(outgoing_command_); + // shared_variables.outgoing_command = outgoing_command_; + shared_variables_.ok_to_publish = true; } - loop_rate.sleep(); + // Store last zero-velocity message flag to prevent superfluous warnings. + // Cartesian and joint commands must both be zero. + if (!have_nonzero_cartesian_command_ && !have_nonzero_joint_command_) + { + // Avoid overflow + if (zero_velocity_count_ < std::numeric_limits::max()) + ++zero_velocity_count_; + } + else + zero_velocity_count_ = 0; } } diff --git a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp index ffc7dc8b3c..5593117d83 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp @@ -42,7 +42,7 @@ namespace { constexpr char LOGNAME[] = "jog_server"; -constexpr char ROS_THREADS = 4; +constexpr char ROS_THREADS = 8; struct JogServerParameters { @@ -113,7 +113,7 @@ int main(int argc, char** argv) planning_scene_monitor->startStateMonitor(); // Create the jog server - moveit_jog_arm::JogArm jog_arm(planning_scene_monitor); + moveit_jog_arm::JogArm jog_arm(nh, planning_scene_monitor); // ROS subscriptions. Share the data with the worker threads auto cmd_sub = diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp index fea4d4cf16..cbd25a1d42 100644 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp @@ -57,8 +57,6 @@ class TestJogCppInterface : public ::testing::Test public: void SetUp() override { - nh_.reset(new ros::NodeHandle("~")); - // Load the planning scene monitor planning_scene_monitor_ = std::make_shared("robot_description"); planning_scene_monitor_->startSceneMonitor(); @@ -69,13 +67,17 @@ class TestJogCppInterface : public ::testing::Test } protected: - std::unique_ptr nh_; + ros::NodeHandle nh_; planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; }; // class TestJogCppInterface TEST_F(TestJogCppInterface, InitTest) { - moveit_jog_arm::JogArm jog_arm(planning_scene_monitor_); + moveit_jog_arm::JogArm jog_arm(nh_, planning_scene_monitor_); + jog_arm.start(); + ros::Duration(1).sleep(); // Give the started thread some time to run + jog_arm.stop(); + ros::Duration(1).sleep(); // Give the started thread some time to run jog_arm.start(); ros::Duration(1).sleep(); // Give the started thread some time to run jog_arm.stop(); @@ -92,7 +94,7 @@ int main(int argc, char** argv) testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "jog_cpp_interface_test_test"); - ros::AsyncSpinner spinner(1); + ros::AsyncSpinner spinner(8); spinner.start(); int result = RUN_ALL_TESTS(); From 4b2b1d3712f9ab3971b4eb883f6d908a9a103289 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 27 May 2020 01:34:07 -0600 Subject: [PATCH 318/612] use built in zero copy message passing instead of spsc_queues --- .../include/moveit_jog_arm/jog_arm.h | 20 ++-- .../include/moveit_jog_arm/jog_arm_data.h | 29 ----- .../include/moveit_jog_arm/jog_calcs.h | 40 +++---- .../cpp_interface_example.cpp | 25 ++-- .../moveit_jog_arm/src/jog_arm.cpp | 92 ++++++--------- .../moveit_jog_arm/src/jog_calcs.cpp | 107 ++++++++++-------- .../moveit_jog_arm/src/jog_server.cpp | 58 ---------- 7 files changed, 134 insertions(+), 237 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h index c8c7cea178..27e689e9b2 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h @@ -95,11 +95,9 @@ class JogArm /** \brief Provide a Cartesian velocity command to the jogger. * The units are determined by settings in the yaml file. */ - void provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command); void provideTwistStampedCommand(const geometry_msgs::TwistStampedConstPtr& msg); /** \brief Send joint position(s) commands */ - void provideJointCommand(const control_msgs::JointJog& joint_command); void provideJointCommand(const control_msgs::JointJogConstPtr& msg); /** @@ -129,7 +127,7 @@ class JogArm private: bool readParameters(); void run(const ros::TimerEvent& timer_event); - void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); + void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryPtr& msg); ros::NodeHandle nh_; @@ -142,11 +140,6 @@ class JogArm // Share data between threads JogArmShared shared_variables_; - // Queues for message passing between threads - std::shared_ptr command_deltas_queue_; - std::shared_ptr joint_command_deltas_queue_; - std::shared_ptr outgoing_command_queue_; - // Jog calcs std::unique_ptr jog_calcs_; @@ -156,13 +149,16 @@ class JogArm // ROS ros::Timer timer_; ros::Duration period_; - ros::Subscriber joint_state_sub_; ros::Publisher outgoing_cmd_pub_; + ros::Publisher twist_stamped_pub_; + ros::Publisher joint_jog_pub_; + ros::ServiceServer drift_dimensions_server_; + ros::ServiceServer dims_server_; + ros::Subscriber joint_trajectory_sub_; - // Latest state + // latest_state_mutex_ is used to protect the state below it mutable std::mutex latest_state_mutex_; - sensor_msgs::JointState latest_joint_state_; - ros::Time latest_command_stamp_; + trajectory_msgs::JointTrajectoryPtr latest_joint_trajectory_; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index be168098c4..4f671ce36f 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -38,9 +38,6 @@ #pragma once -// Boost -#include - // Eigen #include @@ -54,32 +51,6 @@ namespace moveit_jog_arm { -// Types for queues between threads -constexpr size_t QUEUE_SIZE = 10; -template -using MessageQueue = boost::lockfree::spsc_queue>; -using TwistedStampedQueue = MessageQueue; -using JointJogQueue = MessageQueue; -using JointTrajectoryQueue = MessageQueue; - -// Helper function for popping the latest value out of a queue -template -bool popLatest(MessageQueue& queue, T& output) -{ - size_t count = queue.read_available(); - if (count > 0) - { - // clear all but the latest from the queue - for (size_t i = 0; i < (count - 1); ++i) - { - queue.pop(); - } - // read the latest value - queue.pop(output); - } - return true; -} - // Variables to share between threads // Be careful to not read-modify-write any of these because they are not // protected by a mutex. diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 8aa79cc836..87b0e9c162 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -58,10 +58,7 @@ class JogCalcs { public: JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, JogArmShared& shared_variables, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& command_deltas_queue, - const std::shared_ptr& joint_command_deltas_queue, - const std::shared_ptr& outgoing_command_queue); + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); /** \breif Start and stop the timer (thread) */ void start(); @@ -173,6 +170,10 @@ class JogCalcs /* \brief Callback for joint subsription */ void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); + /* \breif Command callbacks */ + void twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg); + void jointJogCB(const control_msgs::JointJogConstPtr& msg); + // ROS node handle ros::NodeHandle nh_; @@ -196,30 +197,20 @@ class JogCalcs bool wait_for_jog_commands_ = true; // Nonzero status flags - bool have_nonzero_cartesian_command_ = false; - bool have_nonzero_joint_command_ = false; + bool have_nonzero_twist_stamped_ = false; + bool have_nonzero_joint_jog_ = false; bool have_nonzero_command_ = false; // Incoming command messages - geometry_msgs::TwistStamped cartesian_deltas_; - control_msgs::JointJog joint_deltas_; + geometry_msgs::TwistStamped twist_stamped_; + control_msgs::JointJog joint_jog_; const moveit::core::JointModelGroup* joint_model_group_; moveit::core::RobotStatePtr kinematic_state_; - // Queues for message passing between threads - std::shared_ptr command_deltas_queue_; - std::shared_ptr joint_command_deltas_queue_; - std::shared_ptr outgoing_command_queue_; - - // latest_state_mutex_ is used to protect - // incoming_joint_state_ and tf_moveit_to_cmd_frame_ - mutable std::mutex latest_state_mutex_; // incoming_joint_state_ is the incoming message. It may contain passive joints or other joints we don't care about. - sensor_msgs::JointState incoming_joint_state_; - Eigen::Isometry3d tf_moveit_to_cmd_frame_; - + // (mutex protected below) // internal_joint_state_ is used in jog calculations. It shouldn't be relied on to be accurate. // original_joint_state_ is the same as incoming_joint_state_ except it only contains the joints jog_arm acts on. sensor_msgs::JointState internal_joint_state_, original_joint_state_; @@ -233,7 +224,10 @@ class JogCalcs ros::Timer timer_; ros::Duration period_; ros::Subscriber joint_state_sub_; + ros::Subscriber twist_stamped_sub_; + ros::Subscriber joint_jog_sub_; ros::Publisher status_pub_; + ros::Publisher joint_trajectory_pub_; StatusCode status_ = NO_WARNING; // Use ArrayXd type to enable more coefficient-wise operations @@ -246,5 +240,13 @@ class JogCalcs // Amount we sleep when waiting ros::Rate default_sleep_rate_ = 1000; + + // latest_state_mutex_ is used to protect the state below it + mutable std::mutex latest_state_mutex_; + sensor_msgs::JointState incoming_joint_state_; + Eigen::Isometry3d tf_moveit_to_cmd_frame_; + geometry_msgs::TwistStampedConstPtr latest_twist_stamped_; + control_msgs::JointJogConstPtr latest_joint_jog_; + ros::Time latest_command_stamp_ = ros::Time(0.); }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index 53c1e971f9..d0a17f39e0 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -74,10 +74,10 @@ int main(int argc, char** argv) jog_arm.start(); // Make a Cartesian velocity message - geometry_msgs::TwistStamped velocity_msg; - velocity_msg.header.frame_id = "base_link"; - velocity_msg.twist.linear.y = 0.01; - velocity_msg.twist.linear.z = -0.01; + auto velocity_msg = boost::make_shared(); + velocity_msg->header.frame_id = "base_link"; + velocity_msg->twist.linear.y = 0.01; + velocity_msg->twist.linear.z = -0.01; ros::Rate cmd_rate(100); uint num_commands = 0; @@ -86,7 +86,7 @@ int main(int argc, char** argv) while (ros::ok() && num_commands < 200) { ++num_commands; - velocity_msg.header.stamp = ros::Time::now(); + velocity_msg->header.stamp = ros::Time::now(); jog_arm.provideTwistStampedCommand(velocity_msg); cmd_rate.sleep(); } @@ -96,26 +96,21 @@ int main(int argc, char** argv) ros::Duration(2).sleep(); // Make a joint command - control_msgs::JointJog base_joint_command; - base_joint_command.joint_names.push_back("elbow_joint"); - base_joint_command.velocities.push_back(0.2); - base_joint_command.header.stamp = ros::Time::now(); + auto base_joint_command = boost::make_shared(); + base_joint_command->joint_names.push_back("elbow_joint"); + base_joint_command->velocities.push_back(0.2); + base_joint_command->header.stamp = ros::Time::now(); // Send a few joint commands num_commands = 0; while (ros::ok() && num_commands < 200) { ++num_commands; - base_joint_command.header.stamp = ros::Time::now(); + base_joint_command->header.stamp = ros::Time::now(); jog_arm.provideJointCommand(base_joint_command); cmd_rate.sleep(); } - // Retrieve the current joint state from the jogger - sensor_msgs::JointState current_joint_state = jog_arm.getJointState(); - ROS_INFO_STREAM_NAMED(LOGNAME, "Current joint state:"); - ROS_INFO_STREAM_NAMED(LOGNAME, current_joint_state); - // Retrieve the current status of the jogger moveit_jog_arm::StatusCode status = jog_arm.getJoggerStatus(); ROS_INFO_STREAM_NAMED(LOGNAME, "Jogger status:\n" << status); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index 29b604bdde..9f3e8280d6 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -53,11 +53,6 @@ JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneM // loop period period_ = ros::Duration(parameters_.publish_period); - // Create queues for message passing between threads - command_deltas_queue_ = std::make_shared(); - joint_command_deltas_queue_ = std::make_shared(); - outgoing_command_queue_ = std::make_shared(); - // Publish freshly-calculated joints to the robot. // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") @@ -65,15 +60,29 @@ JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneM else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, 1); - // subscribe to joints - joint_state_sub_ = nh_.subscribe(parameters_.joint_topic, 1, &JogArm::jointStateCB, this); + // publish commands (for c++ interface) + twist_stamped_pub_ = nh_.advertise(parameters_.cartesian_command_in_topic, 1); + joint_jog_pub_ = nh_.advertise(parameters_.joint_command_in_topic, 1); + + // ROS Server for allowing drift in some dimensions + drift_dimensions_server_ = + nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions", + &JogArm::changeDriftDimensions, this); + + // ROS Server for changing the control dimensions + dims_server_ = + nh_.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", + &JogArm::changeControlDimensions, this); + + // Subscribe to output commands in internal namespace + ros::NodeHandle internal_nh("~internal"); + joint_trajectory_sub_ = internal_nh.subscribe("joint_trajectory", 1, &JogArm::jointTrajectoryCB, this); // Wait for incoming topics to appear ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); ros::topic::waitForMessage(parameters_.joint_topic); - jog_calcs_ = std::make_unique(nh_, parameters_, shared_variables_, planning_scene_monitor_, - command_deltas_queue_, joint_command_deltas_queue_, outgoing_command_queue_); + jog_calcs_ = std::make_unique(nh_, parameters_, shared_variables_, planning_scene_monitor_); collision_checker_ = std::make_unique(nh_, parameters_, shared_variables_, planning_scene_monitor_); @@ -307,18 +316,16 @@ void JogArm::run(const ros::TimerEvent& timer_event) << period_.toSec()); } - trajectory_msgs::JointTrajectory outgoing_command; - - // Check for stale cmds + // Get the latest joint trajectory + trajectory_msgs::JointTrajectory joint_trajectory; { const std::lock_guard lock(latest_state_mutex_); - shared_variables_.command_is_stale = - ((ros::Time::now() - latest_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); + if (latest_joint_trajectory_) + { + joint_trajectory = *latest_joint_trajectory_; + } } - // Read the latest outgoing command out of the queue - popLatest(*outgoing_command_queue_, outgoing_command); - // Publish the most recent trajectory, unless the jogging calculation thread tells not to if (shared_variables_.ok_to_publish) { @@ -326,16 +333,16 @@ void JogArm::run(const ros::TimerEvent& timer_event) // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") { - outgoing_command.header.stamp = ros::Time::now(); - outgoing_cmd_pub_.publish(outgoing_command); + joint_trajectory.header.stamp = ros::Time::now(); + outgoing_cmd_pub_.publish(joint_trajectory); } else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") { std_msgs::Float64MultiArray joints; if (parameters_.publish_joint_positions) - joints.data = outgoing_command.points[0].positions; + joints.data = joint_trajectory.points[0].positions; else if (parameters_.publish_joint_velocities) - joints.data = outgoing_command.points[0].velocities; + joints.data = joint_trajectory.points[0].velocities; outgoing_cmd_pub_.publish(joints); } } @@ -380,13 +387,6 @@ JogArm::~JogArm() stop(); } -// Listen to joint angles. Store them in a shared variable. -void JogArm::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) -{ - const std::lock_guard lock(latest_state_mutex_); - latest_joint_state_ = *msg; -} - bool JogArm::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, moveit_msgs::ChangeDriftDimensions::Response& res) { @@ -415,34 +415,14 @@ bool JogArm::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Reque return true; } -void JogArm::provideTwistStampedCommand(const geometry_msgs::TwistStamped& msg) -{ - command_deltas_queue_->push(msg); - if (msg.header.stamp != ros::Time(0.)) - { - const std::lock_guard lock(latest_state_mutex_); - latest_command_stamp_ = msg.header.stamp; - } -}; - void JogArm::provideTwistStampedCommand(const geometry_msgs::TwistStampedConstPtr& msg) { - provideTwistStampedCommand(*msg); -} - -void JogArm::provideJointCommand(const control_msgs::JointJog& msg) -{ - joint_command_deltas_queue_->push(msg); - if (msg.header.stamp != ros::Time(0.)) - { - const std::lock_guard lock(latest_state_mutex_); - latest_command_stamp_ = msg.header.stamp; - } + twist_stamped_pub_.publish(msg); } void JogArm::provideJointCommand(const control_msgs::JointJogConstPtr& msg) { - provideJointCommand(*msg); + joint_jog_pub_.publish(msg); } void JogArm::setPaused(bool paused) @@ -450,12 +430,6 @@ void JogArm::setPaused(bool paused) shared_variables_.paused = paused; } -sensor_msgs::JointState JogArm::getJointState() -{ - const std::lock_guard lock(latest_state_mutex_); - return latest_joint_state_; -} - bool JogArm::getCommandFrameTransform(Eigen::Isometry3d& transform) { if (!jog_calcs_->isInitialized()) @@ -468,4 +442,10 @@ StatusCode JogArm::getJoggerStatus() const return shared_variables_.status; } +void JogArm::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryPtr& msg) +{ + const std::lock_guard lock(latest_state_mutex_); + latest_joint_trajectory_ = msg; +} + } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 46e123b367..ac45858958 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -66,17 +66,11 @@ bool isNonZero(const control_msgs::JointJog& msg) // Constructor for the class that handles jogging calculations JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, JogArmShared& shared_variables, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& command_deltas_queue, - const std::shared_ptr& joint_command_deltas_queue, - const std::shared_ptr& outgoing_command_queue) + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : nh_(nh) , parameters_(parameters) , shared_variables_(shared_variables) , planning_scene_monitor_(planning_scene_monitor) - , command_deltas_queue_(command_deltas_queue) - , joint_command_deltas_queue_(joint_command_deltas_queue) - , outgoing_command_queue_(outgoing_command_queue) , period_(parameters.publish_period) { // Publish jogger status @@ -84,6 +78,14 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, JogA // subscribe to joints joint_state_sub_ = nh_.subscribe(parameters.joint_topic, 1, &JogCalcs::jointStateCB, this); + + // Subscribe to command topics + twist_stamped_sub_ = nh_.subscribe(parameters.cartesian_command_in_topic, 1, &JogCalcs::twistStampedCB, this); + joint_jog_sub_ = nh.subscribe(parameters.joint_command_in_topic, 1, &JogCalcs::jointJogCB, this); + + // Publish output commands to internal namespace + ros::NodeHandle internal_nh("~internal"); + joint_trajectory_pub_ = internal_nh.advertise("joint_trajectory", 1); } void JogCalcs::init() @@ -136,8 +138,8 @@ void JogCalcs::init() // reset flags wait_for_jog_commands_ = true; - have_nonzero_cartesian_command_ = false; - have_nonzero_joint_command_ = false; + have_nonzero_twist_stamped_ = false; + have_nonzero_joint_jog_ = false; have_nonzero_command_ = false; // initialization is finished @@ -178,43 +180,37 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) default_sleep_rate_.sleep(); } - // Get the transform from MoveIt planning frame to jogging command frame - // We solve (planning_frame -> base -> robot_link_command_frame) - // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) + // Update from latest state { const std::lock_guard lock(latest_state_mutex_); kinematic_state_->setVariableValues(incoming_joint_state_); + if (latest_twist_stamped_) + twist_stamped_ = *latest_twist_stamped_; + if (latest_joint_jog_) + joint_jog_ = *latest_joint_jog_; + + // Check for stale cmds + shared_variables_.command_is_stale = + ((ros::Time::now() - latest_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); } + + // Get the transform from MoveIt planning frame to jogging command frame + // We solve (planning_frame -> base -> robot_link_command_frame) + // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); - // Read the latest cartesian commands out of the queues - bool have_new_cartesian_command = popLatest(*command_deltas_queue_, cartesian_deltas_); - bool have_new_joint_command = popLatest(*joint_command_deltas_queue_, joint_deltas_); - - if (have_new_cartesian_command) + // Input frame determined by YAML file if not passed with message + if (twist_stamped_.header.frame_id.empty()) { - // Update nonzero flags for updated messages - have_nonzero_cartesian_command_ = isNonZero(cartesian_deltas_); - - // Input frame determined by YAML file if not passed with message - if (cartesian_deltas_.header.frame_id.empty()) - { - cartesian_deltas_.header.frame_id = parameters_.robot_link_command_frame; - } + twist_stamped_.header.frame_id = parameters_.robot_link_command_frame; } - if (have_new_joint_command) + if (twist_stamped_.header.frame_id.empty()) { - // Update nonzero flags for updated messages - have_nonzero_joint_command_ = isNonZero(joint_deltas_); - - // Input frame determined by YAML file if not passed with message - if (cartesian_deltas_.header.frame_id.empty()) - { - joint_deltas_.header.frame_id = parameters_.robot_link_command_frame; - } + joint_jog_.header.frame_id = parameters_.robot_link_command_frame; } - have_nonzero_command_ = have_nonzero_cartesian_command_ || have_nonzero_joint_command_; + + have_nonzero_command_ = have_nonzero_twist_stamped_ || have_nonzero_joint_jog_; // If paused or while waiting for initial jog commands, just keep the low-pass filters up to date with current // joints so a jump doesn't occur when restarting @@ -224,8 +220,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) position_filters_[i].reset(original_joint_state_.position[i]); // Check if there are any new commands with valid timestamp - wait_for_jog_commands_ = - cartesian_deltas_.header.stamp == ros::Time(0.) && joint_deltas_.header.stamp == ros::Time(0.); + wait_for_jog_commands_ = twist_stamped_.header.stamp == ros::Time(0.) && joint_jog_.header.stamp == ros::Time(0.); } // If not waiting for initial command, and not paused. // Do jogging calculations only if the robot should move, for efficiency @@ -234,14 +229,14 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) if (!shared_variables_.command_is_stale) { // Prioritize cartesian jogging above joint jogging - if (have_nonzero_cartesian_command_) + if (have_nonzero_twist_stamped_) { - if (!cartesianJogCalcs(cartesian_deltas_, shared_variables_)) + if (!cartesianJogCalcs(twist_stamped_, shared_variables_)) return; } - else if (have_nonzero_joint_command_) + else if (have_nonzero_joint_jog_) { - if (!jointJogCalcs(joint_deltas_, shared_variables_)) + if (!jointJogCalcs(joint_jog_, shared_variables_)) return; } } @@ -254,16 +249,15 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) position_filters_[i].reset(original_joint_state_.position[i]); suddenHalt(outgoing_command_); - have_nonzero_cartesian_command_ = false; - have_nonzero_joint_command_ = false; + have_nonzero_twist_stamped_ = false; + have_nonzero_joint_jog_ = false; } // Send the newest target joints // If everything normal, share the new traj to be published if (have_nonzero_command_) { - outgoing_command_queue_->push(outgoing_command_); - // shared_variables.outgoing_command = outgoing_command_; + joint_trajectory_pub_.publish(outgoing_command_); shared_variables_.ok_to_publish = true; } // Skip the jogging publication if all inputs have been zero for several cycles in a row. @@ -276,14 +270,13 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // The command is invalid but we are publishing num_outgoing_halt_msgs_to_publish else { - outgoing_command_queue_->push(outgoing_command_); - // shared_variables.outgoing_command = outgoing_command_; + joint_trajectory_pub_.publish(outgoing_command_); shared_variables_.ok_to_publish = true; } // Store last zero-velocity message flag to prevent superfluous warnings. // Cartesian and joint commands must both be zero. - if (!have_nonzero_cartesian_command_ && !have_nonzero_joint_command_) + if (!have_nonzero_twist_stamped_ && !have_nonzero_joint_jog_) { // Avoid overflow if (zero_velocity_count_ < std::numeric_limits::max()) @@ -952,4 +945,22 @@ bool JogCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) return !transform.matrix().isZero(0); } +void JogCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg) +{ + const std::lock_guard lock(latest_state_mutex_); + latest_twist_stamped_ = msg; + have_nonzero_twist_stamped_ = isNonZero(*latest_twist_stamped_); + if (msg->header.stamp != ros::Time(0.)) + latest_command_stamp_ = msg->header.stamp; +} + +void JogCalcs::jointJogCB(const control_msgs::JointJogConstPtr& msg) +{ + const std::lock_guard lock(latest_state_mutex_); + latest_joint_jog_ = msg; + have_nonzero_joint_jog_ = isNonZero(*latest_joint_jog_); + if (msg->header.stamp != ros::Time(0.)) + latest_command_stamp_ = msg->header.stamp; +} + } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp index 5593117d83..e33f2907a6 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_server.cpp @@ -44,41 +44,6 @@ namespace constexpr char LOGNAME[] = "jog_server"; constexpr char ROS_THREADS = 8; -struct JogServerParameters -{ - bool read_error = true; - std::string cartesian_command_in_topic = ""; - std::string joint_command_in_topic = ""; -}; - -JogServerParameters readParameters() -{ - ros::NodeHandle nh; - JogServerParameters params; - - // Load parameters - std::string parameter_ns; - ros::param::get("~parameter_ns", parameter_ns); - - if (parameter_ns.empty()) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "A namespace must be specified in the launch file, like:"); - ROS_ERROR_STREAM_NAMED(LOGNAME, ""); - params.read_error = true; - } - else - { - std::size_t error = 0; - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/cartesian_command_in_topic", - params.cartesian_command_in_topic); - error += !rosparam_shortcuts::get("", nh, parameter_ns + "/joint_command_in_topic", params.joint_command_in_topic); - params.read_error = (error != 0); - } - return params; -} - } // namespace int main(int argc, char** argv) @@ -89,13 +54,6 @@ int main(int argc, char** argv) ros::NodeHandle nh; - const auto params = readParameters(); - if (params.read_error) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Error reading parameters."); - exit(EXIT_FAILURE); - } - // Load the planning scene monitor auto planning_scene_monitor = std::make_shared("robot_description"); if (!planning_scene_monitor->getPlanningScene()) @@ -115,22 +73,6 @@ int main(int argc, char** argv) // Create the jog server moveit_jog_arm::JogArm jog_arm(nh, planning_scene_monitor); - // ROS subscriptions. Share the data with the worker threads - auto cmd_sub = - nh.subscribe(params.cartesian_command_in_topic, 1, &moveit_jog_arm::JogArm::provideTwistStampedCommand, &jog_arm); - auto joint_jog_cmd_sub = - nh.subscribe(params.joint_command_in_topic, 1, &moveit_jog_arm::JogArm::provideJointCommand, &jog_arm); - - // ROS Server for allowing drift in some dimensions - auto drift_dimensions_server = - nh.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions", - &moveit_jog_arm::JogArm::changeDriftDimensions, &jog_arm); - - // ROS Server for changing the control dimensions - auto dims_server = - nh.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", - &moveit_jog_arm::JogArm::changeControlDimensions, &jog_arm); - // Start the jog server (runs in the ros spinner) jog_arm.start(); From 485f98bc93fd5afabd814cc50623e5db5da3453e Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 27 May 2020 01:37:19 -0600 Subject: [PATCH 319/612] Add Tyler Weaver as an author to jog_arm --- moveit_experimental/moveit_jog_arm/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_experimental/moveit_jog_arm/package.xml b/moveit_experimental/moveit_jog_arm/package.xml index 0a9858261c..96d38fe627 100644 --- a/moveit_experimental/moveit_jog_arm/package.xml +++ b/moveit_experimental/moveit_jog_arm/package.xml @@ -16,6 +16,7 @@ Andy Zelenak Blake Anderson Alexander Rössler + Tyler Weaver catkin From 81e7460cbb52dcdaedf4a2661d9cd621cb7603b9 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 27 May 2020 09:46:04 -0600 Subject: [PATCH 320/612] update cpp_interface_example for jog_arm --- .../cpp_interface_example.cpp | 38 +++++++++++-------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index d0a17f39e0..10d1ede113 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -73,42 +73,48 @@ int main(int argc, char** argv) moveit_jog_arm::JogArm jog_arm(nh, planning_scene_monitor); jog_arm.start(); - // Make a Cartesian velocity message - auto velocity_msg = boost::make_shared(); - velocity_msg->header.frame_id = "base_link"; - velocity_msg->twist.linear.y = 0.01; - velocity_msg->twist.linear.z = -0.01; - ros::Rate cmd_rate(100); uint num_commands = 0; // Send a few Cartesian velocity commands while (ros::ok() && num_commands < 200) { - ++num_commands; + // Make a Cartesian velocity message + // Messages are sent to jogger as boost::shared_ptr to enable zero-copy message_passing. + // Because this message is not coppied we should not modify it after we send it. + auto velocity_msg = boost::make_shared(); velocity_msg->header.stamp = ros::Time::now(); + velocity_msg->header.frame_id = "base_link"; + velocity_msg->twist.linear.y = 0.01; + velocity_msg->twist.linear.z = -0.01; + + // Send the message jog_arm.provideTwistStampedCommand(velocity_msg); cmd_rate.sleep(); + ++num_commands; } // Leave plenty of time for the jogger to halt its previous motion. // For a faster response, adjust the incoming_command_timeout yaml parameter ros::Duration(2).sleep(); - // Make a joint command - auto base_joint_command = boost::make_shared(); - base_joint_command->joint_names.push_back("elbow_joint"); - base_joint_command->velocities.push_back(0.2); - base_joint_command->header.stamp = ros::Time::now(); - // Send a few joint commands num_commands = 0; while (ros::ok() && num_commands < 200) { - ++num_commands; - base_joint_command->header.stamp = ros::Time::now(); - jog_arm.provideJointCommand(base_joint_command); + // Make a joint command + // Messages are sent to jogger as boost::shared_ptr to enable zero-copy message_passing. + // Because this message is not coppied we should not modify it after we send it. + auto elbow_joint_command = boost::make_shared(); + elbow_joint_command->header.stamp = ros::Time::now(); + elbow_joint_command->joint_names.push_back("elbow_joint"); + elbow_joint_command->velocities.push_back(0.2); + elbow_joint_command->header.stamp = ros::Time::now(); + + // Send the message + jog_arm.provideJointCommand(elbow_joint_command); cmd_rate.sleep(); + ++num_commands; } // Retrieve the current status of the jogger From 8e61efa5dbb9a4085a234c4ebf7785032fc6ac67 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 27 May 2020 10:41:50 -0600 Subject: [PATCH 321/612] remove frame_id fixup for joint_jog command --- moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index ac45858958..ebf42d2757 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -205,10 +205,6 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) { twist_stamped_.header.frame_id = parameters_.robot_link_command_frame; } - if (twist_stamped_.header.frame_id.empty()) - { - joint_jog_.header.frame_id = parameters_.robot_link_command_frame; - } have_nonzero_command_ = have_nonzero_twist_stamped_ || have_nonzero_joint_jog_; From a0abf8b721c9ca8ee30b0b670b46c8536f5e33d9 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 27 May 2020 14:39:50 -0600 Subject: [PATCH 322/612] move server into calcs object and remove unused variables from shared_variables --- .../include/moveit_jog_arm/jog_arm.h | 28 --------- .../include/moveit_jog_arm/jog_arm_data.h | 7 +-- .../include/moveit_jog_arm/jog_calcs.h | 29 ++++++++- .../cpp_interface_example.cpp | 1 - .../moveit_jog_arm/src/jog_arm.cpp | 41 ------------- .../moveit_jog_arm/src/jog_calcs.cpp | 60 ++++++++++++++----- 6 files changed, 74 insertions(+), 92 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h index 27e689e9b2..5c76645ba9 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h @@ -42,8 +42,6 @@ #include #include -#include -#include #include #include #include @@ -76,22 +74,6 @@ class JogArm /** \brief Pause or unpause processing jog commands while keeping the threads alive */ void setPaused(bool paused); - /** - * Allow drift in certain dimensions. For example, may allow the wrist to rotate freely. - * This can help avoid singularities. - * - * @param request the service request - * @param response the service response - * @return true if the adjustment was made - */ - bool changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, - moveit_msgs::ChangeDriftDimensions::Response& res); - - /** \brief Start the main calculation thread */ - // Service callback for changing jogging dimensions - bool changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, - moveit_msgs::ChangeControlDimensions::Response& res); - /** \brief Provide a Cartesian velocity command to the jogger. * The units are determined by settings in the yaml file. */ @@ -100,14 +82,6 @@ class JogArm /** \brief Send joint position(s) commands */ void provideJointCommand(const control_msgs::JointJogConstPtr& msg); - /** - * Returns the most recent JointState that the jogger has received. - * May eliminate the need to create your own joint_state subscriber. - * - * @return the most recent joints known to the jogger - */ - sensor_msgs::JointState getJointState(); - /** * Get the MoveIt planning link transform. * The transform from the MoveIt planning frame to robot_link_command_frame @@ -152,8 +126,6 @@ class JogArm ros::Publisher outgoing_cmd_pub_; ros::Publisher twist_stamped_pub_; ros::Publisher joint_jog_pub_; - ros::ServiceServer drift_dimensions_server_; - ros::ServiceServer dims_server_; ros::Subscriber joint_trajectory_sub_; // latest_state_mutex_ is used to protect the state below it diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 4f671ce36f..d69552473b 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -74,7 +74,7 @@ struct JogArmShared bool ok_to_publish = false; // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw] - bool drift_dimensions[6] = { false, false, false, false, false, false }; + std::array drift_dimensions = { { false, false, false, false, false, false } }; // Status of the jogger. 0 for no warning. The meaning of nonzero values can be seen in status_codes.h StatusCode status = NO_WARNING; @@ -82,11 +82,8 @@ struct JogArmShared // Pause/unpause jog threads - threads are not paused by default bool paused = false; - // Stop jog loop threads - threads are not stopped by default - bool stop_requested = false; - // The dimesions to control. In the command frame. [x, y, z, roll, pitch, yaw] - bool control_dimensions[6] = { true, true, true, true, true, true }; + std::array control_dimensions = { { true, true, true, true, true, true } }; }; // ROS params to be read. See the yaml file in /config for a description of each. diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 87b0e9c162..a4577b4ddf 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -44,6 +44,8 @@ // ROS #include #include +#include +#include #include #include @@ -60,7 +62,7 @@ class JogCalcs JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, JogArmShared& shared_variables, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - /** \breif Start and stop the timer (thread) */ + /** \brief Start and stop the timer (thread) */ void start(); void stop(); @@ -79,7 +81,7 @@ class JogCalcs bool getCommandFrameTransform(Eigen::Isometry3d& transform); private: - /** \breif Init must be called once before run */ + /** \brief Init must be called once before run */ void init(); /** \brief Timer method */ @@ -170,10 +172,26 @@ class JogCalcs /* \brief Callback for joint subsription */ void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); - /* \breif Command callbacks */ + /* \brief Command callbacks */ void twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg); void jointJogCB(const control_msgs::JointJogConstPtr& msg); + /** + * Allow drift in certain dimensions. For example, may allow the wrist to rotate freely. + * This can help avoid singularities. + * + * @param request the service request + * @param response the service response + * @return true if the adjustment was made + */ + bool changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, + moveit_msgs::ChangeDriftDimensions::Response& res); + + /** \brief Start the main calculation thread */ + // Service callback for changing jogging dimensions + bool changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, + moveit_msgs::ChangeControlDimensions::Response& res); + // ROS node handle ros::NodeHandle nh_; @@ -228,7 +246,12 @@ class JogCalcs ros::Subscriber joint_jog_sub_; ros::Publisher status_pub_; ros::Publisher joint_trajectory_pub_; + ros::ServiceServer drift_dimensions_server_; + ros::ServiceServer control_dimensions_server_; + + // Status StatusCode status_ = NO_WARNING; + bool stop_requested_ = false; // Use ArrayXd type to enable more coefficient-wise operations Eigen::ArrayXd delta_theta_; diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index 10d1ede113..18dca5af16 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -109,7 +109,6 @@ int main(int argc, char** argv) elbow_joint_command->header.stamp = ros::Time::now(); elbow_joint_command->joint_names.push_back("elbow_joint"); elbow_joint_command->velocities.push_back(0.2); - elbow_joint_command->header.stamp = ros::Time::now(); // Send the message jog_arm.provideJointCommand(elbow_joint_command); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index 9f3e8280d6..0a37345420 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -64,16 +64,6 @@ JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneM twist_stamped_pub_ = nh_.advertise(parameters_.cartesian_command_in_topic, 1); joint_jog_pub_ = nh_.advertise(parameters_.joint_command_in_topic, 1); - // ROS Server for allowing drift in some dimensions - drift_dimensions_server_ = - nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions", - &JogArm::changeDriftDimensions, this); - - // ROS Server for changing the control dimensions - dims_server_ = - nh_.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", - &JogArm::changeControlDimensions, this); - // Subscribe to output commands in internal namespace ros::NodeHandle internal_nh("~internal"); joint_trajectory_sub_ = internal_nh.subscribe("joint_trajectory", 1, &JogArm::jointTrajectoryCB, this); @@ -359,7 +349,6 @@ void JogArm::run(const ros::TimerEvent& timer_event) void JogArm::start() { - shared_variables_.stop_requested = false; shared_variables_.paused = false; // Crunch the numbers in this thread @@ -375,8 +364,6 @@ void JogArm::start() void JogArm::stop() { - shared_variables_.stop_requested = true; - timer_.stop(); jog_calcs_->stop(); collision_checker_->stop(); @@ -387,34 +374,6 @@ JogArm::~JogArm() stop(); } -bool JogArm::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, - moveit_msgs::ChangeDriftDimensions::Response& res) -{ - shared_variables_.drift_dimensions[0] = req.drift_x_translation; - shared_variables_.drift_dimensions[1] = req.drift_y_translation; - shared_variables_.drift_dimensions[2] = req.drift_z_translation; - shared_variables_.drift_dimensions[3] = req.drift_x_rotation; - shared_variables_.drift_dimensions[4] = req.drift_y_rotation; - shared_variables_.drift_dimensions[5] = req.drift_z_rotation; - - res.success = true; - return true; -} - -bool JogArm::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, - moveit_msgs::ChangeControlDimensions::Response& res) -{ - shared_variables_.control_dimensions[0] = req.control_x_translation; - shared_variables_.control_dimensions[1] = req.control_y_translation; - shared_variables_.control_dimensions[2] = req.control_z_translation; - shared_variables_.control_dimensions[3] = req.control_x_rotation; - shared_variables_.control_dimensions[4] = req.control_y_rotation; - shared_variables_.control_dimensions[5] = req.control_z_rotation; - - res.success = true; - return true; -} - void JogArm::provideTwistStampedCommand(const geometry_msgs::TwistStampedConstPtr& msg) { twist_stamped_pub_.publish(msg); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index ebf42d2757..ccc80ed630 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -83,6 +83,16 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, JogA twist_stamped_sub_ = nh_.subscribe(parameters.cartesian_command_in_topic, 1, &JogCalcs::twistStampedCB, this); joint_jog_sub_ = nh.subscribe(parameters.joint_command_in_topic, 1, &JogCalcs::jointJogCB, this); + // ROS Server for allowing drift in some dimensions + drift_dimensions_server_ = + nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions", + &JogCalcs::changeDriftDimensions, this); + + // ROS Server for changing the control dimensions + control_dimensions_server_ = + nh_.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", + &JogCalcs::changeControlDimensions, this); + // Publish output commands to internal namespace ros::NodeHandle internal_nh("~internal"); joint_trajectory_pub_ = internal_nh.advertise("joint_trajectory", 1); @@ -128,26 +138,17 @@ void JogCalcs::init() position_filters_.emplace_back(parameters_.low_pass_filter_coeff); } - // Initialize the position filters to initial robot joints - while (!updateJoints(shared_variables_) && ros::ok()) - { - if (shared_variables_.stop_requested) - return; - default_sleep_rate_.sleep(); - } - - // reset flags + // initialization is finished + is_initialized_ = true; wait_for_jog_commands_ = true; have_nonzero_twist_stamped_ = false; have_nonzero_joint_jog_ = false; have_nonzero_command_ = false; - - // initialization is finished - is_initialized_ = true; } void JogCalcs::start() { + stop_requested_ = false; init(); timer_ = nh_.createTimer(period_, &JogCalcs::run, this); @@ -155,6 +156,7 @@ void JogCalcs::start() void JogCalcs::stop() { + stop_requested_ = true; timer_.stop(); } @@ -177,6 +179,8 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // 2) so the low-pass filters are up to date and don't cause a jump while (!updateJoints(shared_variables_) && ros::ok()) { + if (stop_requested_) + return; default_sleep_rate_.sleep(); } @@ -354,9 +358,9 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& // Convert from cartesian commands to joint commands Eigen::MatrixXd jacobian = kinematic_state_->getJacobian(joint_model_group_); - // May allow some dimensions to drift, based on shared_variables.drift_dimensions + // May allow some dimensions to drift, based on drift_dimensions // i.e. take advantage of task redundancy. - // Remove the Jacobian rows corresponding to True in the vector shared_variables.drift_dimensions + // Remove the Jacobian rows corresponding to True in the vector drift_dimensions // Work backwards through the 6-vector so indices don't get out of order for (auto dimension = jacobian.rows(); dimension >= 0; --dimension) { @@ -959,4 +963,32 @@ void JogCalcs::jointJogCB(const control_msgs::JointJogConstPtr& msg) latest_command_stamp_ = msg->header.stamp; } +bool JogCalcs::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, + moveit_msgs::ChangeDriftDimensions::Response& res) +{ + shared_variables_.drift_dimensions[0] = req.drift_x_translation; + shared_variables_.drift_dimensions[1] = req.drift_y_translation; + shared_variables_.drift_dimensions[2] = req.drift_z_translation; + shared_variables_.drift_dimensions[3] = req.drift_x_rotation; + shared_variables_.drift_dimensions[4] = req.drift_y_rotation; + shared_variables_.drift_dimensions[5] = req.drift_z_rotation; + + res.success = true; + return true; +} + +bool JogCalcs::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, + moveit_msgs::ChangeControlDimensions::Response& res) +{ + shared_variables_.control_dimensions[0] = req.control_x_translation; + shared_variables_.control_dimensions[1] = req.control_y_translation; + shared_variables_.control_dimensions[2] = req.control_z_translation; + shared_variables_.control_dimensions[3] = req.control_x_rotation; + shared_variables_.control_dimensions[4] = req.control_y_rotation; + shared_variables_.control_dimensions[5] = req.control_z_rotation; + + res.success = true; + return true; +} + } // namespace moveit_jog_arm From 2a1b831ce17484bbea1da3c5e3cef2ff34834f4d Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 28 May 2020 13:44:31 -0600 Subject: [PATCH 323/612] replace jog_arm shared variables with ros pub/sub --- .../moveit_jog_arm/boost_pool_allocation.h | 49 ++++ .../moveit_jog_arm/collision_check_thread.h | 16 +- .../include/moveit_jog_arm/jog_arm.h | 34 ++- .../include/moveit_jog_arm/jog_arm_data.h | 46 ---- .../include/moveit_jog_arm/jog_calcs.h | 54 ++--- .../include/moveit_jog_arm/status_codes.h | 6 +- .../src/collision_check_thread.cpp | 47 ++-- .../cpp_interface_example.cpp | 66 ++++-- .../moveit_jog_arm/src/jog_arm.cpp | 55 ++--- .../moveit_jog_arm/src/jog_calcs.cpp | 209 +++++++++--------- .../test/jog_cpp_interface_test.cpp | 127 +++++++++-- .../test/jog_cpp_interface_test.test | 2 +- .../test_drift_dimensions_service.py | 2 +- .../halt_msg/test_jog_arm_halt_msg.py | 12 +- .../test_jog_arm_msg_reception.py | 14 +- .../moveit_jog_arm/test/python_tests/util.py | 10 +- .../vel_accel_limits/test_vel_accel_limits.py | 5 +- 17 files changed, 435 insertions(+), 319 deletions(-) create mode 100644 moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/boost_pool_allocation.h diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/boost_pool_allocation.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/boost_pool_allocation.h new file mode 100644 index 0000000000..ec1e550f74 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/boost_pool_allocation.h @@ -0,0 +1,49 @@ +/******************************************************************************* + * Title : boost_pool_allocation.h + * Project : moveit_jog_arm + * Created : 1/11/2019 + * Author : Tyler Weaver + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 the copyright holder 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 HOLDER 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 + +// Useful template for creating messages from a message pool +template +boost::shared_ptr make_shared_from_pool() +{ + using allocator_t = boost::fast_pool_allocator>; + return boost::allocate_shared(allocator_t()); +} diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h index b88b0d783d..23b19035de 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h @@ -43,6 +43,7 @@ #include #include #include +#include #include "jog_arm_data.h" #include "low_pass_filter.h" @@ -64,18 +65,20 @@ class CollisionCheckThread * already started when passed into this class */ CollisionCheckThread(ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, - JogArmShared& shared_variables, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - /** \breif start and stop the Thread */ + /** \brief start and stop the Thread */ void start(); void stop(); + /** \brief Pause or unpause processing jog commands while keeping the threads alive */ + void setPaused(bool paused); + private: - void init(); void run(const ros::TimerEvent& timer_event); planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); + void worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg); // ROS node handle ros::NodeHandle nh_; @@ -83,9 +86,6 @@ class CollisionCheckThread // Parameters const moveit_jog_arm::JogArmParameters parameters_; - // Shared variables from JogArm - JogArmShared& shared_variables_; - // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; @@ -101,6 +101,7 @@ class CollisionCheckThread double self_collision_distance_ = 0; double scene_collision_distance_ = 0; bool collision_detected_ = false; + bool paused_ = false; // Variables for stop-distance-based collision checking double current_collision_distance_ = 0; @@ -108,6 +109,7 @@ class CollisionCheckThread double prev_collision_distance_ = 0; double est_time_to_collision_ = 0; double safety_factor_ = 1000; + double worst_case_stop_time_ = std::numeric_limits::max(); const double self_velocity_scale_coefficient_; const double scene_velocity_scale_coefficient_; @@ -120,6 +122,8 @@ class CollisionCheckThread ros::Timer timer_; ros::Duration period_; ros::Subscriber joint_state_sub_; + ros::Publisher collision_velocity_scale_pub_; + ros::Subscriber worst_case_stop_time_sub_; // Latest joint state, updated by ROS callback mutable std::mutex joint_state_mutex_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h index 5c76645ba9..c3d0a75f6d 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h @@ -45,6 +45,7 @@ #include #include #include +#include #include #include "collision_check_thread.h" @@ -74,14 +75,6 @@ class JogArm /** \brief Pause or unpause processing jog commands while keeping the threads alive */ void setPaused(bool paused); - /** \brief Provide a Cartesian velocity command to the jogger. - * The units are determined by settings in the yaml file. - */ - void provideTwistStampedCommand(const geometry_msgs::TwistStampedConstPtr& msg); - - /** \brief Send joint position(s) commands */ - void provideJointCommand(const control_msgs::JointJogConstPtr& msg); - /** * Get the MoveIt planning link transform. * The transform from the MoveIt planning frame to robot_link_command_frame @@ -91,17 +84,14 @@ class JogArm */ bool getCommandFrameTransform(Eigen::Isometry3d& transform); - /** - * Get the status of the jogger. - * - * @return 0 for no warning. The meaning of nonzero values can be seen in status_codes.h - */ - StatusCode getJoggerStatus() const; + /** \brief Get the parameters used by jog arm. */ + const JogArmParameters& getParameters() const; private: bool readParameters(); void run(const ros::TimerEvent& timer_event); - void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryPtr& msg); + void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr& msg); + void okToPublishCB(const std_msgs::BoolConstPtr& msg); ros::NodeHandle nh_; @@ -111,9 +101,6 @@ class JogArm // Store the parameters that were read from ROS server JogArmParameters parameters_; - // Share data between threads - JogArmShared shared_variables_; - // Jog calcs std::unique_ptr jog_calcs_; @@ -124,13 +111,18 @@ class JogArm ros::Timer timer_; ros::Duration period_; ros::Publisher outgoing_cmd_pub_; - ros::Publisher twist_stamped_pub_; - ros::Publisher joint_jog_pub_; ros::Subscriber joint_trajectory_sub_; + ros::Subscriber ok_to_publish_sub_; + + bool paused_ = false; + bool ok_to_publish_ = false; // latest_state_mutex_ is used to protect the state below it mutable std::mutex latest_state_mutex_; - trajectory_msgs::JointTrajectoryPtr latest_joint_trajectory_; + trajectory_msgs::JointTrajectoryConstPtr latest_joint_trajectory_; }; +// JogArmPtr using alias +using JogArmPtr = std::shared_ptr; + } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index d69552473b..ad680028df 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -38,54 +38,8 @@ #pragma once -// Eigen -#include - -// ROS -#include -#include -#include - -// moveit_jog_arm -#include "status_codes.h" - namespace moveit_jog_arm { -// Variables to share between threads -// Be careful to not read-modify-write any of these because they are not -// protected by a mutex. -struct JogArmShared -{ - double collision_velocity_scale = 1; - - // The jog thread communicates the minimum stopping time to the collision check thread via this variable - double worst_case_stop_time = std::numeric_limits::max(); - - // Flag a valid incoming Cartesian command having nonzero velocities - bool have_nonzero_cartesian_cmd = false; - - // Flag a valid incoming joint angle command having nonzero velocities - bool have_nonzero_joint_cmd = false; - - // Indicates that we have not received a new command in some time - bool command_is_stale = false; - - // Indicates no collision, etc, so outgoing commands can be sent - bool ok_to_publish = false; - - // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw] - std::array drift_dimensions = { { false, false, false, false, false, false } }; - - // Status of the jogger. 0 for no warning. The meaning of nonzero values can be seen in status_codes.h - StatusCode status = NO_WARNING; - - // Pause/unpause jog threads - threads are not paused by default - bool paused = false; - - // The dimesions to control. In the command frame. [x, y, z, roll, pitch, yaw] - std::array control_dimensions = { { true, true, true, true, true, true } }; -}; - // ROS params to be read. See the yaml file in /config for a description of each. struct JogArmParameters { diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index a4577b4ddf..cc789795b7 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -48,6 +48,10 @@ #include #include #include +#include +#include +#include +#include // moveit_jog_arm #include "jog_arm_data.h" @@ -59,18 +63,13 @@ namespace moveit_jog_arm class JogCalcs { public: - JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, JogArmShared& shared_variables, + JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); /** \brief Start and stop the timer (thread) */ void start(); void stop(); - /** \brief Check if the robot state is being updated and the end effector transform is known - * @return true if initialized properly - */ - bool isInitialized(); - /** * Get the MoveIt planning link transform. * The transform from the MoveIt planning frame to robot_link_command_frame @@ -80,24 +79,21 @@ class JogCalcs */ bool getCommandFrameTransform(Eigen::Isometry3d& transform); -private: - /** \brief Init must be called once before run */ - void init(); + /** \brief Pause or unpause processing jog commands while keeping the threads alive */ + void setPaused(bool paused); +private: /** \brief Timer method */ void run(const ros::TimerEvent& timer_event); /** \brief Do jogging calculations for Cartesian twist commands. */ - bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables); + bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd); /** \brief Do jogging calculations for direct commands to a joint. */ - bool jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables); - - /** \brief Update the stashed status so it can be retrieved asynchronously */ - void updateCachedStatus(JogArmShared& shared_variables); + bool jointJogCalcs(const control_msgs::JointJog& cmd); /** \brief Parse the incoming joint msg for the joints of our MoveGroup */ - bool updateJoints(JogArmShared& shared_variables); + bool updateJoints(); /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. * Also, multiply by timestep to calculate a position change. @@ -117,9 +113,6 @@ class JogCalcs void suddenHalt(trajectory_msgs::JointTrajectory& joint_traj); void suddenHalt(Eigen::ArrayXd& delta_theta); - /** \brief Publish the status of the jogger to a ROS topic */ - void publishStatus() const; - /** \brief Scale the delta theta to match joint velocity/acceleration limits */ void enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta); @@ -135,11 +128,10 @@ class JogCalcs /** * Slow motion down if close to singularity or collision. - * @param shared_variables data shared between threads, tells how close we are to collision * @param delta_theta motion command, used in calculating new_joint_tray * @param singularity_scale tells how close we are to a singularity */ - void applyVelocityScaling(JogArmShared& shared_variables, Eigen::ArrayXd& delta_theta, double singularity_scale); + void applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singularity_scale); /** \brief Compose the outgoing JointTrajectory message */ trajectory_msgs::JointTrajectory composeJointTrajMessage(sensor_msgs::JointState& joint_state) const; @@ -175,6 +167,7 @@ class JogCalcs /* \brief Command callbacks */ void twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg); void jointJogCB(const control_msgs::JointJogConstPtr& msg); + void collisionVelocityScaleCB(const std_msgs::Float64ConstPtr& msg); /** * Allow drift in certain dimensions. For example, may allow the wrist to rotate freely. @@ -198,15 +191,9 @@ class JogCalcs // Ros parameters from JogArm const JogArmParameters& parameters_; - // Shared variables from JogArm - JogArmShared& shared_variables_; - // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - // Flag that robot state is up to date, end effector transform is known - bool is_initialized_ = false; - // Track the number of cycles during which motion has not occurred. // Will avoid re-publishing zero velocities endlessly. int zero_velocity_count_ = 0; @@ -244,14 +231,21 @@ class JogCalcs ros::Subscriber joint_state_sub_; ros::Subscriber twist_stamped_sub_; ros::Subscriber joint_jog_sub_; + ros::Subscriber collision_velocity_scale_sub_; ros::Publisher status_pub_; ros::Publisher joint_trajectory_pub_; + ros::Publisher worst_case_stop_time_pub_; ros::ServiceServer drift_dimensions_server_; ros::ServiceServer control_dimensions_server_; + ros::Publisher ok_to_publish_pub_; + // Status - StatusCode status_ = NO_WARNING; + StatusCode status_ = StatusCode::NO_WARNING; bool stop_requested_ = false; + bool paused_ = false; + bool command_is_stale_ = false; + double collision_velocity_scale_ = 1.0; // Use ArrayXd type to enable more coefficient-wise operations Eigen::ArrayXd delta_theta_; @@ -261,6 +255,12 @@ class JogCalcs uint num_joints_; + // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw] + std::array drift_dimensions_ = { { false, false, false, false, false, false } }; + + // The dimesions to control. In the command frame. [x, y, z, roll, pitch, yaw] + std::array control_dimensions_ = { { true, true, true, true, true, true } }; + // Amount we sleep when waiting ros::Rate default_sleep_rate_ = 1000; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h index 85660cd50b..7f930f6db6 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h @@ -45,6 +45,7 @@ namespace moveit_jog_arm { enum StatusCode : int8_t { + INVALID = -1, NO_WARNING = 0, DECELERATE_FOR_SINGULARITY = 1, HALT_FOR_SINGULARITY = 2, @@ -53,8 +54,9 @@ enum StatusCode : int8_t JOINT_BOUND = 5 }; -const std::unordered_map - JOG_ARM_STATUS_CODE_MAP({ { NO_WARNING, "No warnings" }, +const std::unordered_map + JOG_ARM_STATUS_CODE_MAP({ { INVALID, "Invalid" }, + { NO_WARNING, "No warnings" }, { DECELERATE_FOR_SINGULARITY, "Close to a singularity, decelerating" }, { HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, { DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index b04232f6ec..98bac5056e 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -37,7 +37,10 @@ * Author : Brian O'Neil, Andy Zelenak, Blake Anderson */ +#include + #include +#include static const std::string LOGNAME = "collision_check_thread"; static constexpr double MIN_RECOMMENDED_COLLISION_RATE = 10; @@ -47,11 +50,10 @@ namespace moveit_jog_arm { // Constructor for the class that handles collision checking CollisionCheckThread::CollisionCheckThread( - ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, JogArmShared& shared_variables, + ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : nh_(nh) , parameters_(parameters) - , shared_variables_(shared_variables) , planning_scene_monitor_(planning_scene_monitor) , self_velocity_scale_coefficient_(-log(0.001) / parameters.self_collision_proximity_threshold) , scene_velocity_scale_coefficient_(-log(0.001) / parameters.scene_collision_proximity_threshold) @@ -70,15 +72,13 @@ CollisionCheckThread::CollisionCheckThread( collision_check_type_ = (parameters_.collision_check_type == "threshold_distance" ? K_THRESHOLD_DISTANCE : K_STOP_DISTANCE); safety_factor_ = parameters_.collision_distance_safety_factor; -} -planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPlanningSceneRO() const -{ - return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); -} + // Internal namespace + ros::NodeHandle internal_nh("~internal"); + collision_velocity_scale_pub_ = internal_nh.advertise("collision_velocity_scale", 1); + worst_case_stop_time_sub_ = + internal_nh.subscribe("worst_case_stop_time", 2, &CollisionCheckThread::worstCaseStopTimeCB, this); -void CollisionCheckThread::init() -{ // Wait for incoming topics to appear ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); ros::topic::waitForMessage(parameters_.joint_topic); @@ -87,9 +87,13 @@ void CollisionCheckThread::init() acm_ = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); } +planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPlanningSceneRO() const +{ + return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); +} + void CollisionCheckThread::start() { - init(); timer_ = nh_.createTimer(period_, &CollisionCheckThread::run, this); } @@ -112,7 +116,7 @@ void CollisionCheckThread::run(const ros::TimerEvent& timer_event) << period_.toSec()); } - if (shared_variables_.paused) + if (paused_) { return; } @@ -191,7 +195,7 @@ void CollisionCheckThread::run(const ros::TimerEvent& timer_event) est_time_to_collision_ = fabs(current_collision_distance_ / derivative_of_collision_distance_); // halt if we can't stop fast enough (including the safety factor) - if (est_time_to_collision_ < (safety_factor_ * shared_variables_.worst_case_stop_time)) + if (est_time_to_collision_ < (safety_factor_ * worst_case_stop_time_)) { velocity_scale_ = 0; } @@ -201,8 +205,12 @@ void CollisionCheckThread::run(const ros::TimerEvent& timer_event) prev_collision_distance_ = current_collision_distance_; } - // Communicate a velocity-scaling factor back to the other threads - shared_variables_.collision_velocity_scale = velocity_scale_; + // publish message + { + auto msg = make_shared_from_pool(); + msg->data = velocity_scale_; + collision_velocity_scale_pub_.publish(msg); + } } void CollisionCheckThread::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) @@ -210,4 +218,15 @@ void CollisionCheckThread::jointStateCB(const sensor_msgs::JointStateConstPtr& m const std::lock_guard lock(joint_state_mutex_); latest_joint_state_ = msg; } + +void CollisionCheckThread::worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg) +{ + worst_case_stop_time_ = msg->data; +} + +void CollisionCheckThread::setPaused(bool paused) +{ + paused_ = paused; +} + } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index 18dca5af16..0b2617f012 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -36,15 +36,41 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -#include "moveit_jog_arm/jog_arm.h" -#include "moveit_jog_arm/status_codes.h" +#include + +#include +#include +#include static const std::string LOGNAME = "cpp_interface_example"; +// Class for monitoring status of jog_arm +class StatusMonitor +{ +public: + StatusMonitor(ros::NodeHandle& nh, const std::string& topic) + { + sub_ = nh.subscribe(topic, 1, &StatusMonitor::statusCB, this); + } + +private: + void statusCB(const std_msgs::Int8ConstPtr& msg) + { + moveit_jog_arm::StatusCode latest_stats = static_cast(msg->data); + if (latest_stats != status_) + { + status_ = latest_stats; + const auto& status_str = moveit_jog_arm::JOG_ARM_STATUS_CODE_MAP.at(status_); + ROS_INFO_STREAM_NAMED(LOGNAME, "Jogger status: " << status_str); + } + } + moveit_jog_arm::StatusCode status_ = moveit_jog_arm::StatusCode::INVALID; + ros::Subscriber sub_; +}; + /** * Instantiate the C++ jogging interface. * Send some Cartesian commands, then some joint commands. - * Then retrieve the current joint state from the jogger. */ int main(int argc, char** argv) { @@ -73,6 +99,14 @@ int main(int argc, char** argv) moveit_jog_arm::JogArm jog_arm(nh, planning_scene_monitor); jog_arm.start(); + // Subscribe to jog_arm status (and log it when it changes) + StatusMonitor status_monitor(nh, jog_arm.getParameters().status_topic); + + // Create publishers to send jog_arm commands + auto twist_stamped_pub = + nh.advertise(jog_arm.getParameters().cartesian_command_in_topic, 1); + auto joint_jog_pub = nh.advertise(jog_arm.getParameters().joint_command_in_topic, 1); + ros::Rate cmd_rate(100); uint num_commands = 0; @@ -82,14 +116,14 @@ int main(int argc, char** argv) // Make a Cartesian velocity message // Messages are sent to jogger as boost::shared_ptr to enable zero-copy message_passing. // Because this message is not coppied we should not modify it after we send it. - auto velocity_msg = boost::make_shared(); - velocity_msg->header.stamp = ros::Time::now(); - velocity_msg->header.frame_id = "base_link"; - velocity_msg->twist.linear.y = 0.01; - velocity_msg->twist.linear.z = -0.01; + auto msg = make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "base_link"; + msg->twist.linear.y = 0.01; + msg->twist.linear.z = -0.01; // Send the message - jog_arm.provideTwistStampedCommand(velocity_msg); + twist_stamped_pub.publish(msg); cmd_rate.sleep(); ++num_commands; } @@ -105,21 +139,17 @@ int main(int argc, char** argv) // Make a joint command // Messages are sent to jogger as boost::shared_ptr to enable zero-copy message_passing. // Because this message is not coppied we should not modify it after we send it. - auto elbow_joint_command = boost::make_shared(); - elbow_joint_command->header.stamp = ros::Time::now(); - elbow_joint_command->joint_names.push_back("elbow_joint"); - elbow_joint_command->velocities.push_back(0.2); + auto msg = make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->joint_names.push_back("elbow_joint"); + msg->velocities.push_back(0.2); // Send the message - jog_arm.provideJointCommand(elbow_joint_command); + joint_jog_pub.publish(msg); cmd_rate.sleep(); ++num_commands; } - // Retrieve the current status of the jogger - moveit_jog_arm::StatusCode status = jog_arm.getJoggerStatus(); - ROS_INFO_STREAM_NAMED(LOGNAME, "Jogger status:\n" << status); - jog_arm.stop(); return 0; } diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index 0a37345420..3afe8d0464 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -60,22 +60,18 @@ JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneM else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, 1); - // publish commands (for c++ interface) - twist_stamped_pub_ = nh_.advertise(parameters_.cartesian_command_in_topic, 1); - joint_jog_pub_ = nh_.advertise(parameters_.joint_command_in_topic, 1); - - // Subscribe to output commands in internal namespace + // Subscribe to internal namespace ros::NodeHandle internal_nh("~internal"); joint_trajectory_sub_ = internal_nh.subscribe("joint_trajectory", 1, &JogArm::jointTrajectoryCB, this); + ok_to_publish_sub_ = internal_nh.subscribe("ok_to_publish", 1, &JogArm::okToPublishCB, this); // Wait for incoming topics to appear ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); ros::topic::waitForMessage(parameters_.joint_topic); - jog_calcs_ = std::make_unique(nh_, parameters_, shared_variables_, planning_scene_monitor_); + jog_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); - collision_checker_ = - std::make_unique(nh_, parameters_, shared_variables_, planning_scene_monitor_); + collision_checker_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); } // Read ROS parameters, typically from YAML file @@ -317,7 +313,7 @@ void JogArm::run(const ros::TimerEvent& timer_event) } // Publish the most recent trajectory, unless the jogging calculation thread tells not to - if (shared_variables_.ok_to_publish) + if (ok_to_publish_ && !paused_) { // Put the outgoing msg in the right format // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). @@ -336,20 +332,11 @@ void JogArm::run(const ros::TimerEvent& timer_event) outgoing_cmd_pub_.publish(joints); } } - else if (shared_variables_.command_is_stale) - { - ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " - "Try a larger 'incoming_command_timeout' parameter?"); - } - else - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); - } } void JogArm::start() { - shared_variables_.paused = false; + setPaused(false); // Crunch the numbers in this thread jog_calcs_->start(); @@ -374,37 +361,33 @@ JogArm::~JogArm() stop(); } -void JogArm::provideTwistStampedCommand(const geometry_msgs::TwistStampedConstPtr& msg) -{ - twist_stamped_pub_.publish(msg); -} - -void JogArm::provideJointCommand(const control_msgs::JointJogConstPtr& msg) -{ - joint_jog_pub_.publish(msg); -} - void JogArm::setPaused(bool paused) { - shared_variables_.paused = paused; + paused_ = paused; + jog_calcs_->setPaused(paused); + collision_checker_->setPaused(paused); } bool JogArm::getCommandFrameTransform(Eigen::Isometry3d& transform) { - if (!jog_calcs_->isInitialized()) - return false; return jog_calcs_->getCommandFrameTransform(transform); } -StatusCode JogArm::getJoggerStatus() const +void JogArm::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr& msg) { - return shared_variables_.status; + const std::lock_guard lock(latest_state_mutex_); + latest_joint_trajectory_ = msg; } -void JogArm::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryPtr& msg) +void JogArm::okToPublishCB(const std_msgs::BoolConstPtr& msg) { const std::lock_guard lock(latest_state_mutex_); - latest_joint_trajectory_ = msg; + ok_to_publish_ = msg->data; +} + +const JogArmParameters& JogArm::getParameters() const +{ + return parameters_; } } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index ccc80ed630..e56768d585 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -37,7 +37,10 @@ * Author : Brian O'Neil, Andy Zelenak, Blake Anderson */ +#include + #include +#include static const std::string LOGNAME = "jog_calcs"; @@ -65,23 +68,33 @@ bool isNonZero(const control_msgs::JointJog& msg) } // namespace // Constructor for the class that handles jogging calculations -JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, JogArmShared& shared_variables, +JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : nh_(nh) , parameters_(parameters) - , shared_variables_(shared_variables) , planning_scene_monitor_(planning_scene_monitor) , period_(parameters.publish_period) { - // Publish jogger status - status_pub_ = nh_.advertise(parameters_.status_topic, 1); + // MoveIt Setup + const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr = planning_scene_monitor_->getRobotModelLoader(); + while (ros::ok() && !model_loader_ptr) + { + ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); + default_sleep_rate_.sleep(); + } + const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); + kinematic_state_ = std::make_shared(kinematic_model); + kinematic_state_->setToDefaultValues(); + + joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); + prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getActiveJointModels().size()); // subscribe to joints - joint_state_sub_ = nh_.subscribe(parameters.joint_topic, 1, &JogCalcs::jointStateCB, this); + joint_state_sub_ = nh_.subscribe(parameters_.joint_topic, 1, &JogCalcs::jointStateCB, this); // Subscribe to command topics - twist_stamped_sub_ = nh_.subscribe(parameters.cartesian_command_in_topic, 1, &JogCalcs::twistStampedCB, this); - joint_jog_sub_ = nh.subscribe(parameters.joint_command_in_topic, 1, &JogCalcs::jointJogCB, this); + twist_stamped_sub_ = nh_.subscribe(parameters_.cartesian_command_in_topic, 100, &JogCalcs::twistStampedCB, this); + joint_jog_sub_ = nh_.subscribe(parameters_.joint_command_in_topic, 1, &JogCalcs::jointJogCB, this); // ROS Server for allowing drift in some dimensions drift_dimensions_server_ = @@ -90,32 +103,16 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, JogA // ROS Server for changing the control dimensions control_dimensions_server_ = - nh_.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", + nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", &JogCalcs::changeControlDimensions, this); - // Publish output commands to internal namespace + // Publish and Subscribe to internal namespace topics ros::NodeHandle internal_nh("~internal"); joint_trajectory_pub_ = internal_nh.advertise("joint_trajectory", 1); -} - -void JogCalcs::init() -{ - // Reset flags - is_initialized_ = false; - - // MoveIt Setup - const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr = planning_scene_monitor_->getRobotModelLoader(); - while (ros::ok() && !model_loader_ptr) - { - ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); - default_sleep_rate_.sleep(); - } - const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); - kinematic_state_ = std::make_shared(kinematic_model); - kinematic_state_->setToDefaultValues(); - - joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); - prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getActiveJointModels().size()); + ok_to_publish_pub_ = internal_nh.advertise("ok_to_publish", 1); + collision_velocity_scale_sub_ = + internal_nh.subscribe("collision_velocity_scale", 1, &JogCalcs::collisionVelocityScaleCB, this); + worst_case_stop_time_pub_ = internal_nh.advertise("worst_case_stop_time", 1); // Wait for initial messages ROS_INFO_NAMED(LOGNAME, "jog_calcs_thread: Waiting for first joint msg."); @@ -139,7 +136,6 @@ void JogCalcs::init() } // initialization is finished - is_initialized_ = true; wait_for_jog_commands_ = true; have_nonzero_twist_stamped_ = false; have_nonzero_joint_jog_ = false; @@ -149,7 +145,7 @@ void JogCalcs::init() void JogCalcs::start() { stop_requested_ = false; - init(); + status_pub_ = nh_.advertise(parameters_.status_topic, 1); timer_ = nh_.createTimer(period_, &JogCalcs::run, this); } @@ -174,10 +170,17 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) << period_.toSec()); } + // Publish status each loop iteration + { + auto status_msg = make_shared_from_pool(); + status_msg->data = static_cast(status_); + status_pub_.publish(status_msg); + } + // Always update the joints and end-effector transform for 2 reasons: // 1) in case the getCommandFrameTransform() method is being used // 2) so the low-pass filters are up to date and don't cause a jump - while (!updateJoints(shared_variables_) && ros::ok()) + while (!updateJoints() && ros::ok()) { if (stop_requested_) return; @@ -194,7 +197,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) joint_jog_ = *latest_joint_jog_; // Check for stale cmds - shared_variables_.command_is_stale = + command_is_stale_ = ((ros::Time::now() - latest_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); } @@ -214,7 +217,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // If paused or while waiting for initial jog commands, just keep the low-pass filters up to date with current // joints so a jump doesn't occur when restarting - if (wait_for_jog_commands_ || shared_variables_.paused) + if (wait_for_jog_commands_ || paused_) { for (std::size_t i = 0; i < num_joints_; ++i) position_filters_[i].reset(original_joint_state_.position[i]); @@ -226,17 +229,17 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // Do jogging calculations only if the robot should move, for efficiency else { - if (!shared_variables_.command_is_stale) + if (!command_is_stale_) { // Prioritize cartesian jogging above joint jogging if (have_nonzero_twist_stamped_) { - if (!cartesianJogCalcs(twist_stamped_, shared_variables_)) + if (!cartesianJogCalcs(twist_stamped_)) return; } else if (have_nonzero_joint_jog_) { - if (!jointJogCalcs(joint_jog_, shared_variables_)) + if (!jointJogCalcs(joint_jog_)) return; } } @@ -258,20 +261,29 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) if (have_nonzero_command_) { joint_trajectory_pub_.publish(outgoing_command_); - shared_variables_.ok_to_publish = true; + + auto bool_msg = boost::make_shared(); + bool_msg->data = true; + ok_to_publish_pub_.publish(bool_msg); } // Skip the jogging publication if all inputs have been zero for several cycles in a row. // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. else if ((parameters_.num_outgoing_halt_msgs_to_publish != 0) && (zero_velocity_count_ > parameters_.num_outgoing_halt_msgs_to_publish)) { - shared_variables_.ok_to_publish = false; + auto bool_msg = boost::make_shared(); + bool_msg->data = false; + ok_to_publish_pub_.publish(bool_msg); + ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); } // The command is invalid but we are publishing num_outgoing_halt_msgs_to_publish else { joint_trajectory_pub_.publish(outgoing_command_); - shared_variables_.ok_to_publish = true; + + auto bool_msg = boost::make_shared(); + bool_msg->data = true; + ok_to_publish_pub_.publish(bool_msg); } // Store last zero-velocity message flag to prevent superfluous warnings. @@ -285,15 +297,15 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) else zero_velocity_count_ = 0; } -} -bool JogCalcs::isInitialized() -{ - return is_initialized_; + if (command_is_stale_) + { + ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " + "Try a larger 'incoming_command_timeout' parameter?"); + } } - // Perform the jogging calculations -bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables) +bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd) { // Check for nan's in the incoming command if (std::isnan(cmd.twist.linear.x) || std::isnan(cmd.twist.linear.y) || std::isnan(cmd.twist.linear.z) || @@ -315,17 +327,17 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& } // Set uncontrolled dimensions to 0 in command frame - if (!shared_variables.control_dimensions[0]) + if (!control_dimensions_[0]) cmd.twist.linear.x = 0; - if (!shared_variables.control_dimensions[1]) + if (!control_dimensions_[1]) cmd.twist.linear.y = 0; - if (!shared_variables.control_dimensions[2]) + if (!control_dimensions_[2]) cmd.twist.linear.z = 0; - if (!shared_variables.control_dimensions[3]) + if (!control_dimensions_[3]) cmd.twist.angular.x = 0; - if (!shared_variables.control_dimensions[4]) + if (!control_dimensions_[4]) cmd.twist.angular.y = 0; - if (!shared_variables.control_dimensions[5]) + if (!control_dimensions_[5]) cmd.twist.angular.z = 0; // Transform the command to the MoveGroup planning frame @@ -364,7 +376,7 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& // Work backwards through the 6-vector so indices don't get out of order for (auto dimension = jacobian.rows(); dimension >= 0; --dimension) { - if (shared_variables.drift_dimensions[dimension] && jacobian.rows() > 1) + if (drift_dimensions_[dimension] && jacobian.rows() > 1) { removeDimension(jacobian, delta_x, dimension); } @@ -380,9 +392,8 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& enforceSRDFAccelVelLimits(delta_theta_); // If close to a collision or a singularity, decelerate - applyVelocityScaling(shared_variables, delta_theta_, - velocityScalingFactorForSingularity(delta_x, svd, jacobian, pseudo_inverse)); - if (status_ == HALT_FOR_COLLISION) + applyVelocityScaling(delta_theta_, velocityScalingFactorForSingularity(delta_x, svd, jacobian, pseudo_inverse)); + if (status_ == StatusCode::HALT_FOR_COLLISION) { ROS_ERROR_STREAM_THROTTLE_NAMED(5, LOGNAME, "Halting for collision!"); suddenHalt(delta_theta_); @@ -390,14 +401,10 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; - publishStatus(); - // Cache the status so it can be retrieved asynchronously - updateCachedStatus(shared_variables); - return convertDeltasToOutgoingCmd(); } -bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables) +bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd) { // Check for nan's or |delta|>1 in the incoming command for (double velocity : cmd.velocities) @@ -418,19 +425,9 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& sh prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; - publishStatus(); - // Cache the status so it can be retrieved asynchronously - updateCachedStatus(shared_variables); - return convertDeltasToOutgoingCmd(); } -void JogCalcs::updateCachedStatus(JogArmShared& shared_variables) -{ - shared_variables.status = status_; - status_ = NO_WARNING; -} - bool JogCalcs::convertDeltasToOutgoingCmd() { internal_joint_state_ = original_joint_state_; @@ -447,7 +444,7 @@ bool JogCalcs::convertDeltasToOutgoingCmd() if (!enforceSRDFPositionLimits(outgoing_command_)) { suddenHalt(outgoing_command_); - status_ = JOINT_BOUND; + status_ = StatusCode::JOINT_BOUND; } // done with calculations @@ -516,20 +513,18 @@ trajectory_msgs::JointTrajectory JogCalcs::composeJointTrajMessage(sensor_msgs:: } // Apply velocity scaling for proximity of collisions and singularities. -// Scale for collisions is read from a shared variable. -void JogCalcs::applyVelocityScaling(JogArmShared& shared_variables, Eigen::ArrayXd& delta_theta, - double singularity_scale) +void JogCalcs::applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singularity_scale) { - double collision_scale = shared_variables.collision_velocity_scale; + double collision_scale = collision_velocity_scale_; if (collision_scale > 0 && collision_scale < 1) { - status_ = DECELERATE_FOR_COLLISION; + status_ = StatusCode::DECELERATE_FOR_COLLISION; ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); } else if (collision_scale == 0) { - status_ = HALT_FOR_COLLISION; + status_ = StatusCode::HALT_FOR_COLLISION; } delta_theta = collision_scale * singularity_scale * delta_theta; @@ -587,7 +582,7 @@ double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& comm velocity_scale = 1. - (ini_condition - parameters_.lower_singularity_threshold) / (parameters_.hard_stop_singularity_threshold - parameters_.lower_singularity_threshold); - status_ = DECELERATE_FOR_SINGULARITY; + status_ = StatusCode::DECELERATE_FOR_SINGULARITY; ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); } @@ -595,7 +590,7 @@ double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& comm else if (ini_condition > parameters_.hard_stop_singularity_threshold) { velocity_scale = 0; - status_ = HALT_FOR_SINGULARITY; + status_ = StatusCode::HALT_FOR_SINGULARITY; ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); } } @@ -716,13 +711,6 @@ bool JogCalcs::enforceSRDFPositionLimits(trajectory_msgs::JointTrajectory& new_j return !halting; } -void JogCalcs::publishStatus() const -{ - std_msgs::Int8 status_msg; - status_msg.data = status_; - status_pub_.publish(status_msg); -} - // Suddenly halt for a joint limit or other critical issue. // Is handled differently for position vs. velocity control. void JogCalcs::suddenHalt(Eigen::ArrayXd& delta_theta) @@ -754,7 +742,7 @@ void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_traj) } // Parse the incoming joint msg for the joints of our MoveGroup -bool JogCalcs::updateJoints(JogArmShared& shared_variables) +bool JogCalcs::updateJoints() { // lock the latest state mutex for the joint states const std::lock_guard lock(latest_state_mutex_); @@ -785,7 +773,6 @@ bool JogCalcs::updateJoints(JogArmShared& shared_variables) // Calculate worst case joint stop time, for collision checking std::string joint_name = ""; - shared_variables.worst_case_stop_time = std::numeric_limits::max(); moveit::core::JointModel::Bounds kinematic_bounds; double accel_limit = 0; double joint_velocity = 0; @@ -823,7 +810,13 @@ bool JogCalcs::updateJoints(JogArmShared& shared_variables) // Calculate worst case stop time worst_case_stop_time = std::max(worst_case_stop_time, fabs(joint_velocity / accel_limit)); } - shared_variables.worst_case_stop_time = worst_case_stop_time; + + // publish message + { + auto msg = make_shared_from_pool(); + msg->data = worst_case_stop_time; + worst_case_stop_time_pub_.publish(msg); + } return true; } @@ -950,6 +943,7 @@ void JogCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg) const std::lock_guard lock(latest_state_mutex_); latest_twist_stamped_ = msg; have_nonzero_twist_stamped_ = isNonZero(*latest_twist_stamped_); + if (msg->header.stamp != ros::Time(0.)) latest_command_stamp_ = msg->header.stamp; } @@ -959,19 +953,25 @@ void JogCalcs::jointJogCB(const control_msgs::JointJogConstPtr& msg) const std::lock_guard lock(latest_state_mutex_); latest_joint_jog_ = msg; have_nonzero_joint_jog_ = isNonZero(*latest_joint_jog_); + if (msg->header.stamp != ros::Time(0.)) latest_command_stamp_ = msg->header.stamp; } +void JogCalcs::collisionVelocityScaleCB(const std_msgs::Float64ConstPtr& msg) +{ + collision_velocity_scale_ = msg->data; +} + bool JogCalcs::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, moveit_msgs::ChangeDriftDimensions::Response& res) { - shared_variables_.drift_dimensions[0] = req.drift_x_translation; - shared_variables_.drift_dimensions[1] = req.drift_y_translation; - shared_variables_.drift_dimensions[2] = req.drift_z_translation; - shared_variables_.drift_dimensions[3] = req.drift_x_rotation; - shared_variables_.drift_dimensions[4] = req.drift_y_rotation; - shared_variables_.drift_dimensions[5] = req.drift_z_rotation; + drift_dimensions_[0] = req.drift_x_translation; + drift_dimensions_[1] = req.drift_y_translation; + drift_dimensions_[2] = req.drift_z_translation; + drift_dimensions_[3] = req.drift_x_rotation; + drift_dimensions_[4] = req.drift_y_rotation; + drift_dimensions_[5] = req.drift_z_rotation; res.success = true; return true; @@ -980,15 +980,20 @@ bool JogCalcs::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request bool JogCalcs::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, moveit_msgs::ChangeControlDimensions::Response& res) { - shared_variables_.control_dimensions[0] = req.control_x_translation; - shared_variables_.control_dimensions[1] = req.control_y_translation; - shared_variables_.control_dimensions[2] = req.control_z_translation; - shared_variables_.control_dimensions[3] = req.control_x_rotation; - shared_variables_.control_dimensions[4] = req.control_y_rotation; - shared_variables_.control_dimensions[5] = req.control_z_rotation; + control_dimensions_[0] = req.control_x_translation; + control_dimensions_[1] = req.control_y_translation; + control_dimensions_[2] = req.control_z_translation; + control_dimensions_[3] = req.control_x_rotation; + control_dimensions_[4] = req.control_y_rotation; + control_dimensions_[5] = req.control_z_rotation; res.success = true; return true; } +void JogCalcs::setPaused(bool paused) +{ + paused_ = paused; +} + } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp index cbd25a1d42..61bb79c760 100644 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Dave Coleman - Desc: Stub test for the C++ interface to JogArm +/* Author: Dave Coleman and Tyler Weaver + Desc: Test for the C++ interface to JogArm */ // C++ @@ -45,14 +45,15 @@ // Testing #include -// Main class +// Jog Arm #include +#include static const std::string LOGNAME = "jog_cpp_interface_test"; namespace moveit_jog_arm { -class TestJogCppInterface : public ::testing::Test +class JogArmFixture : public ::testing::Test { public: void SetUp() override @@ -61,45 +62,129 @@ class TestJogCppInterface : public ::testing::Test planning_scene_monitor_ = std::make_shared("robot_description"); planning_scene_monitor_->startSceneMonitor(); planning_scene_monitor_->startStateMonitor(); + planning_scene_monitor_->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + + // Create jog_arm + jog_arm_ = std::make_shared(nh_, planning_scene_monitor_); } void TearDown() override { } + bool waitForFirstStatus() + { + auto msg = + ros::topic::waitForMessage(jog_arm_->getParameters().status_topic, nh_, ros::Duration(15)); + return static_cast(msg); + } + protected: ros::NodeHandle nh_; planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; -}; // class TestJogCppInterface + moveit_jog_arm::JogArmPtr jog_arm_; +}; // class JogArmFixture -TEST_F(TestJogCppInterface, InitTest) +TEST_F(JogArmFixture, StartStopTest) { - moveit_jog_arm::JogArm jog_arm(nh_, planning_scene_monitor_); - jog_arm.start(); - ros::Duration(1).sleep(); // Give the started thread some time to run - jog_arm.stop(); - ros::Duration(1).sleep(); // Give the started thread some time to run - jog_arm.start(); - ros::Duration(1).sleep(); // Give the started thread some time to run - jog_arm.stop(); + // Start and stop jog arm + jog_arm_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + jog_arm_->stop(); +} + +TEST_F(JogArmFixture, SendTwistStampedTest) +{ + jog_arm_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + + auto parameters = jog_arm_->getParameters(); + + // count trajectory messages sent by jog_arm + size_t received_count = 0; + boost::function traj_callback = + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { received_count++; }; + auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); + + // Create publisher to send jog_arm commands + auto twist_stamped_pub = nh_.advertise(parameters.cartesian_command_in_topic, 1); + + constexpr double test_duration = 1.0; + const double publish_period = parameters.publish_period; + const size_t num_commands = static_cast(test_duration / publish_period); + + ros::Rate publish_rate(1. / publish_period); + + // Send a few Cartesian velocity commands + for (size_t i = 0; i < num_commands && ros::ok(); ++i) + { + auto msg = make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "panda_link0"; + msg->twist.angular.y = 1.0; + + // Send the message + twist_stamped_pub.publish(msg); + publish_rate.sleep(); + } + + EXPECT_GT(received_count, num_commands - 20); + EXPECT_LT(received_count, num_commands + 20); + jog_arm_->stop(); } -// TODO(davetcoleman): due to many blocking checks for ROS messages, and -// an abundance of threads, unit tests are not currently feasible for the -// cpp interface. This should be addressed in future re-factors +TEST_F(JogArmFixture, SendJointJogTest) +{ + jog_arm_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + + auto parameters = jog_arm_->getParameters(); + + // count trajectory messages sent by jog_arm + size_t received_count = 0; + boost::function traj_callback = + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { received_count++; }; + auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); + + // Create publisher to send jog_arm commands + auto joint_jog_pub = nh_.advertise(parameters.joint_command_in_topic, 1); + + constexpr double test_duration = 1.0; + const double publish_period = parameters.publish_period; + const size_t num_commands = static_cast(test_duration / publish_period); + + ros::Rate publish_rate(1. / publish_period); + + // Send a few Cartesian velocity commands + for (size_t i = 0; i < num_commands && ros::ok(); ++i) + { + auto msg = make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "panda_link3"; + msg->velocities.push_back(0.1); + + // Send the message + joint_jog_pub.publish(msg); + publish_rate.sleep(); + } + + EXPECT_GT(received_count, num_commands - 20); + EXPECT_LT(received_count, num_commands + 20); + jog_arm_->stop(); +} } // namespace moveit_jog_arm int main(int argc, char** argv) { + ros::init(argc, argv, LOGNAME); testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "jog_cpp_interface_test_test"); ros::AsyncSpinner spinner(8); spinner.start(); int result = RUN_ALL_TESTS(); - - spinner.stop(); - ros::shutdown(); return result; } diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test index 73d626f086..b487ef2461 100644 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test @@ -11,7 +11,7 @@ - + diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py b/moveit_experimental/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py index d28b5c5a6d..eb049957b4 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py @@ -32,7 +32,7 @@ def node(): def test_drift_dimensions_service(node): - assert util.wait_for_jogger_initialization(SERVICE_NAME) + assert util.wait_for_jogger_initialization() # Service to change drift dimensions drift_service = rospy.ServiceProxy(SERVICE_NAME, ChangeDriftDimensions) diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py b/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py index bb083877b5..fc84d28b6c 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py @@ -22,10 +22,7 @@ CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' # jog_arm should publish a nonzero warning code here -HALT_TOPIC = 'jog_server/status' - -# Check if jogger is initialized with this service -SERVICE_NAME = 'jog_server/change_drift_dimensions' +STATUS_TOPIC = 'jog_server/status' @pytest.fixture @@ -48,11 +45,11 @@ def send_cmd(self, linear, angular): def test_jog_arm_halt_msg(node): - assert util.wait_for_jogger_initialization(SERVICE_NAME) + assert util.wait_for_jogger_initialization() received = [] sub = rospy.Subscriber( - HALT_TOPIC, Int8, lambda msg: received.append(msg) + STATUS_TOPIC, Int8, lambda msg: received.append(msg) ) cartesian_cmd = CartesianJogCmd() @@ -68,8 +65,7 @@ def test_jog_arm_halt_msg(node): # Check the received messages # A non-zero value signifies a warning assert len(received) > 3 - assert (received[-1].data != 0) or (received[-2].data != 0) or (received[-3].data != 0) - + assert any(i != 0 for i in received[-3:]) if __name__ == '__main__': node = node() diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py b/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py index 9147559d43..40714f42d0 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py @@ -22,9 +22,6 @@ COMMAND_OUT_TOPIC = 'jog_server/command' -# Check if jogger is initialized with this service -SERVICE_NAME = 'jog_server/change_drift_dimensions' - @pytest.fixture def node(): @@ -60,7 +57,7 @@ def send_cmd(self, linear, angular): def test_jog_arm_cartesian_command(node): # Test sending a cartesian velocity command - assert util.wait_for_jogger_initialization(SERVICE_NAME) + assert util.wait_for_jogger_initialization() received = [] sub = rospy.Subscriber( @@ -70,10 +67,9 @@ def test_jog_arm_cartesian_command(node): # Repeated zero-commands should produce no output, other than a few halt messages # A subscriber in a different thread fills 'received' - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) + for i in range(4): + cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) + rospy.sleep(0.1) received = [] rospy.sleep(1) assert len(received) <= 4 # 'num_outgoing_halt_msgs_to_publish' in the config file @@ -100,7 +96,7 @@ def test_jog_arm_cartesian_command(node): def test_jog_arm_joint_command(node): # Test sending a joint command - assert util.wait_for_jogger_initialization(SERVICE_NAME) + assert util.wait_for_jogger_initialization() received = [] sub = rospy.Subscriber( diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/util.py b/moveit_experimental/moveit_jog_arm/test/python_tests/util.py index 0ee323b8c7..a76a681d40 100644 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/util.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/util.py @@ -1,10 +1,14 @@ import rospy +from std_msgs.msg import Int8 -def wait_for_jogger_initialization(service_name): +# jog_arm should publish a nonzero warning code here +STATUS_TOPIC = 'jog_server/status' + +def wait_for_jogger_initialization(timeout=15): try: - rospy.wait_for_service(service_name, timeout=15) + rospy.wait_for_message(STATUS_TOPIC, Int8, timeout=timeout) except rospy.ROSException as exc: - rospy.logerr("The jogger service " + service_name + " is not available: " + str(exc)) + rospy.logerr("The jogger topic " + STATUS_TOPIC + " is not available: " + str(exc)) return False return True diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py b/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py index 2baf6a594a..e9bb8665ec 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py @@ -20,9 +20,6 @@ COMMAND_OUT_TOPIC = 'jog_server/command' -# Check if jogger is initialized with this service -SERVICE_NAME = 'jog_server/change_drift_dimensions' - @pytest.fixture def node(): @@ -44,7 +41,7 @@ def send_joint_velocity_cmd(self, joint_pos): def test_vel_limit(node): # Test sending a joint command - assert util.wait_for_jogger_initialization(SERVICE_NAME) + assert util.wait_for_jogger_initialization() received = [] sub = rospy.Subscriber( From 39881a56dfe3f8801b486ceab3ff9740e5149f06 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 29 May 2020 16:50:27 -0600 Subject: [PATCH 324/612] lower polling rate in setup and increase thread count consistantly to 8 --- .../moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h | 2 +- .../src/cpp_interface_example/cpp_interface_example.cpp | 2 +- moveit_experimental/moveit_jog_arm/src/jog_arm.cpp | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index cc789795b7..0054cf2d56 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -262,7 +262,7 @@ class JogCalcs std::array control_dimensions_ = { { true, true, true, true, true, true } }; // Amount we sleep when waiting - ros::Rate default_sleep_rate_ = 1000; + ros::Rate default_sleep_rate_ = 100; // latest_state_mutex_ is used to protect the state below it mutable std::mutex latest_state_mutex_; diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index 0b2617f012..cdd025e493 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -76,7 +76,7 @@ int main(int argc, char** argv) { ros::init(argc, argv, LOGNAME); ros::NodeHandle nh; - ros::AsyncSpinner spinner(4); + ros::AsyncSpinner spinner(8); spinner.start(); // Load the planning scene monitor diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index 3afe8d0464..5bf1b81785 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -381,7 +381,6 @@ void JogArm::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr& m void JogArm::okToPublishCB(const std_msgs::BoolConstPtr& msg) { - const std::lock_guard lock(latest_state_mutex_); ok_to_publish_ = msg->data; } From dfbcd487f1941d5ece6c9ffb161d0a7be04076fc Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 30 May 2020 12:39:36 -0600 Subject: [PATCH 325/612] use condional log for warning when loop rate is too long --- .../moveit_jog_arm/src/collision_check_thread.cpp | 15 ++++----------- .../moveit_jog_arm/src/jog_arm.cpp | 15 ++++----------- .../moveit_jog_arm/src/jog_calcs.cpp | 15 ++++----------- 3 files changed, 12 insertions(+), 33 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 98bac5056e..23cdc2c094 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -104,17 +104,10 @@ void CollisionCheckThread::stop() void CollisionCheckThread::run(const ros::TimerEvent& timer_event) { - // Log last loop duration and warn if it was longer than period - if (timer_event.profile.last_duration.toSec() < period_.toSec()) - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " (" - << period_.toSec() << ")"); - } - else - { - ROS_WARN_STREAM_THROTTLE_NAMED(1, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " > " - << period_.toSec()); - } + // Log warning when the last loop duration was longer than the period + ROS_WARN_STREAM_COND_NAMED(timer_event.profile.last_duration.toSec() > period_.toSec(), LOGNAME, + "last_duration: " << timer_event.profile.last_duration.toSec() << " (" << period_.toSec() + << ")"); if (paused_) { diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index 5bf1b81785..7f2d94637b 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -290,17 +290,10 @@ bool JogArm::readParameters() void JogArm::run(const ros::TimerEvent& timer_event) { - // Log last loop duration and warn if it was longer than period - if (timer_event.profile.last_duration.toSec() < period_.toSec()) - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " (" - << period_.toSec() << ")"); - } - else - { - ROS_WARN_STREAM_THROTTLE_NAMED(1, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " > " - << period_.toSec()); - } + // Log warning when the last loop duration was longer than the period + ROS_WARN_STREAM_COND_NAMED(timer_event.profile.last_duration.toSec() > period_.toSec(), LOGNAME, + "last_duration: " << timer_event.profile.last_duration.toSec() << " (" << period_.toSec() + << ")"); // Get the latest joint trajectory trajectory_msgs::JointTrajectory joint_trajectory; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index e56768d585..efe984943d 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -158,17 +158,10 @@ void JogCalcs::stop() void JogCalcs::run(const ros::TimerEvent& timer_event) { - // Log last loop duration and warn if it was longer than period - if (timer_event.profile.last_duration.toSec() < period_.toSec()) - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " (" - << period_.toSec() << ")"); - } - else - { - ROS_WARN_STREAM_THROTTLE_NAMED(1, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " > " - << period_.toSec()); - } + // Log warning when the last loop duration was longer than the period + ROS_WARN_STREAM_COND_NAMED(timer_event.profile.last_duration.toSec() > period_.toSec(), LOGNAME, + "last_duration: " << timer_event.profile.last_duration.toSec() << " (" << period_.toSec() + << ")"); // Publish status each loop iteration { From af62e15598a1156e9a5951ef8b00e71b8fd73113 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 30 May 2020 13:47:06 -0600 Subject: [PATCH 326/612] outgoing message as pool allocated shared pointer for zero copy --- .../include/moveit_jog_arm/jog_calcs.h | 17 +++-- .../moveit_jog_arm/src/jog_calcs.cpp | 68 ++++++++++--------- 2 files changed, 43 insertions(+), 42 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 0054cf2d56..a6d31ed25b 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -87,10 +87,10 @@ class JogCalcs void run(const ros::TimerEvent& timer_event); /** \brief Do jogging calculations for Cartesian twist commands. */ - bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd); + bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); /** \brief Do jogging calculations for direct commands to a joint. */ - bool jointJogCalcs(const control_msgs::JointJog& cmd); + bool jointJogCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); /** \brief Parse the incoming joint msg for the joints of our MoveGroup */ bool updateJoints(); @@ -110,14 +110,14 @@ class JogCalcs /** \brief Suddenly halt for a joint limit or other critical issue. * Is handled differently for position vs. velocity control. */ - void suddenHalt(trajectory_msgs::JointTrajectory& joint_traj); + void suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory); void suddenHalt(Eigen::ArrayXd& delta_theta); /** \brief Scale the delta theta to match joint velocity/acceleration limits */ void enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta); /** \brief Avoid overshooting joint limits */ - bool enforceSRDFPositionLimits(trajectory_msgs::JointTrajectory& new_joint_traj); + bool enforceSRDFPositionLimits(); /** \brief Possibly calculate a velocity scaling factor, due to proximity of * singularity and direction of motion @@ -134,7 +134,8 @@ class JogCalcs void applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singularity_scale); /** \brief Compose the outgoing JointTrajectory message */ - trajectory_msgs::JointTrajectory composeJointTrajMessage(sensor_msgs::JointState& joint_state) const; + void composeJointTrajMessage(sensor_msgs::JointState& joint_state, + trajectory_msgs::JointTrajectory& joint_trajectory) const; /** \brief Smooth position commands with a lowpass filter */ void lowPassFilterPositions(sensor_msgs::JointState& joint_state); @@ -145,12 +146,12 @@ class JogCalcs /** \brief Convert joint deltas to an outgoing JointTrajectory command. * This happens for joint commands and Cartesian commands. */ - bool convertDeltasToOutgoingCmd(); + bool convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& joint_trajectory); /** \brief Gazebo simulations have very strict message timestamp requirements. * Satisfy Gazebo by stuffing multiple messages into one. */ - void insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& trajectory, int count) const; + void insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& joint_trajectory, int count) const; /** * Remove the Jacobian row and the delta-x element of one Cartesian dimension, to take advantage of task redundancy @@ -221,8 +222,6 @@ class JogCalcs sensor_msgs::JointState internal_joint_state_, original_joint_state_; std::map joint_state_name_map_; - trajectory_msgs::JointTrajectory outgoing_command_; - std::vector position_filters_; // ROS diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index efe984943d..06542e5e53 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -222,17 +222,21 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // Do jogging calculations only if the robot should move, for efficiency else { + // Create new outgoing joint trajectory command message + auto joint_trajectory = make_shared_from_pool(); + + // If the input command isn't stale, run calculations if (!command_is_stale_) { // Prioritize cartesian jogging above joint jogging if (have_nonzero_twist_stamped_) { - if (!cartesianJogCalcs(twist_stamped_)) + if (!cartesianJogCalcs(twist_stamped_, *joint_trajectory)) return; } else if (have_nonzero_joint_jog_) { - if (!jointJogCalcs(joint_jog_)) + if (!jointJogCalcs(joint_jog_, *joint_trajectory)) return; } } @@ -244,7 +248,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) for (std::size_t i = 0; i < num_joints_; ++i) position_filters_[i].reset(original_joint_state_.position[i]); - suddenHalt(outgoing_command_); + suddenHalt(*joint_trajectory); have_nonzero_twist_stamped_ = false; have_nonzero_joint_jog_ = false; } @@ -253,7 +257,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // If everything normal, share the new traj to be published if (have_nonzero_command_) { - joint_trajectory_pub_.publish(outgoing_command_); + joint_trajectory_pub_.publish(joint_trajectory); auto bool_msg = boost::make_shared(); bool_msg->data = true; @@ -272,7 +276,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // The command is invalid but we are publishing num_outgoing_halt_msgs_to_publish else { - joint_trajectory_pub_.publish(outgoing_command_); + joint_trajectory_pub_.publish(joint_trajectory); auto bool_msg = boost::make_shared(); bool_msg->data = true; @@ -298,7 +302,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) } } // Perform the jogging calculations -bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd) +bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory) { // Check for nan's in the incoming command if (std::isnan(cmd.twist.linear.x) || std::isnan(cmd.twist.linear.y) || std::isnan(cmd.twist.linear.z) || @@ -394,10 +398,10 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd) prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; - return convertDeltasToOutgoingCmd(); + return convertDeltasToOutgoingCmd(joint_trajectory); } -bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd) +bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory) { // Check for nan's or |delta|>1 in the incoming command for (double velocity : cmd.velocities) @@ -418,10 +422,10 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd) prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; - return convertDeltasToOutgoingCmd(); + return convertDeltasToOutgoingCmd(joint_trajectory); } -bool JogCalcs::convertDeltasToOutgoingCmd() +bool JogCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& joint_trajectory) { internal_joint_state_ = original_joint_state_; if (!addJointIncrements(internal_joint_state_, delta_theta_)) @@ -432,18 +436,18 @@ bool JogCalcs::convertDeltasToOutgoingCmd() // Calculate joint velocities here so that positions are filtered and SRDF bounds still get checked calculateJointVelocities(internal_joint_state_, delta_theta_); - outgoing_command_ = composeJointTrajMessage(internal_joint_state_); + composeJointTrajMessage(internal_joint_state_, joint_trajectory); - if (!enforceSRDFPositionLimits(outgoing_command_)) + if (!enforceSRDFPositionLimits()) { - suddenHalt(outgoing_command_); + suddenHalt(joint_trajectory); status_ = StatusCode::JOINT_BOUND; } // done with calculations if (parameters_.use_gazebo) { - insertRedundantPointsIntoTrajectory(outgoing_command_, gazebo_redundant_message_count_); + insertRedundantPointsIntoTrajectory(joint_trajectory, gazebo_redundant_message_count_); } return true; @@ -452,14 +456,14 @@ bool JogCalcs::convertDeltasToOutgoingCmd() // Spam several redundant points into the trajectory. The first few may be skipped if the // time stamp is in the past when it reaches the client. Needed for gazebo simulation. // Start from 2 because the first point's timestamp is already 1*parameters_.publish_period -void JogCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& trajectory, int count) const +void JogCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& joint_trajectory, int count) const { - auto point = trajectory.points[0]; + auto point = joint_trajectory.points[0]; // Start from 2 because we already have the first point. End at count+1 so (total #) == count for (int i = 2; i < count + 1; ++i) { point.time_from_start = ros::Duration(i * parameters_.publish_period); - trajectory.points.push_back(point); + joint_trajectory.points.push_back(point); } } @@ -479,12 +483,12 @@ void JogCalcs::calculateJointVelocities(sensor_msgs::JointState& joint_state, co } } -trajectory_msgs::JointTrajectory JogCalcs::composeJointTrajMessage(sensor_msgs::JointState& joint_state) const +void JogCalcs::composeJointTrajMessage(sensor_msgs::JointState& joint_state, + trajectory_msgs::JointTrajectory& joint_trajectory) const { - trajectory_msgs::JointTrajectory new_joint_traj; - new_joint_traj.header.frame_id = parameters_.planning_frame; - new_joint_traj.header.stamp = ros::Time::now(); - new_joint_traj.joint_names = joint_state.name; + joint_trajectory.header.frame_id = parameters_.planning_frame; + joint_trajectory.header.stamp = ros::Time::now(); + joint_trajectory.joint_names = joint_state.name; trajectory_msgs::JointTrajectoryPoint point; point.time_from_start = ros::Duration(parameters_.publish_period); @@ -500,9 +504,7 @@ trajectory_msgs::JointTrajectory JogCalcs::composeJointTrajMessage(sensor_msgs:: std::vector acceleration(num_joints_); point.accelerations = acceleration; } - new_joint_traj.points.push_back(point); - - return new_joint_traj; + joint_trajectory.points.push_back(point); } // Apply velocity scaling for proximity of collisions and singularities. @@ -665,7 +667,7 @@ void JogCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta) } } -bool JogCalcs::enforceSRDFPositionLimits(trajectory_msgs::JointTrajectory& new_joint_traj) +bool JogCalcs::enforceSRDFPositionLimits() { bool halting = false; @@ -713,24 +715,24 @@ void JogCalcs::suddenHalt(Eigen::ArrayXd& delta_theta) // Suddenly halt for a joint limit or other critical issue. // Is handled differently for position vs. velocity control. -void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_traj) +void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory) { - if (joint_traj.points.empty()) + if (joint_trajectory.points.empty()) { - joint_traj.points.push_back(trajectory_msgs::JointTrajectoryPoint()); - joint_traj.points[0].positions.resize(num_joints_); - joint_traj.points[0].velocities.resize(num_joints_); + joint_trajectory.points.push_back(trajectory_msgs::JointTrajectoryPoint()); + joint_trajectory.points[0].positions.resize(num_joints_); + joint_trajectory.points[0].velocities.resize(num_joints_); } for (std::size_t i = 0; i < num_joints_; ++i) { // For position-controlled robots, can reset the joints to a known, good state if (parameters_.publish_joint_positions) - joint_traj.points[0].positions[i] = original_joint_state_.position[i]; + joint_trajectory.points[0].positions[i] = original_joint_state_.position[i]; // For velocity-controlled robots, stop if (parameters_.publish_joint_velocities) - joint_traj.points[0].velocities[i] = 0; + joint_trajectory.points[0].velocities[i] = 0; } } From 9d5d3d66abf085c0c5d4486e5bdf2a534031e555 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 30 May 2020 14:19:49 -0600 Subject: [PATCH 327/612] ok_to_publish as just a bool --- .../include/moveit_jog_arm/jog_arm.h | 3 --- .../include/moveit_jog_arm/jog_calcs.h | 6 ++++-- .../moveit_jog_arm/src/jog_arm.cpp | 8 +------- .../moveit_jog_arm/src/jog_calcs.cpp | 20 ++++++++----------- 4 files changed, 13 insertions(+), 24 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h index c3d0a75f6d..0c964a9dde 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h @@ -91,7 +91,6 @@ class JogArm bool readParameters(); void run(const ros::TimerEvent& timer_event); void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr& msg); - void okToPublishCB(const std_msgs::BoolConstPtr& msg); ros::NodeHandle nh_; @@ -112,10 +111,8 @@ class JogArm ros::Duration period_; ros::Publisher outgoing_cmd_pub_; ros::Subscriber joint_trajectory_sub_; - ros::Subscriber ok_to_publish_sub_; bool paused_ = false; - bool ok_to_publish_ = false; // latest_state_mutex_ is used to protect the state below it mutable std::mutex latest_state_mutex_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index a6d31ed25b..7dc9afb04a 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -82,6 +82,9 @@ class JogCalcs /** \brief Pause or unpause processing jog commands while keeping the threads alive */ void setPaused(bool paused); + /** \brief Accessor for ok_to_publish_ */ + bool getOkToPublish() const; + private: /** \brief Timer method */ void run(const ros::TimerEvent& timer_event); @@ -237,13 +240,12 @@ class JogCalcs ros::ServiceServer drift_dimensions_server_; ros::ServiceServer control_dimensions_server_; - ros::Publisher ok_to_publish_pub_; - // Status StatusCode status_ = StatusCode::NO_WARNING; bool stop_requested_ = false; bool paused_ = false; bool command_is_stale_ = false; + bool ok_to_publish_ = false; double collision_velocity_scale_ = 1.0; // Use ArrayXd type to enable more coefficient-wise operations diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index 7f2d94637b..8c24245e1a 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -63,7 +63,6 @@ JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneM // Subscribe to internal namespace ros::NodeHandle internal_nh("~internal"); joint_trajectory_sub_ = internal_nh.subscribe("joint_trajectory", 1, &JogArm::jointTrajectoryCB, this); - ok_to_publish_sub_ = internal_nh.subscribe("ok_to_publish", 1, &JogArm::okToPublishCB, this); // Wait for incoming topics to appear ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); @@ -306,7 +305,7 @@ void JogArm::run(const ros::TimerEvent& timer_event) } // Publish the most recent trajectory, unless the jogging calculation thread tells not to - if (ok_to_publish_ && !paused_) + if (jog_calcs_->getOkToPublish() && !paused_) { // Put the outgoing msg in the right format // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). @@ -372,11 +371,6 @@ void JogArm::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr& m latest_joint_trajectory_ = msg; } -void JogArm::okToPublishCB(const std_msgs::BoolConstPtr& msg) -{ - ok_to_publish_ = msg->data; -} - const JogArmParameters& JogArm::getParameters() const { return parameters_; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 06542e5e53..62d865e459 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -109,7 +109,6 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, // Publish and Subscribe to internal namespace topics ros::NodeHandle internal_nh("~internal"); joint_trajectory_pub_ = internal_nh.advertise("joint_trajectory", 1); - ok_to_publish_pub_ = internal_nh.advertise("ok_to_publish", 1); collision_velocity_scale_sub_ = internal_nh.subscribe("collision_velocity_scale", 1, &JogCalcs::collisionVelocityScaleCB, this); worst_case_stop_time_pub_ = internal_nh.advertise("worst_case_stop_time", 1); @@ -258,29 +257,21 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) if (have_nonzero_command_) { joint_trajectory_pub_.publish(joint_trajectory); - - auto bool_msg = boost::make_shared(); - bool_msg->data = true; - ok_to_publish_pub_.publish(bool_msg); + ok_to_publish_ = true; } // Skip the jogging publication if all inputs have been zero for several cycles in a row. // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. else if ((parameters_.num_outgoing_halt_msgs_to_publish != 0) && (zero_velocity_count_ > parameters_.num_outgoing_halt_msgs_to_publish)) { - auto bool_msg = boost::make_shared(); - bool_msg->data = false; - ok_to_publish_pub_.publish(bool_msg); + ok_to_publish_ = false; ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); } // The command is invalid but we are publishing num_outgoing_halt_msgs_to_publish else { joint_trajectory_pub_.publish(joint_trajectory); - - auto bool_msg = boost::make_shared(); - bool_msg->data = true; - ok_to_publish_pub_.publish(bool_msg); + ok_to_publish_ = true; } // Store last zero-velocity message flag to prevent superfluous warnings. @@ -991,4 +982,9 @@ void JogCalcs::setPaused(bool paused) paused_ = paused; } +bool JogCalcs::getOkToPublish() const +{ + return ok_to_publish_; +} + } // namespace moveit_jog_arm From d8355a3c998beee59ef18754aed83614a434e50d Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 30 May 2020 14:57:35 -0600 Subject: [PATCH 328/612] renames for clarity and correctness --- .../moveit_jog_arm/CMakeLists.txt | 4 +- ...ision_check_thread.h => collision_check.h} | 14 +++---- .../include/moveit_jog_arm/jog_arm.h | 14 +++---- .../{jog_arm_data.h => jog_arm_parameters.h} | 2 +- .../include/moveit_jog_arm/jog_calcs.h | 8 ++-- ...l_allocation.h => make_shared_from_pool.h} | 9 +++- ...n_check_thread.cpp => collision_check.cpp} | 41 +++++++++---------- .../cpp_interface_example.cpp | 8 ++-- .../moveit_jog_arm/src/jog_arm.cpp | 8 ++-- .../moveit_jog_arm/src/jog_calcs.cpp | 12 +++--- .../test/jog_cpp_interface_test.cpp | 6 +-- .../halt_msg/test_jog_arm_halt_msg.py | 2 +- .../test_jog_arm_msg_reception.py | 4 +- 13 files changed, 69 insertions(+), 63 deletions(-) rename moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/{collision_check_thread.h => collision_check.h} (92%) rename moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/{jog_arm_data.h => jog_arm_parameters.h} (98%) rename moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/{boost_pool_allocation.h => make_shared_from_pool.h} (94%) rename moveit_experimental/moveit_jog_arm/src/{collision_check_thread.cpp => collision_check.cpp} (86%) diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index b816413b7b..1e2701203b 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -55,7 +55,7 @@ include_directories( ######################################### add_library(${LIBRARY_NAME} SHARED - src/collision_check_thread.cpp + src/collision_check.cpp src/jog_calcs.cpp src/jog_arm.cpp src/low_pass_filter.cpp @@ -84,7 +84,7 @@ target_link_libraries(cpp_interface_example ############################ add_executable(jog_server - src/collision_check_thread.cpp + src/collision_check.cpp src/jog_calcs.cpp src/jog_arm.cpp src/jog_server.cpp diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h similarity index 92% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h rename to moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h index 23b19035de..e3786f18c2 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h @@ -1,5 +1,5 @@ /******************************************************************************* - * Title : collision_check_thread.h + * Title : collision_check.h * Project : moveit_jog_arm * Created : 1/11/2019 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson @@ -45,7 +45,7 @@ #include #include -#include "jog_arm_data.h" +#include "jog_arm_parameters.h" #include "low_pass_filter.h" namespace moveit_jog_arm @@ -56,7 +56,7 @@ enum CollisionCheckType K_STOP_DISTANCE = 2 }; -class CollisionCheckThread +class CollisionCheck { public: /** \brief Constructor @@ -64,14 +64,14 @@ class CollisionCheckThread * \param planning_scene_monitor: PSM should have scene monitor and state monitor * already started when passed into this class */ - CollisionCheckThread(ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - /** \brief start and stop the Thread */ + /** \brief start and stop the Timer */ void start(); void stop(); - /** \brief Pause or unpause processing jog commands while keeping the threads alive */ + /** \brief Pause or unpause processing jog commands while keeping the timers alive */ void setPaused(bool paused); private: diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h index 0c964a9dde..161fcdeae0 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h @@ -48,8 +48,8 @@ #include #include -#include "collision_check_thread.h" -#include "jog_arm_data.h" +#include "collision_check.h" +#include "jog_arm_parameters.h" #include "jog_calcs.h" #include "low_pass_filter.h" @@ -57,7 +57,7 @@ namespace moveit_jog_arm { /** * Class JogArm - Base class for C++ interface and ROS node. - * Handles ROS subs & pubs and creates the worker threads. + * Handles ROS subs & pubs and creates the worker timers. */ class JogArm { @@ -66,13 +66,13 @@ class JogArm ~JogArm(); - /** \brief start jog arm threads */ + /** \brief start jog arm timers */ void start(); - /** \brief stop jog arm threads */ + /** \brief stop jog arm timers */ void stop(); - /** \brief Pause or unpause processing jog commands while keeping the threads alive */ + /** \brief Pause or unpause processing jog commands while keeping the timers alive */ void setPaused(bool paused); /** @@ -104,7 +104,7 @@ class JogArm std::unique_ptr jog_calcs_; // Collision checks - std::unique_ptr collision_checker_; + std::unique_ptr collision_checker_; // ROS ros::Timer timer_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h similarity index 98% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h rename to moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h index ad680028df..aa29ee40ab 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h @@ -1,5 +1,5 @@ /******************************************************************************* - * Title : jog_arm_data.h + * Title : jog_arm_parameters.h * Project : moveit_jog_arm * Created : 1/11/2019 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 7dc9afb04a..16883a8a42 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -54,7 +54,7 @@ #include // moveit_jog_arm -#include "jog_arm_data.h" +#include "jog_arm_parameters.h" #include "low_pass_filter.h" #include "status_codes.h" @@ -66,7 +66,7 @@ class JogCalcs JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - /** \brief Start and stop the timer (thread) */ + /** \brief Start and stop the timer */ void start(); void stop(); @@ -79,7 +79,7 @@ class JogCalcs */ bool getCommandFrameTransform(Eigen::Isometry3d& transform); - /** \brief Pause or unpause processing jog commands while keeping the threads alive */ + /** \brief Pause or unpause processing jog commands while keeping the timers alive */ void setPaused(bool paused); /** \brief Accessor for ok_to_publish_ */ @@ -184,7 +184,7 @@ class JogCalcs bool changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, moveit_msgs::ChangeDriftDimensions::Response& res); - /** \brief Start the main calculation thread */ + /** \brief Start the main calculation timer */ // Service callback for changing jogging dimensions bool changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, moveit_msgs::ChangeControlDimensions::Response& res); diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/boost_pool_allocation.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/make_shared_from_pool.h similarity index 94% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/boost_pool_allocation.h rename to moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/make_shared_from_pool.h index ec1e550f74..ca4243625e 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/boost_pool_allocation.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/make_shared_from_pool.h @@ -1,5 +1,5 @@ /******************************************************************************* - * Title : boost_pool_allocation.h + * Title : make_shared_from_pool.h * Project : moveit_jog_arm * Created : 1/11/2019 * Author : Tyler Weaver @@ -40,6 +40,10 @@ #include +namespace moveit +{ +namespace util +{ // Useful template for creating messages from a message pool template boost::shared_ptr make_shared_from_pool() @@ -47,3 +51,6 @@ boost::shared_ptr make_shared_from_pool() using allocator_t = boost::fast_pool_allocator>; return boost::allocate_shared(allocator_t()); } + +} // namespace util +} // namespace moveit diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp similarity index 86% rename from moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp rename to moveit_experimental/moveit_jog_arm/src/collision_check.cpp index 23cdc2c094..3ad3e704d9 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp @@ -31,7 +31,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -/* Title : collision_check_thread.cpp +/* Title : collision_check.cpp * Project : moveit_jog_arm * Created : 1/11/2019 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson @@ -39,19 +39,18 @@ #include -#include -#include +#include +#include -static const std::string LOGNAME = "collision_check_thread"; -static constexpr double MIN_RECOMMENDED_COLLISION_RATE = 10; +static const char LOGNAME[] = "collision_check"; +static const double MIN_RECOMMENDED_COLLISION_RATE = 10; constexpr double EPSILON = 1e-6; // For very small numeric comparisons namespace moveit_jog_arm { // Constructor for the class that handles collision checking -CollisionCheckThread::CollisionCheckThread( - ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) +CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : nh_(nh) , parameters_(parameters) , planning_scene_monitor_(planning_scene_monitor) @@ -67,7 +66,7 @@ CollisionCheckThread::CollisionCheckThread( ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Collision check rate is low, increase it in yaml file if CPU allows"); // subscribe to joints - joint_state_sub_ = nh_.subscribe(parameters.joint_topic, 1, &CollisionCheckThread::jointStateCB, this); + joint_state_sub_ = nh_.subscribe(parameters.joint_topic, 1, &CollisionCheck::jointStateCB, this); collision_check_type_ = (parameters_.collision_check_type == "threshold_distance" ? K_THRESHOLD_DISTANCE : K_STOP_DISTANCE); @@ -77,7 +76,7 @@ CollisionCheckThread::CollisionCheckThread( ros::NodeHandle internal_nh("~internal"); collision_velocity_scale_pub_ = internal_nh.advertise("collision_velocity_scale", 1); worst_case_stop_time_sub_ = - internal_nh.subscribe("worst_case_stop_time", 2, &CollisionCheckThread::worstCaseStopTimeCB, this); + internal_nh.subscribe("worst_case_stop_time", 2, &CollisionCheck::worstCaseStopTimeCB, this); // Wait for incoming topics to appear ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); @@ -87,22 +86,22 @@ CollisionCheckThread::CollisionCheckThread( acm_ = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); } -planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPlanningSceneRO() const +planning_scene_monitor::LockedPlanningSceneRO CollisionCheck::getLockedPlanningSceneRO() const { return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); } -void CollisionCheckThread::start() +void CollisionCheck::start() { - timer_ = nh_.createTimer(period_, &CollisionCheckThread::run, this); + timer_ = nh_.createTimer(period_, &CollisionCheck::run, this); } -void CollisionCheckThread::stop() +void CollisionCheck::stop() { timer_.stop(); } -void CollisionCheckThread::run(const ros::TimerEvent& timer_event) +void CollisionCheck::run(const ros::TimerEvent& timer_event) { // Log warning when the last loop duration was longer than the period ROS_WARN_STREAM_COND_NAMED(timer_event.profile.last_duration.toSec() > period_.toSec(), LOGNAME, @@ -116,7 +115,7 @@ void CollisionCheckThread::run(const ros::TimerEvent& timer_event) { // Copy the latest joint state - const std::lock_guard lock(CollisionCheckThread); + const std::lock_guard lock(CollisionCheck); for (std::size_t i = 0; i < latest_joint_state_->position.size(); ++i) current_state_->setJointPositions(latest_joint_state_->name[i], &latest_joint_state_->position[i]); } @@ -124,7 +123,7 @@ void CollisionCheckThread::run(const ros::TimerEvent& timer_event) current_state_->updateCollisionBodyTransforms(); collision_detected_ = false; - // Do a thread-safe distance-based collision detection + // Do a timer-safe distance-based collision detection collision_result_.clear(); getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, *current_state_); @@ -200,24 +199,24 @@ void CollisionCheckThread::run(const ros::TimerEvent& timer_event) // publish message { - auto msg = make_shared_from_pool(); + auto msg = moveit::util::make_shared_from_pool(); msg->data = velocity_scale_; collision_velocity_scale_pub_.publish(msg); } } -void CollisionCheckThread::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) +void CollisionCheck::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) { const std::lock_guard lock(joint_state_mutex_); latest_joint_state_ = msg; } -void CollisionCheckThread::worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg) +void CollisionCheck::worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg) { worst_case_stop_time_ = msg->data; } -void CollisionCheckThread::setPaused(bool paused) +void CollisionCheck::setPaused(bool paused) { paused_ = paused; } diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index cdd025e493..a64428bb90 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -40,7 +40,7 @@ #include #include -#include +#include static const std::string LOGNAME = "cpp_interface_example"; @@ -95,7 +95,7 @@ int main(int argc, char** argv) false /* skip octomap monitor */); planning_scene_monitor->startStateMonitor(); - // Run the jogging C++ interface in a new thread to ensure a constant outgoing message rate. + // Run the jogging C++ interface in a new timer to ensure a constant outgoing message rate. moveit_jog_arm::JogArm jog_arm(nh, planning_scene_monitor); jog_arm.start(); @@ -116,7 +116,7 @@ int main(int argc, char** argv) // Make a Cartesian velocity message // Messages are sent to jogger as boost::shared_ptr to enable zero-copy message_passing. // Because this message is not coppied we should not modify it after we send it. - auto msg = make_shared_from_pool(); + auto msg = moveit::util::make_shared_from_pool(); msg->header.stamp = ros::Time::now(); msg->header.frame_id = "base_link"; msg->twist.linear.y = 0.01; @@ -139,7 +139,7 @@ int main(int argc, char** argv) // Make a joint command // Messages are sent to jogger as boost::shared_ptr to enable zero-copy message_passing. // Because this message is not coppied we should not modify it after we send it. - auto msg = make_shared_from_pool(); + auto msg = moveit::util::make_shared_from_pool(); msg->header.stamp = ros::Time::now(); msg->joint_names.push_back("elbow_joint"); msg->velocities.push_back(0.2); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index 8c24245e1a..cf4faadefd 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -70,7 +70,7 @@ JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneM jog_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); - collision_checker_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); + collision_checker_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); } // Read ROS parameters, typically from YAML file @@ -304,7 +304,7 @@ void JogArm::run(const ros::TimerEvent& timer_event) } } - // Publish the most recent trajectory, unless the jogging calculation thread tells not to + // Publish the most recent trajectory, unless the jogging calculation timer tells not to if (jog_calcs_->getOkToPublish() && !paused_) { // Put the outgoing msg in the right format @@ -330,10 +330,10 @@ void JogArm::start() { setPaused(false); - // Crunch the numbers in this thread + // Crunch the numbers in this timer jog_calcs_->start(); - // Check collisions in this thread + // Check collisions in this timer if (parameters_.check_collisions) collision_checker_->start(); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 62d865e459..b88f60fc86 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -40,7 +40,7 @@ #include #include -#include +#include static const std::string LOGNAME = "jog_calcs"; @@ -114,9 +114,9 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, worst_case_stop_time_pub_ = internal_nh.advertise("worst_case_stop_time", 1); // Wait for initial messages - ROS_INFO_NAMED(LOGNAME, "jog_calcs_thread: Waiting for first joint msg."); + ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg."); ros::topic::waitForMessage(parameters_.joint_topic); - ROS_INFO_NAMED(LOGNAME, "jog_calcs_thread: Received first joint msg."); + ROS_INFO_NAMED(LOGNAME, "Received first joint msg."); internal_joint_state_.name = joint_model_group_->getActiveJointModelNames(); num_joints_ = internal_joint_state_.name.size(); @@ -164,7 +164,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // Publish status each loop iteration { - auto status_msg = make_shared_from_pool(); + auto status_msg = moveit::util::make_shared_from_pool(); status_msg->data = static_cast(status_); status_pub_.publish(status_msg); } @@ -222,7 +222,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) else { // Create new outgoing joint trajectory command message - auto joint_trajectory = make_shared_from_pool(); + auto joint_trajectory = moveit::util::make_shared_from_pool(); // If the input command isn't stale, run calculations if (!command_is_stale_) @@ -799,7 +799,7 @@ bool JogCalcs::updateJoints() // publish message { - auto msg = make_shared_from_pool(); + auto msg = moveit::util::make_shared_from_pool(); msg->data = worst_case_stop_time; worst_case_stop_time_pub_.publish(msg); } diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp index 61bb79c760..0e52397f9d 100644 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp @@ -47,7 +47,7 @@ // Jog Arm #include -#include +#include static const std::string LOGNAME = "jog_cpp_interface_test"; @@ -120,7 +120,7 @@ TEST_F(JogArmFixture, SendTwistStampedTest) // Send a few Cartesian velocity commands for (size_t i = 0; i < num_commands && ros::ok(); ++i) { - auto msg = make_shared_from_pool(); + auto msg = moveit::util::make_shared_from_pool(); msg->header.stamp = ros::Time::now(); msg->header.frame_id = "panda_link0"; msg->twist.angular.y = 1.0; @@ -160,7 +160,7 @@ TEST_F(JogArmFixture, SendJointJogTest) // Send a few Cartesian velocity commands for (size_t i = 0; i < num_commands && ros::ok(); ++i) { - auto msg = make_shared_from_pool(); + auto msg = moveit::util::make_shared_from_pool(); msg->header.stamp = ros::Time::now(); msg->header.frame_id = "panda_link3"; msg->velocities.push_back(0.1); diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py b/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py index fc84d28b6c..edd540d02c 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py @@ -54,7 +54,7 @@ def test_jog_arm_halt_msg(node): cartesian_cmd = CartesianJogCmd() # This nonzero command should produce jogging output - # A subscriber in a different thread fills `received` + # A subscriber in a different timer fills `received` TEST_DURATION = 1 start_time = rospy.get_rostime() received = [] diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py b/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py index 40714f42d0..e2b9028169 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py @@ -66,7 +66,7 @@ def test_jog_arm_cartesian_command(node): cartesian_cmd = CartesianJogCmd() # Repeated zero-commands should produce no output, other than a few halt messages - # A subscriber in a different thread fills 'received' + # A subscriber in a different timer fills 'received' for i in range(4): cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) rospy.sleep(0.1) @@ -75,7 +75,7 @@ def test_jog_arm_cartesian_command(node): assert len(received) <= 4 # 'num_outgoing_halt_msgs_to_publish' in the config file # This nonzero command should produce jogging output - # A subscriber in a different thread fills `received` + # A subscriber in a different timer fills `received` TEST_DURATION = 1 PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file From 3d5fe8745b06ceda21188e4896ab3d8fbdc3878f Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 30 May 2020 15:04:43 -0600 Subject: [PATCH 329/612] start and stop twice in cpp test --- .../moveit_jog_arm/test/jog_cpp_interface_test.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp index 0e52397f9d..7b72e594e6 100644 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp @@ -89,7 +89,13 @@ class JogArmFixture : public ::testing::Test TEST_F(JogArmFixture, StartStopTest) { - // Start and stop jog arm + jog_arm_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + jog_arm_->stop(); + + ros::Duration(1.0).sleep(); + + // Start and stop again jog_arm_->start(); EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; jog_arm_->stop(); From ca92f7dd11a36a7e6661b74321c1425766a03b14 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 30 May 2020 16:15:58 -0600 Subject: [PATCH 330/612] attempt to fix kinetic node memory issue --- .../moveit_jog_arm/src/jog_calcs.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index b88f60fc86..4adf241018 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -163,11 +163,9 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) << ")"); // Publish status each loop iteration - { - auto status_msg = moveit::util::make_shared_from_pool(); - status_msg->data = static_cast(status_); - status_pub_.publish(status_msg); - } + auto status_msg = moveit::util::make_shared_from_pool(); + status_msg->data = static_cast(status_); + status_pub_.publish(status_msg); // Always update the joints and end-effector transform for 2 reasons: // 1) in case the getCommandFrameTransform() method is being used @@ -449,6 +447,12 @@ bool JogCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& join // Start from 2 because the first point's timestamp is already 1*parameters_.publish_period void JogCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& joint_trajectory, int count) const { + joint_trajectory.points.resize(count); + if (joint_trajectory.points.empty()) + { + joint_trajectory.points.push_back(trajectory_msgs::JointTrajectoryPoint()); + } + auto point = joint_trajectory.points[0]; // Start from 2 because we already have the first point. End at count+1 so (total #) == count for (int i = 2; i < count + 1; ++i) From 0627c0f7928761489aeed2e34e775bb7f642c22a Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 30 May 2020 19:33:49 -0600 Subject: [PATCH 331/612] named size for ros queues --- .../moveit_jog_arm/jog_arm_parameters.h | 3 +++ .../moveit_jog_arm/src/collision_check.cpp | 5 +++-- .../moveit_jog_arm/src/jog_arm.cpp | 6 +++--- .../moveit_jog_arm/src/jog_calcs.cpp | 20 ++++++++----------- .../halt_msg/test_jog_arm_halt_msg.py | 2 +- .../test_jog_arm_msg_reception.py | 4 ++-- .../vel_accel_limits/test_vel_accel_limits.py | 12 +++++++---- 7 files changed, 28 insertions(+), 24 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h index aa29ee40ab..a9e2cfa245 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h @@ -40,6 +40,9 @@ namespace moveit_jog_arm { +// Size of queues used in ros pub/sub/service +constexpr size_t ROS_QUEUE_SIZE = 2; + // ROS params to be read. See the yaml file in /config for a description of each. struct JogArmParameters { diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp index 3ad3e704d9..f3def372a8 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp @@ -66,7 +66,7 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArm ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Collision check rate is low, increase it in yaml file if CPU allows"); // subscribe to joints - joint_state_sub_ = nh_.subscribe(parameters.joint_topic, 1, &CollisionCheck::jointStateCB, this); + joint_state_sub_ = nh_.subscribe(parameters.joint_topic, ROS_QUEUE_SIZE, &CollisionCheck::jointStateCB, this); collision_check_type_ = (parameters_.collision_check_type == "threshold_distance" ? K_THRESHOLD_DISTANCE : K_STOP_DISTANCE); @@ -74,9 +74,10 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArm // Internal namespace ros::NodeHandle internal_nh("~internal"); + collision_velocity_scale_pub_ = internal_nh.advertise("collision_velocity_scale", ROS_QUEUE_SIZE); collision_velocity_scale_pub_ = internal_nh.advertise("collision_velocity_scale", 1); worst_case_stop_time_sub_ = - internal_nh.subscribe("worst_case_stop_time", 2, &CollisionCheck::worstCaseStopTimeCB, this); + internal_nh.subscribe("worst_case_stop_time", ROS_QUEUE_SIZE, &CollisionCheck::worstCaseStopTimeCB, this); // Wait for incoming topics to appear ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index cf4faadefd..d54823b420 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -56,13 +56,13 @@ JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneM // Publish freshly-calculated joints to the robot. // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, 1); + outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, ROS_QUEUE_SIZE); else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") - outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, 1); + outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, ROS_QUEUE_SIZE); // Subscribe to internal namespace ros::NodeHandle internal_nh("~internal"); - joint_trajectory_sub_ = internal_nh.subscribe("joint_trajectory", 1, &JogArm::jointTrajectoryCB, this); + joint_trajectory_sub_ = internal_nh.subscribe("joint_trajectory", ROS_QUEUE_SIZE, &JogArm::jointTrajectoryCB, this); // Wait for incoming topics to appear ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 4adf241018..aae69c4f24 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -90,11 +90,12 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getActiveJointModels().size()); // subscribe to joints - joint_state_sub_ = nh_.subscribe(parameters_.joint_topic, 1, &JogCalcs::jointStateCB, this); + joint_state_sub_ = nh_.subscribe(parameters_.joint_topic, ROS_QUEUE_SIZE, &JogCalcs::jointStateCB, this); // Subscribe to command topics - twist_stamped_sub_ = nh_.subscribe(parameters_.cartesian_command_in_topic, 100, &JogCalcs::twistStampedCB, this); - joint_jog_sub_ = nh_.subscribe(parameters_.joint_command_in_topic, 1, &JogCalcs::jointJogCB, this); + twist_stamped_sub_ = + nh_.subscribe(parameters_.cartesian_command_in_topic, ROS_QUEUE_SIZE, &JogCalcs::twistStampedCB, this); + joint_jog_sub_ = nh_.subscribe(parameters_.joint_command_in_topic, ROS_QUEUE_SIZE, &JogCalcs::jointJogCB, this); // ROS Server for allowing drift in some dimensions drift_dimensions_server_ = @@ -108,10 +109,10 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, // Publish and Subscribe to internal namespace topics ros::NodeHandle internal_nh("~internal"); - joint_trajectory_pub_ = internal_nh.advertise("joint_trajectory", 1); + joint_trajectory_pub_ = internal_nh.advertise("joint_trajectory", ROS_QUEUE_SIZE); collision_velocity_scale_sub_ = - internal_nh.subscribe("collision_velocity_scale", 1, &JogCalcs::collisionVelocityScaleCB, this); - worst_case_stop_time_pub_ = internal_nh.advertise("worst_case_stop_time", 1); + internal_nh.subscribe("collision_velocity_scale", ROS_QUEUE_SIZE, &JogCalcs::collisionVelocityScaleCB, this); + worst_case_stop_time_pub_ = internal_nh.advertise("worst_case_stop_time", ROS_QUEUE_SIZE); // Wait for initial messages ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg."); @@ -144,7 +145,7 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, void JogCalcs::start() { stop_requested_ = false; - status_pub_ = nh_.advertise(parameters_.status_topic, 1); + status_pub_ = nh_.advertise(parameters_.status_topic, ROS_QUEUE_SIZE); timer_ = nh_.createTimer(period_, &JogCalcs::run, this); } @@ -448,11 +449,6 @@ bool JogCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& join void JogCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& joint_trajectory, int count) const { joint_trajectory.points.resize(count); - if (joint_trajectory.points.empty()) - { - joint_trajectory.points.push_back(trajectory_msgs::JointTrajectoryPoint()); - } - auto point = joint_trajectory.points[0]; // Start from 2 because we already have the first point. End at count+1 so (total #) == count for (int i = 2; i < count + 1; ++i) diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py b/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py index edd540d02c..afaf0f972e 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py @@ -33,7 +33,7 @@ def node(): class CartesianJogCmd(object): def __init__(self): self._pub = rospy.Publisher( - CARTESIAN_JOG_COMMAND_TOPIC, TwistStamped, queue_size=1 + CARTESIAN_JOG_COMMAND_TOPIC, TwistStamped, queue_size=10 ) def send_cmd(self, linear, angular): diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py b/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py index e2b9028169..510966a114 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py @@ -30,7 +30,7 @@ def node(): class JointJogCmd(object): def __init__(self): - self._pub = rospy.Publisher(JOINT_JOG_COMMAND_TOPIC, JointJog, queue_size=1) + self._pub = rospy.Publisher(JOINT_JOG_COMMAND_TOPIC, JointJog, queue_size=10) def send_joint_velocity_cmd(self, joint_pos): jj = JointJog() @@ -43,7 +43,7 @@ def send_joint_velocity_cmd(self, joint_pos): class CartesianJogCmd(object): def __init__(self): self._pub = rospy.Publisher( - CARTESIAN_JOG_COMMAND_TOPIC, TwistStamped, queue_size=1 + CARTESIAN_JOG_COMMAND_TOPIC, TwistStamped, queue_size=10 ) def send_cmd(self, linear, angular): diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py b/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py index e9bb8665ec..3bf49883fb 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py +++ b/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py @@ -28,7 +28,7 @@ def node(): class JointJogCmd(object): def __init__(self): - self._pub = rospy.Publisher(JOINT_JOG_COMMAND_TOPIC, JointJog, queue_size=1) + self._pub = rospy.Publisher(JOINT_JOG_COMMAND_TOPIC, JointJog, queue_size=10) def send_joint_velocity_cmd(self, joint_pos): jj = JointJog() @@ -73,9 +73,13 @@ def test_vel_limit(node): # Should be no velocities greater than the limit assert len(received) > 2 for msg_idx in range(1, len(received)): - velocity = \ - (received[msg_idx].points[0].positions[0] - received[msg_idx - 1].points[0].positions[0]) / JOGGER_COMMAND_PERIOD - assert abs(velocity) <= VELOCITY_LIMIT + try: + velocity = \ + (received[msg_idx].points[0].positions[0] - received[msg_idx - 1].points[0].positions[0]) / JOGGER_COMMAND_PERIOD + assert abs(velocity) <= VELOCITY_LIMIT + except IndexError: + # Sometimes a message doesn't have any points + pass if __name__ == '__main__': node = node() From 168ec3f101d123967d08ea3fc0b12eb30266546b Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 1 Jun 2020 00:15:17 -0600 Subject: [PATCH 332/612] publish from jog calcs timer, removing redundant timer and internal messaging to main timer --- .../include/moveit_jog_arm/jog_arm.h | 34 +--- .../include/moveit_jog_arm/jog_calcs.h | 2 + .../moveit_jog_arm/src/jog_arm.cpp | 70 +-------- .../moveit_jog_arm/src/jog_calcs.cpp | 145 ++++++++++-------- 4 files changed, 88 insertions(+), 163 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h index 161fcdeae0..ebf70611ff 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h @@ -37,27 +37,15 @@ *******************************************************************************/ #include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "collision_check.h" -#include "jog_arm_parameters.h" -#include "jog_calcs.h" -#include "low_pass_filter.h" +#include +#include +#include namespace moveit_jog_arm { /** - * Class JogArm - Base class for C++ interface and ROS node. - * Handles ROS subs & pubs and creates the worker timers. + * Class JogArm - Jacobian based robot control with collision avoidance. */ class JogArm { @@ -89,8 +77,6 @@ class JogArm private: bool readParameters(); - void run(const ros::TimerEvent& timer_event); - void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr& msg); ros::NodeHandle nh_; @@ -105,18 +91,6 @@ class JogArm // Collision checks std::unique_ptr collision_checker_; - - // ROS - ros::Timer timer_; - ros::Duration period_; - ros::Publisher outgoing_cmd_pub_; - ros::Subscriber joint_trajectory_sub_; - - bool paused_ = false; - - // latest_state_mutex_ is used to protect the state below it - mutable std::mutex latest_state_mutex_; - trajectory_msgs::JointTrajectoryConstPtr latest_joint_trajectory_; }; // JogArmPtr using alias diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 16883a8a42..fd43536082 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -57,6 +57,7 @@ #include "jog_arm_parameters.h" #include "low_pass_filter.h" #include "status_codes.h" +#include namespace moveit_jog_arm { @@ -237,6 +238,7 @@ class JogCalcs ros::Publisher status_pub_; ros::Publisher joint_trajectory_pub_; ros::Publisher worst_case_stop_time_pub_; + ros::Publisher outgoing_cmd_pub_; ros::ServiceServer drift_dimensions_server_; ros::ServiceServer control_dimensions_server_; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index d54823b420..cb2f8a72ae 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -37,6 +37,8 @@ * Author : Brian O'Neil, Andy Zelenak, Blake Anderson */ +#include + #include "moveit_jog_arm/jog_arm.h" static const std::string LOGNAME = "jog_arm"; @@ -50,24 +52,6 @@ JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneM if (!readParameters()) exit(EXIT_FAILURE); - // loop period - period_ = ros::Duration(parameters_.publish_period); - - // Publish freshly-calculated joints to the robot. - // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, ROS_QUEUE_SIZE); - else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") - outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, ROS_QUEUE_SIZE); - - // Subscribe to internal namespace - ros::NodeHandle internal_nh("~internal"); - joint_trajectory_sub_ = internal_nh.subscribe("joint_trajectory", ROS_QUEUE_SIZE, &JogArm::jointTrajectoryCB, this); - - // Wait for incoming topics to appear - ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); - ros::topic::waitForMessage(parameters_.joint_topic); - jog_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); collision_checker_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); @@ -287,45 +271,6 @@ bool JogArm::readParameters() return true; } -void JogArm::run(const ros::TimerEvent& timer_event) -{ - // Log warning when the last loop duration was longer than the period - ROS_WARN_STREAM_COND_NAMED(timer_event.profile.last_duration.toSec() > period_.toSec(), LOGNAME, - "last_duration: " << timer_event.profile.last_duration.toSec() << " (" << period_.toSec() - << ")"); - - // Get the latest joint trajectory - trajectory_msgs::JointTrajectory joint_trajectory; - { - const std::lock_guard lock(latest_state_mutex_); - if (latest_joint_trajectory_) - { - joint_trajectory = *latest_joint_trajectory_; - } - } - - // Publish the most recent trajectory, unless the jogging calculation timer tells not to - if (jog_calcs_->getOkToPublish() && !paused_) - { - // Put the outgoing msg in the right format - // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - { - joint_trajectory.header.stamp = ros::Time::now(); - outgoing_cmd_pub_.publish(joint_trajectory); - } - else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") - { - std_msgs::Float64MultiArray joints; - if (parameters_.publish_joint_positions) - joints.data = joint_trajectory.points[0].positions; - else if (parameters_.publish_joint_velocities) - joints.data = joint_trajectory.points[0].velocities; - outgoing_cmd_pub_.publish(joints); - } - } -} - void JogArm::start() { setPaused(false); @@ -336,14 +281,10 @@ void JogArm::start() // Check collisions in this timer if (parameters_.check_collisions) collision_checker_->start(); - - // Start the jog server timer - timer_ = nh_.createTimer(period_, &JogArm::run, this); } void JogArm::stop() { - timer_.stop(); jog_calcs_->stop(); collision_checker_->stop(); } @@ -355,7 +296,6 @@ JogArm::~JogArm() void JogArm::setPaused(bool paused) { - paused_ = paused; jog_calcs_->setPaused(paused); collision_checker_->setPaused(paused); } @@ -365,12 +305,6 @@ bool JogArm::getCommandFrameTransform(Eigen::Isometry3d& transform) return jog_calcs_->getCommandFrameTransform(transform); } -void JogArm::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr& msg) -{ - const std::lock_guard lock(latest_state_mutex_); - latest_joint_trajectory_ = msg; -} - const JogArmParameters& JogArm::getParameters() const { return parameters_; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index aae69c4f24..a38395ad14 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include @@ -109,11 +110,17 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, // Publish and Subscribe to internal namespace topics ros::NodeHandle internal_nh("~internal"); - joint_trajectory_pub_ = internal_nh.advertise("joint_trajectory", ROS_QUEUE_SIZE); collision_velocity_scale_sub_ = internal_nh.subscribe("collision_velocity_scale", ROS_QUEUE_SIZE, &JogCalcs::collisionVelocityScaleCB, this); worst_case_stop_time_pub_ = internal_nh.advertise("worst_case_stop_time", ROS_QUEUE_SIZE); + // Publish freshly-calculated joints to the robot. + // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). + if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") + outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, ROS_QUEUE_SIZE); + else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") + outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, ROS_QUEUE_SIZE); + // Wait for initial messages ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg."); ros::topic::waitForMessage(parameters_.joint_topic); @@ -134,12 +141,6 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, { position_filters_.emplace_back(parameters_.low_pass_filter_coeff); } - - // initialization is finished - wait_for_jog_commands_ = true; - have_nonzero_twist_stamped_ = false; - have_nonzero_joint_jog_ = false; - have_nonzero_command_ = false; } void JogCalcs::start() @@ -215,74 +216,68 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // Check if there are any new commands with valid timestamp wait_for_jog_commands_ = twist_stamped_.header.stamp == ros::Time(0.) && joint_jog_.header.stamp == ros::Time(0.); + + // Early exit + return; } + // If not waiting for initial command, and not paused. // Do jogging calculations only if the robot should move, for efficiency - else - { - // Create new outgoing joint trajectory command message - auto joint_trajectory = moveit::util::make_shared_from_pool(); + // Create new outgoing joint trajectory command message + auto joint_trajectory = moveit::util::make_shared_from_pool(); - // If the input command isn't stale, run calculations - if (!command_is_stale_) + // If the input command isn't stale, run calculations + if (!command_is_stale_) + { + // Prioritize cartesian jogging above joint jogging + if (have_nonzero_twist_stamped_) { - // Prioritize cartesian jogging above joint jogging - if (have_nonzero_twist_stamped_) - { - if (!cartesianJogCalcs(twist_stamped_, *joint_trajectory)) - return; - } - else if (have_nonzero_joint_jog_) - { - if (!jointJogCalcs(joint_jog_, *joint_trajectory)) - return; - } + if (!cartesianJogCalcs(twist_stamped_, *joint_trajectory)) + return; } - - // If we should halt - if (!have_nonzero_command_) + else if (have_nonzero_joint_jog_) { - // Keep the joint position filters up-to-date with current joints - for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(original_joint_state_.position[i]); - - suddenHalt(*joint_trajectory); - have_nonzero_twist_stamped_ = false; - have_nonzero_joint_jog_ = false; + if (!jointJogCalcs(joint_jog_, *joint_trajectory)) + return; } + } - // Send the newest target joints - // If everything normal, share the new traj to be published - if (have_nonzero_command_) - { - joint_trajectory_pub_.publish(joint_trajectory); - ok_to_publish_ = true; - } - // Skip the jogging publication if all inputs have been zero for several cycles in a row. - // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. - else if ((parameters_.num_outgoing_halt_msgs_to_publish != 0) && - (zero_velocity_count_ > parameters_.num_outgoing_halt_msgs_to_publish)) - { - ok_to_publish_ = false; - ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); - } - // The command is invalid but we are publishing num_outgoing_halt_msgs_to_publish - else - { - joint_trajectory_pub_.publish(joint_trajectory); - ok_to_publish_ = true; - } + // If we should halt + if (!have_nonzero_command_) + { + // Keep the joint position filters up-to-date with current joints + for (std::size_t i = 0; i < num_joints_; ++i) + position_filters_[i].reset(original_joint_state_.position[i]); - // Store last zero-velocity message flag to prevent superfluous warnings. - // Cartesian and joint commands must both be zero. - if (!have_nonzero_twist_stamped_ && !have_nonzero_joint_jog_) - { - // Avoid overflow - if (zero_velocity_count_ < std::numeric_limits::max()) - ++zero_velocity_count_; - } - else - zero_velocity_count_ = 0; + suddenHalt(*joint_trajectory); + have_nonzero_twist_stamped_ = false; + have_nonzero_joint_jog_ = false; + } + + // Skip the jogging publication if all inputs have been zero for several cycles in a row. + // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. + if (!have_nonzero_command_ && (parameters_.num_outgoing_halt_msgs_to_publish != 0) && + (zero_velocity_count_ > parameters_.num_outgoing_halt_msgs_to_publish)) + { + ok_to_publish_ = false; + ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); + } + else + { + ok_to_publish_ = true; + } + + // Store last zero-velocity message flag to prevent superfluous warnings. + // Cartesian and joint commands must both be zero. + if (!have_nonzero_twist_stamped_ && !have_nonzero_joint_jog_) + { + // Avoid overflow + if (zero_velocity_count_ < std::numeric_limits::max()) + ++zero_velocity_count_; + } + else + { + zero_velocity_count_ = 0; } if (command_is_stale_) @@ -290,6 +285,26 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " "Try a larger 'incoming_command_timeout' parameter?"); } + + if (ok_to_publish_) + { + // Put the outgoing msg in the right format + // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). + if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") + { + joint_trajectory->header.stamp = ros::Time::now(); + outgoing_cmd_pub_.publish(joint_trajectory); + } + else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") + { + auto joints = moveit::util::make_shared_from_pool(); + if (parameters_.publish_joint_positions) + joints->data = joint_trajectory->points[0].positions; + else if (parameters_.publish_joint_velocities) + joints->data = joint_trajectory->points[0].velocities; + outgoing_cmd_pub_.publish(joints); + } + } } // Perform the jogging calculations bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory) From 7416a543eb22a759e975c7324aa1a978816c8d28 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 1 Jun 2020 00:30:49 -0600 Subject: [PATCH 333/612] fixup of comments --- .../moveit_jog_arm/include/moveit_jog_arm/collision_check.h | 5 ++--- .../moveit_jog_arm/include/moveit_jog_arm/jog_arm.h | 4 ++-- .../moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h | 5 ++--- .../src/cpp_interface_example/cpp_interface_example.cpp | 6 +++--- 4 files changed, 9 insertions(+), 11 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h index e3786f18c2..0c98cd9d21 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h @@ -80,11 +80,10 @@ class CollisionCheck void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); void worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg); - // ROS node handle ros::NodeHandle nh_; - // Parameters - const moveit_jog_arm::JogArmParameters parameters_; + // Parameters from yaml + const JogArmParameters& parameters_; // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h index ebf70611ff..d0f77b5324 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h @@ -54,10 +54,10 @@ class JogArm ~JogArm(); - /** \brief start jog arm timers */ + /** \brief start jog arm */ void start(); - /** \brief stop jog arm timers */ + /** \brief stop jog arm */ void stop(); /** \brief Pause or unpause processing jog commands while keeping the timers alive */ diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index fd43536082..62655215c5 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -67,7 +67,7 @@ class JogCalcs JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - /** \brief Start and stop the timer */ + /** \brief Start and stop the timer where we do work and publish outputs */ void start(); void stop(); @@ -190,10 +190,9 @@ class JogCalcs bool changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, moveit_msgs::ChangeControlDimensions::Response& res); - // ROS node handle ros::NodeHandle nh_; - // Ros parameters from JogArm + // Parameters from yaml const JogArmParameters& parameters_; // Pointer to the collision environment diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index a64428bb90..b1e5582c3d 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -56,10 +56,10 @@ class StatusMonitor private: void statusCB(const std_msgs::Int8ConstPtr& msg) { - moveit_jog_arm::StatusCode latest_stats = static_cast(msg->data); - if (latest_stats != status_) + moveit_jog_arm::StatusCode latest_status = static_cast(msg->data); + if (latest_status != status_) { - status_ = latest_stats; + status_ = latest_status; const auto& status_str = moveit_jog_arm::JOG_ARM_STATUS_CODE_MAP.at(status_); ROS_INFO_STREAM_NAMED(LOGNAME, "Jogger status: " << status_str); } From 940f4bac4152b3fb9a6e817c47d0dbbcccd480ba Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 1 Jun 2020 00:39:42 -0600 Subject: [PATCH 334/612] move status publisher into constructor of JogCalcs --- moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index a38395ad14..a9564dc080 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -121,6 +121,9 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, ROS_QUEUE_SIZE); + // Publish status + status_pub_ = nh_.advertise(parameters_.status_topic, ROS_QUEUE_SIZE); + // Wait for initial messages ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg."); ros::topic::waitForMessage(parameters_.joint_topic); @@ -146,8 +149,6 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, void JogCalcs::start() { stop_requested_ = false; - status_pub_ = nh_.advertise(parameters_.status_topic, ROS_QUEUE_SIZE); - timer_ = nh_.createTimer(period_, &JogCalcs::run, this); } From 88805637cc697448e0e88e33df65b8c008df4386 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 1 Jun 2020 00:59:13 -0600 Subject: [PATCH 335/612] remove getOkToPublish from JogCalcs --- .../moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h | 3 --- moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp | 5 ----- 2 files changed, 8 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 62655215c5..5b3aafe26a 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -83,9 +83,6 @@ class JogCalcs /** \brief Pause or unpause processing jog commands while keeping the timers alive */ void setPaused(bool paused); - /** \brief Accessor for ok_to_publish_ */ - bool getOkToPublish() const; - private: /** \brief Timer method */ void run(const ros::TimerEvent& timer_event); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index a9564dc080..5317195d50 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -998,9 +998,4 @@ void JogCalcs::setPaused(bool paused) paused_ = paused; } -bool JogCalcs::getOkToPublish() const -{ - return ok_to_publish_; -} - } // namespace moveit_jog_arm From b160960aeee8e70b841cd3d8ffc34b01dc8994cd Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 1 Jun 2020 09:27:53 -0600 Subject: [PATCH 336/612] Update moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp Co-authored-by: Robert Haschke --- .../src/cpp_interface_example/cpp_interface_example.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index b1e5582c3d..7a0389169c 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -138,7 +138,7 @@ int main(int argc, char** argv) { // Make a joint command // Messages are sent to jogger as boost::shared_ptr to enable zero-copy message_passing. - // Because this message is not coppied we should not modify it after we send it. + // Because this message is not copied we should not modify it after we send it. auto msg = moveit::util::make_shared_from_pool(); msg->header.stamp = ros::Time::now(); msg->joint_names.push_back("elbow_joint"); From ef09fbb68c63344f87057a5c5ac7160e78b1e08b Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 1 Jun 2020 09:28:11 -0600 Subject: [PATCH 337/612] Update moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp Co-authored-by: Robert Haschke --- .../src/cpp_interface_example/cpp_interface_example.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp index 7a0389169c..77dee6b253 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp @@ -115,7 +115,7 @@ int main(int argc, char** argv) { // Make a Cartesian velocity message // Messages are sent to jogger as boost::shared_ptr to enable zero-copy message_passing. - // Because this message is not coppied we should not modify it after we send it. + // Because this message is not copied we should not modify it after we send it. auto msg = moveit::util::make_shared_from_pool(); msg->header.stamp = ros::Time::now(); msg->header.frame_id = "base_link"; From 2eae1b34edbc935f893a417b82412a8216faa2d6 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 1 Jun 2020 10:38:38 -0600 Subject: [PATCH 338/612] fix comment --- .../moveit_jog_arm/include/moveit_jog_arm/jog_arm.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h index d0f77b5324..bd3bf8299b 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h @@ -1,5 +1,5 @@ /******************************************************************************* - * Title : jog_interface_base.cpp + * Title : jog_arm.cpp * Project : moveit_jog_arm * Created : 3/9/2017 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson From f9a1bf0de9c77845431673d28dd75f476ebfb40c Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 1 Jun 2020 11:25:55 -0600 Subject: [PATCH 339/612] attempt to fix kinetic travis issue --- .../include/moveit_jog_arm/jog_calcs.h | 2 +- .../moveit_jog_arm/src/jog_calcs.cpp | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index 5b3aafe26a..f0c56a7983 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -265,7 +265,7 @@ class JogCalcs // latest_state_mutex_ is used to protect the state below it mutable std::mutex latest_state_mutex_; - sensor_msgs::JointState incoming_joint_state_; + sensor_msgs::JointStateConstPtr incoming_joint_state_; Eigen::Isometry3d tf_moveit_to_cmd_frame_; geometry_msgs::TwistStampedConstPtr latest_twist_stamped_; control_msgs::JointJogConstPtr latest_joint_jog_; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 5317195d50..bb15e19539 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -183,7 +183,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // Update from latest state { const std::lock_guard lock(latest_state_mutex_); - kinematic_state_->setVariableValues(incoming_joint_state_); + kinematic_state_->setVariableValues(*incoming_joint_state_); if (latest_twist_stamped_) twist_stamped_ = *latest_twist_stamped_; if (latest_joint_jog_) @@ -750,24 +750,24 @@ bool JogCalcs::updateJoints() const std::lock_guard lock(latest_state_mutex_); // Check that the msg contains enough joints - if (incoming_joint_state_.name.size() < num_joints_) + if (incoming_joint_state_->name.size() < num_joints_) return false; // Store joints in a member variable - for (std::size_t m = 0; m < incoming_joint_state_.name.size(); ++m) + for (std::size_t m = 0; m < incoming_joint_state_->name.size(); ++m) { std::size_t c; try { - c = joint_state_name_map_.at(incoming_joint_state_.name[m]); + c = joint_state_name_map_.at(incoming_joint_state_->name[m]); } catch (const std::out_of_range& e) { - ROS_DEBUG_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_.name[m]); + ROS_DEBUG_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_->name[m]); continue; } - internal_joint_state_.position[c] = incoming_joint_state_.position[m]; + internal_joint_state_.position[c] = incoming_joint_state_->position[m]; } // Cache the original joints in case they need to be reset @@ -873,7 +873,7 @@ Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& comman catch (const std::out_of_range& e) { const std::lock_guard lock(latest_state_mutex_); - ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_.name[m]); + ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_->name[m]); continue; } // Apply user-defined scaling if inputs are unitless [-1:1] @@ -928,7 +928,7 @@ void JogCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta void JogCalcs::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) { const std::lock_guard lock(latest_state_mutex_); - incoming_joint_state_ = *msg; + incoming_joint_state_ = msg; } bool JogCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) From 59aecfea3d45c40df0eae8356cfe2756df55f7a9 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 3 Jun 2020 14:28:18 -0600 Subject: [PATCH 340/612] throttle warning about loop duration to 30 seconds --- .../moveit_jog_arm/src/collision_check.cpp | 8 +++++--- .../moveit_jog_arm/src/jog_calcs.cpp | 14 ++++++++------ 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp index f3def372a8..f4ea2c9bfa 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp @@ -105,9 +105,11 @@ void CollisionCheck::stop() void CollisionCheck::run(const ros::TimerEvent& timer_event) { // Log warning when the last loop duration was longer than the period - ROS_WARN_STREAM_COND_NAMED(timer_event.profile.last_duration.toSec() > period_.toSec(), LOGNAME, - "last_duration: " << timer_event.profile.last_duration.toSec() << " (" << period_.toSec() - << ")"); + if (timer_event.profile.last_duration.toSec() > period_.toSec()) + { + ROS_WARN_STREAM_THROTTLE_NAMED(30, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " (" + << period_.toSec() << ")"); + } if (paused_) { diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index bb15e19539..26ee79a1c0 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -161,9 +161,11 @@ void JogCalcs::stop() void JogCalcs::run(const ros::TimerEvent& timer_event) { // Log warning when the last loop duration was longer than the period - ROS_WARN_STREAM_COND_NAMED(timer_event.profile.last_duration.toSec() > period_.toSec(), LOGNAME, - "last_duration: " << timer_event.profile.last_duration.toSec() << " (" << period_.toSec() - << ")"); + if (timer_event.profile.last_duration.toSec() > period_.toSec()) + { + ROS_WARN_STREAM_THROTTLE_NAMED(30, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " (" + << period_.toSec() << ")"); + } // Publish status each loop iteration auto status_msg = moveit::util::make_shared_from_pool(); @@ -779,9 +781,9 @@ bool JogCalcs::updateJoints() double accel_limit = 0; double joint_velocity = 0; double worst_case_stop_time = 0; - for (size_t jt_state_idx = 0; jt_state_idx < incoming_joint_state_.velocity.size(); ++jt_state_idx) + for (size_t jt_state_idx = 0; jt_state_idx < incoming_joint_state_->velocity.size(); ++jt_state_idx) { - joint_name = incoming_joint_state_.name[jt_state_idx]; + joint_name = incoming_joint_state_->name[jt_state_idx]; // Get acceleration limit for this joint for (auto joint_model : joint_model_group_->getActiveJointModels()) @@ -807,7 +809,7 @@ bool JogCalcs::updateJoints() } // Get the current joint velocity - joint_velocity = incoming_joint_state_.velocity[jt_state_idx]; + joint_velocity = incoming_joint_state_->velocity[jt_state_idx]; // Calculate worst case stop time worst_case_stop_time = std::max(worst_case_stop_time, fabs(joint_velocity / accel_limit)); From fb0a38d7e416f9752002e1525b87f1ab19037fc7 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 3 Jun 2020 23:13:58 -0600 Subject: [PATCH 341/612] throttle warning logs --- .../moveit_jog_arm/jog_arm_parameters.h | 3 ++ .../moveit_jog_arm/src/collision_check.cpp | 2 +- .../moveit_jog_arm/src/jog_calcs.cpp | 49 +++++++++++-------- 3 files changed, 33 insertions(+), 21 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h index a9e2cfa245..d9642ac91e 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h @@ -43,6 +43,9 @@ namespace moveit_jog_arm // Size of queues used in ros pub/sub/service constexpr size_t ROS_QUEUE_SIZE = 2; +// Seconds to throttle logs inside loops +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30; + // ROS params to be read. See the yaml file in /config for a description of each. struct JogArmParameters { diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp index f4ea2c9bfa..88f2ec2621 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp @@ -63,7 +63,7 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArm collision_request_.distance = true; // enable distance-based collision checking if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE) - ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Collision check rate is low, increase it in yaml file if CPU allows"); + ROS_WARN_STREAM_THROTTLE_NAMED(30, LOGNAME, "Collision check rate is low, increase it in yaml file if CPU allows"); // subscribe to joints joint_state_sub_ = nh_.subscribe(parameters.joint_topic, ROS_QUEUE_SIZE, &CollisionCheck::jointStateCB, this); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 26ee79a1c0..98a801dfb4 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -80,7 +80,7 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr = planning_scene_monitor_->getRobotModelLoader(); while (ros::ok() && !model_loader_ptr) { - ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); + ROS_WARN_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); default_sleep_rate_.sleep(); } const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); @@ -163,8 +163,9 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // Log warning when the last loop duration was longer than the period if (timer_event.profile.last_duration.toSec() > period_.toSec()) { - ROS_WARN_STREAM_THROTTLE_NAMED(30, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " (" - << period_.toSec() << ")"); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "last_duration: " << timer_event.profile.last_duration.toSec() << " (" + << period_.toSec() << ")"); } // Publish status each loop iteration @@ -263,7 +264,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) (zero_velocity_count_ > parameters_.num_outgoing_halt_msgs_to_publish)) { ok_to_publish_ = false; - ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); + ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "All-zero command. Doing nothing."); } else { @@ -285,8 +286,9 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) if (command_is_stale_) { - ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " - "Try a larger 'incoming_command_timeout' parameter?"); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "Stale command. " + "Try a larger 'incoming_command_timeout' parameter?"); } if (ok_to_publish_) @@ -316,7 +318,8 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_ms if (std::isnan(cmd.twist.linear.x) || std::isnan(cmd.twist.linear.y) || std::isnan(cmd.twist.linear.z) || std::isnan(cmd.twist.angular.x) || std::isnan(cmd.twist.angular.y) || std::isnan(cmd.twist.angular.z)) { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "nan in incoming command. Skipping this datapoint."); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "nan in incoming command. Skipping this datapoint."); return false; } @@ -326,7 +329,8 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_ms if ((fabs(cmd.twist.linear.x) > 1) || (fabs(cmd.twist.linear.y) > 1) || (fabs(cmd.twist.linear.z) > 1) || (fabs(cmd.twist.angular.x) > 1) || (fabs(cmd.twist.angular.y) > 1) || (fabs(cmd.twist.angular.z) > 1)) { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "Component of incoming command is >1. Skipping this datapoint."); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "Component of incoming command is >1. Skipping this datapoint."); return false; } } @@ -400,7 +404,7 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_ms applyVelocityScaling(delta_theta_, velocityScalingFactorForSingularity(delta_x, svd, jacobian, pseudo_inverse)); if (status_ == StatusCode::HALT_FOR_COLLISION) { - ROS_ERROR_STREAM_THROTTLE_NAMED(5, LOGNAME, "Halting for collision!"); + ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Halting for collision!"); suddenHalt(delta_theta_); } @@ -416,7 +420,8 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, trajectory_msgs: { if (std::isnan(velocity)) { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "nan in incoming command. Skipping this datapoint."); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "nan in incoming command. Skipping this datapoint."); return false; } } @@ -524,7 +529,7 @@ void JogCalcs::applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singular if (collision_scale > 0 && collision_scale < 1) { status_ = StatusCode::DECELERATE_FOR_COLLISION; - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); } else if (collision_scale == 0) { @@ -587,7 +592,7 @@ double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& comm (ini_condition - parameters_.lower_singularity_threshold) / (parameters_.hard_stop_singularity_threshold - parameters_.lower_singularity_threshold); status_ = StatusCode::DECELERATE_FOR_SINGULARITY; - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); } // Very close to singularity, so halt. @@ -595,7 +600,7 @@ double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& comm { velocity_scale = 0; status_ = StatusCode::HALT_FOR_SINGULARITY; - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); } } @@ -704,7 +709,8 @@ bool JogCalcs::enforceSRDFPositionLimits() (kinematic_state_->getJointVelocities(joint)[0] > 0 && (joint_angle > (limits[0].max_position - parameters_.joint_limit_margin)))) { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, ros::this_node::getName() << " " << joint->getName() + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, ros::this_node::getName() + << " " << joint->getName() << " close to a " " position limit. Halting."); halting = true; @@ -765,7 +771,8 @@ bool JogCalcs::updateJoints() } catch (const std::out_of_range& e) { - ROS_DEBUG_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_->name[m]); + ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Ignoring joint " + << incoming_joint_state_->name[m]); continue; } @@ -851,7 +858,7 @@ Eigen::VectorXd JogCalcs::scaleCartesianCommand(const geometry_msgs::TwistStampe result[5] = command.twist.angular.z * parameters_.publish_period; } else - ROS_ERROR_STREAM_NAMED(LOGNAME, "Unexpected command_in_type"); + ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Unexpected command_in_type"); return result; } @@ -875,7 +882,8 @@ Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& comman catch (const std::out_of_range& e) { const std::lock_guard lock(latest_state_mutex_); - ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_->name[m]); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Ignoring joint " + << incoming_joint_state_->name[m]); continue; } // Apply user-defined scaling if inputs are unitless [-1:1] @@ -885,7 +893,7 @@ Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& comman else if (parameters_.command_in_type == "speed_units") result[c] = command.velocities[m] * parameters_.publish_period; else - ROS_ERROR_STREAM_NAMED(LOGNAME, "Unexpected command_in_type, check yaml file."); + ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Unexpected command_in_type, check yaml file."); } return result; @@ -902,8 +910,9 @@ bool JogCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen:: } catch (const std::out_of_range& e) { - ROS_ERROR_STREAM_NAMED(LOGNAME, ros::this_node::getName() << " Lengths of output and " - "increments do not match."); + ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, ros::this_node::getName() + << " Lengths of output and " + "increments do not match."); return false; } } From 40fe760043ac1e7db24104a34ee9fe314c99856d Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 4 Jun 2020 09:07:38 -0600 Subject: [PATCH 342/612] remove unused --- .../moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index f0c56a7983..a85253f39a 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -232,7 +232,6 @@ class JogCalcs ros::Subscriber joint_jog_sub_; ros::Subscriber collision_velocity_scale_sub_; ros::Publisher status_pub_; - ros::Publisher joint_trajectory_pub_; ros::Publisher worst_case_stop_time_pub_; ros::Publisher outgoing_cmd_pub_; ros::ServiceServer drift_dimensions_server_; From 71115b80453d7921f3641f408211bb39ba980754 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 4 Jun 2020 11:42:39 -0600 Subject: [PATCH 343/612] fix bad mutex locking --- .../moveit_jog_arm/src/collision_check.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp index 88f2ec2621..35637cc6e9 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp @@ -63,7 +63,8 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArm collision_request_.distance = true; // enable distance-based collision checking if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE) - ROS_WARN_STREAM_THROTTLE_NAMED(30, LOGNAME, "Collision check rate is low, increase it in yaml file if CPU allows"); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "Collision check rate is low, increase it in yaml file if CPU allows"); // subscribe to joints joint_state_sub_ = nh_.subscribe(parameters.joint_topic, ROS_QUEUE_SIZE, &CollisionCheck::jointStateCB, this); @@ -107,8 +108,9 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event) // Log warning when the last loop duration was longer than the period if (timer_event.profile.last_duration.toSec() > period_.toSec()) { - ROS_WARN_STREAM_THROTTLE_NAMED(30, LOGNAME, "last_duration: " << timer_event.profile.last_duration.toSec() << " (" - << period_.toSec() << ")"); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "last_duration: " << timer_event.profile.last_duration.toSec() << " (" + << period_.toSec() << ")"); } if (paused_) @@ -118,7 +120,7 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event) { // Copy the latest joint state - const std::lock_guard lock(CollisionCheck); + const std::lock_guard lock(joint_state_mutex_); for (std::size_t i = 0; i < latest_joint_state_->position.size(); ++i) current_state_->setJointPositions(latest_joint_state_->name[i], &latest_joint_state_->position[i]); } From 8bb6cf610c9cd52688f713f1eed319efc1f4229a Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 11 Jun 2020 14:08:59 -0600 Subject: [PATCH 344/612] single joint_state subscriber in shared object --- .../moveit_jog_arm/CMakeLists.txt | 6 +- .../include/moveit_jog_arm/collision_check.h | 16 ++--- .../include/moveit_jog_arm/jog_calcs.h | 14 ++-- .../moveit_jog_arm/joint_state_subscriber.h | 69 ++++++++++++++++++ .../moveit_jog_arm/src/collision_check.cpp | 27 ++----- .../moveit_jog_arm/src/jog_arm.cpp | 10 ++- .../moveit_jog_arm/src/jog_calcs.cpp | 45 +++++------- .../src/joint_state_subscriber.cpp | 70 +++++++++++++++++++ 8 files changed, 188 insertions(+), 69 deletions(-) create mode 100644 moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/joint_state_subscriber.h create mode 100644 moveit_experimental/moveit_jog_arm/src/joint_state_subscriber.cpp diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index 1e2701203b..da429665cc 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -58,6 +58,7 @@ add_library(${LIBRARY_NAME} SHARED src/collision_check.cpp src/jog_calcs.cpp src/jog_arm.cpp + src/joint_state_subscriber.cpp src/low_pass_filter.cpp ) add_dependencies(${LIBRARY_NAME} ${catkin_EXPORTED_TARGETS}) @@ -84,14 +85,11 @@ target_link_libraries(cpp_interface_example ############################ add_executable(jog_server - src/collision_check.cpp - src/jog_calcs.cpp - src/jog_arm.cpp src/jog_server.cpp - src/low_pass_filter.cpp ) add_dependencies(jog_server ${catkin_EXPORTED_TARGETS}) target_link_libraries(jog_server + ${LIBRARY_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${Boost_LIBRARIES} diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h index 0c98cd9d21..8055e7f762 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h @@ -45,8 +45,9 @@ #include #include -#include "jog_arm_parameters.h" -#include "low_pass_filter.h" +#include +#include +#include namespace moveit_jog_arm { @@ -65,7 +66,8 @@ class CollisionCheck * already started when passed into this class */ CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::shared_ptr& joint_state_subscriber); /** \brief start and stop the Timer */ void start(); @@ -77,7 +79,6 @@ class CollisionCheck private: void run(const ros::TimerEvent& timer_event); planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; - void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); void worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg); ros::NodeHandle nh_; @@ -88,6 +89,9 @@ class CollisionCheck // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + // Subscriber to joint states + const std::shared_ptr joint_state_subscriber_; + // Robot state and collision matrix from planning scene std::unique_ptr current_state_; collision_detection::AllowedCollisionMatrix acm_; @@ -123,9 +127,5 @@ class CollisionCheck ros::Subscriber joint_state_sub_; ros::Publisher collision_velocity_scale_pub_; ros::Subscriber worst_case_stop_time_sub_; - - // Latest joint state, updated by ROS callback - mutable std::mutex joint_state_mutex_; - sensor_msgs::JointStateConstPtr latest_joint_state_; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index a85253f39a..de9ef06b19 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -54,10 +54,11 @@ #include // moveit_jog_arm -#include "jog_arm_parameters.h" -#include "low_pass_filter.h" -#include "status_codes.h" +#include #include +#include +#include +#include namespace moveit_jog_arm { @@ -65,7 +66,8 @@ class JogCalcs { public: JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::shared_ptr& joint_state_subscriber); /** \brief Start and stop the timer where we do work and publish outputs */ void start(); @@ -195,6 +197,9 @@ class JogCalcs // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + // Subscriber to the latest joint states + const std::shared_ptr joint_state_subscriber_; + // Track the number of cycles during which motion has not occurred. // Will avoid re-publishing zero velocities endlessly. int zero_velocity_count_ = 0; @@ -264,7 +269,6 @@ class JogCalcs // latest_state_mutex_ is used to protect the state below it mutable std::mutex latest_state_mutex_; - sensor_msgs::JointStateConstPtr incoming_joint_state_; Eigen::Isometry3d tf_moveit_to_cmd_frame_; geometry_msgs::TwistStampedConstPtr latest_twist_stamped_; control_msgs::JointJogConstPtr latest_joint_jog_; diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/joint_state_subscriber.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/joint_state_subscriber.h new file mode 100644 index 0000000000..e7b74f2911 --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/joint_state_subscriber.h @@ -0,0 +1,69 @@ +/******************************************************************************* + * Title : joint_state_subscriber.h + * Project : moveit_jog_arm + * Created : 06/11/2020 + * Author : Tyler Weaver + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 the copyright holder 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 HOLDER 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 "jog_arm_parameters.h" + +namespace moveit_jog_arm +{ +class JointStateSubscriber +{ +public: + /** \brief Constructor + */ + JointStateSubscriber(ros::NodeHandle& nh, const std::string& joint_state_topic_name); + + /** \brief Get the latest joint state message */ + sensor_msgs::JointStateConstPtr getLatest() const; + +private: + void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); + + ros::Subscriber joint_state_sub_; + + // Latest joint state, updated by ROS callback + mutable std::mutex joint_state_mutex_; + sensor_msgs::JointStateConstPtr latest_joint_state_; +}; +} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp index 35637cc6e9..1d8619a62b 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp @@ -50,10 +50,12 @@ namespace moveit_jog_arm { // Constructor for the class that handles collision checking CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::shared_ptr& joint_state_subscriber) : nh_(nh) , parameters_(parameters) , planning_scene_monitor_(planning_scene_monitor) + , joint_state_subscriber_(joint_state_subscriber) , self_velocity_scale_coefficient_(-log(0.001) / parameters.self_collision_proximity_threshold) , scene_velocity_scale_coefficient_(-log(0.001) / parameters.scene_collision_proximity_threshold) , period_(1. / parameters_.collision_check_rate) @@ -66,9 +68,6 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArm ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Collision check rate is low, increase it in yaml file if CPU allows"); - // subscribe to joints - joint_state_sub_ = nh_.subscribe(parameters.joint_topic, ROS_QUEUE_SIZE, &CollisionCheck::jointStateCB, this); - collision_check_type_ = (parameters_.collision_check_type == "threshold_distance" ? K_THRESHOLD_DISTANCE : K_STOP_DISTANCE); safety_factor_ = parameters_.collision_distance_safety_factor; @@ -80,10 +79,6 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArm worst_case_stop_time_sub_ = internal_nh.subscribe("worst_case_stop_time", ROS_QUEUE_SIZE, &CollisionCheck::worstCaseStopTimeCB, this); - // Wait for incoming topics to appear - ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); - ros::topic::waitForMessage(parameters_.joint_topic); - current_state_ = std::make_unique(getLockedPlanningSceneRO()->getCurrentState()); acm_ = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); } @@ -118,12 +113,10 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event) return; } - { - // Copy the latest joint state - const std::lock_guard lock(joint_state_mutex_); - for (std::size_t i = 0; i < latest_joint_state_->position.size(); ++i) - current_state_->setJointPositions(latest_joint_state_->name[i], &latest_joint_state_->position[i]); - } + // Copy the latest joint state + auto latest_joint_state = joint_state_subscriber_->getLatest(); + for (std::size_t i = 0; i < latest_joint_state->position.size(); ++i) + current_state_->setJointPositions(latest_joint_state->name[i], &latest_joint_state->position[i]); current_state_->updateCollisionBodyTransforms(); collision_detected_ = false; @@ -210,12 +203,6 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event) } } -void CollisionCheck::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) -{ - const std::lock_guard lock(joint_state_mutex_); - latest_joint_state_ = msg; -} - void CollisionCheck::worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg) { worst_case_stop_time_ = msg->data; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index cb2f8a72ae..8cd6d6ce11 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -39,7 +39,8 @@ #include -#include "moveit_jog_arm/jog_arm.h" +#include +#include static const std::string LOGNAME = "jog_arm"; @@ -52,9 +53,12 @@ JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneM if (!readParameters()) exit(EXIT_FAILURE); - jog_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); + auto joint_state_subscriber = std::make_shared(nh_, parameters_.joint_topic); - collision_checker_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); + jog_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber); + + collision_checker_ = + std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber); } // Read ROS parameters, typically from YAML file diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 98a801dfb4..8934cf4e9e 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -70,10 +70,12 @@ bool isNonZero(const control_msgs::JointJog& msg) // Constructor for the class that handles jogging calculations JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::shared_ptr& joint_state_subscriber) : nh_(nh) , parameters_(parameters) , planning_scene_monitor_(planning_scene_monitor) + , joint_state_subscriber_(joint_state_subscriber) , period_(parameters.publish_period) { // MoveIt Setup @@ -90,9 +92,6 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getActiveJointModels().size()); - // subscribe to joints - joint_state_sub_ = nh_.subscribe(parameters_.joint_topic, ROS_QUEUE_SIZE, &JogCalcs::jointStateCB, this); - // Subscribe to command topics twist_stamped_sub_ = nh_.subscribe(parameters_.cartesian_command_in_topic, ROS_QUEUE_SIZE, &JogCalcs::twistStampedCB, this); @@ -124,11 +123,6 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, // Publish status status_pub_ = nh_.advertise(parameters_.status_topic, ROS_QUEUE_SIZE); - // Wait for initial messages - ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg."); - ros::topic::waitForMessage(parameters_.joint_topic); - ROS_INFO_NAMED(LOGNAME, "Received first joint msg."); - internal_joint_state_.name = joint_model_group_->getActiveJointModelNames(); num_joints_ = internal_joint_state_.name.size(); internal_joint_state_.position.resize(num_joints_); @@ -184,9 +178,10 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) } // Update from latest state + sensor_msgs::JointStateConstPtr latest_joint_state = joint_state_subscriber_->getLatest(); + kinematic_state_->setVariableValues(*latest_joint_state); { const std::lock_guard lock(latest_state_mutex_); - kinematic_state_->setVariableValues(*incoming_joint_state_); if (latest_twist_stamped_) twist_stamped_ = *latest_twist_stamped_; if (latest_joint_jog_) @@ -754,29 +749,28 @@ void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory) // Parse the incoming joint msg for the joints of our MoveGroup bool JogCalcs::updateJoints() { - // lock the latest state mutex for the joint states - const std::lock_guard lock(latest_state_mutex_); + sensor_msgs::JointStateConstPtr latest_joint_state = joint_state_subscriber_->getLatest(); // Check that the msg contains enough joints - if (incoming_joint_state_->name.size() < num_joints_) + if (latest_joint_state->name.size() < num_joints_) return false; // Store joints in a member variable - for (std::size_t m = 0; m < incoming_joint_state_->name.size(); ++m) + for (std::size_t m = 0; m < latest_joint_state->name.size(); ++m) { std::size_t c; try { - c = joint_state_name_map_.at(incoming_joint_state_->name[m]); + c = joint_state_name_map_.at(latest_joint_state->name[m]); } catch (const std::out_of_range& e) { ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Ignoring joint " - << incoming_joint_state_->name[m]); + << latest_joint_state->name[m]); continue; } - internal_joint_state_.position[c] = incoming_joint_state_->position[m]; + internal_joint_state_.position[c] = latest_joint_state->position[m]; } // Cache the original joints in case they need to be reset @@ -788,9 +782,9 @@ bool JogCalcs::updateJoints() double accel_limit = 0; double joint_velocity = 0; double worst_case_stop_time = 0; - for (size_t jt_state_idx = 0; jt_state_idx < incoming_joint_state_->velocity.size(); ++jt_state_idx) + for (size_t jt_state_idx = 0; jt_state_idx < latest_joint_state->velocity.size(); ++jt_state_idx) { - joint_name = incoming_joint_state_->name[jt_state_idx]; + joint_name = latest_joint_state->name[jt_state_idx]; // Get acceleration limit for this joint for (auto joint_model : joint_model_group_->getActiveJointModels()) @@ -816,7 +810,7 @@ bool JogCalcs::updateJoints() } // Get the current joint velocity - joint_velocity = incoming_joint_state_->velocity[jt_state_idx]; + joint_velocity = latest_joint_state->velocity[jt_state_idx]; // Calculate worst case stop time worst_case_stop_time = std::max(worst_case_stop_time, fabs(joint_velocity / accel_limit)); @@ -881,9 +875,8 @@ Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& comman } catch (const std::out_of_range& e) { - const std::lock_guard lock(latest_state_mutex_); - ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Ignoring joint " - << incoming_joint_state_->name[m]); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "Ignoring joint " << joint_state_subscriber_->getLatest()->name[m]); continue; } // Apply user-defined scaling if inputs are unitless [-1:1] @@ -936,12 +929,6 @@ void JogCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta delta_x.conservativeResize(num_rows); } -void JogCalcs::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) -{ - const std::lock_guard lock(latest_state_mutex_); - incoming_joint_state_ = msg; -} - bool JogCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) { const std::lock_guard lock(latest_state_mutex_); diff --git a/moveit_experimental/moveit_jog_arm/src/joint_state_subscriber.cpp b/moveit_experimental/moveit_jog_arm/src/joint_state_subscriber.cpp new file mode 100644 index 0000000000..b9cfb12ebf --- /dev/null +++ b/moveit_experimental/moveit_jog_arm/src/joint_state_subscriber.cpp @@ -0,0 +1,70 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 the copyright holder 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 HOLDER 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. +*******************************************************************************/ + +/* Title : joint_state_subscriber.cpp + * Project : moveit_jog_arm + * Created : 06/11/2020 + * Author : Tyler Weaver + */ + +#include + +namespace moveit_jog_arm +{ +constexpr char LOGNAME[] = "joint_state_subscriber"; + +// Constructor for the class that handles collision checking +JointStateSubscriber::JointStateSubscriber(ros::NodeHandle& nh, const std::string& joint_state_topic_name) +{ + // subscribe to joints + joint_state_sub_ = nh.subscribe(joint_state_topic_name, ROS_QUEUE_SIZE, &JointStateSubscriber::jointStateCB, this); + + // Wait for initial messages + ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg."); + ros::topic::waitForMessage(joint_state_topic_name); + ROS_INFO_NAMED(LOGNAME, "Received first joint msg."); +} + +sensor_msgs::JointStateConstPtr JointStateSubscriber::getLatest() const +{ + const std::lock_guard lock(joint_state_mutex_); + return latest_joint_state_; +} + +void JointStateSubscriber::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) +{ + const std::lock_guard lock(joint_state_mutex_); + latest_joint_state_ = msg; +} + +} // namespace moveit_jog_arm From 26cd2731369f6a4dd11e448c8afbefe47d219388 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 11 Jun 2020 18:07:47 -0600 Subject: [PATCH 345/612] get latest joint state c++ api added --- .../moveit_jog_arm/include/moveit_jog_arm/jog_arm.h | 8 +++++--- moveit_experimental/moveit_jog_arm/src/jog_arm.cpp | 12 ++++++++---- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h index bd3bf8299b..c774344639 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h @@ -41,6 +41,7 @@ #include #include #include +#include namespace moveit_jog_arm { @@ -75,6 +76,9 @@ class JogArm /** \brief Get the parameters used by jog arm. */ const JogArmParameters& getParameters() const; + /** \brief Get the latest joint state. */ + sensor_msgs::JointStateConstPtr getLatestJointState() const; + private: bool readParameters(); @@ -86,10 +90,8 @@ class JogArm // Store the parameters that were read from ROS server JogArmParameters parameters_; - // Jog calcs + std::shared_ptr joint_state_subscriber_; std::unique_ptr jog_calcs_; - - // Collision checks std::unique_ptr collision_checker_; }; diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp index 8cd6d6ce11..d7ef39d9b9 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp @@ -40,7 +40,6 @@ #include #include -#include static const std::string LOGNAME = "jog_arm"; @@ -53,12 +52,12 @@ JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneM if (!readParameters()) exit(EXIT_FAILURE); - auto joint_state_subscriber = std::make_shared(nh_, parameters_.joint_topic); + joint_state_subscriber_ = std::make_shared(nh_, parameters_.joint_topic); - jog_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber); + jog_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); collision_checker_ = - std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber); + std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); } // Read ROS parameters, typically from YAML file @@ -314,4 +313,9 @@ const JogArmParameters& JogArm::getParameters() const return parameters_; } +sensor_msgs::JointStateConstPtr JogArm::getLatestJointState() const +{ + return joint_state_subscriber_->getLatest(); +} + } // namespace moveit_jog_arm From 23fcbc62afa31bb955463e35b719cf1e99e083a2 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Sun, 14 Jun 2020 07:59:36 -0400 Subject: [PATCH 346/612] update dependencies for python3 in noetic (#2131) * update dependencies for python3 in noetic * add one more that rosdep install didn't catch --- moveit_commander/package.xml | 13 ++++++++----- moveit_core/package.xml | 6 ++++-- moveit_kinematics/package.xml | 5 +++-- moveit_ros/planning_interface/package.xml | 9 ++++++--- 4 files changed, 21 insertions(+), 12 deletions(-) diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml index 4d58b563ec..6e6b7fb51d 100644 --- a/moveit_commander/package.xml +++ b/moveit_commander/package.xml @@ -1,4 +1,5 @@ - + + moveit_commander 1.1.0 Python interfaces to MoveIt @@ -16,15 +17,17 @@ https://github.com/ros-planning/moveit catkin - python-catkin-pkg + python-catkin-pkg + python3-catkin-pkg - python + python + python3 geometry_msgs moveit_msgs moveit_ros_planning_interface - python - python-pyassimp + python-pyassimp + python3-pyassimp rospy sensor_msgs shape_msgs diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 26f94f3826..1ddc19cb5f 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -1,4 +1,5 @@ - + + moveit_core 1.1.0 Core libraries used by MoveIt @@ -53,7 +54,8 @@ moveit_resources angles tf2_kdl - orocos_kdl + orocos_kdl + liborocos-kdl-dev rosunit diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 561d943079..a6bf3b78bd 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -1,5 +1,5 @@ - + moveit_kinematics 1.1.0 Package for all inverse kinematics solvers in MoveIt @@ -27,7 +27,8 @@ eigen tf2 tf2_kdl - orocos_kdl + orocos_kdl + liborocos-kdl-dev python-lxml liburdfdom-tools diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index 90dc50db0b..5a864a72c8 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -1,4 +1,5 @@ - + + moveit_ros_planning_interface 1.1.0 Components of MoveIt that offer simpler interfaces to planning and execution @@ -16,7 +17,8 @@ https://github.com/ros-planning/moveit catkin - python-catkin-pkg + python-catkin-pkg + python3-catkin-pkg moveit_ros_planning moveit_ros_warehouse @@ -32,7 +34,8 @@ tf2_eigen tf2_geometry_msgs tf2_ros - python + python + python3 eigenpy eigen From de9f7841a4f41833bdc0f53bdc25cb6de915129f Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 17 Jun 2020 07:26:47 -0600 Subject: [PATCH 347/612] Update moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp Co-authored-by: Henning Kayser --- .../moveit_jog_arm/test/jog_cpp_interface_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp index 7b72e594e6..247d905e66 100644 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp +++ b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp @@ -163,7 +163,7 @@ TEST_F(JogArmFixture, SendJointJogTest) ros::Rate publish_rate(1. / publish_period); - // Send a few Cartesian velocity commands + // Send a few joint velocity commands for (size_t i = 0; i < num_commands && ros::ok(); ++i) { auto msg = moveit::util::make_shared_from_pool(); From b05e0ae5c1726bb3b1453e94f56599f4979042d1 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 17 Jun 2020 13:53:27 -0600 Subject: [PATCH 348/612] pragma once in jog_arm.h (#2152) Fixup #2103. --- .../moveit_jog_arm/include/moveit_jog_arm/jog_arm.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h index c774344639..0949fcb71b 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h @@ -36,6 +36,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ +#pragma once + #include #include From d85c0895601e69c8ea7e31ac3fb03674bbbbb413 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 18 Jun 2020 12:04:50 -0600 Subject: [PATCH 349/612] [jog_arm] remove duplicate line --- moveit_experimental/moveit_jog_arm/src/collision_check.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp index 1d8619a62b..547b6feb63 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp @@ -75,7 +75,6 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArm // Internal namespace ros::NodeHandle internal_nh("~internal"); collision_velocity_scale_pub_ = internal_nh.advertise("collision_velocity_scale", ROS_QUEUE_SIZE); - collision_velocity_scale_pub_ = internal_nh.advertise("collision_velocity_scale", 1); worst_case_stop_time_sub_ = internal_nh.subscribe("worst_case_stop_time", ROS_QUEUE_SIZE, &CollisionCheck::worstCaseStopTimeCB, this); From 9e4a1646faabc87a1aae3ffc9c4802f7ecf10a40 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 19 Jun 2020 08:44:20 -0600 Subject: [PATCH 350/612] [jog_arm] fix access past end of array bug (#2155) * [jog_arm] fix access past end of array bug --- moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 8934cf4e9e..0892103116 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -298,9 +298,9 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") { auto joints = moveit::util::make_shared_from_pool(); - if (parameters_.publish_joint_positions) + if (parameters_.publish_joint_positions && !joint_trajectory->points.empty()) joints->data = joint_trajectory->points[0].positions; - else if (parameters_.publish_joint_velocities) + else if (parameters_.publish_joint_velocities && !joint_trajectory->points.empty()) joints->data = joint_trajectory->points[0].velocities; outgoing_cmd_pub_.publish(joints); } From e2a10d73eaeb1e3469a7b2921a2f93592836bb7d Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Mon, 22 Jun 2020 13:52:56 -0500 Subject: [PATCH 351/612] [jog_arm] Changes before porting to ROS2 (#2151) * throttle warning logs * ROS1 Basic improvements and changes * Fixes to drift dimensions, singularity velocity scaling * tf name changes, const fixes, slight logic changes * Reverting enforceSRDFAccelVelLimits changes for now * Move ROS_LOG_THROTTLE_PERIOD to cpp files * Clang formatting * Track staleness of joint and twist seperately * Ensure joint_trajectory output is always populated with something, even when no jog * Fix joint trajectory redundant points for gazebo pub * Fix crazy joint jog from bad Eigen init * Fix variable type in addJointIncrements() * Initialize last sent command in constructor * More explicit joint_jog_cmd_ and twist_stamped_cmd_ names * Add comment clarying transform calculation / use Co-authored-by: Tyler Weaver Co-authored-by: AndyZe --- .../moveit_jog_arm/jog_arm_parameters.h | 3 - .../include/moveit_jog_arm/jog_calcs.h | 27 ++- .../moveit_jog_arm/src/collision_check.cpp | 3 +- .../moveit_jog_arm/src/jog_calcs.cpp | 215 +++++++++++------- 4 files changed, 155 insertions(+), 93 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h index d9642ac91e..a9e2cfa245 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h @@ -43,9 +43,6 @@ namespace moveit_jog_arm // Size of queues used in ros pub/sub/service constexpr size_t ROS_QUEUE_SIZE = 2; -// Seconds to throttle logs inside loops -constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30; - // ROS params to be read. See the yaml file in /config for a description of each. struct JogArmParameters { diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h index de9ef06b19..9c87ccd5e2 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h @@ -114,7 +114,6 @@ class JogCalcs * Is handled differently for position vs. velocity control. */ void suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory); - void suddenHalt(Eigen::ArrayXd& delta_theta); /** \brief Scale the delta theta to match joint velocity/acceleration limits */ void enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta); @@ -127,7 +126,7 @@ class JogCalcs */ double velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, const Eigen::JacobiSVD& svd, - const Eigen::MatrixXd& jacobian, const Eigen::MatrixXd& pseudo_inverse); + const Eigen::MatrixXd& pseudo_inverse); /** * Slow motion down if close to singularity or collision. @@ -137,12 +136,15 @@ class JogCalcs void applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singularity_scale); /** \brief Compose the outgoing JointTrajectory message */ - void composeJointTrajMessage(sensor_msgs::JointState& joint_state, + void composeJointTrajMessage(const sensor_msgs::JointState& joint_state, trajectory_msgs::JointTrajectory& joint_trajectory) const; /** \brief Smooth position commands with a lowpass filter */ void lowPassFilterPositions(sensor_msgs::JointState& joint_state); + /** \brief Set the filters to the specified values */ + void resetLowPassFilters(const sensor_msgs::JointState& joint_state); + /** \brief Convert an incremental position command to joint velocity message */ void calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta); @@ -207,14 +209,17 @@ class JogCalcs // Flag for staying inactive while there are no incoming commands bool wait_for_jog_commands_ = true; + // Flag saying if the filters were updated during the timer callback + bool updated_filters_ = false; + // Nonzero status flags bool have_nonzero_twist_stamped_ = false; bool have_nonzero_joint_jog_ = false; bool have_nonzero_command_ = false; // Incoming command messages - geometry_msgs::TwistStamped twist_stamped_; - control_msgs::JointJog joint_jog_; + geometry_msgs::TwistStamped twist_stamped_cmd_; + control_msgs::JointJog joint_jog_cmd_; const moveit::core::JointModelGroup* joint_model_group_; @@ -229,6 +234,8 @@ class JogCalcs std::vector position_filters_; + trajectory_msgs::JointTrajectoryConstPtr last_sent_command_; + // ROS ros::Timer timer_; ros::Duration period_; @@ -246,7 +253,8 @@ class JogCalcs StatusCode status_ = StatusCode::NO_WARNING; bool stop_requested_ = false; bool paused_ = false; - bool command_is_stale_ = false; + bool twist_command_is_stale_ = false; + bool joint_command_is_stale_ = false; bool ok_to_publish_ = false; double collision_velocity_scale_ = 1.0; @@ -269,9 +277,12 @@ class JogCalcs // latest_state_mutex_ is used to protect the state below it mutable std::mutex latest_state_mutex_; - Eigen::Isometry3d tf_moveit_to_cmd_frame_; + Eigen::Isometry3d tf_moveit_to_robot_cmd_frame_; geometry_msgs::TwistStampedConstPtr latest_twist_stamped_; control_msgs::JointJogConstPtr latest_joint_jog_; - ros::Time latest_command_stamp_ = ros::Time(0.); + ros::Time latest_twist_command_stamp_ = ros::Time(0.); + ros::Time latest_joint_command_stamp_ = ros::Time(0.); + bool latest_nonzero_twist_stamped_ = false; + bool latest_nonzero_joint_jog_ = false; }; } // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp index 547b6feb63..18f9ff5a6a 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check.cpp @@ -44,7 +44,8 @@ static const char LOGNAME[] = "collision_check"; static const double MIN_RECOMMENDED_COLLISION_RATE = 10; -constexpr double EPSILON = 1e-6; // For very small numeric comparisons +constexpr double EPSILON = 1e-6; // For very small numeric comparisons +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30; // Seconds to throttle logs inside loops namespace moveit_jog_arm { diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 0892103116..fe27c475f7 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -44,6 +44,7 @@ #include static const std::string LOGNAME = "jog_calcs"; +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30; // Seconds to throttle logs inside loops namespace moveit_jog_arm { @@ -127,13 +128,40 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, num_joints_ = internal_joint_state_.name.size(); internal_joint_state_.position.resize(num_joints_); internal_joint_state_.velocity.resize(num_joints_); + + // Set up the "last" published message, in case we need to send it first + auto initial_joint_trajectory = moveit::util::make_shared_from_pool(); + auto latest_joints = joint_state_subscriber_->getLatest(); + initial_joint_trajectory->header.frame_id = parameters_.planning_frame; + initial_joint_trajectory->header.stamp = ros::Time::now(); + initial_joint_trajectory->joint_names = internal_joint_state_.name; + trajectory_msgs::JointTrajectoryPoint point; + point.time_from_start = ros::Duration(parameters_.publish_period); + if (parameters_.publish_joint_positions) + point.positions = latest_joints->position; + if (parameters_.publish_joint_velocities) + { + std::vector velocity(num_joints_); + point.velocities = velocity; + } + if (parameters_.publish_joint_accelerations) + { + // I do not know of a robot that takes acceleration commands. + // However, some controllers check that this data is non-empty. + // Send all zeros, for now. + std::vector acceleration(num_joints_); + point.accelerations = acceleration; + } + initial_joint_trajectory->points.push_back(point); + last_sent_command_ = initial_joint_trajectory; + // A map for the indices of incoming joint commands - for (std::size_t i = 0; i < internal_joint_state_.name.size(); ++i) + for (std::size_t i = 0; i < num_joints_; ++i) { joint_state_name_map_[internal_joint_state_.name[i]] = i; } - // Low-pass filters for the joint positions & velocities + // Low-pass filters for the joint positions for (size_t i = 0; i < num_joints_; ++i) { position_filters_.emplace_back(parameters_.low_pass_filter_coeff); @@ -183,38 +211,41 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) { const std::lock_guard lock(latest_state_mutex_); if (latest_twist_stamped_) - twist_stamped_ = *latest_twist_stamped_; + twist_stamped_cmd_ = *latest_twist_stamped_; if (latest_joint_jog_) - joint_jog_ = *latest_joint_jog_; + joint_jog_cmd_ = *latest_joint_jog_; // Check for stale cmds - command_is_stale_ = - ((ros::Time::now() - latest_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); + twist_command_is_stale_ = + ((ros::Time::now() - latest_twist_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); + joint_command_is_stale_ = + ((ros::Time::now() - latest_joint_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); + + have_nonzero_twist_stamped_ = latest_nonzero_twist_stamped_; + have_nonzero_joint_jog_ = latest_nonzero_joint_jog_; } // Get the transform from MoveIt planning frame to jogging command frame + // Calculate this transform to ensure it is available via C++ API // We solve (planning_frame -> base -> robot_link_command_frame) // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) - tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * - kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); - - // Input frame determined by YAML file if not passed with message - if (twist_stamped_.header.frame_id.empty()) - { - twist_stamped_.header.frame_id = parameters_.robot_link_command_frame; - } + tf_moveit_to_robot_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); have_nonzero_command_ = have_nonzero_twist_stamped_ || have_nonzero_joint_jog_; + // Don't end this function without updating the filters + updated_filters_ = false; + // If paused or while waiting for initial jog commands, just keep the low-pass filters up to date with current // joints so a jump doesn't occur when restarting if (wait_for_jog_commands_ || paused_) { - for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(original_joint_state_.position[i]); + resetLowPassFilters(original_joint_state_); // Check if there are any new commands with valid timestamp - wait_for_jog_commands_ = twist_stamped_.header.stamp == ros::Time(0.) && joint_jog_.header.stamp == ros::Time(0.); + wait_for_jog_commands_ = + twist_stamped_cmd_.header.stamp == ros::Time(0.) && joint_jog_cmd_.header.stamp == ros::Time(0.); // Early exit return; @@ -225,29 +256,44 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // Create new outgoing joint trajectory command message auto joint_trajectory = moveit::util::make_shared_from_pool(); - // If the input command isn't stale, run calculations - if (!command_is_stale_) + // Prioritize cartesian jogging above joint jogging + // Only run commands if not stale and nonzero + if (have_nonzero_twist_stamped_ && !twist_command_is_stale_) + { + if (!cartesianJogCalcs(twist_stamped_cmd_, *joint_trajectory)) + { + resetLowPassFilters(original_joint_state_); + return; + } + } + else if (have_nonzero_joint_jog_ && !joint_command_is_stale_) { - // Prioritize cartesian jogging above joint jogging - if (have_nonzero_twist_stamped_) + if (!jointJogCalcs(joint_jog_cmd_, *joint_trajectory)) { - if (!cartesianJogCalcs(twist_stamped_, *joint_trajectory)) - return; + resetLowPassFilters(original_joint_state_); + return; } - else if (have_nonzero_joint_jog_) + } + else + { + // Joint trajectory is not populated with anything, so set it to the last positions and 0 velocity + *joint_trajectory = *last_sent_command_; + for (auto point : joint_trajectory->points) { - if (!jointJogCalcs(joint_jog_, *joint_trajectory)) - return; + point.velocities.assign(point.velocities.size(), 0); } } + // Print a warning to the user if both are stale + if (!twist_command_is_stale_ || !joint_command_is_stale_) + { + ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " + "Try a larger 'incoming_command_timeout' parameter?"); + } + // If we should halt if (!have_nonzero_command_) { - // Keep the joint position filters up-to-date with current joints - for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(original_joint_state_.position[i]); - suddenHalt(*joint_trajectory); have_nonzero_twist_stamped_ = false; have_nonzero_joint_jog_ = false; @@ -268,7 +314,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // Store last zero-velocity message flag to prevent superfluous warnings. // Cartesian and joint commands must both be zero. - if (!have_nonzero_twist_stamped_ && !have_nonzero_joint_jog_) + if (!have_nonzero_command_) { // Avoid overflow if (zero_velocity_count_ < std::numeric_limits::max()) @@ -279,13 +325,6 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) zero_velocity_count_ = 0; } - if (command_is_stale_) - { - ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, - "Stale command. " - "Try a larger 'incoming_command_timeout' parameter?"); - } - if (ok_to_publish_) { // Put the outgoing msg in the right format @@ -304,7 +343,13 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) joints->data = joint_trajectory->points[0].velocities; outgoing_cmd_pub_.publish(joints); } + + last_sent_command_ = joint_trajectory; } + + // Update the filters if we haven't yet + if (!updated_filters_) + resetLowPassFilters(original_joint_state_); } // Perform the jogging calculations bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory) @@ -350,14 +395,23 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_ms Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z); Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z); - // We solve (planning_frame -> base -> cmd.header.frame_id) - // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id) - const auto tf_planning_to_cmd_frame = - kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * - kinematic_state_->getGlobalLinkTransform(cmd.header.frame_id); - - translation_vector = tf_planning_to_cmd_frame.linear() * translation_vector; - angular_vector = tf_planning_to_cmd_frame.linear() * angular_vector; + // If the incoming frame is empty or is the command frame, we use the previously calculated tf + if (cmd.header.frame_id.empty() || cmd.header.frame_id == parameters_.robot_link_command_frame) + { + translation_vector = tf_moveit_to_robot_cmd_frame_.linear() * translation_vector; + angular_vector = tf_moveit_to_robot_cmd_frame_.linear() * angular_vector; + } + else + { + // We solve (planning_frame -> base -> cmd.header.frame_id) + // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id) + const auto tf_moveit_to_incoming_cmd_frame = + kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + kinematic_state_->getGlobalLinkTransform(cmd.header.frame_id); + + translation_vector = tf_moveit_to_incoming_cmd_frame.linear() * translation_vector; + angular_vector = tf_moveit_to_incoming_cmd_frame.linear() * angular_vector; + } // Put these components back into a TwistStamped cmd.header.frame_id = parameters_.planning_frame; @@ -378,7 +432,7 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_ms // i.e. take advantage of task redundancy. // Remove the Jacobian rows corresponding to True in the vector drift_dimensions // Work backwards through the 6-vector so indices don't get out of order - for (auto dimension = jacobian.rows(); dimension >= 0; --dimension) + for (auto dimension = jacobian.rows() - 1; dimension >= 0; --dimension) { if (drift_dimensions_[dimension] && jacobian.rows() > 1) { @@ -396,11 +450,11 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_ms enforceSRDFAccelVelLimits(delta_theta_); // If close to a collision or a singularity, decelerate - applyVelocityScaling(delta_theta_, velocityScalingFactorForSingularity(delta_x, svd, jacobian, pseudo_inverse)); + applyVelocityScaling(delta_theta_, velocityScalingFactorForSingularity(delta_x, svd, pseudo_inverse)); if (status_ == StatusCode::HALT_FOR_COLLISION) { - ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Halting for collision!"); - suddenHalt(delta_theta_); + ROS_ERROR_STREAM_THROTTLE_NAMED(5, LOGNAME, "Halting for collision!"); + delta_theta_.setZero(); } prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; @@ -410,7 +464,7 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_ms bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory) { - // Check for nan's or |delta|>1 in the incoming command + // Check for nan's for (double velocity : cmd.velocities) { if (std::isnan(velocity)) @@ -426,8 +480,6 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, trajectory_msgs: enforceSRDFAccelVelLimits(delta_theta_); - kinematic_state_->setVariableValues(internal_joint_state_); - prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; return convertDeltasToOutgoingCmd(joint_trajectory); @@ -469,10 +521,10 @@ void JogCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTraject joint_trajectory.points.resize(count); auto point = joint_trajectory.points[0]; // Start from 2 because we already have the first point. End at count+1 so (total #) == count - for (int i = 2; i < count + 1; ++i) + for (int i = 2; i < count; ++i) { point.time_from_start = ros::Duration(i * parameters_.publish_period); - joint_trajectory.points.push_back(point); + joint_trajectory.points[i] = point; } } @@ -482,6 +534,18 @@ void JogCalcs::lowPassFilterPositions(sensor_msgs::JointState& joint_state) { joint_state.position[i] = position_filters_[i].filter(joint_state.position[i]); } + + updated_filters_ = true; +} + +void JogCalcs::resetLowPassFilters(const sensor_msgs::JointState& joint_state) +{ + for (std::size_t i = 0; i < position_filters_.size(); ++i) + { + position_filters_[i].reset(joint_state.position[i]); + } + + updated_filters_ = true; } void JogCalcs::calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta) @@ -492,7 +556,7 @@ void JogCalcs::calculateJointVelocities(sensor_msgs::JointState& joint_state, co } } -void JogCalcs::composeJointTrajMessage(sensor_msgs::JointState& joint_state, +void JogCalcs::composeJointTrajMessage(const sensor_msgs::JointState& joint_state, trajectory_msgs::JointTrajectory& joint_trajectory) const { joint_trajectory.header.frame_id = parameters_.planning_frame; @@ -537,11 +601,10 @@ void JogCalcs::applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singular // Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, const Eigen::JacobiSVD& svd, - const Eigen::MatrixXd& jacobian, const Eigen::MatrixXd& pseudo_inverse) { double velocity_scale = 1; - std::size_t num_dimensions = jacobian.rows(); + std::size_t num_dimensions = commanded_velocity.size(); // Find the direction away from nearest singularity. // The last column of U from the SVD of the Jacobian points directly toward or away from the singularity. @@ -564,8 +627,9 @@ double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& comm kinematic_state_->copyJointGroupPositions(joint_model_group_, new_theta); new_theta += pseudo_inverse * delta_x; kinematic_state_->setJointGroupPositions(joint_model_group_, new_theta); + auto new_jacobian = kinematic_state_->getJacobian(joint_model_group_); - Eigen::JacobiSVD new_svd(jacobian); + Eigen::JacobiSVD new_svd(new_jacobian); double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1); // If new_condition < ini_condition, the singular vector does point towards a // singularity. Otherwise, flip its direction. @@ -671,8 +735,8 @@ void JogCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta) velocity(joint_delta_index) = relative_change * velocity(joint_delta_index); } } - ++joint_delta_index; } + ++joint_delta_index; } } @@ -716,13 +780,6 @@ bool JogCalcs::enforceSRDFPositionLimits() return !halting; } -// Suddenly halt for a joint limit or other critical issue. -// Is handled differently for position vs. velocity control. -void JogCalcs::suddenHalt(Eigen::ArrayXd& delta_theta) -{ - delta_theta = Eigen::ArrayXd::Zero(delta_theta.rows()); -} - // Suddenly halt for a joint limit or other critical issue. // Is handled differently for position vs. velocity control. void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory) @@ -801,9 +858,9 @@ bool JogCalcs::updateJoints() } else { - ROS_ERROR_STREAM_ONCE_NAMED(LOGNAME, "An acceleration limit is not defined for this joint, '" - << joint_model->getName() << "'; minimum stop distance should not " - "be used for collision checking"); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "An acceleration limit is not defined for this joint; minimum stop distance " + "should not be used for collision checking"); } break; } @@ -860,11 +917,7 @@ Eigen::VectorXd JogCalcs::scaleCartesianCommand(const geometry_msgs::TwistStampe Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& command) const { Eigen::VectorXd result(num_joints_); - - for (std::size_t i = 0; i < num_joints_; ++i) - { - result[i] = 0.0; - } + result.setZero(); std::size_t c; for (std::size_t m = 0; m < command.joint_names.size(); ++m) @@ -899,7 +952,7 @@ bool JogCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen:: { try { - output.position[i] += increments[static_cast(i)]; + output.position[i] += increments[i]; } catch (const std::out_of_range& e) { @@ -932,7 +985,7 @@ void JogCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta bool JogCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) { const std::lock_guard lock(latest_state_mutex_); - transform = tf_moveit_to_cmd_frame_; + transform = tf_moveit_to_robot_cmd_frame_; // All zeros means the transform wasn't initialized, so return false return !transform.matrix().isZero(0); @@ -942,20 +995,20 @@ void JogCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg) { const std::lock_guard lock(latest_state_mutex_); latest_twist_stamped_ = msg; - have_nonzero_twist_stamped_ = isNonZero(*latest_twist_stamped_); + latest_nonzero_twist_stamped_ = isNonZero(*latest_twist_stamped_); if (msg->header.stamp != ros::Time(0.)) - latest_command_stamp_ = msg->header.stamp; + latest_twist_command_stamp_ = msg->header.stamp; } void JogCalcs::jointJogCB(const control_msgs::JointJogConstPtr& msg) { const std::lock_guard lock(latest_state_mutex_); latest_joint_jog_ = msg; - have_nonzero_joint_jog_ = isNonZero(*latest_joint_jog_); + latest_nonzero_joint_jog_ = isNonZero(*latest_joint_jog_); if (msg->header.stamp != ros::Time(0.)) - latest_command_stamp_ = msg->header.stamp; + latest_joint_command_stamp_ = msg->header.stamp; } void JogCalcs::collisionVelocityScaleCB(const std_msgs::Float64ConstPtr& msg) From 4f9531d253af14e102b13e5eb416ddd7c8b2e927 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 24 Jun 2020 18:35:28 +0200 Subject: [PATCH 352/612] TEM: fix check for start state position (#2157) distance() respects unlimited revolute joints --- .../src/trajectory_execution_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index c07c592fb8..a7920194f7 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -972,7 +972,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont // normalize positions and compare jm->enforcePositionBounds(&cur_position); jm->enforcePositionBounds(&traj_position); - if (fabs(cur_position - traj_position) > allowed_start_tolerance_) + if (jm->distance(&cur_position, &traj_position) > allowed_start_tolerance_) { ROS_ERROR_NAMED(name_, "\nInvalid Trajectory: start point deviates from current robot state more than %g" "\njoint '%s': expected: %g, current: %g", From 335ba7e7710d2189e98ee769a9688f364a26e330 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 25 Jun 2020 11:38:24 +0200 Subject: [PATCH 353/612] Add missing boost::regex dependency (#2163) --- moveit_core/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index e645a0d179..6c336a1e9d 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -26,7 +26,7 @@ endif() if(WIN32) set(EXTRA_BOOST_COMPONENTS zlib) endif() -find_package(Boost REQUIRED system filesystem date_time thread iostreams ${EXTRA_BOOST_COMPONENTS}) +find_package(Boost REQUIRED system filesystem date_time thread iostreams regex ${EXTRA_BOOST_COMPONENTS}) find_package(Eigen3 REQUIRED) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") From b8b57846650f7662a44106e0b7728e0c9ca36888 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 25 Jun 2020 14:24:45 -0600 Subject: [PATCH 354/612] optional cpp version setting (#2166) --- moveit_core/CMakeLists.txt | 4 +++- moveit_experimental/moveit_jog_arm/CMakeLists.txt | 4 +++- moveit_kinematics/CMakeLists.txt | 4 +++- .../ikfast_kinematics_plugin/templates/CMakeLists.txt | 4 +++- moveit_planners/chomp/chomp_interface/CMakeLists.txt | 4 +++- moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt | 4 +++- moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt | 4 +++- moveit_planners/ompl/CMakeLists.txt | 4 +++- moveit_planners/trajopt/CMakeLists.txt | 4 +++- moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt | 4 +++- moveit_plugins/moveit_ros_control_interface/CMakeLists.txt | 4 +++- .../moveit_simple_controller_manager/CMakeLists.txt | 4 +++- moveit_ros/benchmarks/CMakeLists.txt | 4 +++- moveit_ros/manipulation/CMakeLists.txt | 4 +++- moveit_ros/move_group/CMakeLists.txt | 4 +++- moveit_ros/occupancy_map_monitor/CMakeLists.txt | 4 +++- moveit_ros/perception/CMakeLists.txt | 4 +++- moveit_ros/planning/CMakeLists.txt | 4 +++- moveit_ros/planning_interface/CMakeLists.txt | 4 +++- moveit_ros/robot_interaction/CMakeLists.txt | 4 +++- moveit_ros/visualization/CMakeLists.txt | 4 +++- moveit_ros/warehouse/CMakeLists.txt | 4 +++- moveit_setup_assistant/CMakeLists.txt | 4 +++- 23 files changed, 69 insertions(+), 23 deletions(-) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 6c336a1e9d..04a960fd48 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_core) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_experimental/moveit_jog_arm/CMakeLists.txt index da429665cc..4b7ccda0aa 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_experimental/moveit_jog_arm/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_jog_arm) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index 67f843f1ce..1ba7d8f56f 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/moveit_kinematics/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_kinematics) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt index f1ca31f341..71bc2f0442 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(_PACKAGE_NAME_) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_planners/chomp/chomp_interface/CMakeLists.txt b/moveit_planners/chomp/chomp_interface/CMakeLists.txt index 1c64f232e5..3c49159201 100644 --- a/moveit_planners/chomp/chomp_interface/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_interface/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_planners_chomp) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt index cd3e7a29b0..3aa69715fb 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(chomp_motion_planner) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt index fd4f0bdd5a..a08a00e79e 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_chomp_optimizer_adapter) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_planners/ompl/CMakeLists.txt b/moveit_planners/ompl/CMakeLists.txt index 20577b7fac..e9e745df30 100644 --- a/moveit_planners/ompl/CMakeLists.txt +++ b/moveit_planners/ompl/CMakeLists.txt @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_planners_ompl) # At least C++11 required for OMPL -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_planners/trajopt/CMakeLists.txt b/moveit_planners/trajopt/CMakeLists.txt index ad66fd900c..4d7d8f54e1 100644 --- a/moveit_planners/trajopt/CMakeLists.txt +++ b/moveit_planners/trajopt/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_planners_trajopt) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt index 95636d536f..2d4c43f82b 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_fake_controller_manager) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt index 00988dcb65..04d702e1d1 100644 --- a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt +++ b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_control_interface) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt index caaa598307..b338d15d5f 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_simple_controller_manager) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt index 1a84d206de..3bae2ed4e0 100644 --- a/moveit_ros/benchmarks/CMakeLists.txt +++ b/moveit_ros/benchmarks/CMakeLists.txt @@ -3,7 +3,9 @@ project(moveit_ros_benchmarks) set(MOVEIT_LIB_NAME moveit_ros_benchmarks) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_ros/manipulation/CMakeLists.txt b/moveit_ros/manipulation/CMakeLists.txt index 1288e6caba..0cf81cd7bd 100644 --- a/moveit_ros/manipulation/CMakeLists.txt +++ b/moveit_ros/manipulation/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_manipulation) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index d74c89ea65..5ec56208e7 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_move_group) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_ros/occupancy_map_monitor/CMakeLists.txt b/moveit_ros/occupancy_map_monitor/CMakeLists.txt index 686ef96e38..1390c52719 100644 --- a/moveit_ros/occupancy_map_monitor/CMakeLists.txt +++ b/moveit_ros/occupancy_map_monitor/CMakeLists.txt @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_occupancy_map_monitor) set(MOVEIT_LIB_NAME ${PROJECT_NAME}) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_ros/perception/CMakeLists.txt b/moveit_ros/perception/CMakeLists.txt index 0456a0299f..74c908d4cd 100644 --- a/moveit_ros/perception/CMakeLists.txt +++ b/moveit_ros/perception/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_perception) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 6383fe717c..5c95e7df0b 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_planning) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 3ba534ecf8..879cb6a399 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_planning_interface) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_ros/robot_interaction/CMakeLists.txt b/moveit_ros/robot_interaction/CMakeLists.txt index 0380aef8db..c896af71c2 100644 --- a/moveit_ros/robot_interaction/CMakeLists.txt +++ b/moveit_ros/robot_interaction/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_robot_interaction) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_ros/visualization/CMakeLists.txt b/moveit_ros/visualization/CMakeLists.txt index c1c71a5bbc..51e5011804 100644 --- a/moveit_ros/visualization/CMakeLists.txt +++ b/moveit_ros/visualization/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_visualization) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_ros/warehouse/CMakeLists.txt b/moveit_ros/warehouse/CMakeLists.txt index ec829566b3..e8a615e079 100644 --- a/moveit_ros/warehouse/CMakeLists.txt +++ b/moveit_ros/warehouse/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_warehouse) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) diff --git a/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/CMakeLists.txt index 6f9b85ef44..2a60903728 100644 --- a/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_setup_assistant) -set(CMAKE_CXX_STANDARD 14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) From 0672c9e9d622907db43bfcb19c2a02a3ebfef03a Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Tue, 23 Jun 2020 09:33:42 -0500 Subject: [PATCH 355/612] Move moveit_experimental/moveit_jog_arm -> moveit_ros/moveit_jog_arm --- {moveit_experimental => moveit_ros}/moveit_jog_arm/CMakeLists.txt | 0 {moveit_experimental => moveit_ros}/moveit_jog_arm/README.md | 0 .../moveit_jog_arm/config/spacenav_via_teleop_tools.yaml | 0 .../moveit_jog_arm/config/ur_simulated_config.yaml | 0 .../moveit_jog_arm/include/moveit_jog_arm/collision_check.h | 0 .../moveit_jog_arm/include/moveit_jog_arm/jog_arm.h | 0 .../moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h | 0 .../moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h | 0 .../include/moveit_jog_arm/joint_state_subscriber.h | 0 .../moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h | 0 .../moveit_jog_arm/include/moveit_jog_arm/make_shared_from_pool.h | 0 .../moveit_jog_arm/include/moveit_jog_arm/status_codes.h | 0 .../moveit_jog_arm/launch/cpp_interface_example.launch | 0 .../moveit_jog_arm/launch/spacenav_cpp.launch | 0 .../moveit_jog_arm/launch/spacenav_teleop_tools.launch | 0 {moveit_experimental => moveit_ros}/moveit_jog_arm/package.xml | 0 .../moveit_jog_arm/src/collision_check.cpp | 0 .../src/cpp_interface_example/cpp_interface_example.cpp | 0 .../moveit_jog_arm/src/jog_arm.cpp | 0 .../moveit_jog_arm/src/jog_calcs.cpp | 0 .../moveit_jog_arm/src/jog_server.cpp | 0 .../moveit_jog_arm/src/joint_state_subscriber.cpp | 0 .../moveit_jog_arm/src/low_pass_filter.cpp | 0 .../moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp | 0 .../moveit_jog_arm/test/config/initial_position.yaml | 0 .../moveit_jog_arm/test/config/jog_settings.yaml | 0 .../moveit_jog_arm/test/config/singular_position.yaml | 0 .../moveit_jog_arm/test/jog_cpp_interface_test.cpp | 0 .../moveit_jog_arm/test/jog_cpp_interface_test.test | 0 .../moveit_jog_arm/test/launch/common_test_setup.launch | 0 .../test/launch/jog_arm_drift_dimensions_service_test.test | 0 .../moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test | 0 .../moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test | 0 .../moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test | 0 .../moveit_jog_arm/test/python_tests/__init__.py | 0 .../drift_dimensions_service/test_drift_dimensions_service.py | 0 .../test/python_tests/halt_msg/test_jog_arm_halt_msg.py | 0 .../test/python_tests/msg_reception/test_jog_arm_msg_reception.py | 0 .../moveit_jog_arm/test/python_tests/util.py | 0 .../test/python_tests/vel_accel_limits/test_vel_accel_limits.py | 0 40 files changed, 0 insertions(+), 0 deletions(-) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/CMakeLists.txt (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/README.md (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/config/ur_simulated_config.yaml (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/include/moveit_jog_arm/collision_check.h (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/include/moveit_jog_arm/joint_state_subscriber.h (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/include/moveit_jog_arm/make_shared_from_pool.h (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/include/moveit_jog_arm/status_codes.h (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/launch/cpp_interface_example.launch (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/launch/spacenav_cpp.launch (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/launch/spacenav_teleop_tools.launch (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/package.xml (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/src/collision_check.cpp (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/src/jog_arm.cpp (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/src/jog_calcs.cpp (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/src/jog_server.cpp (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/src/joint_state_subscriber.cpp (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/src/low_pass_filter.cpp (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/config/initial_position.yaml (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/config/jog_settings.yaml (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/config/singular_position.yaml (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/jog_cpp_interface_test.cpp (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/jog_cpp_interface_test.test (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/launch/common_test_setup.launch (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/python_tests/__init__.py (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/python_tests/util.py (100%) rename {moveit_experimental => moveit_ros}/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py (100%) diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_ros/moveit_jog_arm/CMakeLists.txt similarity index 100% rename from moveit_experimental/moveit_jog_arm/CMakeLists.txt rename to moveit_ros/moveit_jog_arm/CMakeLists.txt diff --git a/moveit_experimental/moveit_jog_arm/README.md b/moveit_ros/moveit_jog_arm/README.md similarity index 100% rename from moveit_experimental/moveit_jog_arm/README.md rename to moveit_ros/moveit_jog_arm/README.md diff --git a/moveit_experimental/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml b/moveit_ros/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml similarity index 100% rename from moveit_experimental/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml rename to moveit_ros/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml diff --git a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml b/moveit_ros/moveit_jog_arm/config/ur_simulated_config.yaml similarity index 100% rename from moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml rename to moveit_ros/moveit_jog_arm/config/ur_simulated_config.yaml diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h b/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/collision_check.h similarity index 100% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check.h rename to moveit_ros/moveit_jog_arm/include/moveit_jog_arm/collision_check.h diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h similarity index 100% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h rename to moveit_ros/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h b/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h similarity index 100% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h rename to moveit_ros/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h similarity index 100% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h rename to moveit_ros/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/joint_state_subscriber.h b/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/joint_state_subscriber.h similarity index 100% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/joint_state_subscriber.h rename to moveit_ros/moveit_jog_arm/include/moveit_jog_arm/joint_state_subscriber.h diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h b/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h similarity index 100% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h rename to moveit_ros/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/make_shared_from_pool.h b/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/make_shared_from_pool.h similarity index 100% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/make_shared_from_pool.h rename to moveit_ros/moveit_jog_arm/include/moveit_jog_arm/make_shared_from_pool.h diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h b/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/status_codes.h similarity index 100% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h rename to moveit_ros/moveit_jog_arm/include/moveit_jog_arm/status_codes.h diff --git a/moveit_experimental/moveit_jog_arm/launch/cpp_interface_example.launch b/moveit_ros/moveit_jog_arm/launch/cpp_interface_example.launch similarity index 100% rename from moveit_experimental/moveit_jog_arm/launch/cpp_interface_example.launch rename to moveit_ros/moveit_jog_arm/launch/cpp_interface_example.launch diff --git a/moveit_experimental/moveit_jog_arm/launch/spacenav_cpp.launch b/moveit_ros/moveit_jog_arm/launch/spacenav_cpp.launch similarity index 100% rename from moveit_experimental/moveit_jog_arm/launch/spacenav_cpp.launch rename to moveit_ros/moveit_jog_arm/launch/spacenav_cpp.launch diff --git a/moveit_experimental/moveit_jog_arm/launch/spacenav_teleop_tools.launch b/moveit_ros/moveit_jog_arm/launch/spacenav_teleop_tools.launch similarity index 100% rename from moveit_experimental/moveit_jog_arm/launch/spacenav_teleop_tools.launch rename to moveit_ros/moveit_jog_arm/launch/spacenav_teleop_tools.launch diff --git a/moveit_experimental/moveit_jog_arm/package.xml b/moveit_ros/moveit_jog_arm/package.xml similarity index 100% rename from moveit_experimental/moveit_jog_arm/package.xml rename to moveit_ros/moveit_jog_arm/package.xml diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check.cpp b/moveit_ros/moveit_jog_arm/src/collision_check.cpp similarity index 100% rename from moveit_experimental/moveit_jog_arm/src/collision_check.cpp rename to moveit_ros/moveit_jog_arm/src/collision_check.cpp diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_ros/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp similarity index 100% rename from moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp rename to moveit_ros/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp diff --git a/moveit_experimental/moveit_jog_arm/src/jog_arm.cpp b/moveit_ros/moveit_jog_arm/src/jog_arm.cpp similarity index 100% rename from moveit_experimental/moveit_jog_arm/src/jog_arm.cpp rename to moveit_ros/moveit_jog_arm/src/jog_arm.cpp diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_ros/moveit_jog_arm/src/jog_calcs.cpp similarity index 100% rename from moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp rename to moveit_ros/moveit_jog_arm/src/jog_calcs.cpp diff --git a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp b/moveit_ros/moveit_jog_arm/src/jog_server.cpp similarity index 100% rename from moveit_experimental/moveit_jog_arm/src/jog_server.cpp rename to moveit_ros/moveit_jog_arm/src/jog_server.cpp diff --git a/moveit_experimental/moveit_jog_arm/src/joint_state_subscriber.cpp b/moveit_ros/moveit_jog_arm/src/joint_state_subscriber.cpp similarity index 100% rename from moveit_experimental/moveit_jog_arm/src/joint_state_subscriber.cpp rename to moveit_ros/moveit_jog_arm/src/joint_state_subscriber.cpp diff --git a/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp b/moveit_ros/moveit_jog_arm/src/low_pass_filter.cpp similarity index 100% rename from moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp rename to moveit_ros/moveit_jog_arm/src/low_pass_filter.cpp diff --git a/moveit_experimental/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp b/moveit_ros/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp similarity index 100% rename from moveit_experimental/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp rename to moveit_ros/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp diff --git a/moveit_experimental/moveit_jog_arm/test/config/initial_position.yaml b/moveit_ros/moveit_jog_arm/test/config/initial_position.yaml similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/config/initial_position.yaml rename to moveit_ros/moveit_jog_arm/test/config/initial_position.yaml diff --git a/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml b/moveit_ros/moveit_jog_arm/test/config/jog_settings.yaml similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml rename to moveit_ros/moveit_jog_arm/test/config/jog_settings.yaml diff --git a/moveit_experimental/moveit_jog_arm/test/config/singular_position.yaml b/moveit_ros/moveit_jog_arm/test/config/singular_position.yaml similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/config/singular_position.yaml rename to moveit_ros/moveit_jog_arm/test/config/singular_position.yaml diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_ros/moveit_jog_arm/test/jog_cpp_interface_test.cpp similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp rename to moveit_ros/moveit_jog_arm/test/jog_cpp_interface_test.cpp diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test b/moveit_ros/moveit_jog_arm/test/jog_cpp_interface_test.test similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test rename to moveit_ros/moveit_jog_arm/test/jog_cpp_interface_test.test diff --git a/moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch b/moveit_ros/moveit_jog_arm/test/launch/common_test_setup.launch similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch rename to moveit_ros/moveit_jog_arm/test/launch/common_test_setup.launch diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test b/moveit_ros/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test rename to moveit_ros/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test b/moveit_ros/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test rename to moveit_ros/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test b/moveit_ros/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test rename to moveit_ros/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test b/moveit_ros/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test rename to moveit_ros/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/__init__.py b/moveit_ros/moveit_jog_arm/test/python_tests/__init__.py similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/python_tests/__init__.py rename to moveit_ros/moveit_jog_arm/test/python_tests/__init__.py diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py b/moveit_ros/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py rename to moveit_ros/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py b/moveit_ros/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py rename to moveit_ros/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py b/moveit_ros/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py rename to moveit_ros/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/util.py b/moveit_ros/moveit_jog_arm/test/python_tests/util.py similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/python_tests/util.py rename to moveit_ros/moveit_jog_arm/test/python_tests/util.py diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py b/moveit_ros/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py rename to moveit_ros/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py From 93e29c490f66671b42e14541bc981be96081665c Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Tue, 16 Jun 2020 09:33:35 -0500 Subject: [PATCH 356/612] Rename filepath moveit_ros/moveit_jog_arm -> moveit_ros/moveit_servo --- moveit_ros/{moveit_jog_arm => moveit_servo}/CMakeLists.txt | 0 moveit_ros/{moveit_jog_arm => moveit_servo}/README.md | 0 .../config/spacenav_via_teleop_tools.yaml | 0 .../config/ur_simulated_config.yaml | 0 .../include/moveit_servo}/collision_check.h | 0 .../include/moveit_servo}/jog_arm.h | 0 .../include/moveit_servo}/jog_arm_parameters.h | 0 .../include/moveit_servo}/jog_calcs.h | 0 .../include/moveit_servo}/joint_state_subscriber.h | 0 .../include/moveit_servo}/low_pass_filter.h | 0 .../include/moveit_servo}/make_shared_from_pool.h | 0 .../include/moveit_servo}/status_codes.h | 0 .../launch/cpp_interface_example.launch | 0 .../{moveit_jog_arm => moveit_servo}/launch/spacenav_cpp.launch | 0 .../launch/spacenav_teleop_tools.launch | 0 moveit_ros/{moveit_jog_arm => moveit_servo}/package.xml | 0 .../{moveit_jog_arm => moveit_servo}/src/collision_check.cpp | 0 .../src/cpp_interface_example/cpp_interface_example.cpp | 0 moveit_ros/{moveit_jog_arm => moveit_servo}/src/jog_arm.cpp | 0 moveit_ros/{moveit_jog_arm => moveit_servo}/src/jog_calcs.cpp | 0 moveit_ros/{moveit_jog_arm => moveit_servo}/src/jog_server.cpp | 0 .../src/joint_state_subscriber.cpp | 0 .../{moveit_jog_arm => moveit_servo}/src/low_pass_filter.cpp | 0 .../src/teleop_examples/spacenav_to_twist.cpp | 0 .../test/config/initial_position.yaml | 0 .../test/config/jog_settings.yaml | 0 .../test/config/singular_position.yaml | 0 .../test/jog_cpp_interface_test.cpp | 0 .../test/jog_cpp_interface_test.test | 0 .../test/launch/common_test_setup.launch | 0 .../test/launch/jog_arm_drift_dimensions_service_test.test | 0 .../test/launch/jog_arm_halt_msg_test.test | 0 .../test/launch/jog_arm_msg_reception_test.test | 0 .../test/launch/jog_arm_vel_accel_limits_test.test | 0 .../test/python_tests/__init__.py | 0 .../drift_dimensions_service/test_drift_dimensions_service.py | 0 .../test/python_tests/halt_msg/test_jog_arm_halt_msg.py | 0 .../test/python_tests/msg_reception/test_jog_arm_msg_reception.py | 0 .../{moveit_jog_arm => moveit_servo}/test/python_tests/util.py | 0 .../test/python_tests/vel_accel_limits/test_vel_accel_limits.py | 0 40 files changed, 0 insertions(+), 0 deletions(-) rename moveit_ros/{moveit_jog_arm => moveit_servo}/CMakeLists.txt (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/README.md (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/config/spacenav_via_teleop_tools.yaml (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/config/ur_simulated_config.yaml (100%) rename moveit_ros/{moveit_jog_arm/include/moveit_jog_arm => moveit_servo/include/moveit_servo}/collision_check.h (100%) rename moveit_ros/{moveit_jog_arm/include/moveit_jog_arm => moveit_servo/include/moveit_servo}/jog_arm.h (100%) rename moveit_ros/{moveit_jog_arm/include/moveit_jog_arm => moveit_servo/include/moveit_servo}/jog_arm_parameters.h (100%) rename moveit_ros/{moveit_jog_arm/include/moveit_jog_arm => moveit_servo/include/moveit_servo}/jog_calcs.h (100%) rename moveit_ros/{moveit_jog_arm/include/moveit_jog_arm => moveit_servo/include/moveit_servo}/joint_state_subscriber.h (100%) rename moveit_ros/{moveit_jog_arm/include/moveit_jog_arm => moveit_servo/include/moveit_servo}/low_pass_filter.h (100%) rename moveit_ros/{moveit_jog_arm/include/moveit_jog_arm => moveit_servo/include/moveit_servo}/make_shared_from_pool.h (100%) rename moveit_ros/{moveit_jog_arm/include/moveit_jog_arm => moveit_servo/include/moveit_servo}/status_codes.h (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/launch/cpp_interface_example.launch (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/launch/spacenav_cpp.launch (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/launch/spacenav_teleop_tools.launch (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/package.xml (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/src/collision_check.cpp (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/src/cpp_interface_example/cpp_interface_example.cpp (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/src/jog_arm.cpp (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/src/jog_calcs.cpp (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/src/jog_server.cpp (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/src/joint_state_subscriber.cpp (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/src/low_pass_filter.cpp (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/src/teleop_examples/spacenav_to_twist.cpp (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/config/initial_position.yaml (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/config/jog_settings.yaml (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/config/singular_position.yaml (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/jog_cpp_interface_test.cpp (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/jog_cpp_interface_test.test (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/launch/common_test_setup.launch (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/launch/jog_arm_drift_dimensions_service_test.test (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/launch/jog_arm_halt_msg_test.test (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/launch/jog_arm_msg_reception_test.test (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/launch/jog_arm_vel_accel_limits_test.test (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/python_tests/__init__.py (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/python_tests/halt_msg/test_jog_arm_halt_msg.py (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/python_tests/msg_reception/test_jog_arm_msg_reception.py (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/python_tests/util.py (100%) rename moveit_ros/{moveit_jog_arm => moveit_servo}/test/python_tests/vel_accel_limits/test_vel_accel_limits.py (100%) diff --git a/moveit_ros/moveit_jog_arm/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt similarity index 100% rename from moveit_ros/moveit_jog_arm/CMakeLists.txt rename to moveit_ros/moveit_servo/CMakeLists.txt diff --git a/moveit_ros/moveit_jog_arm/README.md b/moveit_ros/moveit_servo/README.md similarity index 100% rename from moveit_ros/moveit_jog_arm/README.md rename to moveit_ros/moveit_servo/README.md diff --git a/moveit_ros/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml b/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml similarity index 100% rename from moveit_ros/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml rename to moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml diff --git a/moveit_ros/moveit_jog_arm/config/ur_simulated_config.yaml b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml similarity index 100% rename from moveit_ros/moveit_jog_arm/config/ur_simulated_config.yaml rename to moveit_ros/moveit_servo/config/ur_simulated_config.yaml diff --git a/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h similarity index 100% rename from moveit_ros/moveit_jog_arm/include/moveit_jog_arm/collision_check.h rename to moveit_ros/moveit_servo/include/moveit_servo/collision_check.h diff --git a/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h b/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h similarity index 100% rename from moveit_ros/moveit_jog_arm/include/moveit_jog_arm/jog_arm.h rename to moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h diff --git a/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h b/moveit_ros/moveit_servo/include/moveit_servo/jog_arm_parameters.h similarity index 100% rename from moveit_ros/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h rename to moveit_ros/moveit_servo/include/moveit_servo/jog_arm_parameters.h diff --git a/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h similarity index 100% rename from moveit_ros/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h rename to moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h diff --git a/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/joint_state_subscriber.h b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h similarity index 100% rename from moveit_ros/moveit_jog_arm/include/moveit_jog_arm/joint_state_subscriber.h rename to moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h diff --git a/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h b/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h similarity index 100% rename from moveit_ros/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h rename to moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h diff --git a/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/make_shared_from_pool.h b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h similarity index 100% rename from moveit_ros/moveit_jog_arm/include/moveit_jog_arm/make_shared_from_pool.h rename to moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h diff --git a/moveit_ros/moveit_jog_arm/include/moveit_jog_arm/status_codes.h b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h similarity index 100% rename from moveit_ros/moveit_jog_arm/include/moveit_jog_arm/status_codes.h rename to moveit_ros/moveit_servo/include/moveit_servo/status_codes.h diff --git a/moveit_ros/moveit_jog_arm/launch/cpp_interface_example.launch b/moveit_ros/moveit_servo/launch/cpp_interface_example.launch similarity index 100% rename from moveit_ros/moveit_jog_arm/launch/cpp_interface_example.launch rename to moveit_ros/moveit_servo/launch/cpp_interface_example.launch diff --git a/moveit_ros/moveit_jog_arm/launch/spacenav_cpp.launch b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch similarity index 100% rename from moveit_ros/moveit_jog_arm/launch/spacenav_cpp.launch rename to moveit_ros/moveit_servo/launch/spacenav_cpp.launch diff --git a/moveit_ros/moveit_jog_arm/launch/spacenav_teleop_tools.launch b/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch similarity index 100% rename from moveit_ros/moveit_jog_arm/launch/spacenav_teleop_tools.launch rename to moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch diff --git a/moveit_ros/moveit_jog_arm/package.xml b/moveit_ros/moveit_servo/package.xml similarity index 100% rename from moveit_ros/moveit_jog_arm/package.xml rename to moveit_ros/moveit_servo/package.xml diff --git a/moveit_ros/moveit_jog_arm/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp similarity index 100% rename from moveit_ros/moveit_jog_arm/src/collision_check.cpp rename to moveit_ros/moveit_servo/src/collision_check.cpp diff --git a/moveit_ros/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp similarity index 100% rename from moveit_ros/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp rename to moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp diff --git a/moveit_ros/moveit_jog_arm/src/jog_arm.cpp b/moveit_ros/moveit_servo/src/jog_arm.cpp similarity index 100% rename from moveit_ros/moveit_jog_arm/src/jog_arm.cpp rename to moveit_ros/moveit_servo/src/jog_arm.cpp diff --git a/moveit_ros/moveit_jog_arm/src/jog_calcs.cpp b/moveit_ros/moveit_servo/src/jog_calcs.cpp similarity index 100% rename from moveit_ros/moveit_jog_arm/src/jog_calcs.cpp rename to moveit_ros/moveit_servo/src/jog_calcs.cpp diff --git a/moveit_ros/moveit_jog_arm/src/jog_server.cpp b/moveit_ros/moveit_servo/src/jog_server.cpp similarity index 100% rename from moveit_ros/moveit_jog_arm/src/jog_server.cpp rename to moveit_ros/moveit_servo/src/jog_server.cpp diff --git a/moveit_ros/moveit_jog_arm/src/joint_state_subscriber.cpp b/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp similarity index 100% rename from moveit_ros/moveit_jog_arm/src/joint_state_subscriber.cpp rename to moveit_ros/moveit_servo/src/joint_state_subscriber.cpp diff --git a/moveit_ros/moveit_jog_arm/src/low_pass_filter.cpp b/moveit_ros/moveit_servo/src/low_pass_filter.cpp similarity index 100% rename from moveit_ros/moveit_jog_arm/src/low_pass_filter.cpp rename to moveit_ros/moveit_servo/src/low_pass_filter.cpp diff --git a/moveit_ros/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp similarity index 100% rename from moveit_ros/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp rename to moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp diff --git a/moveit_ros/moveit_jog_arm/test/config/initial_position.yaml b/moveit_ros/moveit_servo/test/config/initial_position.yaml similarity index 100% rename from moveit_ros/moveit_jog_arm/test/config/initial_position.yaml rename to moveit_ros/moveit_servo/test/config/initial_position.yaml diff --git a/moveit_ros/moveit_jog_arm/test/config/jog_settings.yaml b/moveit_ros/moveit_servo/test/config/jog_settings.yaml similarity index 100% rename from moveit_ros/moveit_jog_arm/test/config/jog_settings.yaml rename to moveit_ros/moveit_servo/test/config/jog_settings.yaml diff --git a/moveit_ros/moveit_jog_arm/test/config/singular_position.yaml b/moveit_ros/moveit_servo/test/config/singular_position.yaml similarity index 100% rename from moveit_ros/moveit_jog_arm/test/config/singular_position.yaml rename to moveit_ros/moveit_servo/test/config/singular_position.yaml diff --git a/moveit_ros/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp similarity index 100% rename from moveit_ros/moveit_jog_arm/test/jog_cpp_interface_test.cpp rename to moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp diff --git a/moveit_ros/moveit_jog_arm/test/jog_cpp_interface_test.test b/moveit_ros/moveit_servo/test/jog_cpp_interface_test.test similarity index 100% rename from moveit_ros/moveit_jog_arm/test/jog_cpp_interface_test.test rename to moveit_ros/moveit_servo/test/jog_cpp_interface_test.test diff --git a/moveit_ros/moveit_jog_arm/test/launch/common_test_setup.launch b/moveit_ros/moveit_servo/test/launch/common_test_setup.launch similarity index 100% rename from moveit_ros/moveit_jog_arm/test/launch/common_test_setup.launch rename to moveit_ros/moveit_servo/test/launch/common_test_setup.launch diff --git a/moveit_ros/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test b/moveit_ros/moveit_servo/test/launch/jog_arm_drift_dimensions_service_test.test similarity index 100% rename from moveit_ros/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test rename to moveit_ros/moveit_servo/test/launch/jog_arm_drift_dimensions_service_test.test diff --git a/moveit_ros/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test b/moveit_ros/moveit_servo/test/launch/jog_arm_halt_msg_test.test similarity index 100% rename from moveit_ros/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test rename to moveit_ros/moveit_servo/test/launch/jog_arm_halt_msg_test.test diff --git a/moveit_ros/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test b/moveit_ros/moveit_servo/test/launch/jog_arm_msg_reception_test.test similarity index 100% rename from moveit_ros/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test rename to moveit_ros/moveit_servo/test/launch/jog_arm_msg_reception_test.test diff --git a/moveit_ros/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test b/moveit_ros/moveit_servo/test/launch/jog_arm_vel_accel_limits_test.test similarity index 100% rename from moveit_ros/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test rename to moveit_ros/moveit_servo/test/launch/jog_arm_vel_accel_limits_test.test diff --git a/moveit_ros/moveit_jog_arm/test/python_tests/__init__.py b/moveit_ros/moveit_servo/test/python_tests/__init__.py similarity index 100% rename from moveit_ros/moveit_jog_arm/test/python_tests/__init__.py rename to moveit_ros/moveit_servo/test/python_tests/__init__.py diff --git a/moveit_ros/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py b/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py similarity index 100% rename from moveit_ros/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py rename to moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py diff --git a/moveit_ros/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py b/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py similarity index 100% rename from moveit_ros/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py rename to moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py diff --git a/moveit_ros/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py b/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py similarity index 100% rename from moveit_ros/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py rename to moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py diff --git a/moveit_ros/moveit_jog_arm/test/python_tests/util.py b/moveit_ros/moveit_servo/test/python_tests/util.py similarity index 100% rename from moveit_ros/moveit_jog_arm/test/python_tests/util.py rename to moveit_ros/moveit_servo/test/python_tests/util.py diff --git a/moveit_ros/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py b/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py similarity index 100% rename from moveit_ros/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py rename to moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py From c76997881e4998d557a291b2928674656a03c76d Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Tue, 16 Jun 2020 11:17:06 -0500 Subject: [PATCH 357/612] Rename internal namespaces and package/project name to moveit_servo --- moveit_ros/moveit_servo/CMakeLists.txt | 2 +- moveit_ros/moveit_servo/README.md | 8 +-- .../include/moveit_servo/collision_check.h | 18 +++--- .../include/moveit_servo/jog_arm.h | 22 +++---- .../include/moveit_servo/jog_arm_parameters.h | 10 ++-- .../include/moveit_servo/jog_calcs.h | 21 ++++--- .../moveit_servo/joint_state_subscriber.h | 8 +-- .../include/moveit_servo/low_pass_filter.h | 6 +- .../moveit_servo/make_shared_from_pool.h | 2 +- .../include/moveit_servo/status_codes.h | 18 +++--- .../launch/cpp_interface_example.launch | 4 +- .../moveit_servo/launch/spacenav_cpp.launch | 6 +- .../launch/spacenav_teleop_tools.launch | 6 +- moveit_ros/moveit_servo/package.xml | 4 +- .../moveit_servo/src/collision_check.cpp | 12 ++-- .../cpp_interface_example.cpp | 20 +++---- moveit_ros/moveit_servo/src/jog_arm.cpp | 10 ++-- moveit_ros/moveit_servo/src/jog_calcs.cpp | 12 ++-- moveit_ros/moveit_servo/src/jog_server.cpp | 6 +- .../src/joint_state_subscriber.cpp | 8 +-- .../moveit_servo/src/low_pass_filter.cpp | 10 ++-- .../src/teleop_examples/spacenav_to_twist.cpp | 8 +-- .../test/jog_cpp_interface_test.cpp | 59 +++++++++---------- .../test/jog_cpp_interface_test.test | 6 +- .../test/launch/common_test_setup.launch | 6 +- ...jog_arm_drift_dimensions_service_test.test | 6 +- .../test/launch/jog_arm_halt_msg_test.test | 6 +- .../launch/jog_arm_msg_reception_test.test | 6 +- .../launch/jog_arm_vel_accel_limits_test.test | 6 +- .../halt_msg/test_jog_arm_halt_msg.py | 2 +- .../test_jog_arm_msg_reception.py | 8 +-- .../vel_accel_limits/test_vel_accel_limits.py | 2 +- 32 files changed, 163 insertions(+), 165 deletions(-) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 4b7ccda0aa..2599b34458 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.1.3) -project(moveit_jog_arm) +project(moveit_servo) if(NOT "${CMAKE_CXX_STANDARD}") set(CMAKE_CXX_STANDARD 14) diff --git a/moveit_ros/moveit_servo/README.md b/moveit_ros/moveit_servo/README.md index 93c7c036d9..6b55fb804d 100644 --- a/moveit_ros/moveit_servo/README.md +++ b/moveit_ros/moveit_servo/README.md @@ -1,4 +1,4 @@ -## Jog Arm +## Moveit Servo #### Quick Start Guide for UR5 example @@ -30,9 +30,9 @@ stop_controllers: strictness: 2" ``` -Launch the jog node. This example uses commands from a SpaceNavigator joystick-like device: +Launch the servo node. This example uses commands from a SpaceNavigator joystick-like device: - roslaunch moveit_jog_arm spacenav_cpp.launch + roslaunch moveit_servo spacenav_cpp.launch If you dont have a SpaceNavigator, send commands like this: @@ -53,6 +53,6 @@ If you see a warning about "close to singularity", try changing the direction of #### Running Tests -Run tests from the jog\_arm folder: +Run tests from the moveit\_servo folder: catkin run_tests --no-deps --this \ No newline at end of file diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index 8055e7f762..6abdfc8a1b 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -1,6 +1,6 @@ /******************************************************************************* * Title : collision_check.h - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 1/11/2019 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson * @@ -45,11 +45,11 @@ #include #include -#include -#include -#include +#include +#include +#include -namespace moveit_jog_arm +namespace moveit_servo { enum CollisionCheckType { @@ -61,11 +61,11 @@ class CollisionCheck { public: /** \brief Constructor - * \param parameters: common settings of jog_arm + * \param parameters: common settings of moveit_servo * \param planning_scene_monitor: PSM should have scene monitor and state monitor * already started when passed into this class */ - CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, + CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::shared_ptr& joint_state_subscriber); @@ -84,7 +84,7 @@ class CollisionCheck ros::NodeHandle nh_; // Parameters from yaml - const JogArmParameters& parameters_; + const ServoParameters& parameters_; // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; @@ -128,4 +128,4 @@ class CollisionCheck ros::Publisher collision_velocity_scale_pub_; ros::Subscriber worst_case_stop_time_sub_; }; -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h b/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h index 0949fcb71b..4beec792d6 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h @@ -1,6 +1,6 @@ /******************************************************************************* - * Title : jog_arm.cpp - * Project : moveit_jog_arm + * Title : jog_arm.h + * Project : moveit_servo * Created : 3/9/2017 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson * @@ -40,15 +40,15 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include -namespace moveit_jog_arm +namespace moveit_servo { /** - * Class JogArm - Jacobian based robot control with collision avoidance. + * Class Servo - Jacobian based robot control with collision avoidance. */ class JogArm { @@ -76,7 +76,7 @@ class JogArm bool getCommandFrameTransform(Eigen::Isometry3d& transform); /** \brief Get the parameters used by jog arm. */ - const JogArmParameters& getParameters() const; + const ServoParameters& getParameters() const; /** \brief Get the latest joint state. */ sensor_msgs::JointStateConstPtr getLatestJointState() const; @@ -90,7 +90,7 @@ class JogArm planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; // Store the parameters that were read from ROS server - JogArmParameters parameters_; + ServoParameters parameters_; std::shared_ptr joint_state_subscriber_; std::unique_ptr jog_calcs_; @@ -100,4 +100,4 @@ class JogArm // JogArmPtr using alias using JogArmPtr = std::shared_ptr; -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/jog_arm_parameters.h b/moveit_ros/moveit_servo/include/moveit_servo/jog_arm_parameters.h index a9e2cfa245..2fcdea7c5c 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/jog_arm_parameters.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/jog_arm_parameters.h @@ -1,6 +1,6 @@ /******************************************************************************* - * Title : jog_arm_parameters.h - * Project : moveit_jog_arm + * Title : servo_parameters.h + * Project : moveit_servo * Created : 1/11/2019 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson * @@ -38,13 +38,13 @@ #pragma once -namespace moveit_jog_arm +namespace moveit_servo { // Size of queues used in ros pub/sub/service constexpr size_t ROS_QUEUE_SIZE = 2; // ROS params to be read. See the yaml file in /config for a description of each. -struct JogArmParameters +struct ServoParameters { std::string move_group_name; std::string joint_topic; @@ -80,4 +80,4 @@ struct JogArmParameters double min_allowable_collision_distance; }; -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h index 9c87ccd5e2..c2bb494211 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h @@ -1,6 +1,6 @@ /******************************************************************************* * Title : jog_calcs.h - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 1/11/2019 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson * @@ -53,19 +53,18 @@ #include #include -// moveit_jog_arm -#include -#include -#include -#include -#include +// moveit_servo +#include +#include +#include +#include -namespace moveit_jog_arm +namespace moveit_servo { class JogCalcs { public: - JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, + JogCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::shared_ptr& joint_state_subscriber); @@ -194,7 +193,7 @@ class JogCalcs ros::NodeHandle nh_; // Parameters from yaml - const JogArmParameters& parameters_; + const ServoParameters& parameters_; // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; @@ -285,4 +284,4 @@ class JogCalcs bool latest_nonzero_twist_stamped_ = false; bool latest_nonzero_joint_jog_ = false; }; -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h index e7b74f2911..9346ba7ae8 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h @@ -1,6 +1,6 @@ /******************************************************************************* * Title : joint_state_subscriber.h - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 06/11/2020 * Author : Tyler Weaver * @@ -43,9 +43,9 @@ #include #include -#include "jog_arm_parameters.h" +#include -namespace moveit_jog_arm +namespace moveit_servo { class JointStateSubscriber { @@ -66,4 +66,4 @@ class JointStateSubscriber mutable std::mutex joint_state_mutex_; sensor_msgs::JointStateConstPtr latest_joint_state_; }; -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h b/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h index 6d5d6ba2b0..99d9934224 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h @@ -1,6 +1,6 @@ /******************************************************************************* * Title : low_pass_filter.h - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 1/11/2019 * Author : Andy Zelenak * @@ -40,7 +40,7 @@ #include -namespace moveit_jog_arm +namespace moveit_servo { /** * Class LowPassFilter - Filter a signal to soften jerks. @@ -66,4 +66,4 @@ class LowPassFilter const double scale_term_; const double feedback_term_; }; -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h index ca4243625e..73fd73df8d 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h @@ -1,6 +1,6 @@ /******************************************************************************* * Title : make_shared_from_pool.h - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 1/11/2019 * Author : Tyler Weaver * diff --git a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h index 7f930f6db6..bdeca7dc4f 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h @@ -1,6 +1,6 @@ /******************************************************************************* * Title : status_codes.h - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 2/25/2019 * Author : Andy Zelenak * @@ -41,7 +41,7 @@ #include #include -namespace moveit_jog_arm +namespace moveit_servo { enum StatusCode : int8_t { @@ -56,10 +56,10 @@ enum StatusCode : int8_t const std::unordered_map JOG_ARM_STATUS_CODE_MAP({ { INVALID, "Invalid" }, - { NO_WARNING, "No warnings" }, - { DECELERATE_FOR_SINGULARITY, "Close to a singularity, decelerating" }, - { HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, - { DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, - { HALT_FOR_COLLISION, "Collision detected, emergency stop" }, - { JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }); -} // namespace moveit_jog_arm + { NO_WARNING, "No warnings" }, + { DECELERATE_FOR_SINGULARITY, "Close to a singularity, decelerating" }, + { HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, + { DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, + { HALT_FOR_COLLISION, "Collision detected, emergency stop" }, + { JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }); +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/launch/cpp_interface_example.launch b/moveit_ros/moveit_servo/launch/cpp_interface_example.launch index bc347ec658..ab1746a78a 100644 --- a/moveit_ros/moveit_servo/launch/cpp_interface_example.launch +++ b/moveit_ros/moveit_servo/launch/cpp_interface_example.launch @@ -1,9 +1,9 @@ - + - + diff --git a/moveit_ros/moveit_servo/launch/spacenav_cpp.launch b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch index 9aa9f6b0c0..4346e17a09 100644 --- a/moveit_ros/moveit_servo/launch/spacenav_cpp.launch +++ b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch @@ -10,11 +10,11 @@ - + - + - + diff --git a/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch b/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch index 5b04243434..238ce8cc03 100644 --- a/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch +++ b/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch @@ -9,14 +9,14 @@ - + - + - + diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index 96d38fe627..1e8ded83ff 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -1,9 +1,9 @@ - moveit_jog_arm + moveit_servo 1.1.0 - Provides real-time manipulator Cartesian jogging. + Provides real-time manipulator Cartesian and joint jogging. Blake Anderson Andy Zelenak diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp index 18f9ff5a6a..91adabf530 100644 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ b/moveit_ros/moveit_servo/src/collision_check.cpp @@ -32,25 +32,25 @@ *******************************************************************************/ /* Title : collision_check.cpp - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 1/11/2019 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson */ #include -#include -#include +#include +#include static const char LOGNAME[] = "collision_check"; static const double MIN_RECOMMENDED_COLLISION_RATE = 10; constexpr double EPSILON = 1e-6; // For very small numeric comparisons constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30; // Seconds to throttle logs inside loops -namespace moveit_jog_arm +namespace moveit_servo { // Constructor for the class that handles collision checking -CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters, +CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::shared_ptr& joint_state_subscriber) : nh_(nh) @@ -213,4 +213,4 @@ void CollisionCheck::setPaused(bool paused) paused_ = paused; } -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp index 77dee6b253..b79e5f4277 100644 --- a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp @@ -1,6 +1,6 @@ /******************************************************************************* * Title : cpp_interface_example.cpp - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 11/20/2019 * Author : Andy Zelenak * @@ -38,13 +38,13 @@ #include -#include -#include -#include +#include +#include +#include static const std::string LOGNAME = "cpp_interface_example"; -// Class for monitoring status of jog_arm +// Class for monitoring status of moveit_servo class StatusMonitor { public: @@ -56,15 +56,15 @@ class StatusMonitor private: void statusCB(const std_msgs::Int8ConstPtr& msg) { - moveit_jog_arm::StatusCode latest_status = static_cast(msg->data); + moveit_servo::StatusCode latest_status = static_cast(msg->data); if (latest_status != status_) { status_ = latest_status; - const auto& status_str = moveit_jog_arm::JOG_ARM_STATUS_CODE_MAP.at(status_); - ROS_INFO_STREAM_NAMED(LOGNAME, "Jogger status: " << status_str); + const auto& status_str = moveit_servo::JOG_ARM_STATUS_CODE_MAP.at(status_); + ROS_INFO_STREAM_NAMED(LOGNAME, "Servo status: " << status_str); } } - moveit_jog_arm::StatusCode status_ = moveit_jog_arm::StatusCode::INVALID; + moveit_servo::StatusCode status_ = moveit_servo::StatusCode::INVALID; ros::Subscriber sub_; }; @@ -96,7 +96,7 @@ int main(int argc, char** argv) planning_scene_monitor->startStateMonitor(); // Run the jogging C++ interface in a new timer to ensure a constant outgoing message rate. - moveit_jog_arm::JogArm jog_arm(nh, planning_scene_monitor); + moveit_servo::JogArm jog_arm(nh, planning_scene_monitor); jog_arm.start(); // Subscribe to jog_arm status (and log it when it changes) diff --git a/moveit_ros/moveit_servo/src/jog_arm.cpp b/moveit_ros/moveit_servo/src/jog_arm.cpp index d7ef39d9b9..8eb3e3fe29 100644 --- a/moveit_ros/moveit_servo/src/jog_arm.cpp +++ b/moveit_ros/moveit_servo/src/jog_arm.cpp @@ -32,18 +32,18 @@ *******************************************************************************/ /* Title : jog_arm.cpp - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 3/9/2017 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson */ #include -#include +#include static const std::string LOGNAME = "jog_arm"; -namespace moveit_jog_arm +namespace moveit_servo { JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : nh_(nh), planning_scene_monitor_(planning_scene_monitor) @@ -308,7 +308,7 @@ bool JogArm::getCommandFrameTransform(Eigen::Isometry3d& transform) return jog_calcs_->getCommandFrameTransform(transform); } -const JogArmParameters& JogArm::getParameters() const +const ServoParameters& JogArm::getParameters() const { return parameters_; } @@ -318,4 +318,4 @@ sensor_msgs::JointStateConstPtr JogArm::getLatestJointState() const return joint_state_subscriber_->getLatest(); } -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/jog_calcs.cpp b/moveit_ros/moveit_servo/src/jog_calcs.cpp index fe27c475f7..f5a1a5d3cb 100644 --- a/moveit_ros/moveit_servo/src/jog_calcs.cpp +++ b/moveit_ros/moveit_servo/src/jog_calcs.cpp @@ -32,7 +32,7 @@ *******************************************************************************/ /* Title : jog_calcs.cpp - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 1/11/2019 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson */ @@ -40,13 +40,13 @@ #include #include -#include -#include +#include +#include static const std::string LOGNAME = "jog_calcs"; constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30; // Seconds to throttle logs inside loops -namespace moveit_jog_arm +namespace moveit_servo { namespace { @@ -70,7 +70,7 @@ bool isNonZero(const control_msgs::JointJog& msg) } // namespace // Constructor for the class that handles jogging calculations -JogCalcs::JogCalcs(ros::NodeHandle& nh, const JogArmParameters& parameters, +JogCalcs::JogCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::shared_ptr& joint_state_subscriber) : nh_(nh) @@ -1049,4 +1049,4 @@ void JogCalcs::setPaused(bool paused) paused_ = paused; } -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/jog_server.cpp b/moveit_ros/moveit_servo/src/jog_server.cpp index e33f2907a6..8ad03d039d 100644 --- a/moveit_ros/moveit_servo/src/jog_server.cpp +++ b/moveit_ros/moveit_servo/src/jog_server.cpp @@ -32,12 +32,12 @@ *******************************************************************************/ /* Title : jog_server.cpp - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 12/31/2018 * Author : Andy Zelenak */ -#include +#include namespace { @@ -71,7 +71,7 @@ int main(int argc, char** argv) planning_scene_monitor->startStateMonitor(); // Create the jog server - moveit_jog_arm::JogArm jog_arm(nh, planning_scene_monitor); + moveit_servo::JogArm jog_arm(nh, planning_scene_monitor); // Start the jog server (runs in the ros spinner) jog_arm.start(); diff --git a/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp b/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp index b9cfb12ebf..2ed780b650 100644 --- a/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp +++ b/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp @@ -32,14 +32,14 @@ *******************************************************************************/ /* Title : joint_state_subscriber.cpp - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 06/11/2020 * Author : Tyler Weaver */ -#include +#include -namespace moveit_jog_arm +namespace moveit_servo { constexpr char LOGNAME[] = "joint_state_subscriber"; @@ -67,4 +67,4 @@ void JointStateSubscriber::jointStateCB(const sensor_msgs::JointStateConstPtr& m latest_joint_state_ = msg; } -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/low_pass_filter.cpp b/moveit_ros/moveit_servo/src/low_pass_filter.cpp index 4c74829bbc..7013b3b4c9 100644 --- a/moveit_ros/moveit_servo/src/low_pass_filter.cpp +++ b/moveit_ros/moveit_servo/src/low_pass_filter.cpp @@ -32,17 +32,17 @@ *******************************************************************************/ /* Title : low_pass_filter.cpp - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 1/11/2019 * Author : Andy Zelenak */ -#include +#include #include #include #include -namespace moveit_jog_arm +namespace moveit_servo { namespace { @@ -57,7 +57,7 @@ LowPassFilter::LowPassFilter(double low_pass_filter_coeff) , feedback_term_(1. - low_pass_filter_coeff) { // guarantee this doesn't change because the logic below depends on this length implicity - static_assert(LowPassFilter::FILTER_LENGTH == 2, "moveit_jog_arm::LowPassFilter::FILTER_LENGTH should be 2"); + static_assert(LowPassFilter::FILTER_LENGTH == 2, "moveit_servo::LowPassFilter::FILTER_LENGTH should be 2"); ROS_ASSERT_MSG(!std::isinf(feedback_term_), "%s: outputs from filter will be inf because feedback term is inf", LOGNAME); @@ -98,4 +98,4 @@ double LowPassFilter::filter(double new_measurement) return new_filtered_measurement; } -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp index c7f853843d..2b75bf87c9 100644 --- a/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp +++ b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp @@ -1,6 +1,6 @@ /******************************************************************************* * Title : spacenav_to_twist.cpp - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 1/11/2019 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson * @@ -41,7 +41,7 @@ #include "ros/ros.h" #include "sensor_msgs/Joy.h" -namespace moveit_jog_arm +namespace moveit_servo { static const int NUM_SPINNERS = 1; static const int QUEUE_LENGTH = 1; @@ -95,13 +95,13 @@ class SpaceNavToTwist ros::Publisher twist_pub_, joint_delta_pub_; ros::AsyncSpinner spinner_; }; -} // namespace moveit_jog_arm +} // namespace moveit_servo int main(int argc, char** argv) { ros::init(argc, argv, "spacenav_to_twist"); - moveit_jog_arm::SpaceNavToTwist to_twist; + moveit_servo::SpaceNavToTwist to_twist; return 0; } \ No newline at end of file diff --git a/moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp index 247d905e66..bc954976f5 100644 --- a/moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp +++ b/moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp @@ -33,7 +33,7 @@ *********************************************************************/ /* Author: Dave Coleman and Tyler Weaver - Desc: Test for the C++ interface to JogArm + Desc: Test for the C++ interface to moveit_servo */ // C++ @@ -45,15 +45,15 @@ // Testing #include -// Jog Arm -#include -#include +// Servo +#include +#include static const std::string LOGNAME = "jog_cpp_interface_test"; -namespace moveit_jog_arm +namespace moveit_servo { -class JogArmFixture : public ::testing::Test +class ServoFixture : public ::testing::Test { public: void SetUp() override @@ -67,8 +67,8 @@ class JogArmFixture : public ::testing::Test planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, false /* skip octomap monitor */); - // Create jog_arm - jog_arm_ = std::make_shared(nh_, planning_scene_monitor_); + // Create moveit_servo + servo_ = std::make_shared(nh_, planning_scene_monitor_); } void TearDown() override { @@ -76,45 +76,44 @@ class JogArmFixture : public ::testing::Test bool waitForFirstStatus() { - auto msg = - ros::topic::waitForMessage(jog_arm_->getParameters().status_topic, nh_, ros::Duration(15)); + auto msg = ros::topic::waitForMessage(servo_->getParameters().status_topic, nh_, ros::Duration(15)); return static_cast(msg); } protected: ros::NodeHandle nh_; planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - moveit_jog_arm::JogArmPtr jog_arm_; -}; // class JogArmFixture + moveit_servo::ServoPtr servo_; +}; // class ServoFixture -TEST_F(JogArmFixture, StartStopTest) +TEST_F(ServoFixture, StartStopTest) { - jog_arm_->start(); + servo_->start(); EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; - jog_arm_->stop(); + servo_->stop(); ros::Duration(1.0).sleep(); // Start and stop again - jog_arm_->start(); + servo_->start(); EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; - jog_arm_->stop(); + servo_->stop(); } -TEST_F(JogArmFixture, SendTwistStampedTest) +TEST_F(ServoFixture, SendTwistStampedTest) { - jog_arm_->start(); + servo_->start(); EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; - auto parameters = jog_arm_->getParameters(); + auto parameters = servo_->getParameters(); - // count trajectory messages sent by jog_arm + // count trajectory messages sent by servo size_t received_count = 0; boost::function traj_callback = [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { received_count++; }; auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); - // Create publisher to send jog_arm commands + // Create publisher to send servo commands auto twist_stamped_pub = nh_.advertise(parameters.cartesian_command_in_topic, 1); constexpr double test_duration = 1.0; @@ -138,23 +137,23 @@ TEST_F(JogArmFixture, SendTwistStampedTest) EXPECT_GT(received_count, num_commands - 20); EXPECT_LT(received_count, num_commands + 20); - jog_arm_->stop(); + servo_->stop(); } -TEST_F(JogArmFixture, SendJointJogTest) +TEST_F(ServoFixture, SendJointJogTest) { - jog_arm_->start(); + servo_->start(); EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; - auto parameters = jog_arm_->getParameters(); + auto parameters = servo_->getParameters(); - // count trajectory messages sent by jog_arm + // count trajectory messages sent by servo size_t received_count = 0; boost::function traj_callback = [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { received_count++; }; auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); - // Create publisher to send jog_arm commands + // Create publisher to send servo commands auto joint_jog_pub = nh_.advertise(parameters.joint_command_in_topic, 1); constexpr double test_duration = 1.0; @@ -178,10 +177,10 @@ TEST_F(JogArmFixture, SendJointJogTest) EXPECT_GT(received_count, num_commands - 20); EXPECT_LT(received_count, num_commands + 20); - jog_arm_->stop(); + servo_->stop(); } -} // namespace moveit_jog_arm +} // namespace moveit_servo int main(int argc, char** argv) { diff --git a/moveit_ros/moveit_servo/test/jog_cpp_interface_test.test b/moveit_ros/moveit_servo/test/jog_cpp_interface_test.test index b487ef2461..829a33e117 100644 --- a/moveit_ros/moveit_servo/test/jog_cpp_interface_test.test +++ b/moveit_ros/moveit_servo/test/jog_cpp_interface_test.test @@ -7,12 +7,12 @@ - + - + - + diff --git a/moveit_ros/moveit_servo/test/launch/common_test_setup.launch b/moveit_ros/moveit_servo/test/launch/common_test_setup.launch index f406c10bdb..f8f0326693 100644 --- a/moveit_ros/moveit_servo/test/launch/common_test_setup.launch +++ b/moveit_ros/moveit_servo/test/launch/common_test_setup.launch @@ -13,13 +13,13 @@ - + - + - + diff --git a/moveit_ros/moveit_servo/test/launch/jog_arm_drift_dimensions_service_test.test b/moveit_ros/moveit_servo/test/launch/jog_arm_drift_dimensions_service_test.test index 33669dc061..145255f44e 100644 --- a/moveit_ros/moveit_servo/test/launch/jog_arm_drift_dimensions_service_test.test +++ b/moveit_ros/moveit_servo/test/launch/jog_arm_drift_dimensions_service_test.test @@ -3,13 +3,13 @@ - + - + - + diff --git a/moveit_ros/moveit_servo/test/launch/jog_arm_halt_msg_test.test b/moveit_ros/moveit_servo/test/launch/jog_arm_halt_msg_test.test index 0ff461bb8c..efc3c1ae2f 100644 --- a/moveit_ros/moveit_servo/test/launch/jog_arm_halt_msg_test.test +++ b/moveit_ros/moveit_servo/test/launch/jog_arm_halt_msg_test.test @@ -3,13 +3,13 @@ - + - + - + diff --git a/moveit_ros/moveit_servo/test/launch/jog_arm_msg_reception_test.test b/moveit_ros/moveit_servo/test/launch/jog_arm_msg_reception_test.test index 4761fb2ffb..b30ee1a8b7 100644 --- a/moveit_ros/moveit_servo/test/launch/jog_arm_msg_reception_test.test +++ b/moveit_ros/moveit_servo/test/launch/jog_arm_msg_reception_test.test @@ -3,13 +3,13 @@ - + - + - + diff --git a/moveit_ros/moveit_servo/test/launch/jog_arm_vel_accel_limits_test.test b/moveit_ros/moveit_servo/test/launch/jog_arm_vel_accel_limits_test.test index 6a35a19e08..e6e3707bd7 100644 --- a/moveit_ros/moveit_servo/test/launch/jog_arm_vel_accel_limits_test.test +++ b/moveit_ros/moveit_servo/test/launch/jog_arm_vel_accel_limits_test.test @@ -3,13 +3,13 @@ - + - + - + diff --git a/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py b/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py index afaf0f972e..aad950df3f 100755 --- a/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py +++ b/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py @@ -17,7 +17,7 @@ # The jogger should halt and publish a warning. # Listen for a warning message from the jogger. # This can be run as part of a pytest, or like a normal ROS executable: -# rosrun moveit_jog_arm test_jog_arm_halt_msg.py +# rosrun moveit_servo test_jog_arm_halt_msg.py CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' diff --git a/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py b/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py index 510966a114..9c74599f8f 100755 --- a/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py +++ b/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py @@ -15,7 +15,7 @@ # Test that the jogger publishes controller commands when it receives Cartesian or joint commands. # This can be run as part of a pytest, or like a normal ROS executable: -# rosrun moveit_jog_arm test_jog_arm_integration.py +# rosrun moveit_servo test_jog_arm_integration.py JOINT_JOG_COMMAND_TOPIC = 'jog_server/joint_delta_jog_cmds' CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' @@ -77,7 +77,7 @@ def test_jog_arm_cartesian_command(node): # This nonzero command should produce jogging output # A subscriber in a different timer fills `received` TEST_DURATION = 1 - PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file + PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from servo config file # Send a command to start the jogger cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) @@ -105,7 +105,7 @@ def test_jog_arm_joint_command(node): joint_cmd = JointJogCmd() TEST_DURATION = 1 - PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file + PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from servo config file velocities = [0.1] # Send a command to start the jogger @@ -124,6 +124,6 @@ def test_jog_arm_joint_command(node): if __name__ == '__main__': node = node() - time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for jog_arm server to init + time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for servo server to init test_jog_arm_cartesian_command(node) test_jog_arm_joint_command(node) diff --git a/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py b/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py index 3bf49883fb..c6a08de794 100755 --- a/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py +++ b/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py @@ -14,7 +14,7 @@ # Test that commands that are too fast are caught and flagged # This can be run as part of a pytest, or like a normal ROS executable: -# rosrun moveit_jog_arm test_vel_accel_limits.py +# rosrun moveit_servo test_vel_accel_limits.py JOINT_JOG_COMMAND_TOPIC = 'jog_server/joint_delta_jog_cmds' From 349624b91f1c90f4062608a650bea210d3d59ba0 Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Tue, 16 Jun 2020 11:41:10 -0500 Subject: [PATCH 358/612] Rename jog_arm_parameters.h file to servo_parameters.h and fix relevant includes --- moveit_ros/moveit_servo/include/moveit_servo/collision_check.h | 2 +- moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h | 2 +- moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h | 2 +- .../moveit_servo/include/moveit_servo/joint_state_subscriber.h | 2 +- .../moveit_servo/{jog_arm_parameters.h => servo_parameters.h} | 0 5 files changed, 4 insertions(+), 4 deletions(-) rename moveit_ros/moveit_servo/include/moveit_servo/{jog_arm_parameters.h => servo_parameters.h} (100%) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index 6abdfc8a1b..cfd303efed 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -45,7 +45,7 @@ #include #include -#include +#include #include #include diff --git a/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h b/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h index 4beec792d6..e4ee873014 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include diff --git a/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h index c2bb494211..a32f6ae50c 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h @@ -54,7 +54,7 @@ #include // moveit_servo -#include +#include #include #include #include diff --git a/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h index 9346ba7ae8..48d362123a 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h @@ -43,7 +43,7 @@ #include #include -#include +#include namespace moveit_servo { diff --git a/moveit_ros/moveit_servo/include/moveit_servo/jog_arm_parameters.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h similarity index 100% rename from moveit_ros/moveit_servo/include/moveit_servo/jog_arm_parameters.h rename to moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h From 325b2e5629136d05e25a9d88ca43785d7ffe0ead Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Tue, 16 Jun 2020 11:49:31 -0500 Subject: [PATCH 359/612] Internal rename interface class JogArm to Servo --- .../include/moveit_servo/jog_arm.h | 12 +++++------ .../cpp_interface_example.cpp | 16 +++++++-------- moveit_ros/moveit_servo/src/jog_arm.cpp | 20 +++++++++---------- moveit_ros/moveit_servo/src/jog_server.cpp | 6 +++--- 4 files changed, 27 insertions(+), 27 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h b/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h index e4ee873014..d1bca753d0 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h @@ -1,5 +1,5 @@ /******************************************************************************* - * Title : jog_arm.h + * Title : servo.h * Project : moveit_servo * Created : 3/9/2017 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson @@ -50,12 +50,12 @@ namespace moveit_servo /** * Class Servo - Jacobian based robot control with collision avoidance. */ -class JogArm +class Servo { public: - JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - ~JogArm(); + ~Servo(); /** \brief start jog arm */ void start(); @@ -97,7 +97,7 @@ class JogArm std::unique_ptr collision_checker_; }; -// JogArmPtr using alias -using JogArmPtr = std::shared_ptr; +// ServoPtr using alias +using ServoPtr = std::shared_ptr; } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp index b79e5f4277..1292357129 100644 --- a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp @@ -96,16 +96,16 @@ int main(int argc, char** argv) planning_scene_monitor->startStateMonitor(); // Run the jogging C++ interface in a new timer to ensure a constant outgoing message rate. - moveit_servo::JogArm jog_arm(nh, planning_scene_monitor); - jog_arm.start(); + moveit_servo::Servo servo(nh, planning_scene_monitor); + servo.start(); - // Subscribe to jog_arm status (and log it when it changes) - StatusMonitor status_monitor(nh, jog_arm.getParameters().status_topic); + // Subscribe to servo status (and log it when it changes) + StatusMonitor status_monitor(nh, servo.getParameters().status_topic); - // Create publishers to send jog_arm commands + // Create publishers to send servo commands auto twist_stamped_pub = - nh.advertise(jog_arm.getParameters().cartesian_command_in_topic, 1); - auto joint_jog_pub = nh.advertise(jog_arm.getParameters().joint_command_in_topic, 1); + nh.advertise(servo.getParameters().cartesian_command_in_topic, 1); + auto joint_jog_pub = nh.advertise(servo.getParameters().joint_command_in_topic, 1); ros::Rate cmd_rate(100); uint num_commands = 0; @@ -150,6 +150,6 @@ int main(int argc, char** argv) ++num_commands; } - jog_arm.stop(); + servo.stop(); return 0; } diff --git a/moveit_ros/moveit_servo/src/jog_arm.cpp b/moveit_ros/moveit_servo/src/jog_arm.cpp index 8eb3e3fe29..66a845f553 100644 --- a/moveit_ros/moveit_servo/src/jog_arm.cpp +++ b/moveit_ros/moveit_servo/src/jog_arm.cpp @@ -31,7 +31,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -/* Title : jog_arm.cpp +/* Title : servo.cpp * Project : moveit_servo * Created : 3/9/2017 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson @@ -45,7 +45,7 @@ static const std::string LOGNAME = "jog_arm"; namespace moveit_servo { -JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) +Servo::Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : nh_(nh), planning_scene_monitor_(planning_scene_monitor) { // Read ROS parameters, typically from YAML file @@ -61,7 +61,7 @@ JogArm::JogArm(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneM } // Read ROS parameters, typically from YAML file -bool JogArm::readParameters() +bool Servo::readParameters() { std::size_t error = 0; @@ -274,7 +274,7 @@ bool JogArm::readParameters() return true; } -void JogArm::start() +void Servo::start() { setPaused(false); @@ -286,34 +286,34 @@ void JogArm::start() collision_checker_->start(); } -void JogArm::stop() +void Servo::stop() { jog_calcs_->stop(); collision_checker_->stop(); } -JogArm::~JogArm() +Servo::~Servo() { stop(); } -void JogArm::setPaused(bool paused) +void Servo::setPaused(bool paused) { jog_calcs_->setPaused(paused); collision_checker_->setPaused(paused); } -bool JogArm::getCommandFrameTransform(Eigen::Isometry3d& transform) +bool Servo::getCommandFrameTransform(Eigen::Isometry3d& transform) { return jog_calcs_->getCommandFrameTransform(transform); } -const ServoParameters& JogArm::getParameters() const +const ServoParameters& Servo::getParameters() const { return parameters_; } -sensor_msgs::JointStateConstPtr JogArm::getLatestJointState() const +sensor_msgs::JointStateConstPtr Servo::getLatestJointState() const { return joint_state_subscriber_->getLatest(); } diff --git a/moveit_ros/moveit_servo/src/jog_server.cpp b/moveit_ros/moveit_servo/src/jog_server.cpp index 8ad03d039d..51caa34d3a 100644 --- a/moveit_ros/moveit_servo/src/jog_server.cpp +++ b/moveit_ros/moveit_servo/src/jog_server.cpp @@ -71,16 +71,16 @@ int main(int argc, char** argv) planning_scene_monitor->startStateMonitor(); // Create the jog server - moveit_servo::JogArm jog_arm(nh, planning_scene_monitor); + moveit_servo::Servo servo(nh, planning_scene_monitor); // Start the jog server (runs in the ros spinner) - jog_arm.start(); + servo.start(); // Wait for ros to shutdown ros::waitForShutdown(); // Stop the jog server - jog_arm.stop(); + servo.stop(); return 0; } From 3b819d3e0ecaaaa0879c79e5c4e1324780f9558b Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Tue, 16 Jun 2020 11:58:56 -0500 Subject: [PATCH 360/612] Rename jog_arm.cpp and h files to servo.cpp and h files, fix relvant includes --- moveit_ros/moveit_servo/CMakeLists.txt | 2 +- .../moveit_servo/include/moveit_servo/{jog_arm.h => servo.h} | 0 .../src/cpp_interface_example/cpp_interface_example.cpp | 2 +- moveit_ros/moveit_servo/src/jog_server.cpp | 2 +- moveit_ros/moveit_servo/src/{jog_arm.cpp => servo.cpp} | 2 +- 5 files changed, 4 insertions(+), 4 deletions(-) rename moveit_ros/moveit_servo/include/moveit_servo/{jog_arm.h => servo.h} (100%) rename moveit_ros/moveit_servo/src/{jog_arm.cpp => servo.cpp} (99%) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 2599b34458..0307fe28c6 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -59,7 +59,7 @@ include_directories( add_library(${LIBRARY_NAME} SHARED src/collision_check.cpp src/jog_calcs.cpp - src/jog_arm.cpp + src/servo.cpp src/joint_state_subscriber.cpp src/low_pass_filter.cpp ) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h similarity index 100% rename from moveit_ros/moveit_servo/include/moveit_servo/jog_arm.h rename to moveit_ros/moveit_servo/include/moveit_servo/servo.h diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp index 1292357129..84fa96e5cb 100644 --- a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp @@ -38,7 +38,7 @@ #include -#include +#include #include #include diff --git a/moveit_ros/moveit_servo/src/jog_server.cpp b/moveit_ros/moveit_servo/src/jog_server.cpp index 51caa34d3a..9d238c0f52 100644 --- a/moveit_ros/moveit_servo/src/jog_server.cpp +++ b/moveit_ros/moveit_servo/src/jog_server.cpp @@ -37,7 +37,7 @@ * Author : Andy Zelenak */ -#include +#include namespace { diff --git a/moveit_ros/moveit_servo/src/jog_arm.cpp b/moveit_ros/moveit_servo/src/servo.cpp similarity index 99% rename from moveit_ros/moveit_servo/src/jog_arm.cpp rename to moveit_ros/moveit_servo/src/servo.cpp index 66a845f553..886af812d2 100644 --- a/moveit_ros/moveit_servo/src/jog_arm.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -39,7 +39,7 @@ #include -#include +#include static const std::string LOGNAME = "jog_arm"; From 542061950caa05f0feab100ac589dadd5e24bcbe Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Tue, 16 Jun 2020 12:04:12 -0500 Subject: [PATCH 361/612] Internal rename JOG_ARM_STATUS_CODE_MAP to SERVO_STATUS_CODE_MAP --- moveit_ros/moveit_servo/include/moveit_servo/status_codes.h | 2 +- .../src/cpp_interface_example/cpp_interface_example.cpp | 2 +- moveit_ros/moveit_servo/src/jog_calcs.cpp | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h index bdeca7dc4f..f6e9ddcb54 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h @@ -55,7 +55,7 @@ enum StatusCode : int8_t }; const std::unordered_map - JOG_ARM_STATUS_CODE_MAP({ { INVALID, "Invalid" }, + SERVO_STATUS_CODE_MAP({ { INVALID, "Invalid" }, { NO_WARNING, "No warnings" }, { DECELERATE_FOR_SINGULARITY, "Close to a singularity, decelerating" }, { HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp index 84fa96e5cb..2d6b869aa0 100644 --- a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp @@ -60,7 +60,7 @@ class StatusMonitor if (latest_status != status_) { status_ = latest_status; - const auto& status_str = moveit_servo::JOG_ARM_STATUS_CODE_MAP.at(status_); + const auto& status_str = moveit_servo::SERVO_STATUS_CODE_MAP.at(status_); ROS_INFO_STREAM_NAMED(LOGNAME, "Servo status: " << status_str); } } diff --git a/moveit_ros/moveit_servo/src/jog_calcs.cpp b/moveit_ros/moveit_servo/src/jog_calcs.cpp index f5a1a5d3cb..fa41e6867e 100644 --- a/moveit_ros/moveit_servo/src/jog_calcs.cpp +++ b/moveit_ros/moveit_servo/src/jog_calcs.cpp @@ -588,7 +588,7 @@ void JogCalcs::applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singular if (collision_scale > 0 && collision_scale < 1) { status_ = StatusCode::DECELERATE_FOR_COLLISION; - ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, SERVO_STATUS_CODE_MAP.at(status_)); } else if (collision_scale == 0) { @@ -651,7 +651,7 @@ double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& comm (ini_condition - parameters_.lower_singularity_threshold) / (parameters_.hard_stop_singularity_threshold - parameters_.lower_singularity_threshold); status_ = StatusCode::DECELERATE_FOR_SINGULARITY; - ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, SERVO_STATUS_CODE_MAP.at(status_)); } // Very close to singularity, so halt. @@ -659,7 +659,7 @@ double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& comm { velocity_scale = 0; status_ = StatusCode::HALT_FOR_SINGULARITY; - ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, SERVO_STATUS_CODE_MAP.at(status_)); } } From 12ddc0d7d237409469bdc6e17de48d92a8339907 Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Tue, 16 Jun 2020 12:51:07 -0500 Subject: [PATCH 362/612] Internal rename jog_server to servo_server --- moveit_ros/moveit_servo/CMakeLists.txt | 8 ++++---- moveit_ros/moveit_servo/README.md | 2 +- .../moveit_servo/config/spacenav_via_teleop_tools.yaml | 2 +- moveit_ros/moveit_servo/config/ur_simulated_config.yaml | 6 +++--- .../moveit_servo/launch/cpp_interface_example.launch | 4 ++-- moveit_ros/moveit_servo/launch/spacenav_cpp.launch | 4 ++-- .../moveit_servo/launch/spacenav_teleop_tools.launch | 4 ++-- moveit_ros/moveit_servo/src/jog_server.cpp | 4 ++-- moveit_ros/moveit_servo/src/servo.cpp | 2 +- .../src/teleop_examples/spacenav_to_twist.cpp | 4 ++-- moveit_ros/moveit_servo/test/config/jog_settings.yaml | 8 ++++---- .../moveit_servo/test/launch/common_test_setup.launch | 6 +++--- .../test_drift_dimensions_service.py | 6 +++--- .../test/python_tests/halt_msg/test_jog_arm_halt_msg.py | 4 ++-- .../msg_reception/test_jog_arm_msg_reception.py | 6 +++--- moveit_ros/moveit_servo/test/python_tests/util.py | 2 +- .../vel_accel_limits/test_vel_accel_limits.py | 6 +++--- 17 files changed, 39 insertions(+), 39 deletions(-) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 0307fe28c6..01d8b29e45 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -86,11 +86,11 @@ target_link_libraries(cpp_interface_example ## ROS message-based node ## ############################ -add_executable(jog_server +add_executable(servo_server src/jog_server.cpp ) -add_dependencies(jog_server ${catkin_EXPORTED_TARGETS}) -target_link_libraries(jog_server +add_dependencies(servo_server ${catkin_EXPORTED_TARGETS}) +target_link_libraries(servo_server ${LIBRARY_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} @@ -114,7 +114,7 @@ target_link_libraries(spacenav_to_twist ${catkin_LIBRARIES}) install( TARGETS ${LIBRARY_NAME} - jog_server + servo_server spacenav_to_twist ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/moveit_ros/moveit_servo/README.md b/moveit_ros/moveit_servo/README.md index 6b55fb804d..9ee62f4695 100644 --- a/moveit_ros/moveit_servo/README.md +++ b/moveit_ros/moveit_servo/README.md @@ -37,7 +37,7 @@ Launch the servo node. This example uses commands from a SpaceNavigator joystick If you dont have a SpaceNavigator, send commands like this: ```sh -rostopic pub -r 100 /jog_server/delta_jog_cmds geometry_msgs/TwistStamped "header: auto +rostopic pub -r 100 /servo_server/delta_jog_cmds geometry_msgs/TwistStamped "header: auto twist: linear: x: 0.0 diff --git a/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml b/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml index 2308e0c5a0..14b08917f5 100644 --- a/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml +++ b/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml @@ -2,7 +2,7 @@ teleop: cartesian_twist_command: type: topic message_type: geometry_msgs/TwistStamped - topic_name: jog_server/delta_jog_cmds + topic_name: servo_server/delta_jog_cmds axis_mappings: - axis: 0 diff --git a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml index 74e896cfc9..941b8c2087 100644 --- a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml @@ -44,10 +44,10 @@ hard_stop_singularity_threshold: 30 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names -cartesian_command_in_topic: jog_server/delta_jog_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: jog_server/joint_delta_jog_cmds # Topic for incoming joint angle commands +cartesian_command_in_topic: servo_server/delta_jog_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: servo_server/joint_delta_jog_cmds # Topic for incoming joint angle commands joint_topic: joint_states -status_topic: jog_server/status # Publish status to this topic +status_topic: servo_server/status # Publish status to this topic command_out_topic: joint_group_position_controller/command # Publish outgoing commands here ## Collision checking for the entire robot body diff --git a/moveit_ros/moveit_servo/launch/cpp_interface_example.launch b/moveit_ros/moveit_servo/launch/cpp_interface_example.launch index ab1746a78a..c2ac9b8748 100644 --- a/moveit_ros/moveit_servo/launch/cpp_interface_example.launch +++ b/moveit_ros/moveit_servo/launch/cpp_interface_example.launch @@ -1,8 +1,8 @@ - - + + diff --git a/moveit_ros/moveit_servo/launch/spacenav_cpp.launch b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch index 4346e17a09..3364af893d 100644 --- a/moveit_ros/moveit_servo/launch/spacenav_cpp.launch +++ b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch @@ -12,8 +12,8 @@ - - + + diff --git a/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch b/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch index 238ce8cc03..e86ab13530 100644 --- a/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch +++ b/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch @@ -9,8 +9,8 @@ - - + + diff --git a/moveit_ros/moveit_servo/src/jog_server.cpp b/moveit_ros/moveit_servo/src/jog_server.cpp index 9d238c0f52..2405175ba2 100644 --- a/moveit_ros/moveit_servo/src/jog_server.cpp +++ b/moveit_ros/moveit_servo/src/jog_server.cpp @@ -31,7 +31,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -/* Title : jog_server.cpp +/* Title : servo_server.cpp * Project : moveit_servo * Created : 12/31/2018 * Author : Andy Zelenak @@ -41,7 +41,7 @@ namespace { -constexpr char LOGNAME[] = "jog_server"; +constexpr char LOGNAME[] = "servo_server"; constexpr char ROS_THREADS = 8; } // namespace diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 886af812d2..c6145ec659 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -73,7 +73,7 @@ bool Servo::readParameters() ROS_ERROR_STREAM_NAMED(LOGNAME, "A namespace must be specified in the launch file, like:"); ROS_ERROR_STREAM_NAMED(LOGNAME, ""); + "value=\"left_servo_server\" />"); return false; } diff --git a/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp index 2b75bf87c9..7318f70263 100644 --- a/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp +++ b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp @@ -52,8 +52,8 @@ class SpaceNavToTwist SpaceNavToTwist() : spinner_(NUM_SPINNERS) { joy_sub_ = n_.subscribe("spacenav/joy", QUEUE_LENGTH, &SpaceNavToTwist::joyCallback, this); - twist_pub_ = n_.advertise("jog_server/delta_jog_cmds", QUEUE_LENGTH); - joint_delta_pub_ = n_.advertise("jog_server/joint_delta_jog_cmds", QUEUE_LENGTH); + twist_pub_ = n_.advertise("servo_server/delta_jog_cmds", QUEUE_LENGTH); + joint_delta_pub_ = n_.advertise("servo_server/joint_delta_jog_cmds", QUEUE_LENGTH); spinner_.start(); ros::waitForShutdown(); diff --git a/moveit_ros/moveit_servo/test/config/jog_settings.yaml b/moveit_ros/moveit_servo/test/config/jog_settings.yaml index 67e2d93f69..e3d6767fe9 100644 --- a/moveit_ros/moveit_servo/test/config/jog_settings.yaml +++ b/moveit_ros/moveit_servo/test/config/jog_settings.yaml @@ -44,11 +44,11 @@ hard_stop_singularity_threshold: 45 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names -cartesian_command_in_topic: jog_server/delta_jog_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: jog_server/joint_delta_jog_cmds # Topic for incoming joint angle commands +cartesian_command_in_topic: servo_server/delta_jog_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: servo_server/joint_delta_jog_cmds # Topic for incoming joint angle commands joint_topic: joint_states -status_topic: jog_server/status # Publish status to this topic -command_out_topic: jog_server/command # Publish outgoing commands here +status_topic: servo_server/status # Publish status to this topic +command_out_topic: servo_server/command # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? diff --git a/moveit_ros/moveit_servo/test/launch/common_test_setup.launch b/moveit_ros/moveit_servo/test/launch/common_test_setup.launch index f8f0326693..f2ff32c5b0 100644 --- a/moveit_ros/moveit_servo/test/launch/common_test_setup.launch +++ b/moveit_ros/moveit_servo/test/launch/common_test_setup.launch @@ -12,13 +12,13 @@ - + - - + + diff --git a/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py b/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py index eb049957b4..56d541d142 100755 --- a/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py +++ b/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py @@ -19,11 +19,11 @@ # In other words, only the y-dimension will be controlled exactly. # Check that the service returns and the jog node continues to publish commands to the robot. -CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' +CARTESIAN_JOG_COMMAND_TOPIC = 'servo_server/delta_jog_cmds' -COMMAND_OUT_TOPIC = 'jog_server/command' +COMMAND_OUT_TOPIC = 'servo_server/command' -SERVICE_NAME = 'jog_server/change_drift_dimensions' +SERVICE_NAME = 'servo_server/change_drift_dimensions' @pytest.fixture diff --git a/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py b/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py index aad950df3f..fa0e560291 100755 --- a/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py +++ b/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py @@ -19,10 +19,10 @@ # This can be run as part of a pytest, or like a normal ROS executable: # rosrun moveit_servo test_jog_arm_halt_msg.py -CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' +CARTESIAN_JOG_COMMAND_TOPIC = 'servo_server/delta_jog_cmds' # jog_arm should publish a nonzero warning code here -STATUS_TOPIC = 'jog_server/status' +STATUS_TOPIC = 'servo_server/status' @pytest.fixture diff --git a/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py b/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py index 9c74599f8f..9ac5e721ac 100755 --- a/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py +++ b/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py @@ -17,10 +17,10 @@ # This can be run as part of a pytest, or like a normal ROS executable: # rosrun moveit_servo test_jog_arm_integration.py -JOINT_JOG_COMMAND_TOPIC = 'jog_server/joint_delta_jog_cmds' -CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' +JOINT_JOG_COMMAND_TOPIC = 'servo_server/joint_delta_jog_cmds' +CARTESIAN_JOG_COMMAND_TOPIC = 'servo_server/delta_jog_cmds' -COMMAND_OUT_TOPIC = 'jog_server/command' +COMMAND_OUT_TOPIC = 'servo_server/command' @pytest.fixture diff --git a/moveit_ros/moveit_servo/test/python_tests/util.py b/moveit_ros/moveit_servo/test/python_tests/util.py index a76a681d40..473e9e26d3 100644 --- a/moveit_ros/moveit_servo/test/python_tests/util.py +++ b/moveit_ros/moveit_servo/test/python_tests/util.py @@ -2,7 +2,7 @@ from std_msgs.msg import Int8 # jog_arm should publish a nonzero warning code here -STATUS_TOPIC = 'jog_server/status' +STATUS_TOPIC = 'servo_server/status' def wait_for_jogger_initialization(timeout=15): try: diff --git a/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py b/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py index c6a08de794..dbb64e43f0 100755 --- a/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py +++ b/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py @@ -16,9 +16,9 @@ # This can be run as part of a pytest, or like a normal ROS executable: # rosrun moveit_servo test_vel_accel_limits.py -JOINT_JOG_COMMAND_TOPIC = 'jog_server/joint_delta_jog_cmds' +JOINT_JOG_COMMAND_TOPIC = 'servo_server/joint_delta_jog_cmds' -COMMAND_OUT_TOPIC = 'jog_server/command' +COMMAND_OUT_TOPIC = 'servo_server/command' @pytest.fixture @@ -68,7 +68,7 @@ def test_vel_limit(node): time.sleep(0.1) # Period of outgoing commands from the jogger, from yaml - JOGGER_COMMAND_PERIOD = rospy.get_param("/jog_server/publish_period") + JOGGER_COMMAND_PERIOD = rospy.get_param("/servo_server/publish_period") # Should be no velocities greater than the limit assert len(received) > 2 From 572088b6dd75001ea15e5549ff3afd00dec6816c Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Tue, 16 Jun 2020 12:53:57 -0500 Subject: [PATCH 363/612] Rename file jog_server.cpp to servo_server.cpp and fix CMakeList exec --- moveit_ros/moveit_servo/CMakeLists.txt | 2 +- .../moveit_servo/src/{jog_server.cpp => servo_server.cpp} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename moveit_ros/moveit_servo/src/{jog_server.cpp => servo_server.cpp} (100%) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 01d8b29e45..4937bcae61 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -87,7 +87,7 @@ target_link_libraries(cpp_interface_example ############################ add_executable(servo_server - src/jog_server.cpp + src/servo_server.cpp ) add_dependencies(servo_server ${catkin_EXPORTED_TARGETS}) target_link_libraries(servo_server diff --git a/moveit_ros/moveit_servo/src/jog_server.cpp b/moveit_ros/moveit_servo/src/servo_server.cpp similarity index 100% rename from moveit_ros/moveit_servo/src/jog_server.cpp rename to moveit_ros/moveit_servo/src/servo_server.cpp From e95ecc9986f88a585a3bca49171a64fe8b674c2e Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Thu, 25 Jun 2020 09:02:50 -0500 Subject: [PATCH 364/612] Rename a bunch of internals --- moveit_ros/moveit_servo/CMakeLists.txt | 2 +- moveit_ros/moveit_servo/README.md | 2 +- .../config/spacenav_via_teleop_tools.yaml | 2 +- .../config/ur_simulated_config.yaml | 8 ++-- .../include/moveit_servo/collision_check.h | 2 +- .../include/moveit_servo/jog_calcs.h | 6 +-- .../include/moveit_servo/low_pass_filter.h | 2 +- .../moveit_servo/include/moveit_servo/servo.h | 8 ++-- .../moveit_servo/launch/spacenav_cpp.launch | 2 +- .../launch/spacenav_teleop_tools.launch | 4 +- moveit_ros/moveit_servo/package.xml | 2 +- .../cpp_interface_example.cpp | 14 +++---- moveit_ros/moveit_servo/src/jog_calcs.cpp | 12 +++--- moveit_ros/moveit_servo/src/servo.cpp | 6 +-- moveit_ros/moveit_servo/src/servo_server.cpp | 6 +-- .../src/teleop_examples/spacenav_to_twist.cpp | 12 +++--- .../test/config/jog_settings.yaml | 8 ++-- .../test/jog_cpp_interface_test.cpp | 6 +-- .../test_drift_dimensions_service.py | 6 +-- .../halt_msg/test_jog_arm_halt_msg.py | 22 +++++------ .../test_jog_arm_msg_reception.py | 38 +++++++++---------- .../moveit_servo/test/python_tests/util.py | 6 +-- .../vel_accel_limits/test_vel_accel_limits.py | 20 +++++----- 23 files changed, 98 insertions(+), 98 deletions(-) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 4937bcae61..0f32ef3950 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -7,7 +7,7 @@ endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -set(LIBRARY_NAME jog_arm_cpp_api) +set(LIBRARY_NAME moveit_servo_cpp_api) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) diff --git a/moveit_ros/moveit_servo/README.md b/moveit_ros/moveit_servo/README.md index 9ee62f4695..e889244cb8 100644 --- a/moveit_ros/moveit_servo/README.md +++ b/moveit_ros/moveit_servo/README.md @@ -37,7 +37,7 @@ Launch the servo node. This example uses commands from a SpaceNavigator joystick If you dont have a SpaceNavigator, send commands like this: ```sh -rostopic pub -r 100 /servo_server/delta_jog_cmds geometry_msgs/TwistStamped "header: auto +rostopic pub -r 100 /servo_server/delta_twist_cmds geometry_msgs/TwistStamped "header: auto twist: linear: x: 0.0 diff --git a/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml b/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml index 14b08917f5..cbc6cfa1a4 100644 --- a/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml +++ b/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml @@ -2,7 +2,7 @@ teleop: cartesian_twist_command: type: topic message_type: geometry_msgs/TwistStamped - topic_name: servo_server/delta_jog_cmds + topic_name: servo_server/delta_twist_cmds axis_mappings: - axis: 0 diff --git a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml index 941b8c2087..ed3917f236 100644 --- a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml @@ -1,5 +1,5 @@ ############################################### -# Modify all parameters related to jogging here +# Modify all parameters related to servoing here ############################################### use_gazebo: true # Whether the robot is started in a Gazebo simulation environment @@ -33,7 +33,7 @@ move_group_name: manipulator # Often 'manipulator' or 'arm' planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' ## Stopping behaviour -incoming_command_timeout: 0.1 # Stop jogging if X seconds elapse without a new command +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. # Important because ROS may drop some messages and we need the robot to halt reliably. num_outgoing_halt_msgs_to_publish: 4 @@ -44,8 +44,8 @@ hard_stop_singularity_threshold: 30 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names -cartesian_command_in_topic: servo_server/delta_jog_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: servo_server/joint_delta_jog_cmds # Topic for incoming joint angle commands +cartesian_command_in_topic: servo_server/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: servo_server/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: joint_states status_topic: servo_server/status # Publish status to this topic command_out_topic: joint_group_position_controller/command # Publish outgoing commands here diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index cfd303efed..845b5dac84 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -73,7 +73,7 @@ class CollisionCheck void start(); void stop(); - /** \brief Pause or unpause processing jog commands while keeping the timers alive */ + /** \brief Pause or unpause processing servo commands while keeping the timers alive */ void setPaused(bool paused); private: diff --git a/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h index a32f6ae50c..75fbe05bd7 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h @@ -88,10 +88,10 @@ class JogCalcs /** \brief Timer method */ void run(const ros::TimerEvent& timer_event); - /** \brief Do jogging calculations for Cartesian twist commands. */ + /** \brief Do servoing calculations for Cartesian twist commands. */ bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); - /** \brief Do jogging calculations for direct commands to a joint. */ + /** \brief Do servoing calculations for direct commands to a joint. */ bool jointJogCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); /** \brief Parse the incoming joint msg for the joints of our MoveGroup */ @@ -186,7 +186,7 @@ class JogCalcs moveit_msgs::ChangeDriftDimensions::Response& res); /** \brief Start the main calculation timer */ - // Service callback for changing jogging dimensions + // Service callback for changing servoing dimensions bool changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, moveit_msgs::ChangeControlDimensions::Response& res); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h b/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h index 99d9934224..9108626617 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h @@ -51,7 +51,7 @@ namespace moveit_servo class LowPassFilter { public: - // Larger filter_coeff-> more smoothing of jog commands, but more lag. + // Larger filter_coeff-> more smoothing of servo commands, but more lag. // Rough plot, with cutoff frequency on the y-axis: // https://www.wolframalpha.com/input/?i=plot+arccot(c) explicit LowPassFilter(double low_pass_filter_coeff); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h index d1bca753d0..706ea24db9 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -57,13 +57,13 @@ class Servo ~Servo(); - /** \brief start jog arm */ + /** \brief start servo node */ void start(); - /** \brief stop jog arm */ + /** \brief stop servo node */ void stop(); - /** \brief Pause or unpause processing jog commands while keeping the timers alive */ + /** \brief Pause or unpause processing servo commands while keeping the timers alive */ void setPaused(bool paused); /** @@ -75,7 +75,7 @@ class Servo */ bool getCommandFrameTransform(Eigen::Isometry3d& transform); - /** \brief Get the parameters used by jog arm. */ + /** \brief Get the parameters used by servo node. */ const ServoParameters& getParameters() const; /** \brief Get the latest joint state. */ diff --git a/moveit_ros/moveit_servo/launch/spacenav_cpp.launch b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch index 3364af893d..41d81862db 100644 --- a/moveit_ros/moveit_servo/launch/spacenav_cpp.launch +++ b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch @@ -1,5 +1,5 @@ - - + diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index 1e8ded83ff..0e6b0f433b 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -3,7 +3,7 @@ moveit_servo 1.1.0 - Provides real-time manipulator Cartesian and joint jogging. + Provides real-time manipulator Cartesian and joint servoing. Blake Anderson Andy Zelenak diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp index 2d6b869aa0..6db053e4be 100644 --- a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp @@ -69,7 +69,7 @@ class StatusMonitor }; /** - * Instantiate the C++ jogging interface. + * Instantiate the C++ servo node interface. * Send some Cartesian commands, then some joint commands. */ int main(int argc, char** argv) @@ -95,7 +95,7 @@ int main(int argc, char** argv) false /* skip octomap monitor */); planning_scene_monitor->startStateMonitor(); - // Run the jogging C++ interface in a new timer to ensure a constant outgoing message rate. + // Run the servo node C++ interface in a new timer to ensure a constant outgoing message rate. moveit_servo::Servo servo(nh, planning_scene_monitor); servo.start(); @@ -105,7 +105,7 @@ int main(int argc, char** argv) // Create publishers to send servo commands auto twist_stamped_pub = nh.advertise(servo.getParameters().cartesian_command_in_topic, 1); - auto joint_jog_pub = nh.advertise(servo.getParameters().joint_command_in_topic, 1); + auto joint_servo_pub = nh.advertise(servo.getParameters().joint_command_in_topic, 1); ros::Rate cmd_rate(100); uint num_commands = 0; @@ -114,7 +114,7 @@ int main(int argc, char** argv) while (ros::ok() && num_commands < 200) { // Make a Cartesian velocity message - // Messages are sent to jogger as boost::shared_ptr to enable zero-copy message_passing. + // Messages are sent to servo node as boost::shared_ptr to enable zero-copy message_passing. // Because this message is not copied we should not modify it after we send it. auto msg = moveit::util::make_shared_from_pool(); msg->header.stamp = ros::Time::now(); @@ -128,7 +128,7 @@ int main(int argc, char** argv) ++num_commands; } - // Leave plenty of time for the jogger to halt its previous motion. + // Leave plenty of time for the servo node to halt its previous motion. // For a faster response, adjust the incoming_command_timeout yaml parameter ros::Duration(2).sleep(); @@ -137,7 +137,7 @@ int main(int argc, char** argv) while (ros::ok() && num_commands < 200) { // Make a joint command - // Messages are sent to jogger as boost::shared_ptr to enable zero-copy message_passing. + // Messages are sent to servo node as boost::shared_ptr to enable zero-copy message_passing. // Because this message is not copied we should not modify it after we send it. auto msg = moveit::util::make_shared_from_pool(); msg->header.stamp = ros::Time::now(); @@ -145,7 +145,7 @@ int main(int argc, char** argv) msg->velocities.push_back(0.2); // Send the message - joint_jog_pub.publish(msg); + joint_servo_pub.publish(msg); cmd_rate.sleep(); ++num_commands; } diff --git a/moveit_ros/moveit_servo/src/jog_calcs.cpp b/moveit_ros/moveit_servo/src/jog_calcs.cpp index fa41e6867e..2fb50e53b2 100644 --- a/moveit_ros/moveit_servo/src/jog_calcs.cpp +++ b/moveit_ros/moveit_servo/src/jog_calcs.cpp @@ -69,7 +69,7 @@ bool isNonZero(const control_msgs::JointJog& msg) } } // namespace -// Constructor for the class that handles jogging calculations +// Constructor for the class that handles servoing calculations JogCalcs::JogCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::shared_ptr& joint_state_subscriber) @@ -225,7 +225,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) have_nonzero_joint_jog_ = latest_nonzero_joint_jog_; } - // Get the transform from MoveIt planning frame to jogging command frame + // Get the transform from MoveIt planning frame to servoing command frame // Calculate this transform to ensure it is available via C++ API // We solve (planning_frame -> base -> robot_link_command_frame) // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) @@ -252,11 +252,11 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) } // If not waiting for initial command, and not paused. - // Do jogging calculations only if the robot should move, for efficiency + // Do servoing calculations only if the robot should move, for efficiency // Create new outgoing joint trajectory command message auto joint_trajectory = moveit::util::make_shared_from_pool(); - // Prioritize cartesian jogging above joint jogging + // Prioritize cartesian servoing above joint servoing // Only run commands if not stale and nonzero if (have_nonzero_twist_stamped_ && !twist_command_is_stale_) { @@ -299,7 +299,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) have_nonzero_joint_jog_ = false; } - // Skip the jogging publication if all inputs have been zero for several cycles in a row. + // Skip the servoing publication if all inputs have been zero for several cycles in a row. // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. if (!have_nonzero_command_ && (parameters_.num_outgoing_halt_msgs_to_publish != 0) && (zero_velocity_count_ > parameters_.num_outgoing_halt_msgs_to_publish)) @@ -351,7 +351,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) if (!updated_filters_) resetLowPassFilters(original_joint_state_); } -// Perform the jogging calculations +// Perform the servoing calculations bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory) { // Check for nan's in the incoming command diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index c6145ec659..101ffad173 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -41,7 +41,7 @@ #include -static const std::string LOGNAME = "jog_arm"; +static const std::string LOGNAME = "servo_node"; namespace moveit_servo { @@ -129,7 +129,7 @@ bool Servo::readParameters() { ROS_WARN_NAMED(LOGNAME, "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate" "'self_collision_proximity_threshold' and 'scene_collision_proximity_threshold' " - "parameters. Please update the jogging yaml file."); + "parameters. Please update the servoing yaml file."); if (!have_self_collision_proximity_threshold) { parameters_.self_collision_proximity_threshold = collision_proximity_threshold; @@ -154,7 +154,7 @@ bool Servo::readParameters() if (!rosparam_shortcuts::get("", nh_, parameter_ns + "/status_topic", parameters_.status_topic)) { ROS_WARN_NAMED(LOGNAME, "'status_topic' parameter is missing. Recently renamed from 'warning_topic'. Please update " - "the jogging yaml file."); + "the servoing yaml file."); error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/warning_topic", parameters_.status_topic); } diff --git a/moveit_ros/moveit_servo/src/servo_server.cpp b/moveit_ros/moveit_servo/src/servo_server.cpp index 2405175ba2..ef67b50277 100644 --- a/moveit_ros/moveit_servo/src/servo_server.cpp +++ b/moveit_ros/moveit_servo/src/servo_server.cpp @@ -70,16 +70,16 @@ int main(int argc, char** argv) false /* skip octomap monitor */); planning_scene_monitor->startStateMonitor(); - // Create the jog server + // Create the servo server moveit_servo::Servo servo(nh, planning_scene_monitor); - // Start the jog server (runs in the ros spinner) + // Start the servo server (runs in the ros spinner) servo.start(); // Wait for ros to shutdown ros::waitForShutdown(); - // Stop the jog server + // Stop the servo server servo.stop(); return 0; diff --git a/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp index 7318f70263..8294ba897b 100644 --- a/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp +++ b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp @@ -52,20 +52,20 @@ class SpaceNavToTwist SpaceNavToTwist() : spinner_(NUM_SPINNERS) { joy_sub_ = n_.subscribe("spacenav/joy", QUEUE_LENGTH, &SpaceNavToTwist::joyCallback, this); - twist_pub_ = n_.advertise("servo_server/delta_jog_cmds", QUEUE_LENGTH); - joint_delta_pub_ = n_.advertise("servo_server/joint_delta_jog_cmds", QUEUE_LENGTH); + twist_pub_ = n_.advertise("servo_server/delta_twist_cmds", QUEUE_LENGTH); + joint_delta_pub_ = n_.advertise("servo_server/delta_joint_cmds", QUEUE_LENGTH); spinner_.start(); ros::waitForShutdown(); }; private: - // Convert incoming joy commands to TwistStamped commands for jogging. - // The TwistStamped component goes to jogging, while buttons 0 & 1 control + // Convert incoming joy commands to TwistStamped commands for servoing. + // The TwistStamped component goes to servoing, while buttons 0 & 1 control // joints directly. void joyCallback(const sensor_msgs::Joy::ConstPtr& msg) { - // Cartesian jogging with the axes + // Cartesian servoing with the axes geometry_msgs::TwistStamped twist; twist.header.stamp = ros::Time::now(); twist.twist.linear.x = msg->axes[0]; @@ -76,7 +76,7 @@ class SpaceNavToTwist twist.twist.angular.y = msg->axes[4]; twist.twist.angular.z = msg->axes[5]; - // Joint jogging with the buttons + // Joint servoing with the buttons control_msgs::JointJog joint_deltas; // This example is for a UR5. joint_deltas.joint_names.push_back("shoulder_pan_joint"); diff --git a/moveit_ros/moveit_servo/test/config/jog_settings.yaml b/moveit_ros/moveit_servo/test/config/jog_settings.yaml index e3d6767fe9..e98719f5f6 100644 --- a/moveit_ros/moveit_servo/test/config/jog_settings.yaml +++ b/moveit_ros/moveit_servo/test/config/jog_settings.yaml @@ -1,5 +1,5 @@ ############################################### -# Modify all parameters related to jogging here +# Modify all parameters related to servoing here ############################################### use_gazebo: false # Whether the robot is started in a Gazebo simulation environment @@ -33,7 +33,7 @@ move_group_name: panda_arm # Often 'manipulator' or 'arm' planning_frame: panda_link0 # The MoveIt planning frame. Often 'panda_link0' or 'world' ## Stopping behaviour -incoming_command_timeout: 1 # Stop jogging if X seconds elapse without a new command +incoming_command_timeout: 1 # Stop servoing if X seconds elapse without a new command # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. # Important because ROS may drop some messages and we need the robot to halt reliably. num_outgoing_halt_msgs_to_publish: 1 @@ -44,8 +44,8 @@ hard_stop_singularity_threshold: 45 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names -cartesian_command_in_topic: servo_server/delta_jog_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: servo_server/joint_delta_jog_cmds # Topic for incoming joint angle commands +cartesian_command_in_topic: servo_server/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: servo_server/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: joint_states status_topic: servo_server/status # Publish status to this topic command_out_topic: servo_server/command # Publish outgoing commands here diff --git a/moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp index bc954976f5..4b011e6f1c 100644 --- a/moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp +++ b/moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp @@ -140,7 +140,7 @@ TEST_F(ServoFixture, SendTwistStampedTest) servo_->stop(); } -TEST_F(ServoFixture, SendJointJogTest) +TEST_F(ServoFixture, SendJointServoTest) { servo_->start(); EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; @@ -154,7 +154,7 @@ TEST_F(ServoFixture, SendJointJogTest) auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); // Create publisher to send servo commands - auto joint_jog_pub = nh_.advertise(parameters.joint_command_in_topic, 1); + auto joint_servo_pub = nh_.advertise(parameters.joint_command_in_topic, 1); constexpr double test_duration = 1.0; const double publish_period = parameters.publish_period; @@ -171,7 +171,7 @@ TEST_F(ServoFixture, SendJointJogTest) msg->velocities.push_back(0.1); // Send the message - joint_jog_pub.publish(msg); + joint_servo_pub.publish(msg); publish_rate.sleep(); } diff --git a/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py b/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py index 56d541d142..9d44acb223 100755 --- a/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py +++ b/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py @@ -17,9 +17,9 @@ # Send a service call to allow drift in all but the y-dimension. # In other words, only the y-dimension will be controlled exactly. -# Check that the service returns and the jog node continues to publish commands to the robot. +# Check that the service returns and the servo node continues to publish commands to the robot. -CARTESIAN_JOG_COMMAND_TOPIC = 'servo_server/delta_jog_cmds' +CARTESIAN_COMMAND_TOPIC = 'servo_server/delta_twist_cmds' COMMAND_OUT_TOPIC = 'servo_server/command' @@ -32,7 +32,7 @@ def node(): def test_drift_dimensions_service(node): - assert util.wait_for_jogger_initialization() + assert util.wait_for_servo_initialization() # Service to change drift dimensions drift_service = rospy.ServiceProxy(SERVICE_NAME, ChangeDriftDimensions) diff --git a/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py b/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py index fa0e560291..4641bc59a0 100755 --- a/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py +++ b/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py @@ -14,14 +14,14 @@ import util # The robot starts at a singular position (see config file). -# The jogger should halt and publish a warning. -# Listen for a warning message from the jogger. +# The servo node should halt and publish a warning. +# Listen for a warning message from the servo node. # This can be run as part of a pytest, or like a normal ROS executable: # rosrun moveit_servo test_jog_arm_halt_msg.py -CARTESIAN_JOG_COMMAND_TOPIC = 'servo_server/delta_jog_cmds' +CARTESIAN_COMMAND_TOPIC = 'servo_server/delta_twist_cmds' -# jog_arm should publish a nonzero warning code here +# servo should publish a nonzero warning code here STATUS_TOPIC = 'servo_server/status' @@ -30,10 +30,10 @@ def node(): return rospy.init_node('pytest', anonymous=True) -class CartesianJogCmd(object): +class CartesianCmd(object): def __init__(self): self._pub = rospy.Publisher( - CARTESIAN_JOG_COMMAND_TOPIC, TwistStamped, queue_size=10 + CARTESIAN_COMMAND_TOPIC, TwistStamped, queue_size=10 ) def send_cmd(self, linear, angular): @@ -44,16 +44,16 @@ def send_cmd(self, linear, angular): self._pub.publish(ts) -def test_jog_arm_halt_msg(node): - assert util.wait_for_jogger_initialization() +def test_servo_halt_msg(node): + assert util.wait_for_servo_initialization() received = [] sub = rospy.Subscriber( STATUS_TOPIC, Int8, lambda msg: received.append(msg) ) - cartesian_cmd = CartesianJogCmd() + cartesian_cmd = CartesianCmd() - # This nonzero command should produce jogging output + # This nonzero command should produce servoing output # A subscriber in a different timer fills `received` TEST_DURATION = 1 start_time = rospy.get_rostime() @@ -69,4 +69,4 @@ def test_jog_arm_halt_msg(node): if __name__ == '__main__': node = node() - test_jog_arm_halt_msg(node) + test_servo_halt_msg(node) diff --git a/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py b/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py index 9ac5e721ac..a71f6c7a39 100755 --- a/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py +++ b/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py @@ -13,12 +13,12 @@ sys.path.append(path.dirname(path.dirname(path.abspath(__file__)))) import util -# Test that the jogger publishes controller commands when it receives Cartesian or joint commands. +# Test that the servo node publishes controller commands when it receives Cartesian or joint commands. # This can be run as part of a pytest, or like a normal ROS executable: # rosrun moveit_servo test_jog_arm_integration.py -JOINT_JOG_COMMAND_TOPIC = 'servo_server/joint_delta_jog_cmds' -CARTESIAN_JOG_COMMAND_TOPIC = 'servo_server/delta_jog_cmds' +JOINT_COMMAND_TOPIC = 'servo_server/delta_joint_cmds' +CARTESIAN_COMMAND_TOPIC = 'servo_server/delta_twist_cmds' COMMAND_OUT_TOPIC = 'servo_server/command' @@ -28,9 +28,9 @@ def node(): return rospy.init_node('pytest', anonymous=True) -class JointJogCmd(object): +class JointCmd(object): def __init__(self): - self._pub = rospy.Publisher(JOINT_JOG_COMMAND_TOPIC, JointJog, queue_size=10) + self._pub = rospy.Publisher(JOINT_COMMAND_TOPIC, JointJog, queue_size=10) def send_joint_velocity_cmd(self, joint_pos): jj = JointJog() @@ -40,10 +40,10 @@ def send_joint_velocity_cmd(self, joint_pos): self._pub.publish(jj) -class CartesianJogCmd(object): +class CartesianCmd(object): def __init__(self): self._pub = rospy.Publisher( - CARTESIAN_JOG_COMMAND_TOPIC, TwistStamped, queue_size=10 + CARTESIAN_COMMAND_TOPIC, TwistStamped, queue_size=10 ) def send_cmd(self, linear, angular): @@ -54,16 +54,16 @@ def send_cmd(self, linear, angular): self._pub.publish(ts) -def test_jog_arm_cartesian_command(node): +def test_servo_cartesian_command(node): # Test sending a cartesian velocity command - assert util.wait_for_jogger_initialization() + assert util.wait_for_servo_initialization() received = [] sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) - cartesian_cmd = CartesianJogCmd() + cartesian_cmd = CartesianCmd() # Repeated zero-commands should produce no output, other than a few halt messages # A subscriber in a different timer fills 'received' @@ -74,12 +74,12 @@ def test_jog_arm_cartesian_command(node): rospy.sleep(1) assert len(received) <= 4 # 'num_outgoing_halt_msgs_to_publish' in the config file - # This nonzero command should produce jogging output + # This nonzero command should produce servoing output # A subscriber in a different timer fills `received` TEST_DURATION = 1 PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from servo config file - # Send a command to start the jogger + # Send a command to start the servo node cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) start_time = rospy.get_rostime() @@ -93,22 +93,22 @@ def test_jog_arm_cartesian_command(node): assert len(received) <= TEST_DURATION/PUBLISH_PERIOD + 20 -def test_jog_arm_joint_command(node): +def test_servo_joint_command(node): # Test sending a joint command - assert util.wait_for_jogger_initialization() + assert util.wait_for_servo_initialization() received = [] sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) - joint_cmd = JointJogCmd() + joint_cmd = JointCmd() TEST_DURATION = 1 PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from servo config file velocities = [0.1] - # Send a command to start the jogger + # Send a command to start the servo node joint_cmd.send_joint_velocity_cmd(velocities) start_time = rospy.get_rostime() @@ -124,6 +124,6 @@ def test_jog_arm_joint_command(node): if __name__ == '__main__': node = node() - time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for servo server to init - test_jog_arm_cartesian_command(node) - test_jog_arm_joint_command(node) + time.sleep(SERVO_SETTLE_TIME_S) # wait for servo server to init + test_servo_cartesian_command(node) + test_servo_joint_command(node) diff --git a/moveit_ros/moveit_servo/test/python_tests/util.py b/moveit_ros/moveit_servo/test/python_tests/util.py index 473e9e26d3..f813e8dea7 100644 --- a/moveit_ros/moveit_servo/test/python_tests/util.py +++ b/moveit_ros/moveit_servo/test/python_tests/util.py @@ -1,14 +1,14 @@ import rospy from std_msgs.msg import Int8 -# jog_arm should publish a nonzero warning code here +# servo should publish a nonzero warning code here STATUS_TOPIC = 'servo_server/status' -def wait_for_jogger_initialization(timeout=15): +def wait_for_servo_initialization(timeout=15): try: rospy.wait_for_message(STATUS_TOPIC, Int8, timeout=timeout) except rospy.ROSException as exc: - rospy.logerr("The jogger topic " + STATUS_TOPIC + " is not available: " + str(exc)) + rospy.logerr("The servo topic " + STATUS_TOPIC + " is not available: " + str(exc)) return False return True diff --git a/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py b/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py index dbb64e43f0..38dcdb2b8f 100755 --- a/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py +++ b/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py @@ -16,7 +16,7 @@ # This can be run as part of a pytest, or like a normal ROS executable: # rosrun moveit_servo test_vel_accel_limits.py -JOINT_JOG_COMMAND_TOPIC = 'servo_server/joint_delta_jog_cmds' +JOINT_COMMAND_TOPIC = 'servo_server/delta_joint_cmds' COMMAND_OUT_TOPIC = 'servo_server/command' @@ -26,9 +26,9 @@ def node(): return rospy.init_node('pytest', anonymous=True) -class JointJogCmd(object): +class JointServoCmd(object): def __init__(self): - self._pub = rospy.Publisher(JOINT_JOG_COMMAND_TOPIC, JointJog, queue_size=10) + self._pub = rospy.Publisher(JOINT_COMMAND_TOPIC, JointJog, queue_size=10) def send_joint_velocity_cmd(self, joint_pos): jj = JointJog() @@ -41,24 +41,24 @@ def send_joint_velocity_cmd(self, joint_pos): def test_vel_limit(node): # Test sending a joint command - assert util.wait_for_jogger_initialization() + assert util.wait_for_servo_initialization() received = [] sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) - joint_cmd = JointJogCmd() + joint_cmd = JointServoCmd() TEST_DURATION = 1 - PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file + PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from servo config file # Panda arm limit, from joint_limits.yaml VELOCITY_LIMIT = rospy.get_param("/robot_description_planning/joint_limits/panda_joint1/max_velocity") # Send a velocity command that exceeds the limit velocities = [10 * VELOCITY_LIMIT] - # Send a command to start the jogger + # Send a command to start the servo node joint_cmd.send_joint_velocity_cmd(velocities) start_time = rospy.get_rostime() @@ -67,15 +67,15 @@ def test_vel_limit(node): joint_cmd.send_joint_velocity_cmd(velocities) time.sleep(0.1) - # Period of outgoing commands from the jogger, from yaml - JOGGER_COMMAND_PERIOD = rospy.get_param("/servo_server/publish_period") + # Period of outgoing commands from the servo node, from yaml + SERVO_COMMAND_PERIOD = rospy.get_param("/servo_server/publish_period") # Should be no velocities greater than the limit assert len(received) > 2 for msg_idx in range(1, len(received)): try: velocity = \ - (received[msg_idx].points[0].positions[0] - received[msg_idx - 1].points[0].positions[0]) / JOGGER_COMMAND_PERIOD + (received[msg_idx].points[0].positions[0] - received[msg_idx - 1].points[0].positions[0]) / SERVO_COMMAND_PERIOD assert abs(velocity) <= VELOCITY_LIMIT except IndexError: # Sometimes a message doesn't have any points From a602b04f6d453079ddd474b4c976a82d4f046a69 Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Thu, 25 Jun 2020 09:07:48 -0500 Subject: [PATCH 365/612] Internal rename JogCalcs -> ServoCalcs --- .../include/moveit_servo/jog_calcs.h | 12 +-- .../moveit_servo/include/moveit_servo/servo.h | 2 +- moveit_ros/moveit_servo/src/jog_calcs.cpp | 90 ++++++++++--------- moveit_ros/moveit_servo/src/servo.cpp | 10 +-- 4 files changed, 58 insertions(+), 56 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h index 75fbe05bd7..8766341523 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h @@ -61,12 +61,12 @@ namespace moveit_servo { -class JogCalcs +class ServoCalcs { public: - JogCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& joint_state_subscriber); + ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::shared_ptr& joint_state_subscriber); /** \brief Start and stop the timer where we do work and publish outputs */ void start(); @@ -89,10 +89,10 @@ class JogCalcs void run(const ros::TimerEvent& timer_event); /** \brief Do servoing calculations for Cartesian twist commands. */ - bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); + bool cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); /** \brief Do servoing calculations for direct commands to a joint. */ - bool jointJogCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); + bool jointServoCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); /** \brief Parse the incoming joint msg for the joints of our MoveGroup */ bool updateJoints(); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h index 706ea24db9..7de5197b41 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -93,7 +93,7 @@ class Servo ServoParameters parameters_; std::shared_ptr joint_state_subscriber_; - std::unique_ptr jog_calcs_; + std::unique_ptr servo_calcs_; std::unique_ptr collision_checker_; }; diff --git a/moveit_ros/moveit_servo/src/jog_calcs.cpp b/moveit_ros/moveit_servo/src/jog_calcs.cpp index 2fb50e53b2..77daa93e59 100644 --- a/moveit_ros/moveit_servo/src/jog_calcs.cpp +++ b/moveit_ros/moveit_servo/src/jog_calcs.cpp @@ -70,9 +70,9 @@ bool isNonZero(const control_msgs::JointJog& msg) } // namespace // Constructor for the class that handles servoing calculations -JogCalcs::JogCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& joint_state_subscriber) +ServoCalcs::ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::shared_ptr& joint_state_subscriber) : nh_(nh) , parameters_(parameters) , planning_scene_monitor_(planning_scene_monitor) @@ -95,23 +95,23 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, // Subscribe to command topics twist_stamped_sub_ = - nh_.subscribe(parameters_.cartesian_command_in_topic, ROS_QUEUE_SIZE, &JogCalcs::twistStampedCB, this); - joint_jog_sub_ = nh_.subscribe(parameters_.joint_command_in_topic, ROS_QUEUE_SIZE, &JogCalcs::jointJogCB, this); + nh_.subscribe(parameters_.cartesian_command_in_topic, ROS_QUEUE_SIZE, &ServoCalcs::twistStampedCB, this); + joint_jog_sub_ = nh_.subscribe(parameters_.joint_command_in_topic, ROS_QUEUE_SIZE, &ServoCalcs::jointJogCB, this); // ROS Server for allowing drift in some dimensions drift_dimensions_server_ = nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions", - &JogCalcs::changeDriftDimensions, this); + &ServoCalcs::changeDriftDimensions, this); // ROS Server for changing the control dimensions control_dimensions_server_ = nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", - &JogCalcs::changeControlDimensions, this); + &ServoCalcs::changeControlDimensions, this); // Publish and Subscribe to internal namespace topics ros::NodeHandle internal_nh("~internal"); collision_velocity_scale_sub_ = - internal_nh.subscribe("collision_velocity_scale", ROS_QUEUE_SIZE, &JogCalcs::collisionVelocityScaleCB, this); + internal_nh.subscribe("collision_velocity_scale", ROS_QUEUE_SIZE, &ServoCalcs::collisionVelocityScaleCB, this); worst_case_stop_time_pub_ = internal_nh.advertise("worst_case_stop_time", ROS_QUEUE_SIZE); // Publish freshly-calculated joints to the robot. @@ -168,19 +168,19 @@ JogCalcs::JogCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, } } -void JogCalcs::start() +void ServoCalcs::start() { stop_requested_ = false; - timer_ = nh_.createTimer(period_, &JogCalcs::run, this); + timer_ = nh_.createTimer(period_, &ServoCalcs::run, this); } -void JogCalcs::stop() +void ServoCalcs::stop() { stop_requested_ = true; timer_.stop(); } -void JogCalcs::run(const ros::TimerEvent& timer_event) +void ServoCalcs::run(const ros::TimerEvent& timer_event) { // Log warning when the last loop duration was longer than the period if (timer_event.profile.last_duration.toSec() > period_.toSec()) @@ -260,7 +260,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) // Only run commands if not stale and nonzero if (have_nonzero_twist_stamped_ && !twist_command_is_stale_) { - if (!cartesianJogCalcs(twist_stamped_cmd_, *joint_trajectory)) + if (!cartesianServoCalcs(twist_stamped_cmd_, *joint_trajectory)) { resetLowPassFilters(original_joint_state_); return; @@ -268,7 +268,7 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) } else if (have_nonzero_joint_jog_ && !joint_command_is_stale_) { - if (!jointJogCalcs(joint_jog_cmd_, *joint_trajectory)) + if (!jointServoCalcs(joint_jog_cmd_, *joint_trajectory)) { resetLowPassFilters(original_joint_state_); return; @@ -352,7 +352,8 @@ void JogCalcs::run(const ros::TimerEvent& timer_event) resetLowPassFilters(original_joint_state_); } // Perform the servoing calculations -bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory) +bool ServoCalcs::cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, + trajectory_msgs::JointTrajectory& joint_trajectory) { // Check for nan's in the incoming command if (std::isnan(cmd.twist.linear.x) || std::isnan(cmd.twist.linear.y) || std::isnan(cmd.twist.linear.z) || @@ -462,7 +463,7 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, trajectory_ms return convertDeltasToOutgoingCmd(joint_trajectory); } -bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory) +bool ServoCalcs::jointServoCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory) { // Check for nan's for (double velocity : cmd.velocities) @@ -485,7 +486,7 @@ bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, trajectory_msgs: return convertDeltasToOutgoingCmd(joint_trajectory); } -bool JogCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& joint_trajectory) +bool ServoCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& joint_trajectory) { internal_joint_state_ = original_joint_state_; if (!addJointIncrements(internal_joint_state_, delta_theta_)) @@ -516,7 +517,8 @@ bool JogCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& join // Spam several redundant points into the trajectory. The first few may be skipped if the // time stamp is in the past when it reaches the client. Needed for gazebo simulation. // Start from 2 because the first point's timestamp is already 1*parameters_.publish_period -void JogCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& joint_trajectory, int count) const +void ServoCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& joint_trajectory, + int count) const { joint_trajectory.points.resize(count); auto point = joint_trajectory.points[0]; @@ -528,7 +530,7 @@ void JogCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTraject } } -void JogCalcs::lowPassFilterPositions(sensor_msgs::JointState& joint_state) +void ServoCalcs::lowPassFilterPositions(sensor_msgs::JointState& joint_state) { for (size_t i = 0; i < position_filters_.size(); ++i) { @@ -538,7 +540,7 @@ void JogCalcs::lowPassFilterPositions(sensor_msgs::JointState& joint_state) updated_filters_ = true; } -void JogCalcs::resetLowPassFilters(const sensor_msgs::JointState& joint_state) +void ServoCalcs::resetLowPassFilters(const sensor_msgs::JointState& joint_state) { for (std::size_t i = 0; i < position_filters_.size(); ++i) { @@ -548,7 +550,7 @@ void JogCalcs::resetLowPassFilters(const sensor_msgs::JointState& joint_state) updated_filters_ = true; } -void JogCalcs::calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta) +void ServoCalcs::calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta) { for (int i = 0; i < delta_theta.size(); ++i) { @@ -556,8 +558,8 @@ void JogCalcs::calculateJointVelocities(sensor_msgs::JointState& joint_state, co } } -void JogCalcs::composeJointTrajMessage(const sensor_msgs::JointState& joint_state, - trajectory_msgs::JointTrajectory& joint_trajectory) const +void ServoCalcs::composeJointTrajMessage(const sensor_msgs::JointState& joint_state, + trajectory_msgs::JointTrajectory& joint_trajectory) const { joint_trajectory.header.frame_id = parameters_.planning_frame; joint_trajectory.header.stamp = ros::Time::now(); @@ -581,7 +583,7 @@ void JogCalcs::composeJointTrajMessage(const sensor_msgs::JointState& joint_stat } // Apply velocity scaling for proximity of collisions and singularities. -void JogCalcs::applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singularity_scale) +void ServoCalcs::applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singularity_scale) { double collision_scale = collision_velocity_scale_; @@ -599,9 +601,9 @@ void JogCalcs::applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singular } // Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion -double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, - const Eigen::JacobiSVD& svd, - const Eigen::MatrixXd& pseudo_inverse) +double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, + const Eigen::JacobiSVD& svd, + const Eigen::MatrixXd& pseudo_inverse) { double velocity_scale = 1; std::size_t num_dimensions = commanded_velocity.size(); @@ -666,7 +668,7 @@ double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& comm return velocity_scale; } -void JogCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta) +void ServoCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta) { Eigen::ArrayXd velocity = delta_theta / parameters_.publish_period; const Eigen::ArrayXd acceleration = (velocity - prev_joint_velocity_) / parameters_.publish_period; @@ -740,7 +742,7 @@ void JogCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta) } } -bool JogCalcs::enforceSRDFPositionLimits() +bool ServoCalcs::enforceSRDFPositionLimits() { bool halting = false; @@ -782,7 +784,7 @@ bool JogCalcs::enforceSRDFPositionLimits() // Suddenly halt for a joint limit or other critical issue. // Is handled differently for position vs. velocity control. -void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory) +void ServoCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory) { if (joint_trajectory.points.empty()) { @@ -804,7 +806,7 @@ void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory) } // Parse the incoming joint msg for the joints of our MoveGroup -bool JogCalcs::updateJoints() +bool ServoCalcs::updateJoints() { sensor_msgs::JointStateConstPtr latest_joint_state = joint_state_subscriber_->getLatest(); @@ -884,7 +886,7 @@ bool JogCalcs::updateJoints() } // Scale the incoming jog command -Eigen::VectorXd JogCalcs::scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const +Eigen::VectorXd ServoCalcs::scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const { Eigen::VectorXd result(6); @@ -914,7 +916,7 @@ Eigen::VectorXd JogCalcs::scaleCartesianCommand(const geometry_msgs::TwistStampe return result; } -Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& command) const +Eigen::VectorXd ServoCalcs::scaleJointCommand(const control_msgs::JointJog& command) const { Eigen::VectorXd result(num_joints_); result.setZero(); @@ -946,7 +948,7 @@ Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& comman } // Add the deltas to each joint -bool JogCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const +bool ServoCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const { for (std::size_t i = 0, size = static_cast(increments.size()); i < size; ++i) { @@ -966,7 +968,7 @@ bool JogCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen:: return true; } -void JogCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta_x, unsigned int row_to_remove) +void ServoCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta_x, unsigned int row_to_remove) { unsigned int num_rows = jacobian.rows() - 1; unsigned int num_cols = jacobian.cols(); @@ -982,7 +984,7 @@ void JogCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta delta_x.conservativeResize(num_rows); } -bool JogCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) +bool ServoCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) { const std::lock_guard lock(latest_state_mutex_); transform = tf_moveit_to_robot_cmd_frame_; @@ -991,7 +993,7 @@ bool JogCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) return !transform.matrix().isZero(0); } -void JogCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg) +void ServoCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg) { const std::lock_guard lock(latest_state_mutex_); latest_twist_stamped_ = msg; @@ -1001,7 +1003,7 @@ void JogCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg) latest_twist_command_stamp_ = msg->header.stamp; } -void JogCalcs::jointJogCB(const control_msgs::JointJogConstPtr& msg) +void ServoCalcs::jointJogCB(const control_msgs::JointJogConstPtr& msg) { const std::lock_guard lock(latest_state_mutex_); latest_joint_jog_ = msg; @@ -1011,13 +1013,13 @@ void JogCalcs::jointJogCB(const control_msgs::JointJogConstPtr& msg) latest_joint_command_stamp_ = msg->header.stamp; } -void JogCalcs::collisionVelocityScaleCB(const std_msgs::Float64ConstPtr& msg) +void ServoCalcs::collisionVelocityScaleCB(const std_msgs::Float64ConstPtr& msg) { collision_velocity_scale_ = msg->data; } -bool JogCalcs::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, - moveit_msgs::ChangeDriftDimensions::Response& res) +bool ServoCalcs::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, + moveit_msgs::ChangeDriftDimensions::Response& res) { drift_dimensions_[0] = req.drift_x_translation; drift_dimensions_[1] = req.drift_y_translation; @@ -1030,8 +1032,8 @@ bool JogCalcs::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request return true; } -bool JogCalcs::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, - moveit_msgs::ChangeControlDimensions::Response& res) +bool ServoCalcs::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, + moveit_msgs::ChangeControlDimensions::Response& res) { control_dimensions_[0] = req.control_x_translation; control_dimensions_[1] = req.control_y_translation; @@ -1044,7 +1046,7 @@ bool JogCalcs::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Req return true; } -void JogCalcs::setPaused(bool paused) +void ServoCalcs::setPaused(bool paused) { paused_ = paused; } diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 101ffad173..d8db2630df 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -54,7 +54,7 @@ Servo::Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMon joint_state_subscriber_ = std::make_shared(nh_, parameters_.joint_topic); - jog_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); + servo_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); collision_checker_ = std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); @@ -279,7 +279,7 @@ void Servo::start() setPaused(false); // Crunch the numbers in this timer - jog_calcs_->start(); + servo_calcs_->start(); // Check collisions in this timer if (parameters_.check_collisions) @@ -288,7 +288,7 @@ void Servo::start() void Servo::stop() { - jog_calcs_->stop(); + servo_calcs_->stop(); collision_checker_->stop(); } @@ -299,13 +299,13 @@ Servo::~Servo() void Servo::setPaused(bool paused) { - jog_calcs_->setPaused(paused); + servo_calcs_->setPaused(paused); collision_checker_->setPaused(paused); } bool Servo::getCommandFrameTransform(Eigen::Isometry3d& transform) { - return jog_calcs_->getCommandFrameTransform(transform); + return servo_calcs_->getCommandFrameTransform(transform); } const ServoParameters& Servo::getParameters() const From 499665c232106332fe1c9562119fd607e1529d3b Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Thu, 25 Jun 2020 09:13:41 -0500 Subject: [PATCH 366/612] Rename jog_calcs cpp/h -> servo_calcs cpp/h and fix includes --- moveit_ros/moveit_servo/CMakeLists.txt | 2 +- moveit_ros/moveit_servo/include/moveit_servo/servo.h | 2 +- .../include/moveit_servo/{jog_calcs.h => servo_calcs.h} | 2 +- .../moveit_servo/src/{jog_calcs.cpp => servo_calcs.cpp} | 4 ++-- 4 files changed, 5 insertions(+), 5 deletions(-) rename moveit_ros/moveit_servo/include/moveit_servo/{jog_calcs.h => servo_calcs.h} (99%) rename moveit_ros/moveit_servo/src/{jog_calcs.cpp => servo_calcs.cpp} (99%) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 0f32ef3950..7e673b3d78 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -58,7 +58,7 @@ include_directories( add_library(${LIBRARY_NAME} SHARED src/collision_check.cpp - src/jog_calcs.cpp + src/servo_calcs.cpp src/servo.cpp src/joint_state_subscriber.cpp src/low_pass_filter.cpp diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h index 7de5197b41..6a3a29d4bd 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -42,7 +42,7 @@ #include #include -#include +#include #include namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h similarity index 99% rename from moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h rename to moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 8766341523..0e116b64f5 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/jog_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -1,5 +1,5 @@ /******************************************************************************* - * Title : jog_calcs.h + * Title : servo_calcs.h * Project : moveit_servo * Created : 1/11/2019 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson diff --git a/moveit_ros/moveit_servo/src/jog_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp similarity index 99% rename from moveit_ros/moveit_servo/src/jog_calcs.cpp rename to moveit_ros/moveit_servo/src/servo_calcs.cpp index 77daa93e59..b9593af591 100644 --- a/moveit_ros/moveit_servo/src/jog_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -31,7 +31,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -/* Title : jog_calcs.cpp +/* Title : servo_calcs.cpp * Project : moveit_servo * Created : 1/11/2019 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson @@ -40,7 +40,7 @@ #include #include -#include +#include #include static const std::string LOGNAME = "jog_calcs"; From bc991965106f2cbe317160a7108b2244e3c6a3a6 Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Thu, 25 Jun 2020 09:24:25 -0500 Subject: [PATCH 367/612] Rename servo_calcs internals --- .../include/moveit_servo/servo_calcs.h | 20 +++++------ moveit_ros/moveit_servo/src/servo_calcs.cpp | 34 +++++++++---------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 0e116b64f5..1b6bb31f90 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -81,7 +81,7 @@ class ServoCalcs */ bool getCommandFrameTransform(Eigen::Isometry3d& transform); - /** \brief Pause or unpause processing jog commands while keeping the timers alive */ + /** \brief Pause or unpause processing servo commands while keeping the timers alive */ void setPaused(bool paused); private: @@ -171,7 +171,7 @@ class ServoCalcs /* \brief Command callbacks */ void twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg); - void jointJogCB(const control_msgs::JointJogConstPtr& msg); + void jointCmdCB(const control_msgs::JointJogConstPtr& msg); void collisionVelocityScaleCB(const std_msgs::Float64ConstPtr& msg); /** @@ -206,19 +206,19 @@ class ServoCalcs int zero_velocity_count_ = 0; // Flag for staying inactive while there are no incoming commands - bool wait_for_jog_commands_ = true; + bool wait_for_servo_commands_ = true; // Flag saying if the filters were updated during the timer callback bool updated_filters_ = false; // Nonzero status flags bool have_nonzero_twist_stamped_ = false; - bool have_nonzero_joint_jog_ = false; + bool have_nonzero_joint_command_ = false; bool have_nonzero_command_ = false; // Incoming command messages geometry_msgs::TwistStamped twist_stamped_cmd_; - control_msgs::JointJog joint_jog_cmd_; + control_msgs::JointJog joint_servo_cmd_; const moveit::core::JointModelGroup* joint_model_group_; @@ -226,8 +226,8 @@ class ServoCalcs // incoming_joint_state_ is the incoming message. It may contain passive joints or other joints we don't care about. // (mutex protected below) - // internal_joint_state_ is used in jog calculations. It shouldn't be relied on to be accurate. - // original_joint_state_ is the same as incoming_joint_state_ except it only contains the joints jog_arm acts on. + // internal_joint_state_ is used in servo calculations. It shouldn't be relied on to be accurate. + // original_joint_state_ is the same as incoming_joint_state_ except it only contains the joints the servo node acts on. sensor_msgs::JointState internal_joint_state_, original_joint_state_; std::map joint_state_name_map_; @@ -240,7 +240,7 @@ class ServoCalcs ros::Duration period_; ros::Subscriber joint_state_sub_; ros::Subscriber twist_stamped_sub_; - ros::Subscriber joint_jog_sub_; + ros::Subscriber joint_cmd_sub_; ros::Subscriber collision_velocity_scale_sub_; ros::Publisher status_pub_; ros::Publisher worst_case_stop_time_pub_; @@ -278,10 +278,10 @@ class ServoCalcs mutable std::mutex latest_state_mutex_; Eigen::Isometry3d tf_moveit_to_robot_cmd_frame_; geometry_msgs::TwistStampedConstPtr latest_twist_stamped_; - control_msgs::JointJogConstPtr latest_joint_jog_; + control_msgs::JointJogConstPtr latest_joint_cmd_; ros::Time latest_twist_command_stamp_ = ros::Time(0.); ros::Time latest_joint_command_stamp_ = ros::Time(0.); bool latest_nonzero_twist_stamped_ = false; - bool latest_nonzero_joint_jog_ = false; + bool latest_nonzero_joint_cmd_ = false; }; } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index b9593af591..2753d22ae0 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -43,7 +43,7 @@ #include #include -static const std::string LOGNAME = "jog_calcs"; +static const std::string LOGNAME = "servo_calcs"; constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30; // Seconds to throttle logs inside loops namespace moveit_servo @@ -96,7 +96,7 @@ ServoCalcs::ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, // Subscribe to command topics twist_stamped_sub_ = nh_.subscribe(parameters_.cartesian_command_in_topic, ROS_QUEUE_SIZE, &ServoCalcs::twistStampedCB, this); - joint_jog_sub_ = nh_.subscribe(parameters_.joint_command_in_topic, ROS_QUEUE_SIZE, &ServoCalcs::jointJogCB, this); + joint_cmd_sub_ = nh_.subscribe(parameters_.joint_command_in_topic, ROS_QUEUE_SIZE, &ServoCalcs::jointCmdCB, this); // ROS Server for allowing drift in some dimensions drift_dimensions_server_ = @@ -212,8 +212,8 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) const std::lock_guard lock(latest_state_mutex_); if (latest_twist_stamped_) twist_stamped_cmd_ = *latest_twist_stamped_; - if (latest_joint_jog_) - joint_jog_cmd_ = *latest_joint_jog_; + if (latest_joint_cmd_) + joint_servo_cmd_ = *latest_joint_cmd_; // Check for stale cmds twist_command_is_stale_ = @@ -222,7 +222,7 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) ((ros::Time::now() - latest_joint_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); have_nonzero_twist_stamped_ = latest_nonzero_twist_stamped_; - have_nonzero_joint_jog_ = latest_nonzero_joint_jog_; + have_nonzero_joint_command_ = latest_nonzero_joint_cmd_; } // Get the transform from MoveIt planning frame to servoing command frame @@ -232,20 +232,20 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) tf_moveit_to_robot_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); - have_nonzero_command_ = have_nonzero_twist_stamped_ || have_nonzero_joint_jog_; + have_nonzero_command_ = have_nonzero_twist_stamped_ || have_nonzero_joint_command_; // Don't end this function without updating the filters updated_filters_ = false; - // If paused or while waiting for initial jog commands, just keep the low-pass filters up to date with current + // If paused or while waiting for initial servo commands, just keep the low-pass filters up to date with current // joints so a jump doesn't occur when restarting - if (wait_for_jog_commands_ || paused_) + if (wait_for_servo_commands_ || paused_) { resetLowPassFilters(original_joint_state_); // Check if there are any new commands with valid timestamp - wait_for_jog_commands_ = - twist_stamped_cmd_.header.stamp == ros::Time(0.) && joint_jog_cmd_.header.stamp == ros::Time(0.); + wait_for_servo_commands_ = + twist_stamped_cmd_.header.stamp == ros::Time(0.) && joint_servo_cmd_.header.stamp == ros::Time(0.); // Early exit return; @@ -266,9 +266,9 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) return; } } - else if (have_nonzero_joint_jog_ && !joint_command_is_stale_) + else if (have_nonzero_joint_command_ && !joint_command_is_stale_) { - if (!jointServoCalcs(joint_jog_cmd_, *joint_trajectory)) + if (!jointServoCalcs(joint_servo_cmd_, *joint_trajectory)) { resetLowPassFilters(original_joint_state_); return; @@ -296,7 +296,7 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) { suddenHalt(*joint_trajectory); have_nonzero_twist_stamped_ = false; - have_nonzero_joint_jog_ = false; + have_nonzero_joint_command_ = false; } // Skip the servoing publication if all inputs have been zero for several cycles in a row. @@ -885,7 +885,7 @@ bool ServoCalcs::updateJoints() return true; } -// Scale the incoming jog command +// Scale the incoming servo command Eigen::VectorXd ServoCalcs::scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const { Eigen::VectorXd result(6); @@ -1003,11 +1003,11 @@ void ServoCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg) latest_twist_command_stamp_ = msg->header.stamp; } -void ServoCalcs::jointJogCB(const control_msgs::JointJogConstPtr& msg) +void ServoCalcs::jointCmdCB(const control_msgs::JointJogConstPtr& msg) { const std::lock_guard lock(latest_state_mutex_); - latest_joint_jog_ = msg; - latest_nonzero_joint_jog_ = isNonZero(*latest_joint_jog_); + latest_joint_cmd_ = msg; + latest_nonzero_joint_cmd_ = isNonZero(*latest_joint_cmd_); if (msg->header.stamp != ros::Time(0.)) latest_joint_command_stamp_ = msg->header.stamp; From abb90dc722317b113a04806aba77ec5e9e549ba1 Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Thu, 25 Jun 2020 09:28:47 -0500 Subject: [PATCH 368/612] Rename jog_settings.yaml -> servo_settings.yaml and fix includes --- .../test/config/{jog_settings.yaml => servo_settings.yaml} | 0 moveit_ros/moveit_servo/test/jog_cpp_interface_test.test | 2 +- moveit_ros/moveit_servo/test/launch/common_test_setup.launch | 4 ++-- 3 files changed, 3 insertions(+), 3 deletions(-) rename moveit_ros/moveit_servo/test/config/{jog_settings.yaml => servo_settings.yaml} (100%) diff --git a/moveit_ros/moveit_servo/test/config/jog_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml similarity index 100% rename from moveit_ros/moveit_servo/test/config/jog_settings.yaml rename to moveit_ros/moveit_servo/test/config/servo_settings.yaml diff --git a/moveit_ros/moveit_servo/test/jog_cpp_interface_test.test b/moveit_ros/moveit_servo/test/jog_cpp_interface_test.test index 829a33e117..88d1215191 100644 --- a/moveit_ros/moveit_servo/test/jog_cpp_interface_test.test +++ b/moveit_ros/moveit_servo/test/jog_cpp_interface_test.test @@ -13,6 +13,6 @@ - + diff --git a/moveit_ros/moveit_servo/test/launch/common_test_setup.launch b/moveit_ros/moveit_servo/test/launch/common_test_setup.launch index f2ff32c5b0..3d587e12e9 100644 --- a/moveit_ros/moveit_servo/test/launch/common_test_setup.launch +++ b/moveit_ros/moveit_servo/test/launch/common_test_setup.launch @@ -13,13 +13,13 @@ - + - + From 747e82c05f599f642ccb3b005ef24a143aa1c4c6 Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Thu, 25 Jun 2020 09:36:18 -0500 Subject: [PATCH 369/612] Rename test names, fix calls to those names --- moveit_ros/moveit_servo/CMakeLists.txt | 20 +++++++++---------- ... servo_drift_dimensions_service_test.test} | 2 +- ...msg_test.test => servo_halt_msg_test.test} | 2 +- ...est.test => servo_msg_reception_test.test} | 2 +- ....test => servo_vel_accel_limits_test.test} | 2 +- ...arm_halt_msg.py => test_servo_halt_msg.py} | 2 +- ...ception.py => test_servo_msg_reception.py} | 2 +- ..._test.cpp => servo_cpp_interface_test.cpp} | 2 +- ...est.test => servo_cpp_interface_test.test} | 4 ++-- 9 files changed, 19 insertions(+), 19 deletions(-) rename moveit_ros/moveit_servo/test/launch/{jog_arm_drift_dimensions_service_test.test => servo_drift_dimensions_service_test.test} (84%) rename moveit_ros/moveit_servo/test/launch/{jog_arm_halt_msg_test.test => servo_halt_msg_test.test} (86%) rename moveit_ros/moveit_servo/test/launch/{jog_arm_msg_reception_test.test => servo_msg_reception_test.test} (85%) rename moveit_ros/moveit_servo/test/launch/{jog_arm_vel_accel_limits_test.test => servo_vel_accel_limits_test.test} (85%) rename moveit_ros/moveit_servo/test/python_tests/halt_msg/{test_jog_arm_halt_msg.py => test_servo_halt_msg.py} (97%) rename moveit_ros/moveit_servo/test/python_tests/msg_reception/{test_jog_arm_msg_reception.py => test_servo_msg_reception.py} (98%) rename moveit_ros/moveit_servo/test/{jog_cpp_interface_test.cpp => servo_cpp_interface_test.cpp} (99%) rename moveit_ros/moveit_servo/test/{jog_cpp_interface_test.test => servo_cpp_interface_test.test} (75%) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 7e673b3d78..c666edb037 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -134,17 +134,17 @@ if(CATKIN_ENABLE_TESTING) find_package(ros_pytest REQUIRED) # Python integration tests - add_rostest(test/launch/jog_arm_halt_msg_test.test) - add_rostest(test/launch/jog_arm_msg_reception_test.test) - add_rostest(test/launch/jog_arm_vel_accel_limits_test.test) - add_rostest(test/launch/jog_arm_drift_dimensions_service_test.test) - - # jog_cpp_interface - add_rostest_gtest(jog_cpp_interface_test - test/jog_cpp_interface_test.test - test/jog_cpp_interface_test.cpp + add_rostest(test/launch/servo_halt_msg_test.test) + add_rostest(test/launch/servo_msg_reception_test.test) + add_rostest(test/launch/servo_vel_accel_limits_test.test) + add_rostest(test/launch/servo_drift_dimensions_service_test.test) + + # servo_cpp_interface + add_rostest_gtest(servo_cpp_interface_test + test/servo_cpp_interface_test.test + test/servo_cpp_interface_test.cpp ) - target_link_libraries(jog_cpp_interface_test + target_link_libraries(servo_cpp_interface_test ${LIBRARY_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} diff --git a/moveit_ros/moveit_servo/test/launch/jog_arm_drift_dimensions_service_test.test b/moveit_ros/moveit_servo/test/launch/servo_drift_dimensions_service_test.test similarity index 84% rename from moveit_ros/moveit_servo/test/launch/jog_arm_drift_dimensions_service_test.test rename to moveit_ros/moveit_servo/test/launch/servo_drift_dimensions_service_test.test index 145255f44e..b349292a6d 100644 --- a/moveit_ros/moveit_servo/test/launch/jog_arm_drift_dimensions_service_test.test +++ b/moveit_ros/moveit_servo/test/launch/servo_drift_dimensions_service_test.test @@ -11,5 +11,5 @@ - + diff --git a/moveit_ros/moveit_servo/test/launch/jog_arm_halt_msg_test.test b/moveit_ros/moveit_servo/test/launch/servo_halt_msg_test.test similarity index 86% rename from moveit_ros/moveit_servo/test/launch/jog_arm_halt_msg_test.test rename to moveit_ros/moveit_servo/test/launch/servo_halt_msg_test.test index efc3c1ae2f..ef78ce1870 100644 --- a/moveit_ros/moveit_servo/test/launch/jog_arm_halt_msg_test.test +++ b/moveit_ros/moveit_servo/test/launch/servo_halt_msg_test.test @@ -11,5 +11,5 @@ - + diff --git a/moveit_ros/moveit_servo/test/launch/jog_arm_msg_reception_test.test b/moveit_ros/moveit_servo/test/launch/servo_msg_reception_test.test similarity index 85% rename from moveit_ros/moveit_servo/test/launch/jog_arm_msg_reception_test.test rename to moveit_ros/moveit_servo/test/launch/servo_msg_reception_test.test index b30ee1a8b7..38de3317a0 100644 --- a/moveit_ros/moveit_servo/test/launch/jog_arm_msg_reception_test.test +++ b/moveit_ros/moveit_servo/test/launch/servo_msg_reception_test.test @@ -11,5 +11,5 @@ - + diff --git a/moveit_ros/moveit_servo/test/launch/jog_arm_vel_accel_limits_test.test b/moveit_ros/moveit_servo/test/launch/servo_vel_accel_limits_test.test similarity index 85% rename from moveit_ros/moveit_servo/test/launch/jog_arm_vel_accel_limits_test.test rename to moveit_ros/moveit_servo/test/launch/servo_vel_accel_limits_test.test index e6e3707bd7..918556cd82 100644 --- a/moveit_ros/moveit_servo/test/launch/jog_arm_vel_accel_limits_test.test +++ b/moveit_ros/moveit_servo/test/launch/servo_vel_accel_limits_test.test @@ -11,5 +11,5 @@ - + diff --git a/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py b/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_servo_halt_msg.py similarity index 97% rename from moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py rename to moveit_ros/moveit_servo/test/python_tests/halt_msg/test_servo_halt_msg.py index 4641bc59a0..1f09573fc5 100755 --- a/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_jog_arm_halt_msg.py +++ b/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_servo_halt_msg.py @@ -17,7 +17,7 @@ # The servo node should halt and publish a warning. # Listen for a warning message from the servo node. # This can be run as part of a pytest, or like a normal ROS executable: -# rosrun moveit_servo test_jog_arm_halt_msg.py +# rosrun moveit_servo test_servo_halt_msg.py CARTESIAN_COMMAND_TOPIC = 'servo_server/delta_twist_cmds' diff --git a/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py b/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_servo_msg_reception.py similarity index 98% rename from moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py rename to moveit_ros/moveit_servo/test/python_tests/msg_reception/test_servo_msg_reception.py index a71f6c7a39..116f6892ad 100755 --- a/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_jog_arm_msg_reception.py +++ b/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_servo_msg_reception.py @@ -15,7 +15,7 @@ # Test that the servo node publishes controller commands when it receives Cartesian or joint commands. # This can be run as part of a pytest, or like a normal ROS executable: -# rosrun moveit_servo test_jog_arm_integration.py +# rosrun moveit_servo test_servo_integration.py JOINT_COMMAND_TOPIC = 'servo_server/delta_joint_cmds' CARTESIAN_COMMAND_TOPIC = 'servo_server/delta_twist_cmds' diff --git a/moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp similarity index 99% rename from moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp rename to moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp index 4b011e6f1c..4fd01fc73e 100644 --- a/moveit_ros/moveit_servo/test/jog_cpp_interface_test.cpp +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp @@ -49,7 +49,7 @@ #include #include -static const std::string LOGNAME = "jog_cpp_interface_test"; +static const std::string LOGNAME = "servo_cpp_interface_test"; namespace moveit_servo { diff --git a/moveit_ros/moveit_servo/test/jog_cpp_interface_test.test b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test similarity index 75% rename from moveit_ros/moveit_servo/test/jog_cpp_interface_test.test rename to moveit_ros/moveit_servo/test/servo_cpp_interface_test.test index 88d1215191..f34d8503da 100644 --- a/moveit_ros/moveit_servo/test/jog_cpp_interface_test.test +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test @@ -11,8 +11,8 @@ - - + + From 88021ddba4681d1c2b347614d89515dd1ebaa0ae Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Thu, 25 Jun 2020 10:00:13 -0500 Subject: [PATCH 370/612] Clang formatting --- moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 1b6bb31f90..569216238c 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -227,7 +227,8 @@ class ServoCalcs // incoming_joint_state_ is the incoming message. It may contain passive joints or other joints we don't care about. // (mutex protected below) // internal_joint_state_ is used in servo calculations. It shouldn't be relied on to be accurate. - // original_joint_state_ is the same as incoming_joint_state_ except it only contains the joints the servo node acts on. + // original_joint_state_ is the same as incoming_joint_state_ except it only contains the joints the servo node acts + // on. sensor_msgs::JointState internal_joint_state_, original_joint_state_; std::map joint_state_name_map_; From 9dcc93cf96b8910698a0848be9799c04a644e2af Mon Sep 17 00:00:00 2001 From: Constantinos Date: Sun, 28 Jun 2020 16:34:09 -0500 Subject: [PATCH 371/612] added hybridize/interpolate_flags (#2171) --- .../model_based_planning_context.h | 16 ++++++++++ .../src/model_based_planning_context.cpp | 29 ++++++++++++------- 2 files changed, 34 insertions(+), 11 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index 0ed6002e8e..d9792e506e 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -275,6 +275,16 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext simplify_solutions_ = flag; } + void setInterpolation(bool flag) + { + interpolate_ = flag; + } + + void setHybridize(bool flag) + { + hybridize_ = flag; + } + /* @brief Solve the planning problem. Return true if the problem is solved @param timeout The time to spend on solving @param count The number of runs to combine the paths of, in an attempt to generate better quality paths @@ -424,5 +434,11 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext ConstraintsLibraryPtr constraints_library_; bool simplify_solutions_; + + // if false the final solution is not interpolated + bool interpolate_; + + // if false parallel plan returns the first solution found + bool hybridize_; }; } // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 5cee993b16..2f9f9f5a1c 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -92,6 +92,8 @@ ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std:: , use_state_validity_cache_(true) , multi_query_planning_enabled_(false) // maintain "old" behavior by default , simplify_solutions_(true) + , interpolate_(true) + , hybridize_(true) { complete_initial_robot_state_.update(); @@ -670,7 +672,9 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion simplifySolution(request_.allowed_planning_time - ptime); ptime += getLastSimplifyTime(); } - interpolateSolution(); + + if (interpolate_) + interpolateSolution(); // fill the response ROS_DEBUG_NAMED("model_based_planning_context", "%s: Returning successful solution with %lu states", @@ -714,13 +718,16 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion getSolutionPath(*res.trajectory_.back()); } - ompl::time::point start_interpolate = ompl::time::now(); - interpolateSolution(); - res.processing_time_.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate)); - res.description_.emplace_back("interpolate"); - res.trajectory_.resize(res.trajectory_.size() + 1); - res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName())); - getSolutionPath(*res.trajectory_.back()); + if (interpolate_) + { + ompl::time::point start_interpolate = ompl::time::now(); + interpolateSolution(); + res.processing_time_.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate)); + res.description_.emplace_back("interpolate"); + res.trajectory_.resize(res.trajectory_.size() + 1); + res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName())); + getSolutionPath(*res.trajectory_.back()); + } // fill the response ROS_DEBUG_NAMED("model_based_planning_context", "%s: Returning successful solution with %lu states", @@ -768,7 +775,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); registerTerminationCondition(ptc); - result = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION; + result = ompl_parallel_plan_.solve(ptc, 1, count, hybridize_) == ompl::base::PlannerStatus::EXACT_SOLUTION; last_plan_time_ = ompl::time::seconds(ompl::time::now() - start); unregisterTerminationCondition(); } @@ -787,7 +794,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i else for (unsigned int i = 0; i < max_planning_threads_; ++i) ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal())); - bool r = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION; + bool r = ompl_parallel_plan_.solve(ptc, 1, count, hybridize_) == ompl::base::PlannerStatus::EXACT_SOLUTION; result = result && r; } n = count % max_planning_threads_; @@ -800,7 +807,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i else for (int i = 0; i < n; ++i) ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal())); - bool r = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION; + bool r = ompl_parallel_plan_.solve(ptc, 1, count, hybridize_) == ompl::base::PlannerStatus::EXACT_SOLUTION; result = result && r; } last_plan_time_ = ompl::time::seconds(ompl::time::now() - start); From 9daecb6ab59b8c97edaee59b34acff9c31900b21 Mon Sep 17 00:00:00 2001 From: Mark Moll Date: Mon, 29 Jun 2020 08:44:39 -0700 Subject: [PATCH 372/612] add support for setting interpolate_ and hybridize_ flags in ModelBasedPlanningContext via ompl_planning.yaml (#2172) --- .../src/model_based_planning_context.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 2f9f9f5a1c..657374192a 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -340,6 +340,22 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() if (it != cfg.end()) multi_query_planning_enabled_ = boost::lexical_cast(it->second); + // check whether the path returned by the planner should be interpolated + it = cfg.find("interpolate"); + if (it != cfg.end()) + { + interpolate_ = boost::lexical_cast(it->second); + cfg.erase(it); + } + + // check whether solution paths from parallel planning should be hybridized + it = cfg.find("hybridize"); + if (it != cfg.end()) + { + hybridize_ = boost::lexical_cast(it->second); + cfg.erase(it); + } + // remove the 'type' parameter; the rest are parameters for the planner itself it = cfg.find("type"); if (it == cfg.end()) From 966dee4031f475ecbce024237e9c13f084351228 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 1 Jul 2020 07:41:23 -0500 Subject: [PATCH 373/612] Minor moveit_servo header cleanup (#2173) --- .../moveit_servo/include/moveit_servo/collision_check.h | 2 -- .../include/moveit_servo/joint_state_subscriber.h | 3 +-- moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h | 6 +----- 3 files changed, 2 insertions(+), 9 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index 845b5dac84..6b9610d6ce 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -38,8 +38,6 @@ #pragma once -#include - #include #include #include diff --git a/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h index 48d362123a..8300134bab 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h @@ -50,8 +50,7 @@ namespace moveit_servo class JointStateSubscriber { public: - /** \brief Constructor - */ + /** \brief Constructor */ JointStateSubscriber(ros::NodeHandle& nh, const std::string& joint_state_topic_name); /** \brief Get the latest joint state message */ diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 569216238c..a8f676033a 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -38,9 +38,6 @@ #pragma once -// System -#include - // ROS #include #include @@ -185,8 +182,7 @@ class ServoCalcs bool changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, moveit_msgs::ChangeDriftDimensions::Response& res); - /** \brief Start the main calculation timer */ - // Service callback for changing servoing dimensions + /** \brief Service callback for changing servoing dimensions (e.g. ignore rotation about X) */ bool changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, moveit_msgs::ChangeControlDimensions::Response& res); From 349eb83228a37f65ba17023c7fc3ee2a38f341e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Thu, 2 Jul 2020 13:29:26 +0200 Subject: [PATCH 374/612] partially transition unused test bin to rostest (#2158) There is a lot of infrastructure missing to get this to actually run... - the plugin needs to be registered - pr2_moveit_config is not in moveit_resources --- .../CMakeLists.txt | 17 +++++++++++----- ...est_app.cpp => test_execution_manager.cpp} | 0 .../test/test_execution_manager.test | 20 +++++++++++++++++++ 3 files changed, 32 insertions(+), 5 deletions(-) rename moveit_ros/planning/trajectory_execution_manager/test/{test_app.cpp => test_execution_manager.cpp} (100%) create mode 100644 moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.test diff --git a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt index 14a2c23be8..280835786e 100644 --- a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt +++ b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt @@ -11,9 +11,16 @@ install(TARGETS ${MOVEIT_LIB_NAME} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) -add_library(test_controller_manager_plugin test/test_moveit_controller_manager_plugin.cpp) -set_target_properties(test_controller_manager_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(test_controller_manager_plugin ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +if(CATKIN_ENABLE_TESTING) -add_executable(test_controller_manager test/test_app.cpp) -target_link_libraries(test_controller_manager ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +## This needs further cleanup before it can run +# add_library(test_controller_manager_plugin test/test_moveit_controller_manager_plugin.cpp) +# set_target_properties(test_controller_manager_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +# target_link_libraries(test_controller_manager_plugin ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +# +# find_package(rostest REQUIRED) +# add_rostest_gtest(test_execution_manager +# test/test_execution_manager.test +# test/test_execution_manager.cpp) +# target_link_libraries(test_execution_manager ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +endif(CATKIN_ENABLE_TESTING) diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_app.cpp b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp similarity index 100% rename from moveit_ros/planning/trajectory_execution_manager/test/test_app.cpp rename to moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.test b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.test new file mode 100644 index 0000000000..ef1cab7521 --- /dev/null +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + From 5af72583ee1798ae2534aa66d727f7c3dbf13f1c Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Thu, 2 Jul 2020 23:56:43 +0900 Subject: [PATCH 375/612] Clean up Rviz plugin, fix mesh scaling (#2142) * Clarify object importing * Add suggestion (LARGE_MESH_THRESHOLD) * Add file filter * Add tooltip, clarify names * Fix and clarify names in robot states tab * Add dialogue to Clear button in robot states --- .../motion_planning_frame.h | 14 +++-- .../src/motion_planning_frame.cpp | 49 +++++++++++++--- .../src/motion_planning_frame_objects.cpp | 19 ++++--- .../src/motion_planning_frame_states.cpp | 17 +++++- .../ui/motion_planning_rviz_plugin_frame.ui | 56 +++++++++++++------ 5 files changed, 114 insertions(+), 41 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index a858fe2105..133a09fb1b 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -97,6 +97,8 @@ static const std::string TAB_SCENES = "Stored Scenes"; static const std::string TAB_STATES = "Stored States"; static const std::string TAB_STATUS = "Status"; +static const double LARGE_MESH_THRESHOLD = 10.0; + class MotionPlanningFrame : public QWidget { friend class MotionPlanningDisplay; @@ -172,8 +174,8 @@ private Q_SLOTS: void onClearOctomapClicked(); // Scene Objects tab - void importFileButtonClicked(); - void importUrlButtonClicked(); + void importObjectFromFileButtonClicked(); + void importObjectFromUrlButtonClicked(); void clearSceneButtonClicked(); void sceneScaleChanged(int value); void sceneScaleStartChange(); @@ -184,8 +186,8 @@ private Q_SLOTS: void collisionObjectChanged(QListWidgetItem* item); void imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback); void copySelectedCollisionObject(); - void exportAsTextButtonClicked(); - void importFromTextButtonClicked(); + void exportGeometryAsTextButtonClicked(); + void importGeometryFromTextButtonClicked(); // Stored scenes tab void saveSceneButtonClicked(); @@ -249,8 +251,8 @@ private Q_SLOTS: void renameCollisionObject(QListWidgetItem* item); void attachDetachCollisionObject(QListWidgetItem* item); void populateCollisionObjectsList(); - void computeImportFromText(const std::string& path); - void computeExportAsText(const std::string& path); + void computeImportGeometryFromText(const std::string& path); + void computeExportGeometryAsText(const std::string& path); visualization_msgs::InteractiveMarker createObjectMarkerMsg(const collision_detection::CollisionEnv::ObjectConstPtr& obj); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 3993bfd75f..80ea79e3db 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -105,8 +105,8 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: SLOT(planningAlgorithmIndexChanged(int))); connect(ui_->planning_algorithm_combo_box, SIGNAL(currentIndexChanged(int)), this, SLOT(planningAlgorithmIndexChanged(int))); - connect(ui_->import_file_button, SIGNAL(clicked()), this, SLOT(importFileButtonClicked())); - connect(ui_->import_url_button, SIGNAL(clicked()), this, SLOT(importUrlButtonClicked())); + connect(ui_->import_object_file_button, SIGNAL(clicked()), this, SLOT(importObjectFromFileButtonClicked())); + connect(ui_->import_object_url_button, SIGNAL(clicked()), this, SLOT(importObjectFromUrlButtonClicked())); connect(ui_->clear_scene_button, SIGNAL(clicked()), this, SLOT(clearSceneButtonClicked())); connect(ui_->scene_scale, SIGNAL(valueChanged(int)), this, SLOT(sceneScaleChanged(int))); connect(ui_->scene_scale, SIGNAL(sliderPressed()), this, SLOT(sceneScaleStartChange())); @@ -128,8 +128,8 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: connect(ui_->planning_scene_tree, SIGNAL(itemChanged(QTreeWidgetItem*, int)), this, SLOT(warehouseItemNameChanged(QTreeWidgetItem*, int))); connect(ui_->reset_db_button, SIGNAL(clicked()), this, SLOT(resetDbButtonClicked())); - connect(ui_->export_scene_text_button, SIGNAL(clicked()), this, SLOT(exportAsTextButtonClicked())); - connect(ui_->import_scene_text_button, SIGNAL(clicked()), this, SLOT(importFromTextButtonClicked())); + connect(ui_->export_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(exportGeometryAsTextButtonClicked())); + connect(ui_->import_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(importGeometryFromTextButtonClicked())); connect(ui_->load_state_button, SIGNAL(clicked()), this, SLOT(loadStateButtonClicked())); connect(ui_->save_start_state_button, SIGNAL(clicked()), this, SLOT(saveStartStateButtonClicked())); connect(ui_->save_goal_state_button, SIGNAL(clicked()), this, SLOT(saveGoalStateButtonClicked())); @@ -435,9 +435,6 @@ void MotionPlanningFrame::importResource(const std::string& path) { std::size_t slash = path.find_last_of("/\\"); std::string name = path.substr(slash + 1); - shapes::ShapeConstPtr shape(mesh); - Eigen::Isometry3d pose; - pose.setIdentity(); if (planning_display_->getPlanningSceneRO()->getCurrentState().hasAttachedBody(name)) { @@ -448,6 +445,42 @@ void MotionPlanningFrame::importResource(const std::string& path) return; } + // If the object is very large, ask the user if the scale should be reduced. + bool object_is_very_large = false; + for (unsigned int i = 0; i < mesh->vertex_count; i++) + { + if ((abs(mesh->vertices[i * 3 + 0]) > LARGE_MESH_THRESHOLD) || + (abs(mesh->vertices[i * 3 + 1]) > LARGE_MESH_THRESHOLD) || + (abs(mesh->vertices[i * 3 + 2]) > LARGE_MESH_THRESHOLD)) + { + object_is_very_large = true; + break; + } + } + if (object_is_very_large) + { + QMessageBox msg_box; + msg_box.setText( + "The object is very large (greater than 10 m). The file may be in millimeters instead of meters."); + msg_box.setInformativeText("Attempt to fix the size by shrinking the object?"); + msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No); + msg_box.setDefaultButton(QMessageBox::Yes); + if (msg_box.exec() == QMessageBox::Yes) + { + for (unsigned int i = 0; i < mesh->vertex_count; ++i) + { + unsigned int i3 = i * 3; + mesh->vertices[i3] *= 0.001; + mesh->vertices[i3 + 1] *= 0.001; + mesh->vertices[i3 + 2] *= 0.001; + } + } + } + + shapes::ShapeConstPtr shape(mesh); + Eigen::Isometry3d pose; + pose.setIdentity(); + // If the object already exists, ask the user whether to overwrite or rename if (planning_display_->getPlanningSceneRO()->getWorld()->hasObject(name)) { @@ -515,7 +548,7 @@ void MotionPlanningFrame::importResource(const std::string& path) } else { - QMessageBox::warning(this, QString("Import error"), QString("Unable to import scene")); + QMessageBox::warning(this, QString("Import error"), QString("Unable to import object")); return; } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 9405cd123b..9aa972c902 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -73,14 +73,15 @@ QString subframe_poses_to_qstring(const moveit::core::FixedTransformsMap& subfra namespace moveit_rviz_plugin { -void MotionPlanningFrame::importFileButtonClicked() +void MotionPlanningFrame::importObjectFromFileButtonClicked() { - QString path = QFileDialog::getOpenFileName(this, tr("Import Object")); + QString path = QFileDialog::getOpenFileName(this, tr("Import Object Mesh"), QString(), + "CAD files (*.stl *.obj *.dae);;All files (*.*)"); if (!path.isEmpty()) importResource("file://" + path.toStdString()); } -void MotionPlanningFrame::importUrlButtonClicked() +void MotionPlanningFrame::importObjectFromUrlButtonClicked() { bool ok = false; QString url = QInputDialog::getText(this, tr("Import Object"), tr("URL for file to import:"), QLineEdit::Normal, @@ -926,16 +927,16 @@ void MotionPlanningFrame::populateCollisionObjectsList() selectedCollisionObjectChanged(); } -void MotionPlanningFrame::exportAsTextButtonClicked() +void MotionPlanningFrame::exportGeometryAsTextButtonClicked() { QString path = QFileDialog::getSaveFileName(this, tr("Export Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)")); if (!path.isEmpty()) planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeExportAsText, this, path.toStdString()), "export as text"); + boost::bind(&MotionPlanningFrame::computeExportGeometryAsText, this, path.toStdString()), "export as text"); } -void MotionPlanningFrame::computeExportAsText(const std::string& path) +void MotionPlanningFrame::computeExportGeometryAsText(const std::string& path) { planning_scene_monitor::LockedPlanningSceneRO ps = planning_display_->getPlanningSceneRO(); if (ps) @@ -953,7 +954,7 @@ void MotionPlanningFrame::computeExportAsText(const std::string& path) } } -void MotionPlanningFrame::computeImportFromText(const std::string& path) +void MotionPlanningFrame::computeImportGeometryFromText(const std::string& path) { planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); if (ps) @@ -973,12 +974,12 @@ void MotionPlanningFrame::computeImportFromText(const std::string& path) } } -void MotionPlanningFrame::importFromTextButtonClicked() +void MotionPlanningFrame::importGeometryFromTextButtonClicked() { QString path = QFileDialog::getOpenFileName(this, tr("Import Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)")); if (!path.isEmpty()) planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeImportFromText, this, path.toStdString()), "import from text"); + boost::bind(&MotionPlanningFrame::computeImportGeometryFromText, this, path.toStdString()), "import from text"); } } // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp index 584f2329f1..3e554c6b9f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp @@ -245,8 +245,21 @@ void MotionPlanningFrame::removeStateButtonClicked() void MotionPlanningFrame::clearStatesButtonClicked() { - robot_states_.clear(); - populateRobotStatesList(); + QMessageBox msg_box; + msg_box.setText("Clear all stored robot states (from memory, not from the database)?"); + msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); + msg_box.setDefaultButton(QMessageBox::Yes); + int ret = msg_box.exec(); + switch (ret) + { + case QMessageBox::Yes: + { + robot_states_.clear(); + populateRobotStatesList(); + } + break; + } + return; } } // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 363fe0f478..568cccd2b0 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -1169,16 +1169,22 @@ - + - &Export As Text + &Export + + + Export scene geometry to a .scene file (only geometry information is preserved) - + - &Import From Text + &Import + + + Import scene geometry from a .scene file (only geometry information is preserved) @@ -1195,9 +1201,12 @@ - + - Import &URL + From &URL + + + Import a mesh from a URL @@ -1216,9 +1225,12 @@ - + - Import &File + Import &Mesh + + + Import a mesh from a file @@ -1402,13 +1414,16 @@ - Stored States + Stored Robot States - Filter + Load + + + Load a robot state @@ -1417,6 +1432,9 @@ Clear + + Clear robot states from list + @@ -1425,7 +1443,7 @@ - Selected State + Selected Robot State @@ -1455,7 +1473,7 @@ - Current State + Current Robot State @@ -1463,6 +1481,9 @@ Save Start + + Save the current start state + @@ -1470,6 +1491,9 @@ Save Goal + + Save the current goal state + @@ -1661,8 +1685,8 @@ roi_size_y roi_size_z collision_objects_list - import_file_button - import_url_button + import_object_file_button + import_object_url_button remove_object_button clear_scene_button object_x @@ -1673,8 +1697,8 @@ object_rz scene_scale publish_current_scene_button - export_scene_text_button - import_scene_text_button + export_scene_geometry_text_button + import_scene_geometry_text_button planning_scene_tree load_scene_button load_query_button From a617923cb7adc499aa7c2863ac0ddf597c431f9a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 28 Jun 2020 19:25:32 +0200 Subject: [PATCH 376/612] Prefer vendor-specific OpenGL library Co-authored-by: Henning Kayser --- moveit_ros/perception/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/moveit_ros/perception/CMakeLists.txt b/moveit_ros/perception/CMakeLists.txt index 74c908d4cd..44353476bb 100644 --- a/moveit_ros/perception/CMakeLists.txt +++ b/moveit_ros/perception/CMakeLists.txt @@ -16,6 +16,10 @@ endif() find_package(Boost REQUIRED thread) if(WITH_OPENGL) + # Prefer newer vendor-specific OpenGL library + if (POLICY CMP0072) + cmake_policy(SET CMP0072 NEW) + endif() find_package(OpenGL REQUIRED) find_package(GLEW REQUIRED) find_package(GLUT REQUIRED) From a15551f8df441865693dae5e81cf1f3e1679426d Mon Sep 17 00:00:00 2001 From: Patrick Beeson Date: Fri, 3 Jul 2020 07:42:57 -0500 Subject: [PATCH 377/612] Parameterize input trajectory density of Time Optimal trajectory generation (#2185) MoveIt wrapper to Time Optimal Trajectory Generation downsamples trajectory, but its hard coded to keep waypoints with at least 0.001 radians of motion on a joint. For very long and/or dense input trajectories, the downsampled trajectory is still way too large for the iterative optimization algorithm to finish in reasonable time. I was seeing 3000 point trajectory taking ~15 seconds. This fix allows the user to set this downsampling density in minimum radians of motion between two waypoints to something else (presumably higher like 0.5 degrees [0.00875 radians]) in order to drastically improve the speed of this algorithm. --- .../time_optimal_trajectory_generation.h | 4 +++- .../src/time_optimal_trajectory_generation.cpp | 7 ++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index b5b7c80fd4..0ee4d13f41 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -162,7 +162,8 @@ class Trajectory class TimeOptimalTrajectoryGeneration { public: - TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, const double resample_dt = 0.1); + TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, const double resample_dt = 0.1, + const double min_angle_change = 0.001); ~TimeOptimalTrajectoryGeneration(); bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, @@ -171,5 +172,6 @@ class TimeOptimalTrajectoryGeneration private: const double path_tolerance_; const double resample_dt_; + const double min_angle_change_; }; } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 076b1c482b..42a9108ea2 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -863,8 +863,9 @@ Eigen::VectorXd Trajectory::getAcceleration(double time) const return path_acc; } -TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(const double path_tolerance, const double resample_dt) - : path_tolerance_(path_tolerance), resample_dt_(resample_dt) +TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(const double path_tolerance, const double resample_dt, + const double min_angle_change) + : path_tolerance_(path_tolerance), resample_dt_(resample_dt), min_angle_change_(min_angle_change) { } @@ -965,7 +966,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT for (size_t j = 0; j < num_joints; j++) { new_point[j] = waypoint->getVariablePosition(idx[j]); - if (p > 0 && std::abs(new_point[j] - points.back()[j]) > 0.001) + if (p > 0 && std::abs(new_point[j] - points.back()[j]) > min_angle_change_) diverse_point = true; } From 73fa9ea640f507334052bab183163772f1d7b18d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 3 Jul 2020 11:12:19 -0500 Subject: [PATCH 378/612] Fix a parameter mix-up in moveit_cpp loading (#2187) --- .../moveit_cpp/include/moveit/moveit_cpp/planning_component.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 7a48c9446f..6aa2d0ca7d 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -118,8 +118,8 @@ class PlanningComponent std::string ns = "plan_request_params/"; nh.param(ns + "planner_id", planner_id, std::string("")); nh.param(ns + "planning_pipeline", planning_pipeline, std::string("")); - nh.param(ns + "planning_time", planning_attempts, 1); - nh.param(ns + "planning_attempts", planning_time, 5.0); + nh.param(ns + "planning_time", planning_time, 1.0); + nh.param(ns + "planning_attempts", planning_attempts, 5); nh.param(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor, 1.0); nh.param(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor, 1.0); } From 0274ea92df26c6c7ee29afc49b29d6835c1b3f69 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 3 Jul 2020 14:06:25 -0500 Subject: [PATCH 379/612] moveit_servo: delete python integration tests (#2186) which (mostly) are redundant to the corresponding C++ tests. --- moveit_ros/moveit_servo/CMakeLists.txt | 7 - moveit_ros/moveit_servo/package.xml | 1 - .../test/config/singular_position.yaml | 11 -- .../test/launch/common_test_setup.launch | 26 ---- .../servo_drift_dimensions_service_test.test | 15 -- .../test/launch/servo_halt_msg_test.test | 15 -- .../test/launch/servo_msg_reception_test.test | 15 -- .../launch/servo_vel_accel_limits_test.test | 15 -- .../test/python_tests/__init__.py | 0 .../test_drift_dimensions_service.py | 49 ------- .../halt_msg/test_servo_halt_msg.py | 72 ---------- .../msg_reception/test_servo_msg_reception.py | 129 ------------------ .../moveit_servo/test/python_tests/util.py | 14 -- .../vel_accel_limits/test_vel_accel_limits.py | 87 ------------ 14 files changed, 456 deletions(-) delete mode 100644 moveit_ros/moveit_servo/test/config/singular_position.yaml delete mode 100644 moveit_ros/moveit_servo/test/launch/common_test_setup.launch delete mode 100644 moveit_ros/moveit_servo/test/launch/servo_drift_dimensions_service_test.test delete mode 100644 moveit_ros/moveit_servo/test/launch/servo_halt_msg_test.test delete mode 100644 moveit_ros/moveit_servo/test/launch/servo_msg_reception_test.test delete mode 100644 moveit_ros/moveit_servo/test/launch/servo_vel_accel_limits_test.test delete mode 100644 moveit_ros/moveit_servo/test/python_tests/__init__.py delete mode 100755 moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py delete mode 100755 moveit_ros/moveit_servo/test/python_tests/halt_msg/test_servo_halt_msg.py delete mode 100755 moveit_ros/moveit_servo/test/python_tests/msg_reception/test_servo_msg_reception.py delete mode 100644 moveit_ros/moveit_servo/test/python_tests/util.py delete mode 100755 moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index c666edb037..37c15d7da2 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -131,13 +131,6 @@ install(DIRECTORY include/${PROJECT_NAME}/ if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - find_package(ros_pytest REQUIRED) - - # Python integration tests - add_rostest(test/launch/servo_halt_msg_test.test) - add_rostest(test/launch/servo_msg_reception_test.test) - add_rostest(test/launch/servo_vel_accel_limits_test.test) - add_rostest(test/launch/servo_drift_dimensions_service_test.test) # servo_cpp_interface add_rostest_gtest(servo_cpp_interface_test diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index 0e6b0f433b..bb7824858f 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -33,7 +33,6 @@ spacenav_node rostest - ros_pytest moveit_resources diff --git a/moveit_ros/moveit_servo/test/config/singular_position.yaml b/moveit_ros/moveit_servo/test/config/singular_position.yaml deleted file mode 100644 index 5ce15e651a..0000000000 --- a/moveit_ros/moveit_servo/test/config/singular_position.yaml +++ /dev/null @@ -1,11 +0,0 @@ -# This position is at a singularity -zeros: - panda_joint1: 0 - panda_joint2: 0 - panda_joint3: 0 - panda_joint4: 0 - panda_joint5: 0 - panda_joint6: 1.57 - panda_joint7: 1.57 -publish_default_positions: True - diff --git a/moveit_ros/moveit_servo/test/launch/common_test_setup.launch b/moveit_ros/moveit_servo/test/launch/common_test_setup.launch deleted file mode 100644 index 3d587e12e9..0000000000 --- a/moveit_ros/moveit_servo/test/launch/common_test_setup.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_ros/moveit_servo/test/launch/servo_drift_dimensions_service_test.test b/moveit_ros/moveit_servo/test/launch/servo_drift_dimensions_service_test.test deleted file mode 100644 index b349292a6d..0000000000 --- a/moveit_ros/moveit_servo/test/launch/servo_drift_dimensions_service_test.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/moveit_ros/moveit_servo/test/launch/servo_halt_msg_test.test b/moveit_ros/moveit_servo/test/launch/servo_halt_msg_test.test deleted file mode 100644 index ef78ce1870..0000000000 --- a/moveit_ros/moveit_servo/test/launch/servo_halt_msg_test.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/moveit_ros/moveit_servo/test/launch/servo_msg_reception_test.test b/moveit_ros/moveit_servo/test/launch/servo_msg_reception_test.test deleted file mode 100644 index 38de3317a0..0000000000 --- a/moveit_ros/moveit_servo/test/launch/servo_msg_reception_test.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/moveit_ros/moveit_servo/test/launch/servo_vel_accel_limits_test.test b/moveit_ros/moveit_servo/test/launch/servo_vel_accel_limits_test.test deleted file mode 100644 index 918556cd82..0000000000 --- a/moveit_ros/moveit_servo/test/launch/servo_vel_accel_limits_test.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/moveit_ros/moveit_servo/test/python_tests/__init__.py b/moveit_ros/moveit_servo/test/python_tests/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py b/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py deleted file mode 100755 index 9d44acb223..0000000000 --- a/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py +++ /dev/null @@ -1,49 +0,0 @@ -#!/usr/bin/env python -import time - -import pytest -import rospy - -from moveit_msgs.srv import ChangeDriftDimensions -from geometry_msgs.msg import Transform -from geometry_msgs.msg import TwistStamped -from std_msgs.msg import Bool -from trajectory_msgs.msg import JointTrajectory - -# Import common Python test utilities -from os import sys, path -sys.path.append(path.dirname(path.dirname(path.abspath(__file__)))) -import util - -# Send a service call to allow drift in all but the y-dimension. -# In other words, only the y-dimension will be controlled exactly. -# Check that the service returns and the servo node continues to publish commands to the robot. - -CARTESIAN_COMMAND_TOPIC = 'servo_server/delta_twist_cmds' - -COMMAND_OUT_TOPIC = 'servo_server/command' - -SERVICE_NAME = 'servo_server/change_drift_dimensions' - - -@pytest.fixture -def node(): - return rospy.init_node('pytest', anonymous=True) - - -def test_drift_dimensions_service(node): - assert util.wait_for_servo_initialization() - - # Service to change drift dimensions - drift_service = rospy.ServiceProxy(SERVICE_NAME, ChangeDriftDimensions) - - # Service call to allow drift in all dimensions except y-translation - # The transform is an identity matrix, not used for now - drift_response = drift_service(True, False, True, True, True, True, Transform()) - # Check for successful response - assert drift_response.success == True - - -if __name__ == '__main__': - node = node() - test_drift_dimensions_service(node) diff --git a/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_servo_halt_msg.py b/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_servo_halt_msg.py deleted file mode 100755 index 1f09573fc5..0000000000 --- a/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_servo_halt_msg.py +++ /dev/null @@ -1,72 +0,0 @@ -#!/usr/bin/env python -import time - -import pytest -import rospy -from geometry_msgs.msg import TwistStamped - -from control_msgs.msg import JointJog -from std_msgs.msg import Int8 - -# Import common Python test utilities -from os import sys, path -sys.path.append(path.dirname(path.dirname(path.abspath(__file__)))) -import util - -# The robot starts at a singular position (see config file). -# The servo node should halt and publish a warning. -# Listen for a warning message from the servo node. -# This can be run as part of a pytest, or like a normal ROS executable: -# rosrun moveit_servo test_servo_halt_msg.py - -CARTESIAN_COMMAND_TOPIC = 'servo_server/delta_twist_cmds' - -# servo should publish a nonzero warning code here -STATUS_TOPIC = 'servo_server/status' - - -@pytest.fixture -def node(): - return rospy.init_node('pytest', anonymous=True) - - -class CartesianCmd(object): - def __init__(self): - self._pub = rospy.Publisher( - CARTESIAN_COMMAND_TOPIC, TwistStamped, queue_size=10 - ) - - def send_cmd(self, linear, angular): - ts = TwistStamped() - ts.header.stamp = rospy.Time.now() - ts.twist.linear.x, ts.twist.linear.y, ts.twist.linear.z = linear - ts.twist.angular.x, ts.twist.angular.y, ts.twist.angular.z = angular - self._pub.publish(ts) - - -def test_servo_halt_msg(node): - assert util.wait_for_servo_initialization() - - received = [] - sub = rospy.Subscriber( - STATUS_TOPIC, Int8, lambda msg: received.append(msg) - ) - cartesian_cmd = CartesianCmd() - - # This nonzero command should produce servoing output - # A subscriber in a different timer fills `received` - TEST_DURATION = 1 - start_time = rospy.get_rostime() - received = [] - while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION: - cartesian_cmd.send_cmd([1, 1, 1], [0, 0, 1]) - time.sleep(0.1) - - # Check the received messages - # A non-zero value signifies a warning - assert len(received) > 3 - assert any(i != 0 for i in received[-3:]) - -if __name__ == '__main__': - node = node() - test_servo_halt_msg(node) diff --git a/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_servo_msg_reception.py b/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_servo_msg_reception.py deleted file mode 100755 index 116f6892ad..0000000000 --- a/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_servo_msg_reception.py +++ /dev/null @@ -1,129 +0,0 @@ -#!/usr/bin/env python -import time - -import pytest -import rospy -from geometry_msgs.msg import TwistStamped - -from control_msgs.msg import JointJog -from trajectory_msgs.msg import JointTrajectory - -# Import common Python test utilities -from os import sys, path -sys.path.append(path.dirname(path.dirname(path.abspath(__file__)))) -import util - -# Test that the servo node publishes controller commands when it receives Cartesian or joint commands. -# This can be run as part of a pytest, or like a normal ROS executable: -# rosrun moveit_servo test_servo_integration.py - -JOINT_COMMAND_TOPIC = 'servo_server/delta_joint_cmds' -CARTESIAN_COMMAND_TOPIC = 'servo_server/delta_twist_cmds' - -COMMAND_OUT_TOPIC = 'servo_server/command' - - -@pytest.fixture -def node(): - return rospy.init_node('pytest', anonymous=True) - - -class JointCmd(object): - def __init__(self): - self._pub = rospy.Publisher(JOINT_COMMAND_TOPIC, JointJog, queue_size=10) - - def send_joint_velocity_cmd(self, joint_pos): - jj = JointJog() - jj.header.stamp = rospy.Time.now() - jj.joint_names = ['joint_{}'.format(i) for i in range(len(joint_pos))] - jj.velocities = list(map(float, joint_pos)) - self._pub.publish(jj) - - -class CartesianCmd(object): - def __init__(self): - self._pub = rospy.Publisher( - CARTESIAN_COMMAND_TOPIC, TwistStamped, queue_size=10 - ) - - def send_cmd(self, linear, angular): - ts = TwistStamped() - ts.header.stamp = rospy.Time.now() - ts.twist.linear.x, ts.twist.linear.y, ts.twist.linear.z = linear - ts.twist.angular.x, ts.twist.angular.y, ts.twist.angular.z = angular - self._pub.publish(ts) - - -def test_servo_cartesian_command(node): - # Test sending a cartesian velocity command - - assert util.wait_for_servo_initialization() - - received = [] - sub = rospy.Subscriber( - COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) - ) - cartesian_cmd = CartesianCmd() - - # Repeated zero-commands should produce no output, other than a few halt messages - # A subscriber in a different timer fills 'received' - for i in range(4): - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) - rospy.sleep(0.1) - received = [] - rospy.sleep(1) - assert len(received) <= 4 # 'num_outgoing_halt_msgs_to_publish' in the config file - - # This nonzero command should produce servoing output - # A subscriber in a different timer fills `received` - TEST_DURATION = 1 - PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from servo config file - - # Send a command to start the servo node - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) - - start_time = rospy.get_rostime() - received = [] - while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION: - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) - time.sleep(0.1) - # TEST_DURATION/PUBLISH_PERIOD is the expected number of messages in this duration. - # Allow a small +/- window due to rounding/timing errors - assert len(received) >= TEST_DURATION/PUBLISH_PERIOD - 20 - assert len(received) <= TEST_DURATION/PUBLISH_PERIOD + 20 - - -def test_servo_joint_command(node): - # Test sending a joint command - - assert util.wait_for_servo_initialization() - - received = [] - sub = rospy.Subscriber( - COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) - ) - joint_cmd = JointCmd() - - TEST_DURATION = 1 - PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from servo config file - velocities = [0.1] - - # Send a command to start the servo node - joint_cmd.send_joint_velocity_cmd(velocities) - - start_time = rospy.get_rostime() - received = [] - while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION: - joint_cmd.send_joint_velocity_cmd(velocities) - time.sleep(0.1) - # TEST_DURATION/PUBLISH_PERIOD is the expected number of messages in this duration. - # Allow a small +/- window due to rounding/timing errors - assert len(received) >= TEST_DURATION/PUBLISH_PERIOD - 20 - assert len(received) <= TEST_DURATION/PUBLISH_PERIOD + 20 - - -if __name__ == '__main__': - node = node() - time.sleep(SERVO_SETTLE_TIME_S) # wait for servo server to init - test_servo_cartesian_command(node) - test_servo_joint_command(node) diff --git a/moveit_ros/moveit_servo/test/python_tests/util.py b/moveit_ros/moveit_servo/test/python_tests/util.py deleted file mode 100644 index f813e8dea7..0000000000 --- a/moveit_ros/moveit_servo/test/python_tests/util.py +++ /dev/null @@ -1,14 +0,0 @@ -import rospy -from std_msgs.msg import Int8 - -# servo should publish a nonzero warning code here -STATUS_TOPIC = 'servo_server/status' - -def wait_for_servo_initialization(timeout=15): - try: - rospy.wait_for_message(STATUS_TOPIC, Int8, timeout=timeout) - except rospy.ROSException as exc: - rospy.logerr("The servo topic " + STATUS_TOPIC + " is not available: " + str(exc)) - return False - - return True diff --git a/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py b/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py deleted file mode 100755 index 38dcdb2b8f..0000000000 --- a/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python -import time - -import pytest -import rospy - -from control_msgs.msg import JointJog -from trajectory_msgs.msg import JointTrajectory - -# Import common Python test utilities -from os import sys, path -sys.path.append(path.dirname(path.dirname(path.abspath(__file__)))) -import util - -# Test that commands that are too fast are caught and flagged -# This can be run as part of a pytest, or like a normal ROS executable: -# rosrun moveit_servo test_vel_accel_limits.py - -JOINT_COMMAND_TOPIC = 'servo_server/delta_joint_cmds' - -COMMAND_OUT_TOPIC = 'servo_server/command' - - -@pytest.fixture -def node(): - return rospy.init_node('pytest', anonymous=True) - - -class JointServoCmd(object): - def __init__(self): - self._pub = rospy.Publisher(JOINT_COMMAND_TOPIC, JointJog, queue_size=10) - - def send_joint_velocity_cmd(self, joint_pos): - jj = JointJog() - jj.header.stamp = rospy.Time.now() - jj.joint_names = ['joint_{}'.format(i) for i in range(len(joint_pos))] - jj.velocities = list(map(float, joint_pos)) - self._pub.publish(jj) - - -def test_vel_limit(node): - # Test sending a joint command - - assert util.wait_for_servo_initialization() - - received = [] - sub = rospy.Subscriber( - COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) - ) - - joint_cmd = JointServoCmd() - - TEST_DURATION = 1 - PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from servo config file - - # Panda arm limit, from joint_limits.yaml - VELOCITY_LIMIT = rospy.get_param("/robot_description_planning/joint_limits/panda_joint1/max_velocity") - # Send a velocity command that exceeds the limit - velocities = [10 * VELOCITY_LIMIT] - - # Send a command to start the servo node - joint_cmd.send_joint_velocity_cmd(velocities) - - start_time = rospy.get_rostime() - received = [] - while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION: - joint_cmd.send_joint_velocity_cmd(velocities) - time.sleep(0.1) - - # Period of outgoing commands from the servo node, from yaml - SERVO_COMMAND_PERIOD = rospy.get_param("/servo_server/publish_period") - - # Should be no velocities greater than the limit - assert len(received) > 2 - for msg_idx in range(1, len(received)): - try: - velocity = \ - (received[msg_idx].points[0].positions[0] - received[msg_idx - 1].points[0].positions[0]) / SERVO_COMMAND_PERIOD - assert abs(velocity) <= VELOCITY_LIMIT - except IndexError: - # Sometimes a message doesn't have any points - pass - -if __name__ == '__main__': - node = node() - test_vel_limit(node) - # TODO(andyz): add an acceleration limit test (the Panda joint_limits.yaml doesn't define acceleration limits) From 93cdeabf2c774d1a8066a3736e47ebdc68ec11c7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 28 Jun 2020 19:33:48 +0200 Subject: [PATCH 380/612] use SYSTEM includes to suppress external warnings --- moveit_core/CMakeLists.txt | 17 ++++++++--------- moveit_core/constraint_samplers/CMakeLists.txt | 2 +- moveit_kinematics/CMakeLists.txt | 7 ++++--- .../templates/CMakeLists.txt | 3 ++- moveit_kinematics/test/CMakeLists.txt | 3 ++- .../chomp/chomp_interface/CMakeLists.txt | 4 ++-- .../chomp/chomp_motion_planner/CMakeLists.txt | 3 ++- .../chomp_optimizer_adapter/CMakeLists.txt | 2 +- moveit_planners/ompl/CMakeLists.txt | 5 +++-- .../sbpl/core/sbpl_interface/CMakeLists.txt | 2 +- .../sbpl/ros/sbpl_interface_ros/CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../moveit_ros_control_interface/CMakeLists.txt | 3 ++- .../CMakeLists.txt | 3 ++- moveit_ros/benchmarks/CMakeLists.txt | 3 ++- moveit_ros/manipulation/CMakeLists.txt | 4 ++-- moveit_ros/move_group/CMakeLists.txt | 3 ++- moveit_ros/moveit_servo/CMakeLists.txt | 4 ++-- moveit_ros/occupancy_map_monitor/CMakeLists.txt | 7 +++---- moveit_ros/perception/CMakeLists.txt | 6 +++--- moveit_ros/planning/CMakeLists.txt | 8 +++----- moveit_ros/planning_interface/CMakeLists.txt | 6 +++--- moveit_ros/robot_interaction/CMakeLists.txt | 7 ++++--- moveit_ros/visualization/CMakeLists.txt | 6 +++--- moveit_ros/warehouse/CMakeLists.txt | 2 +- moveit_setup_assistant/CMakeLists.txt | 5 +++-- 26 files changed, 63 insertions(+), 56 deletions(-) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 04a960fd48..05c127fcde 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -172,20 +172,19 @@ catkin_package( ${BULLET_ENABLE} ) -include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${urdfdom_INCLUDE_DIRS} + ${urdfdom_headers_INCLUDE_DIRS} ${LIBFCL_INCLUDE_DIRS} ${BULLET_INCLUDE_DIRS} - ) + ${OCTOMAP_INCLUDE_DIRS} + ) #catkin_lint: ignore_once external_directory (${VERSION_FILE_PATH}) include_directories(${THIS_PACKAGE_INCLUDE_DIRS} - ${VERSION_FILE_PATH} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${urdfdom_INCLUDE_DIRS} - ${urdfdom_headers_INCLUDE_DIRS} - ${OCTOMAP_INCLUDE_DIRS} - ) + ${VERSION_FILE_PATH}) # Generate and install version.h string(REGEX REPLACE "^([0-9]+)\\..*" "\\1" MOVEIT_VERSION_MAJOR "${${PROJECT_NAME}_VERSION}") diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index 958e37b563..0cb5fab70e 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -30,7 +30,7 @@ if(CATKIN_ENABLE_TESTING) find_package(tf2_kdl REQUIRED) find_package(moveit_resources REQUIRED) - include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS}) + include_directories(SYSTEM ${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS}) catkin_add_gtest(test_constraint_samplers test/test_constraint_samplers.cpp diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index 1ba7d8f56f..dfe9e7342d 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/moveit_kinematics/CMakeLists.txt @@ -41,11 +41,12 @@ catkin_package( EIGEN3 ) -include_directories(${THIS_PACKAGE_INCLUDE_DIRS} +include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) +include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) + ) add_subdirectory(kdl_kinematics_plugin) add_subdirectory(lma_kinematics_plugin) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt index 71bc2f0442..9c19bba37a 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt @@ -17,7 +17,8 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(LAPACK REQUIRED) -include_directories(${catkin_INCLUDE_DIRS} include) +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) catkin_package() diff --git a/moveit_kinematics/test/CMakeLists.txt b/moveit_kinematics/test/CMakeLists.txt index dd05543291..1b8dc7139e 100644 --- a/moveit_kinematics/test/CMakeLists.txt +++ b/moveit_kinematics/test/CMakeLists.txt @@ -4,7 +4,8 @@ if(CATKIN_ENABLE_TESTING) find_package(xmlrpcpp REQUIRED) find_package(moveit_ros_planning REQUIRED) - include_directories(${xmlrpcpp_INCLUDE_DIRS} ${moveit_ros_planning_INCLUDE_DIRS}) + include_directories(${moveit_ros_planning_INCLUDE_DIRS}) + include_directories(SYSTEM ${xmlrpcpp_INCLUDE_DIRS}) catkin_add_executable_with_gtest(test_kinematics_plugin test_kinematics_plugin.cpp) target_link_libraries(test_kinematics_plugin ${catkin_LIBRARIES} ${moveit_ros_planning_LIBRARIES} ${xmlrpcpp_LIBRARIES}) diff --git a/moveit_planners/chomp/chomp_interface/CMakeLists.txt b/moveit_planners/chomp/chomp_interface/CMakeLists.txt index 3c49159201..bacee69953 100644 --- a/moveit_planners/chomp/chomp_interface/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_interface/CMakeLists.txt @@ -32,8 +32,8 @@ catkin_package( CATKIN_DEPENDS roscpp moveit_core pluginlib ) -include_directories( - include +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} diff --git a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt index 3aa69715fb..ed698c3336 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt @@ -17,7 +17,8 @@ catkin_package( LIBRARIES ${PROJECT_NAME} ) -include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) add_library(${PROJECT_NAME} src/chomp_cost.cpp diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt index a08a00e79e..d17f10a72a 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package() -include_directories(${catkin_INCLUDE_DIRS}) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) add_library(${PROJECT_NAME} src/chomp_optimizer_adapter.cpp) set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") diff --git a/moveit_planners/ompl/CMakeLists.txt b/moveit_planners/ompl/CMakeLists.txt index e9e745df30..16c3c8a544 100644 --- a/moveit_planners/ompl/CMakeLists.txt +++ b/moveit_planners/ompl/CMakeLists.txt @@ -40,9 +40,10 @@ catkin_package( OMPL ) -include_directories(ompl_interface/include - ${Boost_INCLUDE_DIRS} +include_directories(ompl_interface/include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${OMPL_INCLUDE_DIRS}) add_subdirectory(ompl_interface) diff --git a/moveit_planners/sbpl/core/sbpl_interface/CMakeLists.txt b/moveit_planners/sbpl/core/sbpl_interface/CMakeLists.txt index b93b07f3fc..1b436f0143 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/CMakeLists.txt +++ b/moveit_planners/sbpl/core/sbpl_interface/CMakeLists.txt @@ -13,7 +13,7 @@ rosbuild_init() find_package(PkgConfig REQUIRED) pkg_check_modules(SBPL REQUIRED sbpl) -include_directories(${SBPL_INCLUDE_DIRS}) +include_directories(SYSTEM ${SBPL_INCLUDE_DIRS}) link_directories(${SBPL_LIBRARY_DIRS}) #set the default path for built executables to the "bin" directory diff --git a/moveit_planners/sbpl/ros/sbpl_interface_ros/CMakeLists.txt b/moveit_planners/sbpl/ros/sbpl_interface_ros/CMakeLists.txt index 557e38a8fd..edc0cf34bc 100644 --- a/moveit_planners/sbpl/ros/sbpl_interface_ros/CMakeLists.txt +++ b/moveit_planners/sbpl/ros/sbpl_interface_ros/CMakeLists.txt @@ -12,7 +12,7 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) rosbuild_init() find_package(Eigen REQUIRED) -include_directories(${EIGEN_INCLUDE_DIRS}) +include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS}) #set the default path for built executables to the "bin" directory set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) diff --git a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt index 2d4c43f82b..5ffd372631 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt @@ -19,7 +19,7 @@ find_package(catkin COMPONENTS roscpp REQUIRED) -include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) catkin_package( LIBRARIES diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt index 04d702e1d1..8408cc163a 100644 --- a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt +++ b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt @@ -31,7 +31,8 @@ catkin_package( DEPENDS Boost ) -include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) add_library(${PROJECT_NAME}_plugin src/controller_manager_plugin.cpp diff --git a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt index b338d15d5f..b89705f3ac 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt @@ -21,7 +21,8 @@ find_package(catkin COMPONENTS control_msgs REQUIRED) -include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) catkin_package( LIBRARIES diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt index 3bae2ed4e0..0eb7714e85 100644 --- a/moveit_ros/benchmarks/CMakeLists.txt +++ b/moveit_ros/benchmarks/CMakeLists.txt @@ -34,7 +34,8 @@ catkin_package( INCLUDE_DIRS include ) -include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) add_library(${MOVEIT_LIB_NAME} src/BenchmarkOptions.cpp src/BenchmarkExecutor.cpp) diff --git a/moveit_ros/manipulation/CMakeLists.txt b/moveit_ros/manipulation/CMakeLists.txt index 0cf81cd7bd..5a6d452a53 100644 --- a/moveit_ros/manipulation/CMakeLists.txt +++ b/moveit_ros/manipulation/CMakeLists.txt @@ -48,9 +48,9 @@ catkin_package( include_directories(pick_place/include) include_directories(move_group_pick_place_capability/include) -include_directories(${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS}) include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) add_subdirectory(pick_place) diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index 5ec56208e7..b5f89fb410 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -39,7 +39,8 @@ catkin_package( ) include_directories(include) -include_directories(${catkin_INCLUDE_DIRS} +include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) add_library(moveit_move_group_capabilities_base diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index c666edb037..25f4b10db1 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -45,8 +45,8 @@ catkin_package( EIGEN3 ) -include_directories( - include +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} diff --git a/moveit_ros/occupancy_map_monitor/CMakeLists.txt b/moveit_ros/occupancy_map_monitor/CMakeLists.txt index 1390c52719..c62d9c5551 100644 --- a/moveit_ros/occupancy_map_monitor/CMakeLists.txt +++ b/moveit_ros/occupancy_map_monitor/CMakeLists.txt @@ -44,11 +44,10 @@ catkin_package( OCTOMAP ) -include_directories(include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ) +include_directories(include) include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${X11_INCLUDE_DIR} ) diff --git a/moveit_ros/perception/CMakeLists.txt b/moveit_ros/perception/CMakeLists.txt index 44353476bb..73f692e687 100644 --- a/moveit_ros/perception/CMakeLists.txt +++ b/moveit_ros/perception/CMakeLists.txt @@ -85,12 +85,12 @@ include_directories(lazy_free_space_updater/include pointcloud_octomap_updater/include semantic_world/include ${perception_GL_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ) include_directories(SYSTEM - ${OpenCV_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ${SYSTEM_GL_INCLUDE_DIR} ${X11_INCLUDE_DIR} ) diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 5c95e7df0b..eeab8f238d 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -78,13 +78,11 @@ catkin_package( EIGEN3 ) -include_directories(${THIS_PACKAGE_INCLUDE_DIRS} +include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} -) -include_directories(SYSTEM - ${EIGEN3_INCLUDE_DIRS} -) + ${EIGEN3_INCLUDE_DIRS}) add_subdirectory(rdf_loader) add_subdirectory(collision_plugin_loader) diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 879cb6a399..bcc6070c88 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -82,11 +82,11 @@ catkin_package( EIGEN3 ) -include_directories(${THIS_PACKAGE_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS}) +include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS}) diff --git a/moveit_ros/robot_interaction/CMakeLists.txt b/moveit_ros/robot_interaction/CMakeLists.txt index c896af71c2..498b8e1c5d 100644 --- a/moveit_ros/robot_interaction/CMakeLists.txt +++ b/moveit_ros/robot_interaction/CMakeLists.txt @@ -36,9 +36,10 @@ catkin_package( tf2_geometry_msgs ) -include_directories(include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS}) +include_directories(include) +include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS}) add_library(${MOVEIT_LIB_NAME} src/interactive_marker_helpers.cpp diff --git a/moveit_ros/visualization/CMakeLists.txt b/moveit_ros/visualization/CMakeLists.txt index 51e5011804..9bfa70b1d5 100644 --- a/moveit_ros/visualization/CMakeLists.txt +++ b/moveit_ros/visualization/CMakeLists.txt @@ -95,11 +95,11 @@ include_directories(rviz_plugin_render_tools/include robot_state_rviz_plugin/include planning_scene_rviz_plugin/include motion_planning_rviz_plugin/include - trajectory_rviz_plugin/include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS}) + trajectory_rviz_plugin/include) include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${QT_INCLUDE_DIR}) diff --git a/moveit_ros/warehouse/CMakeLists.txt b/moveit_ros/warehouse/CMakeLists.txt index e8a615e079..d974831537 100644 --- a/moveit_ros/warehouse/CMakeLists.txt +++ b/moveit_ros/warehouse/CMakeLists.txt @@ -34,6 +34,6 @@ catkin_package( ) include_directories(warehouse/include) -include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) add_subdirectory(warehouse) diff --git a/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/CMakeLists.txt index 2a60903728..fbac89054f 100644 --- a/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/CMakeLists.txt @@ -31,7 +31,8 @@ find_package(catkin REQUIRED COMPONENTS srdfdom ) find_package(ompl REQUIRED) -include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${OMPL_INCLUDE_DIRS}) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${OMPL_INCLUDE_DIRS}) +include_directories(include) # Qt Stuff if(rviz_QT_VERSION VERSION_LESS "5") @@ -48,7 +49,7 @@ add_definitions(-DQT_NO_KEYWORDS) # Support new yaml-cpp API. find_package(PkgConfig) pkg_check_modules(YAMLCPP REQUIRED yaml-cpp>=0.5) -include_directories(${YAMLCPP_INCLUDE_DIRS}) +include_directories(SYSTEM ${YAMLCPP_INCLUDE_DIRS}) catkin_package( INCLUDE_DIRS From f21bcae714a017e0217651ed1aa2a361b70ae845 Mon Sep 17 00:00:00 2001 From: Jorge Nicho Date: Tue, 2 Jun 2020 15:56:44 -0500 Subject: [PATCH 381/612] rviz panel: widgets for adding geometric primitives --- .../motion_planning_frame.h | 7 + .../src/motion_planning_frame.cpp | 69 ++++++ .../src/motion_planning_frame_objects.cpp | 40 ++++ .../ui/motion_planning_rviz_plugin_frame.ui | 213 ++++++++++++------ 4 files changed, 265 insertions(+), 64 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 133a09fb1b..37452ea6e6 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -99,6 +99,11 @@ static const std::string TAB_STATUS = "Status"; static const double LARGE_MESH_THRESHOLD = 10.0; +static const std::map SHAPES_MAP = { { "box", shapes::BOX }, + { "sphere", shapes::SPHERE }, + { "cone", shapes::CONE }, + { "cylinder", shapes::CYLINDER } }; + class MotionPlanningFrame : public QWidget { friend class MotionPlanningDisplay; @@ -174,6 +179,7 @@ private Q_SLOTS: void onClearOctomapClicked(); // Scene Objects tab + void shapesComboBoxChanged(const QString& text); void importObjectFromFileButtonClicked(); void importObjectFromUrlButtonClicked(); void clearSceneButtonClicked(); @@ -306,6 +312,7 @@ private Q_SLOTS: ros::Subscriber update_custom_goal_state_subscriber_; // General void changePlanningGroupHelper(); + void addPrimitiveShape(); void importResource(const std::string& path); void loadStoredStates(const std::string& pattern); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 80ea79e3db..47e680e667 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -128,6 +128,9 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: connect(ui_->planning_scene_tree, SIGNAL(itemChanged(QTreeWidgetItem*, int)), this, SLOT(warehouseItemNameChanged(QTreeWidgetItem*, int))); connect(ui_->reset_db_button, SIGNAL(clicked()), this, SLOT(resetDbButtonClicked())); + + connect(ui_->add_shape_button, &QPushButton::clicked, this, &MotionPlanningFrame::addPrimitiveShape); + connect(ui_->shapes_combo_box, &QComboBox::currentTextChanged, this, &MotionPlanningFrame::shapesComboBoxChanged); connect(ui_->export_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(exportGeometryAsTextButtonClicked())); connect(ui_->import_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(importGeometryFromTextButtonClicked())); connect(ui_->load_state_button, SIGNAL(clicked()), this, SLOT(loadStateButtonClicked())); @@ -426,6 +429,72 @@ void MotionPlanningFrame::sceneUpdate(planning_scene_monitor::PlanningSceneMonit planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); } +void MotionPlanningFrame::addPrimitiveShape() +{ + static const double MIN_VAL = 1e-6; + + if (!planning_display_->getPlanningSceneMonitor()) + { + return; + } + + // get size values + double x_length = ui_->shape_size_x_spin_box->isEnabled() ? ui_->shape_size_x_spin_box->value() : MIN_VAL; + double y_length = ui_->shape_size_y_spin_box->isEnabled() ? ui_->shape_size_y_spin_box->value() : MIN_VAL; + double z_length = ui_->shape_size_z_spin_box->isEnabled() ? ui_->shape_size_z_spin_box->value() : MIN_VAL; + if (x_length < MIN_VAL || y_length < MIN_VAL || z_length < MIN_VAL) + { + QMessageBox::warning(this, QString("Side length value is too small"), + QString("Value needs to be >= '").append(std::to_string(MIN_VAL).c_str()).append("'")); + return; + } + + // get selected shape + std::string selected_shape = ui_->shapes_combo_box->currentText().toStdString(); + if (SHAPES_MAP.count(selected_shape) == 0) + { + QMessageBox::warning(this, QString("Unsupported shape"), + QString("The '%1' shape is not supported.").arg(selected_shape.c_str())); + return; + } + shapes::ShapeType shape_type = SHAPES_MAP.at(selected_shape); + shapes::ShapeConstPtr shape; + switch (shape_type) + { + case shapes::BOX: + shape = std::make_shared(x_length, y_length, z_length); + break; + case shapes::SPHERE: + shape = std::make_shared(0.5 * x_length); + break; + case shapes::CONE: + shape = std::make_shared(0.5 * x_length, z_length); + break; + case shapes::CYLINDER: + shape = std::make_shared(0.5 * x_length, z_length); + break; + default: + QMessageBox::warning(this, QString("Unsupported shape"), + QString("The '%1' is not supported.").arg(selected_shape.c_str())); + } + + // naming object + int idx = 0; + std::string shape_name = selected_shape + "_" + std::to_string(idx); + while (planning_display_->getPlanningSceneRO()->getWorld()->hasObject(shape_name)) + { + idx++; + shape_name = selected_shape + "_" + std::to_string(idx); + } + + // adding object + Eigen::Isometry3d pose; + pose.setIdentity(); + planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); + if (ps) + addObject(ps->getWorldNonConst(), shape_name, shape, pose); +} + void MotionPlanningFrame::importResource(const std::string& path) { if (planning_display_->getPlanningSceneMonitor()) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 9aa972c902..fb8f9d20a2 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -73,6 +73,46 @@ QString subframe_poses_to_qstring(const moveit::core::FixedTransformsMap& subfra namespace moveit_rviz_plugin { +void MotionPlanningFrame::shapesComboBoxChanged(const QString& text) +{ + const std::string selected_shape = text.toStdString(); + if (SHAPES_MAP.find(selected_shape) == SHAPES_MAP.end()) + { + return; + } + + shapes::ShapeType shape_type = SHAPES_MAP.at(selected_shape); + switch (shape_type) + { + case shapes::BOX: + ui_->shape_size_x_spin_box->setEnabled(true); + ui_->shape_size_y_spin_box->setEnabled(true); + ui_->shape_size_z_spin_box->setEnabled(true); + break; + case shapes::SPHERE: + ui_->shape_size_x_spin_box->setEnabled(true); + ui_->shape_size_y_spin_box->setEnabled(false); + ui_->shape_size_z_spin_box->setEnabled(false); + break; + case shapes::CONE: + ui_->shape_size_x_spin_box->setEnabled(true); + ui_->shape_size_y_spin_box->setEnabled(false); + ui_->shape_size_z_spin_box->setEnabled(true); + break; + case shapes::CYLINDER: + ui_->shape_size_x_spin_box->setEnabled(true); + ui_->shape_size_y_spin_box->setEnabled(false); + ui_->shape_size_z_spin_box->setEnabled(true); + break; + default: + ui_->shape_size_x_spin_box->setEnabled(false); + ui_->shape_size_y_spin_box->setEnabled(false); + ui_->shape_size_z_spin_box->setEnabled(false); + QMessageBox::warning(this, QString("Unsupported shape"), + QString("The '%1' is not supported.").arg(selected_shape.c_str())); + } +} + void MotionPlanningFrame::importObjectFromFileButtonClicked() { QString path = QFileDialog::getOpenFileName(this, tr("Import Object Mesh"), QString(), diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 568cccd2b0..586129bb7b 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -7,7 +7,7 @@ 0 0 692 - 414 + 437 @@ -26,7 +26,7 @@ false - 0 + 3 @@ -1102,37 +1102,62 @@ - - + + + + Press Ctrl+C to duplicate an object + - Object Status + Current Scene Objects - - - - - true - - - QFrame::StyledPanel + + + + + Remove - - QFrame::Raised + + + + + + Clear + + + + - Status + Import &File - - Qt::AutoText + + Import a mesh from file - - true + + + + + + + 0 + 0 + - - Qt::TextBrowserInteraction + + QAbstractItemView::DoubleClicked|QAbstractItemView::EditKeyPressed + + + + Import &URL + + + Import a mesh from URL + + + @@ -1191,72 +1216,132 @@ - - - - Press Ctrl+C to duplicate an object + + + + Qt::Vertical + + + 20 + 40 + + + + + + - Current Scene Objects + Object Status - - - + + + + + true + + + QFrame::StyledPanel + + + QFrame::Raised + - From &URL + Status - - Import a mesh from a URL + + Qt::AutoText + + + true - - - - Clear + + + + + + + + 0 + 0 + + + + Primitive Shapes + + + + + + + box + + + + + sphere + + + + + cone + + + + + cylinder + + + + + + + + 0.050000000000000 + + + 0.200000000000000 - - - - Remove + + + + 0.050000000000000 + + + 0.200000000000000 - - - - Import &Mesh + + + + 0.050000000000000 - - Import a mesh from a file + + 0.200000000000000 - - - - QAbstractItemView::DoubleClicked|QAbstractItemView::EditKeyPressed + + + + + 0 + 0 + + + + Add - - - - Qt::Vertical - - - - 20 - 40 - - - - From 538d15f1834df3065d7a1672d02118b91137b631 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Mon, 6 Jul 2020 16:46:07 +0200 Subject: [PATCH 382/612] Fix segfault in PSM::clearOctomap() (#2193) --- .../src/planning_scene_monitor.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index e2d67803bc..6d896254eb 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -531,9 +531,16 @@ void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningS void PlanningSceneMonitor::clearOctomap() { - octomap_monitor_->getOcTreePtr()->lockWrite(); - octomap_monitor_->getOcTreePtr()->clear(); - octomap_monitor_->getOcTreePtr()->unlockWrite(); + if (octomap_monitor_) + { + octomap_monitor_->getOcTreePtr()->lockWrite(); + octomap_monitor_->getOcTreePtr()->clear(); + octomap_monitor_->getOcTreePtr()->unlockWrite(); + } + else + { + ROS_WARN_NAMED(LOGNAME, "Unable to clear octomap since no octomap monitor has been initialized"); + } } bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene) From d6666a9aee734acd1891a43657cf162c9ca4e9b7 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 6 Jul 2020 08:54:28 -0600 Subject: [PATCH 383/612] use moveit_resources for testing (#2194) --- .../test/move_group_interface_cpp_test.test | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test index af9d7de7c7..b1612e213d 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test @@ -1,6 +1,6 @@ - + @@ -17,7 +17,7 @@ - + From 37b94e7a8c49a56c136aa8c88e5a5935682e5083 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 6 Jul 2020 19:37:22 +0200 Subject: [PATCH 384/612] Unify UI of adding primitive and mesh shapes - Remove "Import from File" and "Import from URL" buttons, but add those options as item in the shape combobox - Integrate MotionPlanningFrame::addObject() into MotionPlanningFrame::addSceneObject() --- .../motion_planning_frame.h | 13 +- .../src/motion_planning_frame.cpp | 217 +++----- .../src/motion_planning_frame_objects.cpp | 53 +- .../ui/motion_planning_rviz_plugin_frame.ui | 508 +++++++++--------- 4 files changed, 350 insertions(+), 441 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 37452ea6e6..3ce7cf7b9f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -99,11 +99,6 @@ static const std::string TAB_STATUS = "Status"; static const double LARGE_MESH_THRESHOLD = 10.0; -static const std::map SHAPES_MAP = { { "box", shapes::BOX }, - { "sphere", shapes::SPHERE }, - { "cone", shapes::CONE }, - { "cylinder", shapes::CYLINDER } }; - class MotionPlanningFrame : public QWidget { friend class MotionPlanningDisplay; @@ -180,8 +175,6 @@ private Q_SLOTS: // Scene Objects tab void shapesComboBoxChanged(const QString& text); - void importObjectFromFileButtonClicked(); - void importObjectFromUrlButtonClicked(); void clearSceneButtonClicked(); void sceneScaleChanged(int value); void sceneScaleStartChange(); @@ -250,8 +243,6 @@ private Q_SLOTS: void goalStateTextChangedExec(const std::string& goal_state); // Scene objects tab - void addObject(const collision_detection::WorldPtr& world, const std::string& id, const shapes::ShapeConstPtr& shape, - const Eigen::Isometry3d& pose); void updateCollisionObjectPose(bool update_marker_position); void createSceneInteractiveMarker(); void renameCollisionObject(QListWidgetItem* item); @@ -312,8 +303,8 @@ private Q_SLOTS: ros::Subscriber update_custom_goal_state_subscriber_; // General void changePlanningGroupHelper(); - void addPrimitiveShape(); - void importResource(const std::string& path); + void addSceneObject(); + shapes::ShapePtr loadMeshResource(const std::string& url); void loadStoredStates(const std::string& pattern); void remotePlanCallback(const std_msgs::EmptyConstPtr& msg); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 47e680e667..e51ce34c51 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -50,6 +50,8 @@ #include #include +#include +#include #include #include "ui_motion_planning_rviz_plugin_frame.h" @@ -67,6 +69,13 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: { // set up the GUI ui_->setupUi(this); + ui_->shapes_combo_box->addItem("Box", shapes::BOX); + ui_->shapes_combo_box->addItem("Sphere", shapes::SPHERE); + ui_->shapes_combo_box->addItem("Cylinder", shapes::CYLINDER); + ui_->shapes_combo_box->addItem("Cone", shapes::CONE); + ui_->shapes_combo_box->addItem("Mesh from file", shapes::MESH); + ui_->shapes_combo_box->addItem("Mesh from URL", shapes::MESH); + // add more tabs joints_tab_ = new MotionPlanningFrameJointsWidget(planning_display_, ui_->tabWidget); ui_->tabWidget->addTab(joints_tab_, "Joints"); @@ -105,8 +114,6 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: SLOT(planningAlgorithmIndexChanged(int))); connect(ui_->planning_algorithm_combo_box, SIGNAL(currentIndexChanged(int)), this, SLOT(planningAlgorithmIndexChanged(int))); - connect(ui_->import_object_file_button, SIGNAL(clicked()), this, SLOT(importObjectFromFileButtonClicked())); - connect(ui_->import_object_url_button, SIGNAL(clicked()), this, SLOT(importObjectFromUrlButtonClicked())); connect(ui_->clear_scene_button, SIGNAL(clicked()), this, SLOT(clearSceneButtonClicked())); connect(ui_->scene_scale, SIGNAL(valueChanged(int)), this, SLOT(sceneScaleChanged(int))); connect(ui_->scene_scale, SIGNAL(sliderPressed()), this, SLOT(sceneScaleStartChange())); @@ -129,7 +136,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: SLOT(warehouseItemNameChanged(QTreeWidgetItem*, int))); connect(ui_->reset_db_button, SIGNAL(clicked()), this, SLOT(resetDbButtonClicked())); - connect(ui_->add_shape_button, &QPushButton::clicked, this, &MotionPlanningFrame::addPrimitiveShape); + connect(ui_->add_object_button, &QPushButton::clicked, this, &MotionPlanningFrame::addSceneObject); connect(ui_->shapes_combo_box, &QComboBox::currentTextChanged, this, &MotionPlanningFrame::shapesComboBoxChanged); connect(ui_->export_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(exportGeometryAsTextButtonClicked())); connect(ui_->import_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(importGeometryFromTextButtonClicked())); @@ -429,7 +436,7 @@ void MotionPlanningFrame::sceneUpdate(planning_scene_monitor::PlanningSceneMonit planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); } -void MotionPlanningFrame::addPrimitiveShape() +void MotionPlanningFrame::addSceneObject() { static const double MIN_VAL = 1e-6; @@ -444,22 +451,14 @@ void MotionPlanningFrame::addPrimitiveShape() double z_length = ui_->shape_size_z_spin_box->isEnabled() ? ui_->shape_size_z_spin_box->value() : MIN_VAL; if (x_length < MIN_VAL || y_length < MIN_VAL || z_length < MIN_VAL) { - QMessageBox::warning(this, QString("Side length value is too small"), - QString("Value needs to be >= '").append(std::to_string(MIN_VAL).c_str()).append("'")); + QMessageBox::warning(this, QString("Dimension is too small"), QString("Size values need to be >= %1").arg(MIN_VAL)); return; } - // get selected shape + // by default, name object by shape type std::string selected_shape = ui_->shapes_combo_box->currentText().toStdString(); - if (SHAPES_MAP.count(selected_shape) == 0) - { - QMessageBox::warning(this, QString("Unsupported shape"), - QString("The '%1' shape is not supported.").arg(selected_shape.c_str())); - return; - } - shapes::ShapeType shape_type = SHAPES_MAP.at(selected_shape); shapes::ShapeConstPtr shape; - switch (shape_type) + switch (ui_->shapes_combo_box->currentData().toInt()) // fetch shape ID from current combobox item { case shapes::BOX: shape = std::make_shared(x_length, y_length, z_length); @@ -473,12 +472,29 @@ void MotionPlanningFrame::addPrimitiveShape() case shapes::CYLINDER: shape = std::make_shared(0.5 * x_length, z_length); break; + case shapes::MESH: + { + QUrl url; + if (ui_->shapes_combo_box->currentText().contains("file")) // open from file + url = QFileDialog::getOpenFileUrl(this, tr("Import Object Mesh"), QString(), + "CAD files (*.stl *.obj *.dae);;All files (*.*)"); + else // open from URL + url = QInputDialog::getText(this, tr("Import Object Mesh"), tr("URL for file to import from:"), + QLineEdit::Normal, QString("http://")); + if (!url.isEmpty()) + shape = loadMeshResource(url.toString().toStdString()); + if (!shape) + return; + // name mesh objects by their file name + selected_shape = url.fileName().toStdString(); + break; + } default: QMessageBox::warning(this, QString("Unsupported shape"), - QString("The '%1' is not supported.").arg(selected_shape.c_str())); + QString("The '%1' is not supported.").arg(ui_->shapes_combo_box->currentText())); } - // naming object + // find available (initial) name of object int idx = 0; std::string shape_name = selected_shape + "_" + std::to_string(idx); while (planning_display_->getPlanningSceneRO()->getWorld()->hasObject(shape_name)) @@ -487,139 +503,64 @@ void MotionPlanningFrame::addPrimitiveShape() shape_name = selected_shape + "_" + std::to_string(idx); } - // adding object - Eigen::Isometry3d pose; - pose.setIdentity(); - planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); - if (ps) - addObject(ps->getWorldNonConst(), shape_name, shape, pose); + // Actually add object to the plugin's PlanningScene + { + planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); + ps->getWorldNonConst()->addToObject(shape_name, shape, Eigen::Isometry3d::Identity()); + } + + planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + + // Automatically select the inserted object so that its IM is displayed + planning_display_->addMainLoopJob( + boost::bind(&MotionPlanningFrame::setItemSelectionInList, this, shape_name, true, ui_->collision_objects_list)); + + planning_display_->queueRenderSceneGeometry(); } -void MotionPlanningFrame::importResource(const std::string& path) +shapes::ShapePtr MotionPlanningFrame::loadMeshResource(const std::string& url) { - if (planning_display_->getPlanningSceneMonitor()) + shapes::Mesh* mesh = shapes::createMeshFromResource(url); + if (mesh) { - shapes::Mesh* mesh = shapes::createMeshFromResource(path); - if (mesh) + // If the object is very large, ask the user if the scale should be reduced. + bool object_is_very_large = false; + for (unsigned int i = 0; i < mesh->vertex_count; i++) { - std::size_t slash = path.find_last_of("/\\"); - std::string name = path.substr(slash + 1); - - if (planning_display_->getPlanningSceneRO()->getCurrentState().hasAttachedBody(name)) - { - QMessageBox::warning(this, QString("Duplicate names"), QString("An attached object named '") - .append(name.c_str()) - .append("' already exists. Please rename the " - "attached object before importing.")); - return; - } - - // If the object is very large, ask the user if the scale should be reduced. - bool object_is_very_large = false; - for (unsigned int i = 0; i < mesh->vertex_count; i++) - { - if ((abs(mesh->vertices[i * 3 + 0]) > LARGE_MESH_THRESHOLD) || - (abs(mesh->vertices[i * 3 + 1]) > LARGE_MESH_THRESHOLD) || - (abs(mesh->vertices[i * 3 + 2]) > LARGE_MESH_THRESHOLD)) - { - object_is_very_large = true; - break; - } - } - if (object_is_very_large) + if ((abs(mesh->vertices[i * 3 + 0]) > LARGE_MESH_THRESHOLD) || + (abs(mesh->vertices[i * 3 + 1]) > LARGE_MESH_THRESHOLD) || + (abs(mesh->vertices[i * 3 + 2]) > LARGE_MESH_THRESHOLD)) { - QMessageBox msg_box; - msg_box.setText( - "The object is very large (greater than 10 m). The file may be in millimeters instead of meters."); - msg_box.setInformativeText("Attempt to fix the size by shrinking the object?"); - msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No); - msg_box.setDefaultButton(QMessageBox::Yes); - if (msg_box.exec() == QMessageBox::Yes) - { - for (unsigned int i = 0; i < mesh->vertex_count; ++i) - { - unsigned int i3 = i * 3; - mesh->vertices[i3] *= 0.001; - mesh->vertices[i3 + 1] *= 0.001; - mesh->vertices[i3 + 2] *= 0.001; - } - } + object_is_very_large = true; + break; } - - shapes::ShapeConstPtr shape(mesh); - Eigen::Isometry3d pose; - pose.setIdentity(); - - // If the object already exists, ask the user whether to overwrite or rename - if (planning_display_->getPlanningSceneRO()->getWorld()->hasObject(name)) + } + if (object_is_very_large) + { + QMessageBox msg_box; + msg_box.setText( + "The object is very large (greater than 10 m). The file may be in millimeters instead of meters."); + msg_box.setInformativeText("Attempt to fix the size by shrinking the object?"); + msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No); + msg_box.setDefaultButton(QMessageBox::Yes); + if (msg_box.exec() == QMessageBox::Yes) { - QMessageBox msg_box; - msg_box.setText("There exists another object with the same name."); - msg_box.setInformativeText("Would you like to overwrite it?"); - msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); - msg_box.setDefaultButton(QMessageBox::No); - int ret = msg_box.exec(); - - switch (ret) + for (unsigned int i = 0; i < mesh->vertex_count; ++i) { - case QMessageBox::Yes: - // Overwrite was clicked - { - planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); - if (ps) - { - ps->getWorldNonConst()->removeObject(name); - addObject(ps->getWorldNonConst(), name, shape, pose); - } - } - break; - case QMessageBox::No: - { - // Don't overwrite was clicked. Ask for another name - bool ok = false; - QString text = QInputDialog::getText( - this, tr("Choose a new name"), tr("Import the new object under the name:"), QLineEdit::Normal, - QString::fromStdString(name + "-" + boost::lexical_cast( - planning_display_->getPlanningSceneRO()->getWorld()->size())), - &ok); - if (ok) - { - if (!text.isEmpty()) - { - name = text.toStdString(); - planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); - if (ps) - { - if (ps->getWorld()->hasObject(name)) - QMessageBox::warning( - this, "Name already exists", - QString("The name '").append(name.c_str()).append("' already exists. Not importing object.")); - else - addObject(ps->getWorldNonConst(), name, shape, pose); - } - } - else - QMessageBox::warning(this, "Object not imported", "Cannot use an empty name for an imported object"); - } - break; - } - default: - // Pressed cancel, do nothing - break; + unsigned int i3 = i * 3; + mesh->vertices[i3] *= 0.001; + mesh->vertices[i3 + 1] *= 0.001; + mesh->vertices[i3 + 2] *= 0.001; } } - else - { - planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); - if (ps) - addObject(ps->getWorldNonConst(), name, shape, pose); - } - } - else - { - QMessageBox::warning(this, QString("Import error"), QString("Unable to import object")); - return; } + + return shapes::ShapePtr(mesh); + } + else + { + QMessageBox::warning(this, QString("Import error"), QString("Unable to import object")); + return shapes::ShapePtr(); } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index fb8f9d20a2..a94a5408ac 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -75,14 +75,7 @@ namespace moveit_rviz_plugin { void MotionPlanningFrame::shapesComboBoxChanged(const QString& text) { - const std::string selected_shape = text.toStdString(); - if (SHAPES_MAP.find(selected_shape) == SHAPES_MAP.end()) - { - return; - } - - shapes::ShapeType shape_type = SHAPES_MAP.at(selected_shape); - switch (shape_type) + switch (ui_->shapes_combo_box->currentData().toInt()) // fetch shape ID from current combobox item { case shapes::BOX: ui_->shape_size_x_spin_box->setEnabled(true); @@ -94,42 +87,22 @@ void MotionPlanningFrame::shapesComboBoxChanged(const QString& text) ui_->shape_size_y_spin_box->setEnabled(false); ui_->shape_size_z_spin_box->setEnabled(false); break; - case shapes::CONE: - ui_->shape_size_x_spin_box->setEnabled(true); - ui_->shape_size_y_spin_box->setEnabled(false); - ui_->shape_size_z_spin_box->setEnabled(true); - break; case shapes::CYLINDER: + case shapes::CONE: ui_->shape_size_x_spin_box->setEnabled(true); ui_->shape_size_y_spin_box->setEnabled(false); ui_->shape_size_z_spin_box->setEnabled(true); break; - default: + case shapes::MESH: ui_->shape_size_x_spin_box->setEnabled(false); ui_->shape_size_y_spin_box->setEnabled(false); ui_->shape_size_z_spin_box->setEnabled(false); - QMessageBox::warning(this, QString("Unsupported shape"), - QString("The '%1' is not supported.").arg(selected_shape.c_str())); + break; + default: + break; } } -void MotionPlanningFrame::importObjectFromFileButtonClicked() -{ - QString path = QFileDialog::getOpenFileName(this, tr("Import Object Mesh"), QString(), - "CAD files (*.stl *.obj *.dae);;All files (*.*)"); - if (!path.isEmpty()) - importResource("file://" + path.toStdString()); -} - -void MotionPlanningFrame::importObjectFromUrlButtonClicked() -{ - bool ok = false; - QString url = QInputDialog::getText(this, tr("Import Object"), tr("URL for file to import:"), QLineEdit::Normal, - QString("http://"), &ok); - if (ok && !url.isEmpty()) - importResource(url.toStdString()); -} - void MotionPlanningFrame::clearSceneButtonClicked() { planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); @@ -733,20 +706,6 @@ void MotionPlanningFrame::computeLoadQueryButtonClicked() } } -void MotionPlanningFrame::addObject(const collision_detection::WorldPtr& world, const std::string& id, - const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& pose) -{ - world->addToObject(id, shape, pose); - - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); - - // Automatically select the inserted object so that its IM is displayed - planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::setItemSelectionInList, this, id, true, ui_->collision_objects_list)); - - planning_display_->queueRenderSceneGeometry(); -} - visualization_msgs::InteractiveMarker MotionPlanningFrame::createObjectMarkerMsg(const collision_detection::CollisionEnv::ObjectConstPtr& obj) { diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 586129bb7b..86e1905bf4 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -6,8 +6,8 @@ 0 0 - 692 - 437 + 630 + 424 @@ -937,6 +937,255 @@ Scene Objects + + + + Scene Geometry + + + + + + + 0 + 0 + + + + &Publish Scene + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Export scene geometry to a .scene file (only geometry information is preserved) + + + &Export + + + + + + + Import scene geometry from a .scene file (only geometry information is preserved) + + + &Import + + + + + + + + + + Object Status + + + + + + true + + + QFrame::NoFrame + + + QFrame::Raised + + + Status + + + Qt::AutoText + + + true + + + + + + + + + + Current Scene Objects + + + + + + + 0 + 0 + + + + Press Ctrl+C to duplicate an object + + + QAbstractItemView::DoubleClicked|QAbstractItemView::EditKeyPressed + + + + + + + + 0 + 0 + + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + + + + 0 + 0 + + + + Size: + + + + + + + 0.050000000000000 + + + 0.200000000000000 + + + + + + + 0.050000000000000 + + + 0.200000000000000 + + + + + + + 0.050000000000000 + + + 0.200000000000000 + + + + + + + + + + + + + + + 0 + 0 + + + + Add object at scene's origin + + + Add + + + + .. + + + + + + + + 0 + 0 + + + + Remove selected object + + + Del + + + + .. + + + + + + + + 0 + 0 + + + + Clear planning scene + + + ... + + + + .. + + + + + + + + + + + @@ -1102,121 +1351,7 @@ - - - - Press Ctrl+C to duplicate an object - - - Current Scene Objects - - - - - - Remove - - - - - - - Clear - - - - - - - Import &File - - - Import a mesh from file - - - - - - - - 0 - 0 - - - - QAbstractItemView::DoubleClicked|QAbstractItemView::EditKeyPressed - - - - - - - Import &URL - - - Import a mesh from URL - - - - - - - - - Scene Geometry - - - - - - - 0 - 0 - - - - &Publish Scene - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - &Export - - - Export scene geometry to a .scene file (only geometry information is preserved) - - - - - - - &Import - - - Import scene geometry from a .scene file (only geometry information is preserved) - - - - - - - Qt::Vertical @@ -1229,119 +1364,6 @@ - - - - Object Status - - - - - - true - - - QFrame::StyledPanel - - - QFrame::Raised - - - Status - - - Qt::AutoText - - - true - - - - - - - - - - - 0 - 0 - - - - Primitive Shapes - - - - - - - box - - - - - sphere - - - - - cone - - - - - cylinder - - - - - - - - 0.050000000000000 - - - 0.200000000000000 - - - - - - - 0.050000000000000 - - - 0.200000000000000 - - - - - - - 0.050000000000000 - - - 0.200000000000000 - - - - - - - - 0 - 0 - - - - Add - - - - - - @@ -1504,22 +1526,22 @@ - - Load - Load a robot state + + Load + - - Clear - Clear robot states from list + + Clear + @@ -1563,22 +1585,22 @@ - - Save Start - Save the current start state + + Save Start + - - Save Goal - Save the current goal state + + Save Goal + @@ -1770,10 +1792,6 @@ roi_size_y roi_size_z collision_objects_list - import_object_file_button - import_object_url_button - remove_object_button - clear_scene_button object_x object_y object_z From a22edb82ad4ce2b4f4efb03512556be27623a02b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 6 Jul 2020 20:36:58 +0200 Subject: [PATCH 385/612] MP plugin: Tweak GUI layout - grid layout for object position/orientation spin boxes (to not rely on chance for their column-wise layout anymore) - Limit position spin boxes to -999..999 This essentially cancels one digit to be displayed, which significantly reduces the minimum width of the widget. --- .../ui/motion_planning_rviz_plugin_frame.ui | 1185 +++++++++-------- 1 file changed, 620 insertions(+), 565 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 86e1905bf4..9adb5aaa9d 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -6,8 +6,8 @@ 0 0 - 630 - 424 + 532 + 389 @@ -355,6 +355,9 @@ + + 0 + @@ -369,6 +372,15 @@ 4 + + 6 + + + 6 + + + 6 + @@ -419,6 +431,13 @@ + + + + Clear octomap + + + @@ -427,17 +446,17 @@ Query - + - 4 + 6 - 4 + 6 - 4 + 6 - + @@ -451,7 +470,7 @@ - + @@ -465,7 +484,7 @@ - + @@ -479,42 +498,44 @@ - - - - Clear octomap - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 6 + + + + Path Constraints + + 6 + 4 + + 6 + - 1 + 6 @@ -552,200 +573,210 @@ - - - Options + + + 0 - - - 2 - - - + + + + Options + + - 1 + 2 + + + 4 - - - Planning Time (s): + + + 1 - + + + + Planning Time (s): + + + + + + + + 0 + 0 + + + + 300.000000000000000 + + + 5.000000000000000 + + + + - - - - 0 - 0 - + + + 1 - - 300.000000000000000 + + + + Planning Attempts: + + + + + + + + 0 + 0 + + + + 1000 + + + 10 + + + + + + + + + 1 - - 5.000000000000000 + + + + Velocity Scaling: + + + + + + + 1.000000000000000 + + + 0.010000000000000 + + + 1.000000000000000 + + + + + + + + + 1 - + + + + Acceleration Scaling: + + + + + + + 1.000000000000000 + + + 0.010000000000000 + + + 1.000000000000000 + + + + - - - - - - 1 - - + - Planning Attempts: + Allow Replanning + + + false - - - - 0 - 0 - - - - 1000 + + + Allow Sensor Positioning - - 10 + + false - - - - - - 1 - - + - Velocity Scaling: + Allow External Comm. + + + false - - - 1.000000000000000 - - - 0.010000000000000 + + + Use Cartesian Path - - 1.000000000000000 + + false - - - - - - 1 - - + - Acceleration Scaling: + Use Collision-Aware IK + + + true - - - 1.000000000000000 - - - 0.010000000000000 - - - 1.000000000000000 + + + Allow Approx IK Solutions - - - - - Allow Replanning - - - false - - - - - - - Allow Sensor Positioning - - - false - - - - - - - Allow External Comm. - - - false - - - - - - - Use Cartesian Path - - - false - - - - - - - Use Collision-Aware IK - - - true - - - - - - - Allow Approx IK Solutions - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + @@ -824,23 +855,6 @@ ROI - - - - 0.010000000000000 - - - 1.500000000000000 - - - - - - - Size (m): - - - @@ -851,43 +865,40 @@ - - + + 0.010000000000000 - 1.200000000000000 - - - - - - - Center (m): + 4.000000000000000 - - + + 0.010000000000000 - 4.000000000000000 + 1.200000000000000 - - - - -99.000000000000000 - + + 0.010000000000000 - 0.600000000000000 + 1.500000000000000 + + + + + + + Size (m): @@ -901,31 +912,25 @@ - - - - Qt::Horizontal - - - - 40 - 20 - + + + + Center (m): - + - - - - Qt::Horizontal + + + + -99.000000000000000 - - - 40 - 20 - + + 0.010000000000000 - + + 0.600000000000000 + + @@ -936,323 +941,263 @@ Scene Objects - - - - - Scene Geometry - - - - - - - 0 - 0 - + + + + + + + Current Scene Objects + + + + 6 - - &Publish Scene + + 6 - - - - - - Qt::Horizontal + + 6 - - - 40 - 20 - + + 6 - - - - - - Export scene geometry to a .scene file (only geometry information is preserved) + + + + + 0 + 0 + + + + Press Ctrl+C to duplicate an object + + + QAbstractItemView::DoubleClicked|QAbstractItemView::EditKeyPressed + + + + + + + + + + + 0 + 0 + + + + Add/Remove scene object(s) + + + + 6 - - &Export + + 6 - - - - - - Import scene geometry from a .scene file (only geometry information is preserved) + + 6 - - &Import + + 6 - - - - + + + + + + x dimension + + + 0.050000000000000 + + + 0.200000000000000 + + + + + + + y dimension + + + 0.050000000000000 + + + 0.200000000000000 + + + + + + + z dimension + + + 0.050000000000000 + + + 0.200000000000000 + + + + + + + + + + + + + + + 0 + 0 + + + + Add object at scene's origin + + + Add + + + + .. + + + + + + + + 0 + 0 + + + + Remove selected object + + + Del + + + + .. + + + + + + + + 0 + 0 + + + + Clear planning scene + + + Clr + + + + .. + + + + + + + + + - - - - Object Status - - - - - - true - - - QFrame::NoFrame - - - QFrame::Raised - - - Status - - - Qt::AutoText - - - true + + + + + + Change pose and scale of selected object + + + + 6 - - - - - - - - - Current Scene Objects - - - - - - - 0 - 0 - + + 6 - - Press Ctrl+C to duplicate an object + + 6 - - QAbstractItemView::DoubleClicked|QAbstractItemView::EditKeyPressed + + 6 - - - - - - - 0 - 0 - + + 6 - - - 0 - - - 0 - - - 0 - - - 0 - - - - - - - - 0 - 0 - - - - Size: - - - - - - - 0.050000000000000 - - - 0.200000000000000 - - - - - - - 0.050000000000000 - - - 0.200000000000000 - - - - - - - 0.050000000000000 - - - 0.200000000000000 - - - - - - - - - - - - - - - 0 - 0 - - - - Add object at scene's origin - - - Add - - - - .. - - - - - - - - 0 - 0 - - - - Remove selected object - - - Del - - - - .. - - - - - - - - 0 - 0 - - - - Clear planning scene - - - ... - - - - .. - - - - - - - - - - - - - - - Change Object Pose and Scale - - - - - + - Position (XYZ): + Position: - + + + + Rotation: + + + + - -1000.000000000000000 + -999.990000000000009 - 1000.000000000000000 + 999.990000000000009 0.100000000000000 - - + + - -1000.000000000000000 + -3.140000000000000 - 1000.000000000000000 + 3.140000000000000 0.100000000000000 - - + + - -1000.000000000000000 + -3.140000000000000 - 1000.000000000000000 + 3.140000000000000 0.100000000000000 - - - - - - - - Rotation (RPY): - - - - - + + -3.140000000000000 @@ -1264,26 +1209,82 @@ - - + + - -3.140000000000000 + -999.990000000000009 - 3.140000000000000 + 999.990000000000009 0.100000000000000 - - + + + + + + Scale: + + + + + + + 0% + + + + + + + + 160 + 0 + + + + 200 + + + 100 + + + 100 + + + Qt::Horizontal + + + false + + + QSlider::NoTicks + + + 0 + + + + + + + 200% + + + + + + + - -3.140000000000000 + -999.990000000000009 - 3.140000000000000 + 999.990000000000009 0.100000000000000 @@ -1291,78 +1292,132 @@ - - - + + + + + + Status of selected object + + + + 6 + + + 6 + + + 6 + + + 6 + - + + + true + + + QFrame::NoFrame + + + QFrame::Raised + - Scale: + Status + + + Qt::AutoText + + + true + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Scene Geometry + + + + 6 + + + 6 + + + 6 + + + 6 + - + + + + 0 + 0 + + - 0% + &Publish - - - - 160 - 0 - - - - 200 - - - 100 - - - 100 - + Qt::Horizontal - - false + + + 40 + 20 + - - QSlider::NoTicks + + + + + + Export scene geometry to a .scene file (only geometry information is preserved) - - 0 + + &Export - + + + Import scene geometry from a .scene file (only geometry information is preserved) + - 200% + &Import - - - - - - - - Qt::Vertical - - - - 20 - 40 - - - + + + From 16a7df13abc5357256a4abf2bc17fd0b43c96f22 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 7 Jul 2020 00:32:33 +0200 Subject: [PATCH 386/612] Update query states if a collision object was attached/detached --- .../motion_planning_display.h | 1 + .../src/motion_planning_display.cpp | 11 +++++++---- .../src/motion_planning_frame_objects.cpp | 4 ++++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index 7dc3db6e20..ca826c59f7 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -134,6 +134,7 @@ class MotionPlanningDisplay : public PlanningSceneDisplay void setQueryStartState(const moveit::core::RobotState& start); void setQueryGoalState(const moveit::core::RobotState& goal); + void updateQueryStates(const moveit::core::RobotState ¤t_state); void updateQueryStartState(); void updateQueryGoalState(); void rememberPreviousStartState(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 28f7b3fbe4..83530a5112 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -1212,11 +1212,8 @@ void MotionPlanningDisplay::updateStateExceptModified(moveit::core::RobotState& dest = src_copy; } -void MotionPlanningDisplay::onSceneMonitorReceivedUpdate( - planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) +void MotionPlanningDisplay::updateQueryStates(const moveit::core::RobotState& current_state) { - PlanningSceneDisplay::onSceneMonitorReceivedUpdate(update_type); - moveit::core::RobotState current_state = getPlanningSceneRO()->getCurrentState(); std::string group = planning_group_property_->getStdString(); if (query_start_state_ && query_start_state_property_->getBool() && !group.empty()) @@ -1232,7 +1229,13 @@ void MotionPlanningDisplay::onSceneMonitorReceivedUpdate( updateStateExceptModified(goal, current_state); setQueryGoalState(goal); } +} +void MotionPlanningDisplay::onSceneMonitorReceivedUpdate( + planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) +{ + PlanningSceneDisplay::onSceneMonitorReceivedUpdate(update_type); + updateQueryStates(getPlanningSceneRO()->getCurrentState()); if (frame_) frame_->sceneUpdate(update_type); } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index a94a5408ac..ad8e3e0abe 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -852,6 +852,8 @@ void MotionPlanningFrame::attachDetachCollisionObject(QListWidgetItem* item) aco.object.operation = moveit_msgs::CollisionObject::REMOVE; } } + + moveit::core::RobotState rs(planning_display_->getRobotModel()); { planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); // we loop through the list in case updates were received since the start of the function @@ -862,9 +864,11 @@ void MotionPlanningFrame::attachDetachCollisionObject(QListWidgetItem* item) break; } ps->processAttachedCollisionObjectMsg(aco); + rs = ps->getCurrentState(); } selectedCollisionObjectChanged(); + planning_display_->updateQueryStates(rs); planning_display_->queueRenderSceneGeometry(); } From 157dd30cf70420b2a5fa21a2894cb12697aef1ec Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 7 Jul 2020 00:47:03 +0200 Subject: [PATCH 387/612] Rename MotionPlanningFrame functions clearSceneButtonClicked() -> clearScene() removeObjectButtonClicked() -> removeSceneObject() publishSceneButtonClicked()-> publishScene() --- .../motion_planning_frame.h | 10 +++++----- .../src/motion_planning_frame.cpp | 6 +++--- .../src/motion_planning_frame_context.cpp | 11 ----------- .../src/motion_planning_frame_objects.cpp | 15 +++++++++++++-- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 3ce7cf7b9f..82e85d5ded 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -151,7 +151,6 @@ private Q_SLOTS: // Context tab void databaseConnectButtonClicked(); - void publishSceneButtonClicked(); void planningAlgorithmIndexChanged(int index); void resetDbButtonClicked(); void approximateIKChanged(int state); @@ -174,12 +173,14 @@ private Q_SLOTS: void onClearOctomapClicked(); // Scene Objects tab - void shapesComboBoxChanged(const QString& text); - void clearSceneButtonClicked(); + void clearScene(); + void publishScene(); void sceneScaleChanged(int value); void sceneScaleStartChange(); void sceneScaleEndChange(); - void removeObjectButtonClicked(); + void shapesComboBoxChanged(const QString& text); + void addSceneObject(); + void removeSceneObject(); void selectedCollisionObjectChanged(); void objectPoseValueChanged(double value); void collisionObjectChanged(QListWidgetItem* item); @@ -303,7 +304,6 @@ private Q_SLOTS: ros::Subscriber update_custom_goal_state_subscriber_; // General void changePlanningGroupHelper(); - void addSceneObject(); shapes::ShapePtr loadMeshResource(const std::string& url); void loadStoredStates(const std::string& pattern); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index e51ce34c51..4d408dfe1a 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -114,18 +114,18 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: SLOT(planningAlgorithmIndexChanged(int))); connect(ui_->planning_algorithm_combo_box, SIGNAL(currentIndexChanged(int)), this, SLOT(planningAlgorithmIndexChanged(int))); - connect(ui_->clear_scene_button, SIGNAL(clicked()), this, SLOT(clearSceneButtonClicked())); + connect(ui_->clear_scene_button, SIGNAL(clicked()), this, SLOT(clearScene())); connect(ui_->scene_scale, SIGNAL(valueChanged(int)), this, SLOT(sceneScaleChanged(int))); connect(ui_->scene_scale, SIGNAL(sliderPressed()), this, SLOT(sceneScaleStartChange())); connect(ui_->scene_scale, SIGNAL(sliderReleased()), this, SLOT(sceneScaleEndChange())); - connect(ui_->remove_object_button, SIGNAL(clicked()), this, SLOT(removeObjectButtonClicked())); + connect(ui_->remove_object_button, SIGNAL(clicked()), this, SLOT(removeSceneObject())); connect(ui_->object_x, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double))); connect(ui_->object_y, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double))); connect(ui_->object_z, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double))); connect(ui_->object_rx, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double))); connect(ui_->object_ry, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double))); connect(ui_->object_rz, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double))); - connect(ui_->publish_current_scene_button, SIGNAL(clicked()), this, SLOT(publishSceneButtonClicked())); + connect(ui_->publish_current_scene_button, SIGNAL(clicked()), this, SLOT(publishScene())); connect(ui_->collision_objects_list, SIGNAL(itemSelectionChanged()), this, SLOT(selectedCollisionObjectChanged())); connect(ui_->collision_objects_list, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(collisionObjectChanged(QListWidgetItem*))); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp index 87c1321723..b27d503181 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp @@ -57,17 +57,6 @@ void MotionPlanningFrame::databaseConnectButtonClicked() "connect to database"); } -void MotionPlanningFrame::publishSceneButtonClicked() -{ - const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); - if (ps) - { - moveit_msgs::PlanningScene msg; - ps->getPlanningSceneMsg(msg); - planning_scene_publisher_.publish(msg); - } -} - void MotionPlanningFrame::planningAlgorithmIndexChanged(int index) { std::string planner_id = ui_->planning_algorithm_combo_box->itemText(index).toStdString(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index ad8e3e0abe..61248c4feb 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -103,7 +103,18 @@ void MotionPlanningFrame::shapesComboBoxChanged(const QString& text) } } -void MotionPlanningFrame::clearSceneButtonClicked() +void MotionPlanningFrame::publishScene() +{ + const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); + if (ps) + { + moveit_msgs::PlanningScene msg; + ps->getPlanningSceneMsg(msg); + planning_scene_publisher_.publish(msg); + } +} + +void MotionPlanningFrame::clearScene() { planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); if (ps) @@ -167,7 +178,7 @@ void MotionPlanningFrame::sceneScaleEndChange() ui_->scene_scale->setSliderPosition(100); } -void MotionPlanningFrame::removeObjectButtonClicked() +void MotionPlanningFrame::removeSceneObject() { QList sel = ui_->collision_objects_list->selectedItems(); if (sel.empty()) From fe59e1dcfa8c1f3b0a35ff46779b6068163cc550 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 7 Jul 2020 01:35:55 +0200 Subject: [PATCH 388/612] Track local PS changes and publish them before planning --- .../motion_planning_display.h | 2 +- .../motion_planning_frame.h | 3 ++ .../src/motion_planning_frame.cpp | 2 ++ .../src/motion_planning_frame_objects.cpp | 28 +++++++++++++++++++ .../src/motion_planning_frame_planning.cpp | 2 ++ 5 files changed, 36 insertions(+), 1 deletion(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index ca826c59f7..0fd36a4088 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -134,7 +134,7 @@ class MotionPlanningDisplay : public PlanningSceneDisplay void setQueryStartState(const moveit::core::RobotState& start); void setQueryGoalState(const moveit::core::RobotState& goal); - void updateQueryStates(const moveit::core::RobotState ¤t_state); + void updateQueryStates(const moveit::core::RobotState& current_state); void updateQueryStartState(); void updateQueryGoalState(); void rememberPreviousStartState(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 82e85d5ded..59f25f464d 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -175,6 +175,9 @@ private Q_SLOTS: // Scene Objects tab void clearScene(); void publishScene(); + void publishSceneIfNeeded(); + void setLocalSceneEdited(bool dirty = true); + bool isLocalSceneDirty() const; void sceneScaleChanged(int value); void sceneScaleStartChange(); void sceneScaleEndChange(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 4d408dfe1a..fc575f108d 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -75,6 +75,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: ui_->shapes_combo_box->addItem("Cone", shapes::CONE); ui_->shapes_combo_box->addItem("Mesh from file", shapes::MESH); ui_->shapes_combo_box->addItem("Mesh from URL", shapes::MESH); + setLocalSceneEdited(false); // add more tabs joints_tab_ = new MotionPlanningFrameJointsWidget(planning_display_, ui_->tabWidget); @@ -508,6 +509,7 @@ void MotionPlanningFrame::addSceneObject() planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); ps->getWorldNonConst()->addToObject(shape_name, shape, Eigen::Isometry3d::Identity()); } + setLocalSceneEdited(); planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 61248c4feb..e1515e4d7e 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -103,6 +103,16 @@ void MotionPlanningFrame::shapesComboBoxChanged(const QString& text) } } +void MotionPlanningFrame::setLocalSceneEdited(bool dirty) +{ + ui_->publish_current_scene_button->setEnabled(dirty); +} + +bool MotionPlanningFrame::isLocalSceneDirty() const +{ + return ui_->publish_current_scene_button->isEnabled(); +} + void MotionPlanningFrame::publishScene() { const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); @@ -111,9 +121,19 @@ void MotionPlanningFrame::publishScene() moveit_msgs::PlanningScene msg; ps->getPlanningSceneMsg(msg); planning_scene_publisher_.publish(msg); + setLocalSceneEdited(false); } } +void MotionPlanningFrame::publishSceneIfNeeded() +{ + if (isLocalSceneDirty() && + QMessageBox::question(this, "Update PlanningScene", "You have local changes to your planning scene.\n" + "Publish them to the move_group node?", + QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes) == QMessageBox::Yes) + publishScene(); +} + void MotionPlanningFrame::clearScene() { planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); @@ -124,6 +144,7 @@ void MotionPlanningFrame::clearScene() moveit_msgs::PlanningScene msg; ps->getPlanningSceneMsg(msg); planning_scene_publisher_.publish(msg); + setLocalSceneEdited(false); planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); planning_display_->queueRenderSceneGeometry(); } @@ -146,6 +167,7 @@ void MotionPlanningFrame::sceneScaleChanged(int value) ps->getWorldNonConst()->addToObject(scaled_object_->id_, shapes::ShapeConstPtr(s), scaled_object_->shape_poses_[i]); } + setLocalSceneEdited(); scene_marker_->processMessage(createObjectMarkerMsg(ps->getWorld()->getObject(scaled_object_->id_))); planning_display_->queueRenderSceneGeometry(); } @@ -192,6 +214,7 @@ void MotionPlanningFrame::removeSceneObject() else ps->getCurrentStateNonConst().clearAttachedBody(sel[i]->text().toStdString()); scene_marker_.reset(); + setLocalSceneEdited(); planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); planning_display_->queueRenderSceneGeometry(); } @@ -367,6 +390,7 @@ void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position) ps->getWorldNonConst()->moveShapeInObject(obj->id_, obj->shapes_[0], p); planning_display_->queueRenderSceneGeometry(); + setLocalSceneEdited(); // Update the interactive marker pose to match the manually introduced one if (update_marker_position && scene_marker_) @@ -460,6 +484,7 @@ void MotionPlanningFrame::copySelectedCollisionObject() ps->getWorldNonConst()->addToObject(name, obj->shapes_, obj->shape_poses_); ROS_DEBUG("Copied collision object to '%s'", name.c_str()); } + setLocalSceneEdited(); planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); } @@ -824,6 +849,7 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item) cs.attachBody(new_ab); } } + setLocalSceneEdited(); } void MotionPlanningFrame::attachDetachCollisionObject(QListWidgetItem* item) @@ -879,6 +905,7 @@ void MotionPlanningFrame::attachDetachCollisionObject(QListWidgetItem* item) } selectedCollisionObjectChanged(); + setLocalSceneEdited(); planning_display_->updateQueryStates(rs); planning_display_->queueRenderSceneGeometry(); } @@ -979,6 +1006,7 @@ void MotionPlanningFrame::computeImportGeometryFromText(const std::string& path) ROS_INFO("Loaded scene geometry from '%s'", path.c_str()); planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); planning_display_->queueRenderSceneGeometry(); + setLocalSceneEdited(); } else { diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 8f8ab01772..6ee3739253 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -52,6 +52,7 @@ namespace moveit_rviz_plugin { void MotionPlanningFrame::planButtonClicked() { + publishSceneIfNeeded(); planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanButtonClicked, this), "compute plan"); } @@ -65,6 +66,7 @@ void MotionPlanningFrame::executeButtonClicked() void MotionPlanningFrame::planAndExecuteButtonClicked() { + publishSceneIfNeeded(); ui_->plan_and_execute_button->setEnabled(false); ui_->execute_button->setEnabled(false); // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution From e69e0336674f66d96715905847d82c5665cf8817 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 7 Jul 2020 19:27:58 +0200 Subject: [PATCH 389/612] Default to Planning tab initially --- .../motion_planning_rviz_plugin/src/motion_planning_frame.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index fc575f108d..36625508f5 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -192,7 +192,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: ui_->background_job_progress->hide(); ui_->background_job_progress->setMaximum(0); - ui_->tabWidget->setCurrentIndex(0); + ui_->tabWidget->setCurrentIndex(1); known_collision_objects_version_ = 0; From 85f62d8cf33f4ea544ae7438f54a6efc7af133b6 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 8 Jul 2020 14:19:44 +0200 Subject: [PATCH 390/612] Explicitly added icon resources --- moveit_ros/visualization/CMakeLists.txt | 9 +++++---- .../motion_planning_rviz_plugin/CMakeLists.txt | 1 + .../src/icons/edit-clear.png | Bin 0 -> 343 bytes .../src/icons/icons.qrc | 7 +++++++ .../src/icons/list-add.png | Bin 0 -> 517 bytes .../src/icons/list-remove.png | Bin 0 -> 228 bytes .../src/ui/motion_planning_rviz_plugin_frame.ui | 12 ++++++------ 7 files changed, 19 insertions(+), 10 deletions(-) create mode 100644 moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/edit-clear.png create mode 100644 moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/icons.qrc create mode 100644 moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/list-add.png create mode 100644 moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/list-remove.png diff --git a/moveit_ros/visualization/CMakeLists.txt b/moveit_ros/visualization/CMakeLists.txt index 51e5011804..eb67e2a861 100644 --- a/moveit_ros/visualization/CMakeLists.txt +++ b/moveit_ros/visualization/CMakeLists.txt @@ -48,14 +48,14 @@ find_package(Eigen3 REQUIRED) # Qt Stuff if(rviz_QT_VERSION VERSION_LESS "5") - find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui) - include(${QT_USE_FILE}) + find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui) + include(${QT_USE_FILE}) macro(qt_wrap_ui) qt4_wrap_ui(${ARGN}) endmacro() else() - find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) - set(QT_LIBRARIES Qt5::Widgets) + find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) + set(QT_LIBRARIES Qt5::Widgets) macro(qt_wrap_ui) qt5_wrap_ui(${ARGN}) endmacro() @@ -63,6 +63,7 @@ endif() set(CMAKE_INCLUDE_CURRENT_DIR ON) set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTORCC ON) add_definitions(-DQT_NO_KEYWORDS) catkin_package( diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt index cfc3325fb5..3d029d2095 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt @@ -23,6 +23,7 @@ set(SOURCE_FILES src/motion_planning_display.cpp src/motion_planning_frame_manipulation.cpp src/motion_planning_param_widget.cpp + src/icons/icons.qrc ) set(MOVEIT_LIB_NAME moveit_motion_planning_rviz_plugin) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/edit-clear.png b/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/edit-clear.png new file mode 100644 index 0000000000000000000000000000000000000000..4c309f6f06fa6af6ceca9fac45bdaeb5015e47ed GIT binary patch literal 343 zcmeAS@N?(olHy`uVBq!ia0vp^0wB!63?wyl`GbK}U4T!BE0F#V2EM+&KpH{@1O)i_ z_`G=W;^xhp5LqC3{P^+Bn>YLU`NhS>0m<<2aDRXQpr9b279ayiLKr~35C%{UkO3!G zuU@@o&6;)V)~#Q^e&fcC&z?O4nz=(+e-h9IiX}mQ!3-=MTzn#8%4UAC35jXB^?T1> zx%=$p$4{Ss{P^+n=kLFN` + + list-add.png + list-remove.png + edit-clear.png + + diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/list-add.png b/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/list-add.png new file mode 100644 index 0000000000000000000000000000000000000000..818abbe146d2623207cc17386b10bc2d8c65429e GIT binary patch literal 517 zcmeAS@N?(olHy`uVBq!ia0vp^0wB!63?wyl`GbKJbFq_W2nPqp?T7vkfZWdkJ|V6^ z8U+3`=$7%TP(`S)pt%vN)oAXRnLuaryW-G)#bM$#r%9K6C*N?H0z@}mr(X4&dd+|8P1otS-Dlnint8`#)?Lrp_q=A` z^PY1nY|ee}xz`is-VUFCBXQxa3GxeO zU}T&*2MG4>-+urO4jw#s2ng=po9Z^N8>n)Mr;B5V#O2&`)?9}I1RO3}PoB>4bAiGR z;llsDt=@vI62hkQ&65)+YTWx2B3>uN5EgxK9kT<&f?M2Wd(J%Ycb>y~MA9XAqK(w7 z{de4jW937(7;Y)rd|CHF>14H4M~)pk#93kP=BqgC=4H-`xK&&D^}ULVT@JaaXtQm~ zD_Fn$eCF2K9M6A0Iy~ox=`@uME&Euj{(WQne980N@0mxZf@06p)z4*}Q$iB}FevDJ literal 0 HcmV?d00001 diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/list-remove.png b/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/list-remove.png new file mode 100644 index 0000000000000000000000000000000000000000..213b471c515afffc08d9a82d8b9bb1053973b15c GIT binary patch literal 228 zcmeAS@N?(olHy`uVBq!ia0vp^0wB!61|;P_|4#%`t36#DLn>}1FA$Po-M-e;{{FV0 z|NdNTFB_!`4tc!(|C{ylzy0wRfB*kJYW%N0g6WX)|N027V+v*-JSyx#%wgP@n9UPb zPkZ-&`PQfZ^VzHZ>=$M@(imaj!ncHNjo1qI!~;&LfB%~wa{3>?X*+Y$?f=ss-r#F_ z_{TdThdqq@O0&TN|64EqbILCMukIKj-qt(O&?$y}5lEv$Z^zB&S&5hbFG-jnr*-t9 b1_Q&EnuDuTzo_v8oz39s>gTe~DWM4fHWglt literal 0 HcmV?d00001 diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 9adb5aaa9d..22154c05da 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -1068,8 +1068,8 @@ Add - - .. + + :/icons/list-add.png:/icons/list-add.png @@ -1088,8 +1088,8 @@ Del - - .. + + :/icons/list-remove.png:/icons/list-remove.png @@ -1108,8 +1108,8 @@ Clr - - .. + + :/icons/edit-clear.png:/icons/edit-clear.png From edc9cf6e55ffcd2f3dbe18f2eb8602d913fdde48 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Mon, 13 Jul 2020 17:40:47 +0300 Subject: [PATCH 391/612] Remove urdf package as build_depend from package.xml (#2207) --- .../src/widgets/configuration_files_widget.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index fbb737ecd7..8bab6e4f4c 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -1099,7 +1099,6 @@ void ConfigurationFilesWidget::loadTemplateStrings() else { std::stringstream deps; - deps << "" << config_data_->urdf_pkg_name_ << "\n"; deps << " " << config_data_->urdf_pkg_name_ << "\n"; addTemplateString("[OTHER_DEPENDENCIES]", deps.str()); // not relative to a ROS package } From a8ee3cb11433716baaf9bc23d8aa2a64d799d5c0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 29 Jun 2020 13:11:23 +0200 Subject: [PATCH 392/612] Fix warnings --- .../include/moveit/collision_detection/collision_matrix.h | 2 +- moveit_core/collision_detection/src/collision_matrix.cpp | 8 -------- moveit_core/macros/include/moveit/macros/diagnostics.h | 2 +- moveit_core/robot_state/src/robot_state.cpp | 2 +- moveit_ros/benchmarks/src/BenchmarkExecutor.cpp | 3 +++ .../src/tools/compute_default_collisions.cpp | 2 +- 6 files changed, 7 insertions(+), 12 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index e83283688c..3438fcd469 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -92,7 +92,7 @@ class AllowedCollisionMatrix AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg); /** @brief Copy constructor */ - AllowedCollisionMatrix(const AllowedCollisionMatrix& acm); + AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) = default; /** @brief Get the type of the allowed collision between two elements. Return true if the entry is included in the * collision matrix. diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index 7d359abb3b..74a0c9543f 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -73,14 +73,6 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisi } } -AllowedCollisionMatrix::AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) -{ - entries_ = acm.entries_; - allowed_contacts_ = acm.allowed_contacts_; - default_entries_ = acm.default_entries_; - default_allowed_contacts_ = acm.default_allowed_contacts_; -} - bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const { auto it1 = allowed_contacts_.find(name1); diff --git a/moveit_core/macros/include/moveit/macros/diagnostics.h b/moveit_core/macros/include/moveit/macros/diagnostics.h index 6d3e78caa8..ff75b446b8 100644 --- a/moveit_core/macros/include/moveit/macros/diagnostics.h +++ b/moveit_core/macros/include/moveit/macros/diagnostics.h @@ -42,7 +42,7 @@ * DIAGNOSTIC_PUSH * SILENT_UNUSED_PARAM * #include - * DIAGNOSTIC_PUSH + * DIAGNOSTIC_POP * * The push and pop operations ensure that the old status is restored afterwards. */ diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index ea54bea928..1c171487d7 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -163,7 +163,7 @@ void RobotState::copyFrom(const RobotState& other) (robot_model_->getVariableCount() * (1 + ((has_velocity_ || has_acceleration_ || has_effort_) ? 1 : 0) + ((has_acceleration_ || has_effort_) ? 1 : 0)) + nr_doubles_for_dirty_joint_transforms); - memcpy(variable_joint_transforms_, other.variable_joint_transforms_, bytes); + memcpy((void*)variable_joint_transforms_, (void*)other.variable_joint_transforms_, bytes); } // copy attached bodies diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 3dd68f7b59..44925d4df8 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -40,7 +40,10 @@ #include #include +// TODO: Remove if boost >= 1.72 (https://github.com/boostorg/timer/issues/12) +#define BOOST_ALLOW_DEPRECATED_HEADERS 1 #include +#undef BOOST_ALLOW_DEPRECATED_HEADERS #include #include #include diff --git a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp index 5d534420a2..58421c7b40 100644 --- a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp +++ b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp @@ -667,7 +667,7 @@ DisabledReason disabledReasonFromString(const std::string& reason) { r = REASONS_FROM_STRING.at(reason); } - catch (std::out_of_range) + catch (const std::out_of_range&) { r = USER; } From 7c0d067fef49498812947df5e2308691fc05dfb4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 30 Jun 2020 23:57:49 +0200 Subject: [PATCH 393/612] Travis: Enable Noetic build --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 740788cb87..cb79a380b8 100644 --- a/.travis.yml +++ b/.travis.yml @@ -24,6 +24,7 @@ jobs: include: - env: TEST="clang-format catkin_lint" - env: TEST=code-coverage + - env: ROS_DISTRO=noetic - env: ROS_DISTRO=melodic - env: ROS_DISTRO=kinetic - compiler: clang From 9dd66f905c93b8c5ca86d6d4df531bfb66495973 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 1 Jul 2020 08:21:25 +0200 Subject: [PATCH 394/612] Remove moveit_noetic.rosinstall --- moveit_noetic.rosinstall | 75 ---------------------------------------- 1 file changed, 75 deletions(-) delete mode 100644 moveit_noetic.rosinstall diff --git a/moveit_noetic.rosinstall b/moveit_noetic.rosinstall deleted file mode 100644 index 532928a195..0000000000 --- a/moveit_noetic.rosinstall +++ /dev/null @@ -1,75 +0,0 @@ -################################# -# Temporary Noetic ROS Install -# Remove all the extra source builds once they are released -################################# - -# This file is intended for users who want to build MoveIt from source. -# Used with wstool, users can download source of all packages of MoveIt. -- git: - local-name: moveit_msgs - uri: https://github.com/ros-planning/moveit_msgs.git - version: master -- git: - local-name: geometric_shapes - uri: https://github.com/ros-planning/geometric_shapes.git - version: noetic-devel -- git: - local-name: moveit - uri: https://github.com/ros-planning/moveit.git - version: master -- git: - local-name: rviz_visual_tools - uri: https://github.com/PickNikRobotics/rviz_visual_tools - version: master -- git: - local-name: moveit_visual_tools - uri: https://github.com/ros-planning/moveit_visual_tools.git - version: master -- git: - local-name: moveit_tutorials - uri: https://github.com/ros-planning/moveit_tutorials.git - version: master -- git: - local-name: panda_moveit_config - uri: https://github.com/ros-planning/panda_moveit_config.git - version: melodic-devel - -#################################### -# Remove the following once released -#################################### -- git: - local-name: rosparam_shortcuts - uri: https://github.com/PickNikRobotics/rosparam_shortcuts.git - version: melodic-devel -- git: - local-name: srdfdom - uri: https://github.com/ros-planning/srdfdom.git - version: melodic-devel -- git: - local-name: ompl - uri: https://github.com/ompl/ompl.git - version: master -- git: - local-name: ros_pytest - uri: https://github.com/machinekoder/ros_pytest.git - version: melodic-devel -- git: - local-name: joystick_drivers - uri: https://github.com/ros-drivers/joystick_drivers.git - version: master -- git: - local-name: teleop_tools - uri: https://github.com/ros-teleop/teleop_tools.git - version: kinetic-devel -- git: - local-name: moveit_resources - uri: https://github.com/ros-planning/moveit_resources.git - version: master -- git: - local-name: franka_ros - uri: https://github.com/frankaemika/franka_ros.git - version: kinetic-devel -- git: - local-name: graph_msgs - uri: https://github.com/davetcoleman/graph_msgs.git - version: jade-devel \ No newline at end of file From e894d3892ba220746c3f4dca183ff655ba9e3d32 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 1 Jul 2020 10:57:00 +0200 Subject: [PATCH 395/612] Further increase acceptance threshold for mesh-filter test Mesa OpenGL 20.x seems to have worsen the situation... --- moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp index 2fa4b7a146..e8c90782bc 100644 --- a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp +++ b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp @@ -231,7 +231,7 @@ void MeshFilterTest::test() float sensor_depth = sensor_data_[idx] * FilterTraits::ToMetricScale; if (fabs(sensor_depth - distance_ - shadow_) > epsilon_ && fabs(sensor_depth - distance_) > epsilon_) { - ASSERT_NEAR(filtered_depth[idx], gt_depth[idx], 1e-6); + ASSERT_NEAR(filtered_depth[idx], gt_depth[idx], 1e-4); ASSERT_EQ(filtered_labels[idx], gt_labels[idx]); } } From fe6498061103aaa299b0ef20d3e8a3dce39964d5 Mon Sep 17 00:00:00 2001 From: Jeroen Date: Mon, 20 Jul 2020 17:45:26 +0200 Subject: [PATCH 396/612] Fix docstrings in robot_state.h (#2215) --- .../robot_state/include/moveit/robot_state/robot_state.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index c2d968e773..9048a0d5ce 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1172,8 +1172,8 @@ class RobotState /** \brief Compute the Jacobian with reference to a particular point on a given link, for a specified group. * \param group The group to compute the Jacobian for - * \param link_name The name of the link - * \param reference_point_position The reference point position (with respect to the link specified in link_name) + * \param link The link model to compute the Jacobian for + * \param reference_point_position The reference point position (with respect to the link specified in link) * \param jacobian The resultant jacobian * \param use_quaternion_representation Flag indicating if the Jacobian should use a quaternion representation * (default is false) @@ -1184,8 +1184,8 @@ class RobotState /** \brief Compute the Jacobian with reference to a particular point on a given link, for a specified group. * \param group The group to compute the Jacobian for - * \param link_name The name of the link - * \param reference_point_position The reference point position (with respect to the link specified in link_name) + * \param link The link model to compute the Jacobian for + * \param reference_point_position The reference point position (with respect to the link specified in link) * \param jacobian The resultant jacobian * \param use_quaternion_representation Flag indicating if the Jacobian should use a quaternion representation * (default is false) From 6dbaf94610039ee3a53c2db74f8d244959096b20 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 21 Jul 2020 12:23:35 +0200 Subject: [PATCH 397/612] Travis: Update dist to Bionic (#2221) ... to fix issue https://github.com/moby/moby/pull/36417 --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index cb79a380b8..e8665fb8ff 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,6 +1,6 @@ # This config file for Travis CI utilizes https://github.com/ros-planning/moveit_ci/ package. os: linux -dist: xenial # distro used by Travis, moveit_ci uses the docker image's distro +dist: bionic # distro used by Travis, moveit_ci uses the docker image's distro services: - docker language: cpp From 89a6752133177557fd4419d9f784d0f9c737c5c2 Mon Sep 17 00:00:00 2001 From: Jeroen Date: Tue, 21 Jul 2020 12:55:35 +0200 Subject: [PATCH 398/612] Add named logging with LOGNAME to OMPL interface (#2211) * add named logging with LOGNAME to ompl interface * clang-format --- .../scripts/generate_state_database.cpp | 2 +- .../src/detail/constrained_goal_sampler.cpp | 13 ++- .../constrained_valid_state_sampler.cpp | 8 +- .../src/detail/constraints_library.cpp | 74 ++++++++------- .../src/detail/state_validity_checker.cpp | 77 ++++++++-------- .../src/model_based_planning_context.cpp | 92 +++++++++---------- .../ompl_interface/src/ompl_interface.cpp | 30 +++--- .../src/ompl_planner_manager.cpp | 6 +- .../model_based_state_space.cpp | 77 ++++++++-------- .../work_space/pose_model_state_space.cpp | 73 ++++++++------- .../src/planning_context_manager.cpp | 31 ++++--- 11 files changed, 255 insertions(+), 228 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp b/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp index d089536586..55548966ca 100644 --- a/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp +++ b/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp @@ -49,7 +49,7 @@ #include #include -static const std::string LOGNAME = "generate_state_database"; +constexpr char LOGNAME[] = "generate_state_database"; static const std::string ROBOT_DESCRIPTION = "robot_description"; diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp index ce417e99c2..24a81ebe9f 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp @@ -41,6 +41,11 @@ #include +namespace ompl_interface +{ +constexpr char LOGNAME[] = "constrained_goal_sampler"; +} // namespace ompl_interface + ompl_interface::ConstrainedGoalSampler::ConstrainedGoalSampler(const ModelBasedPlanningContext* pc, kinematic_constraints::KinematicConstraintSetPtr ks, constraint_samplers::ConstraintSamplerPtr cs) @@ -58,7 +63,7 @@ ompl_interface::ConstrainedGoalSampler::ConstrainedGoalSampler(const ModelBasedP { if (!constraint_sampler_) default_sampler_ = si_->allocStateSampler(); - ROS_DEBUG_NAMED("constrained_goal_sampler", "Constructed a ConstrainedGoalSampler instance at address %p", this); + ROS_DEBUG_NAMED(LOGNAME, "Constructed a ConstrainedGoalSampler instance at address %p", this); startSampling(); } @@ -138,9 +143,9 @@ bool ompl_interface::ConstrainedGoalSampler::sampleUsingConstraintSampler(const if (!warned_invalid_samples_ && invalid_sampled_constraints_ >= (attempts_so_far * 8) / 10) { warned_invalid_samples_ = true; - ROS_WARN_NAMED("constrained_goal_sampler", "More than 80%% of the sampled goal states " - "fail to satisfy the constraints imposed on the goal sampler. " - "Is the constrained sampler working correctly?"); + ROS_WARN_NAMED(LOGNAME, "More than 80%% of the sampled goal states " + "fail to satisfy the constraints imposed on the goal sampler. " + "Is the constrained sampler working correctly?"); } } } diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_valid_state_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_valid_state_sampler.cpp index 3ce0351928..db39b8ef33 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constrained_valid_state_sampler.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constrained_valid_state_sampler.cpp @@ -40,6 +40,11 @@ #include +namespace ompl_interface +{ +constexpr char LOGNAME[] = "constrained_valid_state_sampler"; +} // namespace ompl_interface + ompl_interface::ValidConstrainedSampler::ValidConstrainedSampler(const ModelBasedPlanningContext* pc, kinematic_constraints::KinematicConstraintSetPtr ks, constraint_samplers::ConstraintSamplerPtr cs) @@ -52,8 +57,7 @@ ompl_interface::ValidConstrainedSampler::ValidConstrainedSampler(const ModelBase if (!constraint_sampler_) default_sampler_ = si_->allocStateSampler(); inv_dim_ = si_->getStateSpace()->getDimension() > 0 ? 1.0 / (double)si_->getStateSpace()->getDimension() : 1.0; - ROS_DEBUG_NAMED("constrained_valid_state_sampler", "Constructed a ValidConstrainedSampler instance at address %p", - this); + ROS_DEBUG_NAMED(LOGNAME, "Constructed a ValidConstrainedSampler instance at address %p", this); } bool ompl_interface::ValidConstrainedSampler::project(ompl::base::State* state) diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index 96c6f7c92a..ddc6abc453 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -34,17 +34,19 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include #include #include #include +#include +#include +#include +#include #include namespace ompl_interface { +constexpr char LOGNAME[] = "constraints_library"; + namespace { template @@ -227,7 +229,8 @@ ompl_interface::ConstraintApproximation::getStateSamplerAllocator(const moveit_m milestones_); } /* -void ompl_interface::ConstraintApproximation::visualizeDistribution(const std::string &link_name, unsigned int count, +void ompl_interface::ConstraintApproximation::visualizeDistribution(const +std::string &link_name, unsigned int count, visualization_msgs::MarkerArray &arr) const { moveit::core::RobotState robot_state(robot_model_); @@ -246,7 +249,8 @@ visualization_msgs::MarkerArray &arr) const { state_storage_->getStateSpace()->as()->copyToRobotState(robot_state, state_storage_->getState(rng.uniformInt(0, state_storage_->size() - 1))); - const Eigen::Vector3d &pos = robot_state.getLinkState(link_name)->getGlobalLinkTransform().translation(); + const Eigen::Vector3d &pos = +robot_state.getLinkState(link_name)->getGlobalLinkTransform().translation(); visualization_msgs::Marker mk; mk.header.stamp = ros::Time::now(); @@ -273,12 +277,13 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: std::ifstream fin((path + "/manifest").c_str()); if (!fin.good()) { - ROS_WARN_NAMED("constraints_library", "Manifest not found in folder '%s'. Not loading constraint approximations.", + ROS_WARN_NAMED(LOGNAME, "Manifest not found in folder '%s'. Not loading " + "constraint approximations.", path.c_str()); return; } - ROS_INFO_NAMED("constraints_library", "Loading constrained space approximations from '%s'...", path.c_str()); + ROS_INFO_NAMED(LOGNAME, "Loading constrained space approximations from '%s'...", path.c_str()); while (fin.good() && !fin.eof()) { @@ -305,13 +310,14 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: if (context_->getGroupName() != group && context_->getOMPLStateSpace()->getParameterizationType() != state_space_parameterization) { - ROS_INFO_NAMED("constraints_library", - "Ignoring constraint approximation of type '%s' for group '%s' from '%s'...", + ROS_INFO_NAMED(LOGNAME, "Ignoring constraint approximation of type '%s' " + "for group '%s' from '%s'...", state_space_parameterization.c_str(), group.c_str(), filename.c_str()); continue; } - ROS_INFO_NAMED("constraints_library", "Loading constraint approximation of type '%s' for group '%s' from '%s'...", + ROS_INFO_NAMED(LOGNAME, "Loading constraint approximation of type '%s' for " + "group '%s' from '%s'...", state_space_parameterization.c_str(), group.c_str(), filename.c_str()); moveit_msgs::Constraints msg; hexToMsg(serialization, msg); @@ -321,22 +327,23 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: msg, filename, ompl::base::StateStoragePtr(cass), milestones)); if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end()) - ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", cap->getName().c_str()); + ROS_WARN_NAMED(LOGNAME, "Overwriting constraint approximation named '%s'", cap->getName().c_str()); constraint_approximations_[cap->getName()] = cap; std::size_t sum = 0; for (std::size_t i = 0; i < cass->size(); ++i) sum += cass->getMetadata(i).first.size(); - ROS_INFO_NAMED("constraints_library", "Loaded %lu states (%lu milestones) and %lu connections (%0.1lf per state) " - "for constraint named '%s'%s", + ROS_INFO_NAMED(LOGNAME, "Loaded %lu states (%lu milestones) and %lu " + "connections (%0.1lf per state) " + "for constraint named '%s'%s", cass->size(), cap->getMilestoneCount(), sum, (double)sum / (double)cap->getMilestoneCount(), msg.name.c_str(), explicit_motions ? ". Explicit motions included." : ""); } - ROS_INFO_NAMED("constraints_library", "Done loading constrained space approximations."); + ROS_INFO_NAMED(LOGNAME, "Done loading constrained space approximations."); } void ompl_interface::ConstraintsLibrary::saveConstraintApproximations(const std::string& path) { - ROS_INFO_NAMED("constraints_library", "Saving %u constrained space approximations to '%s'", + ROS_INFO_NAMED(LOGNAME, "Saving %u constrained space approximations to '%s'", (unsigned int)constraint_approximations_.size(), path.c_str()); try { @@ -363,7 +370,7 @@ void ompl_interface::ConstraintsLibrary::saveConstraintApproximations(const std: it->second->getStateStorage()->store((path + "/" + it->second->getFilename()).c_str()); } else - ROS_ERROR_NAMED("constraints_library", "Unable to save constraint approximation to '%s'", path.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Unable to save constraint approximation to '%s'", path.c_str()); fout.close(); } @@ -415,7 +422,7 @@ ompl_interface::ConstraintsLibrary::addConstraintApproximation( if (context_->getGroupName() != group && context_->getOMPLStateSpace()->getParameterizationType() != options.state_space_parameterization) { - ROS_INFO_NAMED("constraints_library", "Ignoring constraint approximation of type '%s' for group '%s'...", + ROS_INFO_NAMED(LOGNAME, "Ignoring constraint approximation of type '%s' for group '%s'...", options.state_space_parameterization.c_str(), group.c_str()); return res; } @@ -427,8 +434,7 @@ ompl_interface::ConstraintsLibrary::addConstraintApproximation( ros::WallTime start = ros::WallTime::now(); ompl::base::StateStoragePtr state_storage = constructConstraintApproximation(context_, constr_sampling, constr_hard, options, res); - ROS_INFO_NAMED("constraints_library", "Spent %lf seconds constructing the database", - (ros::WallTime::now() - start).toSec()); + ROS_INFO_NAMED(LOGNAME, "Spent %lf seconds constructing the database", (ros::WallTime::now() - start).toSec()); if (state_storage) { ConstraintApproximationPtr constraint_approx(new ConstraintApproximation( @@ -437,14 +443,12 @@ ompl_interface::ConstraintsLibrary::addConstraintApproximation( ".ompldb", state_storage, res.milestones)); if (constraint_approximations_.find(constraint_approx->getName()) != constraint_approximations_.end()) - ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", - constraint_approx->getName().c_str()); + ROS_WARN_NAMED(LOGNAME, "Overwriting constraint approximation named '%s'", constraint_approx->getName().c_str()); constraint_approximations_[constraint_approx->getName()] = constraint_approx; res.approx = constraint_approx; } else - ROS_ERROR_NAMED("constraints_library", "Unable to construct constraint approximation for group '%s'", - group.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Unable to construct constraint approximation for group '%s'", group.c_str()); return res; } @@ -498,19 +502,19 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra if (done != done_now) { done = done_now; - ROS_INFO_NAMED("constraints_library", "%d%% complete (kept %0.1lf%% sampled states)", done, + ROS_INFO_NAMED(LOGNAME, "%d%% complete (kept %0.1lf%% sampled states)", done, 100.0 * (double)state_storage->size() / (double)attempts); } if (!slow_warn && attempts > 10 && attempts > state_storage->size() * 100) { slow_warn = true; - ROS_WARN_NAMED("constraints_library", "Computation of valid state database is very slow..."); + ROS_WARN_NAMED(LOGNAME, "Computation of valid state database is very slow..."); } if (attempts > options.samples && state_storage->size() == 0) { - ROS_ERROR_NAMED("constraints_library", "Unable to generate any samples"); + ROS_ERROR_NAMED(LOGNAME, "Unable to generate any samples"); break; } @@ -527,19 +531,18 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra } result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start); - ROS_INFO_NAMED("constraints_library", "Generated %u states in %lf seconds", (unsigned int)state_storage->size(), + ROS_INFO_NAMED(LOGNAME, "Generated %u states in %lf seconds", (unsigned int)state_storage->size(), result.state_sampling_time); if (constrained_sampler) { result.sampling_success_rate = constrained_sampler->getConstrainedSamplingRate(); - ROS_INFO_NAMED("constraints_library", "Constrained sampling rate: %lf", result.sampling_success_rate); + ROS_INFO_NAMED(LOGNAME, "Constrained sampling rate: %lf", result.sampling_success_rate); } result.milestones = state_storage->size(); if (options.edges_per_sample > 0) { - ROS_INFO_NAMED("constraints_library", "Computing graph connections (max %u edges per sample) ...", - options.edges_per_sample); + ROS_INFO_NAMED(LOGNAME, "Computing graph connections (max %u edges per sample) ...", options.edges_per_sample); // construct connexions const ob::StateSpacePtr& space = pcontext->getOMPLSimpleSetup()->getStateSpace(); @@ -557,7 +560,7 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra if (done != done_now) { done = done_now; - ROS_INFO_NAMED("constraints_library", "%d%% complete", done); + ROS_INFO_NAMED(LOGNAME, "%d%% complete", done); } if (cass->getMetadata(j).first.size() >= options.edges_per_sample) continue; @@ -613,15 +616,16 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra } result.state_connection_time = ompl::time::seconds(ompl::time::now() - start); - ROS_INFO_NAMED("constraints_library", "Computed possible connexions in %lf seconds. Added %d connexions", + ROS_INFO_NAMED(LOGNAME, "Computed possible connexions in %lf seconds. Added %d connexions", result.state_connection_time, good); pcontext->getOMPLSimpleSetup()->getSpaceInformation()->freeStates(int_states); return state_storage; } - // TODO(davetcoleman): this function did not originally return a value, causing compiler warnings in ROS Melodic + // TODO(davetcoleman): this function did not originally return a value, + // causing compiler warnings in ROS Melodic // Update with more intelligent logic as needed - ROS_ERROR_NAMED("constraints_library", "No StateStoragePtr found - implement better solution here."); + ROS_ERROR_NAMED(LOGNAME, "No StateStoragePtr found - implement better solution here."); return state_storage; } diff --git a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp index 6205ce4dfd..5e03317aca 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -39,6 +39,11 @@ #include #include +namespace ompl_interface +{ +constexpr char LOGNAME[] = "state_validity_checker"; +} // namespace ompl_interface + ompl_interface::StateValidityChecker::StateValidityChecker(const ModelBasedPlanningContext* pc) : ompl::base::StateValidityChecker(pc->getOMPLSimpleSetup()->getSpaceInformation()) , planning_context_(pc) @@ -115,7 +120,7 @@ bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base: if (!si_->satisfiesBounds(state)) { if (verbose) - ROS_INFO_NAMED("state_validity_checker", "State outside bounds"); + ROS_INFO_NAMED(LOGNAME, "State outside bounds"); return false; } @@ -145,7 +150,7 @@ bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base: if (!si_->satisfiesBounds(state)) { if (verbose) - ROS_INFO_NAMED("state_validity_checker", "State outside bounds"); + ROS_INFO_NAMED(LOGNAME, "State outside bounds"); return false; } @@ -187,7 +192,7 @@ bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::St if (!si_->satisfiesBounds(state)) { if (verbose) - ROS_INFO_NAMED("state_validity_checker", "State outside bounds"); + ROS_INFO_NAMED(LOGNAME, "State outside bounds"); const_cast(state)->as()->markInvalid(); return false; } @@ -238,7 +243,7 @@ bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::St if (!si_->satisfiesBounds(state)) { if (verbose) - ROS_INFO_NAMED("state_validity_checker", "State outside bounds"); + ROS_INFO_NAMED(LOGNAME, "State outside bounds"); const_cast(state)->as()->markInvalid(0.0); return false; } diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 657374192a..98e8b6a98f 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -72,6 +72,11 @@ #include "ompl/base/objectives/MaximizeMinClearanceObjective.h" #include +namespace ompl_interface +{ +constexpr char LOGNAME[] = "model_based_planning_context"; +} // namespace ompl_interface + ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std::string& name, const ModelBasedPlanningContextSpecification& spec) : planning_interface::PlanningContext(name, spec.state_space_->getJointModelGroup()->getName()) @@ -126,7 +131,7 @@ void ompl_interface::ModelBasedPlanningContext::configure(const ros::NodeHandle& if (constraint_approx) { getOMPLStateSpace()->setInterpolationFunction(constraint_approx->getInterpolationFunction()); - ROS_INFO_NAMED("model_based_planning_context", "Using precomputed interpolation states"); + ROS_INFO_NAMED(LOGNAME, "Using precomputed interpolation states"); } } @@ -139,7 +144,7 @@ void ompl_interface::ModelBasedPlanningContext::setProjectionEvaluator(const std { if (!spec_.state_space_) { - ROS_ERROR_NAMED("model_based_planning_context", "No state space is configured yet"); + ROS_ERROR_NAMED(LOGNAME, "No state space is configured yet"); return; } ob::ProjectionEvaluatorPtr projection_eval = getProjectionEvaluator(peval); @@ -156,9 +161,8 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str if (getRobotModel()->hasLinkModel(link_name)) return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorLinkPose(this, link_name)); else - ROS_ERROR_NAMED("model_based_planning_context", - "Attempted to set projection evaluator with respect to position of link '%s', " - "but that link is not known to the kinematic model.", + ROS_ERROR_NAMED(LOGNAME, "Attempted to set projection evaluator with respect to position of link '%s', " + "but that link is not known to the kinematic model.", link_name.c_str()); } else if (peval.find_first_of("joints(") == 0 && peval[peval.length() - 1] == ')') @@ -181,24 +185,21 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str j.push_back(idx + q); } else - ROS_WARN_NAMED("model_based_planning_context", "%s: Ignoring joint '%s' in projection since it has 0 DOF", - name_.c_str(), joint.c_str()); + ROS_WARN_NAMED(LOGNAME, "%s: Ignoring joint '%s' in projection since it has 0 DOF", name_.c_str(), + joint.c_str()); } else - ROS_ERROR_NAMED("model_based_planning_context", - "%s: Attempted to set projection evaluator with respect to value of joint " - "'%s', but that joint is not known to the group '%s'.", + ROS_ERROR_NAMED(LOGNAME, "%s: Attempted to set projection evaluator with respect to value of joint " + "'%s', but that joint is not known to the group '%s'.", name_.c_str(), joint.c_str(), getGroupName().c_str()); } if (j.empty()) - ROS_ERROR_NAMED("model_based_planning_context", "%s: No valid joints specified for joint projection", - name_.c_str()); + ROS_ERROR_NAMED(LOGNAME, "%s: No valid joints specified for joint projection", name_.c_str()); else return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorJointValue(this, j)); } else - ROS_ERROR_NAMED("model_based_planning_context", - "Unable to allocate projection evaluator based on description: '%s'", peval.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Unable to allocate projection evaluator based on description: '%s'", peval.c_str()); return ob::ProjectionEvaluatorPtr(); } @@ -207,13 +208,11 @@ ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const omp { if (spec_.state_space_.get() != state_space) { - ROS_ERROR_NAMED("model_based_planning_context", - "%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str()); + ROS_ERROR_NAMED(LOGNAME, "%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str()); return ompl::base::StateSamplerPtr(); } - ROS_DEBUG_NAMED("model_based_planning_context", - "%s: Allocating a new state sampler (attempts to use path constraints)", name_.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "%s: Allocating a new state sampler (attempts to use path constraints)", name_.c_str()); if (path_constraints_) { @@ -230,7 +229,7 @@ ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const omp ompl::base::StateSamplerPtr state_sampler = state_sampler_allocator(state_space); if (state_sampler) { - ROS_INFO_NAMED("model_based_planning_context", + ROS_INFO_NAMED(LOGNAME, "%s: Using precomputed state sampler (approximated constraint space) for constraint '%s'", name_.c_str(), path_constraints_msg_.name.c_str()); return state_sampler; @@ -246,13 +245,11 @@ ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const omp if (constraint_sampler) { - ROS_INFO_NAMED("model_based_planning_context", "%s: Allocating specialized state sampler for state space", - name_.c_str()); + ROS_INFO_NAMED(LOGNAME, "%s: Allocating specialized state sampler for state space", name_.c_str()); return ob::StateSamplerPtr(new ConstrainedSampler(this, constraint_sampler)); } } - ROS_DEBUG_NAMED("model_based_planning_context", "%s: Allocating default state sampler for state space", - name_.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "%s: Allocating default state sampler for state space", name_.c_str()); return state_space->allocDefaultStateSampler(); } @@ -361,8 +358,7 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() if (it == cfg.end()) { if (name_ != getGroupName()) - ROS_WARN_NAMED("model_based_planning_context", "%s: Attribute 'type' not specified in planner configuration", - name_.c_str()); + ROS_WARN_NAMED(LOGNAME, "%s: Attribute 'type' not specified in planner configuration", name_.c_str()); } else { @@ -371,9 +367,8 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() const std::string planner_name = getGroupName() + "/" + name_; ompl_simple_setup_->setPlannerAllocator( std::bind(spec_.planner_selector_(type), std::placeholders::_1, planner_name, std::cref(spec_))); - ROS_INFO_NAMED("model_based_planning_context", - "Planner configuration '%s' will use planner '%s'. " - "Additional configuration parameters will be set when the planner is constructed.", + ROS_INFO_NAMED(LOGNAME, "Planner configuration '%s' will use planner '%s'. " + "Additional configuration parameters will be set when the planner is constructed.", name_.c_str(), type.c_str()); } @@ -389,11 +384,10 @@ void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_m if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 && wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 && wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0) - ROS_WARN_NAMED("model_based_planning_context", "It looks like the planning volume was not specified."); + ROS_WARN_NAMED(LOGNAME, "It looks like the planning volume was not specified."); - ROS_DEBUG_NAMED("model_based_planning_context", - "%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = " - "[%f, %f], z = [%f, %f]", + ROS_DEBUG_NAMED(LOGNAME, "%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = " + "[%f, %f], z = [%f, %f]", name_.c_str(), wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y, wparams.max_corner.y, wparams.min_corner.z, wparams.max_corner.z); @@ -481,7 +475,7 @@ ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal() if (!goals.empty()) return goals.size() == 1 ? goals[0] : ompl::base::GoalPtr(new GoalSampleableRegionMux(goals)); else - ROS_ERROR_NAMED("model_based_planning_context", "Unable to construct goal representation"); + ROS_ERROR_NAMED(LOGNAME, "Unable to construct goal representation"); return ob::GoalPtr(); } @@ -497,7 +491,7 @@ ompl::base::PlannerTerminationCondition ompl_interface::ModelBasedPlanningContex boost::split(termination_and_params, termination_string, boost::is_any_of("[ ,]")); if (termination_and_params.empty()) - ROS_ERROR_NAMED("model_based_planning_context", "Termination condition not specified"); + ROS_ERROR_NAMED(LOGNAME, "Termination condition not specified"); // Terminate if a maximum number of iterations is exceeded or a timeout occurs. // The semantics of "iterations" are planner-specific, but typically it corresponds to the number of times // an attempt was made to grow a roadmap/tree. @@ -508,7 +502,7 @@ ompl::base::PlannerTerminationCondition ompl_interface::ModelBasedPlanningContex ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)), ob::IterationTerminationCondition(std::stoul(termination_and_params[1]))); else - ROS_ERROR_NAMED("model_based_planning_context", "Missing argument to Iteration termination condition"); + ROS_ERROR_NAMED(LOGNAME, "Missing argument to Iteration termination condition"); } // TODO: remove when ROS Melodic and older are no longer supported #if OMPL_VERSION_VALUE >= 1005000 @@ -539,7 +533,7 @@ ompl::base::PlannerTerminationCondition ompl_interface::ModelBasedPlanningContex ob::exactSolnPlannerTerminationCondition(ompl_simple_setup_->getProblemDefinition())); } else - ROS_ERROR_NAMED("model_based_planning_context", "Unknown planner termination condition"); + ROS_ERROR_NAMED(LOGNAME, "Unknown planner termination condition"); // return a planner termination condition to suppress compiler warning return ob::plannerAlwaysTerminatingCondition(); @@ -606,8 +600,7 @@ bool ompl_interface::ModelBasedPlanningContext::setGoalConstraints( if (goal_constraints_.empty()) { - ROS_WARN_NAMED("model_based_planning_context", "%s: No goal constraints specified. There is no problem to solve.", - name_.c_str()); + ROS_WARN_NAMED(LOGNAME, "%s: No goal constraints specified. There is no problem to solve.", name_.c_str()); if (error) error->val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; return false; @@ -672,10 +665,10 @@ void ompl_interface::ModelBasedPlanningContext::postSolve() stopSampling(); int v = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getValidMotionCount(); int iv = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getInvalidMotionCount(); - ROS_DEBUG_NAMED("model_based_planning_context", "There were %d valid motions and %d invalid motions.", v, iv); + ROS_DEBUG_NAMED(LOGNAME, "There were %d valid motions and %d invalid motions.", v, iv); if (ompl_simple_setup_->getProblemDefinition()->hasApproximateSolution()) - ROS_WARN_NAMED("model_based_planning_context", "Computed solution is approximate"); + ROS_WARN_NAMED(LOGNAME, "Computed solution is approximate"); } bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res) @@ -693,8 +686,8 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion interpolateSolution(); // fill the response - ROS_DEBUG_NAMED("model_based_planning_context", "%s: Returning successful solution with %lu states", - getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount()); + ROS_DEBUG_NAMED(LOGNAME, "%s: Returning successful solution with %lu states", getName().c_str(), + getOMPLSimpleSetup()->getSolutionPath().getStateCount()); res.trajectory_.reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName())); getSolutionPath(*res.trajectory_); @@ -703,7 +696,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion } else { - ROS_INFO_NAMED("model_based_planning_context", "Unable to solve the planning problem"); + ROS_INFO_NAMED(LOGNAME, "Unable to solve the planning problem"); res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; return false; } @@ -746,13 +739,13 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion } // fill the response - ROS_DEBUG_NAMED("model_based_planning_context", "%s: Returning successful solution with %lu states", - getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount()); + ROS_DEBUG_NAMED(LOGNAME, "%s: Returning successful solution with %lu states", getName().c_str(), + getOMPLSimpleSetup()->getSolutionPath().getStateCount()); return true; } else { - ROS_INFO_NAMED("model_based_planning_context", "Unable to solve the planning problem"); + ROS_INFO_NAMED(LOGNAME, "Unable to solve the planning problem"); res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; return false; } @@ -767,7 +760,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i bool result = false; if (count <= 1 || multi_query_planning_enabled_) // multi-query planners should always run in single instances { - ROS_DEBUG_NAMED("model_based_planning_context", "%s: Solving the planning problem once...", name_.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "%s: Solving the planning problem once...", name_.c_str()); ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); registerTerminationCondition(ptc); result = ompl_simple_setup_->solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION; @@ -776,8 +769,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i } else { - ROS_DEBUG_NAMED("model_based_planning_context", "%s: Solving the planning problem %u times...", name_.c_str(), - count); + ROS_DEBUG_NAMED(LOGNAME, "%s: Solving the planning problem %u times...", name_.c_str(), count); ompl_parallel_plan_.clearHybridizationPaths(); if (count <= max_planning_threads_) { @@ -864,7 +856,7 @@ bool ompl_interface::ModelBasedPlanningContext::saveConstraintApproximations(con constraints_library_->saveConstraintApproximations(constraint_path); return true; } - ROS_WARN("ROS param 'constraint_approximations' not found. Unable to save constraint approximations"); + ROS_WARN_NAMED(LOGNAME, "ROS param 'constraint_approximations' not found. Unable to save constraint approximations"); return false; } diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index f01b70e8d7..db37000d87 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -42,6 +42,11 @@ #include #include +namespace ompl_interface +{ +constexpr char LOGNAME[] = "ompl_interface"; +} // namespace ompl_interface + ompl_interface::OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const ros::NodeHandle& nh) : nh_(nh) @@ -51,7 +56,7 @@ ompl_interface::OMPLInterface::OMPLInterface(const moveit::core::RobotModelConst , use_constraints_approximations_(true) , simplify_solutions_(true) { - ROS_DEBUG("Initializing OMPL interface using ROS parameters"); + ROS_DEBUG_NAMED(LOGNAME, "Initializing OMPL interface using ROS parameters"); loadPlannerConfigurations(); loadConstraintSamplers(); } @@ -66,7 +71,7 @@ ompl_interface::OMPLInterface::OMPLInterface(const moveit::core::RobotModelConst , use_constraints_approximations_(true) , simplify_solutions_(true) { - ROS_DEBUG("Initializing OMPL interface using specified configuration"); + ROS_DEBUG_NAMED(LOGNAME, "Initializing OMPL interface using specified configuration"); setPlannerConfigurations(pconfig); loadConstraintSamplers(); } @@ -129,14 +134,14 @@ bool ompl_interface::OMPLInterface::loadPlannerConfiguration( XmlRpc::XmlRpcValue xml_config; if (!nh_.getParam("planner_configs/" + planner_id, xml_config)) { - ROS_ERROR("Could not find the planner configuration '%s' on the param server", planner_id.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Could not find the planner configuration '%s' on the param server", planner_id.c_str()); return false; } if (xml_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("A planning configuration should be of type XmlRpc Struct type (for configuration '%s')", - planner_id.c_str()); + ROS_ERROR_NAMED(LOGNAME, "A planning configuration should be of type XmlRpc Struct type (for configuration '%s')", + planner_id.c_str()); return false; } @@ -237,9 +242,9 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() { if (config_names.getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_ERROR("The planner_configs argument of a group configuration " - "should be an array of strings (for group '%s')", - group_name.c_str()); + ROS_ERROR_NAMED(LOGNAME, "The planner_configs argument of a group configuration " + "should be an array of strings (for group '%s')", + group_name.c_str()); continue; } @@ -247,7 +252,8 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() { if (config_names[j].getType() != XmlRpc::XmlRpcValue::TypeString) { - ROS_ERROR("Planner configuration names must be of type string (for group '%s')", group_name.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Planner configuration names must be of type string (for group '%s')", + group_name.c_str()); continue; } @@ -262,10 +268,10 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() for (const std::pair& config : pconfig) { - ROS_DEBUG_STREAM_NAMED("parameters", "Parameters for configuration '" << config.first << "'"); + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Parameters for configuration '" << config.first << "'"); for (const std::pair& parameters : config.second.config) - ROS_DEBUG_STREAM_NAMED("parameters", " - " << parameters.first << " = " << parameters.second); + ROS_DEBUG_STREAM_NAMED(LOGNAME, " - " << parameters.first << " = " << parameters.second); } setPlannerConfigurations(pconfig); @@ -273,5 +279,5 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() void ompl_interface::OMPLInterface::printStatus() { - ROS_INFO("OMPL ROS interface is running."); + ROS_INFO_NAMED(LOGNAME, "OMPL ROS interface is running."); } diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index 8d284feb38..4e213d775f 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -56,6 +56,8 @@ namespace ompl_interface { using namespace moveit_planners_ompl; +constexpr char LOGNAME[] = "ompl_planner_manager"; + #define OMPL_ROS_LOG(ros_log_level) \ { \ ROSCONSOLE_DEFINE_LOCATION(true, ros_log_level, ROSCONSOLE_NAME_PREFIX ".ompl"); \ @@ -265,13 +267,13 @@ class OMPLPlannerManager : public planning_interface::PlannerManager { pub_markers_.shutdown(); planner_data_link_name_.clear(); - ROS_INFO("Not displaying OMPL exploration data structures."); + ROS_INFO_NAMED(LOGNAME, "Not displaying OMPL exploration data structures."); } else if (!config.link_for_exploration_tree.empty() && planner_data_link_name_.empty()) { pub_markers_ = nh_.advertise("ompl_planner_data_marker_array", 5); planner_data_link_name_ = config.link_for_exploration_tree; - ROS_INFO("Displaying OMPL exploration data structures for %s", planner_data_link_name_.c_str()); + ROS_INFO_NAMED(LOGNAME, "Displaying OMPL exploration data structures for %s", planner_data_link_name_.c_str()); } ompl_interface_->simplifySolutions(config.simplify_solutions); diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp index 5e359da1e4..201fe2e14a 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp @@ -1,42 +1,47 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ #include #include +namespace ompl_interface +{ +constexpr char LOGNAME[] = "model_based_state_space"; +} // namespace ompl_interface + ompl_interface::ModelBasedStateSpace::ModelBasedStateSpace(ModelBasedStateSpaceSpecification spec) : ompl::base::StateSpace(), spec_(std::move(spec)) { @@ -49,8 +54,7 @@ ompl_interface::ModelBasedStateSpace::ModelBasedStateSpace(ModelBasedStateSpaceS // make sure we have bounds for every joint stored within the spec (use default bounds if not specified) if (!spec_.joint_bounds_.empty() && spec_.joint_bounds_.size() != joint_model_vector_.size()) { - ROS_ERROR_NAMED("model_based_state_space", - "Joint group '%s' has incorrect bounds specified. Using the default bounds instead.", + ROS_ERROR_NAMED(LOGNAME, "Joint group '%s' has incorrect bounds specified. Using the default bounds instead.", spec_.joint_model_group_->getName().c_str()); spec_.joint_bounds_.clear(); } @@ -86,9 +90,8 @@ double ompl_interface::ModelBasedStateSpace::getTagSnapToSegment() const void ompl_interface::ModelBasedStateSpace::setTagSnapToSegment(double snap) { if (snap < 0.0 || snap > 1.0) - ROS_WARN_NAMED("model_based_state_space", - "Snap to segment for tags is a ratio. It's value must be between 0.0 and 1.0. " - "Value remains as previously set (%lf)", + ROS_WARN_NAMED(LOGNAME, "Snap to segment for tags is a ratio. It's value must be between 0.0 and 1.0. " + "Value remains as previously set (%lf)", tag_snap_to_segment_); else { diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index 97b27e0884..771dd5973c 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -40,6 +40,11 @@ #include +namespace ompl_interface +{ +constexpr char LOGNAME[] = "pose_model_state_space"; +} // namespace ompl_interface + const std::string ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE = "PoseModel"; ompl_interface::PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec) @@ -56,8 +61,8 @@ ompl_interface::PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSp poses_.emplace_back(it.first, it.second); } if (poses_.empty()) - ROS_ERROR_NAMED("pose_model_state_space", "No kinematics solvers specified. Unable to construct a " - "PoseModelStateSpace"); + ROS_ERROR_NAMED(LOGNAME, "No kinematics solvers specified. Unable to construct a " + "PoseModelStateSpace"); else std::sort(poses_.begin(), poses_.end()); setName(getName() + "_" + PARAMETERIZATION_TYPE); diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 8fb562fabf..fdcde4edbb 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -77,6 +77,8 @@ using namespace std::placeholders; namespace ompl_interface { +constexpr char LOGNAME[] = "planning_context_manager"; + struct PlanningContextManager::CachedContexts { std::map, std::vector > contexts_; @@ -169,7 +171,7 @@ ompl::base::PlannerPtr ompl_interface::MultiQueryPlannerAllocator::allocatePlann storage_.load(file_path.c_str(), data); planner.reset(allocatePersistentPlanner(data)); if (!planner) - ROS_ERROR_NAMED("planning_context_manager", + ROS_ERROR_NAMED(LOGNAME, "Creating a '%s' planner from persistent data is not supported. Going to create a new instance.", new_name.c_str()); } @@ -249,7 +251,7 @@ ompl_interface::PlanningContextManager::plannerSelector(const std::string& plann return it->second; else { - ROS_ERROR_NAMED("planning_context_manager", "Unknown planner: '%s'", planner.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Unknown planner: '%s'", planner.c_str()); return ConfiguredPlannerAllocator(); } } @@ -326,7 +328,7 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana for (const ModelBasedPlanningContextPtr& cached_context : cached_contexts->second) if (cached_context.unique()) { - ROS_DEBUG_NAMED("planning_context_manager", "Reusing cached planning context"); + ROS_DEBUG_NAMED(LOGNAME, "Reusing cached planning context"); context = cached_context; break; } @@ -365,7 +367,7 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana } } - ROS_DEBUG_NAMED("planning_context_manager", "Creating new planning context"); + ROS_DEBUG_NAMED(LOGNAME, "Creating new planning context"); context.reset(new ModelBasedPlanningContext(config.name, context_spec)); context->useStateValidityCache(state_validity_cache); { @@ -395,7 +397,7 @@ const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningCo return f->second; else { - ROS_ERROR_NAMED("planning_context_manager", "Factory of type '%s' was not found", factory_type.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Factory of type '%s' was not found", factory_type.c_str()); static const ModelBasedStateSpaceFactoryPtr EMPTY; return EMPTY; } @@ -419,14 +421,14 @@ const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningCo if (best == state_space_factories_.end()) { - ROS_ERROR_NAMED("planning_context_manager", "There are no known state spaces that can represent the given planning " - "problem"); + ROS_ERROR_NAMED(LOGNAME, "There are no known state spaces that can represent the given planning " + "problem"); static const ModelBasedStateSpaceFactoryPtr EMPTY; return EMPTY; } else { - ROS_DEBUG_NAMED("planning_context_manager", "Using '%s' parameterization for solving problem", best->first.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "Using '%s' parameterization for solving problem", best->first.c_str()); return best->second; } } @@ -437,7 +439,7 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana { if (req.group_name.empty()) { - ROS_ERROR_NAMED("planning_context_manager", "No group specified to plan for"); + ROS_ERROR_NAMED(LOGNAME, "No group specified to plan for"); error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return ModelBasedPlanningContextPtr(); } @@ -446,7 +448,7 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana if (!planning_scene) { - ROS_ERROR_NAMED("planning_context_manager", "No planning scene supplied as input"); + ROS_ERROR_NAMED(LOGNAME, "No planning scene supplied as input"); return ModelBasedPlanningContextPtr(); } @@ -458,7 +460,7 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana req.group_name + "[" + req.planner_id + "]" : req.planner_id); if (pc == planner_configs_.end()) - ROS_WARN_NAMED("planning_context_manager", + ROS_WARN_NAMED(LOGNAME, "Cannot find planning configuration for group '%s' using planner '%s'. Will use defaults instead.", req.group_name.c_str(), req.planner_id.c_str()); } @@ -468,8 +470,7 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana pc = planner_configs_.find(req.group_name); if (pc == planner_configs_.end()) { - ROS_ERROR_NAMED("planning_context_manager", "Cannot find planning configuration for group '%s'", - req.group_name.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Cannot find planning configuration for group '%s'", req.group_name.c_str()); return ModelBasedPlanningContextPtr(); } } @@ -513,12 +514,12 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana try { context->configure(nh, use_constraints_approximation); - ROS_DEBUG_NAMED("planning_context_manager", "%s: New planning context is set.", context->getName().c_str()); + ROS_DEBUG_NAMED(LOGNAME, "%s: New planning context is set.", context->getName().c_str()); error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; } catch (ompl::Exception& ex) { - ROS_ERROR_NAMED("planning_context_manager", "OMPL encountered an error: %s", ex.what()); + ROS_ERROR_NAMED(LOGNAME, "OMPL encountered an error: %s", ex.what()); context.reset(); } } From dc909df5d2cf93868a1773db869e6767e339cf7a Mon Sep 17 00:00:00 2001 From: Jeroen Date: Mon, 27 Jul 2020 17:04:28 +0200 Subject: [PATCH 399/612] Remove dead code from ompl interface (related to subspaces) (#2214) * remove unused subspaces option and code from ompl interface * remove unused code from StateValidityChecker in ompl interface --- .../detail/state_validity_checker.h | 6 - .../model_based_planning_context.h | 13 -- .../src/detail/state_validity_checker.cpp | 143 ++++-------------- .../src/model_based_planning_context.cpp | 1 - .../src/planning_context_manager.cpp | 20 --- 5 files changed, 30 insertions(+), 153 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h index 0b462b25f6..3d3e31a92f 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h @@ -70,12 +70,6 @@ class StateValidityChecker : public ompl::base::StateValidityChecker void setVerbose(bool flag); protected: - bool isValidWithoutCache(const ompl::base::State* state, bool verbose) const; - bool isValidWithoutCache(const ompl::base::State* state, double& dist, bool verbose) const; - - bool isValidWithCache(const ompl::base::State* state, bool verbose) const; - bool isValidWithCache(const ompl::base::State* state, double& dist, bool verbose) const; - const ModelBasedPlanningContext* planning_context_; std::string group_name_; TSStateStorage tss_; diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index d9792e506e..66600fe501 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -68,7 +68,6 @@ struct ModelBasedPlanningContextSpecification constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; ModelBasedStateSpacePtr state_space_; - std::vector subspaces_; og::SimpleSetupPtr ompl_simple_setup_; // pass in the correct simple setup type }; @@ -255,16 +254,6 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext return constraints_library_; } - bool useStateValidityCache() const - { - return use_state_validity_cache_; - } - - void useStateValidityCache(bool flag) - { - use_state_validity_cache_ = flag; - } - bool simplifySolutions() const { return simplify_solutions_; @@ -426,8 +415,6 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext /// needed) unsigned int minimum_waypoint_count_; - bool use_state_validity_cache_; - /// when false, clears planners before running solve() bool multi_query_planning_enabled_; diff --git a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp index 5e03317aca..e2b43f2d74 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp @@ -75,117 +75,7 @@ void ompl_interface::StateValidityChecker::setVerbose(bool flag) bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, bool verbose) const { - // moveit::Profiler::ScopedBlock sblock("isValid"); - return planning_context_->useStateValidityCache() ? isValidWithCache(state, verbose) : - isValidWithoutCache(state, verbose); -} - -bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, double& dist, bool verbose) const -{ - // moveit::Profiler::ScopedBlock sblock("isValid"); - return planning_context_->useStateValidityCache() ? isValidWithCache(state, dist, verbose) : - isValidWithoutCache(state, dist, verbose); -} - -double ompl_interface::StateValidityChecker::cost(const ompl::base::State* state) const -{ - double cost = 0.0; - - moveit::core::RobotState* robot_state = tss_.getStateStorage(); - planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); - - // Calculates cost from a summation of distance to obstacles times the size of the obstacle - collision_detection::CollisionResult res; - planning_context_->getPlanningScene()->checkCollision(collision_request_with_cost_, res, *robot_state); - - for (const collision_detection::CostSource& cost_source : res.cost_sources) - cost += cost_source.cost * cost_source.getVolume(); - - return cost; -} - -double ompl_interface::StateValidityChecker::clearance(const ompl::base::State* state) const -{ - moveit::core::RobotState* robot_state = tss_.getStateStorage(); - planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); - - collision_detection::CollisionResult res; - planning_context_->getPlanningScene()->checkCollision(collision_request_with_distance_, res, *robot_state); - return res.collision ? 0.0 : (res.distance < 0.0 ? std::numeric_limits::infinity() : res.distance); -} - -bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base::State* state, bool verbose) const -{ - // check bounds - if (!si_->satisfiesBounds(state)) - { - if (verbose) - ROS_INFO_NAMED(LOGNAME, "State outside bounds"); - return false; - } - - // convert ompl state to MoveIt robot state - moveit::core::RobotState* robot_state = tss_.getStateStorage(); - planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); - - // check path constraints - const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints(); - if (kset && !kset->decide(*robot_state, verbose).satisfied) - return false; - - // check feasibility - if (!planning_context_->getPlanningScene()->isStateFeasible(*robot_state, verbose)) - return false; - - // check collision avoidance - collision_detection::CollisionResult res; - planning_context_->getPlanningScene()->checkCollision( - verbose ? collision_request_simple_verbose_ : collision_request_simple_, res, *robot_state); - return !res.collision; -} - -bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base::State* state, double& dist, - bool verbose) const -{ - if (!si_->satisfiesBounds(state)) - { - if (verbose) - ROS_INFO_NAMED(LOGNAME, "State outside bounds"); - return false; - } - - moveit::core::RobotState* robot_state = tss_.getStateStorage(); - planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); - - // check path constraints - const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints(); - if (kset) - { - kinematic_constraints::ConstraintEvaluationResult cer = kset->decide(*robot_state, verbose); - if (!cer.satisfied) - { - dist = cer.distance; - return false; - } - } - - // check feasibility - if (!planning_context_->getPlanningScene()->isStateFeasible(*robot_state, verbose)) - { - dist = 0.0; - return false; - } - - // check collision avoidance - collision_detection::CollisionResult res; - planning_context_->getPlanningScene()->checkCollision( - verbose ? collision_request_with_distance_verbose_ : collision_request_with_distance_, res, *robot_state); - dist = res.distance; - return !res.collision; -} - -bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::State* state, bool verbose) const -{ + // Use cached validity if it is available if (state->as()->isValidityKnown()) return state->as()->isMarkedValid(); @@ -230,9 +120,9 @@ bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::St return !res.collision; } -bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::State* state, double& dist, - bool verbose) const +bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, double& dist, bool verbose) const { + // Use cached validity and distance if they are available if (state->as()->isValidityKnown() && state->as()->isGoalDistanceKnown()) { @@ -278,3 +168,30 @@ bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::St dist = res.distance; return !res.collision; } + +double ompl_interface::StateValidityChecker::cost(const ompl::base::State* state) const +{ + double cost = 0.0; + + moveit::core::RobotState* robot_state = tss_.getStateStorage(); + planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); + + // Calculates cost from a summation of distance to obstacles times the size of the obstacle + collision_detection::CollisionResult res; + planning_context_->getPlanningScene()->checkCollision(collision_request_with_cost_, res, *robot_state); + + for (const collision_detection::CostSource& cost_source : res.cost_sources) + cost += cost_source.cost * cost_source.getVolume(); + + return cost; +} + +double ompl_interface::StateValidityChecker::clearance(const ompl::base::State* state) const +{ + moveit::core::RobotState* robot_state = tss_.getStateStorage(); + planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); + + collision_detection::CollisionResult res; + planning_context_->getPlanningScene()->checkCollision(collision_request_with_distance_, res, *robot_state); + return res.collision ? 0.0 : (res.distance < 0.0 ? std::numeric_limits::infinity() : res.distance); +} \ No newline at end of file diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 98e8b6a98f..284e12773a 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -94,7 +94,6 @@ ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std:: , max_planning_threads_(0) , max_solution_segment_length_(0.0) , minimum_waypoint_count_(0) - , use_state_validity_cache_(true) , multi_query_planning_enabled_(false) // maintain "old" behavior by default , simplify_solutions_(true) , interpolate_(true) diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index fdcde4edbb..bb8c835233 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -348,28 +348,8 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana // Choose the correct simple setup type to load context_spec.ompl_simple_setup_.reset(new ompl::geometric::SimpleSetup(context_spec.state_space_)); - bool state_validity_cache = true; - if (config.config.find("subspaces") != config.config.end()) - { - context_spec.config_.erase("subspaces"); - // if the planner operates at subspace level the cache may be unsafe - state_validity_cache = false; - boost::char_separator sep(" "); - boost::tokenizer > tok(config.config.at("subspaces"), sep); - for (boost::tokenizer >::iterator beg = tok.begin(); beg != tok.end(); ++beg) - { - const ompl_interface::ModelBasedStateSpaceFactoryPtr& sub_fact = factory_selector(*beg); - if (sub_fact) - { - ModelBasedStateSpaceSpecification sub_space_spec(robot_model_, *beg); - context_spec.subspaces_.push_back(sub_fact->getNewStateSpace(sub_space_spec)); - } - } - } - ROS_DEBUG_NAMED(LOGNAME, "Creating new planning context"); context.reset(new ModelBasedPlanningContext(config.name, context_spec)); - context->useStateValidityCache(state_validity_cache); { std::unique_lock slock(cached_contexts_->lock_); cached_contexts_->contexts_[std::make_pair(config.name, factory->getType())].push_back(context); From 2f0b5bb4998c958e4e7191ea7b17c4eb452139f0 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Tue, 28 Jul 2020 20:12:32 +0100 Subject: [PATCH 400/612] Exposed missing parameters from the MoveGroup Python interface (#2201) * Exposed the wait_for_servers parameter in the Pyhton interface * Add missing getPlannerId interface from MoveGroup PythonWrapper --- moveit_commander/src/moveit_commander/move_group.py | 6 +++--- .../move_group_interface/src/wrap_python_move_group.cpp | 8 +++++++- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index 8290e880ea..074c3a3922 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/moveit_commander/src/moveit_commander/move_group.py @@ -48,9 +48,9 @@ class MoveGroupCommander(object): Execution of simple commands for a particular group """ - def __init__(self, name, robot_description="robot_description", ns=""): + def __init__(self, name, robot_description="robot_description", ns="", wait_for_servers=5.0): """ Specify the group name for which to construct this commander instance. Throws an exception if there is an initialization error. """ - self._g = _moveit_move_group_interface.MoveGroupInterface(name, robot_description, ns) + self._g = _moveit_move_group_interface.MoveGroupInterface(name, robot_description, ns, wait_for_servers) def get_name(self): """ Get the name of the group this instance was initialized for """ @@ -448,7 +448,7 @@ def set_planner_id(self, planner_id): def get_planner_id(self): """ Get the current planner_id """ - self._g.get_planner_id() + return self._g.get_planner_id() def set_num_planning_attempts(self, num_planning_attempts): """ Set the number of times the motion plan is to be computed from scratch before the shortest solution is returned. The default value is 1. """ diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 7b2e293c77..7d84ed7405 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -381,6 +381,11 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return getName().c_str(); } + const char* getPlannerIdCStr() const + { + return getPlannerId().c_str(); + } + bp::dict getNamedTargetValuesPython(const std::string& name) { bp::dict output; @@ -576,7 +581,7 @@ static void wrap_move_group_interface() eigenpy::enableEigenPy(); bp::class_ move_group_interface_class( - "MoveGroupInterface", bp::init>()); + "MoveGroupInterface", bp::init>()); move_group_interface_class.def("async_move", &MoveGroupInterfaceWrapper::asyncMovePython); move_group_interface_class.def("move", &MoveGroupInterfaceWrapper::movePython); @@ -695,6 +700,7 @@ static void wrap_move_group_interface() move_group_interface_class.def("set_max_acceleration_scaling_factor", &MoveGroupInterfaceWrapper::setMaxAccelerationScalingFactor); move_group_interface_class.def("set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId); + move_group_interface_class.def("get_planner_id", &MoveGroupInterfaceWrapper::getPlannerIdCStr); move_group_interface_class.def("set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts); move_group_interface_class.def("plan", &MoveGroupInterfaceWrapper::planPython); move_group_interface_class.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython); From 909371645c98197a56153c166c32c92ef14f4634 Mon Sep 17 00:00:00 2001 From: Jeroen Date: Wed, 29 Jul 2020 16:54:41 +0200 Subject: [PATCH 401/612] Add documentation to ompl interface (#2226) --- .../moveit/ompl_interface/model_based_planning_context.h | 7 +++++++ .../moveit/ompl_interface/planning_context_manager.h | 9 +++++++++ 2 files changed, 16 insertions(+) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index 66600fe501..e640fe71ce 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -320,6 +320,13 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext * approximations to */ bool saveConstraintApproximations(const ros::NodeHandle& nh); + /** \brief Configure ompl_simple_setup_ and optionally the constraints_library_. + * + * ompl_simple_setup_ gets a start state, state sampler, and state validity checker. + * + * \param nh ROS node handle used to load the constraint approximations. + * \param use_constraints_approximations Set to true if we want to load the constraint approximation. + * */ virtual void configure(const ros::NodeHandle& nh, bool use_constraints_approximations); protected: diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index fc5c71b792..4b213c3409 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -170,6 +170,15 @@ class PlanningContextManager return robot_model_; } + /** \brief Returns a planning context to OMPLInterface, which in turn passes it to OMPLPlannerManager. + * + * This function checks the input and reads planner specific configurations. + * Then it creates the planning context with PlanningContextManager::createPlanningContext. + * Finally, it puts the context into a state appropriate for planning. + * This last step involves setting the start, goal, and state validity checker using the method + * ModelBasedPlanningContext::configure. + * + * */ ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, moveit_msgs::MoveItErrorCodes& error_code, const ros::NodeHandle& nh, From 5136e394c357b55b87c011dda3b8d87f85fba1c2 Mon Sep 17 00:00:00 2001 From: Jeroen Date: Wed, 29 Jul 2020 17:52:18 +0200 Subject: [PATCH 402/612] OMPL interface: fix / ignore clang-tidy warnings (#2227) * fix / ignore clang-tidy warnings in ompl interface * remove NOLINT and fix warning in ompl interface --- .../ompl_interface/model_based_planning_context.h | 4 ++-- .../src/detail/constraints_library.cpp | 2 +- .../src/model_based_planning_context.cpp | 6 +++--- .../ompl/ompl_interface/src/ompl_interface.cpp | 14 +++++++++----- 4 files changed, 15 insertions(+), 11 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index e640fe71ce..64918929d7 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -351,8 +351,8 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext * `Iteration[num]`: Terminate after `num` iterations. Here, `num` should be replaced with a positive integer. - * `CostConvergence[solutionsWindow,epsilon]`: Terminate after the cost (as specified - by an optimization objective) has converged. The parameter `solutionsWindow` + * `CostConvergence[solutions_window,epsilon]`: Terminate after the cost (as specified + by an optimization objective) has converged. The parameter `solutions_window` specifies the minimum number of solutions to use in deciding whether a planner has converged. The parameter `epsilon` is the threshold to consider for convergence. This should be a positive number close to 0. If the cumulative moving average does diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index ddc6abc453..19bdc77dde 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -322,7 +322,7 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: moveit_msgs::Constraints msg; hexToMsg(serialization, msg); auto* cass = new ConstraintApproximationStateStorage(context_->getOMPLSimpleSetup()->getStateSpace()); - cass->load((path + "/" + filename).c_str()); + cass->load((std::string{ path }.append("/").append(filename)).c_str()); ConstraintApproximationPtr cap(new ConstraintApproximation(group, state_space_parameterization, explicit_motions, msg, filename, ompl::base::StateStoragePtr(cass), milestones)); diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 284e12773a..8040d8de39 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -509,17 +509,17 @@ ompl::base::PlannerTerminationCondition ompl_interface::ModelBasedPlanningContex // Only useful for anytime/optimizing planners. else if (termination_and_params[0] == "CostConvergence") { - std::size_t solutionsWindow = 10u; + std::size_t solutions_window = 10u; double epsilon = 0.1; if (termination_and_params.size() > 1) { - solutionsWindow = std::stoul(termination_and_params[1]); + solutions_window = std::stoul(termination_and_params[1]); if (termination_and_params.size() > 2) epsilon = moveit::core::toDouble(termination_and_params[2]); } return ob::plannerOrTerminationCondition( ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)), - ob::CostConvergenceTerminationCondition(ompl_simple_setup_->getProblemDefinition(), solutionsWindow, epsilon)); + ob::CostConvergenceTerminationCondition(ompl_simple_setup_->getProblemDefinition(), solutions_window, epsilon)); } #endif // Terminate as soon as an exact solution is found or a timeout occurs. diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index db37000d87..242afc0f91 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -183,10 +183,14 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() std::map specific_group_params; for (const std::string& k : KNOWN_GROUP_PARAMS) { - if (nh_.hasParam(group_name + "/" + k)) + std::string param_name{ group_name }; + param_name += "/"; + param_name += k; + + if (nh_.hasParam(param_name)) { std::string value; - if (nh_.getParam(group_name + "/" + k, value)) + if (nh_.getParam(param_name, value)) { if (!value.empty()) specific_group_params[k] = value; @@ -194,7 +198,7 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() } double value_d; - if (nh_.getParam(group_name + "/" + k, value_d)) + if (nh_.getParam(param_name, value_d)) { // convert to string using no locale specific_group_params[k] = moveit::core::toString(value_d); @@ -202,14 +206,14 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() } int value_i; - if (nh_.getParam(group_name + "/" + k, value_i)) + if (nh_.getParam(param_name, value_i)) { specific_group_params[k] = std::to_string(value_i); continue; } bool value_b; - if (nh_.getParam(group_name + "/" + k, value_b)) + if (nh_.getParam(param_name, value_b)) { specific_group_params[k] = std::to_string(value_b); continue; From ce67918e868b114b70a20e65b3a14cef9c91f26f Mon Sep 17 00:00:00 2001 From: Markus Vieth <39675748+mvieth@users.noreply.github.com> Date: Fri, 31 Jul 2020 23:52:41 +0200 Subject: [PATCH 403/612] Optimize includes (#2229) Remove most obvious unused includes, as recommended by include-what-you-use (see https://github.com/ros-planning/moveit/issues/2170) --- .../include/moveit/collision_detection/world.h | 1 - .../include/moveit/collision_detection/world_diff.h | 2 -- .../collision_distance_field_types.h | 1 - .../src/collision_env_hybrid.cpp | 1 - .../src/default_constraint_samplers.cpp | 1 - .../test/pr2_arm_kinematics_plugin.cpp | 1 - .../include/moveit/distance_field/distance_field.h | 2 -- .../distance_field/propagation_distance_field.h | 1 - moveit_core/dynamics_solver/src/dynamics_solver.cpp | 4 ---- .../include/moveit/kinematic_constraints/utils.h | 2 +- .../moveit/kinematics_base/kinematics_base.h | 2 +- .../moveit/kinematics_metrics/kinematics_metrics.h | 1 - moveit_core/profiler/src/profiler.cpp | 1 - .../robot_model/src/revolute_joint_model.cpp | 1 - moveit_core/robot_model/src/robot_model.cpp | 1 - .../include/moveit/robot_state/conversions.h | 1 - .../iterative_spline_parameterization.h | 3 --- .../iterative_time_parameterization.h | 3 --- .../src/iterative_spline_parameterization.cpp | 1 - .../src/time_optimal_trajectory_generation.cpp | 2 -- .../include/moveit/transforms/transforms.h | 1 - .../cached_ik_kinematics_plugin/src/ik_cache.cpp | 2 -- .../src/chainiksolver_vel_mimic_svd.cpp | 1 - .../lma_kinematics_plugin/lma_kinematics_plugin.h | 2 +- .../srv_kinematics_plugin/srv_kinematics_plugin.h | 4 +--- .../src/srv_kinematics_plugin.cpp | 1 + moveit_kinematics/test/benchmark_ik.cpp | 1 - .../chomp/chomp_interface/src/chomp_plugin.cpp | 1 - .../chomp_motion_planner/src/chomp_trajectory.cpp | 1 - .../ompl_interface/planning_context_manager.h | 1 - .../ompl_interface/src/planning_context_manager.cpp | 2 -- .../src/moveit_fake_controller_manager.cpp | 1 - .../include/moveit/benchmarks/BenchmarkExecutor.h | 1 - .../manipulation/pick_place/src/pick_place.cpp | 1 - .../include/moveit/mesh_filter/gl_mesh.h | 3 +-- .../include/moveit/mesh_filter/mesh_filter.h | 1 - .../include/moveit/mesh_filter/mesh_filter_base.h | 2 +- .../include/moveit/mesh_filter/sensor_model.h | 2 +- .../mesh_filter/src/depth_self_filter_nodelet.cpp | 1 - .../perception/mesh_filter/src/gl_renderer.cpp | 1 - .../perception/mesh_filter/src/mesh_filter_base.cpp | 1 - .../perception/mesh_filter/src/sensor_model.cpp | 1 - .../moveit/point_containment_filter/shape_mask.h | 1 - .../src/visualize_robot_collision_volume.cpp | 1 - .../moveit_cpp/src/moveit_cpp.cpp | 1 - .../moveit_cpp/src/planning_component.cpp | 13 +------------ .../moveit/robot_interaction/interaction_handler.h | 1 + .../moveit/robot_interaction/robot_interaction.h | 1 - .../robot_interaction/src/interaction_handler.cpp | 1 - .../robot_interaction/src/kinematic_options_map.cpp | 2 -- .../robot_state_rviz_plugin/robot_state_display.h | 5 ----- .../moveit/rviz_plugin_render_tools/render_shapes.h | 1 - 52 files changed, 10 insertions(+), 81 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index 2aae8c9505..e27502427d 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -41,7 +41,6 @@ #include #include #include -#include #include #include #include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h index 4838758b24..77f851e330 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h @@ -39,8 +39,6 @@ #include #include -#include - namespace collision_detection { MOVEIT_CLASS_FORWARD(WorldDiff); diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index f38c57aa24..e66b9b0215 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -39,7 +39,6 @@ #include #include #include -#include #include #include diff --git a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp index 256ad6a9a7..f1abf3fb4c 100644 --- a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp @@ -36,7 +36,6 @@ #include #include -#include namespace collision_detection { diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 4cdf1e1855..abc70021ad 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -35,7 +35,6 @@ /* Author: Ioan Sucan */ #include -#include #include #include diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index 097e8b5951..a8292a4a0a 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -38,7 +38,6 @@ #include #include #include -#include #include #include "pr2_arm_kinematics_plugin.h" diff --git a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h index 56f0bc082e..dd36cc111b 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h @@ -38,8 +38,6 @@ #include #include -#include -#include #include #include #include diff --git a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h index 4b3ad13cd9..58f8de8e2d 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h @@ -39,7 +39,6 @@ #include #include #include -#include #include #include #include diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index 1126d29cdf..9ada138e8d 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -39,10 +39,6 @@ // KDL #include #include -#include -#include -#include -#include #include namespace dynamics_solver diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h index 1828077063..f11a2eb20a 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h @@ -36,7 +36,7 @@ #pragma once -#include +#include #include #include #include diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index c3ad491745..41267eca70 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -36,7 +36,7 @@ #pragma once -#include +#include #include #include #include diff --git a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h index 5a8f61c32a..3d2b1e4381 100644 --- a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h +++ b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h @@ -37,7 +37,6 @@ #pragma once #include -#include /** @brief Namespace for kinematics metrics */ namespace kinematics_metrics diff --git a/moveit_core/profiler/src/profiler.cpp b/moveit_core/profiler/src/profiler.cpp index 04616951fb..50c190c53e 100644 --- a/moveit_core/profiler/src/profiler.cpp +++ b/moveit_core/profiler/src/profiler.cpp @@ -40,7 +40,6 @@ #include #include #include -#include namespace moveit { diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index 4a87291f38..eb59ff8506 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -39,7 +39,6 @@ #include #include #include -#include #include namespace moveit diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 5f06e8d626..68492fb917 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -41,7 +41,6 @@ #include #include #include -#include #include #include #include "order_robot_model_items.inc" diff --git a/moveit_core/robot_state/include/moveit/robot_state/conversions.h b/moveit_core/robot_state/include/moveit/robot_state/conversions.h index 43968a2e83..61c45d1bce 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/conversions.h +++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.h @@ -39,7 +39,6 @@ #include #include #include -#include namespace moveit { diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h index 9ecf66e823..bee235d209 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h @@ -37,9 +37,6 @@ #pragma once -#include -#include -#include #include namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h index a336c83b5b..7a298642fb 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h @@ -36,9 +36,6 @@ #pragma once -#include -#include -#include #include namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp index 38938a01d2..5d5adf073e 100644 --- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp @@ -35,7 +35,6 @@ /* Author: Ken Anderson */ #include -#include #include #include diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 42a9108ea2..e0c0d3be41 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -44,8 +44,6 @@ #include #include -#include - namespace trajectory_processing { const std::string LOGNAME = "trajectory_processing.time_optimal_trajectory_generation"; diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 4880e29c19..799dd8e9a5 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -37,7 +37,6 @@ #pragma once #include -#include #include #include #include diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp b/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp index ff9c922934..28240a060f 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp +++ b/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp @@ -35,8 +35,6 @@ /* Author: Mark Moll */ #include -#include -#include #include #include diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp index 9162b505a6..3c3a1d234a 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp @@ -24,7 +24,6 @@ // Copyright (C) 2013 Sachin Chitta, Willow Garage #include -#include namespace { diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h index a25d154d57..85bf238327 100644 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h +++ b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h @@ -41,7 +41,7 @@ #include // ROS msgs -#include +#include #include #include #include diff --git a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h index 4ae5dfb01e..8d49435417 100644 --- a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h +++ b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h @@ -48,9 +48,7 @@ #include // ROS msgs -#include -#include -#include +#include #include #include diff --git a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp index 4c7695a471..f93ed4c202 100644 --- a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp +++ b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp @@ -34,6 +34,7 @@ /* Author: Dave Coleman, Masaki Murooka */ +#include #include #include #include diff --git a/moveit_kinematics/test/benchmark_ik.cpp b/moveit_kinematics/test/benchmark_ik.cpp index 72d7b8c78d..537df9e59d 100644 --- a/moveit_kinematics/test/benchmark_ik.cpp +++ b/moveit_kinematics/test/benchmark_ik.cpp @@ -34,7 +34,6 @@ /* Author: Mark Moll */ -#include #include #include #include diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp index e73789f733..0a0a96bc9b 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp @@ -36,7 +36,6 @@ #include #include #include -#include #include #include diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp index bb2e0f012a..4467dbfced 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp @@ -36,7 +36,6 @@ #include #include -#include namespace chomp { diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index 4b213c3409..594d3ba9dc 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -43,7 +43,6 @@ #include -#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index bb8c835233..44b15cee8b 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -37,8 +37,6 @@ #include #include #include -#include -#include #include #include diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp index d63a47d38f..826337d386 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp @@ -42,7 +42,6 @@ #include #include #include -#include namespace moveit_fake_controller_manager { diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index 6aac9db366..90d3a29f8e 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -57,7 +57,6 @@ DIAGNOSTIC_PUSH #include #include #include -#include namespace moveit_ros_benchmarks { diff --git a/moveit_ros/manipulation/pick_place/src/pick_place.cpp b/moveit_ros/manipulation/pick_place/src/pick_place.cpp index 8571f24d47..61fbcf68db 100644 --- a/moveit_ros/manipulation/pick_place/src/pick_place.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick_place.cpp @@ -38,7 +38,6 @@ #include #include #include -#include namespace pick_place { diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h index a605425870..969f4e268d 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h @@ -36,14 +36,13 @@ #pragma once -#include +#include // for Isometry3d #include #ifdef __APPLE__ #include #else #include #endif -#include namespace shapes { diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h index ed27f7b136..3872aac0f5 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h @@ -41,7 +41,6 @@ #include #include #include -#include // forward declarations namespace shapes diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h index 6fe3fb0745..df6e35e7da 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h @@ -42,7 +42,7 @@ #include #include #include -#include +#include // for Isometry3d #include // forward declarations diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h index 8c76cdccef..40b831dc8e 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h @@ -37,7 +37,7 @@ #pragma once #include -#include +#include // for Vector3f namespace mesh_filter { diff --git a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp index c195c6ea03..ecd1830dff 100644 --- a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp +++ b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp @@ -42,7 +42,6 @@ #include #include #include -#include #include namespace enc = sensor_msgs::image_encodings; diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index 7e4092cff3..4f351b53e6 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -47,7 +47,6 @@ #include #include #include -#include #include #include diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp index 331dfe3780..58162afa22 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp @@ -43,7 +43,6 @@ #include #include #include -#include #include diff --git a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp index 96ed1ae78c..df29ec2b21 100644 --- a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp +++ b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp @@ -35,7 +35,6 @@ /* Author: Suat Gedikli */ #include -#include #include mesh_filter::SensorModel::~SensorModel() = default; diff --git a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h index fdc238e891..f504acb9f5 100644 --- a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h +++ b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h @@ -39,7 +39,6 @@ #include #include #include -#include #include #include #include diff --git a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp index 50d2941091..e162df1bbd 100644 --- a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp +++ b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp @@ -35,7 +35,6 @@ /* Author: Ioan Sucan */ #include -#include #include #include diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index 8b5b494e7e..1db8d8f33c 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -39,7 +39,6 @@ #include #include -#include #include #include #include diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp index 102489b269..8a910e7f63 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -46,18 +46,7 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include + #include #include #include diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h index 02b63a6e6c..b9914d437a 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h @@ -36,6 +36,7 @@ #pragma once +#include #include #include //#include diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h index e770293d55..ca0698af29 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h @@ -38,7 +38,6 @@ #include #include -#include #include #include #include diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index 1b76f7a196..e26ccdc182 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -48,7 +48,6 @@ #include #include #include -#include #include #include diff --git a/moveit_ros/robot_interaction/src/kinematic_options_map.cpp b/moveit_ros/robot_interaction/src/kinematic_options_map.cpp index c3460c0d8a..013a9c7a61 100644 --- a/moveit_ros/robot_interaction/src/kinematic_options_map.cpp +++ b/moveit_ros/robot_interaction/src/kinematic_options_map.cpp @@ -35,8 +35,6 @@ /* Author: Acorn Pooley */ #include -#include -#include // These strings have no content. They are compared by address. const std::string robot_interaction::KinematicOptionsMap::DEFAULT = ""; diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h index 632692dde0..f8f308190f 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h +++ b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h @@ -45,11 +45,6 @@ #include #endif -namespace Ogre -{ -class SceneNode; -} - namespace rviz { class Robot; diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h index 9e3e9222fe..87161aa639 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h @@ -41,7 +41,6 @@ #include #include #include -#include #include namespace Ogre From b0391a73b3ae84dc5cc9c7035df46bf4c1f979fc Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 3 Aug 2020 04:50:24 -0500 Subject: [PATCH 404/612] Remove unused yaml param (#2232) --- moveit_ros/moveit_servo/config/ur_simulated_config.yaml | 1 - moveit_ros/moveit_servo/test/config/servo_settings.yaml | 1 - 2 files changed, 2 deletions(-) diff --git a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml index ed3917f236..5c477ffb70 100644 --- a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml @@ -16,7 +16,6 @@ low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the ## Properties of outgoing commands publish_period: 0.008 # 1/Nominal publish rate [seconds] -publish_delay: 0.005 # Delay between calculation and execution start of command # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml index e98719f5f6..9a674425b6 100644 --- a/moveit_ros/moveit_servo/test/config/servo_settings.yaml +++ b/moveit_ros/moveit_servo/test/config/servo_settings.yaml @@ -16,7 +16,6 @@ low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the ## Properties of outgoing commands publish_period: 0.01 # 1/Nominal publish rate [seconds] -publish_delay: 0.005 # Delay between calculation and execution start of command # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) From a07cb8aff3d73cc15667b54a0254edf399798da7 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 3 Aug 2020 16:41:14 -0500 Subject: [PATCH 405/612] Check collisions during joint motions, too (#2204) * Check collisions during joint motions, too. Print colliding objects, for debugging. * Small code simplification * Use the argument to printCollisionPairs * Clang format * Fix misspelling * Log all colliding pairs * Adjustments to print formatting and log levels --- .../include/moveit_servo/collision_check.h | 9 ++++++++ .../moveit_servo/src/collision_check.cpp | 22 +++++++++++++++++++ moveit_ros/moveit_servo/src/servo_calcs.cpp | 14 +++++++----- 3 files changed, 40 insertions(+), 5 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index 6b9610d6ce..484c2d5782 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -38,6 +38,7 @@ #pragma once +#include #include #include #include @@ -75,8 +76,16 @@ class CollisionCheck void setPaused(bool paused); private: + /** \brief Run one iteration of collision checking */ void run(const ros::TimerEvent& timer_event); + + /** \brief Print objects in collision. Useful for debugging. */ + void printCollisionPairs(collision_detection::CollisionResult::ContactMap& contact_map); + + /** \brief Get a read-only copy of the planning scene */ planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; + + /** \brief Callback for stopping time, from the thread that is aware of velocity and acceleration */ void worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg); ros::NodeHandle nh_; diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp index 91adabf530..4c7535e620 100644 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ b/moveit_ros/moveit_servo/src/collision_check.cpp @@ -64,6 +64,7 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoPar // Init collision request collision_request_.group_name = parameters_.move_group_name; collision_request_.distance = true; // enable distance-based collision checking + collision_request_.contacts = true; // Record the names of collision pairs if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE) ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, @@ -127,12 +128,15 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event) *current_state_); scene_collision_distance_ = collision_result_.distance; collision_detected_ |= collision_result_.collision; + printCollisionPairs(collision_result_.contacts); collision_result_.clear(); + // Self-collisions and scene collisions are checked separately so different thresholds can be used getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision(collision_request_, collision_result_, *current_state_, acm_); self_collision_distance_ = collision_result_.distance; collision_detected_ |= collision_result_.collision; + printCollisionPairs(collision_result_.contacts); velocity_scale_ = 1; // If we're definitely in collision, stop immediately @@ -203,6 +207,24 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event) } } +void CollisionCheck::printCollisionPairs(collision_detection::CollisionResult::ContactMap& contact_map) +{ + if (!contact_map.empty()) + { + // Throttled error message about the first contact in the list + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Objects in collision (among others, possibly): " + << contact_map.begin()->first.first << ", " + << contact_map.begin()->first.second); + // Log all other contacts if in debug mode + ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Objects in collision:"); + for (auto contact : contact_map) + { + ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "\t" << contact.first.first << ", " + << contact.first.second); + } + } +} + void CollisionCheck::worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg) { worst_case_stop_time_ = msg->data; diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 2753d22ae0..13684e42ca 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -452,11 +452,6 @@ bool ServoCalcs::cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, // If close to a collision or a singularity, decelerate applyVelocityScaling(delta_theta_, velocityScalingFactorForSingularity(delta_x, svd, pseudo_inverse)); - if (status_ == StatusCode::HALT_FOR_COLLISION) - { - ROS_ERROR_STREAM_THROTTLE_NAMED(5, LOGNAME, "Halting for collision!"); - delta_theta_.setZero(); - } prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; @@ -481,6 +476,9 @@ bool ServoCalcs::jointServoCalcs(const control_msgs::JointJog& cmd, trajectory_m enforceSRDFAccelVelLimits(delta_theta_); + // If close to a collision, decelerate + applyVelocityScaling(delta_theta_, 1.0 /* scaling for singularities -- ignore for joint motions */); + prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; return convertDeltasToOutgoingCmd(joint_trajectory); @@ -598,6 +596,12 @@ void ServoCalcs::applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singul } delta_theta = collision_scale * singularity_scale * delta_theta; + + if (status_ == StatusCode::HALT_FOR_COLLISION) + { + ROS_WARN_STREAM_THROTTLE_NAMED(3, LOGNAME, "Halting for collision!"); + delta_theta_.setZero(); + } } // Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion From 7d40d82e8f6e1b2eba3253760c2699e9a49c2841 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Wed, 5 Aug 2020 05:03:29 +0900 Subject: [PATCH 406/612] Fix subframes disappearing when object is detached/scaled/renamed (#1866) * Treat subframes when detaching objects * reduce code complexity - remove code duplication, when fetching attached bodies - simplify function calls when detaching (attached) bodies * Maintain subframes when scaling/renaming objects in Rviz * clang-format * Add review comments * More review comments * minor nits * fixup: use STREAM logging * Add TODO Co-authored-by: Robert Haschke Co-authored-by: v4hn --- .../planning_scene/src/planning_scene.cpp | 47 +++++++++---------- .../moveit/robot_state/attached_body.h | 6 +++ .../src/motion_planning_frame_objects.cpp | 6 +++ 3 files changed, 33 insertions(+), 26 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 7d5dec02e9..0adb7ad31d 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1593,43 +1593,36 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache { // STEP 1: Get info about the object from the RobotState std::vector attached_bodies; - if (object.link_name.empty()) + if (object.object.id.empty()) { - if (object.object.id.empty()) - robot_state_->getAttachedBodies(attached_bodies); + const moveit::core::LinkModel* link_model = + object.link_name.empty() ? nullptr : getRobotModel()->getLinkModel(object.link_name); + if (link_model) // if we have a link model specified, only fetch bodies attached to this link + robot_state_->getAttachedBodies(attached_bodies, link_model); else - { - const moveit::core::AttachedBody* body = robot_state_->getAttachedBody(object.object.id); - if (body) - attached_bodies.push_back(body); - } + robot_state_->getAttachedBodies(attached_bodies); } - else + else // A specific object id will be removed. { - const moveit::core::LinkModel* link_model = getRobotModel()->getLinkModel(object.link_name); - if (link_model) + const moveit::core::AttachedBody* body = robot_state_->getAttachedBody(object.object.id); + if (body) { - // If no specific object id is given, then we remove all objects attached to the link_name. - if (object.object.id.empty()) + if (!object.link_name.empty() && (body->getAttachedLinkName() != object.link_name)) { - robot_state_->getAttachedBodies(attached_bodies, link_model); - } - else // A specific object id will be removed. - { - const moveit::core::AttachedBody* body = robot_state_->getAttachedBody(object.object.id); - if (body) - attached_bodies.push_back(body); + ROS_ERROR_STREAM_NAMED(LOGNAME, "The AttachedCollisionObject message states the object is attached to " + << object.link_name << ", but it is actually attached to " + << body->getAttachedLinkName() + << ". Leave the link_name empty or specify the correct link."); + return false; } + attached_bodies.push_back(body); } } // STEP 2+3: Remove the attached object(s) from the RobotState and put them in the world for (const moveit::core::AttachedBody* attached_body : attached_bodies) { - const std::vector& shapes = attached_body->getShapes(); - const EigenSTL::vector_Isometry3d& poses = attached_body->getGlobalCollisionBodyTransforms(); const std::string& name = attached_body->getName(); - if (world_->hasObject(name)) ROS_WARN_NAMED(LOGNAME, "The collision world already has an object with the same name as the body about to be detached. " @@ -1637,7 +1630,8 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache object.object.id.c_str()); else { - world_->addToObject(name, shapes, poses); + world_->addToObject(name, attached_body->getShapes(), attached_body->getGlobalCollisionBodyTransforms()); + world_->setSubframesOfObject(name, attached_body->getSubframeTransforms()); ROS_DEBUG_NAMED(LOGNAME, "Detached object '%s' from link '%s' and added it back in the collision world", name.c_str(), object.link_name.c_str()); } @@ -1772,8 +1766,8 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject if (!object.type.key.empty() || !object.type.db.empty()) setObjectType(object.id, object.type); - // Add subframes - moveit::core::FixedTransformsMap subframes; + // Add subframes to the newly created (or possibly modified) object + moveit::core::FixedTransformsMap subframes = world_->getObject(object.id)->subframe_poses_; Eigen::Isometry3d frame_pose; for (std::size_t i = 0; i < object.subframe_poses.size(); ++i) { @@ -1830,6 +1824,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::CollisionObjec } collision_detection::World::ObjectConstPtr obj = world_->getObject(object.id); + if (obj->shapes_.size() == new_poses.size()) { std::vector shapes = obj->shapes_; diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index 39cb58afa9..e9ed735251 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -120,6 +120,12 @@ class AttachedBody return subframe_poses_; } + /** \brief Get subframes of this object (in the world frame) */ + const moveit::core::FixedTransformsMap& getGlobalSubframeTransforms() const + { + return global_subframe_poses_; + } + /** \brief Set all subframes of this object. * * Use these to define points of interest on the object to plan with diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index e1515e4d7e..a8968549d7 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -159,6 +159,7 @@ void MotionPlanningFrame::sceneScaleChanged(int value) { if (ps->getWorld()->hasObject(scaled_object_->id_)) { + const moveit::core::FixedTransformsMap subframes = scaled_object_->subframe_poses_; // Keep subframes ps->getWorldNonConst()->removeObject(scaled_object_->id_); for (std::size_t i = 0; i < scaled_object_->shapes_.size(); ++i) { @@ -167,6 +168,8 @@ void MotionPlanningFrame::sceneScaleChanged(int value) ps->getWorldNonConst()->addToObject(scaled_object_->id_, shapes::ShapeConstPtr(s), scaled_object_->shape_poses_[i]); } + // TODO(felixvd): Scale subframes too + ps->getWorldNonConst()->setSubframesOfObject(scaled_object_->id_, subframes); setLocalSceneEdited(); scene_marker_->processMessage(createObjectMarkerMsg(ps->getWorld()->getObject(scaled_object_->id_))); planning_display_->queueRenderSceneGeometry(); @@ -823,9 +826,12 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item) if (obj) { known_collision_objects_[item->type()].first = item_text; + const moveit::core::FixedTransformsMap subframes = obj->subframe_poses_; // Keep subframes + // TODO(felixvd): Scale the subframes with the object ps->getWorldNonConst()->removeObject(obj->id_); ps->getWorldNonConst()->addToObject(known_collision_objects_[item->type()].first, obj->shapes_, obj->shape_poses_); + ps->getWorldNonConst()->setSubframesOfObject(obj->id_, subframes); if (scene_marker_) { scene_marker_.reset(); From 3f2924ce07afbb0ebe51b31628ec49240b722fa5 Mon Sep 17 00:00:00 2001 From: Jeroen Date: Fri, 7 Aug 2020 09:27:03 +0200 Subject: [PATCH 407/612] Fix memcpy bug in copyJointToOMPLState in ompl interface (#2239) --- .../model_based_state_space.cpp | 2 +- .../ompl_interface/test/test_state_space.cpp | 62 ++++++++++++++----- 2 files changed, 49 insertions(+), 15 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp index 201fe2e14a..3500d4e18b 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp @@ -340,7 +340,7 @@ void ompl_interface::ModelBasedStateSpace::copyJointToOMPLState(ompl::base::Stat { // Copy one joint (multiple variables possibly) memcpy(getValueAddressAtIndex(state, ompl_state_joint_index), - robot_state.getVariablePositions() + joint_model->getFirstVariableIndex() * sizeof(double), + robot_state.getVariablePositions() + joint_model->getFirstVariableIndex(), joint_model->getVariableCount() * sizeof(double)); // clear any cached info (such as validity known or not) diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp index 90bbcc1d38..d1db71f973 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp @@ -34,6 +34,8 @@ /* Author: Ioan Sucan */ +#include + #include #include @@ -46,6 +48,8 @@ #include #include +constexpr double EPSILON = std::numeric_limits::epsilon(); + class LoadPlanningModelsPr2 : public testing::Test { protected: @@ -129,15 +133,15 @@ TEST_F(LoadPlanningModelsPr2, StateSpaces) TEST_F(LoadPlanningModelsPr2, StateSpaceCopy) { ompl_interface::ModelBasedStateSpaceSpecification spec(robot_model_, "right_arm"); - ompl_interface::JointModelStateSpace ss(spec); - ss.setPlanningVolume(-1, 1, -1, 1, -1, 1); - ss.setup(); + ompl_interface::JointModelStateSpace joint_model_state_space(spec); + joint_model_state_space.setPlanningVolume(-1, 1, -1, 1, -1, 1); + joint_model_state_space.setup(); std::ofstream fout("ompl_interface_test_state_space_diagram1.dot"); - ss.diagram(fout); + joint_model_state_space.diagram(fout); bool passed = false; try { - ss.sanityChecks(); + joint_model_state_space.sanityChecks(); passed = true; } catch (ompl::Exception& ex) @@ -148,26 +152,56 @@ TEST_F(LoadPlanningModelsPr2, StateSpaceCopy) moveit::core::RobotState robot_state(robot_model_); robot_state.setToRandomPositions(); - EXPECT_TRUE(robot_state.distance(robot_state) < 1e-12); - ompl::base::State* state = ss.allocState(); + EXPECT_TRUE(robot_state.distance(robot_state) < EPSILON); + ompl::base::State* state = joint_model_state_space.allocState(); for (int i = 0; i < 10; ++i) { moveit::core::RobotState robot_state2(robot_state); - EXPECT_TRUE(robot_state.distance(robot_state2) < 1e-12); - ss.copyToOMPLState(state, robot_state); - robot_state.setToRandomPositions(robot_state.getRobotModel()->getJointModelGroup(ss.getJointModelGroupName())); + EXPECT_TRUE(robot_state.distance(robot_state2) < EPSILON); + joint_model_state_space.copyToOMPLState(state, robot_state); + robot_state.setToRandomPositions( + robot_state.getRobotModel()->getJointModelGroup(joint_model_state_space.getJointModelGroupName())); std::cout << (robot_state.getGlobalLinkTransform("r_wrist_roll_link").translation() - robot_state2.getGlobalLinkTransform("r_wrist_roll_link").translation()) << std::endl; - EXPECT_TRUE(robot_state.distance(robot_state2) > 1e-12); - ss.copyToRobotState(robot_state, state); + EXPECT_TRUE(robot_state.distance(robot_state2) > EPSILON); + joint_model_state_space.copyToRobotState(robot_state, state); std::cout << (robot_state.getGlobalLinkTransform("r_wrist_roll_link").translation() - robot_state2.getGlobalLinkTransform("r_wrist_roll_link").translation()) << std::endl; - EXPECT_TRUE(robot_state.distance(robot_state2) < 1e-12); + EXPECT_TRUE(robot_state.distance(robot_state2) < EPSILON); + } + + // repeat the above test with a different method to copy the state to OMPL + for (int i = 0; i < 10; ++i) + { + // create two equal states + moveit::core::RobotState robot_state2(robot_state); + EXPECT_LT(robot_state.distance(robot_state2), EPSILON); + + // copy the first state to OMPL as backup (this is where the 'different' method comes into play) + const moveit::core::JointModelGroup* joint_model_group = + robot_state.getRobotModel()->getJointModelGroup(joint_model_state_space.getJointModelGroupName()); + std::vector joint_model_names = joint_model_group->getActiveJointModelNames(); + for (std::size_t joint_index = 0; joint_index < joint_model_group->getVariableCount(); ++joint_index) + { + const moveit::core::JointModel* joint_model = joint_model_group->getJointModel(joint_model_names[joint_index]); + EXPECT_NE(joint_model, nullptr); + joint_model_state_space.copyJointToOMPLState(state, robot_state, joint_model, joint_index); + } + + // and change the joint values of the moveit state, so it is different that robot_state2 + robot_state.setToRandomPositions( + robot_state.getRobotModel()->getJointModelGroup(joint_model_state_space.getJointModelGroupName())); + EXPECT_GT(robot_state.distance(robot_state2), EPSILON); + + // copy the backup values in the OMPL state back to the first state, + // and check if it is still equal to the second + joint_model_state_space.copyToRobotState(robot_state, state); + EXPECT_LT(robot_state.distance(robot_state2), EPSILON); } - ss.freeState(state); + joint_model_state_space.freeState(state); } int main(int argc, char** argv) From 576c39c3d82744917123cab39dda99ba6e272a6d Mon Sep 17 00:00:00 2001 From: tnaka Date: Mon, 10 Aug 2020 17:18:18 +0900 Subject: [PATCH 408/612] Add planning_adapters for chomp in setup assistant (#2242) CHOMPPlanner doesn't fill velocity, acc or time. We need AddTimeParameterization at least to use this planner with ros_control. --- .../launch/chomp_planning_pipeline.launch.xml | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml index 1f83008b8d..c9eb4e24c8 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml @@ -2,8 +2,19 @@ + + + From c8b9510420dda68c6b093b36e83f94af25942e73 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 10 Aug 2020 17:41:58 +0200 Subject: [PATCH 409/612] Revert "delay loading of robot model to avoid spurious warning" This reverts commit 4e72ac5f96863e7c37a47706645c6634d7672f7b as the delayed loading results in link properties NOT being loaded. --- .../robot_state_display.h | 1 - .../src/robot_state_display.cpp | 20 +++++-------------- .../trajectory_display.h | 1 - .../src/trajectory_display.cpp | 11 +++------- 4 files changed, 8 insertions(+), 25 deletions(-) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h index f8f308190f..f2116748e7 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h +++ b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h @@ -129,7 +129,6 @@ private Q_SLOTS: moveit::core::RobotStatePtr robot_state_; std::map highlights_; bool update_state_; - bool load_robot_model_; // for delayed robot initialization rviz::StringProperty* robot_description_property_; rviz::StringProperty* root_link_name_property_; diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 20dd6f1f93..65acc44d52 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -63,7 +63,7 @@ namespace moveit_rviz_plugin // ****************************************************************************************** // Base class contructor // ****************************************************************************************** -RobotStateDisplay::RobotStateDisplay() : Display(), update_state_(false), load_robot_model_(false) +RobotStateDisplay::RobotStateDisplay() : Display(), update_state_(false) { robot_description_property_ = new rviz::StringProperty( "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", @@ -122,8 +122,8 @@ void RobotStateDisplay::reset() robot_->clear(); rdf_loader_.reset(); Display::reset(); - - loadRobotModel(); + if (isEnabled()) + onEnable(); } void RobotStateDisplay::changedAllLinks() @@ -373,7 +373,6 @@ void RobotStateDisplay::unsetLinkColor(rviz::Robot* robot, const std::string& li // ****************************************************************************************** void RobotStateDisplay::loadRobotModel() { - load_robot_model_ = false; if (!rdf_loader_) rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); @@ -403,7 +402,8 @@ void RobotStateDisplay::loadRobotModel() void RobotStateDisplay::onEnable() { Display::onEnable(); - load_robot_model_ = true; // allow loading of robot model in update() + loadRobotModel(); + changedRobotStateTopic(); calculateOffsetPosition(); } @@ -421,16 +421,6 @@ void RobotStateDisplay::onDisable() void RobotStateDisplay::update(float wall_dt, float ros_dt) { Display::update(wall_dt, ros_dt); - - if (load_robot_model_) - { - loadRobotModel(); - // The following call to changedRobotStateTopic() should not change visibility - bool visible = robot_->isVisible(); - changedRobotStateTopic(); - robot_->setVisible(visible); - } - calculateOffsetPosition(); if (robot_ && update_state_ && robot_state_) { diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h index e0cbf22da6..e0338c3c49 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h +++ b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h @@ -88,7 +88,6 @@ private Q_SLOTS: rdf_loader::RDFLoaderPtr rdf_loader_; moveit::core::RobotModelConstPtr robot_model_; moveit::core::RobotStatePtr robot_state_; - bool load_robot_model_; // for delayed robot initialization // Properties rviz::StringProperty* robot_description_property_; diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp index 2128e624e6..c1a6a26062 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -41,7 +41,7 @@ namespace moveit_rviz_plugin { -TrajectoryDisplay::TrajectoryDisplay() : Display(), load_robot_model_(false) +TrajectoryDisplay::TrajectoryDisplay() : Display() { // The robot description property is only needed when using the trajectory playback standalone (not within motion // planning plugin) @@ -63,7 +63,6 @@ void TrajectoryDisplay::onInitialize() void TrajectoryDisplay::loadRobotModel() { - load_robot_model_ = false; rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); if (!rdf_loader_->getURDF()) @@ -80,7 +79,6 @@ void TrajectoryDisplay::loadRobotModel() // Send to child class trajectory_visual_->onRobotModelLoaded(robot_model_); - trajectory_visual_->onEnable(); } void TrajectoryDisplay::reset() @@ -93,7 +91,8 @@ void TrajectoryDisplay::reset() void TrajectoryDisplay::onEnable() { Display::onEnable(); - load_robot_model_ = true; // allow loading of robot model in update() + loadRobotModel(); + trajectory_visual_->onEnable(); } void TrajectoryDisplay::onDisable() @@ -105,10 +104,6 @@ void TrajectoryDisplay::onDisable() void TrajectoryDisplay::update(float wall_dt, float ros_dt) { Display::update(wall_dt, ros_dt); - - if (load_robot_model_) - loadRobotModel(); - trajectory_visual_->update(wall_dt, ros_dt); } From bf5dfea2cec60bbbd15101df18bc3235f3adb6f3 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 5 Aug 2020 21:29:04 +0200 Subject: [PATCH 410/612] RViz RS & Traj display: ensure robot_description is available onEnable This is a (in my opinion) rather dirty workaround because RViz enables Displays systematically *before* loading their properties. So the patch loads the required property a second time *before* everything else. Assuming this ever changes we can remove this code again. A Display's "enabled" state is encoded in a boolean property the Display inherits from. The way loading works (recursively running through the properties) this special property is loaded first and triggers onEnable because of the Property's changed() signal. A second "Enabled" property exists in RViz's Displays but does not change behavior at all. --- .../moveit/robot_state_rviz_plugin/robot_state_display.h | 1 + .../robot_state_rviz_plugin/src/robot_state_display.cpp | 8 ++++++++ .../moveit/trajectory_rviz_plugin/trajectory_display.h | 1 + .../trajectory_rviz_plugin/src/trajectory_display.cpp | 8 ++++++++ 4 files changed, 18 insertions(+) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h index f2116748e7..8fded5f6b5 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h +++ b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h @@ -67,6 +67,7 @@ class RobotStateDisplay : public rviz::Display RobotStateDisplay(); ~RobotStateDisplay() override; + void load(const rviz::Config& config) override; void update(float wall_dt, float ros_dt) override; void reset() override; diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 65acc44d52..08cdec38a7 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -399,6 +399,14 @@ void RobotStateDisplay::loadRobotModel() highlights_.clear(); } +void RobotStateDisplay::load(const rviz::Config& config) +{ + // This property needs to be loaded in onEnable() below, which is triggered + // in the beginning of Display::load() before the other property would be available + robot_description_property_->load(config.mapGetChild("Robot Description")); + Display::load(config); +} + void RobotStateDisplay::onEnable() { Display::onEnable(); diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h index e0338c3c49..a2de42443d 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h +++ b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h @@ -65,6 +65,7 @@ class TrajectoryDisplay : public rviz::Display void loadRobotModel(); + void load(const rviz::Config& config) override; void update(float wall_dt, float ros_dt) override; void reset() override; diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp index c1a6a26062..0de807b760 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -88,6 +88,14 @@ void TrajectoryDisplay::reset() trajectory_visual_->reset(); } +void TrajectoryDisplay::load(const rviz::Config& config) +{ + // This property needs to be loaded in onEnable() below, which is triggered + // in the beginning of Display::load() before the other property would be available + robot_description_property_->load(config.mapGetChild("Robot Description")); + Display::load(config); +} + void TrajectoryDisplay::onEnable() { Display::onEnable(); From 1a6c0b40ed9eeae5625234d0cfb4837ca07f645e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 10 Aug 2020 18:23:43 +0200 Subject: [PATCH 411/612] Only loadRobotModel() if not yet done --- .../robot_state_rviz_plugin/src/robot_state_display.cpp | 6 +++--- .../trajectory_rviz_plugin/src/trajectory_display.cpp | 3 ++- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 08cdec38a7..a32363ef19 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -373,8 +373,7 @@ void RobotStateDisplay::unsetLinkColor(rviz::Robot* robot, const std::string& li // ****************************************************************************************** void RobotStateDisplay::loadRobotModel() { - if (!rdf_loader_) - rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); + rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); if (rdf_loader_->getURDF()) { @@ -410,7 +409,8 @@ void RobotStateDisplay::load(const rviz::Config& config) void RobotStateDisplay::onEnable() { Display::onEnable(); - loadRobotModel(); + if (!rdf_loader_) + loadRobotModel(); changedRobotStateTopic(); calculateOffsetPosition(); } diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp index 0de807b760..74cb68ade4 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -99,7 +99,8 @@ void TrajectoryDisplay::load(const rviz::Config& config) void TrajectoryDisplay::onEnable() { Display::onEnable(); - loadRobotModel(); + if (!rdf_loader_) + loadRobotModel(); trajectory_visual_->onEnable(); } From 4d0b3ec985493118a0aae6338bfac052403f6821 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 30 Jun 2020 15:59:18 +0200 Subject: [PATCH 412/612] reorder and group clang-format options --- .clang-format | 66 ++++++++++++++++++++++++++++----------------------- 1 file changed, 36 insertions(+), 30 deletions(-) diff --git a/.clang-format b/.clang-format index bef3326796..81b58439e0 100644 --- a/.clang-format +++ b/.clang-format @@ -1,66 +1,72 @@ --- BasedOnStyle: Google +ColumnLimit: 120 +MaxEmptyLinesToKeep: 1 +SortIncludes: false + +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never AccessModifierOffset: -2 ConstructorInitializerIndentWidth: 2 +NamespaceIndentation: None +ContinuationIndentWidth: 4 +IndentCaseLabels: true +IndentFunctionDeclarationAfterType: false + AlignEscapedNewlinesLeft: false AlignTrailingComments: true + AllowAllParametersOfDeclarationOnNextLine: false +ExperimentalAutoDetectBinPacking: false +ObjCSpaceBeforeProtocolList: true +Cpp11BracedListStyle: false + AllowShortIfStatementsOnASingleLine: false AllowShortLoopsOnASingleLine: false AllowShortFunctionsOnASingleLine: None -AllowShortLoopsOnASingleLine: false + AlwaysBreakTemplateDeclarations: true AlwaysBreakBeforeMultilineStrings: false BreakBeforeBinaryOperators: false BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true + BinPackParameters: true -ColumnLimit: 120 ConstructorInitializerAllOnOneLineOrOnePerLine: true DerivePointerBinding: false PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true + +PenaltyExcessCharacter: 1000 PenaltyBreakBeforeFirstCallParameter: 19 PenaltyBreakComment: 60 -PenaltyBreakString: 100 PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 +PenaltyBreakString: 100 PenaltyReturnTypeOnItsOwnLine: 70 + SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false SpacesInParentheses: false SpacesInAngles: false SpaceInEmptyParentheses: false SpacesInCStyleCastParentheses: false +SpaceAfterCStyleCast: false SpaceAfterControlStatementKeyword: true SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false # Configure each individual brace in BraceWrapping BreakBeforeBraces: Custom # Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} +BraceWrapping: + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false ... From 45d0b7ef356c12abd75471c868cf1894c6b71a93 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 30 Jun 2020 16:04:33 +0200 Subject: [PATCH 413/612] add new config options --- .clang-format | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.clang-format b/.clang-format index 81b58439e0..afc14cd19b 100644 --- a/.clang-format +++ b/.clang-format @@ -23,9 +23,11 @@ ExperimentalAutoDetectBinPacking: false ObjCSpaceBeforeProtocolList: true Cpp11BracedListStyle: false +AllowShortBlocksOnASingleLine: true AllowShortIfStatementsOnASingleLine: false AllowShortLoopsOnASingleLine: false AllowShortFunctionsOnASingleLine: None +AllowShortCaseLabelsOnASingleLine: false AlwaysBreakTemplateDeclarations: true AlwaysBreakBeforeMultilineStrings: false @@ -59,6 +61,7 @@ BreakBeforeBraces: Custom # Control of individual brace wrapping cases BraceWrapping: + AfterCaseLabel: true AfterClass: true AfterControlStatement: true AfterEnum: true From 56c4fe10064cec269d91421bf1a12915ce61ab5b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 30 Jun 2020 16:27:51 +0200 Subject: [PATCH 414/612] Tweak penalties Allow some excess characters per line if that improves readability. --- .clang-format | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.clang-format b/.clang-format index afc14cd19b..9a6ec509a4 100644 --- a/.clang-format +++ b/.clang-format @@ -40,12 +40,12 @@ ConstructorInitializerAllOnOneLineOrOnePerLine: true DerivePointerBinding: false PointerBindsToType: true -PenaltyExcessCharacter: 1000 -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 50 +PenaltyBreakBeforeFirstCallParameter: 30 +PenaltyBreakComment: 1000 +PenaltyBreakFirstLessLess: 10 PenaltyBreakString: 100 -PenaltyReturnTypeOnItsOwnLine: 70 +PenaltyReturnTypeOnItsOwnLine: 50 SpacesBeforeTrailingComments: 2 SpacesInParentheses: false From 81ba724069d3d26b412d015832cbddf73b19413e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 11 Aug 2020 08:31:34 +0200 Subject: [PATCH 415/612] Switch docker image to clang-format-10 --- .docker/ci/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index 830c8abd09..01021fea58 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -30,7 +30,7 @@ RUN \ sudo \ # Preferred build tools python-catkin-tools \ - clang clang-format-3.9 clang-tidy clang-tools \ + clang clang-format-10 clang-tidy clang-tools \ ccache && \ # # Download all dependencies of MoveIt From 70e0f83ae3f6a45eac601872602661f5cbc6ff46 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 11 Aug 2020 10:00:37 +0200 Subject: [PATCH 416/612] Run clang-format on Noetic image (with clang-format-10 installed) --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index e8665fb8ff..2830818ac4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -22,7 +22,7 @@ env: jobs: fast_finish: true include: - - env: TEST="clang-format catkin_lint" + - env: TEST="clang-format catkin_lint" ROS_DISTRO=noetic - env: TEST=code-coverage - env: ROS_DISTRO=noetic - env: ROS_DISTRO=melodic From f69a324617a14dc5f90b897d8bf6ae9c995960fd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 11 Aug 2020 08:34:28 +0200 Subject: [PATCH 417/612] apply changes --- .../collision_detector_allocator_allvalid.h | 4 +- .../src/collision_matrix.cpp | 3 +- .../src/collision_tools.cpp | 3 +- .../bullet_discrete_bvh_manager.h | 2 +- .../bullet_integration/bullet_utils.h | 20 +-- .../contact_checker_common.h | 8 +- .../bullet_integration/ros_bullet_utils.h | 8 +- .../collision_detector_allocator_bullet.h | 2 +- .../collision_detector_bullet_plugin_loader.h | 2 +- .../bullet_integration/bullet_bvh_manager.cpp | 6 +- .../bullet_cast_bvh_manager.cpp | 9 +- .../bullet_discrete_bvh_manager.cpp | 10 +- .../src/bullet_integration/bullet_utils.cpp | 5 +- ...ollision_detector_bullet_plugin_loader.cpp | 2 +- .../src/collision_env_bullet.cpp | 9 +- .../collision_common.h | 26 +-- .../collision_detector_allocator_fcl.h | 4 +- .../collision_detector_fcl_plugin_loader.h | 2 +- .../collision_env_fcl.h | 40 ++--- .../collision_detection_fcl/fcl_compat.h | 2 +- .../src/collision_common.cpp | 43 +++-- .../collision_detector_fcl_plugin_loader.cpp | 2 +- ...lision_detector_allocator_distance_field.h | 4 +- .../collision_detector_allocator_hybrid.h | 4 +- .../src/collision_env_distance_field.cpp | 27 +-- .../test/test_collision_distance_field.cpp | 64 +++---- .../src/constraint_sampler_manager.cpp | 38 +++-- .../constraint_samplers/test/pr2_arm_ik.cpp | 12 +- .../test/pr2_arm_kinematics_plugin.h | 8 +- .../moveit/distance_field/distance_field.h | 28 +-- .../test/test_distance_field.cpp | 26 +-- .../kinematic_constraint.h | 6 +- .../moveit/kinematic_constraints/utils.h | 3 +- .../src/kinematic_constraint.cpp | 47 +++--- .../moveit/kinematics_base/kinematics_base.h | 4 +- .../kinematics_base/src/kinematics_base.cpp | 5 +- .../kinematics_metrics/kinematics_metrics.h | 22 +-- .../planning_interface/planning_request.h | 2 +- .../moveit/planning_scene/planning_scene.h | 12 +- .../planning_scene/src/planning_scene.cpp | 38 +++-- .../include/moveit/profiler/profiler.h | 4 +- moveit_core/profiler/src/profiler.cpp | 2 +- .../include/moveit/robot_model/aabb.h | 66 ++++---- .../moveit/robot_model/fixed_joint_model.h | 64 +++---- .../moveit/robot_model/floating_joint_model.h | 64 +++---- .../moveit/robot_model/joint_model_group.h | 66 ++++---- .../include/moveit/robot_model/link_model.h | 64 +++---- .../moveit/robot_model/planar_joint_model.h | 64 +++---- .../robot_model/prismatic_joint_model.h | 64 +++---- .../moveit/robot_model/revolute_joint_model.h | 64 +++---- .../include/moveit/robot_model/robot_model.h | 3 +- moveit_core/robot_model/src/aabb.cpp | 64 +++---- .../robot_model/src/fixed_joint_model.cpp | 64 +++---- .../robot_model/src/floating_joint_model.cpp | 66 ++++---- moveit_core/robot_model/src/joint_model.cpp | 66 ++++---- .../robot_model/src/joint_model_group.cpp | 101 +++++------ moveit_core/robot_model/src/link_model.cpp | 67 ++++---- .../robot_model/src/planar_joint_model.cpp | 66 ++++---- .../robot_model/src/prismatic_joint_model.cpp | 64 +++---- .../robot_model/src/revolute_joint_model.cpp | 76 ++++----- moveit_core/robot_model/src/robot_model.cpp | 22 ++- moveit_core/robot_model/test/test.cpp | 64 +++---- .../moveit/robot_state/attached_body.h | 64 +++---- .../robot_state/cartesian_interpolator.h | 68 ++++---- .../include/moveit/robot_state/conversions.h | 64 +++---- .../include/moveit/robot_state/robot_state.h | 72 ++++---- moveit_core/robot_state/src/attached_body.cpp | 67 ++++---- .../src/cartesian_interpolator.cpp | 78 ++++----- moveit_core/robot_state/src/conversions.cpp | 81 ++++----- moveit_core/robot_state/src/robot_state.cpp | 85 +++++----- .../test/robot_state_benchmark.cpp | 64 +++---- .../robot_state/test/robot_state_test.cpp | 64 +++---- moveit_core/robot_state/test/test_aabb.cpp | 64 +++---- .../test/test_cartesian_interpolator.cpp | 64 +++---- .../test/test_kinematic_complex.cpp | 64 +++---- .../robot_trajectory/src/robot_trajectory.cpp | 3 +- .../test/test_robot_trajectory.cpp | 68 ++++---- .../trajectory_processing/trajectory_tools.h | 2 +- .../src/iterative_spline_parameterization.cpp | 6 +- .../time_optimal_trajectory_generation.cpp | 10 +- .../utils/include/moveit/utils/xmlrpc_casts.h | 3 +- .../utils/src/robot_model_test_utils.cpp | 3 +- .../collision_distance_field_ros_helpers.h | 2 +- .../collision_robot_distance_field_ros.h | 2 +- .../hybrid_collision_robot_ros.h | 2 +- .../kinematics_cache/kinematics_cache.h | 72 ++++---- .../kinematics_cache/src/kinematics_cache.cpp | 72 ++++---- .../kinematics_cache_ros.h | 72 ++++---- .../v1/kinematics_cache_ros/src/example.cpp | 70 ++++---- .../src/kinematics_cache_ros.cpp | 72 ++++---- .../kinematics_cache/kinematics_cache.h | 72 ++++---- .../kinematics_cache/src/kinematics_cache.cpp | 72 ++++---- .../kinematics_constraint_aware.h | 72 ++++---- .../kinematics_request_response.h | 72 ++++---- .../src/kinematics_constraint_aware.cpp | 75 ++++----- .../cached_ik_kinematics_plugin.h | 2 +- .../detail/NearestNeighborsGNAT.h | 8 +- .../src/cached_ik_kinematics_plugin.cpp | 10 +- .../templates/ikfast.h | 2 +- .../ikfast61_moveit_plugin_template.cpp | 34 ++-- .../kdl_kinematics_plugin.h | 8 +- .../src/kdl_kinematics_plugin.cpp | 12 +- .../lma_kinematics_plugin.h | 8 +- .../src/lma_kinematics_plugin.cpp | 4 +- .../srv_kinematics_plugin.h | 8 +- .../src/srv_kinematics_plugin.cpp | 4 +- .../chomp_interface/chomp_planning_context.h | 3 +- .../src/chomp_planner.cpp | 8 +- .../src/chomp_trajectory.cpp | 3 +- .../src/chomp_optimizer_adapter.cpp | 3 +- .../OMPLDynamicReconfigureConfig.h | 2 +- .../detail/constrained_goal_sampler.h | 64 +++---- .../detail/constrained_sampler.h | 64 +++---- .../detail/constrained_valid_state_sampler.h | 64 +++---- .../detail/constraint_approximations.h | 66 ++++---- .../detail/constraints_library.h | 64 +++---- .../moveit/ompl_interface/detail/goal_union.h | 64 +++---- .../detail/projection_evaluators.h | 64 +++---- .../detail/state_validity_checker.h | 64 +++---- .../detail/threadsafe_state_storage.h | 64 +++---- .../model_based_planning_context.h | 64 +++---- .../moveit/ompl_interface/ompl_interface.h | 64 +++---- .../joint_model_state_space_factory.h | 64 +++---- .../work_space/pose_model_state_space.h | 64 +++---- .../pose_model_state_space_factory.h | 64 +++---- .../ompl_interface/planning_context_manager.h | 64 +++---- .../src/detail/constraints_library.cpp | 38 +++-- .../src/model_based_planning_context.cpp | 28 +-- .../ompl_interface/src/ompl_interface.cpp | 10 +- .../joint_space/joint_model_state_space.cpp | 64 +++---- .../joint_model_state_space_factory.cpp | 64 +++---- .../model_based_state_space.cpp | 8 +- .../model_based_state_space_factory.cpp | 64 +++---- .../src/planning_context_manager.cpp | 10 +- .../include/sbpl_interface/bfs3d/BFS_3D.h | 2 +- .../sbpl_interface/environment_chain3d.h | 2 +- .../environment_chain3d_types.h | 2 +- .../include/sbpl_interface/sbpl_interface.h | 2 +- .../sbpl_interface/sbpl_meta_interface.h | 2 +- .../include/sbpl_interface/sbpl_params.h | 2 +- .../core/sbpl_interface/src/bfs3d/BFS_3D.cpp | 2 +- .../core/sbpl_interface/src/bfs3d/Search.cpp | 2 +- .../core/sbpl_interface/src/bresenham.cpp | 64 +++---- .../src/environment_chain3d.cpp | 66 ++++---- .../sbpl_interface/src/sbpl_interface.cpp | 2 +- .../src/sbpl_meta_interface.cpp | 2 +- .../src/sbpl_meta_plugin.cpp | 2 +- .../sbpl_interface_ros/src/sbpl_plugin.cpp | 2 +- .../trajopt_interface/trajopt_interface.h | 2 +- .../trajopt/src/problem_description.cpp | 8 +- .../src/moveit_fake_controller_manager.cpp | 4 +- .../src/controller_manager_plugin.cpp | 3 +- ...ollow_joint_trajectory_controller_handle.h | 2 +- ...low_joint_trajectory_controller_handle.cpp | 5 +- .../src/moveit_simple_controller_manager.cpp | 12 +- .../benchmarks/src/benchmark_execution.cpp | 32 ++-- .../moveit/benchmarks/BenchmarkExecutor.h | 67 ++++---- .../moveit/benchmarks/BenchmarkOptions.h | 64 +++---- .../benchmarks/src/BenchmarkExecutor.cpp | 68 ++++---- .../benchmarks/src/BenchmarkOptions.cpp | 64 +++---- moveit_ros/benchmarks/src/RunBenchmark.cpp | 64 +++---- .../CombinePredefinedPosesBenchmark.cpp | 64 +++---- .../capability_names.h | 2 +- .../src/pick_place_action_capability.cpp | 7 +- .../src/pick_place_action_capability.h | 3 +- .../moveit/pick_place/manipulation_pipeline.h | 64 +++---- .../include/moveit/pick_place/pick_place.h | 64 +++---- .../moveit/pick_place/pick_place_params.h | 64 +++---- .../pick_place/src/manipulation_pipeline.cpp | 68 ++++---- .../manipulation/pick_place/src/place.cpp | 4 +- .../pick_place/src/plan_stage.cpp | 10 +- .../moveit/move_group/move_group_context.h | 2 +- .../cartesian_path_service_capability.cpp | 5 +- .../get_planning_scene_service_capability.h | 2 +- .../kinematics_service_capability.cpp | 7 +- .../include/moveit_servo/collision_check.h | 2 +- .../moveit_servo/joint_state_subscriber.h | 2 +- .../include/moveit_servo/low_pass_filter.h | 2 +- .../moveit_servo/make_shared_from_pool.h | 2 +- .../moveit_servo/include/moveit_servo/servo.h | 2 +- .../include/moveit_servo/servo_calcs.h | 6 +- .../include/moveit_servo/servo_parameters.h | 2 +- .../include/moveit_servo/status_codes.h | 2 +- .../moveit_servo/src/collision_check.cpp | 13 +- .../cpp_interface_example.cpp | 2 +- .../src/joint_state_subscriber.cpp | 2 +- .../moveit_servo/src/low_pass_filter.cpp | 15 +- moveit_ros/moveit_servo/src/servo.cpp | 2 +- moveit_ros/moveit_servo/src/servo_calcs.cpp | 33 ++-- moveit_ros/moveit_servo/src/servo_server.cpp | 2 +- .../src/teleop_examples/spacenav_to_twist.cpp | 2 +- .../src/occupancy_map_monitor.cpp | 3 +- .../src/depth_image_octomap_updater.cpp | 6 +- .../src/updater_plugin.cpp | 3 +- .../src/lazy_free_space_updater.cpp | 7 +- .../moveit/mesh_filter/transform_provider.h | 2 +- .../mesh_filter/src/mesh_filter.cpp | 5 +- .../mesh_filter/src/mesh_filter_base.cpp | 3 +- .../src/shape_mask.cpp | 3 +- .../src/pointcloud_octomap_updater.cpp | 6 +- .../src/constraint_sampler_manager_loader.cpp | 5 +- .../src/kinematics_plugin_loader.cpp | 10 +- .../plan_execution/src/plan_execution.cpp | 9 +- ...re_collision_speed_checking_fcl_bullet.cpp | 18 +- .../src/fix_start_state_bounds.cpp | 4 +- .../src/fix_start_state_collision.cpp | 4 +- .../current_state_monitor.h | 2 +- .../planning_scene_monitor.h | 4 +- .../src/planning_scene_monitor.cpp | 3 +- .../src/robot_model_loader.cpp | 7 +- .../trajectory_execution_manager.h | 3 +- .../src/trajectory_execution_manager.cpp | 19 ++- .../common_objects.h | 4 +- .../src/move_group_interface.cpp | 4 +- .../src/wrap_python_move_group.cpp | 12 +- .../src/planning_scene_interface.cpp | 3 +- .../moveit/py_bindings_tools/serialize_msg.h | 4 +- .../test/move_group_interface_cpp_test.cpp | 66 ++++---- .../test/move_group_pick_place_test.cpp | 66 ++++---- .../test/subframes_test.cpp | 66 ++++---- .../moveit/robot_interaction/interaction.h | 2 +- .../interactive_marker_helpers.h | 5 +- .../src/interactive_marker_helpers.cpp | 5 +- .../src/robot_interaction.cpp | 17 +- .../motion_planning_frame.h | 2 +- .../motion_planning_frame_joints_widget.h | 2 +- .../motion_planning_param_widget.h | 2 +- .../src/motion_planning_display.cpp | 27 +-- .../src/motion_planning_frame.cpp | 3 +- .../src/motion_planning_frame_context.cpp | 5 +- .../src/motion_planning_frame_objects.cpp | 20 ++- .../src/motion_planning_frame_planning.cpp | 6 +- .../src/motion_planning_frame_scenes.cpp | 20 ++- .../src/motion_planning_frame_states.cpp | 8 +- .../src/planning_scene_display.cpp | 48 +++--- .../robot_state_display.h | 2 +- .../src/robot_state_display.cpp | 18 +- .../rviz_plugin_render_tools/render_shapes.h | 2 +- .../src/trajectory_visualization.cpp | 32 ++-- .../src/trajectory_display.cpp | 7 +- .../moveit/warehouse/constraints_storage.h | 3 +- .../warehouse/warehouse/src/broadcast.cpp | 9 +- .../warehouse/src/initialize_demo_db.cpp | 7 +- .../warehouse/src/moveit_message_storage.cpp | 3 +- .../warehouse/src/planning_scene_storage.cpp | 5 +- .../warehouse/warehouse/src/save_as_text.cpp | 7 +- .../warehouse/src/save_to_warehouse.cpp | 7 +- .../src/trajectory_constraints_storage.cpp | 3 +- .../src/tools/collision_linear_model.cpp | 2 +- .../src/tools/moveit_config_data.cpp | 159 +++++++++++------- .../widgets/configuration_files_widget.cpp | 38 +++-- .../src/widgets/default_collisions_widget.cpp | 7 +- .../src/widgets/default_collisions_widget.h | 4 +- .../src/widgets/end_effectors_widget.cpp | 12 +- .../src/widgets/group_edit_widget.cpp | 12 +- .../src/widgets/passive_joints_widget.cpp | 8 +- .../src/widgets/perception_widget.cpp | 3 +- .../src/widgets/planning_groups_widget.cpp | 45 ++--- .../src/widgets/robot_poses_widget.cpp | 18 +- .../src/widgets/ros_controllers_widget.cpp | 3 +- .../src/widgets/setup_assistant_widget.h | 2 +- .../src/widgets/simulation_widget.cpp | 10 +- .../src/widgets/start_screen_widget.cpp | 34 ++-- .../src/widgets/virtual_joints_widget.cpp | 8 +- .../test/moveit_config_data_test.cpp | 64 +++---- 265 files changed, 3508 insertions(+), 3450 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h index 7782670035..fdc9aec03b 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h @@ -43,9 +43,9 @@ namespace collision_detection { /** \brief An allocator for AllValid collision detectors */ class CollisionDetectorAllocatorAllValid - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: static const std::string NAME; // defined in collision_env_allvalid.cpp }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index 74a0c9543f..05ea5b66ba 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -183,8 +183,7 @@ void AllowedCollisionMatrix::removeEntry(const std::string& name1, const std::st } } -void AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector& other_names, - bool allowed) +void AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector& other_names, bool allowed) { for (const auto& other_name : other_names) if (other_name != name) diff --git a/moveit_core/collision_detection/src/collision_tools.cpp b/moveit_core/collision_detection/src/collision_tools.cpp index d1ba1f4b39..8071c6ddfa 100644 --- a/moveit_core/collision_detection/src/collision_tools.cpp +++ b/moveit_core/collision_detection/src/collision_tools.cpp @@ -168,8 +168,7 @@ void intersectCostSources(std::set& cost_sources, const std::set= tmp.aabb_max[0] || tmp.aabb_min[1] >= tmp.aabb_max[1] || - tmp.aabb_min[2] >= tmp.aabb_max[2]) + if (tmp.aabb_min[0] >= tmp.aabb_max[0] || tmp.aabb_min[1] >= tmp.aabb_max[1] || tmp.aabb_min[2] >= tmp.aabb_max[2]) continue; tmp.cost = std::max(source_a.cost, source_b.cost); cost_sources.insert(tmp); diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h index ec1a451814..d749d5c209 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h @@ -65,7 +65,7 @@ class BulletDiscreteBVHManager : public BulletBVHManager const collision_detection::AllowedCollisionMatrix* acm, bool self) override; /**@brief Add a bullet collision object to the manager - * @param cow The bullet collision object */ + * @param cow The bullet collision object */ void addCollisionObject(const CollisionObjectWrapperPtr& cow) override; }; } // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h index 252f8c3e51..26ed9e5928 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h @@ -82,15 +82,15 @@ inline bool acmCheck(const std::string& body_1, const std::string& body_2, } else { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No entry in ACM found, collision check between " - << body_1 << " and " << body_2); + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "No entry in ACM found, collision check between " << body_1 << " and " << body_2); return false; } } else { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "No ACM, collision check between " << body_1 << " and " - << body_2); + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "No ACM, collision check between " << body_1 << " and " << body_2); return false; } } @@ -717,8 +717,8 @@ class TesseractCollisionPairCallback : public btOverlapCallback std::pair pair_names{ cow0->getName(), cow1->getName() }; if (results_callback_.needsCollision(cow0, cow1)) { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Processing " << cow0->getName() << " vs " - << cow1->getName()); + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Processing " << cow0->getName() << " vs " << cow1->getName()); btCollisionObjectWrapper obj0_wrap(nullptr, cow0->getCollisionShape(), cow0, cow0->getWorldTransform(), -1, -1); btCollisionObjectWrapper obj1_wrap(nullptr, cow1->getCollisionShape(), cow1, cow1->getWorldTransform(), -1, -1); @@ -740,8 +740,8 @@ class TesseractCollisionPairCallback : public btOverlapCallback } else { - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not processing " << cow0->getName() << " vs " - << cow1->getName()); + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Not processing " << cow0->getName() << " vs " << cow1->getName()); } return false; } @@ -961,8 +961,8 @@ struct BroadphaseFilterCallback : public btOverlapFilterCallback if (cow0->m_touch_links == cow1->m_touch_links) return false; - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Broadphase pass " << cow0->getName() << " vs " - << cow1->getName()); + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Broadphase pass " << cow0->getName() << " vs " << cow1->getName()); return true; } }; diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h index d6e2861398..c77afce1b0 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h @@ -53,8 +53,8 @@ inline bool isLinkActive(const std::vector& active, const std::stri } /** \brief Stores a single contact result in the requested way. -* \param found Indicates if a contact for this pair of objects has already been found -* \return Pointer to the newly inserted contact */ + * \param found Indicates if a contact for this pair of objects has already been found + * \return Pointer to the newly inserted contact */ inline collision_detection::Contact* processResult(ContactTestData& cdata, collision_detection::Contact& contact, const std::pair& key, bool found) { @@ -67,8 +67,8 @@ inline collision_detection::Contact* processResult(ContactTestData& cdata, colli } } - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Contact btw " << key.first << " and " << key.second - << " dist: " << contact.depth); + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Contact btw " << key.first << " and " << key.second << " dist: " << contact.depth); // case if pair hasn't a contact yet if (!found) { diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h index 7eb2e43f5a..d518c09169 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h @@ -31,10 +31,10 @@ namespace collision_detection_bullet { /** \brief Recursively traverses robot from root to get all active links -* -* \param active_links Stores the active links -* \param urdf_link The current urdf link representation -* \param active Indicates if link is considered active */ + * + * \param active_links Stores the active links + * \param urdf_link The current urdf link representation + * \param active Indicates if link is considered active */ static inline void getActiveLinkNamesRecursive(std::vector& active_links, const urdf::LinkConstSharedPtr& urdf_link, bool active) { diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h index fec946f357..8b3c11057e 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h @@ -43,7 +43,7 @@ namespace collision_detection { /** \brief An allocator for Bullet collision detectors */ class CollisionDetectorAllocatorBullet - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: static const std::string NAME; // defined in collision_env_bullet.cpp diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h index 46c28ae63c..e8612f08fe 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h @@ -46,4 +46,4 @@ class CollisionDetectorBtPluginLoader : public CollisionPlugin public: bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const override; }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp index 800fe90fd0..c59d8de93e 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp @@ -41,9 +41,9 @@ BulletBVHManager::BulletBVHManager() { dispatcher_.reset(new btCollisionDispatcher(&coll_config_)); - dispatcher_->registerCollisionCreateFunc( - BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE, - coll_config_.getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE, CONVEX_SHAPE_PROXYTYPE)); + dispatcher_->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE, + coll_config_.getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE, + CONVEX_SHAPE_PROXYTYPE)); dispatcher_->setDispatcherFlags(dispatcher_->getDispatcherFlags() & ~btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD); diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp index e047be3324..64e9df962c 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp @@ -117,9 +117,8 @@ void BulletCastBVHManager::setCastCollisionObjectsTransform(const std::string& n } else { - ROS_ERROR_NAMED( - "collision_detection.bullet", - "I can only continuous collision check convex shapes and compound shapes made of convex shapes"); + ROS_ERROR_NAMED("collision_detection.bullet", "I can only continuous collision check convex shapes and " + "compound shapes made of convex shapes"); throw std::runtime_error( "I can only continuous collision check convex shapes and compound shapes made of convex shapes"); } @@ -138,8 +137,8 @@ void BulletCastBVHManager::contactTest(collision_detection::CollisionResult& col broadphase_->calculateOverlappingPairs(dispatcher_.get()); btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Number overlapping candidates " - << pair_cache->getNumOverlappingPairs()); + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Number overlapping candidates " << pair_cache->getNumOverlappingPairs()); BroadphaseContactResultCallback cc(cdata, contact_distance_, acm, false, true); TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp index 68eda45091..df78daba46 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp @@ -66,16 +66,16 @@ void BulletDiscreteBVHManager::contactTest(collision_detection::CollisionResult& broadphase_->calculateOverlappingPairs(dispatcher_.get()); btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Num overlapping candidates " - << pair_cache->getNumOverlappingPairs()); + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Num overlapping candidates " << pair_cache->getNumOverlappingPairs()); BroadphaseContactResultCallback cc(cdata, contact_distance_, acm, self); TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get()); - ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", (collisions.collision ? "In" : "No") << " collision with " - << collisions.contact_count - << " collisions"); + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", (collisions.collision ? "In" : "No") + << " collision with " << collisions.contact_count + << " collisions"); } void BulletDiscreteBVHManager::addCollisionObject(const CollisionObjectWrapperPtr& cow) diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp index 2617787cb4..2f2b771964 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp @@ -272,8 +272,9 @@ CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const co , m_shape_poses(shape_poses) , m_collision_object_types(collision_object_types) { - if (shapes.empty() || shape_poses.empty() || (shapes.size() != shape_poses.size() || collision_object_types.empty() || - shapes.size() != collision_object_types.size())) + if (shapes.empty() || shape_poses.empty() || + (shapes.size() != shape_poses.size() || collision_object_types.empty() || + shapes.size() != collision_object_types.size())) { throw std::exception(); } diff --git a/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp b/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp index 8279cf6d0d..2caee9406e 100644 --- a/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp @@ -44,6 +44,6 @@ bool CollisionDetectorBtPluginLoader::initialize(const planning_scene::PlanningS scene->setActiveCollisionDetector(CollisionDetectorAllocatorBullet::create(), exclusive); return true; } -} +} // namespace collision_detection PLUGINLIB_EXPORT_CLASS(collision_detection::CollisionDetectorBtPluginLoader, collision_detection::CollisionPlugin) diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp index 079943d03b..a9f2b57757 100644 --- a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp @@ -312,9 +312,8 @@ void CollisionEnvBullet::notifyObjectChange(const ObjectConstPtr& obj, World::Ac } } -void CollisionEnvBullet::addAttachedOjects( - const moveit::core::RobotState& state, - std::vector& cows) const +void CollisionEnvBullet::addAttachedOjects(const moveit::core::RobotState& state, + std::vector& cows) const { std::vector attached_bodies; state.getAttachedBodies(attached_bodies); @@ -335,8 +334,8 @@ void CollisionEnvBullet::addAttachedOjects( } catch (std::exception&) { - ROS_ERROR_STREAM_NAMED("collision_detetction.bullet", "Not adding " << body->getName() - << " due to bad arguments."); + ROS_ERROR_STREAM_NAMED("collision_detetction.bullet", + "Not adding " << body->getName() << " due to bad arguments."); } } } diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index 5ed83d7490..62db457a55 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -226,7 +226,7 @@ struct FCLGeometry } /** \brief Updates the \e collision_geometry_data_ with new data while also setting the \e collision_geometry_ to the - * new data. */ + * new data. */ template void updateCollisionGeometryData(const T* data, int shape_index, bool newType) { @@ -269,21 +269,21 @@ struct FCLManager }; /** \brief Callback function used by the FCLManager used for each pair of collision objects to -* calculate object contact information. -* -* \param o1 First FCL collision object -* \param o2 Second FCL collision object -* \data General pointer to arbitrary data which is used during the callback -* \return True terminates the distance check, false continues it to the next pair of objects */ + * calculate object contact information. + * + * \param o1 First FCL collision object + * \param o2 Second FCL collision object + * \data General pointer to arbitrary data which is used during the callback + * \return True terminates the distance check, false continues it to the next pair of objects */ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data); /** \brief Callback function used by the FCLManager used for each pair of collision objects to -* calculate collisions and distances. -* -* \param o1 First FCL collision object -* \param o2 Second FCL collision object -* \data General pointer to arbitrary data which is used during the callback -* \return True terminates the collision check, false continues it to the next pair of objects */ + * calculate collisions and distances. + * + * \param o1 First FCL collision object + * \param o2 Second FCL collision object + * \data General pointer to arbitrary data which is used during the callback + * \return True terminates the collision check, false continues it to the next pair of objects */ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& min_dist); /** \brief Create new FCLGeometry object out of robot link model. */ diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h index 88bcfcddae..fde03c4cb5 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h @@ -43,9 +43,9 @@ namespace collision_detection { /** \brief An allocator for FCL collision detectors */ class CollisionDetectorAllocatorFCL - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: static const std::string NAME; // defined in collision_env_fcl.cpp }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h index c18d059178..307130939c 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h @@ -46,4 +46,4 @@ class CollisionDetectorFCLPluginLoader : public CollisionPlugin public: bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const override; }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h index 278071b3ce..c3e81354c3 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h @@ -92,12 +92,12 @@ class CollisionEnvFCL : public CollisionEnv protected: /** \brief Updates the FCL collision geometry and objects saved in the CollisionRobotFCL members to reflect a new - * padding or scaling of the robot links. - * - * It searches for the link through the pointed-to robot model of the CollisionRobot and then constructs new FCL - * collision objects and geometries depending on the changed robot model. - * - * \param links The names of the links which have been updated in the robot model */ + * padding or scaling of the robot links. + * + * It searches for the link through the pointed-to robot model of the CollisionRobot and then constructs new FCL + * collision objects and geometries depending on the changed robot model. + * + * \param links The names of the links which have been updated in the robot model */ void updatedPaddingOrScaling(const std::vector& links) override; /** \brief Bundles the different checkSelfCollision functions into a single function */ @@ -117,26 +117,26 @@ class CollisionEnvFCL : public CollisionEnv void updateFCLObject(const std::string& id); /** \brief Out of the current robot state and its attached bodies construct an FCLObject which can then be used to - * check for collision. - * - * The current state is used to recalculate the AABB of the FCL collision objects. However they are not computed from - * scratch (which would require call to computeLocalAABB()) but are only transformed according to the joint states. - * - * \param state The current robot state - * \param fcl_obj The newly filled object */ + * check for collision. + * + * The current state is used to recalculate the AABB of the FCL collision objects. However they are not computed from + * scratch (which would require call to computeLocalAABB()) but are only transformed according to the joint states. + * + * \param state The current robot state + * \param fcl_obj The newly filled object */ void constructFCLObjectRobot(const moveit::core::RobotState& state, FCLObject& fcl_obj) const; /** \brief Prepares for the collision check through constructing an FCL collision object out of the current robot - * state and specifying a broadphase collision manager of FCL where the constructed object is registered to. */ + * state and specifying a broadphase collision manager of FCL where the constructed object is registered to. */ void allocSelfCollisionBroadPhase(const moveit::core::RobotState& state, FCLManager& manager) const; /** \brief Converts all shapes which make up an atttached body into a vector of FCLGeometryConstPtr. - * - * When they are converted, they can be added to the FCL representation of the robot for collision checking. - * - * \param ab Pointer to the attached body - * \param geoms Output vector of geometries - */ + * + * When they are converted, they can be added to the FCL representation of the robot for collision checking. + * + * \param ab Pointer to the attached body + * \param geoms Output vector of geometries + */ void getAttachedBodyObjects(const moveit::core::AttachedBody* ab, std::vector& geoms) const; /** \brief Vector of shared pointers to the FCL geometry for the objects in fcl_objs_. */ diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h index 7c0cc3334f..6d5c774a87 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h @@ -80,7 +80,7 @@ namespace details struct sse_meta_f4; template struct Vec3Data; -} +} // namespace details template class Vec3fX; #if FCL_HAVE_SSE diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index 30aa876374..bc136f28f7 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -96,10 +96,11 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi { always_allow_collision = true; if (cdata->req_->verbose) - ROS_DEBUG_NAMED( - "collision_detection.fcl", "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. " - "No contacts are computed.", - cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str()); + ROS_DEBUG_NAMED("collision_detection.fcl", + "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. " + "No contacts are computed.", + cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), + cd2->getTypeString().c_str()); } else if (type == AllowedCollision::CONDITIONAL) { @@ -179,8 +180,9 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi std::size_t num_max_cost_sources = cdata->req_->max_cost_sources; bool enable_contact = true; fcl::CollisionResultd col_result; - int num_contacts = fcl::collide(o1, o2, fcl::CollisionRequestd(std::numeric_limits::max(), enable_contact, - num_max_cost_sources, enable_cost), + int num_contacts = fcl::collide(o1, o2, + fcl::CollisionRequestd(std::numeric_limits::max(), enable_contact, + num_max_cost_sources, enable_cost), col_result); if (num_contacts > 0) { @@ -211,8 +213,9 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi cd1->getID().c_str(), cd2->getID().c_str()); } else if (cdata->req_->verbose) - ROS_INFO_NAMED("collision_detection.fcl", "Found unacceptable contact between '%s' (type '%s') and '%s' " - "(type '%s'). Contact was stored.", + ROS_INFO_NAMED("collision_detection.fcl", + "Found unacceptable contact between '%s' (type '%s') and '%s' " + "(type '%s'). Contact was stored.", cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str()); cdata->res_->collision = true; @@ -247,9 +250,10 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi bool enable_contact = true; fcl::CollisionResultd col_result; - int num_contacts = fcl::collide( - o1, o2, fcl::CollisionRequestd(want_contact_count, enable_contact, num_max_cost_sources, enable_cost), - col_result); + int num_contacts = + fcl::collide(o1, o2, + fcl::CollisionRequestd(want_contact_count, enable_contact, num_max_cost_sources, enable_cost), + col_result); if (num_contacts > 0) { int num_contacts_initial = num_contacts; @@ -264,8 +268,9 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi } if (cdata->req_->verbose) - ROS_INFO_NAMED("collision_detection.fcl", "Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), " - "which constitute a collision. %d contacts will be stored", + ROS_INFO_NAMED("collision_detection.fcl", + "Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), " + "which constitute a collision. %d contacts will be stored", num_contacts_initial, cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str(), num_contacts); @@ -309,9 +314,10 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi { cdata->res_->collision = true; if (cdata->req_->verbose) - ROS_INFO_NAMED("collision_detection.fcl", "Found a contact between '%s' (type '%s') and '%s' (type '%s'), " - "which constitutes a collision. " - "Contact information is not stored.", + ROS_INFO_NAMED("collision_detection.fcl", + "Found a contact between '%s' (type '%s') and '%s' (type '%s'), " + "which constitutes a collision. " + "Contact information is not stored.", cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str()); } @@ -348,8 +354,9 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi { cdata->done_ = cdata->req_->is_done(*cdata->res_); if (cdata->done_ && cdata->req_->verbose) - ROS_INFO_NAMED("collision_detection.fcl", "Collision checking is considered complete due to external callback. " - "%s was found. %u contacts are stored.", + ROS_INFO_NAMED("collision_detection.fcl", + "Collision checking is considered complete due to external callback. " + "%s was found. %u contacts are stored.", cdata->res_->collision ? "Collision" : "No collision", (unsigned int)cdata->res_->contact_count); } diff --git a/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp b/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp index f397a7ae94..9e3fbdf39a 100644 --- a/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp @@ -44,6 +44,6 @@ bool CollisionDetectorFCLPluginLoader::initialize(const planning_scene::Planning scene->setActiveCollisionDetector(CollisionDetectorAllocatorFCL::create(), exclusive); return true; } -} +} // namespace collision_detection PLUGINLIB_EXPORT_CLASS(collision_detection::CollisionDetectorFCLPluginLoader, collision_detection::CollisionPlugin) diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h index b699170892..188be97465 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h @@ -43,9 +43,9 @@ namespace collision_detection { /** \brief An allocator for Distance Field collision detectors */ class CollisionDetectorAllocatorDistanceField - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: static const std::string NAME; // defined in collision_env_distance_field.cpp }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h index efce4d7b11..0e35ad38ae 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h @@ -43,9 +43,9 @@ namespace collision_detection { /** \brief An allocator for Hybrid collision detectors */ class CollisionDetectorAllocatorHybrid - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: static const std::string NAME; // defined in collision_env_hybrid.cpp }; -} +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index d65dd91342..1340a8299e 100644 --- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -288,9 +288,10 @@ bool CollisionEnvDistanceField::getSelfCollisions(const collision_detection::Col if (req.contacts) { std::vector colls; - bool coll = getCollisionSphereCollision( - gsr->dfce_->distance_field_.get(), *collision_spheres_1, *sphere_centers_1, max_propogation_distance_, - collision_tolerance_, std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls); + bool coll = + getCollisionSphereCollision(gsr->dfce_->distance_field_.get(), *collision_spheres_1, *sphere_centers_1, + max_propogation_distance_, collision_tolerance_, + std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls); if (coll) { res.collision = true; @@ -817,8 +818,7 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache // << " num " << attached_bodies[j]->getTouchLinks().size() // << std::endl; // touch links take priority - if (link_attached_bodies[j]->getTouchLinks().find(link_name) != - link_attached_bodies[j]->getTouchLinks().end()) + if (link_attached_bodies[j]->getTouchLinks().find(link_name) != link_attached_bodies[j]->getTouchLinks().end()) { dfce->intra_group_collision_enabled_[i][att_count + dfce->link_names_.size()] = false; // std::cerr << "Setting intra group for " << link_name << " and @@ -1082,8 +1082,8 @@ void CollisionEnvDistanceField::addLinkBodyDecompositions( ROS_DEBUG_STREAM(__FUNCTION__ << " Finished "); } -PosedBodySphereDecompositionPtr CollisionEnvDistanceField::getPosedLinkBodySphereDecomposition( - const moveit::core::LinkModel* ls, unsigned int ind) const +PosedBodySphereDecompositionPtr +CollisionEnvDistanceField::getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel* ls, unsigned int ind) const { PosedBodySphereDecompositionPtr ret; ret.reset(new PosedBodySphereDecomposition(link_body_decomposition_vector_[ind])); @@ -1566,9 +1566,9 @@ void CollisionEnvDistanceField::getAllCollisions(const CollisionRequest& req, Co (const_cast(this))->last_gsr_ = gsr; } -bool CollisionEnvDistanceField::getEnvironmentCollisions( - const CollisionRequest& req, CollisionResult& res, const distance_field::DistanceFieldConstPtr& env_distance_field, - GroupStateRepresentationPtr& gsr) const +bool CollisionEnvDistanceField::getEnvironmentCollisions(const CollisionRequest& req, CollisionResult& res, + const distance_field::DistanceFieldConstPtr& env_distance_field, + GroupStateRepresentationPtr& gsr) const { for (unsigned int i = 0; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); i++) { @@ -1597,9 +1597,10 @@ bool CollisionEnvDistanceField::getEnvironmentCollisions( if (req.contacts) { std::vector colls; - bool coll = getCollisionSphereCollision( - env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, max_propogation_distance_, - collision_tolerance_, std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls); + bool coll = + getCollisionSphereCollision(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, + max_propogation_distance_, collision_tolerance_, + std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls); if (coll) { res.collision = true; diff --git a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp index 56b0bc553c..55908d27c3 100644 --- a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp +++ b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /** \author E. Gil Jones */ diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp index 5b82d70ecf..f444d35054 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp @@ -70,8 +70,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // if there are joint constraints, we could possibly get a sampler from those if (!constr.joint_constraints.empty()) { - ROS_DEBUG_NAMED("constraint_samplers", "There are joint constraints specified. " - "Attempting to construct a JointConstraintSampler for group '%s'", + ROS_DEBUG_NAMED("constraint_samplers", + "There are joint constraints specified. " + "Attempting to construct a JointConstraintSampler for group '%s'", jmg->getName().c_str()); std::map joint_coverage; @@ -142,8 +143,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // should be used if (ik_alloc) { - ROS_DEBUG_NAMED("constraint_samplers", "There is an IK allocator for '%s'. " - "Checking for corresponding position and/or orientation constraints", + ROS_DEBUG_NAMED("constraint_samplers", + "There is an IK allocator for '%s'. " + "Checking for corresponding position and/or orientation constraints", jmg->getName().c_str()); // keep track of which links we constrained @@ -177,8 +179,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni { // assign the link to a new constraint sampler used_l[constr.position_constraints[p].link_name] = iks; - ROS_DEBUG_NAMED("constraint_samplers", "Allocated an IK-based sampler for group '%s' " - "satisfying position and orientation constraints on link '%s'", + ROS_DEBUG_NAMED("constraint_samplers", + "Allocated an IK-based sampler for group '%s' " + "satisfying position and orientation constraints on link '%s'", jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str()); } } @@ -209,8 +212,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni if (use) { used_l[position_constraint.link_name] = iks; - ROS_DEBUG_NAMED("constraint_samplers", "Allocated an IK-based sampler for group '%s' " - "satisfying position constraints on link '%s'", + ROS_DEBUG_NAMED("constraint_samplers", + "Allocated an IK-based sampler for group '%s' " + "satisfying position constraints on link '%s'", jmg->getName().c_str(), position_constraint.link_name.c_str()); } } @@ -238,8 +242,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni if (use) { used_l[orientation_constraint.link_name] = iks; - ROS_DEBUG_NAMED("constraint_samplers", "Allocated an IK-based sampler for group '%s' " - "satisfying orientation constraints on link '%s'", + ROS_DEBUG_NAMED("constraint_samplers", + "Allocated an IK-based sampler for group '%s' " + "satisfying orientation constraints on link '%s'", jmg->getName().c_str(), orientation_constraint.link_name.c_str()); } } @@ -264,8 +269,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // find the sampler with the smallest sampling volume; delete the rest IKConstraintSamplerPtr iks = used_l.begin()->second; double msv = iks->getSamplingVolume(); - for (std::map::const_iterator it = ++used_l.begin(); it != used_l.end(); - ++it) + for (std::map::const_iterator it = ++used_l.begin(); it != used_l.end(); ++it) { double v = it->second->getSamplingVolume(); if (v < msv) @@ -290,8 +294,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // we now check to see if we can use samplers from subgroups if (!ik_subgroup_alloc.empty()) { - ROS_DEBUG_NAMED("constraint_samplers", "There are IK allocators for subgroups of group '%s'. " - "Checking for corresponding position and/or orientation constraints", + ROS_DEBUG_NAMED("constraint_samplers", + "There are IK allocators for subgroups of group '%s'. " + "Checking for corresponding position and/or orientation constraints", jmg->getName().c_str()); bool some_sampler_valid = false; @@ -326,8 +331,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni ConstraintSamplerPtr cs = selectDefaultSampler(scene, it->first->getName(), sub_constr); if (cs) { - ROS_DEBUG_NAMED("constraint_samplers", "Constructed a sampler for the joints corresponding to group '%s', " - "but part of group '%s'", + ROS_DEBUG_NAMED("constraint_samplers", + "Constructed a sampler for the joints corresponding to group '%s', " + "but part of group '%s'", it->first->getName().c_str(), jmg->getName().c_str()); some_sampler_valid = true; samplers.push_back(cs); diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index d73fafa9a6..f664f95867 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -298,11 +298,9 @@ void PR2ArmIK::computeIKShoulderPan(const Eigen::Isometry3f& g_in, const double& at = sint1 * (shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint2 * sint4; bt = (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost1 * sint4; - ct = y - - (shoulder_upperarm_offset_ + - cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ + - (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cos(t4))) * - sint1; + ct = y - (shoulder_upperarm_offset_ + cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ + + (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cos(t4))) * + sint1; if (!solveCosineEqn(at, bt, ct, theta3[0], theta3[1])) continue; @@ -731,8 +729,8 @@ void PR2ArmIK::computeIKShoulderRoll(const Eigen::Isometry3f& g_in, const double std::cout << "t5 " << t5 << std::endl; std::cout << "t7 " << t7 << std::endl; #endif -// if(fabs(sin(t6)*sin(t7)-grhs_local(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_local(0,2)) > IK_EPS) -// continue; + // if(fabs(sin(t6)*sin(t7)-grhs_local(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_local(0,2)) > IK_EPS) + // continue; #ifdef DEBUG std::cout << "theta1: " << t1 << std::endl; diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index f03c56525c..8255cfbd12 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -149,10 +149,10 @@ class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase * @param ik_seed_state an initial guess solution for the inverse kinematics * @return True if a valid solution was found, false otherwise */ - 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 override; + 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 override; /** * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. diff --git a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h index dd36cc111b..19af4573a8 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h @@ -74,20 +74,20 @@ enum PlaneVisualizationType MOVEIT_CLASS_FORWARD(DistanceField); /** -* \brief DistanceField is an abstract base class for computing -* distances from sets of 3D obstacle points. The distance assigned to -* a freespace cell should be the distance to the closest obstacle -* cell. Cells that are obstacle cells should either be marked as zero -* distance, or may have a negative distance if a signed version of the -* distance field is being used and an obstacle point is internal to an -* obstacle volume. -* -* Inherited classes must contain methods for holding a dense set of 3D -* voxels as well as methods for computing the required distances. The -* distance field parent class doesn't hold the data or have any way to -* generate distances from that data. -* -*/ + * \brief DistanceField is an abstract base class for computing + * distances from sets of 3D obstacle points. The distance assigned to + * a freespace cell should be the distance to the closest obstacle + * cell. Cells that are obstacle cells should either be marked as zero + * distance, or may have a negative distance if a signed version of the + * distance field is being used and an obstacle point is internal to an + * obstacle volume. + * + * Inherited classes must contain methods for holding a dense set of 3D + * voxels as well as methods for computing the required distances. The + * distance field parent class doesn't hold the data or have any way to + * generate distances from that data. + * + */ class DistanceField { public: diff --git a/moveit_core/distance_field/test/test_distance_field.cpp b/moveit_core/distance_field/test/test_distance_field.cpp index ad4a557a3b..a834b11de2 100644 --- a/moveit_core/distance_field/test/test_distance_field.cpp +++ b/moveit_core/distance_field/test/test_distance_field.cpp @@ -412,15 +412,15 @@ TEST(TestPropagationDistanceField, TestAddRemovePoints) std::cout << "Grad " << grad.x() << " " << grad.y() << " " << grad.z() << " comp " << comp_x << " " << comp_y << " " << comp_z << std::endl; } - ASSERT_NEAR(comp_x, POINT1.x(), RESOLUTION) << dist << x << " " << y << " " << z << " " << grad.x() << " " - << grad.y() << " " << grad.z() << " " << xscale << " " << yscale - << " " << zscale << std::endl; - ASSERT_NEAR(comp_y, POINT1.y(), RESOLUTION) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() - << " " << grad.z() << " " << xscale << " " << yscale << " " - << zscale << std::endl; - ASSERT_NEAR(comp_z, POINT1.z(), RESOLUTION) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() - << " " << grad.z() << " " << xscale << " " << yscale << " " - << zscale << std::endl; + ASSERT_NEAR(comp_x, POINT1.x(), RESOLUTION) + << dist << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " + << xscale << " " << yscale << " " << zscale << std::endl; + ASSERT_NEAR(comp_y, POINT1.y(), RESOLUTION) + << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale + << " " << yscale << " " << zscale << std::endl; + ASSERT_NEAR(comp_z, POINT1.z(), RESOLUTION) + << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale + << " " << yscale << " " << zscale << std::endl; } } } @@ -588,10 +588,10 @@ TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints) EXPECT_EQ(ncell_pos.z(), cell_z); EXPECT_EQ(ncell, cell); #endif - EXPECT_GE(cell->distance_square_, 1) << dist << " " << x << " " << y << " " << z << " " << grad.x() << " " - << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " " - << zscale << " cell " << comp_x << " " << comp_y << " " << comp_z - << std::endl; + EXPECT_GE(cell->distance_square_, 1) + << dist << " " << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() + << " " << xscale << " " << yscale << " " << zscale << " cell " << comp_x << " " << comp_y << " " << comp_z + << std::endl; } } } diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index 86fb490d7b..314b1683ce 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -435,9 +435,9 @@ class OrientationConstraint : public KinematicConstraint /** * \brief The rotation target in the reference frame. * - * @return The target rotation. - * - * The returned matrix is always a valid rotation matrix. + * @return The target rotation. + * + * The returned matrix is always a valid rotation matrix. */ const Eigen::Matrix3d& getDesiredRotationMatrix() const { diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h index f11a2eb20a..7f25af5a36 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h @@ -63,8 +63,7 @@ namespace kinematic_constraints * * @return The merged set of constraints */ -moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints& first, - const moveit_msgs::Constraints& second); +moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints& first, const moveit_msgs::Constraints& second); /** \brief Check if any constraints were specified */ [[deprecated("Use moveit/utils/message_checks.h instead")]] bool isEmpty(const moveit_msgs::Constraints& constr); diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 4bad75319c..f6f74656b3 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -107,8 +107,9 @@ bool JointConstraint::configure(const moveit_msgs::JointConstraint& jc) } else if (joint_model_->getVariableCount() > 1) { - ROS_ERROR_NAMED("kinematic_constraints", "Joint '%s' has more than one parameter to constrain. " - "This type of constraint is not supported.", + ROS_ERROR_NAMED("kinematic_constraints", + "Joint '%s' has more than one parameter to constrain. " + "This type of constraint is not supported.", jc.joint_name.c_str()); joint_model_ = nullptr; } @@ -167,16 +168,18 @@ bool JointConstraint::configure(const moveit_msgs::JointConstraint& jc) { joint_position_ = bounds.min_position_; joint_tolerance_above_ = std::numeric_limits::epsilon(); - ROS_WARN_NAMED("kinematic_constraints", "Joint %s is constrained to be below the minimum bounds. " - "Assuming minimum bounds instead.", + ROS_WARN_NAMED("kinematic_constraints", + "Joint %s is constrained to be below the minimum bounds. " + "Assuming minimum bounds instead.", jc.joint_name.c_str()); } else if (bounds.max_position_ < joint_position_ - joint_tolerance_below_) { joint_position_ = bounds.max_position_; joint_tolerance_below_ = std::numeric_limits::epsilon(); - ROS_WARN_NAMED("kinematic_constraints", "Joint %s is constrained to be above the maximum bounds. " - "Assuming maximum bounds instead.", + ROS_WARN_NAMED("kinematic_constraints", + "Joint %s is constrained to be above the maximum bounds. " + "Assuming maximum bounds instead.", jc.joint_name.c_str()); } } @@ -231,8 +234,9 @@ ConstraintEvaluationResult JointConstraint::decide(const moveit::core::RobotStat bool result = dif <= (joint_tolerance_above_ + 2.0 * std::numeric_limits::epsilon()) && dif >= (-joint_tolerance_below_ - 2.0 * std::numeric_limits::epsilon()); if (verbose) - ROS_INFO_NAMED("kinematic_constraints", "Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, " - "tolerance_above: %f, tolerance_below: %f", + ROS_INFO_NAMED("kinematic_constraints", + "Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, " + "tolerance_above: %f, tolerance_below: %f", result ? "satisfied" : "violated", joint_variable_name_.c_str(), current_joint_position, joint_position_, joint_tolerance_above_, joint_tolerance_below_); return ConstraintEvaluationResult(result, constraint_weight_ * fabs(dif)); @@ -425,9 +429,10 @@ static inline ConstraintEvaluationResult finishPositionConstraintDecision(const double dz = desired.z() - pt.z(); if (verbose) { - ROS_INFO_NAMED( - "kinematic_constraints", "Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f", - result ? "satisfied" : "violated", name.c_str(), desired.x(), desired.y(), desired.z(), pt.x(), pt.y(), pt.z()); + ROS_INFO_NAMED("kinematic_constraints", + "Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f", + result ? "satisfied" : "violated", name.c_str(), desired.x(), desired.y(), desired.z(), pt.x(), + pt.y(), pt.z()); ROS_INFO_NAMED("kinematic_constraints", "Differences %g %g %g", dx, dy, dz); } return ConstraintEvaluationResult(result, weight * sqrt(dx * dx + dy * dy + dz * dz)); @@ -508,8 +513,9 @@ bool OrientationConstraint::configure(const moveit_msgs::OrientationConstraint& tf2::fromMsg(oc.orientation, q); if (fabs(q.norm() - 1.0) > 1e-3) { - ROS_WARN_NAMED("kinematic_constraints", "Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, " - "%f. Assuming identity instead.", + ROS_WARN_NAMED("kinematic_constraints", + "Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, " + "%f. Assuming identity instead.", oc.link_name.c_str(), oc.orientation.x, oc.orientation.y, oc.orientation.z, oc.orientation.w); q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); } @@ -681,8 +687,9 @@ bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc if (vc.cone_sides < 3) { - ROS_WARN_NAMED("kinematic_constraints", "The number of sides for the visibility region must be 3 or more. " - "Assuming 3 sides instead of the specified %d", + ROS_WARN_NAMED("kinematic_constraints", + "The number of sides for the visibility region must be 3 or more. " + "Assuming 3 sides instead of the specified %d", vc.cone_sides); cone_sides_ = 3; } @@ -964,8 +971,9 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo if (max_view_angle_ < ang) { if (verbose) - ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the view angle is %lf " - "(above the maximum allowed of %lf)", + ROS_INFO_NAMED("kinematic_constraints", + "Visibility constraint is violated because the view angle is %lf " + "(above the maximum allowed of %lf)", ang, max_view_angle_); return ConstraintEvaluationResult(false, 0.0); } @@ -986,8 +994,9 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo if (max_range_angle_ < ang) { if (verbose) - ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the range angle is %lf " - "(above the maximum allowed of %lf)", + ROS_INFO_NAMED("kinematic_constraints", + "Visibility constraint is violated because the range angle is %lf " + "(above the maximum allowed of %lf)", ang, max_range_angle_); return ConstraintEvaluationResult(false, 0.0); } diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 41267eca70..5f46385b28 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -51,8 +51,8 @@ namespace core MOVEIT_CLASS_FORWARD(JointModelGroup) MOVEIT_CLASS_FORWARD(RobotState) MOVEIT_CLASS_FORWARD(RobotModel) -} -} +} // namespace core +} // namespace moveit /** @brief API for forward and inverse kinematics */ namespace kinematics diff --git a/moveit_core/kinematics_base/src/kinematics_base.cpp b/moveit_core/kinematics_base/src/kinematics_base.cpp index 5454d3d469..fe26075128 100644 --- a/moveit_core/kinematics_base/src/kinematics_base.cpp +++ b/moveit_core/kinematics_base/src/kinematics_base.cpp @@ -120,8 +120,9 @@ bool KinematicsBase::initialize(const moveit::core::RobotModel& robot_model, con const std::string& base_frame, const std::vector& tip_frames, double search_discretization) { - ROS_WARN_NAMED(LOGNAME, "IK plugin for group '%s' relies on deprecated API. " - "Please implement initialize(RobotModel, ...).", + ROS_WARN_NAMED(LOGNAME, + "IK plugin for group '%s' relies on deprecated API. " + "Please implement initialize(RobotModel, ...).", group_name.c_str()); return false; } diff --git a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h index 3d2b1e4381..26b584e054 100644 --- a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h +++ b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h @@ -139,17 +139,17 @@ class KinematicsMetrics private: /** - * @brief Defines a multiplier for the manipulabilty - * = 1 - exp ( -penalty_multipler_ * product_{i=1}{n} (distance_to_lower_limit * - * distance_to_higher_limit/(joint_range*joint_range))) - * where n is the number of joints in the group. Floating joints are ignored in this computation. Planar joints with - * finite bounds - * are considered. - * Set penalty_multiplier_ to 0 if you don't want this multiplier to have any effect on the manipulability measures. - * See "Workspace Geometric Characterization and Manipulability of Industrial Robots", Ming-June, Tsia, PhD Thesis, - * Ohio State University, 1986, for more details. - * @return multiplier that is multiplied with every manipulability measure computed here - */ + * @brief Defines a multiplier for the manipulabilty + * = 1 - exp ( -penalty_multipler_ * product_{i=1}{n} (distance_to_lower_limit * + * distance_to_higher_limit/(joint_range*joint_range))) + * where n is the number of joints in the group. Floating joints are ignored in this computation. Planar joints with + * finite bounds + * are considered. + * Set penalty_multiplier_ to 0 if you don't want this multiplier to have any effect on the manipulability measures. + * See "Workspace Geometric Characterization and Manipulability of Industrial Robots", Ming-June, Tsia, PhD Thesis, + * Ohio State University, 1986, for more details. + * @return multiplier that is multiplied with every manipulability measure computed here + */ double getJointLimitsPenalty(const moveit::core::RobotState& state, const moveit::core::JointModelGroup* joint_model_group) const; diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h index a738f0a0df..62214d71f8 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h @@ -44,4 +44,4 @@ namespace planning_interface typedef moveit_msgs::MotionPlanRequest MotionPlanRequest; -} // planning_interface +} // namespace planning_interface diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 942cbd3b07..4430d84250 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -72,8 +72,7 @@ typedef boost::function StateFeasib The order of the arguments matters: the notion of feasibility is to be checked for motion segments that start at the first state and end at the second state. The third argument indicates whether the check should be verbose or not. */ -typedef boost::function - MotionFeasibilityFn; +typedef boost::function MotionFeasibilityFn; /** \brief A map from object names (e.g., attached bodies, collision objects) to their colors */ typedef std::map ObjectColorMap; @@ -371,8 +370,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check if a given state is in collision (with the environment or self collision) If a group name is specified, collision checking is done for that group only. */ - bool isStateColliding(const moveit_msgs::RobotState& state, const std::string& group = "", - bool verbose = false) const; + bool isStateColliding(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const; /** \brief Check whether the current state is in collision, and if needed, updates the collision transforms of the * current state before the computation. */ @@ -482,8 +480,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - checkSelfCollision(req, res, static_cast(robot_state), - getAllowedCollisionMatrix()); + checkSelfCollision(req, res, static_cast(robot_state), getAllowedCollisionMatrix()); } /** \brief Check whether a specified state (\e robot_state) is in self collision */ @@ -943,8 +940,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check if a message includes any information about a planning scene, or it is just a default, empty message. */ - [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool - isEmpty(const moveit_msgs::PlanningScene& msg); + [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool isEmpty(const moveit_msgs::PlanningScene& msg); /** \brief Check if a message includes any information about a planning scene world, or it is just a default, empty * message. */ diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 0adb7ad31d..a8a05ccb6f 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -323,8 +323,9 @@ bool PlanningScene::setActiveCollisionDetector(const std::string& collision_dete } else { - ROS_ERROR_NAMED(LOGNAME, "Cannot setActiveCollisionDetector to '%s' -- it has been added to PlanningScene. " - "Keeping existing active collision detector '%s'", + ROS_ERROR_NAMED(LOGNAME, + "Cannot setActiveCollisionDetector to '%s' -- it has been added to PlanningScene. " + "Keeping existing active collision detector '%s'", collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); return false; } @@ -358,8 +359,9 @@ PlanningScene::getCollisionEnvUnpadded(const std::string& collision_detector_nam CollisionDetectorConstIterator it = collision_.find(collision_detector_name); if (it == collision_.end()) { - ROS_ERROR_NAMED(LOGNAME, "Could not get CollisionRobotUnpadded named '%s'. " - "Returning active CollisionRobotUnpadded '%s' instead", + ROS_ERROR_NAMED(LOGNAME, + "Could not get CollisionRobotUnpadded named '%s'. " + "Returning active CollisionRobotUnpadded '%s' instead", collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); return active_collision_->getCollisionEnvUnpadded(); } @@ -1095,8 +1097,9 @@ void PlanningScene::setCurrentState(const moveit_msgs::RobotState& state) { if (!state.is_diff && state.attached_collision_objects[i].object.operation != moveit_msgs::CollisionObject::ADD) { - ROS_ERROR_NAMED(LOGNAME, "The specified RobotState is not marked as is_diff. " - "The request to modify the object '%s' is not supported. Object is ignored.", + ROS_ERROR_NAMED(LOGNAME, + "The specified RobotState is not marked as is_diff. " + "The request to modify the object '%s' is not supported. Object is ignored.", state.attached_collision_objects[i].object.id.c_str()); continue; } @@ -1450,8 +1453,9 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache } else { - ROS_ERROR_NAMED(LOGNAME, "Attempting to attach object '%s' to link '%s' but no geometry specified " - "and such an object does not exist in the collision world", + ROS_ERROR_NAMED(LOGNAME, + "Attempting to attach object '%s' to link '%s' but no geometry specified " + "and such an object does not exist in the collision world", object.object.id.c_str(), object.link_name.c_str()); return false; } @@ -1546,8 +1550,9 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache ROS_DEBUG_NAMED(LOGNAME, "Removing world object with the same name as newly attached object: '%s'", object.object.id.c_str()); else - ROS_WARN_NAMED(LOGNAME, "You tried to append geometry to an attached object " - "that is actually a world object ('%s'). World geometry is ignored.", + ROS_WARN_NAMED(LOGNAME, + "You tried to append geometry to an attached object " + "that is actually a world object ('%s'). World geometry is ignored.", object.object.id.c_str()); } @@ -1556,8 +1561,9 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache !robot_state_->hasAttachedBody(object.object.id)) { if (robot_state_->clearAttachedBody(object.object.id)) - ROS_DEBUG_NAMED(LOGNAME, "The robot state already had an object named '%s' attached to link '%s'. " - "The object was replaced.", + ROS_DEBUG_NAMED(LOGNAME, + "The robot state already had an object named '%s' attached to link '%s'. " + "The object was replaced.", object.object.id.c_str(), object.link_name.c_str()); robot_state_->attachBody(object.object.id, shapes, poses, object.touch_links, object.link_name, @@ -1834,8 +1840,9 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::CollisionObjec } else { - ROS_ERROR_NAMED(LOGNAME, "Number of supplied poses (%zu) for object '%s' does not match number of shapes (%zu). " - "Not moving.", + ROS_ERROR_NAMED(LOGNAME, + "Number of supplied poses (%zu) for object '%s' does not match number of shapes (%zu). " + "Not moving.", new_poses.size(), object.id.c_str(), obj->shapes_.size()); return false; } @@ -2008,8 +2015,7 @@ bool PlanningScene::isStateColliding(const std::string& group, bool verbose) return isStateColliding(getCurrentState(), group, verbose); } -bool PlanningScene::isStateColliding(const moveit::core::RobotState& state, const std::string& group, - bool verbose) const +bool PlanningScene::isStateColliding(const moveit::core::RobotState& state, const std::string& group, bool verbose) const { collision_detection::CollisionRequest req; req.verbose = verbose; diff --git a/moveit_core/profiler/include/moveit/profiler/profiler.h b/moveit_core/profiler/include/moveit/profiler/profiler.h index 1224016d07..3ea38f58fe 100644 --- a/moveit_core/profiler/include/moveit/profiler/profiler.h +++ b/moveit_core/profiler/include/moveit/profiler/profiler.h @@ -443,7 +443,7 @@ class Profiler return false; } }; -} -} +} // namespace tools +} // namespace moveit #endif diff --git a/moveit_core/profiler/src/profiler.cpp b/moveit_core/profiler/src/profiler.cpp index 50c190c53e..c70e0fb27b 100644 --- a/moveit_core/profiler/src/profiler.cpp +++ b/moveit_core/profiler/src/profiler.cpp @@ -120,7 +120,7 @@ inline double to_seconds(const boost::posix_time::time_duration& d) { return (double)d.total_microseconds() / 1000000.0; } -} +} // namespace void Profiler::status(std::ostream& out, bool merge) { diff --git a/moveit_core/robot_model/include/moveit/robot_model/aabb.h b/moveit_core/robot_model/include/moveit/robot_model/aabb.h index 997399675e..ce0315fdd2 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/aabb.h +++ b/moveit_core/robot_model/include/moveit/robot_model/aabb.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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. + *********************************************************************/ /* Author: Martin Pecka */ @@ -49,5 +49,5 @@ class AABB : public Eigen::AlignedBox3d /** \brief Extend with a box transformed by the given transform. */ void extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box); }; -} +} // namespace core } // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h index bf80d6a1a1..855cbec6c7 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h index 73cd5b7a6c..0d0e0a60cc 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index e2f1978f94..64ea874bda 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * 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, Inc. 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.h b/moveit_core/robot_model/include/moveit/robot_model/link_model.h index 682e6580ce..0c00369a48 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/link_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h index 747cc2c2ff..af71e44f4e 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h index c3c5d0ac81..ece60ffe9c 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h index 6d3b080408..9d70cf0628 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 3018008ea2..434afe842e 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -429,8 +429,7 @@ class RobotModel return b; if (!b) return a; - return joint_model_vector_[common_joint_roots_[a->getJointIndex() * joint_model_vector_.size() + - b->getJointIndex()]]; + return joint_model_vector_[common_joint_roots_[a->getJointIndex() * joint_model_vector_.size() + b->getJointIndex()]]; } /// A map of known kinematics solvers (associated to their group name) diff --git a/moveit_core/robot_model/src/aabb.cpp b/moveit_core/robot_model/src/aabb.cpp index 1b115cfefc..91552b0fa2 100644 --- a/moveit_core/robot_model/src/aabb.cpp +++ b/moveit_core/robot_model/src/aabb.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Martin Pecka */ diff --git a/moveit_core/robot_model/src/fixed_joint_model.cpp b/moveit_core/robot_model/src/fixed_joint_model.cpp index 2b514b503a..a5c8d288c3 100644 --- a/moveit_core/robot_model/src/fixed_joint_model.cpp +++ b/moveit_core/robot_model/src/fixed_joint_model.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 913d517c98..37d5826ee0 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/joint_model.cpp b/moveit_core/robot_model/src/joint_model.cpp index d93cec59cc..323a9e53af 100644 --- a/moveit_core/robot_model/src/joint_model.cpp +++ b/moveit_core/robot_model/src/joint_model.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 21998d8880..b9c6685a9c 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan, Dave Coleman */ @@ -327,15 +327,16 @@ void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNum { assert(active_joint_bounds.size() == active_joint_model_vector_.size()); for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i) - active_joint_model_vector_[i]->getVariableRandomPositionsNearBy( - rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i], - near + active_joint_model_start_index_[i], distance); + active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(rng, values + active_joint_model_start_index_[i], + *active_joint_bounds[i], + near + active_joint_model_start_index_[i], + distance); updateMimicJoints(values); } -void JointModelGroup::getVariableRandomPositionsNearBy( - random_numbers::RandomNumberGenerator& rng, double* values, const JointBoundsVector& active_joint_bounds, - const double* near, const std::map& distance_map) const +void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, + const JointBoundsVector& active_joint_bounds, const double* near, + const std::map& distance_map) const { assert(active_joint_bounds.size() == active_joint_model_vector_.size()); for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i) @@ -347,9 +348,10 @@ void JointModelGroup::getVariableRandomPositionsNearBy( distance = iter->second; else ROS_WARN_NAMED(LOGNAME, "Did not pass in distance for '%s'", active_joint_model_vector_[i]->getName().c_str()); - active_joint_model_vector_[i]->getVariableRandomPositionsNearBy( - rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i], - near + active_joint_model_start_index_[i], distance); + active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(rng, values + active_joint_model_start_index_[i], + *active_joint_bounds[i], + near + active_joint_model_start_index_[i], + distance); } updateMimicJoints(values); } @@ -360,14 +362,14 @@ void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNum { assert(active_joint_bounds.size() == active_joint_model_vector_.size()); if (distances.size() != active_joint_model_vector_.size()) - throw Exception("When sampling random values nearby for group '" + name_ + - "', distances vector should be of size " + + throw Exception("When sampling random values nearby for group '" + name_ + "', distances vector should be of size " + boost::lexical_cast(active_joint_model_vector_.size()) + ", but it is of size " + boost::lexical_cast(distances.size())); for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i) - active_joint_model_vector_[i]->getVariableRandomPositionsNearBy( - rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i], - near + active_joint_model_start_index_[i], distances[i]); + active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(rng, values + active_joint_model_start_index_[i], + *active_joint_bounds[i], + near + active_joint_model_start_index_[i], + distances[i]); updateMimicJoints(values); } @@ -566,8 +568,9 @@ bool JointModelGroup::computeIKIndexBijection(const std::vector& ik // skip reported fixed joints if (hasJointModel(ik_jname) && getJointModel(ik_jname)->getType() == JointModel::FIXED) continue; - ROS_ERROR_NAMED(LOGNAME, "IK solver computes joint values for joint '%s' " - "but group '%s' does not contain such a joint.", + ROS_ERROR_NAMED(LOGNAME, + "IK solver computes joint values for joint '%s' " + "but group '%s' does not contain such a joint.", ik_jname.c_str(), getName().c_str()); return false; } diff --git a/moveit_core/robot_model/src/link_model.cpp b/moveit_core/robot_model/src/link_model.cpp index 76f45e3bb6..f0d8ceb3b6 100644 --- a/moveit_core/robot_model/src/link_model.cpp +++ b/moveit_core/robot_model/src/link_model.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -73,8 +73,7 @@ void LinkModel::setParentJointModel(const JointModel* joint) is_parent_joint_fixed_ = joint->getType() == JointModel::FIXED; } -void LinkModel::setGeometry(const std::vector& shapes, - const EigenSTL::vector_Isometry3d& origins) +void LinkModel::setGeometry(const std::vector& shapes, const EigenSTL::vector_Isometry3d& origins) { shapes_ = shapes; collision_origin_transform_ = origins; diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp index 2727ccd48e..301edd3d65 100644 --- a/moveit_core/robot_model/src/planar_joint_model.cpp +++ b/moveit_core/robot_model/src/planar_joint_model.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/prismatic_joint_model.cpp b/moveit_core/robot_model/src/prismatic_joint_model.cpp index fd5897905e..2f2166edb5 100644 --- a/moveit_core/robot_model/src/prismatic_joint_model.cpp +++ b/moveit_core/robot_model/src/prismatic_joint_model.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index eb59ff8506..f3c066e0ba 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -46,15 +46,7 @@ namespace moveit namespace core { RevoluteJointModel::RevoluteJointModel(const std::string& name) - : JointModel(name) - , axis_(0.0, 0.0, 0.0) - , continuous_(false) - , x2_(0.0) - , y2_(0.0) - , z2_(0.0) - , xy_(0.0) - , xz_(0.0) - , yz_(0.0) + : JointModel(name), axis_(0.0, 0.0, 0.0), continuous_(false), x2_(0.0), y2_(0.0), z2_(0.0), xy_(0.0), xz_(0.0), yz_(0.0) { type_ = REVOLUTE; variable_names_.push_back(name_); diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 68492fb917..b00a1a25e4 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -343,14 +343,16 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) for (std::size_t j = 0; j < vn.size(); ++j) state[vn[j]] = jt->second[j]; else - ROS_ERROR_NAMED(LOGNAME, "The model for joint '%s' requires %d variable values, " - "but only %d variable values were supplied in default state '%s' for group '%s'", + ROS_ERROR_NAMED(LOGNAME, + "The model for joint '%s' requires %d variable values, " + "but only %d variable values were supplied in default state '%s' for group '%s'", jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), group_state.name_.c_str(), jmg->getName().c_str()); } else - ROS_ERROR_NAMED(LOGNAME, "Group state '%s' specifies value for joint '%s', " - "but that joint is not part of group '%s'", + ROS_ERROR_NAMED(LOGNAME, + "Group state '%s' specifies value for joint '%s', " + "but that joint is not part of group '%s'", group_state.name_.c_str(), jt->first.c_str(), jmg->getName().c_str()); } if (!remaining_joints.empty()) @@ -620,8 +622,9 @@ void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model) eef.parent_group_.c_str(), eef.name_.c_str()); } else - ROS_ERROR_NAMED(LOGNAME, "Group '%s' was specified as parent group for end-effector '%s' " - "but it does not include the parent link '%s'", + ROS_ERROR_NAMED(LOGNAME, + "Group '%s' was specified as parent group for end-effector '%s' " + "but it does not include the parent link '%s'", eef.parent_group_.c_str(), eef.name_.c_str(), eef.parent_link_.c_str()); } else @@ -915,8 +918,9 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const { if (virtual_joint.child_link_ != child_link->name) { - ROS_WARN_NAMED(LOGNAME, "Skipping virtual joint '%s' because its child frame '%s' " - "does not match the URDF frame '%s'", + ROS_WARN_NAMED(LOGNAME, + "Skipping virtual joint '%s' because its child frame '%s' " + "does not match the URDF frame '%s'", virtual_joint.name_.c_str(), virtual_joint.child_link_.c_str(), child_link->name.c_str()); } else if (virtual_joint.parent_frame_.empty()) @@ -975,7 +979,7 @@ static inline Eigen::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose) Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q); return af; } -} +} // namespace LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) { diff --git a/moveit_core/robot_model/test/test.cpp b/moveit_core/robot_model/test/test.cpp index 45c56e1c63..e170e3ea60 100644 --- a/moveit_core/robot_model/test/test.cpp +++ b/moveit_core/robot_model/test/test.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index e9ed735251..7cd9bd0dce 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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, Inc. 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h index 95fa387d4e..2eb2c1aaf1 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h +++ b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h @@ -1,38 +1,38 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* Copyright (c) 2019, PickNik LLC. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * Copyright (c) 2019, PickNik LLC. + * 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, Inc. 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. + *********************************************************************/ /* Author: Ioan Sucan, Mike Lautman */ diff --git a/moveit_core/robot_state/include/moveit/robot_state/conversions.h b/moveit_core/robot_state/include/moveit/robot_state/conversions.h index 61c45d1bce..51b79b2a0b 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/conversions.h +++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan, Dave Coleman */ diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 9048a0d5ce..968509fe4b 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -462,8 +462,7 @@ class RobotState /** \brief Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, \e missing_variables is filled with the names of the variables that are not set. */ - void setVariableEffort(const std::map& variable_map, - std::vector& missing_variables); + void setVariableEffort(const std::map& variable_map, std::vector& missing_variables); /** \brief Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. */ void setVariableEffort(const std::vector& variable_names, @@ -1741,8 +1740,7 @@ class RobotState void printStatePositions(std::ostream& out = std::cout) const; /** \brief Output to console the current state of the robot's joint limits */ - void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, - std::ostream& out = std::cout) const; + void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out = std::cout) const; void printStateInfo(std::ostream& out = std::cout) const; diff --git a/moveit_core/robot_state/src/attached_body.cpp b/moveit_core/robot_state/src/attached_body.cpp index ff3fbaa3fb..c5be22ab27 100644 --- a/moveit_core/robot_state/src/attached_body.cpp +++ b/moveit_core/robot_state/src/attached_body.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -98,8 +98,7 @@ void AttachedBody::computeTransform(const Eigen::Isometry3d& parent_link_global_ global_collision_body_transforms_[i] = parent_link_global_transform * attach_trans_[i]; // valid isometry // update subframe transforms - for (auto global = global_subframe_poses_.begin(), end = global_subframe_poses_.end(), - local = subframe_poses_.begin(); + for (auto global = global_subframe_poses_.begin(), end = global_subframe_poses_.end(), local = subframe_poses_.begin(); global != end; ++global, ++local) global->second = parent_link_global_transform * local->second; // valid isometry } diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 5431b490af..3dc237d563 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -1,38 +1,38 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* Copyright (c) 2019, PickNik LLC. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * Copyright (c) 2019, PickNik LLC. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */ @@ -236,8 +236,9 @@ double CartesianInterpolator::checkRelativeJointSpaceJump(const JointModelGroup* { if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH) { - ROS_WARN_NAMED(LOGNAME, "The computed trajectory is too short to detect jumps in joint-space " - "Need at least %zu steps, only got %zu. Try a lower max_step.", + ROS_WARN_NAMED(LOGNAME, + "The computed trajectory is too short to detect jumps in joint-space " + "Need at least %zu steps, only got %zu. Try a lower max_step.", MIN_STEPS_FOR_JUMP_THRESH, traj.size()); } @@ -292,8 +293,9 @@ double CartesianInterpolator::checkAbsoluteJointSpaceJump(const JointModelGroup* joint_threshold = prismatic_threshold; break; default: - ROS_WARN_NAMED(LOGNAME, "Joint %s has not supported type %s. \n" - "checkAbsoluteJointSpaceJump only supports prismatic and revolute joints.", + ROS_WARN_NAMED(LOGNAME, + "Joint %s has not supported type %s. \n" + "checkAbsoluteJointSpaceJump only supports prismatic and revolute joints.", joint->getName().c_str(), joint->getTypeName().c_str()); continue; } diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 6bdcea49c3..1a2722caf1 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2011-2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2011-2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan, Dave Coleman */ @@ -101,8 +101,9 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::MultiDOFJointState& m error = true; if (error) - ROS_WARN_NAMED(LOGNAME, "The transform for multi-dof joints was specified in frame '%s' " - "but it was not possible to transform that to frame '%s'", + ROS_WARN_NAMED(LOGNAME, + "The transform for multi-dof joints was specified in frame '%s' " + "but it was not possible to transform that to frame '%s'", mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str()); } @@ -321,8 +322,9 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::Attached else { t0.setIdentity(); - ROS_ERROR_NAMED(LOGNAME, "Cannot properly transform from frame '%s'. " - "The pose of the attached body may be incorrect", + ROS_ERROR_NAMED(LOGNAME, + "Cannot properly transform from frame '%s'. " + "The pose of the attached body may be incorrect", aco.object.header.frame_id.c_str()); } } @@ -339,8 +341,9 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::Attached else { if (state.clearAttachedBody(aco.object.id)) - ROS_DEBUG_NAMED(LOGNAME, "The robot state already had an object named '%s' attached to link '%s'. " - "The object was replaced.", + ROS_DEBUG_NAMED(LOGNAME, + "The robot state already had an object named '%s' attached to link '%s'. " + "The object was replaced.", aco.object.id.c_str(), aco.link_name.c_str()); state.attachBody(aco.object.id, shapes, poses, aco.touch_links, aco.link_name, aco.detach_posture, subframe_poses); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 1c171487d7..010820cdbc 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */ @@ -83,8 +83,7 @@ RobotState::~RobotState() void RobotState::allocMemory() { - static_assert((sizeof(Eigen::Isometry3d) / EIGEN_MAX_ALIGN_BYTES) * EIGEN_MAX_ALIGN_BYTES == - sizeof(Eigen::Isometry3d), + static_assert((sizeof(Eigen::Isometry3d) / EIGEN_MAX_ALIGN_BYTES) * EIGEN_MAX_ALIGN_BYTES == sizeof(Eigen::Isometry3d), "sizeof(Eigen::Isometry3d) should be a multiple of EIGEN_MAX_ALIGN_BYTES"); constexpr unsigned int extra_alignment_bytes = EIGEN_MAX_ALIGN_BYTES - 1; @@ -885,8 +884,7 @@ void RobotState::interpolate(const RobotState& to, double t, RobotState& state) state.dirty_link_transforms_ = state.robot_model_->getRootJoint(); } -void RobotState::interpolate(const RobotState& to, double t, RobotState& state, - const JointModelGroup* joint_group) const +void RobotState::interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const { const std::vector& jm = joint_group->getActiveJointModels(); for (const JointModel* joint : jm) @@ -947,8 +945,7 @@ void RobotState::getAttachedBodies(std::vector& attached_bo attached_bodies.push_back(it.second); } -void RobotState::getAttachedBodies(std::vector& attached_bodies, - const JointModelGroup* group) const +void RobotState::getAttachedBodies(std::vector& attached_bodies, const JointModelGroup* group) const { attached_bodies.clear(); for (const std::pair& it : attached_body_map_) @@ -1081,8 +1078,9 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c return IDENTITY_TRANSFORM; } if (tf.size() > 1) - ROS_DEBUG_NAMED(LOGNAME, "There are multiple geometries associated to attached body '%s'. " - "Returning the transform for the first one.", + ROS_DEBUG_NAMED(LOGNAME, + "There are multiple geometries associated to attached body '%s'. " + "Returning the transform for the first one.", frame_id.c_str()); robot_link = jt->second->getAttachedLink(); frame_found = true; @@ -1571,8 +1569,9 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is std::vector consistency_limits; if (consistency_limit_sets.size() > 1) { - ROS_ERROR_NAMED(LOGNAME, "Invalid number (%zu) of sets of consistency limits for a setFromIK request " - "that is being solved by a single IK solver", + ROS_ERROR_NAMED(LOGNAME, + "Invalid number (%zu) of sets of consistency limits for a setFromIK request " + "that is being solved by a single IK solver", consistency_limit_sets.size()); return false; } diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index 871c0cc524..3a1f8a8b48 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2018, CITEC Bielefeld -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, CITEC Bielefeld + * 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 the 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. + *********************************************************************/ /* Author: Robert Haschke */ #include diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 5c1d4f5559..50b703fdaa 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan */ #include diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index d41129ef28..8ec7a20eb3 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Martin Pecka */ diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index 5c84fce441..ca771677de 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2019, PickNik LLC. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC. + * 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 the 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. + *********************************************************************/ /* Author: Michael Lautman */ diff --git a/moveit_core/robot_state/test/test_kinematic_complex.cpp b/moveit_core/robot_state/test/test_kinematic_complex.cpp index d0db3f5be9..cc8df316bc 100644 --- a/moveit_core/robot_state/test/test_kinematic_complex.cpp +++ b/moveit_core/robot_state/test/test_kinematic_complex.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 4fa428de01..4130f412be 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -397,8 +397,7 @@ void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& refe moveit::core::RobotStatePtr st(new moveit::core::RobotState(copy)); if (trajectory.joint_trajectory.points.size() > i) { - st->setVariablePositions(trajectory.joint_trajectory.joint_names, - trajectory.joint_trajectory.points[i].positions); + st->setVariablePositions(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].positions); if (!trajectory.joint_trajectory.points[i].velocities.empty()) st->setVariableVelocities(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].velocities); diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index 208b16e813..092c9a54f4 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -64,8 +64,8 @@ class RobotTrajectoryTestFixture : public testing::Test void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) { // Init a traj - ASSERT_TRUE(robot_model_->hasJointModelGroup(arm_jmg_name_)) << "Robot model does not have group: " - << arm_jmg_name_; + ASSERT_TRUE(robot_model_->hasJointModelGroup(arm_jmg_name_)) + << "Robot model does not have group: " << arm_jmg_name_; trajectory = std::make_shared(robot_model_, arm_jmg_name_); diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h index 4c818af3e8..bd49dee3c4 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h @@ -42,4 +42,4 @@ namespace trajectory_processing { bool isTrajectoryEmpty(const moveit_msgs::RobotTrajectory& trajectory); std::size_t trajectoryWaypointCount(const moveit_msgs::RobotTrajectory& trajectory); -} +} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp index 5d5adf073e..f12d01314d 100644 --- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp @@ -46,8 +46,7 @@ namespace trajectory_processing static void fit_cubic_spline(const int n, const double dt[], const double x[], double x1[], double x2[]); static void adjust_two_positions(const int n, const double dt[], double x[], double x1[], double x2[], const double x2_i, const double x2_f); -static void init_times(const int n, double dt[], const double x[], const double max_velocity, - const double min_velocity); +static void init_times(const int n, double dt[], const double x[], const double max_velocity, const double min_velocity); static int fit_spline_and_adjust_times(const int n, double dt[], const double x[], double x1[], double x2[], const double max_velocity, const double min_velocity, const double max_acceleration, const double min_acceleration, @@ -300,8 +299,7 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT &t2[j].accelerations_[0], t2[j].initial_acceleration_, t2[j].final_acceleration_); } - fit_cubic_spline(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], - &t2[j].accelerations_[0]); + fit_cubic_spline(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], &t2[j].accelerations_[0]); for (unsigned i = 0; i < num_points; i++) { const double acc = t2[j].accelerations_[i]; diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index e0c0d3be41..a873becbbb 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -435,9 +435,8 @@ bool Trajectory::getNextAccelerationSwitchingPoint(double path_pos, TrajectorySt if ((before_path_vel > after_path_vel || getMinMaxPhaseSlope(switching_path_pos - EPS, switching_path_vel, false) > getAccelerationMaxPathVelocityDeriv(switching_path_pos - 2.0 * EPS)) && - (before_path_vel < after_path_vel || - getMinMaxPhaseSlope(switching_path_pos + EPS, switching_path_vel, true) < - getAccelerationMaxPathVelocityDeriv(switching_path_pos + 2.0 * EPS))) + (before_path_vel < after_path_vel || getMinMaxPhaseSlope(switching_path_pos + EPS, switching_path_vel, true) < + getAccelerationMaxPathVelocityDeriv(switching_path_pos + 2.0 * EPS))) { break; } @@ -477,9 +476,8 @@ bool Trajectory::getNextVelocitySwitchingPoint(double path_pos, TrajectoryStep& { start = true; } - } while ((!start || - getMinMaxPhaseSlope(path_pos, getVelocityMaxPathVelocity(path_pos), false) > - getVelocityMaxPathVelocityDeriv(path_pos)) && + } while ((!start || getMinMaxPhaseSlope(path_pos, getVelocityMaxPathVelocity(path_pos), false) > + getVelocityMaxPathVelocityDeriv(path_pos)) && path_pos < path_.getLength()); if (path_pos >= path_.getLength()) diff --git a/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h b/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h index 839256bfb2..5b9c161a4d 100644 --- a/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h +++ b/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h @@ -51,8 +51,7 @@ double parseDouble(XmlRpc::XmlRpcValue& v); * @param name: if non-empty, print a warning message "name is not an array[size]" * @param description: if non-empty, serves as a descriptor for array items */ -bool isArray(XmlRpc::XmlRpcValue& v, size_t size = 0, const std::string& name = "", - const std::string& description = ""); +bool isArray(XmlRpc::XmlRpcValue& v, size_t size = 0, const std::string& name = "", const std::string& description = ""); /** check that v is a struct with given keys * diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index e8bf5d3407..8f988e14f0 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -320,8 +320,7 @@ void RobotModelBuilder::addVirtualJoint(const std::string& parent_frame, const s srdf_writer_->virtual_joints_.push_back(new_virtual_joint); } -void RobotModelBuilder::addGroupChain(const std::string& base_link, const std::string& tip_link, - const std::string& name) +void RobotModelBuilder::addGroupChain(const std::string& base_link, const std::string& tip_link, const std::string& name) { srdf::Model::Group new_group; if (name.empty()) diff --git a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_distance_field_ros_helpers.h b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_distance_field_ros_helpers.h index 0495d74d2a..8de0d3970d 100644 --- a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_distance_field_ros_helpers.h +++ b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_distance_field_ros_helpers.h @@ -124,4 +124,4 @@ static inline bool loadLinkBodySphereDecompositions( } return true; } -} +} // namespace collision_detection diff --git a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_robot_distance_field_ros.h b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_robot_distance_field_ros.h index 1fb7a58e56..9167d98d62 100644 --- a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_robot_distance_field_ros.h +++ b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_robot_distance_field_ros.h @@ -61,4 +61,4 @@ class CollisionRobotDistanceFieldROS : public CollisionRobotDistanceField max_propogation_distance); } }; -} +} // namespace collision_detection diff --git a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/hybrid_collision_robot_ros.h b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/hybrid_collision_robot_ros.h index 8cb1c75f98..ccedfc2027 100644 --- a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/hybrid_collision_robot_ros.h +++ b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/hybrid_collision_robot_ros.h @@ -61,4 +61,4 @@ class CollisionRobotHybridROS : public CollisionRobotHybrid collision_tolerance, max_propogation_distance); } }; -} +} // namespace collision_detection diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache/include/kinematics_cache/kinematics_cache.h b/moveit_experimental/kinematics_cache/v1/kinematics_cache/include/kinematics_cache/kinematics_cache.h index 952965a57e..763b738ab3 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache/include/kinematics_cache/kinematics_cache.h +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache/include/kinematics_cache/kinematics_cache.h @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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, Inc. 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. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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, Inc. 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. + * + * Author: Sachin Chitta + *********************************************************************/ #pragma once @@ -182,4 +182,4 @@ class KinematicsCache }; typedef std::shared_ptr KinematicsCachePtr; -} +} // namespace kinematics_cache diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache/src/kinematics_cache.cpp b/moveit_experimental/kinematics_cache/v1/kinematics_cache/src/kinematics_cache.cpp index 68c443c62a..f6950a1f4e 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache/src/kinematics_cache.cpp +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache/src/kinematics_cache.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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, Inc. 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. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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, Inc. 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. + * + * Author: Sachin Chitta + *********************************************************************/ #include #include @@ -372,4 +372,4 @@ void KinematicsCache::updateDistances(const geometry_msgs::Pose& pose) min_squared_distance_ = std::min(distance_squared, min_squared_distance_); max_squared_distance_ = std::max(distance_squared, max_squared_distance_); } -} +} // namespace kinematics_cache diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/include/kinematics_cache_ros/kinematics_cache_ros.h b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/include/kinematics_cache_ros/kinematics_cache_ros.h index 187c133f40..bf8e35b420 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/include/kinematics_cache_ros/kinematics_cache_ros.h +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/include/kinematics_cache_ros/kinematics_cache_ros.h @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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, Inc. 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. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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, Inc. 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. + * + * Author: Sachin Chitta + *********************************************************************/ #pragma once @@ -68,4 +68,4 @@ class KinematicsCacheROS : public kinematics_cache::KinematicsCache planning_models::RobotModelPtr kinematic_model_; /** A kinematics model */ }; -} +} // namespace kinematics_cache_ros diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/example.cpp b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/example.cpp index 98d4573c27..089ad3fd0f 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/example.cpp +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/example.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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, Inc. 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. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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, Inc. 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. + * + * Author: Sachin Chitta + *********************************************************************/ #include #include diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/kinematics_cache_ros.cpp b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/kinematics_cache_ros.cpp index edc2f0cb64..7179b6c4cf 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/kinematics_cache_ros.cpp +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/kinematics_cache_ros.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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, Inc. 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. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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, Inc. 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. + * + * Author: Sachin Chitta + *********************************************************************/ #include @@ -85,4 +85,4 @@ bool KinematicsCacheROS::init(const kinematics_cache::KinematicsCache::Options& return true; } -} +} // namespace kinematics_cache_ros diff --git a/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h b/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h index 5a5f72579d..c6b4b6116e 100644 --- a/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h +++ b/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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, Inc. 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. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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, Inc. 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. + * + * Author: Sachin Chitta + *********************************************************************/ #pragma once @@ -180,4 +180,4 @@ class KinematicsCache }; typedef std::shared_ptr KinematicsCachePtr; -} +} // namespace kinematics_cache diff --git a/moveit_experimental/kinematics_cache/v2/kinematics_cache/src/kinematics_cache.cpp b/moveit_experimental/kinematics_cache/v2/kinematics_cache/src/kinematics_cache.cpp index a332d31b2b..7f30439919 100644 --- a/moveit_experimental/kinematics_cache/v2/kinematics_cache/src/kinematics_cache.cpp +++ b/moveit_experimental/kinematics_cache/v2/kinematics_cache/src/kinematics_cache.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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, Inc. 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. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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, Inc. 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. + * + * Author: Sachin Chitta + *********************************************************************/ #include #include @@ -380,4 +380,4 @@ void KinematicsCache::updateDistances(const geometry_msgs::Pose& pose) min_squared_distance_ = std::min(distance_squared, min_squared_distance_); max_squared_distance_ = std::max(distance_squared, max_squared_distance_); } -} +} // namespace kinematics_cache diff --git a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h index 5117918b15..d64788df59 100644 --- a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h +++ b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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, Inc. 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. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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, Inc. 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. + * + * Author: Sachin Chitta + *********************************************************************/ #pragma once @@ -139,4 +139,4 @@ class KinematicsConstraintAware unsigned int ik_attempts_; }; -} +} // namespace kinematics_constraint_aware diff --git a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h index 87be80bcf5..3ca0c75c7e 100644 --- a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h +++ b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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, Inc. 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. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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, Inc. 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. + * + * Author: Sachin Chitta + *********************************************************************/ #pragma once @@ -103,4 +103,4 @@ class KinematicsResponse bool result_; }; -} +} // namespace kinematics_constraint_aware diff --git a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp index 5e25a7def2..141df0748a 100644 --- a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp +++ b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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, Inc. 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. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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, Inc. 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. + * + * Author: Sachin Chitta + *********************************************************************/ // // ROS msgs #include @@ -147,8 +147,7 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt ik_link_names[i] = kinematic_model_->getJointModelGroup(sub_groups_names_[i])->getSolverInstance()->getTipFrame(); } - else if (!kinematic_model_->getJointModelGroup(sub_groups_names_[i]) - ->canSetStateFromIK(request.ik_link_names_[i])) + else if (!kinematic_model_->getJointModelGroup(sub_groups_names_[i])->canSetStateFromIK(request.ik_link_names_[i])) { ROS_ERROR_NAMED("kinematics_constraint_aware", "Could not find IK solver for link %s for group %s", request.ik_link_names_[i].c_str(), sub_groups_names_[i].c_str()); @@ -372,4 +371,4 @@ geometry_msgs::Pose KinematicsConstraintAware::getTipFramePose( result = tf2::toMsg(eigen_pose_in, result); return result; } -} +} // namespace kinematics_constraint_aware diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h index 9038da5a55..5f09634dcd 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h @@ -200,7 +200,7 @@ template struct HasRobotDescApi().KinematicsPlugin::initialize( std::string(), std::string(), std::string(), std::vector(), 0.0))> - : std::true_type + : std::true_type { }; diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h index 090c50057f..927554efad 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h @@ -352,8 +352,7 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> { node_dist = node_queue.top(); node_queue.pop(); - if (node_dist.second > node_dist.first->maxRadius_ + dist || - node_dist.second < node_dist.first->minRadius_ - dist) + if (node_dist.second > node_dist.first->maxRadius_ + dist || node_dist.second < node_dist.first->minRadius_ - dist) continue; node_dist.first->nearestR(*this, data, radius, nbhQueue, node_queue); } @@ -615,8 +614,9 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> dist_to_pivot[i] = gnat.distFun_(data, child->pivot_); insertNeighborR(nbh, r, child->pivot_, dist_to_pivot[i]); for (unsigned int j = 0; j < children_.size(); ++j) - if (permutation[j] >= 0 && i != j && (dist_to_pivot[i] - dist > child->maxRange_[permutation[j]] || - dist_to_pivot[i] + dist < child->minRange_[permutation[j]])) + if (permutation[j] >= 0 && i != j && + (dist_to_pivot[i] - dist > child->maxRange_[permutation[j]] || + dist_to_pivot[i] + dist < child->minRange_[permutation[j]])) permutation[j] = -1; } diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp b/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp index 1f75dc5fa3..d565018280 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp +++ b/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp @@ -42,18 +42,16 @@ #include // register CachedIKKinematicsPlugin as a KinematicsBase implementation -PLUGINLIB_EXPORT_CLASS( - cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, - kinematics::KinematicsBase); +PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, + kinematics::KinematicsBase); // register CachedIKKinematicsPlugin as a KinematicsBase implementation // PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, // kinematics::KinematicsBase); // register CachedIKKinematicsPlugin as a KinematicsBase implementation -PLUGINLIB_EXPORT_CLASS( - cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, - kinematics::KinematicsBase); +PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, + kinematics::KinematicsBase); #ifdef CACHED_IK_KINEMATICS_TRAC_IK #include diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h index e672c974af..0728329205 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h @@ -304,7 +304,7 @@ class IkSolutionList : public IkSolutionListBase protected: std::list > _listsolutions; }; -} +} // namespace ikfast #endif // OPENRAVE_IKFAST_HEADER diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index c2745fe788..5b7370c832 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -215,10 +215,10 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase */ // Returns the IK solution that is within joint limits closest to ik_seed_state - 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 override; + 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 override; /** * @brief Given a desired pose of the end-effector, compute the set joint angles solutions that are able to reach it. @@ -354,22 +354,22 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase bool getCount(int& count, const int& max_count, const int& min_count) const; /** - * @brief samples the designated redundant joint using the chosen discretization method - * @param method An enumeration flag indicating the discretization method to be used - * @param sampled_joint_vals Sampled joint values for the redundant joint - * @return True if sampling succeeded. - */ + * @brief samples the designated redundant joint using the chosen discretization method + * @param method An enumeration flag indicating the discretization method to be used + * @param sampled_joint_vals Sampled joint values for the redundant joint + * @return True if sampling succeeded. + */ bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector& sampled_joint_vals) const; /// Validate that we can compute a fixed transform between from and to links. bool computeRelativeTransform(const std::string& from, const std::string& to, Eigen::Isometry3d& transform, bool& differs_from_identity); /** - * @brief Transforms the input pose to the correct frame for the solver. This assumes that the group includes the - * entire solver chain and that any joints outside of the solver chain within the group are are fixed. - * @param ik_pose The pose to be transformed which should be in the correct frame for the group. - * @param ik_pose_chain The ik_pose to be populated with the apropriate pose for the solver - */ + * @brief Transforms the input pose to the correct frame for the solver. This assumes that the group includes the + * entire solver chain and that any joints outside of the solver chain within the group are are fixed. + * @param ik_pose The pose to be transformed which should be in the correct frame for the group. + * @param ik_pose_chain The ik_pose to be populated with the apropriate pose for the solver + */ void transformToChainFrame(const geometry_msgs::Pose& ik_pose, KDL::Frame& ik_pose_chain) const; }; // end class @@ -925,8 +925,8 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose if (ik_seed_state.size() != num_joints_) { - ROS_ERROR_STREAM_NAMED(name_, "Seed state must have size " << num_joints_ << " instead of size " - << ik_seed_state.size()); + ROS_ERROR_STREAM_NAMED(name_, + "Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } @@ -1395,7 +1395,7 @@ void IKFastKinematicsPlugin::transformToChainFrame(const geometry_msgs::Pose& ik } } -} // end namespace +} // namespace _NAMESPACE_ // register IKFastKinematicsPlugin as a KinematicsBase implementation #include diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h index a94944a989..14d1f4b6fb 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h @@ -77,10 +77,10 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase */ KDLKinematicsPlugin(); - 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 override; + 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 override; bool searchPositionIK( const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp index a0bafdd4af..e35e535fb3 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp @@ -128,10 +128,10 @@ void KDLKinematicsPlugin::getJointWeights() else joint_weights_[it - active_names.begin()] = weights[i]; } - ROS_INFO_STREAM_NAMED("kdl", "Joint weights for group '" - << getGroupName() << "': \n" - << Eigen::Map(joint_weights_.data(), joint_weights_.size()) - .transpose()); + ROS_INFO_STREAM_NAMED( + "kdl", "Joint weights for group '" + << getGroupName() << "': \n" + << Eigen::Map(joint_weights_.data(), joint_weights_.size()).transpose()); } bool KDLKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name, @@ -329,8 +329,8 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c if (ik_seed_state.size() != dimension_) { - ROS_ERROR_STREAM_NAMED("kdl", "Seed state must have size " << dimension_ << " instead of size " - << ik_seed_state.size()); + ROS_ERROR_STREAM_NAMED("kdl", + "Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h index 85bf238327..84787e93be 100644 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h +++ b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h @@ -71,10 +71,10 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase */ LMAKinematicsPlugin(); - 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 override; + 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 override; bool searchPositionIK( const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, diff --git a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp index dd0fd88e9f..a55eb7dd36 100644 --- a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp +++ b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp @@ -221,8 +221,8 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c if (ik_seed_state.size() != dimension_) { - ROS_ERROR_STREAM_NAMED("lma", "Seed state must have size " << dimension_ << " instead of size " - << ik_seed_state.size()); + ROS_ERROR_STREAM_NAMED("lma", + "Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } diff --git a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h index 8d49435417..4c1005c42b 100644 --- a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h +++ b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h @@ -70,10 +70,10 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase */ SrvKinematicsPlugin(); - 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 override; + 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 override; bool searchPositionIK( const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, diff --git a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp index f93ed4c202..a563048a51 100644 --- a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp +++ b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp @@ -241,8 +241,8 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vectoroptimize(); // replan with updated parameters if no solution is found if (params_nonconst.enable_failure_recovery_) { - ROS_INFO_NAMED("chomp_planner", "Planned with Chomp Parameters (learning_rate, ridge_factor, " - "planning_time_limit, max_iterations), attempt: # %d ", + ROS_INFO_NAMED("chomp_planner", + "Planned with Chomp Parameters (learning_rate, ridge_factor, " + "planning_time_limit, max_iterations), attempt: # %d ", (replan_count + 1)); ROS_INFO_NAMED("chomp_planner", "Learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ", params_nonconst.learning_rate_, params_nonconst.ridge_factor_, diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp index 4467dbfced..af2c9c9c04 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp @@ -59,8 +59,7 @@ ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_m init(); } -ChompTrajectory::ChompTrajectory(const ChompTrajectory& source_traj, const std::string& group_name, - int diff_rule_length) +ChompTrajectory::ChompTrajectory(const ChompTrajectory& source_traj, const std::string& group_name, int diff_rule_length) : planning_group_name_(group_name), discretization_(source_traj.discretization_) { num_joints_ = source_traj.getNumJoints(); diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp index f57e080bf2..5a9a9d5405 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp +++ b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp @@ -111,8 +111,7 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter if (!nh.getParam("smoothness_cost_jerk", params_.smoothness_cost_jerk_)) { params_.smoothness_cost_jerk_ = 0.0; - ROS_INFO_STREAM( - "Param smoothness_cost_jerk_ was not set. Using default value: " << params_.smoothness_cost_jerk_); + ROS_INFO_STREAM("Param smoothness_cost_jerk_ was not set. Using default value: " << params_.smoothness_cost_jerk_); } if (!nh.getParam("ridge_factor", params_.ridge_factor_)) { diff --git a/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h b/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h index 4e85adddb2..3814d0356f 100644 --- a/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h +++ b/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h @@ -530,4 +530,4 @@ inline const OMPLDynamicReconfigureConfigStatics* OMPLDynamicReconfigureConfig:: return statics; } -} +} // namespace ompl_interface_ros diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h index be711c991f..8dea8329f3 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h index 884b82e4e9..d0f1d4951d 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h index 7b73f60e0a..e9b4413028 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h index ccf47358ae..132e45b778 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -73,4 +73,4 @@ struct ConstraintApproximation }; MOVEIT_DECLARE_PTR(ConstraintApproximations, std::vector) -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h index 46b97ba692..85c278bff5 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h index a0daa1cf6b..62f1529c35 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h index c612d22454..aad7b06f76 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h index 3d3e31a92f..54259e9d8e 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h index e0a5e6dc7c..ab6d38fc66 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index 64918929d7..8f6522026f 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h index 5677eea4a0..0bbfb4a562 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h index 7386856fc6..d0e3830294 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h index 071a3f262d..5adb82bb44 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h index e6c34ebba9..82b5f48ee6 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index 594d3ba9dc..3912b8a3a3 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index 19bdc77dde..2e8b51547c 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -277,8 +277,9 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: std::ifstream fin((path + "/manifest").c_str()); if (!fin.good()) { - ROS_WARN_NAMED(LOGNAME, "Manifest not found in folder '%s'. Not loading " - "constraint approximations.", + ROS_WARN_NAMED(LOGNAME, + "Manifest not found in folder '%s'. Not loading " + "constraint approximations.", path.c_str()); return; } @@ -310,14 +311,16 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: if (context_->getGroupName() != group && context_->getOMPLStateSpace()->getParameterizationType() != state_space_parameterization) { - ROS_INFO_NAMED(LOGNAME, "Ignoring constraint approximation of type '%s' " - "for group '%s' from '%s'...", + ROS_INFO_NAMED(LOGNAME, + "Ignoring constraint approximation of type '%s' " + "for group '%s' from '%s'...", state_space_parameterization.c_str(), group.c_str(), filename.c_str()); continue; } - ROS_INFO_NAMED(LOGNAME, "Loading constraint approximation of type '%s' for " - "group '%s' from '%s'...", + ROS_INFO_NAMED(LOGNAME, + "Loading constraint approximation of type '%s' for " + "group '%s' from '%s'...", state_space_parameterization.c_str(), group.c_str(), filename.c_str()); moveit_msgs::Constraints msg; hexToMsg(serialization, msg); @@ -332,9 +335,10 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: std::size_t sum = 0; for (std::size_t i = 0; i < cass->size(); ++i) sum += cass->getMetadata(i).first.size(); - ROS_INFO_NAMED(LOGNAME, "Loaded %lu states (%lu milestones) and %lu " - "connections (%0.1lf per state) " - "for constraint named '%s'%s", + ROS_INFO_NAMED(LOGNAME, + "Loaded %lu states (%lu milestones) and %lu " + "connections (%0.1lf per state) " + "for constraint named '%s'%s", cass->size(), cap->getMilestoneCount(), sum, (double)sum / (double)cap->getMilestoneCount(), msg.name.c_str(), explicit_motions ? ". Explicit motions included." : ""); } @@ -405,18 +409,20 @@ ompl_interface::ConstraintsLibrary::getConstraintApproximation(const moveit_msgs } ompl_interface::ConstraintApproximationConstructionResults -ompl_interface::ConstraintsLibrary::addConstraintApproximation( - const moveit_msgs::Constraints& constr, const std::string& group, - const planning_scene::PlanningSceneConstPtr& scene, const ConstraintApproximationConstructionOptions& options) +ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::Constraints& constr, + const std::string& group, + const planning_scene::PlanningSceneConstPtr& scene, + const ConstraintApproximationConstructionOptions& options) { return addConstraintApproximation(constr, constr, group, scene, options); } ompl_interface::ConstraintApproximationConstructionResults -ompl_interface::ConstraintsLibrary::addConstraintApproximation( - const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard, - const std::string& group, const planning_scene::PlanningSceneConstPtr& scene, - const ConstraintApproximationConstructionOptions& options) +ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::Constraints& constr_sampling, + const moveit_msgs::Constraints& constr_hard, + const std::string& group, + const planning_scene::PlanningSceneConstPtr& scene, + const ConstraintApproximationConstructionOptions& options) { ConstraintApproximationConstructionResults res; if (context_->getGroupName() != group && diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 8040d8de39..1a37096d9f 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -104,8 +104,7 @@ ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std:: constraints_library_ = std::make_shared(this); } -void ompl_interface::ModelBasedPlanningContext::configure(const ros::NodeHandle& nh, - bool use_constraints_approximations) +void ompl_interface::ModelBasedPlanningContext::configure(const ros::NodeHandle& nh, bool use_constraints_approximations) { loadConstraintApproximations(nh); if (!use_constraints_approximations) @@ -160,8 +159,9 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str if (getRobotModel()->hasLinkModel(link_name)) return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorLinkPose(this, link_name)); else - ROS_ERROR_NAMED(LOGNAME, "Attempted to set projection evaluator with respect to position of link '%s', " - "but that link is not known to the kinematic model.", + ROS_ERROR_NAMED(LOGNAME, + "Attempted to set projection evaluator with respect to position of link '%s', " + "but that link is not known to the kinematic model.", link_name.c_str()); } else if (peval.find_first_of("joints(") == 0 && peval[peval.length() - 1] == ')') @@ -188,8 +188,9 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str joint.c_str()); } else - ROS_ERROR_NAMED(LOGNAME, "%s: Attempted to set projection evaluator with respect to value of joint " - "'%s', but that joint is not known to the group '%s'.", + ROS_ERROR_NAMED(LOGNAME, + "%s: Attempted to set projection evaluator with respect to value of joint " + "'%s', but that joint is not known to the group '%s'.", name_.c_str(), joint.c_str(), getGroupName().c_str()); } if (j.empty()) @@ -366,8 +367,9 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() const std::string planner_name = getGroupName() + "/" + name_; ompl_simple_setup_->setPlannerAllocator( std::bind(spec_.planner_selector_(type), std::placeholders::_1, planner_name, std::cref(spec_))); - ROS_INFO_NAMED(LOGNAME, "Planner configuration '%s' will use planner '%s'. " - "Additional configuration parameters will be set when the planner is constructed.", + ROS_INFO_NAMED(LOGNAME, + "Planner configuration '%s' will use planner '%s'. " + "Additional configuration parameters will be set when the planner is constructed.", name_.c_str(), type.c_str()); } @@ -385,8 +387,9 @@ void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_m wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0) ROS_WARN_NAMED(LOGNAME, "It looks like the planning volume was not specified."); - ROS_DEBUG_NAMED(LOGNAME, "%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = " - "[%f, %f], z = [%f, %f]", + ROS_DEBUG_NAMED(LOGNAME, + "%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = " + "[%f, %f], z = [%f, %f]", name_.c_str(), wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y, wparams.max_corner.y, wparams.min_corner.z, wparams.max_corner.z); @@ -479,8 +482,9 @@ ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal() return ob::GoalPtr(); } -ompl::base::PlannerTerminationCondition ompl_interface::ModelBasedPlanningContext::constructPlannerTerminationCondition( - double timeout, const ompl::time::point& start) +ompl::base::PlannerTerminationCondition +ompl_interface::ModelBasedPlanningContext::constructPlannerTerminationCondition(double timeout, + const ompl::time::point& start) { auto it = spec_.config_.find("termination_condition"); if (it == spec_.config_.end()) diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index 242afc0f91..97d239af3f 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -96,8 +96,9 @@ void ompl_interface::OMPLInterface::setPlannerConfigurations(const planning_inte context_manager_.setPlannerConfigurations(pconfig2); } -ompl_interface::ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::getPlanningContext( - const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req) const +ompl_interface::ModelBasedPlanningContextPtr +ompl_interface::OMPLInterface::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req) const { moveit_msgs::MoveItErrorCodes dummy; return getPlanningContext(planning_scene, req, dummy); @@ -246,8 +247,9 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() { if (config_names.getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_ERROR_NAMED(LOGNAME, "The planner_configs argument of a group configuration " - "should be an array of strings (for group '%s')", + ROS_ERROR_NAMED(LOGNAME, + "The planner_configs argument of a group configuration " + "should be an array of strings (for group '%s')", group_name.c_str()); continue; } diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space.cpp index 476a90684d..8182da68f5 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp index 2c5952640c..a351fcdf65 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp index 3500d4e18b..7c19e05029 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp @@ -90,8 +90,9 @@ double ompl_interface::ModelBasedStateSpace::getTagSnapToSegment() const void ompl_interface::ModelBasedStateSpace::setTagSnapToSegment(double snap) { if (snap < 0.0 || snap > 1.0) - ROS_WARN_NAMED(LOGNAME, "Snap to segment for tags is a ratio. It's value must be between 0.0 and 1.0. " - "Value remains as previously set (%lf)", + ROS_WARN_NAMED(LOGNAME, + "Snap to segment for tags is a ratio. It's value must be between 0.0 and 1.0. " + "Value remains as previously set (%lf)", tag_snap_to_segment_); else { @@ -136,8 +137,7 @@ void ompl_interface::ModelBasedStateSpace::serialize(void* serialization, const void ompl_interface::ModelBasedStateSpace::deserialize(ompl::base::State* state, const void* serialization) const { state->as()->tag = *reinterpret_cast(serialization); - memcpy(state->as()->values, reinterpret_cast(serialization) + sizeof(int), - state_values_size_); + memcpy(state->as()->values, reinterpret_cast(serialization) + sizeof(int), state_values_size_); } unsigned int ompl_interface::ModelBasedStateSpace::getDimension() const diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp index de23960b62..1a3158654b 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 44b15cee8b..75f870a493 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -367,8 +367,9 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana return context; } -const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningContextManager::getStateSpaceFactory1( - const std::string& /* dummy */, const std::string& factory_type) const +const ompl_interface::ModelBasedStateSpaceFactoryPtr& +ompl_interface::PlanningContextManager::getStateSpaceFactory1(const std::string& /* dummy */, + const std::string& factory_type) const { auto f = factory_type.empty() ? state_space_factories_.begin() : state_space_factories_.find(factory_type); if (f != state_space_factories_.end()) @@ -381,8 +382,9 @@ const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningCo } } -const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningContextManager::getStateSpaceFactory2( - const std::string& group, const moveit_msgs::MotionPlanRequest& req) const +const ompl_interface::ModelBasedStateSpaceFactoryPtr& +ompl_interface::PlanningContextManager::getStateSpaceFactory2(const std::string& group, + const moveit_msgs::MotionPlanRequest& req) const { // find the problem representation to use auto best = state_space_factories_.end(); diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h index cf5700adf4..d37da370c1 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h @@ -75,4 +75,4 @@ class BFS_3D int getDistance(int, int, int); }; -} +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h index 53b258ced3..b27e3a7617 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h @@ -330,4 +330,4 @@ inline void EnvironmentChain3D::convertJointAnglesToCoord(const std::vector motion_primitive_type_names_; }; -} +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp index 29f81140c1..e8b7ab9113 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp @@ -145,4 +145,4 @@ int BFS_3D::getDistance(int x, int y, int z) ; return distance_grid[node]; } -} +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/Search.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/Search.cpp index 9acff406cf..e485e5bf13 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/Search.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/Search.cpp @@ -83,4 +83,4 @@ void BFS_3D::search(int width, int planeSize, int volatile* distance_grid, int* // std::cerr << "Search thread done" << std::endl; running = false; } -} +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/bresenham.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/bresenham.cpp index 7d67420b2f..37712556ed 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/bresenham.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/bresenham.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Maxim Likhachev -* 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 Maxim Likhachev 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Maxim Likhachev + * 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 Maxim Likhachev 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. + *********************************************************************/ /* Author: Benjamin Cohen */ diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp index 5ff2829e1f..0db1d39420 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Maxim Likhachev -* 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 Maxim Likhachev 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Maxim Likhachev + * 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 Maxim Likhachev 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. + *********************************************************************/ /* Author: Benjamin Cohen, E. Gil Jones */ @@ -1214,4 +1214,4 @@ void EnvironmentChain3D::attemptShortcut(const trajectory_msgs::JointTrajectory& } } } -} +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp index 134e816f99..e82bad8793 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp @@ -117,4 +117,4 @@ bool SBPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_ (const_cast(this))->last_planning_statistics_ = stats; return true; } -} +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp index 31cc629039..e4411ea367 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp @@ -170,4 +170,4 @@ void SBPLMetaInterface::runSolver(bool use_first, const planning_scene::Planning std::cerr << "Interruption requested\n"; } } -} +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_meta_plugin.cpp b/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_meta_plugin.cpp index d522429d71..3cd3444134 100644 --- a/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_meta_plugin.cpp +++ b/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_meta_plugin.cpp @@ -106,6 +106,6 @@ class SBPLMetaPlanner : public planning_interface::Planner boost::shared_ptr sbpl_meta_interface_; }; -} // ompl_interface_ros +} // namespace sbpl_interface_ros PLUGINLIB_EXPORT_CLASS(sbpl_interface_ros::SBPLMetaPlanner, planning_interface::Planner); diff --git a/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_plugin.cpp b/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_plugin.cpp index e74d4fcdb4..d7e8de4de3 100644 --- a/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_plugin.cpp +++ b/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_plugin.cpp @@ -111,6 +111,6 @@ class SBPLPlanner : public planning_interface::Planner boost::shared_ptr sbpl_interface_; }; -} // ompl_interface_ros +} // namespace sbpl_interface_ros PLUGINLIB_EXPORT_CLASS(sbpl_interface_ros::SBPLPlanner, planning_interface::Planner); diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h index 74432fa83c..a6a1c18631 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h +++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h @@ -74,4 +74,4 @@ class TrajOptInterface }; void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res); -} +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/src/problem_description.cpp b/moveit_planners/trajopt/src/problem_description.cpp index 59c5de2198..c5d263bd99 100644 --- a/moveit_planners/trajopt/src/problem_description.cpp +++ b/moveit_planners/trajopt/src/problem_description.cpp @@ -247,9 +247,9 @@ TrajOptProblemPtr ConstructProblem(const ProblemInfo& pci) for (int i = 1; i < prob->GetNumSteps(); ++i) { matrix_traj_vars_temp = prob->GetVars(); - prob->addLinearConstraint( - sco::exprSub(sco::AffExpr(matrix_traj_vars_temp(i, dof_ind)), sco::AffExpr(init_traj(0, dof_ind))), - sco::EQ); + prob->addLinearConstraint(sco::exprSub(sco::AffExpr(matrix_traj_vars_temp(i, dof_ind)), + sco::AffExpr(init_traj(0, dof_ind))), + sco::EQ); } } } @@ -596,4 +596,4 @@ void generateInitialTrajectory(const ProblemInfo& pci, const std::vector Eigen::VectorXd::Constant(init_traj.rows(), init_info.dt); } } -} +} // namespace trajopt_interface diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp index 826337d386..9dc5217946 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp @@ -95,8 +95,8 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont if (controller_list[i]["joints"].getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_ERROR_STREAM_NAMED("MoveItFakeControllerManager", "The list of joints for controller " - << name << " is not specified as an array"); + ROS_ERROR_STREAM_NAMED("MoveItFakeControllerManager", + "The list of joints for controller " << name << " is not specified as an array"); continue; } std::vector joints; diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp index e1e0232e5d..b32583b11b 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp @@ -308,8 +308,7 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll resources_bimap claimed_resources; // fill bimap with active controllers and their resources - for (std::pair& active_controller : - active_controllers_) + for (std::pair& active_controller : active_controllers_) { for (std::vector::iterator hir = active_controller.second.claimed_resources.begin(); diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h index 4675b1c332..c6bc9e0f74 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h @@ -47,7 +47,7 @@ namespace moveit_simple_controller_manager * or anything using a control_mgs/FollowJointTrajectoryAction. */ class FollowJointTrajectoryControllerHandle - : public ActionBasedControllerHandle + : public ActionBasedControllerHandle { public: FollowJointTrajectoryControllerHandle(const std::string& name, const std::string& action_ns) diff --git a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp index d5acff96b5..31cf634780 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp @@ -179,8 +179,9 @@ void FollowJointTrajectoryControllerHandle::configure(XmlRpc::XmlRpcValue& confi ROS_WARN_STREAM_NAMED(LOGNAME, "Invalid " << config_name); } -control_msgs::JointTolerance& FollowJointTrajectoryControllerHandle::getTolerance( - std::vector& tolerances, const std::string& name) +control_msgs::JointTolerance& +FollowJointTrajectoryControllerHandle::getTolerance(std::vector& tolerances, + const std::string& name) { auto it = std::lower_bound(tolerances.begin(), tolerances.end(), name, diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp index 83d8a6f7b3..67d600dd16 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp @@ -86,8 +86,8 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo if (!isArray(controller_list[i]["joints"])) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "The list of joints for controller " << name - << " is not specified as an array"); + ROS_ERROR_STREAM_NAMED(LOGNAME, + "The list of joints for controller " << name << " is not specified as an array"); continue; } @@ -113,8 +113,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo static_cast(new_handle.get()) ->setCommandJoint(controller_list[i]["command_joint"]); else - static_cast(new_handle.get()) - ->setCommandJoint(controller_list[i]["joints"][0]); + static_cast(new_handle.get())->setCommandJoint(controller_list[i]["joints"][0]); } if (controller_list[i].hasMember("allow_failure")) @@ -219,8 +218,9 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo } else { - ROS_WARN_NAMED(LOGNAME, "The joints for controller '%s' are not known. Perhaps the controller configuration is " - "not loaded on the param server?", + ROS_WARN_NAMED(LOGNAME, + "The joints for controller '%s' are not known. Perhaps the controller configuration is " + "not loaded on the param server?", name.c_str()); joints.clear(); } diff --git a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp index 1a412ae7b8..43916a8f69 100644 --- a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp +++ b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp @@ -88,8 +88,8 @@ void checkHeader(moveit_msgs::Constraints& c, const std::string& header_frame) c.orientation_constraints[i].header.stamp = ros::Time::now(); } } -} -} +} // namespace +} // namespace moveit_benchmarks moveit_benchmarks::BenchmarkExecution::BenchmarkExecution(const planning_scene::PlanningScenePtr& scene, warehouse_ros::DatabaseConnection::Ptr conn) @@ -894,7 +894,7 @@ bool isIKSolutionCollisionFree(const planning_scene::PlanningScene* scene, movei else return true; } -} +} // namespace void moveit_benchmarks::BenchmarkExecution::runPlanningBenchmark(BenchmarkRequest& req) { @@ -1001,8 +1001,9 @@ void moveit_benchmarks::BenchmarkExecution::runPlanningBenchmark(BenchmarkReques ROS_ERROR("Planning interface '%s' has no planners defined", it->first.c_str()); } else - ROS_WARN_STREAM("Planning interface '" << it->second->getDescription() << "' is not able to solve the specified " - "benchmark problem."); + ROS_WARN_STREAM("Planning interface '" << it->second->getDescription() + << "' is not able to solve the specified " + "benchmark problem."); } // error check @@ -1247,8 +1248,7 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR bool reachable = false; if (req.motion_plan_request.goal_constraints.size() > 0 && req.motion_plan_request.goal_constraints[0].position_constraints.size() > 0 && - req.motion_plan_request.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses.size() > - 0 && + req.motion_plan_request.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses.size() > 0 && req.motion_plan_request.goal_constraints[0].orientation_constraints.size() > 0) { // Compute IK on goal constraints @@ -1276,10 +1276,11 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR // Compute IK ROS_INFO_STREAM("Processing goal " << req.motion_plan_request.goal_constraints[0].name << " ..."); ros::WallTime startTime = ros::WallTime::now(); - success = robot_state.setFromIK( - robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, - req.motion_plan_request.num_planning_attempts, req.motion_plan_request.allowed_planning_time, - boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable)); + success = + robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, + req.motion_plan_request.num_planning_attempts, + req.motion_plan_request.allowed_planning_time, + boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable)); if (success) { ROS_INFO(" Success!"); @@ -1365,10 +1366,11 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR ROS_INFO_STREAM("Processing trajectory waypoint " << req.motion_plan_request.trajectory_constraints.constraints[tc].name << " ..."); startTime = ros::WallTime::now(); - success = robot_state.setFromIK( - robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, - req.motion_plan_request.num_planning_attempts, req.motion_plan_request.allowed_planning_time, - boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable)); + success = + robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, + req.motion_plan_request.num_planning_attempts, + req.motion_plan_request.allowed_planning_time, + boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable)); double duration = (ros::WallTime::now() - startTime).toSec(); if (success) diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index 90d3a29f8e..c6ed75b396 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2015, Rice University -* 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 the Rice University 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Rice University + * 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 the Rice University 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. + *********************************************************************/ /* Author: Ryan Luna */ @@ -93,8 +93,7 @@ class BenchmarkExecutor /// Definition of a post-run benchmark event function. Invoked immediately after each planner calls solve(). typedef boost::function + const planning_interface::MotionPlanDetailedResponse& response, PlannerRunData& run_data)> PostRunEventFunction; BenchmarkExecutor(const std::string& robot_description_param = "robot_description"); diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h index 88caedb796..e90a5076ac 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2015, Rice University -* 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 the Rice University 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Rice University + * 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 the Rice University 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. + *********************************************************************/ /* Author: Ryan Luna */ diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 44925d4df8..35e7e98a18 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2015, Rice University -* 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 the Rice University 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Rice University + * 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 the Rice University 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. + *********************************************************************/ /* Author: Ryan Luna */ @@ -244,8 +244,8 @@ bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& opts) return false; } -bool BenchmarkExecutor::queriesAndPlannersCompatible( - const std::vector& requests, const std::map>& /*planners*/) +bool BenchmarkExecutor::queriesAndPlannersCompatible(const std::vector& requests, + const std::map>& /*planners*/) { // Make sure that the planner interfaces can service the desired queries for (const std::pair& pipeline_entry : planning_pipelines_) diff --git a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp index 96dd820afb..567eac3d5c 100644 --- a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2015, Rice University -* 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 the Rice University 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Rice University + * 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 the Rice University 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. + *********************************************************************/ /* Author: Ryan Luna */ diff --git a/moveit_ros/benchmarks/src/RunBenchmark.cpp b/moveit_ros/benchmarks/src/RunBenchmark.cpp index 19496e54e6..4a512c5c21 100644 --- a/moveit_ros/benchmarks/src/RunBenchmark.cpp +++ b/moveit_ros/benchmarks/src/RunBenchmark.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2015, Rice University -* 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 the Rice University 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Rice University + * 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 the Rice University 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. + *********************************************************************/ /* Author: Ryan Luna */ diff --git a/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp b/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp index 1bd9818579..914b189d03 100644 --- a/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp +++ b/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2019, PickNik LLC -* 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 the PickNik LLC 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * 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 the PickNik LLC 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. + *********************************************************************/ /* Author: Henning Kayser */ /* Description: A simple benchmark that plans trajectories for all combinations of specified predefined poses */ diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h b/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h index 95afa9e2b0..5feaa87575 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h +++ b/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h @@ -42,4 +42,4 @@ namespace move_group { static const std::string PICKUP_ACTION = "pickup"; // name of 'pickup' action static const std::string PLACE_ACTION = "place"; // name of 'place' action -} +} // namespace move_group diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp index 4d413e1c9b..8e2d0c9409 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp +++ b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp @@ -54,8 +54,7 @@ void move_group::MoveGroupPickPlaceAction::initialize() // start the pickup action server pickup_action_server_.reset(new actionlib::SimpleActionServer( - root_node_handle_, PICKUP_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, _1), - false)); + root_node_handle_, PICKUP_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, _1), false)); pickup_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this)); pickup_action_server_->start(); @@ -283,8 +282,8 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanAndExecute( action_res.error_code = plan.error_code_; } -void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanAndExecute( - const moveit_msgs::PlaceGoalConstPtr& goal, moveit_msgs::PlaceResult& action_res) +void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanAndExecute(const moveit_msgs::PlaceGoalConstPtr& goal, + moveit_msgs::PlaceResult& action_res) { plan_execution::PlanExecution::Options opt; diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h index 5987597bc9..5945235850 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h +++ b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h @@ -56,8 +56,7 @@ class MoveGroupPickPlaceAction : public MoveGroupCapability void executePickupCallback(const moveit_msgs::PickupGoalConstPtr& goal); void executePlaceCallback(const moveit_msgs::PlaceGoalConstPtr& goal); - void executePickupCallbackPlanOnly(const moveit_msgs::PickupGoalConstPtr& goal, - moveit_msgs::PickupResult& action_res); + void executePickupCallbackPlanOnly(const moveit_msgs::PickupGoalConstPtr& goal, moveit_msgs::PickupResult& action_res); void executePickupCallbackPlanAndExecute(const moveit_msgs::PickupGoalConstPtr& goal, moveit_msgs::PickupResult& action_res); diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h index f341abf45b..942e8aca59 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h index ab26f50dae..6c2118562a 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h index 32ef5d4277..38be2ba658 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp index 1a3a6aab6b..29cab837f5 100644 --- a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp +++ b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -209,8 +209,8 @@ void ManipulationPipeline::push(const ManipulationPlanPtr& plan) { boost::mutex::scoped_lock slock(queue_access_lock_); queue_.push_back(plan); - ROS_INFO_STREAM_NAMED("manipulation", "Added plan for pipeline '" << name_ << "'. Queue is now of size " - << queue_.size()); + ROS_INFO_STREAM_NAMED("manipulation", + "Added plan for pipeline '" << name_ << "'. Queue is now of size " << queue_.size()); queue_access_cond_.notify_all(); } diff --git a/moveit_ros/manipulation/pick_place/src/place.cpp b/moveit_ros/manipulation/pick_place/src/place.cpp index 2bf053a06c..92eb153a37 100644 --- a/moveit_ros/manipulation/pick_place/src/place.cpp +++ b/moveit_ros/manipulation/pick_place/src/place.cpp @@ -104,8 +104,8 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene const std::vector& eef_names = jmg->getAttachedEndEffectorNames(); if (eef_names.empty()) { - ROS_ERROR_STREAM_NAMED("manipulation", "There are no end-effectors specified for group '" << goal.group_name - << "'"); + ROS_ERROR_STREAM_NAMED("manipulation", + "There are no end-effectors specified for group '" << goal.group_name << "'"); error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } diff --git a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp b/moveit_ros/manipulation/pick_place/src/plan_stage.cpp index 3646c94a0f..c704cd3ede 100644 --- a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/plan_stage.cpp @@ -65,8 +65,8 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const req.planner_id = plan->shared_data_->planner_id_; req.start_state.is_diff = true; - req.goal_constraints.resize( - 1, kinematic_constraints::constructGoalConstraints(*plan->approach_state_, plan->shared_data_->planning_group_)); + req.goal_constraints.resize(1, kinematic_constraints::constructGoalConstraints(*plan->approach_state_, + plan->shared_data_->planning_group_)); unsigned int attempts = 0; do // give the planner two chances { @@ -77,8 +77,7 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const // We have a valid motion plan, now apply pre-approach end effector posture (open gripper) if applicable if (!plan->approach_posture_.joint_names.empty()) { - moveit::core::RobotStatePtr pre_approach_state( - new moveit::core::RobotState(res.trajectory_->getLastWayPoint())); + moveit::core::RobotStatePtr pre_approach_state(new moveit::core::RobotState(res.trajectory_->getLastWayPoint())); robot_trajectory::RobotTrajectoryPtr pre_approach_traj(new robot_trajectory::RobotTrajectory( pre_approach_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName())); pre_approach_traj->setRobotTrajectoryMsg(*pre_approach_state, plan->approach_posture_); @@ -96,8 +95,7 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const ROS_INFO_STREAM("Adding default duration of " << PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION << " seconds to the grasp closure time. Assign time_from_start " << "to your trajectory to avoid this."); - pre_approach_traj->addPrefixWayPoint(pre_approach_state, - PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION); + pre_approach_traj->addPrefixWayPoint(pre_approach_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION); } // Add the open gripper trajectory to the plan diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h index daeeb838a6..97c1ddf881 100644 --- a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h +++ b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h @@ -52,7 +52,7 @@ namespace plan_execution { MOVEIT_CLASS_FORWARD(PlanExecution); MOVEIT_CLASS_FORWARD(PlanWithSensing); -} +} // namespace plan_execution namespace trajectory_execution_manager { diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 321a5ad3dc..e2b939dadc 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -142,8 +142,9 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath kset->empty() ? nullptr : kset.get(), _1, _2, _3); } bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req.header.frame_id); - ROS_INFO_NAMED(getName(), "Attempting to follow %u waypoints for link '%s' using a step of %lf m " - "and jump threshold %lf (in %s reference frame)", + ROS_INFO_NAMED(getName(), + "Attempting to follow %u waypoints for link '%s' using a step of %lf m " + "and jump threshold %lf (in %s reference frame)", (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, req.jump_threshold, global_frame ? "global" : "link"); std::vector traj; diff --git a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h index 9ce1b3bb19..96962faf68 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h @@ -47,4 +47,4 @@ class MoveGroupGetPlanningSceneService : public MoveGroupCapability void initialize() override; }; -} +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index 849ea5251a..d977ace075 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -155,9 +155,10 @@ bool MoveGroupKinematicsService::computeIKService(moveit_msgs::GetPositionIK::Re moveit::core::RobotState rs = ls->getCurrentState(); kset.add(req.ik_request.constraints, ls->getTransforms()); computeIK(req.ik_request, res.solution, res.error_code, rs, - boost::bind(&isIKSolutionValid, req.ik_request.avoid_collisions ? - static_cast(ls).get() : - nullptr, + boost::bind(&isIKSolutionValid, + req.ik_request.avoid_collisions ? + static_cast(ls).get() : + nullptr, kset.empty() ? nullptr : &kset, _1, _2, _3)); } else diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index 484c2d5782..30ddaff162 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -34,7 +34,7 @@ * 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 diff --git a/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h index 8300134bab..64c55367bc 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h @@ -34,7 +34,7 @@ * 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 diff --git a/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h b/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h index 9108626617..b3418e3d7a 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h @@ -34,7 +34,7 @@ * 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 diff --git a/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h index 73fd73df8d..a593da6127 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h @@ -34,7 +34,7 @@ * 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 diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h index 6a3a29d4bd..5e9f26fe64 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -34,7 +34,7 @@ * 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 diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index a8f676033a..d991aee0e5 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -34,7 +34,7 @@ * 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 @@ -145,8 +145,8 @@ class ServoCalcs void calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta); /** \brief Convert joint deltas to an outgoing JointTrajectory command. - * This happens for joint commands and Cartesian commands. - */ + * This happens for joint commands and Cartesian commands. + */ bool convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& joint_trajectory); /** \brief Gazebo simulations have very strict message timestamp requirements. diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h index 2fcdea7c5c..ad0deec16f 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h @@ -34,7 +34,7 @@ * 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 diff --git a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h index f6e9ddcb54..97e92210cc 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h @@ -34,7 +34,7 @@ * 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 diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp index 4c7535e620..18bdb06294 100644 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ b/moveit_ros/moveit_servo/src/collision_check.cpp @@ -29,7 +29,7 @@ * 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. -*******************************************************************************/ + *******************************************************************************/ /* Title : collision_check.cpp * Project : moveit_servo @@ -212,15 +212,16 @@ void CollisionCheck::printCollisionPairs(collision_detection::CollisionResult::C if (!contact_map.empty()) { // Throttled error message about the first contact in the list - ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Objects in collision (among others, possibly): " - << contact_map.begin()->first.first << ", " - << contact_map.begin()->first.second); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "Objects in collision (among others, possibly): " + << contact_map.begin()->first.first << ", " + << contact_map.begin()->first.second); // Log all other contacts if in debug mode ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Objects in collision:"); for (auto contact : contact_map) { - ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "\t" << contact.first.first << ", " - << contact.first.second); + ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "\t" << contact.first.first << ", " << contact.first.second); } } } diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp index 6db053e4be..53e919ee33 100644 --- a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp @@ -34,7 +34,7 @@ * 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 diff --git a/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp b/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp index 2ed780b650..c60b331119 100644 --- a/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp +++ b/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp @@ -29,7 +29,7 @@ * 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. -*******************************************************************************/ + *******************************************************************************/ /* Title : joint_state_subscriber.cpp * Project : moveit_servo diff --git a/moveit_ros/moveit_servo/src/low_pass_filter.cpp b/moveit_ros/moveit_servo/src/low_pass_filter.cpp index 7013b3b4c9..da04cd2716 100644 --- a/moveit_ros/moveit_servo/src/low_pass_filter.cpp +++ b/moveit_ros/moveit_servo/src/low_pass_filter.cpp @@ -29,7 +29,7 @@ * 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. -*******************************************************************************/ + *******************************************************************************/ /* Title : low_pass_filter.cpp * Project : moveit_servo @@ -48,7 +48,7 @@ namespace { constexpr char LOGNAME[] = "low_pass_filter"; constexpr double EPSILON = 1e-9; -} +} // namespace LowPassFilter::LowPassFilter(double low_pass_filter_coeff) : previous_measurements_{ 0., 0. } @@ -67,12 +67,11 @@ LowPassFilter::LowPassFilter(double low_pass_filter_coeff) if (std::abs(feedback_term_) < EPSILON) { - ROS_WARN_STREAM_NAMED( - LOGNAME, "Filter coefficient value of " - << low_pass_filter_coeff - << " resulted in feedback term of 0. " - " This results in a window averaging Finite Impulse Response (FIR) filter with a gain of " - << scale_term_ * LowPassFilter::FILTER_LENGTH); + ROS_WARN_STREAM_NAMED(LOGNAME, "Filter coefficient value of " << low_pass_filter_coeff + << " resulted in feedback term of 0. " + " This results in a window averaging Finite " + "Impulse Response (FIR) filter with a gain of " + << scale_term_ * LowPassFilter::FILTER_LENGTH); } } diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index d8db2630df..ee567f8bb3 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -29,7 +29,7 @@ * 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. -*******************************************************************************/ + *******************************************************************************/ /* Title : servo.cpp * Project : moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 13684e42ca..b6d4fd774e 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -29,7 +29,7 @@ * 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. -*******************************************************************************/ + *******************************************************************************/ /* Title : servo_calcs.cpp * Project : moveit_servo @@ -287,8 +287,9 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) // Print a warning to the user if both are stale if (!twist_command_is_stale_ || !joint_command_is_stale_) { - ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " - "Try a larger 'incoming_command_timeout' parameter?"); + ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, + "Stale command. " + "Try a larger 'incoming_command_timeout' parameter?"); } // If we should halt @@ -515,8 +516,7 @@ bool ServoCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& jo // Spam several redundant points into the trajectory. The first few may be skipped if the // time stamp is in the past when it reaches the client. Needed for gazebo simulation. // Start from 2 because the first point's timestamp is already 1*parameters_.publish_period -void ServoCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& joint_trajectory, - int count) const +void ServoCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& joint_trajectory, int count) const { joint_trajectory.points.resize(count); auto point = joint_trajectory.points[0]; @@ -653,9 +653,8 @@ double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& co if ((ini_condition > parameters_.lower_singularity_threshold) && (ini_condition < parameters_.hard_stop_singularity_threshold)) { - velocity_scale = 1. - - (ini_condition - parameters_.lower_singularity_threshold) / - (parameters_.hard_stop_singularity_threshold - parameters_.lower_singularity_threshold); + velocity_scale = 1. - (ini_condition - parameters_.lower_singularity_threshold) / + (parameters_.hard_stop_singularity_threshold - parameters_.lower_singularity_threshold); status_ = StatusCode::DECELERATE_FOR_SINGULARITY; ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, SERVO_STATUS_CODE_MAP.at(status_)); } @@ -774,10 +773,10 @@ bool ServoCalcs::enforceSRDFPositionLimits() (kinematic_state_->getJointVelocities(joint)[0] > 0 && (joint_angle > (limits[0].max_position - parameters_.joint_limit_margin)))) { - ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, ros::this_node::getName() - << " " << joint->getName() - << " close to a " - " position limit. Halting."); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + ros::this_node::getName() << " " << joint->getName() + << " close to a " + " position limit. Halting."); halting = true; } } @@ -828,8 +827,8 @@ bool ServoCalcs::updateJoints() } catch (const std::out_of_range& e) { - ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Ignoring joint " - << latest_joint_state->name[m]); + ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "Ignoring joint " << latest_joint_state->name[m]); continue; } @@ -962,9 +961,9 @@ bool ServoCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen } catch (const std::out_of_range& e) { - ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, ros::this_node::getName() - << " Lengths of output and " - "increments do not match."); + ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + ros::this_node::getName() << " Lengths of output and " + "increments do not match."); return false; } } diff --git a/moveit_ros/moveit_servo/src/servo_server.cpp b/moveit_ros/moveit_servo/src/servo_server.cpp index ef67b50277..590061609f 100644 --- a/moveit_ros/moveit_servo/src/servo_server.cpp +++ b/moveit_ros/moveit_servo/src/servo_server.cpp @@ -29,7 +29,7 @@ * 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. -*******************************************************************************/ + *******************************************************************************/ /* Title : servo_server.cpp * Project : moveit_servo diff --git a/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp index 8294ba897b..cec304711a 100644 --- a/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp +++ b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp @@ -34,7 +34,7 @@ * 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 "geometry_msgs/TwistStamped.h" #include "control_msgs/JointJog.h" diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index c87ee47189..79ec097a8a 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -83,8 +83,7 @@ void OccupancyMapMonitor::initialize() if (!nh_.getParam("octomap_resolution", map_resolution_)) { map_resolution_ = 0.1; - ROS_WARN_NAMED(LOGNAME, "Resolution not specified for Octomap. Assuming resolution = %g instead", - map_resolution_); + ROS_WARN_NAMED(LOGNAME, "Resolution not specified for Octomap. Assuming resolution = %g instead", map_resolution_); } } ROS_DEBUG_NAMED(LOGNAME, "Using resolution = %lf m for building octomap", map_resolution_); diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index b57d9e5418..4ae40c00a8 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -264,9 +264,9 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP for (int t = 0; t < nt; ++t) try { - tf2::fromMsg( - tf_buffer_->lookupTransform(monitor_->getMapFrame(), depth_msg->header.frame_id, depth_msg->header.stamp), - map_h_sensor); + tf2::fromMsg(tf_buffer_->lookupTransform(monitor_->getMapFrame(), depth_msg->header.frame_id, + depth_msg->header.stamp), + map_h_sensor); found = true; break; } diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/updater_plugin.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/updater_plugin.cpp index ab76ac2076..83d955c803 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/updater_plugin.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/updater_plugin.cpp @@ -37,5 +37,4 @@ #include #include -CLASS_LOADER_REGISTER_CLASS(occupancy_map_monitor::DepthImageOctomapUpdater, - occupancy_map_monitor::OccupancyMapUpdater); +CLASS_LOADER_REGISTER_CLASS(occupancy_map_monitor::DepthImageOctomapUpdater, occupancy_map_monitor::OccupancyMapUpdater); diff --git a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp index cc033993ae..300da502fb 100644 --- a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp +++ b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp @@ -122,9 +122,10 @@ void LazyFreeSpaceUpdater::processThread() if (!running_) break; - ROS_DEBUG_NAMED( - LOGNAME, "Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells", - (long unsigned int)process_occupied_cells_set_->size(), (long unsigned int)process_model_cells_set_->size()); + ROS_DEBUG_NAMED(LOGNAME, + "Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells", + (long unsigned int)process_occupied_cells_set_->size(), + (long unsigned int)process_model_cells_set_->size()); ros::WallTime start = ros::WallTime::now(); tree_->lockRead(); diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h index eb03286787..247f1ae009 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h @@ -48,7 +48,7 @@ namespace tf2_ros { class TransformListener; class Buffer; -} +} // namespace tf2_ros /** * \brief Class that caches and updates transformations for given frames. diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp index 4dd045381d..98ed797ae4 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp @@ -52,8 +52,7 @@ using namespace std; using namespace Eigen; using shapes::Mesh; -mesh_filter::MeshFilter::MeshFilter( - const boost::function& transform_callback) +mesh_filter::MeshFilter::MeshFilter(const boost::function& transform_callback) : mesh_renderer_(640, 480, 0.3, 10) // some default values for buffer sizes and clipping planes , depth_filter_(640, 480, 0.3, 10) , next_handle_(FirstLabel) // 0 and 1 are reserved! @@ -163,7 +162,7 @@ inline bool isAligned16(const void* pointer) { return (((uintptr_t)pointer & 15) == 0); } -} +} // namespace void mesh_filter::MeshFilter::getModelDepth(float* depth) const { diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp index 58162afa22..27d5627f2c 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp @@ -255,8 +255,7 @@ void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const void mesh_filter::MeshFilterBase::getFilteredLabels(LabelType* labels) const { - JobPtr job( - new FilterJob(boost::bind(&GLRenderer::getColorBuffer, depth_filter_.get(), (unsigned char*)labels))); + JobPtr job(new FilterJob(boost::bind(&GLRenderer::getColorBuffer, depth_filter_.get(), (unsigned char*)labels))); addJob(job); job->wait(); } diff --git a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp index 5440274826..2d9da2b231 100644 --- a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp +++ b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp @@ -135,8 +135,7 @@ void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::Poi if (!transform_callback_(it->handle, tmp)) { if (!it->body) - ROS_ERROR_STREAM_NAMED(LOGNAME, "Missing transform for shape with handle " << it->handle - << " without a body"); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Missing transform for shape with handle " << it->handle << " without a body"); else ROS_ERROR_STREAM_NAMED(LOGNAME, "Missing transform for shape " << it->body->getType() << " with handle " << it->handle); diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index 2c92ae43b9..38953339ca 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -194,9 +194,9 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2:: { try { - tf2::fromMsg( - tf_buffer_->lookupTransform(monitor_->getMapFrame(), cloud_msg->header.frame_id, cloud_msg->header.stamp), - map_h_sensor); + tf2::fromMsg(tf_buffer_->lookupTransform(monitor_->getMapFrame(), cloud_msg->header.frame_id, + cloud_msg->header.stamp), + map_h_sensor); } catch (tf2::TransformException& ex) { diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp index a03d06eff7..d3a12b0b6c 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp +++ b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp @@ -88,8 +88,9 @@ class ConstraintSamplerManagerLoader::Helper ConstraintSamplerManagerLoader::ConstraintSamplerManagerLoader( const constraint_samplers::ConstraintSamplerManagerPtr& csm) - : constraint_sampler_manager_(csm ? csm : constraint_samplers::ConstraintSamplerManagerPtr( - new constraint_samplers::ConstraintSamplerManager())) + : constraint_sampler_manager_( + csm ? csm : + constraint_samplers::ConstraintSamplerManagerPtr(new constraint_samplers::ConstraintSamplerManager())) , impl_(new Helper(constraint_sampler_manager_)) { } diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index 286896c521..a5c75bc2a4 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -349,8 +349,9 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const if (nh.searchParam(base_param_name + "/kinematics_solver_attempts", ksolver_attempts_param_name) && nh.hasParam(ksolver_attempts_param_name)) { - ROS_WARN_ONCE_NAMED(LOGNAME, "Kinematics solver doesn't support #attempts anymore, but only a timeout.\n" - "Please remove the parameter '%s' from your configuration.", + ROS_WARN_ONCE_NAMED(LOGNAME, + "Kinematics solver doesn't support #attempts anymore, but only a timeout.\n" + "Please remove the parameter '%s' from your configuration.", ksolver_attempts_param_name.c_str()); } @@ -406,8 +407,9 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const { if (ksolver_ik_links.getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_WARN_STREAM_NAMED(LOGNAME, "rosparam '" << ksolver_ik_links_param_name << "' should be an XmlRpc " - "value type 'Array'"); + ROS_WARN_STREAM_NAMED(LOGNAME, "rosparam '" << ksolver_ik_links_param_name + << "' should be an XmlRpc " + "value type 'Array'"); } else { diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index b282a34a9d..e7657122d9 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -198,8 +198,7 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p plan.error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN || plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA) { - if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA && - opt.replan_delay_ > 0.0) + if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA && opt.replan_delay_ > 0.0) { ros::WallDuration d(opt.replan_delay_); d.sleep(); @@ -394,9 +393,9 @@ moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(E trajectory_monitor_->startTrajectoryMonitor(); // start a trajectory execution thread - trajectory_execution_manager_->execute( - boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1), - boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, _1)); + trajectory_execution_manager_->execute(boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1), + boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, + _1)); // wait for path to be done, while checking that the path does not become invalid ros::Rate r(100); path_became_invalid_ = false; diff --git a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp index a913b5e976..2f0f13edaa 100644 --- a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp +++ b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp @@ -76,10 +76,10 @@ enum class CollisionObjectType /** \brief Clutters the world of the planning scene with random objects in a certain area around the origin. All added * objects are not in collision with the robot. -* -* \param planning_scene The planning scene -* \param num_objects The number of objects to be cluttered -* \param CollisionObjectType Type of object to clutter (mesh or box) */ + * + * \param planning_scene The planning scene + * \param num_objects The number of objects to be cluttered + * \param CollisionObjectType Type of object to clutter (mesh or box) */ void clutterWorld(const planning_scene::PlanningScenePtr& planning_scene, const size_t num_objects, CollisionObjectType type) { @@ -164,11 +164,11 @@ void clutterWorld(const planning_scene::PlanningScenePtr& planning_scene, const } /** \brief Runs a collision detection benchmark and measures the time. -* -* \param trials The number of repeated collision checks for each state -* \param scene The planning scene -* \param CollisionDetector The type of collision detector -* \param only_self Flag for only self collision check performed */ + * + * \param trials The number of repeated collision checks for each state + * \param scene The planning scene + * \param CollisionDetector The type of collision detector + * \param only_self Flag for only self collision check performed */ void runCollisionDetection(unsigned int trials, const planning_scene::PlanningScenePtr& scene, const std::vector& states, const CollisionDetector col_detector, bool only_self, bool distance = false) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp index d3e4fb1617..6fe04103fa 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp @@ -192,8 +192,8 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap { // heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to // the computed trajectory) - res.trajectory_->setWayPointDurationFromPrevious( - 0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration())); + res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, + res.trajectory_->getAverageSegmentDuration())); res.trajectory_->addPrefixWayPoint(prefix_state, 0.0); // we add a prefix point, so we need to bump any previously added index positions for (std::size_t& added_index : added_path_index) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp index 8a3c72431e..754d521260 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp @@ -157,8 +157,8 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA { // heuristically decide a duration offset for the trajectory (induced by the additional point added as a // prefix to the computed trajectory) - res.trajectory_->setWayPointDurationFromPrevious( - 0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration())); + res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, + res.trajectory_->getAverageSegmentDuration())); res.trajectory_->addPrefixWayPoint(prefix_state, 0.0); // we add a prefix point, so we need to bump any previously added index positions for (std::size_t& added_index : added_path_index) diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h index 95292a3634..1df0a8a1af 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h @@ -138,7 +138,7 @@ class CurrentStateMonitor /** @brief Wait for at most \e wait_time seconds (default 1s) for a robot state more recent than t * @return true on success, false if up-to-date robot state wasn't received within \e wait_time - */ + */ bool waitForCurrentState(const ros::Time t = ros::Time::now(), double wait_time = 1.0) const; /** @brief Wait for at most \e wait_time seconds until the complete robot state is known. diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index c7549056b5..b0716f346f 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -628,7 +628,7 @@ class LockedPlanningSceneRO return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene(); } - operator const planning_scene::PlanningSceneConstPtr&() const + operator const planning_scene::PlanningSceneConstPtr &() const { return static_cast(planning_scene_monitor_.get())->getPlanningScene(); } @@ -709,7 +709,7 @@ class LockedPlanningSceneRW : public LockedPlanningSceneRO { } - operator const planning_scene::PlanningScenePtr&() + operator const planning_scene::PlanningScenePtr &() { return planning_scene_monitor_->getPlanningScene(); } diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 6d896254eb..aa43572346 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -763,8 +763,7 @@ void PlanningSceneMonitor::includeWorldObjectsInOctree() boost::recursive_mutex::scoped_lock _(shape_handles_lock_); // clear information about any attached object - for (std::pair>>& + for (std::pair>>& collision_body_shape_handle : collision_body_shape_handles_) for (std::pair& it : collision_body_shape_handle.second) diff --git a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp index d5bc34ca5d..50e9667a28 100644 --- a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp +++ b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp @@ -160,8 +160,8 @@ void RobotModelLoader::configure(const Options& opt) if (model_ && opt.load_kinematics_solvers_) loadKinematicsSolvers(); - ROS_DEBUG_STREAM_NAMED("robot_model_loader", "Loaded kinematic model in " << (ros::WallTime::now() - start).toSec() - << " seconds"); + ROS_DEBUG_STREAM_NAMED("robot_model_loader", + "Loaded kinematic model in " << (ros::WallTime::now() - start).toSec() << " seconds"); } void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::KinematicsPluginLoaderPtr& kloader) @@ -175,8 +175,7 @@ void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::Kin if (kloader) kinematics_loader_ = kloader; else - kinematics_loader_.reset( - new kinematics_plugin_loader::KinematicsPluginLoader(rdf_loader_->getRobotDescription())); + kinematics_loader_.reset(new kinematics_plugin_loader::KinematicsPluginLoader(rdf_loader_->getRobotDescription())); moveit::core::SolverAllocatorFn kinematics_allocator = kinematics_loader_->getLoaderFunction(rdf_loader_->getSRDF()); const std::vector& groups = kinematics_loader_->getKnownGroups(); diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index 269422c203..22a7ee9eea 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -339,8 +339,7 @@ class TrajectoryExecutionManager std::vector trajectories_; std::deque continuous_execution_queue_; - std::unique_ptr > - controller_manager_loader_; + std::unique_ptr > controller_manager_loader_; moveit_controller_manager::MoveItControllerManagerPtr controller_manager_; bool verbose_; diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index a7920194f7..63fd9b5ee7 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -144,8 +144,9 @@ void TrajectoryExecutionManager::initialize() if (classes.size() == 1) { controller = classes[0]; - ROS_WARN_NAMED(name_, "Parameter '~moveit_controller_manager' is not specified but only one " - "matching plugin was found: '%s'. Using that one.", + ROS_WARN_NAMED(name_, + "Parameter '~moveit_controller_manager' is not specified but only one " + "matching plugin was found: '%s'. Using that one.", controller.c_str()); } else @@ -161,8 +162,8 @@ void TrajectoryExecutionManager::initialize() } catch (pluginlib::PluginlibException& ex) { - ROS_FATAL_STREAM_NAMED(name_, "Exception while loading controller manager '" << controller - << "': " << ex.what()); + ROS_FATAL_STREAM_NAMED(name_, + "Exception while loading controller manager '" << controller << "': " << ex.what()); } } @@ -974,8 +975,9 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont jm->enforcePositionBounds(&traj_position); if (jm->distance(&cur_position, &traj_position) > allowed_start_tolerance_) { - ROS_ERROR_NAMED(name_, "\nInvalid Trajectory: start point deviates from current robot state more than %g" - "\njoint '%s': expected: %g, current: %g", + ROS_ERROR_NAMED(name_, + "\nInvalid Trajectory: start point deviates from current robot state more than %g" + "\njoint '%s': expected: %g, current: %g", allowed_start_tolerance_, joint_names[i].c_str(), traj_position, cur_position); return false; } @@ -1478,8 +1480,9 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) if (!handle->waitForExecution(expected_trajectory_duration)) if (!execution_complete_ && ros::Time::now() - current_time > expected_trajectory_duration) { - ROS_ERROR_NAMED(name_, "Controller is taking too long to execute trajectory (the expected upper " - "bound for the trajectory execution was %lf seconds). Stopping trajectory.", + ROS_ERROR_NAMED(name_, + "Controller is taking too long to execute trajectory (the expected upper " + "bound for the trajectory execution was %lf seconds). Stopping trajectory.", expected_trajectory_duration.toSec()); { boost::mutex::scoped_lock slock(execution_state_mutex_); diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h index d457714c84..0359fe677b 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h +++ b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h @@ -55,8 +55,8 @@ moveit::core::RobotModelConstPtr getSharedRobotModel(const std::string& robot_de @param tf_buffer @return */ -planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor( - const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer); +planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, + const std::shared_ptr& tf_buffer); /** @brief getSharedStateMonitor diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 55a1a47bf5..e524e17e2e 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -706,8 +706,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (objects.empty()) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Asked for grasps for the object '" << object - << "', but the object could not be found"); + ROS_ERROR_STREAM_NAMED(LOGNAME, + "Asked for grasps for the object '" << object << "', but the object could not be found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME); } diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 7d84ed7405..24fb87e984 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -554,8 +554,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer } } - Eigen::MatrixXd getJacobianMatrixPython(const bp::list& joint_values, - const bp::object& reference_point = bp::object()) + Eigen::MatrixXd getJacobianMatrixPython(const bp::list& joint_values, const bp::object& reference_point = bp::object()) { const std::vector v = py_bindings_tools::doubleFromList(joint_values); std::vector ref; @@ -599,8 +598,7 @@ static void wrap_move_group_interface() move_group_interface_class.def("get_name", &MoveGroupInterfaceWrapper::getNameCStr); move_group_interface_class.def("get_planning_frame", &MoveGroupInterfaceWrapper::getPlanningFrameCStr); - move_group_interface_class.def("get_interface_description", - &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython); + move_group_interface_class.def("get_interface_description", &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython); move_group_interface_class.def("get_active_joints", &MoveGroupInterfaceWrapper::getActiveJointsList); move_group_interface_class.def("get_joints", &MoveGroupInterfaceWrapper::getJointsList); @@ -657,8 +655,7 @@ static void wrap_move_group_interface() &MoveGroupInterfaceWrapper::rememberJointValues; move_group_interface_class.def("remember_joint_values", remember_joint_values_2); - move_group_interface_class.def("remember_joint_values", - &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList); + move_group_interface_class.def("remember_joint_values", &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList); move_group_interface_class.def("start_state_monitor", &MoveGroupInterfaceWrapper::startStateMonitor); move_group_interface_class.def("get_current_joint_values", &MoveGroupInterfaceWrapper::getCurrentJointValuesList); @@ -686,8 +683,7 @@ static void wrap_move_group_interface() bool (MoveGroupInterfaceWrapper::*set_path_constraints_1)(const std::string&) = &MoveGroupInterfaceWrapper::setPathConstraints; move_group_interface_class.def("set_path_constraints", set_path_constraints_1); - move_group_interface_class.def("set_path_constraints_from_msg", - &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg); + move_group_interface_class.def("set_path_constraints_from_msg", &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg); move_group_interface_class.def("get_path_constraints", &MoveGroupInterfaceWrapper::getPathConstraintsPython); move_group_interface_class.def("clear_path_constraints", &MoveGroupInterfaceWrapper::clearPathConstraints); move_group_interface_class.def("get_known_constraints", &MoveGroupInterfaceWrapper::getKnownConstraintsList); diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index 3a86a62f22..3c2b4a6db2 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -181,8 +181,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl for (const moveit_msgs::CollisionObject& collision_object : response.scene.world.collision_objects) { - if (object_ids.empty() || - std::find(object_ids.begin(), object_ids.end(), collision_object.id) != object_ids.end()) + if (object_ids.empty() || std::find(object_ids.begin(), object_ids.end(), collision_object.id) != object_ids.end()) { result[collision_object.id] = collision_object; } diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h index 9ca48182db..7f72363231 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h @@ -125,9 +125,9 @@ namespace converter template <> struct object_manager_traits #if PY_VERSION_HEX >= 0x03000000 - : pytype_object_manager_traits<&PyBytes_Type, moveit::py_bindings_tools::ByteString> + : pytype_object_manager_traits<&PyBytes_Type, moveit::py_bindings_tools::ByteString> #else - : pytype_object_manager_traits<&PyString_Type, moveit::py_bindings_tools::ByteString> + : pytype_object_manager_traits<&PyString_Type, moveit::py_bindings_tools::ByteString> #endif { diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp index bb2666f24b..ceae107b73 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2020, Tyler Weaver -* 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 PickNik Robotics 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Tyler Weaver + * 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 PickNik Robotics 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. + *********************************************************************/ /* Author: Tyler Weaver */ diff --git a/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp index a7dbffd210..4b2d906448 100644 --- a/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2020, Tyler Weaver -* 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 PickNik Robotics 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Tyler Weaver + * 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 PickNik Robotics 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. + *********************************************************************/ /* Author: Tyler Weaver */ diff --git a/moveit_ros/planning_interface/test/subframes_test.cpp b/moveit_ros/planning_interface/test/subframes_test.cpp index 757fce094b..f38be9ea73 100644 --- a/moveit_ros/planning_interface/test/subframes_test.cpp +++ b/moveit_ros/planning_interface/test/subframes_test.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2019, Felix von Drigalski, Jacob Aas, Tyler Weaver -* 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 OMRON SINIC X or PickNik Robotics 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Felix von Drigalski, Jacob Aas, Tyler Weaver + * 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 OMRON SINIC X or PickNik Robotics 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. + *********************************************************************/ /* Author: Felix von Drigalski, Jacob Aas, Tyler Weaver */ diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h index fc1dafc8d2..95121439e0 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h @@ -49,7 +49,7 @@ namespace core { class RobotState; } -} +} // namespace moveit namespace robot_interaction { diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h index 9404d5dc0b..6673198820 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h @@ -42,9 +42,8 @@ namespace robot_interaction { -visualization_msgs::InteractiveMarker makeEmptyInteractiveMarker(const std::string& name, - const geometry_msgs::PoseStamped& stamped, - double scale); +visualization_msgs::InteractiveMarker +makeEmptyInteractiveMarker(const std::string& name, const geometry_msgs::PoseStamped& stamped, double scale); visualization_msgs::InteractiveMarker make6DOFMarker(const std::string& name, const geometry_msgs::PoseStamped& stamped, double scale, bool orientation_fixed = false); diff --git a/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp b/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp index 9fcc48abc0..5dbcd93191 100644 --- a/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp +++ b/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp @@ -42,9 +42,8 @@ namespace robot_interaction { -visualization_msgs::InteractiveMarker makeEmptyInteractiveMarker(const std::string& name, - const geometry_msgs::PoseStamped& stamped, - double scale) +visualization_msgs::InteractiveMarker +makeEmptyInteractiveMarker(const std::string& name, const geometry_msgs::PoseStamped& stamped, double scale) { visualization_msgs::InteractiveMarker int_marker; int_marker.header = stamped.header; diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 9483f9c4cb..f2ebf2a4a1 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -93,8 +93,9 @@ void RobotInteraction::decideActiveComponents(const std::string& group, Interact decideActiveEndEffectors(group, style); decideActiveJoints(group); if (!group.empty() && active_eef_.empty() && active_vj_.empty() && active_generic_.empty()) - ROS_INFO_NAMED("robot_interaction", "No active joints or end effectors found for group '%s'. " - "Make sure that kinematics.yaml is loaded in this node's namespace.", + ROS_INFO_NAMED("robot_interaction", + "No active joints or end effectors found for group '%s'. " + "Make sure that kinematics.yaml is loaded in this node's namespace.", group.c_str()); } @@ -460,8 +461,7 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle im.name = getMarkerName(handler, active_generic_[i]); shown_markers_[im.name] = i; ims.push_back(im); - ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", im.name.c_str(), - im.scale); + ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", im.name.c_str(), im.scale); } } @@ -504,8 +504,7 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle active_eef_[i].interaction & InteractionStyle::ORIENTATION_EEF); ims.push_back(im); registerMoveInteractiveMarkerTopic(marker_name, handler->getName() + "_" + active_eef_[i].parent_link); - ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), - mscale); + ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), mscale); } for (std::size_t i = 0; i < active_vj_.size(); ++i) { @@ -529,8 +528,7 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle } ims.push_back(im); registerMoveInteractiveMarkerTopic(marker_name, handler->getName() + "_" + active_vj_[i].connecting_link); - ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), - mscale); + ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), mscale); } handlers_[handler->getName()] = handler; } @@ -541,8 +539,7 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle for (const visualization_msgs::InteractiveMarker& im : ims) { int_marker_server_->insert(im); - int_marker_server_->setCallback(im.name, - boost::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, _1)); + int_marker_server_->setCallback(im.name, boost::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, _1)); // Add menu handler to all markers that this interaction handler creates. if (std::shared_ptr mh = handler->getMenuHandler()) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 59f25f464d..0f87a8ca43 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -80,7 +80,7 @@ namespace moveit_warehouse MOVEIT_CLASS_FORWARD(PlanningSceneStorage); MOVEIT_CLASS_FORWARD(ConstraintsStorage); MOVEIT_CLASS_FORWARD(RobotStateStorage); -} +} // namespace moveit_warehouse namespace moveit_rviz_plugin { diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h index 8ae1258030..02a1fbc769 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h @@ -244,8 +244,8 @@ class JogSlider : public QSlider void mouseReleaseEvent(QMouseEvent* event) override; private: - using QSlider::setMinimum; using QSlider::setMaximum; + using QSlider::setMinimum; using QSlider::setRange; Q_SIGNALS: diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h index e423dc8272..0f273a527a 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h @@ -49,7 +49,7 @@ namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface); } -} +} // namespace moveit namespace moveit_rviz_plugin { diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 83530a5112..28fdb4b4a2 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -98,9 +98,10 @@ MotionPlanningDisplay::MotionPlanningDisplay() path_category_ = new rviz::Property("Planned Path", QVariant(), "", this); // Metrics category ----------------------------------------------------------------------------------------- - compute_weight_limit_property_ = new rviz::BoolProperty( - "Show Weight Limit", false, "Shows the weight limit at a particular pose for an end-effector", metrics_category_, - SLOT(changedShowWeightLimit()), this); + compute_weight_limit_property_ = + new rviz::BoolProperty("Show Weight Limit", false, + "Shows the weight limit at a particular pose for an end-effector", metrics_category_, + SLOT(changedShowWeightLimit()), this); show_manipulability_index_property_ = new rviz::BoolProperty("Show Manipulability Index", false, "Shows the manipulability index for an end-effector", @@ -125,11 +126,13 @@ MotionPlanningDisplay::MotionPlanningDisplay() // Planning request category ----------------------------------------------------------------------------------------- - planning_group_property_ = new rviz::EditableEnumProperty( - "Planning Group", "", "The name of the group of links to plan for (from the ones defined in the SRDF)", - plan_category_, SLOT(changedPlanningGroup()), this); - show_workspace_property_ = new rviz::BoolProperty("Show Workspace", false, "Shows the axis-aligned bounding box for " - "the workspace allowed for planning", + planning_group_property_ = + new rviz::EditableEnumProperty("Planning Group", "", + "The name of the group of links to plan for (from the ones defined in the SRDF)", + plan_category_, SLOT(changedPlanningGroup()), this); + show_workspace_property_ = new rviz::BoolProperty("Show Workspace", false, + "Shows the axis-aligned bounding box for " + "the workspace allowed for planning", plan_category_, SLOT(changedWorkspace()), this); query_start_state_property_ = new rviz::BoolProperty("Query Start State", false, "Set a custom start state for the motion planning query", @@ -217,8 +220,7 @@ void MotionPlanningDisplay::onInitialize() color.a = 1.0f; query_robot_start_->setDefaultAttachedObjectColor(color); - query_robot_goal_.reset( - new RobotStateVisualization(planning_scene_node_, context_, "Planning Request Goal", nullptr)); + query_robot_goal_.reset(new RobotStateVisualization(planning_scene_node_, context_, "Planning Request Goal", nullptr)); query_robot_goal_->setCollisionVisible(false); query_robot_goal_->setVisualVisible(true); query_robot_goal_->setVisible(query_goal_state_property_->getBool()); @@ -553,7 +555,7 @@ inline void copyItemIfExists(const std::map& source, std::m if (it != source.end()) dest[key] = it->second; } -} +} // namespace void MotionPlanningDisplay::displayMetrics(bool start) { @@ -1444,8 +1446,7 @@ void MotionPlanningDisplay::visualizePlaceLocations(const std::vectorgetSceneManager())); place_locations_display_[i]->setColor(1.0f, 0.0f, 0.0f, 0.3f); - Ogre::Vector3 center(place_poses[i].pose.position.x, place_poses[i].pose.position.y, - place_poses[i].pose.position.z); + Ogre::Vector3 center(place_poses[i].pose.position.x, place_poses[i].pose.position.y, place_poses[i].pose.position.z); Ogre::Vector3 extents(0.02, 0.02, 0.02); place_locations_display_[i]->setScale(extents); place_locations_display_[i]->setPosition(center); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 36625508f5..697f3698e6 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -58,8 +58,7 @@ namespace moveit_rviz_plugin { -MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, - QWidget* parent) +MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent) : QWidget(parent) , planning_display_(pdisplay) , context_(context) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp index b27d503181..038a0ef72d 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp @@ -70,8 +70,9 @@ void MotionPlanningFrame::planningAlgorithmIndexChanged(int index) void MotionPlanningFrame::resetDbButtonClicked() { - if (QMessageBox::warning(this, "Data about to be deleted", "The following dialog will allow you to drop a MoveIt " - "Warehouse database. Are you sure you want to continue?", + if (QMessageBox::warning(this, "Data about to be deleted", + "The following dialog will allow you to drop a MoveIt " + "Warehouse database. Are you sure you want to continue?", QMessageBox::Yes | QMessageBox::No) == QMessageBox::No) return; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index a8968549d7..00f0f09a70 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -128,8 +128,9 @@ void MotionPlanningFrame::publishScene() void MotionPlanningFrame::publishSceneIfNeeded() { if (isLocalSceneDirty() && - QMessageBox::question(this, "Update PlanningScene", "You have local changes to your planning scene.\n" - "Publish them to the move_group node?", + QMessageBox::question(this, "Update PlanningScene", + "You have local changes to your planning scene.\n" + "Publish them to the move_group node?", QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes) == QMessageBox::Yes) publishScene(); } @@ -829,8 +830,7 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item) const moveit::core::FixedTransformsMap subframes = obj->subframe_poses_; // Keep subframes // TODO(felixvd): Scale the subframes with the object ps->getWorldNonConst()->removeObject(obj->id_); - ps->getWorldNonConst()->addToObject(known_collision_objects_[item->type()].first, obj->shapes_, - obj->shape_poses_); + ps->getWorldNonConst()->addToObject(known_collision_objects_[item->type()].first, obj->shapes_, obj->shape_poses_); ps->getWorldNonConst()->setSubframesOfObject(obj->id_, subframes); if (scene_marker_) { @@ -848,9 +848,10 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item) if (ab) { known_collision_objects_[item->type()].first = item_text; - moveit::core::AttachedBody* new_ab = new moveit::core::AttachedBody( - ab->getAttachedLink(), known_collision_objects_[item->type()].first, ab->getShapes(), - ab->getFixedTransforms(), ab->getTouchLinks(), ab->getDetachPosture(), ab->getSubframeTransforms()); + moveit::core::AttachedBody* new_ab = + new moveit::core::AttachedBody(ab->getAttachedLink(), known_collision_objects_[item->type()].first, + ab->getShapes(), ab->getFixedTransforms(), ab->getTouchLinks(), + ab->getDetachPosture(), ab->getSubframeTransforms()); cs.clearAttachedBody(ab->getName()); cs.attachBody(new_ab); } @@ -1016,8 +1017,9 @@ void MotionPlanningFrame::computeImportGeometryFromText(const std::string& path) } else { - QMessageBox::warning(nullptr, "Loading scene geometry", "Failed to load scene geometry.\n" - "See console output for more details."); + QMessageBox::warning(nullptr, "Loading scene geometry", + "Failed to load scene geometry.\n" + "See console output for more details."); } } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 6ee3739253..b847da57b4 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -268,9 +268,9 @@ void MotionPlanningFrame::onNewPlanningSceneState() void MotionPlanningFrame::startStateTextChanged(const QString& start_state) { // use background job: fetching the current state might take up to a second - planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::startStateTextChangedExec, this, start_state.toStdString()), - "update start state"); + planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::startStateTextChangedExec, this, + start_state.toStdString()), + "update start state"); } void MotionPlanningFrame::startStateTextChangedExec(const std::string& start_state) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp index d2ab420ee3..e4a39f442f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp @@ -86,8 +86,9 @@ void MotionPlanningFrame::saveSceneButtonClicked() if (q->clickedButton() == rename.get()) { bool ok = false; - QString new_name = QInputDialog::getText(this, "Rename Planning Scene", "New name for the planning scene:", - QLineEdit::Normal, QString::fromStdString(name), &ok); + QString new_name = QInputDialog::getText(this, "Rename Planning Scene", + "New name for the planning scene:", QLineEdit::Normal, + QString::fromStdString(name), &ok); if (ok) { planning_display_->getPlanningSceneRW()->setName(new_name.toStdString()); @@ -161,9 +162,9 @@ void MotionPlanningFrame::saveQueryButtonClicked() if (q->clickedButton() == rename.get()) { bool ok = false; - QString new_name = - QInputDialog::getText(this, "Rename Planning Query", "New name for the planning query:", - QLineEdit::Normal, QString::fromStdString(query_name), &ok); + QString new_name = QInputDialog::getText(this, "Rename Planning Query", + "New name for the planning query:", QLineEdit::Normal, + QString::fromStdString(query_name), &ok); if (ok) query_name = new_name.toStdString(); else @@ -237,10 +238,11 @@ void MotionPlanningFrame::warehouseItemNameChanged(QTreeWidgetItem* item, int co if (planning_scene_storage->hasPlanningQuery(scene, new_name)) { planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); - QMessageBox::warning(this, "Query not renamed", QString("The query name '") - .append(item->text(column)) - .append("' already exists for scene ") - .append(item->parent()->text(0))); + QMessageBox::warning(this, "Query not renamed", + QString("The query name '") + .append(item->text(column)) + .append("' already exists for scene ") + .append(item->parent()->text(0))); return; } else diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp index 3e554c6b9f..b8b47752a9 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp @@ -137,9 +137,8 @@ void MotionPlanningFrame::saveRobotStateButtonClicked(const moveit::core::RobotS { name = text.toStdString(); if (robot_states_.find(name) != robot_states_.end()) - QMessageBox::warning( - this, "Name already exists", - QString("The name '").append(name.c_str()).append("' already exists. Not creating state.")); + QMessageBox::warning(this, "Name already exists", + QString("The name '").append(name.c_str()).append("' already exists. Not creating state.")); else { // Store the current start state @@ -161,8 +160,7 @@ void MotionPlanningFrame::saveRobotStateButtonClicked(const moveit::core::RobotS } else { - QMessageBox::warning(this, "Warning", - "Not connected to a database. The state will be created but not stored"); + QMessageBox::warning(this, "Warning", "Not connected to a database. The state will be created but not stored"); } } } diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 5be08a27d2..862e41ba63 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -66,12 +66,14 @@ namespace moveit_rviz_plugin PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool show_scene_robot) : Display(), model_is_loading_(false), planning_scene_needs_render_(true), current_scene_time_(0.0f) { - move_group_ns_property_ = new rviz::StringProperty("Move Group Namespace", "", "The name of the ROS namespace in " - "which the move_group node is running", + move_group_ns_property_ = new rviz::StringProperty("Move Group Namespace", "", + "The name of the ROS namespace in " + "which the move_group node is running", this, SLOT(changedMoveGroupNS()), this); - robot_description_property_ = new rviz::StringProperty( - "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", - this, SLOT(changedRobotDescription()), this); + robot_description_property_ = + new rviz::StringProperty("Robot Description", "robot_description", + "The name of the ROS parameter where the URDF for the robot is loaded", this, + SLOT(changedRobotDescription()), this); if (listen_to_planning_scene) planning_scene_topic_property_ = @@ -97,9 +99,10 @@ PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool s scene_alpha_property_->setMin(0.0); scene_alpha_property_->setMax(1.0); - scene_color_property_ = new rviz::ColorProperty( - "Scene Color", QColor(50, 230, 50), "The color for the planning scene obstacles (if a color is not defined)", - scene_category_, SLOT(changedSceneColor()), this); + scene_color_property_ = + new rviz::ColorProperty("Scene Color", QColor(50, 230, 50), + "The color for the planning scene obstacles (if a color is not defined)", scene_category_, + SLOT(changedSceneColor()), this); octree_render_property_ = new rviz::EnumProperty("Voxel Rendering", "Occupied Voxels", "Select voxel type.", scene_category_, SLOT(changedOctreeRenderMode()), this); @@ -114,25 +117,27 @@ PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool s octree_coloring_property_->addOption("Z-Axis", OCTOMAP_Z_AXIS_COLOR); octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR); - scene_display_time_property_ = - new rviz::FloatProperty("Scene Display Time", 0.01f, "The amount of wall-time to wait in between rendering " - "updates to the planning scene (if any)", - scene_category_, SLOT(changedSceneDisplayTime()), this); + scene_display_time_property_ = new rviz::FloatProperty("Scene Display Time", 0.01f, + "The amount of wall-time to wait in between rendering " + "updates to the planning scene (if any)", + scene_category_, SLOT(changedSceneDisplayTime()), this); scene_display_time_property_->setMin(0.0001); if (show_scene_robot) { robot_category_ = new rviz::Property("Scene Robot", QVariant(), "", this); - scene_robot_visual_enabled_property_ = new rviz::BoolProperty( - "Show Robot Visual", true, "Indicates whether the robot state specified by the planning scene should be " - "displayed as defined for visualisation purposes.", - robot_category_, SLOT(changedSceneRobotVisualEnabled()), this); + scene_robot_visual_enabled_property_ = + new rviz::BoolProperty("Show Robot Visual", true, + "Indicates whether the robot state specified by the planning scene should be " + "displayed as defined for visualisation purposes.", + robot_category_, SLOT(changedSceneRobotVisualEnabled()), this); - scene_robot_collision_enabled_property_ = new rviz::BoolProperty( - "Show Robot Collision", false, "Indicates whether the robot state specified by the planning scene should be " - "displayed as defined for collision detection purposes.", - robot_category_, SLOT(changedSceneRobotCollisionEnabled()), this); + scene_robot_collision_enabled_property_ = + new rviz::BoolProperty("Show Robot Collision", false, + "Indicates whether the robot state specified by the planning scene should be " + "displayed as defined for collision detection purposes.", + robot_category_, SLOT(changedSceneRobotCollisionEnabled()), this); robot_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 1.0f, "Specifies the alpha for the robot links", robot_category_, SLOT(changedRobotSceneAlpha()), this); @@ -531,8 +536,7 @@ void PlanningSceneDisplay::loadRobotModel() if (psm->getPlanningScene()) { planning_scene_monitor_.swap(psm); - planning_scene_monitor_->addUpdateCallback( - boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, _1)); + planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, _1)); addMainLoopJob(boost::bind(&PlanningSceneDisplay::onRobotModelLoaded, this)); setStatus(rviz::StatusProperty::Ok, "PlanningScene", "Planning Scene Loaded Successfully"); waitForAllMainLoopJobs(); diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h index f8f308190f..1ba9bdaca8 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h +++ b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h @@ -53,7 +53,7 @@ class BoolProperty; class FloatProperty; class RosTopicProperty; class ColorProperty; -} +} // namespace rviz namespace moveit_rviz_plugin { diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 20dd6f1f93..5ea133cbc5 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -65,14 +65,16 @@ namespace moveit_rviz_plugin // ****************************************************************************************** RobotStateDisplay::RobotStateDisplay() : Display(), update_state_(false), load_robot_model_(false) { - robot_description_property_ = new rviz::StringProperty( - "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", - this, SLOT(changedRobotDescription()), this); - - robot_state_topic_property_ = new rviz::RosTopicProperty( - "Robot State Topic", "display_robot_state", ros::message_traits::datatype(), - "The topic on which the moveit_msgs::DisplayRobotState messages are received", this, - SLOT(changedRobotStateTopic()), this); + robot_description_property_ = + new rviz::StringProperty("Robot Description", "robot_description", + "The name of the ROS parameter where the URDF for the robot is loaded", this, + SLOT(changedRobotDescription()), this); + + robot_state_topic_property_ = + new rviz::RosTopicProperty("Robot State Topic", "display_robot_state", + ros::message_traits::datatype(), + "The topic on which the moveit_msgs::DisplayRobotState messages are received", this, + SLOT(changedRobotStateTopic()), this); // Planning scene category ------------------------------------------------------------------------------------------- root_link_name_property_ = diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h index 87161aa639..7fcf9c2f0d 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h @@ -52,7 +52,7 @@ namespace rviz { class DisplayContext; class Shape; -} +} // namespace rviz namespace moveit_rviz_plugin { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 04d946e4a6..b8a62aabba 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -74,13 +74,15 @@ TrajectoryVisualization::TrajectoryVisualization(rviz::Property* widget, rviz::D SLOT(changedTrajectoryTopic()), this); display_path_visual_enabled_property_ = - new rviz::BoolProperty("Show Robot Visual", true, "Indicates whether the geometry of the robot as defined for " - "visualisation purposes should be displayed", + new rviz::BoolProperty("Show Robot Visual", true, + "Indicates whether the geometry of the robot as defined for " + "visualisation purposes should be displayed", widget, SLOT(changedDisplayPathVisualEnabled()), this); display_path_collision_enabled_property_ = - new rviz::BoolProperty("Show Robot Collision", false, "Indicates whether the geometry of the robot as defined " - "for collision detection purposes should be displayed", + new rviz::BoolProperty("Show Robot Collision", false, + "Indicates whether the geometry of the robot as defined " + "for collision detection purposes should be displayed", widget, SLOT(changedDisplayPathCollisionEnabled()), this); robot_path_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links", @@ -97,21 +99,24 @@ TrajectoryVisualization::TrajectoryVisualization(rviz::Property* widget, rviz::D state_display_time_property_->addOptionStd("0.1 s"); state_display_time_property_->addOptionStd("0.5 s"); - loop_display_property_ = new rviz::BoolProperty("Loop Animation", false, "Indicates whether the last received path " - "is to be animated in a loop", + loop_display_property_ = new rviz::BoolProperty("Loop Animation", false, + "Indicates whether the last received path " + "is to be animated in a loop", widget, SLOT(changedLoopDisplay()), this); trail_display_property_ = new rviz::BoolProperty("Show Trail", false, "Show a path trail", widget, SLOT(changedShowTrail()), this); - trail_step_size_property_ = new rviz::IntProperty("Trail Step Size", 1, "Specifies the step size of the samples " - "shown in the trajectory trail.", + trail_step_size_property_ = new rviz::IntProperty("Trail Step Size", 1, + "Specifies the step size of the samples " + "shown in the trajectory trail.", widget, SLOT(changedTrailStepSize()), this); trail_step_size_property_->setMin(1); - interrupt_display_property_ = new rviz::BoolProperty( - "Interrupt Display", false, - "Immediately show newly planned trajectory, interrupting the currently displayed one.", widget); + interrupt_display_property_ = + new rviz::BoolProperty("Interrupt Display", false, + "Immediately show newly planned trajectory, interrupting the currently displayed one.", + widget); robot_color_property_ = new rviz::ColorProperty( "Robot Color", QColor(150, 50, 150), "The color of the animated robot", widget, SLOT(changedRobotColor()), this); @@ -422,9 +427,8 @@ void TrajectoryVisualization::update(float wall_dt, float /*ros_dt*/) } else if (tm < 0.0) { // using realtime: skip to next waypoint based on elapsed display time - while (current_state_ < waypoint_count && - (tm = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1)) < - current_state_time_) + while (current_state_ < waypoint_count && (tm = displaying_trajectory_message_->getWayPointDurationFromPrevious( + current_state_ + 1)) < current_state_time_) { current_state_time_ -= tm; if (tm < current_state_time_) // if we are stuck in the while loop we should diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp index 2128e624e6..4b48d7ccac 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -45,9 +45,10 @@ TrajectoryDisplay::TrajectoryDisplay() : Display(), load_robot_model_(false) { // The robot description property is only needed when using the trajectory playback standalone (not within motion // planning plugin) - robot_description_property_ = new rviz::StringProperty( - "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", - this, SLOT(changedRobotDescription()), this); + robot_description_property_ = + new rviz::StringProperty("Robot Description", "robot_description", + "The name of the ROS parameter where the URDF for the robot is loaded", this, + SLOT(changedRobotDescription()), this); trajectory_visual_.reset(new TrajectoryVisualization(this, this)); } diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h index cc08a95d79..337a57dc52 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h @@ -58,8 +58,7 @@ class ConstraintsStorage : public MoveItMessageStorage ConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn); - void addConstraints(const moveit_msgs::Constraints& msg, const std::string& robot = "", - const std::string& group = ""); + void addConstraints(const moveit_msgs::Constraints& msg, const std::string& robot = "", const std::string& group = ""); bool hasConstraints(const std::string& name, const std::string& robot = "", const std::string& group = "") const; void getKnownConstraints(std::vector& names, const std::string& robot = "", const std::string& group = "") const; diff --git a/moveit_ros/warehouse/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/warehouse/src/broadcast.cpp index 53f8241a91..51954964b6 100644 --- a/moveit_ros/warehouse/warehouse/src/broadcast.cpp +++ b/moveit_ros/warehouse/warehouse/src/broadcast.cpp @@ -61,10 +61,11 @@ int main(int argc, char** argv) double delay = 0.001; boost::program_options::options_description desc; - desc.add_options()("help", "Show help message")("host", boost::program_options::value(), "Host for the " - "DB.")( - "port", boost::program_options::value(), - "Port for the DB.")("scene", boost::program_options::value(), "Name of scene to publish.")( + desc.add_options()("help", "Show help message")("host", boost::program_options::value(), + "Host for the " + "DB.")("port", boost::program_options::value(), + "Port for the DB.")( + "scene", boost::program_options::value(), "Name of scene to publish.")( "planning_requests", "Also publish the planning requests that correspond to the scene")( "planning_results", "Also publish the planning results that correspond to the scene")( "constraint", boost::program_options::value(), "Name of constraint to publish.")( diff --git a/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp b/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp index e357870b33..8dab206f3e 100644 --- a/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp +++ b/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp @@ -55,9 +55,10 @@ int main(int argc, char** argv) ros::init(argc, argv, "initialize_demo_db", ros::init_options::AnonymousName); boost::program_options::options_description desc; - desc.add_options()("help", "Show help message")("host", boost::program_options::value(), "Host for the " - "DB.")( - "port", boost::program_options::value(), "Port for the DB."); + desc.add_options()("help", "Show help message")("host", boost::program_options::value(), + "Host for the " + "DB.")("port", boost::program_options::value(), + "Port for the DB."); boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); diff --git a/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp b/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp index 3511f8e8b6..a7ff64e064 100644 --- a/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp +++ b/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp @@ -49,8 +49,7 @@ moveit_warehouse::MoveItMessageStorage::MoveItMessageStorage(warehouse_ros::Data { } -void moveit_warehouse::MoveItMessageStorage::filterNames(const std::string& regex, - std::vector& names) const +void moveit_warehouse::MoveItMessageStorage::filterNames(const std::string& regex, std::vector& names) const { if (!regex.empty()) { diff --git a/moveit_ros/warehouse/warehouse/src/planning_scene_storage.cpp b/moveit_ros/warehouse/warehouse/src/planning_scene_storage.cpp index 18bffebbf1..4010ee5b00 100644 --- a/moveit_ros/warehouse/warehouse/src/planning_scene_storage.cpp +++ b/moveit_ros/warehouse/warehouse/src/planning_scene_storage.cpp @@ -92,8 +92,9 @@ bool moveit_warehouse::PlanningSceneStorage::hasPlanningScene(const std::string& return !planning_scenes.empty(); } -std::string moveit_warehouse::PlanningSceneStorage::getMotionPlanRequestName( - const moveit_msgs::MotionPlanRequest& planning_query, const std::string& scene_name) const +std::string +moveit_warehouse::PlanningSceneStorage::getMotionPlanRequestName(const moveit_msgs::MotionPlanRequest& planning_query, + const std::string& scene_name) const { // get all existing motion planning requests for this planning scene Query::Ptr q = motion_plan_request_collection_->createQuery(); diff --git a/moveit_ros/warehouse/warehouse/src/save_as_text.cpp b/moveit_ros/warehouse/warehouse/src/save_as_text.cpp index 1154fbd7f8..eb5df51ca1 100644 --- a/moveit_ros/warehouse/warehouse/src/save_as_text.cpp +++ b/moveit_ros/warehouse/warehouse/src/save_as_text.cpp @@ -79,9 +79,10 @@ int main(int argc, char** argv) ros::init(argc, argv, "save_warehouse_as_text", ros::init_options::AnonymousName); boost::program_options::options_description desc; - desc.add_options()("help", "Show help message")("host", boost::program_options::value(), "Host for the " - "DB.")( - "port", boost::program_options::value(), "Port for the DB."); + desc.add_options()("help", "Show help message")("host", boost::program_options::value(), + "Host for the " + "DB.")("port", boost::program_options::value(), + "Port for the DB."); boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); diff --git a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp index fe5b2e8713..4b1afa32a3 100644 --- a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp @@ -117,9 +117,10 @@ int main(int argc, char** argv) ros::init(argc, argv, "save_to_warehouse", ros::init_options::AnonymousName); boost::program_options::options_description desc; - desc.add_options()("help", "Show help message")("host", boost::program_options::value(), "Host for the " - "DB.")( - "port", boost::program_options::value(), "Port for the DB."); + desc.add_options()("help", "Show help message")("host", boost::program_options::value(), + "Host for the " + "DB.")("port", boost::program_options::value(), + "Port for the DB."); boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); diff --git a/moveit_ros/warehouse/warehouse/src/trajectory_constraints_storage.cpp b/moveit_ros/warehouse/warehouse/src/trajectory_constraints_storage.cpp index e2983b22ab..49bc25504b 100644 --- a/moveit_ros/warehouse/warehouse/src/trajectory_constraints_storage.cpp +++ b/moveit_ros/warehouse/warehouse/src/trajectory_constraints_storage.cpp @@ -47,8 +47,7 @@ const std::string moveit_warehouse::TrajectoryConstraintsStorage::ROBOT_NAME = " using warehouse_ros::Metadata; using warehouse_ros::Query; -moveit_warehouse::TrajectoryConstraintsStorage::TrajectoryConstraintsStorage( - warehouse_ros::DatabaseConnection::Ptr conn) +moveit_warehouse::TrajectoryConstraintsStorage::TrajectoryConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn) : MoveItMessageStorage(std::move(conn)) { createCollections(); diff --git a/moveit_setup_assistant/src/tools/collision_linear_model.cpp b/moveit_setup_assistant/src/tools/collision_linear_model.cpp index 66fd858872..f28bd9390a 100644 --- a/moveit_setup_assistant/src/tools/collision_linear_model.cpp +++ b/moveit_setup_assistant/src/tools/collision_linear_model.cpp @@ -262,7 +262,7 @@ bool operator<(const QVariant& left, const QVariant& right) else return left.toString() < right.toString(); } -} +} // namespace #endif bool SortFilterProxyModel::lessThan(const QModelIndex& src_left, const QModelIndex& src_right) const diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index c1b7f324a5..ff2e3aa276 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -598,9 +598,10 @@ std::vector MoveItConfigData::getOMPLPlanners() // TODO: remove when ROS Melodic and older are no longer supported #if OMPL_VERSION_VALUE >= 1005000 // This parameter was added in OMPL 1.5.0 - aps.addParameter("planners", "", "A comma-separated list of planner types (e.g., \"PRM,EST,RRTConnect\"" - "Optionally, planner parameters can be passed to change the default:" - "\"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]\""); + aps.addParameter("planners", "", + "A comma-separated list of planner types (e.g., \"PRM,EST,RRTConnect\"" + "Optionally, planner parameters can be passed to change the default:" + "\"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]\""); #endif planner_des.push_back(aps); @@ -614,28 +615,33 @@ std::vector MoveItConfigData::getOMPLPlanners() planner_des.push_back(est); OMPLPlannerDescription lbkpiece("LBKPIECE", "geometric"); - lbkpiece.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + lbkpiece.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); lbkpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9"); lbkpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); planner_des.push_back(lbkpiece); OMPLPlannerDescription bkpiece("BKPIECE", "geometric"); - bkpiece.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + bkpiece.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); bkpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9"); - bkpiece.addParameter("failed_expansion_score_factor", "0.5", "When extending motion fails, scale score by factor. " - "default: 0.5"); + bkpiece.addParameter("failed_expansion_score_factor", "0.5", + "When extending motion fails, scale score by factor. " + "default: 0.5"); bkpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); planner_des.push_back(bkpiece); OMPLPlannerDescription kpiece("KPIECE", "geometric"); - kpiece.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + kpiece.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); kpiece.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); kpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9 (0.0,1.]"); - kpiece.addParameter("failed_expansion_score_factor", "0.5", "When extending motion fails, scale score by factor. " - "default: 0.5"); + kpiece.addParameter("failed_expansion_score_factor", "0.5", + "When extending motion fails, scale score by factor. " + "default: 0.5"); kpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); planner_des.push_back(kpiece); @@ -645,16 +651,19 @@ std::vector MoveItConfigData::getOMPLPlanners() planner_des.push_back(rrt); OMPLPlannerDescription rrt_connect("RRTConnect", "geometric"); - rrt_connect.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + rrt_connect.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); planner_des.push_back(rrt_connect); OMPLPlannerDescription rr_tstar("RRTstar", "geometric"); - rr_tstar.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + rr_tstar.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); rr_tstar.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); - rr_tstar.addParameter("delay_collision_checking", "1", "Stop collision checking as soon as C-free parent found. " - "default 1"); + rr_tstar.addParameter("delay_collision_checking", "1", + "Stop collision checking as soon as C-free parent found. " + "default 1"); planner_des.push_back(rr_tstar); OMPLPlannerDescription trrt("TRRT", "geometric"); @@ -664,8 +673,9 @@ std::vector MoveItConfigData::getOMPLPlanners() trrt.addParameter("temp_change_factor", "2.0", "how much to increase or decrease temp. default: 2.0"); trrt.addParameter("min_temperature", "10e-10", "lower limit of temp change. default: 10e-10"); trrt.addParameter("init_temperature", "10e-6", "initial temperature. default: 10e-6"); - trrt.addParameter("frountier_threshold", "0.0", "dist new state to nearest neighbor to disqualify as frontier. " - "default: 0.0 set in setup()"); + trrt.addParameter("frountier_threshold", "0.0", + "dist new state to nearest neighbor to disqualify as frontier. " + "default: 0.0 set in setup()"); trrt.addParameter("frountierNodeRatio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1"); trrt.addParameter("k_constant", "0.0", "value used to normalize expresssion. default: 0.0 set in setup()"); planner_des.push_back(trrt); @@ -683,24 +693,29 @@ std::vector MoveItConfigData::getOMPLPlanners() fmt.addParameter("nearest_k", "1", "use Knearest strategy. default: 1"); fmt.addParameter("cache_cc", "1", "use collision checking cache. default: 1"); fmt.addParameter("heuristics", "0", "activate cost to go heuristics. default: 0"); - fmt.addParameter("extended_fmt", "1", "activate the extended FMT*: adding new samples if planner does not finish " - "successfully. default: 1"); + fmt.addParameter("extended_fmt", "1", + "activate the extended FMT*: adding new samples if planner does not finish " + "successfully. default: 1"); planner_des.push_back(fmt); OMPLPlannerDescription bfmt("BFMT", "geometric"); bfmt.addParameter("num_samples", "1000", "number of states that the planner should sample. default: 1000"); - bfmt.addParameter("radius_multiplier", "1.0", "multiplier used for the nearest neighbors search radius. default: " - "1.0"); + bfmt.addParameter("radius_multiplier", "1.0", + "multiplier used for the nearest neighbors search radius. default: " + "1.0"); bfmt.addParameter("nearest_k", "1", "use the Knearest strategy. default: 1"); - bfmt.addParameter("balanced", "0", "exploration strategy: balanced true expands one tree every iteration. False will " - "select the tree with lowest maximum cost to go. default: 1"); - bfmt.addParameter("optimality", "1", "termination strategy: optimality true finishes when the best possible path is " - "found. Otherwise, the algorithm will finish when the first feasible path is " - "found. default: 1"); + bfmt.addParameter("balanced", "0", + "exploration strategy: balanced true expands one tree every iteration. False will " + "select the tree with lowest maximum cost to go. default: 1"); + bfmt.addParameter("optimality", "1", + "termination strategy: optimality true finishes when the best possible path is " + "found. Otherwise, the algorithm will finish when the first feasible path is " + "found. default: 1"); bfmt.addParameter("heuristics", "1", "activates cost to go heuristics. default: 1"); bfmt.addParameter("cache_cc", "1", "use the collision checking cache. default: 1"); - bfmt.addParameter("extended_fmt", "1", "Activates the extended FMT*: adding new samples if planner does not finish " - "successfully. default: 1"); + bfmt.addParameter("extended_fmt", "1", + "Activates the extended FMT*: adding new samples if planner does not finish " + "successfully. default: 1"); planner_des.push_back(bfmt); OMPLPlannerDescription pdst("PDST", "geometric"); @@ -708,14 +723,17 @@ std::vector MoveItConfigData::getOMPLPlanners() planner_des.push_back(pdst); OMPLPlannerDescription stride("STRIDE", "geometric"); - stride.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + stride.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); stride.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); - stride.addParameter("use_projected_distance", "0", "whether nearest neighbors are computed based on distances in a " - "projection of the state rather distances in the state space " - "itself. default: 0"); - stride.addParameter("degree", "16", "desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). " - "default: 16"); + stride.addParameter("use_projected_distance", "0", + "whether nearest neighbors are computed based on distances in a " + "projection of the state rather distances in the state space " + "itself. default: 0"); + stride.addParameter("degree", "16", + "desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). " + "default: 16"); stride.addParameter("max_degree", "18", "max degree of a node in the GNAT. default: 12"); stride.addParameter("min_degree", "12", "min degree of a node in the GNAT. default: 12"); stride.addParameter("max_pts_per_leaf", "6", "max points per leaf in the GNAT. default: 6"); @@ -724,57 +742,67 @@ std::vector MoveItConfigData::getOMPLPlanners() planner_des.push_back(stride); OMPLPlannerDescription bi_trrt("BiTRRT", "geometric"); - bi_trrt.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + bi_trrt.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); bi_trrt.addParameter("temp_change_factor", "0.1", "how much to increase or decrease temp. default: 0.1"); bi_trrt.addParameter("init_temperature", "100", "initial temperature. default: 100"); - bi_trrt.addParameter("frountier_threshold", "0.0", "dist new state to nearest neighbor to disqualify as frontier. " - "default: 0.0 set in setup()"); + bi_trrt.addParameter("frountier_threshold", "0.0", + "dist new state to nearest neighbor to disqualify as frontier. " + "default: 0.0 set in setup()"); bi_trrt.addParameter("frountier_node_ratio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1"); - bi_trrt.addParameter("cost_threshold", "1e300", "the cost threshold. Any motion cost that is not better will not be " - "expanded. default: inf"); + bi_trrt.addParameter("cost_threshold", "1e300", + "the cost threshold. Any motion cost that is not better will not be " + "expanded. default: inf"); planner_des.push_back(bi_trrt); OMPLPlannerDescription lbtrrt("LBTRRT", "geometric"); - lbtrrt.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + lbtrrt.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); lbtrrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); lbtrrt.addParameter("epsilon", "0.4", "optimality approximation factor. default: 0.4"); planner_des.push_back(lbtrrt); OMPLPlannerDescription bi_est("BiEST", "geometric"); - bi_est.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + bi_est.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); planner_des.push_back(bi_est); OMPLPlannerDescription proj_est("ProjEST", "geometric"); - proj_est.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + proj_est.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); proj_est.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); planner_des.push_back(proj_est); OMPLPlannerDescription lazy_prm("LazyPRM", "geometric"); - lazy_prm.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + lazy_prm.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); planner_des.push_back(lazy_prm); OMPLPlannerDescription lazy_pr_mstar("LazyPRMstar", "geometric"); // no declares in code planner_des.push_back(lazy_pr_mstar); OMPLPlannerDescription spars("SPARS", "geometric"); - spars.addParameter("stretch_factor", "3.0", "roadmap spanner stretch factor. multiplicative upper bound on path " - "quality. It does not make sense to make this parameter more than 3. " - "default: 3.0"); - spars.addParameter("sparse_delta_fraction", "0.25", "delta fraction for connection distance. This value represents " - "the visibility range of sparse samples. default: 0.25"); + spars.addParameter("stretch_factor", "3.0", + "roadmap spanner stretch factor. multiplicative upper bound on path " + "quality. It does not make sense to make this parameter more than 3. " + "default: 3.0"); + spars.addParameter("sparse_delta_fraction", "0.25", + "delta fraction for connection distance. This value represents " + "the visibility range of sparse samples. default: 0.25"); spars.addParameter("dense_delta_fraction", "0.001", "delta fraction for interface detection. default: 0.001"); spars.addParameter("max_failures", "1000", "maximum consecutive failure limit. default: 1000"); planner_des.push_back(spars); OMPLPlannerDescription spar_stwo("SPARStwo", "geometric"); - spar_stwo.addParameter("stretch_factor", "3.0", "roadmap spanner stretch factor. multiplicative upper bound on path " - "quality. It does not make sense to make this parameter more than 3. " - "default: 3.0"); + spar_stwo.addParameter("stretch_factor", "3.0", + "roadmap spanner stretch factor. multiplicative upper bound on path " + "quality. It does not make sense to make this parameter more than 3. " + "default: 3.0"); spar_stwo.addParameter("sparse_delta_fraction", "0.25", "delta fraction for connection distance. This value represents " "the visibility range of sparse samples. default: 0.25"); @@ -853,7 +881,7 @@ srdf::Model::GroupState MoveItConfigData::getDefaultStartPose() if (!srdf_->group_states_.empty()) return srdf_->group_states_[0]; else - return srdf::Model::GroupState{.name_ = "todo_state_name", .group_ = "todo_group_name", .joint_values_ = {} }; + return srdf::Model::GroupState{ .name_ = "todo_state_name", .group_ = "todo_group_name", .joint_values_ = {} }; } // ****************************************************************************************** @@ -1185,7 +1213,7 @@ class SortableDisabledCollision : dc_(dc), key_(dc.link1_ < dc.link2_ ? (dc.link1_ + "|" + dc.link2_) : (dc.link2_ + "|" + dc.link1_)) { } - operator const srdf::Model::DisabledCollision&() const + operator const srdf::Model::DisabledCollision &() const { return dc_; } @@ -1875,8 +1903,9 @@ srdf::Model::Group* MoveItConfigData::findGroupByName(const std::string& name) // Check if subgroup was found if (searched_group == nullptr) // not found - ROS_FATAL_STREAM("An internal error has occured while searching for groups. Group '" << name << "' was not found " - "in the SRDF."); + ROS_FATAL_STREAM("An internal error has occured while searching for groups. Group '" << name + << "' was not found " + "in the SRDF."); return searched_group; } diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 8bab6e4f4c..38ce197bd8 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -687,8 +687,9 @@ void ConfigurationFilesWidget::changeCheckedState(QListWidgetItem* item) if (!generate && (gen_files_[index].write_on_changes & config_data_->changes)) { - QMessageBox::warning(this, "Package Generation", "You should generate this file to ensure your changes will take " - "effect."); + QMessageBox::warning(this, "Package Generation", + "You should generate this file to ensure your changes will take " + "effect."); } // Enable/disable file @@ -883,8 +884,9 @@ bool ConfigurationFilesWidget::generatePackage() // Check that a valid stack package name has been given if (new_package_path.empty()) { - QMessageBox::warning(this, "Error Generating", "No package path provided. Please choose a directory location to " - "generate the MoveIt configuration files."); + QMessageBox::warning(this, "Error Generating", + "No package path provided. Please choose a directory location to " + "generate the MoveIt configuration files."); return false; } @@ -920,12 +922,12 @@ bool ConfigurationFilesWidget::generatePackage() } // Confirm overwrite - if (QMessageBox::question( - this, "Confirm Package Update", - QString("Are you sure you want to overwrite this existing package with updated configurations?
") - .append(new_package_path.c_str()) - .append(""), - QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) + if (QMessageBox::question(this, "Confirm Package Update", + QString("Are you sure you want to overwrite this existing package with updated " + "configurations?
") + .append(new_package_path.c_str()) + .append(""), + QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) { return false; // abort } @@ -969,10 +971,11 @@ bool ConfigurationFilesWidget::generatePackage() if (!file->gen_func_(absolute_path)) { // Error occured - QMessageBox::critical(this, "Error Generating File", QString("Failed to generate folder or file: '") - .append(file->rel_path_.c_str()) - .append("' at location:\n") - .append(absolute_path.c_str())); + QMessageBox::critical(this, "Error Generating File", + QString("Failed to generate folder or file: '") + .append(file->rel_path_.c_str()) + .append("' at location:\n") + .append(absolute_path.c_str())); return false; } updateProgress(); // Increment and update GUI @@ -986,10 +989,9 @@ bool ConfigurationFilesWidget::generatePackage() // ****************************************************************************************** void ConfigurationFilesWidget::exitSetupAssistant() { - if (has_generated_pkg_ || - QMessageBox::question(this, "Exit Setup Assistant", - QString("Are you sure you want to exit the MoveIt Setup Assistant?"), - QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Ok) + if (has_generated_pkg_ || QMessageBox::question(this, "Exit Setup Assistant", + QString("Are you sure you want to exit the MoveIt Setup Assistant?"), + QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Ok) { QApplication::quit(); } diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp index 9f8bdeb1b3..d6b2a05c98 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp @@ -379,10 +379,9 @@ void DefaultCollisionsWidget::collisionsChanged(const QModelIndex& index) // Hm. For some reason, QTableView doesn't change selection if we click a checkbox bool linear_mode = (view_mode_buttons_->checkedId() == LINEAR_MODE); const QItemSelection& selection = selection_model_->selection(); - if ((linear_mode && !selection.contains(index)) || // in linear mode: index not in selection - (!linear_mode && - !(selection.contains(index) || // in matrix mode: index or symmetric index not in selection - selection.contains(model_->index(index.column(), index.row()))))) + if ((linear_mode && !selection.contains(index)) || // in linear mode: index not in selection + (!linear_mode && !(selection.contains(index) || // in matrix mode: index or symmetric index not in selection + selection.contains(model_->index(index.column(), index.row()))))) { QItemSelectionModel::SelectionFlags flags = QItemSelectionModel::Select | QItemSelectionModel::Current; if (linear_mode) diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.h b/moveit_setup_assistant/src/widgets/default_collisions_widget.h index 105dcb754c..b78ecd61ad 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.h +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.h @@ -139,8 +139,8 @@ private Q_SLOTS: void revertChanges(); /** - * \brief Called when current row has changed - */ + * \brief Called when current row has changed + */ void previewSelectedMatrix(const QModelIndex& index); void previewSelectedLinear(const QModelIndex& index); diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp index 602469a32c..effbe5a7ce 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp @@ -54,12 +54,12 @@ EndEffectorsWidget::EndEffectorsWidget(QWidget* parent, const MoveItConfigDataPt // Top Header Area ------------------------------------------------ - HeaderWidget* header = - new HeaderWidget("Define End Effectors", "Setup your robot's end effectors. These are planning groups " - "corresponding to grippers or tools, attached to a parent " - "planning group (an arm). The specified parent link is used as the " - "reference frame for IK attempts.", - this); + HeaderWidget* header = new HeaderWidget("Define End Effectors", + "Setup your robot's end effectors. These are planning groups " + "corresponding to grippers or tools, attached to a parent " + "planning group (an arm). The specified parent link is used as the " + "reference frame for IK attempts.", + this); layout->addWidget(header); // Create contents screens --------------------------------------- diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp index 3ecd04a16c..c89c5c77c5 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp @@ -324,8 +324,9 @@ void GroupEditWidget::loadKinematicPlannersComboBox() } catch (pluginlib::PluginlibException& ex) { - QMessageBox::warning(this, "Missing Kinematic Solvers", "Exception while creating class loader for kinematic " - "solver plugins"); + QMessageBox::warning(this, "Missing Kinematic Solvers", + "Exception while creating class loader for kinematic " + "solver plugins"); ROS_ERROR_STREAM(ex.what()); return; } @@ -336,9 +337,10 @@ void GroupEditWidget::loadKinematicPlannersComboBox() // Warn if no plugins are found if (classes.empty()) { - QMessageBox::warning(this, "Missing Kinematic Solvers", "No MoveIt-compatible kinematics solvers found. Try " - "installing moveit_kinematics (sudo apt-get install " - "ros-${ROS_DISTRO}-moveit-kinematics)"); + QMessageBox::warning(this, "Missing Kinematic Solvers", + "No MoveIt-compatible kinematics solvers found. Try " + "installing moveit_kinematics (sudo apt-get install " + "ros-${ROS_DISTRO}-moveit-kinematics)"); return; } diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp b/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp index f586963da9..7ca2d4275e 100644 --- a/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp @@ -53,10 +53,10 @@ PassiveJointsWidget::PassiveJointsWidget(QWidget* parent, const MoveItConfigData // Top Header Area ------------------------------------------------ - HeaderWidget* header = - new HeaderWidget("Define Passive Joints", "Specify the set of passive joints (not actuated). Joint " - "state is not expected to be published for these joints.", - this); + HeaderWidget* header = new HeaderWidget("Define Passive Joints", + "Specify the set of passive joints (not actuated). Joint " + "state is not expected to be published for these joints.", + this); layout->addWidget(header); // Joints edit widget diff --git a/moveit_setup_assistant/src/widgets/perception_widget.cpp b/moveit_setup_assistant/src/widgets/perception_widget.cpp index 6af5bc8dfe..11662d4722 100644 --- a/moveit_setup_assistant/src/widgets/perception_widget.cpp +++ b/moveit_setup_assistant/src/widgets/perception_widget.cpp @@ -208,8 +208,7 @@ bool PerceptionWidget::focusLost() "PointCloudOctomapUpdater"); config_data_->addGenericParameterToSensorPluginConfig("point_cloud_topic", point_cloud_topic_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("max_range", - max_range_field_->text().trimmed().toStdString()); + config_data_->addGenericParameterToSensorPluginConfig("max_range", max_range_field_->text().trimmed().toStdString()); config_data_->addGenericParameterToSensorPluginConfig("point_subsample", point_subsample_field_->text().trimmed().toStdString()); config_data_->addGenericParameterToSensorPluginConfig("padding_offset", diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index e3da0c9ec4..304b53becc 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -416,11 +416,12 @@ void PlanningGroupsWidget::loadGroupsTreeRecursive(srdf::Model::Group& group_it, // Check if subgroup was found if (searched_group == nullptr) // not found { - QMessageBox::critical(this, "Error Loading SRDF", QString("Subgroup '") - .append(subgroup_it->c_str()) - .append("' of group '") - .append(group_it.name_.c_str()) - .append("' not found. Your SRDF is invalid")); + QMessageBox::critical(this, "Error Loading SRDF", + QString("Subgroup '") + .append(subgroup_it->c_str()) + .append("' of group '") + .append(group_it.name_.c_str()) + .append("' not found. Your SRDF is invalid")); return; // TODO: something better for error handling? } @@ -589,10 +590,11 @@ void PlanningGroupsWidget::loadChainScreen(srdf::Model::Group* this_group) // Make sure there isn't more than 1 chain pair if (this_group->chains_.size() > 1) { - QMessageBox::warning(this, "Multiple Kinematic Chains", "Warning: This setup assistant is only designed to handle " - "one kinematic chain per group. The loaded SRDF has more " - "than one kinematic chain for a group. A possible loss of " - "data may occur."); + QMessageBox::warning(this, "Multiple Kinematic Chains", + "Warning: This setup assistant is only designed to handle " + "one kinematic chain per group. The loaded SRDF has more " + "than one kinematic chain for a group. A possible loss of " + "data may occur."); } // Set the selected tip and base of chain if one exists @@ -702,12 +704,12 @@ void PlanningGroupsWidget::deleteGroup() srdf::Model::Group* searched_group = config_data_->findGroupByName(group); // Confirm user wants to delete group - if (QMessageBox::question( - this, "Confirm Group Deletion", - QString("Are you sure you want to delete the planning group '") - .append(searched_group->name_.c_str()) - .append("'? This will also delete all references in subgroups, robot poses and end effectors."), - QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) + if (QMessageBox::question(this, "Confirm Group Deletion", + QString("Are you sure you want to delete the planning group '") + .append(searched_group->name_.c_str()) + .append("'? This will also delete all references in subgroups, robot poses and end " + "effectors."), + QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) { return; } @@ -918,8 +920,9 @@ void PlanningGroupsWidget::saveChainScreen() // Check that box the tip and base, or neither, have text if ((!tip.empty() && base.empty()) || (tip.empty() && !base.empty())) { - QMessageBox::warning(this, "Error Saving", "You must specify a link for both the base and tip, or leave both " - "blank."); + QMessageBox::warning(this, "Error Saving", + "You must specify a link for both the base and tip, or leave both " + "blank."); return; } @@ -1197,8 +1200,8 @@ bool PlanningGroupsWidget::saveGroupScreen() // Check if this eef's parent group references old group name. if so, update it if (eef_it->parent_group_.compare(old_group_name) == 0) // same name { - ROS_DEBUG_STREAM_NAMED("setup_assistant", "Changed eef '" << eef_it->name_ << "' to new parent group name " - << group_name); + ROS_DEBUG_STREAM_NAMED("setup_assistant", + "Changed eef '" << eef_it->name_ << "' to new parent group name " << group_name); eef_it->parent_group_ = group_name; // updated config_data_->changes |= MoveItConfigData::END_EFFECTORS; } @@ -1206,8 +1209,8 @@ bool PlanningGroupsWidget::saveGroupScreen() // Check if this eef's group references old group name. if so, update it if (eef_it->component_group_.compare(old_group_name) == 0) // same name { - ROS_DEBUG_STREAM_NAMED("setup_assistant", "Changed eef '" << eef_it->name_ << "' to new group name " - << group_name); + ROS_DEBUG_STREAM_NAMED("setup_assistant", + "Changed eef '" << eef_it->name_ << "' to new group name " << group_name); eef_it->component_group_ = group_name; // updated config_data_->changes |= MoveItConfigData::END_EFFECTORS; } diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index e68b132194..d89889fd9c 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -62,11 +62,12 @@ RobotPosesWidget::RobotPosesWidget(QWidget* parent, const MoveItConfigDataPtr& c // Top Header Area ------------------------------------------------ - HeaderWidget* header = new HeaderWidget( - "Define Robot Poses", "Create poses for the robot. Poses are defined as sets of joint values for " - "particular planning groups. This is useful for things like home position." - "The first pose for each robot will be its initial pose in simulation.", - this); + HeaderWidget* header = + new HeaderWidget("Define Robot Poses", + "Create poses for the robot. Poses are defined as sets of joint values for " + "particular planning groups. This is useful for things like home position." + "The first pose for each robot will be its initial pose in simulation.", + this); layout->addWidget(header); // Create contents screens --------------------------------------- @@ -488,9 +489,10 @@ void RobotPosesWidget::loadJointSliders(const QString& selected) // Check that joint model exist if (!config_data_->getRobotModel()->hasJointModelGroup(group_name)) { - QMessageBox::critical(this, "Error Loading", QString("Unable to find joint model group for group: ") - .append(group_name.c_str()) - .append(" Are you sure this group has associated joints/links?")); + QMessageBox::critical(this, "Error Loading", + QString("Unable to find joint model group for group: ") + .append(group_name.c_str()) + .append(" Are you sure this group has associated joints/links?")); return; } diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index ff61771a14..edd152495c 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -426,8 +426,7 @@ void ROSControllersWidget::loadControllerScreen(moveit_setup_assistant::ROSContr else // load the controller name into the widget { current_edit_controller_ = this_controller->name_; - controller_edit_widget_->setTitle( - QString("Edit Controller '").append(current_edit_controller_.c_str()).append("'")); + controller_edit_widget_->setTitle(QString("Edit Controller '").append(current_edit_controller_.c_str()).append("'")); controller_edit_widget_->showDelete(); controller_edit_widget_->hideNewButtonsWidget(); // not necessary for existing controllers controller_edit_widget_->showSave(); // this is only for edit mode diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h index 20ab7cc114..45dd02b684 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h @@ -78,7 +78,7 @@ namespace rviz class GridDisplay; class RenderPanel; class VisualizationManager; -} +} // namespace rviz namespace moveit_rviz_plugin { diff --git a/moveit_setup_assistant/src/widgets/simulation_widget.cpp b/moveit_setup_assistant/src/widgets/simulation_widget.cpp index 14b9d5a805..0f917a7f87 100644 --- a/moveit_setup_assistant/src/widgets/simulation_widget.cpp +++ b/moveit_setup_assistant/src/widgets/simulation_widget.cpp @@ -61,11 +61,11 @@ SimulationWidget::SimulationWidget(QWidget* parent, const MoveItConfigDataPtr& c // Top Header Area ------------------------------------------------ - HeaderWidget* header = - new HeaderWidget("Simulate With Gazebo", "The following tool will auto-generate the URDF changes needed " - "for Gazebo compatibility with ROSControl and MoveIt. The " - "needed changes are shown in green.", - this); + HeaderWidget* header = new HeaderWidget("Simulate With Gazebo", + "The following tool will auto-generate the URDF changes needed " + "for Gazebo compatibility with ROSControl and MoveIt. The " + "needed changes are shown in green.", + this); layout->addWidget(header); // Spacing diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 6910917c35..96af347714 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -110,11 +110,11 @@ StartScreenWidget::StartScreenWidget(QWidget* parent, const MoveItConfigDataPtr& right_layout->setAlignment(right_image_label_, Qt::AlignRight | Qt::AlignTop); // Top Label Area --------------------------------------------------- - HeaderWidget* header = - new HeaderWidget("MoveIt Setup Assistant", "These tools will assist you in creating a Semantic Robot " - "Description Format (SRDF) file, various yaml configuration and many " - "roslaunch files for utilizing all aspects of MoveIt functionality.", - this); + HeaderWidget* header = new HeaderWidget("MoveIt Setup Assistant", + "These tools will assist you in creating a Semantic Robot " + "Description Format (SRDF) file, various yaml configuration and many " + "roslaunch files for utilizing all aspects of MoveIt functionality.", + this); layout->addWidget(header); // Select Mode Area ------------------------------------------------- @@ -337,11 +337,11 @@ bool StartScreenWidget::loadPackageSettings(bool show_warnings) if (!config_data_->getSetupAssistantYAMLPath(setup_assistant_path)) { if (show_warnings) - QMessageBox::warning( - this, "Incorrect Directory/Package", - QString("The chosen package location exists but was not created using MoveIt Setup Assistant. " - "If this is a mistake, provide the missing file: ") - .append(setup_assistant_path.c_str())); + QMessageBox::warning(this, "Incorrect Directory/Package", + QString( + "The chosen package location exists but was not created using MoveIt Setup Assistant. " + "If this is a mistake, provide the missing file: ") + .append(setup_assistant_path.c_str())); return false; } @@ -502,8 +502,9 @@ bool StartScreenWidget::loadNewFiles() QApplication::processEvents(); // Create blank SRDF file - const std::string blank_srdf = ""; + const std::string blank_srdf = ""; // Load a blank SRDF file to the parameter server if (!setSRDFFile(blank_srdf)) @@ -687,10 +688,11 @@ bool StartScreenWidget::createFullURDFPath() { if (config_data_->urdf_path_.empty()) // no path could be resolved { - QMessageBox::warning(this, "Error Loading Files", QString("ROS was unable to find the package name '") - .append(config_data_->urdf_pkg_name_.c_str()) - .append("'. Verify this package is inside your ROS " - "workspace and is a proper ROS package.")); + QMessageBox::warning(this, "Error Loading Files", + QString("ROS was unable to find the package name '") + .append(config_data_->urdf_pkg_name_.c_str()) + .append("'. Verify this package is inside your ROS " + "workspace and is a proper ROS package.")); } else { diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp index 7203ee29fd..4227d6a984 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp @@ -54,10 +54,10 @@ VirtualJointsWidget::VirtualJointsWidget(QWidget* parent, const MoveItConfigData // Top Header Area ------------------------------------------------ - HeaderWidget* header = - new HeaderWidget("Define Virtual Joints", "Create a virtual joint between a robot link and an external frame of " - "reference (considered fixed with respect to the robot).", - this); + HeaderWidget* header = new HeaderWidget("Define Virtual Joints", + "Create a virtual joint between a robot link and an external frame of " + "reference (considered fixed with respect to the robot).", + this); layout->addWidget(header); // Create contents screens --------------------------------------- diff --git a/moveit_setup_assistant/test/moveit_config_data_test.cpp b/moveit_setup_assistant/test/moveit_config_data_test.cpp index 9cd489015d..c776a50a9c 100644 --- a/moveit_setup_assistant/test/moveit_config_data_test.cpp +++ b/moveit_setup_assistant/test/moveit_config_data_test.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2018, Mohamad Ayman. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Mohamad Ayman. + * 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 the 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. + *********************************************************************/ /* Author: Mohamad Ayman */ #include From 3e4d3ac3f0bad99f4321978c0775f66459723cbf Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 11 Aug 2020 12:56:09 -0500 Subject: [PATCH 418/612] A ROS service to reset the Servo status (#2246) --- moveit_ros/moveit_servo/CMakeLists.txt | 2 ++ .../moveit_servo/include/moveit_servo/servo_calcs.h | 7 ++++++- moveit_ros/moveit_servo/package.xml | 1 + moveit_ros/moveit_servo/src/servo_calcs.cpp | 11 +++++++++++ 4 files changed, 20 insertions(+), 1 deletion(-) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 93bc20ba88..6540e55da0 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(catkin REQUIRED COMPONENTS rosparam_shortcuts sensor_msgs std_msgs + std_srvs trajectory_msgs ) find_package(Eigen3 REQUIRED) @@ -40,6 +41,7 @@ catkin_package( rosparam_shortcuts sensor_msgs std_msgs + std_srvs trajectory_msgs DEPENDS EIGEN3 diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index d991aee0e5..c67afb6e0c 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -44,8 +44,9 @@ #include #include #include -#include #include +#include +#include #include #include #include @@ -186,6 +187,9 @@ class ServoCalcs bool changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, moveit_msgs::ChangeControlDimensions::Response& res); + /** \brief Service callback to reset Servo status, e.g. so the arm can move again after a collision */ + bool resetServoStatus(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + ros::NodeHandle nh_; // Parameters from yaml @@ -244,6 +248,7 @@ class ServoCalcs ros::Publisher outgoing_cmd_pub_; ros::ServiceServer drift_dimensions_server_; ros::ServiceServer control_dimensions_server_; + ros::ServiceServer reset_servo_status_; // Status StatusCode status_ = StatusCode::NO_WARNING; diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index bb7824858f..7b7ccea44e 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -24,6 +24,7 @@ geometry_msgs sensor_msgs std_msgs + std_srvs trajectory_msgs moveit_msgs moveit_ros_planning_interface diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index b6d4fd774e..8dd7f08844 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -108,6 +108,11 @@ ServoCalcs::ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", &ServoCalcs::changeControlDimensions, this); + // ROS Server to reset the status, e.g. so the arm can move again after a collision + reset_servo_status_ = + nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/reset_servo_status", + &ServoCalcs::resetServoStatus, this); + // Publish and Subscribe to internal namespace topics ros::NodeHandle internal_nh("~internal"); collision_velocity_scale_sub_ = @@ -1049,6 +1054,12 @@ bool ServoCalcs::changeControlDimensions(moveit_msgs::ChangeControlDimensions::R return true; } +bool ServoCalcs::resetServoStatus(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) +{ + status_ = StatusCode::NO_WARNING; + return true; +} + void ServoCalcs::setPaused(bool paused) { paused_ = paused; From 9f28498ee0c3e9afeb5e4ecd19e214c432a360bc Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 7 Jul 2020 10:39:16 +0200 Subject: [PATCH 419/612] Remove moveit_resources from CMakeLists.txt There is no reason to include them from cmake anymore. The resources are not in a single path anymore and there is no C header exposed anymore. --- moveit_commander/package.xml | 2 +- moveit_commander/test/CMakeLists.txt | 1 - moveit_core/collision_detection_bullet/CMakeLists.txt | 3 --- moveit_core/collision_detection_fcl/CMakeLists.txt | 3 --- moveit_core/collision_distance_field/CMakeLists.txt | 3 --- moveit_core/constraint_samplers/CMakeLists.txt | 3 +-- moveit_core/kinematic_constraints/CMakeLists.txt | 3 --- moveit_core/package.xml | 3 ++- moveit_core/planning_scene/CMakeLists.txt | 3 --- moveit_core/robot_model/CMakeLists.txt | 3 --- moveit_core/robot_state/CMakeLists.txt | 3 --- moveit_core/robot_state/test/test_cartesian_interpolator.cpp | 1 - moveit_core/robot_trajectory/CMakeLists.txt | 3 --- moveit_core/trajectory_processing/CMakeLists.txt | 2 -- moveit_kinematics/package.xml | 5 ++++- moveit_kinematics/test/CMakeLists.txt | 1 - moveit_planners/ompl/ompl_interface/CMakeLists.txt | 3 --- moveit_planners/ompl/package.xml | 2 +- moveit_ros/move_group/package.xml | 2 +- moveit_ros/moveit_servo/package.xml | 2 +- moveit_ros/planning_interface/package.xml | 3 ++- moveit_ros/planning_interface/test/CMakeLists.txt | 1 - moveit_setup_assistant/CMakeLists.txt | 2 -- moveit_setup_assistant/package.xml | 2 +- 24 files changed, 14 insertions(+), 45 deletions(-) diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml index 6e6b7fb51d..8b277c578d 100644 --- a/moveit_commander/package.xml +++ b/moveit_commander/package.xml @@ -34,5 +34,5 @@ tf rostest - moveit_resources + moveit_resources_fanuc_moveit_config
diff --git a/moveit_commander/test/CMakeLists.txt b/moveit_commander/test/CMakeLists.txt index 157406a545..b097091a89 100644 --- a/moveit_commander/test/CMakeLists.txt +++ b/moveit_commander/test/CMakeLists.txt @@ -1,5 +1,4 @@ if (CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) find_package(rostest REQUIRED) add_rostest(python_moveit_commander.test) diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index 799fead00d..66a1e09e4b 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -28,9 +28,6 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(FILES ../collision_detector_bullet_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_bullet_collision_detection test/test_bullet_collision_detection_pr2.cpp) target_link_libraries(test_bullet_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index 3889763ac4..90f260c8aa 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -24,9 +24,6 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(FILES ../collision_detector_fcl_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection_pr2.cpp) target_link_libraries(test_fcl_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) diff --git a/moveit_core/collision_distance_field/CMakeLists.txt b/moveit_core/collision_distance_field/CMakeLists.txt index 063a1331a3..25c7292649 100644 --- a/moveit_core/collision_distance_field/CMakeLists.txt +++ b/moveit_core/collision_distance_field/CMakeLists.txt @@ -18,9 +18,6 @@ target_link_libraries(${MOVEIT_LIB_NAME} add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_collision_distance_field test/test_collision_distance_field.cpp) target_link_libraries(test_collision_distance_field ${MOVEIT_LIB_NAME}) endif() diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index 0cb5fab70e..1c3d645c85 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -28,9 +28,8 @@ if(CATKIN_ENABLE_TESTING) find_package(orocos_kdl REQUIRED) find_package(angles REQUIRED) find_package(tf2_kdl REQUIRED) - find_package(moveit_resources REQUIRED) - include_directories(SYSTEM ${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS}) + include_directories(SYSTEM ${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS}) catkin_add_gtest(test_constraint_samplers test/test_constraint_samplers.cpp diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index 18ff796f70..619f7a4229 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -18,9 +18,6 @@ install(TARGETS ${MOVEIT_LIB_NAME} install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_constraints test/test_constraints.cpp) target_link_libraries(test_constraints moveit_test_utils ${MOVEIT_LIB_NAME}) endif() diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 1ddc19cb5f..6703fe348f 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -51,7 +51,8 @@ visualization_msgs xmlrpcpp - moveit_resources + moveit_resources_panda_moveit_config + moveit_resources_pr2_description angles tf2_kdl orocos_kdl diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt index 3ecfc4831a..0c132f21b7 100644 --- a/moveit_core/planning_scene/CMakeLists.txt +++ b/moveit_core/planning_scene/CMakeLists.txt @@ -23,9 +23,6 @@ install(TARGETS ${MOVEIT_LIB_NAME} install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_planning_scene test/test_planning_scene.cpp) target_link_libraries(test_planning_scene ${MOVEIT_LIB_NAME}) diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index 40a6199da4..fbd4162f04 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -18,9 +18,6 @@ target_link_libraries(${MOVEIT_LIB_NAME} moveit_profiler moveit_exceptions movei add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_robot_model test/test.cpp) target_link_libraries(test_robot_model moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) endif() diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index 90b30c9db7..fa3c36b45f 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -25,9 +25,6 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) # Unit tests if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_robot_state test/robot_state_test.cpp) target_link_libraries(test_robot_state moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index ca771677de..57c0ac22c0 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -34,7 +34,6 @@ /* Author: Michael Lautman */ -#include #include #include #include diff --git a/moveit_core/robot_trajectory/CMakeLists.txt b/moveit_core/robot_trajectory/CMakeLists.txt index 4f2a069d91..078572677c 100644 --- a/moveit_core/robot_trajectory/CMakeLists.txt +++ b/moveit_core/robot_trajectory/CMakeLists.txt @@ -14,9 +14,6 @@ install(TARGETS ${MOVEIT_LIB_NAME} install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_robot_trajectory test/test_robot_trajectory.cpp) target_link_libraries(test_robot_trajectory moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) endif() diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index 7c51bfbec5..e595724de0 100644 --- a/moveit_core/trajectory_processing/CMakeLists.txt +++ b/moveit_core/trajectory_processing/CMakeLists.txt @@ -19,8 +19,6 @@ install(TARGETS ${MOVEIT_LIB_NAME} install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) catkin_add_gtest(test_time_parameterization test/test_time_parameterization.cpp) target_link_libraries(test_time_parameterization moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) catkin_add_gtest(test_time_optimal_trajectory_generation test/test_time_optimal_trajectory_generation.cpp) diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index a6bf3b78bd..99ab7c3324 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -35,7 +35,10 @@ rostest moveit_ros_planning - moveit_resources + moveit_resources_fanuc_description + moveit_resources_fanuc_moveit_config + moveit_resources_panda_description + moveit_resources_panda_moveit_config xmlrpcpp diff --git a/moveit_kinematics/test/CMakeLists.txt b/moveit_kinematics/test/CMakeLists.txt index 1b8dc7139e..e99ce09920 100644 --- a/moveit_kinematics/test/CMakeLists.txt +++ b/moveit_kinematics/test/CMakeLists.txt @@ -1,5 +1,4 @@ if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) find_package(rostest REQUIRED) find_package(xmlrpcpp REQUIRED) find_package(moveit_ros_planning REQUIRED) diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index 9e9bc57664..81b82b9cc5 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -45,9 +45,6 @@ install(TARGETS moveit_generate_state_database install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_state_space test/test_state_space.cpp) target_link_libraries(test_state_space ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) set_target_properties(test_state_space PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index 1efad63a75..937ad51a10 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -25,7 +25,7 @@ tf2_ros pluginlib - moveit_resources + moveit_resources_pr2_description rosunit diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index 39fdfe514a..3a70092778 100644 --- a/moveit_ros/move_group/package.xml +++ b/moveit_ros/move_group/package.xml @@ -30,7 +30,7 @@ moveit_kinematics rostest - moveit_resources + moveit_resources_fanuc_moveit_config diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index bb7824858f..8186c03e21 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -33,7 +33,7 @@ spacenav_node rostest - moveit_resources + moveit_resources_panda_moveit_config diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index 5a864a72c8..ad49898100 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -40,7 +40,8 @@ eigen - moveit_resources + moveit_resources_fanuc_moveit_config + moveit_resources_panda_moveit_config eigen_conversions rostest eigen_conversions diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 8cb31d6004..1a616bdd88 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -1,5 +1,4 @@ if (CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) find_package(rostest REQUIRED) find_package(eigen_conversions REQUIRED) diff --git a/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/CMakeLists.txt index fbac89054f..8ffbe601c9 100644 --- a/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/CMakeLists.txt @@ -163,8 +163,6 @@ install(DIRECTORY templates DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) # Unit tests if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) catkin_add_gtest(test_reading_writing_config test/moveit_config_data_test.cpp) target_link_libraries(test_reading_writing_config ${PROJECT_NAME}_tools ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${MOVEIT_LIB_NAME}) diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml index 9b73230f22..30ea4d9812 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/package.xml @@ -36,6 +36,6 @@ xacro rviz - moveit_resources + moveit_resources_panda_moveit_config rosunit
From 21ff2e28bca8641decf149c975cd1f751c6f865f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 7 Jul 2020 15:42:43 +0200 Subject: [PATCH 420/612] Replace moveit_resources/ with moveit_resources_ --- moveit_commander/test/python_moveit_commander.test | 2 +- moveit_commander/test/python_moveit_commander_ns.test | 2 +- .../test/python_time_parameterization.test | 2 +- .../collision_detection/test_collision_common_pr2.h | 2 +- .../test_bullet_continuous_collision_checking.cpp | 2 +- .../test/test_collision_distance_field.cpp | 4 ++-- moveit_core/robot_state/test/test_aabb.cpp | 2 +- .../include/moveit/utils/robot_model_test_utils.h | 2 +- moveit_kinematics/test/fanuc-ikfast.test | 2 +- moveit_kinematics/test/fanuc-kdl.test | 2 +- moveit_kinematics/test/panda-ikfast.test | 2 +- moveit_kinematics/test/panda-kdl.test | 2 +- moveit_planners/trajopt/test/trajectory_test.test | 2 +- moveit_ros/benchmarks/examples/demo_fanuc.launch | 6 +++--- moveit_ros/benchmarks/examples/demo_panda.launch | 6 +++--- .../examples/demo_panda_all_planners.launch | 10 +++++----- .../examples/demo_panda_all_planners_obstacles.launch | 10 +++++----- .../examples/demo_panda_predefined_poses.launch | 11 +++++------ .../test/test_cancel_before_plan_execution.test | 4 ++-- .../moveit_servo/test/servo_cpp_interface_test.test | 2 +- .../planning/launch/collision_checker_compare.launch | 8 ++++---- .../compare_collision_speed_checking_fcl_bullet.cpp | 2 +- moveit_ros/planning_interface/test/cleanup.test | 2 +- .../test/move_group_interface_cpp_test.test | 4 ++-- .../test/move_group_pick_place_test.test | 4 ++-- .../planning_interface/test/moveit_cpp_test.test | 8 ++++---- .../planning_interface/test/python_move_group.test | 2 +- .../planning_interface/test/python_move_group_ns.test | 2 +- .../planning_interface/test/robot_state_update.test | 2 +- .../launch/run_benchmark_trajopt.launch | 8 ++++---- .../test/moveit_config_data_test.cpp | 7 ++++--- 31 files changed, 63 insertions(+), 63 deletions(-) diff --git a/moveit_commander/test/python_moveit_commander.test b/moveit_commander/test/python_moveit_commander.test index c97a74198c..eb99c5935d 100644 --- a/moveit_commander/test/python_moveit_commander.test +++ b/moveit_commander/test/python_moveit_commander.test @@ -1,5 +1,5 @@ - + diff --git a/moveit_commander/test/python_moveit_commander_ns.test b/moveit_commander/test/python_moveit_commander_ns.test index feda452a7e..eefed9aec6 100644 --- a/moveit_commander/test/python_moveit_commander_ns.test +++ b/moveit_commander/test/python_moveit_commander_ns.test @@ -1,5 +1,5 @@ - + diff --git a/moveit_commander/test/python_time_parameterization.test b/moveit_commander/test/python_time_parameterization.test index 61c5fa50fd..3a83385699 100644 --- a/moveit_commander/test/python_time_parameterization.test +++ b/moveit_commander/test/python_time_parameterization.test @@ -1,5 +1,5 @@ - + diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h index b8f3f8e22e..75030d8a56 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h @@ -65,7 +65,7 @@ class CollisionDetectorTest : public ::testing::Test value_.reset(new CollisionAllocatorType); robot_model_ = moveit::core::loadTestingRobotModel("pr2"); robot_model_ok_ = static_cast(robot_model_); - kinect_dae_resource_ = "package://moveit_resources/pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae"; + kinect_dae_resource_ = "package://moveit_resources_pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae"; acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp index 3e9d99cef7..8af1398cc6 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp @@ -183,7 +183,7 @@ void addCollisionObjectsMesh(cb::BulletCastBVHManager& checker) Eigen::Isometry3d s_pose; s_pose.setIdentity(); - std::string kinect = "package://moveit_resources/panda_description/meshes/collision/hand.stl"; + std::string kinect = "package://moveit_resources_panda_description/meshes/collision/hand.stl"; shapes::ShapeConstPtr s; s.reset(shapes::createMeshFromResource(kinect)); obj2_shapes.push_back(s); diff --git a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp index 55908d27c3..31ec0bb15f 100644 --- a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp +++ b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp @@ -61,7 +61,7 @@ class DistanceFieldCollisionDetectionTester : public testing::Test { srdf_model_.reset(new srdf::Model()); std::string xml_string; - std::fstream xml_file(ros::package::getPath("moveit_resources") + "/pr2_description/urdf/robot.xml", + std::fstream xml_file(ros::package::getPath("moveit_resources_pr2_description") + "/urdf/robot.xml", std::fstream::in); if (xml_file.is_open()) { @@ -78,7 +78,7 @@ class DistanceFieldCollisionDetectionTester : public testing::Test else urdf_ok_ = false; srdf_ok_ = srdf_model_->initFile(*urdf_model_, - ros::package::getPath("moveit_resources") + "/pr2_description/srdf/robot.xml"); + ros::package::getPath("moveit_resources_pr2_description") + "/srdf/robot.xml"); robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_model_)); diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index 8ec7a20eb3..97d89b46a1 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -278,7 +278,7 @@ TEST_F(TestAABB, TestSimple) builder.addChain("base_footprint->base_link", "fixed", { origin }); tf2::toMsg(tf2::Vector3(0, 0, 0), origin.position); - builder.addCollisionMesh("base_link", "package://moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl", + builder.addCollisionMesh("base_link", "package://moveit_resources_pr2_description/urdf/meshes/base_v0/base_L.stl", origin); tf2::toMsg(tf2::Vector3(0, 0, 0.071), origin.position); diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h index a22d6c635a..22ec072354 100644 --- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h @@ -113,7 +113,7 @@ class RobotModelBuilder /** \brief Adds a collision mesh to a specific link. * \param[in] link_name The name of the link to which the mesh will be added. Must already be in the builder * \param[in] filename The path to the mesh file, e.g. - * "package://moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl" + * "package://moveit_resources_pr2_description/urdf/meshes/base_v0/base_L.stl" * \param[in] origin The origin pose of this collision mesh relative to the link origin */ void addCollisionMesh(const std::string& link_name, const std::string& filename, geometry_msgs::Pose origin); diff --git a/moveit_kinematics/test/fanuc-ikfast.test b/moveit_kinematics/test/fanuc-ikfast.test index a5d09f891c..9de03483d5 100644 --- a/moveit_kinematics/test/fanuc-ikfast.test +++ b/moveit_kinematics/test/fanuc-ikfast.test @@ -1,7 +1,7 @@ - + diff --git a/moveit_kinematics/test/fanuc-kdl.test b/moveit_kinematics/test/fanuc-kdl.test index 6b44ced4d1..9b4fccb953 100644 --- a/moveit_kinematics/test/fanuc-kdl.test +++ b/moveit_kinematics/test/fanuc-kdl.test @@ -6,7 +6,7 @@ - + diff --git a/moveit_kinematics/test/panda-ikfast.test b/moveit_kinematics/test/panda-ikfast.test index 2094d88ddc..0359d5d096 100644 --- a/moveit_kinematics/test/panda-ikfast.test +++ b/moveit_kinematics/test/panda-ikfast.test @@ -1,7 +1,7 @@ - + diff --git a/moveit_kinematics/test/panda-kdl.test b/moveit_kinematics/test/panda-kdl.test index b7b52de74c..95a331cff8 100644 --- a/moveit_kinematics/test/panda-kdl.test +++ b/moveit_kinematics/test/panda-kdl.test @@ -6,7 +6,7 @@ - + diff --git a/moveit_planners/trajopt/test/trajectory_test.test b/moveit_planners/trajopt/test/trajectory_test.test index 347b97a1d9..bec08fda3d 100644 --- a/moveit_planners/trajopt/test/trajectory_test.test +++ b/moveit_planners/trajopt/test/trajectory_test.test @@ -3,7 +3,7 @@ - + diff --git a/moveit_ros/benchmarks/examples/demo_fanuc.launch b/moveit_ros/benchmarks/examples/demo_fanuc.launch index cfecc6f5c5..7fa8f55c94 100644 --- a/moveit_ros/benchmarks/examples/demo_fanuc.launch +++ b/moveit_ros/benchmarks/examples/demo_fanuc.launch @@ -3,17 +3,17 @@ - + - + - + diff --git a/moveit_ros/benchmarks/examples/demo_panda.launch b/moveit_ros/benchmarks/examples/demo_panda.launch index 07ab802d6f..08a1c33c9d 100644 --- a/moveit_ros/benchmarks/examples/demo_panda.launch +++ b/moveit_ros/benchmarks/examples/demo_panda.launch @@ -3,17 +3,17 @@ - + - + - + diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch b/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch index adbbed130f..6cb02f534b 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch @@ -3,24 +3,24 @@ - + - + - + - + - + diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch b/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch index 1ae08adebf..46f3e1a5c5 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch @@ -3,24 +3,24 @@ - + - + - + - + - + diff --git a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch index 91fb0448c5..1f04aedd5e 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch +++ b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch @@ -3,24 +3,24 @@ - + - + - + - + - + @@ -30,4 +30,3 @@ - diff --git a/moveit_ros/move_group/test/test_cancel_before_plan_execution.test b/moveit_ros/move_group/test/test_cancel_before_plan_execution.test index edf4699d93..ef3c39eb55 100644 --- a/moveit_ros/move_group/test/test_cancel_before_plan_execution.test +++ b/moveit_ros/move_group/test/test_cancel_before_plan_execution.test @@ -1,6 +1,6 @@ - + @@ -14,7 +14,7 @@ - + diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test index f34d8503da..f165250d82 100644 --- a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test @@ -1,7 +1,7 @@ - + diff --git a/moveit_ros/planning/launch/collision_checker_compare.launch b/moveit_ros/planning/launch/collision_checker_compare.launch index 5208ade9cd..1c180c13e6 100644 --- a/moveit_ros/planning/launch/collision_checker_compare.launch +++ b/moveit_ros/planning/launch/collision_checker_compare.launch @@ -2,15 +2,15 @@ - + - + - + - + diff --git a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp index 2f0f13edaa..c658792d55 100644 --- a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp +++ b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp @@ -100,7 +100,7 @@ void clutterWorld(const planning_scene::PlanningScenePtr& planning_scene, const // load panda link5 as world collision object std::string name; shapes::ShapeConstPtr shape; - std::string kinect = "package://moveit_resources/panda_description/meshes/collision/link5.stl"; + std::string kinect = "package://moveit_resources_panda_description/meshes/collision/link5.stl"; Eigen::Quaterniond quat; Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; diff --git a/moveit_ros/planning_interface/test/cleanup.test b/moveit_ros/planning_interface/test/cleanup.test index b1a2064369..775ea18666 100644 --- a/moveit_ros/planning_interface/test/cleanup.test +++ b/moveit_ros/planning_interface/test/cleanup.test @@ -1,5 +1,5 @@ - + diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test index b1612e213d..9975f257d2 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test @@ -1,6 +1,6 @@ - + @@ -17,7 +17,7 @@ - + diff --git a/moveit_ros/planning_interface/test/move_group_pick_place_test.test b/moveit_ros/planning_interface/test/move_group_pick_place_test.test index 17dd3c179b..9dd7dbdcc3 100644 --- a/moveit_ros/planning_interface/test/move_group_pick_place_test.test +++ b/moveit_ros/planning_interface/test/move_group_pick_place_test.test @@ -1,6 +1,6 @@ - + @@ -17,7 +17,7 @@ - + diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.test b/moveit_ros/planning_interface/test/moveit_cpp_test.test index b6e5f894ef..306d97553c 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.test +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.test @@ -3,18 +3,18 @@ - + - - + - + diff --git a/moveit_ros/planning_interface/test/python_move_group.test b/moveit_ros/planning_interface/test/python_move_group.test index 01c6651892..4367da03ed 100644 --- a/moveit_ros/planning_interface/test/python_move_group.test +++ b/moveit_ros/planning_interface/test/python_move_group.test @@ -1,7 +1,7 @@ - + - + diff --git a/moveit_ros/planning_interface/test/robot_state_update.test b/moveit_ros/planning_interface/test/robot_state_update.test index e282f0ab22..f7849d7d7c 100644 --- a/moveit_ros/planning_interface/test/robot_state_update.test +++ b/moveit_ros/planning_interface/test/robot_state_update.test @@ -1,5 +1,5 @@ - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch index d411006e39..50631869af 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch @@ -4,19 +4,19 @@ - + - + - - + + diff --git a/moveit_setup_assistant/test/moveit_config_data_test.cpp b/moveit_setup_assistant/test/moveit_config_data_test.cpp index c776a50a9c..ac03cc86c5 100644 --- a/moveit_setup_assistant/test/moveit_config_data_test.cpp +++ b/moveit_setup_assistant/test/moveit_config_data_test.cpp @@ -51,11 +51,11 @@ class MoveItConfigData : public testing::Test protected: void SetUp() override { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); + boost::filesystem::path res_path(ros::package::getPath("moveit_resources_panda_description")); srdf_model.reset(new srdf::Model()); std::string xml_string; - std::fstream xml_file((res_path / "panda_description/urdf/panda.urdf").string().c_str(), std::fstream::in); + std::fstream xml_file((res_path / "urdf/panda.urdf").string().c_str(), std::fstream::in); if (xml_file.is_open()) { while (xml_file.good()) @@ -67,7 +67,8 @@ class MoveItConfigData : public testing::Test xml_file.close(); urdf_model = urdf::parseURDF(xml_string); } - srdf_model->initFile(*urdf_model, (res_path / "panda_moveit_config/config/panda.srdf").string()); + res_path = ros::package::getPath("moveit_resources_panda_moveit_config"); + srdf_model->initFile(*urdf_model, (res_path / "config/panda.srdf").string()); robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model)); }; From e8728c44af55e3a4387b0dfbb1c86c1be9918020 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 7 Jul 2020 21:49:04 +0200 Subject: [PATCH 421/612] Fix loading of test resources - Convert code to use robot_model_test_utils - Fixup loading in robot_model_test_utils --- .../collision_distance_field/CMakeLists.txt | 2 +- .../test/test_collision_distance_field.cpp | 32 +-------- moveit_core/planning_scene/CMakeLists.txt | 4 +- .../test/test_planning_scene.cpp | 67 ++++--------------- moveit_core/robot_state/CMakeLists.txt | 10 +-- .../test/test_kinematic_complex.cpp | 25 ++----- .../moveit/utils/robot_model_test_utils.h | 5 -- .../utils/src/robot_model_test_utils.cpp | 15 +++-- .../ompl/ompl_interface/CMakeLists.txt | 2 +- .../ompl_interface/test/test_state_space.cpp | 24 +------ moveit_setup_assistant/CMakeLists.txt | 3 +- .../test/moveit_config_data_test.cpp | 36 +++------- 12 files changed, 50 insertions(+), 175 deletions(-) diff --git a/moveit_core/collision_distance_field/CMakeLists.txt b/moveit_core/collision_distance_field/CMakeLists.txt index 25c7292649..2c7696896d 100644 --- a/moveit_core/collision_distance_field/CMakeLists.txt +++ b/moveit_core/collision_distance_field/CMakeLists.txt @@ -19,7 +19,7 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_collision_distance_field test/test_collision_distance_field.cpp) - target_link_libraries(test_collision_distance_field ${MOVEIT_LIB_NAME}) + target_link_libraries(test_collision_distance_field ${MOVEIT_LIB_NAME} moveit_test_utils) endif() install(TARGETS ${MOVEIT_LIB_NAME} diff --git a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp index 31ec0bb15f..affec2291e 100644 --- a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp +++ b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -59,28 +60,7 @@ class DistanceFieldCollisionDetectionTester : public testing::Test protected: void SetUp() override { - srdf_model_.reset(new srdf::Model()); - std::string xml_string; - std::fstream xml_file(ros::package::getPath("moveit_resources_pr2_description") + "/urdf/robot.xml", - std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - urdf_model_ = urdf::parseURDF(xml_string); - urdf_ok_ = static_cast(urdf_model_); - } - else - urdf_ok_ = false; - srdf_ok_ = srdf_model_->initFile(*urdf_model_, - ros::package::getPath("moveit_resources_pr2_description") + "/srdf/robot.xml"); - - robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_model_)); + robot_model_ = moveit::core::loadTestingRobotModel("pr2"); acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); @@ -93,12 +73,6 @@ class DistanceFieldCollisionDetectionTester : public testing::Test } protected: - bool urdf_ok_; - bool srdf_ok_; - - urdf::ModelInterfaceSharedPtr urdf_model_; - srdf::ModelSharedPtr srdf_model_; - moveit::core::RobotModelPtr robot_model_; moveit::core::TransformsPtr ftf_; @@ -115,8 +89,6 @@ TEST_F(DistanceFieldCollisionDetectionTester, DefaultNotInCollision) robot_state.setToDefaultValues(); robot_state.update(); - ASSERT_TRUE(urdf_ok_ && srdf_ok_); - collision_detection::CollisionRequest req; collision_detection::CollisionResult res; req.group_name = "whole_body"; diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt index 0c132f21b7..3f233fb98e 100644 --- a/moveit_core/planning_scene/CMakeLists.txt +++ b/moveit_core/planning_scene/CMakeLists.txt @@ -24,8 +24,8 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_planning_scene test/test_planning_scene.cpp) - target_link_libraries(test_planning_scene ${MOVEIT_LIB_NAME}) + target_link_libraries(test_planning_scene ${MOVEIT_LIB_NAME} moveit_test_utils) catkin_add_gtest(test_multi_threaded test/test_multi_threaded.cpp) - target_link_libraries(test_multi_threaded moveit_planning_scene moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES}) + target_link_libraries(test_multi_threaded ${MOVEIT_LIB_NAME} moveit_test_utils) endif() diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 37cddc3296..28dece6b9a 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -44,42 +45,9 @@ #include #include -// This function needs to return void so the gtest FAIL() macro inside -// it works right. -void loadModelFile(std::string filename, std::string& file_content) -{ - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); - std::string xml_string; - std::fstream xml_file((res_path / filename).string().c_str(), std::fstream::in); - EXPECT_TRUE(xml_file.is_open()); - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - file_content = xml_string; -} - -void loadRobotModel(urdf::ModelInterfaceSharedPtr& robot_model_out) -{ - std::string xml_string; - loadModelFile("pr2_description/urdf/robot.xml", xml_string); - robot_model_out = urdf::parseURDF(xml_string); -} -void loadRobotModels(urdf::ModelInterfaceSharedPtr& robot_model_out, srdf::ModelSharedPtr& srdf_model_out) -{ - loadRobotModel(robot_model_out); - std::string xml_string; - loadModelFile("pr2_description/srdf/robot.xml", xml_string); - srdf_model_out->initString(*robot_model_out, xml_string); -} - TEST(PlanningScene, LoadRestore) { - urdf::ModelInterfaceSharedPtr urdf_model; - loadRobotModel(urdf_model); + urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); srdf::ModelSharedPtr srdf_model(new srdf::Model()); planning_scene::PlanningScene ps(urdf_model, srdf_model); moveit_msgs::PlanningScene ps_msg; @@ -93,11 +61,9 @@ TEST(PlanningScene, LoadRestore) TEST(PlanningScene, LoadRestoreDiff) { - urdf::ModelInterfaceSharedPtr urdf_model; - loadRobotModel(urdf_model); + urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); srdf::ModelSharedPtr srdf_model(new srdf::Model()); - - planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); + auto ps = std::make_shared(urdf_model, srdf_model); collision_detection::World& world = *ps->getWorldNonConst(); @@ -155,11 +121,9 @@ TEST(PlanningScene, LoadRestoreDiff) TEST(PlanningScene, MakeAttachedDiff) { + urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); srdf::ModelSharedPtr srdf_model(new srdf::Model()); - urdf::ModelInterfaceSharedPtr urdf_model; - loadRobotModel(urdf_model); - - planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); + auto ps = std::make_shared(urdf_model, srdf_model); /* add a single object to ps's world */ collision_detection::World& world = *ps->getWorldNonConst(); @@ -188,11 +152,8 @@ TEST(PlanningScene, MakeAttachedDiff) TEST(PlanningScene, isStateValid) { - srdf::ModelSharedPtr srdf_model(new srdf::Model()); - urdf::ModelInterfaceSharedPtr urdf_model; - loadRobotModels(urdf_model, srdf_model); - - planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); + moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); + auto ps = std::make_shared(robot_model->getURDF(), robot_model->getSRDF()); moveit::core::RobotState current_state = ps->getCurrentState(); if (ps->isStateColliding(current_state, "left_arm")) { @@ -202,10 +163,8 @@ TEST(PlanningScene, isStateValid) TEST(PlanningScene, loadGoodSceneGeometry) { - srdf::ModelSharedPtr srdf_model(new srdf::Model()); - urdf::ModelInterfaceSharedPtr urdf_model; - loadRobotModels(urdf_model, srdf_model); - planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); + moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); + auto ps = std::make_shared(robot_model->getURDF(), robot_model->getSRDF()); std::istringstream good_scene_geometry; good_scene_geometry.str("foobar_scene\n" @@ -233,10 +192,8 @@ TEST(PlanningScene, loadGoodSceneGeometry) TEST(PlanningScene, loadBadSceneGeometry) { - srdf::ModelSharedPtr srdf_model(new srdf::Model()); - urdf::ModelInterfaceSharedPtr urdf_model; - loadRobotModels(urdf_model, srdf_model); - planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); + moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); + auto ps = std::make_shared(robot_model->getURDF(), robot_model->getSRDF()); std::istringstream empty_scene_geometry; // This should fail since there is no planning scene name and no end of geometry marker. diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index fa3c36b45f..aaef8c7c41 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -26,18 +26,18 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) # Unit tests if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_robot_state test/robot_state_test.cpp) - target_link_libraries(test_robot_state moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_robot_state ${MOVEIT_LIB_NAME} moveit_test_utils) # As an executable, this benchmark is not run as a test by default add_executable(robot_state_benchmark test/robot_state_benchmark.cpp) - target_link_libraries(robot_state_benchmark moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME} ${GTEST_LIBRARIES}) + target_link_libraries(robot_state_benchmark ${MOVEIT_LIB_NAME} moveit_test_utils ${GTEST_LIBRARIES}) catkin_add_gtest(test_cartesian_interpolator test/test_cartesian_interpolator.cpp) - target_link_libraries(test_cartesian_interpolator moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_cartesian_interpolator ${MOVEIT_LIB_NAME} moveit_test_utils) catkin_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp) - target_link_libraries(test_robot_state_complex moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_robot_state_complex ${MOVEIT_LIB_NAME} moveit_test_utils) catkin_add_gtest(test_aabb test/test_aabb.cpp) - target_link_libraries(test_aabb moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_aabb ${MOVEIT_LIB_NAME} moveit_test_utils) endif() diff --git a/moveit_core/robot_state/test/test_kinematic_complex.cpp b/moveit_core/robot_state/test/test_kinematic_complex.cpp index cc8df316bc..c09cc6907c 100644 --- a/moveit_core/robot_state/test/test_kinematic_complex.cpp +++ b/moveit_core/robot_state/test/test_kinematic_complex.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include class LoadPlanningModelsPr2 : public testing::Test @@ -50,24 +51,10 @@ class LoadPlanningModelsPr2 : public testing::Test protected: void SetUp() override { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); - - srdf_model_.reset(new srdf::Model()); - std::string xml_string; - std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - urdf_model_ = urdf::parseURDF(xml_string); - } - srdf_model_->initFile(*urdf_model_, (res_path / "pr2_description/srdf/robot.xml").string()); - robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_model_)); + std::string robot_name = "pr2"; + urdf_model_ = moveit::core::loadModelInterface(robot_name); + srdf_model_ = moveit::core::loadSRDFModel(robot_name); + robot_model_ = std::make_shared(urdf_model_, srdf_model_); }; void TearDown() override @@ -77,7 +64,7 @@ class LoadPlanningModelsPr2 : public testing::Test protected: urdf::ModelInterfaceSharedPtr urdf_model_; srdf::ModelSharedPtr srdf_model_; - moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; }; TEST_F(LoadPlanningModelsPr2, InitOK) diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h index 22ec072354..ad6246dc37 100644 --- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h @@ -37,13 +37,8 @@ #pragma once -#include -#include -#include #include #include -#include -#include #include #include diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index 8f988e14f0..eabeb14af1 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -39,7 +39,8 @@ #include #include -#include "moveit/utils/robot_model_test_utils.h" +#include +#include #include namespace moveit @@ -58,15 +59,15 @@ moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name) urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name) { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); std::string urdf_path; if (robot_name == "pr2") { - urdf_path = (res_path / "pr2_description/urdf/robot.xml").string(); + urdf_path = ros::package::getPath("moveit_resources_pr2_description") + "/urdf/robot.xml"; } else { - urdf_path = (res_path / (robot_name + "_description") / "urdf" / (robot_name + ".urdf")).string(); + urdf_path = + ros::package::getPath("moveit_resources_" + robot_name + "_description") + "/urdf/" + robot_name + ".urdf"; } urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path); if (urdf_model == nullptr) @@ -79,17 +80,17 @@ urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name) srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name) { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); urdf::ModelInterfaceSharedPtr urdf_model = loadModelInterface(robot_name); srdf::ModelSharedPtr srdf_model(new srdf::Model()); std::string srdf_path; if (robot_name == "pr2") { - srdf_path = (res_path / "pr2_description/srdf/robot.xml").string(); + srdf_path = ros::package::getPath("moveit_resources_pr2_description") + "/srdf/robot.xml"; } else { - srdf_path = (res_path / (robot_name + "_moveit_config") / "config" / (robot_name + ".srdf")).string(); + srdf_path = + ros::package::getPath("moveit_resources_" + robot_name + "_moveit_config") + "/config/" + robot_name + ".srdf"; } srdf_model->initFile(*urdf_model, srdf_path); return srdf_model; diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index 81b82b9cc5..e44e0ecb1b 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -46,6 +46,6 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_state_space test/test_state_space.cpp) - target_link_libraries(test_state_space ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + target_link_libraries(test_state_space ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES}) set_target_properties(test_state_space PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") endif() diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp index d1db71f973..879c7330d3 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -55,24 +56,7 @@ class LoadPlanningModelsPr2 : public testing::Test protected: void SetUp() override { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); - - srdf_model_.reset(new srdf::Model()); - std::string xml_string; - std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - urdf_model_ = urdf::parseURDF(xml_string); - } - srdf_model_->initFile(*urdf_model_, (res_path / "pr2_description/srdf/robot.xml").string()); - robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_model_)); + robot_model_ = moveit::core::loadTestingRobotModel("pr2"); }; void TearDown() override @@ -81,10 +65,6 @@ class LoadPlanningModelsPr2 : public testing::Test protected: moveit::core::RobotModelPtr robot_model_; - urdf::ModelInterfaceSharedPtr urdf_model_; - srdf::ModelSharedPtr srdf_model_; - bool urdf_ok_; - bool srdf_ok_; }; TEST_F(LoadPlanningModelsPr2, StateSpace) diff --git a/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/CMakeLists.txt index 8ffbe601c9..c39d7cfe23 100644 --- a/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/CMakeLists.txt @@ -164,6 +164,5 @@ install(DIRECTORY templates DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) # Unit tests if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_reading_writing_config test/moveit_config_data_test.cpp) - target_link_libraries(test_reading_writing_config - ${PROJECT_NAME}_tools ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_reading_writing_config ${PROJECT_NAME}_tools) endif() diff --git a/moveit_setup_assistant/test/moveit_config_data_test.cpp b/moveit_setup_assistant/test/moveit_config_data_test.cpp index ac03cc86c5..658b22f785 100644 --- a/moveit_setup_assistant/test/moveit_config_data_test.cpp +++ b/moveit_setup_assistant/test/moveit_config_data_test.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include // This tests writing/parsing of ros_controller.yaml @@ -51,43 +52,26 @@ class MoveItConfigData : public testing::Test protected: void SetUp() override { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources_panda_description")); - - srdf_model.reset(new srdf::Model()); - std::string xml_string; - std::fstream xml_file((res_path / "urdf/panda.urdf").string().c_str(), std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - urdf_model = urdf::parseURDF(xml_string); - } - res_path = ros::package::getPath("moveit_resources_panda_moveit_config"); - srdf_model->initFile(*urdf_model, (res_path / "config/panda.srdf").string()); - robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model)); + std::string robot_name = "panda"; + urdf_model_ = moveit::core::loadModelInterface(robot_name); + srdf_model_ = moveit::core::loadSRDFModel(robot_name); + robot_model_ = std::make_shared(urdf_model_, srdf_model_); }; protected: - urdf::ModelInterfaceSharedPtr urdf_model; - srdf::ModelSharedPtr srdf_model; - moveit::core::RobotModelPtr robot_model; + urdf::ModelInterfaceSharedPtr urdf_model_; + srdf::ModelSharedPtr srdf_model_; + moveit::core::RobotModelPtr robot_model_; }; TEST_F(MoveItConfigData, ReadingControllers) { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); - // Contains all the configuration data for the setup assistant moveit_setup_assistant::MoveItConfigDataPtr config_data; config_data.reset(new moveit_setup_assistant::MoveItConfigData()); - config_data->srdf_->srdf_model_ = srdf_model; - config_data->setRobotModel(robot_model); + config_data->srdf_->srdf_model_ = srdf_model_; + config_data->setRobotModel(robot_model_); // Initially no controllers EXPECT_EQ(config_data->getROSControllers().size(), 0u); From f0e75b519d49397e2dde3f11ca51ab8c80049a7c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 8 Jul 2020 06:13:11 +0200 Subject: [PATCH 422/612] Adapt expected self-collision distance --- .../moveit/collision_detection/test_collision_common_panda.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 4a42c65924..35d8002977 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -233,7 +233,7 @@ TYPED_TEST_P(CollisionDetectorPandaTest, DistanceSelf) collision_detection::CollisionResult res; this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_FALSE(res.collision); - EXPECT_NEAR(res.distance, 0.13, 0.01); + EXPECT_NEAR(res.distance, 0.022, 0.001); } TYPED_TEST_P(CollisionDetectorPandaTest, DistanceWorld) From 88712bb7f5ab2c4153440c5c55f915798ca83dd3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 13 Aug 2020 01:04:19 +0200 Subject: [PATCH 423/612] Temporarily add moveit_resources to moveit.rosinstall ... as long as moveit_resources is not yet released --- moveit.rosinstall | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/moveit.rosinstall b/moveit.rosinstall index 19c6e7546b..5b2a6c27bd 100644 --- a/moveit.rosinstall +++ b/moveit.rosinstall @@ -4,6 +4,10 @@ local-name: moveit_msgs uri: https://github.com/ros-planning/moveit_msgs.git version: master +- git: + local-name: moveit_resources + uri: https://github.com/ros-planning/moveit_resources.git + version: master - git: local-name: geometric_shapes uri: https://github.com/ros-planning/geometric_shapes.git From 589f83a132da174991b233931d3ca7f31ab1cba4 Mon Sep 17 00:00:00 2001 From: Ruofan Xu <12582927+rfn123@users.noreply.github.com> Date: Thu, 13 Aug 2020 15:30:25 +0200 Subject: [PATCH 424/612] Correctly set velocities to zero when stale (#2255) --- moveit_ros/moveit_servo/src/servo_calcs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 8dd7f08844..f2282ef69c 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -283,7 +283,7 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) { // Joint trajectory is not populated with anything, so set it to the last positions and 0 velocity *joint_trajectory = *last_sent_command_; - for (auto point : joint_trajectory->points) + for (auto& point : joint_trajectory->points) { point.velocities.assign(point.velocities.size(), 0); } From ed3e196d28d670fb154af3cb2866227448b3ad32 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Tue, 18 Aug 2020 18:58:08 +0200 Subject: [PATCH 425/612] MP display: add units to joints tab (#2264) --- .../src/motion_planning_frame_joints_widget.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp index 7b9d683c75..7f4782f66c 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -383,8 +383,20 @@ void ProgressBarDelegate::paint(QPainter* painter, const QStyleOptionViewItem& o { QVariant joint_type = index.data(JointTypeRole); double value = index.data().toDouble(); - bool is_revolute = joint_type.isValid() && joint_type.toInt() == moveit::core::JointModel::REVOLUTE; - style_option.text = option.locale.toString(is_revolute ? value * 180 / M_PI : value, 'f', is_revolute ? 0 : 3); + if (joint_type.isValid()) + { + switch (joint_type.toInt()) + { + case moveit::core::JointModel::REVOLUTE: + style_option.text = option.locale.toString(value * 180 / M_PI, 'f', 0).append("°"); + break; + case moveit::core::JointModel::PRISMATIC: + style_option.text = option.locale.toString(value, 'f', 3).append("m"); + break; + default: + break; + } + } QVariant vbounds = index.data(VariableBoundsRole); if (vbounds.isValid()) From 787a9e9711392b02b42316bf8f56f51723241f63 Mon Sep 17 00:00:00 2001 From: Yoan Mollard Date: Wed, 19 Aug 2020 15:21:16 +0200 Subject: [PATCH 426/612] Start new joint_state_publisher_gui on param use_gui (#2257) --- .../chomp/chomp_interface/test/rrbot_move_group.launch | 8 +++++--- .../test/test_cancel_before_plan_execution.test | 1 - .../moveit_config_pkg_template/launch/demo.launch | 6 ++++-- .../moveit_config_pkg_template/launch/demo_gazebo.launch | 8 ++++++-- .../moveit_config_pkg_template/package.xml.template | 1 + 5 files changed, 16 insertions(+), 8 deletions(-) diff --git a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch index 2bdfe3c0b5..782b702447 100644 --- a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch +++ b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch @@ -1,6 +1,6 @@ - + @@ -17,8 +17,10 @@ - - + + [move_group/fake_controller_joint_states] + + [move_group/fake_controller_joint_states] diff --git a/moveit_ros/move_group/test/test_cancel_before_plan_execution.test b/moveit_ros/move_group/test/test_cancel_before_plan_execution.test index ef3c39eb55..4acb89e476 100644 --- a/moveit_ros/move_group/test/test_cancel_before_plan_execution.test +++ b/moveit_ros/move_group/test/test_cancel_before_plan_execution.test @@ -6,7 +6,6 @@ - [move_group/fake_controller_joint_states] diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch index d7512b437a..4ebe898ffb 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch @@ -31,8 +31,10 @@ [VIRTUAL_JOINT_BROADCASTER] - - + + [move_group/fake_controller_joint_states] + + [move_group/fake_controller_joint_states] diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch index f258075a65..059ef752df 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch @@ -40,8 +40,12 @@ [VIRTUAL_JOINT_BROADCASTER] - - + + [move_group/fake_controller_joint_states] + [/joint_states] + + + [move_group/fake_controller_joint_states] [/joint_states] diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template index bf69cdf642..a4d4c40b2c 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template @@ -23,6 +23,7 @@ moveit_ros_visualization moveit_setup_assistant joint_state_publisher + joint_state_publisher_gui robot_state_publisher tf2_ros xacro From 162b9a9eaf853f0758f0e7d1405239e2f3373287 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 19 Aug 2020 11:19:06 -0600 Subject: [PATCH 427/612] add soname version to moveit_servo (#2266) --- moveit_ros/moveit_servo/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 6540e55da0..8f9f5bd57e 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -65,6 +65,7 @@ add_library(${LIBRARY_NAME} SHARED src/joint_state_subscriber.cpp src/low_pass_filter.cpp ) +set_target_properties(${LIBRARY_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") add_dependencies(${LIBRARY_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${LIBRARY_NAME} ${catkin_LIBRARIES} From a1ebe89dc6f4b6dcc0e74d81d641ec7ab5b17f78 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 19 Aug 2020 14:16:00 -0600 Subject: [PATCH 428/612] Move constraint representation dox to moveit_tutorials (#2147) --- .../dox/constraint_representation.dox | 17 ----------------- .../dox/img/exact_opposites.png | Bin 3079 -> 0 bytes .../dox/img/fingertip_collision.png | Bin 77053 -> 0 bytes .../dox/img/fingertip_cone.jpg | Bin 26612 -> 0 bytes .../dox/img/fingertip_cone.png | Bin 80571 -> 0 bytes .../dox/img/fourty_five.png | Bin 2897 -> 0 bytes .../dox/img/other_side.png | Bin 2771 -> 0 bytes .../dox/img/perpindicular.png | Bin 1699 -> 0 bytes .../dox/img/range_angle.png | Bin 5697 -> 0 bytes 9 files changed, 17 deletions(-) delete mode 100644 moveit_core/kinematic_constraints/dox/constraint_representation.dox delete mode 100644 moveit_core/kinematic_constraints/dox/img/exact_opposites.png delete mode 100644 moveit_core/kinematic_constraints/dox/img/fingertip_collision.png delete mode 100644 moveit_core/kinematic_constraints/dox/img/fingertip_cone.jpg delete mode 100644 moveit_core/kinematic_constraints/dox/img/fingertip_cone.png delete mode 100644 moveit_core/kinematic_constraints/dox/img/fourty_five.png delete mode 100644 moveit_core/kinematic_constraints/dox/img/other_side.png delete mode 100644 moveit_core/kinematic_constraints/dox/img/perpindicular.png delete mode 100644 moveit_core/kinematic_constraints/dox/img/range_angle.png diff --git a/moveit_core/kinematic_constraints/dox/constraint_representation.dox b/moveit_core/kinematic_constraints/dox/constraint_representation.dox deleted file mode 100644 index d786835f0c..0000000000 --- a/moveit_core/kinematic_constraints/dox/constraint_representation.dox +++ /dev/null @@ -1,17 +0,0 @@ -/** -\page constraint_representation Representation and Evaluation of Constraints - -Constraints are integral component of MoveIt and they are used both to constrain robot motion as well as to define planning goals. There following set of constraints are defined in the kinematic_constraints namespace: -- kinematic_constraints::JointConstraint -- kinematic_constraints::OrientationConstraint -- kinematic_constraints::PositionConstraint -- kinematic_constraints::VisibilityConstraint -. - -All of these constraints inherit from the kinematic_constraints::KinematicConstraint base class and thus more constraint types can be added by the user by providing their own derived classes. The main operation each constraint implements is the KinematicConstraint::decide() function, which decides whether a constraint is satisfied, and optionally returns a distance (an error) when a constraint is not satisfied. - -Often multiple constraints need to be imposed on a particular motion plan or for a particular goal. The class kinematic_constraints::KinematicConstraintSet facilitates operating with sets of constraints. - -A related functionality to representing and evaluating constraints is \ref constraint_sampling "generating samples" that satisfy those constraints. - -*/ diff --git a/moveit_core/kinematic_constraints/dox/img/exact_opposites.png b/moveit_core/kinematic_constraints/dox/img/exact_opposites.png deleted file mode 100644 index 64e2470d9cadbb66267962fcc48d85c153e6b716..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3079 zcma)8dpJ~U7oX}x2o*x9PC0RAyHy%9LUI{ptC-!MapzD|$t^V!b5uuCM4}l=VxrBM zL1Cf@r<~y!w~R|s=nyjHe!u10qx!z*d!Fx)?~nED{jTSI-}PH-y}z|qq}|bjveKKR zF&K=j^&v|LgIR$`MYLumsu}u_!azSO&O-;yF}d|>1L#8P>>wKzR03C!z~(Vj zDM_6S=Wxjv^R~&93Q0~`vbo|IKZFI@VOh`t%~=f2ys=I5BQnAxoVXKqN=DX)@$SCs7dw`CXmC+zc1K`g^ zeTPA)6W_>!3Y}a^k8#l7E1;C$IAwR-&;0<@=qNf1+?o zDCmwmdKDk6#eOEABV|3^$n>0e`0oHE_1t2B)vxAt+0CylP!n}zIO@DMz&~My(6=%( zxL`G5=%)x265-}Ip9H$xjlVG=_30=wm|&)<^F0fQZk#4r(upZdDF||Zp_(oZ#@&n6 zMHKHRV0BqW*bk>B(xA30;QiYJt_uE@YzF5lo{L6+HeJ=s51qsr2Y13zL}h0Fz%wgk zjRbS*X|`3dOxIZG*>Cf=unKBbS%6 zaH;>z$)aGgKtj{DIA|EleofeF`n7UB+HUpx9j$;(_Of$)VMDZk};LCMi}~Q%cU2TU;)3kwc-F$vN=;C z3v4_27qL*2#Ej?HzT1_#^v3g#4P@+M`L8T!&sMwR5?OR2fQ0{zH@B6{_$Ld>H85=I zzMLQ+yXQS2n94e!$$2`RuUXc9X%$yKrrclI(Y_819B=QRBrHoP4bRkH#~+75V%-`D zd1Sm>!`Nis4GzP(XLmRH))@)*-mqW^^efy4V_i%45>&X$6zI1?PgKFMfM(#B$!*Mm zgll*Ex3Hj0%KazkBMburG%V3XYcZxeqe$d3|m{a8&y1x1cBj1T5-15M%Wb_al#h>om-o!9Z>g>P3qG37EuRz<~`{*U9By@07!#jAJGy?aInM5 zaRI7-lUwC775PVRfU_Sw)C$UUpPA(dzfwyD+bVa5k8~7dL+2!4_^Di494~S+pYf=p zV5xGs_Ak^oysD{Q`dydMx#a~JWE5z-8><5d(4{~%G*pTKnL|PMb95Sc-n)R_qPr!Q z)$Z7zvV|!-%ga*LB?cN>IZ)vk?rUZ)ul#MgkfIP{7`Ol6FiNNsH6yW~ENv}tqp%>! zRe|IZRYki2pgtG5SB$t#dJM$o&@uLlH$vS9f9s*n8^)p$-`M;qug>cj8on(u zB&?K?Pe|#6APq|PB4OFh@TlrTC||N~BnyhxMp?PDsq=J;CE|;B8MKLvaYb&iSkP?n ztR0rGX1p4WVAK&6=ElZ3I?$;AVM|LTRG*_t(MSaNoVOcLV;bn6>v&>@RzvUz(Pj@Y znRJfd6|!{gjU2Nur2WfR1;tFTE!!%nV7W{P9`T8d8j;{T!sM(P39FPN&u*T0OE{_y zBReFSPMb*BlluYOhR}V$wPz_uT$$~w>JGyk$zX1C^Wa#36HQ!_VNtCVfFvkmPr=OA z?kVrQ9IhiBz+H0+^_lve>G?Jd(pNy~n-hA}X*DTJ-yu+soU)DCT^o%Sx(QybGmRY8 zL;(14WF+^vo!6?HU5?eIezEmzU~re$c@>m}FRfTmu_-Sdvdf~%!)MaDA6vHfHBFz4 z2HSq7&xe7&d=@n3Kj{_;i`IQEutKka+o`Vj+~88b$Zu^x-0gbh`q>O%N96Oy-i$cV zJ76%z(SJH!D75&ps60M9_x?|G zMDX;@%2KpsZ)P>?S?n_d2ecfR zw3a&@y>NOP#j8cxdtvpNdKR?FZHJwgs|B-%5TgI1*BKdaHtJh%{%l$?JT@pAqy*HI z;~evi+yJWi2^DI0`IM#-@lx%q-n*HF*!m?UpBL2;B2nyFgWmc}ni>zAE}K;Kew{Vz zbjaURC*;uGOn&KqJTR>qs8&@WTxT?Ir?oZyfg{qPk#s>zw;;U1^x|_4Z)yK2E0=5X zm-=KP-aAvgV*30?tlXYT=?#VBlTXdL)VAC^hR8-&-Mz1cbANv-)_2s{c(%yr72u2g zP+c=Ih}Rp8advIHsZQhnlG-pik&&1}P8v-rqy3@lc40gx_wT?-A>Af5PQGWn2|3Ix z+#Tk*|J;zxEJq2yzuCt2*8E|h<86#Odqh>Q>2uA%ZS4gjgT2>2RqFiWp0)Dp$sa$B z?>$82@X~q>K3gRXXdH`I!dqYWq%%_XH!4)2q;j=^p})topj zzxM0Jb9--s3JpR}y%qk-qur-XvU5s5hSTh=5n_psflbjI!73RHay{oe{^-Q*$}Di& z=#G49LZ`(<#mHn4fV@td-zwVPZ<;ivLxJ0UZ|gtwn>k%~f_XvZW7PgrWt6Eexp z7sHJjK^(M)3w`D8Sm(~nB1K~RNXqU6O0uCW z#H0j8yDVD7?ofCzd$gWEtZg&D=q-^C&bP%B+G7dH!8yh)MQKFUj`q>&3m?I6ExK`ov;{ zo*9Q>zTI`QcaqHjZ8B^w2TO~mHt|#`?z`XQ{~TWb*I4>r7ocSRJ^_E5eqm`Jk?A33 Ws(Wc$i6J_qW2}isEpyF(3;P$mey#8T diff --git a/moveit_core/kinematic_constraints/dox/img/fingertip_collision.png b/moveit_core/kinematic_constraints/dox/img/fingertip_collision.png deleted file mode 100644 index cf55ddbf1ff9b2928f2ccc38ba322f259fb6b5f7..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 77053 zcmdRWg;!MHA1xxt&;!!aIW!_59Yc3_mvnc_5K_`19n#$(2na}bcL_+Bbid2@x8C3I z)|<6hgX7%8z4x5Y=d<_z>~q7F6eQ76h*03*;LxR|#8lwm;5C6C6A(P`iTSbP2=MmG zMMV+{S2;?&2mAxsQA*nd4h};e_;GusEHnKN_z=leT3#G!8HE&&11rq|DhPZ;=qj$| zD(YZoXKwEbC+ci&>}qaC?qTI>NiHcZucR4>fe!~q4ks-pqUO1H*yiz#TGQ(!Kw&$2 z>aF{q<3roknPVA7tB_g*Y&;$0R~m4^aMF#o0<_Qm^Y}vZLTi!D^91RxKa7G9p?D}^ zMqnwUKqX-*$j*q9Ge%}9VPc=&#n#z<(AeXh@0R0NQyyJ)g-^4+o4+Fjf!BGLf3>Z39 zJ;Kc??ms0%tUOssr*O5URXfOaR1KXzY|QFF?`xK%<}r1nrxowGRjk+ zvUf41n0-Wgb-Z#QpTei#$=RtGa8-u{SCb{l9U8Y;PxbqJx4x-bxMW8;#&IogYM1*5 zO^n2i<>G2#+a>=#R%gfK*$4uZe=OZ(dK&v)i0RCxZIdk>0rz^ADq)wmeG+&l&GdC! zSSnFOe!bYd5_g zWeQXR0s?BZqWU(2WAP1rm6dVS)i;t>u-h3se{dM2@NOU3J!?uB%97+CeC+^!gq5H`xKjpTf z#T5GAiE3#Ycx_7x-6>mw5k<&@=QEgwYVFcFeG?~V$}~^bI>-dw3eA=p^C0om1GQ+b z2r7fMXs9r9IBsJRFoFp9Z}hN|-9l}_{>kxTLs<)J?K}7?$3EXvxJouGD3{&h^wQFO zg@YezqXn2UIA5hC$1T*(mv6(6&RDv{Dy^cTqCLSZD9r_5FVmPHGc)rgJ2dEBc9T7^ zCCSv#V&lPSCYgRRGs|ypZ;$xiT4^JX95%$k!I5vJ!<%W)=Ith!O-4xgUj8l7_1T$I zpVQ|;v~IX?SKc<09u#q9Ya1KY5;b57xYAb|?cVYI$rb$qeD)y~?bM%?b5owNsIjq; z)pr9m7_2{Mw^W!YcsZ?#DR7~btCW-Yi9MAkODnd$@_;>iP|)PmT|WgY)3DKwFI5g3 zYNq|>nstc%Fu46P`oItDAlbq4#_pB%)d(p1M&YZPGkN z|9=xJ7jP~lcs?7+s*{5ujZ$*{j+?D8W9oEVw1yK z5T>9O6CP>^lq+4mkUE0WVeJ-i?0mLdyUr57*?b7^5KA1mLh0UwXiH^t;!wgyx6dT2Kt5)1)GXF4t1Q^M3*Vu)qi+e^Q|5y`!i*xHCm4ErZ@z~ zmYSWYI@s|d8yXroIXHmv{|?Y)A08eeylz6M!g?th$zSb%p7jo~c4UA}7VHf?Fx+>t z{7g?TAF2Fazb)L0fR``WEYtZ=WLFw+K=`%d@;xOb;*MncM+e$q{`27vWf~PGPK@2% z-D!m)0aUK0{|vFj-{VDA=r*yW%8>%)El!dYSTZf|<6er8`q$O_)$1hlH!$JYD(sQsD};oMq?|$AR!N)Z z$H#LZ5#*pT!TZD0Q?_?CqA%5An(5;Dc}?gu>LCfs)76i_BE$WoeNx6GCc(>a*XA{LDCSKb10*9{Qmwvuv9tamw>y+SoJf{YH{)v#Y-t= ze(iTvmo0Fz>;j@_F&1dDIJM~DVH2!eYE&(xhHD@>`}KY^%dq|GxXI3QsF6!v^^7!h z@I36OIV65XNXOghq`&MwwDrDs_>UCRN@9ep&C-XR%-KdiJ9(4QgdzHZr6vd9MfUF9 zJ7QwAs~pOg(q3B5(!352mC7~iaH761Tl0vCIY{32B&EpOTtw_6psN%qhDafcD7=++ z(K9w1_BOxP(lsq_qiJYqcx5URkkS;qtQ*3ndWkyo9jy-oh7|HuqTpKdO7KbFglo;5RS zRxa0=_I-~LY!N#o|L^|0HPqJw^)rU}a8%&Yq-xVsK+(T+x%Vv8PR6@Ur&WDgV?KIb zNNw5AvbfB{Qr>6C368tcm?tD&7KDYs5Uz$@OpRPCg=p-4RJ(&UM#Yk9P)ecFg>6)L z#NFZhXFP`VnBUcP*ezuDmGIcx%lPYMS%GK8o&G+1?xQ1%1{oJndqH9rQ2qS;{Jvn2 z)Y|ECPke9uFL6#6lrm75aabJlB_AD(C5e(#_qaPFxti*!eaLHsf%DR9sPs3hjs=HR zl|Gi6`u(uDj@ohSbLRhpB|@GD5w@aXP?FBigMflTiD8tu;v#q@5EYS}JYjR`SQ-)R z5=!Jeh!i}om`EIO1rbdBc{%+sCS`RzYGn`|-L^G3GcS(alGpk~+uHrYQ!duhIfKB@ z&1O;2kmvg#P1~gfw__RQ@+^Z|CPTl=nU0$t)-0dnA7YeF(5K=Yk3g0G3a6|-T*RvJ zqod~4uWmB>rc`t71X+oXN6$ey)BWM@F(^S)3x0jQ%tNpNYwrQi&GG&d!~Hd%axHe^ zAs}9r_LXG>Qn*>_ql2;cc`FZ^NzV{u5S`fl@C($y#SoM{DHr%31gbq^a&mp!-xzultq5BL#%O|v5xKT? zp4S9#_pklE9W8P81rQw?e%T!;JhPAx53igV`e=SEYN{9-4q1z*&U7B6zrQ*Pf|98V zp`Kbb`#CUbCz})gm*HA+7WRoDtM>&Snm0+4^RF5{b>P?(xW>yI*~%!4qp0mO$*Ju$`ecgUnzpQ=9=G;GJaZ(^WA=qc1tK%LBk zorbWW7Gn8c8(b_*j~gajLawBZ!|x)jzSVfu5W$3^AVS_kDO*d;#Z@Uq{Wi}6RGC{M zW2Y~nP94V$U-LHnTWNnOlckJy<)Og*Z8qj1DN&Mup=X7Sb`NRm_|YP70X(j{h(fq^ zW^qLhWr|;7#LCJ-!1LZ})pLhKT>d`t5x0V=>1Ih$*4>{-R^I5h@2w9Qd6g@%d!mMN zLsI{oasL-XRx2)J0@dRm)iP%AWvQxqiN=cZ{!EA&TwjrLJI@j>Ayc$sUXvUXF(>o) zx#f7djyUO(T^z#a#^+3jSy$xbhvTo(z8#}c%SouA_7%usd+qaZ#KFGU_`z!hFL^7NT6s(eGO;ua2WTHHlNb_DhK^aH-RIZb8c4meyp(fe;vi>W2x@M7S zsgQD{F@ezSiDt}I3qQQi^Jz)g(l`6(w-ryW^sB@Cd6NC-ti+FJrWGdvK0Y%-Wsryg zGv~*<%g4t@0RaJ{T2bVk5$*QO(-|ZncfOPsYiY(zg||Sa@IU_UkU4|@RWHUS8ae*2 zGP9v82H86}6w*B+QsPm*W3_{wMfaKxj=Knb;v5(?&EoVho_Glb1;K^h2OEV$mnh{i z7epR717i-9pse#r^ z^TcN7g;_0(n%U-9;ClyHM4dEzeLwadxuEg=F@b5NzM4&wN+7H9)?&{qMvr-dCA;*^ z-);yzdyC=>R`JEjsXv7npO7mn`2KR|<`qwam64q}FLZ}lv7kkTA7k5}CIg=4%1B0P zJ~UQ-dY4{!*;;gs9^)m3%^Tmyo2E=j z9)y{h`3oi)3l0j5oICn;LCX3}Y6kf2)5_r77b1C$F693LGLX252&JX>+YV2nl77>h zJ{cwZRW}m+tG^2`GjkOhcf{{cUbmjc^XG@mh&Kbn@Pt>TK9QF+ng7IlmtS<&QA-6B za|AO)qiXEwjDZ^3L`oEEkMB$m(dQVHl#@+Hbh`JfKe-_2m5~4VG|EJLo)M4ZfZNbJ zkUJz@C2@}=tH@vXrO3$P`P8NA($l279Ly~}4h^V0$Q{0WhhSdoXg^SGAjHj|QZB6SisOd6yy7XN=Nm|)+pl{V*J%O%6?%qo0SREf&(TAU>VREYh% z-Q)yGe^2(+_WAE6O28gGWWG~X`Ejn@*W0|y+5t{Ut6*|BD{ijc_TY72Q&HErQ3qer zyCt`kqO3KGmc0cc5eX8RlK1UP4UwwfgSvJttvGDF>o_{2H|HDCtE#K*S9nFhzv^!x zQia~E%tJ)P#D_;mGj1y%Z!8ETJUl!C0)*6fGbHTw$p{Dt$jGk05-m1nj_tVVpEdsuv%pZP8p{g|d>AIA&??1S{l~)I8ct^sUw#C@EQ!3((@K zFXgWIeNUVelgo#{>&v3|7ec)8=^Z|m@sjlbRQ)*G5dN-X(|11;gH8LPSY z$In%MXv|YXBxkKEivE3_EX!}Du)*b8K*1_3|6?^D2Y23>ln6n~>m_`>&)c)J8rsQj z`d^B`+267gsHJhl1^>SN=|dSORiF})2WH_Ul8bP#u_-Gl*;v@~B6ZXRBMj97@|zmy z7M4wks@Tk{Q@=nc!b9JqS*wEFxEZy>Ja)c+FbRLgKc`poxG7rYN0p!JK@1JcpH&tK zionX+i7gKEmMJS?5D^%29}X{;=O1fu?$h-hi!7w2_&rP_U2m}O6}_`tG2+(u9ChuA z;(WgxfH*gJzGI<7fr-?ox_||L@n@j#9CORd03f8IqTO*+lH}oXv9Qw8O062`6o>p& z|MV6Tw-`U^$?onr$5+K+bPp!d7=R~Bt2h>t=i7Z(b&*$sh^-H&cLsR&DJ0G|(7Hcv zUw>~h);EYx)0mw4!)3QbpR5O!^0Sk`(c9Nni%3}t&(F^ndxxveWkknN;*5m59b7!4 z=_H=Db4RHbgy*@KAwHIhkqcfZpdhcciLS762ugappyy$jtVYtq`vb`zEw^7ZRiKuWdU@x3lAE%k+WE9)Cwd1*cLxZ2ce zI5T(2O}>IN?zw5UaWjeK<|c0MHsyDEffDz@9F`HqnUPzl87Tt=n=7l3D8@H5W9LbrfDBi$v)$%>EZECaw@GDY z6kk95hJN+0S8(&yU@{TjhlP)c_+oa^AJ@+LX%-l)(X(tsgiN3=T=k(*6*|c@(kfzl zmTbaruCnE55BB$?Nd*SqJVVd>PwA&_rp`0%g0Z%mBvJCffuJt)?_!kxt9v9~s#y?X zK3^uAwCD^yW;zY;$~~UY9i3nm5gEBLn1DGR?)#)~dTUx`B@u$!RMJX`X(QI3P+hRh zE7J4nQ%CZ24!*tp?Sep0(Mwd4SZ@E*F2XuEHU`^SGweGN-5uHYtMwKPEs4*8#AUKo zKkV6OzGg1fwxdf-(AM6bmCJKDMSnB|7l<9uWF1uJ*1Itl{0)uGp&)fgB9GmW$LYDd zyQ{2Z=`a8LnxY_)1~C)sOVUpu=EH=i|EbEUj`B2wL=2jaU3b)Yuw}FUsFdoNmkS-1 z|Gj;;IAEv2MayBne9k1JKI;r6H_rD+$k&R*aR^y#Y41(^zL+<7t|uc-vkYpMxH9Lf$2SBL;2U;Sfw2*r5<_e=S)=&41tvIvP%3yYpt&rbSos}=#TdeoL+)tj^n23WNd#ll^k&B&j#?VDr zZ_lW%;<$4i-q2Gd_Dp~@p0Dh5jepcx{Gxbb@V3MEB@y0b>;ju1&&%_yelze{Ir}U# zW-o(b;I-$LYZ&Yd7W^0DE%HV-wAhn>b$s0MbX`P#%5(ia-!3EX36;q<9vHw12$Z0d zG6>DrhW+e}nR++*yGzkc))Nx`Nltc-jG-OorQBc{Uwj%G78(s&QL>v+LxI!GP4}=^ zmt^+&PY%dMgbTQYxg}Q4mM;9dr-&%G!cv8bP>WCY*j9Pv)VfnPm00Rrbd#46NL9B} z%t~-PIN5dk7~2P+UXvPE-U>*BxppjVuLaUVc+xqn(>5>n!89f6KOtQaCj8Oin>cOa z=r*Wvq4@q7xnQ zj4_+t1oN{7o+sG`+8BYczv`S3YIg~sVFxXooSY?U){c%!>gw!%Sw3uhXbwn0`Z?tU z2`6ai-E8t7@FIaWnkB*n2Fu=Bal@s&*<9(|_qy}JPuDXicW;x~3K|<(vdSlh=y>f~ z*AIe~pu{dF*-3o+1aE`6)uLq2BHOTp2i! zA5mZoD9P&T$~d>n3w=e`$na!*qM*Rc$UIOT{d^_U5FjB(^q8n7eT>fD*xj`(QG-G8 z07Z%t>{h#Sl~v$G6vOgc!8|pivF7P8(}L~G^n^xsNyWsMyfQfNV35oRUco0+ieS%Z zfP6(vsm^##lNwB^Jq%aOmFu=PE=PZcWto;1iN=x|>(YpzVh)T=K&XvcVi07Bgq%fXz#dF6oDCdDSz>tWSpepTl zzPM$$R*i0r9LQB+v(oaTAfVU-ug9@O>ak=|Z{giaqjLM+{*XtOm-TN=LPSP1Fd{Mg z7*bWZ7!(|=Qi2edSD;eT)HLUmk(`_il-16>DA?xaW-XHiJE37ru~nMwc-(b#qn)Ue z2r<4`E>&lGU7~}+SUgXHWXf*qVnnEydxo-rOX&9Y!`&c!1N$m4zYU(5FiV z%!7r7v$LU5L$?{4qRNABxe4CAyfRo5?+-bXWI8qB)IyET>a_8Qx<8Gp$}kVH%YWc7 zYy=i2L;pt>h_nhr5`14cj+ilH%v_|A?!IT4(nr0wfEM=VPY`tT%+u7Aijk#1A0nfwngEG7#RaNw99yI(}CJ^4o!%iz*MJ&3oX z#;X{$j;b%+LIb-26+2{@UCn|wZzs2UVT6+pm12d$T-mUF1px_e<-ZsI9KTzy2M_ZMOO;t4{Rhsk!ZrLg)Ip(GL;e5M$bI-wBrD_IAO3F{&?x zuO)Mp9IUG;mXgV?d~LG4_eoyAe*OLXcaJr{>-qWlKEbOR?W5C;FJ$)C*1SjcQ_*By z=cNSjQ8`AAmTV?Uv1IrN01O@p2{59$6{W)tpO{ddbHf!6yx1G3n2M5dn<3^yR zK7jDI?&a|L{kPRs3@9%zZ~N0_RV2^|{nOO(Xs3*0uC0M1u0G?*pCv_Y4ZLh@4h{~U z_p6>{;;FThjbF=8wV#Ht-drK$o!;(bD^g<*n6anI6;ACPA0Gp)OHB&|KhdBhIlL{z zJ)^u#Mh6Z6YcmhQUl4^-3^_C5x#-W;GI67iodW|0-2?5Qm)bJgy7(6}xWDmZ(x<~; zwyE?^)`kwOx=&klLfQi&CqK_eQ4}3^Ml&Cl7ARxo5W^%c!rt^Die@g4kH=c2H3C#% zWqe#16h|G77}ljsE{wAp{q^gaPc_g|WqNZn&XXwjBGb#itF?O%`uJTZS@`&rXK)Ac zI}+mk{XIo?!$He}?JQdqZ~mxHn zc^NG?)^12nU%#oEn9e18t&s`b{PQQdp$2U3>G2*AWr8cbv#xgno_4CYM?S8-e1FwU z0&JyO;C^`X!(rq#Cckje#ou2BxF9_|iCDQ9tjmy~mk#RX<>m9k*z<60PL0yo{Ygi5 zn>k}-kBMOR*v|R$LBJy`5YP=j!C|F|jvw|rk;r@#j8EjFgxT9969dN^kA$AiVy@Q$ zglvELE5*DGBGdiM@HuN|#sB_DGKO@-*qiwm#oT|L`F{r&$v z;Hj@yVC(xFHH7`zrtxSY?wfG)18xXfAU|v8!iPxA(;(h|Sv!|kSJr9nyW_dvC1Z4w z0pet)jc0~@b5SHD50a+FZd!4Frvi3ke-}Ar=zqHsT#p3>E&TXBdc5P1ckkO(xjpuD zI97UZVqsz7<8z<>I&i2Tgl(?}*3&Oh+uz%}J@)807{Yk#i^`qx?VA*BLRI@+=_WVe z5ePL-qveJU)sjz@0Lj%_d$vgMvcy*~;QpwI3Q?Jk85HpJZ|DT0d^OzT)81Qq384Ed zsvh8BWMt$|-%rPT_EqW4+CN4`Qv7}hHy2l9z_WihTWDw~TKCPqW(SJig&ko_@A=QT zxHvi5*9p7YNg`bZZ`||&g`m#~?#r3ygKGBO^j!NDXFmBpH4=px(`&;UeyCG8@R#(R zdT?EC?|{CT_pG+XNDU{ZyoWT8I;d-KuetoI5m$QVw{QL1&Up}bS64tu20Gs1ioP(i zhsZa^ii$DR(qJ4_hNN=(+dJh>a}}9a!KZf1=yCI zon1v!^CJfbPGk>ucrZB#=rdj%Z4DVd)#gEJwB*!hIy*Z(`728q2t=kh-1Nt`oeQm6 zD(@Sr{6qi7qu3zp?tdih>>eEJ9meiSr{T2)sqzFn8 z<&e^<%o%l~?M1(M^Hdd-y$WOsq37GNAR&a)c+DCuVlpx^Qc~JrI2>H@lBz1C(WBM2 z*6-h6OYMo02j$ryBqXAe=?CC{Xc# z(^8(6&h7|a{|I68-9E)b7dJG7m>SeWMQ%pRD${1Hgf71bfeNsRu@1?fb$Sr6uaZD0 zwcCczcXPOck-q`_!N1Cjd(sU;LR+deM@@(C#PeKub;cA0`8bv(dxJ>8j)Sm~tZQGw z?}$Sm#!D!y#e=FCB`@}VIP(@aC({qX@WlZ1GqVEQ$x#o{4Jqvk(-^S zW%i|nFYQW(5SdumVS}@c6g}q)6A1Ywoc59Ul!X~5*fH?dHw%ni|G6Ej(;4Ri9sCAg zZ?5fbzbxQxDjOiDpe`Pr?u2PtQf^0b(wMp?u-vQnK0v-$$b&{ZpF;E*S#K3+wZju zi&?opzz3Hz9Yw+N8Y0OJRS0OOIB28?ch12n2X&u?#kl0FvB45dfGOym!_Tu(z7sKk ziUyw#burleVV^u+X$6Mwc%u@?W;@?V64`^+?fG~)*EB+V*O5W$x&BIUV2c)CX| z(|SeEPc7Z@@NWoc@nr)(v>#Kb?ik571`xnH^UB{gHl|NlQnIhy{pPYNK;X|*Ou)k? zKe_R(Ze8~8#|j7kw^2sps9m~jDoNFBuFi zQ?Yg7tU7k11EA|8(8M9nhzoJ)*@z}t(`j!ysGm}hrA-)zTBYGle69tdTDpw!o&4-0 z3m}H4EX9iyEn(RW(iF^Q#fvOZ33t>s;<8_sYTgaMZF74urTN;F$|h)}k>b3K5Ooa3 z(*NGx)J_%`1A9dn`kqx4o6&20!mUk(}X5dH`e z%ZGv~t6g|bv`ql2RbT>%>6`-B2!XZjLPZMz=<@PkD-YDk9#?%%5C`EccR*c$H;M_5 zVgT>086G0a?DRC#?npUCV0On57X=dw?aeI5h|D4Rn0Odoja~Z4s;T$Yg2ji_;j5!1 z{5uDNykb1J%DZzHel6`x9(rs&ofWVKxh(^K3RQWslpZs{ka*AuWnT{ zb8|rU>JZ#cR4wJcdx5%KJhsOZG)4$@3*}5hQLv&cZ)<&h5;+7tB@`CDwlGZJ_16>c ze4p>vEQa3}q0U<^U3qleuQWDtf#MpN4rX;7lBlh^L(wBoKPC@t=hO*UBHWTrIG>iW z71J%ELLZ@P1TVfJa{dKB3YWl_8bs=I_)Gc#M; zawSNj49%RIu8xjIl|kEhfN7!CU*{hxC0ie0V5&i0ajJDqWJC*MayFXZ<3S1vj3uajtmk-Z)Kd1Rt&oBgGy^TlRUobWO zXnozBH6$u|r~V#bggh+)a|;lLr6NrU*%8Fi6rj3N2)>PX=Z&5kAFs}F0;(Gm4hj?)!;G zFlE8wsVlVWKAo(#)rrrgv|rG0E%!hUT!NEM(EqT$dGjWIfcgW>!DnfyU+pekMu8P4 zc%uFrDm;Egm=WNC*xiLNuEZ)7*;rfSLD0cfDkX_S+n-x=1@dsCL8CXH;Tu`IaPIUr zFXs$nUe@u>myn?@5P&m*3UHS-ZdyZtTKMJnLpu+4=dqG>yz>t|M+R{_yQ3dFS+byo zRu2cj*vLlNp*OUR8f1o!M`aQc#|})Ahb0r9DTq3FdS(U}1##CyzCdMWrOjFX^p9uB zkFug7x`Y9O&{t!;wM=u<(*&V4+!9K2Zo3am_(Vj*nfjOG7yo;}^4^pPr0 zOQUq%TI*8YNu?x?3S8@j3eWDO$`nXQN=Eklv8if%gX4SJiFE7mqw?S!Wjqa75R7<{ z00G2}>;V|%S`C-O>cebPJ7=7fnzPA(8Ftr4tqQd@-K)=JD_fmhx4!L4kYB{e)qdcf z($alBZ@%xCZA?)>2s0w|!1?4zkPnk;D*N1jpKPwE{NVr^upAcO9DZU0||cTc!?v+7PN6Bmsw8U%~VdcB);kRdIZM z1a4RT>-iefbH$X#{rg^g^gt!V6yGf+&dH_}Jf)Jo@hJU413WdvJB7b6Ce8udfQTrHY8VK$;oQ6IV zUR+)M!UP@=5D;pS8BR5~WRpK>Cy$jeBft+4ipbDMqvk2NgGbGy90w>J4`wS; zCMlBRHxsDi%Cj;v0DOkf(e{0Bt+>g5OMkjxCI>zr4~%!Z#w&c_~opW zbUvL5-p9w=lQ`7i#P?CacD~x!*OwUnGfAIxfl4PD`#H;;TVSi;RdeOg8_;*v!VxFS zSU(!qT7+^egjX-OQR8(N%}@skpSv16Inqxnd;8=4eO|})U!mylauRc_(xSZbAT5=wCTQLf;I^M@2^i?IOTL?0Z7pQ-IxfroujaioV1Rd>s!2jM2>G$mXc z*b1V3jwS4Lu4nA)VGx#TeqcX5K?hEhL{ev)NiPjF{He|%;C{(Mov}fUfd)H#c6Ju< zpdPQby@N_=siIaSRlO(r4@k4%MFQs4t0aSe${Ol3fB$|OJMZo518!@sQkebJQ10l? zGfXL1VC&qA3!%ro8SpkYJ{;DK)!vYkp^DN?NM>j)4JNrf)Ob=gkGmco987Vb+oo=E z$5WS-^7c*dOcWs*ofC>=Q<=rF`(~e(2?f?wmw$SCkNTG!vQuC}O#B@@yG|z#P0U&UpY%ZluFMr;hpde$>EU#3$M^ zsiv=ILLE(b21S6nkj>Z$+e<2x^tGmS7MxLLxW%s!b;y@Z{_EVx_- zfc;ZZ=&>G~U&duvS)h|vo^;jTq$^&pmoqtg5eC(Qw z0}{F&zWw1Ip~`7PkK93E;{<&NHBV_sYbVoVwOqm@=lu(J-|_-lHcA#2bi;~pU$(e0 z3S74bld3~Us$j9H0K@r&Zp;P6t%!j{07Te#mc<{>#ZltCd`lJ{1GWrFocu!O->moN zkZ|sowp+ROhwJR}vbqnh_RGZSm!S7lpaEBMb2*@<>R}8T^;#fYhAUSS!%N?`kB5ln zoUFkQr%AE`cSm++c6PM13a!_Q)b~_GI-Y50|L~BQkT6h2ble5ehtK;0BFoiln_XHO z8vt~}{O+#+pBum;0j}NK+dHTTN7|O_Pw_4Vg%dV&`3hQZ%rllEN4xN|W;X#1U0A=) z{fM8ab$o#+-#2z&@3+Sn82&|%^k{Oxc8jndSxxFcg>N?tSQEv%AcQ+_cmNAdvrk3f zGR6;>@sl<&$pTgO>^0gi0s~8N(H>b7|5>d;PB2xg6);YkAGLeuhQvjgAkr=)=f%=tuQFs)<{*A1T zWI&6l*?tlF%JW!VNwKsxHW;Ll@eWnLa>0g^t}!zTjX8$D|x8i9tsDAyYWSz9F9ST{YqFk=oK8;Z8t78;|Y;ZGm8L zqL#T;o#-tsEr9eaW>|cp{F&(#VswE4sHxNc#=UkjH7|vg0RxxZsty1{tkRUMFm(pj z1+R~mnyjX1*9KJ;1se7Tean@T&ArC51c0uF{@E*^8MnK@U&g;;Eq2Gr&4e$-v;RH* zsykxJOu``J6>FA`Ri9$HXw8>fqvv;LyAq}o`O$CS;H_!5JBA-9x+dw^#JGo=Ie{^% zsbo3bP07UqnVy^jb}I;^XaQ3h4nSr!babBR1UXtsNlD{UQO9BW`hodor%hrk5DdEW zLoZredM(xrm)z{pE&pNJz264qYR;p1ePZEmM7MFc6*V=f36+E!Exu&ANDu{v z$48?oB=q(5LoS^y>TSRsuM4TAIvIp_horDG{GI_tubb@~wX`i;EjHLbeP2Ga6@!#p z7=FZ?4TbiiOEgZK0>|$$)(8PmDsXhPs1{RianJ!SO$jsUm%Gf*)7IM9oG3pHpLh?M zR?+P9m78Trl1Wt2p984n7omp@=cG_y2h*DQWMXaDMgDzhaK;!8j|Va_{A0UTwlp(<>ta-sey!6EzA zWGoP_?d5Bf_q|wtxt+@kb+ths-XN&If5vVD6zu#4@5}uk7Jkbe0YdYvTtiQx@9dke zSA7VnZ~A=SbOFlsLAC5Pt9^cRezee*F85VZ;@e6{br3&1-IRnZ_Hg=EAJ+F)_3xP))j{>k z)ZSOG)#EWOH3laCX8pkhpbE1-z(;}Cn4b_>d%xuv=wT4|r2-;!fyy{h7l-v5im)6j zOG{vNm^wJHraOOR#lb;%fo+AlzkvNe66~pHhL%m(;AGV09mY4!l|H-U6kG{gm957w zf$3@pDY!Uv8BJX?tcG5U&gW@uC@#Q~t zzMu^7eR`FA{(j)CufKA?_UzBDe#@FNqz`NwSjRNlM_jX9Y<%2GPz2a`$hp6G2=o;M zB#`&~bn^T(7w|Zjut&_u7LuzE^L0g2yr`_K1mHm2QAA8Ma>2wcz8+f#{{25$)(I&T ziSnvNrrH=^K{S7jStL>-7F|wMLXh1OhPMc5V7{;9Zj@6gD)md}LCoz%R zV@Xr^Vy(!`Uj*i#|G1*0q(qBJqxlT*=zEO%V#Wj-H43hO)#eZsrUgOSLa@yDzX1)o z8#xX(4sKpvl$w%YaQ_?9{B{$+x4!&Y}@p&I8#rVBSV zggFEx#D(0TQ&FH^=MJJQUU)6%mX`3yJGViyFryO^>-$N*bxbeWx&GwG#Ds+X)WtkxLdwzg zoaso_PX>C4S!T|UA6b7(0j&0e{rA#+-;k}~(!#>fv9HLDOz9(qDkX&U1b&L1|IS2% z+S=NJV}X7m;HEb4-YZb#KSOXGP?4!>|C<)l+`GpAz_u~z&I;M5Z*3hs{N-6+%)@>9<&Z1$YW_lask@=h-@_V8d@l<}V(et~(6FkYQ74 z@1)_EkeG>X;#-efUNor?%amP(3Bq@s&LMvn$}#olkSS`JI!+!>48u?#F01^-Kcyu# zj|=44Rst*AED2F0*<6I=6Pz)^>0kwu<|ynMmV|~BKR8*G#HTUCjkfXukg#;1r4<#K zDLgR29+MhN6v3PIp!0W0M5VS|=>Tmm7j!S^F#-1Z)eO^4Waf3^#Wsw_c1Fr6LIxc) z7?P-^@7BKv)(hhkq|tw3VJi0)SXL4AK=Twyc5-cuW=j+JiH~*_GJ0>;$>2!fr1TQ@ zYOcU!vWOW%HsWPcrT}R2BGTW%U!SavUE9^t2LYR<=a`$8;WGw z&?>riTc6v782X%myuJ}vmO+xS7S|!;7EhgF__WIFs7KAXbPD?!61Zij(YdX=- zk!@!H>!Du6zJkSRl1WiMXt*(zBLrscxVgIGM2f$G)9?zS0ORG681Du`Pxy)ZorrG` zMSb}2U14H0G-WTqHh%VJq9OyGxbiO~1fe@X?Dl)Q-C-T&ykjB>5Tv4_Dp6xdAK{Ju zE>DjaDWX{7(2hX%x{J2UCrcA}9zcm&b^GsthrWP^9>a%_O>P^YWt}72da~*xoq8LV z^tQ$t?{2j#N?_HKC)zZvuu3vfz*y}MNE&g5%x)>!oQHsD6E(IZ*23kt@%GCj2j~n& zMn+KI6Zf}PI={~Y_{F)3WIOgi?cosWU^1E4Rvb`eDN^NNM;{9FOZT$`d^qHF+q^l4 zeXscejl6~{4ZQ~o@OI0w)p13A!Gda3>NEh^5?8EzTn4-H5GMvf0w@Scd|l2%zH<=S z83A2BPm`Bt9IS3hfO&sIgXpq>sIU9mkYP|ix#ueOMIDHGK)`|%j;9t^h7=T9Rq8fH z6N+djBjX3FR0h7G`<)VLo!S8n7g;%LJDFfZ8I@}AGNX9jXKV(cvL&#F{gNRzjs>{p zy*L^Y%gBY*ODXtm1E@7j3TC-anX)R5<7Y@hH=WTWe0b+F3;CByD1v9d-vTL)S!jv1 zeq4;^Es|gV^TYGg8mfo#+U->nA^Z-s!HXK$__coyvn~8>OJZF7e9nA->?QUYt)#h@ z9|$jAdK_1-a~2sZN8X2*k&WGhPh`9ZkkWsyuC5jq(vpGvv>Qbq$yoL0HXuew4)+f` z_!oIv2Pb?@i0}p>2C=Ls3WbyDorXkk&Kf{O!LZz3r8wVlyo3_97=4oYLM_wofiC$% z{3pRrGrt+?VE<~<^ z1UecTz%7bYO56@+m1S~E($?aj*&*J5m^%vW3hwDp0eKobh`x4T56xGh7X(kwi4d)@ z%FxSGV@H1eP^UU*SU7ZWS%#`ko{4qPRtpF(%F15pBpGFs6+-sjE1PUW_8u9T*`vrPo9vm8 zj7YLcR>)pizw3Oyuix|EXS|$qp67n9`?}ui4kgb>;5I&sK8*c#_$o)}x{!99LX0>r zbVrsJD7M?iMFfNcg)QGNFn}d2x&hj{j*E*69d=7f8Bo|gn*soY>)=@{%|d%SuwA%s zwMe8@M{By{(u~wpk#(cR~y(e@} zXecv={ywE!xc*Q4xwWSc~IfL*)w4>E7tB*$+j=hBPBuiXD+Bvm#I@A~cdh}PY| z8-rJ7=W)L*xvaTOK9=Wu*E!xsh5!AFuynABLHgxCbg_=qw#G~asGgkMZ#OX~A|JgL zfCT_yR#DM4sRaVg<3?rLo;#%YW$VysikxkCjT`$vd_PxV`lv$c5!n~Z>X3Lhp=Xp~ zW$WpMEf@dRVcEeKPGBJ-BDxu7s;ebJcO;^$nq(4HFQ4B1n9eH%>GDmQB9J<^J#WR! z_+P5OZV3H)^yR)X%lmYnF|JY;?Uk4P_ChAxpULsD`2t zBkTm2_fn|-8>1t&-58JU3QO`YL57u=x~bb=dT1Jo9Jwnaaw_ z$;n9rj`7jaYX8d%V*leVdq-liv+Xpi?1}H+KY91%M^s3yd%aPM6_Wa_-`Z~+pmdAbu5x(Ji<#od)I;}Q*tC%kq88$jE|=#=kNn@T$W^g-nR8Cn!E6fH(32-mtgKePy?G2po*3=* zK2s{I{}n}lzWNw;0sTrdYz>ou1E9P|Vpy+7O6G~*QDjYw%Kv0m2^KgIy?`9ufMZ2K zNi^rV;d4N|;b}FN8N2k|S|2YbHhNXIp1lHpN(>@Bcm^1-t;P;#e2->Y?eC-c;aty$ zjK7Tm=jlEvxL6T83+5pcsggn=+Fn!$e?KBOb1)pgM;;AgJ1W0co9hpRI=|d7fBF)I zAIfIOX<2a8derj3!LV|5D2{!N_Pk`h~~LL12*_LsDh4-9Fs`x)brRn{G43YUwH7h%`UrudH8OcwC+ z81%2+?$W;T(kUcfy|BEzyrm^e6)E3E9o8XZ^yrQb-U(Y<;uHMv|90nfz^37Kh|U?Y zXG|KfgdWlV^Gz$Lh7|$-`TV*ut)x2;SoiN0lKfVt{K<^KeAzvi%K3iylKTZNHU;xB zDUYd2;VX*;(;DEW;QmdSP79=H_NUB9MQ^b1``C@Qmi~6McmxJbG=k6{ysU0e{+lNPSmG1B9E=`LRWpE|Mgc_@Lum# z_>?o{4AF~tR($wyFHPYZG4TWZAXj`^DgzG0#`@;@ZOMz{9x>62<3$jQew=n85c`Os zgJ_LUQhxS;g6W@lZBl1%cjw3Vvaul@`$)jn=xx(vXY-kSY~5H7F)uczgf+Hwqwl4F4>Gmg>D; zbfkFUV|rgwa*o#HU(Z|C4b4lQe{(mKR`p@eB=JmExWWF>^0QO2O0aCb0(UblVRv3W z3LrN@k*~)=3{(v?*KQ(8`YpPczZ(C7+_PN& zWjFU=^n{@~*5Se7zaJv-yF=m$IO^NoXY9%vKJyHiRle>sZ^uh3l)H0)J*Y7&tcQoV z%VKQJU%28`Xd|P-h4L_1Jq{J1qKj{JUA$3uPjMGy{6)9kef#$9V12v|L?-$l9YSBd zv*-{jS{$93xrdN+b**0U0t%#KEe@Gw%$EG~eFqzBcqR-rCAV{Y5|A#xdtYTZG)AhS zMR{@w-$9L9Z;;g?=}UGELxvb%%K_t2#~aias4zlV%Uu$_JNK8qJWrasofAy7>&zK# za~;;QZSg8F(rusXZOsf@T%Ur5Uie}1l$=hLmt?Ade)bf z#fE5p*gc3H{4!SQeYC`Z`{t<%N+SZ~)$WF9&K4B~2`)(^+1c4K)chP5@9X8yjPq0g z=AEc8?Sdd525<^B13qR^J)yw{y7RADE8#d`x1+st2D(h&wSrWRi5xs8N#7XJ9Q953 zNk_gGi=ShlN&Wyf_I&a07H*{0SeHype5K+9FD?F6t!i51dc6+y!*7{;B9_ee-@r*l zNvYH1@$AVH5GJQsfWUV%S=(r$)%Wy$lq@^y(!}8a_SGMth*DR7ASA>V#iStv+PK>x zz_8acOSU|{g>iprcZMCC8BslG8t@=v0Kf?74=Xd&ui2a)ZLQ3~b_RqSte7y1Hg(cl zK@rWW(Y*`0SydD4=IR8u9C)zb7}__Cb`jd5Q&Yo3d{tqpP8d>An=ZMNL zDk-th9S*dl#CfX|e8;SVNuHnt>dn-YK3g(uRzMaI5$S?3)ReL#M!lFcLza#QgNp}2 z8uE4U3))yh;kx0L zH$_GsaaQR(GxIg9uhdjjLHA5|AF(C|BIynzZhZBH~@)DW;v| za-Yx-=k{-`9Htceyt7uOS$`%+-<%+aFu9KaD}G7YU7SztSc z#far6;8(@KMSXb1gD)va^!A@F{ZG0l-s%+eA&+BeFgAPzt9~Q?Te(1ti@$gw+zwJw zwc@un>4i2RF=bDdhfQMmbAvk2A>#)SnboQaGJacod-I2e3jZl~{?m|y;L1G-=5)5? zsGddS;FZmpPoF*kVvpvp;gtgxVQ>%#&o|0+@q{bLvwcWmn`?T`xz4%y_wQkMM_5X# zP$fq^BiDDtKLyiD9V`lz-1T~+dY$Z0`(SHge*N>EIRBT&`*l+e(7x5N-_02!zILs% zLE@Qt>*~DVN@g|4BBhu*o(_e#tGt!&3EGv$r;vcAtOlLF<#$kO{9*epx>Vue{pqc~ zZLK(ZC8R>+=^7s8i(r8yv&!DgKv-=?R}0K_wKRF@;`*xy1MdM}s$XXx*=<&!X5-}K z1aC-7%lXR(gI8XF_dtjlGcag)=%&1chj&2AyhgmqBd>fULI&CBWW^J7cDH(qs}b+b z+wDgY`KwPE>%A!Vmw2bDL*G&QF<3!<0Y{ zbWCQMP~2o>$r%d&HU04%?|EQyS6qwaN0#5Qf%ikc)&`TTypZ=IPhn=#r*|%?xYWJ1 z1SPG>k%%<64T2vB#V*fq={&E{Me1-VaZTti37Gp6?;VU*EVK;yU+l5J$yjd$uamgM z@;vw{#t9y_Uy;>K;Z+QFE8cz{@gGKrgNqB_>m(o)U}T54F*cr_p3vhb_CbTnYxiUO zS-wqrn5lw-!uKCPB+aAZ^oMyWZeBa_fj)#Mu@3?y|66)&j-8&ul=e*v*S`qri3;sw zF;$d*Onp56euLrDX?s=KI-$v#cF@CuZw&0=GL{!#q{jy3D-tstThoxc(1pf*2q&o> z7Eq=O&%>KtE){kI85cSLBvHOV@>I~>(5fqccxWi+%FBRB5GNzwlE(Fbp4b5GC}KrJ z7ByIxX1baEz;WZvyxu0JydZiYEWMXP-2#H1-<`%FBnzR`;}B^wEHM& zw`k9l^O)qtK}*&P5e=1`i~V=LQiS;&`RZ;QV@V=I{g&0>Fwox@BlqlmKzKE*G0=lO zNsqK!gH2Oiy-V`r&$RX%)CC}Lmb@njC`3(NT~xj&kn_8yq#cF7TTlNIJWHVD%`1P& zeE>pE6&T+ubs%qzFy$y%1}pz8JS+_E_mXek*G*}Wi^mM{$j(B?9C7AevslXz;}BXx zkHk(L_9?M8+}3wIvaJl;Ujji+?v-Qt$WZvACA1gUkXezjJUWPJm0!Qa0Q zp1q^)4`0bPuRPo`mH2)ph9U6i7!OJfNc7M(R&9e6BJr9*Gx=pHhw#f0$;%<^slukM zR_f1B5RClOPLF^!4S6dmj9vK#$qOi8**b8-`NGn242ne-x-iGYU6=`Fc^E!lD!I&Q z;U|h+7aDRO&wh~|@R@KhST=}B_6}9AR8{8B9Vy+x`KZ4KAEYYZXs~GCN;c-`FB3{m zVtdDS+ui4{JL%onMY%{aVeLALC!(SW<=s!ca;0Keq@AxIkWx=qWrjwxx8mY-jHVx& zz$6ueQ!&fdKOl4T*X!Zw`49e=<>OO+o@V(zLl5I0R8>?sU7UFl9CEE}Fb}KgBG%rJ zlLs-V;RmPDAgYI%oJ6wfA_u-9thD2=ZXBID*SG3@xKFq@G357S$Mp*J=FurY2duYm z6Ls~P-(PakHB9bV3@lvG8Sbky;IOKmfJ@(@Ayh3ss$>N#*LKgKl|+mnHGM^(~0?IeyutCMbU=1mpA9V zey@v5DMvI02`EGApMTQAJ8E5AC1Y%vx$)0G^+=SCv0lR}s{ZN|{x&IeK~_=<21_t3n&PxSj;( zQKg%o3n?~fA4!kh0*q)PGvlghpX` zE<#nyjbkFQO+TQpi;INrs*(bJVE^}vxZ{V=(9mliwAxu{P!AOMz}2yvb3T#NSH&)e z&b7%^)Z?LgwxY^FjQRfAo8`$e=87yG>r$DDGLMa)I~4vq6u)_gpc#qgY_)Efm^&Mo zn-5WwR=r|lU2Beg+ihM5tDbxIhi$7mfd$)dm9EJKBsC(0k2*;mUrqI(MlU_pj`H)! z$aNN8w_knjTp}o@Fn<>s+~S)ZCdjE*AVG|j%EcH6C7^IcO=33F$OAk zSl~~L0A<_yJ`fnP$s^!c#OrF;IV9P{%Ky3AmH(6DJMzF}dF<4^CTv-G3APd?xzU;| z;pcU3%LGl%S;Cjy)yjS0C~zT2N@!~K2N=Zl+#tl^?p7Q%(9sF_?S~q&#qyLu0yxIs z!&G)UjF%zgxjo+zjN4~73udjH%d?f*;$*Z1C^}Cw)t0YvT$UO;gj^!)ZsymH)P{3< zf&vCI&Bs`RtoIm)c?LicTda|in#!2dbc6K$%wDj~Rll~j6Ay+iE@s{ez_V&O)f3(V znW=)RNsh@?obg`&@Nw4ruGheNd7IzA{WbJ<_B+}5xRZkY%vHQ~07LTZ_Y5ZEe;<;h z^ltope6_W+Gf87=ZDQsKO%2zF__N!RsAT!jE>m3GkZ(`8PVtNd`S}4HGLOVMWW{`; z-V;k3EV``oPC4hIwC&&B-Sl#wrMq@?;Wj1k$1}BNt|9KgQkhQeS#*v!$zS)rpSB@o zi{cA%!imuuHE%ZB0$ebYu|n?yU?osTCG!Gq_g=-^oNl2&*8i0mF8m7~M9}&tHZP+s z4qh&Mn~wfXDXw1l9#fw&b8VaNEt5-D8?YF}%+*{$}Yf z085`O6<7-n%BDAL5JKM)hxFRwdib|;PLm?F@A9eTc-9npRcLJCg!l(gN=2zc;c6l2QA zh|?`}b>tX_s7EBERo`os(i-}j%+uR1no3UDKQ!p~jBdk1O8V3uiM^S8OIg*!`j%JX zsk3fM>6mS$4ro@#Y{}+V!2IVYW>ELiVRe@vuvj5Y!P3K{PKW(ROAKxJN4s&3dMA+g z4c@Y9xYgcVP)dV-WjJx3rC&a&`@~S!lakX_TPo#2HF_z zviI-VIXR8%olL#F8U{@N>de@?4J*CJ&UsOLd2RGglZJ*v$+r_vSHJ>%MB}s`S9q*8 zNZ`;q5$u|=XBlG#24pRYOObMlzYaQ11EguPc&8<}JHEDL$`T4VOm zO7#SLk}7@>5`iU|hZMvi{@RF&dL@f>VFQC}KFIfdM2WMs10w>I8JL<^>L1tX8yRbk zncw$5T<}R|lZg)zy`~|DR8*IK%wWX^RnO z_VrLAX8{`4b#_x*yF8U%%WBY(2QkbqEXO2-W$NDSz18Y-vb(?URyrn-@rb>@?vL(0 zqcnvZPjyq!Uxt@f(B)Hy4ewhPw}0j|b#yyXPRH8|d_z6Xq@?9{+?O0|OfqDg7+)PP z|5*Ym-_FyMl91+1n^YP_6B6iOQUdse23t4R%kg!hP|0}qKL_4+F|?$#2|0ii3@*8x z_!y~@j*#*-+FPZT>V4q2ch9iV&BoI5v(md=RwI0WkBnR$Cq*p1)|@e05Jd+jkMqQ! z>y^3$wQx|gs164F{%7pp$S^9~%L0Q{HEXlTkSWQ$AFmopnVeRtL?@q(fp*|x4?Q^o z&XU)=*UTaZ4@ib#$wlzBWHZ?6da06GB3Ditw-|@^K#^`a>DOGp-(O8#F9fPag7{y@F>`|sZ@@E!tD ztzu*295`oQdTpd*68AHv*hdg%e!O8^^)^h;sEPqm#yo7uTwmzx55|o?f0#DJ!1V|0 zXMhf>*-0(|^1#V-lz9~)C|L(ACU2gq#sSljeD%79EWyEwYpV3?RGTSVl}lq>A3j3PDOzJ$vN?A6*wm2hYHuDsu9fK!${R zw78pVpw!+5rS?mccf>A`il4ZtvE3l~=I9T&qDfoNl!4CzpCvo{+tD=;IJeEw6XIY? z$i5q`zzPlmS=&nRrc7K9Ga@K!hzaC;OeAqVd&?P)b(f+Ug$-3zFmlx)Fgq@?h1~ya zOjP@|(Whf!Ty9Sg|Mv*lNb!eLzFv4ku*Tzs@B$mEvki=q+e(?%uD71ik+@R?8gd1t zIAhh5#h%CEhz>NMFTs#_1_Yo$9Rn67%Bo1LE-r?;JN+&qbYRu3i_BMGU6i+B&=Bez z{HrCNYf1^7_UPzndLfu=8u$KdOALkq80O39Cdkh^2sNd2k#8!r$?A_?@S>}_oH9O^ z<+QH0d->pe?sC!pERPydv&edn^t-#^xcjtKZXaz2;)lHE*aNG+6$Udnh?IO;VFtZjX)CiOUTp%Lfl`#K=%@B2>_i!}ZI}7#T*1 z?-wN@;hkO5Zk}x0_@pvCu!ppRq%-5FOM6-+$GRwj-{j zpg>(y^JjszO>t+F^WhZ#qv5dxqb>W$p09RltQeDRj&t2lQYU$wHmN%E=8Q;l} zN(;v!?87v3HJNo@Jb9Bn?N05$2=%%C-JQV&gWqG?#xoxs>C=9E57iQVTlfxr^&Oo- zkN8RH+sYoC%%PjIcj5#2L3g_Qb{me*{bb|b6D%v9B%%RIGe|bH$FiFvM>jO%QZfRY z?^WWllw;Scl_l!W(|sTJoe(Sjw7kSl@Tj;Um>+rKWjH$R{`B|g-PC?3Jrhcq0=0(A zi&N8MbbcQU7RME^hvf`?0=sBi=K{rKzbK8y*N3dhsQj$o}cxsuB#Rfya zRv<<5s`@a~^ErhSi^npL4Y`vde)eYqO^}rZB$Hs7nW9k;Y%CXzX= zD|wTcC`{*N?v`8#2^ofa$v;{|k%;;8PH_hCQ({%hawQc zNUo}?T6^2dhLpXZEecZx_GJ$@?pt0z5-N+T%`4t_6Z~j-Q=rgk>H16?dA#5H*YZC zkTTR2k5f_--5QBLUp+q}W{dJ|n@TXTb8>d(&QgMDybYlSJm&Xu2j|6zlIxpBGSC=K&~p#Z`6^dn9`JJR56-0JpNGf4GO=D z3woDnxPAW2Nplw18O{E z?OqGdh{J%>6=<$KN%%fKo;qMjK~4@*I#98Qo2?9Lm^57z!^Q2unMO|Uw%AtRI{JLQ zi)+Ejb6^yK#t-Vi1Qu*qG#TSfi4pO5Ha-~pM`cmNVcw3qtpnC31Cm-o+hl^}j!eSa zJoLqQY*ni_u(G%0;$>j-i+L|1x zdHluEdD1$yEk@-%i?1xaU<8E!NFFVC95g0+>-oaWsnPJ@dI9O%eA_1{K6Ec)wT6D| zFogzd3tYWXUSi%wQAMF=sP<#YLb%B+CS{!W&QA-hI+p*yV$h0g-+ryYx_8oCXwzW= zsmq~~c}@Z}F*0_FVu$CAWzgFBh#I3v>bk~A;EKR#qtCR&B0$!=YmM4(ES#Vu?@0AJE zyCuNqw^ZDwZj8-9t~2 z1_Q8@LgF??5hx-BXzJX^A;-B5ebHYOqvVSh}D9;yU0 z+HV5_VP&!l0)&PpndO8rwB~S{N{lG;6AxmXy+Zq2wrK`9Iia`IVa!EuTq|hL;uE>5 z)4HZyNq&B@r@D1&#r5mhQxfw+pCxI1WMDw5Uon7G71l%mrcx~6b6`&to%`}d9tNqN z8%!Zmsx=$rT%2?BOHEVGzUD)WyrjSiQU(c z8Q}llC)>U&tttqQwz%n@6;W}fsG#6R8gfXon$kFNC(QnC6jD!DH!Uk`j7KrW;`l~9 zvAynQ#pr?D#>RP5sg8@fs zKdeI?c4(L7bjj8(mzS4^e?&w`VtPCqz{`3sC0&~>rs^}Xveyx-8jdVl7y3!s6C=Cv zKEZJInpDg8`x;0r7oEoq21W)3q|zi%or4$to{#r47a{t_gjcaF*EX7HvXR}F!$=qM$j4yK#O!*NrK$KmHo3^~ zVG}*UrbR~z}jSb-h zX81waAugmscG-Y(jLl$FRlAsqF5Hz?-B7tlq07Jgr6jy*cD}1FqlRsa((Z9N6vl(*ZDP+S1ZYeots)tb$j8*NP)DL`+CHFT)38XU@>L zN1M4WUt%8`|F@>w{M17j-i3tR7*_`er4$Rf1im8ega##Jy0gzyH1`tFHOfm)!^Yz(rOOMw9?;ITm|dZ4N~wz#u@A5)ln zUT*xrN0c+lEO9yLJAs;NZGl9N zbHT%IlQY5u7d8wr*N;fVe{{(4Vw~fbEi#rQvoKAzWcthF9^^?)F#hXzjB?jC_L<)i z%{4`eBlH^kZ8SBZezVBA6=DHx@tqY@!o2cqp(}g=^J@(lsTI&FS1w8(MfDU5gM-j6 zvn&1Zm3k|Fbz8W&jNCKsb)FWc;P@%Q&?}9_@xAnP_EUOR7VJ?~>{D(}*!#mI8Bu9J zQ6Ef&xCNxg4_q1)0K^=VsBPmNTTSJZyB>nvsn;kv+4F7zEN3?tMi-HxM;8yzj$lWQ zx4Sw4vd}wG_qn+j%p^my*r^%zzkes4=;)V5!zz#tlyPClCnn}dAGn>BmV-`wfuZ|v z{mF*_@nf&fqW8SSN@&bf#Zcf|*s|4tHg}Z{HI`REF2Goit*!EkiSjS}#m;RKr?!5! zoq2QiEom5xi(Zi&PL@Qz0$eedK_$!Smue6>yK5wHjz#Z3`iP`? zKD}0frUyN=3#x|yYj*2ZY4oh&B|*fY5HmXZgV_(G`PKQqjAZsNaTvD(ZgMj$a3Sa| z7io);Bdq?^ex!~=-&frtxml9SzWWAvAIK>hg=1WN=lxBwjvuF?=`ptxq?93Hd5Ah_`9 zx@fII*t_qGa{TkyriV8*Xe65O`upEP7voZ(=MUh)bJMl1Yu`HnB8$_YOo5 z`Y^z!J+YDb(26p@k~KEYK}FRET<6#ilGuk_(0^W-!X1HlLBnS(3P>!LL^9r|s`rI9|1ISth&qvx;Tyeciz$g) zsq(RI_4?jkuaTDQ{WSRdFu%T%h3!#JjpaJjcWBoZcXT4^V*`b&^y7nF%K;Zs9Hpwo z?{pL$j99}wT^}jqpyfWPP_J>rVj)7Ad*TPp&v!w7D-r>t`S$3r@5jT+A_{nj999pK zy+;vxWgjbsCp_jycB}3xYo=J};Er!ZiU8$w|2}3ktw0;(_T799o$mU94AU!55jq5x zER%;=9*!j9a79g*hB2vww~f;>GI)nM#ZY{F4I$37XV|B6%g=2UlZp`M9?oFl^N_{G zrjOo)OWG7kC)`82;bh8r0S8l4h|<^tfg=3dZ`K+*WS^9pQr3C6xSURQEqZ{hV=GWA z7A)u;;Bsj+FJ1i;->Y1qI!#u`XJ@0$PpE(Qr%tytt0xmHOxR;usnLjy&M2U;0NMn( zXnT7*c1#v@;DZwR^i{0MowlUxQvZ zz^@DM3VR;884j7>ujkccR>r&eu71M1G~&;GkL{>KLTnv}M27zZ(~G36CiESsJ$O3H zgRs}UEw)QiQ}bPr`FMp`jGZ)^y@vN8lE;QQ0&al;Ku+>&n zTzP6~*GXyHU?9=^+lx;ANw1121@b5MFjZLKo%%vF_FP0@KY^I_-+imL#mvP4#XW(-9%qDBemk=2VncTJmvI1dGgoG8H4t)F3S# zj=M0v&kji4ztev>zs`MqxwBU=&V$+=gAQ#fs%2rDksiB)l|T3ux5sK{2d$yce&HhXsV0vg?EA4 z5+bx~p|y4O=jzYDhQeWCIQNJ!^M3oxJW<^E=)ugKkA9=GM!tN(x1V=(t8Ju>pr9ha zhNcCrF9;^OfI}r=Fagn79KT|_-J<9mxi?&V* z1PBd#5++yx_R@Tv(t*9-g*6fOU@P4pE9mLs^~x+Fd?2qL2;){0&})^6`tm)rRB_cxWtg zzxH9d{1Y_1vIj~hNQMyfJTX5M`y@&I;lO>M(I&k>O>UGYuE+K4c!n@oOKT)ep-u;- zQ8H#$?6$U>tRU32x3SQ5W810QY$sXUxXH%?fA*&eiV}ep)FFw7kbjUSyPW3{Z2`~0kTna9%c*ANcEJHoUa|w z1-4`=TG|`8&z{WL5l@0M8@)Dc)twxaVTha{la-!k!FXb9@^rN%zD%?SGCn(ET<-o_h6 zI@#tww7Ok^R^*j7rpU54U;N9U;50IjXEpve#~BwJyMJ)N81lQsfE`c>reGCOM_?-} zbZFLJy@lnvIv;rT<3^kev%4{Svo=IrH8LWaQS$8lHUGTz-D5m#)8;eKE#XpT(BUJV z8LNf!+sbw6q_#KGN=o!lKc^8Mjqgw|@50-Yo|8iyDv2Cwc3V?pC!eaeQidU$l;)}< z?uQh{qwD-^ivoY2`>i~$NIZo+S^W=ML|yH-!dOU5A7a12kH2DzgBb`M;@*kv?d-Ui z7@}*}28V_|HnuJna#4m1q^au1YjbELgVC_b;;qy4!ydDpi&nEdPj?SX!L(){uRl20 zWN=_84tHR{+{&J9dTfD>UY*{6dDVFN&=4nkX8q~ls|C}uXfDN3t^vuscv*J#d-qcN zEoQi+B(}X;0##76^wBa*M&-{3s^xHPq{umSK7IkbLU9zmlqaJ`_VubQuRr~3`AKIm zmxP4Gotu0zfuu$@WeK!i%pq4K5qZ#qNl?q7kndXvj&EjCjIyl!dP;vv*?I=@(-$Ii zU0hC)jI;N;`tNQSK%*9dRGDaFqeKmHeFX>_%~m<8zFBJ;h{Ku6gaF(#KMB&_$5xb< z)=o~Btq;eS%huBC>TBMXu)STjP5)fA0qKKVCm$B`Oi)>amaYvxj5+IkkwGf!q`so`{SYLpRa#YIOViU=h3Wp!4?Q(>=YPAn z5QWBWMupei;@0_>QaB;}p*~w`xq{;6Of{4g$s&fzitYZP1J13-karm7#!x^}MYjS-c8fA~aaZ?AJp%m?F7%FrT*9lG`o-YG)K#}Q^Z z-Uz(+jJ`7+bNz`c^Uv#;Q`|wdOk$~!@iqiPw>^;@rFV>*w-WmZLCmO5cLZ5vuP)C9 zpSNDkh)6Mz-(_?&4Bt^^?*|JCCp&vPrO0Vzc%$ns74NG`19cRsrs7k|A1ent&Ag7X zs^8TLFdx=qtBQY(Nu)CzC%I1&tP|Evr-zKkmBVPuv6zrDt5j>r9)MUl%#fS9DYOws zzzo8J-1DQ`^}HpgK?=q6?SnM)`v4`5u6;du&H-B}orqKP_)}Z#`7yDaT7MB&;k3FZSm>2Ey^SNU4sVuK7bTu^}Ol(Yi9BMf@g!tY@4K|~O z`?Xd_O2>9j%8U9OZxlX(;N{c4PAduY!ZiRkHTLmq2U09P%=Yx#Yo|w>qk0pZXPZZ@ z{9ODL2rM^4NMr{%74zlxTre(iM?ZgeN!in_hk32Ty;VU$tXUVA zKgV2L7uGYFXvO9N}PZ`|#WyPK3@01*N9Jzj3?$b<8vyBvRG%qHD(^>gU+ zxvcc;WdROK!mzG969$vlpF2B$b#^j!aRJ5C#oQGykZ;4EUV&)7lBd*Pec#RSm4zWc z(oDxJmbr`T4Ks7sqeq32TDa7V5KODOFPq?$)Fvtha4k_+5?O(VrSBWgpXsA4xyp2d zu{J!oB$JCb!~XmADk+;evyXGHj?IXMrd4MS^<~4f7=7Ee()pz?b%zZUH)r8iQpQx#IXB}8a4il7ZB45XINZg9O?vQD252V z0@^NmvweSbc3Sg5I|94ivmN8jdePHscvL+)cXZfn({U=|mr4rPPkbCq1E1>t;F=ay zXa9dK0MR3UbZe4-BmCOSnAj@z=y;Hg7upn>7CuNTUR$%~xlV}lsDpvwtwEVVv)i4- zzD|AMIeIY_s}>2nkVI7^W`ft!oUW3Oywt0$b0X#{zZ53X~7u>bSo4(n%M zuuQfsA9Y91yK!U7G1;bD>Xt7k-}Avgnv3HUgLAhb#n+UQ5<63Y)s)Tt&8=AsGWN@m z&)n=in=?)0>j%GGLd}Y;R~t>!*}I%X=<#ugm1#sg=V42;qBN&0EU|4YODh!gJ+Ng> zgynKUeYtm^P@kRrMr5b~$I#x{{QUg3iH~AA+NqJzs%#mW3ZvTjn#*Q9^OI~cbb%hmRQX$CA2W@{E3zuY^HM(J=I6BWI(i0srMR0sB zT<6&4w&ucC>*8*C_@ishgsa_|f2^7F9%q~P^1J!NbdKx+tIjpt>IMdi}`_a?F{TYX<7nOyqxlG9*0V;<@E@w$npQC-U>H ztBN5L<~7C9eeV{KGwFT!K_zwiU_Ap_Stg}=HgMxPeZ7TPf7SW9Z=FIi&UrZr=eGIR z#Kro}sn58-b`)4M42(dKL3#7$a=q~iZgT$;|E$kxXXtYPyvN~;A=i~}eavt|^}$KA z;qmu$LW+=Q+7Ydi%3y;6Zr1!ZSv~*}%Z7X{B=#)!y6u-8;$djA1SA79AR!(I{tgX_ zJel;4cS{Um{!|`;hq5+Ao>u*FRY(uWB>YUI~rWT1%=e6gw5pwa5Hw)^2m z_}eizx>26;L8BK(GtmtO*^Bj{P|FeU8$W!uxXC|LyNKQQnL9>QWt1mxP6!PK;pFDt zMNH=wkUV7iSrB~EQfMOzF$J;2*it^A%yMj4B4@}4`_73E*HFj$!4KXDhhojtn}k?f z$1zh-Aq8Wjr5wJn{qpkLntfQ_asiM;c8kX|XXiUK*Jdir8vWKr3X+q_49Cw!=YFmh z&28N!g;dd$r-CU@DZ^F-YxIU5{~f8lqc{quttLjGP6B5u!raK-j|p<`MC7mcWC{8h5*{k?Z{T5{ zZn0Zg?Tvt}lE+ozZcYrXP#hrvG_PdBQP2E<{9Wxg% zU4BBvaYw?VA`OKu6UyHQo{KxjhR&SfF|hYeg5DPu+AxHWN;e2SfXnXs zP`4E88U4^+fy>hNLS$=ek-kg{4G(da8CWgqkgA*#O*ZPP2RzuqGwjs06a)`NxC6Cr zdpmkFBT~^38HdCWEEW|sA(R+Y4IQ(+-iLxk?`y-(n=ao(`DOv#FQU)@*j*bqaM+|s%v7aSPbt~KM)RT}zAb)w$v7I0# zYhC~7Yr@VQyc%M{g)N(6p{aQ@3?hz(S1jN)GLgwEM8111`7u3~N z=$50_7etS=HFy*IEbr?|YwAK~M%bcG?4p;E38nft(J0qd4FA)^bf3OqMd}YYvZgrZ zo7xmqtLXZBP(-nTjm6h?>ezPF@^xT9kiU|l!Qk-3Cx}4=QXIi(fQhlXv$^A$Ybwu0 z37jK{9e;&*{)x8;$d7J|rH<*J`I%kg|FlXiDAQTz$QoU{TMGU{i`lFaW;*M${^7%R zWP2vQHrD1-cG6EW6>7!3RfHHV$;rts4H~J+=B_uRyE}-i%M6>sA|~P792U8tH<9^> zv0oh~Trg8$Vn`s*{MtbMfv7c}`p6MfjU}Rm$N$6*Qx#q42Ey789h(bD><_#gFqsAT z`NuCL08Y9#u|O*!4nHs1;-+NQl}_8*KYJ$uL9BU=ex-lAUL~Ty9J^Frtbvai?4%=s z1ayUq5=#hR-tut*0W6L>V~VLzd)k^t-iQc3?g2+-^i3@T>r0}NUQ5^rAIWhZv7gFT z%k{n(Cb1`W)?@vdZqIOvvXGFcz75xUvV+wDBP zC-(2#5AGm1WNPNaoCz1gE8FClEg2u`{v}h0)`#4Zb)oEb2MoNlqbrmJ>offNS0(;s1fREZE)iXi zGq3=ml7Y}?+Fqp;_=x=J$6lk!@P9O&cR1Dk|HrdW=AjcJ*;2Pv+56ZEQQ2GemTZw@ zrbraoD?59Hh+`zlD0}b74%zX0oxa!4ANO@%*L~fb^BM2=>-~B?pO5F-L^(mRP)TV5 zQCI$dvziTR54DT5X@X?Y>K}v9w!f|FvKQ-SJN+rsRJl>mfXo4XJXE) z*~~Y*>j6VjwfR@mWuNwW@U=+Tyy24y6S>)w{{k7=fCs@IQF*c*4RvuhwbQT8wt2Ud z`?s%y$@0&T;WLG-rlzyDXBYamalUGK#NR!LXelcF=u(pIvxV6%HLR8Iyw?iRuzy} zIl#nd(B8wx=aLHXK*!y2LG|c)J-LH)SG=#x;Dva7vQsoCO8Lbm$r^U9$G>P&D4qs! zrOV&x%>P&aQ3X*(_ah$eZLT&Wh8nMQbkzS`@&&-2MfvR<{378kDnE3;0MI5>^Mus< z1(|N~U#5(6zr&-wv+j+B2KqjqfFBj>1F#t#N?I1-x6CHSiCke|Xn{V^V-6auV&sBj zxWQeXQn1gz|L#NcTKwFZcN%olU@!$M9K+&_<>dncCE=dZH!^1Ys(stiliM>qDc3&9-c6%>TDv2lUQ2faq#$sr{v)&SPCmmRj#p&+ zDcl7VsnE(RiZJ@Ip>N;5_+wwtf8^Ltv1BBs zwmv*>wI-IyV!9R;v>b2NzQr9diK=~t$H>Nm;DBk(^V&+^c-Z`mnBTl?!^>CvVRG7! zKLoL;HY*yWotn+f7`lh_;+C%j%Y!{u7bja}dUr@DFGJV@$wJvoWjq(P#xZm-)ReNd zRA@ul=NBpfduXVIB9|j?8Y39OAn$gEvff_!s3k)1&jw{xEyz@yjBva~yK?)ZR!pv4 z?In7n4J$&6y0;AMs`r4 zSdcjJy!FsEV%cvS$dZT}nP`CK|9W~a&>&@6(JBW|$>%?x z!Aa^&%7;#uTW^>hU>#cbA=(VkN!!A@B}R60`oAU260?i!44lT9a9bAT7*R53u<;kA z^5Ev4b~(mTv_H1^)f}T%ZLo03^YDei;+#v5C9#*EICZ(X31FJf#3p{4L(PcFoS6D5XgK+QBWZZMrOyd#C z$3NK_FJYs<#-hf$T1=2(+-NlZ#{1$`5l6N?e*yBzpQ=jkpgnYe;sdM-Wry)A^_i=% zjYVcbKa?^&ujE_0jD$l{1f5#+=76Cy>l_I11nAq!X>Vqb-oMGi;`mD{6ZfiAA&e6A zY_tDV^=~>YA(!WJS<0f`Wn^*pc@RyoYRQ|=#QIdwq`Yn?6TT4|PRRs_ir?P;rvF{# zCU4!6?AS@MOM({6(C_(WQPXK}IMMe8bh-v*i48pn_L-XqV0}T6 znzIS39y_YyzN#T2h>4#a@>sz=-+-@=IOo%3KSWH;;R69_Pl zJU4FKo!mZ<(+A@Pm=c1q9i*kJGoxQCPF9La(?s3mQ0-u7KmL{oPfrT=0%_Bs%NhC0 z|5Sej2kv$>G?($LR59AnhY4c9mqJfoBeo4eELtesrY%g4Ki+WXPPwLTtc4lDdrt%3 zmqiyIBEgxR$Uwkk*=OHXKG^V<@pYS-gP;*|aDZ%%u>n{`FO{;v3-yuv_=n*766Cc( zLJ~Wo1un$2|9tln)X%zkckde4-4v9xRE>gs4AZ}3aR^p0PCQe9M6IBbCrTXD&~-3x zzF$N#E2^P|f@kgeaJ+`sD%QbgD0G2=?qMc^bvbfwXuKk5@64VPfv9r9wz(fa?g`KvRi+Z*OaKlheTrM}yF4>Ks@|yJk*_eVGumi5p6(K+?!AI(=!2(j z$JEtb!&oFkFYSSFGH3WuEdL&v>46+K%6^GJ4#6Z7;O}dHluhM7Fp9Q+hDfb7mz3(= ziRFO4I3V{8Nw3tiKiX5g!1m8`JI~0&8QfUOH1dmmKnPPd5ywJwiAsTsNdu-$uGW8^VenZa`u(TS_hE!Wc5OK)8)^R(Q;{Cer_&=woDCYXu7>KUTI3XrhjDg~ zWyX;@&lhz`gy=0d*Oau8(2RUHzm!q90^E;3;E7%+$on+fJskZECQUi_ zw6uj%HS+rH4xF3Y4o64zae1PL_(9D8VflFS>tBD%fKWWZDEa71xXd>mzM(%-P;fPv z)ft5ezuBnCSj%TpaL)TY6Oqr;HG(Ly4!2Lr6}nB)PIo%Xe45!E7c4UEzXN9k`b~ee(I_u|Rld7m_sdyiX zX-urwFVcATIb)EAzAX6Q_6VJ$#j7S*qdx3p3|p5<9rm7`{M`(6A^01?Kn>)YIE9qN z#NWq%%U+7!xPcq8RG<20N4duBtjl7g7U;bO4)vB{G+y}Mf0h^cBj)iagD|cXY8Ust zCEJrYl6pGb%Bn7C=@_99<2eVuM*OD3^EJV;tpc#-9R>z$rxB%h`H#orr zu$x9NY+17^iShqciWh=JHdz5mb8RFQmSO6<#4?%8^pgLPt7NdOYzU{#ldQDeA|#ZX z7ZkmCj!#aZsz%@Qn9udG<61h`)l%b+HDUp#7#1_OxiNpE7ts0i1~(e64Z%fPWQmbiSRPje=KSbUNU*6aK-5dY6EwSPU-N>~YQ^+t>ih0M< zyA!i)e#4xD=(p)?(?&&K#jP<3`voE_a~$e%nC$V%_-h_V1qIP1pcU5AYoT&reh5 zqh>88o_vt@G6Q~5J9KIMrJJg%6mVy1r#iA*@9BWp!6zfmv)YYq-I1JFgNd7fEL8`A zTUbP4;h_ECo4f%^gv9Hn;PH2_s0g17Y#m~8A$l9^F_yDj1gsHiMc6k07vi0X%gtjz~aCf=Dj%3kdVouWYl=#&umY4TGgfL7_Zsap?6%}dx}g3DnG zoa8cm;%gZjeapl4KkB83SS937D#kDz*P0G#uBxcodIukx|!o9I8SMXO0 z|LQHDo&0`j9H=014;8xPLnir!sL`{6<{Z^!)&>48$(Q>(W;^8KQ3ZuC4@4>2UDF(u z?wn9Zwxik#Zp>r`VbTxUu0}Jk4;%|701Q`p+ z!NS*{d~mdt*bl{0({AZWUTPJfJ@=I52}-={#8+z0CvC}mi}BkpqRS9Iqrhf7;YNLN<=&z)X-`5_ zmE4zPe%TR>%@|$<9-csIk1^dyCb3KPtY+;t&ZwKpROYVa+J%Y19lO1#Ma6GoZeFS~ z2#D5kHy5Bbe1c!X(LQi5h6j(sCO7UbxP)8H++KE6R#ea+2o2WH!KDog4~GH+em(7T zk9{&fr#?asJbmtiwEc$g(_CL=o+o7a`Mwu|;6`-#Ia(UyZRiJU>?e5*8LU7ic3t<_#ACe5cix_3_qb8 z50%P>$P6MP(hKd9$jHc^%DaYtw?*BQBSLTaHeT+4_K<4qmUSjiSCDMRhBWg8eNxYI z{nYTVZoz{uyBAb?e}yjRhhhMn0+Obe4-HiVHTkxw3`B67vj?bt^;xJoi9;D(NDTWYG}Kqg&Xqi5mojlykNrxXkF~WC$VdgY zYX49N1+51tB1%RCA$=GW+grMKUS&P0GvtX7A)37agZd?~jHYPUv4nj7;FK++N_Pll zIX4(La8LLjHvIAg3=z`@V{7_ci;tu9Vankz%yVVFXcGO~on1JbgB z2hh)`FHvTr&VK>!rLQYMM`#s*ZUE`FS$sc`+yDTgpxilMnlPY^O@>+MDNvYg!r3}@ zJk^u=xkq#8qvy#eV+dRS^$AjM^9&G%JB{*iq9Yl0L-DcQ7p&X%6i~; zob(uaPds_`&oDAI6qYFAv4Z-mUBj>5pHam8psWELIB*#Jw?;*fIaZTr#7hZ8Tn+#Z z2`L5w47>5ss;JqGo^LVT>uQoh&vtD*{^NYQ%(1d1oR>pMGaUflh##iK(C=jHaD%(| z!wLLW5b66n*=1+ZtHo~Cq#!a20B~y~@%SY9{4Y>sTHYX?Ph0T(cn}yP$q4OhT|)a;T9O!H2(5lrl(RvN4NB7qc@rAZq5xn(B6MSqT$T1h0x8=XxEiLtlPY3`0hr8ku zh0mKk*XZ7Te(K1Eq}I0D>sn!AMBh7%7 z*R~+A!}dKdMb$(t?M}x_X49R_|K5-`_s;*0`IS*kL&!=`p4)FH;imQB_D}N*y4uj5 zW^%$?Ip=4pxSZL2`VpG*>V*FeWa%hzP~uBVZ`{rl@2b$v&2^lrH7L&Vm2gkR0Tu+v z=!yqLDF!WTV;Tsnj~s#8kg%$wxk&O*+G0$`m>I z;G5`4q=1{Cm~mY3tXT;AP^Rn}DZarZ^;%0GI^U9|QcHy}ws(4t%t2q})ZN0Or6>Cq zIeXX5M0|UkBfsF|cc7vSudeTIn3d``7KcgQ6Yy&1!iTDh8-G$#E?fuH<0AdoMi$jLgjQtR%I;BW3>jic&+v%^j? zedoWN^6}#?#LVaT;dyh*OYz6C*&1fi9Wwe1pPCoFB`Mb*ZT+czibr=R!x!&fvd3bz z;WNnojKoJdR$V3CNnZJ^xVo1(zB_WvpS+<=Va(7>R`YrpS2a?PIMq22x&>v7bsoAy zzji^A5Xy&R-}z$nO}fu>CmFO?CR0%IEo;z9clSRk2jh1RB%|ZJL+dTMSPZ$YYAps>fEp0~gYtLb~&u9mbO9F$f-V zmRv?r?{gqdm5$$d1zj{uHZtzdhup&-6fO*gn_UwVt)Wpra`WS$d>td%Meix2NxS}u_ItF7O&B(c*tWC!@;fD_vqVv$4iMB5pVQFvih)xdOu&oF<<-C5G(UxHS{0i ztp=1^M-#p}$Nv!CSPuoi@&_wDkL2(5ZRgdSAQQd?`w{#^vzbbq zzFG3oJ=%7(SgX79$>a(*UTdtT-{dClgOTD(DDWKf`0tU(*g7|@@oy9s;w!H0r#W{Z zFsm&n@HZ^TbUu=$2@$m#xW_Qd5;<3^hve8@)~T!esFf%hfN#fB)-ngD}3`SB>8qrrJz+6YI~nTtT6}uqT9>|0}(3 znXcc*ZtmPjB39_6BV9dc=~PEo*j@D@FfuyWF+IU&4av>BwwGf564Qr~t+7j)5Uq%1lfSygx*FLRfJXV@q; z(Ff=(hKe)s9wA`xX2UH&`6~|HN8!S=q@I1!_#kb2nAGH4^D6@5d5>GdwNX9WW1;PTH%0yz6kH%t-2D1ot-s+FO_|R4@?I26yNPnS@=8l6nQm^$ z0OhH%DH70q9>y*n{(QKtPrhHrkZx1$eBN{Rf(sollb3q1GYIwmnCt*Mb>s-sPp!4V z!QoGP9=daW&AhsLBb7v$5*HHOOXfik!s~Q8xDw2KaL$yk^&CmOm9gEWv11*V3lDp< z5=?wMjj4vY5Pp|}2BWK0vtYw|WXgda$7AiPn8AI34#8b->8>H{m4bADG68eEr$XLX zB(!njCS$R%>t+Zw>~UiY0X;u?_cuo4KKab*spbtgw#}W!UDfYj5RJCd?-%@dOCn2R z#38TBV&0-R3ye5^_5H>sbowLnC-Mq;^eb==jN*G&p~AG%=X%q^zKWC!GvD3+bA!Bf z=cHvW>pw|{!6>W-i0QZ5QiSegk2qwxtdEBy!LJX?B-71H-5vl=jJ}fIX2?tJ#~i>x zBw<4ccebYFL^{1reE+gRIRXPNgA6UH6hHeU+RaT7oOAa0&-oHZZi^tg3B7NtR%`lI ze4D&-%e%jHPG>qG*gdI^RWHzB zMC0BuZeTym`^36S_TGslJf%`v9At`RiL_7v7G+j&R%5((Oa-#=&Yc+;cuaRC=X+Zb1#7E7y&3Lm3Fk3{0*W_I#yn1y zY+bgD;U+o38=o!q5SenGF1O2NNRF+Bv9D$c&cwog*cA_yAOEDEOSJ(o_t%#wwk1sq z9w6lk3q5wS4?hGRi)d@tBPl0q5ejW<2;2>x_@4bzvgN<8claAQRB3z~dAgEve@phd zU54hj4lp=cYy1^u$y(t_<5t)3gbD?x5 zj=v16h^&&V!@sKF*4PkO{D;QSFGRL)qmf8C=nzGd%Y9k>)8h0%yZS{;`+o^6hbb^v(oQ0=cgv6#=mQGrOE`RovI|?8UCXo*aJf1F#{Hk#NDam4T%V(=MPp<-D%`J)Cpcph}QC6cQ%>KwzLKL73rUBO&c=TNv42At? zznP889v`PNV7r4F4G8NmQnq(chinvwIN#K2Ny zCsKvin zT}h#syYiCc=sB1Sfd2_E>^~?82;t0GR-8Ue5-T%arq?qt@l5cEUe916?D5=waykTO z;QdWygmK-Y(8MGdss%Ke;vCqssnhUZU{q&nYASK(ogvDI$>6>F3ZUPtMm4#?SjRzgLw!dia7TpqT^k5H6nXjV z=?vYxkH`IHE13cwo=rbHUt!A+ii&#$7p`gm5G~K3*jEy6pwtzZ7lG6IV~4;uNb+=p z0ND-&pCY9v{`X{b7Xa~)ij|Pb{7b*u7ft}e> zwQdX>48d>(SA(f9(4JO577r{>8#KH%FbdBg|3;%nlmFSVw%q5Hr+yV4=W@*eP+)oU zz*hi68}5{pTXX#Yq1GRwS$WI1?jDypJ@MoA)t|!^KpkoH9V~X`ZZ55%ZVMB}P)Y4- z?lVJmY=0k2gKGlkf)3O!)n^Z;c^h0NugismoQSnoOOt!Y`NkY&!OkWI8v$MoXcF8~-@jF#9aYno%<7bb$&M}ijl z$ljoFczgSvcq5$GCm#hRpAY)dcvBPia`4`XDK;qHmZTDWpJO(uj%cXA^!coJDw(*` zEzJgFn)XItU)js!Pmpw9EfwaRO5_aQ6}E&dmBcI+Eu;Geo$R+?4{Z0mf4GpiPBGo)8)-g(%-GEL}i$(Dni7e(EmdiOK=+jEc`-Yc)f8(def_771 zTVGjZ`|!J#v9gZNmCYTgO*=8BG}#~x-evg^)76=)DK1*$)z=9bwcn3Zm?d$Yc=ovi z0~|?+VfngPvExJ~xg|R}D7D2*%+=y8$wz#}=*7MOiZ+DBDqQ@1)LmYF-qrdmv}7f> zNJBX3R_Y`KEwG4H(qv($M^XgE<_zU5uQ|=}UG30a6=m{+szH2gml>Ii^^O0IX*2&E z7C*h1d4Hye`4CanRF@UN5b`QT{#Ht#U=&E{SfI!ov?p(c=!4f`^kAn%657Nu&HsHG zd;X+Afz9n;%iO4AuGBB(Yeg&j1S?Ti4cCBS{GYMa((uJ#V;dAGI!z}ssmYY|WFp!v zKzz1n=Ek6Y#)0pcRz|zKFFHFy{{U%0INRcE^XQXzuT;~bWC%jt{q%WY>MA@k5G>%z zB6*JLA`j7HYUFfY9fog{MAYf%eUTM`8VdWyd#IdvA<{bL{(Ak z3T)p$CtQ*6nPQm zv~-KzIIA1@u`-zXqU7?WlLcT*MBsM)u1bNim?eHmGu^niOo zVhJWQLl+>P{z;-)8-{v_s&H@x{x<_Y7%BuB9-nl(1ndNnT%l)XmhnGul}>L0%H9*& zQ;qq#ufA{JzRloX0h@hK1Zryz=PgEZu z9YC<>WI@}u*}3>=N;7{SZEPM=#C}ecJy(8jC4~r$@T^D$9c7K4=KBYQOdBUh^D^Hs zPfqqCh}hrzI6^p0jZ>cwtTSmmUC^TS+x@QWlctNZ>Wb$DSoohQI}H-MNelG34!3x# zR55oh&x>Hfhj6HWwqdNFXUY3-P5UPIzti&3S%-yp2u8VP@E!UbQjOo&r6u>4aIZ^= z+v_{NAC!WM*iqDv_o$05CSFb8XC^ipBnEKtW7sDn{cXbLzNF0Q&IPyFaEP1iJ(s9 z$+u%DYlpljC?FU)JEjBA{-#TKZ#kHr)t{Awb%lbNJNt)W!wpGEwSGZGEsZ3v#mcAG zI$T`8zfCx7Kp|8(Xh4>vsr|-Ee`%=fwDr%nbQ7nFEmC--t`4NiePhMnkD8hLj&q|n z8(n8H9I7SUkBamXP`3I*ad)Mk9W7Ae9>Ou=t_nbsyi#d4e#aeYrC)n}w8lvb1>_T? z93*S>=;^ZWcmb?_pLb1Q}Fz!m; zj^*$dYBMrdr!8|+^COCudWb#Uki|WZ*M(iCB*nyj`F~DK4MN6pz((ZQh~KvLsYbBz^Y;ps`6Cp}b6S6@=eldQr~ z<1(GrZ4Iszh$r$+3r{nzFNxy-eK33@sEQ-AN{fakb;@>uOBxAEbX(i=B`Ij{W<#jCN23yX6eOy z6xl_>+>=CwOCB;(e_ti0agl2^D z;8PRY_i(S5vup?tuQ)S?m7Cm`CqX0FLR3(!ElDjDA`{I!{`Ws-*o`eObIaM<+5H0D z5$I8%kj~$ssC6U%DYJWuk<%Ro4JgQ^APGyrP2|mOpKC~L8y1h4Z|`jVZ3MA4Qe)|= z28qYL4Bb})6eN@yntkmNJz?l59Yv4XHFD%-Z(r4->wTlG+!k0^B8*l?U}#gMtji8i zyW2TPIVhg5Cv;8!-SC3?`t189y?ff#CA1+Vpt5;-hZIL#^~rZVPSY4ueu zt_+zWm_UGk+nDzOA=(=(HjvW`x)=W+6wbA?VT}EDU)29@${e5r-!P};1(k$Jrlu^U zBjzoXhp}Gu@~^UY&Yi0*yTBT8msHbCub+eAeze^5!8PvaH+}Iu zRt5IDbkHs;yJl+^4K}}a9ILW(bcFI#YWoZOJ}f$rwz%JUH=qI-4Mk95qfO3f!?5lG zAuxh_(`Bd{Z{VUimW0uS1NK6JZV;Oo-Ehbyc{cz!HTpUY^Utysf?F(~yvt`xTl+;uy zzrFWP?5bGUV|RPr81-;4%LHr zhMDZ!5lVWudg6542{S5(LE$4NEUd#r`|`3FJ|kWS%Qq&N&*0qbg9>c3-x9!VCgJ{5 z4Zb^991h$xF>#r@3!21#x=ueo=}nV5+ghtMFOYQOmssbLR90-*}_1 z{*l1zsmxlXtnsPJOtlKm;S->7wds$=n<2)B|Iq#V$jn$#}iR;J*ZtB=#g+=;JGq+WE) zAaZ*o6O-jYL0Ojp;JE2}kJbIAvnP4Rr|tX!-z@=r^kLf5kQ+6kvOXIaJh39Zp~=ik zfAUsBCZbY)ybohCe5}AD5KiqWcp@QtTt}Oj@RCuNf zWL@>j%KYhenkbHpz{Ct0q9LdMR$_yBf5+eXJ5w(^8MGJRv2|rs+JR_`);1}(u&`)U zP8RTi`quYf4NrX@W>f3;-|oCBksf!EG;8fGBA7B=;YF_3SZUy22b;LpN(Ek;)5L+s z{5Qx!^8@*cew&S6P>r} zbtW@wX|%LZky$K%2_s0j%Se81L-vKtqXk5dLnw*InNzEni%{2_P;G{;is5Jpl;n*k zbeLE03RC`WWj;k-0kg;2Nyrb`tSd-42wDXe@)c(0(&RAGDWKU{CH;iB6UrNDkc_zt zd%~&Tk9>2lUhcj@fqq8nU3;vKy8q;y+)S}>O5{>+`Uf7eN2-GAtvn-)!M6JGxpvwl z3R>@d`#$ToSs3*Tq5DV7YcWpVX=n>n4e`8&{(B+wwE%%W?nx~@IU0T< z=37z_l>4z3rLPrqu`CFjn(sr?U#IAK*wXu7PPn#GfHyYP_v~`p6auT1qAMak za-K-|2PCaJWol|_Uu1JF(J67_$4|mfS}EpPyngX}pZdfp&^}9P9ohY$O76}dt%_7? z8W`^$^lMIS5_!#fWs(+y^RbY${i`K{KIf_@8~ptO__&OrLW5$t4Q7oX>`6HD%rUFV zOJxj8Z07=8Ct#34VFL+AxH2PK`Q=gI_2h1y>lfcTe=p2I{O#G_W|qdsUhBK#!uOo~ zX36^B@E*^v<+Em8|E~qmuIkS1d)2mhwAZmIb*NDH;Eh##oi*m-n=a+Cl^)BRt)~)Z z`Z~tEdLn0EE>`YRp1b7AM%cSEv)chFa4k=>8sE|9lT|tSSy>T@ycAaNAH72~jF=;j zp7}0;VF6bYOkyGMPA?u}1Z?YNK6&dz4vx*}iL26~(7f?C4*^X^LU`?lh-$Sifo0DphF88}1! z{{Hs%Dm19yV8jh!KcB_0uQ=6_tKGcO*nLi>SC?Z>pbE#pu4lN^)UXl$4c497nc5 zgW0(IE|zI(yeYy3d!+~_(ZWwm6m zHYRQJUauZDm`rD(-T*Q(z3UCo0SXF2_V9&ggk2RtyG4co0B=XK!|ig15S!fQA%)df zw`J!vJZg;Y(4|+{C;{8INl6&;1RWxUYJ;eb`55gR!or@v=O9;ZFn>o#Md=HZ6}{5% zpQpyLV0ZziRSEu}{oo(mBqynHkK$TPePsMzhhTSk`ER&gIsL!tU075ffpDHBd)6#E zI$AEA^CT?8$Svpmb1ur;T3W+H!q(t0k?Rx`dC;4Td3Kz|@pL?cYVP~fr zzXJqI#%d(p(LiZXG zC9QZecc>z(lOu(mtL#Y?EZ*>lH2QgM1#R=0+t4c&JosvP6R@;UKw}ICWG;5stOHd} z3uQ>E@LjOm?XsLLJRA^u0TGtJ@5TjbT2vdP(leVHm^fc2#0xVI5wfr5Bl5gLx%)&)f2{8>dzq0O4U`h9uVY^C~LhKwIt z-Mkb%pW?R|cu~wo9qLXk6kxSD&iAu6V1?4oYfyYatHe+)ihlUOWGnTZzh{SU68D_D zA_+4o`tKqYa_(Kaa;3+*lDze8Ze49H{Z{4Wj%QgTh5kW0HzEB30OZ#~GPq%k1b=7X zEs=4bhZKso?MjpDEep-Y$H_IJEbp(&VWGqOeK|*I0KtO*yd>BM&K6Bi7qgk4yfvp! z%H@t^QzeabXpVpkA82T9Zh|0M=6JN1c!D$b9nE>{ogmnM%YHfpYt8)@{o+Kr3V!_z_&lM(;cHK~th@QXG^$_b{&QHo$D);%k$-)-K( zg$M)y7x;sDm<|xmaMtL<)uz+cX)x-z>JH-sUt_)lYGdH-I zYE~$Bo}xZ0c>O#&om8WYD;S9)FW=mQz%RzR=0VwLADuO2bz`$#sF7s|` z!gZ)=;kf_e4X2_OTyjV`fq@o;YeDF9>VI;G3e{H+hxD$}=@kuMeL%XGdWJg;m7IHG zo>F9BJj|(zQ7q5m21EcZN6|6q=gNw`aB`|+&g|7)o{NfF$dJ7BfFp?Xt0*i4lFJ8w zLJ-kTahxilM)HevsD^0Q6O%uYp;cq+%k{e|z{>c+dGcr`n5w4n6$K;L2glKy;^GFy zIuOChZ9^OHcJ*%BM1|G!J0sXX!oXUQmNtQK9bH{c)gK;A6Kmj3?ZVcS#$Y!m))!?K zera5R>KBcLFze~hoM7u7FoTH+86Drdemm7z@3bXIYWaEcL6a{55M?&-W(aRUF#NSh z5H%&LK^D{BLDRo)hLvQyFZBqdz)xzbBG7+j=NYrUSVL0o;O5z{TW4KohlhRD8H=)h zh7Q*jE2xvArJbpu+rq?fTLMO#YAy$mHkk%7UD{6Bv_ng%6q!Jr+Rx@PDH;jveQ4JI zgPBD8HkhoXAckp)d>3&&PGUq9Y2@E1KX?QzZwj2KAVch{NsWaO7=*D)|gyP@I|dI=29uYW!E_4TbcD2}$gc_@c9MU2&e;ftJ^yQ|`KKk!d@0@?F* zC#($jGDpiFFDvKq8f{d&Mwacr)GXkRkdDr`20+ZD<;_GDj-*HQyO_INajw{ZkxMTK z1{+!B)+9HrF!>+o#C!uWYPjB)FyvUmAEt$IC}sjcJ(5>Qmh|rP+Uf(1A0(Xy+10Zz zF`smPdrt_en@K}AI<4ay$sfjpRhB2Msv%JuQ^+_vh%zJE3P0OCqjuIHoIk$qm zbN4lGGw?~`&?#J;#yKJU>nYV;Otu3>2rHEFS?vft2u>zpCWww54D_RFTtjU zh6XqYzddmsW_+5m#L! z3ix>|T_2(?A1dj^JokjOx5f+Nl9J!tE~AkG-P9$5i1H7&LwGsA>^?`}x^cv=UA!Nv zj$Yxu!Ds~$-5@~DyDCJZCHxPzz<1Uvpw{dwp{<=vsj`jExi?y62M3}_4g{vMI{f9u z5!i&mmgeWkuh#!g<$4>1_`@JL(inez{yVa%KIFj%a%eO&hZ5;8qw-ge7P}JM>dgIS zd;2YPS6Y+dOHlPs6* zsyG@~_;|yp}xGX-jTM@L5^dz_ruHXM}F6BEzp47oz2Es(AH>gaMs z(Hpk=wgsjN%USYkpIJ()_wcBRX^Zz%%X#h$d*rb`J%5|LHHPE-t*<_5`yWsossV>x zrW*@(WMfCZJo^oM4PTYHi?taJ3a)r%=F<3*!p~8 zIT-B}2C$;u<2BH%grh-{$v7Tf@vPU{$PFneidm;``@xdMpEyYs(iqUgnsd1B7)#$u zY~qrSetc@!xtohXFyGn|c;OtxWp6@^sbNB7!jbiZ{QVzQQtH`@ThJdrfI}6umeY!p z&SMUmKJD30sXIN8=Qp@3vG2lk|(+8cFRsm=l<;eQKhnb*-&T>6+ z>#M%LzB%9(ZMoY7Y4evx%gkF34!jE$d)66&t`mIRjEN~6sy_swT!iAh3}VQC{jv))AJbc)c+ch-0*yU- z!~}HjiUUHHd~C#+zA4U$Is^IxCM1A7z||rGmEu=R%zxOsq%?MAeHUyrZw(W6z7x_1 z>8gPcTz8i@a?!FCCWGVBvp;8~sE8!a7f@QQ z9>MnZ1&v0+U_9p@mhPVMoR+o+{HzWYPf|X_o^Yit!7Wg#-FhfrXYD&GU96?hH>jSR zl@Dd|E_3nJ!x-eUAPAA3Wr@njK$g`fN8$gvwjs)%l39AqLi)wsCoA_e*ABsvm-Mqv z@SZn}ZyVjIpFL`iX zfC(Dc++yI1;83mL=@KA+5vv@HrKaOEvi-=HCB-*U@8JLfPn{yzf&Z>xnC2EE8N3yU z(75_*thN0)o$(GcB*`dP0qC~iH|&Rw1NY~jT}?w+i3qAQGBHtn;hRcYRbaazCZ?Z2 z)XJNvjK^l^R9bP&P3U2~+1;R{r&v%JnYvXHM~MQgtqf7igK$Vf2gWW;O(DPYHlPYG zcVOGT>hTb_my);2-dl#}Fyh)LJeVhBp8f&(e_>%s@mgN4fdGt59q1){CERa2jR!!2 zHk{G!7d!wX9t2BCAM;Rtt0fa_Qfd`s3l>;c0yr2O+(XHV$!-kQ4<<*CK03 z47sc~?6K@r11g2w&~-d5i2qsNdFCSH*aP&yNx{gz_4e*@XaDuxVv3c9#-I4C~jO2W!$EU7zr(t$q>S?l`xG54!yj$0iXxjBwGS23w zPzkDWg6>^tD3NY*^HUPu^j-jd!mM}ck(d_hq9WFh=A`WS0r$|8snzDIs*hmTf?B~z z_jX`)1|4uC%+1YxeZ`@PynJhnQ81Y9Crkuxb8?^NaDS~?hqAl4q$EwkO9Q2-_Zyq) zI_WVSTHUKz2;`x$!(5P&UyEGvY_qbq7!!VfD~Bx8kzyDTrhN_Z>nb)X>8`U=ubu!4 zQOjUORaI3(!`jhBmNl~#OJtM>u%zxJ@IhLm&CN?;<}98O_%D1g?hCypB~b&$;jcKv zz~C6W0Qt+r?_@byb_NGAF$Wjgaj)xol=MrWN*~5#<-_4`DOFNjob-PD*TwD8N*f_{ z0>cYzWHf}-Wm1OhVhEkjHS20zw*&lo*o6}rw`2H7*W9iN z$}NKyZmlbH<$;-OP?)*k`W~p>GBx##jCRvZZ4IoGa&ztaelUnLAG)~u;C{s7I$yka z0c4Tr`J|iN1MMP@1Vo?X&q_%XEYEuOdo(8WziMGqq$4wk=S0X!r@zF##K9b@@LCa2 z##@1ryw0e`$uQoEyLm~}8HP-7?)%)`1cx=&dn|GhR->@=vWWdwM{5Du@^QJ@awtXO zZrMFeIYI7Od#tekxki%H6^Jt2x*@1O7Gjo`AYj(O`y!ptOA2D&w23?&Z#zP>8x%$T zzaY)0+d4QxF8x2!;fI8EdwTKeIrsV+3Mj6uJg0r%1)a#Nah*^bE5O5<{x&7W@g>o0 zP4D6($mZ-5bpoV_A+35p3zIc`$fR5+Xv$CIe&$fsP*<-mW-mvEGy%dMHulvtWR;V| zrMn|yAMe!U+UP#F(l2e;zTH$Z2)%E%kWX-wUa8mUe|(>mC!s6MJVng?^YMQyE_bw0 zu8wj?=QrN}{I;~65C$CzBR;4)VK+fY!THH^ZWf^u0#ONxYM4l;Klctd&rTkWD>bTl z6?b9ct?*I-%86USFN`FP(tzUY{VwnUd0@8tu>5Tnw`>TIRf(72z=%eI2CNK5@kp;@ zFbErdaHg=jPlygNLCp$%6jVP8JX_zCJIR`okfWBv8SRh#>H=oTO#1|RRlduW>siu& z9>2vz3E3XNhhZAB7mDS8M^>L!@}!pV!fi;<+3Qs*(J6xI9pqXvc@mO;I$=Xt`1&%S zoSM(E=ITYV&9?lyPVLK?p-XlTB_t`C(z}#>`8(pTf}iK5*bUb}igSuukRF6-u2|dM z72Hh%+5()#GW*HBOAN-$^WM0fDk6NIi?paq!A$*K0j1OJ{e#}bja34AmjPwKUBO%K z`Zkya0miZt=U-vwDzO}06}fhV=rWv6a^co0E@Z<>KProVt8<%>4lSPI0Kg!>J?DByn^MMyp>i5b{w<-xQ72 ztiuaG*zL`RXsO@-bl%X*GDlEb8~EHB#0?i zu`V14wnR1OiP~3Iqb$`IkTtRhYD@mYGh@0uuZmbJ{fwsUL>lTL4p$4Qt_nzmSG)g% zbbs8T+JGzWSID8!S^W!@Wu6hv3;kU#l8=`)r$fz}>7h{q8(spx=^l+({n#GDI^b75 zpDN9_mdCf{lXB4v`08_Cbyc{9$mNGU(tjkuD){H;TO{Et4P12OwMhG~jpTwp{(XB`b@u@JK3Q}n~b?`LUf znCLyTi9bpd7w0BOR*T2Z2#{Q(Vd1 zjN=d`%WG*K0OjP&)k{5j55SBYbU9&aSb2ltRd%M zW@D@2U;n+>BlmuIwrImUCKG{|ppJlo5b~G$>ri?=pK&=$kXpAG+)t{im%sksB`- z(PZ7aU&O2V-Uv_riv0kN5WMX*k_((lIoYdF zq124hCS(RgfTEn63@eOszK)1$r*IM9ZIGUPqo=*H`6lBgH_p6;~{+_-48x#K0=Z{BR zyx-Z-pYBK4CegXsKvZOtL*dMF_+<)e$N8yI)V2bnqB8OqWkAkO(RDgPOI)$lOPGO4 zk8GS%$s8IAW>sd&jr?05SMBq$*As)wJu}?F)xD!X%Qh7gE3%u~f)AXTZE|lF^CQTd zGq}A-LTNp|c5c1TP%=mEDNr293VFD?mhutUW{7GJ4U@5?TzJ-{lz8zL<4Hs<%JtI| zEy6ZVlFMN;+|Rj)jj8Hovasvi>^kxpLIy@gSZ1Q`nNE7?dg*Rm zp8!r4IthHZe2h)3EIS!_Ms#D;9T|MC@Ijx=5d;%L5$ja64 zT>hA5R=ySU9v=2Ncn5^l7vjIq&>9kDs+hSLv=cT!rR*}^CD?1khr~;shs?&*??WSaY5AZGGU?sks(NodbR$NeV2OLUvp$su{e#MZ3)TyiapkNE`acE+kdAHl zvI2h|N4j!FE9M&)^RN7FwZ@^ubgvZAlD)ERP!4Tkh#fhqXqTTJ=YtFC-^IJ=+Fulr z>JltDz%^sMp!oV3Hl}m6o>N0)~g^e38^6_iZ0w=|0UhZ~FDanAf%P(uV zBxm;PD1VAKKGzknm`eCN#YrS9X5g2SmVkGe;H z;(p+>6TFKu$c32>e+bNs*TtCYAx_)(pCBO%>ka#B>UTv~$aF9c%(U@(n!FkY?~TG5 z>{twS0dM{Spz*G8sR$jk?%0GNy&p!G4X`h5>Rl4h7qheD01J|L{5mOte`p9 z4?p_7Q-ngyAgcID%?YN&KF=H~hT;Z>XFo-te{+PFww(Q&LZEMkPf7n6mFE5$|71Y? zhjW#V9L45APK#~73G&;mDC!l@xO%gEG99m*{R7_YhY<4JV}gGO^oj>!AJeKa5Lh@KXy0gt z6nh~AzJ7S#t3b<3U^cLBw`zdl)hl~SFrF>DO=KZ&(|WHrDjqwp1Fe%|IE%14em+Gh zPa8|jrW^%{Up~9Klrdo(5PgPlbZ(CrKH8aSyI{)dJ7mL1jQ*yf$42a#h{$m-9J?;L z5}X2aSH&z!nX8jel{w)4Zeslo*{WL=U_A~&sB!9Q?QGDqiH|U{eEe7-a{YQ3uJ`8d z(kF$FAhyR`ENE2MXl>M7&on4w*>HIwS13oY*EqFUT9KM($=PMhp_O~QWy4L z&nSsV&Qisx8)QH_#A!|UPzcI!f_4hsxX-7yGlq$gL;cFJt(Yoo1L%3-mUavVx(T1$O+c=-ytMz=)|zNer>Uss3MM^H{bLT zPM6f;RA3~*;PesuXc!uP$wnJf*@Jd?5b@G{{$KsLYlDlmVYat`ufS(7 zv`D44oBuvJT*-(ZurjarC~W%ei^#1tDfbt<^lNICux&;m%yt?1D%V;;sU&OtO?yf4 z)EF%5f3YwM^Gb+{E?nCkI47`RA>S^_h$?V=qKqkU+IfZxQ}CaZig{i_zGN8hkELfy zuQ~7+7&tZwrj}Yxziy9vZAWg^7jeHl#_Ge)@De8O@6Sc@J{|h%-SEBHWgLT&;QK1c z`&L$Juae@+gxh-s{{Sy-LqS4flJ*Oah6Cq+T#TPDf2ug*2{+Llepr%_*$;&L?zpv; z?T~R5>Hnu!{9*>lgCCULpMte(o*;7(xT6-0h87bWhNaj|AT_>unEwSMybm)VLYm6w z9_x$nh3GtYJdI>wDVmjXr{~FZf`l%|FT#5;lXncs%#_Owf5Z(-X6A=3lWQqy50z(9 z526L2o!57J_fr;eIEtL@%&8(Zp)Eq&mGz{?P9K{2q0mLW=!18{3E(zE@PWGMSq& z)8+TpbiATk9flz@+z}J%2nOikUS#8M+FLBI%(R7_%1V^t-0*85r4}5kKN<1J+<9TM z>bl+XSG9QF@5xQK@MgR@4D;vVSGVTzc7$fzk6eieD%|%kj(rzpd5p7Cp7eowT#hY4 z+vH?2F7G=lX{jfBbbq|=0e6Aa>t*(H0t0Jmm9}0x(^LTTcpl}oTqV0{?8%4py=1et z)nZH_wapU$)U(iS%KvIxOX*#4eeq31816)@vxkQVtW#^>;2qfURO-xK$6F^yOojaS_(HzgO87f~zTYonOV3FF3p#FMS#;dV<9 zU%U{7s0}2$ZPa@633@$a^VY&z2RubfW-~uo2I!?C>_|HKR~Fi#(j?ZGJY$x zm645YyLZcXUi{3BkkrCF+AbF%NVp@HM^K$wpgui^j?awUuORvK=Ozfa8*W>$J*8$X3)`ybNW?I;d) zSHvDAfpM}8Jp@zVJEqP3eCfo_n%u%AxQVgvxJg@s*E|*T&G2p9eb3|jvK;jI;uCIR z*trC=d{E(eJDZ?9ppSiZ*pi&gAnqH-1w->!YMnIB$BZQ2X7ykrk^+!?@RN z1P%t{u!FzY2&x?q3-78wE3c6YzRiXldp9OS?tTTa(pvWn?S2q4$a>P&`R$`!FA*T`>Xqkb+Eg})JStRIg~=QXN!TW89%k|P)HX5mlRT0S z)d`BwzbMFo?Pv&DfS4tYEs`6SE{|5f=Y=RhnSLvHZEoU6RX<<<@#8j$FH<*#+o8C>~SY;Icrt#d~2QfGr;%8>^;zqMnvR& zc2H=fP|HbW|Gi^NR5arPhR`GYr;8bRz`TWa`%9fe@7cLIH*Nx1%5~g?qKhk`j>@5> zS%j?LvCZZMn2Whzth{c3; z&2boDt5f4kOarCkHBH23d=05;9_^V#r^$OG5O*`VURGZLXOpP5T>|gVj5brZf$$kv z`N5$0dz`?CbJde4#19K`Rum6w1X(CB{-e)pHpYS%r1{&&IawRyJp-@w^(kBF6@gBhA?3hd zr#1EoWIfFIJWZez(Jyzj+HxYK`|t%L9scXwcLmf4e_W=&{~jy?{0mfT# z@~jO|?hEe0ZX!$8zuN45(5jY|B4QGr5!rFw4&j&D&I$*^&SGD@ocIs`mVzd#G^2Wm9kMR;_QmM)eGU>OZy0kjntcmU|GZ8ndDz9_| zOQd~07RaV(V=Ck&dGJ-VH~0^c)mh8=hJ5cgok!+mL==J1NXd-vwaV=EQqvvz1W9i* zgT0u{u_K8$R?@n{&h+2pMIfxc_e9Ud5%rUIY-60u&*z_T~5 zmqA1#G0qXKx_3i|3y}*5IStO=UfsD)X4EQ&aV8X{`&uyZ+;FByQc>1tW?|V+FO6bT z=*SB-7_?kmo=5DL#XByLdN$u0f>z2T_0#6**(qD#V*lCv`SlNvl5=@wp8Zmzkea+? zsqzc9z)%6~KLPJ)MKO8$OVL!Fx1clueHilORxU3j=4m8@4}$RZ^5?ED;5K=rUfAwk zSm5%=41YtP>Y$q?M}C<>R9lHu)2*sbky~!*n?pcY?c`b{6FBr%y%QsTTp^$+KyDa} zKmq{n*M2A4*=gV$`kLr-N_ml`k8+7-n^=qg@` zt5eg(mLKfQ!&VurZ;sm^{!To;c>CPLp9i>F;4}hh|IlC%K$d5H=i|h|J+iy|=-vxu zB)&d%=pWQtr2hw4t;onUKAe`@zLCHtPqD2Q&%lUwUv3S89C~%D2hS$A3=>z@9@w1t zu$VgmY>KC$4Fg+i&!|#%{Ql_O^k;+b&W3&KU?t@08Sq1CmlV?*xMcmNn>b)^!_R2a$bWe`+J%gj@1w2sVNm*bSv9u6z$L700Igvlr zqcFuTG%1aX%}`xW2RT|M*yU!)J0Nm7)xpu>38Q!Zqs_MvXA$K@CPCnJv8d>vm1o+Y zgRDKckJ0yheE~F+o>`FAb4do&u5%JPLq8OAcoC0^SlzCeT99}w#{cUiFipj}@~meU z)2y}{Ga;?x~t;NirGK@RLR{QE=7<|bFoGpVD;Sad}Wl~*jT933j&p~1whfNW$9 zYgE7b{7T1wPwcxi(shIE;tD+1DOZ> zC9^mNg$jqbwY&tmHCwr|prZA253iK9hw)Fux0eog=YhJXF_xYn*i+f^35VF#^e2P? z_n@0wCOUbnm->> zwFrH;>O+F7#WrujGFC=Hj)Upo0*%lWisj)?FFBZxgeG?;2{+VGCWDi{>-&G zTRF!%CwjuY9@ZUP>6dRqDja%fc_=U8S2Q*@+H?>aQ?SwoBl|*WIzp_hC$km4Vb+hQ z9;}h>(_s>+hpse3z9mQa77&*&1|LF5pOOx3XTl?yK~m=TCF@Vv5ggcyAsXh^V^@`n zy-yBzVFwMdQ_kNu&H6dQMYVet-LXbAY7(FTKt9wlqu7ToIi+l=Z@xR(`jtLue%=vJ zfypg#-UO4a%Fzw*IW;_*SM?#xD8x)g`Iyw1%fT_M^8KVZz6S!$*B`ADWA)4#zu!i9 z)gn8|{-yT^EJ4T*Y|m7}Yi%;=&i0S7y^eAtYz}hz86!Nt`pBGp$U2Q(J~H9=f))wZ zRDx+~sj1nNWN0KLFa)d>zAeBmT;A^9gOW;6sJO?gFE}%K0;iAmVT`d=;6C&k<*ySR z82B0c|LoX#T0j<==Vv<$X+3HOP_)Y?St*coGKicRpFI_7rvJJXcsxEGNrV;1fXPFj zj`F7i`kWm4AsdD7O&L)a16Py|MLK{7cq6m-Uoieiw^N4EmFH`f5Y%Gzlp1k3To&XB zmd(XKzGx)1N)E3kwTi`vioOT&XLQ$mg8bGz*$Td}RS^8KNLr724*!jV&maF4f_CKR zoreS*%G+$tEcR*t$$`Ns4&r~m-A{B_r^1EN%go!L@&xAY2WzJ0Ua?sUm+)n%F~GO~ z&ZxNBr3J{c_jh>?zX_}+D*m)}zL}RhIAew!`SkCPw(;Z0EZ-#tA2(1E$boTAPmc0L z^(h=w98fdba(cqPo`xjiCil>^MY{uL)niN)a%c748;wtmfo3zU#I1iup2utFd%jK^9c$pnz#9)cWza(6!~P?U6!@w!x%EHxB~YT;Fyu(Q z4=RYUJO%9`>!!w^e8QNEq;o+p*27LRG?iXbgB!_&4xDD9GKYWU&mV^;=JNh}jHj7o zL(ls$x@d`$4F6g>Xj2FN_AK7=gF6d;ap3j-OcQxi;N;FM4tku&%Q~j1yQtO>kcEBL z;nG+$khs#0Dt}s3)X~uqwn6FH-7vGTJWu*5?QiS9qw|%7GX(3&C#w^JzUj6&RU7wv z*hQ3X=&ld5*vg$2U;&2@=mz~xx#L=w|947Xdk8`tUct(FNVItP%FXbHB=nz%0!4u$ z4|7*-@4`om*$kr>F2>42`l5w14P@rc+(!osKIn{()8sV!s<0}3HraCQ?U{AnN+gGb z;vvY^!$UNBvv#X~eQA05dOBoKkm=ohSl-z95)1BlR=N&IxZwBz5P1z(IAv7wf8g(_ zglCRmTFax9=)$Bl<=IGANMcsau+mU)=G{(FfkgTfOTX{E3_0KexJLUlv__aU;rLkQ zZ9xH25vbNc$T037JAh?4l)&#|{+RBRmX>A|fSs31!O*ZUh*MFB0r?azY>n|^YGAXCl5(tdV&Y|i)1;$M6Za+MQ|a@g zmG*l6UycD^GpI1|2(4Tvka`}Tg4-mbI8g<$Ss4XZUi!AR&|x1$%eiQ#A80MYp~uR zm7$N%pVJ_IU$`oL{InjtuRlk_>pWGBEjX?i8WN3it|WM!oVT7K*y!}OC50|zf~~@``98}W8>loylF7K$y`sk zx*_|;jxD=#N>tlTW^Pr>d(tz_!L;S5_aV0WO?9|)&C7>*1~jLTDgh5NM-=4$uGhDm zSGRPyA5*-r@)Nihh52K>L#4d_wM|vXHv;u~kj@;zD(mO*{w4a7O0=*wHKK;$1x-w@ zW)P;wtr_Kjg=~n?Lf31Kp1C&~xdD$LBv@)X(+rmyJ-?y9Qvf8Y}O|+meUn9A69qZAa%Aq_WuxZod_pG__incyU2W(KhF&LDGr`W(jR}+a)G_%Kk-+{Z$ zEn4%dz9PmqZ+Y;jtz6*d;Rzc@p>C+?gOl^?*BJgD5CmN`&sKPmM9Pl{aoVO<*_Fvs z=fXysXe2}8035Pfpm3B?hH7_L2EaKqd*Ag(rl%6?{JN3(KC^`rtRBhwOZO71da`W* zVw<(+>XElOh5EKts1Yo=;+|6SkewbzC{3*((NwucXxOFsz9~NI!Jj$ zOBrKlXs!l2c~50jthcjL_-H;G>0nT%nA4T~l$Am{l7ftd=T-;@pEjTlt_SGrAezyr zIFVp}m|@`4w8n$DfalPd{yjMXNXIgJlJ1D7iay9KONziq&;+O=@cMCSzzZ|Z>zkA}(#m}9MtkCyO1 z;P3lbx*mn^<>3U4)!M(s|Mm0`+ULDFX2?(LY0(0e2squrDGj##3?2UvP4x0zX!Xh; z&3CPQyOI1mEUZ6p>oI?im&V(%YK!7wcv}$NPKsN2Yl@z zG)YXwg<;XnVPh+JLS6oy9Ub|aFW{IBoYOYV$4k>k`a+GPq7OlBHE=KR@bG{RY$)(L zNUeR-@}DZ(SgZ;=`bYeUY4{kZkxcl(aw>fsW40S1l~u+>y>~tZRlPJ+bd*z3Az_Ij zc9ua$izAl)0yD=TJq7_VF+YK$bF@aXjL*&ttoC{;+dellMYdIL!0Z8rU9}*4+#Iiw zuG;|t)OXXaDDUh8qUBE>PM_^fSxvkhZ-uV(KlNnES+7zHfDPe*N=a!c;VL7E4}4vB zs_OWAf*{Zf2?lo^-;Y7|mYSSaYh1G29-vg?Eo%|_;kK@$=Y zvkj^ev-hglw=5KMC*b5CTq`K88uK;h6ggaLG?X;)c2h>O*Dn3Gg`#M{Q*=}6=M93n zhO~m!?cnoc?`RWU<##xEJ z{KVF5rbrQKX-I-NZjk985tz?DuN1xAW7kVfDPw)|0b>?U11ITYi- z*T;TTL+}!KB;bBGY~j^N76O|g*!9AElq+cWDq^9)02|R8-lEHR;@7W#C@&YDS%81Q z`ehsozyPyTJhUp>2uE-0de+fR%xv&;n(mt-up6@T z?>7jfo*-*?268}Ubh;vRel$9AekV`-7Ij2bc1U{D|eKQtn8*iKi#DeKW}_L*EjGXD6N@!36#k(`k+y5^mT5VOQ(-(M+x zn`2O|V29I#WQB6Cqah$kiK_pVG*67}d!vqnc!yDdG6U8F_G`1Vv+(-{$elv~c^Ol( zLvIAl)#qO~Ha4v5l>%QsTqCWTN(fPO6QETGX`u}&QJpKBQK*xUnM(t91+}3q5E?VT z`YYAp^iS3)o<lpFlw1r zGhL}Jb}zqd<)}s3R%L>s#BO@#kCwAOOEOmSN8L9n{}JYbDFmcv0PB{swe)!P!-mZp zybSo`EL(#?ckeDiY8+bglD|-=gK0JYeH$APSn|f$LPYO?NUApXT;c7QW!tNDxw-Em zY=Kq_4IlU|yX`>}LmzoTke`|a%ll9g9nE1r0qG_%C^bW?U7%)z@A86oRPN7Pb{l?< zSMb4;lyO5Ddts4aKHDh-ciM_L#{A4@wCCY7O9hvlQwAIip0j#E(~pn_``3he>2V#C zYvcE2^5>c=hQq+=h9o=s-X!E40K4e#2JlPER-s2cEruL3lBmU!ARWoM;A_$}a zu9MOoGC1ftNh3rSMAovGFNFTf#(N{%$!th8eqLE0%$b(3NI1`IIm$HR8>lRC)e<#P z=$zk0!y+hyaY!9Oxccf8qE?`XV#hF{*WSHM0 z!CE*SjCZuOv?kAJUoU?cElB`v!+V25ob%AL`8pGh*LUqIp6u=lWZr|V7g?AMeVmOW z$SgB*3h6;FFYpr*R;)<_0Jp{)p(Clledd{O?yE#JTwx-L8e>l&6E1$w}Mr*xCKqdvspwyBx~TVM$~ z1M6Z=ZZj`lx{Sxq(2b99D#6{Z>AQ$BVjuUtZP8qqr03Xd*wsnY1wO1M1GkYId z=XUpQB$ju>>bK%;P`G!!UuJ$XRC=q{4%`y_J?)9=9%9D^uBaZ&h2aBm;g0<|g+iQY^zo1=U(7YKJOjS# zWSKpSscC7&!w_6x9;;0PVp^C5bj4j)QYyTfI>fCp$SpuRs&43DVVstlMx&BU^Ge~1 z@J)BDX;iJJUw%U*&^IKgmD|j_OjZxqFq0Zvq;9zTY zzgJwhN@$AhT0x{Gry_P@7KV6P>|{#o(`|Mozm@74YLe2zr7(8;1n}A)#y{yMbA1;j z?+8Ulw*RRx*?{{H=6mq>owVjLVKPJgn?v6gjjnT|)pF(kUkfm5P-uST`R}7f_zw6} zqd!v-+t}ON&v&f?Z;z8{^h@=F%IQWw5?BXdz=tKu{3@!Q~kJj-gY0k<(lrcmv|ROw9hq!V>9$?+NE&_~g7 z*Yufsy8qfOS~%`eUAQE(IZGG*nG(@iraDycub+4Y$WKGl`|i0+JZkiOX5evi*0S%5 zUeds^))l2W;BEmDWKE!E;!^1>UM2zV`e1u|_2#%njGX_$1HrWG+9^4!^mKM6H>8sx z;=1R@0wG4>21hB`t;;U<} zDk>TjC66<^^rZ7G7&1*${}W+1Ru2r(*f#Af310rP;9i%Wp6kCM_!8{7%-+G+glk`s;}N7WK(oUe;{sM_5L=tlqLb@OjlM1>eJe*H zs^j>_tBEtqU4}%{(u{S!wHRM9E@xJ#jzfg=ha)SKRFL6;i|a__MQ!~;dxKP($pJC# zJEo>>pCVv@mwuD*-*?!J$^b}=+_v~C`;h4UstgCya!62yI-^4vR9Co7^`&9X;qhyU z519YGQPS%M+jxnaXU(c`tFP>mNj?%O`H89MRxN1AJhsYvuleW!S z4>|4`L_S62Fp!rJqkfqRVmmk!m2m4V8Wkv8(}9Y@$e59p6?I8ry!yHlUj;W)0kJ5b zMyi8ZOm0lB#P#b^q5?JK|#2z;l6{06oP9Cf54QquCY-@UVi%Z^d&)@o=QQh=5K#w zcpm`l=_I{?_zKlZA`0<(8T~YB_%-=Ykt#D6@HXPTW@KzMHOq^(R$#An<;5QOz-Fhg zmUJ$S2fyhfADIH`_f?yGZIraAiMG6J1~=>AkCk4ZRI1KW>(4V$EwGaWI^D_%J&YO> zVq=BL9>XuHna>fE>oT71p*Y;@tPP;y(N^0pA4@X$i!+Q0V@T8Jc)C7|Kb#VkyOG*Y z^SBOE6h;-L!^j3tDl>3;Zr_?X04{~Ft~m*#zU%rwHg_(BIYNLa^%V0XJdR~Ewb z>nN_;W!iJZLc1oR!~a+Q(!+h;7?L?(Jjs;_Gm|cAUN2kQIUp&5R5Y2(G*u-T__Go2 zG^5d6#4!pt6mKYgA-tg+>BLxBvRcmtrtluB`T1^p&1Bd{A#mX=<3sDKk(iNlr&_~W zkV}~0lJT3lVK#{E?Wi=5Wnp3&CcmE5&mOKI59?GOA+ApiWBI)mW}Yt&ButAu?s|- z@gW-v{@2b^hSW4R7ash~6#hq)%pdYGgzRM`&Yi1=B=m{n)MOoSU3mSB!bRf}Zr4UG zqQbB2{wcg&)Ukp(F&MA@yFGnA@wlH7VOs5C8m$n`g@~zdH!<#H2`%Rh&ku9Ts4snL zOHLVX^TKDE7xPBrYq&ZON2xLqnq}-p;`Hz#N^yrU+$8cfJsTJVsPBtJ8hApS-JMiJ z7*$l60doRt8-G~>l@`4>Of6t@w|({@-I^C%zcmH{jU?lf&mnGW`%t z5n~J5PB9siwWF8H9m3cTEhZ-R;+1W{VObJ$NhYbE0O^18rg(%!f33m7I90@ob6`#n zEspriU${<)(ijs5vU=|_jb<1rY3q=b+%J;)jyY`GKIz*=g}*+Yy+|3VjzK-SI(LzJ zhUR>U=0mUwe3)xwL&7)BNzq6yG^(fC!9sN<<}vk>k!kgl2g*1C9BwZ!87s4+2dJN` zBQQuXU8R51t23wh#Ha$1x68kMJA?sI1e*ii80gc%xG2*PCg+IxS}-b>ljis2&1@d$ z6+zul%MfQnpUf3{6Y>nGRWb@pQx_$mc)5IuxU9dG*c4>*Fp`1VFbuW_(F-TjMro9I zKM|$Z|HD{qO}*5)9_K*26iIQNu9e+syt?OTHCN#s1)1w71m>Mq=qsOV6&r5p5G0M| z!O<&FQqalmCEC#UpYQP$#32>ABqYkts*spxdiIv8v?y5ZORk+yeIZ|PwK?HRaPp+-%Dx$w#{RRU>c2^o1lG!ZxdfX-e*%@h_y zv)I=c40&O!1H0hgM<-xtV{}NAYG6BLBVs;zEXk^@-}#xy>^TsK(8$QhNRXp)U7G)E zXN9q$uQPWA$9}7!wU)bZG_vvMf3LC0_V#v6@dvt|$_T{^x|1JV{9*78Y^BYGJ1;rY z>yy~nXelvBfKfs7)ge+}sBnqms{bZc#hjxp8>_ zQmkx6UOz32lok8Xt!0hHM$q6X4<$-GK07&rL69DWPu}ozm+?ye4v$4_84Jy$NeViY zToFR-pzRqQ9c?Rns__0zm&a?ydO>Wb#Tv@heftVbT|fuXb{n1lc-)9^?s2K~L{$|^ zLqs;@v2|fjXB2B#XxPqx65Fun>8GsoA0HDbrmd~`FrTd`7OBFWlDN7nOKccZouu#y zY&bI&qM@j;CD7ap#i#XbgKa}+(U|7shK_!yx0y74p4pDkY{e(z*4L{RctI& z8*iliH>5}?jw^th*o)9>g)+j3|JUeBeruoFue`gXR2ORW(r_#EgJ8(>5I~7;!&iR% zp8h+Vz#H?somBqs`+qezhbmo?Irt#bsY6s?$Oh>Wu8u}x`-9}W^z5u`OuYpLeAw!m z(taFYqn#ndC$Dt1_{dhvE@gKMn;1M&Y60~=$>PKjlD&_p%lw5p7hDuDl+So6h7Pq9 z+#9rXvs$U*!^d_p78e$(91a=2%&of}_cc3MW_o^R@g~D1#T+_5^GoZtcdjy9A!6IU z-v8(C^tp|Sz9nqTy}LV}pU^R}>sD<7FtN8jMtQ!pv*4Z-6&Z~2*$ zxE!Dt9W_2m2(j{Wdc^QA(!25xc0!!X3ssZqUkN2cL;4QaYNT3nYv=a4Y;)>o{FN>YnKVCb4Q9((5}iO9R4q^t%2Q@(-Z?$*geOaCVBZY~vA z+#sVlG+;W4;X<@mzkf_en`-3V*WPP1BHH&yeP|DS<_&LjP&bR$r9@wekm2>k^Y=hA znAP7?Nk(4HuT1sUr?~$=}EO;+^G9=wYZq+Ucuju zDKSwQP-UoGqOH_@Ep<@^@rvFZcW@Hti;9XI zOric`KHM=de)Z8s=uv=yo%uf@O>(T;iw#Fxe62{qL+sug*?HXjm7Pa)#%=Q;0`7Z% zb2*8fhxCS-DNLD)JpB~Gl=GLA)IJlYDygQF zx{b$EbA!K^v_FiTpKrc8eoQqo`Z?jv@S3YubwyhmuFY7%!>POK<4#WW**9^C?F(x5 zTGrT}Ek)J5QXIRP`SDx$o10y&hmj5s7}kSK-+N87HSZ2}NH!EVb^kh)kr!4>HS$mD zyVugcAAEk#k*p?bBv;%pr+N9rJyw~(!tHEvHydRcSe2eNM$D|oDX5?w%4WQ^WNToBm{;34kgRG=!;0OVR=jIn zdD@UoFu5c;T79}%GYx^t@-n!zxvy4_w_6^X{>(bx?tg=qZ*133ai>psygFU-nwpXA z`KPCTdHOG}*hSi|9OIEiSVuk4@wkGc;vt7eIxmISF`M}Cy)YyEMlVa(I~9L0wm%qm ztr>5!ESq!%V?6}vT8tzxA<`EpJhftSw%=&RP#RyV61#)4N^dn)4%C`eLoa&J%6*^M7S6QAO8KEgn9N~g~(O+jN5wJO|g?+)Jl z(Fb6_Ke8vpW$$){Nz5{$U$32d+LwlhjpfHc7}GB;?i3$s^MWs&1f<8Oa!cl3Qn0^X z<((QM?ncXhNNMDwH}kzpa^=>N^F=l}?7jiT%}ciQSUHQSIGo$LLPAC9LFf0&pQ7*< zUA}%GOnOQ=LiUe0>U{Q<^sZ~l$26uIw*!!{$zKmz(nsNQIP!R~(`O3Y{L4{a3YdXe zw`eZLcdE{%+Vw~}IvPkWF9#tKNdz)1z>X3V?>?|A5E?!Mx$3%b9>3aFyVo_=*OTPn zg|Mq}dQ{*W>G}yJq~v{@n;)V&49s)O{@jPi^9GHFRg1ryIL(*q$vP~4PXS$ko5Loc zo}If($$z_$Oi+zGMFX@^HhO1Qs&$W_bgb_C&2Ur*XZGrhz1{rD(j}rD8C-!R-7Afk zkZm5=ZbUhI8hP2L_Ah721!`VN6#2RRx6G=A)vOA+6MebDL;GovzYg`@m+9FfwLdFj z+L?N6>BbIrZLL}s zZ;NO4o}Qk9{&Uldog?Aw>M}dkq~;P0E)Bu+CPuBdL-g50+kaigRWlsTJMOZaGbs*7 zG>g;mu26;Azu0usRJ^9A-@e*D+art5EJTi5TJ18j?#ZZYBA8Za=f6^RK>{w@d-v|a zL>Qo?;p#VnXZ4iPF2Arf%)tTj+Djxt=}<9U%OH0AsV+Ni`tRr~{+GWxd3$>Y1vMKL z_xAQ)PJv_qrQ51KM~wPLY!fVM?C%ok=A(vw zJrSY@l|R=#{g|66^nQTu~f$kwIfB_Mc?#J*Mdp+f_C9>5mV5?En5BCjZ7s^^$P&F_~vZ}Knyd$Kl~ zE{j$F!Wwjr28Acb)8}Wf#Bv`jtF(baQ6~5-ATW@HRD|~noto{80PX{*)FT9yyTk1m z5eCKTLNH1FoL_&Kea%7W&op_!5XfAV1>$jaoB#Alcg(uM?*wmTVS0d+v{~yF-o81s zkRvB~Sl85Z&zYB9xbz@22J`hVQny zMi4EA>&y=Zf4f|$S*l7y&sV!Uw7LiL)WN|^Z)V%tF04`H=?!!Yug)HM1aw`*?T}P~ zpOTOCA(qp7BXiTsAK2+olLmvN3b%>nsXl*v!LNrw4nHL2mZ+4E<})%g{dZnw;2sLz;q8@LVj0?j4irVebN5VaoM#jdo3Zz8@zDHlwmN2e7U2H8# zQV*rDcMjAS?McH*5dPqXnuI zP0mY#Ap3{)IE+e*sgl5)f3PzE$Ee9N`0S*P>CePYNC}6pnXJx(mT5H>8-7Hi-~O7i zme$d6Ig-_Xyn2@8fDgav>Y}WHlCFy?*~71#MH{vBesMuNZ9pP|-Ofcc9e(13>#uHo z?(=plm#f!I54s0$;pFUi?I?`wJ6FXp*76!Mrx?jB<&Z~pz)!B47kc-qhhsaQ+_bWr z8i2hPp54915M-J_8oCYNDP_QgM7WT#@4oIIt5SE%2L{6+S6$af#JE9Vi4 zC9x1Ei>?I@rLP1bAJgTz@B$OUIH~%#%uU%he@9Dcc0j_80#&RBprDuUqwZv z%Z;`VUJc~BV$n&>q0WG) zmV$_26~S$Km@uK^R5N~gT6HBl+peN<$MmhUeRytmC6yE4o^fZWtV%jDkAt z2XsPnyKYCj=PA(a6AO`-qphx*by31xc2N6$t#^S*-sqpjyIGh-=%r&)yy z6N=8jH==~0*|Ae(psKk!=q7T7oYe*q8H_YbjbNt%)88`+CAKvd%G=F)a0~NvRaaMQ z=3g@Ef*vx*^`_;sS$=XJqPzwr*#*>NKIEQt4w;Jf(17p|mse0kxHyZk3iyM_GY zH5^RGG*qdqjArq>frecq5~KRS;JdrJD@hFpdDuDU3-D~{BiqV0KA!tL?E55WQ6FBh zu@j$b|Jpr}fSJS$xe=>XqNbLe7&X@=e{JFGSB*Qab|R{I`ZeQ=#eNHqbY>NJZzYcP z@y%-+YVt~NRl@?iu`xEnJt!y$l*T~yoadP&i&847*1LqRVe|V|Msz+wLd0T-l71k2 zMaVe-W0j+qofErrku!JoMRY$=>WZ*$@GIf2S7)RmQ6qWLR9Vqof-t&~&Ewmr{FlI1 zs6L0yFH;@u*;V|VF}OT;a?3oC@Q)OsX#!cOYits6?>UW{rilLMap(|HZ6XXwYaHP{ zlwasHt+>ST5B!?2=oDvJeYmSIG_zm7lGyQQ_BO-AlNIorI4&t~FV&4)MTt|76>nXQ z;w!6uf}}(vGnZ0}Q`awR;Tg(kciC*MDO8GTjlY)QY|CM`Wf>r0K`>AC0jqJn%$$4! z%by0Gci6eXn?x3hw~G~@Yi}|%8kc6=i>(^pR1nM7M@o1ZB{AVXQd2=JG!)HU&^GK$ z0ZPPipe4t=F6$$;w!;R7?Q2KDC-ZD)2^OA1MgSpIdzfxdBKnSqBK=23y2;Ayk2$u)#5wZZ_m*Yr^%Ue$&y6ibBNCP@vg7rh zJz4p6f!T0&(lyPCn6D(?pu2kWKTSz$@7*HiE4F%pxkSKfH+;|fz=v1<858fByzq)A;5A$dsR}b3u>jU>epIqDq#0 z=Xr}adRwn8nAeAnC#DrS&lpa{%b7URxyPl6$I_#s_$HFXEb%<$ztb~<3^$p2zZ&P_VgkxNfz(< zRpfVlim!c^tWoN5M*xk5&IA63fQTlht#nI7S(Kgph{R8sVbyx_*M{Sw`w=#h=``ke zPj|4C1fe{Up@xzQ>$%`G)v+qi7m57xwtVmgp4gJ?s{-D48_F2Hp^u6=A*Lk}inE`A zK@6ZLFec<`hPr*Jks$VHx%p!@(}qNnkJXS$ao(=&f<*N>;=SL;WI`#D&omSBR!dVP zQFUbG+$8Oy+6`{Aih^nWzZZyGLtgQyM32BRc}Tpu91R+{KVfVqD6cc$35_qBX6lJK zWwaLDiYR#-eYY$4xhVaVex2OOt$jbMprd><5p1;?8L96TY4fZH^!91$T(D7^cyOb5 zc+FAt-^Pc2pa{)AzBOBE7w#_3Z+TMBeJ|!3AM=+t=6dI&vWn9Z?910*)>8iTXsh8? z&au})Y%2JAsxO{*+yCwrpWg70f~C%l@=@OvA$+Idlt3d){4-gn*@F}-@<5E+13c>2n9R(~ zPqvg?N@GtyM_)V=esm>U|I43r&HT0ff51V50}d!=I1IZP;HsOl*tmu0nqtwG$j9t0 zmqm;dd)!QH!|M>1IT?On9!aq+oZa-?5M{TK5PXtnb{YFEp8Z_kKluAs$g&ZdyQBc0 z|10UrKJWMI^?W}MVAp0}-O|(~iMrrq-iL76@n_d4 z>&LD$;418{+W!4(A+S@solQ;r5nuxedFInNw|af19IF$lDS4f(_as_R9ynT$=y^7{ zw|w&Q@GuuF06q7cp&{J|gA>EWG5+8C%-RY?*TWLU*icnSx0L2-^Qd`?_3$9bHJn>D zClIVxu1?=pUx$mWp5tAg8gH~C+(@~5@F){J!9WKH{(!e0so%poQcpK9g+_*t4+P(8 zg2p&a$~eKq?t@w7y@)sz-q%`E83T6nHR_d%^mFL~uwhXGIDL;UsG42f0-w;(&;WRb z{KfOuF#9sQ*7Mg4F>{TKvPjh=)8M+b!FZcroN(^X846{MC#|=}(1KK`VmU!H?t(+MAoHls{2d29#7f zlpi9g@0#mDP&<(DB@5)9U2Zgls?*G^t#Nq^R%7<%W7fSl>dMalGb#ZMfZ%)MK$DK| z1+8j~Vwjh%8@PWOH*OoqXZmzS1B+?kv#gc}NGwS&%zwTf?EJym1jIT3c3?$*B$oT; z(<7k*;~p!?c`1r8L%1LzyNwzS!`N7cP97BU;>(_A_sA9ki{!(M7iSY*sybT^R??0X z8D`Y6*C<;tK1`~$HenzsUQtT<3MpG5#UME;GC!f+SR`NG;+i_icMX`aO%Y4nkMo9| zCkQa9nH0Sp7#R5Z^U8^0phTA~G`F+fD)be=Ap538kdH-)^rN(J+Q7ZXxbHQ(4_n_U zsgF5iSSgEu2(PQ_fuceWz_~JL@E(TZAY^O9OUtL?&)HjC^8>#%>9H zyWzJU0+43OgtDRX&7v>Sw5+5=S>T%qhIbi7rWSv21NBUdjBqG1hV2C-G=kOxxWMyJ z(!{a-T8N@j1PZh)@kSBGW$zp6PoTFTN!`km!)WU;POgcH)-(fPBm=@Ddc>C@2A-PI z%Xpt%CqpN{pQi1%cC(h2meRIKKeP$t(sF53`~1f&goDY2mmKanYHLPm@Jn`-rn0g< zMuZ0<+*9(;XsEA8qtWw^vx49NSL7=#|6OX;Jn8PY&cUPfY_};Z%9!xx!@#Asa_d&VDWV~o#sCZ9F$!8VQvX@~~Sd%LK z{<$$7>s}`)g>At!K4qj%@+P-^;Q}As!(4m&NJp2d#mMth;Ne0b5Fo)q>#uOg)ZX6S z;$nzbP#B3GQjE$_J^tQKp5i|eoeJM1@tbxZ!uvkAH06`gpo3dC4QFzE#;<|d98giP zIel~B9QIOF)SZi^w(`%VzRb|Iv@r8`q_BqltFpIe=fq4-nSU1E51=ag*6}o43+jhg zf=lcZn^|8#`X5lX>TD|&50xkDgaRgWczE6^mhw2_xI3|3-$S%?#?0?P zVor}JO+E4zL`GC4egTgh6w+*(^RWd|+&y8Z2 z;UdVrkHyr|I%fJHPx5DsCDP3?8m^C8r{OQi$`u#k>q=@Axq?~cAKFR3)y|RhmE|X zg&3g!0#ao@8J-SKKb2~1H$e0XQ${sHEob8p0aaRv%x`;cXeVARa40inQ92%fK<4wsT4Ju9;LY^^ElGA_Gq^ubX=0ZQCXfa$_y{0S5?9 zxW2!g=aMURkqiF%KPFrj;$bR~+5ma|Tcs%3D31jiL=MF;Umpe=9mGLHkuNSCz)m$JHaXtbdoHj$;MBQmI!qSWX406c|xLyR)IoRWkKe5i34p_0~$F zIdPrH@yp8PB(Pl*Wb(#%sci1Sp`pieY$h$Bge9lx=_ryKmpdigxwGqNeGLc8RTv0; z(`mj36denO4{$ZMjK)XsBf@K6?*tGizi?I%%JILbZ(ps^3B=R=NyA_961C>kV+`Tv zAzwfa0mo-0t-Nbd7cMxHgjM0vSkD#GFJCGU5-Z!;0^H~BZh<|RI9Tp`+^lUsm;Ofr zRdMi8^lYqLA~E@F%co!CPd(Y@0lXEkLqJG;6sa^eF(KhPqkgv6^WB!{lg-8gx~jF% zBkZC!vQNDaxmBYNAF>R~24B#fNL>*mqcx7_SNP!>gLA9d2`YrI=}hj}$Cp}=WN|T( zU%LD`C-rq>q5?%Qo4{uLM*QDn`i_jk+IG+dO~_5Vl@HfA;vzb~0;~p5XMt$9<4GzF zzgm;O(O13kY+5)|1ixWZYF%lHV4&{>LXtj?PM*UvKo710(ZJeVqARrK@@xFySMAF_ zu9w$uYiF0^3#1*K+zAIBbo8w7CQfB|FiAUOZ?f6qk8I3=LEjc;SVw)_@G0047wJ*p z0Sq*3qocsX7`SMH3eU6x+!_(_LyQ}ASwBRb!>r6{?RPs{O(Z13;lwekc}X!yZbrGe zQY~RJ8;F0b5Y9gGE9pq$i?9w`yU$kTIY9-JxTv9cN5^|1=*}$}s$%DhP7B7ftMp&v zzy1DT)7X4g(hcQWrP&3-;yyOlgT@0Ko1bX_+p?lroaR55e|dDtvN9q<9GT1ditm_a z>%H9)7i3eAk~0eTPzp0waS#D$-SR!>kUQ*0YASsoISp_XK6|b*Tie=x*ro(Z>tc`t zA{o;NaY+Q(ub-@Q`0;83ujgzm1ioZfjL<&;ih@Ri{r1BLw{gMV&R10T0s0#m@dkk> z(6Vn2z}t>WNLtUuoA}<6xtdB?KUoYi&0j2|Y=%6dY9k38jOB%3LOq*#=m^}m8TUU= zHzQ+xx1_HQ`g&y>G`F>}0`=4~YnWfmN*6&#R5kb-uv%*VrBsjTz`p@o`=l2j{klgx zr3Zuc=)!ZJ(zBfDi#ny<>XL*CjQDURL~fzT4p>7$eDOoV0?E##aadiDyiNa(eHs;n zRrEcbN>*YcxF9rwR59FqH+_6lJVlzC<(q^Q@$&XwTB85~Wrv?^-cI*w(h1Vv5B{8+ zxh-4db6cU@_ER*NI0VjzT)zB9I|D2w&l5UacZ}T!bcfvWSAJMTw{eU0p9 z=o6$~;&n}dWO?{|hR2-`fqFjyk9~FkjHsy7`y0`sl;$`V?@3Gcn*yS3?^;)8LW;3n zCQ`bpiE}HqAa}#S3ESVe*uFtKj4!fRwp1;A<6H@G_q(~gc~ho$4OK}^w^d83(gx7G z(aqPWiW&a@_kv3qiwKaF5|!$?)1Klls%|Gu#n?a9*6DeM^z!wszL5;tk7xT_HV`5@ z*J>U;>Sz1m>e;CZmprwXA-qMt`ymG;2=crG#saNxc}HxD22+3y`w*ydb+#vD=QsBplLP@_h?zgyGNN=NHRXH zM1~rQUTCor2#wj@)Ol9q)A)OXJHUjO3H7c=0CxyTL|$?S?gvcu%e&FJyn@kGfgOEK zS()kQB@6V5J6xpYNI4>(9@}dqa?5nYh*Mkc_mfZf%y3lDSgehLKQI73mM;P6rl;4- zd9$u_S?mocmo`m0;Zo&Xn?N0v^AR~`#&vzQd_cuV3Tq7ssIH0MTSiqZIvTvySV%RO zO(`ph4K5#!23(khg#{iE?$^+|UxyT_-tON%x^LF`CRnl3n01E_yiQu$lOK@IQvVBV C2deM@ diff --git a/moveit_core/kinematic_constraints/dox/img/fingertip_cone.jpg b/moveit_core/kinematic_constraints/dox/img/fingertip_cone.jpg deleted file mode 100644 index 4c6e8ea35e7884bb759cef269b2f485181446514..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 26612 zcmeFZRajhGwkTXU!JPzm5P%0|Olu9UT*g02dPr4;vl* zE$Lf4LPBC%yu60UbF$2edZ`mE46R*$-Ua|fS`Jbo(0B9HpC|F21_}9B4=m1CvCtm%jYq5c*fQ0}6 zzzksE|NdaM;XUR)VB^M}SD1O2A(b z{=Xb}Rj=q{ui6yV>3-kCBP(STi`4laETV`3vU`x?ksUU1el6lrs&eC1MrO4|AWQVA z;qmV8ZFa~@V0WS3iV|FC&F;0CAty^g81ISPyx^Y@NCt&4RqMWwHI=$qI^62`Y~vfs zi4H!-J4%Y-KNUQed%3A9+R*jcdt8Q)ZaoASHCs(8IGOIh>#O&k^ePEs>g$>HLC)Z! z9R&0S>Q+R4g}b=;Z65v`(?VU2MPAG>?#MtKLs`8->K$=CH8Oi&JsJvdAWC++S-qhH zIcGYCX)^)Cob3I!5E%)4pf4F2ATSW}-~1ZfP(53tP47UGom33r+cFHjn1=j7zbD}R z;|Yywx3O4ir0%G^KPl>Nypg}KRky6~1#sbxSN!zup%rg;Ax zm24~i=hXZcz>jD7(*3CF7r?!J)-&;)5Y!d%Q(F2HsP4J!1+cDm;B-g)JhEe5XT9j^ z{kMPi`wM;7ePeHGnp6{%ebuvw!}TMF&%u>nuogQAI5{Yz6!* zIw7to(s6T3VJfb_4GJKp`H1RlD-*nozyMYG&1`4+uYAtTc}frVSfVr*(mx*$lp(V^tRyUpFvWZF>MXV zyCZ+f!sff+=6wMuod4MW`gg1h?9vxr1%E(uKf*nAd;w7SRO{z0o0zoj^T|KSr~Uom zZ#{e7HZ@$Yl*#=`RH*X9(BaLK_yBPs=HH*KUt%1-E^`+{o2@q6!lpm$VL@Ed^%SGt1Th82^4@=$o{g-Z^X!|-z4fsODZC4WC^Mz?O$ zqipM6#2Gd5)A}+`)~yko;!eEw;aS29mgZ?^-Ra?P(*6_Sf53`e7<&P{kvkXO{it@r zk6HG2f@J@}!KUL*O7n+5Am%RrxRvsWCKe2Cb-PXT{ws&%c|1#dvVV}rcFEtN^cZ=M zr1CJ@b?_INHV|@dpGf@;VBN20jP%VPWBO&w-qgB(6=~nzD7(AT9{^>VQBPxEN0Bc8 z+xy~->*=q9E&d0K*-C<1ik+0a^vXl9-Qzt#h^^=|#TGv)Zl#*+1WS?@u>oZU3u)t@ zTAP*qe|`Ub+FI}npt0x$ka^NCi_iT!X75i9K3&M~TM%OBL}* zev>qaHGGEe{41#-!sMRK$d6-zep4s0>h5p&y2;z=)b5gXOHZ7Hi@@IB!LG6IJ+k0V zKbL2X9Y?8#Pg)gvOz6lV1^iXY`xA#=(up!+Bz;Ss0dIBawS@8n|Rm}*=;KgyE4 zddKeq*L3jr>5J__%;4mfG(#okFZm8=bPdS)1C~)7l4~?4prfiZt4p_U2 zR;x}!xLwXOEH2Hn&Z()I2{mzcHF}hOD{an6HLNoKs#ST_*#53wd6hB8?{xp&om;c; zOL!(GR!O5=Hj$o{=y95)QgkX2-fE`?t%|L*B)d4HM&4?@9Ri`OwS`~bH_JkQuEfuZ zApu?Hs7ys1VHB~I){==+rFa~Sg?oh5?rBxEr7wVeMjofK9Hn5hXcN z=?J9KXtusSh8ck1iuISf>B9FiqCoi_6K-yTqu$w4#IzaK%@aEfV9@+9v~Zt z?Sw07Q)fww;{m$_#{k^ebn?~nIcriR?&lHeZDAqR`uD7J>hEIGI>r?<5)`;I}yin;S|QAsUDimitI1t3zf5sJm|SJb+KPYOzpw)b+IzmU_rO&rtsD6zTUz=VCv zSza#K+8Q?Z;>(SA0{WOIUXD85n(HPTNs?EPeq@(C*5w+Sffw ztx>-KVr;r4tx@Gb?Afs?#)43%nmKzY(Q8 z9F57mpLAr2T5*QOe*xfX9ckQ@u%Dkk2}FRk>5l7sQZA{LE(N{UQ;$i~^=(F7+U_u} z+4#h3Ip%koyI^ZFH)Glu%RiPF2&<1xhXHU9DGPOC+_Zc~UjY4lL_dfy({{W`FBa}R zB4WJ@y7`xC(#KpjYbUBUXoZJ?j@V-u1&l7uAje!@}jAK zpl=MSnT@_!Ips53ZQ*{5c%Dj6j)U#h2a6EhC<$#85tCB5Ll8|}V#)8ji%c0;yzSua zf7~2I;0wCsvLDJ()DD;4!jjLLZIWotlu8z>+;FWKp+ITZb7E}sc`|ze9K8VU;`*^@ znz^QPpOOoC$0LTBJ0h}V!zW~WJEnHndjg)kl4?ol_MZeTeoTX z8oj2BL{ajjYm%eqT~L?4ol2DFjLAkQh7Va8oLJq}&UFSDF-fc=d41ZpojQqYOBaNi ztvc0(d=`HJn7;sqofU-RTMH#?k4^4!uIR|ey>LD%sK(y-i0Gz>FivskKGa6sBR|H^ zo72#kBz@TXu;zxt5Y7Y^r;Gh6Z{lXhaX64=(Ht*dUXw@pUNP9Bgz^$TFbkvO)p88M zZ74Q#Si~DIplUawT1F3&6y%g#Bd1U>?)pGJ{bj>qu3_a=fvN{TTRtYfl&#|YgJUs+ z`BNhQkFFgXkam{(mTG;t8>88+ILA$l9iD{6ZccVS%EvxB7n#73)d;R%M^wXu-$cxR zCFJHVL$Rf8>s+@Ynli?lh#JOV6+4v9)cDV{W}N{+$r9#u9-G5_M>p=0KM!=ZfTasr ztYs|8KbJl$*)V)C;iK0(T~6*q27hrwxOKWV>_DpU_$CJzVVTiMtkGYpwf3Swtn z$r3-d-Ywk~xUvl^uGcpc;mRQ5vlU~~#)QH}YsTdPEz25wVIqB?Tf6ymU&Fh8{ zUhz!^AE+0A9K&!^$LTJO$IY8a_{|YbuLVch!!-jh-gkyH9jgTWpsP0nsj-=-V;0on zXj$>$*5RM$y%E2MBWlz0OsOE{<%NNzE{v<~)IQUqCSYXxO`)^+_cV+|CF#t~I!w8o zHnD{u2aS(+kfV^)FGg?7%iR<c;;6q zpJ=yl7?xpdOlplKD3Ye^KUP&tn<6hmw05D|tZi%8ei=r=zk;p{M6S?gk8INgNtVAu z8YMl_U`$=P{Qi7hRKj_7s8#1gtW$aEkuethb@xcpW-zv{xXH)_)p@58H6F!fKiFm? z1Z*;eW%6;9eQJc?KlN8_W_dNbfTid~EDQp>nv+tu97~I7lCIG1bvj3bCe^ZC zuzf-2y8(rGj;n_VAJuWsh`WgGTDIs|76>_P-&!jebCj-B_PP&=Z~99qqINYZYDO%+ zMk|#skoCk|#{~^kJ*fC>u1wBZ$jOAf0Dk3FuRKp0{A_7X^*FUMMElr2^qsBVR9Jvk zKa2odaHYD$;ajz(6>)OkL;`2d6=ppOM0>_PK$9A4rACR5pCv@veTq~Ls|3h=%MRVR zJd`ul&t@Z9P|n1Gwr7oe5!ZXB(*jSlzSD`H>O`3lSB5r`_9Z=Bctq|7CkH}wn^Co^}g$LW^8(R~x6AZl9X)VU(tZW*E}*_!4-zzX?9 zX|Oct6h`c)x%9U5la7y&AuCM{-dycZVy#*A6IrucDoN2_vBeew%(53l0|yTXbK|05 z$7cLj216Wm!A+8aw%)o0RB_S;qMYpv#6!!ty60!WdOl+{{O&qj$W2BCoJz$}brBKf ze?$;1l3n#)02r!F^ViheZn*h9imt)^g*EPuq;;xNv?;DS#$ED;-DdBsz;}#~^b-?C z7@zf(tiy5Gf1XjFE18)ksBj1qK7{K(jVSrIKJwzNkyjW~bI zrqWMaxLJF7g7ZvNHe1b+z|qhZEz{4V62=Q_a~pH|loj55StV$sxlp%2l@LAJbut_T z7S9^`im#FNfIwH@Ux6+F(IPtX$UkMTpsqN6^sC#X`3Z&V0W8+w5GQM%{G3=8TKRMU z-B5woz`!Nb=E|hG?4_*MFly(p2;3;3+Hwu0Is@lazm;=5k?!N4ns zjsu2&jigF+(wf^YN+qCY#$*=eB*h3ljUCNY|6rM2#7AT1e821a9J-$Oynb~yT;~+h zw^}^OJAOKFl<+IRWZAzk7gmNs7C*=D2vu)W&>-2Gfzd9XMA13rZ3k@th^F$*U7di+ zcHxnNwxVRec(JFvP2b+aSUE12IN{hBE7d*2chssBwJ`b0R_HlPsg|JYkCSl1P!!OR z1S82}xaI!v9)e~qBD1roQ8&MZeToUeEgzi|_T0-_?8`F#{}HQf@WeZF(mV@7*3WH; zd$X3SD;sv>CC!a;$U;z`(It5f{c#fdNvkwjsa32yY~M3o2OWe^rTQZol{i{mhIuG2 zvVBj1&EswiFYpkEm#6ozB}__}@(Y|fm=>_^+NTKW&{S})cUps6bFQnlUoWb;I!e@G zx6zgMPz4nWsr?WpOr1#`J-rT}>g%B){w|-iwc;fbzr?I_Ld_l1WiC&kT~xB1(4@~R zFPzQJPA$(*ERU{$Q z4y`(lDtO7Iq_u?RJITmRwW>&-9C)@W?LWcnir@B?2;K9Io{JV2IM>V&NtZ>S@UgyH zZkl7$eqR9P5bjOv_2oD%Ze?oBqYb@?_WSUG<%Wy|!kWN!0OaOR1O{48|F99On^YG~ zal|u~ZT}tEY7EJ8sNk9k@X{oqI32@=@1C0MuV30?;51?lw#uhs10GzsbcRx+l$L_l z#2*LWimu{t2Yq6nr9%j@wU&%0`jE?}aUN7}PYdhVj(2BmveNE8*vM4oUGLwOP44Vn zPGsM>zN#p_#+1g?IPa8FshFq>ckQ8M_ps? zK8#o4vQ$p?f1Y-OSr1VYlyjRUSkGYQ1U!aMOaSw3-L~D`;<% zZ4!}cWlh2NDL_BlsuEVk1bDn2d_&3d>ko(>0?^TbchRbKg{+`7jxsxDLg>&6?!oz~ zk0Y)+Zr-M=><|TUBVM8g>g%hLq4@PkM;;+4?SsI>IIR!D8jADDpx`7*034bCY8_8m zuI8!I!Pz3&51LdVSkNjC>%s^w?hs;R?Cdb10^vSYQZ^8$NA!Xu1fPf?e)LQ;O#6W2 zxHk#gR2!+>{L`%c+7EWBiziNRiTOnazZyt8SuSau#rjBbQ>t#s@b_7*;`r~pTr^=O zt#6CG?zw28Vt_Y()?ii**qP>1)Oh$&$R1o`Gx>e7>edjY9*tYWUCRTr$+}rhzS>$d zO9nz&DMa^!5n^V=_fN+=I7#NlZct1pyL{hL%vMoQc9Vg~H>nIKlEdP5{?C>-jHP@q zL$piEZdX#Y_c^T{G}6!FnXbSY&S<#r2MyDFzLm=jR*CBbj9&CxEfy3yS`muuJ=hgT z{T}gL4_b@U$DTTF=;*HOUq$hx_9c#*xshKZq$Sskws~9;XfikGrDawn zZM1^@W)B(8?wUu#2^v2uj-_VK-6#8xp2ZASgRFXT5}@?A8%>)wUSy`K4ReLtccSdu zr%fa0ls!t`dOwR3dhHo&P35$< z$ZfXPAIpSH)ULC&2(fTM-9BJR+PN*ti4#~t8+rBtH&t1xQ#5@Bf4%^gQG_1Au#L_y zfUxogQOcbb9dF~iA{wJx2OfVq0XAqi$g#jYp;VFz?0}et#xf^f(1nBNg4Nwu5uLuZ z|C$K%N`S=1u#_E{njEGWi4PZJ1#{FN_wp25Si8d3WS{^5bqwMTd&6SwLcPVH{p28AaxT!-Zh`-Ryq zB^&dTp?K7CqDheCbHH|3j-V@u2CI%KuB#u>%5E8!Fj_*#r_Zr$f)Pd2t&SW3a~irD z)3*|+J4Gf8Lvdn6G ze>w|Nn>XveAHl5^mK8yh;0ietQ2f}?M4S^WRm-HJ>^D{`AZ-Ar#sSf$MN80lvJ~ z<`eH75DYj3oNe3{I69X2`EQMSRe6LBMlj3c5z474z5_Q2z8%d9>rKFX=(rgq-O{8f zAg<=PRQ3aNA$TMwb=C#89`E$hy4&|V7V7kT7tz7H`$-mlT(h{LmpB^!%emG$tXiE1 z%_ToK#`(N*b?H&2oxL?d+6~({&*K^IC;4@Hg4x0)c^0Bp(|pO}dmB%$Qlb&}s+pP4 zjpPJLIT*@M9}2Q8?ZoJ&!x5!vy5c(C`Z)l&NAPsh*LD=g?QILNtG6pxY;^f1E(xd7 z+zRDu^9wrY*-f1=xviit(Ysq>~00Pt_Lr)uw16Wtc^XnWZVRA zE@4WjZpn-!JCHf0iJ-r&;=GG?SpU_EnQc`E+*!HR%FFTKBSms7NlWwE__8p2z4EYG zEA&mWUyRm+baB5{b#h0iL?jC){`UCB?#Ac}!!Y;*Jy@)FL1sI^D0xFNRLW|+qvv9o z;#pcGF~=ayxw#l;yo5NoB0R!{C8p!V=X0u+kHkGpQx1)XlFd82@K1NOuFel3kUf&` z@LjS!)EO=6=WGF8CTxVKt7o45$mEBWb2ylatB6bZ5S;$jLYJ2VEU#9m2E)lp-xNx! zIeY6a%U-`jOvkN3*$JT}?e6RWmvfB8m9=*_@8z)(Ho>SldLHf1InP!TrD?JYPvCV~ zzGB?L-a(sL9V`sx)s$EWYeH$l_(GB3=4Z+l@7tuW6NJ+1v8&HIX zHG+AIacdSWGiCnMo<*FZb+7U6(qdWOzh*;ZXU#&njyYZL&#%8QKmorO7*)exbi>m4 z-H=uJMvu4cV}aOu$#JkE=}y_ALQNAwrE!ni`^T5~Oj`$QS>93zwnl{Qn7Nt5{vTHd5e50MH-Bp~F@zRdbyw?+M zU2|UobMy0y$&(5coeX!_7PfD`ifHisna%${6bwMj&7s26d<-maPQ{eg&|K~5XoGjd zW#?X!2Q)CSA>(&NGPEjL>c9`Dbm)f{nwp=fiNUBSy#vh<(SPCme!Zz(07X^zZ84;w ze^&F&MdZ7Xx{ISQ)W4=EpbCVSFRM%Zll(b)eBa-`W<@GF^Fs@cG%bu) zOmPK7n=gQe&UjLV)SYT?qmr+~$8zMDOv{0(Lh`qZGp6=+d|l84e9=FXNyo(N!3^VF zA0%MDMTYpJ5iEr=Z@hh+t&WP4k?JIXn8ER=--_a{Yo%W;jZO&sX#i+-JP!E9gUfEj zH1X|=@?|t@riIPm7dZ88!P?qB-3li)bRUO z?8b80-7sJ*JrDqGwZ5z8C1BeGo>YHO_i`}jil{PNh5hPGX$F)r&azqdm?+r2zL2lZ zd)s3E!P%}y>o!IKU4*fNk(w78<)>QWP*&)d9R90L$~qeA59)qZjFPLqD- zU6d5jvPNnZZ=AG|guEP74u!ISV`#KnH5b4nnp6VS`MHpwVTV+Q_oy+y*G2p@2l?2t z6mw-FIVcMy*Qm$(>9F2zrfM`%Kzgo;u47z7zL!lvc9&sdfR}$y9~1`>PEik;trFkU z)&rQXR)=VNkI4&BDBqq@8Aclf+bbG7s`>V4hr^kKY!aiP2F_iK$---67IIG&M-)Fh zs=WmA?hH8Fy|^b^Rx6^}84bl6tHxZ;`m5Jfoe-hh_0_R#=z{gLm+YgHH2QKQ>w&50 zC3iErI*81j=OFo$O3VfHt896&j?`c-J)(7)Z4~jmNL6 zKK;$Z+{tXzoLMsWEIwUlGa;I%zPXgaq4Elc(Lo)&Q_q^EDC2(`??NV$ospV-nh?#b zaE72dtq6kS9B%m%@axetB0MwBFv_)bhX&S`5LFBE0;^Tw$@A}vy zcrJhi2974Y9N12@=H>oQxPBL0n#<`94m8fuuKV-qHjN%5yrbnS_;=>n;-t`=+5Vii z@_5G<4#e9ubQ!wx^<2H^Md++yu7fuv_FgpYzLSpX+$dzr)!id|gdk~6S%Ijy=HQIp zm4QYMZ~8_+P9Y(MFjZp+9K%?S`gD%u;lKyyjdl6UJh@56wF6Vt*aRTk&yP>E-`z@E zO2u_D#liKpTjPYNl-i{^MhutLjXh*8p~AfF7u?*ucmNP9CRb8Q-gR}Cp_oxtRiZp~ zq~@Wm17RXNsD)2{n&=(Y0Ck|Xq;qo}s@ZC@DYB_44nY0AoMoi4OPAnC(7DFp7pfUz zgvFDRhLt2m4F$wmojLq^e%BOeCgU6H+DF&-mlei_&TfnN>J5w9w8sjDo;7B1qL4xyEHydy>;G8<^DorSaHUWN$W(L7{P+^-80KUoOvV^a zP8ybdl@4M>wpw&}2bDPG-PKiSy@S8s%Hkan6|lq?oRz)Ea(RUTVR52E&AgP3@@#Yu zg_b|V0?qTf>SiEo71tet`rzgmdQ_gK7|Am4QN$BW?PrD zaIqUc4^!W-i$XNBF0+;=%Vv}G7%fMjb>&Wl@6(seDLlr#byR)J24@63UoqLZ;`@os zp+5Y~s9M#Zx})Ak-ezl@8HbfNtS&~n_&<8-n+y7#NUk5y`ec>&6)vV zfpGVw(@qI+*onJ!E{>I4p5c>7yn#b>EVC{lz~ADj!EL4WjaU18?Q6K3M2Z}`dlOn0 zxVci-hj7%273tT2i9fRa0*Kxc7#_D@0t-N7iTPz9vZqZiiIj(CK;+HZuC?6h54q+)6O6VLRXiLI~Amxc6)5}hU%)FX&O^x z(#;7_4#f5lih|iuG4?$l9=xeJBf!sb%@HHLDlnyWQPu``yG4{x^;x<}lX-<6c;t4= z5w~t`Snk`bQ4;zL^seQ|Y!&?alf4&wPN@OefmI&P_t;d^=gMA5C%TX!+Ph2if?U(j z!*U;7uCH&eEq$xIz~jcV8pP!Ir>T%TYO-b`3b-6C`Yys^GPPOr04c>89v~c4VvAc* zU%#X=quu%bq`7?l-lTf;dJ^Z!GI*6v0XIwj->8X&@Hob%7GHvd@-o{Z-qLj_j;v3e z>GB9rtjw~aO6qQ#kng9p`b9Zp>=o~J%~-8WD34Z%Qj{itq&y!`xTDMof#Q%n;gs5Q ztXb-NXY-pwzTf_Psop=T2t(cx_08&%C-;O&hxtSOsycU6?`BEydnj$$+lZQ|OE9mS zft77nk$5;8Jk;ehW)zo^pM&-n7g;@pl7tTJaraP}rm5fbtN%{)ocvS4g^BpMi{xV| z+B!?_n|2h~)zH*lTU(wNz^*-zCUHrcZ~Kg%A7b$V)#hgNV2ZO6f(He7|61}FLd1ab z3j)V1(CAQd45F)q>Eo~&BfCe~l1+sLii5d9xFU+UUC)vgZ|lmJ$Un*sRiqcEt$P^B z&yBjfj|vVo_cR>rfx>bfwznoBtq`w80(O`KcwhqXLPOj034o_nKT=wKi4Il+8lsCe zXM_a6Ah}wNi;Ev+J-A!xF<4^Z*RTJNa>c4K;W35Ycg<}Lt)G%qzF7+xkaxnG!;%KV zXR>j|f$9MZi>1g00YNRMC|=SV9yf_&%U=*-iZO627d$9~{T$3{j)dC|9CaeO)PLFE zI7uh?nAoVfAP;#pg?QQwCd&I&XsJJ*U(zJC-6JD@1)CjHN)xD{f0r7&_9x(mU(b4r zgu#R~XN%_pQ_H@U@XN-clpD9mBWa<E9Z7vLl<~dG% zDtuDo+~NS?8as{Vtn=KfC>cJm7VVE@^uMH)Ax{>?F`snn?1Xj%`ifSge>Vj>e0S;KmPlfxv#iJ5i+s^ zWf@>!7n|M>q25cY{n~s&IMW2IH-|H+vGbTNSXD;QSe$8m6nsb8JJ{_-MS%C6dG9m* z9&q*P0dhM9BBsQffG)3V8ghq)d5273bVQz*HaWZ!)@D?a$Lg87N0Ya3VsQXrZ@=iX z6LNU?+q!Mk;hf&pFl$DGL^wIJ+J+Jb30r6)7}L%#zd8!FDVQ$pK5r*&7)LC%9E(c> zQxJ87Az3}`g>GaJ3BW#GK^=v~wh%CDl6x)=*>Z*83x*tB5? zSES8yHcBjs*!k^flXjpkvA`%&^GJ{ESyby+etvEwDD7)Z6v{hnjP*I?6(VZEtb>>; z^EBi-0|M9@tlbn z=}WP902USwU#E^?h`Q>r4-1uaK{N+%PAYR}H%f)yByjD%>Bv243>TGv4mqcda2R$V zR8tXDeRW@l!`FuO0M%g_c%lwF9M zxX6t{*Gz8;HXQ2XTOHl$yr)LHKNeWvkNb^xdmIOp#hwYs&kHQPNoZ@_qpA5piSSv?E{AGM9xgvVBLzghMR0RB#O^FLxJs&s5>+(r`MPNUl~19cF)4qd<-ol7IrG9gwqGmD)4DNE`` zPhITG%~8AqL)C45>3dawF#4+npg5JidPd>J+$*2sAfuc^SN6kz`r{)?k+JM~zNHT@ z&Nbg0AO@0ZdDwM;byUJ|Uikwv5AH0}kYWOE=bV}()dgV`+#1W783F1|Cg4cw zjL5xP-}f%>pDWKsEv`Xfx`bO<K{bLh!J~aN&G!vO0MH(fnixS z89URDXashNI>lVm<(Iiz&*+p8jED#NXw_W39zUWq$Xmr04vci$$(wCmVaKcMx8`iR z)ivn=0y4QT0BHIUZPt676GVdB>yrO$VW?ZXcxyrKv;>kp@U8u_mHuUBsG1BaFR-@j z+dw2k_Q_sB5+g3sW0uNDBng=WP8vcCiNf+H0oPh4h{9{qdAbfM7<13g#K0&UKeG!yFkk&X1I~B`3Wvc zd1_ZGrIRlq*Sxw+uSVtssHXY!Ce$0)u?SDj)8XH{lkm$r?6GN0*v@Q=5QeKF)!C?- zFUujthV0C+7m^!&cwkLSQ`hPNnue-EnY1otRRgzU4w*N}U#_r+s zC$#{zTJtY^&|g_k0di8D>Lzwctw^`p4)chqcw>@OZ74JwRjwpKq4B0K0Bfffk7hpz zn@-x4K*ib&L3-}+3hBm{6^1?XG36Xsh_Px|+v}2b2g`*c0nd|v?gVKsrN035eV+9m zAlDeKA*|2XW;|EM&@eaI^nf%s#%k6X^JGO+#|K!@2tUj-G41OZmw|nfU@hZV{-|-0NqmPA8lFy?-Q5WPNutfS!FJ z9X$veLwD4Ux`JHpyXc+bXF?ftJ^Enne|3Iz{veX4L%=h9gF!6pg+_ z)pDKEkk4-8bGTvic30&-pl0pfFLBQvHcxDg)_C7wko%w)F88j8!j$e5F81C92fQbg z!P`tg+{StT>ho8Ta1Z2Z>T-Bsb2xBn`(%!ThKG~8?l&eM)TlQfZdU{CMeUO<-rp5x zy_f8fMuvWkO8>XfEA^Z0cjB}Z-84wH`|3SQCv^aRESM^T(@*cMNmbsJvj=5cT(mOE zQQC)veJR%^?`bTY;yPWeMxW?m#*mq|JAx8CQb}HFm`|>hJ~PWY{678^h&mS7`f)H= zU$Lo0^FE<|Uz?QD^&{R{j^PUcn8DZjyRY!qwjPGRsTBUdsTA7;T@&5sPlyd@3wPGu zU2sAbZQ@42G8bU!2q`Ih<(l&LCA4^nv_r?_n!fMZ>!i_e;<-{m^}w^fNw6-*HFqmw zo*xJ@h_Gje?=F_cgWywS6|mcLHqV}$(k7&J1-Ur2l(uwF<>qvn`tD}2Vju$_f9zb- zNm49EOOdcm%29MTUPELK0}OGpMW8`yI<)q*=083k%?Y%KQ-1utB~LMf%-lrC8SURM^r~w92X33h}qcW|QxXVf5v`N1(JiCuT8tA@$pTY1I&C;G!|N2LrQzvC3YDsp_ z>dLpF@PZ6&5S+8Illmb3denFyc!p)xzZ`6V^pA4EaY*%0lf>;Ni;^fM;NLuK98>8} zxbzR$D#N9{HAHY;nFsqDSVc{8=}J#N7Evk0IZw16#BJ$Z{xe#AbsrxuRlt68<7Goh zF-=7n@KaxU7qoH41aWA+Y8LZ(6Iv)`8^-s!vb~J>jb8zIMI4+li5`4Q2)@P)lQ#k{tpA-zq+`17A98o6g4O{S!PZ_HKP#TOW3fLxtgFKa-T|f zh?xj}=(p%Aca-{@+run_@$CgrR_%`}GbV-UFuC3(YLFubzDG9w!+U``(*EZu<`n-v z@=}QD>M*(nwu+h4%CF4^sl7EvYz{K*oAQK(h71Qvyg@o3WiV{M|;+zsob@Xu6S}1 z?amVHDLbpacSoNuWS{o77y00@#1J4>c(=E<85d8RDhbci*25K z6t)RVCl$mch3_?2%3Y7J{)R@kT7eV%CfF)Rt1WBNv_^rPrJx=C6wYt@grw0)6@91H zw{2z=J3jQc8GH^^%4rwM%OS6((elt*`etdgn)0~BBEMOIG-0~9;?VrY*4j?LV(kxz z()JzFLq^}9*(L9Y8Od^1xuq4bh>PsRZjPY7)fo;|_4z%Cbt5g zbb(m{o|BD=h6`Wbe5;2)8GX0we_aO9uSp5CO$PaQU2QkM+Ms2snaY~6Lu2%yabbn9 zwWIhi3u4gWdIoM7V8&5CljNWZ*N;W1YRW7(jqo39Iu=HDj64M7&rr4===OBwsmiiL z7q0Dwz&%@?ZrRt{D#F%mO2$l{@2=ijGpbM;02f4QlTS}&0{5%ccRY4-O454AuIE9n z1*x+e-Wy_uFlTh$6<4ex9I3%jN$D&VA_89jZFm$;F?nN2VZ{(JZuPwZ0}+|dEdz2?4%q4E_X>%P7q(C zXR7$D7Y)sVG^4P5zbng5d{#sS1aO{9X7{tNQ&M`8s%)(t#W~{c?GlN>QiFakV)Z_q z3L>WO#AHL=lGzXmxxc_9lZQ%#L7{Nhtj9_H8JMZwZ+a}^wco`mAVlj0fEinekN}<* zRlrQWBe%7MqPj!XhiVO*NB}p-A22P_)O)N)a#rG?=dM+#9m<5oh31E%6AYo^PC_y! z=jb!NH3yDCx%#`2(Bo59AMeeHYc4p3BN_Ud%YEG1m@idfJ@lvFnC407HGsSU^mzrZ zhz3e)33ZZ}Q8*hW#dub-AB~QnvcN=pTEq2hO9EW^L89#{{kEQ5W+3O`@s16bDo03` zKCRI<4ThjD%E%GH6snJ(Fmb6_cr$omTLkj@J(w;SY_b7 z4XC{eKfsh$^nK&iqqB-MDTDmAs9Er!u6T%Ng~lf$LB|_e}31Ba2n|V>SHcgaB;< zRKx^~Vnd;$Q2ge$)2*jx+K?cXUQvm?Y_^e(`RJzh#>`X(zVGQs5e^|il)VGK50rH1 z69Rqurbu7PwQz@b8;jPCZLu94dN|^lh+hNT2ZBqqMmf`&ndBF1{NKiK?$&}`haALa z%J<>xRfd%GobBrq@Tj^j9>$}wf~_j&-2Gh7Kur7(z1BX%M&sGt<)GIPF}Wn`Ewgd` z0|I1(%3>Cx*AVtZ@2r+jx5LU})WXtdmD(fo`z(T)kc#%sM0TBw_q_jniy|Z8wFm(c zOqf8~DudeC1HRn=p}yS_OtMmaYLzavOL2?1Ujh4upH@p`OJw!=hCiv&QWwj=3W}i_ zboP&P6a*{FOS9rcO~*Ljq34;${%$4jDSe+>%l%|P!D-xKOz}~3E@W)!B@y7%Lc+A6hnpzFR2i9bAH0bUJF3WI58njVbgeLS z_U-lnCE?`CyCWzQQS5%m7x)e@TjpS(}sttSJL?$1@ zSR33DOUQNXkTi}4^2(ekEWEnBU!Nn(LGyVlq8ge&H$-Sq1aaGia*vJuerdo+F z01kzxe4gGQ(SZ`z{6DGJYxR|uFskSpd!%>Wl}T`Y;zpYJb)<9= z@fuK$un7#zOO5=?UIU|MWoTFEkAI~(eBc~o)2=q-VuiOBByv6h(gWZdN>IH3Oo60@ zf5iQ-T$^FLh$K+6EfegH57d`oq$3WGM^py|l?isK!Ykho1^_i>_grm<^W+;tk6G^p zAg0>R{9fA>qkNu=u61|M2$j9Mowctb1C8>tPBPk71q{m^LpdER<~YT4#mF|?Y=L=xE+gt>;e_s83BAy&~W!sNnZzI?%iipX?;t6WmW=4}ubHac5VGzK)n{2qPqu}HTx=yZF+lUW!87~s0L1y=_faAYCDu4|0}bJG@StI zLc~$#mLP)5iQeU_3O3U#K$(6FLg%4r0cluUG@(VkZuPcr;o1Xh@Ju%tr}ifq!;F-R zaP^qxqf<>>cu8dM=K<42iEo)~F$J$Nzp;wKZmQT63@o_j5sLZ@Dwl&4M0Kz|0#8ie z6Hryz(Fxlzq9l-+>+PZjrQztJUnu}%>Ed)5VtIG2>HHHi4WRpUpL5ePwzY(a`OBUz zHgD(R8}7!+=A6cGGn7Do30qqn&h}J71SuL&J9)u6EJq1=;HH*iW4Yz{F`P7XnZ&P2 zbVQhSw3pU4*o~x-~@%$V~3=0R$H6F_1-QN)!>%Z69YSqT2vaLxA!z zb$hk`_q60zOHhH$pjwyN7TdSrICFs6mZ|b4Gi+(xRGz?ZR0gGMh%4|nD z`3FeX#FgO3MRu#an?!?oiq|&h8p6lg@3@fU=Ee zWDLO|7&T+`GLh(v7QG}Hz4tn5L=9dgVK8b$??g$EJ@5DJcVFK=*w;S!pZ^b@=egIt zerx4Bxem0)6T6L={Z$xusC%Bz62Jvn9(YUc&QtzL*kee`sEfbYdOKlE=}z=g7111% zLBfxOHPlqho%)8}qR?L8mD-+Ee?R^Wj6V?#n?pTm81bkfF|J(-b20LWmh;$okVaRu zu;FBFC$SH2okHDPqk8)P*L6@b8IX>K&FUol0rd799TzupY?YZ^rG0j20 zdLHn|)4zVNR#k`;{f#~s9|`{zTeZ=OE^;|l(T@G>B+NN(GCLYY2$;{xUzH()nNAr6 zr5ENF5s{ZukGbmq8ED||boKln%;V2q{|@PZ+r~%fNSQnsByClF(A-`({EME0=V*Gb zW7mhjUtfNU<)e{-Ge7EU*NlL#^hp1wA_>*R2dSWrpd-c+Ywut1Vqsmpj+%!f`NU#@P%fEAvX z4wNi!7oY{vPp0{+a4B!3%v~F~MSRAzOK2qA*Yx8Sr{v_4UCezQzlMMXT^B7CdWWIvv?L|H1v$n1tJ`{Ek2{m1=YS08@ zTz6(*=i<|mM~;Xc=+$PPlIMi%d8XtEuPmlF9zNGL=~1idT^$(g?-1=jvK4Kd=p^4X zVCHXblf+=Ec{i0Dgob72G1^N~^$L7Q?3(OM-#J#VI;aV zw;lWUdT>)=LDJS4Ny9aryK4 z>b`Iq+!`<{TOlDCfqIo}l3XQ(Q->#l!Vqma4 zAWY6ss3A*&-q?65E>K7;-jf^Q62`eax2QL&)|gb|(FR2_BCu z8qh58k|)sG*&_vgm$)V-)A@RVeIj1Qd;Rd2-NH*=;%hx0A@uC>y?v{dsY5pXY_r*S zC}|d@8>%PC_94IbX&TO*Tdd0rUzso7L(-%(%=x8PC>CC3@I!;*?$T`$bN9L|fZ|b8 z(H$3n#k_Y-v)W6osM8NBfd4NOmj4v&S6SpaIMaB^echRaMkT0`gi7&lpvEDxz78eL zO^>$vGc2ti9E+^+j-1L^&ymM$_N(uj>VGUGn{?YuPT-PBedCCO`c*yh9NC}e3Jo4x z1z4M3j}(>b4H0$Mno=o~cK;k~yx&m+6xu~QzcnYmdPGeK;lK30;SRK4=#8d+5HRt82{*(tQy;P%Fd<9Db248_+Ot=`w(}_ zl)nF2NNLQ3bRDVHaz)HalQ(!Cj9^N8oI6cj1ssGRJ{CqU@nZYK!pguH(`DxJl7e@Q z{pEA3%L8HF{WXtOI0~YLmT%|?HW6#$DVJ0P*I(z}dC687cHr|BjJZgf=bpPRL)XsC zLdqp6a;dd!##Q9_#07WQx8yv&>_DtN#Kg#c7v1AKvBv#N&4XAQs$hik)ohOLP2v?s zGO)c2G-#h)5!bAmZ3WBMHOQ66Wj@((tW?im+4gW>FYM5iK38l;OESV-F}`Fk$<0wT zQAaow^GYg(lhcf$Atx*jk^u4R-POPa!}liABMEqxN0YX++5=BG1%gs@R-SRr&CL6wwyaG@!;OgNzj|Wb zyUNtLc*d5BtrOHZ${fgzCnu$nGgt^4kj;PBdwqIF$qB0>s?&_p)UnADGR0ZWA zmrJzv8<2ABkxD3RIDOnkZF0@kg#QX**!l7KLOM#_g4x`AL)1fsBzt$n1V7svJh2o+ zt~Cwwi8hrr96a+r_4W3GT$2-Sq)WO2b;AC5+!eN4X=64-ZlTpF`O#2vIYH?y)I)(; zxQe@H6Gv((^%3o@QLogr^9S$&+NhGz{CsF&J!+i5s>qp#HqKe<&VP^Qg+sV$Ae=88 zFiY2lb<3+idLvzasEIS*3R~>;wx}j;rIv3XNw->NlUJPS?g^{FmA&JXarpc_qKHtK z!`bqAb%~sI!tdghS+Vs7pUV?0GL{AIqlq#*+0x4Q#+==}-Bw#=Z#rJZrNi~Sy0v-E zV`@MQ(d>0n{7T{w>#ZdpksOv%beeZgSL0mb6sVz)tuy1%o@ki6%Q;vatyn(gPDgmt zG`|>oPS-w$_}mA~y|%#to+J?THB5w4z#ctTX|D-hmK_oA;<`WPt!NUrj5r_z+ z=UG<5pN1|pZo+>}4-*jw2jDqd!f`wAk-~KiSdqx(m1H`<@PflM;|wUQ+GY5*Y)-!l z?{_#jMjv%0i(hMPRjTPN7T#a@Nt0TdFef%H0grIA4`Q!`&vH$G9yQ+ztp4Wy*M!X; zUuhPomtz~TnQ5_0)$rEGvZ7l^<(aKqV3Hvya-lsA9jFa1U+TX2zr8~1fqxXITf}KX zhiL6~)8X}RR(=MQ6558HD|)lV(&qJ}nZ zrGw`3V-@hbUZgKbX0}SFBeXaMfUG&Y;3DwzyfburhQq_gr$)=?QZ|RO`6VbOtDA7a=RW;neQ zToYNxj-LOE0Tad8Bx&`{Lsfaa$OA~;_GM2uG8aT+t7s2PpI=p>1N&_vJ3cnTzK5$$KF;vq7MOb26Oy*{r{f z(0*~24_$;!@%q~JiDi*4*S8x_xV@W+Xf=55QaNq45=KJgj^dQFfsoF^WGdak;;V)3 zFTk^EK!+sBch@FHDD~TvGa&ghKTB`x#sD27@a-Dl2rPg+N z(_~{*Hcu@Y2=XCYLPxTP-v$v6K=poT>8>D2asjMhrEnUy;8CAkH4=0Bdo!PZv*5ur z_TW|>ip0wY10jK5Psjza@?l>w%#6bKy>N3D1U97c@K)n;L*#c&A!A=q|#W^eTi+^+7MIPQM#ap=IuQyvERnmU| zCre7be<9HM^S;r+qf5BLaoD?b9!gCyZ~o`JXbL{=m)!2TssKuH832(E;6_u521LO* zdZ^iPWAjHzNv>8ewZ#4QBc;9(I7Ct)2|#T63*3^nQ4+htnSu?83N9-8>w5X&sov9I zwr!nkmtl!d^Mc*Hdnu<@%a%Qj0WIIe#6>K)%yYj{;03D$`{Y1fw02_h0MfY*tqamc z60k>*EARy_E_%i?swYRS7RP;?opEgC7 zLOdT)2ELyn?Nhmb?FV<`)Duys$toxA)3E8;_Lv#GF%z??*CC}p?}Q8Kjscu(TGqik zgsUsbF+QR5W#+kZsj zqj^T7y&s)7>u>(-%)h%GMEyDiAQ#vFlRH>DK8t@%)uYDQQ z#K>GI>i0!cx3A>0wBYYF+F`l|ASqfw{bqJ~3|Spv)TgilibOpBBvS>kw%Mv5S|!~I z|CpXiOsc0B^Zn{w*7O};Pl0Vu*g6!i%afV1vmy!Nj>oerWiRB)hPqL`9gQj~o)j75-c72K7!>69 znPZ&Q1IoMq@{Mgtw*MB0_G}R@VJL>p38j6+^TtiuDsGMTH=4t8!U}4u<=jF^&tC^? zFs!Ni|7$X&h*9wUTq%GupM=f&@yZ%Cl}tbr4#aYwtupF(PuL+rCvYGv__r!nNUSFj07mQm%TT-;|x@s zFXhKW&Cs5LzNnKMQX)MW8mg-Ow>Iydbz$>lCoViMFNfOUkGzIO0#FD&D3HYbO|NO` zvKJ$Z+zaFs*;FUs3CFZLMSfGh((WAbbtO2 z&)|6&%ichX)Rdrp7t@l>6GBvGKnsh3f}k{!e4m+vpqFM;;SH6EA0A+4R9-g`Ulhx- zL|c88%Q1+XpnTHH8Ik@|+43sav9w{nQ3P=_SVlx+T!*AG`dO+{;b$gzyKvzX2=(cNe^epUqDi}-jl`f3+X+q7$iVlgmn9*}V~g2-#AA9U@X zE&O%itx`q43Q1vYJ@aw_+uYA0W;T!jgGx%W_sX<#L{rB@`x8A`Sn6W@bH*E|sc^9r zD{OCCAyMM9x>~J&75gs9OF=F1;?e@d=MnCW(Vfgy#l=*^Xww3u&peB4|qzqF2Y&Fak&6g7I#lJJ+H zUK0#0FLaA{7JhxLE^KWQjKk?AnRl7&Z9EQ7 z!h(9;IR%o`HU0o*;x@%6PAz9=u<_Lw>jpAsok&la(cH&|4)Ee-mv97I5`BMo+M`cA zefS@?Yp4LqVdui^aihJ;4OlR@wuz6Bh=gu%`UWw8Q1g>wdB63s;5!MKXki4v{YgEM zXc<^Nw5Mr~Sa)iOKV#P=HrYash^}ybqC?N9E@=Ef$Ka-(wED7xE5c5Of1=j<{QduF z8+8ANGfZ6ZWb_vX9dHA>&cDE&V1&@fC zNI!kdEno#u`y_Hj#k;*0iVzZ%CRuJxz#qSXm zxa(LAPUOT6Kwt5|%c!-&Lm9aR6+NZ4Z8DB>42n$1DJBd^zlhei6cqL%owso-uSgLN z$@$*;0A6X#xXI}!1)nzQxT$qP43;I&jp@8(cGG#Cmr{H!l1)^HSfp+9y=g_~Cvh@~ zY;Lv{uf|6G_LxpVi8we1$GEi=D6LMK&qeW}&b_yLSy?{_`(g1?wtvI0g-cZkPcncD z7ljgL`jSboHSCJpPJDp|xUzcG;Rxc)pHHZ-$Qlw1G1(_gk;3jw-$yO-pFNBay()9n zYc0zH|5SY`dR59ad0JsSpv_FHLwT3-hJLUxSLK4pC!KG$$iIE#=aGYM#|RLqaO@f? z&R!^8(o@&EYpoZoBj)G2C%KjT<-(=#ELo6G3&8!^L!=^H8w+oo+EKNsJ6Du5RUsc; zo(@?8D}S5RP^ARicm#0P_59~yCiHYPuZ1}#eb*q{Jw3Jxr43UHd@HK)qsWA8M=O*H z7h~t>lDaiO2D<_?>((au>pVtOH!0?HFFdS>E#1{E?0yam_nt3$C{3Pje6-({nr)5TcnII`)xEV;oN5zfmTQn8%KUz44Y|Ja`!0{`AfB?B+VttFL$1 zEXK`?^lf0T_Rp@*R_|@dzEUa(p7N3G^1OI2vgItK?)-wElpgUVpe7EA0zUu=J04B0tOU@G!ZS%;nD5eDE` zB#^C|SR7PS^ETT#pE1q<{RJ)(wKWw|A1}PCweC}HyEfN0PR~$yCjrwI`ldnV;V2|U3>huee_NbHgOPs#;UAY*=NW)}MVdqJqxpzPwuCfPS;H8B zD}1~l7X=J2Snjz1D9Oz2{egz6>pvp?-z0`pnuZ3+a}%?zNYtmbZ7;iH4OOQ;$}5Z8 zf!ecW1JRYgcPqcQZ4~_?^mSraWAj#uuw^K@CcdqNX^Y7D_V^qI({Ee*+DOq^RfRgjT*Xd~kVMbc`?`RvDsRik8O&KCP+M rI!$dyv(l13(WdX9M2E~DUK+5e*fWXKiK1fQad(ZB+`J|9*|H>DBCM}$ zsBfqbezAGo@^Ypl=$^0r5KkQPNaEArE7!HYq{fchJ@425UQ@)Bf+T<$iW(eB1r~hv zF#qNAFb_q(1LhESiidgr@x%O4ZWJ)5+7y16|L>Cjd;b62<)7pt|Mz${ky@>Ph++!M z;J{cqzzSBTC&Y;my`C8~5>U}-vN9|xXOJ`?g6+#)mS@de($qH_T zGe#NA z!Ozdn&CM++c(qD-Px?>Zhq%JjLzS#Gq=U8p#gN%0GD~Y{AgJ@04uxY?w}rKIe_bZc zg|53Il<$7}|3ebc7 zZRQ$g(9h6QRP)Iocx8+qX+JWGqw-yq?JNgV3nL$M&nV`8Q)DeI30QN=={BJjVmKXb%h%x^hMA4pHu3D?R91pVa!*PPLa6 zv-2eOf=b-PvUR_j+*-~c)jGd?p-omHy&`oDZ@55GtQ%|IJ~8~70f{RhSO1xEsjsV zIC*LNeTnR^*Iv+i=O?4_GK;i2O937M{pD-z)~lDO_uQ^Bk?ha?ci7wXtV= zmnjnz0%+HoPiM>GX%(~pP~4bq({lCCf6Dcw+}+m`jP_d+j?)GlkcwMeUd=~MIy|jFu{$5= zh(#IjkB@iH-bcB6Na2ZCZZZUkBaYUm9^EkV;SLSCReT@nijE45dss~EA338de6ugQ zm1(lYs^#j;#IfClDS0PG+=mQl4Q;k2_q=6x#yV(N?p3i6uaHzaVYLfw>CpMt4eU|HDcdKIaUU>m$GuGU)43+AP>yLBx{;dW+3Af2oly!?8 z4k#K5T51$M&F zc8fRfYej(`fmsKK%xJi%_(24rVauRjGno^3c_yTh6c2W<_VVI#B*?JEZK}!ud6ujs z`r-*@aU*=vb|ymTo+!54wtm}vz)M%zG(ol%LR?dM_;R)ARVwT?!o|}hoghga3q7jN`TRM!MNc90-RV~j~ww; zq#kabd66D;nGP**-WYlT`o+ych$Hp-xyGsCz}f!7PP<;EX5EZEZF2v*Yq9Ewo_5=M z-AYZ1J1&w>0Svh=HebE@(VBeoy_+RDdzv87?>8NnbZR&gRFYaYsb#d2aCI(?Ve_nR zf8ufGsu_6`eU~wsz;34SiHm2!^dKBaUOsgHZ7A9_l>OnbW{2h7?pmdcQQmIKL`#|) z8!xAK+*q)arV!{$6^%OcljB6(1a^>P?fFzy?Nt67M~>y{f-Zj^nGMx4my7r?h2^Id z{#s{_E>Jr6-79NE9VZ=Gin-rOJ+tfy} zxPSnTUm2R`ksuNM5&ity{?zEMlcw8H9Ce4P8GRyjx%>ye%PQoHS@nfU_?fHJO9V@c z1x&!r($O(Nl`OG0tTUfHSy{{5@oaZyb;fM&Y;A4rKrxpi3>wl+tmFN2k)qKC6BMfc!ghvcpzA}jnHrQwroiTau>-BNKM2l=F-&jG%CCch6}0{Mj0 zD6J!cydfG_Qwi{pSq3Ube-X@GO25h1E zyRk-xXQ1gg0xGVV0W+nMIE8dV+0QCPr6S*ox#lyEc4l#GeN)EL9tQDN$LQ#2oz=)0 zFwW1buM~QKU_y954mT$&>+@YQ!&hZ`b;i{FeaBLoxMcJQNK7HWqZ{ESjCnXZ_;^-G zOdiHTYyUJ340u^(aM>ewoH$VF_eb?jwl0&OyOj!N^v$}g+~Tq04^gKUln*;3y;fm# z@dRWZduBHBU*4-6A)&07FGe=O+UEFQzD{kd%`!R=YGq{ZJ#ABN0gZbk+v zC1pBO7O*^x4y$u#@L(;lW3+U1#gDD5tmKNvh_v)g<{DbO;+}*efN$zjN8et4TaE_^=o8T5KnDnW1w1o3@!bOyjb7fU*_Fo zgNkEBc(g8836hmyIbTv7#=o&-rw(*ON2@Y~pC6t!Jk~@BKim5^T!qpH;}a8=6ckQR zP9%#)wV8i6%?+wrXKnnp1>E9Y^7F6S%$9XCRaLB3s^44@yWCgC;wc?sf1mN;2S&E&O@G2ZY`=VK%7CIS>p2L@Po2X$ueu!W z{9x){9)wDLgqYeloK!^hDvGAVa+otTUYhSye~;i1G`#pR>S91Jb>N^N88>ND&iW83 zM(@re2tf986fa(6W@IeqlmecTv3`t|{b`7}@n9ODjP^-hT$yo0L7eCVgiTf>6s7mE zKVP_PE)QL9>DX&lO@Fmk&4@r$3Sfbt*l)#&pz0CZIM2nq{a;eoH(3s!e-sF6G3k|h zAC>etO64_DFCOfZu1b9{;3(nV=|lK$``>vQlA zrc2eDpOvo6;!h1Gi_3S$6y9lQcxeK0?cMD`5G788IW{+bG&jZeVzBmufA`#-raoxa zL$i>{NJw~k{)ht^NglmoNVk9}P8lHFj7u+TX-y(0Ior5xkJvM}k~j$%>_1nNQ4y#vgVU$@Ue+y!#d6DH32JT=xS6?s@^1 zh*;LIdWlQr7lA_sN(n{#PJ|9ZFB{g7WB!JYTw>(aT5pEr$UPpw^7|c>4+{VOIs~Dx zF$)`UQ1wf3#u>_81B2d*gOzboAhHrTajMYhoat=bPI2eXOo+a7j{w2YM$U5}V1^H+ z$38@p_RMQMzTR;6o0%EtV$RYaizPB-SrSiG0Ukjt`=@VwTjOR5MHELq2x z@;_c|#(n*8m}kUW?@p_+K&;D$R+gw*3n09p@+NvzCucS^q|&W?Qwi^QMOdUxJdy+R z{bTNYk8FtDZN-$R)U51)iov$`BB_P0_1$v(X^Vs`*c-49#c*pZ#Cr=$!lIT^sLuvE3HHb z5ZccddI!4xrgs9zb#bmWzvfchSf_7@3G|s$D5GDOK|eyHq2(zris2J=e$;;FLhgxP z?z2Lhr`N{DTT=4x7@RCFQ6C-?{C6N{3u%fNfx zGjjyQ-M~vrB9{RU_%!s8;1Se=UYc`WpY7>&cNUFWbCbOyB?Wve5_=bt z3k3os6vo}{SB4TcPC{M2t!SnRQLwy`SdR4)oI9pl(vh^GW`q-hQ^hpNLs-Ju>n+Jt zB}hu5l0UoYvWn15H|?W*9K^O%rg3grxM(&d$lhK>n> z>vG~+dk<&s>xuII#f1L~?SEp+taUv_8VuIrM~5#RP*PA-sWZ+sIvJfN5lMXj445Qz zk%c%m0xH2ukTGm&F_41e9wpX@{_0*p@K7GIP1?OhxV`^sn;0hKibyLUdGygQu8kQA zZ7WyH)pBS1K>cciqIHu)XK%ImCZ}<4e>ZIJfBG+oJ*b-H|4}&)Ha$pmZ{O_fY2IMD z*NhjVzo>NPnIz%h=*XHq41`k-a}5T4N$XGo2L}gE{ek-B*lttS^ucvkU`R=PN|pd~;1;!aePU}4qYoTEi&r4&rM2>FndP0rQX!Q3&6u0d3z(38^XBGe z)$)mmC^*pX%e8yZ ze5O)~cp^I$y&K_#~d3oG~!F<}53TI@{_1u!>mY02$_ZKpm-~|jc zrqmdRImcuTMxw!5^KjQz4K0BqAovdACi8%o*RIirJBMc7WVkNAkWt_F6guC;^@zWs zFL*e%kDBxnkyzrx5j@zAciW@c9qW>R726acz3SHBSU^qz7v~lm&+l z&IdJoZUZ|^NMVeHLN1Ou1pidE&Xh<-pn}nsJ($)8jB<8uVkeI1#s0SPy6o(caACjX8aYC|-%{*7RBU*b_X-W)BW0Fo~$ z0S2-r&6p)ZI)Vom7@03&;`RZR+7n^$NJOPRMSnC!=NI>0Xy+1nytMmSxOub%$F`%8=~T0O)4$d`^y8EmNIOLAOA1%-a>T9kgupoIWf z;%VGFz&wP0Q9?2egEb#Ravs$p#`I~YerFA=$o9$hRR=Vc5(iF9rT_V84<8gm` zPATH64qiFGo?`IgJw_8zvlkK(%dN-8`|)YG@Sieq3=PN%f+vDQ&m9LCBB|)!)#2=F z7tGXSuj;*-e`Y*0*D4(i1Rh=+0KA=exoH+L$(Rq2+-u(D7-YUy*9yzkEi)P=FdmR& zOsykQlLh6g)>w`gOF-4*rMslLzeI($drw$LzjXKz%Ogr= z$EPWEIr_%@G~tiJI>WG@0Reu{dCQVQ>3DM$-%Q8JA0?8g2a`iUqb(H)AUURP5NOI^ zi3JDfu{b-ITBgs$z%x!R+;C^jLS0FY&$TrP97s~_-#4yY$Bil4#O3O{nVGLipeojH z>?LWWh2eCocWD`;S?q$UcW3`~Fj@2=q<@C~H7B5&5x&#=!Aj@qyi(b?=zDWML5yw+A41W?mu@LIgVMW&{9g=-C+Zk#RGd3)A~Tm^mG=1f zW9Y|%ij}X=v3Cy3o+px(XTWqf?`M)uu-uEQ{{9DMb8`)PDp3YO)3?c~n;gQG;rU)2TY03RE%6X{O&Ux&rp;H<{lk?{sjk zuGjL2s8NMmXW>8HTKNi-y-4Puc`=&IQS`Q?kv8tEDe2<{YnC@ZNy_J#{vOK#u1v-?b%U~gLHwCE#wYE|dc(^LWh zabA62Vl_d}&U~Q>gLzS)X=c{ppx`rHIeOvBRO-&LpLz*d$|4#-liQC=-rn}UsMf5U zad&Z-KfFv)R!)pOwbo9QI+0XQjG9c#SWLocpp9#2s!>Eqg21etk9U#?>PDVe;=X0l zDnkwvWopRw+%6bY>a~OlU9YxPN-Q)c;J*$JhZeN?owu*EIQgt4$wWp*0)F|e(*GpM z1b?m;cWEO**!{k=E(h0uCO&TisD#K`l=XDFY`EH?IOn!pczt&Dp03#nc4w3K=EYhy1vy0) zeY|4I5dsG_gi>QLd1uy5Fp_FYU3(pqaPMkxkb0|f@2l7*%`oY4yIoQ3Xk&u_EE16D znUIR+9pG14Cq9^gB6d#Bv9+w4W7s$pKFWE(iW8xls=UpozUY0a-I*zX`tNxQw;@bu zZxqvtsDk}<(_^G|_=f-LMWP$jUXi{aTovah?IfdIEaTfQRCa6+G%249=a-l$hv2U& zhaIPvn;#mw+1zi@s2`dAF$xOUSd@_-!9#8wmryYRSo|a$4`czNPqDPR{3(BWg4p%A zR^>C8o0}q5P4P%krh+v6iD5Fl-PXF_*6I-S+SeDRFsg_Mc~A*qwhSY5q3I8VRnV!-wm(OGHkRS-Fv29QDHEcW+J?NMP!+_zwIj{~$`T?|f()QF!>{_Qr`{epI z{Xd6i|FZNUXUEXad@h~#9ML>mG@~XhJa)hsiV8Xx@v&wq{t3h|5fKjPlQFEr;&6%*#2T_E<~ zi}kzNY5k%qOc691Ysf7u?0>Y@?}{J?KXTsZcq9Qkh)YQD^kCVZQDG)t@Zt&5pEUDz zk0Rc5-`n8Dpq6ANd;VqYub`7M%h-^R$)wKm*#HK`ie-bK*Zj~kSFRtG_>itjVTcr- z`}1yRVFRZNF1qHt?@}j>CmoCOBQu_*&Q#>uL8{G;46KJD6XfVd>gV5>3RkD6_H5$ zr@v$#%Fn;F#)R01W`5yR8x}Sr>V3@h7z`!sWC7*3f25eJ&vCJ+9=X&Jj+JUrmwkp8 zlU!O7tYLUwYf!I5pU%{Au}7ek%OQcy>?4^Eq6&W3=IVm5zdvaV+xu{Tw}&k|#pf3R zEdXN75Xe!K*iAu4$Ng&=k(eL@!2F8)i+bE6e}~-tS7kSm{)<5CQm^{7^lYbw$-jmv zZUT407-c#MMhl0Sy8R;D6ur?ILE~`{1(i$3T>Zfv&>Qoq?Zy5?goWVKkVT4@JvEb=kSw897h5$n*JyIgxd}=F^`*>q**$cppgH_ zZE1w6Qt^C4?2ptFjtb{A<#YKD~r3b)hb+x4Xw}?%-t&-A+$CN>%$IkDW z8J%%%4j?=}YW0GZEPkyvvxWc(AK@uVE|&ngE95xT|D2=t-e}w0pTyjcL-MHH6N!Ms=4U03xF@kdX%<)*Wc}k+FWaeh zkEY*I8Xi_Y{Xbj)EKuG^uYgv36Y;Kk*a9-n;_FFmakwoUrX6v-yFG+IWyG3cp;e-7 zp?TQo6R4YYW#)54UB^Ck+V3|fQuPX90?;W8*{yev#ocRx=qR@JsVEe3pc)3=!M4KW8O*o_!_%k`}mbTUok z0NacFwX_h=tbY<(&`v$PV=`dRLF}Smu!|;~vAK{gw+5nMZjKPLW5h$`_nRIC8Qs^wGV%7Nq4Kx zjP?xGZ0E=%ua`$v{`pxLK$W301iR#$T!KFmBxcO{Pn9u^31!q(`{nxn8O0Orj!XWO*Ay*~SRBMY;) z|J#=AMEe75CH(Kc7;L9UtVi>+gLvWX=YZME8=vb zQ4!)0;XFol4lxq<29dn*Zaem=v^CTikr|1O?zch?Rb9<*L<`S$gMnyRy z+PRwV))hd?8Xka$^7}w9)n}f(R~?XRx`c7&_YK?mq;mQ$ePYj z9LJ#)J(ih}?-IG1!{(%OQv`*WhpkRcE?^w8uFAn--}9jxK^bj`}vXR1hY`(NJ0kD(qcty0c|Rr0vVZ{kr&^A#6!S{7TD z6yNCo%ew58M*L#UL~uzT1q(M<%~Y0W98-oyQ3PzGk2vrgf9dd;FLf~Dr%jRlO{1hh zIZe?4#z6g7At)r|wL8rrEUeF%dJ`x{9j?k4{;BbJM;*k^;+cTEqo^>|#P$T>^nAkW z^jo@KoMlbf7eRX`(o|6_^x;0myjStC#>S1;PLpMlce33Ra5U<#DU#A#)6APXevoD| zyA(oXLzI>Bo*zi5j-Fmnbibcn$2LhVBUTM-+i%ZhGWJmc7g{@(67}3qZ;sAere
1ehVItS#w{c1n6|zc(6dsue>A=5!f5{+3?XjBQ8!o*9~?|@pi-xa zSz%WBW`CUr3C+xWjG8gIzht_Yp8lDYb7{L_UKaIS>M;Zq?#7YLFk)V@gA!J<`0Pdz zFWm^36M&M##|OZ*+0S9~+OmGgPHbwRHb;l|4`DJ!A#?HW{PuUF!{6!oP8zd+lHJfv zwxPbLs(i =;URUdf7&(|YvwxOuW~a2jlA~2IY0qn?qsdMhCtZ|q_RnvD*EELO zP(YdI{()2iASXT)FP@O?n$%Wy|%-zosFKq#%a0>`UlKs zWl_7(=yaw++uylMI$)Ph$}sv@QvsGYo%aFdc}%`J*3>KStD_Oa8JS5OiFK}ijI|VW z9h`pLgu6P{@&9G!=x|)B7M-&F@|0VMgcjrqc>#suD%Eo9ENKRhCd}}>I1exAdIB@{Xp-rZF7rVpyi%wd21wH=A6_r3gz3t+@f5&ZP{9!^^5Fh z%WMc1Ob-jxv{jM~a1rWtQx`akC&`al>d~qgdihbss{OTJeEL8;M)bq$I14wDBfpEOY{RgzUoj*6_l`1V))$3&U+3X7`{VSc%~tmFOGW;Bmz zN38Fsv6&o1O209J!+DT*VUj10VDKq9zcYZnArJ_4#_S*oCz2Pk0tZx3I)iX#wdcJO=uH5SYj^*LHAS1VmSYiQ&Nf-ICe?tHM{`Jm3? z>B#Kn$nEFI%*D^i$1A`mq9&dUZxvoAJ)jbZEVZg%-yS=?_aM)>P#k$G8x4zH{ zr|K(NWxGFOLt9P|+MhVz6QqzgZK>~;QhTAd|LGDgD1{MuYTz5es9Sbz2~|gf zI=57a0QD8n_lCWKLqxqgg~o!&d!h)qoV!uE5ZCV(3VWKvm-K?fU*s;VGu1wf{9Qm&z3nn#-sjZFh6Fo{w?2Bg$x*P;z$m?UV`{S9J z8K9`KmZs;ZGwMPp>v=}kaNfg@DvcU1jp7U=xa>?iImyXHs&mZ%7{p_$qGti_1|z|? zKXrJR=(5x7gf8^XwxZiq!(!-9JZS#|S&q-le5o(t7;$FfD=2$o{bjkux^}B7l~dFx%H?%V%>=2U}ZP z2M3g7WHFMj6NQiZ0EF|Xr0OCZB<|$&3+0MG*m4#OF=no68z8e{TiDEI8UWnL;(=n@y+f7Hz3<1B_#V-U;r?wl;&n zRG{h%v$b7q5K+=RYt{Q#pO4UZ7Mhlh=(xY`NcTEuJw^g-pT)&RfaBsVe0Q;$xa9l; z5A+N1Ys=*a{(>g3qf#TA6BsK_L{1F$n2Kh?JSN!M2MMVZ#mgfFx*h{+c z#ii(*Ss5^bpH!gS zsQcb&N>TR$q?*5NzFJHi1n2web0f}eyB<#x*q*SA+YTHc-V_zlG*9gm&2gX?%|!=dDbRKv@XLn#`|r zA9>b5jOb;LbLM}fhMJor-(#6OoIHGF7N-m^^UXh&-WKXG%r{ zx(a*G*@0ugWvu5FNu;Hk>~)J?LQXl(R2+MJy_Vl;X*fRo@m%%zs-HgBAXsC_u*J(t zm`q`n_^sUBT$qy9oWQu5sLo2O|3=25`u(jW6bIenO^vAcSSpv5$Q0bxr3eQG#)pM0HX>92A*S}9^xT1XGGWRWF8 zgC0iQmfzkr{7}csJ&!VYkBM9DJHsQ_R@1Y!haQ1~xsKNr&<`xO`Stepx*_!6;Ooe) zDHfHK`iwZJogQ6t^^`^Bk15-{oVR6FoZ9zYU-<$0`FC_{Mi^4uGI?;!vaRB}`B+G; z-z|B3vG`fD<3-mEL*TL#Hg#^4m=lQ%e5Q1GUO;mg+%lw-3fYczJdJ21?Z2HMaeEQ= z+->*oX|&|)_6fgKOI?p-L$ehgXA0^!netDcJjoIE8DoE=;t;@f^hyRXh& zQqdVioeZ-c&y82H`UKm-<(*+KM*GLYU$Z4V3ToAhQ*I*xRxc_#CMs#fc%~KV^NDvY zEQ$GxdHKjvKn&fgP915N80Xq^6c3$a=56Rit)UM-HmnN&tm8QUf+s{h`f9E z4hWFAG^?xbw^_NqMdwDnRd|#dDY5st6&~C;fQP=6=7oKeXXkxWQ*&8N4W`c0yvMIp zJ?avvd?v~(%BQ8f?;U=HFKUDi3f6;8OCCeRM=K)_4z zw2ATuDL~JdvDeg?)Mnl|zh)@jM=~=SYC;}`%^r)}{Ax0An1t);9ScMZTQ(K)oeYqi z-(OeHY+74BwsBg?#s4ayZq|O7?KppKWYpl)d$w-%$MMrKYb%Cam?$&SftxI~=)``m zD%T^+&5$v*r>E!WvK0;whbWA5E4aysoN~5r8v77P6c!o$AcgVik38MWTQH+0M(|%Zv5e zHKCC8Yr~AUO++z1td4pxyB;?m7ii;v<>ch#=32x(hcc$dsIhqz!eY|j06HSu5?ARt zD(tn=?<7U7^k;l?F{DiPI|X6X)LL z)n071%Q0vihDPL%v6mTGYR-Y-ZsT!(+i%v>gaCisFz2YZ)%h;)bUQaXObZelUC@6d z-=GSg#wj2-c5M(&23RIE5`U)N*O+Eo7W2S&=SFQgM)9A$Sxwp~E%q#}A;QzL7Tx1< z97=u}X`-FB{=#PeW&(BGuk@ddE;I4*kaz>O>*8ZWgH8tn2@t(j=5RRt4R^IX;C3eF%-wIgL3<`I{I+SFZOF!F*?MW>)jdWX}3opYl=ohlql-^ce%6JRL9}7-ojWd67ABI)%hcIL6kZEk}GsxlNcZ`DvondDt5bdwm0XsH>*)WFF^0-fULt+Lh4B+4i~2pgsxvX=%&4O zgY{a$JX@Nj=kt+z$9C5Bx*`Ci1vuPaN@d*NW-Y(XmMAaG&MMZrp%kZ#{*2yUpC8OM zCUSUgPn3_BDm{DpbilGht8H83h@<0bvbw=(X>n=k87`Mo^ggc87c!qQOrA*9byKDW)`$kme$xL`*v+GXv3M&YYS_2l~s} zByZ%|vuAlCexA3V)~-)J$N>k z(ugc;nSGL$d>b_Fs&g4h5uM_=AR!Q#{iYs zaQ1L!4sC93J|ym!Usrn*2gj^eb-1%z19|&=NESo7+rkZ3%e2bX&9TyE8=5Vr>5+O< za&G17C)XI^pD^vr^T%N4vhOMiws}ML(x3@UNH7{SbS)p^!3^0o2tZ`sRCgd2M6k6!);ds2C804_j6PMORuk!>|>c__^b> zR3olD3rovL4$YR2`)$aJ*S{+1Fwb2W%+`!U$)lu zJTJ>cA_OC>6vKyvO%o0Uq z2Q7L%>tCkdR)^EAu}rEIUzP|VY%f*2gk0A?TIl$l?E?Kc03YsOvjNW~ReFz4`p>rr z{imtGr~MwWm4iESxh=JwtIDQEcDXGr#JyTiD|VDyRIMMz{G2@bzO*{#TK|=R>m!hC z@z~0bsDBAyBlBRx9v#g4kI>R3k}NPyZn;&?f^4pVcg2VjlSh4)7BXkCA<(aMce`Zg zFs1Fi-;$nuWBc+r&46V==r0K27TAm^|NREsX02wLLx`1fqlnXB}tOyk@wsCx?CTet)mn5k>%H)+uE9?Y>vX zoB<~hv`-C{mRDkvbIj(yf{TO!txDH>jZW4m5w?h^2jPiow(M$PfsF3P($zIIWZw#8 zbrpTvh@ev;X?l{QH3w^)uxHM3vm(r`8*gyX&{*iY{*EQOtdjdJ7gYsBwlKDY#|KTT zPkS<2ucMXxP0a)jIEfZFr31{Yc?e&^9W8E*8kefza9viRA6N^s6-)@suH+GmxS57M zfRriBke#COMp!$eN*>OXJuF?Mf(m}1NQp33Y8WiE!2Y7zZ-=Zzfu@<-jTTbm50j}8 zI>2_YQdWpi3Zwp}@pT9Y^Abl(0t81EGf3_AtVgabz-u)}D|BX8s! zkQ-EEQYq+aoMh7HrIL5k)e~L3?ntesWK&cA^=i8Q!ZbmfyC`+#b6T@bY@EVDgtOP*~!Dz_NjGMa|KF*Nif`h7AJ#GV6Kt zd&{9_YdgmjTX+?+*7NHZKuUREth!%P-_sBlkvgoE^|)fvwm`WLb-W+7;5x_xvs!Q) z5#d{KPlm(K=iNqp1r9E${|QsW3o(QI@j$x(T`XgX(VH2)!5`%wrV*Z@88zegN`|%8 zJIsmX`+0aMmZz1MmjmS9?HQ{)yr>@l?bx+^+JgwT>#ZZiys!Y{6A@tv2bK|Z?iEBg zZh~PjNmeC+bQ5a=Xs^Vd;vRyKk)Rl$^UxJ{=TEUf^h4Ds`vYy{<2d`0;aL4Or5dB zM$paUbL!VKwBJNS`oz=Aiv%YE=qHc2%Q!YZwB16=WaQ*L z!3<+zVX4(CsrwmH>W)ZR6kZB#9VBHUj!h*4+A(+2wpuAm=^01*YjO0=P!5!C3`6#v8e;vU8qNR&!w7>MPQ{~*h=!?*{NYWht2K0cx=oUUizB&@#u**67Z^Sk% zS07*LZlb|qM&?`p`K}Aa7n6=*RLx(&=K+$pBwEb+i7x-F`TuYMME{jDnD~Hwt!A0~ zs`c_zQy1l4wH$xhCnNf2=D7X5ZRB=*s&#|s)vH&kA^WMv1M99p7g4J0IvJ38%$I3a zfc~yL3uO*Aw*Eqif36N#X_OYMX>9ng!|ust7-YS0`-M;`<8l6N>J?guLTdSu*kMq(NDm3^KP{kd++EiT|KyJ0Ks)WoA;b^m@@D(FfuY?CawTlm%5Vi zk8<8Jv|=7sxy&Z}nWIQ#JW6bQRde%?VBc(gOW+;|ak1WK=1H1if*QJLB{kz!B53&DL%Ynu% za0xG)Z`OVe-e7GwWNZe&m3-!0k5c?afi`1XyNvo-y#Z&uHPlLIPNRTBlbDD3Lz5^=Zc$ zK`YjD+(+m@afe%>anhDgSYw!Wp#eBI-wyZHKzYy#>P^HT^CCyj`yV^Y#qj}7esgnc zJ?Fsg%wqSaeKuMZMAc3O=rHP+A7>FRs-k|jE#>Z!ql(A30QetJlFYp{Sr3Zmf?))j z?wG%ae5R!cx+C+qdC|}BHuzdR_d>g=^E-L2pc5V|8yLZSZCeVit48;yM!DQ|V}e%6 zxx5v?8M3LRXsY}@?GMWZ&Nm?rkD1VU52L!)xk&}mhXVELdxyw3vQT(S-%TP=b?Vrg zeGJ;)CqkZ9ok8a>INM$y8#LW4{*C18i+2h<&IUnnB7pu5dP3V0j zHU8x{Y>^%i^$f$~15Mp9(~`OxTI>Tr#6kB$pyf@K8Ig12c=T*6i)hVvzuYQn7U0#v z6ejw!(^#OOW4<67zI^}&uBxhn!C=j^$Cv&{=TKTVSWfLurWFja4X^mkv&WLXw}!N> zXB3OobCo0F%5ABI>zv0r-uZd$wp^VF8%!;ox6`#BrB3`=z+z)&6YVgMHhrh;L1NnC zE!;j)@1(=ToUOuI3!wixR{JtyS4nkoMpO`IzKctL(Q|ay_-8=x0rQ}%rFJ`bR&P7J zZ^di>Y>>7hi^-u!uR1XM|D)-;!?FCozpSi~m1IO#$llp|K7_2w3fX%T*&!=?XD8V! zd#_}Zm6es1y|RAC=X?EJSN~Pd-|u_E6!(!9x*phmZS3*j)$|zGBRne# zIYU|~x!NdM?y6}zdeZHlsrnvYZ6;uSbv4Bnn2J#5;%|xb&t4rQ(V#Hf?F$f4evn_xfV+njAOSdPc(th}!UrzaiDr?8fHPM6Q^`iy-{FHX-4S;sI+k-h%~em(-faFX zs!Eu4=9~}gv9(V5x6vk$pl+0$_qfX5K6x)*(A?qWCoj<|3e}Xa7RByuFYE^#18yTh ziC!OqGYP;-`c^KFETXHv;1-YV4{kN*E`}d~K*m!la-@7_y`_5XWQb+5{d8>?@eS9rsLx%7+&>yu%{?@;b)O*EA$j*&$IO`cq| z)IhxzGpwl@d+BiZ&4gXe1tEszJ9qAY#LH%?yfZCrg{^;Vj40O$6h?{^#`X)Ab6%uZ z_=mqt2HNq2r7)s9l^!#=*M3bou&6o#yZMd9;VVOspEvtBq{JJRXzHRRGi#hkdrVT(}alIMfcUJ5L29u?4y6m+qSBdMb z)%M++-%675Vu{MvvHMMVdwK^)#!|)f)V~$=_tx1rCE7Zd+2*dsD5B|L zPgc*)&c2mzlA;cHi83fW^__|AKr?MOnQtD;B1J{?(y2;!dvlWuM-roz^Z^+e85Pxh zjqSXGtS^gU(|9l5C43?;9o{dW<`1qB20~vPwfJY@+D(UAY;I1kmL~G`({4vx=a_l2 zWZOHh=M^$&0@pNn@F*LMwc5Ny*dzf@cQ>6rG_!s6d`(ErnsXU~Da^PnNM-oR2i6W^ zVq$=5h#djr`xX(uv;G`+KU_RxCrveP_f#~=&e< zK#1H(UPpybEDc3)UC@HDGA#O`#c)9(?2+*D8c%$X`d+Zl*(%@j=g(hR^Gaa2AAK$q zTr18qr(CQJ3kT2581y2!2dVdal5xCj4tR>+6zlr-!Hy^!EK6Hf6Hvh_dC6KL;f8qb znA@qO9qFB7v#5+ffU>eySV#y23rJ!&V16|XbhN+{5rza@j2}MqX}+}(ZYy-=Pqp>c z{g_FxmSs~E^?C419New>>j8<@_4UpC-}v>2fblRueR+6M%TGnD%WZ|t4CV2 z_|9IvM^53M-&kglOjn;V{xFW4*o4u8W)4v@p6<=WJgcvIq(&@tW}L7G%fCV$RajgX z>L?;`^YeJ3+Tg?E(2zF!`Qv<*3=yx>(wGCGVe1098jS_Z zE-z-a1*YYR4`;vH>NTiKbc>)JRx4Px`E1$cZnNF%+7$E!{?bPIu(N@|C$$262^kp~ zy}eK^q!u_kJUl(M1Y?ce+)i73QQ}Q_dffk--?OP*xr=n#$)@)Wu?po=iiUH_Zxq4m zn2VX_%NcNM>AITRnp4z}((W_KKVAu|cm0VwHdAFe0je(Y40rCIpSaf4d_jm=+7^*! zY0}h%ckn^7`ET7O>wZEXX$+lmU-_?lyC$Q0{!Pi7v$l@CWmtcs-X!N;Xhh2yTUo(v z1btNF$y@@UZ2cJ&XUxuk0%Kf6tOQ7B|J^@9-t@%=W^l1uB5sQIMw}dN+dxzGSbDy zKEcGq3`tmcn^2`wUwnLizC>+1|4Uixqlx0c+pFDZ6Z!b!zi&l%PIP*)mtr!6zN!@f zTwNTcQdaTP6SJ}~8?WaUr8YaMQ|-%(b86q1<_<>-9MrPln3q8KB+i%9qV7}q&x-{v zFP&7!*UC6j_7?OqXKQ?QKv^*1yzg))!C!c!P{;7m`q`fK-(R&c*?K%=;Slt(c?8x* zE&aDV5`X-#SsCRQ;riO!+w1C*sh`DNxpS&W5aW^hy0v_ZLDXGaYWMN7x@t}Av)fdq zbLPRhbK5oZtknv3r0onD507^x_CT(`_@%Ho`Cb}>Tx%x{)%;qo`~-{bhnIBmP&^e7 zUAQ7Jsu_>@_4?1G(|$;bhlKnEpdefZT<-m*DapzEXI>bYK#s%-TFvd8)X&^EN3P9)sbbrnr(bzo|2XZ?&#H$b?ptzHoTH8zeyBN06jse z-X;I)@t4I=k$3G90{j4!#V1SIF{L#@Eiy&e~e!28V zHdlp4t1PAmk7$;x<>rqcA6g9(lar_GU3cJm-)&(^A_c+bFz zpXH%nZ<3!?%=%iaotpzbW4{FLqXjJ@?^LILG#9H?2LYknNVXk*KGYZi0hFP8qcy$r zEUHA~Gcz;O(}E%*e^1WuN)&1qF|8w+R7^n=U^6QiZ_>WVE<$4TV@Xc_>rrE zd!GLKGaB+O@S>M#!!`$|Mdjq=MDYhE0pRet!)E`b6DZKf7}G}j5Xfx`UE$}2vCZi- z63w}Nt5fJ zA?ntDLINU#`(lhSE)D?(&-SDHyA19xKVAFcD`<6k2jlzG2v5Q*DI+{jp0xz)l~dlc z+XFYC+ttsXKSM$=o9BHF{|tjL=VJuXsWq%Q(x651v@BUoJO6gRcoVj73y~~+l!Awt z@Ws|PVo99;MfaglYI7$2YH&Z8oJ`a!$w*D@;6l(<#!hTj&1^@Vwwy8dq9ZEIIno$N z%3>lVubAIIv822EEZIwY5tCk6GtPz7?OQ=!fL4qnO5am&o;hUWlHF@5h-F8?6d8wIs#YhC1 z9)7J0KZ{<>+SC{zGmPa+#-ir4?WYi-9D{Zc$xE$poLqIU_W8#cb$aYGZ>^B0xl~GX z@0u74fd0rBjJc})p46@KWr64;9k5NDn7@cqPMO|j;}|`E6ds689oY?e6JIr%l}F(x zh0oT;%8KP^_vq-z+1WWd)~`ixnB`H#z|s&xV#Jj5e&INl&{OXVGBgIPwfO2%No>8( zBF!z57~v5S(^kCUG8Go%u!jWr`Au>h}E4X@;vt2K~CG5HF}ZSy-6dfc_s*|iD!+;q;`BHBfN_14jr&?hyj)fyFQ zHY*p73knF-dYw5;dRb3b7UYE*|t)y-=DRH6V1U9HW+tW^xah-TsVqTQrbHF%#l+pyVwtT+HtmS?^-7?3n> zQANh;C~~YF4_nsfUscK5?Z5RqSECtMs0Hbyo~PBM8%)9aaSdsw-UMzeeR>YPbkFw< zvF7k7pJJBe8IWODyPs%la`GPItU?4)O?7p0ziFHTkFYSPByGYv)i&&va@C@C+fKy3 zm(VXdEf8H;EnD~8U0G5dhFwo`*BNU&XZv(-NvvKXQ|y_BqGDLT=>t+yx%|-wyjBk{ zg8|PalW%XM`9s-(-BwuAcWrALQV&bY|$gVtT{ zR7@{BzK5cAz_`Ga8gn(_rYD&8bKOZcl5%@%D>W%er^*6oa3&sSx{N{D>}U0}5eH#C zA0s$Ey{fsPJ25{yo-5^7# z=3l>l#9)QaEE4Y_hD^?ZOz;Z`bs!Yxo8A&h?+K@pZxP=)?XQoHioa$)bn0pqitIj^ zf>cWb9=4y2x)BdzY-?Xe7?Q5PPfL`)ov(VtrJ3WD-o3;@7Oq+NJXh_^t7%|h;A0+| z?}~#!xD3ShETSw4QOPY4dDi>Q=I+%nQuO-!Sk%nWN#*znqB$QiZw>jmn(DAm`))(A z4jOz!K4D2DxANxEKP%o8L_T%8?8y6=t8TpXGM0MS{Xk#G>?3wv3f{gT>6?*fYVZ=d zP8#%*`CviNowO7c`*>eFp7_uL>mNIsI&Satx6jB*Fu2&r!qtd)czLTKd?O?zB#gBV zqb@GN0-y|bViN49s-Z@1geiqB=GyN91Sb|}rmTJ%u(K%m*mn|9nO2$C&{k1&!>ON! z&DIc=XAZlpFBkZx!f_&LajVEkVMMb+gTU2^&-KZSO?|k@6ci@TgXzLXl!*OfiJ%kI z#x&_mJ@RhTg4gEqm+$amy-=}acI@g+=D_=-0_}*kCfOeXl zEW9)wUNeVij&2b>;k-Q6!?7IE>x%;*KyJ>?43CWhNF~$G%Po_wj#>>q`64bdfW=C& zyJT#&-;Dz;f)7E=L*(8SvYPwMUp9IS(m$ybD_k%KyD!=$8YcSs`a%jn14QNUYUnSbD8B)Sc(-RjlGhlEBU31_3Xv9VjXN@x8{p*3%kYg-BUgfyjs9 zPb5e7YI~wRF7#HR=3gZYl%W3gQH-NAT*tKykx5Z~GUsUv0%2Hg3Ph)Bcs5crcf&eq zR_CNJ?%?6Ey{O)TO8y@pK(5d&t_uh~icS`FMwkZ<=`@opY@TqI46gttHA-;Poh zbtuxrzPbYh^}^%ILQB3?BZ@g)6w1lyN3!@5S}*OP=U?!7IR6F^F`O#JyN1ET(PkwzsmGsSeM!Unv<$MWLfUT2P@ge_F1l zxB591?chy%t56!E#{FPbB9k5VfydFtz8)MVkf_JY%S+-s`~Ady^}wB6b2&-{jtXwF zAVa9oTMeT0(J~Vy>dQGk4cSm{-|&pkh3*yKn@D_(dz0mChceU0{vm)XN&CmUWORJg$hXASL( zs--zY#jL9NLx`sB`DQBWO1O`Oueul<>ux@KxN^uqEK#& z{n+F*LYd?0Ia?ti=A8jzrD>M>*E~Kw^#Kcnsoo6pxsR#5O)#3&-fXA0Hoy(|C;)N| zQ&Lj;p7KD)tV~bHikX04G13-+C()4qXz*E)X4u*&bBnrJ{*aA4GTw}3Nn=oN@Y|2O zj*tuYO#oY`uzOzVO@&SxBG|l@n#%0)$fA6nhr+K!*;DGhX9`|Po$@w}Y}Hz0W%8P_ z3CWdeYPhm1lmj0_`4|IKZEbfTdGW>E{recGP@zIynTq8#Zz_M=Z)dIpGimoeH<>b1 zoL};Z0sH41OQy=gu8oqRV)S0Xo|+)psSXyM@j~wea@r=RLsz`2%|W+oj$16!s)a9O+&x-l9iL~*^jBta+WU2U@yNK6F~tqlML>Qs0LaUZ|l^D+Z_#QexDau`YuKS zKa!_7c)8KFmU!Gxq2jRh@@m*(0O)U&d-)D_&>nwA4s8aT4uxOL*99Co`WV2;6?TZl zr_HN$g;EmoAf6Jb$l}oB5`Zg2zNMKN{tK!E%3~7k$;Xv1kXES!po|d-7=rJHj{Uyh z6eXC!CpCRx1JpV*V`Ckzwj^}=1s9KUX;-P3a&vq~N5d+zzH4}r5A0ofljnw=%icS( zCLw0e_24PIBNTuihsf{qH04d%USFRoHAEp$DO${V`F+@hoTTk;Fc;<^r7>yVm@nQN z|M2keRzqJdQjccwZjS(bZqn6w>gw;H%?+mVc7vbG ze@A}Sji~zgb&J@m@(NLrPHx{5-o|P0M72E1m6o@_3+d(`PKq<|gZ(yddY@B96RRH09B zNyYT;=6|yz`6u>p!Aa0?4u5`=QC!4)ieWE*e_!A%XOUZy^-(oL;=H&-+Mp0eLf#>n zUQlTv@9x$22DQQq(jaI;(EY_}Y|%mjXjbye3{L1JFciTOTg0umIN8$jPOqf9*@8RS zOGIVVi75_wt5rbZ84-WzB7boEqBaZf4)amgJI7~mLfN$9B$JU;fdK{8<`-BgQh2S~ z5ZY`n@U}xpjjhie!9wy{=ciE_X#Mcr2~7O8H{za^1@q9E&i(pD9v>I#`^srp8p;Q9 z;s|`?Hb7F$o<%Y|TL_lUeiopr^=7SQ!QisEfrn;`X!`k}p4VQd2GjEqb?>u2{kCwb zi-p^?Yz*}D2K-O7A%d~Y2rcKWf`N*k*N8i-QMro9h1|5^YKVMemW7J$M&>9TmK%D> zC#?#zIR*D3Z4z`h9}`M1f;5EIAtpZl#>NJKBT!D+)GYPKN|A+q3RXujlW^LSQs<1W z!>Q6HUxX$U?@&|~Y?4#|Z!4<;B^m$#PK-c}k5zTST7_|G!@25z~rZcjU?y5I}(8Y=H`lMi7{%PKgK= zYJD5*#lSEy2SFbYATJCNI`%F|q;5${GP!Ddd>nQL=E*_XIKKk^Ukl)$+HWfUm4N^= zc$N|?*pl3BKfx`~MX~FXfDv8E?#%p^=I@f4S(llLK@sd{NCL!YM$1bz95L~4Q~$Yg z%+tG*+j%g#i=7&f5c(LAw?$G@q!_g@SK*e>8rC4~W&^+=M5#hcU|Mk>tM;_}vnpf9 zl6_2X&afE^N#CzuzyADTym)+*T8as6W5pB>I!bYF%+(LN+_IJjVi+J?>n!Wtv-n7Z zS4_oke^(;4&~M4Hr8dousr@Tw$~akw46{yPVfVXE0vD`pl8NHF4JK9I=a+1*zwEoY z28FUm*ZH@^e>_L6+y zd-G&_fOo;S8#(QMaPh}ksMUd);imE=;6Z;sL9AOEo}@I@oiz8(nbN(W&81pn43o9l zuP1ufn&jxschRx1mGKKRcW$z9h)V-8l2KWJ1a`&N?E(wzCpRilt( z159*cdSm5iPfI%4aoX8E8t2To+X^II!s6{1?*;0MWN5wXzLg%}?`KH5YXJuYxK1;G z9{H%q8q#S5W>PA-=rV6)e=^E`{z`<0-Dwb~|KAk=JPP0yST5U~5At?H zdKfZNzqoMNG9X@dGgF^68!HGSiVHgKf?g`s&#A0-&Edi^P9s|@8dwQiJG)ZI9jK|{ zBMGajtSn`D5F%xuq(T$jV_LDxQ(04^PE-YTP!Y7&7VR`;85&Gzb*lNJhWjD4d?u9Q z8G{U5I%ig;*sR}>sHH-MYG%OAU|<_PxJLbqkh;=xg0)d^etH@Ug|P{+Z-~%?x*e&Q z2KQ`tyL0>3tC8=;LpHrL$Qj}KkhB4^hwJD0ql-d!HNIMhe!?fB&2Zw(Unt2LK$%~< z-^VAtI4kxHkONUR8Ie|tL#zYM)rmr#5ezg#xCTK(r$QY744FW3UUyfQP2NE@x->T%AFI235Kh+(hq$%g_yylt8Xa8zbK%NZq}VN#Oa>q(TP-7dgcX zD1G{9NN%MRU9W1vXn)JP3mo^^!;HpxKqAh%ErsiqoPbC&J79HGP2>1*{JFY&BKD5d zt3#U0iu-P#|3wQm&AvUEy%8+4A4h%AG&GuAdZap(@py3?b@i4rK?thsT8MHoabfww z*mT&skoV*Ci-x%Gg~N&_h6O|#K-{Q<$x>QBw&QuG>S@XKKen2gMvL(;kg6!eb)d&M0P(R!4?TI zJb?)U-g{yCc4NHjVTibySexDo$tT-Jbaf4)>Bm9%p!#V#>L64h!a^&PdBV9yD~Eb*<@qu;MM z?tZbF@8hCG>ZjU+VSpGbaQ^DJ23kE)rCsM;NAY7! z3zb$BGTh+Dp@Np1LlBSJX~dDzZ`6+8qfQmNjW5Bi+%6+kGI86#bzlJTHNDj&ZE(%M z^`Y{pX5kUY$RG)Nvf0O{pE95#)CFtTq8n1uDHF#T2tAGNr~946OcI8zMgM?1+JJrc zZOXToDy0f|dmh32moD!ooRzE&Yg47`KbE;;FF#2*eIZ1d&xmC@)|#kkHramWmT~@T z?dB?$I1nsDB=R#vk>8SDHNqUByjs%17V>tWmtBT>@_oKU5b>PS%@gLokUyTGRq2+a zHR2}A9)`}z+Sc}ZKcQI&=8vq2UhDi=FI#>Ftr7>vmbla>kID9`HF$(Ot>RQ6A1J-+ z=|YO9{J4G!&y9T z3amidp5S_El-&g_+%tZf$glS_-knrrT!J>C1yhQhnxr_G;;GAmfcUx-8QDloJH}%5 zloc@66%`e=F9F)-fW9%3>H=r*;*q~ z!n}3)utGig(`$8h-#~I|jUX;{Z=QcyDeMQNK>#~xGmBLa_a}9oX==we9#329#J}C^ zya&;}sfhf5)kpQd3k~iF$pI-E`=2Dsn8_#qn!& zXeerM4H~`H#i^e^T}GApAnQ={#aC~>H`Loi3*$|*5SNOP2|{g&h-t~+0&EVY!7He} zlown{N)o(OfBE`*-`|b(kQPogxF~RoQ*0y)$Ur(6S+5*6>u8o3zt!Ojxm$mPtS++c z`gtZwSRG{jQoGHl|W;M{bB06-d4 z-fpO~^!)`m&cEWlC7d0ZlZ9fB|(v-(s>Vzt=Y#lnAyUs%wKX7SEM9 zt#X!g$m8={C459x@MMgL2+`6zwG)Yie70lQ(Qs@p+7-?qy+lmWp5O0O~* zv8rFQbKlP|p0wiSUt09$L}yKW-&sqz7U?dOE+`x%Ew1)ReSvskJfRKc#x4q}2~AK; zvgkMc+IG<*_Iz)~5Pq5Kg?GQnf#VpzQG*FkTvu23rkRPB zS-$pjU)f_CohC1Vi*@*_%`jbw?7;&HTv>NDm7xsz;^I$?{18i~7T9iBeg|Vsm;QDE zEC#PF`48U;M#tisIR{NElvPxorXl`AlRZ5=;Gd@<7<_({z*+~) zg~`188U@FGt2RyQ`!d>XxN-cdla{}UJJr$NH4EMmaNnoxzEqqG(g#D8bdcz{1rND4 z=2%EUqf#d!F^QFZ!a*d!ahkllr;h5jO=qST@A5Oe$*2zj1+0O}N@$B(?;i05QMu_m z6_Jj<`_x)E_+HD}Gy;AqU(buVD;0`l^qsotCEh#K&$1?Ljuoim2DjVPJitMh^ZBp( z_7;_?(!Fz6WPhe@k&d%W-N?1{bvJIj2_BUQIS%@)4d2uTT%mG&T!of14eneyWHFBm zmXm^YI!}0`PK-J4|5CdIE`%2ElWq|(HzFA&6&31VGyD>#B{AOev#Xwm)k{3>{9@s! z-iF42BXl?=tfqN3vcNlA%*_Do)KE)_C20venwN;f*{YiwD5nqEqx<=>)gzKxv| z?oj)3E5n(BK4-!d*LE93X`oRa@=8^S=2-tg4I`x!+sK~%@-8lLO%8tD~`!vLxnwhYFynDi+ zWM~rEq8W&MZk9iJfgIUzTPF15B5d;0?p<}71_p}8r7$4`0}<{Ss=4eH)*ihGiy}?N z5Bbtu&1|;f9zcEf+{iV^D1)&78ff2vjSn1Ws{vAfw?H}YLLiZ@96#W)VuH$g^#`W?_k)=z}(pvn=FtEKSs1WNFx^e@`&_6856ZesND&2cRLLack>@;4flO^&?2 z*_ige*`IFu`$Rt6o%(aiLx3#b?a z{m;w4GsB&s)!YT0iYH-dLceM6&{K&&W!;pL7&po=)`T$H46lXgXXxU#_*aBBjFAJW zUAUqG2MTC=^fDqG)4k6;tJwA-`dI3X^eL%FBPIG2ZA>pvE5NSWCG0(af^hKdyeEpj z=7NQ___{k{%Bs4yRv@Jx*zD>X~UqVaP(} z^YqnX52FCTnzFLAgwYs~r-2bQxb74hJ|AO1c^jQPYJoh)j61ZQgmz7?pxfN!rHot& zZELW_(+?+FtMV7&EL?n~H~>IoeKzI>orn0dNnVOPTb7_0gplhO{id=D>Tm@Qylvv=+d2$WbgD2Y6Dw zU<}&y{gu8WkH(2j&<#4)&+?{(6aJ9Ic&?Xa_j3esOYg~p-kkMy^Rl9~8$T0sNrM{h zb}3W}z^?N0{u~}|b#Da}j+FWkx`J{TRK%Szv(2ho*Bve5M~OI%0WL13e?fHV8@K!^ zX7Pr}?SqYh@W-7(;_`h78Db*gGK68BfNQrp)00Yd%ClCgi=);`^%J1$1h$ED@*++tXEG4M+2Dg-a4%EJPjz9;kt}u&j&& zs()_o4kM1_yjQGREF{?fId4>xIk~t{X7zR_o@Mp`RIaUjpEYOc>+13)ITtAo*b-eq zGq|}KlSgru57U*(w9#+hcC7vN?`Tt?D<-hQb|m2qrl20JNcs2Zfsje2H*LW_TDZ(>z%uA#V2(k_grv~(l8L9}xIr*p%pCBJ{87M?p!#~5 z&C`8w8iJkA}%Cwa-(~ukLWYV;Shjs?w8CvjNTv)lgu-1JP@ZZ)28%5CD5#uhwA& z=<3@)M$Nc^)SF8rUx+^OzgMKn*ut+QE}Gw=i21SPv!}We?s#C&`FnSGVdd{S%AfE{ z;`QK}b0&R|c})}jUM3r)1r}Av*tsUQnX1}fTEGIn?|XYx3=X9LIgnv+lf5c4l7<<5 zGVty=aZm!X&HuqT!{9fqCuqKP4@)@c`^q8h`#DYYy8)KvA9ouHVDghf{wRd&)V{O< z;r25`)5pZjo~NC(janjTDX%6-8_8Avp6^vvtJ9Fx|@$I#Z}@ny>zCj+o%= zPJpVHofMZ+D6wyawP9C+(TPMuH#l{^LdVZ*-O0ng+fHlH zigzaH&VyyqCMesKl|cXnu@Y9io^UbNzbuBMD&Az<#S@Hrv;?qH^32sj0XFb&ZDGF| zJN5=0dFu$^aowU+TPzF2C+Ub@V^9^q7cJ8UmB!KG@y{*l=Vq=h(^;)PB;pgS)>H|-737?yZvKDND%_sSZ#=wES&mJ1MY9HMAv?~bIJ|7H&d5nWpE=^OaC zwF3|X6;@q_aby0Wu790TOQ!JGa;J_LoEY0xRm-jjcna*wRb~P(@QnTbdMulBT=vP)( zO&uu3xXC~XIg}E$G#6{2Hq|O5Pyr(CvJm{NIj@6$pkl9&CiMAI`pNhA7&M89y|fQF za0nrf3f&)(pKVU&nE@f0z_TVy9@TnZdI)vP=Cz{wL(w8i5b{p(zQvANvWVRROk}Fx z|IQ3A2kr~%3)3>~o;CQkKuL?H6lrb>tfjsN+6Bw(Q+J|nQBW{ci+~~PfXu5>U5SjB z+#l6;z`>ZERP(F$@a$~S#3prN0~wNLfktV;CAi%%dPhL(b@$MvoFzq$b~g*Umz^S= ziY^#dp+x^qe^~h%jG)1`{#TD)q;)RYhsyxVc9oMe#!Q5zTf`fufQEb{N611vEh`FA zvex4Mpygk_lO5g;yd@yvWFcxIMQdaSm;Lhht$y8D*i2& zwee6*yehb?yWGmh2L^M*tXJN;=AWev-Drl|7?ejqwt3fyv6F)KXnBi0@$2uCb3@Af zO3#=`;j;o}2i}t^nbzh|lbGI0;I)&I?l;fFjaH@mgtodRNCH8De$BzH3?dSks0N}& zka~5z)1ur)a*LK*zG*8fkX?!VV+oVE|5^Q4zB6E~7Ht$@k-AH_!*}0qV*tMa4eQ*#rFOmP2y7Ee?^QDucVbyEh@- zW9lkY z2B2>=t}?dw=;@CGk4D?(%j%9mg;6Kolt7X>Kw4AV97D4fLbDjLG7fj&ieE9j2n8rO zd-#2uVIa^N7DHl9Q73<#>OiIhVi*jkC%-*s;C)GaE-Bf8>^085a(FuAw>{sHdli8` z3$f^6*9E@^YUZzE2%NT5@^Tq#e zTJh=llXZ$rcJJB&PP^9%v;JZ;S*!#)@+}9W_p>&^HpYHfk!)AHkM237DE*d#l6RzQ za1f-`5U>hrVqQ`l?5&u46*nRcjLbLb?&uCG{Ka8Q){8tlN%Gk~LxJYm%=Rgd9izSo#IBC-SVNkB+U^ z-ToEcT z?ks&b>d~qyVuIK+wnDXnubP&$1irNr+66#MC=H=Kv0k+MwslvcxkZFzN)^rs?Pu-2 zC|=>8HNHTQ6P#`LkllFB2RP3D$NIP9iGC=MG{Vbw1*i|MagZZgO_jr8pN6=7_ol;z z+YdLmZ^cimYx{MM;$KSd(?#HvJ1nZOjPQ;}fy4(tT}po_1>EFe=M?d4yI)PFWa$Vn zh00k0QxM_?u}^>h)V9mE`O*jRwQqbd<`g^t$d{VXw$QKIMa8>XR0+l@ETR^HX%G&W z|tCMU;K}Y7hiQ?wN?m5Hn0M9?&x^%J&u`&tpCRkB_T>ze)1|R*4E|oZl4R; zfFalB^H&ZZk`;c*p-M2Q1h?O!_OkOPxzpkUu_>p}!66qckQc}r;c1d4=h@4lkzGj4 zq5o9~4cORSt|*e(Jdk667}xK{oFKvbrQ~~(#B!$%nlthy66!}yb??W1F370*IGO-HpU*($uUQ^PgTrvTaEb{gAM)Yd{$C5A5Vm4r_qX6i5;(@MmCnuBlWjh2R3;{QOYYSo+g_pOK>r2? zQGP~7xu>*hpC8`+ab7)CVn}&rEXeC~({vcmz@-eS#(_T0^|Ph^IgH=!rs2SwpPb|e zFib>545hu|y#^OC_b(7}jU%C;Q=!;D1%1;!V)Cjlfr+ zC4kGAz0beF$HKzG#RYD2Se*}P@_C-RZ+x4z`Lm>VIIqJ(f>V1>&+S&44e_fluhnT1 z8RWz{_KB+=+^qvTGqktV=U=`L6A{(}KAVn7fS-*;u$4!L#9cVw3`f*B4o0wb_7d)!D*^+yXEL9u0`rDl4=1@)80nDGC7ZakMjhi9w-*9}sDQ z5-t9Q2ACG4J^2goAIk0uA5XqE{vD`&ijSj*f0z0J=7}lAt;jy1 z)H;+#l>fvc6Prh+Pr>es&!PK;29G}E-dj?ebmTiNA7maOsQn3%lj|R-fZP(CA_4Ah zc5b~)&YaGpMrA$nZ9(_l?7)`y=g1?aU(+exZ{bdaPnE{j;}&%*?`_EOa^p>bsc2DU za8p6N7;vewGBS1b^|fIW=Bq%suJk2v;Vd0_04Y!F62DlaBej1CyrHnf9~kbol8xK` z!4XAU`U}S?vG;z;m{{)u1Xd$R6hDnVe{MAS=$4L4kwiZS-WxS&^BC#<tI!$bmS*bQbNy2 z(;o4abpmD_imh7bVQ- zj=T_E`6#64TK`4^Nl&F0cv_G{d%R2eN@9B)dl!G%`Zo~;V*)*MjX^Q~kRB*oDLicN zM2@r0xcvR~xM&-IHM3;k-9$3*juVZuOW-<^qa_UjK*<$R6g_+QZv^*1=!YG%xnArWY3ijyq-XQ#x6-eCMXAjbHYCeXsK zkp_ebuCX0noE}`C!swAwD&mB3Y=>gK2(CbcYa?=1m?*8_>}~An8~}{oUqV2$?S<}6 zwP8CtrY~Bf%yXvk-Ca9R&!_nh{1kHinpW4HbhC$-9RxzNJ{`xH=rqrZy4F?PySm5D zgpol5O%#;ObmNl}eYTO^F@tM27Z*%|Zx!G;F{O!q=&0TtJaiI|x=Z9XRv>(NIO?@y z0tYz!aFe|SUzlX(5bXWSY0Hu#?J__85flMe_H6R5K*``D(7&KrJJX&vL8?*Pt!Nip zhf0bYJ_#Y-UW`D5P$n6_L-E$e2*s@TxJ5~`KNY@_j73I;c5}uE!k%p5AjQNi@tS{Q=`UwhpE3HAu`atEzx7ftwt1BmOpG36dWvY zBx#p=D@rg8x0(nySn`S|y|J%a{5Jtd_cQtufUk zJ7>ncZC!%?owx&g058P(fjGgKlYb-Wzd6%?TZIJ#F!f|P{14V|jyn0B`%cTSYUyKY z;qlwuL&t2xv@vVBm&4CRTzJ(Pac$MUZ8=cTQ}Cp(YjoJwweNM0%#ythZ`RUd%bJqvHXdHhu2Q3@zF~< z7;C&oFLdjzWJrk6N56-sb}}?$LcDx@84#yLk2X;dcQ>)a`o&!dXioUS>H+1$sb{Yv zcHM~BHE+mB1cXCW;cSTg{xdeF0acO|&4T@tY2U!t!)BFpuGRoZr=wKl(1Gl(@nGO7 zMy4WbtrQ%f**ymAlRtk3%}_x=D3n&5NpL8&4C+K46mpyuH(z-K3_-roi?hGVNotXj zB2oeWkUSyV2#Tk^1h%z7@j@P$K<*9Ig~_@QW{4O)a%)z}xW%SP39aeKYbr3Jb{gG% z1c^lY>gt4lS5TZKQuoAg+U1b&^3CIn?b^T}YR8XL=Aq$nPV8n2bc@@--T5Z1c3c&Z zQVs&`rNu~O9MQKpnPS@%f?o%&!9`u@%)(`|;qQJ!u(c+2P-T}}93{g6U4S(VR@B6!Rg}7RJe8KHq!#BXxEAI_k z50PKtoqOZ{VYo!%STHObA0J1fpaBE=lvNkSF1`dPCss4DMNZT+{%K4(IxQZZ|NHmp zlJBs4kw}!tx)zg01$+X|k1zD@yx#GNGEy9cjzLlqg+UbIA;n}4L4E(aMUFW>e+ne| z0VOC9gi$pTqfX!|>onT)Xasx`+WWZ4-SMyYae`o7aY5+JU&jndj7IpijanNNaws#r zCKsoMh9C@R%f-`un`N~VT#4TMF&VKZFXK%fJJm%qBDA7gDCcWb?+qI?73nqV{{8^5 z2qTijV0;A-R(ugS#Ls`SS9Jiui9(C~+81rUP`F#y~{ zEq~eym{VGJD3I=u7zmn@E0*_i{J(38Nq#lK)hUq~rI5S4b!BnOb6Gp6Qlg3O$q@t~ z0=M?fWqZ0jVGOKhk8Y?)(9$;O*+PU~t{QGI%88eml@*{a@By+Bw9Dy48bLR`lB1%8t2T?TM zFCc5>P7KKYLc5k>o%_D$4hA;z#wYLXoQ5?Nedn;8=Ced2!R~41kMz+<9q`_XiH(k?i0J{!2%=JzczRRuLEBp^y~up18cF3eutuWh%qDXi$H21igXyWh{+DnbrR=>? zBM)FXXQAGA(eLWTW<=*dJ^Xla_&M&$Hj+g<=Npv3ZyxFy7JE&>m)>fQ^X+c+xuj7R zF&u8G!&8tMoqRSOCPfe1?yd07B77C8WQ8gC3HJujtOY>k)DDbB{O2!KkiwbckO8~> zViMZ87@S6YM1%9E;W###qp4@V(I0iE^$W|%C{T~`lq zfzgKI&K>;@ziVKmb_}^*4{$bc;B6lygWOM)W&}vBzCJkt4v})7{e`xJ7e}%*1J%Gg zY87e1c?|v;OI20;@mrZ~vn!-WoSy49V}poSWPqpy&wpPZ_N(ujB~t)In3|%TyFuR_ z1XP38HDC<~h8TWP-Ro|#1>UBOuV?KgFwbrxJ(_xSd?3V?gw&Gz`1n{_u0mAv+s5(3sDB&&e_8OF{(K>C zr@Kf4h&Nfms!qRP=hemlSU`dF=v~QEv-R>4qQK+9d#0hyB;VdqzRw00q+qo^I7^rp z^@du(0*Vyor+K2Z@)&>WsbX243W<2#Z<)9|j6yWMa|-MHe{xfR9)N&t^ZJinh7rCi zdK#lrhQW*{9W(&IK>A36kardmZGe?(Gp>-a7=iDe1XnD~OeIGo0_iykI&Lp~{Dj_* zATK`&jornKZMEcj=MaKSx%|6y_`Hvi5s`q{4fs)sU(JmGB{ZdD5+hvijK`PXZC98d+cUQ^z^Yt-aw#heuAUJFE|SsI`=$OP8Df>h(?&-=M%X=1cz%@EHZ!uTj93Zk-_+IzCUtWS5G`M57Bs!376p9o&!B2%vk9{*uiMdW+ zS)Bwu9{zLW7uUTjE(fmhHPHwfAGyAeG6oxSTA>=M(r$A*2M0?%cUzA}cUu-(Zydz- zm+3O$UO)x;$~5xwcZ%r_#6YH4Vcgc7xT}mfgISC^a=JS#>%vED_J*{O(poLr%oxxi zcrVEZV=9VcqN8o=zSqQi(jwdi(JTf=hwBiBLPA1J0a*9xEt{Y0@}f!D=00Qp(+1fIjvN``~cS4EaJ`i8gxUe6zUAU=l4o)Fuk%{Z+*eC$mwguKCM ztNUQI8-qsbO@d^5B@=>3YV}!j14jUa7s!K<`X;-bStD4h`+F>$DY>F0dA=hLQXOMY zD*$!aEsU@5D{E;nsF+zfdQktF499^v^%;G# ze%ZL1JrfCt8hkz{hSPXqXV2NaR3BV9{QOs=Z~|~sNKKYv=V?_a)vaR49RW?%y(IaP zDYmgzsX^0HYk&U^-SJb@vMUlTldd7qZLegRBFYNn88Kwa=0H90;Tpd0NTo^OD*@*N z_5MT8AXLRyLfIk4Sx>v11X2ei--~ocR0L(>{b7L#xPRmP%2$l`ge|y!6XhtQz=(1+ z$r-*(0~AR079Nd7FUD8pL51YaI&qnJKCk-@tV_%MZ}*mK zzmCQIkqxAO9ug8PEkB$E!cjRMM9^3GYPU4;@c5bL9a0T=)CKgvhW@Ysg`2DZw*qqc z#(inn&kxHa78aIHg|ROPChPRu{H`I>;5pwNm&@om2Ql!W!hZn5QzY?agMFYL0KnN! zQ#C;wwtpF>L7Cbm7$#nJgDVeG|NIzdcX;ueJTf*kgv`+6!bB+h_s$Nn5;x#mQ{}b* zO{R?!)w1_*;EB5l-Hmrb9IbInGlFSiV|_cw{a8-St`pu7ksmFuot{4J1tNTLpQWMC zHHH2^Lr?Jp1W>Lc?^9S@0rJndd^#7nA!npazYeE4D_-#i2M@fQ-h$msm@laV!8jok$J&DK>t>ufb1Ra zZ7h$hiuZU!$|)zsQ}z(_pk0z@wHTyDmN+q3kzUyR27@j)NEA^I=9sAx9Z(bF-?|kW z6O*j}1m7M$bad3f_V#x8yTTx+e-#b+b3kBMHgy09h=_9V_V$Jo_k_$ZKF%zSf5F27 zZZI<+Yv-z9JR$@B6bU8*F=odi{lv-l&JwY-pGzjBUxI0aY#tn3#x--%2G2iEY%MG- zAR``*E8&jbGeyhM|MHKF&Bf^#6);9XOFQ37R_NS)gMmpSwRxV&KZA=P>C{Q&$6$Ub z2o~=yNb~0r7B<)(hnoso8o?=Ybq!&|ZEe1t^;SHbz>EZU0*}@PoN=g>8K}Oqutxdj z=jJFQWMIU7mq@^Z(+W@kJZ9XQlrIu9usVgAiO|I9_Cx5-36a3L^8hM#Pk-;i(?=0m z($D1c7V&C}-(9F`WgoW4(u}ODAK$-+ zAN5=y2t(8q#B?wMN*s8$8UIyan!icBej$Mio85+c%k#>K?$4hk3=Dq24G4A-SfLT+ zDilwqXJ+`}x5v#L&@21^1qHxy^yYWD4F7H@9x-v8zElsYEqQi?Y`&z4q=UkRWXg^H~#15g}xKC5<_IUR4#n4@wEWo{{e|U%9Lq#l$0pux}GF<-4|wM&E+S z4|&ddp=7@t7W8WKb9Jqb-u2mJ`A^HU4CB~ZfINAJaBZFQF zC_@AivF-*4FzkPgAf7^@Ss;r?Xu9I{%gwK{i>TCDC0DZCZe~yRIVRn(i0nqN-8?co zy5I9UHyViVm>77rpR9pVzFvd{tMlUq55{7#RtZq%i~G50X<%<7bu2aGn}=r(1uMX; z0Y^Mo5d8MMx`_MX!-RfIki?kyU%UN$Ow6np6(dx%kPlhvW(Sm>r70o@eRJ!sDFVMI zJCATk$*wD5*LtIR{6yf>r3%;?q>U}*WY^c>RodREcX_dU06 z5J>TI3)Oz9^0!&7e0$9nrS$}C*_Q5wFrtAa0*xnIg;xCK!}+F{Ht?5+_uMc`AU>cf zM7qLiQ)F0>{)upU-h2isjeI+XqLU=gFCQPq34`G*(k_efpG}f|`=@{{1fvAWMS)Yx9*J z2!cw^|5WTa2O7hb!$ycmd|Iv0F$tPGNjX0 zzDW8IHw@6gMCLq!%K{ERh)Q6PrJ%XiDp1Cuu~JcgYnqUy%Z$702u?adn(TjD+>-s& zWGXzkQlBQNQ#k8B7?r1s=aXc6e~dQoh1~lF-?N%r&dW;h7vun{|j_|q!vWG!Gl8B=U$^*#ggoKHjISxbSPZ<_t5V8VEcix`2y>%dGpIpeu z2jgrxoEEa`m|F7gj{ci(%r)A?l^Y)s#DbIh` z$Sg?_SDBAS&Yc*m%o+!;vY*;naZ`YQ*;i~4#3s;&+A^ zw6ais#0*oPFhw%p>oAf)&MDc&1ZWAoJ2A*P0{H3vOBS#VVyWt9ooeWRZFf|obMSJF zT=7NG@~J-k;-BvjM~x4Yv$Mc@fdWgi0i1*YlgUATQ&H3gtPiXo9)p`36j`#qj5W!@d zPfW*~&p*Ii;WGNjPai&vXSiclf#f&M^$9Xw_q5U0Yq zqhiU&+=#AzZLqfbaUJ7tx{~#cNcGAwKF&o&iD6QZGXST|59sRCsJmUxVMbbAiG}#X zf|BblTsIMQIQI%lc8^;bfRG@bz844 zH^<%s4a_A4I!}98Gp4+gVC8mfkX{iM$XnYmQ_$paR&j|{tZ8lV+PygBJk4@fzgjhT z8=DO!DRG>IFZ_&nd^Y|7yN=Z>zPYy}Jhd1{cok(L_WC&0amH5DihMP>fi#TcH`Fbp zSiN{`ARizO4R{a@1<+1^|BH0;B^S1$DDFFg&p7wLp>z z_JCgTxU~oG@Vaz4$f)&8>$_e8_0Q8EDUQ8K^{L;W!w)nFkRJ~+(xS6YD?PF>FGwv@ zWBo8?e?RgUHSN|Q2@k|^%Vs1M^wY|Iy-_%`b2m5b)9V*3W;27&izMzfWk+SsA|OO4 zUFjntLym5Cv%I6@-)etOXJBAe6;dH0@fif7uAq|3ov^Q4k}b5mgn)Y-lF-DR7w|WP z<+<3G2M9jU{2reye&D}PF#$28kW!GW zW4tV)`=5`fP9MJYLXxmEG}h>UtK?)kd@Vf*0pazKOQU3oUonXHRjJ1kS6Y);H~dPoTk zF%MdHH6jYZ(&HD^qQnfC4WcH~=`T-jVLJ*WuK|(N+|ePPt%8PFEsU0ORD{ebpcw%X zEYLW*s&}S?M+ews*D??$4t938qjFFW^2b+>ddravVf8L7FMuin3-u0O&2#?GETXSnA?&L@U>OCSt>Rb7As)e zqPJ*`-m5;baK4ZIsx1&=BtG@7cwV2*AO?f`Pq|l(9!A#4-G56Up8_eAI>-gqXtZ~mT@EnDT~tDLOgY(A7Mg>C_+;ir~7o@d-n z{e=x8h}EP~xe8YxOvoY6=MRlP>`JqM(g8Eq3UXiyoY|lo1oXAmLmBPuVjLH%ua|1z zrfz9dV=Xo5powgM*^~vL1%WpSn^;M3NMzH8tU;L#n$mCHf`R^!KJ=y4O%hE$JoULx z;X9hps;xTiU$swm8MAwDdlrP9yxV7ax)BHu_**t7?O}R85GLvikDXh`2&yj*`7yIx>zs7nflE&0qJ}HKKPg= z&7?EI(OgLN;jq`Ib^q*7C(YtpQ(5_-9S69N&8MXx76WCeibGgb)E7K$T!Rn0d8fNC zZ4w(0%ojQlUF;^!Lo)}h&CDiyN){UiW9qxf@Qu$sD-`=#Yc~Pke^dPDu36s9Ay~_k z6<~-2YZ~cqS}H07kKLyE-@u*7F>MDC!lD0PgIKs>exN6H;qBV@nld>dcg!e&1yq+~ ziq_F^h%=n-_OPqctvo0$26=q-!abNyL_8`%hIZpf`yDgEp>>0~`Ax5>C^qpe&NoWC zl9p0)n^SMr+2s$Cp9&*`n-7+jM#1$609}!+_z^hW^oPg!+Fiwldz)qHSd+{7A?yo)Hv!A6A;{rc*;~!g#SJGA z>+$C+yuF-~%Q-pZ^Wx>57$2`j`3$CH6cLKx0Q`YRrZk0I$t!I1PlZX9-zAG<;rtxH zIo?k2As-ohi`D9rW5n_44W;4LC^}r7F!wD16J+rAHENv;2**`H;kljm8PtJV%{dITo8xMb<sCrR!&fx)zIwC|;ZRnD=Q`>i_qnU0(^wj?>d z!)4*6!W|+6YxWeX)VT9#egjr%q|b{J_i4&>OyNZP1r@WKe~hUqJzXrn{yB+#LQdk)Fjv*r%F#(akYuEB4V)H`I~5a>TX$e^gWQ{$ zg6Hr%NRX3IQ$|)2xOd_6zgqtmnU8%}`rXW3iMvWn2d&Q^lU@ECC{_!%@8i;J>$@{- zQ>ANqCyV$PVYuNlkg!`+#=#!RzzlY!;M$U~e!J@K=)Ce`?+Hux`$rY* zQ@6xQ$PRz58 zalCaF-Czm^sLi_pk;TQZ;ljsrx1}1d3#A`TTv&VNYjPB(hc-EaZ;GytFqTj4#Zzox zJ8(Tm`8yHKWQb9sqwKat4HK`e=!x;LBn874YEl3~Lus8}D?o@;Z)WAKFCIzr~l{<(BIPK?f=`@F@QW-YB* zhL>DRR8+36iBiH|@WqnIF1M;l5`ykcc$x?7K>*B22p=_McJkuaYx&}kYbTk0bh-Py zoOIf!$#mdIKd_DK{yML_Te<(9|Pe9R9>NX+* zCHLFpn0`46P=pDxP&0u}m?K#sLGFWgPq5!sqcaD%9tH z#~mi;?PvHyUB)6b6_1Ez)N6G)KE$`uVP{Xyf%jfyq~Xizw%F9uQ+qc~2m*vsYU*&L zL1!SF^@nu(nzYyo-QsF__70)cfXTm!8x|CpY?SYw3yvITKhW>RG_r)m&CU8!)0Yms zG#|mwrc$VyC7{mk;NOXwQ(U|x^18|Jf??FFwglI2m8#gibJUV*T2_p=(xw!PRi5ak zM?P!4ETa1ZjPakQZG-wBh4@6~Z+0`D=%jtLlgpPprT!u+Oc1~SBOMQfZ@E`};Wrn5 z{W1sR?wTujjxno~2JAsJB#WIbl;!jptK3*Uc(Y?ng?M@4`h7|Ly^{(5L#q zw_yg0jJ&To-J)`}%a9TKED|iEjJ*Qbz^0Y~TQb;aQ^L091Ot7B?z!ji_hH$G!ty+G zad82`QOY?Rr@svDLIl-gYC%^SLiC&sUon$j^LrVmoS8$e)D4Y|*mNN1chQrYA|z^U ztgJN6I`XBb=yl0wHP6z}N)`f&A1px;_IyMqhj56=5a|D@E& zb!wnk%a?}Eq)PYC{=e&v3WJ8<>?VkBv$M0{(AJrjd<`Uy?Z9ZDTwp#cY~ zc+cI2htiZ6F2nDYX3{K9?p3;9b)Frg{Ya~l7rQr~x$0etl&4~R4(5c^odB;NIYt6~ zEQBHOY7PqGBNLq$A%F@2d6;~BClKU?Y%E-WY4F=6CzQT}GJ!R+tmIXX`NPP3GIDZo zUjJ7sK(RWwlGSbQ(lYOlNcF$zJ8R}!Ip-kyS(vv&;1+jWbmt?&k>_uZr-m$xwi;Yo zmbcBlGL)z9O`i~oUfh%Npz&S2+fEk z55kA$XR_Nf20qL~Z?)#n5BwlN!GfY^;}`_vK-;OkMqREoW>W1smN*PpZLbyEo@V&I z{Q4K)a50SvuW*7-d9!9Ws4V37C5%2=B(j^jx6E7DXq`4udEYmn_iUuUonD9*fMts% z&j(rWNp%sTJM@c3bLUW_h#cerP(wpQ17KW}O%wuX7NJZSG|+w6a^W!>2uM$TKrRM@ zL$8ZK3z0;*;4Mh>1Fa&w3+o#jj0xBrk6=|t+b@{e!MKg5tN36RM}#-6x4Lw?!H&9$ zS3Bb|ia7Rzgih(>WzkCUHUxBHgJkCl1jFDlBayus=@6S+V|siW6wcAKsH^Xc@@QXrMb}u#$WSo(lAVNO$xxB>MyIXx~Ch z;53bor$R>*frpRZnQ3EVa}L5&w&Vjo2Y3+%fJ^9!p)%qk@8SjlJ4rVJ;fbxBa5ca1 zp~Px*Lxn`Pn(l7hy&M}9?+n+J=Fsi+-fzhH9dfGsR)X9Ov-xcOaD^YB4Ut@na;Gy?GSgs$$w--*XivZzI!h`E4=p&OY9&}ztz5J{!XAo0A89_%m$^5kiofgVp{mv zt<=rap3Bf+AI=oMEcVKC=vp}GqW$Zfrv~J?$jHhxR;5Cgn6DSuCH9WOK!EOw+&#ON zD~&A`EI0;KR8_$`bN1M>9=V|2jnymC6o~!N;qULH|41_R#bc^h2Od|lD{RgBs;6a3 zgWN;K<$LwJ(L(8kmQ}jWT8%e$r`V*N?jlC#uG!@h!DzfQJf7o5gb_BQ z@o|Jqep^5|A(3pA8mk2J8I@BjJ+{=mioNJWS0rpKO@?;}xZ9L|mj9lsy^28Eukr|LWd1LZyV1bN3X<#%&)Ai3Yezs!PL%=X%|aaU)s@e3-O~>|Qwegw zoSRo+H_f#bO~&_E@4gZ`<&T~2(v|idSFh8)sl1eHoi^>}kaJ2UgzXVT2WD2@qO_jj z1?_R&C0RKOdS5VSRksDkiJzahph=8>o}M z*M^IeMI{edvE~H76^~ufP1)PupV7gC5vo>&>$x7p5v{p;+}n!i<`492d@W*^QXD^# z6<(9f>{G^fyr;{VQCtjyPyWz~fdhSl-6^lo6Pr8yPTP9?C^Y>Dlt21U+dsBt_ms_! zkUDK3W3Gp!A*z2%|0m=wOxcIl%P3Q8lgQeBxhD@^VE~b$qM{%l&Ck!z(ec(_^!mz5 z!noelsl`uY48N5R!k}!*RK2){yMpsjSpCmR!DnOLz&iRGgkYhtqXe9?gEOP)I z#ylY>BLj(9UW_+SdR9&j^YcOVObudC=5lHgl9Q|-iU3Dga@?vtT0VWNz@lSl&;51S zHt3S=f~J@!bnWl$*Zx|b)VzOkrz?j&;3>o6?_U3T%2jE623mcocCL_hkHVZlZ%Us z8S>AKZzIA7JFJl~NTl}lsS_Xu(>Mn1oK>uO;wQAOq18k7eSL%~sETmijqe7F^2RD_ zwkWi5+Hr87Sn2(C3l-hqpi%t^PMkQ=daNAxn@4#s$q+f7>y)zUGU`f&6Kh9$;#N?y z3ANq7JQwg}0?QSqlJSW_rw9PhiAqgwt+Ybqh`Kd-q1`wkPTCCzrz{FIrIb68C_O6tUDf(*Z-$q?a@n zHm#8Vgn_ZgGR9^BC@CM6b3)-elpzJtwRGNwWh$maDv>F6B||RWuFp0Du4xqOJN;F~ zMb4tbHiO4YuKm>x^)?(lMllOdhG#wjC<@x6h982lU{fjEcqk>#7@`J&3c9)y*pgLj zT@Ypr)~}3@F%cOw%Y4FUNimks13_s7L33?~zPIK8=|KwkC_4&cLI{jJvXERMb{<@| zsIxgUJKagQT))sSR;Lgg!(bxnw*B_-Bc=9;zW*2b-Lki);PY&4)m;1N%wC*uaxppG%KI=j7_Vhw;gOV)4n4>+d*aVduD>!bUKrlSp=Le?aBIM_UW~jCRg8Q z-j%5`CykZ*-U?p<&askXn*e;zuysT|&z{(#rl#KA-*;;EiJBA$!K$M>%UbfO8=pya z3)%F|jLg^N%g|aM7)f1``H|=|ySP}vs_;0|$j}hVO07OnWXZh^aunOZxi+l_nJiTAmmm_RVedNNYVF`+j7e3!G26x;)SU239?VM8GvqBRB!%1VPt> z(&)>IS(we=>&kLPxVQL1gaZ&8loS-U&BQ_7=7AoT()K-G_zS-~TZcb1nGX`9=h@$| z@@-Q_GqL(HpSEYt2?2M$5UYS12yrhv`^e5Qu$o)xf%=Xswaxg!KsAXhK#Q2zHZ!weiKdVGm)dz46h9IxPK0ON-#IMg~u~iF*Ly34`}pRh)bxQ)E88RD!m^ zlJQE}IyyR9TFSgPX?(dc3|#Kx)B@E0l+PpvdYgH~rNzbnE-U`o|7A-+`RQ)8^0Mz8 zooKd7*BQ2VCWMv5+!>iNbX|LJCso0o z11)@vOOTJ=#x?Up+}GS345T?8=Piq6H}^sK20Ddx!l-;!brR1UNPJfwutFJmrbUV= z{nEZipfdb@G&_bL=I^<+S^696oLpRBl~_74ip&QE^$X0l!FT!wkz^oS@W!b9-MDhg z;{c{LHFc7T8Q`4I0#Rzs&~=JgBM*SOgQPEzBdfoi*uq0crTF}9tvX$q7SMo^2}Mj= ze(iat8zVkFADQH{HK%{4S50XR5X&Bdw!ZX|Mb2lY^j1$)e{6T49sP#l_=|V<1Az<5hN@3 zCJcmrMi(SG1I)G=oD_gXV4D;>6!y8)Zq-0+Z4mj$m=_TcSF&hb;|aLU3{rO{Cgupg zj&FT5Z^G;uq!=T%i4SFk$+SI%>p?-eP(-c9WPa5!#qUP;HFM@zrQFghB6H@qS8ToWE9lRQMiy>BImvm3`Stb0j# z(==)6fy)1D0S5MZLcYeEayJIe?7$?==l3U=_K&HnDFj(k$Fx z-rAb^yN$$=SE#_e1ssz1xDqCJ=-=dfK!F`H9oK2`?yWy?1+pinb{+$_rlp0for3=b_b@p* zIcU})Kp7TrtES>tc4c`4FR~<1jHNH^*9s|;TFt8(tA7OLm|cbYUtooqareQ7s8J-N zj#PylvUmJFbt?Y&H{;MT-N$BzA@{~I8Vy<+Kjbg=x12#H4-x&zSezdTBW;twrB6GY z0MG^c@_2m+26@;w*OPX4$|INfzT8!h&Nc=((!<~ zm#h%FB2Y2&qbt*~60HAUTG_~=j4Sw|LA}fD2q__CuVe6h(ZD6sUR(p7$>5AKUwZmB~MSd9Qh5erV^?i+<%Y z41Tl0bfmDEOj+M`g2Dq-3NDoH`$Ve@!X2HGkJTntOJ~=^-P#{=59uM(Zt;^c>1rc!fGmFeE)LiwYM# zj$?*%byHv4H8vRfRTm3s)TgSNyKp`(rdM7%N$Kt>kM?=t(cUKcVzZ<< zF1!+@*T^`(BJvl@n*F~z!blEEo``B}og5OgoOO7a&NtVQzxcr;#52f^$0g)|kz3r% zKO&gRVoIZ_sM9IJ>pZpTz4U^**9dwIFU5iR(9*&UMHRSQ!bz+Q7sljxO{@8)JV7UN z!xMndj|mTt=tGcKfUG)=?}@quD5%AF^TZMd%|06FWE4@z-s|l3h!7 z@p|F>V_~88AI%n8L^JvsT~#QD5z9<2_=ry-zIkNL6~5dEUvc;4fY*h|$C_G?GKbb( z5$06@y^IwVZ7EanJ9OCYEG?nXnHUJn3F7;yQEs`nl&rrWSAM%V<@*mnlNfQ()>DRo zZXMpK6)Z9F@UGi&y*eN@FP$kc^#t*by_amzTJc;&+ z-}xEeAw?|zq**8jzNDR`nR}1n7En5Z;r|kn!QDr0zUDk~qguWK7{KppufbxLv7r(c z@T(k;UpMtqsOEitF+HBVaX<88^ax)#6pXX(T3M0gCw8Babdt;x>*w<9dm)(Usg04?m4` z5$LAerKWb!@BL)6SJ%uSKp=jh#+ABPKk@qu&l%S%H?gBWM(jK=m=YUif4D$ntrvmP zwH}_<%evd91XzRH3N`lbvv&r8Z^>uUvt#e#jx0S8VTt$xE=14m$w_$b?#9g)=03H1 zR_j_jlzopXwVgf=L-{9|xxnM><)B3(>s#O@U8tfy2@!aMXf$@ur%P`SKlYg4IEI3T zwZ;lagz1Pc;Nq9BRs~#FG@jlnUdg5|x-AU8?z? z+4x&CmzGyNWw5H=`TYu?EK(|uH6n3hxpBR?1Zo_Kurcr6!Wuc=Qo0U7*HI`QjZ(xmI`$qkehv~B2Rl$+@2#cf3rq65VsJ*Kb2Q9&-=y1`mI$b_ z$*zTuV#ba!dE239-?g=8$7i_|?!}e8w^zfaEFpZZg6nukA^u(i!b9JT0Ue&JqM!zN8J2=HNDb!nL?)wBBJ z#u-H0q#H5^7?NNvi{~|$7(ade3edL~f?(wbk?ymix6FP} zgw|SF$qn^)KlPpI)-c|nn&5+d?lxwhjnc)^DZCiQT2r8pVa^4>98|wyOH4^!^+751-7NWMvZ*+eUu4wAXrdSDO+ODMDnc@AK1-WYw#ivRLKch8`h&Rl<|srG=Pc%)GuThMGLwHgte=gb^P8DTfS_0Y;IhoYpF+e zmHTihdyHHb8|btE(GbE9q?A9y)<|o%L8MS{kCTAMf$4n2 zD5$>XBYc)}^$!y<ED z{C%>qY(}@?vryUcZKn4a7t5Xt%g&!~RmW6fb$gC-#UzV^VRxG;*VuC{85E6iPu_#(8&ZJlNJ>a$Z=>hw(*g#X}!V#33a3uA^>f)fSxQ46=hb{u1KlRl7UPWq> z)oLlG+)K{3%f%HHaJD=W%k9u8`Y^ub@h4j0(TGcdD}r1Ne z|Lhv~QwKP;@u!WF9x0Y{42X;*MFj3O8>)R;au(=A$L6q&CCtwgZ2Ym;qe)G=c1 z|Bu4`j`~?dY2ujV15|DAU-<(Dv-(3f<Et|Dw=`yW|OfX&+Cp3OGIggf|#Z5V8F1-FtTu)R94V=3FX$QT^j3dm|VrH5$$Q@ zQN6-XEq0W0SlfG5#Gqo6DC(83Q{I{)V>vl8-$iONFa~k zu9&wR=|i&gNo+4kWTlrm1*SACY6k}IPPuQoWfVFSR=1QdOvy{tiA~y2FHe*PK+OrP z1`wnRK)qOQoTh94nrrs{gE3yTmc9BPcz?bAU5kVHif49~o}qU1+DOQ|+oulPX7A1+ zHEH=OSMTQSd-9~Q}il{5z6Fjz|vnh5gtcFb%%Pk(WPseGZX8}bv9*I9nn~S|G3OLOD!5`zw5r1 zZA#?@@G47UlUDQvNMx-3h0g>yZ$gsKh@v{5ybnl=Q<|?18)?o)UJdO@&sP?7`OgupJRB+To3?g6Ir{(Jlz9m#iLy|`I|H#P?A9)Qo4Y{zsj-enbyPh$IMRXFpzbl4M zgs9y3F3^CnI1k-Etq@$zG2kUa(`W;yFP6u=bx$|%zds}YD*VVa-5eQ1Oy9ZxQ~e$G zS7tta;pj6OfzLrOE&43u&NRe*9FqG75?&%ZleJ^T#lQ zR`lA?^{`lrSP5%h@lwdQdc94({^Nn*F&6%i&CIdH^oX%n-D$5``FM< zy}r8J#|T8noA2ZD3%-*1ihZ{M=Q^30YQ<;NP#o+cS6N`TV3>4jMNEgCK5uYX_{trf zTtBY;WA_jBfUBGqlvXdlqQl8Pe4#)j)wkvfjqv4qj}it*RdO=2GIQg)S5O~d2gNIV zM9)dJ>b9;a(`);-nxpg}V>X5MM^-qo6YJ@MPCz}c?>KL|5*=ZHlxP4YH7TRr6<`op z9Hw5(|J>{)r`IDz93-5&h>cH0_~TM;m*9Fke*%j=*gfGL;2<-oIN47RgaC#3knZzO z+pTwKX)BtC_;_!cu?12y?B0bzl*B$~qm%s|>o>s?cQ7W{LAjDxQ8+#US_{3 zin}bTe0Ko($fj@7HocS1LzQ|v^n~#PSrmrx(#qjQrqf;HM-Ok~U>t{j+1;fcNYNq{ zG*Zc(v8dBEWkp>V+;!LGHsbnN@u2Z|{O&O>ZlUUkQm$CG9Ft6kA0`X=C`$$ew%BLlA5-V+Vb-R!JJi(Ij9lgN804nZV})VwYoa zcZB^rad9NNU9va^D5#vDX-hmB+%Eoz2L5jJ+h~>r8i8z|n*q?4y^1!<&R`;M+^qZJ zjhzROFngp12bl{;jm;hpwocNc$Cnp;mI;y+REC?C5)v1bk)9bB_I(&wtHkJ?)-@0D zp0OndoGmYpt&tOMc*3a*DTNdIIBSSJt(fy^m`*D=2K=uY{Hbn*pVn#ZNr}mQCy^a9 zuLJUXj4yj&4I^+vK!xxE{1%Db90P2T_622NS-#TU*P^d1@wa%nZFugWb|d%C~8Pi(iVaCmhII@DGCxQY`lz z;d8(rqjL=HoxKY81g0Td@~Kd^;3GlO2pNwYcvgvQ zl{z5EUUvSsu1t0P7o~JIkx7e9#5}}rFmi4RGxjC-}Wtz**l|rL)>^z z#57xqZ)DG-q$nLtwg~kzk>f81tEM}2jPjxd|JLlmxjzbBu4?g|`1>pvjLwhA&Md&( z<0Gc3uE}mLF-kGFbBbyMQA_!(K{Xh{ip`^@)-KeAH(GHC5tW!uoS)II##7r*rYihM zzj$k%%a=K~^U7yd2eE$JI1Q7^OITh8uz;iISJJa*$H%z&tVgZz?Ls`cjWY(=GfR*sqS>*`z^hlBBVmO7b)16RURE7_b;XnBi(g%QoJc8iKkz%6Pa6Iecj0U%6K3gS?yc!sIh=K<_YfJb z=1A9>*Wyy<4VZCX-3d#?&ac*h!KU6rYw%b7M@>5S^BcXU|gbViZ} zenUIj)YAGOmpn*TA)c!`GLQIncy@L{rD1!Kk6(d_#s`_tjN$OwFpSa3W!6z)(<;=S zF|Vmlg)LJ1c+s7S5+gyt8i_T@4CN}1w%xH=sGCNCDAV7Pb`gJSqtQq3)`dAAfMxsO z|Cdbw6d>4DjFr8cbhZW&Af|$Z?EGcz?Bw9d3L4w;;&Q^$|APzRqD(DqJL6u`V zBB#W+ohwg#JQgfo2S2pE9wm;)m66gv6cIqqE22Zr@MdO_b_`Bes~w!dFB- zgJ#7O-XmVs1$YV;Y&S|VG6bv%?LmGP46FH z-aa98RTJ_mWp$@n%@qoEPR&8jO()L!>x2TOoA!t99&-3}1m7DduD z!duB%H~?E$rK4C0L-8%D_Z^g!*Hjb4Gs5D(+ojV6EkR|LrCzY39rg*&Fh-9F->#1t z=Hp;X2l-MtHChA15u@(QRUpHgzW(;0lIhNsfId8tNDgLpwq^TCSDS3jvWz|u*4<)J ziOqd1o}y|6;}Yf^FvdYt(UadlIKs{6N_C3ZJksl?cdKgxFlF^_(z}V$`Lvs6=`DFO z(HKR@W-mMYnH0ddd8W8lvw`fbEK|zx&QSw(=vaBJK?&)~3U9vh6S7vK?JV+lwVpdpmr~ zC*8Tki_dZdSWm)cy_S1cqbV@VV0ZoE z#Te{a(E=#k28OsT@CEt!82be*!oHU1c)!A3dimh6MMxx+c*g70nbbwXc{UoS?!(_N zAHzSH7ee`abS9If4jqmTd9Zfy7=$AAZU|K^mTYhC5QV@%mxby;S+i?Y!H?e<4w@`Z zL+TJu%00}>e}$uhZbwHU-b|3)6Q`K)u0oI9n0Pfjg81-kt}`eoz+X+U85nGBQhIR} zn=eG?9RGj^U6l5ueqL*I6js`Xyx{#hQ~2N_h?O`QPem_QvlA}Z8tKW%+I!Ue=PxL-TIJmEmjHZ446(2JrS-h;VT%1kx(T!B_8I$oLxW$7Me?g z@$j}mg&w$>;`lpRHX_)=VH=mGy5ZVja6vW4MU5aud6`SWbs}ym64lT3D+{2D095LZ zn+^vzrQRc^FZG(`*gF@lv<-GK`(gy}wf}E10q{^jCtC^IaDlcHV@qYXbc) zhH|Catcu7t;oK~_OWUkdZ7{_iOu2WjWuR2Q#r7EaR-3b=kfN;fDBe2-q!XsjrlX<9 zEIBN1Ee>H!2{WRbw<`uJDQ<*lSLe0~G+PdGW(-j)J06zX3RWAqP3=8-H?E{#*#JhF zY<9?Ij+E_rB)@vUp)~cyj(-?kilryLH|fD*C~Zo)l=Yl|O6t8$!^MjohO3`XU(<`0 z=O=NryMW{Z(8OJYYAqS0k4^YYLl3rMqRgl=M?l5vbx0$mDWzP9D#<5k15#vRMHNsG z!ea&Oj0yHPH~13Uc?n(%2z_`TEOlBRIs$ShKyCg*kF?7b>onhdSv3^jw{$Z`iHnl( zP_C>2i|Nbd`6P-*Zb4l7z%N=8b^yC-7V7bS{Q5ke98)@;%j%z&g??#jjD<|*QE?>c zbsz|2;n3)BBTX~+8wpqVXn)>o?M5Q;ZhG+L#)LN;mo4ry&{CPE4k^6gPZ%szcy1Um zxnoAvy?+F|sji%y&hGJ2o$@b_B&ZWcm#e4u0@*n$;n(@58PSH{X-9H}a+K*kgkoqj zc*qpaT*`lsw6Al1xbHE=nEm&_LCdCP!McAZi}XIHktX-f^$CJL!zhF6<5!T6LqQv@ zu)7gF-;&;!^y;N8ZF;nHxQ;!tQZkyt&e88x7Ih~dn_x3cWFhTqqPM5>;ssTP)RyUb zr&L#Yn4Pk-0R{I{)?J(2Ksd%(C;rc;YhSdL&+dUx)^$RKXt6Qe~2?ZPxb&}!K_XVE3`SQ)-?-I3f)5XgnV#phH?OmE z3JErDl{sF_w@RKLq_o<&(7em82bARd6`)l-5D|Gaa(|BGvn{#w!&22tlvQzX^-#Q% zx=>gLXDVdS&CR8{l%1tRrcxudBVRq~TUn^BL)=##>7saJJKOcTP!XoStg|dF$vS3` z3QTCAC4CV~;Rn;nQ0DLzEbQh5;n_9&<>3kv8kpZaMK=7mYGQK-!DZX zsV6bdo-s*y!GPD-mnq}!n^=nqgFO)d-n?i?)uGGnC;1V;@%Set{~`(&(Y{zwn5ZYG zN^W*^61QFgg@u~gr#6-GzS#Rb(s?%Tx?KAgy*J-F?VW{)-MQ0W>GhA5q!v&k>JxAh zh(novMakZOb^x}c4hV7}MS}-Ux*1ja28;&)z=B`H_VooXdr?Y8-x0!|F}9EO_r$k3 z#)zk@e4!eVRi75<{|c#K9Z7Mf1xr@8aute%MKQd)#_+-}^`6thTy-4jGeOqC+J(4J zS#)>UYs-4~jDHH8tQ)4qKe%$Elo|HE)QV+Ik8~q1W-%!u$y2QEbU!^X)6pTQtTwm@ zH9GkxX2m517P*>T_t~X3|JjwH=SNA8Yc*pAQRwmc7|H$lV-$p9lm>_8Z~l@Mk9&k8 zMn?#0hCdh7q2Hz6o*8zq`Epp8Z~IWyQ@qJ$r%6b&9qMFVU*=;Ib*R|CMN?+S%P>8s z6U$ov`TPuQp}X!Ekr@|laaX^JGWM@NQR^o9rl^_ISDQ=W1cZqLR6j`CP$DA8TW2~& z8l4gS68dZi(YOgu)Q{jXIGnSzw0u%#JRelYhC(Fz;(i9`?{v(9EMA_zB=9mL`|_Lc zLelhMs0MKuEnny8Gx{DEKo9<*vXP+)#ixP6U;d~*Z-V0S37h5TvwIKai1a9LNge`YhcV9<%iyHFIC(sqlV4>7tOm6w1bu3Bh5Iuh0_nG>J9~U#P7@%H$`n1I9GfQK1qW zweLAzD(J?rgeAiRR+u5L-A|l`w?%lI(a%1GNqU5*rSb{%{Xl5?VUyfkUZo7+dhO-q z1=7Dr*Sjk5!P)HJ>~xeEV-lB?`?{kQqu0O63lzZ~nT!&UE_X{$%-?frQuJK0ChQ|L}>uCyuIp z!>jU{j$XF5$f8Fwub3C-TQBHjHnH!`eG?iM#7T($v(pxUo&dUC*g63&3RW+`ZilQm z8X6fMAzHu%tvt(tZFlFuKylBWi}>)i{hGTVtV7HaWhxStZi~G?@{&B4tI;gH?s-IB zySj4Ht62z6+w)g<)$c;ds~t*_roF65 zhC8h?eojKhDw$s08$^5=eHNJmh779@g#{3HOd0IqqoboWw29whOL|Te+W1rr0yGr} zmfA~6?^wj1=1X%N$_e0-mJ?vz!WZa?Q}z3qaM}>e zS$!_Q^jW`vN-UeP!`oGy&arNA*_jZZF!-YLZ`Nn$dOyZFqK^o#L_NZ`AbostYzr59 z_r9eDBFgAct$;qP5z)KN2Z3k(;dbc*DCw58O9i-&Z13MBGFXpjkzw9`b#>CT|78C0 zG6R14Z6B)PfsO#V$tCrABH7od6m{w@ErhAy$O^DwQw9d?bT56DQjtN>_gEO2OhzRoVV!I@ z{`j@jo}zA;LD-IXv$t%fsG%)N3(<+sn&OWQJT=3!L62S_WbnQVjd0e%Smke*hR$*6 zmW_^%U-cME5J|Otb7M}|#*MHGBUQ?Nw?1vojJx>w&3hr7RlAp$9m}2al$2mtGTPt9|iPF?~zjmC*0{bSoX&%<0K^1*Dijebnocg97l3?B-n-&aO1 zm<^sIzl{nno(g8o1>szuA@B3F%Q=55_JJ)9%3)?{?`LTli zcY{7NR7Gxq08BN%!5Evt$>!f9%51HP_T+Z>9t8$Re3yFYFV7`q+^A;{K7BhNWlr_j zVvXGJl)R(`I%}BcdwNOR3Oy00g~xe<(bklZaQyqn(HVwxuZ4hfhdL5fh~ynjXZT#) zT=0_Yyfs9H0H=<&M-*KP{k3x|->yw&Dd+C;1&%0oKgH+%y{-l}9EM)KBgA5N?w$G< zbi5CoCLNbL<)b*m55LJ#*1YlFQ*k<*)isvJ`P;viz%qJlH2C)MXV`O+$?Z*5peK7l z+|Ak9S$li?uV23a$N!&xt33Egj)=Ip>)G?B7Gt$SZLnG2nkWl;*!NKh5%Y#yR#ros zEX1YZNm=q#qeK6Cl6hd3bc^HF-Y;1k}5fq}^SZ~&@DAr&XclnuKjr(tMl7@Wy+d2!f(cOPynaf(>~ zFqqN)XtEj2gP&DOd;-(@W&h1yf*AnB|MCe zx3Y2-t1LIR&^yaHuX%XcbS`3Xr&DqUnaBT%>gwS5`$4mTJDUo+rr8PuYt!e_09upCm{l@{!x%&tBeH@Y>-ipxlMD^*_hpq{#HbmDqGS!EX3w zmzw(K&4q2~ZR+eC_HDSZPVZD-koJ4c1B~q_Bxyz*fpYYI>*wDit=lJ$2{4arw<2x2uR)F4IxVZQ%BCP%2GP?NXwBf-fG-(Ztw&u4 z*2%_Imd9|kls?6ddv!0H6!uQrr*B}wtf#=Wp}L;xsD2&*a~jdrTPL0^fOaG^o>2HL z{N+P3ZyyT1ImJ<%^0YdOmH@*eH#@s|k<`?Yv#nsWC|apXZe53Em^Y&kAiis^p#uVq zw>PBA$3padkv3;^qFvQA(EfPCfp_jAo+h==HzqAJ2v=)?x^2G2x=&XSwAWv-_L?ye zkz#2>ARDK<@2yAlnl-}4G>UBYOEDpdSu2Ix-2|rx%0JU=l7R%O{%Ju;n+WBaS-L^H zWb^HPH9!O(#Pv11j;e44dh3u%TzM_tXY`K5I(iJCs&b%`)lA#N6jkqL2k-S^PaZ8b zs6hfxiWukA`!hsS$!S>m$^JTP4)8aLWtCpKT?m@08v`RDgIm=SfIsWpKZ2`x@z4hn z0s_4w(e&IfPg#crOloiAoV;lZwrV7jKOd=RYs-$@r?xp$mVvL!!*OO4m=Z4J?J-*y3F;j z;bCu{PXnaM*j>Yk@SrWQeF=U&xcy23BwMslTNRY-V%LNY&cid)EC-oqW20v+4&{Q0 zJw%^wRhyFbCK2RLTZ;cxiequ0QgXZq(t2Cn)0Wc z{gr`?I32Sj%weO;&Z5>p%dVSs5Q*xM7 zYpLHjygV9C!B3CbGmG>?xeNt6pZmCG+8`b>Iy&Ok1OG=k>U;^R`3@o}j5si^hxne2 zhIHz|K!4&6f9%(IFn6k$gP>R7XHmV6rh({erV4uTIyUy;5{1+MTU{HaZTkuKiZLbgm~= z;!QF25e8OyRVyeDhU8*(Crx#~@>faYzs2ANHz+#ejLm<}Z&WcYr0p2R3pVyx*i8 z5t4z27e26MW69}+x(UiB_OI*CQ@bLx!U@my$uQopK$2mF9@qfRWTc6|NHH$CirrEw zV%jmem8?%(nw*w4t}Lvx?Aa!W47aGojC3KLTz_RoJIR~Rpm@9llZV}7FRend7cxtp zzTV!3WL?gpsjB%E3Iw~1<;tYUc~l4$0_GqfzB?tSq`GI9o&Ei&lfT-YY_-v#O2BWt zI|juJ)RTv8$lLB)+`hgYUZ@8^=eHkl?AIma`^95r_^O2rB z!uhT;z#MmszK1s-<753)-aN7qF9P3Gn>a2=9sIkdUo$FA=~rX-WCZ(cNroW26VM9ow;YwNCM>?G}sjW#9_krDK2 zpERLYKeO`7LvBxm(Hju1+VQrm-MILZ^C>p8v4|qCoW{k%2lM@-_A^N5Ej)hr^Zd)5 zrFN~?f6xw-bc3?l!)pyVk|B)XX`THO?GPCS;$ZKrws?JGV%9=tC3D^c<@_orVOD_j zv0}(+x!FdHW~-rVd5DjJqU8zhvB&8dtcsSgb2EuW=5@2jxba?n=tnU!1J)IDSew&9 zH-3&4YC)F(h5O1cXr=VTnUuofs|QiwMMF@DpF5kU%^h`$H zj3f-TRCsXSZ<2havG2M6dbyo}h!&!{)=6_1VQISwfB;<)EixU>1hMyA3Hs?9V(}1D z1VW3qFE-f%O;9}4Q?0r)T(FL2ZuB~+4S}-jTZVf4b01t!(gfTcWUn&Td_N!+GmBzI zx5zMQ_O4w+n^WtG%Lv+>P2qmA%kyJ%+cxy%5Gx*dfvxap=-kDUpsp$|K)1=*Nq_NB@)3 zPX0QjN`&lLvB7Yr*ruyC3spaHikQQoO2;%(z>K)!78R8!#1MeM4`m=#^^T#EsS`YM z10A4)gy^LO^u2JtUFo%^Yt3m&?yL@h1OkrATkj3LT_6MH@e_F^v>9uf7|`QjT-Txy z&l>RCcjenW)J=V=AGuTw4ZKN;8;oowW1I7`-fx^~bAsdr7Bv9wHP>I^#SU|909Fgcpz%TVY_#cJU}Y+d6f-l6mdbdS9HeFWKG00xm!-qKV;1M3^i34Rt!!fsmjD zP$^AKO%!Z!yoULiPOT@td7FwrY-mY8yIAnGIj1bAAEGZV9b&aD_i@WV5@4Hru%31Xz+e24rYA|kqT6W6)|_J$B%Z($RNo%GulZ5?%~ zX2F-nX(w|S{j?a|yQlSyse8hM2iiJ3WO&id&rm$dSRh zi=Oeo$@*LNdFDjoFyp~d($z-BKU^;#{pY9}$!6=nF<-lqr_3cM!#Q4b+ig6wAX#oKliJw{YO7_7_ZG z#SK5p^eRg+EBnYtkD*S05;b?(!JB&vC5l!fvg0Wam6Vk(^Rt-FP>Mh|nuE3rIDJ5F zfCvDn7uuH1BHRnyCtW=bdQzO8=p0ur{;Pc*O;n9^p_X`}(Y9hcoG#hPQyo=iceMw! zrxD85Lx;RUmgm*SeP6%C zWpKAa+eOtt_Xs*ioZ9*Mwaz;-(9>h_1x0goiyfjG7CdGzAD>gt7}WSUzM#6CdGG1ol(r%o_kb45ol>OF4(G^} z6QC}`LqmC*9I*?+Uypw8dmrbeXzTd&kg9Pr9181>9MrsOMvpJN&VM+S?8oyky;|}6 zH#`hsfiMdJB+KYlOiT<|eMQs?#-lx-6L_D-D98#9D73NYMqp|8Akh|dbdAQT#rT7i z9vj{**cw&%TuPL^d7<^J1ACvAhDO};aP|Cn2et1%UIPS`5P7i5ga0c=VKd<{)D;KJ z)>w#MvaWZ4t|FI1t;I(B0_+(hfqxL|Rch3fd;zehPnv!Ef9~aLzyxZNdA+?GHgx28 zmJ@pWlFc?SB zKL9){P=Ave0~1^hVwS!dRUyi3rGE?bJ+-F!vT5|lQUu3h!yJPDQe}bH> zm^HvH0ZLLZX^>FRs3s&@t&D8_a=$cXKL8nD9hENTqX2T886;J?!z1Xtw9E zfVBKA4zZ6qj5r&$#(SvW*tuMgl7yCX9{<#Fi?iD9&9Y*w5J_PSX z_}gS9Csf=TRp!N3pCiAyJLu|s~_e)?TA;p9!3BHs7R07d?OrKa3r0A=wZWu`!WiU z=XZuBUzss4s`>q!Z~8ZCqI9#;AO@sfPqRRiI-K)KWJSH(vBrRoFl=!jVnpG%!3X}B z$$s9tLx#B;U}?7_RB*5KABR4lZcP5}Mflw~cCx_Ka_2&&S-$O-;tTg2;|NcX3;xMz zN5MrzAym039tIOW(V4PS-la5QW>=NR`EMBc*Y`?EqvZ(-QFAgpB|m-8YNWj^cSFO1 zB=#(u8L0sL&`m(jN4jAi@Xhp9Lf}jkggPCrDsFa;H=@SuLIo<(cgp%74D_ z`#rBVcbxF#h73;On?SpV=a!=HJDUzH1aQM6@H;WkP}^G7*;m1RV`=Qb@QiN>;;O_r z4)Bp*MIlw$p_w9xc+K^-aEz+xOEEt3tuccvluTP?N{=Pi1|S}m6K8rQvY00WtIhd6 z{u!5$kIHO4cPybDyJ_HeEScwNhVy#y4wwf@pKi)&?n$$Cwl8Asv3-Z`PE^j4tIv^ zJkqq}*~F_FX)0nrUojLX{mCYze|`p5bR61QvBjW?4l?Wk4v$se(IlB_bRYK~4^!9_ zxBIZn6>4;mUX`Uv4@#L$0Dmy?@DXhf472Dv#y|NiEnvKLI2arYjCZQ}CRrNiFVCh5 z+E;)b0_zcj%9xAr@sX*GxA)-aqRDF>nawM`SPzH0hr${o<#WMh%>C8l;lz#m@{5q0 z%y2O^FLDc$veUFC)E@l+AsuT4=!#3A1;dy5kJR0|sOCi%c_i{X2ygqVGYZwk!sCBJ zG(5u81(*mgx@5N@e2?^Xj_x~$GEIi1U}iB4{>>1c4S{x9hv@q~Buq9X?L)|K1rOAK z*mozau?b_25lCyU4fuOuL%3~0=U&be#|r6+cX0}C=W239&Xx0P>2p*+tm%xSF{%JD z8~iHp?5%m_{cyMBBbD)d#?ggcnU2o!Ii<-i;!sCrcQ>L4tV$O@FBaxAPH!aVv5yXk z7O<{+(q9)nVA5f|`R{g+u?_TJnl%+M%=+iIFc`y99sw+ec$W3u9~H9&SS9`a@$PQ6 zEs5~pNb|EELW@SL;`ZO0t29)es1v z7(A;E()|o|EPOjN&468>Mf1@kV?EVq? zo~8u^1Y8>nRcN;%A|RF(CWbu!#U<3u?!k_oJ_=|qSLT1Hh-DR2dW?Qaxly^F(DnqZ zjII2VGIA=^w#7f{l{VeA4ACR`jp4+M88aZYGoWO}4dk|z=mktDSbp#DUYt_$Jmm5Q6gHPnEmb6O2>Wi;p~fByeUYoHx<70D zuG)VZ^Xs--UR&A^PYJGI3P$l;_HZkr03O9Up`gkO>;e%?J4O9!(`xZMXfYTQ(T~=q!%N{#!iA9l;7PPj zd}@^1#)@peipt7uc|e|3B@sfD|JTx5P1Z59iRy(3sX=4^?E{t=H7V9=XIhG&`KCj@~P@4M?@ewPB=iCBUUx?uD-kG*tZ%UrgSKR*Ao=m%rX9&I8rIjoS3<45fKrFKF8z0 zfP;)Xwym_=J%GK;()nhNg3#%S(z$g!(e{|K6R;?ts2WHT1h~}rnq~UPmXdBXL2_>_O+7o=B$p@Glz@trks+AR%nvsI_kAx< zOX?QB{+zENI>3-txR!V0c*SsgROb{IP2HE4cGqK zWQuXp77v$R>-aC&>f3Q7(uSb^wn@2OMEzNr*anwE(^~w~%mq~K6)5}3`h;i+pT3qu zImpC(OZVqt8cO=IlqWjgoh{qIE)~hmRBkz8kQQ!kvb(SF%TOi>hK-q@4dl@M-CAh& ztm+?P#`3xv_zpH;xZ@?cu+Q!mg-W3i3%&~hMg6S@!_u|BRp8pqr&Fz$I zxbjTxwQxS`?InKOlu)aCx5qpE>i(@2tYk{_6yz6C?(5rXIs>MB?uJTr#F^5241 z;=1{}`Cu7{sDiXTppXWTMVse%0djv7+qO!_S;;^u(VF3k%cJ(|`W_IqTRo*yamR z&8&y4)B}MRl^m3Bu++Oty!K^E)C9>YZ5`bYH8jxDx#gs>Ji2{dy28m}L$V4@)lo)I z^&{ow;y~#`Lx}tq67Ih31p0gMLAtpKesBqh#r{tZC=Duak)5Nn;=_eL6sIgpwkeF4 zAClu%Lq|U8ZXhk$;(+p(n359o&D`i?+kv36JB5XY@ULg3uQKgrnw0P8K;Cg25u)B< zc7kDWa5~sq*yagZ@ZFTblarI7p`q(s_%+vtoScV~s*EIJ3H^OCcnagh6b}!`5=TTx zWUk}R#!|XpPr9;c5Et0i2z_76ZsnFkwGvL66s+<6`?G1EZLzxcX>d437BB3}i%~10Va*zOTO^N;y-cW@q=%PG!>jhz0El zmC}r3Va=U5^%0RH60Wb$qO*($ZVQ)eox+!iXWiQOyOMYxA3Q-1siXd$OBCU<%<2pH zfr!DUya_?{wad;PsPZ;nbLE~7(MrBky}_biz=V&s?m5m;WkYqq_`nQK0p2?;i>Ex} zF)t3jO;UTL6id8Gz%ht^dvEt)lV+iisOWDfCjB6rK3^?EQ!e3L_P(IF73asm`UY56 zO-xv3hQ}2T(|KYNbjCeBK5V;7oq~gWv};i&5SyQzZ8aVCeD!E3|QkbRyoxA0-DF3H~1qBMS*?vDH{chxiq zBffmWpl%GD5+LpMVz2+ohaCG$uJpQBR$Ko-S$NB+!SMd;lezx8tkv`7+!n~6 z^f2Ak-I-*v9PHy-O~@i63kOn>Yp+t}1}TT$HST;PzIx7BUBUyT=|4 zAx->f_`T!ezjN%S2MYOGji2r4&-ivFwXu7#gYU*DyvfEXF>wO0n^!wRwqUuja>qV- zt6H3?_du;bE=-5yXvWfLaK#|fS;v!J>^X#4LrA)RD2U7$Me*<${27z_*QT^?$U;~< zQ^v3x?c~^4@I|EXw;@i4xSwRjij9H1I)V`N9T5Tj#m%sYk595P=`@wK?_kSMV`4g8 zGpeh+ommyd-PQ--F8u~#(aBdc2f_kD-fXywY|7Xer1#S|zzC~i4bg9~o_}d%3Q5TC zC4O#hZAs&7mCzcbZ7AWsM)u*dF2&pu?z64L!2fXYC`>YT_s6H75Xj1^2nH5DCG|$k zZ8zXXfD)Nd?A%OBG=fzTh^{)lyVM7uigh3bcl`i0Vw%4;`VKfKJFr}Wor>N6^ z;~s&*3-A3DGfk2M_IErbWFh|nj6ZzmT4#!84*roBi~MxXPY=28e@!lbQ1UQTqhsHn zseBHhCfHnP>GQ!(T7Meud?e?g#f{#H#bhnffcth9)P(=7LhQZtc5unykK zgT*$iQRv%{LG`nH;2KxNl!#9mq*@237Z4z&iM6%J9?N7-+~HquFXG0adq8&GIVH4S zKDWc(i&MMr36WLqKj+Bb4bHe|3@D_%#Eh3ZHC6mDyaDa)Je;f)$HqeO^6a;sz&`Bx z_xHl?2M@boimT-v$#JFmzKGtpcV2n%^68u0>SR45zrFOG=+W8z+ippaynGEQ*g1w* z2c&j4!w}9@%=GnvFCSIzDvyx!kin&()M2y(>Iiw(`(I2DM+18ScyFwytZtQjdW%k? zz0#0bGx7J&qSM~DG)al`liB^xe@`R|wJY)bt8G5>BXp+Fk8iCrq;IH*l;qqh4a&1` zRzwS|dBd4yw`EjRPaSL$-Yj-GdEC3A0XVG^Z;G zlyr~QI-!mcJK#hnc*PK9%wH3#UY$qN-TIWrY;>SKbRX#-CN%fDQbEMj6##BEHOCzx zH!`Oo<}ZB%BpN<_cW&G~C?4lI{+GR04qFw#!7|4%=IG+%B$6+(?z@c(a8+t?eN+NC zWpD^V)sJ7N;CQ;)@H0mEUdC-NdZeshRXwy6zzIYt+_`46?On*xtUJuryTHmU!c^U- zjYV0GYo$4|M!L=-lKVnSNG$F!b=z87pTZsJ z16gxJ>2&Jhl2m zB6wd`Liks`L$nTB-tI5dVcODyx79v-2T6;U>-YB98f#;_k1o%SHcWgyk7k`(!7EGi zr&TAI$V$LJZ1{a%`pe1BY6@Zt zN)e}!0wE!hGQ8oSDGV|oM`Rcme@y9%HM1*VdB8|EQs>#r^B0Q%KdOX$2&^T2!q@7x zl-RX%FxD9FM1*y^Od7p{m#@b9{S>P7ZMEL)tLZ-x5#1%)xvvbQKjx-$1o4){b;!RV z<@qob%29UcBRjb8%;GhKBm-u(y!Q}tz*`w4eC-s{*KaI+-?wc+KI|GcEo|%1Ku%Sp zX)KUopyH`es|$1aeloAKT6-V~$U-9{qj-D`-UJEn)7~RLAKyRe^Rye%H+n_J+xbn? zU_J_ObZO!R&yBR}2Z+<>2AafsqM}pd5cYf!D?ih0pmiJny)%{{zM3I!>Llmrc;7hVy7WU~uh?cqzx10M7Pl%L@OC+Dw8)(ej@uvFx=14Hy8_(| zR)VT;RDN62YJ6Cblk?2g^(N9f@EO1Zg%1W2kv2TDoiC4ji7cwbfQ{!WR@_)IZI>{5 zo%*q*A}4QO!Go0lq(gg}1>&Ms-s+uGUc)~nXKNiK4u19A&v+Fu-<`6Jbzl*! zMvU>T4`5&4Cnl~#)Dq{as(yUTgYd64oeDw%B?}qXvuLjeO)q7Z^tNtDcO;Roq84jA0>YvEwD$R&LWho5(a{l+k{9wi8U}(7Y=+cu=FAdrwzd5E22XdclAp$^!Sj4bVxSdL{%pg z+>8pt?t;ycoP_CtHCNV~PszIj888k!6O?Z2-Hsy{{$=KUl(J~=ZlPKO>}MUV(1C^Yxn`!&fJBh8>kgh5h##K3e_4hX9E zcXsShj2&X2gpp+id7jI^%QFvzDz@CJUe>xMu(HaG1S>$~ir~_s898pTge%<=Bapxdoyij6bfizY!W8NSn?!|hVm>Up9OGo5PsyjrdTeA(9nRhr# z$=}H(^poRY05^MdiyM%3>=4`DbgOnyzTg=pf+WUFQB(X9xkCjfL&34gBi**~4suls zd>&Alz$ccqrn(xIvr>*n<6RMSo`u?tKv)_q>%f$rs>Tkn$dJkg3Pa#Zrj+{H8AIkoxB2HU-dnJTRI@9q3YSk=e zL`)wZv#tE{0Q0$J6@(BN+u3m`lis)LQj>c69R{)E=)2=jrgD|JzFYqW-JsW>%z?qo zmDI|g1!v~DWOj~@SP2flVuksLw(z@HJ2hoBzO-{x{#>?no_v3NTbW8(JPp*Y5-&E@ z{jPib&DB*10%PenWp*pvY^3)*^*^(3h9+*Qt>-#bk zu`&3sz!=WmhIqdstFim@XZJo8VPXX?=Jc(B8Pu1oFF>gZ)3208GdU&sFe$q`vU7qh z8qp!y)Yc943a{w=QM=Lg>)4Ds?c_h%psP_}e1BdAt3YnWSlFF5%^Rqw{+7uM(7Fcr z4=5+YB`Rw>>7c=5O zkr(vJ0xevwQQFYo2e#)2rYkTS zC_HnaqN2J^3hRel^ar|4UZx`LBWMpLZl3f$4h=8dd90?)`4O}yq8e%(&QNwdwdxed zoyomh{YxT13Njb(n-#}A3=_|N5IAJO9Y!b92epaZ7H3Vbj!LBnm16Q>R>US#y8FE524t91aNlCaEXb?dQ-S(O&p_b$g9}jLW-J?EScmcdV`P~dB zp-_9I<8!5OT}ZgQz-kaYa)&AKm@UYQLiYvC=)aglu(UYXnT3U2XzziAAtrqDP%i?L z=(T}c+^$c?@T<#H7lDP3_xlUnaYjE`8T21myj0JM;g%KTnc<@-0a^`vc>GN9c>iRg zIc~{T+ni&94iRO4)uVJ}QQFTx!nq(%97dWDGWPp+LTk6T#|M`0l>nF-T4iL7D=c|x zGF|KNR3NE9Yzb8by8QDa9>`^vJ>391+e&_!>bsq2c>vfOodR{>NhlIQL%#hi!0=*{0 zDf!K2TvV~V5QaCn4@H$)a+TG+X=naI1Pin3k?kDOh27sT)MNjoF4n^fq9?t9?exAZ zB<+<_OSy7WJ=}#4N~ufBd=7}zs6mWIAX&6}j?4Hh93CEhv%0c^A$RPrub`&SQe!4t zfsrOqYo94segmx-NHzN@qeBAF+e_y-c`hBfG>yyTuc0@a==wY`M%mHv)E@5-?X~IK zq?V8fX(ytK+k(E}81l;Hh^Rj^0eOh~w$BwFT+?KQxSK)rE-DHDVMw5C2*?;*PcqGT zEpOQ_AxN)v9Cs&!D5lWra%xD6iePf7{{pw|k&!utt$+*&3U&j8x*<$5u`A;0>(avo z=LaO3>3qW8|23Go|2s>3eBAVeg-Pb2>|i_B&PbGX9PN(K7XIp0LN?|^5V6TZ@DZex z495fx4JlAXYH=Ba^yBVrm5d`tur;@NS*E{#*BQQpFm1d;>}YgxN3EY=5*MSKyphi& zppWfbrK(Mf+4y3MSJjV9!~J?xa`aPgt{k=6=gG*8?_l`AFKQoBghWiZq4wHI(p%&zXXXsV zKw)fG!gn^!1a8H$bN{L}M^8MfEb097f;W0@&cY9+D~fn$bCH}ppgQ3)+1`J>6a+}k zb~IOmLf};~J2x^!5dB6=f}LvQ#Aa=C+ub~)cO(A=FYN-THes=HbtWFfqK}KCo!X@w zHaPnSK2tDB)h7&_+k=8FjymGa!_BQtM8hopDFbE|9Simdcqnt zyn7*)r?LH7fmB-*s$J6Doi@ZyzE=!BQ(SvL=TN<8g-(Vg3AWpj^&f*QxxLwo=lcAH zx=F)mU_5s~OZ)dMwkPnK2OXX?{nkQ7R{R)TGfn>6z$ z1LyXlT@Fr?Zz7(`;u!pLi*0f?w)!tbGABAddX(g~q^9@2S5};PE@EUIqEjG8n2!8+ zu}ojPyCy81726sQknk7yBPn6Fu7)My(Qm3(mtsyOdpr@15$r52R$J4wSBDa zIDeA&tu9QyZTq~t4sW{PYX4K*n}Yr3(cUenm| z(~mo>+a09FuHAh$xxwW+zT;rN;zo-09qTwPhf7w50-9V zNlJXD(MMB`UtaGuc&~ys@}K{F+x-l;nW5WFZSXBD3I>o6>MJZKPWLYBUq9{Lyj|bMs zor6H>98isUHCWw(JWw_sUI z5cm>DpmOBWt78wYjc*WuyM=SUe*{>Tw(G5X;FiZ)G7>fHcF0*eyE@XJ)k@_64GeQ}w$ zGcE)X7X#BE5jGPg+DHasedYV9g9Cz3PBib-wSW5b37GC@!}m{dfl31J87O?+iIs9d zQSDZeQzg}tvgBi~6_-++;0B8dw$btN&p9=Y{M6jL8TuV4-mmQ#p{W{`#GaZ8)dt$+ zwA)D>PIUib}z-s0i2UYxpD0Co5UdeE;N@?&fq>dr20v$`%QT5Fq?Qtk{-m z3Nk+A&Go=)-8oKV6nadB_C2K4<>WXN`1|qQK*U^c3?d6}+Yc7YS`N%vh1-ylk>V;v zx}B61j{`lcpPk*?l&`nHm#S@V5lgfB!L&l7O^L)j=tr&mFZm^4tyeVfy;2WM)xXm( zpKcKpBsGc=^B&&y2dujs!<;%smyU(y35=L1SY*wjD!o&ehZu|!GoY6tQdryCG9#bn zGTBsk*<3>O7%L_v)yM8J9deJim5gv^s_b~ z`$4BLFi8(8MuC^6rl#wni4YY*83UX*pzXYUkeE`?n*o1i5W0PEahK1$k!G5F9s8zC z(j79_3o`d~rd<7wf6jv1$mNBZvsI2KJktENyk`!sJi@AHb{le09+^uohO(!|X^8kS zlu~o0*g18scStt9FbdzNBm)Hol4E$ZH>ayYUuTZEy1P4UUG;%%1|{(bsN=mF#6C2S z?cC(JO3s@ zqw~x84z4G2J@eTA2LqGL`5rXeZ|8?$$Gu8x7-nV68kM0CxVb6f9b$84($BK*K%HEj zMHnO@YlHi&j*`qCuoHRzmOBGqv9Sm1Q8{W|^=#5QVIcewD%4S{Ojm z*h5P)7fjbLtUx}021k@Q{3pJlN)blm0exIqzFDi;z4a9v^S36|B zbwzC6PS0v?Ys(e%*N^|ymt9KhX)>62kA>$mQ=;R7A{R>~)Kwk`O=IKmg?SFLXW8*{ zRbAMm&?4U5HqLoi@utpdz?K>a4_%AkKT4E4%UHO{M;g@IG%KIY8q-l_*OLO{(+gX} zESJWFW8OPPyP;iR5;VIykXxrd<{D`~-jF<{+-Wl$-mC`G{6J2!AU0|Ze&>!E>|3)! zBm5ZF(_W+gJGX6n?+Tl9;;mD9ShM-54bx%R2RZ(Y-iURs&Z8S@TJC$9_2C85-1pj& zCx{xL{1q*6`;UxAN|XRy`+eK8+O8+|8m}UTswW0Z)q5kZGXX|`07f)R@aR8_BV6dN zL4A8kU;&0FH++`dSFc<=Jmdm)=ArO%xi>vLF|dUCy62Xz)cMT+^UC$~@6;;8(p;Iz z!X^&t2vpn0=rr{qY)|+W_9Y)C=_aWT&OmwWK#NS!28o>5*%2DtPn0?8{&2P#lHS4m z)j&S2`R%aN=sEN91QzjzJP6Tkq{sFx9ofTCv*7Y8jn!)1ueVeo`TU-D^q+zm?0QP8 zu03INtaPYs&^X}{-7-bT`cCx7YFUc?S_z5WKc7o0CA69ap)AY$2jcWsv`GVASXPnl z?42|-Sr0-31Bon4fsY4sTK??`=8C>QV6&#IsvXIvi~~p`am0COVU;nQFp$w(IPtK- zOpL=dUBl^n>)%`2IaDgc*tN9iF*cmR*gq)y3_pN02Dp^S(!{n4L40S-_QZ`AY6jWH zweL+RGO}M~C~!ILqU5GjG?;ahSF6ZNt!oU*wzU=lUqo*s@T5Byc;a0>tgPlC^ZPrn z>4r|t>3bqBCk0Qb=WM+1TZVfTXtrU$zJl4{w|-A^NX?Uf!}<5Cr?euL_^pZ~E~YJgTP(;#t( z{L+rWm~U^#-uiEdsY$yOKVa;ZpV(eABvb+50dj!#HWjdRZ~NDJ3>2ey0y5eaC4D)_ zx*6V}9YX1A9waQHP$;fjT+Lj~G+PJSD=i!k4NAn!N5-gL6Uy;d89t=sYn3!<5l#tr?3G?^T)`~gra z2L}e)rV_~S8yF0L z&jV9ZzDP;9s$LiK9(^^MN*&ne`)5ub1kqGger25M*9FXk-p5vCq|0BhP@y)ea)}?e zQ3)h}!Ga|T;KUg;{xlZol^(l;;7{$jD=wa8c|2rlw}xd-FsH`oK3%K_M*nkF@RY&{ zE1MhR&J1+xnuYw?I*;#@lg1oA_7;5^zplNN+IMm!TVcz8aF<(w*Jn+H*w?Qf@3~4f z-I=dQ=*Z?_IgAo=2X%CGvT`Tv$bz3%bJ&g}czv9#b2ciu?_4#!k&ewX?<^&tpQjJS zMG=BBy5h;-%|YpXS5s45<{_0y&?_0wbo-gbT^L71DPjQH;gBD#FvIgMjVzZN;L@q_Z_SVe?T}q5=D!O)JbFH{#SEpqkrCV%nU6u*>Spi z5=(3HObIY|$+V)+9 z(J@kEO(R?0yu|Qhl5TbZ5&af^nrG82V^=$Vw}>iCw)%tav%HSYK<<{e<=}*SZAR=)*IU%H7iZWwN$6{VDo`&-sOZ$UQpOVE!BU8(vU#p_XA}cTC{|JjWjn(w+MMuN@rT3;$Dg%01>~>wn#%pqBG0@t(>whB)CZ0dCI(XlhN|1$6n>~u z?Ulwmc*reM>V29Yl_5VKS_VN+W{N^hdUx7sUbe5*qTR85(gMRH^4!@r`6rz{F{2I) zM&80#*UYaC4OsLmk$_0U%(L?vdig;M`Jj98JF%)kSy=t%jIUgeqEe#@f%()}JHO}^mx7q8?knMmYAH+w=a?n=6R^NKr0xefe(bQIqcP^l~N3Uql{h zvPHsU&PuZEMNhje6c^QDt#-C7F|fJM5eX}SnSpg5FUDJ@E4Tr&zI6VG=b)}I&$yMP zY-rHBvpb@ot!av*=nm^#^we;kICt(7I`hQCCDx1a;xzd;A@{m0#z%z+R~O@u zlw%<-&jB_$B=nC0yP*&r)GGA`_NhAsIFo&xD{nYD%fP@G3n$*9b7cH|%1h_cj)d|(V(-cw0q6lsZQ5A6%nr)T!3 zO6MeH5UDp4_?ZHOd62GEOlhqIHPq>Kzd$%V`u?jVE(ADS>2a386igz3We zHrwG<_n8BihVQcgVr-t+Pl7RH#~I(EQ*{;bxfxWUyStvh`*A3C4bp(6vKgzOmCk-i zj6b5Y8NQ;-rULb}@a2xlSBLgJfsL`c^74P8(s!R)9my}dru$1WSqv;52g$hCvC?|o z!?3F3J%;e#)j*(#o!f|457A+N7B1J#JfC4Irk0Hy`t?iVP4dQ)%8D+}%?D<>lLeat zUP&Ancj3v!M7ad2-{2yAE(obHAL;+YwkyZK%$Ka3n3w=EiVuDc?R4^eo6;1U4m)9| zL-}oNY#<2+3^cK0-Th4mTYai+xh38?F;t57k&NBX7_~1dlBDUH@X+WJkvdd~D7wGI z#V7TxAJm(7FLdwSV|)g+RAdW2*{3ien8D%;l4ym~was6Yl9Ysmv`70oWsyGgEe1!n zBg>8NNSE3mdnzdCl>If985DaEz8v4|qA?lK6g9Bl9fRE7M1J^`kfd@Cn zB`;&%JSK}mQ5tRAP^8?PR9evs~nTA5X~ipjNFOMs5Mv%QJTLO zXkD6#*x`0<)LaY}(A2Co5vKc42n;a25Z{z80V-0d56 z?6+^F8%kmwJKT~*wYM)7v|6rSD(VR9wKw!*HB`MB$#`8{ zu>E`CwF{+BIxtobAL5{2vlGd5rR54&RQO2s)Wiw^r$r;@8i_yPhy0&EO$7FG- zXs1kl_v+(^m$}}Ve=ZdHrwrIcmAc|nVuSne&Dq*y;h{aay^hORSpTy6lv50sgG!7I znj&bQ5#D2A8+yMOqZafejT!Po&>xIr#3V%wd0u9d+zVYWH3m}K(U}w!hJ2SP+yP7;>C(^%c6t;9XUv}nrmyX z9SkNXU#Y)a?XHadq}|~5wpAz}6i}+ee`Ajz=;=+L(h>fV(4ej7IzqV>Xc_7$1^Oj5 zE@M2@Gj@o9_0zlWvNa0vPTrVF(LdMHZ&4K@>$64h(L#|M^0jFiRE17c)%NhC`i9Am z8`{x`*-yWXx^ye9+hnzFzI!K$fURzkeJf3VMwx`7IoSE{&*l0tUMT?1);fH76{i&2 zyZp8DAJ|i};=uQ!ERv~FxF3ko<)+aD;5DZ@cygbHmAyOv+@XF3(<~s?wn^hj-)!zt zv+yldSR|jUYdz$rKA8ug0)l+jFsc}+;eaUr`O`f~3-{>KwIYBSLCJct`@Z%!<59iz jTsJzRoekO_ZD(>ykyk|pJpZ=h!AD(1Te)1(D*XQderhJ( diff --git a/moveit_core/kinematic_constraints/dox/img/fourty_five.png b/moveit_core/kinematic_constraints/dox/img/fourty_five.png deleted file mode 100644 index 925cb551244afb751594f3be1a628cb37c2af7a4..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2897 zcmcImZCDdm7M{7zK$uWWFcUsXOMnU`BBXxQxE7c|09_>xm@bM4vkHh-B`PXf3NwHx z%8FXGxI#%m+pHDDN7rIS44|Nhpw=yjZHZV!1vP*QNQBN1tUlfSv->>zV?OS==e+0K z^PYR|%_dbLZ()uY1VL}jnXL?mAUFboz`H!aO6cy}8`%%MK0G7rJN4FZ({jm)cL;;x>}YVgYO>? z5Bt$a-E(tuo12^8MfN=_2!W)K4$=Y|`+xjQ>u7M3S5%(gGF(^amYO|qqRZP?+yKxC z|AjM$tL;Aq5@@F3L)ZA8>%{~*58=YS+0)WLcS6wa4W0%z#-= z&K%O1tH^tNER2clHieD)9hF*~B$cBY+MBEVvkxxc`fn-r>|Q5qnYi>-w2)e|^-5`< zfP_WxC@>QcHKagEgj1L&;ArYNBQY~YOc#G@=B*5Oo~Q-U54ofqPU-7^dC45Z`xotV z-}Ff44^?v(&vH^1=^u?VJ6-1_@Zxj&_6-KTOTlJj+x7m4L{oiRUrfP;e|fF(eZK+B zY-{l7S#c#c<*9WTb0Kl~(AD-$;{|v^?%RGk$S3LNn$zdwZfA_s5q_P))*P&vj9b>_ zHq)FD?~f*_B^iVJzlvCV4t92kYN(ELQEL~SUESN{R@5~dsj|`$(ogE(<*-_-cewL6 zL4HV7g(E_sD?<@m8rx^1XnuK@(SlV5+Yiiql(clLAHLYuC89C;cNwEgBvw@JgYbmB zP}r=Z>fL3pqj7!e4}UB)E8pkm<~xKj2a+!qfuOchUqnrR@IzU=mUMbe14J@efWPfhRNPQpGx zB)lz+Q_UB>Di{WIJF+uZc!eQFENl6?;)pkrhL_^k#2NBd3Buq^D_mDq_e@7`Qj>6w)JzUEhpq0$&Z}o04sFkSacByEU&n%+ zGye$Df_=}7rc`f4N;NJ&D;VH^%$RC67SvSNRCnD{YC*7VCQE3?kH6UP7>x8qoLy4| z0M|t89(?`8U-?mx@?2BD@aNYsiwoNl6NHU7%&Bc zoN>v;wxQ4&P69Bow%Cr$?M>9e74pqRaLS!K#7m_QbdssX%EQaIZqbkfBJ&0CNUf{!gz{05hJ}FSlZdNh; z9I#emy2O?b*&HZ_C=u(!mq$&0NrQtQ?)qHoLp^ zyMqYD=?wQT`h7Uiib1sf~<| zsBy8M2z#Sv1c^vGjE~oO0pxWthcFq7Wx%f5hybT6AKZ8l05K4l!@g`wK;Xx z{S;51P33l`yWcP#3WB7h4B0eyq}CI{TZ$L(tJ-tNe-3q{D<|{BiO4YzI1jb`k{`VU zd(ro(`8<0>S_K-u%^;Z@d=T?aCYE0>*LYRN$)tuTL$pLn`mk751HHk3KyAHw7SR~M z<@eI5G?((7$y+viQgPsG4~iQj9BY3BMQR^6FHWbff^3(75`*nZjY+0_%!SbJ;(dUj2MC-Rw9j=m}ccA$fFpJAPDoAg@&U=Tx(o!emF>cAo;8-m;MP35=n=PQ%$Bd#9$8C0WKRlS zRi)41_bl$I?<8!Oth+s3N;wGtYoEz-&D!RS)6r`Q9t$!mWiK35=UO6)WUPZfvXd{> z0x_FY?wM}Kz6R}(DGYWqQXrSVo(W(%`a-~sEnhs+K@I`bb{1yzE^wD^=>j$im}^D{ z>)=9A1UPj+q!u>EpznckBi4WJG8=(0JOs@gjRnH>avHO@91GG#W~1foBy0$rSllA| zeawm>r80jrvZ0n448N8@FVbJ<^7HD@8EwMs(e($x>*hIjXZro}^gsV3)Q$|_KXyMY T`Q|D9S?`qJ)4E3sx;w8H6=45e1E)qY-L+0{2V;%hmg%KjyX1-rxTAxA!?~ z3b`D%gWW7U0)gNVwl*Y^Kp;jC2-B9@OaqqdA0N`egP0b{UQMX&nmq(QY*W_8q!9>| z_23c`dFzl7U?iu9tq&zXu=60yo%LUjUrE5^mL9q(Jy@`3&vt$~A$afht?Appo0G9K zJ$Vj0Y&|#n0L6tsunPAaPd;vBEBe zMA>BJc8PGv`bdgL${e?Ytn0R7m6hn>+bqxArro>vf2wpDf+vEzLS1)v6tN=KtK^ng zOm>`h?c?B#Jd~)gg2)I(PUSMoMS)lz8U(JtGsfL5W-mF{Gdw=Q`lLR|YJM4U$9fn1 z0%7Ft7;3OUn8W`c_pe5H>GKiw$xDk8d!HzUF`0AZ{y9dLSM)+hYX$k$?9Y5$42LNI z$?1@t-2l-Ok8DX!k2VJ)eu4nx1_w?)34|Rptb6sq>RF zV!T@AZH>Db+P^F`2DXvw&N@zIvH*+TpFITpwz2b2E8`h0Z&ItNnhen$aVJtW( zMea`C%kTxyM|5fJsJ=e*{Zlh7H}*^I#nOpH%6l$+L@*q0y*uOTgRq$^od*y#uj}s^`tvaro2r%UEvyR+?F?1j85obIVOXyH$N|0NViiPeu|no~z2X&z@|6 zF1;9?!Gb#NNs7j8*Btle_<8}KvK%G>Joqtd%xW5zysZ+PyU$Z8i{aZ)G}7tU=03E-nWy=B-ChJ&ey`?3N%P!ti5rN&I4x z!cln2YGqX(9`%!(=B5;N7_|r$h{YZ3CYpmk4mGvy)`rVWNL}7-)+C^57>@}Eu(OPb z)oz43a2`l(Rl0Z2{omZC^0HQb8Hkk(s9b`9RkwnI^4|~(~t()V)nn~rQvNEfmpYD+82zd8V(uniz?eP;PPYR|{Ffy*a#dwPSY`n(spItBnXQ-y zPC$eQtDtu`E0fHJp#U2C!4Il>bqbGrON1D;2T|-1k7+W>b*6Xgs$=_j@DWBn!c(PE z5q=z5p*+=GKvjr0hk~6O^}Qby(Qx;eA0%z+$yh2zquQdpUdZEz%S~VXC_!bU9dT^T zWGoj|A4IKQRDcL|av*Ln`DLJyY%IYOZ?Ln7t3raXTBc7QEE#OV$O?W}5QSNW6k(olo)6VoKHs!QD<-xxo%7e-qGE-hP z%VWO)R@SgFo`3G22jY$Lv5KFXpN-r)x@=72KEQ*u@H=&MScK6z3(;S7-txzrK0~c< zhs?CFuDv4@>9U63o!7M4&=k>%ap@;-K3(#<3**8yH6PqGv4cEV*!8Z=@PMLV_YFr} zx?xXKDAi(*SuH*^1vyOTDn)DD8zM zQb)a~6xDLFom$eENZDa3M+ZHC0WWy% z_mJY(a(~`1KrB0sV~t-6NH5CZSBa|Wc6!e`Q@!M$RIfMdja}HJ_AX>5;pY0pyCSb{}{@U#Qx*tp%;*7rB6Y*-kUmKLDR-Re}FnfPyR% z9&PFG(eKu0#M9r5<}*5K)Rp~0eo-{jb)ZKQs&}mu6}|4rRBS6^V?gw%EIddOnqY+J ziRM?J1il6YWdha`@Hr23={rzo6w^6NQ&YO%{0i1M7y7Z`9?tFl;@428rvRkD1NwPP z2PtSF!nx;zqw=Ou0Ssqs5(d=J044?yz;pewqQgD`n{Su%#}(ZeBzy(--!97Hsbro;CVFf=!FQ|^ov=<^m!%y$SGQsmqOUP}ta#|O3aZ-B*;?C{ zwG5(58Mi=4TfKlh(d6E8Hy*S8m##Je8`Sk^@6?puuvg6lEaM*~c1vF}a;V3k@H$a) vN5j!{`j}cY^7lVsmQ(s~f7cG!j9an7t}IbqpfrLXPC{5HC!}`umi+$#eJ)wL diff --git a/moveit_core/kinematic_constraints/dox/img/perpindicular.png b/moveit_core/kinematic_constraints/dox/img/perpindicular.png deleted file mode 100644 index 8e1787439420c2dc9879a27b7327b252f0a32e39..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1699 zcmeAS@N?(olHy`uVBq!ia0y~yVDw>NVAA1W0*Z(Uu%8D~jKx9jP7LeL$-D$|I14-? ziy0XBe1W)>G0^MEC!ipEiKnkC`yDQM1{D#83o`CNA;}Wgh!W@g+}zZ>5(ej@)Wnk1 z6ovB4k_-iRPv3y>Mm}){1~vgt7srr_TW@de4`gx_Iq-0~vyTU>*oJ8d@6LoeIPjV+ zV4X2bLLh=s^hSg6cGej|>}))Jha1|P>u)^&?%Lz@>A~@L_X@+jW}bh}s8V|E``;Wh zV~!KA?r=CfX*j_+ky(k|g|935cL0LDjcRjGFR8vbR@O2o{E(+B!2q z)j#sb@A#@@xK=`D&QV_TNKYxu(3Hz%-kxHZbKu$`?o=p(NkAPB^8n19ZVc!m`26X@ zM?cjM=SxW*G|YPY?f>V(*PKgre0V0RXZ)TV?yJ0LiaPU=GTX$5(JPxze3s0Y^U2z4 z$mdeGtIs;hUun^l>SUKEC#~e`E7l$oti1KLW>JC5ydNfZh8!;QY7DpcD1VwVw?Fra zVCAoKqJ1ZV=N|)-#rn_IZCCuX1tRedEK$C$07oEp$iO5ZDWDlSX#j(Kryxoo5{LxG ziRk)~lNNfAQ&%-1pCGx!KD2gi88)?UHxl!6HttUHw5}rBr&+BipDVTNS9f-Fw7YG4 z4P>_L-PRMarTDGylY)%f8+T@IDkz=%%#M8x|Bc_b^0pUDJpAF+-Mo8B?TM$qZBKhQ z+4#~t6<%sm(+TJ~1j%ffm%KjAean`G&G!UWAPkP*{UMbkI>jVOb}5hF2{IEA?9xcY)V3ilfZ|g@SsGA@JkJz(c1f3JNN= z;0a#laQKrHEGkj)wpL0VKWQqcZ~Kvt-4B)!R4W%MF2K)^=ucI^1ra@{M6WHOWU9{= z8@%l?=gTUp3JMzv@CWyw2x}kdG!N4Fz`E3veE@4~8c0((`Ui5iZubi7l`+9z>sJIW zBY4#CT`#Q{H>~F?ZDhtFw<26o`>3HO<<6uQ**i6L+`3rJ;rG!`3mDo_y$gq3_Ec<*B_18>4g@LpQ{*ZY49|Hr2PD4Z$a^*eVR z_}!3^T2z}}v0%BVdSF#g&)Ar5m)+|!dSFr|?R)GGu){CoK_$~6%elKC1@K%##t-*R zElMnVQZb6$y$PW`mI1k>;FVOGu|RoHdOI?loPJ2Gk8jeD?a2swo@Q%KJSG!R{U>I_J_gtf?m)3+o!l?U|YSJ$p9t{flS}~{W&3c`JmKAyYLNyPXvfhv0(}Ru|C*86(`~KYk?2R zBSxY<5Sl;`=fiv8EdgMmU+R<|243LcF`MJUyE`$D(Z9QmvOCvZ___o;RYU;7AWI`v zsCCf_4g`bhIx2%)j-a2>nRA_AkHc}r16v&@zvhd+!RYj$R^5To)SN_s(F}jM&{RWSbaVlz_#C0F1q^lwjx{>Au5|!JW$eLJ;(izicF95PHdt!| zOj-mj1#ELvR((_30vcB4?q$YnGXjNA<*F>wRrDZgkBumHKp{XL3PLQQv5mAMWCuI# z=R>_6Kux9XTU!0zO!kHNhCgTHny%uPPqj0?Lk(#Fy&u=sL=E(GEqCIu`Kmp zflOlp<{x6NA~m~fI3H|ji4C8^{cD_o50i{`0SKMd=r28Gj5-1`m8J~|CHsMBXw&FO z9|D$MkET_2ly)YOUYG54WqZo?%wWEb2C=mzPtuvNk02|}1{vWtfK#moVy3dlg0i{D zZ16FA=P#J-_&Q<0xcJm-qrlb)et?Nc36u>MkLK zDc!P;TJmBDAk*^nvfK1{O??J0JGSncP?;Q6^3VC8Htab&R#Fk6EgN{1*B2X8ZG#nU zq10(FA4p{yFzpQ;{o63+x^czKKqyR4(i~y#v~I7m(M#pZ=$7r1=?kB32pr zr`JAN5aTO6XfH+}N0V-h$J6rlF@~z04WwHh7TT<0_N3Vk|A>;yMSrHKXF5)$OUxmE9wo)tT#6#0G%DB#XfO+)3yS^iH$5=vlYdI-KV*N z-+#BM%uprNPBxMC0K`bk6Qy8ko~oV8Y3o+z?rJf+0hh1oNn`2f4h}}w{3akQ@t!DR zQ=M6wLmDX>vyb_H)=&q+C;H;qsp*OYEGi&>yY#PedXz)czVfgHwjh0Pz!v7Tt+<@Q zvlcyT?5f2u=M}CGO>?51y!rjVrbH3p^RrZPgqx24uN zn)}U+ei#gkd}~q2Eil^&I%r5Zq_GzVm%5a@kg)3Qv&m`+&Y>-V=|;BI=jse7VY#5 zb(Qw&w8d*dgD71!)^Szn{&bl2doX3Wb!_sN)GLoQ8*1$eacYC8ymjam&hD7#j#oD@ zS-g8^z2aC`V6R`u$P)D^KzEZtoBGyz<1_~&NQIoi+hu{0h+fQxS$ATT1_t=VWP6#v z=wrP19~~6FAO@-3mY=4mJj7gn7`UR@h+=3?@RUjuW#6XNE%|pO*Slbx+ti%lULcVA zy^772%HmyIouPK%T|ik35_f=oD=%$Bng$faxgc|csGae32w{wz91^KQ5M`4q zm_;|q6*J4c*46`+!iqAG)35{*6`@U8czM?seLDM8Y9rC<2&KAq?};UXo5Yo%fT@Tg zYteM`A8>5jp|RWWtTBBU!SDTyG0VP;EPPu_fBMN#oATj$yB73j#)Qh+BcRt}aqLDS zT*t!^i`jYYg>OSHr|FEnl*oH==^mg zdJwBEWqi($_dY}s89A_nS?`MwIDIxJmlpf`Ma|?zCyCKoDr>8No?2L&^RA5Sw#7wV zU76fMOm6?()%i(wjGzPgv90UuqlhV~R60KXb@l!UE%&^UiFE5PE`1~jo%VB?{Iz4( zhw6whnN4u|^KW~1F>c6rHc%pSMMPh8kWSaHHRz*IX_b_jq{kSM>K-pd77DOgUZJ;D zEyx8g-aMFVZ6t<+QU9<#&Us=$!7Q8hK}ONUO{cF*Ti^j89i&g?&C+A!v~eHG{L48j z5Cl;`FSLF2E7X^5a&Ln8z#?MUp2;HIwUSZEn~UnVom`lo*89kAq(5FeF=SthgA{Q?5YoO*eqUM zk|@{DJg~9Mi@i7=;LJ3+%H$D~tqS3WB6_6}8#{EDdi^cdGY1ooH8y+u1SGkmzRmxU zj;$;g|?LsPy<(gg`^X&zgzY0c~<)9|XmE=vjm>((FxP-l^1Jgo-+g7Z2?* zPrCrO0$154w}j4V91n|vF<#^tc2h0|H|>4@s@;Ek9>C_>t5jFpvX&#-EnAu>70{uN zP36~*pt;JPe&_>zSAF1?`&O$_2yOfxX5ECD+S^W-Ii>%led58n#XdxH)3-;z5TY83 z5!+ULlWF|k#4jB_$n5F%s~)wwbjw_6qqTYax8xvh3?@4JxnTfjD;QRvI!Gr@VPH2o z!kBMX6!FqHpa?)$ozvDGE31G-wnd0nvq*TfZDK|Eqe({#vmmZ=O`eI_!y~ySf5J`@ zFg5c?0|-EHO6MwvHlN1W=v2I4d$*p9H5Q}BzrbE<#=`{|C9^ezsCr?{ovz-}&ebo& z*`Zw?G9rL~%>)C^;DZ_#Kt51b|fGj6Ixr25u-FppxRZ( z8uDr%xd#ZeAeif8CJ-WZWKyGh7w~&uO3DcI+yaR40<-k)hmEX0P3j1yH0K_&Dnl4E zA)qe|(fz;Q`wdLyR#kyUBFQ;*5Jh#I`s_Lwxq?!Rk&I%}7CZ&S!Do=3gb*EZ8GD#g z&7BEmcMczCdhq?M)5%;bHm(JIIhRC;GVLu!cp}+3r zD)V?~BYk=-CXF14);Us$#Jw6qiL_oz2NdS!Q%tbXS~-3Qm89xDx3sp_B(2c|YJk^H z7}%9sQ6>=d*O*GgJwi|>@7QoS$FMV#_6KHB-k1Ie*n<45?`{p9!MHy)sKA8zP+7Y2K*nE9U(3mIFjWvL zh)kK=g>%iAN>-9)_9^Cl!gXQis`2lb){6~-C%nMdG4~dd$F2>tM8ly2VP7H&>kT$# z@Sc&_XLe~8<+Z95`jr}6NXi)oxE$WGbDyjIZ=#gyJ0eng@CbV z2iRs_)aMw9n?x5>tJR$%M4zS7XI-dN;trtf@I`AG2zL&77Gr9%(N*a{gRc*bZFv03 zN9AI?cM8~|)8xZhZ*7|9Fy>Hk2(S=c@`AuNdi#mis1f&P-vdw}Nx3Gk4nABk^=1>` z8Y}zMCX9KTz9zM4o+hzzn4b&XpMk7$5yngpC@7_~7q3SyhNy9sm3+~;#%UKm9nf|$ z5~HX)gF|rfrr=2CD&Gn~pCjfBfPWkS{xJ4qa1JS*14o6iC;PnqHpj(G;PcrlFP{^5 zaxy50=S2v?{H#79IFq;g9X5;bi!f)V;oX!3o&uglh?8;9tDTYg>HG4<0eSa%$NV@B zj)|Vdh^oEzUfvhHa+8fZ&SNx(d={rEcP1!Tw8=PUTuLNBFZ%JtnWk7|Y+jINAZ#*`b81o!cfH$SS~VV7p)S zmZvYrJNk8xPp(egl>l_eMF@|Sh|80B%AvBE4wUK-!>E^?q?e`ZeFiXQ-j<@ng$pRm{Ci8#@!3be>R+F8*GpNrUYg$B zDz@==TeFcdG1={$n!@ym^Q#Vdt?tinH+TicSYMs_XP1~zNPrri*#9VgzQ!7sFoTbs$fF}Q`Dl^x$5$$$#c7l`?HW$ zw-tjH-Jb_+9)eblQ(}!b2fIZCPV)F2llrl<4R`RGRUDheinEawi==I5q%Hk^)UjJM zgpSJi1U9QT!9~D~&~y|;YLh18JNC5B?%DQdgYGl+C!Mx$xArQ18PFW)H|TQfmpq8a z%Fl0>*@%e7x3ZVbd{5U9rZHU4?tM9***wecIx`xNXZBoLUfuOG<#U9dWsUhntL_^- zc{AhveK3(~T^Ib5^+mlSdZC;ad*eMqgQ_v{mFWnZB(MJ8qU(AY#T$znMZ^?eetm_m zwK(2yKAlm>C#G!iY7X6Ujp8$`Z}3xqPOC>o3o${o!0Z#>%bw<`NXq-GXDUZ<{F9?+ zEZ^psa+Spo+}N}bhT zU6?)7n1R^GBfY_tQx1_uyDaz!?{=q;&yiIcbFm{$swdsDgF^?-h(8B-Wq4Y=*+v0| z_@6W!{cqhnQ8(Or{!_V@j%cPK+pO6(wNK*eyu1pkeYiRy#DJ4D=I9b@A?`op>lXE+ z&fs*{)Z+nT633x93SmE+&-5fjvDC^tpX5Fba~!*qtSI>@llR*2vR%H1S6^>*TW&Ge zvZ444vuD@k0#2j#j}|8x!H5=RYRbrF^N){tfd$r@#=|?FA{EO^S>tL?22s2H`FN?S z`|^LgJN%E=g8#g2{J*!3|EBpj!oRz(T+;kI;on_m{y%Rzw=^=At)hExy0EaJ7+miv O;H~Tq799Bf%6|cC6jX)) From d015f6509d442f33de4b5683c48b3f6d4aa297b4 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Fri, 21 Aug 2020 11:44:55 +0100 Subject: [PATCH 429/612] Add missing variants of place from list of PlaceLocations and Poses in the python interface (#2231) --- .../src/moveit_commander/move_group.py | 12 +++++++++-- .../src/wrap_python_move_group.cpp | 20 +++++++++++++++++++ 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index 074c3a3922..bd17ea9f50 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/moveit_commander/src/moveit_commander/move_group.py @@ -571,7 +571,7 @@ def pick(self, object_name, grasp=[], plan_only=False): def place(self, object_name, location=None, plan_only=False): """Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided""" result = False - if location is None: + if not location: result = self._g.place(object_name, plan_only) elif type(location) is PoseStamped: old = self.get_pose_reference_frame() @@ -582,8 +582,16 @@ def place(self, object_name, location=None, plan_only=False): result = self._g.place(object_name, conversions.pose_to_list(location), plan_only) elif type(location) is PlaceLocation: result = self._g.place(object_name, conversions.msg_to_string(location), plan_only) + elif type(location) is list: + if location: + if type(location[0]) is PlaceLocation: + result = self._g.place_locations_list(object_name, [conversions.msg_to_string(x) for x in location], plan_only) + elif type(location[0]) is PoseStamped: + result = self._g.place_poses_list(object_name, [conversions.msg_to_string(x) for x in location], plan_only) + else: + raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped, PlaceLocation, list of PoseStamped or list of PlaceLocation object") else: - raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped or PlaceLocation object") + raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped, PlaceLocation, list of PoseStamped or list of PlaceLocation object") return result def set_support_surface_name(self, value): diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 24fb87e984..8513c003a2 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -265,6 +265,15 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return place(object_name, msg, plan_only) == MoveItErrorCode::SUCCESS; } + bool placePoses(const std::string& object_name, const bp::list& poses_list, bool plan_only = false) + { + int l = bp::len(poses_list); + std::vector poses(l); + for (int i = 0; i < l; ++i) + py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(poses_list[i]), poses[i]); + return place(object_name, poses, plan_only) == MoveItErrorCode::SUCCESS; + } + bool placeLocation(const std::string& object_name, const py_bindings_tools::ByteString& location_str, bool plan_only = false) { @@ -273,6 +282,15 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS; } + bool placeLocations(const std::string& object_name, const bp::list& location_list, bool plan_only = false) + { + int l = bp::len(location_list); + std::vector locations(l); + for (int i = 0; i < l; ++i) + py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(location_list[i]), locations[i]); + return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS; + } + bool placeAnywhere(const std::string& object_name, bool plan_only = false) { return place(object_name, plan_only) == MoveItErrorCode::SUCCESS; @@ -592,7 +610,9 @@ static void wrap_move_group_interface() move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasp); move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasps); move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placePose); + move_group_interface_class.def("place_poses_list", &MoveGroupInterfaceWrapper::placePoses); move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeLocation); + move_group_interface_class.def("place_locations_list", &MoveGroupInterfaceWrapper::placeLocations); move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeAnywhere); move_group_interface_class.def("stop", &MoveGroupInterfaceWrapper::stop); From ab68828e546ac6b9fdecb2c3d38fe07311a863d2 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Fri, 21 Aug 2020 13:35:55 +0100 Subject: [PATCH 430/612] Added GILRelease to pick and place (#2272) --- .../move_group_interface/src/wrap_python_move_group.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 8513c003a2..89e500c64c 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -262,6 +262,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer convertListToPose(pose, msg.pose); msg.header.frame_id = getPoseReferenceFrame(); msg.header.stamp = ros::Time::now(); + GILReleaser gr; return place(object_name, msg, plan_only) == MoveItErrorCode::SUCCESS; } @@ -271,6 +272,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer std::vector poses(l); for (int i = 0; i < l; ++i) py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(poses_list[i]), poses[i]); + GILReleaser gr; return place(object_name, poses, plan_only) == MoveItErrorCode::SUCCESS; } @@ -279,6 +281,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer { std::vector locations(1); py_bindings_tools::deserializeMsg(location_str, locations[0]); + GILReleaser gr; return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS; } @@ -288,11 +291,13 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer std::vector locations(l); for (int i = 0; i < l; ++i) py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(location_list[i]), locations[i]); + GILReleaser gr; return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS; } bool placeAnywhere(const std::string& object_name, bool plan_only = false) { + GILReleaser gr; return place(object_name, plan_only) == MoveItErrorCode::SUCCESS; } @@ -494,6 +499,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer { moveit_msgs::Grasp grasp; py_bindings_tools::deserializeMsg(grasp_str, grasp); + GILReleaser gr; return pick(object, grasp, plan_only).val; } @@ -503,6 +509,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer std::vector grasps(l); for (int i = 0; i < l; ++i) py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(grasp_list[i]), grasps[i]); + GILReleaser gr; return pick(object, std::move(grasps), plan_only).val; } From bc6ac4e3809ff792a03b1185e2da35bcb8219771 Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Wed, 26 Aug 2020 10:50:10 -0500 Subject: [PATCH 431/612] Update last_sent_command_ at ServoCalcs start (#2249) * Move last_sent_command_ update from constructor to start * Tweak demo configs * Add install folders to CMakeLists * Add clarity to parameter warning --- moveit_ros/moveit_servo/CMakeLists.txt | 3 ++ .../config/ur_simulated_config.yaml | 2 +- moveit_ros/moveit_servo/src/servo.cpp | 2 +- moveit_ros/moveit_servo/src/servo_calcs.cpp | 36 ++++++++++--------- 4 files changed, 24 insertions(+), 19 deletions(-) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 8f9f5bd57e..0b06a77205 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -128,6 +128,9 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + ############# ## TESTING ## ############# diff --git a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml index 5c477ffb70..21fd7cbb36 100644 --- a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml @@ -55,7 +55,7 @@ collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if # Two collision check algorithms are available: # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits -collision_check_type: stop_distance +collision_check_type: threshold_distance # Parameters for "threshold_distance"-type collision checking self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index ee567f8bb3..8cc834a047 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -196,7 +196,7 @@ bool Servo::readParameters() if (parameters_.joint_limit_margin < 0.) { ROS_WARN_NAMED(LOGNAME, "Parameter 'joint_limit_margin' should be " - "greater than zero. Check yaml file."); + "greater than or equal to zero. Check yaml file."); return false; } if (parameters_.command_in_type != "unitless" && parameters_.command_in_type != "speed_units") diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index f2282ef69c..5e756697f5 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -134,14 +134,31 @@ ServoCalcs::ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, internal_joint_state_.position.resize(num_joints_); internal_joint_state_.velocity.resize(num_joints_); - // Set up the "last" published message, in case we need to send it first + // A map for the indices of incoming joint commands + for (std::size_t i = 0; i < num_joints_; ++i) + { + joint_state_name_map_[internal_joint_state_.name[i]] = i; + } + + // Low-pass filters for the joint positions + for (size_t i = 0; i < num_joints_; ++i) + { + position_filters_.emplace_back(parameters_.low_pass_filter_coeff); + } +} + +void ServoCalcs::start() +{ + // We will update last_sent_command_ every time we start servo auto initial_joint_trajectory = moveit::util::make_shared_from_pool(); - auto latest_joints = joint_state_subscriber_->getLatest(); + initial_joint_trajectory->header.frame_id = parameters_.planning_frame; initial_joint_trajectory->header.stamp = ros::Time::now(); initial_joint_trajectory->joint_names = internal_joint_state_.name; trajectory_msgs::JointTrajectoryPoint point; point.time_from_start = ros::Duration(parameters_.publish_period); + + auto latest_joints = joint_state_subscriber_->getLatest(); if (parameters_.publish_joint_positions) point.positions = latest_joints->position; if (parameters_.publish_joint_velocities) @@ -160,21 +177,6 @@ ServoCalcs::ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, initial_joint_trajectory->points.push_back(point); last_sent_command_ = initial_joint_trajectory; - // A map for the indices of incoming joint commands - for (std::size_t i = 0; i < num_joints_; ++i) - { - joint_state_name_map_[internal_joint_state_.name[i]] = i; - } - - // Low-pass filters for the joint positions - for (size_t i = 0; i < num_joints_; ++i) - { - position_filters_.emplace_back(parameters_.low_pass_filter_coeff); - } -} - -void ServoCalcs::start() -{ stop_requested_ = false; timer_ = nh_.createTimer(period_, &ServoCalcs::run, this); } From a9d3c979863050bc3cb92c73faad83343423ac13 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 26 Aug 2020 15:22:43 -0500 Subject: [PATCH 432/612] Add a utility to print collision pairs (#2275) * Add a utility for logging collision pairs * Update printing style, rename function, move constants into the struct --- .../collision_detection/collision_common.h | 33 ++++++++++++++++++- .../include/moveit_servo/collision_check.h | 3 -- .../moveit_servo/src/collision_check.cpp | 23 ++----------- 3 files changed, 34 insertions(+), 25 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 35ceecc200..5255032609 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -144,7 +144,12 @@ struct CostSource /** \brief Representation of a collision checking result */ struct CollisionResult { - CollisionResult() : collision(false), distance(std::numeric_limits::max()), contact_count(0) + CollisionResult() + : collision(false) + , distance(std::numeric_limits::max()) + , contact_count(0) + , collision_log_period(5) + , collision_log_name("collision_common") { } typedef std::map, std::vector > ContactMap; @@ -161,6 +166,26 @@ struct CollisionResult cost_sources.clear(); } + /** \brief Throttled warning of the first collision pair, if any. Other collisions are logged at the DEBUG level */ + void print() + { + if (!contacts.empty()) + { + ROS_WARN_STREAM_THROTTLE_NAMED(collision_log_period, collision_log_name, + "Objects in collision (printing first collision pair of " + << contacts.size() << "): " << contacts.begin()->first.first << ", " + << contacts.begin()->first.second); + + // Log all collisions at the debug level + ROS_DEBUG_STREAM_THROTTLE_NAMED(collision_log_period, collision_log_name, "Objects in collision:"); + for (auto contact : contacts) + { + ROS_DEBUG_STREAM_THROTTLE_NAMED(collision_log_period, collision_log_name, + "\t" << contact.first.first << ", " << contact.first.second); + } + } + } + /** \brief True if collision was found, false otherwise */ bool collision; @@ -175,6 +200,12 @@ struct CollisionResult /** \brief These are the individual cost sources when costs are computed */ std::set cost_sources; + + /** \brief Period for logging collisions */ + double collision_log_period; + + /** Log name, for printed messages */ + std::string collision_log_name; }; /** \brief Representation of a collision checking request */ diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index 30ddaff162..a02fe4750a 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -79,9 +79,6 @@ class CollisionCheck /** \brief Run one iteration of collision checking */ void run(const ros::TimerEvent& timer_event); - /** \brief Print objects in collision. Useful for debugging. */ - void printCollisionPairs(collision_detection::CollisionResult::ContactMap& contact_map); - /** \brief Get a read-only copy of the planning scene */ planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp index 18bdb06294..162a9f5831 100644 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ b/moveit_ros/moveit_servo/src/collision_check.cpp @@ -128,7 +128,7 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event) *current_state_); scene_collision_distance_ = collision_result_.distance; collision_detected_ |= collision_result_.collision; - printCollisionPairs(collision_result_.contacts); + collision_result_.print(); collision_result_.clear(); // Self-collisions and scene collisions are checked separately so different thresholds can be used @@ -136,7 +136,7 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event) *current_state_, acm_); self_collision_distance_ = collision_result_.distance; collision_detected_ |= collision_result_.collision; - printCollisionPairs(collision_result_.contacts); + collision_result_.print(); velocity_scale_ = 1; // If we're definitely in collision, stop immediately @@ -207,25 +207,6 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event) } } -void CollisionCheck::printCollisionPairs(collision_detection::CollisionResult::ContactMap& contact_map) -{ - if (!contact_map.empty()) - { - // Throttled error message about the first contact in the list - ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, - "Objects in collision (among others, possibly): " - << contact_map.begin()->first.first << ", " - << contact_map.begin()->first.second); - // Log all other contacts if in debug mode - ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Objects in collision:"); - for (auto contact : contact_map) - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, - "\t" << contact.first.first << ", " << contact.first.second); - } - } -} - void CollisionCheck::worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg) { worst_case_stop_time_ = msg->data; From 0e209abb43498502795366c3e8f92443bd562667 Mon Sep 17 00:00:00 2001 From: Jeroen Date: Thu, 27 Aug 2020 12:27:09 +0200 Subject: [PATCH 433/612] fix some spelling errors in the ompl interface (#2279) --- .../moveit/ompl_interface/model_based_planning_context.h | 2 +- .../ompl/ompl_interface/src/detail/constraints_library.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index 8f6522026f..7391024b66 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -280,7 +280,7 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext */ bool solve(double timeout, unsigned int count); - /* @brief Benchmark the planning problem. Return true on succesful saving of benchmark results + /* @brief Benchmark the planning problem. Return true on successful saving of benchmark results @param timeout The time to spend on solving @param count The number of runs to average in the computation of the benchmark @param filename The name of the file to which the benchmark results are to be saved (automatic names can be diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index 2e8b51547c..475f34ca14 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -550,7 +550,7 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra { ROS_INFO_NAMED(LOGNAME, "Computing graph connections (max %u edges per sample) ...", options.edges_per_sample); - // construct connexions + // construct connections const ob::StateSpacePtr& space = pcontext->getOMPLSimpleSetup()->getStateSpace(); unsigned int milestones = state_storage->size(); std::vector int_states(options.max_explicit_points, nullptr); @@ -622,7 +622,7 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra } result.state_connection_time = ompl::time::seconds(ompl::time::now() - start); - ROS_INFO_NAMED(LOGNAME, "Computed possible connexions in %lf seconds. Added %d connexions", + ROS_INFO_NAMED(LOGNAME, "Computed possible connections in %lf seconds. Added %d connections", result.state_connection_time, good); pcontext->getOMPLSimpleSetup()->getSpaceInformation()->freeStates(int_states); From 1f0e8db08da7b9371f57b6d7055b2c4b0ca71170 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 27 Aug 2020 19:51:41 +0200 Subject: [PATCH 434/612] Cleanup: Add a utility to print collision pairs (#2278) --- .../collision_detection/CMakeLists.txt | 1 + .../collision_detection/collision_common.h | 34 +---------- .../src/collision_common.cpp | 61 +++++++++++++++++++ 3 files changed, 65 insertions(+), 31 deletions(-) create mode 100644 moveit_core/collision_detection/src/collision_common.cpp diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt index 262db3780d..f263ec9a08 100644 --- a/moveit_core/collision_detection/CMakeLists.txt +++ b/moveit_core/collision_detection/CMakeLists.txt @@ -2,6 +2,7 @@ set(MOVEIT_LIB_NAME moveit_collision_detection) add_library(${MOVEIT_LIB_NAME} src/allvalid/collision_env_allvalid.cpp + src/collision_common.cpp src/collision_matrix.cpp src/collision_octomap_filter.cpp src/collision_tools.cpp diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 5255032609..b10ce1a479 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -144,12 +144,7 @@ struct CostSource /** \brief Representation of a collision checking result */ struct CollisionResult { - CollisionResult() - : collision(false) - , distance(std::numeric_limits::max()) - , contact_count(0) - , collision_log_period(5) - , collision_log_name("collision_common") + CollisionResult() : collision(false), distance(std::numeric_limits::max()), contact_count(0) { } typedef std::map, std::vector > ContactMap; @@ -166,25 +161,8 @@ struct CollisionResult cost_sources.clear(); } - /** \brief Throttled warning of the first collision pair, if any. Other collisions are logged at the DEBUG level */ - void print() - { - if (!contacts.empty()) - { - ROS_WARN_STREAM_THROTTLE_NAMED(collision_log_period, collision_log_name, - "Objects in collision (printing first collision pair of " - << contacts.size() << "): " << contacts.begin()->first.first << ", " - << contacts.begin()->first.second); - - // Log all collisions at the debug level - ROS_DEBUG_STREAM_THROTTLE_NAMED(collision_log_period, collision_log_name, "Objects in collision:"); - for (auto contact : contacts) - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(collision_log_period, collision_log_name, - "\t" << contact.first.first << ", " << contact.first.second); - } - } - } + /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */ + void print() const; /** \brief True if collision was found, false otherwise */ bool collision; @@ -200,12 +178,6 @@ struct CollisionResult /** \brief These are the individual cost sources when costs are computed */ std::set cost_sources; - - /** \brief Period for logging collisions */ - double collision_log_period; - - /** Log name, for printed messages */ - std::string collision_log_name; }; /** \brief Representation of a collision checking request */ diff --git a/moveit_core/collision_detection/src/collision_common.cpp b/moveit_core/collision_detection/src/collision_common.cpp new file mode 100644 index 0000000000..0f54dfdfd3 --- /dev/null +++ b/moveit_core/collision_detection/src/collision_common.cpp @@ -0,0 +1,61 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * 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 the copyright holder 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 + +static const char LOGNAME[] = "collision_common"; +constexpr size_t LOG_THROTTLE_PERIOD = 5; + +namespace collision_detection +{ +void CollisionResult::print() const +{ + if (!contacts.empty()) + { + ROS_WARN_STREAM_THROTTLE_NAMED(LOG_THROTTLE_PERIOD, LOGNAME, + "Objects in collision (printing 1st of " + << contacts.size() << " pairs): " << contacts.begin()->first.first << ", " + << contacts.begin()->first.second); + + // Log all collisions at the debug level + ROS_DEBUG_STREAM_THROTTLE_NAMED(LOG_THROTTLE_PERIOD, LOGNAME, "Objects in collision:"); + for (const auto& contact : contacts) + { + ROS_DEBUG_STREAM_THROTTLE_NAMED(LOG_THROTTLE_PERIOD, LOGNAME, + "\t" << contact.first.first << ", " << contact.first.second); + } + } +} + +} // namespace collision_detection From 49737f898406b2b523ccb3eb0506980849fa5c7c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 28 Aug 2020 10:28:18 +0200 Subject: [PATCH 435/612] Replace $(find xacro)/xacro -> xacro (#2282) --- .../chomp/chomp_interface/test/rrbot_move_group.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch index 782b702447..ab54cf53fc 100644 --- a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch +++ b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch @@ -6,7 +6,7 @@ - + From fc1465e4d2fb881327729651796d1912a3af1bb6 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 28 Aug 2020 15:09:11 -0600 Subject: [PATCH 436/612] adjust code coverage yaml to make it more forgiving (#2283) --- codecov.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/codecov.yaml b/codecov.yaml index 9cf0fb0e1e..91bb43379c 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -1,13 +1,13 @@ coverage: + precision: 2 + round: up + range: "45...70" status: project: default: target: auto - # allow coverage to drop by this amount and still post success - threshold: 0.5% - base: auto - branches: - - master - if_not_found: success - if_ci_failed: error - only_pulls: false + threshold: 5% + patch: + default: + target: auto + threshold: 5% From 2ac4db94ffee73933deda961a71262d3a893acb1 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 3 Sep 2020 16:28:44 -0400 Subject: [PATCH 437/612] [maint] Remove kinetic from Travis CI testing (#2295) --- .travis.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 2830818ac4..d0da1ee5bc 100644 --- a/.travis.yml +++ b/.travis.yml @@ -26,7 +26,6 @@ jobs: - env: TEST=code-coverage - env: ROS_DISTRO=noetic - env: ROS_DISTRO=melodic - - env: ROS_DISTRO=kinetic - compiler: clang # test_ikfast_plugins takes ~10 minutes: include here to keep the main jobs shorter env: TEST=clang-tidy-fix From 4db7ccb795ca8efa8de9646efa01b72bf1ff62ad Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 6 Sep 2020 21:16:23 +0200 Subject: [PATCH 438/612] Temporarily add srdfdom to moveit.rosinstall ... until it is released. --- moveit.rosinstall | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/moveit.rosinstall b/moveit.rosinstall index 5b2a6c27bd..17153bb67e 100644 --- a/moveit.rosinstall +++ b/moveit.rosinstall @@ -32,3 +32,7 @@ local-name: panda_moveit_config uri: https://github.com/ros-planning/panda_moveit_config.git version: melodic-devel +- git: + local-name: srdfdom + uri: https://github.com/ros-planning/srdfdom.git + version: noetic-devel From d4d1f66717e313c9a4e5d0ecb28e31ddad2c8ac8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 7 Sep 2020 08:36:09 +0200 Subject: [PATCH 439/612] Revert "Temporarily add srdfdom to moveit.rosinstall" This reverts commit 4db7ccb795ca8efa8de9646efa01b72bf1ff62ad. Instead, use ros-testing repo. --- .travis.yml | 4 ++-- moveit.rosinstall | 4 ---- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/.travis.yml b/.travis.yml index d0da1ee5bc..a2a03182fd 100644 --- a/.travis.yml +++ b/.travis.yml @@ -22,9 +22,9 @@ env: jobs: fast_finish: true include: - - env: TEST="clang-format catkin_lint" ROS_DISTRO=noetic + - env: TEST="clang-format catkin_lint" - env: TEST=code-coverage - - env: ROS_DISTRO=noetic + - env: ROS_DISTRO=noetic ROS_REPO=ros-testing - env: ROS_DISTRO=melodic - compiler: clang # test_ikfast_plugins takes ~10 minutes: include here to keep the main jobs shorter diff --git a/moveit.rosinstall b/moveit.rosinstall index 17153bb67e..5b2a6c27bd 100644 --- a/moveit.rosinstall +++ b/moveit.rosinstall @@ -32,7 +32,3 @@ local-name: panda_moveit_config uri: https://github.com/ros-planning/panda_moveit_config.git version: melodic-devel -- git: - local-name: srdfdom - uri: https://github.com/ros-planning/srdfdom.git - version: noetic-devel From 5a7b067390d9b845a56ec27c2da7076549ed448c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 29 Jun 2020 09:05:04 +0200 Subject: [PATCH 440/612] Noetic docker files --- .docker/ci-shadow-fixed/Dockerfile | 4 +-- .docker/ci/Dockerfile | 39 +++++++++++++++--------------- .docker/release/Dockerfile | 2 +- .docker/source/Dockerfile | 9 ++++--- 4 files changed, 29 insertions(+), 25 deletions(-) diff --git a/.docker/ci-shadow-fixed/Dockerfile b/.docker/ci-shadow-fixed/Dockerfile index fb053bb6c0..8b824de580 100644 --- a/.docker/ci-shadow-fixed/Dockerfile +++ b/.docker/ci-shadow-fixed/Dockerfile @@ -1,7 +1,7 @@ -# moveit/moveit:master-ci-shadow-fixed +# moveit/moveit:noetic-ci-shadow-fixed # Sets up a base image to use for running Continuous Integration on Travis -FROM moveit/moveit:master-ci +FROM moveit/moveit:noetic-ci MAINTAINER Dave Coleman dave@picknik.ai # Switch to ros-shadow-fixed diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index 01021fea58..ad22f50ce6 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -1,7 +1,7 @@ -# moveit/moveit:master-ci +# moveit/moveit:noetic-ci # Sets up a base image to use for running Continuous Integration on Travis -FROM ros:melodic-ros-base +FROM ros:noetic-ros-base MAINTAINER Dave Coleman dave@picknik.ai ENV TERM xterm @@ -12,35 +12,36 @@ WORKDIR /root/ws_moveit # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers RUN \ - mkdir src && \ - cd src && \ - # - # Download moveit source, so that we can get necessary dependencies - wstool init --shallow . https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall && \ - # # Update apt package list as previous containers clear the cache apt-get -qq update && \ apt-get -qq dist-upgrade && \ # # Install some base dependencies - apt-get -qq install -y \ - # Some source builds require a package.xml be downloaded via wget from an external location - wget \ - # Required for rosdep command - sudo \ + apt-get -qq install --no-install-recommends -y \ + # Some basic requirements + wget git sudo \ # Preferred build tools - python-catkin-tools \ + python3-catkin-tools python3-osrf-pycommon \ clang clang-format-10 clang-tidy clang-tools \ ccache && \ # + # Build and install franka_description from https://github.com/frankaemika/franka_ros.git + git clone --depth 1 --branch 0.6.0 https://github.com/frankaemika/franka_ros.git src/franka_ros && \ + mv src/franka_ros/franka_description src/ && rm -rf src/franka_ros && \ + catkin config --extend /opt/ros/noetic --install --install-space /opt/ros/noetic && \ + catkin build && \ + # Cleanup temporary workspace + rm -rf logs build devel .catkin_tools && \ + # + # Download MoveIt source, so that we can fetch all necessary dependencies + wstool init --shallow src https://raw.githubusercontent.com/ros-planning/moveit/${ROS_DISTRO}-devel/moveit.rosinstall && \ + # # Download all dependencies of MoveIt rosdep update && \ - rosdep install -y --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \ - # + DEBIAN_FRONTEND=noninteractive \ + rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \ # Remove the source code from this container - # TODO: in the future we may want to keep this here for further optimization of later containers - cd .. && \ - rm -rf src/ && \ + rm -rf src && \ # # Clear apt-cache to reduce image size rm -rf /var/lib/apt/lists/* diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile index 4611c98b20..79d673f1b6 100644 --- a/.docker/release/Dockerfile +++ b/.docker/release/Dockerfile @@ -1,7 +1,7 @@ # moveit/moveit:melodic-release # Full debian-based install of MoveIt using apt-get -FROM ros:melodic-ros-base +FROM ros:noetic-ros-base MAINTAINER Dave Coleman dave@picknik.ai # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile index 1021b4db18..c9bfae3078 100644 --- a/.docker/source/Dockerfile +++ b/.docker/source/Dockerfile @@ -1,7 +1,7 @@ -# moveit/moveit:master-source +# moveit/moveit:noetic-source # Downloads the moveit source code and install remaining debian dependencies -FROM moveit/moveit:master-ci-shadow-fixed +FROM moveit/moveit:noetic-ci-shadow-fixed MAINTAINER Dave Coleman dave@picknik.ai ENV ROS_UNDERLAY /root/ws_moveit/install @@ -11,7 +11,10 @@ WORKDIR $ROS_UNDERLAY/../src # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers RUN \ # Download moveit source so that we can get necessary dependencies - wstool init . https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall && \ + wstool init . https://raw.githubusercontent.com/ros-planning/moveit/${ROS_DISTRO}-devel/moveit.rosinstall && \ + # We only depend on franka_description (TODO: remove if released) + git clone --depth 1 --branch 0.6.0 https://github.com/frankaemika/franka_ros.git src/franka_ros && \ + mv src/franka_ros/franka_description src/ && rm -rf src/franka_ros && \ # # Update apt package list as cache is cleared in previous container # Usually upgrading involves a few packages only (if container builds became out-of-sync) From 3cc363372a6e52d378e6907096979ccff49cdccf Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 7 Sep 2020 01:46:07 -0600 Subject: [PATCH 441/612] 1.1.0 --- moveit/CHANGELOG.rst | 17 +- moveit_commander/CHANGELOG.rst | 49 ++++- moveit_core/CHANGELOG.rst | 130 +++++++++++- moveit_experimental/CHANGELOG.rst | 4 +- moveit_kinematics/CHANGELOG.rst | 55 ++++- .../chomp/chomp_interface/CHANGELOG.rst | 41 ++++ .../chomp/chomp_motion_planner/CHANGELOG.rst | 44 ++++ .../chomp_optimizer_adapter/CHANGELOG.rst | 33 +++ moveit_planners/moveit_planners/CHANGELOG.rst | 18 ++ moveit_planners/ompl/CHANGELOG.rst | 34 +++- moveit_planners/trajopt/package.xml | 4 +- .../CHANGELOG.rst | 31 ++- moveit_plugins/moveit_plugins/CHANGELOG.rst | 22 ++ .../CHANGELOG.rst | 32 +++ .../CHANGELOG.rst | 42 +++- moveit_ros/benchmarks/CHANGELOG.rst | 55 ++++- moveit_ros/manipulation/CHANGELOG.rst | 28 ++- moveit_ros/move_group/CHANGELOG.rst | 45 ++++- moveit_ros/moveit_ros/CHANGELOG.rst | 22 ++ moveit_ros/moveit_servo/CHANGELOG.rst | 190 ++++++++++++++++++ .../occupancy_map_monitor/CHANGELOG.rst | 29 +++ moveit_ros/perception/CHANGELOG.rst | 54 ++++- moveit_ros/planning/CHANGELOG.rst | 80 +++++++- moveit_ros/planning_interface/CHANGELOG.rst | 88 +++++++- moveit_ros/robot_interaction/CHANGELOG.rst | 36 +++- moveit_ros/visualization/CHANGELOG.rst | 64 +++++- moveit_ros/warehouse/CHANGELOG.rst | 37 +++- moveit_runtime/CHANGELOG.rst | 7 + moveit_setup_assistant/CHANGELOG.rst | 74 ++++++- .../trajopt_planning_pipeline.launch.xml | 2 +- 30 files changed, 1336 insertions(+), 31 deletions(-) create mode 100644 moveit_ros/moveit_servo/CHANGELOG.rst create mode 100644 moveit_ros/occupancy_map_monitor/CHANGELOG.rst diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst index 482cd91835..673950a486 100644 --- a/moveit/CHANGELOG.rst +++ b/moveit/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [maint] Use standard cmake text for metapackages (`#1620 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Jonathan Binney, Robert Haschke, Sean Yen + 1.0.1 (2019-03-08) ------------------ * [fix] segfault in chomp adapter (`#1377 `_) @@ -28,7 +35,7 @@ Changelog for package moveit * [improve][moveit_kinematics] KDL IK solver improvements (`#1321 `_) * [improve][moveit_setup_assistant] support dark themes (`#1173 `_) * [improve][moveit_ros_robot_interaction] cleanup RobotInteraction (`#1287 `_) -* [improve][moveit_ros_robot_interaction] limit IK timeout to 0.1s for a responsive interaction behaviour (`#1291 `_) +* [improve][moveit_ros_robot_interaction] limit IK timeout to 0.1s for a responsive interaction behaviour (`#1291 `_) * [maintenance] cleanup SimpleControllerManager https://github.com/ros-planning/moveit/pull/1352 * [maintenance][moveit_core] Add coverage analysis for moveit_core (`#1133 `_) * Contributors: Alexander Gutenkunst, Dave Coleman, Jonathan Binney, Keerthana Subramanian Manivannan, Martin Oehler, Michael Görner, Mike Lautman, Robert Haschke, Simon Schmeisser @@ -162,7 +169,7 @@ Changelog for package moveit * [fix][moveit_ros_visualization] RobotStateVisualization: clear before load to avoid segfault `#572 `_ * [fix][setup_assistant] Fix for lunar (`#542 `_) (fix `#506 `_) * [fix][moveit_core] segfault due to missing string format parameter. (`#547 `_) - * [fix][moveit_core] doc-comment for robot_state::computeAABB (`#516 `_) + * [fix][moveit_core] doc-comment for robot_state::computeAABB (`#516 `_) * Improvement in the contained packages: * [improve][moveit_ros_planning] Chomp use PlanningScene (`#546 `_) to partially address `#305 `_ @@ -216,12 +223,12 @@ Changelog for package moveit 0.9.5 (2017-03-08) ------------------ -* [fix] correct "simplify widget handling" `#452 `_ This reverts "simplify widget handling (`#442 `_)" -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix] correct "simplify widget handling" `#452 `_ This reverts "simplify widget handling (`#442 `_)" +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [fix] Regression on Ubuntu Xenial; numpy.ndarray indices bug (from `#86 `_) (`#450 `_). * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * [enhancement][MoveGroup] Add getLinkNames function (`#440 `_) -* [doc][moveit_commander] added description for set_start_state (`#447 `_) +* [doc][moveit_commander] added description for set_start_state (`#447 `_) * Contributors: Adam Allevato, Dave Coleman, Bence Magyar, Dave Coleman, Isaac I.Y. Saito, Yannick Jonetzko, Ravi Prakash Joshi 0.9.4 (2017-02-06) diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst index 4184e8de90..547a0dfd0f 100644 --- a/moveit_commander/CHANGELOG.rst +++ b/moveit_commander/CHANGELOG.rst @@ -2,6 +2,51 @@ Changelog for package moveit_commander ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Add missing variants of place from list of PlaceLocations and Poses in the python interface (`#2231 `_) +* [fix] Add default velocity/acceleration scaling factors (`#1890 `_) +* [fix] Handle the updated plan() function of MoveGroupCommander (`#1640 `_) +* [fix] Fix `failing tutorial `_ (`#1459 `_) +* [maint] Update dependencies for python3 in noetic (`#2131 `_) +* [maint] Better align MoveGroupInterface.plan() with C++ MoveGroup::plan() (`#790 `_) +* Contributors: Bence Magyar, Bjar Ne, Dave Coleman, Felix von Drigalski, Gerard Canal, Jafar Abdi, Masaki Murooka, Michael Ferguson, Michael Görner, Pavel-P, Raphael Druon, Robert Haschke, Ryodo Tanaka, Ryosuke Tajima, Sean Yen, v4hn + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [feature] Exposed parameter wait_for_servers and getPlannerId() API in MoveGroup's Python API (`#2201 `_) +* Contributors: Gerard Canal, Robert Haschke, Michael Görner + +1.0.5 (2020-07-08) +------------------ +* [fix] Python 3 fix (`#2030 `_) +* Contributors: Henning Kayser, Michael Görner, Robert Haschke + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [feature] Expose reference_point_position parameter in getJacobian() (`#1595 `_) +* [maint] Improve Python 3 compatibility (`#1870 `_) + * Replaced StringIO with BytesIO for python msg serialization + * Use py_bindings_tools::ByteString as byte-based serialization buffer on C++ side +* [fix] Fix service call to utilize original name space (`#1959 `_) +* [maint] Windows compatibility: fallback to using `pyreadline` (`#1635 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] Fix planning scene interface not respecting custom namespace (`#1815 `_) +* [maint] moveit_commander: python3 import fixes (`#1786 `_) +* [fix] python planning_scene_interface: fix attaching objects (`#1624 `_) +* [feature] Select time parametrization algorithm in retime_trajectory (`#1508 `_) +* Contributors: Bjar Ne, Felix von Drigalski, Masaki Murooka, Pavel-P, Raphael Druon, Robert Haschke, Ryodo Tanaka, Sean Yen, v4hn + +1.0.2 (2019-06-28) +------------------ +* [feature] Add get_jacobian_matrix to moveit_commander (`#1501 `_) +* [maintanance] Cleanup Python PlanningSceneInterface (`#1405 `_, `#789 `_) +* Contributors: Bence Magyar, Robert Haschke, Ryosuke Tajima + 1.0.1 (2019-03-08) ------------------ * [capability] python PlanningSceneInterface.add_cylinder() (`#1372 `_) @@ -70,7 +115,7 @@ Changelog for package moveit_commander 0.9.5 (2017-03-08) ------------------ * [fix] Regression on Ubuntu Xenial; numpy.ndarray indices bug (from `#86 `_) (`#450 `_). -* [doc][moveit_commander] added description for set_start_state (`#447 `_) +* [doc][moveit_commander] added description for set_start_state (`#447 `_) * Contributors: Adam Allevato, Ravi Prakash Joshi 0.9.4 (2017-02-06) @@ -126,7 +171,7 @@ Changelog for package moveit_commander 0.5.6 (2014-03-24) ------------------ -* Added the calls necessary to manage path constraints. +* Added the calls necessary to manage path constraints. * fix joint and link acces on __getattr__ when trying to acces a joint and its paramaters throught * Contributors: Acorn, Emili Boronat, Sachin Chitta diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst index b9990334d2..cf05521604 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -2,6 +2,130 @@ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Add a utility to print collision pairs (`#2275 `_) +* [feature] Fix subframes disappearing when object is detached/scaled/renamed (`#1866 `_) +* [feature] Use Eigen::Transform::linear() instead of rotation() (`#1964 `_) +* [feature] Utilize new geometric_shapes functions to improve performance (`#2038 `_) +* [feature] move_group pick place test (`#2031 `_) +* [feature] Split collision proximity threshold (`#2008 `_) +* [feature] Integration test to defend subframe tutorial (`#1757 `_) +* [feature] List missing joints in group states (`#1935 `_) +* [feature] Improve documentation for setJointPositions() (`#1921 `_) +* [feature] Installs an empty plugin description xml file if bullet is not found (`#1898 `_) +* [feature] Bullet collision detection (`#1839 `_) +* [feature] Improve RobotState documentation (`#1846 `_) +* [feature] Adapt cmake for Bullet (`#1744 `_) +* [feature] Unified Collision Environment Bullet (`#1572 `_) +* [feature] Adding continuous collision detection to Bullet (`#1551 `_) +* [feature] Bullet Collision Detection (`#1504 `_) +* [feature] Generic collision detection test suite (`#1543 `_) +* [feature] Empty collision checker template for usage with tesseract and bullet (`#1499 `_) +* [feature] Add deepcopy option for RobotTrajectory's copy constructor (`#1760 `_) +* [feature] Enable code-coverage test (`#1776 `_) +* [feature] Provide UniquePtr macros (`#1771 `_) +* [feature] Improve variable name in RobotModel (`#1752 `_) +* [feature] Adding documentation to collision detection (`#1645 `_) +* [feature] Unified Collision Environment Integration (`#1584 `_) +* [feature] Document discretization behavior in KinematicsBase (`#1602 `_) +* [feature] Rename lm to link_model (`#1592 `_) +* [feature] Allow ROS namespaces for planning request adapters (`#1530 `_) +* [feature] Add named frames to CollisionObjects (`#1439 `_) +* [feature] More verbose "id" argument in PlanningScene, RobotState & CollisionWorld functions (`#1450 `_) +* [feature] Separate source file for CartesianInterpolator (`#1149 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Change FloatingJointModel::getStateSpaceDimension return value to 7 +* [fix] collision world: check for empty shapes vector before access (`#2026 `_) +* [fix] Fix Condition for Adding current DistanceResultData to DistanceMap for DistanceRequestType::SINGLE (`#1963 `_) +* [fix] Do not override empty URDF link collision geometry (`#1952 `_) +* [fix] Fix issue in unpadded collision checking (`#1899 `_) +* [fix] Remove object from collision world only once (`#1900 `_) +* [fix] Initialize zero dynamics in CurrentStateMonitor (`#1883 `_) +* [fix] getFrameInfo(): Avoid double search for link name (`#1853 `_) +* [fix] Fix RobotTrajectory's copy constructor (`#1834 `_) +* [fix] Fix flaky moveit_cpp test (`#1781 `_) +* [fix] Fix doc string OrientationConstraint (`#1793 `_) +* [fix] Move ASSERT() into test setup (`#1657 `_) +* [fix] Add missing dependencies to library (`#1746 `_) +* [fix] Fix clang-tidy for unified collision environment (`#1638 `_) +* [fix] PlanningRequestAdapter::initialize() = 0 (`#1621 `_) +* [fix] Fix World::getTransform (`#1553 `_) +* [fix] Link moveit_robot_model from moveit_test_utils (`#1534 `_) +* [maint] Move constraint representation dox to moveit_tutorials (`#2147 `_) +* [maint] Update dependencies for python3 in noetic (`#2131 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#2004 `_, `#1419 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Rename PR2-related collision test files (`#1856 `_) +* [maint] Fix compiler warnings (`#1773 `_) +* [maint] Add missing licenses (`#1716 `_) (`#1720 `_) +* [maint] Move isEmpty() test functions to moveit_core/utils (`#1627 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: AndyZe, Aris Synodinos, Ayush Garg, Bryce Willey, Dale Koenig, Dave Coleman, Felix von Drigalski, Henning Kayser, Jafar Abdi, Jens P, Jere Liukkonen, Jeroen, John Stechschulte, Jonas Wittmann, Jonathan Binney, Markus Vieth, Martin Pecka, Michael Ferguson, Michael Görner, Mike Lautman, Niklas Fiedler, Patrick Beeson, Robert Haschke, Sean Yen, Shivang Patel, Tyler Weaver, Wolfgang Merkt, Yu, Yan, tsijs, v4hn + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10, Fix warnings +* [maint] Optimize includes (`#2229 `_) +* [maint] Fix docs in robot_state.h (`#2215 `_) +* Contributors: Jeroen, Markus Vieth, Michael Görner, Robert Haschke + +1.0.5 (2020-07-08) +------------------ +* [fix] Fix memory leaks related to geometric shapes usage (`#2138 `_) +* [fix] Prevent collision checking segfault if octomap has NULL root pointer (`#2104 `_) +* [feature] Allow to parameterize input trajectory density of Time Optimal trajectory generation (`#2185 `_) +* [maint] Optional C++ version setting (`#2166 `_) +* [maint] Added missing boost::regex dependency (`#2163 `_) +* [maint] PropagationDistanceField: Replace eucDistSq with squaredNorm (`#2101 `_) +* [fix] Fix getTransform() (`#2113 `_) + - PlanningScene::getTransforms().getTransform() -> PlanningScene::getFrameTransform() + - PlanningScene::getTransforms().canTransform() -> PlanningScene::knowsFrameTransform() +* [fix] Change FloatingJointModel::getStateSpaceDimension return value to 7 (`#2106 `_) +* [fix] Check for empty quaternion message (`#2089 `_) +* [fix] TOTG: Fix parameterization for single-waypoint trajectories (`#2054 `_) + - RobotState: Added interfaces to zero and remove dynamics +* [maint] Remove unused angles.h includes (`#1985 `_) +* Contributors: Felix von Drigalski, Henning Kayser, Michael Görner, Jere Liukkonen, John Stechschulte, Patrick Beeson, Robert Haschke, Tyler Weaver, Wolfgang Merkt + +1.0.4 (2020-05-30) +------------------ +* Fix broken IKFast generator (`#2116 `_) +* Contributors: Robert Haschke + +1.0.3 (2020-04-26) +------------------ +* [feature] Allow to filter for joint when creating a RobotTrajectory message (`#1927 `_) +* [fix] Fix RobotState::copyFrom() +* [fix] Fix segfault in totg (`#1861 `_) +* [fix] Handle incomplete group states +* [fix] Fix issue in totg giving invalid accelerations (`#1729 `_) +* [feature] New isValidVelocityMove() for checking time between two waypoints given velocity (`#684 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [fix] Fix Condition for adding current DistanceResultData to DistanceMap (`#1968 `_) +* [maint] Fix various build issues on Windows (`#1880 `_) + * remove GCC extensions (`#1583 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] Delete attached body before adding a new one with same id (`#1821 `_) +* [maint] Provide UniquePtr macros (`#1771 `_) +* [maint] Updated deprecation method: MOVEIT_DEPRECATED -> [[deprecated]] (`#1748 `_) +* [feature] Add RobotTrajectory::getDuration() (`#1554 `_) +* Contributors: Ayush Garg, Dale Koenig, Dave Coleman, Felix von Drigalski, Jafar Abdi, Jeroen, Michael Görner, Mike Lautman, Niklas Fiedler, Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ +* [fix] Removed MessageFilter for /collision_object messages (`#1406 `_) +* [fix] Update robot state transforms when initializing a planning scene (`#1474 `_) +* [fix] Fix segfault when detaching attached collision object (`#1438 `_) +* [fix] Normalize quaternions when adding new or moving collision objects (`#1420 `_) +* [fix] Minor bug fixes in (collision) distance field (`#1392 `_) +* [fix] Remove obsolete moveit_resources/config.h () +* [fix] Fix test utilities in moveit_core (`#1391 `_, `#1409 `_, `#1412 `_) +* Contributors: Bryce Willey, Henning Kayser, Mike Lautman, Robert Haschke, tsijs + 1.0.1 (2019-03-08) ------------------ * [capability] Graphically print current robot joint states with joint limits (`#1358 `_) @@ -122,7 +246,7 @@ Changelog for package moveit_core 0.9.9 (2017-08-06) ------------------ * [fix][moveit_core] segfault due to missing string format parameter. (`#547 `_) -* [fix][moveit_core] doc-comment for robot_state::computeAABB (`#516 `_) +* [fix][moveit_core] doc-comment for robot_state::computeAABB (`#516 `_) * Contributors: Martin Pecka, henhenhen 0.9.8 (2017-06-21) @@ -140,7 +264,7 @@ Changelog for package moveit_core 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman @@ -198,7 +322,7 @@ Changelog for package moveit_core * [fix] New getOnlyOneEndEffectorTip() function `#262 `_ * [fix] issue `#258 `_ in jade-devel `#266 `_ * [fix] Segfault in parenthesis operator `#254 `_ -* [fix] API Change of shape_tools `#242 `_ +* [fix] API Change of shape_tools `#242 `_ * [fix] Fixed bug in KinematicConstraintSet::decide that makes it evaluate only joint_constraints. `#250 `_ * [fix] Prevent divide by zero `#246 `_ * [fix] removed the 'f' float specifiers and corrected misspelled method name diff --git a/moveit_experimental/CHANGELOG.rst b/moveit_experimental/CHANGELOG.rst index 813d8a2015..8f5c956f25 100644 --- a/moveit_experimental/CHANGELOG.rst +++ b/moveit_experimental/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package moveit_experimental ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.0.1 (2019-03-08) +1.1.0 (2020-09-04) ------------------ 1.0.0 (2019-02-24) @@ -69,7 +69,7 @@ Changelog for package moveit_experimental 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * Contributors: Dave Coleman 0.9.4 (2017-02-06) diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst index b827103069..364acfb3a8 100644 --- a/moveit_kinematics/CHANGELOG.rst +++ b/moveit_kinematics/CHANGELOG.rst @@ -2,6 +2,59 @@ Changelog for package moveit_kinematics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Implementation of parameter TranslationXY2D IKFast (`#1949 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Delete IKCache copy constructor (`#1750 `_) +* [maint] Move NOLINT instructions to intended positions (`#2058 `_) +* [maint] clang-tidy fixes (`#2050 `_) (`#2004 `_, `#1419 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Fix various build issues on Windows (`#1880 `_) +* [maint] Fix compiler warnings (`#1773 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* [maint] Relax dependencies of moveit_kinematics (`#1529 `_) +* Contributors: Ayush Garg, Christian Henkel, Dave Coleman, Henning Kayser, Immanuel Martini, Jonathan Binney, Markus Vieth, Martin Günther, Michael Ferguson, Michael Görner, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan, edetleon, jschleicher, v4hn + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke, Michael Görner + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ +* Fix broken IKFast generator (`#2116 `_) +* Contributors: Robert Haschke + +1.0.3 (2020-04-26) +------------------ +* [feature] KDL IK: constrain wiggled joints to limits (`#1953 `_) +* [feature] IKFast: optional prefix for link names (`#1599 `_) + If you pass a `link_prefix` parameter in your `kinematics.yaml`, this string is prepended to the base and tip links. + It allows multi-robot setups (e.g. dual-arm) and still instantiate the same solver for both manipulators. +* [feature] IKFast: increase verbosity of generated script (`#1434 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [feature] IKFast: implement `Translation*AxisAngle4D` IK type (`#1823 `_) +* [fix] Fix possible division-by-zero (`#1809 `_) +* Contributors: Christian Henkel, Martin Günther, Max Krichenbauer, Michael Görner, Robert Haschke, Sean Yen, Yu, Yan, jschleicher + +1.0.2 (2019-06-28) +------------------ +* [fix] KDL IK solver: fix handling of mimic joints (`#1490 `_) +* [fix] Fix ROS apt-key in OpenRAVE docker image (`#1503 `_) +* [fix] Fix ikfast plugin-generator script (`#1492 `_, `#1449 `_) +* Contributors: Immanuel Martini, Michael Görner, Robert Haschke + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -97,7 +150,7 @@ Changelog for package moveit_kinematics 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * Contributors: Dave Coleman 0.9.4 (2017-02-06) diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst index 7184007695..c44a1ee598 100644 --- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package chomp_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Replace $(find xacro)/xacro -> xacro (`#2282 `_) +* [feature] Start new joint_state_publisher_gui on param use_gui (`#2257 `_) +* [feature] Optional cpp version setting (`#2166 `_) +* [feature] Change API of ChompPlanner::solve() to not use message +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] clang-tidy fixes (`#2050 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [fix] Small fixes to chomp planner (`#1407 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Add Missing License (`#1779 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Chittaranjan Srinivas Swaminathan, Dave Coleman, Jonathan Binney, Markus Vieth, Robert Haschke, Sean Yen, Tyler Weaver, Yoan Mollard + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Robert Haschke, Sean Yen + +1.0.2 (2019-06-28) +------------------ +* [fix] Fix chomp planner (`#1512 `_) + * Fix start-state handling + * remove time parameterization from planning code +* Contributors: Robert Haschke + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst index d32082addf..949974baea 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst @@ -2,6 +2,50 @@ Changelog for package chomp_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Optional cpp version setting (`#2166 `_) +* [feature] Unified Collision Environment Integration (`#1584 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [fix] Fix calculation of potential (`#1651 `_) +* [fix] Fix Chomp planning adapter (`#1525 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#1419 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Bence Magyar, Dave Coleman, Jens P, Jonathan Binney, Markus Vieth, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Windows build fixes + * Fix header inclusion and other MSVC build errors (`#1636 `_) + * Remove GCC extensions (`#1583 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] Fix possible division-by-zero (`#1809 `_) +* Contributors: Max Krichenbauer, Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ +* [fix] Fix chomp planner (`#1512 `_) + * Fix start-state handling + * remove time parameterization from planning code +* Contributors: Robert Haschke + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst index 5ac9064e2d..b63344237c 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst @@ -2,6 +2,39 @@ Changelog for package moveit_chomp_optimizer_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Optional cpp version setting (`#2166 `_) +* [feature] Allow ROS namespaces for planning request adapters (`#1530 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [fix] Fix Chomp planning adapter (`#1525 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Henning Kayser, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* Contributors: Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ +* [fix] Chomp planning adapter: Fix spurious error message Fix "Found empty JointState message" +* Contributors: Robert Haschke + 1.0.1 (2019-03-08) ------------------ * [fix] segfault in chomp adapter (`#1377 `_) diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst index 68168623d7..aa34f794f2 100644 --- a/moveit_planners/moveit_planners/CHANGELOG.rst +++ b/moveit_planners/moveit_planners/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package moveit_planners ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ + +1.0.6 (2020-08-19) +------------------ + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst index 37a663ac0b..ab987682bf 100644 --- a/moveit_planners/ompl/CHANGELOG.rst +++ b/moveit_planners/ompl/CHANGELOG.rst @@ -2,6 +2,38 @@ Changelog for package moveit_planners_ompl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* [fix] Fix memcpy bug in copyJointToOMPLState in ompl interface (`#2239 `_) +* Contributors: Jeroen, Markus Vieth, Robert Haschke, Michael Görner + +1.0.5 (2020-07-08) +------------------ +* [feature] Added support for hybridize/interpolate flags in ModelBasedPlanningContext via ompl_planning.yaml (`#2171 `_, `#2172 `_) +* Contributors: Constantinos, Mark Moll + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Cleanup OMPL dynamic reconfigure config (`#1649 `_) + * Reduce minimum number of waypoints in solution to 2 +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Michael Görner, Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -79,7 +111,7 @@ Changelog for package moveit_planners_ompl 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * Contributors: Dave Coleman 0.9.4 (2017-02-06) diff --git a/moveit_planners/trajopt/package.xml b/moveit_planners/trajopt/package.xml index 050d71b5d4..87ea353d90 100644 --- a/moveit_planners/trajopt/package.xml +++ b/moveit_planners/trajopt/package.xml @@ -1,12 +1,12 @@ moveit_planners_trajopt - 1.0.1 + 1.1.0 TrajOpt planning plugin, an optimization based motion planner Omid Heidari Omid Heidari - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst index 887619d6ee..a60906cd38 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst @@ -2,6 +2,35 @@ Changelog for package moveit_fake_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [fix] Handle "default" parameter in MoveitControllerManagers + MoveIt{Fake|Simple}ControllerManager::getControllerState() now correctly returns current state +* [fix] Handle incomplete group states +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Robert Haschke, Sean Yen, Yu, Yan, Luca Lach + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -63,7 +92,7 @@ Changelog for package moveit_fake_controller_manager 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst index 4905296463..95539d4b24 100644 --- a/moveit_plugins/moveit_plugins/CHANGELOG.rst +++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package moveit_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Use standard cmake text for metapackages (`#1620 `_) +* [feature] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Jonathan Binney, Robert Haschke, Sean Yen, Tyler Weaver + +1.0.6 (2020-08-19) +------------------ + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ diff --git a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst index d6312849f9..0e26cf41e0 100644 --- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst +++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst @@ -2,6 +2,38 @@ Changelog for package moveit_ros_control_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Optional cpp version setting (`#2166 `_) +* [feature] Remove support for Indigo's ros_control (`#2128 `_) +* [feature] Add support for pos_vel_controllers and pos_vel_acc_controllers (`#1806 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [maint] clang-tidy-fix `modernize-loop-convert` to entire code base (`#1419 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Henning Kayser, Jonathan Binney, Robert Haschke, Sandro Magalhães, Sean Yen, Tyler Weaver, Yu, Yan + +1.0.6 (2020-08-19) +------------------ + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [feature] Add support for pos_vel_controllers and pos_vel_acc_controllers (`#1806 `_) +* Contributors: Robert Haschke, Sandro Magalhães, Sean Yen + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst index 1f699681e5..01fddb343d 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst @@ -2,6 +2,46 @@ Changelog for package moveit_simple_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Optional cpp version setting (`#2166 `_) +* [feature] Allow different controllers for execution `#1832 `_) +* [feature] ControllerManager: wait for done-callback (`#1783 `_) +* [feature] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [fix] Fix binary artifact install locations. (`#1575 `_) +* [fix] add missing space to log (`#1477 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#1419 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Henning Kayser, Jonathan Binney, Leroy Rügemer, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan, llach + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* Contributors: Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [fix] Handle "default" parameter in MoveitControllerManagers + MoveIt{Fake|Simple}ControllerManager::getControllerState() now correctly returns current state +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] `ControllerManager`: wait for done-callback (`#1783 `_) +* Contributors: Robert Haschke, Sean Yen, Luca Lach + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -67,7 +107,7 @@ Changelog for package moveit_simple_controller_manager 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst index 0bfdc8ef5a..c2f0f772f9 100644 --- a/moveit_ros/benchmarks/CHANGELOG.rst +++ b/moveit_ros/benchmarks/CHANGELOG.rst @@ -2,6 +2,59 @@ Changelog for package moveit_ros_benchmarks ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Benchmark combinations of predefined poses (`#1548 `_) +* [feature] Support benchmarking of full planning pipelines (`#1531 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix plot details, correcting xlabels positions and cleaning the graph (`#1658 `_) (`#1668 `_) +* [maint] Optional cpp version setting (`#2166 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#2004 `_, `#1419 `_) +* [maint] Fix usage of panda_moveit_config (`#1904 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Fix compiler warnings (`#1773 `_) +* [maint] Do not install helper scripts in global bin destination (`#1704 `_) +* [maint] Cleanup launch + config files (`#1631 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Dave Coleman, Henning Kayser, Jonathan Binney, Mahmoud Ahmed Selim, Markus Vieth, Michael Görner, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10, Fix warnings +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [feature] MoveIt benchmark improvements (`#1510 `_) + * Add pseudo experiment all_experiments to allow comparing all entries + * Expose loadBenchmarkQueryData() for setting up custom queries + * Add benchmark entry for comparing the 'final' result trajectory + * Add trajectory similarity function to measure repeatability + * Address requested changes + * Fill empty fields in all_experiments + * Improve variable and function names + * Add helper function computeTrajectoryDistance() +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Windows build fixes + * Fix header inclusion and other MSVC build errors (`#1636 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Henning Kayser, Michael Görner, Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ +* [maintenance] Removed unnecessary null pointer checks on deletion (`#1410 `_) +* Contributors: Mahmoud Ahmed Selim + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -69,7 +122,7 @@ Changelog for package moveit_ros_benchmarks 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_ros/manipulation/CHANGELOG.rst b/moveit_ros/manipulation/CHANGELOG.rst index 3a939ead3b..a2af617046 100644 --- a/moveit_ros/manipulation/CHANGELOG.rst +++ b/moveit_ros/manipulation/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package moveit_ros_manipulation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-15) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -70,7 +96,7 @@ Changelog for package moveit_ros_manipulation 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst index b2aa5af725..24e59df9d5 100644 --- a/moveit_ros/move_group/CHANGELOG.rst +++ b/moveit_ros/move_group/CHANGELOG.rst @@ -2,6 +2,49 @@ Changelog for package moveit_ros_move_group ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Start new joint_state_publisher_gui on param use_gui (`#2257 `_) +* [feature] TfPublisher: fixup and add attached collsion objects (`#1792 `_) +* [feature] move_group capability for publishing planning scene frames to the tf system (`#1761 `_) +* [feature] get_planning_scene_service: return full scene when nothing was requested (`#1424 `_) +* [feature] Separate source file for CartesianInterpolator (`#1149 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix TfPublisher subframe publishing (`#2002 `_) +* [maint] Fix compiler warnings (`#1773 `_) +* [maint] clang-tidy fixes (`#2050 `_) (`#2004 `_, `#1419 `_) +* [maint] Optional cpp version setting (`#2166 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Move isEmpty() test functions to moveit_core/utils (`#1627 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Mike Lautman, Robert Haschke, Felix von Drigalski, Jens P, Jonathan Binney, JonasTietz, Michael Görner, Tyler Weaver, Yoan Mollard, Yu, Yan, v4hn + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10 +* Contributors: Robert Haschke, Michael Görner + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Move `get_planning_scene` service into `PlanningSceneMonitor` for reusability (`#1854 `_) +* [maint] Cleanup move_group capabilities (`#1515 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -66,7 +109,7 @@ Changelog for package moveit_ros_move_group 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst index be0815f44c..42f711f6af 100644 --- a/moveit_ros/moveit_ros/CHANGELOG.rst +++ b/moveit_ros/moveit_ros/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package moveit_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [maint] Use standard cmake text for metapackages (`#1620 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Jonathan Binney, Robert Haschke, Sean Yen + +1.0.6 (2020-08-19) +------------------ + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ diff --git a/moveit_ros/moveit_servo/CHANGELOG.rst b/moveit_ros/moveit_servo/CHANGELOG.rst new file mode 100644 index 0000000000..b14a65d9eb --- /dev/null +++ b/moveit_ros/moveit_servo/CHANGELOG.rst @@ -0,0 +1,190 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_servo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.0 (2020-09-04) +------------------ +* [feature] Update last_sent_command\_ at ServoCalcs start (`#2249 `_) +* [feature] Add a utility to print collision pairs (`#2275 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [maint] add soname version to moveit_servo (`#2266 `_) +* [maint] delete python integration tests (`#2186 `_) +* Contributors: AdamPettinger, AndyZe, Robert Haschke, Ruofan Xu, Tyler Weaver, v4hn + +1.0.6 (2020-08-19) +------------------ +* [feature] A ROS service to reset the Servo status (`#2246 `_) +* [feature] Check collisions during joint motions, too (`#2204 `_) +* [fix] Correctly set velocities to zero when stale (`#2255 `_) +* [maint] Remove unused yaml param (`#2232 `_) +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10 +* Contributors: AndyZe, Robert Haschke, Ruofan Xu, Michael Görner + +1.0.5 (2020-07-08) +------------------ +* [maint] Minor moveit_servo header cleanup (`#2173 `_) +* [maint] Move and rename to moveit_ros/moveit_servo (`#2165 `_) +* [maint] Changes before porting to ROS2 (`#2151 `_) + * throttle warning logs + * ROS1 Basic improvements and changes + * Fixes to drift dimensions, singularity velocity scaling + * tf name changes, const fixes, slight logic changes + * Move ROS_LOG_THROTTLE_PERIOD to cpp files + * Track staleness of joint and twist seperately + * Ensure joint_trajectory output is always populated with something, even when no jog + * Fix joint trajectory redundant points for gazebo pub + * Fix crazy joint jog from bad Eigen init + * Fix variable type in addJointIncrements() + * Initialize last sent command in constructor + * More explicit joint_jog_cmd\ and twist_stamped_cmd\ names + * Add comment clarying transform calculation / use +* [fix] Fix access past end of array bug (`#2155 `_) +* [maint] Remove duplicate line (`#2154 `_) +* [maint] pragma once in jog_arm.h (`#2152 `_) +* [feature] Simplify communication between threads (`#2103 `_) + * get latest joint state c++ api + * throttle warning logs + * publish from jog calcs timer, removing redundant timer and internal messaging to main timer + * outgoing message as pool allocated shared pointer for zero copy + * replace jog_arm shared variables with ros pub/sub + * use built in zero copy message passing instead of spsc_queues + * use ros timers instead of threads in jog_arm +* [feature] Added throttle to jogarm accel limit warning (`#2141 `_) +* [feature] Time-based collision avoidance (`#2100 `_) +* [fix] Fix crash on empty jog msgs (`#2094 `_) +* [feature] Jog arm dimensions (`#1724 `_) +* [maint] Clang-tidy fixes (`#2050 `_) +* [feature] Keep updating joints, even while waiting for a valid command (`#2027 `_) +* [fix] Fix param logic bug for self- and scene-collision proximity thresholds (`#2022 `_) +* [feature] Split collision proximity threshold (`#2008 `_) + * separate proximity threshold values for self-collisions and scene collisions + * increase default value of scene collision proximity threshold + * deprecate old parameters +* [fix] Fix valid command flags (`#2013 `_) + * Rename the 'zero command flag' variables for readability + * Reset flags when incoming commands timeout + * Remove debug line, clang format +* [maint] Use default move constructor + assignment operators for MoveItCpp. (`#2004 `_) +* [fix] Fix low-pass filter initialization (`#1982 `_) + * Pause/stop JogArm threads using shared atomic bool variables + * Add pause/unpause flags for jog thread + * Verify valid joints by filtering for active joint models only + * Remove redundant joint state increments + * Wait for initial jog commands in main loop +* [fix] Remove duplicate collision check in JogArm (`#1986 `_) +* [feature] Add a binary collision check (`#1978 `_) +* [feature] Publish more detailed warnings (`#1915 `_) +* [feature] Use wait_for_service() to fix flaky tests (`#1946 `_) +* [maint] Fix versioning (`#1948 `_) +* [feature] SRDF velocity and acceleration limit enforcement (`#1863 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [fix] JogArm C++ API fixes (`#1911 `_) +* [feature] A ROS service to enable task redundancy (`#1855 `_) +* [fix] Fix segfault with uninitialized JogArm thread (`#1882 `_) +* [feature] Add warnings to moveit_jog_arm low pass filter (`#1872 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [fix] Fix initial end effector transform jump (`#1871 `_) +* [feature] Rework the halt msg functionality (`#1868 `_) +* [fix] Various small fixes (`#1859 `_) +* [maint] Improve formatting in comments +* [fix] Prevent a crash at velocity limit (`#1837 `_) +* [feature] Remove scale/joint parameter (`#1838 `_) +* [feature] Pass planning scene monitor into cpp interface (`#1849 `_) +* [maint] Move attribution below license file, standardize with MoveIt (`#1847 `_) +* [maint] Reduce console output warnings (`#1845 `_) +* [fix] Fix command frame transform computation (`#1842 `_) +* [maint] Fix dependencies + catkin_lint issues +* [feature] Update link transforms before calling checkCollision on robot state in jog_arm (`#1825 `_) +* [feature] Add atomic bool flags for terminating JogArm threads gracefully (`#1816 `_) +* [feature] Get transforms from RobotState instead of TF (`#1803 `_) +* [feature] Add a C++ API (`#1763 `_) +* [maint] Fix unused parameter warnings (`#1773 `_) +* [maint] Update license formatting (`#1764 `_) +* [maint] Unify jog_arm package to be C++14 (`#1762 `_) +* [fix] Fix jog_arm segfault (`#1692 `_) +* [fix] Fix double mutex unlock (`#1672 `_) +* [maint] Rename jog_arm->moveit_jog_arm (`#1663 `_) +* [feature] Do not wait for command msg to start spinning (`#1603 `_) +* [maint] Update jog_arm README with rviz config (`#1614 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Separate moveit_experimental packages (`#1606 `_) +* [feature] Use UR5 example (`#1605 `_) +* [feature] Sudden stop for critical issues, filtered deceleration otherwise (`#1468 `_) +* [feature] Change 2nd order Butterworth low pass filter to 1st order (`#1483 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* [feature] JogArm: Remove dependency on move_group node (`#1569 `_) +* [fix] Fix jog arm CI integration test (`#1466 `_) +* [feature] A jogging PR for Melodic. (`#1360 `_) + * Allow for joints in the msg that are not part of the MoveGroup. + * Switching to the Panda robot model for tests. + * Blacklist the test as I can't get it to pass Travis (fine locally). + * Throttling all warnings. Fix build warning re. unit vs int comparison. + * Continue to publish commands even if stationary + * Scale for 'unitless' commands is not tied to publish_period. + * New function name for checkIfJointsWithinBounds() + * Configure the number of msgs to publish when stationary. + * Run jog_calcs at the same rate as the publishing thread. + * Better comments in config file, add spacenav_node dependency + * Add spacenav_node to CMakeLists. +* Contributors: AdamPettinger, AndyZe, Ayush Garg, Dale Koenig, Dave Coleman, Jonathan Binney, Paul Verhoeckx, Henning Kayser, Jafar Abdi, John Stechschulte, Mike Lautman, Robert Haschke, SansoneG, jschleicher, Tyler Weaver, rfeistenauer + +1.0.1 (2019-03-08) +------------------ + +1.0.0 (2019-02-24) +------------------ + +0.10.8 (2018-12-24) +------------------- + +0.10.5 (2018-11-01) +------------------- + +0.10.4 (2018-10-29 19:44) +------------------------- + +0.10.3 (2018-10-29 04:12) +------------------------- + +0.10.2 (2018-10-24) +------------------- + +0.10.1 (2018-05-25) +------------------- + +0.10.0 (2018-05-22) +------------------- + +0.9.11 (2017-12-25) +------------------- + +0.9.10 (2017-12-09) +------------------- + +0.9.9 (2017-08-06) +------------------ + +0.9.8 (2017-06-21) +------------------ + +0.9.7 (2017-06-05) +------------------ + +0.9.6 (2017-04-12) +------------------ + +0.9.5 (2017-03-08) +------------------ + +0.9.4 (2017-02-06) +------------------ + +0.9.3 (2016-11-16) +------------------ + +0.9.2 (2016-11-05) +------------------ + +0.9.1 (2016-10-21) +------------------ diff --git a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst new file mode 100644 index 0000000000..69b65519c8 --- /dev/null +++ b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst @@ -0,0 +1,29 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_ros_occupancy_map_monitor +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.0 (2020-09-04) +------------------ + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* Contributors: Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [fix] Add error message on failure to initialize occupancy map monitor (`#1873 `_) +* [fix] Update occupancy grid when loaded from file (`#1594 `_) +* [maint] Apply clang-tidy fix (`#1394 `_) +* [maint] Windows build fixes + * Fix header inclusion and other MSVC build errors (`#1636 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [maint] move occupancy_map_monitor into its own package (`#1533 `_) +* Contributors: Bjar Ne, Dale Koenig, Raphael Druon, Robert Haschke, Sean Yen, Simon Schmeisser, Yu, Yan, jschleicher diff --git a/moveit_ros/perception/CHANGELOG.rst b/moveit_ros/perception/CHANGELOG.rst index 9df15123be..bdcd077d93 100644 --- a/moveit_ros/perception/CHANGELOG.rst +++ b/moveit_ros/perception/CHANGELOG.rst @@ -2,6 +2,58 @@ Changelog for package moveit_ros_perception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Utilize new geometric_shapes functions to improve performance (`#2038 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix getTransform() (`#2113 `_) +* [fix] depth_image_octomap_updater: correctly set properties of debug images (`#1653 `_) +* [maint] Optional cpp version setting (`#2166 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#2004 `_, `#1419 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] NAMED logging for moveit_ros_perception (`#1897 `_) +* [maint] Fix various build issues on Windows (`#1880 `_) +* [maint] Fix compiler warnings (`#1773 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Bjar Ne, Dale Koenig, Dave Coleman, Henning Kayser, Jonathan Binney, Mahmoud Ahmed Selim, Markus Vieth, Martin Pecka, Matthias Nieuwenhuisen, Michael Görner, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan, jschleicher + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* [maint] Further increase acceptance threshold for mesh-filter test +* [maint] Prefer vendor-specific OpenGL library +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ +* [maint] Fix mesh_filter test (`#2044 `_) +* Contributors: Bjar Ne + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build fixes + * Fix header inclusion and other MSVC build errors (`#1636 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [maint] Allow subclassing of point_containment_filter::ShapeMask. (`#1457 `_) +* [fix] `depth_image_octomap_updater`: reset depth transfer function to standard values (`#1661 `_) +* [fix] `depth_image_octomap_updater`: correctly set properties of debug images (`#1652 `_) +* [maint] Move `occupancy_map_monitor` into its own package (`#1533 `_) +* Contributors: Martin Pecka, Matthias Nieuwenhuisen, Robert Haschke, Sean Yen, Yu, Yan, jschleicher + +1.0.2 (2019-06-28) +------------------ +* [maintenance] Removed unnecessary null pointer checks on deletion (`#1410 `_) +* Contributors: Mahmoud Ahmed Selim + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -79,7 +131,7 @@ Changelog for package moveit_ros_perception 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst index e2bd85fd4d..1fffce8d1a 100644 --- a/moveit_ros/planning/CHANGELOG.rst +++ b/moveit_ros/planning/CHANGELOG.rst @@ -2,6 +2,84 @@ Changelog for package moveit_ros_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Use Eigen::Transform::linear() instead of rotation() (`#1964 `_) +* [feature] Bullet collision detection (`#1839 `_) (`#1504 `_) +* [feature] Allow different controllers for execution (`#1832 `_) +* [feature] Adding continuous collision detection to Bullet (`#1551 `_) +* [feature] plan_execution: refine logging for invalid paths (`#1705 `_) +* [feature] Unified Collision Environment Integration (`#1584 `_) +* [feature] Allow ROS namespaces for planning request adapters (`#1530 `_) +* [feature] Add named frames to CollisionObjects (`#1439 `_) +* [feature] get_planning_scene_service: return full scene when nothing was requested (`#1424 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Initialize zero dynamics in CurrentStateMonitor (`#1883 `_) +* [fix] memory leak (`#1526 `_) +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] partially transition unused test bin to rostest (`#2158 `_) +* [maint] Optional cpp version setting (`#2166 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#2004 `_, `#1419 `_) +* [maint] Fix usage of panda_moveit_config (`#1904 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Adapt cmake for Bullet (`#1744 `_) +* [maint] Readme for speed benchmark (`#1648 `_) +* [maint] Fix compiler warnings (`#1773 `_) +* [maint] Improve variable naming in RobotModelLoader (`#1759 `_) +* [maint] Move isEmpty() test functions to moveit_core/utils (`#1627 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Bianca Homberg, Dave Coleman, Felix von Drigalski, Henning Kayser, Jens P, Jonathan Binney, Markus Vieth, Martin Pecka, Max Krichenbauer, Michael Görner, Robert Haschke, Sean Yen, Simon Schmeisser, Tyler Weaver, Yu, Yan, jschleicher, livanov93, llach + +1.0.6 (2020-08-19) +------------------ +* [fix] Fix segfault in PSM::clearOctomap() (`#2193 `_) +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Henning Kayser, Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ +* [feature] Trajectory Execution: fix check for start state position (`#2157 `_) +* [feature] Improve responsiveness of PlanningSceneDisplay (`#2049 `_) + - PlanningSceneMonitor: increate update frequency from 10Hz to 30Hz + - send RobotState diff if only position changed +* Contributors: Michael Görner, Robert Haschke, Simon Schmeisser + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [fix] `CurrentStateMonitor`: Initialize velocity/effort with unset dynamics +* [fix] Fix spurious warning message (# IK attempts) (`#1876 `_) +* [maint] Move `get_planning_scene` service into `PlanningSceneMonitor` for reusability (`#1854 `_) +* [feature] Forward controller names to TrajectoryExecutionManager +* [fix] Always copy dynamics if enabled in CurrentStateMonitor (`#1676 `_) +* [feature] TrajectoryMonitor: zero sampling frequency disables trajectory recording (`#1542 `_) +* [feature] Add user warning when planning fails with multiple constraints (`#1443 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build fixes + * Fix header inclusion and other MSVC build errors (`#1636 `_) + * Fix binary artifact install locations. (`#1575 `_) + * Favor ros::Duration.sleep over sleep. (`#1634 `_) + * Remove GCC extensions (`#1583 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] Fix potential memory leak in `RDFLoader` (`#1828 `_) + [maint] Use smart pointers to avoid explicit new/delete +* [fix] `TrajectoryExecutionManager`: fix race condition (`#1709 `_) +* [fix] Correctly propagate error if time parameterization fails (`#1562 `_) +* [maint] move `occupancy_map_monitor` into its own package (`#1533 `_) +* [feature] `PlanExecution`: return executed trajectory (`#1538 `_) +* Contributors: Felix von Drigalski, Henning Kayser, Max Krichenbauer, Michael Görner, Robert Haschke, Sean Yen, Yu, Yan, jschleicher, livanov93, Luca Lach + +1.0.2 (2019-06-28) +------------------ +* [fix] Removed MessageFilter for /collision_object messages (`#1406 `_) +* Contributors: Robert Haschke + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -91,7 +169,7 @@ Changelog for package moveit_ros_planning 0.9.9 (2017-08-06) ------------------ * [fix] Change getCurrentExpectedTrajectory index so collision detection is still performed even if the path timing is not known (`#550 `_) -* [fix] Support for MultiDoF only trajectories `#553 `_ +* [fix] Support for MultiDoF only trajectories `#553 `_ * [fix] ros_error macro name (`#544 `_) * [fix] check plan size for plan length=0 `#535 `_ * Contributors: Cyrille Morin, Michael Görner, Mikael Arguedas, Notou, Unknown diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst index 2e0e72f5f1..d72c3b44a2 100644 --- a/moveit_ros/planning_interface/CHANGELOG.rst +++ b/moveit_ros/planning_interface/CHANGELOG.rst @@ -2,6 +2,92 @@ Changelog for package moveit_ros_planning_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Use Eigen::Transform::linear() instead of rotation() (`#1964 `_) +* [feature] move_group pick place test (`#2031 `_) +* [feature] Check for grasp service - general cleanup MGI (`#2077 `_) +* [feature] Integration test to defend subframe tutorial (`#1757 `_) +* [feature] Release Python GIL for C++ calls (`#1947 `_) +* [feature] Add default velocity/acceleration scaling factors (`#1890 `_) +* [feature] Improve move_group_interface's const correctness (`#1715 `_) +* [feature] Add get_jacobian_matrix to moveit_commander (`#1501 `_) +* [feature] Add named frames to CollisionObjects (`#1439 `_) +* [feature] Added GILRelease to pick and place (`#2272 `_) +* [feature] Add missing variants of place from list of PlaceLocations and Poses in the python interface (`#2231 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Resolve PSI lock-up in RViz display (`#1951 `_) +* [fix] Fix flaky moveit_cpp test (`#1781 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [maint] Fix a parameter mix-up in moveit_cpp loading (`#2187 `_) +* [maint] Optional cpp version setting (`#2166 `_) +* [maint] update dependencies for python3 in noetic (`#2131 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#1586 `_, `#1419 `_) +* [maint] Fix some clang tidy issues (`#2004 `_) +* [maint] export moveit_py_bindings_tools library (`#1970 `_) +* [maint] Fix usage of panda_moveit_config (`#1904 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Fix typo in cmake file (`#1857 `_) +* [maint] Reduce console output warnings (`#1845 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* [maint] improve [get|set]JointValueTarget in python wrapper (`#858 `_) +* [maint] moveit_commander.MoveGroupInterface.plan() to better align with C++ MoveGroup::plan() (`#790 `_) +* Contributors: AndyZe, Ayush Garg, Bence Magyar, Bjar Ne, Dave Coleman, Felix von Drigalski, Gerard Canal, Guilhem Saurel, Henning Kayser, Jafar Abdi, JafarAbdi, Jere Liukkonen, Jonathan Binney, Kunal Tyagi, Luca Rinelli, Mahmoud Ahmed Selim, Markus Vieth, Martin Pecka, Masaki Murooka, Michael Ferguson, Michael Görner, Niklas Fiedler, Robert Haschke, Ryosuke Tajima, Sean Yen, Tyler Weaver, Yeshwanth, Yu, Yan, mvieth, v4hn + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10, Fix warnings +* [maint] Optimize includes (`#2229 `_) +* [feature] Exposed parameter wait_for_servers and getPlannerId() API in MoveGroup's Python API (`#2201 `_) +* Contributors: Gerard Canal, Markus Vieth, Robert Haschke, Michael Görner + +1.0.5 (2020-07-08) +------------------ +* [maint] Remove dependency on panda_moveit_config (#2194 `_, #2197 `_) +* [maint] Adapt linking to eigenpy (`#2118 `_) +* [maint] Replace robot_model and robot_state namespaces with moveit::core (`#2135 `_) +* [feature] PlanningComponent: Load plan_request_params (`#2033 `_) +* [feature] MoveItCpp: a high-level C++ planning API (`#1656 `_) +* [fix] Validate action client pointer before access +* [fix] Wait and check for the grasp service +* [maint] Add tests for move_group interface (`#1995 `_) +* Contributors: AndyZe, Henning Kayser, Jafar Abdi, Michael Görner, Robert Haschke, Tyler Weaver, Yeshwanth + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [feature] `MoveGroupInterface`: Add execution methods for moveit_msgs::RobotTrajectory (`#1955 `_) +* [feature] Allow to instantiate a `PlanningSceneInterface` w/ and w/o a running `move_group` node +* [fix] Release Python `GIL` for C++ calls (`#1947 `_) +* [feature] Expose reference_point_position parameter in getJacobian() (`#1595 `_) +* [feature] `MoveGroupInterface`: Expose `constructPickGoal` and `constructPlaceGoal` (`#1498 `_) +* [feature] `python MoveGroupInterface`: Added custom time limit for `wait_for_servers()` (`#1444 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Improve Python 3 compatibility (`#1870 `_) + * Replaced StringIO with BytesIO for python msg serialization + * Use py_bindings_tools::ByteString as byte-based serialization buffer on C++ side +* [feature] Export moveit_py_bindings_tools library +* [maint] Fix various build issues on Windows + * Use `.pyd` as the output suffix for Python module on Windows. (`#1637 `_) + * Favor ros::Duration.sleep over sleep. (`#1634 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [maint] Updated deprecation method: MOVEIT_DEPRECATED -> [[deprecated]] (`#1748 `_) +* [maint] `eigenpy`: switched to system package (`#1737 `_) +* [featue] `PlanningSceneInterface`: wait for its two services +* [feature] Select time parametrization algorithm in retime_trajectory (`#1508 `_) +* Contributors: Bjar Ne, Felix von Drigalski, Kunal Tyagi, Luca Rinelli, Masaki Murooka, Michael Görner, Niklas Fiedler, Robert Haschke, Sean Yen, Yu, Yan, mvieth, v4hn + +1.0.2 (2019-06-28) +------------------ +* [maintenance] Removed unnecessary null pointer checks on deletion (`#1410 `_) +* Contributors: Mahmoud Ahmed Selim + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -91,7 +177,7 @@ Changelog for package moveit_ros_planning_interface 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * [enhancement][MoveGroup] Add getLinkNames function (`#440 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst index 9851f4f460..927f105b7e 100644 --- a/moveit_ros/robot_interaction/CHANGELOG.rst +++ b/moveit_ros/robot_interaction/CHANGELOG.rst @@ -2,6 +2,40 @@ Changelog for package moveit_ros_robot_interaction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Optional cpp version setting (`#2166 `_) +* [fix] Various fixes for upcoming Noetic release `#2180 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#1419 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Dave Coleman, Henning Kayser, Jonathan Binney, Markus Vieth, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -80,7 +114,7 @@ Changelog for package moveit_ros_robot_interaction 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst index 4e24d61666..9257a6de63 100644 --- a/moveit_ros/visualization/CHANGELOG.rst +++ b/moveit_ros/visualization/CHANGELOG.rst @@ -2,6 +2,66 @@ Changelog for package moveit_ros_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ + +1.0.6 (2020-08-19) +------------------ +* [feature] MP display: add units to joints tab (`#2264 `_) +* [feature] Allow adding planning scene shapes from rviz panel (`#2198 `_) +* [feature] Default to Planning tab initially (`#2061 `_) +* [fix] Fix deferred robot model loading (`#2245 `_) +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Jorge Nicho, Markus Vieth, Michael Görner, Robert Haschke, Michael Görner + +1.0.5 (2020-07-08) +------------------ +* [feature] Improve rviz GUI to add PlanningScene objects. Ask for scaling large meshes. (`#2142 `_) +* [maint] Replace robot_model and robot_state namespaces with moveit::core (`#2135 `_) +* [maint] Fix catkin_lint issues (`#2120 `_) +* [feature] PlanningSceneDisplay speedup (`#2049 `_) +* [feature] Added support for PS4 joystick (`#2060 `_) +* [fix] MP display: planning attempts are natural numbers (`#2076 `_, `#2082 `_) +* Contributors: Felix von Drigalski, Henning Kayser, Jafar Abdi, Michael Görner, Robert Haschke, Simon Schmeisser, TrippleBender + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [fix] `MotionPlanningDisplay`: change internal shortcut Ctrl+R to Ctrl+I (`#1967 `_) +* [fix] Remove `PlanningSceneInterface` from rviz display, but use its `PlanningSceneMonitor` instead +* [fix] Fix segfault in `RobotStateVisualization` (`#1941 `_) +* [feature] Provide visual feedback on success of requestPlanningSceneState() +* [feature] Wait for `get_planning_scene` in background (`#1934 `_) +* [feature] Reduce step size for pose-adapting widgets +* [fix] Reset `scene_marker` when disabling motion planning panel +* [fix] Enable/disable motion planning panel with display +* [fix] Enable/disable pose+scale group box when collision object is selected/deselected +* [fix] Correctly populate the list of scene objects in the motion planning panel +* [feature] Resize scene marker with collision object +* [feature] Show attached bodies in trajectory trail (`#1766 `_) +* [fix] Fix `REALTIME` trajectory playback (`#1683 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Notice changes in rviz planning panel requiring saving (`#1991 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Improve Python 3 compatibility (`#1870 `_) + * Replaced StringIO with BytesIO for python msg serialization + * Use py_bindings_tools::ByteString as byte-based serialization buffer on C++ side +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] Fix pruning of enclosed nodes when rendering octomap in RViz (`#1685 `_) +* [fix] Fix missing `scene_manager` initialization in OcTreeRender's constructor (`#1817 `_) +* [feature] new `Joints` tab in RViz motion panel (`#1308 `_) +* [feature] Add `` robot state to RViz motion panel (`#1742 `_) +* Contributors: Bjar Ne, Dale Koenig, MarqRazz, Max Krichenbauer, Michael Görner, Robert Haschke, RyodoTanaka, Sean Yen, Takara Kasai, Yannick Jonetzko, Yu, Yan, v4hn + +1.0.2 (2019-06-28) +------------------ +* [maintenance] Removed unnecessary null pointer checks on deletion (`#1410 `_) +* Contributors: Mahmoud Ahmed Selim + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -113,8 +173,8 @@ Changelog for package moveit_ros_visualization 0.9.5 (2017-03-08) ------------------ -* [fix] correct "simplify widget handling" `#452 `_ This reverts "simplify widget handling (`#442 `_)" -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix] correct "simplify widget handling" `#452 `_ This reverts "simplify widget handling (`#442 `_)" +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman, Isaac I.Y. Saito, Yannick Jonetzko diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst index ee636c4c9d..c3d94b7f33 100644 --- a/moveit_ros/warehouse/CHANGELOG.rst +++ b/moveit_ros/warehouse/CHANGELOG.rst @@ -2,6 +2,41 @@ Changelog for package moveit_ros_warehouse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Optional cpp version setting (`#2166 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#1419 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Dave Coleman, Henning Kayser, Jonathan Binney, Max Krichenbauer, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* Contributors: Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] Fix potential use of uninitialized variable (`#1818 `_) +* Contributors: Max Krichenbauer, Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -65,7 +100,7 @@ Changelog for package moveit_ros_warehouse 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * Contributors: Dave Coleman 0.9.4 (2017-02-06) diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst index bb959b1866..ad382bb465 100644 --- a/moveit_runtime/CHANGELOG.rst +++ b/moveit_runtime/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_runtime ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Use standard cmake text for metapackages (`#1620 `_) +* [feature] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Jonathan Binney, Robert Haschke, Sean Yen, Tyler Weaver + 1.0.1 (2019-03-08) ------------------ diff --git a/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/CHANGELOG.rst index dba4173b1f..a38e7f88a7 100644 --- a/moveit_setup_assistant/CHANGELOG.rst +++ b/moveit_setup_assistant/CHANGELOG.rst @@ -2,6 +2,76 @@ Changelog for package moveit_setup_assistant ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-09-04) +------------------ +* [feature] Start new joint_state_publisher_gui on param use_gui (`#2257 `_) +* [feature] Optional cpp version setting (`#2166 `_) +* [feature] Add default velocity/acceleration scaling factors (`#1890 `_) +* [feature] MSA: use matching group/state name for default controller state (`#1936 `_) +* [feature] MSA: Restore display of current directory (`#1932 `_) +* [feature] Cleanup: use range-based for-loop (`#1830 `_) +* [feature] Add delete process to the doneEditing() function in end_effectors_widgets (`#1829 `_) +* [feature] Fix Rviz argument in demo_gazebo.launch (`#1797 `_) +* [feature] Allow user to specify planner termination condition. (`#1695 `_) +* [feature] Add OMPL planner 'AnytimePathShortening' (`#1686 `_) +* [feature] MVP TrajOpt Planner Plugin (`#1593 `_) +* [feature] Use QDir::currentPath() rather than getenv("PWD") (`#1618 `_) +* [feature] Add named frames to CollisionObjects (`#1439 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix ordering of request adapters (`#2053 `_) +* [fix] Fix some clang tidy issues (`#2004 `_) +* [fix] Fix usage of panda_moveit_config (`#1904 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [fix] Use portable string() on filesystem::path. (`#1571 `_) +* [fix] Fix test utilities in moveit core (`#1409 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#1419 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* [maint] remove obsolete moveit_resources/config.h (`#1412 `_) +* Contributors: AndyZe, Ayush Garg, Daniel Wang, Dave Coleman, Felix von Drigalski, Henning Kayser, Jafar Abdi, Jonathan Binney, Mark Moll, Max Krichenbauer, Michael Görner, Mike Lautman, Mohmmad Ayman, Omid Heidari, Robert Haschke, Sandro Magalhães, Sean Yen, Simon Schmeisser, Tejas Kumar Shastha, Tyler Weaver, Yoan Mollard, Yu, Yan, jschleicher, tnaka, v4hn + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10, fix warnings +* [fix] Define planning adapters for chomp planning pipeline (`#2242 `_) +* [maint] Remove urdf package as build_depend from package.xml (`#2207 `_) +* Contributors: Jafar Abdi, Robert Haschke, tnaka, Michael Görner + +1.0.5 (2020-07-08) +------------------ +* [fix] Fix catkin_lint issues (`#2120 `_) +* [feature] Add use_rviz to demo.launch in setup_assistant (`#2019 `_) +* Contributors: Henning Kayser, Jafar Abdi, Michael Görner, Robert Haschke, Tyler Weaver + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [feature] Allow loading of additional kinematics parameters file (`#1997 `_) +* [feature] Allow adding initial poses to fake_controllers.yaml (`#1892 `_) +* [feature] Display robot poses on selection, not only on click (`#1930 `_) +* [fix] Fix invalid iterator (`#1623 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build fixes + * Fix header inclusion and other MSVC build errors (`#1636 `_) + * Fix binary artifact install locations. (`#1575 `_) + * Favor ros::Duration.sleep over sleep. (`#1634 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [feature] Add support for pos_vel_controllers and pos_vel_acc_controllers (`#1806 `_) +* [feature] Add joint state controller config by default (`#1024 `_) +* Contributors: AndyZe, Daniel Wang, Felix von Drigalski, Jafar Abdi, Max Krichenbauer, Michael Görner, Mohmmad Ayman, Robert Haschke, Sandro Magalhães, Sean Yen, Simon Schmeisser, Tejas Kumar Shastha, Yu, Yan, v4hn + +1.0.2 (2019-06-28) +------------------ +* [fix] static transform publisher does not take a rate (`#1494 `_) +* [feature] Add arguments `load_robot_description`, `pipeline`, `rviz config_file` to launch file templates (`#1397 `_) +* Contributors: Mike Lautman, Robert Haschke, jschleicher + 1.0.1 (2019-03-08) ------------------ * [fix] re-add required build dependencies (`#1373 `_) @@ -105,7 +175,7 @@ Changelog for package moveit_setup_assistant 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * Contributors: Dave Coleman 0.9.4 (2017-02-06) @@ -131,7 +201,7 @@ Changelog for package moveit_setup_assistant 0.7.1 (2016-06-24) ------------------ -* [sys] Qt adjustment. +* [sys] Qt adjustment. * relax Qt-version requirement. Minor Qt version updates are ABI-compatible with each other: https://wiki.qt.io/Qt-Version-Compatibility * auto-select Qt version matching the one from rviz `#114 `_ * Allow to conditionally compile against Qt5 by setting -DUseQt5=On diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml index 3bba189470..e298058e5a 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml @@ -1,6 +1,6 @@ - + - + @@ -17,7 +17,7 @@ - + From 736bb83d94489c1a5cbb4d0a71baf2384c60e2b9 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 12 Sep 2020 04:27:34 -0600 Subject: [PATCH 444/612] [maint] Remove patch status from codecov (#2306) --- codecov.yaml | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/codecov.yaml b/codecov.yaml index 91bb43379c..3999cdb452 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -7,7 +7,4 @@ coverage: default: target: auto threshold: 5% - patch: - default: - target: auto - threshold: 5% + patch: off From 089f983de9ab4903f40c12e4a199a253e1e43e4c Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Mon, 14 Sep 2020 04:06:02 +0900 Subject: [PATCH 445/612] Update codeowners file (#2309) --- .github/CODEOWNERS | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index f67b645235..3ccad28aee 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -51,12 +51,12 @@ /moveit_core/kinematics_base/ @rhaschke @mlautman /moveit_core/kinematics_metrics/ @gavanderhoorn /moveit_core/macros/ @tylerjw -/moveit_core/planning_interface/ @rhaschke -/moveit_core/planning_request_adapter/ @rhaschke -/moveit_core/planning_scene/ @rhaschke +/moveit_core/planning_interface/ @rhaschke @felixvd +/moveit_core/planning_request_adapter/ @rhaschke +/moveit_core/planning_scene/ @rhaschke @felixvd /moveit_core/profiler/ @tylerjw /moveit_core/robot_model/ @tylerjw -/moveit_core/robot_state/ @rhaschke @mlautman +/moveit_core/robot_state/ @rhaschke @mlautman @felixvd /moveit_core/robot_trajectory/ @mlautman /moveit_core/sensor_manager/ @mlautman /moveit_core/trajectory_processing/ @mlautman @@ -75,11 +75,11 @@ /moveit_ros/perception/ @jliukkonen @RoboticsYY /moveit_ros/manipulation/ @v4hn @felixvd /moveit_ros/benchmarks/ @henningkayser @MohmadAyman -/moveit_ros/planning_interface/ @mintar @rhaschke +/moveit_ros/planning_interface/ @mintar @rhaschke @felixvd /moveit_ros/robot_interaction/ @mikeferguson @rhaschke /moveit_ros/warehouse/ @mikeferguson @dg-shadow /moveit_ros/move_group/ @rhaschke @IanTheEngineer -/moveit_ros/visualization/ @rhaschke @tylerjw @RoboticsYY +/moveit_ros/visualization/ @rhaschke @tylerjw @RoboticsYY @felixvd /moveit_ros/planning/collision_plugin_loader/ @j-petit /moveit_ros/planning/constraint_sampler_manager_loader/ @nbbrooks From a3516e6ebcdb5b27c18945c7928dc1284d09fbf3 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sun, 13 Sep 2020 14:02:14 -0600 Subject: [PATCH 446/612] [fix] Servo heap-buffer-overflow bug (#2307) --- moveit_ros/moveit_servo/src/servo_calcs.cpp | 22 +++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 5e756697f5..c28a36fd0b 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -37,6 +37,8 @@ * Author : Brian O'Neil, Andy Zelenak, Blake Anderson */ +#include + #include #include @@ -796,22 +798,26 @@ bool ServoCalcs::enforceSRDFPositionLimits() // Is handled differently for position vs. velocity control. void ServoCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory) { - if (joint_trajectory.points.empty()) - { - joint_trajectory.points.push_back(trajectory_msgs::JointTrajectoryPoint()); - joint_trajectory.points[0].positions.resize(num_joints_); - joint_trajectory.points[0].velocities.resize(num_joints_); - } + // Prepare the joint trajectory message to stop the robot + joint_trajectory.points.clear(); + joint_trajectory.points.emplace_back(); + trajectory_msgs::JointTrajectoryPoint& point = joint_trajectory.points.front(); + point.positions.resize(num_joints_); + point.velocities.resize(num_joints_); + + // Assert the following loop is safe to execute + assert(original_joint_state_.position.size() >= num_joints_); + // Set the positions and velocities vectors for (std::size_t i = 0; i < num_joints_; ++i) { // For position-controlled robots, can reset the joints to a known, good state if (parameters_.publish_joint_positions) - joint_trajectory.points[0].positions[i] = original_joint_state_.position[i]; + point.positions[i] = original_joint_state_.position[i]; // For velocity-controlled robots, stop if (parameters_.publish_joint_velocities) - joint_trajectory.points[0].velocities[i] = 0; + point.velocities[i] = 0; } } From 802e596a2283b64f4582d802c5f79e1f3d57def0 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Tue, 15 Sep 2020 15:36:06 +0900 Subject: [PATCH 447/612] Add comment to MOVEIT_CLASS_FORWARD (#2315) --- .../moveit/collision_detection/collision_common.h | 2 +- .../collision_detector_allocator.h | 2 +- .../moveit/collision_detection/collision_env.h | 2 +- .../moveit/collision_detection/collision_matrix.h | 2 +- .../moveit/collision_detection/collision_plugin.h | 2 +- .../include/moveit/collision_detection/world.h | 4 ++-- .../include/moveit/collision_detection/world_diff.h | 2 +- .../collision_distance_field_types.h | 2 +- .../collision_env_distance_field.h | 2 +- .../moveit/constraint_samplers/constraint_sampler.h | 2 +- .../constraint_sampler_allocator.h | 2 +- .../constraint_samplers/constraint_sampler_manager.h | 2 +- .../default_constraint_samplers.h | 4 ++-- .../moveit/controller_manager/controller_manager.h | 4 ++-- .../include/moveit/distance_field/distance_field.h | 4 ++-- .../include/moveit/dynamics_solver/dynamics_solver.h | 2 +- .../kinematic_constraints/kinematic_constraint.h | 12 ++++++------ .../include/moveit/kinematics_base/kinematics_base.h | 2 +- .../moveit/kinematics_metrics/kinematics_metrics.h | 2 +- .../moveit/planning_interface/planning_interface.h | 6 +++--- .../planning_request_adapter.h | 2 +- .../include/moveit/planning_scene/planning_scene.h | 2 +- .../include/moveit/robot_model/link_model.h | 2 +- .../include/moveit/robot_model/robot_model.h | 2 +- .../include/moveit/robot_state/robot_state.h | 2 +- .../moveit/robot_trajectory/robot_trajectory.h | 2 +- .../include/moveit/sensor_manager/sensor_manager.h | 2 +- .../include/moveit/transforms/transforms.h | 2 +- .../include/chomp_interface/chomp_interface.h | 2 +- .../include/chomp_interface/chomp_planning_context.h | 2 +- .../OMPLDynamicReconfigureConfig.h | 4 ++-- .../detail/constrained_valid_state_sampler.h | 2 +- .../ompl_interface/detail/constraints_library.h | 2 +- .../ompl_interface/model_based_planning_context.h | 4 ++-- .../model_based_state_space_factory.h | 2 +- .../include/trajopt_interface/problem_description.h | 10 +++++----- .../include/trajopt_interface/trajopt_interface.h | 2 +- .../trajopt_interface/trajopt_planning_context.h | 2 +- .../src/moveit_fake_controllers.h | 2 +- .../moveit_ros_control_interface/ControllerHandle.h | 2 +- .../src/controller_manager_plugin.cpp | 2 +- .../action_based_controller_handle.h | 3 ++- .../include/moveit/pick_place/manipulation_stage.h | 2 +- .../include/moveit/pick_place/pick_place.h | 6 +++--- .../moveit/move_group/move_group_capability.h | 2 +- .../include/moveit/move_group/move_group_context.h | 10 +++++----- .../occupancy_map_monitor/occupancy_map_updater.h | 2 +- .../include/moveit/mesh_filter/filter_job.h | 2 +- .../include/moveit/mesh_filter/gl_renderer.h | 2 +- .../include/moveit/mesh_filter/mesh_filter_base.h | 4 ++-- .../include/moveit/mesh_filter/sensor_model.h | 2 +- .../include/moveit/mesh_filter/transform_provider.h | 2 +- .../include/moveit/semantic_world/semantic_world.h | 4 ++-- .../collision_plugin_loader.h | 2 +- .../constraint_sampler_manager_loader.h | 3 ++- .../kinematics_plugin_loader.h | 4 ++-- .../include/moveit/plan_execution/plan_execution.h | 2 +- .../moveit/plan_execution/plan_with_sensing.h | 2 +- .../moveit/planning_pipeline/planning_pipeline.h | 2 +- .../planning_scene_monitor/current_state_monitor.h | 2 +- .../planning_scene_monitor/planning_scene_monitor.h | 2 +- .../planning_scene_monitor/trajectory_monitor.h | 2 +- .../include/moveit/rdf_loader/rdf_loader.h | 2 +- .../moveit/robot_model_loader/robot_model_loader.h | 2 +- .../trajectory_execution_manager.h | 2 +- .../move_group_interface/move_group_interface.h | 2 +- .../include/moveit/moveit_cpp/moveit_cpp.h | 2 +- .../include/moveit/moveit_cpp/planning_component.h | 2 +- .../planning_scene_interface.h | 2 +- .../moveit/robot_interaction/interaction_handler.h | 6 +++--- .../moveit/robot_interaction/locked_robot_state.h | 2 +- .../moveit/robot_interaction/robot_interaction.h | 6 +++--- .../motion_planning_frame.h | 6 +++--- .../motion_planning_param_widget.h | 2 +- .../rviz_plugin_render_tools/planning_scene_render.h | 6 +++--- .../moveit/rviz_plugin_render_tools/render_shapes.h | 4 ++-- .../robot_state_visualization.h | 4 ++-- .../trajectory_visualization.h | 2 +- .../include/moveit/warehouse/constraints_storage.h | 2 +- .../moveit/warehouse/planning_scene_storage.h | 2 +- .../include/moveit/warehouse/state_storage.h | 2 +- .../warehouse/trajectory_constraints_storage.h | 2 +- .../tools/compute_default_collisions.h | 2 +- .../setup_assistant/tools/moveit_config_data.h | 2 +- 84 files changed, 122 insertions(+), 120 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index b10ce1a479..93d594de29 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -47,7 +47,7 @@ namespace collision_detection { -MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); +MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc /** \brief The types of bodies that are considered for collision */ namespace BodyTypes diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h index f428066b42..05df2799a0 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h @@ -41,7 +41,7 @@ namespace collision_detection { -MOVEIT_CLASS_FORWARD(CollisionDetectorAllocator); +MOVEIT_CLASS_FORWARD(CollisionDetectorAllocator); // Defines CollisionDetectorAllocatorPtr, ConstPtr, WeakPtr... etc /** \brief An allocator for a compatible CollisionWorld/CollisionRobot pair. */ class CollisionDetectorAllocator diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h index 0b76908fac..0dc01a5a10 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h @@ -45,7 +45,7 @@ namespace collision_detection { -MOVEIT_CLASS_FORWARD(CollisionEnv); +MOVEIT_CLASS_FORWARD(CollisionEnv); // Defines CollisionEnvPtr, ConstPtr, WeakPtr... etc /** \brief Provides the interface to the individual collision checking libraries. */ class CollisionEnv diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index 3438fcd469..3072591919 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -71,7 +71,7 @@ enum Type * CONDITIONAL) */ typedef boost::function DecideContactFn; -MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); +MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc /** @class AllowedCollisionMatrix * @brief Definition of a structure for the allowed collision matrix. All elements in the collision world are referred diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h index 320265eb6b..0873e6a816 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h @@ -40,7 +40,7 @@ namespace collision_detection { -MOVEIT_CLASS_FORWARD(CollisionPlugin); +MOVEIT_CLASS_FORWARD(CollisionPlugin); // Defines CollisionPluginPtr, ConstPtr, WeakPtr... etc /** * @brief Plugin API for loading a custom collision detection robot/world. diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index e27502427d..6113dd6172 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -48,12 +48,12 @@ namespace shapes { -MOVEIT_CLASS_FORWARD(Shape); +MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc } namespace collision_detection { -MOVEIT_CLASS_FORWARD(World); +MOVEIT_CLASS_FORWARD(World); // Defines WorldPtr, ConstPtr, WeakPtr... etc /** \brief Maintain a representation of the environment */ class World diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h index 77f851e330..9660d2dbce 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h @@ -41,7 +41,7 @@ namespace collision_detection { -MOVEIT_CLASS_FORWARD(WorldDiff); +MOVEIT_CLASS_FORWARD(WorldDiff); // Defines WorldDiffPtr, ConstPtr, WeakPtr... etc /** \brief Maintain a diff list of changes that have happened to a World. */ class WorldDiff diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index e66b9b0215..65601df003 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -109,7 +109,7 @@ struct GradientInfo }; MOVEIT_CLASS_FORWARD(PosedDistanceField) -MOVEIT_CLASS_FORWARD(BodyDecomposition); +MOVEIT_CLASS_FORWARD(BodyDecomposition); // Defines BodyDecompositionPtr, ConstPtr, WeakPtr... etc MOVEIT_CLASS_FORWARD(PosedBodySphereDecomposition) MOVEIT_CLASS_FORWARD(PosedBodyPointDecomposition) MOVEIT_CLASS_FORWARD(PosedBodySphereDecompositionVector) diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h index a1f910d359..fc395a7463 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h @@ -53,7 +53,7 @@ static const double DEFAULT_RESOLUTION = .02; static const double DEFAULT_COLLISION_TOLERANCE = 0.0; static const double DEFAULT_MAX_PROPOGATION_DISTANCE = .25; -MOVEIT_CLASS_FORWARD(CollisionEnvDistanceField); +MOVEIT_CLASS_FORWARD(CollisionEnvDistanceField); // Defines CollisionEnvDistanceFieldPtr, ConstPtr, WeakPtr... etc class CollisionEnvDistanceField : public CollisionEnv { diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h index c7d24b3741..40821f395e 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h @@ -50,7 +50,7 @@ */ namespace constraint_samplers { -MOVEIT_CLASS_FORWARD(ConstraintSampler); +MOVEIT_CLASS_FORWARD(ConstraintSampler); // Defines ConstraintSamplerPtr, ConstPtr, WeakPtr... etc /** * \brief ConstraintSampler is an abstract base class that allows the diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h index eb3d07fa07..8f992ed9d4 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h @@ -41,7 +41,7 @@ namespace constraint_samplers { -MOVEIT_CLASS_FORWARD(ConstraintSamplerAllocator); +MOVEIT_CLASS_FORWARD(ConstraintSamplerAllocator); // Defines ConstraintSamplerAllocatorPtr, ConstPtr, WeakPtr... etc class ConstraintSamplerAllocator { diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h index cfcd2f2ffe..24f4334410 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h @@ -41,7 +41,7 @@ namespace constraint_samplers { -MOVEIT_CLASS_FORWARD(ConstraintSamplerManager); +MOVEIT_CLASS_FORWARD(ConstraintSamplerManager); // Defines ConstraintSamplerManagerPtr, ConstPtr, WeakPtr... etc /** * \brief This class assists in the generation of a ConstraintSampler for a diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h index 2969c3f95a..062c98587c 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h @@ -42,7 +42,7 @@ namespace constraint_samplers { -MOVEIT_CLASS_FORWARD(JointConstraintSampler); +MOVEIT_CLASS_FORWARD(JointConstraintSampler); // Defines JointConstraintSamplerPtr, ConstPtr, WeakPtr... etc /** * \brief JointConstraintSampler is a class that allows the sampling @@ -279,7 +279,7 @@ struct IKSamplingPose orientation_constraint_; /**< \brief Holds the orientation constraint for sampling */ }; -MOVEIT_CLASS_FORWARD(IKConstraintSampler); +MOVEIT_CLASS_FORWARD(IKConstraintSampler); // Defines IKConstraintSamplerPtr, ConstPtr, WeakPtr... etc /** * \brief A class that allows the sampling of IK constraints. diff --git a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h index 18490b75d8..48abe31adb 100644 --- a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h +++ b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h @@ -98,7 +98,7 @@ struct ExecutionStatus Value status_; }; -MOVEIT_CLASS_FORWARD(MoveItControllerHandle); +MOVEIT_CLASS_FORWARD(MoveItControllerHandle); // Defines MoveItControllerHandlePtr, ConstPtr, WeakPtr... etc /** \brief MoveIt sends commands to a controller via a handle that satisfies this interface. */ class MoveItControllerHandle @@ -146,7 +146,7 @@ class MoveItControllerHandle std::string name_; }; -MOVEIT_CLASS_FORWARD(MoveItControllerManager); +MOVEIT_CLASS_FORWARD(MoveItControllerManager); // Defines MoveItControllerManagerPtr, ConstPtr, WeakPtr... etc /** @brief MoveIt does not enforce how controllers are implemented. To make your controllers usable by MoveIt, this interface needs to be implemented. diff --git a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h index 19af4573a8..b5f516c0bf 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h @@ -47,7 +47,7 @@ namespace shapes { -MOVEIT_CLASS_FORWARD(Shape); +MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc } namespace octomap { @@ -71,7 +71,7 @@ enum PlaneVisualizationType YZ_PLANE }; -MOVEIT_CLASS_FORWARD(DistanceField); +MOVEIT_CLASS_FORWARD(DistanceField); // Defines DistanceFieldPtr, ConstPtr, WeakPtr... etc /** * \brief DistanceField is an abstract base class for computing diff --git a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h index 6e46a929c5..a2db344a52 100644 --- a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h +++ b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h @@ -49,7 +49,7 @@ /** \brief This namespace includes the dynamics_solver library */ namespace dynamics_solver { -MOVEIT_CLASS_FORWARD(DynamicsSolver); +MOVEIT_CLASS_FORWARD(DynamicsSolver); // Defines DynamicsSolverPtr, ConstPtr, WeakPtr... etc /** * This solver currently computes the required torques given a diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index 314b1683ce..cc634c4019 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -71,7 +71,7 @@ struct ConstraintEvaluationResult double distance; /**< \brief The distance evaluation from the constraint or constraints */ }; -MOVEIT_CLASS_FORWARD(KinematicConstraint); +MOVEIT_CLASS_FORWARD(KinematicConstraint); // Defines KinematicConstraintPtr, ConstPtr, WeakPtr... etc /// \brief Base class for representing a kinematic constraint class KinematicConstraint @@ -178,7 +178,7 @@ class KinematicConstraint distance computed by the decide() function */ }; -MOVEIT_CLASS_FORWARD(JointConstraint); +MOVEIT_CLASS_FORWARD(JointConstraint); // Defines JointConstraintPtr, ConstPtr, WeakPtr... etc /** * \brief Class for handling single DOF joint constraints. @@ -331,7 +331,7 @@ class JointConstraint : public KinematicConstraint double joint_position_, joint_tolerance_above_, joint_tolerance_below_; /**< \brief Position and tolerance values*/ }; -MOVEIT_CLASS_FORWARD(OrientationConstraint); +MOVEIT_CLASS_FORWARD(OrientationConstraint); // Defines OrientationConstraintPtr, ConstPtr, WeakPtr... etc /** * \brief Class for constraints on the orientation of a link @@ -490,7 +490,7 @@ class OrientationConstraint : public KinematicConstraint absolute_z_axis_tolerance_; /**< \brief Storage for the tolerances */ }; -MOVEIT_CLASS_FORWARD(PositionConstraint); +MOVEIT_CLASS_FORWARD(PositionConstraint); // Defines PositionConstraintPtr, ConstPtr, WeakPtr... etc /** * \brief Class for constraints on the XYZ position of a link @@ -652,7 +652,7 @@ class PositionConstraint : public KinematicConstraint const moveit::core::LinkModel* link_model_; /**< \brief The link model constraint subject */ }; -MOVEIT_CLASS_FORWARD(VisibilityConstraint); +MOVEIT_CLASS_FORWARD(VisibilityConstraint); // Defines VisibilityConstraintPtr, ConstPtr, WeakPtr... etc /** * \brief Class for constraints on the visibility relationship between @@ -852,7 +852,7 @@ class VisibilityConstraint : public KinematicConstraint double max_range_angle_; /**< \brief Storage for the max range angle */ }; -MOVEIT_CLASS_FORWARD(KinematicConstraintSet); +MOVEIT_CLASS_FORWARD(KinematicConstraintSet); // Defines KinematicConstraintSetPtr, ConstPtr, WeakPtr... etc /** * \brief A class that contains many different constraints, and can diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 5f46385b28..1a9b3c9b69 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -135,7 +135,7 @@ struct KinematicsResult of solutions explored. */ }; -MOVEIT_CLASS_FORWARD(KinematicsBase); +MOVEIT_CLASS_FORWARD(KinematicsBase); // Defines KinematicsBasePtr, ConstPtr, WeakPtr... etc /** * @class KinematicsBase diff --git a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h index 26b584e054..ab532a166f 100644 --- a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h +++ b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h @@ -41,7 +41,7 @@ /** @brief Namespace for kinematics metrics */ namespace kinematics_metrics { -MOVEIT_CLASS_FORWARD(KinematicsMetrics); +MOVEIT_CLASS_FORWARD(KinematicsMetrics); // Defines KinematicsMetricsPtr, ConstPtr, WeakPtr... etc /** * \brief Compute different kinds of metrics for kinematics evaluation. Currently includes diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index 5d785abc96..b180d653ed 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -44,7 +44,7 @@ namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene); +MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc } /** \brief This namespace includes the base class for MoveIt planners */ @@ -73,7 +73,7 @@ struct PlannerConfigurationSettings /** \brief Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings */ typedef std::map PlannerConfigurationMap; -MOVEIT_CLASS_FORWARD(PlanningContext); +MOVEIT_CLASS_FORWARD(PlanningContext); // Defines PlanningContextPtr, ConstPtr, WeakPtr... etc /** \brief Representation of a particular planning context -- the planning scene and the request are known, solution is not yet computed. */ @@ -144,7 +144,7 @@ class PlanningContext MotionPlanRequest request_; }; -MOVEIT_CLASS_FORWARD(PlannerManager); +MOVEIT_CLASS_FORWARD(PlannerManager); // Defines PlannerManagerPtr, ConstPtr, WeakPtr... etc /** \brief Base class for a MoveIt planner */ class PlannerManager diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h index ab7f69b8c9..1f834b71ea 100644 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h @@ -44,7 +44,7 @@ /** \brief Generic interface to adapting motion planning requests */ namespace planning_request_adapter { -MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); +MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); // Defines PlanningRequestAdapterPtr, ConstPtr, WeakPtr... etc class PlanningRequestAdapter { diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 4430d84250..74c6532847 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -59,7 +59,7 @@ /** \brief This namespace includes the central class for representing planning contexts */ namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene); +MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc /** \brief This is the function signature for additional feasibility checks to be imposed on states (in addition to respecting constraints and collision avoidance). diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.h b/moveit_core/robot_model/include/moveit/robot_model/link_model.h index 0c00369a48..ce799745da 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/link_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.h @@ -47,7 +47,7 @@ namespace shapes { -MOVEIT_CLASS_FORWARD(Shape); +MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc } namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 434afe842e..a53bdb1af7 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -59,7 +59,7 @@ namespace moveit /** \brief Core components of MoveIt */ namespace core { -MOVEIT_CLASS_FORWARD(RobotModel); +MOVEIT_CLASS_FORWARD(RobotModel); // Defines RobotModelPtr, ConstPtr, WeakPtr... etc /** \brief Definition of a kinematic model. This class is not thread safe, however multiple instances can be created */ diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 968509fe4b..a762528935 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -59,7 +59,7 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotState); +MOVEIT_CLASS_FORWARD(RobotState); // Defines RobotStatePtr, ConstPtr, WeakPtr... etc /** \brief Signature for functions that can verify that if the group \e joint_group in \e robot_state is set to \e joint_group_variable_values diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 6961c1c6c0..f595570b7d 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -44,7 +44,7 @@ namespace robot_trajectory { -MOVEIT_CLASS_FORWARD(RobotTrajectory); +MOVEIT_CLASS_FORWARD(RobotTrajectory); // Defines RobotTrajectoryPtr, ConstPtr, WeakPtr... etc /** \brief Maintain a sequence of waypoints and the time durations between these waypoints */ diff --git a/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h b/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h index 34dc28fb0e..99f50e55a1 100644 --- a/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h +++ b/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h @@ -70,7 +70,7 @@ struct SensorInfo double y_angle; }; -MOVEIT_CLASS_FORWARD(MoveItSensorManager); +MOVEIT_CLASS_FORWARD(MoveItSensorManager); // Defines MoveItSensorManagerPtr, ConstPtr, WeakPtr... etc class MoveItSensorManager { diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 799dd8e9a5..0a13c7b85e 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -45,7 +45,7 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(Transforms); +MOVEIT_CLASS_FORWARD(Transforms); // Defines TransformsPtr, ConstPtr, WeakPtr... etc /// @brief Map frame names to the transformation matrix that can transform objects from the frame name to the planning /// frame diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h index e742d0455b..11778c1951 100644 --- a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h @@ -42,7 +42,7 @@ namespace chomp_interface { -MOVEIT_CLASS_FORWARD(CHOMPInterface); +MOVEIT_CLASS_FORWARD(CHOMPInterface); // Defines CHOMPInterfacePtr, ConstPtr, WeakPtr... etc class CHOMPInterface : public chomp::ChompPlanner { diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h index 124a83f333..31bdc0100f 100644 --- a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h @@ -41,7 +41,7 @@ namespace chomp_interface { -MOVEIT_CLASS_FORWARD(CHOMPPlanningContext); +MOVEIT_CLASS_FORWARD(CHOMPPlanningContext); // Defines CHOMPPlanningContextPtr, ConstPtr, WeakPtr... etc class CHOMPPlanningContext : public planning_interface::PlanningContext { diff --git a/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h b/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h index 3814d0356f..ce7a56b432 100644 --- a/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h +++ b/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h @@ -62,7 +62,7 @@ class OMPLDynamicReconfigureConfigStatics; class OMPLDynamicReconfigureConfig { public: - MOVEIT_CLASS_FORWARD(AbstractParamDescription); + MOVEIT_CLASS_FORWARD(AbstractParamDescription); // Defines AbstractParamDescriptionPtr, ConstPtr, WeakPtr... etc class AbstractParamDescription : public dynamic_reconfigure::ParamDescription { public: @@ -141,7 +141,7 @@ class OMPLDynamicReconfigureConfig } }; - MOVEIT_CLASS_FORWARD(AbstractGroupDescription); + MOVEIT_CLASS_FORWARD(AbstractGroupDescription); // Defines AbstractGroupDescriptionPtr, ConstPtr, WeakPtr... etc class AbstractGroupDescription : public dynamic_reconfigure::Group { public: diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h index e9b4413028..8dbcd93a5e 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h @@ -45,7 +45,7 @@ namespace ompl_interface { class ModelBasedPlanningContext; -MOVEIT_CLASS_FORWARD(ValidStateSampler); +MOVEIT_CLASS_FORWARD(ValidStateSampler); // Defines ValidStateSamplerPtr, ConstPtr, WeakPtr... etc /** @class ValidConstrainedSampler * This class defines a sampler that tries to find a valid sample that satisfies the specified constraints */ diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h index 85c278bff5..2b4c8c759c 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h @@ -155,7 +155,7 @@ struct ConstraintApproximationConstructionResults double sampling_success_rate; }; -MOVEIT_CLASS_FORWARD(ConstraintsLibrary); +MOVEIT_CLASS_FORWARD(ConstraintsLibrary); // Defines ConstraintsLibraryPtr, ConstPtr, WeakPtr... etc class ConstraintsLibrary { diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index 7391024b66..288439e6e9 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -52,8 +52,8 @@ namespace ob = ompl::base; namespace og = ompl::geometric; namespace ot = ompl::tools; -MOVEIT_CLASS_FORWARD(ModelBasedPlanningContext); -MOVEIT_CLASS_FORWARD(ConstraintsLibrary); +MOVEIT_CLASS_FORWARD(ModelBasedPlanningContext); // Defines ModelBasedPlanningContextPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(ConstraintsLibrary); // Defines ConstraintsLibraryPtr, ConstPtr, WeakPtr... etc struct ModelBasedPlanningContextSpecification; typedef std::function groups_; diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h index 274a3ca022..c428071beb 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h @@ -47,7 +47,7 @@ /** \brief This namespace includes functionality specific to the execution and monitoring of motion plans */ namespace plan_execution { -MOVEIT_CLASS_FORWARD(PlanExecution); +MOVEIT_CLASS_FORWARD(PlanExecution); // Defines PlanExecutionPtr, ConstPtr, WeakPtr... etc class PlanExecution { diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h index 5741e2e223..d05760afa0 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h @@ -48,7 +48,7 @@ namespace plan_execution { -MOVEIT_CLASS_FORWARD(PlanWithSensing); +MOVEIT_CLASS_FORWARD(PlanWithSensing); // Defines PlanWithSensingPtr, ConstPtr, WeakPtr... etc class PlanWithSensing { diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index e7514f6702..543ea0f5b2 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -194,5 +194,5 @@ class PlanningPipeline ros::Publisher contacts_publisher_; }; -MOVEIT_CLASS_FORWARD(PlanningPipeline); +MOVEIT_CLASS_FORWARD(PlanningPipeline); // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc } // namespace planning_pipeline diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h index 1df0a8a1af..7203eaa3eb 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h @@ -210,5 +210,5 @@ class CurrentStateMonitor std::shared_ptr tf_connection_; }; -MOVEIT_CLASS_FORWARD(CurrentStateMonitor); +MOVEIT_CLASS_FORWARD(CurrentStateMonitor); // Defines CurrentStateMonitorPtr, ConstPtr, WeakPtr... etc } // namespace planning_scene_monitor diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index b0716f346f..941bb266c6 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -53,7 +53,7 @@ namespace planning_scene_monitor { -MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); +MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); // Defines PlanningSceneMonitorPtr, ConstPtr, WeakPtr... etc /** * @brief PlanningSceneMonitor diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h index 429d0163d8..c5aa8142b2 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h @@ -47,7 +47,7 @@ namespace planning_scene_monitor typedef boost::function TrajectoryStateAddedCallback; -MOVEIT_CLASS_FORWARD(TrajectoryMonitor); +MOVEIT_CLASS_FORWARD(TrajectoryMonitor); // Defines TrajectoryMonitorPtr, ConstPtr, WeakPtr... etc /** @class TrajectoryMonitor @brief Monitors the joint_states topic and tf to record the trajectory of the robot. */ diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h index d365ac8169..40e41a415b 100644 --- a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h @@ -42,7 +42,7 @@ namespace rdf_loader { -MOVEIT_CLASS_FORWARD(RDFLoader); +MOVEIT_CLASS_FORWARD(RDFLoader); // Defines RDFLoaderPtr, ConstPtr, WeakPtr... etc /** @class RDFLoader * @brief Default constructor diff --git a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h index c3b136d2ea..5eb084341f 100644 --- a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h +++ b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h @@ -43,7 +43,7 @@ namespace robot_model_loader { -MOVEIT_CLASS_FORWARD(RobotModelLoader); +MOVEIT_CLASS_FORWARD(RobotModelLoader); // Defines RobotModelLoaderPtr, ConstPtr, WeakPtr... etc /** @class RobotModelLoader */ class RobotModelLoader diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index 22a7ee9eea..1506513902 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -51,7 +51,7 @@ namespace trajectory_execution_manager { -MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); +MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); // Defines TrajectoryExecutionManagerPtr, ConstPtr, WeakPtr... etc // Two modes: // Managed controllers diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index ec8d70f0b4..862f6d7dd2 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -89,7 +89,7 @@ class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes } }; -MOVEIT_CLASS_FORWARD(MoveGroupInterface); +MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc /** \class MoveGroupInterface move_group_interface.h moveit/planning_interface/move_group_interface.h diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 2f6e09caad..4badc68fdf 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -50,7 +50,7 @@ namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(MoveItCpp); +MOVEIT_CLASS_FORWARD(MoveItCpp); // Defines MoveItCppPtr, ConstPtr, WeakPtr... etc class MoveItCpp { diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 6aa2d0ca7d..94d4a896fe 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -49,7 +49,7 @@ namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(PlanningComponent); +MOVEIT_CLASS_FORWARD(PlanningComponent); // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc class PlanningComponent { diff --git a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index 6d872eb116..6a468ddbf1 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h @@ -47,7 +47,7 @@ namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(PlanningSceneInterface); +MOVEIT_CLASS_FORWARD(PlanningSceneInterface); // Defines PlanningSceneInterfacePtr, ConstPtr, WeakPtr... etc class PlanningSceneInterface { diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h index b9914d437a..487d1dd79d 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h @@ -46,9 +46,9 @@ namespace robot_interaction { -MOVEIT_CLASS_FORWARD(InteractionHandler); -MOVEIT_CLASS_FORWARD(RobotInteraction); -MOVEIT_CLASS_FORWARD(KinematicOptionsMap); +MOVEIT_CLASS_FORWARD(InteractionHandler); // Defines InteractionHandlerPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RobotInteraction); // Defines RobotInteractionPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(KinematicOptionsMap); // Defines KinematicOptionsMapPtr, ConstPtr, WeakPtr... etc struct EndEffectorInteraction; struct JointInteraction; diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h index f10ddb99af..ebd2f72de1 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h @@ -44,7 +44,7 @@ namespace robot_interaction { -MOVEIT_CLASS_FORWARD(LockedRobotState); +MOVEIT_CLASS_FORWARD(LockedRobotState); // Defines LockedRobotStatePtr, ConstPtr, WeakPtr... etc /// Maintain a RobotState in a multithreaded environment. // diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h index ca0698af29..6c3eadbfb5 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h @@ -57,9 +57,9 @@ class InteractiveMarkerServer; namespace robot_interaction { -MOVEIT_CLASS_FORWARD(InteractionHandler); -MOVEIT_CLASS_FORWARD(KinematicOptionsMap); -MOVEIT_CLASS_FORWARD(RobotInteraction); +MOVEIT_CLASS_FORWARD(InteractionHandler); // Defines InteractionHandlerPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(KinematicOptionsMap); // Defines KinematicOptionsMapPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RobotInteraction); // Defines RobotInteractionPtr, ConstPtr, WeakPtr... etc // Manage interactive markers for controlling a robot state. // diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 0f87a8ca43..56f199c39f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -77,9 +77,9 @@ class MotionPlanningUI; namespace moveit_warehouse { -MOVEIT_CLASS_FORWARD(PlanningSceneStorage); -MOVEIT_CLASS_FORWARD(ConstraintsStorage); -MOVEIT_CLASS_FORWARD(RobotStateStorage); +MOVEIT_CLASS_FORWARD(PlanningSceneStorage); // Defines PlanningSceneStoragePtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(ConstraintsStorage); // Defines ConstraintsStoragePtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RobotStateStorage); // Defines RobotStateStoragePtr, ConstPtr, WeakPtr... etc } // namespace moveit_warehouse namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h index 0f273a527a..855556953c 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h @@ -47,7 +47,7 @@ namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(MoveGroupInterface); +MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc } } // namespace moveit diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h index ba49862429..3f673c24ed 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h @@ -54,9 +54,9 @@ class DisplayContext; namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(RobotStateVisualization); -MOVEIT_CLASS_FORWARD(RenderShapes); -MOVEIT_CLASS_FORWARD(PlanningSceneRender); +MOVEIT_CLASS_FORWARD(RobotStateVisualization); // Defines RobotStateVisualizationPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RenderShapes); // Defines RenderShapesPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(PlanningSceneRender); // Defines PlanningSceneRenderPtr, ConstPtr, WeakPtr... etc class PlanningSceneRender { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h index 7fcf9c2f0d..194d4d2faa 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h @@ -56,8 +56,8 @@ class Shape; namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(OcTreeRender); -MOVEIT_CLASS_FORWARD(RenderShapes); +MOVEIT_CLASS_FORWARD(OcTreeRender); // Defines OcTreeRenderPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RenderShapes); // Defines RenderShapesPtr, ConstPtr, WeakPtr... etc class RenderShapes { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index 1f3d90c4b3..d210edef5e 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -47,8 +47,8 @@ DIAGNOSTIC_POP namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(RenderShapes); -MOVEIT_CLASS_FORWARD(RobotStateVisualization); +MOVEIT_CLASS_FORWARD(RenderShapes); // Defines RenderShapesPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RobotStateVisualization); // Defines RobotStateVisualizationPtr, ConstPtr, WeakPtr... etc /** \brief Update the links of an rviz::Robot using a moveit::core::RobotState */ class RobotStateVisualization diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h index 3c6d92307b..2af227f429 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h @@ -68,7 +68,7 @@ class MovableText; namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(TrajectoryVisualization); +MOVEIT_CLASS_FORWARD(TrajectoryVisualization); // Defines TrajectoryVisualizationPtr, ConstPtr, WeakPtr... etc class TrajectoryVisualization : public QObject { diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h index 337a57dc52..4b3be06f82 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h @@ -45,7 +45,7 @@ namespace moveit_warehouse typedef warehouse_ros::MessageWithMetadata::ConstPtr ConstraintsWithMetadata; typedef warehouse_ros::MessageCollection::Ptr ConstraintsCollection; -MOVEIT_CLASS_FORWARD(ConstraintsStorage); +MOVEIT_CLASS_FORWARD(ConstraintsStorage); // Defines ConstraintsStoragePtr, ConstPtr, WeakPtr... etc class ConstraintsStorage : public MoveItMessageStorage { diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h index e7899729ac..bb1e24a07a 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h @@ -52,7 +52,7 @@ typedef warehouse_ros::MessageCollection::Ptr Planni typedef warehouse_ros::MessageCollection::Ptr MotionPlanRequestCollection; typedef warehouse_ros::MessageCollection::Ptr RobotTrajectoryCollection; -MOVEIT_CLASS_FORWARD(PlanningSceneStorage); +MOVEIT_CLASS_FORWARD(PlanningSceneStorage); // Defines PlanningSceneStoragePtr, ConstPtr, WeakPtr... etc class PlanningSceneStorage : public MoveItMessageStorage { diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h index e008a5fbd2..3651cb1144 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h @@ -45,7 +45,7 @@ namespace moveit_warehouse typedef warehouse_ros::MessageWithMetadata::ConstPtr RobotStateWithMetadata; typedef warehouse_ros::MessageCollection::Ptr RobotStateCollection; -MOVEIT_CLASS_FORWARD(RobotStateStorage); +MOVEIT_CLASS_FORWARD(RobotStateStorage); // Defines RobotStateStoragePtr, ConstPtr, WeakPtr... etc class RobotStateStorage : public MoveItMessageStorage { diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h index c7e6964a23..934f510a36 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h @@ -46,7 +46,7 @@ typedef warehouse_ros::MessageWithMetadata:: TrajectoryConstraintsWithMetadata; typedef warehouse_ros::MessageCollection::Ptr TrajectoryConstraintsCollection; -MOVEIT_CLASS_FORWARD(TrajectoryConstraintsStorage); +MOVEIT_CLASS_FORWARD(TrajectoryConstraintsStorage); // Defines TrajectoryConstraintsStoragePtr, ConstPtr, WeakPtr... etc class TrajectoryConstraintsStorage : public MoveItMessageStorage { diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h index fc3ef90160..f7f41b1b66 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h @@ -40,7 +40,7 @@ #include namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene); +MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc } namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 99e58e9ece..4601e24d65 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -174,7 +174,7 @@ class GenericParameter std::string comment_; // comment briefly describing what this parameter does }; -MOVEIT_CLASS_FORWARD(MoveItConfigData); +MOVEIT_CLASS_FORWARD(MoveItConfigData); // Defines MoveItConfigDataPtr, ConstPtr, WeakPtr... etc /** \brief This class is shared with all widgets and contains the common configuration data needed for generating each robot's MoveIt configuration package. From a1acf2e1be8258ba39a89cb385a16b6352e0a180 Mon Sep 17 00:00:00 2001 From: Yoan Mollard Date: Tue, 15 Sep 2020 08:52:47 +0200 Subject: [PATCH 448/612] robot_description is already loaded in move_group.launch (#2313) --- .../moveit_config_pkg_template/launch/demo.launch | 9 ++++----- .../moveit_config_pkg_template/launch/demo_gazebo.launch | 9 ++++----- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch index 4ebe898ffb..85451710c8 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch @@ -11,6 +11,9 @@ + + + - - - - [VIRTUAL_JOINT_BROADCASTER] @@ -48,6 +46,7 @@ + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch index 059ef752df..6d2427b16e 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch @@ -8,6 +8,9 @@ + + + - - - - [VIRTUAL_JOINT_BROADCASTER] @@ -58,6 +56,7 @@ + From 964ed5c2db624175e7fa758a5a8eb35ec3dd55cc Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 15 Sep 2020 11:02:11 -0500 Subject: [PATCH 449/612] Refactor velocity limit enforcement and add a unit test (#2260) * Rework velocity limit enforcement, add unit test * Test every command out of Servo * Minor variable renaming * Clang format & clang tidy * Delete unused acceleration variable --- .../include/moveit_servo/servo_calcs.h | 4 +- moveit_ros/moveit_servo/src/servo_calcs.cpp | 68 +++++++---------- .../test/config/servo_settings.yaml | 2 +- .../test/servo_cpp_interface_test.cpp | 73 ++++++++++++++++++- 4 files changed, 98 insertions(+), 49 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index c67afb6e0c..69ad46cf27 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -113,10 +113,10 @@ class ServoCalcs void suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory); /** \brief Scale the delta theta to match joint velocity/acceleration limits */ - void enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta); + void enforceVelLimits(Eigen::ArrayXd& delta_theta); /** \brief Avoid overshooting joint limits */ - bool enforceSRDFPositionLimits(); + bool enforcePositionLimits(); /** \brief Possibly calculate a velocity scaling factor, due to proximity of * singularity and direction of motion diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index c28a36fd0b..ac56a6216d 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -458,7 +458,7 @@ bool ServoCalcs::cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, delta_theta_ = pseudo_inverse * delta_x; - enforceSRDFAccelVelLimits(delta_theta_); + enforceVelLimits(delta_theta_); // If close to a collision or a singularity, decelerate applyVelocityScaling(delta_theta_, velocityScalingFactorForSingularity(delta_x, svd, pseudo_inverse)); @@ -484,7 +484,7 @@ bool ServoCalcs::jointServoCalcs(const control_msgs::JointJog& cmd, trajectory_m // Apply user-defined scaling delta_theta_ = scaleJointCommand(cmd); - enforceSRDFAccelVelLimits(delta_theta_); + enforceVelLimits(delta_theta_); // If close to a collision, decelerate applyVelocityScaling(delta_theta_, 1.0 /* scaling for singularities -- ignore for joint motions */); @@ -507,7 +507,7 @@ bool ServoCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& jo composeJointTrajMessage(internal_joint_state_, joint_trajectory); - if (!enforceSRDFPositionLimits()) + if (!enforcePositionLimits()) { suddenHalt(joint_trajectory); status_ = StatusCode::JOINT_BOUND; @@ -680,45 +680,18 @@ double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& co return velocity_scale; } -void ServoCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta) +void ServoCalcs::enforceVelLimits(Eigen::ArrayXd& delta_theta) { Eigen::ArrayXd velocity = delta_theta / parameters_.publish_period; - const Eigen::ArrayXd acceleration = (velocity - prev_joint_velocity_) / parameters_.publish_period; std::size_t joint_delta_index = 0; + // Track the smallest velocity scaling factor required, across all joints + double velocity_limit_scaling_factor = 1; + for (auto joint : joint_model_group_->getActiveJointModels()) { // Some joints do not have bounds defined const auto bounds = joint->getVariableBounds(joint->getName()); - if (bounds.acceleration_bounded_) - { - bool clip_acceleration = false; - double acceleration_limit = 0.0; - if (acceleration(joint_delta_index) < bounds.min_acceleration_) - { - clip_acceleration = true; - acceleration_limit = bounds.min_acceleration_; - } - else if (acceleration(joint_delta_index) > bounds.max_acceleration_) - { - clip_acceleration = true; - acceleration_limit = bounds.max_acceleration_; - } - - // Apply acceleration bounds - if (clip_acceleration) - { - // accel = (vel - vel_prev) / delta_t = ((delta_theta / delta_t) - vel_prev) / delta_t - // --> delta_theta = (accel * delta_t _ + vel_prev) * delta_t - const double relative_change = - ((acceleration_limit * parameters_.publish_period + prev_joint_velocity_(joint_delta_index)) * - parameters_.publish_period) / - delta_theta(joint_delta_index); - // Avoid nan - if (fabs(relative_change) < 1) - delta_theta(joint_delta_index) = relative_change * delta_theta(joint_delta_index); - } - } if (bounds.velocity_bounded_) { @@ -740,21 +713,30 @@ void ServoCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta) // Apply velocity bounds if (clip_velocity) { - // delta_theta = joint_velocity * delta_t - const double relative_change = (velocity_limit * parameters_.publish_period) / delta_theta(joint_delta_index); - // Avoid nan - if (fabs(relative_change) < 1) - { - delta_theta(joint_delta_index) = relative_change * delta_theta(joint_delta_index); - velocity(joint_delta_index) = relative_change * velocity(joint_delta_index); - } + const double scaling_factor = + fabs(velocity_limit * parameters_.publish_period) / fabs(delta_theta(joint_delta_index)); + + // Store the scaling factor if it's the smallest yet + if (scaling_factor < velocity_limit_scaling_factor) + velocity_limit_scaling_factor = scaling_factor; } } ++joint_delta_index; } + + // Apply the velocity scaling to all joints + if (velocity_limit_scaling_factor < 1) + { + for (joint_delta_index = 0; joint_delta_index < joint_model_group_->getActiveJointModels().size(); + ++joint_delta_index) + { + delta_theta(joint_delta_index) = velocity_limit_scaling_factor * delta_theta(joint_delta_index); + velocity(joint_delta_index) = velocity_limit_scaling_factor * velocity(joint_delta_index); + } + } } -bool ServoCalcs::enforceSRDFPositionLimits() +bool ServoCalcs::enforcePositionLimits() { bool halting = false; diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml index 9a674425b6..30e4cb59d2 100644 --- a/moveit_ros/moveit_servo/test/config/servo_settings.yaml +++ b/moveit_ros/moveit_servo/test/config/servo_settings.yaml @@ -5,7 +5,7 @@ use_gazebo: false # Whether the robot is started in a Gazebo simulation environm ## Properties of incoming commands robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector -command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" linear: 0.003 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp index 4fd01fc73e..c565894e0b 100644 --- a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp @@ -46,10 +46,11 @@ #include // Servo -#include #include +#include static const std::string LOGNAME = "servo_cpp_interface_test"; +static constexpr double LARGEST_ALLOWABLE_PANDA_VEL = 2.8710; // to test joint velocity limit enforcement namespace moveit_servo { @@ -110,7 +111,7 @@ TEST_F(ServoFixture, SendTwistStampedTest) // count trajectory messages sent by servo size_t received_count = 0; boost::function traj_callback = - [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { received_count++; }; + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { ++received_count; }; auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); // Create publisher to send servo commands @@ -136,6 +137,7 @@ TEST_F(ServoFixture, SendTwistStampedTest) } EXPECT_GT(received_count, num_commands - 20); + EXPECT_GT(received_count, (unsigned)0); EXPECT_LT(received_count, num_commands + 20); servo_->stop(); } @@ -150,7 +152,7 @@ TEST_F(ServoFixture, SendJointServoTest) // count trajectory messages sent by servo size_t received_count = 0; boost::function traj_callback = - [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { received_count++; }; + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { ++received_count; }; auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); // Create publisher to send servo commands @@ -180,6 +182,71 @@ TEST_F(ServoFixture, SendJointServoTest) servo_->stop(); } +TEST_F(ServoFixture, JointVelocityEnforcementTest) +{ + servo_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + + auto parameters = servo_->getParameters(); + double publish_period = parameters.publish_period; + + // count trajectory messages sent by servo + size_t received_count = 0; + trajectory_msgs::JointTrajectory joint_command_from_servo; + trajectory_msgs::JointTrajectory prev_joint_command_from_servo; + boost::function traj_callback = + [&](const trajectory_msgs::JointTrajectoryConstPtr& msg) { + ++received_count; + // Store a series of two commands so we can calculate velocities + // from positions + prev_joint_command_from_servo = joint_command_from_servo; + joint_command_from_servo = *msg; + + // Start running checks when we have at least two datapoints + if (received_count > 1) + { + // Need a sequence of two commands to calculate a velocity + EXPECT_GT(joint_command_from_servo.points.size(), (unsigned)0); + EXPECT_GT(prev_joint_command_from_servo.points.size(), (unsigned)0); + EXPECT_EQ(prev_joint_command_from_servo.points.size(), joint_command_from_servo.points.size()); + // No velocities larger than the largest allowable Panda velocity + for (size_t joint_index = 0; joint_index < joint_command_from_servo.points[0].positions.size(); ++joint_index) + { + double joint_velocity = (joint_command_from_servo.points[0].positions[joint_index] - + prev_joint_command_from_servo.points[0].positions[joint_index]) / + publish_period; + EXPECT_LE(joint_velocity, LARGEST_ALLOWABLE_PANDA_VEL); + } + } + }; + auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); + + // Create publisher to send servo commands + auto twist_stamped_pub = nh_.advertise(parameters.cartesian_command_in_topic, 1); + + constexpr double test_duration = 1.0; + const size_t num_commands = static_cast(test_duration / publish_period); + + ros::Rate publish_rate(1. / publish_period); + + // Send a few Cartesian commands with very high velocity + for (size_t i = 0; i < num_commands && ros::ok(); ++i) + { + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "panda_link0"; + msg->twist.linear.x = 10.0; + msg->twist.angular.y = 5 * LARGEST_ALLOWABLE_PANDA_VEL; + + // Send the message + twist_stamped_pub.publish(msg); + publish_rate.sleep(); + } + + EXPECT_GT(received_count, num_commands - 20); + EXPECT_LT(received_count, num_commands + 20); + servo_->stop(); +} } // namespace moveit_servo int main(int argc, char** argv) From 6a5754bf527c78af69ec49459221939cf77fc1ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 16 Sep 2020 13:10:06 +0200 Subject: [PATCH 450/612] Noetic was released (#2304) --- MIGRATION.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MIGRATION.md b/MIGRATION.md index 5b2462012c..4dbbf006a2 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -2,7 +2,7 @@ API changes in MoveIt releases -## ROS Noetic (upcoming changes in master) +## ROS Noetic - RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link. - Planned trajectories are *slow* by default. The default values of the `velocity_scaling_factor` and `acceleration_scaling_factor` can now be customized and default to 0.1 instead of 1.0. From 336e13be44ad68adad80923cfd87cf6864b71d8c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 17 Sep 2020 12:57:01 -0500 Subject: [PATCH 451/612] Fix Servo thread interruption (#2314) --- .../include/moveit_servo/collision_check.h | 8 ++++++-- .../moveit_servo/include/moveit_servo/servo.h | 3 --- .../include/moveit_servo/servo_calcs.h | 9 ++++++--- moveit_ros/moveit_servo/src/collision_check.cpp | 5 ----- .../cpp_interface_example.cpp | 2 +- moveit_ros/moveit_servo/src/servo.cpp | 8 +------- moveit_ros/moveit_servo/src/servo_calcs.cpp | 11 +---------- moveit_ros/moveit_servo/src/servo_server.cpp | 2 +- .../moveit_servo/test/servo_cpp_interface_test.cpp | 14 +++++++------- 9 files changed, 23 insertions(+), 39 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index a02fe4750a..9accf79484 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -68,9 +68,13 @@ class CollisionCheck const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::shared_ptr& joint_state_subscriber); - /** \brief start and stop the Timer */ + ~CollisionCheck() + { + timer_.stop(); + } + + /** \brief start the Timer that regulates collision check rate */ void start(); - void stop(); /** \brief Pause or unpause processing servo commands while keeping the timers alive */ void setPaused(bool paused); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h index 5e9f26fe64..139c11b44b 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -60,9 +60,6 @@ class Servo /** \brief start servo node */ void start(); - /** \brief stop servo node */ - void stop(); - /** \brief Pause or unpause processing servo commands while keeping the timers alive */ void setPaused(bool paused); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 69ad46cf27..fa477eaabc 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -66,9 +66,13 @@ class ServoCalcs const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::shared_ptr& joint_state_subscriber); - /** \brief Start and stop the timer where we do work and publish outputs */ + ~ServoCalcs() + { + timer_.stop(); + } + + /** \brief Start the timer where we do work and publish outputs */ void start(); - void stop(); /** * Get the MoveIt planning link transform. @@ -252,7 +256,6 @@ class ServoCalcs // Status StatusCode status_ = StatusCode::NO_WARNING; - bool stop_requested_ = false; bool paused_ = false; bool twist_command_is_stale_ = false; bool joint_command_is_stale_ = false; diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp index 162a9f5831..ec047c9953 100644 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ b/moveit_ros/moveit_servo/src/collision_check.cpp @@ -94,11 +94,6 @@ void CollisionCheck::start() timer_ = nh_.createTimer(period_, &CollisionCheck::run, this); } -void CollisionCheck::stop() -{ - timer_.stop(); -} - void CollisionCheck::run(const ros::TimerEvent& timer_event) { // Log warning when the last loop duration was longer than the period diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp index 53e919ee33..c1170d10a9 100644 --- a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp @@ -150,6 +150,6 @@ int main(int argc, char** argv) ++num_commands; } - servo.stop(); + servo.setPaused(true); return 0; } diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 8cc834a047..761be5903e 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -286,15 +286,9 @@ void Servo::start() collision_checker_->start(); } -void Servo::stop() -{ - servo_calcs_->stop(); - collision_checker_->stop(); -} - Servo::~Servo() { - stop(); + setPaused(true); } void Servo::setPaused(bool paused) diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index ac56a6216d..dd39f569c1 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -179,16 +179,9 @@ void ServoCalcs::start() initial_joint_trajectory->points.push_back(point); last_sent_command_ = initial_joint_trajectory; - stop_requested_ = false; timer_ = nh_.createTimer(period_, &ServoCalcs::run, this); } -void ServoCalcs::stop() -{ - stop_requested_ = true; - timer_.stop(); -} - void ServoCalcs::run(const ros::TimerEvent& timer_event) { // Log warning when the last loop duration was longer than the period @@ -209,8 +202,6 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) // 2) so the low-pass filters are up to date and don't cause a jump while (!updateJoints() && ros::ok()) { - if (stop_requested_) - return; default_sleep_rate_.sleep(); } @@ -335,7 +326,7 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) zero_velocity_count_ = 0; } - if (ok_to_publish_) + if (ok_to_publish_ && !paused_) { // Put the outgoing msg in the right format // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). diff --git a/moveit_ros/moveit_servo/src/servo_server.cpp b/moveit_ros/moveit_servo/src/servo_server.cpp index 590061609f..31e477c837 100644 --- a/moveit_ros/moveit_servo/src/servo_server.cpp +++ b/moveit_ros/moveit_servo/src/servo_server.cpp @@ -80,7 +80,7 @@ int main(int argc, char** argv) ros::waitForShutdown(); // Stop the servo server - servo.stop(); + servo.setPaused(true); return 0; } diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp index c565894e0b..a5637a6eda 100644 --- a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp @@ -77,7 +77,7 @@ class ServoFixture : public ::testing::Test bool waitForFirstStatus() { - auto msg = ros::topic::waitForMessage(servo_->getParameters().status_topic, nh_, ros::Duration(15)); + auto msg = ros::topic::waitForMessage(servo_->getParameters().status_topic, nh_, ros::Duration(1)); return static_cast(msg); } @@ -91,14 +91,14 @@ TEST_F(ServoFixture, StartStopTest) { servo_->start(); EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; - servo_->stop(); + servo_->setPaused(true); ros::Duration(1.0).sleep(); // Start and stop again - servo_->start(); + servo_->setPaused(false); EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; - servo_->stop(); + servo_->setPaused(true); } TEST_F(ServoFixture, SendTwistStampedTest) @@ -139,7 +139,7 @@ TEST_F(ServoFixture, SendTwistStampedTest) EXPECT_GT(received_count, num_commands - 20); EXPECT_GT(received_count, (unsigned)0); EXPECT_LT(received_count, num_commands + 20); - servo_->stop(); + servo_->setPaused(true); } TEST_F(ServoFixture, SendJointServoTest) @@ -179,7 +179,7 @@ TEST_F(ServoFixture, SendJointServoTest) EXPECT_GT(received_count, num_commands - 20); EXPECT_LT(received_count, num_commands + 20); - servo_->stop(); + servo_->setPaused(true); } TEST_F(ServoFixture, JointVelocityEnforcementTest) @@ -245,7 +245,7 @@ TEST_F(ServoFixture, JointVelocityEnforcementTest) EXPECT_GT(received_count, num_commands - 20); EXPECT_LT(received_count, num_commands + 20); - servo_->stop(); + servo_->setPaused(true); } } // namespace moveit_servo From 91e1f095c2e68187e19f975951011c20f0d8b51e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Fri, 18 Sep 2020 07:58:09 +0200 Subject: [PATCH 452/612] MGC: Improve exception messages (#2318) --- moveit_commander/src/moveit_commander/move_group.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index bd17ea9f50..e0128e6eb4 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/moveit_commander/src/moveit_commander/move_group.py @@ -83,7 +83,7 @@ def get_end_effector_link(self): def set_end_effector_link(self, link_name): """ Set the name of the link to be considered as an end effector """ if not self._g.set_end_effector_link(link_name): - raise MoveItCommanderException("Unable to set end efector link") + raise MoveItCommanderException("Unable to set end effector link") def get_interface_description(self): """ Get the description of the planner interface (list of planner ids) """ @@ -219,7 +219,7 @@ def set_joint_value_target(self, arg1, arg2=None, arg3=None): if approx: raise MoveItCommanderException("Error setting joint target. Does your IK solver support approximate IK?") else: - raise MoveItCommanderException("Error setting joint target. Is IK running?") + raise MoveItCommanderException("Error setting joint target. Is the IK solver functional?") elif (hasattr(arg1, '__iter__')): if (arg2 is not None or arg3 is not None): From 1825936b5f212e9ea72ff78e293ff6f5a2fc7b4b Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Wed, 23 Sep 2020 07:17:42 +0900 Subject: [PATCH 453/612] Clean up Rviz Motion Planning plugin, add tooltips (#2310) - better use of space / overall smaller panel - remove goal constraints field, it's not used and not intuitive in the GUI either - reorder check boxes by subjective usefulness - Add better tooltips - Remove autoScroll from Planning Parameters field - Fix tabstop order - Align columns of Planning tab Co-authored-by: Robert Haschke --- .../src/motion_planning_display.cpp | 4 - .../src/motion_planning_frame.cpp | 2 - .../ui/motion_planning_rviz_plugin_frame.ui | 510 +++++++++++++++--- 3 files changed, 422 insertions(+), 94 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 28fdb4b4a2..41a0a471dc 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -1335,8 +1335,6 @@ void MotionPlanningDisplay::load(const rviz::Config& config) frame_->ui_->velocity_scaling_factor->setValue(d); if (config.mapGetFloat("Acceleration_Scaling_Factor", &d)) frame_->ui_->acceleration_scaling_factor->setValue(d); - if (config.mapGetFloat("MoveIt_Goal_Tolerance", &d)) - frame_->ui_->goal_tolerance->setValue(d); bool b; if (config.mapGetBool("MoveIt_Allow_Replanning", &b)) @@ -1395,8 +1393,6 @@ void MotionPlanningDisplay::save(rviz::Config config) const config.mapSetValue("MoveIt_Warehouse_Host", frame_->ui_->database_host->text()); config.mapSetValue("MoveIt_Warehouse_Port", frame_->ui_->database_port->value()); - config.mapSetValue("MoveIt_Goal_Tolerance", frame_->ui_->goal_tolerance->value()); - // "Options" Section config.mapSetValue("MoveIt_Planning_Time", frame_->ui_->planning_time->value()); config.mapSetValue("MoveIt_Planning_Attempts", frame_->ui_->planning_attempts->value()); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 697f3698e6..9568527651 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -163,8 +163,6 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: connect(ui_->database_host, SIGNAL(textChanged(QString)), this, SIGNAL(configChanged())); connect(ui_->database_port, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); - connect(ui_->goal_tolerance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); - connect(ui_->planning_time, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(ui_->planning_attempts, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); connect(ui_->velocity_scaling_factor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 22154c05da..acde5aa80d 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -6,8 +6,8 @@ 0 0 - 532 - 389 + 507 + 380 @@ -26,7 +26,7 @@ false - 3 + 1 @@ -96,6 +96,9 @@ + + false + 0 @@ -157,6 +160,12 @@ + + + 0 + 0 + + color:red @@ -401,7 +410,7 @@ - Plan and E&xecute + Plan && E&xecute @@ -410,6 +419,9 @@ false + + Stop execution by sending a stop command to the move_group. + &Stop @@ -509,12 +521,12 @@ Qt::Vertical - QSizePolicy::Fixed + QSizePolicy::Expanding 20 - 6 + 32 @@ -538,38 +550,15 @@ 6 - - - - - - - - Goal Tolerance: - - - - - - - + + + Select path constraints from the database. + + - - - - Qt::Vertical - - - - 20 - 40 - - - - @@ -584,7 +573,7 @@ - 2 + 1 4 @@ -596,6 +585,9 @@ + + Total time allowed for planning. Planning stops if time or number of attempts is exceeded, or goal is reached. + Planning Time (s): @@ -609,6 +601,18 @@ 0 + + + 40 + 0 + + + + Total time allowed for planning. Planning stops if time or number of attempts is exceeded, or goal is reached. + + + 1 + 300.000000000000000 @@ -626,6 +630,9 @@ + + Allowed number of planning attempts. Planning stops if time or number of attempts is exceeded, or goal is reached. + Planning Attempts: @@ -639,6 +646,15 @@ 0 + + + 40 + 0 + + + + Allowed number of planning attempts. Planning stops if time or number of attempts is exceeded, or goal is reached. + 1000 @@ -656,6 +672,9 @@ + + Factor (between 0 and 1) to scale down maximum joint velocities + Velocity Scaling: @@ -663,11 +682,20 @@ + + + 40 + 0 + + + + Factor (between 0 and 1) to scale down maximum joint velocities + 1.000000000000000 - 0.010000000000000 + 0.100000000000000 1.000000000000000 @@ -683,18 +711,30 @@ + + Factor (between 0 and 1) to scale down maximum joint accelerations + - Acceleration Scaling: + Accel. Scaling: + + + 40 + 0 + + + + Factor (between 0 and 1) to scale down maximum joint accelerations + 1.000000000000000 - 0.010000000000000 + 0.100000000000000 1.000000000000000 @@ -704,9 +744,39 @@ - + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + Check this to generate a linear path in Cartesian (3D) space. + + This does not plan around obstacles. + - Allow Replanning + Use Cartesian Path false @@ -714,9 +784,26 @@ - + + + + 0 + 0 + + + + + 30 + 0 + + + + If checked, the IK solver tries to avoid collisions when searching for a start/end state set via the interactive marker. + +This is usually achieved by random seeding, which can flip the robot configuration (e.g. elbow up/down). Note that motion planning is always collision-aware, regardless of this checkbox. + - Allow Sensor Positioning + Collision-aware IK false @@ -724,19 +811,46 @@ - - - Allow External Comm. + + + + 0 + 0 + - - false + + + 30 + 0 + + + + Allow approximate IK solutions, which deviate from the target pose. Useful when an exact solution cannot be found, e.g. for underactuated arms or when trying to reach an unreachable target. + + + Approx IK Solutions - + + + + 0 + 0 + + + + + 30 + 0 + + + + Allow other nodes to control start and end poses as well as planning and execution, using ROS topics (/rviz/moveit/ ). + - Use Cartesian Path + External Comm. false @@ -744,38 +858,61 @@ - + + + + 0 + 0 + + + + + 30 + 0 + + + + EXPERIMENTAL: Trigger replanning if new collisions are detected during plan execution. Only works for the "Plan & Execute" button. + + + false + - Use Collision-Aware IK + Replanning - true + false - + + + + 0 + 0 + + + + + 30 + 0 + + + + EXPERIMENTAL: Allows the robot to reposition its sensor to look around to resolve apparent collisions. A sensor needs to be set up and the new measurements have to affect the collision scene, otherwise it has no effect. See tutorial. + - Allow Approx IK Solutions + Sensor Positioning + + + false - - - - Qt::Vertical - - - - 20 - 40 - - - - @@ -857,6 +994,18 @@ + + + 0 + 0 + + + + + 40 + 0 + + -99.000000000000000 @@ -867,6 +1016,18 @@ + + + 0 + 0 + + + + + 40 + 0 + + 0.010000000000000 @@ -877,6 +1038,18 @@ + + + 0 + 0 + + + + + 40 + 0 + + 0.010000000000000 @@ -887,6 +1060,18 @@ + + + 0 + 0 + + + + + 40 + 0 + + 0.010000000000000 @@ -904,6 +1089,18 @@ + + + 0 + 0 + + + + + 40 + 0 + + -99.000000000000000 @@ -921,6 +1118,18 @@ + + + 0 + 0 + + + + + 40 + 0 + + -99.000000000000000 @@ -1009,6 +1218,18 @@ + + + 0 + 0 + + + + + 20 + 0 + + x dimension @@ -1022,6 +1243,18 @@ + + + 0 + 0 + + + + + 20 + 0 + + y dimension @@ -1035,6 +1268,18 @@ + + + 0 + 0 + + + + + 20 + 0 + + z dimension @@ -1051,7 +1296,14 @@ - + + + + 30 + 0 + + + @@ -1124,8 +1376,11 @@ + + Change the pose and scale of the selected object + - Change pose and scale of selected object + Change object pose/scale @@ -1159,6 +1414,12 @@ + + + 20 + 0 + + -999.990000000000009 @@ -1172,6 +1433,12 @@ + + + 20 + 0 + + -3.140000000000000 @@ -1185,6 +1452,12 @@ + + + 20 + 0 + + -3.140000000000000 @@ -1198,6 +1471,12 @@ + + + 20 + 0 + + -3.140000000000000 @@ -1211,6 +1490,12 @@ + + + 20 + 0 + + -999.990000000000009 @@ -1242,7 +1527,7 @@ - 160 + 40 0 @@ -1280,6 +1565,12 @@ + + + 20 + 0 + + -999.990000000000009 @@ -1296,8 +1587,11 @@ + + Describes the selected object's pose, geometry and subframes. + - Status of selected object + Object status @@ -1329,6 +1623,9 @@ Qt::AutoText + + true + true @@ -1371,11 +1668,17 @@ - + 0 0 + + + 40 + 0 + + &Publish @@ -1396,6 +1699,18 @@ + + + 0 + 0 + + + + + 40 + 0 + + Export scene geometry to a .scene file (only geometry information is preserved) @@ -1406,6 +1721,18 @@ + + + 0 + 0 + + + + + 40 + 0 + + Import scene geometry from a .scene file (only geometry information is preserved) @@ -1820,25 +2147,35 @@ database_port reset_db_button database_connect_button + wcenter_x + wcenter_y + wcenter_z + wsize_x + wsize_y + wsize_z plan_button execute_button plan_and_execute_button stop_button + clear_octomap_button planning_group_combo_box start_state_combo_box goal_state_combo_box - clear_octomap_button + path_constraints_combo_box planning_time planning_attempts velocity_scaling_factor acceleration_scaling_factor - allow_replanning - allow_looking - allow_external_program + use_cartesian_path collision_aware_ik approximate_ik - path_constraints_combo_box - goal_tolerance + allow_external_program + allow_replanning + allow_looking + detected_objects_list + detect_objects_button + support_surfaces_list + pick_button place_button roi_center_x roi_center_y @@ -1847,6 +2184,13 @@ roi_size_y roi_size_z collision_objects_list + shape_size_x_spin_box + shape_size_z_spin_box + shape_size_y_spin_box + shapes_combo_box + add_object_button + remove_object_button + clear_scene_button object_x object_y object_z @@ -1874,16 +2218,6 @@ save_goal_state_button status_text tabWidget - wcenter_x - wcenter_y - wcenter_z - wsize_x - wsize_y - wsize_z - pick_button - support_surfaces_list - detected_objects_list - detect_objects_button From 75094667dc07ce47255cef1b91ab8b20f0b33996 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 24 Sep 2020 18:30:58 +0200 Subject: [PATCH 454/612] ikfast: add missing python dependencies --- moveit_kinematics/package.xml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 99ab7c3324..8adef0388f 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -30,8 +30,12 @@ orocos_kdl liborocos-kdl-dev - python-lxml - liburdfdom-tools + + liburdfdom-tools + python-lxml + python3-lxml + python-yaml + python3-yaml rostest moveit_ros_planning From 6cd2d7ac186ea66c31fb8d1047fc1c076e6f1bb4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 24 Sep 2020 18:47:55 +0200 Subject: [PATCH 455/612] Install python scripts via catkin_install_python() ... to correctly rewrite shebang --- moveit_commander/CMakeLists.txt | 5 +++-- moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt | 6 ++++++ moveit_ros/benchmarks/CMakeLists.txt | 5 +++-- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/moveit_commander/CMakeLists.txt b/moveit_commander/CMakeLists.txt index 74a0c315af..cb0618035e 100644 --- a/moveit_commander/CMakeLists.txt +++ b/moveit_commander/CMakeLists.txt @@ -7,7 +7,8 @@ catkin_python_setup() catkin_package() -install(PROGRAMS bin/${PROJECT_NAME}_cmdline.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +catkin_install_python( + PROGRAMS bin/${PROJECT_NAME}_cmdline.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) add_subdirectory(test) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt index 49c1a345ec..1d5f8399cd 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt @@ -7,6 +7,12 @@ set(MOVEIT_LIB_NAME moveit_ikfast_kinematics_plugin) install( PROGRAMS scripts/auto_create_ikfast_moveit_plugin.sh + DESTINATION + ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +catkin_install_python( + PROGRAMS scripts/create_ikfast_moveit_plugin.py scripts/round_collada_numbers.py DESTINATION diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt index 0eb7714e85..f64a99da75 100644 --- a/moveit_ros/benchmarks/CMakeLists.txt +++ b/moveit_ros/benchmarks/CMakeLists.txt @@ -62,7 +62,8 @@ install( install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) -install(PROGRAMS scripts/moveit_benchmark_statistics.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +catkin_install_python( + PROGRAMS scripts/moveit_benchmark_statistics.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY examples/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples) From e009acf335d5e960b38919072eb134b727b0bf83 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 24 Sep 2020 18:52:43 +0200 Subject: [PATCH 456/612] Remove shebang line from moveitjoy_module.py --- .../src/moveit_ros_visualization/moveitjoy_module.py | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py index 1b2408fb34..1bb5e7da7a 100644 --- a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py +++ b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python # ******************************************************************** # Software License Agreement (BSD License) # From 1d8d51e295cd74d3b37ceab8c012ff0fe73003c1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 26 Sep 2020 02:39:18 +0200 Subject: [PATCH 457/612] Adapt to API changes in geometric_shapes (#2324) https://github.com/ros-planning/geometric_shapes/pull/161 introduced a syntax simplification employing C++14 features (constexpr). --- moveit_core/kinematic_constraints/src/utils.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 088fff83db..11ef55a56c 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -167,7 +167,7 @@ moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name, pcm.constraint_region.primitives.resize(1); shape_msgs::SolidPrimitive& bv = pcm.constraint_region.primitives[0]; bv.type = shape_msgs::SolidPrimitive::SPHERE; - bv.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); + bv.dimensions.resize(geometric_shapes::solidPrimitiveDimCount()); bv.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance_pos; pcm.header = pose.header; @@ -203,7 +203,7 @@ moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name, { shape_msgs::SolidPrimitive& bv = goal.position_constraints[0].constraint_region.primitives[0]; bv.type = shape_msgs::SolidPrimitive::BOX; - bv.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); + bv.dimensions.resize(geometric_shapes::solidPrimitiveDimCount()); bv.dimensions[shape_msgs::SolidPrimitive::BOX_X] = tolerance_pos[0]; bv.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = tolerance_pos[1]; bv.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = tolerance_pos[2]; @@ -258,7 +258,7 @@ moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name, pcm.constraint_region.primitives.resize(1); pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE; pcm.constraint_region.primitives[0].dimensions.resize( - geometric_shapes::SolidPrimitiveDimCount::value); + geometric_shapes::solidPrimitiveDimCount()); pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance; pcm.header = goal_point.header; From 1af047c83a2d8051375aa2230a6bad8cebb873b2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 29 Sep 2020 21:43:07 +0200 Subject: [PATCH 458/612] Workaround python3 issue with lxml.ElementTree.write() Fixes https://github.com/ros-planning/moveit/issues/2322 --- .../scripts/create_ikfast_moveit_plugin.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py index e9ce8770d7..b71977adaa 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py @@ -182,8 +182,7 @@ def create_ikfast_package(args): user_name = getuser() root.append(xmlElement("maintainer", email="%s@todo.todo" % user_name, text=user_name)) root.append(xmlElement("buildtool_depend", text="catkin")) - with open(pkg_xml_path, 'w') as f: - etree.ElementTree(root).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8") + etree.ElementTree(root).write(pkg_xml_path, xml_declaration=True, pretty_print=True, encoding="UTF-8") print("Created package.xml at: '%s'" % pkg_xml_path) @@ -247,8 +246,7 @@ def update_ikfast_package(args): # Write plugin definition to file plugin_file_name = ik_library_name + "_description.xml" plugin_file_path = args.ikfast_plugin_pkg_path + "/" + plugin_file_name - with open(plugin_file_path, 'w') as f: - etree.ElementTree(plugin_def).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8") + etree.ElementTree(plugin_def).write(plugin_file_path, xml_declaration=True, pretty_print=True, encoding="UTF-8") print("Created plugin definition at '%s'" % plugin_file_path) # Create CMakeLists file @@ -286,8 +284,7 @@ def update_ikfast_package(args): # Always write the package xml file, even if there are no changes, to ensure # proper encodings are used in the future (UTF-8) - with open(package_file_name, "w") as f: - etree.ElementTree(package_xml).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8") + etree.ElementTree(package_xml).write(package_file_name, xml_declaration=True, pretty_print=True, encoding="UTF-8") print("Wrote package.xml at '%s'" % package_file_name) # Create a script for easily updating the plugin in the future in case the plugin needs to be updated From 26c2bf6a9e3a6d5fd30a09157fcf6d2eede7006b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 29 Sep 2020 22:01:56 +0200 Subject: [PATCH 459/612] Fix some more issues with create_ikfast_moveit_plugin.sh - require python-lxml and python-yaml to be installed - use str() to print Exception --- .../scripts/auto_create_ikfast_moveit_plugin.sh | 2 ++ .../scripts/create_ikfast_moveit_plugin.py | 4 ++-- moveit_kinematics/test/test_ikfast_plugins.sh | 11 +++++++++++ 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh b/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh index 8d506f1091..a04e2077c7 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh @@ -8,6 +8,8 @@ # TODO: Would be nice to integrate this functionality directly into create_ikfast_moveit_plugin.py +set -e # fail on error + function print_help { echo "$(basename $0) [--help|-h] [--quiet|-q] [--keep] [--name|-n] [--pkg|-p] [--iktype|-t] " echo " input .urdf, .dae, or .cpp input file" diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py index b71977adaa..bfa5785947 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py @@ -61,7 +61,7 @@ class InvalidROSPkgException(Exception): pass def get_pkg_dir(pkg_name): - raise InvalidROSPkgException + raise InvalidROSPkgException('Failed to locate ROS package {}'.format(pkg_name)) # Package containing this file plugin_gen_pkg = 'moveit_kinematics' @@ -389,7 +389,7 @@ def main(): try: update_moveit_package(args) except Exception as e: - print("Failed to update MoveIt package:\n" + e.message) + print("Failed to update MoveIt package:\n" + str(e)) if __name__ == '__main__': diff --git a/moveit_kinematics/test/test_ikfast_plugins.sh b/moveit_kinematics/test/test_ikfast_plugins.sh index c023161a16..d96c9e19f8 100755 --- a/moveit_kinematics/test/test_ikfast_plugins.sh +++ b/moveit_kinematics/test/test_ikfast_plugins.sh @@ -7,6 +7,17 @@ # We will create ikfast plugins for fanuc and panda from moveit_resources # using the script auto_create_ikfast_moveit_plugin.sh +sudo update-alternatives --install /usr/bin/python python /usr/bin/python2 1 +sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 2 + +if [ "$ROS_DISTRO" == "noetic" ]; then + sudo update-alternatives --set python /usr/bin/python3 + travis_run sudo apt-get -qq install python3-lxml python3-yaml +else + sudo update-alternatives --set python /usr/bin/python2 + travis_run sudo apt-get -qq install python-lxml python-yaml +fi + # Clone moveit_resources for URDFs. They are not available before running docker. travis_run git clone -q --depth=1 https://github.com/ros-planning/moveit_resources /tmp/resources fanuc=/tmp/resources/fanuc_description/urdf/fanuc.urdf From a9e2fbc8b5af86340d03fbec237d3dfc40e8a187 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 29 Sep 2020 22:03:14 +0200 Subject: [PATCH 460/612] Travis: default to Noetic --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index a2a03182fd..7b038728a7 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,7 +13,7 @@ notifications: env: global: - MOVEIT_CI_TRAVIS_TIMEOUT=85 # Travis grants us 90 min, but we add a safety margin of 5 min - - ROS_DISTRO=melodic + - ROS_DISTRO=noetic - ROS_REPO=ros - UPSTREAM_WORKSPACE=moveit.rosinstall - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" @@ -25,7 +25,7 @@ jobs: - env: TEST="clang-format catkin_lint" - env: TEST=code-coverage - env: ROS_DISTRO=noetic ROS_REPO=ros-testing - - env: ROS_DISTRO=melodic + - env: ROS_DISTRO=melodic BEFORE_DOCKER_SCRIPT="source moveit_kinematics/test/test_ikfast_plugins.sh" - compiler: clang # test_ikfast_plugins takes ~10 minutes: include here to keep the main jobs shorter env: TEST=clang-tidy-fix From d120f5d5418ab6110fcb270cbe63239a53439407 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 30 Sep 2020 07:56:52 +0200 Subject: [PATCH 461/612] IKFast: suppress unused-parameter warnings --- .../ikfast_kinematics_plugin/templates/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt index 9c19bba37a..9f874d8071 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt @@ -26,7 +26,7 @@ set(IKFAST_LIBRARY_NAME _LIBRARY_NAME_) add_library(${IKFAST_LIBRARY_NAME} src/_ROBOT_NAME___GROUP_NAME__ikfast_moveit_plugin.cpp) target_link_libraries(${IKFAST_LIBRARY_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${LAPACK_LIBRARIES}) # suppress warnings about unused variables in OpenRave's solver code -target_compile_options(${IKFAST_LIBRARY_NAME} PRIVATE -Wno-unused-variable) +target_compile_options(${IKFAST_LIBRARY_NAME} PRIVATE -Wno-unused-variable -Wno-unused-parameter) install(TARGETS ${IKFAST_LIBRARY_NAME} From 2954fa2c5f493c7ae2b83892153a2507e23db8b7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 30 Sep 2020 07:52:20 +0200 Subject: [PATCH 462/612] Remove custom copy-assignment operator for DistanceResultsData It was just performing the operations of the default operator. Newer clang requires either both or none of copy constructor and assignment operator to be defined. warning: definition of implicit copy constructor for 'DistanceResultsData' is deprecated because it has a user-declared copy assignment operator [-Wdeprecated-copy] --- .../moveit/collision_detection/collision_common.h | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 93d594de29..53eff3c7a7 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -342,19 +342,6 @@ struct DistanceResultsData normal.setZero(); } - /// Update structure data given DistanceResultsData object - void operator=(const DistanceResultsData& other) - { - distance = other.distance; - nearest_points[0] = other.nearest_points[0]; - nearest_points[1] = other.nearest_points[1]; - link_names[0] = other.link_names[0]; - link_names[1] = other.link_names[1]; - body_types[0] = other.body_types[0]; - body_types[1] = other.body_types[1]; - normal = other.normal; - } - /// Compare if the distance is less than another bool operator<(const DistanceResultsData& other) { From 99ceb7e50b202e95480f491a8d60847bdd1c1c78 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 30 Sep 2020 08:01:52 +0200 Subject: [PATCH 463/612] perception: add missing dependency to OpenMP --- moveit_ros/perception/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml index 19e72c6bd2..f6e6dc57dd 100644 --- a/moveit_ros/perception/package.xml +++ b/moveit_ros/perception/package.xml @@ -27,6 +27,7 @@ image_transport glut libglew-dev + libomp-dev opengl cv_bridge sensor_msgs From c2e3f27f83b0c9f3fcd38a6b8aebab5bfee54de9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 30 Sep 2020 09:23:45 +0200 Subject: [PATCH 464/612] Silent deprecation warnings of gtest's typed test suites --- moveit_core/collision_detection_bullet/CMakeLists.txt | 4 ++++ moveit_core/collision_detection_fcl/CMakeLists.txt | 4 ++++ moveit_ros/perception/mesh_filter/CMakeLists.txt | 2 ++ 3 files changed, 10 insertions(+) diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index 66a1e09e4b..775fba1bd3 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -30,9 +30,13 @@ install(FILES ../collision_detector_bullet_description.xml DESTINATION ${CATKIN_ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_bullet_collision_detection test/test_bullet_collision_detection_pr2.cpp) target_link_libraries(test_bullet_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_bullet_collision_detection PRIVATE -Wno-deprecated-declarations) catkin_add_gtest(test_bullet_collision_detection_panda test/test_bullet_collision_detection_panda.cpp) target_link_libraries(test_bullet_collision_detection_panda moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_bullet_collision_detection_panda PRIVATE -Wno-deprecated-declarations) catkin_add_gtest(test_bullet_continuous_collision_checking test/test_bullet_continuous_collision_checking.cpp) target_link_libraries(test_bullet_continuous_collision_checking moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index 90f260c8aa..48bb00e6e6 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -26,7 +26,11 @@ install(FILES ../collision_detector_fcl_description.xml DESTINATION ${CATKIN_PAC if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection_pr2.cpp) target_link_libraries(test_fcl_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_fcl_collision_detection PRIVATE -Wno-deprecated-declarations) catkin_add_gtest(test_fcl_collision_detection_panda test/test_fcl_collision_detection_panda.cpp) target_link_libraries(test_fcl_collision_detection_panda moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_fcl_collision_detection_panda PRIVATE -Wno-deprecated-declarations) endif() diff --git a/moveit_ros/perception/mesh_filter/CMakeLists.txt b/moveit_ros/perception/mesh_filter/CMakeLists.txt index 8d8a00743b..100350e472 100644 --- a/moveit_ros/perception/mesh_filter/CMakeLists.txt +++ b/moveit_ros/perception/mesh_filter/CMakeLists.txt @@ -17,6 +17,8 @@ if (CATKIN_ENABLE_TESTING) if (DEFINED ENV{DISPLAY} AND NOT $ENV{DISPLAY} STREQUAL "") catkin_add_gtest(mesh_filter_test test/mesh_filter_test.cpp) target_link_libraries(mesh_filter_test ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_mesh_filter) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(mesh_filter_test PRIVATE -Wno-deprecated-declarations) else() message("No display, will not configure tests for moveit_ros_perception/mesh_filter") endif() From 76ea533961ab98aa3a9b486e41b2b1b8290b3323 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 30 Sep 2020 11:46:37 +0200 Subject: [PATCH 465/612] Handle multiple link libraries for FCL (#2325) --- moveit_core/CMakeLists.txt | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 05c127fcde..1aa2450ff0 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -48,10 +48,13 @@ endif() find_package(PkgConfig REQUIRED) pkg_check_modules(LIBFCL_PC REQUIRED fcl) -# find *absolute* paths to LIBFCL_INCLUDE_DIRS and LIBFCL_LIBRARIES -find_path(LIBFCL_INCLUDE_DIRS fcl/config.h HINTS ${LIBFCL_PC_INCLUDE_DIR} ${LIBFCL_PC_INCLUDE_DIRS}) -find_library(LIBFCL_LIBRARIES fcl HINTS ${LIBFCL_PC_LIBRARY_DIRS}) - +set(LIBFCL_INCLUDE_DIRS ${LIBFCL_PC_INCLUDE_DIRS}) +# find *absolute* paths to LIBFCL_LIBRARIES +set(LIBFCL_LIBRARIES) +foreach(_lib ${LIBFCL_PC_LIBRARIES}) + find_library(_lib_${_lib} ${_lib} HINTS ${LIBFCL_PC_LIBRARY_DIRS}) + list(APPEND LIBFCL_LIBRARIES ${_lib_${_lib}}) +endforeach() find_package(octomap REQUIRED) find_package(urdfdom REQUIRED) From 89d63e83ad5e7f319aef65aeecc2e3adf97596be Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 1 Oct 2020 11:06:37 +0200 Subject: [PATCH 466/612] Fix clang-tidy warning (#2334) --- .../src/motion_planning_frame_objects.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 00f0f09a70..bdbe36e17a 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -61,7 +61,7 @@ namespace QString subframe_poses_to_qstring(const moveit::core::FixedTransformsMap& subframes) { QString status_text = "\nIt has the subframes '"; - for (auto subframe : subframes) + for (const auto& subframe : subframes) { status_text += QString::fromStdString(subframe.first) + "', '"; } From 67a37cd08020645a35cf3f684f302a33c01bdd6d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 1 Oct 2020 17:20:06 +0200 Subject: [PATCH 467/612] MSA: Fix disappearing robot on change of reference frame (#2335) - set visibility of robot-state-display to true after reset() - correctly update reference frame also on deletion of virtual joint --- moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp | 1 + moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 61fd9145c9..6b32e381cf 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -187,6 +187,7 @@ void SetupAssistantWidget::virtualJointReferenceFrameChanged() { rviz_manager_->setFixedFrame(QString::fromStdString(config_data_->getRobotModel()->getModelFrame())); robot_state_display_->reset(); + robot_state_display_->setVisible(true); } } diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp index 4227d6a984..ef7f1a137d 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp @@ -436,6 +436,7 @@ void VirtualJointsWidget::deleteSelected() // Reload main screen table loadDataTable(); config_data_->changes |= MoveItConfigData::VIRTUAL_JOINTS; + Q_EMIT referenceFrameChanged(); } // ****************************************************************************************** From a29a30caaecbd130d85056d959d4eb1c30d4088f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 1 Oct 2020 22:30:05 +0200 Subject: [PATCH 468/612] Fix some clang-tidy issues on Travis (#2337) * Replace typedefs with using declarations * Move default destructor definitions to headers * Silent spurious clang-tidy warning * Move variable definitions to their usage location --- .../moveit/collision_detection/collision_common.h | 10 +++++----- .../moveit/collision_detection/collision_env.h | 4 ++-- .../moveit/collision_detection/collision_matrix.h | 2 +- .../include/moveit/collision_detection/world.h | 4 ++-- .../include/moveit/collision_detection/world_diff.h | 2 +- .../collision_detection_fcl/src/collision_common.cpp | 4 ++-- .../include/moveit/kinematics_base/kinematics_base.h | 9 ++++----- .../planning_request_adapter.h | 7 +++---- .../include/moveit/planning_scene/planning_scene.h | 11 ++++++----- .../include/moveit/robot_model/joint_model.h | 8 ++++---- .../include/moveit/robot_model/joint_model_group.h | 10 +++++----- .../include/moveit/robot_model/link_model.h | 7 +++---- .../iterative_spline_parameterization.h | 2 +- .../iterative_time_parameterization.h | 2 +- .../src/iterative_spline_parameterization.cpp | 2 -- .../src/iterative_time_parameterization.cpp | 2 -- .../transforms/include/moveit/transforms/transforms.h | 5 ++--- .../moveit/occupancy_map_monitor/occupancy_map.h | 8 ++++---- .../occupancy_map_monitor/occupancy_map_updater.h | 10 ++++------ .../moveit/plan_execution/plan_representation.h | 2 +- .../planning_scene_monitor/current_state_monitor.h | 4 ++-- .../planning_scene_monitor/planning_scene_monitor.h | 9 ++++----- .../planning_scene_monitor/trajectory_monitor.h | 3 +-- .../trajectory_execution_manager.h | 2 +- 24 files changed, 59 insertions(+), 70 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 53eff3c7a7..fcda6e9efc 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -67,7 +67,7 @@ enum Type } // namespace BodyTypes /** \brief The types of bodies that are considered for collision */ -typedef BodyTypes::Type BodyType; +using BodyType = BodyTypes::Type; /** \brief Definition of a contact point */ struct Contact @@ -147,7 +147,7 @@ struct CollisionResult CollisionResult() : collision(false), distance(std::numeric_limits::max()), contact_count(0) { } - typedef std::map, std::vector > ContactMap; + using ContactMap = std::map, std::vector >; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -240,7 +240,7 @@ enum DistanceRequestType ALL ///< Find all the contacts for a given pair }; } -typedef DistanceRequestTypes::DistanceRequestType DistanceRequestType; +using DistanceRequestType = DistanceRequestTypes::DistanceRequestType; /** \brief Representation of a distance-reporting request */ struct DistanceRequest @@ -303,7 +303,7 @@ struct DistanceRequest }; /** \brief Generic representation of the distance information for a pair of objects */ -struct DistanceResultsData +struct DistanceResultsData // NOLINT(readability-identifier-naming) - suppress spurious clang-tidy warning { DistanceResultsData() { @@ -356,7 +356,7 @@ struct DistanceResultsData }; /** \brief Mapping between the names of the collision objects and the DistanceResultData. */ -typedef std::map, std::vector > DistanceMap; +using DistanceMap = std::map, std::vector >; /** \brief Result of a distance request. */ struct DistanceResult diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h index 0dc01a5a10..9177fae307 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h @@ -248,8 +248,8 @@ class CollisionEnv return world_const_; } - typedef World::ObjectPtr ObjectPtr; - typedef World::ObjectConstPtr ObjectConstPtr; + using ObjectPtr = World::ObjectPtr; + using ObjectConstPtr = World::ObjectConstPtr; /** @brief The kinematic model corresponding to this collision model*/ const moveit::core::RobotModelConstPtr& getRobotModel() const diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index 3072591919..4b7a72910b 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -69,7 +69,7 @@ enum Type /** \brief Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type is * CONDITIONAL) */ -typedef boost::function DecideContactFn; +using DecideContactFn = boost::function; MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index 6113dd6172..c9a52c3e6b 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -119,7 +119,7 @@ class World ObjectConstPtr getObject(const std::string& object_id) const; /** iterator over the objects in the world. */ - typedef std::map::const_iterator const_iterator; + using const_iterator = std::map::const_iterator; /** iterator pointing to first change */ const_iterator begin() const { @@ -254,7 +254,7 @@ class World friend class World; }; - typedef boost::function ObserverCallbackFn; + using ObserverCallbackFn = boost::function; /** \brief register a callback function for notification of changes. * \e callback will be called right after any change occurs to any Object. diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h index 9660d2dbce..1ffbceafda 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h @@ -76,7 +76,7 @@ class WorldDiff return changes_; } - typedef std::map::const_iterator const_iterator; + using const_iterator = std::map::const_iterator; /** iterator pointing to first change */ const_iterator begin() const { diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index bc136f28f7..4e12a5610e 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -493,8 +493,6 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void ROS_DEBUG_NAMED("collision_detection.fcl", "Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str()); - fcl::DistanceResultd fcl_result; - DistanceResultsData dist_result; double dist_threshold = cdata->req->distance_threshold; const std::pair& pc = cd1->getID() < cd2->getID() ? @@ -523,6 +521,7 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void } } + fcl::DistanceResultd fcl_result; fcl_result.min_distance = dist_threshold; // fcl::distance segfaults when given an octree with a null root pointer (using FCL 0.6.1) if ((o1->getObjectType() == fcl::OT_OCTREE && @@ -539,6 +538,7 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void // one and add the new distance information. if (d < dist_threshold) { + DistanceResultsData dist_result; dist_result.distance = fcl_result.min_distance; #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) dist_result.nearest_points[0] = fcl_result.nearest_points[0]; diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 1a9b3c9b69..0c62197dbd 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -76,7 +76,7 @@ enum DiscretizationMethod The unused redundant joints will be fixed at their current value. */ }; } // namespace DiscretizationMethods -typedef DiscretizationMethods::DiscretizationMethod DiscretizationMethod; +using DiscretizationMethod = DiscretizationMethods::DiscretizationMethod; /* * @enum KinematicErrors @@ -98,7 +98,7 @@ enum KinematicError }; } // namespace KinematicErrors -typedef KinematicErrors::KinematicError KinematicError; +using KinematicError = KinematicErrors::KinematicError; /** * @struct KinematicsQueryOptions @@ -148,9 +148,8 @@ class KinematicsBase static const double DEFAULT_TIMEOUT; /* = 1.0 */ /** @brief Signature for a callback to validate an IK solution. Typically used for collision checking. */ - typedef boost::function& ik_solution, - moveit_msgs::MoveItErrorCodes& error_code)> - IKCallbackFn; + using IKCallbackFn = + boost::function&, moveit_msgs::MoveItErrorCodes&)>; /** * @brief Given a desired pose of the end-effector, compute the joint angles to reach it diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h index 1f834b71ea..c721b45134 100644 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h @@ -49,10 +49,9 @@ MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); // Defines PlanningRequestAdapter class PlanningRequestAdapter { public: - typedef boost::function - PlannerFn; + using PlannerFn = + boost::function; PlanningRequestAdapter() { diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 74c6532847..c4f648959e 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -72,13 +72,14 @@ typedef boost::function StateFeasib The order of the arguments matters: the notion of feasibility is to be checked for motion segments that start at the first state and end at the second state. The third argument indicates whether the check should be verbose or not. */ -typedef boost::function MotionFeasibilityFn; +using MotionFeasibilityFn = + boost::function; /** \brief A map from object names (e.g., attached bodies, collision objects) to their colors */ -typedef std::map ObjectColorMap; +using ObjectColorMap = std::map; /** \brief A map from object names (e.g., attached bodies, collision objects) to their types */ -typedef std::map ObjectTypeMap; +using ObjectTypeMap = std::map; /** \brief This class maintains the representation of the environment as seen by a planning instance. The environment @@ -1000,8 +1001,8 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from }; friend struct CollisionDetector; - typedef std::map::iterator CollisionDetectorIterator; - typedef std::map::const_iterator CollisionDetectorConstIterator; + using CollisionDetectorIterator = std::map::iterator; + using CollisionDetectorConstIterator = std::map::const_iterator; void allocateCollisionDetectors(); void allocateCollisionDetectors(CollisionDetector& detector); diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index c0d37829e2..928a19d1ff 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -84,13 +84,13 @@ class JointModel; typedef std::map VariableIndexMap; /** \brief Data type for holding mappings from variable names to their bounds */ -typedef std::map VariableBoundsMap; +using VariableBoundsMap = std::map; /** \brief Map of names to instances for JointModel */ -typedef std::map JointModelMap; +using JointModelMap = std::map; /** \brief Map of names to const instances for JointModel */ -typedef std::map JointModelMapConst; +using JointModelMapConst = std::map; /** \brief A joint from the robot. Models the transform that this joint applies in the kinematic chain. A joint @@ -119,7 +119,7 @@ class JointModel }; /** \brief The datatype for the joint bounds */ - typedef std::vector Bounds; + using Bounds = std::vector; /** \brief Construct a joint named \e name */ JointModel(const std::string& name); diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index 64ea874bda..7626306b6d 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -55,15 +55,15 @@ class JointModelGroup; typedef boost::function SolverAllocatorFn; /** \brief Map from group instances to allocator functions & bijections */ -typedef std::map SolverAllocatorMapFn; +using SolverAllocatorMapFn = std::map; /** \brief Map of names to instances for JointModelGroup */ -typedef std::map JointModelGroupMap; +using JointModelGroupMap = std::map; /** \brief Map of names to const instances for JointModelGroup */ -typedef std::map JointModelGroupMapConst; +using JointModelGroupMapConst = std::map; -typedef std::vector JointBoundsVector; +using JointBoundsVector = std::vector; class JointModelGroup { @@ -102,7 +102,7 @@ class JointModelGroup }; /// Map from group instances to allocator functions & bijections - typedef std::map KinematicsSolverMap; + using KinematicsSolverMap = std::map; JointModelGroup(const std::string& name, const srdf::Model::Group& config, const std::vector& joint_vector, const RobotModel* parent_model); diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.h b/moveit_core/robot_model/include/moveit/robot_model/link_model.h index ce799745da..3bd2dfbe00 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/link_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.h @@ -61,12 +61,11 @@ class LinkModel; typedef std::map LinkModelMap; /** \brief Map of names to const instances for LinkModel */ -typedef std::map LinkModelMapConst; +using LinkModelMapConst = std::map; /** \brief Map from link model instances to Eigen transforms */ -typedef std::map, - Eigen::aligned_allocator > > - LinkTransformMap; +using LinkTransformMap = std::map, + Eigen::aligned_allocator > >; /** \brief A link from the robot. Contains the constant transform applied to the link and its geometry */ class LinkModel diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h index bee235d209..92f14befdb 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h @@ -71,7 +71,7 @@ class IterativeSplineParameterization { public: IterativeSplineParameterization(bool add_points = true); - ~IterativeSplineParameterization(); + ~IterativeSplineParameterization() = default; bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0) const; diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h index 7a298642fb..b142f987a5 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h @@ -46,7 +46,7 @@ class IterativeParabolicTimeParameterization { public: IterativeParabolicTimeParameterization(unsigned int max_iterations = 100, double max_time_change_per_it = .01); - ~IterativeParabolicTimeParameterization(); + ~IterativeParabolicTimeParameterization() = default; bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0) const; diff --git a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp index f12d01314d..855c3a7929 100644 --- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp @@ -76,8 +76,6 @@ IterativeSplineParameterization::IterativeSplineParameterization(bool add_points { } -IterativeSplineParameterization::~IterativeSplineParameterization() = default; - bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor) const diff --git a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp index e739158fcb..afef388e49 100644 --- a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp @@ -50,8 +50,6 @@ IterativeParabolicTimeParameterization::IterativeParabolicTimeParameterization(u { } -IterativeParabolicTimeParameterization::~IterativeParabolicTimeParameterization() = default; - namespace { void printPoint(const trajectory_msgs::JointTrajectoryPoint& point, std::size_t i) diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 0a13c7b85e..e5a26c4c5f 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -49,9 +49,8 @@ MOVEIT_CLASS_FORWARD(Transforms); // Defines TransformsPtr, ConstPtr, WeakPtr.. /// @brief Map frame names to the transformation matrix that can transform objects from the frame name to the planning /// frame -typedef std::map, - Eigen::aligned_allocator > > - FixedTransformsMap; +using FixedTransformsMap = std::map, + Eigen::aligned_allocator > >; /** @brief Provides an implementation of a snapshot of a transform tree that can be easily queried for transforming different quantities. Transforms are maintained as a list of transforms to a particular frame. diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h index 875f3cab22..9ea4815e8e 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h @@ -87,8 +87,8 @@ class OccMapTree : public octomap::OcTree tree_mutex_.unlock(); } - typedef boost::shared_lock ReadLock; - typedef boost::unique_lock WriteLock; + using ReadLock = boost::shared_lock; + using WriteLock = boost::unique_lock; ReadLock reading() { @@ -117,6 +117,6 @@ class OccMapTree : public octomap::OcTree boost::function update_callback_; }; -typedef std::shared_ptr OccMapTreePtr; -typedef std::shared_ptr OccMapTreeConstPtr; +using OccMapTreePtr = std::shared_ptr; +using OccMapTreeConstPtr = std::shared_ptr; } // namespace occupancy_map_monitor diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h index 902246a2a4..408a6f7c32 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h @@ -44,12 +44,10 @@ namespace occupancy_map_monitor { -typedef unsigned int ShapeHandle; -typedef std::map, - Eigen::aligned_allocator > > - ShapeTransformCache; -typedef boost::function - TransformCacheProvider; +using ShapeHandle = unsigned int; +using ShapeTransformCache = std::map, + Eigen::aligned_allocator > >; +using TransformCacheProvider = boost::function; class OccupancyMapMonitor; diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h index fab59a19a1..8e2c728ccb 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h @@ -85,5 +85,5 @@ struct ExecutableMotionPlan }; /// The signature of a function that can compute a motion plan -typedef boost::function ExecutableMotionPlanComputationFn; +using ExecutableMotionPlanComputationFn = boost::function; } // namespace plan_execution diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h index 7203eaa3eb..558a3c8c12 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h @@ -46,13 +46,13 @@ namespace planning_scene_monitor { -typedef boost::function JointStateUpdateCallback; +using JointStateUpdateCallback = boost::function; /** @class CurrentStateMonitor @brief Monitors the joint_states topic and tf to maintain the current state of the robot. */ class CurrentStateMonitor { - typedef boost::signals2::connection TFConnection; + using TFConnection = boost::signals2::connection; public: /** diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index 941bb266c6..34591d2ceb 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -519,11 +519,10 @@ class PlanningSceneMonitor : private boost::noncopyable typedef std::map > > LinkShapeHandles; - typedef std::map > > - AttachedBodyShapeHandles; - typedef std::map > > - CollisionBodyShapeHandles; + using AttachedBodyShapeHandles = std::map > >; + using CollisionBodyShapeHandles = + std::map > >; LinkShapeHandles link_shape_handles_; AttachedBodyShapeHandles attached_body_shape_handles_; diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h index c5aa8142b2..4d30d4ec7d 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h @@ -44,8 +44,7 @@ namespace planning_scene_monitor { -typedef boost::function - TrajectoryStateAddedCallback; +using TrajectoryStateAddedCallback = boost::function; MOVEIT_CLASS_FORWARD(TrajectoryMonitor); // Defines TrajectoryMonitorPtr, ConstPtr, WeakPtr... etc diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index 1506513902..1ec1bcc79a 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -67,7 +67,7 @@ class TrajectoryExecutionManager /// Definition of the function signature that is called when the execution of a pushed trajectory completes /// successfully. - typedef boost::function PathSegmentCompleteCallback; + using PathSegmentCompleteCallback = boost::function; /// Data structure that represents information necessary to execute a trajectory struct TrajectoryExecutionContext From 023b11def6329b165ed7509e5de9aec4c1e29c6c Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 2 Oct 2020 14:09:57 -0600 Subject: [PATCH 469/612] [fix] Segfault when editing pose in moveit_setup_assistant (#2340) --- .../src/widgets/robot_poses_widget.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index d89889fd9c..34d6c12d02 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -321,13 +321,17 @@ void RobotPosesWidget::editDoubleClicked(int /*row*/, int /*column*/) // ****************************************************************************************** void RobotPosesWidget::previewClicked(int row, int /*column*/, int /*previous_row*/, int /*previous_column*/) { - const std::string& name = data_table_->item(row, 0)->text().toStdString(); - const std::string& group = data_table_->item(row, 1)->text().toStdString(); + QTableWidgetItem* name = data_table_->item(row, 0); + QTableWidgetItem* group = data_table_->item(row, 1); - // Find the selected in datastructure - srdf::Model::GroupState* pose = findPoseByName(name, group); + // nullptr check before dereferencing + if (name && group) + { + // Find the selected in datastructure + srdf::Model::GroupState* pose = findPoseByName(name->text().toStdString(), group->text().toStdString()); - showPose(pose); + showPose(pose); + } } // ****************************************************************************************** From 47884198c2585215de8f365a7ff20479f8bb4b51 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Sun, 4 Oct 2020 18:48:42 +0900 Subject: [PATCH 470/612] Fix "Clear Octomap" button, disable when no octomap is published (#2320) * Enable/disable "Clear Octomap" button * Correctly remove octomap from scene in clearOctomap() --- .../planning_scene_monitor/src/planning_scene_monitor.cpp | 2 ++ .../src/motion_planning_frame_objects.cpp | 5 +++++ .../src/motion_planning_frame_planning.cpp | 1 + .../src/ui/motion_planning_rviz_plugin_frame.ui | 3 +++ 4 files changed, 11 insertions(+) diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index aa43572346..799c5df0f2 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -531,6 +531,8 @@ void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningS void PlanningSceneMonitor::clearOctomap() { + if (scene_->getWorldNonConst()->removeObject(scene_->OCTOMAP_NS)) + triggerSceneUpdateEvent(UPDATE_SCENE); if (octomap_monitor_) { octomap_monitor_->getOcTreePtr()->lockWrite(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index bdbe36e17a..c8a1316687 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -921,6 +921,7 @@ void MotionPlanningFrame::populateCollisionObjectsList() { ui_->collision_objects_list->setUpdatesEnabled(false); bool old_state = ui_->collision_objects_list->blockSignals(true); + bool octomap_in_scene = false; { QList sel = ui_->collision_objects_list->selectedItems(); @@ -938,7 +939,10 @@ void MotionPlanningFrame::populateCollisionObjectsList() for (std::size_t i = 0; i < collision_object_names.size(); ++i) { if (collision_object_names[i] == planning_scene::PlanningScene::OCTOMAP_NS) + { + octomap_in_scene = true; continue; + } QListWidgetItem* item = new QListWidgetItem(QString::fromStdString(collision_object_names[i]), ui_->collision_objects_list, (int)i); @@ -970,6 +974,7 @@ void MotionPlanningFrame::populateCollisionObjectsList() } } + ui_->clear_octomap_button->setEnabled(octomap_in_scene); ui_->collision_objects_list->blockSignals(old_state); ui_->collision_objects_list->setUpdatesEnabled(true); selectedCollisionObjectChanged(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index b847da57b4..f0b32d8935 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -110,6 +110,7 @@ void MotionPlanningFrame::onClearOctomapClicked() { std_srvs::Empty srv; clear_octomap_service_client_.call(srv); + ui_->clear_octomap_button->setEnabled(false); } bool MotionPlanningFrame::computeCartesianPlan() diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index acde5aa80d..aa322f4a70 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -445,6 +445,9 @@ + + false + Clear octomap From 9d917fbc7e326d29498a1c10776a3b3d21cae84c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 5 Oct 2020 04:27:25 -0500 Subject: [PATCH 471/612] moveit_cpp: more informative error message, cover another potential failure condition. (#2336) --- .../planning_interface/moveit_cpp/src/moveit_cpp.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index 1db8d8f33c..40b300fc73 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -225,7 +225,9 @@ std::set MoveItCpp::getPlanningPipelineNames(const std::string& gro std::set result_names; if (!group_name.empty() && groups_pipelines_map_.count(group_name) == 0) { - ROS_ERROR_NAMED(LOGNAME, "There are no planning pipelines loaded for group '%s'.", group_name.c_str()); + ROS_ERROR_NAMED(LOGNAME, + "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.", + group_name.c_str()); return result_names; // empty } for (const auto& pipeline_entry : planning_pipelines_) @@ -240,6 +242,11 @@ std::set MoveItCpp::getPlanningPipelineNames(const std::string& gro } result_names.insert(pipeline_name); } + // No valid planning pipelines + if (result_names.empty()) + ROS_ERROR_NAMED(LOGNAME, + "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.", + group_name.c_str()); return result_names; } From c86394fc1640c91d2fd04cf55c2b8ff567cadfb4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 5 Oct 2020 15:04:40 +0200 Subject: [PATCH 472/612] MSA: Update RobotState *before* visualization --- .../src/widgets/robot_poses_widget.cpp | 28 ++++++------------- 1 file changed, 8 insertions(+), 20 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index 34d6c12d02..4fbda0be56 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -806,36 +806,24 @@ void RobotPosesWidget::updateRobotModel(const std::string& name, double value) // ****************************************************************************************** void RobotPosesWidget::publishJoints() { - // Change the scene - // scene.getCurrentState().setToDefaultValues();//set to default values of 0 OR half between low and high joint values - // config_data_->getPlanningScene()->getCurrentState().setToRandomValues(); - // Set the joints based on the map - config_data_->getPlanningScene()->getCurrentStateNonConst().setVariablePositions(joint_state_map_); + moveit::core::RobotState& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); + robot_state.setVariablePositions(joint_state_map_); + // Prevent dirty collision body transforms + robot_state.update(); // Create a planning scene message moveit_msgs::DisplayRobotState msg; - moveit::core::robotStateToRobotStateMsg(config_data_->getPlanningScene()->getCurrentState(), msg.state); + moveit::core::robotStateToRobotStateMsg(robot_state, msg.state); // Publish! pub_robot_state_.publish(msg); - // Prevent dirty collision body transforms - config_data_->getPlanningScene()->getCurrentStateNonConst().update(); - // Decide if current state is in collision collision_detection::CollisionResult result; - config_data_->getPlanningScene()->checkSelfCollision( - request, result, config_data_->getPlanningScene()->getCurrentState(), config_data_->allowed_collision_matrix_); - // Show result notification - if (!result.contacts.empty()) - { - collision_warning_->show(); - } - else - { - collision_warning_->hide(); - } + config_data_->getPlanningScene()->checkSelfCollision(request, result, robot_state, + config_data_->allowed_collision_matrix_); + collision_warning_->setHidden(result.contacts.empty()); } // ****************************************************************************************** From 46a754a9ec43f45d10fa64e8315e598c49d6f864 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 5 Oct 2020 17:08:19 +0200 Subject: [PATCH 473/612] remove commented, copy-pasted code --- .../src/widgets/virtual_joints_widget.cpp | 23 ------------------- 1 file changed, 23 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp index ef7f1a137d..e5b42fc197 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp @@ -255,29 +255,6 @@ void VirtualJointsWidget::editDoubleClicked(int /*row*/, int /*column*/) // ****************************************************************************************** void VirtualJointsWidget::previewClicked(int /*row*/, int /*column*/) { - // TODO: highlight the virtual joint? - - /* // Get list of all selected items - QList selected = data_table_->selectedItems(); - - // Check that an element was selected - if( !selected.size() ) - return; - - // Find the selected in datastructure - srdf::Model::GroupState *vjoint = findVjointByName( selected[0]->text().toStdString() ); - - // Set vjoint joint values by adding them to the local joint state map - for( std::map >::const_iterator value_it = vjoint->joint_values_.begin(); - value_it != vjoint->joint_values_.end(); ++value_it ) - { - // Only copy the first joint value - joint_state_map_[ value_it->first ] = value_it->second[0]; - } - - // Update the joints - publishJoints(); - */ } // ****************************************************************************************** From 865f0fdfb0feaf1fc2c5233c786eb96cbdf30073 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 5 Oct 2020 17:08:53 +0200 Subject: [PATCH 474/612] Use PlanningScene's RobotState for postures .. instead of extra joint_state_map_ --- .../src/widgets/robot_poses_widget.cpp | 86 ++++++------------- .../src/widgets/robot_poses_widget.h | 6 -- 2 files changed, 24 insertions(+), 68 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index 4fbda0be56..2c1ef14747 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -339,12 +339,12 @@ void RobotPosesWidget::previewClicked(int row, int /*column*/, int /*previous_ro // ****************************************************************************************** void RobotPosesWidget::showPose(srdf::Model::GroupState* pose) { - // Set pose joint values by adding them to the local joint state map + // Set the joints based on the SRDF pose + moveit::core::RobotState& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); for (std::map >::const_iterator value_it = pose->joint_values_.begin(); value_it != pose->joint_values_.end(); ++value_it) { - // Only copy the first joint value // TODO: add capability for multi-DOF joints? - joint_state_map_[value_it->first] = value_it->second[0]; + robot_state.setJointPositions(value_it->first, value_it->second); } // Update the joints @@ -362,25 +362,8 @@ void RobotPosesWidget::showPose(srdf::Model::GroupState* pose) // ****************************************************************************************** void RobotPosesWidget::showDefaultPose() { - // Get list of all joints for the robot - std::vector joint_models = config_data_->getRobotModel()->getJointModels(); - - // Iterate through the joints - for (std::vector::const_iterator joint_it = joint_models.begin(); - joint_it < joint_models.end(); ++joint_it) - { - // Check that this joint only represents 1 variable. - if ((*joint_it)->getVariableCount() == 1) - { - double init_value; - - // get the first joint value in its vector - (*joint_it)->getVariableDefaultPositions(&init_value); - - // Change joint's value in joint_state_map to the default - joint_state_map_[(*joint_it)->getName()] = init_value; - } - } + moveit::core::RobotState& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); + robot_state.setToDefaultValues(); // Update the joints publishJoints(); @@ -441,16 +424,7 @@ void RobotPosesWidget::edit(int row) } group_name_field_->setCurrentIndex(index); - // Set pose joint values by adding them to the local joint state map - for (std::map >::const_iterator value_it = pose->joint_values_.begin(); - value_it != pose->joint_values_.end(); ++value_it) - { - // Only copy the first joint value // TODO: add capability for multi-DOF joints? - joint_state_map_[value_it->first] = value_it->second[0]; - } - - // Update robot model in rviz - publishJoints(); + showPose(pose); // Switch to screen - do this before setCurrentIndex stacked_layout_->setCurrentIndex(1); @@ -517,30 +491,18 @@ void RobotPosesWidget::loadJointSliders(const QString& selected) // Get list of associated joints const moveit::core::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup(group_name); - joint_models_ = joint_model_group->getJointModels(); + const auto& robot_state = config_data_->getPlanningScene()->getCurrentState(); // Iterate through the joints int num_joints = 0; - for (const moveit::core::JointModel* joint_model : joint_models_) + for (const moveit::core::JointModel* joint_model : joint_model_group->getJointModels()) { if (joint_model->getVariableCount() != 1 || // only consider 1-variable joints joint_model->isPassive() || // ignore passive joint_model->getMimic()) // and mimic joints continue; - double init_value; - - // Decide what this joint's initial value is - if (joint_state_map_.find(joint_model->getName()) == joint_state_map_.end()) - { - // The joint state map does not yet have an entry for this joint - // Get the first joint value in its vector - joint_model->getVariableDefaultPositions(&init_value); - } - else // There is already a value in the map - { - init_value = joint_state_map_[joint_model->getName()]; - } + double init_value = robot_state.getVariablePosition(joint_model->getVariableNames()[0]); // For each joint in group add slider SliderWidget* sw = new SliderWidget(this, joint_model, init_value); @@ -683,19 +645,22 @@ void RobotPosesWidget::doneEditing() // Clear the old values searched_data->joint_values_.clear(); - // Iterate through the current group's joints and add to SRDF - for (std::vector::const_iterator joint_it = joint_models_.begin(); - joint_it < joint_models_.end(); ++joint_it) + const moveit::core::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup(group); + const auto& robot_state = config_data_->getPlanningScene()->getCurrentState(); + + // Iterate through the current group's joints and add them to SRDF + for (const moveit::core::JointModel* jm : joint_model_group->getJointModels()) { // Check that this joint only represents 1 variable. - if ((*joint_it)->getVariableCount() == 1) + if (jm->getVariableCount() == 1 && !jm->isPassive() && !jm->getMimic()) { // Create vector for new joint values - std::vector joint_value; - joint_value.push_back(joint_state_map_[(*joint_it)->getName()]); + std::vector joint_values(jm->getVariableCount()); + const double* const first_variable = robot_state.getVariablePositions() + jm->getFirstVariableIndex(); + std::copy(first_variable, first_variable + joint_values.size(), joint_values.begin()); // Add joint vector to SRDF - searched_data->joint_values_[(*joint_it)->getName()] = joint_value; + searched_data->joint_values_[jm->getName()] = std::move(joint_values); } } @@ -794,24 +759,21 @@ void RobotPosesWidget::focusGiven() // ****************************************************************************************** void RobotPosesWidget::updateRobotModel(const std::string& name, double value) { - // Save the new value - joint_state_map_[name] = value; + moveit::core::RobotState& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); + robot_state.setVariablePosition(name, value); // Update the robot model/rviz publishJoints(); } // ****************************************************************************************** -// Publish the joint values in the joint_state_map_ to Rviz +// Publish the current RobotState to Rviz // ****************************************************************************************** void RobotPosesWidget::publishJoints() { - // Set the joints based on the map - moveit::core::RobotState& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); - robot_state.setVariablePositions(joint_state_map_); - // Prevent dirty collision body transforms + // Update link + collision transforms + auto& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); robot_state.update(); - // Create a planning scene message moveit_msgs::DisplayRobotState msg; moveit::core::robotStateToRobotStateMsg(robot_state, msg.state); diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.h b/moveit_setup_assistant/src/widgets/robot_poses_widget.h index 86be2084a1..63976bfa7c 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.h +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.h @@ -152,12 +152,6 @@ private Q_SLOTS: /// Pointer to currently edited group state srdf::Model::GroupState* current_edit_pose_; - /// All the joint slider values that have thus far been seen. May contain more than just the current joints' values - std::map joint_state_map_; - - /// The joints currently in the selected planning group - std::vector joint_models_; - /// Remember the publisher for quick publishing later ros::Publisher pub_robot_state_; From c9785b6be8576ea5e734c07642d84e4fcddf5b03 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Mon, 5 Oct 2020 12:23:10 -0400 Subject: [PATCH 475/612] Fix ROS namespacing in moveit_commander's PSI (#2347) --- .../planning_scene_interface.py | 7 +- .../python_moveit_commander_ros_namespace.py | 70 +++++++++++++++++++ ...python_moveit_commander_ros_namespace.test | 9 +++ 3 files changed, 83 insertions(+), 3 deletions(-) create mode 100755 moveit_commander/test/python_moveit_commander_ros_namespace.py create mode 100644 moveit_commander/test/python_moveit_commander_ros_namespace.test diff --git a/moveit_commander/src/moveit_commander/planning_scene_interface.py b/moveit_commander/src/moveit_commander/planning_scene_interface.py index cbc3c72021..9de2da057f 100644 --- a/moveit_commander/src/moveit_commander/planning_scene_interface.py +++ b/moveit_commander/src/moveit_commander/planning_scene_interface.py @@ -33,6 +33,7 @@ # Author: Ioan Sucan, Felix Messmer import rospy +from rosgraph.names import ns_join from . import conversions from moveit_msgs.msg import PlanningScene, CollisionObject, AttachedCollisionObject @@ -60,11 +61,11 @@ def __init__(self, ns='', synchronous=False, service_timeout=5.0): """ Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics. """ self._psi = _moveit_planning_scene_interface.PlanningSceneInterface(ns) - self._pub_co = rospy.Publisher(ns + '/collision_object', CollisionObject, queue_size=100) - self._pub_aco = rospy.Publisher(ns + '/attached_collision_object', AttachedCollisionObject, queue_size=100) + self._pub_co = rospy.Publisher(ns_join(ns, 'collision_object'), CollisionObject, queue_size=100) + self._pub_aco = rospy.Publisher(ns_join(ns, 'attached_collision_object'), AttachedCollisionObject, queue_size=100) self.__synchronous = synchronous if self.__synchronous: - self._apply_planning_scene_diff = rospy.ServiceProxy(ns + '/apply_planning_scene', ApplyPlanningScene) + self._apply_planning_scene_diff = rospy.ServiceProxy(ns_join(ns, 'apply_planning_scene'), ApplyPlanningScene) self._apply_planning_scene_diff.wait_for_service(service_timeout) def __submit(self, collision_object, attach=False): diff --git a/moveit_commander/test/python_moveit_commander_ros_namespace.py b/moveit_commander/test/python_moveit_commander_ros_namespace.py new file mode 100755 index 0000000000..2b36e937a2 --- /dev/null +++ b/moveit_commander/test/python_moveit_commander_ros_namespace.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, Open Source Robotics Foundation +# 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 Open Source Robotics Foundation. 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. +# +# Author: Peter Mitrano +# +# This test is used to ensure planning with a RobotCommander is +# possible if the robot's move_group node is in a different namespace + +import unittest +import rospy +import rostest +import os + +from moveit_commander import PlanningSceneInterface + + +class PythonMoveitCommanderRosNamespaceTest(unittest.TestCase): + + def test_namespace(self): + self.scene = PlanningSceneInterface() + expected_resolved_co_name = "/test_ros_namespace/collision_object" + expected_resolved_aco_name = "/test_ros_namespace/attached_collision_object" + self.assertEqual(self.scene._pub_co.resolved_name, expected_resolved_co_name) + self.assertEqual(self.scene._pub_aco.resolved_name, expected_resolved_aco_name) + + def test_namespace_synchronous(self): + self.scene = PlanningSceneInterface(synchronous=True) + expected_resolved_apply_diff_name = "/test_ros_namespace/apply_planning_scene" + self.assertEqual(self.scene._apply_planning_scene_diff.resolved_name, expected_resolved_apply_diff_name) + + +if __name__ == '__main__': + PKGNAME = 'moveit_ros_planning_interface' + NODENAME = 'moveit_test_python_moveit_commander_ros_namespace' + rospy.init_node(NODENAME) + rostest.rosrun(PKGNAME, NODENAME, PythonMoveitCommanderRosNamespaceTest) + + # suppress cleanup segfault + os._exit(0) diff --git a/moveit_commander/test/python_moveit_commander_ros_namespace.test b/moveit_commander/test/python_moveit_commander_ros_namespace.test new file mode 100644 index 0000000000..a2c3800c57 --- /dev/null +++ b/moveit_commander/test/python_moveit_commander_ros_namespace.test @@ -0,0 +1,9 @@ + + + + + + + From 78576496fbc9ab064c9ba6bd4cd0e741764d92ce Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 5 Oct 2020 17:46:29 +0200 Subject: [PATCH 476/612] Reorder buttons in GroupEditWidgets --- .../src/widgets/group_edit_widget.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp index c89c5c77c5..9129bdc982 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp @@ -142,6 +142,12 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& con add_subtitle->setFont(add_subtitle_font); recommended_options->addWidget(add_subtitle, 0, Qt::AlignLeft); + // Save and add chain + QPushButton* btn_save_chain = new QPushButton("Add Kin. Chain", this); + btn_save_chain->setMaximumWidth(200); + connect(btn_save_chain, SIGNAL(clicked()), this, SIGNAL(saveChain())); + recommended_options->addWidget(btn_save_chain); + // Save and add joints QPushButton* btn_save_joints = new QPushButton("Add Joints", this); btn_save_joints->setMaximumWidth(200); @@ -153,24 +159,18 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& con add_subtitle2->setFont(add_subtitle_font); advanced_options->addWidget(add_subtitle2, 0, Qt::AlignLeft); - // Save and add links - QPushButton* btn_save_links = new QPushButton("Add Links", this); - btn_save_links->setMaximumWidth(200); - connect(btn_save_links, SIGNAL(clicked()), this, SIGNAL(saveLinks())); - advanced_options->addWidget(btn_save_links); - - // Save and add chain - QPushButton* btn_save_chain = new QPushButton("Add Kin. Chain", this); - btn_save_chain->setMaximumWidth(200); - connect(btn_save_chain, SIGNAL(clicked()), this, SIGNAL(saveChain())); - advanced_options->addWidget(btn_save_chain); - // Save and add subgroups QPushButton* btn_save_subgroups = new QPushButton("Add Subgroups", this); btn_save_subgroups->setMaximumWidth(200); connect(btn_save_subgroups, SIGNAL(clicked()), this, SIGNAL(saveSubgroups())); advanced_options->addWidget(btn_save_subgroups); + // Save and add links + QPushButton* btn_save_links = new QPushButton("Add Links", this); + btn_save_links->setMaximumWidth(200); + connect(btn_save_links, SIGNAL(clicked()), this, SIGNAL(saveLinks())); + advanced_options->addWidget(btn_save_links); + // Add layouts new_buttons_layout_container->addLayout(label_layout); new_buttons_layout_container->addLayout(recommended_options); From 8b213d47489de4be3fab4b540aa234b54dc9794a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 5 Oct 2020 18:24:27 +0200 Subject: [PATCH 477/612] Replace dummy QWidgets with QSpacerItems --- .../src/widgets/controller_edit_widget.cpp | 10 +++------- .../src/widgets/double_list_widget.cpp | 4 +--- .../src/widgets/end_effectors_widget.cpp | 8 ++------ .../src/widgets/group_edit_widget.cpp | 10 +++------- .../src/widgets/kinematic_chain_widget.cpp | 4 +--- .../src/widgets/planning_groups_widget.cpp | 4 +--- .../src/widgets/robot_poses_widget.cpp | 8 ++------ .../src/widgets/ros_controllers_widget.cpp | 4 +--- .../src/widgets/start_screen_widget.cpp | 4 +--- .../src/widgets/virtual_joints_widget.cpp | 8 ++------ 10 files changed, 17 insertions(+), 47 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp index f86d0f7d5a..62c2feb300 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp @@ -114,10 +114,8 @@ ControllerEditWidget::ControllerEditWidget(QWidget* parent, const MoveItConfigDa new_buttons_widget_->setLayout(new_buttons_layout); layout->addWidget(new_buttons_widget_); - // Verticle Spacer ----------------------------------------------------- - QWidget* vspacer = new QWidget(this); - vspacer->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); - layout->addWidget(vspacer); + // Vertical Spacer + layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Minimum, QSizePolicy::Expanding)); // Bottom Controls --------------------------------------------------------- QHBoxLayout* controls_layout = new QHBoxLayout(); @@ -130,9 +128,7 @@ ControllerEditWidget::ControllerEditWidget(QWidget* parent, const MoveItConfigDa controls_layout->setAlignment(btn_delete_, Qt::AlignRight); // Horizontal Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save btn_save_ = new QPushButton("&Save", this); diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.cpp b/moveit_setup_assistant/src/widgets/double_list_widget.cpp index ede1601910..eec647763b 100644 --- a/moveit_setup_assistant/src/widgets/double_list_widget.cpp +++ b/moveit_setup_assistant/src/widgets/double_list_widget.cpp @@ -140,9 +140,7 @@ DoubleListWidget::DoubleListWidget(QWidget* parent, const MoveItConfigDataPtr& c controls_layout->setContentsMargins(0, 25, 0, 15); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save QPushButton* btn_save = new QPushButton("&Save", this); diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp index effbe5a7ce..f38816b313 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp @@ -116,9 +116,7 @@ QWidget* EndEffectorsWidget::createContentsWidget() QHBoxLayout* controls_layout = new QHBoxLayout(); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Edit Selected Button btn_edit_ = new QPushButton("&Edit Selected", this); @@ -196,9 +194,7 @@ QWidget* EndEffectorsWidget::createEditWidget() controls_layout->setContentsMargins(0, 25, 0, 15); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save btn_save_ = new QPushButton("&Save", this); diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp index 9129bdc982..4397f5e6f5 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp @@ -180,10 +180,8 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& con new_buttons_widget_->setLayout(new_buttons_layout_container); layout->addWidget(new_buttons_widget_); - // Verticle Spacer ----------------------------------------------------- - QWidget* vspacer = new QWidget(this); - vspacer->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); - layout->addWidget(vspacer); + // Vertical Spacer + layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Minimum, QSizePolicy::Expanding)); // Bottom Controls --------------------------------------------------------- QHBoxLayout* controls_layout = new QHBoxLayout(); @@ -196,9 +194,7 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& con controls_layout->setAlignment(btn_delete_, Qt::AlignRight); // Horizontal Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save btn_save_ = new QPushButton("&Save", this); diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp index a53189872a..5b41aca58b 100644 --- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp +++ b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp @@ -108,9 +108,7 @@ KinematicChainWidget::KinematicChainWidget(QWidget* parent, const MoveItConfigDa controls_layout->addWidget(expand_controls); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save QPushButton* btn_save = new QPushButton("&Save", this); diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index 1f9aabd730..4369a95919 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -208,9 +208,7 @@ QWidget* PlanningGroupsWidget::createContentsWidget() controls_layout->addWidget(expand_controls); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Delete Selected Button btn_delete_ = new QPushButton("&Delete Selected", this); diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index 2c1ef14747..79c8588da4 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -154,9 +154,7 @@ QWidget* RobotPosesWidget::createContentsWidget() controls_layout->setAlignment(btn_play, Qt::AlignLeft); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Edit Selected Button btn_edit_ = new QPushButton("&Edit Selected", this); @@ -258,9 +256,7 @@ QWidget* RobotPosesWidget::createEditWidget() controls_layout->setContentsMargins(0, 25, 0, 15); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save btn_save_ = new QPushButton("&Save", this); diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index edd152495c..b22cb93494 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -162,9 +162,7 @@ QWidget* ROSControllersWidget::createContentsWidget() controls_layout_->addWidget(expand_controls); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout_->addWidget(spacer); + controls_layout_->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Delete btn_delete_ = new QPushButton("&Delete Controller", this); diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 96af347714..c64e8acf05 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -188,9 +188,7 @@ StartScreenWidget::StartScreenWidget(QWidget* parent, const MoveItConfigDataPtr& layout->addLayout(hlayout); // Verticle Spacer - QWidget* vspacer = new QWidget(this); - vspacer->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); - layout->addWidget(vspacer); + layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Minimum, QSizePolicy::Expanding)); // Attach bottom layout layout->addWidget(next_label_); diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp index e5b42fc197..8f1f479fa0 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp @@ -114,9 +114,7 @@ QWidget* VirtualJointsWidget::createContentsWidget() QHBoxLayout* controls_layout = new QHBoxLayout(); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Edit Selected Button btn_edit_ = new QPushButton("&Edit Selected", this); @@ -194,9 +192,7 @@ QWidget* VirtualJointsWidget::createEditWidget() controls_layout->setContentsMargins(0, 25, 0, 15); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save QPushButton* btn_save = new QPushButton("&Save", this); From d41e4a40a8819fa38f14858bc1c03110b41afb80 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 5 Oct 2020 19:47:14 +0200 Subject: [PATCH 478/612] Use QStackedWidget instead of QWidget+QStackedLayout --- .../src/widgets/end_effectors_widget.cpp | 25 +++++++---------- .../src/widgets/end_effectors_widget.h | 4 +-- .../src/widgets/passive_joints_widget.h | 1 - .../src/widgets/planning_groups_widget.cpp | 27 ++++++++----------- .../src/widgets/planning_groups_widget.h | 4 +-- .../src/widgets/robot_poses_widget.cpp | 25 +++++++---------- .../src/widgets/robot_poses_widget.h | 4 +-- .../src/widgets/ros_controllers_widget.cpp | 23 ++++++---------- .../src/widgets/ros_controllers_widget.h | 4 +-- .../src/widgets/setup_assistant_widget.cpp | 11 +++----- .../src/widgets/setup_assistant_widget.h | 3 +-- .../src/widgets/virtual_joints_widget.cpp | 23 +++++++--------- .../src/widgets/virtual_joints_widget.h | 4 +-- 13 files changed, 62 insertions(+), 96 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp index f38816b313..dad3d8b5a0 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp @@ -68,15 +68,10 @@ EndEffectorsWidget::EndEffectorsWidget(QWidget* parent, const MoveItConfigDataPt effector_edit_widget_ = createEditWidget(); // Create stacked layout ----------------------------------------- - stacked_layout_ = new QStackedLayout(this); - stacked_layout_->addWidget(effector_list_widget_); // screen index 0 - stacked_layout_->addWidget(effector_edit_widget_); // screen index 1 - - // Create Widget wrapper for layout - QWidget* stacked_layout_widget = new QWidget(this); - stacked_layout_widget->setLayout(stacked_layout_); - - layout->addWidget(stacked_layout_widget); + stacked_widget_ = new QStackedWidget(this); + stacked_widget_->addWidget(effector_list_widget_); // screen index 0 + stacked_widget_->addWidget(effector_edit_widget_); // screen index 1 + layout->addWidget(stacked_widget_); // Finish Layout -------------------------------------------------- this->setLayout(layout); @@ -234,7 +229,7 @@ void EndEffectorsWidget::showNewScreen() parent_group_name_field_->clearEditText(); // actually this just chooses first option // Switch to screen - stacked_layout_->setCurrentIndex(1); + stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode Q_EMIT isModal(true); @@ -276,7 +271,7 @@ void EndEffectorsWidget::previewClicked(int /*row*/, int /*column*/) void EndEffectorsWidget::previewClickedString(const QString& name) { // Don't highlight if we are on the overview end effectors screen. we are just populating drop down box - if (stacked_layout_->currentIndex() == 0) + if (stacked_widget_->currentIndex() == 0) return; // Unhighlight all links @@ -344,7 +339,7 @@ void EndEffectorsWidget::edit(const std::string& name) parent_group_name_field_->setCurrentIndex(index); // Switch to screen - stacked_layout_->setCurrentIndex(1); + stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode Q_EMIT isModal(true); @@ -568,7 +563,7 @@ void EndEffectorsWidget::doneEditing() loadDataTable(); // Switch to screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode Q_EMIT isModal(false); @@ -580,7 +575,7 @@ void EndEffectorsWidget::doneEditing() void EndEffectorsWidget::cancelEditing() { // Switch to screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Re-highlight the currently selected end effector group previewClicked(0, 0); // the number parameters are actually meaningless @@ -647,7 +642,7 @@ void EndEffectorsWidget::loadDataTable() void EndEffectorsWidget::focusGiven() { // Show the current effectors screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Load the data to the tree loadDataTable(); diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.h b/moveit_setup_assistant/src/widgets/end_effectors_widget.h index 1073ec1866..6befe26c69 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.h +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.h @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include @@ -81,7 +81,7 @@ class EndEffectorsWidget : public SetupScreenWidget QPushButton* btn_delete_; QPushButton* btn_save_; QPushButton* btn_cancel_; - QStackedLayout* stacked_layout_; + QStackedWidget* stacked_widget_; QLineEdit* effector_name_field_; QComboBox* parent_name_field_; QComboBox* parent_group_name_field_; diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.h b/moveit_setup_assistant/src/widgets/passive_joints_widget.h index 9f27cf15b5..c0f7d17767 100644 --- a/moveit_setup_assistant/src/widgets/passive_joints_widget.h +++ b/moveit_setup_assistant/src/widgets/passive_joints_widget.h @@ -43,7 +43,6 @@ #include #include #include -#include #include #include diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index 4369a95919..5aec108049 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include #include @@ -154,24 +154,19 @@ PlanningGroupsWidget::PlanningGroupsWidget(QWidget* parent, const MoveItConfigDa connect(group_edit_widget_, SIGNAL(saveSubgroups()), this, SLOT(saveGroupScreenSubgroups())); // Combine into stack - stacked_layout_ = new QStackedLayout(this); - stacked_layout_->addWidget(groups_tree_widget_); // screen index 0 - stacked_layout_->addWidget(joints_widget_); // screen index 1 - stacked_layout_->addWidget(links_widget_); // screen index 2 - stacked_layout_->addWidget(chain_widget_); // screen index 3 - stacked_layout_->addWidget(subgroups_widget_); // screen index 4 - stacked_layout_->addWidget(group_edit_widget_); // screen index 5 + stacked_widget_ = new QStackedWidget(this); + stacked_widget_->addWidget(groups_tree_widget_); // screen index 0 + stacked_widget_->addWidget(joints_widget_); // screen index 1 + stacked_widget_->addWidget(links_widget_); // screen index 2 + stacked_widget_->addWidget(chain_widget_); // screen index 3 + stacked_widget_->addWidget(subgroups_widget_); // screen index 4 + stacked_widget_->addWidget(group_edit_widget_); // screen index 5 showMainScreen(); // Finish GUI ----------------------------------------------------------- - // Create Widget wrapper for layout - QWidget* stacked_layout_widget = new QWidget(this); - stacked_layout_widget->setLayout(stacked_layout_); - - layout->addWidget(stacked_layout_widget); - + layout->addWidget(stacked_widget_); setLayout(layout); // process the gui @@ -1384,7 +1379,7 @@ void PlanningGroupsWidget::alterTree(const QString& link) // ****************************************************************************************** void PlanningGroupsWidget::showMainScreen() { - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode Q_EMIT isModal(false); @@ -1395,7 +1390,7 @@ void PlanningGroupsWidget::showMainScreen() // ****************************************************************************************** void PlanningGroupsWidget::changeScreen(int index) { - stacked_layout_->setCurrentIndex(index); + stacked_widget_->setCurrentIndex(index); // Announce this widget's mode Q_EMIT isModal(index != 0); diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.h b/moveit_setup_assistant/src/widgets/planning_groups_widget.h index 33fba55316..a61f2539b2 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.h +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.h @@ -38,7 +38,7 @@ // Qt class QPushButton; -class QStackedLayout; +class QStackedWidget; class QTreeWidget; class QTreeWidgetItem; @@ -146,7 +146,7 @@ private Q_SLOTS: QTreeWidget* groups_tree_; /// For changing between table and different add/edit views - QStackedLayout* stacked_layout_; + QStackedWidget* stacked_widget_; /// Show and hide edit button QPushButton* btn_edit_; diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index 79c8588da4..70a740c878 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -75,15 +75,10 @@ RobotPosesWidget::RobotPosesWidget(QWidget* parent, const MoveItConfigDataPtr& c pose_edit_widget_ = createEditWidget(); // Create stacked layout ----------------------------------------- - stacked_layout_ = new QStackedLayout(this); - stacked_layout_->addWidget(pose_list_widget_); // screen index 0 - stacked_layout_->addWidget(pose_edit_widget_); // screen index 1 - - // Create Widget wrapper for layout - QWidget* stacked_layout_widget = new QWidget(this); - stacked_layout_widget->setLayout(stacked_layout_); - - layout->addWidget(stacked_layout_widget); + stacked_widget_ = new QStackedWidget(this); + stacked_widget_->addWidget(pose_list_widget_); // screen index 0 + stacked_widget_->addWidget(pose_edit_widget_); // screen index 1 + layout->addWidget(stacked_widget_); // Finish Layout -------------------------------------------------- this->setLayout(layout); @@ -287,7 +282,7 @@ QWidget* RobotPosesWidget::createEditWidget() void RobotPosesWidget::showNewScreen() { // Switch to screen - do this before clearEditText() - stacked_layout_->setCurrentIndex(1); + stacked_widget_->setCurrentIndex(1); // Remember that this is a new pose current_edit_pose_ = nullptr; @@ -423,7 +418,7 @@ void RobotPosesWidget::edit(int row) showPose(pose); // Switch to screen - do this before setCurrentIndex - stacked_layout_->setCurrentIndex(1); + stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode Q_EMIT isModal(true); @@ -454,7 +449,7 @@ void RobotPosesWidget::loadJointSliders(const QString& selected) { // Ignore this event if the combo box is empty. This occurs when clearing the combo box and reloading with the // newest groups. Also ignore if we are not on the edit screen - if (!group_name_field_->count() || selected.isEmpty() || stacked_layout_->currentIndex() == 0) + if (!group_name_field_->count() || selected.isEmpty() || stacked_widget_->currentIndex() == 0) return; // Get group name from input @@ -673,7 +668,7 @@ void RobotPosesWidget::doneEditing() loadDataTable(); // Switch to screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is done with modal mode Q_EMIT isModal(false); @@ -685,7 +680,7 @@ void RobotPosesWidget::doneEditing() void RobotPosesWidget::cancelEditing() { // Switch to screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is done with modal mode Q_EMIT isModal(false); @@ -741,7 +736,7 @@ void RobotPosesWidget::loadDataTable() void RobotPosesWidget::focusGiven() { // Show the current poses screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Load the data to the tree loadDataTable(); diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.h b/moveit_setup_assistant/src/widgets/robot_poses_widget.h index 63976bfa7c..c85b32f852 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.h +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.h @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include @@ -83,7 +83,7 @@ class RobotPosesWidget : public SetupScreenWidget QPushButton* btn_delete_; QPushButton* btn_save_; QPushButton* btn_cancel_; - QStackedLayout* stacked_layout_; + QStackedWidget* stacked_widget_; QScrollArea* scroll_area_; QVBoxLayout* column2_; QLineEdit* pose_name_field_; diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index b22cb93494..242db5ed9d 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -95,19 +95,12 @@ ROSControllersWidget::ROSControllersWidget(QWidget* parent, const MoveItConfigDa connect(controller_edit_widget_, SIGNAL(saveJointsGroups()), this, SLOT(saveControllerScreenGroups())); // Combine into stack - stacked_layout_ = new QStackedLayout(this); - stacked_layout_->addWidget(controllers_tree_widget_); // screen index 0 - stacked_layout_->addWidget(joints_widget_); // screen index 1 - stacked_layout_->addWidget(controller_edit_widget_); // screen index 2 - stacked_layout_->addWidget(joint_groups_widget_); // screen index 3 - - // Finish GUI ----------------------------------------------------------- - - // Create Widget wrapper for layout - QWidget* stacked_layout_widget = new QWidget(this); - stacked_layout_widget->setLayout(stacked_layout_); - - layout->addWidget(stacked_layout_widget); + stacked_widget_ = new QStackedWidget(this); + stacked_widget_->addWidget(controllers_tree_widget_); // screen index 0 + stacked_widget_->addWidget(joints_widget_); // screen index 1 + stacked_widget_->addWidget(controller_edit_widget_); // screen index 2 + stacked_widget_->addWidget(joint_groups_widget_); // screen index 3 + layout->addWidget(stacked_widget_); this->setLayout(layout); } @@ -797,7 +790,7 @@ void ROSControllersWidget::editController() // ****************************************************************************************** void ROSControllersWidget::showMainScreen() { - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode Q_EMIT isModal(false); @@ -808,7 +801,7 @@ void ROSControllersWidget::showMainScreen() // ****************************************************************************************** void ROSControllersWidget::changeScreen(int index) { - stacked_layout_->setCurrentIndex(index); + stacked_widget_->setCurrentIndex(index); // Announce this widget's mode Q_EMIT isModal(index != 0); diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h index c78e291ceb..515eee1666 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include // SA @@ -128,7 +128,7 @@ private Q_SLOTS: QWidget* controllers_tree_widget_; /// For changing between table and different add/edit views - QStackedLayout* stacked_layout_; + QStackedWidget* stacked_widget_; ControllerEditWidget* controller_edit_widget_; QPushButton* btn_delete_; diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 6b32e381cf..534831283a 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -39,7 +39,7 @@ #include "setup_screen_widget.h" // a base class for screens in the setup assistant #include "setup_assistant_widget.h" // Qt -#include +#include #include #include #include @@ -87,14 +87,9 @@ SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, const boost::program layout->setAlignment(Qt::AlignTop); // Create main content stack for various screens - main_content_ = new QStackedLayout(); + main_content_ = new QStackedWidget(); current_index_ = 0; - // Wrap main_content_ with a widget - middle_frame_ = new QWidget(this); - middle_frame_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); - middle_frame_->setLayout(main_content_); - // Screens -------------------------------------------------------- // Start Screen @@ -149,7 +144,7 @@ SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, const boost::program splitter_ = new QSplitter(Qt::Horizontal, this); splitter_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); splitter_->addWidget(navs_view_); - splitter_->addWidget(middle_frame_); + splitter_->addWidget(main_content_); splitter_->addWidget(rviz_container_); splitter_->setHandleWidth(6); // splitter_->setCollapsible( 0, false ); // don't let navigation collapse diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h index 45dd02b684..850492d3de 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h @@ -202,10 +202,9 @@ private Q_SLOTS: QList nav_name_list_; NavigationWidget* navs_view_; - QWidget* middle_frame_; QWidget* rviz_container_; QSplitter* splitter_; - QStackedLayout* main_content_; + QStackedWidget* main_content_; int current_index_; boost::mutex change_screen_lock_; diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp index 8f1f479fa0..4a404c901c 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp @@ -65,16 +65,11 @@ VirtualJointsWidget::VirtualJointsWidget(QWidget* parent, const MoveItConfigData vjoint_list_widget_ = createContentsWidget(); vjoint_edit_widget_ = createEditWidget(); - // Create stacked layout ----------------------------------------- - stacked_layout_ = new QStackedLayout(this); - stacked_layout_->addWidget(vjoint_list_widget_); // screen index 0 - stacked_layout_->addWidget(vjoint_edit_widget_); // screen index 1 - // Create Widget wrapper for layout - QWidget* stacked_layout_widget = new QWidget(this); - stacked_layout_widget->setLayout(stacked_layout_); - - layout->addWidget(stacked_layout_widget); + stacked_widget_ = new QStackedWidget(this); + stacked_widget_->addWidget(vjoint_list_widget_); // screen index 0 + stacked_widget_->addWidget(vjoint_edit_widget_); // screen index 1 + layout->addWidget(stacked_widget_); // Finish Layout -------------------------------------------------- this->setLayout(layout); @@ -232,7 +227,7 @@ void VirtualJointsWidget::showNewScreen() joint_type_field_->clearEditText(); // actually this just chooses first option // Switch to screen - stacked_layout_->setCurrentIndex(1); + stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode Q_EMIT isModal(true); @@ -303,7 +298,7 @@ void VirtualJointsWidget::edit(const std::string& name) joint_type_field_->setCurrentIndex(index); // Switch to screen - stacked_layout_->setCurrentIndex(1); + stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode Q_EMIT isModal(true); @@ -508,7 +503,7 @@ void VirtualJointsWidget::doneEditing() loadDataTable(); // Switch to screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode Q_EMIT isModal(false); @@ -526,7 +521,7 @@ void VirtualJointsWidget::doneEditing() void VirtualJointsWidget::cancelEditing() { // Switch to screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode Q_EMIT isModal(false); @@ -590,7 +585,7 @@ void VirtualJointsWidget::loadDataTable() void VirtualJointsWidget::focusGiven() { // Show the current vjoints screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Load the data to the tree loadDataTable(); diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h b/moveit_setup_assistant/src/widgets/virtual_joints_widget.h index 0f48d7b2e2..24bf8c486e 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.h @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include @@ -79,7 +79,7 @@ class VirtualJointsWidget : public SetupScreenWidget QPushButton* btn_delete_; QPushButton* btn_save_; QPushButton* btn_cancel_; - QStackedLayout* stacked_layout_; + QStackedWidget* stacked_widget_; QLineEdit* vjoint_name_field_; QLineEdit* parent_name_field_; QComboBox* child_link_field_; From 659489cd8cd5409a6e6b7879e2729010ccf490cf Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 6 Oct 2020 11:49:44 +0200 Subject: [PATCH 479/612] Remove released upstream packages from docker files As franka_description is released, we don't need to build it in our docker images anymore. --- .docker/ci/Dockerfile | 8 -------- .docker/source/Dockerfile | 3 --- 2 files changed, 11 deletions(-) diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index ad22f50ce6..5ce021548f 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -25,14 +25,6 @@ RUN \ clang clang-format-10 clang-tidy clang-tools \ ccache && \ # - # Build and install franka_description from https://github.com/frankaemika/franka_ros.git - git clone --depth 1 --branch 0.6.0 https://github.com/frankaemika/franka_ros.git src/franka_ros && \ - mv src/franka_ros/franka_description src/ && rm -rf src/franka_ros && \ - catkin config --extend /opt/ros/noetic --install --install-space /opt/ros/noetic && \ - catkin build && \ - # Cleanup temporary workspace - rm -rf logs build devel .catkin_tools && \ - # # Download MoveIt source, so that we can fetch all necessary dependencies wstool init --shallow src https://raw.githubusercontent.com/ros-planning/moveit/${ROS_DISTRO}-devel/moveit.rosinstall && \ # diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile index c9bfae3078..1cfeb73e87 100644 --- a/.docker/source/Dockerfile +++ b/.docker/source/Dockerfile @@ -12,9 +12,6 @@ WORKDIR $ROS_UNDERLAY/../src RUN \ # Download moveit source so that we can get necessary dependencies wstool init . https://raw.githubusercontent.com/ros-planning/moveit/${ROS_DISTRO}-devel/moveit.rosinstall && \ - # We only depend on franka_description (TODO: remove if released) - git clone --depth 1 --branch 0.6.0 https://github.com/frankaemika/franka_ros.git src/franka_ros && \ - mv src/franka_ros/franka_description src/ && rm -rf src/franka_ros && \ # # Update apt package list as cache is cleared in previous container # Usually upgrading involves a few packages only (if container builds became out-of-sync) From 59fa491373cfc87e9de1598a5efe47298de3d483 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 5 Oct 2020 21:20:00 +0200 Subject: [PATCH 480/612] Increase minimal window size --- moveit_setup_assistant/src/setup_assistant_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_setup_assistant/src/setup_assistant_main.cpp b/moveit_setup_assistant/src/setup_assistant_main.cpp index 88577607a5..8614ed71cb 100644 --- a/moveit_setup_assistant/src/setup_assistant_main.cpp +++ b/moveit_setup_assistant/src/setup_assistant_main.cpp @@ -95,8 +95,8 @@ int main(int argc, char** argv) // Load Qt Widget moveit_setup_assistant::SetupAssistantWidget saw(nullptr, vm); - saw.setMinimumWidth(980); - saw.setMinimumHeight(550); + saw.setMinimumWidth(1090); + saw.setMinimumHeight(600); // saw.setWindowState( Qt::WindowMaximized ); saw.show(); From 0cb8ec41513b252ea019b38bd70ee34d569ef47d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 5 Oct 2020 22:33:52 +0200 Subject: [PATCH 481/612] Cleanup Qt includes --- .../src/widgets/author_information_widget.cpp | 6 ++-- .../src/widgets/author_information_widget.h | 5 +--- .../widgets/configuration_files_widget.cpp | 19 ++++++++---- .../src/widgets/configuration_files_widget.h | 14 ++++----- .../src/widgets/controller_edit_widget.cpp | 10 +++++-- .../src/widgets/controller_edit_widget.h | 8 ++--- .../src/widgets/default_collisions_widget.cpp | 22 ++++++++++---- .../src/widgets/default_collisions_widget.h | 30 +++++++++++-------- .../src/widgets/double_list_widget.cpp | 12 ++++---- .../src/widgets/double_list_widget.h | 6 ++-- .../src/widgets/end_effectors_widget.cpp | 16 +++++++++- .../src/widgets/end_effectors_widget.h | 17 ++++------- .../src/widgets/group_edit_widget.cpp | 13 +++++--- .../src/widgets/group_edit_widget.h | 8 ++--- .../src/widgets/header_widget.cpp | 8 +++-- .../src/widgets/header_widget.h | 5 ++-- .../src/widgets/kinematic_chain_widget.cpp | 3 +- .../src/widgets/kinematic_chain_widget.h | 6 ++-- .../src/widgets/navigation_widget.cpp | 3 ++ .../src/widgets/navigation_widget.h | 5 +--- .../src/widgets/passive_joints_widget.cpp | 5 ++++ .../src/widgets/passive_joints_widget.h | 13 +------- .../src/widgets/perception_widget.cpp | 9 ++++-- .../src/widgets/perception_widget.h | 9 ++---- .../src/widgets/planning_groups_widget.cpp | 10 +++++-- .../src/widgets/robot_poses_widget.cpp | 13 ++++++-- .../src/widgets/robot_poses_widget.h | 21 ++++++------- .../src/widgets/ros_controllers_widget.cpp | 18 +++++++++-- .../src/widgets/ros_controllers_widget.h | 19 +++++------- .../src/widgets/setup_assistant_widget.cpp | 15 ++++++---- .../src/widgets/setup_assistant_widget.h | 18 +++-------- .../src/widgets/simulation_widget.cpp | 7 +++-- .../src/widgets/simulation_widget.h | 6 ++-- .../src/widgets/start_screen_widget.cpp | 17 +++++++---- .../src/widgets/start_screen_widget.h | 9 ++---- .../src/widgets/virtual_joints_widget.cpp | 13 +++++++- .../src/widgets/virtual_joints_widget.h | 16 ++++------ 37 files changed, 251 insertions(+), 183 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/author_information_widget.cpp b/moveit_setup_assistant/src/widgets/author_information_widget.cpp index a73c84e2ec..6d7c1e5ded 100644 --- a/moveit_setup_assistant/src/widgets/author_information_widget.cpp +++ b/moveit_setup_assistant/src/widgets/author_information_widget.cpp @@ -39,10 +39,10 @@ #include #include #include -#include -// ROS +#include +#include #include "author_information_widget.h" -#include +#include "header_widget.h" namespace moveit_setup_assistant { diff --git a/moveit_setup_assistant/src/widgets/author_information_widget.h b/moveit_setup_assistant/src/widgets/author_information_widget.h index 94763edf93..4f9709c8f2 100644 --- a/moveit_setup_assistant/src/widgets/author_information_widget.h +++ b/moveit_setup_assistant/src/widgets/author_information_widget.h @@ -36,15 +36,12 @@ #pragma once -#include -#include -#include +class QLineEdit; #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 38ce197bd8..872c71d8f9 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -35,15 +35,24 @@ /* Author: Dave Coleman */ // Qt -#include -#include -#include #include -#include +#include +#include +#include +#include +#include +#include +#include #include -// ROS +#include +#include + #include "configuration_files_widget.h" +#include "header_widget.h" + +// ROS #include + // Boost #include // for string find and replace in templates #include // for creating folders/files diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.h b/moveit_setup_assistant/src/widgets/configuration_files_widget.h index da0946e4dd..edc7cf8271 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.h +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.h @@ -36,23 +36,23 @@ #pragma once -#include -#include -#include -#include -#include -#include #include +class QLabel; +class QListWidget; +class QListWidgetItem; +class QProgressBar; +class QPushButton; #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant { +class LoadPathWidget; + // Struct for storing all the file operations struct GenerateFile { diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp index 62c2feb300..38be579c2f 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp @@ -33,12 +33,16 @@ /* Author: Mohamad Ayman */ -#include +#include +#include +#include #include +#include +#include #include -#include +#include #include -#include +#include #include "controller_edit_widget.h" namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.h b/moveit_setup_assistant/src/widgets/controller_edit_widget.h index 1da3f602b2..0b5b8c4de0 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.h +++ b/moveit_setup_assistant/src/widgets/controller_edit_widget.h @@ -36,10 +36,10 @@ #pragma once #include -#include -#include -#include -#include +class QComboBox; +class QLabel; +class QLineEdit; +class QPushButton; #ifndef Q_MOC_RUN #include diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp index d6b2a05c98..76be30b8db 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp @@ -34,16 +34,26 @@ /* Author: Dave Coleman */ -#include -#include -#include -#include -#include +#include #include #include -#include +#include +#include +#include +#include #include +#include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "default_collisions_widget.h" #include "header_widget.h" diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.h b/moveit_setup_assistant/src/widgets/default_collisions_widget.h index b78ecd61ad..983f7cb73d 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.h +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.h @@ -35,20 +35,24 @@ /* Author: Dave Coleman */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include -#include -#include +class QAbstractItemModel; +class QAction; +class QButtonGroup; +class QCheckBox; +class QGroupBox; +class QHeaderView; +class QItemSelection; +class QItemSelectionModel; +class QLabel; +class QLineEdit; +class QProgressBar; +class QPushButton; +class QRadioButton; +class QSlider; +class QSpinBox; +class QTableView; +class QVBoxLayout; #ifndef Q_MOC_RUN #include diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.cpp b/moveit_setup_assistant/src/widgets/double_list_widget.cpp index eec647763b..a0747c3f57 100644 --- a/moveit_setup_assistant/src/widgets/double_list_widget.cpp +++ b/moveit_setup_assistant/src/widgets/double_list_widget.cpp @@ -34,15 +34,17 @@ /* Author: Dave Coleman */ -#include -#include -#include -#include #include +#include +#include +#include +#include #include #include +#include #include -#include +#include +#include #include "double_list_widget.h" namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.h b/moveit_setup_assistant/src/widgets/double_list_widget.h index afcac603bc..509f96402f 100644 --- a/moveit_setup_assistant/src/widgets/double_list_widget.h +++ b/moveit_setup_assistant/src/widgets/double_list_widget.h @@ -37,8 +37,10 @@ #pragma once #include -#include -#include +class QLabel; +class QTableWidget; +class QTableWidgetItem; +class QItemSelection; #ifndef Q_MOC_RUN #include diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp index dad3d8b5a0..d05cec207e 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp @@ -36,10 +36,24 @@ // SA #include "end_effectors_widget.h" +#include "header_widget.h" + // Qt +#include +#include #include +#include +#include +#include +#include #include -#include +#include +#include +#include +#include +#include +#include +#include namespace moveit_setup_assistant { diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.h b/moveit_setup_assistant/src/widgets/end_effectors_widget.h index 6befe26c69..63403fea49 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.h +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.h @@ -37,24 +37,17 @@ #pragma once // Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +class QComboBox; +class QLineEdit; +class QPushButton; +class QStackedWidget; +class QTableWidget; // SA #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp index 4397f5e6f5..3382e95563 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp @@ -34,13 +34,18 @@ /* Author: Dave Coleman */ -#include -#include -#include +#include #include #include -#include #include +#include +#include +#include +#include +#include +#include +#include + #include "group_edit_widget.h" #include // for loading all avail kinematic planners diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.h b/moveit_setup_assistant/src/widgets/group_edit_widget.h index a55fb2943e..0cf37207c7 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.h +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.h @@ -37,10 +37,10 @@ #pragma once #include -#include -#include -#include -#include +class QComboBox; +class QLabel; +class QLineEdit; +class QPushButton; #ifndef Q_MOC_RUN #include diff --git a/moveit_setup_assistant/src/widgets/header_widget.cpp b/moveit_setup_assistant/src/widgets/header_widget.cpp index e42d06f5e1..695efbece0 100644 --- a/moveit_setup_assistant/src/widgets/header_widget.cpp +++ b/moveit_setup_assistant/src/widgets/header_widget.cpp @@ -34,11 +34,13 @@ /* Author: Dave Coleman */ -#include -#include +#include "header_widget.h" #include +#include +#include +#include +#include #include -#include "header_widget.h" namespace moveit_setup_assistant { diff --git a/moveit_setup_assistant/src/widgets/header_widget.h b/moveit_setup_assistant/src/widgets/header_widget.h index 3e82c51728..76bda444d7 100644 --- a/moveit_setup_assistant/src/widgets/header_widget.h +++ b/moveit_setup_assistant/src/widgets/header_widget.h @@ -37,8 +37,9 @@ #pragma once #include -#include -#include +#include +class QLabel; +class QLineEdit; namespace moveit_setup_assistant { diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp index 5b41aca58b..88bda9853e 100644 --- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp +++ b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp @@ -41,7 +41,8 @@ #include #include #include -#include +#include +#include #include "kinematic_chain_widget.h" namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h index 150fe46de0..6fc1fef14c 100644 --- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h +++ b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h @@ -37,8 +37,10 @@ #pragma once #include -#include -#include +class QLabel; +class QLineEdit; +class QTreeWidget; +class QTreeWidgetItem; #ifndef Q_MOC_RUN #include diff --git a/moveit_setup_assistant/src/widgets/navigation_widget.cpp b/moveit_setup_assistant/src/widgets/navigation_widget.cpp index 3d62d771f0..56777454ef 100644 --- a/moveit_setup_assistant/src/widgets/navigation_widget.cpp +++ b/moveit_setup_assistant/src/widgets/navigation_widget.cpp @@ -36,6 +36,9 @@ #include "navigation_widget.h" #include +#include +#include +#include #include namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/navigation_widget.h b/moveit_setup_assistant/src/widgets/navigation_widget.h index 2adb17ca4e..59bfba9e09 100644 --- a/moveit_setup_assistant/src/widgets/navigation_widget.h +++ b/moveit_setup_assistant/src/widgets/navigation_widget.h @@ -37,11 +37,8 @@ #pragma once #include -#include -#include -#include #include -#include +class QStandardItemModel; namespace moveit_setup_assistant { diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp b/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp index 7ca2d4275e..85610b4404 100644 --- a/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp @@ -36,9 +36,14 @@ // SA #include "passive_joints_widget.h" +#include "header_widget.h" +#include "double_list_widget.h" + // Qt #include +#include #include +#include namespace moveit_setup_assistant { diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.h b/moveit_setup_assistant/src/widgets/passive_joints_widget.h index c0f7d17767..91181744ce 100644 --- a/moveit_setup_assistant/src/widgets/passive_joints_widget.h +++ b/moveit_setup_assistant/src/widgets/passive_joints_widget.h @@ -36,27 +36,16 @@ #pragma once -// Qt -#include -#include -#include -#include -#include -#include -#include -#include - // SA #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" -#include "double_list_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant { +class DoubleListWidget; class PassiveJointsWidget : public SetupScreenWidget { Q_OBJECT diff --git a/moveit_setup_assistant/src/widgets/perception_widget.cpp b/moveit_setup_assistant/src/widgets/perception_widget.cpp index 11662d4722..f9dc1c167d 100644 --- a/moveit_setup_assistant/src/widgets/perception_widget.cpp +++ b/moveit_setup_assistant/src/widgets/perception_widget.cpp @@ -35,11 +35,16 @@ // SA #include "perception_widget.h" +#include "header_widget.h" // Qt -#include -#include #include +#include +#include +#include +#include +#include +#include namespace moveit_setup_assistant { diff --git a/moveit_setup_assistant/src/widgets/perception_widget.h b/moveit_setup_assistant/src/widgets/perception_widget.h index e9c6952d6d..6d23c0d5ba 100644 --- a/moveit_setup_assistant/src/widgets/perception_widget.h +++ b/moveit_setup_assistant/src/widgets/perception_widget.h @@ -36,18 +36,15 @@ #pragma once // Qt -#include -#include -#include -#include -#include +class QComboBox; +class QGroupBox; +class QLineEdit; // SA #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index 5aec108049..857b48f5f6 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -55,13 +55,17 @@ #include "group_edit_widget.h" // for group rename page // Qt #include -#include +#include #include -#include -#include +#include +#include #include +#include +#include +#include #include #include +#include //// Cycle checking #include diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index 70a740c878..266ba5fe3a 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -36,12 +36,21 @@ // SA #include "robot_poses_widget.h" +#include "header_widget.h" #include // Qt +#include +#include +#include #include +#include +#include #include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.h b/moveit_setup_assistant/src/widgets/robot_poses_widget.h index c85b32f852..a90c38a98b 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.h +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.h @@ -37,17 +37,15 @@ #pragma once // Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +class QComboBox; +class QLabel; +class QLineEdit; +class QPushButton; +class QScrollArea; +class QSlider; +class QStackedWidget; +class QTableWidget; +class QVBoxLayout; // SA #ifndef Q_MOC_RUN @@ -56,7 +54,6 @@ #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index 242db5ed9d..f29e71817a 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -35,13 +35,27 @@ // SA #include "ros_controllers_widget.h" +#include "double_list_widget.h" +#include "controller_edit_widget.h" +#include "header_widget.h" #include // Qt +#include +#include #include +#include +#include +#include +#include #include -#include +#include +#include +#include +#include +#include +#include #include -#include +#include #include #include diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h index 515eee1666..f38408d1c7 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h @@ -36,27 +36,24 @@ #pragma once // Qt -#include -#include -#include -#include -#include -#include -#include -#include +class QHBoxLayout; +class QPushButton; +class QStackedWidget; +class QTreeWidget; +class QTreeWidgetItem; // SA #ifndef Q_MOC_RUN #include #endif -#include "double_list_widget.h" // for joints, links and subgroups pages -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant -#include "controller_edit_widget.h" namespace moveit_setup_assistant { +class DoubleListWidget; +class ControllerEditWidget; + class ROSControllersWidget : public SetupScreenWidget { Q_OBJECT diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 534831283a..9109169ebe 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -38,15 +38,20 @@ // SA #include "setup_screen_widget.h" // a base class for screens in the setup assistant #include "setup_assistant_widget.h" +#include "header_widget.h" + // Qt -#include -#include -#include +#include +#include #include +#include #include -#include -#include +#include +#include #include +#include +#include +#include #include #include // for loading all avail kinematic planners // Rviz diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h index 850492d3de..154f2dc0b4 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h @@ -37,18 +37,9 @@ #pragma once // Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -// Setup Asst +class QSplitter; + +// Setup Assistant #include "navigation_widget.h" #include "start_screen_widget.h" #include "default_collisions_widget.h" @@ -67,8 +58,7 @@ #include // Other -#include -#include // for parsing input arguments +#include // for parsing input arguments #include #endif diff --git a/moveit_setup_assistant/src/widgets/simulation_widget.cpp b/moveit_setup_assistant/src/widgets/simulation_widget.cpp index 0f917a7f87..c86395a3c3 100644 --- a/moveit_setup_assistant/src/widgets/simulation_widget.cpp +++ b/moveit_setup_assistant/src/widgets/simulation_widget.cpp @@ -35,12 +35,15 @@ // SA #include "simulation_widget.h" +#include "header_widget.h" // Qt -#include +#include +#include #include #include -#include +#include +#include #include #include diff --git a/moveit_setup_assistant/src/widgets/simulation_widget.h b/moveit_setup_assistant/src/widgets/simulation_widget.h index 029874d065..8fb5f79206 100644 --- a/moveit_setup_assistant/src/widgets/simulation_widget.h +++ b/moveit_setup_assistant/src/widgets/simulation_widget.h @@ -36,16 +36,14 @@ #pragma once // Qt -#include -#include -#include +class QLabel; +class QTextEdit; // SA #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index c64e8acf05..98a0bcfa1f 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -35,15 +35,20 @@ /* Author: Dave Coleman */ // Qt -#include -#include -#include +#include +#include +#include #include +#include +#include #include +#include +#include #include -#include -#include -#include +#include +#include +#include + // ROS #include #include // for getting file path for loadng images diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.h b/moveit_setup_assistant/src/widgets/start_screen_widget.h index c81def715e..b7c28ddef9 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.h +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.h @@ -36,12 +36,9 @@ #pragma once -#include -#include -#include -#include -#include -#include +class QLabel; +class QProgressBar; +class QPushButton; #ifndef Q_MOC_RUN #include // for testing a valid urdf is loaded diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp index 4a404c901c..b00d098865 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp @@ -36,10 +36,21 @@ // SA #include "virtual_joints_widget.h" +#include "header_widget.h" + // Qt +#include +#include #include +#include +#include +#include #include -#include +#include +#include +#include +#include +#include namespace moveit_setup_assistant { diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h b/moveit_setup_assistant/src/widgets/virtual_joints_widget.h index 24bf8c486e..f2d5ffaef0 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.h @@ -37,22 +37,18 @@ #pragma once // Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include +class QLabel; +class QLineEdit; +class QPushButton; +class QTableWidget; +class QStackedWidget; +class QComboBox; // SA #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant From 0642015e1a9051a223a0594bb98595fad8eeb0c7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 5 Oct 2020 22:34:08 +0200 Subject: [PATCH 482/612] Cleanup ROS includes --- moveit_setup_assistant/src/collisions_updater.cpp | 1 - .../src/widgets/configuration_files_widget.cpp | 3 --- moveit_setup_assistant/src/widgets/kinematic_chain_widget.h | 1 - moveit_setup_assistant/src/widgets/planning_groups_widget.cpp | 1 - moveit_setup_assistant/src/widgets/robot_poses_widget.cpp | 1 + moveit_setup_assistant/src/widgets/robot_poses_widget.h | 2 -- moveit_setup_assistant/src/widgets/start_screen_widget.cpp | 1 - 7 files changed, 1 insertion(+), 9 deletions(-) diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp index e6d2e015b9..89b418e96e 100644 --- a/moveit_setup_assistant/src/collisions_updater.cpp +++ b/moveit_setup_assistant/src/collisions_updater.cpp @@ -34,7 +34,6 @@ /* Author: Mathias Lüdtke */ -#include #include #include diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 872c71d8f9..b39ea2c013 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -50,9 +50,6 @@ #include "configuration_files_widget.h" #include "header_widget.h" -// ROS -#include - // Boost #include // for string find and replace in templates #include // for creating folders/files diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h index 6fc1fef14c..e5f004f222 100644 --- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h +++ b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h @@ -44,7 +44,6 @@ class QTreeWidgetItem; #ifndef Q_MOC_RUN #include -#include #endif namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index 857b48f5f6..060f723470 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -47,7 +47,6 @@ */ // ****************************************************************************************** -#include "ros/ros.h" #include "header_widget.h" #include "planning_groups_widget.h" #include "double_list_widget.h" // for joints, links and subgroups pages diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index 266ba5fe3a..61a59d1602 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -54,6 +54,7 @@ #include #include +#include namespace moveit_setup_assistant { diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.h b/moveit_setup_assistant/src/widgets/robot_poses_widget.h index a90c38a98b..dcc8377be4 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.h +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.h @@ -50,8 +50,6 @@ class QVBoxLayout; // SA #ifndef Q_MOC_RUN #include -#include // for collision stuff -#include #endif #include "setup_screen_widget.h" // a base class for screens in the setup assistant diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 98a0bcfa1f..757db3252d 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -50,7 +50,6 @@ #include // ROS -#include #include // for getting file path for loadng images // SA #include "header_widget.h" // title and instructions From d8e77c2d4b78b73268f01039b56f083f4a97d9ed Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 5 Oct 2020 23:46:11 +0200 Subject: [PATCH 483/612] Remove diagnostics macros This is more elegantly handled by system includes now. --- .../collision_distance_field_types.h | 4 - .../include/moveit/macros/diagnostics.h | 73 ------------------- .../moveit/benchmarks/BenchmarkExecutor.h | 4 - .../occupancy_map_monitor/occupancy_map.h | 4 - .../pointcloud_octomap_updater.h | 4 - .../motion_planning_frame.h | 4 - .../motion_planning_param_widget.h | 4 - .../src/motion_planning_display.cpp | 4 - .../src/robot_state_display.cpp | 4 - .../rviz_plugin_render_tools/octomap_render.h | 4 - .../planning_link_updater.h | 4 - .../robot_state_visualization.h | 4 - .../src/octomap_render.cpp | 3 - .../src/render_shapes.cpp | 4 - .../warehouse/src/moveit_message_storage.cpp | 4 - .../src/widgets/setup_assistant_widget.cpp | 4 - 16 files changed, 132 deletions(-) delete mode 100644 moveit_core/macros/include/moveit/macros/diagnostics.h diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index 65601df003..94256fe799 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -44,11 +44,7 @@ #include #include -#include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include -DIAGNOSTIC_POP #include #include diff --git a/moveit_core/macros/include/moveit/macros/diagnostics.h b/moveit_core/macros/include/moveit/macros/diagnostics.h deleted file mode 100644 index ff75b446b8..0000000000 --- a/moveit_core/macros/include/moveit/macros/diagnostics.h +++ /dev/null @@ -1,73 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Bielefeld University - * 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 Bielefeld University 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 - -/** The following macros allow to temporarily disable specific warnings, - * which in turn allows us to compile the MoveIt code base with rather strict - * compiler warnings, and still avoid corresponding issues in external sources. - * Just wrap an offending include with: - * - * DIAGNOSTIC_PUSH - * SILENT_UNUSED_PARAM - * #include - * DIAGNOSTIC_POP - * - * The push and pop operations ensure that the old status is restored afterwards. - */ - -#if defined(_MSC_VER) -#define DIAGNOSTIC_PUSH __pragma(warning(push)) -#define DIAGNOSTIC_POP __pragma(warning(pop)) -#define SILENT_UNUSED_PARAM __pragma(warning(disable : 4100)) - -#elif defined(__GNUC__) || defined(__clang__) -#define DO_PRAGMA(X) _Pragma(#X) -#define DIAGNOSTIC_PUSH DO_PRAGMA(GCC diagnostic push) -#define DIAGNOSTIC_POP DO_PRAGMA(GCC diagnostic pop) - -#if defined(__clang__) -#define SILENT_UNUSED_PARAM DO_PRAGMA(GCC diagnostic ignored "-Wunused-parameter") -#else -#define SILENT_UNUSED_PARAM \ - DO_PRAGMA(GCC diagnostic ignored "-Wunused-parameter") \ - DO_PRAGMA(GCC diagnostic ignored "-Wunused-but-set-parameter") -#endif - -#else -#define DIAGNOSTIC_PUSH -#define DIAGNOSTIC_POP -#define SILENT_UNUSED_PARAM - -#endif diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index c6ed75b396..d0af01a6b6 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -46,11 +46,7 @@ #include #include #include -#include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include -DIAGNOSTIC_PUSH #include #include diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h index 9ea4815e8e..35a08b69d0 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h @@ -36,11 +36,7 @@ #pragma once -#include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include -DIAGNOSTIC_PUSH #include #include #include diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h index 6053926ae0..69cd73f047 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h @@ -37,12 +37,8 @@ #pragma once #include -#include #include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include -DIAGNOSTIC_POP #include #include #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 56f199c39f..5177635d1e 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -47,12 +47,8 @@ #include #include #include -#include #include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include -DIAGNOSTIC_POP #include #include #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h index 855556953c..4761e293d0 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h @@ -37,11 +37,7 @@ #pragma once #include -#include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include -DIAGNOSTIC_POP namespace moveit { diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 41a0a471dc..47d13295f9 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -38,10 +38,7 @@ #include #include #include -#include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include #include #include @@ -63,7 +60,6 @@ SILENT_UNUSED_PARAM #include #include #include -DIAGNOSTIC_POP #include #include diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 0c0aaafc0a..983622814f 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -38,9 +38,6 @@ #include #include -#include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include #include #include @@ -56,7 +53,6 @@ SILENT_UNUSED_PARAM #include #include -DIAGNOSTIC_POP namespace moveit_rviz_plugin { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h index 1bb8bf5daa..d573733472 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h @@ -38,11 +38,7 @@ #include #include -#include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include -DIAGNOSTIC_POP #include #include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h index bf9e86af54..0ce29cacb9 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h @@ -36,11 +36,7 @@ #pragma once -#include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include -DIAGNOSTIC_POP #include namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index d210edef5e..f8fdf6d039 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -39,11 +39,7 @@ #include #include #include -#include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include -DIAGNOSTIC_POP namespace moveit_rviz_plugin { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp index 07d5868a90..dc25cf06d6 100755 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp @@ -36,11 +36,8 @@ #include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include #include -DIAGNOSTIC_POP #include #include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index f2ddd86aeb..4ab9391922 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -47,12 +47,8 @@ #include #include -#include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include #include -DIAGNOSTIC_POP #include #include diff --git a/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp b/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp index a7ff64e064..d45cffc0bd 100644 --- a/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp +++ b/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp @@ -35,11 +35,7 @@ /* Author: Ioan Sucan */ #include -#include -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include -DIAGNOSTIC_PUSH #include #include #include diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 9109169ebe..544cb56457 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -34,7 +34,6 @@ /* Author: Dave Coleman */ -#include // SA #include "setup_screen_widget.h" // a base class for screens in the setup assistant #include "setup_assistant_widget.h" @@ -55,14 +54,11 @@ #include #include // for loading all avail kinematic planners // Rviz -DIAGNOSTIC_PUSH -SILENT_UNUSED_PARAM #include #include #include #include #include -DIAGNOSTIC_POP namespace moveit_setup_assistant { From c16e27dc3cb9b296ecf78b1e13dd0489e35e4364 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 6 Oct 2020 07:43:45 +0200 Subject: [PATCH 484/612] Fix unused-parameter warnings --- moveit_ros/moveit_servo/src/servo_calcs.cpp | 2 +- moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp | 4 ++-- .../src/motion_planning_frame_joints_widget.cpp | 6 +++--- .../src/motion_planning_frame_objects.cpp | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index dd39f569c1..8dbb1be123 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -1035,7 +1035,7 @@ bool ServoCalcs::changeControlDimensions(moveit_msgs::ChangeControlDimensions::R return true; } -bool ServoCalcs::resetServoStatus(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) +bool ServoCalcs::resetServoStatus(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/) { status_ = StatusCode::NO_WARNING; return true; diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp index a5637a6eda..1579f1f12b 100644 --- a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp @@ -111,7 +111,7 @@ TEST_F(ServoFixture, SendTwistStampedTest) // count trajectory messages sent by servo size_t received_count = 0; boost::function traj_callback = - [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { ++received_count; }; + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& /*msg*/) { ++received_count; }; auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); // Create publisher to send servo commands @@ -152,7 +152,7 @@ TEST_F(ServoFixture, SendJointServoTest) // count trajectory messages sent by servo size_t received_count = 0; boost::function traj_callback = - [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { ++received_count; }; + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& /*msg*/) { ++received_count; }; auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); // Create publisher to send servo commands diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp index 7f4782f66c..d33563a76d 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -50,7 +50,7 @@ JMGItemModel::JMGItemModel(const moveit::core::RobotState& robot_state, const st jmg_ = robot_state_.getRobotModel()->getJointModelGroup(group_name); } -int JMGItemModel::rowCount(const QModelIndex& parent) const +int JMGItemModel::rowCount(const QModelIndex& /*parent*/) const { if (!jmg_) return robot_state_.getVariableCount(); @@ -58,7 +58,7 @@ int JMGItemModel::rowCount(const QModelIndex& parent) const return jmg_->getVariableCount(); } -int JMGItemModel::columnCount(const QModelIndex& parent) const +int JMGItemModel::columnCount(const QModelIndex& /*parent*/) const { return 2; } @@ -461,7 +461,7 @@ JointsWidgetEventFilter::JointsWidgetEventFilter(QAbstractItemView* view) : QObj { } -bool JointsWidgetEventFilter::eventFilter(QObject* target, QEvent* event) +bool JointsWidgetEventFilter::eventFilter(QObject* /*target*/, QEvent* event) { if (event->type() == QEvent::MouseButtonPress) { diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index c8a1316687..c16bc46399 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -73,7 +73,7 @@ QString subframe_poses_to_qstring(const moveit::core::FixedTransformsMap& subfra namespace moveit_rviz_plugin { -void MotionPlanningFrame::shapesComboBoxChanged(const QString& text) +void MotionPlanningFrame::shapesComboBoxChanged(const QString& /*text*/) { switch (ui_->shapes_combo_box->currentData().toInt()) // fetch shape ID from current combobox item { From b4e26cfb5be55a31e30c5ee33dfe4ce00279200c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 7 Oct 2020 00:50:25 +0200 Subject: [PATCH 485/612] MSA: Allow showing both, visual and collision geometry (#2352) --- .../src/widgets/setup_assistant_widget.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 534831283a..239cba6d5a 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -403,10 +404,25 @@ void SetupAssistantWidget::loadRviz() view->subProp("Distance")->setValue(4.0f); // Add Rviz to Planning Groups Widget - QHBoxLayout* rviz_layout = new QHBoxLayout(); + QVBoxLayout* rviz_layout = new QVBoxLayout(); rviz_layout->addWidget(rviz_render_panel_); rviz_container_->setLayout(rviz_layout); + // visual / collision buttons + auto btn_layout = new QHBoxLayout(); + rviz_layout->addLayout(btn_layout); + + QCheckBox* btn; + btn_layout->addWidget(btn = new QCheckBox("visual"), 0); + btn->setChecked(true); + connect(btn, &QCheckBox::toggled, + [this](bool checked) { robot_state_display_->subProp("Visual Enabled")->setValue(checked); }); + + btn_layout->addWidget(btn = new QCheckBox("collision"), 1); + btn->setChecked(false); + connect(btn, &QCheckBox::toggled, + [this](bool checked) { robot_state_display_->subProp("Collision Enabled")->setValue(checked); }); + rviz_container_->show(); } From 06a601ca93b305b7b4347cec02af4e09f4e309cd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 7 Oct 2020 14:44:44 +0200 Subject: [PATCH 486/612] MSA: Fix group editing (#2350) Group Editing: Cancel falls back to previous screen When entering joints/links/chain/subgroup edit screen from "Add Group" screen, cancel was cancelling the whole operation, loosing all info that was already entered for the group. Now it just returns to the previous screen and thus allows to continue editing the new group. --- .../src/widgets/planning_groups_widget.cpp | 94 ++++++++----------- .../src/widgets/planning_groups_widget.h | 14 +-- 2 files changed, 45 insertions(+), 63 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index 060f723470..9c91095b42 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -156,7 +156,7 @@ PlanningGroupsWidget::PlanningGroupsWidget(QWidget* parent, const MoveItConfigDa connect(group_edit_widget_, SIGNAL(saveChain()), this, SLOT(saveGroupScreenChain())); connect(group_edit_widget_, SIGNAL(saveSubgroups()), this, SLOT(saveGroupScreenSubgroups())); - // Combine into stack + // Combine into stack: Note, order is same as GroupType! stacked_widget_ = new QStackedWidget(this); stacked_widget_->addWidget(groups_tree_widget_); // screen index 0 stacked_widget_->addWidget(joints_widget_); // screen index 1 @@ -465,50 +465,29 @@ void PlanningGroupsWidget::editSelected() // Get the user custom properties of the currently selected row PlanGroupType plan_group = item->data(0, Qt::UserRole).value(); - if (plan_group.type_ == JOINT) + switch (plan_group.type_) { - // Load the data - loadJointsScreen(plan_group.group_); - - // Switch to screen - changeScreen(1); // 1 is index of joints - } - else if (plan_group.type_ == LINK) - { - // Load the data - loadLinksScreen(plan_group.group_); - - // Switch to screen - changeScreen(2); - } - else if (plan_group.type_ == CHAIN) - { - // Load the data - loadChainScreen(plan_group.group_); - - // Switch to screen - changeScreen(3); - } - else if (plan_group.type_ == SUBGROUP) - { - // Load the data - loadSubgroupsScreen(plan_group.group_); - - // Switch to screen - changeScreen(4); - } - else if (plan_group.type_ == GROUP) - { - // Load the data - loadGroupScreen(plan_group.group_); - - // Switch to screen - changeScreen(5); - } - else - { - QMessageBox::critical(this, "Error Loading", "An internal error has occured while loading."); + case JOINT: + loadJointsScreen(plan_group.group_); + break; + case LINK: + loadLinksScreen(plan_group.group_); + break; + case CHAIN: + loadChainScreen(plan_group.group_); + break; + case SUBGROUP: + loadSubgroupsScreen(plan_group.group_); + break; + case GROUP: + loadGroupScreen(plan_group.group_); + break; + default: + QMessageBox::critical(this, "Error Loading", "An internal error has occured while loading."); + return; } + return_screen_ = 0; // return to main screen directly + changeScreen(plan_group.type_); } // ****************************************************************************************** @@ -540,7 +519,6 @@ void PlanningGroupsWidget::loadJointsScreen(srdf::Model::Group* this_group) // Remember what is currently being edited so we can later save changes current_edit_group_ = this_group->name_; - current_edit_element_ = JOINT; } // ****************************************************************************************** @@ -572,7 +550,6 @@ void PlanningGroupsWidget::loadLinksScreen(srdf::Model::Group* this_group) // Remember what is currently being edited so we can later save changes current_edit_group_ = this_group->name_; - current_edit_element_ = LINK; } // ****************************************************************************************** @@ -605,7 +582,6 @@ void PlanningGroupsWidget::loadChainScreen(srdf::Model::Group* this_group) // Remember what is currently being edited so we can later save changes current_edit_group_ = this_group->name_; - current_edit_element_ = CHAIN; } // ****************************************************************************************** @@ -638,7 +614,6 @@ void PlanningGroupsWidget::loadSubgroupsScreen(srdf::Model::Group* this_group) // Remember what is currently being edited so we can later save changes current_edit_group_ = this_group->name_; - current_edit_element_ = SUBGROUP; } // ****************************************************************************************** @@ -669,9 +644,6 @@ void PlanningGroupsWidget::loadGroupScreen(srdf::Model::Group* this_group) // Set the data in the edit box group_edit_widget_->setSelected(current_edit_group_); - - // Remember what is currently being edited so we can later save changes - current_edit_element_ = GROUP; } // ****************************************************************************************** @@ -841,7 +813,7 @@ void PlanningGroupsWidget::addGroup() loadGroupScreen(nullptr); // NULL indicates this is a new group, not an existing one // Switch to screen - changeScreen(5); + changeScreen(GROUP); } // ****************************************************************************************** @@ -1272,9 +1244,10 @@ void PlanningGroupsWidget::saveGroupScreenJoints() // Find the group we are editing based on the goup name string loadJointsScreen(config_data_->findGroupByName(current_edit_group_)); + return_screen_ = GROUP; // Switch to screen - changeScreen(1); // 1 is index of joints + changeScreen(JOINT); } // ****************************************************************************************** @@ -1288,9 +1261,10 @@ void PlanningGroupsWidget::saveGroupScreenLinks() // Find the group we are editing based on the goup name string loadLinksScreen(config_data_->findGroupByName(current_edit_group_)); + return_screen_ = GROUP; // Switch to screen - changeScreen(2); // 2 is index of links + changeScreen(LINK); } // ****************************************************************************************** @@ -1304,9 +1278,10 @@ void PlanningGroupsWidget::saveGroupScreenChain() // Find the group we are editing based on the goup name string loadChainScreen(config_data_->findGroupByName(current_edit_group_)); + return_screen_ = GROUP; // Switch to screen - changeScreen(3); + changeScreen(CHAIN); } // ****************************************************************************************** @@ -1320,9 +1295,10 @@ void PlanningGroupsWidget::saveGroupScreenSubgroups() // Find the group we are editing based on the goup name string loadSubgroupsScreen(config_data_->findGroupByName(current_edit_group_)); + return_screen_ = GROUP; // Switch to screen - changeScreen(4); + changeScreen(SUBGROUP); } // ****************************************************************************************** @@ -1330,6 +1306,12 @@ void PlanningGroupsWidget::saveGroupScreenSubgroups() // ****************************************************************************************** void PlanningGroupsWidget::cancelEditing() { + if (return_screen_) + { + changeScreen(return_screen_); + return_screen_ = 0; + return; + } if (!current_edit_group_.empty() && adding_new_group_) { srdf::Model::Group* editing = config_data_->findGroupByName(current_edit_group_); @@ -1384,7 +1366,7 @@ void PlanningGroupsWidget::showMainScreen() { stacked_widget_->setCurrentIndex(0); - // Announce that this widget is not in modal mode + // Announce that this widget is not in modal mode anymore Q_EMIT isModal(false); } diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.h b/moveit_setup_assistant/src/widgets/planning_groups_widget.h index a61f2539b2..ef860eddbd 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.h +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.h @@ -61,11 +61,11 @@ class GroupEditWidget; // Custom Type enum GroupType { - JOINT, - LINK, - CHAIN, - SUBGROUP, - GROUP + JOINT = 1, + LINK = 2, + CHAIN = 3, + SUBGROUP = 4, + GROUP = 5 }; // ****************************************************************************************** @@ -172,8 +172,8 @@ private Q_SLOTS: /// Remember what group we are editing when an edit screen is being shown std::string current_edit_group_; - /// Remember what group element we are editing when an edit screen is being shown - GroupType current_edit_element_; + /// Remember to which editing screen we should return on Cancel + int return_screen_; /// Remember whethere we're editing a group or adding a new one bool adding_new_group_; From df77fe83969347ea0724294a950dc080d1c07804 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Fri, 18 Sep 2020 16:24:10 +0200 Subject: [PATCH 487/612] Add tests for checking whether an empty world + self-collision list any contacts --- moveit_ros/move_group/CMakeLists.txt | 1 + ...est_check_state_validity_in_empty_scene.py | 44 +++++++++++++++++++ ...t_check_state_validity_in_empty_scene.test | 24 ++++++++++ 3 files changed, 69 insertions(+) create mode 100755 moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.py create mode 100644 moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.test diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index b5f89fb410..a8c2098417 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -97,4 +97,5 @@ if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) # this test is flaky # add_rostest(test/test_cancel_before_plan_execution.test) + add_rostest(test/test_check_state_validity_in_empty_scene.test) endif() diff --git a/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.py b/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.py new file mode 100755 index 0000000000..784a40ae3c --- /dev/null +++ b/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python + +import sys +import rospy +import unittest +from actionlib import SimpleActionClient +import moveit_commander +from moveit_msgs.msg import Constraints +from moveit_msgs.srv import GetStateValidity + + +class TestCheckStateValidityInEmptyScene(unittest.TestCase): + + def setUp(self): + moveit_commander.roscpp_initialize(sys.argv) + self.robot_commander = moveit_commander.RobotCommander() + self.group_name = self.robot_commander.get_group_names()[0] + self.group_commander = moveit_commander.MoveGroupCommander(self.group_name) + + self._check_state_validity = rospy.ServiceProxy('/check_state_validity', GetStateValidity) + + def test_check_collision_free_state_validity_in_empty_scene(self): + current_robot_state = self.robot_commander.get_current_state() + + validity_report = self._check_state_validity(current_robot_state, self.group_name, Constraints()) + self.assertListEqual(validity_report.contacts, [], "In the default_robot_state, the robot should not collide with itself") + + + def test_check_colliding_state_validity_in_empty_scene(self): + current_robot_state = self.robot_commander.get_current_state() + + # force a colliding state with the Fanuc M-10iA + current_robot_state.joint_state.position = list(current_robot_state.joint_state.position) + current_robot_state.joint_state.position[current_robot_state.joint_state.name.index("joint_3")] = -2 + + validity_report = self._check_state_validity(current_robot_state, self.group_name, Constraints()) + + self.assertNotEqual(len(validity_report.contacts), 0, "When the robot collides with itself, it should have some contacts (with itself)") + + +if __name__ == '__main__': + import rostest + rospy.init_node('check_state_validity_in_empty_scene') + rostest.rosrun('moveit_ros_move_group', 'test_check_state_validity_in_empty_scene', TestCheckStateValidityInEmptyScene) diff --git a/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.test b/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.test new file mode 100644 index 0000000000..b12a6d3092 --- /dev/null +++ b/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.test @@ -0,0 +1,24 @@ + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + From fa605f2cedf31eb09d32206265b3f66f3b0397ba Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Fri, 18 Sep 2020 14:37:27 +0200 Subject: [PATCH 488/612] Let the max number of contacts be the amount of world objects + link models with geometry --- .../state_validation_service_capability.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp index 385d715847..1706f57b8d 100644 --- a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp @@ -66,8 +66,8 @@ bool MoveGroupStateValidationService::computeService(moveit_msgs::GetStateValidi creq.group_name = req.group_name; creq.cost = true; creq.contacts = true; - creq.max_contacts = ls->getWorld()->size(); - creq.max_cost_sources = creq.max_contacts + ls->getRobotModel()->getLinkModelsWithCollisionGeometry().size(); + creq.max_contacts = ls->getWorld()->size() + ls->getRobotModel()->getLinkModelsWithCollisionGeometry().size(); + creq.max_cost_sources = creq.max_contacts; creq.max_contacts *= creq.max_contacts; collision_detection::CollisionResult cres; From 13d635c6445de582418853ade3245592443caf7f Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 9 Oct 2020 12:43:17 +0200 Subject: [PATCH 489/612] move_group: simplify test launch files --- .../test_cancel_before_plan_execution.test | 20 ++---------------- ...t_check_state_validity_in_empty_scene.test | 21 ++----------------- 2 files changed, 4 insertions(+), 37 deletions(-) diff --git a/moveit_ros/move_group/test/test_cancel_before_plan_execution.test b/moveit_ros/move_group/test/test_cancel_before_plan_execution.test index 4acb89e476..a4f22e456f 100644 --- a/moveit_ros/move_group/test/test_cancel_before_plan_execution.test +++ b/moveit_ros/move_group/test/test_cancel_before_plan_execution.test @@ -1,22 +1,6 @@ - - - - - - - - [move_group/fake_controller_joint_states] - - - - - - - - - - + + diff --git a/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.test b/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.test index b12a6d3092..b761f4dded 100644 --- a/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.test +++ b/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.test @@ -1,23 +1,6 @@ - - - - - - - - - [move_group/fake_controller_joint_states] - - - - - - - - - - + + From c938c9c359e0335a996c37f5d38f5556fa6077a2 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 11 Oct 2020 14:19:33 -0500 Subject: [PATCH 490/612] A library for servoing toward a moving pose (#2203) * A library for servoing toward a moving pose * Use the control_toolbox version of PID control * Minor cleanup after switching to control_toolbox PID * Use default ROS1 library type * Decrease the default windup limit * Add a basic unit test * Add orientation tracking, too * Warning if no target pose is provided * One PID for orientation instead of 3 * A different method of ensuring a new target pose is received * Multithread the test pose publisher * Correct service namespaces * Clang format and clang tidy * Minor tweaks for CI * Add function to update PID settings * Add a method to get PID errors * Be consistent with service namespacing This is important if there are multiple arms Servoing simultaneously * Address Adam's more minor comments * First attempt at using pose tracking * Small updates to launch files and CMakeLists * Address Andy comments * Get Pose Tracking demo working (#3) * Rework command frame name and target pose * Add EE frame tf and make it available thru C++ interface * Pass getEEFrameTransform thru PoseTracking * Update TF calcs at servo start * Modify pose demo to move slightly from current position * Make PoseTracking use EE tf instead of command frame tf * Clean up the demo * Make helper func for converting eigen to tf message * Make the Servo instance public, to access functions like setPaused() * Use an enum class for status codes, so entries can be duplicated * Add a timeout argument * Clang format * Fix a race condition with initial joint reception * Update config file for testing * Wrap tf lookup in a try/catch block * Use a tf2 utility for Eigen->msg conversion * Explicitly initialize transforms to zero * More robust test setup * Update the command frame at runtime * Update angular PID configs, too * Properly wait for test setup Co-authored-by: AdamPettinger --- moveit_ros/moveit_servo/CMakeLists.txt | 80 ++-- .../config/pose_tracking_settings.yaml | 21 + .../config/ur_simulated_config.yaml | 5 +- .../moveit_servo/make_shared_from_pool.h | 1 + .../include/moveit_servo/pose_tracking.h | 195 ++++++++++ .../moveit_servo/include/moveit_servo/servo.h | 25 +- .../include/moveit_servo/servo_calcs.h | 29 +- .../include/moveit_servo/servo_parameters.h | 1 + .../include/moveit_servo/status_codes.h | 18 +- .../launch/pose_tracking_example.launch | 10 + moveit_ros/moveit_servo/package.xml | 8 +- .../pose_tracking_example.cpp | 155 ++++++++ .../src/joint_state_subscriber.cpp | 8 +- moveit_ros/moveit_servo/src/pose_tracking.cpp | 359 ++++++++++++++++++ moveit_ros/moveit_servo/src/servo.cpp | 113 +++--- moveit_ros/moveit_servo/src/servo_calcs.cpp | 92 ++++- .../test/config/servo_settings.yaml | 5 +- .../moveit_servo/test/pose_tracking_test.cpp | 162 ++++++++ .../moveit_servo/test/pose_tracking_test.test | 24 ++ .../test/servo_cpp_interface_test.cpp | 7 + 20 files changed, 1219 insertions(+), 99 deletions(-) create mode 100644 moveit_ros/moveit_servo/config/pose_tracking_settings.yaml create mode 100644 moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h create mode 100644 moveit_ros/moveit_servo/launch/pose_tracking_example.launch create mode 100644 moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp create mode 100644 moveit_ros/moveit_servo/src/pose_tracking.cpp create mode 100644 moveit_ros/moveit_servo/test/pose_tracking_test.cpp create mode 100644 moveit_ros/moveit_servo/test/pose_tracking_test.test diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 0b06a77205..86c6d1ebcc 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -7,16 +7,17 @@ endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -set(LIBRARY_NAME moveit_servo_cpp_api) +set(SERVO_LIB_NAME moveit_servo_cpp_api) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() find_package(Boost REQUIRED) - +find_package(Eigen3 REQUIRED) find_package(catkin REQUIRED COMPONENTS control_msgs + control_toolbox geometry_msgs moveit_msgs moveit_ros_planning_interface @@ -24,17 +25,19 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs std_srvs + tf2_eigen trajectory_msgs ) -find_package(Eigen3 REQUIRED) catkin_package( INCLUDE_DIRS include LIBRARIES - ${LIBRARY_NAME} + pose_tracking + ${SERVO_LIB_NAME} CATKIN_DEPENDS control_msgs + control_toolbox geometry_msgs moveit_msgs moveit_ros_planning_interface @@ -42,6 +45,7 @@ catkin_package( sensor_msgs std_msgs std_srvs + tf2_eigen trajectory_msgs DEPENDS EIGEN3 @@ -54,35 +58,59 @@ include_directories(SYSTEM ${Boost_INCLUDE_DIR} ) -######################################### -## A library providing a C++ interface ## -######################################### +######################################################### +## Library to process realtime twist or joint commands ## +######################################################### -add_library(${LIBRARY_NAME} SHARED +# This library provides an interface for sending realtime twist or joint commands to a robot +add_library(${SERVO_LIB_NAME} + # These files are used to produce differential motion src/collision_check.cpp src/servo_calcs.cpp src/servo.cpp src/joint_state_subscriber.cpp src/low_pass_filter.cpp ) -set_target_properties(${LIBRARY_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -add_dependencies(${LIBRARY_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${LIBRARY_NAME} +set_target_properties(${SERVO_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +add_dependencies(${SERVO_LIB_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${SERVO_LIB_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${Boost_LIBRARIES} ) -# An example of using the C++ library +# An example of streaming realtime Cartesian and joint commands add_executable(cpp_interface_example src/cpp_interface_example/cpp_interface_example.cpp ) add_dependencies(cpp_interface_example ${catkin_EXPORTED_TARGETS}) target_link_libraries(cpp_interface_example ${catkin_LIBRARIES} - ${Eigen_LIBRARIES} - ${Boost_LIBRARIES} - ${LIBRARY_NAME} + ${SERVO_LIB_NAME} +) + +# An example of pose tracking +add_executable(pose_tracking_example + src/cpp_interface_example/pose_tracking_example.cpp +) +add_dependencies(pose_tracking_example ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pose_tracking_example + ${catkin_LIBRARIES} + ${SERVO_LIB_NAME} + pose_tracking +) + +######################################## +## Library for servoing toward a pose ## +######################################## + +add_library(pose_tracking + src/pose_tracking.cpp +) +add_dependencies(pose_tracking ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pose_tracking + ${catkin_LIBRARIES} + ${SERVO_LIB_NAME} ) ############################ @@ -94,10 +122,8 @@ add_executable(servo_server ) add_dependencies(servo_server ${catkin_EXPORTED_TARGETS}) target_link_libraries(servo_server - ${LIBRARY_NAME} + ${SERVO_LIB_NAME} ${catkin_LIBRARIES} - ${Eigen_LIBRARIES} - ${Boost_LIBRARIES} ) ################################################ @@ -116,7 +142,9 @@ target_link_libraries(spacenav_to_twist ${catkin_LIBRARIES}) install( TARGETS - ${LIBRARY_NAME} + ${SERVO_LIB_NAME} + pose_tracking + pose_tracking_example servo_server spacenav_to_twist ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -144,9 +172,17 @@ if(CATKIN_ENABLE_TESTING) test/servo_cpp_interface_test.cpp ) target_link_libraries(servo_cpp_interface_test - ${LIBRARY_NAME} + ${SERVO_LIB_NAME} + ${catkin_LIBRARIES} + ) + + # pose_tracking + add_rostest_gtest(pose_tracking_test + test/pose_tracking_test.test + test/pose_tracking_test.cpp + ) + target_link_libraries(pose_tracking_test + pose_tracking ${catkin_LIBRARIES} - ${Eigen_LIBRARIES} - ${Boost_LIBRARIES} ) endif() diff --git a/moveit_ros/moveit_servo/config/pose_tracking_settings.yaml b/moveit_ros/moveit_servo/config/pose_tracking_settings.yaml new file mode 100644 index 0000000000..e4b803774d --- /dev/null +++ b/moveit_ros/moveit_servo/config/pose_tracking_settings.yaml @@ -0,0 +1,21 @@ +################################# +# PID parameters for pose seeking +################################# + +# Maximum value of error integral for all PID controllers +windup_limit: 0.05 + +# PID gains +x_proportional_gain: 1.5 +y_proportional_gain: 1.5 +z_proportional_gain: 1.5 +x_integral_gain: 0.0 +y_integral_gain: 0.0 +z_integral_gain: 0.0 +x_derivative_gain: 0.0 +y_derivative_gain: 0.0 +z_derivative_gain: 0.0 + +angular_proportional_gain: 0.5 +angular_integral_gain: 0.0 +angular_derivative_gain: 0.0 diff --git a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml index 21fd7cbb36..d2ea184f40 100644 --- a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml @@ -4,7 +4,6 @@ use_gazebo: true # Whether the robot is started in a Gazebo simulation environment ## Properties of incoming commands -robot_link_command_frame: world # commands must be given in the frame of a robot link. Usually either the base or end effector command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" @@ -31,6 +30,10 @@ publish_joint_accelerations: false move_group_name: manipulator # Often 'manipulator' or 'arm' planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' +## Other frames +ee_frame_name: ee_link # The name of the end effector link, used to return the EE pose +robot_link_command_frame: world # commands must be given in the frame of a robot link. Usually either the base or end effector + ## Stopping behaviour incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. diff --git a/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h index a593da6127..d3931dad37 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h @@ -38,6 +38,7 @@ #pragma once +#include #include namespace moveit diff --git a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h new file mode 100644 index 0000000000..bd7c24fbce --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h @@ -0,0 +1,195 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik LLC + * 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 PickNik LLC 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. + *********************************************************************/ +/* + Author: Andy Zelenak + Desc: Servoing. Track a pose setpoint in real time. +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +// Conventions: +// Calculations are done in the planning_frame_ unless otherwise noted. + +namespace moveit_servo +{ +struct PIDConfig +{ + // Default values + double dt = 0.001; + double k_p = 1; + double k_i = 0; + double k_d = 0; + double windup_limit = 0.1; +}; + +enum class PoseTrackingStatusCode : int8_t +{ + INVALID = -1, + SUCCESS = 0, + NO_RECENT_TARGET_POSE = 1, + NO_RECENT_END_EFFECTOR_POSE = 2, + STOP_REQUESTED = 3 +}; + +const std::unordered_map POSE_TRACKING_STATUS_CODE_MAP( + { { PoseTrackingStatusCode::INVALID, "Invalid" }, + { PoseTrackingStatusCode::SUCCESS, "Success" }, + { PoseTrackingStatusCode::NO_RECENT_TARGET_POSE, "No recent target pose" }, + { PoseTrackingStatusCode::NO_RECENT_END_EFFECTOR_POSE, "No recent end effector pose" }, + { PoseTrackingStatusCode::STOP_REQUESTED, "Stop requested" } }); + +/** + * Class PoseTracking - subscribe to a target pose. + * Servo toward the target pose. + */ +class PoseTracking +{ +public: + /** \brief Constructor. Loads ROS parameters under the given namespace. */ + PoseTracking(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::string& parameter_ns = ""); + + PoseTrackingStatusCode moveToPose(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance, + const double target_pose_timeout); + + /** \brief A method for a different thread to stop motion and return early from control loop */ + void stopMotion() + { + stop_requested_ = true; + } + + /** \brief Change PID parameters. Motion is stopped before the udpate */ + void updatePIDConfig(const double x_proportional_gain, const double x_integral_gain, const double x_derivative_gain, + const double y_proportional_gain, const double y_integral_gain, const double y_derivative_gain, + const double z_proportional_gain, const double z_integral_gain, const double z_derivative_gain, + const double angular_proportional_gain, const double angular_integral_gain, + const double angular_derivative_gain); + + void getPIDErrors(double& x_error, double& y_error, double& z_error, double& orientation_error); + + /** + * Get the End Effector link transform. + * The transform from the MoveIt planning frame to EE link + * + * @param transform the transform that will be calculated + * @return true if a valid transform was available + */ + bool getCommandFrameTransform(geometry_msgs::TransformStamped& transform); + + // moveit_servo::Servo instance. Public so we can access member functions like setPaused() + std::unique_ptr servo_; + +private: + /** \brief Load ROS parameters for controller settings. */ + void readROSParams(); + + /** \brief Initialize a PID controller and add it to vector of controllers */ + void initializePID(const PIDConfig& pid_config, std::vector& pid_vector); + + /** \brief Return true if a target pose has been received within timeout [seconds] */ + bool haveRecentTargetPose(const double timeout); + + /** \brief Return true if an end effector pose has been received within timeout [seconds] */ + bool haveRecentEndEffectorPose(const double timeout); + + /** \brief Check if XYZ, roll/pitch/yaw tolerances are satisfied */ + bool satisfiesPoseTolerance(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance); + + /** \brief Subscribe to the target pose on this topic */ + void targetPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg); + + /** \brief Update PID controller target positions & orientations */ + void updateControllerSetpoints(); + + /** \brief Update PID controller states (positions & orientations) */ + void updateControllerStateMeasurements(); + + /** \brief Use PID controllers to calculate a full spatial velocity toward a pose */ + geometry_msgs::TwistStampedConstPtr calculateTwistCommand(); + + /** \brief Reset flags and PID controllers after a motion completes */ + void doPostMotionReset(); + + ros::NodeHandle nh_; + + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + robot_model::RobotModelConstPtr robot_model_; + const moveit::core::JointModelGroup* joint_model_group_; + // Joint group used for controlling the motions + std::string move_group_name_; + + ros::Rate loop_rate_; + + // ROS interface to Servo + ros::Publisher twist_stamped_pub_; + + std::vector cartesian_position_pids_; + std::vector cartesian_orientation_pids_; + // Cartesian PID configs + PIDConfig x_pid_config_, y_pid_config_, z_pid_config_, angular_pid_config_; + + // Transforms w.r.t. planning_frame_ + Eigen::Isometry3d command_frame_transform_; + ros::Time command_frame_transform_stamp_; + geometry_msgs::PoseStamped target_pose_; + + // Subscribe to target pose + ros::Subscriber target_pose_sub_; + + tf2_ros::Buffer transform_buffer_; + tf2_ros::TransformListener transform_listener_; + + // Expected frame name, for error checking and transforms + std::string planning_frame_; + + // Flag that a different thread has requested a stop. + std::atomic stop_requested_; + + // Read parameters from this namespace + std::string parameter_ns_; + + double angular_error_; +}; + +// using alias +using PoseTrackingPtr = std::shared_ptr; +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h index 139c11b44b..b520d807d6 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -53,7 +53,8 @@ namespace moveit_servo class Servo { public: - Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::string& parameter_ns = ""); ~Servo(); @@ -71,6 +72,17 @@ class Servo * @return true if a valid transform was available */ bool getCommandFrameTransform(Eigen::Isometry3d& transform); + bool getCommandFrameTransform(geometry_msgs::TransformStamped& transform); + + /** + * Get the End Effector link transform. + * The transform from the MoveIt planning frame to EE link + * + * @param transform the transform that will be calculated + * @return true if a valid transform was available + */ + bool getEEFrameTransform(Eigen::Isometry3d& transform); + bool getEEFrameTransform(geometry_msgs::TransformStamped& transform); /** \brief Get the parameters used by servo node. */ const ServoParameters& getParameters() const; @@ -78,6 +90,14 @@ class Servo /** \brief Get the latest joint state. */ sensor_msgs::JointStateConstPtr getLatestJointState() const; + /** \brief Change the controlled link. Often, this is the end effector + * This must be a link on the robot since MoveIt tracks the transform (not tf) + */ + void changeRobotLinkCommandFrame(const std::string& new_command_frame) + { + servo_calcs_->changeRobotLinkCommandFrame(new_command_frame); + } + private: bool readParameters(); @@ -92,6 +112,9 @@ class Servo std::shared_ptr joint_state_subscriber_; std::unique_ptr servo_calcs_; std::unique_ptr collision_checker_; + + // Read parameters from this namespace + std::string parameter_ns_; }; // ServoPtr using alias diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index fa477eaabc..26f7c4c947 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -39,6 +39,9 @@ #pragma once // ROS +#include +#include +#include #include #include #include @@ -47,8 +50,7 @@ #include #include #include -#include -#include +#include #include // moveit_servo @@ -62,9 +64,9 @@ namespace moveit_servo class ServoCalcs { public: - ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, + ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& joint_state_subscriber); + const std::shared_ptr& joint_state_subscriber, std::string& ros_namespace); ~ServoCalcs() { @@ -82,10 +84,26 @@ class ServoCalcs * @return true if a valid transform was available */ bool getCommandFrameTransform(Eigen::Isometry3d& transform); + bool getCommandFrameTransform(geometry_msgs::TransformStamped& transform); + + /** + * Get the End Effector link transform. + * The transform from the MoveIt planning frame to EE link + * + * @param transform the transform that will be calculated + * @return true if a valid transform was available + */ + bool getEEFrameTransform(Eigen::Isometry3d& transform); + bool getEEFrameTransform(geometry_msgs::TransformStamped& transform); /** \brief Pause or unpause processing servo commands while keeping the timers alive */ void setPaused(bool paused); + /** \brief Change the controlled link. Often, this is the end effector + * This must be a link on the robot since MoveIt tracks the transform (not tf) + */ + void changeRobotLinkCommandFrame(const std::string& new_command_frame); + private: /** \brief Timer method */ void run(const ros::TimerEvent& timer_event); @@ -197,7 +215,7 @@ class ServoCalcs ros::NodeHandle nh_; // Parameters from yaml - const ServoParameters& parameters_; + ServoParameters& parameters_; // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; @@ -282,6 +300,7 @@ class ServoCalcs // latest_state_mutex_ is used to protect the state below it mutable std::mutex latest_state_mutex_; Eigen::Isometry3d tf_moveit_to_robot_cmd_frame_; + Eigen::Isometry3d tf_moveit_to_ee_frame_; geometry_msgs::TwistStampedConstPtr latest_twist_stamped_; control_msgs::JointJogConstPtr latest_joint_cmd_; ros::Time latest_twist_command_stamp_ = ros::Time(0.); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h index ad0deec16f..45b208db8e 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h @@ -52,6 +52,7 @@ struct ServoParameters std::string robot_link_command_frame; std::string command_out_topic; std::string planning_frame; + std::string ee_frame_name; std::string status_topic; std::string joint_command_in_topic; std::string command_in_type; diff --git a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h index 97e92210cc..5164f46a9e 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h @@ -43,7 +43,7 @@ namespace moveit_servo { -enum StatusCode : int8_t +enum class StatusCode : int8_t { INVALID = -1, NO_WARNING = 0, @@ -54,12 +54,12 @@ enum StatusCode : int8_t JOINT_BOUND = 5 }; -const std::unordered_map - SERVO_STATUS_CODE_MAP({ { INVALID, "Invalid" }, - { NO_WARNING, "No warnings" }, - { DECELERATE_FOR_SINGULARITY, "Close to a singularity, decelerating" }, - { HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, - { DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, - { HALT_FOR_COLLISION, "Collision detected, emergency stop" }, - { JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }); +const std::unordered_map + SERVO_STATUS_CODE_MAP({ { StatusCode::INVALID, "Invalid" }, + { StatusCode::NO_WARNING, "No warnings" }, + { StatusCode::DECELERATE_FOR_SINGULARITY, "Close to a singularity, decelerating" }, + { StatusCode::HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, + { StatusCode::DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, + { StatusCode::HALT_FOR_COLLISION, "Collision detected, emergency stop" }, + { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }); } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch new file mode 100644 index 0000000000..b3fcc374d2 --- /dev/null +++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index a9d51061ba..954795120c 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -21,14 +21,16 @@ catkin control_msgs + control_toolbox geometry_msgs + moveit_msgs + moveit_ros_planning_interface + rosparam_shortcuts sensor_msgs std_msgs std_srvs + tf2_eigen trajectory_msgs - moveit_msgs - moveit_ros_planning_interface - rosparam_shortcuts joy_teleop spacenav_node diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp new file mode 100644 index 0000000000..95d3a0aa4e --- /dev/null +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp @@ -0,0 +1,155 @@ +/******************************************************************************* + * Title : pose_tracking_example.cpp + * Project : moveit_servo + * Created : 09/04/2020 + * Author : Adam Pettinger + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 the copyright holder 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 HOLDER 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 +#include +#include +#include + +static const std::string LOGNAME = "cpp_interface_example"; + +// Class for monitoring status of moveit_servo +class StatusMonitor +{ +public: + StatusMonitor(ros::NodeHandle& nh, const std::string& topic) + { + sub_ = nh.subscribe(topic, 1, &StatusMonitor::statusCB, this); + } + +private: + void statusCB(const std_msgs::Int8ConstPtr& msg) + { + moveit_servo::StatusCode latest_status = static_cast(msg->data); + if (latest_status != status_) + { + status_ = latest_status; + const auto& status_str = moveit_servo::SERVO_STATUS_CODE_MAP.at(status_); + ROS_INFO_STREAM_NAMED(LOGNAME, "Servo status: " << status_str); + } + } + moveit_servo::StatusCode status_ = moveit_servo::StatusCode::INVALID; + ros::Subscriber sub_; +}; + +/** + * Instantiate the pose tracking interface. + * Send a pose slightly different from the starting pose + * Then keep updating the target pose a little bit + */ +int main(int argc, char** argv) +{ + ros::init(argc, argv, LOGNAME); + ros::NodeHandle nh; + ros::AsyncSpinner spinner(8); + spinner.start(); + + // Load the planning scene monitor + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; + planning_scene_monitor = std::make_shared("robot_description"); + if (!planning_scene_monitor->getPlanningScene()) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); + exit(EXIT_FAILURE); + } + + planning_scene_monitor->startSceneMonitor(); + planning_scene_monitor->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + planning_scene_monitor->startStateMonitor(); + + // Create the pose tracker + moveit_servo::PoseTracking tracker(planning_scene_monitor, "servo_server"); + + // Make a publisher for sending pose commands + ros::Publisher target_pose_pub = + nh.advertise("/servo_server/target_pose", 1 /* queue */, true /* latch */); + + // Subscribe to servo status (and log it when it changes) + StatusMonitor status_monitor(nh, "servo_server/status"); + + Eigen::Vector3d lin_tol{ 0.01, 0.01, 0.01 }; + double rot_tol = 0.1; + + // Get the current EE transform + geometry_msgs::TransformStamped current_ee_tf; + tracker.getCommandFrameTransform(current_ee_tf); + + // Convert it to a Pose + geometry_msgs::PoseStamped target_pose; + target_pose.header.frame_id = current_ee_tf.header.frame_id; + target_pose.pose.position.x = current_ee_tf.transform.translation.x; + target_pose.pose.position.y = current_ee_tf.transform.translation.y; + target_pose.pose.position.z = current_ee_tf.transform.translation.z; + target_pose.pose.orientation = current_ee_tf.transform.rotation; + + // Modify it a little bit + target_pose.pose.position.x += 0.1; + + // Publish target pose + target_pose.header.stamp = ros::Time::now(); + target_pose_pub.publish(target_pose); + + // Run the pose tracking in a new thread + std::thread move_to_pose_thread( + [&tracker, &lin_tol, &rot_tol] { tracker.moveToPose(lin_tol, rot_tol, 0.1 /* target pose timeout */); }); + + ros::Rate loop_rate(50); + for (size_t i = 0; i < 500; ++i) + { + // Modify the pose target a little bit each cycle + // This is a dynamic pose target + target_pose.pose.position.z += 0.0004; + target_pose.header.stamp = ros::Time::now(); + target_pose_pub.publish(target_pose); + + loop_rate.sleep(); + } + + // Make sure the tracker is stopped and clean up + tracker.stopMotion(); + move_to_pose_thread.join(); + + return EXIT_SUCCESS; +} diff --git a/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp b/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp index c60b331119..22b8b4bfab 100644 --- a/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp +++ b/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp @@ -49,9 +49,15 @@ JointStateSubscriber::JointStateSubscriber(ros::NodeHandle& nh, const std::strin // subscribe to joints joint_state_sub_ = nh.subscribe(joint_state_topic_name, ROS_QUEUE_SIZE, &JointStateSubscriber::jointStateCB, this); + ros::AsyncSpinner spinner(1); + spinner.start(); + // Wait for initial messages ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg."); - ros::topic::waitForMessage(joint_state_topic_name); + while (latest_joint_state_ == nullptr) + { + ros::Duration(0.01).sleep(); + } ROS_INFO_NAMED(LOGNAME, "Received first joint msg."); } diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp new file mode 100644 index 0000000000..82ed5b8868 --- /dev/null +++ b/moveit_ros/moveit_servo/src/pose_tracking.cpp @@ -0,0 +1,359 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik LLC + * 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 PickNik LLC 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 "moveit_servo/pose_tracking.h" + +namespace +{ +constexpr char LOGNAME[] = "pose_tracking"; +constexpr double DEFAULT_LOOP_RATE = 100; // Hz +constexpr double ROS_STARTUP_WAIT = 10; // sec +} // namespace + +namespace moveit_servo +{ +PoseTracking::PoseTracking(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::string& parameter_ns) + : planning_scene_monitor_(planning_scene_monitor) + , loop_rate_(DEFAULT_LOOP_RATE) + , transform_listener_(transform_buffer_) + , stop_requested_(false) + , parameter_ns_(parameter_ns) + , angular_error_(0) +{ + readROSParams(); + + robot_model_ = planning_scene_monitor_->getRobotModel(); + joint_model_group_ = robot_model_->getJointModelGroup(move_group_name_); + + // Initialize PID controllers + initializePID(x_pid_config_, cartesian_position_pids_); + initializePID(y_pid_config_, cartesian_position_pids_); + initializePID(z_pid_config_, cartesian_position_pids_); + initializePID(angular_pid_config_, cartesian_orientation_pids_); + + // Use the C++ interface that Servo provides + servo_ = std::make_unique(nh_, planning_scene_monitor_, parameter_ns_); + servo_->start(); + + // Connect to Servo ROS interfaces + std::string target_pose_topic = "/" + parameter_ns_ + "/target_pose"; + target_pose_sub_ = + nh_.subscribe(target_pose_topic, 1, &PoseTracking::targetPoseCallback, this); + + // Publish outgoing twist commands to the Servo object + twist_stamped_pub_ = + nh_.advertise(servo_->getParameters().cartesian_command_in_topic, 1); +} + +PoseTrackingStatusCode PoseTracking::moveToPose(const Eigen::Vector3d& positional_tolerance, + const double angular_tolerance, const double target_pose_timeout) +{ + // Roll back the target pose timestamp to ensure we wait for a new target pose message + target_pose_.header.stamp = ros::Time::now() - ros::Duration(2 * target_pose_timeout); + + // Wait a bit for a target pose message to arrive. + // The target pose may get updated by new messages as the robot moves (in a callback function). + ros::Time start_time = ros::Time::now(); + while ((!haveRecentTargetPose(target_pose_timeout) || !haveRecentEndEffectorPose(target_pose_timeout)) && + ((ros::Time::now() - start_time).toSec() < target_pose_timeout)) + { + if (servo_->getCommandFrameTransform(command_frame_transform_)) + { + command_frame_transform_stamp_ = ros::Time::now(); + } + ros::Duration(0.001).sleep(); + } + if (!haveRecentTargetPose(target_pose_timeout)) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "The target pose was not updated recently. Aborting."); + return PoseTrackingStatusCode::NO_RECENT_TARGET_POSE; + } + + while (ros::ok()) + { + // Check for reasons to stop: + // - Goal tolerance is satisfied + // - Timeout + // - Another thread requested a stop + // - PID controllers aren't initialized + if (satisfiesPoseTolerance(positional_tolerance, angular_tolerance)) + { + break; + } + + // Attempt to update robot pose + if (servo_->getCommandFrameTransform(command_frame_transform_)) + { + command_frame_transform_stamp_ = ros::Time::now(); + } + + if (!haveRecentEndEffectorPose(target_pose_timeout)) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "The end effector pose was not updated in time. Aborting."); + doPostMotionReset(); + return PoseTrackingStatusCode::NO_RECENT_END_EFFECTOR_POSE; + } + if (stop_requested_) + { + ROS_INFO_STREAM_NAMED(LOGNAME, "Halting servo motion, a stop was requested."); + doPostMotionReset(); + return PoseTrackingStatusCode::STOP_REQUESTED; + } + + // Compute servo command from PID controller output + auto msg = calculateTwistCommand(); + + // Send command to the Servo object, for execution + twist_stamped_pub_.publish(*msg); + } + + doPostMotionReset(); + return PoseTrackingStatusCode::SUCCESS; +} + +void PoseTracking::readROSParams() +{ + std::size_t error = 0; + + // Check for parameter namespace from launch file. All other parameters will be read from this namespace. + std::string yaml_namespace; + if (ros::param::get("~parameter_ns", yaml_namespace)) + { + if (!parameter_ns_.empty()) + ROS_WARN_STREAM_NAMED(LOGNAME, + "A parameter namespace was specified in the launch file AND in the constructor argument."); + + parameter_ns_ = yaml_namespace; + } + + // Wait for ROS parameters to load + ros::Time begin = ros::Time::now(); + while (ros::ok() && !ros::param::has(parameter_ns_ + "/planning_frame") && + ((ros::Time::now() - begin).toSec() < ROS_STARTUP_WAIT)) + { + ROS_WARN_STREAM_NAMED(LOGNAME, "Waiting for parameter: " << parameter_ns_ + "/planning_frame"); + ros::Duration(0.1).sleep(); + } + + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/planning_frame", planning_frame_); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/move_group_name", move_group_name_); + if (!planning_scene_monitor_->getRobotModel()->hasJointModelGroup(move_group_name_)) + { + ++error; + ROS_ERROR_STREAM_NAMED(LOGNAME, "Unable to find the specified joint model group: " << move_group_name_); + } + + double publish_period; + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/publish_period", publish_period); + loop_rate_ = ros::Rate(1 / publish_period); + + x_pid_config_.dt = publish_period; + y_pid_config_.dt = publish_period; + z_pid_config_.dt = publish_period; + angular_pid_config_.dt = publish_period; + + double windup_limit; + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/windup_limit", windup_limit); + x_pid_config_.windup_limit = windup_limit; + y_pid_config_.windup_limit = windup_limit; + z_pid_config_.windup_limit = windup_limit; + angular_pid_config_.windup_limit = windup_limit; + + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/x_proportional_gain", x_pid_config_.k_p); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/y_proportional_gain", y_pid_config_.k_p); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/z_proportional_gain", z_pid_config_.k_p); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/x_integral_gain", x_pid_config_.k_i); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/y_integral_gain", y_pid_config_.k_i); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/z_integral_gain", z_pid_config_.k_i); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/x_derivative_gain", x_pid_config_.k_d); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/y_derivative_gain", y_pid_config_.k_d); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/z_derivative_gain", z_pid_config_.k_d); + + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/angular_proportional_gain", angular_pid_config_.k_p); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/angular_integral_gain", angular_pid_config_.k_i); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/angular_derivative_gain", angular_pid_config_.k_d); + + rosparam_shortcuts::shutdownIfError(ros::this_node::getName(), error); +} + +void PoseTracking::initializePID(const PIDConfig& pid_config, std::vector& pid_vector) +{ + bool use_anti_windup = true; + pid_vector.push_back(control_toolbox::Pid(pid_config.k_p, pid_config.k_i, pid_config.k_d, -pid_config.windup_limit, + pid_config.windup_limit, use_anti_windup)); +} + +bool PoseTracking::haveRecentTargetPose(const double timespan) +{ + return ((ros::Time::now() - target_pose_.header.stamp).toSec() < timespan); +} + +bool PoseTracking::haveRecentEndEffectorPose(const double timespan) +{ + return ((ros::Time::now() - command_frame_transform_stamp_).toSec() < timespan); +} + +bool PoseTracking::satisfiesPoseTolerance(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance) +{ + double x_error = target_pose_.pose.position.x - command_frame_transform_.translation()(0); + double y_error = target_pose_.pose.position.y - command_frame_transform_.translation()(1); + double z_error = target_pose_.pose.position.z - command_frame_transform_.translation()(2); + + return (fabs(x_error) < positional_tolerance(0)) && (fabs(y_error) < positional_tolerance(1)) && + (fabs(z_error) < positional_tolerance(2) && fabs(angular_error_) < angular_tolerance); +} + +void PoseTracking::targetPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg) +{ + // Transform to planning frame + target_pose_ = *msg; + geometry_msgs::TransformStamped target_to_planning_frame; + if (target_pose_.header.frame_id != planning_frame_) + { + try + { + target_to_planning_frame = transform_buffer_.lookupTransform(planning_frame_, target_pose_.header.frame_id, + ros::Time(0), ros::Duration(0.1)); + } + catch (tf2::TransformException& ex) + { + ROS_WARN_NAMED(LOGNAME, "%s", ex.what()); + return; + } + tf2::doTransform(target_pose_, target_pose_, target_to_planning_frame); + } + target_pose_.header.stamp = ros::Time::now(); +} + +geometry_msgs::TwistStampedConstPtr PoseTracking::calculateTwistCommand() +{ + // use the shared pool to create a message more efficiently + auto msg = moveit::util::make_shared_from_pool(); + msg->header.frame_id = target_pose_.header.frame_id; + + // Get twist components from PID controllers + geometry_msgs::Twist& twist = msg->twist; + + // Position + twist.linear.x = cartesian_position_pids_[0].computeCommand( + target_pose_.pose.position.x - command_frame_transform_.translation()(0), loop_rate_.expectedCycleTime()); + twist.linear.y = cartesian_position_pids_[1].computeCommand( + target_pose_.pose.position.y - command_frame_transform_.translation()(1), loop_rate_.expectedCycleTime()); + twist.linear.z = cartesian_position_pids_[2].computeCommand( + target_pose_.pose.position.z - command_frame_transform_.translation()(2), loop_rate_.expectedCycleTime()); + + // Orientation algorithm: + // - Find the orientation error as a quaternion: q_error = q_desired * q_current ^ -1 + // - Use the quaternion PID controllers to calculate a quaternion rate, q_error_dot + // - Convert to angular velocity for the TwistStamped message + Eigen::Quaterniond q_desired(target_pose_.pose.orientation.w, target_pose_.pose.orientation.x, + target_pose_.pose.orientation.y, target_pose_.pose.orientation.z); + Eigen::Quaterniond q_current(command_frame_transform_.rotation()); + Eigen::Quaterniond q_error = q_desired * q_current.inverse(); + + // Convert axis-angle to angular velocity + Eigen::AngleAxisd axis_angle(q_error); + // Cache the angular error, for rotation tolerance checking + angular_error_ = axis_angle.angle(); + double ang_vel_magnitude = + cartesian_orientation_pids_[0].computeCommand(angular_error_, loop_rate_.expectedCycleTime()); + twist.angular.x = ang_vel_magnitude * axis_angle.axis()[0]; + twist.angular.y = ang_vel_magnitude * axis_angle.axis()[1]; + twist.angular.z = ang_vel_magnitude * axis_angle.axis()[2]; + + msg->header.stamp = ros::Time::now(); + + return msg; +} + +void PoseTracking::doPostMotionReset() +{ + stop_requested_ = false; + angular_error_ = 0; + + // Reset error integrals and previous errors of PID controllers + cartesian_position_pids_[0].reset(); + cartesian_position_pids_[1].reset(); + cartesian_position_pids_[2].reset(); + cartesian_orientation_pids_[0].reset(); +} + +void PoseTracking::updatePIDConfig(const double x_proportional_gain, const double x_integral_gain, + const double x_derivative_gain, const double y_proportional_gain, + const double y_integral_gain, const double y_derivative_gain, + const double z_proportional_gain, const double z_integral_gain, + const double z_derivative_gain, const double angular_proportional_gain, + const double angular_integral_gain, const double angular_derivative_gain) +{ + stopMotion(); + + x_pid_config_.k_p = x_proportional_gain; + x_pid_config_.k_i = x_integral_gain; + x_pid_config_.k_d = x_derivative_gain; + y_pid_config_.k_p = y_proportional_gain; + y_pid_config_.k_i = y_integral_gain; + y_pid_config_.k_d = y_derivative_gain; + z_pid_config_.k_p = z_proportional_gain; + z_pid_config_.k_i = z_integral_gain; + z_pid_config_.k_d = z_derivative_gain; + + angular_pid_config_.k_p = angular_proportional_gain; + angular_pid_config_.k_i = angular_integral_gain; + angular_pid_config_.k_d = angular_derivative_gain; + + cartesian_position_pids_.clear(); + cartesian_orientation_pids_.clear(); + initializePID(x_pid_config_, cartesian_position_pids_); + initializePID(y_pid_config_, cartesian_position_pids_); + initializePID(z_pid_config_, cartesian_position_pids_); + initializePID(angular_pid_config_, cartesian_orientation_pids_); + + doPostMotionReset(); +} + +void PoseTracking::getPIDErrors(double& x_error, double& y_error, double& z_error, double& orientation_error) +{ + double dummy1, dummy2; + cartesian_position_pids_.at(0).getCurrentPIDErrors(&x_error, &dummy1, &dummy2); + cartesian_position_pids_.at(1).getCurrentPIDErrors(&y_error, &dummy1, &dummy2); + cartesian_position_pids_.at(2).getCurrentPIDErrors(&z_error, &dummy1, &dummy2); + cartesian_orientation_pids_.at(0).getCurrentPIDErrors(&orientation_error, &dummy1, &dummy2); +} + +bool PoseTracking::getCommandFrameTransform(geometry_msgs::TransformStamped& transform) +{ + return servo_->getCommandFrameTransform(transform); +} +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 761be5903e..f278d18f67 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -45,8 +45,9 @@ static const std::string LOGNAME = "servo_node"; namespace moveit_servo { -Servo::Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) - : nh_(nh), planning_scene_monitor_(planning_scene_monitor) +Servo::Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::string& parameter_ns) + : nh_(nh), planning_scene_monitor_(planning_scene_monitor), parameter_ns_(parameter_ns) { // Read ROS parameters, typically from YAML file if (!readParameters()) @@ -54,7 +55,8 @@ Servo::Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMon joint_state_subscriber_ = std::make_shared(nh_, parameters_.joint_topic); - servo_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); + servo_calcs_ = + std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_, parameter_ns_); collision_checker_ = std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); @@ -65,67 +67,67 @@ bool Servo::readParameters() { std::size_t error = 0; - // Specified in the launch file. All other parameters will be read from this namespace. - std::string parameter_ns; - ros::param::get("~parameter_ns", parameter_ns); - if (parameter_ns.empty()) + // Check for parameter namespace from launch file. All other parameters will be read from this namespace. + std::string yaml_namespace; + if (ros::param::get("~parameter_ns", yaml_namespace)) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "A namespace must be specified in the launch file, like:"); - ROS_ERROR_STREAM_NAMED(LOGNAME, ""); - return false; + if (!parameter_ns_.empty()) + ROS_WARN_STREAM_NAMED(LOGNAME, + "A parameter namespace was specified in the launch file AND in the constructor argument."); + + parameter_ns_ = yaml_namespace; } - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_period", parameters_.publish_period); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_check_rate", parameters_.collision_check_rate); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/num_outgoing_halt_msgs_to_publish", + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/publish_period", parameters_.publish_period); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/collision_check_rate", parameters_.collision_check_rate); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/num_outgoing_halt_msgs_to_publish", parameters_.num_outgoing_halt_msgs_to_publish); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/scale/linear", parameters_.linear_scale); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/scale/rotational", parameters_.rotational_scale); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/scale/joint", parameters_.joint_scale); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/scale/linear", parameters_.linear_scale); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/scale/rotational", parameters_.rotational_scale); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/scale/joint", parameters_.joint_scale); error += - !rosparam_shortcuts::get("", nh_, parameter_ns + "/low_pass_filter_coeff", parameters_.low_pass_filter_coeff); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/joint_topic", parameters_.joint_topic); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/command_in_type", parameters_.command_in_type); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/cartesian_command_in_topic", + !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/low_pass_filter_coeff", parameters_.low_pass_filter_coeff); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/joint_topic", parameters_.joint_topic); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/command_in_type", parameters_.command_in_type); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/cartesian_command_in_topic", parameters_.cartesian_command_in_topic); error += - !rosparam_shortcuts::get("", nh_, parameter_ns + "/joint_command_in_topic", parameters_.joint_command_in_topic); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/robot_link_command_frame", + !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/joint_command_in_topic", parameters_.joint_command_in_topic); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/robot_link_command_frame", parameters_.robot_link_command_frame); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/incoming_command_timeout", + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/incoming_command_timeout", parameters_.incoming_command_timeout); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/lower_singularity_threshold", + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/lower_singularity_threshold", parameters_.lower_singularity_threshold); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/hard_stop_singularity_threshold", + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/hard_stop_singularity_threshold", parameters_.hard_stop_singularity_threshold); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/move_group_name", parameters_.move_group_name); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/planning_frame", parameters_.planning_frame); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/use_gazebo", parameters_.use_gazebo); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/joint_limit_margin", parameters_.joint_limit_margin); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/command_out_topic", parameters_.command_out_topic); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/command_out_type", parameters_.command_out_type); - error += - !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_joint_positions", parameters_.publish_joint_positions); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_joint_velocities", + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/move_group_name", parameters_.move_group_name); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/planning_frame", parameters_.planning_frame); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/ee_frame_name", parameters_.ee_frame_name); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/use_gazebo", parameters_.use_gazebo); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/joint_limit_margin", parameters_.joint_limit_margin); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/command_out_topic", parameters_.command_out_topic); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/command_out_type", parameters_.command_out_type); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/publish_joint_positions", + parameters_.publish_joint_positions); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/publish_joint_velocities", parameters_.publish_joint_velocities); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_joint_accelerations", + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/publish_joint_accelerations", parameters_.publish_joint_accelerations); // Parameters for collision checking - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/check_collisions", parameters_.check_collisions); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_check_type", parameters_.collision_check_type); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/check_collisions", parameters_.check_collisions); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/collision_check_type", parameters_.collision_check_type); bool have_self_collision_proximity_threshold = rosparam_shortcuts::get( - "", nh_, parameter_ns + "/self_collision_proximity_threshold", parameters_.self_collision_proximity_threshold); + "", nh_, parameter_ns_ + "/self_collision_proximity_threshold", parameters_.self_collision_proximity_threshold); bool have_scene_collision_proximity_threshold = rosparam_shortcuts::get( - "", nh_, parameter_ns + "/scene_collision_proximity_threshold", parameters_.scene_collision_proximity_threshold); + "", nh_, parameter_ns_ + "/scene_collision_proximity_threshold", parameters_.scene_collision_proximity_threshold); double collision_proximity_threshold; // 'collision_proximity_threshold' parameter was removed, replaced with separate self- and scene-collision proximity // thresholds // TODO(JStech): remove this deprecation warning in ROS Noetic; simplify error case handling - if (nh_.hasParam(parameter_ns + "/collision_proximity_threshold") && - rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_proximity_threshold", collision_proximity_threshold)) + if (nh_.hasParam(parameter_ns_ + "/collision_proximity_threshold") && + rosparam_shortcuts::get("", nh_, parameter_ns_ + "/collision_proximity_threshold", collision_proximity_threshold)) { ROS_WARN_NAMED(LOGNAME, "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate" "'self_collision_proximity_threshold' and 'scene_collision_proximity_threshold' " @@ -143,22 +145,22 @@ bool Servo::readParameters() } error += !have_self_collision_proximity_threshold; error += !have_scene_collision_proximity_threshold; - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_distance_safety_factor", + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/collision_distance_safety_factor", parameters_.collision_distance_safety_factor); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/min_allowable_collision_distance", + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/min_allowable_collision_distance", parameters_.min_allowable_collision_distance); // This parameter name was changed recently. // Try retrieving from the correct name. If it fails, then try the deprecated name. // TODO(andyz): remove this deprecation warning in ROS Noetic - if (!rosparam_shortcuts::get("", nh_, parameter_ns + "/status_topic", parameters_.status_topic)) + if (!rosparam_shortcuts::get("", nh_, parameter_ns_ + "/status_topic", parameters_.status_topic)) { ROS_WARN_NAMED(LOGNAME, "'status_topic' parameter is missing. Recently renamed from 'warning_topic'. Please update " "the servoing yaml file."); - error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/warning_topic", parameters_.status_topic); + error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/warning_topic", parameters_.status_topic); } - rosparam_shortcuts::shutdownIfError(parameter_ns, error); + rosparam_shortcuts::shutdownIfError(parameter_ns_, error); // Input checking if (parameters_.publish_period <= 0.) @@ -302,6 +304,21 @@ bool Servo::getCommandFrameTransform(Eigen::Isometry3d& transform) return servo_calcs_->getCommandFrameTransform(transform); } +bool Servo::getCommandFrameTransform(geometry_msgs::TransformStamped& transform) +{ + return servo_calcs_->getEEFrameTransform(transform); +} + +bool Servo::getEEFrameTransform(Eigen::Isometry3d& transform) +{ + return servo_calcs_->getEEFrameTransform(transform); +} + +bool Servo::getEEFrameTransform(geometry_msgs::TransformStamped& transform) +{ + return servo_calcs_->getEEFrameTransform(transform); +} + const ServoParameters& Servo::getParameters() const { return parameters_; diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 8dbb1be123..f7cbfc48c3 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -69,12 +69,24 @@ bool isNonZero(const control_msgs::JointJog& msg) }; return !all_zeros; } + +// Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped +geometry_msgs::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, + const std::string& parent_frame, + const std::string& child_frame) +{ + geometry_msgs::TransformStamped output = tf2::eigenToTransform(eigen_tf); + output.header.frame_id = parent_frame; + output.child_frame_id = child_frame; + + return output; +} } // namespace // Constructor for the class that handles servoing calculations -ServoCalcs::ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, +ServoCalcs::ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& joint_state_subscriber) + const std::shared_ptr& joint_state_subscriber, std::string& ros_namespace) : nh_(nh) , parameters_(parameters) , planning_scene_monitor_(planning_scene_monitor) @@ -102,18 +114,15 @@ ServoCalcs::ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, // ROS Server for allowing drift in some dimensions drift_dimensions_server_ = - nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions", - &ServoCalcs::changeDriftDimensions, this); + nh_.advertiseService(ros_namespace + "/change_drift_dimensions", &ServoCalcs::changeDriftDimensions, this); // ROS Server for changing the control dimensions control_dimensions_server_ = - nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", - &ServoCalcs::changeControlDimensions, this); + nh_.advertiseService(ros_namespace + "/change_control_dimensions", &ServoCalcs::changeControlDimensions, this); // ROS Server to reset the status, e.g. so the arm can move again after a collision reset_servo_status_ = - nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/reset_servo_status", - &ServoCalcs::resetServoStatus, this); + nh_.advertiseService(ros_namespace + "/reset_servo_status", &ServoCalcs::resetServoStatus, this); // Publish and Subscribe to internal namespace topics ros::NodeHandle internal_nh("~internal"); @@ -147,6 +156,12 @@ ServoCalcs::ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, { position_filters_.emplace_back(parameters_.low_pass_filter_coeff); } + + // A matrix of all zeros is used to check whether matrices have been initialized + Eigen::Matrix3d empty_matrix; + empty_matrix.setZero(); + tf_moveit_to_ee_frame_ = empty_matrix; + tf_moveit_to_robot_cmd_frame_ = empty_matrix; } void ServoCalcs::start() @@ -180,6 +195,15 @@ void ServoCalcs::start() last_sent_command_ = initial_joint_trajectory; timer_ = nh_.createTimer(period_, &ServoCalcs::run, this); + + // TODO(adamp or andyz): after rebasing on upstream master, update this section. Maybe put the tf updates in a tiny + // function by themselves + sensor_msgs::JointStateConstPtr latest_joint_state = joint_state_subscriber_->getLatest(); + kinematic_state_->setVariableValues(*latest_joint_state); + tf_moveit_to_ee_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + kinematic_state_->getGlobalLinkTransform(parameters_.ee_frame_name); + tf_moveit_to_robot_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); } void ServoCalcs::run(const ros::TimerEvent& timer_event) @@ -232,6 +256,11 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) tf_moveit_to_robot_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); + // Calculate the transform from MoveIt planning frame to End Effector frame + // Calculate this transform to ensure it is available via C++ API + tf_moveit_to_ee_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + kinematic_state_->getGlobalLinkTransform(parameters_.ee_frame_name); + have_nonzero_command_ = have_nonzero_twist_stamped_ || have_nonzero_joint_command_; // Don't end this function without updating the filters @@ -403,6 +432,12 @@ bool ServoCalcs::cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, translation_vector = tf_moveit_to_robot_cmd_frame_.linear() * translation_vector; angular_vector = tf_moveit_to_robot_cmd_frame_.linear() * angular_vector; } + else if (cmd.header.frame_id == parameters_.ee_frame_name) + { + // If the frame is the EE frame, we already have that transform as well + translation_vector = tf_moveit_to_ee_frame_.linear() * translation_vector; + angular_vector = tf_moveit_to_ee_frame_.linear() * angular_vector; + } else { // We solve (planning_frame -> base -> cmd.header.frame_id) @@ -982,6 +1017,42 @@ bool ServoCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) return !transform.matrix().isZero(0); } +bool ServoCalcs::getCommandFrameTransform(geometry_msgs::TransformStamped& transform) +{ + const std::lock_guard lock(latest_state_mutex_); + // All zeros means the transform wasn't initialized, so return false + if (tf_moveit_to_robot_cmd_frame_.matrix().isZero(0)) + { + return false; + } + + transform = convertIsometryToTransform(tf_moveit_to_robot_cmd_frame_, parameters_.planning_frame, + parameters_.robot_link_command_frame); + return true; +} + +bool ServoCalcs::getEEFrameTransform(Eigen::Isometry3d& transform) +{ + const std::lock_guard lock(latest_state_mutex_); + transform = tf_moveit_to_ee_frame_; + + // All zeros means the transform wasn't initialized, so return false + return !transform.matrix().isZero(0); +} + +bool ServoCalcs::getEEFrameTransform(geometry_msgs::TransformStamped& transform) +{ + const std::lock_guard lock(latest_state_mutex_); + // All zeros means the transform wasn't initialized, so return false + if (tf_moveit_to_ee_frame_.matrix().isZero(0)) + { + return false; + } + + transform = convertIsometryToTransform(tf_moveit_to_ee_frame_, parameters_.planning_frame, parameters_.ee_frame_name); + return true; +} + void ServoCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg) { const std::lock_guard lock(latest_state_mutex_); @@ -1046,4 +1117,9 @@ void ServoCalcs::setPaused(bool paused) paused_ = paused; } +void ServoCalcs::changeRobotLinkCommandFrame(const std::string& new_command_frame) +{ + parameters_.robot_link_command_frame = new_command_frame; +} + } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml index 30e4cb59d2..802aeadca2 100644 --- a/moveit_ros/moveit_servo/test/config/servo_settings.yaml +++ b/moveit_ros/moveit_servo/test/config/servo_settings.yaml @@ -4,7 +4,6 @@ use_gazebo: false # Whether the robot is started in a Gazebo simulation environment ## Properties of incoming commands -robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" @@ -31,6 +30,10 @@ publish_joint_accelerations: false move_group_name: panda_arm # Often 'manipulator' or 'arm' planning_frame: panda_link0 # The MoveIt planning frame. Often 'panda_link0' or 'world' +## Other frames +ee_frame_name: panda_link7 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector + ## Stopping behaviour incoming_command_timeout: 1 # Stop servoing if X seconds elapse without a new command # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. diff --git a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp new file mode 100644 index 0000000000..782fa23c5a --- /dev/null +++ b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp @@ -0,0 +1,162 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik LLC + * 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 PickNik LLC 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. + *********************************************************************/ + +/* Author: Andy Zelenak + Desc: Test of tracking toward a pose +*/ + +// C++ +#include +#include + +// ROS +#include + +// Testing +#include + +// Servo +#include +#include + +static const std::string LOGNAME = "servo_cpp_interface_test"; +static constexpr double TRANSLATION_TOLERANCE = 0.01; // meters +static constexpr double ROTATION_TOLERANCE = 0.1; // quaternion +static constexpr double ROS_PUB_SUB_DELAY = 4; // allow for subscribers to initialize + +namespace moveit_servo +{ +class PoseTrackingFixture : public ::testing::Test +{ +public: + void SetUp() override + { + // Wait for several key topics / parameters + ros::topic::waitForMessage("/joint_states"); + while (!nh_.hasParam("/robot_description") && ros::ok()) + { + ros::Duration(0.1).sleep(); + } + + // Load the planning scene monitor + planning_scene_monitor_ = std::make_shared("robot_description"); + planning_scene_monitor_->startSceneMonitor(); + planning_scene_monitor_->startStateMonitor(); + planning_scene_monitor_->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + + tracker_ = std::make_shared(planning_scene_monitor_, ""); + + target_pose_pub_ = + nh_.advertise("/pose_tracking_test/target_pose", 1 /* queue */, true /* latch */); + + // Tolerance for pose seeking + translation_tolerance_ << TRANSLATION_TOLERANCE, TRANSLATION_TOLERANCE, TRANSLATION_TOLERANCE; + } + void TearDown() override + { + } + +protected: + ros::NodeHandle nh_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + Eigen::Vector3d translation_tolerance_; + moveit_servo::PoseTrackingPtr tracker_; + ros::Publisher target_pose_pub_; +}; // class PoseTrackingFixture + +// Check for commands going out to ros_control +TEST_F(PoseTrackingFixture, OutgoingMsgTest) +{ + // halt Servoing when first msg to ros_control is received + // and test some conditions + trajectory_msgs::JointTrajectory last_received_msg; + boost::function traj_callback = + [&/* this */](const trajectory_msgs::JointTrajectoryConstPtr& msg) { + EXPECT_EQ(msg->header.frame_id, "panda_link0"); + // Check for an expected joint position command + // As of now, the robot doesn't actually move because there are no controllers enabled. + double angle_tolerance = 0.08; // rad + EXPECT_NEAR(msg->points[0].positions[0], 0, angle_tolerance); + EXPECT_NEAR(msg->points[0].positions[1], -0.785, angle_tolerance); + EXPECT_NEAR(msg->points[0].positions[2], 0, angle_tolerance); + EXPECT_NEAR(msg->points[0].positions[3], -2.360, angle_tolerance); + EXPECT_NEAR(msg->points[0].positions[4], 0, angle_tolerance); + EXPECT_NEAR(msg->points[0].positions[5], 1.571, angle_tolerance); + EXPECT_NEAR(msg->points[0].positions[6], 0.785, angle_tolerance); + + this->tracker_->stopMotion(); + return; + }; + auto traj_sub = nh_.subscribe("servo_server/command", 1, traj_callback); + + geometry_msgs::PoseStamped target_pose; + target_pose.header.frame_id = "panda_link4"; + target_pose.header.stamp = ros::Time::now(); + target_pose.pose.position.x = 0.2; + target_pose.pose.position.y = 0.2; + target_pose.pose.position.z = 0.2; + target_pose.pose.orientation.w = 1; + + // Republish the target pose in a new thread, as if the target is moving + std::thread target_pub_thread([&] { + size_t msg_count = 0; + while (++msg_count < 100) + { + target_pose_pub_.publish(target_pose); + ros::Duration(0.01).sleep(); + } + }); + + ros::Duration(ROS_PUB_SUB_DELAY).sleep(); + tracker_->moveToPose(translation_tolerance_, ROTATION_TOLERANCE, 1 /* target pose timeout */); + + target_pub_thread.join(); +} + +} // namespace moveit_servo + +int main(int argc, char** argv) +{ + ros::init(argc, argv, LOGNAME); + testing::InitGoogleTest(&argc, argv); + + ros::AsyncSpinner spinner(8); + spinner.start(); + + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/moveit_ros/moveit_servo/test/pose_tracking_test.test b/moveit_ros/moveit_servo/test/pose_tracking_test.test new file mode 100644 index 0000000000..4f162d0c0d --- /dev/null +++ b/moveit_ros/moveit_servo/test/pose_tracking_test.test @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp index 1579f1f12b..53684c1787 100644 --- a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp @@ -59,6 +59,13 @@ class ServoFixture : public ::testing::Test public: void SetUp() override { + // Wait for several key topics / parameters + ros::topic::waitForMessage("/joint_states"); + while (!nh_.hasParam("/robot_description") && ros::ok()) + { + ros::Duration(0.1).sleep(); + } + // Load the planning scene monitor planning_scene_monitor_ = std::make_shared("robot_description"); planning_scene_monitor_->startSceneMonitor(); From e236475455026aa070cbfa936affb2ec3c87a891 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 12 Oct 2020 23:31:32 +0200 Subject: [PATCH 491/612] Depend on ros-noetic-fcl (0.6) in Noetic (#2359) * Depend on ros-noetic-fcl (0.6) in Noetic * Travis: switch to ros-testing repos --- .travis.yml | 2 +- moveit_core/package.xml | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 7b038728a7..ac6d1d5f18 100644 --- a/.travis.yml +++ b/.travis.yml @@ -14,7 +14,7 @@ env: global: - MOVEIT_CI_TRAVIS_TIMEOUT=85 # Travis grants us 90 min, but we add a safety margin of 5 min - ROS_DISTRO=noetic - - ROS_REPO=ros + - ROS_REPO=ros-testing - UPSTREAM_WORKSPACE=moveit.rosinstall - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" - WARNINGS_OK=false diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 6703fe348f..77f95622e1 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -26,7 +26,8 @@ bullet eigen_stl_containers eigen_conversions - libfcl-dev + libfcl-dev + fcl geometric_shapes geometry_msgs kdl_parser From c43b588dfb759918954aeeb3d7317e0f9d506f35 Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Tue, 13 Oct 2020 12:51:14 +0100 Subject: [PATCH 492/612] Make GILReleaser exception-safe (#2363) If an exception occurs while the GIL is released, it won't be reacquired and the stack unwinding will take place without the GIL. GILReleaser::release() and GILReleaser::reacquire() is removed, because it is unsafe. --- .../src/wrap_python_move_group.cpp | 76 ++++++++++--------- .../moveit/py_bindings_tools/gil_releaser.h | 27 ++----- 2 files changed, 47 insertions(+), 56 deletions(-) diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 89e500c64c..628f5880e6 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -458,10 +458,12 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer bp::tuple planPython() { - GILReleaser gr; MoveGroupInterface::Plan plan; - moveit_msgs::MoveItErrorCodes res = MoveGroupInterface::plan(plan); - gr.reacquire(); + moveit_msgs::MoveItErrorCodes res; + { + GILReleaser gr; + res = MoveGroupInterface::plan(plan); + } return bp::make_tuple(py_bindings_tools::serializeMsg(res), py_bindings_tools::serializeMsg(plan.trajectory_), plan.planning_time_); } @@ -488,10 +490,11 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer std::vector poses; convertListToArrayOfPoses(waypoints, poses); moveit_msgs::RobotTrajectory trajectory; - GILReleaser gr; - double fraction = - computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions); - gr.reacquire(); + double fraction; + { + GILReleaser gr; + fraction = computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions); + } return bp::make_tuple(py_bindings_tools::serializeMsg(trajectory), fraction); } @@ -540,36 +543,39 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer // Convert trajectory message to object moveit_msgs::RobotTrajectory traj_msg; py_bindings_tools::deserializeMsg(traj_str, traj_msg); - GILReleaser gr; - robot_trajectory::RobotTrajectory traj_obj(getRobotModel(), getName()); - traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg); - - // Do the actual retiming - if (algorithm == "iterative_time_parameterization") - { - trajectory_processing::IterativeParabolicTimeParameterization time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); - } - else if (algorithm == "iterative_spline_parameterization") + bool algorithm_found = true; { - trajectory_processing::IterativeSplineParameterization time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); - } - else if (algorithm == "time_optimal_trajectory_generation") - { - trajectory_processing::TimeOptimalTrajectoryGeneration time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); - } - else - { - ROS_ERROR_STREAM_NAMED("move_group_py", "Unknown time parameterization algorithm: " << algorithm); - gr.reacquire(); - return py_bindings_tools::serializeMsg(moveit_msgs::RobotTrajectory()); - } + GILReleaser gr; + robot_trajectory::RobotTrajectory traj_obj(getRobotModel(), getName()); + traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg); + + // Do the actual retiming + if (algorithm == "iterative_time_parameterization") + { + trajectory_processing::IterativeParabolicTimeParameterization time_param; + time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); + } + else if (algorithm == "iterative_spline_parameterization") + { + trajectory_processing::IterativeSplineParameterization time_param; + time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); + } + else if (algorithm == "time_optimal_trajectory_generation") + { + trajectory_processing::TimeOptimalTrajectoryGeneration time_param; + time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); + } + else + { + ROS_ERROR_STREAM_NAMED("move_group_py", "Unknown time parameterization algorithm: " << algorithm); + algorithm_found = false; + traj_msg = moveit_msgs::RobotTrajectory(); + } - // Convert the retimed trajectory back into a message - traj_obj.getRobotTrajectoryMsg(traj_msg); - gr.reacquire(); + if (algorithm_found) + // Convert the retimed trajectory back into a message + traj_obj.getRobotTrajectoryMsg(traj_msg); + } return py_bindings_tools::serializeMsg(traj_msg); } else diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h index 697a0a446e..215a470810 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h @@ -59,13 +59,16 @@ class GILReleaser /** \brief Release the GIL on construction */ GILReleaser() noexcept { - m_thread_state = nullptr; - release(); + m_thread_state = PyEval_SaveThread(); } /** \brief Reacquire the GIL on destruction */ ~GILReleaser() noexcept { - reacquire(); + if (m_thread_state) + { + PyEval_RestoreThread(m_thread_state); + m_thread_state = nullptr; + } } GILReleaser(const GILReleaser&) = delete; @@ -87,24 +90,6 @@ class GILReleaser { std::swap(other.m_thread_state, m_thread_state); } - - /** \brief Reacquire the GIL, noop if already acquired */ - void reacquire() noexcept - { - if (m_thread_state) - { - PyEval_RestoreThread(m_thread_state); - m_thread_state = nullptr; - } - } - /** \brief Release the GIL (again), noop if already released */ - void release() noexcept - { - if (!m_thread_state) - { - m_thread_state = PyEval_SaveThread(); - } - } }; } // namespace py_bindings_tools From 176a1d52fe14c47aae58c8e213c08683ffeb8bd8 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 2 Oct 2020 15:01:56 -0600 Subject: [PATCH 493/612] Changelogs for Noetic 1.1.1 --- moveit/CHANGELOG.rst | 10 ++++++++++ moveit_commander/CHANGELOG.rst | 7 +++++++ moveit_core/CHANGELOG.rst | 11 +++++++++++ moveit_experimental/CHANGELOG.rst | 3 +++ moveit_kinematics/CHANGELOG.rst | 6 ++++++ moveit_planners/chomp/chomp_interface/CHANGELOG.rst | 5 +++++ .../chomp/chomp_motion_planner/CHANGELOG.rst | 3 +++ .../chomp/chomp_optimizer_adapter/CHANGELOG.rst | 3 +++ moveit_planners/moveit_planners/CHANGELOG.rst | 3 +++ moveit_planners/ompl/CHANGELOG.rst | 5 +++++ .../moveit_fake_controller_manager/CHANGELOG.rst | 5 +++++ moveit_plugins/moveit_plugins/CHANGELOG.rst | 3 +++ .../moveit_ros_control_interface/CHANGELOG.rst | 5 +++++ .../moveit_simple_controller_manager/CHANGELOG.rst | 5 +++++ moveit_ros/benchmarks/CHANGELOG.rst | 6 ++++++ moveit_ros/manipulation/CHANGELOG.rst | 5 +++++ moveit_ros/move_group/CHANGELOG.rst | 6 ++++++ moveit_ros/moveit_ros/CHANGELOG.rst | 3 +++ moveit_ros/moveit_servo/CHANGELOG.rst | 9 +++++++++ moveit_ros/occupancy_map_monitor/CHANGELOG.rst | 11 +++++++++++ moveit_ros/perception/CHANGELOG.rst | 7 +++++++ moveit_ros/planning/CHANGELOG.rst | 8 ++++++++ moveit_ros/planning_interface/CHANGELOG.rst | 8 ++++++++ moveit_ros/robot_interaction/CHANGELOG.rst | 5 +++++ moveit_ros/visualization/CHANGELOG.rst | 10 ++++++++++ moveit_ros/warehouse/CHANGELOG.rst | 6 ++++++ moveit_runtime/CHANGELOG.rst | 3 +++ moveit_setup_assistant/CHANGELOG.rst | 13 +++++++++++++ 28 files changed, 174 insertions(+) diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst index 673950a486..bafe0e5fc4 100644 --- a/moveit/CHANGELOG.rst +++ b/moveit/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature][visualization] Clean up Rviz Motion Planning plugin, add tooltips (`#2310 `_) +* [feature][moveit_servo] A library for servoing toward a moving pose (`#2203 `_) +* [feature][moveit_setup_assistant] Allow showing both, visual and collision geometry (`#2352 `_) +* [fix][moveit_setup_assistant] layout (`#2349 `_) +* [fix][moveit_setup_assistant] group editing (`#2350 `_) +* [fix][moveit_setup_assistant] disappearing robot on change of reference frame (`#2335 `_) +* Contributors: Felix von Drigalski, Michael Görner, Robert Haschke, Tyler Weaver, Yoan Mollard + 1.1.0 (2020-09-04) ------------------ * [maint] Use standard cmake text for metapackages (`#1620 `_) diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst index 547a0dfd0f..0b6b4fb7e2 100644 --- a/moveit_commander/CHANGELOG.rst +++ b/moveit_commander/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_commander ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature] MGC: Improve exception messages (`#2318 `_) +* [fix] ROS namespacing in moveit_commander's PSI (`#2347 `_) +* [fix] python3 issues (`#2323 `_) +* Contributors: Michael Görner, Peter Mitrano, Robert Haschke + 1.1.0 (2020-09-04) ------------------ * [feature] Add missing variants of place from list of PlaceLocations and Poses in the python interface (`#2231 `_) diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst index cf05521604..552ada72b3 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature] Handle multiple link libraries for FCL (`#2325 `_) +* [feature] Adapt to API changes in geometric_shapes (`#2324 `_) +* [fix] clang-tidy issues (`#2337 `_) +* [fix] various issues with Noetic build (`#2327 `_) +* [maint] Depend on ros-noetic-fcl (0.6) in Noetic (`#2359 `_) +* [maint] Cleanup MSA includes (`#2351 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, G.A. vd. Hoorn, Robert Haschke + 1.1.0 (2020-09-04) ------------------ * [feature] Add a utility to print collision pairs (`#2275 `_) diff --git a/moveit_experimental/CHANGELOG.rst b/moveit_experimental/CHANGELOG.rst index 8f5c956f25..b86a0e191b 100644 --- a/moveit_experimental/CHANGELOG.rst +++ b/moveit_experimental/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_experimental ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + 1.1.0 (2020-09-04) ------------------ diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst index 364acfb3a8..bb6a34e373 100644 --- a/moveit_kinematics/CHANGELOG.rst +++ b/moveit_kinematics/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_kinematics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [fix] various issues with Noetic build (`#2327 `_) +* [fix] python3 issues (`#2323 `_) +* Contributors: G.A. vd. Hoorn, Michael Görner, Robert Haschke + 1.1.0 (2020-09-04) ------------------ * [feature] Implementation of parameter TranslationXY2D IKFast (`#1949 `_) diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst index c44a1ee598..50f520c3ae 100644 --- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package chomp_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + 1.1.0 (2020-09-04) ------------------ * [feature] Replace $(find xacro)/xacro -> xacro (`#2282 `_) diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst index 949974baea..d66596da94 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package chomp_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + 1.1.0 (2020-09-04) ------------------ * [feature] Optional cpp version setting (`#2166 `_) diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst index b63344237c..413398e078 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_chomp_optimizer_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + 1.1.0 (2020-09-04) ------------------ * [feature] Optional cpp version setting (`#2166 `_) diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst index aa34f794f2..051d06bfc2 100644 --- a/moveit_planners/moveit_planners/CHANGELOG.rst +++ b/moveit_planners/moveit_planners/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_planners ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + 1.1.0 (2020-09-04) ------------------ diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst index ab987682bf..5bf99c74e5 100644 --- a/moveit_planners/ompl/CHANGELOG.rst +++ b/moveit_planners/ompl/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_planners_ompl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + 1.1.0 (2020-09-04) ------------------ diff --git a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst index a60906cd38..5efd4dcc8e 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_fake_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + 1.1.0 (2020-09-04) ------------------ diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst index 95539d4b24..1240cc9447 100644 --- a/moveit_plugins/moveit_plugins/CHANGELOG.rst +++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + 1.1.0 (2020-09-04) ------------------ * [feature] Use standard cmake text for metapackages (`#1620 `_) diff --git a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst index 0e26cf41e0..3be2a43d9d 100644 --- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst +++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_control_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + 1.1.0 (2020-09-04) ------------------ * [feature] Optional cpp version setting (`#2166 `_) diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst index 01fddb343d..99577cd5c9 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_simple_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + 1.1.0 (2020-09-04) ------------------ * [feature] Optional cpp version setting (`#2166 `_) diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst index c2f0f772f9..017ee36224 100644 --- a/moveit_ros/benchmarks/CHANGELOG.rst +++ b/moveit_ros/benchmarks/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_benchmarks ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [fix] python3 issues (`#2323 `_) +* [maint] Cleanup MSA includes (`#2351 `_) +* Contributors: Michael Görner, Robert Haschke + 1.1.0 (2020-09-04) ------------------ * [feature] Benchmark combinations of predefined poses (`#1548 `_) diff --git a/moveit_ros/manipulation/CHANGELOG.rst b/moveit_ros/manipulation/CHANGELOG.rst index a2af617046..c531e54c7a 100644 --- a/moveit_ros/manipulation/CHANGELOG.rst +++ b/moveit_ros/manipulation/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_manipulation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + 1.1.0 (2020-09-04) ------------------ diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst index 24e59df9d5..3a395043fe 100644 --- a/moveit_ros/move_group/CHANGELOG.rst +++ b/moveit_ros/move_group/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_move_group ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [fix] Let the max number of contacts be the amount of world objects + link models with geometry (`#2355 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, Loy van Beek, Michael Görner, v4hn + 1.1.0 (2020-09-04) ------------------ * [feature] Start new joint_state_publisher_gui on param use_gui (`#2257 `_) diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst index 42f711f6af..0239bdfeca 100644 --- a/moveit_ros/moveit_ros/CHANGELOG.rst +++ b/moveit_ros/moveit_ros/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + 1.1.0 (2020-09-04) ------------------ * [maint] Use standard cmake text for metapackages (`#1620 `_) diff --git a/moveit_ros/moveit_servo/CHANGELOG.rst b/moveit_ros/moveit_servo/CHANGELOG.rst index b14a65d9eb..5868a7b118 100644 --- a/moveit_ros/moveit_servo/CHANGELOG.rst +++ b/moveit_ros/moveit_servo/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package moveit_servo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature] A library for servoing toward a moving pose (`#2203 `_) +* [feature] Refactor velocity limit enforcement and add a unit test (`#2260 `_) +* [fix] Servo thread interruption (`#2314 `_) +* [fix] Servo heap-buffer-overflow bug (`#2307 `_) +* [maint] Cleanup MSA includes (`#2351 `_) +* Contributors: AndyZe, Robert Haschke, Tyler Weaver + 1.1.0 (2020-09-04) ------------------ * [feature] Update last_sent_command\_ at ServoCalcs start (`#2249 `_) diff --git a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst index 69b65519c8..6307a04d83 100644 --- a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst +++ b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit_ros_occupancy_map_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [fix] clang-tidy issues on Travis (`#2337 `_) + * Replace typedefs with using declarations + * Move default destructor definitions to headers + * Silent spurious clang-tidy warning + * Move variable definitions to their usage location +* [maint] Cleanup MSA includes (`#2351 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, Robert Haschke + 1.1.0 (2020-09-04) ------------------ diff --git a/moveit_ros/perception/CHANGELOG.rst b/moveit_ros/perception/CHANGELOG.rst index bdcd077d93..9f6dddd4f8 100644 --- a/moveit_ros/perception/CHANGELOG.rst +++ b/moveit_ros/perception/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_ros_perception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [fix] various issues with Noetic build (`#2327 `_) +* [maint] Cleanup MSA includes (`#2351 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, G.A. vd. Hoorn, Robert Haschke + 1.1.0 (2020-09-04) ------------------ * [feature] Utilize new geometric_shapes functions to improve performance (`#2038 `_) diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst index 1fffce8d1a..101db3f469 100644 --- a/moveit_ros/planning/CHANGELOG.rst +++ b/moveit_ros/planning/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package moveit_ros_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [fix] some clang-tidy issues on Travis (`#2337 `_) +* [fix] various issues with Noetic build (`#2327 `_) +* [fix] "Clear Octomap" button, disable when no octomap is published (`#2320 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, Robert Haschke + 1.1.0 (2020-09-04) ------------------ * [feature] Use Eigen::Transform::linear() instead of rotation() (`#1964 `_) diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst index d72c3b44a2..fbcb7a2659 100644 --- a/moveit_ros/planning_interface/CHANGELOG.rst +++ b/moveit_ros/planning_interface/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package moveit_ros_planning_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature] moveit_cpp: more informative error message, cover another potential failure condition. (`#2336 `_) +* [fix] Make GILReleaser exception-safe (`#2363 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* [maint] Replace panda_moveit_config -> moveit_resources_panda_moveit_config (`#2300 `_) +* Contributors: AndyZe, Bjar Ne, Felix von Drigalski, Robert Haschke + 1.1.0 (2020-09-04) ------------------ * [feature] Use Eigen::Transform::linear() instead of rotation() (`#1964 `_) diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst index 927f105b7e..58db765fb2 100644 --- a/moveit_ros/robot_interaction/CHANGELOG.rst +++ b/moveit_ros/robot_interaction/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_robot_interaction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + 1.1.0 (2020-09-04) ------------------ * [feature] Optional cpp version setting (`#2166 `_) diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst index 9257a6de63..983d6a24df 100644 --- a/moveit_ros/visualization/CHANGELOG.rst +++ b/moveit_ros/visualization/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package moveit_ros_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature] Clean up Rviz Motion Planning plugin, add tooltips (`#2310 `_) +* [fix] "Clear Octomap" button, disable when no octomap is published (`#2320 `_) +* [fix] clang-tidy warning (`#2334 `_) +* [fix] python3 issues (`#2323 `_) +* [maint] Cleanup MSA includes (`#2351 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, Michael Görner, Robert Haschke + 1.1.0 (2020-09-04) ------------------ diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst index c3d94b7f33..aceba5378e 100644 --- a/moveit_ros/warehouse/CHANGELOG.rst +++ b/moveit_ros/warehouse/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_warehouse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Cleanup MSA includes (`#2351 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, Robert Haschke + 1.1.0 (2020-09-04) ------------------ * [feature] Optional cpp version setting (`#2166 `_) diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst index ad382bb465..93bff64b2d 100644 --- a/moveit_runtime/CHANGELOG.rst +++ b/moveit_runtime/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_runtime ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + 1.1.0 (2020-09-04) ------------------ * [feature] Use standard cmake text for metapackages (`#1620 `_) diff --git a/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/CHANGELOG.rst index a38e7f88a7..3ec3076863 100644 --- a/moveit_setup_assistant/CHANGELOG.rst +++ b/moveit_setup_assistant/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package moveit_setup_assistant ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature] Allow showing both, visual and collision geometry (`#2352 `_) +* [fix] layout (`#2349 `_) +* [fix] group editing (`#2350 `_) +* [fix] only write default_planner_config field if any is selected (`#2293 `_) +* [fix] Segfault when editing pose in moveit_setup_assistant (`#2340 `_) +* [fix] disappearing robot on change of reference frame (`#2335 `_) +* [fix] robot_description is already loaded in move_group.launch (`#2313 `_) +* [maint] Cleanup MSA includes (`#2351 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, Michael Görner, Robert Haschke, Tyler Weaver, Yoan Mollard + 1.1.0 (2020-09-04) ------------------ * [feature] Start new joint_state_publisher_gui on param use_gui (`#2257 `_) From f9b2e2bc6c2dfd300eab63881475e8f621817037 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 13 Oct 2020 09:12:05 -0600 Subject: [PATCH 494/612] 1.1.1 --- moveit/package.xml | 2 +- moveit_commander/package.xml | 2 +- moveit_core/package.xml | 2 +- moveit_kinematics/package.xml | 2 +- moveit_planners/chomp/chomp_interface/package.xml | 2 +- moveit_planners/chomp/chomp_motion_planner/package.xml | 2 +- moveit_planners/chomp/chomp_optimizer_adapter/package.xml | 2 +- moveit_planners/moveit_planners/package.xml | 2 +- moveit_planners/ompl/package.xml | 2 +- moveit_plugins/moveit_fake_controller_manager/package.xml | 2 +- moveit_plugins/moveit_plugins/package.xml | 2 +- moveit_plugins/moveit_ros_control_interface/package.xml | 2 +- moveit_plugins/moveit_simple_controller_manager/package.xml | 2 +- moveit_ros/benchmarks/package.xml | 2 +- moveit_ros/manipulation/package.xml | 2 +- moveit_ros/move_group/package.xml | 2 +- moveit_ros/moveit_ros/package.xml | 2 +- moveit_ros/moveit_servo/package.xml | 2 +- moveit_ros/occupancy_map_monitor/package.xml | 2 +- moveit_ros/perception/package.xml | 2 +- moveit_ros/planning/package.xml | 2 +- moveit_ros/planning_interface/package.xml | 2 +- moveit_ros/robot_interaction/package.xml | 2 +- moveit_ros/visualization/package.xml | 2 +- moveit_ros/warehouse/package.xml | 2 +- moveit_runtime/package.xml | 2 +- moveit_setup_assistant/package.xml | 2 +- 27 files changed, 27 insertions(+), 27 deletions(-) diff --git a/moveit/package.xml b/moveit/package.xml index ac350c8c1c..0af3c9d8f5 100644 --- a/moveit/package.xml +++ b/moveit/package.xml @@ -1,7 +1,7 @@ moveit - 1.1.0 + 1.1.1 Meta package that contains all essential package of MoveIt. Until Summer 2016 MoveIt had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See the detailed discussion for the merge of several repositories. Dave Coleman Michael Ferguson diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml index 8b277c578d..e0fc5eb801 100644 --- a/moveit_commander/package.xml +++ b/moveit_commander/package.xml @@ -1,7 +1,7 @@ moveit_commander - 1.1.0 + 1.1.1 Python interfaces to MoveIt Ioan Sucan diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 77f95622e1..7c3fadcb8c 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -1,7 +1,7 @@ moveit_core - 1.1.0 + 1.1.1 Core libraries used by MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 8adef0388f..7a5861e0dd 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -1,7 +1,7 @@ moveit_kinematics - 1.1.0 + 1.1.1 Package for all inverse kinematics solvers in MoveIt Dave Coleman diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml index 8a4291483f..0960164a84 100644 --- a/moveit_planners/chomp/chomp_interface/package.xml +++ b/moveit_planners/chomp/chomp_interface/package.xml @@ -2,7 +2,7 @@ moveit_planners_chomp The interface for using CHOMP within MoveIt - 1.1.0 + 1.1.1 Gil Jones Chittaranjan Srinivas Swaminathan diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml index 1473bafc5a..343c3641e1 100644 --- a/moveit_planners/chomp/chomp_motion_planner/package.xml +++ b/moveit_planners/chomp/chomp_motion_planner/package.xml @@ -2,7 +2,7 @@ chomp_motion_planner chomp_motion_planner - 1.1.0 + 1.1.1 Gil Jones Mrinal Kalakrishnan diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml index d68ec3800f..1920747a59 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml +++ b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml @@ -3,7 +3,7 @@ moveit_chomp_optimizer_adapter MoveIt planning request adapter utilizing chomp for solution optimization - 1.1.0 + 1.1.1 Raghavender Sahdev Raghavender Sahdev MoveIt Release Team diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index bc7dc31873..b01309b3f2 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -1,6 +1,6 @@ moveit_planners - 1.1.0 + 1.1.1 Metapacakge that installs all available planners for MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index 937ad51a10..877590134e 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -1,6 +1,6 @@ moveit_planners_ompl - 1.1.0 + 1.1.1 MoveIt interface to OMPL Ioan Sucan diff --git a/moveit_plugins/moveit_fake_controller_manager/package.xml b/moveit_plugins/moveit_fake_controller_manager/package.xml index 3c7dd57332..fc7d207bc7 100644 --- a/moveit_plugins/moveit_fake_controller_manager/package.xml +++ b/moveit_plugins/moveit_fake_controller_manager/package.xml @@ -1,6 +1,6 @@ moveit_fake_controller_manager - 1.1.0 + 1.1.1 A fake controller manager plugin for MoveIt. Ioan Sucan diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml index f6598bb080..b4c96441b0 100644 --- a/moveit_plugins/moveit_plugins/package.xml +++ b/moveit_plugins/moveit_plugins/package.xml @@ -1,6 +1,6 @@ moveit_plugins - 1.1.0 + 1.1.1 Metapackage for MoveIt plugins. Michael Ferguson diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml index f3d1626a5c..ad88665726 100644 --- a/moveit_plugins/moveit_ros_control_interface/package.xml +++ b/moveit_plugins/moveit_ros_control_interface/package.xml @@ -1,7 +1,7 @@ moveit_ros_control_interface - 1.1.0 + 1.1.1 ros_control controller manager interface for MoveIt Mathias Lüdtke diff --git a/moveit_plugins/moveit_simple_controller_manager/package.xml b/moveit_plugins/moveit_simple_controller_manager/package.xml index af672e7e71..8525ccdf60 100644 --- a/moveit_plugins/moveit_simple_controller_manager/package.xml +++ b/moveit_plugins/moveit_simple_controller_manager/package.xml @@ -1,6 +1,6 @@ moveit_simple_controller_manager - 1.1.0 + 1.1.1 A generic, simple controller manager plugin for MoveIt. Michael Ferguson diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index d5d3ecb890..4ee7757f63 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -1,6 +1,6 @@ moveit_ros_benchmarks - 1.1.0 + 1.1.1 Enhanced tools for benchmarks in MoveIt diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml index 49053da42f..7362ba4306 100644 --- a/moveit_ros/manipulation/package.xml +++ b/moveit_ros/manipulation/package.xml @@ -1,6 +1,6 @@ moveit_ros_manipulation - 1.1.0 + 1.1.1 Components of MoveIt used for manipulation Ioan Sucan diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index 3a70092778..93f84d7ce3 100644 --- a/moveit_ros/move_group/package.xml +++ b/moveit_ros/move_group/package.xml @@ -1,6 +1,6 @@ moveit_ros_move_group - 1.1.0 + 1.1.1 The move_group node for MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index 3a2337f53b..75a0f30e15 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -1,6 +1,6 @@ moveit_ros - 1.1.0 + 1.1.1 Components of MoveIt that use ROS Ioan Sucan Sachin Chitta diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index 954795120c..086a7b60fd 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -1,7 +1,7 @@ moveit_servo - 1.1.0 + 1.1.1 Provides real-time manipulator Cartesian and joint servoing. diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml index f56023495d..595f0cbba6 100644 --- a/moveit_ros/occupancy_map_monitor/package.xml +++ b/moveit_ros/occupancy_map_monitor/package.xml @@ -1,7 +1,7 @@ moveit_ros_occupancy_map_monitor - 1.1.0 + 1.1.1 Components of MoveIt connecting to occupancy map Ioan Sucan diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml index f6e6dc57dd..7ace55ed69 100644 --- a/moveit_ros/perception/package.xml +++ b/moveit_ros/perception/package.xml @@ -1,6 +1,6 @@ moveit_ros_perception - 1.1.0 + 1.1.1 Components of MoveIt connecting to perception Ioan Sucan diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index 1cf2bb163f..9eb394c6f0 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -1,7 +1,7 @@ moveit_ros_planning - 1.1.0 + 1.1.1 Planning components of MoveIt that use ROS Ioan Sucan Sachin Chitta diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index ad49898100..95778baf9f 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -1,7 +1,7 @@ moveit_ros_planning_interface - 1.1.0 + 1.1.1 Components of MoveIt that offer simpler interfaces to planning and execution Ioan Sucan diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml index f18f2e4b1e..e48bdc421d 100644 --- a/moveit_ros/robot_interaction/package.xml +++ b/moveit_ros/robot_interaction/package.xml @@ -1,6 +1,6 @@ moveit_ros_robot_interaction - 1.1.0 + 1.1.1 Components of MoveIt that offer interaction via interactive markers Ioan Sucan diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index 40fcbd843f..cb462642ff 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -1,6 +1,6 @@ moveit_ros_visualization - 1.1.0 + 1.1.1 Components of MoveIt that offer visualization Ioan Sucan diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml index b815fa139a..1d117afe12 100644 --- a/moveit_ros/warehouse/package.xml +++ b/moveit_ros/warehouse/package.xml @@ -1,6 +1,6 @@ moveit_ros_warehouse - 1.1.0 + 1.1.1 Components of MoveIt connecting to MongoDB Ioan Sucan diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml index bc8a465c1e..e13e5882b8 100644 --- a/moveit_runtime/package.xml +++ b/moveit_runtime/package.xml @@ -1,7 +1,7 @@ moveit_runtime - 1.1.0 + 1.1.1 moveit_runtime meta package contains MoveIt packages that are essential for its runtime (e.g. running MoveIt on robots). Isaac I. Y. Saito diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml index 30ea4d9812..4f12137953 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/package.xml @@ -1,6 +1,6 @@ moveit_setup_assistant - 1.1.0 + 1.1.1 Generates a configuration package that makes it easy to use MoveIt From 03c24d092d3e0af1bd1a9ee542162bb6f39f6939 Mon Sep 17 00:00:00 2001 From: Bjar Ne <43565432+gleichdick@users.noreply.github.com> Date: Tue, 13 Oct 2020 22:31:16 +0100 Subject: [PATCH 495/612] Unit Test for ByteString-based ROS msg conversion (#2362) --- .../planning_interface/test/CMakeLists.txt | 12 ++ .../planning_interface/test/serialize_msg.py | 110 ++++++++++++++ .../test/serialize_msg_python_cpp_helpers.cpp | 134 ++++++++++++++++++ 3 files changed, 256 insertions(+) create mode 100644 moveit_ros/planning_interface/test/serialize_msg.py create mode 100644 moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 1a616bdd88..e964d528d2 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -22,4 +22,16 @@ if (CATKIN_ENABLE_TESTING) add_rostest(python_move_group_ns.test) add_rostest(robot_state_update.test) add_rostest(cleanup.test) + + SET(HELPER_LIB moveit_planning_interface_test_serialize_msg_cpp_helper) + add_library(${HELPER_LIB} serialize_msg_python_cpp_helpers.cpp) + set_target_properties(${HELPER_LIB} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + target_link_libraries(${HELPER_LIB} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) + set_target_properties(${HELPER_LIB} PROPERTIES OUTPUT_NAME "_${HELPER_LIB}" PREFIX "") + set_target_properties(${HELPER_LIB} PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") + if(WIN32) + set_target_properties(${HELPER_LIB} PROPERTIES SUFFIX .pyd) + endif(WIN32) + + catkin_add_nosetests(serialize_msg.py) endif() diff --git a/moveit_ros/planning_interface/test/serialize_msg.py b/moveit_ros/planning_interface/test/serialize_msg.py new file mode 100644 index 0000000000..0497d5d9f9 --- /dev/null +++ b/moveit_ros/planning_interface/test/serialize_msg.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, RWTH Aachen University +# 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 RWTH Aachen University 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. +# +# Author: Bjarne von Horn +# + +from moveit_ros_planning_interface._moveit_planning_interface_test_serialize_msg_cpp_helper import ByteStringTestHelper +from geometry_msgs.msg import Vector3 +import unittest + +try: + # Try Python 2.7 behaviour first + from StringIO import StringIO + py_version_maj = 2 +except ImportError: + # Use Python 3.x behaviour as fallback and choose the non-unicode version + from io import BytesIO as StringIO + py_version_maj = 3 + + +class PythonMsgSerializeTest(unittest.TestCase): + def setUp(self): + self.helper = ByteStringTestHelper() + + def test_EmbeddedZeros(self): + self.assertTrue(self.helper.compareEmbeddedZeros(b"\xff\xef\x00\x10")) + + def test_ByteStringFromPchar(self): + # ByteString(const char*) constructor + self.assertEqual(self.helper.getBytesPChar(), b"abcdef") + + def test_ByteStringFromStdString(self): + # ByteString(const std::string&) constructor + self.assertEqual(self.helper.getBytesStdString(), b"\xff\xfe\x10\x00\x00") + + def test_ByteStringDefaultCtor(self): + self.assertEqual(self.helper.getDefaultBytes(), b"") + + def test_CxxTupleToPy(self): + # sending a tuple from C++ to Python + ans = self.helper.getTuple() + self.assertIsInstance(ans, tuple) + self.assertEqual(len(ans), 1) + self.assertEqual(ans[0], b"abcdef") + + def test_PyTupleToCxx(self): + # sending a tuple from Python to C++ + self.assertTrue(self.helper.compareTuple((b"mno",))) + + def test_sendMessage(self): + tmp = StringIO() + Vector3(x=1.0, y=-2.0, z=0.25).serialize(tmp) + self.assertTrue(self.helper.compareVector(tmp.getvalue())) + + def test_recieveMessage(self): + tmp = Vector3() + tmp.deserialize(self.helper.getVector()) + self.assertEqual(tmp, Vector3(1.0, -2.0, 0.25)) + + def test_rejectInt(self): + with self.assertRaisesRegexp(Exception, "Python argument types in"): + self.helper.compareEmbeddedZeros(4711) + + def test_rejectIntTuple(self): + with self.assertRaisesRegexp(Exception, "Python argument types in"): + self.helper.compareEmbeddedZeros((4711,)) + + def test_rejectUnicode(self): + with self.assertRaisesRegexp(Exception, "Python argument types in"): + self.helper.compareEmbeddedZeros(u'kdasd') + + @unittest.skipIf(py_version_maj == 2, "does not trigger with python 2.7") + def test_rejectUnicodeTuple(self): + with self.assertRaisesRegexp(RuntimeError, "Underlying python object is not a Bytes/String instance"): + self.helper.compareVectorTuple((u'kdasd',)) + + +if __name__ == '__main__': + unittest.main() diff --git a/moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp b/moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp new file mode 100644 index 0000000000..13c6e446d6 --- /dev/null +++ b/moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp @@ -0,0 +1,134 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, RWTH Aachen University. + * 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 RWTH Aachen University 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. + *********************************************************************/ + +/* Author: Bjarne von Horn */ + +#include +#include +#include +#include +#include + +namespace bp = boost::python; +using moveit::py_bindings_tools::ByteString; + +// Helper class to be exposed to Python +class ByteStringTestHelper +{ + // Helper to test whether a vector of unsigned chars has the same content as a bytes Object + bool doCompare(const std::vector& data, PyObject* obj) + { + const char* py_data = PyBytes_AsString(obj); + if (!py_data) + return false; + Py_ssize_t size = PyBytes_GET_SIZE(obj); + if (size < 0 || std::vector::size_type(size) != data.size()) + return false; + return std::memcmp(py_data, &data[0], size) == 0; + } + +public: + bool compareEmbeddedZeros(const ByteString& s) + { + const std::vector testdata{ 0xff, 0xef, 0x00, 0x10 }; + return doCompare(testdata, s.ptr()); + } + bool compareTuple(const bp::tuple& t) + { + const ByteString s(t[0]); + const std::vector testdata{ 'm', 'n', 'o' }; + return doCompare(testdata, s.ptr()); + } + + ByteString getBytesPChar() + { + return ByteString("abcdef"); + } + ByteString getBytesStdString() + { + std::string s; + s.push_back('\xff'); + s.push_back('\xfe'); + s.push_back('\x10'); + s.push_back('\x00'); + s.push_back('\x00'); + return ByteString(s); + } + ByteString getDefaultBytes() + { + return ByteString(); + } + bp::tuple getTuple() + { + return bp::make_tuple(ByteString("abcdef")); + } + ByteString getVector() + { + geometry_msgs::Vector3 v; + v.x = 1.0; + v.y = -2.0; + v.z = 0.25; + return ByteString(v); + } + bool compareVector(const ByteString& s) + { + geometry_msgs::Vector3 v; + s.deserialize(v); + return v.x == 1.0 && v.y == -2.0 && v.z == 0.25; + } + bool compareVectorTuple(const bp::tuple& t) + { + const ByteString s(t[0]); + return compareVector(s); + } + + static void setup() + { + bp::class_ cls("ByteStringTestHelper"); + cls.def("compareEmbeddedZeros", &ByteStringTestHelper::compareEmbeddedZeros); + cls.def("compareTuple", &ByteStringTestHelper::compareTuple); + cls.def("compareVectorTuple", &ByteStringTestHelper::compareVectorTuple); + cls.def("getBytesPChar", &ByteStringTestHelper::getBytesPChar); + cls.def("getBytesStdString", &ByteStringTestHelper::getBytesStdString); + cls.def("getDefaultBytes", &ByteStringTestHelper::getDefaultBytes); + cls.def("getTuple", &ByteStringTestHelper::getTuple); + cls.def("getVector", &ByteStringTestHelper::getVector); + cls.def("compareVector", &ByteStringTestHelper::compareVector); + } +}; + +BOOST_PYTHON_MODULE(_moveit_planning_interface_test_serialize_msg_cpp_helper) +{ + ByteStringTestHelper::setup(); +} From 73416fb9fb1b242f5c6af05a628d076425c6ea25 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Wed, 14 Oct 2020 11:21:36 -0400 Subject: [PATCH 496/612] Python interface improvements. Fix #1966, add enforceBounds (#2356) * implement get state and get state bounded * add enforceBounds Co-authored-by: Peter --- .../src/moveit_commander/move_group.py | 21 +++++++++++++- .../test/python_moveit_commander.py | 29 +++++++++++++++++++ .../src/wrap_python_move_group.cpp | 28 ++++++++++++++++++ 3 files changed, 77 insertions(+), 1 deletion(-) diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index e0128e6eb4..df1f267361 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/moveit_commander/src/moveit_commander/move_group.py @@ -156,6 +156,20 @@ def set_start_state(self, msg): """ self._g.set_start_state(conversions.msg_to_string(msg)) + def get_current_state_bounded(self): + """ Get the current state of the robot bounded.""" + s = RobotState() + c_str = self._g.get_current_state_bounded() + conversions.msg_from_string(s, c_str) + return s + + def get_current_state(self): + """ Get the current state of the robot.""" + s = RobotState() + c_str = self._g.get_current_state() + conversions.msg_from_string(s, c_str) + return s + def get_joint_value_target(self): return self._g.get_joint_value_target() @@ -230,7 +244,6 @@ def set_joint_value_target(self, arg1, arg2=None, arg3=None): else: raise MoveItCommanderException("Unsupported argument of type %s" % type(arg1)) - def set_rpy_target(self, rpy, end_effector_link=""): """ Specify a target orientation for the end-effector. Any position of the end-effector is acceptable.""" if len(end_effector_link) > 0 or self.has_end_effector_link(): @@ -610,3 +623,9 @@ def get_jacobian_matrix(self, joint_values, reference_point=None): """ Get the jacobian matrix of the group as a list""" return self._g.get_jacobian_matrix(joint_values, [0.0, 0.0, 0.0] if reference_point is None else reference_point) + def enforce_bounds(self, robot_state_msg): + """ Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds() """ + s = RobotState() + c_str = self._g.enforce_bounds(conversions.msg_to_string(robot_state_msg)) + conversions.msg_from_string(s, c_str) + return s diff --git a/moveit_commander/test/python_moveit_commander.py b/moveit_commander/test/python_moveit_commander.py index 48d01ce524..3c6178562d 100755 --- a/moveit_commander/test/python_moveit_commander.py +++ b/moveit_commander/test/python_moveit_commander.py @@ -35,11 +35,15 @@ # Author: William Baker import unittest + +import genpy import numpy as np import rospy import rostest import os +from moveit_msgs.msg import RobotState + from moveit_commander import RobotCommander, PlanningSceneInterface @@ -55,6 +59,31 @@ def setUpClass(self): def tearDown(self): pass + def test_enforce_bounds_empty_state(self): + in_msg = RobotState() + with self.assertRaises(genpy.DeserializationError): + self.group.enforce_bounds(in_msg) + + def test_enforce_bounds(self): + in_msg = RobotState() + in_msg.joint_state.header.frame_id = 'base_link' + in_msg.joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] + in_msg.joint_state.position = [0] * 6 + in_msg.joint_state.position[0] = 1000 + + out_msg = self.group.enforce_bounds(in_msg) + + self.assertEqual(in_msg.joint_state.position[0], 1000) + self.assertLess(out_msg.joint_state.position[0], 1000) + + def test_get_current_state(self): + expected_state = RobotState() + expected_state.joint_state.header.frame_id = 'base_link' + expected_state.multi_dof_joint_state.header.frame_id = 'base_link' + expected_state.joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] + expected_state.joint_state.position = [0] * 6 + self.assertEqual(self.group.get_current_state(), expected_state) + def check_target_setting(self, expect, *args): if len(args) == 0: args = [expect] diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 628f5880e6..b063ff1e30 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -342,6 +342,14 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return output; } + py_bindings_tools::ByteString getCurrentStatePython() + { + moveit::core::RobotStatePtr current_state = getCurrentState(); + moveit_msgs::RobotState state_message; + moveit::core::robotStateToRobotStateMsg(*current_state, state_message); + return py_bindings_tools::serializeMsg(state_message); + } + void setStartStatePython(const py_bindings_tools::ByteString& msg_str) { moveit_msgs::RobotState msg; @@ -602,6 +610,24 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer state.setJointGroupPositions(group, v); return state.getJacobian(group, Eigen::Map(&ref[0])); } + + py_bindings_tools::ByteString enforceBoundsPython(const py_bindings_tools::ByteString& msg_str) + { + moveit_msgs::RobotState state_msg; + py_bindings_tools::deserializeMsg(msg_str, state_msg); + moveit::core::RobotState state(getRobotModel()); + if (moveit::core::robotStateMsgToRobotState(state_msg, state, true)) + { + state.enforceBounds(); + moveit::core::robotStateToRobotStateMsg(state, state_msg); + return py_bindings_tools::serializeMsg(state_msg); + } + else + { + ROS_ERROR("Unable to convert RobotState message to RobotState instance."); + return py_bindings_tools::ByteString(""); + } + } }; BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getJacobianMatrixOverloads, getJacobianMatrixPython, 1, 2) @@ -742,8 +768,10 @@ static void wrap_move_group_interface() move_group_interface_class.def("get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython); move_group_interface_class.def("get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython); move_group_interface_class.def("get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython); + move_group_interface_class.def("get_current_state", &MoveGroupInterfaceWrapper::getCurrentStatePython); move_group_interface_class.def("get_jacobian_matrix", &MoveGroupInterfaceWrapper::getJacobianMatrixPython, getJacobianMatrixOverloads()); + move_group_interface_class.def("enforce_bounds", &MoveGroupInterfaceWrapper::enforceBoundsPython); } } // namespace planning_interface } // namespace moveit From b2b04b51ec7fd3ccc17bb4c168fbcd8e195d2acb Mon Sep 17 00:00:00 2001 From: Nathan Brooks Date: Thu, 15 Oct 2020 11:12:49 -0600 Subject: [PATCH 497/612] Fix ordering of windup args to control_toolbox::Pid (#2370) --- moveit_ros/moveit_servo/src/pose_tracking.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp index 82ed5b8868..aeee8255fb 100644 --- a/moveit_ros/moveit_servo/src/pose_tracking.cpp +++ b/moveit_ros/moveit_servo/src/pose_tracking.cpp @@ -211,8 +211,8 @@ void PoseTracking::readROSParams() void PoseTracking::initializePID(const PIDConfig& pid_config, std::vector& pid_vector) { bool use_anti_windup = true; - pid_vector.push_back(control_toolbox::Pid(pid_config.k_p, pid_config.k_i, pid_config.k_d, -pid_config.windup_limit, - pid_config.windup_limit, use_anti_windup)); + pid_vector.push_back(control_toolbox::Pid(pid_config.k_p, pid_config.k_i, pid_config.k_d, pid_config.windup_limit, + -pid_config.windup_limit, use_anti_windup)); } bool PoseTracking::haveRecentTargetPose(const double timespan) From e6ca319c1b6f05a990631290208c70f1df5d7ac2 Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Fri, 16 Oct 2020 03:13:40 -0400 Subject: [PATCH 498/612] MSA launch files: fix indentation (#2371) converting tabs to spaces --- moveit_setup_assistant/launch/setup_assistant.launch | 2 +- .../moveit_config_pkg_template/launch/demo.launch | 2 +- .../launch/edit_configuration_package.launch | 4 ++-- .../moveit_config_pkg_template/launch/move_group.launch | 2 +- .../moveit_config_pkg_template/launch/moveit_rviz.launch | 2 +- .../launch/trajopt_planning_pipeline.launch.xml | 8 ++++---- 6 files changed, 10 insertions(+), 10 deletions(-) diff --git a/moveit_setup_assistant/launch/setup_assistant.launch b/moveit_setup_assistant/launch/setup_assistant.launch index 852c19c4ce..104dc655da 100644 --- a/moveit_setup_assistant/launch/setup_assistant.launch +++ b/moveit_setup_assistant/launch/setup_assistant.launch @@ -8,7 +8,7 @@ diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch index 85451710c8..192182a7f0 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch @@ -26,7 +26,7 @@ - [VIRTUAL_JOINT_BROADCASTER] +[VIRTUAL_JOINT_BROADCASTER] diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/edit_configuration_package.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/edit_configuration_package.launch index 04d28dac6e..cce0611246 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/edit_configuration_package.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/edit_configuration_package.launch @@ -8,8 +8,8 @@ diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch index fb1959b048..b41620c229 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch @@ -4,7 +4,7 @@ + value="gdb -x $(find [GENERATED_PACKAGE_NAME])/launch/gdb_settings.gdb --ex run --args" /> diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch index 615b2f12a5..a4605c0374 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch @@ -9,7 +9,7 @@ + args="$(arg command_args)" output="screen"> diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml index e298058e5a..07ebf68e85 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml @@ -6,10 +6,10 @@ + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStateCollision + default_planner_request_adapters/FixStartStatePathConstraints" /> From 144adb7d687cc19c7b79274c93bb0bbdad582d9d Mon Sep 17 00:00:00 2001 From: Jeroen Date: Mon, 19 Oct 2020 00:17:28 +0200 Subject: [PATCH 499/612] Add test for PlanningContextManager in ompl interface (#2248) --- .../ompl/ompl_interface/CMakeLists.txt | 8 + .../ompl_interface/test/load_test_robot.h | 130 +++++++ .../test/test_planning_context_manager.cpp | 358 ++++++++++++++++++ .../test/test_planning_context_manager.test | 4 + moveit_planners/ompl/package.xml | 4 + 5 files changed, 504 insertions(+) create mode 100644 moveit_planners/ompl/ompl_interface/test/load_test_robot.h create mode 100644 moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp create mode 100644 moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index e44e0ecb1b..5945661811 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -48,4 +48,12 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_state_space test/test_state_space.cpp) target_link_libraries(test_state_space ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES}) set_target_properties(test_state_space PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + + find_package(rostest REQUIRED) + find_package(eigen_conversions REQUIRED) + + add_rostest_gtest(test_planning_context_manager + test/test_planning_context_manager.test + test/test_planning_context_manager.cpp) + target_link_libraries(test_planning_context_manager ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) endif() diff --git a/moveit_planners/ompl/ompl_interface/test/load_test_robot.h b/moveit_planners/ompl/ompl_interface/test/load_test_robot.h new file mode 100644 index 0000000000..94c606a14a --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/load_test_robot.h @@ -0,0 +1,130 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * 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 KU Leuven 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. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ + +#pragma once + +#include +#include +#include + +namespace ompl_interface_testing +{ +/** \brief Robot independent test class setup + * + * The desired robot tests can be impelmented in a derived class in a generic way. + * + * It is implemented this way to avoid the ros specific test framework + * outside moveit_ros. + * + * (This is an (uglier) alternative to using the rostest framework + * and reading the robot settings from the parameter server. + * Then we have several rostest launch files that load the parameters + * for a specific robot and run the same compiled tests for all robots.) + * + * based on + * https://stackoverflow.com/questions/38207346/specify-constructor-arguments-for-a-google-test-fixture/38218657 + * (answer by PiotrNycz) + * + * --- example.cpp --- + * + * #include + * #include "load_test_robot.h" + * + * class GenericTests : public ompl_interface_testing::LoadTestRobot, public testing::Test + * { + * public: + * GenericTests(const std::string& robot_name, const std::string& group_name) + * : LoadTestRobot(robot_name, group_name) { } + * void SetUp() override { } + * void TearDown() override { } + * + * void someTest() + * { + * // use robot_model_, robot_state_, .. here + * EXPECT_TRUE(true); + * } + * }; + * + * // now you can run `someTest()` on different robots: + * + * class PandaTest : public GenericTests + * { + * protected: + * PandaTest() : GenericTests("panda", "panda_arm") { } + * }; + * + * TEST_F(PandaTest, someTest) + * { + * someTest(); + * } + * + * --- end example.cpp --- + * + * + * */ +class LoadTestRobot +{ +protected: + LoadTestRobot(const std::string& robot_name, const std::string& group_name) + : group_name_(group_name), robot_name_(robot_name) + { + // load robot + robot_model_ = moveit::core::loadTestingRobotModel(robot_name_); + robot_state_ = std::make_shared(robot_model_); + robot_state_->setToDefaultValues(); + joint_model_group_ = robot_state_->getJointModelGroup(group_name_); + + // extract useful parameters for tests + num_dofs_ = joint_model_group_->getVariableCount(); + ee_link_name_ = joint_model_group_->getLinkModelNames().back(); + base_link_name_ = robot_model_->getRootLinkName(); + + ROS_INFO_STREAM("Created test robot named: " << robot_name_ << " for planning group " << group_name_); + } + +protected: + const std::string group_name_; + const std::string robot_name_; + + moveit::core::RobotModelPtr robot_model_; + robot_state::RobotStatePtr robot_state_; + const robot_state::JointModelGroup* joint_model_group_; + + std::size_t num_dofs_; + std::string base_link_name_; + std::string ee_link_name_; +}; +} // namespace ompl_interface_testing diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp new file mode 100644 index 0000000000..9bdd7c303d --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp @@ -0,0 +1,358 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * 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 KU Leuven 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. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ + +/** Test the creation of a ModelBasedPlanningContext through the PlanningContextManager. + * + * The tests use a default robot state as start state, and then moves the last joint 0.1 radians/meters to create a + * joint goal. This creates an extremely simple planning problem to test general mechanics of the interface. + * + * In the test with path constraints, we create an orientation constraint around the start orientation of the + * end-effector, with a tolerance on the rotation larger than the change in the last joint of the goal state. + * This makes sure the path constraints are easy to satisfy. + * + * TODO(jeroendm) I also tried something similar with position constraints, but get a segmentation fault + * that occurs in the 'geometric_shapes' package, in the method 'useDimensions' in 'bodies.h'. + * git permalink: + * https://github.com/ros-planning/geometric_shapes/blob/df0478870b8592ce789ee1919f3124058c4327d7/include/geometric_shapes/bodies.h#L196 + * + **/ + +#include "load_test_robot.h" + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include + +/** \brief Generic implementation of the tests that can be executed on different robots. **/ +class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public testing::Test +{ +public: + TestPlanningContext(const std::string& robot_name, const std::string& group_name) + : LoadTestRobot(robot_name, group_name) + { + } + + // /*************************************************************************** + // * START Test implementations + // * ************************************************************************/ + + void testSimpleRequest(const std::vector& start, const std::vector& goal) + { + // create all the test specific input necessary to make the getPlanningContext call possible + planning_interface::PlannerConfigurationSettings pconfig_settings; + pconfig_settings.group = group_name_; + pconfig_settings.name = group_name_; + pconfig_settings.config = { { "enforce_joint_model_state_space", "0" } }; + + planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } }; + moveit_msgs::MoveItErrorCodes error_code; + planning_interface::MotionPlanRequest request = createRequest(start, goal); + + // setup the planning context manager + ompl_interface::PlanningContextManager pcm(robot_model_, constraint_sampler_manager_); + pcm.setPlannerConfigurations(pconfig_map); + + // see if it returns the expected planning context + auto pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_handle_, false); + + // the planning context should have a simple setup created + EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); + + // the OMPL state space in the planning context should be of type JointModelStateSpace + EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); + + // there did not magically appear path constraints in the planning context + EXPECT_TRUE(pc->getPathConstraints()->empty()); + + // solve the planning problem + planning_interface::MotionPlanDetailedResponse res; + ASSERT_TRUE(pc->solve(res)); + } + + void testPathConstraints(const std::vector& start, const std::vector& goal) + { + // create all the test specific input necessary to make the getPlanningContext call possible + planning_interface::PlannerConfigurationSettings pconfig_settings; + pconfig_settings.group = group_name_; + pconfig_settings.name = group_name_; + pconfig_settings.config = { { "enforce_joint_model_state_space", "0" } }; + + planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } }; + moveit_msgs::MoveItErrorCodes error_code; + planning_interface::MotionPlanRequest request = createRequest(start, goal); + + // give it some more time, as rejection sampling of the path constraints occasionally takes some time + request.allowed_planning_time = 10.0; + + // create path constraints around start state, to make sure they are satisfied + robot_state_->setJointGroupPositions(joint_model_group_, start); + Eigen::Isometry3d ee_pose = robot_state_->getGlobalLinkTransform(ee_link_name_); + geometry_msgs::Quaternion ee_orientation; + tf::quaternionEigenToMsg(Eigen::Quaterniond(ee_pose.rotation()), ee_orientation); + + // setup the planning context manager + ompl_interface::PlanningContextManager pcm(robot_model_, constraint_sampler_manager_); + pcm.setPlannerConfigurations(pconfig_map); + + // ORIENTATION CONSTRAINTS + // *********************** + request.path_constraints.orientation_constraints.push_back(createOrientationConstraint(ee_orientation)); + + // See if the planning context manager returns the expected planning context + auto pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_handle_, false); + + EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); + + // As the joint_model_group_ has no IK solver initialized, we expect a joint model state space + EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); + + planning_interface::MotionPlanDetailedResponse response; + ASSERT_TRUE(pc->solve(response)); + + // Are the path constraints created in the planning context? + auto path_constraints = pc->getPathConstraints(); + EXPECT_FALSE(path_constraints->empty()); + EXPECT_EQ(path_constraints->getOrientationConstraints().size(), 1u); + EXPECT_TRUE(path_constraints->getPositionConstraints().empty()); + EXPECT_TRUE(path_constraints->getJointConstraints().empty()); + EXPECT_TRUE(path_constraints->getVisibilityConstraints().empty()); + + // Check if all the states in the solution satisfy the path constraints. + // A detailed response returns 3 solutions: the ompl solution, the simplified solution and the interpolated + // solution. We test all of them here. + for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response.trajectory_) + { + for (std::size_t pt_index = { 0 }; pt_index < trajectory->getWayPointCount(); ++pt_index) + { + EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied); + } + } + + // POSITION CONSTRAINTS + // *********************** + request.path_constraints.orientation_constraints.clear(); + request.path_constraints.position_constraints.push_back(createPositionConstraint( + { ee_pose.translation().x(), ee_pose.translation().y(), ee_pose.translation().z() }, { 0.1, 0.1, 0.1 })); + + // See if the planning context manager returns the expected planning context + pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_handle_, false); + + EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); + + // As the joint_model_group_ has no IK solver initialized, we expect a joint model state space + EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); + + // Create a new response, because the solve method does not clear the given respone + planning_interface::MotionPlanDetailedResponse response2; + ASSERT_TRUE(pc->solve(response2)); + + // Are the path constraints created in the planning context? + path_constraints = pc->getPathConstraints(); + EXPECT_FALSE(path_constraints->empty()); + EXPECT_EQ(path_constraints->getPositionConstraints().size(), 1u); + EXPECT_TRUE(path_constraints->getOrientationConstraints().empty()); + + // Check if all the states in the solution satisfy the path constraints. + // A detailed response returns 3 solutions: the ompl solution, the simplified solution and the interpolated + // solution. We test all of them here. + for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response2.trajectory_) + { + for (std::size_t pt_index = { 0 }; pt_index < trajectory->getWayPointCount(); ++pt_index) + { + EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied); + } + } + } + + // /*************************************************************************** + // * END Test implementation + // * ************************************************************************/ + +protected: + void SetUp() override + { + // create all the fixed input necessary for all planning context managers + constraint_sampler_manager_ = std::make_shared(); + planning_scene_ = std::make_shared(robot_model_); + } + + void TearDown() override + { + } + + /** Create a planning request to plan from a given start state to a joint space goal. **/ + planning_interface::MotionPlanRequest createRequest(const std::vector& start, + const std::vector& goal) const + { + planning_interface::MotionPlanRequest request; + request.group_name = group_name_; + request.allowed_planning_time = 5.0; + + // fill out start state in request + robot_state::RobotState start_state(robot_model_); + start_state.setToDefaultValues(); + start_state.setJointGroupPositions(joint_model_group_, start); + moveit::core::robotStateToRobotStateMsg(start_state, request.start_state); + + // fill out goal state in request + robot_state::RobotState goal_state(robot_model_); + goal_state.setToDefaultValues(); + goal_state.setJointGroupPositions(joint_model_group_, goal); + moveit_msgs::Constraints joint_goal = + kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_, 0.001); + request.goal_constraints.push_back(joint_goal); + + return request; + } + + /** \brief Helper function to create a position constraint. **/ + moveit_msgs::PositionConstraint createPositionConstraint(std::array position, + std::array dimensions) + { + shape_msgs::SolidPrimitive box; + box.type = shape_msgs::SolidPrimitive::BOX; + box.dimensions.resize(3); + box.dimensions[shape_msgs::SolidPrimitive::BOX_X] = dimensions[0]; + box.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = dimensions[1]; + box.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = dimensions[2]; + + geometry_msgs::Pose box_pose; + box_pose.position.x = position[0]; + box_pose.position.y = position[1]; + box_pose.position.z = position[2]; + box_pose.orientation.w = 1.0; + + moveit_msgs::PositionConstraint pc; + pc.header.frame_id = base_link_name_; + pc.link_name = ee_link_name_; + pc.constraint_region.primitives.push_back(box); + pc.constraint_region.primitive_poses.push_back(box_pose); + + return pc; + } + + /** \brief Helper function to create a orientation constraint. **/ + moveit_msgs::OrientationConstraint createOrientationConstraint(const geometry_msgs::Quaternion& nominal_orientation) + { + moveit_msgs::OrientationConstraint oc; + oc.header.frame_id = base_link_name_; + oc.link_name = ee_link_name_; + oc.orientation = nominal_orientation; + oc.absolute_x_axis_tolerance = 0.3; + oc.absolute_y_axis_tolerance = 0.3; + oc.absolute_z_axis_tolerance = 0.3; + + return oc; + } + + ompl_interface::ModelBasedStateSpacePtr state_space_; + ompl_interface::ModelBasedPlanningContextSpecification planning_context_spec_; + ompl_interface::ModelBasedPlanningContextPtr planning_context_; + planning_scene::PlanningScenePtr planning_scene_; + + constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; + + /** Ideally we add an IK plugin to the joint_model_group_ to test the PoseModel state space, using the pluginlib to + * load the default KDL plugin? **/ + // std::shared_ptr ik_plugin_; + + /** we need a node handle to call getPlanningRequest, but it is never used, as we disable the + * 'use_constraints_approximation' option. **/ + ros::NodeHandle node_handle_; +}; + +/*************************************************************************** + * Run all tests on the Panda robot + * ************************************************************************/ +class PandaTestPlanningContext : public TestPlanningContext +{ +protected: + PandaTestPlanningContext() : TestPlanningContext("panda", "panda_arm") + { + } +}; + +TEST_F(PandaTestPlanningContext, testSimpleRequest) +{ + // use the panda "ready" state from the srdf config as start state + // we know this state should be within limits and self-collision free + testSimpleRequest({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }, { 0, -0.785, 0, -2.356, 0, 1.571, 0.685 }); +} + +TEST_F(PandaTestPlanningContext, testPathConstraints) +{ + testPathConstraints({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }, { 0, -0.785, 0, -2.356, 0, 1.571, 0.685 }); +} + +/*************************************************************************** + * Run all tests on the Fanuc robot + * ************************************************************************/ +class FanucTestPlanningContext : public TestPlanningContext +{ +protected: + FanucTestPlanningContext() : TestPlanningContext("fanuc", "manipulator") + { + } +}; + +TEST_F(FanucTestPlanningContext, testSimpleRequest) +{ + testSimpleRequest({ 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0.1 }); +} + +TEST_F(FanucTestPlanningContext, testPathConstraints) +{ + testPathConstraints({ 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0.1 }); +} + +/*************************************************************************** + * MAIN + * ************************************************************************/ +int main(int argc, char** argv) +{ + ros::init(argc, argv, "planning_context_manager_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test new file mode 100644 index 0000000000..64a3e6c087 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index 877590134e..379fdf337a 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -26,7 +26,11 @@ pluginlib moveit_resources_pr2_description + moveit_resources_fanuc_description + moveit_resources_panda_description rosunit + rostest + eigen_conversions From 4dc9ceeb54935549f992402ac1877b7947600759 Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Wed, 21 Oct 2020 02:05:52 -0400 Subject: [PATCH 500/612] Update .github files (#2380) --- .github/CODEOWNERS | 40 ++++++++++++++-------------- .github/ISSUE_TEMPLATE/bug_report.md | 2 +- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 3ccad28aee..b21c6f4486 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -32,10 +32,10 @@ # FOLDER-SPECIFIC REVIEWERS: -/moveit_plugins/moveit_ros_control_interface/ @ipa-mdl @bmagyar -/moveit_plugins/moveit_fake_controller_manager/ @v4hn @rhaschke -/moveit_plugins/moveit_simple_controller_manager/ @mikeferguson @v4hn -/moveit_plugins/moveit_controller_manager_example/ @v4hn +/moveit_plugins/moveit_ros_control_interface/ @ipa-mdl @bmagyar +/moveit_plugins/moveit_fake_controller_manager/ @v4hn @rhaschke +/moveit_plugins/moveit_simple_controller_manager/ @mikeferguson @v4hn +/moveit_plugins/moveit_controller_manager_example/ @v4hn /moveit_core/background_processing/ @tylerjw /moveit_core/backtrace/ @tylerjw @@ -52,7 +52,7 @@ /moveit_core/kinematics_metrics/ @gavanderhoorn /moveit_core/macros/ @tylerjw /moveit_core/planning_interface/ @rhaschke @felixvd -/moveit_core/planning_request_adapter/ @rhaschke +/moveit_core/planning_request_adapter/ @rhaschke /moveit_core/planning_scene/ @rhaschke @felixvd /moveit_core/profiler/ @tylerjw /moveit_core/robot_model/ @tylerjw @@ -66,20 +66,20 @@ /moveit_commander/ @rhaschke @willcbaker -/moveit/ @130s +/moveit/ @130s -/moveit_kinematics/ @rhaschke @gavanderhoorn @jrgnicho +/moveit_kinematics/ @rhaschke @gavanderhoorn @jrgnicho -/moveit_experimental/ @AndyZe +/moveit_experimental/ @AndyZe -/moveit_ros/perception/ @jliukkonen @RoboticsYY -/moveit_ros/manipulation/ @v4hn @felixvd -/moveit_ros/benchmarks/ @henningkayser @MohmadAyman -/moveit_ros/planning_interface/ @mintar @rhaschke @felixvd -/moveit_ros/robot_interaction/ @mikeferguson @rhaschke -/moveit_ros/warehouse/ @mikeferguson @dg-shadow -/moveit_ros/move_group/ @rhaschke @IanTheEngineer -/moveit_ros/visualization/ @rhaschke @tylerjw @RoboticsYY @felixvd +/moveit_ros/perception/ @jliukkonen @RoboticsYY +/moveit_ros/manipulation/ @v4hn @felixvd +/moveit_ros/benchmarks/ @henningkayser @MohmadAyman +/moveit_ros/planning_interface/ @mintar @rhaschke @felixvd +/moveit_ros/robot_interaction/ @mikeferguson @rhaschke +/moveit_ros/warehouse/ @mikeferguson @dg-shadow +/moveit_ros/move_group/ @rhaschke @IanTheEngineer +/moveit_ros/visualization/ @rhaschke @tylerjw @RoboticsYY @felixvd /moveit_ros/planning/collision_plugin_loader/ @j-petit /moveit_ros/planning/constraint_sampler_manager_loader/ @nbbrooks @@ -93,10 +93,10 @@ /moveit_ros/planning/robot_model_loader/ @nbbrooks /moveit_ros/planning/trajectory_execution_manager/ @rhaschke -/moveit_setup_assistant/ @RoboticsYY @rhaschke @MohmadAyman +/moveit_setup_assistant/ @RoboticsYY @rhaschke @MohmadAyman -/moveit_planners/ompl/ @BryceStevenWilley @mamoll +/moveit_planners/ompl/ @BryceStevenWilley @mamoll /moveit_planners/chomp/chomp_interface/ @raghavendersahdev @knorth55 @bmagyar -/moveit_planners/chomp/chomp_optimizer_adapter/ @raghavendersahdev @knorth55 @bmagyar -/moveit_planners/chomp/chomp_motion_planner/ @raghavendersahdev @knorth55 @bmagyar +/moveit_planners/chomp/chomp_optimizer_adapter/ @raghavendersahdev @knorth55 @bmagyar +/moveit_planners/chomp/chomp_motion_planner/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/trajopt @ommmid @mamoll diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index c7c77614ea..6806c4801e 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -12,7 +12,7 @@ assignees: '' Overview of your issue here. ### Your environment -* ROS Distro: [Kinetic|Melodic] +* ROS Distro: [Kinetic|Melodic|Noetic] * OS Version: e.g. Ubuntu 18.04 * Source or Binary build? * If binary, which release version? From 181f235572378330fab00b8f1d087c14940ab8dd Mon Sep 17 00:00:00 2001 From: Jere Liukkonen Date: Wed, 21 Oct 2020 12:58:50 -0600 Subject: [PATCH 501/612] Fix servo trajectory point timestamping (#2375) --- moveit_ros/moveit_servo/src/pose_tracking.cpp | 7 ++----- moveit_ros/moveit_servo/src/servo_calcs.cpp | 19 ++++++++++++++++--- 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp index aeee8255fb..bd45fbf91f 100644 --- a/moveit_ros/moveit_servo/src/pose_tracking.cpp +++ b/moveit_ros/moveit_servo/src/pose_tracking.cpp @@ -132,11 +132,8 @@ PoseTrackingStatusCode PoseTracking::moveToPose(const Eigen::Vector3d& positiona return PoseTrackingStatusCode::STOP_REQUESTED; } - // Compute servo command from PID controller output - auto msg = calculateTwistCommand(); - - // Send command to the Servo object, for execution - twist_stamped_pub_.publish(*msg); + // Compute servo command from PID controller output and send it to the Servo object, for execution + twist_stamped_pub_.publish(calculateTwistCommand()); } doPostMotionReset(); diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index f7cbfc48c3..aca414e649 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -169,8 +169,10 @@ void ServoCalcs::start() // We will update last_sent_command_ every time we start servo auto initial_joint_trajectory = moveit::util::make_shared_from_pool(); + // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately" + // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement + initial_joint_trajectory->header.stamp = ros::Time(0); initial_joint_trajectory->header.frame_id = parameters_.planning_frame; - initial_joint_trajectory->header.stamp = ros::Time::now(); initial_joint_trajectory->joint_names = internal_joint_state_.name; trajectory_msgs::JointTrajectoryPoint point; point.time_from_start = ros::Duration(parameters_.publish_period); @@ -361,7 +363,9 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") { - joint_trajectory->header.stamp = ros::Time::now(); + // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately" + // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement + joint_trajectory->header.stamp = ros::Time(0); outgoing_cmd_pub_.publish(joint_trajectory); } else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") @@ -594,8 +598,10 @@ void ServoCalcs::calculateJointVelocities(sensor_msgs::JointState& joint_state, void ServoCalcs::composeJointTrajMessage(const sensor_msgs::JointState& joint_state, trajectory_msgs::JointTrajectory& joint_trajectory) const { + // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately" + // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement + joint_trajectory.header.stamp = ros::Time(0); joint_trajectory.header.frame_id = parameters_.planning_frame; - joint_trajectory.header.stamp = ros::Time::now(); joint_trajectory.joint_names = joint_state.name; trajectory_msgs::JointTrajectoryPoint point; @@ -810,6 +816,13 @@ void ServoCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory) joint_trajectory.points.clear(); joint_trajectory.points.emplace_back(); trajectory_msgs::JointTrajectoryPoint& point = joint_trajectory.points.front(); + + // When sending out trajectory_msgs/JointTrajectory type messages, the "trajectory" is just a single point. + // That point cannot have the same timestamp as the start of trajectory execution since that would mean the + // arm has to reach the first trajectory point the moment execution begins. To prevent errors about points + // being 0 seconds in the past, the smallest supported timestep is added as time from start to the trajectory point. + point.time_from_start.fromNSec(1); + point.positions.resize(num_joints_); point.velocities.resize(num_joints_); From 1ebdb2d75e760910eeada4c2a0ad7520f4dae571 Mon Sep 17 00:00:00 2001 From: Jere Liukkonen Date: Thu, 22 Oct 2020 11:20:38 -0600 Subject: [PATCH 502/612] Change servo namespacing logic (#2354) * Publish services correctly in the namespace of the node. * Set relative joint state topic names be relative to the parent namespace of the servo node. * Make parameter sub-namespace optional. By default load the parameters into node namespace. * Fix node handles for the servo server node and the cpp interface example to use node namespace instead of the parent namespace. * Fix formatting * Reading ee_frame_name parameter got lost somewhere along the way. Add it back. * Remove explicit parameter_ns variables and used namespaced node handles instead. * Fix internal namespace to truly be internal even in case where two or more Servo instances are launched from a single node. * Fix command_out_topic parameter in ur_simulated_config.yaml example config. * Apply namespacing fixes also to tests. * Shorten the comment about joint states topic names. --- .../config/ur_simulated_config.yaml | 10 +- .../include/moveit_servo/pose_tracking.h | 6 +- .../moveit_servo/include/moveit_servo/servo.h | 6 +- .../include/moveit_servo/servo_calcs.h | 2 +- .../launch/cpp_interface_example.launch | 4 +- .../moveit_servo/launch/spacenav_cpp.launch | 4 +- .../launch/spacenav_teleop_tools.launch | 4 +- .../moveit_servo/src/collision_check.cpp | 2 +- .../cpp_interface_example.cpp | 2 +- .../pose_tracking_example.cpp | 8 +- moveit_ros/moveit_servo/src/pose_tracking.cpp | 72 ++++++------ moveit_ros/moveit_servo/src/servo.cpp | 110 ++++++++---------- moveit_ros/moveit_servo/src/servo_calcs.cpp | 16 +-- moveit_ros/moveit_servo/src/servo_server.cpp | 2 +- .../moveit_servo/test/pose_tracking_test.cpp | 7 +- .../moveit_servo/test/pose_tracking_test.test | 2 +- .../test/servo_cpp_interface_test.cpp | 2 +- .../test/servo_cpp_interface_test.test | 4 +- 18 files changed, 121 insertions(+), 142 deletions(-) diff --git a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml index d2ea184f40..59b6599e5d 100644 --- a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml @@ -46,11 +46,11 @@ hard_stop_singularity_threshold: 30 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names -cartesian_command_in_topic: servo_server/delta_twist_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: servo_server/delta_joint_cmds # Topic for incoming joint angle commands -joint_topic: joint_states -status_topic: servo_server/status # Publish status to this topic -command_out_topic: joint_group_position_controller/command # Publish outgoing commands here +cartesian_command_in_topic: delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: joint_states +status_topic: status # Publish status to this topic +command_out_topic: /joint_group_position_controller/command # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? diff --git a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h index bd7c24fbce..d76c58481b 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h @@ -85,8 +85,7 @@ class PoseTracking { public: /** \brief Constructor. Loads ROS parameters under the given namespace. */ - PoseTracking(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::string& parameter_ns = ""); + PoseTracking(const ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); PoseTrackingStatusCode moveToPose(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance, const double target_pose_timeout); @@ -184,9 +183,6 @@ class PoseTracking // Flag that a different thread has requested a stop. std::atomic stop_requested_; - // Read parameters from this namespace - std::string parameter_ns_; - double angular_error_; }; diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h index b520d807d6..6205e09c97 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -53,8 +53,7 @@ namespace moveit_servo class Servo { public: - Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::string& parameter_ns = ""); + Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); ~Servo(); @@ -112,9 +111,6 @@ class Servo std::shared_ptr joint_state_subscriber_; std::unique_ptr servo_calcs_; std::unique_ptr collision_checker_; - - // Read parameters from this namespace - std::string parameter_ns_; }; // ServoPtr using alias diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 26f7c4c947..37e7e647db 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -66,7 +66,7 @@ class ServoCalcs public: ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& joint_state_subscriber, std::string& ros_namespace); + const std::shared_ptr& joint_state_subscriber); ~ServoCalcs() { diff --git a/moveit_ros/moveit_servo/launch/cpp_interface_example.launch b/moveit_ros/moveit_servo/launch/cpp_interface_example.launch index c2ac9b8748..7498446ff8 100644 --- a/moveit_ros/moveit_servo/launch/cpp_interface_example.launch +++ b/moveit_ros/moveit_servo/launch/cpp_interface_example.launch @@ -2,8 +2,8 @@ - - + + diff --git a/moveit_ros/moveit_servo/launch/spacenav_cpp.launch b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch index 41d81862db..b60db1fa0e 100644 --- a/moveit_ros/moveit_servo/launch/spacenav_cpp.launch +++ b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch @@ -13,8 +13,8 @@ - - + + diff --git a/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch b/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch index 93847e35a1..e3834b5e16 100644 --- a/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch +++ b/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch @@ -10,8 +10,8 @@ - - + + diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp index ec047c9953..73c42dbdcd 100644 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ b/moveit_ros/moveit_servo/src/collision_check.cpp @@ -75,7 +75,7 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoPar safety_factor_ = parameters_.collision_distance_safety_factor; // Internal namespace - ros::NodeHandle internal_nh("~internal"); + ros::NodeHandle internal_nh(nh_, "internal"); collision_velocity_scale_pub_ = internal_nh.advertise("collision_velocity_scale", ROS_QUEUE_SIZE); worst_case_stop_time_sub_ = internal_nh.subscribe("worst_case_stop_time", ROS_QUEUE_SIZE, &CollisionCheck::worstCaseStopTimeCB, this); diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp index c1170d10a9..d51ce11b19 100644 --- a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp @@ -75,7 +75,7 @@ class StatusMonitor int main(int argc, char** argv) { ros::init(argc, argv, LOGNAME); - ros::NodeHandle nh; + ros::NodeHandle nh("~"); ros::AsyncSpinner spinner(8); spinner.start(); diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp index 95d3a0aa4e..b5ec8d176f 100644 --- a/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp @@ -79,7 +79,7 @@ class StatusMonitor int main(int argc, char** argv) { ros::init(argc, argv, LOGNAME); - ros::NodeHandle nh; + ros::NodeHandle nh("~"); ros::AsyncSpinner spinner(8); spinner.start(); @@ -100,14 +100,14 @@ int main(int argc, char** argv) planning_scene_monitor->startStateMonitor(); // Create the pose tracker - moveit_servo::PoseTracking tracker(planning_scene_monitor, "servo_server"); + moveit_servo::PoseTracking tracker(nh, planning_scene_monitor); // Make a publisher for sending pose commands ros::Publisher target_pose_pub = - nh.advertise("/servo_server/target_pose", 1 /* queue */, true /* latch */); + nh.advertise("target_pose", 1 /* queue */, true /* latch */); // Subscribe to servo status (and log it when it changes) - StatusMonitor status_monitor(nh, "servo_server/status"); + StatusMonitor status_monitor(nh, "status"); Eigen::Vector3d lin_tol{ 0.01, 0.01, 0.01 }; double rot_tol = 0.1; diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp index bd45fbf91f..065e547f8e 100644 --- a/moveit_ros/moveit_servo/src/pose_tracking.cpp +++ b/moveit_ros/moveit_servo/src/pose_tracking.cpp @@ -43,13 +43,13 @@ constexpr double ROS_STARTUP_WAIT = 10; // sec namespace moveit_servo { -PoseTracking::PoseTracking(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::string& parameter_ns) - : planning_scene_monitor_(planning_scene_monitor) +PoseTracking::PoseTracking(const ros::NodeHandle& nh, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : nh_(nh) + , planning_scene_monitor_(planning_scene_monitor) , loop_rate_(DEFAULT_LOOP_RATE) , transform_listener_(transform_buffer_) , stop_requested_(false) - , parameter_ns_(parameter_ns) , angular_error_(0) { readROSParams(); @@ -64,13 +64,12 @@ PoseTracking::PoseTracking(const planning_scene_monitor::PlanningSceneMonitorPtr initializePID(angular_pid_config_, cartesian_orientation_pids_); // Use the C++ interface that Servo provides - servo_ = std::make_unique(nh_, planning_scene_monitor_, parameter_ns_); + servo_ = std::make_unique(nh_, planning_scene_monitor_); servo_->start(); // Connect to Servo ROS interfaces - std::string target_pose_topic = "/" + parameter_ns_ + "/target_pose"; target_pose_sub_ = - nh_.subscribe(target_pose_topic, 1, &PoseTracking::targetPoseCallback, this); + nh_.subscribe("target_pose", 1, &PoseTracking::targetPoseCallback, this); // Publish outgoing twist commands to the Servo object twist_stamped_pub_ = @@ -142,30 +141,27 @@ PoseTrackingStatusCode PoseTracking::moveToPose(const Eigen::Vector3d& positiona void PoseTracking::readROSParams() { - std::size_t error = 0; - - // Check for parameter namespace from launch file. All other parameters will be read from this namespace. - std::string yaml_namespace; - if (ros::param::get("~parameter_ns", yaml_namespace)) - { - if (!parameter_ns_.empty()) - ROS_WARN_STREAM_NAMED(LOGNAME, - "A parameter namespace was specified in the launch file AND in the constructor argument."); + // Optional parameter sub-namespace specified in the launch file. All other parameters will be read from this namespace. + std::string parameter_ns; + ros::param::get("~parameter_ns", parameter_ns); - parameter_ns_ = yaml_namespace; - } + // If parameters have been loaded into sub-namespace within the node namespace, append the parameter namespace + // to load the parameters correctly. + ros::NodeHandle nh = parameter_ns.empty() ? nh_ : ros::NodeHandle(nh_, parameter_ns); // Wait for ROS parameters to load ros::Time begin = ros::Time::now(); - while (ros::ok() && !ros::param::has(parameter_ns_ + "/planning_frame") && - ((ros::Time::now() - begin).toSec() < ROS_STARTUP_WAIT)) + while (ros::ok() && !nh.hasParam("planning_frame") && ((ros::Time::now() - begin).toSec() < ROS_STARTUP_WAIT)) { - ROS_WARN_STREAM_NAMED(LOGNAME, "Waiting for parameter: " << parameter_ns_ + "/planning_frame"); + ROS_WARN_STREAM_NAMED(LOGNAME, "Waiting for parameter: " + << "planning_frame"); ros::Duration(0.1).sleep(); } - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/planning_frame", planning_frame_); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/move_group_name", move_group_name_); + std::size_t error = 0; + + error += !rosparam_shortcuts::get(LOGNAME, nh, "planning_frame", planning_frame_); + error += !rosparam_shortcuts::get(LOGNAME, nh, "move_group_name", move_group_name_); if (!planning_scene_monitor_->getRobotModel()->hasJointModelGroup(move_group_name_)) { ++error; @@ -173,7 +169,7 @@ void PoseTracking::readROSParams() } double publish_period; - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/publish_period", publish_period); + error += !rosparam_shortcuts::get(LOGNAME, nh, "publish_period", publish_period); loop_rate_ = ros::Rate(1 / publish_period); x_pid_config_.dt = publish_period; @@ -182,25 +178,25 @@ void PoseTracking::readROSParams() angular_pid_config_.dt = publish_period; double windup_limit; - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/windup_limit", windup_limit); + error += !rosparam_shortcuts::get(LOGNAME, nh, "windup_limit", windup_limit); x_pid_config_.windup_limit = windup_limit; y_pid_config_.windup_limit = windup_limit; z_pid_config_.windup_limit = windup_limit; angular_pid_config_.windup_limit = windup_limit; - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/x_proportional_gain", x_pid_config_.k_p); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/y_proportional_gain", y_pid_config_.k_p); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/z_proportional_gain", z_pid_config_.k_p); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/x_integral_gain", x_pid_config_.k_i); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/y_integral_gain", y_pid_config_.k_i); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/z_integral_gain", z_pid_config_.k_i); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/x_derivative_gain", x_pid_config_.k_d); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/y_derivative_gain", y_pid_config_.k_d); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/z_derivative_gain", z_pid_config_.k_d); - - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/angular_proportional_gain", angular_pid_config_.k_p); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/angular_integral_gain", angular_pid_config_.k_i); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/angular_derivative_gain", angular_pid_config_.k_d); + error += !rosparam_shortcuts::get(LOGNAME, nh, "x_proportional_gain", x_pid_config_.k_p); + error += !rosparam_shortcuts::get(LOGNAME, nh, "y_proportional_gain", y_pid_config_.k_p); + error += !rosparam_shortcuts::get(LOGNAME, nh, "z_proportional_gain", z_pid_config_.k_p); + error += !rosparam_shortcuts::get(LOGNAME, nh, "x_integral_gain", x_pid_config_.k_i); + error += !rosparam_shortcuts::get(LOGNAME, nh, "y_integral_gain", y_pid_config_.k_i); + error += !rosparam_shortcuts::get(LOGNAME, nh, "z_integral_gain", z_pid_config_.k_i); + error += !rosparam_shortcuts::get(LOGNAME, nh, "x_derivative_gain", x_pid_config_.k_d); + error += !rosparam_shortcuts::get(LOGNAME, nh, "y_derivative_gain", y_pid_config_.k_d); + error += !rosparam_shortcuts::get(LOGNAME, nh, "z_derivative_gain", z_pid_config_.k_d); + + error += !rosparam_shortcuts::get(LOGNAME, nh, "angular_proportional_gain", angular_pid_config_.k_p); + error += !rosparam_shortcuts::get(LOGNAME, nh, "angular_integral_gain", angular_pid_config_.k_i); + error += !rosparam_shortcuts::get(LOGNAME, nh, "angular_derivative_gain", angular_pid_config_.k_d); rosparam_shortcuts::shutdownIfError(ros::this_node::getName(), error); } diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index f278d18f67..8467837429 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -45,18 +45,19 @@ static const std::string LOGNAME = "servo_node"; namespace moveit_servo { -Servo::Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::string& parameter_ns) - : nh_(nh), planning_scene_monitor_(planning_scene_monitor), parameter_ns_(parameter_ns) +Servo::Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : nh_(nh), planning_scene_monitor_(planning_scene_monitor) { // Read ROS parameters, typically from YAML file if (!readParameters()) exit(EXIT_FAILURE); - joint_state_subscriber_ = std::make_shared(nh_, parameters_.joint_topic); + // By default, joint topic name is relative to the node parent namespace. Fully custom joint name topics can be + // set by using absolute topic names in config files. For example, "/foo/my_joint_state_topic". + ros::NodeHandle nh_parent_ns = ros::NodeHandle(""); + joint_state_subscriber_ = std::make_shared(nh_parent_ns, parameters_.joint_topic); - servo_calcs_ = - std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_, parameter_ns_); + servo_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); collision_checker_ = std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); @@ -67,67 +68,58 @@ bool Servo::readParameters() { std::size_t error = 0; - // Check for parameter namespace from launch file. All other parameters will be read from this namespace. - std::string yaml_namespace; - if (ros::param::get("~parameter_ns", yaml_namespace)) - { - if (!parameter_ns_.empty()) - ROS_WARN_STREAM_NAMED(LOGNAME, - "A parameter namespace was specified in the launch file AND in the constructor argument."); + // Optional parameter sub-namespace specified in the launch file. All other parameters will be read from this namespace. + std::string parameter_ns; + ros::param::get("~parameter_ns", parameter_ns); - parameter_ns_ = yaml_namespace; - } + // If parameters have been loaded into sub-namespace within the node namespace, append the parameter namespace + // to load the parameters correctly. + ros::NodeHandle nh = parameter_ns.empty() ? nh_ : ros::NodeHandle(nh_, parameter_ns); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/publish_period", parameters_.publish_period); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/collision_check_rate", parameters_.collision_check_rate); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/num_outgoing_halt_msgs_to_publish", + error += !rosparam_shortcuts::get(LOGNAME, nh, "publish_period", parameters_.publish_period); + error += !rosparam_shortcuts::get(LOGNAME, nh, "collision_check_rate", parameters_.collision_check_rate); + error += !rosparam_shortcuts::get(LOGNAME, nh, "num_outgoing_halt_msgs_to_publish", parameters_.num_outgoing_halt_msgs_to_publish); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/scale/linear", parameters_.linear_scale); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/scale/rotational", parameters_.rotational_scale); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/scale/joint", parameters_.joint_scale); - error += - !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/low_pass_filter_coeff", parameters_.low_pass_filter_coeff); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/joint_topic", parameters_.joint_topic); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/command_in_type", parameters_.command_in_type); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/cartesian_command_in_topic", - parameters_.cartesian_command_in_topic); + error += !rosparam_shortcuts::get(LOGNAME, nh, "scale/linear", parameters_.linear_scale); + error += !rosparam_shortcuts::get(LOGNAME, nh, "scale/rotational", parameters_.rotational_scale); + error += !rosparam_shortcuts::get(LOGNAME, nh, "scale/joint", parameters_.joint_scale); + error += !rosparam_shortcuts::get(LOGNAME, nh, "low_pass_filter_coeff", parameters_.low_pass_filter_coeff); + error += !rosparam_shortcuts::get(LOGNAME, nh, "joint_topic", parameters_.joint_topic); + error += !rosparam_shortcuts::get(LOGNAME, nh, "command_in_type", parameters_.command_in_type); + error += !rosparam_shortcuts::get(LOGNAME, nh, "cartesian_command_in_topic", parameters_.cartesian_command_in_topic); + error += !rosparam_shortcuts::get(LOGNAME, nh, "joint_command_in_topic", parameters_.joint_command_in_topic); + error += !rosparam_shortcuts::get(LOGNAME, nh, "robot_link_command_frame", parameters_.robot_link_command_frame); + error += !rosparam_shortcuts::get(LOGNAME, nh, "incoming_command_timeout", parameters_.incoming_command_timeout); error += - !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/joint_command_in_topic", parameters_.joint_command_in_topic); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/robot_link_command_frame", - parameters_.robot_link_command_frame); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/incoming_command_timeout", - parameters_.incoming_command_timeout); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/lower_singularity_threshold", - parameters_.lower_singularity_threshold); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/hard_stop_singularity_threshold", + !rosparam_shortcuts::get(LOGNAME, nh, "lower_singularity_threshold", parameters_.lower_singularity_threshold); + error += !rosparam_shortcuts::get(LOGNAME, nh, "hard_stop_singularity_threshold", parameters_.hard_stop_singularity_threshold); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/move_group_name", parameters_.move_group_name); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/planning_frame", parameters_.planning_frame); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/ee_frame_name", parameters_.ee_frame_name); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/use_gazebo", parameters_.use_gazebo); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/joint_limit_margin", parameters_.joint_limit_margin); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/command_out_topic", parameters_.command_out_topic); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/command_out_type", parameters_.command_out_type); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/publish_joint_positions", - parameters_.publish_joint_positions); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/publish_joint_velocities", - parameters_.publish_joint_velocities); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/publish_joint_accelerations", - parameters_.publish_joint_accelerations); + error += !rosparam_shortcuts::get(LOGNAME, nh, "move_group_name", parameters_.move_group_name); + error += !rosparam_shortcuts::get(LOGNAME, nh, "planning_frame", parameters_.planning_frame); + error += !rosparam_shortcuts::get(LOGNAME, nh, "ee_frame_name", parameters_.ee_frame_name); + error += !rosparam_shortcuts::get(LOGNAME, nh, "use_gazebo", parameters_.use_gazebo); + error += !rosparam_shortcuts::get(LOGNAME, nh, "joint_limit_margin", parameters_.joint_limit_margin); + error += !rosparam_shortcuts::get(LOGNAME, nh, "command_out_topic", parameters_.command_out_topic); + error += !rosparam_shortcuts::get(LOGNAME, nh, "command_out_type", parameters_.command_out_type); + error += !rosparam_shortcuts::get(LOGNAME, nh, "publish_joint_positions", parameters_.publish_joint_positions); + error += !rosparam_shortcuts::get(LOGNAME, nh, "publish_joint_velocities", parameters_.publish_joint_velocities); + error += + !rosparam_shortcuts::get(LOGNAME, nh, "publish_joint_accelerations", parameters_.publish_joint_accelerations); // Parameters for collision checking - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/check_collisions", parameters_.check_collisions); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/collision_check_type", parameters_.collision_check_type); + error += !rosparam_shortcuts::get(LOGNAME, nh, "check_collisions", parameters_.check_collisions); + error += !rosparam_shortcuts::get(LOGNAME, nh, "collision_check_type", parameters_.collision_check_type); bool have_self_collision_proximity_threshold = rosparam_shortcuts::get( - "", nh_, parameter_ns_ + "/self_collision_proximity_threshold", parameters_.self_collision_proximity_threshold); + LOGNAME, nh, "self_collision_proximity_threshold", parameters_.self_collision_proximity_threshold); bool have_scene_collision_proximity_threshold = rosparam_shortcuts::get( - "", nh_, parameter_ns_ + "/scene_collision_proximity_threshold", parameters_.scene_collision_proximity_threshold); + LOGNAME, nh, "scene_collision_proximity_threshold", parameters_.scene_collision_proximity_threshold); + double collision_proximity_threshold; // 'collision_proximity_threshold' parameter was removed, replaced with separate self- and scene-collision proximity // thresholds // TODO(JStech): remove this deprecation warning in ROS Noetic; simplify error case handling - if (nh_.hasParam(parameter_ns_ + "/collision_proximity_threshold") && - rosparam_shortcuts::get("", nh_, parameter_ns_ + "/collision_proximity_threshold", collision_proximity_threshold)) + if (nh_.hasParam("collision_proximity_threshold") && + rosparam_shortcuts::get(LOGNAME, nh, "collision_proximity_threshold", collision_proximity_threshold)) { ROS_WARN_NAMED(LOGNAME, "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate" "'self_collision_proximity_threshold' and 'scene_collision_proximity_threshold' " @@ -145,22 +137,22 @@ bool Servo::readParameters() } error += !have_self_collision_proximity_threshold; error += !have_scene_collision_proximity_threshold; - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/collision_distance_safety_factor", + error += !rosparam_shortcuts::get(LOGNAME, nh, "collision_distance_safety_factor", parameters_.collision_distance_safety_factor); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/min_allowable_collision_distance", + error += !rosparam_shortcuts::get(LOGNAME, nh, "min_allowable_collision_distance", parameters_.min_allowable_collision_distance); // This parameter name was changed recently. // Try retrieving from the correct name. If it fails, then try the deprecated name. // TODO(andyz): remove this deprecation warning in ROS Noetic - if (!rosparam_shortcuts::get("", nh_, parameter_ns_ + "/status_topic", parameters_.status_topic)) + if (!rosparam_shortcuts::get(LOGNAME, nh, "status_topic", parameters_.status_topic)) { ROS_WARN_NAMED(LOGNAME, "'status_topic' parameter is missing. Recently renamed from 'warning_topic'. Please update " "the servoing yaml file."); - error += !rosparam_shortcuts::get("", nh_, parameter_ns_ + "/warning_topic", parameters_.status_topic); + error += !rosparam_shortcuts::get(LOGNAME, nh, "warning_topic", parameters_.status_topic); } - rosparam_shortcuts::shutdownIfError(parameter_ns_, error); + rosparam_shortcuts::shutdownIfError(LOGNAME, error); // Input checking if (parameters_.publish_period <= 0.) diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index aca414e649..4b06627621 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -86,7 +86,7 @@ geometry_msgs::TransformStamped convertIsometryToTransform(const Eigen::Isometry // Constructor for the class that handles servoing calculations ServoCalcs::ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& joint_state_subscriber, std::string& ros_namespace) + const std::shared_ptr& joint_state_subscriber) : nh_(nh) , parameters_(parameters) , planning_scene_monitor_(planning_scene_monitor) @@ -113,19 +113,19 @@ ServoCalcs::ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, joint_cmd_sub_ = nh_.subscribe(parameters_.joint_command_in_topic, ROS_QUEUE_SIZE, &ServoCalcs::jointCmdCB, this); // ROS Server for allowing drift in some dimensions - drift_dimensions_server_ = - nh_.advertiseService(ros_namespace + "/change_drift_dimensions", &ServoCalcs::changeDriftDimensions, this); + drift_dimensions_server_ = nh_.advertiseService(ros::names::append(nh_.getNamespace(), "change_drift_dimensions"), + &ServoCalcs::changeDriftDimensions, this); // ROS Server for changing the control dimensions - control_dimensions_server_ = - nh_.advertiseService(ros_namespace + "/change_control_dimensions", &ServoCalcs::changeControlDimensions, this); + control_dimensions_server_ = nh_.advertiseService(ros::names::append(nh_.getNamespace(), "change_control_dimensions"), + &ServoCalcs::changeControlDimensions, this); // ROS Server to reset the status, e.g. so the arm can move again after a collision - reset_servo_status_ = - nh_.advertiseService(ros_namespace + "/reset_servo_status", &ServoCalcs::resetServoStatus, this); + reset_servo_status_ = nh_.advertiseService(ros::names::append(nh_.getNamespace(), "reset_servo_status"), + &ServoCalcs::resetServoStatus, this); // Publish and Subscribe to internal namespace topics - ros::NodeHandle internal_nh("~internal"); + ros::NodeHandle internal_nh(nh_, "internal"); collision_velocity_scale_sub_ = internal_nh.subscribe("collision_velocity_scale", ROS_QUEUE_SIZE, &ServoCalcs::collisionVelocityScaleCB, this); worst_case_stop_time_pub_ = internal_nh.advertise("worst_case_stop_time", ROS_QUEUE_SIZE); diff --git a/moveit_ros/moveit_servo/src/servo_server.cpp b/moveit_ros/moveit_servo/src/servo_server.cpp index 31e477c837..5c630f3082 100644 --- a/moveit_ros/moveit_servo/src/servo_server.cpp +++ b/moveit_ros/moveit_servo/src/servo_server.cpp @@ -52,7 +52,7 @@ int main(int argc, char** argv) ros::AsyncSpinner spinner(ROS_THREADS); spinner.start(); - ros::NodeHandle nh; + ros::NodeHandle nh("~"); // Load the planning scene monitor auto planning_scene_monitor = std::make_shared("robot_description"); diff --git a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp index 782fa23c5a..765094899d 100644 --- a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp +++ b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp @@ -78,10 +78,9 @@ class PoseTrackingFixture : public ::testing::Test planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, false /* skip octomap monitor */); - tracker_ = std::make_shared(planning_scene_monitor_, ""); + tracker_ = std::make_shared(nh_, planning_scene_monitor_); - target_pose_pub_ = - nh_.advertise("/pose_tracking_test/target_pose", 1 /* queue */, true /* latch */); + target_pose_pub_ = nh_.advertise("target_pose", 1 /* queue */, true /* latch */); // Tolerance for pose seeking translation_tolerance_ << TRANSLATION_TOLERANCE, TRANSLATION_TOLERANCE, TRANSLATION_TOLERANCE; @@ -91,7 +90,7 @@ class PoseTrackingFixture : public ::testing::Test } protected: - ros::NodeHandle nh_; + ros::NodeHandle nh_{ "~" }; planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; Eigen::Vector3d translation_tolerance_; moveit_servo::PoseTrackingPtr tracker_; diff --git a/moveit_ros/moveit_servo/test/pose_tracking_test.test b/moveit_ros/moveit_servo/test/pose_tracking_test.test index 4f162d0c0d..f149e48c32 100644 --- a/moveit_ros/moveit_servo/test/pose_tracking_test.test +++ b/moveit_ros/moveit_servo/test/pose_tracking_test.test @@ -17,7 +17,7 @@ - + diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp index 53684c1787..fa61c32e0a 100644 --- a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp @@ -89,7 +89,7 @@ class ServoFixture : public ::testing::Test } protected: - ros::NodeHandle nh_; + ros::NodeHandle nh_{ "~" }; planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; moveit_servo::ServoPtr servo_; }; // class ServoFixture diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test index f165250d82..94c6a2a7f4 100644 --- a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test @@ -12,7 +12,7 @@ - - + + From e337c0808be9b561d62f5c41c267643f5b46eb00 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 22 Oct 2020 17:07:46 -0600 Subject: [PATCH 503/612] Cleanup current state handling in servo (#2372) --- moveit_ros/moveit_servo/CMakeLists.txt | 1 - moveit_ros/moveit_servo/MIGRATION.md | 6 + .../include/moveit_servo/collision_check.h | 9 +- .../moveit_servo/joint_state_subscriber.h | 68 ----------- .../moveit_servo/include/moveit_servo/servo.h | 5 - .../include/moveit_servo/servo_calcs.h | 17 +-- .../moveit_servo/src/collision_check.cpp | 13 +- .../src/joint_state_subscriber.cpp | 76 ------------ moveit_ros/moveit_servo/src/servo.cpp | 37 ++++-- moveit_ros/moveit_servo/src/servo_calcs.cpp | 115 ++++++------------ 10 files changed, 79 insertions(+), 268 deletions(-) create mode 100644 moveit_ros/moveit_servo/MIGRATION.md delete mode 100644 moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h delete mode 100644 moveit_ros/moveit_servo/src/joint_state_subscriber.cpp diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 86c6d1ebcc..fcded51b2f 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -68,7 +68,6 @@ add_library(${SERVO_LIB_NAME} src/collision_check.cpp src/servo_calcs.cpp src/servo.cpp - src/joint_state_subscriber.cpp src/low_pass_filter.cpp ) set_target_properties(${SERVO_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") diff --git a/moveit_ros/moveit_servo/MIGRATION.md b/moveit_ros/moveit_servo/MIGRATION.md new file mode 100644 index 0000000000..ae97f6a75a --- /dev/null +++ b/moveit_ros/moveit_servo/MIGRATION.md @@ -0,0 +1,6 @@ +# Migration Notes + +API changes since last Noetic Release + +- Servo::getLatestJointState was removed. To get the latest joint positions of the group servo is working with use the CSM in the PSM. Here is an example of how to get the latest joint positions: + planning_scene_monitor_->getStateMonitor()->getCurrentState()->copyJointGroupPositions(move_group_name, positions); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index 9accf79484..08aadaad46 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -46,7 +46,6 @@ #include #include -#include namespace moveit_servo { @@ -65,8 +64,7 @@ class CollisionCheck * already started when passed into this class */ CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& joint_state_subscriber); + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); ~CollisionCheck() { @@ -97,11 +95,8 @@ class CollisionCheck // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - // Subscriber to joint states - const std::shared_ptr joint_state_subscriber_; - // Robot state and collision matrix from planning scene - std::unique_ptr current_state_; + std::shared_ptr current_state_; collision_detection::AllowedCollisionMatrix acm_; // Scale robot velocity according to collision proximity and user-defined thresholds. diff --git a/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h deleted file mode 100644 index 64c55367bc..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h +++ /dev/null @@ -1,68 +0,0 @@ -/******************************************************************************* - * Title : joint_state_subscriber.h - * Project : moveit_servo - * Created : 06/11/2020 - * Author : Tyler Weaver - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 the copyright holder 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 HOLDER 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 moveit_servo -{ -class JointStateSubscriber -{ -public: - /** \brief Constructor */ - JointStateSubscriber(ros::NodeHandle& nh, const std::string& joint_state_topic_name); - - /** \brief Get the latest joint state message */ - sensor_msgs::JointStateConstPtr getLatest() const; - -private: - void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); - - ros::Subscriber joint_state_sub_; - - // Latest joint state, updated by ROS callback - mutable std::mutex joint_state_mutex_; - sensor_msgs::JointStateConstPtr latest_joint_state_; -}; -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h index 6205e09c97..aeb61dab2b 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -43,7 +43,6 @@ #include #include #include -#include namespace moveit_servo { @@ -86,9 +85,6 @@ class Servo /** \brief Get the parameters used by servo node. */ const ServoParameters& getParameters() const; - /** \brief Get the latest joint state. */ - sensor_msgs::JointStateConstPtr getLatestJointState() const; - /** \brief Change the controlled link. Often, this is the end effector * This must be a link on the robot since MoveIt tracks the transform (not tf) */ @@ -108,7 +104,6 @@ class Servo // Store the parameters that were read from ROS server ServoParameters parameters_; - std::shared_ptr joint_state_subscriber_; std::unique_ptr servo_calcs_; std::unique_ptr collision_checker_; }; diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 37e7e647db..c92171b179 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -38,6 +38,9 @@ #pragma once +// C++ +#include + // ROS #include #include @@ -57,7 +60,6 @@ #include #include #include -#include namespace moveit_servo { @@ -65,8 +67,7 @@ class ServoCalcs { public: ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& joint_state_subscriber); + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); ~ServoCalcs() { @@ -115,7 +116,7 @@ class ServoCalcs bool jointServoCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); /** \brief Parse the incoming joint msg for the joints of our MoveGroup */ - bool updateJoints(); + void updateJoints(); /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. * Also, multiply by timestep to calculate a position change. @@ -220,9 +221,6 @@ class ServoCalcs // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - // Subscriber to the latest joint states - const std::shared_ptr joint_state_subscriber_; - // Track the number of cycles during which motion has not occurred. // Will avoid re-publishing zero velocities endlessly. int zero_velocity_count_ = 0; @@ -244,7 +242,7 @@ class ServoCalcs const moveit::core::JointModelGroup* joint_model_group_; - moveit::core::RobotStatePtr kinematic_state_; + moveit::core::RobotStatePtr current_state_; // incoming_joint_state_ is the incoming message. It may contain passive joints or other joints we don't care about. // (mutex protected below) @@ -294,9 +292,6 @@ class ServoCalcs // The dimesions to control. In the command frame. [x, y, z, roll, pitch, yaw] std::array control_dimensions_ = { { true, true, true, true, true, true } }; - // Amount we sleep when waiting - ros::Rate default_sleep_rate_ = 100; - // latest_state_mutex_ is used to protect the state below it mutable std::mutex latest_state_mutex_; Eigen::Isometry3d tf_moveit_to_robot_cmd_frame_; diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp index 73c42dbdcd..85aa7d4810 100644 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ b/moveit_ros/moveit_servo/src/collision_check.cpp @@ -51,12 +51,10 @@ namespace moveit_servo { // Constructor for the class that handles collision checking CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& joint_state_subscriber) + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : nh_(nh) , parameters_(parameters) , planning_scene_monitor_(planning_scene_monitor) - , joint_state_subscriber_(joint_state_subscriber) , self_velocity_scale_coefficient_(-log(0.001) / parameters.self_collision_proximity_threshold) , scene_velocity_scale_coefficient_(-log(0.001) / parameters.scene_collision_proximity_threshold) , period_(1. / parameters_.collision_check_rate) @@ -80,7 +78,7 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoPar worst_case_stop_time_sub_ = internal_nh.subscribe("worst_case_stop_time", ROS_QUEUE_SIZE, &CollisionCheck::worstCaseStopTimeCB, this); - current_state_ = std::make_unique(getLockedPlanningSceneRO()->getCurrentState()); + current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); acm_ = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); } @@ -109,11 +107,8 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event) return; } - // Copy the latest joint state - auto latest_joint_state = joint_state_subscriber_->getLatest(); - for (std::size_t i = 0; i < latest_joint_state->position.size(); ++i) - current_state_->setJointPositions(latest_joint_state->name[i], &latest_joint_state->position[i]); - + // Update to the latest current state + current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); current_state_->updateCollisionBodyTransforms(); collision_detected_ = false; diff --git a/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp b/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp deleted file mode 100644 index 22b8b4bfab..0000000000 --- a/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp +++ /dev/null @@ -1,76 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 the copyright holder 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 HOLDER 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. - *******************************************************************************/ - -/* Title : joint_state_subscriber.cpp - * Project : moveit_servo - * Created : 06/11/2020 - * Author : Tyler Weaver - */ - -#include - -namespace moveit_servo -{ -constexpr char LOGNAME[] = "joint_state_subscriber"; - -// Constructor for the class that handles collision checking -JointStateSubscriber::JointStateSubscriber(ros::NodeHandle& nh, const std::string& joint_state_topic_name) -{ - // subscribe to joints - joint_state_sub_ = nh.subscribe(joint_state_topic_name, ROS_QUEUE_SIZE, &JointStateSubscriber::jointStateCB, this); - - ros::AsyncSpinner spinner(1); - spinner.start(); - - // Wait for initial messages - ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg."); - while (latest_joint_state_ == nullptr) - { - ros::Duration(0.01).sleep(); - } - ROS_INFO_NAMED(LOGNAME, "Received first joint msg."); -} - -sensor_msgs::JointStateConstPtr JointStateSubscriber::getLatest() const -{ - const std::lock_guard lock(joint_state_mutex_); - return latest_joint_state_; -} - -void JointStateSubscriber::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) -{ - const std::lock_guard lock(joint_state_mutex_); - latest_joint_state_ = msg; -} - -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 8467837429..b35267cc5b 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -39,12 +39,18 @@ #include +#include #include static const std::string LOGNAME = "servo_node"; namespace moveit_servo { +namespace +{ +constexpr double ROBOT_STATE_WAIT_TIME = 10.0; // seconds +} // namespace + Servo::Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : nh_(nh), planning_scene_monitor_(planning_scene_monitor) { @@ -52,15 +58,27 @@ Servo::Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMon if (!readParameters()) exit(EXIT_FAILURE); - // By default, joint topic name is relative to the node parent namespace. Fully custom joint name topics can be - // set by using absolute topic names in config files. For example, "/foo/my_joint_state_topic". - ros::NodeHandle nh_parent_ns = ros::NodeHandle(""); - joint_state_subscriber_ = std::make_shared(nh_parent_ns, parameters_.joint_topic); + // Async spinner is needed to receive messages to wait for the robot state to be complete + ros::AsyncSpinner spinner(1); + spinner.start(); + + // Confirm the planning scene monitor is ready to be used + if (!planning_scene_monitor_->getStateMonitor()) + { + planning_scene_monitor_->startStateMonitor(parameters_.joint_topic); + } + planning_scene_monitor->getStateMonitor()->enableCopyDynamics(true); + + if (!planning_scene_monitor_->getStateMonitor()->waitForCompleteState(parameters_.move_group_name, + ROBOT_STATE_WAIT_TIME)) + { + ROS_FATAL_NAMED(LOGNAME, "Timeout waiting for current state"); + exit(EXIT_FAILURE); + } - servo_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); + servo_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); - collision_checker_ = - std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); + collision_checker_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); } // Read ROS parameters, typically from YAML file @@ -316,9 +334,4 @@ const ServoParameters& Servo::getParameters() const return parameters_; } -sensor_msgs::JointStateConstPtr Servo::getLatestJointState() const -{ - return joint_state_subscriber_->getLatest(); -} - } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 4b06627621..8e34341380 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -42,8 +42,8 @@ #include #include -#include #include +#include static const std::string LOGNAME = "servo_calcs"; constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30; // Seconds to throttle logs inside loops @@ -85,26 +85,12 @@ geometry_msgs::TransformStamped convertIsometryToTransform(const Eigen::Isometry // Constructor for the class that handles servoing calculations ServoCalcs::ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& joint_state_subscriber) - : nh_(nh) - , parameters_(parameters) - , planning_scene_monitor_(planning_scene_monitor) - , joint_state_subscriber_(joint_state_subscriber) - , period_(parameters.publish_period) + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : nh_(nh), parameters_(parameters), planning_scene_monitor_(planning_scene_monitor), period_(parameters.publish_period) { // MoveIt Setup - const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr = planning_scene_monitor_->getRobotModelLoader(); - while (ros::ok() && !model_loader_ptr) - { - ROS_WARN_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); - default_sleep_rate_.sleep(); - } - const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); - kinematic_state_ = std::make_shared(kinematic_model); - kinematic_state_->setToDefaultValues(); - - joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); + current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + joint_model_group_ = current_state_->getJointModelGroup(parameters_.move_group_name); prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getActiveJointModels().size()); // Subscribe to command topics @@ -177,9 +163,9 @@ void ServoCalcs::start() trajectory_msgs::JointTrajectoryPoint point; point.time_from_start = ros::Duration(parameters_.publish_period); - auto latest_joints = joint_state_subscriber_->getLatest(); if (parameters_.publish_joint_positions) - point.positions = latest_joints->position; + planning_scene_monitor_->getStateMonitor()->getCurrentState()->copyJointGroupPositions(joint_model_group_, + point.positions); if (parameters_.publish_joint_velocities) { std::vector velocity(num_joints_); @@ -198,14 +184,11 @@ void ServoCalcs::start() timer_ = nh_.createTimer(period_, &ServoCalcs::run, this); - // TODO(adamp or andyz): after rebasing on upstream master, update this section. Maybe put the tf updates in a tiny - // function by themselves - sensor_msgs::JointStateConstPtr latest_joint_state = joint_state_subscriber_->getLatest(); - kinematic_state_->setVariableValues(*latest_joint_state); - tf_moveit_to_ee_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * - kinematic_state_->getGlobalLinkTransform(parameters_.ee_frame_name); - tf_moveit_to_robot_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * - kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); + current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + tf_moveit_to_ee_frame_ = current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + current_state_->getGlobalLinkTransform(parameters_.ee_frame_name); + tf_moveit_to_robot_cmd_frame_ = current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + current_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); } void ServoCalcs::run(const ros::TimerEvent& timer_event) @@ -226,14 +209,10 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) // Always update the joints and end-effector transform for 2 reasons: // 1) in case the getCommandFrameTransform() method is being used // 2) so the low-pass filters are up to date and don't cause a jump - while (!updateJoints() && ros::ok()) - { - default_sleep_rate_.sleep(); - } + updateJoints(); // Update from latest state - sensor_msgs::JointStateConstPtr latest_joint_state = joint_state_subscriber_->getLatest(); - kinematic_state_->setVariableValues(*latest_joint_state); + current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); { const std::lock_guard lock(latest_state_mutex_); if (latest_twist_stamped_) @@ -255,13 +234,13 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) // Calculate this transform to ensure it is available via C++ API // We solve (planning_frame -> base -> robot_link_command_frame) // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) - tf_moveit_to_robot_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * - kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); + tf_moveit_to_robot_cmd_frame_ = current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + current_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); // Calculate the transform from MoveIt planning frame to End Effector frame // Calculate this transform to ensure it is available via C++ API - tf_moveit_to_ee_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * - kinematic_state_->getGlobalLinkTransform(parameters_.ee_frame_name); + tf_moveit_to_ee_frame_ = current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + current_state_->getGlobalLinkTransform(parameters_.ee_frame_name); have_nonzero_command_ = have_nonzero_twist_stamped_ || have_nonzero_joint_command_; @@ -447,8 +426,8 @@ bool ServoCalcs::cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, // We solve (planning_frame -> base -> cmd.header.frame_id) // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id) const auto tf_moveit_to_incoming_cmd_frame = - kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * - kinematic_state_->getGlobalLinkTransform(cmd.header.frame_id); + current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + current_state_->getGlobalLinkTransform(cmd.header.frame_id); translation_vector = tf_moveit_to_incoming_cmd_frame.linear() * translation_vector; angular_vector = tf_moveit_to_incoming_cmd_frame.linear() * angular_vector; @@ -467,7 +446,7 @@ bool ServoCalcs::cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); // Convert from cartesian commands to joint commands - Eigen::MatrixXd jacobian = kinematic_state_->getJacobian(joint_model_group_); + Eigen::MatrixXd jacobian = current_state_->getJacobian(joint_model_group_); // May allow some dimensions to drift, based on drift_dimensions // i.e. take advantage of task redundancy. @@ -671,10 +650,10 @@ double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& co // Calculate a small change in joints Eigen::VectorXd new_theta; - kinematic_state_->copyJointGroupPositions(joint_model_group_, new_theta); + current_state_->copyJointGroupPositions(joint_model_group_, new_theta); new_theta += pseudo_inverse * delta_x; - kinematic_state_->setJointGroupPositions(joint_model_group_, new_theta); - auto new_jacobian = kinematic_state_->getJacobian(joint_model_group_); + current_state_->setJointGroupPositions(joint_model_group_, new_theta); + Eigen::MatrixXd new_jacobian = current_state_->getJacobian(joint_model_group_); Eigen::JacobiSVD new_svd(new_jacobian); double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1); @@ -784,16 +763,16 @@ bool ServoCalcs::enforcePositionLimits() break; } } - if (!kinematic_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin)) + if (!current_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin)) { const std::vector limits = joint->getVariableBoundsMsg(); // Joint limits are not defined for some joints. Skip them. if (!limits.empty()) { - if ((kinematic_state_->getJointVelocities(joint)[0] < 0 && + if ((current_state_->getJointVelocities(joint)[0] < 0 && (joint_angle < (limits[0].min_position + parameters_.joint_limit_margin))) || - (kinematic_state_->getJointVelocities(joint)[0] > 0 && + (current_state_->getJointVelocities(joint)[0] > 0 && (joint_angle > (limits[0].max_position - parameters_.joint_limit_margin)))) { ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, @@ -843,31 +822,12 @@ void ServoCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory) } // Parse the incoming joint msg for the joints of our MoveGroup -bool ServoCalcs::updateJoints() +void ServoCalcs::updateJoints() { - sensor_msgs::JointStateConstPtr latest_joint_state = joint_state_subscriber_->getLatest(); - - // Check that the msg contains enough joints - if (latest_joint_state->name.size() < num_joints_) - return false; - - // Store joints in a member variable - for (std::size_t m = 0; m < latest_joint_state->name.size(); ++m) - { - std::size_t c; - try - { - c = joint_state_name_map_.at(latest_joint_state->name[m]); - } - catch (const std::out_of_range& e) - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, - "Ignoring joint " << latest_joint_state->name[m]); - continue; - } - - internal_joint_state_.position[c] = latest_joint_state->position[m]; - } + // Get the latest joint group positions + current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + current_state_->copyJointGroupPositions(joint_model_group_, internal_joint_state_.position); + current_state_->copyJointGroupVelocities(joint_model_group_, internal_joint_state_.velocity); // Cache the original joints in case they need to be reset original_joint_state_ = internal_joint_state_; @@ -878,9 +838,9 @@ bool ServoCalcs::updateJoints() double accel_limit = 0; double joint_velocity = 0; double worst_case_stop_time = 0; - for (size_t jt_state_idx = 0; jt_state_idx < latest_joint_state->velocity.size(); ++jt_state_idx) + for (size_t jt_state_idx = 0; jt_state_idx < internal_joint_state_.velocity.size(); ++jt_state_idx) { - joint_name = latest_joint_state->name[jt_state_idx]; + joint_name = internal_joint_state_.name[jt_state_idx]; // Get acceleration limit for this joint for (auto joint_model : joint_model_group_->getActiveJointModels()) @@ -906,7 +866,7 @@ bool ServoCalcs::updateJoints() } // Get the current joint velocity - joint_velocity = latest_joint_state->velocity[jt_state_idx]; + joint_velocity = internal_joint_state_.velocity[jt_state_idx]; // Calculate worst case stop time worst_case_stop_time = std::max(worst_case_stop_time, fabs(joint_velocity / accel_limit)); @@ -918,8 +878,6 @@ bool ServoCalcs::updateJoints() msg->data = worst_case_stop_time; worst_case_stop_time_pub_.publish(msg); } - - return true; } // Scale the incoming servo command @@ -967,8 +925,7 @@ Eigen::VectorXd ServoCalcs::scaleJointCommand(const control_msgs::JointJog& comm } catch (const std::out_of_range& e) { - ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, - "Ignoring joint " << joint_state_subscriber_->getLatest()->name[m]); + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Ignoring joint " << command.joint_names[m]); continue; } // Apply user-defined scaling if inputs are unitless [-1:1] From 225cb77cbd9d50e93c42d9e3ad6aa4490c6b30ed Mon Sep 17 00:00:00 2001 From: Jeroen Date: Mon, 26 Oct 2020 19:35:43 +0100 Subject: [PATCH 504/612] Add test to ompl interface for StateValidityChecker (#2247) * add generic robot test class to ompl interface test folder * add test for ompl interface StateValidityChecker * add test with path constraints to ompl interface StateValidityChecker * use debug log statements, add newline, cleanup comments * remove dead code and update comment * Change company name in license * update state validity test comment * improve assert statements * remove empty line from load_test_robot.h --- .../ompl/ompl_interface/CMakeLists.txt | 4 + .../test/test_state_validity_checker.cpp | 342 ++++++++++++++++++ 2 files changed, 346 insertions(+) create mode 100644 moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index 5945661811..b8092d65b6 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -56,4 +56,8 @@ if(CATKIN_ENABLE_TESTING) test/test_planning_context_manager.test test/test_planning_context_manager.cpp) target_link_libraries(test_planning_context_manager ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) + + catkin_add_gtest(test_state_validity_checker test/test_state_validity_checker.cpp) + target_link_libraries(test_state_validity_checker ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES}) + set_target_properties(test_state_validity_checker PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") endif() diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp new file mode 100644 index 0000000000..9b937e2a63 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp @@ -0,0 +1,342 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * 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 KU Leuven 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. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ +/** + * This test checks the basics of a StateValidityChecker: + * - Can we create one? + * - States inside and outside joint limits. + * - States that are in self-collision. + * - Position constraints on the robot's end-effector link. + * + * It does not yet test: + * - Collision with objects in the environment. + * - Orientation constraints, visibility constraints, ... + * - A user-specified feasibility function in the planning scene. + * + * The test do show what is minimally required to create a working StateValidityChecker. + **/ + +#include "load_test_robot.h" + +#include +#include + +#include + +#include +#include +#include +#include + +#include + +/** \brief This flag sets the verbosity level for the state validity checker. **/ +constexpr bool VERBOSE{ false }; + +constexpr char LOGNAME[] = "test_state_validity_checker"; + +/** \brief Pretty print std:vectors **/ +std::ostream& operator<<(std::ostream& os, const std::vector& v) +{ + os << "( "; + for (auto value : v) + os << value << ", "; + os << " )"; + return os; +} + +/** \brief Generic implementation of the tests that can be executed on different robots. **/ +class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, public testing::Test +{ +public: + TestStateValidityChecker(const std::string& robot_name, const std::string& group_name) + : LoadTestRobot(robot_name, group_name) + { + } + + /*************************************************************************** + * START Test implementations + * ************************************************************************/ + + void testConstructor() + { + ompl::base::StateValidityCheckerPtr checker = + std::make_shared(planning_context_.get()); + } + + /** This test takes a state that is inside the joint limits and collision free as input. **/ + void testJointLimits(const std::vector& position_in_limits) + { + // create a validity checker for this test + auto checker = std::make_shared(planning_context_.get()); + checker->setVerbose(VERBOSE); + + robot_state_->setJointGroupPositions(joint_model_group_, position_in_limits); + + // use a scoped OMPL state so we don't have to call allocState and freeState + // (as recommended in the OMPL documantion) + ompl::base::ScopedState<> ompl_state(state_space_); + state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); + + ROS_DEBUG_STREAM_NAMED(LOGNAME, ompl_state.reals()); + + // assume the given position is not in self-collision + // and there are no collision objects or path constraints so this state should be valid + EXPECT_TRUE(checker->isValid(ompl_state.get())); + + // move first joint obviously outside any joint limits + ompl_state->as()->values[0] = std::numeric_limits::max(); + ompl_state->as()->clearKnownInformation(); + + ROS_DEBUG_STREAM_NAMED(LOGNAME, ompl_state.reals()); + + EXPECT_FALSE(checker->isValid(ompl_state.get())); + } + + /** This test takes a state that is known to be in self-collision and inside the joint limits as input. **/ + void testSelfCollision(const std::vector& position_in_self_collision) + { + // create a validity checker for this test + auto checker = std::make_shared(planning_context_.get()); + checker->setVerbose(VERBOSE); + + robot_state_->setJointGroupPositions(joint_model_group_, position_in_self_collision); + + // use a scoped OMPL state so we don't have to call allocState and freeState + // (as recommended in the OMPL documantion) + ompl::base::ScopedState<> ompl_state(state_space_); + state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); + + ROS_DEBUG_STREAM_NAMED(LOGNAME, ompl_state.reals()); + + // the given state is known to be in self-collision, we check it here + EXPECT_FALSE(checker->isValid(ompl_state.get())); + + // but it should respect the joint limits + EXPECT_TRUE(robot_state_->satisfiesBounds()); + } + + void testPathConstraints(const std::vector& position_in_joint_limits) + { + ASSERT_NE(planning_context_, nullptr) << "Initialize planning context before adding path constraints."; + + // set the robot to a known position that is withing the joint limits and collision free + robot_state_->setJointGroupPositions(joint_model_group_, position_in_joint_limits); + + // create position constraints around the given robot state + moveit_msgs::Constraints path_constraints; + Eigen::Isometry3d ee_pose = robot_state_->getGlobalLinkTransform(ee_link_name_); + path_constraints.name = "test_position_constraints"; + path_constraints.position_constraints.push_back(createPositionConstraint( + { ee_pose.translation().x(), ee_pose.translation().y(), ee_pose.translation().z() }, { 0.1, 0.1, 0.1 })); + + moveit_msgs::MoveItErrorCodes error_code_not_used; + ASSERT_TRUE(planning_context_->setPathConstraints(path_constraints, &error_code_not_used)); + + auto checker = std::make_shared(planning_context_.get()); + checker->setVerbose(VERBOSE); + + // use a scoped OMPL state so we don't have to call allocState and freeState + // (as recommended in the OMPL documantion) + ompl::base::ScopedState<> ompl_state(state_space_); + state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); + + ROS_DEBUG_STREAM_NAMED(LOGNAME, ompl_state.reals()); + + EXPECT_TRUE(checker->isValid(ompl_state.get())); + + // move the position constraints away from the curren end-effector position to make it fail + moveit_msgs::Constraints path_constraints_2(path_constraints); + path_constraints_2.position_constraints.at(0).constraint_region.primitive_poses.at(0).position.z += 0.2; + + ASSERT_TRUE(planning_context_->setPathConstraints(path_constraints_2, &error_code_not_used)); + + // clear the cached validity of the state before checking again, + // otherwise the path constraints will not be checked. + ompl_state->as()->clearKnownInformation(); + + EXPECT_FALSE(checker->isValid(ompl_state.get())); + } + + /*************************************************************************** + * END Test implementation + * ************************************************************************/ + +protected: + void SetUp() override + { + // setup all the input we need to create a StateValidityChecker + setupStateSpace(); + setupPlanningContext(); + }; + + void setupStateSpace() + { + ompl_interface::ModelBasedStateSpaceSpecification space_spec(robot_model_, group_name_); + state_space_ = std::make_shared(space_spec); + state_space_->computeLocations(); // this gets normally called in the state space factory + } + + void setupPlanningContext() + { + ASSERT_NE(state_space_, nullptr) << "Initialize state space before creating the planning context."; + + planning_context_spec_.state_space_ = state_space_; + planning_context_spec_.ompl_simple_setup_ = std::make_shared(state_space_); + planning_context_ = + std::make_shared(group_name_, planning_context_spec_); + + planning_scene_ = std::make_shared(robot_model_); + planning_context_->setPlanningScene(planning_scene_); + moveit::core::RobotState start_state(robot_model_); + start_state.setToDefaultValues(); + planning_context_->setCompleteInitialState(start_state); + } + + /** \brief Helper function to create a position constraint. **/ + moveit_msgs::PositionConstraint createPositionConstraint(std::array position, + std::array dimensions) + { + shape_msgs::SolidPrimitive box; + box.type = shape_msgs::SolidPrimitive::BOX; + box.dimensions.resize(3); + box.dimensions[shape_msgs::SolidPrimitive::BOX_X] = dimensions[0]; + box.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = dimensions[1]; + box.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = dimensions[2]; + + geometry_msgs::Pose box_pose; + box_pose.position.x = position[0]; + box_pose.position.y = position[1]; + box_pose.position.z = position[2]; + box_pose.orientation.w = 1.0; + + moveit_msgs::PositionConstraint position_constraint; + position_constraint.header.frame_id = base_link_name_; + position_constraint.link_name = ee_link_name_; + position_constraint.constraint_region.primitives.push_back(box); + position_constraint.constraint_region.primitive_poses.push_back(box_pose); + + // set the default weight to avoid warning in test output + position_constraint.weight = 1.0; + + return position_constraint; + } + + ompl_interface::ModelBasedStateSpacePtr state_space_; + ompl_interface::ModelBasedPlanningContextSpecification planning_context_spec_; + ompl_interface::ModelBasedPlanningContextPtr planning_context_; + planning_scene::PlanningScenePtr planning_scene_; +}; + +// /*************************************************************************** +// * Run all tests on the Panda robot +// * ************************************************************************/ +class PandaValidity : public TestStateValidityChecker +{ +protected: + PandaValidity() : TestStateValidityChecker("panda", "panda_arm") + { + } +}; + +TEST_F(PandaValidity, testConstructor) +{ + testConstructor(); +} + +TEST_F(PandaValidity, testJointLimits) +{ + // use the panda "ready" state from the srdf config + // we know this state should be within limits and self-collision free + testJointLimits({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }); +} + +TEST_F(PandaValidity, testSelfCollision) +{ + // the given state has self collision between "hand" and "panda_link2" + // (I just tried a couple of random states until I found one that collided.) + testSelfCollision({ 2.31827, -0.169668, 2.5225, -2.98568, -0.36355, 0.808339, 0.0843406 }); +} + +TEST_F(PandaValidity, testPathConstraints) +{ + // use the panda "ready" state from the srdf config + // we know this state should be within limits and self-collision free + testPathConstraints({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }); +} + +/*************************************************************************** + * Run all tests on the Fanuc robot + * ************************************************************************/ +class FanucTest : public TestStateValidityChecker +{ +protected: + FanucTest() : TestStateValidityChecker("fanuc", "manipulator") + { + } +}; + +TEST_F(FanucTest, createStateValidityChecker) +{ + testConstructor(); +} + +TEST_F(FanucTest, testJointLimits) +{ + // I assume the Fanucs's zero state is within limits and self-collision free + testJointLimits({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }); +} + +TEST_F(FanucTest, testSelfCollision) +{ + // the given state has self collision between "base_link" and "link_5" + // (I just tried a couple of random states until I found one that collided.) + testSelfCollision({ -2.95993, -0.682185, -2.43873, -0.939784, 3.0544, 0.882294 }); +} + +TEST_F(FanucTest, testPathConstraints) +{ + // I assume the Fanucs's zero state is within limits and self-collision free + testPathConstraints({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }); +} + +/*************************************************************************** + * MAIN + * ************************************************************************/ +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From a461efe7dbef01328e3d72c1c83e66dce050eb2c Mon Sep 17 00:00:00 2001 From: Alex Goldman Date: Wed, 28 Oct 2020 03:21:39 -0600 Subject: [PATCH 505/612] Fix broken documentation link in README (#2393) --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 18a0d11020..f84724d9e7 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ - [Overview of MoveIt](https://moveit.ros.org) - [Installation Instructions](https://moveit.ros.org/install/) -- [Documentation](https://moveit.ros.org/documentation/) +- [Documentation](https://moveit.ros.org/documentation/source-code-api/) - [Get Involved](https://moveit.ros.org/about/get_involved/) ## Branches Policy From d5dcc9d6618bd14eed7bebf48a619a37a5f3a6fb Mon Sep 17 00:00:00 2001 From: Jere Liukkonen Date: Sat, 31 Oct 2020 07:06:36 -0600 Subject: [PATCH 506/612] Fix pose tracking race condition (#2395) * Stop rewriting target_pose_ timestamps. It is caller's responsibility to ensure timestamp is correct and recent enough. * Prevent data races on target_pose_ by limiting access to it with a mutex. * Re-arrange part in targetPoseCallback function to make the callback lightweight when the target pose is in planning frame. * Small improvements to comments and code readability. * Set mutex as mutable. * Fix formatting. * Improve comment re. angle-axis PID tracking * Implement resetTargetPose() and add it to test & example * Add mutex protection to resetTargetPose() * Add missing semicolon and fix formatting. Co-authored-by: AndyZe --- .../include/moveit_servo/pose_tracking.h | 4 + .../pose_tracking_example.cpp | 4 + moveit_ros/moveit_servo/src/pose_tracking.cpp | 91 +++++++++++-------- .../moveit_servo/test/pose_tracking_test.cpp | 5 + 4 files changed, 64 insertions(+), 40 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h index d76c58481b..ad391b158b 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h @@ -114,6 +114,9 @@ class PoseTracking */ bool getCommandFrameTransform(geometry_msgs::TransformStamped& transform); + /** \brief Re-initialize the target pose to an empty message. Can be used to reset motion between waypoints. */ + void resetTargetPose(); + // moveit_servo::Servo instance. Public so we can access member functions like setPaused() std::unique_ptr servo_; @@ -170,6 +173,7 @@ class PoseTracking Eigen::Isometry3d command_frame_transform_; ros::Time command_frame_transform_stamp_; geometry_msgs::PoseStamped target_pose_; + mutable std::mutex target_pose_mtx_; // Subscribe to target pose ros::Subscriber target_pose_sub_; diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp index b5ec8d176f..3657d52864 100644 --- a/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp @@ -127,6 +127,10 @@ int main(int argc, char** argv) // Modify it a little bit target_pose.pose.position.x += 0.1; + // resetTargetPose() can be used to clear the target pose and wait for a new one, e.g. when moving between multiple + // waypoints + tracker.resetTargetPose(); + // Publish target pose target_pose.header.stamp = ros::Time::now(); target_pose_pub.publish(target_pose); diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp index 065e547f8e..e8dcff87b1 100644 --- a/moveit_ros/moveit_servo/src/pose_tracking.cpp +++ b/moveit_ros/moveit_servo/src/pose_tracking.cpp @@ -79,12 +79,9 @@ PoseTracking::PoseTracking(const ros::NodeHandle& nh, PoseTrackingStatusCode PoseTracking::moveToPose(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance, const double target_pose_timeout) { - // Roll back the target pose timestamp to ensure we wait for a new target pose message - target_pose_.header.stamp = ros::Time::now() - ros::Duration(2 * target_pose_timeout); - // Wait a bit for a target pose message to arrive. // The target pose may get updated by new messages as the robot moves (in a callback function). - ros::Time start_time = ros::Time::now(); + const ros::Time start_time = ros::Time::now(); while ((!haveRecentTargetPose(target_pose_timeout) || !haveRecentEndEffectorPose(target_pose_timeout)) && ((ros::Time::now() - start_time).toSec() < target_pose_timeout)) { @@ -94,36 +91,34 @@ PoseTrackingStatusCode PoseTracking::moveToPose(const Eigen::Vector3d& positiona } ros::Duration(0.001).sleep(); } + if (!haveRecentTargetPose(target_pose_timeout)) { ROS_ERROR_STREAM_NAMED(LOGNAME, "The target pose was not updated recently. Aborting."); return PoseTrackingStatusCode::NO_RECENT_TARGET_POSE; } - while (ros::ok()) + // Continue sending PID controller output to Servo until one of the following conditions is met: + // - Goal tolerance is satisfied + // - Target pose becomes outdated + // - Command frame transform becomes outdated + // - Another thread requested a stop + while (ros::ok() && !satisfiesPoseTolerance(positional_tolerance, angular_tolerance)) { - // Check for reasons to stop: - // - Goal tolerance is satisfied - // - Timeout - // - Another thread requested a stop - // - PID controllers aren't initialized - if (satisfiesPoseTolerance(positional_tolerance, angular_tolerance)) - { - break; - } - // Attempt to update robot pose if (servo_->getCommandFrameTransform(command_frame_transform_)) { command_frame_transform_stamp_ = ros::Time::now(); } + // Check that end-effector pose (command frame transform) is recent enough. if (!haveRecentEndEffectorPose(target_pose_timeout)) { ROS_ERROR_STREAM_NAMED(LOGNAME, "The end effector pose was not updated in time. Aborting."); doPostMotionReset(); return PoseTrackingStatusCode::NO_RECENT_END_EFFECTOR_POSE; } + if (stop_requested_) { ROS_INFO_STREAM_NAMED(LOGNAME, "Halting servo motion, a stop was requested."); @@ -210,6 +205,7 @@ void PoseTracking::initializePID(const PIDConfig& pid_config, std::vector lock(target_pose_mtx_); return ((ros::Time::now() - target_pose_.header.stamp).toSec() < timespan); } @@ -220,59 +216,67 @@ bool PoseTracking::haveRecentEndEffectorPose(const double timespan) bool PoseTracking::satisfiesPoseTolerance(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance) { + std::lock_guard lock(target_pose_mtx_); double x_error = target_pose_.pose.position.x - command_frame_transform_.translation()(0); double y_error = target_pose_.pose.position.y - command_frame_transform_.translation()(1); double z_error = target_pose_.pose.position.z - command_frame_transform_.translation()(2); - return (fabs(x_error) < positional_tolerance(0)) && (fabs(y_error) < positional_tolerance(1)) && - (fabs(z_error) < positional_tolerance(2) && fabs(angular_error_) < angular_tolerance); + return ((std::abs(x_error) < positional_tolerance(0)) && (std::abs(y_error) < positional_tolerance(1)) && + (std::abs(z_error) < positional_tolerance(2)) && (std::abs(angular_error_) < angular_tolerance)); } void PoseTracking::targetPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg) { - // Transform to planning frame + std::lock_guard lock(target_pose_mtx_); target_pose_ = *msg; - geometry_msgs::TransformStamped target_to_planning_frame; + + // If the target pose is not defined in planning frame, transform the target pose. if (target_pose_.header.frame_id != planning_frame_) { try { - target_to_planning_frame = transform_buffer_.lookupTransform(planning_frame_, target_pose_.header.frame_id, - ros::Time(0), ros::Duration(0.1)); + geometry_msgs::TransformStamped target_to_planning_frame = transform_buffer_.lookupTransform( + planning_frame_, target_pose_.header.frame_id, ros::Time(0), ros::Duration(0.1)); + tf2::doTransform(target_pose_, target_pose_, target_to_planning_frame); } - catch (tf2::TransformException& ex) + catch (const tf2::TransformException& ex) { - ROS_WARN_NAMED(LOGNAME, "%s", ex.what()); + ROS_WARN_STREAM_NAMED(LOGNAME, ex.what()); return; } - tf2::doTransform(target_pose_, target_pose_, target_to_planning_frame); } - target_pose_.header.stamp = ros::Time::now(); } geometry_msgs::TwistStampedConstPtr PoseTracking::calculateTwistCommand() { // use the shared pool to create a message more efficiently auto msg = moveit::util::make_shared_from_pool(); - msg->header.frame_id = target_pose_.header.frame_id; // Get twist components from PID controllers geometry_msgs::Twist& twist = msg->twist; + Eigen::Quaterniond q_desired; + + // Scope mutex locking only to operations which require access to target pose. + { + std::lock_guard lock(target_pose_mtx_); + msg->header.frame_id = target_pose_.header.frame_id; + + // Position + twist.linear.x = cartesian_position_pids_[0].computeCommand( + target_pose_.pose.position.x - command_frame_transform_.translation()(0), loop_rate_.expectedCycleTime()); + twist.linear.y = cartesian_position_pids_[1].computeCommand( + target_pose_.pose.position.y - command_frame_transform_.translation()(1), loop_rate_.expectedCycleTime()); + twist.linear.z = cartesian_position_pids_[2].computeCommand( + target_pose_.pose.position.z - command_frame_transform_.translation()(2), loop_rate_.expectedCycleTime()); + + // Orientation algorithm: + // - Find the orientation error as a quaternion: q_error = q_desired * q_current ^ -1 + // - Use the angle-axis PID controller to calculate an angular rate + // - Convert to angular velocity for the TwistStamped message + q_desired = Eigen::Quaterniond(target_pose_.pose.orientation.w, target_pose_.pose.orientation.x, + target_pose_.pose.orientation.y, target_pose_.pose.orientation.z); + } - // Position - twist.linear.x = cartesian_position_pids_[0].computeCommand( - target_pose_.pose.position.x - command_frame_transform_.translation()(0), loop_rate_.expectedCycleTime()); - twist.linear.y = cartesian_position_pids_[1].computeCommand( - target_pose_.pose.position.y - command_frame_transform_.translation()(1), loop_rate_.expectedCycleTime()); - twist.linear.z = cartesian_position_pids_[2].computeCommand( - target_pose_.pose.position.z - command_frame_transform_.translation()(2), loop_rate_.expectedCycleTime()); - - // Orientation algorithm: - // - Find the orientation error as a quaternion: q_error = q_desired * q_current ^ -1 - // - Use the quaternion PID controllers to calculate a quaternion rate, q_error_dot - // - Convert to angular velocity for the TwistStamped message - Eigen::Quaterniond q_desired(target_pose_.pose.orientation.w, target_pose_.pose.orientation.x, - target_pose_.pose.orientation.y, target_pose_.pose.orientation.z); Eigen::Quaterniond q_current(command_frame_transform_.rotation()); Eigen::Quaterniond q_error = q_desired * q_current.inverse(); @@ -345,6 +349,13 @@ void PoseTracking::getPIDErrors(double& x_error, double& y_error, double& z_erro cartesian_orientation_pids_.at(0).getCurrentPIDErrors(&orientation_error, &dummy1, &dummy2); } +void PoseTracking::resetTargetPose() +{ + std::lock_guard lock(target_pose_mtx_); + target_pose_ = geometry_msgs::PoseStamped(); + target_pose_.header.stamp = ros::Time(0); +} + bool PoseTracking::getCommandFrameTransform(geometry_msgs::TransformStamped& transform) { return servo_->getCommandFrameTransform(transform); diff --git a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp index 765094899d..5dab02713f 100644 --- a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp +++ b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp @@ -141,6 +141,11 @@ TEST_F(PoseTrackingFixture, OutgoingMsgTest) }); ros::Duration(ROS_PUB_SUB_DELAY).sleep(); + + // resetTargetPose() can be used to clear the target pose and wait for a new one, e.g. when moving between multiple + // waypoints + tracker_->resetTargetPose(); + tracker_->moveToPose(translation_tolerance_, ROTATION_TOLERANCE, 1 /* target pose timeout */); target_pub_thread.join(); From cf218879dbc23aadf88dd56b8abe7970b7d61030 Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Tue, 3 Nov 2020 10:40:19 +0100 Subject: [PATCH 507/612] Add Pilz industrial motion planner (#1893) * Add pilz_industrial_motion_planner to moveit_planners * Update license, document consent to this change by contributers * Fix formatting, code style, catkin_lint, clang-tidy * Add and update tests, use new prvt_moveit_config from moveit_resources * New codeowners for pilz_industrial_motion planner * Add pipeline configuration teimplates to MSA Co-authored-by: Joachim Schleicher Co-authored-by: rfeistenauer Co-authored-by: Giuseppe Sansone Co-authored-by: Immanuel Martini --- .github/CODEOWNERS | 2 + .../CHANGELOG.rst | 106 + .../CMakeLists.txt | 519 ++ .../pilz_industrial_motion_planner/README.md | 4 + .../doc/.gitignore | 12 + .../doc/MotionBlendAlgorithmDescription.pdf | Bin 0 -> 534094 bytes .../doc/MotionBlendAlgorithmDescription.tex | 114 + .../doc/README.md | 22 + .../doc/diag_class_capabilities.png | Bin 0 -> 41398 bytes .../doc/diag_class_capabilities.uxf | 413 ++ .../doc/diag_class_planning_context.png | Bin 0 -> 133824 bytes .../doc/diag_class_planning_context.uxf | 906 ++++ .../doc/figure/blend_case_1.eps | 1648 +++++++ .../doc/figure/blend_case_2.eps | 1548 ++++++ .../doc/figure/blend_case_3.eps | 1567 ++++++ .../doc/figure/blend_radius.png | Bin 0 -> 56819 bytes .../doc/figure/original_trajectories.eps | 4376 +++++++++++++++++ .../doc/figure/ptp.png | Bin 0 -> 16444 bytes .../doc/figure/ptp.svg | 860 ++++ .../doc/figure/ptp_movement_1.png | Bin 0 -> 100740 bytes .../doc/figure/ptp_movement_2.png | Bin 0 -> 117675 bytes .../doc/figure/rviz_planner.png | Bin 0 -> 177046 bytes .../doc/sequence_processing_overview.png | Bin 0 -> 187694 bytes .../doc/sequence_processing_overview.uxf | 1130 +++++ .../capability_names.h | 42 + .../cartesian_limit.h | 159 + .../cartesian_limits_aggregator.h | 65 + .../cartesian_trajectory.h | 51 + .../cartesian_trajectory_point.h | 53 + .../command_list_manager.h | 225 + .../joint_limits_aggregator.h | 150 + .../joint_limits_container.h | 163 + .../joint_limits_extension.h | 66 + .../joint_limits_interface_extension.h | 101 + .../joint_limits_validator.h | 153 + .../limits_container.h | 104 + .../move_group_sequence_action.h | 90 + .../move_group_sequence_service.h | 66 + .../path_circle_generator.h | 101 + .../pilz_industrial_motion_planner.h | 139 + .../plan_components_builder.h | 158 + .../planning_context_base.h | 181 + .../planning_context_circ.h | 67 + .../planning_context_lin.h | 65 + .../planning_context_loader.h | 144 + .../planning_context_loader_circ.h | 68 + .../planning_context_loader_lin.h | 68 + .../planning_context_loader_ptp.h | 68 + .../planning_context_ptp.h | 67 + .../planning_exceptions.h | 71 + .../tip_frame_getter.h | 90 + .../trajectory_blend_request.h | 59 + .../trajectory_blend_response.h | 57 + .../trajectory_blender.h | 74 + .../trajectory_blender_transition_window.h | 178 + .../trajectory_functions.h | 219 + .../trajectory_generation_exceptions.h | 121 + .../trajectory_generator.h | 291 ++ .../trajectory_generator_circ.h | 107 + .../trajectory_generator_lin.h | 85 + .../trajectory_generator_ptp.h | 94 + .../velocity_profile_atrap.h | 215 + .../package.xml | 55 + ...rial_motion_planner_plugin_description.xml | 5 + .../planning_context_plugin_description.xml | 18 + ...sequence_capability_plugin_description.xml | 13 + .../rosdoc.yaml | 6 + .../src/cartesian_limit.cpp | 119 + .../src/cartesian_limits_aggregator.cpp | 96 + .../src/command_list_manager.cpp | 315 ++ .../src/joint_limits_aggregator.cpp | 181 + .../src/joint_limits_container.cpp | 182 + .../src/joint_limits_validator.cpp | 157 + .../src/limits_container.cpp | 78 + .../src/move_group_sequence_action.cpp | 294 ++ .../src/move_group_sequence_service.cpp | 105 + .../src/path_circle_generator.cpp | 147 + .../src/pilz_industrial_motion_planner.cpp | 173 + .../src/plan_components_builder.cpp | 137 + .../src/planning_context_loader.cpp | 64 + .../src/planning_context_loader_circ.cpp | 75 + .../src/planning_context_loader_lin.cpp | 75 + .../src/planning_context_loader_ptp.cpp | 74 + .../trajectory_blender_transition_window.cpp | 301 ++ .../src/trajectory_functions.cpp | 584 +++ .../src/trajectory_generator.cpp | 332 ++ .../src/trajectory_generator_circ.cpp | 270 + .../src/trajectory_generator_lin.cpp | 203 + .../src/trajectory_generator_ptp.cpp | 261 + .../src/velocity_profile_atrap.cpp | 451 ++ .../acceptance_tests/acceptance_test_lin.md | 62 + .../acceptance_tests/acceptance_test_ptp.md | 54 + .../img/acceptance_test_lin_img1.png | Bin 0 -> 229896 bytes .../img/acceptance_test_lin_img2.png | Bin 0 -> 280710 bytes .../img/acceptance_test_lin_img3.png | Bin 0 -> 253603 bytes .../img/acceptance_test_lin_img4.png | Bin 0 -> 280613 bytes .../img/acceptance_test_ptp_img1.png | Bin 0 -> 163741 bytes .../img/acceptance_test_ptp_img2.png | Bin 0 -> 215064 bytes .../integrationtest_command_list_manager.cpp | 661 +++ .../integrationtest_command_list_manager.test | 68 + .../test/integrationtest_command_planning.cpp | 501 ++ .../integrationtest_command_planning.test | 62 + ...st_command_planning_frankaemika_panda.test | 63 + ...iontest_command_planning_with_gripper.test | 61 + .../integrationtest_get_solver_tip_frame.cpp | 116 + .../integrationtest_get_solver_tip_frame.test | 48 + ...ntegrationtest_plan_components_builder.cpp | 132 + ...tegrationtest_plan_components_builder.test | 65 + .../test/integrationtest_sequence_action.cpp | 628 +++ .../test/integrationtest_sequence_action.test | 59 + ...e_action_capability_frankaemika_panda.test | 65 + ...quence_action_capability_with_gripper.test | 60 + ...grationtest_sequence_action_preemption.cpp | 180 + ...rationtest_sequence_action_preemption.test | 59 + ...rationtest_sequence_service_capability.cpp | 426 ++ ...ationtest_sequence_service_capability.test | 57 + ..._service_capability_frankaemika_panda.test | 59 + ...uence_service_capability_with_gripper.test | 57 + .../test/test_robots/concept_testdata.odp | Bin 0 -> 14825 bytes .../test/test_robots/concept_testdata.png | Bin 0 -> 74304 bytes .../launch/moveit_planning_execution.launch | 77 + .../test_data/testdata_sequence.xml | 173 + .../prbt/launch/test_context.launch | 43 + .../prbt/test_data/testdata_deprecated.xml | 434 ++ .../prbt/test_data/testdata_sequence.xml | 227 + .../prbt/test_data/testdata_with_gripper.xml | 95 + .../test_cartesian_limit_all.yaml | 37 + .../test_cartesian_limit_only_vel.yaml | 34 + .../test_joint_limits_valid_1.yaml | 52 + ...est_joint_limits_violate_position_max.yaml | 37 + ...est_joint_limits_violate_position_min.yaml | 37 + .../test_joint_limits_violate_velocity.yaml | 36 + .../test/test_robots/testpoints.py | 141 + .../test/test_utils.cpp | 1430 ++++++ .../test/test_utils.h | 484 ++ .../unittest_cartesian_limits_aggregator.cpp | 92 + .../unittest_cartesian_limits_aggregator.test | 53 + .../test/unittest_get_solver_tip_frame.cpp | 145 + .../test/unittest_joint_limit.cpp | 111 + .../test/unittest_joint_limit.test | 40 + .../test/unittest_joint_limit_config.yaml | 31 + .../test/unittest_joint_limits_aggregator.cpp | 208 + .../unittest_joint_limits_aggregator.test | 58 + .../test/unittest_joint_limits_container.cpp | 229 + .../test/unittest_joint_limits_validator.cpp | 490 ++ ...nittest_pilz_industrial_motion_planner.cpp | 232 + ...ittest_pilz_industrial_motion_planner.test | 48 + ..._pilz_industrial_motion_planner_direct.cpp | 121 + .../test/unittest_planning_context.cpp | 282 ++ .../test/unittest_planning_context.test | 48 + .../unittest_planning_context_loaders.cpp | 179 + .../unittest_planning_context_loaders.test | 48 + ...t_trajectory_blender_transition_window.cpp | 727 +++ ..._trajectory_blender_transition_window.test | 65 + .../test/unittest_trajectory_functions.cpp | 971 ++++ .../test/unittest_trajectory_functions.test | 53 + .../test/unittest_trajectory_generator.cpp | 146 + .../unittest_trajectory_generator_circ.cpp | 785 +++ .../unittest_trajectory_generator_circ.test | 61 + .../unittest_trajectory_generator_common.cpp | 434 ++ .../unittest_trajectory_generator_common.test | 59 + .../unittest_trajectory_generator_lin.cpp | 470 ++ .../unittest_trajectory_generator_lin.test | 62 + .../unittest_trajectory_generator_ptp.cpp | 971 ++++ .../unittest_trajectory_generator_ptp.test | 56 + .../test/unittest_velocity_profile_atrap.cpp | 631 +++ .../CHANGELOG.rst | 84 + .../CMakeLists.txt | 88 + .../doc/README.md | 34 + .../diagrams/diag_class_circ_auxiliary.png | Bin 0 -> 24644 bytes .../diagrams/diag_class_circ_auxiliary.uxf | 133 + .../doc/diagrams/diag_class_commands.png | Bin 0 -> 9020 bytes .../doc/diagrams/diag_class_commands.uxf | 175 + .../diag_class_robot_configurations.png | Bin 0 -> 18548 bytes .../diag_class_robot_configurations.uxf | 133 + .../diag_seq_testdataloader_usage.png | Bin 0 -> 16098 bytes .../diag_seq_testdataloader_usage.uxf | 215 + .../async_test.h | 176 + .../basecmd.h | 128 + .../cartesianconfiguration.h | 188 + .../cartesianpathconstraintsbuilder.h | 90 + .../center.h | 60 + .../checks.h | 62 + .../circ.h | 106 + .../circ_auxiliary_types.h | 49 + .../circauxiliary.h | 91 + .../command_types_typedef.h | 69 + .../default_values.h | 51 + .../exception_types.h | 52 + .../goalconstraintsmsgconvertible.h | 56 + .../gripper.h | 48 + .../interim.h | 60 + .../jointconfiguration.h | 145 + .../lin.h | 67 + .../motioncmd.h | 93 + .../motionplanrequestconvertible.h | 54 + .../ptp.h | 67 + .../robotconfiguration.h | 92 + .../robotstatemsgconvertible.h | 54 + .../sequence.h | 147 + .../testdata_loader.h | 117 + .../xml_constants.h | 80 + .../xml_testdata_loader.h | 223 + .../package.xml | 29 + .../src/cartesianconfiguration.cpp | 132 + .../src/jointconfiguration.cpp | 161 + .../src/robotconfiguration.cpp | 62 + .../src/sequence.cpp | 120 + .../src/xml_testdata_loader.cpp | 555 +++ .../src/tools/moveit_config_data.cpp | 2 + .../widgets/configuration_files_widget.cpp | 23 + .../config/cartesian_limits.yaml | 5 + .../launch/chomp_planning_pipeline.launch.xml | 43 +- .../fake_moveit_controller_manager.launch.xml | 5 +- .../launch/move_group.launch | 6 +- .../launch/ompl_planning_pipeline.launch.xml | 8 + ...otion_planner_planning_pipeline.launch.xml | 28 + .../launch/planning_context.launch | 1 + .../launch/planning_pipeline.launch.xml | 11 +- .../launch/trajectory_execution.launch.xml | 6 +- .../trajopt_planning_pipeline.launch.xml | 8 + 221 files changed, 41529 insertions(+), 22 deletions(-) create mode 100644 moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst create mode 100644 moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt create mode 100644 moveit_planners/pilz_industrial_motion_planner/README.md create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/.gitignore create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.pdf create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.tex create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/README.md create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.uxf create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_1.eps create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_2.eps create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_3.eps create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_radius.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/figure/original_trajectories.eps create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp.svg create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp_movement_1.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp_movement_2.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/figure/rviz_planner.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/sequence_processing_overview.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/doc/sequence_processing_overview.uxf create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limit.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/package.xml create mode 100644 moveit_planners/pilz_industrial_motion_planner/plugins/pilz_industrial_motion_planner_plugin_description.xml create mode 100644 moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml create mode 100644 moveit_planners/pilz_industrial_motion_planner/plugins/sequence_capability_plugin_description.xml create mode 100644 moveit_planners/pilz_industrial_motion_planner/rosdoc.yaml create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/cartesian_limit.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img1.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img2.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img3.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img4.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_ptp_img1.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_ptp_img2.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.odp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.png create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_deprecated.xml create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_sequence.xml create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_with_gripper.xml create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_all.yaml create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_only_vel.yaml create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_valid_1.yaml create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_max.yaml create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_min.yaml create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_velocity.yaml create mode 100755 moveit_planners/pilz_industrial_motion_planner/test/test_robots/testpoints.py create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_utils.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit_config.yaml create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_validator.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner_direct.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.test create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/unittest_velocity_profile_atrap.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/doc/README.md create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.png create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.uxf create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.png create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.uxf create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.png create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.uxf create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.png create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.uxf create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/package.xml create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/src/robotconfiguration.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp create mode 100644 moveit_setup_assistant/templates/moveit_config_pkg_template/config/cartesian_limits.yaml create mode 100644 moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index b21c6f4486..6175b31cf6 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -100,3 +100,5 @@ /moveit_planners/chomp/chomp_optimizer_adapter/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/chomp/chomp_motion_planner/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/trajopt @ommmid @mamoll +/moveit_planners/pilz_industrial_motion_planner @jschleicher @ct2034 +/moveit_planners/pilz_industrial_motion_planner_testutils/ @jschleicher @ct2034 diff --git a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst new file mode 100644 index 0000000000..1acf7f0272 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst @@ -0,0 +1,106 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pilz_industrial_motion_planner +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.4.10 (2019-12-04) +------------------- + +0.4.9 (2019-11-28) +------------------ + +0.4.8 (2019-11-22) +------------------ + +0.4.7 (2019-09-10) +------------------ +* Fix clang-tidy issues +* integrate clang-tidy via CMake flag +* Contributors: Pilz GmbH and Co. KG + +0.4.6 (2019-09-04) +------------------ + +0.4.5 (2019-09-03) +------------------ +* Adapt to changes in pilz_robots +* add static code analyzing (clang-tidy) +* drop deprecated isRobotStateEqual() +* Contributors: Pilz GmbH and Co. KG + +0.4.4 (2019-06-19) +------------------ +* fixed an error that led to trajectories not strictly increasing in time +* Contributors: Pilz GmbH and Co. KG + +0.4.3 (2019-04-08) +------------------ +* update dependencies of trajectory_generation +* fix CIRC path generator and increase test coverage +* adopt strictest limits in ptp planner (refactor JointLimitsContainer and TrajectoryGeneratorPTP) +* Enable gripper commands inside a sequence +* Contributors: Pilz GmbH and Co. KG + +0.4.2 (2019-03-13) +------------------ +* re-adapt to new RobotState API: remove #attempts +* Contributors: Pilz GmbH and Co. KG + +0.4.1 (2019-02-27) +------------------ + +0.3.6 (2019-02-26) +------------------ +* refactor the testdataloader +* adapt to new RobotState API: remove #attempts +* Contributors: Pilz GmbH and Co. KG + +0.3.5 (2019-02-06) +------------------ +* Increase line coverage for blending to 100% +* refactor determining the trajectory alignment in the blend implementation +* extend and refactor unittest of blender_transition_window +* add planning group check to blender_transition_window +* add more details to blend algorithm description +* change handling of empty sequences in capabilities to be non-erroneous +* rename command_planner -> pilz_command_planner +* use pilz_testutils package for blend test +* use collision-aware ik calculation +* Contributors: Pilz GmbH and Co. KG + +0.4.0 (2018-12-18) +------------------ +* Use Eigen::Isometry3d to keep up with the recent changes in moveit +* Contributors: Chris Lalancette + +0.3.1 (2018-12-17) +------------------ + +0.3.0 (2018-11-28) +------------------ +* add append method for avoiding duplicate points in robot_trajectory trajectories +* Relax the precondition on trajectory generators from v_start==0 to |v_start| < 1e-10 to gain robustness +* Set last point of generated trajectories to have vel=acc=0 to match the first point. +* add sequence action and service capabilities to concatenate multiple requests +* Contributors: Pilz GmbH and Co. KG + +0.2.2 (2018-09-26) +------------------ + +0.2.1 (2018-09-25) +------------------ + +0.1.1 (2018-09-25) +------------------ +* port to melodic +* drop unused dependencies +* Contributors: Pilz GmbH and Co. KG + +0.2.0 (2018-09-14) +------------------ +* Changes for melodic +* Contributors: Pilz GmbH and Co. KG + +0.1.0 (2018-09-14) +------------------ +* Created trajectory generation package with ptp, lin, circ and blend planner +* Contributors: Pilz GmbH and Co. KG diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt new file mode 100644 index 0000000000..7c3f86c8a1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -0,0 +1,519 @@ +cmake_minimum_required(VERSION 2.8.3) +project(pilz_industrial_motion_planner) + +## Add support for C++11, supported in ROS Kinetic and newer +add_definitions(-std=c++11) +add_definitions(-Wall) +add_definitions(-Wextra) +add_definitions(-Wno-unused-parameter) + +find_package(catkin REQUIRED COMPONENTS + eigen_conversions + kdl_conversions + moveit_core + moveit_msgs + moveit_ros_move_group + moveit_ros_planning + moveit_ros_planning_interface + pluginlib + roscpp + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros +) + +find_package(orocos_kdl) +find_package(Boost REQUIRED COMPONENTS ) + +if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) + find_package(code_coverage REQUIRED) + APPEND_COVERAGE_COMPILER_FLAGS() +endif() + +################ +## Clang tidy ## +################ +if(CATKIN_ENABLE_CLANG_TIDY) + find_program( + CLANG_TIDY_EXE + NAMES "clang-tidy" + DOC "Path to clang-tidy executable" + ) + if(NOT CLANG_TIDY_EXE) + message(FATAL_ERROR "clang-tidy not found.") + else() + message(STATUS "clang-tidy found: ${CLANG_TIDY_EXE}") + set(CMAKE_CXX_CLANG_TIDY "${CLANG_TIDY_EXE}") + endif() +endif() + +################################### +## catkin specific configuration ## +################################### +catkin_package( + CATKIN_DEPENDS + moveit_msgs + roscpp + tf2_geometry_msgs +) + +########### +## Build ## +########### +include_directories( + include +) + +# To avoid Warnings from clang-tidy declare system +include_directories( + SYSTEM + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME}_lib + src/${PROJECT_NAME}.cpp + src/planning_context_loader.cpp + src/joint_limits_validator.cpp + src/joint_limits_aggregator.cpp + src/joint_limits_container.cpp + src/cartesian_limits_aggregator.cpp + src/cartesian_limit.cpp + src/limits_container.cpp + src/trajectory_functions.cpp + src/plan_components_builder.cpp +) + +target_link_libraries(${PROJECT_NAME}_lib + ${catkin_LIBRARIES} +) + +############# +## Plugins ## +############# +add_library(${PROJECT_NAME} + src/${PROJECT_NAME}.cpp + src/planning_context_loader.cpp + src/joint_limits_aggregator.cpp + src/joint_limits_container.cpp + src/limits_container.cpp + src/cartesian_limit.cpp + src/cartesian_limits_aggregator.cpp + ) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES}) + +add_library(planning_context_loader_ptp + src/planning_context_loader_ptp.cpp + src/planning_context_loader.cpp + src/trajectory_functions.cpp + src/trajectory_generator.cpp + src/trajectory_generator_ptp.cpp + src/velocity_profile_atrap.cpp + src/joint_limits_container.cpp + ) +target_link_libraries(planning_context_loader_ptp + ${catkin_LIBRARIES}) + +add_library(planning_context_loader_lin + src/planning_context_loader_lin.cpp + src/planning_context_loader.cpp + src/trajectory_functions.cpp + src/trajectory_generator.cpp + src/trajectory_generator_lin.cpp + src/velocity_profile_atrap.cpp + ) +target_link_libraries(planning_context_loader_lin + ${catkin_LIBRARIES}) + +add_library(planning_context_loader_circ + src/planning_context_loader_circ.cpp + src/planning_context_loader.cpp + src/trajectory_functions.cpp + src/trajectory_generator.cpp + src/trajectory_generator_circ.cpp + src/path_circle_generator.cpp + ) +target_link_libraries(planning_context_loader_circ + ${catkin_LIBRARIES}) + +add_library(command_list_manager + src/command_list_manager.cpp + src/plan_components_builder.cpp) +target_link_libraries(command_list_manager + ${catkin_LIBRARIES}) +add_dependencies(command_list_manager + ${catkin_EXPORTED_TARGETS}) + +add_library(sequence_capability + src/move_group_sequence_action.cpp + src/move_group_sequence_service.cpp + src/plan_components_builder.cpp + src/command_list_manager.cpp + src/trajectory_blender_transition_window.cpp + src/joint_limits_aggregator.cpp # do we need joint limits and cartesian_limit here? + src/joint_limits_container.cpp + src/limits_container.cpp + src/cartesian_limit.cpp + src/cartesian_limits_aggregator.cpp + ) +target_link_libraries(sequence_capability + ${catkin_LIBRARIES}) +add_dependencies(sequence_capability + ${catkin_EXPORTED_TARGETS}) + +############# +## Install ## +############# +install(FILES + plugins/sequence_capability_plugin_description.xml + plugins/${PROJECT_NAME}_plugin_description.xml + plugins/planning_context_plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/plugins + ) + +## Mark libraries for installation +install(TARGETS + ${PROJECT_NAME} + planning_context_loader_ptp + planning_context_loader_lin + planning_context_loader_circ + command_list_manager + sequence_capability + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +############# +## Testing ## +############# +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + find_package(${PROJECT_NAME}_testutils REQUIRED) + + include_directories( + ${${PROJECT_NAME}_testutils_INCLUDE_DIRS} + ) + + if(ENABLE_COVERAGE_TESTING) + include(CodeCoverage) + APPEND_COVERAGE_COMPILER_FLAGS() + endif() + + ######################### + ####Integration Tests#### + ######################### + set(${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES + sequence_capability + ${PROJECT_NAME} + planning_context_loader_ptp + planning_context_loader_lin + planning_context_loader_circ + ) + + # Planning Integration tests + add_rostest_gmock(integrationtest_sequence_action_preemption + test/integrationtest_sequence_action_preemption.test + test/integrationtest_sequence_action_preemption.cpp + ) + target_link_libraries(integrationtest_sequence_action_preemption + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + add_rostest_gmock(integrationtest_sequence_action + test/integrationtest_sequence_action.test + test/integrationtest_sequence_action.cpp + ) + target_link_libraries(integrationtest_sequence_action + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + add_rostest(test/integrationtest_sequence_action_capability_with_gripper.test + DEPENDENCIES integrationtest_sequence_action + ) + + add_rostest(test/integrationtest_sequence_action_capability_frankaemika_panda.test + DEPENDENCIES integrationtest_sequence_action + ) + + add_rostest_gtest(integrationtest_sequence_service_capability + test/integrationtest_sequence_service_capability.test + test/integrationtest_sequence_service_capability.cpp + ) + target_link_libraries(integrationtest_sequence_service_capability + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + add_rostest(test/integrationtest_sequence_service_capability_frankaemika_panda.test + DEPENDENCIES integrationtest_sequence_service_capability ) + + add_rostest(test/integrationtest_sequence_service_capability_with_gripper.test + DEPENDENCIES integrationtest_sequence_service_capability + ) + + # Planning Integration tests + add_rostest_gtest(integrationtest_command_planning + test/integrationtest_command_planning.test + test/integrationtest_command_planning.cpp + test/test_utils.cpp + ) + target_link_libraries(integrationtest_command_planning + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + add_rostest(test/integrationtest_command_planning_with_gripper.test + DEPENDENCIES integrationtest_command_planning + ) + + add_rostest(test/integrationtest_command_planning_frankaemika_panda.test + DEPENDENCIES integrationtest_command_planning ) + + # Blending Integration tests + add_rostest_gtest(integrationtest_command_list_manager + test/integrationtest_command_list_manager.test + test/integrationtest_command_list_manager.cpp + test/test_utils.cpp + ) + target_link_libraries(integrationtest_command_list_manager + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + add_rostest_gtest(integrationtest_get_solver_tip_frame + test/integrationtest_get_solver_tip_frame.test + test/integrationtest_get_solver_tip_frame.cpp + ) + target_link_libraries(integrationtest_get_solver_tip_frame + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + add_rostest_gtest(integrationtest_plan_components_builder + test/integrationtest_plan_components_builder.test + test/integrationtest_plan_components_builder.cpp + ) + target_link_libraries(integrationtest_plan_components_builder + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + ################## + ####Unit Tests#### + ################## + add_library(${PROJECT_NAME}_testhelpers + test/test_utils.cpp + src/trajectory_generator.cpp + src/trajectory_generator_circ.cpp + src/trajectory_generator_lin.cpp + src/trajectory_generator_ptp.cpp + src/path_circle_generator.cpp + src/velocity_profile_atrap.cpp + ) + + target_link_libraries(${PROJECT_NAME}_testhelpers ${PROJECT_NAME}_lib) + + ## Add gtest based cpp test target and link libraries + catkin_add_gtest(unittest_${PROJECT_NAME}_direct + test/unittest_${PROJECT_NAME}_direct.cpp + src/planning_context_loader_ptp.cpp + ) + target_link_libraries(unittest_${PROJECT_NAME}_direct + ${catkin_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + ## Add gtest based cpp test target and link libraries + catkin_add_gtest(unittest_velocity_profile_atrap + test/unittest_velocity_profile_atrap.cpp + src/velocity_profile_atrap.cpp + ) + target_link_libraries(unittest_velocity_profile_atrap + ${catkin_LIBRARIES} + ) + + catkin_add_gtest(unittest_trajectory_generator + test/unittest_trajectory_generator.cpp + src/trajectory_generator.cpp + ) + target_link_libraries(unittest_trajectory_generator + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # Trajectory Generator Unit Test + add_rostest_gtest(unittest_trajectory_functions + test/unittest_trajectory_functions.test + test/unittest_trajectory_functions.cpp + ) + target_link_libraries(unittest_trajectory_functions + ${catkin_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # unittest for trajectory blender transition window + add_rostest_gtest(unittest_trajectory_blender_transition_window + test/unittest_trajectory_blender_transition_window.test + test/unittest_trajectory_blender_transition_window.cpp + src/trajectory_blender_transition_window.cpp + ) + target_link_libraries(unittest_trajectory_blender_transition_window + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # trajectory generator Unit Test + add_rostest_gtest(unittest_trajectory_generator_common + test/unittest_trajectory_generator_common.test + test/unittest_trajectory_generator_common.cpp + ) + target_link_libraries(unittest_trajectory_generator_common + ${catkin_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # trajectory generator circ Unit Test + add_rostest_gtest(unittest_trajectory_generator_circ + test/unittest_trajectory_generator_circ.test + test/unittest_trajectory_generator_circ.cpp + ) + target_link_libraries(unittest_trajectory_generator_circ + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # trajectory generator lin Unit Test + add_rostest_gtest(unittest_trajectory_generator_lin + test/unittest_trajectory_generator_lin.test + test/unittest_trajectory_generator_lin.cpp + ) + target_link_libraries(unittest_trajectory_generator_lin + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # trajectory generator ptp Unit Test + add_rostest_gtest(unittest_trajectory_generator_ptp + test/unittest_trajectory_generator_ptp.test + test/unittest_trajectory_generator_ptp.cpp + ) + target_link_libraries(unittest_trajectory_generator_ptp + ${catkin_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # Command Planner Unit Test + add_rostest_gtest(unittest_${PROJECT_NAME} + test/unittest_${PROJECT_NAME}.test + test/unittest_${PROJECT_NAME}.cpp + ) + target_link_libraries(unittest_${PROJECT_NAME} + ${catkin_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # JointLimits Unit Test + add_rostest_gtest(unittest_joint_limit + test/unittest_joint_limit.test + test/unittest_joint_limit.cpp + ) + target_link_libraries(unittest_joint_limit + ${catkin_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # JointLimitsAggregator Unit Test + add_rostest_gtest(unittest_joint_limits_aggregator + test/unittest_joint_limits_aggregator.test + test/unittest_joint_limits_aggregator.cpp + ) + target_link_libraries(unittest_joint_limits_aggregator + ${catkin_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # JointLimitsContainer Unit Test + catkin_add_gtest(unittest_joint_limits_container + test/unittest_joint_limits_container.cpp + ) + target_link_libraries(unittest_joint_limits_container + ${catkin_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # JointLimitsValidator Unit Test + catkin_add_gtest(unittest_joint_limits_validator + test/unittest_joint_limits_validator.cpp + ) + target_link_libraries(unittest_joint_limits_validator + ${catkin_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # Cartesian Limits Aggregator Unit Test + add_rostest_gtest(unittest_cartesian_limits_aggregator + test/unittest_cartesian_limits_aggregator.test + test/unittest_cartesian_limits_aggregator.cpp + ) + target_link_libraries(unittest_cartesian_limits_aggregator + ${catkin_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # PlanningContextLoaderPTP Unit Test + add_rostest_gtest(unittest_planning_context_loaders + test/unittest_planning_context_loaders.test + test/unittest_planning_context_loaders.cpp + ) + target_link_libraries(unittest_planning_context_loaders + ${catkin_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # PlanningContext Unit Test (Typed test) + add_rostest_gtest(unittest_planning_context + test/unittest_planning_context.test + test/unittest_planning_context.cpp + src/planning_context_loader_circ.cpp + src/planning_context_loader_lin.cpp + src/planning_context_loader_ptp.cpp + ) + target_link_libraries(unittest_planning_context + ${catkin_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # GetTipFrames Unit Test + catkin_add_gmock(unittest_get_solver_tip_frame + test/unittest_get_solver_tip_frame.cpp + ) + target_link_libraries(unittest_get_solver_tip_frame + ${catkin_LIBRARIES} + ) + + # to run: catkin_make -DCMAKE_BUILD_TYPE=Debug -DENABLE_COVERAGE_TESTING=ON package_name_coverage + if(ENABLE_COVERAGE_TESTING) + set(COVERAGE_EXCLUDES "*/${PROJECT_NAME}/test*") + add_code_coverage( + NAME ${PROJECT_NAME}_coverage + # specifying dependencies in a reliable way is on open issue + # see https://github.com/mikeferguson/code_coverage/pull/14 + #DEPENDENCIES tests + ) + endif() +endif() diff --git a/moveit_planners/pilz_industrial_motion_planner/README.md b/moveit_planners/pilz_industrial_motion_planner/README.md new file mode 100644 index 0000000000..632224f5ec --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/README.md @@ -0,0 +1,4 @@ +Please consult tutorials and the official [documentation](https://moveit.ros.org/documentation/concepts/). + +For details about the blend algorithm please refer to +![doc/MotionBlendAlgorithmDescription.pdf](doc/MotionBlendAlgorithmDescription.pdf). diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/.gitignore b/moveit_planners/pilz_industrial_motion_planner/doc/.gitignore new file mode 100644 index 0000000000..2edfaf4621 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/.gitignore @@ -0,0 +1,12 @@ +*.aux +*.bbl +*.blg +*.dvi +*.log +*.ps +*.tps +*.fdb_latexmk +*.fls +*.out +*.synctex.gz +*.pdf \ No newline at end of file diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.pdf b/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.pdf new file mode 100644 index 0000000000000000000000000000000000000000..7a3f127e884496614820c3f9e8098850d918137f GIT binary patch literal 534094 zcmeFZbzGERyDlykiiCoK2*_9t5G=-El|qk>Gs>T4t;}tsT|hu%J)~J;X#y zAB&3%i%tw?tZVrk!ps1L{P@A56Vx-bgqcI=1PwJUVUJ+iCOR-I9v&ANacx3&r+L2&l+k1` zjE*k28rAarDu>^5CS@LI0{6M;5azS3S9AL_uN(~eW-K;5K3`9S%I&LJNKm0GmCl5{ zx%loqmEwzFciA>KJm}~Bcc{|9yZ1{u1KXCH;D*Jbb&^`;_vaX!L|+!C1%~v#=f!K*|xc}Ldz4Cw!E4!(ENhOfkjUz z%Yc=UoU(cU@?<^5IJR^VxmI5gbSa}muzs73@phBw+l?NEwFDk$Eiruu3`Tk|z|zSi z@h}CeSE??>$CIZs@k(hGLlAkPVf@TWRX=k1tZ4;)Q=qfhg*%eVdWu{`!p7x(n(3B& zb?<^-@MTR%CU?Cwq7pvy0xMFLV#-xyNxFR~lKXP$TLF%85_W8iSDB9lP?U)x%+`I3 zDNJ3c?p|r-bn(*Hxg8ixBPQ8++dJyBZ8xY!1|N0bimp%|7%f>Su-nieA*@!7F0f!NZwFkz>J( zb$*Hh_zf*aOvlI*l%1a8KPpe1%Aywi`Iky&%c+7PrQe&p! zbzqNcR@{L36DaPlPoFKD)QBY(?V=vf^f>(>`fE!X$x@+Lmi%JV%^RLt`etT1^zYIY zXqn)TgfsFL=y(K6$Ex#I?@V&-q;u#5;we2rW5u@JB?wv?svYPnHGy5H_ zSF+0F_@pNOV|=PYk*k#M=Dg>pGJ+eNnG_=&e#K&V0{UnZ-Yk4>0V(ugaR#jaK z&NXnh;$P!?|H#7a)@>=z6BTcSAJv9)H=MbIrNHoRn>*M{gMHYCa6oH*pDVns%5sk$ z(~0;KFCPlO-s4%VhRGQu!OpUdwZpx3e$E zM;P8SVAa{5Z3*DG@4$+mHI&i{N^pL^nu>qIJhL80(rw3=Sze;l{r&Rl$xba;p zV4GWPtUM;HNJYNcwP4@ywMiKCAQRc|m#t^5Z2BqHB_MYoU@^+eHo*QMB4|IR$nqK= z@vRE#X<>fpcQ`i-QnRc!9K-}dQ!)eujY^vO2`--hiaXu$)qCK*r*m)TRqwgHc?y$V zR&Awd-EV`UhQeH(tunezsn1b4hIM{CI{gKLfe*qngDd~t|ctgTl| z>3cKek65+8dR-iN``~~+cx7+;@}-zdn=&1*SmM{HWJt594p?zL?o@A?@rYa>2^hQO z(7s21UW>k-o&q;TJ4(E_gCOzSbM@74M$a-hPZN~wTU9KUoj+eyi$U?KmEfzs&!d`t zGwG`ql-Y~y4)M2rCmCNT>pU+>n7&(CyIRZ!>2{taLIpers*77q1p72upmK|4)%6GL zLYOg-9EL%hm?7qS1wCW5UtY6d_eeoy61jbm2M>I`NYa<9B*}{!@4XGPtFdq}wtd9s z?8+!?KehS1{x_s@4UWonT8S5l9cPl)-4^^kr**ooXCr!*qela?1(m(u#tj{x(LM_oIl%|_d@~qBlK8fQd4pi9SKLflu8-@! zO3W(crhev0l@M7vw@{&0?NbWFE^kIKM4ohJu)8;^wWxYN@v*JFlkO*4)hLxi)1B~y zhAop^7yIf;aq(2gE#U`|w=v+(WxhU7&z)l6vxA128FpI>KD@i0d&^Do zG6T|_N#FG5#6`o8D?D@-rH zJmYza>s`$&FNx@-z@aQc&QDvhwW68{rxmj@UVBw42c;M2P|gQ(kBD#Yao#Bp^u{$C zaZr34_90p46lprtiuYFM!unS^k6;HMGL+CVNf6ViyoHL3jDBQIGlD!==iel5$ZL!@ zc8y-^R*@?yZcHxqpiDY=DI{LaxGAlh;D%*Fn?{Y>Tx*;Wdfo4n$vn;m9Iw_;YJMN6lZ{Ml?1fvDAQb^XQ#l8Z`-Z4xDC z-{D9VQQwz)c?t4*-fF}g-+;)T&@W?(W|VEmFGw*Wmxr=XZq-q0(20*?i*3aILHYO; zbz6M`1NYbmwllro%;X$Htc5K*V;#-vn5pdAr03qm;lj%OgzF}idmMB^Ni_o59#d3) z>>0r2AG|^v?}*if@#fR_Q002pQ}R!4kGH6V7L;Wyr?O$(6RBoeTnWX$J+cpJ7k++9 z-Z(7Wq&nAxm_y&t;#}I2F=w9qIb-#>q$^`lIn|$r`2#`(7pXThu(rhdxRe|e>N*9| zao}q*#p~*^A8*A_2u0zJ)#Y0(EyQv9RZq4IeoPxMuvvaMT8(M=A|xaH)djz&pz83ybzam!Of%qBhz>?n zjZ|*!pT+GjE!7Nj%EdBAeMqrR6+RJ#dT@SE-bfBb%u<3A3-8R2J|S+L9zA){dv@Nn zLq(R^gPW@&G&aqlTeMU zMLTw)M7*-Pr4>=B2GgHad?p-OOS`L=LTbwh5w}$ggrc*UYE^c62Znbh`~5Pq7{s%i*U_FnO1S=CsY*O%=a5Xmj9FV~aL^l)E($m%^~8L?Qg84nr6aW23; zFEy)1FqcqxO)m5q>l3zwK>J9Z7IB@4k0_`@xwprR*j*yOcE)!LYQ1x1r(YJ@H4HoJ zzlz0lE4=Bjy>hCjF4E6?j>-fRLZs?5&v&ZG9;a#M?7d{OT&>Z<)qJswy^{k0ZnxPJ zl*^SZaBkFX+$FDwHj_0IrnZ>Xa{_uDf z>3&5==mx}ysU~Rb1y)o(swshAQvE_jPew0gN#C3I&0`^Lmn6pcX*f=}LDr+EMzqs) z?coI%@M*SNk%kI-QNtc?#3N2CC7W^(+AIEf90G&>Jb~2?I;8lY-&BvzbTL$M1%L5H z?CB1i+|1jE{cwFun3)?2zxF^ShhHpjBW-`|oZv~VyC)vz$t>Rwz861SJ`wV@b5_hI z^@7C1e9C3#_uC^ADIyisdpA!c8%M5pN@>KTU{<8jBwV|faFfY@f8xIN!^R~yEaBzk zs#n&ZUsjObzg9$H_R20@Tj$mS_O1|ef_PG@SjX~dU)2{l?nJTRsEfu_&m!Hm5sbY# z|161b>N`rQE#K~o*Lt;uIG=HhCx*+C|shuedLZ=|1r4Q4##G;e2(y~1Ki=c_Q5wQQjzlTpSV9`lF6;PlB z1|dvb(+CE+OH21mPuI#EMrUHKr>keIX{c^#u8D30{2yj4o+4YM$@GS`M#K+yK8q`8T<49pUu450&7Erd?Q2(-%&0nZ88S_;Wn0+aRdIcZk# z`eCa)JV!JA@dXn*gqih6f6`2B5N5Wc-@zN~N58W`SkS`&W0GcKhOjUk{mukoL0dbA ze`kcSFdqF5g|ILk{muYk`8gkHCVB|Vk5SM`GqOWij{0R}gRmU+%g730IqH{@1;TpN zFW52Gqkb8gAgo9IGBQF~kNRbVLRgRbWn_S`9`(ye4`Ds(7s?J{J?aJL(ru2y93FLKz@zNBu(SA#6weGO$C~j{0R_ zgRmX-%fJd@JL;E#1;T#RF9S1#{it6CCJ6gczYL5J_M?6QOTm8BF9QRF{it6CdI_2+t0Y86m9W0D2|DPmBcuJ6H=F=P4?vuJ#l24p})!mQN&uNAcJosKo zepCGpB^NIH>B5}k>y|{juXY_pc2tRR8p5MC_O|<$N0<}y)G@#0jjCWQb=@Uz{Su|_ zlU7^UKzP#gt6N-FO~_QtYlRld_2t@S>A)`HqQLcj)$l^wS7*`_qHdfIxdYLgs=Dn) zjjByX&bWy8?aB5o-Z3MRk>n}(Bz9eNW+q_ASsAYqf2X5DizAJe`ZK@R+dcCW?|RiZ zII3eyM1>v@W%8)nk!aSvyqJMCP1;`L&Zm2t$yGn0U(elB@=2MskQyvLtJ7=#b}z-a zKG%hs_gaLE54D37Z(Xas?ypDEn1yvCEc-z~*ZGyVSyQ_KY@tufh^3-R!;zImIsP>G zT^=8D#LE;HvZ3O7LTW*#&1%VpsEYb36bJ3Iwg+p`KA|ku;T;izb5BYNEaPKn3|na@ z`zsn`A4_vIbn=C=L=Qi4CcNAJ_yJOTDDeu*Y+J8VY4z!Ar`822nZL@Gsa#O@8^+)y zrMy=10Gd+xVCPMOLRlC>vL6JjOuMm2|1j*C!PR0&)k+OV0zYq^RKojqL7M zW~NN6JF{KSwgpQzF-QjQ<~bFlTOj(^-B4UrQVTnwoB_V6F6_>yl6)g{e%IZH>gtHF z`$JhDYCbGFeiK6za~V@jZJ^N~!L0SPVbVf;SakAwI)K)LGO}UO3B&YspIZVY02HjA zrG+HSoZrOA)WjHOYzYBhA4m6rme91Hq^7yik=ljn08MQIlsEHXpH@c37C)Ll9DoIc z5zP@E(TPm-Y=5N;!{@uojZR8keDu%+dhtxT=?%*Ki`e*85180aDr;QrHI|RqY~8WH zHg8@yz%#t494m%NCBxSc=m)(MD|Y_>{I3dfNwW5*9H;Gz2e;#rcCXE@@9YarisKb0 z2CDNn5X6!~#?MRR`mLO%!W6c98F(s|>tO?ibR=Z_1eJuJ`HMhGVfF{;$I=IkeQUMt z1xTncNpU>}jNxiu1&@93KiJ-HkBwvmZld<4KDd0hhbcO9cpl7#Dzk@V5;XHi8VWY-5=zIv3Zz50y`} zr@#lT6dVJ5z`r2Dzb`~PVBn+d&I}ZM4C7l7gAY^BAFn<7?X{(|Fc@qlEiKJNHtO84 z5xNP1)ybA9dQHCdnNEoajw_chuMJBKR}-o4e)pnt-Wu^(7$`I#pRIU(!C`ZOjOn>w z&h|`FTB<_E_rk@J*@42mnTImc(sC(ER--jIJ%(M$idtIVt2_zTea`K+OM;QU?akJ+ zm}m_3_xC4ed=?uY&$C5ft6{ahG*ZcJzjl?wa*8s_QEOoJ#EiL_nOULrJl2H^QBE5@ zQ&Z0qnhm;=*`0T{-dwsBd|x@F0u?zq>&*pKb#?V(-H5=zY>o1hC~|>R)k4Bcm)-{iWSI<= zG)HjKkP&SOeFTe}{f>my`0FPDPMZalQs?6A0&fyl+^h6uolEPp-EoSKlS)c4Jw^^Wt*6soGj7DJab01$X1GCzIMzb z;LDTn13k5h4Raa2@PQO-LFM&k?0CdKQ$25CJd{U2%N+32nuAn8i` z3vEn5M=PtVVN80Nx-F5z!^3lPhI&z))^qt5lRodbcb6)^e*L<__#y|QT%uq7_KJL( zT8dPR;DZNcV4ugaQ&dxw^O`x>6oBAP^!VBIf+K zPmArXdRlh3S4mh5rF|qpdu#0yJmWaBSPNY#%9Rxr_UkjuhTqb`*7v2U^4Khdb6sM$up>N$i5p0n zX*u0SKq-Ekg5q&rE^jvJmoHzQFAbNQ=QA602|v%QTr52pTpf#>(tY;Kc&MaUz0{ea zB%q9!44k~^@6(^71`!sM%cHf*+0So;Sxh!ZKw)p47_OKUExUIN88|RQE(cDiJSmgGBC6MHoSe#!(*sq1PO-T+*QhT!`JRi*L1rdY zU47`G+gYRj{I8k zph6%DZ@j%D=rt=G7K7@^vvPUH16Y$O%>_F3{##qNeT_b3oNv9n#slY~Vsdnq zd#q&O4r@~*larJZLp#J}&yd0Q85kJoik2!qFdG=I5)%`fSB~7h?p#z8|0FEZW#3M3 z+XI&@(}jq4+?#mjW3S#2q?32SxQ!iCZlDn_x+IA*lp=$4e znwpxhu&{!{w^)&YxkX-=eaFX--*!k&`|wy>A*85CKohCwcNquZL7Y5ug z@gg~G#BrU5%P^)}f$U{mQDn91`RwTWDuuAGzr?9f(EHkbEVIKymCccD;o&@|6DlKQ&K?((;9-+&5WkFezR z3k{_r>*zecwm77&6yvL^S^|z8<*N9so`^_&AeBs8dwa^zYLZ&9J(&F4w{O7#=T1OM zd#O4-7tGP`JbCIAood0@OzZWo@osgOJvTg_=WVf`V(#wl1)--U6~R66LKCQfslGfD zIIU_dgoBI8Zbc^{Qpoo@4jx{q^}N!92QSv@z;xIKd{G#){1F4$MqVkjsO@b#=wf!H zQYlw=l02QAVABWYfYiC!+1YfliYKbHG&BJm`zuJgG!;bKzCu?a5@;)+B2sZxHC2Nn zcsKhk!dOifIuZg-kB{K%o`{SAiWZv$s#6C>X@s)8;LsHoJzDE~uvCc$lc5Go|NUHb zF)sa%F9U@(X{v>}V0nzsmh8;XB$V#&@0T9z7w*LXp+w&lI5;_kx4)&SrYdGVd%e2Z zB{Q7~hL3&W0&2ZmeLhO`>C-^RkCayIH*bCdH&m_w5la`z7}(U{$?t2esSDXeWMsvl z`H+ySJ)Y|qUY=+~->_l@w#!;Eg5I$cY6X_{-QC?IUvCNdWE=I#6~tJ|OO|A(>gu?iV(BYNRXd=ztY$i>?%nGzva_(X%j5!@u~?ChKSU-iO#d3NqHFnrx8FSE0= z14|FC=IrZEa~ZX5ZEc2&cjVb1I11MXz)8%pTZ}5V*h+G~(#)CiS;&W!7>F29#3@Zc zX*7p2QbHgPIP&%=MU)p`BfazU^VilaJyb=at~2XDm6Ho*Rsl1w51{-UAKxqI{xp|X zncQ`9hD2&4YV_>z4RC^^XM%!*ud|s(1LZs4n^WlP3dCB8<_@?VpBwu*R96Mlxt)-Fa z>1kOf!a?_6x^2E8J(IbqP++O&=;$aL^*ZmyFLSVX6Id|Eov*}n+?3_LjvLx{{jXq2 zew(w({bLt1N#NJDM^D-;=lOA~nM!!vtgAae9k-nS;X&u4l980+;a>p$D7IgJ7k=7G z@zn2A%SR<3U%!5R>MXXfs3;n(Cxu$i_YUNnRqjUu==|o*xzCjyFq~tiNv#TBU0DGd zPVMQ_?B`!^%3C4w^Vy(KC@n257gxXaXUlPS91@O*&$I@^zptIVXMrQYK6l3X`1yTF z*}uor0=Ps$LGcU*L(J6TenDP4W}>p6Xw^#`5fV{6+O5h^(-JBGZuypdk4@ zaP4gIe;I3Mk!oS{o!davTRVa?NFbxSb z47fIcCgibQiZ-;KYSjc*3D7iFIl42MXZLlx{}cfC`QCAYE3%!G7`BExmQi}`$zj*` z64hT@Apo3)G6LMXw6yg7`}Z>!2!%V>%w=~4H0f##I%B`w4Jt6aUwoCmZ37Y+#$~qx3?CMgL6t&lqtBeNzlaot6r6ck ziMd`wRSjJFz|YtM2_5!2I~en-p*pz#K{L$?B5{QCP57GNDz=lu$jUH^JUKRix{HsG|9x?B(b@m_M{my;%&~)|{OjS>$G1`>NRtitf*kZdrgbn^ zo6)}>g`ea0ESMYko3x8`z@8MzJu#PqAK=|jvQRsQGC_0M{}g5NS8|S#f&Tv@I|rpd zWb2M1`oibYoQV}l*)nrdy?LYPSB;qWR9b|=PxPt%^Yn{?SYR?SFFsJ|joqbs{Bpuu z(J2sQM?3#^Kyav`vGe5hmJ2tVj z?}1;X2cASEm)$^t)s-t(R??kN8vq*bp*GXZwWV_{SH|kSXbZjoWCpCt{mmhlG!A-$ zSW*Il)rl|@G^TE@1jyEEzSpWTgkgPs7hvfY5|g6$a5$^sH+ps+U_S;JAi>CK3+FP+ zyhVEau{AO>GNOQq1WXK=Z5X>n{GdUEd*Cd*k)|2oLco>WrlP7>yh(Q%AAe;k#BI6f|}e#N4NTV5Q0IvOa|=CrLim4RTO7yUf|8mhu1cceG_ zUIla<8MpmY@vO;62V~th!Yx=_uv50g(duka`tfYHDhL{x~?4ob!OX9VC!; zwgDW)`1p8c0JMZwA-xPSSNCkC5xN%?+WC>+BNZ|60bn^h{8G17)M9DrYBMK*l`YT4T9NNQ zWoJL7IDt0Q;^1mCobuu#BB8u4C0S_np0(SWte9ogms?Lo8CGxF2PT;y^Z7cbwOHg> zTIoIq0Mb3WQDnP!8$93p`K1F3rfh*b*73vBpv>J2a0~1LBeq^WFEew+Uz8Rx;!YB) zr+5c1juJr>=-SkyP{T{iHm-m?CqGVoJ_+d6m*Q{)p;GLg)U`18B)+TBL#8o%Si6Q`VF%5})?d}eY$)wEC zr>I887B(cn);GQ7wtd(4~s~KuSql@5RtpPO7kkG@Cil{%Ys8^nuq92 zD?2)#5~qwWa+{9{4{f{4TP3O4j-08NiuZ4poV>HSKii$QxfM(5C^{`hu~1^;;h**{ z4VuFZME^CB+FkA|goI}$6rn*<2jIX5V+^9WpaQ|3ujUX#4%P>(FuPnpn%I=Rz<_!t zrfN%QKkiZ0X>Dj}fg2Po@=Yr&eA2;1%MkMFG^#sD(G~kGl-#POqf8dCfqGW!oJCZa z>=IlD6H{TCjny!%))Zw{MCdG_dw@!Fw23YVjK~sbSlY{fMA2kUQMVf_M}_Pcp@l$K zmpSEJg?nDCQNxuGeF3PeL*vkLKh7J7m!dj=rwi7L+bs1pqX-0w&XaVRT3$rj{Ji+% z$Nr_oS-XI6^PeNL=^tU0x2DMKG@6EIGRm4ht7-PHz3w2kK;-thNVnyBPYXDqq0H<_ zV>$su-oCzY&gKuF?`3U+^XdZl*1^rumk`*gW|DJlas68;T6pYj8RBA{x^O7$v9YnS zxVVoFxVFU+?O5l}13qhI7lol2l(YaCXh3Xkulka(im!YCC<0h|01glm6Hjk1a$bFy zyC$j_nQOB+*cM>XRZptqybqXlR^xs)t*Uc91c+%zV5*Ok-;CQ~p{9JHqC93dhD}9B z=j-Lwmt!EWs2Dm#LKZnAvk&^=04DE8e#}xIK%m^+taTL%3d9j+02X%8M;gqg_L^izp)F7L=`_bdN|3_7doJbemiKfQJwT+Q5bHDqi zCK~`TE+Q&w;T+t@)&r%ce!Sw3tk{_EL#tnKJOBhF9(5uKa?#-9dl^pOEUxA-pJSdi z)qkdo9yYleQ^wDvS@DXRck0IY*(V`o4^>hz2SG75>o9Z3{eOcGg zV9w(u$bA!bDmK|Q5XUdmy*ExpT3T93DY1ZG7ntuasSkW1r=^}geX5{v)saMSyxm($ z@e~2}(er1eSspZ86Ye=dWdQc}XCwa;{z%8kSO$;i#89{Hy2wCnpz%`kMIf_ZCWds^ zgNC!hil?YZATXR*!~fU+$bVnP$GfvXl(9N;psLRg^e<{7p#5tWr9 z#6b6&dMd>?svvmi<+~)2292bSjvnzavU%nO9jC#Yt3k~4<@EmwHF6QSq5=Z6i;0Z+8_w!mwMy~-Cr@+- z?fBA?X=gEp_?Z}JGuyAq^t`mYs2^12P--I(y4`j1c{4}2gHCBt|fTJ1DhCXv(Qg?UrAw6{?KP8Q?M#6cijz3os`m z(bZG#l=c2l%!r9EUno4ipyqRkpq4pcxPZZ-#-@kuvK0v~4pPd!=$fZ+OJ}x%4-)VE;u&Jk#lEB&YsxwI*?LbwOR#c=C zQhe43DEgd%JVBw8K?$Z{ZDRF?0(p0)VgMnGx~_AB0Gl|G=$^vx=IvM&A@j9Dn?)Xt zvKQk>IH#zZ-FOf%y6UH-$Hj1&N8p(j)5zxL;6m<27V~f8kk4^(+k3QB-KqeT^c!qW zedYH_?$0dR9CQF~58#R1tnu!>31c*ODb}oX2Zr9}OsBz2M}oPF(|C|Ptyg)J^{f6;6b;aNdOj1U9vdt za_dKa85xh=xogOXxF^!$;$Bx{dYBr)x8xo8g4Q>ypXxjkF}z&Pl7)Xm@u{u*#p>zov-vLE9a^P(%s-(5kv z?NZYI2hPC}-Z0o&UCL&cR`kU+I2@oIP4gn*Hh<%UCjr@9up*O;7Ni^ImkxmY!^5A;{72;-Y$M zIiZo$!%;hq^~}N}YuXjD^%!w^HFj*qR*+JU)CWe|a%KUwvt|4)&79qJ(v3!Kz{JJS zw0f?$xLfr4#R}d1^)wd&oC0uAO1CG%+zvL^`;yX1J^&MUT6csb@x!fzld@O7=ugk* z#ngDCT9V#>yMBE%S*fAnZExwpD4d2SW@V+{#l;;l=lvPjCBUBcTFY9(k!^ba)HM<7 zX-<`j-1lT9En1X!-=_z%7Pn=i6Hm(CXMhK>B?X5dokw%;N_LWpT=|mtKvuL7cBeV< z&P3)={(Y>c%tF#<5~(FS@hucRv-CX?bF9W*C;zEtja~Auw0R>8-~QK{`4^4u|CMI1 zg(DmRK?UMMyh+(tuuLjd7Vo7n#;u3?QEl*$wD^5Ecyx0{Bd?X z5y@4G;2>;}{JsPX@V%PfSFaIPYNmYoi@r}R)>Cj`z>xq=<6HEY{N60nIg4#-2KbfV zm*l^4Wd2_f%KuiqaqJ>Uw;)J~h}Jhth1Pnu-2bO(l!QqY=e4h2tY$xjAbCc9Ki>OI z96OuO2o-hpQb3TCaa!wGMJ;~$0%8?`s|~~xs;$%@;dH!CJC>G~Yk=A9PXgh5 zAZjXqH=uRkAGPKP0+8U&z@rLW;sbc%q0l>=oQ1KmWB#nojg5vTCUkf23X6#u8W{mc zgTdX)y+9uWz-_-*J)lyuBJ%{qpgeg}oOmF=l~+(u0KBQfycdT4xMumaCBj2wm97=- zmVc3I;oeZtiSpHDOL8}VEUGOYeooDqnR2pIG3!(Egt?KCk(ihmV1brq03-dZIUE7J zvuNnO~C#N*C>CPw5#eYuwW0t?8JUm;!@?fkT^_l{HN%Cov_ZscF`f_oUyk*WcdOjc>q% z0VJm!uRM~)=mSYr`AHB5wAq_uFu)tx*eJF+17bjdLv&+dps%y@iq$eneNbRvRZWdM z3U>UF@kPw#pXu-DU;;LxX%zJWA=c}D<(k@B->VGY8%*ktpIQ^_%(18DzkKifi>|YO zH~X)H3dgT)!lb#pyu7xywxi=NKw)XS!N;me|K@R^A3?aCrL}bdyuH1h)nt$qX!?cY z6?|+Ml9`#AMlSgqpcp`ast-A@nxv#(csK~rCR~_5R>HwMz!3;|kd!Z9&Z(uOrm7*# z$AQ`Ty+3FQ$3oHdcLQQ&H6}5U$GOLpHj3B9xh+;iKtKTai*T-=f6YfgA*KKVQH$(W z?f0VS(Ew>9F;SdrZ|&e6Ir;SDNDzTZiHNiX8ehfb4eht+&V?YmuAT_CZyuj+kD+y* z=+>YG(SUSH+0TLLt~fx>CMa-+k`mlChW+^&-3RB4mkbuuO zJ@B($=nsvI9NZfQ!JL^{S%9Gg0||5*xgZL^e}ShIho+xUl>4Cg#)-1lwP`7reT&$r zSMulqpmY>(;HdmI?wYW@f6+ z&iBrQCx*i|h2GhI%w__AX2m|ie=r$!up3pnzwVEtK@TUnc8&0!LhEO`nsXB?Afz&c z-i7euUKu7lxGU{oh;9ilUxvfU{&bX=?v}kI+U$}YneKQxC>yGM;;I8`ULH;-LCl

5o-*aZU|#`fi~(L;+=914$E zSy=(z>R}HJi7=Lw?Cdox-qT@deVkFH68zUuuy?9hgPZWfhYvO0BsOs$cbA8P5r)2f zTEC8=0@07z2*WV;0+1UU6PPnU`)gu=7gzt@>_lc>{(WHaKuMn-E*koI=312v!n|r= zwz|6=8as!JF%g0s{GO|zMT<0*Je=;KZA)s=qYR{=5k`y)tm2=4|Da9l;Pv~^8et}8 z%J+U*1RuoQ{eKVkzjhVOT;h&Q^%Z?SJeX#IEnV1F>T0>TFY3E#Ij1!xT4>)Zvu zgXqoDMis*0-}H#eq6H!@Zrq!7aLT#k$r#Av5DTII2m&}+Sy^rAw_=>R50=!Ui;D-0 zW?#K}6*)v$*)$G9qSZ=g`>RXuTl8E%Mp&1;$ND9GJUx3#oE(<)^G$9OIZjstPeL%Q z>NT$KHJcz7uRjUs=;DL6V>Rabc5NUCYk3A-5#au`=(7Q7V&(QA7;_k%k42|O>R6oV z@DcoEXEV{zBq0!Xor$u*;4oJ6KwWxqh(Uf;!t3)M8gTFv6m*97v;#V|>LTU^wk;*y zO#|4oX9e;gbY2;RuYw2wV8nj$8WK*8jlIpn!ZMeJJa*NFCNf6}O=V=-L11HrbK=$z z-$y?OBrttJ^42a~xBS?bq9!v>A1i|YR?z>x`T)bd@>d%5u}Rf&LKJRIn+FV2kS_-U zqiWs}%iD7vtEFFAfbcfn--7f_uF3NcNXyNH{Su zAs)dIgdn~2JMxfmvPNWOS%6r4V^&7^t=GS6?OxuQ?#~Fv+I+Kvt1QXgrIEqG!5=>8 zZ&mT&%*@Pyc=4Ni_qg6m%LGWPk`+1LWVR|*ZltgHE6ju6o?lhLea&~MFW1rfa$A0T z&iwRpdE6bI!oNepQ+t0Z#TlR!(+U>~7UV;bm#TilVhGpfaWEFvLE9KqC%8^Pd|xg~ zcEbEL08QhQLb9?>yHJas%YVgmPA^NWLlK7mHLRsKxzhOq0{-P}~Pqv(h2w6pQ2bH{NUQRKM2 z0-_v+goMaBEbEGauQC%j-}7m9t9T}$?iVLAshpMN(Qd{ccIDo7jPrpNI_;t$4jw zTow?@07zZib&#t7az0h^O-Ddfpo0_Y0JYr$Qh?`A?Yg`aE$VB-XS6nqje4#A{d=VN zlW~4)Wktx{L+5rs{vdgdQ5&;ik0e z+4N@?RsM(n?#Cbxjq*P|*gtwdgnWPc=?-~`A13)9p6Va_a;PQ#bdvs9 zxI^~f4@dVQ+3*8F{pl$E-w$$odmDs)0`{lm+P@>!+{wk71y_?>}Ahe@sjY4Ctr3_K#6~0*C6SWB-qN3H;g(dZo}j$PfShpVLBz z5&U#${;^PK2mep+{~t^L>i`iF4Jy}nOQ}FQWUKu9P;?oLGQTnvbcu`#t-}%-nhfX? z886a9(BC~!XhfIDfZUrvm&mwCXn`(~an1{YuB`E@`fsn{Y^W^E%;*~$zVr1ZXEz_y z*mvT^0WnoT8Q-l?0vuu*I6olc17!2p2=fw2gmXLOCM4)pygCo;()Z4^C1yrpVPT)e zLh+|5PrkAmsW`Lg+^~oCWu<{61z}#YXq$3}17K(YN4X_0+ZPMOr%zfSpe71rYE6eP9TR3#E7Bx7F`y33M|q<#z)s{;QH2&jq1K= z4Gb!D4GfS+fTgy#(_LC(HY%7}`mv&-0#IBrLcW4Ko?9A!cf)ygtS<9EdD5iq%e~R! zvfp0pSc${RwZAoLeD=|f2#zN3Z4IFYH)&mVQwtaDSL!J-54LS%fWrw6H2KmXFBMfn zQ4!bGCO202fUIJMRyBw!1CD(QknsQlgMqILuyEzug{YKY&}u(Op8;W72klY&mOE?H zi^ZFRAjKb-oTtcUQ7td*)?SBuBFIT5QnS?r^989iv=bn$*|>OJZnGTFp>RrTkWm0K z7+ah-vKq%-4UCMe7-{L~=olC>P!IEhKtMKGxh``DGZWK2kS8L$SB~dw3Q~PQ+DS1; zamdNZ*_~~5b~xDEJXnF3t~pjYm8qzz?o|;;fFVDcQN*zV>BW|dI|H>=0B3&|k1!Wq zUj?)&h;O>e9AeS4w`yS!gZ3(R?(tj2=zx3$kT2x6db;Q`)@y3o$HKyF=40n9?1ojO zH(mQdri3Zz3DY+_CkF)b?Js%os$@Tx(uwN3IL;#msFF7B&HmM4H@vgHhC?L-PG5j4 zr>fMCEFa`6)b2G$`undm(4^7BF`JFpHb+#PoVZcnS_f=~xB>A2(ivE6*8v{_!u#xI z5^4^ZLz2fe&!$jj{u9#xBG( zF64NWm~Rg3QP6{|#o8b+U zr@+Aw8}){lgM))YBC-hNuy8IG0jyB1>_vEg2Q_rC;65AGl z0%P0(L39h9fq|Dd_x)Wia9p&qvO*_gnK-Y80ZKg$xR+62CjqMuh_Z?(wRCe3Q3*1M zeDQ?OHhXu1G>4&Lhla*RM<5Ub4~pk=`yxSr-j|0ct#`vw;~-hh!CCJBxCQzn0B#_D zML7)araF+O78w;46&zgC-GisHq}E=1k(^u|1Z=bpXM`1KvDqQp6C?>%61zu8F^u2x!esCR2^_|cB0ss2a%*` z=>~_5{sY5BQ0YffRW$-t+~M?Xx0!SC^kSl#{q&u@5KKK#($o)H`SGk==-9Czf!F~6+*%f^HbEXtK?+D9xaxQV zW9MU161pf92#NydfEU%%g3e7bNJY3yK-ZP&=RmopTu?}7_>ztNfJx$ltc}VoRKGjZ zCg%eXJh16Oiq`zV5xCW*aB355VZYsEgku_vpnUyc+1F)`!hA_sNT_x`Z)ib$8bmsC zedCSS0g(fs*aa^rwSr2~1>E7lEb*x-mE7;a;iv&h7M2bFl-+SUm+fF>W6zGPYcu^$ zE+FC-Bo&8;hW7OL7wLCWZ|+04UZxB4GD|GwAmsq%2m%u%m6T=()lB934l(IA3+}1q zzM=iD;!PX!>q8(LDxT4}H8&R1TStx9qg*%GsWD>i%JC=M*f+4P%v zI8d&Ge9D3$9CVssn&UW)+YWy^5{VqZgaenqHMwHJkna{w9hOB;1E}KRycR>Qy1!*p zvPxFcTr$K1LX)F*$0&_0PNz}axx>9&MF5BkZJnqfi4de*^B*+k@aJK`e1kCYC0%LE zX0orKbk+(e^kPa!dHZ%ghAB9eLg4zg1Ddi3m68LX$+vV(fJ=Bx7ZbszE)L3`={|@L zGd5q`M1vIO5Z+F@;Ye51BnL=}a)&PIS%AY)x;JkEBD{CUlpQNx^WE4?ZHSQN+0&Z> zyA7&M1&j9X@khUqh}KAOo)%S~*-b5jt;iD(2s(oE=wCjR};A=kdUha2qua%mC ze9SoF&o5sHFmbJzH!FFKSX`=9QA_K_+;t+NYJd2zE8q(X1Jm~9#_7HwpGWOa!1o&_ z6%2|Gri!LplOAuA9#g6Jw*t983;MnQrGIU?+GMido0si5mktDv6`J9@=W<#I~aulBf9Ds+|fe@evuno9LA5V3!lNyW%c|7#+ZlcenUV9m0TSqFyE zqm;8ES$6|(V|+Ya!TRp3sPx?HHhM=4G6Bl!8X^8C)v?Qdae-QwN*+C9=868oz#dy4 z%huW&eJeUt;H}Pc*GrIAi%I)t9%T);^1TbqJVPzy&1%54*wqWSXi-Te33fZCl;p8PNN z-UOb?zF!}09v{sp#M3Bq%8-O+l_F}9xrmS{OJrQl<&mi}WULffL}n|)YC@(cNn|KO z=FH==&UMouwa@wQ_w4uV|2b#x&%3Mlz1Mx;zxy|ShwFP?2N;aL>XI6`Xb6W@fD+0? zN^?@HJQ`=clAz`VI>OP(i8Ry9<^nhb0fBbvDN%EFc9x9a@%k}peB`V?zeIWfE_ zL#lP-_3-cpJSF%{jSI@kBAe0x$wZVdfax<QbM9x7gUoC#x!lStmwM@U$h zC1jL6jv90jlrB(4oNKLJjpeXV2>1`~|K5|G|`OchfIFcuV`_UC|kY4hWd-FgTn3V_? z5d5ArF}c2XF6mJ9KRk?(S8SH56Fn<>V+|Ioa)oNMNYDUKQ;aur`On6-f;dBU_C0 zuTJVF=H%ue1fW4D`~^`CL{mKN>RyM+GZ>5zhc;S}v@OTU!)o(p&04o^-K^QOmEF2f zKn&PyyoOfVnNuhkjde57R+agXo!tPJ5O^H`Yg6!L=z7HvSs73#lWg2t!3KIaNea1} zFQ-icE?%70voLXS=v)5=;mGuetTc;iSxL!cP)34B&6zRC0*>Xpyk-QvBtmQucmmCA zT6+4!f-H0Xb?d%-L?~z;*Q|N-%qf18$_Z}kFV@#S=;nih=J{oodT*5XsldI&r7^k|cC1N{A8W9FL6n;0Us+iRNhpKb9s&b!JZF)a$-D5Wv^1I&jbbc=JAW{E5WL9h6kuSfL{SXsrD|Bdh*KYx>9MylxI=g*(7 zl9Qu5@l7eZFxn>I9R?9NL=EkmH!AunF|k>%Uw43`WMdMh_m`4LUyT_4n7S9$C_yMH zDgt*L0+^sBy+Jz9&or}8zm=M&mz{<#(&)1k#0;z{VtC~ zi)iA5$1NcE6{0Xt>_25gCcC=0#14PVbY({@mNZqhr1-e9FXkg6_<{{qOHEFmB#H2d z868B?HWmno)ta7CSu~|%eVXH_?bH{M8oDYfciAuzn`>(BzS<%#o>&rNs&z$`!up_6 z_Hz=mKqVds`1eZJtVe_>Io)Zb3o8s0R<$iHNjU)9EJpNG8CA0EL$`nu)lNE4zq9<(U)LK7Kq(I!0ou z3b?tE8VHh98=Is7*;84E{{c@K1z5W*39K9CI!&_(4TpHSo|A)5ny^h#eYln`_4D>7 z9X|XSq19Gtj`=#q1NIVU-HF4^T8A3YR-PCc78X`^GGWb{HPBFz!@R}zxnukG!=Xg^ zlD+*P_IxWc*-aqPhhUw2Us{@c_wIIaW2`o$j*%)rRh21{ZpbNT$&&bZButOk+0kMT z7#Q5&+y(~4g6ZXP!az1XM)qm*W)L7#lgxr|YiVedBaAvYXn#6|%{)2Ey#|?)jG*%T z;g75v6O#wJ8k+Yl+8RKdxW1HQH*2>;3nw)2M!$x>QNkjkDu54T7u|?00 ze*7e8(NHwehN?Q$5vkt{FfUHF&{a|@^yyP|2ue>+uZv!*7?^@MaX?z-hO@7(VNW`X zv)KSuu!q9-mnE5L%7@H7+e=K-+XwJ=aOi`(cFvqR8L0C{A(~D1vdKmwgjrHfWCF8x zn&iz~Fs0vPH^yyi01CEhYV=STVqwwlF3Ud2{!c~?)jc8)bQFU_^XI^v873`P^+ked zWTduSLdAP4wR~a3G~(|?dq`6vOiI^*#2@kSF0fdU@yA&qkkAuIy{Z7C~ITdUy))_vk|sLxn#G{{BLF?Id;L9f_+{ zzQ1dlN{Rbp)A-T$$ln`n*XTp2%TJdXdQpkXd(fost(I1Gy~b*M`)?l?x(i+PThm1UfB56s@9I#tiSvyh z`NNSQWU*J<=ZTTm#CgR_^1X5Wx`gTbgKBEwIy0wDi_)c!Bz-WCPZB+?KIQlR4BDgp z3MJnzE?D6OsQLsLQXF>6{P`X)UhF*i;gIi|KMak_+d5BAPna&)!iAHF4zZ$hNk+NT1_W4q)u*KRa1E%bn3x z@<2K4>Q$TINq=2qo47g@HIx14@dyj^TP+uRnWnfy8_S!hy9X0v_O)gDI*p$`Ed;kI zJ9}x1{Gy>=H(?QGmv>VxA7AU1$xD|CO-z2LQ6QBI)SJxp{Q6{S%C@O zt`0PMFa>ldj2=TdtBR+eK2iC5YKG-TVq=v;m}Z@!TPYL@wq%>&_mluI+klSdZfN1N zcN1o@Mhdq~Q@X8%QmuWP+vYIgfXCJYea+{l9uQDnzb2H?U#l{4f}itl&Yi(>nn>g# zp-3^_ki~MkY^VOa3%~d2(eea2PVy@1ZFg?7H!8kvJiS2ufDJ_>I(E22SKqBE&Lt~q z@^52au2Z0+k@O(U-h6fVKXEs16eIVnFPLP%iq@r_x>+~%-5DV_n$vH+RbpctEuaRo z9+&_zUzH#aK6Q{#pE=WyGp8Z{7Lm*M|3E&(to^(M>E4ZtcN*ol{yssIg7AG;c6)G} zoobdaoBtGfutR!!cRLoe08zJg_3DJq1ubh7{6ka@=$i}dcAc;XCAvj1(nMD3xrT&{ zgbyxjYHn77cT_0TmvHbV3zKVokKYz3Cy8D22)-30<=O?d9nP#+bAvJ8}+qlE~7A%-{!84AeFgf&W zjr-g-6|)P*qI`ePaXUYE_fD}rOU~6eWH08`*BpG7kj!kfWXK*U43yfkJlgIR*|5mC z8?IC4omaWJxsRoqJfVRCVgIQ1#KZ;sVfNL!LJb^zwqQ9^|Lo+JmV(uIs9!f zS*3St?v#5i=Wb5lQ#O@vu4mj#6}G<(=HC za$n=WoV^)(>hRS`eDbvq7ED%g&r)77!OP((p| zW2gjvc-FELZ=ElK^EC#nA`3l^0r~;TS49x?u9Q75W&NcHDbnlk^6_&Gn^kp2T%>eD za|oy-xN4TYVd6WpsSc3dWu6c&0s)tQYVC~a)fzznPh&=z*k zV*CDcQ)dFDA8%4Z&CT666bq9SHT~@<;C7R~VI}5H)gd@cL3gcz{lQy_2FMeojn)8b zOkFdP=S~}+5~^`f<{SJoOS^|tr1wajR)reLc*)A&9-t^vWR z<7Y2<*o8|?W3NCIhv1F^MysTcx6ULquB-G4kIC*s@`u8+>gq3BJen7_|5j&#WOV{^ zCbChm$Brnhsjyq-RM3RzPzlInQc? zzwQSb+vP9X;%J2G(^INp(d}&rt;oQKc6J^?X1W3na$q2r^fO>$s~x)v4k^iqpGgsl zFdM>ypJfjq&z=UBQ=%V?YOy6MO1CCJd#L1$?25W0ie;+LVQS3|c`pb!+G=To4|}Vf z9cx^8QMo3KI(U%s=DdmB_iA%<6rAw9&280upb=1njco(m-W6@}a<{by{7uUfa;_uL z5iO*4KGk&DQF&0~_kq2by{}M13QSk39I%^v8_E;I4n*T;&cMkKZr;(uL`dr)7b^Bt zhT2bv+{@3;2NdaTfAs|~E5UHnsC(@hL(vF1^mhRCp##`2GX18z6ZS^KB}AwqzEL8j z4=9m1@x4ZN%lDNDmz$a-6c1v)otL5a*SJD><46TGkdi8&&DUG}DE*A!Tutta!b4QS z^&lX<>F3wq6d9AnkSQ%+Z+f(>tSoWZN`l;nD3i`=k&J#lH)&6+YvI^jL{RzmHNH?b z6#%kU`P8G2Vc?tD$A5UIN_S?MPS-nmwrB(hkpMHTL=%$Mkz5SQFoG;QX3l;e3fsYS zUK^;hwFA}TO60AhZnl~p75OWm(o7WQ5r<}UOL=1Rv$-N$-dTXK?k$}{Nj(p(ET=Zv{nMXnlIoDQZsbGnILFlj)6L=S!O!IAQ>SYA-kJ z_^MvU_LoHEj6*H4DiV-IcvM1r$QEV%;py6Xj_d_mwDR-WJ~D|+e`4#Q>OOtki(pW4 zeM>&KeSp-F$sbtn)GacVf}~WY6S$03)dc_-svHSxC5U-uE~?qU1Kg2bbB`ir|z;lIQ8 z+NTz=>);*)WJ;|u1y3o}w$<)o>?L;I_9!)hEc-;xPan)gzXBBm7K<_y@wRrQ%V3)3 z(%`=sPN2OJgm5_Xb(s~4czlI4*|esYZ!=BOi95d6(pShtsO+Oq!@*qKNmS#cd&0on z&U+4&;6;Xm2)S_-C&&Z0M5;EY?{Nz$_rwO;Bxktz-hIPQQfy+x`@0;>Y7 zs~{E==9RE)58{6pE%eD_v);Ws3!!mc-R#vk6j)bo{pAqqVqSAy&U-qSWg(MS>3`|m z%~*QmtjG-aNxFY=x%qj^JbJR=e}e=2ztmQ66n#W|-Qa)T9FCq_@!xBW{MUY5 z0CiGz7{}y{1LFVR`?yiNv0v{fF?3TLMJ>hjRC}LH+3~->!TFyX;;)%ch_9R0ulLdD zMO4eV>6icg)-*)fz_wWudR6?MpyhU@&F+i;Q|9`W#R@bSN712^o6f5*<6W+@``X_I zFJ}E;{Z;pP5a$vK>no7p0`#7my=bmY%lp?bpui7%(E@;PK_MXo`KWO+Tzv9vIN$?e zK@g!{C2Ls7NdHM2u&Ii0q76-ro8tBPpgaYX?DAF z(LfLe!)=Bp(xN7XaS#1)s6{TFQyyL-jE6XN3T4f2I3d5eA0A45`A)o~uEgVj|Mv&S-!dXG!%b6G^8@XQO#&kKEGKfmH0 zms9{h{5T{=xrb9wOo8YH9Tvkwjl?(eqCQct$HiV-uUL9jk;;;rR$)1)}|URq16 zA359j*U~%_xi@AC6q0#$aA)-_hF0&9e5sm*UyB42MWi0)uFE$N@r{}KWa8}_G}$Sf zUbGeihT!6f@AcQykxig;+*rwI=gPu!w&XOjxKOkecGx#C}o4NI1Ea(XHUxsrNY9}4YzA%HGeT=GLjzd*;ryjEN658R5Ekwa~yLGmtI(mrBMJZKUai zrEVj{>mo842}a{<|6Zh1KeAp4^2_5E$qL+Fq2xMaT&3ams7Mp;?WYZdyS6V=bLlbm z8b1%OT9C?$S+s8Dal6uz`-9omjH`OQSop4M<7ygLxRWwPgj?Pb z?Fn(IHGUmBt}ybdHi^+oHWzjn?!T^-*_Nh|N~Xv%C7Nj!B_$<@S*WONfVNX4=Zaf-Iu-0>Ek%W6 z%Hv8RU)(pDOsU^(L$(9Z2X!rPL0Znx1>F_FBfsPFUwa3v-&1C94rcEp3%5!*>kUE8uM)$*TvKOBH4x;r>f`IA)%s;>;W38 zkWu#bZ2~|d1ajskWk?o&vlFu8n=C(Wee6`>irLtqo!=JX^Y{2Eut`V!YIwQfW?u-~ zTIUHRzb!2#N-}c#RxcueL||QP(ySg-jhSz5WCU!1JW5qAOKz9;P4^Qv%vMs^I(`VW zqB)9Or=QsDi`d9|pqhA?{n_a7%Jx98$0Gs*JWz4-1tN}y(h^t)1U6$N0AV0x;rmPG zZ@0ZsXdvP)lGV~_JbT`_kv|_#y5P{gv6wcDqj`X*4i7E`@dZJ^wD_sa;_UIQUPXG|z353u$i z$mJr_3t36X9JdXvx^OEmxYo7It9a3d@dG1#n5Arcp1rWZGzs5yOQ%-7yi3K4WXJ!) zD~2QPVT-7*9Gm}%toQ#68_)h_8#Pj6saEav%L}nOB`QCZaFKOf)3#p(cdURzeqG_F zWWQC88ry%_W2vO)kG*B;I~!7ZY8CF_EJAiI#f)YiS|n@fGX~VySnkbGgY<;=F9+7m5ly z$8R$`BTRNW4^Jmcxm_I|=vlEZe9h+ZJIBgH;+s7{E?DgP%y#DBf?LIl_{MkC>jxY` zL`MxRtjpwbD_$f%{^G(191)`z*~>Dy&J{1(HvZzV7>-2Z;>JZar3Gy}-YY4Hjo;}n zrkhMV!Ah2zx^Q^I%ehQrm$l>O|D}U!ZuOF&&AjFEQEy(4v5VmNi(ggU>WR@&zgn(0 zuie;X!}yDys%|aB#WI(-dh;5MUB>U-m)5Jg^?Sr06+XeYnNoFZi_mNNadUaWW2%=u z5(D*Cy(>E_FBS!{@s97ui;*VX$#L0QuauXtky8|11;+Q{_QNGE#K)4^2Q#_u7S}8o z-=qa3w~q>f;V-ma?MP{#wqmo;yz%GxEQOLHxkr9d^;X=^7($mH1+pdo^4h!99hznO z!VLm1T&0$TXG)FlDFc>aW0VboO3GJz*r&dA5W#gav} zwybQH7p>*eNm25I-`=JcYnF{|k03HS2|Uh5!~8F`&ns~DUuD}^`uBLkhD(#Emxp+j zKPzqWMX{oZi3w4<42gx!yUAs9^MWp)^c>H{u>DTt<@$5mm*;3Hu3q#(Rutg@)Rptv zkfpe)uYdgYdkz*+Mc?NJ8wnGfFi6^EAza?#kFk* ztz-H^IAwx|(VIAS3sl|eRC}LwRTyM9#IZlotN*o2|Bm8t z4-r&8lV3N5za{9}uVZtbYGSG*(PlDwF?s!jhU}qcoe^+oq6OYb$v23(?Rsm%%}ym} zEDStoTxYinO`Nn@q}uMae z%AWo@fmT%GZISDJcWjNH__og^?}veNFiEFP{O;{-Bs-TnF~7V!$I^eh>xj4SvpwjO zwPCs?r+!^zFYohf(vS|r`#1A-r7?)V;ty1b@ zCtm8-_2!Ykjy?BF-Q(Y`9m03N#IZKVYLMUa*AdvRODfBiIwmZ?v$s%m!UpD<#IGr( zmS6YfDsk%hS{HtC#Z}bm?yv8BqUG*?jV|{2*9q@=7mt+7uX~Ww z?;=0pK6^C-fd)4k*S1@~&DfC0Tm?bRqr&osW=tILXD3K@e1Aa&5_^O+N{}A;nBGZ$6N(ce=zbSChKR@?kmJdI}MWCj|(*Ly3zvotWI*$Yk40F zynrGP+3pu#YA#tycS{icU`kxTm{boUANpx8;&!u<2LQ7LA5~OjWB~BuAg3Dg%VS|- z0eOazv2o7V-Rpf>9{J5ZKmO$}RDE*VUc<*Ea>_;31x|%^yIk3VAoTUpa|LhSc)F$>uVrDqNtMUdW8f_OARv;p6`)+iP82ry650C=Y1%k?gnc-hq`54kZZrGeZbq zGPjh~@m!C-2CHgjCOj2TI6GS`HQ#LgASxoFZ=;W_9d}niykWY+^E5E+Lw!u#{|a#*Jg%n-*fnW?tNc7YAhckQpn9c6uZ%B)V*mN27ee*Dj_Q9=ggrRKsdw(I zsKbo>P<#5g*tn4@NxN5;n|3`)YN$b&H^e>`>I#H%@&Gr*afqjJ%#o2iE-Y|w?go)vI%;R+TrF6SFu z@?aXpr2B-6mDS{$|6q3_zirS}xl5&CeO+GU@&eT+U_YM$L4`i~4Zf_hjL;Me~OpWV2fWKz3pe?PA*H+5+s3!JE(^yr=fGLY*y*tKUTDV zP(C*k?reQrynP58E>0PL@zOQA+I|u_-{8dkO+ttSo?_YXqZe=G*4ud>2J4oMYE$tu zy;t+c-{>XCoVRH7o!-3(_v}byHuBn(8MnSKvyu@5sAWZ%fbc6SA^Z}1?#rp;?k=ad z@O&O?)8J;jR%`rg87~|8ZjQZHF8oqv;No$gnkYgWIlYPs7I5iECebGDaZmME<)(;- zP{AhtD)&pm*II?R7uej!!ps z2ePq|v&Mf)m0&RM-qAiXu+=z{f9un@v60LE(!KV-<5IgVccvyv~sNXxRmC)(v-9=v1Bv`(f3Eri|rKdkGT}n|aaZGY^AWg_RLu7QuLk=F-iKT{v7KU(I(q6j&Y_^ z?Q!q25H_|h-f<6(p|rY9%7kyO^)GaBnt^bFHmS5CXonAGT=_YnF91mk1aWu zxPO+C!ZlqD_n%$crp8i}8rP;Kv}uSq$_QzlGaWU6NUfl(HZ&re12h&v}7Iq znU-o%uulr%oqcb5imv!oHNDexlc!~cb#PPQfE*nYddTTR`%!m?K$Wn1-0IQ0Jwtqs zjK3Qp#oJ0Z-PKsOuh3am3#K7WAO)ccEr)dIO`a!Os_R}DyHtCK{G9LS^AO-Wm{Uic z>{VC?j8ZBRmGEP*fmdet6JE^=W+`Z1%q;x6{K5SOp?HQ;h4q(%>W7L?hb#5jGjXa{ z2Z}dqvol}LIVf&ECV0l5q9I<_kbU0C$~w{Hto@n-dtu&1E~sO)r4yjVU}geBtw~9} zS)3LGcxfgJ$IUj$Pp=IwO>sK4EZ*0rR^f;JQW3?`t+0otE`D%8_UGAEhDsS3*FNyb zfyZ6bBKc&{hM(&XK+V#6|&+`bq)z zP?fTV2)p+5^e{%&VuwE;>~oubU3lrZF;NwtrddXgF`2a|IF+dGgd~t$chQc@m^G!6 z;EcMXq_9P2-MGz1k;Ia%+RGKmF|U_A0Ps8d)dB2IF%JnIy8ZrQe4~Rm>tn);qwqSu zZ_iJIsCU4gqd?m6+*ug;H14RPYcpX}Lt|GAu}%Tq76+*ye~XJcWV3-Vd-v?G`6=X}?CK!n0*%Hqed z+~;&Aq@=_r&k< z_LY_zD^vVd9TYeD_V$a9O^)j_#qcAD;zbz+;_cV&DvPUpznp9`ji;!Qt%~ZEiEkHE zRCt?F!s-swfEZX1ZFlQmW*`mI8k@cZqtCNGlX*k4f>5_ZH*MFnd% z@=faQY2P{?UD&A*WHVR0(dtz66!p=`8 z2fvV5WWIg*3TjiXYVUSZ2`h0KVbGCf`d(2S=HDg-=Eog$OgME1rt3dV#=VDwZ(6K z`?P7g;xFbGeHQGezB-1smxr&3s^)OT{Qf>{mLzt3`WX^CF)9Pai*hr$utIix>$jIw zcXI=eA?hSdNZ~-AuR@0GROGBiIYuu%q3-^4UxA$ERHF6n*F!9!1h%rkCM%8YNq);C zIM#mq((@4}{>h1((-!En zUS9h|n`CE1Fj!ivxUu z1hN)@Qnc9)YTCWHx{!}@qCW2Yq3|_gh3m;wlTDtZlBc9~)+I}^^$thsg*f{f17oTm@RACaxl7fr!6{vF-ys40E!alk_&5RQ4Q{YE%{&m|_yTSLVDp-X5E&y$a?!V3~S+`=3XL3*uH$T zwNW7jlFgYR&j3BG1twbBsOWYir5K=ofzAfTT>M?u1H|IZPm z!tyyv<>qm3`Qp)tY$I<-ObfFHG>x2+=20k+K6z3z$y6N`?aW>^268zQ&RH|rIE_Wa4-2Mha>I5{3uOq7|sAWPvcI}Vcn5>;w{g}JCmZD{YXal@W zMnq5>!WFDFRvL3s{c2Sb&wg7S2%aJdRG~KAic(qUyk?OL_-}n~-h`ldO1lRf=NDfR zmVC|hBMjvqcg$eSVVtYYFj3s(yD?i!_3CS5zK>I9i&8b#eVbt|ky-5qwb9s^2%o28 zv(P1t{0^$df^V0vA&MyXbnIfRw9s_*{Mbn9w^Q(G9KYtkeC$xfPVXMRi|$u@gmi6e z0iIVcIWn^8y7jlK03$eNUNa`G`PMrUBz-30wOCKYDdMQ)+BtNp<01dI>G;20hIdVU1jSHw_b znWt7a{=E3)XP)$BI=={~F9wV7n@?%T8m9ZfSNpb;xZ{~lH5r8GE2x+kzMp;LkHp+) z_*HJKotef1*DCeGet;j;)#|dnj-MSGb<{Trh3H#fy8q*1841D04bR8AHPEL@FoW_l z7Zvv)SM}A>?~PG;=^%ROJ4WKtehf$ax8Vr%dE2R(|MLq&(J_M9@X(`d;cFgl(J2f5 zk$prAmI$HO#C#j#-PkMG}a(8ZAbkuw{Owpx6D zW6t)h@o$XIRZV*Gz291nCyg7Zb}Ax))4#XoxX^_>u^%JVuSL?C^L*C#PL1<Bn3{sex_vL(3h8F3($Va1_N-5K#gC(3G2v=?6wm1XZza%MVE z%WK?6aajD0T0P|_?OY5r@dEynVB}!MnRTAQ0!Xs6ha3x;q#gT zJ?G@K;jjxcFQjaH^{pA@A7X1O-rO$@lC2zKnAQzr;-8T71ztff?od?1%{2^{&xIcn5QNAv!#%@U>`SDIgy9u*fbx#oMs2RpiA=NGG$ zUIZr!zymid%*zuRK^)tNS|-1|OD~K?J=-io4wI%^kpsKiEGQ1eexlo|=adp+*;!}i zUH)t8lfbVrgW8~r1wW5H7@H*l2*L?~!0Y{?Js_g2mr$LQoIlh&6C7>`N%t6GA0iNCBJls#zYoR0!T`gLvSO3~zdPa)a z^Qrlh{?4q;4MRKrkZenr4TF3P$RhecTn(~upH9hR!UC2Wf&;DtiKSbD)7EVr+p<0t zQ@y=S4R-uBi9GP?2V4XxI496|BVZh0kzt1zYGr%K7CS2vspN)|?5%Il`Xxp2z~t2k zEpOy(uY4E58w;S%X9sW!Bhn5obTFNS4?|yK&P2GJ|BYdi9Az0D9N_>`0gz!jdVrO| zcm%WXeG%slS3bF5V%&el1yZGQv@#H*?V~dp2n&1;0Ji!A%BC?_ik-F))Av`*9+n9E zb%F(mASDL>2OzsW3@=DqDgIQlALG%V-C%`n==X+>?miPAI=UlI=!Vg)c;ZuWR%HJ6 z3GiShLIlV5$6v5|NC1y+i@%~CM-Rl`dh_4YNMyVTMc?j+mMlN^EJD7+hEU_w!ytIZG#E7 z&5UtVvkW|N1)K`&-A(kDC{ zj_)Z-&J=h40N@iK<1?LhoJ{~?zlIXr$+U&0GO;@rNzU9OG*Cu)`#>33u@>hO+ZqV$ zKd}=dg)~Hm@^Lxbrv517`|0TnXbWhdN&rUa`3xa1Td;u>MExjQ98t3!2>^CKoyP_Y z0NbjgXAz4A+5x&)jh_Pud{$``Okc^vAls^_aKz4V+v4h6)VU&%m>JwoNYr8HJD8l! zLLeLGgRvPW!w`Msjc*2sml-cZCU2{LFzckT@xTzBITTsVbhB`*&EX9Z9Bw_81^bd& zr#rF+-%*ir3Q`C;ueDNWEj-@sPVEkGkQDVv4w~iqG zL9w40S&9O&lu9~>ngtyV9_aCfIbD89kBtMv8jPlYLf6GC#@hF)Bqi{S38ud2H$Q5H$?H4l-P z9X!kqI6XK7q0gEOBB1Q#9pv6rfUog|bcc*==6M4t#LT1N$a&-+jlB z_JzN?72itfg|N%t*O_ zkWddjw?)U{r4OZw;re9!0Utqc^~K!CAnGKYYY#tlRa5jaj+sx$=+$vien~bl|=H^H;dvuvS zsFbayQ|R=WbHscZj89DUk^XEs`220o4bX0Y$&`rypHeYNZ9w1!bDt3u2u92eg=)tD5|j^wq6o1xumu3{6Az{O+ZM{7As^qvPI;mw@u^w}v=XAnUwXoAeol#xippm? z=GboQdRg>L2?^w1XF9pwE#S$f3?);VV?0rf(^^5hK{I{E6l>H~gc#Krz;<}{7{0c$ ztNrCF0p>smlMt%^V4hkqzqwlf!-o%)-uRFK-E%0Zacvj5oFL&ze4VfBEl(ONcGkI3akCCBoKKG-Py;|f$mp^?56KJN?N-xxL`p$+J003L^y7Wf^udxC=hl(LjM&?{}mCZMg`}pXX&1SayLOzHOzY5@do?#_)IN{gkKUE zR~!?Plfiq{=eQ;V6LnCN1{v6aol`#Fh5`bPq)Lks$ej?2w;>tb?SC@*)3&g{HCHOZ zL2EOZ^ozalb;HqPm(ZM>5-uGa2?VPKlWgky_8RlB|Bf z4%SlUn2*JQ2Baefy4vq5$qsy`ksF3}Js^z4Gl`H+D3avw)H(BMI`fS^wto@FPjg|; zu=j^hK_1vjq3+{m5*iI74Gfqv4wr@_ocE3hePwD9U$^n*xfI$DBYlx`r_gp`uRXYR zc!+;Kg@f4O5i4xxZ5>&0aOZBUa(j+PIUp}cu zAO9z93URUZzfP*kboa@}mR|l8v^!?O>#Mni$Bcr`sjO0#o^jv({0vs!^P8r+Z@oj> z`+}PEo+nni%IRHvAL-UDksFe=uY4QV(zJv{o^&bfS;oC*+1h{if3926qJk&)zcEiv zC9g^C)IZGcJ;bxQt@~m4zFd!OxeJLh_EnA|b+3(R_oyG;!kzrVZ1kUa zD!~{fPcTNg6O2*+(e3}~AD*eXd1JM*$Jv<*i{F#Ohr{c)?c-w}RLRoamb?Df|0E@S zX#He)MmoQt>%I!#jhuUFt%mVAhVdcJ$!%Hp#T3Jft!RQv=9HN9WNlDMDlwH{cg;+_ zk*Hy|iR)c+4^Op(Z=N`ZTbZd1BfYxaA-(G8`u;PGE8Mg7!;5bfHZ*h%*dGyO3omX| zd(Je>jqz4rape!Qebc`>+sD`b{u^fde^~mvQEdIj3Df=5{Fnb9APVh#p;BCK{~jl(&_wGn?vB-5w0?Xx3~0vya1z5 zyqHF%QsE>6!-UE|`-Yz{A=1I{lF!JR@9siWRs;HD!NDVl7+ej^i?SrYhoz!@x`b5k zXZQ%V$c-9{e^;M>B53tyl15+?yUn~uVL@RfC4ZLW!Q zhPWD;On%6lT4}yf5^>T2xGn)o^4FGn-MA5t!Zts@rOmU!K`s$K1UObf2wW7ilP6OL zn)y!_z685+{!Hi!O4e3!nE-uDL~sfzRd9V!CFkF8 zM)ruZ^07I7WUw8~6XZR6j+j&170%L34UNDb2X<5khtz>?6kMjDwh*$l8#Zo)14)P) zBCQP)b1)R-wrF=x zu@AuH@$>uAQ(h;lbb`kc#BB(st0x$FsaGMj&#M@?K6|*vRU}Y;&w&G=Df@_;8h;-j ziH~mDSc)*?SVomd)>r>Lr{Gs3KK=dum@P1DysbAI&jVSzp4IcV!pfVkU@qolHOIVdzZIfp=I zZRhL&mBihtm`>4E|#c=3L}R1PU>d@1v@MgSj{Az}eAe+`Z>=pNz-%QIdq zds6ol@~{`#u8?V=p{w}$bDdc)7TA}(xVy_0p4R;3o5c+Cmo{JdDp>C)B_*ZcBN!1C zW!-Z^xXk#^MVjKdO@X6Ta%reWqf5QJqe}xERLI--ELe!8tKv{P6^ZX;n5EtvTWv< zBs03vo|5&&Dn!ZaDI`rm-HMW98(UjCmMpM)fSU|K2O@tlhPoedgM0VxZRyJ2i0uJk zAVdu{;k0vbayrnxD&P{Kc$sVY@}OI{jFZ~IGyYMWJhUY!l1Zi|CW=c)Bt=H%ag~kK z@kx+bAdNURZ}!pKV;|-y%xPFF+w=AiYRKM&D=RA_n_offeaUnq3k%(NLpLc!B_)rZ zi@BVKB=5| zgFqGhJc6(qG}!2+!akSyl*%?BU0%IXjQ1WI=6l@cx^mSj2cGOtjgu}e`#(9*=2!`b zY(CAuc5VIR=IAcRN8pc(jXj0pZygUpv_tK|(RwS$DeRapgN^C)z~xNMw za0b=lsA}gFH@W%Qn+VnWYSSA!!LpNvss@A+ao9W$&O<~Pg3K5xOsxLK@DQXpm+|me zFrKXr0qfZe>k{1f#Nf3D%@K#4Hv0j8u|^bV2a~}-SPR^zm~Ncx!-xoVM92xcNe`a( zu7ifck^t`a*PNP7HMvdQbRB=pApSLj`cK!sau3=t+Hfx{tR4X(L&LRe*OrrMh*E(3 z_(Pf_)9J8XcuvU+;XKeoj_ACnRDd=HXVJ_d{%0(tOs~V=o`Z)4 zs=e73kT-1Mx#_o$y}|rYhe1c)xKr=iHhB`uVODt+mcu1m@h|*ws*_wc_%s7BUGUO1 z&wjTmSZdZ-S2Tcpq|6O@JG$(hPY#wJF(S*iSj`6p{hOR2S>l+mv-c8 zB{?6+L3|P$2Ew9V0Rhrx{JO7CEAQgt#=ps(#+8(!DK=rSM17yL*}7u({PLE1tM@hLuhEUGzjrA5 zH(P6-W`)qi{zIjkYw9ix`}hvyB1@T`N;aA>LG^{N2(__I94VC1W&XdrvBIlS51n#~ zwXc%b^7iNBYKrbpPCc9QTd$^g`sJ+&m5(tuW8GBL-w}QD=FtpME?bG;e+c_v8#yID z<^ieT4?R=Cb58B_RX5|`yK;r7ZwQZc(mElbmlm?tFMFC}uB$Chn51vN-bd{Bx1c#w z?N3~*Ox3@bZKg}@Tpu%NBad$+P7hTbc zzS-G%j(f3h4Sm}$)BBY`BXiQx;``hG;2lc+tKQ-GSjfMF8R|a_hHR7&75mjatZ*f=3V295uTNR&9)&n7}24S{Q{?d7j%P>CkA}ci>UUKb9MxN1NMf0;IQ(@R=5eNARIO1 zcC28Ma;+d%@lWf;56n*sk+)R<>lvR5O?EeReNB=Y=@d+$vE1$KAHYX#Kn6xu7yW z0aYZh+dP6-cLure$4Cyr{5hm3wZ`WO3Q>X}a^d9 z-j)$AF{2`c@AX*)1q58hWd#HTbe3;9W7&EC#j=sM&!>*)=;#E}z)u`z+j!p>S*z^5 z9QjOWgABlxs%4MCso>(}m*c`4ywpMvMIRQ9EPH6dHlCd@dTrp6is+|^fLd!N!$oh;eT3IW}}+)#Y_0!dh*JL z4lU+9OV|w^IleV>?{Bi>V#_l_%>>+?Gsp~X$^oHq5>yoWw@1+P`ZL!F3KA^7wQbU# zofMY)+PSf6!XhF{TiiAi#Kbs|fn;(Ci#? zbnOUOe24=wE?m{0C*RXjD!)?<)$wI&p=^v5xtVzy7P(B+bZ-B!WpU`cQQ`K}(% z8^DLpNP4(!drL-8VYP35{k_0JbpGKXcynvBIETx{BJSSRL(=+`5)C|eM@j>>T)x?C zoSN+9);6$txM0RAzI_}>%c6f5a+ZBQ+M=hQI075FrUPmU`f32l@(?0fpKmBZWPy;! z(cfjEm>Z#_!~AQxzn4{DE0WQi_yKx<&~~})-v)hx7gfTCQAkDTS$90ogY_{lE29^Z zJrs=Uja9KiphP;5Hk7@*3YH#R_Nu^}heFbJ@uAwu7B=OnfD~{KzO5GF3xD{Qj{=&t zj^~IUIHM(tSDhlWaaO>phpQz~Riv-WnQWgkUktR0>6lJ%8b-gqK#7)+&^%x zdPw&Dt+~bmr*l^%#m7>PfP>yFxss4U4+!8)z5u9G)9cV|01ay7Ii<^iM|%Hpw;5c~ zlhAT&5pBBCk4*0iljYK#m`A%KdUW@l>NE@+29m1>@fOS-md&!v;jWm+P1LOFd1x~8 zmQ}?EIQLn>caS3s*0CuwfDZij&fP*bQ({*-GZ}pyn^vt_#rE;#=H85+9ROF<5-5Gl z3wSD7inm7;_!O)1Z`Pg#k#AP9qVu|{Nqot+v7C_2aJe51JcXNXCB_`ZLY%JV|IBxq z&%Nc$o4ve=HJ`KD_*Nb8-0%i0f#d$#nW|VOGo4GnRje*45|o%CB84)p);n@%+>6@! zfA^1~xSMeQk)yr0*knFn?*7B{xRsX>K0>1HZT2I6X7Lek*7rx3dd5xRhEi zet;uSPQtws7RJTR?yFBn%GIrORK0SXV^uFl*5!Mv#Y_I(9~;oj5yC#B#+CJbgXUIg zz71+~|C=AzJ)SZQIVwd^*;`Sg`>B27ILqZ{8rl{;lS3&w=!| zPRODQ6g;?39`uSJb`6ha2g3$!LW-B|_j)-zNeMDnMID%j;szVqU@Ty?7BR3@S zDt+Y?NR+)5Zf-et)AMOMg~8?x{`Pg*evCBcDCq$lTV84M*Qvp|A(T&Z*02GA@>9x3lH!J|mofA^@syGw9q z9eQF3SVsT+0o$B|G-yHxQGJ**-!YpU;IUhrlN_TLEsiZ*a&vMIbnPcfUP8{5b*k( zl~s=Vwtf5d#dL2;4{^Sya1qgwzB~GoGCBhaR#E0eahut9UpczMUA08*JDB}jhAaC4 zL|WF}*XXs^xw*gs5z6Z;n^Yb~QkNioggNI~C8E!y7O+tIkoShsKYIDA@-nB%{_}JE zs}b+`eCU57;{AsSQBH0S?n$hYGRsT%3WieJNkI$dCLhU}H=jy##hlf1UT&#vyv)Gj zzrRWI?J2f18ysY>kj?h#Eg}W%pYu9t&V{#GQWrO@X_DE#UFwO;ZVIh%m0verVDQ;z-2=!xjS7RtALR>t1IG&%MOx~F8 zQ4p*DBlq)s#j+J^+2;wxlm*T`+#QEhNHf469nOR9Kg*8@S{zZ5SB zVml|Bz2ZZcui6&sMv|B7|B^Q$IPH$?x7W zFGGhlkFkjuR`%)ang6?DeJ=^Ba;l2IWQU@+bp8;C7t@$>Sxc4|Wh+F4xCOUenR40k zBjyq3%_;>%4o&yFyfW9+>l(Jxoip7afme?GDK+gwu(gmBw>aZvr ze8=?iMOAt?owEF6Yv>cnOn)R{@S=!Exwp;Z%Nc{EMg^C~N}c~aM@#V) zXad6o>j=%dI;&9TvErTI(~Z?6 zT`t$hXXTR2KXr4IL(geFcN;gp0d>Atff>sD5)w_Z+|^;TqMwG`tE-zsU zA<4yd5j9r;6`*0N2XU+BL;NgY$%mB>aI>+zQdp68R`0JOLP}9ZYUCg`0b-v*Tke~6 z)q-n1R~NI|d-RvaoEz#_kt6+L`eyEM9SeuJ31SXyt~Nl00P?>`RJrzOD9WCV2-qOV zuRyPNhAyE&$VEkN`r19F6>-SNQ65c(Gs782|Lv@f%Y?J=!3_!NH*X+Z2{+_q=U)xbJH z|Ap-CCkvidxw9xRm<`FkIwu-_Tv)3Y>F_tj(u&PCCr(O-T_Jf3j=xxtTYhu10k#{^ zD;_HoMZq|3wG+Z)O8NOowZG!2PS-qRvwQtjOv z4CC*~jXoJgD2XUJgQ7|~94eG*qr;a8vTmqfMpr5G*wL4EzRE7t{<~mXgLDM5Jr7s% z_}g>UH^(jtrac;$x8q%GoR9ntiqX(mLDI;UxL1iIrB(#B1C{)&LK`fq8B9w#QR&tj zU7+l4`Y3(IVv1t=SV@*`e(1Y*z>(f!;t)+RdVL+!7d3m#OQcR=q`&mjdeP#=1cgPA zfBj<&MLK2tP~-u%-{2%>ugc@u?I8X?j(1SnqgQ;JJ+7F=V%;vuFp@xk3(xpR0293? z`YD;LjqKpzr76bwyII7fU#2lRZGI(1se}Bp9*bG^q}9BJE_tIR)PT^$G-%ac$J6)OJh70yO<3gil)LJ zy=m>L+!1cDd4SaU{qDT+ubZ&n*`^ZBULAnysD;(^i_h5~v^~+bO^JU4KPgr?z@>gM z=^D22(b++K-On`r<8$_^Bt}Kax|D&c+DNjqJx_ojkMJDcA<_nn!#gLuFKz(*QH{~4Ic4ko$w4Rzq0N#bBjEG zEr`6XmdR<)G3E(RV6{itiU-66PLYpjF4d|rmR>hufZy9Z9>rpSSCx6`j#6t)b}qh0 zZ@GMNg{R|XxlF#>qO_AX??uge3!8lsw`_VvGTae9_3(GSg}ub#%cR+xUy9OFx0Z-X z8~(8Zgp0L0ux!OGVtAkk$YJ&Kty;Cp^PZROI}=~k-z$*f8GUc|ctt$Ot0#HqgbnC# zbNFNVw26tiub8#c0#9L%zjp8W+G@#V&cF9z%5*e{Q^4w77D8MpE9iz#$8_I7@knFJ z>RK~_(8k$IwHjXkHh_d5yCxcisvb)-GU*CK*~`POi_*UPWBcGjTiz}s>SdL;3N>zD zs#W}FYa)iuYC=@V4zcgB=V|_9g#MhfuJLf%Zmsi<6NXz%bkiu3k}f~INom64O437Z zaFpn_+PjAur@8))%{Y)d&4C{{knBIr)pL(eFyq9nusD^45sU)M9zLzy16_Cbd@#H# zU>882yKBK8(M zrCL@_6YkHol$EG~K-#7fg`0eOrDhEqp7G437kI3|iR`}Ig_6>C)1QbAYTmV?`(l+26@ym0$dxZZo&y<<6!zrCw zsOt+@U@H6shX@$OT3+pZRG_;%b6O$1@pKC4GZnX+Fkop~PoVH=8gV- zbuqdZ1z0&&elolp)x0J1xhU;~cjcZN83R7!lRJXUCe?`bX?eAc8ecq*gKAz{%35KB z6zThN_w1TFJkYt&J5;OrPzc*L+(eY(*6yBh+XnIMpYpH~oh<=MIhTDdsXagpdmun~ zeeP##662`-`2C!a$J=>Za*4}lP{iQk3?t_iDj)s~OvhFaD5%+@nsEmkn`D;{RNU&H zdY(aZRn4N1hXvKtTM^mJ-Tp!h=3S;>)3`0@seZ{&74*;;Kuz*-2NevRYZvuoBm2g0 z;fJIvQ8MU@nxBs&y+j^KCD9>ey(RjDShuF5Q1vyiTVPkuMt003?ry_Ysu3oq4YE`s zV@~t6>M$Qaa!?TJrS1s*D!$Q{8X0^yH*ycETah)51#7d_17t6vt1Bv-6q<#p9Me6O zOiG};(_~{0Pjd*BQbY%IU|dBn6-<%5SvMRpXFlS)t3Eu~tgb5XW{f!vC!NxP%@If& zAg{8}XyYb6wBKzIJDY!7U!LiyeqX5VkPdj3L^2T-w?uSNfl|(@HaAVh@^Fpg0Oru? zwI;c#ffbR2P-|p0Q9Fv}MyH1C=Pbe%_uu8YX3w)?F{lOv#B;MX8#kLA`Jh2qPf-}kyLZqSgb;}Uwxeg{%d1m}eC*GPk>MV&kJ(vS zQA=su<=Z+p7n(u>$RzhXzxYde#b(T{#V2!r(&sKXS@83(7{k-u9q1FYNry3bfVfY2 zw9fP&SR>5{FZG*#$+2XF6ETn{Fm3s249JN=KXEzX-9rFRsooa=S4`{srs3h9c)UHK zyphX$vAPv|U2jyX81|BH%pCQ^41ZM`!sB;$a!rZ2*3P{b)l#ef_ zuOiPJ-QEfw3`NEuF%2M2=;w#o?H8TD5P)1*7h?3fq}aqq>pgqiE;=!%HVbBbH5pQaxp*9B{iPMYe~??G@pPZ7P)tVKf~hB ze!8LVXO;W&q3Ju#*H0rOp9P$^tGl1+L%uTVI}|8B3=p96jC!w|^)m_1HY@x4hQ}bD zr|9Y1K59MAYGG`S(mX=$aBzUf($TZIZqHHL%eOPv%Tmo4TV&0lY@ugj`(X^~ISyr& z+(~cw7U68&TmgIf?TwZp+6tUOK&p2G6_7P3&|GK9E)d_2r}8a8!1|l|#h|)Xe_ppi z*ZhHJk`DzTW8bS#W!oFNlro+Apw~>;I-)bJ?Z`ld-8?D-TP5%eLBu?broJ%zg5e!I zoE)8zndi_a^rCQENjB7>^b6{BuZ;8!+U#d;jKjL736~FQPI8u_S(^5 zz<)N$&+4U}zWlz|3Z;pqfhV36+q>2*~g;&1*5t!EV8aqxdrner=UEBWrgC%Xd9-JUW6@=qr+eJ)4B=|2I{sOPnr z6!@IEEx?7daCEi)^_*Vp7bTf*b26Gb=+&-dI6teVIUpT~0|C<<$bEqWqUWZL7`jfP zv5J9`9M<0hxf#~|_*fq+U{pH+mCjES+ zJ!7es*(u%`lb?cBh8Jy_i_Tgzq-C3KtQ!2?1ZA_8N*- z%wLO!xki;V#N=cZOcuk+{3}urC*O@Hx#K4hw&ACBg(!(D*mJoA?h~LK6 zIkOb6{0l|hQL7ThZmTxb{&;H&qv1W$M}gdm#luIj|laM z2{~byyq}n;C`*09Ajz?0G8MJ(sTj4zG(H!Ixbh)3W-Z07EpB&?M=sjUy$4?)`?uUUi9=bKnffvL91ImGa2v`3WM7nN7 zSPm)5m$$9+#K!p*UI!!_Al@l#^2EoT5I<}iT1Q~e7y%`d@Rt=L@a=f0dZ0p0@wLjq z$9<8+u1wjkD9}6~kihn1gjphBWO$wcsgcP%T<-U0L&7t^s)mPR`{gTX&K&H)`(^c;XY&H1)2ejs&3|iZ+3}IvW zStX5`jHN=lt}BxnFG@=}QF>Qn2h#L}L|d^Z66|1TrhYU<&Q2K#cES^AyO;RhZ4FzK@nH5EerfOvQmmm_4t3Irm;K9L3#2;t6Ou~k3JjuZTUm4uVk7|51n(8v z>x8<+SZ3q5hSxZoPRy1z)F+@Y1!@qGhgGimK$i;m`6W^cl$d1Pt)3!T3j{0=Cdc94 zQzPlYMnKWZSA^q)RISzV3Day}FzqmKzhGJ$8QyL5o^B}<9@-a@sHONOeBPdDOGr%g zq)#T|oA7yK@{yyYPlm4r@M$;s$UTyqpO2x@Yk_;D+0K&>mXhodvd)}$jJ?a6=q z{0Sz8^pmW8mW|Jn#LF?+jO+=fy&Q1?nXws@jYY2_ucla$$!J0M2V){2xcz73+!=B|H<8hA?+?N> zmU0_)3MYPl7HK4<^)7*xPcn#sr=1zA&^9#MnudjRRA|9G{ zq;GaQckrYk?=4uh*;-sxw z)MphH>Yc2RbMUH7sz47Z_S0{WbZiPQkS$okE+BT5OK?8Y5aYcJ;yKTJ>0Q31UR+92 zXU-dKuLx7;?0INy?cCtFn1gn54(?(8q&RSk@y*?(0Jb9IW?jgUU@!h0+EV~b?yF}*g z&+ZF1uwW#0Fu|}U=m#ar+OkF2W}PE-?Jp#fBg~eL?<9C^d*|Y87rPK4v9*u#67d>* zCp6=7Q!%I%xHVId713pt!1(@rybL$;46n@;+%e@y!R3}N`WF{3->Df`k~!s!EhN$f zv!5SBZSvWfJ7)^IwsG@J85Ai{;cZrJy+h6nhQl19VteSHTu82k)b(>Pmz;`RoFrq4|J%TWxG&>4jwq6k2mxl zkR6y*nn0Q#S6qSLQ$Mj4q7ixhp1oqh8V%xAoPt{eJB^-IcVJVsU$U+PkXRYAu&?#a za~>ar7OgLrS{n4ZDxgRX4xA~_Y_vqqq4)@f6FMeBj=4GGOZ!rR<6;Kf($7WMiV6#} zgdPEo0@8M9Fj7%gwvbF>>4|u(44tA&P&@=CwZmw2<0tH#lTP(cJC@t%z@9@X66yhx zo6An@9@00*lL6n4m7SfD_UuQoWzTd!)wmCNvUq$jFset?OwIjTC49XByF92^h3!N0jv76t4QN(Rua z3h9YZffDY7nL=VZMOx{U!+FK~xtm*-bAQ1)Pfu2by5YNknkYD$wIJRJJVca>77XmZ zml(hMtToZO%%sJ;adx^PBpq>W%Mixv7qNSdYC&V2Cy?Hn)uzeXzgIs!%3ny$sy=*Z zsM_Gdg%&Uq)(>=esouFc61}yWOU^TMZd1}z_VGX86KwHWBA;rRVr+=5;u%Q!vPP7y z7%%{^gbLZ4*76#PXOO+F^;FI}=lG!WQpB7Y6XZbSncPkc=p25i1a`PNQ75AOUZB@u z(;Wxmmv@HGR)E3@jrC=*qFo-njKe`dnZ{rq*cyWJK}1IL7*9%TD4LW$Qk0V705`P3 zGsZs9fV}{rLCR7I!iOmIUGL*dojl&1`kgdPjci6T18p!j z_A?a&XZ8}d7G%8oP;G*RARK@0wVo?W@ZbSi0`trJ@Q6NXy7N^=KG-wCsT&2OrtB`1 zd-@B8n#>;f9i#k}#Tf&*M%x!mj9R@!Nco3hlq&V z`*+7K;=hC3?&+5>QV@VK;q~8Wz!aTWY#>uLVUd8OIB$QZ|EMGPA3tH#BggcT5l^D} z{6$)32*eR+GV~TZ$uoH3tGiJb4%_u!*|e$f0<=1;irz@0i$J{X2-KK={m9!^?B-TFe&t9Tkq5!`1D{SegX4d#JSnTA0XG9k8!!c1qU^as&>Yc5eqnOs!D& zg$9)6cr@TtHxGUrdWY`e=r3;t0Zu5ch*TTY;U>euL8=(6+YO4R5Y=*(fjI!{v}-!J@$<^AoA+5YRww; z`k7`gK$vQs3CffuloQr^a@V`Qr!go(f6W3pWrkQau(`0k4t0mx3ZYGMsqnkI2JCr6 zehTS=DwG#Gmvf;ndXKa@&M-(_k^#a_s!+J;k6bl$dT~s@Z^-??QXm2u&N@|5f>@8- zO&o|f2^TjG?V8Zx>mSKV;UliYm)8ZM}`)LOItawue|i9RhzRP+pWJTS~UrN8Vyt zWTH(_ozZ@Ev!ULpSd{jbMLb6y-Ucjwy}d$M)p&Dp;)P394~*Xq0d8b8KFB-wt%o=sP9Z3aoi)_G1!?+^yUEa!%u z{WxoQ#P#OP+4Caf-;=zX=2y+`IdB?135L31Dv&&+w(W_oaN=OIgfrp){{2_$qCK_u zP7+G3L<+jU@|v&0Q@ChEOK@;qexl4m7=49~W@vqb1uB_|SO0LpRlg){F?)=ZloXho z_6?qTQNActM&>gXC9;MC9};vRZB3LupahtZaBZ+6YGkR=#Bh!=Y0N+OT6_TpAs3%IQgi5FcIpuvh?KY!KcRhdm>UyGt#)tVlg2R%l6Gc)!xUotjb7&hp_ymRp>7x41z`V zi0Qil^^zbt_Hss^3s9%-Mk_+fIg|0TI_^t{zCh|^wXK8(6bfoDf6q(%{ZuBR3I?FolvtyLm)%*@Sd?e=cSY4T@vNu zS!=4p{E4#oglh}Y0!Oqtw&w0l5L)2`r^K(Uz^pC}Ry=cNy^#22o@UT~VRT-zGvZ}o zHV9w$T--EJF%qZnln{Y^H{J_>ewd6U#LSyw>P(0c*xU(|~o-EoOQ6cy8G%u(ISa(oR(??*`I-u6I_f!a@_qJWp zx@ypWL2(h$j@{?yV;$pB=55UddaiaDtrF`VQ+8)G+e!oSO{kUud4|-FVsqve7hi%v z6vBKF!mt-c8XL8H6fIh#>fc|K#*S)l$!CUHcUQ!&qm96wDvh{ z+rZ+OF6(R6k?N!O{>S%kNS<%ZDtX=lmTV+s+sZPQ4~1bxK$Q77hGRGoXq6DDtM-TE ztKW6<5`^NpklD4_%4?{1lTac66Tm25540J&8Il6E_Y_3}zgQB*LMNNB{bPZDRuoyo z7Pt--alspH7#Y zhj+42^T!Jt_bt0|;`jpkWeYcKSa`K^zvLyRwVMx>E?=>4*=(KpXVZ(>idy5_9x60S zH=Wed+;!u^0&Ojquu(tq77o>J*%|xOGPtR{~JGiL(C(-6&&jv z*|pxesr$X z&?*1uQeHRF%o-_f0pdRqd$$!e8NWS!?0H`9?PPtcSQTNvA|-uczin=ZbK02LJ=1mh z2EM!7Jg*ZmIyU3_G}DsjU!9f$yuAO(X-PT#bkmZXk5^!#zg=c~ltd`REs?HDS@rV4 z{Y zt1|cAQ+~F6*KD<4*4cSkdUCs@Dmv$xSO^cluP^8CG0f8Gc|~JtZy8O&XP}eCiP=A(<+xnSsyCt7*CM$Jwz%{{PIOEdAkG?Cha;%9npQL7YtK#fukE6!!J? zMG^~G^o0=yFA(h!)%Bop(w+4}r!8Ni(j+D(1~KwFA5^}!L8X(>vxRzvCi?}4Teq}z zb?>eIrXlcMy@h*xOcMPLZ2si`gTvs((ifI*|@4L6`?AS`8(&mYneD&Oxd zy0ztRCod_+zcZv{q@n3cI;R( z?~1QtN^)}ikeWu16LGyfubJO+yk^e0wH`fTa1ZA-GyvJ9AkQbQu3m?NgVVIa5EZ#6 zv7>NA@6l5+1?`NNY}?f2)CUI#OIvj{HSf}umzV1a^;RY8w!cwg*EG&ZNqJuIE*5ug z&u9}#CN~15laIF%a$5I_!D-!$M^Ewa0A(R$6+sIWfSsQ{ecE=+y9;kG+KEBLW)E~P zpfn3W=WgbQ4 z7Z(@6<^=8n7`q}=IUdqv+x)D!SXi3S%!o!KIw$Dbai{gL~0@$Y-Bg*xpmC#PeOsXlzzh3H%f(LLx$hvu%zybonR zlqv%^NK(86lmoc}l8lUa3a{Yn?#uZV4h{}THyjcZORpo^JzBQ#mhjMY_gbpzrN`*E zw?m+e{h;zM`6J=YhUd=t^VqydXm4)^y6zGC71>Q2n>a^8)UM-X*bass@ZPBwE?g&K zGYE(bQtFi!42EiIPw5?+qv(^6cdXqD-B$GWes!sG|BIp!iS+#d8NmtjtOXjj-9L3D zq13`!yOVUr)qxJA~UcD zF`2rLIcL)nD|j_((p}H&2E!2Zkgf;)`cOf_ zzhT2(J+Gp=BSpcXp(5rT0Zj*ELEdxAgetg1^3Hhyc95MsN&7PYNYQO;6#lPRxzg?K z-JgJGEhBmAPCPl(jh2K3xB=kfe8R%z4^q?81WlT~nq8YXX|r{IDcw%q17&VTMn(av z>g*YLz{epgj*^`$fUw@35KK4tbitj^=KDOSbzkD`F_GU%zJU%;d|*tS9=NK7UR(@~i&G52A;AVSYaP2flmCDZ#DxLzO3> zdfuEl4|eH4e)?1^W3T-aOtogzA@PDy`Sa(c9}ard!2ZR*qK$bw+T7t5TiV*#Nd`dP zGKHPHFSnn|UUU81Sw3|}M!$C%{NJRWYD?@x82gfVOkB_@7KX6D)zx#fsl|%uKdF6E)0zJp$Ji+%g$wE;xIfQ0S zwhur1wr%xztWaePO!_dN(P{%#advha43Meyo0u3YAb}r0e&jDRa!aWQRsHqrm!9!( z)eUB2zU|w$Z{A#kDnxxD59HrcsqqjX%%59SR20*na6aZ3BngZJJk&k)eq?!k@tR4+ zqdwzuCfXQ7df=kgdx&QKM7GEBO<>@59v+#-C;75JLQ&dsTtdPUl1kN8RgPD_em%V- zlxTi1yzh&}-mf(fnAZk^YiD=85#6g#Xy7|sWlBRWL&r(AL*Y~j7%CdJ8B~xM^fG1 z$7mOjv6;xp$e>#sFEjHIf+3gQxNF@%eg1$G!NnO1m zeHLo_Q!`ono_F4WtQXQp5=wEuzUnA=3s_!pc6M%hnoGlo+rQj*(0YcTz(48!2tMPS zKX>lik;ibFKE!`H=TtKIb#w7`8Q#0(dC~=Y$+Y4HCI`q@WOyT=9#J(}I{H_n*)Oy= zkiIQ{L3~BkWas~}zYUIzjO^;_x>LURITML0fVjK6X6FDGESCSt;?jQ&C8+D!8m6>>x&U5Gef*i|Ao5{* z^RTg=glcPU?TGS9P3l;Sg0_-sv^N!Nvk(87t~73bKN|3}*(vblH7di0SDh>&Y-HdT+Qq9JRd~Lvp;DRavR5rgYT3Q5d*?2` zu6^N!{`}=h*By>9JX~0;5-@8PDe-_zA{EuGFa5EVH&O%AzZAw*NKj|Dg=WP*=k~l0 z21#pv;#Q)&R@-@mQrsX8x-B)#CZwKO75ET@A@q0=Cxs&OLlDV^U?cx`LqZi9o^Wg4ki^Ltipskl1Ap42l5Wx%> z14NGrqR}W~v~9c%u^xvq;b=hJ`xmO>YlI#^n_XA88Eg^SpIZ{aQbA#RByyDqU=IaG z*(^L^Xy>A}Q$U~(aTPrIprZzmA1JVVIU<@#)k+lWRPDO&1s^=uL8tsp>T|0W(mk&R z2}$9;R}d{=6t>#6c1fDO%<0qp@I$}4t8Rq;M(5SkQr%>%ZEI2?`@Q`*+jA$yL|Xk!!@6EBu0{*p=Zge7+}J7uoY7qd z8r1L<3GxNeoC%Nv|Je^SF1Eo%$;-`E&+uY$|Ge=$T=uXgG&Q;TstHUG^oe%e6N9Ei z2;=0|T`D)PUrj?J5f=B#dlR)=-TCuqB!fOzsGs7$@U;u&*Kn@`!GDtWz19slV;DRV zgAPbr$$PAUQ3qoI1zFKGt`hs3)aBzrj4RqrD8Zkw(JTxu0eL0%q~wi<;Ar8_yg1G! zfLT*>co?B5BEScrzPXogyzisNxz%oqjyB&(AMe^to;nTaP_@xd|KPzSbPBUVtSM0# zCb9mAw1Lic?g#v*)c4)rS8MJ0BIK(Qe5{Lb^m~61G#do$gim$!THH$B^wc~=J_|l= z<~6^GxcP{J!CDXyI>A|Cv}j)m z^v{7IM4v8)PgDhVDe$(qk|B1wN2hRSRG~c|(F{BcnpVnP4+G61NAF^PSeq-)QpDbE zhwQUTKrVX_H~Y(lzN-WMju2=E1LXcrrv343>_w=LB7h&dt(AnYS{{NpG40rywZ85ZJ z=u>;SGku|euChCOhvaSVvaR8ACr-F1R(#?=lGgD$)uJD|>+Y#sgR?zqpJdz#b zPL3u(EN{sf&T`>{dNt=NdD9;&$}WKCWT09E?gprek8*?L6vFSWJhbBG&6^8_UhaHU zd;VwPiTvhOeY!PaEc~_xMT~Juva|-Oo8=4-zl4DbOItD|@IgUQuO#eX`H;5hVN%$K zhaiCj`^UCmxLHr)kswZV&Dx+b9|TJ0}RgG}gA+_TF_ zOLKke7NI1`XP%RvFB3Jh`$V{E%*EB4Gg0Gz{i5fq0Ga#A=UeGTY^)$%2OQ-d+HNn6 z>H$r1(LB4jYFpu8lOl8o@bmQC7GVna-X&qZ+4n2mO+_eF*hHpBQ4*z5ed#T;$oHS4=Dkz8#Zx@gm6vSvobe0KYMSQ?#>TwTM!D9g>2 zj(XP{Si+f`3F$V>=T>s``FAWkcitNqYG}~B{|&mnC(fK<`-Vmb+j7j5OVV;5e^m+_ zLX7IN@JwN3SS(OU;As8__VbFZm(?oUx$=U#`m>(!(t^~_=udV3Yb?trzz_XBZMKl% z>FP4N-z<7REnW}hdli z+Qi3y9a+b_alt&hD|XBEyEAI#vWxS8t_e!r&s@|BpLW^fXT;7)t{6_yvX23tb zx&N_ANf{CzEkwRuU01};7ChEFG>paD^hERg)sJU4EOXd%BdTPEVfZ{*+P|OTe{*q) zKGb2uKRb5h?0%GYt)lI?xcDpKTZgu;T{3UhmCvePK2#k1I%l7r`%hS-O>mz?L@Ka6 zL^Jss`{SPQ1(wX?)p?{&mmhq^`+`*XJneV;GHL(uUj`axa&mH7h|L}p%gEey>;!k| zzRXMWc|S4aFEN$+2mdpu%phzP`Q{B;Xug9OR$)rjRcJO5f;QMxE9sywbo=&gc%7%T zEVtw<+M-QP>gUg=BJ>`{6ao2UKtmyP7Crq&^PIJ!Q!#vNdn&qjXFpyA>G&ES(LM?3 z4#1mkL?70N?(U>KpQD-G{1DzE=$~kTwCRvzZ=KIY@Qw4~pW%}7jB=_Z%0_2^em{^R zx~_dD-ew50@c=2=FLuD8M415%z}trpm0Gx^4296a=oOkgA#DW7)UJ1*w-}(525u;v zmRCaW4%Gv6XK_YyrxP1mg3fwDBTaaSYMwn(&z#}n0n1|&6nx*fuA3r}o z9^;!6ve`(q0n)iKzi=&RoEPDt-g|KlE*cljHEoZ#@QEKN4k0A^;dHN+=q3Yzlz@v# ze)ir?sX%uT*`SwT!?%dgh^LPq?=RPs7h+e8{PL@qmO)F#BMFsZZo!{)2;Ucp!SbK{ z@}G~0Zf(X(AXy`eysdA>dsM*#OGjOTnL=TR64AZyzN+@=g318qf@Y-|(9N%@W9Kf!FD%~I{}`q5$6t$MW@O2Y@$SY1-{ar-KfR1!)wN8f!6NwnRV-qBrv5*HMf}55JtwCCH}9le zJ*4fsOOo4HQ9E>~?_31|+`Y4o?z(0tzVZ{(nm020=TdK2wDr-7J8Nd@zLuG9hU54p1*GO>wC2GpWM58gma;_ZMRnEGp3r_OUfl3odbP# zslq!e+dJY+LzVcVEBaDIhL_7W#dSWk9q!K&JBRYlVdYkW>~Gxil{a6b@@8o%GBUQ- zNYWM4UA)$%8}lic`7U|Z3-3+RbEQdvu8pRe8Re%Sql^Vc*BJhDj+6W?dC~^bTxnjh zfJFghsVyYhaFJ7MMS0YBsFSY9tQIezcOlN;RHCrck%=~ZRXBMsc~OA&9r4xTB%oug zYm%2uI>%nsUuzbtIwbE*8h`CaoR+t*a~@FjP?05mDU-SU#jF?1U%Xk{qw6UKkigXLx|PD3AKZ*Bm!AGo&+cLl~35Mq=Q!;Sqlim-PSB zr_0Q*&i_gC?IeF?>H>YRPgPP}ToYo4l9G~_ zIN9NG)TAQGuVW*f&ANU2T6jXJJv&Zv33|lI@@mTRyqiwO<&U<1$b=^uw=1Sg?Lh`y6RTyHr$hwfU7mezOga-g1$Sq_5f z3FvE=@aYrM@QrNn5R8G@ZWCEOWNXXfB!79DThdc4e|63-x*X+fh!vQZn!qPR;vReg zQb;NA7Gws|_gw7YK~zTVJoW0)n>R+-&VeO`{PLOa6m{@+6WjZSYVFWvfPYr5G~7C( zv;a^;Gn6O;vlCYO@JD22wlG+gy{gPfS+B4>H?N&)ywUVOLgsUdLUrNBjgNsEeoO-N zK%gP0(*b1r@|RC1efTVDPO5hLiN?0l$WImhXws3TIo?THx2*|T7IaNQYBgv`i8g%J zRFk`_ESY}rK?RC)02ZI>6R-7~yM$iY$_&-H*fL%Y9$FW|NldM;5sw!?;nbPt?Qxr# z3KY3-+PD!UsjIi2TbM?D=UofoVMcyE#Y+Kz0Kg__tpY$y9?IfTpF4FgxRmU77P(d~ zc$L_bqWizWdwH}s9$9RN4j`*-^z*A)PZF}pby*MT7#RuOQ~03Z1$^dhK54yo!}N2q z z#`=`iV0_#&P9N33osz#@zG*$w1rzsd?YOPe+N3~QqEQ>DF6n7$rA7Ms`iLACQ&Wo+ zxNBuBn(8_4%l$7l=*IqOZRrNB43KCco`NWRAzNK-?J?gy=EyB&9?x;0T*RN*Day1> z%Gc2r1ll1a4DI`O##BAy*?3i9hGl7pHyy`W6rL-ITCa(#JVe4iG|#CINlj$xj!H<- zq3`jffH!a6U^_y2`&z&PZ*8x^?6F4sr#Y?L#h#cLGg9YMszzIFQs5D-WE>Ilq3`pf z@mq7K3@7+`D_5*Q5i_ZUv9G*`3tNS0krI!(L}?h^W6o-;u7)S)xYe)qZyro&A^Qge zyech~@|$z%-4fbPH0&#-N}IYT7Yw^V<2Qd$NEQF6sARP##$7;*7R#=N>M~y% z8d~B`FLN(j*=eIv)i33DT`p3Iw?^+oS&MdH_N-kJOgb3^q98CF8+f&Te@1QHdl_?m zFQGFA8V$#7`{QlvB#$mpc+YchTQ#Ge92Lpt&V4`X*GW#~t3yph&XfzZM{Wz!JiK@X z6G}iL`r^fi73ZPL$+~ysd1azFI~Q$KSc0!xs?8CpC%D4!NMwbj`!r^bx^ zDhslc_?L_!FqC!cKZf!gQ{_tu9$V}baAefXP!JNBj-_^FN+>+lGW=6t)+qHNK<`_( zHg4KP$D?`Hc*XRl;h&CLR}nA}76wor_#O=NNR>-y4Q*_O(UUDVFAu#_-@s_q)YO2) zk&%(%7Ah|-UDa=-qM`!+MbbVwPdZ+0nHJM$MA$jy#xKot5@2HTKkT*@i0H>8RF;79 z7OD^tLZHbZ@=yaaX)Z=Y=S$uww~q)2rVhr4t`pKkh;O|-JowPT>bp1s3ShBl#=&XS z@P%0pmNg?Yla-lSFw>0^{7xOkdna?WBqbDr-)NqQ@oBfcedi9sX%xT6y0aYj;-~~A zx3aPlDtX2xZ>t_D`f9K91*cec6;oUUqM47r~e8x4D8k*{017-f@O%r#V*_~ zH2VSq&O^5!kv5`PLKNa{MJaEccQzLvAK%ukb#I34eqJ0+2rTH9@4Z2wd|yWQ(fP^| zdPAuJhs~LU7%Dh|Zq#KRGaDIl_}-M5*v-k=X8+5|D&Y~c%;9cjr;^pB1Qn#J0TOOD zhyJUGcC* zr6}Or>KYny1JTpZIP%2|v?c(lS}*BDk?ZjXyK+D-f^y6f`du8m)(+Mvj|IHN1x-kG z$l5$vQsjjOfM;41a;}4!`!qS|%{ph!+h`I!1UhIn4qS;ZF!v#gY z-rinN6(sL-)dC3#Ip^0nd)D-u^MmOEUiz5U5^I878g&~6X=rGu*K(Xi3sR8#UQ}vNZ?&&E5lW|s zW6*{C2WXgD8E>7ZaSjiwvazye_8mfQ8e*^eRF^Is3rD2flOeLkBt+MS0maFMmir4Y zJ|6Jw#?X)BBmLgVy0_^_a}TRR4F3{pv50EWOW!{rT*~tjy*hRo1Z=bG7t@Bb>dCB@ z$>v0@6fnbVk_N3|;BQx=Fcgw*CbR2lM!)q&q|db9DXOk*B?a&K@5eX$I*&xnU(=)ul}9RYr4j633_% z;s(*HY2$wDjd45JwvtpwIz_&nvAI9rEIIN7X;)Fbmt2S<(RcmUZK4ZpSFQvLTdtR= z!{`l-%imoJaRM`|7aYTvktm$Ag)*0nVgRT8veEc(T<)$izScN{N8NBN)SR4Aql9df zk8#O2$_`HX7|qkYaq8pwD;oTN`!*%3NAa^JCjcpvXw zmpbJH#61(K&arzL&EgVIXY}IZMu0@>!QUsussH3rH~t-UW@S6RFX(}RhLBsxr(q<{Y1yZr#yF?D`=+Qw_U-0Z#Hs;Ng z+_8NIYAlKTQPRiL7c$@;gQd^T!eXeLl9Yra&X&hm;6y?X(~Da}5`F0QTeH>2hQ{@4 zL(UTwYqu;f*R!EZW8&Oz@YSUJiuzgrnV~Y*7VFsVHq_%jf$tMJmxBjhVM0Ozn4LHJ z4_JMfl&F(>=sdrpvy;$}%N7|awYEkfbcV0cw}Q)3e);CX$bLauZ&zQh6MuOxgv$zt z`+7H{n&rNR@4`iU5gCMqhuf7aS~`_nAX-*W6CQo-gCr@kC#c6ny?_zA&+*h5@|$fR1SGmgB_<{Y1_s`;3t@xw-S%r5#%I2b=A@JYL`M3uiJE;R zaKHiIJ$LTh!6yy!AZup>zmw^6u>A1A)3XOf&YL%HzSbDL=hgYt6{c-%>&vL0R;i5il63xhI~n}=!MC%Cw|tyDA`QC4D?hbS`;Rh{}dF(^{-?sS4f-&*|IF4?M8 zF`)HzDQHH76yNvHK*SIs5)5My>2Lfh%~EdtXaT|%qJ&m-%bO>LOjhaaIwaoOox|(( zpzPP091GM7`iX0t`bwj=*00aGP#WoSOIHo7DHV%bS(Z4#l#uBxwRWMuAHK9bM* zb!km$TJ*OoP)C?g13}Fm6rV&}h^Y#Tp|q8d)&s$)dOaWd!T8|Gl&VIRmX<W#Qv34#x(F=5d!*7rj0&<(buxIBt#3$4Yf?beaoI_*V=~Iz`)ZtKn zcgTu-X;qWei9>Bf7;9x^mGluYIZ8K(R_M5cw71K8&ITe92@%ZmI_PWts{}P2L={bA zQ0UU`s0TfYu>;UW$Ncv*tGbC91XYJ?{f=3dfIQiU=$uNf(WQe+cFo=%l<)#2VueSi z*Z7HMpFVf45?k32icXNGB(QF9v5-6_(#d_C^H@-b90{=q^P?&W-36#8kBWYdQQ-O- zAx})23R_-0_gxh9R!(is$ZpS_^3r|o!ziQI*Ku=kIk9-#cOlyF&%4`V5U#DFqf6Im zJvOK3Ve4&F&o(zVBl5g~5nc>otT0Gt4Y_U9T566ShCJ#kTYYoO9&i1f9**27efl)? zB^uL2c%Acl2(4R2kvQNMY$W~=*cEgRG<4o88={yh!g+t~u1k*w3Y;_pk0-8dy}<)o zU2i`*D8vHeC3H9ulg0vz*v`ys(Bu8`^xJ;u=~_d6;YPoa)%SUCac4juB#s>$LV*c1 z9}j8?>tJF_^d@8S*TWrniN?MtFh>VXw#bI$w~+G$nb1qnSX=XW^PaazjAQQ)@GSmhs%&qwv_3lK!RC^N;*8GKCI&&F^f7&C5F7m5bl^{bV?epad z?*Pv&4(<-TK%yNi#W=&15zRu~Z4+A|D0!(iP3VKq9&-a$-n#GggUllH7Tx)|EN_uh ziANAa$oA+;qfNAphV55;vat{ljmr$1UNH46K}kxvU{|mc_;r}!+Zf|f?z4TPMrLLs z2$i^+LZ1_tqN-kbi~xJ4;O>uZU4SNxIc4da3txUiflA6F-)NoC*ut^_Ls9LGW=3+g zbC*L>yMDEtV&#gE*fA6#VYSQmjcsfix07`>F?+z`Ly5a>5?w|mJt~S)Qc~n>?Bmd- zDY2jybci{j+ds%}XJ;3I@<3A3Y8KhSDw=pRsF5_~zxWh>OoKJh^mpn#@mMG_Ff?V6 z6bq*}MCi}9->n9}0!)vZ%&@F7JX)0eSd`n99CjI|b|9EdTeU8`5$gTBymys<${gjO#_87PBFUUe2>23=heh?ywnv_xCBB@Mg z9{bY8-7n0q7I7-91H*JQmEUA>h9d|QA}u|=I+h{pYnu&(IAQA7R~Tjsfz^vf1}KgF zfUV|qyNTPb-J!9+yd~Tt47h3v;{yXfmwB9lWOu7%Bbq=MN6f__CA!$0T*-3<)2?V4gzb;qQEea`vt_ESo zzxcWz*UXT;LoeWpZ4V3LyQ%^9R&N)VkIsRSfU=@K6}mu4NW;A|jg+n&E%>a^ z9T|K_oSd8hlk-K*5y&9$GVb8ywAE}x+7IG_2fOU^o$1|nV$`z9Mohh?=?y;#HVr^g zATJu{;U23`ht15o2;g6JS4iUd*PTxB*-!AcLmZQIo`W5@a}$sc322|u{A!yq2M9Ew z3n3nt<1-Yz&mVF}DCF4k;Y4C)rSBW*fNi!0w=eRbd0-`dS&U$qdQgpsfMgh&mnB0( zch-~Pf&2-v!3K7Qit;alZMa1Rkf$agA(1f%dgcbCpTV=Jfq}|!lq>?~urs1|*|u-T!#Wv;#nX*BG`h&GL{?|!*Ve9KZlLJDV6(vSI{qUt z-zGN)HwE-uT#V)xqhAyA6I9c?@Xp3lScjPwea!10k0gZ)Sw5kw@nX9ks|>jmO2D(Pi(5L6~g8jh_| z5@Uw%1vy>%1ffw8`Z!pgMPaxdJPFCm?x4i7-{urALy52TxG%6q)~VUeS`H3_u)4=` z4x_Dt%t8Mmz&Ei@G*L?&1_}t^nq~4j3V$^KvL9tQe^%Qv0U5E8E)o);L}R84zj* z^5jtVT(3xj{OM}u_9?Z)YS7aZEC^tk3sDeDOu=iPl5$rwft*!h>f{nPu5+ZD^Z!t<- z$Qy={6Dt;3O%j2&Wr!qKf{Z}zQ$s77xpT6NEeH=Ek!{=VLrcPg4Mv=gICIsnsb^S3 zh&;k+1S#AaS5%aP3Bki*^b?zerQ22n%OQ(8K-CO!h^|;MDS&x+IUp~>_)`L-Ka1EN z35f|zKHda6-%rqUYm7o?1oK3h?N=u0Jq~A|gGd07?MVPn5W~`4}OHLltga$O`_h!AJ+{f{!mP9UL5>MaGUDlW|>Yo2oWp zcPkGm*EWE2o%i|eTR@Qkz?JxUE@bN0DED^xWqN-Rb1(Ss#@zpw|IY&e$Nh#j*0Oq5 z2h2?^%+2)7Y&Hrli5(USeUTJQ z6Z+-vAAirm|C@5)N4rlx?Ac#zc%grH!~d3K^FOfR|8 zZ4O%a_QBPAm+9NHLPsDG_Q%IK_U^0$O66MU%ICCUpp^B&RqO=-w7n$#dGmG;D7Qsw4!SG%PUCu-@St7>YV>cpm36q5b5_DDAe#Uo%*`s#A|Yl z@c|d^r9ayI=h6L}@7?V`aEL?YkJkOMRNrc~kA#}9PtgjE_^|8*2m4)xRVm^MTMe}; zwdI2!+|$?Er~Ng2;1y3pU9S2qky4A%3Gp&9>$c2x8`s`a>$Z?;z>IQUIPA_~y8ZRu z&?};^^f%L{1V3MBy8Xw0{GS@I0SP#*xFFOD*wHR~eY2k{U>i29%onk|6fhq9TEAj% zX#Y8Z^$ShkEvz+Jk)lFtkW6}|FLtM_=zsI!fQ^lflT%h@Q2RRHz| z$yDo^n6hlXXxp=I-rNk+5?BTCc(?u|0zM*m4kZBh8e2SX=z;VtCuHjbb`!1};Mk>e zNdt`uwj@)5I`{*_`biZ=n5vV?%0j1~TtFuH`!TzQ_wPRgJQ;AwpIi5ww+n?eXf-pl z&j>qGli1jz5)u;V_JO8p51e)Y!$y0R`t5SZdEIWoZ=khz@4OU6K?M9;Ar*~VGjH_F z(uf9q&v!NO@QgN2Y$So)u~I)!j=9Y|S*5GKlI8t6=7|(*c!B_t=K{!WU~_^-thxE3JPnvzEs5q3hABUHS>UZIWRCVvQ`5GioguM$u7Xp-;WpdlJh5F z(?ZC`-5fXx%8@RJfy>LwBgldWZckur9DEMbRau#7V_t3sb1Q!>u(jT{EXvS*b!VTD zXXyNjHMO0)y`GL1`={6E#fv4)SB?*;PW-lBj&Y#7k@Y+t{p6H_t%H{1qmH6uJv}g$ z!EV@?Vc7yeZE@Gp4AVLR033jT@rg-ExByIHT_1ASZy=b0Pi&i<^95bOX1IRb<@)vvLuR9|$Bh55$fND89=B^%`B z1|9c`O)V4vtaSO&^2o&UXQmF(+W`s0~LA42P;w6QK79{7e(Fq zqrU&=&opNlL4@(ZB&Dn%pqXJYzh(gYwPWB-^c9J-!xvU=>k1ehbx~lLY?4_^AtOfi z!kwqpiJE^rF6=Z&UQbA_`ab##8PO4uk&zGOo(3FC@lN@6{Gj(s{f}abhsQ*gP(ESd z#&FYG~Oht?T&`U5F8Xe*ty)9^dTfG7F&KK!mDt?}2`}LsApqKGA6}deF zoi%I7=FQzw3m0LP&E(|%5OCo#k*xFyp$=pP2ysNjKF9nXrTUbbEsCRSOq9DDvfHz0 zt&{!WlT=btI?y;g?9?ygkr574&O>+aD_7i&Vin`Fu$7M-S!!n(Akk=e$E>`hld7=LSV(TcCNXaH zEQOkVdk*}lKOU;O=ub~RS$gUYI|V9a5y9jdt=2=@xUmVa<44Qxq-Dm$7~(+67zK*> z;5sIzkbr>g0-j6GhheP@EtlOFj-yMwOBV{9Ur6m`=bf82?Q>iU8{7OL7DqGq_!Ej{ zAadFvL+h8n-fH5J4ZFBS!UDK> zz3pZA4`;VYOj-UsDPP$~|NLAP;`w)Vg}Kkw{{da$U%pm(1%&^RRpb|YLr<)*?Z(i1 z?_TXYF?v3lW9iYP<9Dr2Mac8ACogI(US_;e?~j+VhXXW|S^Eww-?yw}mxuhzs>|2) zgh;`utIBrmzO&$~_M#nqW1mz8oC`}lAGdR+C%3oI+E-+T11T`^7Ok_T=m5`WSl%DO z{5S7&(Z)%%;-*EDrf7?clhHXs2PR!eVZW#w$|tRZ^UnMtci;suHq76QzlyE+SH+t4 z@2Ow{lJws0CFdpxU~#n)q__b92fl|Ppf+Aw2oK65hn{9Ynd2h*U_x+W60T)}(HHO~ z1~pGdy7Tj4xg}^A;9`BNec-@TTrE{*#x;ZR@VtBX?twspp7FdC0wG_m1U?fXq)%PF zx{7>Y15E}{8zIRO;JAJD>eUAm*%T5h_)7^dyO);}s9%TkTah5Ja=k!w+3TSBfrPUx zBLF#1C0yB^K7h%F$GOcHz48AtzC;nP$HHkBMg3~@%A{} zqg*n;ss@ikPC|m&+i1BWrr0XVo<72h29kA3A#xC+9z5VudGze(b-=>oq*}RS*De!d zW8NY{4REJsNpcG;Dv~6>L&m?aDMbuH87B}f(qp2n-Ha$1MD#?Alc~}(GaZ4_&BDS0 z*AR{dWdw`E85iiCgseIwcd)awzpcA9;Ya|E;_Abcmg1zu_06YWP(_GN!smu47DGL3DI=(&R%(rjff*@Jvgt2jK z$192l<%6i+%e5t$zBLY`2*wge_*Bs#e3&ux35)9lLa$%H%l-q7M5t{(Bv41@<>hH; zXb`xAt@Y;Px7^4VdkUWstfc@4EDEWwSL4@05(o}+F;@#F-lp@!4pg!MNCDg)wD4&W z@ajWGaXo!~qu4z@B>1=y=ce$L{FT3G9bgoZcLOKlZP#H#Z*`s90;VCMp>ez|BLOQ&>u{-tMyV z&5V!#27s!x{OG5zK72@^Waj6)0dM`-G4Jf`?B4Q@98&D!M5i31pO_h=9w?8P9-|6i z!8`ZHU`H(nqr^P5x4^PqxNsqnlnF=k3C{A{Vk2NMQRIJ%RHJow=*bJax>}J8BWL?g z8#x=Hq1oAqo*gli7AT~Q_KDs;KJ^cEr0iAgJ_K=2sEKK-Q9mMbPGc2ooZS2krdz4T zof#V&8?R`rqZ65)p3YtdT{uNdRYir7nYlBk;Z!G6FOXOxj8fvy*R5+0@>R7C+hhCk z>C*!@wMGOx?iLrRK~q@08p$^4mMxN+nvFQF2u!UXTV|#(0r;{2`CQhZsnvV z1TV;Yo0*vfBsVTeY19YWF97^MAOI2pb$#!HZX%hfs=6A!$B_`@gopVLGNrILsR%Nf zgkx3j;CUNF3?Uqttc1&aJh=9dH+?Q>!J+PgF7|?HD-N|xyjBq1gxC`0@?51NsGMYP zGM!pH-{RO>JV;AHj!;+D;$zG%VE%!5!v<3yVX%61G#cGw@1n^cVgP?fXsB_%4&El@ zL&Bn>IS6XTn+F$t?&$OB&TyTmdIL8bmS0%(Bo5y7*zy1+hLXd#dGr38wIIR)>ITxW z5L@I1@UQ^u_0|y0!-p?d8Ikq&@bRVO=AMPmSQA)P@Y-Pm_?S{OTtPF$6Am_ghfY(IO9^OCw zO<;7e9i;A6F!b~HpFpswPoXzg)?xl?f;k7dT}2^WZoM+L^?+BwbhN10+Yf-rLKm@+ zKdZG6Fri?Y3}^`D93c84D$_J7?A!X})^O|B9gp#V1O%DMdQ$lrB19X>+PM=s)6=5= zs}4(p2flSj)^CWd$}(>hmywaFW3BsYR*EMN+`t_!#^26>*r3MT2!413%H-y( z--oHSyr@Ips~WCLEmIJ?2Q?&bbr%e=8e zM6oDUv&_Pb=-Tm$H46rnh&%kwNw6@TIN=)}t_hOrz1_>_iT$P&1iKrSj*pKsjGrL9 zisI{%ije>Oars1EnAzh4EN z@FxDXNdD$tqLj#H;>J4dVE^znTWp z{|yx{THgz8fflKG*knXvCF>ELfP#OXoK=RUoSd8p9YkbS$LG(o-f+N|m6zk*J7wQu zSh_lh=bvP7v#&713F6{9iFnWD%Z&{T4kJnuXxGP%9#ycl6S`fr%YEsVN#UFn$6t$f zC!d6H?_mP&4~J$;ON%c*jv?3wh!_<_KrrWB_*dmrt9zWMo$d%OK=ARAk%f_XW;8EE?wf|f`q{gT~}`|CL(aT#!M z#3m*22noIaIFBiL(RLrn%1XCD{U*)!A0?QLKlM!qGpD4n**B9yVc)aI3BZ3)<{EtE zd~1dz-N8#wG`3vcGlKk&2S-Z`_A!zerPDbXGKX*%fejNy%H7=R2WTjWAg zo7$&Nhr|4B1Sb$(D`Lr!dov&uL`?M?nTA{Hinq3Ko6q}M{7gu(zoYBbeZB$K;;B-X z`PvXWqpzzAEUP**o_1zml?7`aNMwCwj+Eq|7|3U_p~)3eOg%|$ar9{K5JO|{h=zn^ z+*D`Bi*Pj{nsJx8c{i7BO>r?B56`QQ#|#LGI@|VXt-7+ZiiXB}&&F-mEy_1nZBSQK zq_-RAKNP0lMe9fYb#}aJ3?cw6fqLIL+P|EB7kGAT23Hfyb-Yn%g7w=^JNYVq9rF33 zwx3@KU1DfH88YIdGd0Ux&&GnBfs9aa;fn_g_q=Vyf%o`uYPNZze8|Yf>A%7OS$N^g z!b@&3&+;@TPZO&Z05a8j&S?|Kk1pZkU!gy!Ohw60;P`6RQQi zmP?+I=M~g=J-)L({9mjHz`%3ecpGhd_IR%HZ(NU z*LSiUdM%qw&Cl`w?vFp!thoY9+IzF+`>qN;zWwIWOes0NM>QsE4|#+?tu-mAjVq9T z>JdU0UGw*k|D!oLnjjol(%S3Ta8B~_@$EN`-mj00-d$rN;C1(L#c5LkEmK9UsE_-~ zmc8x#`^W$J9Biu&D{1{Plw-Dc%fan8j@*~;l2Zsa5mIHwYISYMf zDBaNyg1xtjlA1(TQsqVT{*TPBfss+imoJw7n+>CDBHua|H{Q{wHB$Vq{@9V(HtaNh z=9mK4JAEJ7w?3cHeQN$K!qJQ`_LUu`F_N^B<-P6^a*cV;C&y^QV*^ttbS-~I-XgJp ztqw2PiJERmSiH%hL{DR9Yl}$Oj{wg4Xse^s0y?j8z89ziFZTu{UBIBSUeL@178@7` z-~h898@5F)RnXdCON6fs#@NFBW%yo~ZQl)R!;`J~puYv=BfOX0ot>lK=%O=^L-9TJ zNEet9C2(=JZ23r^iEIeeyX#cSbCEPkFWC1ICh!(+!T83EHSn|q-uP42Li49TDr;-u zTpfd3YGn=$f8ZAwvC7KIa6#fx%E7!MyWM6Fth)Nl%KXIqz)be!brOQsljzqP(Ku%U ze#JApk$6e*9vFbZ!O+~i5s&#`yMs$%`2$A!Blx!lpTj5q^x&n2UU(dHbYk~J zR3>r4b8GWF%dJf2wrEY>S?7r(G*6_r?0S#6Vs+SH#C39T9zF0nwF|v4)?z6$I3p25 zoK!SwsB~$9tYGc3FvsrODThIhur&|Fj{m@OgnHc8h)WZXaED=y42#o2_1_ou!k~d^ zLkdLdE5W0O%A`MD89_WQ^kYrHCWE@^$(jLpc%^TPeqm8cWv2|G8sN9+MV)?CT`dSy z51@xm{G3na78%_(7|dNi@9CSy@oXzrKJK zjYuf~XA<%_Ef(&2rhXx$WB?3GJe5d8qe(P2Hh#Le1ySH1J)PCaCR$~W@ePKl7R@wZ zj)Rp>#kEFp>52TaBLXJC2WJz#fg#?79s0Z5;sd|Q{=|YOuP8!)p>yv3@9^3DPw1Tg z(rH854Y$qj5grX^Eyz_713f22YL8Wo&OfwXBxUgW^`#pRr||7uWl^9Zbzr%vl(F}L zXADd3k|PvT4z2n#Wn<()?LGG+YPD}g$geDXeR>a1#OCT~vIVp8z3S6kev<1J$E{cy zQ9)^k)1j^PQ%+pt2jlp$u~Ez7dw<>@u}!?CDSiEh%-C1WiKBoD?9jS(e{xrw*63)v1Oc&%IzBwt=ZcD^>?3<7P{!2jc1l`zWhlmPiCI9Vb5hVF zUGi9XBUhZwlX1-rwkQf^y!!SX3O;2i@hc6M---dt*m&5gV9WSZQu1gsDafPH{e`?+ zMM3aNZ~^t+QXt=Usz4C-HI_R@D^5P4icvuEHs#`Hk9JiruN&Yzr=qJe0P_an@4dk4a%4P zR*)rb*TTtiBPiGTd%5y=+b6qYMR{p1@M86E<+$j>_eTX_2To9#TsV zUlV^wz8_7Ib-;3%tHr*%rhtA!5V!RfL`xL0B~zHlba4u;f}wo{v&*5lfcE$znJe3p zj21;xv|`y41*b676v{*5s!NObitE!?D|c3Wm;^Ub^q@S9*3PoQo~GOj*`JdEt^PJZI4s zi1U#0UXlyQItR&~ODNkd_^f9)8eNC`l>rZ%XmZOTvZn!MyXkKqKe=BSdun}^tRqGC zd`{Wk@q4Qu+^?MFirOF7v4FyUP?2Ab`_|(*Q>aBXVQDoIdT} z!_(y=N)tm7!ADE7Jx$EoIn(r*d0mwFYU-0!I~B;6^C(+1mtgvTolk_k_$+LBt0wI! zU&W8l_bJOIOg0wIX?PxWQEFxwrq=^?D^(>)&0T@{%`;Zs1%w2&Y zSFQTdJ71m2f^y_dA1E}gS;kazx~0LoPO#nTWiO>1Wpl_IMg!P6=1A0!fdV0sz?0%i zwv1#t?L?+e27b9l4%26rs2Go5{@$BWjvT9*;Of|ATLOWb`R2@u%_OMkQpjKPpM3oc7TNt?48YtzZ{H$8kJ?}C zr?Q)OCLAs>{DI*ac}-uPqJm2fT%!5bTag?@C%T7+CD$faiY&IDOhp~Lm?vaIT!+YH zS1uLSS5{UIUU~icH8}Ugo@x`$Zm4_&hIZq@F89K?rV{s_uO89d8;P69RtMODOJ^E7h*2BX?!#>j3eZ345N&~a(21m zYz^t{K~MyR4PI^Jy~WGjRtN5Ms5*xZ9pcXIyY85rlS4Si*O_%)9@8RmWVmVNgqG}N zr3_=2l`Hl;@rkHbX%gFhzqrJZV3T$Ng|gdCAFew%N4|ag1{W8&~+LRe9K>`~^@XFsAJeY7IWOdgP8lvhIg$; zQ4n3dD33-(>z}dYvygFaiqpW;6&ect%clxP8LjNd7mORxp_pqhddcH-osxFHUD*Ew#oQu#3Ukt{Pu3W zqbLuwbsAQ2#EyWk`0Y$(SQvx ztqf5VTtL}CZYR9vF)|wJh@vz@oWt0dC&Z_k#z%^EEw4qq(Yk0#lM-1|{MOG{3d7{i zbD_5mfX4@DC`$73R_|2K=#Ae%ktmwu>mYH;Ra%g^L`t4ZNq{CfcQ2v-EMc(zsa(3O zi#bTE&FbZS%BBd)d3$p1y0Q;sTh=KZBVY@4Q8B~Smj%}-?)dCaZeT4FSF~lG{=A&J zXw2U&(<;Kq0X;G#7fTfXOrexapEKGLJRi$i|6EA+I7p^)i_IE6t2VA^OFvawF^78l z^UN@>Y@=utwk4mDO0Fh19Gp_k=vv&D+Hyr@tTaT=2PkHK`i-%BE>9go97PLTN{+?0 zxz}CAHMk(Uqv_VxbCiTlQg0^uLF*&JTJbhxnKX2|R2$e}!( zR9E;K#t zL0NWM$=8_gk~%;AwEmE_@W%V;W}9M*c87Q*GKoz!yQ|XO@)KFE<%#KG+yqCWnZ^vH z_g7t>R+e_s9ABWx97#Rd?BT;zmnAOA9Io0i{aHnVr?ho#`lgi>_N`Gye>SoOgi)?d zclnXCR#1uIAslD=6$F`6Du8qM+Rn#U3ZfAvYLYDpebz?{#DG%Kt#r#h^UjVi5A-Z?$TPiXON zs-Ow^i-EKWdBgNKXns%JP9AalweBy-wvoaw$cLsUc$qLyj*#Tk$B*Pg;k|4*lm*j! z-~;u=%&__Co3J5e$vtQF6lPV6dU8ZHkngp^}?sdFZ4Wq6|606R;q}ec~2-x6J6OhIX8EV?9(K9 z$-iD!5YCt51w4Vt#gi+o@a^$Sg9o!O4HcGkt|yO3PY&Lll1nEZ5;I$;0QE(N;^%a~ z#+NEXB`&ph_UW`(6fz)cv0k2dQjDB@J`f={JC0JKH>^{=S?uUPclbjE-(QyuO)fGO zML(c%Zg&`IR&e`W`?XScI?nxYXg)`o(ct^-Ay}SG#O|e{(CqLxmyVnr$XS+kZC376 zNbgpS+nKJ_eZmFeus#CQJzxZ*? zVs<@ZziM(?*KD2|WWu8%678Ht9#<++OY@#u_YhLUSrEraFszL-(kJg9 z*5u5OdB!zWk?Zc6*)m}0g(emy6P^b?5v0EB59;7n=uQ0T!)w--`l%Bq3NaOJoKM%R zoET*zbGGa!E+&iLm8x~#!cP4B1g=BrY>(1cG?ofD{_uP7;6pfk5a4}}f<*T0 zVUmyo^ESd7lY=79bPXQj;5=- z+7KspJX9b)EzJ%dUqtVpAxZf(D%^egOYS&VNeFCM$)$j;5`9}o9EJ$MN|f5@9kaUK)vea!KVW2#jA*IwJMlRj-vriyl^uh87_DLs>Y>O0-= zs5%Aqkog(1P>t@Wm#S!!+Sz)|**_NSKvTpMnulB=)S~ZNTDy624wHh;@;70+^{KCrd)BR3dBac}hQ3aO_6wF>dRbf;NdS4bsj}&eAUZ z3EiP(Qq1cn&$G#C+a0pJcydALlg~WL-#1&VFX5#(AH(ITa^au)To%u^N0?zknTMZF zl@zoZ`@uTb<7F+WA>XHzBx{c@dEFfg+mSA!aNxKq}5_KY!B_1rW&kn zJ?GxVp6JsDQNxiE5a z*`ccBQ%3b~teBLX0&TnV%}1b7pj(7o5XKcJ>#=D;cDV%xsj`Wl?Br5_7LpXS zg|79+ve{zqh=EVdmlRub;^Wzl?4WFS;j^CF!?!MLUc50gm-`e5W64w>FCTO}@M&h@ zUXf39nhQ+NC1|#Z`-uPOLo%uES_3#m{!hAS7{Qy z9IByv<&+4)a&p0Ey2@|XpY-@=#d&i=$8)JQm`ZH?q##b~c9r=v)9iUUcFzyGW78i^ zto3GqPro~-w7BGafr+tjJU6xC+swUDrpOawK*h4Do-S|hRFk&Ug0erk8J=nI=TtH~zuqW^ z<2g(suzgN%9(z*Uef1fxU%W(E14o6&Yk$nP=?6ME45vFy-1oVCyKQr|ESB@}Y zOXN@zw%hVcBXhgdBweK?nJjYTz<#o47=tyaz~JdPa^wZBcT=$?`Si&!pv3+&wMkQiHICfAw+z=g(!I-x1Lg0Cpt$a+rmBs_h-P(o2lvP ziR#&Dk&!wx2ANEi(qLAYYV=_5QJkP%irhNKfiCU=hjwU5lvvG#=t0$8v^&=$mJrT7 zpAoqW7l|t!s#*(dA_lDv@-lMd2r06Nw|jTB7di zSM#Q#_N0qf%+wxx+(Qv}J@mHc-fb8X(<(^aU=D`rR`DZyx6^Odn2t{EpZ0S%Zhc}$ zgMpnyj6W`yZ`#rmVU0*E0i8tUm{@}wJT416NL$>>`7NQ!=kLTAhE1!-Pt3$Nsg>}d!T1W zj1z#uv?QD!2b6Ort}C&E}BW3{sYQ&!dpTaMlbQK!DoKvyD!~)?M7)KA=F0Z>1jHZl>|67JdVK67C_ena7MqpEsy&kz0`@1e#g zFg?z$pIhu6k&v+y9lMEx?`hcFPl>Tv!XdfR9lP+#KT8_<;dRroY38i>o=fqFtQojx z2En__knAx)<1I9!s==mH+k0^SR@+r%y6f3NPv=UECl6&$f9~beTWwd8=~ib2dHi;P zX%SW#k0Tvx(WVV4d$jq_v{@bZf%BjBgK(vM zh-g;u%9A1q zL&E6xR$HITzIaA8x>`W#;I7-wV+g<6s&afAnB9|CODV5eS02@u=clkQdW7!e&WiNb zG>RD?n?TH3Oo2-2k#WK-G$VV5t;m%I(pK^~b1ozWIdW!-&ncKgX?_uNfsGCp&jIh? z%zD<@wY;E2c3G(VZ#P?rqIpDc!>C-u}d;YFJX8>!2w($dzj*=-?j}PR_73X4qu;jI=HwTVF3TnCxQxsnXF0 ztLL0d(IK(0#Y|TCO1rI))H$pD+%aO=?PdY0g(F6+41^WM9+`q^X*jx9u!suvhVyw3 z-FE(<%~dyddPj8!7g!ylI`R#VC0Wk4qM{-x*0X&GP-+Wj1H>N^!GQETK5k*z<3HXa zp}0Hn+uQ~}43BKO|Kf%{AO8uz)=gAC%Ly9{qG+t36vZn))J9$&a`)O0tOdZ#lgRmM z43}u}nfUT~aKFF(?!e{G=1j-_KH(7fc7UK>;}VzdJy{hpk}7~Ijb8PIA}|U_1NF$l zv`9Wb@*_s3--XsX(_)VBajxRNL`RrtM!bh$PeD+853az3%(jp|XLu(F=zHB4*O6D5 zP%&}Tzp(VFz49#``e<7Ub^!{p* zVqg=3cp@t+OMHUk%-$JXPs0iH8wSxuMAKbsMhqOma z39naEisT&%UADa&a+*e8MI^VlZrnTGtWp|2qBa(BApODMiEWBKYL};;&Qz4zC%k3f zZ=v(Rh6JvtcvSbx9bDs;ZsYAoduhpJ7m17f^)-{vY~s07wV5}>zUz~S**5sLCPsqZ zNVKQJYKOdI8#kGOaH`ITuj#r^H+OYT-Z?}gzwE3V9c_h^Zz7AiI$kx<)w)0_P=~}Z zXHWgIsqPzGe;h!PqCg)O{NNA?8ynK+)fZ+7(kCyNBlo{dz6$xF==IifOS=k}f> z0W*f$Ij7c)NqE;k)jAo$Gi~6Q?$2aQ&Oc1iWVp<O+)?bkv=IcGXX|ZZxKQfnF;@ydRr5( z+L=hDsV6QW$TRZ{mGGAeGiOpJfaf7Y4PGVZ>Rl!hl_4sfX9{?7&4 zX|wCDk3vw&@$SSyM8*yu&{^thSoiLo0N|1{SeLv?M^<#O|H-xqzjdVMlsD4eBOm9D z9i=@-khlS;K)u_?h9oE;fP|_@WOnmSh$6uVm>A#X-UI+$&1E02~ z#MMiHZJgZeo~CQ=nlJkjFayVny0q^?+LjX;IIc@Nt_TpI&t5PmCis6qw{y$X^Qi z!aQ)G_dSmJ+m3C)z^ew#Yh_=UgxmQ2Kn){tPrngMDs9ki;9=CwcqZ(zHkTSq35?di zn-Y9`0vGrGg@%_fTcdS>(034V7qS!Cvd^ohC3(0s7-6mXl6)Qx0OOeDSK-a1*FtMzx$$+px7>?(Ls6T=M z0t+hDJMaA4IT0VX3e|*eymjHS2SPnTl7QEyxJ$;}yUmv*Mn3|+runSN`0nsHB8?a4 zXUhir`fIKg0A(pbt=dvh7%a8jLr8h@O)L*@iAqxRJ2j*3t^`Q7QFAeo*w# zVMjnK0}wf?QRkh8ic0X%!DQoan`qmfhmI7eoVj(=)w(Q%KYy^0yEI3K#PAu(=8a=r z_EJ9UPK%qmevY`LT90PR5>&Q`G_eQwZx1rUZE~$)YO1QL z0xFMx)BFoB<5u;)0A9nSgU4I6=h+$pQ@zaG)fyBMMPtf2XWAN|C#7H>kSAY;g!{BQ zGw+c<905UWn?K>ET&fsU^5gdantGS#pGqPejSUSmfyCi9Qr8IFC1B}#NvB!QwdIE{e z!8#|Elo%c_T*Bh5@(IY;u|WEci~Ho<6@S>RlW#&+UjFr~S3H2O$zZ+KSSmjLGd(mHGp8q zcDzw&Qi8}5s2HGT40e5^mO$W@uS8@IM80^WKr|H)&OjE|fz44ytY5)tGht`J8Pbq1 zx5m*K)G=U0s6{@&1AE?OHto0U(^67`oMVPchY%Qa!Y$gVE3>`Sz|Vjbc0SlQo+WV9Ssg$9(e}s|nFRvj5)~o*U%|z4W#r+Y5vV_)leU%L*4RYAf(?I( zf(kSM$Z1zZAX`dg-BH6C)pjx)spc81mjlNUNL&G&4v;I1zDv)m>X!y3M=O9mz~SLG zfdpI3a=0JP?nHn>fXSp|e2X1a@oS7qZEJEomorGF5IR})nDw1L=fSma=;@Qy_3eTp zOt_@2uRtXCYb8{``iKky`-!Zyp{oEJ(AC((XP#bfRf0?ar~HY~#?kEuwwkw{0`QhO z=vN0AUh;T5Vt=?8E4sdxc@6r@THQ{kb_kgr;-;-5=(~(HM?!`^SNOj5>N{gEmba-H z`)gddJxphr)WDRjEOgU#*BeTK$=-z~l}Mo=Lg6vRcDV)2%WM!`M{F)P&pezm6{W2( zob}nS-AIe@A|gNEsTVoM!!;GIKan>aW>;${TK8vqUyf>$)?>gogt+==I+dNR$}!}s zc3=(7FYRH)*-{_ z8>nB?Z_cBFP<-SW;N)Cs{T0j_Hr3}O-IYtzg0`J8+Y5R9N(v;-9u2D(+bqQ>^MbZa zfGio4{CL*Zyoukg zrW2=DTV(!F-{6B#Gb3|zfPH-grpj#`p$UPZ2VKX}#}G=+pEnM*4W_51g*bl!R!VMMu2Lxu>-Zm{(VSRp zLMV<`Bs9`eW$YAF%>VAmZ8O1c@ODxQ`@a0Il(8X+CgG-zzsQo{{dcn@=4R#!0%ZL^ z1-!m{H_z`W68|>>uMcvZ`SKSy`9ggE?rQypL-yalTK_Vog;z-M_tp9vH2eQINN}1( zMt><({Qs^{{dNQh{O67UUSWaXg{uDXm8W}G9h4Jc+{ro8e85gA&YS^x#s+mk52Q=wO!r=Kn0VtxM}$@ja?VC~x9@{t)v> z9``MgMrAQFfB1p2HE+Wr+LjuX%l0nfKNIbEK7{wJ=)N*WquY1dR`AQnT3-xdix!^z zGt2x&%|s<9F9D-+2k~(K;w@r_*8gvc{1g@&V@_=iC8@ zx^mTu_OSLA|A=bq?#$fpDQtnUsbKTdTL&x2xsA25gwf>OQZK@~!gFBKo}#~F5*~5I zq^Tt+f-tVk<6TP_W3(<#+FeS26=M9SLY?!2F^BJa$6~Clt!F9=TARF@ zg=Kt{E>e+$ogK-y_Me{rVe37d0t80FoB&&jugBMp4gow4;jl-rCPfATlTH0jyt{V; z4Oo{s$>xi^Z5FT$6~~nB8{sV)YM#ePN0*93Y_L5xDuF#nkJ;RHPvb&F7sM=3WQDT5 zzdzS08q*5^#Jb1h&xu6=zGfE8=x|r38ot58n|MyH!VyyJYG1tVe0+f2rx-pC9v=X3 zA$Gvk$_nP58?0hZe+CB+A+d*3<eVDb9JXvIh0gy5hK*0h$gDThI`V+Ch_R_W)Gowp z=dCYLme~&gK2elKrX~F8^4y>U!neA)ATrX&kFRhD(4QEbsk_weM@8``CDrK9K7M|# zKR#UY#ql$C?R!xNvR11YlqD_(U^G@^nIh;ArcAw)Y>KJ&qY z?Ynos9~RH_bVrR^vXLkF3tqfhhSiUaRE3I5ZDcPV*wQ& zrd*fiXG?7hVDdtq`R%J$zYn#CU%GS&;NfRLd7@3InEE;l{Ew+-ROEt1kP3hI?&g*^;9ft6%o#yW_1-u9XZPM%?}39ln>ehcWmQxhf#>@9vq58i zShe!6gAxW*1unr6^UH7ZRzoKHEcc6Un0M*Fdbpp83TkO?J~e(1mq3BUE2BcSQ>SWw zwA%_OB4J{6-zk1z!h)a=pnvgW2SXyNDOpm_}#hXp(>?yw^+ma}YylNgQlNZDuePvAcswq>hvN}%uscflVA!W5P`MgG~e5qLxB|t}h zDeBO++cnkTO>H|~#Z=}SN43aH*Cy*cq@bxg)s(f#p${poK?*>!aHY`VXVDe0=z?I+LNb^{DRaGy%~49rg$j(Noi+mocEBu+Pk6)t3vAsuU)yV@(kuG|@@IsRe;k_>P) z#k=k?E2gX-RZM*l1f*g>eBh=RW2t}!0?#xbcxyp=yGZ1=c0fU6=gfk)-6m_MpIg4{ z$@u8Wju-O;bZcbBO=}))lTx=x?CWX}5fn@ZdhTNy#X^&D1Kh{VB<%~^WJ<*VAPDWC zYDQ)WMnQPAA3n5m8g zr#{!&9wsNK5XCUbGVBU*^i?_Utg+ni$m9ld^sOmNQRRPstT1QmiqT{AYkOfJB(Pih z-Ts84&v^<|ko_>(4w}IWqksH(DU9R`KX_^fXtn3p^y5(JJ4NiO(74(smZPVr;v7Uz zYnVq<*Xhb%I!#{sZX&@4R|a_GVXBWkDqu!XwF1ODJR~FwitRc3h@fp2v%65mIKCtB z`MV;^?;rJh9VKzKecN=8 ztrk{h^j>V4{Hu zoTaOF>U+FwC7=w!0r_d*B_pPA&S{`uMWNsRNYavdRbwCMJ#i|6`s@zUoA+<9s(M-5+88sFv3CA&rrEv|Z*r1m%^ zh57Sc`rv)*6vl(jHadLVEtqcC?b4H#EzaupJhz?2zx{S^eY zet1?seegE1M*8j9iLn7w&3s;d*0sp71_fR(Ejeef@hIpUUju-$KRcx<7_-=kxDtj+PzXcQIVn$Sm+g zr~ov6^A}N3VOB@MFMLgkrKY?LUdS}4xd3%K_T~Q1x}?(way)NH?}@ojw|n%0ymfXx zn12a^+#8-aUjYNkDqx~q8|r&o(vLaQ=F4k`(Gu=_@SJADIW=^QS_xidxaKtQcv)|r z^YFgyuSyP!NGQJ{z5{1BVe=%;K@&Hds_l& z?sn&m5Qs4dGlz@sP8@AIEja#%zaz`qqgjWi_56%sAZkt)qR^=IaNcrM77F*xi16H2=J`( z4Jz+C>~RA(OK%&J^;ypLRpL-1K#;Dld4q@yJde)8+v*I?3lP;R5o2)yxLd&GNmpILI*DLSTcrRT^Q2Zdb}|X z^D2sret)XN4(e8JH+UuGG_0(ct6fN`(Fq9z-!mS(I%7q>ZHfXcu^X&QNoT?=iI+Bf zn2hU(fK#1rT1P)2dn0|7j&bsi@EQC?e+ppv5E_S=&5ghL;1 zQMAhPzu0>Zcq;$*f1LK#GKxrv$~-n1MT#QAu}AjadzYd?gzVL(V^d~Egsdba+3P5K z@A13tLq&Q2zrX+cGrphi_s8S?K05Am?)!DW?(22EuJOE{*C=*dG;|4u6m$gi7g2Wu zp(%>9gOme|`fS@++^Fb9<%I0Xgw1XOD(O=qqUpzd=?n-ia^YUVv6* zz&~))QaxQPe(y~)@abC|M=OFJjP=y^-Z}1d=TJTnS|tFWtTV4ZAVjgnHkPO@2Y~u_ zyTqK$_k5Ee3ni$J$-@R-4_64%qY@GYSi>}^dkx7@B@&jeBq@+`DY!D5({lg&Tcni( ztcj;y+@~2C2O$x5&n%msEiGr5TD9})DE{#7CTE3&X=o0R-QjhneTscS>teYX(oGv! z186g-ur$y!<(}SIrJ*KZnZF!Br+2v`&Lk|P!8K&E+P(rVBuLf*Vz8CMktvBPzE(Y1 z5lRXmy#!8WAsI*foUf`G6fj@_WEC&^RnnrDGzy#g2s{5u#p2FOBT*{Y`7EI=IAj#D z-QC5tZ8PJ&Fj{3uGGFA^PV4IGLe<}IHO}Y?$j{FYGGsL#26~7)s59E#07dbVBh9TT zEqa+jA8p6KC~u+EBNoPqq9)kAd-w2|>rMvf{Y>mzXM{SD4$8{~@}{<7etriEOdlQ@ zZ7rhzTvLN2PM5eGoZIix9R6`Kt|KTGpd{df46y^i$L4C0#uWoe!7#IxP_sKvv^5FzF92iffAPy0qPQ~#V8}tMt8ZUl$2h1a zzXg;PsAIGI?KG?^Up{_Jgg#^t+G3Y)-ozGV0zp5BgM($}sS)ny#{yktfbcsOI35S) zd-8gEd%-~0D?j$r%TErxD3c7Rd2yfrwan!~E629Q7wfF@sx+V%qc z!;61m3K9UBlQB;LqES&<*^+9c2*KH8EFiLY$36-|gv#sY8^A`v4t4J476`o^G2K}c z_5$#Hs@C;LsT66mO@%KIb+Y<}^_c5Tg4AIc@HwV0REQ zo`@(KLP6O-0pHY7@OP)Jqq)Q)9Y;jK^M-DPh$*&HV}b{;oB(+tv57rC0}#v$o+SGO z3&c2@4r-iB#s-z`2F8BAd1Hu*zYD3FhxR#Vz(U9x$c(|A4GOmqF)kQwclun1}2qy)Pw z0>K2UAW$wD=sSVqr;w!jQ|z9_mm(E+3Gx7}_pVUYJe;%7>;a6w1nUwWQY{MrPuUhg z%$t!lrn^pmd+L1!T6{=}^Rm@*NCI!CS>ZgC<<-^JDg#6Y3O(=84p@BPf{Y6nJjdve z8sGx`3T2E;)l9m&x}0D#-}CSwu)X1$sw-*H9>U0n*U`h2B%m(R(VJfs&Ec}ASAt56ZFjX<(FRLD2YWvzo_u&CgB6kmxjHN0Sg6+NU}< z>9S};*9&10sLZ<2x0vnh09aViQGvYFDtyQWM9Xmr?F}j)e4{`Lrnwllep* zT^E19|I4t8iH!8Sg{g}>Cm%~i$u-58ynpcJTNTm57bRT9yFfO^<*oWek+}1SA7T;L zP3Ch*qfKKlv>x?wn0Vb}{U(1ytz&f!#i&Udb6#BCeYC89VIKhw_1}mA^t-$Ux|+S+Ov(WpagP9L)c19EJ}2z~3MJ zPF?V+Du=-dhlx%NkL4oq2_jq#gfSXOL|+VOlqjY}aG5=^x|i#-)mI7%zX69&l#+(4 zI5Len4TK(g{A^r&59Lc<{$Qa{#FY)CqhZKtWA4lZG*S1Li6(vxFU954EIFS{GT}0< zfWuPlLmDPIDBGu~pk(uAolBP&7e|BSOJq!!s`~rrf8M{f@?pV@4+SNJ2A_A;HYS|? zK@YLw6VlXzTnnbmKMNdw-MMxetIbIo7wa{T(LCdQ=*NGCYY4LgsfGgHj9+&XR(W2K zi=iAN`R9?_HE<*vQ@_lPm5r-va#e)#1-lbpOM4!Z5ewHE;_A)h%72Xn>-aHtt3Q2? z;p-N(g)35WmY5q9gTdsCh7YnjE&}c_6Fl@=xo*qFCb$!FJ&buw)~!|Bh&x>$Bi0)> zAHdC9HBCab4(xYUZTW5xVBGGq(?fo#a21(Vi(}(Dswdse!*tN2_(;jtQ5TsaNbCZt zc^JVGe~y*E-{ijXLiG=eZkB!`PMCPAuY#ka>h!P!U^2i!tB_3!LJS<%q(4e#5OLAD z7{L>*5kkTb4R?JdVi;j&YW7U<2A9OQ;szRAsh@F5x3DsaqEk4xg z=-Mx@W4`6x*i&2UMswxW73VTnR~8|mi;x#R#Ss7DgOi6vy#4~N(xnIeIwJ@;v>hsW zC(9%G`M*|(43VU4P*sN!z_Q>eGL((rhnt{99}z&x6t_`mAX}a)IPI|O&-z|w5OKb| z@}?U;HR_!1Ha%I~<$~_Uk#FS}&W$IyIOSc(zdyeVn+ag!pX(tk<6xROA54-s&`}=5 zO_D-6fO*OJ9Bq9g-zpKWd}7s~8)>d}R!Vr46=33VgO~Q0CI)CIB@cyuRx6sQwFQJa*j%iu~)aT-8d@3YK@hR0-qG-xN> z&AwNCOGUWyAzks&QRMO+#Vjn-k*yHjPD#gP^96qV~( zOhCHVIIBJs_IAdRne{4IEpv_iRB^j4P$~Y$jg@1rER)Foyurux+NtKsYaMB$gCl}# z9ic>yXsk87d6ja*uFkcNI4Q#s>2#|&?i))4cRv;3Oj=NV=}U_>Mm??vqYf=wW0q>6 zkWl7|_ZpLc>GcSX3)}wN^AL!hsn>}NhYFQ@wDIr1wtSO8+YqxTa#{Smy;~mBVCp~t z%#CSPmODTT;L{m^lMvSgG==@Q$Iwm^vcmVtc{B4xF>G`J4 zZecfJm}>sw=eT`TE4_!^s8Uox2v2U?Y_P1&e)<_w4S^ktN z*j1plDP-to0o&g9^>y!kj%H4%PywS7>L?D*M>XGC(o(817d)n+Ko&sv@^j&l;C28Y ze4!S~rY3V2v&T$qV)XeKwR5t?D}D%{>wMuHR8S!37znoUfpY}S2<4@uMu9K{-snNl z`~Lj9T`56+i^1S~0UgJo=A@57jLKVfAAX1!Q>rllZPx~kd^wPT?Eu%p6_!T`5eYa< zx5@2KX7i^4Z3=9J%-v~VOqWzNViW*3uAmSyw|I4;|0~+E_3Z7Qp8Nwe3N&xdoqHSn z2w_5K)cGA6@rC57TOKDc%tW)bkZWKss%iTDTyL}9 zsKp-H+z=+Ud=@CXiZgMdUHAb0=N?jaL*Q8bu_c3q>!JuCm3#Y;YGf7=^G~aPbXZpog@^faTb($+dge*sZA6B$30 z2RvqiBixo{^l1+R16!;cjD~HRO$&B&G+7?-;0gtcEl1`)QOdL%R}@lEWnl-LAh5t# z7j=YXLC6UI7%Wd03HIEk~rn82}q!rOo>;hLv}hk@LfNP)ZvG3JC>V#*^epd0dwY z&|wXEU!>C+7&CT&=gNU%6X%I~9dL1T+wakqTN)3jdpW7p8V=iw*Cq}vUOHap!PPHI zH9W3RLBCvr3q{O5KUBtx^q>lA_KUTmnxj}mf?UR9RFz-rqnnQEF=D?F=#50_ciuE) zz>2E)ysI-{OZ&;QiEP7f>o5QZ0@c(D_#odpEUQO-Un`8%#VZ$Sz5%YZe6F+89Pc3p zYiO{K)gPY1?a5~#vQi6XJincp$(-u5FWVOav>?GW2E!t;U|S*rBVT;&JL>k zDOF>*f3jU7d;DH=bV?9d?j4~0c%i0mahPHerC)P$G#3(v&P`E#mx(Q?Mx*p&VGTl_ zM~`2|$O+}4(#O5#KvE8Q0Ch8@$r9{9JR^{XFQ-pq975^nu)@^hwLyTRsE)jB!m6lL z{v?iU`?Nys;~y2$OAgLqZC1plgd)aQ<6D75Xp5TV0UqGmelrI_>W=UZ!!U zC4Q8kFFRw*311uGleGpHtm)#a!rIiY%o2)(co_UDtX{SWuf^#1h}R;$spaM6$9l0| zSWzMi>IC6WN=h+1R!t1}m%ZpvN6dw}yN7(V?_Pu!C5X?(7Bc2*->^ zV5%U$nTed(#(AvjV##9ILE@BgSt>n-_@7niKdesy_L$F3m8j{j)B=L>=ib zk5DWCe5Z)oocD((GV)Ac!|y8{HnZ}rT-HR9T;EGML*0CSDjIS5D<{P<_U_B;(zoyp z^cRAXh=CmhEDZ<(`yd6cP=v~6GSOQ% zxRrnETz+KrLA7P(hzH$hQbz~X`NA+0@P)e3M}(Gn1WMEq;#m+Bq%#yi7$IB=8ax*0 zB*24(O#mSW=Msr=i0yEM3se9v#Q)ospQB2)k=l=ylLzFZ=-pa=?~oK=6MiC{W$2p3XyQpDekKC>GdJ z4lF;vvMPK3 zsj^hqYk3(61i)zYNX8R`1eYJ>&j{Lkh1y+d=C=yTn18YzFZr|rL>Pto(tW>1kf*-v zwf10M@@Qo=r%j%CtG?&Z?O4b~Kt}!|b*&)`r8_9%_ zg4+Qda^;jiS*w==bj(}IGL|>MZZ)4AgynEuG-;&+%8c+hxZj( z@vqwf#%VAnVXy528S-sN#2Hx9=xA9slU2|0J@F{WHF#aX1{2uH{zH|uqemTSFa8>~ zt2N4`F@5+hO+!Y_3rM zz=sQpmX@AAecBzq!Q}#*&##*YZyR9Ux%2%dp{E)FJ_c9=CbY}%$&gh3TNLvelH$f7O!M`wU zj)3mesZ$ILALhI^KTl3J2S{H=4sp(7eXMo=X>QX;f>u^)8s47&*HcBIO}28m~KXh zO;O*7g!zDAVE<2*q^E0+8!^36_~NQ11hLnhkSBa&i(0?}B*kUyDMdC+3BAIrbZ8;} z>jV|P?H#2_a)lvmf+;p>%qtd0XTo&xXpn^)@cz^uWXtf^u!>0T!v*hGnWr}}FNM1r zW_$RwAVIL=A%GL~c1a7dWsK!ySFSt=WXZT_IH_NWcRnsq|3f!jL*ZgfweHM|NoKe( z|8HWi;O#FS8C|!SoLcz=8&lzadswFdq&?Ut6e^We_7%VJoTyAFdolZ%!A$H39HAX;z9_@VG z#kt1q#LHHsLBw*60T|cLHJ0vM^_4bt1YZat-ULYAf+<6ldtxd0q+Uy%W=`kv`_*CX zA~hmVjs$M(c@yUj(v~2Aw)r+K1&29h+5Alg%W7d8S@m>$3wa>LDy!;b1?lOVTXuhi zcm<@sp<*W&V39?}OA$3rX|~8n_p9E5Xbn1S;_@c+c0nCMpF=ejKg0td2P`y*l<`1! z!^y#x7NEBIk>W6Yf)P~YYCm7gxkBgJd1j=hPC!%&yxM!su-t|b4iK)hP}ijRM~(+|RZ^pML3j21#|+P4(F6yTGshy*E%cZ~nKcg5Ej zu)q%tLV$Wb}aAqH8J`Ys7P)r3{=a3Jr@FQ8gQ#jL8T8n zdx)K<{0&AF;Yb|jsdzy&O$&y)SmUSI39zfY?Sy)>jbMMX>DNGdKVoPLZkG{QkRI2d zy$D|S=@t9TlKBP<5=b1!q=gR`mI@JP@*twWv~0k+IYx&pB8w7CFsol$1|DAg%a^Fg zs|Y+4((302xeyMWKBP+$7{AnW^h=lh5W_tTMuGn*recMeK~-7G)L{#5vIj9EVlG1q z2H?dt1H}x&rv#hFk5Fu9B1Rl;vpCLR#To_|1EC6nI48tZ*z8ll@CA+A2eP1%rh~K3 z%_VC)aIt;JR{l_9`Ga#v;CRq^GK)bu^A*%UOw;3xTsJz|ac5YinKHlDuK>HV%mqK3 z1?y%gV>JoxD@4{;jA6;odw-?&EIH_eUWFAGqQg12HE6*GSd#Hb&UKaAhHi1?zmIL{ zD+kf@4nn?Ii1+?+wpiKmk-r#2q8wW%7*`#iU;m*`fe&6Le)Zffk2~?SA{UaP`F(Jd zewa3#{l1 zDZtGoaG4Su73l&%H$;>Xz%m4GD=e=p`=O6xV~-J$mdd2A7OpR<<-ksrZ9g6Y)veG< z>&YZ!2a?m%r>ZVe9AVKG&2Py^Hux`uyzy_WdLd;3VQzdyE~8cV2`d!Y0iYNJDNK}; zBgyUtnlhxGnBRHsm0tC-B~o-OBAKphmGC4Fr!XVXbugQ<0agv}jkxVb#6~LG5s@6W zdDT_UjK5BBG6E8G5v;jqVeH!*B7%O3Tt!j=n^!++ zeN=*6QvR+kAQ*E1Ur?+$2=UYO0w6W3rZ#?)fCv?cWMWpVOb9H1{sDe$1TsBL!4yb_ zY_NWr(#{7jjInxF@;${kt(q(3i9k>U;&zY$mos5N@Z|7>M3yvdkTmP$sWJrevBJcH z;62nljeXJZW^-IrwrN6uXv^E82t&5XAoP4&X?B3xBDcV)t)L>!k*7>ZR{Zxh{^D`D z%Ry*n@TXN$F*B?U>LjMptC zbn$Peien=1G^V|nAB(~MHZ&RCtrxQWk3U9N>{(^W0InpTi>bc8)W;N~?f1up$bt-w zyM(u)Q~oe?a|uKzz{(rMWc+TO1_W-UK3sYMIa-F{>i%sY0Xkc${kQsSl*W+_$LdF{ zdLUpXmxXThqXBzFtoF?x=iR!MIssDEH!*g9n08SFqCG8NbToBnFNVd3r2Rhq=EHfN zbELB&o$9r7e+)=*D>Xsnb)N@{Xyvxw2TQxP$RGBT=NQFRrv#VuCIR2clP7h7hXCi2 zj?Cpfo7&_l*KF7&93gAO|}+QA!q}L5v`gxXkG-75_BYli5ej3Q>^~s z?i?4>DZXg3vOx$bMPm3?*|qR-h$BH7*$`^+kZL`qYj_@9H0I9NLS`SC(^W=X_mTJz zV_&jTxXz7}j6$YXG1XA-1&arlthwzqM|(gU@a;Oa$_3EP9c3H`ue1`JQt(PsZHon< zei-^nJ~CXmzZ86XJNINM{3c>7FJby3`kQW_%R+4(l0f{vQ2$^aLuneL|JUYQNJ?S3 zM)_y0(aWOV6w>jx85@z+w!%!y;VCLtPi27afMq(TX^L-CZvI{3Ew@sSKPIzSJlADE z#q{j=RUFWs$2wV0jG9EwP9V5r)pG%30@}P$0$zoH!u{YXp@5u7DM$h8p++-j{D=1<_;3O8Ebw{2rd64qtoQUj*n1mmv#EN8&y^IlpL8VtX%dcXX6#cKu> z#XT;)Di`>%OT^Qw<qB$DvgkfEHVyjC=4?ty2wmXoU{eT92a;H z$4$Tbl0f8b)PS%}mIG;_*O`H-0*VDDNbEvtmL6J_BsIhkTlk8RrLcN*n7{TWU4e33 zc0s}Eg$NUvbcsOq@T~$uVNlsT4>*W#npn+a70Tr$#HNDQ9A?VXDQkTl+0bD_Mm$(> zg28e)V=r!G)DIa$9lqj5P;=W|%4%i3XBu*yPL1aZq+R#Z<@C~+e)SjUYKC%NpbLP+ zFW0EuiG>ev8~y+;BS z`|%#2^ebwqPcvzOSQouq;?Xvf60ADIlGT;uvK(f!9=NNZrPT$sisnv?j)G{=Ltuo3 zCM+&T-9YwamlGpkN>375_+E57U_S7;{nhY^7r?N+O0Buh7Y9xD)mwl;$kr==$j!}r zzNw``d@y0B4n7C*TBEfXb8;T-u0&W9n_?3N8W*7TeQ_u@JFC^l`=$<@<1OBM(f1{ zUtpza1^D0_Cx1iL6=>OI&kZ79M78SY90Z}$H^sq&2lsvs0GrFE*)B>s%Nkl$eHeEd zE{0y>5MFbEK32eS1P`N-<3&G&Ux1=HwD-&gD(m5Pp;5ciITN62KyZD%SsPhTAI1yk zw$g9OaheO4S1yODPK8X%?_g?GBtV=g-F8@=&)}=1loX5lyF2h#q#iXRhw;NK@N`Dn z_q>c;oa@H!ik6}4-hVO&!8`+ol1jw7Axd2cE zG1l3<+Y0$pK8FdzCZzZnseF~4EZzv_tOGV!2UxV#oSkaXvb?i>DcW$(D_pS{OwXB? z=_>`3Ezk`KVpydM^TQ<&wO4_H6J0!r^=UxtuFCjBvpRcJ7u0#v%_-`IJ4QE;LZo@xJ0J%HcZT#>k010I1>*YQuZ!v^MKy*paGoI8 zgN56OvrAFUb;e3jFLJROfii|vzZ0OyAPR<*tsvYXfUWbR7He-HF|3DZH@J`Q>!@Uc zvbljjZ1lU{`v;UDd`b1uNW*&H6PMNH9b_(yC0LC2)LKP;Z%H#zwJ?N|r3Y?jT6u|5 z-h(-$2um5bb}a_t?(o^H44#*d^O}d&7bB{v{uqQn8SrgST0xkcDh=?Y>RUNknif@FrdVaEHF?-O7G0mp@Dx`iR zp`9J{Ca6nLr4Z|cDtHKf#SR=`^GO9o)(qutVC#5WvoMbV=N@nhgm^#4(smczCb$@h z0j2`=3l;%HU=bzrKtqN?!=}~Yt8%SgSc-Z%&g9F1{odJj%yMGWQPW%IJpxw%kq7&T z(dqkT(D)VO*`uXKX-(QWuM{)Pwc-cw_(7usRk0wZwqR)N0tDZLE;Ize^&_CCa{+`5 zrD+pDx9$rkegml8X3(KlxrGD3$dBqMb-cj=&k2^^x7oQK=iLXhZ!@BdfbCgJ`}Uzy zY&&b9E{Hlo`M5kd0Fau^TvTpwEG6T%Jju_^Y6NiKnCNKTsRf|O3#asDw}?YH;6Qs> zxk$LnmxHQle<}a{F8MPc*5EkQG57`$*1+&j%}`VBG~cVr5jj77R$pkWRymvI{9FP5 zTmf2ES!;*R;Id7q>n=P-&9D4{fha`?;tQdWFG0FHEaF?dmk<-H0CQe$l4cD0+trkJ zfkwDcdGhRGq>vMku2Z4V_-3)n+j$Il-J!dk>&4JdL={;-;V9*`27K`$92H$54YODV zi#ezw-M8r50pVPjA?$4}tpSMU!4vG(VQScaWD!`>XSGjU9OQxm(U!T2mY|*E!^ISp z6SP6)u)b=duL6oMXZj=3CUWZf1@|R-Z$3OL(51~vIWs*m5UTZyap=Og=mh*#HmDy; zL3gC7AM66y6z#FjM5Pus@gW{!Rxb*EBnS*j;LXBJ?=S_&-LbiL;#?;5^wldw!5ym5hmS9y*|J&;>sM1P&Tqtb z*s%G{bi(3HLdwkPdO!lPs;XZ|@HNWGVWse%!qeOdeBHox0ZbGcyIWkK#&I}oRutit zN9gv0k5q;7+KmIMml!Xd#iUp=)8)GjE^`Wz>RG3E>?%LP^s{FWynlon15z!3#B zde}OsPGZ^E`qdzzVm#cu8Cn3A7uQ16SruUPU`RwTMPjroBaHaaLdA=;s?+zTYe3nw z$g!)AGO7>Un3i<_n>^9~1|FC=$gZgjG$yuIN}U;BzbcH@mny~Z?9nen(tbcVQO&Y2 zBXQRGUQthA?p#;qA|Y!7&fqd#;`jc|?wWR~rQZHzjWSBEze~Tmirkmu-$(AtjQWlK z=YJi)w-vwQz{uzQ=| zy1u47q5iHstz7#oD5ih9K;h0WXkut$ZgCXHwah50WvpYVdz6#uw*u<_b&G#b==a3n z^_TTI_{Zd6O6OH%{u)~d^WRg-$ed9HW78;nwRhdruloo zGk+EZEb}LeAhK5oall6;z%Rc(ru8XUmxA?$uv!=F8Sz+iANt#ErBGbZ--&VAmtkgM z`={HUOBYzn_(XBC{?@GM%Q<{JrfqnS>-k>=e3?JGB_)dhavgYh&ZPLek>6*}oke<_ z;yu{=5&4BjwTTt?d;9WXf%U`d(}IV$E)MIfWL-O~FN6Q>y8)*hHdkZ~r}S@kEnH;` zaIyaL15tl%3~;b8uWmBew{8rqTRH329T>gqRxM5p){XroA81`1*2Mv4VBN`CcXB`s z*0tigRs=CvkBF^D0znMcZN>G#Fo?mraqz!A^s^osK*W4q&e!D}5rcIj_`kyl9(%1( zyaAd1n{#m&lMI+%{e07aq}u!^p4eZr8UUzt<11Ps_SEtS^Ig?SN>* zb*=b6ZnF>~_L}EkBj?KYcY(mHz_#(P#9!ggzZU$86UDWbm95`l0pN_wS7- zGw`^3aojm~5!t}-@}iu!FNfT=E&V>f{u?(l>+`TK4~QWAuapBzp2*7T?={Zg>Tg2U zza`n8+r=Hj^Ycx75CwRKr~pgkg-IOT{zroFa@}qNWN3~cFXaco>if1AUam#p?Z!KM z8(#26H-Tf!jl2ZWKv3%=CA{SE;2*}jz6W0Lu(+%IW2YRB!JD_w9+;dRx8EHJ!{ie| z2G=2rKQryvY@C)mJ9G*Uuhs8gKajt3Z+EtQE9)}nA+PKnL69Y_7ZB>(Pew z*vopjX+8M19&cQaH~v2uZ(P6SbtCZKY6S94o2{AU;{3ZF)vEaRzrv;Z>&ikdb`Bip zm4&6bmYUHH+pxYbZ;dwK@7;9HZ~J+H(|dRLm<5L)*^qo9jTK|>Ei*e%W*4D~hc`56 zoYpWqpQDlx>FM2Jo2|k>OW@5PSvfnuV9Vn?PY$23Ax7d;w|<7N4*!g9>HdmT{c@z; z*FU%%{9B)%pV7^Aq4*IIUKfgWp;*oXtSi!=xs&zn<41C6T`1OtVmT$azG<#+n#;N6 zb^W-mAD0jX>-OWi{kU#F;=ncQOJRK}tS^Q2rGVhu)(xa}18Lnr!r@TYm%{o|SYHb3 zO97I0|MMb9Lrq>s8tz#Mq4rvy8jy~2vU(!9D}4jaR!1A&11#6xoJFAYdAQrcA?-<# zTx2JLnm0spkq;11eIlX&*FT8N%=+}K3&r|USl1N)+4|9CVQ$)ce3}udfNG`!qniPofj7BX13ws%_S_?vW6jpgda|1!-OAG0T?>`wBoQnczAi+k-Gx^h6Bi6 z0zWj!UBJ&3WUqyv^%MLn=inXxL*w=re-`|pP%ArnF1I*j=3r+!iu>8YctOw5Qp@}( z;{`)CORe)-8YY@rD=84ZRw!KdYfRmj(<@tksxVGB-FDwmv_Dzj`On;pfv*TD4*GkP zwGEtYf1u&}v@SuM<8bSP6uHXN$x=Ce*YWOLoUCh7<(LfZr3tJYMf(sbe5)PrKTZ7d zT7BtzI&`ptvWZNBABExQv^xEB(VPb5ZGMePL!FzgqdN&|7L5W`V*N{fbCvJ#Oz+{A zrye~&^3Y7qo0!-nK3CPu$NZ_RL2$v>kk0`_>P2$qMmAQX+f{1pm<{{;g)7VyJ7NMW zxgse(J-ZY_`1Azdr(G$Z^wc!H4kSwxo4>}sJUw@0lURr<`gv)rw$wraGf}9-x#tL0eRpVPcuNOGOAAGa{n?7>V;TD4BiG0;dwAZI z=9<%fg&(Ujndtu6*ldn|UY+`TpL?czQiIga!CvZ)?TKd9HS|Y5T1}oX3^`$PPF;EN z`?0_hDIUr*IjvNJG2B6o#J(SSx-gvk_Ur6)>=f9`Hz(vi`{dU5Sh)(UWg5yqQ99H@ zPTC{8U9QF>+Zo@p{t#2h2@(4}LR^DHEyYS7)Hq-Vrid$`Le z-S;z*hxe-DQ)7KbIIZNBDr)_(!Q4A;C2=MmD$DtLPx^H7TxWS0l~uzy3E#bv?Y6I# zURwkg*v=<4_LCBbTxHsJs#NgP?R_Do$z1Ghr|yY`N71=FE5uy9Hd;qOVQPNnV}chK zXM{c~`+$E&#K3K4fBBKd<|8xQgeT~C?s@yRX|UbF`vXZN#(YR%b1{=-uOQR*v@LFW z9F#_EwYdWE0ytGqYc8a{<*otcvDCff9hPItB_fuk;! z%BajOwdGAxK^L)Cds!mzu3k17OMwtSJQ_ud#DRHJ^KUFW7?sPMI(l51OT(rBwr zKgXVB+O)|jSsBljGR0=^=dao z#M4|#L#pqDt`41G&@|m)dNS%n-nLUa+{jJwQ!X*iU*AbGksh>L|Mt!(2?n7MwY_d~ zl0#t=mS%x&={xTyA12TeH1gPIE1UL>;KGiiMlPOL(Wg!iY0Bn=MrdDdvKABkGI~}+ z#O*R=&l%G|19Fv&ly_NI2o4;Z-HYDSC?@)~{@qaFXrGek)`vPfi*G92VS0K@>t1BN zXynGx>?VNVWj3@GgUG^V{}r~Z+NhRlRWAzkvV?@w8#eLO)uQ&=%yaLwSJ zxhz2^SMWn`zaif$Mz4nju?Ke0s$;j*p%wVA6cLPe@%Y>4)NiVB73Yh&_MS1O3Qh5- z|TO-Ni&&LIaf`- zeN*mx_<*pxCq^o{n&r_>Yjw)2Zb!HD9};d8-l#A|9xQY}Zh^CM&yBj9mnyc43Ap&t z9B*~F={2Lb>8e3K!wn_=!=3C~Mvn1j4l$Y?E9-d}!&go{f5_vskm0#_sftSX>_(Jt zec1LQJ0IyLV;7RADA}kvT>+i_oa5KzTzj|iwm5OxTJN0gJ|M!m+xlpXr^`5&yCP6_ zQKOMfL@nnt#nZ1N(=smhdGtr55_-SLM5Wn_4s0_NOF5R&%QVB`s&w6j>k+hLVTS-lJeN{ZT$p-(rirhi>k7Z(F2b^B)I(N$%{fKmbSgEOQ z(W&=)XOCWJ$B^5rJiGkeYwr!p&a+HJADA}ZqR>8yzkjd0}{hT`q)q^%NHrEr`(ON%!)L{Hff`(1i zDT2GM)LV}A8hac!Qr_w}E-2McOhMgh_~{E*R#^XP;rg@fkhe6{t+#BX<)n&vXx*f>j8n%Fz>QRZ#pBMO7tK0McV z4(*om7wrAalCW{fhlw~4w8;@W+(-Y>T{Dl1s zJOXuQ)up*sf~^)$HSs>myPU7oy!(}wg)*XjkEmP_#ekTQ)K+c zWcFVkwOBdb_holelp~i-UO-dG>~eUYX#W11SM6PvFVgs-`>+Kbb7?2rrh2)kZ+&`a zaC?^mUX!FxJG#!qJ?bi>ZzL3koN6~%a9=2F9hAO4^J+M0a&}J&y3)m|m9S*zzR7^Mfqa}htg|rqPfEIiOE?KZ#&BS zn(x}eHbyMIvoxGVCwwj?H9eBO*d@xA?W?vd_lxb*`TVg_$M?DE1~c=%og$PeG3;Xf zB=L1jP-I8u+g#`^m=sgLJ zqy?Cd@*#St#p|0)RA2Mmlp@QI=B#KH=4g86VUjgreb4Pt^n+^U>2{IoFIo2YqM6#B zONHDB-9__iw3cQYWmen!a|tR>$QQ>?Psx2F@{GB|oUN4;VDg5U$X__*dqT#M;`yPz zk&zEx4FzHfR#{~^q@TCBYPLKy+L@QrXfN`S7Z=#F!rYug)KMancB6DMvgyy)b#Q^dDJGxD|7-VS)#YA!uYSH2~Llk7t@ zQyAXOR4Rp&+PmI;XUM?P&h6v8abQfa$*5O3kt_SOM-z2sT}*P#>H?*kM_^k0M=elg=E_4?pO z#@SolkG;H(8`-FK&>5xaXR?2#b;KyMo~Sk|E$-usaE&k2>5qLhEynHhLU*t%{>v@; zRzHb*TzIUeJNh*yAIC|b&C+(OJ(5lTS-FYle0OI2J@z19Yb(pR_T>JatZ!ciBn5Pj zkkwkJoZ88LGqBTL&m___Fs#r(+KilWDDRE=MitrZjC}b!`^g!-3Od>@=9}XC-BQ!h z>xw*}9lT+2r`jix(@`P%TLi{<#c443)i1nz@9sFsIuNnprNEbq4@UMq<2m=9;Ff#m z`EQilq&4;)V%xv}={FD0_EQ8w`q(kL=fXPAYP?Pg=_pc$PI<*-g^sj6NHa)NyRBx` zRD*uSkRc(~F#qC9#OP3NF_$#sXREX7J$rOc?s=ZkNIj5lMw&x6H;3xVXAV$oh8v;qAF9?=g7|>Xx{_Tre>=f&~PxxKE&VFiJ=Z$uWSdsdYuoNbBfH21ac? z9V>G!Ms-6iV@*{JH480O6up+I1-*uev9*@DrIseWr3r(nruM1j?@FlYXkD=~QU_P{ z$LU0jwN1|JX;>abF(XWalF+7Az{K{bBI59JaIhX_V@4_OV3gFdFtIY%(6Ttn!vp6s zH_?#NvOKDIlu_dRg`9J}1cmXGNIHB*7W}afeGsP+V{b zq0xW~N}@R74N?JydxLz5>&F}RqbL?+D7YhR@CMO0xHqiu25FLxd&2^6kWRI@H_Y$` znPA)-6ueo!F-h>^Q7lX2VU&acu>2SRT%Tp>`h4)SgK=4rA)Vv?E7v~EU+SFXEvCJ1 zM(<5u*WhL;p|asmRB^oi+(FV{d=b56n{dG9&+TU)W$CyZV+Fj<#pm|Sk3V&AWanPA zd}L9VYxDlxQOy@#t~NznRg;R7&B_y|_Kz!F`EQK`Ifh)@*3DBYE{I=6R)0#Kj+rEn zYoR2eO0x07>yB6n;@hWo#_jr4DUr^^IO@rnH<{Rh&v|1$uymzy23A1u&3;Ej?Iaw`}gb5 z%D$ztZq~LSo^HcyDInUY-S`?+s@~OkBt{q$Noum0g()Wec|&QqLrCS@v&rTH^r_Gv%3B508H*+o?IJd?U!3l?ny8aGC;C`+NoTTkw#Ia6FnW|~P!a1*Ht*4Vv|B3XvDsWbL$l7nrtu{_` znEzZ5DZ?wY!I=i1g{GC{AAJex7>3%$YT9qqtwBIA#U1_$`RD)FY10^53)+TCP>W?9 z!(WWkz7tx`+TD}cG*@*hPoXS%8;Qbpl6!GGzvO0_SY|a%%sSa`Zu-!jBK2)o)W`B^ z&x6OWVK~RSv|7xXRCQ~| zDT;^vWf(#v_g__JjSaHW*za?Ro$&riQo94^g&6dSeTny7RlcR{#&PwaG9B~m&RX;* z{^aO5(SkmLEpJ~CS9_p6D5!;kyvAo2cx=CzR7~^c@Q2RM*zqtF)z{m!3{JM&dcK+@{2(j`hg#dryBe=*VqMd><33QNFZk2M^XTP(kv!{koM6tMNIIf9_0@4?)P4( zUC=_t8uyjtzxayq_YY?V9ed-_a;Dlno?i|O;96*Q7E}-?du1XUUqT12 z_e^uwMrl_#TE{W}Mp|b@SE?n4igj<>?h|S=0uucTm%;jRzg4+ZXW)AYD)j|<+1_-exKW}1VO-sjE(=XI zEy|9LUH-DpbHl}2WdTkL)7@POJI@NjG+$3P&Yhp2S4{Ps-uG>Kw0m-Ve14>ikyzRC zYPGG#R4Hvj<7-h9NQZ%p7aAJ2-2G&lUF@BNI0R2V`UA%;4gc@R;PDvFLn= zjceInG(_PtcXd{)HN>ptP=xKSzzhbw-H7a8`aU>lVqg%H`&2Ov<|ZN{0zMl+uLu&- zQ|vu^rr>~Qo9~YqJ$1&qPcB)r1QC<1<21Q5Ju#%l>5pxPhKng$EUU%DI_|f@#3jM= zvYvbYUU+zJqQ4<`abeaVKI6s9cO(pU%6Ny>VTC-D?E3OOpM zQx@3rLu6!*gV~cZ2`Wvj%_IyFN=fR4@N_&_ct*=v8d8mVdwO~X2I9jfZfT@kTzOKR-J;$Vl{^P*w>w?DO=tF-n(!oG0COxILIf=gNc? zJ|`tC%xlS>v+<00)i>bsO!A&;*Y(QaSq9M+G3C9|f;$uqSXfw4b(yz%>$*8br;@LB zazozY8p?)6NFzNi&S*zG(nCT^w25S(w>Q*vr`chteIncLauWL7dT>W-pBa~|<}Rs2 zr;lx?+!uAsckh0))5jRP?|G`778IhQq-vKwzhB_k6`^-sN`;cx6X&%6BZDD z_wr797%j8=%U#PS!ku$ppoLHolmFx86XDL;ZXW-hN>u3s zqxAm$E3bNz)qQfHsiD4}iIImC~Xohu9BZSaG|V$})ZD!UhFsF3-2QhnB`ok;(H&9#RePO=6D22VYRuj|py zY<*Sg-R6EJOS8nc(1ZA*v1fH9q>E1s!+P$sG`T+Q_jp&(95tLyXf&Wejy<~YsTdvVvMZOj|f0^wd57g;)dvHx~} zs?&IFdk0lq%jeIZJ9tS((+85v7-%i9&CSgd6B8dRe0T|$rMOKs68F$8^$9m}*3QI< z)qmCq@f!G2CBDVPN`1wu=$sQcL5bYqOIeCMaoDp(936U3zRozaS#c8g(3fTV{j}R9 z0pxp99nZF!CYcA)E6TKF7j~4SXLBN}Z+WHsnCq%%7^7Bq=S`V%Zw%_<>B(QErHMCu zpEX&&@>R=SfttGx1pbgLYC)rofsO~#7n)68e;y=RYkart;m5(D%&>K)A(xchFsLsp z!!u*wJIDogzYMpk?cc_!vMt?uAz4NAwd?hcB+O~|moj@zd1TOqOtm{UcokzX zBrg=s-(m=Bl!)#BubHII7|+l zySb&duFgX>NjoZ2aj)r{5M}hnExUsFoMu8za_mZv3w9r%xuOIH;1r>OH6{zr7E_NF z$}17b8`if*?=ww?@ARmkU0~OP-TQNbD%eX4umaiH*j}fmdXaL(4=iqsU^n{eG})Bq zG&=#7-K{{%I#6dSmQ_M-9tSt%boyRyw(YnN>WP(|Jzg$Jy*vo@Jb?n;B-K<6_iTyo zZ_JQtF`|plndyP8GGZ<`YcOZ_sW+cOs$o5tM~-9Ps=6Z_29Upk!2r9gi+LVp6-y=0 zD42e#>85JB<^zox{r&w86Mg@Oz4w6U@_+k=BUzD=JwqjAXRnYXA|#caC}n)h-pVQp z*;_`L8L8}Dkr@fuvPbrw&vDXFKmY%A-Pe8H_v^l&=lQ)}y?!{)^K*XYG2X}fI8MaS z!JYCjRZYIMy#DAgbF|ay==AJ#UqxRaajWkkUl&pg8qWL_ZJR!Kur<3+(a2F)oyeRs zkd>{(7wF`&ogI*+vrU)|BM@Ki1@o)>tx3o7OusieVr*Beo*t6eFfG!MWmPT%lgWk z1Q7iNX|)^Y1!N z-UdTSK7@nap|h=Rd38J^Q6>3iwC!QxnDvcHKF~gdY6xj$?}v;ER!xZx(ED>O(Bf_ zmO*y}yUoMp|C&FtarSC*gftA>TbzEw6s4G`!m@PYQ^AewuL{RiTUaJa=_X1)-92dX zAIQi(1?T_qe+(i=afH+i&xr{?Yk#VksG1yTPcowQ>Z|eb0G4qQD_sIB-H}}P|Ld0Y zKiH4?v*`7M_f{N=Ww9=aG>uxY27!VslK-{!C&~`t(dK;mKYsQ8ozC8)^Xl5Ad72zC z4ND^~w`GN+6rZb1DnGOSXqX+Q@FGpM?Y>ys{n&o8_W%6ReF0biAP`nyU7RiDH@F~1 z=>6X&1F8?^g1aj#yZ@QyMCs`ZM|6*rVN;yrLdPV2>~bH2>0kVM*%3etz%9h53g_a? zXvOR-rn-yb&pt_6Px3pNm6{YE&*UJ!jzA4y9DImASLzQpD~9nHeDpttpfB@XU$bu} zejd#zUQL0r1o>8Lhys^-CNa#{~XQw6!%H~#HzmG_{x%{#xjh&83@PW+G88Ex?)(mFdNR>v`c9_!6Jo8E)T556nmZmaS ztcs%fPqES+?d@?A-p6(28k;>MVm@w#3z%u!$dxX0dR=aWVbR-9WTD85^eQ~0vS1^^ zmI7lcQryw5F%RP;OZcjcH<{ydpC?0mdEr$1n{j~M@U5H4wQcllj#?3r6&G<|GtT5jbyg<~UbWPAz~7*`8;9b3QX!HSugB-H8*|WY!kH7tDO1 z2JeU8s41adKfQ3a`c6hsuuH+!#^xBoyK(XH@pE0MgY&TN&VcOcNsISC=5?qzlR>s2 zSDkye|LkLmnX%LiGHp6xixZ;6oSM1Q0HB@}4skh_CE-5#j+blGdxW+9wb5sQ5Jk+o z3$XwhON?;}mvG*^zaL8+td7l%HS#F=Qet!u4l7dk_t^kY=~w!=`%+U;mD4U{>Q%%I z6LWo%cdyI>3xT-({0Xi{ZB)J?HX?5$nV0!gw4NNu_Jvk5Drd`wyQ9^A@}!_#!Z7_>nYe z!+ti;mAgGl(_tP4*#$>!iv$X$9L4CJZ&*nfzoO@DSTfX^oE)Ab-&%6s3L$X>VYIn1 z<=nI1+6`l|42!-KHgs0^K|x=?BeF90z&yN~ZFoUWPHrFqU+~WQ@(h@;nR%ZM+qbx4 zcJ`NcC<8Kpneqp1H>cu|jtLDI)dID?n-fITnj z?%lge-!By&fk%l3PN~1+!n=7(J1O%a($^^EYuBh&p6kBhMTtl~pdpo1G?&4vKzR(g z<9Z|SCMi3hgWIyGJ%YVwPtFAoGJc;mi7L1v zV{K#08KoXXrg$;Zu(Ure`|{-r2;`|z$MYm9Qc{5z|C-0d1V-L{5!`Kf6|`HAxZ7|6 z;7R#=0P@RTwGW3SN*cy(Qy0$1;OqaM!dMI4(~VC0uP$lE zT_%~-ZKPpjJ5S7abp9yogUBH-3XeX#x`U)-M8pHc8FAnl-?(|xTJ$^_vw~O3yMs7l zzFtQX^B6t!1f}Ps+-2M_^3J53D6e#tKYyOWHJEc^H{$KQoj~spXj~%3_PBLlJ=pdRqEJREE4dd?(HhKBA z<#@})#Dq`BiVaF3mzww+*5CJVT(%gfA}Z-I^XPl_y^vGbLlX~Y#^bLc2@SL2T0c9f zP3XfuEG0$rcQ@-}y~>M@h&rt=dw6(UnINjad*bLj_U*}&!m9wN{=$U|u9)~^pA(e^ z%uG#9yCLiYS~NXxD7-4?28BoDj{}i@iDdSGUN(p%%6M#s}$H&L( zbUr;ZvmbaWd+pk_+8DK|6iU|^huM#H5duA^3Vens4?T)|Kcvok6Pdyz6T!LoIEP}9 zEilr_q-#5F_C{)|}iO3>Yq88Q~H!pI=-!xcYW|eVr3xly6$K=h8RF)JyCp z^M-46dIAFj{r>1{cAVcZ;n0CT8?qRNGWKoZ7)9*?h_N_?VboGDAJ}BM06~O=mA)Xvt)fXNL`z9(zKSV^LZB)gqlLR z_4DW1&)GbA@}&R66YmA>Qd>Aexhc1{HaCVEW&jmJ7XVD5b$G4|f=^lPY!+uV-7#dh zMNM;^-tr^nXs!}IUV@tj24LvEe*JpBx6IY7c*QW+HdNh%A#r8vS{Vo%&2`vYSFRf3 znMx#KeY?8uhGD(kgoPm?Ym+t%0+Hido^{goc zG_@nPG)1AMmyHfmZR|6I#G2butuarpO+>Th+k0s2jY=ciAE0UMG_??jIt-M4*e>P6o8Q@X0EsIaoJ6{Yqm zzO)$m6e|MOJQ&1$`!jO}=SM&1015+k6@r=lvE=?KDU20TDIa zg9p96yk0296kAVqKX~u}Y)#wj88Bv zi8%$k8CC{@){cMlJ}c#6&RJ#(Iv-GUAlMsiD>Ae04OV-3c?AcPnG5mqnvXU;|M>Ca z&@4o@A$)VwN%iX0t3zYZkQST>4Gpk}!z~tXH60{7c}_`*z*+#W!te8rdgVAy`lY!! zR8H^~Q)Tr%DgxOZ1$@GWg1^?G0N|zO=1XRHKBWbNZ~&rFm_YD#Q53uUA6;93h7MCEioIxe_8~M zsV4@YOBBW)eYZMkuY2#&De~uDud=g6iuu|V${o%LD*syKU1};#on%(J+z`|B)~!q% z&W8ef^j@fd)P3C#1yc*lKMT}Z123L+p7f)qy?^Fr6$)sy2+gJ zkN}72ktb8z>A!SrWG18dQ9Ob@CPq<384RzUDT2;&Pq#07b;z=S@lc5S={uV< zgSzT)?uyXQ9l6WY&{Su`waq;V@7?QS2rFDn^YFmzro!Lr`Y_u!)YXSBdyKO6qn8vN z$2cI?jqU8Y9(&yp<|U{}#_@2EOXPXTU=;Tnr%Y0nLV0p>vLrPa2#(@9&DICg_u6Fc zsY(6!0vKKHbT`(@)(_u6Q*VAEw%dhE|Z<(||e++!^`lHz=@d=^(6z&H~t z(m>(waZh>?;~^S+oS##s{7^6V&6_vL$wr@^Njk31KU%LdF)?vouAr|~=N(@h`@&Yv zPWelWMyfvzbIZ=ou4^rIzJ2RfuXOpy(O)f1b7`uny}AB=N0cxz>3@A?0HTDUBs)Fb zw6ENqSuv6;1ki~waErq_JPW9QAOD-I0EVj}p0A8`?Lw;xro&vp%ZiybAyFAsPdahnqcj*l#Ue7bRj1GVQ)b zs9Pic&pn9P6p@jUP8+LgYHGGfFtx5{ZFSXdPC$I5gU+V+J_M+3zPG>W{^BdK*;d<7 z6T(R^Vr9HrluFSm#lE{A-zfI0P4kLr2(EY#(2GNa7~FL`jqr#F;J{qd)D$&oe_c8y z1oQilhzW^|$ORm?+^wH55-&cG+1(OoPeAYUZhuKBC6Ig$$il$D$t#&hrtPVysh#KJ z>5G8j0b~qqN5>6#)p5nhV#1&k|JN`~=hoC$0#IK!-SP&*1hE4}l)iVx> z9=dF2mrrR?Q&Y3QWEg-jgml5n%L}T$2KYo%5aEEBDJ_?7Dd>T6TGe*E3a$2+09N== z^1kriVn2u4t@_0md^_4?tB*#&|Dl3n+f@@&M9U39##Yuee0@IX|OV?#(Zpm9`oe{d=R`#EClM(L0sHzt@{BX^W>OnIXM-F-LLziv$fhf zWC&ChCooHaa0JVQ1Pdbs%z#^Cy?ak0uIn`w72@1u(I z85wbhKm#EhT4PaMMi3pF8>-_iNlr^UITdWk?*!(fQk1X(M-MQnhJwMe!rP_c`yO!E z@aVBVAa(m;qCD`_i+}URvLRXw#2!&S^@P!#J0dpIx&T~)8N}G*|LYi@X_>iEe(cz> z_wV1|*GR&?{Bss?x$$K8|5`(A<^h(#lx^Ypi9PxEUB8`P>|}Z7!!H=GNdCiNj*W+< zN>5KeB!t7POmvdrA5J&n(aw)<6&K3oE@8<3oS5IL9|Kni$DaAHkBgV?0R$=d!#i}d z;^X6UDv?0Re_{zNkKOC_WvV}>{&_$EikP&F_@6$xN6%D7axzZmx49JW|fSE~YW)YaA39Ev$MXm%~gFLT2G8Uu45_;uKc7jZrJ zY$71cO;uHOe~#w#jdG%kL}~xTLP&6JdHPJ1{>6luNGX;BOEP#}VbAxo{+zL4rQvQbw&h`N1NH*oZ*Ng;~!iezk@w>#`RRs51nuIe3$NQR_o1Z;t40YRmlz(qe z92SsXp#9^y|HURGR;rR)Ml6p4*}bMsmjyb#ecBLZF(ox`*w?_f8`UGTlHQ=EOh1BrAJR|Me5p8mZu;I-O7 z1OP%Bns08a^?JU7Q*`g%J+hNmKFx@*v9UR?-NdUHLEGty=exy!bOi_(5i9Iz+C{=l zOz|5rpFe*F!|aN-Hr>35;d~J=K(xW_eaa-8u91_@TY(C(x-$XRz=y}9O*u#U`1tvO zk;2hpk2Xx?e^@b6P+3(q8~7@vPsC$56*o`q5+u!0dduMP3f-)W7VFUbKEPGWRzcXb z?)*}hh~a$+AaV(ytxYCY{y98w-XU}}QrnSGQ(X-V;<|mEV6!A-tIk81y$o+eA(Dc-N*IFhTt=5YZWH|4Kr<&n z>ww!?9@Tv0@-E#ZJk@;O5KmD~4z%NZx`)Uc6|fUwQ?Gql7;oKLAC~BOb4NWxyNDxy zByN{8uhQvNp?bnyde@TKR-m-BwDk1!l$9G{N`P#raq%LTaT}Kze>d1v`Ih6eb90_} zl*IE)%e7o~J5V!=xTPq{qEqn?`Makk508vA=J>I%hXjIxf=K6?C_EZ};KVaSRZp2f zd5J5ucZ)=%82B*8R!Qc&rsPj`JrfUp zOu)AZlJkAL*8Mk_{>|<1zfwOW1j2NJ;0%KX@oAj2pxo83UsyZKP^U1;;@qy|_)AMB zP#~UROoO_X7Hz!P*zvu@l1dst5uk-`>95lZ3WI|AWw!iMFe(bU_Pry1y9W4Ml#6i= z^u5v)`d1yG3*P5hGxl&Vx!SBxJcPPRtGt&^wL4o?VGtA;NJ36tdBb@kZZB1};DRBd zd0BSPao>_JtT3=V^Luao_Lse){pg7mUH;`|u#a!vycv;kpZakBzv^Bb!+@vTHj;5= zLa}+z5u|;dX6QSzSLcp;&jYrBz1LEH4EDQ8tk_I|gSDsebQPpX|I(T(|M6*0H`hJ) z(g32Xx~Zb3>4^_X?KOP=i);U)UkRoMZ8sCZ()jl6Tj+!m-k!tCm+e~#V0^aMdRVFf zG#4S48yj2Z@T5-sJ=I<#OqQ1_=DYUsU$&uL3Ame)u(Qf;CHZ?l*JCfW+_G^cERwY52T8Zq|GHY?mmemqIl|RY7ladd;a{rW_?xFmAA%ML043o ztpSazE;}x#+pb2Te&{wt(#bfrMeXKCc46i!7z{du{2&)?Bn1g~4H+DBOG`_4cXxoB zRbME4%`wx_*475=S4Kvr(kw#6I^Fi|>l+`5=jKNsRsw&dcUtayaUr7i63s4;7nd1O z)IfSZ;oatIJ++7Vy?eE%G}P1vwIBk)CvE2F==kaLv!NkF-lO1V*teuz?G6SE9bq^k zOySxvUg_@NwgAIS%LvD=E%r}Ho{V=MScA=?hr{_zuJtZOirU@-Z3@~*K~8Q4VM}!1 zt6{gbwFP~RWBVK2JRk%-H!G{RD^$5p^$lds{@#HIid>uXQyq5<{8Gn+D~W~RO~Wi@ zK!6_M9L!?=(pU#@R~}@B-G^}b^(>M4UE2DubfY7X|7zjq z)`eXOVXYqEAxcV?0b8N)s||2J4ENPHG|=3fgeE`zCy3rPmi~x*8DX;wti=TYqE3fT8jJPQ?f$9e0^1ne zMa+qZ3E#41u}S&@dV!{|NO4>cU&JPhnMirC_9DXG#6#yDOKwk@WSKK^U{0_yyxTvcKJL972^` zUo7UC;*n_oeWVO@e-C27;vCTj-EQ)(fXH<08P~r(7;9-<>%fz6d9L#%_FebCFOy5- z3Sd$jis+*n%(EEZG5lQ8pJqrW?jvqN@jNx6Z~y36x*Lfe9-V}fgQevHL`Ll_JPAvK zya&wj%TGva~XsD z?};T*XMm^)_(|X-50vm=R=&(l!Jb425xb-DC+MVRwQ>1*GxG>%F&+2?>)%VuoZI*J zO1|e$sG54HC~RMz;7RWk$}LzAL5g1p(yyV&og;T(X;UZ>kMW(uEO_9_^baUTnZVPc zEaU;DtJb@Uf0{r^H>L7>@r-SFxuzak5fRA#!{dxg@~|k&k!GGzK}i8V{tGGkxn)V7 z_zBMmgX!w3uLUeHL*lpgQzN#bH2*{xjNJ$DOXY6Lc2?yKJnwy1u}5#tQTPk&Hxpx< zWiO%(ojZCA!0dm*KV8_#bNf-myqSmgdW-Pkrl;A!G;8^YCisZ*3guocFi78yajI(t zmt1i(3GL<+dWq4+wm%0Pm7-xxL z0o;C^2xS4!e;H?I?EJnH1YkVM`xVoPuivyOCb1_Wn{(y7Q|?lujyBrabGs*}iw05T zkQ@m1$F?R15Z)A2c`j2W09z-oy-w<|$Dx3nL0)~iv?X-1P%KWP9X6Bnfb&o}2{HGt=sHO`ng&#~*j9Puq=|t3_ z^F~@K39t{o-?zGT64*~GC+nTn6LEl5fBn3>3 zjcEi-HSrdSaolz(R5~gTBv%2L{zYpAI7g4sidd!TlsEzZ>5Td(`=I?92u1Qf_5Kp8 zM?+g1IJ?191XMR4uBYTRv=}vMqPalzlbNu~!UZq6(kOX5F- z7$A!I`NG@y1)`ZgaY1q}{MW39Q~}txFA3N? z$p3#epxEr$2T6x7Uc7*mtA)|e%qc)MZ0YPovinlg^T#}>O&=~4m)bHbMT??(PeZC+ znl*+Z;74l}^qacHhagoL;F_#%+T7~30)E|67s!1+-m-?i6e=4F!T?k3%@B(~{?QvU zxH0!+ttrw|Qm$oPmywtE&Wn8dG|TwwtAT-mkuF-|280ZusksioUzn$j`5QQ`-sGHg zK-f-OI(Uff#EDzR#_Ir4k&n~fw+Fk3j6>7r+dCZd6=0`mzrOW!@yeAemoM`h zR0l#H7zi4O3#whZbY^tY&u^$cssO@Okh1ic9@2vTaJ-10EToso?O3AS$3isAxgMtj zsTkqUIEeQsK?qf1V^j(G2ON_kgBmhzH^~qH2;0|{H6ZuP)N~R^OtsnZZ0WhzfmD*! z&6(Q=nek2~TfWZg{YWma_=o90YAxbdjBk|k;sp`RG*m}NO)bb8GT2(ygMn8CggL_J z_Gca#nVRzZ4nZ(0n(t0<&LBKY0a#yvk~oA0YuhHNH7({xl;*%LRnj{fM_Y5~6gvPanbpOeI|$VdjEszu)6*Bz!@1kml{*S;v!ex$#o5;e#HD1L z7OxCJqW=lh<31JvVHQo9a9G?sf9nwM6?Otz;n%sjHo$sr%#}SF4PdotPq8DA#KGa{ z4u*ZFVVZXgoFyoskkut12)XRvf#pggYLmILKl4^2a7-*dj($#*1#UH@5Gz2&7;LSa z!QtVEn_(G6i{aKlSYQ2O-XMlV6&Fh^PM_0pS{Z!z=rBb6?wOlIx>ncrEJl&1GuFW@ z^5I43jO?kPpntcff!iGt6jU@@&6d+;rIEIDzz0yu_3tsFQ^rrd; z3*TPsZAq%DtA*|6AtpKsjDHAuUKrA|01tM65wD^> z8rdd`KKUQ*`E}nzK0%D_P}r2?ls!-ZS~E}TsRJ#_#N?&z*k~QskZr|pVA^`Rw=An$ zB)1Pz0zWo1K=Nc?(NcFu@zcbJk(8fQHTt1-5C*lXDkVJ<5K%z(J<%!T40REjQe0jR z6uDNBx%748r=#5G92cKj`{_Da!p9Hq4v#zD4=}6?e*^o-r(-H_nC=6GMH8IR$-=pM za61*FMe`i{la%9+D}= z4F7AQql`C;*KDJ4jecDJK~N`Qt1K5)zL(-fAgVI$b(=505gC{PMKQt zU>tDr{NdJ;%{MFF{9q?QO6dS3EATe(Avux2)&z`Qq@@5e1lW86e0*WDu{ej1(?~cK zCM4WIxPbzug`2D6`bBq8(v{oWE9IHjDTzU!Z+loOEH`6M5C8Jrzc2MTOTZ4#fs@vu zAc|Cb0Rl$tASC;KU00?SMCqnk8qo9XLwK4HlLN~*ABv`c;EMA1C$?|}HaJp>rpRt#HML|l6*MyI z$nE(h(PYq4kiY|6NHKG3M4`L55!q32fW6JhDdD(ku@;O)XQK;T;qUsxkX>S0@cr%9 zO3W5_2vYV2SP@v-wA|Ov2v`k%R+3<6V@pj+8mWCk&VKC@#4I~IjX9jR)_On%Z&(-4 zMVWT8Aa6JwOIR^Y;56+DntB5gnzv?roxvn11)<*3+>DefF(h^Z2&Txk?Qz-=CeVh8 z27@((wZBJ$>uUpC-5AHkuLsPG&h`LjWn)faE4yb(AbSb&cf!RtK_l6L7oU0p7Lq zdZn`_RGJWv0ih8+-jQS0^i{yH1lAB?W-;aDZ9qa+_g$>wg_mX<{Suq;lzAl^-_5{< zLqvY}BCPumWM??eye9{pb#lvYyQ7ar3iY5)N?mQzcg`)$l{zygbKoTvBMC}cR$sv% zg%tBy&aGKaNYi>5rn^7GheoK$0ZDJu){dsssq?*-V;Z$2d7Za9oFOkW6V;R;w*}Lv zb?K7Dds6jNaCSxb`sY;vHv@Hx0IEQ4&fi$<5Setz`w9VDM6OK19sm*>qO-RWkpe;! zixL~3C1m=oF_1o<<3h+}r@wzrY-n&=%m#$kM_}Q_Mcy>U7;&-kj)b~PqIbW&12BBt zY-`ahp#D(X%1wM);rXqNk}UywLf&?y%C=&f8A#(H&MP5v!wt@hUvG+ZzYeI#TxkcoIaoZ)p0G7&tvmJ3 zwwO7NYsfj%U9U?(cGhCd3X~!WeFE`jrRzIqd_IuSEC4AZ)tau=G4rCEJ5s>3(+0Mp zthh@HV1mHh-CWSwdIM#jU~Tv+=KPPz*Pb*vtHXPA3CMfXm7or6L7K`96I3uL(Hbw- zJst;=!{Or!y(UdacU?>min|1daTUeug?;25d0~jjzyCWF5V8Gg5MnuWs|n62Y*Qc` zIgLIK*_@o(-Nl4Pd&Jn+e2vUEW2tfdwhdWi!C z3P|}d$xBc;NdjsEbT0yTu=;T&$Y19WabDwu#Kfhko~@jcjhEIzs%+anME+4A8E4o+h!XA|2o6DpLNJIg z$Sg2}bOHz#^YB`MCrtUa?o&iejE=f`4!9Yx-|Qs@pt_eGl;M(nLS^t_2wUWPk)AzJ z9g&kNvE1tl#i}YDd5`MUM zZs6)8jsNn_>bJ1w=ZYTfm1MWO(C_~p^%!~n%CBoFdxWk`DAUMI`6SNuuD)Xas4m8n zvRf8nTn8hDr(cAcw`|1T+I+q_iPU1`{f>cWjzfWYm<#W?E`>rVSE!2~7g~ibxTXIG(SDnu3oLcJm{&w^sey864fH}ASp`QO|-kWy;tz@6yCiT<3 z_8wB_^?yFW^M7xhMlNoiztTG;ZUv4~edsgJcFzpDa^i&dLo{j}Y${3qpswKS1(KgE z;vPQ4W{5jt_29~tP?=AcPrj~?%Y1SoSnkaiOz$_Svf~&HG?))$nFBs9iSi2dh_CGlC7jfn-dCz!q>J%%$Nj`l{V!I0CrR^$<7k3IT3T@Y79NfMT_&<4piK-$XAb@H4 zB)p94VdIlNKro0L7(ax1o!c&yhPz$Ei52RpeWqe%bVo{nu^=VC9;PC-)9xfaBtc3o zC2W^iOTPzo(q6_gs%%$c+bO434K>kTLrt{**EPm1B2oZk(9b#tUNR#k$#zc^SdyYZ zH&=8feT|g*`eVi}i6&qYOJ6$f6t@x$}|6SNJ+Zw8GfgR6rAH@ zrN2asgVed(HM?gAUC%Qj%d|c3MCwS%xt1h{i*nng(suVNh=BFccD=Uk)zBP-ig?%J zXC;q4{crCi)!N3=Bk=lSa!zt)C@9iRuJ?uL9CunXku5%Pt-`k^fQEx=m0f^7 zfxo|2(nN7QeOA&yzQ?ya{rU4pC7WX#91>fi+`Rh4pWViPO-5251$^^t4wCl(=4)wf z71_#zR)7fz{-i^8D|N*?HSr{em&23adF|H56~EySczr$8`fDYWE4UpX&k?+!0_&;z z+FB^N&k+5E6zw|P8TTUvYoTZGaL zkm6|3Gdt5?35j|Dz(fPnVz_`}`|bJ}WduS3pTY`gHzb~6y~iv6J?!Qic+OA*3n*&< zn}DbDz1RS91^|_Tq&I{GzQXZ=i>T}Q>lu4r3W?G99FbckjbC#PZxzj`UsfkDTNxES z@xr&dN5F4;gXC6|4zBZ$R{GKdIJWjNfEf_W@fCFaP{Vv)n{yImH54Ox_G_GN^-s3? zp_&&#T#iD`(HDukpFj4v5A3#v`g$--XCYxAqUZ&$N6rGb2)&7&o$#`I<89|nTN_*3 zp6cpuC|_WdSVFeRInli_)mM?)Q+W5@?9}LJkcnd?V7XDkmMLq)XuRW2W0*1CcNZ*W zK7}It3ueyCr%%AF8&X2SY#(Wimj&8P`-DL>99iVa*Sj?s?*Y!g;F)0}ethd1Cm7rw6FtP2AcxRkEyM(`U zGC_{vS55;wLQX~opl0Dr2HV0-yzH>=qWId^G$Sb+L*6e_O@9!0G#bDAnF}AL0aymO z5wU>lh{eITh2**hYi^H~ZmUYe()4`AR@)_gf;M^+r`zc)K&-K}nuG{DN>{0Qfh>$oS z&GRS>Mf>#3c2P@23gcFn_E_rANmzt0GdhaB(3us$MiJgAz26HA%C z&HQZ1BY_3CpZnc_6n|X5AAN&=+=D6sU#SC<3s7O$Uyf4 z&QEPR|CoWdCW%hgrpRy_JqL(pmhaN;5|+jl^_z!zkZxJACP*I7l6_~=_q+L3f|N%G z==ZW%kYe=zC5vTu^>Q9Tf&YEg0{Qq(3I1uYbRP73vanS;b6EL&dO%HJd&2X;4j;UG zbuaxjS(`tlj~~hLlcHS6=8n_+OqQy^A)}fn^WoTM({QbOkE9p}!yi%&u5Jw^&p8W+ zR0^$aL~n>U2u-yD>{4J~A3Y~lIoCZkeAO;J-(u9CAbenG=tV+8sP=)vmypQ{o~D+Y zb+MNVUv*X0$d}ZRQMJ+z>^2H?4`FnEC=bL@?V_rxpBMV>)p)^_Kb!g@-|c~ZEg5YY zN|ZvYe`rE?t-@z-7j>dcBMiI)7{*j+(rCk_$I~>1J94ICd~qmnm)T$A7#S4{nQyLy zy2QG;KzW~AQl-ZUxlS?Y9>%5!C(Ojbz`KCmhJo&J9K#qLP5Ll8|9zKO9JGkjrJvmX zdv1?AD7fu}3%odZ9^=Vytz3QmTk&qSoJryIg-6Z?K=U*(kXc}#C|&WPr3mMe`fq&o z|Js55FK%71L@rNGzlmxpVyG5(=D(%({eoMjVZji-lf< zxY}G45)$%q0U`s$XY})**#J?X@P=IVOus-?Wtcd+2VLWV+Cz~7uahn+4F3-_+mZ{J2Npv$D6O^YV2^#IfO z_V!)!xh{1(tvw>moZ0Rh$9;6GitX2j5TX^_+`i3w%SDTC`vvc9OHHY_Tl>bZYrXU6 z3}xn>?HS1z#A~bDd4uMGiOrqnoX{8FZ8t~GjAp+x=sAbd?wvsw(q;~S*q%%RtcmyA z57Y;s*;{YFbxyl9dS`&5WSOlybKZbqj500;PVV*D+|$u*ypK700T2Xix-G3*&dO@m z{}@Z9Zx5k^>S$A9KmdVSx&suW)Z(^V1PhRwUTmMYF*BseE@tF9V)2Xf*JzwAMLlwf zZ~VgaTHdKcp%g{XfH4u>!+)5ovw|9pdVAg``xafac#zR|R($y}Be|aT`>kN{$(PUo zea?b%z-Z1wdVmolSS*`bO2W)27AmS_d$Fh#w&1j~Rfr$YHour3Gs6Qk|UU$&WO<3{e$LMyP^xu*@b+qYSGsbKH% z3!B<`vY z3=PW3j^AlXgAW8Kt?OKUbNdU_PJkpDT`~FGn*>)9L>mKHRKZ>{|M3+82r_I999X3{ z2L>|W%)wYsH6Y#|932f2edVhTV2MKFFUNRoRh6G*bu~bP#|Ri3iXj;~HKVwF)k;Vs zI0N|01yb&~*EY|@4SKnBN$__ZlPU%1{Z?~QBb2{rfN1614Im)pn0B#x<`&`OC`@wQ zs3J_(nv}Ty?jc~N;>^sl_g>KrK-K(IAa%c6f$+vhSEx6(3Ks6Y5d;dncb+BrQtD-P z5fP2@0#u(AsuWO5NIo8gQmm5=#TO#@UmdQ_5tJx!d>Sw7i$J;qmu-b0sP>e=i-hHs z(dXmImrNo5N8J;j+OTUCzS&R{|BF*;UPgVTUuxe)TG$%fd5Ib>1+wd-wg zbM{T8s|}E}tiM`0Q0h<`O%d^(m_gS_$!9;yWpRM(%>b6Hhp!x{T3vFc2HTobIqnQV z+6KCfguo*k*5l2|0S?NOhGGw23s_HTj=%Vlst%rp10cXaYA|U{ z?}D-!21##C{mXIzQfAm6yrJ4IZabT>PU3e#LuCsJf*Z_PI9Y8mB49Dl89FyLrHY>e zeVX2Jz7g_&srZdg$opS*?YWOatEuOdAiBXDa@P^^=XnCAdP;hPOSsCfY|E+#`xkE& z71x-(S?6@2DTdVSH#%C+ASleUS}i$cChenz^WtKxR14;vl3^$;>>yc!GlzAOI@#Ec zM^QsM&5s{KLx*{Ejild-gc%Kr=7B@N<5y#w;n216b;CQ5o_<#I9+~bFVlttUpqn9y z0U=Bh=cU9mq)TV-&vP<=fioAh7Aaz_NXO8WBZ66rF_8zynaJrNd)YFUyfW-TA)X1S zZreEJ?B|CjFWS@%t8Wg43a<({ZY;FED)O{XI({LXgi~8n@iKfN;Lw(1WQstG`Xhn}`Sth|Tt|B6X8|J*2{=1$TC%lSaOT+HTEl&5Oihp$fV*GC><1>% zRe_JjG_yIM$ZXvpo>DD06e(nW<5&xMR7_V%6@0kkBbtdb$Ea$tGQU!o8ze`orCeeW zat)FM|4CSR3&Qwxg(o#jiMN=1MH!i!Ah6>1w0n!%h-sEjcM6ozBf@DQ*7HDJKgg-u zgjj}FzQxn65V7nwgH4_3#mO!tr{R^d^?1t#pD8%XC*e9oAfbZ%oIAmlVqpAR3yDMl zh1}sJb14vAAYpQIg|T!24MO@=pkDWCNa*JK8b=-;9*}t#3#?4Wr@T0{3s2Z*7?(pN zDzky*?10X;s)JKZsX2)10alHJ9h8Kudi0#ny}iRPDH^oFSBXKjCIkp~99x=t!wJ#UV0{5FG87OxGmLtZb~-7~x|b4X{?{$7VHWlWWHS zl3CnRl&3qxKMa!pS`L)Ez9dyN!?X1bq@f*zYBeMp*QtvaBIY1dU5K}JZAg|)0^P@T zTWu`{cx2=aQ4KrC9qD*V^Q}n%g30lAD7iZjr3Us6E7L8v298*$#dLma)A=Ux%1bgz zjI@%Tw^a>?zD^r_uAi&6d}LcX!wmviu@qbG_##A5H56+M==qC+DX?e+5c7fg&qke~&vP0rvOn~@;O z0FyaLa>5T{V$spvXS%;TyBD5Ft7xTdq3M$kXgwBFP5la}Shn|PS!u{9QEJz&`dhxa zHqJtmZauy}`E?XW@b1{cv9qP7xoK&BM&=+Fp{i8c#n=Zll})8kPQzw-T5s69++!_& zE}Y51sO1ZPO&|-CcL_wzLH18(R=K1$o%h^q{IFt|b7)GtR3m|**Da+rgaWOg4=RAI z0LJ|O zI#VIV%ek%r%a6h@TW#LUtd?U;I237kT5~STByaMmY(L&3RWR`CRgOEE)D3cBa zrGQn;$HrD8>{Fo?+SKCR&=)uF8a|@=l-C}iUHx0GVzqWIHM3hO* z!N!)D!J`IbVn@IS0j=Y_`YK^VDzlFRw*b@#iro9j^!3;E)(5fMY@Zf$p^4~Bwe)p; zra?K-SN0v1+MaE`o2yF4-P81-7)8rnn#s6K(EUM`I4~i$-RC&Yj;}YNqg!bwa4MAU z%mTO#XRzDwLxSq+>(c>F1M}#GLKtIA^(H6%7N~3>Yu$w}2RZa{;!;nJ*WdA(etrOh z<|E^V`tu;k4_ncKI#a%_vfYWo#rG4-=1fZ`@d4Z?BeT`(W`Lm zvaKU3e%))XJ1oWP4a*$E? zg)=!DscYUSRWo9`dczG{@b)9Hfqh-#!9XTJ_n|_o5QD#A;4NLm1$S6hu}nXkYiX>j zKme`c*wJ1Wd56sXI%pzXovuW}w`YUHw9%)xu-iyM6R=g(ReXu#A2{N#g$Z)If#;p9s_<%f(hMYx)|rT#l7 zDdG8Z14F}bz-ykkJb5(s5=)9vS08a=e}gnWE>9Q`|MtK5=ll_LMJO0BdVAj)Z(53T z*cf;=6lnYD&X&4(5^?qF{hMDiQ=p;>_!Rk6&?`@1rx6Jaq{|7@%NQ11}c5iL28u!7l28j55dwnRV8i_Vbf%Y+S`v)Z#h5$uJ+NKa^lUO|By3_L6b-?%5#0svb> z4znc`XF_VeAPoZ%8K`%?lD;l;-=NBynfB%9=Yt@93=<2X6v6$^6A~O&2176&`_cHs zDMksCvR=9cVyPt&>k!SYp5l#xj{)Q00Zt#mh$IKDG}PD6Q>aogg3+R*2L}X0dS8vL z_j*~?fHZuzzEjx(Y8rw+r4S|j2KuM&K!{#{aOJBlghUu+ysBa(QKjHuEsyA6JVuq% z$^gq4oRP{02M$8QF3gkd>b%itA@H+6Cm(boM88Z-lQ{r%BH!u)9q=V>=Z1*5(ogiB z0Ew&y^gej}gieV5DELD>^32DMv0r8+Pbj!<+6A}E26@-_4B z-8*k=mxW4piN6Jz~D;l>X>hBXh>W#kz0Usc* zQUIS2wxBPq2tQO41o~(iIDWSpm8hX(fKy=fpDj{^I3_AcVE#HGXDon9j5>Q%WX zya*qBs7co-c343zG3b_7U{g#(8FJCbE@%X65}fb#+4cgUcYk5_e~7N}S54&qegyj8 z)I{dt7y65{(dGR3(!$rJndEL0bT!k8)%Z`h8gb><-Jg+#OzU%BpAu(((MF}Aicd{K z7!w?Ki^Q0PgyiM+X&&-5;xXO~ahC z(C^NjnxZWy_bD3wldYYR5vN!L9P?+nAvu^0KB?j}tAp#@gsT!MbE#x-zgDErvj;~c zeMlth&Ap1HD9_q5H1`%FyADpHWvfVJK%qlyh+jX4H23<5nx@62Z~NSX87X8SWz6fy zm-hCV()~SbPhL8$Ia&U0N3&M;O z68& z60lVQ`!MJ{cxb3J_ym1Fi(LxER#_pXYHzowI^#tszVnK2Jeup`+~OBKy0Wfpzr;QE zh+5(8aQ1JnDb5o~o!#!Unx;p5^7XF#Qv}EF2I|R9wDgK9E?v}PJ7-Mf`SG$B;mPf` zSgKLz1k#j>Aq!NIm7{d5CovzffBSSrfe8J~b}KTJioJR#DvUm!#jo$TARs;K`i|`G z#m6tOWB(Y~YwJq8jD@N`0rJ3ou$5^l=uc zBiO_P=ZF$cRN#o>83`u3q}U>FI5*6!)j zl%Tu4y&_dt{Mvvz@j=Dy^&cG&TmCWK_Q(rtzseJ-NbY_?e&Ltx`FZ*UIvAOd_5vG+ zY1eY;v5W7lcohn1U%^Aw?dbO2`7yvuuUQd^yflIWWJTwYSD=L{5&8e|3gVTn=yPHC zea5ZZZ|Y-!m1vFdBw;}NV3(z2nSf?8{9pU+4JVGG3oxYA7Lg37yNk&wib>UKNA= z=Oj@%r9r`#f&%+e7c^xyA5{-?ot5)Xu(0O`&0?j9%%y$UuXC$l{5(v09~)FXo)-GN zc)>qp(R>?9z=5thw|1f_$Dron;$kcYuj|EURT?&~-8^>3=G%Fb4jiT`Xv zs_x}Wrnkl)ohMKqUf@+6aV~lDfYIrZ_G9*M?S70uWYYDUS7ztF1nKo~+7G&yF}~k& z*d${m;TD)LPkngjwAT*-x4kN3)tM#EJ)PvPhUvzXi|Zb8cUQENF23vh#QFANs?^V0 ztCt>MpiIx0oim_QH9T2Njx-inPIlKJ0h@Ja{v8_H0{iTems`pvFx4xUq}&;YJZ9S% zeOyT-4{Yz3z$wZIp0teX`1RZuTS|9s@M>dBf4J)gjcXqI5@a5E>H_+Tho&IsdfR%UX zu4#$j>6EyQLhW4qG-&?!MQT35i#rq2?3zP;=jqRFusvN{u2eLqc7`-o*`W6MkLl`b zxaf*MU34+_VP0%{q$b~mT?_ub2v60uZs#HcT38R`@m*83sNs=Z_F-o-{2mKxyt7iu z+}$!=4HsYA(Vf@&2YxxPu)E<@Xc&KP`>hPNgBjuVlnxBsyC&mp2#*3jf5XlaUb?=so3*D^rUfn`N`Odp_!=DBlX6i@m+mxZB5;u*h8t*kMKa zmQPgj-A>DXSA|k{x2(ZlaT)V?(#+0;IP}Hz$q`-JQ5P%GS>J>Drag}~yHB}~U303d zR5u#r6}Un)`!#xZ`zn-&Y`LVmU|&y?koV5sDb85G`s`W5s7qd1FjvF+?v>%MMuk=o z5)azP+A04(#l@@Th<9827&4d%tgt@1Ajno^gJA?^ScnHP@WaeCBv{$LT@d6Mbue z`G;-2C4M9xv5DluQT(YPVB1qVlRiKC&9;)*e5Llrq`0nXYN6K4*sU%?KE~r0Sh(H9 zf9`JKURbsF(I68yuBn}zwLfkrc8d~Vqt?Pgpj)uUwD`@6ih zeb;{breXtplL&eci@&{FeD{v{)e2cPT8VD_T^pB3SDs%xyA7R{1Wjfxsj4-ztU2jM z{g}{LsUT8NeM*w2jp^U{Z)2}%*HeA7D*n-Hf&6VfyS?(rB{i#7qOo%mZohnS;)<%D zi?cHnmpBhNGXNv0JI8TF>YH=1MI8kID81j7X(r^Q657kmdUo=DPF?oIRY-8&*|HQ6OC z4c1j2@+T#aR1#;Z89%muNdERn3Z_3%6_@?3+Tw}|!|zRa7xcyMlN|Rqp zvXoUgGaR=_IQ&xoq#$QR?pMCQ^}Xy3cAVQHO|ZPpa<@c%tn(4pv7WJL#MW_4-Sr(lcTJsVwWA3KKYL)g}l`9$RWc}?E;t8-R2+U zcI`pB$(qr#3D#7?HR(Dzl;0%giY~jYEs9Pq_E0B$J4LKueh7WU>U?G@HbO59`A)`a z(bBl2Hf_6GEejMm+2S8HG=-_|daBowoPIl;hJA@Kpe47KIPKQIgAB8%3eTNTHy(Q_8MDlQTrt1n7yi~(6&pl4&7uB58%ZvNhXYR z3iP3fBpJPP-V6UZ9o%S?f~~R%I|d;o-5Q5rKaENS`s~* z!{$-M!OFAoVFFVoCWK~8Na9@75SV8O-w5#D16uU+DF*qFp7ftb|} z^w3L%fCVnrm)9Stc+_8b`}Wr_)Is5kYuOqdeSMT}NWxLsY^eqKby6#1f&RD!i|Ble z$QgB72_iNg&39|~@=}U#?J$H|STZZHr68BR&}zUPYH&q^#U7zw=cJSV>CMvdl024V zd^*mb&*Dc8tufbxAs0692?84)Wx_Ok+m6?caGUg*z_dwFxlHOuy5Cbyhk)ccjq(@{ zgNW*%-HgzW8FESzKi0-Hk|~X+`HuNP8(suAC2|Xe%UjLw{^Bi=@e2RF8$kz}kB6@zDs`b)7$2C2jM#uag?~Q-eid0rL z35>b=oGyVd^rQRRQ+$_!SgZ1_bo?BNrx0et0pFbxY7x2OIl2%%eKGo)biC8vFkMa+x zT*@dUH;%-=9w1}ydOL%twAu@!47Ig?N_?6CYp292zD_`~QBTUe7sj(ce`nJ`em4pj zuUWw$rN91eZ+9kR*I=XeZhZO&KWnjcmSkLV0`o9EoZouDoFKAD?v1CT7RzwQJa<0z zHJT&I?F?h-YB4X4-`3>W-3|7qM5dDQF9T;8)^KV#3M?^2uH<_CwtJT}{jhG_T zj_a`7k#NTHSBr<~q5s~Zt{+yH7$5yTn0cPh`r|c8eE9S}p}&2Hf>WMyW`@pTmc2Xv zMEbj-tKZ>+({>~q)}N*^j^8u{)D}+-O==^gA;KI;Az*D`M_MNmq7^OkC+mPT*{6XYI=A z^Tx57=0`lHyCq2dP&zvNHMXOJgO2NuT1d68ntzMc=t_>#uY}9gDUeVaDifF7Zegu6 zpq`*P9Rv@sNJe z5$Wvxa!{DTO^`i8e_uCrT=y$0E&W2pZ!=N{)=NBZ5C*6$MOC3ITEdhO3y$>-3_H7C zqw@8dr;hm?3^kdoaUMVe5H;h4Tto2c22%=09lR+K1`TT8(<1Iux0tG2UkvT95RUkk zakMA~=ZG=M{>f4>hwbR-hznQ+^V$e~dPKt}XL?^|kk`pQw}y3#gX3c=4%eEzG&K(T z&v!nP->l`TQQIy+f)3{94=G6UOn=_t^>l8Dk-ai*{8UrbO8&XEwIynhc;(#LX$l_N zji>pqUU@&dzPzy8Y`hb`xcfF^cTtk%@ekcy`{wt-+f=4@VPGYY*&HggB!vn}LS>HB zeXF5lWGiy<8#0IoKx$OR*1{rUW$S~IYR}q94D(vl5v7I9-OAj6?%Gm}VF=5Pii2M` zNeki=FbnHqe);I>>_fHsF_ygY4-J~9(t_d-CS8rQ26A12BSC+t4RNqj2N*&VgcQ&HvH!w zhBVzuh?Wiip=lmFvvBlhhv_+Lc$83EK}lRJvv6mWw^S*vwQs?OrNMx@nN-lh;y)UG zh@)IUUMs}sn;X}3>&U9|8Hp(2)KNnL-#=Stj;E>R@)v5%F814%wcQQ&SCdy-FVZ@( zDHXVgt0R&^Bs>#;u=Csq?HW{Z;OLK{`_~|5} z@ehV~q!Tsgw7#8^s@>IOsZqoqe?%q=ZnXV*XI`SIms+e`E{@*v)cJw0IP~mT@9c=V zYASPmAI{Yb`3mur(A`Z~{rt;J@LMZj?}+-xM| zdQ%s;>$R@4JErWH^zQQ`GVxI%iTz^&JfR&KB%ROIdAQqr+V3l?R^lG(W&e2E=?BM@ zP3k2o5B-%5(P(JwT4MVt{C^wEJC#zYF4$Q{RR%DaD=Mlxl!fTGqgFPoA6l859(#2n zQgB1Qf%~k{kn~BjJy-soOff$Z!>>Ff_`Nac$B}r#?p|8{Hvey(dVR#DAG&=*sCbmu zR2sN3^43%QjP|_sh>@i@a`wvTSDUa@HY0Nu);4zAq{zlyW)G|B2wb5ZqSocgZq4;w z+dz=S!6oUFnLkZ8#4Fh*t5kdSdG;;5v@iaKw~sJ6)hx#NgUNVE!TcAq;XaL+yr_gp z(S7gn8nuqW5s!SuAM?yLrv{}@{%c_pf+M(tL4v>Ne%o|ShYnu+Y?V&zfFqm*)sM*j zH5idrvkU{UZ{wxSpMHm?gGVWq;lTof0%_U{Q|uXf65!@smJ5HwF+6gchjNXmm1udB z3l5C`=ojY$#OA|!G2KyttC{oWh}&-J-#W$+ksPqhl{>5+Ekd#A`zA>nPrILZR|z=_ z$;fX}@QK&{*~hDAC5Q%I@~6nWSU>$Pw)+HUFdVj5ezAO!fm}T=amQT$O1z?7pqb|C z{iq>Oj>?D$e;{(;qx>u#o*|=kl{%z^C=P2}R0V9V|NIW0gahlHUJMgmF9yVm?B$NGQ7+V@ci@^E#J;Y4cLKeae{ucCK}SqLL`A>d*^;#_sV-L+ZsSXB5FHDxhPFU=bqXorbqay9xrvI~uplKWvIu~X7BgNMCz`ih?x zKU*;`ug!EVPCr~UD&kU1Z1bqcPcCN*R?3j(3MP0+%&%o=dG6_v5n9tdARNyUx=lTSsyvKsbV4aH`S=(TH)Xu&^$Z4GJ=pv|9w( zzPjxzLEWqx7KX2kQ6l@C`MU}>48X`HiPi~bjuY%{$s0$8iPxe^JU?EdhNxXTDfHpJ zvg(h|U>r@DJ2u4|g3qJ73J5_dR* zIo;(mUSR`8fL?2|M9YOmSp@}!r3t815Y43vj0|1M0}7;mT2ZH2mG=T+?d#=6kM}|= zoO~>qLm4UWd68t)!oos?zO1Cg2|P-0VeSGj1d#KP7Yw^sCBntLiCr}%5P7Y`yGBHI zFW?j%OxUqPuY48hGk^`+0oY*r@p0sC0|Jn3Z%3FnnqzpL4NxaxU*aUkBFfl{L)i~! z;}YxzD{m+$t$$UB@FOb6-QBY_xON&Qnf&gx?yh#W5=Nr1| zm>5VvAd|XY)({637M&nG?h%-2D=>kuzyNLyc*Dj4%l>sv0MwSLa}q5D5^hQFkF&%> zwc0HgzEv-tgk1$#bKuDUb4(LlTp4o@8m!ekOqYrPxp5p9n=~Jq1^S^BH$v`EWY`RP zjZt{y0txvptF3Ok=}Gzn?HM^|&)L&^sFv=9F-F**GdqWXbE&75=knTFB`|gD?Pmt6 zs^%wnK2WG^rD+0Z3NQv`q$O+suM=W6wd=lZ<{p{e`Eo*RF9unkzCy{{gbGLUMbp-r z`ZpNgML{6gT}#2%5}!a|Dp0c925SD~06U{BK{MDYZ+gWgVvtTK88npp2%7FbG~0_> z2x+o>cB7OVQY)FNYz8>Y8gBOUWJGO04x zujz`Tme`a*VtH-(W^fLv`}C_1K>Oj^XWvQGubpwBfqUcyyDCP=-rR6Ka*Np$spiW8 zlS^-7H5w4YX)dU-0Ay6J)Coi;QXWB#$G*8xWKj%~XkC#-VJ7N!>15dJY012bYSn=M z7iGUh=gEvGw@WkTI=L#A0EH91vbXlWRf_uPd!18?D!hLjiMWo4%PUpPQ6w#j)DXeR zNW38KduH#m+OZQg$bVqkym`)iZ+zawuiDXuKr`p#?k=?t(UT# z*!|0?oRxw)oy<_3j_HJ|g zJE=$6X!Va@*_K@nel=Gy+WzC$OUqrfjC&%V#VSEse75x0)fjoSaj;ZdgvQ_gc-1sO z;0tQE1U^b;*Z#f7UJWi>sEJUMReJx22%JfKwZ91JA`;zYzc=cGUVqSN2BGot?~URG zbd>ME+G!QQ>-zf&V7hhW9{egG(Ovn+huzf<)?Pi178mB9AaV#nxC6p2h_9LC>`le> zIk@J8>`N1E1fIwKdc6#zWrF9k;H159cRotyq%a3ntG8B^r@L}N{v<_*g~i{&Pf7GZ)g-a<6yCo_R#4=4mOlib2YN9rCx_5mPre48 zttB8P0{+Gca3UeeOpS?J`EU&z z6{~9x;xIV$W&ymaiY3@U_vH?WJH84~c#$7+^mPUnfsGDAYdZnJr3!>ho7-1mehm-e zXAfXlHsw78WN;##U};#X$lFl+-OW)B%o6}QOn|rpQ=GN<&=k;<0i`NXZI=DMy=yxYGs%UfZyk7e*sgWZXpeoq2ovc|ZOLSKd}lHcHqH)VpgR=7(y4@=j|!oPRyKRylz1QJ^K zV>0%`8-I=y&M+ieg15IlQ6W&&Z7(P)Dhe{OQV%S?e>;yHKnECu-+Fj3^(Zkyq3=u% zASe4VXqZ}PQtSZ4@N-7~@A+tbBmeaW*nNAJ7*ei3^IEhVU4^PjRE+My^Yc z>?prf1(bGD6n8H7_#e(%Xt4l%z3`9^!Do@vT?FyIP*=w}_)Ac4x=a;|ji60u5q6Gt z*$PsdY<4z81fg{VPdC)gQ?2s9v_kRkh0HD>R0Ii!16wapuRK|~^ClPKkbvP{4;JXZ zviXl*3joP(S1f-4AiESbLbr?X1245lt%$uzfE=uCT#vO&J5Do@hz9uUlaVAp>`~74 zRzW*Rl{{TpR~MYQBp3cQye!06kc{jlxR*mCx?I*|8n zZ>|nl*PH};~{e2nBNLJT^W6K_QSO-hv8tL$NsHoqxtH3Kb1WO7L_u2%V@(rqukN zd3=Jx0=*u)+mP3XLfZrh%jG6$F@hgJl5A+JVYS+e$(@QUOU`n z>*zwUA?pCH#=R(9z>z$WRwDQr2 zpp(UrXxR+dmq<2s=j&0Q3Lq5(k@IJ!l{J@bYmoafSZ`LY-^fApm{C(&u6-TYark(y zv)6elTXdr}^n|Wa@8tqG$bW!ER<%|e2P~!1XCD^6e%+KPD%xl0O;llyO($Tkn29Y+ zfe683P9G#$tiLvE3#fX_)vA=8{`xW&Z-5+O0<1VAtYH?+)?6o2E?n90R2!6KUabI< z?3ZP;8*gyk71xHHfJ@-b5#lh~chS4qGC6YPiQChr;M3Ux>nVQt-kUJg%#IgfZ$g7> zUu0SzQ}7yHS!#K_8?jmb>}cr4l`hDq%K@y!+wA^(=F3YKk*GR3CjkyXFJq& zFAjsHdSRiC5Er`;3QwqmisxY<{40xFTk?U;N*TSVtb*=W5(SxwA--5SVe%fUheD@h3Q>annh(r-e7A+fey zD{QAOiyH}cpQVw+ovCy^${bS3TRTt?QmW-9)Z;)(bMJzbN#=hCN77oaZ40G+yAo(a z_7d2S0L>zc_vI`e^n%>t?e79rPLQi3VA(aFZRN%`bIJRpn*}UEW(S)@g4FE!sG+~0 zMgkp^{&59D+;$*FXkVQIHc}QYZ?TWeHR(496Ciih1=(#Mpr%<+Lc2UX_8RxZd*ld# zloBAmo}QSfr>uNpw-m$=$fDPwSSCv@_t>Dx&wEX7L=E*0VG$Og;3oM%Nc{ylZxHU| z$@FJ_;80t6Rh_$Kw**YTkDgq)gheNy=v^AUSJUB;_$AUkp3vey|6bfh(_AvV|F?9L zbN?NDM*`gc@6jEd6+HVp+fnzMTV!H{AM^`@iVrc@yngF@l=&^w3D3qteBGx<$*PS9 zwdh8xCxaJPuU5{Lvt11So_6D-eCP+mBTHwGlWgE~pJmj$DO>#s=!AICE*YZxy~n@~ zK2AbL@W&5r&K2@4hqk^ulwDj;A-J3J(PVM!a}6~|j|4iEbagui)|%aUw~4d6k>7sY z6{PsAGG74{oQegrY+6F>2wKbNomFZ@x~cROKhiExQ=FlKSRd|<9k<S zLk!vY|Ls5e@;r+)(Qs|6Jj%FY_HUQeH6N|b&XdXt*X>_~YzY=aKL zBbNFh+yAb=^P+};#uRcf=ixA(hy|iY2m%P(j9lq(-D~Lljpw{uU~WQsUQ-L?3wnse zu7fQLW<8jNl5LkRUBachFg`V9F~qlbyZEQR*?0}02?PWLV2a;`nk348(b3URixIx- z0*X{2QKS# zJ3u%IJ*Ffjl|D^HK|w)IUS>Nc|FJA@3;z<CwYc3J(?GE_(uy?C*rvjx3>FD$cvTQwDum|?3i zfudQb|Feq%#%nM#NIEwbFnbMgE7a|I|3)08^7;wyw0!hAQBhG5k=fJMwx845GY!@m z{QmkSLHv%vot+&Zr7X+3QK?ZseHEMIkL&ae`Bu)x`GtjrrKKhCpFsXDLZk`B*1<%e z@4GA9$i?&ekIOc6zl^;m9Iz$q?CdWPDS`99%wyMGS68>fCHL<$mx0}xUs$*TJmL55 z-8*`&>(yT$1DsO#4fh90z6V~7ecwSN5-BGCkFEo#v7ct&y>j5}umL>g{GXnLC_Zx^3Q1iNMvkzYU)pvsW9y{*u4jz~oc&BFJ zAK%{`E&soI_!eu;?)F$B-201$?tQ9t-`*&I7~F z6;-tOzCw6JY>-jM>Y~B?DgowjqZY3zLju9=3k62?H1-c>SGPBJaVRGTrcBYQAk2P4 zUg=4LwStwEF!(YCBW=vz$`L;<_cz(KH(MDfFC6etbYk#ok6SW+xlVi>Dq{TuR@G}o z5=Y^pj6$smr%~0DFF*ept& zT2;YB?DBp1CFQ%KF*br4IT4^=)=rl*UK8EkUfkXJ>UT_>80%{4kgI!6;*oNxu*YgT z-CZvF;4ZUxssT8^#R(-0|u{S4&PRLPpkPmo-JyyC%Y5B2BFn3kM zwf%OOj``P)kFi3IyXPWgQeEQ-iF&EoGHRmQxTJVz7#l`MVn*7I;U-~!p5A&+!59oIT74K(;I2y#Bi>vo51s2!KhH?JFtW; z7VvUiUW6bStG`OFs`2gHoaW`Ds(lSlyFCPtvbszjjT9=W5|Dqd^5%QXcl?rsrLB-W zQQ6weG{|M zt|v<6mUXacE+)5KQ2CZSl=7qW<;djiW4GJYhHeCCU393LR?eXCubw%sm}&PdH_!6< zDn@+{e=rMdkjuY*Jb4yNx#M1CxKZtF$b@!ATwmtSkBSLi zig(d99*G`;U8O<^L9C%qtjV#;@_z7tI?2-0ANK9=J1n_=xsEaBmn2e=f|KtFzfsy> z3d`*T7xllxZa*R>)02}``IVTN@C#pJPeW`L#XI4+gei@e7H9ddT3yr*OYV6{nSC`i zC{4TjhK|{<5o|ZOkyBy;z3A@te(7caw~3-JO^j zs~o9%+p*zc0^)CCUI3*KLTTBkW2}TugegYez0;QO3ABiYAGkVOnewj}dP%XEmyA>| zaohEE#YeEc$Y1;75;8KrXwlj9{UItcy{CNrM(>@Fh^I(N*?!) zS4Yvs^Fk6XFY@{Xh3M2-INy)1y)U&A{5;UF%KUoghQn{l9_hx+t+3wKs`^-;$)W+) zij0*fR}+79stv3O-%hWbshMPcG~&~7M<68g&Y8x64VS<>udcJ(SzScM*-+YD-|4Bi z(x&%_c{S83vDwJ#?hYoJlRr{?lz+?M`_`BppiY z&{_TLUQs=HMTPG}1Cv(_+xwl{|M_g7*I8cPhco1!F04|!?q5r(ELZw1I@hA;M03xZ zN2>C0`VA=;T3H5Yyh;;R+J|-^dFcJQ7XT3|aHVx~C6%E_EbB)R^^qSfxx$NlrWFg6 zSeW}VB{tIw%mQA>L@Bd_!lu55tAVrT$+A7;7*u=Wt}wGNp9a>JDJ|c zS}VwYn%t7J-}TZrky{HxnveHE7(aLQ>oS@SmTI#nOqFsctWRMx&DFD1Yx{My>hyFe zmlOKwvtS2ibf4@0Xy?lIAF~rhBVkqOO;e}nndPH|<}g=kYI0ctpE9IFNh;lvQ-}3* zO+rHF$tXLf=Ydtl#m_<-nfBtRzpQHoO{8I}vYCSee?qu@UrgLi0J`O|tY^&+X96Gd zm*9ORHO)e0W&iSyTG;SKP63CK7xtXvrCCxqhxnnvu>b*LtJ(I}PyZZqHAcnxVS2Wy z-MviL{&vL?_^)1Z{=Z`)7CQF_6LB9_L=eJ6{NTRLaS43>2az@xZDjDJ1p-piZ>rPN z6EchxrqUPn9w$#TmQatb3%{knUy$|C?o5Hc(k<5WBYE4RRc<>}h9(XUrlKt(1M%If zQ-UrICJ=z@=9$}ePwZZ{Zw4Rf*uBJ85CWjO*t4WWBkr?glc`xW3zcdTKLod&8T4!x z@;5yia(rU+ay1T(p5T4YKHx9{DpQW(!#jXehtw{CO#@)>fC{NHfT8J~0V*+8)pSLu zu@!dv^VdqZGS}n8x~_M`rnN#n`%eEr1G>0dfKU=q|B|I{1%W)UX`5GXGazLnzX8Kw zbPGZZjyOwWcxrYTO;gj-z{SKZB7)EahRpTiVN4e$pPC0>QJF#QiOt7w&V$L?_yJt} zh2WZ9|2hNMpvlgh0brk7-vYKRs52gXvDsx{CN9TdXuSkIx38O;I9tDy05g@Sv zwjvTCn-Oq7({(m}0M02981kmUu9acCAsXaXF|c))1pOj>-S381;sr*AMW+T0B$ zJNKB^IAHqSZs5asU8pmdXS*(c2TJeYaW~IoSabajDE0>|@n|r#bRCWsWBTB|cJYFiHG~(6K0!O!XWQqq;C~0oL~n!g3;Y z0nU~6=jwaMImqndr}RcdMs^51ajd4QVtMV>2Gnle(^?2+S}%aA0*Tld1t3(U2^5|> zK#+f{s4uEbU8;IsjSzjsiMSNp%MTmC$8P@Rh1tp4FHg;_XfE9bV!{EZG0IpoUFV5Z z2APPk3@gANe1#flxS>wR1=pI5`ONGgy76UMWPlx=R&Q;=n`Ck<1 zv-@%mE$tKkL>f@vz`v0Ms+%%G!ja+YHMpWnM!UUg-z$*h#|H#V5Hl`^*cy^G$m=}D zFgPf@x^6~*HkN0Kj+1_~66@AgSy_ljXO#z)d(4S9t3$%bE`*G_JL}%LKtFk_KooR< zJ_r1{VbPx6b8()>yU-*q41!lyOh;1Xxh(0~n_>1J0jn_j4k_ab0S>tGMlE*u_+V_x zO@PyxjgF2EP<&iy#|DJZ2yzbx+^rUuS26ZOV2&WL&^Q}EKLjAEwt?#*ODzisVL7>Y zg;e5!&>^l^#t=yD<_tqGGUnf$+oA$GHPYBo*)j+~eNJ1I6LXNS#+W{YjsPpcYhirN zb+VnMqzfPmzF5RPR>JjKMR4bUk`0k9P z(Q=7|cUx4~-%Ylq;iA>B;6tVbb|BvMKs*v0gA+6S=ziXqUAKUV3NQC7CJ7F$UJ)KQ z#pUYTbiXLDV9y~MbO+6M$0&Fj8<40az)~_G&)3fGb-s%OO2dVC8XJc~(KqM{VDL1m z6ty82cx~KBJBN7eu)f* zH;t_jI8fX3j}WWHGOeIOchGHhCGI)R+1T*DwicAoWUCZ4Y`vEoEG++kLZ+p%zRu1j z3^Zb}O#vj*zay=jBvE~vZ5Ykv#CBdDsGkzx3bN?P996NJ=_!Dq$oa8EE=qI;@n@#U*&N;3K1R&hN5c)x4rXa7&CH1Du z-C^U=nH?9L`w?;KD)2lPs1ADkQKy!zoTa5@CGLxVqMKhMzX`IVf1w+!8`%xA>Uqu5 zp7FTCNQ=t%?8D0v62T%au~}hPs$uk5%Bcu#3NS*_e}_UC>RH!5ej}a-j0=zcTOT;2 zBG_;6GBPr9ayIXLxb!C46aLrqiyQg8e0;z#-3o#T0mijs6xPc(oCt+t;BdI&!096B zFdfrmLx7JDF;~!zUm)-WZX(Uy!`>~*1!6e%z|IWG?T?28rjfiy$Tnu{>w!(Q(^9-;9D4kYQdwBSSH;zwt-wdbqd-o8WP5pHkl)FNYXi<>x z7w2xyI9(3A|M!0VC`{E9G8@mY7EP>n@|?kn|fytL2Ue#iW?9b9F^&A>XEKy&_gy_ zh6>Z1uOTF&aZ!Hf(d&NSQ-w>4Z~PjF;!(RNvpGL zS%{l-G1WncH8!1zk+H#5lX3lv*?nAehbOyyRMgaogbjIlN1V~)Lbo@=qMyZG*EpP! z3gm#ig>`HlI9(bdd6ysHe>pT&k-A)!JoW1eG8N3ocx!8raP=NaK%yzUv(ydE(A2j+X%Ks;!<4mzkz~%&3bGg&##i!S|P< zh+@40EsRRpYM!4{bryAgIn}p;FDoo8}|8knO&tv%C#iTL~=!Yc9bG3@Kk zK)ldcP8<@?V<=hem~^q^h^3rGHlNNEnNS_j#y21%WtF@3;<{3^{7ez{3)|%ukxkgb zCjFraQ_e*@XqrqTHV=_ZdZ_+HmsaowHk!m4J&TGuJUYqgzOo`XIv14QH`#PgJ0@)| zP2}snK_^WIEy|L!0BpT4;g|x=4OZklhPD_2We;D$$HyPB6+(+5@B#c926mUn@{Kw9 z7rQ@R!$Hie5*f%V7S^&0Cp|>!)g?ve4rAJ+FPySDO3y_3c?RT-UMRMuF=3nf;u_LD zD8fEb#=>;h=9OeH z84#itjwOqsM+wU)*|b4@;f5#Bu3CLQ59! z?4b1iW2Z(GM;S>~$C_ddvkksJmm_ESvN7?p<=HXdixt&tnGon2eSM+MxgE}%`?y_< zClej}op!2R2Ibz9Iwf800u%Ox+1TeAbzQd(P<~pwwx4LSS~QBDr>084P7=ypUFH>l zL%@5Ls|!P@ohGYm*slEoslcbZzdboKf;d~s&p!fnBlEL)bm+2nNSOCy0cR(^p zP*9B+KOI;R)@N1$1ay5jYYpmZXX|F!#Rh%m-DxDGV`XjFI9W(TP5`GsaTpkP5q3~! z%d$#%MnuRoDrj?gN*%14K~4O>86=D?#S@qQxqN1jo7<}Y!kh=yp^?J~yX&Vt9#Q8# zUe{2BSaf`k==I`WIhZ}>*QGt`QL6vNhsCi3!6M|~$o>a6ef)~NeBrK(MKw}D6v8ZF zvu`mikJ523#UPZa|J^PBZ*}wk>W6(#TLbYpkha~UQfv)}VjOwKSKFlSy=7ovFosO& zGkv~zUD%R(<2=4dO`!jR5d7aUyE-?~q$DKz?AqkAH3|YAJYcEzW+lTUaZWli?e-?5 zd*j{ur9e9j?24^p(=$zV6hxzEzL&%P5+cZ*@kp!toEsLTS+MZQwt4Rrq?)x{w@nXi zwTss0+-3iS7Rl;QjV)!Lz?#o@jR<=5@Zs>cZxuy#+j8d4BEgm$$N zj*WS8wj@}6weq_m-g9Y<78=>M)gz6|7L}Ifm2}DbFsbLVRQA4EaQG3U-Z;@bm(9o* z+gYnXW2TvUs?^bM$(WDt5^yi|Zp*RHCBPLr?$p>H)#HNL|xhU@(e(`Y2lkzw{%@=+i3vpBx*$R1dqI?{V&H?i9%rx3Dy(821oZD4_ z_nr?p;dVXj%$c!!AMJ8-+H}DEd-AzHl3?!x?3pS@Axz}Ip1T&%4pYqs{KKPr>)+0f6O`D)h(_W`q0j%UYExE zTUaK$;T5}|&>^l2w?eERcd!bYnq)T(H>X35`S==lcFL9?xU$m`=ZpRzKJD>lS~PE! zdYvxUaC!(haqn(TXo4MlG0opO=VMDLv&i|);hmtM%xE355j0KzyPnjemB%+jUvGDQ ztfSH6yGu;wtiQ%-mqPRZRKsF+#Q=3GP%*`dobVAq=Vwewqr$xZ-=MmG*|k_4gHTz9 zaJ=;pCJ`OCBK;}uQ|~cJFby%VeR$B1z6HK${UFbK+>+J*kkIMT1mPY0{GtF5UU0an zy|~`G`@9P7=U`0AJwAJ9H~o|jdSw9IM@F32uS3-WsQq%JoDMC^$jAt(6%HwA%3$7a zTg`x-2}PEHV`66K-kBf_S0dP4=0G_d8ymae1Q>uvns%!ffcu=u!Jcn3MGGd z1T^jJmcU1|byyyFjv82uuwDcrS4_wv0SK`JxbZfp zeLNSx!EL*IG)mLRk3Ev#s=t>>PsnWIOZJnvqB`B6CRa=nw9zG2Kr`F~Lj3ab6EwYWn{CEUg^NlF%+h_b;$1N4>x{db>fapj~!J@MG9pq{d? zzk0h{aTER852s1vg|;(a!Iv^hi$=%gsv`k+X;R?lZb}ADZ*!tDIH5tX03g~2E?bdR z20fz0v>jjI0R^%y28}5v1sNF`Fgi$=8KSjl)80MDJ?$IOl64b00fsvzRQm)3z|1W` zY|;`;Kr+c%Z?vbHYr+QPs6e_(y+=NnfE%LAp0#6Yufg^NuaFy5vj><%QIQ#pAE@*( zyYxjAw!xkQF`3phK5-U|S8&}ncmiki9b5kg;vaY8Bfvxi5%GXjjW4x8Y1+W@12S2u z?+&6RZsO<4fTesZ?l2wKmaYW2@hISO2;+s;wxPL`q`XF8djsu1hsyJy7PsTqeLa(i z=hf)B5};HkTqLiHgocDfk=n!kd3)|^^*?7A(hR|Peil>;IX@rG$p4W1P0^8@Yk%d4 z7W}V<27W_7{hz2(_&+jGzQofPx$*5oox{ zU<5usU2t3?m4)O?YBDLa*JsQ=%JtppvF0Cr7eiZCRxLKy*EiiC7(0eYvu~_lbQf85 z-FDe%juRqyJpcuIAj?VDZjehHck@T6M>%MZ+f^s-=8&Z*voI5S8NAw3&~_6GJK>e2 z%F1^(f{X<7^TbM(3_ZC&9WI314PFWID%Xt#^10hKWGJu_VV^t_E4v=pjLN9Xk4pwB z9M>OQy9->p>kaCq+pL2;j<>a#31w36sj|kmL^saAc>BnR>B+4*3vXRPigo%-BB{iU z;v;iW&c)w__nD~o!=An;9^@V-W@E)|viW2?Rx_JuSIR~lFnA0{N{2O)sBefSaaTV+ z&3DQ}@9E*}{aoIIR|8I~avlk`>?=JtDL@zEG9;n*{AnvDj+8rFB%=spcuS{?#&Vj$ zEn((8;zbvqwlZ}hoG>i3W;G3(?L z9!za1^X53BtI@NDC&=*O`QWT~v*SFx_?-cptX0pIGm?^$?%hMnzP6v8ybo#-mq>X$ z^K{e1Ww!fKm%0w*`3_sR=fAy1L3M-i(KG)WBJ%xL#&kQlsY$*;W|i{x$9JzeO>DCzyO zmR+b(toX-}&ZXv&Ut`^&ohRh5JSKM<{AWCHFRG4J zFy{MJ?AT~vwI&mF;Kj2gi%s(@i62|&qxQ0>WGxLu4P7^;!?W&vCHyT+TdwG$)~87; zCwkVbk6>?-k}_4Az#-h#G`n=^X5$mJain> zCa4_r!lF$Cu7bEx-=4#hIv5Er^Uh6KECg1E?=@`@DI>;7{BAB^?RIUqrDBQH&(RlL z_e*q{E%|!8YzB_fJ2Q@C0L5qLQdN-a!#O{DEuMgN6@zoGw&(4P^kZ>kD!Bu@ZyzIe zpZ`{G+q^#9=jgcPn>1!Eh~N|8TmJt3i|boZ`H9R`1oGaZh%A6N^Qu-gJ9H{ zy!p2T)Bqp52KN!%O~!ScrnN>GCzO4@`}ki`e~@_Gce&z9#u+ViX|=vd4Utv~sp|3gUo>??O#gqqM-RZP>eV&Wy~1S!N|e`&DPfG^eN z!z_g9I&U+~G~^1Kx}& zh}Yi1ghlTOFK9m2GUQfVKKSptse{j0#pl5Nrf%#F6dGNCyFx{f6gY+9WfiZ zyWgjmmReGaMdIo)n&h8+*y}BQ&p#GmcS11^0Nq}IjJQcrV3vU)tEpf2?8{l-Y8r^^ z_pf2A9unb>xm0JQ68;~fQU#_U;L)k?% z7!!Q;5`l5C04KT>D)qEgFHDO;zC+ZNPxuzeo|x|+9GcfwY3J=CEwHCtCib@n2GPeO z^^^^PwGR{uFB>@Y(Bnvj9Y80N__Iu~qB>37@*ICo+fAM;L!m<7_=R6=Y30Y%FZFBT z(%#wI;QOi)Pan;p>U)~jk(aAt`PRA3WGD<}GEK+!_*LD}HqLHKhx^?VI;eR;Ae!u4 z?dK?#Qp-=jmNrYd48H%;5wbG@@Zd|#IFk%2?0)UWN>KaL5kZdK0UGvPWI zb`ojIod>(TGzsHAAOrp%2(EYiIMN?&4ll_H-G4+5xFGKaIhl=xm+R@|$RVodhdEHQB==*PHq7_Dhq zOTrPqNWn+PW8zm(+6-Cd@l;>3ow&;Dc#dp?ne$ukhSFA8omsAoBVb|ed0Nk?)g4M_7NAp9?dEMo~`Z&f!@XO7#f54z11PLfNi{yvB1Of2#^^Od0$BA!ioKH+$y<1%>V#(PEw-8$B}3(?G{`$S(Ap}#ya=T8{H zg>Fk!{Oln$Jv}vxSHQa?W?v{NRWf=%owCw=@UWQh%_S7r7QamKnLL$2BljxCZ0WnB z$EkouKD?Kvr08{cc&xw%*1*@d0(pCqbRO;D6WX`L$#RYv&7V+oT2 zQA0{+SM7AI(PWB}LDtqlcs#*qe-@nCuN36BwK7Ey`5L&te3AJ<+2>{M9#p$^K|HvG z$=Z(edqVZamV=&|hiQ?@`>&n}dN=(#Q%4T(d_8J0^RWuP(^`+rDX`RNVHDOJJbpLq zEw#jlX<_Zt*9`g{jGZq?jANpP5zO9>(j!N-F!<~CpO3+O=wj$#RX^lnOK*~9ESrLP z%VclipEZkTTONxKslvQ)yhQza76b2{szteJ?IR2^ z$(EvEDK?+Lk>ib0t~d8;I?$Fd#x;&E{K#+1@fBeHPVv-hzm@S2>$#v#U0jng>d%*{ z9jx3TR969xo)S##Ge6&+y#FL##HEL9Hq312h3CPWaSb0ly~D#N36}r|e550=Itq9! z57-UzGn?hrt)7v%4Pe&fLzxAz8z7h2#W!&aAX~BJXRPh|dtGiUZ&>$khk;%DlhS~U zxLJNKI5wU$@qFVJB zhZAmghULW@xmUmfdO*+c&}NLPL`%`pp|opCfSFMObz(o}TNbH)^P&hH$5-GI83{>P z`5Cx5{G^4LxStr0)1uT{BE|SmhKBzD$@!V3o{k8m&U(No@A=>OT{j-syVxsrh~S+(lcg?1eXO&A6zb$ zde%0iMAry~K=zDk)l(@Bwx+_f^dI8q-}Q|pAN$#yrFe0Nt?OpIi8KAYx!-4hkDvH~qO8WJd6_)x#-uyq< zd+$Il`@e6vp@oPjMWJbB7o~_&QpqMnk(o`DnXHVGv!XInl7u8>WY5Y-rLsq4WF>pv zuS0YS=XGD>cU{kYKhM)2XXm%?@jX7r@fq*^e!ak;z42pZ>*IS9ud^tJ-P}8!oWgwI z;LRMAB+~Gi?0CV+s+?1@zkgk|A>n?p==F1_-JA6_^DkD#R#*Q_vY)vL{wfFxYcdyw z-LoPvXliqsRw@Ws#W-wGu%)l~`Z(`STgSC;B_V@h%`@=4L%UhR3$|JHM znb8hDmdT>!^De4XiXv62Y}#6qCV?m4nD%{5Bg9d<@lg2i+Zl7r+N!A&5w+c@^JZaR zy`j?7V}l4HKR+LJR|K6Pc)Zic`D{bUmA=QUc6j(wUMuf|YSmY^M-0kQ=dDklW*U7_ zrc^CA^OaulK>k+HdUrqHapSdRlgg6G$4XFl8)z^8*_};^&bFjj@SOJ5IaBrz4CL?I zJ)|wz9$!yq5#HwKDzfa!-Y>m$7Mo8zzP4wXXmOwVz=WZCkJ0s}zp+*R(%q&TF@k58 ztG@kf(!$of?RHwcjG#C~z!mE?j?x4aGiHNI0Hd%!Ju(-*`iR|I^|DXjeQI7F$fKdgFu`##uo89JW$u2p#cL2(7Wyrjvh+g~ak z^ZhYZ-->rxB&iAO)V5xmk{L4W+Jh+2OYePu6h*66z|C>-@xFFV9WWZ@hW)@o@*Y(x z-rXE{0vHnsK82r@DrFU$938&&N8J*5WcNhi^sA)JGl#IbBy?{F4QUK@eo%SxhuqK4 z%0rE&PSQuob;7`d0FqzN7cHOYlkpFS>o8wSmo4i;dZH!$VeA2Ig~4X`wJ);QeQOi% zl&swG%va&_%w5>$y$3F#ZeSA%voZB9xBa zpST<2R}bV7`Sj)K#o0%hc}&&S}> z_5&m=y1_8`*`& z$5JJD---ZOda6$?QVH&&hS!LEsF0|_-KlM)_y!O{?nK${!@LI~eUT^Zn}upUMi%QT zDoS$bJe#3b+KghTBRq3^s&8xd2Pxk9{d?2IQ=bscM<`xtM54n_<-?W<;cN-v8KPnf zyJR?%KN)_;snlS(r{Jtg%EA@D&d7yxTa}!VDi{F@xmzt;jmnBES~JA{S+A%ZSFt4O z{1U0=`0{%=1e0%$jcHex;e@mZ3W$j^*$;prK@G-3)T$|_{5bY22=QCDoou{oI@)fj z(RBZE45co8@b44!6tI$=_4V~}aXbTkZDmoYqAo*ZVBqYpK-O=Cf6VJ{P)w)5cQ&&< zLB(fzTg#`k31V{X-abBzLRN+-cRO!j3g-}B`)?hz{T9mTx0<2k?ooVp@ygX?cCZ>gMnu2&T`bAIE}w|{o8z^ z1M-hM9y$ms=|n9eZq4_dokh2;E_~6&zbd~cZnBYU=8erh=tOLms+Egm3Ru}N5VW*G zS$NGazyI$U;g8xQG?sn4yfu_p=v12YUaeYvrfW@FQz3}oHjLQ!EP5KkCO6J%g&(7L zeeNZf|38h_}l!jJe7Y%4E{FNn3SYSD-uNGl+&acM1+Fq*S|T-%c>JW zl{@$GMU5prAz=~mPTm$M6>(}Se9xoB?D@A!Utu1{YJNPCSEza|pHK(ek5%y>9zC_=AROwMdiZ+4PVxGco-ZfE z2K$Q@R?}O8A(W62!HLp=MF!*HEo^f)BT14~ zc)`wk&A2L0t^Obib~Dx+M|gxfMCQBlMFh($X$C$a^aM#pe1P9o)c(9=zL(f@GYtDN z?KRgnW9h>7=OOc66SV2G`*!GHolzZS4}?-j^5)q=F+!}5)mS>c?gUBUsp#i>D$a+G z19!B%GR7|IkyHSfteYdk9EnT#0dAIDgfb2lp~7mdl)kRXR0K@_}q zE|s@79+C3g<1^<%eO6ESMwJ3=0xT4_gQWg?5dP@KoJ~y~5T+mh;ls!wydH5c5(z`k zJ(uR(tDY53)8T=YpIz{hms}zqMbEj@sDwo+&ID~RL!dXP+|qq;x-BPGm#q+M{oSQ|l zvMI8e!RFD|i8$Y9;JJS8C6%mfw)it5mxQZtwWEf@%ZIaz`te$~b^Rro!lYwS|*YchZ@6Em!-|eu9^46C1s}&*m z=ykZo6(+mYX9_SKIRpwQX-Nr#y--S^*w7QS2YWSVs2pTb4@8vf2|o)sH$uQ367iRc$Nh8fU`1zeP`@dnYLP&2%1bCd8o3^GB0s zRrVHM?5u2|l#=pv;`6uiAh4=++uAK_y=y7i%_>IQpU*pB`SGa`NMVVT>U=jbP>YnG zb_XT*a&w>V7K@wrPC7}FND#ls>k{gCW4i5zS#F|5Y-HuU6OoXo(4ZODqGHAa{k!wC zyHVl%V^ibE2D3;+$`5_YNp*16{GA8=9lb!pluf5t>I4^5erG9^!XzTXA9`i8xaZ^e z4>Zx(-9{^V(iZXpaY33Z#&+}eB0lF5!~8HhHXib+N$_2%dR3k zAu?fus8Nnmj0PH?%Coaa`b0mwZ~>XAU@GD9+nT|4_;@IWdM&BwXxfK;$T92|N7@mU zGMxZl6S8cPRvQU&n`I_p^t`}*-#S{^dgXOZo0cog+oLDOX6Gtnlq%Qemly@ z;XblxfGlQvsYz-g9oTPbRe_WgM|OaKi38Bjhet*nd+#$QnJ7I`HUmyr(yE+&z*7Ii zFvEyaFzPy^hW0$kIiNoEivOi#-*5Y!cs_RF;ZLd-8W*)of0m?ZjD!Vq>lQO8=^_J_ zES3JuPg$z8UHeNy6?&dxhdc6x`{P1;?$o0(Kph25_0F+r!)*zl)kng)>|5z;2M;we zX5B)fcRZ4I=IWnPWQ#6o>OqivgqN7+c>j91g4IA4D>ESqqhK0afBQfTiDI4(V7&BD z0up}KOtP$y441t(8CiLL4hh~}aq%Dg}1CQ6F;g%oY=(=U!Ya#l-qBO<~2CuLe zqH;Q6#0nKwz7wQ#ci@UfkfKZ)sXNV_8(^joyO3Vbs4DRXTo&};si5}}Ehk;Tha-p+ z;ho}-?>xJOB`&L6U&*6-ZGulow0aD!ZoKFsoroO~a3cuzB|~(H-O&wR$H+*@_ENC| ztALzm!&Juj580D-JRs;=%PGRzsVbX1O>yMzXqy1%{0ky(^fyx<4*D7D+=x3VRHh){?fo`DNB7uB2^h9Db^VQrr-XlQP>{ z$LVVn`0Qzs#N=RQ^Q+{;=pU~8NJdz@+tMY%Ws!7y1RwcVp5PBfc-Ye>-x~rqGQEo} z?t6AQJl=UlWt-@Y_z(6n>8)&LDz?ONoTGXrWMZM>T*a6}O8f~2GI0K<|5&NEnsbd; z4DDJNc(OI2j?Gh?%{yLgns1Tqqe--s*A4U-DL$R8`|Y%&9YyWfA;) zlsrx`8ef?I*?qO^+n-0}#EKg~Yl0KFfgiT@Wb{7!Pr0-lE;6Yr(=R*r{QUQ#+Lnkl zTpB}5xHr20aLq%DXu(O3nL;&~%coqxD4EJ+f6O|?=Iy&OVfkN;tH#P;!Eb#7=R zO12MoBa~u+<7_>eyA?cHHFc&_h}FL^k=9eIv(zM)*(YV#3-k$~H-b-4@b;ZOLPjKW zue~cBnM2kyB#X_6{LCR~KK!#%6XTxs{_Z6nzv2886WVtX~ zoU?yH(ND7jTd=C24G%6%d6;sNs96&Az$aHtm`R!Szd-t@kberw9_m%JTOO2YkDm87 z0e8aMwyBDEm!Cb?Aoe3;f8Efh5A%!`q)};HHVcQ2M9ftIUr_ zuzV9U(aETVY@%jOE(#U+z_JHsw ze7t*eeNIHMscqdT`NiFn705~MQ{)0uSXB|X!rE5e@%%Aj7|zDXwvmobLV_K<=CjuG za-fYTv7&-F?rdK__^Pox>{w{cHqV7>BeFE=EX9Yfo(PE!jg)_vsyN?#SVgcfglt^5 zF48EMiuO7Eyl=bVs7!~u=XZs76ffpm>GRCuJl1T%#R-}NC$`CT=5-|Tt)6FzlJA5q zRW)11@u(^m_5f3dXWYhlw$0W};_(R$a2m4$__TS~-I{!8>Nx3ok*R1Ktbpa_uO(%^ zcpg4<`EK=#;jm!kRPDHton>vSc4AnGPhzewr*aiUDO)$vNb7Yz_Jz>DXh>bWc(tl*3Ct;zE%x`aR{sG@E-4fF-qVZreDJPP z6Z%AP;#Ir#?rnNK^Oi%m!@Aji6ck9LeY>BlKsC0DBd`9{T?67IGp=Q$p3W!{kotk2 zSq#1gDYUH^U92YO#=FU@&+h|^04fgdxO#TZ z1U>&8z zk8cKe#72603yoNngf_tTws&>;jI5q(DG(1+r0#}p%rtaV*N%P@s2%Tr8Kq5BH>)Bc zfQCKbT1@o3_618z%M-akj&xk`zGQmIG7UGrMcD=i+=yv$fp(@8%;OE)_-k_|RLnCeU?zxz9kR>&S)V(p4<2r}_ z0fJyzZi3XN2Dk>X;B_GCh$nf6rcvmdXpY~kN4ejmq259m>J{4~Z=Q)EKTyzfA{yvh zGvM|oubsBQ4^LU(FNZsx^MYTzKENdH4v)U&?YY}QFN{1y(5#_ZWa46<%x80v(Rxay z^dLpI?2c!EJBTx`PVQb&zd1qG;P`?0x6`c>a-o=|B|$<1=#pQV-t-IOU~Ds`zuW^?ezSKy*_^^-aI3lG*gYmE2!cYa25m3_+r z#qPO(`73G!^`k(RR>NyrfCWZ@^MY$HAen%`&jW#zr_rn-jr6~H(MQZz%uLi<#J z{}X`a>FMeAjvH)By1-jwxNAgXqEDl9RaaVSOPCiy6$1Qh%aQY$S`eRmfN_9w+mtb} zIy8P$?Cs82Y?9jNH!{3BWH3h{edzeWr%xSgmrQ_{Bwm`urRyA^j(!05B{D~^3Meqb z0MkzSa1W)k$-_($ReEmMLO1wg-i}h^bp8wa00<;7THrq%Sa&F%0zEQ6iv%cFt{m&A9q&NY!TR$l>#^Pl z5qkg{B`$Y9 z=R{JavfHv{%c>pIK;7n23)IxwF(n6d#>+*@2aF%;yVjOowotut1pF;>Nq zbPk{stR8Uv7~HygC?HX2!);2b1RnEH3;!=Mi(*5LkEBnGpn@E*!3jc6rYH9xfMuONQZ3Jx z0Mw5Dn}_17m{~$f+%+aj8OMBuT1N--MQX~FNJ5T95tFo}@9on_Ipa6nqo$1ozGw=x zBS;xOJC1!u9PMQ#*D~SVKovNhyTQ7uJ}sR5{t~q57k*CbPaxI@EKKB;T__#tDyB6! zyxQrI9k?1qfO#cEikWyt)T*gIe0i;P2Y^{eQus>1U7(rGKvwravKWsCq_#q0@ph)B|!TL5-qeZ z62ZkbXM(6eWFV=;cN0J7XylvcUKcQHj$>5;ShZWaq(Cy`o&NF>;bPiN@_B zL+{*$d+RA36TP^|9e{07L_qC~EhvL!m1NUr(0@)`E-U2m=vR^P`>2%Pj1c$u4UwVM z{$T`m8ze&7?UP!9O^4!j9u~B-TGOMkWLY=*SDGIN7z3Znf0t3kbIaEEZDLDw`arZ{ zgT_DfzX!bFAgUrxISe&b+bDEz2w8XN+&G5FPQ71kmcM~e5$N4)E#6rRdKAXdx@Ca! zQGcni-tHNCC&4R_c>i6q@IaD4t(^rBG|;{Y>XC@%l91XC|D_^@Pl%m&@`LQzp_?gJ zwZD|3uLpteFJEqtfSsZYG0p}~8oeqx5Eu|=j*oHTZTjd@;ib-gGH#{3YlBVfzpHko zjl_)D0A?Ga78};`r3TI3pP+l-^nVK)Oh-Gry^q?rb<~vlacR_ad|qO5kRs9i&DDp; zj`}vqb@uz!hw_GAfR(v~Y|T>;7^ID$M#%w0EbE{GBGzI z;62n4CA)(zamQyBGH|H_?*Qwq#Oe;*vgih>7cn5$=0{ z3D=2VuskehdB#$E@;X@oCLVs`MzZLkoPNV!-|+0lS3JK4l;sh`2j1xq#E z{^<`p@qw5*JftiSKRytM`ja2{@PWXdoczFx4@9@=Ay~SGOsk+^za9i1PeG>{xmrM99`P8sKG)33V_?y@@(U|nMvA+AZcDwdW zScY1BPPZ-)Wjg!t+GXp!-K7Jn12w1mO>OR#To!TZxovm%^z!DtZ%!OoSjte&dXj^e zE@Suj>)2A+szV81pGdEDXC;d!e_tY<$j$ZBTQFnb>DPr^G*qr*r#IyAZZ2h_y0+); zotD-pF7f5m^qOV6+V?)2C|B9??18k@-9n2VEuKV$E{~JBJ2t%i_O!)VS;M(}f2_09 zl4>#?`r7PW%l48!y!pJ`VDSD~mE@}LQuJ3Z`;IL3?29i;meSb*Ht%eTx1Z9iw+>#t zTE1T?bBk4tj>X!c`USPwYZmKNCGfn_Y--#PCGj|f-ed_McT{3{<(nYeyCs?XpP7s8 zc%lAz*KNOjvEL&{mIzL)7Mwt-&iMgEV87&-~gW6X5 zTH3OQ#Ynl7^v_PN8kjvKwDooMESUtb&BgRBEu^*0511I4niy*vTQUjGzo@tqvAEJ_ z%#EhEx%OF{3=Z8rDZe5*}%>Db0aOB*+x-p{>g zDaq#l$-jJzDjzm4sJrp~`*+69*Q9DEkZ%%kY<1!qYm+;e(RVQ8-7k2QNzfeoNfaH% zNu-?r%0Htg<)oS#|30#ULm7Geh8Y`u@_T9SdPymg7#`catdPEAeUVZC0r;Ry!m)N? z_NLQG%jwoEd@G*ubh$F;BDW^<497A4>#mnh-5)xNp0yQpx0Zj|N#Z?xuHq6Ht8Hy; zY;0}QtACD;zKV)EIuWvxin}dVe{p9^%k7|`YLGrzm4IQ&!^0yrbzhOL<<7c?_8JpQ zI5l@NZC+?WtFmR0@qQBTkiTf3wxpXr5)1_K0|=GWIIV4MzxUa=ySY(&*uAHzsnN!d)jqU7FKM<|YPP6=yfJ@3F`TL*`<@k9R{c{xiK{=N zn)hdzW>3&`=jP_NwY9x|?Xjm%A*v-KBcr^$+<&17z}7i9UWu5Ro6C;L+o~w6jl1UK zbNKM#AW;Kvje!Fr3K=Hl6#c&Q$qqu6|EFZfA6Ntb4Z`D}p$2$)1ZU$AYv?KVEzbY3 z+s)|Namh^&yks97S<53WFDZ8*T=TdUvn8{5SU3%{q^CfIf+HvOy;Cjw$+EmUhYxs= ziSFRrA;Wv+g|W^fEdjC{()97!U77A}Zh&7bUAh#=KxUuA-LBQQKLa+8uGP1g zbKjD<7SIyU#D-dP5m2>6#K-fgz9?rrH;L<1QCUe~#J+dBr!@!)u%a}A*bk^`w^RSJ zf0>r%0I))o|LP1~4(AEnFoLKtnq~t}?NUtwU)hEY8vs`9I6ksSnhMjw6pQ{;4)s@w z_wLoB+}r`K;mF8H{Du+KZwm+rfS4uCQm2KEb7eKkoj;b95s}`6ganKr1adIc=%V=g z-ut2VcL3Bv1Ps6=S1uv~emuY$>Z4+s09|#|Siq)<1!zpL>ZkyUU3<*Ll4P)tU>;t+ z96&JP(RyR@k^q4&@ZOhK6{QJrb6-z&fS+tg%>|dJt(cDBOqUAUqoAP?l4=e>lEWJ> zHX=X(@xg8{7}%V)Kt2bPTo8p>qJIRRl5tp5e{3Tn@!*JmjGGhmmGI1)eiD>#s;ZSl zr!`z;rRsIr)#3=!)z#JIYs+O*QJZivaomDjBgtGF z5yRxw8H&ebwR9XQnzpFVu}@VUcbx@_#r!>#U4N8QHGUTEa`drrbJ%b4p?594yg zO`f$1WzmfTZ4UzaZL|B>oqDP~)8gs9;Ezg20rQkj14G_P^CJlc(-b`0~aeC|Y zf$}Z})HQgU-hB6sHZ9RFYYYqx?cnSYjDCO-S6nIJhL}-@VL1U9RBjCxBou!qpd7qs z=g-kX)8MJM4CO6nNrhL4gS8j!-M%L%HYAsaj$ywOXB&LUacNu&CSGGd(XnScw2@_; zJ&nEVO{+zB`aO>Gf;>+ia&G-~kRe*c+ydSPDlIO~EUcDn!NeazWnBbM)yI$g6xNU} zm-gnFkP``lqhl3$bZ`stnZ@OxAT1SVlc)C?(#=iFZe5xDJUR1O;cC7- z$Id($4W;(|V98C&)$72W4_UY3n zc+|S^>Nhj+xvlW;ah5zNwkgo_ikO6p996w+42vE)tQ%zP?#0H&LX^X|&zxueQlt4ERszz1j$3XhI90@_C&_gALL7vtZ39HHj0 z+C>Yl`;|KKqOwt1Ny$IDAy6yvy(+4qjA%@}S=%v=Ig5eb$* zNJG5ua({DieCa#+{_Eu~>X^n^bv>Q;%gvih>R7oixyw??EGK4E?G5d^Y??!p@2bx+ zT1g$ONuPLZp5=Uy=oXYIcYd+|j8zrOH}nQuA3@goFN{vatRndVqiZKjohB?h>5spB=xV(8ri>W~(B>vXAQh z;^#-AujztcXxB+xE(4evnBPoZ(afq{POV#B>{;2pi4C2|&WmDu_= zYrYDC7IQpXqqm@-016YuML(h9h%5tm=o`S}5Q7vf%6quf5=|?f6TrFzwL!q<6T6#k zZawNpu;ylb0c3J_Azw)qu;0H#6#XCwk-r?qo=rjjSilW_UE!ReWzBN0Di9wWCa~Wd zGAEk4`f9h=U3pn!6LrKAQ8=dC|d3W_pEN9S>9uXD5!`u z)@78)jrXoQIK!*1AxFFc~r9eJJO;tNYP; zg9TG5fV+sjET>_VU`m;RUTY^v+0=9VL=Qopggu9xN-$0FnKBlO%mHr3HBl-vH__8)=zO99%}%G)g) zTl#E*q_*!P+vaclQmB;Ks*YTU`@HeJ{}g%Vf1Gafzxfx{E{l!*s?P+y^S%H1`}sXV zy&SjIlB+upx_@!vYtQ(Y->NGR;wuzPg=R37dw1SJ@#{}<@@;4=B-vKt2bya zD}K6Iu6fO#Lm3}e2>xIHpW}jcI^`Qat=O>JCRIJ+!;zB9ufr=lq%5y&5X`1H7Npwp z-+FNV`;O$SJO7JM=r+sNGiv`DZrask7eMIaetuWR!E&eiD@>>LzzXcOf=&e#!J3QQ z)~x1!4yNE-67n6O4gUR=0%7m*iY!&H_jz7my zMWs^`Y!4ULM@TwjE67d2d`d7$qq>P_uoY<W{Ptze0yc}rh49EPiE1V2m7>%l_};tex+z6QO_Ah=e>4PdB<|9> z&&-OYAiB1M0!o8lqs0YJznmiiY1;lUFU~e0k4qERkWlAPep*Ll?g$7lWTMuZ{%m<~ zw-5@`Ano?{5Ifln@9(FhS}?{Pc%eiW0r(HbTaS>8TJuj>jl&g2p4lkPIr9?1YeSUY z!wD-2S@i8R#S2fH)EXp%`=NPvy)=RPbFd(*ew7YI3E~OL;DZ@kofis}umPA<3mEm_ ztik^s$QX@GQ+STPRw&((zm9V1*O+{IkD^2iF8fa>b&XKO)@@z7h_f}Nx2ULuU@_L0M!N;Fio#CEG=&GhXf1?A`YJq3RzW) z47c1TbgK17KAl3njUFFvX#drwDh{TbdaUZ3TkIx@^=^yvi5&qKOe4WSp;mdJfA6Vb zFh^GCMNV50{CF9)N3;3}=@>%??d z;S4@*$J{!$3Ya_a$YCQ{VI!b##}+;XRtIR^Z_)TXoJX2?FyrIZ6wkZ49{?0__;AGN z#Dsw|))YzleS(S^+(&l$XchhJ{(*|P3jK%;?R5W+lk#sIqW{oWaT5!#oc1u7&z>f7 zT>mM1`j3buui(Glp#JBOB@g#*et|!7VI9?udS!hZ^M7dfqRmQus$&Y=!eJ)@0({1_qCm)M=;w`J9cOE*M_*7Xh;LRqnLg^!$%?>1f*?&DN zjPZ0>ft6L4KSf|se0+jMO_xRSfJIG7K4V<;hLSUTMKns@jwQFE5Q?1(Gl@!EzmJl>m@7Qwy2`@9ijnxS zF3E+DF8dNWv*Gg|gMVHKZh^xzvUND?*pk5#4tcfe0 zp_c(e2|OG%1T%|&%|c!t^+rmz%cNqL8E8?=D(-zoFuO<8b(7fo#YZm$K3dMUe1*9@ z@FfvottWrG42|si5-B8jYSm|6^I+kC@Cqas8oFZtRy4DgW{s%;^NXyHy#whVp|lt z<Ub*`i>5)YRV^1 zogtMUZ@>Tr9MpHHH$0p|Z$h3wT=4ku zW6jhR?*o*ovs}sK2+8Bje9`}V_uCqAVOO6mMs!b>d+eV&!Fqb+Xxw@J>eVa6qXzTC zLPJlZYz1NQ%_d3El->SEnj0JKfS>}5+KnB?gM~noort2PIZ5Vmf^7Tc1kdYC(_5*5 zAsbX}J+M7gm6QewHv1rAP~Y*gwi7}A_I3s%Nv#h22(qxUvND;j>2*EknnNeGN{*J#1kh-7q)S{JkqABsl6_ zcx9=muTC%EO07hMmXni{A3oGUtvm7%w{G16Ee?gz*RT0L2stlX_UHRwbSBdfTQ0ya}4iUV;rYC@z<-?N(88hgQljeoQP)wKSEa%@|uVy zCcJpTsq(zQmOxRdS1q)Mgkod4u7S%--mEh1xmGyd}>x?Bt{6N2YfY%Nsf=l00k=h+WAM z%hS|PaS7(xLp@)Cp~Up%sARon4`oCQC0!x!@^Q`t z`Vd3i+DOaXG-s&I7|J00u(-tT*+cO!$57__(p00DW)HQJ7)ouuJc-D(fipdU=KG+Q z7!%d0>}Koyf>u0)aJ{8?H|4C6Hbp=)jktWE%@mpacvUMswze~NDw1f-o<4JdEqPBJ z6IH3~X3u#+DF#Yw&=c>bpFPlFVj$1^w3#Ne2T~))KmweKB#hION25)?%sCS-SJSs| zQXo`lr>CT#2o`ayv7NZXrz>@v`;7N*{kWF6?^Q3_O!w*g{N-v=QVbpsMvkj_FJt|7I-M$aM?a%#i5^gp~C1@&cMPlOy4A0YOOCJ zRgagaiO|n*>)s1Q41Iu>cMZ4g{V>h{yoKz8#1rAnA?V=l!u``oes$LM9BBh*P-c zM7Y|&z2{k{`&(D8%Lcb66m>GZYY>L6RY^ylT?6iBH&@)Xj48Vy#wCZkpHGSHa>=ww zw0?c|J;qi0g1K%GDN@jEXh78>wCr9>cYc>U&y;RsXXbGmE4<}dXE8mVTb^l4zINH? z&;EXX6pn8TN(TD-`t~8hfg*S*Gc&WntAtup~dyYG?%eVh_(c$LTv550*(n zJr%uqV_6Zr)mSE-JU>LH9$euQoJRp{Zn-7Y2i%id^fOmaN zObm)^kf8-#M_+%xqM~AP@IImER|yisnwpKvNqTyEUMZnv&!Ve(m*JPEXtP2@y{xiu zEtmRU*kZ6IW@y9-Jmgr(1a0Pu-w)||;bGJ9T4dBgjnLJUm6Zj4Qa6V)vWE}ngA_(Z zRdwwS6&jlpCiLkeohjINuLo_v+jQLt2oS3HZ5d1u=QU)4{qpMRyqFngd93}pO6SKM^kD4TX8eJP27^4)1O#iyrv+KCh<$3P5>iX>~N_vi07 z+JS+*Mir?TrVs73zuNc`19A4KiknUwUcU#56~jPplxQ};noSfl~u!C>go5*5v)>zej=UQ=RxhZ*CC74Wp%bF8PQU$%7VD}T^k zKSe(yNZsQmt{^Drh?XW8&|0r3r&I#uT>gx=#mP-cPJTSE63pum+Or2;1VjrAubZzP zp;5NtxS6Un%$OLscc}X#BB(}2D^n}V%ahP3fD%jix{n0XOzSTT1c1QE2{g^zQ}>wl z2=G5pwA9?(Y=YKa>U_rhT^DsMNndGHrXvGI0yoH8+L01#}N(HCEr{+X(_{@ zthdaa@iw>>K_`FfR>Je=G_FVOei>RRL_7TUTpv9Pe{rXMDRzykETYnH& zn}Fv6#hi9JbVp7PX+(#z=rjM&ujkL3@rb#6Zn39QUcgDiA9Uqhh{e=05K}vMS_p0v zux0Jl*w&G1(_JTOvf#vRZsxCDv}35W+9egr~U zq?J`y?_62u>-Y&x7(08&9v_%eI*>8DXB?XrH;rhbdjy8Uw{PFxMcp4W2tUm&C@4th z&WyJY+|8@;!s39IA_Wf}qo~p7kg;S?a$;ri|eGt0p&6_vn@w)CW zegN#Iva)iq(|&bz^&G~`0rJheiFKWIFp^mtJnjnE2i(1ja=};rWMpIr6QlaJHMOd= zG!6~FC`Ca}qxdM5_Zc9(BO2mC;jz_l;fy0kv)hete4MQ%I5;>kFz_9@V&;^U0OQbX z_us)XnLN`?Q%FOGC5xO!M@L~r`XboY0nTT{!^FkKLBjVqG7>4R zLbQ#-ocucgG)M68sHv&p)jr}_zl#bA3IZK@duJztlN%iZ#>X8@wh+k5^*$9uEebp* z+b%Ul#lLT-H;P&0SwG_;5vBusV0A#^-%-YvzoU%qwcns-^RI0PfbR*Tx@AY^X;U!v z)hvs9{Fv>xU@H~GuG1o)aXg?9`umOM3o~j>u!2kx1-|D@OdOH$yr1SQ;HG7N_I`)K z?WV2C0M?QhAr&+UoOYhPJ?sveQqI^|{QPbJCsTok_vzEFQ7sv1>8Gf@adUIy8NUK9 z$i1kZ}P-fHTQHF;lVp7&Ndl<7Plh4wEJ~^$i_L>=#kp zo$9Nmps=%;5(Vp%dhIF|w*S|h8A|kTtHr=TF=AL!SI@j~ zM)>a^gx*zzQM{FBih$|JAE6emES|BB{~Nut5(cFA;5>RH%oGYE=juH2#}()Pz~yW$G2>+XZxmV1T8Tdl-zoY1{He>mdB%{(KfBHyx=}exdghLp66#{v{-z+i zWfr|M_a|F632#3}I6AYPfE=SnrNgU<#L4XB*bk3k`>aXtV^%&*oZ--j$H{rWd6ISX zLgUtJL}YKa6LRJCmvnKD`M#k`cO0F6q+G$Vuhhj%Tl7NbJoo7RdOks|-mQV667S~b z*61n+{Y{rLi_T3fYVuR@34RUn`-g8I_~Nb%$faHnb()h-qhh{xV!8XIgExD!m}Nxh z1F7E5e%LMRvolY3gK`dq;y1P(Uq7;M>cH?iADC3hzw@gNjM+7ZqftzL`gDfvMYISY zr1Pe*kf9^!*Ds3J8!gt&**WKD4B4o&U##_G~C?Zf}T2)X|Vq##hzGLjyPmizD zTHt;p?ZIS7^Y^l_6#t{Hxi&ohD{!cj6 zoE-TnF9$>`7sUOH;cIhzXuU)=(e&OhBbnTceF^VjhGw|0_**lyAR)GtTi9dV-o$TW zXtm0W-=LG`!69>p*FzG1v0zmCdsngqPp6LZd$r$>F4!@dojCZ%5uqueqZmtkf@IG7 z_wVaX){SpYn6{9q2;cj+zbg}%uA+~Y2q;82TKANO$|NNgP3D)0eDmLHJiS;uckTrE zYFkG~^phun`)sW)(EC7eO(rw{JXU7c&iKq@{qL?2n#zgIdXCZQ^ozN7>h_ zuZ2zdHGkgWrfsyz$lJ^uu;|pp{UJT3r}_8$!B|{6l{@}DHH18zKE1y;5Fz!Z=VhjF zah{se-PWh$d2ky_8d zu-9??)qqkZkRi7pyM4XQeX)}3xcb}T;xwSK$c`!3ZR}#}1yF@hL*&jANmMr;(XO+5 z6ZC~=6o_X0yLM5KlV9NCQhs_GF?cwo9w|Z(LZXyn!9NiIDvaLXOE$4O+S*3Inn5@t z(m~Di3sT@ZLUp z)Ph7UP9KndcB6d_xk=fvcXbbbr9G={zDIkX$!Q~?k(FZAQEo*vxgiRJ?mbi=^rP3W z;Rg2=qKgaw8;D0s9zBY_P&PKUZBl_>%FFMx)vZXJdsJ<*7sqDD# zBTSlV19}aV8#V!H*D8&L>c@{~`uh4qsMZHHY59i_e0F)=H|f9ftBCuBm~S_jbQnf% z8&3zME-*GB;f!=T@LG7O2Cnp4U>70MRD?YPLEh_!(8#$@99^M`3JT%lY5y*4Ey!V+TB|dI1~VgkSXy9*KvQpQ6kjsq_B+ zegHLbW0-6D#>O@2juR-ptRM5|@m6*nmqnzi7JZz_KoL<(YS`!W=*g4Dl%7@WvEiPc zp6IcmS+iyw$TKxJ!q9A{H_%y)joU3`(tyoA3Mi4D{=)8c*>c9-wR*Hy&o;AKhvjvn zMeZ$1H!3RJCQN-VU%rf*KLLLJEe#ue`?1k}BGZG~gb)*F(I-e-q}UCao4m$}%TIa6 z=SCGZl@aRwKThl&*IuHM!U2tW?rd4J`W^$JqQJC;&X#i;h%3m^BU@TSBq zn#7u!(bHkrc_ay~X1# z$gs!q(c{N$X(OFPZKdAWM$o(Cs_b-rDmowJV}=0g932&9+vs~3JFXiuG0@j{-pcBk zJC78sf~!1k(ff#HpcwmjJWXdY8wG=4H&RM?K}XEhnrmAvFMbXLeY?$K!RKdQHLoPP zK`ktP0)k|>R-U)K$ezUdUj-Ja0;hw~W6@22G1)^oNW{?rT}|2PcYeb2>Iwb}Nxc`z z$$JufR>oYp&9k@Z5W^w*?4^_Y(>={PJuNNm`E$!b)xpD-IdAB9)HvCJz4e80_1(7o zMcZ%r``a$Q&{!E_)5fYpo);M#+XYM?JG4r8dVz2^P{=xFp#6p?x5GV01&_fk!2;e9 z$L{o?M=`wHDU+Fs&n=?r%7KqOhCf-Y7op7JS8c~DN)-M$6bm)HtEuTcRTTC3ah6Ei zhARr(bmU`8hpq$L<}555qW{%p#cE@SKpJ80#N5hMh!Il9#bsD!S!Lydl5^t6#3mm1 zfC*_Yr1HI&YCwWQ@>N|Ny)P;TjPz5N>MZOla!=w!Q75(U$d4tl0Xm{tke z4_hK_3?X3y>jf<}-{B_;nbu`s_sKypy7^Adg~Gu}$zV#ZJ33b0|7Vh{N0o)!VcIQB zvxjJ~qbG03qdQdf>H>jg1$*!USIKRj?2)58&{bSUNqP-l_G5udseRPLp0^N@M5-7^ zMqY$dQ0qCDX%mh*SJ|U@S!(_70d%*&W|F8W$PB|8~ooT+(+?iNC|5pH!@bdF< z|5=tBWP9wy1YCz1Lu;iKJwNq+%X`u+3nEQvgbTvvK(9P({8PPs%D|z8Ft3o z*U;Oe>TdM&tq(j(pNl2cQ3=`HV@71ReWxXR%_--pB5{mBRzIy-2tZ+JX$Q=%sS+_$on5I z(yES9;!4$Gav4S5U;U%SQJvE}T9M-)JF$;>ss1~KgH8!9i&lTVmys;$VDrnlwZ8EfR9Q0}mC<3`kq-R0M_yH2W`@i8>R;HjY#n(_sqz7Duk zj3qrr%>}$gsGtzH>Ql*<`fqNs!sbHn#*=L;*E2pCCBJ#=R&GuXicSvS_`>hLD9UNk zxi3L0NEtS6>}+dG)+^ogD(}R>!TRiF$2C7NkUm*T`s9rDHkPLhq=sd5B)h(+8$ar+ z-(QC;3)JYt@bI#aANPN2Y;5E_m8?8gp9g087bdmp%F17!oa7S`5xIGH!ye03Q94C; zX&D&oM*59tLKGAf=o%oJEpo0>?^QKLQuCDLv?@%@TgmwjBpk>KEx$5Pmnqn2Ltg522e$5&;x@Yn6;p<84 zna%la2fNQ82G}cnI^)v)!#NbB$tEucCvSv9}~n?%K8Mf-S0MkWSM`vs-s( zWfao2E;(DRTuKl-;Z+tB+Pxc<7F4@e#riJYz`$_*Woepum+W;IkEkm?m1?t-I^oMP z1~MD>FNx<)AHBG_wYBx>hGo|r!bJ~8Qztu4j3ewG#<}VYOs9o^og2EyPl9dcwU+2o z>fzZUYTAb^UUTf6;(+lcxe4B82YMV6=gtUM6(Yvam;M zqNM3Id#$ITyUY9m5Em<~ogfi5*M{-$?~(63MG80V#s%gsvI`e4p6{$0!oI`WiQRI# zUS|;P&~##$8dxD$m!b%q-);%5O!tBqockmD@l=R@V0apC9pT@*+_ey zJmKI_!h=w!-j6k@P4kcyyHvT%=I~zF)Vb%J$Jmy8>j|B2a}hnSr`LkCi^J%k(?j`S zy&bX|GcD|j_jc_Xq)~xw;{;wof>cvnUq4d$QB?}fmP7H0$C~UjPqZf@jgymeuu2m7 zduewx-gqTwhC0B%dYvA5v<_(p7&40&w@B_TPhs|&V4@>re) z{Xc&?E1h5A&BI5%&HO9s?liTu!kb7_-#WXzN9Yhp7UERus5VO@{LseNt9cB>*LjbK z>F9ie(Pw(;L&S;0JRu<=T$CFt&z?OS>OIigOOwuyq?2f;YqFD*Q_mjpwUg(Jz@z~W)~JI z0f}?#giX8{1OdEu-ETTE3{?s^Iz^Sge173r~K3zu^=N=iuRwWJkh_<1CqV2`?%Zl;$oNFQJJ%tg^z zo6NBDMn&XT6NbBG>AK+xf^m=UWf+8JthdrFU-Wijy^OGGH1XWhr&p$&+q-CDC6Q^1gA*DW`thT@E7Q3mNw4cUh6!P0u0zJRN`j#YsrtzMGJ1Azd!p4Qq|AqO}D>a)YE)%9B5at%DK2=Gcxx7y4%-VlJreO#>FRDMOm zwSSBECXP+M*ROvef3U_Q)A;l*%dM?#E3f2gKII=2(0?xy5s}}T#3{W=yqN#Q7eVPrkTQyl zudN~QwhO;@U9AVk)2OR;4z(QBR+ocP;x|`O@22(r4w#cdZux^Nn0=@Iy>SmfQuxSe zWyB;Jr~W;u7$_X%2Y&tJ*T2VD0Ma8{`v2{>x`4VR^o>k>_qklZ!Xwa=g?WoP+osP+ zo3w79u@rtpLyb9{IUy}}(yUu>Ntny^g2*ds%|MoMD;)}zm6$b`_kaE`_TB@Y%l_{h z&Is|#R%Fi*S(OW8e%ifl3x*;}|j2kKP0?$>ku zpX>TR_v?PH*X#VBXaA1lcO2j2J3r(7dA}tehatC+N-;fUvQGelA1c`qUq!}0h9S5C zH(Cn0MG+6)UhR}oO=(7%K*V|L&k+=%*TDZbG_V$P;rspd+2_xtn1)3D_(=Yz-8fU{ zyoQF2FBgXV(NXm$HgFORNpmRMwr0%MBtyO~?LbdCnUc{$t<%;V+?s39Kc;WR1YGlr zuYT+7K#vEG{n1mkjb>c^ZWB4_VM<1%8O(ol&&3l5$A|CaIlP+~J+vyEGL@gi{nv}% z9=3n#m%Fw~jjl)k`R=P33(K9KjSB|#vgUN3ZGKOtfbzx z)(2#XD#v6G(xIK`G0_hHG?*ZyADjay9a{uP0N8_jfAoVZ08HJo- zJ+rhCD!=$uUpm`=XOp;v{2=(c3=v$pP&z#*$pR|_LZ|u$23~NQB1jhT9!j>h<%dTj z9~j&gwZyS8ID3)knfuxVCgb;2^-KdZjemyEBAo9vBsamx1-PS)*C&KI416nKJ%ciP z({J}F(StAm&1aQ*1F2rm3zT`eSvQZl93;5x9)S)T3t-w_OczX0UduD4SFe5t%N9}$ zT`PCNR80`FGn$Qbg<~t;P0g$mYS)8g7+s<*GTAA2$2jfyi=AY(887=+uC(i-%vsql>to@I3^E(FRjy9 zOrTzjP5d7h(F-0Z2?z!{U>OuS zj)K`*c0G%)J~44M;ezRiZ&B`n`_UWolpzw>ek1>E4hNua&!F@*Kx806GW?^03jO}Q z{@Wb=vn!DexK4Mue`MnFtoUg%enuz!vuz~FR84_%3+W11pxz5?>;O+3`2CL0pAGIG zHVeLVBq`J6>^jz&Ir=5yJ$tmfHuf+_itpde{y>DnL4PRcDSc@$wApMN458ee0 z6Oz-$DI~ma9tOA{XUXI1i(%M@^QgyQqVVo9f$vQVH&z{M7iaxPrN`rSl1e@L-tqh0lkdjol_YVkjg8X-{4#l7&pv zrN+dQPzp1CN3aLMA+yP?LDh(Pg46lzi}n$a27{ze9w3bMOW{|rEqOwD?$Os%Z(YZL zS>geov8;U`ME;>(L^%Uk)tVkkI{B-@+A*mPBVm>Wdz9rO5+sH>09ap?9#Y0lTrQ4|uqT2UpqICxgxBxa*Z#R$VH3S=+&v|MD!3U&_Lw2I8*?lWJb+Jgn|=x4v-{$ z39gxHvMds)fuyS27{Vne;%MPq?gRm@?>UX)&r?%ffscNGCHxK4fbtX$MhuP=xu3{A zIY{TgEOEgjgVKD4YEiiQ#D44_kQAucH>Q8n7I-J(?Q1!ZvKhPiYzcAy*H_VmowTVRYgUpKm7cezeDRF}3&+(U$_M`h*jkW%kLFdyP3msRbt>ks>l2XB?65 z3A`c=38HP(Q&o4UznqwdWJrY`--J9uB?3W}NH;_{v*MuSg6jUOM1}9VszYPezQ-7c zT|I;+R3X!zXQ`E4rBIReCA;EQt=&AH>xRWVMktj4TZuQyZuL&DOLwP6$hnroX)gE^ z?Jt>UmcBjD1R!1X>v>3ozFW>w)>Kz-g}DJI5mKNExwvT{+mG#{j&##q95LDWdU)an z{GCimr?Tkx+%9A5LAg3Sp+$zyW5l*Iv?Oc5^2L397y}EZqUAayC51ESFed5K#rsl^ zd`haRgFGz6^itH)T{lMJK>X!&E%Fx&HJuBHLRZ(;%zt(T3T}v zq3Md^P|x;N_;h#FzXr5i;|tnYXN5}fl47*K_CO}3rnts;K#u;p<7d-YQ=1K#@Qp%W zf@ywhm{^#cgcFaHM`cA>Zb5-7oMpe-g`}$?E(H;^CFlTze`IL{3X*BZ&~sd&-E3#? z%>fL*;eo^kXcYvRP65NpD>yA&GejXF)HxXM0|^d9j0K?tTN0-8hTW3BRZz4KC{-#? zat}L_)A2ya5s9RJJ#l^plAteH^-F)u)J>PW0W|O3l#wx4rJz-NQ`5qBR~GR3B4n8d zc=Wr#9?-T($yD&5eF3s$(7G}pI)Q4k0Vd>)fDsLhmYR4TJ6*@rHw=SJL?gs&rpaaf zWe9X)$QnIsj4~&?LO%+Jowo!p zU1~&NVa)YjCcwux(lpT4rd+HVoH#TRC?omf%onj~{wkV;c&ORif3@)>vb@L5`?`FA z7Ht-k#_oAAvK>~)sDaL;9Sd2yCyzsgTP1jI#pVLGhhg4?cWY8VLgV?wCgi%&dL_-B z`?EmX@;N2)9V&z;04nPwmM8i4BN7ZSMKFBLC7{a4AM%RM-#(=Aby7;NfX^g zB)nmnc$C$~*r>smY&iAwFBj<71NPE--TFVN!T;XwDq_Q>X`S#U{Mx4qkS%TPuXKAa z#AtAS{2*O`;GqesTYYAa@pk+uHzJ7 zB%or|F%*;g3}^I}Z;V|xP8MKEza#DVKV!Q8duPv%rEY0o&i9@0t2ZGXNlE$st1Z{5kK>PT~v z()?|47y1ZMv}Y`SZkk0;_I81hhb3}o&)G^^N+EV)-mrJu8yFuZ&I~06mRqp*4KdmB zQ*_LR456^U{eJm5f`CC7!=Ntjv(5MRm&tI#ft$Fop~_hHLG(6oxEQa3G2%Y=wHd18 zt`V^qDx5ugR#ml`IaG`vG~32qpokqQD+hhbjg9g!xo-(FkQuuV_Avl1NF<;HOV|@= z{p-bJyzldrB3Zuz;5-3gc2itkpp${qA5AD4>>Vy$l4Akn*?h zDxDlQ_eXN&@23H%54ldjti0UD%Hre$5?isN3Z92xo=)F(gZ|h@ zIe*3C004cSKYZ8++~|iy9KVVyOO?MT=}mz1QFx}B`HM>!?G)5JAOI1_kyJ_SquhVl z;(+G)^F3f4Kq50}s1w*!jrP%Y0zp6*RKbl$5@)f9_PL&?5-?NJT?6a+^+JawcJTGu zI2nM@0i>_i8w1P8z5X$jZfl=$0gwqKOIB@~vAJpr=3XvQK`a7tqzVcOuUQO_$yRY) zcxhA#lLJ~!n|~vv9Za6HalHLN!%8XYY*qDg^Jf64xlroicKW{VA^-=WHl*@p62iIn zCA|HlcCHFl&Q~xDxPYw(h6}3jTUY14ce;S*0YYio+m8S!c92Dm3$?Jl$--k?QuqlV zX=uQO0QE6Gql$nzambEP3)wPYOqx^0A_*XXSC6`Y*|h{E6*e8Y9M;tU4okI(vuMuV zh#*a0`1WxO><&{o)bb(+}x`EAYPIIO_;@Sp&LX`mTA!hdj+@KN%^s<0=&hN6X zgTM-ujX*91%E>=NI3?J4pSDza(?`+UMl)c}LIL`#<;jiDwUFT{a{hkrIu`~Ow}x0q zT5+GU+(-k_Mrx>r$wsQm=e zv-_Bj_VDx9s%YHwJi!ofsTr&NVTF)`SMj;BRv`zv#S5$PlO1DgE6iq3>>7I9#e1C= z%o{$B=uY)+tmi*b0>ju1ENBrgC{x%63!!#d8P?baunNSuOY;u;5CnNhFH4SD!14n2 z#K?7WY}VR4EL1Po3ys1$-svaWQoa@1)&du9@JZqT^tr*cHa$oYv{;hDDYs{(ZO zred|*InY@h;TU__i~nHJT|f15^3q%Np^em=J}h4G)PB5B-Occs?2EAdDct^qBd&=u zQJ11t$So=li@p2e_UK92`8doY=pidSlrfZtOJ@7?qh#L1AKkpKto9nm3*z(7M{eCa z@vZV@{FMaVu&-#(%|}v6llf&BHc*H)GSz1BFiH>|sY3AHURL6%11!7JBT+HRg% ze@TI-7PjcvwAq0sN@3^(aw_Q{OlhhUJh?7d=R)8!=%OyAU?O7>!6!-RqGp2Pz(<+3 zv$SUVQrt%_*L34X{fIn52Jaky=(qjO!$03?rL;7O6Hf!*d6FG1zjwRw@#8-m-z1V5 z>-d`dycje|k>wgLu{Ko$-iAyE3DMRoKTuYOCzY%CNe|BIG|3?289qmQ>as-HjHwX1 zQElFP`-SErn;-c^$}}#Y!@sMeN;V%}NqU`B!jv+4VEHvh5Yqkz$L;o0nyL7;mg`Vu z?3>dkQcG;421R5lvcXe_WvarE*IiQK&A~EFt)JuRKMyj%vmQNrWUy=E7;v{<1H$FL zK6h!^ecSkT$-dIkpB8%{c#1@lpRM}xxu9T1nHykFzwU~^ zxNy4Y>zPZ_BH9$Bo3H*ccEr^|^TE=mDn|R%Ye8XzNH;3W$cOW#u=7Usmr=OBbvSov z^6WQkVNsQcXJTCO8wZ|?PuJMvYn+sk$Mu0p`Q`rhq#V|1lHI!P$HYvp#mB>*&aHUa zdYZ~4EjN(iOT*i2Y?Tw$aJM%%@;NL~{pSL5oETH^I8KEH!{0ubt5b=c9^pEHQnZjv*|S7EzxN^Opzf*Vysj}=GbEkd@iYS!w{HzG8~Tw+I@vFD=4fR81tp^nO6PLpbIb4sAuWHs)$)K^TT9C^i0gDi zPz7W|U$lRZx}Yt}l446m$R;JP4^5TPSj#tiglfaKjaSLoqmF%hvu`Z7`sN2e4Q(FK z*}yq}bY%&Gm|mb@3z8`@8_xL1NKjVpg~30~P?k4qaEhlsuu(wQYM+1%uRpY*I-z5E zYqIdwO@SQ&5FH>?8i1fbaPm1q>4FL?(laF0Ht)|- zHOM=Zcb4IL53QlLpE;Pa^<5QQXuxR~g2qNK)I-!i1(+IC2R8un3NVt9BhKq0^Zia) z#OlcR-JH5Omb?GbxgW%YnyikcL|KLTm!9Aa6C=a-VRQK6giM#9G@Bl15q|pE?ZgAAQs$h)su+e4`}Vqbe(&||3hMUE=IKu-PMjZm(&>M=BiO=cKfU%d!O=B03MyN9MlximUPaAUpca zkF)z_r}B>#KP&EN^s*Hilt_9y`=-Ak6GwGvOvVdykA@y^q{yJ?ag#i+%3!=`AsPx! zG)Td2^lq%ZVtd$(;VZ=Wiv8wsn{<@!#k;qN7-;(M42MmAh_5@)N5ey^ zL`5hIO#!hcanRK@J$Y7y6Ai}t)gSM%DHj7^R5LE;zwA%2t5mC9$Lq8|j-AM)EeREu zL6{%bGa4%hFHgrbgehzI)yi*+H}MNpYq^q?)~u#D9P+ zAyNn|s#x zR_v*~hTj=ED!=%VTlln5{j~wzHyzfWGZ5X3CTFxoIxW@6zD%LT%VC4D?LZ?(5(u`R=s$PISBi&pZ}vm~yoc^(%z&{H=VV8HejUK4Q?PgFlm4>fKJSaQ&W7MUGG@J9`6{fO(#=ftFPe~Cgb+a*F@v_fHzpe7KylU$IfNZcq0I?b2 zQI%miIPR{0K26cYu#*FQ>Mqh8Rh^j<1+}xM7I`)Mgu$|m>vwx3B^Q?W>@?1+1b$Rh zAo3WBO3gPin`Ww+tA1(|iAg%AUeo=|0=uY71)I|xA0`Ls-#3)neLj-!({***)J=*n z;4I+!?Zav#KKl~qBQFX?UgT7+`G(IGv4?-X=uCn8!_{{S9*tH6W)G-mpALs{j4nC>k?ux z`^0yW+i7Mo<#Rj(gO(WPiHOe&O}?A+eYR3^$H&J?C^ug^!izku;Zjic_MnF3M)lu& z=zNPGRUs3`Fv0?tl2l2>TiHhf#pQ}}3_UG_fdxuF?9BBb=mSi(2T7Pe zAER<8B}ApijH9qF2NNWb5kr%7P%WM*13x2<{AntCEpPqQYn4aLcT9znuee9f$zm_s zU)pZEZmpWe(kNKu-9#yh@aF?RZw^>VO{*2v=x2*>O0U>*feJsQ1U!}2QoW)^j1lwV z0{+cW@_pOOg?5x1trOjuUfadlhGPB*x70sIZV4+S|GD(w3B%UW{J|QqiUp$J+X!PG zk*EE9yxIjn0nY1JDNN(+u=#wjg~>Pu&PHIGGs*0O5kXci(t!L}Nh%y8M)@!TWL&MJ z`ZyHHx4)zRA*xNotdY~1g##^r+y}dGk9vjXn_=v@q@bSZbrS2YbLH?S7OzCOYJ1sG{a2T2u4l>MD6XSud_ z#(N%7{iwB`Z=+u+w)ez&athbTltXR467Ru5RHd?px>Fubw2>c;}Fs;A*u7w4hZ;pbY9Je`bl%B zdJx{&xBY}Q3Kr#(bLIvoX1S1okAf` zLbNyebfUTHG=CRFLc6-+*Pv>3;`_!VpiYvFftj)liDH6qM9rc!;$_07^L3J;(8`6c zm%U}^CFl0(CaivXzpRc}W72@BE5MQ9pE^&Z&edwP8;oesmEr~1KEMMGSiC(Rb;$zR zyS~0=NGoZ`rur<9GP7`>-(xFkP@RW#(KK{XW!+Rw2^U^~%mcJ}H%Ldg8we}vA4PqGiWqy#D7NnNxE8k{|r{8>x;B*H5(O`1P zhF?cHbTY@fYo8DQ_8nwzpZgFn|JE9tFxU5kgdGW`YjzewC%$iv#pA?U%{RBgnW}%8 zDk@Q;@?t_=pO;OAR{8v&LcoX>n^Z@0PB$R<{tZ4$={tc{m<71 z@asm*63O3?6ZW9vM#d!NgFIrf{?-GWj_++_8vWYzGm@Lv!}}LDTKmH{cTPor4=?r? z^^q38-JXPsP3dc9_YjT`D)++L+zFgb-x*fIVM6rVP)79+D2kE`)3cwG@-ExIIU^jN zEjfPm`0H`;PZwGB{4f}7HD9;(jqjW=6!@gqg`{n1*sJ!$;|)HZ7Z%+)S)%%x-U{JG zUD?A(Fmlu1&~Lw!@gdizc;Q2wxP0)$X1l6?8wB{R z53>s9?pG?7Ct|lIru`pNo4~Ggm38aLq4}*NDt!kFiLdo6+W+){exEududkiox3$m9 z{XXx6c=AQ{w|A#MTG&wcJtTH})A_9((T7<48xK4|?b^s_a+dtOrv3+ejJ9me-`g40 z-$gT#y|uUej5_~ZBqPJNv3qxEq$*V#%G{0Jzh^;fGrS4z#?JAJCSj2{*wk{f?)M<{ zrWF+c6*_la%6*|i_BckQ^j2_!(wGVT$~oPBI`k7k_QtyWMGCA2$u+ItWk1uRB%UBxh8}PmU+=oGjtFC_u%!!9Rdw z;xnaGKn|z?{;qKU>mN3E5^Ukk4U#mvANcwapg!0uJ^42Dzi#zN{4iuiBj z0+NrUd0mJc5fs8lU1E6;E*n$rl$%rOO+ z%xGx-CHdz7cp-$~M#n{r+1mXr5c?Q^XGu*>)}C}laj}Qhe@FL_=b#Xz8YDD8KJ?Hr zizY}?x|vW?^etzeSJ7||Q~S|=Imm^3`^?|m)!?ix0Ost%LgCfRfT08bT@)-a$OAUq zJAPyZAA-t=+eTu#WW&*ucXP`I4B6Z;gJS#Z$7IYUPD1?^C?5f|kc6TSU|XjRmcb?h zjL#Yb5_AD^x130LcFUobvGa-g)Drh;O_6TPCfnRTQeCOU0r><~OgRri4jsP&u`X4N zi604w_#^a){W|fsC6t7EaJVBc>bt7U&eb=dE6@eD-=|M8YjdC@abglE#ft{_|c9p}~ z<EWa8x1Sm9 zn1Dae?4CMXd}qczyL|=!kHvSgF8}Fk@{ICNdJO~lU+lIwDEOKbDc$Q?3fnoL+b$EV z6NCDz9`d_&O#>JNR+@^%7wIT>>C_Z5mfNxWTW`+q_SCXa6m|DF!_Jxf^M!eGemO75 z&)n{$;oRk^gSn{toqbum{jfIc&VIt*^LJaDtr|f@`nAiJmcH+{<3T&^EU}nN()X`m z?^p@jF&8jov}{CMwydGwH{r`THHtO4*$K|OeVU7Ddxh+U-RDEtZl_tTB!1Zyzp`66 zyM5FegUnTd_6P;Cb2z&+ab~{Sw(q|Afn6>s4u<#NQ+dv2Dnzx*JYrAdk0;USM>(U% zcWV;Z$h$Nq>9h~I*>{w~bG6-OksAjZ!&)t9w=Ur^Enpj8^xzNKWmU6Mq%^JAT69Jp z-KCk;GE{_Dj9UVB`CTD9`nZ^!h|Vs*Vt>Q1_oF`tT4Cqf3amwr0@zY7T)c>=;ldtC zzVy?}RrI1FG!am{v*SJFWhg+llstOW57aGz;uBDFFgFc6KeInWpyaI7Da1h*c%5O+ zJK6xT(WdL;b-glkM=^dOquyza| zxYV8qK}^xtB%pCN5kCnEN=wsU0LkD6p6$>N&tJQ+^v>P{m*oMVG645_A@9mZWCI@f z>98bK_-b^EC{ktM?O09_twzcVIMkm21?6RcX@Uc6ZEX$QHN^n2_zWl^j|~>^VtW*U z7o>0ld-TBG%yOsEQ-KkLzs|nM;ztjyJE(tuDttG+#QX&fo*ejYn`dDlgXUPj4&8wa@|_R)-G2oz&Wp^sAaj@c6j(}dGtc7U4(U|SH-KkzFw^vp4g({_lLh(L>G#{}LHxtF2XuV#9M^C0TC8-8xD&)zbl zbrk9^!tmql7`30lGmZEBy2;&Im)~mc?1!I6>oek%LCzzl1-n_Ss!l#pDwac6`Z;%g z$Ir8sg99zbrmMMQ$^LYJQ?E$2q-RF;?|LMEL`9tUeY{U=xAWzheOMCRk@%v(QnBNT zyKVf>4E%D8f(_`%2zM^^pB;gpifW6}vAF#9%}!(VXL}!Eb5s>Gnbz^1E$EO^APL?~ zlk9NY*`pXLEndBTZp-^0T#eTBbMtXD-1X{12B!kgf+ymYden>UGn zL_T568sk{M;mv5KI0%IQ=258zNY=hywkALDPnbw>zhvoCgncs3h;8tH?r^vi#HljXNDM8ADB15if( z)dpX3B-H_3@Y`4X?8M*t;s4D;!TE}bi2)5>P(l-jgVC*ja?T^*(XGGU9%SGL%CwvmrCGCdLs+QzrFzSmHe z-QS_(hX^9Iw9G)2jfprCo`kxfV%QHYGYZ*87Tv`wnxuXX)vpNx+aT0iZa|5K#8RJc z{Zr}_R@zX52`N(u|2lYp>oHgcYJwr z|1PbBGY5$Hfb;aTG91lxYl{GaM5xC=8Upl?L%;(r10Vr(6QBs{+WdF#Rd5ag5DRSU zJgcMNjh#yr69%_(8y+xQiDbBeJp%L~;NzVe=m(aGr3rq{pv~MgvidY$qvSp=0^akp zvK;W!JlObQUKLCQ>3V9wm@bT|@_}hI0>pEPv;GwYW|UAmzsLJ#MWId~q7JC`&^vGY7)BcP9O{l2*9h$<`Of3}zsvt;>&&g|Z1xU?L2{$joF^CjoK_%Ps z5=LIA+ySh#akE}$UYmX=AOx=15WcAG%A6l<9Rca&%!OCwi5B>4Kx#Q|d*wZcU~Pne z454yiBsUtSvL4uTaO@|A+JLeo$Q~v`Wp7P4P0I-I9P*URwLqm{5?RxZ#7k~R=Uy}z zIzOW>9n_faz{u1Cb`Kb)r|BW*BCvv#h<@y0Z_fcS4ya)^pJ+prt_6ENA{B#M62#o> zp-NUYXadG>{GjlVtd7Xj>hBybKBht|LbhPh?CYjc;|OYk zsRVjrC+r8S0VMuDL39y10c^N&KojH%?q@^fSAKeR11)PT5(_1Us~UP34K03D7L zm8=e|GxyfBg6U9)2F#1;FuPvCD9M@Mh2l!d;a0H4xj+XLVoaaiMHrqU#c)vA7dRsT zJ^V=8-^>#Aw2NJ@l-x~x=-e}u9Ihjz((c^3!z>q4@5y)JIVAe@axGLz^YMs2Wg7H0 zHD!S?50Irk?&!oc28DwhfcA%quduK1m{JbacBMg`+CTn9$T&={%tc2 zIu~ROLYt6RI`3XgT$DPH2$P0>V@d9Vc}E-NxkA+<4~}p3jT|m&rc0pfb{$0N+tM$# ztyjwIfD8p*JpGMU%yla8a3bXiE6Uh64|PG)?H=zXsShSN`wx+yb*|;Do!|CQRNVv5 zF(h#zlN~AvAiM|DQXrG#j=c;s`|7b#7oXR?ZBG|LS$}Ls1%@zMTVZB(0e7L&S&V3Ueb}$ zI_9ty^;7Z>Y@qJ>C*lW9%v|{N#t$^Gd!b}90KXojujAI3p5A9~13y?cl&*W-r~wWE zZV^`Kg&D}j!i>sFlD^X)Ma?&O1dZ%Loanoji*S;Q!0cEs=nwtMuBCAyY47rO-VO#m z647H#-Q%*p0wuc2cO(O${S$D!!5;2ZAIJBZhmeY&MP!vlhFxzslarRN;&zZ7Og?1Z zaavQNMpcZ3hGapPFRM{~0=xIZDAXjRLa8I6<*>T`2rVB7B=X%Go}Zup9D5D7_G3rK zJvZFtV5EK+7%_1PX@GaQ?$$wl?aKYW+^UmE=5_(|8Hm(h0@TuL{Ufyz%g<|X^OP=u zh_G3yJ#dpSO8cGWxCR`bn;+|deC*J-c&2>d1dZvh7xiw0J?rH=I#JlcV2eeHOvk16 z2CYEV^(15jpJx<3LubEU`Su)l{~?X^bRQ>OfnZm0@Xfx`U4`ino8B5suw;k#uz<%q z^2S=u@T4z1c@qIk5I6_QAf5om3L-=ToAHm2S_W6Ui}Jt||JZY^y{WLuw)T#_lYICH zbVZ%*`v;+~AQQd<8S&W*ZBaMO)CLt<)E%9k?zh}N-# zgTtI16jmpT=7H&@>7Hx|-(a4ENs|{_FK~Wk;d?Ql86l~xC-!LEr>ES9pVxln0F*rh9Gz zJQ?|~EdbMh`|rrcp47_QvAY#b2>+`^6FV6O1bP2`1Ot46$B+L}FmX|y{OX=6yXdH3 zcBSuq7}B-Dw`OFE16V3pq~1hi2P;j#%{#>zluZ_GW?{(Ap7c28O^iy*n-5cLFLl{T zSjN2t_1kMed_m`Jss9N-yY3jL;d%Fj&m%4*6ZLZwla%HuU!G$P%K{$0|W^=XFYtYShzPc=z z4%?b59`}B@kuvKTDsIlWyoJ}WB^2Wf+FL;ahwDFa!E$7BlE?jEwODRtrFWimxrdZI z-up~eoENlMY^s!5*r>SEm&!?b4M}fdoGJHKAjg5aoWCv@PI$wfyEfW-cnDG0fw&kI zIeChMc5ye9>67MfNPGKhu9N@~G=z2f`}=eF3OrHTm=3tF0SW<}n8jSdp`n@S>1P!b zn#HVuq!M;o*x7D806GwZ^XL6Ao@SC-0wX2(5&)P0a!7LV0%CYTE3OAXV_`36KZ6jg zKu8M+It=YY0t1^*i~~BD`@k7%>!Y4k4*=xwHmg-@-!U<<_j46}c&y~Rsi~>pOP$xy5E3kZ3mcY5Xa)#nzq0!5vrfWt{_MGP7t%iga61)i z;l49o`ir;A&-~xK{0y*WUIAQeY{QQL(gBTe5m8ZqxUOWrtMuOJA;3zk9mFX1z`ygE zDD7cVTpZhpnj-kfUxDn)GAcV=@SrAx;;P< zBzk9OXF<#E^}hDmKC}IC52QAFJ%sy!RcPR`x~8TRkcEy!K+bfag5q!y@cEaNlt9g0 z)@Q3){}w7LDwspml$2A*CkPlGhmPjKMGTh;68$rF4rE0|t)ondSp9doZm`@f;eI)wG9;T=|DTpat+ zqovN@I|c_?tl3ytjtL4Lp%?8g^P(pkZf~dbB*eoj7v?sq5>6L^GxAYX6!`}@Bh#|8 z`R6%_wi{K5wLscn59C5Gfdr$escF=bgxBm#N=nM8K?^4h;k&Z+J2yw(yubOs**^Zm zeP!#yjY6yw?@di<3FQn73{Iau?cwpdJ$$cGIcc1zA(`&^1+dW0KmTZ!Bkc7^L`0s%Bb9q&<36muI`U}~}FGu)>{pLO}@k+H=Q zX6tdaD_5?#yBGHzK8uYbW**0G*s6Q}Reru8Gc(%grI_ar%227voMW-|?0p-1288%~ zb~&z(^Zu*H^=~zf{|)E$e_Z0o&nqbK$AMjThTnK zy`Mz#G~?AvXQZXcTuhr|(@wUyWU>TQQtbhBS;(@c&do5D}77!c|DLVQ7;nJ?j72g@llPAb*DK zXcE#*^VGm9ECKcyi04W45W=%Mb>xhfKpzYiU}R(@I2(FyUqF*J$Gq{tfdes~*CCGp z?jj)((c<)7ph`Ry@&4Tu$mYS02uk0G=vp+Nn5d{IA0Lv)0s?Iy%e@Sg%Rp=jlcWR} z9B-B390-+od*7*=g5BcD)2HTUW&nE-6BNw9hJuepDt6)I(_McDQxn)nMaY$z!F5OfVFBtS95+wL2L(LWZDuo|LU^U&b6Pm{ zPx|$ixVu0#F^~(xS(f>FwndDNmi9Vy);*$+=zIkvsnyy0wgF*v)g(U5GN5r9pONG# z(*UQ|hi0Ot6=PHxl$OKs3nsP*ARlU1Y2dhoLlcognkC`f*ucsD^HZvL->Yo3N1hRR zpzcu6H7Yb>*I%2zlHbdwx4_DFJ80|Lub0(P>aW}Bz|He_Aua8@#C~dQ z#fw?9bXq4;S!It(Bp)g_^S*mMWe=Z+5yl>-;7bqvHYP-jUGLA1V(y_t8B?;`@U}6O zEm`f^V@2laec=7n-V)MRA2s+R+_TfyQKHT02RUO~>$XA@7&D(xMH6w}r)>YhAFB0v zrn&f**P#sNpJR^$R$#yI)a=4(%guH3Ip2_|5whl)b(UPe=!FJ@&r?jyE; zJ-+#qf9~g)?kR&sSL6Gog|AAPoUyleke}b^i@N-^ZK8Yd-kK%#;+#`&ulUW;PMg0! z<(s9vwB_}W`H_~+%ZqD2j^8Y%NBb65FF*LZPwb^HIIOK-DzS84>+R^KS>I^BIv}~r zw=2pgSoM93vFWHLYU$`>&bS^(U9~nSvbAU0MFTzRW!WKL#UxjGy`%m56YXb@4r?kK zTC@^PwHb4t^y<~3-R0(WFVg~|F&FgHmhb+{UuvkUPX663s$Nx7hPYGLM5wFAN+LD8 zgxh{?(LYz^(4GDap2iy?@16XnpE!JGnYj7)cQP@~PmyK4^1QIHvPgNclKSr#HGc~` zU)t&Bdq1la<8}Js!O0E2^LCB93?fZSuLQ=}WL?%QRaei2aaW3qZ!PV24(BOG>b0vS zdLjL^{G@^xE8$?yQePwJ*rR@MH*<&YUp04jMkn}%{_7bXHlHtTV`XFSz_4lE@F$)C91|1}{KMLDSJ7~yF1J$?c;A^i#F}SUUHsVf4Gk?9tJ{RMVkfGtid{lf9K z1gHg!9V%NIAXr`j`vcfUK!)+PUHMR)^+X@;jl1{3B9TV`C3xv#3ij^F@7@^hjqGB&mik_X^>f=oE@ zQ=F;Qf#w5q#e|p$q`3f~VGMz|$NSnBtIp6*fkO{^O{04W4pdZCt<2|5OKd)p>n$2> z>c=HL2I+-}gald=l4;=AsjV-Bj0iXzv2PsU2nG^)z-`1-$p%yMnbwlul_?%MNK4C& z^##xy($doRk6R9}hlSpW`Kiw4M{UBXh zWKyQLyF6-ZjKO6q0i{WWQau}+NTzg(*+h)9u)L|wb>J@Sk8aQ z1^QVrnv?ZvKj_avO*Z$tsw1SBCr*`59sJM#TjeDd6zbT3AKK+bU(E`f?oiReeIa-1 zODhFAImmCZv#_Lqi|N_*B6M<~NU9u%)47~Z!hSph)O?d=@61dplbf5Eh(X#HEWNLt z`I+yeA@>;!8XMq$37;7IQq0c!J}0&(k3fi(^)ZdII^)^9{I|Tz|Jfi zNp!CYr8k(MZ54ALRVatPi{ybRQCoDB*;bfhHG{(?#*@JG`pLT7X?v?)y-Gao$$NY2 z$yY8spTa<_T`1+@=O?G2P^do>!*v$W&t@m z5WgCPBowoH6C^*YIVM0$`!guuj5GtPiS_W|2O>_-zzu*v7sQ?C3AJN%N<9j>$POGh z0VpnHKEDFzz7RsbK;U@=f>w}WZGdSoqyX6|_a95sFf>l1Ev(|=rLd412)o=TOOWIO zg^Ols0?g_fX;_+(B`Z!!I=m?u>Fv5vB{7r+`ra*|M?C65J39&yL7VKjLY7-zx+)&)Sp z1bP-6=1-}r$eL-e!K;-;vO#@)hJot*k#`W@UEth|FuuT+KNG_6*lE0-@5qrcAVmz% z-#6(!^e{Hh#21pxsumWhvjl74AXvi|0;wO^3{C)m4v}{|dGaLqJrH}3$`1tR3U;kY zH#ax6hpS?5-(^5R0uc>^HbK9^&7S?d%ha zT@7)9B@dkr@l-buO^lzEadv`T4?tSJi_g!(%=0L3=>afx!jU?J*A@1e=w@wM4XtuS zvRwUXh!VdkrQ@d+WVD!uAcA^p>pyX!~KBCxY zQ!;=4@{^|Qr12IA6vT(0&5phM7O-a^)HTeN1!*^!TA&E2H{r5(+*k|Hhb+C}tI`}C z9BOGElQZ}Q601;z1(5U=V4JPwo!}aM%{6mJzMW2#ZE&s_=2L^r+jv@6NZ0GNVz^*i zJ$#FcRA&`F_*(mtNjgZ0Ko3LoaE85#^JGtVUYr<IZcGeYtH#J7+6?dO<5x5^qLhv)7I7u6=0NiI zU3So_H35|s$hrVt&uo%5lF#5uV`F1%U{M#(?A5!6gq||e;kBQ-wPDU8Tpb2E|HR5`@i z5-caKF~bHY-ZwR6+x9}Odg_S(?NvO4w_rtR&R3C8yFr8XurRe%TSmfc6TszpI(g*E zL0beepW$Se@7{y$Gsn8SsKIcIBr83g!>`UIxsL0Ux`mZFvJiu zqc>7siNjS=6Etx!&CA^RJK97 zp>PE#Sr4}wZ#AO?%lTaO5Q{J)qom@|(GJ7S88EKPV-Gqpz{3{yNP1livIF(1-4eGT z)-(gv1`&~w0PtiGs6ue0ov+R|kOI9i@!%|e1URL=kfMb3H)tu05fTs(Mj5ofef3+; zM1qN_X-)Z&eKi-oyFj8Zor{P0wbDGS)o-O(;?A1qb>Mu77Y>E3uvAV*b$Axy_Vb?3 zy)Zb01*xg>Rr(4p5ayU@r6SsFAVdsogk??bj|d{CxNy4DlaL9FIx7J-HZv&pn7P#z zCG7#JfQ-V_P;XN=4n?zQ`+&N;S%U|usC2B#d$bYjJqYKG9)4f?D)j-t>pRkExbL9|)7k9pK_=eur< z-w+kPH*_lYjj)hVff98r-QfNt7(MX}(XlDJd7Y7FIjn80bnji0{w z+ZWDPbdCB?#=2{Z{ZRA#Dy!Ol-ut^4FmR8#jfLCw+||1uR(K79!(i4dLo#ngJJ@?a z@zgvQ3{-=^sVhWbp3WLax5Wjw@UPdACnGCaIcr%vZtJe&$LAr_2y`APW38i>Hh?UM zu!95#q`a^5D3s&iQLe)|x%KbbmB<=zB5WE%5ZLPSYN2Gu#XJG*oXmy-*r=2L^T*A8 zDB;KD^6%E{enkBah+EKa-@aJHY>R~$bauvDJS$%k%4GHJ-Ar8;V;vFj~pJ(OZNPF>|7qin4+mBA&;(w;`ziBxixmd2HoAlzk zf!l4X2j6qnBJz8SG>>4Kty)Q_1 z!?+K8&m>v@`9E^Bo*^Q?{a?T4Y7f|gsC>q^o;~bGWDWcLPL-e91N^AoB)r7sc$9dC z*eGmXOdL$0PsITG)IC6-`p;$UF+j1{V{OaJ1qcOC6JxnDQ^>nm?sq{PN`s z*zDW}f)-7uN(l)FX7yve>y&#C5E1B=l|eFm(i>FA%o-D~12~b4*Moh59g#leJy zfg?D?V+NiCcCb;jBRN9O#lm6&FT}z@6%5GY6^IR%%z~E;IKkL5tVwT(mqWovE*ON% zk>08i5g|#C7C~%tP%M061Dfdkw_;Wy5dsJ7_YxA!^V495X@WOB^l|b-S}PPu;8AJ~ z+L0p?>5xYRpD|)}NW1p^`{+VPC_{msc`b39&5$BAR-a7*`hX5tTfv(jWWz{4CKi%3 zH#Z0VjpKN`DnJ6-U%YrxU0n^1Q|O`NwZ?kzGS|^B0|S5^U~3l1Tm`B!V1T%GS5@%J z2l;b>=`IJ_I6{*xV3dJ^b$WUlpgXqQt&l81FlFm2V{~JrSwZj4NVv_I1b%XV31+)n z*_=1}3;POG%mA!t4&(|@RQwfrN){FtkZ{@8-YBNO0(qZ<6cmMYFcq#!NxcO;ugR$l z-NNi{>w%)-HHDzu4nP?KxKu=Nb_S%Z5#oHH8=KaS_0|kpBk#E7U>N1MsG19Cz4rEY z005l5cZ>t{Ioa6Q4j(?u%$y89GSV=74C&1FWO>3wIMZN;Jlt5%^sA_-08Hn#%roWX zPKxK=4SE!SQl(3mot#1)@$YM(ePa*pb>`;XqH6sAYE-YJtJ19nu)OWd0 zxB(5B=VvDZYVpR!?o}ffGrx^VuMO%?9$;4kns1}WM*?r}E0{PiC(YM%%2WE${;{4U zy3^03L}m=6atFV~siT?K^)Jk_GH(<>g(;(C2vousCiMqj((Ye=$duQi+Sc7ody_op zMxE|tDYKWPrkAqZhNAAqcUxf}LMa~_3Mg)n2?oBX<%dbCs=e(rQFBQn*CzI_5pb1} z%xDeFWCqqjbpq*NDuBd~7$H19xD@*;t^4?^Y%v4Z*^UUU)(zNWQ{2QB<^+m?AWDQP zOHxKJ%{(iI^Nmck zhYCticOV~@4Ezuo0enypLa!0^VU^A#scnrhWjM(7>=VVk+LaoAw%O_EG8y&Z!bB1^ z%MaJN@<(@hjK3qUEvwX(G?qpiGh2$k+Vj!(UbDtXOVmn>jQm~pcEO-Ds4v)OuK{(a z6nqFzhwlZ$X6iJZO#jpJpMzG)mijk358bWU(UO+XeuAR~eSoEoo^%73nWuO>yNSpQ zp(K&yPW02K(RQIHQKS2SL%fLUTbBOkJK2XQp+>$JVYUM@YKI2lr#~9NTZ2d<)7%#- zgj7@IHshN;;?LY9a;4;#RnIZaY_z-S?d%1$kp7VeUY^ZKGJrQ%`>>M!mJ*2Be6Mu( z4Jzc`yf{_9U#d2F$&0YoG zUw9xVX!cm(j?Q>YgNAM89fYA=S`-dLm8RvJ`v>(Lo59zFngg==={J;YVQx|L}UbKo7?_{cdzDsfsg(F@~M8GS>$@f{T zoIdzJ7sn{Q064_efCZ1~>4F?QfH_Y>*bQ*>M}Bs#gT+)@XoUXB?4B}d;)s*dhwbJG z)Xc*VQv6zxOnGforBF7|ATss>SO<>HmGL9hkq?JYzi*X%r+3o+xc3o~2u-;ef;BU~ zP%(d*pX;e5xF1Nm3m`)ef!JteR@&fx2sgPnIo(=UrraFY9E!CFc}A_!E?ARN*+BH#_Q7HXujZkd?=mEOdg@$i@nl|Vww3%PiIuM8R7%q1+-cfrAg@^Yc5eMHRGc8vhdfnq z@gaqG5cKzQWGGNCN~SjaGk-_ueFm=cT1ineXY(HJT(>uaOM1>w(h-L%2RN_{OYwhk^>IoM)M(%U<8Riz)fR7fdN`O$yCmL zvtRX+w;D|>iW9Nq?;g*RKKJ&)uZ^*C&{;bQk;jHgbY>F6!QsfYw4I$!g8Yx>!uHN< z6};cx`h%F4jeS_KSpAO^?44S$@$S5lh%L4#mM$mL>IVy%1y+H7^vNQ4`-tgKr+z@d zy+Fp)!D^c0Oib&RGIGw(-K}bStc%%HJ-&~UwNk3(?pKv{8wWYygSHEGPf08ko$n0T z7Z|`kxYQ!oM{#fCo2l-y_Dl4aoTET>QpE)XyzAC=`1WSwb3)CS-pMoWXidhP(W=}< zs!um3kG{6+7TYg5P${0O@bmfiMCqRe)YN?Jf~V@)mlo6<53`i_k*uAX^5|XGEkzf5 zX?s;)&|)bl^)se=cZtQvX{Wqg_pQKh{`$iTyb+ADwhv<)oE2+yW|@PSG*$WND%O81 z-~V%s&6e?z@(<{}lw$`(KRfhCmxoq>=)}oQL;z(ws%<}L-%MLiVtbUEg{5(qF4Z!* zQJn3&4V`*TKKSN^SwfI-i6bMWN@^yDCACuvMOYohnw$*X$M|l5zVq60>897g`vQra zUUFlLSnanR-n;_50Vjes0O9><`MyOLui77@QX|FXJCkCuGgMX+9?R z_!M^|yG!``zTISach&RbwWybuGURaIIx9EI_q2-LC1m}#{$+eeltt<_kz(IteFteZ zB|Y`$#X#+$o$F_@K{S#L^^WzOzX%M+ zJL?xyY;dE1#hPWFHF$~jTi5;2H~vQ^x?CL4PSaN@8+EqF>6YnP*Y|r~YWIv| z*x4;@_o`QpQ#0x78>&~xWlb0{emC8_zBN*#-81&}t+cGTkuuk5dQ)feUMN!g{JL!+ zpSho=RKbDmDm&8a{XJMSW3G7aBo>44wbRGl*?h}E{g*P9PVF<#Xb)?>wp?$>(2M($ z9W^>Q?9~~??5l_x3EScDo0_ecBMuDC-j$(Xk><^%uFU9e%t}$|j;p<($RDu!E&ZhK zr9W9g9Y%cYE zDLm$&NzKJ>jvt2bBVz*{@=<5iB%`O)Umf{uzS@LYF^S;%VL#Ack-?&Vkz8QRsaL6wWuVmeUm1D( zvYqYCf(y+GLCYm^Z9hONA+I^EMiG^_O(Gz`a#aU55Hxk=d*nHJ@82;mAX@^O(}&43 z+Hw6^bhG_j?1H4LtKrM_qg_tYKQXHfie+$EQ}$0N1c%zwP`+DTSHkzYJTtw@ z-^@MgURf8fFW}}|mnz5D_(``gJ4f=$@WyG5HV)ignC5Jb5AOFT!zndfmm;wXWSS{i zM;oB4s~wChGH)C)sO(|5ivQXXuCK~^mrIyAS=I~(+q41CKtG8tC(YIAyq9dP(yjGN z!=Bvf{XlmjyPd{`X_On3D`d%rV_#M--;L_YGFHcbF}P}^w6qlR?<655K=ag5p(VBu zk-+bZ=W|}tOci>{u08z-Yw_`$?RC1jhu3t$YWtDg=Md#tT3YJ-2uXzQP-hX;pJd6F zZW_k9`x1Q-=!1)AZBdk9#~=f(AQ${jrTbOx?TT^|QaYRs1-dB!2nwLuAOuK_AUz84 zo(v7e37*r?)zy7h-EWN&nXHb9AX#3VZRP{JY~?lR?9C~IycsS^29vIu6re>zz88&{ zV=pG+sE&sd#HgX*VH*SuW6&5GsbY7G=v2u0(68a9{l;yv6)XE>ANBozmPc3Zx;8AgnaXz+>vZ$4ZZ55q^r-`;H6N$Cz3(5y8!eV7B&`~X$SBK7 z9f(i2iV9R;-Lk2vv|I-rY91+397N--X+cchg!5^n9uJg7?DOPG)JP}8>8iNnDK*vI zPidd4u||fH3cmM!%G`!fLKDCU`X%0%zBQnX^+~-~cjvuiovS{++AKP2&j+;_j1#~K zoQ7NmNS|+g`!>hE>*aW(R7qi&4H{=CtSkb3E=y|q#0vb7gUNP`|I&+sg1Py5f8*TT z+$_Tv&mmKE(0|3`TUmj$9Np{(@Ne#o+D=SN4Co0V$R>=THCQq7R<`CVl@@m1lDgJ( zG%_)Wy-yj|)w^P4tUT&dx?ZLBe9N1)=dGIa$T6MD!Dnrplqa*Ki+{0Idw%8Ce99MB zosO=}(uzI2+HTT}lEk4~=`kA>Y~(1rm)!b9i11A%soGvM{~lzDB6J%oxAno%Yq1F# zYqPkaGtX}vbeLf{$$m8+l)E=NN|4iC zON*l^%FQjx7#mFJ{mkeJu;~XJ1%BSbpHJ(DhU*Dil<35KWr6ES{G_i~D2UA4M@?<~ zgPThCO@S-Yw{`9n+I;cQ8CUp=NNiCZ!K*B0jOzOwvIw-D*IRQ-;l_>R97?r2 zH8IhWl8pg$^)-VJw;owjge8``Dl*Ijul{+XjEdP=M5)EQdsddWAC@@m-HpS|F%?&zv*A_`5w$kU71i#0uIE(D0th`k_1dd+(S+M#5i zBjU6+#EZ7F>(nTiH28yiY@1>#Xb?;2_h&1~V%-PN9eC_X;MOKRMHt}55=UDi1i}I!@6Z?D#%`}9LFusEtBbn zcgn8Lw;p+#3pIKE19R%!fJyAN?Frl`qyqW!I{hGAqf^`uS=J>(PwHf(VsQQ?oWcwa zluD8`N6Mg9&BMX588p-^XW%ZeKqx`sI@@;(09x~LPOmvjoPg5+qB10lZdJgiy`n~g zlY)vfrI|gv@PRp{$jL}98662-#ES{gFQ`oB0kx)+KiE-kyg?vk28M*%hTBr^EW9{q zUgY5QgkpE2Ku&sK;kU<2q{L{~vwzSW!M>ry^=lZYTtE#PPKi?W005w$2p{miQFH5r zLJjD_9)EP}0MlbY?@=Yvke!Ec65!1m=5l=9ai=U9IZMOvz}2}^^aU@@%-*Jo4#K`U zk9+EW)Wb=(dX9w4kj?Wt%o-NbMFrf$z7b5|ekc{_AMFXy)3I1!;uy;=t40AL#}R-x z_gxXX7YI!z03Z=m8S1%M{&jlD-Io|e>hwGprW~y1h#tjL_srBKnsx}R0a>h}_c+ky z90B4o8>yi?Hko4IGjbTgPxz)IV8aZ0%RanxrJGEpJ8Gk+9#5g z*Np{UgXxJ0l^Fh{v9uI_H1M(wWM*dm{*>3USi=gH?vyxPB5D|w@tCe=aydTzrvp$B zh_C1%O+7#k$=eOGO{52wH?|)jn9-`2oZa0)wkA z@zn!aTJc%3&S=*Q?E?-wai%ONao3BBixUzOfYoRoz|f(~iW^qnKN>eaD@pX)Mk^Uy z#+iLhG^&t#2F_OeW(d&IJx3kX@$hi)_CK@$2tjbB%_vaK8-cU`kL@gDtyj-Abp)LA ze~5;F8=l%6;C<7JlFx^U2n)CFhw zxg&>zWlM|=I>D{`yI_TTFFngrGf27?6|?GBSa*O(E1m=eQc5>Po6wypmLbNKl`#-X zA@GJ_W~T^t=9$I-yQIhN^DdN{f2aU3XuG4+MSBmleCGOhT_9=o%=$+o5YhR!pSL1> z?_SVn&YKdxrqFF1##x=VL(#V+lS!QVd8@h3lVA`i_{+wSSFN#SBm5v zQd7lU2(_D}Q9y7tvj<>}D}(G>BU(!dy$tE7jIP7`u=&8Bwp@PK`fXO@5%^4Sx<|cuR%5oSLLeX@aKEPkI-O=GflCwDA9Z(y6`G@4$NspFXxn4h*d>p# z6qV!8)|&;ms9T_xmO$vez{BM<(GtcX>rE6S1jbvK%$A<_vr!`li>{yk^X_hLtWRw> z7YdV0=w?$nQr*!m?NTn6DgrIjJp$i*(5WqfkGCddtwyrfPAcYuX{pvU9}|gCJ$?jU zN$e5U&$P9L-GpZE)j!9vM(BRBIXVAVxzHWu1RB0$q&~qPe!BiV<_v*eXWpmY$xu~m z{4Dqaq!jp**I+gpi|JohnU^>Kb&0D9FTf#7Cb5th!U?O&Qh z=G0SM%~8&%>8Ys%$5)&W5}0vU!t$*gx%UMc7m3y?vu_qwK|=Zi)gGRPkSp)59)YX_ulL%z7hPO{v-LaF zUEz%A0zrNrdIphBgwa-+iMIYL#E;vZ8jp&!IcpBsRpLVHzb0cJ^%cdDc=JVc{jlBm zbk=x$7dRMu!o_*@Dx)!Opb|m|{KwCq3BnijXZ`)%C4fE+g6AR=xnx!LV~xcKzA9|# zl-hY|w~e|Z;v*0;_f}B&ws)2mv#|7OX(>42Sh_U|(z8MUAN2*$^nIUV0p<;KA|EqO z@17GMi8rkowM$$q)Uy(YGY<#h2;0$JVUpL zK3Bybkk=}UgRZQFSev@g2yZc{gSR?7E2(w&c@9196G^YK|Mu;PPL|P8NXKa2exqG; z_J+RpdjHfK?t`NN6Sx&B47NcvZ{RO8_K3QrBeP?}4#H_lp!nMX2@GJ!!F&1wN+SWG z$R1t;H{gxBmFvLs`%=X_aJQCC{1_Y@93Ce3ZfkEx_%49{GPUFKMy+f$x&=-&j;&j_ zzHs?L2SvE+QM{%&Jja?+v>JQ-Q>*(G9Rot4x2WyHzgxP5kS=7%^!%Ps{Hw%{af%7 ziX4S11kfQAPZ9@S?%28QNu_ghb4cNNV4wEt<2J6HbGqyfX3BE4)Djwq5Hh4FVt|4% zslC0uQ~3)Vh~Z*iV=|-|^t9&+gdvFw==Y9(Z`?LTq?@FvDt3SH!<{p>g6vOOgK}FN z7|Ibk)m&RcA0MBQ?*KGHlI?)5vthnRO#(@PZ1kSWfn=y{B6rJT!3$mhSsT3hz1K6O z_-LL3?xbXCG7DfFr+rwnkrYjU-OhoXylRc@`fR&tcG=Q_4;|7SUm@g*U`WQt$3vxh z*r?XcOhGbalpau|0?6A00B}k!Ib#9|BM7S>*ldlWJ)s#=LNQ>${tKLT^C89ytQeXw z51{F@g>wo?B^f>yE?+K`>E<3xa7&&*xE6dS?lB#OY&dUk z4vKF?pPV5)(~@lpC?hzi9>$?t9odpnu@W7QA~q(|&s@c=8q;H)# z6IeP<)d=XlP_HBC&vLPB~cj^0?(>}b}k8*MZ^6D1KnU>T%#^F8mADdM|Ei$#G6Waskfd4bA zVDRDH#M3#JhK{xS`)JE~M6QQCtQqHV&8e~<*?R=K@bl+Sn8fG~+PL?SMFUQkqK28> zU1d?HB=fD`KLkGCIUD17{SS#dLQez17-DiikXq!N@u4k0tMx79GeXq}Y^B33k9$fl zZ!Ozd0ST?{T8wnmPv12)p#fVrH8q7WEtZ{Z`UtByh|z=Nd*crGy(+D%61znpS|IR@ zE&E<=bsj0$Ds<(*O4tpkGhy3Sz1dz?goao0(i|X{?~EQy9%=WU|8WLV{(wKQ&y@e{ zJ7C9kf-0^=%3ojZ;6r5&g_;TuHu@}$Gq|==WZdc`W<7fjG#>aZkN$ zf9e9J8Pt}=A9gFgKl-qkSrm*`m zyZ1~vQ}INzGO}x{M2bg&1#+o-Z%GSa<3NFGDOUvRAU_5@^yB7>(-1k!h+U{B)&hpG z*@IHEwUBN-2kD0HqBq}A8#sYyWe=*eIgs=xy*CUh)UI1nHlAySRg$wbWKwRtGXh*iS3TDP~S&acOr{>8fltp*y~{79Jrnko0X zwu)Oi1(4+lm}%;TXE)n>-Pv#mtj96yMRvnOyrj{KM#a=HFNv&oq0Y4e#xKTzMTffYakvqmP|cnh@2qE z_JI|iGpOf1TW4H6@^VMcT!sll1g-oLA9$XB)>$}c`!D3SO+hvQ@${Wx@L|C=cI^&6 zXt|hHcK!A1*T7ATfpp=Hg|8wAv34Rn7OArV2MT1N$4@>WrnknOa$4GQ3Aj@(Xs35> zQF?lMNr}|VMNiN9&(Cb!sma)P}ju)rR#^pz4ce;Hp3|$X|U3ucwo@Z zCEspv;`!MewW8c)V#sfTOzTV1gQ0XnTp-Z`qko3K7VSYL9H-OI>m$}UJ4b`#jb^sP zaX4+-1dZ)~+Eilexe3}={oN<4Vw{yHe#)tgG zeFII#r=}&R$18vCyoIXi1q)^lUOvlnRu>&D_{`2**qR$-Or0!@1$Zp%op>;Iwk{Tq z&KBl8&UU=^=9ZkR_o|s%S*TvTU}oXSjDMVxt)<=Za~Nl4w2%OFN&`qUW$fIT&mg2; zA|is!LWj|YWPBPHPIea^F&0kD5)$w%M>~wBg){RRWXCvhJH-~+ia z?|W+4OfG8^lPKQY_M!XO-4|AYwqIq!r87$g7bov|dI=v{cE0QMvBb4Tn%Vr{a6i{7 zv8L%W)9r>+_8v)%eo_HrF?_S~{KGvzd>I;>w~4L&agXCT5E8BX@2Od`st(BcY@>doMgB@nrdv zKKqjtHPt)yPGcB!p7wC?=}I1ky06&-wtB|?pN?huU*6WX!;-$ETzI<#*}pfQezKmq zXRcq+`dyVi%rA0&D|RyHeSxYayQ-92`L4PT#jg8j!oofs(=B6nX}5IRJKMFXqkPX+ z%hqS;S7v>^4C#sw6Y1@?3Gk=q-T(3`-t$gl*|8^%GCa?XJ_^U&Jeu`AZGM~R@@{DY zTLiZ_?98mmkOjB4CvYo`j1Nb0qG@l6fsJ$A!sQ&sLPJ4@j8FHRIk26qr&$q!WiE5h z*-6d9QP%E)y`8OvtuwPI;Yo!jA(N|S>UaUa%q`4eXV}4J5yG{3@q#Tds~iSmSwbu; zqJL1zNhw;Z?<*Zm7d);ddpv}B*F}0dO8t8p38WPiNZIU`6U{f7C|v<&-AmF%;41G;-Zw#x66EsgPO-nCy$rDQ7El~ zBv*=TG!g0&(RoO9W!g*+slxv&e^xmVb=tr{@aT>j`O+%E^QDXt)e}^ALR58#&a1F( zuc!Y(*Pt8g8^&UyM|5cK=5kh(d=i>Ho9pCCvq|)Lw*>Y%mijCTUc2IJcyCr&{L6Dv znrM<1A)?SZo6o+dPMret-NVzfq-}g*p)fsNc{y<>1^-~yxvf7324dpl+aSQ>(g+$N z(5e*`9c{337V7->lJW93UNa$Pw#`n|`s`b5j}e6?UZ>6;D6vl!jk|U07LcMF@Va_> zexLLJ>Q&0%ik6m`NwH2yO#==;nWXHZqDQt54c>lx+SugdFI0x#f$fB$`VBK>C;Ngk9W`1-YqQ-|llUl}U$9kwMGD1~Z3G}%B&A#o|d4z>G-oAZ1&{0}9Xg#Q)-V7b}S&+<7Mf^Tf)6?;f zA{E(V8;-;RqTyv%wt!;TXnOy#Qv32{su3V|1zW4OHu%V-cKQHd?0~@T)@D0k%sq6d zP|DuXQDafdQ(t@U=hIjmaR?1PGI=GKD89M!f5_Lv^A4C=U5JoEP1YuDk%n-3a$94N+L zycILRiZp{8)@367xh8UqFjc=!(C^c?eQ zMEzkF$KCSjFSn#jfPIb-cq#J{Avdj%(M^C^b@e z*0B>+{KxhqE$pOcWGuGtk{Oj#R<47ZURettI>NfZF5IdRVf;z=?1{2;py zNd7NC>MmgRJ^~xfV@wI8<$|&c#@dJQd`HyBz@NYv34Vz=pghTa`m_-40xm1?N>+nz zsVQFUq5@Ut0J>KQl(pSih*tqPH*z7S5B6oo!o|&J!6w?I_5pznlx=4+m`pqdXg%<_ z!;b0zyZj_uSQj`M5=9-7V_sCc!a0Neh;U=5bb~?g;9C}20xVnL)yjSTd`}keE}H^b zhZGRb05kEAF8kZxx4iiWX7p|bFi%i#y$JL*y2W~DN5W2j)95RfD;otY~+{GL>2Vso9mrs61 zF96F#E7+`)C5upI97gT4EcsF3-C`126beKVuC_C}-Sw^q>(Z#cr`xwX`})UicUh42 zaN6JBA2Ef&+SQ)t(LwBBm|8d`z)Y8?Ek_t)Zg6T29z=MIc8avVX=)mU)F6??hGqS4 z577o+3pNQ>z)IyvkBLWK45*yYI#7P$A~+ME_TI-2!yK3yNYQHqoWFyENJFJVy=nw#lRDbk45hp)5@{JNP`3(sH|<0g|A|?|{0HX>yfM>J zkEcuBkf;PMVLh)=$QyG34&E@qz>ux*A|angVVID@sZiJ+itfN^_esla-IF+7hJe2X zrYZ;GDBGF5vcLI+IKwnR4B#!yHj+075MW{Cd3%8`2qEm*V+9*R1gKaS00;ne1=3wU z`|K;&BWYr4>cf&-f4nP{J5Pr-52iTc+d@J%|3+4gfEkXpn1{s%8{*tK zcYw4QdC&IEiPbIC|5_~QN)Dui0TKWKw?C zaJQ!Mrc8+2Y$|nwA<8`S+yB0S0E3k>G<4p^W6yamMj$@kT z?Z<|ia>>fE*d(g@=^WuORF%3l>iF*+4nH0C2p1Q~E6+4|O-);_75IO0=c)i^jg*=6nKx8(M}}a1&*fi>aFOB&Q`Zlo zSVt;5J3?l=e#m;g?p?ozjP%+7kyhCo8+Y*JwjRDTH=!Pm4h*iFmEX^@Vf>RZGvNP( z(-dJd1D163E7K}|hd&xnKZ}~4oP-*awm@hNhjtZz4^KaRbO18jYm-FRLsJaQz)<1` z>toNrzyNhh9--@rYW`@nN;DABO9~1SJORSN`x)e zjsR^q)R+pV3utDQHJSo<(qAv6-DGk;x#U>- z0?m|p3^Vxz(z&oC)<(C@ogNrif5Ipjp1kYSw56Nuj|#x5q7Q_X*2+PBpP{tON5Lt6 z+yS93URl7aJ$JnzhiNGd{TCEC8S;<*7^UkX?9o?jO+hhw%K7}(ck&LPhQmq?aIMZUyqCJPF+sDP1M5m{xTwkA;7=$d`n3q!KKnI6tJsx{{LC#5JJF5NIKI7Gv2?iS^ysuY5BD z3ci%KfNOiCUZ4>w;q|mUXc$EtX)OvMm|uW5J8l;xe=;`!d}A!ErPe)D$b$mp>{e_riXJoxtp`| z#6SH^aS^^>PLxudyw*@9Dj~4|3XKRK)L6=eT?lG`E%X)W_4qoGC=S%iNjZ@oN zy$XQ65Q&Ko)j$<85zn!fQvYqOh3;3!$yMb`FYNEUiTF5qC8|#%al7sbG7thLXegmkK9>vj4pBYT=GY4{fo%uSCz!!`ex+S@Yn3*$7nzlpr}@%zOi@e_w)3<}zQ9uI1{ zEHG61F8@k61^>k@|1bHmdsp6Rm!OL>3gJdsAHO*_=a@Ktb9u3w!EF2M?5&#pdydGL zz9SX=fBcV=qMeqX7(S3PhG4mOyRI^uoJDVz4I!n`S1EmCwk2@SZvLk+zDbot zD4qlBE@^|p^h4=jyfakFBlms^pcyzf>tjWRI})jX9RA7mxx~k3d7&A0M-)HF@a+2#yw?!{+5=%J*CX*| zkGCw%-|=C9aODSx!k0k!$bU0Za|nV(l2ERcsGcSC1s14mqksxz_9j#d1jS2UK>>KL zmMS5V2Dy^{A|6|o#G%W%NcbM)N<&ctFlUL;jDTfq59U7NUw~4Pk9}!BvgkCO1XBqF z5W=Q};R6%+)1e0l^+R$9@1S_F?{YW<+z;txcOpeCA@cUMF|lK>{rEt$*Jcoy*2OUZ zYS$3u5|9-;{_JTm_e1mLNzDh^Y(3{$;+#0R0D())Od`8WOjB zHQDtX>rvE>#KJ)G3g_B^`15w!(N>5QvVc+)C3*>W~PI0MKCP+U-6x*VHIF@m>0G+6wAQnvS?OZQ=d)Vxpk|$0Z^t_!Ov=zuxzq z<(|`Nf;bw`Ew@87R`5%y-eV*Z`^3B!%;{oa-zt8LyAdH4$PY|_ z_VYCmzErWjqhjq}qI6ClffFEw+aKQo41qOcFoN3yYSrm5YWv!}k}Hy>0{lLeom@yv z52#*1M{C z9u?Io7BA)5cdPWf+aqW_^wkC!)JH3_yRq)#IOuKy{0dV{<2 zze1t@yT1~R=0~G91j2r_`J8<}$yhxa^`1%OoR2}1BV~T2+Ic9!c1M){v~*q{N`PM- zli48sAx0}F<6Q$f$GP>9ki2MvBT#OR(+a^ku7y172fdxDR^jERI^fGkwh%W8cb;Z16 zhiJq#es9EOG7^%M6|=^j(ucJLWHa?m97jMErUEsE4oq6f19WHZ%B2>u#QmE;`N=D)#gqlj=5H3k5fO>X5Spg4k>s@~-RM>-5 zVX0dYPrY;+$!4wC^@@NzSk zALUBwxg#K(Ma#z!6yRao3d-;wAC(J&u z51|COG1Xo4mn-dod|fk_F0WC~tb1OH;LRAqEu}oY;cFMh7*1#qnos%NviHT_4Zx+0 zt{?2?Q!k6fj}RQW=3o*3qAar{bX-iXc{K~%D?$)o~sK-2@C^H!W z?}V6`7@*UzUVVTZ0^uSFROjL1Kd&wzoJOZ{3+fzma2DzOx&;-sue`lK`SD{16O&R; zPvB*TbM{qD4T)z1VEsTCLj1_Xt^;HPBCGH%hq8|2=C6uz!a)Y7E5RA8(8Wcn(5 zARB<%>lZHkfKwN#-Jq@>38fC+y%U^+sM7@?giK9O=jP=61`c38KJZMNSy}b#iIlHB z@~M{IhIJzn+LfG~3`AW3g`L}5N5r>E%+4HUHUFfwWfQ2~CYu9E857Z5^&kN{E<>1w*RmevAD zaL@>D`#69cfo6_6_9|LhTDHU@AY0sianu?-wBg;8&|Ux_X>M+Y68AuT_-1+PE7Uz0 z1r12vpkcmqY-|j$d{k6a3y?zAzFh(kbimNS_}#J)z3<1-^oI|zQ(TI6Mi+PNm&(1w49d#iwkNWbPdxku|4xbOm(P8p->n&w6h1GpM{Xw zXlfS-&}ZDSTFa7_zhOVy&b&VXiF6J1_2+GEr})Ar)1%>e3E;g1b*Tqb#PJR%Py~TD z)5CKHDyQ%86j<8;nTnq_>~iOFnxBJ%44zSSypvwdh<|9zZegdcu$p~E6hN@!U#x~wwJxJR=alzV}4r(Q~ zLsf&8d?zb;`M_yVMXG^?9}55N<@mKfL z_$oJ?H%&F@AyQ(0K)$vYPg>a%Ehzw%8qZ5U?J*#ESlI*j^WGV6*jA=h_7ZD5?vr3$NS{@1(L zMic*~b)_4Of4Ne7UHC6olp0}(kWbc!6K?O%Y9XSCf2Fpulx6czcu8@kc76Vvh~7jAj> z>>1>IfZ1UnCnslQWTdJ2b|4)o6Ax!4QtktyW(e;tNHGAmdV}v4Qxvl%P^rj6O__=D z?`%s`4$nXg$qN@I`GEP^gBwV2#JRa`D&8jmw+>js79gIM;P~hBn0JR@EMo1hlTjL0 z7dEI`JHxu5E^vQe-vz*^gKKrk6Bu5g=r;sR3jyBV^)VmLZHSI`w6x418N=h2kOi|M z0Gxr-WxbXnihoT?awTc`M+0ygcLO+oXG!>SBp7w#4&BEAhXSEUMhIxX zdiCn@(nW{P;SJKxjbh302)}ATN@`)D#KiOIKDOX#_Of-F3G^6h^WKb(hGE^gY|z`^ z&u3Jamy~2WbFmmon}GHI`0m|K@Fy5RmNa;Z5l;nJ(XP(Up_2ml-7RPwZ!Uj;Om(Hm zhFe$GD~e{c>@Z3^R(5T)wDgRA8?BaQhtp7oe*gac;im4#hvtgYM1R6hj3JwA&*ZK9U<+53T@uS`!?SNOm;=OGYaQYf* zYW#EkszGZfdM!F|jTp6#x{W^&B3uvC(`~G+A*bN|moH~dpBD3+I`2czB;{QQ*k`Ee z1i|hXGl1&#WJ0Kc!PBrf_dToKvKUXI57vou_KFR|QgE(9lh$rdSV(9m3gu?E zmx^krHUI7M6(tD0K`_S50Wy4(;GM~ zL%aT2r}F$dUUn+Gi4wf9S=o(<;DtTPZi0j_VE%G}Oj-00p*!fLZjR`!Tjp!l z>;MeG!IHe0O+@I1S(GoUgd4Ot^5T_+tWmGubNh&67ChfJxfxc&D0uKo0| zpaZpOvPkrQH-`@eRZxB~%V%9lx&vncl_hGse;G)FzTRGyJA&sVd@B0tZXx&m(3)^Ua;%)@~c zc8Hqy2^EaasI!z)!|P20foKCQEkP*A{o#YoGGgyRjU_+=Lecb>MzfFz(|+C?lAToJ zM9XO83lS65oS*$>_7Q(-DBOf2rK_X_^KogE{TO16Xe=!8&gY%G*nTL!nX+cHBVP54xT zwzjsMCv}PlU(=Nxdk@STmAd~c!vz)Xi+&s}JUE(=LaRy5;=Qh$gvC|G`_~Cap+5T< zxI2-I7f8a;4UF7`6HY2uD(YT3m_1~zAjjKBdQ~HUJVQW51_p(u<;25~HWrNdVj|Af7qJYcP$#(x*gL8#3J2`Lf%Us8%fAd_Q)cm(q9w)kWXAQCY@e-`SN?fd>EY1Nvm zQB*wCm3LT!qC0HWVF4ELl#hW99Ii@AwIkE0QFma z9tZR^$jB*fT3a9s*7v~_0`nMv@tCNa9M%Cx8z#tV)q?0J&2MKS5+^~eEqF5WaWS-zx_F7Y?p{W$b9PG72zc=-b z3Yt3IoW{zIBe7)u<>gcV8~{xvKTQM2!*2%94MwCX?xS2X&40X=G_0i*Wq<57GW;IE z%d%!>_A2}4`_giX+kWpwCNlNgGIax}et)Z8XLue}m$RHA!|yl!41}8)e(D6=z&j!E zL(zK{Zo1KZR!&jq_nUZ;n}m3E0;pD4L4WyGd(B{ww0g|s6s3Q^=`}Lw(yz4x-u-ct z4z%>HS4U3q^zS$ALT);}OFKYI{ql-sA>hg!ylITce*hL zW3FjDs7rM4kLmS>p1->_M_oqx`|~&%DV1ADIgKa`ejnHyui$xxk*}!BSpK+)9&Tdc zJVVj?`@r6~12?r@ze8OnhQH}$XqO4U8`MOJi;IIQqjJTL;D7^+2$Un~Mf`4K3cx^* z_#D*9VCY>`$#jVJ;uph)#Us z8Jv8)C!S$DIo1xIze}cb6YmSj#ldL@0ZCKSE;yc`Mm<0#oggzA@;I*)tvxmk9vgll z4cp1P?y>7Uj=)b3sf{gdZSm33LJLPAm!VzJ8;m%a#AkqvS%etnlw{u1r-Hu@wn3Ow z7u`WEq6cd)PFD^E@23y^Tyr?m7yWWLj(kUq7qF&4a@E$?Pju`?5d-q3?-C_=1z)y9 z`E5r;uHJ`Z+w{SGMLBzhUq}DYk1vEgj;za^0KQBEh2PH32jZjU-o7G%_lm4AKdfW= z_7?yPC=vnTqsB|J!2_cS%))7E6wRA{J)5wX@%RiIjN>@qd!Wp%)KCfU)aWaiq2))^ zRQF{R3l9wyXr+>z^IbiRBb^ag=I8lVP+GiZx`J}sT)yZrRjry=O5|{u5??n0eAT*U zSEQN3Pu?0(6VHz>=k4|Xz!uQFBS7&L(pi~A15WWt}FDKQBV{qm@tuFs%mEna`r8wwz+a&PiS=I?41CYY>WrS|w zgIgE~-6A2=y<$+g5B4wc$q1y=zkf4rYA#>AddFV)3@_NSmH(DRxdX@!Me8OwE)Rc`Ay@@hYLOpVE=(Eey|0^=S|i<0~E z&#aad!uGzjp?ZJ57541o%M`lnZ)u|0MeGlW7q4n-Zv#Vi@2ZQi((O27spK$41af?> zoevyS=%Gs==;-NzzNj#FFF-?}Y*=V$=wnHQJQB_$(=e;WzyQYYI2CBMW>Fv3Y+pI3 zvV>iNdQ6Qgr;4!_pe*KC!Z$Wp^>>TO8>wesBt9YCr`!GyH9MxUr$34P+^xHp)9f2j3cOqPY+kVR@5jH0jE^0OFzFJbJDk<9T^` zZeSL5Az_$fvC|w><nR^ zts6H`K8+(xX+m38rQ~nJF_Rt*)Ju16-~QCvI%M3d%gwTR#lZ%=#;Zg$H@UG6D5!j+ zt*Z-R$_)Ky4m2veGO>=hjb z;SCg47;kUyC&MIhf})~sOAGG9%3mNm3`!(l8+x%-ch>kLgnR)G7Y=viReMs+O_wYm z?M@Rql}{0!O-<|dilGCIWPu3^3-3aluzY&iCQ$qt4BycgVkwEhGmj*tA?|?$Uyr>4 z0s>IjOA;La<{f;Z=OIb0!)yE`1U{hyt#82!*VUZ{7?hjcsoRGuJ&_vjwZ)qDr!HMu zgi4MJSvCmoEaW;hvRBO@70Mw;(?~Aj=#<_-yhNY8mb|X{6sPOH>g?&1q=JHi=g(ar z-~d*5dTMG-T^)F#v2Pn#c-#RM>X?$0l$4!40t7fv1Rer5Vn7}QXszp)K6WDX)+b|y zk<7YK)8pS^z~Ci}0I#LYSLEFHDmsi_1UfU7o>kl}M|1rCqemCOu>-MLaI%9dqpz2?e)K?h15CV)%iz;qo^AAz5eR(Hdi6vLwqCL?yoZLDi|fvji`F(a={`-K zcc&AM0Bwzxg+=AJJ&~3WA}LaE8g}moRDCBTT6g&PfFfIT?S!&&lxIh0G6d}1PEj$s zbzX+%_yZ{(Ay7gS*MjF7R1zh!_T-EG!^+R_>Fh=*XNsL(+6BcAM|hF(cWRwfRD`^ zVoM_e_bGUWSvV#xG(&P8;7UhfZAN)SbY7>xtt;tX^!Y(fV8^i)g>)xaH`k(~#=%l- zhn&pXVsZQ0Kb(7LQRe|QZSD7$rrVHh>9fcvhR7OxOQYhHNlouAI1m4iH`Wx&q5BQ`za+jP|g?{#<$KB>xRal zVXLF6=*o$43t)cPr39i{DJkYNo-@kM$Lr{M+An)S2{tHfat9{;Ak3)yR=0`yM2|sE zOH@@_Ru;nbhjanZ6*9wd;8>m>io)`HFLgW&b)#scuRlyJCytPS8le>;{)1c-`q}!*V0RZLn08v&9cpxE@ zHm*XPIM?lkEAVwC`5Z%vn*$_gWL%$udf84JfFogmEq?9O2ov=6b^Lej3E(h7b`SME zD-;~GkD91&UwXw*4e%A>k>89yac8y5J`+$~15WZDB;)rg#vOuPjc|mYo+iue=|Hgw z0}MHTn)(8iPY1N?Jk)C~>c0alJ`!FtBTz*zVdw?%EiEmrILS)|nVCN!`?UySeh7^d zKxi36?M>z;zSS1n*q09=C&O`@+kXGI&`oeO)&LObxrAwnT7wu~Z10~L;? zkpzNQfv9X@F63x-T~e>BtyP+e_rqiw6%Fn*u`8PCcziuR-tf$sSf~U%G{4O%baH2= zL|@fRSZd9f=Fge%uF{TOAC?+wR?jmn&K4-Y|NT?+l@vWidF~T~R5xvS8w>?Q(bvP! zk!NzEp9ct{>27w6_b;|tCEBSbzVDAKPIOXjtY&g~-9P4CBAGbu?PZiToz|Y=v%GXc zjs$%wgOi`->yB*)DngS=17grrG5-hzZrN>CrDh^iPeZ11l~(pLDH_AU&ti%C%a^`G zj@em;tt`F9g!ByOVDbhq2nktA0#v66B=Zy}SJ7%ctRfF?(pJ zcOJb$t4AMmIk-#k&{j2lV2$a1ne>HqF+p%W7mz!LE29`o0eQF~fGf7bv056SHIi;hAU zFY^pxuij4f;LtD$DPT4GSItBS8fN{E{nC403X~#{zwvIqq^q~DPfS#FV(@+4)i!M% zojp`kZf(gL z5i5F#lan;e#O~O!Qb34&d1e#T37C-rz>Zejj*Pr4st~@9mVqHm9wL{Kv9VAB*2&?B zWnD`GSey40$T64p?eyD{Ue>)WGb3Z)Q4eQ5y%vz|{Nb@s?nzC}mxb4LtKXGHQy}z3 zT%_5ivd}J*q!&s2aW-I*1nlVLIB*`E?L~MnB)v-C7#Dd6C6K`10siXl?hdHbN=&!i zWmBXmJ8&KDKPQE$LOtDNb;I-4_WW~i4?Ja}r4@rLMu5b0WrGV;%434$rkS0W*8(se zSG0q+HZo#gaVvnT*c8IuJUo)8rTvLvwVa2Ny{sufqyVm0(6TcgOWiBD zTWX!{cDbt^^l+0y?Eno)HZJk5sj5l6P7krq)!J))$uY;6b^If#_WH0g7iQ20)QMA}$_Ma4g`l zLyx?oZGGcC7FcC^g{262U?a<@Dcks6j~lK3?A8r!18|E{km+9GZvG~cR@rE77elC} z1^kqNo0jrkICdGk?`pFCkc?^Qp@+IAX9HE{XUDB1wt+Xm-z%hfQ>|vcD`yQv;k&LB z@^$w0A%$xZC9C)ny*9CoU;4ATXrCQw$|e>52VVj!s(R)}^WAgM{$ufvT_O?`4D{vj!+{`Cw=kBOUv#7!5u`ToALqE?L z!KvX6;~|fQ8T99@bk2HJ^?whrvrqhNJ99)ne_H<(ElP7X%iKghj97hZh#+rcDMxT! zU0tLqHPQ1q0cr&n`>CPRF|x7cj2HKwRTX2&i@i986IU%g6F$ic%y zO&{%f%dg`6o^q}By$cn=+&USGCr-S)SMmDwu8VFeDxodh2L>T{pEws(Km^1vFM%Y3 zKN2{Bf@+&Zn(rLL^#wBFkAqWQL7|-1oMz#Sp59p-8}4O45e|+4fbgcJr2%$N)|8f# z((zmKeihNS*4B){Jv8)Nw`_TmlM}%t75*`1)BNmgS$Y9cRU9qp?tBM02w_V$yIg7S zla~1gAYq8`Cz0@Ramg4%fW&8RA}!`wGOEgnML@u*z0f@m;U!6qX>4q~wLJoJ`0!!1 zDj=7_CQ`qD_l|j5rVXf9+~bpRONDO)VnXOv_15kmzMHr5Kc>=7ZQaSDec}X}Q6~>^ zuIhuRSL)j?J4j5(j*{-?-z`rJtS`E$Rb8Y8mlo%qEqsin_PDkEw4q_xMEKjLz3m=u zZpU=LiSxvTJds(*qqlQ-|IhpOb|c0GpiBV>Smo}MCuX{1$H7(wo{-i&k&+~23q3ET zw9f;Z^HoW*Z&CzHc<)emck|tZit@DhLw=G=7oiUAOA z?L2ymHs+r&Q_@GoWGX5|u)sI|s@v1jdMjF60b`)gLx*{$4_`q3wD*}N{OQj765adS zd5<3^m0I5|l^=TjWZTVUp=7UT`WWdIDQ5VVA`cD6-W#Wnht8@W+I?E*!(gNFp`-># zK!80-Pag~KUR_z~?&+z#eZY`P$Ml8F|Ha)`KxMUk`-&ifev06yf^5=sbyq%4-v7Jzj`8jpXB^LP?{Dq3 z=9+7+nZMb*Tz*C~rmD)r+Ij&>^}W5kK-QwYo)j^kK2MbWK9CLFRd%J)#hNiMJdgFZ zoZi)|pQfe~A|fKf!aj|R_}qd9Y*&<&l(e;T${BBJeT*{58A}g%f0mq#4A`r@ynHV< z_A$(8Icm)2k=?yc+dB|SmH1%fW$R#(69j5YJyfO@#J4=fZF zE)3Gl191KVwv06f;;In?Va2%ffb2i?06%yL3~IP`$076OD)b5(?#6-9J0CZ2Zh$fc zz`CH|WN!{3S-X?(m?DsmNsiw_5ga-McM<}G1VH+qk%mPx{p(-?A_ z);E3!ZIU<8_W?>Ny$Y=!7FyaEe_rd%WRPH1*kW0=kNla!8bnu zO{f5|3T~&epds+JFKrol1xnK(AT9?GK$c-F*PK!uM2{=o7K~08#=}V0W zLQ$b2(p&+N@fGV>J$ytEqCmzKL%Gq zV0Z}CUZk`UyF*4ageC3^yPSo?zBUE*rVJlyqGt8;v;5J&4?GEaCxCdV^n-c1_DW7x4Clt=`)ovlp-`-#$Sv_g^ML}0_XB!_-9}`i1Lr@cp<3k-_hq@aMtmK~J%t9T5JgvoDz8R~AA>jUr$Uz`*kC&NN8x zWa@(0sqkNkiF`{%B#VxS2%u>&WWE&zKr`2a8uv$C_!}F*3=~%%+>@3DFq+*x2bd*J zW=a(z3@{Of8+H~cIqH3p2I1yih65C+`YhMWG8nL;Uzh&hGfDlH#O-Z;Nj8@y?ikwN z<}lGazK0^$Zr*HDt0tDBgzVSNfq6HYOmKOY{=F#8w?41|Av*WkBbF_uYq)IV77DO?>d<1>62G9gR?r9X7w!;A_9qb^c&sh+A zn)=y=Jb0CZPmGp2IBxttpH_6!FUQj0JLs7V@`;dbAu8g&(X(a+EjSRzHF&xiN-H>J zHAlTatYj>7v|UZ$rnq+^ss87zvKlyD=RUox0&&zGEfYwUj6#APl-+Cn#6XlKIVoC~7X8lo#;IKmv% z8WaHfD%Y zz6?p$M;+yi8f)vIG}e`sZ#XuXK%}O;+Inc`0D&0h&!u+*(TX7Lz$XF7!MuUAuzqfA zi<{xY`^bC~h4zT6FYE5cNN#UGcNaF;9vB)L0@FBS+n}LXAYxmwIq&*o{~Noa?iH~+ zPz8XrK(ehs#7DuFG!(UXRS2#5@}{u8fZN~}*(Sb;sC3^7v&YdS%z*__%qZXshTX@~ zS){EFglga-k+bDCh-KpPUhnX@aipdCn8$`7N4p90_KVwp{pgG@#G{5qVpdqlhAXv8o zIw`45?rHF(0%^%b>xhkm0w@m6bQpk~Nr@x4zOmvqJ(OX?|Ca=tG(ru+?L9YC^V!1# zyuFZYF!`OSp2IwO-E^+%3JARTy0G#_+d-?JziD(z9+DxcS(>{%hmoCIv<4+_=`0xk zc05VAzHmNkcQ8yUMdn;UaZjdizi|c4nl}d84iTMk_FLlw27Kr}dkSVebT3u({7EB3 z;IRKCjqq2`BNrd<{~Smc_gVhm6+)+*Dx(zjw=UEk3rZ zdvK^AUko&wv)YyLD{ynUL$rgfeD0Ow6jFZ-VLFUV*nyXpb+I;EueqiP{tWsip|Kv;sPmu28Dp$qV(%g9)%%h^KV5egex(QB?b#3lUqEC$+4_ zpVIcR0(_A~XgLN)w&~V#z9<@tR|=*HVZ)0SX#ufiw`u}jMBN|d^v(r*m!8-@nhswA zN(3$NtR1~szOe8<1Ljat-kIF0ErBnw2KS2HAq5fKh25lcP^CM3b{S$ywOX7r(ny>a z0s3>=fc*N}6Jp;K51A9ZXy|(#8ol{7+91sQBpzR_1X59$UoTUe_c_1z4$u+Laocnm zD->F^R(PUrtp^V}^nX+dk?&x5j{ETA%<4@%_LXpua=r zU_X?+&EIMukLe?gaMD~EGu$3$n7IvqCsFZHLGWu~pMk}QR1+TG3f0FmN30ge2{!@F z?X=q$ENtA`4mSWq;m@JKnx%Xy@+pg@E9FfSRKP)NvNWYfj?I@^7e<(7?z_yy3A@A~8t}H)sytFeSknqcBXs-5auhVLWq2!aq0mdkjFF zI)T*xYhIv^yVXMrTa%UzAOM0h^L-40kK|J|ORS;&0LQpAe3zXK;CV6V$ zeqEqrjv%1Kd>l#if+#$6C*now$-nbcctRp3WhbxTC%97^gin95MN_atne`c?#>nxr zste!?dUi@1u~z1*ouC-AVNTzGwGK)6ZTt3vlqH4Nboe zMHU;Yi=X`*Nmh!%Gq8ke;@oGj%{T(x%N{*_8q6>hyLOabHp}4+VjV0EUO#x?fc_O7 zb#>yE`jMHFV*wIk8<#!{&2!b0B_%oz2OKhQ6-JKtw8^JFZv^ZHa9QJBsp%PHa^3(j z$)`fT$4ldq?62pQU$eZ1~&EU-1ys` z=6J*cd!JM5j^p+CdOoDpCEe>A^8faiId8BU3I!iv3pheJ6@C18snqT0Q&AUrmXFL- zQ}o?_Xn=1Hc=LpLhp@z@s?Xw}$gw^i;jqQI`Ty}>%Avk{hU`0c)NZyb*z>llMD;le zT3xD89!}jY|Dx?VDc14tEQS9o)~gkP9dg~_+?4@B-$8d={rvy; zO#Isk{NKLQ88R^xStovzJ>4UoO^uD1eSV~J18`H3eGa(V{!Y9E+r?_XZ^)^C;TUUm z?w#p+`Vou#kY~p5VH6n}kNC9#4r+vRFYW>{7PeE6=WQX$A0mU0Bdpw%@4tf zgRobx<=4I!P`R0JIas;9tw>$!eq0S|U-d%v%c(k$yWsCj`h2*{NQ{SLwAmU$aJJ-u(waQa;5zw+H0u0`+$ zp}<5C0nQ=eHYT(0(ZdNMn@o~`-gsniK|?{{EF?oh5)NRvV=b_vp z5-@n8qUt_;0fn~tFcQ#J)3SRH4fcwmm~yXVg>`cZ0MtE)%M5|r(S{VcI$LPkH6U?9 zEAkf6UW*XK;C~%P4KCPqh$yeE%qA0B36<8eKLCYw{;m_y1D|;LhUsyL{$v9H0ko7m zDPTJ;qHw+4kPQY#wLP7d2&FN5& ztA5D=d=dbhux{Co!}_LV>DhM@5m!e=MMVfmmDA~-k*sIE27pG8i{rd9LtcD4tp}7K zg;%+DZ3stt|HE+lBKAZ9dpOrE76Xp5i2dxX^sFOw9EgQ@^su@+#v zB%vACKqlpmy{y zxquWfNvG=#)^e~T16B3H`UA%72Fq{qNxNaRV~nZ($X;AeGVHhWXR)L~EtJaCA88 zix^^>V* z-m9Q4Fa{9ZizN=aca7-c1H;=QQVkS-y>Y#9HvMCXQ>D4HV*~{SY=S1O8#C)7Rs7s0 zmEV%LH!^8GAP#{zSPquZM$SMF#f7_cS0}oo63&DB>po{bre&i;H5%GcR2bE?CFHm+V z{QYsbL_aUCT4f9S<@@`Y_jZt_X-Xa0+mT2^d>+a+0;$IMB_j0<6<2Or4Ja-kU-?3H zr{W9QLMp!mhtKY88EZ_hMDXICuz5G*^nixyt=RBpjjE|UA&UCvY}YPNl)liWFO@(( z!$nBP#`!cc04*l!g~P9e!{5r3f=YcTX^CRHJ9azJQx-?2#$Ws>y%NqQ9?^(2;fPSC zKUdJ&nZyG*L&z77iVeT5X^fL5T%wf@>x@m2+#h~!Do|kUA?h<IFsSeh2n4I2z)mZ{L(q(6_5N1$Lq^s<25%(E)t$WMv0=i!SSkbd)$v} zT8k-}I59Y9g2K4>0Fj$Ig9ek=H5`2FQl$$dlIoFqBbjep(-&hkx@`#xn)e0Vz0PFF zCeao^B_^*H=!n}*k1VFFbEHxIKr60VC(mWQ=sK$T)tV3LvV>^ZKloB(p4@%q3nccn zwWQt&ZO9w#RPBCOIOAzd#lxsFS%BsV$daeMUZ`E@PB(k-igiltBKv9x(NzV)NUC4Q z2+<_tDLLPL?#|UN@7i!*+%9FDQ?Op!bpKP0n3+g5%``nEZAU(`Om!>m;q7I#7(P&4 zi%CBq|BX!`W6d)eo%6kAD2?Pu+e0d`$Hc<;dKk5En7nW?Ypz{0j6nD(wPWOBJ0UVz z)iYvB?yD;$5)m|!cQLDGtU2o>PiWLrKG{n3b<_t{CqlI`rnrRyy1QCryHfX%*}zU(qxX2H2LUnGIIs9N2yGfG`iSZr)Xr#=Og}lqVB!W9E+{@}t~$mrH_?V&@kp zkzaIhl|y{+O#}nL!i|7G!(zc8atP&G30#LvR)Q?;dk%H3YAw_G@L$#xfcXwsoX;U; z2{ME4agjxTfEgn`~XD~M=L zlNQHTcr%V5CoPJXfayHe4jH(2c!U_2v*P@@?x19X5XprvKZXI`*ysV;)6UZyvCHO)6j^_*IJ@`&22D=r zExtUU;cz_1?We5VM0dz;Wmoq4m#nXJoK*@>b1gJbwOQZC&AB>jWAf3C#Os zPccVx=$oPNiG7?I-Q#ZtKYsXN*~atv#9cjf;24^kYDZ+mJ;R8M(?h{8M-7~VwaXZM z61II0H(HUSQZEZ-;$abDc&aG~Bj9F8O}ww@ItOd?G{nGKXqoWFn9eiq9MO{on4>w%dNozl5VglJ&dy$ED>CSPvIREC_zDodbWI7$XifDJf4rIoxShyHJv^{I;m% zF)XC1TK68U1fCa04yo&6c4$uRadCLT%Sn!kdgg93GG$*DCn;A=n2btKXmPtc1@>+} zquKo^-?aju$wsuE{65l}rIX1tl%0>)ul4soHLYhqmdebn)o_SoM@*`#A=A?_Dw*L) z=}a)PB#+m34whvi7(ek@vpY!VEUxO=G(52%r!o*IIFgQ1)4BQ_SNuJ0M)zywk{IaF5O7-mAYJWR{b2wOhnFS+}NF? z@}vORC;S^%vy1oqsSLHwNFmpYXKVJ_U$Ob+dyWW2FE&}NeXeNYplxFM`Cbldf8y_h z)pIrX>b`rqbR*_|gM4eWdiB7K^FQaMdKl&>han7 zkjdhl2fwTYLt#b(k^#6;z~G_)taDO}5mTZ!7WptvR5d?neR_#HcmD|EMTk6>F^rnkll_OEf9@f~hS8 zzHLZk#y?7*HYhuZ6i}UD)9m@=Uddduy(n6v@gfFLr|fxAw-VZWuYRMaO2u5Zuf`tJ zrWr<{CZ;!T#7)m=YYj)6hCT8ATp!Hh!czHa3GW&lbR!h_oIN^HHWe{^;bl#J)(q?Q|j0fWv#< z^660ARLajHcZem_A#n#ArNEND?Il*S+~eaCjfuo%dFS8Us)>mu#Tp-SGUv$dlXyBq zlTnI-GPRF@bD^QZ9+80(m+x+8U`=@w@a!Oqt(=q)v+=|MqMI}lVaiv04{A5$M-DZP z=0j|O3jz|8vno1D*|-2&D7zAt!tGP7J>7Ped_F8Tokcni?0}p|^s)%GX5)eA`P8T! zNK}9payd}1Y+cYAs+xil51F7)vh_XJ6JDA@OF>XzyAS_tIL+(pj5S85bx4}#0Cfx6 z32{S+9ZHJqpi~e+q5!0;MElPJj5T2frGDrn>Ld1PITQX3HLO}bVl7AI}2oLHodauENkm8%$=~Pt~`B-DAOK$Z(NvCwV2alHN-G$V`ByXLpe-&rmUy zH;mY80w9280U^Ee#TGLqZrjIO8ZM>mmo3PL92s=8xTb1Ecs@ z1^zWmM0N!XV_h`$`NuHw_h?cXe3T>gVqap&)v__ItJN8vyh%a+V+-Sy4Of00B00kC z2@H|nX^{4VulmtflK0Q#Ii2T|An_%^FyBb)8<8#^a{k^4&W>YrJr@G0VxFQhWpjo8 z`jBErW}rd(ue)u=DZxWt@@g$#95V`@=ZDL9jaCn(i>{V>;Us{Cnjba9cbp7ETPfl* zej5^1p|INa*%`+lhhmCbqM{XFs^9}`5Kz|ZyO4BJl1_h!v49%=j8}%4K%k3`l$=_a z?olQ$%?R`r4{Id}!zmPIuW9{ry$n}6{3I`w+fT-a&)e@nH=~4NC0i*REMN&* z|Idw9%#HY!HwkBfP>QvJ0`@hjde@qADn2BouTJC#A6-%&8QY~g;RW+BMxxcbvk}q2B8EM? ztQrNwP$&KvN0C8#-j2tpW0@x&*`E~D?xAQI+GS^2|244``4Of{h6JEnTKA7}&d)z> zH-CsZ5IF65C>IyDLiO-v9$1{AVE54-E{Jmy`RhyVtkRKzLUn}*t8sq4YO=#>hMzZn z&Nthrq-MS`EWz|zf~~+~_p_CfzMDjEEtdW44q2z+eVueWUsCqb66|6Q?+FUAB|QvZjmm! zkctE&fg4J0Q#ai9EBe0G*YYM~I)C>@SKy=ro{PMqCl``Wsw~fM1W8D8?!F*vTCtB| z6`|Ak~xgu5k%^{O?5cX?Mi z0jx-fKn3TWB#FEthgaZ(vLa6MBCwWwO^#czS<3J!3 z@rZ(3-%9WolISh0Olava$$r zUj15s$ie`VaSt+kqbHoUL2a_L{i-@TZO#wPmq@Rz7H2r5TRt%Z$5mGLGJW5Wpae*4 zB;yv^4+1U}0HQ-k+?Ok8uOqm+7}v{$ACZW~4HTJcMwHVm>hv@Q7#Y{EPx-5syN1x+fRkqrdwsDWrD@0 z7qSZ$v(t;qT#9wcnjvC7IP>9emvt{tIJ478uqauCyicn_O zELS-co5f_ z7;4~vyJ-cE8M23pzQ?$Vb*5s5_MWYnxzAO+KBt4(J(2V#`LN7;!r+)faU-juJ494R zOZi;;w!3DawR41(R^(7gUJkf0ojTVeA_Qt}s(v1dS2%CF?m7o^yql=Js5npBzfN)J zD<4@X!jrF~5(gqbBc7E$aBE^%82tp5nMHX!&C;WzN~%g$ktv2G6D8$?c{@Nz9xoH| zj_phWU5RpNJ9j?+n(y7d@%Z)hALnF5Y?hwPK7R%Als3-+6)%6Hip#Rdh&8ouh#DF$ z1_#VRgn}t0qt55~MiIYMEaF>yFlWRkL4g0VUS_3(a;3CEiu}gPiAe*5IRZ5}%~F1@ z$eOi=TqT_2qGh~WTb^6w5F=*k5|7p*BABg{`hSuMEh2sLv7+tLjLH>_Xl)zTD{J%* z$uSgFWd0s!kGKnW2tP`cMk9K?>u4~g$dkuTU)!{&UDkOOlxf z!9EvAX0VNV_%Q2;ja=?x`DLtQbXUlkj!)V@etswgMcRPK4)pE(zmYD|Agqulosm(1 zUvv3;&diJ|C%K|;@XlJ#O>_@0{Ulx`;&J7_yhK;yA*y<<-Bx9d6ODu3hi@ECQ+81l zBan9KJR_Ep=#!xzN-_BQn4A>zx5HdYXu4R5K-S4k#XaPaL-*hU#K_4&ynFYTPai*( zOg3Y;5|}~z$~Z2C>Lsg^VaWZXub6%=<=Dxz?aIf;t&J-8YT1y?Fm68DYnX|--bp&v z1qjFmGR%K@S<6@XWVGPt#)+C`iVGREh|w<QN>nN#`k<&u zY2m6MD}^{ALeP)Ww|%S{5x*)&4I)sP;akI}+P%C6gk(u1Tfiij4!`DmANLo|TooH8 z>ZBeS@JqPw$tEh{zZdb|chnCT3?g1)UlPP+&MYc@BbWaA#Y&fFxr5s7fk*Zsp7XTe zhkqkg6eZo=skDpyGt!M52QVv-%yayey{S%%F^_TE7(_I!4jp%Y6pen#hZ)Cbf}!*$ zd!?O{&PW#TqO+p3_}YD4hA;>m{tu3d^CYD~x8Q`jGT>!$6D43vnu>W$E53XDj{X2DaT7z(uC=%^>nDCEi~zaq#5c zJ#7En``jjFuO*hj&lO)0V66*XR&Rge#y!0bI&^bqvTMWTLy5H4Upt!|BgGeT690%D)3 z$(g?nBUP%yRMQrOe0h|42J>aOUXkXgP!hwgRcZ_Mn@HngaKu6Y`Ws{m?Lwb2UjRv>tZ^hmSK1!#Q}1abocg_#P; z+AGPIeW}NgkcyUA3TiAPT`R$Ed+H(~;u4{iaI&(-@Xpo7Ij9#w4BH#;vcND?t+at~ z_(8T3s@?!7e6!oeHzKyigpZJ&zQ5-F%r>F>?M3h{?~H>5rrorm(YM4D4z&n(m;q#| z6&k;d2dtaI83H<^lK#QoEVY`aP*4Whi8<3xJ)ItVok=a=-gLHea0t*Kg2wWnRaa(a z-cNMjOUh-I$ir;ZzITRi{1e8|`xt8=3XU{ZhoVBryzCt};o}Bns`VN*ENi&gYU^a> z2f4gOyL3Fnn-!iyRbGK%mt`$Pje%VDumQ3&rk9tu`}2)1<Bw3~mEU z9{4p}Tj%0=%#O3Fw?Vb_a7gNpwVd?6K*^@$U-6Vvg2NwSJfmv-p_329)@gW@9=; z^W1^KWC-X30QS_@(I0nu+8H=(uUwyHi+Vh@jk0%m@z<{f|<5-^F>1{>7}8(Yaf%}`^mX&KDmQ_ zxfdf3pZ1%aRUG>3`k^8fOCi|*?n}$f(FXH0yh#jCs=S?k3l4fDjJ9*Tyq1KR@590> z1`Vf#xL~xHFow`C@%1==PCE{t$c@imw(MD}$W3~QwqRs0+kHlVnGuupk{)&M;hWZ; zMobcyLCQdI`7wLsm6u9!!BdFEatGlf)_noTv}tMw%xtfmzQp9E4}`Q(uEihn?90gV z=0b@h%AO%BEUtO)*U|wK3Znl03?-bWL2^=;-{=K?A4&T}mcfYF*O=fg30?G>{T+9r zhbv6}=cMy7jVtw5_VvA$6k;dqD zjV~!M&Ru*yA_aUmJC`WEx5{G3l`}DmSG@;}Vhy+q4Xg#cxZIU$kA|=Q{k>Z1b5jSL zHZ}Xz;5A-IPKiJI>b3Y1(+#g5TB9M4bh*^yV-jD}F9tg6ms8)l07pDI6)hmqPDW;!P{$N_}qvFFlY}BJl{D8W~d~SeF=;9W5Tl$FKP`sG?mp z(6r8!&UXUOsNg=$jXVe((~12YZz4hYVI@lKjK|O!lQp+MLxJ;;o@DxmW^`6N2h#2C z$~;cMVhwq^FcX!G;^NUd@(@Z{zu~x$%M__hlWs4>GAfBkcBHnPDqkdOLFU^{x>U*Bd1HoK>-O~xS+Rpptju#SFj>vON?qT*Y8y)V3{ zZl*J`?kbu$#jHR2UD~W!lqT%%wPt({OUa7@+DsX;Na*bndl+@Ec&w+%{Qy-rdfA;t ztFe`K`a)tSVW1HW&_=c8LZCGba}7w@L6FptBXtIPt$h)d`5|CwP)faT^Gpx4u%aVU zB*y}UgdFP^jVCp@RMli*f#(YY`P5~WL!9Ydy)z)Y4ge)P0k9m7sioPLJ zjZ`QFz|V~+xGzM2r@7Gqz(1nUpR2YvXQ5mxdtCv1lt{|Ro1K{@z2)&r4KJCQv{yiK1*zTQ1n`y$PPGOjqAs zq-L&p6Otg}=DbkHTR9&Le)fVEf$Vk#Yh%zs5KDc2KT{)jP2(1$Fw^h|$DTXyn!8K` z?n^v%bc;b0IzSIgYA$j^#YC?|d1orKEW>U1f z2dG%k@psf9R9JN-W>-y<5>vWL`!RJ{!77qw1j7D1=|cJCCVYo#?-4YM=4*edxGa=h2(>WSb(DuPc5W?C{yU9c;K_}WSI?qC{yv-hiP%Xgi16Af? z;5>k(l>(#)wYcxQO{X8X_z3uRS&S&rmM_ZC)Rf4Li)Rg*!!s16kRz1lQ1+GFG8p}G z=Y)0%wCIIw`>MYv`);@SueM1)5VGC%j>>ZMsfM>8ig)O+lFzw+4zzY;4sD@n z;+?${J>N|T`!UCN#Y;X`zGpv<3t6#jcfkmMn97H?^4{mCqEBjy1s=iqnU%J46XGek zjJWaT{96x?_cGrK;wka^(+1EnQO+K)2>Ih9;H(|9|8H9Y+xw>pg1+~Mi6Oj(Bp>{? zabUUrX*b}t9pfKSuXb-o%%&mH{NIQVXZ8;ag}nAJP8#eVxWBfc|1YGl_K%qSknTqi zYX6)q`T?pno@WYh0{fmBNJS)rVY54tpJb7~wkz162=F#nQ2Fkpev$|>nE-OovE~r3 zruCas0N{7rh}da&{XZAhsO=jHHvrTQF|1qE`V)fj3vLw4U3UDr)ZV7K$5*nE3S8*6 z&9H5rjw)VXhn8OX=EV9%P++Wt#gCl6pL2u&GEz&JA)mDN>L7d2NuC>kp_E7>iM6|9 zKeu9ytp++r0@S{=<>8ZOzyufX*0gyGK6AuV>gvZ8qz`8gb9jHQ4`e$__g#5QNa?nN znldmeFCMT0$clTj5Njo6ulzm?)0G$9T#{t>`&NFJ@W3vO|Jv%`1v~Vpm%*CZjOd0) zEg(BiZ4tGW5kLIrmyMiS=OBa&WQ!hf1-ykANc{$}=ni)_Ad1e2&j0ai;o~;GERL-l zfx8Fn=awUK@Sg<7YP4bzn$`C?|AX)UZ|{Tt_8%R(E^27+41zMU$5R!p;){j+UmF+@ z$e&hz|HXeYtZ*T192^FmOrS+P_*+_*LykQAyUV8cbK|B_YIE=j7BQoIQ*1rkINTICG_FA`!exU{E2moUw zuVf2Bp3K3KFjDi0B${HsKK|&Tkq3H!DjX`b@cIyMUmmy&0DuYNBGB(Jioc&cCGx^J z)Yj{i+D<^vBqYcK(Ge(;{R+i+{EN-pO#^3Q3wMGJ$yTrMBlQZW0I9R`>+_Em5a)-M z$SaW60fV~1|7|U0iryl^HFtuBfCyhgc00svKy@lBC|FF3kVi1yV6CJu+%G|;>%l`X z-A4&OrVDim--5VX93dZ_nKvRpgx}T?k~dmFFHD>v44;^okTG5yDY1c!#i&`N;}tln zoQTq(=V45lc^;CfFMAk`)X5ePI76bLeLqwhAVf-rOpnI5(6yE^pcC*E?#VprJLG*D z;fwsz&2Pn3Py00C{IvOTd}SC_x)DU<3e}`0lasnevTTdvVp~S3um4 ztk7y>dsMO;dJ|FBGTjSJ9$vU5srKqZ4yA8kr8 zupZ#m1cP`he)&KP@kc^S?+AKs@Guz~XQEQIkgp-Q;b|3g`;W*(1Iou@pG|9$O$$E+ zN=M^6Ox3vdrL#wf%$6^@uapfQBH>DVciQU|53+lPZRMykfsqVUjW4h#5X=CC0@5G_ zib@&>$keSt`kUpvFa{(A2%E}VDEWrJgr3?R@82)KwRB$ug*B^iunORo9r|ubEnhl% zTXh3iezQquD<61$7QRObr9$i;t4;l74wgN6w?%@ELmo$W8p=g*q$L}rmQ(u1?Cg=+ zzq$Qh)#P1Jf0Ky7^rY=k6Ogs;n}Y&h$ZMH|gdG4rbnMmGPr9`+CJHw*3loRv#t=cL zrFBUFoT8$sc(a^G?%ZR@oJPWypwD?Mz+b=kh|6;02FkDR^#er`d&k#FY9u-34jxe0 z0)WDgwhk+#q-x1rm%$xDs`BS*2x2Nub4~<-l&OOqYX+we%8Yb|eP2eW zt!P6H`vhs02%rT3%BD?)YGL}qlg3t$twqYsajB?TGAK66Z(_ToWD8kI$iYu>K%~gm z$z7q>$l%$Uz&qQeVc8ERJ77n-&DWjC#6C9O2@+Wd+T#uGK_)E7h!I0tK^UrYK{OhehxJ1<@bG?>MKcTg9w1~JNh{e&gn7<6Yrvdo{ZmhSw^d-*}K^RJ4|F$d-!jWRaXWn@fA zkA4|_x#Q)#Ir?XsjD&R4p1j&UZ=j*Y)mN2%XMJz=N85c_c1h|Tb0C~)UYeQC-*z*x zH(%J^TrZ|1B#98krsrbAl{M5w#lEg4<_N#dsY`=xP<;Wn-ZKCdgmsjJ7+&D`{e{KQ zyLTfh2ritd1J_7--#zX;X($ex-+IJgZ6=vZgUd>UO}}Y;+zL7sJedhrc=Y(Ofc1#~ zbj5=g`*H*0wvM$_TU+7MZ#v?gJ_Iik=bb^tE*VsJaOie~VqL27lo7>c{ZBkF8=IYF zcRBG|sIacivq-$!^}ICR+uT#QPsp+96^Zk3P_Y;Es);ysiSA%ss`ivY<3iEdKOX4L zc!G|#Inl{5`UODzAv!~XMyJ`Rm-SZybZ+ssxToKhH3UZpnkWEPPXT9%J4$`8&Hi$z zqXSn{8Ea5zXfD*#$;h+|-2{|b2!UDjmE2Aqtn$!NRrSTn%q23jwY7!I4dCcno14)h zB}ZU@&i$@{(F3j(#!Dg|TSya0cAZLH>yUte_OsI#XpV(@^Fwzwu0^t~__llDT07Mz=+yuUcs8#i2oK0KM6fRb4Rq6Cj403Q zuwQC<`39oacjtmXh#%XyTKT(fHK2h4pb-H3L$P0-9NA{qyLZr42dX|jU|s-=t|HIn z=j#iVd`n9coj{?Xxn;;-6tthNUO?bPd-8@L_5+ErW~Qdqu9a|qKrJ9-H(tFuN;ah9 zZBX#))vL-%k(+byvX74si&_pH1A~GGAF=EE$5)7jpoeF}7>YPo4)@6y8?54@A|n%% z?)UGZwpx#f4Z2!0F^Rb#xV!B%990z+KHlnA0NElCngemT z=Qn%3wg~A|<>ckBJ?#QFT^9DY1be)vPhPD)_IvrPH$~zu2lsv)9NqVD14EfEz~Vxt z%vx?ko#*zzLG1gDBUojw2ZDPE9z95jjU^sanNC!RIs_H8s!!B!Pjhk(`@u@c)>1|Q z#n;GriC(wW+km^!8PJ=?tYdf&78B16y69Ca$E}KE75lXvFnsQ@y#@OIpiTtWH+X3nSd z)UArU0Kg1w%tamN&Rz9#2D1BKBqb$XTwF|uk1xmXS^Q*tC^q^0_Mj{OC(!wZ)0MAW zu><_Wv3mffH(Yy+p%h|w6%`ebLY&uov(c{u;AMe_NQsH(kR@>I@&KXP_oIN>YYDkB zx}fkkI4B6Puq&LG4@k9yhld01bga;=jB9FYen-{L_(qrjPG_pAojdePxe4qQXAgU8 zYg_0X<@b^#?<96tO9S@(*jPrWJOw>uWQ5KFfO4U$1q-EMf0YO2(Jwtc#O{aj@T&Pw z>(}!?6{vzPCy|lpchCg)5p-5vW+wPCsTbJah+3?5${n8p=1W^!+k_b*hwk z)l~fk04b{3?~M!iJst`O2$ZI$OSHDMQsA=Qp1?l4W@KbzZ4G~YO+y3ftQzX;7Zw-S z*Vq00{CH+Jra`oocyzqAmLpHqQ9Ulz0;d@|!aH`}Vk4oX1e5BzkT*^K%$l>4$nZz>cc513VFD~Y#rLEC_Cgi?B z9D07X;7m+2tt8}93WyLOk0ljnB;tY??ruLB|~nFiU7 z$4{wlF#2Us9q_twDL#-B0m^v&z0{(JEQ{ZYYNftuG+Pnn+ava+~-JtQ(m(_?ip zgQAsxw%A^zk}`B+pe=$dIe9il%}HJy?xGKfgCYt=f@ni31}N-3doG7?f{WrDKc;uX zC^7ON@;~;VKV%5-=i#JJgG^e6Vr@1sn&!dO^Vkr8QBe6g^>e@!Qc_Y33=FMFg2fAW zp_2q$AOLxzzBUVXy$H0b2blVURZ8%_bp9&w{p4gsUj|VCD4)-`d`(PD80hK2zyuVh zbogZ`{1X%;D<@~ZL(vWsu%#d>5s*;VCXh& zuYRp;=KFs1`Z| zK{mjRGjMiZgS(D}viwXa5C#Nd$&C2&yg`SDl5x2(&hW`exS?Y}aRJ48$ z=5H}jz6#1Xz%@K|ZHAj1yd&#?mtaTva3PsD?DrV}*5>b!pzm6Vw?OrVYsYz$b0S~i zpSY>+^>Yg!jo=yz2@XE(QN57+#QoA!Q5E;VHz~g%jxKZi#%Pv1BB2}5FEqDwwhM_q zp8JgolQyWF)!r~(95 zq@xxBWk~UGLHVE9u-TmD;cyv4&m{1I*~h+YVix@;lInSb5|=-Hte)fT<=;S8gY@G6 zfUPS2+q(WhTfol@ii?Yb1endQxn5s>2QK{+J}UR05KjO1ylCD@Jiv3Nki6<(bx5)B ziLV#?9_)EQZ$SLOGl}nEzxp0eJOg4+k3;pk_GNqaJon>7RU0qNpl5LRRC~_hUOSVz zaCcqiovSo<5q)==H|pJqo?sCR@3(5Ty1T%{uzd*htICNPUQ;V9)>e@U+tg@zB zr+RfZp3S5j3ztJcWp|)scL=MBTK?`(QZaD4)N#pl&AX#YK;(RD=)h~D zfg*3_;aLpsEx>mEnB+TCX|1i(GmG?%%D4)v|r$d_EQUHPU(x+Ox&w()hX8>`ziiv~CDFJ#E;2rO>O^YfE#1b%E2 zA9+vHx@@kftu{myB-NBW4J20oZRQ^iTrc44+urAz=T7t@*Sv4Y=J{Gzm#PGc^#@Qi z!psN^rTLvgW0Xl%r2~guac~(~o5rmKvuqmN=-<9vb*y9I$=8K@q^|9j2AvMdq(bB# z^Zk#tLjJ-shGT_b{F)|+PcGO!?Xn~&brH-d3ev+3{%t68yhk+$-PClf*D0!tGAvEB z2tIEMo5-{#{e)5lWMRz7G1rpTs#>&ZRnwX(W+C^R7f{gqblvVJ>?Ryi3|;!do{uMs z*_Qncm>1y+MQ3TP>7lpm1zoepC(Vf2-82OIDE~0|vidwMp|Dbl`fO2) zufZKZF=lr;BL6&;ZEY1K2)G6=3#tno{=B^H^LeGX?VoSbP%qDKwc=V~ce(2DZi*-< zD3p;Aj#Ww~2Kr}f?k>>o@b#}O5-#M`U6vo+J*vvwOsobi_x5!IWc3b~GG=*p;@37y z2X(eR4yIdBc2a$~{Y3xy{q>W8x{Tqz%iWTeI2EpX(?Y)H$;_qdblnumfEF8+pfQV95vOV zUE%!xk+KOGXgMxv1K^XENr$ds7NFVy0d#Uj%OdlNQGeQD}ufw1!)&TT^bo`!` z`b(5Z6G#Lu5`qe~_4RAZMf0MVAz5G5)EN1)icbNY6tpHICnTH)G^)nt7tnwnte2RV zHV|TZ^X3ht(L~ov1`%@_HKOAr%EnHRlb^=^42qDrxHvT3`LY!j8XUv*DKRne_~~mn z`}a4=4s5!=D1bzuEsqLYyq@Yq6mv*%iF{HiAWEjB(7b>~Ao!btF`C=6FDFhw$AYihdZm4G=+I#j~_1ua7 z{C_V$!v^enOE7HvJSbnE0+dB)=L4z4bDukpq0!(tW~8G_1Dn;o?@8$FaOwGKTxR=f z3Q_Cn97uoyU+}`L0@}pTQ0V(7Z}7B?e=N>z5C`OO(1YX2?5*Tqb0yfp!t>z!Ht6Y_wMV z$JTS09HQM18o3EK70z4V9Mr1AQ=g6B*Nk4fmOY0B3UC!*3$Vc` z((KR$EEU)qJLm=@B2ozlspW7V1pU|{^mxqSN@Wso^&4?Gnh}IZi&P%7t|j+x54LlT zQbFuI>GY)#!0`e

bC`A$wVSTIg`f&E2`2pl*Ghjr1y*1aQ2Cw9WUPy*z}?#>iL# zXAJr^ApmC5*ns#iVmls2Ry2ckDU5rI8;YHPvS5LCWho+Oro|GkQPKj?hM{FxfHGG^ za3WY(fLVLb;2M4LLI_p5-E+B5MZprobO#wNaW+R z`6AmJe2Da{qv$$h${VRAoq}kFLa#K$fC2gRlTgfhh#eexAUZf`oghwKHby$I6)@vRp>Qwq0w8xG0@sGvPF$ zwU%wtJ%ydxw*ipIaMI@?jQ1vsk)Q$qT)AlYSuVChu7&FLjJEugX#`I#FWXBXv70$4{Biv zGUQla26L{zC{~Ehp;5m@5|H2-kE)GPcfaR`CJt0bGL9B4y89EM>jFUVtQ==LfD^93;63fpI8uFrFok;MBQ+ zbP) zZ(^##DZoX`(0RSvPTNTPbVREX?(}>MK>CSHj6i~0l%hQD2K4=Cl6_CBOIe20W;pRO zeS0K)e>1SVY`KSXH3Je#=uGO`+uI3*=BmMQmt;2MObY@|wcp2dXB~K?mFHE zpark-EqPx&tlpw>ih-Z>W_M1~VjHCXv43Qjss>{WiYvp9UXP0h@mT=bBrUpAgn6M_ zb~*zuqz3Q|IUIa1u9#E}FIx!jJ-jw+oe0ii*qnR!M8AK+=GS8Xb_bEFBD#+t_NmqK zHZhTm|NpS})=^b%(c37cq?8~?m!y<*3rIJTk^&+~cZZ~GP)fQ}q!B4;0ciAF!U5PdE?-M7wFSK#LT zEc&_X7zC(jz8GG0qZZy#zHN?*hn!qknrZ2~{^bf-)n>K@5Cf1=7?vh_&JV&3T}op@ z{=j*)4GpzNMuHm7WNe=R7QlR@T<$YApa;jC-9d=}_3$PDM;_RJvt~T7uU@@8Apa(>+_v6hs4VUK<5 z7PW6dNX|Onq@ns8Nt`D4#WW+=9AS(>lGoUt^Hn#v7cWSQJLx`zt|;9sehwin4rX`n zWUO$$SbfgN6F+x9rjdu6yHb`ghGh0eI+#7dLcWzvn?22iKIrXeF``D@w0_Vj$Aju{ z;c}0(dU=MvoTNuLdu6PnIsk3&p5lo{fcK42GFU`*mz8jv@i?7?uqs#JZ3b%15de4` zYlL{;z}(!s3nuDwAYgj}sXRM|QA4pI{rAp0aWbGgS)uqyBlhYk4i0D~SO5yiT=}wO zW_5mg6k!7*CID;c!7SBq9SQ#&+>=-T?>LTZ_r^dV(g#$7qh9^^w1{w=fHs&?pA7*` z;Q#!1DTms7h<5|WRL9Z?TS3?rl+w&UfE$VdPVIMvACb&`vG84G)4DNPc9DkBU`0c{ z|Id&AKgpq}3%2vIld99=rxI{t6c@9s)Hdwhp`&}7pT`0>qK^?z{z2V1VMRCFKQ*rK6)G2<#S}>5>=Y zxB)`|xBU!Qnb1;OS<+Duop5heTRS^&A07j8!Tpt?o|8UcVuGk1F{qXEeFGm(vP7Z} zU`u9ZW`NGlsK;f}d%q4H6?I29#=BEtoETV*ptyBCh^}vVf9dZx?n(9lAl#cQ?zH=~ z-+>1j7~~Wb6v-fm6vU80HCdoK0c2v6Te?KUoDIOL0X;G+GqY`U9z-$5VM&3Z1gg8Q z_h)s2VrEGAE!#FYF9DJg7!JT_akQMDoo$-}V6$`(pl~+#^X|+@O-%)XQ^3YF0Bn5l zzICkP#1CzHP&~{WvoWwF5JC*1>~R250TUIK5qKf|b+pQ9aimKA+1edoC4*o^Og;RD zH>erN1vDt25FSN(D@8=SN(UhpsBX@|p_*+4yv+CfqfbC+6I`%swODX!TR;y}1=jT3 z;Ksn)F97_8gs(npg98*|4-l9HHeb1rgiY+JNB{O2E-!r~_W+yfiaC4ZsN0eY`~{ zpoBv$sGJ+8#Pw`IQa-4_>DCZkXod-xWk6WGwY5bl9%9Y-1u#K_;7c8-!9=oxT?}cJ z5du|VgTA_z0JYtve%HHchV~f1H9>01E-3ebcKcp)nXHM42?(Ab5BKov9s}SI9xiSb z30MjiA|f9_VOXR4pv=g-G0W)>^Jh|!_um1NB`8_O%eAw;J^LD3E6?y#$d*Qow<_ce z=exh1{_B7PrT2gy1JpAHhyuXAK_?_6L`_W%VpEiq8Nfz{b_@kpv}ia(5t(7B!B|tq&hMGy9b6ZV2C{^`~Kxsr_@Oh_5hiBra4?Z)H~#G zp=MzjAZe%Y1uMuVBvr}8+^fw$!`3;7WqF~@6A+nBGcftzh9I*BT-M(j976qlaHg3g zL~lvjaR;<=v?+l^9`$B?kT8ltKYfR=;SPU077o=1q84M9-Gn^kJ}#0WG6}$Qtc_&| zvS9^tUi8&-2M{$$FxhA25t9oS)3|&yi_{9e(n7$=29%|*$23H4$D2&N^WYu`p-K*-#H1I zoI=J!AQzU;N+*|KjjY_PBoclFNRg{3J^@k@cSU09^Vb@Cx=l#Pwh-JqF2`JIHcB=; zK18@64@4{k5Q-e7^0GSPqS=^Adn{2Znd! zFghDt|Ku0KQ>748zfP>#9FSneAT1QY4EX3kAxG7^g&>+_fInvt5sGC6`GtsO&Jq0n zy$wYwR_DmbwsJW+?&2SJo?_#{{T%aaTQuN!x`0v#07nXd7pi+M0A^U=WWYM!3IPe% zf;lZ=vrj2I2@d2K{1B?LRdEvI#m^dGY`uTIpC>>MAclcY2_~IqOvD(knT?PEI38BuL&^!o_+(w{)jZu2 z{LtNiwiuj;a!I@#D?Mf*b*nab;fO!Tk z%r&HnNUA=N(@?d|6{Z_Ta_=%{$2M0*Rfp+gUapvDMQrY%JQWo144m3XXK$|NhDT|~ zzSl-QbadSFCiblDBc)Zga<(8l+Bf8lIG2Oz4}tm#GxPGyfKN6`;SO=g6!1Jb+1vY_ zGh8+C4N}*LUgbJ_;h6wrvH%Hizrm(v<2lPir6GJdf_PT#i8gE6cqMj{$>V8TI)Lep^O>~p`L=e1o;R9`RH<^Ds@;zT3T6o0XODX zkQxbYZh)`{w~;6C?|{avOihu`oIe3}AJTzrsq*Kc_gvZJB*WNLlupw)T_LDVijr-N zKW7GDzwUx+bRZ@GrDgnj(Jzf5cnF2H6Yu?4t}kz$%<>K9PY3{kbgAO-1|iD7+Y+^i zsR|1dy*%`SlJDoedGjVOZv`O8q4?ezp@^%^f<;lMjdQNM0$+&@>}L}()yT?Y6}-h* zt;g(CvnW^d&_hiedik_!pW+0fz@cu{3$9i_1fCV#C^eheEvD(l9-0+$a*WHZX#i#K zK~&92CPlF9=O7ma8)#Yu$IgNFIExGeaufTV=pP3e9npzr+XKYWK{;ly(M@n!gV>bc zhlO4H;{=l{oHw;S6mNS58ZuDBxe%bTWWN1Ur7Nsg`*LVkM>GNRe%3>0NFwwpA9Wl@ zDZ2W+O(Cg1*=rcca+h=ZDD(yUQ(J}o5ar>S0FAa9dnFR`4e>uZ_9O^# zQ)#sLD>$Wia&s=?Z#s-kp-|4!Qn91`I?aw#KY93P6I8s};txb%9H%GPE}E)b&75JR zVu$LJOhTU@Q2>I2B?CmSMcaRQj+2Tu{pa10NHl-|Y62!NZOZh0qTcPCsY3g;%5jChVtJGw=~PR?X z`6H0buZjGE=#aBuFt_RH_q2{DvPww)@IMc=UP3+ZlinoR_ePM5g4(z{C{qfpwBm=Vw0dq^@pQ(D(-A&Rc zh)WdM79Zn+o%C~jA@igIkdyamp+UYQOA$~L$#hX_8T3j-U^Or+>jk-_6O8#3_ zKoZFDP0_u>WK3BhZo+*j_H?44*ygN*`BOKqKiBh7s(3w#vSim$9hS)TtL|@`Y>J8w z&O8uQ|FcQS&G-fh!_RX97~4O(UvHqt%WM@H`@3IN(fTkLZ6iA9+wE5)T$m>1+uqR^ z{x!l7eqKkyNE76Rh4Smv&UMqGsJL+~vwClg&o}>gi0EyVRZvZ6`ziW$N0-ZoZbvLg z$^LF7#x0HN!iJ zbSM3>U`zra384>a{neI!K9!%B*nGQ=mh^W!1b3zh`=?!lL;H4w{VAYZ4p)3rG3xxW zRLJk4$4AuTY&MJp!SA{&roDjv{u<8C;b1Y}o!@&@a&S+%;Y?Y4;pe+h@~Zw6pQ*|y z<3Nd@e+kf!6d!=mruzh6$mrG*()@0N3fIF5)9Fhpek{EX&k74^ zB_iA0&(wZ3L>r{nB06sHyB$2a;ThW<-LsrDpzqUO9$)lga?|7&&*FXwd$`<4;czrF zM(`OsFf$;~kuGhaF8k@eD9vN)nQ30CjrRi{dT;I=dTr!UD=ryhW`RBqS zuYIAa?OL=T;gZ=j2UWwdUryE4)qylOLtuRYdIZ-~iCb1^7<;wdD2A~T&Gf*W!MPNk zGKUjZl$9O4_v}?KmRhYh^(;V|0zh&%7J-sT+aNis;{E#v9>p!J{D(I5a}}&KXC@?j z*ubo~TClw~t7??H)V@+7R99^PWg2)K0HHF7+jkIJ)!2Y|{!?b*f${)C~JqSLi#)TfC}o=i#&dY6kFaioYT{0`;k4#z%qL3*0UMe8~fp>)ueS zQT$hDGK2UpiJSN>IedPK%m>mD7T|7*Y{`fT0Y(A8`fFn}Xi<3Zc4)aPkXEkk3J^R! zKnLCc{7+hzRd-H9-<}NmzB}r$s0BTe?VkO)&r&JaGjp}FFScfyU$k7xRhsRRjudp_ z^XJdNHxV^)yK4r-I-oYdlvhY2>*%+tlFt)2U*h_40i1ch5NfOh^!Uknfegw@GS2Iv ztv3?v+-@WRHU!Ye<5^GpWtah-m=$nyL|qJ0DL!Ogne1VZY| zphBnGP=(aO^0&;#zgCwI#8s4DZo2J+le$C(7?lfE>FkD;yY`yEW0uTerZ(qBpQ4<1 zWkCH{F7*9@I6F6-WDTDFm?OPMO96zI_#Dp$0LDYc%J{HfdJ|Z;SLdCuKYQk3im)N> zNyC-&cmklwl}7Cmz5j0*+B6x5XeXE9I3o@X{r5u(SXP_YRjvuWe<#?`z#)O zyu7Y%ZoYaI<>kPERGyV}XW94mrNV!;4i(dgl!vL9>Vt>+Xz8!yd(29-xUw<=sAB=+ znxU~Z@_$}IfZabSrCk;p*WB`J*UVm-5IFDjejYgAJ+8G4;RA>7B;bD zr{N|qMg7Aiae27LGE>**`Zf)aL&o6|qQBFgE|@OPO400&bii4qH&@ zDAv2cf~kKrYA%|q)8f01q}d{F{o_FRvt1GqdWGrD5n*bZ^|>?(jbG;fj}C{Yg!xDj zIq|hoeJ(jwL8$)a*|<6i=(q8ePb1AsAUianxQzroxc?oHb|6kE7?4ZE3Pj)Urx)Yr zJUz+k&z9Ty!G$*uKL@X;|6-x#;PVFyWx_1 z{r!1Bizuo9Kd z7?&;Ri>*2qLBn4Se>9bpHBgmj@S*>!M<_$&VzSfyZZ34$v)bwicajz3?v<%zmNWb7XYr`T2kW)mqGA1I&i=Zye8S!-XENhF{KIqkV}B#jX(>e>E+&JP8Be}N&v^cNEF3l4F@OoZzQR<&TjtKzH zG1S|2yru)e9HbwftRD)o3T6b~mcqNJ<>W-@#z=o{cnIhOG(c1g3cM)u%wqw3R)4Ic zQ*b~zjK^Kd0=Q;{06iKM<_9WQL~YLe=jIib95Sw)Fby zYRJXV<@ACfYH4W!E}Jjq<>i3SRUd%wK%8!6X$e4PIzid&+s9k(Pk^KMx31*pCrn?b zsC{y0CWp`Q@{|Z5X=>~c0Qu`j-GI%+LF1wa4WM^y0Z1py>2GIf!e64mO!vZ;@VKyF z^WeK&jH01aKx>--FvK}7z!R_O0~I>AWf^j0(`RfB_FwVo?!{vS$eZc-XPNKCI*a(GO;D)UN1VZP2WvxT$_F zF=jvm0N{dmYiaPi06Y<6T)S9bc(MK?KJW_vKEXw>rKoM}1aIO*Etn~F(DXk0f8+!G zwQ{hRvCAN#;~i{4;nC0rSfP|MbDc38!zD8ye%f>l6hS~CZYi&FfFF$N+xzzeTpCciF>Fv9focvbBq&OH1(|9D|c&ZUcdrA5}jx$f3~ zbS4zwh9|}ytP`=hMqTpsAKz8C5-SNlL{cje{l|wfg%s4R^e+Fi&BWsLL)9)S;0`ML z5#aivH~%$P>85IN5`y*KP^U6WC?are;AB+X54F4F3_H|tUg6ZvQXAbJ% ze@#m%dap3&a#ylP4x{{I+B!wWnr6HbfmZ3}l0ypu4A3yw?}ZYR z0($H?_a=Sb@X|5G%UfCl+?a_@)O#HAHuSlqe>aO;kRC6k z%?DJ&Wr&)Rz~7sruo!o*Xgby^%@~`3-dqfs6}~nck9?2G{@Z<;J6qdxIw^w2(l}CP`aW#sBXwx~t)oLGsjZRy|65OnI&b_XH_f&3r=|ag(gpltkek(z z&Wn&YY5V_bmf~?<<-U%kddXh~12{xpL6#quKxXP=z7Mke^SivFbw)OEfvT%N!?M4R zm<$SXwJny3|C(Ij_#nbH1)i=!x3?Ni!+%)<;H;And#T75GP56X_V{0IrU%d$O=r5b z&;PqSJ5=XO(~nz|bH8_l-gf)TK!L^@XbDAc%=CICj#&J&^;Q2HGvxIz67&B3a`z?D zPY&*Ud#v9q4gWO_Q`yaM0a`!5PYwcAM^ z9w57B>l1(=<&NtG|8@EB(Az%Vu+! zSeD0tQz^>O8Bmvjx_Is&<|%VYAP7>n0WSj)VEtdTYQYa6igC++;(u(q?>d66^m6oNTBDa$I3g(z*#!*P(P_+ipK~!c*fxd{3|> ztg6?_UF+rIS<%Ru4zBulC1Bv7w6{mw0Fh_lB%M3Z1!eJ|e0@jVxvbi*AMydn3{Y<6 z?ZRdqs0e1M;a&!&|2SMP&2i}+wx;LM0)+!PN`T#kxwc(~Ar1LIhw#;N{|oN7pRt}S zqNeBi$By>)yZigs>n$xTpr|ex8JU3%aLxRwU;c0gn$ancjLxbWY5+U~5cOTe^1Il6O+k|7 zbw9Lra&p@2<$V0nZ{`&r(~1J3S`^6;I1B9nkj5B%C3%wG{=4ZL)v-r*oi9K5sm*Ri zSC<$%1DbQFwYj!-!&3m%hy=8oF_3l z(R&Nmnm|d5&7t~pK)1-SqJXdua!)KkrW&c&smdm;RaW z*YLDKM#^<+)(CJaOaKN7Uq~Tv;cxR*&57$gW3YAW9BW`D8f6FZ6(c~P3FR^bkXLXF zgG{OckPeh8gzk?+wF@Qi(=0Xq{@xUt-U`fQz$TXjm~ImtzG`ZEJ!3Hi=+c96w++1d zLy-5rya!Ocpl%5$6oXL!I2ym^YXb1P zg_#-OQf6eiu=u$B*mF*R#u5QKQ$FpEuY6 z!bKEFDslz{d1JPnj4uywD)9rJ-eKUk0A)L&b!PhAj*pK&3tgN}rZNGq#z@bNc_Kk~ zZhSV7T7*FYL*hIM3pxy?bqr?uU;-j*P`azLsh2VBgO#xG=yMj6_s!&qNp}PIDd?7siQjq8hWsX(wI)1oWAM$u0Z#Q)xq}SDL zcQAm9A5fy}Tf>)FPlmh1R$hFdsGqD5rG_$xVdb1S%9i2qAS0I{K4;C*d;*|_A?30h1)P})z&pbVHhVOb zvJr4XrtsL!13E_Hns$6~6o4BCYQ&a${qf+_6p!-vo)>_LwL6aO&9>If=Tx46jIj;? z2?4DB_!FS<2m9r08n1pBtQ!Dr|JEc44DhO0*44G>Y9$al%TI9cxvm28isxvAj@sv9 z^v(f4qBD1NFRr#-xzz*-+GKwK@;ad>fXNo=)t}4g9rpvMZ0Y%`ev{t$b___mMu4B^ zn>dm`N*c9H*Bu)h1CVWxISe}hroFgfDFv!6m4fJ*xe%9z($$ca42P#KSH#lO0|4F^ z0BN2b2b(dAIqEEreBh79Hawkx=S|a#i3O_pG{O{mTmX_(3u82FD8?R;s-6JmP+M>W z&N=CVvI31`G`Qqd{S&z=9$3uypPnY#d@AxfTR{_I#s4AWG{fNqMkn}RaX*#Kl(Jpw zo}E`XP^kXovpSH(yZx{hes6hYg`w8Ry&XzP3OI)s!ORP;SI)cw`R1UxVeHj$DC_R4y{E6DowEGEVc@r+mxpe=I11Vy_%t0hFeuT zXE}+_R10?q4mM~8E^wX!T3IMr?3^zO+EU8J=>iAf&<5nZ>cpK;HdL^)Y}8V4^Jkt0 zgnx$CKA6N>gBAb+Ck3?b!=x_s5OD^9VyKHVUx-7ae|-ELupvW9P!m8w32^8i?to?i z%5ws|6RgP6^8tO)wUDW3!oA$feF@*XS$tkp4)BbBC`*h*BF|^R&Z`1{-t3=ojoIH6 zy6!uG-z3TSVgb7908x>z9-vSYbXJ5iE`LqI2{{5g(+8F)Kc?R)KLi}v;FJeD>GSc9 z&uyiN$qsN3{6m7R@^4Ei4mv$(0b{S#X!>F|;yz@+Wfr`^$=J zibTV=7*T|Mt8WrGRZL=2RmyB;o2I7Oys@cBeXfsuYcyhV9l^;7`4&5OBS~BwAKSsX z-kz=l?wHw#=E-WA0`Bg>@}pJdTj=xpJcw@Uj+W?5j~f_pO+740VDRvm5%KUa2n1?5 zn{iRjLy2GN;;pX2*%afQt2Z+|W$E2(r-rxpKvA(NMx^~93_Dr_V+s4%Qur|w6AT7k zOs^LlaiTx$z57ltuHE;CPhHfkJpS8?GO&*usnwje5nLFq}A%zc?dpzmM70_!-M&<621t zoQ6oMzIS}$y}-OT41_QyR$F|SNDem69f%*`$rV$N9yX8;Jcr?-y8*NLwyFNu4wgrh zNJocTo2++2!1kPJ$G-dB{;eXS)zyamLMD^?qxSoWjj3?~in1od!mp zR2;oU)QipVrEJ)Kd!v}S8T)kC|GFLQ2VdC2&zAan!}Y`$FMBJiU~gfDF*FdFUn`L~ zd2<|j+x&>eGrpd!`KEr`9+i+K%+N;9M14pKj`5T}#@;c_X(*Q6vpOZkfv6{Ed2{ti z{r%|4F!+2z%-%_XDTEE(-3$nrvkP#C=T5{9WV6g9+849*!&oD#d z?eOC8xg%!IkRFyohl8ERcR1X@H#)@^6N&RUxb+6<%-NwK>G<7*Y zk<@ex^KIE+oSDLdfk{ewu4|&LpgcXbfY?x(^0g0c^(YVmn@F^eUZ_Cl{bt@BHX8l2 zNj(jH1IFa*V~gXP$S~Yl7verOF!KKOeKJfiZ;pjtEJ#^pD4-g@Yo8^Nj9CTR~H{wnhIwC)~m?*}60bph;pUeOc> z`V$0L37Hf9GEa;HyZI_A&pwJs85ee;J#wgfExo?ctoi-*lMi9qsUHH@xKuLyxTsCp z$Bswis7+p3ain#>#M*o1`8_6gIn7K9&2D2hgU}?n7iJ<`GG%RcA6rHM@8Wxg z)_7^|8zUQ9b7L99YUTULXN5*SX9e;hO8Qec9(ddoM5^!ch7<(#!;D986I2O549%E=6dX|C4^$@Zhj7#JnP*L z4H5lD+442l2p5|V{6ARezBjVA_XsoUVc%3m-k*2YAd)on)&1awksCEA?GvMOM}ym@ z$09y(KE3aQox&`!j$qUh(cM0?71x~4HQ0ZF;w&6@KiK3;mO(Br;M?UU}b%6)tyF(F24?~!<38H8`E zn;E)#R@cTe3b^@Qwnny|N^^?pso{>)l#o=b2(A12+_@$Dpu&oZHMyXh*VL-Yzf1Eq zzE7*+7YjdJzEv#5D9a2Jm-n_pgmohuM`RYSXjoOU;YepOw^fVZAt9Z<#*XWhR3o)? z&~?s|r{h-^(B;Q^UiVaq&LVt~tx)lvBhK;nW=fH3)#>&)vJ8}VU1w>p)tTvv1R@Mg zR`cb;eI@nF4Is+wstGDt7v9<8{LOKGLGNWb773?XaYA0|e@ENmg->`Tvod;4@Kq%V zF|?yKCBejL(0*&8`V(i!*F6;;-8r@;d3@qLORI-Q74~0urVYpexo@I~oQ*awMD^pq zN}+VGmSOGPvKq+Or{BAVtOKjN{6=Ys4c(XrxsIoW@;>nNurLHyY2vlYBg*ZzhjcBl zJLV{eCEy%K3=BRbFRM^U^LWZku=VxvZC&?9t@0VCsw3VBU+!Ty8PUUU_CnG(aldvQ zv$R&*h=e^gL{>`XYd^uhm%U5IuLR#m`@G8DGppt8F8!gD+6JHYQ8W>g^Sq z-j?S{g(0wRPoyQpzP}##QHB`G-T90$9IrF=y7yglSQAZ2?ja(hpX%%7CSS2%3r z;U>Cr*LeQ}Eew6kVU_gj3=O_5d>Rk*j^l2_)P ztL@wNyW0>gxfht*TM!onrmts7h%#Vjlr%AO4cwSqbDRk`U2C|vyfCvQh6VR>7J0+c z4`s9^|H+8lUYu*~bdyTaL#0^1HWa59jE7Gj*s1RxNr%NBZoQq6XW7+zLgp3ik>-3` z>-(cXLw2qw`6;6J`(wA$KTynAl5{8waXer;4M-2rN&Hx%w|TEhw`!~WcqFm0)vD+; zw@#SZlBpwoGyZOWrO*vjyX)t08cL;D?xJRz+a3wNS*#yjeJGmqP~qQGo7MP#UhPyL z7_ihw`KT1EDBycwYoIg1bQZ*urjfMDTo&qg>OOMT5%9&u+?AYd*jt%6Fm9uC4lOS} zqt4VXkJMYJg+>00xBLL_$sk1H8R}qzYv0mDa*h~w(* z@qR=0uOpq10^WUdXF2V)l(3SMuT2oRyT2g!TyXp82F9stTjKCmslP^KM7yIEY_2@hBQR{Lag@8Lw%tNQAeaT|Q}ezPrLmjL(J8L> zfg*mtl%c%lB*0RhCju|o$yguDN!gq@8T*2Rw@_>J;a)MfQ;QAT=WA(`qP{h@4Kcll!D$`2?>qXy#Whf7FEIPz@tr-2VEy`pcaL^n#8zDtPxK`%y_?=S zQ5QzRPCpge3GXdqtK?hgF>L7u;_6v~s~A@WkL-)`~Bh&6n?)^M*#s zo^LpfEgCQvQKjj6ghg09CX-@9!lgM1qkC7XZoaJO{z96_*fWPJw;pnM)D&sTwJ?T! zLv$iUjqT<-CY{Gn2Ul9oeyNotgF5+_`X_GfVKhk1t7WNf&h?2QmeUQIKIzM ze3esv;i~pYy(kUAFd2_90Ch0Zv>emQmJI$vgQd+nQGwdmuh_Tb&fv``((W1e4klCL za1#V@C>hW_e7k~C=$c@NYjoT1Id!2sDLh8pQc%>w>$`)_#W4(UTwh^ia~}@!ZbhvQ zPPX29dXXRdkPEH5|9TO=g%)kNGv&P&6|J z>3*1wI_Mq>pc#8CtJfA(YV~G~4a@5mSGYh^YD*dTC+anS zN#~s4$>W-kPqFumeLaKP$}agF!t4smK%eg@?LdB`cH-_glW1<2v+WNIVwTJg5$t%b z@pZIZCmwcUUxH6BA6k7#i<4zWN3~WlMS3l>a-X+g1etx#Pw=T}kAA~+ob7F8g+(>d znKOUHo`R9KLK5a_6%vc@PKuCsEc0y`i!*b{SStbiOb^kqK9 z)|hHZ+jpcyGenc~(AltR`Yz%+w+Ij7FO$r^;}G%3%dpRtBS%fIIuHxRV&^=TlsjfZ zI-vh-WG7_AkcS~C%P`Tms!Up}5m9Wc*t8-kMje9A-=rTo*9zEwhHob|&>}va4W=}|>Uo6}u ztHy@lc3U^Zd~G}`5n|S84DZ&fk)qJ4e)|?iGOq&fkIt;^ty_w9160aWzindQSdA$qOdL2POlyF^O%ytAnnqvd(?uTA4zED@6S0O=(WI zHTF7LL|`T!=qeDfwU98UB3mWFxey3Rh0HKBHj0#1aL$z)3#LnZ;;wf;k3L(rfwin3 zO|puzO6-04&YV}h+2XV2v#}mhWp6wVGj7O}814@&w;l_P(1=7I45qum?lx9Fa%zYz zD0496-*0QEDstbzMWQ*NF4Q3Tg1%~7(Or)0w%WY6)%(>c?)?!ejU zBVoH<*Au6A4lhM3&1mcuEW5t>n0|~RnytLsHS^r)C1Pf%s3t?S`!==clkyx8afsmj3x4DZj;ekKl0g>`8pQna1n*dFfLi>HBhAcg&-y*wDyr+kkG2~ zJ6N6I8nR72&sP{cLhQZjDIHr8n4FLI6Vbz%Go9SONZMWo1s~CEQ-nS+k+L zaf{b7jD5?=;fS!S@8#*ucwmLxtUs#6bkKhDOvUs|GHz> zRsXTd2MSyFTlmGT^zyhEW8Qv3mPqx9oL60+P+%#WaWbA9u+^x!e@&_F#*IvdHdl_E zq{ivn+Wm&^TJIu1y31c{_X%{GV{4P_^G~=}J_11-Nb-}>P^^|iLCdK$F)sJ<Ms>~IIz^s`TjJ&fSJ3cUHBFX3JnDP!Ju%6e)YRs{GM+kI=PO0r>hPM!#3v?2S0TC zCY7Fu1suEFWz2Rq;LLULUH~Z@OR5hWQ*G5foNj$kH`S0*-dxyG@?5o%lah~}tNWCf zQkQiLel5qNG{(~|;j^mSKopX@+=1rI7M>hy^s)=OLm;dLePlj$i1}L$3ApUUU{{uv zs;ToC&(CdJyQHF=e5#M?Zej#Nrp6a%m>w&ADVxan`qcbI3SNQAkqan!n3x$d$xf5M z6r15+t6GK=6;n%VE-UC*e-bUJ2~Rp9v*qI=;?`v)+YTcP*^J@EQ`W|Om(Ckb8d;gr zdtFrh9OBj*&`x>d8H+N5olK?R1$tF1htKi@Ohw$M>t?KHT9%nFZ#x;S-(heHN`eu5 zPT$xt6odTWWB}tSNAAJ_F=rLwGxyhGj0igg^PQnuDFovmtLNL2T$3!-Ep$$exC4@0 zOz__kJfNeV=N-%_;mnswC$;+it+e2K&T!`=di~)vJkyScYRhEN>h+De?%(1(3kamO zUmqL_BhyI7j#i#TvO7p&l;l3}rg%KCg+7yy)0$<$C#zChv{T!Bi};PpI<6gyI>$>-SbSCYc|I4$CyIC8q?(JN<@ zkk^sqwDVY^c`>s`aEpJee;CtE0Wpe=X%9asOCR0RIH! z@z9>A)=0747kWqzjg6=fJ7GDSP9{@(v_-(Dr}tDCl&ibT4ywpwPh8Okf|wK zMOxRssPdQZim~;19J7uT+AS-n@k=_JBwY9NxZWCnP0iN$NO!{hbILo1^QAYKa5RJo zR!)L@Y-6?j@A?x9zOamyQqfx1Vl2qz80jp>v-#h%(czn6rXLr6T)P4B4QGGC{ zN3(4A=}e|tj`bT4Bi>`QuT5BRTXxS1_@35(&E6Gvu8X}Jrrp|P=+dc9G7d|Y`|T!W z*1>^pG-LLoQ1+ea3L-V*7^8l;Z9bC&kCXm(=Jn&dyP?x&NK6KVyH7l(>WoMN0xW&^ zHsfWTN>deI8n{1qGkPJaB&naCXrju0dqd@W670vO?Erd#KFIZVL9Iybw$}D|cYTG+ z`Ng@1)X=<%t0d0Zl0-f%$|U(Hc;0&yJ+Uhyo|UD{+WeJ`6w_&T?6LYojQc6GXB>KS zPn}GHv*TKmH}oo&Mt6@p>ts4aWNfJKF)kWhWp4= zG!fbTZs)QHGdP4in#|D>O`Kl~pz;fdKc^OZo33?)D)!mXp0{58b;-aWVcoab1lSk! z(yEKf4%a`%e9J5#jzvaBuR{>UwI$$T_}KS|^?{Vb^4CYS4=6`a*KZsw@w$Z3AI4-+ zp|%dfeWF(1kjDx55Z@W$Ggd6B+0P-yr>4O5%=DE%59a=vcz>bN?GCG^d9(K^^}Y|O z)p2tp4sIG=d-Wd1j@zp{fP+;vLm<7398Fe`7d1*_D0JWAq2=ti3fbNahNB>+8?M=w zl`gQVre4pyU+BI1Hqo1fAAbwMA(~6j*-va4-!VsHm1y1UeYQZphMGir$e=S4GxHa8 z16KTvTy}_2@c=TF_NYZ{j=j9|Scew7Htj?*C`vfh{sJGpHdJdA_x+=bz!~5X(@F943)DE z^dea3LWkV9vrG>g?*x+h`ml!cW7zeS&U>WmFIR`j3&>Z`eturAyM6G^tb4Su^#MZB zi?Nq|&KauN3L0_E&!)Ff3@XH1BG5^t1~D_^<@NN?IJ%lmBoIdi%?VSBxnCM zwmb1cxL70!n%{^BVharnknZGT;LD?}!Fkd=6U!9&ys|$xsyjdYT-;gn0q`SY1RzpA46gc^*039-({6HX5T! z`f)B4@sy>LtsQ&d@#!XgKdZK2@5b_{?Q)|-Bpb* zIZwl}jDGq20SeVurE_vC^w1giG9hKJ>B1*!@u~|At9a_<#UrE&4*L(xsgJ6*=9aT5 z5yZW0Cyk{E0%i^!2>QaX5eu^8!i0KyLYz{xMs4;mEA=ex%%)NWw$~&Gni97j5$2{p zo@iQl(liFsZe@Y?-jp-0Z|-ZQaCgGo{TPf88>Jgfe_A8suX1$Uu2of>X!^taG% z@xNMOMl*d8^jXq8`zv1G9OrxNKw`+pz~`jY>luEwo_*Fsa?DSj#L~^yqGVuUd(dP# zzFl0(6+8;xN1?$YD+5Lkp9zJUrI#-(5P5kENYP%SPgw_5`*R)`!aIl(6lsRL~#P@#c%}Ta|ii8U#yC z8XaT!XK0jtla|0LSo9~@ZIY2?IN?!raN5v1wqSivRP9Q5IIQcpR`}V_cQy_C zc@K391$=bOi$rj5)BJYlL+EoW6efRe?Q65850y1&JoEzKbp;qOSggEm>zGoIJcoIv zLTV_t=8jt;w-RQqU*2^XBoQQpdNQ_<9bHG5{(;46>VwsYf&Tv8Jo0yzG71 zsVIgQ3^NRNu5Af-k*~!W+@#mmn|Zc+H?i(iF&v^&oJ_2UjbZMpc0B0Q{Q9#A3}sP` zc?P1+Dz3?O1j>o>O3cm&JbPr0r}9*k)r#I!xvjp+{)H87ujMoz&3sFQWc8D{ESgD% zxP}sapSdM^h}PO=JpP7iMD&$?W}f2qo5y3cj07r@dyB9G`6!}NE1gK+EM02zQSD;t zoz%(Qg~6H05QUlgrFzsQO(%qN{UGa`AccRc zMf97^vTlz8O;~=M&Av#rwd3azo2q*?Fl9*r)_qmuqf1X63bZP!!by5`XjlunXrroJ z4o?Y|_jR?=GA;ZP;Z_5h9(#02x6da_RIncshVy$jlfml6p>fYKu@PA#0oNV)HMtb7)$J@L+gyd31u+37EwwD&2L|TE_1s3;)w))} z)Sv^$!G(y({AnMbkEBENfyrG8_^FH9~OtdMslO^-I#L@X;O58jm@IHse0+c|qq=resmeotsgZSm!OBHuW| z`OI`G#pmg{B|X`$?NR+(EwgJUMg)>m3ALTbvw9hs+$r=_*I&;J)zIcDp+uJ6rP< z*rhH#KoD=Z6RFcmrZ0m{u28gyd;Vycwxok@k=lMu-lf3wphd2s6z=@X;*1(4g?a~S zl#YS=sK$)^!)4_-&CcEpD=eLw;KJycXW#Ck?P(l@e7_qPCFMt1rSh@xRfeQIk5o32 zeS5H=S?M^Vc-YBT%x4iV2|Z^&-$RRe4|xdZd?#sXtwDP*b+A!O^8m~24hLsTgim4H zK~!^(yyBEst`lLCqvJui0R^ke=S>gEw<^~>?7byNe|#s}PkPW`__X?475zL~4m~X=Umn4#i9&4(98$?7F0&i*gP-l<8qE>M$w z%eHOX#wy#kZQHh2ag}Y`wr$&Xe{mwtj(yP=eLa4_95FM;lP9N~lT)7g^2X1|mSdls z0+HkKV}p8|jKPqNAAI4peEZlE_VF+~nFVL|uVb<7f;&-bd;S*k&<3HVFwy(W?f?}} z%{3&leg{xR)GFc<4^@5M1m{pTC`uXF=cS3BP;jU+j9Cgy))A575PK2NKeqI=;0Fn8 zQ<>!gamO(JJ%8G7j7@s3_`A5m+6K7}ChjP=pJMk(hk<6mqss(qliRFBwRAd0#(>Wn z_V2i+q{UnL{E$mv!Y(v^lbAgd?PC%7gqS6lE)7pZP>n!X4&sfkR1{LD_eo)lET$;K z3TjcyoVgL9+y^ZH z<}E)VYOG6moM>CdpTdHFDF`-kywK>0Z_pA@R5EY=`ed z84#aDW9c|zpBC(gq%6Aqv!ada!9pYmQf`92 z=$vDai6;+39Bq~=%x+Ym157_Fdhwn34BJ~|tdTIrKo+g$ux$VVS0iC=BCfIMH#rNf ziz>+1FUx_dSI{u2;SX(LlCn>t>5ckIUh15zN(a3l3R@2_#Y@`IOof zL=y1{yQgMN>jf;9TCvoZHIMS@y6Jb25-~o}M;g)m8)XIe7j<%U+QfPqY&%h9aRXDL zZ<^Eb`)4~lCF9uIQvF_Jza{tC`fCL6S1DizDEV{mX1C8s{7HXE{KGSK-A<>pRnG;D z8QP*~!RFB6fa=ySCZ3Adu?kK9F&QB99vtN_sJ@bUs7K)H4eJ#NSJ=UIjj3cc>V{l0 zWWb#+Z)kPj9P`J9xHjM_KJYJk2ZxC}-)EyokZaSj?8%35hy4gO>bK#8-q}p6*_clRgwcw@sCkWA^X!G<3+1?SGV6h~{;32ki}3Ea{?B zLHja&!M8QJ=r-O?6MNy230t!ci8FmW=VdcipJyA!M5~4P{UevdZR2j;Nj@+INk8;9 z{}T1F46N1^STloHYJa7+j8no0-xH^MCfggExZ2S}^%UmJ5S347o|jI0WfvM?4!nCF z*zBUJ0?ignfOcu{QBU%+S*U9#1>S9|aGt2wOMAwL%RM2jHdMAGJG{AJ2`-xgqr;WR zW@4ZfrAg!I)>1>ncasyg)OIVJArmSgdkmxyJoRaoyn4_%5b>kVet9 zaPl}ccDhYl@wa7)ok@;UlJ)X1M?sA`#zAt{%KfLv86qsWCMrFw->k|t2*;f4(rWlZ zuomhgGT}4Hc_cZe#ya9p^y4C%OvTYe<@B$thME6OQ(#8NTJw^}Dx{K?e}{xbR}T&E zqT!gTw-l?Lis`4IV*~l``NJW?!hTY4h~-Yss-HP7NRv+<_&!}QVy;v(s&_dGLBYC2 z*8cUN4b~AxR3+4l;5*IM#>LA%OS=YWd0}`#-8F)8pEQ9Zj5RmD{kk~lfPvYnPwJxZ zFhWyVBfMH}y{*8~Q|hPIrAO~S+CY3uXYkdSu&c3t3uLY1d*8Sok{$FKdmBZn2x&hm zH7FIC*+GzmSoV~@hB}q}Ui2h&`L3KE2S0y@!pF%TA9=raw%Y+lEAzE5XnK^z0IV2~ zklK+S!6`kv2rqNUe7w3zliJY5@nE%H1qv^3%5*c1k!bq$u@#_ ztaP8%!^%42PqDMwQ59FYhG`MXC^KZw6x)r-NvLNapU6m$0fDQroE9&1r%jZ2rm8_u z)>5@;R)1>0RDwfZ>m>reAI)kv&N0DKdlAy@cHl_MN^trn#(4LOoR(0UdnxO07>c%I z47>%1n*WgcqheMpm(T+*495`3HoQy;C4>0J^oTO#4vr;9tV}Z9Idduc+xk(m#=VSJ z4U9VIZD^MahGe_WRUm<_=qdN;mzyuHC5K!!&V9Ms0sQpK9F6nbcMRDB?O(5>JIW?B z^EiJa6H!Taa$A$th6J!$s4sAjQuu78W@67-u{mMhh+KyE5*@zao5HQRPczEfdK$bP zX6X(~SD3*N_*bRi;$6?W2*9%cNfQ^_IZs(BkI0aA#o1eG*~4c&GybFS3sT3khf?)j zgq3o&Mm^|*3ppv$ILhCQ+&A=AuISpd;SIIb|(y%fAFmrMNcfIw!5xb>zk*R7mWK>e}+vxyq(ZJxRH@m!N_j;bw6HxBd#SF$Rp+b zuiHZ(ORzGnBI}Z1CA@x-HQdwR2ADZs8m6E5{C)=&T{v8CjAnF=_=QID&tpCP{^KPQ zH}~uYWsC#uwMp}$_a90T9;uFl^H*lS;U7GiA$@es?&OyiR3l0Uyz>q|G_4$k-`&*G ze}RgjQF`uX`PG+B^BGtuZJdJ=h!yS{gBUDW-GE!aU@G;vwO(jl+}~=}+Gi*Qr0fkA zlD`&!pZv2d5lVLmL@6Q!MC2*Fj-+=Yn^V2_AXh(@GTVwe434(Eb#?f7b9NaFtop_c zV>}k87dbNln1>98{BdbEq5t)+$9rL)I+n>@u%6cCi^m7ystT& z+-)0@ZG!m%DmE}tTn9*XGT!joNA_(hPCecL!$mh}L39>f+NZnh#VTK*iwHa-Y#S;z zooMthXT4JW{m3D3oLrb{G;QJ#%6>Dz5rnmz5!T$al3H2ObqT);p9d4He_HSuQy+YQ zP8y!bG<@YqyK>?NS*g>gv%LS)0Cutu*2Bt!ul-aldo>NzpH1ApEJ)GcS{G%4%V@B? zeQE~3n8X`^a*dx{oi-th9wn4`gsSJy)^JmDcBCXGSsn5HS(y|16z@N6&qkbqL*L1_ zgoJ!Tj6mL8!KW|oMLQdbp5V6NBtjGX=30PxcrB7`DY_%t@uuYcNUR-zD>r?5UP`F< znOb^vy3>!T>j%>(Wa=_xkc zO_p55Li>xBMo$ntcxIX~KHuggQ&fg1TLCJ36(!pOT&T+|L>eh zOYecBRF!ZVWn`^kdfBpX*;IPTOL^f@joMh~G5!@Hy|jXH`*jva6^-oHdGs&q$^Y{u@cen-!m?nFz!tg}v?7Ae z^URARI17U%S2RoNkzJ4~|q-kCH(A(fX5` z2&sjp8#C8Wq`%?SLT^4uKEUEtmXJ_zI`VU4-8d`UpWNqViL zn==$Bh=72gS62wbfNV-LJq~vl(7plWl25OY!h;I@rphP?a|QXWjKu(iBe)BH@*rLl z-sJcUD9i&i0pJ_Mh;`=h?I0mh!36E$=aiQLylz3nKbY2EOncy8%Qyf6hTY!5@8fS8 zMA-XHEF0Tc7Z+fXE(1vI06189FkZElJ=bTKWAFf^1pP3BHW=h<1UHDmJp-Di{@ba3 zfGHOq08@8Q|F~VF&_J$+zKfpmtrj`3cOb8`DkOlC5DzaOQXL~m|D9?;LBZN(vpc|F zZUrdtN$~9p%*FsRV!D=?&`wSe1axX0uA<`YFO-(ZUF#J31vDaXpdj%8F`$eEaA9c+ z@r9u*x9!iTPoNZdSUekR z+mFhZeNGqe0?OVX2`Tj6J_az*+uKkGfcPwQS76X0*wDWi=wnkI%`g}=;(Hu@yTsr0 z>IVae^9LJVu1~<&l4!mv6qxgeyd6sd(j@k0|5HHuyLIw6e!sWi%bxW2HY8bry+1+F-d~FqJcy`ZTLg2pS`56m4-)tz9ga_H4e>O*A4Gm+kA}*}@w=r2 zWf&Xsh0W+kWC*|v1@=bDi^q%&0r2~rNrnjW`72`rjzkG{=jj11j(z|f(x|;vPiPpR z0HiCX82@B}>3Hu#e*?Sog9n_9h)GQM5ot4mPzU<_J&#Nz1OupWOS>p8YHkH4zMIUje=cH!J zt1Xl>*6p(@+fY|B)Bx?sqa@d7!ze&Mkn;{wQi*D5a5eSK$&wu|haP zTjmi(w@U4tdBR^qgD`jG2wVA3RV8=$U9ShuVAs~J$Zts!$V%8gSbK=>|(hK z{(K}*B)javyH^c5M@Aq28PcvdRhE|5(pEHx%rHNrvdH`JCk>Gs#E@Q@CI8-h=BR(- zVF8P!nbe~e+FIZ~ao0slVu?ur(yw4Jn@;oZEVYh_d{yq`kLAc7l-R;B#1uf({Fhpq zq!TEa2o@nYDzr0eGt9(!fVj#{eodzm;(9!dUM&=>g8S_2@~W(Mmdv;@)QLOP_lA%D zT`eMYFuLnk@!c4j4j0zgAjah1l9Y6!1gKgF#Nj}ams=ibhzYh3{j}Ti6qv^4DOg+S zh_{MI&ckAe-Z@LP<5T8Oq$m?-Pf8|(kJ@y^G1$bQ*?@q7+<}LlF&2(5z31(lAxvx7 z7g7@3&OH7=c|dVZOybpxk9`F^zP|C5ysIAhc+TBJrmy`i=wL|b$bj4Jr>F~*Z}I5^ zh+#aqr{WZbXhm4#GHTHeOKDV>%SZ8s=0bb!5>JBGeAW)ZQ-~ydF*Fx`0NrC$h5pW2 zJW7Y)GofhIe}5y{Xrb!|C!)Z85TbA#@PMm|%DS#dlI0fM`YQyAK9#&bIr-6{wXP-@ z_m5P!ee?Lmqt-Se`|5E~zRHrR(UGObyid1B)F(wt6o`R}a(+9Uq8~3^VmPP3BPHzF zxQP~ySK7VvL@I>ID*jdGZf^W#0TC<0LHJzsa=qC>ttu{p=OSf+&I~rUmp$UPj1tWZ z9h7aH$#k=pjo{q=NPe0Ygm<0GYYeDWW36CwLRJ*C|O0b_&0gQ8&vH1T{sOC1YF~0$ctl^msI3DiNS)Gnh&4vxTfyz){&mY zSj$kR0tu7(I=C^K`dw8@!ww=j-G}Q% z28HGy-yBboDVRn7cS<7DCEX%w^+076{O`0GS7JJT__Al|$mJNfE)T5{jVtfRS~h1U zvBJyzqofiBUW(O5ScqeQQAge;F?_s7+TzywXsbN;8^340zB0ZPyrr*t4ye)q>?kGnuk8WEZs0}MgGU6S}&7z(o1vZI=z=bxa4Q+JYF$0o= zg_CEoTy#3OM5W%=Sn$$5#NtvqSKMC=AUWVtD>CB;d7Ei1v2#sH`=gf`rz}%ASC=06 zr0r`Pj*fOi(e2QPtteJXr>Vz26$12=i#-bJs^TmV`R5z7?=jDIu?`*CCFTmG+J?w-AcgGI}tghn%jW4)%K$-I{S) zB2e05Y}{llH*&>rGVTa5Li{5(atyc!!+LB9nu7cS>B*G-bTl2VFffzZM3$z88;bmt z4R&ogfrch$Jx0gTs4e7Cg&%g=1%4VjM4IgE@$$4}$Cbe9v*CI;H6}?;NnWtL{rc%6 zob0(_8UhzJt7R9L8D&TQhOa_-!~(dOx^z6fvA!~`B0LXTtY2S|XH1aJZ@5dB4_pW= zqFP(bk!XAl<{Ql><+p#jLp;=NH8$6kDFcgv&yO~;Wk)l{?cUiP ziNJxSu%6>uE~flr6a3OjUUl_AQafbq;o%6AwRRYP3CcT7nVL39 zDA)KK4v?t|GGv*O@3suZjBQ@6e;r-KtzJI((Ub+SHnTsCPq}<+5KW}6s?ps!lZIk0 z+m_3nuOl|*ZMIg7YdCG0#c5mlYOgIDEO=?H-$NhfAmCa}gdVEk=lG5xbb$@vTuQ*3!028|%fzYd0DDcY61 z?njG_AvpID4-FiiX~s>74G|bRQI%KJvTl4^IYKpO`YHa{$eW`gDu$S`_y!lN%VAB@ z7T=g>W-KojBW^^>e+@mlmw(#aZs&V{FX*|?glw`D=DWkPRnS)8k&Nf#`k1`8PZHSo z)fuBeu3M=HPQeznmBd*gf7VEYlEw@r>zmC6fBCYScqqd*Rr>e}u@;qJp1OJ0BU(~{-RzY~~ zfxikoMz6@*-?}vllYK3tBM|eBjF-8Nc|jy};_7~kEal3TR^m}l%)pt0ztMWXP%OT1 z%ia#xXy~5cfcGyG1U(46V68OC>(sT@vCcnq;WCSZt#aiedHToW-xLYIOUWQg#ggY% z?DzBF%2MljRRAuNR^oN83ca&8@X+>1e7bAMqDZC*2r*YG|1T%$f!uJ}uMx@tEB)&H z0BDPu_EQt2aRKRXh_2M~zQgCjW97Km7gA)nl5y?GWB|T2rksTsH~*u2{a5 z%t_G2qhMB>G5j`)PBiz9vQU^eJ3`mv7Jb6tmfj)19_4sR7;yxyZ?odPjJ zk?_(M=A(Wu+Bz14Rxpcpo!-jOEy;(sk`N~51e(UN2@NRAJ^ts9I4TD=mZZV_yaDve zA#@=~!is7SSuRmL$6Jwy8w&L=`Oa9H6lyGq&+$<5JJl|>vMR&+JbU#h4plde0bj}M zzQ~*^!Gj*n-Kf+yWcXV7cC};^cogv;zR#zR@|11W>E>6JU8X|#`P`#(M$vjqE5XWFOATxH`)0AFdkjm1CFbK}jJ<|aWk>g$EP?7ZT&dCO(U zj!_W0B-az*q=__L_tsg(C;Dj6r>xl{v>bO7S34B+^0S8=E!@SZ$V%}i@RjzC7+7SE zvPAS?>I#MBE}HB@v)CLAPr^gsHC)j{=@-wilr}PlE%N4Y=Hk)HWpy`gbz?LIbzl>Z z-$lA-JTS?V^$A?*(vEfJ=90M|aK2+lUEA*f3OC0jXEh9kycQ&zz+O*sFq%mnN?yWD zdaYzj-Q^$0v!u!vCSb`bbA5BYHN99-DKBIhD=9GvRu{R7wJxw^b{EV5pRCQ-LHIIfDt4GNS}Eb}X>Rr@5{sgL@1pQ5$oA&4 zyB@-NH=F3eK#Rd!ZcL1uuN1+rv!uv+5NMb%`1p6$6=-i{5W8!Y3xU$_0!Db!fs!k% zsoHsI$3&rISrPpdO9zZrg$vnETQw&oIwdBzo@8nA`&fsI1ihaycK$Xac6z4nU;J*@o8nW7PxF0W=Po$qB+d+V0>*@5%kG!*;F;AkSOa>^lr%OSUB^607)oA znS&k$7KhW3w4ynux|v?0f}++Y@t#DxT@_?kt(~4SHSLqfrBr0^)4`55O|_qTeDpdy zx2sKDvZR=_E<70dT1tzAAD0o&S{bE7SY+XG?Q?~T&YM^9=b7{wx@&s4W^j?Ac%tY9 zwE7JWH;|0T3fsP!%c8ZY@qX-Uw@NEUxz5SWS3OUc8&hU{(zdB<{s;Xkx$raCtME38 zD+j13j!byQd+LYOm4m&QIDS~Hl~C;%*<%j15o)1-S6X`>=h8_E0h5qE-02W2Ed^2w zJ4vx76LYt~t0|`;);o!VKWT3tSuA*4{E{x;oTb9PJmS59+zXpq zTpSv8)ntFA14p4nptC%Ii^+j#(=;SATiS)92ar)TMIjF(K+}ltSNF=eCzV+R6nab|v{5e%xBPjFj?`fIO>Y&e5g3{1LwuXz z{AfsoG6+nRlmw+0K6<=}wKTY;6OXm)7kqk+A*FQV%`e^UV9Ncg{zb;Yo zikOFG+~~IQQ#bZxIzGIAnq zzdgp#v_V>doiH2spB7@{@?|+9JJY&$-H8Xg;YM=Q+CN(Xf=+K85;ZZNMmTFzWaB&; za8XqPG=>9=3mvCJvh(F-BE%n-wf-hGUJdnodLk~^6kmeCYF|dkI2FBd5gwi+jewS# zNO^OT#Xp&t1+jM7ItumocJnWxeSTy@LYk@mIhlAd^4HKEJK(<1Po|MaRc>Z)>uDX; z`R9NBm$N<9$3D8Am}Xrv^{9H{8(TMce1Gs7ZD!qzZGB>Pk;9J8*l<^`O54m>9Lt}Q zuD#kp>O8FYGE`*OVo0g37;zx z3exWM#cT1-w~?EH#E&;XIbkf%ZJ`1!+G!g9_Qx+%HX|pAsAzSE?eqLkqy^`XtMpn* z?fSl|gRVlRW7oNc#{js^QV8N*QcW=i zk$!yL3E=&S8@g&K5xcTW##)AP*c2(_$&J%& zB%Y6eB208lDSV8XF8rX*$}jd%4es6_i`>z$0+n9WU~pLqho;JZZ#>c#=gQpDId6T; zMCTz9=yCiSvn$WE;}6kep*^P-tYc&2R;QzQ`{hjEd9?0B4G8i?=PkkZ4G99CbV2umo)}*vzBYvni7s5kqdH9q z2W4!UFIIF$Hs&>xb1+wg=ybAr%zK2%Z$`1ZAibyabG+f3S8ll6QW5vOyJQ0!Wc5qV z!wp9lmd?=~s;(RG2S%7O6DF4d%z|)XSz~LoQzLtqH1e{J9}*f^`Yn%qua3WB2N5+w zLw>o>eruQN10Rn%i(s5uSC}oY!C0tFKp|t};5N!d8R#w+>8arR=eD6BlG@~yUbzda z>-riRC_Xg3fFJ!x~Cv@Z9mO|lBS)C_o_wUp$A63tSd!lt{2iB}t22Vz;vFZsj z$Rja2@s6a%w9eT+i&jrLq@`Dnnxa@COaK1SW8dnA?9tCVar$a zsawphn4S2A?E8bs>zrh-qq4BXg2H@yy{hLL^Dz0GJmRjIZN|$!`Z02L);iW_Gxcjpn)?eflZJP*{dd-Lx6!#}_q5^`E%(yf&)B!je2l~%q8X@Ca65tb ztIG!&AZlyt>cEyTp12#;A{nC=zAX#e4NA* zm%jk;nk^UL>)@bZ%fAieNVnf1f^G~h4^D_XNLP_!BS;4jrl1XpdH$$Rv{sCpD3K0| zcy@d|4C5qvC$taGjJpM3fLuT_0Eh+b|1;p}yG;V(CRD%EFTOu$1dhQaiV#2xQ}8ar zfP@Fn1EojQuEPO7KG-*d2m?_l1%X{|26QmiZRU<>`C~hP{8+&U!Gk<_ z31=Bw`HxtGw7&~(;R0R|F&G8aMWZp!V~@ZCwyykef^-lSQUu0OKplbB^I(4@@IVz3 zS0Mrv(0=}V280eDNQBe{88LZ_9oRdVw^4OvRto9r3f4)4^w#^R9Msp}Qr7bc_{FYi z#95l0zO^?&aBOV+)eXymfVH>+cYFm@(tMi+Bk%u`I0Hz427HVPf{X?Lbv%G8+e^qV z-+)8}*st)U-&Raj;Q#_yXpptS2>^uPReu?Qj{xWx3asD&UmuX)ui6*K!09Q(+8>)l z08Smq5ZrI>870%&k9t$3%Dmfk43R>#2Y|I%U}UpKTqVAEsa9Awg4RSCB&r z9skrXg5n_HC7iuc5=y`x65akcc7z&@Y}i0_ZzUy#ISe=ejUZZ}9gF#&TJ& z^UGH>=#{@MB*b2C@VTJk5*WzZui*y^zahW&62|W@%kHnk`)}d)Y?ypBhAr++JFRrDwr{6+xURE%4S~Ha4)|_Rb9ho`~?vU#sOr!X@spPOd!4 z=|P5}Ns6aZ+#rQOkKpSkLJ93r0n5kIt+&^KcEPPaF9pz}SG3@VgTqe(JgZ5Y9K}^g z;NfWjRYFj!;{<=$2M+MUw2R(H1-Hh~jqe?PAVVYoBs|3XUSU|g&;UTdZz*#lu>045 z12A?8E?S@iD9Z!^yeM$~-2RD%0tyi874;`XH4P-hNGCZHKsIy)AT}S={7U$l!!>;( zpdtYPcz*g}W&A%V;AA1+D8jIz3a|ki-;=~2Zb{5oK z42Jdr@nz*SV9OdTmMtWKvcSymNhV?g`X?>5E=9Daa>XIAz<6bpiK&B!MPgHY zJ`cIZ;N$QJLYf0bMcnyz7tkY&R{l0FJ6tJ74FI`y{UAX)|{(?yCQ_ zCa;P7IkIb$Tz&lU?~QlAAhdRo!dL(Xs;$^2-H|AxAuHSjX)ckB7YmUUpFT|w6Q{LE zh~>wW{Ht7*j<-cgrNB2Q;>1jK4ymtp&ktW-U^Cy$B0p~Tv!g5ac?UCwD*UKjfUm?p zAX@t7fXaKVsk~3Q9(I(SBx6DJer|=3Md{BUA$bY0VkV?ILusBy42Q=b-7T~L8O82&p9T}I`08MbRh`WunoG(tZ zuVx5*FnPfUm9N8ImEu&E%Qf2t|{Dd)0K8T8@1F#9dPOedzA0jCwt(6*v8S;d&R7 zClJG#iLd1Rvf|QkV_OORFX2bqR@rB{I=ZKX;casp?zxJ;%GiWap1IunC}tgsm~5bF zOa{I11;IOyGq##@n?VC=%&PA`&vin4WZmLgwqfJcxUs4Z`kYiJ$ z731Fer))wU2H#9W_Wp~8p90%WYGf5v5%iGIwGjp-G7`~%T{Y`E(KZZM2Hs2m2-ie< zE|hx@?aJa*eAvP&>j}gw5S(xqeHGby8jEc#yz!Hkp+8%|fLj2y{QUfO~=+>aJw+!A? zPXV05mw9&Yt2_HoFf>0!tz>VTL|xstL9_M)>%n50zvd8dlgthMM~|1!Vqm%9c@W@w zn#z^PZ%8rgduhTxdkkG=E4JVtc2?B6mEUFs0dql+M<-pHh7vT57IO^lCTCIKrh*vj zCnC_|u5Fkl`TZ%H6|*4B<+vi<0m>CMrkhq0G*H&7-=K$ED|KiB8Rwh^e>|6QMAgq8 zz;`*VJ^N_Z20%LL1jMO{GAtS_aHScF7YLjTzKwYb_+xYthh}^RoJ4CH|Da>I5fD>K z>f=<+?Gn!ddOD17dHLJpnfi6|Ejt@6L*xBvoxk@tAi0TjrzGWfz6_5RN;%IaNiezC zrAzKP`N~+!*eTUD^!Fn3{wtxQY$v3&UxX0+zUNd&a?7%XL3X0)WWDo3W0_%~qBcCK znKS704WI*S@u6{dB=ehK2AlW1K53RYH62~4Eq*B-{$9N`dZu7_f>gigCH}YCD6HR9 z>HxUiM802uSbm=(&TH{fBCdOvQ|pt4P2>Ou$|UlEP$OSfIlMJ?L>CLJ73}~PFL?9g zq9GN9-|D~}^C0tO+f4l^F~e8q3pl@)T#>^^Y^oU;Oq$NN6~0gC?qAiE-0cIW(&Uh6 zI#k)(`gil>^XrStnY#6cK=YI;+D+~ABWzZJHKM(s)H3K%xzg4ZT<0#@}Z^a zmoW;qyP#>5&B9ooR4w&(^b(S4~JE?U-F2A$kgTXxM%{_6-bRj}k$a}4W zqw+ad)hm5zDt`E0*)MYVbg_8F7T^{#S)7Rdov?XSE$>FK=3%gRgN;NU-=2;F2Zv&U z{n}~(tfFaBW54j;0?Kf%(onomQZ|G6ILQ|B2s}Jb20^iOd+n!kFKbA34axiiyCgE| zkkN>7@1{>>(D&G1`wU&?nO_24b!gJ9Qzz*vSJS~>dmrfKZ{Igv51(%LRqy(m3n(n<|`(PsQHoI&3(F{?2T>dV#`e3PX)iy zl50;bnY6vD#(vRY?8<-W`M{VK#$+#Fh@l@Fj9=sb(Gd7Y|E=k)9jQ514WMAAvzBqi z^PJJtqn>|LO__S8E5#;cKl0oYnUwc^l)e4@wCwD|^UVt9nl`f1 zr1wblGP|Tw1Aeo4WG;#=U!DwnZ7#_;)V{bAx#a6@Fzm0%igpG@=+XNcwv&J(ohI;y z9rW*>g_>W7|N3a?_i*M)cUt_|E0_#s_z1%|e2@L1>wK4UYSvLvhnJ-Y(T~%gcChXt zmDNCVe%ltFv^FhUh!#m%P^CdLlM&w&ZJ$M9;OuN8SXDRX{)frqc z)EB1Zr&qX4UM=b0eSzJGAu2bex=kpS#G0pXV$NMt)=H$cmiMNX!hJWfxsQ>?J@gZB z1g4%^TcZ#5&lLCQt!#+)oQ!&9T}7I`ES^UerY2B2VEqXN(DB{9g;taA9dc_HY`R`F zS>Jf8p#CV5SILw4CbV>sj4?z>4I>+xFJBo)Rr(XKCjE4iIc;gnDrJ6>j{A4nUOB2k z3J{$DwJE>2?||2m0A%ltXvNl`jra9zurZ^p&3F`hBczaS{fz^m9^Hq5-UBsbMtmv( z-@#x-zs0BT5DT6n+O<|bTM{6>NYSoN=5eKhyEH)pBb3sA;S*El+fRC;@- z>|NoISztrFm|U0ai`?bRDLrAbl-{pQ-_vlRYdC~*=grTwu0Yn)q~*iP*UXsI8YXEc z{pzS=0{ajFj(AI*%y*%fNe`Utv|^L_3h`e2@S+z zjj(ujDFZAP9Nop&{8VlS>gdTJlqg0Z=aO{NZ^t3)zJ2MFOs4QebL+&kABc3}DoDf#B zW470SUlHTVq+T_9oWHsz_jOXYf%{P!Go*O`3TyClzN&Q}%EFG4?G&!?!OVIgJMXD9 zmk1d)t-VAhWP!P03R%}F_(s;rmorfRd`+>9tz-~r4K}~yau~TdW3iNJdvXk%FotQ@ zFMOB%rTZF*@saPQ^furILD~7S$*(^h4)3&A2RA5dHeax!*}4IedHEbYpQyn7#2R*# zy2|8eq$|-Vt-CuXF~1r}xF>oCCP#i@K;O7zbuJq6AVn>WW}pkEAvbeTw$@4G_j^bz z5uk5;LQ{)_E4epq_hmBb{7tnVT4%&go1^~)*}iAP{e!rIZy8(6l;(4ZySTz!QE|mN z-9jggg$zzftY2Q#C^xN}j1upe4&+N-nhMJ-)ygP{>_hQ#UF9V+x$bL5i#1co`==8r z4c6D*BBnKlF3B>=A&1kwCiG$s(CKNsa_~}Tjt5t-IvXrUMj(>s0N7* zu-?hAU;DWz>!hLv8OcV#*}11Rbz)BbP>19M-|d1fyipG~zjAX+yF0WID$R_Qk>T%$ z_1J<&Le~UerQgSq!?grb^!s*>xw*_^bA{ZQ|2O)9a*39!^|GkQDr$ z|LqBb(*NDUCxXsoy*{ODgyB{0K31O5z#A2&`Jo=|-O)fkg)5I^_q_BIaBOP7(3Juk zr?r{P*Es$6H-7lxsxezMB%t%MzuE@>_7r(~WKvnOHyNc6Js*Q~U-z94s@4@6p;S_> zjwkz7?c++Vt8*DIwHQKJPdrV7RI?9AOi~vpq#ELaq2{F#EX{(N04Ytxz-;H{MPRs6 zZZuc2;koVJl8GJCO|Rc0(u>3rGp@j!1APk%|66P)w^>LkM z5Q*T*T4Nof*5FVI#%c=KMgxnt&H{LPuGpo+w&>Zaw5)J#S7lnAhw8D9_wWPXw483X{^nTwxiem@6gKHd$1#~G)SC}%Z2|Q60%qTv zpSJU9V@agqSfRU7Q)RtpIiqPLeC_^L7ZkOrHgnL?*Up2U<#TdQ*(!GMiU5C6x+A6s zfuHP#XXN~8ECp{r&#lG_WUH!VtQp5I%r0Vwt+0VROsd%Uck|uJ;2tuRyG%tOkvPG0 z@rO2=B-LZ*m%QO>VE0e&4jbK}Y;qC-4$yuMN3o1*_=7Kdr){w4v-&0daTR^fsE=lz z;E-BF(!YYZgpw-eHpAvPgY7 z%;%Q0m88Q+tVwPbirK!(J_#Q`;!5wWwLBHM;F6um^>djGF&Zt)&GPELMsp2AWg!6@ zus_SPIq?t0)uI*~8 zpHYbs*aL<(`^~h9GZdG`PS$O3#aK*)*Lh?Anjlc)!>s#%)Uig{Rd+F2pl(Q-iHFFo z-WI}1z6Ip2-OiYuXgmEk2o)to4~qW}WAD@>4A(_lmTmJX+qP}nwr$(CZQHhO+jdpI z>7+a7;=4G1VP_{RS#z#2E>4TkCA`*Bn_rDarjdPVTQ;xLdZkD^4kYU`mr?g1(%>;l zd|#sVSH=?4wKlgXTfcG`R=dr@<#E#$VC}{9RZw zc1`Qnrlho*ULri5?$UT`W2p;mPmIy?SNqZa<8;%5~ml8y@;k4e}MM}2o)EfbGSKcrZpcFE|H*CYRh&0H-`+Z zQj56UO0pa{HbjGBg!dvqdE?wa$c-FL@|Q?j#G)e_6DnH!dNve}Dl3}1{CnefC~jv> zgOVgS@i3*vY`8L}#a8Fe7M;~~_(J0uEnWnv@Q(AOIlM-OCzA68ksqmY%BtN8ZUdh? z=#<%w%Z(p&a&$PrA|9WPs|;FoBhm93z;)5{_|_~BQckM$4l^{e*qT+l<6Z@qajlb8 zq+c{AOM?NxgjQL7a;inKGA=d4mgIgB4p&}GA7meQ34bvCY*(d~91C z=-tsgueB_(9*vwq2pPg{Hb-BenRJ?JYo0t;A(pJJUWGo)I^lyaOvG7(;Q+<`W&m}u-EHl zwP%EkvH}@fO$tT+yITbV=3u;cPP*Ug!DN0y`UlJySN+Llmf8?xq3*w|$Fl2Ej>KF? z%mkO~e75jpk;jk6F-RKis{Z9`vwg%suAfG6fYUdeMg7G~jE{4%-s-b{SvN~{yK16*%;HmgVg8Gcbz#YL(4YU~F)?%sI72peLj!Z%(HsK!j-@qQX&QG6tjEJO!cV z8DwiFio1H~W}x%>G<9zK-`K_-1pfZdUpqn*@s;W0Wh~cn?~}RnaEaQ9rQvzYAH*M{5Aj&!a+mgFS!Ok=$|Cr^I%71_{xP>o)0!|{Zn`BsP|g0UGxv6)^K&KU;^;YG8x)$MU+9=|C?^|OT9Ck_|$ ztVDR2dauAgq@3;RO_cycpd?ovn~aC9w2WTppW(3RC2{YppcG&BUHntGf)k%hY?Xv8 z*z^<=ybo7Oy-$_6l7vun#;Ya%xJml4z5byFeheTwNt78KvrGlC(=LHG=G!XSyJ4F? zw(uN&WI67Be&de=|HZW2u!{2Zmd{g?L-~))tH(A`r2q%z7GaIs8-cT|P9oj=1JshYxB|`)olvF zq{QAj<-2*zd!<^P*PKgdvvr+|qM3@%xG|$fBFptV8s-P2;!j-HQzlqh+UI4g`qXJ& z%`-qJC1Xeh-03s{u&_6iEI0m&V4V8=omH6^~$7l^kkJB)1wdXnA3br1_&aq@w8RGe1nyeBdPK0n1+^u0iI z-;rt+*bSMjSfInAo(x9KNr;4*9>UUd8t>Q*+tivRuiT+|gr0ZP-EJDkyi|+G4qeuhpnR!!>LZ2dW3Euinzli{jh*i-VM6<; z=ryieH`Pf_sj{vx?x1GlfyD<6DF$M2L7s6cPw3NjnWnsrIFC|;32U@Fs54JWsrMA~RS)f@*3vXk zP1sHF83d=hXp6(E%&jZ)oaL3(&I}%!q`++(H<$42_HOM?gR_U(=$d*2M%sfPm=dob zJh`38trXtqI|^m-iX6qIR{<4E$!3{<{Q>i(J@Cij`l7!2Oz#MLJ`hJWic=$|qY}Z*0UuwUJPrYSG}r8?jz`ECYfTT~ zqTYWTU|;PYl(E|lFAa>EPLS6AaZ_Qu!H7D;@C|pXV9ylx2}t6OryBqJbie&n@utC5 z$ z5mWIP+5r-y9uXfq={b^u7m5KZ;VC&YGJhZeuR!!nZzt{G$)={7feQk3YgcJN{ny@_ zR=3+%R=;9sY9v_>qc=c+wW6hXJzJ|IUqM0_&7N0yL&TaA9aH-N zS{APn7f{0@#%fp`R{YY|9*(q0C~;&o{cy8&o<1K#63Dqt6O1J;G zx6Aesu%$AgzChWTP;1qV`~}pQxm1~Tj>#pwBk<_@-fi`)tGz(n;M_6pEO@=LteJM( zu#0%r*hw1QzGvpL%cR)jKh8>(O8;>roPdzeVnNuIJ-U3BOe)<-!d`Q)NU-;{75(I9 zNl?C~@!V4z#+`KJV1J<#y}-C-6y*xAhA?8UWRs+E9lP#>Yx+eumMc^yY+F`S_uYPu z5ni@iCk8tffe8V&I#lX%AC1}kVGXtw;gJ{R)K{d~Fgw&lD~fC{HDmlfs8wRHc2yv6 zn?d@VZq$wVK65-$JZ`qSRuOk6W;IGSKsZV|FV3}D-XQdE2W>lLR8>!1JD#h~=zf3n zrJ44np-wwBcCVj(&ur&E=M0H?9lE+BXES1X8tU8j;c0ygy-;Qn6_3;#<0Tb#y>vV7 zBlD3;-u(i3tk+%fE~$lVf`luwFCOA*K>p1<*(Dh0I5JUIQI?f6(st1;8Y=-+Nzn~X zK$5b!rQ{NVR9Mc6imD)yU8_6Y+`&+oZ?N#&rhiN_g+#ji!P*;yE=ci?C1NnC8wC2FTH=Ttkv(SY=@e0oY7{qt2C)t}O_<8AnL zLmy0}q6p!`F*M#j3m#hLD#NuWt$+`BC|yY}t5F8U?pL9yivYDB`I1KDhTf%fz120a2zz@z_2J%&gFePZ^WpV z4SHR>&$mM!ClO-_?wG{_StM+E(!Y(Ed|TJYNKQueBL8({<*OU-K7HKX&_09jx)eBE zzIsXs167P>%JsucEz~)dm&l(=J$?<+agn_SACA0iMyddzKu3T9foCH#4B!)QFNf&) zJf0~1==RtN-TE-PG!`^f<0W$T2`#d;vAb9hG~%-=c7yrQp}zIeEu}Du8GvV8e#Uv9 zNHdpjI$2t7UjjdzfE$)7!)t577QA>0a$)FEqeT^BhfvMwp+BaU6%&=j6zbn}0lCvl zXnaRZVjjP;J&J|YMa~8|wa*IgG?@O-A9T%GTddxdLKV%yk~nR^;;t% zmjgaXkx6}YnOYe;@l=|QA{hU`$)-x3|5vkz{eM{lnb=wWzb+3WJ}W)_|4+#HKbaV; z94!C$Oblmm<}JIgHjT*CO)V6WnqgfPCdq|OO-({bLt^3U&E;J$su)C(8O5sBO%#!e z1IqasU*a~F_3_v7OD4BK{Rhq{=3;T{A;}m81cye`HgCgAf7%` zN1$&zYd9(%9EGhd^-~9}EAoJsLxTex4>UO`J{|~Qj}EYnY)SS?)sv9M^xgK*hQ$|9 z7soV-sUL&~bQ@J4aP&v+txa%i4*+%p_V)6z7wRX8f{y?Y>Td|JYOe*7c*wW$VA(!{ zhuP=NJcz6hH*5qC0`U9!^Ysh?Plo|@asGn$w)cg~#LBwZX-4>LcIdaItfG($fafiP z=sG`&K=Zx78oIsSd0wY#qX?)_aD)xR`9X#_Ry4f?*dcd5{$i|09B z=6HFOYxKL}l43vvW~@Vz06iYv#6coJIeCojC$>gF!$NTsFg&3CxohZpfdUHO8cdKx zpzx00MTKu7x@FjyQS@I}aPI}biM|QgFt5P^6ecC|a=~yP7xAYDDO6VYgcJjj#hhm& zCWnNBv$zR{9jv2NsfbXHjV3pT{R~=!_XeM=+{!73+p#w<-aM;z%N}BQk@o(pR_)aX zCG@ahWC>oe>$S3SY>3>9(n`lk^r9}vTPNX#?hbEk(=D5Ev7g|+PAATZBQPIGby2Qn zjk=easYGjBQCE(n8WUjYBA2{{$Yow8zuiV9?fWwGsBq?<=_@x>Eje-|9!d)|udlDu zS{H|AF;L4c@EY$UpU95Vg88vpS_xGPMm75v=DKwsrli#|%_P~F1^GA3Hb5qkJWT<| zBeicT|~W;ULD{>dUy8TrgY zGuyYPo#$0~4k6HKHECz#tGWjrp>*cHk&ian z1fhB}YBiy4S8R4%_K1`Wl*uSpnm1y_3fdoid(4R#!oyV}A5lXZwolML0Tj)czp zn~`gNz3>t1WmpEN6fV0d6`OIq9pD4W6q!V*PqS17{OE54E0L%a&zFg_bX?xcdDK9C z^Sxc7D8X7GKCP}9Nid9%{fQpraXjY`y_yM2FPKll8Ty5ap9(f9IHGXq9lo zOCYD827aSq{pPD8Sv=S1*nRH(kGGUJvxp-+mk$GLC#^Q96C+_Bat|Rq=w8Hp*(W%3 zV+79}X&kXy7Hz4KBnivPI_h865Sb+T43|`URl~JShqWT1uN~97s-|q52JHA$D7(WD zs~H9co7azuI=|3J;!fyMV&MC>%$AMPFyQ~*tn}vLf~PV~U!Yc5CL*ys_+_-#&81JY zb!2B3tnHf}IbuIiFHGY-RTTbBMN-vfs$@su%-}3Dx)T>$&XuE-lY-RRF}_7GtA|JI zk$#`wE#1V+drakL)!NdqgWx-U?@^VqcOi5=45}cS%MR2-jN`XuCHu7c$Qv_+$7Po*7Rj_SF@HyW<{32{8xT`-Z2Sd&b# zRK$>_TX~1g>6o%6O}mZK4ICLHhlaG+Oq?$z>VxpMp;Kbd;{Q2d&a zja7o?j!4c^3R+6%8Xjw`M{HDP7I;4S0VD4Xl!)EY?#v6!rzpU)YlExLDVfiUr0k%9 z!Rt=957F^(uoZ4e#_MW{?esXTtzfqFRQbHme{I)jA-X!v^cARki2F~A22=}6i5|Zi z6}!%oWl0vOOLFe|eVM<{f=NeRYfQ&YNSEOr=ibG*)=mi(50uLL3y7Vml2$g zH(Z-2V}UH9gGD2>q*`U_S+`tQ_D3mo4NmMczsfjrhQZc4+uF;BTO(=;MZtxr2Vb?s zgV@#EYF@Nt^=-k+$>?JSu1_vYdk)Iio$tFR@wOQ*{qD-P-T}ws8R)T}A`}-r?+H2Q z?JUmy7bor!&fB|z7WX4LMI1hq53|PH3uHk1HwwznanA_7IHTqEB-#uY z>}sL1`ao~6Lrr3!K1OCi zoQmGoeAmo^*BQiKXdapB(ROIL4@0}GH*Z^wCjVB0PgT*aoi-lQse4<;6YVwW-4?R? zD#>Awd&e;m=#(Ct4jADHKWPnlb5=_GP;USgsgXx~O&2aTDZJ^9YuK~VxaK2AXe~})I@%5Ua$*d$)=-D!n8fMZ=n6ZBg zOIT`4`RZC6)7(M~5&T-xQF4eLnIkm6qsDTL-qqj5R%$?l&6b2Yq96konuRO53yq_& z2;GMfOTU^VZ%W-ld`XGIvvXBJEZdrCb%%u-W5d2)@TvXk4mP1{4e608jx%BT%%|Td zUOae$Aj>Rh@uUClSZ+T3ESr?Lvh~0&Wo)wQyy1cQ!jdeLVDd~2a5S_)Pop6o&TFl^1pJcyK3(2n=~elmA~=~FeeDnnAf za#nmFS#a0$yFX;L(t3m%dYV2-M99Pc^s3wVQ4G(L>4;sKP25y*S9UmRW;ZGb^_5VG z+BL%=79Ne6$RSKZ1Bp{B?ZvJphkaGdkTc1IBd*rjoLS-p&4DAaXJ^U)sde-as5

znplA&?2%io%rMo^*pu-d={lJm=I)XJeYD}s8T~rk)FgD5 zulRWj5(PUIdusA;ho)Um(qbtHJsG?5=mrL^2>w$~Th?{F>t_Ff1e6DLWkrNl;;*T0 zu=l)^dFfEcl0Go^&UIy*W_zKyeg!wO1!PX;eQjK(T0eKeK1g;Xhq_(x-<7&ljij6O z!zEftv5z`CSq4J~a0@(Z+pNc_Jn-ON`8*u8NoAj7JPP3v(gzC?uWOC(s({wos~CaM zW$(Im+4OrfRyL+&XjQ;x>tkxLFD1BFX((AgH5Zb;;5QLq{0bYdWhdG4o*H*rZ!e-Z z-{H{H_j``7s#&*P;8a%po=n%wLYm&)qcd1RwBc*uWh z_f|)^S`Q{ePmv1QN>w^T(3oEC0KOEd>;NQ^(7k%wsFB7@G~-WV{g{n+sMzS7A2q3! z#`5Yp=AIxFwUKeqi~)M40=JMfHB6+8>isTTyf69Vmk+2(y~%oZ|sGckWl|8q30qry3n;8l3s*y#zRixOrUfLOvuN%kXP*`Je+ zBILpIOifz2r*|){{dHwjR5;xQ^O-nUNL^g+c#Q_e!p-n6{l~9|HFHFGE(4;NP~5vC zU9esdE`2M1BjgMX4kJ5J)Et`L;|j?*G~JljLXT~(=G6T&KYMBy~$LCUzysTpveGH8pf9@c9gN?8=M z-LXW6D=FyOc;Bg|dS{;u9hqGaCX3$)r=*J{Bt;3p2tFLwC!8{8m^(AO8;|hPRYh=OXaGE_*HB*2;fS2uM zk+^Lbq+}>s=-l?F#9e=+fMOp&Xhiwm!p%bEtLX1C#5ZB7MqLp3arW ztsL&5r8KHM?U~Hq6*)|v+A5U=D2!xM&E-=V#OOPC(XQuCO3i@wzM?Q#@L}~=n5U1qn`&ko97&3KIY{`+EMzU6+S<9 zDaC&<6@Aujt$ea8xry?e`|R42S_x)gBLN5TXQ0+zvTUu{%U+HDMZxWVYzvPC*uCwGDAs3gRN3u+TaZqDUq;r%Q z?Sdyfcg*HJ*>{bvZqHX!5lQoXama3N?`+jB1?wciYtGt-12u0mHyHgHr~9~hb#h4% z(w!fJd_ZkO#!|FXAg`00`q)*nk){Jw2eN8rKsxavpZ%N*E1 zPnEtd_QF$3v%HQh4(fonrzk$X*hZ@~xEvY{F?M)Al9g2A(em{ioDQ#ROwkcD`fd}6 zWZK)T<>u{-r*`C%cau%U>e{zeo^`z_*|MUUn0D^rQRzvEc1PUWdVsOEcjNi7ik zoOmV5SidyZ6r~CJXrCE2uio@Ri0(kFVN6$(JLjp2ix#R!;)_geb=Yg#(}s{3oAQS; z&u`Yc%x7QnfOEBrl|RI%{*^a=3=|ayqiOyCAEEawB0jV_z1)1=j0I~{=oGJl4cFyb zq@*4gnW66 zd2T-?Z)Ok>qSl|DodQxC2^Ls#IDsVJ0h(ei;qrAUWS1u7Ic{llO$yCb&2#t>Af4=a z=ZxnoPg=%|{C=;_k6{;)2F!fHwi{u$4O!}ITQmV@V`M(!r!_pkTVx?tc(+^hQ{myR$?k62$B+ zc$62*X}&?+NiFVhCV8!5s#JA(Lrz4YXC&;EO63M_8m1WvJYhs^a)0Rc^8M z-Fv0k+vbpQLBA?@;jPf6JR>T&A@btcdcD=9Zc(V@SU+s!apf;xL~J0H2GNxe4kfJ$li|`)7Bj8m2!AI%(HA*@DTMXWJqIJg6hWO zjnnA!{+c|nOwp2mL5ZSe<)$>wQyq)W87mdXKoXa%dRlvKJep4PTX+3m%VB_Z=*Vff#>2MwmI!j zU8P<2e4*H4swHp{G~)$#A6GKTVj55j>P%bFc!wdI9Xr)DB&hC4v_;t@-K7gfoCsB& zKVG>00e2j)(83)1t*U^mG^~S+hyjVJJ2MCyjC|&h8Aml1nXUqN>2xb24*Sx5SeQ00 zYad2juFkVHTL!^1zXt(=aK31LHW|VZpkrB%dp8E3!d50;5iN(l;G3H+dq816*52fT z!L1MiX)e27)Y&tl5!6_Gu)#O_E*)RPG7Z`PQ=oyx^ch$p=z z-0b8ctR7&bk{jUwJIg*+c5vVh%V&=yupes|$s9d|{Bg7RGTP~aXR8B$+@6W^@{$q? z@sxYf#g_&0sWXcz&v&^mKw9U~bF{T^?;kCaTA*$ofjJ04gB42h7O8@3H4(Kj%%k2l z^*S-ORpHNSKz@?oF|<*XSaE&zFt`nk)a=d8HEyCiZvpDaDFu@7*1b|eL=V5qhA@^rC;Af|jakabr!+3lY-u;cK7S@H zZA1?wk?sy-z7P*!pwP^l% zR<9u7yEuZ(@@o0VO3RW3$&T>j%E%H*uDx}_NrXnqWxtFjZspratmk}V1tRI$MV)%A zrLI)*Q6mBvpw$kY6Xmg(_puiT4zJ;B$a3Dd4Q%T-=t)|-?uMvS)E(`MXt>&rps1?^ zdV;+9z3ccz3NL``09}WitwHedzAA+!KzH6h*zrd@u=dhsLbl!(2|5rKRg4N6>**AJ6t!}x z40?ko7KD^5ycLxZNWar5xbfV)?a>k!%g!@a)!dCTJ`WMi=&B#cQC{XDT?39N8bhu1 z>i(kPNflxHG$R4#qy0EO^Ew3|X(xNLV_A)T{9u%RqIx@?=cO+&XJX&vBtb!@Ef5e) z`X$jn!I}~6cO3(@IwUr^QJjF2s38LH9}Nh+u<6u~?74;%7GM>LCk{<^_tuKF<0#{O z-~T~N&1L!RQq>bakdir$mcyWp`RvVjc>VAdHQIuF6`$F_Q-dCl^DAdl&V-^}hA|^{u|uXjq+I_vH1=?VfSZX$%eK{|BwN0&WW= zA21OAhd?KfgI`xrKnMf?1nM5Z@9T|*=0m;+`Iv^CH-c~&>OW|3_y-v87UZ8mo(F^x zF=)|m0HESV1|SXqfP@Af5e^&x;EOMRt;gS<#m*0S4cG!`eh;L4Ko0?wC-U$1@CK}< zNvyE-V-2a_b_NjO|7F(8f{$|wZRfA*WAFnKUxdg zBIFscMF5yQ0BH&BM<|DJONSkX_20zFFDrzbcMRzN^u|prf!#_{MW(mlvgWd+!kHP{v1aJ9oKHmEv>cc|-T%5u^JpIh|^U-)CKnD?2j9;dVLl*V6$H5UOWH;u2Wb}d3HY5;{>yp%GkUM1{KKC3>mnd=BP-i8 zc-?#WBZF}b<@ERies?#p zC#D4T&)k65hjttKbNR1t0l-_OSG$J>K@PGX^czM2v+wi{uLRP6_TC057=YL}3MPanOiv0}$b9Jw-fBuPUy#?NTihHs&&&}=q6 zo6a*S=RW&&ny$&@9}HzNGU@9?iwdSk-=nRq%p`x46{XKD0?KKBt{R3EpfJWdyjxTp zsx}~ts%+1x2mmmv=Kq+J2KWz4&fOfIc)-8rp=?eo7HgeY{*#w1W_-A>*MjQvF}M~c7eUG5>|(yentN4=#2 z&T*sEjAy(J4L8>{I%A&Qa$>EYtlVX7rhy{vceU%3$+4anwBIzP6?xl!#PeFFJTC5o zst*Zc>B+u3OrX1ra0E}OxNJSEBOHg;7C-A<_ihv`WQU)PW9w)gs-eQM-l*=l}uXk$s*vB+RNjgN(W=Sc^Cdiix4)wY268Ggdq4-Hht z&qu8CF_@n<^e)zDjSxL;c7PmO;d_d`juWYN4j$qXxx$mHW5ZSVAWV-;&0oJNzlEE? zFnCKaK1;16@x?8WR4N*ACRBu8qwPWa;S!^k7feF>ETA6SJd{i`UmJZ#)CwREfcfX< zh`x<;!o==%mRe<)?%Gp&@ag^3Vcd|PB8Mi&!w?-_j)ki&`H53s!r)|qKY&%Evt}IN zH=n3@qdw!uVK2!{YB7U(`p3M1;g3^={p^GabeM)$GQ(tCHGR`;*YN zo4z-7jwN|aut;%X8|5lCw zPI)u*;MthLwc{44pl}O#QH!=r6~Bo3M1B^ix?C6HA7bnl|xy!_kDFw^$7i}wl|OIgTkCf8j+ zJI;Gcmd3h?B!1e+Zwfw+1lv2T#$~s?^GC4Fi^wCjJ!Vj3YoB)iDosrvB4BZoEOYr< zQMqU~^YNqwF6Q7?JfxZJPGrrfcdP-Mr8*mvi?$na=BBsSwrdqW&P#=YGwx+&G@5Rb z1Q0RSw9*wR*M4fa_x6(M3VcHd zVz0}rl~{g@rE(;Fi?@@Q+0jFaQq=D>Vrq&o=(&sstJ{gqsJX`8&foSDeP*`#&fIZJ z%gUy(1?;t=lX>5971d6D=B%=gf~5C7mT5vpRZD@;fCQ7eZ+mCJTToPVN!mA>OzKjM z&+@Am&Kh8?ernv;fE(77aA3I6qVD!V^}*zJFsI_CKUxnK`4(t6OVWopXaAOQ>y4)$(l0O#19O0Z6)(mtSJataMQUp%_0(DKB}AeiLY`bcC8R z&!n##iqkhCSKW9qs7+4VVOz}tox3ays^7mbBM^KPWybMPpa=*7OdB&OavHSw=UZkh z68PF>AG(F;!UntGQR?JtML( zGN_b{kob5zO>Z?BL*B@|7QO@qtr|ISjr`1zQ=xx_DpBJ0p$f{30hdVnajU^=KcP_pnN>5u# z%)B}!NYu6P!Evu-9@Z!BFtvYnF^Yv=2%E9J@+g0Nqf>ivet3TWc!o2N*!k6El-VKY&@ab?9Nb)l{t8V zVa+`5wYj`}O7DdEU!$K+`swx|JK{#sSbQEdt?=z?AK8l)JhY;TU0I@b(@>U!2+zK3 zR$~6&B&a1H7NsXQBMmw9w=fcI{v+{>KZ?_-w&&eLBWwxyDWMlx%}TS@dDP|<=aX77 zqT!iLJbw&NEPZL#90Tgxe}6yDt$i{FG3(GIO8}$a85me{y7gCIbNBQ;+)S$Rl!iW1 z-2tNlP%IG)jctF_=iR(- zEkoBypbki^djdkM(M*#&P#wn?6TXuOi=iqZ^9gy9^i3yU2IGU4?J?-&Y?Mk&*20OW zN=ydh0mZ{AL{GvTYHx?#(JB0Nr|fT@k6EN%<;;R$FI=VulZdy);*mRh1k8Vw)J@de zmCo3KC<*2Hwd8(4;-q6e-1Y_hV!8(tUhS>Rc+MF6EYnBxlZ6enaw2%#VtPW-H#eq| z8QhS2R$@;x7)J5nb=k+)B14-cjZ-$|<)0@uBDt=(+h*%AQ8XsOazZ<%N+g#i>jJq) z;DLBLPni_AJ$dgR?E#hg%7KE>z4~f5cf=EMjLc|wBkQemP}9p4;Sk;$r(P@&N&dNs zyw|O6o7edCY6+@@k3#It!m;CXB~!5Sth(Tu#)OmA=J<=weo~J|shg@rLK-cL?XoM7 z031Av(!WN~cnMc=;wv+n+85IKZtoc;3+>?U*E|)&a41_bt>Ls$>{+i#xxd}GD6ucv zpSim&uS+RI$t?IiquLED-{@b3dD?g1dvyf;>peuRnFrUyaOkmEa4Yu!OZS3MrfrXv zIG$a9@>kifb_u)efRE*T6kav6f=eNiERafv)Ymq|6K5i=!#J?sO$tll+M<>Az02V) z3^4ij`bch6Cmq6tfGY2<-byy<(a42;HsH5WZGg`8g>$M}RXuIKss^2XEu}~Ys;+(+ z;#HRC_DCi^o&nD4VIe+jk5#S7q;tl*7x%wUfTrws6|~qsX6#wjVx6!#?hqv#3BmJ^ z130%?u$gtO($d0b(+x0>lE`LbMbQ@1zcLU)O%Q^Y2k25?2@Rv6RfC#Onc4kf(f zJD%QJ*D}kE+=YVh+{e^7iyCG@YI$#h>k3^X*g9DGg)#yKET4kTJvdBWg5S@`lrpqI z>vX2uaY2o!wJzy~yz7Hu=TTDcWCym#nsX09;(|xXF6;enanW847H8UMVF1Ixu5%Xd z#$LqHGZ`Bz*YG|fm)Kj&UIR=&9D9we7}3U$>>Ha@ywa11epuDD*9YdWQ_ga4*N9(_ zx7NAl+Mw#Q4lfe*WXK8*<(o_|e;x^C9QA&0_#dT3sYZ65nrTRy$8Ow=_Q=@rPdDbP zU1I0ne4mouN5t&o+%z1-Q%Orex^HEVIWpt(P!OMRH8g>n2t5>#bOxM?4q*QOQrH zX5bz(n%@_Vq)L=4vr+qwexm2B?*2#j(io`succgm=0(V$_PnB z8BKl58;lSHwiWwkaHakQFQj#{PuWl}RfxB~v26`~*}zyyx-cbKw6YXB$~3h?5A;lp zT;rJARV`w;==zv8WF{9}I-Nwat=Xca=BT@s(OtcKMWYte{HEhVZylTzGL}{1r7tMD zIcySk{gYr`Uwg#3NoH+=a9dL|5=qt#Xi`o4^3Dk<`z}osc6(i_?i+E>ZI#QL4L=%% zO1;XlfQ1t(CePpTjxsz0H>jyd(v*IsijL$jhDh4@wAJ14p_?Jr5CXLUH!hd$v37xp z_zgT~ey6AQfk~^Ej+Jz1_pOH`_u*N3p@RJla}zxQx=5HD+LvNGk9JAinj|>dg~+W9k^iepWVK(s{g2v zg&dP-0}1DQfzC!S=GB!yY{frc-$&p75Drh^DLU9qK=Ie*j`%nrF441+P3rJMHTiQO zM5pA`aJ5TtGLPPx(y_Xbm8FuYH0Sj(YDpss&S8{Gc1=rWp!fH zY57{5x12erB)WK)dB!Y8PkQyqoTJO2KtXC@BSPH>*!uKqZC{%Y0mW-%oAoJD8&k~y z!Tiu>Zh*I$#Nk<)u!Fdz!}HNH(|9UN>)~l(S&0qvztY30})e0^?+kMI(WTMb=1py9G^GFKR9yZYbf^)txs=X2<$?*-MZd9<~;Arr_B)k z-yM9CZhb3;(|^l#HZ4o0>;}K#B=m7#lLb3nG!Z)63oc3>XR0-Wdt@`XFqSsZpV`sJ zi;ZX4-N1Cmx+=iG2hcfIEb@`gbCIf<0>S^TYb-xvWOtkh;9F!yq{Ull)aiZ92^}dWEu)VJ6Egu)m-6T9d zeLkq6p1N7U2lKM>QdX}m<1)QvLTR%v37YJmvXA73Wq8PuS2q@ESUrS;oq@Y1=Lf(xuWQafXB+ zEyYZ7?pne@D&DdsTgEElD7LU`T6N@#F^GahE}}F!mQ&vjPYw4?L{JX9nffU|D$CEe zEbP5Z2)z}+=C^|CZ3{lsJ`)ys2|oGS3uy%eODp5%TUwj?GI0@dWG_gj>o9$;&W*;S zdWtrE`iCTmlxI`tWw{wm@iiK+)Bm&eLbINsQkZ9XsG&HdrFjpeYn%)*(VyC@AU<=2 zj9bb{kTe?p6=YBG`l$>j&6aMzQ!0o}4!WtJm3Vjq|J2!AZhQTTM31->FY|Fg6n^%> z7=dFjzt+hrIe|=u{ifam@9*W;y=0eq*V3Eer;cT^953j?wHQ*1Ob6j;Wto(s3dM(; zIL?;`%K(&R=r>sk}ebDYlhdMtaeXErY;2Pjt#rov>O)&9)j!0V3dx-Q;VGr3ceR& zOT}Y&K(J8(d$qr(^sn(&ZMqPh{xY! zCz5d|wO1;|9lp9Rw0!~}f7?o#v;vsWXog0%0cvPLQTCF6o)YHC7~U>HQ`ERWolVVN zQ_hKIYum@bLMAh-YK8oN7(0hpQQAPu9-H^rwr$(CZQHhO+qP}rW81dheLd+O{DYoW zQk5^M8dS3O-iuFnT~k%_xIe0a>6WF-!>8h5$?4>=3}xaq2FV+Re^dNH$f89u&WeAg z{#Is3dn$RTY`Qti=0Sc`Kmb+mT}Lk(sjHXKrqcARdf4Y%8&(ur0!G(F2Ds$!d+>bG zef`c(eX&WtX(lIHp3>tt9p{i2O6~QivC~@%I7V(+QqhTMS8k75m$AW;16I^&F-;x}F)_;Ey@{!MS)Q2d7Pz~%N9YCK4PMbPW-gECbM!Cx zM?MUxQ6xu0+*qyz29ukk{cr{1@t%2mb0)a6r(MMKO7&`7_RSCq#kM!Ci(5j;+!eO? zSXxuL`GXZp)tc#Ie(V)@lwP=9H#*Ypsk#y*X0Xy(UYGh6sTKpuiY|XXmQAxO&aLOD zI{34QA=F)1-o33d{Hj;5KR2##K2qD;eNIZ1`lB(E*5Cg!JlYE0dPD2#a>1>5urvNG ziK)(qA`W0Dx%NHZA0BPjQ)_oot>BEaK6cv#a#dd8g91O@qbjag28>}GT6$90F3Dxg z60lmY&a_;G{U1WAg=bipGANb4w9d3Y->*B@jt&c`ih^y=D_l?5v?jx*OVWHB2|3}^ zCFP+yxZ*R4rBVy}!Q2F295AZkGCb*Z6?w6#_Zef8_Bkhc>3!u9k~itHh>t8^`)eb1 z6Hcsu5hRF&^@->19Q<5uL-ORy#c4CQlwK>pLK~x=%~0G>qUPbIa1Yfo_@C`6jaV9H zH-kkjdL;_cbg(g0rok4AdBdDuoJMPcd+~M(M=~rPueey-j3tt~XT57)ZTR%JLheQE zyZj$;vb#JpgJ(E7Z`PMr3sl`)bQTcmNZ&Ho0Z%c5N~G1>@R1CK_&7)CHoZG4tIPt) z+V1w)N{VUIA2CoG1!|tNX?pZQA#8lm;gak?>ct}If5Xm=31fm|yg>Dmc)5{5qO%@l; z?RQ$4WxOLp2WaR#bAW}X-O6eg&9W|~n&{lFo>UNn*@!>EE%4K0FYbBowmM4+t>S;jCr7M-*=WYoXQ}@yt!qWq%&WB*p0^I zK_E_Wa^W(HFEXTxY0&fKA1f#Fnre~VCpimfXG?-*waWPnoqU(8@f-}lZC4(#a9NP4 zh9nT4Hhp&|e1-&Svz@xyd+rL?0=+m0o_1Kna)IQyX07KQ@*srN>Rap<9JF#^JBbklHkE`YgXM~<7AAVGvL z303r-WmON7F%k7!qM1JDwwc8MpNJ4{`vaQ}-641CdDb8XoMWq(_N3~1x)v0u3d9Y5 z-D*KM{u%-&Y*%qS06GJ{aX1~o084gU9QWAG5&4v?G@*<7aEQV8@oD0?(t)~GLZ8cG z=Gunmv4z+++C82o6Ffnavcmy;hUkJ}@LX4uv#-h_+V&WK-W>Rn+!FWUUBVWb5?dpv zVxU)P&S*ntw+uj_X`IFvzLp1B~G9r6{N`ViEnXV05Hr{AEW z-jK9V=bcf>ZyQctg>;{__`8?nE=j@09 z(+qtS;A}kPyhj49k?34TI61aga3YT zoV_a6g=iV?^6e+2i;4JTjXXnwIQ`JnL!_9-I2^v&X%d~U?6XVr+&FLjc;ghWen^mY zd(eS+%Q(md=)_GdrA(0-5c*eCyo~HjTW?2q#i7q*@eE}60WfxiTy2*pdDt~_uM$G^`DFd*f5sbc6OA1g4BT>oKiwZKK2djKBKzgT(~A z;lDMPPS3Sr3)_Vx^d6M30)}%ny040xrEA|$au&JqaTNVsfM+v}&c&xZv8l51A}u|< z8akJvT~9}Jd#9x~hAKpkWoM|`Fn@*t((LSd1*WejH4j$4->vBkfo*)s@_itlHhBlz zrwi(M#!*#=z>3e5T<>cEJ+KOO*{Cxaze}8-+AqjD}FlWf2 zX#k<={^@yq8u75jihSoZ=OFQa^Y}sd_$AKYW9+A2ey2MvH#$u#Y`ea$-Clj49=xok zHafN#5Q9*a;r>M(@9zID&-8F-!dYfR(gx0K9+zyG9JW1`PNJ{qP`>KU4?-$iU-4+6D;zVDS7XfMJE= zYexvOxZ>$CG?-q~eY$|y`mz9d<>hxpyXOEHV&s3ozy<&@@tbM>B6<;`9|6pZ_#4PK zkMT*`hi?`x$QpKc+uGW4!lNS#M(W4p0{IJW3On+X;9kIAD-VEq4)lEgu^a2B zN$l+n009#XrY>NkZ=d+9JT@qQ$;ayYE}qWSj~srCiwE#_`FhLdMBRfA(H=DBH|Tep z_F9op+mHzNzx4e&DMh3mxV!yb1aRw1kRJd~UL68I+UNe=48sQD$_}gl_o%YJ25~K# z@7nP8T#u&f#{^(Z&&3|-M?)GZpo9jw{~j+j3oyiA|0?MAH|w%b>BkQFSLOJ3kz1*-csmS_mB=Na(RjGQ{F!aFU}51XGu{d(v> z5!Y92di6TrQ$I9*3n?zWvl;l_hoF@vl#dDA$7hP$ZHJ6em>mAq%u#P$hWi-> z6vC2h((%3Iu6P=u#j+T_OsN`>hfAf>Vg^42M8=f6vAA4v3+pZIi%gxYG;}jLX@bfJ zRvwg|z^in7R0{IkaE)oXM8Ru?(|e@F~Xs!FyMvk~ZoKxadap=5rxrhm&gorKNiV6?b# zMzlIErySH+uSNWc%LeuuMUkwXQ{DRn) z0PVmppTEV!U@=D&8>jO{{M60srS;(gco_3=Q_+SAVf(Z7IX+u$^~KpD`=af^b*KTQ zLmf1M}?)ule>fs?V;?MV%UjP9A?7-4IO+2{)!%w+qz4u5;#d+i(f3=G;Q_)9p z>LdoUmU<~j?2b1VE-|&-Tk4*IN#T6Qbg3vA%RY^rr}dBS!~889$qwG!<(dVi5rQYd zrduPnfiRJOnb}&8Z%xWkNJG~9#DQimoK=@nC96vw(M?+0L9D@K;ZI%Enr)WOyho;C zYE8<3in3yM5tOik9P`_bAEBj^&}woaxu$^9Vnc1cbbuNT?diI&<9_GV{;B$yKF+Kv25?ESLNv5D2rE>?(IkC_LirfGg# z|D@ygHxgq8f)JNQuqQlR7?)rz+nRf9p*!2lgamGzCcoD|exHjL$7>9t4z@iJfh{rk ze>m~JRI_;qXX87PP+Qngl|NAWa94%+>l8IUa~EjJ{=V_w)Xoaa<^w0xKoQXW+Gh8P zh&-czLN7w&f$pE+uzzFPQr}oB?(s0-46o?IL(k5z2ffWGp{0?&5IZ`^U*Y852iHTt zLgm$*WwJ)Smv1*Y_uL1?LE|13IR`^MV?*hLs6EEkJ;`NttOP9?YHq9ra|hzkM%<{m zWiie*7phb^2epVjW3qj*qf)str8~KosIl<6q+7Wp;A8$2OMJfJz*HmA&cLs1G{ zb59CL6y-+?JfT4?K73K=$4@viGS$`(*tC=v#PJXUkX)9H%Y`g(qxS~PJvI=#QFC9T zx-8jfc=lCjnia}A+X!oSUf-b|o+RV(5|WZ#Rc$LD-Zza5oV zX!p)JPa2Kr>i$kv%#D(x)ARkB_(s=rqjEuI(D{V-d=b371=@{`Fr^-A`)r zB@S^_1l*Ada(--Eb^Vf5qguII(f4jTt|dz~vY?a#pDf7}>_v_A(_&h@adRS%)sc)k z8egu(OO+!i^>ShG*wYY)||CkC%oiH@D1(Oj^gK1$K zWz`-B<)LVI;0fDYp?X~Fs~yA4J5j>DKX(%6v9tex(gL@45L%?OU!o^~PxDaA@f_P8 z9~>n*>?#kJyWyOB5I>0`;`7Fd)tAie(ki?n_L|KYZc$VoUSbab+_hf3&7xJRiDbFa zY@!JyC!+;2IwU<7g@F*Fz|>~BkFZ;E0Ifw}e|#gEfu1-uFf|TZw&rz0+e3*ZD>mBT ztj4}FX>YpKYz&fKkCPWRN8n~A@Mbo;3)`cIe_MD*zrhlXjOoI4f)d5wC_8;@(He7p zC4Vfr)b=|vkEGW$&B4{}O?SVscpd^xfZ;}Dr$jtF5BE%@D}8`F^D`UL;uUuojoe(^ zENf>n-BC{M@qIfS>I~K_pd=YB5f|B{1h;kIV>-Cutl!f@>NEYF-mCD)N{N(=TnKFA zG+@sSoWE2lU5~psiP^HU+0vXu&(*`cSBrVPeP|?Insii(=RIC&DB#)nS={fG!A3av z^I;xl-(P|CPd|ieUxL9w+w6_@(#S8S0d0-e_3-#+J8GuYv;^+qrAm~Qp(8TSD!)iO=v$Y*s={fS)w>MyK4@)-9AxRe{H zfB4JbCuXB#EiRl3v_t|`{+$r(0oBjLztxryLZPZ3grrK9MTzRWr`g_DF*R z{#vseO+a=7Lq7fvx2P<0?YV_ZCIr6m>JG*;e+7M|T(sU`F3cQ@$o{^0Bh}zpeTO(n zK1e*9R;6cmvVLsZimTJ~!8R)z`#9TvF;BXhd`dYOb-mI>M0jPA>L3FJ6)^ zBA3hhCq@P41vfg)sML4~wjckyPD38eU)DC-gL}cEMhUsqoUoH&;FrWxiJ9sZ$uM!2 zl}n45k($2>B(K1?;2=>^GRL`*8f^B^9}?=!A!`G zup$TNeO_dNXgh~!c;!5J(<@~_cq>gj0-(suv&PHt$cGOJX`>HQ%XZ2jV_#SDGkyMq zP}we3zQca5|Exe7`!_2nBNXKP$cImgy9$=0jQQHwXhz@o8VQsQX5t!R2qj%i>2&3% zQR;4{x+K<&>wi6+EN@`724dT>F1-Jp+Io;Ya112qaZ7NG*qtoe$iVoMstZvnjd7{JDpP~Ve>L=q(S*ulL=6e@CC7*##I?F z@EQpLR}8F6fkkUlQpXL_uohyOuaqtF&v9ziQGr?nl?%D89tE2(H2ylr=7cf>b7`l< z#K)UgM5q7Px?p@r?v7BDh^o$xf`mfq&?lP;X%7RmHaqA3A!w{vfIX-bo7m#L%(Bnx zU)@{l{jU~jhE@%)&Sm`vrA!YxoDR*d{wK*k{hZt(owsZwsuakS10KS%LqLZ6;fCfM zPQUK-nG>noDy}#kQJM!up{OIL5vh`##^}^5y9O&SZq%RVhw{Img=K|&%GEX2nH z=4mSFEi@#q?jE*qy)BKbUD{!I6RQv{@1sE z03r80R?;a@piek=6al$IbE~=6hq3vh7n!_e&?2C6V#z2{I$uIhAr%KFv^#Q++snhNUN~+2QMYaI#^tLK>D_RI~xn3X(G*76O;LO}f2H zK34QDI`Vx6>;<5}9JODlEKra=;BD zov@auw!gQCa{gk(klaSXh{~vtoO;wH??Y#rhALvX5I@TmD%Gjg=a`RW-Tv;2l*@d4r{O#jMNM+=(v>^#&3NEuS zrpD!gWO5(xb~^yxKt=uLuHRfv!=oP08RzzN4_oATI=5J5KOpH(xMHgCNwa5Z)3d^h zm)H0`too7)xS2Q!I16F2ToG3vpi%}ez@=jY23dBN%+Hw@{G|m&TxC_uavIvE@HqWG zkJEZr(Lyr(qQY<#K{j^x5l?Mpi(dB}D#IZw4dFr5$iK7i!MWF0OF0&HNDPB0$OCbMMggA4w|kK!zg^yvD1;rFmysZTOAuGP8{4fr8i!WX zw4IP4CVOn-35=a(shBOAz}gXOOb3vhUopW=WnTaZ68O{-CcP29fFTIzZTMm53b%pUyNMhkI2AouR7j%mPu=Fd42`u=HVm2%$6`F7AbdGVIrIs`(WD5Jzvf<&(s)01uB#laE4)1&4zm$G_;MreFYUZB_A+FJecA{|qiz{jnxgBg5xrcWLnD^GmzrINHp1v7H; z^{V51jCEdWDwUn24B0o)ovDQ9goUjvPqufKT`Pe$L67n}!g`(--SJAkIgTG2lhb9i zYEeTy)Gt6J?_hD1wOc#ukDn2^Z0K9KV=kA&Ac3gsN^EvY_@Zdsr@3d?vxs1ViX4== zuWM8pW8tn%(W82X0 z_Xe(|Dc}5^Ape|lnC#m6^a?7D5`zv#N8qQ5EuZJWVb78N9bt)hV@Q<6%&ZZ+-5>Mx zX!yEZB$iq*+{OrzXxmD8gUQE9v?pKF<(`yil27ydnII2@lxS(+yvIE84u5!H<^Aq1 zaGW_-mcn)hB++eFg=-v1WX&E#Jex%dBl)5A*02-qzA_J&O>RoKX3L1did1!3dNUeL zMU1zAx7rEVbJAIU#-h195f(L`L*}bSW9cHKV`6Gv@v1?7iuoAwrFzWfmNLdRlMMTp zfGSHWpL4w#!CfJb^xXxQugjigZ7Rif?i*CIt_ebx7c3Csk%&+ZHql-G=| zsjKQiJf3gz;6qJuZZk>6?JQ~*@;ZDnN?f6+G|VkLyOf6K!w4d(Ds6jOrd_@lQI-Im zP!ItTo{1%Ui&Tgf_S`)L>X?3e;~pNf(#;pPeVLem$IJdU{SBPbS{8cWI(X9x3>2tU zq|@XslEMD&ebk?vR24=X+N_EHUcqdwD_c#^B(J64UB!YA=4>4QTeb`SPn16VOL}{v zlCqP#oPWb6t8*IKlukPN*dn;H7o_o8kF5*B^{Xh)NlRqn5^kEBkZvt;N?l$2{G>dn zGF(t@Ss5j9>FeXF=jvG)`KrjD{j~P{l$_Cbrgk>3)ER<3-$uIi3mz=^e5aAxM9gi$svj&nQVA_}c%ztLEvzkX^{>&7|UUw_Vg z3hP&+AEQvG;#CnBiP%XzY2%}nw0^C+I(Bxo0e-DWlajjqT06m<=z@{V4O^^Qw#MZK zS~-6U?BFJdVn$q))@LQV)bu8AT_7=^{w>L!PfGqsqA|xo2fe4lCsymORLE!& z@wcst(3KReTM%VCyG{JCpz(n0I#q6q?t3)HI70Yl%q)RJTuq@+M9u(M7d@-x+=`!v#aZb&r`2h}KuMVlV4qED-$Pe5H0G z+S|-N67Q1O@W2xoceD=<-8bS2E<_Tcc?h$waxq39Ke!T6+Hw!0PMj)VmXfN^IRBEq z&al+P^xvt-Im73Mf01+B#OU(}5hc<7nh+OANj$aBe;x#EmDbYvR0@&6=f@`8Rsx6V?XKvk=_U0dX#-ts9oJsG|rGa z&Ck|f1Ol0US4=a&Z}Tviek{%-y}=?g?^e{4(zFlmQiN(*Gl!CH zFDuM;C8``Y0nkmq%c7ROC=(ccD2Y39pb1jlcHhZ6Q+o*5B|6q$OGXvy1l)4$6{|O@ z%BAe=hxz2jI_srPD3Z2wg$Z3ZiVWAowyI_g4ZL~p@Bap_RZGs~+=?-m_R}>Xm`mMX zBASF0^4TiuZ7EFJ{8arlEc(2+p9w(0+sSkUKm7nUA_&a>i+3{r2k&HLW@h-mypxfh zgY`dwCnF;hJN^GD97EBGT39=qIO5ZZS{pc<2%8w$8Jj@y@tNCzasz0|@`M^ASHFo3F{ngTE}VG%JgQ2~L5bpFe5-tifNK*{7cMYRO+ z+~R>5&hjIv#Y=AuqZpU%>BGmnG=bLF1FExoXtcX$WB^uAPjmkoo$XCSBd|HNv4WG+ z0~zC71<*nu8^Xov&aR}Y*5=dyJ|gkkhyv^H@9!7>ZsOn@f;+deGBtpcr?+bYUh^Z) zNLvG1!kMn{tJVD$=AX1Vy0{#f7(U$H=`%Mt893XwA{Q5cyQ_C;0xAd4@|(HkbL`ax z0!EU$_WMy93FiZpYiV?S5ve)V+dtDgfC2FXX<0Gl_dt&i&Fo;G!2p1QmrslXCu0Y^ z{Gw0&8u7!w$=Lx=M^pbUevf_95n6rr<;YA=V_#v*T=i66(*UBbwDS8W7IPO>l~M8Z z5AS@@2_b2rfyehnbVan(L{NB@Boa~rAjt>(Qp}A^uWu|Yhb)Y2ywv^4_$dT* z(HPv(o*tNmGrzqEyi@Z%SefN#lHcq}|H-%l_X3Ca%KHU?w(@d{>ib9UoQxOi+gu%i zC&PcoK!^q3XqrGef!NbCGCJJX0QkuO=9i`;&*?l}=*0GRB~$x`^P#@Hv2m~gQS%A? zeOFNV=lvjfX-RMR{i~X==I2i3M*N6CP*MFmS~95mkNQ|`~3Vc{8y&726k31+unB{O%#_^(U6jgKg$mMTnY=}{;dLoxtan1Q-L%2 zAJ>r_0)5|o+F_a?UH%r|ht);*gaG@W=2;8aeB?!M|26^q{pEne?Dun`;Ph`rg~fALx(a$0Iu{b2Z|E>EC1 z|GlbtjJ01oC7L}xu_^%OHrJ-lT}=!%qeX%jAg#fhdAxS zI;y6YR&YwLcJ<%qY5;WpqIW&cd5a7m03ICL{FE;(P!OG2zp&(bHYfVu%VBj6c0khE zSyEkye1aeZ@cuAR=cp6@xxO^?{$camJt#*&|H(&Sb#`{5FBSz^IDn*ay+S_(GJlCD zwgVvgh;Pv?0MdIuL^LLUi3c`AKzfO9(P{wF^WAXRe&R>02H?Ko%AV;C011-(5V6QR zBXWq$uYWx|e_8+~Wd1!3Ui#nX$qOK`iwM2C>)7gd{JS;S2PKba`b!9hkdYBYV-Ei# zHaPo}HavUdhUrHGk@Sv-fc)nz3s4#Eu8+|Er^L+>7-!iIP2P>#S13MU1n-Wn? zgR9)Y)$a(-a@5WgF20KA%=l+MDAMUI2WEVIzwd9pA+}$YfN^X-UBS2COl;0E-{NmB zHW#_oJfk|-S1u@5Ibv+9Ev?^O5V5Quzr59tIW}}Td|wO)za-kc*DoDbIXapa5?U1> zHa~-(8GxsFhIUPMfZsHjvVT?B5FQ*pT-(31q}}|xmy7!coqK%q5gwxO^#eFi^Z1*K z10jiR{yk6r_ZdB-gCofE_~Z2y=t=P#{^#=$9>f!91|jtXXZnqI{jaZOR~@3q?La*n zExk}f*>g(hfZ0UH_BA2~YeXsM;bG`wjrgKG+r>pJ$aBmc<9*begATU#Wzubv zLiWX9U8=zYV%J~cfd+9MJOXaOpw7GA+o+^E(|Pc5(>uIIWDZ%|t?@|@^jF*-#atI3 z6RM(uM=#3H4m|*+nGqhatCbl-Fvvnl9;YJ5% z2wyS<>k!_~tm*}LryVPli)z{&_apbt_TYH7vm&Kr-ok%!UUq0+l8~zJsN9*akm2>) zGbL3Fc?@`srK%bgqAM(~-i|n0m-?#{VnAMnp4+iCnK#vqaEl}9%3zgkOXPt4kC`cK)h3-@AOq0l3-GaAjHTJX+6du{r%BWz1}6liFm?yA;*MllNhcAWpw(Q{li zDp!8941jGKFp);fN?D6ODA65$xZlK%>H`~xUN8pwQL@B zJgSPn$`z)M#$g44jRXsOdf3Xuv)xTii^4R-;V|dbR`&Ep#fjWeJpD-iED>U%?^+U} zP?01CDm=3_s|DXlzEaltn=@R?045@(v*%z|-E_7d)GIBT9aw2EQ6P5{&NuBgp|8+g z4_-$qkN!C&V}x&I)3wOMg02X5p~);V`U6Frq3|hRbW*40z3^&z@#=)-1HcH-U&@L>{Yci!~fACoko zK(4^J=E`a+J#dd?!7o6eGTP})fOE3xEtopf>Q;J389k0woh$_CPiKQoa#F?-nIao+I?sfH+BGBlnDor+tWh#kRz+xM3@=~J~2 zl%xDE4llY4pHLR#)9-YS*wBFXIYi3m(2l2(tIp0_eOeu_u~U8Sy;;En8De!_UMY!| zIoOQac_hUwwi6SyYrufxc7#q``PqnJC%9;X8xY-{vGY#dWd$Hbh*Y|L@^HHC~2 z49o^6Y?tmvOQWmh=`BrIegWh$7oYYl(J|>j*jc41tSq8A)wh( zU9y{dYI|$)1783Owp_eJ5-h=DqWsDw(3H6x#cAS&QjU0)=y*t)={2?0B ztf3S*Plp|!D{WnH1j$~if8(xpa|N#F0veO{+)7G0B*#8_gO5mR`tg*$X`W9dGAHYj z_YJ-~W@?Z){$DAeiaXoZvvANGodxAdj`7O-M`b#7_C+h&<|MWPCEJx0ppT1c<{M&% z%|>fGTfs3Pq)}P9(wnYOCXg%Ix`Z-_m@En&r9MB5S4?oxj1ANYUolzAr?LNwAI0p-zp34s9ZZXZY_|hV}z(?LV$Otj?CuWMx^Q^j% z2OGN{*Ob`34q&OL-b~hgOb#=Ip-8HR9J3~qAW2^$*>5>BQQ_1sV=y8D1_p|ia<3*b z@QbwvH6vbxii`kYxZAB)1=-D7s`XG1E#(xLMI@4YZut^O&@Gj$?S?U8d0x29y56;s zvD?8qYde$}EC3oSwb>KyYqanzC}$q5n=xMYv%o<1 z>AXAlE*N}k9Q~C04HN^gpZ}Ub;zpcWc;4+?`IFn;)d}^WaAa7$Poa$wpUG^wqv)v$ zGB4<+eYPCXh&Jmt{K%dz(Lme&6-N5xd2^cUp&ksS%bxiS#geuuNNJqa(aiH%t3`V6*e?gos4NRWmH8g z;+b$$CyA$`Pi?xm(rYT}_LidGfXCdMvY8<=BSj(p?r1~sWgfIA3+FH6I}SSb^(pV* zpxuuk4;nk}U<>AGCbQxDAs!|JH<1T8h){~!+((QLD|N|`gi)!tZC5rNJ^e$*`$ohm zn6&T5d#|x6V^GP3BEt4z+d@b{D`hs1QcJdXr5bPBh7eo8Jv)SpAx`Vodl0(OX`URpGn-bm)P%MvCK=+fghaHbkR;F{17%+prE(u(sD{|C}GD;O~O^_`k}3BZCGC zSv`x!k{#hcs-?x5KL)vy0;DWjPO1j;P~$=}b+BOx#Ge50LVaht7zOFZ-WP-yOUC zWXsgUCVCf#3TQ_wcSF$uoo;SXGW*q>`Er8B)d~CT-qKCVY@83h1uKA^FKP@A z?c$?6%1RHi3qO+2SvL8UrvXWk7-%=n3v~3jc|p;z`*%~f<~T>j)LhGEm<5~)vrUGO z!DLUmmnEKUM?5yJ0M{8B?oZ@TP0#Qf5;f3Ai_j~E5)`axLXc2(fqPceupRKeQ&+8U zh}Mj0ivLFZi96T&!L3kKU>&_nSB0-3?i|2WN@!5iD4t|{n-hxopp@9YC9#-NtyymH z6T!ix1hSopc|eH{f)-pR|9gq7b<}QHxqqgtbSn3h4ZV~H?qd5vFYA)+?`yM*3Kr_2@&6Lo)k8>_RQ=9JWr*``;QgV$=-<2?FY-gMYLP3=m6$2rbLme&e70<+& z5h}l)FMdWuwqD=hXz)g`?cB5ZQ^xmF(ctaU3%C_25gq=;6Mk|rN{jHYv(uoz{Tu}p5;x(^CN4z4<|c(_9k@iuvEJKn(2{5M-!C@ej8Wl*etctut`oHm&+V?A#AVWK z^5wu*6LDNwZl(kI&SNqMRHNfWYhbfq>hA#R_NFRdUm^1BdQKF5kQdOZT!mFM?((M zPC=*VokM-+yv@nGjUx|&RECNK8LfNww_QRKzfFn0kd~UdY1IQWH7wKZxF3I8`1+P$~6L{i>I|K1X!g@YS?D7*av5bpgiEeW3!Cwgl^zoDJgVZHJ55qS7&S z$OL*BrQ-My?I8qXTwoGvv{^L_Jl>QNX`bscz#Q2NQTO5}ag%b22*a?^+E<|Ix6(?@ zOdPg}wK+Q+mD)T?Gq-6Zhi=96ZUyFCAYn?u^dpUlHOS77$ku=-sM}$-s%Ld4q*-&Z zM+!6wgk6>(7nC~v&lbz02R`{(&{Mep)|aKBGf^JOb0y2WRWjg)cEtQxPSxm;;QRv{ zW0lXhi185Nm5~DgmCuo5;m}=$!|BEJzcxmZS$GG1M6>Y?P37zlPH#+|X6Id@&H(Xf}^>qqMP%>m~n9u!3lXHVTrM zk{}N!O#Vv1zJ}C4I~SK;rr8UU*phl@N{IO21DjTg`)jzLCDa6=y7vG*o4`afsT(n@ zN8S$>_r%7{L^5R6(FAiDq&PsQMBB_t^`EdM_I#XfWnmoO>?7q3bE8LG)SUty#*&7G zh6_Zgj!EL-$1q{dExKXf;w4aaYFh-s|C8XpUhurJxc1GIFpAw)yk`30Hy@SLXfa<>E@6c)PquO!LfN^6ua};eP2Ao z%|c0}tt^*?FR;7lU4amJvYq!Bg?DifVjhbimY4T#Kjp{(v`K!tE0xc{DY5?GW*LND zFX0qC&jvQ%Sz5=B2;WVGk*bUyK4qb~RrUjNi8@y@wB*>{uPl&g)tMP;*MeMe6X#@k zG;~h#b`^mQ-7YKIVv8!C*T2F_QH#it*GeZeh1X>%6m6nGWL@d?HYjiSv3mYNdvXdc z*lZ(jI%*&!^QJxrW<%grbEns3&bCcW>^eRq52Lt4XSWU6NO5rGM58chLU?a42r|7^ zMO^D?-E>;#b@644sXdJL6ejm|n!-YAv6cm?(uC*$6Gw0m5#lr@k181}UbkhSphCB7 z*vQ>m$f9xe3R2?gKTe1m`wU*GqE)jjlm_vMN5WpdC37q}G`&;y%_n*!VPC(%Fs z1hhA5^D=<&)+>)hGOT`Io(p@n_zs=QzMV^N>@LlXmZ5cAhyvRUxm1Y@E5ej>i(k>q7JuIa1o}sRzhx`Fni>Lh(36#fsH*T&s zrgfBNDlK5!C)3TnU_gyxA(f3xRs_fRvqgBGeML}`qN8vqc%!{SP#H?Z9?aXO;UOl@ z(L?M=9wn^S`};2DM1}pq*x4`BXn2FmQRY>H+1m-Xf!2t!Mfsl0_HuvF1YF=;4_UdH z)Rh|sAc5^9L-Fq6*$HfoRt;6L8Ym-FTB=y~fm>dnWnVSUn;aPiu2TqgD|b**J1B1L z#{bm(bR`rbVjzHp=}SX(s1wpU7ib6VA!PTX)jy*o1dx6q4 z#GyeJCva3A)-y7Ie0XQ*HU#&+sn#$FWThDW&E*-V5Y8}}lDU4M%j@MOu*OCcZ~wFK z^8B)D7B$QPCbqawq0>xFnZ8&w)ea=Z)q9A4jf}zoImSa-7DL$Q;IABbRa@X(w@{|2 z9D-1wlL&V+ z`Lz2}enn^fFa+L!W9IQTa~lDbb*=8mhc)OWv%h-!!pXs1Y=H(lt^k~3B$a7c^P-vw zBaC6+l>>af*ZK7U*24l_pPFy2E-X=ZJllMs^=xfTyH@Zux9N`{Y(k3~0G+LF-E6l5 z!@dTqkY5X2R(ayypUo>{47R&>z!5al;g+FLlS)3FqyBdm?5awq$_o@b<&~Z+-PkoW z?4L$nV?qfh9#&T1{Y$6L$uiHLWf$KVS{q%U^0+A~K~v{8_~p2b6<1gpsg{pDebsU+iwVF4TzZt(FlP1r1V}D|(DX>+*%Thm zosh4Pf@Jnt3U9&_M#I~f3|{be0{S(8evY1JF>5*nQN2N?qqTdBvA`qrJJgkCsO6Vr zqR~p>$>Q=ruLdtQJckR?GBJ`_4K%Wnl#WjO$_QB(*OpLpOOmj8666C^m_;y=K5Mh2 zq9BYf!;(EUi=Ew<-LPiHEy4{BOr8qJaXfr2+&cz3$EWlUVF=T2^Y5fJz}PVk1L>i$ z9k1ayrI|fR{&n+HWUe!*AkKUlk!^(%c?Fd^_Al9qf;|zbh^FX;ZojHOdg({{1jq8j z2z^ke{BRAHD%A;bgDx)4yu~Syo_aC05>=Nux=5n5|HIfh1PKzWh%5LGhg54_$~E~^dPekH)5N-bA7(BfF8GIcjik;rQ33{g{@T7;71!VKu& zxJ1Z3_=(%J#H^J$d`LHGp`cJHrfJ>sz^9K%j)@17;#vOt2w%Bt;zvjOy8T*Lsdm=R z>Jna?xk5h2M&*D<&2?O*67koSkp~N zs>=NxZN7DbRvJ3ulY08CjguDQDeRZhRUI5=j?C@R#lKXf;=3H~*i2Ah54uA}=#e)I zZ}tYe33krf2e;1qP_S)Glt6anJ670R%~1zBX1H+Pls3=|N@7rMjO_z-A7hh3?P2uQ zIb6zr!9(tp4eDKX5Ti>eY=vLte@uyjkBwGGQ*=I26KqwoKblXbfznvNb(LoNOBqUdvN5G9mjOze+pE~w%*6<33?++Ew`q?=s|AGoeo z_&p=m{>`}clo?RkN?JTi2y)0ZoDr!&uh53cy0J$;)d$ zCbMS;o1wGUe40+k2%DaX9e<&&JEnJ^=M!Upcr zL;|M~uSBZqWMw@@4b(7+LhxTnnCWgxK%e@@rS$&{jkX2ZYF4PcT{5-2Pk@u|2i$}_ z3g=iAcIY|7Gut-HCbm zhZeC0G+_Q2nU~y|kuVbe>p55S4V7I|toF|db&o5Th4q`0(mZ)^5)XN|s<~bTodfjm zxKx(lPzZYUfZ zk`(&tD9q?OhK2^lVL6Q9eD4{=ikHFE?7NNb54&gmxxLd4w4l+jp@ROB7Qfh9_I7sE zz_9#@h7{ZhLEw6wU~niK2;9z0o0Aw^WhQgDT!Wv3Z&S*(PS}2Y z37RZ>q9@)gg0seUww5w*J9R0hb2@PfUxARX`Anipb0K@l#)9cgBj*$qd2n|^T-DEg zgiMO)(OHQrTtSH6f2H{}E*^zNPFoS~2|VZ^reV#n%Y;nd^l z8Gond@8iyT>~~8-0Iq@V0+O}|`aAji;{gXs0z49K?%pM6IN{~2&o&3YmCAieybGag zbhNI;pWa|sW*AgNPyWXbFCIIVl2HWe7rcinVfC&iB@`33=u8+~rBt1`UnCBD#fX?w zCOk1b0g}n_eo1ncOoqy<2#;LWVQl&$Tw3O)wccc2uqwk-i*X*W_bW`Nna^^pt4ZC7 zZ-~VotfAnCnp{Al%JSXvPLa3FPS&#vr)E>OxzEIAgTP5+8klDdbjV8iVOJeo_WIta z!AC)9wjngKAzo_+cuggEV9OM#r>@%U8zgMQBPcc&=zwi0po93~oAUH9je?Hi|JbA)n00kenM*W2DZyTmOa*oehjB zv@~#tQ(hjOc}+g;t&WPUPC*N8am2Uu>X;f5<=C6%^!~J*X zLa^o>F@*iRrfX{h8||1h54JfLYRSHOSQ)vPjl(;d^Zc4R>Dc>bYgaR_sj@?2benGVDxV9lCkTlPIFu6~bhFJoc)0A)y`utGerOfGyD( zxO+HGLkw;Wx}Du3MLaZ9@1e?)^I_9sMJYQsCoIynJ-KP;XCvuhS1ZnYV~;P+)#Wyl!5@wuAkET(eJ|oArPdY!ehfiim$0*Njgh?G9>+BP_~0o%4jJ}X zgy)%cnpj?J*0b>?pdszi^J0iwJG4e@ZHNuy;HmKADB&yclRNh$kHJYvdp}rEIB2nC zl`ZgTkJ0Qu7uprv$WtKwQR#N!Hm8H+; z6CUJ(;L_Wm`9}IQO)w+`jlGa{IHCd<$7QhM%8~7@?~ghT(ESTwXoyX-;p$xWQjDIp zUm~pN6nqb1TSWwbyKJj#PB<~Y-+dQmc8NCvhD-IV!LhZ3&EQ&!|M#MLiVa5ALhH%0 z1_xz&=+A?C$^G?ACOoxPHa~}2oNN=#E%`wet>~3{ub7EGpTau2u%2$cN=wR6V8+XA z0s22`7=kfsz}hDjleRC&c2=Tm=|ggdBYJ&ibO`?PUXGGLp z%4P?>_T37-r@EX!fYH(y)-C$>yy>KYaV4M zqT8;|{Zw+Zkz;*_wp0el;s(ODtmN+I(A8O<4qs3b*v{U;Gm!NqEY)-FmjQn2Ec`~o zh8p>^0qOjbhJD>b=2tm}^h+2WIPZemImcHfM{v(4!8#uPV0eP#{-o;s(LZGp3OF^# zLpxE$w5Dv2q@`H-f*%QDIJnSzS7qrzG;$AlRFv-?|CJ>kT(~0UVPWOAcf4&9f~lX_ z*=$k~FV-R14xZnMrdVJ8-$y3)ZTG^`zu|af#gf^&JfS`83Cd`CgIo^afnJiaoBH#g01GPWS$^ zQldKDFb<0#_})k3KG7mZPDM*7LXvrCD5o-_IsVyAGKPgnHDL&04H$j+AHycZ-lr58 zvIJg6!A2SJWM4c8zk5xDxr>yecYqfmHf4|@&U};h$xnK+mRoF0^@s06+Kx!~P8dfK zHBhQj>4+mJQeB^`Zc)Amx!YJ_<)^hEMBUA=b@s8pnP|Obz0CCu>AMNB?MiqJ`tqM5F({Mc_ zfBJ9_BNg)aS8#gP}-2&Gya9eg#0vT(211u_!_{z;=BE#*nWM*3@)mPt&@)UUTfc z;=*L1C9o>vF3$@+&)?E@6!7{}4%bD>H66klQQ2z+BurA`%*Oa0%Y9CIhq~@b=`|fr z&x-R(F+Y-#@7=a@Kuon(mAsFkG%eR2FV)Wj9n1<=I`ItyVJ8clcgo*e>v{VcE=L}3 zlgj!^@0Tfk@Ww`YdSSZwRJR9hd*fd%ebrq+=o=Kw4KLLD7iii}s;lcyVF-hzy??3O zK8ZrGZdx>=EDp8BiCc$U5t8}Spee6_Jp$E$48|63o+4l<-ZA6V3DKbYmy!E5QEcn= zxEmblWDq#x6*c3$mcz4t?gu^Q@P;dZmvwbc2hq3eG*GwcNZ1O3MBvG^5=C`Z@HO+! zyeZ-t>J?I<3abmVoC0JpWW7p$>b;XJJ7PaMA#drER9rPENSOWlYQ1R{87Ij{Jt(9+hGwc96ATm$ctmALqw<&cECzZiuiST=W>=?BaiS4g@_ zM(U&Syj{maa8@?gXM=&lwG~;BMEmerLpD&sPd4$JU{g3j#{WfCX2Gbl6ljl!k~y?l zo9JvL*dg};`%SL4H5vbWcO9f%8ii_oY(Z4SRt`D(5NrU3Kf+GVI4nnjg+6;ha|7WNq;^-uhwdwnECPGA0yn ze1YR0?Ss$3S)?p@q1r9`I5K7d^DpC7l6n%sG7TXiXyp;`v69jUO&_IB*h93vc=}uJ zlU##=u(ObZlhEB%u?bwhq>EsVNNR%)pQgO1nW5?DT9|3nm-}w5#>TIE(W3gDapDoQ z#tMZ7QGzo)Agk5roht>(+USLXa|*DQ4jJmkUAKCk&sihQpdWcl`fZaiVf+n z;lGA;3T_B#9-LodvZZw+&+T_eTRLyEaafe*B{Vv74RWgRrCvI$FYtU{Rc|~bI8N_$ zWiOALV0?-B`S=Xl+|JM=UA!4rZ{dh(3412!b?Bj+LO38Dsb1-kEK3(p95tCjGXxr< z7rn_y1<5>;-DBP|5KDa9qi$wzRpe`_x3Xo`SlPTL@ACwKM9Kko0po@ak8)UCnjF4u zL(RtFZV{!M-B7L1;?PcQhs(koGssJ5pAIJG#A1JmM5oY!!95bq39=Z{UE4ZB3HQNhsphP5UzrQ|@U2&0(G~$Q(ht zQXhTg&vOJfKdJmlNf>Uv-NRkfudPniQ438{Z;dQFE(^aMjVI2Lx2KS?@}xE8F85`W z&N%5eSaChV@SFeJ$GVXv!k|ycN<*Smb8fc}{f1WLdbQyY2dr%{HjVSxBJ}Zz*TqGLP#bp#K0vc%zBU)B z3ZQ!UH+4`G>u2yoBAc#~Y!zmTRBY-k%{Xmq6PGf?x8`~d683A( z23 zY-Bwl+X>Stdo^h?lp#*VvAQg~#gU-RdV=aAt5gw%F0Ckv_K%mqoOyP{2CYPXEBvcdOwqisgEEloOHJfQQl1&P<%03Df)WO1jMpA zw7;qhu73?m3?l5*CU!#Q>U&eY|MVp7uC*sul{u(~AACyat6XK=Xby<-1z!5?Am&Vt z*?8pVeXINlbTKJ18l1l);i{GRb)9U)Dl`^wzRmh(mlUUdDmP}56uP-O`nzJuuzpW; zj|#*z84CZ`0&Hd&oEp^zd6@82R-Uh!vc_e19C%583A(bKkROvs^b=}D@b5Dv8h%We z{4dByf3iZL7yDM23er?R(X(u5UIw(a^65+Ke|}#?_5HAHt=XAqJw2{veLn9 zSRKF6{n!PSgrb4~fvUDEwr}Du*ZggHSZO8U8VEh<`q;a}j{<@VMrW8Xi#a(~BM6ot z0pMfxGkC&aPh12_WOXsB89gcCU1eDkqNLTGCyZWom!+*>9LyUDiMN{ZH|GU|qulTP zq9gXZ?25j8o`QN=Ur;cesF{?(ARRbnT4!L>)ymJ&APsemn$8_AUdts}0VMv-5kr_@k7?o%Ua#@0qkL6?-Nkcl6z9 z{j!tyeef0qJ)}3mE3r|TgXj8L`y>_W5>8*4kET5FZ(k@+>j5s0s1BGrtanW(Q5pGS z4i2F$e)Y=X`}kBZ)2zahtkm%B9NaImOsU5dh{gBas1i3qz-S;_gFGBCcg$GrbJZEj zCne2Kd>M08qvjx6&>N)@Y}@%gjP}{Sn*X~)gLLhj@p5)gAFv05-s;6`pwu{|9$6K< z)YB{sWmC{wuKR49h7&p?YR&y|^&4rmJ>|r<8F}>9Adypj{A_DZkvJF_i<@>-{U$Nj zHB*hEFj4&KJ_Q|S*hAAQ#9I;(8Qz05{RKZWiQJN(Ytm%>;Wq_Nv?DXY$Waf4Q|OkO%JIf>PDbo}RtU|2sM$8E3>_lBmR_FX6E|`9saPq6ILG^rJdLm^ zZ1uv0S!n|F66cGep$3Pi};=E#`DLudkYCy0T++e+2$dM*smqG7m*yzF6~6;6Ddoc6rh_{ zi|FmX)yC#et!lfgO%M(o)5)9HpwW1z<=Cy->$f`sT+?^X3ty7b<{7_10b-#|{hTl| zn}i8J>SN9TLHT`)A@tOyUkA$|Y4A^eWwehwQ9T9qAq&|9vm6D%?nOhPK$EoFY>Ya5c0vjP=u;bz`N42t?!+DQ}KsJfb!T!4$K zv<(%z?2G5uD$Rk;#5=Qo+QvW{);hjcNTf;qzmclUSYh?c4z&K}5`-3qBh_wn2ri$J z0qd*GZ1yKjKCh;X+Vj++a2#RIWvNV z;7htbwT~dudJLPBlXfRW!Yuk?-?`3vCcClpe5@?Hib5Q4Ks8)y%cbt zG-Y&{llIs|SlJGB{p=pl}rff-`U$gCFO|ab}8pCc7Yu9tUaOeTCGz za4+;gvY_X4_B`x8=0(W)-saJxlb)0CrGKjeyHuuGg^;UE&UQ@Hp^xN)(%n!@SI*_U z_+kUs30qhG6*3%Z-ry$ss5NdU&RU^q!#k_0Tnng=hc3^H%=b^!8ml`I)v@4-hvgX6 zLLx*u{Et+!#~rsNNd!7S!}3jH<0Quvvs|-$EqExOFxlig*Pnl+rZ=(G8DT(c)~U77DE-oaCG(#n^e2c*e5&EH-NW7Df{UgSvyxPPi6JX59MD-MK#y%J>pyI-wt> z7!^qp@9;vDG&lOegj8OiSg<@2F?&a}(z^u|?sw#;GV^9)v1g2j>;ZP)hsEZyqsuH> z^{XJ9Vo(Y;^uuo%-v*h4D;PQrvsv7b4`3%|KOnNzjA6II+}eO7pz-`u_y=*=BnmIQ zKG48*+U^g^OsDP~N&|l~iX+X>&A&L+VOAXZTfEC`g6)XTfKVX}* z0$Y-#D5iA3>ukg>eg|gwB0Av971}Uvey=WB-y^EBiU#d(T=iJ*grpC{^*(YX3s)H` z>H_`XW1~vyOu|Kx56J7hng{Z=hRQx@Kj57_PwY@xKaD+=YGyJ$O%uz@!}D% zH};D`w2GcuHsU4eB{~y0RDfXa)NqUIAf1bfVs1aT6y5)KFSV;>bK z`miV6DasGamDT>)BNJlrqejy2c6?-TL;Ss*m?VUWoy0C!<9Q4Yk>P&{gW6=}2Rfmr zYW6&IetZm+y%WI^nJIEC3>>ps@K>HP3bV%AJ6mMniHu9U#6+#LZ`wfp#`4&DrQgR>nh>-CCQ7#84hPI*MtrF< z@`L-~1msl#5aeGRmL)X%%tB!UW+ycXzb~R!XrlZidF; zo*69L8i?AIUl;K`p?4~%3szHo?!KkeeroC257BLQZS*{_RxyyCxE!0-{c%O3RTm+3 zdZjj!B!?pFU8DOaFqmTGILwh&*3YRQW(J#vjRu{i*A%B;rZKiHl?r*Hdbx+HRx;6| zz8+Jj!yt2_81JORM-)B5S5)~Ue~a~qk2yC2Wn86ldBi>Q=}tku#5Z>USvSk~bLb^a zshvHuzjbv(gh_f$Ua!m;$nuz#M>G<{Ub;5Ap9HGWJ34q}B1lClE7jpnM zvkFMX(?KH6a-!_j(I5LNsFtVd^W<7^BE0n5Dy}y$P*q{o!x@A+=QoN8^eXC&)hyB@ zh$-P^4Wn$>sPV(I8c8c0FW?sa2*=iWsVemHy01)+qo9SYN<^sqLMot8!J_Si(Bky4 z7b6Y~Y9B|V(^z;Hy5^|%y?wG!4Xkl7J;y*Hm~U>Qu093n;F{2p30oHX*iVvy8ORq{l5Vr=S~E zNA%P}yXnyu5^*9#4oISGF~I4mFIih80Z{YeT3Ks=hhAYTU^62o?rQh9TlJ!(MqVW$ z-ooP+JGuq=hsKUprm!i7FT-*Yw~+fptI#?thkn4@1kYr}-PqHY`|pGyI8V(Rof#*vX&E>~cYvG;jQS z2vxnvg?q?GHR=N3ezc0zxsw#@bj^8Bq#M#W6AZUxvSqa5SJWMr{tr2Rm}F0pRm?#p z!2(4--Drd!({ae2b8SzZrCQx{N0DBFnn2YfLg8@3#Z>&A2~f#wCE8OYa}x$s>S6OJ zAn56ft$A?V0^!B#L$>SX0O-T|yj{_~_=*nJ{5(5_46!2;PwwuQ*oy7v_ge9SkLi%N zg2D$T1bG#VdD|_cS*wHhtU}wZl18i1Zi-@#yyTbD4Nzu3jA40l2WmiIdDm7$;`nD| zeUDw>6|;*M&f3XnLPB#eQP@>KzoPZ?aRPsH>q%~Df}+4NM^rmr1YT!bvnD77lj~ZGG?{Z zX%h+QW_`ZQ$_8KA8g#f}wM5~Z?tMC|t&ugvpLu(RXJJdeEYi+z`XwUz59}s_jWAWvn!& zZpbmWp5LMqhb*s{p*_-QFQIvf^#u;md^X)M7CLU5gI}zIYos=LgmG-@U~AV|bVTfq zn zpMfqgLFPDrU{9(AQnG*OV}qOu{mMRd5frHLoHeA(%;qcoTt|2Rlp$)2Ua_SKcFBex zlib*-ZQ((U05m}@sPer51f90J~sbSpzp;kzZYGJkgnj_%dUyz!L0|pqE&O zi(;?bbm!(aGy?yj>Ei2=2{Q}Uuzkd_vkAgPc?k|kK_hyfS&h`(SOU#{r1)&WaOtRR zL;3LrB8;awz!wcGqjRn+kYGG^yyG%V4o1v$VgfEDW$2+vDv?=W_4W?MBh}}G_`a4Z z@KQoH+&WC=pwRt=9|(>C$Nj32LP%kILP{Cxvp!G;kml_13(Q{P(oAI}H<$5WHsSNvh_~>5C3915=sa9e|u7 zp|V6e(2?J3X-|%|Z;PuujX=*!$=GOwI(COb?okN*g$aQ?Njby44WS?~PCdFEyavDVJ`<<$ z&w&q8Xrc?K;X(g$hW5226uPjv+0_UU<0GW5UrT>>+KVHtQEDRmlezw>H4L-`4lO2BPMy!6V%!}V!S zWhhEvhl1hN=f&%KwgpsNtJe#=+kBT{IYLgmnLdd2oL|T2j=Q6iV!vB%7zX}+Vmai3 zpN1&yv0-NHjcET{z_~~l$k~0e!h!GaX^(g?1$NiJo#k(e`t;yG+SiN=cj3}=gBC6# zRkAG5=#w#Q*ZG3DFT7iaqCcMm=4*#^%;&-~D!2LUFINS{SGZ%GgATg1`D0`K=?|Jl zSQ5f{TY5m^Ui;2L;zJ49VRy0t8HO2)mdMY;3L73xXE}<`5p`$=NDMU}Bc?!o{n_^% zr<&t9`ei*)-|c=KDIpC`aq%eqdw@-xPvaC7W&mB(&eVteQ+36=3`acD90Rnju7u*Vdl(oNsTg|fT!q%tdlE1D4-PhV(E5oi=?sd8(g+&*=g z{`#jfk&NX8_^?06!OOcnne+L5PD?;6qopfhXZ8$@-ApJoSro|+{4V!v;b0mOZrU{5 zBTDEUIaGCUI>6C$kRt0RBn#$jfG1M&k3vse^xi9DihS8vJ`D6H92Ht~Y8Fhh>=VCu z9_lh)7l>ZOqF#fXQE_1E0_Zq(JwL^6N1$BhB+2BEvqAFh{1W;g;IUD0hpC?D_<=lH zEpW9i2|treo4%--Ivhq(nmbl}aN~$pF=)BIAW6*AV!XtyAVT%GT`n|5IbXASD(aD{ z_UcSIwqA4-Z1^P=`gOG$&XmEZkRXMA3pK@bn=uPHtQ4&U{X}N7Y42N+a(}@u3LH_! zx?VPET-r+18i*6>v3%c1(KnEz)fqLa z%hP6_$6D%*H*;Y*OT$J10zK09@L1B4x?(VvrLpw@IlsYMcIXa$Dhe0%o?MeEdT+}B z%{!vv;GZg5kkjfGx?Ln8Ggd=?7-RWz_`4;U2#ahzH17QYW z>LZ!%8WoJ9=fTh>4TuRw)DxJs-w#n=r&XP!lggMfcZa;jtSClZV_i~Ou;nnHd=P{t)P*30BD)g6yK{p!(d*YQ#_L}yfa@3x?&c`C6jmtqHnL^ESyiG z8^}O0Yck<`8U);W+86YZHsB{h^IhlxiQrHPMZ8NPfe+s9sZGak%5P??|Mk<3oo0Z9 z)<0h_dk1&dS!uU71^5G(F9+vwC$K@kx+=WG`Vf1e)`;-ceFoy2U4r zaR#J<8~Pg#Eg^Q0_4!W26i4L3?I$ywu)!kHO?Sd39x;dZFAy~@3?*E-m-(r`1n$aK zc6Xx8qOP2;%X2%h-lyMR0MUKa_T4^Nwf1nHIFuw{bI1G9OC8~`le9sfQ!ukO_LYMqhE@cII9_{z z`Z4BkIZgh4GZrqXoZSWVre6H;o&_ispvM|+xMItwwHj})-68MwS$ky>+4?i^&7dfl zQ7R-4`!KoT*w_QyHl$>t0$h~TeQ3!cFHf+P&keF*s)^hc(W9h07f$DVRIgQD8)%o0 zdw?H;nbDF+3KCjm0a0MHLs>0T z)S}rrLOEg_2}>h}-W5(9uKXFP10!tZCg#L$$rt9>f>`4n$6eJswQ@2U%g+6sFqsn= z_ywhf@wyqOVO{RsRh53%!jy8f$9J&bt1lC16Ji^z#JVL&f9-KAWF^c~ zQ>W3nd2zTU^hgce7j&+r5$-1jhXfvHS8aml{ZC|Sk#pvBAM1Z6D3@8QhV($ejkItOX^CIX}D=PmxIrzwufLFU;!ocC)1Bb@jm@Z&>DfsftTL` zdX+kGXCG40vr{o#t8z{tZ`(sjCGO>pPqqiC`v;GEwj`in9mSk&5Oe3`tfU@cYhaCh zDiLszdGMpnz`zJ!6u#TsoZw0AB;_o2x1z6U0BsJzsh6*OYO@e`Bb(n)k;0TkdeEa#28bwpL-P z{B<^omNso`@FjD4z9~~GN2fnVI*-^G)rw6!tNoJi_5_msFIiCY-~}6OhMs~JlvZgW zmg>K^=%^$O{tO8TUy|x`C{1}*? zgLGPECFSm$^3TyWih6d6({R*~3b7g;CxY_GVuqBdY+8&u*@m=a<3;?? zr|Ra7X@(^85V$on<;9KG>;NgsT`Z!?B8-~xWKAe@?SvIdgZ8FeAN_kV55kd8_L@#J z3=ZUXSXkPRx!Ss~Zon(I5t_dsyigEu0K_Vh4(5=*DS9ucdC~LEoBHjCymfs#h4W&= z%O7ZHsx%#?AbXgdBql^!^h(?Jdd4o@>J;H}2|hPgQIiexa$+~)Ur|i|W0Ak5w^VT3 z1$+kidO5kx$W9*SX#4R#!d@(v>_?A{E?T?eU_W1gnY4Xr5j2#rSqAy9=`|`hn$qh1r;zbagY41W%06>lVe8BNP_B z)jdQZZFt8M2<-Q@Ex2VR&$M)rdIGSHS~`F1W%3L<>U>;5I!I zt(;u=$5W||(0b@*^Snn0KS{AwQOq$rd?Gs+`l=(~VC_i4!d9yjh)oh{d(Y<$FGtVo zrf4{V$<#$%RSSFXVlWobtuK_8ombiwWPYiG?&MY!($J<+ecH@Ap(x(8PGBn3*4dT+GSdQigkGZ8<}fW0g{q^EW07@;;&lvl}j-m0U%8brM6q zIh9vbVzPU^+Z&zUNl4mw#?Sx|Fu+eNqrau(W47Ak%Pgoz9slf|S4&9@Eitc%mFH!x zAhzkpxW!q1)Lp@76xG-D`xyIq-dB|>El;p6LlQNNSIm2Rc*~KrLt2vLp-xCIr;hDG z$kyY~M%t`H!v!QeR9a^s;HRVpD-o~o`0g*%BE#YC(LS>5q?dHy9Y=4jH`ZZH&WkuxJAk@91rn;u7ufwBQt(MsO6tIX7LKjT{oD!QJ>4MHQ`ExfJ3<%uur?QSVjN+h zfB!80A2#bBtu9PjTW+x;8wAG@9`*BMEpYZczyvh^jD3fwAG1yHw@jj(-Tkmq;}OVk zNbtU6Hv`Yg6bF6HUIX%9(R z6HJY@=B4@G0~Z`brbGySCL4mp&p%$-I2-^bxWUmq(>K7oytti}Y~7}31F_ClSJOh< zh--kj;(4|!@hm{izeN5IqSR^W;y~$Bk7Kt*lYmXwMy{%n-R1Iv6nU`A6#gn7qPjSg zvA|@eLn)C-=7PanRt!(q?aBIXaMoMq*C_v9^-An~bPC#T0g zdn2@hfzKbop!!Q+7IOG5a6xeQoB%qafOlGWA?(#pwF0)r$ny4t{cB!Omdy^*+Pi_}Qm6o3fJiaq_IRGSa7ea&HC$5Fh26E|d*ZBBX8bCSi9w#^gcx!nW{FBV z;fOsY*KUg?Kw@pQl98;;H(nJjTj zE#uIef@b}gUT^mD=ec4MVITS4MEa~i!l(19gLruAtoFy+_C9eNIv2dmykHzF{cA#T z1{nm^422Q9CU%J8#4Qfa667ps8vHAybupl8ObvZfu856mGZMpRx_S59akKp4t&m3S6&cj%hv0yj ze}!hDE*F$DGCxK1&uGg>mt)0AcMJ)6tVP&0-!156Rx+Hk(4SV{et!ekPt}1qCX{wZ zZ7G7Mze-F1FTVTu$;ayShJ0;qR##j79o>F9?Xkag5-6xNHFhKS$s-p2ZZ8AI&dS;V+z__OsNT56jw4-X9L}EVg;*Sv$~<&v`a2BdjBW(KT+Z zbYrlZv`N!L(;Oie{F6fcNYq4-SE|`Jz(a($V>49;so?T76o*iZw4jE_YuR{QPDO_$ zhc<&*`?Kv@W?u81hwH*i3wwndyq!~F=|Am3P-E{OPvWGrUP291e0kRMe8p#WsVoO% zAUbKlxm^Avo-4mzgXmw!wh@>R^p0trQUeaJcY7n_!B|;DPk*m{1r{$m7c^Cw`I{vb zzrFB_O%>5r1MWE)Oo=nu-Hra{CqsRg4TPL1N8xt4-luWqS7~oUgrr&bL1+rt2lktI z!PBUPK}%332-)Z=E&4W46pF4Zwt0XazYCkX0yuBkdkUZQ1do~ReUhJ}?>>ipYrhz^ zWUVSm?Z-&oT&uf%in>dD!b<%#mrS59h;+MT+6*c_bYnOptQEg3AkaR+)MmIOu1T{a zb`idoxuf$UA3kz})1`_^08vbW8db%7ulv_|=e*TweY@nau6g&%O>^-0iDhMc9=L(R=8vsvG0gq7O zpU@Nj(?oW4~YZ7ZyKy`aQM-!v$xQXJW${l0!%}500&1<-VI_2 zOdqxZ9C%5u+@6y-n-hSbeA$j@YzOiM0p1Z*@JbL3KA4{h98g!>3P=DR_GdXai4^DG z&1&cvq~Uj)==GKbUMV$9bNt_Kee!1HSG&*S|6oCrVGUhQzaC9>1q|%vr+R%@KrM|w z+rcSO#BFpa4!0)9_8;~UK>8rw@_F$9?p{xC{m9@S8UqEdw|@(M9eTGKJsTq(v9A1FzuM%)z(N7s z9Utz2IXpn!0f+_$00;pI`aX9xSOnsGD*=9xD?wU90Y!djUJN9EDc7FsKF!{(*wq1l zXG*}l5NI*_9^yt~0)PZ2!{~E=%}Ra|AAZU2b~S$Nuzq&~b=%iAfA>w_2Y-JZf^Z04 zZu}7OSS}B~^8HlS2GIn5(<ck^8Q^3z~SNF_j}K4oqBv3mNs}UnBdpm2Osr#OMG0A%22h7 zJwiiS0+7oCUu6y|_Elg2z>bb`@`V88Za#~x4;y6k+oK>@0KWdk z_8k!H4zLCFOP~wTcEB$V(D(gdM*;(4Z}k=P77VyW06q$?@wIdHt1gOHjrNb{yC)1Z z=GPBi_w6T$2+%gTE5BDjb2bR@bI*ST!+vEq_HQigJ1t8Oq3_$C2jpe;9A5L#wKXK@ zmymZVesldgkA&whJgnze(qRDsJj0jVZ*QpeIZzls53^r&{~+ua`0ssbKtDu1>B|Am zlp@VsEnWR#qgdufjMHb*5X)h!FBEmzDbe4>V=Q>)gn!YEAMB43mqUBn-eW1xWXBTH zcFn1v6|kweR-ox8eI0faN6q-P>s1J3WB>hsv2_kXnlN3XY}>YN+qN~$Y1_7~X}h1c zZQHhO+q`dl5qI}*Yge^6ixXLyL7?X!z}j>;3i##twVRy!UVgR8ItiW0*v>m`P=}UB z$OawHv_uz>6RDEHr!DO672pO3Tgmc{66X>)pXjqpC)y@6DtyBZCq=$P5-u@)0PbII z`DA`S#ZtV{xNBdmVa+qkwi#Ss_RD&e{K3m5`EemjDdA`@1vZ}XBkIPqA~}KEKPqhy zgz~xMmw1~F9;3#+qX+fXC9crBzlIY-TnrN;(*3bA1!&0aDX=#bC2Gp zSnxU4npsawj)g3vvd7le&KT=87gN*e!v_`cg*5=umk+8%DL6Sa7k`W-v=wL#3L$3x zWk_$o03%p?J?no4;J%oi^{vV$vTQeapI=Pi?ZXX#(1}l_pH-SA|BGeDWC@LgS8i7? z_y>PHcgCB#*QNSBv9nnJS}IBLo-O29!-8vz?W4>7={^+p=c1lQa-7p2lrHowb$Z;nO4;n&YcWvdr)v?xA~S|ds$vnZ&6wkC^Y_I~ zkg5t)B$Ag-;@pm_%JF6^s3jv@ec`(FYJZV12S~6$*^e|~+`qb75Q{Q#v@csF^yq+qw-^FjWFul}o`XVyJ z0dZ?eK^pyFrF5(*Oha`miFMPmZ>HJ|p&B)O+Pm zE^1|rQqikQKWZk{*wr~9fnGSrl*wzebHY{hn50>c(x&>fpr@3c4H6rBn!J3fT5vY4 zhnXB*v)aC0Qy^rNpq_KVM6Y7>DjuDE;)hMl2-00!R8J+Zu)pq&?6^+sX{_|w+3Cwad*s8lh4I) z_%|^Kpl|bkT$7}vm8SaZC;#;qU6E>#bBg(wvjC1>Lg*QB8;-VPQnBfUB)uljOnT!R zm5d}9x5STfuIZQRk%_fJc~*S8dVI zYpE({eBaCJGeuK}I>UgdzCdycF58Zt=vCQ`KEgJyQ0nf_jRJ zT;4Lye)mn2s%HDChisrj*pqkSsrg4`QgM8 zNw|eEt;Vli?KoPFQcLQMih7(ZKRkf{+N*m-;ATqh-%~Pof+s1cm4*V^O)C!fP zDghs1;A(J%LIRBU2~kT^F9Awr7p5$ioz>a*!nU(8cT>Wn9GoQzQhXdb$_a(Gw$$3( z!?33?h2JrcnsxMnC7B$ykg^Q{EZgURl0DZ@h~8BC@_N6*+4gh~T%tD$xUlimgRzQZh-w>2HVHzc zmj0lUOz$(&pxoC;Q-NTsih=;iWWtb3?iZal?oi?BIfL|P*aMy>nPzXQCQR0x78&xK zFT^OmJex_(!<%BW(2YnH7=vbkpXYHYx$9vIzpbM*Jk)i(0# z=%2MF{MaAlUj4JesMfz_?JK<4Fbu4&3J0$(=s=^@G!>x(pXjtHFx*R7%-|WTu+*F* z(ASR_=Ru40ptr%xOT8*|{GtK>>Liw28wk#Aom(2&7HK&-aC_W2wFWQf@;S?2(tqjL zXP1w3yu_nu@95zyvso#G6^e!X;Cjm1x((h(Mta*aKcRhE*8qQK&8oD6nTbJw%j!Nh zQkG)JG7@^4`LLw0Epcb3J3URC2T;Z+GF_pg8(0vhCzCX~SL&c*;7&p?hC;72c?*ZL z_^j#RaGP2UadIKj$TSab$^SrUAuBeNPZ~oH3Dhhv*LRob*1KP$Se9}ai}z&aG1DU`@;Boe8BPT8~sSfxO1MdYdTU^#uFozX_jxHk}U;%Nr;SCNPk8rgpMbnG_ zVnpMP{XDhh{tJOp{CF(7h;e4b33aNz?z}L$)wMdiQn^LujrYlcb2EzaVaJ(`%Q|k6 z)_Fed0ffK!t#QXQJAg~O^(G$|LS0TsaRn>yn6uw7`9@HEswAS=PRGnIwgS zQ1Yj@*w?B9$GD<@Vr^Cj)sv5gh5h(YY&N;|;va5&r{e90g0=rPiv2K+%7Sdsp~y7>{3i{S2Pv_`wwdOuNjmrBy&*aV>lp)Ym7E#VapCcZJdh&fh7+(_sAVBkt&&ms%?Ycg>^bN*uZU5LsmJ%5D(2{!w6IKVN`{ zMs~slj`d-&`DtD}Kg1Nyd#4bpoQ0s<>`P^yn)G&!Foe8r|lz1zWhL+=(Y zJpaByR!EY16iRExpWAHg7#EOkUheei!rBiuu0vk}tXRCHo>-I5NQgTlIFo2#t*FU< z@hso?a1Ji&I@A?sJp7hdbM*wpjZ}#9i7)Xz9@k@DALphnH=R+8N`i&avVEBys)Ocy z_`LE}7EDbjl@>d~JLzvIHQpF`CYlSM=$N`+Fd28wLNHN{l%4C9R15^>oxwZT{=sSh zRwFG7qbsHskcTI=XH6|PR@CIXYvqHg)l77?wYF3Q;Fy5T(398i;>~ufC4sx8UQCdK z>uN+Iw2N+KsoBeu$3PR4VJ|O!s#7X~c9t`P^vCe{G%Zm0(-SD-v5*D0M+~UY?*D6E zKeh_(Zu~>^1gZ5k6TjtQJ>J3tq(-mjEJ+pWX&U*mtgf)wd4+~)PO=}Dv?%z|>fpY5 zZRWIXnJ4PXOXHGa1|E$6aXjg@l0oihjmld|^ z(%<2pJojgOzGIAa5cRc^iDoLQ{Ft+)VF32-9Jke^eZ-@@Xy|E&LA(_W z8A)l`ptG~Rv>0mk`LHnqjD3T>Pu9>FD2SxVSsEN-esx(zZT0(+g0Pk9-4c+^lkl#n z_ObbF!+wd;xeB>nvX6~}qy0`@^20SdW-JCvauYAin%OtC{qp`YQlFF^+bt)SdpL!q z_NCL7>Y#77(ENzVoh@6U#P{m!BQubRInmA<{2Ae)3TJr?(5Qq0M?GjSeya-H#Rw;d z;tt<1>njD-dE-CTefnC>n_)&Nm&LdpbcxEU!)M8XgTu#LK8g{FozV7wBpuC|({+xQ zn&mxtictcyTSKPy*5-SWbh^Jin0dS>v9gTjFSLGSzCF8@dfq^_xJ$pRrmz0fku@V_iJ>x8dbYdzC^13wU*S=ssF)wiKhSAg zNt6@iU()XPN774=h68iP_GZuqJ$@iAl5y$;1|`K)*Xek8{-ViU9Hq^ zagTu>SCG!uYz$SAPi6+)xk0y?F?Z|ium^O4&Tt&ef0I8-5)+2c#@d%|O7O--5}Q-l z*8_5_5=l1~8Cc)MM^riqNwq3+YGrhbFnqt#9=SEg&MCJJj8qs%dXdK!_n|3T`5$J^ zI>AB6y)zpj%o5w{x3+SZFBGBOuHjGCQ;h9mv*D)=!5lc}U0FpU(<51z9JlkbiqF#N zZn9eQ06pK#*J|nL?9bs`*%>Ch$xcvaS^AXmCkJs}mQv<~Z93XT&H6VOe5Kwap83#RG{uTB-v2ChZfk)RbLaLw3Y8qkI^o3VsEu+` z(SGl<__d(q1wv^vcK0meP6^MF3~jJ73u9K!rR#%IFQA;@@}pvZqZ5U_9=fbl$?6Eb zBA|TrT=U%hTRFauRTb-;2 zjiHW z;j?3g^%3{66c$LZs(;lHyVkRK5n#_c50<`CeWh&WDJP?a8g@-tufMS-T}#G9Dne(% zRy9<~2`gV(z+G~F|LLS>-inXmkbHh3Zjav3qr@p zStDHvXf%d^tb1(joRvxWS5sgvp*S|>$LS@$x^RDkR~eML_!Y*)!hEH-HLiu{Ysg%x zdETCyOQVZ485KCka8a;T4I`Jbt%3a<8=5egeR&!`I%=?n~Ph0#xjk}O{pk>I<;CMjqhfeIsMA>eSDj@R5Ptfd%X4hSf z#z8l~+Wk;E2>*;$Q9QvbP!#jRqCL9dGH3m@hfgueHq=yh1(ym$&#&liBJfNMf94=xyZ!q2Iiu3%WGJSsgGWb{CjswYR^ zdN*AfEv}O*nAGO)*)hi{P8aPIX7Yy-z>k@M#d^Nh{_>~$6xOXzd)cxS{5tGgGOf(_ z15ur9;7W5}Paxz>!Ap@f{|I?r}2Q@7?iYDb)tlO17(q z#^n+9!u@08t#~TOh8wMxH<9I4!p2823$C33t>(?iPH|3FVt@HKXJAp>`L3`#$;+Pc zi7slbf{*if=^k5`Z1WU_>LPLyVe>tgfw)psb?ns^)(LwlqoUSiW&Q`>u;p)BiG~J~?Xf+eu&5Lj`$ne6zzaQ=M&H}3Sb;?~ zw-SJ)<}c)eb?|pN(*-LdmZGE4$smdKwx{T}9L>?D`De`YZHNmPSw0D4rH&C-V1%9z zw=5o4+;-K#&jEpy=Ej68_vA;n*V8uST%&khXiTMV=D~>%59p5NPEnIk=Lj7)!6Cxl zm)lBcd=wL1W#%sKz@fF=K4>$kuQT9@w@aJOso#%-R?7F2X@x7Y38qj$v&=SowdAHd zfzaqQ;rJ8!h~)Yp!yF4fRK~|#N%s36M`f}AGK?cVE9xUJ_z-7Ut5koWXYlS2Y_sI_ zs8f#w@gTOrX}o|6b~Py$TC09L2au8x5GiC^=GThcZgEzD5Y$(VsGS(#=S`jW82(Q_ zuOxGJ9SFH-l;3b?!s1Tq0~drtA4)r$JL7{gZnc(0?3Zgme7NZp8Fpbc?S~$$EMu(y z2{M{=8M<*BuLASqDih)v)fd(3FTtTIM-Sn5&VS!&BBb3b$EV}A?K;^lXo|7{2{`5b zYG*-=?0RU#z0VS!iKZ5pq(-`JW%?U&b)0acfx9E4GG@s2{|G3H&0Dauw+cX}I7-;u zQqiku_=kx~QhA*|+8RU->e$Ef+;fa&D=M8()dSbq=B5(pQC+M>8IgESIXz7X>K=nV zPXdBPHp?CweGF^^HOx}96x&0S1B>t@q)>aKiGqd>zn+rrj3Bl}sIw><4n#0mDqU6TI>j&HNuIoKK=>2^02|L|D z*Lifaq0QgLhRw0Q(cY2e%8SE2?*`8Pfx*pk@;u0J|H0fm2*0hMIKCAh={X*vgebY! z55^Og+gc#6If$$cnPb4YtMSw(pGjn43^%}e51A0u`^XTGsMxJ7K~2hiYaCCxgM*K= zVc7^f&h_&YP4D~ASJm|Iw*DMpy8!B5w`d9$j0;|r?UZmg$m^wgAw5T!k!qC3=gf%s zc+#b)3A~2@+uf!APRqnk%yZv?L`PuQHje+8brK`SL1+1|auo58HO%ta#GpU6qlY&W zCxnIWCaV0P=8(+Fi4<~pqFQ%KN3I`9mrPvNeas~zf^)NFWFeC8hDp>fSkf$UOwMSw za!M-QD_Y+a>q#jgOZLEyHkaoxzq$Pn!kx8?fII8XB zDbv5CX6fCtO@dbz)g1InWpFOe$)yQMq|zl848`;Hy^Cm?U(QG*rXJPRFUCRY)Zg85ZO*yW&UIIBJfKCq<~W zE?HIKTrbJ!%mwMtnASx!cXu-IMxXw}Do>hVY=yoI2y30>P}xn&$W|iy%T$V`lQh1a zKe~|lq_Cqps0$e}jb-TQ0(mg1xW$8w;WEJd6Qr6(2lFexPgd-H-*M=`y<+Aaue0D7rRQI8rKZHrO((}Kw4I%x z@B6v*p_vk_rwt_S+)crSulTgv?#OH@lWYy9$7WtCu;x0nt4Ia(fTX{A%#BLD{m!~- z=0qUHYcnM1B^N12tcZXaOf6>zj#_@F-LziOnbYX{l%|~Aw|XL$2QwO5_%+jLyIfdedRA8>5w~h6Z-IHd+M-#N0$17sJmOmVNv~eg3n%+Rl2vn& zT%~CBauAA*z`ml4v(Zr13K7DpJ>@8A;-+*QgRLpWmJ-5X7v;7UxYBOf!04}^m%rgXfP?R{g~P(sQ}~a$xF!!!DzX; zIZbdk+D!d0!eG#_r}3j7aH^kAgt=gUdDe!=!3y z6W*W2FmNbQcZVVoHZ=#!xwg*NoT}x%gr3#^e5>Q6MHIRrv%D)})G;l|8&u73ah?3H z>tmu5IkIV?qFbp9sV9mrFMJGrL^>WTz>{D+VFDd#8=*z65Co#?1vaU(4T1cUOHW&&wvo(%h_*kiaEu(k{GwuiH?xq)s75 z6Uzpd$>X4?+#_XqoQRv>*3Q)Hi;@X%` zLZoeMvqA>8MQAu##jT@QeT?Xk2*4l{TA<))pPUFS$i|pVR7W&#o`Fxe9JZ)2$#B?- zN1I>WI)IDE(-yj!vwN^YJU1$NDR0169@afq18j7}{^inbLim|Vv(l&k^#0~F+kNj5%XKMvG1pqk18h`#(c^@5U#zZT;<7$ zn)K2{U-1vnUr-{q`mdOEA=*cB&tCgsw%}{55XX7yAyN1c%tkyb@P?!+<$jBsF0v{uL{q0uO8;r5=!knRtUVs&)Ef_d zQVJeQV`H?T9}2`;7ea3PkVsY^XdI*-%z$g)if+Bd-|&Z2wWf6~AO`}%TzK*Y7yxtvV>F>tyG0P^g_Aw?lR0u?4n9%)&QG(}WbA=Vw80IRC z-4zciUD+k8|KiA^d$ofP|^(esCw`=a6T7$$nDTxaD$A&#+o|00s1 zCDHs-oo@O~QcyOk3$oi8t_?06l59RqOP*8>H%cme4=Mx;yxVl?F5|!NnhfrK2%WM( zHriN#{s8cn#H3RvxEYS13>wAgp_ zb+cIB^}**X-UY;~{3Q~gIA$fJl&{48m5%eT7PR0nJXmaKNCS8%wxFQH84mkk!om`u<-0*GCZhxL0QP?;iA^!ZAt!9s64hI41#q~%ic{@t!l+wJZwVh z*RWLs5R^CN$$Jxc(X#w#)2bfZ#^(~L?5?EkU(e1HMrIcbile3wz`ooTPk0pQQoA>t z7;UdjBb1g3&tqnl1b*qxLD|ByepHkQAyvDP{MuMiHy8`A5rNh(i1ZhC^due4$7Dei z{0O|skvWt<&HwQSk{UdR|G%I|uKx>qWaH-e|7;@*5hpv_{|P;E{dfL9=8*`7QNq&3 z#ng$2QNqU1#Z=7H*xtkxMnC|@*~Q7!&=$sHBen%xEqNP<18!kkPn1*CeN+h{XMwp7 zh*dP)r4_0`Dh&!{ffycSK`PuOVnHg+@9ZhdEa zzHR2Kg^OHa!M(czQl~is<>Pg8eW&)=knlc$0&{K?Hhx z8vWiVVqXB)d#}0q?-@{Df=Ip%fc`)rD7AGD+M~Se*a?YpEp6i#)O@**1w(%da!5vC z3WWdw267@`K#q_?Usu@yy_2|aox(p{;C@+zSYY67#9KfJh-5^ z4|ic;;9raPyHGkpKz|XNA3@ec@bVq5e%1V$MDczG5W{bQpFz{_1uYbT`hGv(U(<$z zyRMKOG=G0Xez^_uQVUA*BPm`DVt%hx)x_R_K|Df3fVzVIgD;^Ckpt4}z=+OzE6>)1 zes6+)1MsLF(_<0S1j*0-W;p4OUs(&X8A)_JzfkN`Y68DbS zC4>hJ@No!P6@}Rg)JN(A-Vgl#6cZN*78>3y5tNdH0|NWS{l0;;_(h}w0uuQF4h0e# z^%p`0Li#|YLICRcfrQ~p4*CTZNEZ156>!;qhWx?g=CwzR55NB*?S(w~9q_+Fff_Ocays6uywF_Ld@&{Sa?^RWPAQ|9(OW08Pc$s>)EZ~A4z!(tBS-DP zWW=Yyysszhe|jHAbKH4jgos76yUP0}%RdO9v<{lBG0$bt#Dem2Q?6Iy&o2_?rXaaAAqu6UIq;^EPq{45Tg`P=ITWo^}8L&+{K#7?XsL8qmiGxa>aip}V)P?*L zmx6ddJI1#lUC&Jxn|qYXk9ctZvIy{B+;+UuDqFxjy;trMo=+Y_^LJM_m$@mvFftS% z-oVU(t49bruDYo`Z&-oWNEh7DE{{kmx-;Bg(2Q&(0=*l#Zc}!o*1DeOd0g2F`9)oR zVe3p1lL)p-dVZQDlEie6#A7##^5uI-ehirK&Jz*3O1V;R(qHOlKd&Tckw47XcHXbE z$JF)1RY%z0vvId6m zwK;0n+5TQK+DgE3957bm9Q^wUz33HcKh_7FBk>d#G58avp{EEUt)JOw^HOt0sYqs< z=64W1Ip=l5Cz(|*-Yr^!xiHGKJcl=fv{C-d19A~XKnqR=mnhIngHDyh4Tw7Yi68$M z%&E%f!k@*bMoTxASJ!d--7f${`8i2!{Lxe*jy`nu3`f+y^ZcrU&uUE$K;$doUk{47 zU?N-Ln7m=ec#h4>4D=a%6FtkqH(V-{ z+s}9x@ z;-%MinjV=koEnqQR=g`At8s=&@&@NQRa)2GuYAvNrT5KR>wMxA_o{{S@e%brw`hy! zi~B}_Ol#$W$Pi{lgOPhSb{QjE2I#vvvc%Z6+O^4dKN;TP0#$Eax6 zr8&U$9bbOa9~5@;@+L7xdu{M)sc)i&>au_Cp~PER03`J|@5I?DY5v zdpguPyvG#zRjBVmjLlSR7t$NE`WQtF+hGsw?M4V9b|-(2MdiL6(IFZQ81}(T*}D`+ z{(8$;fjqDg+5W-HRt0s7&MX;}{?Qzaj)q6^Y9wpJ)o|X1re>{xxyB5C1EUrn4z6aD z^_7j0hxgL>AmtE6?8fp3Q;6q=cQAIim)n|^MM~#-k4_F>gRaLwzVW>_3m%-_@v{YD;H|H>Skn1{eV=5mcrr~rg3roow!#4WVy z*31o`2q-*abbYUfJ~Rq%63ZquLJT{Ij0=lr*CHrxrwjfyJxscI+8iuvve$wqi(*f^ zFI0{q^HQ^$QmCFh6MO5-!OhgDWIoN=Ym5#R5i(NWB!bCvgq>p{2Lvhzq{=Z0NW6S6=3!ZF|wtcyq|zYR|*1l#E^ zJTYMUBahnJw33L6iWPO?Y~NEytvxbHsy9!pIg-cKnZv5KMn5b2bb(tfSgWNMs)0Ek zR8Qd0!YbOFUW%xdep5BAYa=QT6heH;eR7(vM10(ypt;?DF=tv58^q8F*A;VR6bwp* z)n#STJ`E+xr6Ko37jD`fx8i*jCr=JMP_X2qins}C`_o6S{l&&UB~Hwqnx2kBvj|2D=pK3bGhrvqi)Jh6 zr2W0BI_l!=$jxVHF(U?^5U_1R*BR?Jci&Mi6*-MG4zwfff!ARW#+%e!;n#M@?6g_i9;yurI#M~3=*89i;>^7cD9Z#Dcecqk+Cq z8@mhmKnQO6Y_}Ym44ojqq{^nH{)&{-ytsm^H&Uh*u8Uy?&oYCO-8z|x2qR_EJay&ww^ z+jEV|koNLIzMT=k?Lcwq-P#!gp=hRV!l8d2_zuL{j<7?zBHLk>Sea z*5K!NPaa5p1NoO3i*v{IO7?hhl&p4?%${HNY9n54u65D$(M-xSf>$g}J=uQKkAlc6yW`Cu{trHC^GM=`i*C#!{9vB>a>w1aMIKT@{_HlA zaA8?jkAS9=Ydi^AKl`Dx#~AQ-M`@ci>0e$7s%3mJ_pXF+Frla%P_AxTuBG zXv3H|G{?7o6!DK|MLhqW>l?|CMT8{@an_}?X=@PjH7djXP}r)=C(jXTyZRyEFT1rD zR=S4_xmmLIkEW#eUTYx?Sp&AvR~PH!vxT;*lv|0cBg3$c7TxrZ$>XXN1V)#D$s4J1 z9}iAU`pPBZ;iN7$EGk3!b0?=~i8kq);|eo5?m`(AIz3|W;xkiyi%&fGj-=tCwelpA z*m+ur<5E3@!_{f?3^&pVW|WKSvZ!>`&@?ll;$&HKBU^5a5jrqwQFY7;Z=l)PW8GB} zyk8ur<5_`v`hys{YQ0c|pd{!`lW0Yo?_#X?gs){mY)0fWc1q+Y#b_i7T?s9{sVO%b zsGel}_INIRGK}iU@eLkEUokW$lo>+TolN6?4asMkX)2aBBb$cFCBH}+S^8>fv`NU@ z=6JeMk78nKYhY$hTkTi3_&)}xbsj+v&O5ek`2kkOJI2{bZ})-jdBxuJ?c0jMS8h%& z8dR}ntO6dZZaokxg^D?n@>paN)=%^1FM`^k?;RsV#@A3@KsH4~m%sSW1G( zqKi;EU^*&@x$5oB9PI7vbm}H@I_AZ5jA}RIc?uSGHJr34fD(;?dGfw)RRdPU=<9kj zZ*lrp+woT7UAYR`Z}OUq#stcq{)0J7D_S)+VvT0Ci6>Sxxlph5oeUQn{)T~wsXgGxmm>@EZw&`c`N{V)%LkClMV&rj{uE8t&d|;;n9bN9+__K1RTv* z%_Vxc?$b!g1Si%-=f%r-5TvBbCtN{hazVO&qqVvd14-YP9pCuTGc)qEP1{prl!;(= zoGUy?y>WA@AF^)_x73jEAiK4LAtt6nIgNUjpgSQ0@&R{t-4j@x=*L?0K~XoqUqHUj zgask2Zqi&%H7woP*uDH098SHYHlyBchjM~He?qAKKSW21afcX^Pcuj&au~Us^)7Z_ z-2H0MYAB3#-}g6e*}t#NbJruZ{u#@SfiFiv8N2{WMztp8{|rJ9_^S!a)V-)&WPXC| z8Aw|mh%RXsF69Q5+u?H#Yo99)W%9_={0kJcj0 z2>hW^zN|$Zq_pg7uVGYuXh!Zgl-~H##i=w7Hs3$Z zRTas*vNzhv6eXX5qbY(VM~wy9I+!}I_J^zKi^!yru`J& zL5VbL26Q29sh4jjNmWFrpqe0v#lBgYQ0#bAW(Xkn$mXO1%Yo6Y74hGb!{2Qve(HDr z%Y#e9wK&7QBjuVWu2o=a6QWZw!;ghw*y$gS zpLCIwtm7^Ba?zt2GLjSW&VW5Fu^yS=n8MKa8z{D`#pnLz(FSmPrxM0-+s@YstgJkH z#Yq>NM>*erz;@*mq;Q;0XL44PcEr`S>*J)iN3eSYzk3^ioD}k39|qr%47(Eolv?>F zo4#CDOv8-3VTv_%<=TbtY@>!H=WyrJUP@~y8ru-d_FfbVb{~$MHe}Z2>H(_C6f05g zec^`CMbPx}y1~6Y_BSlXgBoRLvuW8Xb2hguJbF*BZw)QE6{O!F6J1u#P4^xfZzDLr z=-=HIe(e@8*KE4yW$w+C?@SsbLCEr|I$uI&^yD-M zdvI~eO79*26#l!KGGHP>3C9pb+}R7N1V-!ks*^6q=LboR?O<5oU)#EES=h9!>8ieF%y%$)bXQ|qK#RP>mca?iHyinerQIC3dg zZ*ik>N&d8{ytRrMRf2ZUF%?9t={D6ghv6<*E-nt6it8OadaJAXOV{eWWmmF~uGp;M z5{Z1gGIOT{8KUA|5|^E*GrJ-gftBHurT6_X#OV zM{2t1DjLgJJ-=>2c%r=mtUqF?8f87@CPvCkF1N9nk8R2-gucL%*YQ~*no4k>?(qrH zN841jT4v7L?mfwtGo_M3!?4!n8*5c1wAO20sqZ@rMAj9+uZ4|+@w5NXRZ|}m-P{5PN2h>b ziv~*QR@p)uw0EM*+vc~j$Mf+!9D0LvV44xE2%07cCOl?1WAQpY%{oR5FN$+u>b(Bu zOjqwUIaOpmGe+R8DyJ#_U@BPS*PGuPDWD(+*#s*l9RNoeLcA+M$VI?Bv5g0FW&=+!`k;84WUqhMdV;5hc5a}Gq+Q{I5ywi$eOqH!z1(>a9_ zf}Lu#vtA_Pd$vc1UpBdU0`{`4=|1CcG>Q>`JXl^R2j6$_cZ6*PQst?KM#rz@4;@cK zX3_eFT@!YrKQHrK<9D)=>e_7c_qm#%yfM4~3WVrmDMXu=NT8u~1eIzU(P1p*Hu^YR zyKC0$$PJ#)st*u_npC$&YDdLkkW;Jb)3UO)TlVBj`Okt!R&v@1pJ@n!q!`6m7~|pj za-e#<93Dk<-GAJ)X%I3JO%<@)`X;Yw8|5kQ$L!bh$#b*rY8?rbOxwvCw%{8>J5gHo zGyAXOS$6JL)tDCh`J;LuB6*JywK5%_FKs+eBHxn%iUqkB|7em83nWf$ zUv=14K*_vJvhqJ}oO0i&I^}vntX>g3V;|cyvecpG7!A#wpQKaQSPScus@&%nyfDZS zVyqq@b8!xH3T3+g+d<)%4GbIj_1rPEX=JB)RP8v_Z5aW) z^+yPrpfs-+kD<#+GNr69=Wc4|h1#jwpr7Kff%HkBd&)mIz95PIKN>wIV$~F_@Sj$Q z#2&_B)d%&UPC;n?m(?{&#+%^Ov!jPq-3>MKMr%R`vOmR3&mZ#(!$}r&>uIT(TISZ> zAMKggLkn0)F;T;l5$^V!O($nweccTs&>c*K11*WvhqxyZcOvR<78v(+jRaQ~nJbaD zceGe0acmDN?wN&(0HVqps8Dr`)3(b++Sx*b`)Q-QbGc679xg4+iRH-0B?TiN+0}|) zhW(Rj8D7lyZX9^3oF$o~0SG|t%z{ew=%=~7nIu2tp|CS?++8Nu2l;(xyMR;2lcy=t z1pKBWMWjgtqBqX%Xd`W-U?BP{tLEluELNt0AD|T3sA?&)x4H z)MxuAg$X`=FCg4|35y`M zLy_ITMwmZ6KXKLBLDCjl#+_l%Ny#{h^QTA8iOGC(qhEw0CJ@o(v zescMM7;{)ZR*>8OO4CpnGgbdprdFe!il8=gTu7sIROP%hf56&Xt2eIRLNhqoGPh>d z)d52o0za$(bhgnA~GgKE2v?CjQCo7N8A={>>3>y zjjkZcO`_8p%H@WVo?Quhsb04vhTWIl+BN zOfsh?nAjLm0{Cz_odgvJsq3~B@$wXBl^iTS0x<_SY5ArO(-%6 ziFhko4mIONX>IwlVDrv3p9RNjDH1)0PP{R7uN!(f_N|np7^}2byu%Ir`UBB9MmMA2 zD2qmVedgB4*j7RUKe8uNmuxBcOKU)FC$Rd^iIujQyEkQ{xO7|T+e!HR&Yp9#fbA2| ztKMZC8Bg694B||bXB|^m*QiH~u^OwiUR#i6jp-WD`ecNZ5=!Ll8K#6Jh5%twr$(?#I|kQwr$(qb1u%=yXyQec31Vyd(pqDzI%JE z^(@L|%6Kh^gr|X$Y`Sj243TWFvVWoEOG}@)eSt1+zPfeD&Jfb`h=dRl1gLygMexwE z73e*iv1l3iZZb<||6}rbDNC*0ztP)^PMj_ztXBH$)L0@3DYU|xiC2Y=BK0RJD5m&P z^6!4F>e)TUo)b*6jOGO9lwWCmv(;ORFpZ)ztX?XTFHc-3Y$P%93#)mIp^m<0Zb?JN zL1m4h!8vl#aRHc364L*KRd<=UW=?A^V?i3{te4HjtC7%;TCZn=}(@mmQ4Atpi(*GwKOm{%o()?w<$< zAEj$I5a57T zNITjS`MVqbe#5A-X1R$ruw9c!IC&munP78s)H$4}-jC<4&|Y;p6zk8=dnsmTG6)tS zv;`Ju>_g87@N#>e;#38vJ0L66MDC_cMq6xNcv8n#!Xp{KR0uaL7qMs)Jte6Y8pC>j zP5jAA!?Q7L%;8XdQD#%hYP}*HlNgW0rK@rk&$r!KS6&LpIlK zD{`SMKd*o*S~ghaYVKl&>^V*5%n3_g5s_7`+q1cyWdERn)MOI==J~ugj~3}TE?tk{ zqvqnEBELP2PY?W|uR=*;-dXLwNnCRX;V4ThRXK@Z)~)N%yL9#oX=)n6 z4>H7jD-I<;O(89**m8pxTU3h$G5U-UD=)lB%2?~d>%(0y)=9!R}*E>s`{9C-73P`gyNJ%P1aR{h)Upu zEW$sbyzeyfGY12=&<(b~qq6~4v?A8k<7#Sg@_LSJb5-(6g536ZV znGo}*wYQ;JoK$KFe$@wQUEh(kf^)8dRc1(bfFqH1Rj|*SSVgV2!|*X*c=Txw@Mdoi zd2m`EtG%KvJ~gc?t7_)JoE_YK^+&cpXPI{!piCX$AFEv4z$c*%p(^f3c|JXcx2!+E z1(uT69p?uwK9jSfF3%5l#-ysEy{ZO1!4k{Yh{p#38mAb~@1@bWkFE{g~McQW2?2U7Q z9YaxzGct{6TOv5^8vczQ2tD-JI$%q=bvyf(t3_I8*j6bJ03Vz|G@8<>tj2W>Zb)(+)4p)bN+yGT zZKI)>Q|{QKna$z!3?$I_h5L*p?JDow?DE{s0bx8I#VuqcFu}JUHx#uVP@;0k$1t>Q z{0Q)BT(hZf02aEIY$0=hB2)Wpt#1erPOU?IK3cV~vL-gn15ea_j)g?lp$^IHW|&9RFsOx1LEg|a4lUk)6jr^CHj=eUZVX5aAt*^{Qs^F20FU`=<4`y z9|r>g3o9Gz|MhtMPm~oaD+}BI+STC#sf4^<&jO+#aHB1dfB=n~xb0apF93x|039R; zDkh&Uh6IdMNTH2n{#y+4OqfEUP_?MsPsv?=rt2j8@70YMy?U+fG>%w5PAKd_eyT_ZkDu+l3)2Nyn2 z&n^`LERc~=h|u5Tdz-J87?G*~C|McE-|rVe+-DD*ycu{d1ToMx3=<0Id0reHFHjE0 zI2i`K>r3yh%-hN@av)T&fTJU0VV@3AWh6Q#92kBOctQ*S28io#lzn(6QT{zTsaswF zsLjV-Juj5OAy81zAp$5~dX#l-aXByys1Y}yDLo%LC!k5x*9xs%BrxC)?NkgufY~1K z%Liyi!6uzQA`@H~Xy=|B#ysF;H`XaQ&%d?7|F|83&k!HuKDMq89~5atoB1s~=F0PWY?MM%RI zxDzhur%_(a2UXqOQ~nT`zsG~YOV8W-QUf#O8{DT|PdcxR8_d@mhr!!h^U1VphJoUG z(YN2TaULuL{=3e1*~YH0Vx#7l)|dAnb-bOfZ))|hE*cxuE$cDD08kM>L0lyl6x0KF ze?TtY{=dIMu#aA!gk4=anjgAKz&PY-2qiEYglpiQ50KhFz7kv*|HnXo?T`4UBP39u zf3+Ps#3_hlC=sx)=tv)33$Lfv$=of$E;XWzn;yaG%E=BTWM;FLyx7fd&y+8#>gJ%D z;L51ft83p+@#3N!KVM~O5S5&g02UZnAPKQweXH;)Z$T>GuCFoh`-Up|SqRAZk2spE zVH0d z?fGcj5~Nl@`o}suiv4CKAOWC^aBKNu`}yx<0GKX$5fKrfg}$x`AyEMSc|Slu zqiE-`A_pc!QtzrA_lrWXeo#R?>kK3!-jB zvaQROogCxpBQ;7ZLv>z5G=x2nI@{*UM^bJsXhqEx#9VP!i*S_|w0Sr&7~4$RzZFDs zE;~O1PrWCEK1ToAcY~ymAx~}!VaOx=B3%C>D8CyhpkMEdmz3#J(&t-!_8$9Pxs@+8 zKPEJ~=?Dwm!-A%R#%Y2OAh_Q5vQlS}f$1TIUsojjAUksP6?s6y+qB1S7Ai}VFun~J zQZnA9a80tS;i~&yw#??bedP zwvUw|2mG1MN5hKVS;-B^#V3?55!(KG%v@xa7MiKczox;4%^Up=6HGKDBrpmL4}S_o z#6D}`*khxOt*7^rJ?Nb}mhIknE&R|~5uRVpN+ha>@O>WQudy#7Op1*NC96D`2XkCt zAicJqZ>mFZL_|s0emS31lNEE_ZnJ&ae;Sav3p8@nfT@SeBRqQK$lsWi+g}Q`T;94a zOs^zn4fc z##)&2DHt4a>BeMn!KNGH?J$XVrcgth?yZ<$^*ZZ#?G;KX5m}65u{K)xGisSyPgLru zY5aZlYK4%s6_6bzI=CHqgeeSIN-GcEYe&ryklXBB88zEaB6EJ?9+ ziHt4&R@j zX$cED=>M8jpsd(V90>V!DUU+0<$D zpA1=Rd$ZcP46iVB^I@Z%@e$X(qVKtD2Len|GHs4c7r z3;>5h*p>_rn63=M;}N(SDENq?2p0HT+9H`0n{PD|%&(?TRJRb>9%fS%-^%7Q^f|i1-R@y}nFYie+Nb7h z!;5dEq}(lpl8Qp1A z@aC|L4e6&Luv8HcD;+>FW^~Zc!@f-Txz_07SnNS8gA9YmQv(AdI_iYD)#$MJLb{8C zP$z-#E&mp!*&r4pk@4QD*X7xC;j9{|k!i|!G`tZc^B_7+@f@R6nG!n_6p7>DPPoPL zNqU|QFl`K1*6#;$*VI8vX}Eun9Vj@9eI_rv=dr1~sO<1{;w(8wQK2e<-tLGRtb~U< zW-Ao;`4&$$#$<`$pp+9Q{_=@b@SG+*HLq#(dOCQgYn6pQ^n0cHVax33L1hy>S(lbL zIE?$iOOBy#6myNpaK<(d?g0Hp2gt zpe>QMWJv%8dr590^R@UUCXK;eoeZ*-IyIHjxq$~XI<0kB?p+Pk|K5NBRPBrwQ?5gl z{RB7gE}_9$zL#{zn}*z)YuHTPfAPc@Kw8x?$&xUYl6d!NE_-XzK1zGr!g$rrUU9x~ z@OQ6lbUi8DXXxxWB<(X>G(gjQm7^ymA_MB9bhPwQ#TUy~)EZErGfnY3si8_3_<0hA zH-ezQSP?U%(fnC8h^8c@F8;GoKv(Si4SGDX!+1GSb)`A3s{GBimc30`J@dD;`YxMQ z^RfJ9^J<|wV!v2K(VV5ewjUmC9L+~`3-Rb}P*ipMHo+Lj zW7{fb|0UQ=d_7IBOKGAEkZLUnFQH5tt!@T>Q&S44jk%F>nDM2-_QgDx!7`5Jb{7L5 z+6|)nPSuCgz`5j+kj7AFNhRta^!&>6SDYrwL<3{1{d$ zk=5gc?ZDY`AKtT#+x+Ktt4R|kvw!^@2?laWqkmIm(Ql4^c|)_#B^Hq?tsDdOA%6C% zAf9WmverPkO&L84A>O%r>@@$dWtQ>N3ttE=tfQ=SXkOW+;M)*Z@`jo+W3$Vt&Oqux zvg$jT?or757JK9Nv!rR#hS7Pd0ND$!Yi@)vGt{*S9$k811T{el;+aXL^G*c-XGZ1a zVYTyiT7?teFv!k-BAr8iNwe!-?5#tgEtkp$j-YyYJfdieuqsJ8x}nw$=|Q@iVNBcl zz;?N(`LTR-xyGDWT+JlRWrNQb6Ma;H%(b=&dpsRYwpRXph;*%febzLSK3ELT-HDso zEuVc&w=QMur8UWcT?*~FD`?>S{6&TG%J8VQ(l`C|CW25}jj&XIZBaB*p@7-$dZXb8 zr}#Tjw!m|K z*`hDzbx}WjS>nL(7vb<(_eBs1NsITtI6rB<{Rzm;x6s9N@Qe5UA+`LyCRz3lyO)F$ zs*-Vqg2()-<{xj3X7Mkqij7RB;2I%T3@>M*axCU@9nKcqsj@$LtHmz)oga_ZoGt1& z(x|yxTV=Auo!^(CHK``#h~Zc`V~ufbz^cvSc9@9~*_==VpMFQpU&MOD_&IG})^s2U zJMGyfyB4e{E(A!1U)&iYR+8n!6dG17WAV0PrDll*A@+QR98NPbZ6+r%ms!M?x8F(V zhXN>AtkNpIpxFpycacVf9@I`FXK#)|x_IRFUPUWVt zv!F+y7oh4tf&gsC5`uU?v<b#nYJ2sV_XbGn6gnS+!ZU2P<)JzV^ z85IuTGfVj{$9XS|l-h4QDZFKwULEaOVM{zr&Vn&P+*1f91GX0$V=vRIxsbE>vUs=v2i$lOR`(vu!qT%m=L=4Eh3&x$`8F~w zRSNC)ot_nd$yHss===yrofZz(nb;*-bQXZV)U|C!CS5b?QM#Ds{Z~A*X@@&a(DySj zG;eGmb6Kj%L{mK9GMhYo&`n@1BVxl<$M3vr82uo1RnWIa5QL(HZh9?SE1SWg^S5ci za;svTJh(lIb3(%V`h$5>#zX|_wdVV9qTZdN7~=|p>ZMz{U&D59WGvOaK^?b=ETlwW z-(dFOkZxCe8U!<(yMv0!vE=sq|y^ zyr_X9^tyYhv5F>pbXD+B$f0Hb;RBV1MaZO+|Wam&<2%!kXuc0yOhH>)74H8vB;}RUnCLNDAK&i{}bA zeS3gxXlkB3F`fvl@mn`W$waq=>-y{wP5T%&zp$b~=W)l))NjWmJ}n1{40a6vFGOXR z>Dd?NO@3<>hfmJnsvJyL4l>{kT1_-#^?kYLIQPQN&~P;4Z(|VZN&2`O55aTEcJxVk zff)7hrcMJh;@;fnJtKjK-hDp5410it@#C>+x@3^Un$Ahq$OJAtx?xpwN1Bv%J#tKP z?rL^T$Un9o%kKqmQS#5Y{gJ}M1`!1MB3|AsB>D1;8}#>9DTJt|XPxzz7`+3T_B0Zq zURYV`>Wj3ivJYLdF2t4A8AWhQOc_{QHrS40SC^BA`&9CCXh)Z@yV@$@koEx%uTIg= z$E_5`T%JfnZTm_Jq(ap%tWKZ|F{gv^`344!ea>l;I!;!gKg`SLj-b4q`y^J*Xl3}t zW)C)J(yq8MCB|4Q6lbua3bnA7Q$FZK2Ma%qif(Un9w8A9#}A!NTgvxNzT_Pg&Uc710$3un~?QFIfcQfblJQ&}}ISt7O??RSOnXktF)_kH<^3cXT($Df1eagDM46t+iZvDJCq&A1&p z>(pGs&~0_@e0*S=(@XLPD6W`!on8TBfd^D!U(QsK18M}w&7pE%>YB)fTmD+=b(-(lnz!EnwFmlbJTO8 zw+k#%)N}m2{Da0B?jIHSQ*SkQ(m{SOwX>{Gq)*v4EIheEJo z1CRVriva~FFDo20w2~d`R=dugZs{zU&@I^4s;{b37!&`Uyg4qbhBo}!2~VGklPRz1 zsQ0BCn7Xz~H70}OLXk%+h`=*9n-PagnsjP2)(id#gVEvGYh`P)lE(P4P1HVi+DWNu z(M(!*7FR}Y<)IxmzJr8h-IfL9+G*mQxH2^M^f{s)h-<2ULHN}RsTf$lOeayd>r`iO zPz86F9b7=M#P7QPqE%5EqMZf)tfE*-Li(|Gr(NZleN;~o3k{H1*23f)Zt+N`YB;AP z65d7TForCy9^GBSR`(4vw^pZQ`ywsfa5$w<3Kxw$L*O>R^eLUe}~IiV<;=NJ-Fn%{bS_E+anzUt(5*8%=fz z_c~x?73JFypgAaKQ)_d#3O9A)9Ax4CS)t3$*+HBjq4#wIuPsva(}2-D|Y zA2!i-vY&MyHjf!D89I#_tP{@~6Ui3KtSz9dBw>vq9L%<9lNrXgFNw*Kj~;|&+;<>^ z?V^{IogtA{s6)I+6V;uA%;rU%jk!VstM7#^RABdKoeVJoe%7pV4Ab9^$z9Ud5f-+V z&(>vDC;Xu)oS1p$!1R=JjK6kO`i$x~BRkxyrx=0^>NTQu&M)>O-F7Ls%~vj+P1Iss zPAXm%m(6xc$D>%0G9}%B<YdAMNMt#6vV=OwHQB;{ z+Nj;LxSA<6q#GIO*r%#H!nkFtQ3n}(Iie*jn91yqj9}wp=PMZ!r^^Dm zxK>1)c?jclHUX&LLK*=0Q}7ML*4vlTfE$-)OsaZ<*AsmqGG-lE^QQZe_q3z=ccg^2;@oG}-9%-T4zn?Sz&dinAb0fi8 z^j`7=&Umyl7gG>nN}ld8&`P6l@a0(Fr*SVr6 z9F+rDs;RoNxOaP}GEexreKbyrUM`5~;V5%qd&Xf*fPsgr5`HzxzGvN9EikyaUZ=U`(T@ZqrMVay!zs*k z8`;^ta?+34^w$Kq z(ytB}z4WdfX3MJM9G|#G{!%=ERyeclitw<`&9txD0q9c;?qC=vc{txfyW zZI;hs4#&iQ`ls7yR%Xt=kQKVg1>8Z$nSecu49~cAua0cNcBspehg%aqZdQ;6DhQo8 z^U^yQAR>1idfv^8X8p~_GNvll2P>(^Rb5rXTVjPp_`Ld64l$3^L4pqj zA_3aHT@uH?Azr>{CBq|r&Iff(9Ox^TN<1_bqFAayls1-ODq{~GU#DA6)!q#Q%U*Bv z^JHk6@>fLtMe<3n0hjq*`^hO|8_)%2x)0vajE6e8lXvK3&m+bBj}%%(71$;y8r&hy zNrp=5;!O$xB#9;pBMVdaT1%|el2dM!LiPAZ`MkLi+tqkC0}n>~Xm~-f@*gZhL0SkA zTHI6iDfNvwNE@YRFPh3d@_gqbCc!H=AvUsH+f_-7u;b3s76oLo?jXz`G;Dd>M;_PW zM>oX!QV#qdg{aj`IURmEVIu}@8lU?v-%8<@H$y1-xdk~!G;Z^}XLmk5QgD6PbZ(3V z?2a0^at0rc?Cml3eZ&9UXzT2@F)5heUs>wjlPRT*&Q6B%Ji_ouk1P{3ZVwJK9I?&*z($6ao)QJS$Z%5_Bi+em?VN0)Mz8+m# zEh~D&*>Erf>4x88OGcZ&1adqV&EHc3^RHrvo^Z)F*Nx)m{Y{#m;S7z=KWWsiNgmZG zN2`<~r0yfcJ~gKCob}q$QZT1T?ih+Xe1u*?dzhO(u`5unL#98fgeUQ@nk_V~mp9q< zOZfKjF*C}U+}|iDpMbA*5W+0$vVq^G!g-rib(57dh``0u2f7bs3Q_uu1m$gw(9o5kh65P?z0pa=*+`-wm z^=}dK1%SJ{LSN%C-H!5{WOx1RV=8}%C?8_=aC))IO2|}`57t>--2N9uu{{x(nd}_| zMNCpWhM=RbWB9jw;}a@0+E;;oNP>w}AvxGJ*EjAy1JRv9u-d#v2cxuj77s74fEcP- zfJ{_?7$E_SWB_Djzu@rj4_}n4EH@wn5u4;~vEGp@!OCSdk8Z^F*SY+uaE1*Bmq{)2fGd~XeL#BKC`@I9B zr`OlhdPWzMdN)QieA1A1ElzbnAdY`pKU$vf zMU!28F|pH9S2s8?*4#DL(?BPwto?w%q17SK0q7il@mXj1!bI4n9{^7xRw7Akq60v1 z4+txW7!)ON6wm#KtIIU0Ofmw^G~9GDi!X!G=fQEo=)OyF{&RCPA1AtLEAL&nItQ>c zPxHD3 zjPzf$*M=2RmAo|YC$a9Iksv-)Q@;2VHl_x)@1`AUtL#7`GBP6BcKFoae3wAB$Fa;z z;2xjMeLx}*oSrEFk5EQr&y3jj z&)A^otJuNi=Nr%;l$g0k{M!Su-5oqOo>||l-n&p&r!S1A9%&v2<`3*|{$yWIy)V$M z@4b6~?alck-~)jA;M*IELH!+*`UUFFp$2sIop(pr@Co?|HGhtOTadf_xVCf z*W%FAzsWQ<|0cJ!{}hDde{=l={4_C;R?Fb0wS77ld0#I4?Dpu85zK;`fwa_{+4f^w zEn#dvsnJH=$n@}z@rriW#LGzEbnP!*y+?s%323L_wjMo7vKbcwjJ;Je&vnd_PFr5g zMH!t|SBQ(I(Q{M-!H&|tWLl5REBng!759qqgKEJZ%LO2LdT_lLK`Z+>`FbYePB_~g z2r1C=dD{VE9!y`QTG#;XO;wK4gIBm84R%TV0<St!9kmm_~J z-ycf^YTNTHF;aiD=@+Vyra?{543cN_KFb1)q@gy*6gXAvP{dC>Yp;-g%|mi!2+y&O zP9tx-wSvH!AaX!Fjdd1Y@Lm{6#k2zZcy(1zpS9N!_JfWO3_@NT!h;1jLA#Wk(kxPl+U*2os1BctaEkt@PY8pA*l=x%;OM0{Yqku}1PktUOI`*`P}u7 z_@L-q)NFAvoPTiMiq-xpDqHArtuY7nR~3QH5)o;|2X=9_wtiMX0B;}dFdC;yaPRJb zyGXafgCTb;=f9&%w%*VI^qGH_w{%d(pd{Cw9j5FOaZ-r zIIz=j%97WkWq?SId7hn|GY2ttau|@))U*eouhT30?|n{|+vqXvLUz%8cF*oDFB>Sv zPP;MdT&9)62nNJ2X643)Uc1VzCG$pYGeFBaE&Wl3}Cs77>L8lGtTaLdjoWKd& zQpSJtZ&lKh|M60ff(pw)txKcRIdn&ab;~(2)=7YaERQhtMrUSQhlRYi57IY4xszu* zj4fyo`^^r|*|D6Ug?_jfp+v(JY-2`s0&%*{Rk%`bj)Ebs;JLf8tCi|LW(5*DvNML{ z3uFL`ri&v6D`W=ioubm?)Xim}fWX3({HNmdBt>tEBBsd*BUpSBe??Hy`(eU-(EK}C zC}MF=5ecW${~uJFyVXHG$U-FD4@9FBzGb>-yuE>xx=Ypx?>m z2e0@&s;t?eOb4m1vc`Y*SzIx?U&jLEyBOj6vv?>25Nq0IKnm#4h5Y~tG3fNiHdurf zq-4>dc<~g@NikTOUefaJ@G3Y(%G_a(Ev`-#Y??Yt6fqxssm{r+%O)z!*cb^1PtNNN=!Ou8>&`l6Z ztAk1p@JuCtB+<4ecOSM+mLDRIcIp`5a#`r+JTqS$&06YD!92#Ez>Y^@>h-o5 z(bXk%dFfawb#7tB;Zp1beaX$N9$ThnY|YRhrk=1KE@B{&y)TroP|1?haL;0sFRNEhQS<+cP1Jdc)GF&grhc-1z+WG154_ltPYSD2Eo8~759bz;tQjV zLK5f^Q9Jzdn?mP+%Kg1<#+K)68gh*X?4R@?H~!e%^RQYu`^wO#`z4J z{=T?x5>;b0VC=jGs2q&Izv^(v45J$F63I_H#P7uRbAf)jSHA{mtMC0$}q?40GR;S(DU&!{IH|c%r#D;DU zZXasgW_7W-hn9MfU%OD*xGG0XW<=}Q8#MmSYQK&sMz7wfL_&edo^;CWtnpR4`Q~@F zasmiw3>~#q#f*%Q3Rj7&(tc%*E?I1Gp|R5CeLCC>c(R^>3sWXBU+d-v)+E3ZG<)eG zVV*pWm4LF2ijB-z!igDqr01E&MK){NOU4xij`3>}xvThW<= z&02o#qyFyoii}CM3vE#Gs3L9#+4-b>3q2(rFRuDWuVDgVqGmVXjAy1MKKfqIr9ia4 zYXlCV5G?eng~V04sW{kM*_Ct~C1YQ!V5O7g;W6TP==uBl=DlpS$@RwP?RnR%bC?1{g7T#dnD$)m zwc+PZ*4sO)rouBjm@^pB*iPEqVaa3t+Sv*TTO1hp{j%@FnsP&6K#D7)Rj;WxImLgN zV4hc*on2uCDMsTD4xFj1^LaZsD`|llC}zCATyW&$X0-5~yGmHfr2N${7pE7`h+(OH z85~->$1}^H0bor72_1yLiW$k?n|)L4C6m)B3KZzt|6v|w+J&PwuCWF+qJXk_jMO(93?fT8d61kV!gN0WUIY%g5a7!M zpHjaY`dp-Ce&;a_p1xtoU6trZ^e)YigI_>|#p5s2t2cXacl2rjQ8Xr;u~aRua#7q{CNde#{V6fl>gyfi=Y1^3(R z24{ZGm8nVS`+K+S(z@deo+*V-1B3j{Z&Vhuf&vDDJ&X%^Eh@sP)&y62QxpY9kXR0|sTq3TT~Fwa4dM5mLE@PL*2IJVFVXCK(o%<5!E zz!6m^XC7>WG~7cX)r3}HD>W1;W-welv-Fn}gD%Hgv$0t$0o2r&61Kd_o;Nv?;S_ZM z4B3R%hn>cOxolAoQ5xTHVx3aWh=!mCh)4;6;`q@)izELpZ+7RoI=aQ0Su}Yj;p`!i^h->d7bCT3 z4Ylk!>kA6^twVL|88=(iik_6MJYAX0)`y1>VRtqOfjLte0i2wm^DhuKL?}x(scd<| z=lkW~vSx3CJ92PR6m9zGSnT#uh|XW^JK}HdgG4lK{b80^+u$KMoP_~eS(YiE6zc9D zA=3xQzeXIA0EgP8m;2zmapHgC5k>ZHQQ~%bu=1fxgx3l@Fk-bLh_KJJl3!2;60qv|6cCt<9#tP7Xo1F!-|H>%oatGMSR0iiqQmf6*#hV=X z7dPK1#ZqON-`8yILJ4OO3nMRuNcEG|L36-d3M1uQIhx2v{E;POx)0*;nlEa1{;Omg z7lL73rMvQCSMvQXmrO@qmYKz71EQtB zGLtGSFe&YW@o^>GJ}axqPS3uu+F7ilP{CBllqxJ7Bx)X;^#fN~h3dP$V4xrBC(TLq zL-(9_R;Okk-A-diL3aCyP~M2(3r$65$R`%4h_HG&QYGdOG7=pwcV^fkEW=utSxvx_ z`Et5cz=%I`f4F11pDj9vei0N;-_DNFtZ(X-tX{bjEs~%LF#KVRh4dv6G|VAx@Iy%7 z*Cww2%0HIpze>0>)NNbrgG&~XG@ufO!R;EsgpTEbiU}jVz1;x#v>P$7h}A536JO{V zUQmLt_Ss=7+tU_POM^P1{*_*ezS-Y7P9U9R;pt*2u`KFCF4fbtYSgs4CEIj zVgZrVaLN8YgRh%7ce`U|2tg zj%Gu~cFj$Thpiz=X(wU`rvvlsV59y%`SMKmDyKS9y=MnKPPr-Hs&-P+7_PNg8_9?^ik3x`~U!2 zO}nLWJhwf^5^71oNl<37%(W#F;(E`8luErFO%pCPVI+&vH`m6jHL3^NOhFls7EVKI ztz2e9;f#I3HQ2fd-1Qb*TZxonSE{hLw6~!7*sDUi3skFEO0*4AwlNPE?Wo!A86`ZvkMY|NlQeg>6NBjeA(TNd%qyy*Pn^4OZT?X|h=pM4p1m;E+-kyi*6NC#aYMGvw{ zXoh`Ml4jRoQsK^x9f|@+lHo&Dq&ILPxg)&^F3Fnr?0|CeJ)%^T^YkK9sW)=q9)1;g z?1ETqj^LMswNL^n9wLXa~OfNlpp080KC zU1b=k5YTvLc-23;1={>VE$zf?_+BE=pP^87mYgr!Fa)V~{pZ=G&t_2shndBL<{>cw z4W+xlmk#v$tsjW8kzWDr5OQj({Kzl$@!@RWdfXWjOW#a=+k5AhqYaFtQOw3p958CR zm1-vDVn~mjB^~XOIBiv72%P4uKRclNxm*3x|{j zRH=pAFvlZ3H_^~cCN=ovzdX@+SYHKBmha?l=4XDT{M@5)%V6c{nO=q zI`z_f-d!{EwtdSFlV)n1a|q3{+wnwdh0ePc-#=j?W3I!+ru;CC$eXIsm25RGe-&1{Oq$X&q!ahv(#nAtl>w)GlMGvsKVSz{*?7|_4e&a zklB!J#ALTxWNk7VN_p6FSMZ?f@_KLeojm7+SaAt$ix#0sbIiBc1WOw3p!_J+#+0fp zG#KxAWBn|{94Kd2p7+A}kYTOS&^d95l38i+VcYN3GGWM-(|Wtj8+=U@#GRp!|#`ZCT!g~so&}< z==)1@0+Hx$Wcot+3*AKG7`l>Fi$%nCTW^MftLDc|h2qGh0#Fkmj|Df#X73;k0autY ziq1L8G%h}SS2cWt|GJv9-S^EnR-p0+e6dWYNiN@fIB`c5si4&@HC#`jGk?kI!~zgOPR|}gEQt0o{LH9&z-Qa-SND94d38RFO$3G_(`zJ%T*i8=y&2FX%I6r zHvT=S6_s$ET#>8;;XNzQD(+Q`{~iFoYW`EUR{&<)`3?Yw*5K)-$ur0V9gh3cga(x)pZoq{+A2#x0-v=W&Io=$Pb;vuhrNU1=79B* ziS=j*a66P}%c4Zu7PvRT2@WtH{FT!*4=?w5^2%2j4K7sU&w*ZtE3jbfap)gZG@yGc z&g&?GbTpa6g%3}uNn)hg0A&h&sfY-6#%hDJ_s4VUHX~02r$m7_T@%`tSl%NDGmj$teJo0kX|2PU8CBz9^b|LV=EVFY;aQXEHQ1~L~7qdfu9$kHy#H@xQbxb0h1W)70y?0X)+{aXv8woCSP z)U~I>L6;NZmi}SG$GgzNQeK)!k251O`{MRi;PWMAe`OL*Kh^rWUC-4VvvD4K!vfW3 ztxRIcv73ItmTe_m;SCoHaIbDikjHrDbOcaFf6fy9Wey7!Hx;IKkHl}V^1=v3C(_=Q~bJSl3A%rVA>Rq#;$~w&;Vq3gm(sAp<4O_&7DZE4=`8&rNT1^%*8D44Hu+fLyIirEv|8_M9|b~&DRJE?9pLZ(;D~&Jac@fZNzEmYeBB%dBCC|$t%xUbWDR7JY*R?$ z`vt$?Ct7H>ps*0zSSzD1BhnEPz$dpsyU&_EX-Ym1{eh?1;X)~^3+7vR@Y&e=HXsGz|KiCP9VW3t0$$QYKE)SjJ!>~v<8$VBV;EGfB+rahWgdn#u_+)1rnFkQ zvXsKs1w3O^l@5nZbz0mCx@?@q^iICxZyA}7fc9;iNysc*3tjY`s4ZH5&OskW)^|&sWcA&ApQ8RnrKMz{@cBm-ml*v%EbCoZ$p>_cXm ztw!?r3QAJ*Xbw4a{$TyM4z_q#Y2wM1-g8y54}>*R)(k>Z6Wi=zIwP1{5dzL%P$pl7mi{0yF`%Q zIYc8-*z^6ZSl4BgWxA{9OC>VyVg?O4mH2SFv>FS%KK={~B8gfj$hZ4mFw$$eDWeqNYiIHs+7{E*d~L{4VLlg-Fi z!rGAQVfECsUzc32#yWnioa`NaiX@CzL9|SNu+{Rl{O)_2;BPw7=6BxQsKIh$9_`+# z{r3qQDmN41VoCnHZYRa&zz(4o&B*4XL#!Jfw4+SWdTu+}5qi5Mx#%Sz`9v!lRdH%0 zvqAld04EFW-;xz(Z;0<9oE6e-UO9CUM_n6ZUU@7T_Px^k@cZE)R!nXfH{xdZE9ex_ zx6=r$M>+{vg=)$_4HliaBIKHEIYVr-tp>u2avpVTY7Zjm&8h=61;WTThHnWjZ(^4? z(78)B9av3u9EaE1d16KzehVb#=AgogqrvE}*F9;ios6#m@%((=01CzfQzcY>rqwtE zM9;mKtkmp!8&&c~rTL{i4-Hn^!kGi_+%1Ar*L+B&H>QJwc}Rk?ybJRIPX^b+{jx#X ztAJ1oJ@Ap&nlS)(Z$c=Oc{Xspb$()-=|uhB1ViE^7YaxEE`7$qoQ*5c(bGtvMnU4a zJ6xFHv+(_y!-2BG$-oEZG}|oo+x|Woa+4l@k?R1gJtau~^|^=0N0kwx5FS``7xb zogBkRZJ!CoE4i2AQik&KvOB=xI+=nKT_sGBHVReM?9_wVVBgJ6Ad>5F$3-wwoRA(H zHI84yHi2qz9DG2Kvzw3m3ajR}X%xjTs;d&$N4VT#u0-{P{Ewmp#XE{h;Ga#38tKcA zvz@wTY$4=H^pI?|S8MSEOs`GD|wXSNj=Cz0{FySzLvtRSTNxiDNH0+ zTg2+WVUV_?SKF7BxQE-h^pIGj+OFl>S=PoNXvjcueaCj=vBN{L*ETNbJzjBUkUb9S zc)a$;%TriWln`qg6S^5MwV@+p|8fk7$5RNY=p{lOp*qI2r0UCG*^&9$A^V^OJaBUt z@?faVi`^E0jvXtUg;cEPYQaJnRrRbq0c@p>^hwrh>9%Yh&?~4^(eFmk1ehshUN;H#zf8d5h!q5#Ix!iFqB{6F@%pNaYJ96 z$eBwAEIK)#0WEks3Iw0?N@%WuXcD=cLw}N2tUhmJX?r9lk)$ay7X~=5-!wq36g>@| za9Jf%GPO6l2}^J%XcGxgl&#h7cXpqk>BMbbLtVPKL~1U*z@(SIvt&u{h6*mF)2V?3K- z0#i%E3`blTu702~sX)v9c+-6-^Niw<0F-hv40ihxjrVfCmgAgy5G~v-D&^!^phBY9 zYF&PZo@GTL{1wFP#=hN?7sH^fyQz7AJDhc_%j$gW^7ut`0CGSV7b~lWWTH`{9j`Xl z@o0iqD^~DfS|MwBa+!y5s{<)AF{%f?GmU469}={Nts~A-|2*C4llt&k8n?BcJkql_ zm5J)NoyxP2>RERM?8#$P@Kk0OurkkihaRYiSz$-w?QJ*Dme5$Kp#zJ(${)8VB17?P z)5^1v^gB?Xw4b{(Sz*A<(1F1{D?o0!x^BdsKzFti7eX2SD4jl7^JKaS@m0-icDg?1*R?Sih2JBs6i2ZR zjpI7bDlRIUUvLSHHYFDU7K}ND3RU=B`iJX{!u1k29Pzi5u6Z>zJg|`qa5^-W)%o2^ zDq{jGl?jnXE3U9PD4uD;`0&8yZ_i!sQH*5Gc1Ls+rH+oz0M`DarH%%qLB$H$fmO0b zaBFS18KcpI_9kQJPcy^4IQvAIQOV&LVG&K@_5P3=aw>X+HH@Yd70Jd*JjX8uba;)$ zex|6_u}ZiSoRqsz^V}4BIxN78#!W3psid~w4CZaP1O0i=3p@`+>XRI*r8ad zGh3#`-XhUAjNPxg19hLiIy2v#jF9PcEuUzPN|dk$3f|et83g%^J_7>G@;wc<$|y>r zw&2fCO+y)axes(^R(j>27}=Jdf(-vL`idJB0<{9|yO-PU^g)Akfw$_8e8kMC4?LoY zZV*UDIWs=*HgWS&DJ{zgTWt6v>GQ5Zyp>*mb4s3pOdh z8PDg;lr6u=Fa+b&Om+Rxe@a?xH@=~O7)B++P>f3DXK9yvwZc4e-cjc6cLZLRD10WC zzGu2{s&oOA+HD88Yf+3okY5`&aeo= zg_1?G(`Q86{L=0rU^KW};OWNmf=*urxc@}69q~=0C?LE0mNwO-&*r?uLq zknHqrD$lDBOQ4hS=Ajfd-dme&^ieje%j~*2ZiDWmo)=u)gh8heigBM}ZFgCyca_r` zA`s3%LDnRvx{9p_sQHr2NQ{rT$z%+LzE4*LT&yF~BFH$KpI}5@zy<5f+FwGKd)dNu zo#dm;)rUX`RnG|RHjM1RK@{ioj7?L_5oE5?B0TXwOm*KsE+O(GlyPZ`+QdSBu?Qn8jY_&h-SvqPZXi#-*3plg zDA0e2SPB_OIhg1U_kh7re$*)vYkOCAJq%U>0p6t1>tuN>dyx$oW{;+=Pz;P}T~BY) zFYOE>eHOx>in3mBP+vNvlkjdrh5gWmPl9$|E$LDrxM%0MAb@rYUZ=*RB3cwYJ8YGq zvyN%D2L7=+G9Ggiq$}&@*|_)X8^ts_kLn*~Igd{rps_0TV;0%lOcoSxvO4RTCeC4k z3-FKuo9zT9!ODfn+1hMbq3_>*rHgUZkI)DV7Q`_xuUyvP2%cRi&X?ZC6WEO}a;bWU ztTzjGE=T@OwW4-c;0s&^cDroG|11^P6+WrH+%19V9tw*oLK<*>irXw=?2 z4a3$|u7??jJK>HF=59Ion4OP)PdE|f6iGZeK)8m|TU{nJ)Ic&K%HD~EB2D~B!z3@Q z^wvj&*Gk=5!nT44nizDApnxfXEhMOrHmk;~a9chByRTNvg(ESXMTwKT_$oQ`zZG6A zT&qm3#a0s@$1!JB2z3_LfIwO`(YOW<5IqEWP!PDxlva{6btn6{V7i+zrEvYSsZByE zF0y`NNWL_&Y#(Fyoa~c_gU3RvkdyFC;i;u{c{(W2bL)pX0XFr55yF@|yi7@U&_d=$ z>w(7{yDjrc9$eVQAgCS`W@F>`-DU)HU7&x0U^sQ= zx9dAmelC@+TtOgomd2X&82VE!l6k~XGUzW_LX@@@_BuAXp4?GW%wDy)1uZ`Uc8sDW zWF)r306H$Zn|S|n;5i!yz$iX}hkv0+6W!PJform=EC;`)+xE~xO6^}s_~hXrhmu;A z^cy?PL1e5zk(-Cpy&Q<)z&8HR54_1~@ z;jpx(uKT%(drm#TgiAShMF5g7a2HW)dg;Q1i&a0tph5YLqtv%J?sbN^wdDSe(JJ#c z2u>(aD|U5kHeR+?JEY$AMwg_+m&2uz-&8ifD1M#X;4+MNdiv1*Ke&#mYd z{wxq-G?lK)7OWOMuaz|9pm?`H-^oO1f>v~g52B)s(+&s*N`z&aUQ=rcT5U$?PLk2Y#rKwN7TEsxz8PIEK{ydgc5Mh~UxwDe{ z=7uWRer-aES@h~SbpZN7w@!4DPx16~lHJF`=Gu{GL-HePwXdoOmATP>V-%x?rg`!B z9!sTWf=G*thB=2g_?5gaO_k|$%MeK2GkmuaEu~uZZed4p&|TV|bzEt}c$Xwz+#v6s z7iUOW3YY~LT99u2x5IzTYKi>Q zC24zEr@LaxOHRH_lq|m!HXq#%y{FnKT9M8)-sg zYn(e6AvaW`7f(O52_wWL!(w?OX~3OSp6xHeh&pphitG_`paky&hJR$j_C`2A?$7um z`l#CKb{u!M8a_|@!Cai)Cs^b6^YZhmdot=>N|KhAP^rT*3U!KUjECHgFR@y2+GR?| zKZ2j3xj#5$h-F&BSKAiH4+yv0Ti^A=xZ|PH1b-M67+8bcI;Jv~P8k8RPTt z6nY9GK0uEg6HLMMT~bfJPD1?Oh_t|K@l6eL@p((g4-J$QV5Er#H8&-#Hb8Yy+d2)p zW<@)hZyK;+nDoX2+El4XY?X1J>V?%(TwYKW>+bC%^x$!AN_*2iKFqc(R)#Xt7-#;; z3fPV`qtcT*cWbRQYUmqMok{njhP1vgiHXi5#fPk#2K)))9%nwe<4S$o3nTt|Kafx` z0*{hPDG6Ok&TE0alffWln>RS787}fml=hv20)XJF zQ-Vd=is0H>jb{8+4qG1z=Ni*>wAdj2QhyM4t4IZGJ`po85bI8J#T7ELQx-S9v)oTJ zj%!vK^Taper;iuFfliOyymUYhwdY9dWcS=jZWIk@PoW&w+r~i~Qtsm6v+oKi?%^TL7X`;HG;RR2^c@ofPRqlL9^47!k4KUN9z?LCj2~q=VV7L3z{(FH377XcNi;&HSm}NkO zI0INFxv2}w`{Wm$T!vBNFeHQU`MBqbI6TCzHYEv9yhCV7>aFt96SSmf+L_VpcWvc= za#!yGog#&0Q@`~l%G5t)Lx(+iJjy;-P>sUGd+-YF9Qt{Lot;c%Sw8oh|63M znC;agP3jQ6G)PbAmN~a=i6sTd zon6|~^@jR@|HeR5h*>IQZ!uX@JfRx(BY5CpMe5f{w-CL`b*vc5HT5!7Pu)%rQxo@;uOX;p#kGp zFL8ceG_Os_ssmytS@QrqY~u87%2V30ZGb?0757 zEQsZ#=2`Qtpc+hyHF54!ad~RoA1I9I!1_C)VBNqgz4)kGcyV5oAHMmX+Mzy`sJL5X zqV2;>zEZw-_DFePu-Pn{opiMhxo|U5BNIjDF}wEfz&^0gO)rgz(2rF|F&Y(Kje+2f zBIW+a_7H#`<|}Qc6csz_`n7gkMUyg(Q94i5;^F8qQ>i)sq1-&jbiEGKM0?;bwP?wG z7BCHyg>lTFK8u!@Q1V&%c5({ZlD+7!g(Ewqe?&;RkmbAsM+`el_ z$#DrZjGYUBzzH)WBI+8w0zaG~X>7OC81UDqxXg>D2T}>Zc=V)g3>*Un2EZ)6DU)&v zX$SrL_^)Oxvv%JuGMXoq01hhA3AKG}aFa%6fkFkD_8Li9;?u6AG}7k;iY^|*oFH8~ z_TViuw!n#SF@(j~@9I0DBE@5hYm|{8kjArgai7i4=suXfHXI+FRcDG0@iniX4yuGr z_}21_j6~w_wjjcWlVYEJE=eIOHg`)8Aw4@{7z$jRVtJ5^zTxW;;5 zOulQi7{R&yYWTl>5BB|sWp}hTAQo609!u#ObJ;P*s@?4eR=Ifa!k{*jK8NHSYiIPC z0c}y-&el(PX|0B{F1_c4dlVnZM0^MC-T^^XgGG4jhk%&JV~@Q1y8&jvk8Jt|?G!)m zm>K$-0)Xwok*xg1NFq`e(3nufh43JNm^0`w7`epo|*YynC6uBO6>V9=??iP}F zNDGsBf-26Fi(o^(1kMfFJ?|RwT>X%8hFWbm%=u-KACl0F0A0%*n|3ws?KFTUs=mMK zfjU}*nl#swoxuR%(fs@4&s5XfnxCf`x}?S^;wNeEZ!bSywo%wsoDu?UplB8w#f`A; zBX1BCLDSu%^8DBp+kI@vQ1Csj2+m@Z#VdOQllye+^(zU|83iXFa`Auh5FT$UbR%0 zPiC0e9eusqG})i8>YPAFcesB;fs89vF6kDe_lNozUWJt0l@|l%MKra0C_tr-SdV>b zAZpy#AKP*~19n08Z1g`gR$bQMs$1I?wwpaF%?pp`BHxW;$QHl>B43I2AOtQ`I1M6RPw+?57dj!E!|d zf8RbHkV{r@t_ml#6dXG)Ym3~DPcjq=MldSKUN^(s;ML$65wK<%jU&^!ldwN%F0yC? zcMv4b5`^V?y65U9P8>WYfQy2 zR|#T2bXZl6Van(wnJ66-9eNc*%lzKRlU!$UuW6Nr1Ql#eCkXwL2%$ySUQ9<{NN3PI zbP%RcVx5%OT&f0f^nwP61ye`DBDtGBexqCQtk}L;|8iKtOdVe7WaBB^glqRIewM8xX~%?_gC<^O z$44f?m>D^)6ZjL}zBcm={dE*Cz{yggTQ8``Ul0jhWcD+q&R8fB12G!9$ddBl8io-uL zBYfAZJ83g7`KiOEPqccZ%Y8pGc)nLe8#)3a$Sq9;p zV{is1f2_KK*r4S?$~QCCGq&BE)_Qo#6H?z1JCkESSSAG3^6G1yi{SCijMYxm*HnWCh_HQY-w5W2CmEyR$} zidHUltUQu3rw6=$L-jr}06E*LTGZ#qvC5)|6`OUg-0PC@m5;L%-`y(@x^zhgq}U$x#!-OiloonZEK z)dcA?Ovu$bYGd;5VtqiEWCs~)t5N3llE6~-<9?Rw*@I7)5J13QJLWN?81lf+K4}luVWG{L90@J#hSGru>_?Dt?qm7 z;b}_U$Jgie%h2!-HnR~EUPzPpKc3~ZPR!LYWs;|@WCr)_{@7jB!V2?K`_Pa%$7?1`mz zwJ=)l*4Z20(+u5leS=RNl)>}a3RKjFqgFHA0*+%(0h~iq1pg{N)B*zyE{8;GwWnKf zp(8Ljl#pI(Fl^5Y)`Pp^YLML7kte!2@>JP5N&;Ytj0F*1vThnGHRXNhQiL*6^YD#g z;nYBn2;yVp+G{nsOy*X}ZhvZ*s8xon&kCe z6Oy35{>HDBRFzEf1W^dQ{`L$TG7`P`0n zmqhZ~e0gEklL}n0wc~Od~QrG9%#qu z=p}IiJI!Gt_UD*({IY@=-)i!e4NtPZ>E9QE&lMB(&{AS6tiSJuZ6-177RmHl4=QB6 z+pM6dC;{iZyO2)9voNB%V30#}Vr902i$sE>x+p*f3I{XtbHG00AcD*MQ}LR`!;Zu! znpW1dbo64}Dga^pgQ9^>VQH~;#D6da@TQ~xJlioHml#4b$WT4(k;#E9z|#@BhcFB| zaaph5jRI$_+?I{6HArs>Sl!_UZlibJ5<4Frv=sFj8E!-InKIAMNF_1xI?H7E$saUQ zleCEoVW#;g=Ln=NnACjxV%?(E@_uHvjK^`QgphHonyD#DmZEJ)K)%R$S*KxFopqZv z1<7(siRkPVFefw{ctRa)44?QcpVO1Ue(QlKiUBbC16z-C82Vp8J@fwn>KT}s{~tmB zpCF!zgXMn;;+fc3{*R{r|Hae0gRyVfZ1onFXSP^|Z8DYXc-{51iGwGU50}1H_gGT3 zh$(orA&cB-sx0fapx7+X-7KB??4AAm#T|XcVv9X?&0Te``sA(UU>le$vh1fg@vjG^ zAlM_<0pQ91%g;B_Cj|fo2oUt<&&TJbsvbF2OcgC9xe$G03d)b|7}M&sPix1{0zbc!2AJl8Nr?&F;4~X;PeRA z#aXc6>H7zKAHd-s@bHL;$A5nKW9@-k`FI@ce3LN8e_e%4f!F!~K)~RTq95Nxz+*&Y z$dfXGd3#e+QFF5>6Jhpl$*0GF?g9$306z4H*S8R^0N<7v`8H>eze|{Syj1=35Ru`tN$iGkxIP{(MFN2!G?L&>Vh#b<2XK55ZuG^0H}__jU?H2D#E^W` zY{BGHmw?s_xW1S05RYM9ga|(3<=5+QKm0-j?3xg_1pp5&Aw!Ge-}^3=f;)o*&VAkv z{+!xr?Ss*_@9t25A>I7HEr$k%V)npceXapjmA;1rgx>!~u?NWg>E*SBh1CH7e4zon zg0kQJNa&v(fc{v2Ll<~}-#ZAl0pOjn_5sL&qksF~gf=|_0mD+(cl5t z`31=RdDami$Nt=$nW00!TSW8vxR$Z>0fndJ006#UKG%{^G!0x_y^ zXv_wG%1-<+PEHVZ0r+$U902Ot|63mT@BM>EK>-E+W{R!Fd6vWZJE#H;!v{F@ONrVo z>yvu@E&`0@0~&;QH>0)z*6h#+80!nMg+K-koVWY;XV&rO`teust(N?!@*fA5&xXWB;+a3Mlpn%B_wT!}f^@%6R1NAH;KlyOt_pJ!mIubsUaYy$ z0|fUG5brlcC}C3Gu0Dts8N~5ZipB3hwC@xKG$hCcLHxW93-ksA_%{uP#~2^&nI=$J z>ZuJPI=8^@R3$J79=0!3MqV9Q|LoTGEGUlG>RA$fpUgj360H8sjsfWXV;>xO6U6_P z0RTXt{pgQV9nb)vkH<4^{-4-UAAvKaFFo|P)#z^sL?HuuC+xiu+_S__Ord}p`8Lo8 z*1MPnKAf9d^Pk;MzSXbS&zB%TP|pDV_Xh^BIOFt(iu#yI4R#IM-6;6%qokU1p@v>E zeWcB6M_v-CZR+H)k%v~;<=4=h^VuX3sp+>x2P~~v56#{&FkMO2X%T($=jw5sI}~VN zPOk$n6Fa^+_Ly*Oh>BR!lmZrW|eOdanc1YAKxJ}PCb?2?1ZAR1`pf(`M8L{+5 zb2)oQcP-C!nW;22RXG8ZRN}kWEh+|XsHLdUTDn}p1KJ}_!}6Z@D)k{#7%JM{$@3cz z5+}v-c@08EEPfH`n~K>amMm_5sraOVov2~`1XLbXav2Gu5?YsNJ&nOeB<)W+<+Yt0 zCj6th^Rg;`=PwKHbS?;D8^a>TlW?EWdyfiJ@&kkG$>i7 ztLaV8j*O)$67L_s<#Y*gVC7P}i1U^`=`o2k$mQ9Y!BwkUg?Q#vW^zCjv0;nEzcc+jM*MIFf}bPUM*Och8>$FUguA_J9Qm+yVx`@ z8ZB+JY7NRT>1~({ya{%>_`AwAZo!VbQTRS|Hrx=zVsIa9(!#sAM!VU-V>3x;^KVz= z)`AD!4xxq?4o=^vKi&DiDt2Lw>#(xg`Len1Qcl7BK7cjZk)@o_oCCX6qmP6qT z9&K8l7;^#|W~p>vJ@d*c=VA|S1A&{pSp078 zzyG>z;&pi?$)1Xhc^jGZAs(+mG%hqiqvhX8i;VI~$Vz(>`y@e*xjzmPFX{kp#L@Bx zi;(h)-Sa~MXrgBj8KS|CLZi25<~KJ?t6xE2{yYDp{&1wFpzksd~B3x}vOg^4OM$CI9wY9v<~yqI(`d+?OaFbQFpM3;Fq?yV<2 z5~&vzawf^rGZjggzr-_0t9`1P8zvLlA2k#-QI>*!f30P`f}7e5uCcj=NW3fy$<;7& zy8tKlunuKYJd^BR;MP*IH;QYRO@+KmR=iQw!vZ$)D{CP@&IHJpKST)xru6So1Mvy9 znmogDM$O*95=gT4^oFWS3iWe;BTq7e=^_GGaU7wjGO9WMHhQF2=EIY{;!>SAj#l)b z>3}^+WuBg!nCZDNQYBeQ{2)KL`j#i5!W%Fin zYtTMTOlv30VD4stz5RCh^1_gV>#4FoscTj@p}Vs^DXc&Y;iW2qmcn^W;YxP9OAuUM z=0(IJE&oatT}Q#=wO=7do64i)Jv)V4E&@LYlA^?s?-)`?#7ML0(Dt&#&RWZ;5pD=p zo2X~38?egARErJkn}c6Wjj6QM*Rg(wS5&NKsY!=o+5}vK%hIV-z@Cm?sFDYAK&L?x zA+{{ka97bm7v3EQ$ubBZ6^o%la_>W=hgiBVr-M5iU{CKOq8u5ie6taD`vLmOBYp)o zzAKz>%bYdj3$@lx@VgY)-uDMJhnCMQ`XB{w9^s>f$W)KpB6Etk0YtB^BV7?p#y>?< z9>!i40_JVI9;vIEY%(lx|4siOc_{-@lMF=&6JM0cCY!~{P{jVuetA9!1NpP0-ts2p zZjI!yMKq(;+!CVU*bzrv%`CNbeXa4Fp%3eTdy5}YV5ubeY!h1`gPE-XP|PlW{o^=7 z6#Fz#Rf*bo?WKfdoUSJL-5)WeE4}G_Mo4HsF!b56Nu4=WjmXQvV+Z3pO2M7BLF1}d zd$Dz1dTeDy=tdgeFS`M%#~lL`o^@Lqo%Jasw8w$1W9TdN&?=W8W_|s%=7nYU-eeEO z&l{_boLE(+`JFF)xVUbnh+{gUx;^5)-2jQtkki}~_j2%krSWFOs(EWzYwd1)Y47(M zWnXK)HsdgIIXXKb4{97BGz7~S;Z%aOW{j0@n!j&DqunHpF|B(`Im5M?0$VP-YhR{& zKBT?6bMnr{Z9>g9A^|j<)Y0*CfUz12$06Ab_~;5HEZy~?@Ds0TTM$*iQQc3HI?YSG zUr2h`C3s63F7ysA(g;ac`MU%8q8+vEX?c|;i(Fl0tNha0|8IzX9gCw5ewl z_5I@7C>;fDd&~Tqv;M<6?U|7_Eb6nE>6m$l}#8Q?(MsJYHT_Vh-3e$*`t%P$CU2mi>zT!q|`5cC{lK==6uM-%kQLiw+Zes)YD^KSSu)0fCTrXHK#}F!Kad z>La#}XvrOU4N4emblK)C9j<$mFYrw)Qmhhz%n-r{Q`a*#%E6XH``@auFo8D{gF;6U zkCzK@=IDal?Cwu@r!xt0*hUPZW<#Rife~$kj5S@GN5vX+2~@F$o0rcn+8ljwRFw26 z^6LQ;j7)!F7PiNPN4JEUxq5w^g5xcIeuCb)qQ(dBs0TxRBYYex>~l$_yoW^J;5&OK zhrwL!wy<5;3Y+z1?`PF2#}Z-YATB|2CfvQv8Wit!1=xZQDg|8Rxey8-xevI6t5cQ2 zX(jcSW?S-s$9XN&2|XlQ$Q})0&TwFOnB&U4FGI@vW(x_=d;-Y;OJlJF+yHh&(K(8o zX_J&|>T&5?kvW*??g926dD`{5R@FcYmUZPqP>25Z@GX~}@276;FVPv!`@qNg&Zost ztn%XS^f1WGf(hYE&rlMMm5BL9i^xO*@T0b zw4p2B6)2dtw#=v0PxOr7U+x9M>ciU2$cCtC1YR2BFj%!+vHii46IbU1$iwnucfzAv zEiy8x3OY2x%rB=*ymz|p(z|tREOESDm|jP8s%yOqx_Wx-;+%#>)WES(>C-QZtLL~ zgK%m|Y(9AR3aF011KAadxo82s-ti61F`Le_Tagk}i6}d68$Kjw=GC9@BF-z==U#b( zj|@31)o!j)H2HZ>&o>`SCybS{2HKN^X=QmE9aY8f<)2G7c+yI(XzigK6mRx#EnRlI zqL-0PcH^_NJkt-vKut~)ua=Ev?YtA3-gs6M&$CQWUfe{S5Cz1HZ{u8MRr#IXX_T*h zL6#inuY@_1*jBIt9m-o<2MsekJ&8X8Qpv7@cN=8sdlZSNsO(G?VzQJ~5(oVxfrC@I zre_a#A{lXPSXt9SW-W{~7w#W^#4i+a)FAhlUlgsU{J)};h2BClG#zLp=C77F-`iO= z`?A3B%AibK6z&HDx1T`oWQ%4b>f2G{r4WtEwTFi*POk{pR>`4*$s1@3!s*_|gtb{! zn792h!P3P`kY2{8$S(oK&9s9y&YOeaX9wT7it8En;*7JA_SQUCnArB@4{#kqRU=vk zUcf-vB9%2ya3tg&RV})j$UtyT3nYqH?A$L35+!JqH{KV>+%nw>;uE6+dfg4jLV=U) zug?1R_Ih=m?W%%)-hTp`kiBd1%~bkX#Z$S^dB5R7%?_|ilPH| zuAWs7&m1`hlSi~C5DeB?Sr#0N$F}dd8M|QHc_Fo!55CsDiE*V}PxgU2a;wz&g?D4* zSg(${*iB9fx_qoKjNKr70z8E|u`V13V@VWf&LyYQShzviEvwgMHPxuPpWOTERShOT z=V}5{ij&I)WAQ_9#zM4SZMg!i$5b6vr8{}Jlr(sn`J}v@m*(>$7d`qP|J`Ph*{4q9 z*0IyILDlf#NyC!mfuuR+Zke{2K`ByH{3*3*q)uh*a*6{7Ba)*x0&Z=xmzsc%XII`P zikOMF7t`Iz-wlQ z8dM#(h*gcsUgC57ecr^gAKxAUk)zqQNdNPe2o| zS+m`1uU+#%EUDAqPt0X#ASR|shf}O_)P0iZ3sQ;6UW}!o$tK!uIw?K4q;nZQWSuK0 zH2&q?MrIaE^ClT6Ap76hG9^Ajt#%}=d``_9S3oSKXLYSj48qhI1#Sm5%P;aOvLI+% z4ZK3C>&R6@aQ){PalCZ_p?@u%>P>8}TGC22Lv#8Fa?Tt`_bLGCq_8d|M`k$f2#c&M z4m}!*D?@UCPMpEbp5}T<+~dP2E_rizg~ikHQE^wKTdKj`oXxAArZyZ_gG$~V9N<3s zEiPa~0WCJg&8*Ch--O~7y`{bh^CViXQ8lGX-}TaoIH!ID;hupKynz9OOu|LMtb>n%i!sbCWfHSWo#xO>~!MEA@Jv`WXT!uKvYyluA(eo7U7=aCXTZc=Cc z+YBDPMeioFh@wQfax(OobA^Div~T8SOXbL}kS{5)ngSR+Y7l&X@NHcwSINUY^-B6W z;B=B;R;U&BZlF7?in~46a#n~j!*9mp;9DQ=GJxCg+#-;dVHJDUWb{vXC*M}2qDNyM zB47ZMEl=Wri&H0Jn)4pdM>s)`(AjO%Syt*5ktW5;GEz^0OgC-gGBdqdyKh@r0)N+j zx4jt_Vjv_V$t2%TraAwtNoU`Ck+%3s<%ucd4(F1u$kGbcmv1MIgdz1c-K?bIfpz2V zDk|+JAy^8M%#j*qpkKrQc|qcUqHNbh_N!e+NP%@___$TnR^N@_u+|*H{k-(=4Q32W zFFZ?7xTw;&0b!5{4to0Mm?|DlteA=a1V}UE@Ch8b!#oopn5`wzd@JsS14g$*pgLVQ zcS*e3!ji_9_-XscAo1S-8@EUUPz>3N64Nv|cGL3{mMC2?*2ab`sZ!~p){eG<3C8eG zL)&cU@L?6%%JhaEw$ibYqmkIxsSDiU9Pvrb3!?L+QgE+})5IQ1CESaDn^6NZUDXTH z&F^&E!T{jjx^KVPw7y4=H5Ot&m2AP-U#q)N6)tu%8Pl7ozP(#x>jNs#1X`I3byYgz zSJ5rk8rMx%3Tszj94W2X;#Wt5L~s7NC#5CLsN-c}`3jWnq>FKsdQ0N6V5%#)0!fTh zE_(sOQYv%J_p23B51HoZB(f0yL0-`|ttD0<>EUAhv;U(hz{LB>Rw)yKva!<1TZ)W5BfRmD4xKId?nWn*|VpPdH=sK>vF3Izu6s^=oyCwu)16VCJ$pqYpd%MKu z5zvcW6Z)uUfbUEymJnc){yAc?jjFrYZvmuv~~*05q4bX4LI4yX2U zBewfVJ?elbc3jY_!;XTvDsG^vE{!~7R_B?(b{>fPU%q?v)*7F@(Aj##ZGQH-K2wBS zYi86QnIKry@-6#}G~d$#dbd$DVtVb>dWFk2ct{twMddCRcM>dGyp35td|EN8y+=R4 zQUz)%jsVALzR%-gySmNTqr;~;^RCWXt=x}82AH^YLV&M=Wn-VUB%*WRr=(7z`ygFo z1Mz$yNzZ}vaQZ^nDt2Q~R~Km&ZyaSz9zU5bh?-U|roB*!=V>p@@LqjeyF*6Ni&sNj zPl;~Wa+){otnIdQQ>M8!{w7;^IbV{kt0v_i(pr==yS_}_SU0D-r-dT()KNrRK&D>BThUpL+~jmzJ`IiPXf zwJpz9qh~Z<ImY90la+c7ECHX64)!xRKy{VhWAPfdtYF22; zl3nfGR{BN*5{eWwl)xBd9Cr1b=Kb$Z3B$xO3*Gy|QP27O%hz4VriZpXNv&rEoNIby ziMWmz@4x0pKrPvvL5fr}>$RzTGcz#>B;Fe-n@BxrmacqF2j7o73$$_B^xon&8XTlC zd}61UKGLU<1t~a*bply`q6VEfN#G(c7n&^EBIxHh0HtkcV_v5GGp{sdkalF~?FYLr z9g0240xaYguJ;(Ft}P~Q(Jw-;LD@E6j{~m^qiffQm`Sa=w)qS~jnBZPa!{;%5VI;w z=2mn@nn$Qcgo(0t=Rb>ImKzu*EaV(Fyv+iFB!#<(NHw8IBs{O@t7N-s^Di}M6=XZi$1DaS&eG17UvxU8y*v1l zenl+G6I9OClT^1k{S&sbz7nofnynnLML1tceUGR;9ZTN@RdDNP={|Rd<;s4M!2$JU ziw8|SmX#WROE3P_ay4_52k{!!CXb!e2sl);E=};BqOmj3-U4LVzB~i9ASadO#ux)?n{4(r}_UnrUB_D`0akt}HvyPr| z^YRM?c;u@^lJWgtF)Hi-ic#6v{@=3pe_nD%HhR|om7_8+u`{y$Ut0D5$4mYUqMSYT zI0|(H?AA8$4|r|=_SO~%ntmW-UOY?<$3v1vl>xV78PHEOpKh03@Z$uUs6OXAPJ~{U|?o+U?4V&IzXgsEIv48PjXUI8rAwJ>SyDeA1`2Zsx*KsU_cwSUnmeUHh-h2p_;)t zx~|Njn;^|OGy((U9`PgkV*MaMX=t)>vI1gt^~JFLS=R8t zHAdzoC&v4MkDx;GzVqdQxi&R?7s67pnYrSM)kR^8 z3%uLqq9b4Mv0@{l^iB5n_s(AH%*oB|IQJNWuB?&dAOHU8XVe z`|4ZGP4X$@L(SoW`FC^#e0qO(-}MGhj6>IfXbA$N5n$I5U+}H#O%Yu7z8-(;^ydH( z$EH0Wg4uumec7c;N`r57bZmNgAN`u8GEGZJR7p$F|L#2cp_P>6^Z@ZzZ}$MM((b|m zl9i>!>)T92=>3^4ga`YnKGduK6}ut}6wrIrleOqa()qIu8tsPwg}mqIRLXZyk@^!?-VhOhChhyC^QC()&`^~WjuFJ0JAu1g;1+Rmk2T-uCF zP>*y#W%%&?y}k1`*6Nm_~U} zerk)K%e0$4Mt2uceA>#V6Z9ja&`)P=bYXGQ zGXCJm@FXNqM+Zo^yxz@wh&wQE7M+p>`2A;N2Jn@2tn6GGVxLruFOU_v+7cgaY8*lz z?OWKn+`%bOUFDCEHjwMIUpRJO%@e|3pta_o{8k{>?w`ay7~mg5U!cxmJ`y1P)=#=G z1%}S$-gx9b_;>UksJd_T9*Vj*^d9lLFS`4?p7^xJ>&ecixX~!MPde}Nr|sT5k&^#J z>DRv~8{~UsRKNO}_+*^8lKmM#&!71hCN=*8Gv04M3ihn`&P4CUFaDCWaW(gq@5TE3 z8jwm00MY zr}8&7skhfmHhwqtJkJl)yDRcbeC1L5x@>Oz;EeMi8;xfCos|$*LGv(c)Gc$(*fyc? zddy|1qFP!$M{8jU+`dE9N!@73D-QDSsLAuVkSQOI{Z$&3)s&XeF<6=2wYhyVqk@Ep zx3~8BeL*e~My?7HE7Fq_F4QvuvxB7GBnsmfyG4kRHuqkHn&kSvz}AH^czF7(6k0|E zVZQ69`-DFka-b(?Zk{ILw}+TPbQ?lJ7&HWjnubk+8o#&}&F;;(7=C54(%cIH?m)}$ z6ajyk$pI8dxjZx&J%C@`=ZUm$NKY&xopS&{T7Fe)mo~hdFdBHZAEt=wiJCd?rWp>Y z-tk64hOm;2xa5kqCgG$ygl zCGD_j%!7BWmJQ8@?4`%=cQYGfCtCPTjr1XG@M_1$n5Lq1mRaK_%048I)h5ylp+~kN zrwzSZh6!G|Kh{BKCS-pGu+}V3i)~VzNIG}e3d~?P`Ll1?7r5`tt9hJ3}UC6bZlI1-s|!ofq2Z*=YIkGl}Hq(Wt7O;zvCS0ZiuH;A^JkelW|t2ql@ z_C@23S`1jdQ2JB1nl`=JG3u7Li7j{?sZr92Z<~wl_nXT#{VpX{>jU2}ZP%}}%HnMK z-;@F^rj30(0BqV_GC1?W_`)n9J6`IqX5?fpxkBL-xzJ9>mv(A6AZ2kT)oQ=RcDI6r z#`h06?jHGG90W=vR$f}@Fhb((qwl~Tu5;{eg!ptJm*CRcSQUzvcR5bMq7Z9P{)*Oj zp~fzI-tVzL7CW8yLFPIeTI!T(w~>lNAQA7klr0Mh(hBIwHdK|7H>i2RzH2CXm`66B ztJ$5@NEb3}>X~Wey~^FezyTnCv^Ezzy%bm06tHrbQwQ)9Rark(Y^Vy)IAW*Oh>EEE ztQp0II&iXU*b87t>vDC^*-ny;EBd?x1YYQyDuA8vUoJ#Tzlsb>jE;2r9!;cHx_Bd5 z^irArkl$r#lBxan@3O`6l)4i9xriB4Tm<)7I5ewL+lhFK+-g923_JMi%2D5_3^;96 zy$Y~DCR=Ac{SxTcHc}H385nsh`NMRP$S_Y1W)bVXV=kWT~GxCGBQ| z7TyG-?z`Z#!l!+Bzucb8<|dS}loT3v@6|m?QU9Z=+U*HR83r9D9)=`T=}zG>o{%zT zXM2LLO%3x9=>o&Yg8%M=eW9j@tEBI~kX*0x!3gFjC$WPDbt!)BoqP*vd-<+^OEfDV zPk{wK>f#phR)49o9T~il?4wAb#*x?I&w36Iy$&v=)J5m6ZjC#u5CmQUKyHw8G)ZC zQkVN(SpDd~&FnHGsfP6RuQ>-+%0J)PBf^_OJVRp@Mo{OfS?RHS4%!*O!59RGO!sZR z-jnAbQ%AF67G$i}4u6rXO6Sji9cYzHQ)RNE3tBfn(phCW7-4*Yoc-#B?Az28Ih)xj z!-#%?4p-{g0{2BfiHy5L*02WlYqDKur}D{*FNNI4qR7~i`;)}a6y6!-M$JLlq1Tny z4dHP9UnxxS{b=5y|B8a;_IAgm8OkMayaoYH8hoWu-yK{DlQ;o4iTt9ujgsSJ#Uv-KPe6WXcmtd zX5pb>=^I>7h@UO4{AUJTT4atylJAmX_RnSKLKY(Lk?@}|qrhmZreI~4!1Wrry+2)-k?8ylAokdPMB2beE!%o%yM}f`LQ9L z_lJHqnHvhQGWly2lS~R8`geD=hK(>Cx;FmQ4Yk`O>aiFK?z5;lgye)pSR4c<)w#PtG{&q}x;zX`+Rm_go zcJK26`%NL#F9X3hBq5zT1z{7*Rt-rcpNhEZFM;dMKx4MIfiB&e{()@&`KIkr>a_>a z8^kxRdUuBljzOyQ9!BZ`){D!rIi2uzd!b9M=cR{}1@^e$`Fsy(b|wsE7NvDTHb|x! zy=>oj!&de>^eqoN?vv^NOs+=AfbFSrir25D4;0Z=pLI@Kj}v@D4AB1LVi zqln`^F3Ba1V;fGmkg~}0DmLH3wxj}rK)MQYLS{nWYWq3)tDJTTl@wnfZ}mR)AtO>V z5~M+g%sR8Fwn-10*M$I!)7eQsLJHaT2UlL;zKsBI6Vu<%qhT7h2fAu}BQnsj!1gL% z)`OG*AnRgOfj=O6Hn>xl*!Mr;Mz}%C((K`jo;XOh7@$9qk)SSpAU(Ny7wxp2_%d2EB<^75g3hmuv~P9Xb= zO0&}cct$~39_*A4`mi1(Zf;;BG;A_UH{~65z_DAn#QUL-pP$~ZsoIvzh@5r8AN~AW zGx+hf-l~!Hxr`h=ZyCF?X9M{_e{=htfX@!bKyhjja)bE*{ri+*?ev%!yAt`6jk8O% z+SDl>(wv7$`U9PSGJ0 z*xHVD8Joz$IfanTpkb`BH+|D{yaV^Yvd$zM#nVw^15J7P8p`XwYr>-Y`{ z^ekNv``f>R>FL;lb=)?%kIlNoyu9Ig&WdUpE&8C0>u!*p%3sW*f@w|u9AnaM3Dwf+ z5zq7;*Fh}~a5MU_y$&6GoxApNd3I1i{C+c2VBsNmkp!NSmsOJSX~TKsK0){lMf=NMePqa&0LNKw%PDpVLqn#6_+C)c!2jo?QF0vzOuVWZH=ObFQv6B zZ)Y3+DoUPeNu57n2g931#^9QdA@cN=Hnu;+zeTwVUKB&No>E!39&gu}i?|v^Kx3G# zHw?%)>ytlWekyXumnY4)!f%{APo06<>jJQAvTT`mg?o*ZxWo4ye(ki+Ys13nU@Y2W z*1mtS*r?9TINn=cj+GL?47aL3Oww!>GE%c{;LYbj_jD!)U*l~Ppb^{Er1?|)jUA8ab~QsexD+ilzD zI)q_%&yHJ5be~x-{KXL}D zKPo@>4{JB$%WGu156deHHu8VSX8kx=Oh+?-GDnt=(I_Mxqhv3X`P`;A*`;fjMy&#;_(1;Jmu^X%EME6!lOoUUY98g6ylC+hrt z)#DbU81k;25%6H-8N9uOhg9A>b`>gy(WDYQ+-w>sWK_Xe`vy5zQdXMqzlSc-k8)OhfZiqq3qnymy^~`Zj{XAuiK7gaC7otC1s~ z*;+0mz^GKMx#Vxv->HpEGEFo7M|4VBlVv5r{VYpj7|TYzmQ!GR0S;Ah{Ec<| z63!=3$$|ZQ4GPcuLf*T#N!!Zt4A}K-{Za{!YNmUmfm+cbXS^O+CRwcvtPfpiY`Vf~ z`UU}3R(}oVZuLm+?6_1{LyRl~EPcOVG1#V`_I`v3W=wkaz|rD`IIb1;C)tucLdA;Z zSFK)OWoBnTJ0PUK9W?@x_xEnqsp1JH^L5~@Hic_2Q_TME&?)>SFXhkUSB3am<@&?i z#b}VLlQ3=JDe(|$-g}M7t|irTsKGXp#Ww%4fZju(YdJ5~tA{ zO~r{4VRZS=$u{!6!UvDIkkH%pKEuvx3zLNZ!yoCIac4qR!2`q781LQM4eyA|`s=s$UWo~BH#dl0WbKP5O2iAJ+3xIxzqe&w%$EX7 z+@=_;d_h`C<@QPVQ%QWPcv%geqT0abZX}!m?^!)K+3h)y-VA0tZj8Z^z_P+n z!?j#u^3-D>KBata=f|#2=Sbt)8+qK|#bD;gOshLJ`c3{d-TvVJpp1iq8lO=V8*FXp(N2SeuM_th6GIxi zXY)Trk>{kfN#SEXvQmS>?RT&?0-UB)xG>b$B{>s7Uc#QHjNF5IQRi`kmh>7#09c5ad1>>m9R&e>6^>FRHvY zVIo$+^ui_mwW~RubOnM7&`eeb4WP zIT)xG<*K5%SGIr}^jeoYIYm_)ZZ%jD?jwKdg-YUR}f10<;tF_B)B2sGI2?i zM>j!JT4`If_VMA+VNH|OmSv-U)pVm!F(;;vbuJt1x9gt;KZR}BU>3tL&&_s_ye{BxCu?#0BT?SUZmu_RMF1>MByo&eV5&BAYu8`B#Lm6H(2NS1q#SjJQ zpoFY^e9d`EWcUaV&#cy5hoVsg7qVDt1_?fV(>8#l4mPog9H*w1ljxx>a#kmY{i0@v z;EQ$H7cTW{#HH8fEMJ7}SEI%Q3Gc~BqY_Gr`H*b5CMFpMYEY|Nas%3{Go}^0JEke= z{>~vkjQ6||4@4X+p=|x)Cj1RwK?M_niAAw?D!EvAC!&HRSuH}F4XkVsKHxRNwxzU- zbTkZ~Z$q2b3`(S zquv8XFZ&z88>b43C{D_Jy|7_FKePO>-s~20ZZ+5}=UJt$1?p+MIa@ew+y~!!0Ne0r zEOCF*PEdm;Wl(U5wjHRFOYlLIrESi>$HSatnXLVN6Z-+8K5W_js7mnZj)Ic8e6HoH z(TVIB9P=FYYNvVKNRg&m0)t$!EK`I;GQ)whp#|z=^&{$of9!6m@0g!WNn5(2 zOm|x|we0ne53ETH-0KH%A@n5-Us#e1MY)CA+cUq`DU46NGal6iFXdDUfUrnx@sm}xLKJ=q?vD95?Kk4#yTegrUpb8R^OleIlR0? zFzYFuwu!nKv-j(Nb7lyw(OqM;2#~p<_ee7x8kk94zUKs*(R`k|6ZW$Iycf3*yO~9r z1^2rt5SDIv@^!LAAl``3u|&;ByUhSiF^h}`&Et|a(l+JnwjDaXmzvgqO@+aUy*?vSiWEOoG7%d6=E}YVh`uMtl#Jf8jUmb3Z4Q zx||-;E?@~s*!XH>@-}i|?!MP3MuaPDf&u%3xuAcbLz5Rh><^l(%IBD_M2=Nq=>&n{ zmSH{v*z!Rb^r>rt)#~tHvd1M1*@&?cda5B*s5vcD94f+i+V&nY>pKiTMJ1dr>~7f6 z3?Gsrlfbbk$A+rMwNv#psw2M-)7V!H7DB3cm%Bibb~!7h5kG|BFrwa6jG zS&rYWx6a4vz?-g!46WtV*W=`%4BJaahRWFhI#_41y*9Imr`rm=Cuwp&d3d zwQ%Cd$)w_rioHEq2t_y<10`^(z!Ef3pvOy!UV3zdRh_vr89iQ}Ts1+Y8SL$2&>3XV zbRlWutTBiWb30ciLbF-!n6D*&$*Bo;ABHG|`PXETvl%k#ChD z+Vtj-UQ(-hg<|fp6jS>i)mh!mu#rH?^c+%nNIRtpP#?Eg`M*)sZ!X(Fqw9PVIYz6S z!D_5XY4^nCKqRBRg^4?o0QZKVviI9#3&W)Ta>*8nE^fm|?Wnn`OSw;uD z-h?b>i<%mStqWZ}`O=x;iX5h{LUV4omu!?GoA+<>hvshx$(ufjQ@6sl;aK6iUPm*L zuN6x})q{_r@0_kRzM9hqDp@&o`ERXP-0Rs4M@6M+WQZs(E7X}aRgVQ;UdD@v?6F%& zD+m>98Ti-^Nl|tVmlz7aVMlm(I?`j#JR(|v$^*^TYy-I&sY}*HWLz<;2BZa-8t47r zWT42FZLMwAKeLTSt_M8OX7JP6o_GcxuE}m)WG>j55x~UpRdC#hxMzCfet}!E8 zOxxr5Fk5yJXX{Gyetnd14H(MjehG~VUGGP7YkNsI3}%bL2T->;JuKUDP^wz(_8qo9 z?u~@R&r1d?q1G?WR=aboj#cvKhk2eg!T;`Ss#H`8&5$!&dzut7#-`UdNN5x}e$iJJ zY+yXZwVu$Bi;HE!ZU&L-XQjY}w9ie2v{}}`nQT3(A&gZn<>o7SZE|d+sI478F&*vo zoHY0nwcr{`Rn|k+$|P4iYI>@e(8n1&n9fp-4MA&TOOt!*e9JG;EO^q{{N~*wxHLi@Qx${R;W!?{273fnIaVt9tQ1I+@ zu}Nvt2*Pa}Ny%J3c8)9&*Q@ct_0yzK!xfM#KLx5)OF>><%ANa|%3R2m-Qt`_UK{TDXPT(u$d2l9!mV*~s5A~FfBS59wBrnfYDQOK={T{|8 zYzZ_60T$hgYWwXw90V;KcvojJx``UXwrC$q)MfVpqBvAaAv=a4bb7O(UX_=duGQKR z%6uU5$ATRCD>0-P%fgZqwEjorpQjJ3lx%9LvvzKxSQyrOWJMqkkmlkkg(Vg_&h2R0 z!I-}~JeGXty~g+a$%<#v;G;`9?6~K@`nZ@@W@%88m5J(4sbKRM?>84ApM~?OT8s>M znuXNX`@XGEuOslJ&{0?^Q{ zoQ+WHWWQ(202JRbk)9|}+X5@#U$_h=HH7~l4@XxY0| zeW4SxRv$khadte8^a2I;FSQ0MtDolyZZsb=2jI8a8hYBQiZG_0Y=g%dxNx3-Q$4oS44*Nu zP@fHc4ld?p*l1h_4%x<@mN#2Q=*+5e4gXG<-U6E>{>Z@`7d9}ro%NX2e9q&f?bahy z^tS#6x%OXw!8cf{ZdY$HKN^kg-IK4#3`;V1qa{K=Oo_ceByC58LibjHH$U2WeP8L9 z{c@Iu*~xRUS{DzbLz*T2D31~0_T}rst+Oy7fpz1dfa!3jN3LH9LfBmW^3V{BwieL; z_=;uT7)<~uQaJb$rJ{Gq9|jdUL!{BoT77hBijO6+4%F|pCU$3S#RQW%zb~J8x87(+ zKfO`-PdLe!(+iu{+UR@UxfRPJAG~+)i+&D(M#1B8MJq2Aik_I3Ki}To{AqOyaL;<` zk=}|^&z`X17LE5W0XmK!(_GkEj_SBRa-R166a@-vkPo#N+yNj?=>Jq+37l@;_o?X# zbD28t1*P|uF|vQY@M^CHY*-^33wp{tlX1Dlw6@f9jeC}Fpx@RAp8CyVH#*TGsA<;$ z@+Y%Ur=H|46D^RVE?f3XVeG>yBgKd}v=?tVC{(DQhg74^t6nv}?F;dcid>DlpwZ=; zsJY}z>@KE;hcTE(Lh58P#eZGZLkx{;8H<@qX+0ItlFsB{7_yJC5${O~{CRjA_pC?L z7drI9EM95bF=6t_sI?cP5_t5l`TKG<(YbNTz@#Fbz8i(pl?IDfW8L|0X2jw&^2VGL ze}e1f+oW(k`{995NbtzYZjoS2rogpoAE+M0z@I`E{Cs&9|GG4>^4-16^4;Fa0mm2$ zeI*Oc!t;iM{rG-V^3w8}3!*hM`{d_-Dwz1(3Y4cS*&;|PeD26;^8OkT^I;{g@KNFqj+QvPXHF)GTblWAMV>Yc zfzq(#zw7a&|4al;ag6a8l@=}Ln9W;%q@*&Y4N_Y5r}mzqY7kSkR%)>6BPhVAm-Rfq z(STnn#V zI5y-yBiCKa&##Wg*FZ$_8a(Y_70A3ub`b@w_pLl;loQcdHvUNk@uf-9+@kuz;krV_u z(`9A95j%8j`QQ6!J1PbHvRE*()V!Yv4e|}gB*Wm*PlD*otAWX1k+qq%Lo~A%yqTn) zjY!^1P#b0kiFK|3I}QYeb~myfq;N6azo1DUbwS{XD^*^p7buT?odtdZuI?h+7xw=o zJqzgXxH?Pt{B&#niCChQW0#pxf_iu+87_^uh1xgs`l)uw(oM1#>G6@;)a@oYVUy zTZdF6i$U|%zeLh%uh2+qT`2JNOa+QhEJoZizj6&o z!0g`~INT}xpjGPcI~hJ|9KI*v=q)9v9GHrrmP0|G7;@eRZ^jLiYnos^KWayX{M06L zX&zd!1?5W#T#Kr8)4bsceYr`VzhE2Bt*AhPKUhLVf;~d%amOokZ(T2!Nq1I1ZZyN^ znYeLoboRa>wf0)ui(^}wEZZBP+dhfTMH4@qYDaFv`i{nJD2u1XxFf}gP!tly_H%C= z=%+ERa*|pgS}Lx+i*>MIe?ebANoz`0Em=;TD=jW9QEAV#rxHy>N{R}%nno*nsTl6H zUW%i=YVd)ANV!|On|(+0IZTKPm+{LU=t^sel&4qL0WNRDN;oi1Ou?N}7{G~z#X@O& z&n1HmON{@42sKVmtex3{pQtr{rFd4hqr4tIlqCJ;AKtbH>4|VU^c4?Og~xA&Uy`jl z^_}8HEMQB&KD%m|z^wK#Qnv1x(_YF%8pwW@xM)w+Eg^6KA9cHa?xVD?w_HuJY)Q}s zQ}@)cRV!Rjd4{scA$j>>@lb>vU;c((DDCWVjlzjZeuaW_OnMX523+iR7woN9jO0#C zm1%&Jb$bMUkw&J4`rEzf*3F`@KYR+r0??(d0T=)j6V=unA55vpKJ2T#q0Yb;9ik++_YL&0iVfXJpKO zIbWp(Y>r+|ALfaxg{`cp3O2}4D2&uHF?l9j|+r3?j)gX=x zEZLEeB)1-tshLZmjCW@8Bf3j^NA9=t(!)Ho6gTStLgD%`2;)OIUzNWElFQIfqEFms z!9#bhTrg0K>J@}87?FqWBX+mt-@GcP)e?w>Mb1n)N*GlVe`Wg;E>x*Ao8+Q884PKD z8IUayb%#^F)b||E9A^|gP8oGusf79VU8z03NS-|yt%-(V(4y&HuZ)rBcOyixZ#05b zPXBJ(l{I(<_3rff9a(rqY2tfr_-|od1ASntY%mn3~}&mZON2w!_-Jg<2)j>{SE-0UrJsWr+Sb8JV7 z8OO!JGRUC(tFJ|odFmB;Bl^Ft{BCXfe$bk+-TzaixkR!Kx1YFh-v=}=>(J8|_;hP^ zOJi}4CFD1f3RTvUWvtZ`QEHP<$|mZ#;>I%8I{K9T4nH1mx+P9Faqf?z@V0^VtHg=^F}{;0>!(IK4~cRwFlAGFnIqg;J-}C_+^1}%QR~XV-Hn1W_ON@h`Td-J z;!q-DR_V80m*;8}APjnS z0n9mE$!77o*UR-}jMuV=4N*##Dx;mT!wbUVivlg=-}@xxim64 zJvO)P9q(P!$&X;qgmtjX6)m>hlYV{=peg$ol4vuC~mRxFCMzk z8AyF1@dEgw6uBsyAl%2n$x50!l0`u8s-dS?%Dpy%kE60j=BzGO{Wq!j#G#Gal*hAE zgz*ntUq-4O2*+_Y@U5rD%Z%{`IofeV;R)sL_4eVka6l})NP)jo_6-3P>Zhq&EZ%p6 z@=Zu{!*Z-Z7U|Ve*@f42izBu}k0=he{uWgl_KW3R0PVD)qPiW^!)iL4`7JsqvH_iV z=TaodU=O6T(jf)Jss^V^qf!aPE|+&}bAI1RWe;nAKyd@j$hL3LDc(qqRS5wtJh5;g z+E(Fjw4eI*rK}U%z z6GYZsQiB@(U|l)Kb6EPY4zLZ$@>d_c9~<>+jQcu1!DwH#FM<;|>AOY~R=gb8pl5dU z=kaDx{DyxtN8$7}V1uY3M8hwr71!mj-a}i>NYWQ~PpZD&Tc*wAX+gs>Mi;zE0H==T zx(ZAUSZ^*N!<9K3Zz9)}%7jA^gePiv4DDUqU5h~SptP~6OmlMD&CXIsrqAmekN!_+ z#R*ig*BQ;nT@|YjN)|;$dF_j7KDWyHLL3k z$4A~okA|;E@(&bkw-v%x;`NeVz)Y|RqJQlUuCL7H~aHBn3uIyzwUc+OVSigup= zAj%@!V{O5F&0{W1TuA9DfR>nrcJ6+NgJlT`K=t~aMOtQ~wF-C+>uJhtoMSSZTy*5O z@TFOq_NGLluQLSCp=PN{aT=t#qZ*fuuPCiNx0qkXZ#xfbK1$Fx%PCH`Tr_?c`9>2y zd-BNQ1ml1csWqurt`(Jdj~u^$t8tT&)RS-WkWZSU_ix69vbt&y4=NwWJZK5}plCea zi%to5B#znDP)C}JS`dbE0I9Xe*^H;-QDbtfD)*oTn0BY;U!5WnmpY6k>S^&}j|6%M z&l73>zD8X7+d^HxoAes{BVZ?(a`zyz8yH+jg{=Kzx!e81c7hDjvp|j|aOPwR)Kp*; z6F2k4X3zvSuev=zhkBXUKNd$@Xpf{d{Tyo_I>YtMp#$~{UNrqRAdh$Mw}x4EFYcnuYsAyvL7F=~^P6$g$|xe5iJ(Nzo}S3&DZP%aVAaw%H})a}lrKUl>J0+(p!PKVN1||f zX|eqN*Qd>?(8wXZYqi=d_CWb{)f0ura1`ynUTP2f2}DYZ412Lw`LQ4Weh(&>X7{*@k|(zqBb=KRk@}FH(cN{T_Fo=`T2DZ-ihTwH6Wsf z_y}~0{|`Si?@+6Yul()a*NkAm?+C;C`^z+erFX5JfsZX!rA2{- zs*nNQs&%T~$8?NuDSf=#^5EO&ByxIp24_=BgUxCwx;`_rtY0d>fQiZGKIH>(PMz;< zfK7;sivy~-;WBoaF&&Y(m&4D3Ywa@iZ-?||3oASXNB8^_cqdR!>^cP*Q~>Fabm4!|Q^X@Ks0*N4bDC$vYT( z2ZhV!g6fFT>eKDl>q>gqT~4Fgt0CVaW8|}$+%B{`2QEy;BC*{@UcC2i|3NTIl1WpZQ+u(W%O_k#e(1r+V-y6?Z>j$#>WH?i*;Z+0L)DCF$w3)$!8R zdFGm=fHgZz~%*#3s+tQ2JiGmR^+Cz z@C&@oo|Off(vsoXNQ#do+Ac#H9E|Ko_Zq^l>UP#CBZ4k7kI3@36j7>uT&P`_!xcm8 z^aVVf^(O^Rl~X6f`!;=J+)znedf7Q;ITQ!oxN113Pmd^b*OdE3hk%7C)zf5Nnzv>U z=Ag}QV*`vmH8@prsM4tp?o}BI8MusN0NXODXZ76&zbat9JBS0cjv71JH_#a5WkSUe zGXV@4F1>wk8OOR9hZ^SbS)%kW>6szGXey^7FsBy7@%1lJl7*;5OfGq^S4-#z&$P_f zehvuW!MME#AuNILbN_ys;geh_KXW~I)wwL?a33S>2LUG;0{Q=9&)EM@_Kb&BOa%Ws?n%JFz{bS`? zY_Wj3L6h(kv~~Xbkh8#`(H;|sU$VS@YCUURRc?7oX4^-!Rb)OSRkBqUa14JX2CWHc zaICWj11<2w!O6k{K|-=&Lqekep{a(zu!MQnik19;KRSX9U4!|W6x@LRcdAMqPv_5t z3qbVp4~(MmPr~jSCm$N88|nw%JJ>_~+(bMe0gGpF00)5jTLXcI^NeApsA3)L?Sr*6 z1@fN0eIWPgk3j9Apd6XK4dN15Kstf|GS)!-$>gcxx1B_pkhT0dhhSj?JwMf;AbwL( zED3~XZ~r4|r-x=T;0}x_N5(+xL4~S;ED6lwlc>gU?mA8U8zOMeYFF0eEr799^-uXw zFA!nM01?Pgchq&@fPky)TpjJJX#rXP(<>ks4^sa~(6MhhqhdZdbs#@D$iDHBw_JNa zj~{rjmY>1(p($9yQ+>l*;FeY}b$=THAXE-g)-tYSaD79&@6EZP70ml9y8}y*78V8{ z*4IL3WdE!qE&p++Fu$i$>k}|nf6hjZARE7zv2U)ahjf$c>0&}xR|2mxri)z@KjTya3jegAAR&JM z43Hz3|JHQXnjiVxojuT3{?R9m?@j;cB=7-Hox3gQ5oik#Pp^D;mW;LlP+1bp*y!bM zb8`n-LzjW#@^ zG67CE2j4Oh{mS@fJ0Es2|GfsmNpD(oR2qW*!IAMnz{KAv5?WndfAoLXlFXp39|8vd z>}fm@A6H;*w^PtMfA;)uUPXa_5ZSohe6Bc9pY1yW5?CGJ3-T}ToY0(7Lz9l?!MNLBX`o_9o>88`ZI3uK{?AtCae0}cFFd1s9rn99>`>?RAA@wYjwp&M{G}hIC?s}?8Te5nA z_Z_EYm4NSV@?Fc@%*PIetc_zSri(#fo*nm#ctb}TPDr$hkm2e_|NMuJKb$wQbti~T zYGD*m$wgWkn=Mc6cBNNTs+i!Z>&{K;17*QBx=g*Oh3Ez3(d?Wlz&De$KrpZ&u~&b9 z$7M9Ly(@EM8pp3dIowF}i6i#l`=YqBAN9*>#i?01XtF&hJybm{9OoO!%{XyJ%92kb zz<#HafX-3J`I`xn7<=knoUZuA{-g3%&{OihEf}XoHY%`I%-lP(avij(b6$bbbtwin zy#gne{xl+;W^3z6*H6J>?2=&%*FDY=_hiT#-Z@4<_4#gNTU)qoiK9cA`(2l%=9Htz ztUe^LBcxZ6uKCZy-OfBDLb$MNJ=NTPwpF&SxYGHu4Ug{?{dLb}&CZ4z^~WGMGAaif zB^=C$$;~LI??!*yI}bO}Ahdv>&E(9l2az@&AoS!W#gZljp95;)2&_x9G9 zC66`o=);kRjKd{m_4Gl{$(`BB3>+Gz>aT5iDGF*pnuIj#B<^4=$nLF< z8y2d`ZpNX!7NgoWUc}066mAId)S0iR-NCASZh;6|jduk4cc@yJkA;?SYXzMfAKQnI z9SAjW`PG?P_dY{M6@^C=ZO+&FB~|p8o%p!Auqr$&+LOVAXEA0InBq&2(CM+G&OxfB zMP$e_1iq+|$Wo~aC^lzKouOn;q%NAxc}%Yj#p@C$je;`VzWDp=%;7<3jmsHnJ=;Z-O zd3goflew;ulGg@#r#UD=I{2WB7NU_V7{_G!7entKYt@7H^isnz>fFuTJzISCq6?dma^sjw-e~$zpN@(c)V6`_$cK?9gd#`jECeaNFIn)MSn$u z(8xKz_2fuGZ(pGZoN%G`AdRuPeGdBy^C7wBlX^b!5q4)jm&SLh$fcdH>W#}Ht`A9$ z5=@H@(jFtifP65NWzB#LXHcA3jqC{iPpuI)2XcPHVSv(8r5*`AS??>dBa+*x@;GKi z1bL&cPJz2rZ{V)e`p|=YV$zlW zUm>1N(T($2e<+O?e%$wxfPzb*9eNHXJ_WB!)|%=SjUo=Ft}k^q*~hXEK!T60-=eff z&Ntx{+=g4>KC|RMD6o&ZAqQCWauuX zR%zBhF!9`obtCJt)2oI{KJMapsL^vDrdzE&&Sk=6)NnNtceU$*m685DgL|?mB&S^0 zx_Ig45)*NCL@O@6sLcQ)?{Cm@(%izEPOb0nuf-GY`eb-&CONSE4`b)lD@wGW!EM{N zZQHhO+qP}vY}>YN+c?`c=VX$Z%)@<{hyDkhUY&fks2Nkx{0io|k8#)c19-21r5`@HHVB0#UmfwV?`S_UOM{X77+jxRT1Y@ zE<0O5BNr~3rkv>-m3~T`Ql@rclPK0*(o1%1zE}$nl7JuQbqiZj;dPOVv4-V?e6mDT zFCjWT5F5wc4F!tjm8ith4o;0w2+n|a2!}d3Qt9x52h%l^{@eH%tHwiTs~h6pf|?X$ zIRJm}H$djDdRo{@6z;8lwc9}ky$PX4APyR*wrpz1HxaLg_oYFyLSKbKN`r&owpg9Q zPVq}dDNa$AF$~_HG*~lwAG;?bvYzGx8B4)e`55|(QhO?=L+!(0a*2n+Bdw;7@Q!5%PAXBudQ=#-N45M3We zRP%-!sRfUE66?2i?)PeSs$IHU*kAkH2DON+cGkKLXnT8q!L!)(pc1S{WnBsA5h}q& zTWhd^O>g^55Gz=5USP(5(Avm401C5xE;vwxYYlmMu$W~PV|nBbiXgbi7k-c^8l3s5 z1Pf`e32rfu1+l8Xj+cD>l$3qAqlr`jK0MDy0;+A5EnylP1H1VP+O3|ZAMdZH&AWhZ z=M@}hwbvGDGK$)&cOzH98y1p2@-C%g0ql=l#^?#24Im}E&?877x5eR^Yl4YWy9>AY z_>}>P6$*I|2veiYSh_ulQ#7RAv>Vn9al@$wdFtHrC~yJjFH*DEtL`pvSlx%l*$4f2R?10z(Pt4%(FX=0Oz?C@^^fs4BqMK=QR zBQ**G&%2?K)(g8YG6R!4*Y-(h{FvVyow^EaK|0F@iA6mRzxNbKinq{>e71xD?Fig-a+1|hq#U7%gweuoz9WT|c})6{(&`|1+d zUF$9KgVu%37Yq=tg?PYxG-A?Ozq18aHkbc+0W8RMM5dj{Dr8~ABQjeCZ|BiVcU1(1 zihbN#3}H8Xs|Nz*%6>adHnE*$%&RKcC~;01u<0R#?U36N||&<29K` z-Aqv_P$kZ5tI%jGYfeh3p}%HYBd`vXFeA@_4s5zrBEpEaQ%d%Cvm-#K%sKDo#+E}b z=e*g!@)2MvQ&Tu3LGXvH#zgR!T*HZS?D7L?I0_C?y`1!7;JRC8@-Kd(bJ7w<|3BQ5p z%wO+&=4q2mhU8A-m)eG1Fe(VXTp0))uF*d-yOC_g!<1TQ@b#&O#d zcke}vY%w4Z&N+Dxgpod_@;lKM**xT2B55*1Ea0r}B}Bv`N=}}_qCEP?(qm`#MEMM3 zFE@S?uz@B!%(8}V)~4uI9EbSk&RyTi55b&pCE=kQS79Up`B+VM886Sy$jAsqNC;J_ zu!ZY<^B%jW#Rbz36ko1eQ3x|--IssVzIHLUi5umr92LeNK8BFr>oA|J(-&Ziw=w8EiTX!hWy84i2TK`gLd9MZc?V~- zdg&h7tfrZ>t}67L9-0in8s-tFL1YwD&hKN#?V;GHT)CwI4k2H6MF7ZBSzFr_;kQ^N$c*={25bT>!0fakr(UMpGpYg_39e0M9r zh{pMnrYML4I~*YSR(uI3C}e z?COf>i=Pg9DlLr~_XWeG`<_l!{;aaxM<4!AFOG${-5h@pCRyFY-bU4##mx{^$?t`YpD66r-_Vq}Fj6-vphYC568J!|)-cHn4GjYg zvwMylP`{5DQ!~BC>7xp_JVa)eMCR;hMJKo=>z@5nlPwyHULxw+_I~Bm^J@pgsE_W= zI8Nk~yqX91!BC8)^wr>7===R`E!`Ne$Aidmj@8KaKCG54*XY5f+q%yl-mY@DhW7Q3 zATKfc_`;aG5>IKLyOJW;1;MT^SN7=-CZU zLNE76Zq!I4bEUP^y_Z46gVgHeu7EGhTS=e0dT!hR6 zkjm^Z5iB0ZXfMC0l#=*MK91P?&7nPlPebqRSIj5vX)Hnl`yc5&nOAJVj@CEM;sOu> zq&(=0aY-v8kIzt4n94s)DB&4Ad?F0>8=y9=mpmV`1ADM5GHYG6$nc3`UIO+>KsbEx zT(Cmfg`HvlE}>dnB9k4wq{Td@Y%L#jEDGruC zR;y$1E6-eidl~oHP};|FteDr}+WQ}DFnRXkD>_7OIy`-x9(+di(dGEI6GisyN-=dN zQs4+Bwy+o84{XyVTD5OGV4cTxJC^RE^oqCL6M^fv<9-d7Ch-vhW?GWyr#73byuIA4 zH=G~Dgi?%|&uSf5ZCW2fQDhn1&w6!AH^kG$f#AU5k}N<|c{TOC_U?&(1F@trxia#{;}UBfe7f^7Oae6iJXPcJ-M z(|565prEvQjv^W9&hPZiM;nx z{X!~MuLcaQ6YV0x0TTl?uoB)jhVK#s{St#3Q#{E|QT3hA`^vwqNlC)=q8rc+#rT|X zp#`7lmQLdD?sXK1Yz%`>P4ks@!1l>O8-R#q+=|}{zv(JpZOS%Ln)%|8Jtah@Cc5Ka zP%Vc7sLw6DCnrxxo-Bzd@`)N!LA-qIQH;0-Gz6G>&-5o z$w%Uk$)=olIeU}f%OYun2) z8nYK{;9urB()>~@Iu&nTv}8`Y=L5Y@4ykkD7l1z$;y5edKTTweA+mwS&bYD?+!q6> z-Nuq@)qG$0K>E{9h%}9ErOb$IxHqt@E)VS@_)B?lPR9L(Fd9RYnxA<>fnIz8 z@hstOM$Qn)V5}S753RMPc$MhUucwJiUMP2g3OQtRl5O^b$!ZK{`k={E@XZS=pmB~p zd?SXlev6cf`wau`c046zjz)6fem)+3Q*#*8?RMhm1;wlnZeYgRk^F%T&6^LCeS8Ei zKjzyOI=HCX8sbyQpad}5Y~O*F(5O{G2ple`6vOgrewMOE>3LA`dQX*UeDDRh@m&kJ zb>S)^bC7pO+Khwo@S4NoFAY^;mb0?KLYZNd0(EwR8zH-1-I7f0h=CKT6}IK$NodcC zEoqNTQ10FG=x+VBbp!2<$3w-RRXk6(Xpv>j@yeFG-|VP%y3r;WKVydCJ3}ip(AV~V zTZyfkJEe)D48oZajiXKPC?**oB?}ZYbjWucR)`tX;MyTvj40c0#Q^*2OWC7h-fAC(6k2A`Rp^k-MJambkOfSMcGrpqlF0&uCAfdOlN}@&#}yg!^=}DQQ;Z>ypqM)GJPPUi}yI zb?3(8e6kMK^W)fA!%)RA!OP=Y>Pan!D^^&MnxbsR3S_w^DGnfwR64lJL`zQ{7Aqza zRcv?#Z{H-|oPg041UYBma+o+b%(d_a8^5BHg=yuu0Zax}7Zg!S`=rulMW;x1i{qY(Le^f@GGm(Z4 zxg-3Zn5K;%kIuFIu~YV;Lh_Ztb(;Bpj0-ERK(Jk`HK+OBqF@3wbM$lSS2on4F@gBU zae{DYuu>opjX7Ct8_yHcTQA~-4=&Lbhw=wgr8i`JkFodny4ycmXPge3^|B1FVtQ>p z`Rx62^N|}fGm2%$)$iLKB~SX%6bumK%CGe#pfVz*BX1EXQ99*YPR=0~e;P43g4;;> zRJUc1RSM)KPBFYTL@uo#=)x~Ay39OXAH?iELwjoSSg7?(BaD-_79{ce23rAQlqI3hn8YS7QHeMzUF(y*7747c{aHt$EevSV-OH;+^~ zk}!rfsj_>Y?foIEmSUT7R`7!=DGz1O$*8w~No%%URCsQ3<2Y%iq0sdxYZY^M5bO#} zF&Soe09_=QLd0AXC6P$2JYCYNqFcA~-20S(e1qy2M1#r-A9fol*}NuVZGbs$?b~v0 zW4IqckOK1(oy5?FXy4H&P?c@RKFBIu>oN|2BiS<{2yWR&-`x1o;EQY?3Wb<&wW-YhdmYVRf$@<0| z-;fg>K+hwpU6l5cta8R3a{O;( zf~uXjdl`scqzrIBygD|_E?sxq3!*9#(~gmT)u{GNb!an$9%85htS2xje{3(uH4MDr~>%6 ziA9o|R1yFATKuvO+)2o$1t9&`$BSVi4`ccspNqgOd)kJ<>UeWT=w21L2eT#l<%Zgc z_oRDT0^f317V?RkaymX|_0ABVA1K?)Wo$gOQH?lfbyV5(-2TYpt-FIGf!a(dR;ujQ z$Q&jg%#0a1EHHdz{`u>!U7}c}6!}=tx-%eBL;Q3DmLeGnM@m;;mNnh3F0hTp@tv(N zeM%}tWU!%X{#Swtf$bz*bc&`ji|0)IYaU^<4u?ZseRcDf%-)V3T?skW zDevD^l5YLOB((SUGLz_C`M#BJD>ZA>CV2}$a-tlP)z!aMl*kNPm%0r2-4-ar4|^jm zS4b3i%w!=lTEO7}q>s;40HRucj&qc(iT zY4V2QJp>gH>Ubs>dwOEr|CW&B*N$DOM`2>WlXVSbMyi`?5b0QpN#tjM#Z#GVrtT=O z=}L?z)zZsXTHr3b zux31A^T3(q-7EE`pfH3uz9ZyTMWyqW5ShD{Nwn8h)T6`4J=t>|jdMJBI2q^g-FaCN z>CNHqazsYwKRU;-tw>SxvZ!gAa`=zhVDm9grWH@>RN&pi#mzrf`JO3uGow)*8b3S_ zlY*mi0oY7LSuiyqQD2dlGwq6FbJ`x-tdV5{VAR5 z!q{Q>PwdnL?x(kuM)?Qrdd)wOue1}ubhsn25GLjo{UgN?jQ>oOe!=7y^ zs7r9s?b#|0dPwc`-XCU#Y|95cl$5zN=Ya{lROj=6N+q1wNXiGtzo+Y>AlXoi&Z4rs zKuDw)VN_u@*?suE^XnSzj&Bh|1Pu9$XZ}^r<5Xfnr?-5lI3kr~K^njCz?ga_=(}hW zJ;HhoJcQPbglFgMAsX%ve|J=Bhz5ph_U;!Dsz_U3=%7l4^@Vs{wSw^3fd2m!A~`Jpl_8-JzJv zYeKz^`Es-`Vj=;VVilJhp&jR7%^{Jts)^G(+LDO8C7$2@zh0f$nXnOoloQaH?82Qc z^^s$ON_MlxYPB$F4;D-6ghKl9_R!3QK?nhFSGODN z3s}dI$b6@9?D}RB7dNMWXtvcVZH5*eHtEq!Bx&4io9@S23wLE+Ai<+dsu28?<|4YP zec8Nhb6St~egMGsxc=eSl>#RwylaOH3r}lHV$iK*L)Mu$RqyC5=6%a-n=LT7#H7F+ zY7kLp*8RKcL0xSjt%bHid;7Z15j7Z)b<|goYN_#o1D=X43f{U-^x{>M3al*dnQQNE zC-H3O1f(@U@{!79npIW05I@yzKcLgf&OsTc;aconS4wO!ehsI4{36_e)-BUQ_;;~7 z>a`Asi(S|&?^`_45+aoj1aG99aymbVnnN!a@y?Rk7aPuPSUTow0XL1XwyQELySuHv zx_eMwt)$ETG5AKgbUwl=Q|L4eoSt1pfL_(-&%2>FgnOBMrHO7<4{h%s7c8?7UhrkqHWa1t?5|#l|2vR;FCMI zfKvla!C}4bN4r%H;l{l?MNv=Y*0v~IvSQT&3ei|W8pnx{(m?sVIMR3>%oD|W6jzmY zRfR+73GsFI3#!=LQ0V%t2|*#di)mD_a$G;p5g&A!7s!P*D8y)GaeK4vRq^u-ALNgY z4gCvHX`SohSc7%pNXDm==dY7BT79Z4Q!y?Yg9dV1(rYA%;;o`$IL?t0c?eySR03pk zwDMO6Kt@F54PNPT}Vw?Nl#?@kWEDf(U&iUh1IQ6qX12NL=BSh zn)zI>e;`l9UygGA?nD3~RZC_8uN9VL9m36ksncRpZ^s4N)5PsS<*3*WDD#U;Ytfn# zSdsvrH_VtR2h2l7+%v4fO&SGc4HEq+LSdY|vy5PcPtSco!;rn!r2I{31O_TKzp0Z! z@8bNkUz*B#h2X{WDtK&7ffi*tbMRXuFlHo0(d>RcO2PrnJ-5U zI$Vt{6wRtngnx$~n%BR)hreGsMJls)B-v2qxfXWYoz=7%jM&%4us{5IW>IJ4bOK?4 zT?+vgh6E37XsyNHHlWFx4F)DH0c)zs$P!d+ar%9HG!(T=1}@IG5={i#3heH2zM_i( z7RS)tBNX%n-*H6Dd>e4SDN3_jp-6NZ#dGDwd8|CCv7V#WT$qY!D(_ssda$HDLb=@Z zF{F{QPqNRaO}qZcXYisk=Exrp;C(7j>gdE3B9C(3@_%eOX<%r3Y;Lb~B^OBETkkSA z(LK8yH_l=Eq|~{WN$^B7JmsF2&%RNOy-zU!TcgxaHiFGv1Xu3Pg_pKm>XguX@2@z3 zhM4Uxz=(7VPkGfR7zbmwn-xsi2+(i zk&TVO=7NAl!AVg$4=4?2HE4e1D`-u_BVDUXySazaEM2QVp&6#ewm4(I^|(u_Mdt|~ zepm>@N4gb~0AgX&S%_^4cQtSQWWglb-Y-kQK0uh%BXyf(w9ERTV`ZwWm$PIQKV{8Y z1#{%)m8O)cNTiyB`{$&tVXY-0h5NJPnt%rC5*?+kwxV3Ngo2RVS_Q|As`BFq@kWs{ z3)WB4y)DFbdVNc#Azzq-<++slsK+bo8Nc^Pe&Y+OsM}C=N8!Z%Mv^1lZO!m-%*XoNb2jWrWJe2L z4vI%k=i5mAfECOc{A&Ok$B&Ibr4^~62Uoc=L$+$J&q9Q0JyG1!@cUd-<-sbMnVvW|)A zk9>@>&~Xi`d${A9PzZq(^d;Cmb`j!O$sR99B&aVY_%w^lJi5H+f<~BoVA|j1?P=bV ze!YzWGc1h`d^@h_0ktG8470E_D6h!ZIxH*gc=FPuikJQTk3)ss5V# zC1N=!@_O0I-0`c3VYOlkG1k7FruQO0{l(bI=S#nb+tV0m2e1L~de`*&Q9bYOM7rlc zY2*t|Cr74>ciM?H7e}?FPbwb?8`|lGN?3AGRgBMLJT>>jqnTiR)qIXCHR=!@sDUdV zM^sMbt(93wXvZqV1 zG->uRw!tB~YiOm*<`muj3U?2VH#gR8N6p4_;&~19J!J(R2pg^b%+|1$U6gURRMWs# zY{gvNeaX+vbUV*>Uw){*4+ll~E;IZrFsCV~dY_BcI4`1LAO)p1Z;&4xm)I}1jI-6| zcf(F^gig%glM4&PCsck&S8P|ppAto)g#g2e@ucNo?ZQ!e*KyLLbLZbA;Oipi1Z0&h zef9MF-=vkD>zU@PC$F4x;bKENE3 zaa#|0x$}J7J~S>=vqjj|kF|dm3&lY=a<6u_jF_!ON^@e@a&+h$1|KGdZ=Tm+!i(Zg zvqtT$C}+3?2Bgs5jKJ@a1j-fkX#565V5HPUkw08qTQs_|81jd>cVfGEHrS%TlzNd& z?0dOA+lbv!KnO}83j?>HCS(!Jhn8GhH!G9)wyR!14=STNLU?RoUl#0Op;yJXr+$Ia z309r{15VD!@SkvUR#w*k;m4T>*g0AL7a{+@G&u(+8^eFYOq0s#G;SYQ(qNNjty`@BHkfP3&2odr9lYn z@Xw71Gy_cewE_D(W#BcTzw0QZ0SI4PVCR?Vnp?e4dtfeMKQ}Op6<}c3wkg3u8iM)$&VhifDC(b1 zVvry8`bmAjtNuQ7;N#QdANrlWjlPIKcY6~C(9q!C;tbosP;FpZf`$lxI&+}N2XP1B zK!#}jL4yqG;N0I0)EQ8~*!rN~bU867$SWX0_E_KCz3I?q6~sfJ%b-zSszm4Mv$`m! z25C!hudWFlLZJ86eVG)@BSv=Cc?JA;s&TgmU~it;>w^YqYy4OZBV9nw`wQeGA_}MX z2;Is0`Pr-?fX3%WMo5RJU_&?n32)HY{^&o=@!}Bn<@U|ESN{yUr{BW3f@3-ESjJ0?_;c>JmG4Io&@SHOU1`~`Q~QylmWa94rz7r-MJzlR?O2jTn$yoGam2ZjO=jNsqbof-Eh zn8Qu|YyVa^p+S(Fd#e|=x7dTd<$rf;4gusCt^vR|!=VuYwzUe<*HxZW>~ysLMZq`G zUXeaMcv{&M$i@uNPm$!E1Kjx$Hv+HIfH?FxJ~pRLH9g3*+lAQXbtQH*Za^KjGROA1X~ zz?_|>oeDJRu||rFQ|D5s2ZAEUc$#4@le}b1$WiX7af{oDESsAryE}!|Q=%GW3jB7} zbLe|%+CGW=ZM^ARam5)gO%4hiO32Cjao&^YNJd{lA1P(hq$dihNPXCpsY8=P;e1!S zEleq$%sj8ng$MVp$+lMn^yr8(p!H`H6q)9%j5G>*)A9kU2-yFKuuf}fC7cl^)(ZLn z825!8Ouq`a1J9sx*(JwT9loJL+D$Wx)!IaZ`}!7aQBdDxv%39nv{Zxc+9f+Pty#kx z=W_8juuf`QFlt|RK`(8>*C?mVChDQVCU_16P0O?d<4?0X%|TMgjl^0LLpI6geNphW z*E4{HVsX~#SRt@Ui+}b8D`NK4nAkyoLbOw?Q*;$V?L( z@quBbM`zp9yIUzexiF?e^C$PS=gKtAqCgb zlE;mSpRRbeLYrL*<4J*RG$U1Hssig$a0m*u6ds3H-A^TkEpb z=as8h)k|h0465&E4F&urGn-5Z2A>}VHmF65Bk+!L%x_XgpeY{t;v3%^G<)%z0g48t=;YSNt zcs)DoC%guB0Pex5uTCW{E+>L>1>9&Vx>^S6Q zaSEgwKaIuc=+N$hQJ*K}_7n z%X^jYxHmAvBW-c4Zp@49U;k19q^u$?s1DLc5_?SD|Me2uO`bq;R~-F@i0^MP7_PI+ zlGeN{|0lJfe;MBPZITybG7JIeEG_QFr1HZm?^+qtHpzF=CX`u4^~fNs{i}g6+X|N* zc^)(uw(y{3j?RnbL(f!ebSa-lZ&=8)IQJwZ)`WZAS+QlLFRMtg7AW>0hlk(>< z(pU!R0`bqt(Z={32J-p9lD9ZN3M3YW9`!i32{G9W1!Rz8gqt2sLPhZw)2>EpiNA>Z z?cT=HsMJtiywk3>mJNO9+_SO@4>Fln%feB94Oi{kG9X2dvOIi$i+`UE_L7%UVTJ(_ z)04nX7x0ATgQrfc3AU$9=8c(MD?8ox;BPb8uMy^$k|ZTaU*&6p_3yF9uj{}8LCcXQ zTG)d4@ckkcCG07|19T1KkI zc$4_*A_;sQ38F=DVT)h^|6j1!=S&FXmy+7;9*X4miqENPyouIudiCZh1wyqpUh|NH zAv{hcLKfei{$^JY>>Duse;eOi0l3K3MqCq22hxoU`!36#SM(c-&-)Fo2qh$%Bxr4E z)JbWqYx-__E#C4vmar`R*@K{$-}%cdN81gdpzEneh7^%d<~L|ZpjVEtW24nF@#9G- zg@Vkl%ffW5QxbI;nUTW6s0UJw)XchZF5uZrQ~j!k@8z38Y@|*U*8GjcE&99L{e5$H z(g+v#8ky|99MoJ(XB) z%K45=)D9!nx7VMoq+;CgK474P$q`lfa;vJ!=EXeS6tqTIPbJetQ?iyLR)~(aiMUcwI@m=&@|Af;dM5+(OPQ8j0NkNe$4<%K_y6b~CZKlobwzy0j z>pwBfK(ORt+;MoCi2mkqIuWDCr>!ZQy{RJ=*WS15%e zaEEx89k;m$KuMz1mQpi>@tzs?qG5-qi7mF3y&OeFY!VRC3VWM7KvB6VfA9WG)xVcw z=4$$z^{SG*y~ywJ-1uQ*JEKHqUB|^1CmbQ2mmU!mt_R@P_U2+K&6;Vv(Z_gdQ~HT= z8fi#=cgK*Xac~d@Nc7Vlntmk@Fc_9Q71sUnL3fToIU0IwXbu5bKL?%btI_joP6g|> zZgSdJn$=mz)&-V+;j$=OXl>R-gqMD7(&`X+5;5nk?#jsz;|6Mujbg%e6nzrPO$^pR)7`CQDk$K&T;Kt zfGu@YB*&OTWM?Z6-@2+l~sW+W2RGeGrC= z(m46zEyfS~zG(*<3bY!J(=ljZww0sK;3PW~27%7MslXi_KAfjcgdy+Wh^8=bZn z&TABVVrYZ!Fs={_>LLihh6icqqaQGZ?c03|3TD$jpLaKGm&P)GU8UE4fZACsyJ|_Q z<26%7{$oP0leHja^q*^8f&=9@5Han_;rImu>Ed9M?BHhoB=3n4iPmTFm{tt~@iUsR zcIyNmJ0{$!b*{H=S_;VmRPIZ&!wyng)Fzrlp6@PW~YiGkZ zXk!i?@8)mmBxS$pmIapbv!%(IzYG$VdHA2*evAx=o0{3qR!1zJXUO(+UE5mc5{cUD zRHkua4*DP59uFgUY<)SZ89I-5$Ug>K_!wcc=hA!J`|gc_>b$i%+Avn}HOLN0P_MnD46Y|G4IzGSfSTUzKG@#4wgx)MlTi zuCf;<%tO)H8Slh%`0}8MqQ9Rnw3txaL3=3EC7aiga>nN7d>9G{QN&L4d7;=j<@Zu@ZYtM$>%9{KoeznQF=44-BgoV_!$ zW+H}q8SAmzwxErQRtO4XSe_BA?phYQb-t)T{<@IGTvHx|pSe@4$TAQGPRiy&1*h4M z$WKQ5Wt?&`85#H^*LAY7AvPQVz9eS58$8G@w-YVseZfRUN;Vcr!`7^and{^|#B{?X z&BH{PiV_mL;=X1cz8<&e=SuSe_3+bHR?|)umNSz^alBlwV^#J@`0X3fWX}S-y2@@(t^>G;3`#$lm5qA|&*Q6rYB%cuw#F>2>eZEDQeq3lB z++RyIG*sD#^vs_@c%a`AnPue!*mFkvQZi-?1DPI@!ZY#ZOI?qxT3@Kn0;_PZJ??Me zDbf9bz$0%B6MX-?m&9^iC|qTuta%crIT?X7T&0^VBl#s7= zIMEZcsu5IIX>YNpRu8?b(6z*08CVay)dBHYCjsFuRF*L6Xc^wGcI=GwjB6bHzEcKb zF3eAp+}n>RYSBq(Nx6u)%=Z50V!r|u>Ku|?#*&$v*|WAWQH4Gl*8Mhmv!eDFS>W0H zIJmS#Z(I3Wmmw7m4I`dqr(q(5GD#_fNQ&?kh8Va_2DE4Q^SBXaexBlty5U;%ouO{6 zyR%O1c7VN17@xzbiG^g~YR(A>d{fG5!ZJ6=I`us2=_ypNoM|X}sSjCB+&0<+ylg(h zd5qLH7R!l@uWDgxG0x*b%%<8y=UmcD6}*vRMzf^5P!l9c9S686gE@Xp!gH;}M{vVW zTQ|v`t%;ig-MGqvs|4cX7XaDkv{-IqYe~auD%YBqT&85!cxx-j;^}s)lb-$0b-O>4 zb&u-xaN^vJGeIr14^M|Otn zO(p?fZ=GiCwR^k$7z%H??Ttn8nU(B-QjtMwNU}Ouzyo_KD!p_$o5{CmY-+J5s=HN? zpK#3afPmHisTvK}7pe8D4uyi*%uwIxoR~3^HAYXTrVymXM&hC^fU>9H9V62I4t4*e z*=^=3CDtQ(?#?JxFVpKT>tC5(%cuSB-VZ`70kQQ!ug{Aj>_;S|I-I5ZTKY31A1T84T$IW%h>U0 zrRO0r58bpp`Ie_obZh`GH3XA6t8c`iyJ1$N7&T?Az{pbArJdd;eNGya=JnZ6SnQlpb$ejVC-K5Xg>}O_8a4_H89~dfgaK2 z!-rf(x!Xb!mv>;|T7Ev8&xF+Ih=Jy_bI`6s=g=@!+2och)hwp}n=(pJ)|5>iXF@FNL+0~waJ3C*@RWkne5CB}XUGMUO;8G9`DFx^h%rA6mOrITY= zIlDcwJ^-cr?(c;Ih-hAaHC4}#!~9246F`}JHq(Ndj_?cJ;9R*kv$CwRaqHCql-Xqx zVNw&x3vvZ5Ne3UHVUkmtJi_&Gj%?&Ti?wt=9 z_Wg1LXD)s1GpJy;q*{_lUyCO~J%@_Ra;%LaEaE43fhIvON}D@19rj-s+t(utDCst| zD_mYNU=5M3TVWOB6s39W7@f4l(^vxrix=7#r0`nj|^i_aG6KtsK(FC0@Rpt;w zKwduTkssM%lUwy3L{HEY1`kV zD71GhYZq2k;!?A==Rba=P)w@nzBF;YAq!8gjL+7o@dNKqKDE&^_Fep_T_3N_STFgD zhCpm(;&8QUFeG#;M8r4V@3HN*&BKkL27O=UqgHneei<{6Rz88w08EO8nq`sI(Zz8^ zjjtL9r6~Mx@XEV2=4)jd;)H-ltfn`CLh=F#`e$qFY7wG;=+x?_!4(O6bD6p!JI9m4Tq`xjx zqbAxy?kKv}nj(3ogn;k|Q_u01A@fs2>F3&<6!s1z82&s`~H8wXw68X18qhS=19fp$ry%nGkZUl_)s}18|wBVLgK2YMl9p)neJ`Co_ zO(8Yq-(RqSBpF5UrKiW(y=MIULn%zgsl_DrZ33o85D-|S=R(ZiFk2r+{785cvMSN! zsy!|$BP&L*p;fFzTQE*8MsX^P&9qAn9~0bUIA8p`a!9T#N5prHDD)-{KEcx%o2XY* z1_}w`K8%vINuXXy6uZb7yGzp|Kl4|XKj@#rjQP5r`)uwWP`+~AjPwx*o$o3}uKP$^ z^Exv=ylf>M%l~G>qIFs=D^SV}nw)&E_^jU)ACMQWMr)|&~qAO3c-{G-g zd~EY=`t{0SadlySBL`2{g$vMY5)YFiiHF3L?T}bCNI3{I78@A20BuJk4_kNHbb0j6RrQjZe8pPjZCdf)l#2 zf&9$Wlb6xD#)Qtj7zl482{SBK^|@bTBRUTmA3yW@N(T=^y|i6gb0#K1Uxs34w9365q7Vde$3JE2QM}Ei;h`(q~~Fl7#GqDg#_rdHX)YqXbO^z#_DBrLX2r#Dtp zX_9UM^dwa*lVqD8`B{U?C)8xW4SPiO;RG_*Uv;*N`7+2c4qxt(**Kld>@mpA)NVsm zH3T2q!?ZAuTX@i%SNCw4VX);B)XqT}6Os`LhuvhQa~5P+Egn#qJYu_$bn!11yxrVo z#tieBJwM>;w8r$m+VkEEl_t_o2X-1c=uwJ;RdV`P`6C)%!G;Pl<)_g*k3K+mQlZqu zdG`6OFHb|epC^$Xu!hOr!am4$;XJICi46<()mMG~^0B)Ia4el;^5c$G<2s$A)#=qG zRc;bH)+Z6n1oc~jpR!^qqQ4CWOwcuIZWOHkH$okpT{U{a0bx0hL3(jIIo!;r^S0Tu zROTuuwD2Tngv?|Uz!NlCQo?Z1hb@(LS+N=}NY)Md(k8aZQuit$FR3lXHJ0$RF#|5A zhd?g83tM7i!{5!{5&00!~AdomEbz}!% zEci}Qr|`R;PEMQj&_0`Z@W_LEGxrh}Yg&OFoI+!X#WayF$zpN~pOlz?=^y!bk&ANb z{6N?!go7O&P+qtHc(w2n5UDa(b`g}YYs-jZ;8FJ2SS;@US7ToR6i2kI9Rk5!Lh!}i zo#3!&fB=gv4vWj;!3oY{!QBZE+#P}hx8Se&N4H)iCQeev^X zM=rnLy`5ej@oOJ*f$3k@Ky=AY0}DnJ59mT&T2>xJP(=$w?eK z9lb!Hn}Vf(+np%~9klrI-zf1m{)BwQ8JTG@uRx;YK3FQ6Z0M}0LBIs5gyd6fd8z1S zAH$TRpY63u_>^RCs(F9(Rf|Z;M>HO)o@Cog>>H-Ro?ZVjJ&X7F zK6w>gP%Rn&A`7|vT3)K2o0*_XIOd;MA9zGL)@_s*;Z;~&^MRh7UR8H{b4t9}z_TGu zNwzjws{;l<-Tf=>t+nO0n&37K1sgq5f?6|)*g(+9q`k*%Bb)UY;;x+vlq)w8liU_% zEAp6e$;wqgdZzY?LAk52133M$T*9F2j#Z=id`DkviM{j);lopU07LmdyAEH3s~?N*7WZusqtEXR!WNbVR_j z6&?R}ZaWUNM}B9x^*b-O5qvPAvl;DF1!&IGe+2viZxy9Q)Q}++>B#1DwWuju3>O{v zn7Fi`+zUpFCTfg)iP7=JK_@`-$zg=K*Z~+HSAyPJ)DRGWp*2iJY9P+mJEMUNPkgUW zA8L!)Y{wevN0b88r*I!6XT4lDe8!}YJ!L-*7?`emvVE<6XaMsQm78`dMCjFlHYn8< zNvA8yUg@is018JCMyHDsFY4==?cbqbHITk-K)^q+CFE<_S$#K&dcHWBP+Rxv)m6^r z(;^SX-Xty3fh8;+@MbCOPsH8{NGM&7O#2P5Q5W`SzPzrJgBhQx1uo6%p?BLDV^1Rj zJauVqbTMfR*qq!f!d9&8eh>tUnJfow`wwOjyV349*ge?A14t<6>pw-uF~O)suaQK> z!Au<;Dnd&hgC@uCYRunv73UQKEo0tsXL@7%B^^OH%Z%%M^PbTT^hg2{oABqIPhW^m zA5-lMi~Y67)mpL@E1$(~1??^i(PKddIWau2oe3O6(`tcOQOjcqIl94ElDjsQzv>$g zAAP4^O37x|+;F_>7LW2gVhoY53%e1@&f}E%K@}5c-kW&=lVD-s4Ba9aaB(m5P$P*< zC@;r|re3hx7anHtRN3-UOBo9-pW7)0rOHUylt|=(^{Me>fu)npL>NB#74WLUj1O8l z-sbwXOa^1ZjaBPpdSfteir(AP22^Dozi4{g%}V-!M~1{-jW?(|w~2YDocI+Ys*(AZ zkj<38##;5cA6`F=O?yGc6aBEO%I=?gxH;WH*Y&1vHs zs*oM~(-oXb!PJ0pOVql}ZO28VOqKHUH$|FWdQ|9_ZiMFS^zTv-CU=1(s=D289+eTD zL69&5ab`hLCgH?A^!iL#}UBCFYwRYjsFL_&56E^D}mzlQs z9vx>xuf&7i!w*45x3ND2a+|bh=E0MCX>lTQXi;M3q0OrA7=4k_-Xf5xMp@Tx2J)bN z;U#sgu#S2Es*1gafP>h4&{S*#`wmq}7G6@}gq9o2Ji~3V$&YG6_@_^6?K3`nw>Alp z0})RtG*i6GJ}bQF-jDS7M({3SbQzpn56hZQXml2N`#1*Zp$Di zY7>l|);MkiT8_l)z#d-lnggs&Ze;sRg{?li=~djgAATZ|JmTMCv0)4*EGDc+k{Y2d zY%l^adc1ZtYJ3AWdI370GY$T&h&LWBHIhM41{aJ52z@^prZ8+J5Ob_xfHt2<$Q&kX zDai;5O%MOgudfx9ZJod$H$$p8NY{;5vOJ~AkRFyLBI08fK{!4J)^=wgvC-fe{=#Px zp;`{$D|G4=G5UdqaT?jbY+tLKM&2W2aF8`ek&sRMi=4y@AdPuK|2g4ANC2f^seEGy z*swtI8()C}m*E=OU5F_~TVVnj#doOK_4#{TxU@JWC03{};57PZTM;Vs?b`6-w(HaB zG0vda9f$b!QI8_j$@}&fY!s(nX2!fdc-Y$0Ww?A@*mML|85QM!)-w9Zk381$7B+SM z!-6CW5hW;UQ0He#_ur3 zbwpFpdDeUA*|7tK*y{3Arce;du{)zxZg`>Eb{A~5$C=(}zK~m(1oayz)WihOYVEt9 z&;BbWbK?5;WKaJdWCTW$2a%lFeF>OOXV38Ym2O}2C+7uq=rjsVT{sYEV z;EY{(?3+nwNbHeRJT@!c3^b_Vy3E(ZX&jd+-u3pIl#`B~X`?s@jen3dj|q#_>ai?O6r&=dIh`erLENrPY(!LpXo~IqW^q1P z6G13yxH>lg@Wu3N`#zKOqe?ITubN#?mAS(&Z;qvF*lqgXCX}{ueFeK3Ip+W?pM(hl?e_B(BKbOVba($!|V27gff0F@rTY+l^qCZ z;j$xY#9@}L(+W4qKSAmFmT8Y;Fi{ zm;NlWSnP!ELr-DJZ zf@rzujYL+4pYt_nSFWhHfft&LUL5AoZTQ(tUP~h zW)r(AQRMVD_P*#+@%BJGROhqHsjnoOBR0wW7HgYGSynYg-#==;EI2m7I*;3u&d^c*su%vAaEeB*-~20{g*)yr zP$ZNi1nigeNRuz8L_?hqC}@v%w+*RO5Q=(AceZq!AxV(Fp^IB)6%&iPe)}!}H!!CX z73WWP3ExlWx1OS>MI{dkvK>|*<*jntIMs7wZrDOWD{Yewz3*5;jnWoGMv4tXggREx7r*_xk&PyK=yfMOmSj4X`oa^oxm9o~m%T>NQ z&Fs4QQW=;Oexy@!JFWxnzB&s`Rv)mFQn|*WODAEbhTuOp%Spab-ke4efT-kWuh`!) zrxMyScMD@Ta>Q+de=bKQHCIIE3W2v1aH0? zWA3#AD>Bp`PT@@MIQAsM*qq0LZ4OtPkyLb$%(JAMIIpprxS;xMP23cPfU2tH4NC>Z85Z&ui z9**TA`KRNEN@otD5@Es(OgA$jdc6T_LgkF>V|?{r2aT`i3)x|KOV= z$617lju)mPBEZR_vw(}$f4{(w2|>FJTg z+7%7hw{F7YNBOVx$jTh9zZ2biRs+|*AP0LWCm3xAs)rKaNp3 z#b`GCq06`OP#e&kpKjRe*V_TDqIUG0MG0l{c-%EY0)-1l>r+tQ=@rJzcNB>k8BUg_ zZ(D;liSsc+#xq7826PCgm)S9syh#4;NmRc?V9@~STWU)lv<9XRbj9o(w4;lboUeZ@ zy7fB~+B5=N54nZ1W?b4+Spd7!zdNv&PIZ?Jb?Qf{t71+V%rl!jn&tU39R}*Auf6D( zh>8=T-p#no3Jm>OT;!c`yTUf|r~R*Uhv*W6O0rnfMs*=Uu2{c|t3mK)XVyg{^ym)u zu=1-n)IhO0vY$+A&DRv}E67>tu=nFb88Rt;C725`j%8&_s-}5kULkWu(`(bL{N0K$ zY8iu_9X7t$6_WJy<8W5?!IOH19?Lt6h&4sp_GvgZab6#G3MgX&|CY!k79B}xR8M_` zGu7^gJeiVbG$w>vnS5r}EVY%>Xza@J4ap?mw@rD%jS+bAr6`}(h^=eFLU6uFLB$Rs zirBjijjb2y7H(noQ z4%XqblT>GGVNW?PVnd3)Ww)}`U;>kC^GRW9q_NWprfBVp zdXJ;_HS8zOUwEl8lR`(Wi#ZdRK0Cg-YUddXfx)6;?luz>c_V8eGy}k;?{}Rc_A8!C zzgbn!!BvWvJY4mRxBPIr?scq8@4dPKSq^Fb)CF?T;jXe0v_s9VRCsla;5IRXyd3@! zFq+nS?pXbC8`bJlg_GKvv-_G6b6PgK%9%)ZVLc7DKG*nC-2_QjMW40M{?(FzYFKSv z+!}<*fpG5(j6-|4xe?qZT0{{$@}t*k)%BSCq3M2fG(2E+X~n-1OW3^)>}c+^Co14`Wv5|f)&6Kk80YTvr$2S;TSqw_niB;!+?MW{ zwX{no##!#W7v~Qh7&kf71W(=#6_d(;E#i-A&=xq2$WO!rh7H2)(&yVY)7L(9QRE~V z4_|r4CpVz6c2R!!{r;Gy2U|FTj-Yo&`72i(Bv<&FNIW$3X=rEGxSijfHxG94D6<-j z@02Ra1?V#zem6~sIb`Ry5HSB`ZMBb=CZfA*rK*yy@t5Jl_?G+-t6l+D8!gq4{3Ul6 z&P9|jP40-;-w)#Z8{VkJ#v?kYJgcr;5~tXzdCNG3rxzvO)728H#aDQy^5i0vzr%ym zXXX>2$=9!ys4o7}KHZ$3eL_0g+>O}2Sz3sy_nu5sHqN1S!|{%Q*Z)!Jvb)j}WhLg1 zY&kHF-R%-0jP^0!X<4VpRZo&Ayk$|UcsBG;G2YSn8~#v8iA7A*tR4dQYQ4G&_S1$( zB1MS62uxHDJ+7NXG>&V&=Sa#sbY4e3fTvl_GDf#WnC%IU`fosPq#OqA@!iFY#GdLG3 ze2X`2t{kZRi`TX=&ii`n{J?4iABB~sEE@v>E!X|pqrF6A*cl>^QYd2JFoQoK9 zys+&Yzv~5c5;od#tEX|<3E`n~|*<<`J6EQ5Ky&hLPsNqm}RTr?1`kp@Qy6WJ; z1t!x;Q`cm)Tm;-$JB_vTuijJ2>;T|CQDr<}uNSUai&961$gE#9toTkL4;kI4GFX{E zt>=*$cg<#Tte>VrOVG{z+p#NGu5jSrXJt5ysZy&M4V%(*Dvwt87C8A`N1OM{n>k(1 z_7}oi{@y5KEl%I)aT@2&l)cnEK=`45oSXvfGW+DedqVvNG>ta$kdz_HyroDab+>@X5x_8eshbIk~u4SOT0(&FxIBESNbzxvj$pQl4lryz=wLs_m2m@R|Ifg;{{s}4 ztmOb*=E8l-F?OaJElyZ;-qI~&;l3;QKIy@##joMW@=N=h)y~ zku=ygesGWqAaRnrn*BS@*RwV?OK@0ktCm1X^zLB$`in6q@()%S3Ai%nT7HTC9mqH9 z=~qX9mzU!qFf%)j;k{}<*>P6$(%Fuzmee9=%20pMISZA}d9ZGFx4kp3u4B>f>l_bO zIL8fNZYGU3B6cwmgf6Y8F^pAB5Z0Q*GP@llmZjO;o>;^qMDAmi`m3?3Ig zZ`a~Pv{qI5)?)RW#4@~A8(e3)!Z|}CEU zPaB=wz?%Ok0h?~No_X0{^mLq^PL&ZDN zc+#LHFVI2>aiS9Fn6PSfd--eSF5*cooX}t zAaDlQHbcN#SRgG!#x^)3Ik`01I*3ma2t*8IkEDU8tF!kd$Kg#$l7tDx(GYb`e$$F# z1Jfh17u-Ed2ueJiwcq;g=Uh#252OX8aHrrwKQ@?xUp*jvkY>Xl&PW{w!s#;j!-r+; z>w@@I?j+@{%O&yI20{+VuM}{FWw8?;zouik#WFBtGi8UwJ8_FQ3q$~vhcpr-w<6TI zK*s56zvI1K(jKp+2yFlu!!-CZZxeO#rKS62J9x}!W+asmhuRUrNDama@>bpb>l>Dg z?@C{p-**4O;#x(q5#&A!Lv5DY9|kXk@GV19L`-yM$1 zHHR!78z0&5L!$j_kJ#JndA>to~-VoCW_fpMCpEmCqBhs8GxwK`RYgStWO zq<_36L9Z36Uc3Jpg{!3J#WY@7H7a8d6Wu=AeG?-`;7sT~VCEV%4agzASX@+9gW!IP zM&^Ieqk0uPp52HAp&e$-R6t;+144M28R)+zCB?IU%>qU2pTc#tBCQ^piYvGNihr_i zhEc$)ZQ2)0=Q2DZ44>pQlNZ^8Gs^n?m&!}!j;mKQJR-Nns!Fx9l3uH{_t~7xHt1)? zGOPIXc^Q?Ls%AmOGJZ!28r}c(`uW%~?7wC#aaJvn_6PMWS_0JJ+`_yTf_6oPW(Pa` ePg`?!Gj(zEeA%AS`FVM{h56Cx>49o;=>G$h#<`#X literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.tex b/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.tex new file mode 100644 index 0000000000..a040465426 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.tex @@ -0,0 +1,114 @@ +\documentclass{article} +%-------------------------------------------------------- +\usepackage{hyperref} +\usepackage{caption} +\usepackage{subcaption} + +\usepackage{graphicx} + + +\usepackage[left=2.9cm, right=2.9cm, top=3cm, bottom=2cm]{geometry} + +\usepackage{fancyhdr} +\pagestyle{fancy} +\fancyhf{} +\rhead{31.01.2019} +\lhead{Pilz GmbH \& Co. KG} +\cfoot{\thepage} +\renewcommand{\headrulewidth}{0pt} +\renewcommand{\footrulewidth}{0pt} + +\begin{document} +\title{Motion Blending} +\author{Pilz GmbH \& Co. KG} +\date{31.01.2019} + +\maketitle + +\begin{abstract} +\noindent This article describes the techniques used to blend the position and orientation of two robot trajectories within the {\tt pilz\_industrial\_motion\_planner}\cite{pilztrajectorygeneration} ROS package. +\end{abstract} + + +\newpage + +\section{Position blending} + +\subsection{Introduction} + +Suppose we have two trajectories $x_1(t)$, $t\in[0,T_1]$ and $x_2(t)$, $t\in[0,T_2]$ and we want to make a transition from $x_1(t)$ to $x_2(t)$. The transition window technique assumes that the transition happens in a predefined time window. During this transition window, the resulted trajectory (blending trajectory) is given by: +\begin{equation} + x_b(t) = x_1(t) + \alpha(s(t))(x_2(t)-x_1(t)), t \in [t_0, t_0 + T] +\end{equation} +in which $x_b(t)$ is the transition trajectory. $t_0$ is the start time of transition window and $T$ represents the transition time. $\alpha(s)$ is the blend function and $s$ is normalized time parameter: +\begin{equation} +s = \frac{t - t_0}{T} +\label{eq:1} +\end{equation} +which changes from 0 to 1 during the transition window. Following polynomial is selected as $\alpha(s)$ so that the boundary conditions at the start and end point of the transition window are fulfilled \cite{lloyd1993trajectory}: +\begin{equation} +\alpha(s) = 6s^5 - 15s^4 + 10s^3. +\label{eq:2} +\end{equation} + +\subsection{Application for blending robot trajectory} +We want to move the robot from $p_1$ to $p_2$, then from $p_2$ to $p_3$. $p_2$ is a blending way-point which means that it does not need to be reached exactly. We want the robot moves alongside $p_2$ without stop. The whole process is described below with an one dimensional example. + +\begin{enumerate} + \item Generate motion trajectories $x_1(t)$ from $p_1$ to $p_2$ and $x_2(t)$ from $p_2$ to $p_3$. $x_1(t)$ and $x_2(t)$ both start and stop with zero velocity/acceleration. Both trajectories start with time zero. As a simple example we generated two one-dimensional linear trajectories in Figure.\ref{ori_traj}. For robot motion without blending, the two trajectories are executed one after the other, which means $x_2(t)$ needs to be timely shifted by the duration of $x_1(t)$. + \begin{figure}[ht]% + \includegraphics[width=0.9\columnwidth]{figure/original_trajectories.eps}% + \caption{One-dimensional linear trajectory}% + \label{ori_traj} + \end{figure} + + \item According to the blending radius $r$, the points $p_{b1}$ on $x_1(t)$ and $p_{b2}$ on $x_2(t)$ which intersects with the blending sphere are computed. We also compute the durations $d_1$ for moving from $p_{b1}$ to $p_2$ on $x_1$ and $d_2$ for moving from $p_2$ to $p_{b2}$ on $x_2$. The transition window should start earliest from the time of $p_{b1}$ on $x_1(t)$ is reached, and ends latest at the time of $p_{b2}$ on $x_2(t)$ is reached. In the example, we take $r=3$ and $p_{b1} = p_{b2} = 5$ (see Figure.\ref{ori_traj}). + + \item Timely shift the $x_2(t)$ and select the transition window time $T$ according to the above rules. In order to avoid stop on the blending trajectory, the time shift $T_s$ of $x_2(t)$ should be smaller than the duration of $x_1(t)$. We now have the second trajectory as $x_2(t-T_s)$ for blending. In the example the duration of $x_1(t)$ is $6s$. Figure.\ref{blend_case_1} shows a blending case that we shift the $x_2(t)$ with $6s$, which is almost the same as motion without blending. Figure.\ref{blend_case_2} shows a blending case that we shift the $x_2(t)$ with $3.5s$ and the blending starts at 3.5s, ends at 5.5s. Figure.\ref{blend_case_3} shows a blending case that we shift the $x_2(t)$ with $4.5s$ and the blending starts at 3.5s, ends at 6.5s. +\end{enumerate} + +In the actual implementation, we make the following choice given the durations $d_1$ and $d_2$ inside the blending sphere: +\begin{itemize} +\item If $d_1\leq d_2$ we shift exactly to the time when $p_{b1}$ is reached on $x_1$ (when the blending sphere is entered), +\item if $d_1>d_2$ we compute $T_s$ such that $T_s+d_2=T_1$. This choice minimizes increases in acceleration/deceleration on the resulting blend trajectory. +\end{itemize} + + +\section{Blending the orientation} +To blend the orientation, the method described in \cite{dantam2014orientation} is used. The equations (18)-(20) in \cite{dantam2014orientation} are used to calculate the orientation along the blend trajectory. In our application, due to the fact that the orientation change along the original (not blended) trajectories has smooth acceleration and deceleration phases, (18) and (19) from paper \cite{dantam2014orientation} do not need to be calculated.\newline +For the sake of clarity, it is important to note that our functions for $u_{ij}(t)$ and $u_{jk}(t)$ are different. However, we account for this difference by not explicitly calculating (18) and (19) and using the given samples of the original (not blended) trajectories instead. Furthermore, (\ref{eq:2}) is used for $u_{j}(t)$, in other words, $u_{j}(t) = \alpha(s(t))$. + + +\begin{figure}[ht] + \vspace*{-5mm} + \centering + \begin{subfigure}[ht]{0.8\textwidth}% + \centering + \includegraphics[width=0.8\columnwidth]{figure/blend_case_1.eps}% + \caption{Motion blend case 1: $T_s = 6s$, blending starts at 3.5s, ends at 8s. The resulted blending trajectory comes to a stop in the middle.}% + \label{blend_case_1}% + \end{subfigure} + + \begin{subfigure}[ht]{0.8\textwidth}% + \centering + \includegraphics[width=0.8\columnwidth]{figure/blend_case_2.eps}% + \caption{Motion blend case 2: $T_s = 3.5s$, blending starts at 3.5s, ends at 5.5s. The resulted blending trajectory smoothly transits from first trajectory to the second trajectory. The velocity profile has no jumps.}% + \label{blend_case_2}% + \end{subfigure} + + \begin{subfigure}[ht]{0.8\textwidth}% + \centering + \includegraphics[width=0.8\columnwidth]{figure/blend_case_3.eps}% + \caption{Motion blend case 3: $T_s = 4.5s$, blending starts at 3.5s, ends at 6.5s. The resulted blending trajectory smoothly transits from first trajectory to the second trajectory. The velocity profile has no jumps.}% + \label{blend_case_3}% + \end{subfigure} + +\end{figure} + + +\begin{thebibliography}{9} +\bibitem {lloyd1993trajectory}Lloyd, John and Hayward, Vincent \textit{Trajectory generation for sensor-driven and time-varying tasks}, The International journal of robotics research, 1993 +\bibitem {dantam2014orientation}Dantam, Neil and Stilman, Mike \textit{Spherical Parabolic Blends for Robot Workspace Trajectories}, International Conference on Intelligent Robots and Systems (IROS), 2014 +\bibitem{pilztrajectorygeneration}\url{https://github.com/ros-planning/moveit/tree/master/moveit_planners/pilz_industrial_motion_planner} +\end{thebibliography} +\end{document} diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/README.md b/moveit_planners/pilz_industrial_motion_planner/doc/README.md new file mode 100644 index 0000000000..65966e8415 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/README.md @@ -0,0 +1,22 @@ +# Trajectory generation + +## Overview sequence processing +The following diagram shows the steps to process and execute +a list of commands given as sequence. The diagram also shows the resulting data +structures of each processing step. +![OverviewProcessing](sequence_processing_overview.png) + +## Overview classes PlanningContext +The following diagram shows the different classes responsible for the loading +of the different planning contexts (e.g. Ptp, Lin, Circ) and the +relationship between them. +![DiagClassPlanningContext](diag_class_planning_context.png) + +## Relationship MoveGroupCapabilities and ComandListManager +The following diagram shows the relationship between the MoveGroupCapabilities +and the CommandListManager. +![DiagClassCapabilities](diag_class_capabilities.png) + +## Blending algorithm description +A description of the used blending algorithm can be found +[here](MotionBlendAlgorithmDescription.pdf). diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.png b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.png new file mode 100644 index 0000000000000000000000000000000000000000..eeb13715b7c475621885cac19501c728f9aa8fc3 GIT binary patch literal 41398 zcmeFZ1yq&o)-H@9prne3NZE*#fOOf2NGl*6(%lV8h=_tngMdg&gLFxWNOyyPbmxLa ztTP|bSKqz&_nmS6|C}+-IOE-8?BVNjttal7^P1On&D%>-T$l*|G(Hv<7Lmxk+tOHA zxNoqq4kjKx2%li+817(UJ;f5aedD2R@9cn+=|ip3GtPcCW~PUq9jkqQ^aE+L*Nr4T z{KVmt3X?wUFXtIEB>?LXKloLPret^z zzym$)=DtG?e_~yCyxfg^aQ)T)Eq^+uyAII96Me?AIOu)ij0`$fE#b!z_yD`ci@AGB z{mK0VS8Dj{8zCINNl!J|3~czza-168&e3{&YxwW$r6*?J)4Dz`Cx*X#4R)noy>*pg z?M+ql(L(7eCkg-J=T{W$6=*}T))|)K#fps5PoFFs z!Z>(%V_leE`-jszsb=swZY+M%Ya4W2X=o4Q8nhklw`d|_Xrnag&WMhUebSd}r0TTx zXoDgs%5FTA#wKLbU|x3kg}Ro3!Az;s)=4IXPo~2aG)>m6u_9}0YwFq5eAe!VY==F_ z3uzNSe$?MupF8VUx-sIdRTng1T1iwwa}gyPO83SnyCIw_I504-k!hQ7A_&xGc zihN|B?afL4`0>iNp@D&cp<(gJONJm8RhmwhjbTr96F!@H!nz`x1!fi&6297jCv`zg zq?aCa7u#8IYsi;9seMaD-AWStMWiX9l%?Av%2yDrYhO&xD8JaP>8oYTZ!uo~nvzTV zc}vRUZ2Q-_{i&xxz+z&)C!20(X0XJ8;mQ^1I(Wc$+-4(b%0)1>FmfF#;YyB6 z)e`(mw|A+{Kd`*^kEExIRkPVx9A0S>!tBIURQR12X5*q^*RG$4a)~rP9#k2o62oMZ zY+9wlcQM6KPp`KBmXwqfYEEe5yW8fn|Ssq$x^|_DpgP14c{}{s) zT*N(HZ68EpX*vNJ@rUK}U;WOz*gX6=JFuB& z@@U|bo=jowm)DeS3-yH2+jDtC>-iX})Mb`#tId0}altCRPFqqjcXM)cSoOjl`hIRh-Y|6^Y zQc^9wIR@e3;gyzx+x8ugGCvOQSXmg9CP}qI8!Yge>lEEle067grmp@@f*? zG7M&kkk2ztfcYb1Noq6If3PxX1LnV0HwIhnkyB%W=pFDLEMrt-{Uwgm8))V}|niVZ0z> z$v%7tn_gh}&>=1u)*}qJE+jP`ZSU%83T4-6Pn0?@>_bRkWo2b#Wc29Kqnw{0n=?pQ^bG?XXF^$MSVfjllHe zTSBCnICJo9QW@Add|rpJusnF{JW8XOUv~QoIBIEWML90lJ}nIN;_ZJHv-gQP(-RFY;^p#VDLri)7Y!2(Qot>Qr%o#|n^09>*E*^)ypcX{nB&2*IbLmuR zoM`awtd7vkMjB2}cjM8@){w;qOJp+O9>QQ5@55zX# z6ANx1#Cm$D-aL9C!TTI1XJuD!FE89k(YWB^(!X3~6_cn{8emkAk>G8|h1m^uq;i<_ zbRNt%(-aY@tv%h$Tmwr*V4-+LHbvfVRxxzz!tUh$W1c`Wa?Ngj@MfMa6-?nb!xMny zT^n(U37(!y!8RP;$+~4Dhv_MfJt_Xr8TUj-4GSLJ13@rF2Y8uGA~7Bna&! zrKB)gYEQN{XNEcj7C>;ANsx%5IcV}1ENYC9>HUITjqOZop7TZ}ZRu*OXl+qo68m@G-7?PLdstYKliB?l6P2DM65*V4 zFzOFd?M7d7H432mGuWJ(i6}V(0s;*Ca_3-r@r5dv*_-%DJCP(k)0Hl8U_=TZ&Y zuRCgIRwp#>9UhJC_pY*Cs}$H)CuYC+(hh~%lpDhmfRK>XNGNoxMw_AALyg$%IA>p; z$q*!mlgvsXTdU^Rrkn+P4i-7|TkC~0-Ge)@saLq0S`AOCdx{1zPI0)EPArzdSm{LC zjYkSN9JgkW(fLff<6^|@hlTaNS*IXLKnOh^0p&hbj)Sj%d2RBJn=$CtvPwz>YBpm?io7#)PPj+S8qBXP0W)aL*HouSA3M5#B{pTnA znKFuMjC_de3E#pXbN`S+8JRzkjNBPYb|3iGPO&JAQ4JTpfQ5B=^|7&jlPByD%=XHV z&=tm5{999QJ{0VivcHl6@M?^~oO*0#wj@mJ6maFT`!ts9qn|(2eqqS9D^^J`3k3e{AfQGD3=u>f?{YWQw!c?9W<=Y>ZkQeO-d@5 z%k2|{r2!EULNT*egkeXXVJbRT4fXZBZ+)ZyALF(!W=Tle6sVOJwz{VdxE#|hblZu)hL zg>%M*8}{e3?nJi-t2%Gi2<@85b5Fdps4%w=&nW*E$l$;~Lc6=@sk%Kv-qRSn6*$g~ z?#|RiEeu6DPvvx})GhgSw->cS)et!5kssFa_tOYVx80EBk(RA6cHTA2P9& zY)`)C4LOJJ=ETHkLiOUZyAoA7M%#uWdG~H=#Efj#oKQE|x9a(r^W|F;2!U`7!MYQ7%<Z`^4N5HlFXlPp25JvGE`_~FrL6sDHZ=9ba`tCQ;HSuT?25K^?f*(NhAQ; z#t6PjQ9!fvp03KMvtLhkm(Xx2zH$E?0>uqP`o1wyy8mK2Au%J9l<*)F2KddQ#oBa7 zX!w9x4fQQgVj`iUYw79fQS3U6iGb^Us3=YXjBI70=Cw@B&0Vh`VBw$p)ONG4wY3#c z3W$F*xpfPK}SxZ@laO7cV>)AbQ&=X`6q5al`m$(T%mh=!lLWEm0y z@6Ku)W-|&?@Yhh3e|(gDNi6Kd!T1j!Mj*K#>H9JNnX0BCTjbY>Kz@pG_a!_ zZV;BkL^tv+6qh)lPJXdmI%L^3QtdrtKa&bcUQWd>?&HU4UzhC|ZkBvo*$lVcd~77F z-CLhk@U4DwxS~b)_36OfN#g?#$R)1Zq$(G!7q2w(!H~v_M<^&NhHpyLNnKt7;Jagn z7XBBJnCR)nk=i{CKaXV#+DPZ&^ z?-Sj9L|KLaUiMKL;>#(2e43@xb(rUvfg{0K{s&@;(|h~;|L2X6OBRe8i@TORybzhk zh7$P$J>#4eJ~(o}`P@&~4|w{YPyQJVC;x@zNFDWVa|sSA7Wz8 zugw5{H7;3`9gvnkFaP+l-XVI%-<3X2QcO%tVWB+~lT#d!Z>y`TAqf{Y()S;OM__+_ zhBe_Y&ROzJXPPn}RHgF_I`tub&NU}lY0JaHPM^P5E`?CedpyEre#pNUFJ5FRS(Qcw z>+9~>kmu6|6?>oWMtatH|d#~i797T&mVUBO?|xoY{@cW z%f{u}Z1Vg2^--*@N4-r03BCsGAsnm~omXe7&MJ)^1kMJj9X@YUoIh-KSgzpd1FtY7 z5nXlpa`+}y#Uti3Kcn&9B|QJR1VTvtGZz1>i+}dW-{OV6)3lo)U2pXD><8tOk!!Ma1HMx_-l0W&uyRhtpY-GMp1u#oPHbYL?I!%J3uh^dx zQ}ddQRH^PR2ZKyD<0-leHTOIqZoRhn2%l@#9-h6WThW_Y-M};2I7T69^$W^NWyNebC*~Vg`x>W_v=&WhyQh zkUR-B?`k<7ZHi*vd{!MZ+*&2;No^Shh1tdIz#l+;IijZ$di=sqntwbpO0B$az9G~%5DRgsdi-~{JUd~50W z_cE7k0b=Mhgpr;6`0-Jm@N=T96L17-PX)yY|6T0wp& zuMuuHXZbbPAUCStqVn4+`!FIA-M^n})VJOtr|J=HGSR57b2s|#k4C^-?|yiXgp{z)6=ps)VP8oC#t2v~}bc7lSl+2pco;p^FG z)~J|g%*4P@*RLqp;c3W6Na;>Y(sg_tIDk@M{+N{VuXg+)f7;St1K~v^eIjV?B@lwG zwLD)~SEiTw^PWj&__N_eu$8;)Heoi0J!y#vP=gL8^79;DEgUa!+L-9@8Q~P&tam!@ zsJSpPdIf~_<}Df3>GFjZVOp`K3%|ruLDHX`U?^#Od$>#_5Q4x~@Rv9P%mMuN#NMtc zQeY~0eSSbuO3F`xhR6JggM)*Q%>Aw#=CmbU)3@wh4KZOI|sx+a!-}HK_;F?rarPU z-PSmNi&vWC9b7|q`(J`!w!+i!>>~pn`m))Em;2pAxPUD zxfv72Ki`n2acoaYh~xz-JtH~ejlCXlX`xa0v-KOr()v6l{{6xWIp{Z=Jf+RRuV2pT z@9*DkzD2^Mm}`h$o${xXa0a@~USOg*w{CDbN$^RZ`ZSY*04D{H`462ar%tC>^LL85 z4BuK}Md>C^)zl}~Vfx<+r2DgZM6ywG8E>0l=E~yol=4m03rB*COnC6E3mo|R^lEbN zMXL#y3w$|&a98_VdgnU|6yG6`_PGWuWza;IN@QtW0Vgg2!$3#?%ag>Yt?w1^v!lL3 z^Gol6^`1K7f)Z|?C;6}`N6RMU9}kAB4k_e6 zxbqC<0%Al^q+dCGLKVkhHe!+4;L>tl@>aFnv!OrcOCE|uBf=;a=H6#;)n5r zr?H-@M4?+roP~X;Hj84U7s#f!`{Csq1`3l$_k!|ccejWeeOiE zX-Yk#H!zEyLa_-on-6KtsgPi_g<6m(TkSPSAKP#oLL}pz_o3M{R1@z;SDbhn&-M{p zJQy|5*KdvTs}C6M&r+81xikQRKDV}U=tKe0JJ*N+`l^6UkWQMoM_;j&ZL^hJV)W!l zO5gpOn|Jos^C!+H^Xr5)ve$B5mD%(esel3 zP=EU_75(NvlYJOhBwW*Ws#^DGK|t>o!mJ3<@>)(t?M_8i(Cm)7Z2Oi(u7EldJg3^+ zWJcU$9in-f)>;rU6D+6Px-z?{pRX-IZ0|c7QlJ$%!D)(w$D=ai^F<50B$#0#%N|8} z($0x)MF`=Ysfuk;CUeViq&@b)(kwHr9JoLaF0NlK+T}c0jqyyf4Uqy{gK>9; zI;boV?VC~rV->|97=kV?kvasC7kFJh?fMW?S$L;)2pgE3)KsP@3zckbG4Snh_kq>m zQL=0tNx&#o*4$OpwO!bt_*+LD2s}ljtbHu%%6lB zkk4LWV3?ViVV46|Srgb?y2*MzUfwiFKT!Y)kn$501JLmWLB_M1=~RY=veV3jZ2S69 zc9*qe)e<0w@knUEOJX-)Fp&mH9`1*4Z5$86@ph8L+6_qxHhRqr4UPv(EZqWsmmX0W zzoo~WBa;$BJC=V#0I{XD7iuA-u%0yd;P8%xXB&LL2CS0Iym?qT3_f0?8t&iCbbdgJr;7g*@X6HiF0 zsL-riINtaG!Q|;<+7s3Y#Ot*vi+AaN#U=ea7+YkH`BvregB<4G%7ne1Zl4D=P+-9Q~Vj%=0Vd^3Ml56rgP zOEo5?;9)W@obsZJoLrXnVe14CXA4e|=N@Bq!JfHOMt5JGT3V{1McGyX90WT3Vqd7G z&>VPWLTN`EWHah^{JKY(iM7+3%;w4$oKZy(|NB9?z_&(>h_=(3-*J(;A)@~peSqr# zh;X~}iS2zeyqNX#ncQX*Nv}+oQ?`0E9S34(XM4N5YGoQC)Pm#(*h^^hhkp-T&!-ezegHc`$!4L_ z08<~ZXmv|*q+nrllph)#+--~6T?O%at7`=K?v}uObhFzP0}GdWEUd+L&%HIq9V>DW-x?OuSPtTQ zpkTYO=X)5}24`7KbM&nu$1m)O|C^K~wc!7`l4LI+>$9&I5wn8HYBg-WYp}CZ_R%A< zG~R*jXipma393$1zNahk$CQR0R&1=#GPJJuKb@}|d2kQEUe#E>Ib!nWt>Ot{Vxt;{ZY>1@Ev;8u5=yy|HEhg{_=`v%kf$F3cX*$ zD_dK!Tk3c}H-=OOL-V9`#>~nd!v6IJ znfLr{J7r5v#DKN9RTO1}|{#}vsbP_yp zlw2F^r)B?hK}k2`6g_-=MGIgxVqTgb2WeDGL_}nq=1(I<4>1>{5dnwwSQn_sOdvq$ z+kx2%>{$Jf4FMH`LK-IF5AKQ$h^}8JDpu&b8{YA-D>=Yk)eaGo}LH>4LO9IjLd3i6OY#MDmazQM!&>a zbHE;}7oZY!|4e%1oEcrT1Y)YTeI2xMRAZX20~ahE5>xJ z@5o!EkcWJI&zn2G8*bgvLdvc{d*k>wPPIy0W=s2v|lx+>XIeTk9f7FLR++C(2Vb99D@Hm5y zzA_aO`RC7JYd4l*=ljnx_JO+<1`;HUne?(ql^(zn548AjiVXWKf4W;sHbDjlI!C4s z#&yJud{(<5EC*hWM%5mG8mzT;aBg-!)&YuvBkI!G3z6U+oSy-MdXWoecQrk|WL;r8 z;5i}1GSDvCd>w!hp5LpReYq;EA@R)=@yK6&s_pdVD@4=_9Ca#SHtEUM0X9lU)|72b zHCF~~ykcNAwcXvEDB?mG7BCDbD)u2_I5^=(nY#gvlaY~uj+|Riun8|gT&7~x@ko8= zXAp;#Vnegj>h&{@7la=gb_eLNF`M0Fz1iyJ0p10LPr9)9)wf`KfG3N|>OSo4vv1gh zhI+w4P%-A|6kl|1Cyta(D<3IZPaVw`yk`nH=xwmp>({RlObD|B{#n4g=6T-|3=`CX zjlv<9iifgkfz23-=~74_4u*q&Ib2gdeOd))jljICvZSi25Fj-$8oZ|BVPkCX=l~!J zzz!Ix_&mGGXis3nDiL$Mq9E9N)ay(&z=|~GZ(?kWuxa^bqZR1IX#j>Y2`4y-ySS0C zPhri|VJYX1So{S`2_9JgeY@4xWdRlP158Vd%*-m88r7^$*|+Dj2Cx{r3mi7Q+c0}I z)P830|IA1Uc3tphu$LVf4k=#iR7?OSjUM}Xk!KlqCT|*ILXAWpefS~bkaiDHElSGD z0w#mSkYm$pdL6;|4l{!ckfx@lOFTq#t0AMPu9KRllId1YBV&1QHbsK&>UCr(W-x-` zQ$ADU4p^I%Z3a4Kgw%qUfdi-y=St7YVq2l*p2@pL-|tGjJF+^r#~$qGi3>b+rzt6+ zc!tVr36R2sE<`+YaNnIICm#Yp!t>Dq-x}VFkcV7w(jrH`qH$E(uoqcrZFX zr!>~p*XQ-p`=I6tNsmDHSB%0wTNf410g0yx$|_`iq*>A&PY z3hu|b&`gk(m4#SDVA!Cif|;KFF<|!xtpHSSka3VV{{lp~4}-v-IA~tSgbc0=W&tC> z2odPS?9qo(76*v1_Or1LmUa&q_(y{-;aea6m{{->_s{hG&u4G`uNoJ+%-ox zRrwoWkn(hsz+#0mx4*qTmSN$Nlf+LoemJj9ROo09>iBC$Js%LL<1m4<8 zoX~Br>**IjyA$@coKyz@1P2GlfLvn;>-n8Xz7f*BHmP65FY>05XMQTvW!vVc;PJ)? zhc0ECsSo)$d(QT~phErlr1v#HH@dGc$r#egcF{Dd=owXvf|3%I^K_j2{ufJx)^0?R z&Pl$19Iqy4s-U0%#4RNyWwwziTn^fIB!uZ<3OzMXvf1lZDn7*`CV?|)XJq5Bte%r1Qwh|h#Jo{j!FU)MgY0DT#=d6E>mw5JEouCeIS+L5 zG4z!cUX0v-)QSO!xKMj*`VdJEQDztq@H$nqR(AlHts=vIrUg{v;pi8bys* znKf{9rO0RAR6U#a{YTKjb_(>wItMH;*zGefxpiJ5iFEIqT9EIP6BEB?9(chp872Lc z_ZFtV^rnytv&s)d>TeBY6;h! zW{A+L5?tW0zIgG>a9ELLc|q;yl)pIA{HXSRa#=eOkT`hNKYjW%NdbE#7WT-Sr#E6H zc#(MU^x22H@M!I($P(>_(*v>{VaH*2k~DSQVk$2YsO2d3_R-&85xWlTmvj$8GHjPLl%M6h<_G@$`We?aIiF8-1J=1M z2Yb;N*^7q=&pEZ$?#0~axMnz+hpp^>ILGnz#33K^;qR z8XoLy`m<owr#liI% z-~jSaX8%0YKO^;XO$j}=v&4U5mbKqkoGk_m&0A8)3y1H#=n(F6WTV^q+)=u5{HAib zPu1k_2RQ{=sNxww253I7PcW{UBDiH8)Gj$XI~LXS05%ocZ|xa?#IN5o>hK2&v;(qUt=UiP~9n#SR^dH>X zU7#PK+l`i zPkLai019E?cfU)_CiK=KVtx}iqQP~@ke08pxjd%6<4Nl*FD;#AG0~V&3>-Lc*BN>J zVy+;&J^;)Io`&Jc;kb*UL6+m&{RZ>CRiHMVeRnHA2xR;#+k|!CLwt}9PdstU!BKBv zuw=BxcOgqqsTUeBNvOCNtI1V=^knutFD2Pp3@Mb_=fjWIpXrs*zTz>8Gz$Z!m{O&L z-jwv8y(v6lP0$sJsN$+H3VW*fAT-ZZd6C-8^*(~`wCUvZ@wOi$-Ym}0Tw^~M%1>Yo z$^tY`YX0I>lDiD*fniwuUXFBTLqE&{h__#;G=p_&#%=q|RoB$_dgpB;Z(nuOWeIqA zKhP7Ggy!Au8c<`KQSy+4zQ>3QthL{8!}WMwc@y0MrSo%yLbr_+1NZ%Ak51HDFT35z zStz(xfQU(OA*xevFjLYDF2x7a11EH}_Zb(Z!v929emAw$m1$bgmPnv8WRPLtmpBJA zM}8lMAvZ5?;_<;5W>xi z2M()|cP0gp+jv_nv@1hD-E5ZmPgxj)23a^xLJyb%=#Dqa&5i(JGF~jawG5F`5VC(} z8`>3sAJ8{45@VX^$>i}FlykCuu(yRklR2z?bdHWau)-`sJi@*or>L;75Ui#-23?$7 zTmVYDhP#r}3a_NNKu0h1Yn8#WYm8Ba)mGt-8gdb`@mB+?1}HQ~a3ipnT~!~h@BkXH zH_t@1+jgHr7mbqMS6tbJ^btk`I*}^{D+eo@GT!aBdQo+SV1mV0^PRyr0Mx~PJBL0D zl2M*h_wPY!p^Zq&!(p!y>XpzapZ{MVKV&c?ioSo*AJAI?plbj+hSkMDuzq_{6dJS1 zWuFIkrmIE?#$_!1sP%{P>fPM7T$&OOEDaDR_Lz{yiPL4!EFK*_>`K6VH~|WUFrAw9 zjW|-LzTXcSaSc#-8D?i80l*!oq%ASR$o7DJ=`k3(`5MYvRWy7=9pN@o*bCynMvvz)<2(v_sSpsegye&U21}nYY^|bQQtNUAA zTLqoEuf08BuyVh>5xQ0vmqu$?REqNp3U~&lpW)G5hNBA-$D&@_rL#%9fP?^r`?ir*1O*@n^7J zeFo}|=>7Y+M5klb@Srz#HDAElQ0Cr?^;iU#zTHquAi-DD2zq!=SUHb{N4+)StC~o&&hQ;5-KwfXCK= zVq;NP!M0(#+Wh1<$vqth`!Ogmu%oLh0ds6g^Gdfe^x$BIFer=@aE-8tw#(qZ8>58E zvJ-5&se$Hw_ZiyNLKRkkJ9`IhwCY;pK{aN}Jz;yDGpceMJYbh<&xGD7G6jz)*a%R> z$tjstFOSu=;e|oegy_%x#*N}Q>D-{Yu*L^vh)ic>NvzTD)h^vTfA`e}^h;%z@a~`g z5brXOpwU77l$h# z-KH`)RRN|SYaV87m6JGUKm&h&r5}0?F-np50)vH|GBL(|JCZIcRm60!Ws%)=__Rz- z7|{AqA+^p_)cNhW}Ok&2HLiYtb7}?mulA0NO9lni3=!3Fqet$z9sP%<) z=)J_|4>`-)lBV{r<5(bcJUj+^(vBCNn1$8wuZKfe1PrzsWRd0auIvVN11?!XBv7b@ zX{{eY>(MA7v>PtLByYxt8{0#|L;v`Z%~5Nn46}2O{NyN417}}NYSDX{sS{sEp6n42 z*M|g`X53tp%nT?yLWn*K z{RQmr6yfw0ne@&-xOkr86MJ4F4US)c8>x)%syZxxR`vHxk~Sh;PC^&x2ExG5*2Z zzMCzD|Av>pOD27B7}{iGu`#=8e!Ib>B74Bg>D)q}fDRu<9CpzQ6s=iPIicy#2D|#J zrXK`YariJS!&}hyx>>f9hXE%cxbJom(+@!=Hk<%L;Nm?qgV7%Ip^lJA`0@dW@dmC4 zBBBTmjtH(HrF#-TdK!n`9l{cy#KskgxMzA3+F3S$*@kHA02cxvy1P*$gs**y8#YYv z6!@tSBkacJjzo+}nfS1~^;a-{DJv;$OJG(dl2j3=3kY1)_sZrHO%I-z!YT5ko!<_2 zVZXp;qB3#bce~+BvaFSOXe*E9q>YeFl<3B4TVtjsAA3z&>Rb_ry3@Bd2^(3!RpVL9 z%Z+^|3(ziVPdvr-K5lnr%@8#|x834fm1CokU$o-6T4l#&OX^ zVXgah&&$YyCZ4_*Sl7?Rv!buRc3aT;w?jBq%aZ6q)X&^@032ImChKH=7E%=8APfLY z%uf1D>`=_I&@-3q8kcP_xkeU@e`82@gwqZ1nS1x=Awg^gJWIL_lqQzYZE3T22vPH6 z>DB@x#R>4Cpx7A`N>9&BCzmmhK!QOHgas%U@g{#P-2$Tyh--G$WD#}2C{tp)(n@Bn zg~5fFlFFT)F@{6C$^(nuEWE)?GsVNt%k*f{7~Yv07)+EpMR=dIf);uMCzM59ZqInb z(4(2@nKB3Z?RVGchg_AXw~J^m&z{Z6qvqok73EX4jSe=hm&&NKZsfUp2lQqH zDg+|p1l@;yPcQ>^X=njw1poon2Oj(2`rDrWaIVu%9i?Lh=bnbWgE3if{{G?0O0{qL z8O$LvGs%Z1kHw!Xxc8zn=!1{)wQIRwb>)csRSK((jJh1siyXq+2M+?iM^f~YCD~w*(e9UtM}~x4rcs>h8hi=5Vi@;Z;|Pogs`-!LgQ%l^mv-DR~k%8Y+Em^tj`UlWEq8T-dzpL zGh}TFS!!5rf4I__I>aw%JsX}dJ)KtOR=_QT8FJ$%|8 z3~=6w@B>XQZRoqoWm8K7^YZTnq&=>&^0SL5ZfOw#GWZXp) z365|qEiHlTlss7#1byi957J6Pj&MMbWwjf*s-qNtZm)?a19)Jp*YK)@0IPld`W0Ye zm*c3xElmxLD%`jP@R~FFC~ZThv$Tv1a+Js;)v^K5$(}&%1mWu>-*E{h`HatlDzns2 zE56h~dFFyaxdgH7LbEmvCFP8D%e)>=kwI5|-hg&mYRPVtZS&x6Dme*-Mycgg_@gS# z%qqvxMXT#dt_%rAevuY>T;7GS{*wL$9K-gWWCHgFaZ zSPXrmm6_Nl^Ezi#5-8OtsL z^@%1`%w5iyO#jz)ID8J253>sk7;yMF65`>FyI$eLXXy{~&jC|Zb?rIlWUuY<#ZynN zUGBD%E*BTe@~*M>e!L1QM@4Dm9mUdFaQ(qFA0Z(LmPPF#Ly+0gW%c_b-zRvMyELIDI~I#EgW%T9(!;f;Y!sd-iU* z`0z_OpRL^IF)d8(Ak~~P03{9_z6kN)@NkFSZ9D(kC)ZAsz0_TP3+?zrAMaldx=iK! z3EKb94lr~JKzAir$y~tEz(|gZi>vZ!#hTJAOdsDGPFl&f!NRAc1?Qlc*z2*kNPWo4 z%4$qLMbmj_a9HZb0t3f?{FqTaPEH0k{A$4~ z-A6n;x{?h?s&W)8oOjo-CtK^w^@Yt>UUu59bu@rwE)+!_XR#Yk!~dSY4o8Ke&eETr5Z6;0g*(!)`D~)^j;qYCt#x=Au;w z%5rseb?CVT4iX5*k)|GqcXtgPVNZkPl! zD=u&4)JKs+5L?^Ym`Ij^;ednV{QK?{O`Mo*wm4TkG7SN=7A%g0%tHh%qj zn1lp{LP>~=!?w!)^eq_Sb~z_$f%c;$@wLB@7yVY@E>t<7BL1PM?_@qy)$3Q78sZA2n%m*ZtCUq zR#e;^qm4j3O%EQ_*VXywWvf@>vfg`Bb~6oJZNT7^mzTqy9k~%&-DfmtKf|`AH^Y#k#tB-2KqX(*s>)+$=0j^IeN(QcCW%Pr#{O2h-cw*g0isW@ZKz z^mgp&;%A4~AiTG?KRmV#LL|54gJT&yw+Cs93a9c(jMeC0*) zo%Ip!=gJQeuGIXOdO;6QOM?RhK|zI+{Gcocn&R5wGfBzGbxlpa-rj9fmUI%4u{XZo zz~-wlk#Q=Y2NgI-R6I%$q`a{D`g)+$?hQ$jo;r1zeAKX~RAp9l*3Xq1g02(k+&na3 zvGYGBPj_5D+jhGlPfVxP+I`7>m{XBFwsm zZ_s~u5>ODXwCY|960#?HF9Tg8C%j~&rEki7ZospqX5k8b;NjuXx+MaxC90evP&q-# zclY$%+S;PKcoCQ?5t`^ZaKVAJ@a);MbJ?&Rpj(H5DLpYU1nPEEQ&UOFH@0VnQfEsd zj~qt^{^Z<=+3&0^;^oAqJQ{-Fe46-Nd+NIYIIB-mP>3`T8w+O4s13tkKPfN`b$>l{ znL2Y5(&?3k2_int7xK&IK~-Bj5+6Uom;c-BMx zC)n87&YeH+?d`oOD0AuF9*hl?@jCIm65Gv{`uk03X=!`{0zJI<6XpPeva)pX@+Uqw zD#0nY{{As7y0YmiM%Pa3k^cN;wagUXQ|d;1ZJ6y!V9?Lg(|cKCQB~+(yLAn z^|vZQWa|CzpON8J9eBKT32#^=D?8i!_guhXIl}Jvgx#-is*Os5%S=w{nswc@?Ghyojig*M(>yLsJ9SmEz3ablp$pGZ^}DhF|2~y%*(b@2Q^-N~U4#lZt`;q*RrxO`aqsV-gMb%4Ow zr=kwTp89_Cm?-6&@4qJKW;wB%BkmiGrv!Hxjjp@8{TxZa%1@UluD?9Md?NGHfg{Af z9tT$V7ac5m+7I9X$a&XeT z%+Kf6w>AxZ5k~}l1AiN+?}7B%Y@x7s&!0O7Mn~`@>t4=(07C@bPEcF%B^_e(fDLYD zlY*QU3xR#!ieR>bFIo2uVgYt{C!`hZCrh(US2>gTDh-mPZjM5PxXtnpk+8$eCt(8q zHp{mC)CR!aLxq*+FmSNL&>A#hq^XRHlUY_NN52CMnLRaf>yR3THp8N#hQN<5{*Jq) zkNlp1f9*BH?hH^D!J1S@P(2E*8jhH)A=LL1d_a^!qZLFTJw5sz5+ybbJ?!7x5mw1! zK3``Thd~gt^0XaMU`U89yoGTJHvmQubxVc5D;HUVqe#@gzqNH4{BRfO=!zO~pxyqq zfx<^Wo2ai{rrwmP&`&rs^{t~_c$NKsrGx}!%E|-zcs}k z2#F|vBGTa|9L;(2CYWfPpnjR^0UIAe)Y)%?0C8_&5ucbil1=O7>XcB4ije8|&v5du zx10|ZPwUW-kf2~`O3H&x`r+B0>|@7{MUw~f1%Y3wN3xAQ>VCL)W97LM|{^(w(DL4xn~g+TVY=srqm8N+>Ajk zt=oW9#KB?9Wv9#K;-yQthYsBq7A7YruaoHM3w8xA8kh~1V2q)$@yYN<7SB$E-7FY; zlcB#t0w z?{xbu*MXW3M9DfRC!hw3&ssti0HOv4^p*Ffy!xCD+Z$%7vIYg6q!RqBP$>YPSQZyI zw!OK6GzZh#em`L6_Q45O8XOH&%}ILvIHTNbhS&v=S?`O9?JQQ3Ap8{#EgaY`j?%K) zWW+p>kRT_&w+q>j?b4;Y4LH)e_+tdISz@sldwP0IO4d4ApmQ1A7qSZGK-#}(0GH(` z96USVP3yrfO-$$@Z0yi&89nJH801QwE%;Oo7dG{Y*;93`Q*S(#OG^P79>K?llXFvw zf56FYP&J+;fdh)GWWW)Tqnr$b@`K-5TsB3gGP~n% z7DyVC4#VLO@2=^+ouj)Lh>DZUpoC^W7%P&c>q@lSvsBfxU2p;%CLr)1)IL{2 zG>tR_ueJy?$-%)6wi+5AmSrknsKKNcsESw*UcSVyO8{9d!vSTC?4=&)F-rjFeNZsc z7bFP#5SB34D)fuKLwYiP5JmD>+_h)S1uLnY8pd zs5r>6qKU=V@x`K>Z%_B#&PUow)GMppfX!DlDak1%xOQd>(%|Bv$CJRGa_eH(ueic%ROMWrIMBtujxNs)HQ)ME&t5|WHt#I8t@lp$jg zm02>+kugIOnH5E58A9qk*TZht_p?9m@4w&izQ^m>|I}VBYu)P}uIs$c^Sr7}#JyG_ z0>vx!G7k0`>&>}kncwsyP_M*fhSp+JlH8zugyLSo-D$nrGgnYbfu!fo0X!3hJWENO@ zt+MR*Lc%1^w_pzS^g>kodGzSn*L1=KYmZv&5EVW4y+J=@DY!e>8`9e&i_tux<+*Z4 z>eij?N)Py1?<={4ECqNOA#*WiMscm+wr%D}W-!x3zg~o<;B}dQ4*OmPv*=YEQ$B4| z$1!xAe~*}u=I8Hs?wkwv6dd~sf%xac-?oMN?Kf_+>6id?3yc=A1-3hOVwI9Hcrz1+Y|&@6eBK=#M_; zAt1NawIOu8s-)-}5canNlR)%l2yav5f!lt(|Euy96Q@M^_><7kP+3ThUaZ_J!Fdb0 z>>eCR2UhOh;u3ei+3?h<2j7mwoyUCdhT(EONCWWHEO~|k|M=WE>(rxf-Yl@5NZD=h z%M3s;3wx30yK~>Xc>}RrPU~2dwIainQzPZ)SGHc^hwNDzaxg@wRh`YWF7fBqi?QF@ z&iqe!_Tl>q`<(pOW&*A_?h)g+)Ygs=150LEjm^uBI8rZw*n&bC=c-j=VB3H;12x>? z=U#A$8<=#eI*&6_!|7shvHG~wEB*ok9W^?HjI48h*q$gDxYR#97!aZK{E^MG_v=fa zc%?hR;l3n7F=`Ad)92E%DN%KGbvn;X@7=vyzU~{Uo4i&;;*kgF}slu^&Ff3 zPAPDz4tH8-44j(@KB5KY9zhv!0RfrL;{i-Yu{%0^3^vOwv9>gPe0V#k3>yZH{L41m zo|x2YCZKn#!ekZ(L#x@?c49(cdq7z6ATO_dT;=>~Qir^ZSl|AAB!@2>uP>n|%2d(J zU89CEhFGv<12g1i-fjoyblyJjCAOWe$UAOEm$ z;SZ~kt^NN2x8jGHnAZheB$H4ISEb>7vFvT`qj(F8sDAv zTu{qsv#A2hzrCg92v5URcaq`l7n8J0O$*`v)aSqOXko~RZuDlG;|-BoP{}R&HbHKxa;MOs>!LTYOxKw$4XeXi0f@zN}`j&+ZobTvktoL zS%zG;t)M;&;TO#G0MbFVaM4A0H+&zk0vJTM2?^c2b!+m=7nUE11_k<-i3F9XI%xa% z@86N}$=dXk6Gsl=fscI>+(tl|0W0Fh?-5i6urWB+tdWEX42-)YQ&Nt16oXm@8rTNR zczg&%lrb^9jtPX#1tN|0;hCB5|2WTRcLyXgkS|XPXJj1e4HMW&YcqV_m9DB z<8SmV6#&%g=jR9L|AQtyb>Y$f{1$yIfVqHYq8`V(GBcm~iKpqvJM5(g6%|1~`~gY> zaBc^{>uMKaD*Y%syTwpU4jMP$acFIGj|hS?EVS<>si>&PS$E09J_2Q{6tqtPXTItr zpjUQh_Bg^~2GuPDWd7j-!os(Fe4xW!byn6Li!~QetAG$Zn^?!z9kU z2JNzUZ^rpnNyl-MCs`owKe}pkDQ+)r0)KYjYqHw_RLpb39e zM!Xz@C-a5J>9jeZhvJXzh!kXGWEj|6YGe)kd0wTsKff7fuR><|Q*O$uVe(NXLE zGM+zOTzZ?xId%yzOqMe;HVjM#TsNDGV!^b6@ncq27Jzht>BqV1S7Q|vW{9dz14y|k zWVze=b#LFkOz;W+_vZ=Sm(=v><3|`Hi5TRE`j*(i@+bB-mnN<3y`(q&w(o61_qukN zh_Ah+=)A%5Ihlt^5sgC5@U*r@jjOVSGb~@8>oCTXN3XX{ZX;=FbE3XMq4DkduD8{N zDa{AB8z(cZ z<)z9GN~kB(^ev}yP`6hwpcn7`__=1?pBKaWlG{!=I%a9cy{fLhyO*wNCv{Mi`&?ub zlMP?R|Sxl+8GDr^&p7?8s5B9$T^FNR(AQ-kj&CME#Wqljg5 zi}dkv*Pil?^tRVXeLj5yZu%=Q_f<6NypYCudU_&%t}zk6@E@0TFfCk`_41`+;_AZn zY;E*wd)#VPk$&#RDjJ1HX*ZS@y40ocRngKfKE`c+*1@31Zd4NTH?GV@GKLAUuV4Jl z#2hkyNKIFVV`|UALAxQubjIO)$*7ycjs-X`(qYS7z5$*c9zs93&H*vb+L@ zq!qV~y7u(#|3CB>xqTAnSiWh~rZk%AeHpuSM5H;aNWZ*TsIZt=ow>woCV8vJiHSbm z-cek5%|^Ut(Uvu&_SG(2L`V%=u)Ktk67!;^Oa=lh zVqudGQH_Q5dn|DsnH(Axc2w4SgKeCDue--#Abpcu>n(%eGTw*r{~+0s&E z2p=p#HGq+cH-$1G7C(y_uz1+=KEy3=-?^iP$^GAM`FUV~=!L?|a+FG1E^Gh({YoOk z9>aG3@sOFMjwMSb=sNN%UFr}O7^?is$~`{*<;#v8JI>G-rtre3#<+~B1+ylHm^CU! zRCQeSO$2&bra&uFNWrex%i7(HA+0}+o=`Y5iQxvr&v-wnqk(d1R#x^Gx=u3RE}>)U zWO6()eubMOiwgfUwDnJd)*b+S;8h7;@0r1Di`nAF#gyXsFla{5=?y?wi|Y?^ev zytek3vNG|7?xO~DbJ>3`Y`Hp^z3}kxAjdRkh~&el0l9}bKv~^-1F;zpzai#-{)QNI zD^omlNJ>)jHLLtIj#hIC&OOVkThDjCZJC8F^CA3jK)3($PCHT=eae6`5_JLwOdFRCJ^3pZKQf6jy&g(h zwsEXpU1aqKNq}w5nj3fd1qGupD%o=`x9227!w;n=-3`E_nV6U?)>-)Ypi|emT(l6_ zP*YPI85tq8IU^o?*I=dFa5ld zth0>*SDOf`cZL(mtu_0k#d_1HUwNi+ewfB$LY zSw~TU>3zqtiC^B?#Euo8oo+gM($j z??A2d%ma?w+HXGyUHa;jBARjtORL?l_59DV9u3WLwtD>J$?CGU=H~L}G{ot1MY-3n z!o&&Ozi8i*8}tDiykOmEe&B$|w@)bCSbRY*2s>zAUVi`smYtP#+erI3s)2BmxQklm zZp!H9+2xZa)F@{mwL?vNo)ik7UdAYud~Bg24xPfr@$wE|RX7}2A%;Z{4RcFNWX!|IKSSmtP|SeqbA~2ddCtvA7r<){y)6OP+zB@XSmQrC#O1`f)0f<$%7d z0m+GA_+N>Pe zQ0&=4Lab$EH8o}~u2;T)ktj#1^EMI+FtZyM9w^Ad7Pc1}agFy?c>9BZ<(->3v-#(R z#5o@sNaSfOY;F~b^eVjACSC&`?lHEsu)vmrSLxxQ{` zxtAG2df5^{wY!5Jbw)L3Uf8U~LWyJA4p7bos=U>RM1mfZR3!83p}at0)xaR>&uSp= zNN+wi@sWOehIRiu1FQua*FPJoB3W;ksM*Jqd3_-USb%|#j?!x1@r=+`6C z+Wu1z-i#8tSns9&d~SW*N?jldK22w;Nmi68O^23i+(vIK5kp zzJEvEMGzD~u}EqX{Ink#5#;|nWl7-@a;AeTehEtDBTBc^^yJfj+~`8Lo<~;J8G^gY ztjLCb9K>e?|39vOn3yOcA(57!-=QzU%Es1?JRmoZ$?f_I-?h#jD*qh5b!Hc;Nm(OT zWRxPP4pmS{B*S9th#OD_#y)UnD~Vh3fA?GmlbXPlLFdp`aygh3uUER1%mo;U}Z_jZDc}%Rejs#*kF<~1=-p0?5Jr6~YDXK2V7sf9;HmX}D z4(Aq0HF+^HPUx)PAwTSF$n@H*@XAfaC`womc(KS(OhHlch%%}0%eAtbdHZ78$nB0E zR*5WJYxG!z?*jfjv=YRS0w5UKFRZ`b>WT++_7=g3Rz?Xb*% zOU@xh#q7DE=n>JlGWNkX5p}0AI^&e=b$a&4vfbP0!|YR+|IuxTFa{gjnk;L$VtINl z&C@>?LK-a}V?Mxpo4+%j^5hk-5YGrVUvef0by`_!HdJ!-wtT&J?25bI^|)F|%dpfn#hhUINon`Skn}_v`&XJ#Ov-!du_B$Hr2}i>|8Kj+dxXZxoG{}32*E0sY zn>}(n`LnZ6Ybh~|u$laa4Grs%kNj16&CbDanfm+xmmkjlaf=m%Id^Prs*lqKW71Qdu`!c7p0$qe@Vnd14}g z;i0B{?^`9p2v^KIu0->sqkn-7v3j$j=Lf(}t>k3GEB9UaL#rUOEa?y39%tuaiGFs*MMUg4}81b7YxN`%y9Lz9|T6cI; z=_t6nLdM$N?k%#XdyNOIMK!N)(F{!DrImdt(-)47hVzO7iBkOo<@ZdHYIsPn)wjfA zOsTsoKOh)4QsgU$0XTX2>4OIZ_w#IcsvL=+rk}2an6NjtRO-@ajX08~da`=vDul1Bt}XgZ zE2~&{eli+4E$~9Sx5)XFnk~O%wjZn7^0%z;+$iX2~BWrYW@}9v(2t=4x>d)|3U7gaGG$~&d zXFLF6^s5fqTZ$iuAk}xGUTaML@}nTW)*KO(ah5VL%z|ln-uI7F24UfW(qV*@ZHjJ8 zepYgKRXX#K@jFeU!?DkoU471~d+hzl$cA<6&=?HU%4m`^F5tMUp3n+(z68YC5W~`a zR#b_#Mz4Fn=MLuD8(SB{~u;cAzeg; z1Qo3Gh8Xpw%yBL{P_obLC#HU`_>LXw7IL6iw%v0N%P~0uy0?J3B0kn(wFxoF?7R3_ zNjlG7ieG!xF5Ju?>Nlhx&*Zfs%rKPZ_R<9&Pk-^}8o~V+!pJEvQs-~KLHX+Z%FWW! z&4yx1zAL%qnx#%JjaxbERR&mVa-{PI%-`+=qH>=V87cqjYVs_qiAp=Zpeos7iSKf@ z2B7D>&$^xsanHX^bRvipN{X6zAQ|Vsv-Qu}q!-Vfv_t+dUpsm7J8H1#kKoi>$<`aA z{#7YnyM25}iC{iQe4f~^loX$k02}P&H#fNh_|Mjqc2sG^+^qDNz7uV&Wbs&qLD^=x zAo&mHSc~C!L$e(AxC0Lj%cSY?F&0>0U~_5x^?nyN`_pqfdmw>LA3Ak&!`?ljqDdeJ zU`Hp+Y(vCLu+9kS1XNiNY^olP_@)F64xKRtxMCVI(llb7!2Ooj+A#SHiL~j$ zVTFAfthzDECu^#!fkkRqW}{5e@*55EC$Tv~Epyc(57OaLa~IJ|9?X^}Yj+E&zX|ou z3w*NN_#XYp*2CS8oB!~~C-&YNusz;--%uDf@k^N)4rbASg^h^hMWQh37UpMq5_tX1 z&CSrPdOLo{r)=py1sXH3O7I`Ov)W+aNPO>oDo0C~HvqDTYy9}>lQ&JDW6>~5bScx=O2Q|fubWfTVY;0Z_0=9liYe8% zXU}mZHoj>2Yk!sPnJOsTSz7zk`auI!*sQ#dhgz{i6^)0`o2mH4i&8(&b17#Uc&qf9 zTU$UFLCJ(y|4F_1A`f$C6nvB38`PGw?$k*d72kO({(D#6CjRwo>&_CV8F&&^9GdyR zo@VS-TK?`_AJ*6A3^VQ3wU=?3{4xDzXKQE5A*Fj%wsX$oP5b+ib9{t8luc;8M0H!)I+3%d+LmZ+dwVt;2gq9$#`QsyY6u z-*fl6%oU7BpnA%7^4T{ZKNv-pcZhn)3Tk9(xnEiGWQnvGB`LcQTeDqI z5S{FwVn)tScOFwwAwM%+p+~Zx-qRe`$~KsN7ocak!mvl4b*jQ$|0F%Df)BOk5}I0C zKTr!+J$iKL(e7;HN@=?SsLzj~AC=$_g4h5yy0A{Fg8n$P**f3*8Oo~jn2FcyDkSP> z3Z-GY%HX^V9G8lQ?j|}XfDtg;@+AL9M1^$}$Cny+y+8sD3@&#`W>*dHRQtU5U;*pe zweL`W7^Ace1x5VY)vGHwCHkOMM=dTbWB5ODoSt45UX@ioU&^H!+wm_i(=jEQ(c8=v>M!jzBv#32N#J)RWHj3@OEv*^n?cz44*e~1FMl_s;eOAvU ztDGUplIyZjTnr z@Yk6wWO!ZOr>W^ykfw|F?A1w3YVS3wKxR&`bNXvbN=k5ArlqG74Mf3P(8+bUaD9Lv z(C4cCD&LCJ47Pe>t9X+m1#FeLh)b#MBe1PUT!=Wy6}#YAl7rBXYd3qxqGMx~PwcHA zx+n z&1KGRrWPA09R1%-5h_8LShjWNK&UP!KLq5GtjqmnQVh zB_3e*Yt3@zpg^jf|D)!!SayBQ6w`>=-Z>q8{cwkk-zUe${61$9&F4^ZTr1|*y35!q zauM|?yf{3(eg_cmovHx?=j^kzU%!3J`@<1H8HN}br}9=YSO7CMHJVH0OQF3w2hqQ(EX0ZZn*7C{+Q3xC+e|wnMYG97g!@39D7zz z9unkM>O5|k^C~%6EKlFc(P3PDD*bJDQJjU{)c`H&^0qee1u??*9Lx$4{ao`L(O&nk z;;qRUWiZvApNQSdwh~?pyeD44u)Ond=SeSYp+Zef!!~hS0SQV;O{4nD>tA6m(GvH4 z2_0P}8;>$-Tf0S@b@GV*I7){PuU>hX=q~^Q6GLl!62TNc;hM&fu!-rV{-%nKw@$AI z8HAH^hks&*{7l+U17)HO&8!j#s~b3=y)ObGOC;|M(c9p4Q!~)YfhgZSpREtIpEGm5 z4m0?N_2z`gysOJ_%nH$E<}lbdxA)$va-(W{yVjW|k97fo3BR}iv?N|gB^%^aB?K&? zr(d_6u73=Klc9ThyvM@9N;J%I0n+Ru|N6*}@ACz-p9QP7{#N9yHP%fyCmyWm%RLW~ zkr#XU-jTX)LAZBpbA7A!s$L-NTJfQsL+4+$J#!Ds3(%U8RR$w~Fd}h!oBl3g4|Ci& zv$lxWjT_kY86foQkl;(-z{x3~+cDn6SN)XHu5yFd(CFx21gq0!Fr_T|SQi8eH_D8b z(a>3+G#gwoVfPc!3^S$!#{ROx#$`w(LsuM@>1gT>rX+lc&=BXX{yR8N{%6TE1sDCI zsR{}Tg21$@_Mj!QCINvXx%%IqQ_Zliv{-p^y_$suyiE3-8!1O^SNFT-f!ITFb)2xU zc|L8tbSm9+Sf{8)ze+GZ1nf+>WOy2{qMun1Y}9|AOn+rL8O0(XdZqVH89IjJsK^8x zw0A3D>JdWWO<>Yfn*I8+t+I0683pqRM7Zx$j0x*i28!gVLO`lF^R6t`3kjt2Vl`=c zUz0Rt`_)v2j>zBNe|~-*M~wcTV+XIMD*pzTdVs!pW(H(E@PD?Xuj1mGhmlDoBZ9pT z0|V!>{Qe1pT;X?>WZeVHnq|wD&ESpWeaHxU*jZTokvw!K-EEQ8?R~pVK!7=RV|sk! zSqM>tBU0^$ck?&DJLh=y()8fF9gvM6^0=jlDI40-q8r!LDw)&xRzsh-J}gD5&KU2kQB;H(O>`+JJ%T==7BOY$i!Nff{=4YL>#+LTl=w@Z1`;jph^FtwIAxa!Qb@q z_J&$0QeAb6v~*^4^j|`BG<1bJzdX`A8Eq{sc1}*1I#?woASyr~G}S%KjJ}_Kz1ML! zueA#biEYhU(dg_kI5^1EdPD`a7Xst!PwrZ=K8W#V0RU4=acN^Y2C?ll6eR}AD1p&U z1Em(xPoGw%Cm<^-mAJ;ls9$_#@r`+jH(tz7DZGOdkcIU3_iLU%p{Dk*@$4CzKR!?qSNUaAJX*S8qCe*}Zz%XX{QCIZJP0=Wqo;+aBFu z_}k+*p$S?VQuumJ0&Mp5ME%}YdsKCIE$z!r%zUYS2@YRsmFk5AXMa)6^Lv4T=br1< zYhv0Oz1)ADHmUK;-(N-5V{F^va}Xt4vOXM_%mrt>GcbV9o&Fxc*Js4IX@i2OD5TWV zDh4~~vrqyB8wQ&@ek3IBMnu`zzvtoIvIW*zB08zjpzjD@mVDFhcc^D|Ij#7kX(Co! z{7r@440cG7pzT|8U_HMmrGHFons6 zN&y}6vbJoFOU6CP@||W_35$!!&1}semNXcl_2xa)2hZO`28;a!q_dDt) zq{qZu!-e?dgox|E-4Xgsj^%VB+%CarU;Y%;UK{=09^a+Q zRfFG*mM25bRA)sZ)&fXL1>Ue*mO^3>*P9+|k=3)(_3Hs!+q8&?2%!5yLU$ulTmFsA zR~*)X@9tZ^S(Lxok28@)5~7l8`UK1Z2(`^da;vHAXESxezCry7$m-r(Ju^VwBez%nX13v$=7?!*z~mTty`$5oIi8s z_}&msaT8Ddq`*LfY}<+si_ehQg||1<+dE&p=yzxdU98I4Oy%Ei@xePPY|_BN=THuhD%>4?)Y@C#F$~lXz~B%`hR5K zc)$gbXvR0iQG!5?&K>XhkvFM4pwe$9_pH8B#P82J%>3J^%b7?54No&HatG24<`#s& z@R!rsb5dJd+G)ak@7}!$njsMpqWT9OP<6chFE`1pfm|Iq$x{2ezpqGKOZ=MI6^nSM z{-%G)<-3{gd&}{NNuO4GvGWBiL*)yoh}}uPs78>Lot!pw>4j}rhi3C?V)~+@#+e!?#na_Gn$KdoQ=4B<{!UZe!-Zxt-Iq@>FGyB zGN(mCukV!p`Hu>}Fxqv_=*r6%3TL#9gL;{PytQnPb6oTA{!@%DjZ5=hvp`ri1ZOT| z>}%lJqe!B7i*piYZ`KHjx$9yZ?5C5n26slDc-anR!gl&zYLR<^$gJGr2s;H6#dQ1LuS z6*y(Z#@@f$$+&>mzn>oY8aSHveRZ2WibjcsR?t&|e2cpI7q>lO`8n5LJxVp4hLVSU z%^JHWy@z>3`42cKig=!U@$a-?TP?I8eEtgIgs8Z9szJf!*LR3i3%ylA+*}8_2pY%KH8-1EwL_l7yLBt=k|jU{ z;>lTR)n>n;Ak~s1S-15ywY53m&hqIK-*>%&%P!ZiYoKd`O^?#{<98$2c03v#9fiRO zS?+T8Rdyg=QEgwRnA8SO<6RNfvmb+>7^Ki~*h?Euyh*#4Cw0%(AwRt}dc;oS+Ebxs z*y;JM-J!Ut=I}$Nm1>(j$b4SNeX2jAmDirlo1@lAMS>!s-Es6T-Z*n|2s6@O&~33q zkxfo!@t8|oGZ?SgZ%elm2C&EW&THu3f$$EQG#Z_u0f|w?ovl#P4qLZFf#t1r!}X-D zE~JO13tXC%+<)9@)VWgrgrPmxus!ISMlH$kO+Ycs)3x*iX4fSLB?MltB~-@YzA3Dm zrsO2_Q3eXE2gMaFvPx$}5l$L4y_YV{wOwEr57@HZJAc|acrYw!NU}d?F4K_j`~3Wf zt0?`6X$Cv}AcZOM@<(?(-NOhS0T}zXNrs4KAKJ*8+&I~3xvhD77PZn6f^J%*I`rog zRCWMy6LvAGsy>xZ|H?bDD{Nob9+#F5Zz$7*Ue0nQg5lvb*u1@J{{zi9Ho|%!5ngDv z*`07AfE!D@jr(?#RG3yiLHSNZBoUa=>_rHP-b*|xgMWi!)bVbUA+G4Oz{<)aGfx(9 z3cKtZmFcN@DpNIDr$Q#$T{M#kV?}VmT~VL~J2qF$Yw+}K<*k0f?qToNb8t97E|}Qi z2KI|?eyob)!kZ9zESYKxbyqY+V2gUP&fM@zU)8U3b7e1ILL~-vwXN>GKlC3pti2l) zq9Y<>KO7nqeHDO22*}Ap)_RMD6$<-D{rwe*IKMse(Yc)vlW5n>6P7?7n~uVr6aC#HiNg1U|czgv4WF@xRSjM?eZ%0H&q+|51qNz-={L zW9ysR$asAa2cSnscVH{=U=-BzUa&k1#Jp><+zSftrybkkE_FJ5xOMj~>sXf-+&ewQrYgrZpNkWD71@{L{KUQwWhFvKX5iGn^(zUayc-kD}6HTR;m zijX;$FqmiWOW&V{XrOugWa0S*bRfRBb$7K?zWHI5=90^$Yd|>{;?=?f8xdNYj(q}Y zCV`qMO`4i~an=x-IVIWmoh?#b<^Smrw`24tIFu;yT&1%Q+aj)>oHJ&ZH3Fp&RGEYY zCo+d8O#^V`9fE-8Nfy;^nT^12BrEZ--XBLeR*Wq z6h|v77-kw8G(i~8E`G4-gCchnSO>R23E$Y(9rNlS^Jp!D z);;_4?tZ`Bo#`)GbCFf}wd8FUNR%elEyl9)nI0dh5pKP>TKsEV^Z(N29pFJ=xa*kO zJ`;(n^%%tvVIk5Tt#;Q#u@`CA=VEjPC1_LsHAz|^XZPYl5c^eQqqyE|_NCnL$gi29 zX_1JqZ~}l8WK)DpT2+R{m);>ODh0S-$!9p&shcJNz9w$uwYe+}%WW zC77o-yw?!T%us;<>zbW|IZq$y;aM>bbB2HZiHZc;C){%9s-rI&d`K(1FSY^Y(2G|# z>7ZAXot@-R-HC-9(ItZa*Gsc$bFklmnEWN@@+xHO;17ST`D(e5@?w2+DyJTzQG*2` zz*=hBB;33#I5=22P^6c0^XARQ$n?_=OyB-HI~J>leVQbLo&1I+&y?8*m(v{}lgThg z4ty6P3?VsOFnZP76&KR&vfyF`bdGq3nd-8tT0}2-N9Wg=QShHWYU!VYw+8ajloY?1 zzGakb2+8;9m|t%n$37T6bAn8612jJ~yea8n(*RDw5b)Lw437PV83Yl{so)F-b=$iY z40a#aMHp9BS3{Ks)Z}B7ANR59hhM&Y!CTwd*krc-gHZhXc*Dn&{nta`8`}U;){Wzr zP^ki?)Y0`XzE=?_YN0jI{jjivdUI5_pl&*YB%gwFl;7>BVddo{kt$auW?d+}C0-iF zV8^_@oT%IA>gu9VkfC9heb;I*=%F-$ateSv3R+(6t>v4?m-fIR#hEfn@@w5;5OE2n zs3cMocW6BwnsDSd98ack+KqY+6^SHmXRWvnuQ{iB2;#d*6c5pfpsTrP?Rhle9F*n4 z+cyy;P!h?_nBFW6eSV&e*y5AwuLM+voBDA5|8UbVo#YRDzyJL2OZg8HDRTL~A|`}> zYhY`QAGcOypcDImo6}JKd=~FW;l<(|@$-CPvS8mOe%y2yCMD$!;AkgInRtj-DE~Kd z&EJAX${+vmrYsDo5Ga3qO!zK&EQ}NW;tkrx{Ga-3cMB6=7(Yp`<90H+sq~*&d u3=idhk;qFIUz=2T70J>6Wq(=zgZ`b`U%`i^KZFoZe)yoOLgIcy_x}YrqHE*; literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.uxf b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.uxf new file mode 100644 index 0000000000..6922b787e5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.uxf @@ -0,0 +1,413 @@ + + // Uncomment the following line to change the fontsize and font: +fontsize=10 +// fontfamily=SansSerif //possible: SansSerif,Serif,Monospaced + + +////////////////////////////////////////////////////////////////////////////////////////////// +// Welcome to UMLet! +// +// Double-click on elements to add them to the diagram, or to copy them +// Edit elements by modifying the text in this panel +// Hold Ctrl to select multiple elements +// Use Ctrl+mouse to select via lasso +// +// Use +/- or Ctrl+mouse wheel to zoom +// Drag a whole relation at its central square icon +// +// Press Ctrl+C to copy the whole diagram to the system clipboard (then just paste it to, eg, Word) +// Edit the files in the "palettes" directory to create your own element palettes +// +// Select "Custom Elements > New..." to create new element types +////////////////////////////////////////////////////////////////////////////////////////////// + + +// This text will be stored with each diagram; use it for notes. + 7 + + UMLClass + + 819 + 308 + 147 + 56 + + MoveGroupCapability +-- + + + + + UMLClass + + 917 + 420 + 168 + 63 + + MoveGroupSequenceAction +-- + + + + + + UMLClass + + 742 + 420 + 140 + 63 + + MoveGroupSequenceService +-- + + + + + + Relation + + 833 + 357 + 21 + 77 + + lt=<<- + 10.0;10.0;10.0;90.0 + + + Relation + + 938 + 357 + 21 + 77 + + lt=<<- + 10.0;10.0;10.0;90.0 + + + UMLNote + + 980 + 308 + 98 + 21 + + Plugin interface + + + + Relation + + 959 + 308 + 35 + 21 + + lt=.. + 30.0;10.0;10.0;10.0 + + + UMLClass + + 763 + 532 + 168 + 63 + + CommandListManager +-- + + + + + + + UMLClass + + 1155 + 525 + 126 + 35 + + plan_execution::PlanExecution +-- ++planAndExecute() + + + + Relation + + 1204 + 469 + 21 + 70 + + lt=<<<<- + 10.0;10.0;10.0;80.0 + + + UMLClass + + 1155 + 427 + 112 + 49 + + MoveGroupContext +-- ++ plan_execution_ + + + + + Relation + + 1078 + 448 + 91 + 21 + + lt=<<<<- + 10.0;10.0;110.0;10.0 + + + UMLClass + + 749 + 644 + 182 + 63 + + TrajectoryBlenderTransitionWindow +-- + + + + + Relation + + 924 + 476 + 21 + 70 + + lt=<<<<- + 10.0;10.0;10.0;80.0 + + + Relation + + 784 + 476 + 21 + 70 + + lt=<<<<- + 10.0;10.0;10.0;80.0 + + + UMLState + + 329 + 553 + 203 + 70 + + /planning_interface::PlannerManager/ +CommandPlanner + + + + Relation + + 392 + 616 + 21 + 63 + + lt=<- + 10.0;70.0;10.0;10.0 + + + Relation + + 434 + 616 + 21 + 63 + + lt=<- + 10.0;10.0;10.0;70.0 + + + UMLState + + 168 + 665 + 126 + 49 + + Plugin +PTP TrajectoryGenerator + + + + + UMLState + + 546 + 665 + 133 + 49 + + Plugin +CIRC TrajectoryGenerator + + + + + UMLState + + 364 + 665 + 126 + 49 + + Plugin +LIN TrajectoryGenerator + + + + + Relation + + 210 + 574 + 133 + 105 + + lt=<- + 10.0;130.0;10.0;10.0;170.0;10.0 + + + Relation + + 224 + 588 + 119 + 91 + + lt=<- + 150.0;10.0;10.0;10.0;10.0;110.0 + + + Relation + + 525 + 588 + 126 + 91 + + lt=<- + 160.0;110.0;160.0;10.0;10.0;10.0 + + + UMLGeneric + + 161 + 539 + 532 + 182 + + MoveIt Planning Pipeline +halign=left +fg=blue + + + + Text + + 252 + 637 + 203 + 21 + + planning_interface::MotionPlanRequest + + + + Text + + 441 + 637 + 210 + 21 + + planning_pipeline::MotionPlanResponse + + + + Relation + + 525 + 574 + 140 + 105 + + lt=<- + 10.0;10.0;180.0;10.0;180.0;130.0 + + + Relation + + 686 + 546 + 91 + 28 + + lt=<- +<<uses>> + 10.0;20.0;110.0;20.0 + + + Relation + + 826 + 588 + 42 + 70 + + lt=<- +<<uses>> + 10.0;80.0;10.0;10.0 + + + UMLClass + + 147 + 287 + 959 + 448 + + Motion planning +layer=-1000 +halign=left + + + + UMLClass + + 1127 + 287 + 168 + 448 + + Motion execution +layer=-1000 +halign=left + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png new file mode 100644 index 0000000000000000000000000000000000000000..724cdf19115eaad006e7ebfa97dcbfb3afbfb950 GIT binary patch literal 133824 zcmeFaXINBQwl!QHTRA2$f{K6{6$KHM3@Ro>K}3lH0t%9moG~FP2#88hf*8m-QzRvd zL?wp;$vNlzjZN8oLZ80(zR&F+@B2N?kMkTtRqeg@T62y$#+cJ5m&MPmS-Ev3i9}i> zdj9kk5^3c$iA0n8>o53;l#osWiFB7Fdivy5^V(jDuIsfn8 zMdI6g>T}M?Ray$CeM0WVdGS0vY&-aX%fH3rbk0pr^*p{UC+|PqyD$CTFFV!ONRaknTd@yQfDQiQ%PaVaonAv?A&$3GWhI z3GwABQ~Yfi^?ykxY$f}D{x7L@OB?Oqf9(7J@{f0~+IqLF&}KPtGuwGZuaUOPmHv=B z=&8DRcf2B3Hj&QLA2p|66|kB3$%$gutIJ4k%hvej+~2?WB3G{6*OuJ9dGdiq^o^&B z%htO3fBrUseR92;HecV&xVXxM$x2E~yYu&rmMWGkB|fZm8TE|}zLuz#OvNgtS*~F` z-s@}C&uiRS`lO!S?0nKx@V1X3A>Gfo8+b6d!}*Y3Ncs0474I`UpT=X@dR|0CL|3=Zo7YIZbzG`}>vDf-ymnPfo{L}jh}JQB zR>OdLmC3=Tv9U3)Ropt&hgGwG{rSlM@lI@J=WEieCYCW=FV&i|8b~uMyTtwK<;&HZ z5C6zz)UQu5({FDV^I&Jjj-(sWnL?%w9?{Ors{sXEgFDG9-2Cf((QXT`mav~>mg3FWlsnCX|_ za*e9du%SMrjhlY1?bfeMxH5%9EXTCUr^kBenD|C(H2f$Q9X@9Dch0r9?>gsf-4|bl ziyZH*F`DU1Y&5OY6&2DlqYIS|4+#niij1Ty$ZGT#aX9;me>d-iuK}0dWF|k%C@Ly? z_Uu`el2u=-#mE(H@z+PYvgm}Jy4^jQWz#IjTz80^(iT~oDWp;Cy~XHl7=MYSl9$?Z z10KHP^qiM;D-JdFMXTjy+9v4Mxy#3x^i(=>r1KmUz>OFaHE^)1)> zU0e65YqrY~KO55Yma5 zxBjtqg*$tOYinzhjXG>cznweh!YpV#X%MRQAZX17+l}$^!-fv6YdR?@6L~Cl-XAu| zA9~Vt&&|y(p^a9OrhD-A-YwMq_deae89T^_OQYDucJ%#dmAtp=zg!cZU2U{;(_`9N zS{Kr6=f0r0%}+_c zImZ-~_1^0yb;~?EV8&xRJGn_&%qBi7n42TO>OK2|)*_)Esnx_ocpLQd{BvFDKdfHp zNhXuq+uIWxZLP8&uhYO;uL!yJ>eVYoZao3>!5fo|N`5-YBG;}})W&Ob*_}|KkWKdN z*(2tDuqIJoxmwrUieggbbN+Jw+OnmL$90khGgm3wPPGLD1mx!CdU0vwZrj&WtY+E! z;n90FwRL6XL8hkvdS4K0ux8B~wLIs(nph1BizyL0L1Xo=O!pBLXqoup@F0#IJ4Poo zdE@pcQQi5>RUB8PrA>P(4;lGPPfv@!*gMu!6^ZBHltVV@5#PPf>~Yt<@_T1pn3;sa zgM!TQDAj0Rnsl%#^slY(TnrKX(*33P{$)tGe6&QBTfad74}X_ZYw_%@KZnr!w)FgJ zxlU#c7J z&X!kHF!7rfK4$Qbwc4`JjCeXEp8eEa_&HYgF!j_JKBAtQ!GrV{If*~H>%Aj*B?9vO5X=8_Z$t7{@8?=v(0oxqppEwtqVy=_K1zy=%;JwPuYSo- z!q2yGsjuF0q?Ki1h{c48d2>!AFD0F5n3)@}TVSvoVptdvUXYTMjMk&@F%ejubmFrV zPP^*_LNWvU_039QN@?VG@0tgjvZnt={Qhoc0N~g5jBv%2gn)o{0?8yMHdyvh%o@WL zh(93h)A`CoA`Mpq2qDhRH9K+yc=S?I%u3&$%vhKkFrlLCWfzGHVOBsKQJ$+%j#o6F zYV!!p7@)%Rl`e)fq`TH8fNL=^F?MrRcDgf?z;?p8-kzO{AG7k+;-8F-_$^r1#%an4 zYw750J9@j+ZehZ%XAkvH+=xF(HU9DG-pL2cj~zdLHhhry^RF-W^AV*A>F)gx#LaLZ z^ee_{lsL;Z;eLSM=8N=g-$yRK$3B6@yD4c%vnml)$33Xr5?n;dC+-nNf~!X@J*T&k zbLrw;-BsPeN=v%D@174nnD~17$eV|B!vAlk#GBx2-&VtB63Kpj^?quQe0%%Llcl7M z7j6VnG40)V|9OM|tN}LWfA+zD)&TM1Kl|YSziaTxk9qAcB!k??!bHyFG5LOxG%tLV zbhmUB@!$n^75!~{2Q6v)D3O-8mz?KX@&y_5*ED=b`oe`jHbnY4#X)>K>~wU)U2p#X z`lUH05=Lc9A1VN#FpxIU@O;;~@$L%kPj{D~WLApTx|ZHpRy-AJ za3$-}nv@P-E7UBl0W{Rl-nV-3voH5LZ{t0pSDyqRF~e`#ds|bJ*Wkye*T=L2#C={5 zOvH^k8r(+>6dxaN)LwY!AD_H@@sszN(g_THiqkApm6lC1P%$wvL1~)Vc*c?bqdpUz z@T|eDTenJl1v%12zZbpX7M^N-fh*}L3oLoKVuNXay_dxc(WLgjGD2L~IQ75J3&$!5nv?o_2@!&}{zoGO_Q z%^GbIgM+*Bgy)CAy>Q=Y&K|8})JxhiviF5Sk>vZc;mR5FrT!v*HsduusJ!x+%sxyU zHf;T3FdCH)Qb8|cZoEz}<D&1EmDcjxMuG7!#WM!eYj)~Vwu$gesV5p4g}|N&d+){f2zaJ&r%RK zbam>?g~io$cXtn0NK~>Js~~Vxa@Q?ptSKkUC0~Il&?pz|;!yd6a4O$hm!NADy@a}N z`5b?)^B4YlNP4eSnY$hF=W-yWfu5`a?JeDH@+I_Q+dQ9goC zv1~c8e?Kp=lLJm(~QyYSqD)KtGR)uwfwA+`!9%UC~Uro~0rG;kU9JbCN=JQMBMt-7^wK@xe-7$*qi+ZZ7m zy>m{Dp3}_D`4#^t%KG5o{bSQ(Rmu`qlw4c$4yx)|N}dz=_RY|tly?>VL4$vKOIjA8 z**<-{85-fq&(2Kz>9fNHOKE6RhrhB(1&Uv?AzM!k;|nzE(PPK#Gn!07Qhyk~gmMW*cWo<}!)@+wG9 z6fEVntHk1~WIdvQ$8&TjJ;xwcuqv-qgMZj<`p%%S3-TZ378w8d>P1Cu3TMHcZ>7yNP@Zn8E%*-Z7Ukan_2cMej?)~#DnJ$4j&8SPADS^6-*nUTAH zrg32&yH{Rbo^~5glzP$YV5vagfcb%HabLufjJdjuBA?LC!s|EG@6-jqY!MVd(#Tkt z$yjh%m~<)aMF@2%5f~}FCZqT&!-%)(jP270n=1XlX@O@22W6YJhTGHZvN`o^@7`VX zx6&a)hjYvpAKtxtM`hbqF|Ze2ANlB^v{lg3800;ucrfwlsP=qE7H(0duzfj_e%zNX zXhdFlqel+@@Zlzwkh{UW1+^bnT>^-DPPN>JOJBLW7khH5;j|2~-0J65AMuQ}!uo+E zr!P}eX~Ri~QkhJ-{q3D;U0H%ct?3U2W$Seba}|-cuaugi^O#d`g3FX2ebZu1tTz;^ zTNq~=x2U=0VQ=H^;lZJl$`cSjM36qMuU4VLrvC7vv3Xd*;ba`6Sv_sV*h z>agc+W9y0kW@?&UKT#VYCnF;xEnNxXd31ViW@2HqOkzVroy4t%oW`W#I}JJ9!AnPI z#ibVG0BMCN*AnBXMN)OsrcKaLkVl&akiN>a^-zA72TA@Z|7?hE?01bp*#G|hyL3TT zL}jG1U1zC(iO@`MG5dsV}>$-1moL! zV5N;`cL@S1B31?j2VDi`W0g#sJH@y3gvI#UR`x&hTP4}|q;_}DiM*?{9|Nv9PZ%U! zZZvOsmf&L@GglMM-dBCynOC|}dzX57GY#9A>Guz!D*YPlsSEQn#HWBx_7Lsw3p_H= ztp8z(XF&D!M&W@Us~r|0nXBv34JT*{nD$=ube-BY!6p-NF_PC}xE0(A+uS#7P7vzi z+3x(vVLBA@Sa)b7AcAS(N#TXr5l|$BMT}dwe%B|E(nyK0@cesem*#iJU3gQlcLy3X zghNuWM2V_7Pe<$83Orm`gwrVj3ALhEX@KB{z(3c=X-a_`$1`B!x`=(Fsi#+b-LUPI zjL<~A(Fz5|#vceX+()}^M!p+;-@JX>(YZIvM03eYms0kt`CO|kE2&KzGTIR$DEa1Q zUj+1naO(1`U%qCmalxU)*@BGO0t7W;U7g1?^2l*&M!fB^a*@wGCAH0>GV65f5^k|$ zH+-2$pGd=wRI-~J^HR<-;A>Z$eRh}~NZyCi7VY%TB8My%@l{!vVpSt*+(2>Ys{qQa=7_>$mZ;qFNz;JY&f&{UY5) zORUb!vPsGCXZi-414=Vl<_8@l1ctJqp=>ju4^Pmo@Pn|R%k8wahjz7bpp%KToP@CU zL~ev7&xPd1teNq?Rf2>j=ZsVx6JrR{ovE|L*+et1-LNn^Mv0L%W)tV` zEn($#z*dEO%)n7`*i97-HWzU>t`q-g={D4R@?G%?~tSu9xInZ47@`#9jm5T8=bt(E-axc z_sO_z;m-E=3EGc*S7douQc6qLs#|um?W5;V640&;EA}y4q5r|+C{9Uvv;|Gz{>3`z z?n9&-zK*woqJDi}e~(Er5?5 zyCd$j-cfs>ad33F**j1Vcxma}sCe$j8v+dm2A&g~-h|C3Ud&G{&1cis& z%ujV}NEkoeT>H|8-^{j!vGI!~@tDUxPH$ipzbI50IHqDi{h2Ona+`+B}q*c?^EMl2P*F zGtufUL!iKyesz>TFzf7^NGTUaHRp;1g&5NxG%VJa<>g;bmvlA_K=&m|vW5>8i@AKU z!ZGmYWKc41D&WZ2wzaalng`k3voe?49L#IRT%W#3R;Dznyg_`T@7O_kU4E$l5WYt5 zoeJE;(o;5lSV%oA_N(RMda`H7v1+{tV_jcH$9n8du{52sNyRk+3oOhJEIbMxou3&X z^y`H|x`onve@=6&rBk%o6%u-%vPD$K+!E_P$DRPNfPiG&R~z3a!|y$v%72HifT{Rv zs-xz2(IS2x${*MJ24Jd)18pCQflHT)9?%I@qrMqEhx~`?svDclPsk?I5g6Luo+EYe z6w1~_JxZ<3**D3l{{AYx)zK;)ZEbCIf}^Eg`^_E;r{KNjT3Lm+2w9uUn*SFdT5Cd9 zH|3))4FUI|#a*!DwjIpgAmPL;NKQ`X?Geu$2BVG2-OB6S>Yj&}PQ5{a$c&c@mFzUC8@-pQ)%Bir+v57#hSgzxB`J)@7@F%|m|%SkW7=qw zE3+cg#%l@h1!{SN02JLbfp*h@g<^abcBV;U{D(c%oHrTi-Yj>-3xZnG~QmnMe$B=?-}F;#3H32 z#g{G`6Q7pS-JBn2{Us^aDZb{Rg<1Qh*GDy>C$qCYHMJyQwQB(p0(_19u&IF~%IfP= zZxy(soG$Vc&Tu7vj4EMQj^I?&J+9JluZuwSBsl`Q2y~?1%y$!GPyPVA297{bXsFmi zgSE=O-q->U?cRgVW4tw+FU01jat*xw5y(osZtB#3S7{9ICjh(U>v8Ml)22C3H{tPH z%>CFs@Msfhc7z}cKFP5b{mp>hu%Agt)gQ$lL55FpBDT*CJor;5#L+38lj@D>X- z5sx1EvmfSi;6O5&JWbFC0J%mMwf$Ib93Vh76p>S7k#>FU%c=Cx+q<~MeP&+?(v3@`-sj{+PvH>0V7CoRs`<-!z;tt1g%|AKgMp5r`EnNVbr`3MMob;2w zX%JOZR0JCIU{{pat4|{_*m3rH zk9jd%v0fVEyNcw)xLAsQ$|pdIn~^fr8Y3MpUr}COPK|}-!$fR-^$KTATiJT(49p3U z*OQOLJK*s=+M_ZD)L(B4V-CesLF)yo==o-bjOq+KJ2A0>0o#d`uChSC+Z+WfLQH(d zw{2`}s_P!DVf!X>7^w*4@_tKBdr8A^T_R3zdD9^gYQ{rn?S->bVr2qzuBs!aKU~lR+AN{gXj@|A-?*;a%?0c3Iszzp3f!XtgHeKe~7>M$((adq1Az zbes&YWlGtg?7{!i-Fr}#tgE+ zUazrm_76eJPy1l~HZQK<2^vtIH}jK~E!tj{2u@tDI|feW_Xt)fevyzG{<{e%jex44 zwkZAU6l`AGW->k8rG!*5#Au5Ij+5zS^;k=y{9L$i+c{k!&~r$oAB9);2?QHDcuL>W zJN9?5ecbp4Y8%TUWBZdSK3+A4B-OSWg>8s#qiq^M8TS3v5<>3eDiL^*EMHz>xD$ZL zHgqytTT`&q9FJ#qEMRQ%GIUsikICLU%g22zFkTjw$}QY(XJO`Y(q4(+X&ClXx$*+LEfQ` zg&sc>KcHGB2%dCylUc03=FK3aocUBkbqRx!#Y@7*!$?Go>CrBnWpNunJ7lIK6KgAc zgIsEDO-@!8Wlbe$)rr31&yG7_WYHO|r`&@-$5F`)Y&v6E1k~x->+EZ0u?>&3}35&-?hU*5OS^cct zKJ;X6dNaoN1lg}fAyd@2O3`d?*dt>#M}zf95kk05W4d6xz(|$g5tq8|Q%|){lqZu) zpHP#q@mqhTwYO%N@Cbk7a8tM2{AoiSBZnmJkr zKhMKG0HzaX7!*exglGLJtM-R1u`T@N$7(9KR=Nb7r<`H`Uzw!>+&NIOr=|*6Z*_M7 zC)v$=yKrd>g6-^hVum2xSk5vW62|D34LXYO#Pk^3`bIM zXsEWH9uW)AIc>fJK#w?jO-?QuYiV#6_?NmKH~)Zvc`h%Sx#Ck^OzJ(^TIQ*i6DKTc zf3lod5UvUO*oyt`{HMnXMHLTmH(HNm+OMkXBU8E2n@U;1iWHzpxX5NuUuRB(umwqR zb9ybOl^D+x_d^P%Ih$mVw)Tv%ZK_Mu&uDX3Ak2H?p-eHp^27oDNMT!YxAk{BVBC9V z6-Vk}%h684D0ua%z;x&Pt~OA;K@k!C*qe(~QIOyVk5nv&jH1c4frc~`vPV&7qTC2D zRg#md0qq^;K)b5!+=|d!%z{R9Ke<(UFh&Ho3bWe|$S6%5fKfAjlEG8z@^~NA+q3A& zlE!mL1%?hUxc~kK#?Ji8BwmaPjy<{6E!}3uc{ArbEH^S& z@bSd9?>m(vy59*hkhoz~hkWMo-^pjK7m5|T8#jH~KmNTuSjuiZ)~~I6t#^Q+U%$3G z74{_?-D;pZ>T}UEp~VQR*Kr)(jR|!E^GX_qaP~b{h~;dZz#)wj-VOg#O0SxnRIQ2-E{kRVgz^+yM@CU1GQq_NORizZUIY39Ch@jXkX`zzL%Cio%`XqX z0RS*Xmi*vwB#$OYs(-0Z^@jwj$K47p-us8Cf}%B$sH-FIg!MtQ&8R*A8`?8Lb~$b4 zYz}+L@9h;U1okEAPD?sGIs?_w?>~NQ1Gj=o9K~R_*S3DxNG(R%aTNx5w%qjw4=lD$ z6VQi!a*G*RB1F3?0s%-^VGS);sK5V?>vh;D`EyIU!)8vfDm{}cV2T+JKUe(Nh*+O5 z+IT=Ueyas%2eZH}AYrDCeis9oJKOp~f|l`k81$YEdoNZe8)>wE1#cUMM~4Dz@uUz; zaEG6rZTkX;zMdYfSmtnf-A{WpfEI$uPwlQYWXp;S_w5$D7#SG@FTFVq{V1v(&)?&a zg1h|umt8J^Ko8xk%F7?do@Zgxnwu1|?hX!=7i@ZScZ zx$)^Pqkifg#DXppcXmZS_*)I?!KA{`)mxXyk$=wX+!Ogj>O#4V@$bgBm@wwonuITp z?t3I@vz{x4Q|;xTbSkLVRqRRICusJ~3$5k4<3@MAu6XrCVN_qF?O15xlp)wihc>wh z*LeN+l^1#5`x}WYipibOhVS;a(c%G^Pf*O>ynbyv+rr`pfs$Z5GKyw}NB>ImY-^k? zYCNb&KAKjnZ#(iPYWJ8Wus%xHm3D95yum{t3QKIDc-^y^`XLh4U;Yx-I@teaLGN+M zK(%~{-gG(nN; z+O72_VL@|{E{;=1sIsX9H&46u`{ixF1D*+1pR>)>trJiCh*j#w%2Tm=<~4?5zHK(` zg95+GUW*eBB4uR9UnOEn@6YdmkuOe->}6R>+v&zFBPzFhsbJ$q_r}-xn@F z9Zt{1(lQ{wf{v-tM2_Qb;hOi;yR~k%sx-=|&@#$9_C3XxKxzvdrz7 z!0<(yj>#30XPwWa``@?M8J)u6(CQ;&i{mhQb3Dcs^2*69bKnRJm5k=ud*KpH+Xd zc0}^tG=t&Re8{-g(_;$yX|LvQPxMnD7W{y{lc zUo-z^nz*8XY5j3Sh{#Pz!m~|ue&)?jT!iO&D8rC#Zzc$&iB^ui7;VD#hlyXmZ&u#? zxt4@wqTEb(c2!9)e_~2=24zRuNYEb1KAjYG>tvB~Ta>}?EQQ*wV`;xCno-6Il;@A6 znvWE5=Q(YOrSibaOU^qE6awz>d4$!eR|HD?ag%HDk zYUx+~FY;Wr^f9p4Nc-L_{Pp_gc=;)=x6(*Q8qbc2mVe20kt`JbINDni zixY5t1RWsB!Y4Q%m!dh}O$9rCjsvkABq$);C+o8hE zPuS+AKC^s%3`G$Qv-$Q^PlWl!CW4jdvvEGKo!}*g6Msi2zlJJh_AV_iRJTf^+Ec^N z#=yLZ zl-uo5PBk9_r6I5D8^smN{=`n}%}sav0HWe`u<8P{9T^hxl&@%a8!h2c=_p>q1n~3? znK|z&opr?v`r=F{=V)4A-X%lyC> z%ps@<*DzA@BUS|RQ@LtdSM^_bJ{mgw0r%U;Ouf-hpYpJq@@UEdw%(`|DD$fyyKK~3 zin=l0CNwggFIb(`x-|orq;K8A$XaX7ZDsCb@?vy)1)~vA7F)Xee8`hRme?<~GB}Bg z;J!FPyZ8;aKB{=hyr;Dig(XL`z4a?kPPGHhf2Lvi;DL<3>tHf^k3_iV!86~Ezm{2C zNZzg9yIv?JBs~1m!r6SyzK@VFW-Ar+pYat*&#XSDX+{5sMvlU=(T5{LA80A#$JbYw z^eK^$Y>vgOh-@`|^L*_6b;h@zGoM5+c?S_E4S0Gr8XbTHF3y67&YfMgz9KQd>R}nv zZ^ga$mEW!O3ey9?2^!J2M+U4~jELGz)L-Sz!^Iff2Inizqfp>cjZ(Qfbo3)tEgFxCQ~K8dh|cB{!b?4JNg$m!`qX#!~- zo+Cwk`ykY$G(Tla(5Z2o9-iu}OKdbOI0OK!J5xcM&(GE4<{2$E`Q?C)Ns~PTQR1l3 zf%k&;D1gL&9kLe8M9;_Rm?;_b-?gluZNJHenS6TtM**~;>Afyo~a0Oy>P7qWvvER0_3 zFsWxa$+Fxr!p{4Xsu`8t}y?eK_Vnk)*k%?_4 zKvFTNsz78;pyW8n!mW3`dIZ%6K_(Wxeid?$r>59Re!U5MK8u{a3L)rs1{w4dx>i@` z$A#IF1;XsG`DIY(QJXM>&o>ix7{&iAnN0s)GPP@3BH;N+bIuf666lG#ow;Hp)r^~8 z7BCqa+*R#gn$Id;_$j`x5T+s!%(v#hmu`>{Sa){E!4k4VQ2M3xz25!5$!#Z!=Om6( zn{4JzD5XG!u|=V<>C;7SmNjds-nqAm0KPeI{*t?H%(?5OC!~7!r>{i`!Q;!Aw^gA8 zm-jogDP1?r--B4s1S<~?M|(-4NQ7du%QlFu`yGP&U@{jVoau9k7TesbB0_IKMo9LePOH=_5wzrH4YBg4&& z?e;1UYc%>ABNGz?XAA;5mIcg=OsRE`b{YAOcn^$tm(ewjl+Km;t)w5EkWR&vxF=SMl++CsJ(q^hQ-COQv27k}^CcoDY$&KiV{ zE2!$fe|IytfN=#UK`?W%$~|GrMOi|6O9L`-i?Ho166>4mMo{URo0~y8iWyKpMQ!m@ zZovLC5BjRywC#iF3%ACJ#b1)9@`!8n4FRG4;lpC*M_oG6kGL@2IR9qtVt0jW~i*M7qrwk1XmLCu-@YyK+bQovBL3_&q)yWF{K%bQppA<3YNqF19(6I+LmQu`~UL|HqHF z3OzXi?AB9TKZe~FKm1xYgMh$zwHoxO^*gCo^=k1Kvbh}QJZOizb7u(jK=xv{#$D@M zWb~xaxeu*k)=TM20C+z4hb7n1R302eA25IhwY)VGQmy^$>8*ORZv zE`HJ%ed=z==P;i{e&W~szI(~y+HD{3!YL?i*^~aG3F)V406Bkp;n_$rzdkr})Loko z-}v-+-7fk=^2V)S)?egF`kinL;wt(mj|JaI+i&s1KnYaz>E!93|O0@;B z7M{^r{5eU`<0UugMEdI9+KXn78>m0CWZ$p9_{YSJ;OoR^*Wu-)FCO=Nh{eU%#?}Ao z4{E5ZwsAG@oj<+92AV&Jxb|Bup-pl1Psh-HoG%0elJU_iy|5$5ykYcW9PhpCmTJ&$o9Kt}@I7nT{?F8z@(3 z_)PE}DPuz40kip!dxRTZOBwVJYBBI+Ej8g+( zp}4U{wdvDRWggVsn*Ra}SS2Wn#)PjrdFhQszT;D}#G+kfctoCP zR>)yc_D+}wp*xc=VS5DrCWt_T)8=S+g~2UlTd;|*$bh#R zRnE*<4^h9u+uy2$5d~h{`bycN_RnDQ$#iZSc)E$L*gR}xUiry0g5d+~ayW!4n#FqX zGf)_sfujZp4)*3f!j=edm^q!W=XCw<*lQd+i{T*Nsw?t5+Bg42ctiyI{{3AQp&quh z)aT*~$_7nGQx=B^izLPJL zhjJN-cHxe2F83(#9bl?a7(0wL4FpNPce7GNO%7M)SJSglbvX9#U$Kmt-}K<@ngW88 zObOZ8Tc6D3Z8BL`T6$SQ!D_5RM&{Zz`!O_ONbWNrEONZ%{ty#izKDLP!=3-Q`PWS$lM^Gx7P1yjA%pgM#y=Dj|LcM%bjO- zV7}qd3{C<2zgw9Z9*XKHl{*c5YAXPHC`otJ4Cf%&`qgWrZy4V3BGa?zifV{b5gBen zaU6Kj>wCn3W!Oa4@TItM&@l_h%E@`jHei3jT}}pVgMJJsi(GYzoC>5}As&Z{!Gw#6 zKlwGYKR+t7Vv(>4uD`R>GiP@4YQdkQ={~+{|BGbni^F0~;=phiH z6Wq8ie_H_mO-G~ALKmGeV~^!jZuPbp^=4<~#TD&fu-PVTD-b;kwb!`B*ddN3_Sb^^ zLoi6e5a!Y61W9bRH@0N2vR-x6jGm~@)X9p_0-$H$grv7h%Aw`Xy4Ti6~7jPN_Gatfl3h)5R5IKq8zf(rmU z?EdOiAG9yfJ=mQ2hai4LBsD+vo9op8XCi1Zd{a@8!m==#2WFYIK{3O24%;tLw~i1$ z2e}WjcC0*oH5dbYKoRBpk*h&pf%3ppuxb7B{PMlQ;n&&`D|&>%k$~peovyHRHm%i7 z3=8OrjeYM~kWvBpC zrmv7`q3a!Ox$X|p^oB}z=f)iegZ1OXf4l1Ai&uRj5zdk-6!h6k4pX^NXV4pHo*~zR zhH5}X0(u66g>(^@T?B?7d;ZK5rR%6K_fUW^1E&xL=W-#YfE|}1WYYcePk;7*kX;s7 z(OP*Tk#n9f>Ah`1tXOQ+q3%-4%|E+_{%`;GZXxeP`72GvFW;7ujDIpwwK!snG)1fc zb*0A_k2DR5y6|rue)dcKo4%bC=0$}`em1n|m}L4pc|_f_rCC3zsDJ&f_j}$6@yKh& zn3#|RO@`N~eYbM4uiy;qA^pj^9V)9ylMe~<<%c&;Z>bIMgbvO0n~bS9OO`Hwn4u^i zTK-ENeZx>zMO7680|U|MkgSfz9E41Luu9~BuJiFJaR#JMtOto4uZURtmOp)bJ2?nB zQy3y_<4nT@xijgG*7u>TaJks_TJ6Y73cS!N0{v{sj;{ma9RtDWP=u*V>4|+}=+{5k zqr5;yABJ*p%RqgqEVWl0$qdk`=!jPz_C>oNktSXd!5fugAbH#?e5BXd+bEixuw()kLc+WGEW1fMy9qNp272460_8_-PPorXBulgmD8WXB zK}hx)DpP11m2f`{2M3tiAl-XgGAM*Y+W(=!g#;bZTotHF zz*SB>!L0?J_JQ`Hy4f)~tnW-ncgJZLb=2*9A$4yHx87W4bPkcf#^tu)-m~V~V)j4UL(*y=29Uj-+fuNl8h}tCNq0m0V?R%L86u#-t{$1&y?&Y+VY%fJW;D=*p2Fn@0e&FGf}NMW(Ki!I0^3GX$$`(1S} zo#qYZDERsn8?v!t)*W{W_A(PtvT>ojNXu*QyGHwd z6ITEBIn&6ue=TvZR@82--yafb=>+U=n5+l{kXZkRq(7)fso?AH<%V-a5xdL5PCM_fAW0Z*Sk#o9m0Kv&Hd>5*~|&10HxY~pGXze@!>N+VHgmoukiZ6})?R)zEX#NuwJ zjxI)P!p2f>)DNE265J}4<{thnO}N|cOvfnLqZs~t1*p@I+8dYr>%Dq zJ}q$6i2NmbH>|!IJ1`GjZBJcjM4~xx;6Oj>%hIokmTF=k6tI?F41r&!K8eg&@qN|6ZsT}u3Gg)j%c*d50L$$PK9 zMSnPeD0<#x3a1^Op(H?m_(X3F7rNIH7#~gx!?RCm6c&8&H|r>Z9WRvv(7?_n%)qG3 zk^TV%7GOoG3Rm!uU4X#y43iSk&5L75Ox?py)Bd{fi^Tb`sg#VFlsMKH+=1nESD=(j z5>PaDG>)V_v7^sZCZDkS{_60nKH9Dx695KQ{%i8?`sgMJh2t*X*qSC!#R( z|AYt-aXoq1$$wQ2#z}g-d^w6J{_a9wcP=q;0d&GPCa+fKrts36P2mPfFt48(c*VJ$ zskfxBk5{uJ17b+KT%cs~zc<{pkHua`fgI`gV>*&~o&)kG zl)1_dr7EI0;kE&Mb836&5h(7n6_NxV>;~-yDB6VmQRS9l0qPRm6k+s3MZd}}YnZ69 z&9za7pkD}O2+-3c>iM(I+q~-86qv5CU6z&}hp8g4LIY zHEic+rub^-DW+Jxbrmz*3+;`B)hoG%=xB@DYfCvIeNdqd!x)S)r%(res;e9u3D1ll zr?E;SY8wJIe7p8kNUyGo7KeaO?dE!Z=dH&M#5XBf59M^G9kDGrS&Sp)`$F0wd$*^P z59@Tch<(;qgZi}*aOQFHrfqMjU>fS9U4fYi+n~xO9Fls&W8UfSphxZs0ANs08R>1z z$bhQdzU+Ggy%X(UtnF%M2g^yyl z`%ezS&;tu8;dNwBv;YQ8w6^4L*Kz;3UYPArTIwq82gtU^9f$q(xFO&ZwqWI#z~npK z^pa_WABnISYjMrZH7?l9lH;|>Sw#PUK~h59xrgk%Z@l-cr8vr~T{JsC?|G(qhmH69 zz+?632;7n;lR&eOJl3(z5?=V_wA(gYp2{uWVABj68GsTK-%*|FsK=rFs$`MJQ$#0s z3(92l6%{7j(%Rt1bEnx|K~E0B8^I7m3cRD+0ehPGo>|g*Qy}Aco(@f#wN9r8{uZ0o+kM~+_hoPLnX3Z`6aKUW z%h6NiQkZ3kAu3pxp5TiBXt^x=AOEVjPgu^cQ)wg9$E)aK9fH+Cm02*n>6FyjfmeN> z?evIEQnuBKEHe7UVWmdurkkncU3wddF0G)TU#<4AhdL<)93O&%cP>sV*Rh#CH!|Mxl1Nl!MwXsq-%y&>|qtC~t=Q3*cMvDD8_*dS%Ex2qGc6(IC3!KUcXm z%DPUsEnr%Jj-dwtboNx+V;z(8g3p(&qQ^)=zUHHNhU5b|`u87utg9d>!-a~BX&IHK zy2g1^*~MGa)Me1ce#lq;dVZ5+VjS1g&ND;rN8yHAjgiwxkawJYD-Hbjzn{=zbafsj zHg@e4j72DzI9k+)vV6lu_D1$Uj&)A2uG5M5)#G@8QHm-{I@_s;A{#R(Tu}s|xfq@4b|I0GM1Z5BwT;p^+$Mn1*u`*NpEqYq zO@$R^eI3zm3leLpZE79OrS#u$;h}9i2|mi<_3uC)bw^uf!)aCM!_WP5LE>}gGlti3 z>tPE~ixF+ALByPFbZ=Ydl0R{+4FDLU+gVc4+xA2`iRhO_uL{nr&#aci4S4y$eN)eU-6s0Gdd$B;dR}oe zu=5NNm~8{oB|QTJYPkg@(I;a_4@A2s#&Tc2dUYdxf*$3y>;gtJzkKy-tgDPw@`iU_ z)@SS^K%(jKKF0Y74Bat_#gqup;p2;ZnBmlLTd>uwFp=)%0H*%61TIY|b26_vb4`mt ztKTZD5V}uMG8^NDOJoz{68P1QWfGMyfP~%ST=Z7i=91$Dhjh|{;swi?h#UbBlgn#4 z9B%MBcvEqLOx<1MDB;A!jyOS83}J%3P#B)=wc}`(AR3n8OwiJ=exZFti855eSL}{r zzzcgHDVT+p6P;&k?-8~sc=5;U^x_q(Tdiv1W}V-SLL$2mlNB%r^Z}9f&|mxN&9W z41iNqVF{G&-nC%6DRZpNUE42j|6R_?>mq!lu&^+qy8`1V@Xkp%=GoJjpQZ}#vcPXaJ!dv%8E86h;Ke!yb=62haBy?O)c!q{gcBMF&V|RPRFEFZ&q#w?wDOsWdptHvx4ZnY zNZu>%Jqgz|;WNZMD&#RgVLMydzvhy14ONBwe9>`&l9HHij&n+w8VORwBY(k+d9G*a zmL54OGSm?0|0{~A#QZHG^{qK|3wD?n1Xi0OCG8ScUpMmdeFn<9TDY;tcmmz8_up_en&EVe=n;RCzoAC;O&_s7nYYINSq(EC z%kNxelM44B#im zhb>GiFZ6I^%pNHY-s8c)xz#(6-&oRYE04jAeLEC%VtsUq75AM^`S9U`qRo-%%=$Dd zv*B>iZS`=w67nUNhWIf5?(Y~U=D@a3eU@uH^l?;nFC)p~C+|#UsRW9_QbEze*FaK> zGde#}tmT>Wm+H*zfrdZ5nO!I7Q3Z|s|i zEUoqt=1%lpZ4vAIVeG&YxXq?=i^P=E82SUn1bcwVd^r;qdhF&RiE?90p!8`!Lz)NP zBgPJRd{C=md~{OCr9=Qh@q(d;tn66^PvS!*&MJGt)1I$L*w~ckx`7|iz)L8z=+#@_ zBfU4}{f4JCst2g<;@OLTfxrVq6^Nyyb-H8eW=R#T948b3v8&s9w^Q z^^NgShHYPgyal6)VK^vsITBl>eRzkfihRa<2d-{#hY4&VY} zrVuo`7+v?p)5DGdTga)b?nG1fxVYKlq{C(7)0Oz=3;@e^D-&cu7?^acbj%0=y0Q}w zyB@Dq0f*$;*l&Ni!3@3o597EB17e||Km(dG{yhys@%{?-3RQ{mSa$^&^cDK{9#z+k zE~(BynmweDuscI1!_ikFkZF&CU{#nLK`S_Y#5hJVlSJlz&W!o?Eq_FtM9zeqx($aItkLY$ z2+<5KoAJ+b-jlK4m_4_QbFKk>O@`(|KBg~q#&mAq{*{C1PIR*8P)G=a`yI0DqUqp> z-iD#u$+}GM-q#Lw(Q2M6tUUT{Ow`w8UpIE9u$my(apeR>aX%rVIxF90y8DanE^F+8 z(tl`>0cc?d2a(<#5_%M>h`9 z=axn=Uw6e(XBe=Ho88vZvP4&6E{5!(caY#%(Y}U%`=YmfgA==ddU|?N5`ju6EJ1qv zrs{)LG=)PIUZW3O-h5?BI0onMG2R4N#hm%^bpZd$EtgU6~6WxVeoMjc3C( zP?=90l}a)p27QDRmR#_KqCmxb8f2pV1fCk82!_R)A$s^<7 z4S>D@^$Mi@oMZg;8(U>Pf0HXkC5eD)Kyw4ped-oz*k*@Q;gzH!PZS zoc2I{*ght#_h`&twdwS!oKs%FXv@%u+H% zqLLvAjYO*fr6LifB}L}UM5r_o6@^Nf=OHsql8DSn=6Rmq{Y1lBclY}~??2x^-+OP* zc5mCg9*gU`&htFZ-2*DuJ}Qs@%WK%_-6dh= zDhuhFJxOr~-ey(SEKhlOSHT*oWceAeu<@V!ih%r?o=88}$w&v49o(8Nc*WtBw@3#r z(iT(!Brm3@wFoK$TmNyG2k!1f6?kigeT3}MdZq)ohxv{L^Y-$kx3(`pS9km73Wpz?VFkwg0+LXEGM z;^C@ja1N!V$#s}N(gz;9@gm2<~->vQQN-<#|5y{+Tx5(ImTZK8Q2 z7;Jc|s}_r0^{Pl<(t@ERuhW(WZXQeh_(=wv=WZ8v+O%PXh;h&GDMp(GSRtNHmD6p> zZgi$+j&C@3w~Wc*w>*=Mle1#qhOY|#GnsRi2#hO;F&9iJZ&}|R<^0+(6m{TFz0!=1 z!KIw?#Zg`w7X5Lz`i}B6NqVf6jCfic6j{-^f2mZB+bs{K!sc78$2Y{uOmrGPnd!X7 z-kTcdw%|^lr`^tTPed709 zzUHkgR{xuawOXCs_(^Td-_Af0? zOvWiDit^E=a^ElSFlJV`Dg22QKA)$?MLLbiN5tm!v^G=i{O3mmbu6HRVX4dpEjzdL zYdr4-AL4??zeYTvw3XRY_dI&}fKF1a-^?|}^k}TUWtqQNBx}N+wnfBPgvih%yJ2Lx z52L$KaJvr_>g2ec`(PZ(`uD^8vNl^Tj>X<)Z`ZA_%D#rdW%-~Pj=*m~pH?AkY}5)SC??pu{|2O0VXw+;AP%N$^-5 z2A*>ckr*?oDpyy|b98%ot!U)Lp4L&|PW(Z3u>+qmas)|}4KSfcjyUuOwY2<`w*^pl zES*z$aOzIe@~9(LoUyL=(9GKYwm_L$PXAl**coNbhey|ZERA4@DId0xe&o{}csD{$ zQAe3QDd^kzyE&;2rbZ8%Ib${ne%{e`*Y@qBqgpQfbjACGhzo(g*4K<-m_YRs0La9V z|8q|22cBc!%kmwdt(HU2lBlN*2<0zc*|2M%{6$vq%ebm7CzQXe1r6_9P$c>;t)w*a zZCBZyE3kF4sMGA^h7NQf;KNc1Z3q6d#U0d+rtv#OhvRRZgl<%h)N$g0j>K8@xiy-y z>gMjlE~JwKrT(d{2J!Ralw<=y66*rh!%v&@(vf)MBYC}KloSeRV7?|cZCa$S{X$ic z`!Jj`TT7p5c>7)2Qa``)pb!t&QK@R~4*!H~Q?sjJ6Hau&vaf+!&h{Ix1E(CKU>tA6 zhsUJ0B4MyLA((SuNB>*n3GT0l*;C0Ei@ju#yj^kLjw;{xWfzO%ecU?D@`Sh7gI=@k z7ZTe9#CH(UziCj?z;y3h8sV>}&f(}`CDVJnOk$TP^m`vMi|j^qt7w*WEq1?Rl%b#d zuoJtaaXHGy>n0OLQzLS`@S?oj&~cnk`tf6)#YH$0)c%H;9|oU`?ayX3EoO|!{q~_B z{~z7@twGqp|D+Q>?u^D4oVkbAnD0!l{Pf8T#xdjVr{yY<(I5}P^uNjBd^vEPq`%5X z_|mVz4U+&#>G;Xkf0Qoa7{(|1GU@xh$Vs>lf)k|pHREsBjN8xy;uXbjF+icJ53zZ8 zKHJ7K2U=U4wj@LxR+LXZmz)j9S3pZ}c3iqo67X@cFh{E$Jnqwh>(pzbH2@j%_vVAw zI7f^#O%7rI>S6!x4wa5gSbmauWsFHeV9Qct#iF-moL@NwN42*!iW_!xqfDXlNaVdA z(e~&p_tNSXPc5*DI*B(qe0*;WpPT}WiEQQpO8an~l?>(AF~z!tLY-W72KcR`?!l{@6m-an|`xZeB|z8hSS2xV*%?OKV!_Hf!@o zu!EygEx*(}galh*zORxTr{AF1-GhmdNaW}f;;`*p{riw3gLM8k+z+Irq`JVo9?2Ll z8lK~;&D5uG)s{6wZjJMYqo~ip%b;x%2VRo1$~Rujdzj)8rXDa&gsEP1;T*zlU$W3(@AO=y(nSd@^KG5`OdkWNDTTtcksDT zR1BA%uj7XrF1&*~0Wh@SHAV|0)b@bQO(sI+)@ye<^;CwHj9t1y&;4b2(RzIga6NLD zAe>FtK3q+KQ)1e&2F1h+^23L2F!;dZ?etaVC|;~NXNlq!vn-L0$_1O8nr~XLP4^l; z2uwrbeR3`+Qzv0SX9a&a;?M%4V0TVYIZ>vF2g?^9%%gJH+U#`nO9D;ztx|y=vu7lP zJ|IMce^Eb1UcxGeHbDVINc(eRhYUO)eumJFUyVdu#Xcwcf(tru6A6shk0H{f= z-P8n<@{iLxM#USe<}a?>2ZjZZZ7?`UrX9Z|y2A=p7UBYMaZw?3i5sC(-5Zc*`r@={ z652Ocs5w}WSAjetlG>dGo*RrtqBM4O1~R^XBci_u5DF+e7!A?o64N$$z34vMrLxW6 zamcQ%N=r>051MQVO3v8L=cJVn?qL_q(;+puka6XGob)lHC(1Fb^H%nqCza_i3!8+2 z+AyCbXeGV97vkI49`B|R4UqI;4GVCrsL_8NZv&GKoHlrknTTk{7AP{4kM&+<@uIc% z($$|^dI8H4#s)Fa=5C;3O+&Uv%z3jFK$$}oYlOaLd#SHwysb?BN#LNwkq@0YXy_4E zi3S@$!L{4g2X_^mtwn!K8_b{T3zuZlOoFNzdsCKgF+B!U27?CrE1sTD^nxwp9^6hZ zF!msSW%H`khB$pY34iADJFYa95F&tE3IoW~h_MSS9-B?Ms#rDJtc(lnXrcQ%-fpns zG_uw0TB!qNS`q09q;Rok3GuO?n^^KV7QQrxlg?e_e&HJ|OUW5&HPQ7on0mvH!IOePU~~BW$Jwy`xja@P69fGFK@_j@!5| z!EP+UuvjGq=GUL5bXG22w1#8u@_k!BUB;!z`o?>qg`%}MF8$3XW9mLj;u{K2(f(6qR z>G#VEfP$yOpIN#_zIcD@@p(IHdy=P;3l6I&9?TjDG?7D|?eYjeef!b_yYYOxaVV{qSvG)b_?5PZY*wyqGn;kFU&I8^OM6tl$JMLE z3&a*%BI0gu3(*U)+Fj$m`0`K87tS(oL@795o-;NcM-^#1*%w4?j0=wvarjT(t;tM# zqHrz~eVBOQ?)Z7DTQ}fBj}%`v3{GYhZb&lLfaS{nuz+RVtx^iVSMZ_WBXbc{nk4ww zeYL)mY-OEr z(g!yenaIv^L1WIZc3{^iOPw3P^{y2!(-!oM9e#@Cg$D#oU^##RsVMfj>?ZuE)G z<#_^IM>XG!i1=Puxnr6C!O~{cZsVaax0NP}yK~+3%ywUly!$24HXhWqLMCoDY|ecv z>51eTCcdmYx#ui6ANuB|2+Ypmh}T1G2}rY{qPKg@qAf1CPu>%rMh`X* zu>t5uO@oNJ)ppzwY*eVw#Xrg2><*eNvD-C~?)6}%a*p$fJx{Lo;>OSgTeYlD<_VvS z!5|J|H#KrF>~ufVJyJD;55)PEEToqKIlwt)uBnD3=g7E}bqmSsHKx4SJ9_`EYPOiS zBjG6Z%C%pwna97l>c$niK(gVMp-er%R5I>jcs_2OefPchiuX)6RJ4Az!@O>jv$Bp+(#6@#{V%V{`9+t?{*6nZL4&apG3VKr zuUIh!p8|lP9#bc#r#0GSzc6eYy;oH;86wkNx- zxNd8( zRqT_jC(};|XS@n^FPg-sQ@IsQB2g+&jR#F#0Q-OKh9vR%+^9M<6BQ_xw>Xz}8~3=g zCb+#*VA3;s?Fl}jxa}#+_&l%L=wh#xF{gt4#Z5H1^`m-T-Y4=cQiF>XmT^9MS+c=) z60#t$&Q3zQ<=z(NID5L9Tj)+VCL6r#e=emv17rNT<#i*&ZE5v`nU!fIjPP?ogVByQ zLIGCgJ--pLJh!wm!Ac{))SS0rVp;I@JX!Z5O#EWXUrkRRmutGlwszg<93H5ssE8V4N}GU*_C-uofkPM@M|QKBD>_3N9#e(xSMD}7xaX)c2Q6!0?u`~qE9O`=;iw%AQ zhIMA4@Tlk{sCJ`0-gefH7qDsFc)lvzd(CA1tJcGHIEJ`Vw2-B%G2Y> zQBy{Ba1phJQhf|OE^KtebqX?;G|JDN?K@4H~|+RKc|E%CAzZ_w2kt(t_* z#WL$wB%QTmX|>YeIT+hyF+YORwRSvosy`?lGgd^7M1M1x8Z@Eq)@3H6X2^&N<9fNN zrl|n@6X@8`hV`mdQAl*44A$7HyecsdhUMd1$Hwjpbx6bPV2KiPEHV7Hu}b~1{e>

?>}Y?Dips^-A-~MLDeT|XgWUk% zGVg&%GL^|Wo;fvw2iQd;5zl{Q^Ap+X6UQ$K9YX?)p|hW*7uL(^ixqg$1L;s%y=o7u zv$16TaRu>V&?JbvW?tYZl8<~J&KS4*7SybT+ydb~UPIF0@He-#T*jGjY>R^VGtc3& z*%e7t&l6Qq%@vo(Tlm5UR3`{s3E^tOen-$ov>#(YJ#@1@I+8JwFf|B93{6El>fyfL zUI*cXgam3k$HwLfN3}YM(gx#zS{@rmE{({+<`@o&Pw}ueM!Jfpdjq>O>(pgEKtId*5|*DbgmIkVFB9<3MNKK<+2G2FRz1KZqTc8x+R)to_r zQG1yCadgidRr7`6PL2Y>KJWNbSMxd6VowlkRGHIenK55t4czjMjVoQhZ?XN_IzJVc z+n>0uwwRuQ`qn|C7TeXaV|?^;#?Pha`LXnF&wb9GJqtT@RX)be5S(Xhyo8n|ne!ns z)hV|E)THu?3f5yqXm0G+)(-IVTRvV5vsJTGy1Kehdvp)tA)$cVQZI}+vTp5Kf^XrP zLhv1r$?OEZX) z#|#3$^p-8(i-I^OcD06~$NvIMhAs8ML(G<#@->8ua%!@63PU_1Ync?f?}Y|hK8=gp z81l(s;`_I60YHP}ean$BeuL*y$AvKj&RVZQc{S^ik&)s@w6dD#DfJh{-A*%9)}7k6 zz->y%hPiXOpK(`~4MgiNWA(Oore3?JuJU(GclNqi}IzjM9)+~lpe z9qSh?GtEm`*YRjbNnxK&cxo?b^VqKGU@mZ)nn650JrPx?6gI`t2@TQTuW*ZUi22*?jM@c1R{N#XJQ1w+pzIwz)^RlGw5Cx!h{CUKvmV7t?l`Be&j@C&KlwonNI|ayV?7;U8v};Z33Q0Y?v85 z9d_L*OO~Mum7f2lvJ#yBWvTq%zAl-5(7Narx{zn&2I2#pudX|XA|4j=;menGYNKZO zo0NJ_9iKptQwFpffic`Z=0PTcaGjm~hn{<$MNufUD?bO#{IKKc*(VIcKThmd-$QQb7N1aAtGs8J|qT!GIKw;i3@#Fu$QDgBSf*^ExHVj ziw~43Z2MiDH5Yzx+2*x?a=yj+BzQgj3*~uVN2<}x`LdpzJdrdtgd%&6IR=WX%xAde zS5hIGy?VTYM6(aPEMFq^4)Xb>9M{(ibEceS`Tgm*zBSOBCU0*w>1u7Y1aMCD;9%*& z0s;^LNNML-c4?@p@xCsY{#(|tfIIPRR3iv3Zuq@MoN~GE$kam6ij%=8fc;2mb#--F z*&v)>B95hL8b008&p!P;_WK_sC0z#1IU*uLVUE2l+yfsdum8p=SO1I+Y`J5|3ebAN zSj`qlT#>saK{G)-?;%?QN8c%-f5e0WCMNndNl8hWN-&}TEU@A^)U-yC;@`UA@Bw&@ z(440e72`2bh>oJ)2>uKtcX7Cna0{)e>_U5&03!rr@R#gB!RqFW6AxnZh#3g&_25N~TJO%HLbr`fGX*4PW^NA6VqWPl*yv=|9v=Zux7e!)Vw1y6t6m}BGeb5_(!at%Y4lvcgo=oX=|iIyoD!lYsBJ3d zudIVx39B*paAk3Eaa9%3Po9HZWbg%u;AFB!shim|SJCMWM!DMwTy_8nKN$(>;NUPi zYB^_){OrPh@eW$Q@&Ts0C0k>@nUgEtq^nPO(aHcMbl_z(9@IzvqNQ@lnDEI#LT|4} zkpKqjs%i^lh{hAh4K0DLg{t2p9^HJ5Eqw+z&YlNB4A zRyMu2D0;~ldW2T@d>ZsaMAoitZf#{`qV%_rli|QUKD;Pq4*E#`KCU+Dt4}zB&$W5; zW};<j4lh}eb9A~4K zP~~r(G=G7=H{bm<0 zKDLOB+DOoVLe?|al*|TZ|5lQN>`p_&78EwRMw(h$7fekFor{K!LedU96U6 z)Cb=PnSKqbLu{pEEoQEe#^XSim6Zj`*~xAiuB$(rQ2pe)f#MV|Fu|>zk;A28E2~EYL zHfd>T&Y9W-jMBeRnvyY&XiVx>Rz&G4!xjn9b~mYL3q6G5pA6YzDglnHgBM2h$$%Vq zXiGKsd4RGwP3W1P0_vQpU}KYZFL>$ExY;cnMw+)3hbl2jk-9nr(k2ha+0Tm59K2!n z=wWE2@R~KDn59)g!vWw5VqLgw&V2sge~!#;Cx*xh$3B#lXlQDVn!`imagM}%WuDF_6yz(8{(rHSuy*`a3hJ$#4-y0*|%l?xTWUV zkJC_07gHz_X%ggq^?`*_kU`-J#aXnR|6u8`08r+szY za{l&mZ-ksbUV_8)0((t3tu2`1&jU6M2nYZKugEF6abx56@0TuJqO*j!1&N~m zFsvxR)jr+`Fv8KVa4^tVqj(<13|&*?0XOcdtmg`wcVGr%awprW6Z`iU!))VYNePoF zUr&%kh3$6Pi|hIn92!C5UBS#uHI{|}e30Oz1MY(g`2>yw>=w}IYngaa6J!Jdquf|U z(O2^*T*Xic3Fbz>z=a|pa2Hqs!;Lzxo+8|}D^^geF0eyQSJ9<0dOf-HU&|uDPR79} zWf)9aqTsBotdv9d27P7eOZhf{i!d@o^YrN^bkpMz#mBlb~MGSlzs zQNj*m`H!%jPo2OG0*GN0q!e9x=mBoDAMIHwBqU@F=d6a$pqD&JOx%>K{-&r%4pN-R ze8fFOKSEY74h7oI>8|MDJqU&Ve86X*;W^7a77f?3Sw0hpxu)jY2Oy;j`N+F4>F*j0 z_O==Zutx?Qr|Ar$pNR~lxfgh0(S7OL-45;T?Z_WOOapqz+n}eUKzH%S#d;#L4e~1= z-kpOx8-`Z)hHJon9M6B0PWEC}Hd3E8R)@x^)~E(n6&Ekf_zu-g=IdpM<_K z%yP-DVcPQHFAPQ|K09FyRpbi``x=xwwbLU9nAE?cqU z^r=$|Dy`AFZwT^|nL$G{{`dx#^I6ZJjSyw>bRjbcJz`?TZh5=@aWDK{=l$Q_mcLbl zKR4h9UKOMHRPhzv<}f&H1U=MSMjNs@U2hplr~J)Z)@66E2vd@`^cd~Qg1m=bT4hbK;szH=@aP;n3G_vn=kOkKS2nW zl1e~U6-H;<{gB0f!BZ_6Yh(yq!A#ubmrth%*=4*eS(6G~du{%EZMdaj%f{@!iv*`1 z!N8F_-S@b(iDF*4fSNUC`nT@jov-&{=PHVi-5io+*gq@hU3hsgRBpm>+qO)nMNF9d zfwJ$b&d5yVw(p(<=?;O|)zZ>3@;+gQPCUd?XpPEB+5YKjY@iRv%o;{_Pt2nmjU2jV z#VK!Q19OMSd~{3^SBcdlSW@*HT)K!35dzzVlG51Z=vM_GPvzUSsP6xup>t>3|W|kGnJceD^)=OZlwgk#o;9Svop8GHb`j$IAsF z4||7GNo|Le`kuc(A(epA*Pj&L>@_8uz-e3$%(Sh&ofu`pu#H$f=5v1hc?TBgQ8TO+ z6tn_~QdB$fLv=N3Qj_snz@7;6-@tLWm;oy2HvTXYI- z()lM(s9bT$xwLFq*tvo3`!AjB>s6g7i0IRIqTWewu8J6>6_9aWmDy_6tkw?E{tIgu zHl0)jr{x8ND@A(&rt84czLs|!G)7)um2vL-oBrnc#&v^T{y38S%_oZXC6h+Z`#pHn=<+Oh+Lo z-D+i&5DePAC!4(;4Ejgz4Z8|38z>knD__}igSKR3*Fu&_#f2nKCi$=Ylm3P2H?-R~ z;?goOGP)|`nbF{Xo1y{yIMP8Wrzz$ zpbbP@fiRT7h^WM&%IqO4=xk(%{9jl1fS$|$?L@|#;`0OO_l4YKdToTENHgnaBVrO5_I+MyDw_yBrQ1908stg(zn7NWW(5tC?uk-jIVVl8FNfvP- zFfb5Xu3M`&Ep$QuWy*fi7lH*31m%w2YDbUW?;gC;A?26Sz)c5Ow-N`x*Wc zxXPFYqmg)`at~F1y+}?!3RJM}G&^7`s9u3D>`N1IUS^+Hm7ne0PHNkO7Vz>?+T-=e{!m@`Z$sl&SJ}J8r6F)aQ7WD_*QS3GgcCi&5 zQh3$9MuCESMSdUJEaFkh486u^)!Euk&P35b@GueZ@&dPD2q|L26I8dboH!0upzPrh zoGGOHNc3fG{qNtuPdHatVTS~OeFHG34uh>$7S&6Gj2`oRCT$K6k~usJ4(8p|4ty#9SU2h;)KgSq?Fp$41K0#> zMMU1ee!Vz;1i@=CWk^=;Kt|B&BP3e%FJEi8RcA@BUVbU$p?U_iKLh2`QeM(t#1qcn zPz@gtwy~3r@{!{x{91-u4thUS6Abk{w`ReD1rVCMOVa*0t*=TumcpQ@9(rbWc6Jg) z(Rzb%FaMoUo2=cI+am(RlkdYUn~7Llz?9Hs4ykEL`j_Y-_;uMQbSsCw5#@rKq_uzSm zn#sTvDjPi~^BiZ*&0g0oeXOY={YZ$;cCMtNe5RVI<+H1JS)SS|dQ`HOzqy`f)Xlls zPZ&6l81i={+)*2`OOOU()w63gO1erSI95O82sCB0c7Mt=kr#s9iqwk)CVa>jODNOZ z>tiwSX4ZKvo!%kvF0i1pLTX5CrI-%BIF>frge(3pZ`$P2yKJowg+sf>pP`DaKA*jW zxZ1JazowU*-5pDWJP7O|Po60K_N?1$co*L7jKp1x40q2x2}67?s9z!2x9$(e2C9Wi zFv{`($+KVB&8v#Pd|^9UTUDhEc+B5l8rU+tL2+9TBuzk;0y-8vROe3@o@48;cZ^Ec zUfLX&#Fp8M^q7W*#xWQkUH`SXm2T||{&}arzynR+>4!zOtwl=Wpa$e@fe9`B{$Y(# zo8;19zE9h=_C;-kF)O;~4G6ANUqvSs-04nBqaW#^$dfUgD;j%(a{er3Iu@>vMaj!L z?++#K3c$Yq(~`HsY78`OZ$+L={S5Fk!E>;NozR;!{3>d(l7MbCD6@GQ%We$ka~mC{xDw zU+LPTT6G3F!F7`Zh5D|BYp&=_jegF&JLDx*gIen6er6b4sGv_P@%vTXb=wB~dtVj4 zIB^9@I5N$zU$uLOlRsMZ9^aH$bYQNj%^UPHg4=MR0-PgL z2GBIbLVb|U&Hy>W6jU3lv*$k8b6km zF>`t#qjGUtJ1#IGTiv>!v6Wk{V#f-ANkp98qB{9a)wcIWkoLaI$=MK0phbSN4c=xq zjgz;pi^&zD@#iVfxqfpM%4v&J(<`H zu>~k#x2XYpR=l9b2lyt8(zv-Vc=>c)ye73BTLa!D0=KTV&?T4hxmX&!5x8{Q2EPFt zCiAY~7G;6l4f{U4V$?K57ryX`ftW(pBgLOLk&=%0aY_k0t{X%rj-#@mfd^?B7#b#F zG@8iJfG#r#-<~oA)DD6dtMO+rPhdecu$+DT)e~vH_OK#v1b9*~JDnyOI&@dd-mI?A z7s5PTHpWNYq5WfK&y;tDxyA=joxhfd1c35HI!uf=m0Mux&-}Pc1WHXP&{z2kkz!_@iJ7mX-SgJP-TmMr0czvJD>Yx8oC~MkaZ4b1$~r zm7$todDDq}HoprwKNwvclb{aiM#Js?)?V|14=y49@x|DtacS7*+_`;w2)?`dW#o{< z58(|e;=;mk)Xs*6XqA{i;dMLm{;vvlI^bEEbEnwiNFmNa*T;GEM!R`EwOlp#3FS07 z&B$n6$QBkzh|5!m*AHqT{LTXooLiUE?1+3xW_4{J9w<4YsTE{Hzi!=N}4*i@S)=6uc=Ogp{<7 z_{x*?j*a;oR(t$P+*mBg%BkP&Br^^;Cz_RU6G5R2H5O>JCgU+M76iEmbT#-NT!xUu z;d6hDLkk^m#&64u#?dEIgAz4tSCAFO%Pr&~M`0tGg#}D!ZO9&D6;Y>$0+8{^=S^Q}Zi4AC>Pnos$u$zdkHs@oixVt>7J`$u3;t<=-WM}Zqh<_C_NSMidOKZ}16P>e_6?!WR&1E-5a zN^54(&CaK&t&j&4D&cKaCMd&~s7dO=@||N_7yIMrwq^99xnA_P$=f}&5|Qif+eaFt zkLWSo0Jt%lCa&vU90d~RC~x$;z1r$CnnNLzgD4EZf3|>|>J9$A_i*XLpcmqrk8Fa~ zt@YA6r*aCfc?xn}oQEEP{o9ktjdgx@(|E@EuDLM|;mvq5{nSVGf$2}NCQrsle5IiI@- zyxI#B!mRp9PT!mw=hR?wr#BZ zc_^hvHX(kd)<>dT19xVeoLVI2u%20uR2gkwAb$O}hq~C&`GNEJH{`J^1q21*ccIho z2RsN+xGi%AVStk!uD~r71`n>M+txZzs7&@;8n@vnrJO4oVjY=7E$3%qt|B(f?W@W> zjwFLhaJ#Hk^qE_hrCT6Jf=CGa_AR>e)qm%+qm`aqnr)sgJ}#yoQ*2PZk*qDPy~*^3 zA>D_P zFlWvNw#r|i`2@$#PqkAMwJ$Z}p4RfJuI`;jXA3l5eUa^OQ^$tk;zFjv)(a{dcc-0M zP&)P>_(t17`r47#-T8OkDFt8rOyL@sw;pkSx+F(8L9Yl5;1HPY40fZz9)(fXNxk^0 z?lKpl-vW64X5yX7VW~GEU*Dviv%q|tU+6(}!=6*g+W~@@jiB6HL=WzN zAZZDpJ$|Npj#ZZddLbwydW?YcvOM3jIzDr>+{sa%;TAIV0V}>M5MWFcb50baVOi7V z8_GzQJr{K9`EuiZgRJ8y#*fqdc*R^Gr|ydA+jm)?8rFTQA6%SlMDZD&y~iV!yYGKH z`SLE>-db8(?I-%LT{PRgX3gpGF6|5NHlnFRQ$|g5d(7XL!Fc_o$iWk6{}s?Q~N`!5C?EMVIe*pWIa6P{!1RY|}BPeC{LP zs8@{xB@ba72flAZB?mBddR1(GwFH!w9=JFi4_GHzG%5^)+Sy1*qk5I_+k&|4F)w)k z{#r&4XLR@+7|X^xU)gKE@Ujh-eC}_E!#qTsYcJ4sF+O+hnq39KmpnYwg?veKd;U1l zm5mQEg&b=RS0(2MiHgeV;fYBL<*qu{sXIH?l|HY18VB!YXxDKYM23cj;%J=UXK0bp z!L!9w7{qqYKAYKea0IOH^VR#v{5MgDX3_ez+Uqm;{hG>Q@I7Jv&theP#)&MDi;8+@ zlhh(Vl7U(&ivjop%6HW4GMZoj3wnxv;`u7Gc>cFA^rx@FYzpRi^v^%s*kSuBQx#@4 zJ_Y&Ib~ud)Q-#NF?b_IuaAxnXM!r}HyUV~Iw!z0w1jfPG*Apk|VZdyQG?y@&txm!5 z2B!>aNH}G3Ze6)G6<4go08%!1Ub#;d+7EkShs%kIzI^!bA;<#5D}|IzMnWy}Wvm-) zUaTUhID3RJGzWluC8v4bRZ}k%|~+z z9XN9~HxGbyyC&=gV4^f4vC9~JR7+m{jIlAS;AkWRXA(RA&h^im1hYAHC0BZt9i3-&opk0or00s8SH8L>{8zQ}2#mB}D3 z=O8bhX2x`BDq*zns< zUu!!JU;J}r@4m77`Z|MR>OrzU@jeyjmo@WYvfl>csOv}|P|Sxfovy+Mh8{vcczk~XfMnp+_7W0MxM zEQ$NytSHxo5moI!e-NLfs(aV}{1t!g_h5M|t1%cP5XA&>BffbPb8PnQk$6 zTDUCjvK%(?;MIPk5 zM=+BFjfXD$r3chQIZUdb%E~x4n|=cvGYlcyIOkM@-6{>=5tFHo4A4sK(9#M|9R|M< zchxxq1GQil8_!v$zx|2&UwiMB!2rBxC%>(cTV%IaRh0;0kkGTx?gOljoOzw-3mB|C zGV83`?B}sRZ#XxBrG_n<=miM*i5wpLE4%*Y$QH%0Y%(g&*u?-@7ZJBl)+lELVwTd( zlKgeGI0ygoozC@o%in-aWt#9~^Hb*eeckGUuSdL!ja|Y*Ly27+a9-7(0pOd70E`X@ zD>2RL=;j?iocL>tu3z_9-1}j8xFOa>z&0&23}dG+&cM6^ek&l@f+L)4-`fpmRs_5B zTs09(-8l2tXDeRHyzBVWu)~-v)tM+|@%{_D5y&~|2OKzNjo*odgS){qT~E3^(ezw1 zSMFzrUGgYCDL)S)i_d}YVzF?wMt>MPH<38%g2qaCYTPeL9~L`n0LtTOnH%~ylkG*5 z&(TE@vJ;~UcAQnH%vmq=+gV{br5=#4rlApoDOU9uw85E&1M>Ay-jhwwk&4ug``dvD zmy8k!QbA$_93C2&46MU(6Y;6Q3O$FZTCZaB%Si3zHL_%G> zTxP9o=%(1XxE(r!pg)kk>ozzB`0{kkJl1#rWvvd0_+6$2)SrjXl(Q}qgiS}fqg?`Y zJ!n#uVYtGFt_@vRXn*KfD6}*QPq0m9f|k;aWldr*tDC%gI&kTW8uF_>gK^h9+y<(q@XVTlMsj00; zbts#WZ2z!z=V2vNZ(LtplylK6{SEX|<|ewsPFgUc3CCVmV1T-!HF zNPr`X?}E&+#Z%8LqbQa7h2~yn2Md=3rSAfaKM}wE!N~o=QDSsowb@khwBE3lb&t!X z2E1U^z>$St?{==K0nhDL_GFJRHDnb%Txvq$)eH0|_H{&vQO z$?!uJzG!TDB+h!(J{CNZ9{6qyrqNt7%Xf3k{bxwHKG5HfYHkM)j?FbUs-Cx2$GiV@ zB=X`>{3AHQ|0Fg(-h-;}&d@7+R_sJ%+s~nwn)llmV9CbRIlmh>5K4*`E)Wp!x+3^r z_V~T{_;?JV0zC&D2z3b-N*@Q5X#W{{UX{a)hrwMDW-L{urK{O!uV1Ej+F(2T0dViw zR|)^P_SWC(pMIs~%a(z-s zeb)(EalDaB!p!&l(J^FsY-wqUGIa6c#SlI2aP-Yfb&bmYbHI6J@CiMvW5F~F45lpLx|M|DMBJfIBN(lPYbaX`7JY~Pj=t+=Y zYGG8c?$$%GpQn!&AN+u(rsj|LIfozQ`Qu+7Z#%L;MX-sLYcE(nP%pBNFJGLutdB!H z{3Z8%7Pfi6F)%ttfTzf-04c7l|kpNq+>s7Uca1GCL-lu}VwNq}KX$AJcZ9+viO6V`s7)U=nYnYL zW{7G&Ma#)RqfHjN({S{>wTU&ESP9yiF}cb7DYuSBVq5^RhU36?t4T8|B~dEv#xtip zD-KGo(kY;-yE&JQrtYT0V#=n+4=ia*nF+-N&+jw<>60D&!na6E%fSQ%>0S*or5=+Y z*ffIn`K6|2a+csl=Bcey9Y}G;V&XNS(57m0%s`i=5aWNMZ-)`a^svysfaBsrka2%a z$e1`A9qFW42Www4G}R$^2_0AYclgc(`>+or^~lN{vOW@WcvXww)$Xr}GtKqD=MM0n zlw)+CUr8*E95OO{;c@dKtnT`P4QpQ>HzFGS$W^Q4LA{N<&n6fMm>Dvq_ycOURbnX3qz#&z{A{Q+k zrk|>p8-F=9kfC0+KWJsw1U4v7?pGC^BiN;vE)4?Ky6$ZBulDrIusQ{Y5h!#$G}BCl+6jR2 zBCW|fM|!APkdFkv#Bglu@k&^6d}`u$mMiUIh9g2TI>lO-q1h6&vviXvHb5X@j+QEV2aboZlza1XAzUk2mMXv$J&0N_KwB(lK#zP0r z*JoBi78BWPH(4}w_esf<4eL!8hndQX>&E}#tNR+$^t2(7{D<6WbrqNDVBWHzU4zk| z5KU|;6%`d^lGeIyDudx4D9qVR-PyF?tllc&8elwR>AIKsdzPaZtxQN?>bWh zIw0Q4fb?&;WqSGT*ER8O(0>NgmscMG+^OCQEcm7)KM73W+cLd?D`&Z;gcy4x0I_WIR4ESix{H3VtGoe5?k>WdVz?)fYf(n%j*kukg@3&d}OSC8-R`a9$z zKO=&** z>B?enJYKNh(Voh`%HtC*BXbSQ@VF39`zsm|U*`^AQ7kk8?rEJ)l^jAuSapAf6_`DVT zDF>hER;}~TlrnRD{XE#qgJ&N%*X}!a-S|3h?pVe1?Ae`Ddh%!46cvN|_Lx*xnGMO9 z^?j+fma{Xfwi-(hxX{@Za3N{vymTBcuZ#>GYY#)6pb!3go_zVDIUvf9lCs4t83|Qj zAV-vzSPG^GuQ^6*?sHQfxq+!^FBY-=l9jSD^ixsaOpiCX=&j75Hm#(r`|zW}9KL*CQtGJR_! za%$|arAwDmITF8MjSW@XKyKZ=bLS#@dMW>Ri0aIXx()kac@X;K33BSEpJBBww05l< z8etwDj7*3Es;MlUTmo~Nu842gkei?s+=KB_*XLcXN_v^i9?Q5Pyh*`yi_?`WS4a;H zr|e{XRni4)dJv2a8?~O5-n(^+nVFei7C}1y{MRa)lKS&{dU}V2m{0Wq=)W4nb<60! zrAVv2JLCQy;P!o%8vDvGp7gsW~5}9ApoxOX#ddFmA6=I|2_%^yPZZIr*K9#zP=RD07{6+r9ofnK~Nd_<>+N=R3C1nHm zR!AcVw-sg{XAm=)ub$G=bK3B}<$=eXZ4BKa)QEJT4ARikADa5+HP)yGbPDcCD7_h( zn8?_tl9GnM6~m>biiDrQz69-Rwxvs>(6B@Gf!c?d=Hm}c)748(F?mzBHaBAu>0Si| zhw`0>)d20+FsH+y;^afVE{+XtV&+bPg5@2g(GJQv!3_!z+r3rT)Aap&d_uz2w6oR88~OS9MMYKb9!;~1llrWS&5yx~ zmrP8$K79B9A)@wy1ANR|IB%ahr7akvnbc=d<>~1Ou_g8|&H|Ldr;rrZthoK5s!C{( zOO~Auhd3YnPh?Vl5vWdJ`?hVRbaspoz3$HW$vNIu`f*uBx@Xn9wzWM-KNz(0%o2x9 z-&i^+$JJY96Y?l8B7bfqc?-$`F)sOe$7~)zu7P6v6i&M^ZmNbx^bP#QE32#7SF9M) zTa^uS=gRvfX=!O9F=Aq3HQL_qz#l^aB|^i?%bUv)EB?aoNJN#Fr8+ckp`Y8*L`5Eg zlna*0>q9&=`Y}f;TEVD`jnX^9k#~KenLFpu+cVDO`~Rig>Ub?;bdQA*Lk@TTd1}fH z^`WGVR7jeC1Pw~oJnXYMY~8K%a`W<@az6JkjLKLubFbEsUUF{~}YI_28 z7&kZf$)r1KWvpy$kGxjD>0N(I@xku##M1`_$G4X84{*fNwHgiF4JqSt+fI(;e;$xs z7L+>&VmH+7(pRVdHgTTXTbi}+Jar*|ja*$SI+@5K;TyEvSO02muU@Rq93NWq54$49 z4k2HIsZ#>DuN)j4t5)@k$GyH@P@po<@U3NH!>xL{$(tf5D$tlVM&>W4;i~?H|6y6hy;`JI?v{ot&L9_qD)Z+p}*@=-8D{NHLLG8H()( zLWh1-L}{$J;N@d&wpGo|1}W8@0bu4iAd==RjX3x$DAt13yj!(*{UHC`xJQQ=Z9C(3 zrm03a%HGA-=q=+aAc1O5t;l)iwXf_&l+Jn0k-sx{(p#>TGQp(ZU6LewoH|>++cR>X&eHk4TLLJaaZ3!w65#~=$+WPjw;T2Q> zMWI!zR$0SB84MOP=;%NV9~>AUAUpm?@7rC0pOZPI1O?xmjM}v78YId)t}>5ose1V0 z#lFyzzmUfD_q*j>t$=`c@1d*MW>b9hx+Xkof|j!J!~WN2~Pdqq078O|~Hr zH;{jZ;mIhmetmaNm&pxx<`a-r11{yMz=;$T5_0pvCHS;JSbs_ocR0?y|FL z&|J$^IF7Gs=T4&UiCx*c`rxy_v{_x8o!yIidrd_}T?2N5-yMkf$;S57c3n$Pu4aSv zs}|b_;L}3*AS5m>E-RZ3i^!Omk|gc%(mqhjM6?sv^YENRzZCGHoXu$L)|l%}ah>^R zjf}uTW0sa;p+$Myb1~5o<9!Z;NQwM)`SR8LU?x6!mWxkPd)+BYYH8T=G|l?Zzx_F9~vkK3X{y;kfiS1vnM3B$ElvqzSb3iioZv%3YP-F+rN+4g0F;{ z4S5k<+(DH*Xm+>t9ejRcqNAY?;2nhtqZBSMFa&k;xXVR zfiL4MubW@V$B#KKoxIF49B&#JLXeW{>+5@L+{<5r4AsTem70nwCmtI^bJ}NF&`z`i_UO#d*_tEDzzxn{pCQWr|rE0!Nh5NOa7bI4Nta!3w z@W!$71_iOPl-dcAjT=iFg{UokN^sjdb2xG6ZIy8gsrsh75!K#JxmqP*y#Mj7`>Bt= zH8}lBM~tZ%=r&psI%$?0AA7B#HK&ZrBHFJ{N+=8np8E7=gSa^0D?B5}4Jl}j!#L!b z6dEXUc9W^q=U^xD1vXHaoRcY|rQ2VRG&C4I5%v~@x)Nha=}=@d z*Kgc_dV4RwhLO>;g9e7nAMjn)Yz(-4JG5#SQ_lC-?^m}hYXfpp;eSDRXJ45A{39vZ zNHN~*B>|$a(|I+`{TMJZAeJQjUdP~0lIBe>d;nQ^r3taCsBugd2gy3~FX-QdY=oMXlH}h!0v zaW(uxDqN;ytdu5%3K@$sCZ&PMtW+{(Ccb@LD0O$=&-1?Dvu*FUZQsiu_r3kPTx+d! zo#!#^$A0X`H#V3$$u*z<`TWn4L{7d3=1WsZo;>6n(Du@UGf75KSD3@Pl)UxwcweKO z5Z2y)@ZUX)!6Dh#CE%xMZ`uUH@G3<`MLD^pIZ`o4nm+sV9gy|AqjfN_s4h<|XY)tf zeTX~IBOian8+-@|1%9hZH`q$GPTPEAds(-_S<}xvy`f^FHh}{BUD6Owq-#m(NGS$q z-4m`NDW5q%s&kZfr7@5(ZtFTbcXrvc{3#qo1L%7bTV0cVB4Z8g@%lhmWXduoj!uE* zLRslx#3x?2-kQU> zU0iF|u3cqgR}Rk!H;y;-8%_k~silSHlyFSXJa8aBxyF{A8XB`t1V=^M!)}GcZ-mGlc1UMO*7$Cv%Kj{2yuxG2P z9|Lj^F&0@mJ8+4Jimn}7uHh~!B68&D zQMa1YhL1N<q-Vzc94#oRRsl>!41*eiEm5MT<^}O?5s4iz|nd&B9B?{?=)E z+ag$dyHN}PBZJoV{LL*o;9yj(&{eQhVLOm zh0}>YEkJ%8pW4=g*VtU-T-qZv!^ura57)O%hC*Q$+!%6FY$kG+QZzl zEzgxpbp33o89Npm*1(oi7jb7PaaflKokAHR+2Nu(7h**xI6ZSg)!YE;LlaL?~oGZ^@6|MOII&l3HmwV?2Y{evli^Ee6wi ziHr=^?E?C{$`aSET}!qN`1=bu1w=;v0Q)(e{|7{#Fr&#J?sm>4HbM#%X*|_kmMzcL(YQ;czWDU7#IXz!rUa8FddK)l|?p4h%H^pF&w~O zm~Q9hHiSN9_4oPu7_5UCIc}ef{*tdYceU0HJX?drKe!4+VhcvHOxTxThZY_qWC5)z6>+b+5Fk-6*L$}3g&96pM5jSQt~{2~iF;PHWK_(J zsTFHC%oO91`7zLCU3AyE5n&fdp3|pKFMAGPApdvEWRr}nL!D8MaGUVKzi^jVd~ z*Tav{8EX4hJ|)Pv|+k1F;fUXISI~nRSGw(Dcg{O^;D^a&7@Yf6#p)wSuX8p`_gONob z(ha7NLZBGOJ!kIq>(|Lj!)pt54GlM*WinC5b9@$#IeH;1i9V~a$2Pxi-@-gz!to`s z#e`fg=oDR^M?+m`FWSjVL2cv4>TgFs^#HYheFu%C=s5%*T@_P4~4Xm$ISda{r%P7`X+K)>RI&b81k%le)ZzN6~U@{*3x|?N6sEeD}(g$ClDd_rJGaEhD~QnT1#r*IS?)kj?|6EP%v~ zpkUVP=rw+zqLjW;iTLvaJC-l%0J>7od5>}{r-tz-V) zQ@Pm+E)}MZ&(n=`_c1X7u$s%3ZPEc!t+cStk$D$%Ml`PJ^SrcQ9U2^b>)n0Rc*qhV zT53$m%rrBpg_agVbJ*D8bwMaETz`}n6tAWQ(eYesdPO^O+c^AN(b#xqH5uwQApeg|2i9CN`UX& zMNSpcHM^eZ8Rx32g6KkO^6f34)YG z{o2PDB^F;=FIjn3ft8KzU|lv|FWHp)3r2W0)RyfvIyyQqFhDA)aPOEq<`5@(t-0X> zv|!Gh0hmqawnbooTWiQ-{I`r7q3gsiN7{3G+Mzw|zqmx8Si(IlTL`vu|R3Z(2I*;E$H=f~`HBMu_V;^OlFpXNo5uOo|g3CfQ&FBN)gUaI_r3Z>$LpjcJm z&;4mNwlAn}&S1t;!Jd_mI3Fy~SAL-~mRTA6*s?OmGbgk2#Ij@kPtc;8ttTSow>^G+ z`Lbh4Merr4{o^DokTS1%^$Jss13=eySG)53W#M=#=iCLp>WS~X&YW?b zTW$J4Z;q>#8$^MyA?nSu%O_Ux6`9jb{7$P`$$Zm_pkwE7S{Gq^`IOJH8D|yF_rJaL z+-|OCb2JijbT2j<=tLtEm3UED2wfk3F0M4BtzfT#y>Zum>y91!SE9Rc(YK?=H*7fl z*mBF!*31nJT`{IuNpzs^l$fW`M$7C^J6Yt4%mkfNXjdbu9ET4dzH6KW=EXLNoI_1# zD|eBRz=G-0VLDbSrvLa~zC45}e1Y{ z!^JNKpuQ7GrRE!2ozPcC4{7(4{_N%lGwR}~p#`@MH{IW4ZchczzP46!brqQIIyyS$ z=25a40PXejG}dqJwz?VzS|b6mv_?>Jv)r$YU7z?x*gyH3#y@K! zF1*ogPWMSf)%R2bTT|AmkibApCci$q*b{0Sl3x)B>Ww@|EdAKhBIY?b)5_TvP9^}L zH6QyjdH>4H`1s9(w=*&npjTkM;J--ZV>v($K|bv^Ewl~TnvOX|a@y5k52^1WCd<(} z_Cq{fD6)ISDC*(aW23{hoZ0;%Qey5M+U9(BOvPSe18E&Up5bh&7FlAiqjP@Eu50Re z>_&j#wFM6izXdXydwS-0Z(X}YU;yc}=j1PGHztl6ku`nhMDjT@W`1%_V)^$0Cgr02 zoDVLxjB2Qcx_rLSAmY0or2tZOV$K{jNf>UD&mZsPpYvwx*Ec?3Ik$-i$bB$_}2$AwQ7r5^xbkbKcQ9C1UhXl)I5 z#ajOrO2|u0ml$U$Dl04Z%mht$!FF|banT?LfWyWZexrYEwvXDQ8gA#Ji-j@652^Go zH|2y5QtyhM)4X$2Y0j*-XrF37(Qw?N8`~tgxh3Fk@En|)3XHiiOze>TYFoMIpF4&{ zzCHjJI;?lRa@{+PFZCHkHM1u6Z+n5h|Azd8t(zGnp?y}t_4^E^_%)qc=A)l}%$n7k z(etd6sxGLYfS#AS1)jTnRB9k@X5IBE){tI!_?_F4t8bm6i%q(VOQ(Cf#3Yw-)w;k(~$ynytit?77Zl=uCOaVt=yGUz# z9^{+#CH8(%i*Hd5PH7Gfn>*UBY8xq&+3UD$e^n2rjG!g*wMeT(|2W8R=GyRNTCXTI z#dsXJ@MC_n)_;W2i&iSoG?84LWE#)cZvg{Ar3!s{2AvHOz-y5_&XL_4?*L5wre6z{ zH(*`=8Lb&M@HFX}9tJukFI*v40yCW{MV^JN1GS-9&|fIwJA;DfQYrgmjh|-;3O#S19yqVC`2#mh ztM2B_NL%KF&5yP+^?0T}6c`xzB0|2Yn{7e-PvhDzE4njePjF^`>a+V#G^oN_MHz`w zKHo^$TTC0o(BQT>99)?H?tR=Mo!O(kiO`60dPy+0&tLsRYVvn7XlOOab*86T7(#H( z&i)y~DU%DD=*e`^Oun37!XL1|g%c9{L$%7E=L8gXV<160oL>zLC)&)_UN?AP7F=nm&L_Y0 zM;+@V{QUfK+#SdJb}(s}2sOXvf=20_WCH6w!!_6+FC-u!ymu?~j|$5_Ml17o*$^W6zHwD0fffx6rhVPWs* z>P6@*yIs*C(@9RO@$OYuYP?FvGE}m8GBTs#)p5YA@k>koXs$l+P@?L7!|wJC@u6eK znn?@3_#zCrSD1ZSyesh7PuEZv%3A9pCRq(zZv$ z7HVbW2s`gbLm;Pt7tr?pfv>>=9+8gdlN$r+x0o#5KrYK|1{R{&pBDd2yB~sl7)!Fg z%ppawtbvWME_aFNx2qf59T*SGH*%RLF9mbJ4pndX?yjybB*;`j&hAvxeOYVu_owUa zUPqd4cb6D#?)$ZFt?UoZH?ZBO$|7ZgzH|+`3LorwDfF^fj#=w@_Zb8EX(IeRCpQYv zZCg<6KN>n3`h}pU$j0hj!7p9B$dsTt^FxFqvnufVQr)?{8%O|&RWmq`z{P9|zT+T20NTK=!eMD?VfM#IPJU+gItHYo6QXkpqj^`A8 zQikI>#l4|Bh*HemZKb(jqf|at#2z^;6A5T8#Fs3+7Wo)(Mgo@GXfM)tthHP)K3;h6 z@-x$C$f&0*2HrE}4iad<6wB5m=luBQFxKg%5^GF276Gsl-EAUUBM6pDR@R4usSdxh zLYyD`h4N8G*3!zq_Cnps>e2PRa5O^QYI+_up};L~Ne8%UgS?wz}_YP^aU zX#tfzQnDUVFCZn7#0O=iX4#@W5bIfeE)xEZiu(vs>%lkUQ9X3OpBgsa zf%Z~TBuo6YYWvmt=Y{qI$mWc;8iKwpb&i231lM05N6T--up!lbv%_+0fOV4Gv*N8OW zq}a}2EIEG%$Ex?_UBmsFnuRZ~Z7c{WL|AKU=p-PcAS>H=$N`iO7&TC4htLX$0@iXZ zif$Z+YZ=%@E>?`FuwuVrf!4f-g%^z_MI$V5ZBkXKNoAwyM!yRe=2v`avGR${51JmD zOLY)}cC_q&mP;SVs6$my3F>z0Cc-noLr(czpi1!ZM( z_pO`8jRXVD3;e37P3zqUva!qQCC0}GFOijyXv1n!7jI%)`DM%WIB4Bqc7drl z9hE=D7=_t4`+C#1k4K)*JTD5X*j*VFkxeoil<@^o8x_a#Skt3)eOq0Y4bE=uU(Rms z*ypcbef|78T3gFa8tI=Xr)pitJ*ZaFUn)C)``7I@{M$Kw&ajO`$76*zA2 z$ocmtc#uKDc{Z|d-oCx<8Th2d+mzW&h__uJ!BKgf&XVzgG=|todqiSx+;}#AC|51w z=4NF&og4_A(@0$-aMLR)79SO{Gm1BD8QF92LZ4WWUby3EkK=^$P z>H-0Gr409(IZ5 zkDE^3PmYlwxs5=?g%&SvYHIR5cdmJZFee76zvWK3Y9!@-tnB09$RT2$s1sZtkwm}P(Nh|wCQix0q z-5*t=&M|TrT8Il$f~UnnT$7+5_g_H2`mr>2tC&*^mmJb$P3_)6v+Je?}yecX@%@qACI3&e%WQ>R*gFWd<8%!E}M%k1xR6BK#khS^kCJn+G@FVW?1&&0G0HfJT9=ojJq! z=_6S|U@(b)>+kOms)<`5=_-4vm6esXwdS^lI=&)UKxoUM@Kp!0ZxQEdBL9A}vkdRh zKS;rS@B8fZ@#!i!JMKQ?46DKoHJbBy&H6<9I-)eS%;_Kf#U@OiK5Ng6(`!=xpkN+0 z0hi*5x48JApNVU!rkR`qOJt|PdsOX?J&QikXA-cw>z27O7$g;+!QMt+;n48#(>4sr z%=DWTe+2b-km;>McDeKfsF*2vq0f|t3*f5WIX(r zAac_Fh`OY0fKmYjYSq-v0JTrq?dP1H*HBnkcn`dTA-JQm($aY)CpMjZvu`t)=rd=| zB`fO1-9LK>ic^5sm34MH_D8=$HTw5%g4%+!HjyYtFS?*{NG+wx%+T_IU!azHRI-TUO7OS8}YU7}DSGo>Tr8ze+5VBTR)S@;uP9h~JA#lMi&$mgOF} zDh!0%mMN9uNy8*RR@^P2H?q$w@V?&qGOy>i8vgy)-2D$<^Y}<LPP z#V8j#=^tKX5~8^Ie~_l}AN|MPZ{^lc=Ajke9g#Eiajj`Nos@|jlTZo>iNZ)|d?jwq z{VcPQ@5QNPH68Ex`~*}oQqfn|o#vzIZfAExGt+qYIAi1Fj{k%Hqc&PC{>*f!mg#QV zbUPto!LiS+tq@y&Ybtlv=}vt7DsBW(7pzo3r)E06&ZHua=hk@W=i&+(jL^-{nOFB9 zeGP|?1&xrgxBhP#&i{-9;jPv>tW}9>2&o#EakuGE16T2K(qZHacuOqZKNPgM*O57t zS8~V0Qrkd8br@E#ECuQQ%JAn{<7Wy8ghV-vbf49a^|+ZF)7nTVh3G}vi#$+n(NuyH^J?Uf4F5@h zyqf#e$V$j@|H3SAZB+eTq0%7+8V5nlz3AnO0FbO352i|%Joqhv0#UIw`8_n(@nlD_ z_-EBFI>RaKS{qq*!sc|rKh^?sUPND`CsxFpS>nM#!@Zg@kGe%ZGW00Ycq18;A?+mK|D{c=jVGMNu6>J zxPj3SMxwCGaW=wN#EueUZ%Z23erFA8ns}gl9bgL@sAQHaqci~c5@z=|TL0dbTRBBG z5ZL5hxG~8(DI`&{s1=h!GeodYJRy0d)v)g7k`o%5>N&I3yXm=H9P7P zAhV%_+1WWA!~GbDORS1!j0msSUz8}x>2>;l>oqndf0WtvMajFz+u?#n z6EYLwnFU*JL$R6jI18;;4V_M3Ry}Zf{gbM2QSucOg>Uh-$JmQF#S52;(EXX{NRoT& z$@yo?mqBW&3U^Pa{@Pi&DjP1kM!s}iip@t5^$uFt6n;uHBf7YR;(3;*3#wck!mb01 z1eni+4in#T+7a0p5+^Rx4v%5*(0RSCv-2iL(WV9SA%fLLevd#wq#M!(lEqpdEx0kW zY>(nR^1?)CsxYozz&Ek4X;+7s6w4e%cX|RuLqe3{=hcHY#PFCVr+Y@3omH@GOH;?^ zloZG>DSVxCcM*0kp{2HNTURB+`0i1xaLRY|q7+Ged#&?q!))?{gr_z=(d?SWf}8?b z7F!Jney5|OmEnExtYb|LbHUbNCZres{78tWcnFj}F#uu}dKe&=2aW+G@l4R2xoz7v z5WIHpj&pu+$0#vjM=BaUKy7!x_C!y>?3p0ELrEMm;c;!>&>0FWdv=LP5yAzxEM;L~ zH51I(Li^bQK0ZmZ3ov-q8)1{V%&SlbcTTC2IC|ZDaGbn{lw-CynpDvjVEp3^hSf-A z%W|<|2k`F*(`Nf7W5B0(=64_R>is5aB5~WcXT}QdRP;K>>}DpPU7k;po-}3~Lt5(2 zwE*~p$UIg6JAgbX1Zo)te`R<2a`R3xFsd zrJktsr%ur)1t;g*rT8de(8({(K=E*(@gzS${nP}uu-(}?Mf2gnVL2Z3cZeI_UAlbv z5f5?{8K8nMU-l&h58O^mb7fUuB7a{mY%9Bqrk0j|M<==_+jnSn&o{<7+ojQT1td+p z=o}F`!fDDmn_p5rwK?p6G4U7`Cd>7Ur!yvr+uoUQ3>IkXqp|DBS?~RIo}LlY#3ycS zoNQN=z^c7z*4&s)lz~2@5x4>`|?- z)1Ez=S;DJVqgT%;Y(7Ah{w4Zx6^K!9tucCd&d-ni#}f$qsCVZ($ux{0(EWlvz`y$> z-o3$%*T^?%~^r!3JClY~{_(a{!Lw>sgCgB11n z@w(7A@E7BSyc9J5s~15Uyu*j@ ze>mu(i$N>vvP_Zrp(F}L-tp?~iOn`>%|RJBs0zJ)EiU|6ZayV9;kql$oFb$@xKj|{ z4S7gjh1F&Bp<|c+F}wj37~18*iIO`X2@lH5JjBxP6!@Q3p2sA_^J|Nv7m|A4r;Uzs z(0RbDvRH{6s6<&=S!rwgURn?h{ySzBD2wEgt;N4M<=usKH0bQ&`SY4bA-rKtqqqb= z58g1Pw>b(;VPtdjvQI`<&zqZ@K|Aw~2eAxZrC`fp>IQAu2mcNcb8($O#3uLiR12A> zK5ikCM|MbgDS%!G9DSpj8W-zX2&Kt|D)8X0SW$Yo>p5O@Osa{qK_>Q#CY~Un;xXb1 zr-mJ-xE~f`4flm#L|-T5LgZlf!-vs@c|b=5o_82y%!? zO6qEAa(FG6j!`pYzVUF*LZi51ky%vtX`kd~-b4uUB}iGGb0d(Qm#d_47nP^1pilyKP{puTJp39(noqZ=QPVHFpIh zPswm@zGrBCc?sOgaYxR1?9p%rUZgt+juf46fVn+qQCbtd&aKy$At$=ep($N`mDc8J zq=8_>l9L*N39m)hh`jvZ3n-YCMXTs3un}f8y= zh0qYK=WB=VXV0gs%^e#8@XkokhGfN|A#JSUV5*AyKqr{G9gt23s=23tj^ew>t&j4x z4eqERhV^;@bz ziaR&!=|`(2Rz}w16+j-^YO@s3M(}Qh@ajRRQH!qT&l_BjGRZNM3&qf|-?(+#JKow> zqbcp&24DKw$LeStxqHbYm`KQ3GK(n>T(ao2AKL!TtMs>kUU{{ghNl(?WB4)DX2-yM zFHSw0p(H5S0L#V+F^UD8wsO|M$6+OQ1U$gb_YKB+#wUGl8KUaMw zAD;Xra%~P@ejF5uUi*rsYCp4Q-}lCADAKmqDdm#fd5etlnYMg$gO)qH=fH;EGh4r+ zdIa*?BzKMMZ5UpD8!ehZ`+bj~%k3xg96CWDh-RfHH`GCAIzuOI5g(s2WdA_nx3;n} zKhe8AiP@rh9YDV=_L4`PzPuJ|2{{*xsIJ%JQ0vdPZQ*n z?!vAeiR^G+vSdktd>1%nw|!O9RfHf9CSPay*~u5N^yaW{?d=zReVuhNG9izH!HQ`x4 zRe<^SfBQ~j%hxuM-BXwey)B^(#n-(kn3;HB{MbgN1i9H)V}eY=pNnp7m(fBPiyjkq zfOyV^5tl766{PGlIxOJFw8G@(-YQ-8`{NHP(;@gYs7cA_r}VTT3~fC$u!D-@{LF7^ zxG8Db*h@C&A@(7=DDwdyfphnW1@V7S6;QxP-TwS<3*L|v_)~MmGwc)GMJ|GIyXn&* zBhPgv->}EWuBi-NBgYOPu9~0I98S*~7~RZ?r)@pddWf*J`WX+i4(u*`9@D^PI*k)o zNLLrRD3Y$;t2fr8M;3}tC~fGr5E=v#a;%Gg?B(!|ZIa^6pFV{K1(B1}U0q#K&%47h z5gLrPuhKu2ze$C^rDVe=20Sb-N{aC5N!Dy3Z&LZg)vL4U z3AdxN2CORi{(Q3U-;<2Y5tMiVNU-?cY%4v z$2LP}*z})!kR87B=F8|7VRQ;3o^CIgHFM_ip4I?aaa~aHm%RSxh6mlP;dID=3_g|e z5PYj2+K@B)HD?rKQ*H_Sm8rQI#9lktXke0VV@S0ybw^y?e<$k0iLu{zmsBCQWdB9h z$pTD_Ag5**NhRVR?ezLG{W{AZrU$Lha}yo((Nn;6L8JF$u~FmFGX5FcMQdiUvq#Gv zvclkAfC=QY=kvKMZLpe~vr!Vod41`jKMQ>Z9_isDz$uyI^78!+< z`?rj&Z%(Ad3?B2HQchiCLE>*}YHDn3yl>wZB(*iZ8cqfqNGkNpJt*2kf(2ejsdfacKafxKPvQ` z#0y>VG$dBiXFWO9d z?>M=qmo3`f(Elj&@Ip==8mj(p}bj;hA4v<>$bOq+-b zPVzbBf7N;U>J>FIQfawYI|fceJQr;CA`=2a8pr!?9UD+XoZlk{-i|bS0|bqI$@~tu z$$3?FTDTSWaWeU3v1X&HUZReFLF#fxvE zB)YP4)OfD_^D|Di?6fN(3#YKVvPxUOkct#goyiKUgxnYt*U4T)OTx+P$mI|>uh2jP z_P<<$AnxZ*`Nt`U!oax#-17Jrk8(_Y@kEYBesOCdQBF^`Do|9;@N^XiEB#z320jMH zq0i2ry8DAJ_1dkPxP){T?fPbsBuYjM?3hEiKeZ;x1c)zEBnN6>L*eX1{KL9aUI`eK$O()eezF^7h}|4SFDI?kD~Js;j>6%eCf)*N1q)?p zCb=+wy??pW#Lb*3&;I+`=PqPa{V@%RKAk5EZ6o|jn{$n*$CCqx&QJ>2gM=sUetkRp z>NrNgF{c<1^I}60(yQ!lVI072aHLd*UivXHF+~mhaP)#m6PguaRD5k-(3FGjJ$mHk z7TBE3e56W4L*uf}4Vt~g9Kav(!gM;;Gj#+hG%guBC`IG~r!Ih#@_83sNCU-yp*b$x zFgkb4W4!?NGPFZ69&cy5lP%Ji#KgoKH|!we&?IrLi}S|Yw)%QYNDo_#wpbu<)g4$U zeEwwafsAM825%Nz?)8cgk5)VeEhvM#du79p zwpGd?3y_u;c=#dJAv!q^AvR4y#HH@^@Q-%%evCkjnEQbyz(}=rQ0LJY{C~j%qhm7uSSv0MN>z|z1!=>%fBfr^!rCed5n%6%lJ_H<_$TR z2ISo1-{0Ikj`8iQ$MO2~0UdxV520{&2}8`&+U1ySvk z!Uw;>HSP&z&fQjb=E-CxLG!b+eK0Mw(3 zJD;dTS#IqAs*g|usK|EkmO)%lJYo^32t1Gt9&f=Y zeNStL910Si)ct7#8u#2T#rY9N5zEU1@T#(reem$%?G=sY5BFWI8U5rry8E>dIUH6l zqGYY_*fiR`T^=l#6BH6a@*CU!`UM{l%K9@*GCBq?}x z5&KDjuzqQ9tCIB`vbm42!(T%*wD<|4My9V}JGyFp2Lu(Ar{TdPnQ5LLua!P%STcjZ?a8!>k$6^w52m&&)`VO0s(2^@s9w2po5`y`rF76IiZ06U??fUd+bAcP zJXK_~47)l(OzPF2{gVrI8=|5}IT!4<%l~q8=c}@pj1$2N#b(y5Ac5JcBfRR;fn8f@ z)vDY&Dp4=4;%S`ymtOzIplNRx;$xK5cSp!VfU4uq@ZxN=ibUego37-f0LaTjM#)4X z1n{G-iDl$ET*USG&W*E6ZGQ83#$~iGvoL$7|Hsj7+$9f3OAsE1@K^X>MrJ$^ODkbDws0-gE!QgvC8y9=FC(fBWE6T~l*2#Jl*@rEjX7AE-Fr3_~E*H)OG+&ZJjKu3_gjXc^?P4S45 z5ig#M@gMK!-?6=lpWrz#C{oO^|N48?y@^_anlt(?upTnZ`&xc@k&wyj-ktk|ePu=( z_(k^%nT*h#U089XoqbE#W5$%Sz3bi!YM)bYp7#C8xtW6bE6*_{S=*Xje`u5`d(^7G z_VABGVy5qpmkqj)rll+ON98HK_+ImASh4QA<3X?+P^CoN-Dy*}hNEv82EX<+tySmP zElIjlOUdcds$=^bz)sN?o`WMswupDnI+Jydj$r9Z)5Y4Vo%Youyyb{%R=E#4w=H{v zEEhk3enH{uXL_Qmz)W~QDc?CeKWyH7!W(6wTw;UyOg+&+oH9(fG=@BaFIuy#2;#|_ z1OvuHA>o|r63Y=L6PE6?LetkCYE%PL4`#0J0N*_nYl#xxTpI?$L(BynwR}3R`FYpl zNZ|#WA}X-u!8ORPB5Kq1t+*=af1n?xFZG=N zlziME+T(_hwIGfQ>B6~7x&5Oc3Wn!JeWz@M^3TZj{M%dv<=(}w^XNnu5gfB5mFWZ; ziSy@l+?IvuK=W$>C&?~n9b|f9{*)lj95;(&X)OJ~OQ_md2lYCQmzcH;4m78Pu_NT18fs&L@O?Ar2!R>6LrM|Tl%*~X z5+esd#8u>Fts7m|_F8~YReN^ioIqL42RcFx!KNhz43yoTr$@F|-daacyw87~w=Tj2 z9C|`^|9&vAZ3g7s9$YrZ*?z5`?@A`GV}y$!PfHC2jii zB7@?qSTNQX-~QXm%=-1;*F~gfFpR(V7HZDc(*)6+kA0IL)dI{V>=rmkc=ybYEQkzU$4!0%C}o4(-AN08|M;2Y`K;GQ?Ht#Ku`*C zWi!dCxA=b-ChWgjm}^TTb${PaQ{VWA6Bt$)wYg`{p2FnTf43zei;X5=p#V*^1hDlp zU7y#t%n%MlpX-lA29Dtnvd8ccp0oRnh57^sBV7dc9ZOD>baC(lRO@&W? zF7kQ}qLJqiq2a=@W9QB+=-MEg;cn4*aBe<5p*oLdZOq`r19pr#;{@TdZ#TQnS?>_+V-+Kk8It~&i_$)CwK-erf zsxf89_T_2)gHs$|IwCZL9yD+B=yW9%1BK`KKy2`u*>?ttrQxf?$RvBZyQw#z<4pd!wzR zaCHCl)@D~EMS?dWw!`=o=$flX&!t^-JS^I}ZQQJ=o%UuhOP9P3+$KQtE(S4vXIL=A zqt8H4B52lo;&e)fGIAwoYl#M;fChO6kVb}hXiI!>a)}+grW3Q7E$3wfp}7`;GQ=?- zW@PM`xafH3IrXCF4~B02X@0&qtIe8tUyY^X7BtsvjwMCR;0B1k%8|NXol8k0_W7q} znD7*bMg|U@(7pdF6w=bCn^;UCwwM>|5DVp#w>afnRim#l!EUx-L8pWh6lG>|>s_O5 zJ+Wo5NZ1Nj*X7go$lpYqRe6EtdB$HpBBWMlGHxOb*!YV!bEiklTK*{pUv0ILjHX1v zXA;UFZL+uA#FvwHjkd1|_z-6;;>1O6#_=$9Y7jWYLq>h)X?8gF=J*-#W}3}B@#VBw z`r}m=GH5~+AM=8=uVC%w$Z1){_!vW zswr0prAb+(W)copUp2K7_J|?R>0-qg!k6g6(_wXKau zvJtkx7NajbO6k4lk8i;xKlcOqj_awA>ctm(xkGX)B_^gGl0HIg9OC2aN?IAjVB;sj zY^!V-8EK%Cj`@WMDT-%gwB+YK?E38<;t;0+UslZrXFv->b+&EFr)$)p9mYbR$&*2| z_*_1R*G6gzqc=cy2e?9zz#Dd$eTXtJzB`<($bB$jB`nXN?hh8k3g;qp&Lx#s3Hpmaplc?6=yH~ue6_XHd%KjCC{kMKga+!u(o zLNtZ9-g=FhFtx`%@j7b>Je>L$WQw;QiJf?WrdjvcYjDFRUsfA2+W>*Hu`!q8Wo`OH zt50@ToTnqMO3NBCHrF547euo)I{LsdOc)D!Jagfl+qWUWtag;5|M_|D8(HK}h4JSv zz*Zm?rgW|aKLLINyjU;aHYB(v$u>UxDMWerg-2*|S;jaE$rJI+(<%Y0;>1j_JOJSV zYbR)dAqr?qkaKGGE@X>Yvmp;3qwLWwolhtKZmF#=VQGUs1`5%~oXS3n!ZcC<|Iyk2 zUR8&)XOX%R0g;!)5uLf<8#4fjVo%ToWL>{55}+BoRqW4?kxTJJGI*2793O~l5o!(G0ZmMto;m)9kUZl@NP$wzN_!F^Wf>zz zT3X043qAOt39Pe0WuYF!B&kKQg>3%eSyl7D%|DXSSly1k5OMM)1@)8JA z92@x##t6G|A0#74sfwB!#cck)owJkO3r8K=Ev8`)dH#6k`Qw92-G!l|8P2%A^hKSL zrmK^C!0p!_C|p37`{JI}({q4Hv33!Q4xVdMT`Bq^G^gfDM-eE9b-_@`bs;p`ptnrb(TR%7$UHO zU|#g~F%4%!aWIS_8zTm>0Uq}~-dH#)=w;Sk{t1oXC$>vJ30!q@8un?;Zd1O+Xt{{M zfb-qEciWXvTG)Q=eF2>f3-zx>Pi+6ffQz=BPF`4Ox@v%`1Ed;WU!Mnv_Am)MG5BHZ z1A5wXpxTOUi5UyzSQ$UX?m|uTN6oX*(*SkgzV;7`?AiIy+!0vy^gFJpcir2k3sF?Gy`UNO1bj6pT$Gh z%WdEj9QG>Bu4zC8jJM&C8o6Ovrd4#if^Um<3*PSQXgh`w2_m`kp5&`ieXW{DtglV# z858*j1e|8bfSzddS(422soEiDxuj%!4oW1)KfZvbY@K7?ti{JZQ#Yo4O~}AQtf&S{ zIjm|w5F|V(Z~OTWFf1_{q%<1PIh4vi*`A2aMLB?-cWuz%clZMzwW}S2pt&7PZwW!G z5(b7sjK(#M`eIZt1$rSM&S<#u%qDl&`p5Sb%_nVzM_*1x9x6l-sByZgU%Y@$U|-BG zLa@VFHYZ1V@2gmFq%9v}rtFG(Ig)FOuQ`Z$lG!fca{+;!F2Uhwxja$&JvT^39WS;p#qM5~5%;cEYQzgjZu=Z=6n8H^xP= z_vT63q|=o$%GmFJ0_a8iPJ}fn9nYg+GNQYIkiq%6kw6 zVsp`emcR#$VEXt11={{G8Q%uwt?0q;>=BeTI>m6PMJ=oC1qu7tsJ(rd2nXe6m^Aczc61!a zG$C>1is^tIaVI4U&wBD2d1>9(;cv-Fgx#Dk`q-n5U+JW}4yl46Mm>F#YYT@K_LvS#bcL zGKgc+14H?^n;R5$j4dpptA1eEJgaE@<32$@|CSAQQYT5C)65y}XO@UJV#}zuuUPJP ziw6G$Y`VTc!WgWVOFJ%XeaJ$QInO=U!=bymC>8ypD_$s8R97FD`Gk|Qj-+Bj)z70d z33Ve((CZ#`5#>*$@)XfAn|Tjm@^0`Td&V`nInohCsewi-Mj3$~^Tmi30Bk`3u+Ban zntkZ+fy4jAecgUGVa>Hlb|cwn1>!xTktp}>A|Ahuk2mgwh$#ljqX3=+&$uM8XlKUJ zALoZ1EUr*XJ#yiaMjNF@g`6$A^m(Rp=@K8sOkgv4l-%xP12y@nn5Q{5fWZ~(v5FPX z%F>drcq)!j#kZXhh&|+a*v}CU7k=>3-rPPYRQ6o);=7Cu4d2V}-uW0g|7b=IhV+nS zK*eKDXrkS``8h}o8r`onFLuHQfMJNaAnB-iNi#nTR}m78-` z+32{#4`H*MazS$jSvJJzhSbzGmHjx@FL*l9iwj;f>qBj~*rKajF3y*XI%5 zjs?lk!bSLiF>0%Dl{-*PbPVMkx8l#qM-hG}ZH2S4V6yI3-?zJ9onI*d_D#}Uz@EJ@CuR_-obOf%to4jz;g6T?1oxToYAv0}Hb zU1Bwc0OP&oMe0K{5;6sjx!7*g{2qVD{h7brIHL{o^W?tPCxN|j?OGFv#MSq9ZMgRX z0u!ukX`uW2s(cvBgmji%ng2$v1}CrjE=-{7fO<>U%UQ)j3YNtbT9EfL`qIYEpl0rJ zd*X0?sOK5HWO9IZKOE0oJ;i{QLYUkGxct}$Y>XA2QOYNV&MrF0RiquN_++Qchc`7f z5mh9EtJEAb#pPki9e3Y9{P`8E*6vY^YsQT|bYVH#Hny)*vC_$eUkn8;|NcPCLL9n%Ct-&9tSB4v4;T?~uM0|lWm`EHTeNcw zKu0bEqov5DyV}A=5o4f*v(is|jxp+KCD1)*Qe__X^(+6~uNd5V?a$f9?69ywJ1=W_ ziIZ2Gie12o^gI@oZy1J6#y}0DJ{a&VsiNYE{&ka0-N_*ejC{x2l1Q#;+MaX<|Ge4E zXPc|ZNW3W|$){edXjM!V(VFYoHG9Y*di4QQ(D&-Tv#X3a+J`8x4#@b{ANv@IvEjz) zPA%gvfmh+E%_v3>93SIP;c8#p^669m2eT`B_ewCg{jB0BNT@Ks6}7cwpagL)g_IMy zSf2DeGQ=f^G2tMh3Uh;X4;Sts*IV@CY(~)n z=`*|Z4bINr5i8;C_9I3nw`-{amV(!%rVDQtY{ND*pKFQ+Vp{YlV%xg@eFg>BWtf}@sjolVp5OgULwj#zOO2FSTIU^$AH`10xj}Ta zY>4Y6X=qlurJvO~>hZ6>0Ko=iroayxd|W^tP6T+vbu*5eDZi@c|IU|SDYZ9OQohoj zVA)&q;t)vOOQocYnz)%_tk;p}iWqgU;(Khs{icQOgdVBZ0A*klG?qlWcUv8dTXD({ z*8f!jb|vVwh@es11(~F{=7U@2xx)y!1!wkgwo?~BXQKE&`@ypfsRC*>QBhF{Qy>NT zk5pCWe z{!yXsbc(GaI9{bipT-QOM<|T!Pe`|e7!-Q9NR5SmS+_^j|8E(5$BapXpJ17ixm!4+ zSu=$Wo|bsITAK}0>*2X z_QCns^<9>=Erwi%m%=rqtfcbv_uV+fk;pM%QSTud+2F-yJBl>ZMkt{5cs88Tj)Up8 zIPi!7A*{fcu$5fzUb&y^?RS=LVnb4-HdM+$7h^Aa zJd>1vWlwN5rLk}5{zm959KS{*?HZMmE|>oI%=s8>YS_2Ol3+q{cMsrb;*98mPKlr+ z6AbUtKDv!0v4U6b*MavKHzyBEcyYG2VJK%sg)`dEFtOmmb0;snW26j7VLKo$Gn4Q1 zr!bcrTW=|}r!{pI@T(vQguC(A5k;W?1M+7%9dB;jxPcW!#k4}h2-0k_M5t2HHEJHO@0rHj7~`ySe}cBZtl5$n18=hJgDS>EtT7segs%UI8y`dp5-=yPmtX#vSc46JJ~ zh`{WacsNSHZ4g42Ed#S)JBofp?m%)49T}zI#)!qF$~vl1m=}a}0q%Dh$sV)hneyke zZ{5EAKJ%D!XDsYfY^-qbCb7BXXEKd{T|&DW&C`f0*G9GveJCz>0vz zvVJ!04_=I4AqitjATeU!`3%me^ameNAG)ZUiT%X>HVD-vxZ|rSNxR3NcKfnm$mxwD zv17^s{@M#XP+W-6DDhw8is`c#EVj^A-FJM|4Kb0x457Ne6f{_&_eBo)5~GtgWvZ!f zKyxQ*tVK3ki^ykf61rT%Tfe)Um*9{rvPlNj<=Qptv^7v-ed%|LLHZp2VizhpxW~`O zA&MLYEx=30v`5eq70Slyb$558A~0_napon&Ye(FIfQF)*H!B|=2v4TByl-=` zlOsQre84^!{*3te=cpr)Z>2~r-w5y_czPhvUIo|gb6Z=%eBp zl$WzeJxtJCPg~&J;;LNQva@A?(UJS0UJGM@(cJwp)rgLAkE@g6%V{vaMdwV{=cDMe ztAdzBmb6BI@!$_>m%R4Z9l;VQvBs5!{si_p81h@`Enm+{z5DprL+Xw%2M{+qZ4BjJ zx9%}^?mRZ)%x~MmQ3NIdKh5-E-jx_b@D7Np~{VcTR0Gr`lLt8@m&=7Fq9NHaAHSP6&eESNVBrjoV)ERNPtfCIr2 ziN7+&uqI4;7>eqE14ZdiQh`qTk=g?M8E$V&e&JuY1;w$bre4sy33+zdHvoSw{||d_ z9*-upzkK@>nwXe1IZEb5?nBRMPJLQbTS>W@O zeCFjd>|Rw=9049?*p;#v$-y@I_7~bcVBJvy(t$kb;;Z#6UcZR4AvmAdem&#tQ?q7w zJg!#JgXKT-;|=Df99vd0lbajua+80ub3SOX~PsM$ai+t#E0^yXTGB&gGMr$P0Y?cIVcuwxAU>Fj0gB%HfxC zQa2Fb-ZN1|WvZM?b?qKFcHmXXj~F~{|M-rWaW82~+tI4%)0)#ql-5tk;XXyMYKn>G ziMjrNvu)K37?c;XI?j~=y%8Uq!n=INvf#Ab$MC`EXp#Cr`o$kSSQRG*BrRvl1XeH( z@UgurC+1%NbB3L9fH9)I7VS4D&4nG9DSP^nu_jWD% z(oin@w^C5O#=mW%DpT9Pgc8ERkIx>Xf0j-k7`ybbKyqm#IvIde_wjnB;0-%{-$eKy zKd!r$eqB17gJE>B)~cPDTm4p&a5Ln_9-ggW>yz|>Yd7euUivez3Dci}&YKv!DPg?( ze{SlZm7+}f$EHx{`e*l{72$uHOHfYEx=b+@-cC`zYOZ`r8NLyuD!9Fxt&~e5lqpHI zciGN-1+Rm6)ea8k5_Lw3FnimnoO1Z;|Ia>{%+SFMmJClN_t8Z7e|qT1x_rj9ieumN z_c`2_N;X4P9O;JS0ct5<+S*2NGJ`VvX;8;;4Gd^GU>@9+@t_UHjeH!b z0*HgffG82hb4fT)j>Ig}LmiEAckj08OTB7Dg^bjB2P1W~w1(Q8vWx7K9=qpfQOyYE zoXzX@@+YA}ttXSkEkWFDW!E|Wu7oBL2G0n#-|fpjO~88zA+Ic}zer{Z;JU)S2B=_S zxY}}RBsy--s!c#YCKs)PVE7xl+CgGC3~}6WC5**x%lP&(H)Pqs<@c%s$SY|#w{a$A}8dtSlp>&BU5y}bg9KE!>Z z`oBSMa@T#!w|MIEUj4~9FQ~NX|6U}T6uEn7|HQR5lACvT)no#AggIR7mjZQR_RtYV zpo)XAoSE|dn6Et26fm;zo;xTRz@3>!f=85umsR?-x;A2Ui%#=P0>aU=0PR7JDG^9052=!D7i7lq1Ac5YM} zE>&8h|3KWii|DY^uz^sTgLMFnmnq6#S?TlKkP}yQ4*gj8#XeNdB7_*%Oz%I8i$Ndt zp?4_V_YUDm&1aqV7|?DSpt0P%_?tIfFw~yKrntD621szwMi&(V5SErx={T?n4^z+P z*is<*{ivGpW(Ye+R*XQ^BZbQSjXS%@?qlFym->rM*uMT0Yy+`iS-0 zjAm44pdjy42!ZU*jT>zKTeolT3ZV4`g!_$>30kOuAiChh1@xFT0Kke~NC?zF(+I|* z1dC_2qQS=j479=SA=L>C9nIZ$_s*T+UMj73lSh7Q!_s;~Z>QOJZ*TBO%F-pT*lFA= zWw zjG|mgzll0(MfzaVjY1}BZI{4=sVdI5WHUs2gx1e7v|J)|EJV9RZR+J)JZ0TY58gd) zvvYEN7xA{V=f;h%+J*16cb)Q@u~=_CbF$^xTFcy<%uDjFU6`b|ZsX+*Z0b2v`>IwC z`;~@2Uz#z&zTHicYCB*0P)%e?`=R#T^C?1zte=!mt~LgAsjFLYa_T&_)<+*0FjoyEj)!RcF5N7BiBVbm$^^<>2j8n~ zyM?3950j&~?TqDBHb9Lw!I^@K$xFMyO3{0{#Ez0v$GPr{9qVx}$}}E|FB4xpXDUcZ zDC5Utg6il^$xk9TwQtKrN)SIqg!5Q369&O(@7xKpCMM__jeclo+=mjGv~_gwcw4Xu zI$6ap#QA*bTks4uUCD<{SJbrM?Fm?1^S=DmC7aZZ&8dQ4A>acOcZ``$>2Sis?G|n< z_Fx!YDT~87*h6@J1P?;_Xuce4>iB52hZM&g6gc~uHL0sXkwjF_1o49CMCfF71>?W- zGv<+T&gk;T9AUriFD-!CFou{zx^hZJtgOGVd$4iAk;Y4*BQK5jNV#u`>Ny@{HHU%ECgXXH{kWmpsEWX*M7Gc+A@d724dX z*zEwK0(4H{MR%mw>P)yB_-$YIm^DmTT?_pz*Y96enmjuLi>`FFIBz}S>E1*HALrLs z)iHKz_f|GSg3-6hybtaH@X8ZT$95-MY_r|4+4>-2JJ7?7S}%4okxe>p*^2(u$JnrC z7$BYjPR{^Bk0L{WTLCdVIgk3~AfAU3t$tovzQIE--#^4?xmX?9a zaryI+%8$oZKBzMcTi`Tj3SU3!^EPvsC~kZR;oSBkturYiauhW_Hdof$H*U<5ckTcH z=AZ1ds{d>csJXza_3vA95A_YS#2Z044+#w-cUpbWCit)L{Z*(U11P(V(jXJdrWJ-LT83L}kR$^P;xCJ)t1r?2mW-S3?DD%8?eJ)G2h z@zSb;?~6Tp?uqXNp7H=G+~=f@eaiqQA_tc z!XiW!#WA;`-`ltyADlO_IwV8;`r0iSAHfp62wqZ1;fpsO%imfd#3bZK4=3Byc$6pX z0e|mc*OMKS+#t@m=IFk~QRnB|T}=z`Qs5v+8eX9HW4XqNL+?Psuy}O^#&ipiG-A;R zO4_Am8OCsm}(ZshJTL>wGUIP6;Va(+`cNpBX8Nj_giH!|^w^0`P zVenwVb(;rN+6cApv0##xu5B{v$}i3Cfvrl|y0%`V+B(pEJ{)lKkTnNNy=5MP%CWFj~CH_oqzuP`LJgcil9&WT-(!yci@F_II$C&0V_NF zMj^Q%z-qdnd2BiAM!;*&wV+xsaxK(w<4`I%WrQ63v{A7)&1mS;%(PY~Y*O%%)js86 zq5bBpTs-WIF~(A$AcNcZjLHY3olH!mf)SbrpCsN8*eq1%s+;ZM{z(VFYeGj^&;6ts zS{rp6JYeV?TW*5ES#V|3(6ZA{_LWyiU+gPcJtbpFr?g_ydAD=-AAr>HWZ&yh1IG#` zF8RQr&*FJY&7X&zH&MGCd@XP~JNu+bpnqj*Fh@3sFMr#*KlTz&Yg2-sR9cSrvz~qk z#DV9c@H*?9Nxa698A7k_dr#NwPXrkiZ@1d``l09B!z%Q*U4Qg^dA{jvX@8yU58$7EA?c! zmB54gi?{DbM=L_P(q=@hW9~?kI7HqIPj)@#@U;+}*C zUX4SihM&~7H;BxVixy3Mcd9IZ&)~~#-G%2>8Kwn%U(sK;$7r8yp7`E6sOS{D1fg9TF{T3yGdafPQ z!8HsRw(~jfRE>H(ip!jaZi63c{HgbqD+7g944pq77auGAu!c4QzP`6~F{(QSi}T;ObAE90zoKJ)#8$W{K=)lCebe+lJF@Uw9jB~6 zZ{D>U{(+_d-O00-=KOG9?6pG`lB1bTV2VO_f?$Q?mp3?dN{kFt?2CHQy{+NKwiqzf z&SecFx%3fmS?GQQsfyB2$K!fY6qek&U#U)4A{@%_k*jOPd0IeQh^G#>)NO@Gy7`a<*>TdQ$QMa)1 zhHFEjQd^*h*5*i)kVnrm?dD6WSJ?R1J+o_eI!c(NjumD|*mCknPDA;b(`0@&R7 z85Btfk$_Z3?QaMrVtE>ZJ0VhYGa&)zxZ^n6zsFYUr~{R0mUujxQ{j*aEfFaM=mq4! zF*3ry#3J8cf3Bd$BpZERV1!L*=c=AP#GBzyT~S@1%j%=GeS1)!ws$I_{D+3J4svMM zz(K!~R`WY=ku|R1nm?RyXulf# zvG0P&Cb~c7+a@f%Y8Aq56!F`nD}rfzKMf_;MEJry5P)(*-X+3=Sdoo0SOG5> zD_c-{gvohR8(&-uZK&;SttbUQh6vg&Dk4NR5DPd26^BqVKZr&X+vhmA5$#PbS8Te= z1@~X;V6bail+}Pa#_=KVG{hKT@tr;RIP;)FtNHZoNu2YyP1Sw3QK^-Wcl&h{w&zhw zN){|@-a@ev+>~B5_O#2FKVsHq8v>yBTE(x)rCEJXNykl>+6T?O1t3@}qV5KF5Pb}t z3P~uK&exPL)ot8P#?iZdx;6DR*Sx2f>pD|a3b&c9&TBm$Zc?XV7?f1Qm*-day4`~| zSObEhf((D@EJ|%1vQ!Drzp_+M0hg=D(a?^h`EEAo;&VIKPd)4O;b4kr)^RV}*%%~= zCXis2dy`!9K$f*erZ{Bqy9ZO|QrM2Lu-PppXCg_tAPkHll{;TB;=L(Pryp6KFB(ni z85EM$QD_$cjRvW!>-Lw|a6&DhKBCaQMLO>6v*YLUqv^A!z$D|q34j!lmsY-vm^K

msolXK)FgQ;O3!gnx^=0z?RMTA!fdr^B46c-nju!`#Dw8HMw6SUJ9D9iyuWy0~> zVW#Qwk`AHXwkmxst!@PQg|^Fs-K2J|888oM_lMVbgWTxE36`C4#f+4^8RKq1zjlp; zUa|!e_(>7djk*>@n=jZB;SA;5TN~YZC^FZ7dkW=au1y4^=|)Eh&M8PkfdFjwCa+f*a7;`?~K?yt*XK5=Pp_%^-NcF0P_|4z)H?E*~W-_@Kz9ey5#Uq7txip)?qeF#L!EP*h??w4hy5=nwP=Wn)NyyI(Lz!&rl)qoaks|A{+3fxEDnc3(15Z#jKgJ4~JC{ zaJPTb(ND2PyZ&aYeK_m+8htHKjKNQQ3>rYAeR0?r0GWh@#LL5h;j2glATr6LkB{4# zPRmznJF6;CK(d_$&hPifGAjvu4-6YTyRSm0aJC;*{YPAoyXFS?8UcBa8iB(h_-GXBkp(!@>a42hng|3duyX+r7ib_|ms`2^^9iX+53>Clr00_dZDxO@7A> z*4_7sw0kxioFQMqx~P}Wj6fFnnG%X~8HzkORGV|Od&xc{!Z0nkm4S{Cv?4U)q(3Cd zIN^X9d9zCvE>xpdA{h*S2c5j6<2aB*gt% z#5P-hUp-C_jgg%RBAXP<->>h9DZE;++3e)~*!U`{eW@z`#Ozez-a{U% z)jS1+(i`(aYp_{0xFIhF!LzhqpeB#so-A=coEb^JZkUmq+d)U34s9IEG;b=bXpkH7 zF7xd@GiRkGG8|d!1zGhcwVRRR;S@OZ9X|P@v&xU;%gOwGD?mOpIEA@mMPGE~i319F zfS~#$-UAuv=k4ShuSIa;D8}n>WT+kVkOcBe5%?HmOKL$}$|Mt#Lm0q$QUDPO5nE zG~l1GqRiZEB!8}Oi92`gLKn^>*c@{1J(a58c13b#1h<0qV6Vo2_%Ca`SsJ#_ z2RFqTkL-Osmxb8U)Oj0L9FYiTDX0vPFU|5=(hZ;0f5xM~$mm%ONzH2WaD>(AjGd6= zHdIqD;h%i!o)sK};Oa-jrn=55wgq0Bw_?$z7oR{!2k!G_Js!Br3763^JYt4G>1~4UdwpOpdjdo{ zz(+>W zroFr<-JbBhyj~)SZ)eDk1NJ+@EomiFu?EfO1G?>e5G)K_#1u}!3u|?feGtvO3UjCb z&STW8bK#|v-YJ#C8&awq^}DDiZ9YCOOSP-hjyL2l_?V)r`SnRumwG$fnp;_@L2>Ak zn3H(ox0o}xQiZhEAzy??MECL@1R9)qdb$1UiXZQIH0I=0~A4Z@v${s&6iP5i`B}agGY!SG&>C~LHf2^rN8 zkFCRR81vY2#Qt24B6rG}uT!pYrefK_!F>_7ECv&Y$5!KKN;&#%35RoVS4e;3F;r^w zGj3derJbXn*}8Dy!c@dAlf*FOw{X%Y9S_OTJ+!?b#YFDdV-)4Ow^B%=ZZqySqJ0== zA(IUp_pIK3l=gEi(P*%Tbj1x$ytjEz6X@bvL7? zv7`paPH#?9ViM<4z@x)62C9y;h$$hQGJ75!gRemQ&)CjrAOD#voV$=YXlt+9`{ZQp z@llKJ=l$!zG-V8-_nYH!XUI9mK^unHCef`K1>`Af+nDQ}Mxp$_J}6xO&j+#D|5+Z?cm6SofA-A({;e=IW2WsS)9A-c z`R?_KLMxSDqhmsOU0ct(E|w6N^^Yk_Gtf0Ti?1XMKNl*TnSZyxAoRgq{HPl3i{4BY z&&gg6XE!Yys^6qvwv;lt-C~w!F2f3Q{QTr|=A;82UDxg$m_<3^v|@5jEoF`re!lKA zW5r6P{@3pydh%EfO^vn`zuEZt+(SoOFGRWRZ$ZQ4M}7{br81t+Nc-j4G-byZHPaqw z+~8%QY~!ys^EtsVi;r)?oQe2lawU7cTk2;e(R(J9UB0@~rZDv)xolr9TEeNrw!|du*PQ(yp2a1n$n=z@0k$0L9JKR(|@h) zFl6lVA&+y3#q9d8QkL8H(-XUL7#T;+0%+uKXCk35B1X z&|!iURc#&`w1;1Bk^~Ic`ubXtXz1=F;O-hNM0pK$ApX^dXDv0(S&~vY?ex=WMWkC@ zyd4M$rk!$K8=e{Ugy-|iOZw<##Io1cpx$y#uowq3#ZF&MkZmLrHzMgpPD$$u&7KfX z*bQ64my5dY2fS=7iGjhuo-8_U2`IQ|L@BxhL#Zg+xafw4nZr6#jXuA&8oUIMN$=_G zmAJ)cFU2Nwb4U0*YUP^ll=>!{qz>;gtRJdV)R6aQ2XpMy{miREDVEz94x=aoJv-UZ zFPq~00eOoW*_+pFSwXkOE zk8)@(Zu)*rsjURHxhDY?DbxMv+iFQj9{O-l(=eEUcz{>@I#=^-5OeeQ5005^uR^U@SI2Rz| zg*BK;5t<9p>mRa2oeT}zMOoS*UK__AslaIZoQUQ|u!&%_t6cYte=_znBjU`nsKa1% za>DTN=c>{TOfqj61)QcRFInJ;nUjQvvHPavaDc7|wQPOQ*L5-UjdvP=e6~bIHkJA* zk5I@#FeYp>s-QX2^$WXG(C76+RS!={OkOAVw2ELA%PH!I#y!2R&t~rvshK|U-_s_H z>cYV?X_rT1`#IIH{dc;VipGkW(_>3e{zV-KhaR9~s45mM_NkoA$QJuVXOEyk+v$<5 z>u7t#Bs$T3Kk^+l6GaqRRRo+U7Z$9%@V!ur{^G9W@dtfxtvG~rleKO%ts-r`p7I~< zeBOwHkcJ~1W;JaQ=Q&_y^}L%S1P#oZ$@{N6+{xrc$}UuZx28YNLMjy^+RH1 zMFArk+@1H~U{o<7OE^G|^5Wc_7cORtJ@vFG{7)gz<~R&vW)Z6fv48vxUs~+|70}b! z9*|v=s#1rnN;}pMft}NaDi*+A`8LW5Rm&AE!EwYzkRm8L85*~{FA~e@|9la) z_5r0Si4U^6DT-KmAx&WI4h3LD?t`y1G9p@rL|ZMq&F=PplYj6r?RWlxVV2vG>vde5 zacHscrv~y3G_{|=5X}Dt0)h{oMLDQG;PwNXn#f%Orm8=9w5qE!PLBj`poSEOgD3Kh z5UGik&HKyBEUm1DTK6frLb)RwUCUrpt|^Hi5Ij2OI}?`82pi=y9IAL(@bEk7r9ASe zjx%o4yLcldtMUX?8BzLcyqn%uXdhsL8UQ$TEx-+7vfeXqY2P^|N$vaSLnLRGph(aj z>Ds0)4nZp__F$0oaxg`(MSw1_5O7<(@ZCvI$>$rdrJ}q*9Q1N)S1Y`PcrHl9jx&*aN-JrQisoL_=g9RTJC5 zuF^3yy!&>K%=X=C9C1f4^Wu-Xx_|wnv#5;g?cOa9LJ1%3u{E@W;iR^0dxg~C{_4r9 zr`Y3!S1vtF(xMUa5DSdnKRm&(dd`xaCMI>8rK=!0e;5Q!miT)a_khL&uLlpsH-2u} zm!*7*B(9!#lT!i(bjlHzqkIJiJS>7ZxfU2t&}QSikHV_PrPcSLU_?9zjfgfPQ30p0 zux3W6b-sIy^mY|Xp=SK8kW#-RYu9z8qtP)_Szw@ZK+;7sy7a;K=Xd=MRNs1Rdo@+r zay0+M{!$jW@4&%yc*WYeJg0~I@|1wI5jP`kRlarGwq*!^AT929nB_lXrWyx5SFWx5 zD<@V~ou`F51!)Awe~7BC-_yN%{3qr;W!}z2Pn7%8fBo>W*V#V1f4_k%1F1|a)O~!W zFJ%;)P_JlT)5NizNrWgyMVw4mIGb_wn&GRZ%Bm{2R30na9ew9dkMG;EkTz>(CDU&_ zdG(6PGiK_^jZfHr{L$t6-am8gfBtCf(D6TdA^huCD*bDtI3@JR($W&TYWEH`ZduO| z5xv!}jQdJhtNDx#Hp-UKMB5;m*?yl?A?PyI$o>ioH_|`Vd35%=WfdNM3JmE6r%SnP=7GHz3X$G6^+_g_x&8MW9O2;g9VcO1gA0MVHIVX4V>`p^N zMTqUzJ}nFu8wAy?y!?iSr#Tn5VAz`apsF*EnmE)r-jFVfWQ{hf4W$^Rt&2?a^wUpD zhT#9z47^L3!CumO`4f}WN$sSI+h%1R4ZQvg1xvqbOA>_U7GHY7cIxR~Z1Vuc0gJr_ zI1Pb42B#Hm>-m|3T;|y5W!BAKiDcfaTboGHOi{qaO2BJ`BITtlTNL|zX}XLh;`Hlm z{c)WQP$_z)Pk(Sl+kH}&5K#rz9ED13w^49tD6N)NY3n*=sS~HTo(nB5u->qzo8{5P zIdI6EXm8<5e%#`-OD}Gu)%nXoCAzXc9&Z8#F1X+;=RH49A~&ctdV|MStc0S{`}%k< zyAMrw=zW4B*PfMAm z9m9)hixro8cPvPsef$;gNn6X?yVG8e-QIF?d-!rvbEOtTq^>=+j?;{J#ikp5Ig{s} z{ms4_4(*fLPM-brY1AI2GWWc_xf4V#-D--h27T4n(N~=z?x-G~!7~K!^+rLLk^0`m za=dMV?~hxqA7*2VH@P{4wsAS3+fSdraBlQ$6=I9QzuRJj+{LMH>Zzg?X@~Y965$|v zo~z`{FYo)WD?f)tS$1Uy5hYW){c)CL{EN?7Z0wI0AK?EFv!*3BNfrvZTA$sjl0pF zbv)$lPrumus#K>5Sdhqn;qO#d`ge&PiKbD}@JnDgJd~OzW^83aUvnYXD;tA&wzO}cyUD9})4A6?5i9rO&Ix0}y88|645~L{-hXb?S$m9M z-T0sJJvjZ%3NKcdZ}X(TvOm}OmPltR6a9jJuJV6(mF48iPYAlA}B|0_Xut!_zUQ-6;F7r zA2wf)k;#WLvn@Efi`Y}FDz<~>3%Tt3kj?58C5YHix**PkhtV8mgNy)@Ik>MyVv5}* zDiG_nhLj`-xOX8+j{@`4P(kF^3zYf`&}{)ySqbP-=<9y4zjS`5`?Q!1ey6et3yCyC z1Ft2$J0R<#OP6Y6VWFlJX?y_R!Le~sB7qV81eFYbtYX*jL;b1iGQ6sGF`SW&hL5;W z!=oDnbPbXNgSc7p2BL=Q7c^&YJVGO_S7=XnempNg-oE^?n$lIG-b=8)9Xw#pMFX>f zk`(UK*sTD~mZVz%2PUX9x`O5h@a!eJ2D;)j-4`FZ1D1T-BX&tQKqw%`)!;}(9f+kq zLxi^O-Mr+I##^zgYsQLPtu6e_Oga-s>)m8C~FE?+Ed{d%HmD)jSDgkdQK%u7`W*oL_){qQH-cRb- zj^eC^mQ!X)0q5GI(DZt?H?iIiQjZG2%XNZ63Govzwhe&t^i6;l47If0)OSL>&Am6h z#xFYyxyklb*W%>N=o{#W%lN%+QzTY^b1rE~#F*!u7`44UD^|E~C!n|2Is}YDk5{qi zJB3s8DpPEX64D91wt;jd;IK4J33PYk>Wm}AvcTIOD%M2TB7qwQK1*kZI~NnnMe9=v zTF1Yu>Y=$Z)iAc@{=7bY{aE6FXLSIdhnrvha z_3 z@HEM+zP*4V+N#oV&QZXu0z!uGd#g0`SZOziuz+be9wY$o?~~q6UKiv-3S7f6d}Zh$ zvLW^dyA2IJo^tCh@&w}afeovpF&zIFeFMVW$)OBnwdrtmZ<6u2RK_cj8t6|frX zrmT+Rl9di+1|Z`Vv_y9G4)lDz=rOrD?+^y^SXU&8n(awwBRtP~QLt`_x05^EWQ$HU zbjVQQ;{k4bfe-5{Ta3{@9*uG}6RWkiZY{yY95fe^i8Ld(;9U(I-tNpj8-=OUSjN-w z-|GZvsW=?v3RhuvN@vR3GExlso-53(-vJ#V_@W!KX8gh^DTv$_c(f24H@4@3ifY@UEl&$&Qtt zey(dDA5)A(ee?L~x%`yO(bUw@gn8*_Po$199>F34PNRq8?mxn>WJ{@+Vf{+pT6zs=S99RK5Muruh#qW_sw^{xvRSrRdpEUW#IEF+Qo`tP&D5ljvj z*uC?JgufA6=o8~f!a~VsC!FLIGFJY=ylyQ!W8@pgz1+O2tL?9E@o|wajWpTf;Q!#1 zkKT*R793^?SL&F9Pus__sU_$|2Txm~vZ!Ik4NnUd-zjTzmUI}@MYh$9G$c**TISmD zt>0U>qU?TT?~%ywSw&`yAN8Hs*W|O7ALmvsX4rZC;j*l`z90CP63N{uSq>>vj=6an z>|*HV+iG@`w@xf5wTdAIzoPu}Xa5UJvYA``g6vzrZCEeQt#{Io3)M5VKAIHW=(%`D z)hweeHSmi&wr@vI{js$UCPc_MI{fY*pE80JD+(;Kl$a^LVTt2q21$OaKv>m0D*R}@*=-1dhy~; zT-?;@W4CFZ(#GQz$d$k@<{8O!Q^Uc;O_m=D7)=;Ig*mGu&QKQ_?(k8X!SgRazdsq1 z45Ok*Jtg0-ddZ6S?BGT&TwOv1*rCh#%lX@54KML)pvv{(zX^;6Ol z7@K6T9L)$S&WS@wU`5yGFsNeGKArpd_EEZJNP7Hyo2n}Ub-HF-9D69e?1uY3j?pW( z{Jb*XD`mrieac^X*U*1;%x3(bZXMh(<&OLFTm;H49bf>oFLFwG-DvG?s7qj*Mj3oXRLhlX(<`=| z)|lLHMMAPr{dhMn^kM&5U*RY}5*3cQjd#GNa!ZyktG`G0?1Aw=L(EB*u8oQ+MMaKg zVv(Fd|__V!t5 zMkXvSw47T!P{{krIU08S$KP+DNQJ;)51>3U(+;zJK{qiB3OP8$i5f^3kTZxM?yTAe z%3crQ9zjf#;O5Z7@nTYHc4V#6*CDZZPTJ4BV07`-gTCI+az|!Dnz9oTfr+%lXDdoc zNzI#Q^61z^!VQyj1$jy$v->sSRW!XaLC2zlk|p?nKo9^+tODaWPlc6&zX#J|{A5|4zuVyNu~ogZZG=m z-#oQpucHqN2rRt7=y!v90We4rb3n&*`FbuudIvC$I}ZOq5Ksrq#u?Dk{mrMRr=ec& zouW#$YKrmF%zyC$2g`#Uvbou^va*Sq&Zy~oIWf~9ZpLavI?2K8%e?IFKRl}{4AV)V zV1fqTk}w%`ep%;uGAyWSi*{{UjC)j`_c!lbVMJ3N2SPcb*kuicKfQQaAa{`RZCZQwrd3r zw|g(=qx#yf1(cWQ_{a-bt@yl)Xit#2FBpYlWg6k_4MEdGn*a3;xvJC{Lg#At?hKr| zF_(GAj_I_d=g*&?uy$G~Ce{MoCV*JH-?mM(c2e8C*&W^eN*@w4pnXap9zs`q#@B2E zhRr5G+E+jDAGR@|XJ+-|y@81O1LTZYNoGXG}taJUbGAh~hl zMi9v$tx*n)w-2ha!R`%WM!H~5VFD4hjSWV%>IvS5sRH6Q27k?0$r1(&bYJr&?PpTv>IN`R6esb*^pE3u2_1mhY zl?>n=MFI}qRg?K-Ixu}Qh^ruS*29#)_txH4HplP>-ytOi{1{bb8;A#jP1-;%)OPSc zA04_()9ugALjN+Jl;USFE-IZUR{Bu9f7iKS{d2((yi+9}5Ix4!<#Kel+Oxogv=?;#k@g<;IJF-|!F-Pk2FmZDl~@?|X6UN;%Of1E6W);o-Aq&qgmiIH(p)nebX0VHY?=LGI82TT*rI+44G~RMO#$ zQVQ7Au1PQBLr=OtLeDx16Qmulwg$WBO;r@2Z^G46bm_KeU4p#`ztf+mH1ev1;rg{} z=s#Z7ZKX2E*ViCP4ZnDiw^hT?&@jOyad`47fl;%5aG;F%T(})q^yN!J1EBWw2`<4b z)I@f8ICo4W0!xII91EdLJ(vUb%7i`pg%57+{TF6q@!l#PQ3R1=1oF10N1#@dgGvOS ztbIKMs4ozGn7AD+?(L{taAz#0aK6kpIa2?`u@Kn=uQINaMY zY8z)*0z{(F%}itY0G@bUx)SsgU%Ae|;_>~KQI+F|gA+Lu;Pi;NP>cJ`%kG>LugG2Z;a-He!T~-Q0mX=$;Y@vk9!T+{__w1JTWnZ&5Ti@KA}& z%A#IbqTq&=c^0XcWU4;&l@syO&ziuo?~sc)%HtVqDxj9q8V97FlzO6C!d4%kTXyZ~ z@fq6*n5T=4lZ<2QuGTz+YiM&P0(E_roe&rlw7uOGNB`}LZvmh}4}n>enRkKGu>Q2~ zBiDQ%)_efOjs)R}$K(gzs$bePYtbt}NZ($5pfbl3>9a)vcEH@x;r|927%Uuf-T(uJ z3lXVqYS1eTwp{*)kf%T0A_=%|D0Nv~6t(hOX1lP8|bot05xzi;Zflmp2H1 zWB?^M-`@WEIK)0%p+v#yZq8m|QFoRdOzb}+D+~v+qULue$go-8EHEVsp>oALb$(Ur&l*Q*zUw#P$?a*bZVRk^JpDy%W5cJ^|(|$tR z>;uW6@8~0NcJC-eEOdoR7ze+3!Y1sy5!8e13z_(q?R|oE1OHZ#jABE^X#^6J8xhZ} zZaf!NT~mS=S~K)nI=2pDBL6QQ0}I8%B~Xc!2@M`(>e1y7{EQH4;+M5I#Rx+*8zdyc zB{$U}<#~>rR#8}5x)*wBCl$(ZMaW6^aV=W1g|(+H>aQNr(;yO8=#@oB!8eCEEbCJS|+a>-K`jPRI{1qpbfOi%U|2Y z7nuV^KKA}ESXlCk;JKq(Qf6>-AFR$ugWp5>2;0c5>9UUq3q4kMRQ&E^m_<~A?;})0 zXdAjl&t}@cLBa(D^46fU!M?8bke6x!ksZB62(&dIQbSXJ)8xY>yRW_X`$1~bRx`E9 zB4mi#cmT|EvC3YdBlv}o+c?Y9nmvuFfC<0(6}iS~kK1VF@D4=10FkspQC{94x#nP#(V+kFqhu z?l9Xb`S$1`Y5a0U`nX+tbF%Nzdh8|7j$aUDWbFUZMHqX|3wcM5O1?u#X%MCyv-A+1 z0O*wtUNVj@pXax=O?6QSLpC*vmvKiCHOF%%%uz1>WvL=&t`($VTwW?(Ga~pE28yt3 zHv7?uD?B5ASp*r$3kOq-*sm`6c zjKTA&(%*hlU>{Npd$mmm8jBLwX4_pUHsgT{nZD*>B$t4&Og7fmR(j7W6x2gRB8qo zjhHXBeE9~yIrp)l=F0>m=bY23QmF<3z|gQ7@`<7gC?YNR7QNaZt5vc!cul4j>&q&N zn?U1nBIU`yx*Nsqm(VCcBnPjpzr~2=QkiPE*;c?VUF8j43Qnac{0b>C2rVjZUt7Ys zz;^8JDUH^(8aH{RXG8{ndjvU88IPg9XBi%#z@gmfZf;O|ILEUhVlZr{X&ZTB#bniQqBlH-Q;9;*Qr|7wYnure+_ z8Cx`EsQU8}Vkf-sDo8sv)j=}XqyDTaj@`B>6hk}F2_r=`hMc`F>7q>U=-2~bg3KZx ziw@u0-a>>LWqS-{L_OMxbU$FWy#Fw2{b9~NF{7E2nAb0vll>>BB?b>yNJ~Ru`hKpy z7sdYP3gz8zqHSfzr)s7#}JnCUy(*>3bNJgI>fR%elZX{)rIj z6_89epJ=Mh^{%L>c=Z)_X#=O~U{qRCH&M|IHw>u^|KU`qMMJmg?cg(H#~V_-FU5aH zI?F%3-L`u=^3NHAouGZR!pJ^3L7CDGhr`6!5OLjUA84qL5L!@TtXIs-qM`#$fy2+H zQrJEWZDhMWn=-mRC@-r#zwkV}u2lc5rY!-X3&nKD=GUSFMh&P;T?qgVu+PklP-EO7 z%}i8h%UgI|G#{pL0Gh{!?Jrk!-!<06{Ia^_SEkLQ_pxS!`@9 zI3w#*Y9I>l5|e_}lyq<29 zEHwKdnS6`J%(w2t<1?W09k>kx%*gxr*azN+oM%w$7x9q44j2ptxRbP)X_&rF&V86Y z-Vrip%dzsOPIy>Z_-M}0sJeWa7-=#&Gyoa&nm1{L#kOnJkH|zpny^`?iZu4hIa(ba#Lxnw4U46v;hef`K&zE& zHY^tz9awDXIJAI@cP3N1$Lkr@pmhm|Ed)D*&TvHE5|S)^dm&vXcmydE<}Gt>el1Kr z3Z@W*^!VyKAb;gGgTwwcg@LlA`mZ*)Y%9OAuhS3BC z7JPL6tu`D}H8F3Z&k3D-4pFdNoja_^sL!80JJmH8tL9Xgd8)=gvjGS)?<%WVD=bQI zj@Rq`3J@I*S0$kn`hRXM`ZcGhU&<3rkX!p@bJZbSB+4y?RakjaJSAJZmrVG)?-v8y z|HA-vfutkvzf)5>MgA>XlONht;B*zNV>sKj9j~HQa)+s@sTV)3=)9fy&4P*f*suK*CbFzj&;`}3@eRj8;y*KR~Tr;6) z0m?sssE_mlu(y}#fdK~lbS6s4j944tgl@*sWRAXNc2toj@c@tRn5G^wFW~IiM7-Pb z9pH%GH>BajZE*P2dTftZGb`cmV+S1^l%#Ir2?(|H$_D}%;0U}D6gFcyDXDL`Z`UC) zDw;(I53n=94ahRBqS)iQdSt(F)b|hk{sIbwo(qbw9GtH#Pysw3Q3{9ioh{@unetKI zo&9M4?4M5C;=&gRaq-=l3DKGt8|ktbS0OT;ir+~KFfXj*#_ij?;XW{YLISW7Lh-X| zGRI*@+x8*Hju60Gan&i^trg!eoTNW?525R;M9AZe!yx&-V43%JVP3|g7p{;qzSmRWn{2DVBK7Y z>`yq#^1*sk}o;K^w1A&9v4WY+G;1ub?>M{&8NF zr;ZHiv3@A&ov&!Pz?xF~=5O8>99Ru}_|QBww~!eX@Rwe(S7`gwfUV@tBY-mL8pZ$^ zTD2Z+>S`QA+a#Xalnp-&!+<4dFW&&;6;nULISE9QOxVgzZT}0mvhR)~E$xhq$GC*52X;kb(VzI}Pr@0hB5 z(U`104mmT2%JVz@Xw?()#1=4kZn%_n{BIJ%yRPYvafoSgUjsGh!q^|Z{GG-zZ8{=Z zReO+HBeFumVpDgD_k<`O+InPxn-Xj$I!33a$~gBB(QqO|2^%SMN8#jRtuw-W$qhKk z-@|-W_nwUQ9l4I|6&BCGa&lw}K}p$h!5T(iTG)Ef-RQ(xmLBIJGB{Z_t*4USwm#nwyF!JrF>mz#6$O1 zILc)1i#u^!*ltLoO7ZXYYcD@=vdRXRz$DYKmN;FO0yZKkKio4wkkg9y$FiybQYpnS z5MLXo8~5>cjiDqxZ^Ie9ju(Z6=%4t6MY41^R;g)mWTn(U~pO%P;6%`kgHAuMpaOG$S{OmmL+qT+tlF09$c+%u?%?2+7-L3k{^=yPaj^%wM#~NSh#F>N~lmh(%s5m zl7*l|O6_F!h$QEtqM~@rsqyk+e0xXTwP$3r0Jyg*BXcaA72%b(QTCe_EfPRLoWDNSZoDa z0Mi()KX^=>zknfn{Vz`*e1@5_Z&&nX`pV<|tk}Q3m21}x>&;zO{PrDcTeiGJR)Zsg zmqrWHZ(yW6?v`+ksL7!yL6=&_#=|b1-xv79m-_sy0VGX0caBmr_m|b%_>9q0N_=aI zGtMU<8KSzAX;rbDtcQRPpFA0{R33nXD4EqzC5^6&rTLF12qwW;%L4>G`t~%wyd)YH zF~O6^?iZU_&O?dVb5!rC96}A^JlyARN)90-ak6=n_4($;?FK~zSN^&EEn#jmw% zUxideUm!vLwvk-F^Osc1RGcF7n6dMz{;pl+5G_U%?k%jzA%*%0DRMLLZ{|$MEi9Pk zi%tKeTEE@rIELDzP|ppLHYy8qk!87!Z}7aT6V`lB|7;0c`Y3so66Kg z056HXM1%zW7M;}0M_)O~QMm1w9apf6VtayFD`n}bVSFVfLi8Y;#o7@n{U_fAdo}T5 z_XDy420>muIeE1kpbFbj2q?L9;2TfAI)$zAUwmPaC>Av0)qnc8t4(DWu~7m||JX4I zo=t%bSB0SCLE_!mwjMQZ9LQr-Le6EFQ{E~C;Ho}>^=TuiUc7`(s%XVxo1xz%wi3^=xZRPtv31NH1Kli-d+e& zvU3+!WB$zi)aEttlc)on{=3g*IV0)27#b=~9dU#2VWgC}#gcdP`z^xr>T7FY};7tlFB+cW>T2hin;0+YrP;T-{N1L4-niVMe#Vh&#KWpKl6G z_|;dW&O}3xKJWuav!)%u2Tm(;`>q4`1pc)Z^zeA(d_C3W>~r>Gzyu50d{{9JW_j&axN8J%R^~R7khKo}DMWrecL948 zViju9-FWP1cyuZg;pRR8&fEU2HHO}7J0+cm!6EJwk?!IX{Z<*&H1CONqa(GZWebJb z*}KpfJ3wHkb7lmX)H6H%d2aUex3amJ!XjFW3RZ3TJsI9)It8KLl`e`hR(U7_^PGEn zP%iV1Q3>egb1eb2e06?-bM7P1<88;)vU5Ndg7$;m8sBUxfc-|-rVh+e~OvQgv()Fg+xh{4hx@~SQ$x#uQQ1dqsYHN_JnyZsj5 zer@?mBH?m?A#FdoXC;R;$stTNZt=)y|9)|lYY+uNiinC0kMOwIpO283mD<#P%mH?w zph3Y@u5rfVDR}*X7z<0FL4}19lNSjLVVeu)RlS447H#BKC8Mv$E1}GpMPLRcvJ&nu za})7%K9=?fK4b123pWK<0rndmsn?`xAtY{Sm>no3X1_x|u)*UBgrIl`pY2Rmxg0<9 z3X8@KI59nw^T&Fk?|=N8Xs!&Q>L8}O*o&QF{+H+Ea=G)GjJq3d5vW86w36n}x32l~ zjX#I)e*g&f#UJr5-8d`rUMyGN?prptPW#+UI}Vn0?l8&U?R2w+6{D9tMRDb~&3_i`M0FPqx+ z0+|?tvm@Xf?xvu%1Jo}!uKS9Ut}nET2~`d|Djd=8e|EVmW`>?@EhK2Ie>b$@nZ*W4YNi8RY(|nx(h)uM+yMvV z2*ee8^8W3sqi)d*a6IixoZ#A=yr-BoNP_@KxMIdnp+?3foiu`7?|3 zbB=Fl!vikc&(9Br7Y>9o3FQhxtc_3&-NJS>7)D30C%^p0v%h93!oFo$Wn?G$8!TywCfKk zpxWi8E`L1SVE&hr0?G2FK{OjpDr(7r-L6nTlRdAMq(?GFy&bL!#TSu2zFX;Tuy=30#lQa(eUUHXWy(P2 zqOl+7*PYVNZ7Jr8ijQEl7#0esl^rM=dkgk3zpUc~(VyUch;zdVwWk(l-A4%X_m zer&ky!9OhaL%fbv@%Gd800oa#L^bv=`wce`i6<+}M3gv8T_5dNYXr&zV~K4J>K9*~ z($34mE4!cna_herhj?(OTMHW|eQS6RVteRDNzknbk^Z|?nyvp@-*b!5+fN%Ei7NA_ zB``r=0w2Ri8Q!Tb&zDqU`Slv*c9Nru^D$S&aXLs&L#Td@3qhL1elIuPZ$evIai++l z`?uZMP2OVv`}Y5bZ~m7kgcR~w8no~q?{K_2ChTezL%FGTrrzV{3gGXa#|fQZ0ecG4 z$*F@c8aC8G0Z)>(3ad^vwtk3hq5;m^QG2p_8kdGh@|qKF;uQQ|wu z@Je;CNQ3P=ph6r#N(YM3-2w<5vaw36tbZ0W1g0Y)+;Q_C&1jcPq#0%~@gb!Z@eG<*IMUwt}g zhWVO3TmSl)UOtqA{6$!k!}aeM(dl<9J3AYF3h*4)pZ`Vszx_qizk*akwFp08n$h{l!Enp9VKdaH z#KC5IoTdV9J9^|h#8LjYzi>~s;oZ4xdYI!j5p#icMWIn$J#EpF-&R&9 zN2?|GQ$;r}vCXwe-nwEx!)E?fAD23YDy5_y8M6vSO_(o1q`c)E3?Hz)qe z$cHUwZ=c`5|2MycpOX44-l{XKExr)-iB$WozqMQ9r$0jce67|0_dm~gDn}_FaFk-0 zp-O)(+kQp+J&Q@gTfLb!5pHgz^Sjo~4cw_+-n^^oLI(x4c=Xi{3R2tAUEQm4h#!+Q zS?oXjKmB8NtJ=2sO)I*3nk+=`z7b%MHKzOI@Bbx;vizZ*M|FzN5*AX2I->#&E#ZAi z`qF2eFStzlq{kSDK8T=SnI#!)0~=S)uhY)8?(G-%>6sa5gbIKyp(<~SlaP=oM3xxO zX>AU@a>Aj`m#3o+I$Bhfk1Ow!Dc`nn|1r-4d3!0)^s{NxCSc^BZIie{d4+{-Ak##O zTjDlTC^RWK;hQe^1qR`Ce_ck{pO^9A0mKGtBr;rJM-Q}ftBB%2gDq91oRN_c+``eL zi>k=US;ET7s-M2}LPA z_(hhdt*qwYx{C@2tz09|o~c>CYgMy9PbDT)PD$!kUdwCa4R>}dfj$03cKiEwZO9ed zzWvI&ZN0EXuZw>Ccy~_ny?gr&@1nx5s8DcspX=-E1Gr~*?pz^bYIb(*sXSQX`Bp;Z zS1slfsTm`xUi{WH$D%n`dDbvxi`4k1qR+S~Osr+%-lm?HE`i`IAN9w)ZvggZ(`#yK z#A2SK?QfvRdd6l^l9l}#=O=u`e1li7baPg*vTljU6G<@7(M12(q4JACK|#RPpkRKo zLGhzNCJz_a?gPO*bwHxh+JiiXLeptdBzGKR2!GKhnj+3T@N`Sr@|7#yA(d-bnFpv7 zM!en{n+yNw>zGmAHM{`5F6vRjin017+7&|3Kfye?@wqLP!x?&dDU9s%4Bq7BP4pL& z94ZQoA}_H#+cgCvXkretl4c{=TF~nK{SeM}b#*AB09qKUq)(u=UR`wFdMLF?VpQa4Z> z0i)1$!i}&_how=RD)XsIWO^qsLfE>~Ed$a?|I~2(%ettpdO;#g1ObqHyAXG@imo=~^0 zwO6EH3prOj+S)-rH61Af6kdd9k))pBVXn4|ubvqh=kc1j!LhWBZ$*7uEybeL{_Q?t z$>rwehJUD*025{cfyp0Mzj5QnO}F=hgM&RioGP0bV|`aMF$v6_FwGHoy(2=ME3_+x zCrq$%v`&Enjg_2?Zs=)6v+thL6p#4$_=Z!@pFhuQ84MT=NL_4Lee+nMM;#}B&KnB? zq8PD#nV>lQgi%F*!0=-zwCFhQ4NB{0nSak3|*M5*@rz`_R7lM2fz8sFHyK;BL_!fem(Oa zmaR_czwjZh_F9EUIe`rny>s{3E*(*y*8~dV-o1M(R;;*o?OH}g zhQ_fYg_KqL^80k>H!cl^G7zv7xVKZ{Vb!)?ZOI!a^3d2WvT0K%`b-*a2!n?uOP2hH z=R?G?4Pu{w<(aBHj$p5k4IHm_`xY#sz2ACB~a;4Xl=n*CEwiL0)E>~OPs_H`Z2Fw&7n67N-MUy zcjO4;@aK}+BUR;9%KPT(nRO*@w#0HV4?uMM?%g|_27^Od8ZyV(BBmlP9?0qe+6e*O zG-iA^Wa)CW0RxAi?cZ*NUD6sR0pgjb zE^Hj->v<{Mx~o5=)ewjp^}BOLCCSNLqN1-;ErNrCQ3%#C zxPqK?S}DF(^vQsGp~v?)BZ21#x&w%G!%96^=rb^kf?4+FU}t%B=ke;qFy-zK?vwr8 z*7p%}3O~Jm`SN9LE&L&35m2I{-e3mxzYECIyPP3K-{I$A+X}0SsJJ+m)vJ#RM^(t- zKjy(`$4%dt78n@lU^|)BNi;WcaZOH4s9)WB7^_hui^1q-LIQfcI8ENXdSwf9RmA6g zs+EN)=IE$Q>#n42h0o12ye7eEnVOn1nZ#bVxrO7ss!GYk#3bh{#9ohf8tMc^ z^wY?>y3PP~EOi<^WtJu{D|?>j3FS&D*c#Z*OxmSn(8U zMd;J(V>hHNYK;vI`DSWFE_0F!G#6ex!J~&b9WCPHw6S|@#ozDZ=Fr81n1~+W=a-h1 z#ckjvH2=!%&Bn#!kiR1I04(+m40MOx=q-9V8u_>X@aD&B1v5o>cut}vg8i<=!_n~1 zYWAdwY6oI~3mS~pu3fufLri>pdqacbwN*9`M~yGD?Pglgt-Q)ifq94+mq(Ii{mjuf zGDfwTN{(jXO4%yygCZ0p3EptEOXqvhwuuw>lUCT!Hd!JWW!qzSfr})|ofML~s@8@0 zA?ex*#dR{2o=j5W4wHKy94?lKd460pCTT&HYUexV2p2`WmkUXKiMqlJ3!;gpQc?%6 zy!N$#cnUZ7)lN_J<`UO zwFreQ_o5%y=wESzr|ZP|APAiY~X4d4{g8u8)vWZRm-qc$%Uf!)0ulV}Ncl4)=`u zMm85P0SJM}BTY)qQ?$E-cK&!4adIZdjU~ogFB>qN1Lzu7Ln!BkMXgli)Tp z0X~ZBp&|5Do=)kj*8A}hZcrru0zBgP?~dJl^X&N9z`5(ov0OC+k>Re-iALkYh;pOf4{+HDcXAvAXP%h3nr znXBY%d6sEPk@wnVr|k5na^IY%-@bm0ab9AM!`=7q-|y(?z{?TXN5_gOZR$D56;Smc zZ-M%uL{wGVL_$JC(R;wd2*z9FbhF4rG8~Mx%f60{jSUaeEuvqh!MS?k3z*@$y+5Zv znJ-2$#pUPpr*gX^qVoQX*Kgk*C~Ey=(B9T2D05N8F;@+V!}n!~(TGrRYjGe@O8&Ha52Td%@Xh<%4k~Frk5iKwoZ7_pb#IL-`y!DlEaxK&1_HlYsWr!-bTx zxEVCMrX2J%=Ps>Epqy$`pT8TsUte2zkkQa?I{la}(vumGf#}RQuzacp11(Rth;E*9 zO&jSqbhnIlaCTOmvnW5k;M!%Ttv2Ks0Ivem!&qZzMGCT7N=?1vY_Gr5620U#I7mj$ z6+Q%&{<-jjU=Cy&Tf9YDj$A9q{QTaQDoOQ|e42O$?GhO=v3w+vC=K%R8u~w~*>63! z(r#vZ1h%QqB}kqG46ptgh;dh7hso5m5Ketv8b5Ti$j1I-R=vCXqEzNmZ3!P;j4PMqpqKGK6j;#b{rD|Mc6p z>8OHJ1O|~^eNU9X-Drq?y>QVYPL3te2W}rdN|Z;|6*v1#>S*Ea&11~T zGI*|N$0OL?KqQ`_dpxB~0#-;d!NF|thKV}j5T`)3GI;#^8awb$Ypk%no*7+1-7;)! zDK_=Hdg}8m->B19Cc{q&uNRMt2n)OVycbmo60ISgh@_Pdb6rduTkA7cEw`$oVCQp) zLmL&8!VqwXXKdl;U$$fkLaZtr!Vyy+)uV#i5w69-@ApUtlVxyxAma*4uc&XaEIN(; z%3bo$dZUH%Bp|Ai$fvW>0eh5 zHwxn!4;y>Ue7UW~Y*hZYI#3zVax(;G#^7P2!NcR?MK!Ka1DKMX-Fz5&u7*>iLqoyt z3|AgPAb{(fnV!bPqin~o$5R@8GvMfpVG$u&7N<@XLfC=4C@o3jZKgh7;N$N8v5=|S zQyS(l)ej8~Rn#*Oh=BEm__c$4cWlhuy#`O|nVvUyhexs0o#X`q| z?)4*Seaq@p6~^z)GWwHCxrA}{T_dX}oRZ`hQBg_G==$oq4e39>)$Zu*?Cj|1Giti) zX`TyaC1TUj&*&fN@%CG>eEG-!xD=aPskBwXXi!0nLq6Bj-Oa(t2{H>Bt6P;ZG@0?4 z_e#t6>LIF@Bk}_0A`bzB{!uv*)O|aedGWS_`DTwbhyvS{AN86aT3;e?Gua zv*c^U?_`GQor#I2AHB`)Q(KZE5goRRiAhGlbzoyVlyMma1qS_ESb8)oSGHqrZ=jdW zq?|>Lz-cI3HuLcGea&;hWkI86KPk7Sk-r2j9x${?%PMU~O>$mJ*_5fELWzs%<5D&S zs-_g~@KKEuBA{APbV5r6`o%hC~WJbuZv~_8D zttXT0mx7Wn@JdFqtG>d=Yzh+ z4(K>3BS*EdxdS-FA<_i(RlOxd8cJ$vtBueMNo==9`5Ba`IL-pxOLfI{F?8eT(ZsB* ztR_c@Dot6<_JSux%Pt10JUL3YtW$GQl5o*XmBOv8EaF7OUDLQgVQ$x}+LXSDtVl{Yf3d-)zt}aYa+a66i?`4k(i@SwyPPlK)Y;Bl_M$ZACsO zh_!paYW;luu}W%V_btKNUyG>gcjwC;%5qA%fPn?XrK@o6WxGB>yp$xd;pdkgSz}XS z%uBm0h}-MVa#zV$hx9L|9D9I}a3g)y!^)M)S1Dp>TmW>hCqO_;>6Y>?TPHYzhW5jc{I-Osc~N`QhItQwd z*Fn6yvx(saB7@5#TdHA(hNPMC1op<)uiGhD?E^_`WuIbbWolog%mFI&Ej zz3cVk2bYlaX32Vh!i=~8nAbHzG7f_T>22Hgt~RUUd&@q#B;P`|bilrJ!9+D>+SC5= zdDNbM8K@BhbXF@VRsBds2e)u%OheF#qZO42A!Nd+Ta2S`GY+nw<_M?!)SSmJB&2QO z8xUX&DR`iXxJJc-?kBY2i(x|tZ_{0yduX2`+#SFB=IK+_I7*_3)i_EnV!J1Amn(%l ze&IRnLj!%Nd(tRitpqM&tY6M`UA8zT8Pt(c+sz|JMA+DFph^%F)G%Zhbirw(a!EUN zlXWcWGJr4ImvMXb-4s<~bX7h%ooahSMpWdV$)`wORZ9l2$$Q#PGQ_^bF2VNeqrt0X zzmOX$MfckLv4-BJr^jzM-v^%?ozqn{Nj-7ib<*q;LgASjQ?8uAU}a0|Lb7B_ zhwl&A3_5kL&=#nuti;gwvZSOQxSrvG;jQ#|LkzZ|(n6|(SdK1Q8ECKxLy$oRaXW(~ zN;Kwag{ez54~_w=Lsuf?=+WMbszN1HJdhm1q2{#?OHOQDoEv;w%;2;8omqI~qd@cQ z#fx)J4I4R?ZyuLR@}aT8oNYHXwPz@t5dw|!+4lurVa=&|F4iuUztr-dSOR%X_VI+LTHmjre(&Bu3=-p5tKZsO_L zM~CJFchD|dE2)Xdu$-E@z+osM;wkM@BpBgVC1}jW4Q3D5U!fBsV$AhQ_~ov;=4QA? zHlu2VE7IGXyB}<+YW(AYQ@BF0YPxHB`)rELjIzL6_qiE}Tuk4ghKz9k=3#y~?6%TJ z-kH-|z%=$dcj=0zhP@qq^?mPMzUrl6ze@fa{wnyq($UvOs0MJvhKi;JcDKe=jrRK0 zUzI^}6{}6KAtBMxx82lIjwR{bD={c)wG@sDJ0d;QCE5etFf!0UTHK8ol}-MG`fw{IF);Y-Xq%C!L?|n#hJVAJ+k(LX2sM#0nimKa% zfnx2D49CXCq8SuARrDvAt$R`=om&5VO{0+enKRkb$vy^*=3xQ-=k}-Ss?;+tprq6r zxV`e5f^M{I|A3R7iLyN#RZx~w%>1U(k#I9KjPASq7k|Jfv*M<;dSHx5+@_5iJ1`?n zLE!v_fkgTE#m=Kg7HSyD%gMcx$z)07aP-IlqSV;f_$`AE=f;{L8&+1Y00EcWz7Bde{Q9q$a?sr&&v(66`E?EBp7`!;oxhQlLV77d3} z?Tf4%LaBkIi1{guRf0bE66qpcO6=~Q#HDVA09AqEMlo)u{cxlb^dUrt=F99tYciYQd|FPpRtu2zGtmv*0n8L zy!83P&DSX`fiW?RTl6aHk^N00F{yiBq_+xa5KPh7|<3UCh9)8Kw6FoLbIXOe1PCNUt?Y#}oOQqfvX=-ks zgbU9Q92N;9&^>!9(N{t=@q)S2R24>CDj58u1^p)cj}=g*Nky*@MOEn4BVeq*G z;0$Kh+Dh7o{2px*%&t`D<2+GFK3NYnIriClq1VHr<&Qy>|A?G;uZQq|g+W(c*Pr(c zbejO9s-og0+b-G3{3TadS^2_Mn)e(~BB$8I$P+Y50~S6~Qjog`&>#q^PCXzK@luX= zs-LqbId2;H<{y?BfQK@1>f}zrMH8ngh?QbaE+J8RDy#vGbj>9n6&2#|mG#?Iy6gq+ zS=vnTi#M0`6`_4ZT3VWir|PJ%&!U%?eCF>a4^pFI^x=hasnb=K^(V(*L>cC^v21=) zYdr>Lf9L^tU<(KqPKG41Z98mk{(^RuT&K`h9!LN(q@zI%QS~AJNs~2p*!^4wpn6Y!`&V+GUqJJyib!>nCiH~}_hc)swoJPyS>#k4^p{-zdlC+-X^@7deBZqf`9C>8=BV`vX zxt)_Egl;V??V1{t14D4h)4KQhrmbdTrSopfte@_k??A#5Qmt}oH*Ftbq=cqF&@$a7 zEL``{t^+{A63oHGLWS0?i(6#%B%hO`*V=)XndO)JzNg@~1f#(@gsHHw&AS%}3kwrb z!K??ih=hAQx_XpkQj}h;u*w!{CFc~^8-ITNa+8B%1@iQyFR>b@u8Q=rMP=W;dlsT) zpcOSzU!M`~a>&lmV1rgln*zS03Xf{E7%$InNmAFl7dw9AL8uy`)`#hg8#t@d;c9-s zl)BXOXvlri@_A~6TxYB~xqxJ=!{h`CB)ZXIv82h%g$F*L(OYww*_t|tdsiZ@-8?WzaxD(MUf;qxBSmOdpdiQG zWlBm)4-XG`nCnBR-ttw&!_nRSXuFsZiL_K-Sz(v%-29}sJAe@KIeh{GGH>6uab7wY zdGGl1_1cSvZs{CKIYt%wWc9jryn^n~JiwTd_0@6HLP{x-Cm~svw|}+W)=Htf=C?{@ zH?!}0t6$OE;*5+zq(7jfV=xZSs)k>hfhsj4LtRzXD92RMD(g{8i&205&cXa8fOd$9 zcR@kdZ5HM~1(RQfbq!@ac(b^uC|<#%RnbqBh{H%k!+zt}pe%BgbZs@LCb_TVC%^TO zymr@>_&onib*_4ZtFS-j6W^EZ?J^P)PB7EL(`prLzt!)QFwegzAH#Trc@a#&cChTI z8yg$-r7YhTXg5FapJLIQHEKSc>WMZvuI=0Ph+NX|Ski5+IT;JM)|kJo*_)65JEoO3GY z?)@FPQzU)oL!}q+*Kkqv144dLJ(T+o86s~3X2%}X)#YGBJ;fc7$MpO(13_y-eo~a8 z$<))>Ff+yE_dB1Xaw1qpFDr;B0>i;|<4L)VyxfB(F^<;|Xp})pv}TJjSe7|-_(_9B z2&T6oO{l1r>eNV4hP%9SeAP>_4*5h9e_nU!x zQ&aPAVSie7b>;^Qn)@5#NLTqy$ku#zTq{)<=LT|%1d{0lR9^s?)|fjdc-3_3Wv{@aX9{I%ER%K?+u^7He-!tJH61_oV?d11hN+!?br5(?t)DEyl$>(hN^cuha>(3i5|nAHPdj)YVy{b_sD0X>94I zLDk$U?b^vxCio6P0Dhz^mn}Q1dx_^p-TrRP2L8mfw5Ow&5}Xt{rAA-vO=y5xVcW;= z`Q58+HL{$nqy020`YBv)KZH1$h@^z>y<;-`p{CZF z6gfEa^qrpQ=vaxAzIlzd}*R7QLA`5;6_BG5#8#ZhJb@%YW1Jr0ZNo2-5D8n_G z=F_%Eah*GGhJ}8N5 z6E`CeP~3U2)s%AN`ey$Aj7YRn!0&li>4GYK;yc9G{Uey^`90m6{|>657h78?Hg*%( zG&%bxzF)fz+^8oLaUFmD8c70s?qA;I>L=3t1N!S)`j#UHCgd*E;(xvL+Do7ghrELY zWPz~}gHFg_hq^oDY5)8W{=L2p`kJ4 + 7 + + UMLClass + + 287 + 273 + 567 + 133 + + PlanningContextLoader +-- +... +-- ++getAlgorithm(): std::string ++setModel(model: moveit::core::RobotModelConstPtr&): bool ++setLimits(joint_limits: pilz_industrial_motion_planner::JointLimitsContainer&): bool ++loadContext(planning_context: planning_interface::PlanningContextPtr&, name: std::string&, group: std::string&): bool +#loadContext<T>(planning_context: planning_interface::PlanningContextPtr&, name: std::string&, group: std::string&): bool +-- + + + + UMLGeneric + + 0 + 525 + 259 + 70 + + symbol=component +PlanningContextLoaderPTP +-- +-- ++loadContext(planning_context, name, group) +-- + + + + UMLGeneric + + 448 + 525 + 259 + 70 + + symbol=component +PlanningContextLoaderLIN +-- +-- ++loadContext(planning_context, name, group) +-- + + + + Relation + + 119 + 350 + 182 + 189 + + lt=<<- + 240.0;10.0;10.0;10.0;10.0;250.0 + + + Relation + + 567 + 399 + 21 + 140 + + lt=<<- + 10.0;10.0;10.0;180.0 + + + UMLGeneric + + 35 + 756 + 133 + 35 + + PlanningContextPTP +-- +-- + + + + UMLGeneric + + 511 + 749 + 126 + 35 + + PlanningContextLIN +-- +-- + + + + UMLClass + + 1260 + 616 + 119 + 126 + + template=T +PlanningContextBase +-- +-- + + + + Text + + 21 + 609 + 175 + 14 + + <<bind>><T->TrajectoryGeneratorPTP> + + + + Relation + + 133 + 644 + 1141 + 126 + + lt=<<- + 1610.0;10.0;10.0;10.0;10.0;160.0 + + + Relation + + 602 + 672 + 672 + 91 + + lt=<<- + 940.0;10.0;10.0;10.0;10.0;110.0 + + + Text + + 490 + 609 + 175 + 14 + + <<bind>><T->TrajectoryGeneratorLIN> + + + + UMLGeneric + + 511 + 1001 + 126 + 35 + + TrajectoryGeneratorLIN +-- +-- + + + + Relation + + 98 + 588 + 21 + 182 + + lt=<.. + 10.0;240.0;10.0;10.0 + + + UMLGeneric + + 35 + 1001 + 126 + 35 + + TrajectoryGeneratorPTP +-- +-- + + + + UMLGeneric + + 1176 + 861 + 308 + 98 + + TrajectoryGenerator +-- +-- +TrajectoryGenerator(RobotModelConstPtr, LimitsContainer) +generate(request, response, sampling_time) + + + + + Relation + + 567 + 588 + 21 + 175 + + lt=<.. + 10.0;230.0;10.0;10.0 + + + Relation + + 567 + 777 + 21 + 238 + + lt=<<<<- +m1=1 + 10.0;10.0;10.0;320.0 + + + Relation + + 98 + 784 + 21 + 231 + + lt=<<<<- +m1=1 + 10.0;10.0;10.0;310.0 + + + Relation + + 602 + 903 + 588 + 112 + + lt=<<- + 820.0;10.0;10.0;10.0;10.0;140.0 + + + Relation + + 126 + 875 + 1064 + 140 + + lt=<<- + 1500.0;10.0;10.0;10.0;10.0;180.0 + + + UMLNote + + 0 + 441 + 98 + 49 + + Plugin + + + + + Relation + + 42 + 483 + 21 + 56 + + lt=.. + 10.0;10.0;10.0;60.0 + + + UMLClass + + 1589 + 525 + 147 + 112 + + JointLimitsContainer +-- +-- +-- + + + + + UMLClass + + 1792 + 525 + 147 + 112 + + CartesianLimit +-- ++max_trans_vel_: double ++max_trans_acc_: double ++max_trans_dec_: double ++max_rot_vel_: double ++max_rot_acc_: double ++max_rot_dec_: double +-- +-- + + + + + UMLClass + + 1673 + 399 + 196 + 91 + + LimitsContainer +-- +-- ++hasCartesianLimit(): bool ++getCartesianLimit(): pilz_industrial_motion_planner::CartesianLimit ++hasJointLimits(): bool ++getJointLimits(): pilz_industrial_motion_planner::JointLimitsContainer +-- + + + + + Relation + + 1764 + 483 + 126 + 56 + + lt=<<<<- +m1=1 + 10.0;10.0;160.0;60.0 + + + Relation + + 1659 + 483 + 70 + 56 + + lt=<<<<- +m1=1 + 80.0;10.0;10.0;60.0 + + + UMLClass + + 168 + 1232 + 126 + 42 + + /KDL::VelocityProfile/ +-- +-- +-- + + + + + UMLClass + + 28 + 1302 + 147 + 63 + + VelocityProfileATrap +-- ++ SetProfile ++ SetProfileDuration ++ SetProfileAllDurations +-- +-- + + + + + + Relation + + 126 + 1246 + 56 + 70 + + lt=<<- + 60.0;10.0;10.0;10.0;10.0;80.0 + + + Relation + + 91 + 1029 + 21 + 287 + + lt=<<<<- +m1=n + 10.0;10.0;10.0;390.0 + + + UMLClass + + 525 + 1316 + 91 + 42 + + Package::KDL +Path_Line +-- +-- + + + + + UMLClass + + 833 + 1316 + 84 + 42 + + Package::KDL +Path_Circle +-- +-- + + + + + UMLClass + + 980 + 1316 + 105 + 42 + + Package::KDL +Path_Circle_Wrapper +-- +-- + + + + + UMLNote + + 1393 + 1176 + 98 + 49 + + Class to manage the Cartesian path +bg=white + + + + Relation + + 1330 + 1190 + 77 + 21 + + lt=.. + 10.0;10.0;90.0;10.0 + + + Relation + + 560 + 1029 + 21 + 301 + + lt=<<<<- + 10.0;10.0;10.0;410.0 + + + UMLGeneric + + 882 + 1001 + 168 + 35 + + TrajectoryGeneratorCIRC +-- +-- + + + + Relation + + 1022 + 1029 + 21 + 301 + + lt=<<<<- + 10.0;10.0;10.0;410.0 + + + Relation + + 581 + 1197 + 693 + 133 + + lt=<<- + 970.0;10.0;10.0;10.0;10.0;170.0 + + + Relation + + 889 + 1232 + 385 + 98 + + lt=<<- + 530.0;10.0;10.0;10.0;10.0;120.0 + + + Relation + + 1050 + 1267 + 224 + 63 + + lt=<<- + 300.0;10.0;10.0;10.0;10.0;70.0 + + + UMLGeneric + + 896 + 742 + 126 + 35 + + PlanningContextCIRC +-- +-- + + + + Relation + + 952 + 770 + 21 + 245 + + lt=<<<<- +m1=1 + 10.0;10.0;10.0;330.0 + + + Relation + + 987 + 700 + 287 + 56 + + lt=<<- + 390.0;10.0;10.0;10.0;10.0;60.0 + + + UMLGeneric + + 826 + 525 + 259 + 70 + + symbol=component +PlanningContextLoaderCIRC +-- +-- ++loadContext(planning_context, name, group) +-- + + + + Relation + + 952 + 588 + 21 + 168 + + lt=<.. + 10.0;220.0;10.0;10.0 + + + Text + + 875 + 609 + 175 + 14 + + <<bind>><T->TrajectoryGeneratorLIN> + + + + Relation + + 987 + 931 + 203 + 84 + + lt=<<- + 270.0;10.0;10.0;10.0;10.0;100.0 + + + Relation + + 847 + 350 + 119 + 189 + + lt=<<- + 10.0;10.0;150.0;10.0;150.0;250.0 + + + Relation + + 91 + 469 + 434 + 70 + + lt=.. + 10.0;10.0;600.0;10.0;600.0;80.0 + + + Relation + + 91 + 448 + 812 + 91 + + lt=.. + 10.0;10.0;1140.0;10.0;1140.0;110.0 + + + UMLClass + + 287 + 126 + 567 + 63 + + CommandPlanner +-- +-- ++getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, +const planning_interface::MotionPlanRequest& req, +moveit_msgs::MoveItErrorCodes& error_code):planning_interface::PlanningContextPtr +-- + + + + + Relation + + 567 + 182 + 21 + 105 + + lt=<<<<- +m1=n + 10.0;10.0;10.0;130.0 + + + UMLClass + + 476 + 0 + 203 + 49 + + PlanningInterface::PlannerManager +-- +-- + +-- + + + + Relation + + 567 + 42 + 21 + 98 + + lt=<<- + 10.0;10.0;10.0;120.0 + + + UMLClass + + 1211 + 518 + 203 + 49 + + PlanningInterface::PlanningContext +-- +-- + +-- + + + + Relation + + 1309 + 560 + 21 + 77 + + lt=<<- + 10.0;10.0;10.0;90.0 + + + Relation + + 910 + 1330 + 84 + 21 + + lt=<<<<- + 100.0;10.0;10.0;10.0 + + + UMLClass + + 308 + 1302 + 147 + 63 + + Package::KDL +VelocityProfile_Trap +-- ++ SetProfile ++ SetProfileDuration +-- +-- + + + + + + Relation + + 420 + 1008 + 476 + 308 + + lt=<<<<- + 660.0;10.0;410.0;10.0;410.0;140.0;10.0;140.0;10.0;420.0 + + + UMLClass + + 770 + 1141 + 126 + 42 + + Package::KDL +Trajectory_Segment +-- +-- + + + + + Relation + + 889 + 1029 + 63 + 147 + + lt=<<<<- + 70.0;10.0;70.0;190.0;10.0;190.0 + + + Relation + + 287 + 1246 + 63 + 70 + + lt=<<- + 10.0;10.0;70.0;10.0;70.0;80.0 + + + UMLClass + + 784 + 1064 + 98 + 35 + + /KDL::Trajectory/ +-- +-- +-- + + + + + Relation + + 826 + 1092 + 21 + 63 + + lt=<<- + 10.0;10.0;10.0;70.0 + + + UMLClass + + 1260 + 1183 + 77 + 112 + + /KDL::Path/ +-- +-- + + + + + Relation + + 588 + 1029 + 196 + 147 + + lt=<<<<- + 10.0;10.0;10.0;190.0;260.0;190.0 + + + Relation + + 357 + 1008 + 168 + 308 + + lt=<<<<- + 220.0;10.0;10.0;10.0;10.0;420.0 + + diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_1.eps b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_1.eps new file mode 100644 index 0000000000..91d0feecd9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_1.eps @@ -0,0 +1,1648 @@ +%!PS-Adobe-3.0 EPSF-3.0 +%%Creator: (MATLAB, The Mathworks, Inc. Version 9.2.0.556344 \(R2017a\). Operating System: Windows 7) +%%Title: I:/Entw-Technologien/Studenten/temp_kai/blend_case_1.eps +%%CreationDate: 2017-10-06T19:06:40 +%%Pages: (atend) +%%BoundingBox: 0 0 774 521 +%%LanguageLevel: 3 +%%EndComments +%%BeginProlog +%%BeginResource: procset (Apache XML Graphics Std ProcSet) 1.2 0 +%%Version: 1.2 0 +%%Copyright: (Copyright 2001-2003,2010 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/bd{bind def}bind def +/ld{load def}bd +/GR/grestore ld +/M/moveto ld +/LJ/setlinejoin ld +/C/curveto ld +/f/fill ld +/LW/setlinewidth ld +/GC/setgray ld +/t/show ld +/N/newpath ld +/CT/concat ld +/cp/closepath ld +/S/stroke ld +/L/lineto ld +/CC/setcmykcolor ld +/A/ashow ld +/GS/gsave ld +/RC/setrgbcolor ld +/RM/rmoveto ld +/ML/setmiterlimit ld +/re {4 2 roll M +1 index 0 rlineto +0 exch rlineto +neg 0 rlineto +cp } bd +/_ctm matrix def +/_tm matrix def +/BT { _ctm currentmatrix pop matrix _tm copy pop 0 0 moveto } bd +/ET { _ctm setmatrix } bd +/iTm { _ctm setmatrix _tm concat } bd +/Tm { _tm astore pop iTm 0 0 moveto } bd +/ux 0.0 def +/uy 0.0 def +/F { + /Tp exch def + /Tf exch def + Tf findfont Tp scalefont setfont + /cf Tf def /cs Tp def +} bd +/ULS {currentpoint /uy exch def /ux exch def} bd +/ULE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add moveto Tcx uy To add lineto + Tt setlinewidth stroke + grestore +} bd +/OLE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs add moveto Tcx uy To add cs add lineto + Tt setlinewidth stroke + grestore +} bd +/SOE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs 10 mul 26 idiv add moveto Tcx uy To add cs 10 mul 26 idiv add lineto + Tt setlinewidth stroke + grestore +} bd +/QT { +/Y22 exch store +/X22 exch store +/Y21 exch store +/X21 exch store +currentpoint +/Y21 load 2 mul add 3 div exch +/X21 load 2 mul add 3 div exch +/X21 load 2 mul /X22 load add 3 div +/Y21 load 2 mul /Y22 load add 3 div +/X22 load /Y22 load curveto +} bd +/SSPD { +dup length /d exch dict def +{ +/v exch def +/k exch def +currentpagedevice k known { +/cpdv currentpagedevice k get def +v cpdv ne { +/upd false def +/nullv v type /nulltype eq def +/nullcpdv cpdv type /nulltype eq def +nullv nullcpdv or +{ +/upd true def +} { +/sametype v type cpdv type eq def +sametype { +v type /arraytype eq { +/vlen v length def +/cpdvlen cpdv length def +vlen cpdvlen eq { +0 1 vlen 1 sub { +/i exch def +/obj v i get def +/cpdobj cpdv i get def +obj cpdobj ne { +/upd true def +exit +} if +} for +} { +/upd true def +} ifelse +} { +v type /dicttype eq { +v { +/dv exch def +/dk exch def +/cpddv cpdv dk get def +dv cpddv ne { +/upd true def +exit +} if +} forall +} { +/upd true def +} ifelse +} ifelse +} if +} ifelse +upd true eq { +d k v put +} if +} if +} if +} forall +d length 0 gt { +d setpagedevice +} if +} bd +/RE { % /NewFontName [NewEncodingArray] /FontName RE - + findfont dup length dict begin + { + 1 index /FID ne + {def} {pop pop} ifelse + } forall + /Encoding exch def + /FontName 1 index def + currentdict definefont pop + end +} bind def +%%EndResource +%%BeginResource: procset (Apache XML Graphics EPS ProcSet) 1.0 0 +%%Version: 1.0 0 +%%Copyright: (Copyright 2002-2003 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/BeginEPSF { %def +/b4_Inc_state save def % Save state for cleanup +/dict_count countdictstack def % Count objects on dict stack +/op_count count 1 sub def % Count objects on operand stack +userdict begin % Push userdict on dict stack +/showpage { } def % Redefine showpage, { } = null proc +0 setgray 0 setlinecap % Prepare graphics state +1 setlinewidth 0 setlinejoin +10 setmiterlimit [ ] 0 setdash newpath +/languagelevel where % If level not equal to 1 then +{pop languagelevel % set strokeadjust and +1 ne % overprint to their defaults. +{false setstrokeadjust false setoverprint +} if +} if +} bd +/EndEPSF { %def +count op_count sub {pop} repeat % Clean up stacks +countdictstack dict_count sub {end} repeat +b4_Inc_state restore +} bd +%%EndResource +%%EndProlog +%%Page: 1 1 +%%PageBoundingBox: 0 0 774 521 +%%BeginPageSetup +[1 0 0 -1 0 521] CT +%%EndPageSetup +GS +[0.6 0 0 0.60069 0 0.20001] CT +1 GC +N +0 0 1290 867 re +f +GR +GS +[0.6 0 0 0.60069 0 0.20001] CT +N +0 0 M +0 867 L +1290 867 L +1290 0 L +cp +clip +1 GC +N +0 0 1290 868 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1283 286 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +0.941 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.48 0 0 0.48055 0 33.71863] CT +[1 0 0 1 0 0] CT +N +0 -69.75 M +0 286.5 L +1603.75 286.5 L +1603.75 -69.75 L +cp +clip +GS +0 0 translate +1284 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Wd;*"]RJ?g=lsJ]:"^N!^JqJ&N0pjXffo+9<\GSZd=Asr^Jj\`gnWKAW"Fg]ukHtS*Q +MgT5QlW?uD#_u/G`2SPP'fF5e9\c\P^,kYJ5eE:[j5oCaDKi4Qp6Lp"]ggq8kK)a$%!"=BP>O^;fEK%$ +HC-?[8AqK`3#+G0X[LSK%YosU04)0km3'K:;6&/&t4,#1R$1CFk/S[,-u]:ccG7c<6. +f_iq/3#p2"I1ORSS-d\kFY#i`&=9MC/#J^](X"[jrsZD[e+% +*;BDD@H+B;lH3#/]*[?laMn&UV_+2\4K*0,hRdA26^PFn@,GOEL9rpd71kKfdhQ)";&Ams]#doj,oB)s+J-/ao= +7%B.%9Qc.r=rR,[_K&U\IB[]0-K8u4S;JZdpu@F-3-!s#Pq+e2gph\'CAI8]e#0tYo#l%1Irn$5DYI/` +l2:AKds_T3>?Udpfs?/jHM!-o4:Cr*OX"MbT"T71dWAj&OGHgI#Oq&EQ*kN(Mk`e_D3q43Ot/%5dse.# +M8KpFMd4ZGB&W(-YCH,b-iMqOp=X(iJ:6[YQYL-kKDc*B=pRZl(\.NhEOGHgI+kr??CHaaf +j#$YrE($4RikGHkmpE_PESS;d$PR0D:=lE`6.b!/VPtM##765H'bOLWWb9YL/X(+t:i,/KHZD(i:.5il +qWk?BIJ;QkRn/lKq"a*F+8;CpKo*-2&)U^>!hT'X^`s/B3#.RMKXCD[imlSJkKR!c(p(J&$]E6fLc?,D +UtQE/q=2egj`cK-I+Y82#D_;n>osu*_Ao@WJD7@+]Sd(RYeQ/mC)7Ub@,BM,#1Rj +\p3c)ace+0H0:ahcSoe!4Ep5Kj,ZERqtBDshes8Wc8oglp;bd"/ccKTPtgdp,#TC7V!=kJ[6N64 +b?[8ijD[MWMOJ,m-I+ZS=3\M9DKk047n8$LCu2>qO!Z.3<()U?8@9D7X,jing,QbG*0,hR;:Eb+`*&u_ +eNA@lgmpqOr,R@MLf4m7&jdMCEI-j2%m:l0kEj'uE@P^l=dV27Zu=/7i,K8o>22cd,702d'p0[rUk+NS +j.6Naj7HSs3$M*d9f!.jB:Wq4l-rr>*^91`f,I!\$&LVVOWm#cmIuI+[u'CP+o-VPWH'sB:.7l5$PU": +lZCX9T^1at7CWqOcsh_t/esMC\WBin=$)sJrBtJ^oR(U;,#1R$1CFl*;!W5,H_S"QHuKH.&e.\M.@LaG +qERdRO&*H5[%5)HE@P^l=dZ_0?`3dM(9gHI+`e_NGU*0,hR;:Eb+`3j]Xm^rZb +p=!1t!?[brL;Zb_&Lh<9Ut+#?:1oWEfZ*.n2A6hQE[kgm=dZ^BXj]H%oi`o^VMk7SrM4*@>^M5%JN3LfpLIVO<-p4+q`[q^gYGPHRWpmH&joS;KkbhN5n7 +jm)-rR((996a8Sk_pR=uQlRYkRTZ#fk18eB9>KSt"?\%On;1'U.HVbKRZ5c,h_K&=DUKM(`71cbBkCf5 +En>$m-/]A"hf]Od=b618jtFsH3rj+'pq +MF]I7+Ur=ZY.M4KWTL9+VF!^G1CFk?i,q*C=ho1OYK'#S0aE*c<()Uh%;G-@aPH8*\!saE3m#$8`*lP@ +e',\I9tF()/ch#]"L(P7$ci@@3#.Qbc9nU\&mb);"rjHrb]uVs$5GGC/54iJf'/Pp^uKl!m1Sn4Ekj%t +7AO%AONJO6l>#,oZ]3Eg$W.KF;c2#f_Sk!+e*S)?f3ACN/79N>^aDlGUfa^[b"t3Lj)jtCl(3m`P?Y,G +BSMD5Ael_2BUt0p&bI.9Y9,m5l$uP6'p2r;6Q3@PH;M**om*Bj/7M%Y;c2#f_SfI?[.[Y?$mc!.i?dkt)mS;NbI(1s1^h?\F7oC(u0lEjnUD;p(AS;Ki\ +Ud8Q9/oTsE:1qekTPU,>9h!""W+fjPC/jfnW.tr8?NTXiqh7ajc.L'hO<2.1SNV>N-",^Pn;1'U.>1Z1grAT6V[2,HVR$==O[2E11CLO7s!,=3>G1`^/Du;M +J?;aJ#`1MUO[;Tcl>N160aeNK0lqoFLc?,,bq-N3b.Cbu$`jSOA7M83]B=g.XA`"&M)Z646:guTD4Id) +dJ;@)ie>%[QAUJs;:Eb+$Y@rf4*ktsmPhEITBoaZU_"/Y.O'S.-^OrUd;@&+='p1gR%GF"N +WVQ2;gh`K]&TK:8RTXmdll:J0lokA$02LO2NRfO-KLD>f80Oe+Q*[#:nCB_\@Lb:kX,&_M29d[7?+k+8 +LYk7,iCP,?e.Cr1'LF!=7ALLCTdaV?h/9]6+tin=b%E^>V'=nue24N$+mXX?[iF*LHJhYM(@GQ-/Uu9/ +YYIb5,iVck$6^Fq?L>bsqe&-1gGl;Zi`+DaZZ)\^\6;$k@*B(`p8.VZkk6BRVu?X.%-s,l66]/nU_(sR +$uVRV]O!;i@Bc%o&TK:8RTVUI2>Q\5S>1KqoB=P=H^2H/-&%G:;\:I31gP_QVoRU7FS7XgX,&_M29d\" +@-'XZB%0McWf>J3PAe]/6YCZ([#0Z<;:Eb+qc)t;g$9tdqgI0q +s"hDA*I`cGVkak:E@N/dOCh%kjfW41h5kLakkb;8h'4ZCDB-&%G:;\:I3 +1lKNP=ZN%QR>>Klj0X]";c2!ZGo#i*d^crio9c.0:?peZNljUM98DR4=f8&,&ofBFKofnJF\gHU)lK$g +Q<%X.j0X]";c2$+IMT(-i7n9>n+`;pZJ/UVofq_G%_VDS5p5A +.QWrVNK"H4pu&3V^=r0?k?o(n:V6SOo9S@Be*2g:HLClQDcIfZb9'se:R(mihX!A^GM9,s;@&+='p1gR +Xg)s-oZ.CUe%/MP,2uRD!3a'+b44N4O)B=,R:gEp^0U?TZTeh!+5e`+Ym6o[^&Q$FnnWT)bbXrZH`LnR +F#EYDlil9pZ;*t7,iVck$6ch$K8n>;]D)kcj0X]";phDD?+X.WgT+DZhRe"#P>=A&F6:^"-B/BMqVgn\hc[ +526,)gMai4VVR]s]_e=THhVCNJ!J-\s(ccMricjYW7ZYB4Xf52n+KraUcgE5dq5P)J7 +S&dW:Koh*h&LiC_C?99>SFY4#'->H=Bb_5XU5=9pmC-Z8IH-;GqHmB4?S>c3rUnd_r?m*hGCR*/do9r0 +pn(p,p]goiZG"M0M_b!]L[#&DM#bJ2bIEoXNaLf;\hhL"_)1ASZUc9$Yc4E9B!M_De( +mQW_==Z-!;=.>U>SVR*1+'dZF&$+s$:.ZTSi`&;Q+jH"8NRJD60q6iUXWlb(1m&_9DNn;69Z2Qj4F-MY +$ST.HmQ[--/:?N0o!2&04+$[^j]Aq'cFDYUNf<6t$cC[B(s5c5\B:(a:6b<(%':c)^i` +l+0BA,]0&P&TK:ha9Nf70^0YC^]42G?Q62X9B6$oBHc]+'LF!=7ALLCI+p]U/H]ph;`>;D2bpE03#,/` +\l71LDn\7(Q,1(H%@R1X$T%[];c3mX3#.QB8;j2N`SGFf6sT`Y6#X]A,2uQYWH77J4nm`QrSJqc;[LIK +:.ZTSi`&;Q+YB"#T"M_K=hA["Fn1O@.O%VVPIbr)=rD6cHgS4fp[3J-n],:fS/E<+G;BU?B:Y2P8AqK` +'LGtk[2Z$Qn!UMh4NYTW'bR@<"1WC5b44N4O)DRKiPP!:[qi_2/52RV8dZ^RV]o"EBq^O?3'D3sfqiSq +]=*c[djcP:70g0(Z1643+4X0''sfT6egW&$7%B-*+X@7N:60\-arVM"5Wf +gDd%PS^."cNG`U'*dMB>'bR@\CI3'H9k`(Nam[WgZZ3SV<8qf)]4U([]/UQ`bq)PYF\2ua&Z6gN#`6&''ibq;^rGU^ALLlmELHog +785QG'5*l!6:f%oL_N0"DJV`P2GW]ZKca(.2GR1j/8CVVQoODr'LF!=7ALJ-EI1_c4d``k,.;J0a.fH! +Ko*-28KfA=<(%':&\$)DWen5Z^,89_NRJ9YZ)DAK@H+@e;@&+='p1gR`(a3.R>B&%gY$B&oVRVs$c]HM +BkCf5XA`"&M)Z646:f$8XJ2/Ce+]J,h!K"535%!cMje5n-I+XuKoh*h&LiB$>bn*V)lJH49Y>*"oE&t_ +C78:m39ueC66]/nU_"/YnVRN0:@EWinbn1*ZMF\m9iY!d,<=^FZR>>^MSl:Y< +9f!.jB:Y2P8AqK`'LEur^S5He/sC5PARaeX@o:_!_SfTTU_"/Y.O'S.Lhj#b1(g^mDK\)6'62@f80K8<)R$)_j7LdJ;RbCdZZ3S<*/g7t(+.$L(:.ZTSi`&;Q+VAqN'b+uLhQ<6&D4GJaa.fH!Ko*-28KfA=<(%':&N?8<[P&;TRYL9,`&:B"MdoW5 +beh;$Vkak:E@N/dOGKB87D3#GH$Qqo^,89_NRP#f=O]160QGYm.>1*ZMF\m9ib"mYe^`2FVc,elgGh@> +AtbspAel`=3[KKALf4m7KLFV^<)d-MY*k))]A61#Ve=HE<&Cf&_SfTTU_"/Y.O'S.Lmtf0`bh4eH8+lr +?R@kY>"/&'/52RV8dZ^RV]o"EM7AtUBP?Gun?[tPM:fi4feB`Ibmh!5ERlQfKLD>f80K8Uq>)/eciSS6iOAq@L'?U/iuV'5*l!6:f%oL_N0"Y/M-@ +M0O/cjMpH##89o1f +80K8$G4sG.[D\Z&R48k-^r:@2:383[KKALf4m7KLFUC=&c'Q1@s'j`+<93ZuJ-C^+5WUAms]# +;c3mX3#.QB8Am)H7D7E7dgmNobX!gI7FPf`-I+XuKoh*h&LiB$gd5!tEoZeP#8/;@2:38 +3[KKALf4m7KLFSs<]%=+HM=b&[sm!i4eW%\k`D3dM6frK+XH&H6jb(L\$-TI)lGVVS^HK%NRO(nfSU/u +=dV27PS?G.:.:ri&ofDTg(`B1mP,=.3*f0G9f!.jB:Y2P8AqK`'LF!=7JE']\'2MF-=RplCT47HAms]# +;c3mX3#.QB8AqK@LGBInDOdjN9K_]D3HJ0L1p\lUegW&$7%B-*+XH&H1tnmfdt8/X/e`6N`p!_-e24Kc +ERlQfKLD>f80K8<<)dMn`p)f80K8<<"q0,j7N:3DO$''!?Oq9 +U8F?4M6frK+XH&H6jb(LVkOm;2bjlJS%-r#SDT=T.VbDTQoODr'LF!=7ALJ-E@OT1h`*b]#S15gT#Zi> +.QW)M++Po1*ZMF\m9i`+F#1(c/mO.D*iB]HIW-?]:`oePCT%)50R_SfTTU_"/Y.O'S.Lf712b!]J=hRVK& +XeJBm^3ob+WDi]oI^:_JegW&$7%B-*+XH&HFEkfDS?AI"eEqG2g:;3,7?dSi:O[?PgpnO,prCF`SNhV< +=7R3S/52RV8dZ^RV]o"EM)Z4j[S\!A#B`(ue[B=bGp6.#n8P$e!L_N0":.8+4lZKhq0`uE22GUdX@q4QbYJ5P\ +h04Wl,$eg>kF[6Qg`Ku@-I+XuKoh*h&LiB$S;LoKR>B&%gZ`fp-K=-T]]6AV-;s5e^3o`ss!qYe5'H;F +b:gUmmQWE6'5*l!6:f%oL_N0":.6t;=1`HulPWCPmcH';S6"%5/mc28o&Ro:hKc$so?Ea`c^d/lpu@Fm +^3t@`pR?knZVF(8,iVck$6^"ZU7#"uS6fB&)P2Wf+&"d9)lH[mWA=0R\od2hZ")=>LnB.ml-DTo]]T+9 +_SfTTU_"/Y.O'S.Lf;][)qm87ipjru,+eMJBb_Z4qYL&D++F"02rEutSRup^fAZX/K`E@H+@e;@&+='p1gR`2T]kb"u?TEOsolXu3t6>").A$PR0DP!V^XWJD6T,#1Su +@*HXT_$:XW2S!g2785QG'5*l!6:f%oL_N0":.=baf>d%uAKkGuB&g7URV!<^c8m8*OGHgI.>1*ZMWW9< +NRK+(26_((785QG'5*l!6:f%oL_N0":.=cSfBY"A0:0e)bY4P;EEYe*1CFk/SWZ<1`2SP,66]/no;t^) +A`2rt3].RjI,hSkRV!<^c8m8*OGHgI.>1*ZMAAStMS%\^bsYh9mH-\n:91=-Ams]#;c3mX3#.QB8AqK@ +U"!nKIe[=1aYutqW+fjPiiH'V#`1MUO[;UWWFua!_S[sZ&bf4?329d[GS97F2+VCUr;@&+='eucu`SGFf7,714mG'\'U8F?4M6frK+XH&H6jb(LV]nU2 +h)J\'L2$=s`b?/Fbq)PYF\2ua&Z6gN#`1MU;.n+ml+mT<0/"u^XK5+mL!D]b0QE-HU_"/Y.O'S.Lf4o_ +;I(X+*e\`%SG:0f,McWEVb(D7-I1#1'LF!=7ALJ-E@PE!gCs,*ddCiEiFV$!g,i&N0P4bQ*:6<@,iVck +$6^"Z+stk+oub))l#^YeioT:`X[b72`%4TPUHL&Z7%B-*+XH&H`.=1f\'2KB'=!PG=j4b$3SKZ`8AqK` +'LF!=70Jm4q!?\?2&iI^5FC)7iW-;=EK9ag&/&t4,)),7;\=#GU2_-MLE9.%kq_d[R"^BfL`SSeOL8:.:ri&ofBFDXWJ*7(g+B=`^"@)N%fI*lBO&S>/4rKLD>f +80K8<<(,LE(%$.3fTV[l1(+cYBMSsY&Z6gN#`1MU&N=\u@kJE*4e?nDH.c#!0aeZXBMSsY&Z6gN#`1MU +&U3`%b##a`]X9hIf.9$@'_`C?^=-\&iZ.>W+XH&H6jb(LV]t,p'igTW]A53jnGS3Re*3qdl3E3k_SeIg +P/4rKLD>f80K8<<((!?'ifUYLPFg9:FMg`U"Cni +S>/4rKLD>f80K8<<(,N8SSeOL8:.:ri&ofBFD@.,/4rKLD>f80K8<<(,Lu/)'/_r89TSCMR]aH`7$X@H&9o8KfA=<(%':&Z6i4Atl$5L>no)1::mG2@C0W +'C1nV',#:7,)),7;\=#UC9oU6ZaEPB/Kk_(,#S]'3#*#aUD44H`I"'DMoB:: +ib!;'dPaE9:.:ri&ofBFDUp?'or@0h=#Zf/cVV837*j:dC_e@8f%2?&+XH&H6jb(LV]t,(<)KjN>@(Xm +F"!WfB/>s0M/1L"AX3XZ3#.QB8AqK`\2IAe4fZ$HmFKP!]:W4gV2N<)@H-"N:74C7,#1S&&/&t4,-KH2 +osP<8/DfET`J6VnRtZ]7of?Z-#`1MUO[;UWWJJ'pWMmC14EM,!4m(`k@2:2-DUp8c,p_JL6jb(LV]t-_ +Z_G,S=FC/#TeHif2kTE#6RO/=<(%':&Z6i4k.IED'X+WU/h`9WM[tMER(,.`V]6Hq&Z6gN#`1MU&[t"? +m@K4HKiseQ"^7f\8P$e!L_N0":.<)QBLUHVHmm4U`%C16-(C!P;\:I37%B-j;c2#f_SfTTU_"/Y.O'S. +Lf4n*.VbDTQoODr'LF!=7ALJ-E@N/-785QG'5*l!6:f%oL_N0":.<+#W+fjPiiH'V#`1MUO[;UWWJEMK +mHN(q5Q0lJYD0d$Z*Ei8#9W=3qJKnRiiH'V#`1MUO[;UWWJEMKe[otieu;TGY$\q-pr-<@H0+hkj%$RA +o?9#"Dr7_n'?n>3=f8&,&ofBFKoh*h&TK;#EV&>;1M>!t@$kXOI/3=XO$*!_;bmkI_G<;a`3[cRg]-ZqDnfSlU"AUiqkMBP`*'"6 +&/&t4,)),7;\="!)X6Rk^OGOSa+!Q:?^-$WS;"`Vp=o/m2r8eSs.*_9/mio&p".7_;.a7rXA`"&M)Z64 +6:f%oLc?-/<$pD4GOKEe&&j1-n9aW;=+:%l47X^nPi%d`Gjp&TSilhc$SRJ9'95hdg[2]^4.STcIbHH+Gp"Bl<(%':&Z6i4(:g/Ss*?pDZgb[V +D]%kA5I2PdOGHgI.>1*ZMMP3YeZ2aV0?`4_F(b""%aDj4I.2\MdMtI:PS?G.:.:ri&ofBF:.;X0h7>dX +@q0$f80K8<<(*76RV!<^c8m8*OGHgI.>1*ZMMP2.e',\I=f8&, +&ofBFKoh*h&TK:8RTZ#fDRV^pQbY]p&ofBFKoh*h&TK:8RTZ";'p1gR`2SP,66]/n`2T\Lre?8.&LiB$ +S;Ki\,iVckS;NctDjWL!O[;UWWJD6T,#1SFWMjP1L5Ah@.>1*ZMF\m9i`&=9MON[GQoGhC6:f%oL_N0" +:.<+#W+l6cM,P!8KLD>f80K8<<(*76Rc\OU3#.QB8AqK`'LF!=7Du1(C&WjD;\:I37%B-*+XH&H7%B.' +k&6VH'p1gR`2SP,66]/n`2T\Lre?8.&LiB$S;Ki\,iVckS;NctDjWL!O[;UWWJD6T,#1SFWMjP1L5Ah@ +.>1*ZMF\m9i`&=9MON[GQoGhC6:f%oL_N0":.<+#W+l6cM,P!8KLD>f80K8<<(*76Rc\OU3#.QB8AqK` +'LF!=7Du1(Bh&1(!+9D4HV+JBzzzzzzzzz!!!"tZa$e*_$;'0p!m-mINc)NqYGN;kd/T!S+[4u*o+JXq +bM_=(]g)Pmci9='g16q!mB7[ +ls?,1Z]Lmf*B(SPadNrY/i&9\oQ(mr[+gT]mB/(]h^ifUms>*s8;K;I +SmJrV+[/t;Z6R_IpX]%ieoJ$h=sN7F<$MR;tc&&fR9LS8pEJOVKn<:A;[?:H\O@Zi/dqrV#"-n +%@9<:%&hE1@+8la,V0ejcrpD?R_nDV4quS0u[3_Hi<%ZIjr:YKaS^@_Ap@PIe9E,kKimPqt]EI5PE`>n +%\o%'OMJi<";4U,d[O*\))G3]W^"NaI^OGdH/QjmlPWR^1hgSf/!oJ7_NW*#%? +ZPtbl`\&e`E,L?gqJ46\%hn/(LMR;s2r@$7]GV75QCNn?PP/8n(t`"rpY\-qq9<<;91<4V!d*Q>7Nj3q +q^_2.p?[r%,@upTJ(V_Bu0K@$2j_"P4B%fGma^FW[hi=CF*p'[RB?@M_3J,6'm\o[%p=dT;9pYC&:_<8]H5:ds@q;db>plGHbZ +EiI'GoT(05rsJmpj`4^J,IXmr(f?K4FR(u^\l9i9`%9+!bK6^HhZsjp,Fif`Alsks2J'GRoAaJ)`MZ!^ +VHQ?MOZeSnq@ldR+=p)m6:C1UQU_"^i08qJ,F:DaHV04$Zc<_qq^^565M7ZGj;9+hK\(?TE"iPj`'jE+ +8iq1UcEsjQ'J*TI.)F>qqL$-2XOR6Rg`"G::WB8qH;H"GFX@YY!;jC&#W0nqU(@QFQ^r?BtkEek3h';h +u2tes6IH!X88r$Qim-NDgu!Je:.21+81KIpYUIMBO;tMa%q3]e&NMVXIOmFYHQirH1Ut0o;>BV2@d'CA +ms\6AmqUm.B<8^g9d,\=kG0IQKhuWDnc&0cr^,NSOpNmo#ml"T +De+hds`IIe[*RcHZTs#%/]ePB:kH$Y$JXtpr18pHV?k%V#$e-cm?>Sro\e1ktcdiXd;HA$r&HO\N,2&Z +`mBr>j'D'Hg\H-qS.0*G7NR(f3^2a%,U"b-FL+`Ys.*4^\m1>TmV:B09%mSF?5$rT)/$$0AWUm8 +Q@uSq:2Ztec3eJ]6DF1Bk:jeMQ_n\ffd;6j^qE&.rre'Y'TU3CTkEpn(ta=5Q:H%R59K]'__1<(LNC_K +^X?XHZ/:Nh#Rfl`oi%9R,@i(2s!d$jd0=\?XLQjldjN\5@*-t^A6o\Btl9(k2O@_&C-1F/$PtpSJr./9k*mU)[e^7<&S>G]XMf2Fe>_^uo +]233hRn,Tg96>+\T?rZ$ST/3m+ATBIAHo8Va!oQ7F@!P+gbHrG3&AE,]_$:zzz!!'f$q>UJuAP96~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 33.71863] CT +[1 0 0 1 0 0] CT +N +-1284 -69.75 M +-1284 286.5 L +319.75 286.5 L +319.75 -69.75 L +cp +clip +GS +0 0 translate +320 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WIqY,?hoG8OWe;(N#3ajLkLEb@^8BI&2TF\6>2(lYTuV)L8867 +7]aPH1l$HiU^X+\[<\qWW^fLi +e(hKTa)4Ud6sX)hpF;h^<4"a7Y9!Qc6hHj(qF-/&2eQ.Jn&:2$cc#bRdnbJlop:*3h;-mj<2FflSNhW' +`f1pIZuA,'burTk]5#lr\fqRm%AuslZ7P_KqsT%-eQ9ZMldr=YfW_(1?.lf8p$226e%b\;Mm6B/-KmC$ +[F"BlWDekuI/NNT1M>"_NQU4O(]3?aDe_;kZ>*CG-1B"'KaS\3L1aZtY@9]=PTGNGm^g +4d2d7oU@i]n@,g59pmcun])`L?c3[tnC+9CXfW90leoV?2d!BC5QCX[htYp5CW"]rp@a7`IX';:8bMcL +ldi1rT4"6PGAbNc-#,sE2E!H73H?D#^1,'=GM[VC0`2kc%?#PJd\TUOqWb-V?0MaK_bQ^]Z$#B`qsM'l +pGOldl1DXLcfitfJ(8%Pn(t/SoS^6=%Go[5)^joPn](l,p3ZT(@=@2)^]2##Dqh]pQa^eadb3:k%^"p< +R<.u^:#0shp?`_RAj#hCF#rU;]rP5`k2`E\].?rB/5Q=uuqtrBaiHj.X=gRa=opYC7 +Vk<7[,1SIbs,8f6hR[j10H;kH,-D^XeZ!H,*JY+FHL8,l1__DcF`dTYFcCf1@uVOZn`%OOd/)hFPIdi? +_d,Vs6Q/3'r>(9NT;0$H(TrP8>q5k'5p'MXDD`j%<#R,JN+nmd:qb7cTbbR +50VTFW$-E7VG4'M4uekWDu.?DYa1T@kk;r177m-0K]3HD;b6Z3fR*VGaa6@rj6)5YBse,oC&CGkRHe<@ +>X!Z:]T/U3:S+YCkp0?.bQ@qLL%>(7?[1S^9_=6i:n-o<7s%/$/Wfb;`T"jpeSP.!NP)8\#L@_cRPejf +jn5+WE;HD<],L->EG5gIfs<';V8'cPUTMh(Fns7L1E%e(Dau=kVP^4WH2XTnS:9_SY/^@(Pq,r$!?*JD +L5&[__M&A?c5YV3eiC$rRhBT)C'(^W;L[Vn&)^?Jc'MIZYe6T6!K:a:S=4N.-m4qTToV,")!l1\sOpkVm/H +M[\!OcKH&^U>MKg0/>r13NN"*X(M?5XnJ_mVtB@e^>o^MDpGAsh^Y!Xp?'Vh/M/R%LT"k1RUob"YsZ8d +X+c!MqY[V!=0J2ZIJV?d7u)hRqYL&S/mVXf?Z0*CYsZ8dX+c!Mo_bsTCt?\>F6Cj4Q5_c#B]XI*g,2@G +BL0Bn/$ZpkCBE1G5(Dq7^d(C[8,19ei:cQm@8>#:I_rXihjftai70]9>ntR*S$7,*?7g5oaeF42DpE*k +hl<)/[P7W"U"9X2h<"T[o1$JTX+c#'B)Ynl\^RHP?at&`(r=69qTW0"%:3cp5Q;W$jn6,M^H&@kI`o\-Z'h9BSC&uF7)(aW?Y"tAF%3pc&H1aMIZYfMbo-I( +6hHj(qF-/&2eVc@%+0"=kSG,2hji(I1.8l;X879DAomA)lK[d(c1^B+"pWAC3AWWSR-eA'BMR:*d;R=1 +^>G1.>c1;cQMIG7NupTh9mZBkal9oFe'(k24t!Jofuln#3MYJJT.ZsIb;>KR2JkKoi.,CX`cRBf#lWnE +I^=I61$[ANI]HB:#![P4FDHR[IF/M46B'As-M\kj:\$'>W-)[.oHM"/=l6#jg2a``Q6]oY:[D1'=05[BEMm?LGdF\@*SgBdc?XE]B`B3k'=:/b-aN0Q$Qp^O61 +m>"?#/3.n?f\s`'m-O*&kg74H61cOU.g9jTR:u>IO;r?S` +927:DB2PI>7uc_uQBi:<YkB`kWL'EnX9f;4QF:hqb*()B5-\sP`Qk&DDj6,Lt*gWFk]eLEpK4gg) +2XIA>^LW6)Gf_C-&iBJ\>Zer\/92A?ENcN$B[:,ZiidOg,28#<@I-SYVO#jO_ +@`@t6WEGcFlgh>mbG*+$MpZIgliZ$&VnFg681BOXNJ[0aIEsR*gi'.U_M&BF?/]Vi*r=ECVY+*!BT^Th +_\CR.W(loRZE!H8#]6f:bOktR?N7Mo,V@@1M_=$-kMu-4Oe\T'fh%bFO+Io?ZqsCkLB-28*gZ!\"agb*- +[F%n7!MePBeO%#n__X[FE3Zm]GJSk71M=uAT5T\'ru>X-FnOP#F4!3K&bWfELb]4(%!`c[:I%ke''-ofs524 +UcY$6RP\pI`&$AR?!TQpjC[9a_NSTKp;XA&oKc0i8TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]u +dB=A_KcU,$>+BVCMW7k$BHLTk/0jC1e6GKb*W8G1RW9)L+:QO2_Q]6H&QV"q(4[KD, +s*hAa(7FoVKo*?CH$,P1DiFm8N+]5c\G$0paBoB=$s^UL4B\ZZF8oK*/$ +(t7O!@LgBlmf#6fhMrQNX&A1TA,0-8r/'%o8><'OFC!0PotBuZeuoT8TB2EQr9ENj"Q@g=i?8'Vn8W\4 +\^ot)*k";VBDIkT=4D.a:XB%*Z?+[a_fS/EYFapKqru3MolY]N>^cRsJ,V6tJ%dJK\sHdsj)8PHH$;5? ++$NM^KN$^F=`AGE_VJRFE@;OW)#pU8qU/JpX5MZWqZzzz!,C?`$c)b2s56`'e]qsSt1p8Q:Q?[T+h^K7UFeHTBmFt +NZ%,Un)fb:gWcdaAb6mHa)-5C`YkhgFIhRT8SGhga8O4E05//en&\Z^$k`HKhQ@hYQ;s:ZrT>)/1bPR& +d9YSXmKq=2jgSWJt."rXfk=gYUfdbVRT#\oip[O^b-"kffGN]))C"heP_c^3o`T:HbNVl+t2%f9NI5`V +of*_[g29lSAF5*/V@",Hn98j@dPD*??0h^AG(Y`E''B_7S'^.mENLqWXp8HgeXeQ$uSj`*RQ`pi0Ti.9 +I:cc?sbm>4u1@lBKrl4*Kt8ie]$XSN?GoYd`M=qiOAid=k5[0);Xt`jY#IGf ++%YW``fSY?SK=jE+pKH)+!t79?WsdFDaINl<(rJ,XihUPGgo3[`WGV@L]ZYjJqDqBpI[;4B#H0(;[M\b-]H.-aZWtPBJY2iobSY +rHd.@bl>?bX1^\(>Eg!8N4#TqTYr042JCh0VQjzE;%&'Z?jq~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 0.07987] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 356.5 L +1603.75 356.5 L +1603.75 0.25 L +cp +clip +GS +0 0 translate +1284 70 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 70 0 0] + /Height 70 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W4c^QD)?sS.r.6biY#7Yb,R!3ErW*]^ncLFrO:XRQT^dLs6DTIe7#D[-"^<0*82ONp6j8:6_s%`d +;]p`+/X)UH0^GpMpN^-#@pbQDTr`6mGP6L1hOY6b>lap*zzzzzzzzzz!!%lSo#i]$s8MoNNNZ[%]"#7P +]H$tP!;c,mhgTqPrV+)*p=o"p?[r%I]mKMA<^-BDUW^j(I6B^n!!#RUf3a"j4$+r#Dmp+HT:_IZ]Qiu+ +p$:3os86q`LX/0]Cl_@)!!!"^q"sNp^\mZGqtKPDHLtmR$\.s1LS&tb8c.U.hgAAZ@5aUWHDnFU!!$sl +eo]k8rqPN+%mG7/j@T0&]:CSPf3a#m?Tru=]\Amu>Fn!)[\aFN:qg?Y.3]'a,#4"d6BI"e,3#D3WNtL^ +q<+AO<@R?Zi?*ODU&=Jnn%S,Eq8%[g?[VC_We[m6M3B-H&1$82La3+[3%6%HMUn9__V]kCIJII-LqED, +n%JHZs8;IgO0J$.lLB,s(p5BAZ1"U+0b>ViUkg9!M)Zq.mZ^YWZS&o4IgFVe,*EK_ihVAU"Go)e,#4$H +S*Rp-(Di*oB(r*W,Qrr`.3]'a,#4#OPG7a4\o<(oXHtSI[WeZgl!@)"N\*qp'Tn%k&LU!m+\9OC&TL^U +hHUjL4ad%odn`2F^YIm+Q*NS@:4X+AKHLjS73j$I`(:P\;RG:i(Ht&J05l.4o]b\nYCHE5VmE_9DDba2 +$POmV,U#^s8J+N"&ogf;S.r*7fS)t*IJ).(HhZgK5(EM#a-W-ZCEC;--;KFh6BI"e,*EK_ihVAi.Cu'O +Y?q2&j,`M1E6T.F:5VS/OGK)EOq8nuUkguZb0:\K:5VS/OGK)EOq8nuUkguZb0:\K:5VS/OGK)EOq8nu +UkguZb0:\K:5VS/OGK)EOq8nuUkguZb0:\K:5VS/OGK)EOq8nuUkguZb0:\K:5VS/OGK)EOq8nuUkguZ +b0:\KDEK!/q^HIm"Go)e+pgOZ:5XjkU>0j!iZ:=5]^Mc60b>ViUkg9!M)Zq.:5X)@Ko*-28OAMpP9Q/$ +,iYVU3%5bFc9#+FTeV]*s.)5uIh(TH7Z`XHMU"nbR4QRlR;F$qWB5=lbhY$h>W";V +NOr,J-KRokdrm(5U7BVE3dUDDH=k]4R1l"#GQ5/Hk@"-M;Ind.asl&,_8$mtN1$/<1W<`UX_-Zaj$F=EAc51c&#oeTeOJ'@h)skj$3"HHSq?4':E;l +hYV@J7kfP%mE?o@Em!cMT6S!-537O$c@I"MRBhFPZYe=[rnU;GqFe_[,Do!.!UQ=,4;g"hm$>u+46?'@ +SGHGoC-3Q@pc$GUR8!UN132chB.2>Hk]X#t>$JFun2Hoc5Fa^?lADJfk\lo%$XF+LMb[tc?X2HHeT5ck[0q1r( +.B<+ab)SM7$[gXgRm"B?]'tTL6+MEs+YQ@(8J,r?Paa:!S97F:Ji'C.]k%N(VDE'naG.rGg.=*Lk$["J +\)]0D\%.Fa/aP&X9kA5"b(';S7(%q5hLu4%:8UASqlNL(mLUkguZb"W[!c8m6X6(1_QX'6:8X_+j' +[QV*t%Q$VtC3!1bmkAW3>W076O1*f'qti028-^%YH:<^IPG6iS@H+@eOuVtSeSV9@I`D6))C=1,&@d[\+bAFJD)GMZ>[+@XJ]7p0c#Tc4Ig(_5pg?K5^O`"#?NEDi$j/khdX8eQO\ +dZ*@4_6uH"k%cX9oeAnEFAk9X>ht>4rV[_%'+,B*8.fi>Paa:!S97F:I14O^L#_;JCi=5s+&eCtI:RJ# +NU+K^-eH[(^*/qfpq5Q`3U%ZKL_LFK7Ch["`*&u`dA%hlf=BUS[p/#(?TWZ,l;WT_X\HI=D?5#SM,T7%=tQV@+&MSWZCT+DoJTDskKbD_H>q8njddjL_75 +1`H%8B<`Qk-UksF;/Ws!ERlR1A.$c)TDUcacMYB,J*Y"DICrEaGWEpCdu1+CIoK?rO.[76ihVAi.EY@$ +-I+XuM6oPuIrfOOBAM3i8*r1+?1=l_]H$<*Faijs4>'*unRfJEGDf(TLm'%BUaS0-QoODr,iV,To\UTP +p?5][mG%*oXQ8JVf:$$YIV82YX/l!R1]p#f:5XjkU>0j!iiH(A\6Y1so\Us]]P[OaHfhO2H[NIGmF,l0 +RnT^0cJg-=Gk!$!%_*2\&ogf;S.t(^$PR0DP/J\5RT1a'DU;Sm-BlQ?]hGJUbLV/=NL-EXZIpT^?!FRh;2PO63%6%HMK71"M6frK +@P14%iKfZB=+S*E[qQ(3T.DWcLTB-3]LaP9KT@Su,iYVU\5Q/M58)L<5Q~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 0.07987] CT +[1 0 0 1 0 0] CT +N +-1284 0.25 M +-1284 356.5 L +319.75 356.5 L +319.75 0.25 L +cp +clip +GS +0 0 translate +320 70 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 70 0 0] + /Height 70 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Rd;?`c&4KGtj#ST"g[Z/WUmn_W;F)#_/Nb\VUFBS.RZ!7L%fcS0zz!!(s5m(gTIIhb/3c'*3;/N- +Dme.j"Jo=Ohl'=aPeLB#RmY^(8lW5[o@\Y9k/-g-@jCn@.IC9`&)Y,K>%agn,]Rj[BV>&9e#.r)>7Eiu +@[k/k0.(7Q]7MHXhfS1A=lA$hlDo(:[W*(Tl4/5O#4qB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj +'JITuG;:;8CW-)]W9rK>WJMgid=lM#HXdq7LPlYeJXc`)B&bntMqdB>:jNdcsjG>@gX^RJpn``N*JB(b +]m\]+;UEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ +ckh\Q0Rdr1.nam$9'^Z>%AU0hs;c##/bs)1]J\Pilk^ZkL3oZKZ!I51[UdI[55!A>Agq#~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1283 286 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +0.941 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.48 0 0 0.48055 0 207.31862] CT +[1 0 0 1 0 0] CT +N +0 -71 M +0 286.5 L +1603.75 286.5 L +1603.75 -71 L +cp +clip +GS +0 0 translate +1284 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W6&8h.rAX4^rgS,Z9f?:g$7(9s8>$B%@4>JQM^V,m1lI7pWCRDkpg8j(64&'EO;<[$LfR,5L]j]e +-t4EYOCB^aV,W(Lb*sME#Z2q%@>;Fm\TPR1bWY^Uqg6UKcZ@Cp_kA?>M-`umcaggZ4QkRJo4AdK*&u`M?AOGK)EihWMi\o>3#BYdWn +55T/#s(KEJh]QXO_r,Q'&qWNA'G2]]D:=23e9kr%*c6u+egX2O&>\RMWaB,`+0!02ars1^J05FO7Dqbp +gN;D[jLKDESWZBA6)ms^I3#a^S>WGd7(EGW3%6%HMV=p8QQj`k4Ct(H6uG:o\EGd/%-O[lX4\=]PUaD('*R70H.AAeR7%=r'rYWV[,DVDo#ATp*fD?3h0` +;FHa%gcf"'Xg,:l.9CkZ(i#.K&9hie.AC=(7%=r'V$jb.(J\P3/U5FgSg4L[,TF[m)/tJsOrt>a77?CW +mF-cQW8I=er2f4!:D/9jOq$C\$pI+%6]d+f,,02FR4.?^MWPY^'.%"4hX)*IZG#kf)GP^o/Zms*uf#- +-Sb0+Obt7n-UgEtFQ''5Ci!@kIf8E2;V\&U06llb_m.^"SJ#=H\7pRT9SuA-OGK)EU3,>o?.dR'n_r3U +Z`pGC-Vj=4&@+!XS6oLtE<#p`pKV@]f;h-<^8LFTR40R/#sfb1M?s()NL(l]Z#sJFH&&"p&J2_F%>bPR +eOU9D#SM,T7$N,YJE=B3p7!1co'iC3cnRVc:TRJqB5H5:.u77L73j$I`(:P<'4#j*bB;c@T&/_%=Yjg\ +)tt^+2.`>ALp:Aa2Md\D73j$I`(:P+\9OC&Q*0,!*si@Kc_1K +06fM..>\l\lId:t6Ae<#OGK)EUHiBn("Ogj(`O6r6+i9nOb=hh-UgFkYraY"dq^VH7FioE +(`OC!6!Qd784&DoP9QaFfGth*l*V>_3pW"uN"+%b+J))\84&DoP9QaFfGtgsl##()7lrFPA"Ad=Z!gcn +Lj,%,Oq8nuUkgETFpEbHHF;[+=f3NSCfl5tiZ4hN,Qrr`.3conh19"*?7iYlegZFm)cgR'GafD0KHLjS +77"o$5%,nXWSgqjN[_-[MFLa3+[3%6&oN89&J +:TkP("m_C@N"&?8q$=M3?P92k,Qrr`.3conh17=WY83W?l]aer%1/#;K34"%6BI"e,,3<6*@U=IC;JT. +:.`CVOOcBMLo#nQ"Go)e,0Gl#JT=Mh(7Zi>PXLH>D&D#SC.T"/#SM,T7$JS$"!sID/N@Os.>\l\g,#=) +VPT(o"Go)e,0Gl#J\4d^(0gk*PXLH>XW#;5do&RHJjc+C&S8uM5d%"d$SI0V8g;90%Ldf==GfX9OGK)E +UkLa3+[3%6&oN89&T:9PF<,jU[_N"'&2C*T#gOq8nuUkgETFpIRt +]ioBkXA]`d2KZ/%QK]#h6nCbP+\9OC&Q*0,%eP0q[G^%IV^,_fkf(=qLo#nQ"Go)e,0Gl#Jc8EJ(-E/Z +.>\l\H8M!Y/4Cg-8An(3;.f/)oPo2bi'FVC8]99.])a`t.3]'a,#4#O11PO$gXE2,&!1p+,0#@21jK[Q +[T@/l+pgOZ:5Xi_@s=THe#NW_Q?>eD`eJl*Hc;9pOq8nuUkgETFpH_c^#dO=1h*#=Lp8*^-!n9]La3+[ +3%6&oN89%C:N))2'MGc-8]98CVm3@j3k/k%$/8An(3;.f/) +UcuJgbY'3kSW\f6+b0;piMD>U"Go)e,0Gl#JTReKru$Yo=JqoO?1Z=%74[9J:l:dHp/49kJjc+C&S8uM +5`4$[o3=]LXOC%7mD&=0Ibn(4IPj#APU4X+^D)f`+;#ti7%(;A&3IRNc9cWq3ZWk?$G:3TD,QA.]K-lK +9%'XOH-2Wm]DSr4:qe[@1').4X,YnO`V* +@JD)S5pB']Lb`IaT\8S[JsjeC*Kmj/Qm_ +opVa'7DmRO?Mc#D1457<+;#ti7%(;A0`Uc[_),Vl%`#s3hVM/frNDN]+^2;dY8fL=-V4RVEDf2h)7out +SR*n8-!7(AX5=ETAuSGN3%XaU>YSOc^74)F,Qrr`.3coV[&VBqbmP=?>e+ldn[EuGoP22nbrRF[9bHf6 +WCtX[/Gk"Kbr:OiT-PRN`(:PtnSI1bmfWCtW@/c5@o +$TEtQT-PRN`(:P +]im'q>Plb\(3*LdEDf2hl"EXIDVV1Zmn5=I[/H8LNK*3RB?m$8qfh1H@4dj9B%Q;65)gfM,HE#9e(($Q +XL;9E0DnSpN8:VsICQ`:s+T0(p`W(dki#GuQLsn*Dcb3Xc[YrclkdG0X`$B%pu?GqcWK3UR%P67k?gRZp;[-$4%Yl:H@t`<.2dL\.rhlXVf[%lHOWU9`Ie:5s]K8Q2njtW!p5<;1pJm)o +^[T9b*)1]VF#Y-W*i>\:/mPojloieSr9"#S#72%F.sg.XYD=Y)aC4U?&&ZqhJ1AP$Du2"QoA1$V,A+46 +bE4a"E!-["OX#=&Pp0"0%8>Y.leK$l42*KIDH],$J(i"3A&q]_N<-aKd=hmr/FL@n0oNR^Obi7X3%8%^ +h07cTi.DPXs*!@6.d=gg=f4Y*5)Gb#dJ;NDP=9DsV%L$mQQm+t*>'YfbBij3=0$ZEqqqF&CMW6g%mDt% +r:k(chG"e5c8m8on4DE<'MVq2]W`!K_gIHe&W)m8Lm+PJ4T4p$LCP>n^?tO6P:*P>]2nOJ\hJ-T.8bgq +io[%M@3`3Hbh'?EObi7X3%8%Uk2th'n%W5eJu<^!iiH(Q%%kJgVFVr/.@27l0c^iR7;^M-M6fs6140_d ++@T!CYrY!j/k70r3[.=Vg855kB:TZHpa!"^MQ4+#K:'2SN_%*17%=t!9c#/%3[Mr<8_c'MZ9c]>/k70r +3[.=Vg855kB:TZHp`sbQ'\,-O)oNu_E^V+K,N>MtD7VE@egXIN:Z>DLdDB+be_tLf3?f'a&TL^u2I2\0 +l]`g8-ZO\Ofc]E-/k70r3[.=Vg855kB:TZHp`qJ^+reJuLW^9dR#ZudU:0='`*'!+)*WmMOWka_\/^Wg +l%h;JC7?hLS97GgiKLcn.C)6%C+`"%@QG].MV=p8'5*jKAb[N'Bo3[D&?E^$j`3a5VBb)rW,9+"ERlR9 +L/0":(Y;&(WnAM<^[tpE*0H'ALc?huRk@C)F\7nWPL[n3^.jq<)oSN3E^V+K,N>MtD7VE@egXIN:KC;u +CR[@Roc(j7n=[92Obi7X3%8U3/khdX8sJ:uV:9G7[Z_p0994eM&W)m8Lm+RAl,(AAVaMCdBs.G@T%cr$ +qkZg!jbQ;u8&RtrgN6l`XAbi%T/`M@nF@;W5[7qGac8m8on4C_Qmk2EgW +HE^Y78AoUM!9^T.;G>)9Up5tPi7fh2;&Jrm[S^$aSjNh +jDmX4VBb)rW,9+"ERlR9L/2u.\/i5aV\H,J6fW2T,,QUEUo8.A0QGYm;2.kY/\M+.cWB3$Q_KqhlfH%G +EB,3Tdh-Rte2CU"39ueW_^AAUh%[Oih^Pis4:loGM8:kd&LU"adtNXb:5L`REc_&GU3F[$7I'@RA.U#@ +_/r)8'X^XO-I+Xub4%E3OmV(DH)*tO/m]Sb@K9(l`(hn*Ua0!cZt0%1DP;UdU1QoODr.)Y6#,e5@ML\t'2Id&'<(F0?! +3[.=Vg855kB:TZHp`,Pu2b!1sAW#!rnmiU`8n9t,:5[c&$PR0D&nUhXM)Mep\)j&&Fgs-9#)V?,Neimn +[ctZI=f4[4%\M3e6*I4,QmOCEpj[V0'umk"PpGB4;&7UmEDk:E?'mV;Pc39o;^icZQ>VT:+o;W,h6b=d +I?5k2l%h;JC7?hLS97HRSm>GI*e7:0MVb%[$KHU"!gF6AMI8Ih:5[c&$PR0D&lmhCm@TZBK:ATZ.&=_Y +_mq9C8EA*=7%=t!9c#/%3[O&lDu+EiXfa^!`!,DfVW]&CWA*UVf#e:3R55iq-Uhn$Ko*-28`Ds#RVjJ' +mjG\d3bMaT'YVDEG@8TU(u@t;?b.qJR#ZudU54joqWQ(e/+qep$PR0DP/Pt(Ztq0l*dZH::!l&h\C80F +8m1b'>1l.6iR3EO`,nD;'Wp`MV+X=>*e""[fB^1G(FIEg-&KM42&@oP4F[3QeT5kA3aeMA6sYn?C;`_aB:AbudTNQ=pW6[nLHus+M_7[IVBb)rBNZ8n)c&WieD\<# +n%JI%?'oM3V^/N,88asC9)T]>FoVJ.ad(:E%qe9H-5"]=EDj0FX&lJG4*KudkF\r,TaTt]N'i3@Cfm

alNC!rL`P(NiT?bJ(jW!*5;H#d.s"U^%um/%u5b)NeeWqMAEYVNK&pu1KNq\$i`Xl +Y=Vu-TkKO('=T6f2oo1`^F:XXLXem*pWU?*M3sB!>M +nuEH>?7ANr-a:JP_B`SO,#4#1>e5%Aobj6gS$RUE$e$!ZPU'-2a_*:r1jm(+bZaMcc4lOK.8,-V-.@Q> +`,nD;'Wk%FVDHdP8Ycl[cX9pWq7Gp=UoE&eq;JVmM/W^dMUcjq:lZ!rbboY0774/o,s:nLke\TS3[.=Vg855kB:Y2Rf0NS`S%+tP=D)Rl_)j2=;9>*/jMtR@?'n#R8&Rtr +gN6l`XAb;G-V2Lo@F:orL&]BVrYocYS>,l+>0?_,>TN6a/kdId,N>MtD7VE@egX2_*2M`O`3Z:"n*2`q +VE^hiMMc89Kn!oZ$$9Sg0c^iR7;^M-M6frK,)[QqS1Q%f$G>;\pdC]UmV$P8(E!(FIEg-&GghN!%ZOh.2q3hLBg_4=1H/AH2_SSO=j3NR6ITI'Iddd/khdX8eSZ>.6a0gDT@*ijbKbhlp4^&2#F2^0EK(=;PRGoe2CU"39ueS7N/*!ZCFdC.7:BT +eJ-3n]&d#Yh\EcdLYVfNkdKC3XoA@Ufg?Q&@&-S5,UGlrdtNXb:.[d6LeF/1L%e7'qSS3WhH[YN7W=]4 +Fl)U2l,!RJ;Bnqog855kB:Y3=+;'r(7E'-UB2OGNC;J_uab:7$(FHq])KA[)2I2\0l]`XtM7?Sa<9=C) +T65D3gS+L-HSn%Ud"MeqUdgjg[7qGac8m6X5pEJN'G\,DgquVmB6ZL['D'M6/kgdC2<(F1D7VE@egX/r +&om<9W5&l%mbV20o@orTg@J#&_?>*],>O-MV@+&MSW\XK&_BI/(2a[rf(E4M_kjj#D7e,Y/kgdC2<(F1 +D7VE@egX/r&oeqgBOUe]XS9,#LE!rU34IY9l[ARNp#4V.0oq`-Gu>k^MV=p8'5*l!KHLlYBo3[T9PmOa +39dGGuU*Gd?dl(\US'j-+nN +/+=*Sn5M#+U1@.ig^$icrVY=X?'r,6Y#F$s8AsVTU88cJ+4[[n\,H65AL_ZVrOk"4"c5!+&TM;RdHhBX +]:fTG2n!j$9*C*]*)kAbNMehlDr#jcE,H1;)`\+ZdRXsiUIrpo]9Tt$$PPJ!F\6u`M7@q3eGO`nG&J/: +)sD,6D>kCGmJCGODj7XPR[s3EQ+_eKtEQcG3$_/kKAA/q:kmQJTq,e7b*,2rg!U3T10(T&=-?`/!5OGGDB7EaOOSbDjT4*T56-)90:/i#t.7Z*!@E/Nq!P4Cfb'P,#FPV*7T +EFl%]eQ;FH?@MY&mT#$\Q6%=B+@X3X2+@161gSspU^Ihpe9+C("GsWSV.e`OJ"(Q@H*8l=2IWQ%K*i5S +OWrchRA-dS@WZ&<#.[LnSfeV(MA4UR7om4I.B^4.D%(Q3'LAI);FK1)]K_4/mS2Hj,Y"* +V_1Zu^t'r_&L?A3.'7bZ]c[0aBaGDl;FK<%;Uis>dHg,aO$tEKk50:0ieFg8l&d)# +-Ul-gFYd67"Go)e,&T(`lKkM2O[gHQHM$D?DnN?_[<;<9jiM%RqsCk"\o_ZCpZo]7%;)0Z00pdWMV9c$ +3\3h?8J,s[85X"q+r_I?#7hl'k*]kj^N/GYQ)a48pY9i]re`=%gb@;.]b;[NUo5SLX2BNWS.lQ:HA_"F +H6W0#D6@0Vp=a;c?+t]3q^\/*RoS3ko#W9Br=rV*\C/tXR1LY'Uo6EsO#6eJ,iYVU\=uod_[fUOI.>1U +mCrJc]CWa&(FH7Ru6#6]lC.3bqM']ngpnc&FS+$"B&Tna\hq47[]RS5'S_Uk>;Kps[0uWk5O@EHT9]5CN:^GE3e.HJa/F +Xfnq[GMsmV)JV*c.kDr:`(GpOROGK*pcIBLSGQ@J2$PIGW6sNkDlM/*W9:%9E5%AYTF8GRYVb^A'pPC8hB^u\]Ko%aHlXc[F3IFCF06m7BX+Lm+R= +A35)D&ogf;>\>K(eU>iOFCF06m7BX+Lm+PGAN5;K&ogf;>W4)MgO7JUFCF06m7BX+Lm+Q"jgIH+8An(3 +EG_U%[qfpVFCF06m7BX+Lm+R-jg.6(8An(3ENQ)dT1a'_FCF06m7BX+Lm+R]/UTpfDT4Iu,#4#OCG/+d +^+Xeu/khe94\5nlEDeUnp%Rfq1T(NlEDf20)_#f7oIO:83*1Hi&1$82BJB;=+;Q6^UklPA.<8ef%_=?6 +Y4a+a8An*IOS$%K/4d]N&TMM`jM'j6FCF06m7BX+Lm+Rl=a3#8)]KZH7%=r'QRYGlMk3ET-I1/5,Qrr` +.2o4?[nN\?j91jY*LOGK*p&Sbg.=HJ?&,3"n\Oo(kSTF2FrC/5Io +7%=sN;t:B,5pB']LhiS+pI28T(FIGXSg4Gq3%5cPK^Yld8An(3ED9?l^?>i.QoJ/oOq8nuUkg9<\g0hb +8J,qM;rk[V+C1?o=*p)A,iYVURjg7O"c52f,-F*umYd+SKo'lY-OE'_:5[[>fcV%UihVC?Gq!c@F=OjU +EN]!\+\9OC&[PNp+;Q6^UklND;rk[VKQLY^>jY*LOGK)EG$AC-=HJ?&,3!dDaF2HMl,(BLg6714&LU$6 +C"Wr)5pB']Lmq,$qkBRlG4"!Fn%;Ttn&"PCKo'lY-OE'_:5Z!tI"PO_Lm'%B[Su8$k_=!1\FJdXNun)E +:Oi5iqF7Oo'Ba++&qWNA'N)'m`eklN-UksJ;rk[Vk'liY?eg,-m;K]2(Z!m/k2r@)LRrnaamH:%5(3:c_1Prr>P%iO/khe94\5nlEDf1l +oc(jHpO@(t,fo)'T^""+?&RJS]p+M9NA&$@@f*W70PkZ!a5ABB +YT:B^Wr-28G4!MA1ImHFq;(Lg=7_uo0QDhCOq8nuUkiU4^S-t2iG`=M.)J]kp"!?rpWiX7pV6asJ,d.C +3tElV17:64?[(MJM7t&56Qeq<8J*J?9[9iDI5p\%7#/\=aPj)#;7CF$38aHVZQfhcn&EAeeJY]C<;lO1 +N>qoma$9"(n%N;PNn)Wfqng9/M(:,&8An(3/$hP.E&b-dC^9EPp?^Jc*S^8[p"eMHjiWj@#7mCeF_]bt +6l.2qn4=7E7%=r'0muEkM-#!a;B/TqL`bA#;M98+FQq6>1H%0NY?nnGrSl:%5Fqb],=dbVr:%VqrZSj, +EJ@8P&1$82La3+[3%6%HMO%/Jj/UitpY9iE?b(R[EqP^Trq>NkG2MS(4\q?G(OWh06')XaTorBCiiE(FIEg,mcj]:5VS/OGK)EihU6jlg*kt4S$SeMhpi4kFR`! +rj_+fF^a:2;j%\;ihVAU"Go)e,#4"rF5t9"$PtF#\T29?GO!&`=f3Mg8An(38I,rK;FI#h[7qGac8m6X +5pB']84&DoP9T"^W,9+"ERlR1#SM,T6kY)>S.lP_7;^M-M6frK6BI"e,*EK_ihVAi.;G>)9Up5t&qWNA +'Tn%k&LU#c8Q"5a@H+@eOq8nuUkg9!M)Zq.:5[c&$PR0DP(Xqi-Uf:(8An(3EDk:E?'mV;PU$Bg3%6$X +5pB']Lm+RAl,(A5-Bl.c!^e)$`(:P4#SM,T7%=t!9c#-OMF^'qLm'$G&1$82Lc?huRkma.'Tn%k&LU!m ++\9OC&TL^u2J8C:.3]'a,#4"d6BI"e,3#DsD9ahT;FD.L7%=sRKHLjS7DqbpgRM^3Ukg9!M)Zq.#sfb1 +MMRIj[Q27D8J+N"&ogf;&qWNA'G2]]D0(GgP9Q/$,iYVU,Qrr`.3_NEg?&eW-Uf:(8An(38I,rK;FI#h +[ETR9:5VS/OGK)EOq8nuUkguZC7@"QS.q*=+;#ti,U#^s8J,r?e2Cs,3%6$X5pB']84&DoP9T"^W,9g6 +EDf1;Jjc+COb=hh-UksF;;7ULihVAU"Go)e+pgOZ:5XjkU:*)"`(:P4#SM,T6kY)>S.lP_7;[+#M3B-H +&1$82La3+[3%6%HMV<4R!!!#)d&6uh +jm;ILO>MkKn;Jg8frUZZ]r5V&$;>m6Xa$RV>cYK/%qJ"gtKHbj:d7:f=CEkc2feZ&dm^/R2k*\Sl%NW( +@rql/f7aSjk&VX@p?[q^q7h6Y'3).d@GMH&td(US9AUEp_#7hn?h"RS:8?]c(lIstGoQ#'Oq!kiNI_5& +=7$2rkX']^.YMF9G4W6%J\TI)adA'3=J,T&n0,hB(b3fr]\Wa(+L?_I(4aV?fC[:G=s*HU]4S6ufI6T+ +eo>i/;<(lnD2r:K2FSYLha^fGhmC-0_\*q@+WN%IHF^Lmf0B0!Zg)RQjN%p;0J)P +#kFS`+o^^0Y=3kXu04+SEGJALV?$Gu?TI<9pR;/J!+uJ%`N8,sqcP.8-PP`0,Idk,T%rD:>O+7(;Fj<5 +IjHKQa&V\ojfJn-Fi%r\AZi-/(m&X)a%2(ff.a%FfJ,QDt0.0Pee:h&AIJ;QAil$]@hu=5DDggs +A`ujd9+Y`sM^A)0&X*(M3hh^%GEKnTjT:pSo@lFk9pVq9F1ZSN>jIH*!Y +WO:E9lo]olaqCd?o9/V+"Xf_QgB0Y_Z5uE04iEj9JrT:4C1YZAtgb^rC7CsGJKAM"Ic0>H"G[HRX7uf_ +*fOK=m,YIh1qsRSune)d?nA,>O9T)U;CL*;Br97",WZlQUjN2c;^%\@i?@)1a-`bG<4nmalh5"2iWDf] +)I-Jp@!U +2]_:&peW\\tJ,$qKo)$]on*IP;&Xmt>iJ3I74'%?6)]K_404$b\il'R)RQ=,`\hJ+]?.d5h`:X!dh690 +XYIsEueBQ;s4-*A?L +OieoIi/(/?bo:$dC'kQ(G@Z9*D?sXX-Ei1[@g&AO'kB;6lO3h%\l-lQ2j?2tN'7E(V9(ft%%ij5[(.3- +bJ,F;OR9*aDFnUa6NZF<-Q88E4b)S'd+92,V%mTu2?Or"QltA+CDH3*&)1I%If4S8765S +V<0*)tZKuH`7W+O"4%2X,jJF$srVPLPIVW`bK+C.*If0!-3e`8??bTc>#^>-Nk:9muI0V$"NRnQER[7B +1\U%.8IJWTU`l=iJQ[\S%+'$X&?[VB4hiE]4r8]4iRD*";RZ=DG\Ha1#i:boDs4>$bebA*u(qX"2Odfq +L89KI0hu;4$K;'WAE7XSk2th'n%Wd4!\aUY>?`1to[2dDQhu4SP\iH%H2D7f13*H +szzz!&,o&+5.R)MZ~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 207.31862] CT +[1 0 0 1 0 0] CT +N +-1284 -71 M +-1284 286.5 L +319.75 286.5 L +319.75 -71 L +cp +clip +GS +0 0 translate +320 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WD2;/&)o-G:,X`iPOGFHNaKY5fXHY5nR+[7N$n6O5M*YZ(()%@<+RB9b%h#XCCa$J'b9A.Y +)Fhm_;c-DkbE=Ee/A2ntLrEE<4I=pNF1?F#N\c%<1Y6pJs-O>'1M>n+F#s=]pDNX'.Nt0gZ"Q9'Uu&+O +X/q)<;uQ[nejD^fdB=;]Kc^1rTjR"@eo.7t/_'008&qGfLCQ3-5:poKZ$mK@iYM;9#m1Q(^\!HVU0mac +H?&q"E*Kl9ILu_?59#\rknbMiY6"Jg+S9H"ILuka)PCF?6GE`g(P0r:!W#5-oP8u+&IA=RTO.gg^BWm< +J^CS$qF)P!COgjG#lP[7?N`^1#3ZkSd'!KH73M5M>9"@Bj-kSJfsY6"Jg+S9H"ILuka)PCF? +6GE`g(P+EmDm:P)Q2B.M6%TdXKiJRshM235j7nNtkgoh5+qXT.5edSXILXXW!his'oSYuu +f)Yan&Ge7L^BfLA%F?a0TjIooMa6LX!<")QqDCOQ#_[X9cn>Ho?N]qY5j6f"r?;gL2*en^KROBW0*7hS +"8%L:kgoh5+qXT.5edSXILXXW!his'oSYuuf)Yan&Ge7L^BfLA%F?a0TjIooMa6LX!<")QqDCOQ#_[X9 +cn>Ho?N]qY5j6f"r?;gL2*en^KROBW0*7hS"8%L:kgoh5+qXT.5edSXILXXW!his'oSYuuf)Yan&Ge7L +^BfLA%F?a0TjIooMa6LX!<")QqDCOQ#_[X9cn>Ho?N]qY5j6f"r?;gLD6ZH`8gdYcrN9CN.:I,L(3H>/ +QB?3!P=r*7(4#k!j/0=De8MTom)O\l$7E!gmQYTidkA1,2ibX)+@f`ulpb3MI^t0Dd;Q@V`@JF7U-k*J +gb,-'>G"0#9D+9nRjU8UXo.+A#LKlS0UVkdMG4(pBT>kg=?g,n@7\b$:+8nNeIMZU)F(QNm6B?R3 +@)6nr+8nNeIN,,O1]-APHLtE+rLDP2^X/b53X(p-qreiBoNRAQF(QNm6B?TUKq1$skSJR;)^G-Jfgig( +UYf7D0k^4-0D+J;;Y0%p55KsSrK_0_SCpEId%arTZffOrqLp!h3j"f+qreiBoNQ(hSCpEId%fK!cC'+Q +$b\[sg\Nq:O+*dhgO;lZbWblW?9\7oRPXj)(OCAmf65I+UL36HWaG?XZ]BR/Q_/_j0PC+,0D+h^DfY!a +55Kt>k5.m&SCpEId*$UI8XhO5kSJPJm5LGSAfpt<09c>]G0]HL?g)()h^$O9IImo\/G5M13O=cqTpM0r +;__qYd4b-td;N4NbWblW?J>9Nf::Ma^X,Vr^T!\YqreiBl0UEhF(QNm68+>sVK^>PUL35]Y2<4)R=4]8 +]l`q:p,TI?I\E*oILYJaMh!)CGKts>/Ja9C((,a/f3S-CWo_C`*?D%hJ`a)JCl8=KA8b^1TCn#C^jOF$ +^ISjZn[HIMni\f-,!"*"DfWR)VI(T=.f)#p1]-APacZ(A'/F4od4^Y-ao*Q!cKq<5,Y)>@:"5]dQhc*N +`l:cG5$di\dF$A3naWia4+I8-ace)bAnFBCTRMtW>9IVmfP^iSbeD:Q0+\$(rk*OJk2G:XpYUJ8Xf_!QRrA;lYJ3^$du_:S;`sZq +1Qp;I_hVeX$"8"#2L:K#.opK?[-=?H5'l\2pZ0&Am.Pi&A6?KhnLLfU,/5OWaL\DF4#/Q8O8II#cKj)c +^Z*uG613F47i(FXm?PY$8po)^2p$DdOU;sno^'B/J;eIEp?a/E"sS?8>EB[!pu@Fs6gX-B^R"!Cm6j=) +(#=&fD2msM!L`\OME/!r&,f+dj$oqe.qf6qH?Z`In^\2?&;c?Wc.rmBa/`tp5^4#n>^5T2\7HE&Hn;A)9qI^@N +o4Rl`Lh6g.)"'q3`N\56c'a!*kdTpV;d=-b0fU48"Oj]\JOMqlAAn6ns&6I)**f#*BB3M +:W]lFl84%N1LchH@M%n853IgHn`/!F)G'?sp=S<2=e +p[$S>ns!Z5pLWJ%W*p^J+OpbqfV.Os8D]>;spHeek=-LVgNTFO,s1]`36l@6`03g@Vji0d7o/I6/4^(b'_h^ +9OmoN>s8_dm-Jg;%h9*#g&eD(idOfeBD1G\?eNPr+"&E`AOh1/NAjHEVj=r=muA0I6`M,br:AgGrCS2Q +7qSIB;mj>hG2@Q2/NM,!MWDuJX +7*CJcn"NO`ZJY<"R=0AE=5K_%+YunSnB&UKg2VVWdJmsj]=ZgGTd9N>"=6Q0q=0qm^560lC;4e=a,V22 +3lAkQEajCfkg:>e\07a,g"G%)99r+j]tM,Yj"Y?D#J#9[G54uF`3M#;H,G@GbEa`HBp0h.e@_, +p:Ke-CMR^,hf&CEQXs^pP'6JQ7'c=Qjicc%Z<<'AQM$a43O:@LrQ9"2F(TWAV)<&>6rQoH%11)R8W[.m +`JPE0>VrgZfFP#cfj',Rl@rD*VK^kT*`mM +>4da@47gkId71T^E%U94"*8']k51-/10=CC$$%__+snW`)LZTSCM[f\3WH1%mBRL1QiGZ1l@7BAr`n3' +p=X([Y?p&KBUb^KrFTQbm)r2>W(N)t14MK,i.1Jc/no(8R@,l.kVp!381g$HRC#J6+jF8UqT[Z_fgif& +[Vt';4aZlr:JY1*jF;55I]g"Re:5F(QNm+uo_uS)HB6O+*32U&*%-SCpEI8<+hkAq&%lUL,:% +ZJV1rR=4^cjO!OgT/j52oDPQbqQUFWN42ib)4.2MoUDo!DC"iPVZ$@6$e('D2@e.7oeY8;f#?dK[IuaAb^T_S!egj`sp7&O99DRd1 ++kMd79M\I-n*\i5,J&\$XfK4V>SX]mX3S^<-LWI2O&m<3WCFYm(Uf=VHBg!\rqTFJi[s9Pqr`W;5IOZF +arJi8,j3lsYMHEk6aRfa\ccTgV((`Cl>6B-PojluK.PR:2u^\&0jCjP9e$,A7LnORh]uGk<_aKCFe,(o +EZ-R:maD"`^=KmlCWodelb)M!r*j79`Xf@SUYkhSgW"'Cb42k*qK83gLOPG+rE)##N(tY;JHH.Oh72%d +?Vd1O;D\]Y0^#A&'.NslI,\HQ_R +rO(5:nRJ`nXd/Q`cQK"+chcBArkc,9gH9cF2^-e6mHs;SlMoSmgZ98%kFp;.[FWl8CHW_'juVC'jnWim ++T9L0qV*:p_RB*k4IPXsP(`/AcHa_X1!Z=n<`SJNY;'+4X`M_Z^U5R%ZL%.goaJ-$o\6>BoS_e*haZ!H +P(S3&7DDA'qJ?rF+uqLY(5p02r1RICL[:#$k-H++da9_2KDBS`('."dn!s#cV18J/)#tG&I]^sMo]Xm6 +rBcQoXjA`nkgp1?6B?StcnG%Tf*_ICO++4X0D+J[$N3;PI]^sMo]Xm6rBcQoXjA`nkgp1?6B?StcnG%T +f*_ICO++4X0D+J[$N3;PI]^sMo]Xm6rBcQoXjA`nkgp1?6B?StcnG%Tf*_ICO++4X0D+J[$N3;PI]^sM +o]Xm6rBcQoXjA`nkgp1?6B?StcnG%Tf*_ICO++4X0D+J[$N3;PI]^sMo]Xm6rBcQoXjA`nkgp1?6B?St +cnG%Tf*_ICO++4X0D+J[$N3;PI]^sMo]Xm6rBcQoXjA`nkgp1?6B?StcnG#0lF0kHp*5[%XtK#`+E7aB +_Q!etn&#/\qDnOANIJ"V0D+J[$N3;PI]^sMo]Xm6rBcQoXjA`nkgp1?6B?StcnG%Tf*_ICO++4X0D+J[ +$N3;PI]^sMo]Xm6rBcQoXjA`nkgrG#:p:uB;NH:/YiVS!?=\aA%mBV1.r9LpYnpPOb7#+T7?k9l`\&=$AGm'f@HDQTi$=>rV+-\#Go\#*dXc"^LtlaPmtHKk>(kB/MVV",EFt8e^YE53& +s"0DV_nM5QBMJQl5P:++MB**#KNS4-fcefuh2C5Q&A+0>.2M]C_"bC@+H1b`s92*BRjp]F:!":>*^bbP +qkuqsE_8]B>c4/mc1]^PGa&004CLV#:#<='s(0Zut]c]C*7'-IO\+1T#fLVTOA&rUsI)n`;t5U)mlmOW +8Z.7lgC.H1g.6LL5K&t@h +ti_ZDueH7l^N;4Y$1MR68Y;I=6P7iIC:N\]5b + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 173.19932] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 357.5 L +1603.75 357.5 L +1603.75 0 L +cp +clip +GS +0 0 translate +1284 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W^M4I0*6)FU+ZEdT"9KdHdb/pM2ATNL=_]>@Jt6:7&.;]6@k"iQ5\r#'h9J>19i,kbZOm%uh9=1A +rU&moo.\-L',-,HDbaUs`j7H$zzzzzzzzzzzzzz!!&\32#mUVphO\^$ZG/00k)W,KTO9SL_<#uV]p.2C +<[bGV]n_=M4BGO+XD(X,,_0dggP4QU4eWLWJD0R,,ZW*.Vd3K-Dr%[#`5(/-m9jV.O(dbf%a\S.\_TXa +Jjt.66`F=4)iSh40"i`eujA\MR+d#\83>Jr6$^5X,.3TsW.\_TXa +JjucMH[??aZ68`&/$u<;*mh7<('JMXkCSPS5W#cM4BGO+XD(X,,_0dCghA0.4'X8:.:BY&ig\h;c.>=0 +KJ<\&jde?,"`,tS;PCeW&7eHS9%:/+d#\8-%nZ]7H@FtROq4Y3MhCjaJjt.66`-iq)3N&)Q&ig\(6:g0:6r$1Qftug?;b@=PS;J^<,]Y@YV +kVaY@<0XB"$D'^2OjYc*?9H3r($Kc0T=$a-%nZ]7H@E)bj"BV]6E^-IJS'M>M?sTS5WJ`F7V#eX&mWM> +e0XpI`ipI(lon6#`5(/aJiiY-RU8P*^&*2h7`NsRl>8/UNjc4gVW$A;\h!U]dl#0l>*Y\NFLJuEC:*VO +])iP's6g$rVQKhqet?(q=3LcHp41IB%O4ns8;]aZF@;saZ674<',I(N+.^45!4iR+XD(X,,_0CS=Z>?m +d<8eqXj\e^3ob)XkdX9Sh5:*>F:q%3ar3*_g0ng^%;j-1,43%*)T$njA\NB7;M_qp$:qqYNPNQq6,2Gr +k2OJNupSmp+Ed-M8`:Ps,fgSnQk7GpVN!>L"'C68:<[lQ\ek]QOT/j&ig\h;c2btM\k4'4[)&9p'.rZ_ +^ni-U[O%^NSqBep9#I@h#m]TAa#^='3iEQ`g7.!&/$u4F&ig\hRrUsV&3`9AhV[5*IJ``[aiWg"* +.B$-_e[(%^K,`;F,<,RHTb^_p)5[Ko)F'RR?4A?,'#.Ok32#BDf>+4h7K_CnI]_84mMXY[P4cTfcW@XRfWbgMMGGN'Ybp^.$6Q^20HqON=DWS;PlQT?m9*p28]tFnDa/n)#dF(L*7kXh-H\; +bF,+P"!TK4H:g5,]Y@YVkUT3HM-R?+9)3-iPT6q@LMe6@Jdo%V8RnG/7O(gEC:*VO])iP't%W]Z]Ju1k +EHU36aDM.kA`lr+d#\83>Jr6$^:#bkEHTf8/=nIZT;/-G'7 +&,iiQ>[7oK?"QMKLAR(jAaWt/Na3=?QF2_.FBuJ92C*TLr[?lL_<#uV]p.2C<[M8\'9;]W,kRf%>87(O +.OB(-%nZ]7H@FtROpX&S,Ab=c7(%t(6V*K@NJ%(&/$uES0]o*TNYf@Tt#K=iBIl^V]#e&jde?,'$Vbk +W`KG?*+~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 173.19932] CT +[1 0 0 1 0 0] CT +N +-1284 0 M +-1284 357.5 L +319.75 357.5 L +319.75 0 L +cp +clip +GS +0 0 translate +320 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Od:pKp'L]cT>a7M&QE#'g#`*U_17>-"ES2SMhZ*WVzz!!!#7;nRX9WP8f?PfpD>U5I.#Z_E?#Za6 +B`bHnOLMT`?4U5I.#Z_E?#Za6B`bHnOLMT`?4U5I.#Z_E?#Za6B`bHnOLMT`?4U5I.#Z_E?#Za6CKH(O +[Bpj2e=>h;t?JZl!/MLO7C\"[5QqQ8tJSUN*cAZ+FCIbEtpU0i*?Xlu+BIOk8+Vr*A\g$,u??Zu:Jka. +ih/,EhThhlm9l*mBoG5A?)Q^rRUqIQS^MLO7C\"[5Q\,*f5SDuTo-h'+YH+n1m]8m'/\GUiEl*mBoG5A +?)Q^rRUqIQS^MLO7C\"[5QqQ8tJSUN*cAZ+FCIbEtpU0i*?Xlu+BIOk8+Vr*A\g$,u??Zu:Jka.ih/,E +hThhlm9l*mBoG.7>H!A,)A%f~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1283 285 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +0.941 GC +N +0 0 1284 286 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 286 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 286 re +f +GR +GS +[0.48 0 0 0.48055 0 381.51931] CT +[1 0 0 1 0 0] CT +N +0 -71 M +0 285.25 L +1603.75 285.25 L +1603.75 -71 L +cp +clip +GS +0 0 translate +1284 286 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 286 0 0] + /Height 286 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WIq[X3IJE=t$)JSLN0Z69.%u,-/3F/G6Y?rt?!;u^j#JYbg=;/"#SA<4@GH-XGSt6&U9p#*'H8". +j>?H>F5iu2Asa-"`-"qH-?VJTA2-Os,f2*RL+mcj7>JWrI,>n[l0e3(p&=0(4W]+;55OL8M0Qa!^/saj +F16(kLCG1\XNU(+S,N38^M`]fcjbXrS;Q<fLf70ubeh;$Vdpqri`*9gPn>uM\18GJ]CIE/\qEJAMMP2. +dEKJG=f5dR,iVe1E[uDZ\2",pqFFgU8AqK`3#*1Q/52RV8es`;:.6\3JG/7%B-j;c1rd_SfTTA0E5X<)hEG9u6'uSd94#8AqK`3#*1Q/52RV8es`; +:.;X]:;Pd!]L"q/:.<+#W+]dOiiH(a&/&tG?;ZnZ@sL*,Wq:O,A>(4,<)b]\0QGYm=U9koSup'JS6+$k +6q/XM:.7T-$PR0DP(f!RlbSII^EVC$K%Hk)6R"T,&Z;@tR>$L(:<>gn`:[(u9s,)![\rT&q>`TZ:PD2& +<(*76MImVNc8m6Z+VE$L(:<>gn`-$JsBaQJX8uCO@FaZf^!u2EG +U8"'0M6ft!6:f%;ECFRKDKLF\;fHnc*"qR,7Du1(BP(]4XAcD9&ol`2;3NH]eZk"YXWas@<(*76MImVN +c8m6Z+VE=S(!XtHe@e,5:J**9pP$Q^;\="!.[jM-39ue[KLB'(j#/[o#i\5CN017glQ1k.O%VV75G;bB:Y3>OGN5@ +MIpI4"pYCH.(ZO&@S!jF'bR@ZA\L7Oa#=heBFO+Q-iiH(a&/&u">EPh9 +4%n=_6jHR$E@ORuh7Ajueqo6?lB_8Aq6]8Yn`*8dIdF>[*dQh/lHNt5Gp_o=4$23\cHX5E=f5dR,i[Oa +U:Qs;m-3GOet+&4=]@eZMNW&DcD?Bjm+J_Q)`M]lI.6(1\i]ZZp&3eR5PX=nIsLn_l##1`IJ_dVDK=oP +@H+@eZ4IXgm8f@IK0*&EKg[:d&TK9mVP^6=`0uG9g5Z(0rD;W)+^%^B!Ep"BBWDk>f +KhpF9HZuM$JSlSUinCE,rVQ>=p9UAs1^0d/BmZlkNFfIs89?6p&_G2@X_ej1[r:0TX]o.[n\t>"q9,*X +Va'+JD;1M<_k%Ykm\p.u71MX.Y'he"H^Uth$[U1Fg*\.,Q\pSt;)19t]u&AlVr;jIqhVF_^Y[SopVS*P +YgK#2Ko*-2a[/P:[6mB*?Pi+c_4@DC6DDr.Lc?.*rqK)g-Vp==[&*J2=qr^(o[;0#OMCIjDsc;gYL/o1 +Ko*-2a[/P:[Ci6FQcVjpEGLVmeS9A#F9**gVkT&S]6100%smB3g%l'E@N/-72;#MPA+\nG3i>A>eYIQikW>pFYl:3rqsjYaJDOr.Q8Gr +m'4_1L!uX"Za7Qe_tW&#g^:7<]WI,N;ZjS;Oc^\6As:%$R`9Up5t(.'4J,o)E#O1S(dQZ5&1:.<+#W7Ji&_M&@6 +W`/DmHT(m\V$aO_ERlRA#`6(aY:.Z.9*\m$#Br5,,N;ZjS;N29(+.< +o]p0BV^(2#7%B-lAel`=3[G.W&Q_dtF^GsP%DlDpl%h;6&Z;@tR>$L(:<>gn`+?-NmMgi.QFstdG2m[4 +S;N,$;:3V)`*'!K+XH(<<`Jf&_PAZfOYE\,X_DiPJV]t,"<&Cp8ERlRA#`6&k$L(:<>gn`7;!2-)4TpV^(2# +7%B-lAel`=3[G.W&\#-A;Q8_L=fdq<&TK9E:.7T-$PR0DP(f!R>?'"u.qPR)\5gR=#B;dK:.<+#W+]dO +iiH(a&/'!5MHhp*#cAp,<(*76C+Utf9M\Gk]68&E4j2sX)fNCZ]ANQ>IF3FPBLf4n*.]Q&"D&QSRS2a@uQS2XUPU>1JOcd5%:/=Z`4ibR^DVi&%hL"^V8u;Odr[(DeRtZ^*:<>gn +`9lHsh]PU_dh-RL,#1QA4*Ks-*ZhK@4E]pgRUKi@AsVl\kbF4,hS$OIoBj$EhR[jqq=Ebgh6U7O39ue[ +KLF28cVAN&B\(2fLc?+]VkW8'X]oLKSX>eCDOI,h:Kri$5JCgWRl>8_m:X_7=o"VXKo*-2a[/Ndq!97] +E]PC>7Du/R3HsmrRTRn-4^5O$l:#9$#gFQ#.egX`- +M)FnX8`\uT7%B-j;c,;-23fN>7@1gTn*`J[i4u'Mc^["Zp)FthHMHHS-RL13I.GCdgHuK7eDtAm)ceCN +\i:%h[!VQ8+NUb,MD;q4$l+$[JNbEg;B4*U,*I/.fH +]/9s0B:Y3>OBC)>+!Rd1Z4IXg.O%VV[2'Mn/mPp5r:>Ljb2r2`Dn\4n41jjNAms]#;mHrIbq6V>o]YN& +U4$!/(.'3?7Du1(BP(]4XAcD9&rkE>m%j1AnYLT4Lb+3IS;N,$;:3V)`*'!K+XH>VeV=F2Lb+3IS;N,$ +;:3V)`*'!K+XEARq]Wh9#3";d+ql+3V]t,"<&Cp8ERlRA#`1tgI>Dj++&NVQ#`1MU&Z;@tR>$L(:<>gn +e>bS90DeZt+q#P+V]t,"<&Cp8ERlRA#`5SOnaYb6S^`IQa?iG9WJEMK(!S:'S97F>6D@q2FEDAPn8/o& +M3*Nl&TK9MRTZ";l]`rR6rGM"((J,pbK><>7:[Pei`&=9MMc!]-I+Xu$R)eIZ]djK7osgD.O#6D&Z6i4 +<)b]\0QGYm=U;!nebp'OLeZP%hTj)+4'gg<(%Kg,#1SFWMjKB@H+@eZ4KnMX#gSW5!,_U'p433Lf4n*.OpliQoODr +/;2NTdNEqh&k*5G[oGgC66]/n`2T\KAms]#;mHs4=/u-@_?CCc[oGgC66]/n`2T\KAms]#;mHs4pSg4[ +(]#(N6l&*5:.<+#W+]dOiiH(a&5ni4+!30"6\Lq*&QoBr3#*%'U8"'0M6ft!6:fpCFEDX'#N=De+ql+3 +V]t,"<&Cp8ERlRA#cVU(8u4t/E8%T]^KO':+XH&H7%B-lAel`=3[G.WFbcfcSMWAg\o,8g7:[Pei`&=9 +MMc!]-I+Xu$R'Z(\fW+Y5!H]Pa.OW^&/&t4,#1Qq1CFk/SW\\g3a1?*\@Bj[C.(L$+XH&H7%B-L/M&GH +:JY?0G&;s1ccs_Jp=a<,pG!$hB:Y3>O?1uSIu&%W.O#6D&Z6i4<)e#$aH=3N^(#;Qfs<'N\]aunhS"7p +D;*Ke+1!Y@egX`-WC+)>+FIB^A0N;Y<(*76.OHMje'NF#`//NFI-K(UJUrB7OcecjP'0Sb@MWPr=Jo[Q +Ue\;h6\No2gqr)K&QoBr3#*%'U5BsQFpnll=)XZkFXkY?1G^hHp4)D1\,-"!LPCQ-_Q3k*PUqL;EE`#Y +_G-tb/;-E]MMP2.EpM;Y1pKeL9N8LFcd/L@a,_=G]77@rIcQV]*e4+X["DMKF^Uc'Koe>g$U7%h#o/SV +OGHgIE@OT+SNE/bf!B#8eo'9UH1Sk)#OCf-kg6$,IeBa#DVQoRZTbHhZ4Kn=C$gZ]2k_G$&/&t4,#1SK +eC7Hd=OTZC]4p8HJO?182fNA!Z@-KJaXZiof +:ISe;,iVck>VB0cL0=<6c'o5+nA(*7r8YpocO7'CNr"6+j<18l2`!72f+P3sA9;*,+q#P+V]t,"PY=kK +YOFEt:H\0cQbN*g[-,pZhu2teqtg0;(N6tV7\`%($PR0DP(f"O>[)W";BmY8$R$+[,2uRDqr92Ja/Bl^j +qWXn:D;(5g8D>*>ZWWYTl]`rRe@+*#Z]c;)&QoBr3#*%'U5I(7aY^@G4*I]1/ml89EF<1I3Z=VTIstp= +jO0kt97P+]KioMK.9QqU4Y=?XFP@;B[oGgC66]/n`2T^#nDV95I=7C44:_l1c-=L5[\#O!,&Na:RTfP4 +Ams8GF\3E9C,[q)1N!$B8WB[)/;-E]MMP2.XbDI_$PtEpVr0,M_SfTTA0IbW;KiZn+!5FoE@jOHA0E5X +<(*76MImVNc8m6Z+gmB-Q],gkcVA4C7:[Pei`&=9MMc!]-I+Xu$R)p7Nqht:7T;bk66]/n`2T\KAms]# +;mHs44'6gNkVNRSqOgkT7:[Pei`&=9MMc!]-I+Xu$R)o_B3d:(/esM&OGHgIE@NH,=dV27PUqL;6?TJr +<@X8;I9`hM,-hamE@N/-77B!?'5*l!KofIa%DkcAPh#c"N35fBA0E5X<(*76MImVNc8m6Z+gj`$?8B)E +'p433Lf4n*.OpliQoODr/;1D0l$L(:<>gnl_\I#PdQa5a?iG9WJEMK(!S:'S97F>66HZGIhB9IeMTA+T`2SPP'c"tE9Up5t(.-0BGMXITLl08?8AqK`3#*1Q/52RV8esb9 +V=#]:GO$3$(JtcI#`1MU&Z;@tR>$L(:<>gn=cAE&pXC.8(JtcI#`1MU&Z;@tR>$L(:<>gnED'XTh=Y[t +?:74m+XH&H7%B-lAel`=3[G.W7J,pcKGRJt=U9jD'bR@N>jfWp)G%l9LEf`]/9s0B:Y3>OIE-FFE$7(i'gU5 +(.'3?7Du1(@WHKE%BV!QS1'`IQS)P^(`4)-)6(72rr)`'Ct\C-9/!,)B4kjMV:,>:huE,Vml%aiZS6CM +.9QqUOh-n$lLF1/2khM%&/&t4,#1QA4*Ks-*ZhK@4E]pgRUMOH$C[TZhmd[dOX!Ar>HV8DQM,^kWM^Zj +ZWWYTl]`rR<0u3*YDS4Y6:f%oLc?+q[r1"o'C3TT7!2.kH./)>a,_n-F)Gm`o]ai2Za816$O[>VcR5?4 +ZI,54Ko*-2a[/OVbfn;dGMdg$PI2ki+q#P+V]t,"PajgZ/krE[c^q8p\9[].K*Mo>qm0'_rH.nh='pB` +d10bsiiH(a&5oXA:BqC?%^u5Dg7=,B6:f%oLc?+q;c?V/@SIMUmDf/)rW)SNJe@`'[\ +8u87\82E$jr[S$ka#9S>teVD]V<(%Kg,#1SFollX*p!"$ST/sC"&rQTqUV$n85Kc3EW1idGlB-qU?dW +a]arNYD?G:7%B-j;pjunYEX2#jN0hpC9OL7ld2=ml+[ +.O%VV't\YVg:MQIok'uF<(`/m5G#Shqlrc$L(:.E95g$Sdo(rhQ2P"KLD>fLf70ijQ'hi +>)s[c<\B^l?G&6O0MidLc_!Ed1,A_29Xm +OGHgIE@RtLI/%Yf?XMo(c9jrdF)uDfm3bZ!86_sgVUF'D:S+Y7n`M7f/2dkLB:Y2S:PG%1h5-_am-3H< +SuRV+#`1MU&Z;@,$O[=Y/mP>M_-*.ZM6ft!U.7P=R@3SeE,g3",)R]mi`&=9MMc!]-I+XuMd?[gmM,$P +i/DuU,1_=,Lf4n*.OpliQoODr/@mp=7N\&ZZ\.l7mYY+*66]/n`2T\KAms]#;mPW(()?W@KK.r<\u2.#5Y/hmLc?+a +bq)PYF\3Di*E&(MfScKU;b'nWq[aB[.O%VV75G;bB:Y2SkD%Js$ML4XFk4)9#(RXu2OE(b:^7C43]&Z6i4<)b]\0QGYm=`d\8VVq0t=V]>mKLD>fLf70ubeh;$Ve"1(+Z5[>#6tKH +AnG4pJ3C#5.\b/oM)Z4^VkToRKo*-2aeI$IC3"GJ;fMVIo8;`G66]/n`2T\KAms]#;mL)kOIAt]/ed&A +g9r1dMi``n&TK9MRTZ";l]`sS0I0S9a(`GKkX7`]:.<+#W+]dOiiH(aBWI6C'O3,Q9YhaIXX^&d$&LVV +&Z;@tR>$L(:kB'*-66]/n +`2T\KAms]#;mL(.8BX1/SA++VJiee;&QoBr3#*%'U8"'0M6ft!@\#kip5*0UE$oZe+q#P+V]t,"<&Cp8 +ERlRAXA;C1Nke5c<:Y(n%cqe#(.'3?7Du1(BP(]4XAcDQ1o+\i$gif/mbi1T(.'3?7Du1(BP(]4XAcDQ +,,CJd$[gQslXGj7O(\L[X?WU-Z4IXg.O%VV75G;bB:Y2SY)k.)mn-&W:BYk`V^&JY&ofBF:.7T-$PR0D +P!q(#ap:*h34Qg;fK4\sP1("G#`1MU&Z;@tR>$L(:G0jq:>o-CRgF+6:f%oLc?+abq)PYF\3El6`LX2V,D#]_f',c9uF:2!r!h8,I.jnE@N/-7;K66 +YV84'II5arAbS"h'5*l!71-"Tq6'"NhaL@I +nu.MM[79I2eJS3dAOI!Q\]on:o.S;XY^n?Y7%B-j;phW#\ofMt^(V.neLgCao4cDt[20e^nc.Soh?`nk8+rr'g.-L#Ol +2NWWSLf4n*.U&+:WMq]^Vhqp,@t0".JHH,qrCEt2$No5OZZi5:d^F#I*kW^l^1l^M)]LlM`9uHnQYqIV +;%Zq:M)Z4^f?4o2cRf!@^\<)-&e&..$Ec&j'5*l!Koh*h&QoBr +3#*%'U8"'0M6ft!6:f%oLb+3IS;N,$;:3V)`*'!K+XH&H6l&*5:.<+#W+]dOi_$%FWt0c!+=,rG6l&*5 +:.<+#W+]dOi`&;]66]/nA0E5X<(*76MWQQcS;PBS8AqK`/;-E]MMP2.dJS]T;\<.&_^@RnT)`LeI)B9_j/%!h37 +;(AeGOMD'Rp(4SFJR%#a_fe@CB=iN7:OI7%FsjS +RO_BAl\+sj<8_j4*Ks-*ZhK@4E]pgqO>gTbd?eokV2W"o,BW+4EBLRF\@&5R4:f>,[,FlFKW+0H7KFX6 +UO4Q@FjAa7j$R#H[#/FgMaj?FmIWMkKgX,OQ]*s'U0YL_A_@,Vc26TP]=m86":O_kuo^CDL%mR^?M +Xcdt'l$^1LYUS=\ofP%!@K3@Wa!mn3Ggd*r-/U1MU!t92g>,jl.ObClB[-XKiHH`:HmIKUS:,@bN258H +0"M9+&?MmZS8G&oB+;>ni:dR9WJ\WNq_19O/6npp#\'V4Hn3nrKe#ZqS3(-^TC_Lrl`(S9DYB+MXcdtQ +"s?]LYURR4*N=U!RI?#>J"nHk04,A*c0@Md06+jqWZM[X097kl3!aO*'/3*X'bhjN>jh#_Q:R(YIp#n5 +-Ma#H4"BLcCJ>Ans@D:`k3OW2+S7>qYL&;n:Mh\ns@*b%mII>g\1#QJ%O5970c?sm`h4[Dr2FFC8VmZ\ +QQI4'eSMjGL4jh<;H\L\unSRJ,b'%#?l7@FLhhr/u405"bnnEGk#6SO@`fKKis$aB?m<+L5(F(Ie_lYM +2FFJ]O4Pqrd]f45ZjO?8BCoesKkK.L>RiIs,$$k3YKQ)qe8:H\0D07>:?Ni)7>mWZG#f@QA'GiOcB>(((+N +0djsWH.C`g5R.jZ<1XZe`pqmjg'qJp`']U>qCX?`1p5hA&b1lVcSijPsMuSLVC)o?W. +$aE!j,^\G5j21b_o!GY0u^A;=0>e"btLl-eo'9SFfUC%Q-0,+=nEEb\%_O;IJ``GlB(3hMl%=%\QOQrZ +`o5?H0s=nC[^,neXJ2\c5IZeIQV1?H]<5OXbDI_q&3$nrUcs]S'i*eH`I[::,4dg=Sq6[X&lL9;CeeJq +4B._4S)6hbKZM8dhuk9Coi=\7;#aa`H]JKC+ZR/6gj:j+8gQRfWfJ-I.?48JUrC+FUH<\SXiK9&@QN7r +k^$_Y(-Xjj8Hirc*P_[S,Q8HFQh*Lr;=1q^AH/nIP^r7G5_:1J&/B+r,bEg;-B]m[3Q +S2]`ob#SsDVVaAi4so')`Lg$,Q6>3YI45"kTk:Xhc4T0>Cm1%G25cd0n^0&>LOeZ7"ZpK[R_s8:4C/A8 +S(55OKq:9J5o:&0-fChkd`'#k2ZGN0Q`l@ +G\qsMUFr/8>bMtc+YIr2q7!!!"^YCHN:Bu'Z$^\unDiMN)cF*%BZci8C=4nj?oMdNQG_a!t=:XVH,oj@ +`!pZC3-c/8HDk%=X^%^C\cpJ=4IPh**!57`s5Q,:71Xg0PB_sH3nV^ +1kl;MD%XkXm6)>Wp&5@+6UU75r)nlb``ro?khp9h)(DuT(;rbFRWs8MKe%NYTM!!'^s:]L&_^\Gc$f)8 +XK<(c3?s8MKeIf8S>s2#SdOM:_:=2+7D'2L0jRJ-RGDh%ZIJ,\VF3_gXkqtm"4m^_VRUq&?WZ>'2K54b +2UHhXCaX)&ju*PM4(Hde%O4.2P+)QDogrpK7'Sf`@L6(31^*^*]?cOW8<0PrglkYQh[(B=F8\l?4FY'o +kbOYkL`On8D3TDsuPplFgs\)2W.hra@^>? +VSJ(d(@0p.4Y1`*&]2LT@eH?sOLn=G!p8U'aN47Cs/c[GQDQhF*;S*jg-EjGDkdUYKt1fXB[#QOjTDY* +l)n#%o3TDn)Il@-2tK`:VA5J>nF2:=.lpgVr/%DnC"BtbW0*_s%\RZW+"pO@\]hS%I3\oQn)m5)C*I#W +6ZkO_^;?tqWt+hH&,%fcS09t^)C4))q:hY4`cpU_rGKg5@jK0T:O5Q(HB5JM-o^,3eTrG5Ps.$jfY0ITE"_Z%mKbrjYMtGp7\1RpYUIM@U^P;9H- +4p8UpD7IoZsJ,BKA.ogNq>g`IJ!I/cOU +NsVpTY5Siu?(oYl_A^=_XNf?V6-Y+8S';V&*_a/V@+fi!/S/H>bN=Lk((r9'?mFj]o4s7_"=g]-4Srh4 +A=aXmA4/$F?^.po9)'(P?N6hHZr>@qM\5P;!OrSp-A@s.\AldsUWX*qa%dbis^Q[egteXJa$#ljsUTWt +c,1g9n3TDdI7cTe$+gXE&rVbsq/V=LGOmA@5Z>Aq3?^.u$"47Bg\/sjnS3HNM0p\b!Is*aapnF1B8LXY +bid75:Td5^NWDZ9@=^THA7h07cTgiLfTrV#!jO0J1im2u;j8*U#&orDHUHgeYUUG2`K!3AJTJ%'7IEnA +@[Ykc!$I4l&$eK(*n7f$a^qq3(knK1h]2GLf1p?\aed:ia?[r5X8IDj5D^@6jAjectRq,HA"II;/jHgP +mpNBHaL'@m!5^4#nUn%YLX^Dm4RJDU"YLEHNnk + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 381.51931] CT +[1 0 0 1 0 0] CT +N +-1284 -71 M +-1284 285.25 L +319.75 285.25 L +319.75 -71 L +cp +clip +GS +0 0 translate +320 286 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 286 0 0] + /Height 286 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W9k[en)Sg>9PV,@]i'L\)\-Q'G.4K_.7AU_8(CI"u#7N@Y&Yup95nk$/],%5R0d%2s6!B22cp]D7 +;DeRp?+?!M+9ZCO5u^1u@Vs27Bt-OSSBRPoREUjgI_XFWSE]:-2pZtc:?]a8!e&O62RAac\i2Z2R_=@` +WmPKj07e7A^KL2%S:7g.QT4qUd4]M9c(\S)oUA:a`9B4=IYlNlV:ui[m-jX/VVb"#C1(k#KRPEhWe%G* +&c/V?W!:7B8*IFFTc+bP*MM@_L2(+4FbG2o)6/7meIhY[A>f^P<_)A_dY8dBWMukbR@.ra3B]P(j,ZFE +J&`#1eU=.K8fMrPm^V;;rVQ>5o?=i7]'K1TT9'#4)g8:Z6GJdDlqlqjIF2u0%6IPc?!m5.Bt/r`qXs/m +]^a#?pYS1j19tWCVFM\QjQtR+MhdVl]6E`#aiVYk?[d:l/`lDs9:&g0=mWVfVq`]`)&jOllDq,fn+k]I +pK"IUrDmYTW\;*]U:rg>nA,@Q^6Vs)G3>E?*^+iAbaC77:7TtJFmJG(hgXtH[=s,(>Je&Mm^o*=o#[l1 +W=n-A%mG6qIJ9!b>=V>X4*U*DB/k)$=K/]uoC'HomX5"O[[QXK[o3*"^%Kr\KDE\MI90I`oF%(ZTAR>/ +pNuABf6ZN=ackh#Vb`pkB^>ELHgbeT4$,N.Si/;Xd5kI">n"/aP*1+Ro67OkFAKZ:g4sEVqH`COeZ5TC +SU07K(Wt4LNJ4fiQX'uc&'<-]qeYS3H(Spo3d:!slQdLrhp#3N4oJDr+0&c7\(PS`cb8G_ZlIo-A]nSE +4a,QerP&=2qb)FtIr8s@?2F/noB4H/:=A,!h/"t)dr2)VTdj-+fb^so;RpgTqLr1cFK%Yh5!D.j3VhEJ +^Qh;]=Y'1Bat'YHrO!/!o[(=#qcZK?qsAe1DRQ#%l*o(P&(Rppa):e;e``\oLK<]L]QLZSG3-l3Ocbds +jOV`$IQO1:h%4Mt!9go?&.d=@e5Z8/%NIG^J'1>(I8iDiQ:F]A4LrhiDgD8gf3Z(?5VPg=nqfff^P<_)A_ +;W%Ke!?G^#\(<^.EZ,_M).S'lo]9b +.#XWDc4jipo>I&k/9[UE;j>8H'5*3.fQlIM;&25hS12TikD:u`=mS7jVC7@n-a'?rlLeM=#?KUE]f5ul +zzYeaVXGk'BmJ#tgi2`LKu\).[?4fh/=;`$^'J)S7u++F!YgdkSud(=]$o%;UIR58J*O1t879A?ml)t9 +"mG:KM(EcU`u7iu%7lO;h6j'R92A_-F,e]c62g"iV0S[[B+(LQ9Fod?AZEqo +._E\4_Dmp854m^r0g5.c"*HM?6>([pXUDCh9O;lmkQL4T3n%A8]_[iA&HhQ +5HDnl8E!tRLjDHs\j+a9KYA]pt7o?TWSGDo&WkqJ+%8L=rl'J+:Jo/,qUb4&6+)VWaqMIO] +Oh-RY6EB?-#QVPrsLmJP4u2fC8;iJ/M@cd1]BGs$;AXC^c`7G54mkCE^?>'I/V]iq"(0#o_J2pK]d*grC_-nkRd0401r\`^uQa0/d\QFP#0UgN+2Vl1$Mf\"j9p",cuHY-l%ZBban$hNu](GB ++-Eofpe63$uczzzzzz!!$Dtq(EY1K?s~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 347.4] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 356.25 L +1603.75 356.25 L +1603.75 0 L +cp +clip +GS +0 0 translate +1284 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W:InH%)o,lJ+sOlmX]gA\^(D(lKh+a*[$-OV$3c=hN0X-r#.SFQ)1bhUDaJ,TQNzzzzzzzzzzzz!"*rDm+IhrR\?FK$J1 +n3G:m)a3&k%(]`J06JDurM!<<,PB["hY/c#6[gU;6JG*_#n4psT"n=[J.4ho +\(g=kmO5C`[FXSi2#!'h,c7Rf:ShOKS7f!+h7In"rr) +^g9hj<,oB4Gdf&pi6lK[[#l`X[1IeW9d!s]I*dE=o/@&!.Z8`4F(uHqk*WLfop:No]];'FoErId*O +I15+_drH@#GOdA,C7QS2_.>OjWV%j%7.R)lc7q!hkGY$K`#dS%KClk]X%!/`Ha0[SS/4(i%er'#K$XSc +i//DS_';nfu\^H^Itl-cF+l@Ng*g=j]a:ofcX!WW3cEFrfpL51PjOnR(k4(lj/WN3'MT3LL)_B2!k/H> +C"M2?%YCMR^72.,tIl-o2\(G8t62ElEWV"4Hc!!#7r^4-%7dEthf!it&/g;_.PqYL'-BAptSampCmY%A +XImbO@>?1<=nc'a89fe3=*!#4D1M%eS;H2RI-^:q>?iSj%cl(>_FhaM/9dKb>E$be7+paf[lIK9EK+M# +nIdIPn19Ci+L[)afrkg?0roa?4TG`c.=_YjN9!'kQX?G+?kc_dS:nr:tf5L85r4>8INDj`ft+$4YI[!l +V!-N>.OOcbcRbEB2NL&2#R!.a.90fK&fmcDWY#A2f%EF\=cXZLapbVhY6r&A%AIhp>ehV@#Gs7#!bk2t +gmC.Ccal"69PKe(q3!!!!5iUuNgq1&(^AKtqs?XG^NAs]]-]pc-m57?A_ICl2H+odfZ>IN>"H*dE,/,? +hs-2RiG!.a'fNfI_Hle'kBOt4`=gh`S,#3>\tYDV0i!!!##Q3CXe"TSOQFj)BF!!'f(@ho7K!+;Q>*?? +.=)B'\>?gToVzzz;?$Ur!5P%#[9:"#4(94/&P37b3#.R/OGHgIE@NH,,>=Vb84#kcWJDgQ7%B-j;c1p^ +@H(6S&jdd;73j$%i`&=9MMgO%';m%;&/&t4+pfD)V]t,"<&@+43#.R/OGHgI8I1/4'bR@8I1/4'p402 +Lf4n*.Opl39Upu26:f%oLa7XAS;N,$;:-@li`&;Y66]/n,U"Gm<(*76MWP0XV]q98&ofBF#pBnY,2uR$ +1e3om'p402Lf4mW#`1MU&Z;@t3Pr_6La7XAS;Kj(8AqK`3#*1Q&LL7A,U"Gm<(%?c,#1SFWMjKj0QHLI +mAkXO=:^'QS;Kj(8AqK`3#*1Q/50;mGP>k0"kEc!&Z6h9&/&t4,#1Qq1CFlZ4=(:W&Z6h9&/&t4,#1Qq +1CFlZ4=(:W&Z6h9&/&t4,#1Q=Dr//&o&\'SCWsZ63DW458AqK`,_SRUMMP3YMLG9hiUGgDK7ef70/EtL +k<($s+VCUrOq87G.O%VV.l0(Af%f&Nc+.FpWk`mP&jdd;73j$%i`&=9MO&$mC9">8C]FD4Vb`pC$WA(k +;j%\)E@N.=+VCUri`*9ihnFM#Eo]be:JZ/M/511G:J!lD`2RDpGl,[6BDjomo69T:CK[P$g!Ohi"m/Vn +E>uH_Ps%B+9hg'9rr)$H:i$oE3^!3`#pBnYU33+ckE[IMI$h%>YHRc.`IA61h^/%o;Y_lV/D9&\2uKh\ +X_kK7!pbE!Lf;^YjjpP<=0>g(c`HIn`qJfS(Op2,j8o>4LT8V9_&YhN(Lfp$8AqK@/URBgD>mFO92!HC +5KNt79aa/ER"_LX:Hh?:8AqK@mE"2MSA31VT]*BZQdY'?b!.-+\WD:kPPof]/)a9'7,187./4Wi(8bh/ +q'67i&Z;ARQ#QLM%Cr=72LB2'Ip#g\Q_OKk+VCUri`*9oAmq^ZGtJ]9,#1SW=.4V?4S+jChL+%Z_Pk'l ++VCUri`*9oAmq^ZGtJ]9,#1SWZ/YRYJ(O0OLQIo2Z2b+~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 347.4] CT +[1 0 0 1 0 0] CT +N +-1284 0 M +-1284 356.25 L +319.75 356.25 L +319.75 0 L +cp +clip +GS +0 0 translate +320 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0R5n1^s&-ThB$'%82>(-XT'+1YB1g[^"_.g:DLX-V6zzzzzzz!!!"L-[GkRe,CgZPi^#tU)(q9F%% +[H-g-@j/5OP+[90#j.r)=O3EO^>H#g'd`U`(p;2aPX\Y9k_PB0/EQ7(jQ=tEIp't%/8S1A?Z4?6Km@X3 +!sW+E*]IKJQo\)fT0p(J9=^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV5\ZLj&kLq[&4jeaEmU'E, +dQqLuJgqYbH*[9!/2N9C4^MuV,ao\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.\(0pX4Q?4u~> + +%AXGEndBitmap +GR +GR +%%Trailer +%%Pages: 1 +%%EOF diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_2.eps b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_2.eps new file mode 100644 index 0000000000..772e51f015 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_2.eps @@ -0,0 +1,1548 @@ +%!PS-Adobe-3.0 EPSF-3.0 +%%Creator: (MATLAB, The Mathworks, Inc. Version 9.2.0.556344 \(R2017a\). Operating System: Windows 7) +%%Title: I:/Entw-Technologien/Studenten/temp_kai/blend_case_2.eps +%%CreationDate: 2017-10-06T19:21:34 +%%Pages: (atend) +%%BoundingBox: 0 0 774 521 +%%LanguageLevel: 3 +%%EndComments +%%BeginProlog +%%BeginResource: procset (Apache XML Graphics Std ProcSet) 1.2 0 +%%Version: 1.2 0 +%%Copyright: (Copyright 2001-2003,2010 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/bd{bind def}bind def +/ld{load def}bd +/GR/grestore ld +/M/moveto ld +/LJ/setlinejoin ld +/C/curveto ld +/f/fill ld +/LW/setlinewidth ld +/GC/setgray ld +/t/show ld +/N/newpath ld +/CT/concat ld +/cp/closepath ld +/S/stroke ld +/L/lineto ld +/CC/setcmykcolor ld +/A/ashow ld +/GS/gsave ld +/RC/setrgbcolor ld +/RM/rmoveto ld +/ML/setmiterlimit ld +/re {4 2 roll M +1 index 0 rlineto +0 exch rlineto +neg 0 rlineto +cp } bd +/_ctm matrix def +/_tm matrix def +/BT { _ctm currentmatrix pop matrix _tm copy pop 0 0 moveto } bd +/ET { _ctm setmatrix } bd +/iTm { _ctm setmatrix _tm concat } bd +/Tm { _tm astore pop iTm 0 0 moveto } bd +/ux 0.0 def +/uy 0.0 def +/F { + /Tp exch def + /Tf exch def + Tf findfont Tp scalefont setfont + /cf Tf def /cs Tp def +} bd +/ULS {currentpoint /uy exch def /ux exch def} bd +/ULE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add moveto Tcx uy To add lineto + Tt setlinewidth stroke + grestore +} bd +/OLE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs add moveto Tcx uy To add cs add lineto + Tt setlinewidth stroke + grestore +} bd +/SOE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs 10 mul 26 idiv add moveto Tcx uy To add cs 10 mul 26 idiv add lineto + Tt setlinewidth stroke + grestore +} bd +/QT { +/Y22 exch store +/X22 exch store +/Y21 exch store +/X21 exch store +currentpoint +/Y21 load 2 mul add 3 div exch +/X21 load 2 mul add 3 div exch +/X21 load 2 mul /X22 load add 3 div +/Y21 load 2 mul /Y22 load add 3 div +/X22 load /Y22 load curveto +} bd +/SSPD { +dup length /d exch dict def +{ +/v exch def +/k exch def +currentpagedevice k known { +/cpdv currentpagedevice k get def +v cpdv ne { +/upd false def +/nullv v type /nulltype eq def +/nullcpdv cpdv type /nulltype eq def +nullv nullcpdv or +{ +/upd true def +} { +/sametype v type cpdv type eq def +sametype { +v type /arraytype eq { +/vlen v length def +/cpdvlen cpdv length def +vlen cpdvlen eq { +0 1 vlen 1 sub { +/i exch def +/obj v i get def +/cpdobj cpdv i get def +obj cpdobj ne { +/upd true def +exit +} if +} for +} { +/upd true def +} ifelse +} { +v type /dicttype eq { +v { +/dv exch def +/dk exch def +/cpddv cpdv dk get def +dv cpddv ne { +/upd true def +exit +} if +} forall +} { +/upd true def +} ifelse +} ifelse +} if +} ifelse +upd true eq { +d k v put +} if +} if +} if +} forall +d length 0 gt { +d setpagedevice +} if +} bd +/RE { % /NewFontName [NewEncodingArray] /FontName RE - + findfont dup length dict begin + { + 1 index /FID ne + {def} {pop pop} ifelse + } forall + /Encoding exch def + /FontName 1 index def + currentdict definefont pop + end +} bind def +%%EndResource +%%BeginResource: procset (Apache XML Graphics EPS ProcSet) 1.0 0 +%%Version: 1.0 0 +%%Copyright: (Copyright 2002-2003 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/BeginEPSF { %def +/b4_Inc_state save def % Save state for cleanup +/dict_count countdictstack def % Count objects on dict stack +/op_count count 1 sub def % Count objects on operand stack +userdict begin % Push userdict on dict stack +/showpage { } def % Redefine showpage, { } = null proc +0 setgray 0 setlinecap % Prepare graphics state +1 setlinewidth 0 setlinejoin +10 setmiterlimit [ ] 0 setdash newpath +/languagelevel where % If level not equal to 1 then +{pop languagelevel % set strokeadjust and +1 ne % overprint to their defaults. +{false setstrokeadjust false setoverprint +} if +} if +} bd +/EndEPSF { %def +count op_count sub {pop} repeat % Clean up stacks +countdictstack dict_count sub {end} repeat +b4_Inc_state restore +} bd +%%EndResource +%%EndProlog +%%Page: 1 1 +%%PageBoundingBox: 0 0 774 521 +%%BeginPageSetup +[1 0 0 -1 0 521] CT +%%EndPageSetup +GS +[0.6 0 0 0.60069 0 0.20001] CT +1 GC +N +0 0 1290 867 re +f +GR +GS +[0.6 0 0 0.60069 0 0.20001] CT +N +0 0 M +0 867 L +1290 867 L +1290 0 L +cp +clip +1 GC +N +0 0 1290 868 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1283 286 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +0.941 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.48 0 0 0.48055 0 33.71863] CT +[1 0 0 1 0 0] CT +N +0 -69.75 M +0 286.5 L +1603.75 286.5 L +1603.75 -69.75 L +cp +clip +GS +0 0 translate +1284 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WnVbokS]C9tCIARD']Ju+5d/rRV'JSjlZTSZbg+GurL*J1B-%T]6+=]!kE1[E8DcJlU^2^2Q*,KTO9SLn'?+Z\?L]V_$cZ27FCMPR2.6s.@&T((P4jT:Mlb +N[Q%rE[i92U=O-hEm?,?S+LRB"+/+J#pGF0&NLT6AdafD:-$hR$#C!qA+i^I7XhXJ +N[Q%rE[i92U=O-hEm?+4/A-S&O&>h>b7Q%R6r$23<7I?g9[5Y0//*7c2L?.M*)T$njA\NB7BP7[jIB/g +)YXY9j!X=;+VgJ/L]@)J`7e'#-%nZ]7H@FtROq4Y3G$]neMUqB+!@@+VJbAc>Jr"cKTO9SLn'?+Z\?L] +V]>CNaI+*A/#9U#%,j&_`@@U8ON=DWS;LLVKul?Q)lr\SN.nVYL]8(_OK"$K66`KoI-10kruMF\;*mh7<('JM$[D[HkDt#BLECt)l()["EmEo1RH9R;NTQ@bqV[IMqrg0X;*mh7<('JMoD8%$ +0=uLVq=o_u9Q3TkMJB%XjCbGZ:%ouFEE!VC^QX,%BT,,ZW*.[ntK)`P?R4(\=2?ji0>:5A/> +f@X6G"PtN%5&0`-iQ\=i;eaKTO9SLn'@b +p%+gUr9W+C5Q0hBO$7^Sgq!=Vmn\6/,P/MShpl;HlC5TJVLF$`4'$J3L"(V$^J2B+>PEVH6jY$!;\?Q7 +Y%Y'lr5&gp*'\n"CMW7eX]sD[?0hK7qXj#*^A)0[(1-?7"T&t[Q.BPr/`7@k(\h-")Nq(LX0TUsV'8M; +M4BIEWG#TsDVVaKp[8#&5Q16'DXG12em"?ph\IR%SG(+tB0>JLK3O$d5Z8bA^QMfs,"`,tS;PCe./1V$ +rp\pdMj+-A>F:q%\oQ-"OJ2$u;*l2$WBY[BjP-CQ8dL9DMTDc1/e9$f]RB\NJL?TJ[d^+dQR;YU[P0`) +FB,2#ld^M4n3/!)h3@)2>M:K+&LE)u:.=5C1mbmG6smKFAcF=m*]uE.o]XlWetdf6(+/470%9I'EF9.b +MicN*iFt3I,"`,tS;PCeBO2K.?9E5e`6Fc1]AMMXGOOnOZS/u\_P9V^-G@/G9[5Y(K%VL?;SN66:.X\] +F:q%gjI-kj,Fe54h!T@<0Y-X]\s3G]!0C-m>^nn`dK-.)1Wf+XD(X,,_0dCghA0Bo,u!!dH:En3/"t +^%#=:F;1d3&jde?,'#/Jbn_YhS1\XO(BRpprB!VB)e/^V/m?1CU4eWLWJI(%>-iq)3N$qdnnkWT^(=0c ++`3_Rl)TUQ66`"*@7Kj`'e[<.2>]H8E<`<:.;;<_W3WESBg[<^aAQ`2D?Cg1"Y;< +WJD0R,,ZW*.Vd3D-FY/]FI&6n[XSr0&eYSAAJ(u!&jde?,'#/Jbn_Y8hWK"#(BWIJ_Q1DU_m\DF;.ZnK +'>n9aNl\2US;LLVKupIS3ShGGIHW`Qn)Ln'?+Z\?L1qOAW'E(fr)QY+SfZ!)q\"W#>(jbTFaaJl*q>F=4(cKSd7pH)THV&-dkq"IgD#a30ImRVSVH#JG +YJulQ)]8XB,B>7XVkVaY@gmaTrLW;*/^0lu +i0.BHU;O/iC`&K`E[i92U=O-hE\5T$$kq9Ae90f':G/WfeV8K)8*!W;:.;;<_W2'Mkq"IglqZfVEks,) +;UP8TNX)4n&sjh`.O(dbenec8iAmIj-%J_J:#LV!Y4Sp[->[+KGQYGqO])iP't%W]P?[YaoF?-Q@/G/s +\DWQ$YA!hF[YWP+b7Q`LO],+m[P58FcfrZW"`+6Y+r>WI%4h#/D4AK*Ln'@DWMh[YR,"CuFQNnBSD-\4 +\_rZU7R\q<)o1QkS;PCeW&7eHgt6[/r?q!fH/r*\puI`BOc.psF#bGB,'#/Jbn_Y8Inln(jct1N*6JGW +gp/H!qQ'op]d8n`3?k@FE[khK(+)!=R@d!Y0MLGW:Ms_Y9mm5]e_DDO/k=u%6r$1Qfto:PLAgFe+S0Th +-b&)mVS4V:.W6/FE]G>A,'#/JbnanDZb-FF4+8a#"eT$A.WSs&"+:H3Ti2O`'t%W]1Q+V-!nc%5d0N'6?n+IEj8f +KLB\h'ch>VV$@2:&j<-:E[khK(>]UXrlePdGK`(DKTO7aX.nfr+_bXm+jj(8,,_0dCuI60_u;eYLDT!@ +;F3q8L0B^.gbV;,M*=cK3>Jr6$a5t$?M:=6"94X-8dQZ')g4XcEGl7I,l0eqjAaWt/\D:"I+b:KrB&.^ +V1arOX[>Mu38F]&&/)%@,'#/Jbn_@pfjFHOdJ,l?3MhCjaK8H2;m5Gd^2[q+X[6Jc<('JMXkCRDY^bPC +c?bk[&kf,HE[khK(@J(lO)$rOf1J$#PRnJ<[B4+F5P)a`[1.s=;@to( +[:Tg@`:T\hS\BeZc8h_SON=PJ7>HATTZDG7`,_f4S;LLVKi_jBVg9E?_L:I7;F3q82PD=QGR68O,[++Y +3>Jr6$STc;cgs8lq_l_K-%n[(UFI2M=,m46W/t[\7H@FtRP!=9!mgI#gq,;d.4'X8D?DDXpdM!N%jW'L +%1J)6.O(dbenj=#ZZeS/p6nLR3N&)Q'"O"(B]9f9L_mA%+]1d!jAaWt/Ng6$hohShf/dnH.3TsW.QZXk +&[N&^3>IZT;/%K.Q#9 +#`5(/aJl,GNS;8@5)aci/\LBK\5dQE.3TsW.N35HIV>X8ON=DWS;J)gZa4`9cCM/QqWcH%>e_l'n7.hu +hnlGn:[B+bPS-;,VkQOPrDL,i)qQ<$,,ZW*.U#+38NV+^fTE((LMR[_M&BpJ,J5W&%]/qs8Msgn)&!j0B`5@F+89ME)%i*,]YA +O]0Y4^A6o\DVMo&m^r@7g"tTRhX\rRmnERJqR0D)55nGDhu#_%PS-;,VkSkco!Nb\#`5(/aJii*[r:0l +QLE(f2b2bMr:oe@hgBLn5I.;kRXG%6oZXjek>m!E=9h/:VkVQ1YL'>aFiu!TE[i9BaV?D3Kl>eYeSX8_ +&WaZr`/,.G]^a!<[dX@ueZ@P8\\\A6,,_.J^/>>qS;PCeBJ.!P_JB_FPS-;,VkR@Mg^.(i66`i9OSk#6#E_\&,,]YALiK1ec?UmF,V]p.2R`jPDA\IipHO&C_qJ9=?fYZ^>;b@=PS;Q#\jH3m: +77cK=6r$2B3d^SeCTkF[s2PkgV@oB93MhCjaJihj2II4,M4BIEWG#ViY@$b\mFuQo]t:oRaX<^N3HJeS +)F)C\-Kgms6:g0:ZrHA'LkS5l7H@FTJ?>rUA&jTMH0Y>'o8aqURe"b40KJ<\&jde?U(peN74]^).O(db +`U)sXI/NNTdoZ#[>F:q%8dQZ';pi`]I9"6dhmg3'M4BIEWMh[YR%/4i#pGF0dHZ_G_Ll(266`-iq)3N&)Q&ig^"b1&`4lU^?&FR@Y((rVBO],+m[P0`)PS-;,VkTlR1l#-hMM4Gn +7H@FtROq4Y3MhCjaJikBqQYj16r$23<7I?g9[5YpKTO9SBMGRS&IXGcMTDcq2-ZH[ +^BTm`aJjucMH[??aZ68`&/$u_Yp74]^).O(dbenec8c7(&m6r$/i@d5'g(r\fcV]p.2C<[M@kF>F:8EJr6$^5X,.3TsW.b[SXMc`O2<('JMXkCSPS5W#cM4BG]g%Z`'cjLKpM4BIEWMh[YR%/4i#pGF0&JMYC +4H!<4jA\NB7BP7[jIB.@#`5(/fJdCJcTI)=6r$23<7I?g9[5YpKTO9SLb\%!`'PZG,'#/Jbn_YhS5M[E +jAa&co.4>oO])iP't%W]PB0JJ+XD(XiXiFZE*>fn<('JMXkCSPS5W#cM4BHje[R3D:IsPG6r$1Qftug? +;b@=PS;N][KIsk+$PUEAQQpN#C<[M@kF>F:8E3E[kO,PLm$9bT^;P +aJl*q>F:q%8dQZ';\:12Qms^YjAaWt/POGMV1arOWJD0m8/F:8E<``O5c!cNt +WEbJ)WMh[YR%/4i#pGF0&L@p?1iWn-`(]<66r$1Qftug?;b@=PS;J^L.AS[cfI1>5QQpN#C<[M@kF>F: +8E<`F:8E<`9L/D.:M)3?kAq +enec8c7(&m6r$18=>6PmMqH3,D'5/iE[khK(+/47;F3q8<()QkUiAcS$PRG$QQpN#C<[M@kF>F:8E<`< +M1Ug9Cgi6HF1<3P;c.>=0KJ<\&jde?,"`Q(2"n>6NT*[jE^WYkXkCSPS5W#cM4BGO;&dsU3G];2/!hK$ +.Vd3D-Kgms6:g0:7#=AV=i;8ujbTFa>-iq)3N&)Q&ig\(U1m049tVWSNl\2u2-ZHJr6$^5X,.3TsW.\aj:6Q3^nNT/o<3?kAqenec8c7(&m6r$18=FNg$LTj30A1gC5<7I?g +9[5YpKTO9SL_?`saWb%-/k=u%e=%s_c8h_SON=DW&uuOWas&a$#@d?@0mkHcbn_YhS5M[EjA\O(*ee,: +%p"`@@^&h]ROq4Y3MhCjaJjt.]aJPk*;1:EIFc#f(a'R$AdafD:+7>3E[meO:ICSTKe;;5l=iZqMMbTD +Kul?QPRnHf'iC^o+[%W7NT)#EHUS?oMH[??aZ68`&/$u<;.:([9jY^QEA%`p,'m!QXkCSPS5W#cM4BGO +0qaUZinbSM7J)n'>-iq)3N&)Q&ig\(@W3Jb?c5G(8dqjHH:UK,ftug?;b@=PS;J^Ld#cK1?5[EFoSSN0 +Z\?L]Vj%`*3>GD#kTPp'_HrUSl%WLVIA!;<4TG?N8&"<,%mQTVcF,DekEHTG,,ZVWX@Gep\$/<6'^JU. +X/2P9IJMukh7UFKq==D-\i3B1Ip8;1F%Q`^O])iM(-;Cn3%bF: +8E<`(rV^SAEm?*[KLAR(PZ[O=/E3L.o86WW(!Z(-5QCQ4ZY+da +e"7\0lJ/pkp[@"3o);/AG87imbA>]fJTW(\E[mg%-A43e&[2&.XbfcNLt4JpU;aS1$ST/3r:ofaXfYg: +DI'ktXo%/Ce#0tUieo6iGd2`1lruGN(aUGJ.\aicKf!ZTHPr[n)cREoW+_(UmHoZRp>1&S`f7TSn*[s: +Vk8FlXkgJCH>Cq6Z%^:[Vj%`*3>GD#'13\IKoP*1-<#3V7<@./7.c`p=8(P"mH(*5O)AfO:&7@r@<0XB-%nZ]7?eA3[tcPEU\IY8,,_/\?+bFP +`/,-dDr0i0J+)7-ZY.UunA5Qo*^,tarjFL@kEHTG,,ZVW#\fo"]J((_;pi7hg&(X3Y9,oRFRP!@?[:au +cT_5Bb59r1jIB.@#`5(/-m>C!:#*TlR!8ZIO]0Y:[^W_1qWO]X^]+)VpU#Xr3N&)Q&ig\(6:d;6gqjis +1K6b;7BP7[jIB.@#`5(/-m>BRN>]SKnTUsgS;LLVKul?QPRnHf'i@:gZ^N_+NFEW"5t.p/_W74c8dL9D +MC9XD\QQks:_>]&[:Tf1F&"/-,]YB/KTHs[&W]a#3>Jr6$^5X,.3TsW.\_TX,*Ni57_K"iMTDcq2-ZH< +F%Q`^O])ibPt-<:^='t%W]PB0JJ+XD(XU4eUXN>oa(Y_Z:I:.;;<_W74c8dL9DMC9XDH5!aV +)D>P7O],+m[P0`)PS-;,V]n_=WKF04GhkioMTDcq2-ZH=0KJ<\&jde? +,"`,T3S&NhX,*:'>-iq)3N&)Q&ig\(6:f9VIL$JaKo&et:.;;<_W74c8dL9DMC9XDPo.8i#Jr6$^5X,.3TsW.\_TXH=X8; +-<:^='t%W]PB0JJ+XD(XU4eX)BP<<+7H@FtROq4Y3MhCjaJjt.6L(ao>I^:Y_k\Js.Vd3D-Kgms6:g0: +6jY%0h1RB8X$s*6E[khK(+/47;F3q8<(%$9%E@$:-go#',[:Tf1F&"/-,]YB/KTHCKl5F!j,$#:9 +'t%W]PB0JJ+XD(XU4eV_W,ISr7H@FtROq4Y3MhCjaJjt.65m$+I%+5,jAaWt/POGMV1arOWJD0R)S0We +GZc];U`sh/C<[M@kF>F:8E<`<#pHF1M-ILg,$#:9't%W]PB0JJ+XD(XU4eWZbkN@:GSI37S;LLVKul?Q +PRnHf'i@:gCMV'4XrbP3,,_0dCghA0.4'X8:.:BYde[aoLfHi<7BP7[jIB.@#`5(/-m>B\ES.aWPNSK; +&NLT6AdafD:+7>3E[mf:O>bDF/\LR'0r]u!MH[??aZ68`&/$u<;*mgCCYTJ-m,a((6r$1Qftug?;b@=P +S;J^=0KJ<\&jde?,"`,t;2^EC'p2El +CUPoWkEHTG,,ZVW#`5erftuWp4XLo+;c.>=0KJ<\&jde?,"`,tl)&=V^]%+C3>Jr6$^5X,.3TsW.\_TX +R*h]QI..q'`3lO$>F:q%8dQZ';\:=/`7tin_aZBPF"M.O(+/47;F3q8<(%$9Z+/IVnWYDoL4-3nXkCSP +S5W#cM4BGO+X@C7[Y56?<@2Z:C<[M@kF>F:8E<`<#pGH.:.8*jY[,HuY!oJG_W74c8dL9DMC9VnEoSZ2 +C^V3$N,gLbbn_YhS5M[EjA\MR+d&7#d0sgC0#k;t3Eql@F%Q`^O])i`2dlKul?QPRnHf'i@:gjJj\u +Ba`\7CWa&`2-ZH`2dlKul?QPRnHf'i@:gjI0.,gD9LIe:Zmee=%s_ +c8h_SON=DW&jdeOe;@kp_PZk-\/)Ja$^5X,.3TsW.\_TXaK;J:H)C(<(U5&=mOQn`enec8c7(&m6r$18 +&/)N;<&EJJp,$%Qdt`h3Rk7=Z3MhCjaJjt.66b%!MWPkpeo;5mYbF!b$^5X,.3TsW.\_TXaS!$-0KJ<\&jde?,"`,th!cdsF:400Fu'kf=4,IP]6E_EYJ*'e`grZk4*U*Tqt=[U_W74c8dL9D +MC9VnEpHP!Y_C+^#+IWT6U`RiGjsYPaceZ$g_bHLpIRGim+@Gu?@DLs7un]cq$=D_3TlV>66`HV8D[_'.up[6l**A1X/2!'AB_W74c8dL9DMC9Vn +Egom&jVFHTqp)8A04)(]W+Z4dgY5f4>^>bJk?`W5jTA`^`>;moE8e\5+$]PYKC0cJhnMD?6eT?;S5W#c +M4BGO+XD(I>>`j$]ZAYFZAku"\q^e%<-pRBP^r671XFFS]4h08VNZWk]1>MaYgSOjpG(+/47 +;F3q8<(%$9O]0XE/(qd'r\-S9P`E>/qrGk_DkE'Ld4KbCp?h:/2fIOmIHTY2O2(MQS!-%cOni::mlG[\ +go>8olTe#%<(%$9O]0Y4HFUqAr7-8o=oNI'^_XY,lW3c#D@qrDc\W;_hK\(?.9h%_Ih>@oe_baP5tL'9 +6jY$!;c06rgj20`+WpNZmHs:r,7]B8$e#?`p66+$3>GCX8Er3>IZTd<#W%E9HT@Z`h(9gAW2-V@lP[S5M[EjA\MR+d#\83>G9_lk[r3>IZT;/66`GCX8E<`<#pGF0&NLT6jtf\+7?e>r3>GCX8E<`<:.;;\%$OU%-%nZ]7?e>r3>IZT;/8A5 +jA\MR+d#\8-%nZ]7H@FtRa3ZJ'i@:gjA\MR+d#\83>Jsa1JCP4PRnHf'i@:gjA\NB7BQ@oP>`&>KLAR( +PRnHf'p2ElCHfYs<(%$9O])icCI%(IJ)-jD[6HR]tN6%o]`9GY53mW%N[cZoVXqPC"9\E +U"\>YrUZZ]r'15Q5M^6YI9RPm]ru3YI.>1UD[:u;n(t`jmom?Hc!87hj=YYYX&lJG4*Ku3;m3q3C3LJ9 +e[L?cH2RI-^\l9i*'JUiYG1K6msEjs?1j1dfNrpl[HF-.DUl4J6+VODqWZJ,pF\sKcCDFnqt@%.&VK`, +>ArnY?2N$1IUj$HQNn$BS32)f`f1q4Dfh>8\NF)Y]A2MZq[D:no%LKe]=Y\!n)!I"2I*h8C"5/"JU.4e +f=RgSLHk^1X6B0$5QBE_IeN^2cZePre[LI1lE3\6FibD2!StG@/tU7Q00aaQjNB3kln1dU:7Uf*)!:Ku +s7K`)8AG,jDnE2gp[6l7rqF2-dRu6qqF31V>r1;"l7SdX(#po*EuA2E]A2LSI.>/`(G>(SnUh?9kW\2( +n)%L29k%55\$qD>4aQedebut!G<fm^4*cno#h8=N4[lhBMq:eEgQ[cRr>l6i4JHl]Dn2b +PMEk5STipniN6Qk*^+iA]ANSTo?TYYJ'6-_:05iHIeB`VhC8_EGB`f%5Q:1+J,.:U.V&oCY:!,;F;LS, +s8Mbn^**pA`JYPECXN,'J+K/eFD6_`V1Z#o5(%V?e>ZgTqUaPA7TYP\grsY67IG2_]7'R"pBeKXoB,6U +NupUck*p:ll6`b?hL>@hY,aednW0`b+Ul]Jf%*(bj$,_Vr7Dcs!IJ2'55OKI[J#4Se^8S/GTemW(+q8JOD3?1-(A>IV1>i.:&+ +3H=&s)fNBoWggrRIJiWU>s`uCL';tqaX;F&VYe%YbQ<7nqqlT(SNhW'jd0>'pYC%CorDGBQ_PZ$kXV4= +RKZs1I/1ijrSRoFq!uT]=+,uDOom5S?#CE3I1*[1ST?KaouB;rn`7fU5P`Ds*^>+dlJ'1k4F-MYqqqGQ +J,\V\metNKAc][Dh=%p.C-V^fO8$]`LECtii,OT-MdQDA(T1$!lL'7ADf=OFGOO=tF/Mg+2M[]k/PL') +(3Z%V`8:*&HGB_2hLGS(DS2K(J)7/8!WVk3s7Z2:s7b@2kgi]4qQKpFm+AT22h1huT3$Hi(lU]Wi=C:] +rB'jFpGSF@2gu;8+Qq?`J)+Cmro66B@thCL@6(Q$guW,KPI@^-gf=&sD#_3ChE13BItc1\[^NWUEdmi- +?`s6[0N8i@>W=L!'uTB%rUeRARHrJ.O1t/6WDb9U/pi)IEo:/Hs7B+eoS[)S@T>@D_)\`/guW,KA%/\S +gpPG!4o1Z%GM[VCb@^B,hI0_P4$21AhL>@hkFR%'qb?hYBJo3AYtViD?.rrcr\u +iE,tpC'Ts(dB[t@Xe4/hf`AV[^NWQ\$mGAGjrj+YB4.'^Adup + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 33.71863] CT +[1 0 0 1 0 0] CT +N +-1284 -69.75 M +-1284 286.5 L +319.75 286.5 L +319.75 -69.75 L +cp +clip +GS +0 0 translate +320 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0VDeN2Es2XXaA;!JZ+VDU26&]7/YR=r48hIXuO^_l1)%fc!)A3VtLkuP>5Y=EWKTI)f.nW3FR+\+( +_&*b16R:-V(e]V)@"h&#`V"Ya&"_\K>j"/[Y.ZL7Hb4Yhkf.aNkNn_bFZRj>F"V[-;@3B,WN"!d#WGR] +=4`$-$S"m,_Uf;GSd5]q#!%K5GXB'r`?l0[rr]fUOOI!f0hgD=1@6'"g54K2iP +V+[/LWCj?j.$Uat2gaN6p\X[=Demd:YJ"!.S'N,Rk3lpM=#OmITG_jQ//F-nDT.JJ$PtE8)]R8%hgYGs +Vl]48Xj^3C4b!==k1m7R9BlB];]WG(RZ,=bpA>Jh`2UQak[4WCSCA7S)(28ck>gCf]*(1(Ren%S[:;]8qu>;se95em,1 +m!c"2/)9+BiQIQ@VbYbS;o^uEPHfdjrqU2=$Z@<_Rm6a<^!L]skSNnY^\OX3e-l6;O+'qZoUA)A7D7&d +rob0k6GN>k?doJ^k)Xsg0D(_uqF/*EDuLZehr3PQ`&E2j6UO6(c_%E0l6[i):7aR.3B:7"`Fcc@=bV=r +f@^$Krld?>_1Di5s"!*F_*Mqa'.6P>:V(f@@W-2RbSH]go08b/[sN`daGFMrVH$V +gJV1h#7hk\\o_Y-F0k"Vm^_N??Ph-d*BSF\gU:r,(G=2GKgJ'A\^AnI4*E/YZq.C(>.&,6hS#oeC7[NT +\3g@#]mB?1hg?*'H0!$Y4k1'XFeu&j)ep+3hKc#*R5]%j7n:U$?[WXui/K\PW+N!s]^nmc1k5N&c'pYI +S*cHD]ejk9!*']pm-3bkDGuW6QklX@naBfK@U-*i00:Q.DuLZh?gWW>SXlC%qWUKlIAApl]'HNg*Su%U +]6E_pMI(VnV;oUW%h&k[2H:<:,u=tP]QirK%m4gpd%Lt6pcslTEnI5]RmF1Dg9jR?b=>=e^&X#BoC^.F"i9*PE8+ZmAA2<;Jg@Zi_f6dGca.n?ahWlND+Z=d:='\6qhf%Kj +bP0nDYI!#'YM*3!]lh;O($5C+$oRr<8R6IJmF0qE2DHo!.5e0lANaA+AkdAgh/`.W^P;29IS)'4Q7Y`L +J$\nI;WW?p9kVbAM'dqW`>SqTWBbpT$G5H9+)JW]`%@Ak)`RlbRQ7\NkpL*G+6^^T%13Ni).2&6nDjkL+JNL3)'f[2ZdSk*tK2 +Adig&E`/*Em+;nE$J4P"IJ7#VmF0qE[_1L:0s1mpD7#9!JC/<7)SY++WB3K4FRf;)o,ShlXf\_ami1;Z +5DQj=WWMRXC.'H%H9*f:&hTp-]cqTaWo`H;=Y(>L*BSHAmC-0^\,8.jnP"[lL@g+eV`7,c31WD%'nqhg +5fQXnj7DH[f<56-iUlB`iFCru[:eCL[_,tl[V4&4/Ko*hPK]:Hq4lj#Cg^X9(TR#OV=W)3A]"a'Y4]Y8@%h*W:;X?eEc0(]&h+ +rHb/Ef[6l=qLosgHD+]udB=A_KcU,jg4lq +]6sHdq(.$`/t[Sq50KS1bNJII0783P48SV:^TaJ?0D#W6qXm1iYbPPYoF!bWog6EtUL,\H$Tn4X6GKb" +?SD$Z48SV:^TaJ?0D#W6qXm1iYbPPYCjF[<(O9H4_iDFO$X4ZqlpY75nuuUWo"p]J*IEnb[QNp/MnH:U +@E2^8Ke3fsp*)/+H.]A97tS7:+X;Y[]?W_NortH]],Q4RC^Y]WgL+a'Rh\/J#I^p7aGF +ZcnF+jT#8\zzz!!&C]cCI%@q=19k]5o#+k+'r7kigEhpYC%#pc?jGPq0WJp04`Z2(JRMe:uN9:$q]8O$D2@qF.jB]kDY8=KfC:qW +iY0p"-$5Q_&[&l$Mj/Vp]9#C3B47e\->(Wm)o_GTsQQ?[r#S)uSB%T;>moP-GgGF`hhNG=PG!7n8n!?T +p;'j>"j_1bg_Ym3rN7*og2_\gp#:H0CrWp?^HGT$f9lOk>RUqs6+%eKn<3XKHsDMH]]m@q/,Iq40TkG'%hRrU +neB2l3^]Don.gH>rTE=l44.2G8l7T@'s829"X"j(VpR][&og#_XNYAa%q55VO558N(Df>8iI!f0Tk]4mWFH%jEoB-D2^S +jWt?DNct]s;S655k$o)]RPAET>3,/-#D)&'$so,l34#l<^9WVbedpl`[^3f:G'Jc>.Y)AtW6W.bij1s4 +r#Ez!''itABo#@~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 0.07987] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 356.5 L +1603.75 356.5 L +1603.75 0.25 L +cp +clip +GS +0 0 translate +1284 70 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 70 0 0] + /Height 70 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W0l'_b+3/@CNC1ZNEU6%(*..m@%_=at/-I,Si8>^`JfPk">WRbPF`;@8To4`6GRsNJ0/60sP((-[ +F40.Bi]GqKgb^sN +eYeP\fIer^$[;4C^hS":/hd-$NmFr-m4?1%Mp?gW&!FNTjmFd2[>.igA.\_TXaJjt.66`+Q?ba1AY?nn'I:TfEGPAatq=;1rnd^j^&LE)u:.:BY&ig\h;pek+LAUT*cTcoGiosb +Dj2[5:c1g\a:I"XNp$5DF4?;l00(=Gb"TG5)8dL9DMC9VnE[i9BbnVh:'[Q-_+$P)9DKZuhoODdo\FK@ +aj.9P$R4e\+<(%$9O])i]'W.$$`Jjo66`< +O8dL9DMTDcqhom^#nscrA]QnSpJ

WJ",!tmk?a:;*mh7<(%$9O])iP(%n'LKfaMHgj/\aiTX'dV\0) +.&LE)u:.:BY&ig\h;c1<9]C=\6??a_R(GCf0O$A-n]Xtd;]c*O)*@@[.8dL9DMC9VnE[i92U4-a!iQec +BcZXJT:B(6fqtKP$*Zk#;K6N]QMC9VnE[mf:ON=DWS;Jr]kih6ZVbd7qn-SP;3>GCX8E<`<#pGF0&NLT +6jrGeH,"`,tS;J^<,]Y@YVkVbT_rOcb#pGF0&LE)u:.=5Ce=(TKS;J^<,]YB/KTO9SLn'?+o+$s_&LE) +u:.:BY&ig\h;c.>e@<*ilKTO9SL_<#uV]p.2C<\h6DDIW=r=[`c66`66`66`66`ik,qY+iWn$C1 +L-%nZ]7H@FtRU);NF3;&EH#),=QZ7e_ic7En+d#\83>Jr6(FJ=8'%t?kC_5@-i@`IS<11#SX[["]-N^f8dL9DMTDcq27kP&kEHT0KXB.mT5?OGE^U3WO])iP't%Wi-Kgms+q!S_mYZ2 +E=tW=0&/$u/-?NcJ+f2&LE)u:.=5Ce='[1S9%;Z+[R)pmBlQsbiOWn66`!u2M.%3WJI(%>-eU#F&"/=A4Z` +)?Pbk=Q!JX6*LYJ`N[Q%rE[i92U=S\JjIB.@1_"0uR_%"`pihM,i%EgQ5D],?N[Q%rE[i92U=S\JjIB. +@e:s3(WYbS3dV#30kJ;dl3MM1gaJjucMH[=oPB0JJ;%hdDr7C7@=.N@>`L3U,]Y@YVkVc/_W74 +c9&q.$pg`'a?Zl&18'%_pf9O<^J-B2<-m9jV.O(dbf'L97S5W%es'0g$ht+MWQ^R`=2(NZ,U4eWLWJI( +%>-eU#F&"13AeuM3s#pBn/MlSXn1;iGL_<#uV]p.2C<\A)c8h^hh(H>EbAZiB>\OeY&[0.7L_<#uV]p. +2C<\A)c8h^h'%4A[^N6Nk%U4f,E]KkION=DWS;LLV$^5X,.5;jk14.p?>CLp$MMnta;*mh7<('JMXkAE +M3N&*$$#,feDaX6U\X$QZa*s^tb2nEl66`^M!q%!P?%q#~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 0.07987] CT +[1 0 0 1 0 0] CT +N +-1284 0.25 M +-1284 356.5 L +319.75 356.5 L +319.75 0.25 L +cp +clip +GS +0 0 translate +320 70 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 70 0 0] + /Height 70 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V5tEJ"!!i"K4s&0_\u>s(N1#XT^r6kBqN,/.h\6jF#/O-#Ie+KdstF8c-k_mZn2mPo]/Am]I6NJNCHLkpkCLkpkCLkpkCLkpkCLkpkCLkr"7 +Iu*J96-Op?.e]nNkNLd +Cj?Z6?=Xk\4GdQ$Z3sm.`3C-3Ke6*[fSf+5Q7$\=7W<%>;TEcK=,HHsh)gt^]:^C_A)*DNA.,1R/3%*[&DlheY10cH$0/<]>hn^6Ce +>[CaFUN.fN?hGD2hqm5H56`J_-XNtm#8)`WX'K(>7m6]4dYc7,/<]5rrdJNo#TjiR91BBF*61*a1I?Qf +qYEleQVOGue_Aj5;B!oH?g09'hqHfLl00'))e4?=U^Qekp@a6A?].#tH$S)Q^s$Ds +PEV3O%SH\KIlne(O3c%om-a:N#KaieL_E+Kd*9gpZ,jLsS9B$t'oU'-qtj5/#R%@oDtj@H]Y*0mrV_02 +0Y^eHGKr*_?'jH,M[Ne_naS@-2`E\#^Tk&+9IBG3ouP3FV,BTV[9=Rj*1WId_Q4]QiG]WHhVI"%L.,u, +PF7f*^e5eN:JXd?hs4X-b&NlrkJQcJ45oPm%/U;Z?Ld>oJarL-oi:kpo$>X#CEFGc/$M#r^KBcmIl]_U +?!1=H:l$MC8&\pL@inaQp%.eNI!oaXI=7*'KNBh,eS5pd:&3>Rbb+2ies5:&iSe4pW>?_s.)LdVd;PH1 +cWB(;g`2nme'2NaPm[s-qTUO=+P%c?LLoW?oX+q/[S=[((MEe9DNT;_2PRrELt1q=F(4^Mt/-\QCVRb@Rfdr*!0>qI_/;fC&CiWOb+)Qr2o7 +9n.%85!G"N)NS:e7;=N%`lX-sMct2t[BA;aq/,L8RGXlMRU62emro.rj?XHBc;H[C[UdWdhl +d0g'Y>''MIY+5;F@X_P4c2!(HfiahqZ*IE2+`Y>+&8PVN:,:DAQnr6_MB>]a9I=cAg=i"hGP\&]-rLqD +`s[TQWQ:fSk1>FbD-<4g([G!nN*S'&IWs!(pD`Q\,n^Nff;f_H*))ssorKlrfk_Egie1DbAh(*bSG>$O +ITtt$r&+j3ge9&IJ2AgrLm_#9)uel6S]-):YccbQQs]NR?]p-=RI.#eQa(!9cGW`fM:`mNEjBMHY&&>/%\G~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1283 286 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +0.941 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.48 0 0 0.48055 0 207.31862] CT +[1 0 0 1 0 0] CT +N +0 -71 M +0 286.5 L +1603.75 286.5 L +1603.75 -71 L +cp +clip +GS +0 0 translate +1284 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Wd>Lueh$nd[EDf2p'U>O'&(N3G33+B.VaD,6P)K>0g`Yju +j('-l6BI"e,3#Ds1gP>[T'a)"0T/JI8Tg_F;Q't=`1pc]O[>.cP9T"^W&.eiFYbp_?'mXjgS6Ap:Q-Mr +#SM,T7%=sVV@0kHhc;i'.na9@%a%US(Z)JCN.c1=$:,k2MMRIjdWFGKbkmjt10LNmOVjkh^K$sMl,'3) +*0ERLg9\RVjD;7f>CF_idWFH:4h(&Zr[u2O*L*WZc$s^^g4a=Ub)sn-,)%/IVtV*@)q%arZ`#]d\3bPn +@f;583ZX7QCsDR"ap"cQ,ss]4&Ldj^0tgX9gVC74Qpmk9_NeeUJc:FI3Y:O!S+@R7%8tG$Ld]5p.0QV) +]iQT9^p\!F7A]7crL.:L\]`Oq-S;@$01WgrNUe/0VQ-!baUAnf!PXhS"AVaF:`#g`EcQOosi(E0V6N^tVj+b>M2&qZ;-1J>Vp*mcH?OB+P._tN$D4+ +;Ic_slr)=M.2jL<36Nkt\#-QuaZ:FD?'mo3N^tl5Te'.D-lJbcM%4'hhB+Z<6)d6,)%-sVTP;Z7?*G(ELNrafkgP& +f'*`.=*!m#%a8Ip0QG)E.0QV)O'T^OtRTJJ<^cQmtV:'K&@;`i67V]X1gVn!MBjL+5[uF'.-PGgKgIf>&3Z+`Z7% +$:/E#Q+%r8e/0VQVokV$>l45"YQ#qj&mM7AMsu8,D'4NT25\MV/k(b^U2d4"),IMc5i)RU,gL#NQML1; +_Na5lJJ<^cQn%:/'K&@;`i67VHst>Zge$sa*lR$gUrq*$XEU9j@VG2u,,TKo'm*5RKel9U.tMMAd^.j&F\fI\8j-n4m]39c[9XWZ3&'C<99/eA&0\JM4tE +d!LWe#!lutQ+%r8e/0VQVokV$I8,!rZ^64>kL?uAQPY@Vn%uBc[,+a"0F1QdWZ9noe/0VQVokV$h7Np& +%A&;tp%$m4r_%S[pjtU.]r-Zp&>[Dm.3]b0e%!FQ$kgF6dR3&Ha.HNN$rA +92T2pBUgPs`kDD_D4[>r/cSshTaT>P)H+KR#Z>04]G6R'`^nK:p%443r3bD]gcSA4]D?jA-N(QB%"*nL +=sYVlFb8,1.L3Es2+EJCBt;RbZR'hDqk%Y]+]DB$3]L6&:Rj%20pSPjed55aWZ9ele/0VQVt/1//?f!& +(HlBmq.l3I4IK$TX.C$Q1dPkMPuCnE)C%]2<9>'jnbpcqO/-2aNXcPr$Vr,!mA_O&1ej&M;ruMpU;faO +39<<`#JU5Kb(pYlH#6Nq:Y^DKHW:/TXrCA@MJ/>!25\NY/ObY]U2^N)j5'i#G+RV]l,!Z;m2"6<+rVau +NN$r)92T2pBUfid`b0]!F%pACN%.=&#LeA;,2+WK/O_KgO;_'Z]U#;38C;s(DJn=EHhY/@^,u+`1I'p+ +^]1jA"Ata<^.#_"CFsYe`tb*lk#GiIBTXDpVTN=!7?*G(U$,8*b0%lu:]0KZ^O#P9dBlD>IE#o9^6POL +jV!.\@0XoJ7Q$cL1fnAIAh5Ff5RaU@46cBc4nYG6#')YZV-/o,=V"spV6aa2;\p.pgE88)]KaP +=0>f=1@`[QH,^A2^,:Urb09P).(0Jl)Ns\[[0+2$o<\o6B;>4ng2&\%Suip]?I[V7n'SQ1g#dMAcCM-6 +If/`V]>+-9+8tEj\)3eV]MI78kP'$(G3OLQfOA&2iTYfc[j-lH\QAqRr)f=:X']` +OaHN,VuHFi*;,n6gWh^l]6E_hq]>"inI,7`Ya]@QJtAQYPm8`lS#\BhnP"Etm"/5.jXHo=Jjc+CWZ6dEM_c8+gW9.P[gleFOZp,RNP/>8Ppc\]aA#n[;-s8Rb,fR2nOB_ +q=(=8ZZKX8XA]nd<#:!GN$"cOf6r0%bUDpM_5(`F(GB*"=]sGW +miS)`HhPj^7oLJ1$PR0DZFDg)E,uiEVp'JrP9+)m\S*0Ue5)0^PG2A?pPIhmUhDKPS"'0?h4*8Q\*:cJ +=f4Zqn?]HE1jrA0E:83]R#qt+ihU80)Z<(nFAc),/khdX8sCs@*"*QkY7!-AJjc+C/^;#5!^E:Gn$m4s +U=+-liiH'FWiT1K27Uh*/A0m;rkU)1JoO0c5pB']MAj\dJu=g,dWApWXA_G(]QuHFb>0\%a1:r<15F5S97GU7XfO3NqTWg_**8@O&pT6g.Dt-&gZJ4.,?"1&NlTuQ)4p&B:Y2N(G8DuL7&>\ +EDi$7^n1n(:5VZmKo*-2M5mj]*N5gJNpj#$ba8YKEDdK-WqShU,"8?m_SfTT7(%;cCMBtp@@2fm&LU!T +S+9nLRP$SC+@W-k_SfTT6oB=RCLbDkp@?nGVU]n[`XsX&M)Zqf^ngO*drf9%Uo6+UQoODrW%)$%N#L>m +>o;]L8An*I>UI-#\u$uO&#?-\?'mV;Pc0nOGn',P/due6,iYW`5X#d-+=Sd7(FIEg-*$Kn?e)c\\9#58 +UEN1H8^S59&Nh9e.SU^+c8m8OnOm9?]:ZfY45ZsY_`!NG+\9OCdCsuPGV2lnRP%:(F\5VuX_KK]m^k([ +o+ND/UhC:2#[W#qQ)4p&B:T[crY;]p,KS]7lJKs#*0ERLLm+RR4qSb]7A]7c`*'!+%H>f_QXQWK&@9Aa:&N>jeq*]1!,=tanFCF11;kaJ[g!mJ29u>_Hm=er1hO#SM-08Wia6pLl60 +Wg582S97HR5VK0$^Fid`S(DsBA/p^uOGFi@MPe$&\u*;.A-5A"j`FqjXAbjnCfPJg*KA[)+\%QLCH=o@ +$XJUd;q(qUQoODrW.A8dq`1b>r2mB`)];/A&1$8(9@nd[EL!HC-\@ga +H&;ek(FIEg-#1fYi2%LJd`uC*G>?-i.0PL;$<-"!_9Jr570B:m'5*jK2=ugsZKq"#<;EU58#-\Aii5V5 +:BWVVWY>o*/khdX8qK?9Q'Z]6b<1ba5%V_G.0PL;8lF^`_+gsU70BCp'5*l!TZU`/BGh%6oQ)u"j+nD$ +6sRJURdseK&RQa9@ht?,$PR0DP$eD/BjGriN$D4+&okU\aEsTc`[iT6(FIEg-,Bkif#8/>"R#8>`XsX& +M7=k08.-IaZ`#]dQoODr'`SuFTjP#=iR3Z0;@!nT91tBS4CDbuBt?!f@'lje0rSN/SuZ#SM-2;WPO&4@!7o+F4o3u3-=B2mZ[A#GMdO[pV$=XS+K3m +@H+@e;Z%n5A%Laa*:'!;fR\u+;@!nT$`$<$Ae24]/rq&:\om>ilK[Z[=#Mur]=.V)]C/Mk,J#RKegW%; +hLub1gbGA)!uXV)&1$7I:QZ".,\Bp#jia"&[en`jT1sXIoCKQs:HNX[9]*It&8VScpDlNVHaF3.mWfTC +%V%S1dhtZO&1$7I8^[/G]S)iOEat\0nKNALhRt.(NkG2MRo:BgF^a9-iX2$lpU]k&kRimEN(m_#O[>.c +S!"sK">_:/BSqe/NUM)EoDa:5fk3g9*e!_3=4c$SVL0%_QoODr'`T8N;;Ms;GdsM2=&Tr+U_"g2p.^Yb +bZrAd.B[!j])/VQ]0#J@)`J6K5(*(jg$(s#YqA]Sl]`)Z"_iAY":O&o\Vkd&EC:.",&VU^<+$JO(/=>g +iPU_"g"bBG!c9(L=nkF47iiH'Vke`85(V-)_RW1_&5pC2.79VAWB$n8o>e.0=j/lp@]IlmJ +F\7OR^IS.j!lmIT'/bJ`6jc4(8-L>o=YuJqe^_Qm&"bt0o#B.mpu73'Imb)"c8m8jhUdHOH.+J)EKBcD +9jo$(6BG<:(/,pYBJ.o??f1UmK(SckDbp3S\hJ-T.4KAtD7X\>EC:."@U:N<-(%,tl,(AAVkh[nGKfe> +Bm)4k80LOJ:(*p=Z`#]dQoODr'YT@+N^tMf(Ilb`6K +N^tO''5*l!65P*:/j(fIL_PHZZZc?^jXfkSQ6P=aW&.eI39ueCH\8-[*Va72&YGI_ +N$D4+'"dNP_R"_9=GhouU=+-liiH'Vked5Ns/*u]RD:d+KconFr'R`bLc?h59c#/%3[McEmuc2rT%JB: +S='\B,>nfqaA(p6ZO^=j,#4"fdtNXb:._,e:\WA\E+UFNN^t`-h&TL]JRP%:(F\3!HGS[m[FN+nA"DK-5T*V#`MMRIjdWApWXA`"j%$JW>a0'bHk@P7b +L_PFtGU]6jSkb0!PG6KI@H+@e;X>JNYCSn'kq;>plt.6"6BHpOpCkBl8J,r?<15F5S97F2?$I^B']GUd +4n]N"EC:F*)OP/Eq(:1#R./aZ1QG!L8Ae:#'>`-h&TL]JRP%:(F\3!HGU?A8`q5>/V/2DQN^tqjh+m +7%=sVV@+&MSWZ;U51j\L@`i?C#2b>u,>neFF"//1;FI#hQ)4p&B:Y2P(S8`2-_YVu[b5K*b-rG&4!IFW +9%/KNZVF(H/P)/^g%6n9*dYRG[FoB)j_U"Dpn@QPNeimndWApWXA`$\_?[%",kQPPCU;]1;mq"^4!IFW +9%/KNZVF)c$Ql%UPL02$YK^[cD/U/0G=7r8Q)4p&B:Y4FJ07j@e$C$(55*YH8#-\AihVAi.4S%.-I+Xu +Ke@`QB4(q`&Ldj>3%6%HMFu?OM6frKTaOXPo#%)fEC:.",#4#OPG6KI@H+@e;Wu!HN;W0XAfQq"OGK)E +ihWM0?'mV;PS=?-C!;!Ebf;mXj+nD$7%=r'-Ujlr_SfTTUsOf+0[?GbPArtt5pB']Lm+R]FCF11;c;hr +'92_$k@Ycs6BI"e,3#Ds1gQJ.l]`*pn3j$7nG*qlU_"g2UkguZ.SU^+c8m8jX?db5+4Q!WAfQq"OGK)E +ihWM0?'mV;PS>nWBsb!]Cra.9j+nD$7%=r'-Ujlr_SfTTUsOhi2o7ol0TJ[I8An(3EDfc)/khdX8d\jK +Ij7):!98kLJjc+C&LU#Dl,(AAVkf^?o>79fL1<=%(upDdJjc+C&LU#Dl,(AAVkf\)8E4End[ +EDf2p'U>O''5*l!_X#)9(tHH70TJ[I8An(3EDfc)/khdX8d\jK+/:mL%)rl."Go)e,#4"fdtNXb:.[1t +H_a0[&BAjf8W4ll#SM,T7%=sVV@+&MSWZ;UhJ4BlRE@K5KcgsT7Dqbp`1oh`56(QPl&X5GM6frKTaUq!>@$8[]5+\9OC&TL]*`Q#p; +pUbNS=^PNcl(saGr9$:_5p![C*F"s8MaWrpc%6rqPML +T(q.1I^cVgdf&1+YJ-It9lsh'XA`"J&u;4E`h=GTH,^FF"Go)e,#4#mGkgde8%WSTDOrq6DVn7bSj.^R +dPoR-M6frKTaO(rGXo+T`XsX&M)Zq.:5\2^e_uKn/ml>?q!d81(Z![jr=N>&39ueC]'t1Xnt;;f3$O&! +&LU#c8Pt/e$aUi(.4KAtrtsf_/W4U:$:,k2MMRIj#[NhZYYIcp@#tTG*/AVeb@MZU6BI"e,3#Ds1gQJ. +l]`*A#AH`REKJ_"M2WZW#SM,T7%=sVV@+&MSW^kU_Z+7VLU2*B&Ldj>3%6%HMFu?OM6frKTaUmmJK<%a +)+UJ6,iYVU3%6=O(FIEg,t04QK5$@WE/\d)$:,k2MMRIjdWApWXA`#M4tf$Wr]$8Il&TgP$:,k2MMRIj +dWApWXA`$\_S\BqrYS/$&1$82Lc?h59c#/%3[I5e4jjFGOi'/9?5Z8/&1$82Lc?h59c#/%3[Mbt]C5e1 +T`W4G.0PJe.3_NE9%/KNZVF)c$TEP@'ekY=+\9OC&TL]JRP%:(F\3!H\G8LjJ8msa5pB']Lm+R]FCF11 +;c4kDWTb1t$YM/R80LOB8J,r?<15F5S97G]BTS\NP"kCm9oL;p#SM,T7%=sVV@+&MSWZ;UDKY,X@N:Ls +OGK)EihWM0?'mV;PS?.HO`%O'[_(^f0oedJ8An(3EDfc)/khdX8rB,][Fcc?iB5*l.0PJe.3_NE9%/KN +ZVF(HhUdH#hE&Ldj>3%6%HMFu?OM6frK1"%]0g^_r\ +3$O&!&LU#c8Ps350QGYm.nd[EDf2p'U>O''5*l!+tZ?&[EH#P[X,q*.0PJe.3_NE9%/KN +ZVF(H`\-&nXu=-V,)%-O-UksF;/*TqERlQfFCF0f,>nd[EDf2p'U>O''5*l!+u;clFNdeZZQ7$t.0PJe +.3_NE9%/KNZVF(H\h;dV::A/%,)%-O-UksF;/*TqERlQfFCF0f,>nd[EDf2p'U>O''5*l!+ur3e2pM3n +YX-8@.0PJe.3_NE9%/KNZVF)S+/G>eVBT9"EC:.",#4#OPG6KI@H+@e;N0paiQ?AJ=iM%&"Go)e,#4"f +dtNXb:.\=DFl@))VCl,.EC:.",#4#OPG6KI@H+@e;O$I`(&#meGH?54&Ldj>3%6%HMFu?OM6frKTaMNH +*0ERLLm'%BUo6+UQoODr'LdUQBBuX?Y6Mg&,)%-O-UksF;/*TqERlQf`mTPkgnb$+N^t.cP9T"^W&.eI39ueCV1MZN*Nj7/L;c+rKcgsT7DqbpBt?!G80LOB8J,r?<15F5S97G];0:d;hB6*iEC:.",#4#OPG6KI@H+@e;A9O;ojQgA:*8YQ"'QIrrGmr^^=^39ueC_sbI-m)G\98#-\AihVAi.9aO3Rl?BM5Q:Qo-Vg2AF6Ci5#OB7F +=f8&D)l+BuBFXhmFN+nA"Go)e,#4$XFnb=[Y23KpE++0CrS)8*II4GoNZ:(Mh07aNHFtk(S97G]JTp)0 +G@2DL,>nd[EDf2p'[>so='n+3k02+hs7NRF*BSH2dn`3!oQXZ/39ueC\hJ-D&Ldj>3%6%HMWV4Y^>="e +%mTu2p-58Z'5*l!U3MdrD54]3;A_p"&1$82Lc?iPbFL_=\o.Da(LLF[Gk':_;a:*":G,n^nEA8p.=^FS +q-ATMSN1bgmp0<,8dB'S)_Ln&n5pB']L`:Dm(?iUWIe[@'Bkt_Se#A=4h"#60ddT=YC+8MfD,l5pB']Lm+P^2E!J-hkg4mX]r9GDL;6Pl(.DBl)1.s?g2n, +Ui2t8'5*l!@^'-?&RH[AL_PG/S.lP_78mI)H?JsdQe0g>DVr1arSlP=CV([3jN*JTkKcs#-FtmNegW&6 +'VshL;dFX)N$D4+&ogf;S.n]fnDV9GG1k[+I.6&[f55H_]XdoJa,V1hKdk/*-,oG,eCTVq\o;r7&Ldj> +3%6%HMWXTkM_Dg?k*nHIrT9Sf]!ctIb*>PZf7f'f:.[1af7d5_jYDie+\9OC&TL]*YA;amFnPIf`JYZS +)\4#Sr5C=e +4BNCHc8m8j!5#HN]QD.*.0PJe.3_NE(:1EVeQ(PXe^XaDW='APSW[I861Uc>&/e!6&1$82Lc?h59c#/% +3[MaCK=%/7WWb&HEC:.",#4#OPG6KI@H+@e;Y9IiE7$`57H$EI8#-\AihVAi.4S%.-I+Xu7A!jF]rij% +4Y_sf+\9OC&TL]JRP%:(F\7N4JtrbbNc@Go5pB']Lm+R]FCF11;c<6$UdCnX$grjk6jc4(:5XjkU=+-l +iiH'V%&=8X7CZF=&Ldj>3%6%HMFu?OM6frKd9D]QgT)s^/)uF"Jjc+C&LU#Dl,(AAVkiN486H*s&al-. +6jc4(:5XjkU=+-liiH'VoGjZOHS,)HWL8`c6jc4(:5XjkU=+-liiH'Vo\Y?I2-(*fK5j9d5pB']Lm+R] +FCF11;c6TEL$.j0MJtG3,>nd[EDf2p'U>O''5*l!U6Zoj\hJQ5,t0XcOGK)EihWM0?'mV;PSCM.Bn9`o +#Zmde'[\m+,#4#OPG6KI@H+@e;Uc>T\[o"h7ouRR'[\m+,#4#OPG6KI@H+@e;H+@+4eEaD[c\_,!.%O? +MJ/D#&LU#c8Ps350QGYm.I%8gZYap/7Q'I'&^:q6ihVAi.4S%.-I+Xu7?'Uem^Xfd4).G$%2'rGUkguZ +.SU^+c8m7?NBu!H>>o$hBTO>K-UksF;/*TqERlQfWXb1-gk_t/6otO.3%6%HMFu?OM6frK+\9OC&Ldj> +3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%H +MFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?O +M6frK+\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM6frK ++\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM3B,]#SM,T6jc4(:5XjkU=,Q?ihVAEJjc+C +O[>.cP9T"^W&/.S3%6&*+;#tiU_"g2UkguZ.a8hX:5VRc8An(3.0PJe.3_NE8oo`DP9Nm1&ogf;$:,k2 +MMRIjd[Xb*Ukf_:7%=sR6BI"e,3#Ds1hW18.3aHg&LU#C&1$82Lc?hebf7ZBh7ImGq<".K+6a1I-UjgB +,iYVU'S8`C'G2]]$i%U]rc%)JU$&%mq;ms@2e$Ee3%6&*+;#tiU_"g2UkguZRb)oT^"8g^CY,_+D/J+D +J,c@3?935oe3%6&* ++;#ti`(A@h?@VqHe#-"1r:^*`hs\kLlK[Z1a#h2W8J*BSM)Zq.KcgsT7DqdF]^FRcZa-n-a$9RnDh%Z= +X]r8p+$]bj?iPGpj.Dt$-$YkXO@#%bP9Nm1&ogf;gmD$IE#lZ3iPJ?.4aHVZ\)2Wr]aHLk(Vr!Y,)%-O +-UjgB,iYVU3%:;*@q4Qr?+.NCem!&54*KtcO$;k.hu!0:T/guTq!mBClEE'Xgd/UoM$+r]J'7)ff)?;\T;&fFD5;tjnU9QorDGB\"J0<&WKpt>Pmf+Is +^D+U$WQ-pu74f$LN2>?cUWqqo$?L,\";ZgXHZ0u*P/Dn;ckqtBD07BXl$keMcOs6WLHPl0FR+4$G,_3\8Yl)IJqo=^[9%gOX!B/"N.n/HgeX#8Ct"Ml`P$tn)&WZV3 +arJ3nT\6h&UVSU"qjgfl^ +E#CEhMitk^WTpJ%Fst/$HUkVo?1-Ak%t^jH_m`^]49$mKA"._$<&/ktZ9nPq,q=oXIu0jjBhn[r1#*?! +Z:^b3%huQ_J]]ICI81U/>-!>Gk8hIcm8G\gaE_1U$KEoksPPpu.!Vmg.0brUndS=g[eTpYUJ8Mq7;H*U +N+hn%V)1m+>\EnsAb\]3C4uDItgV7;fZbh77U)4+SK'r:02.rocj:HMd*=qq<>#i:?!Q?*6Bgm'FEMK_ +AZ`Gk%[rrTW%iRP)ec[P)B&I.9WAUf40drT#=j\T$MZ:ZSd2/k[]F.t@KW+92&D^NDK-a,_>Boril$pJ +0+dY2QOeYIFjT-hea/mPpUr-t8"H1K^.^[6B/BZZh]j-H/<9^fi9\` +557EddZ7B1J"(IJ),lf@JOLhA7*:^:sTp9/$8>c[XgNXY.g!DV_nS?.;WA5.smD5Q16'4mu$s=hOFY>K +;aElY]!I2E(!\^OPCiRH)>KX2E.V(FF#l(O!um@f,>*HG]q9DVVbLQ'\gicWZSbi:k"drJuI+j),ibo# +1!GpUc]UQ0oGqIJ$Ut]1+SohnHf&Z[[1KdG6/@lH8Re%s%[=_[kQkqU;)9eDjl_Q9?h>/N4')e:l_!BA +*::rq5U/pO'qETeim1(GBsRH0:`BqVDffFme%qkP(aj0Y7Q#*d!g\H1U1d0-32u9T#4\p-5SDGmWr+^\ +m1tDnX]Q0%$1P/]KA$$eVor]B.?2o:Q'\NAo_Hq5kb.c2t.R\^JH/pWn)>H#7G:DX;E>o&[cE%sPeP^> +AUed?5;QHu\n9VnVuh)aCp8#b]G9Pd*_og-Ep=g2*SiuC(5J^U-?ba>D>? +cLYL'V(PKlu?7HM$Fp+?FZ1eG]:=qq`ROZaR;crquRZLCW_L*9kd6s1IfkU*Z#[hOHMi2` +L3]]4X\:0E><]>?qD=?!ZiE7qgZNh2rd$9d7(%zzz!!!"Xb5VN/E!_L~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 207.31862] CT +[1 0 0 1 0 0] CT +N +-1284 -71 M +-1284 286.5 L +319.75 286.5 L +319.75 -71 L +cp +clip +GS +0 0 translate +320 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V>Gt6o+33oBoY6K`>e#k'2u +0^\m!X(OhU^9/ +\DOW*cc(eYa.;\V^\?s>A(e/!l+tW"n@n# +:?#^;H2[S^9Z!5UCXRDQPItVYVX3"%-rKS6Ci4-nm>VRJiVQ>ME[q'ikKTLJ;6\-fS2h7HY[PG(1Bldk +jCQG.Ek,GdLWD8X+0R\Rr-7#-HhVFS<%R^Jr>'_Nk"WL4d@s(O:1DDsX&mdIKdps>jd<+HB^rX]chP[L +G#[]"nDV8RP7WHZ+$P,Lo^(6]rLrjfK8/hR4cC_b9UV%pX`T[I4ErP,TmQP%s7q/NB$EqlP-!2%\cVWn +gD>(n.%*9lp?gU,rSG,.CYZF>nPeEN]mB>i2/6+%N0E^]dW\^Frl[#tq9_gLgbK)e;5)lX'%/S+`/GQi +i_Z40H_hgAIV[="r>(tfL*8p='HNu)1.c[fFp0484F$?^oATu\UeeUJfo]X]#:7[9KmZ)dWN6*h- +]Ut"$g3?8emCsW^Pom-,hsSiti;Dr7l!?GCqP[]`f<,6--krVPXA1q&o6E?]Zo +4Op2sO!lEJ]mp(]J,oa]W#=?ra'@^r/*aU45t773*>2_3Kl]"^cCFZdE?L9c7F#o(h7In"j?+T4p5748 +%2ut<=C#plh"9(R_1U2m]u!2ZCO=U30c"*932TbHbRfZ(H7/hGq,"qCu(Od=+*sGX:$M)2#"'rRTn6dP'l- +rRQLJ;((S:#5!g76,,:M?doDYXTABp:=*ngRXhtHgD^&MpZkiO5:6OjTO/N3KoUUfkSH;VFeN1+kgtN; +Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[I +UL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo`] +>ea!-gph@)T,[`Lj7`6(ka*6t^\:44gpme>S9kO?43a>J7GeNpf#mM!1AZ-e?g-_* +Kmo_j?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWP +I\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN; +Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[I +UL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo`] +Cqm+I9lde7HO-Q3c(3HTh[ITWb4-9'qK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t +(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R +3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(c +qK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;V +FeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kW7I:.t3[e0'=8Wl:uE`FjW,MZF+`NKoCIdkSH;VFeN1+kgtN; +Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[I +UL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j +?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPR>6f& +jMK,.a!u+a$Sr;6X2p!_LQ5Bq_TYBf.WSt[TjnhJ0_t+R:rm@K2%j!ih4-i`$]XG8WQBmgE(G:r#G5:^jK*De]_aD:JVsGloH1L%;OX$(): +,lk0QttmG#)X(G6[gFD.N"4ET/`Dr5a+g +_jtSSis]%^Frp+L^@FoA-r4/P)NBtDr/-3/sebLq=XqTtp'gU:sWCU'9`BC]9jhlcUqBB$jd4*Ks]* +mu^A;7)3UT0=8+itBfNu0V2o&\'prFs`X9`kV1N'!(A4n=UHorMgA?+P,,* +ZdIj[.#qm#[Y2j[:=<ZLB`hL(.W-rH:Ucs5V.\D[#k&h&GP'E +@82Aa,,+CqHK3Sj2\?*Yp+Vkk09B?6!TF17b>`(rr+psoe3=B\r,M.3O\;.[4AuhgY1BDc_!F6ba8n_4 +r=OXOQ[LlZE#Ah:J4>REr@k4g9`5'T"'L[TEb4>5-fV0lBQ]=z!1<]qK$Uc=~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 173.19932] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 357.5 L +1603.75 357.5 L +1603.75 0 L +cp +clip +GS +0 0 translate +1284 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0VY>/>J*6)EiL6*Mr\,p40!)ZUG_Lh,++IeJB<7GE>o`u2?!@6\Z'c>A2Q7N?1%eJl-H,%)qB:^J\ +G4uR#_rl6t2a]YJ!rr<$zzzzzzzzzzzzz['BuGCX8E +<`<:.;;<_W74c8dL9DMC9VnE[i92U=O-hEm?*[KLAR(PRnHf'p2ElCUPoWkEHTG,,ZVW#`5(/aJl*q>F +:q%8dQZ';\:=/6r$23<7I?g9[5YpKTO9SL_<#uV]p.2C<[M@kF>F:8E<`<#pGF0&NLT6AdafD:+7>3E[ +mf:ON=DWS;LLVKul?QXCPgNp.Is;<(%$9O])iP't%W]PB0K%C>Sr&5uS;PCeW&7eHS9)8ZSd/us6sU@e3D;pa8!FPt3>IZT;/ +Jr6$^5X,#Oeklfk +cIfhnnEY+j=aD8!FPt3>IZT;/FYZIgY7&p`=&iXKul?Q6e,lVf?M]C6SaFBal +A0i66`r3h.h66`nKaC&ig\h>N^n7+ae]d]s +G%dG^-aV)Xe'_`Uh6>qF@^i]Z(>.b]rEa/r=\\G`M^4(OU6-U?#BO&/$uR:g$T:X==rl*[=;:=o0Pre9im++J`o^i(/?+XD(X,,_/acHa_D\$lkX%MfT6Yq=2W?4Ng"dqNQ732 +a[D@pVk%c0H + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 173.19932] CT +[1 0 0 1 0 0] CT +N +-1284 0 M +-1284 357.5 L +319.75 357.5 L +319.75 0 L +cp +clip +GS +0 0 translate +320 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"/k;+.!_*68)B,eJlSb]NBssjD>ssidVMc]kP,+#3X(7A=-:sAe +bTo"5^>&,F3'FtD=e4`$4&=3"rRf*ZR63n`f&4-XGKFgHU+bUCn#U+j463n`f +&4-XGKFgHU+bW7Ca+TP9iVKO8Ts3X8J(TZa_3QJJ@4uX)>FtVF]uUq)H/]q>,$2#1M1=09>eu^Rra@I# +"^@SnLV:bDCjG"^p**SBjb\K>Md4)a-iRO-EWo[=pkQQ('l80T+8-m[YiU^kfSa"NQ7)Z?06.5#T(n)S +a;&N:iZIk<=,`)s\.4)FGr"(1rs/o?G'u?frH0+^+b-0"(=6D$''QM&oTt46qJ--1,p@j=>!RR/8>*^5 +csICukcY0^Q\5Ms182$`ZJl5oNoLCsZMsU@%FJ +qtF;u>*:5g]6CH\XY&2jk3(pTSkD^L5o&TJ8A,1eNfH.]3eE/'+b%2)'6 +Bn?7e5^rk]K`:8`hro'fd;\+5rpt[.qXWak2fIR6k2tgej>8\M*$-.nP"1MRaiT^/P,t`9jF$Xce`l=t +9#=R9$O]*\4o+pLaA>on=GI#(6W]dkT`aGbbD5C]]VYu8T0K7efPrT"`e^D95o +?hEOHlKYpj\Z)R-==dKP<-3SZ?:A8,9*MuNugI$]niHa0rWU,Yq\Z!eC9`0Ag#G6 +[t"NmAnLNDgYCRPIb]qj#o,UKo#.71"U4k4+S,Lu2\?hlm-NYZj[[?`%@DqP$_s<)C4C_?AWoI=e?<5o +jA/m:&(u==3O=JqZOQP\;<P?8g')(`RT1mPi5^3r?]1rM+\c<-@TH#r@&8?#6V,Qt2cZ!t\'4@t^e +C[`.f_HEt>EW#b1F",6To[e]9qlA$m6"t&4B4s)CIT8p:Imq'&D+s0b_Ka`2>E[9]*tmEX;$dE-RVj#_!OC:/Vq'SkiD$TK7a8Je.3Wb,d&[MH[:!;2@QQ% +N:cd];L]hq%PS8j8ZCQ2$fg:-(hZ\Xsj +$O[Id4>?PWEH.,1Rg\Jl]=[b-L>);CS!of8-O+&0"e@.n.e<3&eV^c0UE_EF&+akd7R#'e.bk`"6.BL6'A@SYY^sM)Z7R.:fon>6@ft^bu8W3 +L^]lY1`L2]K>]!*9UBS*.K+B)$A"cu[CWqQ4rr4,a"p"Ed+!d?c?sJGmA=_>3:tgSB`"i\1B78a!X9kd +Z+n&'e&#PEqYIeLq<'VN7kLZ4s*Df[;[)j)[aM&,`MKEV,$1O+:g>fZ8kB`!/.pQ)^,*Ok/S*S]"_rbOm-rOqOof@c:H>>&5XIb)lMf$Ib3k\ku> +gL'pBL\eHd5s]uPTYCMj3W>lXqJ/B[7WZ;#WbKJ(TL8;Gr)U9:2 +^?2QV^1M"jCMIUG,es@:"R4Tg^7(,t`o/@T%aq]^X?NmDDes3&]qDlt^V9e1nU3M~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1283 285 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +0.941 GC +N +0 0 1284 286 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 286 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 286 re +f +GR +GS +[0.48 0 0 0.48055 0 381.51931] CT +[1 0 0 1 0 0] CT +N +0 -71 M +0 285.25 L +1603.75 285.25 L +1603.75 -71 L +cp +clip +GS +0 0 translate +1284 286 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 286 0 0] + /Height 286 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WcYHL(s8V'TY7VY7&eXCQ$S,Mm:dg13RtV+SQn4-A]$a;%=D/;h%02jcJjD^-^P<;imkhdQnp6kNS=/\HRCSBhL[]O1kLoe0pi.tRgUD)srdX-Df<8P"pJ[_(#V#1+ +s(M-?Bc@eL6:f%oLc?-7bn_YO3@0B@K)X5nh9Vp7<(%':&Z6i4<)bb[QoO8n'LLN]s/a];B@Jgh,)),7 +;\="!.[r"`S9%:0+mmSO+`%NROH>8s&TK9MRk7=(F%Qc_m=GYWL5JMF,iVckS;PI$$PV]nP!V]$AI,K; +,#S]'3#*%'U9`Ih`&X`k&COps;+VCUri`&l8>F7D5PS?H!AIIse +MF\m9i`&=9MTTN='BbmK6:ah*n?/aH'LF!=7Du1(Cpl$;c7(2qe6iPPW/)-S,#1SFWMjXA0QGAe.>7?@ +&.VVN.>1*ZMMP2.fkb*VS5W;kl(]gMVBSnDM)Z4^VkUKM_SeI4U_&\4*2auY#`1MU&Z;A/fttZfVkiPH +O::(^4^J_'KLD>fLf71(Cgl=n;c<748@EJp*j:AN66]/n`2T]clK[Zigt^YnDa/O/b0!=rjm1O%5C'(W +9Up/rMI$e7&C/_I'p2$X`2SPP'f?^JMH[-jmr.C8T3kc*g?ctSIemdYrr%g,:S0haocghuM/uE`dBau6 +_MNTI,'"!H3#*%'U5#YHWFE50:/9C+onA*Rrr2nZOc^*Vr9^uNMCWW%]NJM8m+J^Ta,Z]QH#ei;C^Up> +ScCJabH2I=&il^H1T5XFpD.2Fo +;c<7"8;;)dhQ2NT66]/n`2T^)iPUF[Ytdtd1`ok'=XbC0+!4j>nV%>ecC?oDI.P@;)o\)#Mi4(,Oc'pXrDrE;I=4iQ!rgOW?H7^D7'J;bVS/'ZtU/m[2GIt.(Q83_FmBT%(M +$PV]nP+iaZ15*j%$PfoOKLD>fLf70Oe^`4Gq>'.[roB>Ks#-\X>F7D5PSCeBOtBqI?DrZC+=,rG7%B.W +Ad]:8Sh5hB)Uh@L<>.O%VVA[@)IF&#=:3#*%'U9`Ih`&X`kkjFOKiP!Xp +'sVD)7%B-j;c24a@H*eU;KQnLL@Cf!ielA]66]/n`2T\O[P2^H.4Mhf,[5N>FE'b9,p4S#k,%$XGFP; +,iVckS;PI$$PV]nP+qh?15,C.pJjUc#`1MU&Z;A/fttZfVkiOjODNmhf6nC](.'3?7Du1(Cpl$;c7(3R +/0mTm/\8#-g-nQNWJEMKQ-C'kc8d1>Q\ArX[mcW`7(W_]3#*%'U9`Ih`&X`kkbaFMiHFI_MJ+4X&Z6i4 +<)bb[QoO8n'\sId/_hu7<"Wsm,iVckS;PI$$PV]nP+l_Y15+D\CFZ'lKLD>fLf71(Cgl=n;c7^38@EIV +F\P<3Q]R#%&TK9MRk7=(F%V>X#g!WqYO8?F[M'9n.O%VVA[@)IF&#9Up/rMCo1V(2g`:W/+1k,iVckS;PI$$PV]nP+q4b)8_r;l4q\i +KLD>fLf71(Cgl=n;c6RU,X](/<4X87LQIF7D5PS@++aUmks?dISkEH[G$crtJ5aAC+c6O[p<(*76)N&fEN7Q+maa1Y7YcpqTgl[_]%Bl:kRk(YSHM-Ro +o(&8/DpFn8f@SW``M\l9@H*eU;H,7o>IXc8Xs,ojOGHgIE@P]_ET4p`cqs>Y4*U*,)tq7/([J7eHhQgA +^d.M9^]+)qnDV9kZa6.7B3U`_kEH\2$HWkI9\$/!N[5t1<(*76$6MHZ-l2-e`'j3ZQ'IWoOn%u32-2C` +0,K],]6A2jn(\[*c8d1>[6h0Q4&`O:U14Ld3#*%'U;G-u9X.\A4F#6Li5($N042H+iK]HG59:gc4aHVZ +(LMQ0p$1'Z^9dUphck?'Y/A!`$k0N?.F8MR'N46.DYJ52?Rf"eSP2SmWJJ&jY,F?^Vl-GN"hNhd@.j3c +?+P,lc*4m*mp:"qi8ENp?[Xdd0lcU7[GnL@Vb_7$e"hCuR(2J]RhZej'Tu6!GH;-O6miW\E@N/-73?`Z +L%!m3>?lI9baIVgSXlHY/$WfZM=%$$oM)Z4^VkUKM_SeI4V!-m$%,n5MjGKlI+VCUri`&l8>F7D5PSBZ. +OtBoRSQlaA+sc/I7%B.WAd]:8S(f;6MMP2.fkb*VS5W=-Ce2!RKlh-"E(+nK79#V8 +Lf4n*.Opo>9Up/rMTuOM&?`1]WJF.[&ofBF:.8a#Ko'kG8^TOAftu:i3>GD*66]/n`2T\O[P2^H.4I;' +,X])i3i!t":.W=&Lc?-7bn_YO3@.+LKObHl>)-'FeHq'l'bR@=qkQ4'p:.<+# +W,/mrER$!^3[#bQ\EZ,K,.`;dLf4n*.Opo>9Up/rMRJ:N#^:j.=%%6D&ofBF:.8a#Ko'kG8^[6nCuI"n +p58&D66]/n`2T\O[P2^H.4NsiOs/U$gl=oK,iVckS;PI$$PV]nP/7t]cL$E%66]/n`2T\O[P2^H.4Nsf +Os/UdX5-hK";EQ]u5/-"Kom6S\R57pI>U_;0L"QRR.4OsG,X](G\K-f!8AqK`3#*0?AnHq>qV]2Becu^, +4F$\mjUJC8*?G+Lb:hbfJ%s5[]MmG@hZa8hgAM`L#(%4clPA?`s&8WAgCYlf7%B-jlfF"mF*2eT\lbWE +,&Z5@I=6O1/mVYqD4`q9;I'.pn`/WT4tk.:\F.+7O^ZR&nk%dmk&s-"+VCUri`,Pi-X1h@H26*tA&&$> +CZA,nR(_IKP/7t]cL$E%66]/n`2T[h2`J4jYEAhS`udPGr:Apjl+d2d_[j.YXn]f_Vb]6ukI*'ac7(49 +-6tsg]=ArE6:f%oLc?,tr9#G=h7@`Ebp0>T9rh]9BP>fY^ig`qFfYYS;*K`ZZ`2SPP'fDLB +F_Z/G\G>9!^JeihF`jP"f\"i-!sa]J'T[2+So%oqS5W>P5tH`Lf5j"tKoh*h&TK:8R$sHn7Vf8Glh1+< +-VsU9qjZ&ZG'8%>A7T+W@K#*SrPIn1kEH[h0QCu#U_"/Y.O%VV9r"JY[9B^&2:$o-K1ej1NQ3Co-]:Kl +Vl//H'jI'=iLH(*m#VY7?Joup$6^"Z,2uSOC3=ZAIX]#AJlC==`&X`kY_@pT4!KB?Lf4n*.Opo>9Up/r +MGcesKR8C,&LiB$S;N,$;;#boig`qFfYYS;*K`ZZ`2SPP'p[#Z-I+Ut7YN^t<=i`&=9MTTN='BbmKU83kZiL]%P6jb(LV]t,"<&RsJ39QM?[P2]=8#-\/E@N/-7:ifZ +M/uE`;+=*2E3F7C,)),7;\="!.[r"`S9%:0>F7CZ,>nf(3#*%'U9`Ih`&X`k8.WfRmQk_\U_"/Y.O%VV +A[@)IF&#;8$PV]H6jb(LV]t,"<&RsJ39QM?\KMj-]ufYt'LF!=7Du1(Cpl$;c7(2Y@H-(&80K8<<(*76 +b%;"pkF@\Z0dK,rB=T%F6:f%oLc?-7bn_YO3@.*p8;;(:F#tS"OGHgIE@NH-/PM[U8d\di34C,1#`1MU +&Z;A/fttZfVkciiaUml[DF^K6&ofBF:.8a#Ko'kG8RVu?kB96#+VCUri`&l8>F7D5PSEYL$t7qO80K8< +<(*76b%;"pkF@^P(+.>oL_N0":.<+#W,/mrER$!^Jfu#KHuqFoM3*Nl&TK9MRk7=(F%QdDQoMQ";@&+= +'bR@F7D5PSC_>aUk>km>)HC,#1SFWMjXA0QGAe.;WGPgl=oK +,iVckS;PI$$PV]nP%&VY)8`b$4!KB?Lf4n*.Opo>9Up/r$nf(3#*%'U5#YHWFE50:/9C+onA*Rrr2nZOc^*Vr9h2c%NRTPs8MuVb/M?=F6Chsp=X)^?G5(Z4kmil +3N&[WKo'k4,)),7;\="!C,g4m`6ck-o?B@@mM$W6`JYOJ/mDI]ftW#dlNchEgUG.e^]!l\B(+Cg-I+Ut +`V)YSK\;==&LiB$S;N,$;;oEJ&&i"KR$_PN;Jb4LOn%u32-2C^/`"*eGB%W-q8hjdC2r@CCY(IkkI*'a +c7(3N)3Q/e(>Op@+XH&H7%B,A1G^i+@X[b(60j..%j&B2q=5p5mb$/onTQ'=I(7i5qf_Bgr7f$&5eEWi +h-c]q[;Os/T+[FLG!lZ\&Z6i4HF]d4>b.IX/f&-*Lg)fS?XM]e9:*(._XmBj`FYNeB5qu#Ir\B'eunmg +(aQ_SeI4V"eIkL+o7\G!lZ\&Z6i4<)f6S +S(m2Z8Dh.0qnRabK7ee\&)iSX0H$BZ;V8k)\K-f!8AqK`3#-l>k9jis\ohf5,HX1)X4?FtB^riD\T27C +n(\[*c8d2iefbpJCkpaW.>1*ZMMP2._l6S4L51PRDf581PmZR3j,G.![E\G^$PV]nP%'?2$t6Tdm>)HC +,#1SFWMj6?C8r,Q@q0$AHMtsL-iO#Bc"T:%4$,2QZEUQ0r-<:%n\B\Xa,_=WrGR`=5Mk`igVUlhVkgPN +M/ta&Koh*h&TK:8)BBj#eS:*$C,@n0\?rWmG^OOPZ?To+HFinCX]r7mr%1HDHtT]99Up/rMTnICK\8Pu +,>nf(3#*%'U/n=qX7PrSbnZbc(+.%m>)HC,#1SFWMjXA0QGAe.Fs/_#dXnV8#-\/E@N/-7:ifZM/uE`@1*b3G!lZ\&Z6i4 +<)bb[QoO8n'WiMr_X*<;&LiB$S;N,$;;#boig`qF1da$Ofep5`;@&+='bR@F7D5PS?PWL+ko28#-\/E@N/-7:ifZM/uE`@1*b3G!lZ\&Z6i4<)bb[QoO8n'S\;]DF^K6 +&ofBF:.8a#Ko'kGM)0fViH@Tr+VCUri`&l8>F7D5PTW>\/8dtR&/&t4,#1R9Up1H0!,*$BH[tc +,#S]'3#*%'U9`Ih`&Xa^e:uj*/0s4S6jb(LV]t,"<&RsJ39QMHg$FK*$&LVVO[;UWWJEMKQ-C'kc8fJ5 +[eQ!466]/nU_"/Y.O%VVA[@)IF&(DcXabb^OGHgI.>1*ZMMP2.fkb*VS5ZlVPJ9Ui8AqK`'LF!=7Du1( +Cpl$;c7,CXaW^RZ87YO/.>1*ZMMP2.fkb*VS5ZlcT7>VT8g+eE,#S]'3#*%'U9`Ih`&X`3B)=sjUQEQi +,#S]'3#*%'U9`Ih`&X`spRcRlq,`5;S;Ki\,iVckS;PI$$PV]ndVA^fhTc]3-mURHL_N0":.<+#W,/mr +ER$"-]3E)\66]/nU_"/Y.O%VVA[@)IF&(EXF29pV_B4J""r37e8AqK`3#1i+lK[Zigt^YnDa/ND/PM[U +g<3ApE.a7.;\:I37%B-j;pd33+1,4R[dS.tc_"QJ>2%tF>F7D5Zj1G+JTnST)h^VLMF\m9i`&=9MKW`L +C9!Z1PEV4'H@941Tte?lA7T5/lYU(iSC8Oa`Vti-iK$;LK8N2C,iVckS;MLT`/,-p6UT*Wn(bHPXPF[G +?>Oeu/PM[Ug1@_!GnVQEPE:T!&LiB$S;N,$d>iVLn=eLC',)&(7oiYkMFY\,a#ElG]6M^2d$/b[F2KA# +7ALJ-E@N/-76<7\1K,j^#7hl_5'ZST_98/uDt[8drAkjqV!B1*ZMMP3YNnD36>l",oNOe!]V]o"EM)Z4^VkP3"S"&0s +f1>el=i<=uU_"/Y.O'S.Lf4n*.b^$):Wg7kldhLp>(>c7(2q7%B-*+XH&H7%B-XlI;e3_o'C: +2uQJtp4.3V$6^"Z,#S]'3#*%'U7IaARO[Ief[s<>?@VP,8Ul5>lSMqo(+.2%tF>F7D5PS?G.:.:ri&ofBF:.;e$]"0pe5JQjuDQnhOF&#:M,iVck$6^"Z,2uR$ +2HuP.kEHZI,#1S&&/&t4,#1RF7AbO[;UWWJD6T,#1SFWMjVs_SiF0 +'LF!=7ALJ-E@N/-7:l'&-;I"*&/&t4,)),7;\="!.[uJQE@N/dOGHgI.>1*ZMMP2.fmJI0;\:I37%B-* ++XH&H7%B.Wjs*QiMF\m9i`&;Q+VCUri`&l8+(=/.6jb(LV]o"EM)Z4^VkUKm#SX*[;@&+='p1gR`2SPP +'p[$6QoGhC6:f%oL_N0":.<+#W,+Z7`2SP,66]/nU_"/Y.O%VVAb-K\:.:ri&ofBFKoh*h&TK9MRu]7C +.O'S.Lf4m7KLD>fLf71(I"[>T&LiB$S;Ki\,iVckS;PJO,>=Vb80K8<<(%':&Z6i4<)bat@H(6S$6^"Z +,#S]'3#*%'U9a.g!!%P0dI*8.zzzzzzzzzzzr-tcJX)i?'Dr89GpCj7;<`W7YMOm%^hX2aX2cs6eHM$D +u]:T7,EjHu0s!K2Zpi3!]R'89d%NhAs8IB2K3iT),O6lNOult>R\BLI:/4R8lh,scL#Q[]aN-oaT+i%3h07`k8+ll +[qaEr3Mi6cDSbqc[c\h>KmcR#='mHJ5^17VX$cT/u,+bi:Y:_I]^NoFJ\/k88p%]:148%?H^D.pJ]Qpj +YGO=(iB?-#Q4*gC-]:Te&;)D4aU+R'n>qoZ+ZQgDij<]#]'.6N`%1P2P]Q<72nM,O`i^FfY'%O:nZEeu +[rp/1>2]jJTFqtf-0-o9HXJi:g04,LTAT2V8(1nB.Sio.h0[P4=YJ:&tI/*3tltdW5&lhodJr1A4br9R +u^0UZKjsq)?UuFhjJ,fE'inprC^A;ThMXck!D;)'80=fP[q!b!P!)?3]1`n/1aMfCaj5Y!(<`I8jdaHs +`G8D&3RuaYRXqhaX]6:<9c\dU[fB[(pS@N\%8oYZQL"WHejlPLaF77u!G!?Qgn%sbd`P&5Gk)C<=MR6R +eV1T6Umsk@!I/*42rSRViIqW+B??3B#RoSMqGIj31RGn>Di5(&s#7hk?pda`Aj2XSZ-Vp?3ieoIm?G5Vco%8YjV-:"po?TZ$n])`jMpEVXlh!Sd.HsPnGundF%,s%qCZ'be?5];e]lFd(CBmQ(j=l[P:)>5;uG; +aAf'mk]2p773&p]`='nZq9O,2QXgfCB5(2-\P@bW6?G3r=+30EaJ+Mes]:T[8Z=_(#aiuGS]nO/\NcQn +rp$gqX6\c/B4aZo$qtKK,/l^'co('%qa+"![f>a;g0Y;;umbQkE[52I@1ro(Om-L1E98)]BGk'eP_hX/ +:TIVNCc^m9=(`55QjG5h?p=[=$D6\f.jA++tH(2NUK+te5h_)pli8C8,aY*Mdj,]g^rcrKFRf;%P5Q"? +YeVZRRIJU>,:QA*LRW5Y!UoBlq"T)r:$^,MLRhUS?!V/!2E#a:^k(JU`T-?7C/i=&UISP +'H0Y(gf7$UJ44X+e.Q;8#BKi`7]Nj6SY$Sfq`b.`JXC!\gEjJ^1/+Sib?/l`N(G@l9p%jk":\stc>L+a +Gdc07qnI"7Mn`.+YXmm'd@!@"50Y;Dtk0;m-[F*CLQ'IV9Y?lYUp".0tC[^,nC\RFp@P1oqd.i(Kimgo +rceQmoctAu!p@@=gG!@W0A,V;TP+/"G;j!F%,q3.Ve7uD2QQltM%f5i3m)Z;5[l404DY90^m9 + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 381.51931] CT +[1 0 0 1 0 0] CT +N +-1284 -71 M +-1284 285.25 L +319.75 285.25 L +319.75 -71 L +cp +clip +GS +0 0 translate +320 286 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 286 0 0] + /Height 286 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V^M@-:"39r[_+@n%r!,"@U7n;j#Ciir<(,dk%dIG7'a-fjE)I7@E#1ge5uB%2"VhUa8o[[H&8Dpo +M'=-t#V$TW^(1\1;"EXUr$6`3_s91EHd9/^IId#UT6!f$=^>Uj9W-A5&@QcGd%U.Ep<_79dcHg&Ie1fC +^]!l\kKfbBHes2Fh=3(X5KE)0keRKG>^PY!='k^dUQ8aX[F\c8rEXX'[=7cIqsRlVoZ8JQXIWnqq)8[o +HXU>+q/ZHlrDpT1qWm/\I/j0>fCqE%giM6CHdb\JXhX8$T7/hKq!`P"qU$nlSULL^oK/uC>B0N3l1#3( +5CS"AU/tZHjbbRk(jk+cl`J]3rJpi;5Q:GMJ,]9BpYV$9l;^nk[Cj/bIWWb^B:jZ0rQZkj[QO)u'8QU+ +EH1NTcT\u-ZHJhclD`<.rSE##mGeY20"Uc1CVTlFlh-HXfC/(tDl2DPV;J#^rq^Ec.+b%d\$KE)2rB!= +Sn`KJr+kpdVt@UID7ILO&cL!(rU+b'AHr,$F!jbT4EKZ/?N2VRSQ#Ar%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EF +Di9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>oL^JfkP",^^hcsD?XikK +d4[NY[cj'JITuG;:;8CW-)]W9rK>WJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?- +I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs;D:(!srO1pVS:4]7 +92<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>WJMgie4[me'mo\6*I +E]`Z4atB)U-LDt\mk!h\*aKYNDWJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q` +^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs; +D:(!srO1pVS:4]792<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>WJ +Mgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.\'of(m#oJ$dN/JecGIlod?T:9VXD4;o\6*IEr'Y# +000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.n +am$9'^Z>%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG; +:;8CW-)]W9rK>WJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIo,g@(83 +17:hi6X\eSfXX/V1,.J4rKin#>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro +/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![ +d4[NY[cj'JITuG;:;8CW-)]W9rK>WJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?- +I`^i`7DIpUgS/%qqJFV53W6DMQeY@5S[(m8P=^*,loPS:[msL(At-C3'%:cWI`^i`7DIpUgS/%qqJFV5 +3W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.nam$:Rb`fAj>0K)hX/O"]PI$-MbWIndW5ZuEZ\hr14?9I@`QXOP +MH\4d3NK*tb`fAj>0K)hX/O"]PI$-MbWIndW5ZuEZ\hr14?9I@`QXP#d44u/^DnIGU^]P +*PGMRDrIEZ3!/mPpUieoIM2XeBWmOe>7m^qr-KKo+U6i(@U1KMo:pYJ!"9\).Ppu-uQ7uj*'IC_/W"6f +[Yo4'skHKh9r@WT(\mt>i0>882l.E7N\))Gsp`R8_]NcKD_6gc10>IF*D=Y__rUNa#=@M8q4#Y-sfD&VLg2! +lTs8;KK=mXEqn+9QB?+9WncY^/Ip;.1jXDR6hIq$;FHKUiC`o_LK>7Ym>q2RV5jk^gbV@>?]bVRT#E5N +,,IItq-(Hq`hdoTn)oltKMm+Ledg!;RXU(9KdE,<8ak*q^OBAA(\HgJ#WkROm-1]Qs^%r_:0"O0%bCfj +-`If&Nb4E]r=V=-EbjG6gClScubhgbZSi=[fYr:9X\4C4!Fh/T`mD>l!,/[sMUr:ogTc(BLbH+[QJN\s +:MODXgbkB3AGq'S)(U@I2floB(F`45Rq/M5e&hgJ"U*rYK8hhm%KORu1`b>gA7cf3sDg@Nq$A&llc44b +Aln1UlEXGgtFr:.i$5QCcazzzzzz!!$EPr""0p]0Z~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 347.4] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 356.25 L +1603.75 356.25 L +1603.75 0 L +cp +clip +GS +0 0 translate +1284 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W:InH#(r0RBLkp$!W^TH$&JYn?85r!aEtL]($]0[Y'.mAk&J;-sB][BrZk/'0+V>;%/L3ZWKsViS +[#g(L60&e26hf@GGF4k3U[Rs%>kE;B1WRg_L6(4X?cG;pIK0?Jzzzzzzzzzzzzfpt?W^9+MSI!iTCobT +5c.+Z.\q(h9:!WW4NI*BG-!!$glAmkZ>!7mu][JJKN`IhM]oXe7NIX]"5^[tLuZZ'om\[f8IkA94j!!! +"\ah)ccH@'utD;*M5;t8eiR5<=@e$TAPZga51=13&)rpf$h'.6P>laL`cG5M"#q@f1$n=rL7/mc1mT!l +>G]f>2A04.b&>lXj)aC0+0EqAG<`HZai`f8s9iIQQ2mdBLP1-KmO?u8X:/6pK2]6I+Z +os;]N>iku)AH\Yc-sUU.;uk09Cin)*$;It)I6(nXlP>lXj)aPldUdn>2iH2$a_`PKlm5F504cGEjm?*g +lDj[U+'(QTOr;B1)]p2,)N>]\i=J[r>%o#URohfnMD=WFNV2;H8q!<<*BLU"+hiSia&%[P$$X&lLiT@V +?*6UO66lUOH8Mh>*7?7hd%];Dd8YPo&I20)t&!!(*<>-1On9GA`=]mKM!SNGHNd8<-;/tI\\IJQqCl#U +oe`PLEfhXYBkk5"W3okQ0e^0DY*p?YrBf*)-SJ/i05B);E%COa."]tM+k]=YDUF6-PLIX]$1,fS6li1< +)Yb,Oe/)5/VX6a^S6)Bg9n2XDV_n3FmIU!4*D;CT0@[,/mC@EQTs=h3]d-4n`-B7Ntt +Ogqtk0,rSoHai,o&#!$I'kn)'K3e[JM:W$P=Vq,=J;qQ"$ig:C7T69c`lH +,4B4kHI63$ucz!.a]b!rr>:s$kILiT=C$F\G4L+XH&H6kVg1:.<+#-oX4]S;Kj(8AqK`,_SRUMMP2.DK +_C,'p402Lf4mW#`1MU&Z;AYlnEE/+pfD)V]q98&ofBF:.;_H@H(6S&jdd;73j$%i`&=9MK6&@`2SPf84#kcWJEMKj^s:);\;S,M)Z64KTM!g&TK;CbmkDb,*EKME@N.=+VCUri`)/3*@=NM,U"Gm<(%?c,# +1SFW@0\e9Upu26:f%oLa7XAS;N,$;/UQE3#.R/OGHgI8I1/4'bR@6kVg1:.6EO,iVckS;M?n_o/O1,_SRUMF^')`2SPP($0%_M,P!X#`1MUObAgP;\="!b0:D0V]q98&o +fBF#pBnY,2uRdRiNhnE,`&307id]8AqK`,_SRUMMP2.DJ!>:gnuD@,iVck&jdd;7Du/R2mY2XmSf8[&o +fBF#pBnY,2uRdRou+M)Z64KTM!g&TK:XR$sK?qXs0sCWsZ6\PGd`8AqK`,_SRUMMP3YRS?RR+$ ++N-`JYN_0/EtLk<:0u+VCUrOq87G.O%VVCGRh*FmRbc@a6h=dff);Oq87G.O#0B&Z6i4<4#g:$O[%="8 +hfZ4+$\&=d\>jT9=khLf4mW#`1MU&Z;ARR[KR(I/*3Xcqs8J/51,p:J!lDs%dJ3!6qp[Dd!nl(;>4op> +b=4gu'`r>(cF&3J)(2,prD*#i>V.mdkl^6c6EK9[7%(1ec>dK.erCj=dZ'@_N9G4R] +4hEiW&t-/,4OQ=dZ'@_N9G4R\9I`N1*F/,U"Gm<(*76f$#[=e$r1F6:b7]^Thuc.O#0B&Z6i4<4$7L@@ +G2a8eOJ5ADPSHoi+ + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 347.4] CT +[1 0 0 1 0 0] CT +N +-1284 0 M +-1284 356.25 L +319.75 356.25 L +319.75 0 L +cp +clip +GS +0 0 translate +320 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0RYt?6<'F%qWfg8(0$S)gY74`;ob!N>bb0E=)`7?m:,uRhNiWE:bLffIhL]e*X5\qu&ln'bVAhOKY +2SnaiGKn7fpJjfQjjkkj>05%FImlW.#Q<6cP/gqH1I +At.(d+'d9hIYPM[d:S)46m/*CHbDj5^rDtI-+7c_%UjHi2^CJ0+*jL;SYQU6.:97A>N\$k@aU:9rX\\1 +baj\-A\P'NqcQVLmHY-c;5;tVG]=qeJ-?K`Pkg8i8>`S2:[)"H0JYM@^iD%h^J'M1T_UAI=5 +C)>&#l4jXWhk8*QDie'k$9^QXkFAROuqn'b],4$2P=b3XI_b9ACthYs4sUDq\q?WNOo~> + +%AXGEndBitmap +GR +GR +%%Trailer +%%Pages: 1 +%%EOF diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_3.eps b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_3.eps new file mode 100644 index 0000000000..d80c371859 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_3.eps @@ -0,0 +1,1567 @@ +%!PS-Adobe-3.0 EPSF-3.0 +%%Creator: (MATLAB, The Mathworks, Inc. Version 9.2.0.556344 \(R2017a\). Operating System: Windows 7) +%%Title: I:/Entw-Technologien/Studenten/temp_kai/blend_case_3.eps +%%CreationDate: 2017-10-06T19:28:14 +%%Pages: (atend) +%%BoundingBox: 0 0 774 521 +%%LanguageLevel: 3 +%%EndComments +%%BeginProlog +%%BeginResource: procset (Apache XML Graphics Std ProcSet) 1.2 0 +%%Version: 1.2 0 +%%Copyright: (Copyright 2001-2003,2010 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/bd{bind def}bind def +/ld{load def}bd +/GR/grestore ld +/M/moveto ld +/LJ/setlinejoin ld +/C/curveto ld +/f/fill ld +/LW/setlinewidth ld +/GC/setgray ld +/t/show ld +/N/newpath ld +/CT/concat ld +/cp/closepath ld +/S/stroke ld +/L/lineto ld +/CC/setcmykcolor ld +/A/ashow ld +/GS/gsave ld +/RC/setrgbcolor ld +/RM/rmoveto ld +/ML/setmiterlimit ld +/re {4 2 roll M +1 index 0 rlineto +0 exch rlineto +neg 0 rlineto +cp } bd +/_ctm matrix def +/_tm matrix def +/BT { _ctm currentmatrix pop matrix _tm copy pop 0 0 moveto } bd +/ET { _ctm setmatrix } bd +/iTm { _ctm setmatrix _tm concat } bd +/Tm { _tm astore pop iTm 0 0 moveto } bd +/ux 0.0 def +/uy 0.0 def +/F { + /Tp exch def + /Tf exch def + Tf findfont Tp scalefont setfont + /cf Tf def /cs Tp def +} bd +/ULS {currentpoint /uy exch def /ux exch def} bd +/ULE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add moveto Tcx uy To add lineto + Tt setlinewidth stroke + grestore +} bd +/OLE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs add moveto Tcx uy To add cs add lineto + Tt setlinewidth stroke + grestore +} bd +/SOE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs 10 mul 26 idiv add moveto Tcx uy To add cs 10 mul 26 idiv add lineto + Tt setlinewidth stroke + grestore +} bd +/QT { +/Y22 exch store +/X22 exch store +/Y21 exch store +/X21 exch store +currentpoint +/Y21 load 2 mul add 3 div exch +/X21 load 2 mul add 3 div exch +/X21 load 2 mul /X22 load add 3 div +/Y21 load 2 mul /Y22 load add 3 div +/X22 load /Y22 load curveto +} bd +/SSPD { +dup length /d exch dict def +{ +/v exch def +/k exch def +currentpagedevice k known { +/cpdv currentpagedevice k get def +v cpdv ne { +/upd false def +/nullv v type /nulltype eq def +/nullcpdv cpdv type /nulltype eq def +nullv nullcpdv or +{ +/upd true def +} { +/sametype v type cpdv type eq def +sametype { +v type /arraytype eq { +/vlen v length def +/cpdvlen cpdv length def +vlen cpdvlen eq { +0 1 vlen 1 sub { +/i exch def +/obj v i get def +/cpdobj cpdv i get def +obj cpdobj ne { +/upd true def +exit +} if +} for +} { +/upd true def +} ifelse +} { +v type /dicttype eq { +v { +/dv exch def +/dk exch def +/cpddv cpdv dk get def +dv cpddv ne { +/upd true def +exit +} if +} forall +} { +/upd true def +} ifelse +} ifelse +} if +} ifelse +upd true eq { +d k v put +} if +} if +} if +} forall +d length 0 gt { +d setpagedevice +} if +} bd +/RE { % /NewFontName [NewEncodingArray] /FontName RE - + findfont dup length dict begin + { + 1 index /FID ne + {def} {pop pop} ifelse + } forall + /Encoding exch def + /FontName 1 index def + currentdict definefont pop + end +} bind def +%%EndResource +%%BeginResource: procset (Apache XML Graphics EPS ProcSet) 1.0 0 +%%Version: 1.0 0 +%%Copyright: (Copyright 2002-2003 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/BeginEPSF { %def +/b4_Inc_state save def % Save state for cleanup +/dict_count countdictstack def % Count objects on dict stack +/op_count count 1 sub def % Count objects on operand stack +userdict begin % Push userdict on dict stack +/showpage { } def % Redefine showpage, { } = null proc +0 setgray 0 setlinecap % Prepare graphics state +1 setlinewidth 0 setlinejoin +10 setmiterlimit [ ] 0 setdash newpath +/languagelevel where % If level not equal to 1 then +{pop languagelevel % set strokeadjust and +1 ne % overprint to their defaults. +{false setstrokeadjust false setoverprint +} if +} if +} bd +/EndEPSF { %def +count op_count sub {pop} repeat % Clean up stacks +countdictstack dict_count sub {end} repeat +b4_Inc_state restore +} bd +%%EndResource +%%EndProlog +%%Page: 1 1 +%%PageBoundingBox: 0 0 774 521 +%%BeginPageSetup +[1 0 0 -1 0 521] CT +%%EndPageSetup +GS +[0.6 0 0 0.60069 0 0.20001] CT +1 GC +N +0 0 1290 867 re +f +GR +GS +[0.6 0 0 0.60069 0 0.20001] CT +N +0 0 M +0 867 L +1290 867 L +1290 0 L +cp +clip +1 GC +N +0 0 1290 868 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1283 286 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +0.941 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.48 0 0 0.48055 0 33.71863] CT +[1 0 0 1 0 0] CT +N +0 -69.75 M +0 286.5 L +1603.75 286.5 L +1603.75 -69.75 L +cp +clip +GS +0 0 translate +1284 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Wd>M9(IE(7D2+#@i,9a!(hQo1HVNhgDa;7ueQlaa](gp?%i_ +pAGP)#Q$jahY3oH!se\Z'Y7^M^93Ss&NLT6AdafD:'p:tQJIX79OO?E6iC@\e$cK2R%(9pjA\NB7BP7[ +jIB/7F6I/>Kiupn(>f#Qn>]k'?eA,'#/Jbn_YhS2+g@YW-"lmh$2CALa.H=#Q#\&NLSg;c.>=0KJ<\_nP)O +Y"9c^E`#9sHliH[_<$KCMTDcq2-ZH=0KJ<H8G"Dbj8Ln'@DWG#.9I.9Wa +D_GJ3r8*+pNRQbA(+/47Nnk]PG@Ck;,:%:[X(^c62N$+`Qn)Ln'>ISKU]6:=RmHN)t5QCZQ)-_?rrt+O"3`fLSqmCrUrE$c>&e__U66\A8M?`ZNq93QG+YbGr,,_/h4FHqnp?gW3Sj^0E_Nul\ReEr; +_W769^5Au_1SLQO']MGFkXT>$^5X,=6\)d +l,bM>/j^a@r/*&b;&FaQ%%iEuWG#-F04$b\il'*22nNGS]6E`1fsB!fpY;N6O/BV*/POGM?%Meun@7q` +Kg]fH's6RH?5k*rM8<(%3>F-GS%Z#>qWcJ'7sA>?kbXVPqXi64B=tu29[5Y8B:bFi?CREs+d!^L70fIH +Oa_5h)AugO%$6@fWG":r>?`1upu,:]IDhCBN)&894;/=0Y%WQVt7P(YWL1LL4hrn)d\Xq +>\1f?U4eEFWMh[YR,!"[dlCb@G]sPirF!Z_]gH6d<:nGZ7I3&#k9MefHDSlYsd")t$nJko-12)bGH8VBK,7H@FtROq3Np#gGfVT]0L)_UpH<)dOpT3X`RjA\NB7BP7[jApG! +VqmKg^[&]>T:Ce]D<<#X&ig\h;c.>=0Y+(DFS9S(^+IR-\_rZM;'Uj'Lt@QHaJjucMH[??agF++F%kNb +]E-bt0)4L,eCoZTM566SWJI(%>-iq)'qWm"?38a^?<8cSjA`K);n)8-/7Kcb;\;#)[:Tf1$d\'"0*,kj +DZU`&E[k7hK88r"/7Kcb;\;#)[:Tf1>OJ"&q@^U;n"t*@-`<\fXPL5Y$]1C2aJl*q>F=4DkB-.D!]GTg +OUF?5(%lBK(9rtBWJI(%>-iq)*I:KDT"qCP]F:#44uj>e_YC>*66`-iq)c$ngJr6$T!i2?NPI32_^2W$U-QcNQ@1tZ&m&+Ln'?+Z\=fZSsKEu_As#"_Zc[&MT0>\ +*:E(WMic#ZLn'?+Z\=f(rsJj'mU,.A&jde?U@F\`L&4uj66`a!8hJk+C>/8E4-%nZ]7:UQ2 +&75/V((rVBO],+m[dXKinXX>$:#J1[.4'X8:.8qCqQ^eK&ig\h;c.>=n:ip`#tb]]_XhhD.3TsW.U#2f +/7Kcb;\;#)[:TfMYrVOHr].V2kEHTG,,_0C[G"&#,]Y@YVkVaYiAEF;b8upci4j;-.3TsW.U$noqSm4m +O])iP't%W]YCS]@p8E07:3')Y+XD(Xg(Z?Xi8l%e8E<`<:.;;<_IGG&b8rMZ^\^!gPRnHf'lB8)J,#L\ +?m1Y2&NLT6Ada[:hA>khs&mam:H58:&/$uF98_nJ5@ +M4BIEWMh[YG_Xo/_n>1Q?=V80-%nZ]74_jCf5)Um;\;#)[:Tg8>4+n,B5HOdig`n58EL +,B>8KD54Q/U>Nu'ckc'Hs'21d;b@=PS;LG*bklq)*us>$E^WYkbLtb/^2rJ_QX>2ek^0Ji(1-iaR-3AD +IK*Xo*9WP!66`0E\ +]5@Gj47DOLASZ(^Hct/^^HLsk);8,nqY2T:I.RB.;b@=PS;J_7?q=)NYIO+1#+$QT.U'p\gUFfVHh6CE +aYt8*If/lj?_@%qoX.\_O8gO/F7.ho^)mFB5Y0s87#;d)c+]$;jb%+%>`QmBE^`W;iENd/F^)@[`05+% +ET4oPYIui[/\K%%CO3HpY-5k%;*dVY):TF6F#^(^&/$u<;.BNd%_4r9K=Mta;pdr" +gpscTpIGh[H))<,kEHTG,,ZVW//lq[Ip9]XE]G>AU4qqt;qASL^:WhcPL$,6O"F&N3MhCjaJjt.bD0U? +o2dP7jA`LbZY.Uuo#`J0^[:PKQ.R)%.4'X8:.:C<6h4/d\/\@37>na+rH^D@hg4UjSXl=45Q:H%R5:&U +ph@/j3TlV>66`8;_UY\gNu^<*jBLfnT7?Lsd>W:XKul?QPRnHf'iC\bUmWmB;>D;C +86h1rWG"F:8E<`66`Jr6$^5X,.3TsW.\al(6F)F!R*-(%'t%W]PB0JJ+XD(X +U;VpVo%1fqalS>KMH[??aZ68`&/$u<;50D=Ao1stG_B=^aJl*q>F:q%8dQZ';\:>P%I)e&m"L;:`oW#H +MH[??aZ68`&/$u<;.9eS*B]!,ipdjP7?!dp>-iq)3N&)Q&ig\(@Q5N*h80rti!lS.`_/b&ftug?;b@=P +S;J^L`/or;Yo.">N/`]uZ\?L]Vj%`*3>GD#cllQ1,;JD>(^UqbAdafD:+7>3E[mg%#XXuoLqO#A5`D?[ +##@[iftug?;b@=PS;J^LRZOC_@BW9c"J_rP!E2j2[P0`)PS-;,V]n`l0;PM#nFqn$^5]hW3A$(P1;,M,;0KJ<\&jde?,"deTUmZkf,$pg;=2[A&W&7eHS9%:/+d#\8-.X37 +fo+K2?@au4Lu,,02-ZH# +2-ZHM(-;?sP9lPuQ8%/POGMV1arOWJD2Q*R`FhC_<";mtt3V't%W]PB0JJ+XD(X +U;Wp^1i3^A`>(;-,$IlACUPoWkEHTG,,ZVWCg9LHE<@.]&Ik\;2-ZH3E[mg%7Y`g3%XSY^7;[-8'N%_C[P0`)PS-;,V]n_=l2Ma1KtT21r)?'Z7BP7[jIB.@#`5(/ +-m>D$3G8\&`:He;3>Jr6$^5X,.3TsW.\_TXp6iXL/`Na/6r$1Qftug?;b@=PS;J^Af)o%2/6r$1Qftug?;b@=PS;J^C4Rl?4)6r$1Qftug? +;b@=PS;J^=0KJ<\&jde?,"`,T'rGg+ +[2Hq5,,_0dCghA0.4'X8:.:BYd`Td=_rG(A.O(dbenec8c7(&m6r$18&CTjGh]c)53>Jr6$^5X,.3TsW +.\_TXqQ`kG9roOZ.Vd3D-Kgms6:g0:6jY%0C[)HPeS?kGCX8-, +[:Tf1F&"/-,]YB/KTHE"i#t5X7B@\R.Vd3D-Kgms6:g0:6jY"Ge>O.fMTDcq2-ZH6LG]epH +7H@FtROq4Y3MhCjaJjt.6=0KJ<\&jde?,"`,TS]kEYm7+@F:q%8dQZ';\:=/ +[!l%?LK-`;7BP7[jIB.@#`5(/-m>BDG468bnA<*n,,_0dCghA0.4'X8:.:BYdb6d-#a[/Q'p2GB&tXOF +fd#DI_$;&UHY6QMS9%:/+d#\8-%rWKVJb@M7B@\R.U(=D2G'RA8uP)GBK>Hr=E'966`KeXmFntiDd@hi +q!iq(p7i`.ki0R>dlrR>9[5YpKTO9SL_<$`Rd"U4rutJU3>Kg.i5($b'=PBZ+$P'sqtBE*:EnV3iCfLi +:&?'A0KJ<\&jde?,"`,tfk)nsh,16tE[k86mWV'+\T29M/_s_sHM$FOX&l4t/%.,lkEHTG,,ZVW#`3iN +Z\Td`>-t5d(GD@>ET6-*S)(::h7In)m+LQnHg]:V8(6>P(+/47;F3q8<(%$9Z&kqhGc;K+L-;\.Ms=pC +X096`o]X]Y+!Q0uV3aCEo[=]HD[VbM:+7>3E[mf:OBBBL/Sl$*/D$WXCi+$;pu.!8IJ)-amqE/nF&"/- +,]YB/KTK`2dlKul?QPRnHf'i@:gjQ1Aum9[5YpKTO9SL_<#u[p2?;0p>k#>-iq)3N&)Q&ig\(6:g1SMC`5N +E@kqO(+/47;F3q8<(%$9OXmEpi$%H@M)U0);/66``2dlKul?QPRnHf +'i@:gjB>N)n.mp>f9^8MCUPoWkEHTG,,ZVW#`2g@MCdc3i*+X!/POGMV1arOWJD0R,.EGe/E1g8>1Aum +9[5YpKTO9SL_<#u\%7H//7PB!jBRUHZ\?L]Vj%`*3>GCX8E>)qkb'NoAXGs3[AYreR%/4i#pGF0&LE)u +Y.':cCgi6HN*M9EZ\?L]Vj%`*3>GCX8E>*"kb')8&W_rf;/66`3E[mf:ON7m8,,9C)#$B9AZ%^:[Vj%`*3>GCX8E9tZ +dB"qdAXE[6YSF8L>F:q%8dQZ';\:=/6uMc7:#3Z%nR)d?U9V%YaZ68`&/$u<;*mh7`_t4#6eEe.L9X]7 +?FDOJkEHTG,,ZVW#`7?H.[mc!/`QbgX.h>SKul?QPRnHf'i@:gj@3C:;*5[?Y%T(s@<0XB-%nZ]7?e>r +\^=Jh&J)\@.iLNSR%/4i#pGF0&LE)uDLF"'15EOUdgE0P771VoPB0JJ+XD(XU4eWLX]n6C\$/`bcFR"4 +4LQq(kEHTG,,ZVW#`7?j<]"@n>Jr+pD+0(c$^5X,.3TsW.\_TXaK9YT1Y8j.>\:Z")?O-1S9%:/+d#\8 +-%n[(6RjjKS(`s]H=RDIF&"/-,]YB/KTO8l<(2Sg@F%-NP`;]A(Uc"XkEHTG,,ZVW#`7>Kr3>ERM-si!m>-iq)3N&)Q&ig\(6:g0:1s=*'lj9WbW2MRXR%/4i#pGF0&LE)u:.95l\V6#P't%W] +PB0JJ+XD(XU4eWLWG""B\+%E*@MQ:_$^5X,.3TsW.\_TXaJii5LV?aX7ZCjl[:Tf1F&"/-,]YB/KTO9S +-tF$AUS@Hu@<0XB-%nZ]7?e>r3>GOSA-fC3dnm*D2-ZH%Ia#9T +a7AS$.Vd3D-Kgms6:g0:6jY$!;c3SWhSOp$`@>WI(+/47;F3q8<(%$9O]0Z3bVt312N:_dROq4Y3MhCj +aJjt.66`>%_-1X_p\G"t.Vd3D-Kgms6:g0:6jY$!;pd!E0SK=JH>42&AdafD:+7>3E[mf:ON=D7N#(9e +:]2tfmGh&@CUPoWkEHTG,,ZVW#`5(/f"p?M\Ft]<IGUUIUAWHj!Z@S9%:/+d#\8-%nZ]72r15$hsBX?))K0hRt^Xc?*Q>EXQ6hI_C*O[;4B;G'<<1^"^6< +V+[.eHk9MLS?"e!KLAR(PRnHf(%G#t4OoAi]lW*2O&ob*C[9-./'g?Lb_n-g\8Ma>s8;I)/mR'+mJH@e +hn?r+qtI":-sN(/X$%b^6r$18&/$u<0hp+K]]@ZClN?JI1Gbqa%j.B(Fm@SnrcRsnaY!G`f3`a:IHR/F +p>3,[c.uU\If&MH+3G?GkA!d!+d#\8-%nZ]74b4;/Ng\,5N@sW/_h^LSp^.s.%]52[r:0lIXC\Kc,k/n +If0!,YJ39g6QK=bS+'_HcCO:u+g;H06r$18&/$uISKm^[2g,jSPH,ET4CX +?CS3RkP5Y)$uc!+:FZX,S;J^<,]Y@Y2h+9^\qRW%[N82rp]&q_AdDZ/.3TsW.\_TXaJk!Nl7*_;(;=tZ +g>quU5CWNTnknl1_KYG'=lOe(X$;kc**7JZ#`5(/-m9jV.O(db#7hm2R'=9;4o)k\gVUSKPkFIVn*[m, +mTV$19kthTR%/4i#pGF0&LE)u:.=5C1u#_rgc9`Tlmo9PZM.S,WPf-'Vj%`*3>GCX8E<`<:.;KP]nNXs +^YZZcXgd0@g"H2rj,_NV3AW[TT4SIpF&"/-,]YB/KTO9SLn'?7H[6'DY$Sefm+AT!T.`BXrqrN]O(NbB +>F:q%8dQZ';\:=/6r$23<4&1_mHoruqIoR0hIR(YF&"/-,]YB/KTO9SLn'?+Z\?L]Vj%`*3>GCX8E<`< +:.;;<_W74c8dL9DMC9VnE[i92U=O-hEm?*[KLAR(PRnHf'p2ElCUPoWkEHTG,,ZVW#`5(/aJl*q>F:q% +8dQZ';\:=/6r$23<7I?g9[5YpKTO9SL_<#uV]p.2C<[M@V]n_=M4BGO+XD(X,,_0dI"obBL_<#uV]n_= +M4BIEWMh\H@<*ilKTO9SL_<#uV]p.2C<^CYS;J^<,]YB/KTO9SLn'?+o,/mQ,"`,tS;J^<,]Y@YVkVai +L<6EN&jde?,"`,tS;PCeW&:2UE[mf:ON=DW&jde?,'#/Jc"EE6MC9VnE[mf:ON=DWS;LMA)()7*8dL9D +MC9VnE[i92U=O^Haf1(/66`Ep[11mC[IFWKcF3W>OG[5^A6or +A&jUWlfFI?mVY>fIRK.=j2[5\j^8#Kkg?0;'n-,ZhsXA +V+[0OISc!IX097k*S!CPi=B3_T7-Fd5Q0%q4o=TPh>-I8EA:MM'reetfto;6].<+)\U=5DP:'/?pu75" +Ecgl[]mE!=_%0jsiJ"uf'Vj&5e(mlF#u.Q>\(iOq^l$ofQ[A`-k:^)M +YkQoHhg`E3cBbe2?/pI3d +mHtX4oB+<>2]b24r:SZKn*;.%#2X[lXfe]hrpc%t57kitlIDs8n.5Qgq!hiuQmM*J;>]k#&%dp0Gi=@k +s80RGI/1(?aC>N^9?3&5qtGU?p](6]s6abg:S0iLciM(NOM;@@LoF2b(Gji??]('h?r->3'-2cN4rS-6.h;<@AUKcXPf73bNA&jUP^\IDms4m?9 +@l42SLHk,hp[@/^h/0HrDZ&r"6-c[JqPq.OrPsEc7ur^gES7g1giB`SBj94AGEpu+gOJtRGPAei+/-$> +X&lKJjo"\9^[1iK0NJuB>W?a8.u*kJo#W7b1Y8g;T7#[u;lqd[FL>ROKaFA +=r5N,P:(Q-6nc4B_S( + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 33.71863] CT +[1 0 0 1 0 0] CT +N +-1284 -69.75 M +-1284 286.5 L +319.75 286.5 L +319.75 -69.75 L +cp +clip +GS +0 0 translate +320 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0VDeN2Es2XXaA;!JZ+VDU26&]7/YR=r48hIXuO^_l1)%fc!)A3VtLkuP>5Y=EWKTI)f.nW3FR+\+( +_&*b16R:-V(e]V)@"h&#`V"Ya&"_\K>j"/[Y.ZL7Hb4Yhkf.aNkNn_bFZRj>F"V[-;@3B,WN"!d#WGR] +=4`$-$S"m,_Uf;GSd5]q#!%K5GXB'r`?l0[rr]fUOOI!f0hgD=1@6'"g54K2iP +V+[/LWCj?j.$Uat2gaN6p\X[=Demd:YJ"!.S'N,Rk3lpM=#OmITG_jQ//F-nDT.JJ$PtE8)]R8%hgYGs +Vl]48Xj^3C4b!==k1m7R9BlB];]WG(RZ,=bpA>Jh`2UQak[4WCSCA7S)(28ck>gCf]*(1(Ren%S[:;]8qu>;se95em,1 +m!c"2/)9+BiQIQ@VbYbS;o^uEPHfdjrqU2=$Z@<_Rm6a<^!L]skSNnY^\OX3e-l6;O+'qZoUA)A7D7&d +rob0k6GN>k?doJ^k)Xsg0D(_uqF/*EDuLZehr3PQ`&E2j6UO6(c_%E0l6[i):7aR.3B:7"`Fcc@=bV=r +f@^$Krld?>_1Di5s"!*F_*Mqa'.6P>:V(f@@W-2RbSH]go08b/[sN`daGFMrVH$V +gJV1h#7hk\\o_Y-F0k"Vm^_N??Ph-d*BSF\gU:r,(G=2GKgJ'A\^AnI4*E/YZq.C(>.&,6hS#oeC7[NT +\3g@#]mB?1hg?*'H0!$Y4k1'XFeu&j)ep+3hKc#*R5]%j7n:U$?[WXui/K\PW+N!s]^nmc1k5N&c'pYI +S*cHD]ejk9!*']pm-3bkDGuW6QklX@naBfK@U-*i00:Q.DuLZh?gWW>SXlC%qWUKlIAApl]'HNg*Su%U +]6E_pMI(VnV;oUW%h&k[2H:<:,u=tP]QirK%m4gpd%Lt6pcslTEnI5]RmF1Dg9jR?b=>=e^&X#BoC^.F"i9*PE8+ZmAA2<;Jg@Zi_f6dGca.n?ahWlND+Z=d:='\6qhf%Kj +bP0nDYI!#'YM*3!]lh;O($5C+$oRr<8R6IJmF0qE2DHo!.5e0lANaA+AkdAgh/`.W^P;29IS)'4Q7Y`L +J$\nI;WW?p9kVbAM'dqW`>SqTWBbpT$G5H9+)JW]`%@Ak)`RlbRQ7\NkpL*G+6^^T%13Ni).2&6nDjkL+JNL3)'f[2ZdSk*tK2 +Adig&E`/*Em+;nE$J4P"IJ7#VmF0qE[_1L:0s1mpD7#9!JC/<7)SY++WB3K4FRf;)o,ShlXf\_ami1;Z +5DQj=WWMRXC.'H%H9*f:&hTp-]cqTaWo`H;=Y(>L*BSHAmC-0^\,8.jnP"[lL@g+eV`7,c31WD%'nqhg +5fQXnj7DH[f<56-iUlB`iFCru[:eCL[_,tl[V4&4/Ko*hPK]:Hq4lj#Cg^X9(TR#OV=W)3A]"a'Y4]Y8@%h*W:;X?eEc0(]&h+ +rHb/Ef[6l=qLosgHD+]udB=A_KcU,jg4lq +]6sHdq(.$`/t[Sq50KS1bNJII0783P48SV:^TaJ?0D#W6qXm1iYbPPYoF!bWog6EtUL,\H$Tn4X6GKb" +?SD$Z48SV:^TaJ?0D#W6qXm1iYbPPYCjF[<(O9H4_iDFO$X4ZqlpY75nuuUWo"p]J*IEnb[QNp/MnH:U +@E2^8Ke3fsp*)/+H.]A97tS7:+X;Y[]?W_NortH]],Q4RC^Y]WgL+a'Rh\/J#I^p7aGF +ZcnF+jT#8\zzz!!&C]cCI%@q=19k]5o#+k+'r7kigEhpYC%#pc?jGPq0WJp04`Z2(JRMe:uN9:$q]8O$D2@qF.jB]kDY8=KfC:qW +iY0p"-$5Q_&[&l$Mj/Vp]9#C3B47e\->(Wm)o_GTsQQ?[r#S)uSB%T;>moP-GgGF`hhNG=PG!7n8n!?T +p;'j>"j_1bg_Ym3rN7*og2_\gp#:H0CrWp?^HGT$f9lOk>RUqs6+%eKn<3XKHsDMH]]m@q/,Iq40TkG'%hRrU +neB2l3^]Don.gH>rTE=l44.2G8l7T@'s829"X"j(VpR][&og#_XNYAa%q55VO558N(Df>8iI!f0Tk]4mWFH%jEoB-D2^S +jWt?DNct]s;S655k$o)]RPAET>3,/-#D)&'$so,l34#l<^9WVbedpl`[^3f:G'Jc>.Y)AtW6W.bij1s4 +r#Ez!''itABo#@~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 0.07987] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 356.5 L +1603.75 356.5 L +1603.75 0.25 L +cp +clip +GS +0 0 translate +1284 70 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 70 0 0] + /Height 70 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WZ"atg(rtT?NCCcPZ-4L"(1)QbL%\R(Kcm0'/1^oi0VsJZ9hH#0'NUMDU_/^lc`!AB;sIJ`O'E*[Yu!!!"6*?6'cm-Jg%n*D[>\$suFo&@WHj5]0\+$Xr;E<#q9Se#iJ!!!!g +UM#/Vn`.Zb7uo90^3o7tX\404_DcAf\9muFn!)G2A!pKup-`P +fj4C/]6E)#Dr/-pIHN+^Xuq+,n@T:>`&FcAiGY`hKTO9SL_<#uV]p-t\Y:6?nYoghmG#,+S)$Orq`\)b +?@D>$6-h/Gj:afi'i@:gjA\MR+d#\83>M4n$Squ:U4eWLWJD0R,,ZW*.dD4m(Z+',?fErkD=8YbJ;P`u +,]YB/KTO9SLn'A!S_Aetr1pE\041<`_k1.FJ""q;XtVjN;*mh7<(%$9O])iP(%jYr$[6LF]Qru&]otDR +RnBc4MC9VnE[mf:ON=DWS;R0fn%sEuHeg8LT0>D%55t'1_p6QQltSZ/'dfh&8dL9DMC9VnE[i92U42^G +E9HcOa4oA'J,]DA4*U*4*Zk$&"BUt\MC9VnE[mf:ON=DWS;NVRFEI0SrVQRGp^^GnS;J^<,]YB/KTO9S +Ln'?+F+q`8L_<#uV]n_=M4BIEWMh[UR%0$^+XD(XU4eWLWJI(%>'#D>WJD0R,,ZVW#`5(/aJl*q=.#M8 +-m9jV.\_TXaJjucMHY(Taf1(/66`Jr6(FJ=8;F3q8<(%$9O])iP't%Wi-Kgms6:g0:6jY$!;\;#)[:U4%kF>F:8E<`<#pGF0 +&NLT6AeufWS5M[EjA\MR+d#\83>Jr6(FJ=8;F3q8<(%$9O])iP't%Wi-Kgms6:g0:6jY$!;\;#)[:U4% +kF>F:8E<`<#pGF0&NLT6AeufWS5M[EjA\MR+d#\83>Jr6(FJ=8;F3q8<(%$9O])iP't%Wi-Kgms6:g0: +6jY$!;\;#)[:U4%kF>F:8E<`<#pGF0&NLR`\$]e+0rA?W[eATV@uN.^XCZdY@>lq15FZ?R(Y'@4N%pM; +?a:MYN;5?kYkH7JN%r>>$SoH+0ub(%O7@$oN%r>>NLSJfZa7aKHi91rHq8%&gA'8uaP-+4$D%JM?P2<@ +,]Y@YVkVc/_W74c8mK`)c<:8c2/p$pq]mE?@B3Y[MPqjIE[i92U=S\JjIB.@#jFaP?$MK;HX%NXE%7T8 +ON=DWS;LLV$^5X,.C#iqD(WIkNaNZpq\0^KW>2[I66`OpYEH'EKc=KL/jiS)!l?IiR.& +W#GA8j$iE&cgK]EKIUHTKLAR(jAaWt?'q.'8r789P^IOp4_sNE?W_b1chqj7eX0B+N0]RHWJI(%>-eU# +F&"/=XcV7R^>lNKYG(Er1cDIn(bHnn;\;#)[:U4%kF>FZ=s6lt^Cq:L_QERlBiW)b$&P10aJl*q].c8. +PS39hBq`4V1,!cVNGKDKYnVOEU4eWLWJI(%>-eU#F&"0hifFAIA!Z!U/@u]7h?=E[PRnHf'p2ElCNa-, +c7(&?&CS@:biW,;n6T@Hm5u@c6:g0:6r$1QfnIO.Vj(j2d]dXbDHVEml.q850<9jQ8dL9DMTDcq27kP& +kEHUe+V!:X(L)Pa2?'od%nDiq&ig\h;c.=R@<0XB--AS)fmd^LWP*PWpNOTo&LE)u:.=5Ce='[1S9%;Z +3Nr6p>b7uO])iP't%Wi-KgmsilmM0 +ghuJn(Rh1fp,oO@;\;#)[:U3Z7]:g?Z2aCIMHG7agW`dc:Wp7;~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 0.07987] CT +[1 0 0 1 0 0] CT +N +-1284 0.25 M +-1284 356.5 L +319.75 356.5 L +319.75 0.25 L +cp +clip +GS +0 0 translate +320 70 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 70 0 0] + /Height 70 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V5tEJ"!!i"K4s&0_\u>s(N1#XT^r6kBqN,/.h\6jF#/O-#Ie+KdstF8c-k_mZn2mPo]/Am]I6NJNCHLkpkCLkpkCLkpkCLkpkCLkpkCLkr"7 +Iu*J96-Op?.e]nNkNLd +Cj?Z6?=Xk\4GdQ$Z3sm.`3C-3Ke6*[fSf+5Q7$\=7W<%>;TEcK=,HHsh)gt^]:^C_A)*DNA.,1R/3%*[&DlheY10cH$0/<]>hn^6Ce +>[CaFUN.fN?hGD2hqm5H56`J_-XNtm#8)`WX'K(>7m6]4dYc7,/<]5rrdJNo#TjiR91BBF*61*a1I?Qf +qYEleQVOGue_Aj5;B!oH?g09'hqHfLl00'))e4?=U^Qekp@a6A?].#tH$S)Q^s$Ds +PEV3O%SH\KIlne(O3c%om-a:N#KaieL_E+Kd*9gpZ,jLsS9B$t'oU'-qtj5/#R%@oDtj@H]Y*0mrV_02 +0Y^eHGKr*_?'jH,M[Ne_naS@-2`E\#^Tk&+9IBG3ouP3FV,BTV[9=Rj*1WId_Q4]QiG]WHhVI"%L.,u, +PF7f*^e5eN:JXd?hs4X-b&NlrkJQcJ45oPm%/U;Z?Ld>oJarL-oi:kpo$>X#CEFGc/$M#r^KBcmIl]_U +?!1=H:l$MC8&\pL@inaQp%.eNI!oaXI=7*'KNBh,eS5pd:&3>Rbb+2ies5:&iSe4pW>?_s.)LdVd;PH1 +cWB(;g`2nme'2NaPm[s-qTUO=+P%c?LLoW?oX+q/[S=[((MEe9DNT;_2PRrELt1q=F(4^Mt/-\QCVRb@Rfdr*!0>qI_/;fC&CiWOb+)Qr2o7 +9n.%85!G"N)NS:e7;=N%`lX-sMct2t[BA;aq/,L8RGXlMRU62emro.rj?XHBc;H[C[UdWdhl +d0g'Y>''MIY+5;F@X_P4c2!(HfiahqZ*IE2+`Y>+&8PVN:,:DAQnr6_MB>]a9I=cAg=i"hGP\&]-rLqD +`s[TQWQ:fSk1>FbD-<4g([G!nN*S'&IWs!(pD`Q\,n^Nff;f_H*))ssorKlrfk_Egie1DbAh(*bSG>$O +ITtt$r&+j3ge9&IJ2AgrLm_#9)uel6S]-):YccbQQs]NR?]p-=RI.#eQa(!9cGW`fM:`mNEjBMHY&&>/%\G~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1283 286 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +0.941 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.48 0 0 0.48055 0 207.31862] CT +[1 0 0 1 0 0] CT +N +0 -71 M +0 286.5 L +1603.75 286.5 L +1603.75 -71 L +cp +clip +GS +0 0 translate +1284 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Wd>M70Sd067)@6_&,QoG)H#R=9A/PskOki,.!sqYS>m0gl?Xt8e]*PPN#`'%Z49ugh&K_2hOapt. +<+Hi-+aWp4e'h;:6eoALma[aTZjeG#JS^O<.E4$2ctkbF3Ajm+V57iqL' +1&^h6++l1O\l@Uo0RDE(<,.#h:>&N5UBHE5.3_NEC=@l,@_2K'/]'U3.>;:?OtPOaCal)UJjc+C&LU#T +l,%k6AWpu&nCMqYrVQ1F^Yp*(8OR+US+W'.AB[k7%=r'-UkH-_O[)p +GcP/jS#6s_3Co$H.l+#YL'SE3;FI#heYW^uVVVgecXEDC7-5W&45>_;.A@u;7%=r'-UkH-_B#$86c&(s +Sl.u=+F156.l+#YL'SE3;FI#heYW_`lX9`5l<<"+V2-IMU^JR%7#K&]OGK)EihWM2?+<>rILF_AbYsC4 +,"NS`*-3s!,4ZBmihVAi.B6)YDGGp*_>BUt'[]=1P"[oKBd-",-V20e9[Tr"8;C^FUG'2t/,%E#08pbm +R;oX46*19hh[Q7aXe^d6"GsXI.2oig3BB.rU'i@CaQsuUc6M@a:P0Pm;U1]O:/`Sc1I33;=,I28:l`h] +?Kf7?9kS>#gY(Xkm:fU*60`GWkWrE%+tZ))/m!o7Y>1,'NaCqd#rS']>cU$4C+`'!-g!BGQQgG_,iWj@ +.0%gCLQS=bFQ#@Eo\\_db;E(0r[I,>54fL3HEACRal)5>+Do> +CG\Ij,=oc78s,+59i=r[/j-5t?4MC^>k=1l[2u.+AoMQIXUkGBt<@Tc"]C4h9>)[[@SS*&1$7UV8sG&-7Prh(LETq?\Hs. +&`1Vf:MEi760`/j2l_KEOb"VeQRbMuV`%+DD7]4_FRsP:6fjc71om)gFYgF0e4kf;Q66d+.n0DBI?&po +eYW`+1EDd6DtbO/4Jc3:"KBGTU)Dd_Lg^T&EOetXB53VEeYW^UdeAaZI<*S2Rak,o`\?`-l6\q.99=tQ +Pp4s\S"uu@?+?t'IjfQ>pX`+$R\r!C^EE24nT3tK71;MELs)'X*a?i*85X68E'^81(Ha;$4Jc2O"_gqo +5TZJmLnP(eEOetX:MQ(-eYW]Zc2"'kn]3R`Bi]^Ll>b:oY.kZ*#XKXe=,LfbD3CWL)`MZi^3tA-a6:B+ +j1Gu::Tet8ZapRnQrf#.FI=?(:QUCUPp3h;S"s_/2fIP8ET4o,++6!G_fXiPN:VgVkK#MVBi`8?C1Q5c +6,tE`SHOeZ>UaEf`NVjihj"B.!UbWo9:ODCH9XH%q__X>g.N`3kW#nh6g;IgkaP8#V7cCI$E:S)ssSTcE!/CK5b +hWAo>Q9$->f7I@ul*;@YVhh_m,iWj@a`rH>IjBD'=0O8E46]+tJ +5p@p_MJPLoP+n3nf7h7>[gOos'U!7JYJuO#C*Y7&2F/N>=,Q?-D3CWYl,!rgVuMd"8640g]HFOc$[uZ! +9I6H+&bQDBEOf!.C9:3.Znbp3NkNU;(lX_!"1?K9J +O<>.%.n-n72G$bgNB`rtqc$bg0FW:%QG";$s"ilULG=c\)0:Z`8bg/bh[eRGt#u+pUctpZq6.TCZ45GT;-bLl#\p=fW"f'NHO1tbblR4U'O%/ +/m!qs/o?cGlI%m:589WYB;J8/T@UpBF05[r;!-6LUkg2tM7?e>EjaIF.B5#^_jPWlR#JVH(F-D\D`f'8 +._->miV:,oKG7::+;'qj7J+$(%KHLkn.n0/s2G$bg3?HJ[ +T*!mD7oJYn?B!P[:T40-#FJVoOWoYo,iWj@aJRB4.B7:LLNB^"p,C!^o:0r8rD0?tc"9M4Q0Y`,AdK)i +\Pct=D3GZ62r);2oNq$ICVT"R3A3j;W+T%d%V&I=OR'3'AdK)i\Pcs.Bp0622pfJ46c&`jr:NGo:<((# +/(\>7bn;;]Ei$j#dO$ECDN,1icl9gh"ZNX.Gqi%r5*]#&QVgmjY0.$<,tPu98s+P19i=sH=rtu*rnHIOS4O7<)1MMEfRNpEq"3hH(ic:m:cI!sfWpX:+BDN5/HQ2s]bBYfTH(MW1B@7%f +2G:3r#+?i/`8sURD50g'<8H>bj$k^5-Q2?,(jep?gTF +s3SY)cG:I9mKGCF47e%/UjD#uSP2*RfKm&#)ebN[&XP":p=V>qd0_4QG4b"G7t:*q?iTujh7N8BqS:!g +0::il8Y]0HEN)^1*QIH=>aGmV99=tQPp6*AS"s_#++3RoU]Oo:H?FR]qbM_Z?[1O`H%4eNLGSmr +g^%LP/^+S4Om7crFY`>GLhgK_;4e%q1PP8=mEb4Cj3UFI6/7I,1l5cuH +).Yh$VAQs[\LnhEXH3pI,MtdKEc#RUK6lq=[7TugjHEB:htaUNrTr'9?_.4iG/)t1Mn&s$)Lb\1XEk;G +S\IgB8e%2;8r#TsiLMuMB'<0<^u0:hiG^dM*CI#g/0W-42P$N6AT`9jIer<)X%jT'oK/*U5Q8eUYKZhn +PE0)89>Qjd=*;P57k/4KX:`Hjc_W@G$f0c\39D2;.*8@[5`QI)G-rs8_A@e/%)0A9<]EuKHXc@rKY*CP +2Z2Che(io$3;*h#2!h)_G`ECapA4IJbJ$GL(FCcd`@!7C=bdm3?+H7GcUtNa"c5-/&TOe]E.5F6.I4\A +hV7drl//AGO"O+reXi_JQJ/F!i_HrDaA^Ng>X>)Y;FK:_.;J]:"]%A]o_J+Ap\ss7IoAoT-I-@%<20h= +4pEdoEod%0RuDpB[2aHUR[#Bo +Zq6.2>A&h^^)]?'il-gGNum-UpQ*Uk4aZqkh)QRY$PR0\-`Vfa2iWq1A?P#bYa^n.8@1sGL`G +l`75'Rbr^@l,(AAm>:+?nP*EP".$/)BU@X<`]TQtQ6X4`G5!1bdkk?t33,2>f&\q^p^3s)bL+s3^E^:@7%:!.\:Q.Ji?c;aFCF11p-`&SWu9[=k$!eb2YnH9 +'dVKY7Dop4dIcha]g&eq(FIFB:R'!ue#5_pf5`^JU$kXW+OMMb+tfH2[tEkmKo*.i'4mT7BiLjsCH3<` +d.\hfOD'gl&Jn$$gVJfpKo*.i';[H`2P'rp[2b0=gq/b#8@1sGLg)l.D?hgc(FIFB:R(O`^6+@oY2s=B +KANA.6)%(O7,5'sR_B0rG46\t@H+B^7&^UfNF[uh/Oq$'oH**$&oEdUi`>B^G+Q\;_SfUEM6_*(9D;qd +CH3$oB&s@4D3?+&(/3,i$>eI"S1[L*eXcgnheUXJjc*(Lq)!H0T4D6M6ko7L3:/Cm`.<= +En(&,pFB=,BTj,BpWsVb*?:j_'5+_NLqa,8C0k9)A"7][\X'"N#J,,iJjc*(Lq$S:E\Fp:?'mUP4[hK[Krsf$FZdIMQD9HF\p(]:7%:!.\:Z7$:F;@' +$PR0\Vhi7&YZ8Dt?3`IAEn'D'O$Kijd3^:dDI89+*u1[]'5+_NL^/NBXfnqO1![0HFZID#O=Nl/8W&>Y +99IuFQ0?*'=m[YIIus;YqYD/e>g&6<2_.skBTj,B\!nSNKBpp=M6ko7`$%FPkZ??m`gU['Ep_;V>X>)Y +;FI&0_6rOE`*)IVip\()'/fIWXl`m3;.kMC-Fn^rS"tk5rq>Q/FCF11p(T6f$h*jcBLSKZZalBsc3AJd +EWq:X8J*Io4*YUa@H+B^'JV733^lb+NGbo\W2GauR9W%P9iAoEg^[XLMUj_4L; +naY+O1T]VH?R&='=Ot=C_,=7%/khd\8eDR?P$1^c+%*[:E\s>i6bDgp"!k7uZEFif74`?-DBhTXl,(B, +V\J[j+K6)5oeF%Y/4XcMDm0`;_TfBi&LU!uB]!,N]jjT1`/P]pDVr/ks'p.Zilk>Y==?^T75]M+%XknL +BNEY8?'mVCPTlFW_YQ\A*=V%BGf@9Z,Y24jKs'%Pm&!NYEbJ$H9'J[ejhh[PlASl=& +^@g&]a,V0ep=X)$@Fi=YV2,jj%&GgE\r4;6?mf"PFlD31f8,V7%^je:P,)boGOF6HH1L#Xn%:H%6Pj>t +bYsC$r?j`WWp-3_ijY$T+5nE9Vs0lp2u=M5;T(aLCdCt+XNf#7HhZsqr)AOim)$rW":mP7k022M5S'.i +ddBS&l"!8$HD_h\>a@/n7Je]'ia6KqPK`i%A\T5NA7$QVMV)QK&ROP%N&a>C;XD+OCD2aZm)8o]*(k!d +;!4[R7U"r(:AJ^lI>p38@b9HHYel,;7<<9_DVgeen,M[iXGqKt&^`54DA:Xb\1Z9bbL2)kA7$QVMV&ZS +<0?b=\ +s&U"KD9Pq9.+![5NXFJYdJYLtcRT64I0m/pQ@4`P.2oRr.BEW3%5cD]d:G7_Cn>.dtLpDo9:Gn6..j[BPU!X8C9KiTF:M5^.+V,NQo0V_*uq(.9`,lbhAG^ +'N$2J8k0$o,ct^Sba.5:4%(0&.'9 +)KAY3&iFP3mua?uFCF1Q;i3!\Kb]]DRMa=;]'qcJqmrNS/Gg?*;D\UN>4=-nW_F9Vc9.qZ_ElU68o(0R +4j\o7:D`GQI>p38lO-m[[o?_MJCUoM).pV +U1+qi6jh%"U)D6P([cm'NArBPd_Qb!<$au@l,(B,V\Dul8=E'QGm?4WRa?M[1+DM( +i![(0eYW]fB:`!hh&r[B'dTf*s#>X!bL,77:]r$N'$ZT6Fq&m^G`NQhV@+&MSl.uL+R(5LV&7SbcQLVU +P[\`:;5,2\/1G>]_1)SQ([dbF9UpB#&aK^#T'`Wr!tXRG0:uo<'"4)P$51UHU?:OI*P=n"C=@ln1J]H/ +X[,Rba`TlcSk0`9Y(a_d62noR!qcZ<&VC"SQ:5\U8?A*Nu +W'4LS3:i@YL?udF2o9a>D]i\B--Hlh#SM,TPZIe<_0_uV2I2\0o9:FsFQ(G*@LI9dqB/>I8PrY#D>R41 +Rk@C)GtJ[d#:]J/fSJfo(NZcfL//Ce&1$82.%;!eq(5%Cl,(B,V\Ck5OP8D2Z`6*E&,Y_2Vhcr2&ogg6 +9liaJ7uHT\Ko%T].:W&p[CAr.<,j%'@Fdc,Lm+PY>.blg7uD'1Ko%T].3_EH>21\#WH-Oo^aIBi+rVh" +`(eYW]fB:`!hXrqkkKB?!B+;b#.\Pbi%"Go)e)G17DKD.o6D7VE@js`a[$Bd#H +aPJdGoX!buXH=V;-Um>[h1GOV^nmtf?'mVCPTk,OWMcjTJ^A?go9XNpLh6r+EDj/UICptS+"=3a(FIEi +V2/ABl>K$1eEWAK9E)>(.ABss7%=snc+Xu_pU"j3;/NluETS]=j9h&&>["'60.^1^BU0bQ-Um@KjmCth +;RDn;c->S+YCHH:[h&F.?'mVCPTk,OWT0Ol3(jeIo$9Y*'[\Ht,#4#q3quJtFh(dNdBYYg2I:k.jWd9U +4Sk8^++JZ9MW]B"/khd\8eJUcU^PeJ$e#5E,#4#qlMGbo*[t:D9/!u#il$\75Q5nSHi)Br>JL$bY9&&W +Z(![jcMDckQoO]%,'!j$$sW'0Q5J5=Sq:2rU03(9S.sMXY6&p_92!Z12E*RU*'#c;BX_p;T![=_hgTq' +?dXRa1J]GD=-,nWb77sSTj;g=F/=\)#nh5U<5Wran1H.nb2'[R5Q(#g>ISM;]k;+rCXRC`GiOcdImk/# +c9.qZ>X9:Y?FtB=G2_QE8eK;\,iYVuTMaMQSgHllQS2^;[,'%ms*WB&4M&0un=VRMc4#+.oH7.qVK^gh +c_XG7Y%jj6=(Lf!V20:$&ogfKd$u\_DduO>]Ej#[eQ9eP++N7'(Zp+(qng8dM(@WodKmVb]QQ*GaWDAt +>UcCMJjc+C'f""g_0_tkNm5hn4&bkOhp4Oer5=F*cThI-`l?#g:SJ41B:`!hXrr^LlWq.$@iQ?Z>UcCM +Jjc+C"\o#\8Yb+$V.bmi=0GB6FEtIh^An30a,V0%HhQh(,.sZY;hB9e&a=m(k1t6pr]/EcMk5#>U.p5- +S.nDNr-%QN.@T;Zc'oeLg"P06IK.9E(FK[d[F\_hjHJfE`0mM;OY025iHL6S9[^= +fn6(.=/$(dO!##^7C0KE]Jq@?9c#/%4Jdl#6*bQ513Z:`>r"Qbm'EUG7L0WH;KS#T_98"9g2pc_bYsC4 ++@td:=c8:&oMs@-a1d*=d/lTX:5Yo#qi+GkH]r#Nk7H>dd`(?(8 +*%+1_l,(B,V\INVODO;MYlUA9IFV5Q85,,$)fpo1rK7CijiYT5U@lleM).pVU89!ci$1jQhQ1(_eP_8q +9TY&LD2R*Y/nhU@=rUb73:i@Y)@M^FIV"4fmbiMKajF4Ja@_PM)e"W$QfT7&,HHJ0VJ+4r@H,L0MJ+b9 +G5>d8rZ8qm)\ASSigO,487mN(Z47[0_A',?/khd\8eFc%ad0]+C=c%4hBRc?27X]hb]m$m\)+\:>hjR% +1_a5_3:i@YRLGA9[?Sg.ojAU0bcmN_QQqY2:PsNAZC^T^BILGGETS]=1`7Q?!/k()5TT]aC!R42C2uO+/XD=&D;fqN<9RD4j8up,IR<4rChf;tkN`20q/khd\8eDL;ajn0o +Xg\Z[qIR?TR`..s*0E^o.C/!9;m.Xm6Ph)p4JdkrKB0=GQ8Mu/*DFk^=&"%QR%sK=L]mQIObIoUqFa[D +\hJ-d.7LTa,Zb2VXS9#7..UU"Wh5aJ#pS=Zcr:=c*j?Ko%T].AHV\\O8F,HH5*?.<:oF75Vb! +O[(iSZ="0@RhV]q$PR0E;FDluEeuJem8m&@UhELDp?]&bPN@TdooUGVjKSJO;.dBnETS]=oFN:7$e'hI +NMTOK'JiAmb7R*W4[iDj$sHG(I_%4Qjs``@&gUNeHd[,s?1[Bd-Zbe==cO=ulR>_b1MAp?q<>c7RAUbH +:QSgEljWSNJOk[\h^0E9P;tu./4U,Hd1dW!9nNkQd1eH+(FIEiV20SPUco5H!q?sF]J$>uO&/'a&]8<> +797'#)GO'rFCF1Q;i9cg8?O3BHJes*1oh"Fr.uGNY*L::dT28!I05QVg.G5#FCF1Q;i9cj8>C#rK0JcC +.It@?&aF3n@L-hIaNBfI>:FH>)O)0uW.&$P4n%\ekbE)Y?J,I?:b\tG20QH5( +7J%InnQ]Vc\>!/>MItZkV?;6!8$#eS'Y,7>MTdNll1NY8J*aG\Y$JXtH2Gc<^H?Z(e>*T#q06243:i@Y +\dd(YC/U&s3(Sq^,OS@:b7SNS1I]$1f9VpU8o&3Xlfc`kDnPV[hKpk]]QiMYOmVQ'=arjV-I+`MLn95U +L22ehO)Cm1\OYGCSsL\Q-"2\d%W5%Y'IC6!\bY9H5OSJidYfL?=;Rhao9:Hq%oTSI+.;EOi$k,fbI&<" +LEkP>IY2QRNLH*h]6E^M\T6g,n`0ZH^]-CY,9L4J-$[e)1M;G+"a\O>OJ]3GL-Crep0Ci2I6(M/2#&B" +_r5RY3egr`=[J9GD%Mg^I-oXqh;8"3s8&pKrTdhRGN$UZJfWaTU6,A,[!NP9R:HgX:Po@kgiYRrNe7RR +ag7rm[(3_mB?KlHRf)^TrVM*Do:MY)rqVa*:X5rW\hJ-d.7K1>83L7@4Hf64cEa9[nEXD9_r960BMQ!1 +/X2iSH"1@Ei5aWeIJ_*J,8Vt;R.X$m@H,L0MASdD*dQh=s(u&a$`m4$M#%723%6%HMWVEooB&PA +[^NX4gpo4dj88;[TDl(Ta,e"1iet-,M(VJlGtJYn$<#"^\X]`3@72Q=-:&hNMMRIjV+fV5Ut`W^p=jr_ +\TR5hj?7Fqq<"cnU'hWISl3L_K%0Xtee4VQLf-W8Jjc+C&LU#j3e./,n`.YSrUeR\gGa$*Gl$[&:RsQ> +_[euUqKQ;53:i@Y7Mhl=NUe5*TD@\K6lgkIS.lP_7EaE;fQcYWQhGlapmGcdq=`Q]kBsop9UpB#&Ru5I +i_hd8mW0FZMPre%Lm'%BUhDX[n)(m/rVGp9[ibI/\hJ-d.7K1P8@be_"'Tq`pIJ4NBnAaC'G2]][HYQH +RAU`Rl6'Z?QN,H$&`+CNMC:uVLm'%BUo61WQoO]%,"Aab\NOWU.A@eJ,#4#OPG6cQ@H,L0MAUJt +pAWn,S:+ ++;#ti`(L6S9[^=JonNe+5ksjWC?+;Te3e9,.o-h'I$)d8J,r?<1>L6 +S9[^=K(KTXHY[g?G`NP9KHLjS7DqbpD7VE@js`_)d_ef:(LFPocFOD=;%_ZI+pno-ihVAi.B6)Y-I+`M +#XMY2r*m:fd2+7+#XKY0MMRIjg2pc_bYsB1Uuc:+LQNd<&1$82Lc?i`9c#/%4J`(mai+=X#j5pB'] +Lm+ReFCF1Q;i2-^W3Q'RBi7>2Jjc+C&LU#Tl,(B,V\C:F`(:P0#SM,T7%=tAV@+&MSl14a&LU!]+\9OC +&TL]JRk@C)GtJ[#7%=rgKHLjS7DqbpD7VE@js`_)&ogf;&VL6S9[^=Jjc+C;1p&(-UksF;/NluETS]=#SM,T6kP#=S.lP_7CDBs +`0mM;+\9OC&Ocsg\7. +""BrM5pB']-pj#OP9T"^BE]2*?MqbqI^-/_Z!\ASeg5R&<)A5j&LU!]+\9OC&TM;Ud:u,Fl`\'p%,.%T +IQ;].I::\oMC:fQLm'$?&1$82Lc?iP@rl__cBQFtmbPML`"PqZq<"0Y+.s"J*`>Br2fIRN +BZeoWf"#kb%fH^k='g/`egE2h`(:P0#SM,T6kP#=S.lP_78'&uiQWg<04-ndm+CBYano`L'dHDB'N'N+ +&LU!]+\9OC&TL\_iTGnt%k44Y5pB']-pj#OP9T"^-pe_^4f!lX"Go)eU'O%/:5XjkU=tP[EDf1:Jjc+C +;1p&(-UksF;/Jdi3%:R-5pB']-pj#OP9T"^W&>koS.nhR+;#ti'Hp#c8J,r?<1=IH:5UGdOGK)EM@_&m +UkguZWpS/_Nu.is!!Y0"9*P@rzzzzzzzzzz!!*"4CMW84p$9)*h]DV^B\;J0js3CQIf&NZ-UJ@3hS&g* +(LIT!(:7h'Hi':N^A7Ams#bR:k]op87&uUFaJK4:Oi5ijKd`u(L@sTUhItF +0>031WZQ`gnK^Q4DVb0>YC?/s@uj@Hn'L5dX!T@FA6mW;=i'1!?kT%=(G=LTGOL;/I[DZVa,_OeA-$O@Dr8fTXSNF=Ta$9L\&!JaJ +b?t6(i$m'>c-=KX7ugicFD:2,PKV6CMC',P".g7pqX1ua-KX@UfsA7D.sV'hkKfYYZhMMTrq3HB:S.LN +Zd/h<-!E-.JgM;%H[gGCrV%]:RTCd*:m6g3\))FHZ@>%7m'HRmN(\%(1b2!I\Vn(Ls8HO/hgP7T^%Y2) +H1IOW=0;sVZ\+\[b[C%k`d2R#:1#Q3^W9s!>kFR%g[F\`SX?sKPbIVodhIQ0Piue:\ +oSLr-hu)UqPI5)mO2(SJmj?GNhW`rpGOF6HH1L#Xn%:H%A%KXhjCWD<;WO"Q\80^!9a:*N:Hk1qkKeVN +ZWCL^Ff865".(ZJT&f2.cT`NJ6tjocM\gm5MdL::D!8a;@!>QHo#i[66$hgqVuGdbroM"N$[s:\'hkoT +o]X^&rpTmd^]42Gai3GB;lBWd5Q>fgPG/)AL3rCG]q(e\b]Jb_aj&gWVp:iZ`>;lDH1L%W&p/29o>Zb/B?m#-Fm>2Pbi3_QioY>kQeY$rrk;)[ +fV"&phg^,R?I#-*CK@`mhu<>Y5QCQ*-d0o85C*5#f8gt93dKbTDn[+ce7ckB++Ls#]76a9l"ZDn:!2q= ++6<@i`ucZZdf96krsX-QZt3U&EQQH4Ap%OKgnjB:f`$6d]smGn^4jkd&M0Mc96Ij +/V_5mqsCkVb^Aor#Li-(H1IaFZ'='_WpI?Op[A.]mk;EMi_ZO@F8!!YQe\\#cn=[J]<%p&Ies!ApmT\\LU"KV(a +_Q;no^]47C;nI;Eio]Ccc'pZlmGIn[ZEO6%j,ZR"(QJ78Dgo%io>6]lUWGED!f*k01*o26)in:YNrUU[Rjh:EF47oAOIKYB2?'pmO_K4_TM;'cB1.)) +2E"lejcq&\NpQ]ckqBdG@d'#M-*pq6lB7XUm7WR7mWu=M[_DV'.oR(2%W$'%:Fbk9>IT>N&_a[?Fl_.E +;ijXizzzz!!%P0rBH8AK'W~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 207.31862] CT +[1 0 0 1 0 0] CT +N +-1284 -71 M +-1284 286.5 L +319.75 286.5 L +319.75 -71 L +cp +clip +GS +0 0 translate +320 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V?\lu&+33nCg[@J`VHgWu>e$10>VHang@aW$aPS>8UhqRH6>gs>85cUPPaWO7$6dY:J)b07dY9$P +G,7#GBi7;crg`-FcF@.^]*cD+61Y%%j-N:+22@3Re0mH+$WM-QL&/+QneDa5'_DDjN2Z?gpqJZ?bRL< +CtQ15gX8$`?b\dr\od17d4jRnZY.V@]mFt7V5pXI??tMHL#gp]!Q:%:D/F_"qK:KYrVc]JA$5Bt`TN,l +`G;IkA6?*G7D'q;PB[pgSiqE9POQaK^SL:nmBSf[eVSh]&%Me+]?5jAc%[GB@p8'J/g] +B@"RCCu2i#Ck6ghH!.G:>V;B2+nCTDa(nG@3Ycl,gprV4'hc5V^P/7DIie(WiJ/M3R4qEo]bb_H1)moq28n +Gk'd8-coOdUatrm_1Q_TNfrX*04);ieF.=jrV#&kho/BJ(OYi!2q+Wu9:'Yh4])!X9%uQljsiT!l'GVo ++9sCgRnQ.pL6G`LE+'a/ZY-K66d]fmR_I[rZc=b._M"Y9<(85!:q1+p53Li_9p9;V$J*HRJfXf1.D5n6lX2jo?VJ,d3$cSK1WQoAqip?\')LOSR9o_ld5 +8fp6FqD9hUP"WH0OXF4L)`MY.?P\FH3.)EHs-?rAZ/FZfY$JZfVtTK1;mr%R,W!,q$p/Y!8=+='?XF4L +bEaaEZcPYqeZ2cpDSLJgB(hUmr+JOtAe/pT6GHB8d3QZ@G9"HNX]q,THNPSX[gl-iDg]?&^i=u#`ufe) +R51VH[5E7J-s5P=p$1)4c=9kp!A;rNS7'CT]6E_@4/oRgisIO7'*kUd5(E"U\P]+kQ0n>Bp$8l2mFnuD +:C\C+'PF>KV5pXIGsOPMI/*1Z.%\+rn(sEd@q)ASgU"q*j[Uid\!fTPR.210?\=&RZ?5?Zpk`u5Nd!q; +ERm9^`/*J:+a`p?cu0qn^>E$uER*A1anYDddke\S7365krVH3/F^GtKH?A*Ib?0>c`^i(HLb5mLrVQ=j +3a^^I@Df=a[C[es,qiYpuo0soSWo>^NH2V1^Nr["9"r[cu0rg(CbtFld,\iN;D,p^U[^TU48[Ae:E'SkSH;V +FeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8 +TjR$If7e[IUL+i6Ks'!U4#'9H\X\!PBK0k:)kq6TAF&=O$Q8oScnBM5lU&A5dBgoME9"s? +EGkElNSBeHUL.oo^>'\+EoZlThY@&7$bsH.Ndb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN; +Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[I +UL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j +?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+C@/JDK-Zf,\VG]idoO((qIW"$:6*)NdBbe/9.BK0k:)kq6TA7Obq +@hlO.o?ab">aTg#=@Fjd4ERYK(M4^L"*T3 +j%KG?C?WAOzzzz!+2ZpNumufp6L-,YHREGX]thpGjm"/]8H1Tr54:#n)(l%G:]M(H9FDSmHs9W04$4,l +q]kE1iD*YIs=NZifm_;h;-mcVbTtcF/8'LVuQVZYHHhAX8'u!M2gaOg[F[ma5\WF7CKM&kf/T$Z"\Eab-GU%2Jq/iq!__g74"4";&jZ;iVE*.]u+Vpn]_r84RhTrTX'Wn)cB?nH5\-,HWN@Wo=&hX"#fURh2IbV;J;l37!EokGn5 +MF&@s#tsm-gQF>lUN:V +5l">Sn#t#]mJe_^SduBq= + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 173.19932] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 357.5 L +1603.75 357.5 L +1603.75 0 L +cp +clip +GS +0 0 translate +1284 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WZ"m;8+3.gi)@:R8.1R@=`C6:X34QIH#XZ/fio4"cThst4crh/4EKGco.+H@-3_9DtV,3j^8kXZm +kInj8->i>QS48_*iGd3"'5`f6n!AJcYrOfFzzzzzzzzzzz0#HK%hY['9]DioE#6Fl*!0''"3*99b\GWN +8%j%*o!<<*:DblreR@^!V>?gFmG]RqIG':;U%ZgI=!WW4NVApTs?+FueEH1O@OennN?tNO\(f6$5OW[U&4?: +Lr0Jl]+XD(XU4eWLWJK=T=-UqGCMR_CkLLS2DLDH]\c44Nr'kY",)QY_S;J^<,]Y@YVkW/:md;,9*.7\ +ZEGf[^lIDq``aMr9S;J^<,]YB/KTO9SLn'>h4$,O+]a.GPD\p7W&LE)u:.:BY&ig\h;ph93p\3k6k>1" +8kglk?>?b<[c?FuaWJD0R,,ZVW#`5(/aJii4#6tM"lg$Vo4(k/L$BnC#-%nZ]7?e>r3>IZT;:AcYE[mf +:ON=DW&jde?,'#/JRO_',7?e>r3>GCX8E<`<:.7luKupGCX8E<`<#pGF +0&NLR`bnVS',"`,tS;J^<,]Y@YVkU'!_W4Za#pGF0&LE)u:.=5Ce216oh"(%,W':F)JWRsPL_<#uV]p. +2C78:mS97F1+d#\8-%nZ]7H@Ft1p\krl]`#K,,ZVW#`5(/aJl+PAmo`2.4'X8:.:BY&ig\h;c2#f_W7d +s8dL9DMC9VnE[i92U8F?4a]YO+&/$u<;*mh7<('JM9f!0@ZVDqm,]YB/KTO9SLn'?kAel_+SPhdFjA\M +R+d#\83>FEr/54>MV1arOWJD0R,,ZW*.VbDTR%/:k#pGF0&LE)u:.=5Ce24Kc3U;nB66`)BA,,ZW*.VbDTR%/VdO>8!FPt3>IZTd6J0P%pQ0eNu)YGB;[iYY,n_sHAYjmS9=]&lMhVj(OV^[2C*L%-^3qR&jde?,'#. +OaPPe1ldr=%S^-IE7n6)*Bm4Q:Z.Dpu_BdFu!a3\kmNupS +MccKd-qDd[:7-g'MURHm2`8edZ6jY$!;\;#)%;L$.p%u5-3p6[)OtkTYYHWe_''\Oa-,hG&jde +?,'#.O8>jq)R[Ug:rVESpg>(KMob=_d58iH&Q^gbeb^d^RFPG&jde?,'#/JRTZ#M^A]3 +`$F!8S1%LE"`+*39'epb_6r$23<7Eb2n0IUj5Q&Ho!!!kHrrM99Z).~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 173.19932] CT +[1 0 0 1 0 0] CT +N +-1284 0 M +-1284 357.5 L +319.75 357.5 L +319.75 0 L +cp +clip +GS +0 0 translate +320 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"/k;+.!_*68)B,eJkXA^\Oj-IYC-R3_$/P"B+0>pPPCOr6iS>sr9[]81!\R4`*7,g#BsYHF%!-BUub +7$;*Ch08bp*N*Q=H`H["n'9DRIQr#G][&Ua3PXbDI%GE)o=gc+Oq9qc+bUCn#U+j463n`f&4-XGKFgHU ++bUCn#U+j463n`3j$"V3A@R(?ih%1_U'VGPBR/2QOO1i6pU7l'-iRO-EWo[=pkQQ('l80T+8-m[ +YiU^kfSa"NQ7)Z?06.5#T(n)Sa;&N:iZIk<=0UeCJ(TZa_3QJJ@4uX)>FtVF]uUq)H/]q>,$2#1M1=09 +>eu^Rra@I#"^@SnLV:bD',)%^6Qik0D"dTf6:-<1+3nnV?&(\dDs4A.oTt46qJ--1,p@j=>!RR/8>*^5 +csICukcY0^Q\5MVZaH[C*8:JW;3dEtgJoA70-BEZi^q=A60`kd]X5Leds.HAY/hHn?R +ndhD.(.@"do]Q)IqYBs_g"G&'4C22pH@C=7]W;>869Fi6pHttl+$Led7(Bb::kb-uPgn76IJim1rVQ=K +L<.g7nRT_cboI`B_hAX)Ff"_SARu&.FmG`BF`mAB[Hg*g%1OK8^q]rU@7jd0>`="A8HE9sWDc_oLp4:p +26"hcTI4UX"b-=?#nO<6ol]?9mAQ+Cd=VZ7/@nYScsI/)mk8br@DUR!?ZEjO8&n42R"tbNURo&3B4j_G +P="3Wc_!G`,:l;67c`Ui@gMu&j\'_>FA=cE7MlssjVYlc/sPqO[AtGEp\/Y6(:,K[grmd[#`]LdV\!)C +_9^EPoG*$lrf!b.RMA@11/b`9r`TkYIm**fr*M[6ng(36+cBB[D@7@RQ#'>h>?dRH!I>qSnmETX!E)L+3t(XX(0r*Z!(BlJc8gaq=ds^ +/Nt'l)l:k22:-Kn#S8KS1+b(0WY#+TR.;&W=<+7W/AQioS\MeK]1@O.36'/[X)^4=Y$D2[FfC4NJ@-t0 +X>l:h8]l>grZYN*M`!AP+$Foq76-&#=qGt/oiWT>W2T1oD2-?>$;'KcW401BV35@dRB@qh5OmsihVQiT +QtuFYA3@WFW3*1e$*XitqY6!EYRIB5J!5AJiAEJM,4`t%9Y)#72pbn>,4@0G\b`hF[tfu)5!GS9CM>dg +32j,k8B"u4YCBLI$UJ`^=f=qbp\nWB+ar>m1OU4`qbb*2'ZOc^s+jjQ$Gt$kElL<*?$# +Xf\#iSXZ+475QLBhaKVXr`TkYIrU+F"#E^]I$hF\*Vk,YrbApCX%QbITasL=N032`Kde +\@g_MK1@!4%^jiR7*$@+`Gj6]95t%3[35&>U>'2'/DHqbQ^=a%W3-?CO`,gs>=hDC4r6*OM[Q5_Hu4N< +0uBBWii!G,4?mOI' +)u==NrQS(k;Iaa-8\=gbRWI\r"nX`h)&X;0)B(Xr3,fnZs3P*\kV!5Ks2NrQBL.,>8<@2,KDRUJFmRDH +^Eb(DrbMW6,p@j=>!RR/8>*^5csICukcY0;p@l#a+)=@nDTtNq[\]h8(.?p7gV/0.*LXFFjQ5cA%1R(^ +rO%1T#"Z-"ec5PDhj-/KkcY0^2aF[hhb6:b/P+9&c8W!8D?Hu%llV_po/B8i+Wc[hB5VU)F`mB"b)2c6 +jmK=,o7iX9H#'pn\[f8*L'ApFhu=UHoP?am[\X]I)r$fcmrH1<^Hpf5_*Ir._u?p6XfW%69he>*($1[F +Q(b6c\bg0RoB)FDH#'^h-7":bGY4?UoP?am:Me5ogDX6K-uS,70A:IAc2~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1283 285 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +0.941 GC +N +0 0 1284 286 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 286 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 286 re +f +GR +GS +[0.48 0 0 0.48055 0 381.51931] CT +[1 0 0 1 0 0] CT +N +0 -71 M +0 285.25 L +1603.75 285.25 L +1603.75 -71 L +cp +clip +GS +0 0 translate +1284 286 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 286 0 0] + /Height 286 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WcYJd>*r"Xbf8,BA7"i.m_BW?r:dg13Rmdo%0Nf*`bt9pkO@"k%%KqH*5VNQhU)7-+&IF&b+=\!2 +,,.mBLrb,Q"pl10\Y%0rH)CCKP0`=:bWU#Q.l6?HB,Ft4ESaDFBf("NSiqFDjo:*BG5:^j:>Z5$JWSNq +J,N=3Fqp-+8AqK`3#._'(+.63+EeeU$73iHXE@N/- +7:ifZM/uE`+f'RN5(jk),)MD;;\="!.[r"`S9%:0+n^mO&JB(5,#1RfLf71(Cgl=n;c3n#L[E@uDQ&@:66]/n`2T\O[P2^H.4Kr"PBPp^U>)Z''LF!=7Du1( +Cpl$;c7(2qe9c@a#WG^N&g7jb,2uR$2HuP.kEHZIl92aAKGuBbM3*Nl&TK9MRk7=(F%Qc_ooeJ4h;og8 +:_?9P7%B-j;c24a@H*eU;@'77m/E?(>?!ut+VCUri`&l8>F7D5PS?IL-+gpDRoDc_+VCUri`+EFAS#Fj +DXQ>mGFuNgH$Qs1o$*X_T=iSf-I+UtKogUG#JVJ[#6tLS`l?TCo%.5u/Zua8 +rH`JFoB4G\*^+i/`/.2PZRnk@F%Qc_&JB(5,#1R\NupS%MtPf3CTUg,Q^B/>GZQb$`.]9>],2At<;@&+='bR@<`\t(*5W."O[MaYWJEMK($=Z@[eN#6`VSiIJS&)E3M3Z+$=d%2E!HX5MuD.gpqLNT,0,_>Zb)F)&YGRi4uBp +Y;Z7$[P2^H.4KpL$/I^s)_*%gOGHgIE@P^kS=H\2C<^sX*ZcA/>[2+%5!;&7gbLrQg\gQYLC`G*j*UOg +D7u#&UIUA'q6[RaER$!^K[ft3HjDZ16kUXTV]t,"e8Ai/jiN]6XVLcU.l[KF8kl`JER$!^K[f+pIg@u4 +6kUXTV]t,"<&RsJ39QM?6L&0J(r?^sO[MaYWJEMKQ-C'kc8d2)OLeImL2]LK,S:Z/.O%VVA[@)IF&#:M +V!_3U+l#m+8VBL%MMP2.fkb*VS5W;kW=)^A#WG^N&g7jb,2uR$2HuP.kEHZIl9_p5rCKPuWJEMKQ-C'k +c8d2)OLe0JL2]LK,S:Z/.O%VVA[@)IF&#:MUomV7+l#m+8VBL%MMP2.fkb*VS5W;kWI%Td#WG^N&g7jb +,2uR$2HuP.kEHZIl4VVf_@76B6qG7qLc?-7bn_YO3@0B@\j<0JE'm&>&JB(5,#1RF7D5PS?I<"hd'P)_*%gOGHgIE@NH-/PM[U8dZ`, +XlkIY6U`74&ofBF:.8a#Ko'kG8Kjm-otq>YW/)]c,#1SFWMjXA0QGAe.>1YM>8XeD8AqK`3#._'(+.b.k>[^F77u!qrF%8HMuO'B7KjL8+DAZYI'"?F&#:MUltAr&8nG&,XN\M +7Du1(V0FD'[G%4eS3;4BEH,un.q`I+%Ndmi]Q.t1nG`C`Rs\CDQoO8n'LF9:?ObtKMF\p:i`&=9MOKEJ +>E%.tl03N-5!M8=/DRM8Z$!7?S!tq2S=#[G>$?7Lp?gI=Im#J>YfW5pD.2Fo;c3n#%(7kVJ<5Rc&TK:8 +3e%#%aN-p<_csTf'cZl.bEm"J4EKZ/d*PVT_a"2!o?a0u%j!jckVLu1`&X`k&5p)g)o<%!O[MaYWJEMK +'g@*E==i$=f<2N!=^GDeDOHr^>'8rTdSb)^SND&(rlK"cig`qF#Urgs%cN/"80T>=<(*76$MX#H0,Ele +HhZ64gqS@=KaS^J^%1^Ra-2^N-Vg3,pV6_k0eorT:DPngg;;Te)?+>`BW0C(JR0Ip7.!@Oe+[ETWJD6T +,#1SFolER(\N2f6>:e-$Dh0c"0JG[S^1uM^qSq52@-l%j!i8p&3FAfYYQeVkal%QG+BV%2G!n8AqK`3#+=M +IV\/LI#mpU]6B/`Y@#'$IcJ#?\]#=2c8d2)OAb`B&8nG&,XN\M7Du1(nb5+Vh7.H7eLbROgABmKMrOGu +R@3af%im]A$WAP/d'p1mT`2SPP't"bI[T9FdJ#gudn8R\P,u%02(UqW6cF0mGR[Q9^n6kk1 +'.6O;J,XhZ:MU=OkF@]%8.gSqPDVmS,iVckS;Q%&XK4O0nmq@L(Yhunf8BsJYd!W=.rA_SS5W;kBu^Y@ +();ru8AqK`3#._'(+.F7D5PS?I<3^\-:.O'S.Lf4n*.Opo>9Up/r$6a8K+hS*rO[;UWWJEMK +Q-C'kc8d2)OLE4GhLpbW+XH&H7%B.WAd]:8S@@eU:.:ri&ofBF:.8a#Ko'kG8Kjm*15%k`7ALJ- +E@N/-7:ifZM/uE`+f*@#_HW]X;@&+='bR@fLf71(Cgl=n;c3m8'8Gul +WJD6T,#1SFWMjXA0QGAe.>5&?6[7or6jb(LV]t,"<&RsJ39QM?6<]^!]IbTWKoh*h&TK9MRk7=(F%Qc_ +XihL:();ru8AqK`3#._'(+.k`,.>1*ZMMP2.fkb*V +S5W;kBbais#?4(u+VCUri`&l8>F7D5PS?H!B*dsbMF\m9i`&=9MTTN='BbmK6:`\_n;aK('LF!=7Du1( +Cpl$;c7(2qe78hTW/)-S,#1SFWMjXA0QGAe.>62]$V..O'LF!=7Du1(Cpl$;c7(2qe1cQn<(%':&Z6i4 +<)bb[QoO8n'LFR.j&ADR+VCUri`&l8>F7D5PSANeOs-?icRU2tOGHgIE@NH-/PM[U8r>eCaVIY!k7s+J +8AqK`3#._'(+.6oP!i3Cgl=n;c<7)8;;)X +^9*2r66]/n`2T]3GUR=7g7H'L4aXWi\Y&dMUSFU]p?gW4)I&RgkEH\Z)3Q/5X$8*YSWJY+&TK:8R[KQm +7dK^=FM2"QT:[2F*#on:g\e5h(DhS?(1n@BVkiP&ODNl\rE0`sKLD>fLf71"n`.Y[7RgQ'hR[j1fW[#9 +^>.rK=0Gp>$k*OT&)4tf!p>d^I2J+jS?=FcE8AqK`3#*1rhnNMLepI/Z[GSKDs8EBB0!'EujQ,CbT7?k!_hTg] +iU+2bs"lPheEOS10BfHug4sXFmn6I@X,8CTbt.c5$ep=N'SGG$n]cI!g>/[XJ#EBlW\' +a,c-CqmnJnj0J\s7>AM`aVTY(dT7/\*[),n,2uSOfZk7=>$T7g_\Q`;\="!C:CLSo&RoVed>/\$PV]nP+m_!15'jG%p-\e&e]16,#1RD +,#1SFWMjXA0QGAe.=*Wm6[8@u?:AshOGHgIE@NH-/PM[U8r=YtaUmlr*ik)l&JB(5,#1Rc\,g7G>QUb6tZg_3#*%' +U9`Ih`&X`kk`(Z4iQg_JMWd#C&Z6i4<)bb[QoO8n'Km"j+l%'#:_DMK8AqK`3#._'(+.tA.*Td +%obqU&/&t4,#1RF[Ip;]16uoU8nQV3#*%'U9`Ih`&X`kW+\_VE*'#P7fLf71(Cgl=n;c6RI,X]'L%,pX+r=r0966]/n`2T\O[P2^H.4Mhm,[7eU +FalhZJrkde&TK9MRk7=(F%Vb.k>[^F77u!qrF$cbKIki +04)fLf7/ZDJj@(h]mR@k08>Op'hNCDa&gn[P/Sh;c7]q,X](#GWI%& +OGHgIE@P`$rD$KSDVS?:9Dc>8[@<\V/H47Z(GB[fPMLf4n*.VbE?R41VkPq,r$p%m.8:Bq+0>LWA`K*Pmr +A+T<``Ldq^(A@.MGO3teS,`-Un`,TmZRnk@F%V>G&'4tB4e@[)+XH&H7%B-<-7:0V[9E12)@[2f0.nk8 +DkEVm/oI)JSLshgj2R($Iga[O5?Ve1'BbmK@[BIQjku`kl%g$?,#1SFWMnXqb0#nq:Kf8.Ke5ugF%V>G +%-_.!Zto]VPg.Yh.>1*ZMMP2.fkb*VS5W>P>"FBe&+am2$6^"Z,2uR$2HuP.kEH[m'9X*cgH6L=6:f%o +Lc?-7bn_YO3@2WsKObA_4ASJC&/&t4,#1RF7D5PSBsKA-ho9 +:Ih2l&ofBF:.8a#Ko'kG8PrebL2d1^F@6HZ&Z6i4<)bb[QoO8n'P2sY/Yi3&V^#(FM)Z4^VkUKM_SeI4 +Udu$<&FMoWN^t<=i`&=9MTTN='BbmK+u^c7\3G7",)),7;\="!.[r"`S9%;[`/kDd^?+U8'LF!=7Du1( +Cpl$;c7(2?6F)4?S>RT0#`1MU&Z;A/fttZfVkf-(A-h5[L_N0":.<+#W,/mrER$!^Jfu#K1cue@6qG7q +Lc?-7bn_YO3@2Y)6i,CQ\T`u>nT7/++VCUri`&l8>F7D5PS?1eaU4o9V^#(FM)Z4^VkUKM_SeI4UtBQU +^)Y@#SrQ>b,iVckS;PI$$PV]nP%%W=)8`Judh+$^7%B-j;c24a@H*eU;J\'5/!s%U3[09>Lf4n*.Opo> +9Up/rMGJu6Fp/i%XQ&K#'LF!=7Du1(Cpl$;c7(2#&'5/j.m+i76:f%oLc?-7bn_YO3@2X6+o-.4>Nb7, +'LF!=7Du1(Cpl$;c7(3nL1CIo?(R'I+XH&H7%B.WAd]:8SF7D5PS?PW +lV:hTR'#t%6jb(LV]t,"<&RsJ39QMc/!q$BN^t<=i`&=9MTTN='Bbku@0Mk9dh+$^7%B-j;c24a@H*eU +G$pL?C^%.TSrQ>b,iVckS;PI$$PV]nZ?7KJV=J^r$6^"Z,2uS/1Fjt+F)uEkhS&gIQ*rAdc7,7;I/gWU +Y`OZ>&LiB$S;N,$d1-j$>-1i[o9ZdrcRph&DKr+8m`Oqb5 +T!$]2oCMR+K7dZDA"-_A$PV]nZE\H$D^nJTOa5&i%rKoh*h +&TK;SQM^FF(X)R0]mK-*mbbeZ_M&@:rKU-m7L"1cjW,3W)VWkOKGG;b6:f%oLq%o:BEqadG':=H_Sm^: +^N!Xh@!m@W('_pfKBK;CG4rQRm8QI\+VCUri`*9:[P3fGr26^t`tLXV.O'S.Lf4n*.b^3!\[h*VO0nZ, +,0i1pKoh*h&LiB$S;N,$dC(mh.I5!5)lmjjIlFK0Cgl=n;c3mX3#.QB8AqK`3#-;^ace)B7n6c*qOL7F +!p>d^66]/nU_"/Y.O%VVgGLcC;p)c2qc'd3N&Ya&ofBFKoh*h&TK9MRk7=( +F%Qc_&Z6gN#`1MU&Z;A/fttZfVkak:E@N/dOGHgIE@NH-/PM[l80K8<<(%':&Z6i4<)bc:0QAQdKoh*h +&LiB$S;N,$;;"(K`2SP,66]/nU_"/Y.O%VVAb-0@V]o"EM)Z646:f%oLc?-7bp,oPMF\m9i`&;Q+VCUr +i`&l8O"bQ',)),7;\:I37%B-j;c241_SiF0'LF!=7ALJ-E@N/-7:l&r';m$P#`1MUO[;UWWJEMKQ-COI +S;Ki\,iVck$6^"Z,2uR$2NKP3.O'S.Lf4m7KLD>fLf71(4Mo%9L_N0":.:ri&ofBF:.8ac#SX*[;@&+= +'p1gR`2SPP'p[#g9Upu2+XH&H6jb(LV]t,"<&TdkE@N/dOGHgI.>1*ZMMP2.ff[-iWJD6T,#1S&&/&t4 +,#1RLopt9C?&O041m4C4#r#p?^Jl-"s4F +&Jaor_h[]pqeklZ^!2Fp*'$oecq`IIWblj"FPjf\S\>n>d`fqZiPUG?:Ig%iKD4iln+8c;P(*?!a4A"@ +VbWegLtS*GY?rA6Gjr3"IVZ^G95_1LD+.cOV^l(0'T_>IoQ7cQT:_`/?8db%NHP(m;)-sWqe^=Pn(t`^ +<4&m6nFbC?+#n5435;#u?W:%qN)Vnmfu!)@nZ&7=ZBkci!gUG/NdD3PJ0>Xku^]+(F&^p@N*;Ei2!Jr:n@S8F+/t[r)qAq9,*X4*[*jQK@5i]mKK[ +^3mJ69C]dW\pF/@@AiQ" +eMB3Gq>'3GJ,&NL(1mf?s/8)0r4!$s`/,0-rqF/T-78XY*]m3*<3+PuqWXp8J,d3,nTjp!iGZI+VUKaH +$7H\XdaD%RY;t(G@_Cc6lSbbV/MFYeAnPb7#B85jM@9n%N/s$Dl7PqV:S'Zt'(R8$m(h)J:Ra_Kpp2Qr +%c"U9VatKGGkd1M^:8@Rg8.H/MPm4JVbVLrBm*ln@Ud[@NZC4Tp?gU0h]o.3eV`m[RiU./$[]u9k?sJ= +iPUEU%j!iSk0/Sk+$+gWW0jEXV5$PArpn5cbn"Z"G2W)]gVlBlVb`q&NSasO*j#4P7+dCgkKb52FJsEj +]"G]Rr;#rXTqTAZ&"CIQSLmOj-.@'ait<3["Z3ScI2Wqr>R\5 +XLcB#6>*nW-RXgm/mc3CkrV43NJrh_=cX<8f"H=uU1e.NAu>OH6\c/NLi$l*8!R(]_Y`W!YKib)hl44U +jfDW???3@SlY^,a\T=<(H#e"K[.U+OG\78YVP]5eg>Cp2[VKGcA\$'N.p)H3gK4r^8`5G%KmhOBYG/\^Jc$$4ka@krCc!UBS]^2`OlVF-:*?6&85H"!B +$O[?].thEQNnsW2juX^%cM2ZnG38Yolbu)'PEV3RbKI(r^]*fI08k>3hS4Qtl2*ogS3(qtq=DJBX/"[H +\WN#:!s]=KHM+;epP[0r]4/G6B"M)Dj<]"AMUr9E>-7VfLr7-JIf0E(EoY0!Fhb?7O2)<8$aX>349lB< +Cd&H%3-!sRoB2[Y7o[fi%j/g3a7o#XgNjt+7un^6qG'm6)guq2QeZE5Wf"IM0$h\_b*+2K*(^-V5eUJW +-Lemh6Ds8Xe^f+jjR&^6j + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 381.51931] CT +[1 0 0 1 0 0] CT +N +-1284 -71 M +-1284 285.25 L +319.75 285.25 L +319.75 -71 L +cp +clip +GS +0 0 translate +320 286 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 286 0 0] + /Height 286 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V^M@-:"39r[_+@n%r!,"@U7n;j#Ciir<(,dk%dIG7'a-fjE)I7@E#1ge5uB%2"VhUa8o[[H&8Dpo +M'=-t#V$TW^(1\1;"EXUr$6`3_s91EHd9/^IId#UT6!f$=^>Uj9W-A5&@QcGd%U.Ep<_79dcHg&Ie1fC +^]!l\kKfbBHes2Fh=3(X5KE)0keRKG>^PY!='k^dUQ8aX[F\c8rEXX'[=7cIqsRlVoZ8JQXIWnqq)8[o +HXU>+q/ZHlrDpT1qWm/\I/j0>fCqE%giM6CHdb\JXhX8$T7/hKq!`P"qU$nlSULL^oK/uC>B0N3l1#3( +5CS"AU/tZHjbbRk(jk+cl`J]3rJpi;5Q:GMJ,]9BpYV$9l;^nk[Cj/bIWWb^B:jZ0rQZkj[QO)u'8QU+ +EH1NTcT\u-ZHJhclD`<.rSE##mGeY20"Uc1CVTlFlh-HXfC/(tDl2DPV;J#^rq^Ec.+b%d\$KE)2rB!= +Sn`KJr+kpdVt@UID7ILO&cL!(rU+b'AHr,$F!jbT4EKZ/?N2VRSQ#Ar%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EF +Di9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>oL^JfkP",^^hcsD?XikK +d4[NY[cj'JITuG;:;8CW-)]W9rK>WJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?- +I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs;D:(!srO1pVS:4]7 +92<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>WJMgie4[me'mo\6*I +E]`Z4atB)U-LDt\mk!h\*aKYNDWJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q` +^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs; +D:(!srO1pVS:4]792<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>WJ +Mgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.\'of(m#oJ$dN/JecGIlod?T:9VXD4;o\6*IEr'Y# +000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.n +am$9'^Z>%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG; +:;8CW-)]W9rK>WJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIo,g@(83 +17:hi6X\eSfXX/V1,.J4rKin#>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro +/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![ +d4[NY[cj'JITuG;:;8CW-)]W9rK>WJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?- +I`^i`7DIpUgS/%qqJFV53W6DMQeY@5S[(m8P=^*,loPS:[msL(At-C3'%:cWI`^i`7DIpUgS/%qqJFV5 +3W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.nam$:Rb`fAj>0K)hX/O"]PI$-MbWIndW5ZuEZ\hr14?9I@`QXOP +MH\4d3NK*tb`fAj>0K)hX/O"]PI$-MbWIndW5ZuEZ\hr14?9I@`QXP#d44u/^DnIGU^]P +*PGMRDrIEZ3!/mPpUieoIM2XeBWmOe>7m^qr-KKo+U6i(@U1KMo:pYJ!"9\).Ppu-uQ7uj*'IC_/W"6f +[Yo4'skHKh9r@WT(\mt>i0>882l.E7N\))Gsp`R8_]NcKD_6gc10>IF*D=Y__rUNa#=@M8q4#Y-sfD&VLg2! +lTs8;KK=mXEqn+9QB?+9WncY^/Ip;.1jXDR6hIq$;FHKUiC`o_LK>7Ym>q2RV5jk^gbV@>?]bVRT#E5N +,,IItq-(Hq`hdoTn)oltKMm+Ledg!;RXU(9KdE,<8ak*q^OBAA(\HgJ#WkROm-1]Qs^%r_:0"O0%bCfj +-`If&Nb4E]r=V=-EbjG6gClScubhgbZSi=[fYr:9X\4C4!Fh/T`mD>l!,/[sMUr:ogTc(BLbH+[QJN\s +:MODXgbkB3AGq'S)(U@I2floB(F`45Rq/M5e&hgJ"U*rYK8hhm%KORu1`b>gA7cf3sDg@Nq$A&llc44b +Aln1UlEXGgtFr:.i$5QCcazzzzzz!!$EPr""0p]0Z~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 347.4] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 356.25 L +1603.75 356.25 L +1603.75 0 L +cp +clip +GS +0 0 translate +1284 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W0k_5M)o1uA6q!i/PCjRCLo?,cV21"b>6b`S*/]PC(NFC,6q!OJEU)&M30YTs;FEpQ3#%K!%`L/F +E@<"TKb@^g/]%2NGf.4ls-VsE?0]1R5#ZP$oe%6A9DX_srUBsizzzzzzzzzzzz!%?\!oB3`s\u)C'>?l +[:_M)h)bEjkT]iP16!)O`C0E;(QIiG^J!!!!<;QX!scVN41^O5@>B&TS_V/1S?Y)9Q)[^U"Zg:5I0NXG +ge!$upbmbQgZi205<\phf93-46+r;:qj6#d[SEokI;mFsjSr-75>M1#91D;/sc^0TN&4psT$o]Y;RMtq +8&]=YCtIeiPG9udD;!.ZiKcH^=a]K$pLDqqiW5!D.c1=G(;T1u0f%j%gU.F,g+S@`RgY.]2V5'ZSTiPU +EQ!s@5B9:%9]fB6pqH?smaHZ.cRH2mmBrVQ>%>I*dE9udD;!.Zhp5(1$4n"4gmhOSN]r:tD_ZZ-cKlg% +b[>g0spZEeN3aH:TF=at;rcou^MM,LhcI!f'lO +k#iN]Ymiqgd=a_^"fX&nn^X0*TF;29$cBm"oae:lmdBN3n*fZ2D*A=\/M.]]P24>?mb+ +oo^U`Y0<;L/ADWSA.!!#iM;lgq*F`,rM*ET0.Bs>efS$Kq$<'/KP].!!&1/oB&b*HO^Gm'Km^6i^VLH+!ldjXh>BfGdO_oLBoP)%@E>Hk"]QnOW/'hRL +CV"BnY5h7ZS6C%;j%\)E@N.=+VCUri`+F!\[f7\a,X_PY$%,%E1N/FOGHgI8I1/4'bR@<[9?mC*Z5jK[ +r:0WZY.P"FGB+G,_SRUMF^')`2SPP's6$'_SY9LLW@ESZ7jG=FGB+G,_SQja"$?U!(t_sZh*bYo!`PpI +f(1C>F^E!C)$[)d*U-chZYn>+VC':U@j!7#CsQpHa@[^f7iIV;(q*pJrke(US@F1[Kce](&WKq/54)$@ +7W^U`3i^pH9]:L!6kXr+#")O0h*"C&/&tW.f,p3U\VfQ3$M*d`2T\PAmma?F\395,-J0pF"V>!E($4RL +f71*beeI\l]`ZJ6sRDLP:mLhNegUGE@P^m=dY4hSW\YfLhlNZT\''Ynu)(u7%B-j;c2;n_K:b&PU(oMK +_^sf4<+s2,2uR$2R>)WY#CJp&oBE+PEptKi`&=9MTTNH#2nCa#Rg\PaaX?B+4)k~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 347.4] CT +[1 0 0 1 0 0] CT +N +-1284 0 M +-1284 356.25 L +319.75 356.25 L +319.75 0 L +cp +clip +GS +0 0 translate +320 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0RYt?6<'F%qWfg8(0$S)gY74`;ob!N>bb0E=)`7?m:,uRhNiWE:bLffIhL]e*X5\qu&ln'bVAhOKY +2SnaiGKn7fpJjfQjjkkj>05%FImlW.#Q<6cP/gqH1I +At.(d+'d9hIYPM[d:S)46m/*CHbDj5^rDtI-+7c_%UjHi2^CJ0+*jL;SYQU6.:97A>N\$k@aU:9rX\\1 +baj\-A\P'NqcQVLmHY-c;5;tVG]=qeJ-?K`Pkg8i8>`S2:[)"H0JYM@^iD%h^J'M1T_UAI=5 +C)>&#l4jXWhk8*QDie'k$9^QXkFAROuqn'b],4$2P=b3XI_b9ACthYs4sUDq\q?WNOo~> + +%AXGEndBitmap +GR +GR +%%Trailer +%%Pages: 1 +%%EOF diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_radius.png b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_radius.png new file mode 100644 index 0000000000000000000000000000000000000000..889411ede8b1c8e4c437efe6355a6bb93ae2ab93 GIT binary patch literal 56819 zcmXtf1yoeu*Y+KdkdSUkk&;e9N*DzMM7p~hqqkR&6nN#KgS4g-1W~ozf5YqsT|~i)XwEY7uh9NsVqoKQF)6>3h9DY92L4jTeQJBw zO+(q70(Jkz!xwU7zanYVY0?81=jeW+`_% z3cs8{I7Ur`AL@-#jug{h8ppdE74oE+5@CIGE1)e4?o;a!7YCjx@=X4i2v% zs6*h7(mZ{-&KleEVy?lfC*BfQaphYu-IIZERVL01b&ho|9J=(WAYJhwtGEoQaUTm? zUH$FVu}!XFn)L2+yV>LR+SMlfl({X21pTwA-YQmcHuUXF#2*^myd zLd4CZ{N^)GmGbr++x$cE-|)adD-u`O(>z*247s;&!}J73)e6F7xOWCki zk6_^`vi)KI0p}Apph+4lR)|iB8?kNW4a_0`zB|{j+WZ;&2}YHzB;FDYU7pAU%J&@9 zcT6Qtt?1bqlapi#_t&4Qbs0`9(Bj*Ct4H;j3!9FP7y(xay}n5+%*gQj)7#5FH8@xv z7lf;*>f~l0g%s#agh2{^EqyBBevhC4i6Ja;ABLCk(N_#Te(}2f!6t8J zrlqMSd?yrT3rhME)6@Dbc$WIRMJk?<|LI2A$~s`_4~|5*_68}fveDBESj zs?iqgp+v}}lq`pzT=1`!{l#JCE>s?ms`|z2Y}ZmsN=gc>K|~zY_M!+MXccmgyrZ8(Eg?l%qFf0>`?QD#8;m@8&tq}qQVqi0l~K!q zBtfY=Bf60^$uA$6nVH8FyyVo?)w2oKBdiI0A@aWP1Nun5>cmLHh6@gK?`h(m}!HjnbyJWt_LL2&cr zV*5SG;VfIh7InYJ6`l*KjC>$_w-wJW*^yr_A>af+qz9$;drDe_~gH5(mr46G2%}hthj&6Ol`x z1>_$upwRc>^2qytl*23^(hwt5YJ3QrElgn=?Uc<{5L@I%MQHj8>^oA!>$laOd7 z(ZX^gh8x>69;rkwS;%GAZt~Yk2UwGt=tW#v+0pU~x83vWyDtsW^a5O5GTfY;olI<$m64$?<1s zSaIZS@bxaF60lcw3<@dU1>2>db~YI4=}BL`dNr9O;#DcBrWT_+X>4X@b}%$F#3Lss z_a;RN;&*d!K*zzt5JOB43N;@81L=!@N@W2NB;X{wjf&TP#I>oEgo%sfbe-aB~ z`Ygv6K5PBSXx)3`8B*bMBA&;Aah!${0hr#|KYv=**3|TMb$978%AQV%258JqgG2$I9TV8`^sA2!!-M9_65 zgGLhIA8%&s!q0h`ELvo;UUJBSe~z0Yx!9|#t9KmR5|WbBqBZj$kkMSliqDmaz1wDM zU0$eECa0wAs;H>Y8~v>a4hvJ6tK8A{o0=B5TuijDh7;o9VK(~(Gr3J~26aq*ucLhQ z<6N+#8|F|qYexeDl-jk&V<*~Gw|sM{_pRr)CkiFSx@LNa!#!l(TsT#F=K*R9oLy=g zjrut%54I;|e}BKFY1PX68do{FkllQ$XM%#0)iYTw{rzD)AI2m~OAS$PE@g1KOeR`D zFB=)p_wE_5p4q8W5MEWiG*e744QuofJNujp%i<@(!ZSa0hE5k=5r6he*xt60(a_Mi^7{(KTFX_P zPF1pbAt9d7;eGjlkC&StyMbhdZJ=F~&PCL`%6K~D6@bKSME(it*8Wsm&0o9nq(p zxI6#N*=JA%E$f4M)D|V2ambyCS{Wim7C&uu>JER|<@NiF#_KgqCR0tw)n62$`0ycK zVLWO>-V>>?k&Wr9y|uQsHYF%Rn!b(4ynp77r9-R2D*EPhXL_Dv`tN(@? z3U#?&(_pVNx-4cwQZSYSM~0(x#RnRjoxZ}nF$_9*sGen9eB66kH?u>>|1dY7fH9)Z z@K1;uF>(J67!1q*{(NyE=Fr<#sRV-yxdF7FxY=sQjF)gYJpbD_{N!I9bDmp;^L1`} zDF?rb@ra4dI4%EXc0GOibP6&sGWte}kAH|ZTs&LrM(qinviznb!Lk$c`|11wqj^mb zd%+1>OFeME$oJrwN*-RguY@xF0t0jVd5vP-I@sC$8W!7vVI&-SSUDEdr5dbz;KSFl zX;(h>nOvH$_fXzkTN4mrWXxuVEUc}~IW&Knq=kmc(}HV;gcg3ylAnI>dUN@@hI>+g zobfG!00|E-*8}GM@Gd2qheDhIK>@v$SVz^0RA+`xdS?4gqatemj_Y3UIaV_=<{A)K`D=)90 z;Kuyyw()AWZci$l$|p8gnT&v-M~;%xQpLA11ha(zZ}cP5zDrw7o$b}f0cJ5uUtZAA zM;m>GHgQtugBc_BZ$-dVIQscirp$)Jn?zgqJ(mM{Mi0I>xa?(U^ z43^DQJKE47$e?OEVZ6wM+->$B@z7gnJ$)tx$v^|;N`ckQiyDD+zd|1$8&Y12#@^ww zPq5)1o7|qq>oL#Niyl9T?Wus8!SHMc*C64u8VTfKR}SzzeO$&MeBHU>bH&bsMMuZ? zr0An@KbzZ?2=pYibene9_r>8u@WHdQ z*ABF+?0g0m!}VGR)8=)rZm%-9C|}%K7wXZ`DIu$J{Tb(i{E*zGG!59%_F;%mxD3rX z`K;H%JtAuY&_A0;O6u%PR;3Rny-Y=zcT;~ySMem$BBi9v`W(?lt@`ET$B%wDHsN2tT8X%BPmFOmVUdu;miqeo;>4Tn zm>3V!C`(Bt=)8-F8n|yN#SqrBDqT&?QWA;kZZM1$XbP}3d}%zG5$_-|SEfdql;V@HKw8SG+il%L{S}bXN)~@)uc<>{py13P&n7028*+y zKjU!rUcB|wg%Os?2j<1VKlbf4nEP;YalHw*#Es0;C^4}3qSwG%g!aJTo=oNuR>7Q~ zon>SDKlA$xA;D&ts`kX8CB4N7I_Oz9k>8>DPA5`C&etG#wO)4EsQimMR1ZLqR02{` z6jMHbHY<{V$=|p3pKqXT)csC8&CPYJENTksWHd|sB0Izo<*F8%cxYJo z`!OVPfFvtj0@3G5V4(aG7M?l*Qpp_&*e=z@?HS%W&9dDd7_0I9pSC}^OTjz-&2yTM zO!ELi=FjDCk6e9A$MNHLNKjt??G4jR)t<_8$QJBT9vtxWi__^(Xs_so4DUD1_G9;K z%+kivT{*N3E#%Emp&8r-H#TqS!A2m)Z?v9S2jL(No{KMTiiAnf#au;f5c#Hoe2cD6eo?IzA3D zxxaIva;FnmxTD74EmW_bJwZ@`VlGjgQ-83~lfC@{x!J3Gx{ijG=-@BMX2Cc7Y%F&*+NX-tm9c|QNA%S2 z0URFPBB41EedD37nHkM-`}e!cSQKcdhoX20FUV8<-L>kv%hyUKX!bjil{OSLnFYUN zz)!O_Sq>l@FQ%?ACx^EpMFa+>(BU+5K~d22fAg5XV;uuHbALP*=Y*`GIihRkCU1YA ziw-?E;z1s$5{dM|!V7fe$k!#fYoA|TrJB;I!~;I4G(5al*xM!3fKM4}1i;AgU#)+sT^hSfO~zMxDx&@=F<$!n z;XM8i&7gmDvx~nvSd3}iQ4qIKBN``BvmCna%i^+zpdW0pBou_itCrjtc^Gt&Ipy2- z@p4(T(QjnqFjQlTbiXjV?g(9f2L*BeK-jAYBZxRk{luWq?^O!1es-qpi*pZJrif~B zFRL!01r9ouEl@}L&?C8zdEJ2J&P-KUlh|8XX?61EaQj?Z4&y8>74wk<6ZM^LzZ3@i z+S76+_QP|We_st;a=w|;qQ2!jM3nyYgj4WMC>j6uGWL!{?m0{Xx4+nL{#<00Wbw-A{ohh{# zAD`;@-@n{VHZPt$k*u?M{6mMGM1~$8Mf-zZM`xs6D#O!P70he_OCOM{dw^7^rxi+p ztU+Xbx`Mop{3GR8^EULanb%}weACwsW8H<<8bS3gj_1)y#+#Hoe z9!Q$p9je&!%64(aS}yjQLBS+;#(DVAQn|_ZMuUPP(XX#JCp()lwr3^1EttejtI_+S znn6AxiI0nm*~oe@pM|y8>wAQQarWoW+s^O@9WZ>nhL7%0B5iL=4Ys&TA+`?;V1%s{ zwEeqzvb;R6RZ>$I3LJ3^Jy@pJn=}LvLCk`IG*m6^#>V62N__|P4@%0%!f&syohIar zjq}IZTNjs>u#0qR4qa_*Y?>4JtUX0-QcqjeB35sCNqsW%2y|3rFBlGZl0T%^{bhtU;sPvazf+ zuHCgQ}?VjV4gg`;zp!~6>6rMZ5zJUl=D-QM&( z)?bbzV0@c&-A1DCjY~~!zn|VVZ{Kt?#`XNU`|kmfX&rI#? zVj2>)^Ba!VQfCK?W~Z5xUo`oNYqE;EdEPYLUgj%mX=xpupP%oNu$KN*V&*Ud4-f^?)AY+51P>H*D1Dsr`vCvT3YIbPBuLr>z6Dn?irP8XpY>B za5FPcISu~U&@L$4T2plv1`N0N`}glsMy1cq|7Ob2a$JgnGBGkTHhvYwsGw(LW~SB| zlNL)GOA=((xA}q^;bp_B_Mga1lb1EFiE8lu7Y!lu`J?98kkk>~V!grP!9m@ByK`?3 zcdF3bxFV;Fx5oBnPxti4s!BNZs~ndeWjCE0ZT)O4>7)MnW59D!bb6ldtQ39vOUfYRL9KeU^I$(m{1VX(4LH zhx%YT#sm>UN2j07W^0*vxk^K(N@!Oo7}6VoPct++Gs9mc`GvNNXeahNVa5v=t1r3X zR6lEd_kSz=zPXh5H9K3le^LSbX9F-F z8@@Ls{H|i13CsZHkPm^#&EBapYJ0HfxRh`c5golaR$p`1{`IREq|xN- zvj*hErOeFCaq>?q|Nc4Zr@ad#!U%W{WTvlduQ^WJes6z9MkT%5+zYRYN5zZ0j8Iv! zUO^u8ci`{gQ&p`#*r`o$gDi;Q&k5ird15`m^|JDk1u{Jegt(rT1t6;N0*S6A2Z z>+9=u{gWF_4UOezgkLAl=KcKgtE$|{wL|n;OO3ZE;IC7*p5p)0ki%mC|E01zPVvY9 z`<4vzoU~;M(Pe|7q<(vQu1znom+C8Y)1z>KVJS{sCO7+W7nmB zw#$6@Kz+yMyza&^JA3o@r>u*Qw>OK=>_}&)^b(M3obNXCiz^$}-{Y%z{mh(c;P3Y8{NRyNjc`o2W8*P5Dwj@!}X!$VvV zZkxw;-|M`lmASZLv8hGnoSR&>7 z36+(umZtTf|G+IB9QIqv%gY&LWMsm%E390;6uusKzuc)+*L0_+;eG3Uocx{eJNJK* zbFl>MAI1UJ z73!Ipz4TyC9=jah@gN<8}ClO*)&$0I}vt{KuO>H?;rZ8JU) zG!pe17F-vaHwS>bLxLk7un9pPbQyqUYZvJ$H4|Chii;k;FN$*jXk4Fb=w9iyna__r~4o`Q9(0TslWbL#?wt zDZQ@#82dV}Sam2UaHK6UA+iOv84LQ1OYH9k+b2u>G@>Z;a8uOV{jOoUYJlH_d~<1B zU#e2C;j&Eg$?N9|f?lrXv*tUD_z#!18*fer&6Trkavbo^j*p#_8@$fOVx{T74rNHC z?vCURH+1{X$Hc^3jE#+X;NjshfQj(9$>++|t&%~cFH>eKY-dW9^Ro4!ub}Kb#yV+( zol`ZlLSig?S4ao~Aalw4ini$JB%p;ouCd@4en%Y^7D`P@p3sq%HD@X6W_RbIoK;Tw zErC4yYS}j^sLB><9xQoV8Wi-(@l}V;FB6$PYzqF~1Rhw=R40}W69Zk85 zjZNI|lP$*UigIV_$tnKc{B^$N_cg%qDHMewOG_Q2IYv2w(j(^L?EF`&%C13`CHfEp z=cWhz&8Dbt(il7q0Srw#C607~>q{*tOLF~I^R&-{8uBZ%lumx|U?EAG{&cQU#48RM z3;l4Fj?dV?cN6*h5@@5z8E+0cZU$1veDCS?(HhQ<)75DW6p*f>tgbFL+uO@8By!_> z^YNn&CUq(Td)}MiaOPR_`x>3&4r)JMzpcWm7Hn+n;QY%nA|j%oigK2pyqEu%1t@<5 zqj~W{yG%$gN~I%|f^knib&ktyfJ*q@klkF8*N=P$oj*i~AmGqzNJ}yf_j6NV#FT^M zhs4}m>;!?cCQ~D&*e%OkTyyiiUI6g8_2=iZ>l<#AI>KxZhrndr)eT>H9b zXGQL3OjmTp_8+G^&O-UIo`n zN$GP7g9k%|A`vxywidfVe~1g1DeF-RqVX5!2gd4dZf83@X1x=7$*Gn-F>Ik=6@rw| z<=Nhy?cME-ed$ljR`l2|aF;}%J}nV*Mh}wn2h(oEpGzhPdW+ooz#yF!T^niQQOWef z+%LPchHl#vQwLpLrJz3})tn|^Oq3d@Ujb#!%k z@{gQ6-Q6<-zI@rbJU`#|SW9+mnLK;|RoQ=evo{}J-{99|MESqUBtO<`MBRUiumQ$$ z7_{EbKi$MDE$p0xV9jxR9ey;G7C&Irf`Rjx&|4#YWx=YLOETiNX6nuZJUQD&?ygL&n^!Ba1F_7;udrlo)Y$J&Xc@Ipqv|2s^4x&3eOh8U8 zR9-7-v}hY&Y(Maz^!!3h4>%b!w4$POgMa=!=>yXKmb~xkJ*X*eytJC>iemh-xwWP5 zGrad@F6rhfcKm8Ju{nHu+XTnh2)7L|GYv>BIvDy3c|(RtD#n18t0M?nBEHJXyxizZ z^ia%r-LPk8V35U>DC}>=0G!{MMjytM$gW>Y1O)bi+1c|)!miul>_bB&vfkdFbIjIa zr5Yh>g_kp169>r|pwCMyC^&?NH;pX@5RnZlPeD4>Q8WPf5%x^Qn5*0M!JvZmeEGqiJ zJd9F%J}W1unTeUXwyV)cR?F1fJd4kIOr*j_%*SWxo87Ev^~U--899YloBQC8;2Vp1_b0=ZKyattbY0t&HsKv)8d`c)@LhLqj#jmX>6qj*G3IY;LCVFA5A zCEMH)hwX*m`qbW2aol+xfB)`UhEt3?X8~cY*5~S__1;{=KhLw>&@hCAN>$OH(KN{> z{rR*%KbFTmLIE@~77(bC;@c@J)~+h52@hA$0oLIJQF+W{y@%@`+1N+LdL;{l=6CdT zs;1}*5JC;N==KB&w?*T@H}dn8R_Ha7JHn5_zw}Fq+=IyOuc2*)+Eo~*d^QtP5tcch zEr*330g5;`J@2<%Ou$eeNI?2rsJF9o+e&AKX!;9IlyHCY^yJCm%FW-`p;K+-Wbq9S z>LR_| z@V;0uekrK^8loa2UmeGB_4LQi&X=vlz1SL?y=x!X`=ft9_@rLxqCXbsw;sX~!`h{b zB@wAoZv!%s4?aGA?KX&KOdTwB>{u_F14wm?4c(_9?gz9mg;n^>3@bL^T%i~!V?018 zen|&mLL;hn_GtT0j4!GZE~&3zEAypL5zGR~AnnlpExII&Y#P%p1qsRAQ|`lSR{?=} z8wMpO-J17(Js-x3hzFhhvc9D6onVydH$$C7@73Q$*(c$9c zROffHIm%h8q4O8)q!-^oSvV=z;>Y-SO^0{yaLo|~rjH*Bo5;q@oWG^VpS{#2Gy(Jn#X((_8jcc$>qY-?}(&nrE#Rlp-zSO(r3)4;!(-|M_QAZ_>@yvh!ijrF}`9?rS= z__6(l7C(s4?}pF5yK4S)Ls8}XU2$r>cBMDT0|=1oOtC!;a4qj=FJ4SvfSE`AGdp`L zr)ACWY}SoqePiP(AW1r0TwGmiWNEkjwr?m{iT}jPfL$qpIH{X0R!;4p1~S<;ILo1E zPdnuLv4s5x2SV0HROuDqM-9F=W|x@6Iy3B%($$=n*np!2@^^$4UTLt$T5oQWXV%nS zCUFY5Ne2hjEJsE-lx0Lldd#$kS`MeWC6$zL2A^z=Hz^nEHBb;R$URSINfi?m)Et*?=~^=GBWNQfS+47T}c_CHFRXk9sgN{ zg-vk0*Z_nPv2V&v^+dI2gi$}N z^T@E4)|8Oka-VjkP0~P((~J44a_8D(AmW&h74K1cUK}>Q4G8dH7%xs;cQ{;rT?f$1 zl8udyz9(x^L&Hr4aO6p`A3t{pZfNic%QIWv+M<{TxhG5zLP7zSs%Jt%!cTzbwm%m$ zG@hq#HMa#z`MoPnxrXaJ{u_^uH~Ov#d0%Nx6x{&wf4U@%jVUg^j^s&*5b)nkC5E&l zH0bD}ARk z<#e~{d*rJX9_luDo{RuI@|WLss&yYr38&8OZZR=3a+wyN_BlUs06@c5q?C{(Ozy+r zrvp}?F}w;{9sSCps)%7IA1;e~dGlH_j&zlg|4A;npWpMn*{kWX_%WN`)g>f8$|@>G9)J;zWPNFLdc5d+bF!(R#M}$0H9rV`lDAW~ z3>HUsFYZBzeyFJ+4;zev?Er_-z05-j2tr31`~`q z9=~he%ifgisAv?n`)VD04Y_=8 zo5zEEG_OID5Er){sqz6QWi#7~4<@vqXrWX==dcLKc_krXC%xQKpX2+(T9B_KFf>*c zqlQ}k;8YEcjP#QUIiKE{*Y@`Jk32xQ?x0d7iTZS;tx*mGf5LC7+%mPMwsx27h3hu0 zOLx!AOufPGObvMUXQ-YsFuVciW3#zTx+37S-C1~O!(m0(qhp|}jndFT$S__dyVOr5 zonEjE4-s;f%f>HE!Oe0j0uVMVB%nk?IP%TUxjLc9;HBdkV>&sxKcU4 zP2A{l567JldM8fI>>a4X(6BIGkk3228^?Kv8iR(JOFZ1E^7EBjL9r+>z|GaLdrUXs1pVEX(0gJYr}c~7ux zMf2c#Ua#;Zj5DeYoq;eT?n=rP9?;v(Hmdb+nfaE6TNMz`wr=60+6 z?tTRgXS?m_H_B#bWx4tS`Fbae@<<0pLq|9AoY(n^hK;SEfL-_VQx#`9@Q0uoj-CTL z!2xzunZTugU~*Eh5Xjf}61FFxkX|W@c^ivu4Xc2_*lkaLnZMsfTrJ$sKBLOSerdQ2 z-jQ4h-D!F1P#MUH%!Q%uQz2@hMwETp#AuZOm9JS_FMmwqeGEq z-j^F+8&;^ThtkI*BO;!Lju%`AyLW_LaMjerSGKjiy1akRpGK3bJ-=E5=xF=!@Q@X3 za|M_%qn@tn92~`8U@%y)R;7(cHpttCd#L4RJ(U%`SpWCuO6+ty=n{k>z&_I4YOd{l z{dK-nJ0C4Q+CfzTm0(NgFVy0Wp*(~|v}8S1H86zvf4(JiUg`eb4|3Q9eegUfC8dzL z-B~f5{lm>#panQqb&IVo5yX9uapm9>6l`Y}5Ey>RN*wF%bvAK0s{R5;c(3|i5SyKy zRbV0f>V=GoRh_mg=;`U11)3x=h}XP{W!Ig`0NG_OqtZCWI1E8Yqo`M}Un?0&qsIWm z>G?-UQe({?$-hRVA9)>v=F5-@yXv0n7*Vq!x-1+H?ISoPEu{F_a=V*;H;coA;YL@c zJ#&h?c2F<(c1}I`McXxKLPJ^obi;(RG{s(wdryJG)g}_6e;R9+timR z;8h7y)MEfJ6!Wjd$Hg%vi~3w%-z(FuJ5y|wKt7O3F=biFlBTb1BMn@~EMkL|`-_b1vzd1m8ZkFqdz4uQaudj%kexbFhPsOKln z_L}^BcMf8KU{g*I+}4I4l=;K7C#J&g@n{^E@okcz(`sh8*zGwMSOP8{o>fwO{G!IY z2=!C|IU8R3(DW`g5wry4ZsB?!nsO)PWw39B^C5s%k1SE8#)D4KmSjv2#Zv8NzjL`Y zt`JgL;vl@Yy&Vmz>vKv<+RT7`tBcpwYxBLChXTImoI^aU+u~~kjQmrR6TE??m9D-& z*{f4hQgQ)=5wSBhb@>C7ri{uJD=?o`vfn1R`1?P&*=xMA%aLQ~adL9<>f|X}~aKRr~@CL&V-q{)GjyIt^Uzt zjk(K%r7xC18yHBIcVFp_wgn3I*-#JxqmY-E7qtK|`Sbcb-u(RA3OG;2{^{x53V=8R zWG}6=RP(dkQ-y0RRrKbJXX8k>&A>64K9I@8Ucgz|+bqRh-Iq4yWrg(BSP{JFJRfV3 zEV6`aToJL~WKeW!d3hF!n|yV?B`05$csSjeFer%t3xSC8#iOu;6Ko-30)njQ7}s%S zZEb-H+1NC^0sAu96L3h0LQ6x#ALsFN&hkq0jfI7X*`KnppLV9J!a;R1F)>B#vl#R( zt*^&}BzTPS#f2b+u~7sFpyiNbD-!#@#9iHDsypm>I+GVE-6t9j^XHQ!2j@7Nshahm zhKUiCVU!0y1Zjk3(_a-A{{HQ^XG~`g@{7+NCkWuk@$pqKdq>_75D@$X(R_S!bMvqa zrLW=P0vff>8xbErB7M^Ly)dOB$Q87k6E^&on3z1Ar#hGlVnK5Vaj1^g`Kli6hX~?9 zyHdkQVX7vcziej_XbqKx-B_{Y};^xMxSIf(dED?+DiL9ejPrbE4rdFNw z(!?b5;3Enqs-U1?o8a^3%VVJ1M|9mhp{H-XH;4ARO}MS0DhfWURy|U_)+9k>)SH{L zmM(Vo=VTN;`nBM|4Auuu^$uc42@2|-pS*Bd$9~rVBaec)Bay-uejGLiyu0DU$t89_ zMn`$_y4pF3zi=4JI#|ujnj6hsKLF}b{e7yR3Pd#?#q_MuF`U|i2WwXq4ed(?P^Yw~ zs_f?hA8`V4^FWxx8~xNW69UDG_y-FLPzNlICAcXJ+T|uEMntcO`dgY*Ksw0pkkVw= z9ry)u5#L5es@%rrrzW#OGCFS2lk(BXD1>-P>T`mM45a`8EVaJorI?coDMF zr?_Tc6wa(r!i3%u2hfA%{$7k}Vn*vCb509RYiG)h^X1pi|h zqk>&g47`;n^)XNwalyv6dSCH6(<$vb?<4{6L+{ z*mx+&}6eQajGIw zXy_7*jny|UI5w2AWeY-0 zu4KZ`-DiiX#)pO?LEqZ@J2WIi!EMsXpLJ)1gp6z~cCj3#41&I;!1Fo-gjO<3sbQ=* zb;y1FU7aE_+EfCiU<-#hIFN_g#DE)PXkA>-K`BlGRgo(6Suwd(LCtW84A+cG39>+= z`QE3%rwebbDFR-$JfKGiby(_R)x4-3NR=l*@KKS;RBs+PF0^wWzVX^fPu@ zs-1;J%3%+Oq4lkHM9(YJS&^RkERwTPb^Qiv3W_azYilo05buzUlRhEi$?D3fb1QaJ zVj8r%^GyN=VpLx7wHZOwNSzPfku*O+7ujzC3sa)FTMI%E z-f|{!|2&CN&1VRY>Irsl6m6R*?dfOJa(2|YxfDe^JaXFHo#3+H987Z^4u7m^ea79P z3AnF<+8A=S2N%vd`8P8|=6k(5EFtD~t&b|2f`S}kZIFO`WOt{HnS8`vA-96@) zqL=U@_5=CcOlV4C-9?>5oY>zkmG#!@#}wypc-SFD&?nq>6kqgze~)eV-}81&|lTCi*JdVj5|nxFT%l)D}{_s1k**8rx3fisX-8WtB9P4xBW-Lq~FK@KT~ zDYnNrOFpquURE}E?LA*SSBBJJR}@82v7XP`bHEQ-@}9Crqbx8%47jUP`^EPQhO*hNX-{&i-ZAUHuTF>mUvioeK3vhw9b_+F7Dg= z4n79d{5W(xRc>rRz2mSAND-bZUz>tMzMvS$4`NCPydyxzR7wB`_MZP4Mp*rljWt81 zd|qO7vDC4V3&ejJx#gxT`K@C+iI&1sKx#FB3m4f`>q_a#%YXW2;}e?OEKt}djpiGF z&Vfpnp`zNMj`5odrN|Buu)8*s*qd7dSa3J>a@i3W7cR#qC$b3w_9>*exVV64p8%<} zTY#Q^`3NL9+JS6~2@V^@a7E|++nzagv{=`#D=Qo32Mz{E%_}~2f^p&?1R-N|Yuhu( z;loe~RKQFJEB7zKY9&Iy`L2zmfk_jajWnuKRD=t2@A#eO=wED)>lfs8NbR~Tn9-ML z9xlLN8KPIce@zYz4)c6>+Yw>~8K{+woE%IBGBTOaVtui>gVvx>{>sWCM>REe4+$>r z8eK)jM*8ROXZ?)f+CTw`=e7Ln2J%rB;J6Egv7uq)K)HTuRFv`*u+Fu~$jRrdr^+{o zh!LvBSEtmv1F7@6LdNa=e5|*DIo~XqtmKx8g=+FO{H}9QrS+Klf)fK92n`;PW zgdy(L`}R z7R*|eSeG*NEpqowqw5;>fC;nl*J3eRu-K>)MfPG0(m5dl&%f;-8!am)Ugz)!t6z(Y zNP%tSUwdLDLAH>gl^gHnc#)*B+jgA3`%-%l;{1GnDJ)D+P+=E19yvG=Xe#wUM-<)l z=)l6nECXQmEd`emJe0Hp7aXK=yt#Dw^8GtuVp39)P)$vZ*~P_$89;KMfByWL01jAr zGL2_QvJ#VgZCk{bm+!OD)t!~gRk?hpuP^qeyL+>-uI@?~beHb}A}K)ER;+c~n|lZh z`m?HeUkXmI(`_+#{-(x)0^UAgGuDDb-ez-|GS-6i9|Bo|A0Qq#KSj5;PGk-i^Mlq% z!>w`)b;Ucx*NeRgn(bD6bfL#f%rE&=L5$4FbvLKN;ore2u35#?pSjA@IJF)}&eX`C zgdQ4quxn3L?jZc+*8w~_!Z9-bQ|ouoHil;#)b)Q^fQJgAr}hFgzN)UPo8Jc6 zuky4s!wNUH6fr;Fa3Be0Nq%{xyR@`)gfKtc!hG~--R0u&)(uV<^6=qreBkg!i&Fy_ zEd|x%{1Ejw9rJ{q_qc07dFcd@ka!$vI~p{eolc(banSM#huk%`<9{tE>iuImXe^v5$v`XBHe!zZ;HLmXv&=0B)lqOLUg( z&%bg{o@8Kw?+#Q{RPG?SQeJU!Dh>oH03T$pm34Fq4%UaBWB^!ImX!2ZIbO~zCOLWM zosN#pdHF{mxbe31_3e0rF3B-HIjP#y$_k1=#r5eS=oebx9ov#DPfoUHYMSl(5(aLT z-(N)BTt)T+IEwF$x$q3aFx`nj{@1O~lwk^LD`)(FXNh(cRnYtjFDU(EH5v&ga7YWsCNyDkDqh_5BRX;XRI3f=r{S^g(G5j*TY-; z|Nc4v{n-pI&5Dn7pN6Ro6Kilg)K^_@oc z`T5o*1;6(!KkYxYKcFdy7#sIwR?XkO_uCrtdA4l?CjBeFyPGO)kczfbU%3+VJkI$8 z;wN=L;;fABSrIZlv$nD_o1U1^u^IgFn-B18?fWFuXr%f3_rK;qyl>Wiwss{7JQ6yB zDY(#&pFdAJySg^usiHn%?j9Z@1$k%tz#@6nH}soWHAUn9EPJK$>drW z+i^ib4^tGhy@$E-sXoURX9~Ns!4~?JY{ar($$2e247$IIxoT+%H63dCfDEm5_Tb>4 zA~7+s;So5r2m(2(mb1S>xWxj%26+&J`$Q$;Ir3xM7aKcN*xGvQXl`z94V*ip%gd;< zeD$lWjL&UnYK4r7YL4OQ)1LtnX@ijevu|o@YHP^I$alK{&`YQirePAZlJWy5ap(Wh zblu@p{_p=ecIH84?@AvlC3}yAGRoevw~(xGN;nx&NVd#uk)2T)W$(QrD}?O*yM3?g z_qneAsLv_Sd7k%kzwi6C-mxB8Wfh{{ZW%i-@LBLK(a@-kH>8$rQuaC=G03yP~D(u}%)fL;(o!tU}Y&Lscg_;hhz5kVri?8x(<3 zwg>8>-JGn+vhdgS1J#+CKV$$Jey}ohBBY-1c_C9FeW%a++3u)K1`ONc1Npk^KR{~P zkXK6FFdcAK*9;oXLzu{hxNA*KP0yU++fy`vYtsf`cn3@2DQNG%11az{G&J)iTvaMRt~!%)zX#kL;FY_8SfehWI;ji!H z`R{=;s*eVTv#E{*5T`Fa*2X+wf2qsRzPeS)4&fDBvM~9pS?vuM8t(NqKF1Z-w*COn zpz|=(nZ*mvU6xb=>61{rqXxVM~Hh$}9F6r3%uwI4dO~>z3qquwTeph45hMYZ2rr7<)f+p6RN#6w*?!Cm4f`HgRkhwh!8G>yisVVr%-megp5!%gPyaO+ z8{NGj-*sMb?Onlp?Fm_ahhID=I}4jf4J*H`mX`kY25u_zD2U|9@%W_G*l)}oYSsvo zs9RGRAd-8D(1z-iS$ER>c_Z$BT-E+>zFoQ=s3QKXhjwH#kKNqF&RQ5;3Qj=dOd;ev zp_ijb1LP?Lz=&DRG(P_TO+Y2HgqK;d#`RTVV&WvwXH1Cm@Dx~|9PUt?D5_gOeOl2A z3y&aAi%S!-3swMkWPG(b(82sZNvu3WQTdgdixJ}pu98A`J9>7aJ_P|J>_vXA+KYa1*F@qZuU>?=2{Y`msh59ALd#D1iB#^;Gdcr80awZ zZ+`Kt+yMG3_c}o7j-Jl7tf}A7LzkY}bxT^c#i#=*B3@Hf#b^rXQ)KrMJ^a2Cv_nTe z=H}+9rFjXvySus}6fCEP@Q3Tm{UD$5a99D^=a&Z>8X91VAtNIJPNUpmw&~SflP7!gb*52luPfP9+Qz{l;GvK7A{D*DHanOyQ09b`3;~Xa4tF%F zAZAKQ94G0%y0)PV0`igQI6E`5p@*i6I7ND!YH>Y1ywpfzXQ9>7SUDv_>b6%`}IsqFl2{f{(TJ@r)I$EQXe>a0#Z(APAni#xys{@nZhGkX1ctcoF0^7-?Z@f`J3 zr{acFpP2f;o+?=n>w(Jn`{>alju@3JGGyT60}j`XnXidBI5@MW+XrQ(rS~hGC&#S* z{r-8^8&nG$8x3mvezoZqjC zi+4)ieiBxjVG$@|v)B@8Tr$zqqsK2K*Lw*L4LH7IjrsRn~yEL;?sK zUk+<_*E>G}(0{`YK*2^QYdytntgI&YHaC?Q-keSg-yojnR6D+QgO86mG_utS-q7x^ zs76sF5cz`Zs{9!TK{+0OLg^*{M$uBtFe8Ro5>rJStkrF;*y`H2A#OgtByFvjVq(&y zG|w#zBjZFU4lbt=U{2NVq8^u<=;(wF=W2={8P!~LoL`&V%FeRtv@t1o29}z_Vx#JZ z#lYbF2MoQ>et~p@UH`Rd2sh~jA}^l;@%S@P&F}s1cNQ)oc{t#RgVxjeps`a1sRlb3 z^^31uxl#qXv0a0&hl|F_t1;DZb=! zyh9qTZEP6S6K>nxadrJ<9Cn%aX-9$}_s1^{U0HB1E(v|KL;Whgs08`%v<*#nrZ}=A|K%64A>J&(Bbb&O3<}{ z^zu^Uc@#uOj@e8}e2gcil;pU96d`=_jJu->_Q&`*B453>S$vVRVM(m}f1#|GpMN5Z zy2f-xL0jjUMBlgXG-E6xB~KwAr58kmCbbzyWTET_UjrE?*g4ck#zzO+D@#gre$Ept zjkx}pR7(zK3Ja@sd>Fw8fM^A846*K@(ZDij(ad_-Kl77MsE-`~M}8sCqsr*$kn!mq)YiUd^=INJNe+mqgf1s!s3Xi#^z~Lc^&NokTUXeb@ZA2(AYDd&p08tk zyhdX*`ay$#*%p{IPHc=%4WF(ZjzPMFi`0Yb|JE{F?kJxecB;Q-s{PkpDu|#!e9WW< z>L#gc+$;Xywe$G`puu2o_|hf}rFLqpFP}gF+$53etG8)blG& zUFzUB-d;^Bd_MvZ6f;m!Q*R}(61mCCe}4fDnWNW`oH*zf5)zkc@di2d1`9;H5$&Q~14-S_$rA&@ts>7kl^ zCWC6csVIM5m6!!>F+h9dtov`UesWb-el^k2A-Bl)UOi>v*Y;dwYL@^%|8o!DCJ4v>VB(>6XX zo(1Al^|gcc&Q8GuN#EU(rKP10FIZ{e1)Mnd(Oo8I5aJ2-ooEoaefuKBP+aDS{^<;_ z@9i7FE!6|c8iK_VZM|j^HxY&pv>iz7Wu&Kz-F6sSJ$FIO?UeU9c{^i4aA@vhyHxD# z>N@gGG8~)oKpcxtj*q=V;czcPn*69B7{32HBOCci68Q`RR$h)lmXTdWD{E(+LYJDl z-@;=>NX4jjcZ|T;_hh|pQD_c$ZL)=qQRy<(#Cm;XqxWtKbs2OH&LfZARSFHdnjbvK zag0$3k{#V^p92wHQKFF3ApxdidOBd#(522}UO6%{BBEUXlX-K1o4Y#$4hBM^mSKgS zY~Q*pS*!-n)}cV}lle16MMVeXBm9r;fm}^p$csob%HX(>3XwY5u_s%w@Wj(Cu^ zLF7_5KT%&*WoDy9#OGGbo^I1aC&LfoY~p2~3Unh<;+|pfR^UZ|2MdP2k#3pwCho>1 z3pQgj%a_}4sqQ>~nET^FZj6{m2B*;O3vGijruA%9aV9Q@iM6S^Ojnh*&exMQ5r=g; zyo=@t_O;Z!h)c>@C>g`G%Kz5g&wX5CqN2&L9fk&Mbqaq^9nw3uM%QIH z3jjM#<{oWoxjb*X-}ibzq)p=7-e2DL-&$BGB|}R03dJ0cDH$;8x+IO$NJm!pG)~Xk zPa4{L!=~4Hcoag&7@X@}kDkUHcf?P6Jee=QtC;c_hjnXXLm6qxOZDA-c2rU5lxzs7 zg|zN&RZiTQya6){t_Y6dIm%&P$k`x;|$cVsmetko)WG3fd z5e+S%J!`*yy~!v(V3e-!^zP5LTNf1>8BLuTa(we!O!idcE3^pAU zZ>vY%b^|WvL!aybP9p&}HU^vbdEbi>bn@fl4FL{u(*db0Jz4D=a)@~atc4mfVVcTv$ON8obr8atW_z53f%;aCN(1? zBVTn@)$e5sscApqr!#fFpTJoVt=`)FT(U<&%j7S@j8ekmCzesQ^*MzGSO`h&WkC<) zTx3;Cuft`1@hPfE>E3u9qzhV?7&)W+zj#-Mw>{$pK&C-nPR?t1WP~I+lJTJWK?MEN zSAYNVo;9#rCJLRb=VVX$cNMGx!*#nOL|Kuog_^lrHT0K*|ouAZ(i*UemASX5=02MihNOAi`)R(L^v)R!g3C6XVc# z=7bawYP8uZ(cHFw*&ZC+vPZ)sa*;2tJ%W3y{K@jI>H>mR-KDuXcU``-#m|cq6Gj>F0yh3&)Egp40Kf%~;ET8T>5;2e?~#05;_~t` z9e6AY!4Nxnu(dco0Q|-m*rtQdPV_7+EXrAllp${F)w#PV!{wgb@uD=-^k}``=?t%7 z{qez!tb*G*Yr4(GC5$y9hWeO4xNkuza7fsNdiGQenPt@>IALt0Qk1K){Q3gbIXm~g zGLog?DX<=a{oF@81;*k<-#6zP>T)%HkJHh`|1Ni8r~+!F@Iy_Er{Dte~_zX4U#bZ)Mx*GzNl~-&z4-aA!n8tfrO%!=TaSmTDdW$ zpDYsVJ!_$^_@pr$&ME2FLty<__5dtlcr|F{L*{j!i5FnnkN^Wd()fGgs%XAKxzZ? z3NN6{myoYumpxbKm6m8;-TL=W1(GIT!GD{)&?%8qP`Kz;I!|&#>|dnQ1f-3e?En_! z55p91JBZF+uKb&ZsXS@H+InaYG^szotc@?a938n>x<|66yX_M-`Xi4J{~*0JO*Z3v zX7ls)+`cK)#HE7M)ch*~VYKGSA8y_c9FdhB`nAN&N^w~a;d(+4NTB}5+w63JP-6Skw z!ek80xDVPNZ;r*lF8qq`9^uF|x>c_dwISYA}=+HeQJ1<#pnqqWWfB3U9Dl>eS?7UzQoS%X$iE@*UneEG@#$bLa>` zkt%L(ZjYc*rEae;0%QhcFQFGqM^GKdiyFg(F+g^2(f3{L>bf4=eh-(Iry$zIcdbmX z`gkw}UI-=r@seLITIxH;AAwD4?duMfY%V2b`{`g;eFYH!-DUkDS(7G$B zI@MQh@b}-pDVi|CZa`medj4f8DStWG*z83rLiWjBm}%C3&b#3e5(59xNXRXEcVx>V5((GJ0bjUkdp1-AJ=CM9kF^>>(er9vrlG%M3BhzML z3{8`A#U35Wa;Uhc#-G*jWLa5JaYFLdrO&Q&F^?u|8#Yz=WFi?yA9_xGX}q(!`N;V1 zw7RYTqFRi)?UK$}G-oB?s=il5#DsWa+R1uy`+LNb#mK}jvjjA!d8|k+K6`fjFm-pr zt(p;B^b(0NF;~tuzEkXtjf`AJ&Eyx?(*rdnu4iw=m>?Z+`t&cpc4AoAy%dnF55gCp zhK$BUEEc;9J9qeF508_(>NDrg1ZSuiWuQ(o6oTZqj=8)LojdJu4TAe!$sbLIi;}Ug z!sKZyMW8wnHf8-ZV&yyP(X&4$vpR1l3%tmY?03fQIFE5lVqqH=#6itW)lI+4$(*2n z6CM2qY$`G;cTUw@X|31w@7$?=?d;4eS6AoDA|#OiCLu9VpHuwkdFyh&crEwE1J5?% zMm@vv*zJ`)e@k!rfZ<}(Vtp+w%b7oa5~g7p{wQq$6?*$IWI+lMBYz;(sRp`^PNP4v zp7hT_;T#%8|9t2iMNCbRfw{c{_y#^e+9@C!FghFuxI#E9Q7r&)HrhKLD0rIA zH!Tz^t3hH7JlLIN^&}aoNn&7XKq-NVR+Fb-;mh^ zOh?R>#j?*uZM<(mH9*Z(p+EPLlAej;~*myPKNnV zc*F5o%PK1yYAk=g9dtZi&W2tf3r6l;ZrFE>;2nk6bxa?)&>^eoz1`ie+x-0K-Sz2d zTflt#&tcNM+38s_3vrCE{mJKQy8*%JT)!|(CxNW~!tG^U)!wxu0ybStYQ8o+#F&3(tQwERjl z=V^_N^)+GVzRbC0NFSeeEqx;X;*mz6)$ZSF$hyn5CaWK9 z?|g`?6`Bal<{zM{a3FxY7`G&(>e`IAqI}KLg6eOlQ8&5d_f-cgzkk2))z{Dcuf87l zZ`}`f0J5`o*Uug-C5sPV1h`28PuA%oRVnz^9;qc%dai#y^FMyaTNVNUpFsput#xfs z&m~-8yPY4L71X7Og56ZxPj>u(#MYsG5997fMr)O`V7b*b2QIlttg5=*PKeoAk+x%?^L~a23F&_GLH^8r-md#R=j;$#qhu6qIK80#B4|jC{H?9C zEAL;^Bm?GGUq(hoqB8q*6C^CE%E~{hA!;EJFyb6o=EG1kJl_L*$r{&A39+cvZu4!+ zFeP7w(a@7?PSB*AKez51etqk5pLV4)+u{)Kug8il@50m4)`giw|J_UcJ2?%>Nam+ScgadL{Qda@i}Zf$*8{lpl8 zb|b}}uF~3rMVuP44S)ps#DeS}9y*`S1X7Gp7q^0ZIF*)xAzw{h-37v1bRiUK>?1tW zVU!tA-SPmeeE~ZYJ;X8ZX;@u6`&3%WJyB*;?XIlco14a3&w}pNq{Go8heUW}MiSX6 zrw{lLj)iY$pu)!2GD>fLwMPgY60`t)l$FPy-}>)jE>eK>uA1X+7jeKF6B#GKKUv6_ zCFT_r75@ds#3V+*VlkYmXX@=58{>0b8~cbquODg-8mv0t1ej}TY8Fx@y!!-rcvg9l z(qJ-%U@&>n_*U<1o5JSl@-igEPzodnk07~y5gyhSn6GH3p(8egNB$bx`_r!wR&bAQwZG2r;>F`|!%%G7BaWsTs$!IvVNDSaf<|UU=GcUg{A?05 zO&|rH8R4f}OHxpSdHBeQU0fX1)o+aT)_8Qk_1?l+5ET`?|Ky3|{{ZLnJ&!7j?b2E5xGzAS$U1n8Xs zQZ}r+voo_azdTiUR1T^l5kTOoCmI48fB73{y9^j`;ifN*6Ta z9EeCr@;w1fu!c@$&t@3hdd^AZ(sqYOA{?_L( ztj0)6-lFWx#O_0^iS#2zF;XlSTysNk5ZwQvDju!F=wZSbS);OfPer*yRzC7a%*<7i z4ntkqK|%O`efRvw)3x)Z>~5?aBquXmdY7|Fi0~SG{$E^>1*j00K(L@O{Nu-U7ho^9 zBqb!=s7&<{k3&^e?#_!`t00SqH@!luT}o`=4rsPTxA zwTo~Smz8w351kaP;KKpo@5n*sbU?RWT)cNN$d$D1lEsQbLr?2q!9H7qiIwA=>xRau z6Zw$BLWhe3(DHU>VV5Ou4CT!m8NF!`2F`^G#>-DC zS+^+hzLvm|IK6Q7$8YYKehi0lXaKHH0CU|E;dQG4@!m_&B2L0P8$b>&~z_ zJ~aL938BbzgSnd0E0NOmI~rVJYQUNu_$z4f=^&>B8+1*?{~TD=XxiS<2gK1*YTp*+ z_^5&E*)HWnnSE1InjRUKH_?irDb%{$ENI(vD?g33QqF}3GQlJ!U`6Tz#QgsKdx<0n zar)DyHy4pt0h)pB_i!z2z&^ZY1}oDax zDQIL|Zu;aYfoQocYO`Ke)=c> zw4%CN-#RyUbKyoAyv~3cFt|gM5IGn}k`b6WhZ;U@-c>gT+zT+TU3(CZ_(Dg!bS|J1 z5fz=EUs#~`+ZojT4kuQu#O+lN6)~+J?T&j$H~0$wd8XEt@%-x54Hy-%ZMi}ii@pXn zbO>e$CkmBD!w|5Y=SptYtSO&#w09G6Sl+S-588h4OoCa^A=s2zl51qJ#$$N3TWY05 zikJ66FHDkcVN!eW(%y%H@dfq6ez=H9DB$$iblz?1dpaj<>O6&TV=RH>i#wlmLGvRd z0x>_Hu=`H~P^}vfa7H~p8d-nl|1GVf!hD*k)6E zTqVLDhFPVX>AIo1>cxtDp1YcK8Vd%JDjDP|+tzQqFwU*kJo z!W~4iOjn%WUCbnUkd@+rW;Fj%%PU-hn;8$_~uJY{9|7XFeQ{ z2J`lCH6J=Zm3};P)Z=$_@z>_&jQ?GQ`FR}IlxYoQ7+J+sEtt{+dRaiK9vdu)Def{v z=S!j*eUXHlRDGTSXAw(lzzUz%A&kL#=#qlP$O!2?tyR>sa%bS6O9+~nzdD7#%nPg%Q4q9$X|cOoBn=Ro zK}X7@O_qxGzcl?azIgS+>T^82Z?!U&S$T}fG$ZY1B zxI81lA!6QbO3J|weG!cXwhmBjdK?jaS0j-J9=3SbKv7eCo5Mo=Mo{{UWNhAVwN$C~ zYag|^M(08wLv@Ph(J%e|9R$7K2=RCJI6p!6PD-G(@q9n z6kemD7~5@}4FQV}_Cx6Kjg3Y&UES%_(z5mOfP<`QiqX+!eHemki%+V~i)kV#a~F^v z(3wSm-u71%ozH*gqg!8JA2aB&a|%_mgtV_aTU%RAI$ol$+4?h99;e>i%=Y>}kiERe zV@1nQBkM8+hw*r6gpIv;S{}5wUg$a#HhoB=L7pHR^cZP65^YX4wklexQU(;NmI)tK z9{o1m5{Y_GYqtz~hXCldowi_XKF7I*tiQdyz!^_pXVlwFd`IcmWNp{Ng9omK_QNe7 zj`lXMVottjaHQKsCd_g$KbZaLBG0TF^q`;GZ0&hi!?SirrE+lR_*Z@>v$@ijVo|4F)@Z&SFaur z=<5qpYjKtLo*XTaxfTxD-G2qEfO|QC50RlMY%#@w7sm1UK`e&_UaYk=2G!Tfg1i6x zjfTgo~=}K z>uQ`8H?ZQO`2e($(O%Hr+j`*4=Z|8AlN%n!y(1=6Q3iLRSbT%V>;U!Hl6m{T}a&B(C;z^huqfFI6);!)r7=5fJ8zE6rIP)#ShExs6i@@uAKa=1Q3pRrYCb>*bACkt)%X1Nk zl=&4cvT7BYiG}khb=jjtfBz*rGfaBJDb?F)|Jtck;G^=I!y=1|wcF!q7V(hl*MA5q zslBe)-d>H8gkXv<>FE-YeGjWxq{4sOc+pcJy2t=VFWH+m`f7TsErl5a<`B*OCj@ti zgf*-ez<8}GrwvJrE7})jD8ncxqm}&n`8|k>yN*=hR-IK5k~5PsWKrcrgx|j;D|<(P zf>Cc+=*Ieo<&O{C&1o%mE>XJ=0fc@^a{qn@mH(M!ovB$`!2`Sh+#uoNET308>PvsL?jZ@WwhK3(crzI#vQV&%oq<0WQE%R8KEGi@XiJLJd->I<2xYVk89BIUCw z)tf1I{1QT7DCqk{zafow*mmjPH`)8H^5AF+#^8vs`%TczCds7#Nn^%X^|cgfqh_c>4MtOWEdKN2JgF zhybH<=yhQ(&3@Jf(Lv|W*5iMA_OzNw1Vhcn_MR{LCo{9;VVJ(Zf5lwJ-Q3D^HQrWt zZKQ~sn+!gph`RnVZLNvS&mr^oP6gvrZ1s3ixA z9(yb?U-UJnF)`aHI}@#epTqt~PRsJ*O3SRaj;2RlCctCqPGujBh&Q-oG$$Yisa<9@TQVnp!`eDWbdk9 z7`*n5S2-0#mbt(yX-(j1Yl67*=Rzkaem;NZWfXSq?0v)G0A{wml6(WQb@`CTTbCLb zMKRPp9qlFrW@43856aC*!Qnv-a!Xv0<9%n{C7@d7*+q0D8V?{_G#rSt_a*#)K5Je;S|P3X*%^rID+3Y-Odr@*6-aKR9C3pw3}3lY+Y2Cf&b%$sd*LuX$0 zx;?3;H+m|_;%EAR0E8A~Nz1r8s6wAzMCo}|<5DfUJ<2^?beY-|8!L@6Bf!XCI%q#V zBnd*KPy-Nkx3PDZ4?}NXerRcL|15$|poR~!-_}3Rh`g{EF%pb~G+x3aht>Ez?fqE)((#t-sUnm=#sFgIEGR3J;_u0?-zu-|uHDK&rs zS9ReoB82;%%g>LdiQVezIZ~Aw+DAQGbL%N`4Og#8Nhz{RR9a5pu<=N{0TYGpVBkmY z-Hk}lTv`;)DZU@v0xGsH)Q`j2FcX$bG~D;4j#lI9y1JO}Oc>pOz2<^-bv4EL5R?c9 zBaxlUDfK{qS?+B#5lf3S15hHy3ldcH;<(n;205d}6|&{>LA;Vs!x*LYq?ca(IePh< zl}xh{ENVa5>b{C%w1*|I4jQpIw#2Nj-$XI*GnzLW3N|svJVl2&X(c?zZT7||mLb8OIM%BI5m^SuTJ5*B2xoW zq`J^_r ztZF|{QpZSZJ94LxZw=CzpL2lvvF&6jL(w!LaooDlz++R_xh=r+-|uxbeEiXC=ap0K zea~$4z`*|N`1o8uh|U%wdEui6%i{h0Yre?lwK%??D1L%jem@H&Ix~?@A#>bXmeFp9 z65{}Hzz&=u6yh7rY))Y|SixXko=b(*{y0J+Tf>8A{xyyiA=7B4>1L8*Bse&*E!LS+ zbiEm~u%IL$BosDUTlskP_yBh*^tF`0lOIt(r6cKgtUBTaYFEI8XY)S6K&$po`q6Ho z^W)_{|8>u4V;_)*YY6%I43k~HEVKxG!0qe&J^V6-Iy?OQjp?|A+twGqn`7N|!%+LU zA}#WXN;zb~BCR)+eOz!bg6N!?lK+)bZW~foIs4-^oDDG!HYP!wMzz9jX;g_?+$jcf zZtb1PL%>nGv-O=Zp0upy37ybVAjObn{g&D!$-8$Io}Q~elIlFyLxX{m-60@-Py2Uy zl;5)y}h=$HTb=Q-CZ%m8TYR!T-jYoheY6Niw7 zhE>99#<&-OpOhoJ+aDnGvIXS!gyi^F`|jYPRzpNM{iHF-@=81EIOLov*NP98VdQ1#t>hnTbJRH#NxIVQ?#8kll|6RDq?Q$xa%$e!z>9$)k?wZ|nTjsw?^1|dOd z9gaJ5?F(Tky3Yt6APREwqE8OPijJgYlLp{?P&!JHY;A%Qjeiu|2WUF3;^+J9(9>{T z3?ksx`9f{x?5w{Jm@kED{Or+TXJTA1aIRDPu5)+aBfpM~Upfp01@@dSV;38q4Hq@1JbLtXO47I9uPES*;_cHlgH`>E;KFa+F00Ag z+gGAryl~`TNEBnY4weuWZohu(p8~JDdwU;fm974cSLQu}h||Sk(^NnGLcTY= z2DexJe0>*V?u5ym0Rt=Ka_!1X3ffziZLJ)N9mKwKWJdyS)sH4lPgGJ-xR(C*f>SZ= zpSBxOM&|^u*i(hJFW!SS{BrzFY-v{!F{J97e6o^FQT~t&2J0*+T^=5O2pJ(^oUK=|fft0^u)TKPcK8WSxM#oupdZRp0UcGGG& zW^GE?yxXFzDbvV%r0S!GlVIE?oW%TDT}=6o>?;11KKzi+Q)YwcIKQ-3S`oAwG`Gu( zgub~bq2Xzk{>(_<{ewrQXQ#7dtN{Ud5SJUgT_<#UO0>20owIN6-O&+;1_g7SR)t4QT-0vU$AZygNGmVVh^<&SY4@X4onDQ_5eS&ym$rgEq znl$F77q~cjS)q9q)jbmhdPJX|NWH25_p<8ZOT>(1jx~=eoUKc;fh_H6lbk9JauxF? zJOumXr`$5$E)j+Te$)@`6>~l0F^ddYi=5Bj&>^YmS8!!D*U#SqJIx5#d*kPVBS-b#zV)&IN~{YJ`*DS@4V znvPH##p19Qjf1HJMCHSSp6wT{vITNV3YXe7R^}k5vDh(>bU5`YdmS4ty2#_}TBp)S zDt(j}>sq9=43Ez5uX-O7QW9^-u0BU29p+aQF0~OfdSivz4CVW`_7E8oxY38%gtt8t z8mt(#Ue(9$es@Hkh6m<}iZ+R2tb#ZJR!S!>!%LWFYV$dLh`MccTOqSpO_{USq3;f1 zZ~g+g>aQ?!B%1aDCC`WDOAW-0m^Wx`B5K4YP2Ycs977Nugx!V{>$Ccl*F8(NMe2Nf zKU5!aA@3WNux8|+Xwn^H2wcL^brR%g7fdC=OwQnb7{12&a`z~?v%)@iuSK?4!?V!O zMFgY1+5K~0VNf5zwD)bkp=gYKgRZ+sm^H^mJ^OcUv68D+9BJQ>0dpP&tx2Ac?BA}c@M9VYd$c}i zBeb1LSY`*w+miDcw1mCxxfEuc0Ggg;ueR)6HQ9sfvchG(Cq|coJ6!i6)at@a{pW^x z6_=)hD#dLt&2oCovY9%5Z`mz^$M?j7&C~Qje4jNAp$LO3WLcPFc)pxUYF4lYQGdi{$-vx ziq5feReGk;2~9@3q9tQFG1YyoThPJ!vhEU7QBq>w7|_s4g$HsqM(!_8G!%uu{AhHH ze9W9%6dLiB-M-S*Q;_9tbl$jODCgMN`yl^=n}}B3>;JU?$ycIT$;H`y^d&yTd!-Vv zl7At&r)5%sZXb#h#A&d;qM-gIMV~ND2L6RhjFSoq+j`06YVVrS+%}YTq(@mNT*T(< zcR1i%-8VqJ=U}?4g7DH_=#~Ebyec>wg;6ya7I64FMxdae{+S}dgxC{6t zzWIcfq;aq%{_g@J1=of&J?f)Gh|6C`R>PY6c&u}evs(^{`&!$veXJ;}xj!$|^hi`; z`?&taNIx6$12qG96a5OQYfb3Y+mZqHSO;4w%3gsI{3x}6W40An|(rH)-WnhdTA{)D7mY$Oa1#x5P)56K?EFwCVb@+*&pXd#EU z$kT~M>+ujjczp6}th+C`)BZCNPWptVl$6#)JTs&JT!t6zWr{2rHt1y=p9VFnjx|Sm z+?;UX9JiX74Yf95J^F4;6-w#d>*qMw6)U(p5+CY>&FvtBo}Y><$GfM4BLWpdNnPit zh7`%J?w~U*UMo$Lz1=-%0BYMC@wrLQo*pMp@QLXDsPRH<_{+AhX#LDu_;oG^Ro3T;;_9GL=IJ~Haa3Iu%SfJ&>;$x0poS19|t z^K#!^=DxEv*315s$OK_eQZJfPJDhdd(iayaTqVSMQhV5Q{|60gmnM69X#ZvN4_T!a ze^nLbd=_}IlK$Ku^a%whW#kp*6|`LG<;z6A5F0&ve>!=iXChvburUZ~M-?4&TFhbZWX@j`~rFS%O{ZHU5Y%*7~B`X zy0!?bnYi<1_U8arjAHo9D{GlyU8N?od$4}FJr!1H!(UeE(7!3lMRkfbjd4WsoG;7# zoH~&&^o?waa&ImA{3%gj<%i|F-6r@_#Hh%zYxkm}oLCFq?3V^I==52a4+F7SuAj7p z(QX$-7z2@jLceqdxV2BFmc-nF`-%;gdMpZclEvxMwF7g$iCw8#jN zYvD-ge|SL%4lNnwo4@a|0p!2DBHk=X8WxhY?I2?UROwvpv@$lfq2I!-uV}qmkQEqK zN1`awge{=p@dhL2X>`PGR>=1BLpxs^q{N(2K|6)9(c3oIah#tj@5%{l*A92BwJlsO z_DM9$ZApwDU5JVMOTFzo$YTogs~YTiWA=zrv-GeohmUqRn;NAziFsry!g^tLEI>ga z^SfMT^r11Kr^Molf>>6;^>Boh(Oi~1J+$Kf%Gk`UDrbJmHzxYHs-lbJ;qvRN z&#fQWS+vMzMqjo!4v(~9)RN^nPYJo_5I_GSn|9hsXOYYy-Mw4$dy+4!eV1j(d&hsI+>anzBr=DM7eR-oKRl{I!VEq+uQ8)z=4R z?&;K$~l*2GlS=+I?F>c0Beh;aFwAFR6* z#K(p{jWL^WD2cYdIiAwj0#N^FZS?Lx)L9Dawo z{`U2r*W)@NpIbE8OCQ>^y(dr_k?>6)&~YH2dOB`aQ=}1CfxPso?Q(ELP?es9wyI7K>B?vad@|P9fM$=gaVntiBZ*nv(5WjC z<|f>ebWXeM!IJmnGh~Vnub$-Y5KP0u)#lzoO$0#^rB#%JLxBYDYGL$#8FoPt`6!{I z=W>-JXrl*JY8 zHVM1&->8q!hvgOOpt*%f5F!58_U)G%t?Z;VaNl>sTesvLKI9{?+$540&eubFbeI5_ z;gI~fi%>!rIAYZGAZxlD%J7$P!fYIA)3mT8ap$%suU%}=e?GK!i~K6fu8G$nk8@FXmN339d5h%6H=b+RU6cL(JVLABW_tv zCU~sSz*6?Mml7oWy5mlJH|u7wwQ-Iwn&c~*5;gl}&8x5lBhJz5a4(JSkbxqN4HjT} zoZq7dI_L$?mIrp1W&5d*$mdwud2b;(s)Qy`+iJHTawQRwuGW?ZCWKo(1E)v3xa=670 z4Q+#u!eI?=^2oF=yX$=dI?S&_de6wl)w z)B8x$pby0<-4{wF1$9;%ylpBXI7TY_ZUOO z+6*bw2!OO%VnSoqPfnV%E*ziSEKEb1iG#j!$)b7IRv8IMaK4d}sSbfMW@UD^&k!Aj z{CJJXn6bj3>)JcbveKRc3EK91eFIY!JMXL5L>U@kNnF z7M)OJEM8G(^jaYxU;^(Pv07RTskf30_4!yoFthbsg!l?&!LG8xyySn>byoyY6QH7V zm8_XVq^x)@wOlqwF>4}msLxQNwaBFq|EN=nKrd?Lozehh86=nl#~P9v&Z=EZH^CSG z^iV`-cY>6YGja=j{02;n$AK#lOJX48GPSdSr7SZi0eL(rDXE#g{ez-m5K`EHoVl0` z70++o@nXfWvTNsXF0xtEVW6b8+R5-wtHbaqi|-!O(7?ei>e%;f6!PfC14|Mv_7_!` zgs-!TF;<%ayj$buhae}{eO;^9`4i|%m}cV{yyo!>i^NYT)<#?UO>M4M z_ip~d4vnCOZ^xQ=*=3(OB0ghu%O77N7eRk$dCSjxsAdsC^?5WPVX*n{iSc^yM+A+k z3?|)nCwYkB0u__Op!d#n#@1G;DcLVmvTTf-WuAA~RvurT?w{)%93+VeInX;=>rTE( zJvs4|M#J!l%KY+0U;R$%r!IOq*>2TvjiQtniYcRmOM`i~Ip$WB4Cm(^zKiP7u1yu7+}Oed$*Nu6pDb6?t-yC4Oqzo;;c zOS)KuX81qvWYqQ{9lgWzqZ@v#eh#u05||`@*>uk6_C^R4GMbfvhiNGgD$Ac%{V<<3*;A@KIu0wz+vXSwaHy6mgtER-KOh`L^G8-?QG?t&bQu{vR*T z!uNG5nPx5APRO%iT5qy{#?f)CA3lUoJ1c5CU1n1k_FSp0ae2JdtNQvY}G* zZoz3KVOsJVIJhMt6K=5XD^}LmYgj(882hNmHxue;V!EY6*s4@Okz8RDLF~@V2PH1T< zQJ1yQn(R==V?FTIDsZYHnLIc+9lN6JtzLmfZFG#=nU}Zsho4`eGYqt|u&}VDmpriTM%mq`oFgT5@%WUV@1bR2;Lb64 zO;ezNc!$DM(tO=`)}Z!uRTlG9i3cNckb#_7Qp`k&4JTUn@}{(<^qr83RzD*y1j`jB zUDX_uAI*>yEm~{KGrUo>*6)GAX}4~y-840Ic~1Bt;XbitSCR)zw)Mng=)vv{kkWe4 zBFfm-xs3wqOM)?)VJiFhbi}!fIwab=w?J&!MCr|m!u}=v;CID{PiN3QuN$g~K%9h0 zs4UQ07^5@>7%le6-om2GChbKlEwa|(BqcR~HW9b&)}*(<*jQfv&!42U*1H6>zKWi^ z^Lj#XM?ioV5DJ5HJ@UauyF4_0Kh}(tXj{=P>`~$3i(>Yo&>zmR`*kV(;&_g-!y5Rn z2@^&4#_Y}Z=gAxyqvs#+B`l+b{F+HgDgOM)HBwVM7=7X|>Y165asM*G-^jq=Wfy14 zGPkz+T!th^2^sk!NyVG&XDWpAR;S0K8ca*>E##@rR3SvZ$xdHer2&~h)giIPY zE{Q`(mb&w?$&!9t*=Qh68tYFy5$)O@5}0|sX=!%V)zX@eDz5t8SU0Sf6-GTacmUU@ z)mb$C`mesbXIDt{Ag=y)_2DKUA0j-x18I=`{(}<8i#8(28Cc15|lPoBzjIHMqAd#kVF)_JH zKdua{ChBe=VAs=s0#S=Hx}O$IhTD#>sEW9$RQRbEZC2(l+)dtkRGBp$enj6IYAJ^w zCewr3=oUdNrZ^*vQleHaN25Hoq)E_wr7SBe zYs$~hcLGd^08-jCqw})lX!Dg(x@glEKC9tkc?`TXuQJoy{J_#(F70JNT%hb^@OO=1 zv=*;T2r=a&57u+lHi@#xE;R(YQlVfCHBaB%_5s=~vKWPlAVJ!!;JqEc-8#oNUw{)E zrIdT=@SEz|)7M70mMDJd5<=atRm{Vg{_q?+_3%Q|Kh5e^99?DvEcYiPGo7;ewe`Lzwz=pa^P z=}vnhZU`aL)n@du+VSbYY~M{w)B4RRcK{FsF-m?rqi^qS24C5e^lWUrsRDlF6{5AA z4T;7wd`^fsbCkZKGXVVC6 z&;mpBuAMJ@PPik1Nyodih5M~Hx$sx-=Du8FA&b>;Vbk399xxWGJh$fO9hf;d6l0>J z2bw>BZbcVwOig`7BX#Jzx>^-kTdxqJtWH9aokM#*I9hA)QiYHn!7k3V-Gmb&Vo#2E zi_m))GarSmv-CbSXQ2c^1_LZ1k zJ7~oX!TOxH7($z^;P7@&d3}EVJAGkcB>^{1Her#B^|`mkX=w?%-c+y5%m&+)a#@&` z0zcQ|H_Acya*D5~$7xI^tlu>5{pIc3#GM`v-}hc4Edc&51_B!!HB9fzi>B+}@DqS> z;X;^`w^Vw44U@lg=yi3>q2%Z=Lo&;Tc!=QmT)Q`d+scU$VOBw&=B^G)FXz8#Oh;ly zq6dEd=ui5lsMBwgzr#f+>HVg+;|IoG&-n4`qhd5aI%>g#eM9Zxj{g1(as3vPOc}M6 zxrK^s>rm3ibFifT8c|ziJ<+7{qUY}BXxRqMK#nafoUJ`(WLYrA z`1y(7`qoFFx=Wm+CEMI+-%)f##17S9(f#x9TueZ;3JaX>=5QxwOQ)B?ojH@Z7qx;g zU}a@J1kAwW@O$USZ>&8@D9LZjEIFj!CCuYeO4-=Joz+2v7PK7OsAOZ(-3|=$`xNv5 z9Ur6J3saTp=$4n7e`ZN)(Zj}1pLTK`&DyvKpPxRVOlEKNJnpZ{H24s zyu3dzU(Fq$1e}`I*xV=7sVp7#x;QEKmr*M`A5iQfKz&~wIbf8wQ=9B|{bgoeg^ zCZA+9n?9z+jE^0Wp!Mp8uRl5k;R{TQmxP3)K)WF;N_^ycX=-k+p&f0!vbOdWxvmtQ zmuH~`idt1N^xR!NJw2;H?_v$vp#`kE6L2SM3M*qQiZV0tJUtrtOVZtyH4-B($Wnj8Whi>sa@C4-N!U~S%e$5On$<`R7;3{ zIK7cU(jd%TE_0JJOOO#`PE4cuVDH@*xu6@)&Z)o4%1BCpSX=w><*4xHkk!+;Jd24v z6f{v!QyUw7j+<%~rg0?2#ri*7Hu#w+{XFD;Jw$seu4Q*(=duSoJd+HcHfG+u!H-XP zpY1CkCv;(dXiGti{SWy(wGEPp*?e7gM^NKqUjzS{jVJOTqQe+as8z6GnA+Xb6AsMR zyRb6S4VYlaax8MU`acXMcYT&k=2XV=Dy+3wzrZ*sC|2@MSN{^^H86fNZJ z(6=$>2$DQOX_Xm5{Vmrg%V}wkG7-5_Pj4J{b<1uEJN*0f2pBzGzq(FMBnvd&zmPpk zmw(qFVSf`ai@Z!r?~xD2!0_Z~HOGHXJC0YUW@}zqR9}hp<~X1dbflg>4KvY>!}cQy zZ4c^86Inp+`DIZJGm(bYfmKc1S^1j<9^C z0PIdv>1?GXZoI1I;*y_VRu<^im!_Ti?wt$Ayo3736fyln?ZdV+rC*+EXm{TlF`1)& zL8Z(7N*t|O-j?>p?erQ`6d zdk|kbA^_dEc?wL!Ensj(!ZNdn@7});ZKZ~7M}nYLgNlgniPmlMaN`VaWA5sV#9*NR z98(^P->(EBUdoS!5m@8nrEIJVMxBugjOuFhx8!WENb85Pr~hhb2VfwjAv zC&Xr&jSGELE-sLlxFC}d&J+#}4z_4I?T=+_99(LTIwhvTqG-ci(5c9$t+lTA!rKrnmgV2h1$uy8(l0M)!0sx)@)m z6c(kXztrL%9Fx$3zi2tE_2Yn~GCC0EBBbsq?{|00`UACi_xyO9c8b^-*8eigVdFOs zV$J&~c04>hD)76TNc&s~`!L?G)YJwASy;L!+u9bkW*&wB$*BNdl0?zQ2GM;vxsY+j z2Upv|t`e9TsF7YylT(KA37w>NbtH1q(h_F!y)H(1F*&g@F_TCpE1#OWx{3Uk5?D-7 ztzlH07d0WHN#fJ=WFckb9r$YzcZd3wN3pH`j*6;<4<_BeCNW7(ILQ6&L(b)+zY0@x z+4V!)=EMQMuC1eUKDM{mux|JHV21RIhz9Uo#oquL)N6o z#j5Z1J|Hd9V`OHw%!BCc?3@LP-kQ3a8rhexUjd(xPK^zQMzQWG7zC{zKR?6>aDIL`Z;W!awYM+I!upv0^+Z`pimv0Ei ztY?pGl$RhNL=Bb@r}z|scY@%wo6UZmp~PC_yl&It>kd$GicF6**b~fOhYQ^BI8gK& z*e*x_{zH_{syV_xJsSW2EPw(Ea>d1Z+=*lOuLb*ukox{qLozF8=iaLr>VQaBhS+-p zKYzMcrM~KOoSt^n8(g+3(JGs-fP;Cm=O$d1l=Si=Y}o;;;Lj%zS}*!ZrCO%->lM&L zMn8Z4%m5CE2_JK>Bb>6v1(YT=X~lfr*a2ZV77lC{>@NlZd@W;6sCN!_idUz%n;Wk8 zzWLL-d!I|hmdE3(%z#a>;Pgk&s8b7had}=-1;D-+nFRMq^<3;$(vO$yDJhfxE%&8c z+1KeROGyR(1v^RwTQlFo!ow*ySsahIXZWswAruD=1BNM^TVUz;dhGZ2zxLwAQBm6e z=G#%qhKE(2m@KBckB`?T#!5*_VA1*Zjghllo|i<~j62Eo^jfPl+1>{lJd2Ei!dCIu zuW?9<9zc|HL_n?jcUv%?HAs+qYu?4@sqZTROa*13ev6L<95FmZSab8$0BDlg0P@&M z!?{e8fSeo|Rb>c)M_zzGvR#$GJl*8zLB(I{!x7I)^x(n#0~0P}adS_3WmI|KM(e%X zw?&44elO*~f1Uhse0*HJxVTsu7Z+Cd z4AIP+znM}#kdpce7{4)DRQo<>C*XPXOIurI&m$!Wotubg*N;00fA2W`OG0ckkxO>= zKWCF1g{?!;IT6wNjy^sQ_1F|0F}t+?Ou44Z;xNfH8eNyR<|)mN0+1-zVk@ zr&;Tzvhv>)Lo#;=p%seY@zbFm#}B&}5ANOjKKb`AR{{K5#OB7v7Sz7Ew!QTix}sTo zjf}V5Q2)ru1&ICck^8gV-C7&vDT@^i45R{~sm#p=jCd1CtQcd8;Pt(YFi<{y+d zx>VUQPXZ!q#ztp_>U5XPEiI2rk-;@Z=55dma!1$!Riy*Das|eo3z&=4z})y1{pk~n z=iyrH=M8%{W@cq=ctA6tEf9gk7nDYcf!DBxq|40AJJh|gv$HE$=!oD@l#|ik64U;_?EbpUAdvd06hyg4fBuRqeyo|P0NynK14awlF9 zs6(_bUp0{0L-5>o|wYxq&3eFk9h(M(c=O2XffhtDfCXw=b`VH5hT(DF zmP{6WOnSg?|E9uy2BP-ldvpT?{8a3M&U>AMLqm(gUMprv3GaEjU;kGY@3MCZk~MvR z4dE{UqF79SPhKi`P6JhEvB~SS{0&eaF&5_M*?t(6DT5Y+F)T8FhZyPW!3%Ru4*JrU zY^0QXrx%%^Z*I%U$(ieQOnZxEZ+!$!FfZRS8v{M=N;dH+rn~WufmWfG{rB2u5p!Q| zYhR#Cx;9P~u;_jOp1CT_$)x5A2HD2ti=uyRt4zPm;2` zpmV_#^?td=G7$Jsk+-V3QfyMGuf z%L~{;eI#tGDlSeVmiyQn_!7oIHEBc<2cUxseAnwU$7HYr$0CvOk>}q0LBZj=_gPSo z(S1Z5cW7ipeEgk36YhWc`Ia%T%IU2SKzL=z8fI9O09MA{@l9n=gH_Wkgtjhdm-Nip zHpOxzhXr8NCz&iCm4{fr`A_EbWuFvpmofXFlP3Gqmej0_Yjng}yyLK1<&12U)& zRF5`!9g5Nrva$|=^SA+gfSbTH`geAxbX{&=_6=r6BZVmc^ z#~|o^hX61N%21^E+0o&;xPNL7*7@e)R5jA~Ld4hW1}qsFP$*V{FqQ_8ba*e~*aQml z@=njqKY`D#EQ2VdvZA6#Q(ZkA)Dm95hhCP2b2OF?*Rmr5ou6jj_n?N3?v^5h<~+Vm-3O}_W*tH?LDAw!vYw1+NV#SI>pJ* z3j#!j{U$7G061~=aHtsKs2s3vY!BJlt5Fcf#tSG1F7Er1DywzakC zKyt~~|NA$)zNV(8isoh#U&($U7)LONAa?@W2r z`1m989JvAF{5w=Qc>%D+^YCF2eVj=h9KF=@ojFNKlhtFeT%sH-ES6{p5CyUWclEK# z{eqJ3qd?bU`CDCfQ`I|8&@_wCkUz;*za!NjU?iM^9uV}GC?_u}2UbtVgk0uquKTxcqsAJ4%4`@}xU z3=<1WKV;Lun#s^qQ~Ob}ex$78t^ zL`8;jSs@{}nxOJnl)Kq(sgN@I$8@w*it#4u6E+hnI@&vUcwvhU?laoj0wSQM{$8?s z{P+E5ADtQ@hXG}S25f!rL-jw&eLJ@SYYhKXq9Js2CiWq!6V*tbA@VcQZ=n`)acggH zzYX^s6~W2P?SWL=@&-x5c3+<>u%&Xq`iezCfbP>KFT5irc=>|*7B3$}b3x_jR_p58 z=0{BEZ2ZMnQephnTSnIU_u#`DflEfIKX zkowJj1q$g~`2_`hoqF!@KG%I9S7%0$J$h8JKI45ngvrjU%m)_ z_ikUPuxJsW?3EG3bhM#Ao(g+k>AK9cn+)~@O<&E;fUC*Zx3MW!g=WbqfGRj{-9W9Y zu69v}pWO!TDJeECE@=_uY5-1B4OhG@9E6B5eFio8$WKi;>^-DPH$_3Zl8TqG}d>jxGV z(`OvU03{R|iqxx1YjXDZ8RzlJZM54sFNe=Ah*?<+mVw0D!UX{Ei94L%xl$3aO-c+kh6*-HMQ}VY22S6>p=JTu?8?@EgFFA)-!$4AbvLz zDfP<@zsvxdP4bY~?+dwZ)y6>h0_&@E1z$^C57*T9;q>)}0fB@rRyGiV63ED0;YXkpWuMsr3Nz_d&FoW_7e|L5w{nHYKId1b}eB`N%AXT1N)p5k)5OJ$rnvFaNAR}56f&T_85_}TE475|qYqqIRdinSi2M^cfQw5zREiCe4KYZxgg!;mk zhv!u)lvDyCK)u!-k(x!`y^G6BO;F*bek-oT39SKjNJUn4p{&&is5<{cf@6K(Lo4rt z6$(6Y!KDlLY^4W;{Z4faSrH+(d|Tc=>B+o2kLKeGdYmrg`uhkxh9gQTL~c4K%Z3SX z2|eIzkDl4+>3|pl@)AP`DVjmlH8P;(oC=h7@1zL3mk$6>YZ9cMd}ef%R8=1##e6!h ztlE&}D36YxKRpyv_;;-|HAmDKFjbk`IO$^M0wU{wX5mMUXJ=;aNxXV>9aR8O+qH#5 zLDwxRC%A200A!Lu_XGcts#9hZsGy?qsRIH4bdFc`YD4g`>3z#1T~vwdBPGeyAU*1? zt{z(*5+eHnaIdZqP?kV;Cb|A-i<}(mCwF2cHi82o#6*N;Q7<8BA8o?=>_hv#hM)bW zaEDQWpL@d(`1LQt$++gnCL$&v%z|vj7;>!Z$yK0H8iO;{0o|8cpu!)v$3!@<#o|I-B!9|A3d z9c%WsG7u)eh(AikBj5A&BoE3655Thc-q`rZbgVquy54D5WzpgEjj_csXtt$-H%u_H zg|gCix)~uLb}$+=^~WkMYi3yBYWG9^@jci#-8~@Vxy8GHj?S*LlM_8M?FXf*jajJS zQ7gk6;dSzoju@U$(a;$EWb&7lhuD>Q01NtzgkOehICusJBQ4F%KOOD&ttPLXAA-IK z&*kN%H<%qFB>q+i0h&F07m(#;p#Yj%2tp{GFFaZ4f z_hrd48FAWvsK!CSLTHWl#Ik*V`OSxjzJ4#w5e`Di`9pY`L_!FS+yDOEEd)kye{w}P z#N#sEfQz|3I)4Dat+{NXXcJ^5 z`geBhqRY$6mylx?TX0fRIbJeEcah-+qd|3W9{jmnoXHC)__=2`*La+8SXkJ9&w4h( z6W?|3OIKGX@@);0Ks8x3A%{`eUHIG2GGoouvwuW0=t7RnQ8N6tq@Gb-uYXNQhQ9On z*Gc8KH+ljdm3cJWMV|>=o05$B-X?AL!%bC{S}RS^;s^IXIj87{rvg z`n7h3Z|mw#p~E&zom+6zY+79^@ckqA@|z(cdiq$mzt9o{@G#ie;Jd8OTU2|UmMG-s zOK2N!NWOfTR9aiBwWIHdVpVXme|VVseR-dCxI~}!^Dq0c>P^S^0!`nvn|C?sZ`@~| z01q0dfxPAipf_6fkspFqdP zHwttnA8~M$08zL>@ZxB~RvAob4LJK0XfC|MLI6_fJ8VCb0{^}mz*-_O1Qj+z)d5^G z%kCIv4A4K=o4J?(U!bcMxA?x@s5gim%0nP$R@~lR(qce%f;uWgE-N#Tk@-#!j}~1& zf%}WPm$>+F($7&0kWPz&pUEU7q+<c+NwluoD7-iHZ51TFmF- zzDam!S((Y(q$E>WDJcsI6G6Ic_P-?Jd*7h{4&tiwwIC(mWV2FDCG1wZ4{)11sI5R3 z(!?d_{>LGyf`*EkQmo%1szFbf*t~8Jky|>@B-1FM-mPD3*ZrE?6abQ!9!DGG3mY3L z6Mwj5o_`n~ZFHYx1T6vmRDt^*a>%Ec+rs6*4R-v|*~tb0WQ;APqy7>U>-ZpMYfxg6 zn<+Ey=pWb1JMCSrC@I+{hCs0#>^F_=4MXPFWJ`qbmxg*{W+l|hzyB_-naY%UrUy)a zbr#8FQZ_BliSOUiW=}YFM4%<*(rLOWCZ@KYJQ)cOZiXpNrj& zA$Y%Up#tBmrgNVOX;a{9)(63wz;IfztH0?vH<(yIWWU>A8+tUBBJkTVBSR+V|LoeQ zUeLr8Mc$w*wFX|B>H3)C6&52mcYkeVrJ6dt*TRAV9jNN4@9CdQ zoSvQe?9R7i^FeHCV3MrkaOvUKb+oA(2i1?XTUg)Npw*V7hGLHO`Gx9APqN6VU(4^Q z;yi$mc~Q{PiiN>5Kh&+Y%is?COY#{z;KuCi!tdV@Ih9FXm0K;b6VOjQPqLRfVcFnt|&Wx2LF5l7(p|HbBa>;qOO4U{MdJe z3&@dV8_UbfudS`UYsx^Jxpg70(_J{0JFhZ7fjbpnB_7J^d0ZSE37a$N8Mfx5-!;a@ z~sfc!jhKfyvT4FbadE>9>~2{3u@QD8W+WCTih#ogcB_}28Rzu zG@s-6Up{zH)OK;X;K~yy3KyfX4Gc{@N-H$|9($dw=KS6E49m>Ccl9%zlJO%YANwB& z?^{4>Sn9=#f_#FYtT4j_gKj5eh43jeI7ia>g8pwaVul}~2dgzUUITqcQE>O7AOPNn)sUY*eylWY&p>1%me8>fD=5Jf~$2235cQPKzx>na%uV{r`*32Vr4hX(0JQX~kW+Vh#2wNtmVtS#gJqG{z(J z=8G4><;q!cKKrvzI{R9QWVIxc5U#FHl-vPC`Cm*DJU6s{=YJinxw!E929ab(3shTY z01v8BTVMb6Y6AJ441qZg1`ZnipNR>f46t>M4<9~!L1}f(%G%?O(E2SEfzGxzE&#OweA?-Q_j33SGRXynCXMivFsi+<`@TsoK)Y zD#zU1Tv#6@WVCT}mX?K);TQ<#WI;Z73~iM9T&_mNiE8cVA5(35#s~8RChTFze=~Pz=mb=>O!es4mcH9Y+T?!$l&xdALHvscduQ&ITpxFZ0+wUMB?gAu00Wi>$ zE62U8EZy@31?$a8oQ9h({_jMu@(Zr%QF9r+9i8xWxfPr>X#wG|#u`}kpL2HjWMrcn zK>NNhoyl`0AtxW(fH?965Ejp%T+;{_Pg29!Eq!mVzZMHC$|E+GBp$jpVchYNKOZt< zl?UTt?_XX#9?Qh~I1e2)<_+-i*to&|U=YR}_^I-mn|&6OJCFe_BtC!ZfaYicIzqCX zTwHa0(6M{4JNDSur-tKrBeCh{FewKS5rO;|GX95(S7Qw%U6jl;>oMHLL}cyF8L-z@ z$VPQ|-i(jW{UCP_hiauI2)Rqy_*E=orH$s>Nad>_BIi>?YkmLp9u!#ai6Y3z$<=f7 z@)p73=DmIU#7A`Ag9~>oXzhU6^raPmz0I9LJ{3T>&}^A9c|i1KfpXV zC!QWxH>2O&EU$OpAN9T+UP@^-?B}C}-_IlJ-`j4atf;P@QOJC(3dJ&gkmE)#(|WT5 zx|F^m2CaS%>cqRpDrRZ`&i(CWoQc1L1ZNmG(ps8hGP&EPrR}7MHucW$>^zbVFS~@y zVDA>>eFRig!s_&dm+N{7@O~BhM@D=tAr)-mLK@ZAorQNQOgDY)7tk-S6=YPwlf>!i zu{>Gmp!A&?Jg30NpVbB8B3*osyNQWOzI;_W78afyRFyb!%<;tka~7wiZ0OlJV(Bqy zX`ctkCbppqnhuURub^NVqWD|an{{Bvi;Q zjX3!32sqRY=4VR0#)k>>2IgaD&{$KftgO@vqZS&rg)7*XY$*mM|Jr8rzXXi97|s6` z4WyybaXg$)@@aA^C%d*zz6%Bv;vZKZ)tLX409WF5-NVs>FcTO;CgF>0^~_0p&J z{;os4d3opbLR!eFyp%J!x5tx2M`iEqrO`bHW`8V;$@22Mwo??E)z+cHkp7K4n9_VJ ze%IC6xyi!JEPf{DJTZp&%j$;k$CZwaV(XXFsjBgEPAjp z_J|1aU*85)5@>dpJ0hqA_AH=6Fa$9u@zK8 zqhj*JvY*1 z_2gf)VkT&o97(9FS7|+amV>HP$j=Csa%3z8AHU2ezYsisM-lwiSD?1L1**hHetq%+ zyQI5kQ5r00%E^gc(PQiSSS2-z94w;Y_R)!Bd%Y^lyVe7eFbQ0F`$ir?oXI(=`S9p@ zRBCFG>6=TP?GzI)FW4xUwIg6O14A}UPC@O{*K_)fogt!R8$?a zRF-{rr;J+Y$7aLXEm3EoHclutNUYt2rqZ@`C%%(*W$wuPl@%+G@Fou1kQNvr5bA?w zfji8EB|!ar0eXy1RxI1`=`&2492|PJ!5Bd*M-Vorh(!cbPy{iDd-Gp)KxwGzmG6n+ zxGB5>d?|l?75)FNqJzL>x=4f2`8R*;WGbpNzB2)A2*bx=Mk@h?_HH8QX`sa4eMjHR zzA>*+9~M}!g4-S%XeYjh>+rR$14EaVc8(WejbO8POg;$7?5~Z9zf3^c}kd7Gd})d_atNn%3(Nvd^~z-b-m_KGN<9CYRLONq7SmQL=SkNg`r*C z!D)9J7rm%o4l|Fh5);S$x&j^NsY9I{>>ZR6-(Fl^dc4ZtW`JCM;*lpHAcLglZBa&s z*vAZW9$YCY0WV|Yf&v6hUV;B(KS=wxZQXC7>4?)o7^h=3IQF)|s%`dtHzy8N8RQy88_IX&fr7FwD$f%xdVgeFLH;HdcBo*f}skwXZCx=s-Z8X@WEc+4jfth3;&ax3uEbv@1y;>^&-S0w z0(NIkW4!U%;t4@p^_??174$A+m-#A(> zjA#f543)LCa^dYULyI&DJUTfgvusNFGXjm9HfTWyfxQoR6u(}*nK21Z--J&DeawW zY@w_O4P<|k=|wWkDegHrIr-2YF-6G?KD`%Z1G8Hnb=j9S?pO;#V3-C%BF;=u;GS=x zj_6sGm6Mk@9wHYT{!(aB`9G5pum`oe2<*Y|4jPy1kH(K6r6QYJe_=dMf zR`ma}fJS)^I~Akl401Qc@u23=wp`FfSuU@jpx|mdb+&%|6y{RKVc5Xq20FSAbVhkl zI3e!5d;_JT4~|Q}+;^9kq1{dbx-BbV>FG1pLPA1+XBr2K0LnK5dpO7H(CT}L5QYhR zec0&4g1Lb5pp^!&^ds5XOq6wvmQR)N_x_t$4u{?oo{HLmhsv7`>g|}AjwHAxyL%>K zaZ0)Ser?~3;r!E@5G05=G2N#Wn|7-%*%tpoeajLWJ@(MxDuAAmZtueG4?iRG6^N^Y zFHY->e};X4FD}^?rUbCBl`c>a8yg!>p?5KJz0?dDwE*DV-6aoz!>-PIZ~lU@t}d@> ze~%ko)szsq3>+@@mS1aYQ^Ym<%NJv2DM|N(6)s`m!0iX4V;ECQ`A0@X>*7MaD23+3 z2fVXpFY^0pOV!AtLP)dbtp*pa=;i2Ut5P;XjX}f4Xc>#mz z9+C$=qoWcVbGwU-h-Tc|?aJM<|1&4R*FLnFn&jA?sA+xvAo@>!x$Iq0QC+L=H=-Hz z^cqExhfEK&ZBPtc;@S1kU=JtuivAUHetdc|PWH>YjtHyfRxQBpN1BYy)O-fv(ZXX9 ztIG@N+Q5Jl$Ce9+j=)I#!t5~m048kMtP@Qr+5Rlg7J9v4S34t@)_6)Fm2DUpu(7G6 z6uJ7NoHNH9f3M7!#z0f1lX+$3BK>b$aK`$@1^cDJn>VjA!mN3@xmkSAeL(Yt#gBrd z7>4qk+#PjblE)e3wk!_E;!}lkjxe!>M3~t&4rKWFbnBe3>SDqJ@J*g*Lg~nuj7nK` zK+x1==xy_COe<_t6|b)*@}rDCgOCVeAR*!6otk?2{dB^%i zXl-sW#>rZQ(h5^S7~r|dE~ku9--;K0{rUjLMmni^%$4%v<3}*Onek|ZN8w;MBtpl3 z9YzGwHG+=kJ7||(WEFj{1Vuy^Ho_?*Ra^)q5T)GY4|d#AO!k*)`BB8>$tm3zeugX9 z+N`gxHt}$Aq5!ybtI0 z7SnaUeih|{p813;wU7q24$Q>Miv4#&p6~6wTdcL07J6A(sxZ@VJoY=tvIDXXPg4_< zs>_mP=#XGTb#)c=hrU7IrXla^kWFN&0K+;WKkis<^w=RWOKNIokEqeydruC>Ymrl`MFF3LY_wr zrLzVpS-m9-4aXVL3=tq$$$n%purbX5op_u!=rfT~wK3_mpO-*Si4P!|s` zq7Rl^Vl34~pu}``!uAT);OFT+a)Rl~vJ?O3zu_YNWifXL>%pk|tP^}ro*Go)W8?K(41h4WO=DbOv zGBE`edZ+2LhT52zm_skoYmWV=&^N0B48L_^e0)w~Lc%DwpkTcf6!iD`A}Gno8asP> z_N_ZdmzT{jajCTWO-^8mJ;RUY z2PF?n5buee5!TStV~2kJkmu!DRiYY;y7&IJm#BUvR38qZt)=K4KQh|v-QcXHl>l=9 zq{O78mXKyv$;iuFbHvIHA;h0OW84RSk_?JY8fN=D_wN0sOckv9vVC?KuZex*U4miT zXU8R+5T=3uZ7jaaVQf70s!IPEq2FTs<9*6_ynVAS4m;t&GwKk~} z_ArKYPpsvI*%}b7?!ag-2UP89VA|}$zxvJlkdS^V=pCfMxW|!gNGlA*mK+=&7Fae! z0M1~0zpu9j-?WQw z`-alZA|Yceu8DYF7z_mEMM2Vxj!7Wvb0Q<8GDB5PP?^5#)zNmppafH6CTkrqH!0$ z+tP|*xuygG*76mkeM6^w5s(@`hkN_bBR*Ets(X6cNBYg1H|cBDPZ<~)N5)s)Z#O3o zLI!*>E8vLhakw%^qJQZb@+ItE01tZQEv7u@cy>DLADC6s4gxN}(p!AJ(?yLR#a0!; zY+g%^{DbISm}w1wq1w!!gzL0E34eKcoVG7Me}454CG6|Q{eZ@ZKT1?LeJqQc$}zHOCvlEG!rIar}8> zoPo(8`=V#CAeU)DT_f&MQj*~O*FQP1c`~DkFdstLy8fEi5fxrm=;d ze)~51d`uFVEZ`%)eLIn2G12KwsaubSKl;$b+RVwRvOReM8N5Q7EuJUnr293YDYESH z{d?j23NwcEn-~k^Ce*MxB_SF0dpRpujb(Etqs>qMcw7Gsmvz$S1w=x zopy!p;pV|XH4tije`UL+qpdAs!&Wv`ZLKo%{{81ZFK>+BU*zrw3;EfQ5O6fv1 zIo_d8SmQyoNPHJd0h12W8W8T`X=!}wF5l}1x01+R#VR164P)`ww%ejvnT`GYe%Bmm zlH`N$(Uqg#b5)zODf1XT3CBlA#gq->dS_s`Sw_$FTwL?jeOVr|=b4!A>!a>jhkSc1 zeUs!ix;6pe9bdB%$C3wGZ2M?UMOS!+)w4bnZQ-zUn^>&tOLR;-p00E4cK{7`vIt^1#K0@Z*6U+95%XuN;61{Ei~>CIJ|mQH%&)3qm`5G#p^p+yBh1_Qo>jLqXM$E zlfb~hch6dqK{r`zX?eLN3FaBZ`1oAQWxuSQolL@#xqP z@NZ=3l{A$MSQIVp>CYI1ks~IvU=0XnBHxjM@zkR&jSMl&Q2PMsS(jq02tSF8S&8QSyOtqa*5*XYMaw z4*u)!*Y|j+-@MF|Zfo&o^@uE7S~XCD3np&E@IRzo(1daQ1ZkS9gvdy{v$0z(sO#aB z-d8F*1sgC@zKhipxA`1sEw;sKA;zE8MgObP?Rgaz+9`aQ|KWZ9vD1>(x*Ylh*f==C z$fY;^agPhwOSm~aoC<>g487WoZYy`o5<I|N71(#BvegCMJIV+LVrks?(Z!htpVGP~yWHSgni=55N6f zh528MazLa;tJKq0dJe@qXaY}AN=iFPDtVRa1w*Ka6!E*}u;fw8Kzu6{CSY>i z;?CJ$2@0Pq^|Go~lxMcNZq(M+A9uL$gnd`z-iOw2{sa%u z(Nb(?Gt62Y0<3&hmUS8;JnC?X!ZqMj9!P{d+q4;C@!!s`r{49 z(u1i&i%ae8b&nT1e3hoCtd;ZDF<%F3y+ zl@;q@G$RIvJr$`8Ju?0t$B4qLBg9I1$cFPatgL)+uxN>@!`|6>h!3IrAr!l6qTFO@ z8LO%LO$0-b+f_G5`QH8Wpd;zMB7}`)jq0CUSX!_2WGWOG@%O zhK5d>9vV7n1^~!phwS(6U9~YYGdcjk1s>111;DRbttNBQB=#UN@wQ4L!F;I{=cu;` zAulT$0VJ*F@@41%ECdRm*AEg_SC>XlnDAlzs#OU;zpOEdjGXNvlN}5>b}Y_3E6b1y zU=L*qA*8)|JY8KZMzog~;#De@HDjz=D8yX=b@lakeg<&%#~=STtGGBf@cQ)&XZ`&{ zFC03wVN`1B)(ti`-LEAk-r8{F$hLk731OeT@`|k@A)(^0+1dMg$HXkzED&^3Bqi0) zlgqE@uU?f+XUwDyz=D#J-x>kT(dn8B{QcRjM+=Y*qaGoI5Gnvk8&gqHWCB2AYRWGL z0HyLb%a+kW~V6@-vKgUn1k1n>|5Bof2{h<9~u zvSN&>JUnoob}ez>@$iL8 zb-#fzcEa5q=8Pc+QXK zcTmvQ=UU~hM~@!i)z#Gl0l>}8?e8yQ70|G-up#^R@Avxs-_&Y#r`p9(b1fCnaqeuz&x$&&|yRq9aEdrcovkLSA+*F0hcvpmA}5g+PEF zjG>9PW*~%+7lTFsNt+>+;sF2@73G)%FflddR~QTimY@Gqmysid_gbr6m6es9D=RC9 zJn6Vjr?dRe<3^+Lso$?nN!i-Z(Q(*4H@A6NiHYk*YBUY;lm&zk0?1_0{O;#XiU=Wu zPytB(#KdC6PMe1GfPhUkd3mA`x%}#mi4#AJU9;xEsi$4*;NT$V@pyUw=+&#&-51dw zdwcsjdwct<&v{MRA01Cii|QK}7rLI$=O0f`Z;Ao%KJ`2ygb+dqX>K$Hl(cE??s%Uu z?C|kfu_P$yE32repm#t1c;k6HyWX1i_P<=doZV&LzBSXQPMwqC?!H9q=VuWCAXY5K zDe7rL2qAojTBS z(xll{F)_;qiA2#~0@y2&U_SLMA%qY@2%!RykSFl+LN~o0OFDGueE8dM|Dj*7;Ej{r zx;d8Y-hDNut+sjfYUf!+MLAxhN54P)%P%w2JUy9@N`*|B40`HOLI@#*5Yp1h2*S-y9h<$hp2X+qrDn+d=p5-|n7} zP#IWPr`!*~>cWMrO2(KGz!B<6LI@#*5Yk#`1dz1r{B?1GKqfB?w#j~&w7{ub3-bqQDXZPzj^5?Lyw0J*1>unm1sy~2X z5($*lO@t6a2qC1cFrg5Tc8rtB&;S5dR@?-R6g_ zPm|v`j(hFXPhD0Bh2U1MeCIokwZ6-7+y@yMJJ-&b(Xnd%darnn<30!q za=aiEf-5h-?wyjdEkGy)H*1z{3deDF-rnXrg+k=JxuGurczU8IMV=5s2qA>Dbu> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"-Q5m[ZK&4Jl8OV0*_(+G@Y.C0eEKu^JqF`*XtIa8)r!'`0W\G*^"53L-Mh`kI`mCg8:"TSN&!5Q@u +T)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7 +!<<*"!%@'Wz!#5'>S8!!%P +$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\ +#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!! +`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!! +&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2 +,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT! +!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol +!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*" +!%@'Wz!#5'>S8!!%P$rX8c +,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8 +-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)h +o3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56 +Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!r +r<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$) +#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2 +cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'W +z!#5'>S8!!%P$rX8c,!!#8 +MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii +""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!! +B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!! +#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!) +V%7!<<*"!%@'Wz!#5'>S8! +!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:! +.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF +!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5' +>S8!!%P$rX8c,!!#8MIg,l +Q!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW +'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7 +CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT) +eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!< +<*"!%@'Wz!#5'>S8!!%P$r +X8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ +ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`* +B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[ +b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,u +L!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!! +3$)#sX:!.]\#$ig8-!.`a+;ZZ.&$Pj6eoOe.(!!!!+qD/*O!!!!&r>>S8!!% +P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.] +\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!! +!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ! +!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'! +2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT +!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eo +l!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<* +"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[r]/+NCf:N +'D!!!!Ikg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)e +ol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V>(F3` +AjzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzJ:DhF +b7^kp~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 921.6 112.8] CT +[1 0 0 1 0 0] CT +N +-1920 -235 M +480 -235 L +480 940 L +-1920 940 L +-1920 -235 L +cp +clip +GS +0 0 translate +480 940 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 480 + /ImageType 1 + /DataSource Data + /ImageMatrix [480 0 0 940 0 0] + /Height 940 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzi,?jb%4q~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 0] CT +[1 0 0 1 0 0] CT +N +0 0 M +2400 0 L +2400 1175 L +0 1175 L +0 0 L +cp +clip +GS +0 0 translate +1920 235 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1920 + /ImageType 1 + /DataSource Data + /ImageMatrix [1920 0 0 235 0 0] + /Height 235 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0R5n/K4!!GS<83mS\ed55T$<"C?^a1"AS#Z3Fk2#b@zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!!'f4k+$?sz!+8+I[U +%MXp'Uj#!!!""UH\h\!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>S8!!%P$rX8c,!! +#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!._q]rqp&^1! +2QXz/E0NVSoa>s?JY\_!!!",&c+9`z"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!I +kg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"FC +rV8F5o/qMI"TSN&!5Q@uT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii" +"onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B +&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!# +jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V +%7!<<*"!%@'Wz!#5' + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 921.6 0] CT +[1 0 0 1 0 0] CT +N +-1920 0 M +480 0 L +480 1175 L +-1920 1175 L +-1920 0 L +cp +clip +GS +0 0 translate +480 235 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 480 + /ImageType 1 + /DataSource Data + /ImageMatrix [480 0 0 235 0 0] + /Height 235 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlYNTE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzJ9M*dVuo]~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 0] CT +1 GC +N +0 0 1920 940 re +f +GR +GS +[0.48 0 0 0.48 0 112.8] CT +[1 0 0 1 0 0] CT +N +0 -235 M +0 940 L +2400 940 L +2400 -235 L +cp +clip +GS +0 0 translate +1920 940 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1920 + /ImageType 1 + /DataSource Data + /ImageMatrix [1920 0 0 940 0 0] + /Height 940 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"-:! + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 921.6 112.8] CT +[1 0 0 1 0 0] CT +N +-1920 -235 M +-1920 940 L +480 940 L +480 -235 L +cp +clip +GS +0 0 translate +480 940 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 480 + /ImageType 1 + /DataSource Data + /ImageMatrix [480 0 0 940 0 0] + /Height 940 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzi,?jb%4q~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 0] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 1175 L +2400 1175 L +2400 0 L +cp +clip +GS +0 0 translate +1920 235 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1920 + /ImageType 1 + /DataSource Data + /ImageMatrix [1920 0 0 235 0 0] + /Height 235 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzi,?jb%4q~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 921.6 0] CT +[1 0 0 1 0 0] CT +N +-1920 0 M +-1920 1175 L +480 1175 L +480 0 L +cp +clip +GS +0 0 translate +480 235 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 480 + /ImageType 1 + /DataSource Data + /ImageMatrix [480 0 0 235 0 0] + /Height 235 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlYNTE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzJ9M*dVuo]~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 -0.6] CT +N +0 1 M +0 310 L +955 310 L +955 1 L +cp +clip +1 GC +N +0 0 955 310 re +f +GR +GS +[0.48 0 0 0.48 0 36.84] CT +[1 0 0 1 0 0] CT +N +0 -76.75 M +0 309.5 L +1193.75 309.5 L +1193.75 -76.75 L +cp +clip +GS +0 0 translate +955 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlSlTE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz! +8q],& + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 36.84] CT +[1 0 0 1 0 0] CT +N +-955 -76.75 M +-955 309.5 L +238.75 309.5 L +238.75 -76.75 L +cp +clip +GS +0 0 translate +239 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzp_ +u9P$uu~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 -0.12] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 386.5 L +1193.75 386.5 L +1193.75 0.25 L +cp +clip +GS +0 0 translate +955 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WTS+'TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!!% +O*SPYH+~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 -0.12] CT +[1 0 0 1 0 0] CT +N +-955 0.25 M +-955 386.5 L +238.75 386.5 L +238.75 0.25 L +cp +clip +GS +0 0 translate +239 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;! + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 -0.6] CT +N +0 1 M +0 310 L +955 310 L +955 1 L +cp +clip +0.941 GC +N +0 0 955 311 re +f +GR +GS +[0.6 0 0 0.6 0 -0.6] CT +N +0 1 M +0 310 L +955 310 L +955 1 L +cp +clip +1 GC +N +0 0 955 311 re +f +GR +GS +[0.6 0 0 0.6 0 -0.6] CT +N +0 1 M +0 310 L +955 310 L +955 1 L +cp +clip +1 GC +N +0 0 955 311 re +f +GR +GS +[0.48 0 0 0.48 0 36.36] CT +[1 0 0 1 0 0] CT +N +0 -75.75 M +0 310.5 L +1193.75 310.5 L +1193.75 -75.75 L +cp +clip +GS +0 0 translate +955 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WDeR\qh>d"3DiENij!)0V0m8OSW$BS;+gNsOLi)AcZ#^7If.E5>"B!*gUj1_ZUq.>(NCc3g==@IO +#Vqt<.T/\46]-uM207&'ru`&E60a%i8XcL_r]WQ]oN\<7S%6I_YEBrGd#Z4SFXK%s&k-1k?@b.QU,MC7,/t9ln/EARVW!ZL(YY^V$XC1#.`eUQ$ +@9$kmVlWeHi_FB!,X48F,G)pdi^?e>We,r)$t]YN7lEP1<=#WTK/mM>WX9NA17/e-*81*`I25M\B*AWX9NA17/e- +*81*`j!reFf'3a7lTo]4rrhW^'Pfmo@](3AY:pRl5,` +oH=WT"g/XFW[`[%Q,/uc]QSWWnSbJZ9(*Wi&U_Eb"n.dA,7\`aDj*H;U?86SLjOb=JWu<<[*ejnI#G[i)qPYZ.;mGRJZZl06=e+[h-d$=QqWT99!HqXT(LZObfB;E[LO\nKFk5<>@&aAM>W:33r(JW^9ff +)9gX]pd/QhYJ-B$=QaE18?+pT"UeC(Nq95ZIsq<>j!f(633YW2^D0eMILe;`PNB\JbU6.Ss-]6<+5rJqYEg+0$fDl-:fumq&4ifd<%!W<-'7/6$17.p+d:nY)sL=B/iObY7,%KQXF%JYWWPR$l+A$gV%,!_F-*Wh?CR4["5`il)9A0">7f^F+Y:Ja5`JeuBZICj>U"8$?41k18HX +pO2Y&hqYecCMWqAYc7mgR,%sY.Q9#^F#=4KQla+'6!/K37"@ +)p+.*rqQ6&s8@0!IISJb$AFM#NffHd9(usP.r`0M`*,#;bYL@k`9?DuG0?4ZIeiWb:V.SGns@+is7EOq"uM^8b2.hpXp0HQ +$<1#%<*d1`F\be/n[I,R?XL<^iT\gh/)?i6XMFM'&8Tg40Yd5)4/FmMs8Mo``f1orWrM^qWWk2Fh$3<4 +]#7SA0!'+O#UEECWe0uFj5]0;!$=@M[.Y#=0Iq+U.)]:H)pH4;_kf&>?Fn]B;A]15AIkuiFH$AV7*foQ):ka8+$lT +DL"IpY_@5&#(`QW<)k?IgAG>BEo`&R$Y@kgqXeKGM]9uB#'9Bc0$8E!ROAhbea+195Q0hBV)L=4 +)Wjg[rH.AVXmTQ%Gk'eN=+4=/XK'jC@HKZK=t-Oo2$bI-TW3VhorDfK.RAj7U%Dh%XgG0Ah%i2')5B)Pg0iueflIDqQ!$=CP\Fddp$PtRc>E%l:+kE[H@!8,ip=]2tJ,bRZr*sqp\,5RH[;1]p +H1]KSh;,+2H`gYdp8r7]bP`Alr24Em(CC=)S)+)rh4.nR2):P3ICXB&gE!/sG@@&C5p3-.<>?3Jh-VQL +2Jaf@p?'VhDA)Em\$+6Y)9`Fm0++sbbm*d"*BTj;ie_J35Oul?^\ZKa9(+S%@/f=Rs7u&#J(oBfV88ZN +^.uQtkNm=0@9(R#0\?*jObpgSGkpYEH*/)?i6 +XMFM'&8Tg40HWB"bl.D,J,fD]^qc:DIT^id.YIPT0>;BEp`&,c`XdOfK38G#VY#!L5PicgRf@l2ATAd7 +FhIu(eoN]U<:u9I$<1":Nf?%mTsXZ\K&5rkfiWnG)%)5OlZ*T7?k9^&-,0O,].T]>'m%*R+9G=+0R\ +)J6C%0K8o!,dErAq4am5,ZUM;Jq^35Ad&Q42EnBHX`6%EpUf>-(:*P8J,RQ=86M%/#BTTs=b!#t"g&cg +c+FW[XQO?4W9PBp'r\-?Tdk[t_8;=o:TRPL?+MjH0Mq]g3Se*2mHlL=ja5n^hG8Xte'_Hh*6^j:I\)8t +XVo@-s2_]PcddD?eN8pD:fY=u<k\3sd\TTdr8SlchVr<)B5G:W6TpfbrH..ccW)!c +H:Lj:W[_,Lb$\8SE+J!mE`G&cf<8OgH:pauLZfW<;V#0PTc;4)hhWQ>F&e!qhN:cgWX9NA17/e-*81+_ +>h';Gd^K:jIf9,\%mU!SRr=VL7kpBhN4_nQ1mi'(\T2!%ZDW)Ql;6"mOt':._*V-+Z\!lU)kW@q]#!aQ +n1T:ebKiEQr5t9T:HeC#GJ<[>ZBm^I#BQdHI +*6\l,&P9Yqgt6"P_.taIE3:ZeeN8pD:fY=u<uuroo>gT<)p/!Q,0P0iQ0#oWX@1SQ!UBI7di]O=6!nqI5'hAPr"r2$<6\YU^C($YY^V$ +XC1#.`eUQ$Xf5Lt3fUFHARsR\Sb:l=j'_T[7O7Zd>ZIGsG9g1]We,r)$t]YN7lEP1uB!*nVND/FBkGeS`]d'KX<>@&aAM>W:33r(J +W^;WEB!9IX[$HW>g"gMb&"dhqS!qJ;c;b%JH:Lj:W[_,Lb$\8SE+IurWS5]5-\A2\Z.npmmsHD<%Q:KP+` +K3+pOS%88]t$m_mud4&ePEr#nghJi +YNuE0%m@;pTncC3HF`U*N)YcF-:fR+JU^C($:fL`- +B[QbRo69Z[IHo=]!s]=Ghfa-bh7S$>df&@QVGJ\RKfZcJ;t>AC&Yc:WfrBbCaf_d>D/FL2rabU"%;,?q +g`>2+np[Aj0O$DJ<:u9I$<1"RX6O7M+$4Z4r;#s'hd4!BeW&E+4*FR@b[19RPk&rR;2V@\#-1f'+W16/ +0\^tU\Wjq1&YQ.J,ZUM;6>NSR?+P.6\oj;;.>(cSlU?B)$J)6&p-pgT<)rEo +p%Ie-!s8X:59duSjp`7f<;I.1ZNAa:!`^!%<3>8[.mNa[iiQE\k5@h`B;]KWnrk;JZ9'7WbrdMp%>`%[q@qcil)^@]RKg*Q2]d'KX<>BUU-RU9sL5,-Q]fYOC)9`Fm0+(ZYaJJ0@K/mLBoo>gT<)p/!Q,0P0iQ0#oWX:>Yg>l5Y0UPd.j!f +(633YW2e3rWa;QCL;BB8Wd705"fa%SW6/LKY]+.56WF;4eQQ:^>l,J&0UPd.j!f(633Y +W2cMklLB_m+pN,2W9PBp'r\-?Tdk[t_8;=o%oj>Y<>AFm_dH4W5e]-I%Ug=JlPYosmOt':._*V-+Z\!lU)]u(0GcBEd)j/X._439\ +H:Lj:W[_,Lb$\8SE+Iurj!f(633YW2`Ze<=;_c_dH4W4%W9RIX`,]):SKOeM +L;BB8Wd705"fa&.WqQ2]5QCZQ@q0!Wk.GZH-rAH\+C;35,6i53W9PBp'r\.*W-[me3OaQpL5+KeCE1RK +'afZ(:oBnVT"H.3KW*V;6K)HVf;0tC._C2kmbmN!pEQ5sqi'%Vf9c6,QlY@i!`coYW<-'7b,RA7QH.%d +T6p":2r8d(q^p,JkJbWl6lE!_al",#e;It,;asdU8?+pT"n,GuJ,T$^rpY[j:7O:dhmRCVk0BO`IW=o; +`C_5h?rl33;4q[A_L2k_)$Y0)W9PBp'r\.j^$==ZCM/#t`TGF7.t%\U=b5m-JZ90HFXjl.h$IO?B,JiA +UT/M`hn>UZ,dqiTH\_439\H:Lj:W[asJnf$Z' +E4mYr[75;P&P6)))9aP)Wco^gTW/'G3EBB<$<4,r/`55_qAPC$(1YKN4;RS2.*j7oBMj.TiXl?rjLNAE-&-#;uPQikh_< +?qqS>]d'KX<>@&aAM>W:33r(JC)J[C6kWO##'7qcl5`G2W2a7!.YCm?_NBfgj!f(633Y'!Ed2Hg@o9172!0j!f +(633YP-%p>)j/W#XJu]kPYosmOt':._*V-+Z\!lU)kSsff)%o^oa7`LK/mLBoo>gT<)p/!Q,0P0iQ0"d +[AotQ@$Ctu.YB`m<:u9I$<1$0:mJku@,]2H-_79>RECr%W\Tr?7TJnJW^'Pf$WS(g$5SY_U^C($YY^V$XC1#.`[CV_?J^L6?=?iF0W0l0hN:cgWX9NA17/e-*81*T>da6#//O"?Q,/tG +Wd705"fa%SW6/LKY]+.5'4o]6VfF,cWXS$S;:Rak.Ss-]6<+5rJqYEgT?+%p5*[p4b*fK6172!0j!f(633Y_Q@Qo.VHVub$[s^V?GMX4hhDP'U96 +qB,u`b,FSG8JS2(TW:+-0T^_3hpS>oY@#%NjdpO*;^`@:6CiFKHLcLj&[GQI:cR.YFfDM1:8OrVrfOgF +]3F-QFCmG3r?u7oZndgGun)!X]r7a +h4MNb_"FF`A(?rQK2EtsF-XS319,Xj;?a'M.&r;eNV2u;'n=6BMA7YE?G0P(rXX;B$0ti-0W0l0hN:cgWX9NA17/e-*81+oZ>-mf +6rL4mc;b'ib,FSG8JS2(@%f'&=i%t;NDF>i-M#)q;-gT<)p/!Q,0P0iQ0#/>rB$hS-eLd +AMC$@Wnrk;JZ9%mW\TqV/0*@Wsn-T;:Rak.Ss-]6<+5rJqYEg+%=`ThM@t6@@aG-k*/`n +4-D'igT<)p/!Q,0P0 +iQ0$:RB#Ma&P9[Q3`d3C.ANne,ZUM;Jq^35Ad&Q42Em+#jb`*a_E(a9bUK_-%-EdO +eN8pD:fY=u<3P]tJj>II?P?\"'MST>@VE +oqIOJjb[Q`_E(a9bUKi5LPLWmKu/EF$Z=b0;0//m2 +ZcXj8f"&pZbUKK/mLBoo>gT<)laoh7%+Vn@X&ZE/QWXS$S;:Rak.Ss-]6<+5rJqYH(HFg)G8[GF#=B-6_.ANne,ZUM;Jq^35Ad&Q4[[#?@3KmXs +ed/<+K/mLBoo>gT<)p/!Q,0P0iQ/GsCSl,!SZ8e+<`K?Fb,FSG8JS2(@%f'&=i%t;]d'KX<>@&aAM>W:33kTIT?HchsVf,TZ9%C/!4-(CD5T``$7EHs0"M"i[G@grPMI7bHb'`% +nUkXUl`6m*XK(\E;IVGURX"&`^AQ8>_U^C($YY^V$XAJ$"(.U,A\R$&S +QhZ56Q,/tGWd705"fa%SW6/LKGjrf*qd_HDq*>e!QhZ56Q,/tGWd705"fa%SW6/LK-luN7Z:>\5QhZ56 +Q,/tGWd705"fa%SW6/NAc4/U4Y"S+lmjq]snHl6GK/mLBoo>gT<)p/!Q,2f[1J]@(PA=UcHAPF"<>j"0 +UT/M`]d'KX<>@&aAMD`:Z'!7*DOuRJ,sGa;AMC$@Wnrk; +JZ9%mW\TrWdJQOI-TBAgob*n$W\Tr?7TJnJW^'Pf$WS(gPr(Udh>c3_mP'g;ZZ*Ur-`jqOG3!H5%.H&0W0l0hN:cgWX9NA-9dei(NP]a\b+!S$V**=<)j4).YB`m +<:u9I$<1$0:c6)*gs"6g281Wp_NBfg<>EBNb$[s^,G_SVFp%oj@]Wle^E)-+?pW<-'7FB,69V?R817n,s[]r0Q"r89JN +INJB7)S$3X(BpfgKN4;RS2.*j7oBMj.Yt+mfs>>ZgpqLo>I.qdkic]F46J\mB3`Ei[df"A\7P+S_Y'Wg +E1&)qn4TW1=\<>j"0 +UT/M`]WcSHg>[$8r:!'4M*Lt2dn9AE0 +EME*Fq=Edao#`JJ5,>;t=6g*1mAnn=2P(N4MZQln_Q(ad:)R'F,H1`E'tdU2Fm>21b\f>*hUp6'Z$Gs' +8fuL(_;*UbVlT,]V>h8;-f9eM:JXdC%j=8=<=#Y*qu#$sf9c6,QsEfo6U=`@8?+pTKsI_p%H].d$-2R( +EPoF/9pD?O6gT<)rEoF`;<(hNqdNpOE3]?)'@'!F.:\(BpfgKN4;RS2.*j7oBMj.X8.c +I>d!V6\ig%.K)0?11oFR,\UF9$<1"2X"&`^AQ8>_U^C($:fL`7I/3X3l\iONraY%d$a(9LW[`[%Q,/tG +Wd705"fa%#Wb-0So'X-Y:/=ZX4*Rj>]=W/L5D[.W_"FEe@Y^2lU?B)$=QqG0W0l0hN:cgWX?>;;c?V-c^q7&Pqrh\"g/XFW[`[%Q,/tGWd705"fa%SW6/LKY]+.5 +6WJiEW\Tr?7TJnJW^'Pf$WS(g$5SYgT<)p/!Q,0P0iQ0#oWX<07 +AMC$@Wnrk;JZ9%mW\TqV/0*@<:fS[?WXS$S;:Rak.Ss-]6<+5rJqYEg+0$fD +]d'KX<>@&aAM>W:33r(JW^9ff)9aP)Wco^gTW3TtWXNJC$V**=<)j4).YB`m<:u9I$<1$0:mJku:ad%K +IL$O[W^9ff6Q+*V<=K+3]d'KX<>@&aAM>W2m.Ss+gXJu]kPYosmOt':._*V-0O!-\dDI45ZIuf +.T"YWOt':._*Z[4!!!!a+S+C7zzzzzzzzzzzz!._XT9hc',%ZZL,jS_=fFSYK=_Kn%!3U+i>X&lLerBR +)43]_T`oP+&Lo6qeoE`G@"jHI4u"]3V=o^p+\(GB%'i-pg_@f`UO#/JKQ2tM=qHhQfo4*U+_qpqiO_dA +iRh7.I"Za98UZ"&&DO;Nq9NP58\bO+]aft"\=nR?_$jSba'2_d&GWm:F_`f1r5%,fb6p$:ecS!WlELr= +lK$g)i-qa)t)#8O\bLC\TOF_*;>]=U<,puB"/;:s0a3AY:pRl5-+qk413a,V1Z!O-%PMUp*cGHnYBqS7 +XamRtapkKgd+A]'63/Ue,5\oQnilfmsqR(__LlZ6E]*'\o-SZBZG_hCFnrU&j`,g#q'Kl,SQD(aka^ju +=Td*RmOX4(qJ]H:*8go"3I)1]Qi`Fkfkk$i5c^AWZsm`)m]=25"F#IC5dj5[Udh/],e4$#Ft:SQ*Q)*d +mWpG\Fsr8EDQk5EZDp@d6Z]8Qt**cqM"NN;66HZ%NBAM&?>0>-ekg9k^S^NITH4etke-$6m"J*t^9l\%B2A%5pK?=[FVdo8O",6M;c@c#cJ?$PBCV;erdNIes"`oZ[;4CLr93P8AMA +Ji5*#ORjIKgIJ``GlnLomp?gW&)L:M:GMFhgb[-6gNZL3$`-q2*hVuPHCKijnciA:3hn=BGpflA!9kX +`Po3B15X&a)jZGh5QAMAJ9CY,`7cHa'E76JT1a5*Z?UIUC-rBOi#B$HcWSpA=mXh5`HnCF^Gk<>XVJ,A +)Ln%#d6F:tO#r:.hq:HWXV7-T^&kKeY>Puh/_(o$(\M#1G!n,@?>0T5;R]@XGRF660Q%jV?sh7511gpH +D2k]l^D2tMJX1$J)pJ,]9,a2Z+9be@oK\8h?8jY+=>;Ytl*4[&_`ct3[p%(K*,6UO6/>[=dSRG2l=[r: +G6(d'j%l4dTgpu;#4gpqKUrTdhjq9OOSEo]bm=4]*SnV^F3JF$s8]u]+0NOa?+k#O[)LbY8ccCR2b:/k +Pj:M;EF1B.E\h@c+j3M6F;W6YIIP&XDLV +6Op9Ku!7Rfl$gY:O#np*H]JA(sYlTb=+&)HF!;tk.`[VbHf\aC7M04)m"TuCX.d\Z9\XipT=K^/@&j4T +J6.(taUb^=^/eQ3P<=>2TiQ1kB;JQp3,hd1O$mToR4of'Y]:!4SFp+:`i?NF`In-,[B$td9Nqtj[F/R, +[ErBMRcc;/Y)QMjlFnRW!Fqf\KGQ3<:$P2)R +6o@di0eoB+;=^YlZ"An:4FfMRodhVR\7LsGNfKl6G?`MHp]\?DJ!0j.gf]691+^$JU>q7l[\Hc9 +4/a2d$!eJA2*d-Q`bhL5.Sl07GRkK\3oGF.`11l9'8M]6^)l<(?&8_MFCcb?Ri%s3G +$>/*Ib`0J%O_L=qo9ldiUf21#d@G-3H$["t@>d#']%aFd@\Ttl:PK(V^Z8\L-^>ETSIh[ +8;(sK_(^RO#h>'t6n\*j%ZeE4=5Za6to9=erm1A@&)hN'&hqtKQ'4l)GJGs@C8\A,%%Dr+a3r0Js;o#W +8OjRPj=VP_Y14j-e/dPN[50fV/6>N3L`2aW7RL3OK`UZq%-bEjlkni\+G`aiSErrrf1zzz!!"-3rso[G +'$U~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 36.36] CT +[1 0 0 1 0 0] CT +N +-955 -75.75 M +-955 310.5 L +238.75 310.5 L +238.75 -75.75 L +cp +clip +GS +0 0 translate +239 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0VH\bAq+33nY8Wk>FIn9F5X@R)1q.&0djA4C%UeR>SZpf"J?/cqT;0(?>KTHJ5?]pRQC&_!^M[bN] +-J'Meg\forIa`[Wf,-U@niF+KO/1HMNoS@H4Iu;89d5SY"KmR-ag2)R8%&"s4PAd`'K ++D)!jf'8b0r&[o5G?#S_Bo,5&i0l]n.SF<`Fs%&CX-thhRtRL^(e?:QBmiheaKXj?>F?BM\e&;Cln-"9$G8A +1M9JG+"N9g2RPh`kc4eZ2d3NGm[d\Kk[P +'6D:7]P,ck9mum1)*`R41$?s[^":iT_Qs+4cTb^J_(K6so]];enHB:9Uqb]uHhYUD,CB+KeZW>pKaS]/ +:J,Q^]K']SU^/F#<`W7m4F&]qI-/Y@>ISL0(Qhj*0OoaWP:'_OHoZORrgh)AkKfc=Cht(NE+#cLWiCq* +I@'-\eZ2cVc1^PZirq1#nTW!#r:of,j*UOgNRo+-Cl$\Yf<2f+iBhhp11J1Xp[@"[Wc-e/%3@WVlI;df +3Jk.bkK]&=Y@#&9b*j0fm9XXHD>Y-rf[H`o;u`FFkLk,s2Jh$Mpp9K$jQP!dnN)Pe^TmB7Lb&(P%j+\6 +WrCa^^38mI;o/HfQt(Gim/IP;5.kFJP.4%P[c)B"=>sgY1@:1Q[M20?3<+3PDGfUE_Cn3AIEm +9f"Q*o'\\!)a8Xolnc=)P)`O^Xj2!"$]MXcZX7bGp%9'7P3:b]oJa^YoLc +I?bBs3?J3j:_IKMjHZj-nRnra,V1.--r2JO6^c/X5e>DF)tSTZbO$_*EH0?V$`,@s3?J3j:_IKMjHZj +0RNN[fH@>kEA&+'F`=8MWpZEc7_QL2na1IHp-9s9Fk@ZHSB")"NVoB+?Y4)Q7ge/[Iib7X'L/f3$ZcL?cp.TdRs9@-"[iDi?skW;7M:O:guoQ&q30()21`Y#pj0t0&\1BQr1AH +T2;+:LYni,V0G*+e=qetJ-ljo"YhM=ZNW'6&@2Np.hK#j0e3gA"YhM=ZNW'6&@2Np.hK#j0e3gA"YhM= +ZNW'6&@2Np.hK#j0e3gA"YhM=ZNW'6&@2Np.hK#j0e5eec]"KqX/g:`6M^.8T,BlB`0FJUQn=+sJXA$F +=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2ql +Lfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r +/VmP\Qn=+sJXA$F=b5`l>O[%&DW(?7aje7o=b)kkLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+s +JXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F +=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\k;-?3I-,>)q96t1 +`0FJUQn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\ +Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+s +JXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:aNEbnVnS[8nj2ofhQ!&X)/VmP\Qn=+sJXA$F=b2ql +Lfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r +/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\ +Qn=+sJ`egnn9jA9i;(U=eARVTJ=%pE=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F +=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2ql +Lfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo;l1L'?rk&0,QbfSAQQ!&X)/VmP\ +Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+s +JXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F +=b2qlLfo:r/VmP\Qn=+sJ`egnn9jA9i;(U=eARVTJ=%pE=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r +/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\ +Qn@UhleR?C!thjC_lgeR=to:MN2d'!Y/$u>0^K.*WWg;C^o1EjljA]O"iILYW*1nA$B1tg_lgeR=to:M +^H8I1HpB;l,l2-Tzzzr*^[2Ht[5.H2mlWq`oI`qtKQoIf2TnWi2kPhj +5p!p?p`upHb(RB@!/L;GVOR(^4#lJNujkk:NZ3kcY&=Stc-=KfWg>>Dat/Cq_=Yt)GZ86(')hUqA*QS7feHjEE[>@[_q;5(f<&7i36IgC`] +lX&(GB*b=`WfWW>KnmFN)=X3W8Ia8S;s?=]lVd.upCjal6ebL:XOjmbkp6ldr>DY-+?N@n(Xupg(gS'_ +g>Sp%8tG:0b,;9m_6aZt[uiY?pT[W%sYPF50JGc*7!BrHlbV\Mks,:<_tW]pQQG2FoLC^*Q0=eCN1l3, +g#3jbXWu@[s&QH"&n3EgIG\Lmd"l7>ip.nuXp7jia"24SeF$];ZmY]5clND;;YKI8K%-F3NkjR`%XiUS +Hl#X]mY?2;)iA<8P<8[p[%(oT*-bK!An1QC)Br&$!9bN]HIZZR>A)lkj\jS?3H7HfN*T17!gVNBnn$!! +!"l8H/eDbXls~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 -0.12] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 386.5 L +1193.75 386.5 L +1193.75 0.25 L +cp +clip +GS +0 0 translate +955 76 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 76 0 0] + /Height 76 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W?UtlH*6.MVOV%%LN^S+8LVQ5,7-[+6$`P7:9U7*D,SgtHJjc=7YIY"%Ul1$]"Q;mL6kg@[&s>P( +@8IY+ajTbKo>bKeQ\T#)?!@C2zzzzzzzzzzzz!!!"+8Yo +>,A?siX!<<*"F$4d;?sA@^-ra2He^`1bNCGenleK$lC?*dle'?EE/GA/HP?^bi7Vcf*Z"(h_EfU2#!3h +A[R&d9g_uFLjDc\g%ZEdBQ0/#!Yq!k,^IAL$)p[@"?NH]SjB3i)+qYU0?*\#;5!!)NH'P:?G2p>'o;J4 +:F;2\U]>'0-^R7scbXJd_Oa-2^NC?+Q43IRm6jlPTf-Vldidj`K@!!)MMX"Z&2^3jp%>RsOepRPCH@Kc'jj!8_`2k"hU5*B8"'/aDZfF65Hk1_1'+\msaiq#:6he]aGL)>1A4UdT@WoSk[d2hs^C[41 +9QH6pkL-"B;NcFZ/^Q2:@RT0Ih:A#cT1eP,FibUs5Y!!!#33JH.s28\J3"ec:go('()[^*35Wit]^)ci +SuQr(\"\QrDmb\Gu0q0@0u,*>F2WOVH7>fc?g/@>TJ-HJ!5Mgi_RsO-So?Z:lddZ^mUa +p5e!m@1%8cZHGkJSMYKs3$%1Nc5041<=RG%'_G_Hsr6?_Op1,UqSCG0s1QiR0ei6D.kr`gLOC8'ck<)b +iZP$VDdQ84.]quCeQGJkicuW(1*C"mOiRGXP: +jT_@-DE!,i#VB'&gXj(dZRqtJo,WDr0PiPTE-g5,>@F:AZgR2^@b#VNG;GBJ&=K/XKMn(hP\`FEL4'P7 +=F!-g$,0WRfN;,Io4Ka%_c-71&\5CVD?f][_QZ89;lZ0CE5B!k2&iS1h[n(=n?['V[Uf?2@acT?=qX3U +Y@q=D"QU76@NRB(SVb_<8!k7*bS +S#'$4CIGB[1kaYbjj$8VY^!45s:?<22*rV,m2-'QI&Y@"=2L&?&6b0#UAfibd_i9gRFzzzzzz#_Lk4!! +%NqMH-%%ZH4GO\-`r#puH__D1g],SDR"+nuVJ9!BgG>7.]BWS2r'[3JQUUaj36<%6$(7Rfk)I0 +`#)WY4YjeX"'g=/r\O^9LtGc/AZuqlT_0p_6-`dbX"^ug(C:e;O(WVWYB%O-Ce6=+]ShlV+P/+1/cY_80s@t"; +=\00M=+Ybk_jL9!BgG$?M;kWcgc[JQ*q_O=GreJdH)@In3Rk?)^_hK8`d;,>niD.`$d;.EiDu34&TOWY +4YjQ,4MOUoGGt8g%+?-DX7BQ=.RtocI(s?rs?BjrbmKm>+^CWCeif<=6&8'@*]ZOK&Gq;Pc&P)6%@@U_ +63uJdJDSQmJ,=B)RKa-.nJ4KfR4Fk8<=*iF8mr2b;Updu-'5,0'2kY\9/,>Kq@BPtYU`]\F-)NaG!mg2<$fV^]"5I62`DP2`U]%b0MC@&,( +W]>fTdq?3QO`k4!KqTWY0t_*?#irXf^Em/fY@Jf4GAr"PtdE86(7IWD]+9N)[20;@0-K5pFk,d8AGF+$ +Fq78u3:=(60G!HK)jLO7^,Z=$sm6,mo[/4rmHsB\2V5:"nj!i1bGeZcl<=*iF8mr2b;U+g+hj +Kq%U;+/]'<)a+<@qfqBC"jJDVVblk09C;aiUEWVMc6DHAnaU9AkG.!i\er.ocptO9I#^bnf6Ps'FHgNJ +T)LP:hK?K=h08daI^ZlW9.`]CUkI+sYa9cHa^E++b#cWW]q3V?)^_hK?WCUZZbB''P"Q"hnCb.8]sN%>EJ-BHh +$*L._rBsZfs9lSitichk+[!$F9l!5M5?lWY4YjQ,4MOL8;E"h0;j%=AA)M$>2o*5!u=_[&:LF\>Tc(q" +V?\kJrW*qtBCr*$!21$ADUamhZ;+.`&8e.Ek(YXAh,8AA-eYD]dh8!T#A6W"MX$JjcWP]tM]Sbc+QVBk +aQEVl.8Crq#=!\].R8-1HAG5cPI`8sR75A6Y74joo9I%67KE(@B"bnNrL+WY-RT9dC?QY$>sr)S;iBjk +[Om*oe]n&pV/^+B8_kV+P/+18.[T?U2ojNJh#"F5_g&JQ*r:V>BN0Y7D^=G^lEWK>K0jpPT/PQ!d[n'] +pOeQ)_iYCf*.:MAN8q;O(WVWYB%O-Ce6=+]ShlV+P/+Z9Lo<]d.B0a;/pYXE^F2+B8^e;Z.-ZZhqu~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 -0.12] CT +[1 0 0 1 0 0] CT +N +-955 0.25 M +-955 386.5 L +238.75 386.5 L +238.75 0.25 L +cp +clip +GS +0 0 translate +239 76 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 76 0 0] + /Height 76 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0R!K"\o'L]d;3=g6G>EMrD4UlrpYGC@U9Ue6HfgH:5F0]bR$!WLcHX@"YhCC]HH?iQl!]2&G*ch +R#f.5i1A7rrXAP5X"cg[c@+r6g"kf/lu$mYR?R6e]JtBs')lJoLcHX@"YhCC]HH?iQl!]2&G(L@'4P:X +)Y61%Nn`^~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 187.2] CT +1 GC +N +0 0 955 310 re +f +GR +GS +[0.48 0 0 0.48 0 224.64] CT +[1 0 0 1 0 0] CT +N +0 -78 M +0 309.5 L +1193.75 309.5 L +1193.75 -78 L +cp +clip +GS +0 0 translate +955 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlSlTE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz! +8q],& + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 224.64] CT +[1 0 0 1 0 0] CT +N +-955 -78 M +-955 309.5 L +238.75 309.5 L +238.75 -78 L +cp +clip +GS +0 0 translate +239 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzp_ +u9P$uu~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 187.2] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 387.5 L +1193.75 387.5 L +1193.75 0 L +cp +clip +GS +0 0 translate +955 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;JH,ZM!5fq/l + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 187.2] CT +[1 0 0 1 0 0] CT +N +-955 0 M +-955 387.5 L +238.75 387.5 L +238.75 0 L +cp +clip +GS +0 0 translate +239 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;!=]#/!5bE.WG`:P'EA+5zzzzzzzzzzzz!!'fe!EZk263~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 187.2] CT +N +0 0 M +0 310 L +955 310 L +955 0 L +cp +clip +0.941 GC +N +0 0 955 311 re +f +GR +GS +[0.6 0 0 0.6 0 187.2] CT +N +0 0 M +0 310 L +955 310 L +955 0 L +cp +clip +1 GC +N +0 0 955 311 re +f +GR +GS +[0.6 0 0 0.6 0 187.2] CT +N +0 0 M +0 310 L +955 310 L +955 0 L +cp +clip +1 GC +N +0 0 955 311 re +f +GR +GS +[0.48 0 0 0.48 0 224.16] CT +[1 0 0 1 0 0] CT +N +0 -77 M +0 310.5 L +1193.75 310.5 L +1193.75 -77 L +cp +clip +GS +0 0 translate +955 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W?\h_F]t!TRe!kFE9eObW0HuC5:b76pKO+dK0H^[q&dAqF`'R#"(Eo$i,18ek)&*0!-QrVG#neGU +'T!DjYbVjC$)o*Q3JNV<@RQ>eAH,ukr&3#lfmEFbpK?gohXu8Xi+sdPGi#jZrpE"WG,J8kp$:5]n]r!= +?iTt(h4)bFs80R%s7r;4J,S!i?g5T&7pT.A=I%763]jsr?G10qer\ZgZSO&HWe:XX*LI0%^ADg(S'WFu +9ZnHW19=+3@b$;Zh(h((I%7Dj_5`0bd*!FmFV'!+]-=`qfP;)Eh#C3CpM=HsGkWWP%#_TKkWl3LfuJlN,X?*+1EjWX7Q0,HdoIbsPq\l21\98S +=0j\3_5`.oTfiY@_EFJlN*hW]DgI,-5TG'>>)r/WZPn0,kojU9M2N=+[T06AjV09%3jgGBN?E]PqZrQ0/PTrpXSV(9!Bcg +6<+8&6A1V`+RdLu<[Ck50Lf&Tju']QAnP"n<='8;3\V3kXDA#b0/dTGJlN-;"Vhl6g['#q*>#FZ(Z^7(Ta$!&??W9V/lLp42X,+^Y3CgFPdA@8rta/Xe!X#`&?b'c-/P7jrhLeuWQR1Ne< +COt?i\$dd/WeAKp3]!u#^7(Ta$!&??W9V/lLp428U[R3s=dY3/?Sba +?F?JAorLc9Wj[:eG$iM>U]d_2,#i\m-:Cs=<%W=L`E00q<=)p\2UBV8X4`o*?r]OHWXd7+#V4)tlRog9 +WcbQr!Nk)0%lA95c-E*fWtHUUF^QOa=BS1S:N.%R[741CcrJhGTfMktlqC$U.Nq,Y#A-;UQ%d_4H)lOt +f#XkE$ECORAp.P+cD1TD&gC:t?m\9D=[KgrdA9mGQW4@B<,LC6m(SpZ8I8ED7&]Cd9SfpYg!tQ?>\)C= +=d;fdeQ.*kM>M%2-7)cZ&>?*+1EjWXk#M;cgaTLC"BHgJbf,k'e5m*AKC3CpM=HsGkWWP%#_TGV%g9fWa +U1uqo-.Gr&m$ZOT2*7!7X@e:FeUa,"@:^f#n8_M4<"6pKKGpm,4>XUQAnP"n<='8;3\V3kXR%3AhWHf1 +C.qYSe6=fgUYRKV=0j\3_5`.oO]drGHds4d$pf94CeN<='8;3\V3kXR&IrEQ?eCWf:Cd6Cb'l?T&foS>sO^.D`$;nZ#p1If8]j +]=GAdJXOldj/!@:Ei3VOS&21Y:fSb?@)>H$<@rhp$!&@2;>&5*44D[-B?m"bn3*7"DdNI9/mc3Cn!b>b +#n8q%RPHI=)HF12X@e:L[>^M6>1E\AP[IM.W)[ck\1ocqP=F=."7m$fr/eYOaH7^>/jhEg9k_c^mtBPID^RAGo6j!6;ro2B&/5#V/!>= +Pn:9:K,u&:.V@4#1F^/?K]/ug]6A2ZeueDSpKm^@i.3o5e^N!r_f(/01SGE$9=(MW:.)$>N`?B?X#B2? +=0%3][F%Z?YUX(WB:'c(HIR4$3=d0u%im^*mp._#7*C##NgpqLtoCM4iD;2LIs8D]j^46)bAnPc6eVMZ6Pe&e^OP/;? +eueg`jdkPBD?$bcdfp+b[@sZoLg7@FiLb!]IoQ0n%2-3d6^a0H +U]d_2Z4G182djkSc1PtbF6CiueC:a5a85ag%a9TVa%OmZj\Ems"gBJM.92ip1H2.FK:%m2.BF8P[up[@#.pD5kE'*94.9tpgH-Y1a/=I%7+WXYDb)cX3o9M2N= ++j7Y`5saf:$V?*+1SMgR/Tk7b`&M)G@oYkEX2^ZtRpIrpP7jrhLeuWQR8Dqd2W^+%@u,*: +PfKeW!icGeP8$hN)Ntf88r-l +NgK<=Pu)kY>%ZpVX&dQB"KN.Z<-;Rq5V!f7C8c93E4hFUWe<90XaXl"6e.'^P[IKXKW5O+"CbqHGZfu: ++ri8tYpuFn<@`M(hib!>1+C&>KEtQuQjMGIenMZ.j*3#6<=/&l>326lg/GC7-7)cZ&>?*+1El503\>PE +3*sa;4.5d]TgI:KXOm_AnGpbeI%\.?r]OHWXd7+N%NWr1NR?4E_K*0.Nq,Y#A1i+/)0V*.-W+r +nPUF/'&$TUS!i#9'k1'm<='8;3\V3o=]mD9d8,GUV*%d)[(Im\OMBmW#pps#Pn9Xh"#FBPQ$$7O+N!YH +*ifpT9"_.IWnEf)?r]OHWXd7+nQ+4TH*@Rp96mU2Y\FE1RB;pK8I8ED7&]Cd9Gh8NSX.kAiHQf4<=)^n +au_C`2*7!7X@e:FeUa,B`le,Y-#8BW]TgI:"KV6,4L80J4-L;Y8r-?*+1SN^K3\=,O_XUMFWtMMFVD6qqB%?6A9!Bcg6<+8&_Q#-PZok#fa%L]Z9!IS77\,sLR[m"V<[C/4 +lG-(q\[#O^H#aLWQh"7D@+?E[1c&>n4-L;Y8r-?*+ +1L]Oj3\BM-iHQf4<=)\HacJ0k=Z&:)'!+]]:t:,bcs'YNjbtB'0BKGg_5a7Kk!qP8R[m"V<[C/4lG-*W +Y,p)m?4i3anlV/beM4-c1Ku^uP7jrhTW;D6If8]j]7[l=JXOkj=[/Q)0@)rfWtMKpT#!4h]u]m#Z`6RF_h/OBG+OBn +<[CjjC]iP9QHlORDd@)4;$g8L&60M^(?XcA2K%<[CY(0Dk$"Porjm5:3A"n,=bpHcJlO2GDe;c-Tm\g8h'%!Yr&'T[NcHa/)+jA5\Z1=G&h3S`&+4\&I-.onXGMS`b+KH-VRROuTlF(R0Y'\V) +iJpO7.UcK1\d%<0s)'poA4Qm0+!^&QQ!r3J]\F(@*L1#4!a=-*\M?q'eaP+XYU[Jh)*'KogeeAY,rdTF +_%.rnL1_Z]<=)^NbF'7-N_6N*_1B,sX.IGHr"6cO&Ab@LX(LPd&d3DXJlN*ebY?^XGn2R4:[fo'YU[Jh +fr]RKHY;II\YBe=8oS0UZiR?CHFsfj9!Be]We=A:6.MBK'(2\$WtHUUF^Q9:i"%5mYd3Aq.`&+fWtJ<1 +J&46"G>s/o#na0u0TTYePBMVE/WWm<<[Ck5d&lT2WFk4O9!Bcg6<+9Q;%<+27td-ZO&ZKe-!6GZX6N,0 +EcFSq=HsGkWWVi`L3u9@_TJ1bWW]@8X@cSueX8E*G>s/o#na0u0TTbc9,g<%g%&71?r_eY$-T>01Jc:: +Q!dTYKW5O+_]/UQ*+(k]99gXX@f]HTjLf4k-;Il'!+]] +:t:,BYjkK-&QZ>&<[Ck5?rb'oWm8iKECV`jjfp;OWW]@8eu\.jk!nHm9!Bcg6<+9Q`fOgq/!X@e:FeU`!G +'40NO_TJ1bWW]@8X@a>8W\a89geeAY,t+]s^qA+VkM$7m>99gXX@f]H@JHjOTZN;N;$g8L7&]Cd]M+W( +-R,X.H)sWB.`&-'\<8r- +ROmKZ9!Bcg6IjWsbilAB&QZ>&<[Ck5?rb))lFIP +_TJ1bWW]@8X@hEN[e9etgeeAY,t+\8r4lR\ZNh(C[QIS:=I!8o!`ue;(X6nh-!6EDTk`6_F'YuW79J76 +=I!8oJlN+jeTT6^m\g8h&gC:,G[R]EW^.4t[QIS:=I!8o!aW3f1UB.m_5`.oW'UgPpgiUJlN*hWYRBEa-i5T0ik_F<[Ck5?r]Pd40=2g%&71?r_eY$+3_aHY.Fk-!6EDTkXT1F20;=7Te@7=I!8oJlN,"<@rH-G"3i\ +Pn9X(dg,$&Vm6RY>99gXX@f]HTuO)5T9dU9*L1#46A2@u*ggG4bFKJ#>99gXX@f]HU#)fshY*ifnt +<,Lj7*;kuj&QZ>&<[Ck5?r]OTWkn.P7M6t\@B<(W:pl,^F8X)R&QZ>&<[Ck5?r]OTWki'"*(c6fKs<*N +6Ij'bbjhUm:5kGiO&ZKe-!6EDVeRqBF.B$PYps23Wf)dnQS)RTHhVEtD.qM9gk][Tj71!j.`&+fWtIa! +/RrB&Q*_B/-&bgr`/,-H2JcLqe=JgMmbbe:mlNIHhRrhP?+bEeh`!K-?uF>V0Ki_J<[Ck5?r]P#WWgVj +d?9g3WeB?3Sj%Q`9t'Zbo1tr?\T[Aobfn;IK*PKVc6%N1+arO9,=aq#,2Lo?Df3_5goOG<`s]_`"KN0: +8dJP,5u6OB.U^[+=HtRfeJDP:@GeeI]e;VKo[>Y\hR[iFj`d:VF!gk5J-gLh7L[McR[-#V0+=loX@f]H ++_K9%]MWWJ0p=Md<=)#nIJik[)J`-?4P@>X@::>8o$KrLXo.=l\[gthl/B)prkR=!Bt<9o_QYN+WW]@8 +_H(OAbT&NTUKNj9ACV&(/u5cGm+J^7jR5o<9k-?IFaK"FKSP*I0)Om)M\7NjL_hSc+r9!D$\#3C*q!mD/s-fRP:LA"o[dEt\ +^[Lo`Y@#K>m-M@Wfs^;*/#VKOl[G:nrM%<6>#Ta;;YMLPaJoBD'!+\2?F>Xa9`OS/UIPOtbl.O>Fq&lDhXLUn_lWnLB^Q.:f\kN=nU4>9>eLjA#12-G3n,$ +^YbZ<*LNTYNraF[]6E`Aq=Dc^Oc]k1Ib17Y +6lhX64Y:k??5Q]h=I!8o66iKgCM%HPd`/OA@JHiaGO@S4i'?bSI[?>c^&__]DQ\3,SND%mVP]@o44`k= +%QH.$rQ:EHNu*ON$!&?SQ-bbq%@W]SP-eAn8f59WpOc,qJ,fHp:S(h)H?Eh3VDg_H]Qiu)_1TGq/aqT3 +J'4-0h=kjC\8``fg=s)T+7QOcd0A_;SSAAuGTg1S?r_eY#nkcE`q\sId0f:i=rQ\JOIJ*=$gB7H/:]kbVdrNLhKjdu5<=)\P1ok@9hH8F3UeP8t_PgHN +(Dj8[\ME+-igtjXL!,f95Bu[*c0pTI0GcMsGGDN`2)p+Pn9X(OF_\#[[_'B'4YUs@o\+\$!&?/ZteZK[tZ4=cDlO'i)jCM6XAJYq!Si"KN0(>;bckgt\]f=d9PlC:s^jOje=u`laUf +WW]@8X@d/JeX/.6UQ^FD1^P0nC3RtKU"-M +<=)\P=Hu]JeS%_l'p(6pWtHUUX^tlmHWojU=P<7IQ!dUD<=*59%J<3/16P+:*PqA%s$!&?SPu(B/+>\(18`R$)+[X_:#Nd.A$!N"I/WWm< +<[Ck5:fu6RcQ*ad1^L,p,#k[b?uF)ngki;Ajfp;OWW]@8$EG*(h2-h3%Do,u#ZWAoR+jDKG@IXiH)sWB +.`&-(i.mO&ZKe-!6F/ACZ!=js$#?cDlO'<@\oRCOZn$]a-,m*ifpT +9!Be=e>bC_7@'CL?0E!&Pb#06<[C/4aplRsSX)>l$dJ= +DQ;ei+]]mWW&i&9m'PPDKU"-M<=)\P=I%8(Wl2Lk>FlO<&<[Ck5?r_eYQc_'>S!7;\6A2?`%Er.! +Gt8NJ[QIS:=I!8oJlS/*HtG8c_5`.onfkG^E&QZ>&<[Ck5?r_eYf/2b'*1<%!@_5[X='!+-Q +O_/3h'!+]]:j(<>fafL!:kY)cO&ZKe-!6F?eKGAd1RjV:+ud%<.pF'q'!+^0.`&.&)tI,b +P7jrhLer6I_!JFu7>-pXO&ZKe-!6F?e_pu5^.OS%X@e:FFm6^.[%'dBPqA%s$!&?SQ!i.)$o9]/P[IKX +KW;_:fafLQ.)O,ma/T9C'!+^0lDW:(R8$;X.Nq./m_J1#1%L4+ZK8>@_5[X='!+-K_0>;o-!6GJmFnt9R@/[4%hAC6TDe-VS +fDX=-4N4G*GC:e@?r_eY$!&?S2&Dltp/f^m:%$H+9;B3Ij-aP/f6p#BDoTl>'0F%1"KN0:9!E&"koS`+ +gK*o`>CR5b-eJ;/^Jh%*/\>TYNr^l^qe0eWjGBF8`1('!+\JWcgV:jSei1J+D[BeZ2baAnI@ScRV"0'0KS,<[Ck5?r_eYZSO&HWW]9cn`.Z: ++-!sXn(tl&NU=MTn9?*YQ!dUD<=)^NcD1TD'"Fs"Mi1Z>>T#XRG<%>j4C/:o-!6F?&<[Ck5?r_eYZSO&H +We:XX*LI-F&<[Ck5?r_eYZSO&HWe:XX*LI-F&<[Ck5?r_eYZSO&HWe:XX*LI-F +b*YU[Jh +"KN0:R[m"V<[C/4)IXT-@o\+\$!&?SQ!i//9M2N=+[X`POsEUs6e"7\99LPlOsEV^,Y=.E8Wk>jOsEV^ +,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8WlQO\@?eJd\XR"n(,/q"hK0]\m[Bu=BJ,klHPeWle[_t +gqS@=il-i?^NFHcJPt%%^A=ktXcWC*o&RnOhgG%gf3bH5e@dH^+Pd(VqsV;'CBI>%33co0L@n@0fsA^\ +c&D)G\MHB3IHJbA\i6Df%j+"5m;IJ92)Wq?=.ap7flZ0%]hsd`q!mBDFoD>akg1L!!-oSEE+%_GNHK/. ++Z=8q]`(-l#C:N2<3r,ZIc<"-RdJ9O^KeOBf3a$@mS.K0UIUC=#(W%HHE$bG3?9[8 +T:V[I:Cf%\?+ZD_Q\,MRm%=T.H+BXF%Ptm*__'h5$)Q4o4]h+4Ef(bEa`o +,W^dHnWX!;m^Z6V:I"fbmS.K*^]!P<.HAGjJ"-^n1gE3+R]nU3B6kX/EDj+KBF^RVm+bEPmM?n%A6en!7J(Nht%817Q7lhgV'7 +O1fo73\Y3/:8I-9G='U=Puh;F]mFtlbM0LEDr&"T-g`_dm^qpcdQdZg%s;e]j/!E'H9g*Z1-H7AB7h\k +SGDsMj7G!r4F+5V9,^IO/K]E5;.\KYUjMk(V.8T"pYJQXEF@)Orr)_dIJ-f)q=JPr_d7)iFP,%ZdNSYK +_1UU2WFm-C*LJ4@)SZ.?bAY)e<]Zd40P<+!2`El_?2%Baj3aVlknNuTJ,H)0%4Tu%4aVYRb?q,LX6pF2 +]`%a%V_`)Vb7?i80^ZPH7fqCu$Q%7+GkULWV)N;G[->gohlLW5MNijWF=ui@SX^^f+Wl+$6#-;YQXA=W ++XK\.?$]&)7_:6V[f#g%J$"'=8oX.LZ![Ip]Hq9DnRTDFaf^Pe'rTTsSr9%^bIC="I[bq#F2cX`\G;Y`#aZnTk*^4t] +b0;?%"#K#\Pq-Q=p&4TpFRM8S*BSH"^4!W]c"Xhk=0Hd;On'Yo]$mbN +kN/0c%q<'>FD8 +bN\9U='g16iPUFnDbD8%Nr&P@j0-5s3`Y"IFfR=i,T]*po\=N6rT,\^l$hm4S4bo*?FtB_pZ9Yh2q-IQ +kg?._DGm6Xp=X(-i6*dggmaBY?G*f7?()[9HG#bWSiV"omL:V`l`\'pm[MWr?t!L&`f1qLGOJ:fQ`HXE +]0@J)>;A/Jbh7F8aX:g#fXl@2dk]nMh7HTa;sAj\h&FdMJ,J=KJ,]&3Vt92aVqun"m0t!mUu-Rkbr9c` +>%r0HfWfr+rjoS5WBU@[oBp/pQJUg]B"@j=pV5UuH1U%T@bhu$Ji"W(qsh6&%j%Pp:L&be>`G +Cu)#Q:S0haoZ"";\%8>HnV^cP_@pW9h$8/Ih@B)A?iR^s_E2u04E04Dmn5;aQ^7eq7d8<3MRiB%Egk@G +4*QurRd[lW^.Ys-hG0^u/1rP-/FgO]+)K`B1&],TRO6FQmcNZoHFK]`GA:kB4c$7\X.&J0B:iK+La,'+ +p+fcRI/EePH+AnGZ>r2^2b0^"T'\TDpUs8;K3 +XpO"a\LNRj*Ud4@CcQ_->j(@p?Ki&D`GZ/8T'p+5AcMc.Bj^8ALTnCoDVr1aX#`r6j,ZE"pJC*?7s[Fe +*e:p"lDnV'=F-N0NT]b[s19(eat"M3h=iUI5IkUf^3uJ0Db^g:.c--Ea,fsY)LLr0]q0^/Ea[2Se>ZBI +^!+k[)u%X[t+X&0'U)!mAmqH`_FpQ5FjjG[!thH:Nr#:pu74rO\";4<.^;]:Ss`J +@7]2n;N>j#FnkIbhVnM4o?VpZeQ9.pal;=>Pq,oUQ)/YMr:ofY9:,QI>fE%aONWtDq^cDM:S'\ZIf4SX +T)g*pV`V!V)Q4oQOsEV^,Y=.E8Wk>jP,'#p5:N-Y)Z~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 224.16] CT +[1 0 0 1 0 0] CT +N +-955 -77 M +-955 310.5 L +238.75 310.5 L +238.75 -77 L +cp +clip +GS +0 0 translate +239 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0T_1u%j+33pNR\;"TCqc<+m5%qQ*4_5'ZmdFqdB6J;RS@N/0smlj,dh>@M^7gEM5X.3OcmrPGC50# +VOoVU?G2ftGW81e\7Rq]o=XgFcJ3mH=9fWp>Zd&*_dDBkq/).ONW&Yp\n0'N5Q=(Wq0RR[XdId8rd7'4 +=!_IUVl/9BfS%1J+m&n^=,8k&bS=S^&9EPm'?"-!jodD?LcIcF'k0oLF+otp`*=JJ">Nu63C1k(@SRJU +KpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^=,8k&bS=S^&9EPm'?"-!jodD?LcIcF'k0oLF+otp`*=JJ +">Nu63C1k(@SRJUKpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^=,8k&bS=S^&9EPm'?"-!jodD?LcIcF +'k0oLF+otp`*=JJ">Nu63C1k(@SRJUKpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^=,8k&bS=S^=Dc9g +Za8,^g!nMNHO1Y;BJ^Y)S^>Q5?^UQ4*$FAN0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r# +jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH@]g8`Js\(,S06r$0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<" +bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH@]g8`Js\(,S06r$0dhdf6HfaFXq5Z+ +Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH@]g8`Js\(,S06r$ +0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH +X,VCJG'9<3S!od\04ElYNCIEm'7-)9$e*t#"2nFfjodD?LcIcF'k0oLF+otp`*=JJ">Nu63C1k(@SRJU +KpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^=,8k&bS=S^&9EPm'?"-!jodD?LcIcF'k0oLF+otp`*=JJ +">Nu63C1k(@SRJUKpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^=,8k&bS=S^&9EPm'?"-!jodD?LcIcF +'k0oLF+otp`*=JJ">Nu63C1k(@SRJUKpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^=,8k&bS=S^&9EPm +'?"-!jodD?LcIcF'k0oLF+otp`*=JJ">Nu63C1k(@SRJUKpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^ +=,8k&bS=S^=Dc9gZa8,^g!nMNHO1Y;BJ^Y)S^>Q5?^UQ4*$FAN0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<" +bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH@]g8`Js\(,S06r$0dhdf6HfaFXq5Z+ +Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH@]g8`Js\(,S06r$ +0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH +@]g8`Js\(,S06r$0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0 +`14'cQq&CA3ZS1to5B$dS*sHrbZhM8*.R2rB#gc/`9c,5LHT_?Y\-]NrgH^CY8IXUXq5Z+Qn-+F+Qa"c +-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH@]g8`Js\(,S06r$0dhdf6HfaF +Xq5Z+Qn(Y$;0rV=OI.PW4i()JN'5!V.uZX-<%Enj_,MqD4i()JN'5!V.uZX-<%Enj_,MqD4i()JN'1UW +;]UM*=Qj;tzzJ(U)@?_0B*o(hn?p-LZoQS2]pr:k9Nanbc.@E:[*B>&]ip?hM:dp';!*?,q:^7I<(@W$ +(d]r6qXh7ImCG3ukjs8MSQfGl,/8e_iQNY^:\Q"\3:C8L9"a&fQ4qXqmY%Be5K:E2fIP8GOF8Sh7NFMhnM9l<_L +Y*_nGbW1toQcn!PtEhS4P)3B;B]o]c(D2R!oN%sQL2)&;c7o>@0Q]uaHZLjAN6DqMfV4^mm)nlnH3`L] +5>DVVb,oPeDSAsi2E[r70tg?;SU4;`6Z7un]sHK\.G+X^'W*;7K-?G-('hsPB0I/Np +h!VQmA0nEei7M2@)ND6mR2k2m-q5.pYFl`e48Gk'dtdqCCLA,A:K_:>SfQTi\9#4CO*r"J2R0F_3" + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 187.2] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 387.5 L +1193.75 387.5 L +1193.75 0 L +cp +clip +GS +0 0 translate +955 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WIq4iC+33n+a^d.Q8Wp"lQ"9o:B7D1hR7rd,aYAWGJ#%mL8WlX;7oEjn0f=ek8WpHMrlETrdN?Tt +T;c.`l?"B(V">:2B2\gku1A.34<>;%U7lOa,f'4rr2ojp3 +'dNYH+^.^OEl"V#^Pr+F,"lHt[YEp$1*'#-;%J5p@\+36d(cI.t3'+$hMH\aRJ]'Xf(!(nDJjLI-QE2?smC6k07Jp;4%@a!)V>,_h +Y?Q*^>+$r'Mi,S?RdoS2iD)Tu"Q2l`oL5:KX!dmUifY$-*jgb_VV&I-]@c\T?rfEnmp8$8VY^!"/&Vl* +9*TGk'e`&)#0@e#%QioI64b0JK_Dc^p#O$?p`\.J8L;"k4eTC:AB8sD;&O$EUVaI]1l]W.#GNsC_OS_Yk09B.GHIWH]mBADl-i[fmB +0IHB`+2M?`^uQ0s.R[eq)XUU3rtOqs +BI1R5o?NS2kYb6a(CQ_M +&Am1%nFTAS";!75W"uDf<,=BjKHKN=,:Hn8NYuH.70cp[@"#+1S`q[:SKj\nn4P[C+Pu:uT:XWd_Uuf@ +8;PhmR-'bT!H6!rr=o852fQefGnrHK=M&[C,+D'@<<8S4]V]rl]^EE9"rtDHKl*XlAK5dG\1d[eWR_/f +jhs5=1#4-XelJ +dkJF`gD^DT?4?g&30geA*hr+Rl@P7G>)EL!!'f13.Lq?(! +*hl[WCWWfaW/a[E.Rqkd5'QWiE(>c=-/,lYC?SNNTJt'kV[$KferOc5in@Z=Ss+>Oc"E19D2D'AXH2-S +Q!4Z#"V`l&7kFkMB&QpM\s']mB?dHdnW%/bAr.d2B$hN,^Dr(68esYPG._Bamm%FKI?_.g_)#sX:5Gb-M_gR@php:@dBld,)'cLJb6p>^t98Z1&-Kt*o2(GB+mWV9%o"onXR_U-Y+a* +0Pq:X3T\@)*\!r$V4/NTom!Soe]?U=j?@B2S?pou19e)#sX:pnRKUpQq55n(=n?e11S#!!%]6o&\&sXf +_$S[U6du?0]r@!!!#9&tMi>s%u)_!!!!o='&I->FYT#e11S#z!!!"L5LC)D!!(q9;6%K" +]A>C<mtaqo<#%_j22$*C*`>u\<"]A>CC<"]G#R8/4Zq+bWf^=GPr\<"]A>C<mtaqo<#%_j22$*CSod#!l;^@t;H4i>"]A>C< +UY`CJQ*@N8"]A>Cu\<mtaqo<#%_jQ,9U6_Nmn;;H4i>"]A>C< +UY`CJQ*@N8u\<mtaqo<#%_jQ,9U6_Nmn;;H4i>"]A>Cu\<mtaqo<#%_jQ,9U6_Nmn;; +H4i>"]A>Cu\<e!i[Xt;B6_sgqS>g.[=3.h:f8PXZ717:LR;^o:eLM$Da[eWX>Qe*K_h5We)GFIJ`_<='%V3n +mlia1m?%We*3,J=+9r6+l@MOZN?a8"]A>C<NB[5eeTfWsV +u#]/X&Z3$2)R6+(pda.l#aKS5H1?MU"$-)&F&iEZY'/7o&dj.oP9ZX]s\Ha$r*AuU8/4Zq+X^:)D +>D//(Y.'d>ISJj*#pM7R/O`O`qTe9-WdJKH]5,gSVN&9.MKjjIGsXpG=8GrWX?=:n(taiI/3?(4*MHXb +@;5u%6cFD2doq!3@-mQ2.70]0`.Cfr$NPVfaIe^H:Ob" +d^^0fe<&!mtaqo<#%_jQ,9U6_Nt/fUskNT + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 187.2] CT +[1 0 0 1 0 0] CT +N +-955 0 M +-955 387.5 L +238.75 387.5 L +238.75 0 L +cp +clip +GS +0 0 translate +239 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"/k?USsb*60-VR3[nIP%'rtmEo-Yh2U!@CNAmuH?!S=^5n]*p*ZcAE.]B\BPWA1@%hIE2[\p%$ +bT`Cs[648_cS=n%dFJ[,TI:a='&JWcBXY0%/_XOK5m;*himm/p!rO* +Q^3rG[^K(GI56TXo4Q55r`ak-#m:;Hj6c69G%Z/K]@QJ2NK*)_k)tl?]A5Y755t?P.#ZMa``:C4d"%Qr +ARupD8"7>L66X_^MC'YsA8X1W0X/'s#U'd`iFG_[cggN[.V%N+m7eX^e/E\^q?bN:b0%k70Sg1\XAA!J +Z,kSl2u_=YB,MiaJ7&`m$ZZaZ1.Ej5rEsnr)'>cSV[j7I[^NUUal+N)Td\I6m.:$L'?LPqipb[U>,Tgu +Ug7/-P!s/%CXZKC91-#+Xpj!cac&KD;2KnT!t0J1O7QhpRT`4"k6!k?NJ +;6_]0sH$amBgP>iTCk$qa3#kC"%a97mcXb$9!EY%I +BlHSX2BoTT8l=`@A.IMc[m'K`h@Fq('h$@4lKXe*RFH`T2e'%Z."Xn_BX?AaIqJnlpcY]A9WPh-Ri>hp\+;Ker@"LheZibM_aJ7MOYe9./#MY]j<@`i/h$8#W[j]iFm"B'1YW^;$Z[0F8>f0 +j?M:rIa7b6h;.W: +[$2FE#e!U1]b02PfFs*0EqZ+\k76$.b`'U&3GZLdBJcaf@c]+/');`9JB-*X`l?p'm:WB(RZtA8 +5kIO7&I\m8eY"t(8XYJA`,HtUaDIGBB?jjOrAs5F-=neZ!eGmJk=['5H#-EMeYLF-+R6$TAL7UYB/P0? +k'mE1;fQ+~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 375.6] CT +N +0 0 M +0 309 L +955 309 L +955 0 L +cp +clip +1 GC +N +0 0 955 309 re +f +GR +GS +[0.48 0 0 0.48 0 413.04] CT +[1 0 0 1 0 0] CT +N +0 -78 M +0 308.25 L +1193.75 308.25 L +1193.75 -78 L +cp +clip +GS +0 0 translate +955 309 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 309 0 0] + /Height 309 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlQ&TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!! +%P-!Re\u[f~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 413.04] CT +[1 0 0 1 0 0] CT +N +-955 -78 M +-955 308.25 L +238.75 308.25 L +238.75 -78 L +cp +clip +GS +0 0 translate +239 309 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 309 0 0] + /Height 309 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;!=]#/!5bE.WG`8*TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!!! +#WfDo,d64s~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 375.6] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 386.25 L +1193.75 386.25 L +1193.75 0 L +cp +clip +GS +0 0 translate +955 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;JH,ZM!5fq/l + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 375.6] CT +[1 0 0 1 0 0] CT +N +-955 0 M +-955 386.25 L +238.75 386.25 L +238.75 0 L +cp +clip +GS +0 0 translate +239 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;!=]#/!5bE.WG`:P'EA+5zzzzzzzzzzzz!!'fe!EZk263~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 375.6] CT +N +0 0 M +0 309 L +955 309 L +955 0 L +cp +clip +0.941 GC +N +0 0 955 310 re +f +GR +GS +[0.6 0 0 0.6 0 375.6] CT +N +0 0 M +0 309 L +955 309 L +955 0 L +cp +clip +1 GC +N +0 0 955 310 re +f +GR +GS +[0.6 0 0 0.6 0 375.6] CT +N +0 0 M +0 309 L +955 309 L +955 0 L +cp +clip +1 GC +N +0 0 955 310 re +f +GR +GS +[0.48 0 0 0.48 0 412.56] CT +[1 0 0 1 0 0] CT +N +0 -77 M +0 309.25 L +1193.75 309.25 L +1193.75 -77 L +cp +clip +GS +0 0 translate +955 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WIq[C,q=jDF.AMhW`le:B1&B]JK$l(kL!:4JXW9cF43Q>ph4t/[5m_EC2tr*nB"K6uK6AOh$Dhn$ +*+BkCZ.lS=CiH?J"j1H(FRQ3HqV8 +54OL0H[4Y2G'r_(^&7HEm662L4T>-UgX:+F[&DeN;]U(I@&a?.%;2I>;^GsFe\Y#4]QnS1&"t$[AsBo" +0NE_(L,2_ZW=gt^C1/H"Qu"W2bU@\#@&a?.%;2I>;^GsFe\Y#$0\L2CQr39$_,L`<)U:k[VFnpkWeHk& +@C"Ff0VdE'JuKAV24TdA9U1\_<=:T+_dofV@7If."B-V6C,dFaQms7HWYT25L<3K6_MiM:$)U9OK+#& +OC>HFYV;j-pYjf_,S;i.D\si),djr'/E7i)AZqL +]1ObWJuOJ\;M(k\18JYn-=iK[2(Zs"GElR9"B,bBU]aUBA4Y7f9ZSp@C06j#mOH%P$)SWd8-u.caH3EU +R?1j_eZ^a&gf8s*&kk0QOV1?QO7Crrb$?<\.`#*%)(EC$#oQXK0G?38[Mfon +Pe0OAZ/-"KQs_G&$)fkOWV;=5pM/^/kVPHBRn)*oU6-7Crrb$?<\.`#*% +)(EC$#oQXK0G?38[MfonPe0OAZ/-"KQs_G&$)fkOWV +;=5pMZ/-!XELRl5,P4*U,Zo9M%:Ie)RAamM0=rqtFk[V*pX +amNtR-@!6-Z7r'/IJG*qU%D!p\/lTG#&&S/Be7CQTuusV+[03IJ;Q=cge\Y#LHjO=l7_,Q&;^IfG3$T`:.UedF +2L`S_gU:r4DJB&(^kNLM:@-]BCVm9r."urPha)J3_T_Ls$"RI6\Z5j3-(&F,,:/^,@JZrU?+P.>b*C5P +PPn[q`X=Ksqtlkl*'&%McC?m62JecI,?pNAShGXGbU@\#@&a?.%;2I>;^G+%e]&[`fs524q"XX`rVO4< +\siSCc^m:JI=9Al/[&9]U8+KYnbS5t#7m,t]l;rI.K1lkKpglp'$.qKE`WRE8hee77S5@7_F:<4ET>($ +T>p<`l"P>!_83s.bOG"?j(c,Qp$1(Kk0.Ppo<,^n3dg_l'HboC9^mE9A77s9OK+#&OC>HFYV;j- +pYjf_,S;i.D\si),djr'/E7i)AZqL]1ObWJuOJ\ +;M(k\18JYn-=iK[2(Zs"GElR9"B,bBU]aUBA4Y7f9ZSp@C06j#mOH%P$)SWd8-u.caH3EUR?1j_eZ^a& +gf8s*&kk0QOV1?QO7Crrb$?<\.`#*%)(EC$#oQXK0G?38[MfonPe0OAZ/-"KQs_G&$)fkOWV;=5pM7Crrb$?<\.`#*%)(EC$#oQXK0G?38[MfonPe0OAZ/ +-"KQs_G&$)fkOWV;=5pM/^/kVPHBRn)*oU6 +- +;^GsFe\Y#$0\L2CQr39$_,L`<)U:k[VFnpkWeHk&@C"Ff0VdE'JuKAV24TdA9U1\_<=:T+_dofV@7If. +"B-V6C,dFaQms7HWYT25L<3K6_MiM:$)UgJUur8USFUGQE=B#:A-:U +AqCQ[JQ`Gf(GAk.V*)2qqn/niO/oN$/SbqY%&h7H\Vg!BAq>A%@S +jo]rQ$aZ0MnPUE0(!FYJ5i(8`5=`A2\`Crq)]g%QKt5^qef-\33nCB81YKCar]&`ih4&PLQ7Z=lmR*pF +GP;J^5*5PE^O=dTMe0*G3"Qtd`O +1Wiu4eGA_Z$Zdj8PsD/!pqnC^[VQg'+5ah_Iof]QCOc=3$T#WjKj^n`;[m8G7_,RN/Bf(MPlRYlET>($ +T>p<`l'_BD@p><,ShC4F-(&Ei$T#q2%:io0\>TN_eZlBaIJ`_4?'Q?[bQQ6.JuKAV23Z=%E<%Z=I%$RfSW=hl^ +OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I +%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'H +O/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2 +L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6 +Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfS +W=hl^OG+S6Xp/Vmb`>!Fm\?b>Mb;'PWeHk&@C#P\IGXPVb>4]l3S?RYc_!H!_tNHM1YJ2/CcjoZdg\/hC:Sj2R)? +]`n]HbLHb((*#DZCpFR8qr;dBGAl&$.aM&II0^QC\mgi)B(YUi'?o!TgOUXq`af5K2WKHakigd[b3$q!)k22C*::/N,hu%q&O(-Mb:Ke +We5qVXl&&"IJWTWaN4!\P4;CBQ7Q1D@`;kr[4i7V?O3N$0\&E(AsBnGaMQ/@;$e%4ouN7?6S6hQbN +frpZV]E"_A^V>%eWS_dZbh94/_djkB_G'HH+daaqW@cDVWnB +e(?fH4j=JlGg5jLn8RE&\UF/9eT9lsC#&<_]C%_-q!\t>f"t,MHm+k,_,LM>Qr39d+:C!G73u2TWafOK +qtBEHinMFHUSFT2p[11elIQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m? +a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d +=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$ +0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G +73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b +8CbRNa=u +Qr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(S +C*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i +.`#'d=dW(SC*3m?a$h<$WP8B.lc5&.TDC_eAqK1R_,L`<)T]dSE`W?GQoQcK!DW7.OX$EfSO5b3;r>j: +9^mEQQn#pO"'M!gmV3jP1WMp1E2am@Gd+6LX-@$#F'En'&58?sWdu_F.!IP1Kpgtq&r9LW7nU^HsF"AD4$.*m`c^[":mX0)a0Xlu'$c,dKVAeZP(YW5F]Qh!>MhO0W&#oahe`dn4WHEE/ +Dq7?\[VT*Y&"u=e8hedt\)-t%g\[$J0@RZe=Rbd&T/$Q)V^=^c4O*(cXeAj=E +JuOK/\#qir*aQ/:S;^GP5CZY@Y +0NE_(&knQuWo8o$@&a>;F0.Q3#c:3JjOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk^[f\"itaN3o3a4l"b'mfiOf*sXbkic^5 +eO,*%[JN4hEq8;54aZmes0hei.TB;ojcrp$I2(,*`lH."orilDo:Q%6s(M/385j4:J7HW[[HdfND;$gj +mbHLY/RQ6I0:IHP*')I#ff:lGeF!F<_Z\B2m^o[@\80^!o'a8=pQdJ'V),UXT^#Aq>0A.@KfR*+DnfSp>ebad1G-b/m67['fZ;-),LSA\ +cFibo468c9USGa,-$hRDN6clRq8u](IdXc0g3mX9[RVEf[)Db#P>@JG:s.<_2r/T1D7@dZ1#-fm/\Sj2 +c)YEQjcJ>Dj,Fp?N67*>NqW5AQ`lQrgi8+'3CJ_24LNtOBiFVhnu,m#/LOC7O6T=r2E!I;mbER=HDl,4 +L(,(ocMjHh&9#;YAuQk84ZsSC&ESJMSFST!5i;P.GF'mcf"?>(nDP=)][:ZO#9?Jhj[r.bAA#Rh4'.:p_o%7H5Q%)o'6_$8aElX1\jh[j? +9.>36gX`[.92JCZ;l^3H*TV +AcC$&bHYj7Y4<.8GOOCoX1-o.>Np:rf<=p=(]k-W&,VS:9'NBtkKa%VYC?/R^\r#iNZAKC/GS07IlNVs +d$WsK(\sE$Qtt9SRE<@FR"ZE`Dr8:7W4q+P9&iQWZ"1t>_hSc3qf&]_qtKPt<0?`i$ZhPd$Q%*X2fI`r +NTD\Bi8A"R>/"[Yn`T#;FJ['D/a_fe\`8]3"pP8QHK_'nTR-M&\,/CWU+AI2h+(>'nq=1A6^3KF07N)T +G@pEP>F9+?B#71o^!t`gq;(Lgp[m_!m66h.PXR`^p?Y$Dq9P5hNAo^-B>,?ucCDH0VON&+S)6'*>*u(L +#LW\Z%Q(@13cq;_\j#oIK5E`H/)$"(4%b%\q9F2Q%=-9k4VM.uWCp9`0:+B%D;3Y0"0'4%pY8]]2O+Mu +dF$?o#7iFk[]DSuT#o:5p^()1P/4CWLO80OB1i-dl05H)'94/!5%i02njL4J#BBupi.2-Ir8fSua49/? +m^qr%\T?r"J,JV>s(_;%eRlpd,qE#tB_PA/WW)hY5<"&Y5jODYa"5@dIdr?loB20oNOAXKC/>i>KHIBZ +Fm;qO>lF?!Im%;^d.e3fen77->C;#?`PNU1_hJUHc[5,o\(q9eM6`ab3@F`#mID"@?Q"/MH@'ulpkr8A ++d\tjlE4F)-N&a[;ODl(GiOc9C5B'BorC9<7VOuqS+(HS+8k%%1S!s_V+Xmm>hQdT +YEb#W@1H8.?'[0KJ$YXtdubkF-N%VmUIUB2>FI2mcm9MJ)]NYagY`#(L)Y"Ys6tA0Q_VsrqYU/Z\!69N +1M9I.'&CR&]$`06[&EX87sXOhYK7.Ak*p;c]9`UW:B05MmWZo[cC?mn*`m"LItfl\E64Ct>K4_(p[@"s +e^`4(T66PQbY^up.qco^`JZ,A:ADiTjNR$UgK)]as@?+7"2O.Qbrb^H=mel>!b?+DD +Za6tMl#!oc/TqhSu@0JH>o/5JXeI3A22?`M\Y=joM3(GB+mat'[&j[dD2`qJfSB2t8s4.SN]G%>.% +5DcH=8dX1_GH]m-%5^74rqbracfEu6O=j8&XSVr^,tX7F8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>j +OsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jP,.0qJ,fHf4*D#@$p=L\dmoH?rTG8^*BWuE;br[f^A[c[+2']; +55=&gSND&@fVaoY8WoOnX]p"o2:m5M`V7+@^"-qEnDM,YJ,7WR2nf(QWtVXBg*\J[]W\MWRWi)QIf7eu +?etL9hu)UqhgP7T9DbNJ,Y?G'](=3/9+,%5C/q7Ljm'sO-QdWpO__/6p5<;U@^@lbp\1$Or8Sn!\DYn1 +F8+L7?[(=Ke%+W`rpY^6koP9FdIHbsQX>2eeuW#`hgYIaHtl69,\`^=GiFS.78!ZQE;O#BqU_dN\uKl+ +`mqf;^/'4KPKgR3\9_kd?^SV#V(XE'ZD;d=pV6`V3Tbhkqqm`c!0SU!d`;4,=2+7E07NGCDsHcY/tMf& +s7t!3GM`.L079JkqrXAdIc,b^-n^Adf/QO7#9m-*YXlD^\M+/G.U;Kp=Of3ECAb*=L#7r-no +DV_m,ZY#JH.3E)FGe!3/Ie9BWQ$^7>hKu(?Dr/.;q(^cVs8?u]L(,*;>U2n(UiY_=a2_E!NBBMk]qq&Z +rUeQgqU;&P04'&!eQ8)e^\HfH''0/>f9Qs4gn8'H[:#5KHs-PoO%"AGMauXo]JI= +q$#[0BW=ijn\hse?]m-#1fWmDg2c4g.H.tR]qmEL,5nK:I;7ldQ-K%aFQcQepPPmEP)SsobBoM,Bt^c. +mE'[g,Y=.5jfQ#K.UD]GqsV;X>GkVkhg=h*c[=k2TDRF,m^_MP;Jo-'DVX(s\ttDTBMjZTpV-C8`Z6VS +OsEUkY47[6A]pBFn?%]-:%@SVc^t$D+8Ym3huA/kXg^3;m9@-7Vl/_6GMaDX(arJ/NM$-QDh%Z=NV@0> +bh$.AXBDmPRs+5TktWHhlH]L&&tPh^nd[L%_XXhARIXJ,]9,YJ'Y`TD[atj\)Oo+"^6,idQj +TDe]N5Mp_C;Mg_n?f#JMV'k/AcTTN;C&N`=I%gAZ1sVZlmEO6gG?qbmot/S7H/O5YRNH24il1n2Ce9d8 +c+Nf.J,_0Oq9-$Qa"Hc8hg4Uj^]*o0c/4]3Hd%SQFkqkVRq8ad*,?AAIOsEV^,Y=.E8Wk>j +OsK;7np^X\*>L[~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 412.56] CT +[1 0 0 1 0 0] CT +N +-955 -77 M +-955 309.25 L +238.75 309.25 L +238.75 -77 L +cp +clip +GS +0 0 translate +239 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0VD2;/&"3,=H66\aLkX.`[!]Q>pA0jj60F@SRRT'hQa<7c2Ma\JZAhW3].AT?_(W%^S,Uk1I#f4G% +Q<5P^`FL^!,^O#6HZe!o4?SDJX?5J-?k+:7>FL8G'T7$2\7WkB<^:l$pVCZ#a +eGVY!?@(l,ji]%G5*$dMY2_1=??Y"5VI/DRDn_W'IJWBf]:4a>[tM?(]$BuA`hb&)ddQeEp\3QplT)QD +[bEFGkb9/Jpqcf/NgQ*7!\UZ?o#N&Z]^j5+5(n=R`>;T\EJ4a6*qQog7*Lj+qtBFR`=EtM1j/*5%3%3- ++6u.IH.)ZekFXfm:7F0/L)Y#Dl^6RZJ$`O#f?6f?S^hYlE8eYQDnJq3/7-!alDq++f_aZd0@#tA8"F"- +aX&H=Xup^`7*AjtG3gWc?[VC_.orbHl)1/rRoEm812P'q-#^KieucFIRAJVQ5%?d*_:= +^V:Vbb]L^$Bc;BW^k(LRj\'X;Q7^]nF%pY-]tM,0pDj]ipYC%9[khX?d"(QPmXk`TSoK4$?lH@kj$s!o +ehLqDUhssp3Lf-"eq*\"?[1N5rN(J?o=fYMb.Z+`o8L,Z?+]gDpYOZ5D4@-GHg\GBq6"A&I9,*(X]hRg +4*U*lJLX\J\)LG"bftq-9WKb&A"gDb(D0l,>k`EBis"pP8I/6JRWnoWoT*/$?-UUn]tro&T$!WepOJ[dVtc96 +mO38CWg_=lYoMQVhlemlqF\5ClD^](@=#WNcAS_q>'kCpDDG;3I"B>>1X^^Gh,dp2i.2-_^p.CioX!Ct2'I>@ +mN$YTnTJW)[d++&[mC,QHgeYeJ0O-A_c0h8F4L$BJ$5!XQ7].$qAEdlrUnc"k2VLeY23EA?7i"r@63A6 +]6)=?n!>W6Hd#Z:`g=F&I*mP\\`+JHX8%-8cW\5dUGUg[/Jtj#Vtt--G1U#D'a'>\>7OSGe^'))K7d>< +BY84?IJWU3oB3TA=mHESd:0_6GZlK\,[VSC0GU-!F(`rOp[ZK\hu.KF?[ocGiq<:TS3O(#YMT#3Fm4._ +Re0r8@W"3-)RJj4\*E5Bfs>>;0>IE?pi"1Z*=/iUQ5's6&c\pESeU#K,7 +[i*GGe%$9Wcl$\gA^#_%i4J8d0Im1+_AmBLGc0$f@18_Aa=/W;]YhjY!)4WV"6bZm3k_OMtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf +">MtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf">MtL +Es+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z +&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/ +3tQH90foqf">MtLEs+3Z&9@t/3tQH90fr&ck/"mMI[mlNXd2/(p/A0H]36r&9[H0lB4_6TNZ_p=@ZDRC +#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9 +Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e* ++D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jf +NZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p= +@ZDRC#@`]9Ad;e*+D(jfNZ_qhM8hY;37s;`FpDV:Jn]F:<[3uT#A,iFBm3rc,FEWZAi50SG@4GrR!&^H21fOd +R-:W[r'/d66)KtTzzzn/j`;o&RU%^ki^PdKe2D^AdupT7?jN@H4=cV_lM*[pu`An])`P=0J2QCK=M#*'C5ZV*oh1mC1Kl^]dI**jr@gS1RV)4'-t +3\7BC1#YI`j=PXBDmlq!mC&(+i_'H?K(Wg/p_dqD?GP,N@4P;d)!Kg#RN;`*@]Fe,$0d:i*+[,fHK\Y[ +G<@mG#gb]6Vk09Ci`"PqB&$!=&NOehZM, +"s6,9rh0qIr`$:UP%5QJpf]ji`u,IqI44A,Z8s>%scCWkFCO=e:-Bq=;kbRW/7rmA%5jjHEB\fLjVn8L +@pIzzzzzzz!!'fFq$/E;B\*~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 375.6] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 386.25 L +1193.75 386.25 L +1193.75 0 L +cp +clip +GS +0 0 translate +955 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WEF`)L+33nk:%lhB^,*kVYpPa[1B"IB-BX]A(!1!L81)e-OX->_dN;'#oUVCAA29R07?[T^P9ar7 +jf2#*Zg"D +d]RAHJY!!!"J_:>SfMi*BIREV-ZDjFo8#7hl_#s0dM,/!6@WMt=Cp(biI0EV:T/0cN;S?N>Z1c3_9g1kj"hS#C4G^= +\ilK,c%YgB-Q($;-+cAM%f:S72Z:=%+e_>/dFiFS33*=XNpCu;sTHhH[S[V9qNS&,R6!3f_Pbfm.n\T5 +r`20OIoGhBU_Y$J[1LJ:H9*'\nbUnkpL+K_ZD\om=>i<,sSVQ>bIO02j)^9b.%rq)_tr6t1:o#o5"a2Z +-MX]oE?.P!$Ug$IMldIO03VW-TEpI)X"GOODP`,'d$BkaQK?a4'o4l+B0]i;B'naZ,o7ul5PB[bS:?dI +X%jQ'imF0jUt-BgH3kg95$mh9`MC"Q(h?UcHf!!!#B868lZ4!2?lDVDJ>(14h**?SeQd)qr^\(90eh0n +D-'e@qF(ffSV!S2_/CtW9U]6E`3l05JPkn^%Rc@'&e$Q8Dne(3$u<)ci`fWi?&4u[t?Z>7H3r?af*8M`*P-e(_?M[uK++g=t@fBJtj'S?9 +n'#GNZk!!!#BLt+WRDkj)&DXl/O90(PPS2tf8gK;L:7`7,P +_m523GeA%*9iAfFdlGh6+\3]c81"pYB[H?sVgUO2+$mNrW[S%lZ2kdQ(5It,3BYh+_mn*92\hb&Hdg7U +,%!!"PdB@!0Wnojs+nV8je=Dh(DVl-GV$XWCSZ*?$-$:O(Og=tB<8Jc8H<1"%5molbLjSUT5diKPO[Xr +r'o&TrY%]ijA.UH["$F>R*j+nFa>@1ZA*?RL"0G#>t9@p&90EV:T//OS:Psl(ZYHN.[ZIu<@Vg"gN8KQ +enkX$1,)r7`s0Gc_JLBVg@Eq7kfA'nZ]AMT[b\iTReJV=g#KF;JGqQdE4KM>C4!<<,JB5hgoB`=(E3qK +k2@e/NtMn/WsFR=NU;q"(:PioP4&KE=E;U4SWjU3Jq*2UZmHko8*d/nhkF=)c:m_Il#4,*+F*# +X8g_&\KWiC_H;j[Rc!7L&GJHJ?G.p^)a4F,?rC2Ig1M^@t8:+E]t=^>>#rX(;e^p@\"MMmCpJ`f.ac"= +T"]:V32iYLS;0GcK.^S$UINk09lEo'GV6UQEYfpu=[SU(lf#JD^lKf6Wkb(WZZ!<<,:2_d%La^e":9AO +%/*7f/4e?)uddTm.?,(LGP$AsVaH:.0r2"T(g1C#*H[DMXD>s5$p$poj.*MYk!5F/e=0GeEZ_G(YJ=;oF)fX +iW%D2Z&bo:H5GrTQ[aTJj%X?Gj/le]=%\$*!]?2r0h6s!uY(Zn'(k8.*5[dJ8![^N +W\H0\YL:Bsu,DJX-J*mO`:GWGfgn#,2q=AYVQHfhKBlr\bGlL$83(Vd8_4!&9C>$C +K(ar7ZLV!M4F5(EQZr;7*51B7CT(,@a)AY!dNAE;VWq"!Oid(Gl!2D=,ma#g&kUG`?-n-NUk"[@G3P]N +:8S9lLNJ'S?"zzzzzzzJ<\^e!!!!M'2)>mQXm_m7]HfkW^*QkUoH\[h@Z^gW@ER_C'\lO+&Z)]<U<4sMS"]A>++k7.dO +?TJ?5cP0eW^*QkV!8t_.ZgYs#cWP4L\)`i"]A>C<Q%5cP0eW^*T@&ZGi2.YNmu#]iNY;H4i>" +]A>C<Y.tk->dt^m;86(4?'hHk:,#oacKb..Y0Iqg\B +@DW8'hD>U<#%sS4!IZIY.tk->dt^m;86(4?'hHk:,#oacKb..Y0Iqg\B@DW8'hD>U<#%sS4!IZIY.tk->dt^m;86(4?'hHk:,#oac" +a-iK?G64)iC!/Q$F9fk(Bm]:+B8SCr;#sI\bYAm87=,#il5h\h#@6C^OQ:Nr>Y@?j(Gt*W@F"0G=8GrWX@0R.EpSqGMB@\>mJ#WDJieG. +ARNS.ZgZS7&iMP$9>(`R(730^,7bBrK_td+VLTN5cWZQW<0%K85oJ`_VEQppX`d-]\Qap*\@+,;KK/T- +\Y]YTBuu]W^*T@&ZGi2.YL`f+$@nEjEJQ:`?X]m'P8@LWsYZ&5cWZQW<0%K:mf^hmu'[MAgopE$Da[eW +nn>e!i[YGR+$r]ManIn.6ad>UoH\[h@Z^gW@C`3du*UP7hfPa5-#$dW^*T@&ZGi2.YNm5KioRP4e!?(A +gopE$Da[eWnn>e!i[Z&O=Gre+bW"J)AHd45-#$dW^*T@&ZGi2.YNm5KioRP4e!?(AgopE$Da[eWnn>e! +i[Z&OK.Uir5W6\6J/WD6"-)=Z`69j$Dcr-OXcHm63sPgQk_s?Z`-6!QK#6N5cP0el5c!:OK(WBWrE&%\ +V`?~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 375.6] CT +[1 0 0 1 0 0] CT +N +-955 0 M +-955 386.25 L +238.75 386.25 L +238.75 0 L +cp +clip +GS +0 0 translate +239 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0VZ#,+i!(G:/PGos4MfgG0HWQ"4OEYY9GEJWpjZ25]%.O(=\kmF^!rr<$zzzzz!'GYG$Z!L!n&Xi% +FSm^(UZR8k__X%S0XY>*R<@c:b@+>Uj-Wmdnlj6^p0RJCO/*;+p?XZ3/Y#q!dI>%]Y\O+LH +@]g8`Js\(,S06r$0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0 +`1.t4$8Eq+2!0L)R&6j2L&YeWB0T.=6&RC;J=%k*SRhllZ20n05pk'J$~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 574.8 -0.6] CT +N +0 1 M +0 310 L +953 310 L +953 1 L +cp +clip +1 GC +N +0 0 953 310 re +f +GR +GS +[0.48 0 0 0.48 574.8 36.84] CT +[1 0 0 1 0 0] CT +N +0 -76.75 M +0 309.5 L +1191.25 309.5 L +1191.25 -76.75 L +cp +clip +GS +0 0 translate +953 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 953 + /ImageType 1 + /DataSource Data + /ImageMatrix [953 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlHSTE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!! +!"LhZ+\G + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.24 36.84] CT +[1 0 0 1 0 0] CT +N +-953 -76.75 M +-953 309.5 L +238.25 309.5 L +238.25 -76.75 L +cp +clip +GS +0 0 translate +239 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzp_ +u9P$uu~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 574.8 -0.12] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 386.5 L +1191.25 386.5 L +1191.25 0.25 L +cp +clip +GS +0 0 translate +953 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 953 + /ImageType 1 + /DataSource Data + /ImageMatrix [953 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlN5TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!!" +tq=u!uW~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.24 -0.12] CT +[1 0 0 1 0 0] CT +N +-953 0.25 M +-953 386.5 L +238.25 386.5 L +238.25 0.25 L +cp +clip +GS +0 0 translate +239 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;! + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 574.8 -0.6] CT +N +0 1 M +0 310 L +953 310 L +953 1 L +cp +clip +0.941 GC +N +0 0 954 311 re +f +GR +GS +[0.6 0 0 0.6 574.8 -0.6] CT +N +0 1 M +0 310 L +953 310 L +953 1 L +cp +clip +1 GC +N +0 0 954 311 re +f +GR +GS +[0.6 0 0 0.6 574.8 -0.6] CT +N +0 1 M +0 310 L +953 310 L +953 1 L +cp +clip +1 GC +N +0 0 954 311 re +f +GR +GS +[0.48 0 0 0.48 574.8 36.36] CT +[1 0 0 1 0 0] CT +N +0 -75.75 M +0 310.5 L +1191.25 310.5 L +1191.25 -75.75 L +cp +clip +GS +0 0 translate +954 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 954 + /ImageType 1 + /DataSource Data + /ImageMatrix [954 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WDePF!rQ"G[K2T/+#PrD(;Q(hc.NuZt&qXo.#@`f$Y+8j[9S,[N6#l[-Y0;&VA;.'g?9Ph\,p'eE:3=qH)7'&0$c^:fW(H$<1#uW]:S])f&WKeS`rIm**bL^cojW2`*_"fa%KeXVDj9Zsc3FbDZCW#;2-kM^`D$qV24W^'PfW[^iqS;3n6bI&jgW3(ee +-Kd-H>VD3_W<-'7W2`*_"fe$_OHaJ49O`61e(S"&r63X8AU(^k,"#63W[^hFJZ9&V2$qV24W^'PfW[^iqS;3n6b6kSZD#M7m@o(AkY*[4k<.V%V<)oTk!`dKj8BS<@:smE4LM%ss +$@''e,"#63W[^hFJZ9&V2L1dDnP7@N +Y*[4k<.V%V<)oTk!`dKj8BS<@@(ER:c?FNC?6Va7"fa%KH\[c'*4OSoFCB]gRUN$<1#uWWH08 +W+sUF;Pn#Z/6B:Zak?"4m,<8I$qV24W^'PfW[^iqS;3n6b6f:kp+*OFcn<>DI45ZMkHOtnU>GZ`Y)Er1Nrl`0bK;?a'M:fW(H$n<>DHIc9kL,AH`Sp/!p+bg^mK7 +78a;h.Ss,rWX;]_k92;'Z@[js9i"r&F(83E"\BH-W^'PfW[^iqS;3n6b6f:k2?VsD\O!8GO]6n3$<1#u +WWH2FGt?)bXPdMuZ_SJ.)ni6O8JS2(5ZIuf.T"ZcLSRVe$6E7!L"aQ/1,C^dc&ktVWX;^*TW/&nc&%T9 +?bUpXh2WRA?LH*4b6hK*V5bH'b<4d7"7WI!]A?*b2jV'?e$9#k93HCr +bu?Pj;?a'M:fW(H$n:L(-kdF`qr"5C\-Bq_KfSE3WBe@Uh3o +`JLT[<<]BP:fW(HjqLDAgY8&H>IJbEhP$X:JUrCA@;BZEjia"6h1+sB]"5IZ8!UqM.9R`Z?6-_mj!+Et +I,Vqfa6%?5_OZa.QI^^&WWH08W2`*_F6OrU]m=fs:J\P$Q"aBPMoATU)k5NS;'et:"fa%KR +i*oRY\gfculu8[K:fW(H$<1#uW]HYHK' +B[i"TTGe$'r\,t<<]BP:t;'gl."54M%_6`-77lg-e#@&(X!e]Zgf9$4EB=q_X7*s_0N&d6_estW!ZL( +TW/&n'rYlm6W0dm"V3-L7$rsp*^(ERj0#PfjAX*?$\.6o:K;+/G%aj6'30U`$<1#uWWH2FGt?)bXPdMu +Zdm[Z=8?Z;af'Tfg8aMdVuS3F!`]u6W^'PfWg`O@P\F::_ocRA9Y@c-bP,mU/ul0+4#G+?6rfn[!`]u6 +W^'R**g/91.sP0!%glO*]=PR'*0nEl4i_39[jjHrm_Oa[5ZIuf.Ss,rWocDhAQi.(Yd1DGFt_1TFaJZ9&VZ]<1h4S$S,0WO&B:+kj5J_R4e'!0) +%A2@S%CniY'30U`$<1#uWWH2FGt?)bXPdMuSHomLaX^Y]4L[HAG3,H_W]&98%8Kk+W[^hFJZ9&V2o)8G8+-gc@?aToXBQ0[q:fW(H$<1#uW]:S]R?eRMXDEWK>EidV@ZgT1^8aA6 +2H?O/U!_dGWX)T>W2`*_"fe$_OHaH^_8FE-jkd*U8]a]MQ2G0";T;^HgnE-:-`RYV'r\,t<<]Cko8o2N +=i&"uVQVZ[Y?\VYNn8k5DHa2d<5.IlC.(-?]HiIE<>DI45ZIufRe)W*Bj&9ib,2/JojZbq\?K[mNjlKh +K:k:C=B:fW(H$<1#uW]:S]R?eRMXK8tL5)PeY7^%8dXm1.d7UD;T +7TBVl.Ss,rWocDhAQi.(Yd1\EF7S+i%]>W]W\/L/j?'>B<)oTk!`]u6Cj +RNkDh$rn%@W^'PfWg`O@P\F::_ogQ6g/j&.MkfRhI(W^*<(Yo=WWH08W2`*_EupK"/Lu'JffE]V7L/'3c<<]BP:fW(H +juao#>#t-sZ?,X2l>W$X?Sr:i4#BL.5ZIuf.Ss,rWocDhAQi.(Yd/?>^IJZ9&V2u$L"M5ig@"fa%KeXVDj9Zmd2YAEW5*Ng$C=3W\c +]::"#'30U`$<1#uW]:S]R?eRC?eK+a:JT'FC(/agTuW"i5ZIuf.T"ZcLSRVe$6E5+5#eq3HYi]HVu52@ +@>tq#JZ9&Vn<>DHIc9kL,AH`Sp)]u)KX$%mn1s4iHn<>DHIc9kL,AVD%'p?'VhPEV2fK8G>< +lQn:j8_KeW%o-(-W[^hFJZ@ip,\:.[W,Q=WCtZ+-d@q!b0!&[?* +n:ZVW>P)&?L1;]gr]L.;bS)G(ldLB]-+!`]u6W^'R**g/91.o\AX?b1>.H/li]7(@70 +lID=jb:sOZ94ZIDg.4:^3)LEhW^'PfWg`O@P\F9oWbM:kc'ufo_h[3jVPY]bU#hYslQn;(,]/&P)P!`5 +<)oTk!`dKj8BS<@:t;p80eng'Olr%RnSl00-Hi4Tma0l[n<>DHIc9kL,AVI.]j`fU12gHYFQHSf; +]gO4EUbV;aaM)DJW2`*_"fe$_OHaH^TrMh;]tM+(bKS1(iAE@hrU54PDAa<03HO?,*1uju^G7)i*PLj!GkWYuJL.Ss,rWocDhAQi-5C0D+=HKqK=hgYGUJ,)`A>?glAe##jYq^bpX.,8[j +5<^eVdFm3`@)7H3-[GLGLVqb\o7+P%&(aHb4=8>Q%LnJ!o8fmY<)oTk3F1(tl04=mOfMQec?i?aAVI.] +jk[Om5PE`>Ko,PT#9J-cjd22*R:BELrqY_!HK_&HhgK]\AnGjqlpj;:dC6mhm0a^_)#PIpM2*3pIZro> +LPF]dGt(d;W[^iqS1*X9IJ]<]=gIh2N>jelht^_^9ZmcC:!80AKAlS)q9OPFLPNYBo#m3nPadtWhRpC, +_1Gt)Fqnr+EofpgCp1MB8f\0))@o-rEf`,54#CtS22A-15ZIufRStIpI/%XtJ-Q8_AnGXMT0D)^f!]/5 +Q9"BZ[&d*AZs6@nQCi[+q%m@^iIM4T\GuR*f(grNEuNIr&r&GgA'XLX5[p?i@"MK6N6/ ++EMg#R\(T=H)d;T<)oTk!`dJoP!8K]1W`q+n%JI%R"s"[5@_TE"iP7j!0O +Z,[p_ieoJX^#s17]bWfdnF1CV#ONQC60@a)1_3A>kHAVA:fW(H$=@KJ)oJ45;G"bi.DQC?+[PR"3q&kK$<=9C#riGl;';#\>j%<n@LW +d7Ikn#Vm2`;L^bb@]8Q&<>DI45ZMmf8V`%T_XndZk2_NHc->"PD_LiT(=/nZX6Xn-WO&q[l;N,o45VL_ +Cl^g[q"!OiM]_rrk$JnJcPJ4(GAIc`$26P3HXr&3W[h%K +TW/&nc'9R!C0Ie?mmRPHeLXh0B[PteS!1=7"YDpIr:8#aQb5M'`!i]@+MsoaSna:iWWQ4#TW/&nc']q" +)&Zj1L'P09Eq7KS`l?"6/6MDqCTiHLn(G#pN696YOc(.h60=n9Rd&!Q7p@iL<)oTk!`dL5-S>6ihk5GiH?mrU0IUGSlq=^]4;1VPaC*gY`&1GH8L4)]jUT`LeuF +f?f>gC)3bJ5ZIufRStL!kibS:_`b1GQO2D\q'SE&8TSroiQ_?CJ,JgmB?l1b%idQJheYo6,VUQ-"IX%u +Uab`YBV;@SU8e8p'rYkBOgOWG,[lq+R@-'Xl;Utl>KO&"&kpIUMNo2&*DO6a8H[rqaBZV/9kB)L_c9D7 +<>DI45ZMm"UjMk(/\^h!GBZ"7I%DI45ZMkHOtnU>GZ`Yo:t6OW93LW.TW/&n +'r\,tDI45ZIufRe)W*Bj&9ib/T4:k"%OoRW`?a%KD,>c;b%J.T"Zc +LSRVe$6E5+5#aC0dRmLq2LiG[U#HNh)]\%@!`]u6C#t-sZLOWg3KhrAQ$55S''H7r<>DHIc9kL,AH`Sp)]u)K,AFiUmrF"*c;b%J.T"ZcLSRVe$6E5+5#aC0 +;D[[d&E@"qTW/&nc$;k&[&i1oA`H-XF!NVdHY,Ijg#t-sZLOWg3KblA;O&7@oANqSTW/&nc$;k&[&i1oA`H-XF!Oba +X"$1fb2?N)TW/&nc$;k&[&i1oA`H-XF!Oa>W6,U0HZ6,;:fW(Hjuao#>#t-sZLOWg3KbjolS>uur#t-sZLOWg3Kbl1?T!b.CQ?/E(9"5uGZ`Yo:t6OW'cO?WIYUB;?-N[$JZ@ip +,\:.[n?B4gTrL,9.T!NrqKFqc''H7rFr%apnk*S>uur2$A%U3 +q=Ee4\3JHJ!Z-arC[Is-F$g4(K_gTBb_loh4HtDADL%Y?p&6 +do5[u0J+eoYp`H!*(^mDWfUuGL=r9:\Z.npF*/hCGk'd$_Y!Km[;4BKLPJN/&0jll>IJ>spIUa<'t@?[ +W+q?g$<1$HVJ>J$Tu":>5ZMmf8IPDJH@(!s[tKULWMsUXSiu37#h-'\(Q/>pc&ktVWX;m0NH^-^kGeS` +bpehLo^jt!2]k&#,lE+5FL7gBm]NFdm"YH"k#W]=A5WMuj1-Vd?/9[%iZVI]o`('U_,,H1`E +'g,OArsfL!#t-sZLOWg3KbjgrB*iCG(%2/bQ5 +nkh8_W+sUF;Pn"/%kAS@WgY03!`^!+<&Hm92Gt?)b +XPdMuj$Qgf,ZUM;6<$t@4OkEO1P0W[^iqS;3n6b6f:k2Ek(t7oBMj.T!OS3d,15GZ`Yo:t6OW'r\,t<udrZTu#Ad*p9O`%nW1T'g/R6 +I&Le?U^C($TW5lqW([f.FYSSLR!i0#t-sZLOWg3KbjgC;Z+WS?(d7,\:.[n?B4gTrL,9.Ss,rWX@I[236!BT:@oC +c$;k&[&i1oA`H-XF!ObYWWH08;QtjeXVDj9Zmba<]3-@2+LOJW[^hF+oA8s]EXr(-_.NH)BB>[ +R?eRMXDE9_C6#(s<)oTkK!21s52kj?EqWKj$6G?c/Lu'Jg%\Co*(^mDW^'PfVc@MZ_V*17%B!4tbX5:* +AH`Sp)]u)K,H1`E'r\-oK;pqZ_Dqj$o>X-qVI]oA.iTS1RNnd5<>DI4OJf,O5N/-`jIE-J3T2,1e$=R\ +Q&Q;Sc&ktVWX;^*aD4f5PB1'23p%S7e$=R\Q&Q;Sc&ktVWX;^*LnSR#>;]#"B@Gg"L8rOD8BS<@i*g3W +6WJ1Qn<>D7.>4WEQek\@tDI4ck'4CS=HX0g:8B,VTV>)VI]oA.iTS1RNnd5<>DI4cuq45UpI)ni8%LSRVe$6E5+5#aC0;?a'M:fW(Hhl-Qhk<$TQRFYCLD^cM:%jrjUZ@[js +N=T%6O]6n3$<1#uC0C3omn!fs?Z^RL*IY@E6U,";NGZ`Yo:t6OW'r\,t<<]BPPZBA]o%@c,/iQ,;3(9=,W+XGJ]mB?d +ac4cudj$(^pYL?PVI]oPJeY.uNhd"]n<>FZ"WgfV*SYGF7;(l7gf[A2'3&m;kP,HZb6g?7.8\P78JS2(5ZIuf.b%AhhYGiPKUu\2G!AaRH(p=ZNjc@'KlK%3/K[0(38?8C0JG:= +F8GOnE/-#L[`n85R$JJ3#Z@=q3Kbjg\7WMuk4$&WH- +r%O-L^$b/ib?.Tk*(^mDW^'PfW[^D=Fl"LE;h".\2?O$`a_,oI-RY,XCWWqG1,?^A$kFPp=<+!63Kbjg +DI45ZIuf +R]AYeag<%0FED?U]Xde\Y99/DI45ZIufRSuo'Km+n)E8\OUbfm%bVqTkYP/Hn)R$JJs +=;7]KNhd"]n<>DHI:,+=*HhZq[^@QJkG;nun<>DI45ZMkHOtnU>GZ`Yo:t6OW'r\,t<<]BP:t;(kUeJr=*`b0_<&I0D"fa%Kn%oj@CC6#(s<)oTk!`]u6Cn<>DI45ZMkHP'T*BpeXtAWocjP +5ZIuf.Ss,rWocDh>mp]5!9?GsK/3K-zzzzzzzzzzz!!!#W0%8:u,9nF7Y?oKM5U(=Dlrh1DR[TZPkN]U +!kH[:5G`7]E+$Y3:r&G8J$jY?eaurL#Z +N.H.HO(`3P9<1Z:%3a=4?[VSP2(h,?`3Z<=!]M-Kq[Vab7^@h9ZK+Gi0HKikSY-+qseMDS;P4`hVfLmm +2'H.Gl%u9&73,p-id0p+uecuqNn@0AhJ#6cj=0>d70mE$=ohIi`1$dlB[ulf86IkDRYCH9!BAl.a\[e, +X%'<%@ps.^4=0<,1^%^C,qTs9L:Oi4>B_^._DhYCo@Bt4Ul)3.7=d/D24S-cQRl5+5IM`hW*7FZSk>OR +'a@o<&p%J*6YEJ5&X^bNm&X%n;/,/Gc=1fhAUkcD;,0KDREc$,KHHD`LSpRWHZa9Gcf\"i6'e=OKhU&)lQU9-Xrqk,`$4\0fS897/ +\N>jgnhq6iD?b@]/,iP&A#7m\T$rOTZm97"bp@$l\I`&O/@XNS$rV,1[O$(Bc_QXj1-AO``.pVNV',/= +q6d.5K1G.331jo#MB4$&V`UI!PZtuJG\$r31-0q=^b4Y)P_aEYFrqF.XfN$Yhg0T +p_BWgc_`-J#1Ru%uGsLcLZ")a24EBLRo#>7aQL3>0,IM$;;A_a;"4=7*@l)T>a;]'1&__^/iJ1PX8FD2 +Z/poGfk.9^;k1F_aK;a+b$BRVqHXm,[%hlSL^d*$.FBS2_*Uf/7? +pKV<;8Y$LtdJt\m3+hf$Q&Veg^9:#MTpV6aK5Q:3PqsV#&[f58&^I[qr$;4O-X$;!NgqJ2`RYMJ0VIbm +R-VoI1Ps+gf$=>WBeZ2c4NK%Lt34X7ue>^:r&q806Co+hYIG(o;HhZq;?a^.C#/Z/S.qXN#*?Thd`J"u +5]=WFG\omuMrNEs3C3WQ\]65e+\n%Yt[]GN6'&d'*if?d9>.*Y%cTY"nDeE6S@p2q.nA>c@>$@MEm^^H +]afLOL#2q,HET>'Z++G8nY#qr\J[liRY?ueUl)4RNLHcJ:A6QDS*^rks5$e*UgpqLbTlp+th&.83hS&s +gqk;uTcTYPQO,kAI?EuR)2%/:pLRIHlIDq09hjSNpoFuOa+!cphu2I<56qe"6asc$dS!6!/.')eo[?UB5.&tD?G3qJV^Lf +*aKZ7>pn7+;"3DL-]$/<8Ie0&jbEl"#:M5(u\JN9GP+SYAR$\/?X092,8nuCCpmJkWpHF:8pt!k#e$?) +=HhN,a0>@3jecuh?E,p[hZfL#L_q.Le(UP6Q2JnoUeQ/)'l4M=liSiaY(W6U.14-:k;0[QhnDV:,)cs+ +C_7==?aX<_AZ.)2#gU`PR&3p'FcVi'CdF#4EoB,%f]RlY*VI`-R>K[R6q''jXa2NrQ94!5=d\TUqQJsL +ue6mTKH7Im52r7ZJFD:LZ:GLCp0\?!kp(hQQB5^^I;u$&H_kW+)b*=K@hf4>uJ&(TrcuX&bLun9a,,WDK('Yr=U#%h0! +(])Ag1bc93*4khgN`gT3kb4;>N%)3pnf9`6PmH;=hfE6QiWAR.Z)mWoril +,-(>Mk1GNZUbK&Pg`>QBjkf2/CcrmmOHFU),SK'/eb8CbMjOIDET>'[a9Z^.$ie*=ET4nCT)>rQ1Ga+Gm+F5QlN*qM@8c2o561`azzz!2Su]! +MNt&`;~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.72 36.36] CT +[1 0 0 1 0 0] CT +N +-954 -75.75 M +-954 310.5 L +237.25 310.5 L +237.25 -75.75 L +cp +clip +GS +0 0 translate +238 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 238 + /ImageType 1 + /DataSource Data + /ImageMatrix [238 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0TIqVjT+33of&KA?9N)T +XT+PFGjo*>;cE&\n])aOok!$+kFK+Xo(C_ECS-%KfntQ^`L8Z"SppP#rr2oenaYqsYJ:#+b>1:oe>!44 +`ES?g:B=;[3p<5>=`6l$%[e&\.:E9BVJ,I^G +f,(pUigtMkm`",AHgeYi5+oX=fBcWa@k`Rl>7l>VeAS4ia8+936VUie\>pR^f!An:9)AP+RQt&LB@u/5""Ho>s(kjQ;uN=gVki +U6R61(*5"ni]\6hjis8R8'e"jbfQ)SrShg#/se*V0728L'_gR2laM-/ioI7+fa"l"R4:5iGAYh'G-CCk +eOoNC%mYSPq/;%GH&LXbd\#qEqP,hs&VNPFT&k?F3O8X>Q3NK0H*1=bSm+J_C\EB2c +IYg.*XJH:8?HqO0=o(ub7+g)An@F_AXEC/GFj3:Z4(TYnb)^h7E +`*,3.csp2tho=&pJUoe%bEa`6LHFkjmZ'Kk/R.dK=m;V0(S>6[EQo=f92!IELCF\%CYpdNVigQ?INKo&]X):>!!l>M5]"(S>6[]tM)gPnZFk013k]ENp,t +*Zi%8j$q"_E>OU]@.b%Wn(bJFmk-f;-lL]'aiFZY7Q+'uZ@S1$%?uffpb.tmN1!9KoilJ8lKRP)HO(KW +aaTJU5fnJ>X?amVPcR-lI[$p!aaTJU5fnJ>X?amVPcR-lI[$p!aaTJU5fnJ>X?amVPcR-lI[$p!aaTJU +5fnJ>XADj;,9nErR'KsLQhD!U:HkChIN71m";UTK:DTU65>"tLA3W[eTO^8Z"tLA3W[e +TO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z +"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tL +A3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tTWT92( +OWI2!/sf4X#k]R;cSn(KmV.91HFP=1:pM/h.o_-&NA'PgVISKeBk_:!DN6s=I.PTe[r/$*W;?5ZcTenH[1*TUQL< +PCJ,bE[^6niM\97,g1!.o4mC1u5J<(^42Ej>(_[etRG':#o]/K%BeZnSXs*i(Q-';tWaeqq,N]kK0J+L +oDEC+knGD_$sT&5"o/S=e,:HpH!M3l36'/+qdNiGk1]mKMFcTbd@@)3e#]MV`#MA7YQ]"3dVSiV!D::P +K7=ph%KW,q.I`K\B7V*!^+n<6X38S^lilfQH]mHs:JIedQ&;-J"AhrUZ*&Y]/RR-.^,GMp8fmG#*'?G-'@f3TtE)muF$[)>&lHp5;]fs5hZI&jo!!#9drW2ZJ&B"~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 574.8 -0.12] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 386.5 L +1191.25 386.5 L +1191.25 0.25 L +cp +clip +GS +0 0 translate +954 76 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 954 + /ImageType 1 + /DataSource Data + /ImageMatrix [954 0 0 76 0 0] + /Height 76 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W?"apb)i&EA&IIG_BTODrP_+ek:kDS3aaluK8e_`B+;.9P,)D&"TRrbg,,J*u#XRhc@[B?<+BE:i +&Ku(,HOu(-[#.6`O"@!CQ69l\f-S,P_`M[Y2m_!*rkE_6\UKd+R@TD`2&,J5g"GV(zzzzzzzzzzzzz=9 +(rLk5/Ea!!"-J(GB*";PR&:XBN$3\$pR'FO]hc++F!Y*W5'ERpg;QD!]<0/$lphA*;[WDdQFA;tsp2Hg +S5iVba?h$31'VKd]XT76XYlWf_HnEnu>mGKn1epUQ]QX>3Hrt2IKg$ +a1Rgpt/5^3/bmfqSGKJb6ST!!$,W_R+-CC=W0$\\)S$*dd.bpYPL&=/f/fR[UN,gX"KS?*f/b:`F*?,s`mHk?5_]?L)cC@Ha/sT$V:7Q!8D@ +Wj.2)QC6E+"'@4PAZN_1O%V9GB4]K.aCnH0fA=$YQrO'8LHg/tT@CG(*GH]9eZi +.U>Y?h[9X#)dqB`$MQcWUbX!!%pEb-X8l3bg<"?bSWRrquB6iJufuY-)6=q!"]KY$&*N(2[Rk)]UkY`" +mRbgc7?(a,V1Kps(ti,R7$29X\"a4$2imp$:5ambnq;Wr&)$ShA]m]tN5+l/TgFT/q,[NAo^u2-[P-rC +7IVIJ7%%lh^;?s(F%c!!)*:Ct%sgHhTF8g12iW5(#>k$+DX=r:.hL2TZuS_o'C#\9-t5)dFm(Z=V4F6D +7eB$B(2@E:'25GjQ6]::+%A8Cp +U+?.@Y[$&?XZ7BP>IFZcRB:Am2Wkn)(mP]K/&5=a>dAp1ZP1l&o>0`F@pe@Ed$lClPJsYM.`KDJjpqkY +[Z5!<<,(W77B(f<&9/e^`4-hk/]/Cr9oIR$t5gCqX\(59E(:36^#lD/De(*ZZ:CY50hXp1ZP1l&o>Pm\ +%Z#,eEU1ceFcZqu6*&%iJ7G!!$ES8cP#^,39CP[_B;_3W+:$o]bl?ikPcG:HnS"feWH3SFL]D#NLclEg +Z5qY?kN1a)D"Y!2*60C&.VulaBr?GB_U6nAjBCCXuIf*$$%_J,]A+*BOU^K"X3)3#L4ZT9_:)DJmpDWD +j4s5^`Ir\$OuK$ig9X74@3'4?5E)Gmd"agbSE9#9SnaNS1"gceeoF@aSN<]6A0qbW[U;4C.>,FcTI> +;JJbfOK"!<<+M\h[%(B%sb2[cNi_I[R8dQcgBi*OaB+&fs!`f%*lKnDp!U[F90XpJ"^`\`p#m]1^;h`2 +Q\5QHXaDFi3)Q!!!#?4tYMOgt^[i-koFH3&*6HFSYLhp@e4LJE=#OqsV:ToYhMI)Gq0IAf!!'f^H2-l;7n8>o`0JgBd*RlV\TI(6\QljqpTA@pHqdLECB4D8J;# +-CSG`<\o7;ID2rB"(p[6jQs#r#QhnFO=rr'suf3a#E)a&TI3+Z:=IU6PQ<<]D6,H1`E'r\,t<<]D6Rrd +$8If"!Rk04\p>d,;"Vr^/Cr6lE7?G)YABlg\aq;R;cgtr33:fW(HmRX?C<<]BP:fW(HmcL%hB&MQrs*] +3*Cp':o^\dF\>9"\*Y2"9>L(,M+SofGm83p5ZWk'FS!`]u6W^'PfWk/*%8ZS`MDnc%e(2[Zdgnq3UmF\ +[R.ISEc:J];dn./[2q0i]36reFVn<>DI45ZN#.8`skmT#5i9NiEFcF[o=5W[^hFJZ9&VFm&b5jcP +Z;m_jn^<>DItNhd"]n<>DItc@&k+TBZ7nmCiZ8/?K&gq9;[!`e'=<.V%V<)oTk!`e&*VWI=7,fO5t>V?'Oj,Y3$*L4OKi]WqM<>DItNhd"]n<>DIt:,+>iZY.T2hgE+UhnJ`$^?2^N:fW(HmRX?C<<]BP:fW(HmW;$-]QeGZcCOSf=oBBWZ_!b[q0i +]36reFVn<>DI45ZN"_,\:.[R+q34OcHI08?+pT"fa%Kn<>DIt/r$/qb6ft/g&.PD4-D'iDI45ZIuf\r.^\e$;:K=#PQ,lAhYI<)oTk!`]u6l7Rge9Zm +bt>hT%KW]Q@n:fW(H$<1#uWk0+21+b#pE+SWrWpWEX5ZIuf.Ss,rWpX=eb-\5]_j/IgWk'FS!`]u6W^' +PfWk+*3.FYRh$s,?:W9TD=$<1#uWWH08W9Q!iUeJr=0jNE0TrL2;.Ss,rWX;^*TW5U@OtnU>`,K;ZLT9 +c5W^'PfW[^hFJZA-I8BS<@&AsdX+0)msW<-'7W2`*_"ff.l+XtmG64?pRJ#MXmU^C($TW/&n'r^CML87 +Md$8Gf=r1U,POt':.JZ9&VDI45ZIuf\r.`rnX!USqn:-1_9Volh/J[OO$Pg]/"8b&F[o=5W[^hFJZ9&VFVdEnJ+ZJD)ar^.ZY( +5QPuDc5=#PQ,lAhYI<)oTk!`]u6l7RgeoCE*qN+9;hT%KW]Q@n:fW(H$<1#uWk0+27fC +!]oV#GC908-[0jNE0TrL2;.Ss,rWX;^*TW5U@P"%SdVb/c1.)"hIf?PL)r>'!mj2U;W^' +PfW[^hFJZA. + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.72 -0.12] CT +[1 0 0 1 0 0] CT +N +-954 0.25 M +-954 386.5 L +237.25 386.5 L +237.25 0.25 L +cp +clip +GS +0 0 translate +238 76 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 238 + /ImageType 1 + /DataSource Data + /ImageMatrix [238 0 0 76 0 0] + /Height 76 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0S0kpbe(kp@f>HkOE\0DBDl!;\Q`Zg[0&3&MOXK9Wr9cu``n\:hPk>]=>j*CG'_.FEFe.F3YK#]>XIX?JW +9!4iJ4=+'*Eh8FE-ok)F'pP5dj9eN);fW$fk:*rnP4OoC6-4CEY40.OJ'a]?4hPpZ;dY)g;DN)c12TiC +D, + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 574.8 187.2] CT +N +0 0 M +0 310 L +953 310 L +953 0 L +cp +clip +1 GC +N +0 0 953 310 re +f +GR +GS +[0.48 0 0 0.48 574.8 224.64] CT +[1 0 0 1 0 0] CT +N +0 -78 M +0 309.5 L +1191.25 309.5 L +1191.25 -78 L +cp +clip +GS +0 0 translate +953 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 953 + /ImageType 1 + /DataSource Data + /ImageMatrix [953 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlHSTE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!! +!"LhZ+\G + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.24 224.64] CT +[1 0 0 1 0 0] CT +N +-953 -78 M +-953 309.5 L +238.25 309.5 L +238.25 -78 L +cp +clip +GS +0 0 translate +239 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzp_ +u9P$uu~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 574.8 187.2] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 387.5 L +1191.25 387.5 L +1191.25 0 L +cp +clip +GS +0 0 translate +953 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 953 + /ImageType 1 + /DataSource Data + /ImageMatrix [953 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlQ&TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!+ +#lt)s+F~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.24 187.2] CT +[1 0 0 1 0 0] CT +N +-953 0 M +-953 387.5 L +238.25 387.5 L +238.25 0 L +cp +clip +GS +0 0 translate +239 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;!=]#/!5bE.WG`:P'EA+5zzzzzzzzzzzz!!'fe!EZk263~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 574.8 187.2] CT +N +0 0 M +0 310 L +953 310 L +953 0 L +cp +clip +0.941 GC +N +0 0 954 311 re +f +GR +GS +[0.6 0 0 0.6 574.8 187.2] CT +N +0 0 M +0 310 L +953 310 L +953 0 L +cp +clip +1 GC +N +0 0 954 311 re +f +GR +GS +[0.6 0 0 0.6 574.8 187.2] CT +N +0 0 M +0 310 L +953 310 L +953 0 L +cp +clip +1 GC +N +0 0 954 311 re +f +GR +GS +[0.48 0 0 0.48 574.8 224.16] CT +[1 0 0 1 0 0] CT +N +0 -77 M +0 310.5 L +1191.25 310.5 L +1191.25 -77 L +cp +clip +GS +0 0 translate +954 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 954 + /ImageType 1 + /DataSource Data + /ImageMatrix [954 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Ws(bSHg;iaHJ++P)IM*j@&0`Z.,e0*;5qF;cWK[iI'Tkq*Td]k.A1>-Hm"0%Ccf +)H$W>g]f3*qC'0(,`!b"<$Gb.CX[=pC+,l1Si% +WnI)%0Xo*?b/T4:k")L;<<]BP:fW(Hm:uF7EhcL\6dj!'1,$c5NJEdY%opl:<.V%V<)oTk!`e"'p8FMV +8D8;l:)K@FiaHEY6WJ1Qn<>DI45h2EJ1+FY8OuAks'*dUeH!&bp:t6OW'r\,t<<]BP:j!#fB$4)O +g#):\[+2OR0JI^m_ofuSWocjP5ZIuf.Ss,rWi^A#< +?p,dkH!3s(IYA(jXDE9_C6#(s<)oTk!`]u6g/nlhn<>DID?-gj) +H:ZcC[*0gOg>cUQj$Qgf,ZUM;!`]u6W^,+d=M-3$6dj8Wjb/2q/KRqN5#aC0;?a'M:fW(H$<6t`d7)SJ +,jfh[:)K@Bk$_i]6WJ1Qn<>DI45h05oc&*R*Fu(]t>_IA-*`b0_<&I0D"fa%K0#/s4P=LR!i0n+6$=9h"+_aa$ +El,WIR"prd%opl:<.V%V<)oTk6IDh@/iDu=Bs6!+_F9h2n?B4gTrL,9.Ss,rWX;^*.!/J61T2@X)GJ=Z +,3'+R<]3-@2+LOJW[^hFJZ9'rXX\\&JVU$*drN=@]UahCN=T%6O]6n3$<1#uW^9"P.USP.+fW_nS1lY3 +`l#s=LT9K-W^'PfW[^hFTnl^foDYcU1URBfY-X97Gi. +A`H-XF!ObYWWH08W2`[qWi8*Tl63.s[%))0\X,F[Cj`0rNhd"]n<>>j?26BpoGXuR6/LLtN/ljko +<&I0D"fa%K,bI:()BqPj=Z%cYKmqX:U)Y`KU8g5:.Ss,rWX;^*-q)U-T+Pp)l.99l +*QHgtl05=PXTo@Xm,,R-@Uc(j4eFla(@HJ9gprns_Muh>A`:"$i(ISpia>=uDt*4PiOe/LSLXR,^D5Z9VE2,ZUM;!`]u6WXsI+/7)uD +B%"rKbqRUQ^OEk>9=\:;[p6OgmEI^g:/29.%CNq[g1`PrkeJ26_3,D1RNnd5<>DI45ZNN*X(HI=i&6dJ +o?B4&VS.'6:Ap0!p[11ck00<=5'c_[kih6XUIY2f)&jN1p_7h0Hs0:&GS0)*&!lQYeJq-qW2`*_"fa%? +>;a#?5MJjm4$+Bng+Y>7h`*STpsDSR]m8e'Oc_=g?bCVn<>D70g2MpBD33M['UPH +meQJ;>>35b^1=<9f9ElelU/ih;:QXX'r\,t<<]BP6s)>4Qddoe3^#W3s4$JV`l>m46Mmt)Ggi?RLW7BT +Rsd0-BfRWh^`;=LiW*!'O]6n3$<1#uW^;A=Zb@Q$_kbK!b;48jPtL^nqJP)u;7fWsQMTM9X'/_3S"#o% +G!V'@/N:;s>a"ZbS7LM>*>'>D%T([q8JS2(5ZIuf.`>B<_=O2a-QO3J4`TILmF,ZKOK6+`@WHLpG"\49 +qtg/dVP]g\b@sbq=[/ONNn=gW+/iq7U^C($TW/&n'r%_n?rTl^Is7(%5Q?+HC[8V$^>$DPIr0G%T:_aC +R&I]!H$k!eE,`&XZAhA8*ec=qA7VLT)ul`t/LQ5hR%l]Egn:Z-Ot':.JZ9&VpRaQndc-nD?s6=b+.2(K=?N:W3Kbjgh.r7SG5XDrNr(_3&#\]g%\Co*(^mDW^'PfW[d);Wn1*.nWD>?[%,).XsG,i +2Ek(t7oBMj.Ss,rWX=$Rl?Ea/OOVI\m.`k@(Hed8I&Le?U^C($TW/&n'lmI-pQ,)4,kWoE-W(>s*`b0_ +<&I0D"fa%K[2(^2<%*Tbn2Ek(t +7oBMj.Ss,rWX=$ORZk4on;u/U[%-2hq-*9lZLOWg3Kbjg[4"Vg?*gf*lCj`0r +Nhd"]n<>E/Jc.f1epdj[r>#&=00"V8t.iTS1RNnd5<>DI45ZJ!V0c)ZTZ"mn`eUSV +8?+pT"fa%KUcFg.PPbm6*`b0_<&I0D +"fa%Kf8[A)SSFDPlM@:Wr-6A`H-XF!ObY +WWH08W2edYC9F.ECreul4AF!jDLRi.@:eIdDI45ZItXn<>?KRc/GLa)tK(IQd^!7I37s_ZLOWg3Kbjg +PSCj`0rNhd"]n<>@9i)X42jpt/GRR$JD0YL;sFZLOWg3Kbjg +o'W>c,U^C($TW/&n'f)^+or+C:P:eLA%h/umbfmH. +>DI45ZNNF +X1ESJ:e-YT>V8fI:A4GV+$P(^LsrYjT1a[F2+LOJW[^hFJZ9&+<6]U'8`-tPS^,.7Ed=c^ +f@+cn<>C0e$TT[aDI0fRgBK,6?cBu\&N>-)Eu8SQ6:Yd3emW]?4l:fW(H$<1$0XBO,3?u4k37kNN'S>>[B%1as.+0(bS +W<-'7W2`*_"gb4fb.mMOUD@iQ:?2M`LR!i01?1q2rnC;/ag!9#kpe!.iTS1RNnd5<>DI45ZIu?WeJ1n`\nl]e3ZsSZCc5`=P*Vt\%3B]B6;Pd]C1-b>ZLOWg3Kbjgn<>DI4ct-30K$g*D*L1@7(T*8X4KN@HW+q?g$<1#u +WWH08\uEiNgldF5Q"g%QG-;_QYd3emW]?4l:fW(H$<1$h\149E:-@mA9ZsUo)ON[hCj`0rNhd"]n +<>@Qqb,F)"%m)8inobcNZ\Og*%opl:<.V%V<)oTk6>';D(Qp\$fMS'&)sDS9ortalTrL,9.Ss,rWX;^* +kb'EWMeeB/*L1?LJY%?HiaHEY6WJ1Qn<>DI4cu)iQjgKHe;43^hlYU&'Yd3emW]?4l:fW(H$<1#= +X"'lb-Tpao9Zp0'0JIiUg%\Co*(^mDW^'PfW[d@?Q$4D9OG=jABt9LH#A+Js)]u)K,H1`E'r\,te +UB_i`<2Sae[/Iee_ofuSWocjP5ZIuf.Ss-=5ugD9hK[[MR?fq]p5=qnXDE9_C6#(s<)oTk!`]uD=(PA! +R>2fJBt6%e%Eoko2Ek(t7oBMj.Ss,rWX>q0jjM9e4RB"?b8kA.@.ph1nY)sCOt':.JZ9&V@$TobKJ'r6UNXc1G/#N0JKa.Fn>4rc0if_ +glj&eo3G\<0>NG#*(^mDW^'PfW[dr(n1^WCX\O)i7un_YhnMD"[k4K=B@!0'4.JBVmUi#/A$c%g/bc9m +7@8S][>X(^*%,oN,ZUM;!`]u6C@ALT?4Hsob,3nM;9S(ljQ'j$J,eKFLKdQtiCi,>/]QgfR$N*r3IiFc@%59K6JePaVOt':.JZ9&V[D]!p +e'W6[04):^?B*43Vra*i:QCHq*^+hVn!1/_Z,_nI\+l;XQ>J<0lZ$Am7oBMj.Ss,rWb-Jl&u@4(e/bIK +Ocbd>HhUjZG]@nMG'\HqHoaAPr:K7&B(t2rP>\B%)/Zlf.Q)IYd]:tRhkd$#nB`iS9o'6M'r\,t<<]Ck +i4HbX[N2b:^6Zt([5sjRbMc))*^$BL]!SgYmXP66q%hG"ZS/"^4M"M'bgR]c8?+pT"fa%KeGqg:en_dj +:/=[;5IJ,.Yu1PO*N/!X*BJ:UI!iT[=.,A6n]E-q:@.+6W^'PfW[^iQ=,b;$BXjfi0/(Z$WDkA??YqqX +LTf"Jc#('87[TJh-750u"4OP=/g-q*M;9.C'r\,t<<]DrUrfRZ-"P8CZ(nb!QE.4$R%0\Jh%F>g^:q=\ +WMsM0+OdV_j1"Sf@."IOG&np/TW/&n'r]iM7dme$gQO(R9[!&!o+McN[FZmhO,je]%c4a;-[GB&-DR?2 +,9nH!lg)gp6)J4IQRghO3+?:`)CK]L,ZUM;!`]u6g:cXP\tl>dPA.T(3p52Lg$5JD0KsXE<*s!>ojuJ< +^]4:s-VmqDK1sGq03=I+1Ki`PA<*Gm<)oTk!`ck^ne[OhC/1,&>V7Z^/j=daI)gRh/M$b)Z4+sS3!G]p +W<-'7W2`,5DX@#-r)>Q:+XtmG_J^sJ^hh'/nIc_&.Ss,rWX;_m=<#kKV5cNd8XYkZ?<^Pb_JI0.nIc_& +.Ss,rWX;_m9RBQb;:V1lUeJr=gK0]!0O+$c]L7LM$<1#uWWH0^d.U7mWG9^G;Pn#Z"tRm4gbTJ]94USn +5ZIuf.T!NYS\1naWocDhAQi,J]Wqsi^%$'S7#%KE<)oTk!`e"'iU7jceXVDj9Zmc;=-eaupV=]:&fHi^ +:fW(H$<5hakJ+#m$2'lDL"3J,"#63 +W[^hFJZ:fqlLg??2XA4W^c<<]BP:plR+J(9@Tjuao#>#s:$AJpjskM^ZB$qV24 +W^'PfW]&DFDrjpJS;3n6b6i*g/FQcj+V2UdA4W^c<<]BP:pmRb^,o9DS;3n6b6mX>/D$-PKSSS61*fkm +WWH08W7jM_5P_.U3=e]LQ5)G_=U+#sGrT+S6W+)f.Ss,rWnU[h8U&30*fcT.OApe2$<1"R +>h)f&>[+_

UWUQ"6Bj&9$AJpk`*&!EtW<-'7W2`[e +W[V8@[6Ra@U.\ZBQPUDQQBmh[]I216Jn/F#5;7-c1.P4GW[^hFTdWpirU*gQW[Ugkh:U@>IJS%m?f19Q +%NIG>`o\'2B3J>4*A#0Yf+LhEk")L;<<]BP'V]@-:G`f\C:Y*N:RsO(1c0=Op?euaXd,0:;Q2K,0X/4& +h!G#6b6l+=V81]IitQ$aU^C($TW1?Ve/;LY:qOYBFgE' +%a:Gpb`PkUWX;^*Lqd[?QXGffC?j$,Y-5%d\ohe"AYjQCV,@Wkn:_5dRf*tuAHhnG;TOEW;ij#&.Ss,r +WX=kic0-*XjAiOJ6IMo9YD.>thS"9\m_H32ON,+7]QYsd'P*6"$<1#uW^=p0Z[5Frk,K:r-bY$0rnu_m +`5G'%qs<02WDpHf%ZQI-WW*WU9;S8)m99c)@/N<7MHL +Uc32HBR!g@1M=uiq^Y@`$J?CMYn,'KK$9?VJZ9&Vq,,sjaLSRVe$CS#tVk6GHo5M?1L85/UY(k[<<]BPjL)#J;)ua:!^kVI]oA/,Pbt +r:F_om],Ck8JS2(5ZNM#g"c.gcFo2.AH4="KADI4OFsPnI$,i^D#*$V?`<>DI4i4B2STkGE!COApe2$<1$p +=>f9:EoOE?c?Vt'[&i1oA_WKBJN6M+F+U1p<)oTkK"J%.^;alH];k\i+XtmGJqoWY5Y_OGS5mf9Ot':. +JZ9'kWoiA(d>iTSNIlq]9ZmbaGZ`XZ\UV"h0!"h< +;?a'M:fY>)eVH]V`)"p>GY#uaXPdMu]A\*Ec>-G#8?+pT"fa&^W#F9dS2dJd(Cs**Q5)N`CmQ+PbW2ik +'hK.=W^'Pf(KL'Mjo6@3W#?H_8XYkZ4KNAn]Ulb+?.C6a.Ss,rWX@E\Fo;/2fV+c4>p?6L<>DI4OE&ef8*)#fW\O(+P\F::_oh\kjhRD`WrA)ZJZ9&V1$kSL,qTb6n[c91qp4gVC1t&BVcJCg`:!W[B&lTW/&n'sah(loU+c +p/Q2$-f*G-3.:XF?XM^Y"WHAm/5kk@!iP$pZpTh&rZu<>DI4O?qFL8*,DP +-o\nMX(a$F(U&4pL0%W]ECC"fa%K +FjRp/QZ%PHM*mhn!rGAS%]]9;PMm;so>I(Z)grjQ,CKcqsVo^pL0SiPFM]@#hI+ejuf+<<]BP +Ufu=7Q2Jn3YF=cduPh,:dh[;R&L@p_+&/ST?T/XOXrFg_agp[6iV)B(YW +jN,?\7`4J7AnL2AK*Se[Q^3rS`-_L]q%hG"ZS/"fQXbGWRHY+*U90k)Ot':.JZ9&k<%ZP)b#Rf/jr,S; +St<*7C:AXA^3T<^It.L'A&k$tapf@PFD-&L]\-ASQ5*jm=L&m5A$274Ot':.JZ9(JWh-@ZRMtm"Et+QeOU +p/Q2$-bX]j=r;&@0-]"EolV(7PEWCY_X-+1f,#e$Aq,MK=X!cs-&WkJ`EtKF,ZUM;!`]tgDD5Bu\Uc.=O"/r$RTcd,B\9[mCL4?bqm3&l0GcCOIf7un^NNfK-YN3'4d2uCX`Q5'aHYbXH: +(SYZU8?+pT"fa&NWYThY:=rVOg.ZhQ`f5^SX#P1_9_.N8S7cJIX*5&+nYoHbC@@;`rql.KH1IcM5L-X; +:m?SYh1<*bGZ`X`V"cc5JZ>T$U^C($TW3Tle`'jfmS3&Kjuao#>#t-sZKXqr?nPL1<01(1WX;^* +0WrQsr!N>$3=e]LQ5)N`CoUANQjZtqO/*+^#t-sZ<>TabXJ]b5Z.cc.Ss-= +VlG]u9sCgio8o2N=i&"uAk[/pHE*DYW<-'7W2e3Ce\]'/8Mf_6juao#>#t-sZ5F<n<>E*Hb,F).ql2Xe8XYkZ4KPUlV?8VSc&ktVWX;^*kb]i) +nkRh=c$;k&[&i1oA\G;eR&Oh\U^C($TW1?!:sjWYQ/ +@:(nT+XtmGJqoWa`SuYs<&I0D"fa%K&FN66!I?T%!i.FYSSLR!"[kCK&$k")L;<<]BPZs/em\J+>1 +juao#>#t-sZ6&Zs0N6LA8JS2(5ZNNHW\Q&JLjdgDLSRVe$6E7!W?*[*:K;H]n<>F5fb.EaWLjdgD +LSRVe$6E7!W?.Y.TW1#8.Ss,rWX:ghja(p;@p_+V+XtmGJqoWaX4r2nWocjP5ZIuf.dTus;7S4DX6)Mi +AQi.(Yd1ES2)%tZ*(^mDW^'R<<`L$&]>_L!c9kL,AH`SplWrZU1KD\^<.V%V<)mU8Q$4D9!W/lW%nW1T +'g/R6%Wlq^VQ)Y>.Ss,rWX=`dEY48eYcjMeOHaH^_8FFX6RP+bUT->;.Ss,rWX9381+N$WIFXjCUeJr= +*`b0Xg)cW\C6#(s<)oTk_Y1!gaqiW2c$;k&[&i1oAb2_2JXNP#8?+pT"fa%gYKE],/[2FIo8o2N=i&"u +k"H_,Br[UHW<-'7W2f>jGZ`Ys +fkFikW]?4l:fW(H$GTPO<&V_0C/1+;I=6NmIQm\uNq['f8HC2`DQP]XYI2 +4?OSfjqd](>#sQhkt.(iq>Z2)Zei"?YbN[E?b=VrX.,[!XDB%)@anD:[FUh9sLHC:fW(H`jiC]%/[%b7FH4er;?B^g8AA"%eDL*Elp@D$oIk#P>[5J +X,iTCf4Vc+nU46!W]Bi^<0gqm0W.P^`oHabokW0^%1WmIo:Q&!EDNHC"V):ec(=mlK+C0@i5>8@;:u3W +J2po`\[f99DttD7FQgi1-S=bsHRGXfkqY:V2E$lo-:HkT'2"[B'B+QS559s1j6'^[mde'J#QoUfD+Qf0 +M9G#O>+PLCKhjWd2o#Clq]eUkm+]"Q`PB]=b6hOV-5tq/ANonIK+I2OcRPDn!&3#7dcp#\qBphQ2E'F) +cg'e1?XM^$F6B"j,>?8f*%[DmBq,tlC-G0\e\$9+:S0hQ^Y@]) +i.iD'f^'\g>#sQh;4spLOt':.JZ9&V<_lDP@f;uBj$">7=hK$8?+pT +"fa%Kn<>DI45ZMm:UmmjC$O[?CX/e>=PA+0NK11?kk")L;<<]BP +:fW(Hjuao#>#t-sZLOWg3Kbjgn<>DHIc9kL,AH`Sp)]u)K +,H1`E'r\,t<<]Cko8o2N=i&"u`eUSV8?+pT"fa%K>H%oj@CC6#(s<)oTk!`]u6 +Cn +<>DI45ZMkHP'Yc8peXtAWocjP5ZIuf.Ss,rWocDhornnqTrJ/oc&ktVWX;^*TW/&nc$;kF@2n2n<<]Ck +,H1`E'r\,t<<]Cko8qJ>2T1BN$A:1pjp.4>\lO1Gb6fL;[ +J0A)m4GdsZXmeU@,+JHOlHaa0`QD0'bS,i/F"P:'.HptIiLi^A9GJ(@g9M1L5/h6.3Io!7,Nh#$s6YD7 +mgX$-4GhuCq[hdjDhq0)5`^"nZqpu.!2Dd'lG5C^E@h>c;ThbAT4%S'.d:JXb^LBj(T*]s]5bJ_=eb1d +K.7un^FSfoc9ZADMX:5+HE[r:.6qs)>H8uYbsh`\m;OfZ#Ba$0Ltj3Va_DD(j4EH*^\dgQ>"ef,?bE*a +!/5LZopB@!0'R-2bc]3-N:R.YXagVUfo[H`Yd07Nc^Vf5NS*$"$pTtL^LDn[iOk6d?dqWb,_2_E(Q>e( +IcIei)liOaT,XPS9hX`rWI-fo2iqsCkR+$X*/1b<=mUrSN<%b"LN9du_dedI2G!ECR@B\8iP0X9qXrPo@3u;i>e>/2[;C`Jg=f +^i5X,/=e*mU>AJ=CqlaLcf$>5sa/Rd9>D1AQguH3B9 +)V1mIZ67-2CG(Z?))msk@!lOA^(#9S?_LHk\[bZ\.f8A:(EO,#N+mF*`&p`f(9p%*^f2rAurrel!ggo8 +&.n(t`jcC@IC)kn%$ODlC9?B*3'b9C#k?4Cc?h7K$HL?)YXmD=L<0\6aD%fZ'ls3,e1@PBYsouB;rEFW +L2C"^YNP:&jj7un]Sg@rbBfGHgTmEs_E>W@Ei/hVn7opp<`#t*OZF6E>C"[fW&ZTK9`&Veh!HhRC9msk +A`J,XNTeudoiiBRAdt>?Wpd\!/jEXI1AX +Vo>7l_]:p0LBP6VU>r9:,Odep:)/ZUTX/)7a\DeNhJaH7\d)]Mpu9+8[D]",BIrV,3)V#0lQ@pn`DM\e +&'HL%\/55D#N.FYSfE#*lki'HbZV5:#-C-@(GI"n,TGIMJG$4Ym>n%IG"4?l'FmDYLW>'R\"@c"V7,O5 +eq_1s+](qqq_PlB(Q?AYAnZu#0mc'r/>3-OX*UU%G\e^_CJ^@2[tlOCt@^]! +P<^\cjU_F\nDQ&e0/GgDsEi=A0Nn3bcr(7R[H%ZQO1B>#$?G5M"#l?*>nI!U$(+8B=$HV(keG9m$ZUWQ6!\@Ea@$u*'\ +nbiRLts5^F%9e[`6$D&:+rdk\,Gl:(CAq6.R4])K]tqt@:uReZjMJ'Y[T7.k!E[T>cmktce$3Uc1qk_T +s#C7fMM3Eo2#L;H=$&@;?[Ep1L5o('4^n?&dnKNi2p,!ZqoPtJU4eZ2dKqtn8QG`&I:e[]Rae0eHT9K3UAs%N[aBmDA>Bhk`Y`_SJDq3&iu]S%ST3=*X)%nr(>Ya6T%nN/s$TIC8p__L9:uO]1GX*&dJ@tp]qdB]-^T&Qjo<3ZBMb>rG$AX-(Fj&>0-9H\SpqX[1GY""gAmDunZ1c +Jpa,V2#r*tOsfuf9n2]D[GS(HfX%CRL$Ohd*PUsl9" +II'-?b;B+$NBi]cM\V<$t[pq?[-%zzz!$GkQ&*UGn)?~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.72 224.16] CT +[1 0 0 1 0 0] CT +N +-954 -77 M +-954 310.5 L +237.25 310.5 L +237.25 -77 L +cp +clip +GS +0 0 translate +238 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 238 + /ImageType 1 + /DataSource Data + /ImageMatrix [238 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WH\bY_"30jQKSW#a&H*aa,.M%t6';d^_P2O3ZD*#.72?J$QAX&;DC,Q"78u\&Jr!!P,UFB.&V>gi +/@!PrP*3cqPGKdff[XP[kjU[(AoFh6cZ=9Lmg#!21M:=W3W#@khD8Zda9\6-^V@PO+gdr'NfL]+E7*q; +mB(^@D2=AH.+s07o-i:CP4L*TJWfUQfn#=X.+s07o-i:CP4L*TJWfUQfn#=X.+s07o-i:CP4L*TJWfUQ +fn#=X.+s07o-i:CP4L*TJWfUQfn#=X.+s07o-i:CP4L*TJWfUQfn#=X.+s07o-i:CP4L*TJWfUQfn#=X +.+s07o-i:CP4L*TJWfUQfn#=X.+s07o-i:CP4L*TJWfUQfjRK!p\4FV@bSE)]':>'lfmj*m[/t%@=g6, +1++#;EZQMg:i[U'4&Br=,s+qD*[NnTHHH1S)jsG-D,>9/6U5]hEns)JF&6o56U5]hEns)JF&6o56U5]h +Ens)JF&6o56U5]hEns*%3HF1OIt.Kc(C^ia*^4uHpYUImoG3H/5fNGlK"L@4a1//tHnEp3WtetPL;nak'k +?Z,\uLEAADQX4uPbl'!$EKbkEg=V:4[VTZf.pKK#eK*.UghDSn%CBenA:JC>I9cc4aXUaeO,*%BkMkapYUIe +=4_Ir>8kNWb:gWsg9mum;-20$)=Q73OrH?f;9?tR@9V,ZXLh_"j3&sV]V9>rY@%>"`ucfNc/B"-P@8HF +[H,T%2]!3C:L3qaX05j6?pRYk-HYhQmG#*WEQ&YrLDSPlf#\8!Nud'1SiqEqSN?GsB:iosZXIL-]&K*4GJ'+3>bHjAZ'*LU +J\K]?4i+o[D=L.WE8\O!5CL2-[Dl7\>'Q,+qlHLGRG`:bT<;%*[g=f2U#bIorKa*fYb(:U +7<`sb-IQr3`"A#/8hQQNG`UmN>AhgqTpXK5F0\gSjTIYOaeJ*EG'4Wml`>0?0bc";3l.?YDGkN26a/)l +[$m*>baDCXH+@&UC?8]9JTh%bdn`I\/:$^@ +]u\=c;>bqdRmjM\h^-J@Z>H>$c=*D7>c`24#WTd@madC%Wo? +5?_:2.s&ak*Fo)hHK2\A6cjH]TmiFOQ0#fs^nc<,g:k;8lFSG,F&AoaV7N>k:eeK!*US)"\aCVGUbm"* +Fn,%G0/r2$.Cnji>]_fLCYb^2c%6*,-N0:-kDtel*l>m0ldi2-2fB`W*,OVO;KCS\5]]bBh*XGK:ibAZ +5]]aWg;\5,B_@Nm&c]%0#>3::A-Y`PULXU%cD+nT9$X2B#>3::A-Y`PULXU%cD+nT54(:i:]%g*I+n#t +eB=@u5fn;9lp/[o`FV#WA@@(F0Yfmk>AMe +-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2 +""!,-Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2""!,- +Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9 +;6g9Mk>AMeTD"bpVu9p%54*Oul=E^KT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%X +T]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4- +G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY +8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G2UhuoD#]M$c&\=c9T(eaTe!"8')7*S4C[2 +Pak7c%[kB)eUDb!b(n])`@2JlX-E+)7,L]1VpIf7X?:u-b=D;TjCrCo6,FPsrclGgZ!u7`hoUAk#TYLd-2rIu8/\GuR*)bRj9c9'X\?!DciDr3bGE8nfm)FiDW%NRU+6`Ro%)t +bKOieoJ@25Jgrl`\&U=gDT8p?h^N>-c^tqKCJn?0.Ym?hpEp!cntN3Z1Bi9T(ciJ/L-[s[u0LH=e'ihiKEjPl"l4]hA\]Qg^,;! +Jm'gXTOem!'@ie]$XT0Bqt`JRBZ2Ussh(=l +aalfV#YBf3a"jo'_HRac[sr55T0PK5,7u9.o'=d>TeB4T+<-?ZpU9P%QNoXS*7Y:rqY_6Co,%MK1s/_!!'f6r + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 574.8 187.2] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 387.5 L +1191.25 387.5 L +1191.25 0 L +cp +clip +GS +0 0 translate +954 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 954 + /ImageType 1 + /DataSource Data + /ImageMatrix [954 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WIq4lB+33n+PFYD09JW.acH/[)O\YU?'$8G#reP**Rm^k/6rD&OM'SLq]6[Yzzzzzzzzzzzz!!!"Ld#F +!RmBZcm!'n&oWiGX\nerDbZL-#edUG;feC71`J3.r@Yl4%I^$Fnca*-k(AI#krqmO,if@SX2hEI,*g(A +MshHa5cQoAp)OsR#%TD!9%!!!!iR[Z6Sg;LL=h7@DaCtuPJG.<&%r\P-6SX-n"]8MQ(CtARMU\4BUQ^3 +rk!@oWL/mPn_?XLQil-_6]hZ3]W!.6jt7n4@J>\N/,:J]3=CE/tl*Q5p"TSNfZ5blF,\)@`*'\o-m[A6k_V;PO;-3p[4$$Bts<2jic&b[C*Cg,pd_"qk9seaEFe_fbW>_`d% +AhihC3-Veu=gJ,_\%0C%cbL`bDdD/H\u*]eCVMGaES!;ImXfW`33`X[C!<23d&Rl@Cp5pDm]P62fEMcG5KqAC'1\e82Sdbl^$qpMM_ +c<\ZDmVNf[#6h*R7SX&m>;qsE.I2W9m-VR\nk!!'_j,9nH)k09A[It"btf54['G]C*Oh,RER<*37iZ08[1^"o +P[)l2<7BNcGu*mW*XO+uL^B4klTP-t6d4(cKQAXdpK>?fo:auq>g3Bo^CQ^=&nY\dnGFT1ciNXjgS\@? +e3AK(UL!.\!P>AIa3X]r9ic*k'hN48esjiq@JAPN3ch?d:e=m:*fVIc681DY!j2Ak3BV+[/8NN\eTT/U +W@PiK]&:WkZHc^d.AiPL<:V@C5I_p4f7Vt2:JX3`.=,oK?P!!!"L%,Kaq2EFl?=1ark2fI!fc[Yc]A[I +)ZI^@_5AW,:pR[/#BIYDVl/hS4Br!@N#[Va3-@_&DuZ-OpigtE8Xn)H3R%f>teZTW0LNj%FB!!!#)=g` +"pS2kYFLPK_;[SaT0U*>'T_hSc#S8?5Te$Al-PA'lS\80:U,<,l4#Mg2Mj>c(^drIL?[Z*1$gWrNY/1E +/&c$b&\iO3FOVo4buhI1gY:K:g);PTLo+ +amb/19jLi_1H[r:0lDAJ8U32Xs@jB&C,Q%l!&,i@:#.ab9R;E$\$suO=EensUSFTt]tLGG.W>6cZ(p +WlnS-Y\fAbfLr;:q(>%_L2eZ2d'X]oH4g7tBP^3K1bNZ%4oUU;*o5!R)cjeknH\i%el0ZOtW];6`q=72 +,PIeDukqs/(-M@%S"4Zh\Q!!!!iZEC>_pTD2GT;Z?(o9Lt4U,Vi"N4O)ncM6\^N:6>Dq:8hFG37)@>@1 +a0Z0n?aVoN'YG&2D`?YYTsb@@0J7@R7+'gIMdNd]m'KcGT5qlm\KB(CZ0!!!!aA8#YTCL"mB,\Jne0OW +r/_hSaYk2_BrFE_q)Gr5DpK7edoLT7ASRf:>Y!rr<$hi>Fb^ARp,?+Y:)p5&bL!!!%6DJj?/D0=a5rV5 +pCffgDV!!",ad3@lNf+%i^!!!iTs3?k;[Pn$OCnCt5zzzi1lnE!!%P!K@u]ZBBAk7)c[WOl5`G2W2`*_ +"fa%Ke[lBUL2[XE6WF;iH:Lj:W[^hFJZ9&Voe(on+e1pj:fW(HhN:cgWX;^*TW/&n]aGut#j7:3<)oTk +G9g1]WWH08W2`*_mm9P`_DrkP<>DIt*D%!EW^'PfW[^iq]+C-[E"9uW<<]CK7oBMj.Ss,rWX;_u/qg#D +S3ujD#kpfR8"a&H:Lj:W[^hFJZ9&Voe(on9Zmbd4L<-sK^d24%op:fW<-'7W2`*_"fckb6;j[lKb0j/jIrSt,ZUM;!`]u6W^'SQ +6:f8+X:W6)Rlj[CNhd"]n<>DIt>mO#iQ5)`2As8"MhN:cgWX;^*TW/&n]aGu4e$;:GcYW/KeN8pD +:fW(H$<1#uWcoU3AQi+?iL*C]Wd705"fa%K:iLBiJtFA\V,EsuYSj&*(d9eI^Qnq#oo>gT<)oTk!`]u6l9VkZb+>KlF?9As.krg*JUuJZ?7ZF_oo>gT<)oTk +!`]u6l9QmWacL<@Wi(!_mQH!MY$K6'&qAeGVFf*hJU(jPYp`H!4-D'ik%\:E9,;!PccUnso!FEI0$5Q<8f096Xi'-1>^ +@.M8[%*UNCMa`2shN:cgWX;^*TW/&n^L!5,b("(g[-deeo?fL(n(ta)l>OD^[[NY9q.oM0;4sq5,ZUM; +!`]u6W^,+agPhs."-moh)Z~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.72 187.2] CT +[1 0 0 1 0 0] CT +N +-954 0 M +-954 387.5 L +237.25 387.5 L +237.25 0 L +cp +clip +GS +0 0 translate +238 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 238 + /ImageType 1 + /DataSource Data + /ImageMatrix [238 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0R!K"\o'L]d;3=g6G>EsAHH5aZ*0.!1J$OFo`KY61Bzzzzz!!!#7Vu)#UG4kb&F_nd"eXk#%XP1#V +JlR.U?a+:kjpP5WfmLW94e"*,[:f(7X7gAD@+Cjn(Tc=PNoTCp\030m?E_u2O)RHYmOWOr'A<;t>O'HV.U@E_jdb9("-R6-2890.=B/j96k(Pe<[MSrsnSgf*cqP']f-:i]@R$VKh9 +H/W#Ma5R#I^X)*)cZ$W\=7YD1gh34Ah*"NTmN1OQ\NeunMt?D3kMO71P']f-:i]@R$VKh9GsTP8,qg[W +VrWP1-3"!sjhMr~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 574.8 375.6] CT +N +0 0 M +0 309 L +953 309 L +953 0 L +cp +clip +1 GC +N +0 0 953 309 re +f +GR +GS +[0.48 0 0 0.48 574.8 413.04] CT +[1 0 0 1 0 0] CT +N +0 -78 M +0 308.25 L +1191.25 308.25 L +1191.25 -78 L +cp +clip +GS +0 0 translate +953 309 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 953 + /ImageType 1 + /DataSource Data + /ImageMatrix [953 0 0 309 0 0] + /Height 309 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;JH,ZM!5fq/l + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.24 413.04] CT +[1 0 0 1 0 0] CT +N +-953 -78 M +-953 308.25 L +238.25 308.25 L +238.25 -78 L +cp +clip +GS +0 0 translate +239 309 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 309 0 0] + /Height 309 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;!=]#/!5bE.WG`8*TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!!! +#WfDo,d64s~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 574.8 375.6] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 386.25 L +1191.25 386.25 L +1191.25 0 L +cp +clip +GS +0 0 translate +953 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 953 + /ImageType 1 + /DataSource Data + /ImageMatrix [953 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlQ&TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!+ +#lt)s+F~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.24 375.6] CT +[1 0 0 1 0 0] CT +N +-953 0 M +-953 386.25 L +238.25 386.25 L +238.25 0 L +cp +clip +GS +0 0 translate +239 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;!=]#/!5bE.WG`:P'EA+5zzzzzzzzzzzz!!'fe!EZk263~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 574.8 375.6] CT +N +0 0 M +0 309 L +953 309 L +953 0 L +cp +clip +0.941 GC +N +0 0 954 310 re +f +GR +GS +[0.6 0 0 0.6 574.8 375.6] CT +N +0 0 M +0 309 L +953 309 L +953 0 L +cp +clip +1 GC +N +0 0 954 310 re +f +GR +GS +[0.6 0 0 0.6 574.8 375.6] CT +N +0 0 M +0 309 L +953 309 L +953 0 L +cp +clip +1 GC +N +0 0 954 310 re +f +GR +GS +[0.48 0 0 0.48 574.8 412.56] CT +[1 0 0 1 0 0] CT +N +0 -77 M +0 309.25 L +1191.25 309.25 L +1191.25 -77 L +cp +clip +GS +0 0 translate +954 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 954 + /ImageType 1 + /DataSource Data + /ImageMatrix [954 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W@;+3SrV,j^j$3QVHglNr4p`Kj&gE9n&1hB'MRK<'/[?KbJjb3Cip_eh[;:Y8:c?b;Hs4Y>< +rUncB5D9/A24R5E<[Cj*dbVN2rp.=JHmk4l2:AX559Ag"D= +C*_DOdC`TfJlN+XNT=7iGuc0#QAanM6Ek2aP7jrhY\GH81agGA1D\dq)_jQ`scU?$!&?S23MaCjB7K'=tZ7&$c/k%9M2N=_5`W& +b9YDK,hI(a]NGM@%;/Rj<=)]cjB,]WtMKPagFh5X@@4^(X8G'@+B3C-!6F?eOZiSP%QY8CUn-8/-N#.1F^/?"KP"4 +/X&;tO^-'uo4B.J24R5E<[Cj*P&$KH=cKd;C\d\kgN.FWCVPjERGc=VY\H.]'!+^0C*_Cdag2nB?d#d^ +ZX$jBj5]08^R8l*i6=TmC,hCiX@f^3,hFfnZPj-Vb;1q14ZtrB$+lKRiQX]nC,hCiX@f^3,hFfnZPj-S +bI(MjA7T6*[V^BcmWC*_`LDQheSli]=I!:E8?QNeB/L=1QKRGoGOF73k007c&$G#oGd@s_)U>,]WtMKP +agFh5X@=Bu\%K5\puL[UFtHe7RtY8#E$cl5Q!i-5Y%P"M:p$XnEeQS"3pZdSW=u*/tcD1TD'!*"ncRk>CX-%H!gbSDc7WRH.M1jB=1)2[dI%iClkJ.`&-I>g"D=C*_DOdC`TfJlN+XNT=7iGuc0# +QAanM6Ek2aP7jrhY\GH81cU?$!&?S23MaCjB7K' +=tZ7&$c/k%9M2N=_5`W&b9YDK,hI(a]NGM@%;/Rj<=)]cjBVp@7JlT^VP[ILCn.>g#S-Ag(86S4O#f-!3$fCRO6dNT=7qRPtVjWo6WDYU[Jh +O^-'TcYB9,[J]!='g<5]1;TH6@+B3C-!6F?eOZiSP%QZ#/$7oZ]fn;22)dH1pdAB1"E,'-jZ`Y/JGd"#Z_CI;M%mT-4)sF7.hoBA[rEA>H9,lfFapCRO,*6Ek2aP7jrhY\GH81[qtIQae_f9ageU3p\X!QY/-N#.1F^/?"KP"4/X&;tOk-k)-RL1gVG3Pjop))=SO\_^IJ/YI +H[F5'*ZZ7qs0&]4Z1MLLGP?^p%p&SPgKnDu@,SEC.=I:Uoi!9#D2n7EhG24R5E<[Cj*P&$KH=cN&G5Fpm%fsAlFbI;BFlKRPINp"O+al24;k1;'n%1RH'FuI'N +@qK="c0Xc7WRH.M1jB=1)2[dI%iClkJ.`&-I>g"D=C*_DOdC`TfJlN+XNT=7i +Guc0#QAanM6Ek2aP7jrhY\GH81cU?$!&?S23MaC +jB7K'=tZ7&$c/k%9M2N=_5`W&b9YDK,hI(a]NGM@%;/Rj<=)]cjB,]WtMKPagFh5X@@4^(X8G'@+B3C-!6F?eOZiS +P%QY8CUn-8/-N#.1F^/?"KP"4/X&;tO^-'uo4B.J24R5E<[Cj*P&$KH=cEQfja+U(2)R6M2tt1uF=e0= +=U8%;B2X=]$!*&H=seMq,EZd;e>Z@e*?Aum*B8#rdA'3I]Wl7?i6=TmC,hCiX@f^3,hFfnZPj-SbEjhe +8csODIM=seL#`;AVZIs-&9lc/H, +pFrRH\&uUgOcS)>f=@3B3<164[&iA2DArh*_ld:WQIbMi.p#n]\UFAKA&jV3p4%'6'>.J"IIbL@pPTP( +5(*-]HFT993<182Y3"eU_^6]WdeZ\-\b25YSXi93*aS151@hcDI/:At.iZgNbf"^sgP%trnbn@!u/P"SCC@\#1X@f]H_5[X=L*MlTp%9=dFK##p$bq=aCRO,*'$7P? +nXpMr9!Be]WW]AW,Eas63m$']B:"sODr&"Te'lc,StEp.%.IkcjBBQJ:^L/K=I!8oJlT]S8Ol9@Rm6^p +E,]d`T7$$0-?ujbE\1c`W!'.6X@f]H_5cJ(V"K'4k09AKfZ=T*AUl^4,hFfkK1`?t_5[X='!*!(MkmLe +5SdG0(p7G/#F/[sJlN+X-!3$0(.hfTJ1Rj>0jMm=%k>Dq"KN0:9!E'?/<[T1!Ei[\@^q^Y*`R_k$!&?S +Q!i-]=X8,A"1#GB`GdD=4K&C`'!+^0.`&.DZ:O7a#%V^bMr@jZGYe]I-!6F?7B+=`e-mf.`&+fWtMKP/rQO. +AO%#4[MZ/YN!GVUS]n:bCD.HCb]2<(_8+4WW]@8X@f^3\1EfSQ2t,nfj_IW0cj;G<=)\P +=I!:EE*4H//H`;gZGgc7@m$[mWtML+YU[JhiNc#>=pAMWAW,MN`ct;d<[Ck5?r_eY_e"t[[1(.9aqhn& +N:EPRX@f]H_5[X=L_5[X='!+^0C:GgX\shNSrQ8'F[JlN+X +-!6F?eSnY:FY*^$\?a(m/RpoA"KN0:9!Be]Wo:6Sl<4C&EFk'e>/ke`$!&?SQ!dUDaXK'!+^0.`&+fX+neAWGa>!8R9Mi/?WeD>g!rp.`&+fWtML+Y\L]@-EYnc;c?V9F*!I>_R0gA +Ar5SP`ct;d<[Ck5?r_eY_lYHRWDf]U3]bud3cO#mUIUAGHcmEkG-q4tjQ,C%^TVDji6;nV[>aXK'!+^0 +.`&+fX+kusUmE]>8u$THCi"BBH$XeJVl+0PGkegfq=*@\oTmOY+9/'(HKD0A[;-Z4ZtBf.t.2-'E$]j$=tnQ!dUD<=)\8pQA+7QBor! +5(EQZo/3pZ/\U<%\T?*2n]8dB='+!7SqLksi6;nV[>aXK'!+^0.`&+fX+kuCUt;bT9AB6aPq/u?gqJ46 +Z"(hGIH/5#&,5bimGG[nrMfeqro\_;qF@]"J2^+Hf.t?u9!Be]WW]@8X;^LQP>4eFUpZ^NF6b><4$/KE +4o4Lap\+_5[X= +'!+^0C:NFR*?,r5rRC8P_SXgPeB<=f`j9jWhE<\P[-BDq"KN0:9!E'?k9cajSK2WG +YLa_CVshKV$dH.p]VbAjrADgFcX4A3Fi?;X88ELYD`@DDY%<,B9-i+E`e-mf.`&+fWtMKPmcBZ+p%9=d +FK##pC>V=[nT>Wm,9tIbV![!phL>%Va]3h+f.t.2-'E$]j$=tnQ!dUD<=)\8p[Ulp5Fpm%fsAlFbI;BF +lKRPINp"O+al24;k1;'n%1RH'FuI'N@qK="c0X7B+=`e-mf.`&+fWtMKP/rQO.AO%#4[MZ/YN!GVU +S]n:bCD.HCb]2<(_8+4WW]@8X@f^3\1EfSQ2t,nfj_IW0cj;G<=)\P=I!:EE*4H//H`;g +ZGgc7@m$[mWtML+YU[JhiNc#>=pAMWAW,MN`ct;d<[Ck5?r_eY_e"t[[1(.9aqhn&N:EPRX@f]H_5[X= +L_5[X='!+^0C:GgX\shNSrQ8'F[JlN+X-!6F?eSnY:FY*^$ +\?a(m/RpoA"KN0:9!Be]Wo:6Sl<4C&EFk'e>/ke`$!&?SQ!dUDaXK'!+^0.`&+f +X+neAWGa>!8R9Mi/?WeD>g!rp.`&+fWtML+Y\L[!P.Hq[=^9Wh\<\ij<.\*V<[Ck5@+B4!,]t[@ +ZFI3ZE%PQ^W!'.6X@f]H_5cJ"8Es@_A8u1=iEF6G:^L/K=I!8oJlT^"OjpcIaPt>X_mPHnTG"=uYU[Jh +"KR9PPMX$%HZ!`h"`NI*#'dIBE\1c`W!'.6X@f]H_5cJ@UpID6?!Uarj5Ve;44c7'RjF55)<4"/=I!8o +JlN+X%EBccR@3=(l"OpKF"LJ1RjF55)<4"/=I!8oJlN+X%7_\GeS6SV1C8?QN_#F/[sJlN+X-!3#EoXX)&`Pj3#Bg!rp +.`&+fWtML+Y\H-Sl+5I:l`\"Q[YfWeJ^[f#rr2nsT0bQPId=,u>5i/YGL]Ke-lu?><=)\P=I!:EE%@*S +G7W;?[J:RW=j)833S#]c9s)Ra/RpoA"KN0:9!Be]Wo5k'FDPcpoom0!$Oa)Ol<:?P2bPFJ1<+r=YU[Jh +"KN0:)K(1!B4kkt>L`!3emEPO=3-jW;Hh/EAVLGT?r_eY$!&?S2&r(7[9OknQ:S/iN!GVU_5[X='!+^0C,jgcWOu@X\X"rk<.\*V<[Ck5@+B4!,]t[@ZFI3ZE%PQ^W!'.6X@f]H +_5cJ"8Es@_A8u1=iEF6G:^L/K=I!8oJlT^"OjpcIaPt>X_mPHnTG"=uYU[Jh"KR:#,,o>qON!J:L2.[e +5p]Wu?r_eY$!%M%78h_m+H%aS%+d8SK1`?t_5[X='!*!(MkmLe5SdG0(p7G/#F/[sJlN+X-!3$0(.hfT +J1Rj>0jMm=%k>Dq"KN0:9!E'?/<[T1!Ei[\@^q^Y*`R_k$!&?SQ!i-]=X8,A"1#GB`GdD=4K&C`'!+^0 +.`&.DZ:O7a#%V^bMr@jZGYe]I-!6F?7B+=`e-mf.`&+fWtMKP/rQO.AO%#4[MZ/YN!GVUS]n: +bCD.HCb]2<(_8+4WW]@8X@f^3\1EfSQ2t,nfj_IW0cj;G<=)\P=I!:EE*4H//H`;gZGgc7@m$[mWtML+ +YU[JhiNc#>=pAMWAW,MN`ct;d<[Ck5?r_eY_e"t[[1(.9aqhn&N:EPRX@f]H_5[X=L7B+= +`e-mf.`&+fWtMKP/rQO.AO%#4[MZ/YN!GVUT`DsCL1GdBeY/RpoA"KN0:9!Be]Wo78JBrtDT)&_,9D;3p\He2TIl'Kg;X>Q&? +BT/#`5CE%D-U+7G?_pV$olDPEZ"o\A)<-!cJlN+X-!6F?eSm16FFJ[fkK]W2cC@K+YkD6qUUmM,CE63F +\O1c>aT(4&%cr%[JlN+X-!6F?eSsjNh"'Iaifg:HH0r"eq!d7V^:sRCpUTpCj5hAc +j\WZm-WlXA3]\uM0fCqU?L7+JIdl:3WkW'*qo=JQQ[=,W^]/XaU!cH[ +]0Aa6\-b]J8'.6h-qg\Pl&='rJ'T='%?m\->JeUBCSDD2#okp:V]&?HeqU]d_2 +X@f]H_5cIcRrigsG.^8W?>oem[F]HqeZ2cTY5!_gBe9i+&$P[H>g!7g(4oQ(R?V)ZJlN+X-!3%[.jSDI +/N:E3@Us#Fq3n\(gY:IYSijR;Ds1@OGd@s_)U>,]WtML+YU[JhiBl,djQ,Ckn8Ru7Q%t*K3]fF,>e0Fd +kta,"lg*m(m="Wjd;$FiCRO,*6Ek2aP7jrhYU[Jh"KR7b=j)f.F.SbdBY9^enW>%dg=t@$?hV-NhnFI3 +m]H!u\sNF-!6F?hS[M24R5E<[Ck5?r_eY_r[!0[7n[#b*L)JiClkJ.`&+fWtMKP/r-7*AO6l.[QIS" +B2X=]$!&?SQ!i-]3@&`!'6:UgjfrP[P7jrhYU[Jh"KR:#+fT7F0JG1Woi!9#JlN+X-!6F?eSmMoc/C[Ia/T9CL,19pWW]@8X@f^3 +\/^[on/TS\MaU:`cYMX;]B[ +"KN0:9!Be]Wo7thR_s1pO&ZKe%;/Rj<=)\P=I!:EE&f1\P9`__[XtN7ZQr,*,Y=.E8Wk>jOsEV^,Y=.E +8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Ws'?ea)n-WN!G=\Z28mJSsAZD:h+]=tQ;q4*U*lrC"fukg?0I +aiX1iri5(>"q=(V`Hk"g7C?Ei.-;j#+D*R +mHWsT+!7]_M3MCV@H"]1rG?n.HfM3qg9iHe3)r2BX_hcd.4&j.\&3t3Hi(iEFQh)[D;2(>(5:DG@n59L +V2t2IISsD,V+[/d]=T"FH[:"@?+X/11YUQc?bbIk4R^&bIaq52CHHOq7.X4SGM[UXo/Ca%;prF,ChtYN +:cU[%SSjbPiSdL[%],V^R7TP??Is4R,=dci%m@Z)o#l,Un)(lE0JJUfoLp?f<;jJ-0\< +16VX$gq.d6CH4dTRQdl:Fm;rt2B9&2.'$!K;A5GS,!5U91qqB +hg#.uq;uJ@-:tC$GOF7`4bk6+cd/L.jNO#JkoX0R+W`dmFSYLhoU_uVRPa_9gX`X-oh+uUj2[4M0;b+N +5J*4?/iA%Nh02*Yi.L[9e?p9*A7VL:X/kMu;3]&=`UkeTmsk@aePDuO#7__urUndkeuJS'Ci)O,6g6aK +o>T$>q^`cDeic:1G3rK02dX%8V=g,.[r4KSm+A0uLE`Q,P26d6GrhF#bE,LarA:pqIf4TC:LqXAdk6&& +P'1,DBBNY$ns?AK:'j[e4Sc$brG#bL_6k9EnDV95I=;"emr*]U^VA\+#.d*%Y?jrl6>tYhOcJb^Dnhi4 +h=9OGrL41G-71);GU,ZqWuW3$J,J=Ks7)/">eYTr]=[r!^\Q,d5(U_M>k(hmktfNu7uqQMdjUT!)05?2 +c#ce\)Z()*5;\S[]6E/EGH^;0ZfU#SFfR5`dQb@0U\+9RDVr0VFEBAWSg9 +K7eOFmQH(!r5Tn/SDL6Zo?KGsf;0mD:K@?E$B[N:C]9U@2rB#SLECutb^[6?RH@[">eba$X/kR"pYK+V +=K)),PTZOFV5:#,^\p&:h77U)K6\[W(DktRj,H;'q9P$:?-?*nHdV*Bl/mnJmbCD&>#1YJ;;kI-l7QGX +S/T-=m^p_h4?5F4ieoVcb0n>R.HZa/l(()A;mWq(XigkYr$N2`K@FhOZoaYPO)l@=S'i++EarPn(p2a1fX.S +fYYB#o#o&I)/+GpT.Z&m]ANR)a,V25HG/SkZK7p.]]/D/4b!;g]jL`eTDl33c_9lQqH?,WBtVFr/YfhA +A">`-1A1S'K+Fu2+sfd81S)`)s"->2=m>Yah2$[g%mKa"Xh$bU,+[8b=0Je%L)=VVqmBNFdJ3Y;V+[.Y +?b(7@:=![2?iKV_^+TrCponYT=$cc\f>$2']R>3VLCP>>^?Bd&Mre+PPWlHJ0jDDLS2Oi"G':j6i4FL= +IX4`[WsJ>)lLfJ*`M]>W\o$31oFHD.Bd4P.DId:Rq3lDOgY:K3LX-]!WrhYA[B&sFNqBT_h\7TZpYUJh +pK3I(\G\Yhm'Ghi()Ie9qpeF?c_-8i:3-'pg#_>W>ISMCo^oMNB5N`G3D(31l0`ffbr8^Oj7gRNV:)D9 +2g]1MkJY/.,6?HqC,p,B`t;kS702nj1?n[$=I*^$rBa,]d8aX063dF$A(:VIki*;'2*+P1Xu$PtEXpHNWo2>.uo +YcuLL3E+0E-b>`'n,5J%CH5#`K.3AAk)[5N]?\0O)Bp3LY-+ose[OFb4+/APoeS)6q=EcJch&q_ePDtP +l&Vj>7uoQHs#8h_o:'[4WT9Jslh-`+Dn;./s$04M92ebfrjZ1o0?oOs2fAE4cWJB6bI;EIhTVVb+MEBH +;Y@eR8s1GkOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E +8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>j +OsEV^,Y=.E8PT3qZZ9$e~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.72 412.56] CT +[1 0 0 1 0 0] CT +N +-954 -77 M +-954 309.25 L +237.25 309.25 L +237.25 -77 L +cp +clip +GS +0 0 translate +238 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 238 + /ImageType 1 + /DataSource Data + /ImageMatrix [238 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WEG1U0"35CI66[V,k`:Q!#W*<"a<^W(#a6B_ZD)r,+btjr_]k$pZPiuZ79#)n+O%bjP*-H9&53LC +`1X2&ClC\QU9#2@ANBJ]j?jI@D0L&=T?lnbpPU8>ch#rZ\aR6#H+nJMD05Ddp%J*NosVZNVV',jq=CM" +:TD[.29U!KBOatWL6=8Y:i^Ut4*GH?rr#(]r3Q8:*B8"GE,a2#/,K/?%j*ttm<"0@nLX5J,]9, +U5LY8q<"0m47><+@G+M`\lHH*eOL[+AF%gKds1oeh05L2kPb/4*uPqVEob;92r/Sl"W)RdrV,>i9Z^#9 +A]kiPdXqc8lEKmHZd2g)=.nX=V+HAqjdBbQ.@98;Y2O^+q<,fS^1Bq9jHIt`V,]0e]J8il>e5%Ap"*QS ++)\jlp+*;UU1WfYYJ7eZ_#cch3Y8DrZUUkH$!1uu[]NY\rqY`\?G0O+nF@n`-.goBc9(LE0i1A)XjV5A +q=Dos2f.->h:!"-41)oE.DE:#pX2\fSMOo=2Vto]?ZFicfAi`&S$'Epb$l`"X]e_rFL9^k++9s^S3(qt +eol4[lCGfE_>3K-hmRI\Y![3>7&M_mZ$uZJEq'km%Q%6mf[d;e[2klFm.0qh +d3S`c`R?bHnWg0#f/8bO;S?)!,\X^unoN)uf3NS8)jDX-hGTcl]J1BC(skrAgZqm/:(rqpl%HLm"M=MZ +F?%h0I<<\gFVC(BHdhE=Em$$D1*1;+P1tn,[[16>aj)\JHgS4%Zg4u&*&aji>b1;@\S/ZV%H9%*,ra\d +VI\MhDV3Li[;4AH]`kIiJU/UY0:&&F8T)&sL1:!9WKK+^d998ulTMB`-0H:1G"d\$2/Cdm/DmsWUE6r- +NLKKSVG[:f@Z&:gB:jTQDq0UbI]2?I2Jae.lNj@ti7NBeQ\jPQP*;[3_Co( +9hKR$i5(&bXf=UtBWu&BaTi:qd&uXmNh!OpgGVb[ZpaB[MET>sl_RQSn#I,P8OpNm#A;jb@;AUnGOF7k ++*!7j03S%K8OZ->a4<%\Qc:%G9$UYF@)9L$o>Y(4Vc]".E,\X5WKZn2g8.H/QCl4K>G^El558M?Te4"5 +Xj3]Qj9AhKSL:UJ^OGm+3]fFr!`WRH8O;l^Heh&/KWe-MPN:`r;Z)E+$Q#+I!7.t3ibPaRA$=J$1IfY[les,UlL,I4F,_iokrDis/-k`UHMF/g&iXr4]!of"!s^*:bpbBN%Gp7 +XJmIX.N&\[?U/*>;0!^(4&Br=,s+f#jFMl55"o+'GB^B;U%MA7Q2)*`]hC,_5*!_hPak7c%[\<,PP.@s?Kp'!Ra+\eZrk^eNET+6^4?gArT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg +4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'di +j?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%X +T]A4-G+dnY8p6lg4A'dij?-%XQrM00!s3:: +A-Y`PULXU%cD+nT9$X2B#>3::A-Y`PULXU%cD+nT9$X2B#>3::A-Y`PULXU%cD+nT9$X2B#>3::A-Y`P +ULXU%cD+nT9$X2B#>3::A-Y`PULXU%cD+nT9$X2B#>3::A-Y`PULXU%cD+nT9$X2B#>3::A-Y`PULXU% +cD+nT9$X2B#>3::A-Y`PULXU%cD+nT9$X2B#>3::A-Y`PULXU%cD+nTreai"J&Uaon5mhGTK\CX#*bWJ +aUXQ*8')7*S4C[2Pak7c%[[1j^%m&1-Kojpm>b*b4WRW9,hs%ZG-DA+%.2jtZ!.N+sDD +m_Gd`1$/t)C*$mQ^o6BMjpm>b*b4UlWDT5UIk_0bOmci_zz!!!"fm_jqqrVc]8Nur<6NjOX]^]!lX?[o +1\GjG'J8AffRanY.EnuiZAl:=L! +HDSotW/r3-7ZZ"(hqaN2`Td'@6+Q7lV%m%%m"K7ee^?!YU\]&[//G%jN&aBsGq[5R75l"T-P[\$oUq*c +P+dRu6Y96=@e&tK/p\5t9In%\nZ\8gQZ='pCMh07J8I-[oDY$/65]_]GL+)$;q^AG&>HeG:imG"N^eUO +V9l6Q/[F3c^ZQWi#0^]2#]dA"mT2sZ7aOu7aO+tHhWh!FS[M"b>$ +cm(8X4ppu@F_dn*W-r9:,OedSihF3QS'4n_tkp9ID,n9$Y%s8Mo=-1APASq`e=gf2df>b(T=F7hF!(G= +3!P\08fgX=bOj4rJHrT9Sf55t?;M\e%\j,Q4$gW!c$EjOu#Fer!Hn\uLiIItp^7uo9,[VXd2Fj8dB(04 +A>hEEa-a?+P0Pig*#)4?ANP)K'YR^SJ#'MGI`"(LP]/6T8ibEh_aNZC2I8(kP>=Bqm%%m.@nSGg):9%+A7XcE0>Hl=lDb=5dpdQHGid)5`ODe7=$6 +HRe!aZ$H`('%fTP?]='d>jXk&$oD7,3)p5:T[Y6LDL!<<*"zzzzzzz5\p0[Q'>a+~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 574.8 375.6] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 386.25 L +1191.25 386.25 L +1191.25 0 L +cp +clip +GS +0 0 translate +954 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 954 + /ImageType 1 + /DataSource Data + /ImageMatrix [954 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WIq2jZ+33njeO7B0ar_?SUPmccMG.hS7?sm/Ff]Bm'fFO!R*$MGVIH8dK^877f#\ +M2>5L;cuN!g5>)L&Qp\?q7b\2f5YtOn&`-=IU9R_a7J9hOaHNZhXe:]HZ-Rboss7'D#jV:zzzzzzzzzz +zz!'nP@(0#Sq!!!!I_kg"mWiE(>\_P)X%kBDdO$$;gd*tQX<5CVC2IIlSqohIaMo[E +*jlDfHf?@2)!`J`/-!WW59QabgtX1#If>25HOIT_d>lr[K+HhVDP9oTn>@*9%ejlPUc]mKM-?J=/g042 +HKf@SX$csUK^S2d4s.0'>J]U51XZf^uWkF^AVe^`:'s7=Q$4$SK@CWkUbs1ReJgm0lX2uimSDs=t'l04 +&;'3SODZZdh+EHZM+[pK+8!;ul.]=Y2R9i"Qs;E$4X<%\6`"eJW& +9'bg"/r3Z,;Be)%]\mn5=I'H&ei3bgJ8lg'JD/cYkO.VNmEF8,+Y

fC/mC/C3O3!,^f8U>MaP92>fUap+.aG]_^oo2P@du5+g/>'g=!`Df=O@CR;Z-rUI +]/*s@M,m92A)27D>PX'OQ+d+q[*l_UGS'.6Q-J7-tr6USi)?RZ[uAn\$g[Ti@1Nd($Q7CNU_BRoUTXSTM.#9=g*=0G +r>?s1/N.K]C9">84*^7fm-J?pWX.XXT_83sRQ52F'?-DAB?.u_D2E"b'FfQY%l]r +VWIdCZaGJoZTE8cE!IQM=t,J/F!h03_62PpEPIM\GIUTEMe*BI`_\oHb"=k<]T!5M@8Nb=o"IX^.E*>f +WLoMX/mgM7bSSN@X(ali9-G3sV;Puj+W3-44Uj?qFp:S0O(2qu>ZIegs"$+K"Aj'?6P^OO"&L>e07pAX +sX(GFrjhks[^>&n86=(6j;ZCAl0oB1a!K*B9De'jFbgT5u]G#)sbbf@m=DAe2b8Aals!!(YsMi3MEfih +Q_Jh>P'F1L(t4aY&(D!UBOWiN0FMg.`]7`NHfgp!.HemiK1C@'BiWnm48ESIl9@s9hEb;LonGZS%'LPN +cWVP^5n?G.8fO7&n_9K=:i3aiR*n@(-djJ*"h_@:@!SHl2[grr.AUf"qk(g2b(#Y-M4o0&EARm+L&pCY'@T>W? +onfZPq_*:C'dD:sSHEPPbbD/X*$#`k%JTLaMK3kI%'db^&_o[?Uj%L)n55R#]if2n6ag=ki3jR[Z+Njm +Mt`"-f%[$2e*NjrL`>>@0WRI,*-A``!pPJL>uEk,FJ98iZc@$7rIMXqFECIsf"E>9h@'AaJ4_hFlr[9O\/&N@,LE$R*d> +'(+L7*]$>2^nKDXA.b^b'7m^Y3dO\>?etQm$JkA0StdmL7Fs8dcN=.I+B:%"\n(ca>&L;?9he>?i:?!Qp@lI[&$>\3]0:T8m,[6'Pt?1QiLh74&'3 +r*G,b@,G@G#jI!j]ilHRG?N-baGG^F6CiZ+'u432Cg`Qrr)_3&[E-`=/!Mj2*X +OCg];bKJ?[_TIR1C;&E[80%prDhI^ZDrlph.%aHhZs=UFSE,SuoK6j1g;/+RM?!2dK]p`A_@Ng3o@T=PrQ4lO, +?R.CUn@A)EQWm;HqG!!(qGZ+@M]L%+Nd]?`Fol-lQZ"d(#cQW!6rB7>ArPR(g=f_@Blo`S,BI +pI0k11`zzzzzzzn=k.J!!!"D&QGb^=(K)!7]Hfkl5c!:OK&A].ZgY3WHsnc8CK8Jllf@4;Y?&@'hD>U< +#%r(JQ0n\Cn'Vb])Wm,EU +oH\[h@Z^gW@F"0!i[Z2Wcr051_"NI*`>u\C<Qe*K_h5WWK.LOK&A]HC'@WdkHRD+]S\Do +o;QX86(4?'hD>U<4tplUhQ2`p2kG"j3\J&?0tGU^Eot5 +cP0eW^*T@V'/ajZ9fHNg$Bk^mtaqo<#%r(JQ*@Te[mY+8m>=e_]tX6V!8t_.ZgY3W@F"0!i^L4L.H%%$ +>F`qZ\MMn,>n\Cn'VarD-8"kO)4NOWcpk25cP0eW^*QkV!;5(,,HkAUu@H\*`DZk8/4ZqJ +Q*@T"]G"G%?]))( +"2TmAe1ke8#-KfW^*QkUoH\[hPDAng9O$`*nfm'9*N7ksqs6K].k +K]VrK5&L9]N>k=eN4sT+B8SC$G45ho<%bjlL%mE;oe2s+K%E;WDK7$G9IT8C]dG< +?P9F;H4i>"]G"GH))Au)hWcqDnc&0LECs^hj3XR)\>r/,A>7-]N>k=eN4sT+B8SCk +<&.F1HIR3J&Jg-/(Mnhc,o-6>?9\$U-Pa8&ZGi2.ZgZ[[r+@;!moNe6 +>_a$b<8!O7un]SIf+G#6"ainK*MorDOIr7^cuuG;Y?&@'hD>U<'@j9CTFIm';lZ/eCRdQ'7AQCI,Mek\ +)2Z3l`\(;IJ`a2ldtOjHgeYW['WeIr9N,%lKT9WRI!X?V5KD@V!8t_.ZgY3WVTWjo#$;_RA\0`hZ#`cI +C*Rtkgs0k%F-RY0&$)OJUs2sS7KA_9>X14AKb(BLfAq)'hHlQ#7i<@HoQ*qqt/,lae.VJ[cLucmHpfHS +LSb2D-;F>ljBQR*K_h5WWK0r'PY`k69+d&hYCCGf=aS-V[fglRiAH#D4Uh*^+7SC`F?3.WpBNAHcBk$Dcr-OXcHm!ic#a>?9aolBAukI +-8eGmdBLNXn&GiVja'CVPg?$$GU1no]X[)#7dikQ3G>J>Rb\ +o#!+qmJS%`ONCd\$<3c*RWm,O$CqQo.`0p>?9\$U-Pa8&ZGi2.ZgZ+Zt1PTX$/[b>>AjAA)laEIpQ>`g +NFiZ=b.BI!i^K9U^Eot5cR#qQBc]tqBTt%ajHChGd/ZYWcpk25cP0el4\#;M?9aolI3)nnp^[4c +)&h~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.72 375.6] CT +[1 0 0 1 0 0] CT +N +-954 0 M +-954 386.25 L +237.25 386.25 L +237.25 0 L +cp +clip +GS +0 0 translate +238 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 238 + /ImageType 1 + /DataSource Data + /ImageMatrix [238 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V_$u#N'L]c:PH$UP/QYS[5fp0cR4/,aXWAHB:B1@pzzzzz!9!;%E+>s+O2nMNJlV\aSm^)4RjV=G +cfPO7mB_(i)UZ[%.&qeVj3e\3cXl4Z(OjM!Y;l4Z(Oj?=k;.(N-q#tjVaEe9JO +PanE-4s_R)\PL\K`D8nJT+Ae3.L +5LSu3,Q~> + +%AXGEndBitmap +GR +GR +%%Trailer +%%Pages: 1 +%%EOF diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp.png b/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp.png new file mode 100644 index 0000000000000000000000000000000000000000..b57a9282868512a0a689d083e067ed2e2f393bc9 GIT binary patch literal 16444 zcmajG2T+qu^e+sAD!mC(#7LJ;=v5Iz2dRRzfDn4`JyDP%9RxxV1f(ly=rxM;P^EXJ zN{JL9Xec+l@B7X7zjyA;9VTp^eNJ}I?m4@A%5UQz>uXToV7oy;KtQdfscu9-KqQ6# z{+pZ>|3C4({SCe#wb#*5C%C%)E9xlEz}HZCYg+gb5HNUNe+bc}Uv2P}Wd2%ukH~(I z($I*}C!Cav5fI!V&{BV35lgyAu$;xR=jA;>n|<@xX_e$vLR|rH0391dRg%^?n~37wOfRHm9BKR zIsxapQ+L!AS)?$ROb5E*OXY9uyWyRs9FjBHbqmE&=wnnCMn$=EH0jym4b~egH}OpJ zIOnDs9?DOkTPHt8uAFhKVSc@?>nhOXmbZ{FW_s+WHhIn|PkKH13{raiNj$Mjw}IeJ z-gB$KQHsJ)iU*UNv-iZHJ9N@(Xm<~3LOh+?b7-0mHU+NuV>iXG8cPm867&wEXtTUA zTcRS7!6=P0-sKU-li@}8s7&tfrxL_Tzpz+to@XGIB!M>Q3i807PsnxgLi)ef$|_l@ z+iCp?!9+Q8N2qD}&)nBPHhlA(Pxt;z&rAoXkQ27Gc%fx|@4EBMh9ZiD67g=d`z_bw z2xourwRAiljy2;PR_To<+Dc%bQL?210@t*0p@P+Ojl+9%tAlisuLzr3a2UNhTQV7M zY^b3^@gx=reGXyu|SQtQeXZT!s zpm-&&toExQz2vklCk*<@K!NYW{z2yJfy%kaQeFLP-F)=zf$8IMHP6TDi4DCUXX32X z)Z(&QRYirPVU^DOwSBk<~@l0&{XrN)VV3vJYZjz_`P0x0Hty6f8z z1zymLJiC=x#2lt-gXAb^b@ryp=4EOBWR~+YA;jEk#3RczKA;Ua+eULNu58a;fhe0$ z8T4wD(vbUJ;b)Z3q{6YRY1>n(Go26C@uvfqC{M(%QNk;-@V8a;3{oP;MZa93Zh3PF zQ0PfKA!xUYe76lTryv0esoa_xMgN`p*q(t1X#DE>=UJ`=7szr3eWOLMt#`{FIxT

Y(5X zo!YpHy_dN}s@A2rec39J=cnDz^m>p#!Sm${&H53=H1I-C-oD14T+G z9V=_rH8pj5)UM#%CpzcimxO~Xhb9`9c?rc@Y$Sv1MsUnvk-4*Om~U~RwNa{5Y(r`B zH1^Q=9~`S5XVYI3v5xB3_D*)+ufMLHsd;>|TD#37RX{si=(21I@@tX#YVNYK&M@o8 z;(){Ci>c|Jb|EvzOgd`{EN9#poep)X#*7^?x)=0-RF1%mT0NC_>2-Idnm$__?dYBN zQ{F+^`9e>*0`m&mZ_rbdHbZn;T8^p|qB+-9v$5inj#?l^ARtrdvA)BzdApC&HXTXD zNvg`_$5>E+Vv`3U<&-smIpn?bftA$CIwi#2?ez!}2v;Ajg*WPpp#%oX`x}9VNWp8O zS-RowFt64qgqE4S@_Q;Ceb6JmGGJ_{jQiZo+V=UlU4&x<=~?&CmA>h5oDS>kv|aXL zZ9DCJ!gVj`tQO{AL7~@W^b$$fKO)>we}o z=7-b$PoB~*R_jW=UH|QkEE~F$ET)*;iJ&arIG2IkpOE=OV zSPVAcu~QaNAs~Bd_FJkk&y3t&?@n^xgRH`2!1PsjS|M%jus3lnR3Ik1V+V|Eg@QhG zy9{3eHkOW+ma)@?xgLOGt)}JCvTkdR&j(LJ14zl8Sqiz-l#84dat3~RIb$`w*% zcYyUv%g`I@BFm&xjy(42dC}$23c{))ZG{Ujw5$T9Z*?1=Uz%aIf65Sf4Gg?aN)6(@ z0+__&CFQb>3mluU4mt{Q3!EK>=+!rLoUirWrP+f1GUtRI%LK>nVl!Lh;-p)vTJ;0S zT@7n%hE|*@X}B{5!HaG+Q-1+ZM=gRh}&3# zJ(*X{_hQi3iduKEqS{pH2J@EsfLQ|OFruE&o z0ta@bL1)Q|DvtpVYG^c{iO8B8M$UA{o<9c{gKJvs?(W&HPd=B*8(Ze?l3XuofjeiD zwau~|KWKN*+Zk_pA(e?a2Z_cR1ON4U>Dc0mz$dEX#2_PC&Y-!jhq|f0;&-QG0<2e; z9C*5O3z8(K@XovzHzw$hato@lqP<=NmILPqk=woDlEw#UPr8;bElhQOHIrZeo_^b` zhup=UxJaZ`Y7(<)s-soQAhC?89W5>RXj5|NgVJY}E_zoB@MSkiwJnD)YFbpV+Alcm zJb5?3_xN(10Y$TgQK~Q2JARcziZ=A(RW&iE-hTN)xI2D3H2+(w>=6$(h6WJX;4W?D}3EFk%{)(qr?K&foMB9Z|e{YAZNq}W73kErt=kjveq6C9R zR*618FA>2B%(a*v#$6#e;qxbwNVy(1Y*KDQNbIZt=F7Eoool|r#wnJURK2)m^IP$_ z?x#BYyQQ6mZ&N--*a9C<$cpKXSHz1do5$e4JP-ttFN6TE=%9f&kM|b_MCi@ET;j&_ zFlV!MHV{D15CC=X0u8`GOnivfY(O)*0c@gT;#gvF(UqK4&@+U)i%Xa;P0f|iQr|Pl zHCDSplU$a+}J?0V|g%m!pWI|YuFEY$u z*H`5ml5+))yjmMk`jeB@GxcEwF~BVmOrC-31I-qx#_?Qw_?nn!U~|MT1q4Nv)__9* zkn5Mz^{3Iy_uH@k2IE#$!>ZK(lDpAKWc*;ut zJ8;B~Oqol$;?7LvBuHnmNriP>?a^NgoV`k^(7Fc8lDdL7n4f?I~@Br7%xr* tAi-W+ABDaJ2KzRq3}0(pkf$=3p8RPd`M~RV3G~lbG;}T&owvU8e*oMT{g(g$ literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/sequence_processing_overview.png b/moveit_planners/pilz_industrial_motion_planner/doc/sequence_processing_overview.png new file mode 100644 index 0000000000000000000000000000000000000000..2b82cc09bd5725d2da07ead00780a01834fbbd43 GIT binary patch literal 187694 zcmd?Rby!yI_AUHas34&TNC>Ee5`v_JV4#3BN|y-IB`J*}AOa#SEg{`q0)irlfOJcT zbT>R_F2XnWKELyw>-xSwKCb<4ka(W8?ltc@#~fqK+g(yz5F3*O6NN%y3kwNIqfj`v zQ7DYq6NlkD;a*S4P$(yqu)wW{&$_01?agIc!>E4%o zr#zM;qZ^Lt@>p(@S0<>Tgi%RKdqeaC^8+5Q+;G8Xc&9f{yr8W1IKlWf=dhj(CTnex zqO1bf=jJe3nd()pBXB+Y zA6Km0@eaO5q2Agw$shXtPn{$GfBToxRsY|2fAij3f-7F^+fDMv{#cTx@R#K+E{Z3X z6o3C7<;4rk7Ey$UKN9}_)+?bCqACe4H`9;({?^4ahaV8>UHGB@`?V-pl}*V9=}*_3 zRbjfI^C^w##P47J;TjTNOY4weP(dm zEA3!n-l9AHM~M88j?IkQCOG@zAr8jxmw5j{NJDIs{_@d8vQxjmc~iuvDr7}Y@`NP(3N#gNX3I>%2-59^-)Fpj?&$NJ8G;SwgWq^N6B2%$djDos+i|JeRjgFD8Ski1 zTFiHKbt8#aI+R?gDIb~(LUmt>2#v6wM zm5e&Go`_&5cM4M2=a`Mv4`dVfzW;D_Nk9D~;oVG>ZEe z*d!07q)w~JtE%!no9maI7-VN>S6izfDqZ`+RJyfLDwcmCx?@6b@?xuL=tR*Qp4 z)DPa>De$5fdmZw8kz1|}WDMoCHnkgw750m;U+G-)B4Uu2l$U=O5J133bXED>eZH-^ z%7`1bpkv45qw zEc})8@N^Uru)n zk98UP>g~ANYZc=wAjsb=PcrFr=m;GhT~bogx7u1WGc!)(pLU}LNh5F0^N{eG`*Ij} zK6Vk*sxi_N4-Vm81#OkPjjV%cB*{8w|lL?{PiUac!O>Bh=*vBUO82Hg~k+35Fs z-Gp0peOq-|PiiYFBpep}GROjGlTyV)hstQYEu*!L8<)&iwaTgWPMv5koDnXPwbLi% z{WP5UJ1T`TMl1R*L$5VSXMMR{&0BW5udFN~;c{LWJmjslc`8QFGkSg$+oBq} zDN;L6@bq47H?OEi$)&62_c?58c;Co_rIO%Vrz+ic@8Z?^{*F7Bo!X0C%0=B-rP%WB z$Z=fqsm^R|18WGF3rWkBzOja|8;V9l^u;51i34HTmx3GYX#A&zQ(=u1t65>JuelVr9yx6q&oD zR|gwDZ$C0M;7X!eZ){)-tRg{6bh~{tH8p))8xRm6!N0xqarLK?qGCv7Bzht_oug|s z!#KPv#J4IWp`wGSpQAv;(sBi&c|xs#%1|j=tIiph@0=l*VOP#&Ty`l#;U;-MK_01W z=jiC@zO7Nt-#15*^B-?cfV$zvY7hL+3}R1^-?&qUMBfB*TwN$96T|m#9?mIJ!{!en zL92a^WuG`sK?Hx(PwDgBU^K8H(TDx;ph#=Fwkmo&#yi><9`$W&-!5*;skIu~NVbj$sz&jqspAxOCM#2& z*sf{rr_H#o|J?csm{e0 zznGMqMn_MN6*<2N^?{m}R=dWR8sdS%aqH{|NOj7xSM4=2;GJ8y7saE-0)&=HJ>4<8EHeq3r2iL#sO$fTD| zS?hPk6%XfL-t0s3wJ__qrQGW-c{)^8Qc|+k6@7$S_f9mXpR$XjHPkUxyZJ9B??2kE zLgHq%mhtA>7?&&CXywjz@s*5-&3p0Ug;*#@Yg=2}>=EJU@Yqf{HTz{!{SWUcLiL)?A^gml)zzU4H6}fX7l)%WlIPHcKZ` zNv@mpAHn67R8i^m=U`IIxeWO_16U_~v&w z7^Za+Cp5`-kM1ENZJWk>SyJdVZ}i%{l5KoPDhTwr{$v=SQ-$vyBS|TV?$exHUu(LlSsZMNv_)tI%wW&wk_kyK6C7Su9=p0A!CHI~HXP zmlu^dwxpi|nF@RCCxFQxc>|ShOlqZDzUZrY+W^oehL^=d*nV|tM|!0v5qnAGh>&EBbzJ1$Ro$X6g;n_yA$W^v|_(}}_aB7+R{VdLD z#mejHMo$B}>IM7_`H=jE4V#_oh{a^vmx;mwSJIyjo6`V0H)qSx3$;uXP$`#|M?;O@ z@IS`SC*w9B&ok@>5V{i2zx@j^&-N&18F8zmvNG8la!W3g{&FZq@nWHbO1ZktaZp)a zzkZ#OR8qpr$+;TVB=3exu@ghFJw7*3Nh=Xy2WuM#eM?-Fnnr4BDmHdUwzfoyCS>nB zuBT4A%qwxk%)48jY)e<82$x$UV}5jVO1?Ya7}wUNVPaweiW9`cQ+POy&#SyC(9p+B z48z@;9Mb!y^!V?)1PPxa^IxWb+XTA3N!a!3Lf9MITa)Fy$!~DWHgp@8ou@xl+`ewJ zJ`(g5d9+oVQ1@X;DOxk20_n5@hOk|kB32@Y1z||#pkZ5v`zf}V%yUHV3}1L{uTpAf z?eAdo^}JPzeCEZP3n3kA0>wu;drT@fmM3G<2go$OhA^vFOs1ES2e?%c5fI!x{`!m| zDK>3Q|-RLN%y*Xwy^MZ>vst!95_ zItrxUal>ik$j!~gj<_o>Uhi{Ba4^(LTsI|KyJ4m^xjnyhIW<1gVPku>&)DQ^NBa*a z^-9-i9(sn);sd;-kvc*09i+VJKe}+VbB+5-0Z&IUGYGUs@LEgbLh^LN^4m*Pf4#PL zRom(Ma2|ZaNGdOO;auSz{rkS@Lsed(H*8mIXFtk4cwmO$`jd%L!qMgptYRV>*npB2 zt1o%mt=FpI$Uq6F)*UcwxM78$Py5wgdbuKbc{Urp5a^DWYmm0`Q+MF3Bf zQsWt|-(K}mKE>M9Q)q_l{@uqo;XmZAG9Z;9)^KoH?Slvk)X!bM!o` z)^`N*g<7TQrue|VwV3^RgTZ{NW8P1KFR^SJq7}WBGTyG)kd4He5p3?|$#y}tN>c;F z6FXb$i@o~D6-@6X^6GQH6FKA$bGXR{`x&#owJOe^Hj}9UPR?_g4u1$z4S{~yV0xl8 z89KMx+S=r#qzY~MHy^z6?vd#0>gp;eC|Fn^cq928 z@yGbl>t87x$t)cq6+!N4aIN?ax;VJqBDII`r%guA`2-=Ndi z&o5iE#upMo2Ay3v%ahl(*l-8RYX_7I`~#KvN2vLH7yA0<{ARr4M*J;j3=3$^mGX@^ ze?A+6_Axpa5H%A61Hea(HRS>mb?4g5Q6NzU#5D zu#lCN1sk4_K_0J6&KZ6FvO^z{n%z_Qb7+8mK{U1`K3JXZCc=z#+*#k++5+hGInUrR zi9Ra{MB+P>t4AcG^#uiw3U#S=CIr<=Z15B1A_9^N%#ZJ zGk>o*<(>jc3R#*p?Cr{CK;ht!^A@C}q~ztvio&l``t1>9!tk}z**)m-!hAM22$lMM z7nj^5-Gv|hx1X^cxteEnb}w+=&U|dsIdtdycla~Z+fTbdWKIVIj70>$l*bUCJ5kT+cZc!lub=u}gv_76Bc&Ea{CyYpcxyePyxu2@lZzK? zo6fFSj>pQ1{8W>97s~L4Jb&JA$I8`m`~UWhnMNBQ$KB*)V)H-IU`19H?;wW|5fc7%L~ld0lc~EsXi&I` zv`#mx=8#+Ozi7?I(@+wkm5`chKj==IR<^S-(_0Mf0c_FPbl}?ZjCw;t613ze#J??A zM%TaH;Kcaja&km8{)PoYeX49CwCcmw1JdnA^2@dj3$6ey<_4|NIUY*`E%OR!w06Ea zZhb{|A9|f19seJ%$xPb0>xY?G24g{z zQjNrzlgDslinTR6P>AXc5jVWg^AH+B({vsh;wk-TPoI5GucrIKADkYy zP4wDl+Q^qGkPa%YMjev4vHUPctTt|j?NaTCXtuOfNKn4ns4|No3HGmGj&D^yRrmFQ zxo-N{u6jLY1TX~95o%kvodFi4GG4v<^Dn@l(LF`=BXd# z#z-Dv?g@yhX>KgnyxllPo!;GrEK`jPQg6hv7xhXHXh9@M5L%Fm_!2cWb=l5xx?kk- z+Wb%-6vG56DF^X|uv%#0KaYJd_*uf0m-OU;Rfx}s>1YpphLww>W-*)?TWslz%k?vf z&1_PPB1Do8QMFEDVq!46ej5;RkFhQZz7cKe8mSEuWZgM$)sy-7%j-4#lL09T*)%&l zoUn8l5*$1F3m+kr2vWJ9%{_vrTY0;q^qp=V`o|@X)6#Ndn>k5J=bA{hH#ASxna$8@~ z`1-~xP>I<|kcKr>Dcx!7OZN-btaaVLxQo~}pJ4eg^R%lveno}M0%(=R|Y`8hdk z{QPL(M!f>uY&G4oyC5*23})ENze@d9Q{#@lv1sx>j>;aWSmEx9K1`@ANfeb-hRKWi z19Hkw?-mrst$AuSM4XqkpH%9@d~Oc}WN4Gr9phj?3MuFwyTGVvKBRxU`FVHCiQ2G< z_-Zsym4x8aiZ4yHP69K?W5&VH!Vt-8&60P^*VE^!O3_QL7OuKA$0$Aqfn_SLjo>QN ztCPbYhKGj*)cz+6B=(i9t-o+AqIfLAcBXW5W*n`k)5_KschnGc3V`75GtKjyuH36b zG&J^A=xAG@hjJKu%t=Kx#G3MS`k<#nZ?u;ov5%+=XCtLDv>PG>$*qTL)pkr13ZYmw zz9F#@8E0*hXD5YwbHcE0h069;*jvjhE;B+`R<%yWs+S$QuncCanVt-doLjy}y@BAo zryGMEVX&QT1kk|FRd|7|?b%pxQF94HkeOJouIkVF;Kll!MEoWJFXL${GToFM*#sBL zUfbzoEQ)%1dZjD5>0cb?co{yU`>mZ*Z`f}vMMe^As@<#KTBYE39B?6uRzXX_Mqj*Px?emOM!q#zc@7T<~`CW_C`noym_RaQTmbB`3`r4 zNvPJ*rVElUl}nDz45A?eOt&VHVMf+uH z53DVO49a9Y_PtQMiwalQAj*M5T%UG8NaI5;@HK!Bn8F&*7=`U7&kC+w4#ixcrh9sN z0GdHd2M967Rssq}oLFdd^KVdA!n=dIbkqZ!Y zl1#EMs3P1pzNh>$m>-opt(Eu(soGrmM;aaNVKwx1W6P7|CyoD0BK^g#7-CwC{Wo?p zVQYOXvZ0}2_D9qmXMT(bopgcK&_5W(>iT-}JxUP|%`-h<#ObiLhCt<9^$R_`(J~%{ zjF|bAuql9^H_3LNz|9Xf`8ojvGl&)U@81W;fW&begI;t;By+&-&W)%%lLFdEyR62% z)^x+g(K;y35+lSep;l{`Vnl+jezaK*;kKA4wcjKuv2@MTLJ;E*0o7&rq)g?l1q-#f zJ?qF1Da@{|lT)~=8VFHn3GR8GdkR?A)gBs?*^@AcM@ccGP5{!bS$)HuV&rzI9W^V?&@&@erFdDUq`#2og+HtB$#~) zXYbW=aQ_Y6ipD;oM7mpBMiT0V6*{ zJ-T@8$ftEJ)7=b`+Gv_KP&89CIT7Smaqp5ffCTHsQ34_&HJe`_fHW=`34}JJ^J475 ziYGyI?!U}Oyo;107{8Oz?n{yUm5Uf-= z%77RRlGoWlpsIi!^Gz827a_JW>5B)ETg6^59>XFO2uj(>^Or**_R+P8@U@rvTsVKL@6lCxi-pO#djq?27g5%D_ti z=Lf`zd%zTke-T72F643UVt} zc`%dcPIrp*WtDMn5&QpE^}L6HyWdhFeMPx|&2zVF%cqS#UeYo#zJ>=kFh+6c3a7Nu zA3*A!2x~>I-TK0~toc5nqex)n0KFx*{f4>0G_=E&Vw`y;T6G~H|Mmi5=6z$O1CTl* zG9+}J#HT6Q*_do>-`-pWqUTHCS5M+>quwGNnS|IYt1SWquGk3V3ZUS9hE&YV5g=_q zD$UxtLaR$k%zMz_RVI@=eUA`g(ycyrE^JX$1G`(bvIrsM?y2;Z7gc?x$4f+(+FIXP;y2u4WIRu5P3;)&t z?!cRKWwxN3G)wT?&wgAD3=A9{8!H1{Epx#nefqC5CKngie3^#>VBWZ%2lz7QHr13! zdZ>ALcveKFEU5nF+Pm|t6Gi`}3+sA8>i~NTT6-^2&KW=-T$h09!PnJ*c#k2yuBJ%6 zAMm=W1@P0tIR+a31F?x&sPcLagvvUkE%;p$x+2SN3`GCVpNJ%lssM}nfEgO7IKjBa7eJ?r$`zXp$O4IIWY|l7G zeNf;*mJA_E0}B9pU7^uCSTMOFdkL%rmQx+qU{S_{%SuhGPOa8I zV-9f$B5|K*-;;8uLj(i_QGj0-ApL+U-JVV*)&024QC3;G1olox;Jy>XuBis-wn_y^ z2+QM*F$1g&g6M!$ah^Ref9Xg%oVr;aq9&gs-$)B$5@6RJ94O8 z?JO}Nq=BO6iyubz$(F!COu4NjAhjwgD%>a>t~)q`P?;v5IS9i0b!hiz9nqU}4BVik zb|n3Gth6S$F@7`3bxxEt!l3f%0P%I+$$QCXWZn%&kkHMxr}ud?__H;dbL!2Ez$r|SO zw#5i~PfbldKXROJuKY-AYinbdNjn+8gI#-?N@Z6Do{>sp5|kX5Zar{UJ-e@#@#vQM zdlNq_t5T>XRsdFGV>N#@eV8}Y$*7=OZRrPihP(W-t70I+ZlNYF$4m75V=crM=Hxjb zgL@N0G#0wS<~qPUumTy(Ag!;VtNdfF5!*m}S}*X?a`7c__vh>NfGX&qY{4VEZEb03 z>5L{fYHpVA1Em>I6ar=g#Yk~Zmx+WswsO+a#ye00&C(s0muK0Lp}tuUhL1=S56f9F zA0=??;5E+XPz!79G857G-oJnXRaYMX!-@tGb?N9ZvU&;HP|!1L?H3g=4m*`yX5l+5 zZDf9AtajaXmk}XhOjU=6-H28k@FngFLD<|~a*^@~P1ERyOov(9zR8Zvq17_BcgF^j_Au>k#Amrq;$m(%E}-@Bh|6DPxY1cQ4e|U>IA5+5MTz0nSI(? zTI{Cv)kxX(9vY~FFUO0+7_I^6-3}-`h;akdp*0zJ{1N<)4%;eKUZnZp584<30f85Y zh^1wCN}f1fu-y>jk76tQ6*Mp%3rP3C?H&paJC9w~AEdQqi3?KQ(T1?Ox5u=pt zJlg)i7+cN@ej zCAYw?((q6Z%3%Sq1Cd{`N*%3n%xp@J#Wq{AaQp`5B5v zni((5n8s~?XFYlq3=i4K%tjUUJ^-ql$)_D`h7-RV3nHWaXg}<_TLR}wphv+^NhB>7 zz=S15-tvz|X8&@^7Z2aWkjRKL+P#>6e?wFQ7K)hnQ%KRF|I)?qGNCl*j_OZfF#AL} z9))aMv@sHu(bx~7=i{Fx)_|*_snvzFYE(E92<2z|;lU+|h#Um;-u)qOH^#-up@c~z z|A5oUeQE0wv(AtA@L1O>pt3@Iagu}q!RatUueJ_G6kEV%f3zm8i3bZC6!^Emi6BCC zX&U)gVfGvMo4c2TZcQT*L8f+Hw<%UT(w-y5W^+0}y=37VkXQmx?~`m+XAlZD19AQ# z_D=TnT_rmffGh~(KoJ0a=`PEtT5|oY){`et`5EOiD2p(cnFQ^GodGuQ$?7EEHkW>} zf|LW`YE1=N+S9TlqoY?9a}ur?vf}Nxd`u~S)l>Sd54{%~y$2%C4u*KpiI%oDLLwqC zhtbMrfeL@&j_b<`c@v}3_0hGXKcG2s93t&|#dyFS>jB-zXFmR8PC-TGBj~OmY5>~j z%+mv=#M;^#E)*KCkr4zMib1Y_sNO-z<^V}5Di83vD0EWL!epxa3!%($d z&YvZmKQArY>+*$>+L);Zo|ys-=ocH4;L(9v7tUwTod?h^6E@u&#&m`3xPMZ&NtXXc z-KHqzUsWWa)4flkAB|awffV^~xa7Pbz&CU(fWUxIxoCgCoVuGRB-CFle(c{Pm<|Le zr?F#BW+5j-2bF1McGtAD{F0=S5|2SghP}3ni+q#b*|5`>?xm;{b1*=c;dl;Y^Q{>m zZvJ6&tomjP`D-8r?2`HV?N=WX5I#-@F#U#%>rAT&(rWA)Tvzn7uxETBY#@9#;=^JM zF%Iu-t=VH>;rVm(58EbZO7yZ9L3v*}G4b<1*Ecn0FWDQLQ`zlmj`s3v;N2kIjB3L` zI%;T4Ge;n4m7sSvv5gR1LKBw>_O^S}?sTHQ>mg7~ACc($=KaFhr4>v{iIo458GdB3 zMnM&HJff|6^v~5uOswt?1fCI=kOWlD>UiU&JhHqnsD93&Uf^KvEwX5A2jX7f_{5(L zi#c@QN^FwC+eQsRq#|}PZ)vNh9EaGGBSTJS-EdD zwCv0?RC@aK>HY=&8`&2}x=*gCn*23kBN?)`{v`;sTiA9!0+bG|0G0Tc0UXYKIzCTo z01!T}+h`KL34T{-CMcfuKL^8yj^W+AUqB;)U?pjDb%DnwkW9#^2n8r8AVB8f!<V( z9g>&~Wns4hl}mt~4EW|h|GJ4dtNMem$ymDGxYM16%LduF4luV0G#9&j9K#{$uOjYE zeO{(#Sq1N}$}ZFnJihg9v`WjnnM0Qxy7)M^1TZo{vt&X5$rmXS=hh7I{u=1@W7Zco zdOtxT0`KQslp8siQ{_c&C;))o8gQeydi5$!fSP6J6MPy`d9ZYTS?M^39(n@+@yg}P zYij68$MfLk38M?D!ZhRyY%mMU6JFfSo%>HM?$%##@_OW$OTsXW5+Li(X@r=Dn=eWb zMpa0~+_T@FRC5F&5e(Oe3mckx_-g&Jt$}CXC_FLavh5Z08Uz@6S6xYsI`WOxwxaG}1KNsmh3+3F z5|XY2(CQgx?2E2+EgS%KLPA1@ZB-WfTztWbGA}gIOa5e=osr`KP9XL31U|KnB z?)t`lMJgH_OPI>_ZIifpFx7ix+yV~>NNL~@*FIZLEoPSHm&7LP#I`|tZvS~-@58#h zOM-5aDtLf_ zmzI^0Ie*>c5#UTHQy!_WsiSE0kS-Y(l){qNW=Uu5R4sy}Wu8@HbCztcbVpVE)Afb0 z2QY|Gk@UL-=Tx+gZUZs(Mwr%GN?ICXUxTQC#qS>HYIFYyE8w^kLvfzZZVi-f@T_)0 z%hS-Z$=-%gCBR_Gf$^GIC7Ov`DiRMmOy2#`szM}S|OD1vNeygg2}XEUtR?}a7hFA=G|ux zjbvlH^Ko3TAF=N8Ly&uE^=kI#6zo-iMhCV)gP;`dKC>djHatB@&B1yrN~0$6T2vmi z9~{WdioatX_|xPk!u_G25c^^S$JKq^xeQoPNlB!6v&Z1zrcc2%_koZ(z?RASA4hxKUhG zR8${`Q1q~RaChgsvj9lX7q_>yaXfo9teXNY`YSckz&pYQtI(NzZI>O>-&#Q2U;5QG zHM%(q0;$vBZu_lNN|Fn_=crh8UORw)r*%@7*hpX>3NGnilV#D70jb$}@X^TDCTRwqcLD7IuG94v0^rFi=+JUt z-h+IP`tud`G8UWc+o}mfO$}>UybTiykgybGWC)aYMwlEsKeM#~r-#_J+!a~COiHBs z?7~J7Fu71}BDp{jMU0T`%s_pcCGnP088AEUY?)yz0yTw@yBMpW{HlRNk+f$UEOme% zwGFr!$atSIDWHN{yfL~%mlw#-kXAdrx5IAq(G8`OBHdKfS_Ljetu*eAQeK_0!lIz zW+>0FkuzJqo88wqzt@9`N6(j{OI&Ap$HYZCp3cfkGzwVb%@$pgCd2hvhf z*Y-p=Ep2T#1R<~$wIc?8oL*3sb#LyCm{=4jpGEc?R!^XtS}S6_hlDo*ZEQ49KMg>eqnmkqu>oisL!NVNxpD&)x-u+PDLm`}DD0tW(K zTnW}oiK`V7a{-7A{H;#|R)oD1N2CO#$v?l>BKGcui9I2igz!ZNIj#p;VBV<#I*RH0 z?$F5hNPhm(IY5C8wYB=HB$L))Wn2JiF^~ziWi1noo#}$(_QQt{P4XWyix6=;f+!5j zD4 zVPS6;mu(iCEa?ORN&5`&$Na8Rg0z!ngUh&GkGcm}?>E)Y+Rxf%o&aS)qbXMSfBV<>l8t<4bJKqPNTV7lRvK_WS$$Bc7zL8&jLBj_B3-a50i(q*_DQ**_TeNURr? z%I%|inE12WS=3H6P*31!4cx0|jPI&8K)$nEPFC|$OPGZz(e+74d*hRRJJWqgo@#7& z!6G6gAaH4bu1_Xe796LM#Zc&OuX{Tz#ee{{4Zp7siQqDLLrk3u-=P~3zmDk&q|JB9+Vz=HjJTpx1AW%tsZ&dBY~2NE|3>{arOQ~-EY-cB$C z5~T*MzR{tDOpFcUi-Ku~#E|$&%srqPxM8X;EG!Jn(l=mEaGoENRAvVe)W5ZWONkV~ zj?!3xJ5C6d1e-Mvk}~fmtAa!f81QRguNh-LEi2|Z-cUw3H-*rA>>Y}tE=>M zkfyuKmoxp7zJEy)m2Qf=PoowSZeOkd{J&#U(dQ@a#9Ah0)0DYD zUx6u#mS3hmKQsj2kP%N-wntL`j{@UCWR(rD;eF=KV(Pn2O-OV@a!^WjA&Qr+3SkhX zp5H6CzBH4q4Vfxw;78l}vb8;e6o+VfFq}ZL-KCQZTq^(9>PlRGWl71myhh8o{+GQh zAO^tRzV{qkJ||7ch3b6eR-!?J34IxH=toqMY^3giJLFM~iv8C|1rYun%ad=r;Y)S@ zat~}3ZH?kM_Hp}RAHnE!B#dvn z2%jMXOohbnk9Xh@bzF0EOlW?m6C|f<{b4O-+>IajVe=o{h8}&BVO_zO*;)D4Ve{S> z5kW7jq1ktCN*O{U;|Q%wv9`D!MnfgL^#?IsC_fqcQY z0YwPT1SMAw(`GSFm!G7JLK^IhN073BV$pcPZgC`MLO?I%uH5l*HmsteMZ=*zFKpm_ z4X+QjU|ff!Co~L6uMlixh^?P{1Sw`rAf+6*$=8aE&{Nx(1O0KJM+QW(yr zDdy@Ss>$@`^5p6e1^W6or!AzKA$^cKxW^&qNuAO5?fcaOApVj!FALOt*jJDt#MHmu zvvOAp7=LfkMOfC(mCH(Zg^>|*-wdKc0GDq%@(uXCDu)&h;U01rxCD<)Jyb<*$%jfE z+klj|;Hgw3p%)!@Bpe(Z{;@uB@tLkGFV|k`lcY`II<8f6>n1L{kdul$ctaSQ84?IW zDzLG^*AVho(c&U|U3Bu1MOc#Vm0tiYcMGD3iqHiOsz&ia=~v3!38yoH(o-jG_r z1LHY>5yi}ZY{1h`{x$7+A4<%QAsDCtX<_-Sf7j+?h0O+K=HAc>)Lz$D7C6s(kx1fEj+I&H?T`}%GVktYVeZThRw^C8U;?3eN{9KTnnsCxoQ zZYr|o7GvF2dsP#G9~Zq^?O|m5H!1&KhE}>cr8qSW$4R&C$=yG4A-$Ij34N1%ui)O| z?=kPN_#ZpsI{(ETBSi+p^4>I6@rIfPj5GZCv;&ID4N}f$<4{ds-Wl7Yog4L>5kHNk zs|;X6_3Sl|fN=qH3zhTbY?J9&jw<65iS8JT#JlVby%$030d7LEJlo$OHd12QFxf=G zZLagzmYVd7`d(jb$tE7XXJZ1kdU?F$9J`w@iJJr!|)R~|M`{Y_Pl zz783bmXBdx%^B)RDmaNZiXcNl??l^GP`dA9aj^E)1Ir6Ob@);B?Egn^Ymg3LDy$WD z9xyI0X1G{VWXLQ(Mc`WcEf73#DEP?iv1_BK8mpF{fJvC!JOO!6vpv?hjIKytSX@A^ zz4ef2_D<1UyGvR`h|Uk+gTQHE51$XwPfJuI$0YTts%1huPK9HcoR#S+lc@GOec{5< zQ&gik7{k6EX?RBJcPX7-OIhA^pR+F-oas|!n#8=QNPTXYgy|8+;cBs?ZFrPNFW?Eu z3#}=u+~-RU449Hue3hGWTJE;wseW{hUpi}W@JuijYw)j{#g@dxh-Q27$i@0!tM>7W zA`OjjG)Ml)lP8hf)0I03qy(Iz@ci~!VtR69A_aw7D~12#zW}3)%qbo-l19iu2ry_J zKyR7B=~4RQ^RL897cVN(vH6fH-o_7i&uCGYK%oj0vaqtS0&ONrpZ2sPinkvW=FAq*Y3zY; zggn_MCM_*JoR*M~0Ih!!3x{UP4$0eB(0;>5`6>_>Wlu8-j8G_V?k1cOEg0B>Wd*SY z^bA{=Mq9T8ISgm|k#Qi(^FC&S|!pp1)mTJ|A2#X3Z*WlEPj-j@6BEw9#?hro?33Tyb_%@FlgdJLeIcP z!^j@WX)G=(>K{O;?|JT8g(8X@tkEFNf6eLzG!Npy^{G6t3UP5Mlzx+h>3GDO#tY~> zPz9{>_dr;#)5`XA1R+gIN-C{dK9}47c=e}r6V5Q3c9!NOt+)PQM_ z0Vpcd<_6XB+t(>iBiRwO&{-@L%3xNllZz~ zFF({vA4iqf`Kr8C$!O{RO42X>K;v*sx}s(ZV*u;7b~r~sM#k}{MY{@A{fVo@HL&Dw zDEN&!LjwcztQWOvR|da$R903Z#26@ymN3N$wh_I21X_Zps5Kv^DXv|+riTls1w62- z5m)xf)U&vZ+mWN|r&tEH9hR?l1-J#mtI9h3Pr@@VXeC@0Yc5?&q8D;-KaK{(@ar3 zuNXlynIWvBGY)2$srVFDh*1Z!e~(e=9)=9qS}0Dea%t^SRStF<=8dGgRb80G$y7{C zvpbu!4K+1Vk&lvQ6f+)udVAr{a20r?^{ge6?|`8>^4&X~PMnh`0rOZqlZQR5;-3kJ z(j>Dho}aj^1_Q05l2u86di(yWcZYx6>bE&Q^9Y59v}&meMpU zQMda<2enSlGc4oiV4K7rELB?#LSpgtYcqI1&ix6s2@*suFm0eW*BU~V2E8#?U#>Qf zcL9t$R?p;NU>X$UmV6H)CRvHIgoJq@x>~dYDr;4Ni>sDzZf;hyoo3&p;nuiQHSzV3 z`*pw;MxIyDM{ZV~C_jISa^?u$>c@$L&@E7Sln6l7h|uXnz1>HHgH&k2BD<}E%p;kb zFM+4s=#o%fZS5wY9b~E*7SEfM(+CKHKouAZ8;ABsgy|z3FC;4^b)ip7OH0ub=~aOm z;s~wFDuwW;qq~2@X9}s(K#G{Q!ys|^+qds$nkFO#tLS(Tg3!UCl(!VlyW!#K{?)NI z^pfH@)gVv<1=r#z%~34(P_VF1DPUn$j(bdEpym=TeW^Pv0z>d7*(e5!(a9$V8zkaw zCOMy7hb9XmleHaBQ7&Ymr%whJEh;JsJWY$9#H_&bwSLsf=DKw0PM)vrNEMv;)S?1K zV;Ug6q@<*%=yxdJt@xmkLD_V3b31A`TYkHDW<7ht)2HxtSSDQaA_Vez4i0KJS=u7;3 zeZe39Hgn;td*oY$^;fu>i1qf`VUy!WUTva2+Z*%9D|3Re{QMOoLU|~FPNByhC9)V;8&Alw{GekahT`Q-jzImj9GZ}PcJgd{oh~Y*v+c@NSg3Q?m%rV zlA7;?W(vwq4K?U+{N%>)$!d8dVNTBqJNJbDz1Eb`EwvBl-E zE&PNtap267II#~Y!otGyx1P98*@0_vWjU)UV6ls$xvKFs`qK65YrrT+fb|q$RnOz2 zlMyj7F<{%Lr=t@(>yvrdiW$ZGG``xT=H}#WP5OIJngw*4jaKGg8r}$MphQkPz~%Z6 ztBUF58GvjZa<40lUL_#;mqIUZNcW@;1_|vR6rLd>(&h+FrN_d=92^=_qSdfyZ*F-* zexnFT;H9M{kcj$JSY&g@fL?_Ify{wWrg~j1|D5L>Dx%z*BM;5VjVuv0cc~iI0RF4KTtKTKbP(96YI%1k#{G@&nwqL@0c2!kWPp5=%npe( zt7_wu7L=ap_Vmb+U~M?`2y7(MlMke&HSHfhY=K$u=}76g*BY9YNGaN=z+#d=zKsZW z;Dmr&t61rG#C9}Td$_ZeW*>7bUjrdUCF9Zax}Ir?2@`|1RP(BJAmHQ~e!#W{@3s&= zm%lF%dIACh9WCfz5{}4eL2$koj4=Dc#xc}ux4p{9n7fn$g9-!$UT}T@FR$`U5sU_? z+Aqt9$eHRv1qw_=PTtvYHRa}T({3aVNU;O2ek$!7PXZ{g{iy}B6g5ubuUh!!JPt?i zEXqz(wCJ&(-acBMsNSpJ4NeHbd-vdAhKIfM{K~}NK;L~I(4a#t&e6Oi;4g@@RM(|Z zSK;^*vMVT}!;8xn|6Iz)_`3?iTqUlOafcZ=YIk;4%jEj_wwIdQB-rU;98O$3FdZ@h zv@BLVPrm|9bzu^uOLAi3LU4G!evPRR3E)7oHB+ZC8c4=eILiy9y#*PgTAB$TVq#)Y zRkA3gWn@}mU;w69VIYEP!VFAnDlTUc1!zFh$hVg1BTXy6||j!wzpOK%glV7t6A zXO>66VJ!IE}&rJ#&E>=2&jBL1bcxb)bfk)9m1A0Qtz?FA*cEel@XZ_pOG8 zGR*Ck2NK{#odcK%2;dFd`wz*GShG7B8ZI(k$!}GAW7L~+KH{$S!-d#|JN^_Oc%@g2 z8Gk&j2CCSQ1H!QM{H2>pGiTS;Sl$y{439|uaHw%PKC??gfd7yQNpr7JH*|IKIT?h| z>Bi&K1lZlv4~clSP>mcrDTrC_0=ebVvHshSN=uFQs|f*$nEomBpu8jx43;QhBm*Ws z-WClouNie@T;StVwGZ|8r_ygZz$d8z*bp10cUJOD25!eE;aGP7oJsvc2uMKI`(bMW z9avF?NSlVTUZ0tRc7&4sL_X3EA;51^VNeXyzm$GO5^ zQ4BO2*v}bhX$1wBO@S_gr=}3u8xX-*_Rc`cW5VvDFR4+%tqQQ4U*VV@RBB3@;0uic{`$*yI9iZlOa3c<<8F z50dhq^66v(H!xbT@?>150!ErzT2O8EzVrbl^dTcd(g?9CQB!}hk$d*+Sv>j@CFRYC z+eXzJKYmEo;xJR`gjp5eM@AyJ4F#x1^QnG0^6 zVv#M+*Ep)P9DcD(6eH($Y_nM|8)cqQ)!uH`U0QmJD4=wlKu<;8f2Q(XJ7?p6yv3Wi zN4_ELxL61k=M2noLaC6S#XNaZ>ci_c5Qb8xCon_v2uHp-ZnryvuyRL@#WyI(3i!~m z;o$^ay2S)fndAW3*G4Ll5fMAE1n^q%Nl6n8jyu`QSLJL1#gMbL0OW&RLK%&pIR_`K z@PP(ewl%^uvKDDGk(gI4Z$b0Kk#Jk8^Mo?2cOsLCPb~p4J*tnWoYCs_)^*|xT%Vr- znO$W4_Yc3vzi~so8-|Te5wi#w@q!^e(5Zd9&_yrcM^CS#KNvOF7Q+YVyCVl#p2bgS8x(!b+u4v@yW&mlJIx1;kyE6UBWZgPXR2S9 zXI+tuM^)!V;^Z1lX>E0mP$fEz+zf#B&^oMN`(<~L<17&o;>tx2O>z1wUDJ2de#%Y9 zwT2vY0WPwI$^eR|q>e`KmQDgr>K_ch$S;mDb!YFncWL9K)yk^~qs*h@X$|PxZ$brw z!|aBywGGJVU-(5OPVV(TO*V%o!I6th zG2H$AyQqSiq~s4M^jlkY3a}~s!@~`^__(JkNJ>fR$xy#zXJKG z;eFcH1*o9P+JhyC&S4bL{VW{$_<^45e0FDwxEM(kW47fp!f>gYh!MBT{P)+NgOb^j z_~`D{haTJt2N}n(@WJqEjT6a!p5`+%QK_1MFrt7kQsScObY5Q6x8OS#{#p17z7#nG z-CmB_52Nqx?FTsY@ILqy)VQB~XAa0)O|x~qvRVTUZB5O0qH&N&3Ss;eQ--S%h|~W5 zewZyNTO0Jydjh^q7_p4SL2khN%^RFkr~K351i}}SWg_&QaCj1oO!Wd`OjskIE3U`S z0a}H3_Fa}X@OC4I9yZ;FHXQh03PnZ@dze9g!oLwI1cWi13nc^%|FdV1=AJ&y{6KG^ zXSy*#+d84Z!|PF>F#L7(J8MI=catEr2GEf{?Q8HoG>Mn!m==g3u<2fKRm0-ip&u+M zZUzwi>egwvd~4+d#K{hW!-fCDUF4#Cc*%l8({mTS@;&nbKm}`H9BvPkz=X4k`{pj= zM0p&e|Mq_RW>(V)-*4(5{4*0DPdq2MII?US`J8pKreEAaWd|eI2i^BT1l>4xdLro z04WHeLLwsBMyj&1D_}U0fP>D|K-t(>Qph(7|G>w~Thfd^jfY2B>`NM`0+lX%NBD8! z^y{#@$#-fWjF54h#Kz9uI+2CVt#N6eXK>$yOD$!(zTe;{IzqE5F-I#vi;#PdFH+!% zga>FS?=5d-jXW{{_L$|QQ=s!+m#pf3<@|&Ci#4^i#b6ExjiH4$II$Q+A!f~Lyux`n zB~FpNWArWPbhI=yig|Z#-v-X&Jb4>XTSOxa6S!oP<_R%QzuHnm?{In4cf8F;{V(F) z1e(gd{U6q-38#c86lL0mY*SQJ#*87`Oqnx=%%M`|nN5Z$g|N-@*ks(2%$YN13}v3) z>o!#9dCv2De*gb^*ZZz@&RMIo4rlNC{tnmmnXXOX;$I$L^Q1Y(wg5a(5V-L5>#c}0 zIPEt*eT9N66I>Awp$x5l>kQd2&TDgRW1P?=T$hqMOhg0CWf;rn0T6fs}s$&3)KMM*vvl8Iv2jy<6V+l@9Mg}`) z)sGG^HZr;^K;t2E&r}_V?!ds)Ra{1&Pe-7EOHP7!#Sn4_#5JINl@AD21aDAqQ)oH} z%-N}`ZlhX6cwO_YI#`5+gcukYPM?0O9geKvC%wh0Z)%Ju+zRScHKfn0MHXc=ul4*w zeRXS8^bd?4J9>tk{rE*Y1*WtxEJ`$hW*ZMkecE_?h!wg-+G(z_?*~$f;iiJ{1lX5c z;1MfKZ9zi$pba%{4-*#zX*V%R(~>0u9snDbRLpDM^?DPA`xFf2ZHGyEJ6{9kNh|VA zylv~?Sb4N_%sXDIq@tiuhE1-ltPEM}Cr_$lFb1ydl;I?_f?xz60>z6ESkVR{jyi#c zz9|^0o0?&e062m98sapD@$BQfjHvU#lDfEPU;AY+tN|v>@wbd>IL=Qfa(E#xjFMl%-J2-$muDt~+ zbAXS<{ix#0o?UXt8p;4z{OQvZ4wV3v;*|?E6%_iLFQ}!OnwoH(eTD&N%u>G($ErQR zpMmm=rBQQ+=Q z*aEuqae$KG#Wj=G&_Gh7wwfA?T`34|f%+O_PbIb)WX<{kT3OH@)eeAjH7h&&nLiQ# z;mynVUk}L3XLOZBLxSOw1Vl=0LjyL2rnI!L<42(En>9!3W5IUmfhxukKDYS-K9+@v z_Loz^$W{QP8{&;j6!}fRoR8S}LVF2rLx?q&a7LYEU}PNMdf;%{=(m^M?R!+$IX!*1 zHt*xzSM4nra9V+E*v|M=DrOmtd4>TJ{O5Pz2Cj(pEtt2)W>Gm)rl1arp%f^>ih9*| zST_^k@u)D8CivD=I$q4j=p@r^=rLU9kQKQ{F_4(1=m>b6d&c*G$|VAeMO=)`x#3E{tEG*GBGhRFc^TSdCUu{e0X56I^5jcfNoeokgR}r zw><=`$ZH^!PfsX|&!GsKlLl<&8N*$h!0Xn5*tP@THj|=T0eZnWJN3PP;^}!{#oBp& zX#~9fpvTQwEik)v>$&4c_-Lp6Z@7lr#JAoCFgc$xYW?TP#|TnNN<^Lmf`24WFHAB* z;y#qR^MtkZdt4JhUyslohJ`PLXi^yj@=9ISiax2^L|PfQHihz}oq%Hlo}6q7+?R_B z0GghDIqk$ZE~n~O=G%hZ`;si_uvGW=)Lm2Dn+ylfV_z~NR6I}oYl)xVIi|{rXV|te zRpx|Mk_$RW^{QCcp{TIu&yog;#RUNciy9Sxs6f;C0~AcW+}!t}2_}v|$;-n&)yR*F ze?(FPde?AP)!DauNUHN7k0MJh zc>yDN{rW*G@|x+?WHeSYa(aNT0AjgznLd!wO5j8)rd0;q3~5@W z#@$_5%z6=!NYzZMJ$1{97+unG0D8^-^($y>eg(YMLbuM>E21C0w{FB9f93gVH=0-|PM%U-|s^z(yODR!+8TJ_l=Ljon2>&BDjWC%%2?LQB9E?ALEc#8R*}A?4-e04u9+ z6lJ1PS8e`092q?GWh*-xo1er_EwmEYMQ$<&%UPQX4ttJIx%!Cic&h<+`(BDih>)Fo z97^Y3+(8nf9uC+SC}m)So;h z&eq>Y)iuH^pLYKu*T#3qH6Wv?D$u~a!k5MUwbd@v7(eiRw8MF{^6BG@RG3p)e++xk zTrS6BiEYnpQ7>@*{fA#lEmH{rD?dSYmQz-y{6pYgKjrOw}X5gNpFT_2gXiq$Q4B!IF+mf zEj8QwqUn)(a6MFhaXqxgSY_;&cw)R5-5MKC@uV?}uEFsJ>erD2+#PJ)&F&X^^y1&7 z=s)&O%+)H}AM)jY`jG#DtdYE0xxMXxt~o$WuzfH7Z<^FYXw0u>{pvS@YE^>&Ep?|5 zm`S(wvcz@&)64p2dKLS~7XZL*Nc=AWg6(?if5UI4dDF{#(&r_53eon!YrC$OAwGJOUD7KL6pI_-7`} zH7v7iA*TA1IfnoCqc26dsv{{FuUBV}p2C%olOJ07`1XZLQyF&XDVw^8TI2yDGAP(a zn?%jb2G~Qc-(N``zG3&BJSj;srCcQQ)eDo9 zDjFTCnVG3GsWzgSbaFng(U^&u>92za>CTEAp*owKQjs7NXQ5qjfnMC33%I-fAV7h% zvo#(QKZxGJBwk7t;~@Vr)6VIaZ7`wmgA|C@f{A9 znVw!pJ&3)0D6mNF3KrtzCdnAt*5(!%ic>C3;&rEqD(dW6&t#UW0U=xIBQ&^@);6C= zDo*q0>ROYoT-P_A0m?K?sEfGQH})pT>f;1TM~|fE-26GkHa{V6TZ1Vq{8ZfNK$U_K zuBi^{Z)P51%{KIzzRPKRv#GNtB+7AdfYyv}8&$4WW&fws=X&E=WD0k9fDAz=% z`mZ6vas8l^ZRzkOY`>=+U}|BoQg6M0%^b@KPHZw(Qb+NcAD0tkZfp$o$j0-ljnUJK z-g>SeU~_EOE=f{QvydGBHzld&(=(mLyZZD@5*nq+%vsIGLq)zyN@aMu&d54{l9N1=p`qLtgW^BH zeYo^PX3llZq{-_ZJTEOw7!ylY_c(k?*p1)dhg|%*447h*iXLu#P_a)|NB`k0{YPk( zO1;upK1pyvBb&(iVA5P#zs}KX;C20W z#&Rm!YPIi!&f5nGq@zC*xGuFj+{^h=tjsSj<5yj0cJ+@hLtdBpfAv+gD#vc7Bm-7Y ze*HS0UUUfD9J(+9i|)+Tz598o?=(eyzz7I52z!(}FEGp9;)xLHE$e?mA>!=ZbQ_D+ z`FNrs-X}{LBx`(=YQ-4&to?*OgL;|9O1d7ox!#RAhH^)b(J3fagt67hrnW?_f6y5U z)mP3Ec7Svuwa?hn)EufJc3rc95&x~XAV{wGPdC~>zJ=vf-xiL;2HWlQp339@>*Eut z6zFtCf5}b$uYXYPl@Gu{#GDa(uhd@MgpsG;70Brk>gw-9813qZlMA~SiS&TIhQIZW zcZG1i+5!3htQ=+@yXQ)YJXR^V!8N7GG_OA-+3=DOnB6iCkn=dxU97mPx(gwCt^UNN zEsFn8IsUEqAwpHG=&w08q=efKQ*KFUw0%}YY`u?W?}k4T48HxNu;{V<)lLOvF5j!E zB@JPqv|nv+@6T=9x|yE485TmBsl~3I@BKxE)7s#q^|h_vnqP%{xaa@#hojc4Ga`1RHylGogC(S zwTgHE7`$XZ72F9a7>P09AtJ)3+NwJIeEm9wRA`+^a6_YHC9N^a4z9YUnok&-dk=ld z0zm#jeW6zLgXBzt+Bq&o=Bes~APbsy-qm$t!E55Uu;KP={zH*eI{B-+q0=?%$Pl}H zYr#AylI5DPs7{mA#y1=Q%{ayb+ez3ZZ)T1wW7s%tD8 zZ@ZMiUA}9VAJ1l~GE|s4o11atdvj|-U}|s(7Gu>q-j;tyAZsb-PPNL61lZSuvNgDI z$J4RJ-^uq7vSg8MF9C042{in-B`}tBK7g9kjS!VSHy4*6=6jNHc9sgdVufgd_)EuZ z=1q^%u#5Q_J6?3ia?-;cJlIdWVT1{ZWLcl3V5Ih^qz6gcjG1L7-JQ$#Bs5B1m*6LZ zkA>^uEG^@*6e46rpLqI_>FcM)ej07^enh_gcJFS!-Dk7YA0)*}ZBmM@cGg!pvc5>N zwpT56dEK$6Dr5KW0;=3jykZIRFRa0(9njv918L3gILl~_3ul4iUzpm zSJj4Q#$_8l*F6~JcuR9vZ$vKV6{sAcoeG8@y*4`%q19Cbnh!G z1?;86LmO*C4aNDhwFFbO+wZ@F26_J*)rSz_Rp1`GzxbqX=_LJlw~OGOG+$Iq3_$A! zip(ZxH2VJepD98`r>Llav*U&sKtSLfjBd`$AgTCZIwU@E{ww4meCsJ)K_=5dj#cCO z8pr!!`M|3VRolw8l$b+YXobD|S>hELkN)-i)M-9eajiE!xEFQm`PC{H?eegh_{iFD zGpCH_Wu;c}cPtjBym1(3RKC6+3c;PsT3P6JQ<{h$mOyC5ySma>KSrPfv2>z0(!xzi8Hk@fu{xlc4 zY@yDkDprgtSPdw>YTW{G*5Rm>KsPQTtF7 zUuG29Javt9yBGx?_UdhK$F-U|{JV6uToypsE?l_4$!Q7UZp0&VkZaiuDw3l|j{2ZhhfQCy}H_rJ>?oy;VzqI=t%p0QYM=szOxkmxP(bdt}y?b|# zJ0sydOn9u$zL?Vq*ra*UD8o3_GwVD0SBcg6qdjZ%mghSys9hp?3CHymp#+Oi#(<|h zs+E>Vs{7j?58I^4_C{cVV{Us---jpd6FijT%}F{R+zohqD14m0n?3a9;ZmIFX!&V7 zBC9VJHn~+m6|XP189alPYXUKIFrQ2?)MbtETO;y5=2ARzLW73lB8& zMUI%izlN%c3NRnHwzPmVvb?e~S%}W`ayp<6fcY;%;u3fuT@jOH6^)bhWdq2f1czvF zT|G=>_$*8UG=CjdvvtpB0aF05fMgBJtWoBdbyiGFOhhI*fk_=wl$Jn%2b__fK0tn; z(1-FO1ad>Vx-@Fvfnzl!Bp6Pzxhxl;34xGZ9%wM$#5!~f+sE6|g&bZ-<@iPw zxWnan$e_q~DNn*kHyH6W6BkmZGkhQ1aDbZ8h{?isWS!onZGDu={$ zf|mk9*+qugh%bist-#y5H>07Z8i+xy)B{NM0w#=NNp7N|vV;^EhXMyet;5L!Z3Bg^ zmT7sASypDe)lBy%-xF?wV#u{2D}8)r0abra$pl}EB@8nEa@yFWB3=sL=|$QpS6Or? zSDv^=MwTh-rV}QiV6V3`3ury9k89rf;Ju8GoOX~yZH6B zJV$U+mG6*RPvT7X()I9t^Vg@g3j2(GTK(o&QvBHZ5 z|CAE|!jmL0z;*(rS-dL45`~0>%vHT?+1wlZt+I)Th`^6r{qbXtY4w%S>5KrO^#bt4 zjdcQrkEcq!eG@g&AuSW5nQ&^X6S$ea4<0GvkcHEju!Ia>fazzcq?J zmBx!X6vD-58?evr#@M*1-mArlYy&J*7_&b_#oP<}_5;>nByITVW=wptjd8Z>*>lV! zkys%4Wae^n>G+g?PB34)CSsf%$DJFNnVkCSYHKRIr}cHaq2u8k^#8jz)%xb~4$HPT{mxsZGOq}}SO>L3Fcm^!sPG6bcWyJ)_n7T3oBps$%(;!x(TdOBEE_xTK_ptSBDDSPuNI=B029 z`KuOupW#h)b(D1g#IvK>*x1gTxf)s9hs(`kUZK~N<42;uK!yxp^OyTqSZ05~@S?o? z{UNUIzVF-E?{B*!%*4Ypm+e&xGG><2C-1xVYHHTJdo!;_xG}I7sijZXo@0E4fa-`& z9`15I+%KhkqyK5$Iy^oSLaxAO-v0UXH29k@9)H!*Tu>l2k`V?l81PdXFt-S1Tb}Mj zNHr>cY7fUq5SFAUD6h#WrfwfHdkR(I3WH%Ak(|2jUG|VWfl1ezlswULs2GuV=72wG zq%LX!(u{?B&C#M@B40;CY9Ks_VtaPD+fyAtRcu~5hw-p(z$Z)u+`Bw<0nl5JWY6j2 zW);OM$7=sL;ZYTCXw3QrC|p4S{x5sk&bwrC2Ur#h&+J%@o&q!6#7IlL6vZ6j{Y}~f z-3rp+srGE~)bP%^F0Q65PGJ2~>EVX62bVIChiQ=B zX-xH;mNfUhIY{H+CT+=|oj@ebvO;o2LzZ1*fQ|>CB8Z2%QP8i(I|w=}B~w7ormJ46~Dv@$~CN zthCB*OV@S9r&BS!q(Q!LWW%>Z#*4VX{4)qR zf|1sBvj@~y+%~tFjRG5mhk4`~-qzx(B{V_vn~{R@p= zWnujJIk4Jgt4R_7l~;=W$VKZjK36Ke5#!CnQ(wNJF~GQnhJvRsG%zs3yj#HI2dLUb z9334&vuA2L0YN-1fAwy2fvI*6LRQwmxP6l$b6*3uqIiPz(Y`inA0F zC?7t2ICiS#r!Ye83T#ox}mEM z@QHn9_iu)$`tnFF$ga+vd(u)3T+XIbs3!m2!(VuVYxHk_&G%XrMFkOk{cODVp_v~X zc{%xc!jsb;w6Fl7qF9}>GY`pB<4w~z8tcMR89)&rB>Tq)ffhN{^Mx6lu8*RDfJY9~ zyCL+j+4vzyeSI`B7ExLJ1v25U=cK|isJ0BV;P6}Gy>NcpNGCaZz%}}SdQP<-dYiFQ z*707@$9*ql%*u{Zl6`tQ&)lqaBUuT*=gT8EY}@;5g!K~Eujex?^!W{H$%DcYE;Xkr zb8tEry-v01TAfg}Y?8^Fi*;t0dZR5O;%VLuXq8v`8*^R?3MWCkv`oX?i5@luMXbA* zm(IK0P%7PQZT4hV6seB^ME+Hxk69;GaqtgGS;RzJSnwlzRhx*^|8(Ola zbz!oyqAK?GQH;?>)ho!q@YKZPqoq|{O~L-ab5?bIQM{9A3yFB+xuR9p`7(sXsN%-j z+qTBLqsvVy=X5JQIm+(xT)s;+BPAvJy+@~>Mn5M@kgsjVVQ$3aG87sob2GwbKV9r^ z5bq|h3d%%er$?Ud8TdyNDaPBiR;4rTwkYD{`CR$5k%s@au-j8kVLbz0^CKq>QYOFT zrCwKB`s${$GH=jG)PBQ&xRuJk@58fN6&tl@7aDH5u9tjk`0%ct+;!G#afqXWRPNiT zTG@)Md?0vc!0{pE362jn{FluBmjRJAh9MzXzcfv(*$RJhB$?{qDe(YXwEkKq|0SQ+ zK)oQ^-j5M@@g(yt7JpH+Yhk#XCNb&JhZD)Pi)vKt>SuM8Hr6V!Ki}T5$Nq5Ma2^?H ziA|piWI>5adea@qE5TZad-)ajUqE9JBi^q6q)?Td(WOjL=f)xEBSFUr#SVJLXzuF{ zEMJCd@s;IXc+zUqpOkHBUT1{MBe*1kPLj7g>2O_`!e?LmlaH|eB6;qt$Wm$#zcrPI zarfj%ENUzw=JLq^Q2IM(;auv;&5N!d4NmqD6=PYeZcGZ9ReyV|@XYuS!63{DEwA+J zPf3p~g1*$?hqve5KOO^b4UiL8hEiC!`bC$c3#AVE;zCNFyRr9%Uq+5~()`2wFDu>sH$Py7wmxpEF@p{<>KW-u z&m5#oVRc;&S?vB^ZsFSblN((Rs&chEb=5_JD5(Z#m!<3kw(TkD9FR=DMJqhFM_cMc zyt7tck#9@I&nu`B1AVVJI>I?#VPpB{;bFc$q)JM7&enHDla{BpMAZ-Y&+m9g=axkR zGqait=>&=bD)v&h2hhW}Oz>?f{o`APlI{?kWme6O%D`v(K}F_$MWhUG=5ziWr~E`O zbZXXJQJ557yh>AJ=m-`F%m5*B(7Py9vefP&@l;Bo%CD`(R%Mv|(LH0A50aKY+#~51 zeeLQ)sL?9Z`upJ`Y$}9+@)4Z-rG>7_>4Zp z*keC<`ja~?um~q79W5;MtLi+GB>GT`oF6a3JHTl#K`ai=n$45owk_)%2>dvR-uc_8CFqL2c{f{N&D9)VmwjE5(SL4uSLn$uH_hK;KoVuZgg%O)gu|lC;`iaYGOS}53 zC4xd;USer!#0qfmui1=8%kA6}wB)$06EcT$YUNuu&Dx1%Qsj7?{Ba2SsI){vwLYbo z%REY>X9UacF`&l>fAb!Y%g)o|-lrDB= z=1kf~$Y5ntQ#p(TCLj~ixZXhv3?m`63-0y-K8iSqWM%z7WSht|^_;%smrJkxXJPpF zXZCQRqxHW$H*wsjbEaYM? zCD}~F00jzC(t!+Qgp$zmH~P6rFvtVeGE@l_2bk!bm5)4)hyWx}cFxSwas~{ilO8u! zRU?9ft=5)pYLTcC*V!6I0n&+%+>}$&K7xYdcREh)g@Uu}SJ@B8+&&{R^R{1yE25x5 zTxp28Lr!l&A}P&$etK@6b>x%R!#x%4+0`n3{X6aF>Ye?o2hngXa~K?V;g?yOhlkY4 z1M`Xet7+nv?b)o){w^?c>ZgRXOYdZk-{ga+p==xaDM3Z9o?JF_bL z0s&;V{ocdIw+>LEKNXLp>m%#ok3qVSyuE{Eq$1CLeFCuZ*GmV=&l*T6j(my`3sU>jTplO+(>lm5*C`Z!)fJ$?PU z2(A(EESee_Ekd?IDoI8UWSeEP9Lr^(ql*KVb`(80n+3r9P&k9cJNA}!kDh{8RS5BU z5Fd5L_GxTvZ`_>@T42nN9O}~jcPW41KcaG*%sqJas;HBC3)-JQe-1U_*vAy$7hv0m zO+t{VQ4KN;-kdjTa;1LLsAlD;b=|;V7!a=Je9*v!It!cv5K~w~14ip&FI_P&7HkL1 zfECt>3cGuDyQy19b#PAsNS;Db1ee;{6p4y_O^*o~X* z;K?*VkdntwyuRK#O3mgnhl=O>o_swg!Fnw8Tf3r0nJ;vg7f*bCH&A9!{bT4#SJ%>|goU{;6Ut{-wC6<}=Vm%q)%cciUlmN}zy>khRVGQnYoP zJa?;dj)m*!n5TO3^*Iooq!pJF(lV*OQ`YtZQ4l~#?73)3|AxxFdp7Hg?Gu)1;1mFl_ofTJYgCT8JuCp7wMUcX1qwdd zkthohk;hWvdE8YQa_2j^s*G$N!6^lO{}fe5qNOzefFFyHk-9RYK|Oh2pHB1Z zg!~Z3VsX-tz|-q90R}utN&OtImW_Q+Il=8f)k{u+@(uwlNQDm`x%@1z>l~wCWI|WuNjr{C^SIg7^R+>VvOE~A zf?F%Rm%1)To(Q=vU-YH<{zhpCv0RL)V^l=a0UjI~;|Gu8vz+o+rmSijjn%8SJ$6W5 z@Tpv>@?S{}&2;4Zt5~@*nFo0RZEwia%f=dm*y=b5RK|Djf8N4urqBC1UH2U%;RBLHOW)9Nj=VC58hndRJqnXe=f7F6%5G)J-R@--Z26;!Y!n(TdNn8-6>FYT*t}&Dyf|UKkrGy?6(U zV)s+sF99zuTl-=dPi)1sY>WHcF8s@3re;>N_qjaW;a(3*LlIyhcu*66`-{kVR z_MFSKUGwW!Im0!_nXhhJcN6ucsp7M`bGcW##3)nh?&Sjn>bxb1@G4x1S@K;t>X2nJupFh$6z(c|6|iHn6>ESP;vb z3e9~?kkOePG~iKA^Te_0A0Z>_Fa91%ODyQBb)4)DUd6{p)D&lRcT+j0G+CW!)~QH1DaXEGcDuWQ8-w(7$2qAI?RPPg5Sxso7K)v@)SF+_=!z&{TD-kjKa9(F!41 zD8UH`vNYUkRO}{hxajho&rsFZk3YdBc5jqUnnC|%N!R5u%9E#P6ty|%=qn6kc~Vwq z#Diiv(CBg^KF_aTD<6-fyS7})ZD)l;knlYmg6|WSJJf=rP>%D*&zzy-Q9AlaMM}CE z(|CHkHP;0>6s){!!vQn~+n#`Zm)-TsF_VV{erU|f=!IyhlZm9{1{W!m&!)nTJmvp_ z+RB%g;lC_1_rT1!+wnOG$H~0b*DAX0oE8E|VNDP`U~Pk#AX*MI5r(#0(oL*>D0ajLMrH@&CXy?Xbqb>(kB9D5%32w(79n(9L&~^)ZUk@#kMC<>gPbjEIZ| zLZ{FeAf30|gVnNEE629-y0#0B#RB>*b2?=x#n#>6tXSzS?9rNSpDDM7un>^SK7=49 zp101KHqeFCfSqY83P?^b@wDz-+>RA-Hk|Uxz4y5UTB;!gT}pe9xS7v-n{_u>5inEP zimUyFvR|6%^yf1s|2S$rCUyw(E6LD8K?Y_6-fmY^dduU5s9;tI{-x4~fT|N4Hx z{~zIZP))R1Uz0Nd86uJ~rQro@?hr6^-*#Fgc-53BX2QU#-5IYoLtTC)ekm*>>ufvc zDv(s6B#Jc8%^lVYY@N4Q=)Dr}CRY3WvP$@p1!e|Y5ja3p78RoN{hgNe1li}CiDBSB z;6GF0!+(3l=>LrmgeA?3`fnfT!H2oe#ws`8YVttD((qxnoq9}ib9HVfRb7*nn30tI z5`&;ZP@5Ad_X}?FbsWEP#LyU^H1NC3HAuj3JiZ(Gg4axT%7!Ax+NhQFRm(W~eZRus zbmMbeWxss-pME2UF}r$ZN_Qm)3a6vZEUM#nrybt>kZIeNrBq)f^u5<^j;f8D zF2vBf{B@YZd)FT1@SEzcC|&TLwbI?#_4aQr2uf7^n)Q3zR-|Kwd%fTo!Z}YJcWBtL zrE8L)^1G~Rffc_4!ZGL)?u!eLP7{_ni$ssX?e!@=BEM7otJ^UZJ>1vB%y4RdEh5-l zxZB+gI2#gPdE)T~STeFW*NxU{l};5a>xy(&w)J^F(fZ}@u0E5U72g^V?JMFpc3nWY)O@tsd2$T1ht5I>Dr%a)7q)N$8fc9ba zqg>H7DNS~tLj@`O-#&SJ|DDp-F8?o1izA)?zd9{k`iKRpzn`!w-2?k?v%g-(?VJup zG#DiP7T7@74VT;m98S9R=FOY0dA1_bwC~=%3!E;@xCEbs zY*yB7q`h;JwoyJa~XHLIKB4C$7(*{scm#Uo`egUo~}1g@Ue7 z5|YRbreg50t3y~349mc!GLi)T@bmJ@{D@4Vw7PKkS?1`G$_|ZX-3;?Dl>u_&q&YP8 zkVp+gD2VHld4t9oFI`#%|5dfhyQ=SGaEaf#zGoGVz)M@{04Da7<8$Nvg(Hz>ulbVF z|BWWH)zVPW9ZaCv=7*lS5g>YDGR^c{04Pm%=sTBu*kvMmy?rw@fTge$Vl}^g`v!g1+U;UNt8uC*&8!h5 z6cs$sLqkK*kCnn8JY1BBsAxo;LA%4%s}H+ez^Q8{&_Edlc3b4l-EFH%{Bf|u*Rb64 zC~vTijRer{`R#W-TW`ZldwGx%F26ADw{q|Y>#gFwGAt9&eo8J81A_s;@A78xW}T7c zktBjlc@`E4C80!iQ>F=W)Ph}wzDkKLUZgT}BlSKsm#OlHW`crr#%|6GN8uGE(zVP4 zOea)k8pLHR>SL5>N{riSc#`E$r`Y!21bYWKmR@!Dxc2^bll<18qyWxzAm z3DKR-AjKT(gw%X6*B72~QZKmsqKPpP|ACxC%QV#s#{6VB4kEF3&Ka!_sMm=L^xKk^ zQXpPIRYm0lHT5JuLzA}9-?t?a(g9$mVQrCDauc??Ffl&9{azdw@7lXBPo)fxOf4_o z6TBKV0|~kyA%GM-^&Y$`B>k#RUUi9_1#8c#PuIrJ!%5okm=wweDXOnvEB(5i{{a@H z|Ia);>q1T=yD>`Gg^hE{atw64Uz|T3ew_MDvjMu~c6!8XRet`EgyD<)GEIxVbs_2D zNyDi#TJB-PoF7G+IEfAh9Y4lzziVTIHBl>;QYx{t_g-ZdtGx8VmGthu?&$Ghf{{Fu z*!t|4AGp$at+rB?H@!V0wF27C`sxX`@>FPyA?3N@8D*}HQ^}SNwAR)qzvEVOx?V#u zVGE9hdq#?XUDypPvga0oEiEN#7iEeCg-RHb;mrQZAR7(*K4>IhG_kfAxZ@!$Iyxd4 zlrCWVtgVKeU`Uc>lM@va+rP#^?XnsyDyHWMT9Oa3zWJF(pbi3nFWr%804-YDIBWQg zhYDwQ4)8AelX*rBbjp#Fjqqj?(_P5O~en7|KY2_$9r=oO{Av^k&xUs^jP)$u<~sJ4R2Q719{DgR9^|<4}~ub z4*V1}*`ra(@?n0mKS&(hPHj1TuSfK7RE4Y`v^Q8>mYXW=tNi;vQdWi6=xBn}kDEK* zC^Vs=kr-0aEMw@(g|3y1K7jYKJ;dvNs1dy z+tjrb6$hj%1#Z240Lh?}0LX)45svsUL-JVpfdi z{`6HTrOc9mS@R;uX0@k8W2Rq!@PaMNOf-h`306~F&&8#iRiiUl%DrDDH!lt9EOT=Cd)VdRq zo;!yji#R;dDieLVGjx5bScdEl@}Qzm$*UMSwN}JURN+;{fV`O%6r6rwCD(EQjhTRi z(r@km!_$haleDp!d&;Q`IB2scmm*Bfq=fmeFx-Wj+6*1RDyMH?@F{|;O0X6t=fPA> zomj}g)=*T`U6Puqzx2b`vB0z){X?$tV)Io2lMHvPMy8IahDH~Ji0IXVV7T2hn@6n& zFf$;>UVA_m@y+qv(a?9dZiv?yad${aKt!i`yS9zC!W`N!558}Uxm&S$Y$f$Q`<1~` zL|$5QrIJnR#6?@2v^+g=@dUEOK8M*flBpF9qLUuv8xyRa`K*1{<~8N+(t0)LT9rDM zmiFvax{hNCwThymfGs|ij?2R=g<58ta-S0o#WEL13$WIyvJ*Y~Hr=Pw$vXqZT8eUo z7pYF;SA1eN4pn7XUoIQ{7vDQGx6bSt@II!e_+DVm%d$%3X}%9e{^m5s%urQ?n} zukb3pl8+Soq1QJv(_hDj^_Gn}u9%qjoM|P~Dy#u$%aQ;M(n}GT1=$dT3Y}{wE}Ba6%w^YFmOL zT-6Zf2LXjjkOu`+k=fZ-Spi}%ol<8&^5!PBs=4cjwdDI!{TPSgx+vk zRuP|x4RdpgO3PucyN$e_GUxsnr%-AYqWzWwemd*-Ek_la^p%u=+-XP(vBHS4utdt1 zu4cvQ5fPEyxuY}LQMtSRnPO-HCnp`Nw4S~fn>sGp785ydkeOAR#iaTiXTG*feq3J& z^9c<;I3gU!Rwt~*zFQm#Y&&`6?G)q$K<_%ayS5P`!5g0BkQu~3e|}{+ICehKS8M#FLoog&<3;!X9OgjmeZR)5;R7&0^;B&n)KsF zj4h0eU;}kS@+w6A(pkin@E}o_Fw$z|+1v=&0gyRL%kz^cMkUB+g~AF1HEX_8CJ%GP1F5HjHvTndo; z1amTAkYlVljIx6C4=t7Zp#OtCKY>i7T!q5ZWeAUjM+(jZ^&Y!kM|b1~;NJ6<PuV0ftLx@o*yAljlsHR zKg4~!i!QUD0i?1_UH1z27-YsvL7ts{fR3(iqI856FjUq>=RPg76G{$axir|OA>F(X zJSX}^AC65(*@{VpYy8{00c-(2TopcD-a)#Ijeo~_T>@74z~tUqQ~h=ew;<)1L02h`bo zi6sj?PDm4_AE)L7sgMwk45LlhH407(>6g5EC6dq!L93AJCGXQw;*c%PSp~ymLn9)X zg&+^*9!xocQ7dd}Io0=p!yBK>?`<`z_dH6EM1%2Es7?xTGwNcSlAyHB%WKs`pJQNv zEmHf_*Vi{PlFAUyuXEzp8SuMd{akjK9fb0i-_$+3tX8&WH^fSx9>^jBrY7r@-^odQ zHzZ4co-@C;ZuMcet-}Y5+KX0zUm<10C1b@)FK$mBCp$;ZVQai_aPLZnWA3+B zA~>V#4}TRduJ5GTZQ*^t;BD4Dn4xg*3<%sN2ipu*MJic@km!D;PzMNXf*7*8ta(_a zh;SBclQ+2`&Wq9kvdtid$d(703=gql#;dT!#vl$-7LI|40eH5s1*s5|C-O>}1Co@I7UDnGO(D5UFpi~t(8KMv&aXnMfTR~c)&-9X1_i}UY{jY5U7UaiD?gIRR4 z3o(mMTYf20I?Lca+SK`t6s{VW&D3FpY;|zSf7llmk0kmZPU;b5^fb6)hsM-E6Hn_f z7=+C3O@FgqRL4wYIRedXiT#WYrD~2*(8m<5Civ_?&0oNAa&am8`(}rIv1_-XGy(EH z8{N9S4qrAH*WmPv#Cp#mL&YQ{s=*!=RSJO5u4yK~I2yHSNqB^xFQ|6*=;Ol398w!E zJC&Q|d-+pW$hpwbIN#JL`L>9TprK}yjn0q^t>+qV1(~2oI6EEp&GITA?RABT&XIxi zy?T506Iq>qP!-_w^n|>!hcA{4f;c*|!@;2Ywf2Sz)AG-wc0KL3v4}yp{_tn9?Xtq- zS6QKlo4gV6@LRbuyN6l@Y#x_&`r%X+HaVD=KtVx~qS^z=zi{+M4S;w7l;`=r5cw=s z*M}vl6TkOmIVs$P_>FH{4?g{=IugG znL?t6%kO2k{Z5{n3(bIKuXBIf8{7NsK^(Lt`*tmNTl-VyD79;@|-}t>j$~IYibyvd`!xoClJBt_o%( zPiAw$8{K>n1KGlo($d@!(%?v(Rjgzqi-Q6PbOj-?VGfWt0C{gPQWZ%vo#_{KxM1+{ z#E&Eh`U1IpwXkY4KojcP+O7cKy?=kr5RLKcfD~(PIG+G>sAYk34%W5_vo3VzaIPs> zgGxv|0h~NbFeuIereAF8;NgF_y7U8mnU62Oy@*>62j>{%&w&uF!+8Tz8R0spb9#rb zhqnaNEvty7Zq(X63G2~cReAf8;7TQa{N7FxtcR01YC=-PInl+Ol_g?dAaVWZVREKx z3O_1LT0IB#j*>H=(IPT_bo`|=VzSH$P4l%%OD2#CewNU z*q|sRoDpnS9>jjkCT-r$H}!ilOA)c@@$I>%4n?5YWNOYimEIOM;eKZv*v=?(ZF^@z zzX3Z_wcv+rLwYQo0UVn!0&Vf(zrKJZb`i#`AhER4b_x6^@Qe;YP9#j1f;9z$jdD$pIRw!D4$Xm?1ZL&MHs5PM(oSH_ z1*2io!-OE*5v&*0A4pWDgNc}F%}6+$?A zVRVd%Vdj#Ud;$z?0b~+DECsE0NU{R^FlfPQXkQL7zieP)430hsRjW|%EZV_s@L6yy zZ#y*QgM;B*y|>O+ONfd-aU27)8+agL>{H29(YRg@7^q9TiI?gh zyv}JHs*>}11`lt4W&j`d+IqwrGF?nnn)(VX*^J(T>_b7`BQZFnLTKX-HW+)Z$E7%8 z%J;+>!4=$0#%qb7oT*Mmc8wc?aN(Dqqcl+nex0mPq>J_{7m?TMaikpgBHgr{qrlpo zqssx-?st5B>nmgdj^h}+CB(vNul0bpW20syaVB z6Qcb~A;KC4y@O)u=>^MvXzN={f|XF#(ecyR#Cqc^J{z!7M>U%e}ApA`-LUQ zbR<5$w&*hOr+oRMqACle@@lB1gJ0n)RBCW**NlVHt^?G0pqxta5OzOe8BhP|P@9n} zjJTP|TE!(N2Tzv391I%pj~f)5T311(e;MKdA@7m~EEiCS!Rvx33VRbFs4)-R2zW%@ z)Lu+YiNf@_?%Z9;@$bRU4?%bL?8id0#FLir163zU->i4cpGRlyBU)@pFMt2UUykyK z_x)Ceq1hpWq95xkG9JURa!MpV-sNB4vDc_5<)$3TdffLxokO?OUj7a^=Knb6NZv!t zS&^Nx3*}p9!84n+-E+Tf42k3yhU6HOnH(uw1M`Sr5MVl6_7@oYZEA{htb;ziSrK%X zqmA)CVm9Ed23Mx-Q}z@L2~@U`5fOLTk(s;0`kV|j>D{gmv=A14`MdPg+&uyB_^(Dx4Y`l^6 zH=RiohUp?iC|wln%zOz9Q9^S|dtR@vqy3O%Q+t{wnQAHmc2$s3jxYVd6vo8`X6Es^ zJw|#KzK-MExV;u0OFu;O&2{FC7nc90l~`o=U2E<}B>-U)@yE9Cv7n%!VT)Ku9c+fs zK4T3EmB$_)Z{}Ln=SSgU$h+gN8pk4a@&00lejl$#+P!ksKB~#^%S6lC0;`Kk&rzCr zqboUgn)LX=2we|@H%9>9hOL^PV4K`>K8YaUed$N=VhgPg2$>$!$3u4Ws%oXNKftti z;U0F|PgDelM=A8Dg#2)I;V$J^ZGj$;NyPFIDvJoNx^bRdl4nnbYAZg!$h)Rg8yL+h z8zPuaq91FlDgnS&#oIcy~givMgl+tC)z%bmUJ7{}el zU{NcQ4)F0=X^LIf+vxU74#iSJ7V+Q++>vZ-J~_T%7=&p(Mn0j69pf-G<)m4fvmZKK zT24aZd?GU1K26UuZzx&V@NL3HWE3SNpZpYq`1{b1ke9?+GFpZoOoZMeC-IE26X>(a zsi~b|>IRQb$s2gb#F~IIeaU)zYI`-BC|)`%0!pWr_IGYi8onK+9Iy1#`Fz^4?@=oc zDWmtJ-6^5^wdI<2xmPM9pRsxSL=pGe3y%iz_oPN$aLOBPF8;1Y2E`Hv3nlzx1F>-=kP@|O)? zj48)=Ai0>Jpc#nSSK+1S;>wN(u$kHtQy{i7v=3guog`nL3m3i^yu@Ikh=CMaWVnC} z>jS>a-q3CILPkT_Q772BX+o=gBx8NZY_-C|JDbV=k;oYc(CPLecspVp7B1xHO4Xve z_e8`*MU8)!M9?N)UR&S?z{?4Rlael5It)*~1+xH9M0uHua9WMVbF07nNJc@y=zOC6 zD@cW_7omIjE=jLxx8JmgzG@Pz$zb(#6&0-jQyc^&Tg0}tPYay9z2eBTY|c<% zhRlh-0n%`*8NVD;DYS|&{TK&V*4ES~7AB@|pcYTj$7OjxefqSd>jG((PU{2>kzLgDk5?TRyB}c=4m0_Iait??ZgmD#iCQ*Xk5Kf?FZN=w8m^v~Ws!in6Fy%!vy( zH0+)oAX4H^Xi7vmB?+W-Y{H0Q*$9ltyw=4J$#f9nbNe>9E9E2AkgCe+N^82^ofB;G zO+t%TlH$^cDN&n+i6I9C!nJH8vYQ+47xss2JeWXJUR6a1Q{}!j49rx~+A(2a^r9j% zno`JQ1%SC+wMt`hm8Y(k@}V>cYe}lhSh}v!rjPq1(2u)2IS=z#K`Tvl#2wO?DOJHg zlC7%Orq=>^>M#;M4P}SNlPAF23=a;%=q=Exz75zn9VyQF?{tqQvkW+)%6uV4UGVbd z?%7c8+gCTFkZxN_Pb5KV;M+9gs97XZ%fNk`?15%iY(}xU4)@IM@ASL@+a5ZTCR4vz z$*7-@h^$OC3TrFjBHU?>%eA|>^P4W2^Ex~uO3yx;m#+Ns4ew--9hB9v*dVAiUmw%* zIyHi@JeOvLUCM8VYnB|LMCE;y+p2KPzN0ZhBt8^;QyXJL3oBDBlDsZ8!M5uN1P9Xn z+6uOONDzLyR=*T#{7|&^kp{z}J{Y#1d~d>ps5N zuU9Ei+FPQ$5{*q>iJ;^i!GvJgv$s)n+8i0IFcptRQz9s{@u(iI;M4=!!(UMWaYaRr zAD?nU@nl@y4yPa%5Dox&8X6iHLTYAjf(rm9(M@C@IdT40P4Kz-(Rc6Of!8SPjmQR^ z*{_k4ooi#TRQSWBj++@8r0*|5{g8F||Do(H!?IepwqX!kN&yuRP*ji>q(Kl#DMh-4 zo9^xs5Trzr5Rj0R4(V1xLb{|I>5|TGOw<+o+50_?Z~a+6RyglDuNdPTXY{i)szB00 zp}4&5t!}*p30tLGD3+_3bI*tZguLO1ubwyZTpWmRSKk{0Yhu77|2--hm>ActSIE(J z;q1bEKCBC*=hlilg~F9ga;jE9O-10Z{njLyGymiB02*lX2Mz3x)=tN+pJQwl}gNwBW z^_WvXikI%TIr1UqzY~tyu;NMLtxJp6(W+uhvO#96{cSMYT!bb-+ z0{ECuAw`Sw))8etQ_qhW*9rif=RQ;uY16Z)7CbS|wz;`jI#J?p4H4J5vKCI@w?gW; zzzCl>V9c8R3--W7dL?$@Pzb>ypx%|FrwTBYIR1Rn6UbVvMWQH$jeP^1A2!7DVRvY2 zewrFkZ>yt7@$~j1HpLI*rJtJ{yot)EU1fQk@|(V6LJVSx<|Gf3Dn<|3sKanjp_qP&Pz1uHn}1x_*Fvw`yso%T^S- zgUsY4NGQ!C{kyLfTL@m`qF5^8=U&JC-K$h(E%2B+o-LzV`>W)^Z$Ae=Y6`i;xX(X9 zax$jsa)uNB0Q4u}^hv*;ne%&9KXlx-h=Y6cvLrd@Mh zE^VzhQcoeb2n?@6xP zAGT{51Z09=zQo-cyjii|k;*_H>a=TxYB2a-EUl$YMJWBW#{E=Wh1w%-`=^7>g~wNp zW)<3MzhQu2gG*u})j`k|)9-CgVfh2o*_U+z96?Z!lbOgXhxz{Bz1d4iNZq49Gy3(! zsFYhch>O6RD!zRNkIUrl)o8~hj&eu*?@W4Af94zK(2u4B!V<-9#>c5S%-;Tu@%l)t z20gZ0jCgl{fI6<^3>Fr6-mV_*1yF&^-(^?&wiI3EM-gD9I`7rIxX_sF50sexiE$k7OG!a2ML)*kj)YlpkTjo^3W{pV^(4vJnvC>KSSS_X3Ebe{u zmDYake2u3poZhgoS%a=6;+995Gn5~NPXmzS(;ofAm!+Qt-Z0Vq8+sL>belPOe17~J z5{6p_w@O{INbry7leLzZXY6~9<{-QF8EHbodqRDqokDi~ZP$Tc;IyA}No%s7g=SW_ zH?vXggTZCOFskN81s02jxogu;&t1SvNzoiGt4mV4yf>TddxC``X|-|m*+7(bz8I@z zHJ0ydnQ~fAB`RUB*z4^dJS?93*IV_W+0PYL5)iY2A992*r|WR4x|CG2m13zewxM~? zEyBrnlhdrjPj;w)z5tV=>(`HWXuX2--*XUTP+-P^#(O#Tz9T-V^Xjk*r=Re1I*)?C zE>hf&@Ds)U{)r4)=}bl=^kCctju+z_y~dA=A^(gv>=UVC(vQhIC9SQz@-{cho%-g2 zX>K>snpV;Q!(IKb!%Fe%D~VmH$d$vr@o?Rm@-6Cs)+WQZ&~Qp`=Ib#(8s@RbLmW`b z(9m`N6ZWSvIQ_uNy$K!K&c(j?HgJj$SU&l>iX2rNx-sQj!bPvgju-?%Zpw;UyUFKWsl-alWO zQc8K?Dzs>|OmddnGdJ5aB7twU`h@kHo{v^Z z4S<#aUPC`Xp?qf`h4I}NBFjdgjRI)S1@O_(g-D9~05ME&)}!w%nz|~hsv6r<-@mUK z4O&M8VVKNT^D#>OW=Prrc2LSaajg0PdO`hm28UFpH9|ZI#}~tN=B2a1GlCezmvXD-uDgymxo``l!2Ag8Lcz4mRc^ zmve0fuJw$kKXRc}Ks<1Np2%zeKL-y$O8&nJmlMPBubUw`;vP`|gdIUS#&o#ch6;Ad*F=#e^ z8J`1B>!WU1Jmx~t&YAdRX?3VF7_MjTz(Rjk_$p$K!7(QfLD={jPydF8-;h;^QX(NlkjuB(d?aVe%h(9WB?t=!hAtG{3k6Ri|%{Y6ELH>pI$rZ}BTM z$8Z$j$pv7PH8(d0lMi{_xsbI|JDex_FYueqO@uYT}l_vE@7p~`vLXkuZK%A z&_qhuF#y23?(@+pLumzCS`HItqVk)F|6Hz7q`x7K4wySk)n#s8;hKLjRT~~24v31m z!r?EJt;47sIy{8(HubuZDT*Zp=G*l2kQ8OTMnnkC6Lc77{w!V?Z~kFk{HurjC*!L_ z?j6A~7>z|NV3ZCP{&3DCZ?YkNCD(wQp1wto84jLfunan;MV2sVGycWLf(KWHeF=Q@ z9-yZTQ1EK2INqI@n9!b(6-+s(ZE?`BUj;fZoS}&4CmfZESqN~%8WS|}n>yC{i#kTq zQ))H)l*4}wIMT9=nbn(36b+Y19cFHn8ydaNaZmnp;DLabfD5byNb^CCK0tJt?h@+j zT+u;sv1Y?10C#`)$^Wc@Izl6U{l*MsFl2tmQ?AHxmXK!U{D}v`q%XE)@!<9TB1$c_ z$A5+9Y3a!`B$XYQt=?W(K=?S^H zPkSJy+XR85%>Jq|K|}qUFVX(ozdM&aOSVKeuxOFBJtuy<;c-oTps(K*5_O8s_;{cT z%K2bnX%2BRYP|$2N>4A}ZM)SbbzkQ_CtAEWSXY_rKckWI(rao8sFECS`UhNvd-V$E z{#ivCzfnYZ88&xl@tt>HaxGicUVt6}8tpbe;W4~!PqmYzka`RP{564w-;LN@FOWvM z0|2>z&HMRJ2?L43zq?v62beJ1Th!b#ky55fLeZ2g`3k$& zK4);K`?Aysz;BGz^m4WLo&p6q@iQh4(yBof?Ex6W!>4rHG~W7;3BX**@#8-CV&PIg zmub%jG9Y06;}YXAId?q?hU;+iQT5gqDsDyani}sDKAa zhmrhWHpzX!Xr3?}_fNkZWupdV~-Mqu1ZohAb8t+vfAQciSh()j54N)j+Tcz^gd(+yVpi(g?ZaeH+ zIor1X$$U;j>djT|@3#rlQf)%`Lhm}+!4m@Awpr+yh^aU7e+HP%$0rGyCx}n1mNWMs zN{W?T2dNs(Sv1#8gmI(U^x!XG0@y=2@eueb6&VL6ZdQ3`vE?2YWwBWpV-)CWZ60upoEnP)5KHKJ{eaMZmcwhBz;mXrOs7?;h(lIxO)HmkzpYy zcmwr|$Bd(Jb|19Y^c@XUkkLYvL;pP$wFA^g95z80;QQwB+@EVES02z41B>zN@dV$yN*~i@Y^a_{`Gy5 zNFH{Efk@D$G?i+ve6_j~4UeK_o?i>h#^gT_58o>SXRk8U7c$9(5~VgXhrji*8gyjJi*|E& zjo@Sp_#_I*(&XeFwcpU1=-s|{iQG?7EKK~g7ef$ZpFpddXp*L zRx77oTdS#hK3*~}p!YG3F^WgEPpn(9`?!-}jX*jHiCO5uQltgN{UN_WQPH=f>uFu; zbPz_aOSxFJ-hs46z?cV4F!kIVlsqe^G5U*#gsXt5`!@xSN(~pC&~QK%dyoKm|8Bfz z@Y#|3{H!t2?;+kib*_l>J! zP$ucE^Iz*E_-VgHD*sVMm#n+Xe5n>RNPWj84`jfLs~;sUHr(uJ0 zGq?e08ybasSM4|{#cA}xio21L^==-7(Ir*q8-~y+@$-kY?mMNatjQ=vSWw>En?Z9E9w;06z3YN;c=5qc@1^ghix+|GUlXRz*<5OwT;KTACNv~NDf>ZJ zFIytcwT}su6ux2VHvP8(4pSvKT~yX*ZdB}L&KenK0P3pl{KCJR4nD1~S~MIxC$<~o z+vM6k2{F$;ta8O?M=PeRscK>Ix3Ya)p z>1)XZJH;ZZ007Awkrj$Qfv7yus?PMhy<+tXW*rx=b2!)tScS{CsQ9nTHz-Nv`0kZ5 zixBr^%_}PlSIJziqb6j0=(MRQEvl3BF&!U7o{^?C-@-NPFu&RZpqd%{j~amdc6pwR z)iS-!;!`D>&SXhzl`I=qx=3admH5j&*VOCf=$Y^ z3?$K&5aH)QbTz$n4XM*Be=$Yn7!VT;PdQI8(KtF8%`>zw^Xk_Rs>nfpP{l!N-G%@C zgHT_oiC^|~Q@sH&t67vTBjGmN;PsYhWA5(!wC&ybBpPj%q>sfdF@>B21l{79Fg?st z$r0UImljkZ#_RlEKl<#eeSCa~G}FmbKG-C|(oE!?Sf^LJ<(qrchSVb|u&&*)S&|_q zbG`*2pJ*5=GID^ue@z;;O%Vg zV|d0}xVn=qem5fl5)ityGA7S5+>XF|03Q@-Zsja^x~(4Pp+MYj-Gz?OoQRuY41R?p z;I|JZLaXJ_Y`ed|#%LP7y>F>)DcvX8bb)q)gbNVgYrzZyJvM}C62lqFwXbi8PPV#U zh8*n%g{iYQQGyxYvHxtM2hf|aoddOiQTPbf@!>Onbo}aj2-_AG>PDXbmNkv^gT?nr zb!XWL76hILsf8O20?(YingvG{tg~#Bve;b*M&pPzZ66Mc`3%X#_wat8Qo2Ha$K2l8 z+`N-YU;GS?Nu%sjO>5DiH(iRE#}YWO3bL8IWnf4=vA4wqu`9m*LT9D`D^|bKmps7y zsI+pEKOz_c{z443R3ySsdsP^szKGVUG_Ezj=!~x`k z^;ip+V5N)iZNP01c38{+@rWG5v4#}A1vi#6=6S?H6ysC%N>ahbrdm-3Bt5Ns=UJXlPt6QQ!>$BUA+5F*O1x#eJ(X zbu||;BY)U@(S%TZTo>j!DP(YFk%ACHcQ@p2 zvwzD+d0S|kWqS}shU9=Eo|%eP{7q=ZiaRLLF^@G`+FNqRB@c6VbVy=w+b3OV!^Ox1 zm{xx%HRDrpE#Ka%w9cWt&m@;`b`Lss&`wMM0-z=@|07gXxm+>DJCp+M8uSGzq_n0T zbgkt7_gaKf5{`kM7rgs?a9FY$WxG-X2!njbk?+WNTy~Y6(Ejh=@vhqIKz`?Pa`_F} zMeuMnUhwtrcBc1E)=r)L_mfaY;|`Vhbf=N<{Qagy!MqkN<4e;&4y5UGC+cY6YC&_u8PsGrJC@?eFCo!c zm!pQA{FtWlqa#9`EG)eSxJLvMoq@$R`0VW=Sd1Qm;m}LqEWdmSnOcq&`)guN3m~$Y z9jOH4csnq252lg^etL5I0svF?s}TCu6)dl2KLw7YtfuHReZ`i#a&n=xbhmG7gTX1p zw9JL9bperLDg~&?kiD>T=7(}7=irH+wQ&#%Ku-&K1C6cw`%YI%C(hH6Si%p1f&q4u z6Yg|t@9TeEF^5#BDJ-tM2<@i#_y7L!jYsHuXGVF@LGPHo=QgzU;~#)$77V1`MAL6B zZw0XC!RSOO)$jf+u$1v#=DJbh9@@ z=5claEzw&Fi4K!Zz%@f(zcz!2Wx}kK6yP7dA#6^@CgHp_9|Sk6Dkm`UA%WpCost$0 zC|mo1BLjk#2H(jP(BmAALcFDO>c@{FtPcuyK*s?jHUM3ehN9#Kso+PGLY1`t=-&rO zhJVOcGxyS%MgU?EJ^>r+sb5jOh|UZ>d+%>0+%YQ*s@X9T!ub#y`C-T9a$rO*!uRa? zxDFD1puT5+b6KB}+E@W;1%Qdtk2wa?*WtSWy$kp~qK_Vdn98N7qFQF-Q9#g_rIe1x%meyf`at- zL&yK`hvvL-ol_qkTAwg2JUctPtE&rqGyuSFNa{d_Fkd{+0kLD62DlA$pV(lKWRzj^ zDL@DX&s$(RViFGK0VMl3f#$T9PRl3sEgAE4wY<7>_dW$_+9qNL2dMJ531y7 zonw--B`^t(e*5+w7!ZI!sSE^$fSC1Di(j#h`CF+&hX=_xc92Ky-7738$U=iM2EkLQ zf4=pQetQ&PaBjlTJ2n3Qer*L$tu(`zni=Xg7x6q!7Z(dfc9eBS{a@wJQ+j(vt2}n| zFVs07_|lary)ZATmb`3x0(q1!J7U_gPTK)tp9ZY%JVK=<12>0uJ*#)ODJhe)v)d?gfDxn%1{64;P*ctC_^rv& zs0RN4m}onK*Ci+mphSaaR((+&w%NbQd_@%CG%;2Km%6i)k}j7J1H1l%49{I%Rvuvdv%mA zF%R_C$uW?-7xH{loY)x$3O8+CDo?iy;D+6z9Dpvxu$9R z-&%W#XqrlCErD}VJbB&dd)w~kFndIeGZ*@Wt`i0Xh^GN;V>ss#qA2Ln@5763T_6X>JRXl&~5~RVUlDN5gy)6 zyr~pGd_U$l`GMat90Vkd250OtsLGM_ zh2#s8JZMZ@w?{XB{RRwKXz7^i`AWo%3d{m@zxBLWuE2wEw+iQ4>t3Hs)ml|sA75Xz z)jRu7%)q$`*+qvs|NCcZ{R2p4U1R@D;PEO4{ib*@a-rn)uBY zuX|+_C}SK%d{Oqcz4zB0fliB(cr1UI+|Jalb;)@*PpGcH?yhy}+V*~+>m7$4h6>z) zg5q_pV-srR`<J;!8Qxs{83G|X+IbvS zek*Z_9k|AXX`d>J(zt#rI!dj~zrHG9k_&$-|D$McdOL(hPPf!X_Ey2%2mdB~xaNMe zDo^?PK16DKzwWGVz!A$PrM7*X1R|P_={qmIN!PsmyU8XKj&Hf9edFWysvj*h=Wrv7 zoUNDId|fZcIxU1su~cry`*qusse8O5pFN8C=TU$0!KPxwp-&E`8Xqi9a-GO?V#+_q zWlPN~Pi~o*@bc7!^ypl5d%;J+1hCygQd^sd>e?0ciJI^IAYy;Q5%TVinoIFc+_j3D zHWLr6W}>NNr7vFuguFhcT#RJ(AvTpctvmB%b@gk#0)~Mg=n=XgAT0TD3|v`nqoYX) z38~QbNjaqUHs;wZChH0B=>YN2X=f^oof2&+m+C)HVdB?fw#LSE7Z}jCeQ*{o5*i{t zD&dze2o05fO)a%SS@&Eiw9?(Nm~x~b)=m`?k-SX#SSe9Ur*ZE`p7EwJuxMnTQs zgV)GrR~c`VbvKpat`d@u%;n$$az4k$l*jmQFnU85P3 zosOAu_)WwYZ!8rHJMAQPmbJ(*Z3Yo}OLWX+^(Rbxg~;$mQoFgnex-|eH!F6}1Ac|u z|BIEH0pJ}ZWFUqeb4t`PSbLksyB`9#hVgh0Qo1ovQg1XV;4XzeF^0+HkOjv$E%}X@ z9Hn%E&zmIsH7!Q{r=57_*7*~gWSj}yBu$TZoakaAgaa=5U1o`ctV1r zmWUOsj_*js+b1T*IojqJhFOSCM{wrv#4(e`L}>?OlB6nVDK-rjH!fFU68BiKo2W>i zJK9h$0Fp1K5+MjbPh%0`laSokZVX0J%CwkBz_I{HowRGCNA*4q3*U2~Fo&ucDepi( z0)!Rd|N3J-7%)EnD~+8P%q1|MZ8?`#ogVH`1<$O|P#val1xNHMGN)(2cISUsvo)d| zMZ&FcHNYVRA=fLa_YX$}5QCjVBb!a8>sw~?xI1^UMHLIg2d(8VAQfL+QPJ#509!D2 z#02ympL8Q7%l9%v{JZyG#6^J8L=OAV3v0Y$?{&^YE_(!(vSExzMb)@OfrTYOML^IPt#Rs9W8l1M*~e?I z2;+$XPqBT!kr*)4S1P~1|3j(Ldj>`mTKR>(JHoFmErKb+#m5IsLzWX;q6uESkgN=) zp?=!Mx~3^>AMX;wrXQ612BaIJY&0|>SUR-ePvih)qY}q9pjDMsfYVtvbW^$sn9*Vm9id-uBAMT#mHeiCgXMmJ>r`1tXZk#69r;zqWIei5v%!dBJz4k_G~6PwrU^HymeVFM*M6#r^Zx z1)}MxPg$4dx*qM23qv&n4CwAdMYY0u1)y7JGZ-&OhS3)|n+tY0%m%H=_WT;er0#bu z9}i-J$xcX$%Xr>gu9(lm5{h3#eZFy{fS1r|>3uH0GBSmcb_ud;x=cfu79ob+vb&K{ zCIKVbV094b6ZALy^|fS`BTx-kY|O^uOXA9?7VUU1A76u?`GZ0@#B9K;9TB+kgp%K&iM;R)XL{`RGwr)>ZcZ>CWP&Gy1-x73+yFt2EH6Q z$egXb$xVr}UM)V%awXtB&_|V_grWGJXwu5btXCi=ig?N_+Nl@4erCZq(@(JP$~+R0nfrnNfX-hGy=29@5w-+Ef&o~i4eGPnT2)-c|& zQ&U#vf`F5q@APEDir(l|@;uqlf7<2C0yXtaH*0YDpn4b?t^Cz z_?aS>^y-)YFgwQ`!6y0?2Z!b`H~7&vPABC?wcKK8siDWd%|Zt!;N(z=EGA)`z~TOn zx{(l*QWfxl&gxV|9s4|~HYp_fgtKOt|F9r5FS6XpH*93^t&d$gNQn+2^z+Nuq+&Cj zCAmZn3DhYY=Alhc@+Rg7*#`)H4Z|YVDGewot}+>lICK{DvK=u_6+sS{M{iaCuZh-! z3HsS-(5F_Eg_%iR^{Rn;rPhALVw4P#yg68?t2k}Q2wY3}Rld;7tL&f+hIh~5u zmhp}b3)doVQA)mi^cLR|g;-zh{}AGPzIN{>VtqAt=U59@ zT#@?}oM2?$2tn2J_Qlt)erzK$)SUKDck=Oi{c_1@bO}Ko*0`tFb1e<-PnS}rZ@`lP zOifSWL(~&x<;akbe1{DKuM<^|xrN}hp_SQUEbyQ)^=xKa_4f9{8snm)LjqNZ!otOq zJUu-@{`iRq!)@!zQ&;>bc7e(hZD=!o)vS)0hV=?Jp8T37e^EbVth3f{0Ylh)!+fM@ zdGtum;2j*I=@aHq=2TWfgc>WS^AD2tbkg~#$S<;b10rX!=|b)9Z{`9#Musf`ukgVxUz{b z$Lok|_jlwX)5SWuH9gms$=1^SMWjDQJM3r+HV_d}cXZ(Mo60gPGUk$Gi}~F)*K--ZGut zH;-P{0BNm30C`Q;UxYVF_U||8re0FHzt#kS1FuN$_lMX%0q~#%JcGbb=&7=DQv@d3 zu!=}uK*(rds-jjpE^gMvcURqn$CgQ7IHxr>)w-c8^%GgrZ^&3!ycJ0lz&eYP(%T$u5 z3>OTZG(TDA>jeY-%`_z|YZ=l(_4TRv(MK)+{cLVwOCSaZr_wK9zQ|3gmOJEsQDa}Smen5G=P={ zs{N%oCjEI-$0=5MKzIeOy}aTojhrt;O&!>z-0aX#06Z)#AfTnITM3JSNEU-V0gxg} z;#?#|@*KD0rjeo}=&pflie`q+Aqx#ZWmpAb=c-y#ZY0e;MqjbsA$FSo>edyd=W3I* zNG>nLF2UPV{Q23kM+35)z!8NTG}fkMmhTpaD?MEFU{V#!0CS+UE77iC#zD3C1!z-B zsxO_~B}GJDQ+RD%T|CKse*c;PP$ps?})LW`T3rocNdOi?aVB_-)ycOs~SrKF=>c4sS( z4wk`yOs&$ToO=VYuFVkRuS1mm_UxdtY;_S3GNL!jQMgC(mg?3mEikhISw3$iyzrkc zIq(L%>Q6iid~XUhJpkm1y$0!7SqvcV5)mnaH+Y3!ML|jm;ZB1mnI%IN!K4g_&4PlW zVjtdP?l*@wp`1}Gpo(tG780~pD*K)?M2WjlJ=Yzl%;sFL0L2U5tN{Eg_jhOFp<77b z8ReM2PWJ`Eb>rgW;a&BSkJPE+MfWYGam|<_1GTm&n9p>_@6W3rH8wO5vYDtD8ygS! z5i#qvfiLgCW?WUw=lH7DRd3trmy%~+O7<^VtelcStF8eHr0b(nc|Y<=l&8!8>;Pam zK=`@-rx=I16p$e+C%qG=2v3A{V;~Bko0ce~6gh8#)=w=OY;k1fJ6`92O0~WkOt^k( zSX<}c%So}x(ec6}rocP;;d=Ph$qsB|?Z2nVGX&70VHblz*oE`w%cOe&eKP_4ey7lE zdqqQtv|keV)^c(^uxQFRGolCX3kB1|EcXuw*7`as#5Dnk_ZP0vmpSb)iCwlkqkoaq zdA;4!^JFjEqD_;hZo%FP_HM|~*Ic~y8JNj$T(h8hd!ZlPpQkcU%+Jq52yc6qdLwEdjUU;1 zZ?1C5eO+vACJ+m~j>39CI1k=kO^uDT(h%nqiWs+%u$w&~T>?MWIyzgU12AW#kbHj? z9F_3#wdDL7Un+%p`Llqb5NWET4(yT7YbSdBSS*LEd;5;WWzEd)vYu6HgvKA9b?k*N z*v!0#bY5ss>aGonK=LsR0>IKHkc@YUB+?xQ-~-v#X(HnIoympWlMD}}SPnkq7j!B^ z;yf#yEAmVnH(w5z;K2+!W?z7N_hX0AM%U^D#JYBM-#}sI5m3PgH#t@O+*hIO>41IWs`%h`sYl|rws&CV+E zKvMvVh2iLsD~}8kz@wt0mtNdg9GYQ_v$~N-j(^#g$GIlTw&XMAdft1%U3pk+huICd zdCK1&;>x>x$`3%c+)u6<%Hu7Ug>1RScU$5cGe`YE^=AiScEDbldn=uS#jG!ve3I`WMCg=&5!F!d2{zoA$+#l1`b8`kw7npq)+dgUy*=CQH*ff$E>U{*>uVZCPh5B~Pf;k(tW}Sz z@5QZp(e4a#K`ifaWNf-0GoI##RME+H8*2EA!E}tmubGk}EaRp@W$YJV?3gqeR6d;j;gs{v z9F2CEViSJ2bH^>Mbo7}AT;i3|CE5?c3a6=cb>gdXG|~D*<+f2@?vx)xMUg6}J3S%^ zYML5@@I>Cdi-b9458}Tg4p!T|hpRYEIuMC{hnAKW99oAVyEU`^1RSzj;n-=X+eQJ6 zf?J9-LlZ1AC+Y%FA3w)~2c@pIw!Onw^+G%UJE~_hE_ATX_C9`-v0drE0a=95aj#_s zC7yFyniLQgj^VU!v<&3% za^=p7W!dua>PKwVQVnfSInDbqF~+JMaOBC>&B5vilV@zC)izmpRSsW8FD3S?TJwYi zUtslTTft@zf0o#hhLfXzHULY{BEPqQPSTqFY!JKsQ-7k{m+1`3o+_10d<{8@1)7|I zkgrB{1oqbe%lu8R)2l~^G27K-pa|q-r=eRwY^uncR|iblI-@S$tbw2$(#r%?LS`d8 z;L-wS9=C7PE0!bf9f?+7wJ!Pi#FY(plRB?F*Std~E&2WlCOuP6m1QhR(sHoc#1SMy zXxh(_9c>HS*x0~u9m4g9X8B5JQ6XU0dE@r&+GlqMtvV$@g&ulE51+V5O%Abw6s!z+gbRiA*H+DU4mvA^vB zdOkTI=T75N+j1M@%|i-N8?^yLIbmh&7w8DqzkjqZqn$Bl-_HDK59GkItgJs3gF*Vh z-*oT8#LrHNdA8FvS(OF%gHN~DrZy)e(;T&HxIX_JdsRI$xG5}>>gT~$Bz7}zu#W!8 zGdMO4b18LFrxo4j9p4|#$!m^1ObZ4*#WIVZzw9zLvmdLQ@Dc9$3l~TZyY|3tf<@zt zC#-x=FE2GnhzDs}=EC=T}MX4f0{MM*@5@O1?uIEZj6sTpz%-Uhzyk z-M!B`1xL@yKe=03T*AWxO#TpTY3wwXPEDrUOkwY~A@}O@F{n6>75?DQnNtY@VW1}NbgtWDQOveArfl=PoEo}cgQ3CM9{E^f@*87 z4hmdmH3}Wn!ZYa^QCn8D&H6$5NwebBjo})C-rAQtK4ceHZ$>=3d@A0n^7G3YJVpK! zMEk(qBt%DlbHtTbY4a|e-XZBnysllew63JBb+59itbF&L+{V4`o9mYq*%MK|Z!|X| zA{-Kfiu89KmuHWb$QSytiPFS~Z`l!`jSddOT^$`0V~W4iZ%_35uiEOGm>Hb9~m@asr#i-J?3@=-a8&7HRSW-zoEGqO|K@-{6drWLK5w zw;VKe`AHlVpBx zCONvPbDT{})wwr|0_4$%(m*N84CiBVQW6og{u3~n_VMw71qq@S){9pO35#LD0R~yL z8u7v6*$ZI}ejM~y=9L?k>6`8kP=aUy1a%jerQ7bJRlOp8VX4Tlz(tttUPmo=S()G~ z?UW#`g6IsR=1=x_Z5TvpP5pdTxv4?~MGk4F0zw!~-i8$0wQF|jr>q5orPuV<^{k9V zf_wH%vg~4H@#%@m<(c%wa;8(F2c(NaIuRpYD&x`fu{kB77y*35Xbb%p^OTGwBmy+P z)ddh!eFeneDQ$xz^QVr-J*QK<=fLYbr?|N9iR?||$5EUcVC&z3d;lP0MY5T)Iubil zUi!ol@$JF#7~B=WsR7Sf5>_JxU>d>ryv-NiJt*js1GCK{t%dkqGzIU%k%IEadh(op zQfo;`6fvYKl`g6;6HvaZ8yg2;Xqcd{(gZNN#?|Wox3U8dXy_;u;>|n!{mI&ZJ*mt|G{&p)b?>CYxBSW z7ie_AxG`N%ZyQ*L@VfI)I%#?f;EES7+S|~x6wZH)1DzdQ8*&*+-ED2LxawV>S+CHY z$u?A zA^r4?ja}m#;uBYBN6(*ks4Y8S-RhFQ+@^p+6|0syEK<-f@JVujGvabbullFW>w$sG z@n?zIEEpM8EI);d5wn`L-akW>RvI9Zos0Ht`PH+M>L1F$=J|P zxzgqS(9T?1mT!47@;4`EXK?IN2Ub-_hsEytEO3(!3(zr6i`Yk1x3n*mzR%5N!odjy z&thmM4*^ryH*tr?T7duQiG}hoA0s?}w^;m`wl~ajUyu^eS0&#d1h@K=7<52%B)$QX zN02n!F$-OB%Z3}aze&|u9QN(ng!W;5)6TB2czeCFKH~2ZuPQAYgQ?(~H#awfXHIe_ zbO1g9WYMYRi#)Vk?gMtGVsn%NARZ{K+aMx<+TiEOXyNyE6DPoXW@QVk|D-W}O3cPi zNc;dK;H<4+iSP~cq3WkcgfqM@udXqil_P}J;%&VGQ>FEFE2bsC%XH*4NOVG4RPPX} zj1=NNnQtf$Re)$>V@peg!$xYSu&H;zz8;4g#_q#H488PMy5-+P>I`JA<>b~gV0-+v zbnZS3RKOC^_~}+RCuGQU5SO|bg&ywx?VGr(t1B59Sq?8hCLt37XFw^q*Q9>%gO_oT0%&mcOf!&mJtMY{%RyRLo-lg&`mNoKi|Z3-F0J5BXhmOoIPWyIlLL8}c83sdIA zCyt`%NN|r2uMgXwg>I$xw7vhziseFH&@&>`NqB7^qzLox+q|7vPM~jySsTcY9mH;7XMa9{{_5sG5yh_-k z?<(3B;ijrqV0iT*hloHpotDY3I;rgCDY0|+lQ`ME+NwXIQyG!M>(-P=B9qNO5I~$` zp^zOIP~M#$2D86vFRVR~*3S4I`NMHE%M3QN>jN%0L|rsNr;oz#rRbYiRP18pdaPmX zRyaFM2%c#862$VY^~cg6qouOgh2*E^x-ILz3p1k@mc{y=MV~!59zQOB)cCFIXyYO6 zd>J=Av&<100UWhUYTV%){+K3{A}vjRe*Rqp6#QwF332BYD=PxWxA%y*1U^?B&8zRy zDk<}!tl78>^gq-r6$(#%S4rw)C{=vp_id6j{S5mfdW|BwM95U>WdLGTh8&o`zb5mY zty)kf^t$7xm+pV9xuR+89*et_jT>>HZkxiz2dMlw;^AUT;k53j`*8R1CBuCdhw{zQ zyu-?g6>ER?>$w3JHa0fmVYiE!*TM~_V{^#e>iQRr3dTVZ z%|aw0D0oUpO!;}LOom*O?lRTB1Py%p?7u#JlIyKcX9BwO*mau7Ke1yrUbu;r7HJ+B zXYXqUJ|d4i!z|aU{&rSf)|j&RXeZ`q_K|8o_4eK#4lye&FE3d}54u`>TBia5DNwP8 zD}}FdKs{* z*FRBGN^RFe5a0dPPoF+nf`_3}MnP(7W1Q#X`y0XSV&{Ds*DH)fP_<2WIga;Mo5I)+ zg+d){A{uyrAxUkTo}Qjm8S1+ijj^N0seby+iMn+9hGMYiGJyZk2Vp)>QXgc~s5)Kl zT&D164g1(Rs{{DO*x2U={s#}LV9cj8#OAgu3&k42XBTF4LtQQ$QV`9iu0kBP+K&BRbVo=M*bwX7_d~S7Z?UE-hnEuzLLE-i5{y!sUb%&kQ*Ns324DhoAd}V&pff$QFU$;l ztdi7ywh$lxk&z>K+=rMX(*7&S7nsxp1qFpt>hnvT_X@t>^9=|X$~O!%D*^9nU?y34 z)KRy_zphx)cyolgPQxqw>dMJ9e>jE8x$Hzm8^Lwoi~>f;l-Xj zefs=`Ezi)<%f5_*y`l>~XxKADIxi|W`wj7rN+EXu30yNZ&4NJ!J$*Eb;bm1{AD=PD z^Y{5IT)r>dChBo=a;l%;gi<3P-5?J*%-pq*?9*usSrV)}lHdc-HG37=P_SM?UqK3n z@yC6b^L_M@e9!*If^rJt*X5r)sjBqE*yVRT9Y6pTNLuL_IPAix3(Fr#$NUP5eQH@EF;9lJ@qXR4E+5uec@|?q5bUK9Mhh^UIiIG zzQ8%Ea|a!(Fd8o5Lxud15>emEu7^T9BL>K!Z>euZ+CgZ=+!d!>IQXj9^Ou9-c*> zKhTM2)Qqmf4NiSCh=92VRvT0V7ig9NB47)$goKWUx0IcxV0;5~I;IYNj|5`q!e83j8&pro z17yW>1&Gr6e8)%B^q7fKJ*TI2b`w#Di>Gc-Q_l_Aj+DC`I5;})!88n<>RVeR*`uJt z(a`vSTdChk;rnJy0PW1W3FBDTEu{yECUz9^@MTq!4)uQ1EyOv28(oK)z(v21x~sS~$NfdpKd zlC+@G2IR|&?HQcXrBVE-DMoNaT(edOGrDR9D&l-jp|_|`g}wl?&>?i2k*I|zi^dZS zCz$25Utt1p0w5(AtH42+x#!CXL^&#|l-5?SAc>tH0O8?3V#H^74X z^dK!CMzwG|s7V$j^5t>_bicOFfzuAUiJKhI0fV*Y7kz$y54kjgpcr=ZhecLkDpOVU z;EdbP?GTU2PxuC5|>J zU(}U=k_AR@#5^ae83q}n&yKncW2s*@`?R^B67w1t4@FU-`}L8Z{S7ABOQVa;L$+iP zV9=aj(C_7@d542J7>uimBH709N-zQl(1P`^?2h4|3mu}cQ4deM-_!SmrcpE46WzOA zVXu0ZST9|YkWf@itlI!elQdOlb|_a>z>o<7R%K-g{4e;ExC!HQoI4>xDH=^DBp`6L z&+y{o0giZY;&&M!m4U1o_NlS{{7rFX>_9jscwuIVj*d=BN(vUzm4MRtR|F;CMxG6< zpV~daeg32!{(FIcKQZ+$&}yXECWcc)=G2IE#T4vSl+ z6n94c^_D#^-W0mJM9TLSznY9eG1t>z8PZ()i_9lrs$`tlBUxUd zWwNAs;QhLTy~=9Jlknyjy07!0YA?O*&QcC8%SshwmEj4HJ!kc_O#up~Ge~*Tt_PBX zL7gXbz{yGL1_yO_=jS;Xqd)3b-B%7z!wFq#vjSwr87t14`sW!hFMVc#A-ar!fTuKR zrmA6U-jtQKHC)zGNv``7WWbLZXm2+^+}{Dt!S=S@seS=F0s7u6qf*za;%8#;I5B%) zr5Qwuq-12kB{{ip+SGIzc`X(mZ3f0uEbeu_qnB9MGYa>eot@zUKVTT!B;8_F4|<5& zSY5q%=iSrX2^{}Ao6`Z^cPJPc&B3si`6px>85tVBCI5A<9YF1LpX{Y8$daB)OH(mH zV2LtOK*-7(5c5|5KgQlW9_#*nAHR%v1R7>v}z(=Qz&eJdQRpyN|l9{?lM-;#2V-XWEW* z<$Lese8`@^gw%J~TPyos;!xm*#u*UHL+_=Yf~Ef=>j?;7b{-z zd>PLP$uC&^-a9WGyii+PnN4fCGFtOKTSKj&Uc+w72iKK2t{`DtqeTG~-k(MjlSiAs zw``tP1#MiL>8KAIPO{MV@Ve>TW#NLVszl*tT#}oe{7XY~Tfc`gm*+j-R35*QjUrhj zQpnAjZ3`Ybqj^@DJG6{@Qf8J?bg%l1S6hEz*)At12S$sTltC}-p%f9z8!S`QA0evxuRT)ECBWM@GuAbjQ0bLn&4lcmz&xF zvl})!)^|6~Qoy1&Q#yHVexBgU6&Bc&-mLyu>l1!?R^+-@k3aE1Z>sB!!{%xcf(S_3|tTC5=tSIR50 z!%_896qIJ9sf}lw=@ZNeITJ9LxGDv|uk+04vrHd=Ih&iC=^q3(Fp(GgDJgv~!n;A4 zg4a&O|Dhj;5^E>ENxmVy|3mp+h&|Vzzb9pnddRK=&_)`=g9i^Rr|T^^_kdVe9Ad{} zGI)oVld#q4wugMQ6rwTXYl+^gCO*f?)&Q3q7uOys6BASPDJo9~@i>ROY&>#Jzu8c! zBHayn-OO_I3|W4Bub^cQ)JcE8N-LZ-B(F0#Sm}L9$Rs~a_uh3Z<3K&!W_`QkYMsiVg~{g#u^&CNO8Eor&xBsA*LQ0+6i&OZjj>vu9@o6T zD>1OHC0-R7wcN>cw6`a#Uv6n=dsW`A$^9heH-MLj6pk7MDg8C!0=KET79upCyqktk z#(nzbZ0TINl$v_tplp+y@)Te^{Z=;_%Q{Juyi}I4ABeMcP~2^nEhpBc$;3b@a2yIa zxw&CGbjzQ^QenyjNF=xb6cqF_`i{aHCs-80;twDcp9Z#V^)R=Mkd?aE9vAek6J?(2 zPxT-dCrP-mw-=~a1WTRmRy2J64XdP1T;Rst)7(rYAOOso1|Uwq#BlB>uK=H!>TM9a zJ2*THrWC9%EiKK+ScJVQ7$&}Yj)sAArF$y>ZlK1b4Z6cE(iO0~pxuPrsw+CyAM8Fk zoCy9$@d*jQ$dSsz!oqURTqKZNiyYX<_yz;!Q?R*_n+M#Oe`*7eMK;nZLgdMtB71$F zwaGY?8r>8bwZQb*8_ED9cPO=?|4$p47&U`H{Ad;;uP+gY(t-C+=Q^&`+FFgQq0cx- znf;P}0rv1Gwx>|y10|xZtqlO~!DS%nd{S|pxhw!B|9Mphrw(?X07A4dF+rMMqw#A+ zt;wfA9T%$)$FhV-h?a>dQ{JDD3CrsXe63KNC*BKUDo`!C%gZ}K8vuP6K;oxbt(jHQ z@f?1HnOal^B+}^CU4vMNN;?+^59zV-PX4n8#AGge+8`xt**;pZrxMlA-)`}BG@$ws zR;LBaf;I40xPqd>ezy3z{ElHBl4$8M3Xp!cVAwnN+%2xvtm?&+{+p~ln*REyo*fU= zBQJ}JY8V(WALc9N36k(CxSOj9zb<+Fx+^wx;91ysbzHBI1U2^dJ{$W^h#WTzBM^~> z#1Hjao3bHaC6;LnWaVj(`V`|PXM7*LO`5l{M@F~yDEWI-SBYuV4 z90hBLQy+DjJ6@-HLOKH20l?MG7*PCN?t=wiAHh1H`y_3y%beqVoRhXsJIsI%kXe;L zaY=h&E}H7&rOzsmvV!W9jEBA8Ff{~K1=KeLG*WON>r{Ebc>zq}wzsy@=x*NtuT;=2_L@IFK8BMcEiGLD zIsBKJAIB*CeK?=+g_yUo^jR}sk@ zF%84(*rOAGr~Zpcd{bAVwg{1f+3 zow!Wp z%)J=`$6QRjyZ6W|-LCBBehwR#R7RRU z(3LtmJxb6o10oRV+$x%ltGI1Q_2iM ziakEOVQr>%o=^84_SA-3Zkpr^n^c(3&dqIOo^1&_?p0Mp-2i;@cX-COWyzlk3AV^# z6ZNMudwqO-JZz{!oyoJU)kr~4Q*J|JKG?dzcW1atVxDf$pNQf>@s>gkpkEM2&W+q2 zG_+d)j9A7SPFBnU`yj$xKHF`}(uw7s}K$<>ft<=vk*B(*Xbs zfdMRJ=>uuu1xuBtaxQ$_ZJtueS&x`lt-pMPxpyISt2SLl~7FVf5_1C z8U-bxn}5QK23VBztAh?{?T- zdL*UsD{Ssw?1W7DEEvYb9}*3kuT}$~;TM&E-gWW8_oJ8IvCKzxqF%>q*I^C!eplf$`K${mwZEk7;^F>@F z>yiSN$`0HWqU9TrCKucbQS*SEw49kUGc}+-b5ysNnyBWIr8M4` zb|SpELCa!^D9%+gJOpu$VB4E+zXP;t!5PfB?Ajtw!aywCXjS*iCbk%uQCi(iOmjB265W8x9T*%w-Kru6@O5 z%fC{s1KQD$+0rMRmQsB|dZar<;BX1J8CyF$u5e2Ld`cfqVEgyl`(s?1PmF&+Rm#m` z*OzZDo(!@|OJHHmiiuSgGHngE$aSU{NkNzQp)|vr&n;r+D zu(fVQy9wm$dB+;rypcaW6Z?}6i188O;Xa_=ai9E8$2L0%MCf<$Y4IQ5C#RsOciu7- zZXN^AD-8{D`zLI@KqpF(FApEQ&*P4i=oXaj^<+4Lhy;V>eV`%HIveVgO*yWahS(`T z`j~3oY?M>Qu|uCeq(WVdsrx-@n1n0%YlZX&ISa;^uoru7R8b6U+k_$+0|U>=8o#z6 zZ{E|+z;6~_kSBp|tPxASqt2jS9vBE#l^3pwChERO=diK)+97z{E_kZ5WE=kQQ<7fK zZ^j{KN&rj$sp~5dOE1SB}>0)X4&+T1w)XBR05(tt=)NM*{ zqLg6-*IF#W>)+c8_7iA+$VK(w-u3qMJk2Y+Jq=BH*wMT2aGRgQbP1vOK0)hW5y@a39)zP6Hp!CH zQXfp{(m~P3<`~580Ug)?;Al3IR5W=fa}0?#%FJQD+S=THfOGls34k$lu#*(YBz4}m>wN<05U0Zo7#>`X~l=T&Du1* z8D=psz@TYi@p(!2gd;7Qq!YAC?vvsYrB_wOxh*>Z_+?l|ME@ZceOI19y!B)HYP*g~ zoR4Y=?vfW40Zv|PDDU!9!DrvaP@m+F<+X#BG)yR_>Rl5;UV&6j!^y7tPtp~W(78&G zqfwN&IFjJ&iy1cz)Q@)(G}rB_tN%xH8%Z$^*F0zZZO)_nf^Z(;56S#4(eJ2k4CHKZ z_uQk?j6f%)-a-4)2f0bJ)+vmXqzb)DOSiabjo!A**W$@_WIJ#2Q&}XdNX+qaOAq}b z#r2$+E99@F)#ano$zEdj_D+W0nv@o-TKfuI9mb_8AC+?R>v2Ll?Zs|*n>q^jh)C+B zin#H~c&=NT79DFnMTXzd&?}onM|R(O3b9z&3|5-0pS3qN=0EuB4c)#tWE(qjaOs=u zaj#CAJCNVB1rs=85)zS!N|;j&X4@|f+&aa_?FJ@N3Tr6SZ+D;c4nUAV69fYt9Xh)9 zwl=_KrdVol$v6XH@ysr>v!nIon_uO%pr}1`P$3t6X}xU+J<}4(=LlC&H8^_ZU`|R; zRWM%m7L1bR>yML7!QV;GE@V8@Ak5N3e&WtKmOPbGgu0t&8n=9*`zMJ+9Gj5SFJC1y zlX?IykPb{a3NTPOZV!ihDU))3FO!Ei9UhTZm;1^j+;z?j2T=?kE4WTd1L^P=?4j_>V4 zl=8~d=vg68CJMM#FSk3AyeH`we}ARA1g=&{Ei@0Gmw_sGl+V>#ZnW>k^h2ga8?n4Sj>uq@dPri_^hcw}Fb>l}y27)3Q;JrAnP))?_tv2uX#+wOHn0OGX?hQZ>C#jI!Pp{>-0* zwWM!tg@9wa`>AApXnK-01la$byLU;2Q6b&{Z4&l(3ih*QA zwFSc=UE>ee`x3_QqvV<7<2WkxcrHd*!5VLhivT#Df};j|WY0##bq*zmd*uRNySuw^ zOhG?8AZt^^(X90j{bavR1us?WL*7S-<)?-;Wp8eDn@*EmNZ7X}PlR>n(}FmT`? zrh6Yq$PEZ4#NKDY)UwDV5#a~vAmzUQo7HDTM5`?#u`}{OFm4_3b4HC?d36)kSbV6A@GM?mU z{*acBO>Ax1ljPE4!>^g(6BD;55Awr|&KX7)3LQ%|J}^@{)%Zb?6)oyK(-i0*hyP258Uw5My}IgoX@AGf(_`!)>A!(_!R-u*}mY{Ig6ZpG*+T?9dz`=otDT104i$eM|8qqE|Ww z$eP$C?^YPgy;uEAf2JSMMy#KYKlD}@QmU&DlN`T^N=jys+uGV5hyZ7-WL1Lx{9PM9 zeCV-EPwzv!+%GP1ah^J2IcNN(ytzyHgkDQLj&XjWkY3qzN57}#6;8uwo)%Ug`Ev9& z8D@jGhOubEHp_s|I9rJ58K}rDhcs_QT0Be=(zEX}fAdK1e&B4w?*1oC%E@Z`ytK6D zC5)1V&%iW@HEh>n%h&NCl{3ya=fRP516%m%)7^^s_7LLSj*XS*E-i})%RQZO3o;D6 zg{G#;C%%b#rYFahbuLFAlm`UAyn&jw9LQw(7%VJc)?1(yVDQAKK)P772I!ts0R7U? zJewn*UX4fSEq*d3HA}waLkZsV^dD*;3uJxd%hxo?k7m*qb2PK4)Xncvd=V;jLI>7srAWzjyS{!#E>zdb`vT8LG{(0#l#+5k1g1nwgKJYYL30PaV-l_i{|7I(Y2C#zee~t&;@LT_T0<-aU9Iga=qDOx zg||K!7Zmvz{unIU*v_!N3RsaTd- zn3#k0D|jx1&DF+f5@_c?h~D7fA(pkw!=H{qG^Cm8YHyJrd} z7s6GZMbBV;8-95P3;2m6MB0Iqfr8ZKm|I`FBQErkkk=8Sb#ETN#=)DmbIkT?6)j z*dWxnD>F;n`Hp+z-FM0(;FEE1b61{E3}D6&>;}I&t5^;b{QHPQqHgb7ra=CH`35v& zm9Q)ThPchm%?IMc2_lB#;@z;vMF|K69mnWjBY_1OCK$W<1qXkh2SP~>B@rQ^%!l+m zyTcX&X_YLZC!{rOqNgyF1{C<2vn*y0vQWp*6aDE3IEc@59|6JbB%q30`CIg#69Dho zEid%a_t8WuHu{Wg!qAZJQ-4@hc;gj%?>10zYj)WLR$d&x3ui*xOOO)SOs0`a6gi2k z9)zN93FWs2u0Oz9siXqH@Gvu@cix2Nbmeh_cesJP%Pb#KgpGqNwi%tO6MuMxIiu4Q#GN6o1GdP;{co+62TbE~!o=J8{te(jQ3kCle zv)m9ugKC2z6-G5;8!Xa1p}e`9NjE+0ZZ||sm}Fwnev*eg`%o)1mK-?=zz!ty$Ok}H zfQ@Z1jAImbe}FVysRkXbk+P?LcKLX{RZ=OCgOKuyWo0{H z-`UsKcg=~!D?2+Ib`|%4&<_jFAy4Mp5ep||@pHNrQau{b_>67@nbjbY71h>`rGs`SIh2yq+FGCKh1fb|Ig?YzMAgB^VhS ziwz6=6p`B*14Ot5X28teklquLy98d}dl>^Nd;d0m$X2{$6@GD8E8Fi7AfLm-!AB5b z?A3}gpr3@Ny5o;77Yy7CqX6BDaojKFQXY984nN}!ijuYJGKcfog+s4o-Ef?7Y2|Ex zelo%8O`B!jvmo*}ik z*apN_UcY{=5Cnmay5bB<_2Z0xv7nef4{L9I?Hl_I(JP8oGX+azK>CTW|VZcmc3wQO;jesCK-XFJJOXMuGrL z5!=922^KjI3lx0UPVeJ#_sp>z8=))+!mct81fEI@h~C2<7aHPO+qwRZy27OK_6!Cv zQ#j2H6OfBsj+WFbGu9M2;7}4*ZhdrK>$=gD4M_|s6-GY(w#Oq&pqyT`yPW!38GWo) zvdi55yz2E(MnHWNw+zm07`Zm>(p*PlZw*K%Kclcmj+U`j^~Ke5os6_Q6WJ=)U94ab91^STB{e# z$7(D_x;Jhjyeo_rhg5LcZB#OWMZ?~2je;^c^Jy|&bQ0FIPL50BzFK};P`C#q$d^J6 z_Ak!%F`?o-II?~B4hVabAKY4RaL@E)1ojKe!@Tp=tIFT=RYC#Vrk7$&O)JBwsz#Nd zlGsFBE0KXg#3#4-ii(N|7#3-jlDin`BuyESsZuBC@f3Pf1vLAz8&$qLK@qZI*9MKd zglJmrZgpRM>ToWj$~Xb-bj18^&>}W=<<^g?V7$WmcPB_?pPtF4{v*m0eCjk&sw-dU8$=eP#; zP|l!rVfqy&Mxd58RcaFP(J@353Yp)qVh5q@5T;3xb;rvs67)9hA3m|LvZB#CDQc%U z+H*g1e71W5KEpZg5!;I3M8Y1O_(z9_ygtoh_1?36?#5-V-_^CWl7VV)^(r`x>v*5v zc+KIPvePtWR$cV79Oi@p6)@y( z5lPlfl$CqP3yiPxH;qGTrmP&J6KFyd_Xhh2UN-3(8@&=*W*4ByAHOlSHofF2ELb?v>$)6ZVm*zyGVkg?2I_Dqy%7-zfL-UnblxHXqG zA6qPA9()K;=iig z$@Z%rhVH1nMdVQ6&&4tZNcu-4_12v@P`)H}CRVDvru)qsRrAt-3boi;iDQD$lP6E? z?Cf5?tR|eu}39sD}@E^dD6w)t^#(SP+V5NvZHufj|GY)7A z{s?5`kbnR(s5AgJ0Fu;FWI?)}WM8u28H@zXI7pe9KR_yD)~MWo8J+d$#nnl!0zj09 zojP7*cnWyS++BZODZL&-n-arx(Xx%I=nfTjbjgQ5~i0~PAiUQEQ<-$;*ET5Ur znPq*Y#+rl$U-d1aaE@@%@ZS1ujv!Y0P)iUeK=STeLx6|;ofsf=A5k!b6pjJb&F0qD z(QvQGYfyHPIa+5|w6OREo~@wSA$mf6@1DO=ihq=lSVC#`$R{Mt?6oS6c1dnD^Bi0Y zGT3^6RX8NpU21$)t)S%WeL7$_O<=}drwMUF8%wrk=B!a zs=Wy_4b$AXJh&^yKX}YaO{e;>%OI#%5;SJw1Vef%ydmzi9N# z;dEJBZ@|WLK{iw4?SL)Nod1-DAYc8_Ve(IsEH;W)+LOTuvIqSvh>uM!*?%pIQtuf8 zd)saY$YMkUpmp$lcJ?YIC6u7kT0rrlqVg;RX^=1`SNIF{_4R;SFaymVm{+ccc4}(z zv7~SvfPg;(Lmq(rU7$SXO_6`yX{sc;9DnD|o$b;$Nw98I?etq0Hg~B)9?a!mRl%}S zdT}T4rkTexF8w zGFHsbah7NPvmr2*Vgqs6!wjWI@3snWOjOw6>7;Rtg-gvdiN_y=tX`>Y zI5ufKQa7&x8t)}FKQ~7rCicZ^x^V|7|6=%Nn+zp}gzqsRe7+iv55l zAZKDrbOE=(68b_PE*Q0}O;*W^$Z2TwSYem^StPjh8*)ZOZktN|3`>m?g|y?r6~bsQu~ebp6Z?CNVxWmssthD;+~*1L3@W+MtRU z4n#vkJD5i<;1|RzU3$WGs|FBzz+RB1tWAa$feN4pkTE3i^<4(kU;lAFCDTKZ-1E{s zrF?x7V4VXoc;+JP-b7<`N^vN~d;LX_TR?^auE-LYz{E2j?C%fg`(+Y<91&p^baXHk z-sE#gAWAr2`1mcPOI%r|zh0XXqP;hvzNAUF5ewmONc(uK4@ zT{hfxFq#HfWkyi#>hcwPAq5W!7IP zrKgy~T`Y9YcoVPqF|X^mzTn75fKQqv)>D_}$!G7Mz@O0F!D2ho-X7Z~##&JDy_2iS z#O0_ik4kvSkGY8sHs%OP&E(}jCcqPpK?|@*J7{Bbvsa zG)oLM%)V_uHEer40RM-@ZU(8xsE$ja=?PG>ifdl-aZG*q1@>ipaT|OuQ$nDE-hPd@ z8l-4|LJo=pbk12g5_%~6Jbjw%n9JxW=@FJfY&WiRj0Oke-O8-5cLRGD)3TR0oqv$8 zXIv&EIEVdnJ|Py!T<|DI=$OHTMA$n-ra#glGVVUqHaT^zu8<91uL7y$lG2 zb@Xq=VYbKe@83vb<`slVG*8UKwSK60?V; zgf$BPJ2#*$+u9rWokAf~Lps2InUFL#Bt#nOj%E34BoS|}{5gm@Gd?+}sHpq%z{d?D z;|l-42Wsf>+6TD@vic%NMgGK2H%yHZ(QH1A8i(HPTWoghKk>lx;+lYGr%0K8Cw}$e zpg6Ovq6Q3JHLuZ6+^PAidcb0U>Y>~(GB@{Lyz@jzGbuUw>iT;BSrtmbX!&!hi^0$@ zS(=2-C?Y&;UASIepE3M3K4OwDAOJoR*Vq*p6yFtt8RxN)yP26wtHe7A#je42S~J*+ zekZNhITxR=k#j~^e04qgZ1cnC3J-wj--Lu*)|(Za9Jpvxi@Agq-4foE_ z*>;&H*g;W@zNwudf~WhiR^R}8=uN8+$9|0b6mt+Kc_HHU^8~m}TF#46)4rFDN+DDkM{gzVdQyB0tn4<{b?2%N5Skwj*1(f}}?BvG3#GKyr6(0ey zO^m&r4cLsWSLp*R0-7tw&tIUn6EU_O^q2M<9I$=NPESvN`}X_t@==Snk*+SWAmI{V zMaPl|p-@~nCeL7e=K(A=(!5tO%~c#q5U~7bxmq8b4+i36V@0wI8|(%C#Y}Tsd6w33 zeL>iE66*Z}d0v(~31XiUQ~f-kfd{cLZ_6|`-tJ_DiGL&s6B% zuO$S|k=YlfTb)qVEX!`T6A=-?mWJRB31lw9O}2mj{GF=7H@Bl-S?^wo<AZ)7FNwSiwIQ03_*p*{?`1ygR}dK552|$Iu2;OeCHSiAu&i{cp(E) z|HmaPGX_su4N*V0EtXrEr3ww|DvwaUjAfs_VT2mQ3-|5at?w_iUZqZJN@UH~AInK& zsus8OSU`iBk-qJ3V)X-`V23v{>IIkOQbUo;0h_YIov!+ABX*pLLQcnUvW;ZV8;m^+|7 z-m@2FP{BcJ_=n)@MGmJ@2kHlpjK*D|ZODkzi{nsAn|mA8s>7i~wcaH6<;|=n?En&U zq8sXgmA60j1cs+6vYs%26ZVl=49%GkW_VBe8~bJZ=CJ-?4vsvD;(9@TeyOn#Lg4>` zaB_mA9zWPVK^8nafPK$hc*|k^jq+G@(83r&0g5bQEiCwzfaz3$9HQ<#p%n<26c116*fA2b)Dt;_Pueu~du36nft zYFR+NmR46mzSBmwKe8uZAaD}$2^bzY%r;%dFGxDx}%fKUvoAY#`8!&7C^|c~|Ef?t29UV*Z zT=FJ>*u&t+AWKY23dx+K3ujX+F_g5k<49wOF7Fgj{{y8sT@JYYl|hTFtJ-cEpU3+2 z^c0x5;dQsj=JBTwQU0$$vl4vckogxLh+q^WnNAT~Tl246cQ7+DIB&+Ue$C1X3~ls4 z@QYIkCfC@u_gFA8CW^Ht@eI)yr+IewE@n#3V_g3JfPd#{Xph8+kCCqGkHr_MsfPtK zOO8|3F293h1dC4V4rW2CNqb`C>Qu zx_!L8M}~(R>uJGrYvu>7#c%XKh(-;0U&gussj}WI7Nh-*q1L)BbSaXVa^9q-(i9P#6P~G$u(|-JjB0GJd~cF z&v5^~J?vV;006o@_gQaZ*lngUI*7n_9ul*EKVq7r<|SRN{!EX0VQ~>C0CGt~$Uz;B zRd;9S@9k}llWAD2H~@|qtg0=k4x#X{H)Vi0*yQ9S5WR3I9>fmHQGuz^G00xFZeQ)L zD=K1zF4KK!fsSZbq7!}5TLEZ5$n_#h_nF4m(9FC90gm($B%vIZkE=2=g#7zp2m*R8 z>5PiYAkE_)NP^64g5Jp84DxW{aV%4IJN78yItD;2l1!)TU4bVrARqwpEKvGD=^9w9 zGJH3q^bdNwL2*r}9Bhh+l;r`NH}1%bkk+Oqi-2(d{pm+cC$HTN(m&0bxF`AtR7B#} zn$O(`s7AZ8itatA{oPh4IYC~Fb3#Mzw2f#MCFj|KS{$@Y;l~ig z@^95FHD}OdE9DV`0if6oP?E^VA`>f=$onfeka)@Js~$|4IKxLVgChZlvm&zb=lXhb zWF#R8NhP#k4<2JW1>NLE1#z{EM>ayFW+0*pxw$}UO-))%)-u_X@9p*-CVm>IS*&TE z5yG;VwPvB8IAJ@i5>D}J&4aFCbAnSN9mLaNV-BBW=F&Xmds{=kH^=v#*0Q!Z$dZ3A z*6^n%Fa8q?MdLU|fyxSkIa$dM=m27KlLFn88IdKsYS>jCE92MWLJBQq zcp6EWPn2S%5zMgIIXD!mm37^I6IS5#)%aK6%Res=XcZ%V5s!XIu{&c=z{rYFAkomk zK3-qSrgH_ps_a#1P8?er3T>7>q`#UAlkPt9Q<|k#$?W) zqc0^rT_17}$QA{>eH+VbZ|da4eb#II7yyxU(Fgz?Ie`ZZ?F>+{v$C=RPO?z`qv$YS zLn_yDa&Qa}4Mn(pDNv{BpkrW2FPi$pyXWxX;^ZVXl$Dof2huHY!mwRFAHNu~ zVKY`IXy%B7DLgMw5;q{$kH_W;WThYAwP?M!izL22*phho5OIAJ0#m%9{NsB=ffCf> zvOZznviEmYukeh=5%oFgi>*vSzyao?$|VFj8mT$#NvxX61N9M#joS*KJ>S|I)~g0u z$-p<)zKY;`t+4_%B8VZ_r~)R&@P2hH_fuLK-^yPKhe%DUxXdBtY$na>NZ{fmCQc3y zr#C(-OHI86B(nSWN4C?FJvPtVkjJAv=i|zBSj4RGp%>6-R#pI|GQGn$J%1fCb6S4} zdM#f`RoF9TV`nEIAOK@H7%^lwKJmdxz0S~D3*1#;ITY4&f*=Rtj~qva_r3pw%ouCE+;`whHp5FD7P;_?YiOPel1cL6^AlQ*& zKjE*)VDJL=!C-qT+5MDGUy8aOUVMS z0sbW>Es}dN?p}q$dM6-pzqYdCZBO2a4cnKne0|#l5NjAq5lR7-8U)8bb@RZi_lo-; z<%%>0*s_5$1YlBg^4&Xk5dwY=Sf1Kd30;#o>krAI;gha<4{60Vq?s_w?K=&fdX0~cptm2SDTdQDdoXgB7EPE z9LOXO4$1}2A`l%Kf8!oMiim!@vb#L_PFzR$UlJco%rLI1e5I4EaeB3yFlcAp{mkW| z$26JM+u(?c7c+HsOqp1qx+misxF$-*s@t-&^2J*oq`00-!SGA9(??YTgc^W=WVZoS zl$Q2(@y}i7e9(QZKn97tFwH9m@1>|4JPc>Z}Y55++~kYOe3DO(i5Iu15NrDGOfv_e1p`-yL2M5xEjN zeUlIFWVY<&*?}PB*s<4~L07e33W5aG-SLe#3Y0^Y`kmoGxCLSZ{$D;K*`rec(iDzpi2=!>Z+{fnnyCq&z5f%<`1v)nhIF%*9AL*t|)p|x$(00N?~E&^mH=b9~A?}x{h=SXvW7AfRVnpI~qR$Nu^Ul#-ebdp?wgfA_cP z-=H|~scPk3@GOY=VrySOL)@vid>!@}(y%I8zOL^YR3JGcribb&iBEBE|NOCIwc9 zNBV|F_Y!}1FbKiofUfTQIu~82K-CHgv$H)C(8thVmO5z6fs2ca_mK{+G`E>%tIq#= zr8fs8BeYI z=!ab8HWQRe$s5C50hR(afYYj49MZh`P46n3gJ;=5C*#h)IO<(xTlMxLe;i=)F4;-3 zu}d?I^hh%aP%jJI@URJmOBffo>=6i26S7hf4ZXH4l#ohkpSgYR;KD5{8C4|j{O6Vx z{iP21!b5R?U}|_c5?*p-q#d|d@EOQzX;EIkE^6x>Ai#=_K-_$B^8ymaCpcj8edS0E z;2`iyGBYurL>I`Ys)BkD&?gg@0(cukgt}44NB>s(>Fz$y{3;BP`LQ#L-ZcAbYr@O} z*0##cWm~$jD$lof+MfkZx2yArN#9l;??=1`;!6H55jGSp%vrQKFr23Ffw- zMFT;)?I9fUB#eI!`B;l4RBXyQ3Xs>DvZ<%JV5~)?_#Mb7=)az4j1itk zEaT7fIQ_bB*+XL4jFAD-isGazP|Z_OQQgS&gO^co7h^*lp)RRvQvXxLv`i{X9rmUS zV2`{S8qtXfHFF9yl?g9^L(s z?8KGB^Kx^+E(2t@;hkAn6b!k_`W_UW%lZB-i{%W>Z~NjUwC|fCG9Y?vt#UR0l6Z{G z>(}5liT(0CO1@DmXluXk;T8aLbFr}`?LS;VvK@7^w5DTu^=AKtz2U`MXJ{3ql|j{tikB+^D#9apZaOTy7A$Y z2ym@3jydhPj6g8WqIv%PVC8FKz;sqpQi39ObdXU~H2CdXl~Q9_&|G=^N*417`7oMC z|M@ThetF%3kA((RW=T+#=6_X0?btNIZa{Z5qqwwm`vC5?bW3Y1u%J^Y5RJzB;g409niYYv5Bx20P+$aehgq2!wUxL+tY#zNv}V z!%-CI;e(RGcdkc-!uR%ecNei?8-z$XlYAD;i)+8ne>=2Dre)Po7Nv0wOn-#q-q9g4 zjE127q=ya^bri&krH9$LYyR#{?TijTw0wHJDfqlY#vCv^RZ0iqyljpFU_m5tCc%V9XV4Jc-?a9Yv97eN$+$3IGlMC_6{sm3G-&>KLZ5IQ6O`llLUhic@J zo=_uFowY!b57sUaS78fUPbs}Gh*Pzxy+cyji+G-ik%1{tp zze!LK$QGlTY(~w2GLQeF3Z2ayVg=*L9(-QFQ{i}z=eg{yfyIxLo11`-E&`Fr_Rn=k zW0z#0O3^_q;hl4F8}Z(E6mNRRy*POGM=a2z?Vhv+e0|{HOh6#7sObHP+*%O4XJJ`^ z@p&ytO%=jp5FGlD>N_9TFe;;`+D$K`D)}dJH&(X~Xv)dGm&=i;%Khj3k@&))i@uEW z|9Rr!&b8EZmDh|u#1d0u$fC!`|XNkBIWy4|x6IZ|2TKqQcmKthNZI`B;_{SzPl z?{8u$6VOSCz&I!WRr$(gT(+Gm`v8}DT&4$!WDjD=Sl_t(q$?~PF8dX!i&YPa%TXVC zl=+5r5Jo`mu)u$ZhcEqGE?%z02EsL5UTF4V52sAc0tiTOi;A%55giqD=MWq!GQ5qs zLh$$~w=jt^n&e{{UkmmOZ7nSu$pX>BU9~g6@lOf2z^Wd6geM zk?^ICA+k_q4!X|}NYA`Zq^i`$zu|Eo7V>UUdAjVXu5Fv}C+~C-S$^eYx&<96AYjf< zd0}-Li0$AH@(0-9Mp$13q4lSEBS5DH&Rssln?5YWSXZV8lJS6zfGDTEhrFX=VYZkm zdt}wNHa5oBv}9yFiii;i>is_-Q8!|*kHqvNGORVGol8`rI$RMd7to5m4#{v$TyLs2 z&gJ;`^RUj&$!Ny9z#}9`P&Kc-wTRwggOKldPIk!&7u#hRWSX);mn^w7PS6K9W zjao~qFh^8jWU8jBL&P;V^or$yO<~E_w~Whz!_VY|)d~dUpX2Tt7U>R>{QRAhDLJHt zjXk%yb##rIJa#ln<#AK|{mB-;#2DhefhV8`Z*$J%Os=;9z303MRB}4>hOZghB7YA@%&=DSBYX()XMl0^7`wAnFwITWv+k6E>3;;N2fl1T4 zdBLCI{-0-MlG3ZQ^*uTpccGl2&2u?BYiU7v*d)v4t$emy^51O;Hf%O* zR5JQzVh*0Mt=sI;?p{jeMH9b@wO=oM=e?9B?EIC!!pbq>zEOYl4Za77F{aN(%F4I1 zf+=o{wV8hWLX%v%!mJrStYD;SksN)u#ITd{G{xvvoqc!RxEGeqaivR9wVHJ8PudW& z9~>Rkc=!Wtrq&#u-ST_CrX1@+OkUBVoj+PsR9{z~avgC*#8gf83LV36|FB|Xn}0AW z!Xu+Z11H!ch{)qA&cnT;BK$Fx-e1~90C+qt?f+!=xW$c>FwmEXjJ`#CqKyo#gSLbA zqIv%RqwTxHqRO6a+khx2>L>ytpdun!Q9z;*5D^ft$w6&`WF<;gQBlcRvPy1Z0}YY| zq)7sjGpOX8b9&WHQ0F(!z4yKQX8xP+o966u_O4yEYOPg!)xUP%t#bOJuzFCb8OLXa z4k=2(NK-`Ks;;%SYGw%x>|wlAeMuqlD!F0j>_fHL(fEDd395G#>vyJNjt)mi_8BDx z9JL%`Q%t=vnx`&Ri4N$GmJ;`3hudtFHJEQe^-ey9nL#)~b?Tz@W$M`c>b>k7OkxQt z6GD*_@AvA*t(xDtIGU#?nkX!tP843niclV>Qaqm)6cc{u`-@6dqJAZBkQ;ihihpYPB?clAipOf zd_D=Y%QCvbaV9;i(RY{AATK)VwF@~75#q_mQsZ+MVHY7p&LY~2aX0UxxY8YpAXFWw z79y_QbFf_J_BgGae%t{E9crV!$HU;?Bn+p`6)y)t4o=j9=mSma*mm|R0s4%L%7SQV zA$^Z@qjI2mKde|8P>L{T1sjSFna$N>;DE9J>`J3pN440NGp zw4Qw&1V-m|cQQ;%Ro5npM-NdQtN1oKIi=YrIflJLi=N4uOh=7#%gr34VEy{v-yZr$ z>rU@y;g)-qpX$Q1%U#-C`aqKcRqnge{16I%%rbpyvP=DU8P)8ju)F#v3^LHEcQ`pe zYk6hzt)R=uS|7;y4tFMM^Uhdau(zg~zdkjoQB1ZltDIpeTN&iRYm&-ei$Q0rNWD~& zImyK(qn=0+b?Z^WV!67;Cqzv2!_bmPm$6H28OE#G9jjkYP;q{?J?Rier4qvTIL|KG zB2h>h>GLJD^W`;cuKcR`-VPi(B&^V6nE5VBSaciWQ;j0~F40dol25yLy<2Kh*rOlU zYo62uiR#IgzPo%U?w{YM8~bnjM5zsT^lPkcHahwS__oQuK{xKB4)Vz|7t@L~9r)2v zGxR`cj&uAq3=otOc&~j|;-=>nhwcc!@b~#Sio!3P={N+f`myNSYNKk{M`7m4=0m5O zo0AQLr=Di- z2={-EPPRLryjpcZ!D`O#({)^5O~B`WI>@*f7cMoTXMqq4VACSXbx3}`9Ou~s&WHV5 z^Y5dO(9E?3I~S;hGP+#(t>#AYye4o3F!9=UoYlVTt>i1pTMjMX@4@~)_tt6hX9yhF z3V}@u7|dkP&h@Z*GKPur*fv-JJ>`?p^DWoT%4p!jD)Jo2X`n5$7z)#20VrOZNwkLv z)$|-3ZVA+gf||aa+`)+J!RC+Qx32E({g8d`i8H*1ryz7U4LCMz=|Khck$zm0!UF#F z;G=>^o%fA!C+Jp4-MsmYbk74~_|o&)2(1C3!lOIsg-_!@Jvk&{jSzLx&7nL1+`K)@ zqN=P+DNWzM-OwUf6EHET{(`|l<1`&R{%n83AAik@!KaC>)iiLB%u}=E`yFahsFp?`UWG?q zFDMM~tU>kM%*&KI5RV#B5|e-z`KgcStn3`s$O%d&P7Fj2bP2TF0VKCg-~+pL@Ls6b z11$v-Sq~ay`8b8|eC5E91wI%HGKAZp1+*`!)&|YV!u6@SAwVH}2NP$(f5i@lJMp!E zR%J&B5hAXm?=Ff+Kkj|hF`F6<%BMWL#`~V&@3=Q)HQnx&gh+oM#W4T#WgZrYdp;no z!fMgM1ml1qxCwDQ@z1P!us{EX$7>o^Zn+}su^c~NUv3cQ;t7tRYy?|2U%h&8jR6( znAg3X9h9F!cW}$$KkE&zYkY(d`O1asC8~(TpRc~4@sedS*k5x)Kn0YlIz-{0jf5PhILilO3Usb$}`;y~9ec`hzgAw(PaEBCP^k7r|$hea#IB zT^56NVl;~6D%u&)Z3(kkSv14;@PPS8-dfdjbXfwDEs+s?_5rwvuQL2^K5rAoAdXGy zux<4vTcwwKC#*Xbt;EE=&T9)W!`)fIqxpq$pk6%F%*$@2J0`08+m-X0dYzzgx+iwa zn{MSsh`pM6njTU!VtRny#idDMzypDJcTGFvvBI81s6NSzIW>Q7w!ObBHJuGk+aZ1h z{kV8bFz5rikB}(H=q3Q8$`9%RjWW2KMKD8~?uR_wcvWoBx3pBlV7~!MQJ_qmW9=;UXXHHr!6n{bsS1IY<=nCL%Roo;4}4ga`ZV{ zT^JA;OV9exOUMAeI54_sdVmgq*Xfz~=E2D6wOOOJND>&@>4t3X^93LQI&k{7XB<)x zPwJPB>Bl{B;W9Pr^*UX+8)C&JV*NOL0CGR=Xu5F@ht|lIeze@3Gb}Cj7;R@@5eIBS zM5Y*LlzqbA-_!kg;Y%Hs!QKka2kxgBN}Djov--EA)KYKAC2}WMRR%B^<&8*Bwdir8 zM4t4`mQAjkIIN%Jr)&1;5%p-F4~wVg59u)T z(DTI9EP{~OoA$F(>}RCoPX3W6Gh6ecr823Um zehRwSc$H4l-KQvMu37>H1|bYg3v6w3l!7=l-{hq%&lg`(OVM(%nz0b%4g^NG9TeU1 z&}{@;;7j&)pMH5i)7}pM@{w1AgGJrS#=kFO-(NwaHGW)I#1v%oG+|^EW1^0$CSUjT z(rmjpcmK+jZ&PS?+ewWK+tw0XfIm~H{iw8;7d@Tr;di`U#pUwqycFW%*oc{LS7PGp z6Vch)Qn$GqR#zfET;(a&@^aqG`|xu>a|KPnC#2q?aGSM|!G^i^m z8iOh?yQ$q%AKT$nTDnbgai-CVtg+m0C8Y77m{^O@5VE!o-UFjlCI!u<&_N|uHlOM+ zhh$7~->brKM{%Pqfh|AZv zHo8bNmB=@jZOgoOQ4C3#4-OZ2!`JJNCWVz&h1!~MaSEDXe(@+ts~yOP`U7uo)MLXM zKTL>CO?Hoe|J2HO+NxCnDS--`o8f=R*75PXT5DE%T7BjbB;@cYRE~KCD$(2fonfzD z;=mrg5p(^xiQ_kHPDAN=R+ofU?qu0Sjksq3vtHv%UdGRa3v9Ra+(Oz5D4OnX6;0f7 zpKwPr`ND?(7*|$D!e75O{(kjI&UW_2wj8c_p7Jl=XlAznxbpV#WYQ*f?KFbU3t2r5 z$j_YSap_}nMgHeKeO_z@6Y4iRcci-2#}$H{*xayTqT$>d~;`Wt7_vvm*>0GB%dqR9Pzr`XJD71P8(z3~~*JuxYOmmKr)f|F5cVjIog!oFRcto3j&#Vb~M(p{r z%Cp;z_I|5^-0$Gh4wig;Cig+JNx|}YuZGKn2C5w^&UrpUoWiZ_4GGmHHLGd&@KdU1 zINU;9ApX?UbWiKJl?C!>Rve`|cXjmRwM?^G+S-Dvf_~_{YvkmmpPpKMP@KuU=a7{f zDbiczCuDo-#1&{0)TTzb)MNcJPXuxkeOy(`$+j=3*VI`VK5zR5C-Q&!^j$kxu2}v0 z^m6GK+~YWvfuJ?6GS0&Z*J7OPdwg71@hLC#p2`^*Pr;pf_Sv?6p)$$eioyTfiLVuC z74~!#zHuREE5w!2UtrYxj63Sb`5B)xF5KFUK-{Z4Y*X zU@@3Xx{nerPr^*@t|&c3{H>f$*C%zyIc6HoOP@ zo_*H7ia_iRi~KArN%>y<YjR6irZ25FuVY*vRh^D$nq5-OktdlEtMK z{=V>=<2~0^Nd5}lvkh_FIpDsbWHcpjjc5*f@8jhL>gSKe*fTBspv1bgdhIB6Aqr(+ z+5L2fX6ausiHsXT=oMSLcB@>A8+pT57$ITB<)@9#{*Y~QGBwoU^CByk&Fj4m<}jN$a-`p+N`Z$B_awkmX5ik_C5jSjjXmS!+@ z)-6Ffit*e?gG8QT)3&(gu_~tB8a7u|L^9MyiH8b7BD7!N zcxyWU{)iTz;P}=En6OROrqQK0WqHKj@A>+LT>MLNO?b;e=H>VZ<>mQTlIUgcd>qM< zo2?LPsfaEUFBeln7M4DhUEoDrB~N#33(&VQ5UTny=6NT3{Fw#5 zkPW3cE@T>7O}m{smQV;Gp1hjn9r5ykDmE&7S|Ot*QR(3I$D6g?5%Kjmf#8;V$??EA zsZUsob!|5QagSDduJ_}2A>h3ybZ^2b&r&_Y>fWZJeXs6}QP;rih@pJXaC0)s(Q!t0 zB)aCM?`mCj*p60!susHsS*%2eFH&z$Y&UBAYS`jP=c^q$V|!B6W-TY}O`3r9Sy3Ug zo3yrd14qn06M#fVTW)@33cK+gs)p?%e!R#Rx+pcZC#R{^1FKTzvrdr@PLLlN6deHQ z1b>NscEp$Jzh0tIG|A5#eO?(KrWjg_TpcmOV4fA#aCcB%7TE@M?bTW+mFgZ=q(MJiN^_h^u`m*pkJh6= z%5mXAT3xtIzfxJHwINpIztcxH2g|a(Se-I78Eqz2kiOQM%@QRSO;Z7`>F# z3%19|H&dP4GlSdf(BSz1;A6(aeA{TfMu8R|&%fT4;u?FWSXbG_USu+QG4j(zTOpsW zZ)Upx*vx5VgcJvA%GI3Uf`U(q2LS)VuH#<_sP5;V{Zic@j8d0;UXG7r5c;o6ye6WK zeRF9|F~nNZZidBjPIU}+b*V)I@*{^sbFhD?#$V3BME=pILbZ>I0 zGmA(3mg$mc&W~=fg{n1g|MN3NiHe6jUFzRS$5-8dC#>U}nYE|+Z@hY`Qwb)RuJKX>^Zx;G8s98jR$(y|}i54DU;emTF$I7ahA zI?U!R6PUO;($;)``v%*?j72H>TT}Rm!w>^f)vU63WxII))mf5iTuYc^T|IBPXXi^qtl85`NLVVP6f*e~?io;a9a6}J`FH`FcrBJGPTG@~u>Hn8~ z#`st_I31 z!e9IWn@dqqOVGZ<#^e!jG$ygpNj9>7QOv75Z%w%|`gHchOM&@!Q@2-^;AY5DU796& zkUW815OB&9mw8VcTBwT$9{>6DFy0`n>QbbH*ZXT&f55R7F8t~1G{V=J+xj|RD)u;F zFz7SPo=Y<9-d*@kly`_ToFY zE~U6=_ld5VJBeaqnYYyvSms83vIPKNNWI{H?wXX(;6?9PRrCU+Nrs!Nc%xYec>%s6 z{S`e~5Ab(2i1xD_T$yjKnr^C0GcYK5mV~()7hyoaR-7Z@ec$sJ^JWt4@%JQH^78tNvRly`XqA8C)apxg5A{OfA7d!09!K(3 zs9QDJhH3>Yf$;@3l?+r*tIg^16ErR z>>J^Zx_7HZVH@iy%p<0)f(MT^LDo*0k1(avWK6;Y;E_^7Vsq#$>GoGbvGVDRq@2}I z2L+Nm7VrAB4WC*_kPU3!d5HBp{kn`S0b2Q|AG&tr`ezCJvAwZ?xX&Gi0A$X$x*M^1 zTWnv4w2ylRJj0`Q>yY*tekh9DwlbaMW9x4S53y~n z=zb@zW%7FUn&H5-4gmCHmP11fY!a106VdvHzC4}Y=TPDD90Ls!i!)wPA)XGz70c!t z{{nTvyLsZ8uta$gx=;kjmVnl~6CG?)kk&rlv*VR1{&SJDg_UmGL@kNdJ6)GgaY5|8 zqLRmcX(i?ufA3s&AWu2U&9bg(gwr4(;XiPyWpl9E@8+`Mq6$d%@v$F-*2 zBfJj2%u&=gXww92<*@LD;3EV-_1=HHhKdNT^Hk^4#1}WvNpd&>>mc-D(q@I6hXzU3 z>D}Eo=0-hz7|MO&o&lSahEOGLf2_($ZYHg9IyYp4K6b&HVUr z-`rFg;Mx<>(pgph0Hnh9@9)oad+P98arxi;nsGwZL)BrNHXrQ_9=M~K{Vz((#XU6S zHIKU^H^_9ukJY1FgNmmzN8$E?`D_s6M=|C6?}0%0O>O$Hz4+6W-x7~5jVqYiBgW$T zahK;vzgBaSz}~~3u7{iWI(!Oz{}vhzP?t%*JzngVMG+f2b1cFY3hTeA8PBvTtNIIEL=P%Uy+(`LR947e=wVXth`5oJ;RcPf_>QGpTIP3D zMTPqIlnd$(4$Wwy*0;sSQ54s2+X7X@S|U6*x~MMY$68I-L4S{Srlsgfbh85gRW%4k zaGZZ|{L68^Z$_=B6ks~=AO&&7qh~^J%3ELON<8$ApeOUiy69h&mRn zqd&c}&s}cFgpQfZ9!s^jlM}+S*JFjjnA*G;~zW%6RZSf^~1Lfdm zw-*!bQZN1S(KX3nPfYAL!liP@b(M^`%bRF8-ck8 z_ZWA0e7O~ao{XjvQfZe78=Byg9OrK6?Y(Ufmzc|pDz;iOL>go?uPBXa3XE1;u>uMpCe^qm zJRHqys}y1*<91_t+;Q^rq0!D%ot&}Jc2BP`ZC|83R^NZAQxCcF>R_f2{WWgGX=|}v zBW9{Hnh|X#ZYp`BYRdAAwu>b`0Ku#W6A_VnpVsS^oyRlbbeG?cFFm!d!TvcsHU!Wx zNG!x8FupEWLgN)R(qAoe=dc~Mp8KJvUjjgNL(3H_U*6@Go{tL%WYN?dEt>17Wi$4c zmF=lvm_xf=n!Q*OB1miPLQU~*k5LW-z35GYcsFEG5vMgb#88p5@4|+J@Rj&4g#;#u zs6Cg>iNgBuaT-2n-km!Sh@&bZbKVn^?QCM1Z&n~D z;I5X-EMxDlEsE;oE=e9?^L#;SJ7zr{-6501eWBleR|x(rf1RPTIc~dDwlp>ri=3J2 zTM|lqWUZ_wbLD9*>Cmf7D`IvNq}mHFD6&c64Zz+l2$ZILtRO361!zZlALFZWTnl!w zl>ttm;$Ck5@cc!5gX`x3Y)HmvX_l_8SgFa#OIT)v%!Y-7d3AWM8i%C+UxL&yt#Xmy z)0d0K@P=J(?yf+I+%>f-4&wpO;kJQB9LyfiL&{KR<=!*4r8F%T~l9U-Y?jF?x|k+TRvo(As%voxM8}{c;7<(7`0N4;|r>xZZx`Dv3U>0^ zwdE+N#`ifPiRwEnI``SAb4?)NzDTm(e0_35_YGD*ZnKjtttkDzucN>Eny07f`)mO~?_TZ_ z6R*AWp)q-~S7OD>oL;1a#-g7tyS(J9y1UIXrV|jb0lK20mincr30>XkTc>OnA^lq& z$ca}L)Q?NZsoU3_%JgYwZm?%XkCR<|NIP^u_5wHLEO61|Z#Fz;{PSizl%n{xE@m+l zRWE9ce9I}kpELDNsc)N^-x9I1)tqmve7iC_8U1Laeo|eb9_{Uou*R0+th!!`R`{vw znizx%RXPpV6wT%8=veu@sSJ*gWz4xre#chEQzhfC`E1rsthE zvb*tpT~XfTO#wH$gY!K&qRo2U9+G8*E>Aba^1#Y>-eSqK;QS<~F zO?O8Q@+ljBO%B3Gw)u;>nQ31Q;?$N5TTi2K{RLxojQr-0n-uD4aJV;oJDBJ$cXN^u zdX1Z9mW%Ut$U{a1{xb*Ak2^ABb5{X?5NPf0PQ>GO8~r&1;^2tMeQwg^)*gTDQ#_4| zlw4(F#A_S%Q@tkE7V0gZnu_YMcI%PmE=(C0eN0|Xrn!%DdW#Nfr25+6U{Ft?<4TJ{ z{p&YxY>r*Jq`t(a)M%~h3Fo-8W_6{faQ_T9dKfJ~XP&=ZwF_U8Y;DW4jfH5L)8y5q zXTEX)Q`+dvDFf8QcwFR`O>czoq&#$516s9ND1sje-Om<+0?i{-+?Eiuj5 zSiAP5d=y6``pd26)Y7iqLUS!xaiiu4C%5yvQZaUuuXDqn?=)ZoQ&RW@$UApR<|nLX z8wkj?MW`U7DBZnJh`IW-+*1y-X}M-tY)wj(&60tQ)|Dm&eCDWi9>0YYCsINO+H_je z`ZW(jDr4C9Pgyz~(SO2_9a+zm-AmWYLMv+9ui4IhPT0K5U`Qo2Rv9)cr?3F&cbXS6 zg8xSq#Je-a7}V%%RM;`-2dLM@Cv0y0RQz*apYDqAJH>@;NN_P1c?#u;`346lzy+ye za#h9?kxH|LUP9$Zxq#NHN2>t<375$_6{Ae@vxFQ3LBZ_^7eKr=^cEA?8uP4GTmp?G(-(aJa;WF)37HOH73NF1pVDJX3H)H>5 zgYI)mA|Jqw0(q`jxf$q^%F&%!4+~#vAS}EJjDr;2YA2%mlg1S|jCE9Sy8+jYlLUX4 z-QptLWncNYcFjx4N8S4l{J`h4tg0w>SGa&A`*+#ETbXuJU_AVz6*V-t6V#B-|8pV|Yf_S?UbpE-f`=T~G;sqe&=j#jAZruxg zGhA76NV%J#LN!eQTJ_s)gEtMuDTH;~U;=H-*^=!|9c2?_3WRrr4NksVd+yxQC4sVmb?q+SQS+c~DdW95>+D7rG zWm-cZDYQA6XNBw*u0O_m5+-=c*(^lpH=ne$)Y9T`6F6zV;FKFeN}(~Uzpzl1!>G}o zw(oCmc%6Gf*-F?I{HT@-RD5>=6`_p6fPQ~?6m)k-%glx} zzFihHi?xEdHCCaspis}=+-yZrKltMZw}2mu{Iqihn_k_h2|l1_ssmy5839_*P+t!W zs3zPSN1q4UUqTZ_`Ju`1+wOVYzd!DJGalOn=wr9YoV58#!{qI-y$&MWcQ~FS6eBQAdmaP4W!SlpffCT7t@dN%dM5!s0Onto-8fpGz_xFygRNC4`@JQo^R)}h*qWZ zJTN?4{<1RZu$GdNwOTTNGW6w(chw~ej^6GPQ93Bf<|iiWbkhnc-Xbr27d_5L<>PuLB?Oyxq6}XmzBAIw-r(r$3;K8biq`yX-{nUNy?kHw8 zM7@F5AD;YpSYnUxvu!P+-hu#KJw3Sh$z4-A%V!@;N#Oq$MO-FpYhaCT=Wa;7Fi;~b z+E=+4HA0U5)>LzDAsT46MaSd!@EDcF$WE5fFUpVZb!NJ9^t!VF{;SIb znoh!?i*O>HBW8F80p`kBM~mE$(j86Yp|3crk$_@w?7Siv(5O~>sG(&9|8@L$$laws zXF?fz`$LT9o4;3~aD6MSU*D)2T-k2^)X@6am6$W@<^8Q{@3sB!c3fWE3Gw&c(v#6* z<4veavRglnKqCm;7iPePjFw&gMtm^TrCwe6s=nkndn>+DHRd3rO9%<5E=m=WbpNeV zeQQYbsMlFT6L25=`FNv{isAdQY?(2{KKCQj%<4^pVf*yZ%YC%gF-SOAKAvem?z=gp zc9bjhD{GVKHQcbuTurAEtr)1C?DkT6W7?K#TE#?xPt2hqN4CTo{o zqv_6FZPeB%!MWIRl)lSJ5J$&(TaS7SHMZ*w4TejFev<7%K?(Z6_}hA7c1{n~qh0mt zVeHl$IP6UJB$$YRZ*Kc)Yl7!{5%S3bP1HyW(y)C}f!`|iFu8CW&T2Gh^?g=St3pmd zS0dYY1J`_9ik6tzYwJfdx|YimY0dZ;8uukzSc(x5Npi%|uTC>A6mbOgP^gWw<4g4i#?+e#Af!s`yB?2^R&Ps6K@ghnh)km` z2EXJ-{WwL%Wc$33@>--Ufc#`Ec#wa^VN&-|nfKXwp(rW8ycG%M3qWRTIyX{jGf_0Y z@%E%sFoqp*s`Lp()I6sysjE9wobI$wj8e`r6*C{bkPLJR-B6d7j-HH1O|rdTJTgqM zk3_LU1iEt9aW(E>+a>3OwM2U#N7~7^pRk&@f|-@mp%f!*wg-+fr9`Br1>f)`>NQKB zq!2y^HzDTpS|8U|t_?_vQ<*II*ZCX}aF*r8qcBi&Hv@Y>w)fQ|9=O>PrKj z4BfAOvLmy;5I5K&-0mU~xLw6QQLW>W%y4q5-K@WmFbcZ#ge~~W{8lh}jr_WfJM%67 z4-D9dAF15{Bc1o28)cJE%uI&>)YTH(v!)-7WM(|q5i7ynK+Pkd3*L5tdOps4w@Y7F z*&;*U$;I@yDe{@s#NN45ADa!7XZ-6+Y-EU;Ms1X;{w?)6!2->e>GgW$YoCBS0V(gN z&3qatlIw{8AVm57h$ck@p*?_T=h@=^Gy2+ZfBS%nMLsj?iAzhP+;=|j%t?8**JibQ z2r&I4e`(5A1)3L;rx*I^skm?1tyxb&0ZQH7H}twMZ$U)L5S@_Hvx}`9AKad~b;atD zw&Mn|V`&DQTF>qZatyunV&N9sv;jSw%p8xSj$+!{IC+!u$wrfVGe}9YoyB$FSON#mtS);j6QQ_-LaW z<$@L69}{chyPs}W4~1l8W}bWm)_lzwwW2wV3{>gqGi)QB>O#x6jl#+jtjvr+c)*na zItfT#)4y-_Mj$QR!fz>Q5d5SBqRWnVOFwBYpW_BTGd8TO7QZo=u<-COB;%HYlJQPZ(7j{##xt9m^jof?)sW4asz^dO8bvP6^2#r?vaGiNduN*zz^JJquKG~$ zY8RdHSdN`RTu+bHk9yC)Vq!cNt#bO`epXc#At(%T!vO@p)S0od9?e+sqZhvlTE^4Y z9MY*oPu+O=hqKuXZ&F~XQ^54#UuhfXFC%ef=YO2JlJOc|E@sU2$)Kt z1p!kzt7Z`Q#)ICrF1G9m@&0IQ1DByj&7PYex&xjS06xWZ7UjY9_S+bC?JM4B8=d>p z9*1J>>A+5Wn+Q_N@)zw1BWSJQm6f^nLs_0sfcoML(}`=l8vBLUxqip+V`~{lBjB8Fcr&V-)10Z@JJIl;OYOtM|V-}_W z=J=rkmm;h!GNvT`d8Bl8gJL~ByIZ*O{4%Z6L+n*XeDEEffwlRE@r$*grV)J7 z=4nDPU7XI0PBRuD)iN38y8m3w$KwPw&n_YF*sqIf3$4-RrBV2zc3g!hMxiTG@q;6&?e$g53`LOJKonhrkvM#_wr% zod(Tx-szd#iu8qIZB|zm%`OK@P~X}C+;UfNlD$d0G+Qt03j%JA;?F%eN6xn{^_{M7FsO)R znNT2ZiAnlC>S}W$A{wRNAlDG&U6ud!;-5J`bN+xzO9zisUfu;zwXOzagOerL-HKQ- zH>?%mjuMY4>9s11m@6h-HZ}#nxhoBAr8qWhP(^uBmXMBfH4S-5 zwaxgki|L|M;?h8>rkK*b-~MZgdq-wZhyE56%&2DOUF>3VR6~_`&jS<}UKkd$G&`Er zQG7wj8=A4OxS!>|XUDz_8#NQnO2pY7?ll7Jro5lZ9= zUSVb0G@CpB!wIC$LV-3DKZ-!804|HyyVg&TQtSVQAg|?rQ;>J0?oXNN+28|jC4m#* z{Gsdf8}gPgjp}tQMYP_lz8ycEvGt{np{Mfw1Mi%Gnp_UV_%rnp^1=hc0v=`1FYD$-) zwNnVRu$!j0T(z{VNI?4d)I_;)cezWSwK=G!X1_869`M#&3l95*N-8T2b#>p=#EWRf zttMd8T)e9zWz-wRvqEW6#AWu&lujQSZRA zcxE;q=e;X*C(%=KiRj@Z7ed!*@zQA<+KNuKT|&|fxB1OW(mQoa`AkH5zf_OjDn?Hl zuT(MJY}M19w!a)*#3UOO>y6$^d7PYl^bjvuw0S7Md}U~3D;;0=MCft6j99>I+-^l< zOwINKJ_X3z=csF&xI99c?Sd2(b?j@})Ytl?sQe*&UgvrsKD9X)U>isqcV$_KOS+nl zC9u(!`|>g#k_A4|A5oyt|9ceZL{g0Cd-KxDhOB5g3X1ga87dhc*aX%wAi@T;O9LBi zsUSQst`tW-}bce16Zg5Pgyj7v~y&ao8Ik}Eve@p$fV_+my))3+3csfU%Z~@DuO%X1Od8LjU8u2M$UPpO^b9v$cQn*J zh|&oaQnL2BX!Kvm9ADY87jFTh|6IaB+K1;XBi=$!vI=K49le(>L7n>O^cSu5ONg!5 zwLXtiU7k_g_l9xuYQ^0~(5fV5*0!;#+q2~@xO@qsnrK#7!7@zup79A1Xy&aA$T!o{ zD(ef)qA)S;4s+XnL3{b}!XS%V%v4P+)+HKGHZ}efGj(`n$@XC+7)DwSIv9PEo_;jeJ{AuEOrczW3ANCilNmtl#K$k|4 zFUh6;o|zf1;nd`_3X%{zC0~xY=$OR)0!HjqM1>S1g<2_^v3X)1U_K0fh38w=4;wTs z*C|h>)Ie(d@TZ#k*AIf%oJ?~JqoJl+OU=1TeoUF~G@aQ{^4zE(4s(>?LI1OY&P?VQQLUaI}0O&STi>0Il?82m!iN- z$9J?9TT+z0>et=GyiUiQrZ{}~?Za3Ps@Oa|3}lQ1_7`4|{Yv-dshT3-<1$9?Z$=nE zj*2Jk?lPm(B-Yd1gl7$9k9@Y^hgu1QnWiH5^idMGyh$lq^eGMW$igHi9O$mwY&>Uv zmj3zk5$Lj-NK@jK#iQtJqJ;0R0u z58^RWv$L}^H=h-pYYPhy(5;IG6LS1?WN^^b4LsAne(VVCDgC(aeZV%5mU#W#5$m}4 zB-c1Xy`Aw@1I9c)JQC0XJSq1b;LFc@wgA|4Cej})9E2lroiFS`M%u3=g-6sw877Oh z=1ovgR!lVdj&m2^?NUq$5C5F=ksxGg-4e14#vP3y85~v3>`qvlN4Yr+L))?Z z;|6<0fGNm#Ug{X+6; zn_sBf*nprW@0&EO_x2kq}$Pj8|q6jsFvV`A5P!X zF;-l~>dGIq!0khkgrv>sSK`62xq)zdMr{f34w$K^tgOvC>tdPtS7D?rxYihSN$B9$$|I z_fakb3Ye>;ugntdCp9d4$Tzeo`2_uHDR7Nx>zG*gp4(JYW#PZIaLOjw{`YTf-pIaE zIH#KV_>LUD1z8j3c#2`*s--2RO{n#pE^!p!-J5T?B@Elnq#HQ`K`)b6sP!){YQWmU zBT5HqM+1Hn)|ik90I8t*{hK%VRzh_gzLlV;C`&)pw=M|afh9l zK8B;s7#FzNI{PSrr)(2W2H?%Iz+DO1&b?R}dElC=<+xY12W;eOPZ(?p26FJwkaU>S zeKt(T57@xZpIu#9Dzx8>Dgh|eKCP53#j9Xl0<+t(YLge8*-YwoOv-6D)Iv&`VKdd*y?*pT$?AgFcEw=-vC@D+G$YSW~lg;G$n)i!MPCc4LW0)PMnNF@#ezQsxM zlB1<~<0g?;x8-6L<%dxPd@V3BrqE{dnHED%)MN8U+pW?@e;2a8j~bN36AS^tG~lJV za6?K_$UH7ckec>|KbLriU29k`wowD;j1W~z5vi#Ko`!Y>Al&yS4BG{IPtIBHV>5cu z!osgS7NMl7a?H~h-QM>FTB^TjGmq3pmVSNTlJ;z!IxmjzSjtsxwG<**cSABg09sDg zVCnhKnSbdNrOjuR+~9G9bf1(Ml;7Z9#=*iu+I5aYE&HvPSBh>u`r$_Qp&utgv-U^s zq4aOu!*GlC2(~1D|H(R8;~&q*D(YIQF3m-Qu-8q9+B4L8 zYpXKS?*maOVj?e(UHod-BVZ!J{{Vxjq2GqMOk9cITfLvF>>|$(8nhPpY<@FgeA|oc z*Qi^7*Z2d(1HClo4~1HX<&2C}RolT!6f~xK*^1!021;EpTY`@U@IwNLsOvFI>&UJw zn5pyV)01srs<)?g{X7)mbh`_J&R%h)v*3`EAL6}9qbw)K$C9WvqjpP@>|sqYtrvjO z0Ri2uMl%-@;1cPxG00I|XD>$QI0u?chFe!pT+>M?s*#}*0>0cARq6 zJ~%uAMjhEuna;PSCns~Vu}RRJ5ZFv0Vm(ODvq@x{R##`Fm=1>X+$JjaO*gV0?#DJo z>NVx51Eotk^liTZ3d!Z-k{h+#;!h&f!}@VF67H0*p@>|Up0;dBPkA;Mfj?HibS5EK z7W{1yzJ`evTIH3Mvpg|_a8tn)wo|(SDdAAh9iBQ4ca#J1e(sQTuBP?k)5)d2D2d$|mk#x3LLzCo3LRtl4NV2rqR2YHALbYn_0X@9RAXOi8OhyWh2#>+ ztbeL_!z2&W^mrqOwBhRv`klcZ6@X!2-Oqdl3 zqaVxq-k+8Tn1%@qj^8)$5;MOnJR?Y;!IhRxSomN%rbXW~cqGiOft87AB3dy=iPk=Q zAE8!bG2vilkp4$bVj~b4A7Q?PxNL}NjeYf=2D$g&px;oaf^)`iz4tV&D-SpcryI|k zc;o6Xt&pAr?+#?uLh?R-e&C3PjbGCTuW_kRN&FZcDDAcRt0D%+>V$2592|2zQ+m{fgk>3s{vvg~x+!%f z`)^2HH!cDK#fJ0EW8JaYkycU?5~W7S3&ctwW%@AVQCnLJE(KRJ*AYr!4DD`N_j4iG z-QY=zn+zy$jWOSB_ktPM_F52{5U4g9hbQVf;K5V5**hvir zVY;s`gpIqQWVU}x23yt$hwp&(?`jHgHu)1I_gU5&oB$2D1Lb?bN}Ayq%zSJeF;8?b zf*IQ|e)7c6R9+nvzYInYt)K`|>3M!I^Hj1rv}tYBo{+s;dK6&+T=rzpHqd+=5^+Nu zH(xyVA72Fw3NBiV2Fdq8YJKX~F<#@(L*HO~9%R2ul zlx1pntF8{2)p@|*@&oI%4SJmB&nI~F;~uq}g^3~#@Z&Ad{`Qt(VkkEkmn9PB8eCA} zZh&FQi4LJKCIW_-|MU9aEO0mEJ4Ada2&>WZvK}*VbKz2%icrmC54AbDurS$XaxVqL zN5W%R0z8BMu*q_D&y73xv-fCmf(sTVcxSqB9T^TMwAw^L3q5GE1t>m)Z5;M0YIZSu z_ysA&%W_;M%!5dDfU%@7oWjwxf&hBMmU4afAxADAd@~AggpYprP2^jnzXAXA0Q~Q1 z0);s+(C6RMS46dj&8WE=q#62vOw22@-7@M+AR`x7u0>*kZ`SB!0+_l0MkpiG?Rd$X zF~q&xPq5{$ZjY)^3)*IK0?N~I4sS1VC`(md4p}-q0N~%LQ_$imnH{OT7Gn+sck>t* zC#M}e&GRibAw``Gz&*ehc!{IMK#qudd33jWV0X>q-1>$?*o1Yj5!u@VS1uP>)VWc< zA3D#e(It}Sr}0&-5eCke3C{Jel|G7(kFC9G-$MeN{?SdwKlrEfi;E@Zf7?%?2gl(~e2?H3s>_!G%7s z!iPDfeeW0N8mAi3DbVK|HvV?`7ZEE*_BU0$I0{Nh(smxp)Z*jF@u>H>f=0QFy<0?t z{7B+$6g)!=O+$DsnV<&K;gmG{3n3z~3H z?Af!JZl!KhZ5hM1f2qf>MX)x?&7_=NHCx-4ODNePYqUm0rcp~rXYnF4pG89+QQU1J zWFq_J_{BMUzw{1IQi?XmO!JB`RVJoi)ZNk6oR-N`Pto$^>txkO@0YY(XDOpdRxi{k z%(pQ686;zX7a?K-Oa!KGf$;dt#1`$=>Hn<;f{V{U6&>DzOe<5``&eHr`xTv4b9{LS_0*+1M6 za0yoi@+6wsN=iz?Jq%5bbRlDazhDIEFNfmeo$9lAI}NCMpGPxQm+Toax9p|r;~bb)ULXjsFrKb(9Bg7kawCm6_Y09l+O@ToPPJ|RPiesa6!Z0#vi}ho)@)# zCPuEL7mXRmBH5Mu$iY<6;nx1;Qa!s|P^5%)40*PknjVy#9pj+0i%!>JYIr|%ky|d) z5IDcAO?V5ztsEZu&r&1t2?>(o;wh~oYBPX>y>C^3e4+6kOq1fe`{~KgJn`h}oeSAp zg4mnh#s#hEn$XyIhA~O)LD$HXmfQ~YPrY&-ta=-2n-4>b5}s?TMyg=}<2dTcZw{5$X7JpsQS*^jieJ?5 zRyJg@7<>;1Z?r^_X*BsjYCPtd_%)XoiSfqGe2Kd|a7Q()zDcRoYfcX_TF(uNqtTuV zQ^Bp)sxpDk)m~(VKiwgC`pJH$KW~gj4IMC8bsR8HH6QaV!OwdX@p)jV9*oPWy0m5! zy_M4()Mh?vzU3QG)E#uTgl?RTxkku2L3IWwc$HRXmi@M##QZpaN$ouv-=?>BZZR*q8s`aJds^+`ETTZRPL6~(eEkuMZGbYGEqZFv6T{G zN|-pm08Cdf$2RN7yQP7Vd8l18cP9aYZfU||wx>EJS#O<6e8#md7Xn?d;K@hBZ;QK! zs-36$H2ZJIfGR-OXG?bcxBN9RClP1|fEd^&;QwY|uZD!ABs&L(k!=tZgJ>kou(|hf zJ>kdbpom^E5#X=4p6BkH1H)>4A4&nP9C^n=Hm@SQUe|I50&)G0 z>>nlB1cnI&DUHKBsfehARnpS4vjqeNS0GjkHMWe*LIS+w{LtHTDCl1=(=7M+saL{e z2I4&%SSMiG;Ea(CSN};gUOxa~hu|#yuO%ngaDjFA6Rlc6Cn1`m*+@xY=88%*34lf! zHVeu5?79w6fl?aPrv2kmULIwUE2)b)lDk^!)#D?-RX}w}*IinyadW|jnYROb`?F-| z-&S`l-J&L33_p{yF*W85OazUJhyWu@=Af<92`azIUC=9uj#T(WkbDcoPzdqFCi+1o zYk~_)&QScPZn&kuHPufg9zsWD6q!Jzh9~jib&n21xuuY)`Yt^gberC|QjVwnN1pW& zU!vB+@YM@Cp3KkfQUOpSqda>&n|71p0WbAuN5O?_Tx{QcLi5?}XqGB?Z~KV4-cSvO z{?is#+eJ0FU7x#iBtSqh&7g2yJNU4D>>YQ5`EXCf>Wxbq@%?@--W~bZ>^ZkpFTE8y zrrus&;74CS>Hq_@4PaIp3g?^w6IAB6Z~)LJZ}f67;NQ_{qqEWYWe4;_-{0KpBDY=F zJ^%^`^dDE6+DNY}4#29!x%#AN7k26W3sj581hORn6S!;oH>_4&bVNuS`f=;;)(%om z)a_B1`aGJNF{za<>K(g=D)t-@67*kv9U~X>-(JNXU4XNUi@x-9y>c#C8%pmXt*A; zBs5(6?(4+NVt=s#FDtJhF#UTsnEt7q<;K0vvtI#r#VZ)oMvJGx%(pBDvzU%t+*fx= zErO0(Pjn;`bQiCJ@t(_Dv4MmW98NnR@<3w30f%>{QGLIbOg$Vg80-W638_#aL2rMR z9qWNm>DT>q-;#RhrwR9+-lpW_8b!+)^xS=8<0oE1vu~G2jB4smb=t-DnPv6S3hq%E zs_^to4aLQZjA*t6c`>#-Wn@@ge;<*kIo;mc;D&ar=y>KH1pBMn4w_cA(X7 z%A~91mNV(;0OLO9m0g2$iT`#^9d*ZmayTlCXlD0}hYc(b)j))sW9 z8e)<`Xnqdxw1de5t*J(llC4m4es6aVY@YagOL*X0irU2s=rO^mIJ$1)#Ln)9_U z*;T-1ct*ACl)&3p_civN`+lXMeGiq~-JxgEhmkz)uUKBZWBtzL_VPqV-c-B4`rP!F z=|X`Z-oEKsI_A&&dy1(J&E?cxi4*}hNL zE`79Z`xtVrq1zW)@Co*FiNcb{1Qb>WFvYkai2+?>-i!zj${}SVqyQR=bVfRp4|AM- zn;AMpsh8H1Yd$k(G>|C05{R94!noGOC<|_%Jm0rFnDQzkqW{cZnisD3cN&htKu++` zo$leVw_IIbfc*RzkD*F(4&2w5X6j@i_C9VmYs6x)W?IUQUGni?4zH-P;7IPrE$%|d z#528c!<@EONz7Hclq$WX+m5leSjK%9%9DHCSNTS{l8zqi#~g_j}I4MkddgmvZTC7?S|o&7@$DW#K@*w?d;aBJji$Mxg_GNG$kJi z&5IEU82Wm?xw}!_UeG}u%PBi;5l_kT3WE+qge%FC96>>+KYLECNUE=LU#wh2h+cDA z{7|`*wR1Vg{-FkBoAFe|cdyW4QD4oj9#r!S}al!9juF|-L~Lw@?3K7`$GuR!!KR0 zir%w6q&{Cj;VFTim$xfVS2MDIOzE850Tfbn+YM~wVOAWUt<`ct=QYQ&^i*t7ibuRM zh55*vPWM0+)zk;OjoJLvj?&s*5c=L~Pru*@7WE*ZXmnEOXdcdwX$vm%+}s2z6((aLM+;gYKcc zqWzx_@NK&+dawVC>EX0F7@synEHwV~|FHL+aZPT^yRj<>sDN~^BO*UUP28mxhnx|+jGwTpL;*t5C5MZ_s+|D z*P1o+%*^x5dpkrKe7q_*Hy2A#;}(?lZu3XpXG|&GcK4?F)sKdVr5*-UwR6imf0nMU z^833C*6@wZhxoE{s4k`r#rqkaIO$qe9a3KGiZhFI?#ko47D=b?65o#ox^iv}-;S*E z)m{d4z~hS{i3$VzT9bL)T9f(Va*}hpv(uA#;nsaQ+-J28Y22D<=Qmdmh;nGie8C>N zc4?|GB_0NMPV9>FmgY4idI_l8?0j%gf61N>{#mCWU58{^DQZMxo^?aSamO+B;Ja&e zVwX658}~uPDP-j16EaFj_@czkF|!(sj{aKB8Rr2$R8oh!Eot>1}tsl#j@i ze!yB?)90W{yHmYF^CMyV1H|eX_p}u6cm)*W_~n?w*5gN@_Cbx0k2-9>8&uY)GIClD zzX(1>_&HiEDHfZOi?o*Iug4R+`6Y@(Z?MO_r^e8CGb1YiTfWcEy*(e+zF<+nudUn`z_FTeBA*K zE8TDUZx{8r6uL91-&l(RDQ1Yc<$X^Vvu+%OJe=Jv@-J^)<>lU95F}!N7n2eeE5M}_ z#?rwDTzyCX_V9YN8CdOc#&=+`N70d!e_U%(3HY$QDERuBH|_~+zIPB*Eqm>1dCKQg z)Q$ZKY4q@20_vVSAn!4B0_rjW^sFB*^k{TY31BQZH3ih~-W+C)rJJ+V=Ka|0+~Ei_5~GFSl3V zT;5V@UT@@i(`GOZJH>LAC$D=J7wqjJ>eX6vt*I}^5OD;ZYz;Zm8hH$KEC)Z;2ym-| z?#_=RYC5lXk1@q<=g7U)u3hh|x9m&coP__|zFu(Bo`Cb0hOA~|V*@9)(iH6VV&xAj z4U}Zn5;Ly!-n2t+u4*lwex8T|j+dZ{v=oJjxp(RGWUs@U|1(=Mr+l zLAs!%+0Z#6%cDtgq>s!=h>JJ>&5ZpSQ=Tn93(Xcp7$QXPZwqYk4DGoSnRX#SO=|o6 zhxN((Ew{Heneio?>=*0l@pM%0r&CL%U5$T>V6p+NpB6vW@ZZ*~)U@5XgZVu;{(rL( z2iq?ih26VpOhG(E_8j6A7b1m7&zSjCGxN`$K_P&_>ZF}h#fM)f-I!JtNHc=V5Fw{7 zukOoJeg^G-oU!pTFq^=)qZHsjAHP7;-N$=bQFt?w_&@7Vd4#0yvkQG$hI&Xo)S&H8 zjTH~Vh0cj(o{MHu7f@&Ycvo($w8rYZq6Ro~_HAxo2~HQyGPiDWKwZ$K07raSN*x{H zBpu0S9qI0#HwXJFum%<(_J6eqUn1B|lYg|Ef82=&{>ZYs`|W)J5Rm78^{g;m@|WYN zzOWVgD?d$~uF&+eE~vSNgN(d8GBXNF~yxh)F(7{P|e)x3e7=y??Ke zX3Q&>7Qx7UA^BIgY+;=ng;3)mD4Zr?nz7RmQw&|nPH{Pszu;(D|D*3|@3j!oO3_~X z*#_PAIRpSuGba6RwL>Yx5CM5#|67>pu*y`=!Sls$!4@sm`9mX&9}n(t@w~qDN3Uey zp4D?Z)eQIl(E-t&bjUQU6aejcfFZ>x$`8OD@voPK|JiQH9lgZKtHmi&5a8jfv=U`NFTM@HUth+Qa)!!(Y6xQcWA+~@KjW|+AcXw_>VACw|Mb4vF^V%? z@Kqbx32+~yA%Yr4=loA@t?H7h2cy@HPo(!8EKYcue7-`Htu&)LXTw9D)eaEq7&S9x zG9M1vMn=P*-n`u_ex!c;Z?`{z((vKDm_OeIUp61c6w=}PD5hoU8oiz2Gl(}tfM{rH z$>u!}sK$ABshzE%6(aFGjYboNkL8QNJ`^^J2?K+C4xX}6chuIn;&LPkcFvkg- zKTy_Idg{M=%Eis6tSlJRyvT4cT|ah){2cxN;c5b%pgXPGidjdcZ?|@y1EHf z?t$PV_WbTN86GlnLrk6N3=w4G5-zCU2#Cu;$eIl19V|~6`~>SC8Gv92Vq2a0|8OO& zvs>)xLdY*Wp4(kf$&ih*}ntgQMco77IZ0q{%*De1qnX3Vkx7*_e@CPtxOlK zX`~r=FOZ0XmQ_5)=60><{g+EJ3K05mp~qwHg8x6=U^y>C1j*+$H@{A>!5_2U2sROI z04v_wKiqeJBZbva^?=+BcKz^MFmSl|2)X^Kll)Ox_~40qS-|n8rqW^%S_wZr>NE*@ z`;!x4t^fR#qxZgC_EnRQe{cxCaL71Am$X$mT9nU^AB5~T_z$K_7x>I|d(y}KDM6R^ zoJ5bEth-e}G9dG;!zbjAK^V*a!>MpFAMe_!G{B_J=0klopj9E_qg^F1VvI4*<)FVH z{15+wt=>2<=esZ3%Sh9aPW-fhI-kMrT6&VjMCBY2JGAY?0Sp}n8VQTlhAC1yfvw{nEfXC$dDXWM zC8iO?n~9Vq-Wsj!5%}JvvTRd>iFTWBFR|5}RV&NbLg9(a&hl#`J6~0cQ_T1YP{Y~>iJa)p{d@)=;Rrt(Kcm_+yf69#{HT4t zDAP1ZcRAKtd(leI$GgBst3|B#bH>o+T2U1_S9yojF7?5noZ15_)&0+P7AN+l7avM1 zJfrJs)F$&*ky2T*X=w#Ya1k9x;HOJH4TkG&us#OMvuPZb9fy>p#bU1uyOvpY8GgvN z3dlicwQ82^;n>d^eSqEN2;sDA%kz^9amnZo%@iqqTf(xz>SF$j@pKy6Qx7S5Ps2*) zn`Z?**2ksz$nJrzur{YC=)t+)J?D<``@k6pmgWd`yTDs%g!jYmR9WWvJf;N+ZX_ZB zvJoKKHhjBm5Yoxvp?TUq`)>|{ZAet(r15wtC#o`8+mXB7ioYiB7eQDmv zo-R2TIJMbc-~C&dGJ)tl-Jc@R4Muz59Rp#jkpR65I?^2>Ljztujh9?&3YvqA<0r!_ zvRtP6IQY(L(j_bRQT*ud<0$`eP0fS}Oa`0~O;J|G)%`(qxt9eK(WDh-?)|ucm zNdd)Q%%3W>S?s_KBC1b0^-7!GU=%tNbUb*!JnKP@=ei?nD(2=c;R;~?hB##fp@9;!%~^m@G!I*(!KipE7x{~oBmfRq$Owkf2S z=UQh~qJjjJpH_U^ouHuY8v&5MR*jS+3p)k~F%j1qu-veHpu90e8}ya|o{D=nHBbem z=2*GjHe@?cmXf~cnKx+4@)*f3ezP=@Y-kvav>}2@T5UQhb@2oH62exa zAU~D+mCH*Xk{|7kOXN67)1Cc(?dk0*%c*V`1MuaxUUxN_uvYd~mw=oX9*T0a?P@72 zrCqxi9NCHaC4x;4AHx`Y`Z77N}0tYxfhjUMq?p~jw}vp z)201S){=xHy}_p=<>DqvtUc1C9X;(P4~ljq4M(qM}1QQD_v?_f%4>nIhJM#s+ z?xAxn4H{zc)^-Jg!k|Xmo_R8MD7(QVW$_)eSd;5qS;>+wbe*es)e3x^4_y9hCQgKR zsXVd!9eWBh%r)AJMD+Et-;K*diLRiw)!3`A4_@mO4QecTriL3w2$p76f|eA&Vs21} zFFfaj;wokuWo$1>IQyPdSi#*SyS;Xo0B}MQf>nu>7 z-7(Oy;&C%DdIXb-RgLpExn*g(FDd#^>GJ0cYuK{YPbsi|uT64L=! zFaq$hx|aqEV`-e-IozidIhCp5RlZtTWl~Ew&lYxOwMuJQWL8(Jo>Y!FJC#=S(`z|C zO)nw`y5k1xzt-)A3j);9R5W2iA-_^PSZllhlJ71Ki z%aSelkx=E7ICahMk*#I&{%y^`{b&sovy$jhgkIMw*@EvoZ90s!9C9uZPYRHvf8^rR zpPw|f@gxxx6X*}*^+|^C?UU>?NTUj*jJ-Ji)gHVAVnuY!+C@h_b><RK%8;xq$X==e7N*G0Na>c`8Go30FZV18q5@u{S>b^qtJ z86wKr;p=O|m73F~5+41uLn&-*xa;nAwSX82u)uUFix+WqCV|7pbuP2}AYmLu z4IF@OoCb_aL-!zIYT7OKspGnvfV$uz$nnBjkeZ#lmQweUw(9BTVvdpzbL5{ahUgk! z)XXMqO`T=zlMqDlCCQJ)>Je9Sy@dC)yFq6*zmtXNT;0rIh=7G#4Oe{P$!Kpce9!b4 z-+Zul01apgtO#I07Yw>)usoKU=UdC0GB*esE+I-3@~kSQwhLpVE{mf%smr3FXSmcS zH~SQTHJRsBu%cWJRI^C$-qck3yc0BF(CkmIlgYf1BqHg__LN4pr;>kSPobK_1Hpb- z_5Q@EE(WKm{S5kyVSLkt!^^{dlU)PL^64n)YY_){^Iv%^G%K)Zj{wY(R+8%#;Mkcx z)72N2ULQ#x>y}&}!l!va-|f9eEjm!3wb0SLy+nc#rCUlQfY*JmD&y|n{Tj5(Kp*Xy zJo9>}G|}T)sT0bmDu^)-qz%AIo|CO41h?E!<>a=$a&#P2CqW}e<_4+SzVD}NYnIGt z?v*Pmsu@6k>){ss*sM%|>k(Sg<3m~gML9gZbj?okqUo?yp7nGg?z4!yehhznyie(p zzUb=2j5Sf+1Xh%I()UggOv>g}eF${bqLL@3FBBuyD(-Bx&S-AZl4E~@CQc;12{t+` z%|O6$a9ed0iKOLj0Qzpyg)4`auC&^e?ZZWF^UdG{)#p{p{iRbW6LR(ryFlnT_-fCT z2E&UlAB?J13Vyv{lytLb0^rZh2KH|^X$MqP@cV0xCJyeGi^F}YG#%A&(LT3 zsqRSeFn~@ZWb@HS8uypzaCC$T03WskXABY5t?R3+y&ZgdQZ5<&)F8DtZ_PMDRG{0b>%h{+#k`35 zwFxnHf0hM4S!^?N%Vny-rb!m70AmhfG-vOBV~7Bbc-P+?G3JarVgT)<0ka%Qms`yl z?pmkzUd_YC5#D1GK-iOT4oi=9RG+0xUx+S=OZvz-9xwAC5ZP;|AWB^GZ?qf+k^Nd6 zFWO9Q`>ns63m$NNk{Ya`n%3yoym2ku9{Hnn~?qvBXMh2tk2A-a#isA zULs-Uy|PamEPbUNbbJw7xVp@0?H(JMlgz}@w_fF{{p(4sXX4$ZWO9hQeM8jDl-@y+IL73hg#a=}yL0%oRlF)Tm< zAc9`uARc^>3_9KmD&a78Oe#05Wck}nwelh#8la~3?Drl*9b$^P+y(r$hPqaxizcz} zU7iD^=JIJ5Lrcdyjv{5I5L2Xg6w~P|@-}zC`MjXrAShjz)R*_jB*i z4d_VCj3dSKH!7F)Kxf=bmDCy^Pospb8TJKxuHjr72Kzi$`$r9Ui}}yO*sZ;1K7yNp zy|<^o^Hd(6R`ZgbxwZ9@TH$9Gb?3=l=g5gD+YwO1cSnzo^Sq7h4UhcSKC%M9Upk`x|vRC7hF1-{< zuTNN^hkGt|Bs+Tz$1w6w&LM$xBq@#;=D9rNQrbRvDYFE+Zn{2%>Fu3LY3XHGk`mf| zUn2cf7a^+8K~E_x$H8<>Dx4RTrTE{l`2aNf1nFccI+Z_@lRkQywe%Bqt7$_R&Yv!I zF>FyuIAe8fQI6s4-hnE=urQbSRLc>Nhg18|c;}BLEKX8}oT^N}#?>=BRfOwOFm9lp zc89H5g>b3%m(BoIhW1Lb8jkzATyPrx*bBX)evTNWSXOjUU|*(PgA~002IO>o)q2jY z9k1a*qpsg(ZoFAo9v(RM6BPJ!FFAS*t;tt#B9_;PNN_P|dW}b_E}iS>!BvH$DyQ4w z6G@BFFX zuG+$AKJ2q&PnoK+k6^?8K7!Mt0EXQ;0t)pQV8POOKjIWo#AL0q6~fp>c#BueI2z&* zU0>k{DfS{SPtoaO7pzVn#Zs@%LAatwuhu6lraa=}LC8IS%cBN{!R?RiUam*bq zv7Sa=&B@DKN_QA*nW><~NmmawPgYMaMw<8Ykg_PqepC6Q0QN>3C~CjGJwh@~+?Hv+ z&2?|riH9S6+K-V7+^oM64&?naw}&Ap(JZ%<1dL?w^uvZ=#v`W}G0U{(fZDX7Pj<^sY}RZ?Jqg+XOJ^u1E6h`-Y>xIJ&kbN3^g?i4)vcC+UOFtl#O(wrSI& zZ)hA&m@4AywQqSR9i|CFND8b{jafoiP64&rlPrL_ot@{mi?Y8%M@PHgCpUMt+2pf- zgUxL9P|jq@RiRvPLRL#tHuFtbFdE5<<%Glb>lHzlK%Q>lL6}22Yoqk#u`{yuTV1@? z8SKN&)a+zeEI+O(c+u9~4&F1IzKW~4tK9D0qJn*)TINlhHXst>_q}rP{>CI&w$N4; z5mRyaai7+fL9vD@lXMFo>?yZz@Oi>Z0b6+<3&;u+d{5I08|*Z`ubjj@txt#+{85b&9CsPOq?T@mw|neIDO}gr}tRcndK_kxu)P z#nx#HY?qD(Uo?*h3p-3c^=oEVzx`6iU7HP=YCyR;N^a|l5Ez;c8lNk^1B~lO5qIE9 zPmlz!2bR*HEv=M;Fk6eo54iUd$|$j4;;2(ovFp<`aVs}n)))LOO=n)hTocD%D!}Ky z;o01(cO)s(h;$!l(0<;XZ*PK+LhTFTteVD13`fs|#5qW*W$Fof5wXO$sSaAobr$PK$ZuPY<{+F6V>|c^ryR!Nem1Jt za1B56^?@wWoE=oLUOUn8uUVC9kxWfxt5@B6XOVH-pS~(PS4k5L6DM2UDIp^lw=z)B z$~2;9xH|bUUItJw%d;AsdS;{g9@2Q8?$vZ71hT--!pAWgfaxew;6YsXjC+k(({#jJV?PyF;K99D1wH|+-Da;0ddZ|417;QsLlLYEEALVHDSucJv&J*<$9_whSy`ACp<7NPzZVJqRVKaT-Sp_dN5shARHw% zb3wOjaIsnA>!Kzo_e_>v4cG4s^_gVtBYNd8b{^o4C|=nD|Wu3+JGO5$sZRYbB;Dc3xRZ6q&7yfj#Ped3chQ z3{TP?HPlBl@vVguo2$aVp1k@p^!AA^QEUB}?}l~qxa)ElZy=x1C~%VPuvcPZUcU0~ zW4RQbn8r?@bktBL41k5WzVprOEj;Pfi-h^4g>W5r_oDP0?(UL1IRJ&hZ|Fe#a-w8nUp@2Lf2qBk22enoUG-H*| zbVsh~y@(~Y!v_j3DbQTu<;-g#)sk;!=1X&^opZ7P%wKDd1aq|_=;X9F#}~m6PUjxr zZDUQsQrGKZf}qmiQ#j61=eeYn&jm@I^J1_$1~5h};=*hKaRpXR<8?saGiU9lb%`2M zFE!OsxL~nxq2PsC%8P(OXFFV_9^ai=>vQD#(PxzDi z2*{~|x24kz$1w1DjO*wP7jPttgKUpP20d0URzMxoBAxlP_mcaaK@|{_QMDb&1dm#9 z_k$!1yKL(O4C*3m`X6Jn;;3};jJpj^aL<6SgjDIutAa{i?K}crKQcXm|1TSI_TaUZ zJp>vf;$ufl7ud=+Z&)ZLgLA&o)4fHVfNEm`$P!x2=uds=JEGnqk2lHKie8la0BR6kE!UD8bSqF=dvBIP_>u5#a3 zzc)cokdc4|{OXatl7JziRVld8)jmRqo7M5A1+`Uv{CLClL)Fr25rko=zz1Ph=e1AKnOzd;n>JTHAgvzx8icjAJXZwD-BpeTl;X`v?~&z9l zxR`P4qGY&^fNTM+r~jfOw?Zx&Oa zaq{kFZKT)_ys|T_33M!Qi?6g@6~m;>)jfPelzG!Fr`61g8#&?Mk>gYn52?KM;pu0w zUEl+aKkA3X-k~ze^4TH1B}w91hNt`5j&O1twIZY;JWb!tblr#D+rvWoxzzx;z9@pD zOiE<;avq8Yrq~&t6q}s&;?02TXrez#OnUSUs4q|4d=C1)foq>_gHg&?J3nsYu2c#J1%%CL=bB5=!bBCxMNxA#!mW>4In-H2} zhv4AW?K}vl2mf}F*i~wa=g}d`>d_vaq5!2n1Qel*3<8+f*EZ>nyMmBPk#r58V*@t& zr@s^tVdEJ_3Zm=tE6yw;X~)o!x;-e=^oOgXV;>*TJBu#1M`ZHoDQ?8Z(gpQO8Eu_P zO?nuFO%^2)_~tZpNjHDw^-Llkm3)8@O&mZaW2BU>V{^sl&=dv7Ks0{o5KzWTUA*D) z-h;ZRj06ao(Qalg6G)4zlXx8V;~sgdpJ=_yfyf{h1`t5wX=ACrwS$pjfP1jw93{*% z(XCbLhS91{&%qQf=m+j5)hrU};XUL^p~LnIv`%zXhbuJn_f>JgN(%wRmIXpcTwj-U zy35Vk1W-T^e%@T*WC@BpUNEb_|DIB&wk`%VIh#U7 z_K836nL)@Wv57)^4!}Qev2wYt1vrwMvh>gx2@!;Ls9S(zS8jyrd-puDP9`+H*TWt^ z$hpb@2Q)H^&YPu?FV=4F2%)6q^lj7U?jzI*(kt5$*Xd~rzTUrArP-DSx*dQRP1=o3 zYQ@bbNC0fxh5$;ou^-3a_wZDss)ufTR=gI&+}Q=cFE~p3_S(K+$s5BeKhUW^D$sSl z$t7$luaCM-kD(M_S<11BAHc;{qgEFyp{WHkWf>l>#p@sUl<(twxQG3bc~p1l?JIq? zsU;;n8l%k$MVT2f1FL;BZTek~%j2+V*NY(D_$XR={LXKF^yfkv-6@hhht&FglI@_N zqa!KUQRoPAq6}y2;=(&k2P!oAO3wSrKIimUopf2uM4g~yW}rkqgdL=A;b0533JciOgZp|1*~ZkLGwGMniF zhkw0M)Lx*f3f;psQUmj_%^y9rrJ>;9N`!`tfr|0DB#x^c@-h7aq-^ve zxp`Gz9t3i5Xp`#!M;{@j=dWLTbxK*()$W?{2@MvlR?pS4H&<}x-U@l3slK+mcSHJv zigT`c5i#uY?WJ}K62v6tzNQ53(mLn1G?)JD+eW0CADzcm%@0x@e#gPh>O z9&W4U^=Wv%FxQuu=lbs&s!n-co=vz7BzdC!kMIeDm+ah|n~QOB<$n%YI3k|${R}}<&~Xk)B9bWAQp^tg4JZO?YioLFQ+$wzDLfNxDR{`VUwr< zUA5wYAe&xSYy`aDPjsWriDwgHVy|(*V`q@TK7io?Ta(56c|RK0U1?gJEMGqwM2e$F z;vAvn$~v>KDf4izR#-qQNs=ZOXK%*_=fME}S=yJ6#?4&Rb|=hZL!d*zx*ApXn*`*{ zmEGHc-NM!qtBmHs5};yTiVK|agI0t@of_Ag6m0?nomw?kT?n^(c5}cdaqSD^11hnD zWqGCaQzd!8b+7qWk{P}`{vgEg@K(Cu(UVE8I?B5VOPb{$jX zFA#=V1u)x#jXfR7F-=BP{AVso>(!P1CkFi$ATuwH?)Lujb~gAq^Lb~hWfef=!X-2_ zT}hELBfGyH0mvW@j$oLBzSjgYx$5!r@1;dyi;;mv_LQ6M?(W2^NlH}8k_V?lba{JX z*R0YDPW9d>d__KGkznpq_v2|#KCia! zb|fzSNt_1|94d*O^vbIP&Cb5{G{Pk!z6w7BT0q2t>Y;Ey4nl^<0!C*budl^xHk7;k z3TGu>30F_Po8y{Su>kx`>5cTILq$a4=YC+6Gsklad}q!&KC~1kw@i~HAwN6Aoeo-p z^8AdWj!+r_Om`UAJMUYqsD|dg3Sc9Sw>RZ|CWC0r)fTXoudO2+_uUOLyi>JR7SzJx`@7dW|GOMy#~Yf-wqvKq7-Qo>+%ul#X&keiUaW#VvN?|DYJ)aryw_wByY znOd0d7o`+txTX0Mc@a!1Wsy@QU(A&>r`QwHT7k66`)Q=;G)#x=A+ZE-8^n(vf{_~|SxD*cY&+aYg%5-8P;JqaD`6Q& zX}>hFuMNg=>e?gkGrEP^OXm9G4D&J556#n4!Nz8rVo8WEMu^)>PXCo;>)j#0Od2m*W`k2)0^z?Jiu(L5r6$Ya+ zdHzsb)%FukPHj&2qSr8;Xm`mNDC$dfjDA-l#csd^l_ngtC$Vvg=e4r>1*1LeTrJ(x zZv;+GuBUXBNU(Z3;K6q{=%|yr29N&jO8NvLd4Ev#xN5|w3p@cI$hx#+ZtUScDxUe0 zrT#t$u?y8&4ZcKBaZ_+B}3bh#Ywo}4q6VqvyN%grNEc>t!fR?y(%y;kL~ zvp5x)!m}kO1&O!o=_R47L`brC&TsZ6AJ9scld2RqH9Z^p_+$eJ%VSB<<@v@z?+I^s zaM17#{U($6#t7p$h!K3IA*7m~jvH;YPrbVnQ9Qg?gGG0QR=EPT&1CveKCyOjt;h_NE5ay5sIxyn=6U{eSUz9)-P3P&Rw4}e?i!GlW3NR zdYl53o1db8Rc^j^8K~y0xWP7-DR6Axukk0Q*x9`U%WNw~&k{voZFIUa@36NhSOV=pS*l;qE7_#Zl_{RSZPhTAwjG*V%*48F_LbUf~CR~ zU$R4`CGoGI0XUAnQ#7~@WVk5u!COFZ2M{;C*bZzKbcI#5x4Ob@v!E;d<;=h73inIg zmq2N`^jG9}H%~T5s*aoc@a4a%ms|h32b;hF$g)eggdFp7dJO=)!Rp<58mwf6ZefGw%0yg_Y3`WQt1JtZr`j~HB8*-;rmanLYm)n4>?&>{51R%Wd z==7UbUnL2r=bml~rvu5t=|}cFKX+mC0dck!RNeuJP$<`ua=Mrn^ zvM2dgP63Pu*|$PkQQD){{Xc^tAkQ6yIy3{4{ojQ&lh${MhQ_-|lEq#A$X;idq-y5| zWFoat7>dFDqqMY?i8B{-I1R9Z8b^*b3=pD2^^pB)d>C+UPrw9P(3AMa-#qT6 zWQbTky97RS2Ci=0-!H<3@5zS@CJkwF;ZCf`Ua%E z#{rifElA>bi2M5nG4Jn+oX31h06xZ>qZlq<*>||lRvxq{@tg&4^g|F#;?^zN3xx&@ zV-O!m4177uzSwXm^R@69b(k*7*#JJMb(Ru@ShBz^`k5kOk#D=#s<25MLUYPTb?b%Ny~%IJVm zuB3-yVK#(itTRgCB-(9x2>qqLEHb;)LdrIial! zqXhFS?C0!_Juc6(XcjqL_k%wt)s8l3dVPO8(~SUEB=)<;8*e>!``Q?(cmk|CR_@JO3U3!QltdOiC;)((QX~AN3B~fk0G=G zP8~aI^zr^ov%-*$uvNf&OsY=#R1dCiaCUvEEf}dpN+<{3T6P^KCzR(vDiy>gAk9st zhfhua?)WxDff0UUuKuK?)?BqfC_Uf$x6#kEje#wr$>y8Pe9~c!a>m- z+>pPC=vAQ(+sm!U3%pzGOji&{s5k`;6+zo0oey*(Jw!4%YYsw_^Y>W`RP>vc9=vONnZ+NPP?5>9U@T+KRo?aqE7nL zKrrS&N5!sDt?;LM2EV=WdXb)&t7cxB$HKR$a!zOm2<_2g1|TD7jaxr!b;}N92{WDM zLtSyMZ~L#1hzCa*boqe+0ZpJk_XY`{bdi5q8dfarxf7|Ou}b-y#9Z>BJoG{U{RR1^ zuePDZ6`G(ks*_t`|18YP)cgr2hh84e7~2gP1k~Y-K`HxX{?Thp z7x7(-IxH8MoR2J#I;(jA9ZbW+gS({D1dCM(ija~hp7sQ-lL!GYEjLyn2W^JXrZ=v8G#sNyAnA!%oF^$1!YV?UB!q#B z3whfEMWwaLMJbY!uX`H6#NM2{O#2|ao!Y_3fXl1-8zTddD9CR! z41P8cWL)3KW;T3ijtzvNwYO2Tt3n_O03wSEXw_^!(}n!Zh5VWfBOt#2PtS}S-*MnL z*4&C@D4&)<4YI_18z92%pUHRvyh*QZD7FsN*bTk6{WcK6Z)Ri<=%+aVcN@FYHrvA)sJlR~PvGAwEdTUNyF=t(g7FwRsC7Acfiek_7Vs!AjJuMX z3c!tD|4{q?P|OBOR%o(RZ|UkTLBN^0A~r;+x4gIgBKO~t4FX{B612(c{-;0cKx?uz z{7-+x1k&=*@A5(jxU;jGo086dy_cE($VHLzLk3S?%x5FSg z5Jza<)_?^@(EcEaF5782X@3YA$r}yX27fEAZc8#3g7}l_mwhO!^N93bm}Fj16F`na8ymefVa5a! zPs^kZkEr@FFM^`+=9YS^_IubP`;qP~KeA3018AD%l9+TF(82?`I-v92eLK3PAVcaJ zp?|vWTN7};q&GeWiq|@Lxvx#9sR6D(ErqnirmZCoe~>HD0HZNU7ZSOp3-JWvTHFsWYq#ZP&mR(h zW+3G24Y->?c$DGgSs4h{=QFkST8@9-!X&GWzlCc}_oY?ZYu>h4tOwSZzqN_xJb(Uk zE!5;qgaU0MpuOZz=1O~k4ElQG)-p-*l?}*3cJu{Xj(+Uhb_0Bmhkf5Z4Y2D!6o5d| z_Qzenz}v|0>v$qL<{d!yuU>5}i=@o^(=L#Eh@|lLL(#U0ynZ>AAkq{Q(gc#9gns}3 zJg)xJW8^mzore#AR$NYd>jH`Y3v-qYK!yS83K$0fBt<~-8v47WsPsRb2?g`NQS|&G zt^Cs^OajH#dQu4%KTxZoksg$EhfVO*22=T}*Li#6wWj$|gUL?+Eya!JYYe%4;&VJ` zq;SZGp)3i?rma)H+Q%Wi@P1}xT#ciMOvj1U0Cui^+^ zx<=xRPINQ9u+Zfm@x!a0cG}%22)1mDyUH2eR7dw!8Prmz9l1{`(Rsz}48l(^+3JcU=TES{QxS ze6$(wPNrntx+6gWWSul%8h}l=E!G_Np{gA+h@K-4Y{8R57D_A><0Ax|1Sg{`CsqPk~9eYPl~*{Ku?L(4%%4s98mg8G7*~- z$#ks3YVAK^Mx~2zL<|Ep>t1Ck`g%!0kJYY@Doyup1H#%&p!GxO+R@nV^I8p4&Ldu~ z-T5^lUk@79-GDi=z_p8)P~uNG+rucRO-D z$@@hmrk*#gr!V62#YUbpp$x7n1|kvpvY61ZPok>Ynrv(o+{~{I-M_SVRif%PuWQMb zBVw`L9@6%l_EQB*vBMq>>(UKk4Y=BcFdZ>bqtWp z(~uMp(&Ve6&xa$8x$G^F#;c3`j%EXPT3WjA#cXb^IWNx|$JG?yyh*2?W|8Y749x*U zDeAZF=)8k0KL^B|{?4W|U-Pr)8Bllf0)*Uds`(Vy55TNH*FIr5bEZB=|HdKf_>7tE z-rU~Av$$MohpJ$|Cw$Z#UJ>YYH74#0)G(EI1`GJt>_Cg;F*!exMP8))zgT3iCb~0) z!_m>UV2q)4C5&g-s~tQ9@>q}jkW1`f3I=HkNwOO0DI>d%7TpZZxqA?N35y@(UL`va z=hW}Av)R8uo?2_q8Skd4I<+S1!cTyXv5UR}Qpq>zr&}`IP7v`;!DXMGVTtQd4LU2> ze9%+90&53bvryc^DzH0kh^`OguHRStA?g%On?WSQ>QUPK=NPD*vMl2FBQWq z{`}UgvYhPxN)YfrVg1YKeOyrO|6)P@N`>P{2h;rJZ~DbVksdm)6;)YI((|vQ$NXO_ z!!B~c7wES$J_8^2n%f20VG+kz7!!PVK0xd}kGN^4gJIFZIMkkviI~iVfRGNbQJ}=b*~#>%FK`8|H(A%b>gh&1-5w^8z=WE9V`~*O0qIkw$6$ zeSbe*waHd9Cz(pFs zOmL!yh4^(uhQ0?_tHUZob;m+#>? zFWr-Il-Crn6j`C{#K15S>#3Ve4aDV7cT+N@&8I1(;Sqg1n zLxauqvcdYV+i8suQ;T~Gx;h;rpZ&O4Ia?RY&6H`p*cB-E?3HO)A9+8^#>TiW38zWA zWeS6|+6oYq`0eMM0$pM`ilGKj9i>aPKoX{pEE$PCTio z(OkW>55Y~5$LFT;<36omJht6s?^h3A)wp2IYEN3|I~9WuckxXq?~yh>FPZGdnffe~ zD9?<_FG|8Io#qB(E%~uIA&%@aez|Aiff(>Lbf?c(zm2|E9oA z+xg=*Kp>-u8!lL18!hf|N7Uh+D#{6y=pXpjnxvW{uB6AL&YeC(vAQIpBj{@9a4Sow z$uKL151yKiYCMPWUP%HCjoQMl5u`PY5vt#`4l><#@;u}AK3~*}OWZn5yVc@&Dl~L`NnHn)a#rUsV9*gmmC7Rlk^pK@-152ti~_(u zX+nNagHgV=E&6COMtNHvsKaNl)dkVDQa|2Q=EHHqDAvw99qQ0gkU)W)Fky^TS83qpol6YzSs}5*@>rg13vzIuZH1lBxR-(^z zHA})7Jsm*6V9>;DW5ODk?O5zJjNq3^EQ}@%BnQ1USBjwkDZRUz7L)Yy34&}kkfs$nPKF_^+7GmS z{L8w&rQ&vt{9zu7Ck$0NLMm^-gtAkLX1UJ}ho0v{MO#9N15hyF5;L(!H%i*7R16Fb z+;F0gls+!?vJmZ``TC5fnGY0m#N_n3a(3Iu`49K-ONI$Pr+bIvrt^CyaKb%_Bgt?;6giTQ zHKkkavKHZ$x9o9-c|;3Rl4AA&JBA;=%~hLpVbL zVg~U{q>q)T8Zxqzfd_QF;xeORZ6gaOGVP(>(uRMDy3Kb>Bn;hA{HCf%ym)#3hL_jI z*1T8!j4391>FTn?B(VGyE@SB>TuA*`-=BSb)z53aKm4`N9Z!ds4uhAgCXIvhDtTqw zgrkHjDW^-jCQQC2z}>$n2FgpUboImV`LAtOFMVSBll`10^4)cB8^HRpcpqGRa%QpQ zgM9WJbp1VX7eA!HN%mPuK#l?Si*h-OHEGHlbh(-VhVJaAvj6swgTTLr3&sUU$*p$G zQz?e~QB(O*=OEYb^zjx4ODh&gB4H;c+fvZbVAH zXm>90J)a|Nr&OavMBKUNp1hg#H2eC&=3AX>7!n{VGc)FJm}gGUIn2BG%uL~2sEHD2 z+XgQ1IB~fbr_zi9E^1C&|I%NzFBh7SB}=_7ipC#FpzG4qr6MK4P1FT>s$HYG&CQ{1 zG|C8JS(A7UqtEv0H7!Jgu4h%$U04!-E6Zn5DFLWxPxA#cfP84zuK3Jzyhj5GaAF3OF!#$1}Dh=ANW8)1;0ghm_tA*B@OcJ0ZYP)oOB|>F3bbD-pk` z;YZ8apF{;c#4(AE5WrS44Cr;DpfAFNOHq})M!|UtR`NyX4ay2Q(k)&7suC72Q~Wo+ zffa=wiw6j`)K#|0EgekyP)!o%4G*mhjoh&Z4WNeg;`=%USy_Xj8e^S1bE@NQRnx5e zuL2KCoP~Rir@Dj#16NQ0Q}w|D6Xey@A|ylz3z*_!m-yHHy5lEL;to?#HcVQM>(v~x z9+z=mbIHf|&tUz)g$y}px0LgDU&U&K*np%>|* z>w&kW*+W^QW(K}~v{`7ye%Pi#CtmurC`T`eX%n$QF7s@{Lvh>)k1Qv2wPOZ+TMC5! zw9}F+OEf@lpaEMqRQgix>yxUpUe3q{`_$8u_9XzZD*&{9Adoj<8(5z%#%13BbqHXO zxOfpxDof%XlFywTz|x-5PbEK)G=QXBWPm8&^jLo7iPHaJ?>)n!%DQz?Y}?Qx7|F?m zfJn~SgouEMq#_GQECI{dXOppqmjNG@{Dtw14JQXyG#k{~&A#sVYw^}hE!d!K#J zzMoJ3=%;G2YOOiP9OE7Dc*mUlvL*4uzyD3-;^xM#0ARx8JD7k};)|&dm!2LFr3=Mq z$|J^piicVvz%|%pqHM*vQK~0KL>@aj@A5Ri>(|fqIv4cTTo{1%sxleY$_D1SF%)b6 zZveP$8k|Qo72r(fN=V?mKGZ|AezI%48VSVpktG#A$Qyz`{YCVJ%)`6MKh-MERlfd@ z`;3p}{8F7(+FunEROw)(MW~9_=XBAdqi>nAOBtPKf^W(myAe(}Sxf_cxvci{B(`bm z3B?Ck3Xf0LK@Vh7{@d<#qbiy^j`K=77-Nn7xlhLVYu;_;OL!LyzYHo_f*5}lDqneP zP(|n2am4H7P=TKp6A@n=&A7*p?>`M?Z-DgLFl(hYNmgV5Gg(^C;)(C zGg;_GvDUre31=9T1H3Kl^MXB-aCd7WyM|9Om#`=^G95hQF{%+O~v1~UE% zpT6SEOxRoi&Rap{Wqc0h&~pW9fQHg>_|?4%TG-pvT$+UWlU$^?5HaXR&^j1V(H`gZ znAV`@&*7Lssthh`8lbm=SaE3Tg&tO-4*O~u13CttQpLBaK}zo1o3 z&wC2`eHDwi)zVEgR4*oYG>_w-v`+lhBGI0^rb#wt+mv#Sf7d-xQ)(T|%%DqWew~oe zy%X!A4X)M`o$Wc(!}b2sEV-{`J;V5r9-ECh4K;dgNYwESRWM7ljf8D&=|dfjBaMCR zHPwx%{B%9*`TgCtdW+@Gce4gd-EFe{Qu6@GB>loWgb;2p3ZI!2 zG(?MOI%wbUgmFs9xtBbq}8Q|`sJBIo23LpT}fjTO;UQH=3u z79u;Taqk!IzG7YD^D7b=i{GBMF+K)^C!``Lq#FGwOC?;h{<(N5cq)0Lt(6@Y5s~8T zoX+J#C$^7U-%+=lsnMlqEy!(+(XAQ1tr%{1PSxIs*QvlW05)y3?u^oeeG+3-Al8Q5TwQjk<^xsp+{ z;k7OW8Dh7gD@|1_dhK-^i{7x;57)}BmT2!_6GAkOUfU87i`v~5gR8jTuAV`h^DT+5 z42ldCK%_NfUcdIRTw7jrli7DrKPxT%9Dr|E0(@KB&zD3+7XngoEpEw9nee2r@|t3y z6@^p9cG&`zbspqLz%CyoKi={J)UvB8@#Eu?N=h$SPp*%&4o^-G4B-?N-5MHL?JP(I zd3}9L{i3C+P#ocp3F|BUs6Gu%^}KSC6d!|C#$!7D8gcUvJIa0)x;%&=8qLlUm5~4e zHAa0%q+{_(iiTY3{bp1UL3;E`$om%PM*z|Hv!qi%YQP8S04X# z+3)X8-YCM;_()N{Fes{VsprihS>t_Q}t)gA!$r2dpbudMj@DSDr5043A3G3i}FTAJIf675X zsjNZ?D3!!}G!yxM2~`@^7+nqLsZyS8)zzB^r`g9%O6giw6oI$1*U;Y}V7 z?}7vXLJz)|r}17poPh71wXx%vn^Pb=-vi4_asMViiuaNhrX5T9{s`VJ;iu=o$?B1z z711b|>c4JbM;U+Lr1bFKKWA6K(oDf~9$JSbf0uP66D>wP|_mwCa>}Z(BQF{KKfcTC z(TIaakt10BPxFa~r_1*8^dla(%iM3j(7IdL4_M+SyQnS}@KL56QTXbr8o*V7lt}5| z@c!PKo~NEE-%<*7upr>jwJwMqHX(Go`|u0XytO=mY(%_Pl6q;Xn_^tI1>~h(CuM=IAgsuA-zj6ZzHH3%v_xZgRzkeRQ_wFj{8$M`5Z2u{vgU`=MlcCMV zE4jb@HiFIJUxh5RY3HX&eH|;rt;5IvzWu)k2jK!>6#i>1=d~vB?%B+d0(M;gPWSZR zy#D^dm3Xi|JBj!b0-N@IO`c`TVm*kSSI#RpgqvYA8^M|8Otm0G8# zeoZE{urG(!R}wDd)+DN3yBQ9fHI_i5xIx3~C(L>JrH_te!;T=LqN2&`4HO5-*zdf3 z_{Slk+4pe5Jou0Uxnetq>Zx}6i@dx%73182F^!F76Q`Nrpr9Z)RJeF!eWiH0Ia#+Z zth2fE5l%9MH#I&!eguwlu4GIum=>%=vD`E;7=Z%-m-4&UCerJKSB4|KaY<_JnpX;Y zB}6Pon*=5c%;Bg9*dC<2`A8{aKefTmP~lahMeR6S-)>SYR7C-2jmq7+#eV1A)6ae7 zEJ|swlaeMjf>wF;A5?n@%~W&nbXv$ZdUxCOdBU&VZFGc_v%^_6-@;HuK)%OPmzEjb zZm<^Tx`2Vx9x8nq>L=9fMxx-{Yil?uxkSR&))t0x?AFa^aF8Rr^0CZYs;ZQkAG&HY zP>o%DIKJtpL7&X{|!?~uAxs}9xG4CjsYV%W=>TF7qp8G7^&%b(E1h(Pfy_e8q)2j<} zoO-h_n3lU787CR%`1rzJeYdgOlomGT@I~ zxlD@J=H=lG&&28iTiBCFyWFcYRl6JpDsnT44BkkN*tc$;%TNC8njNy?pr+(yG=&GWPk|aF`<;0`5@d&r~l_ zA9GzoBHtn-GEvRpa~aiDPMAz!8JvafW{2ZsW56;t1Z0ybjf^^SOr=hD7CEli(u{Je zR2;23ZWK@8x9$oShWI4?@(*j#B z=~bfAX(HyWSe6ff5@BhEw+$xKZYb2+7}~OO?c4ZNw|-vPn8QW+3cvmQry1cU zqpat&{`B)Jun*4lvg*G^3T$MF(pIi)TgG;M=LZLaP1JMl^;G!KMY`<4CvL*wbk)hB zxRX&-HcxS-3LcGoDtCQtWnrv^MVX8#Bb$N%ndjnZzvP02T>YL;t?5$Tf?B1IBE`IQ zg^x0cp})N4;~?~;ESwN8Ei~7(`;Z{3PNi=|LI6^;JLHrcT8A9ULwz~FcqM>CQy3xj>{63G4A@z1sC(p90x#>P0%wUdY8 znX{R-$E%}-*Jc}ex@`J_&`C=b(oPM5hbCInhlWHpmUxPnYUuOej7*&@E8sSiBG8@J z{nZ{fuw1{SgXxtr`qx%Ak*oKEm5EW?_TyAcjFSDxICOpe{4#TE{@lGU3@oFDN&u#~ zMYv^379*wccpj=v>hB!x^kF>-HtLqot5i+L)q+5xk}dsr=t#xX09kEBo1O^=O>?i! zHS%;jaLnBDzwE2C0LPuD8+YarCtEqmP8O{gXGoNL(@ai1Ae#hoda9>{Lj`@r;JAG@ z<)|d1k`=@CVps?~|71)f=Lf+U;N;ocjV^n^MuXr0=D+sfpJQYcfg?P@ZI`$mN><5E zdgb@{@#EFi)#e06_!rJ=ddiNxFumsi9MjyLWu(%$QSabSRaKCoSpw|RU$Kec;fGB! zI&Mk_I-g5Stn=(49!H)M$ofUVS1Qc~a;?Z)f6 z==yRJ7IlZIk8g(8APiK#7WZLi(c!zIWX009{(?j+0nB!l$kavYj)gz5D z0Zb|oD>9I%_x|O@J$v>nbXs=@L*x3CA)G$nk!$WoK;a({K*A&^C&!7=D0XrL7kGcy zZuXVNKz7_UC8gGfe;nIONJ+Y;R^(tymyH{YNo!J=_>cG8$DOM)C+i3+%AfQ!w(B`U}P~-9v)#$vk z0Sj@X?dNF0I}7gLz&OG=BRK+ZQ}0oif00@lmyJ-V>?OAPH5&z2DE?2!Z(2Y@ws0GvKGF-rDnqfr+Vb z22L~p7y*Dsj|SjETP7M}B2yd!YnD{=?E=2#S-|}R_)LGnN%^N(F)#pjdbjwwFW|h=Ek(ru z+YvbV*+YSloHI5jCkJW~GfWxl{6R?u<`3#AkW-ex6YH-3SUi4%jjb3aH* z>+}5d6askSMJDsck}}aPy6W&lLA<8DV9^xnp+ffa0OC4R3QYU3GLct1P0Ojgh2}pb zspZ#(@cFcUDzKfgn(PX-wSyD1dwY9b2VlV%brtZ<+{$=X>zkUD{brcJ1h(JNUF#`v z6Z4{MO>uy;<%Kp@XA&>hrkOxO?b1=Ry}W?r?cxSyM`iMTqByR-vokp z04NvXf?z+GAt@1cHMO-SS!p4tH;8f%7KkGW>gJd110US)*8A7RYId zrKKf6iB)%dhU+6CmKvEnASgcDJ}5RlJ?%xw*TCO@ekJ-(ER3w}pT}YrC=TK8uXOiy0RaGRu8^C*TdvpFbhp?4ny(oJ&sMCS6!!X(S4!M^_UU)sgY5&h)`B#|gu z?1dvvQbs7OK7BB4H_G-TWjn>fGWnP>3FLlGaw;mpwwdwqh7%pRfUBRVdlH{JcaEJU z5tSN|l5!8?OIcajz`#JDYkWe2goK2cH?@$>)W_$~p937=>~!Pic@gEomFBdztS?ZH zix^Z$Qf&=%^x4}>WM;j+T!8R7(b*r;bR-5_BpvZLjk?upj`Oo5iSyMu1fT;9Hw2_NpkMHA7Ckex!b_6?)E>Q^ZJLJ`~Noj?%%KSFJR1nFR8k8 z{_F)pw$RC{Y!=_t8QS0H;;)J`8)3RBT#If1C)$8yml&$EJ=kIUfXHhutezVZME6W zd3hl$P~MtrKGa{~6A9J(Ob8s|BF^g#Q4$>GcfjM*I80p(mOf3veV0_g>Jxw@FB%c* zHAEfT)zKBK(hiYuAV8xC5>UvRVn-Sp8VGsvjoP!`zI_YuXZWwkc26a3=i$A{+ZyYrSyT=%_($6D*S4+ba)W}C#3eyUHyVBvXCd+>MQ(d(QEw# zgcdt(;L%T^0 z4GmdmLKPnrms~%LNb|xr=mG_%M!g|k4miYuk#Ve=&A{;y3;`T2z;Y+dxFd(HO?k+B zD1^@(0$Yonqkl~;oS!!{SR@|PqSSj?>~ zI3$j3y!x|;FuQK`3la?nw&g{jW+2K7bbOLvDU{nJ%lvE^8gKgl2X7_d1| zQ-BpCT;L(#CxhK$F_^nx*bvD*HVr$W+b48 z_{kH1=*`rh>{6?jNAL z-5G9v`@G+aAMP))!x3+OD{dx6g@^Yp>OUBhpFKSCmoKI7A%D3QlP?+HRh)dDxp~K} zH`yxaG4^9OQP&SY^*`YcV7MgjQ)=;d2Ye|oBky-1{NsPZKyMFsn|J;{U{_7^b4jfc zd~tFC=M3jWw3z3U=?uldW_uq`RoAdhJz%m`JQ1KF$U!x)N z?e}GMmTrhw<|d>IkTgIjBNuU6jh9bo^H@0Kyja2myQuF zXvsjN=m8FAb|QZ=3925|5>hG#lSa?z!fYT3VQtISKNPuEJx%m*P&s>gvCo@?h=_n% za1r>@R!GGgr9~Xeh!pz=25Lh1W`J!(=fz0{bNl)FURgOt^^{h^zp1%d+!E2a2yHs^ z%{ThK+{`jPEP=|GmzU?_;@XO6?b#+KRFe=jH9!RKLnpmHBJFJV@y~rlGoh1rx;P*! zqY|*XLL&m15L6jRn3S6W1A-1N&=D7@?quq5mW% zK-ElqFcd;cd!fDc;0SQR;CG|!G$s}i1cykQH^=C^)5%rF+I{+vTHOe={2=hwUJGjr z-B6NghkTGqnCJH2_p9xJ#%~wA-l}1kmsW^mnGt10YS+ldfFMa1tU3P^*DxHV50GuZ zi{&dX^z(~022`Pjssm?AXx68^omaKh)H-9ro!WFS8b09)bzrD!&&vjCeG>Tl;a8X4 zUgyDmzEszKzRyns1|UMYTmaVfDDDnF>XzxO&3_OOJ?5|k!4mcln|drRGcfDTz>B!%SxH7OW3fxEO-jX#EMvGJ>*Rzm@5HY+Fj znn@!M@IAs*G_&J?HTdJ$S$OW0*jRm_TaB>E$4alr`v5e(8gSMw{K!7)p z&0E?0#x%>u(eVrZC|t4zjSDNxohws_1{wPGHx>XQ@Dqdv0BreB7SX0+WeLRjF}fSR zQ2_rI{_3LMEDCK7)X{;jo)W-mK2(mnZHGdEPKJt&BNR`dxKXDQCpU>MJP~loD*PG_ ziQae@BECk~NI3X8K@s5q5?JzvBY+Dr1G;5yZjNB3Bj}UgXl@(68N(ea`R#xAKQ1c& zbFese!tF)v!azGoCCbrdbKKw4Z6cseNsanRo~#7x3gyi&wjAD;pohpzdy_UszWCz3 z|3Ci+>mv5nyQ$=DhYqby-#9RRboXC(Z1LqY{wl~&*>f&s>>PcOj1ih~bg9MAdLzUy zOChCMXk~#)w6u-<%Ha&yDtbtkG@@R1=RsNex;c0Ymi-l^eaW@j^1y#SfbFTQ+pfmyBW z%G1lLwLC+~ETyQzFEep|@#-e-N+50FB^|9vAv=K| zZXB^$B3?_j8gC;aB0~JoP;ID^p?s&5FlfJ3fZ;AZCcRn>J847QhN^}M$Ogi#Hgc_P z8L@34D=M_uQ6}g%hWFEbiwrq3u#|i)yg;rN4M#jNWLgEH8;)66p!qYv#65JfGv692 zl}M=riY0+j3eBB&8eWTQKr;bsA1>kqQUfSOPP}!&hcLBfu)qR8BJH47+i$uS$hLi? zHeUv~Z!svbfd4)!k!9RT?hhrjGydmQvhKcndJ<~zW8K9YQaqh@0!>*#=B$*1YFZQ9~98}xrBooIm6^Lrr z1Pj3%`ms?IOXobMnw76lj{&PxoxNW8Iy8O z8Q0`Y?|l{eYVy@@uLxg>yW5~cpuE14sS8?$g*j34v9w9|uWxuQSDOfrS1VaflL z21SLY>JT`TkU`OYBr4zrZ_mThMxPt}4CloKg^;5A-49P~@(dcIkr@a~$p*aH`%(>G zOe5`%c<%@xJ75L^Q8Qp$ahXe?ynurpC#lV=*+9h*#ABj9Ev4yWT?7V@G6+R}KGWV( zJ8H%w1_W;n0VUlimhW7k$0*#q3FkDEbi!nf;Sv#%(CCaSoW;EYgK3J!`tfLj$$>RKowv7@qnIpw7aHQ}DZ8`YfQFuzZ9Kj?HHBin-O?_WA><*@RSC8~WPP!oh2 z#0P+JFZjkV>Bz#%9)uiR-KNKdbp}*Tc8gOz6)Z%@k7q3BgIQyfgIYAuMbA2=_ z>*}P7BtNL0xVvT_|QYo<#ouq>Q!LnE3O zfX9hE^d}37p+KK(TTZvCHbXXARHW~!HZSz-t6Q!7w)zd&aDX5+K!v0^2sMiw7&;#f z!|tLrsJE$E`#?fb;+=|X%Q}liYFZl$&KuUtdE=m;C?nCJHdHl0^=F8D$S$U)GF9ol^Lyv-=D0X9tyK(&TY-a zc`%wxt8kcf6+jizB?u9pA+@fKpq{nDPia+e?L%J}2w(1OA*o5JDxzcm;sY;!jTSHV z=3|#2!Ai~@4C)W$?Ow)ZU-ck0g;F$5xyZ0N9!48PJ$F0v5Wn6tqNEo}fwfv_kyhQ&m#8(VdDg3p+>gF+_6 zB(^3;$5&MhUSoA0Ul-0MQbEFI4))DW6W8YV1CrD&xq4p=`0_UcPBLX}3fAPZ;PWZB zocQWmo;pN!p(?AYOp2BUfsZACyaG)02qoLw`+_5{ea=e0Ur6BUNgRK4d@#B(V zg0_$zRsM#@>G!XplF4mG1q?ks{bJq5Y8^1gv3tde#@pc>RQ4#SQ$p!7eit66`#=2s zHQ*e<>B3Yg9ti4qKVBc{c7_q}{TT#7(f{?zSZ-`ulj^6TvU|Y5!oosN|5P)|J^l|( zlh!m*2E6;HSI?c*OEM*lIqjKg)Ht5`Dy&`aA+vK{-!g~x_hGhs5D3~#$T^$-j7EbP z6G#MweRv=D7e9v60vkwRgT0!2u{`Yy3-8vv?~8C;-AcGQ&veJ~c+8Ep93o|XnU;2o zK|e3cq+1;nnFft#qww&&`V(GsvNE&cLK=JATtj`;v{A%~! z?5W_XO_qNM#61BS`)g1E!g?$oi4HzWa#nK*)L13cU{ZB()YX6~x;jiKKQ=ZNrnScu z)C%sAXM$!&6Os}6AW|RD@&QK%$}R{N_NI>KSy)6sl~BSDA%hBPuYKMkVbjB=of!G2 zIsO#j$KMY~JN^ttOh`dEHYYJ;)Yju|^6KhNSTem1I3z;T zR$N@XE=(vK!aO`^iUR;Z(>P01?fl^nn~iWgih~ z9VZnfyn>o_XC}N&q5Z=8AWv~1uc=mLWVvO!@x>0$W-#uMz-=>I0=vfhs6&8O_LJ6c zjFtjCgD6QrY>~Zg5<<#IU^3DW1wr{H%=fjT#}z%KW01Zc&7KA!iv|ky!mJ%(tiWR3 zV?b|1&U*X83qHVF;WoYQ%g675j#Mt_r^+)%t>*E|0oHELRmjKy)r8>q`&XAc4#Ru zQ6JT&m4)Te6(vCexe&58D7{i~%CNOI8%)az$jNUhDdmDxs-Nij5IF1b{qAS0>gq&R zNzsX1G+^FHj0}Xny$uvdRxhqB0$0-k${nyX%?~}m1c}J=$g1B#(8>FOgOn}dWIyC) z6bZGN6L2=#^6V7nl9OnU0f+{Bxy6rZ!ya1BWMLqdnNBRm=g`6|dhf-1?;!lNzq;J- zOYApTZibLjTD*j4ipt7RB}5lZO-(!3U`G($`Uuf|LdL8hTZxhgI2*>Sly>G5hYo_T zS7t{Le3x?RMY6LVA_IpBjS<`&$ij&%*F44}YQnzTY*o9NFFq2)C_ul`JXgH7u6QwF zluDb5Ls!x!57u;047w$WNW09L6Yu`u%%5lUgp+0`yPZMpw=zHeWUK{-AF`NSxCq25 zBM*<=c=RMSNjdWz$XVX}wMQk}xHH#W&jwC!(Q_Gyii#?2<=+VS_}Xt=a$=9Fv(7Mfqmm2b4>ff0K>k2ozf0ra&jho!(nsvP1KJW zqhs@r)(>3fOZv$_>XlxcJ z?Z$gIF5SYnWI6yO>8F-tjU4*e5t&Y>wF&EDD0rekF9r{`7753*GGoa>6zfSPkOg8S z0LGfSke9k&h9-hDpI(`C7XeW!VcS6tq&9GP0BQ-n%?GQ+Wk>JrY5);>@%mgcya_C7 zh<3vOk4Ye=h46j`6(I<@Ty?UxZBG9*#i8_mKA+$wDh~%{pMi8J>v07J=H2kzur02 z{)aH4R|>^mgW!HmC z00C%p36+QX^+&)`5lA^TOVPb}Rm-LEh5*y~K+xKXik2ZNCs2XeVNsA2ZeKS~+ryI_ zRmJsN(+2+jb9cD?MDEq4mmgs5K#JQ&?avA7)L2#;#oE~DgFExeLR&p+JEPZ0a##goS|G@ejufJ(%Ot}ZS z?bBxImRpzm8}v-VtAlf~WHz;hu6%Br^ba2S-$*w@B|{8!31C@B`VT%J!{dD`^5w4M z#+W+^P1kR4p%py7+Ymm%VKUOwzYwrC_S0Z~cXBTW-o1y;dGn?&y3;Vges}$p|M-$= zzl>vKVxj`t7?k2K7GrJ%zh@zFy7OlLF1(=G7&MKd{#nE0%tAg&%Z9q>8Hg~|OQsWgyZ{APEynu})J&yklQOoR%REZDmET|RnE~a$E!ZurAtlgm zR}zh`o59)A*3u!oY-`5AvJlU=g0K`S_1zYYQc$p}|MRH8Mr)lDx6|rkzfrj-#li<| zZ(15_5Y>Xe0;TvLoBryq3fe6*D99`!s2OQGf>q=?XgS%n_FRIwHO~@_3JVCxhFMu& zp#R+P;E*sWvFmOEQq~icoVnHpj|0uzOpPS&vIFNebqhQaD}ms z{TZuP=Ekxi%1Sp!-i9!USS%wWzCD=yIKWXyEsVi+-!!9(-!bRZtq%z2xX(x>Xc>;L z2SV)g%QXZU{|qa|Uk8?SfiToZ^`Rf5B6VB-VQXK`PK=01$u2yxkI1@tC+?ajn{RqO z@%JxY{Cz)wE!wpMFaYm>5=_^V4wAYdvNj7MnKNO=;DZ0Yzf1meYAqB}1&?U1wzU*x z-RpVV?GV61(W+w2p99hvdRZ(LqPcGMPa23V=0XWp9*$ z%5pYWD_c1wP+F6e7UAIi@LAL>o8@0!hek82=`KT}lfDB_yIp+6_%e*CEm(KsaW2P< zandxr6rZW+@Vj=}_nSXFvCyDg zFp!?-F=oJkMS2Tg&#gc@QM{cM%^7h(RDsn1bz6EC$v##~W;3l`bTo0QG;?5sLUKLE zWMJF%7$%J(!KDlCg;d}^DzbUkAUU&te>BRV9a>+AnUv+MeK?I0HrkgVoVnqkpCod9 z#Q~c9_(8G%K_&6j!?Zj7YS$Z*i*_?nV52y&`)YXMS5bz+2sixC#jC%MBiM8wq)!~U& zy>J4FOB8@?i|U-$XWR?tMjGjI_ag%MuS=zbtQrr`bA5z7WdE`R_WSaGYJY3p>@x%) z$jQxt#0_7n)`Lag2(JafeKi|)-qLabk_-^26;Ob{R8(UCnxBV`2|93MEg-xo6&^dL zO#r3+CyyU1XB$&dLs8uar4C_Wo;X`x^}g8;%u_I8mtUEu?nrXrq+u9LVoRWL2&x*P zA2uL)0ZiZY#l4fU_k$mi61J$MiY^v27&iGJ{XvDd0~#F&+p?sIwAFIW27rfdn4AG- z5WG2TFgT8X-~cqZEJ8!dnR)+MD;u%r?ehlRE}UZGGsKj&xA)@d&q}v^yz*XyUDlA6 zb&H(5-_aLzE;zEi}BAd=(9fzoX%uxx)_!IsJi1^GiWQ~cf<1R|Lf&-55G^}any+g zsN3Iz^3~`CSw-mkHSXYrES5Y#^{||K>hr=M?+CQF7PLddg*?z~1KkQrRCxEXv?RaF zYhmsr|FGcoAa4QKAhRzh11K`+&WbE3PzBEF7uWI`Kob@|@%fwkf|B5lOMn8P9Qm@o zV$VfC03<)G$s27cnwKwL{8K=w;Yy-LvGBoy`TlhlcDgeI2(9_v-`6*GFF??z z_M5);E%y5S_cab59~Sc< zV#EFXlAAxIg?W^!@Utbr=cPxqMLjZqwj}uY1e|@Xh1q5f@VRLzisFP&9IX~cYHY{D zHlo;Yz~8Nh*-ykFxx@woC~}fD1Og#owrS(SAXC~zwcBE(3ZKyo=V@Q zWAjNpiL{~8J1PRZ?zSkC`b!3Lk6!~Krdkk)LIhR-jyGFh8sfR&F?a2e&*N0?gk#~7 z{@+$9^7uEj%TO&CrAE*y6EP~B@yDpf&Z6WeCcP!7wCnPE7TN!CBK`K}> z4K8IBDmAh8#SK?(0o($q@{|pThH1;m5Y-4k?ri<~-g^-tK>I>dQ)XJOr8jBTfhv4_ ze6)Y}y%K`C)-x0S36|Z@@;aB6>YRlExdC@-Is6{skK9kSv~fexijlG@D7@uR*B@Q2;NCylq@aM|md+8>#j6Jvj*t`@ws4@rZ9faYWi%|2%pMlI_hX{Tu)C=wYXjrxH4a)-iHtYK?3tbERdY2z$Ut`KBm@$Td;u;2CAvGLsg z=MsiBujT5ROHmv)y*?G+-4VQFX~k6!OQ?p!{+RQi**(g^ps12^`yzDC9+QSp^Y(dZ zBa1^1KD6dZ3nDG1Q1W~6hkZUA;C(1)@?e5gxUdkPIUk7UN`oM?x+X*s!j4^rT9C-{ za3t7DC5!=~U1^H^sSxsqmLRiM4!?>M!73*Rz`n5RoiHAG?N9|tdTZ$&99>*7&cMt( z(HK*=JU508gWBqBjEK~ylX3!0f^0r%_;adK9$XK0&eGjd6jXQl44Yz+(pj|!l<^?u z8`r>saDs@dsoPax3!-I7r#L8qFl{ITy?06Ctt6@oG7sO4GmeST)Rs*$z`3&=lP+;U zJx6gYh36(aKZx87ecd9kUBf;hEmS&g8f>aL=Gh$7C`W4{#Rp(>>;S5N7|wZ!JIsoE|ONNElNStWVJ=E2P&XxGb(o{n5+)(IHP4I)MFm3Cm?zVIkE7ao8N zDqSRJg-SMUaT6q8K#$}FTYz9-N*m*b`&jCVY-U02F!crk7Z5Jf*B${+$_}L{M06P! z6NaoSeM2pmelAcJDE2`3PRtwxor48n3AaFGUyN+nmfTF3ODM&7ts|HC0s- z$J4>;0X%OGXL}e7r%D&N2SK10G+kK33*4XsQQ*9I_3OLRLW;+hU5OAX3Bjqm0ULhgc&*;|tR{MqY9dTK!b4hP17+X50!q-Q_ zqoE~w{gGw@tA?s-2#UJR_We~bkQneN0Q5neXbD}~(CxlFKOW4EC0FdqInZJx9kh;P zGSQd*aCy204Hj~9yg$%)^k3M9(&JQIH{9W^3{uDh4Cr=<1~Po0O!8UHArCtHn{PRl zwJPH3=RYlh(vmfG3TOkMBXuHbTGZ{0;7yb>4T-3eVfzWr+`8G;a!v|Fa{@_&L$5(4 zOvcfw%lO9#ZgjuP>x^^um;zlYev;SdlZ9ND4f6vCYkUb8WULu2kMx1tFqxvZ|n$Lr?G94$lO-&co`u@ zJ>WP%j6$MqbZ*Bgr)0woz63S_SbA_0#=cYf>grvPm^TlL9{$x+9MO&n{1;#RGV0z) z!lKe!rcbAwZNO?_0);+T+}R%UsZZJqhjen|va~R{b~_v?jKv@Uv36pe`}>X?JJz=p zKbAa*2eu!9ej7mF3s7cvr7*r0Si8C-U_o*88fIo=y?aAoBBu|@6Chm%(kT&4R+xpB zhSoUqHhc=G@SqVs)4_2`+3Mj7ToOs&Lggt(7DKKP2v!&aY5I52otp_PNPhsbO@R6+ zOewOriElZKkk^e#wKkH zEC^%=0cdFlUDn%Gp#`+X(K0+d)m-r_7v(x+d0P2EjT*k%PlvGjkiam5^b<^uC-nvh zg`8Gl8|E5`>(^@`d*Z0&jfD5&Iboa$MUz?JUZahA*fN+L+0cdLVT7=U5ES9|jf+la zLAv}$f=fAMN>I3Rg!FE8KD$_LrUA@A{^rfY%0z9))Iq53P}zqpSs-~12`R&F2B1{X z^nqd)m!X^#aTa#2MI*f93>)sO{=0Ip|pjWpLAu_zkb{%1LUKK9QF#z~5%TtEO$=g$JoUmzIMbpo+2 zNUO#!dQ&Li>qy?nBm}9Jjt?wkqz(j4V?i;mrbX69q+zi8emU4*V-xJYFdEGPc!3|h zC#Wv2IJW@9R>}`lzeM1OiyT{G+kTMk^U?I0WO^imS3b&n8IDTGarIa!oxE zD#)nPH;&na5;K#8D}EO9p#frBo&NMKFoUk`pm%kWgS9@^yqfjhGnGRN1BiW*+au5 zcYv4;7s9t=d2!B>jseAeR2uXmVL5dAW>quO)3AFFdJuA6inCaL=qu>Ya#@-!y9>By z+EZ;TiL&KMzu&$?)DGjBEzuntU&7D>YHJv8aJ^z?;w8d;K^b8DUqeVJD+@M=V~1}E ziKf;D5rG&~?qDY<$AwOkLA{zFc^-2Fuc|@j2P84%SHqA*mz1LPs$ZCXN-eJq6B5W^ z5_E+p`UD%6=sTQ}QMMk)Q9qOH06y6YA~q~=6!b*#8rvYP>p=mR90A#KgAu1V067Ml zDQVjd0EI%nY?u&wgHs;CX=|^R3w}#OUM;W=fXcyaub+EVAe2fGOpYU(ss zWidxr>|&b`JwsmjwKv z#V^;82G5a0f>Rg5g&osUhroMf0hmZ#=F-M`K$Q!ud~k=ApT-6WK>}?La;mDzN|0S4 zjR>&ykRHPc`_<3oJlFSuw*UqR78Nr#NPEW=LU{O;+v7?Bm$U&32me9ZY6t2jE~BKe zr-$m{bzxB-Cn4dv{uvAloL*A2*=2}IgB{)2ZK(e&mG&Ms+})7HJ89n&a=;dlfgRCK z{5ea8j~^lDyP|*2wBh3mlxzy6sDx5fh5GhD^52p?{ue$vodg>W+1kTMUHx|g2@kH* zJEkWDF#2Ti#ya8BQhj3fRSEwWk_2pPi{d{tc1-lu7qxtJ{3EYNSrbUn(K`{n z8;!P~CKuP+Sx=KIYs;eELRNoYV#6H_-CbrHQ+>f zWb$3PU4}|`-rvlffF|SmoRa1)^u6@;|Wheow)jLj~dq+PG zjyBhOQ8jyOIxS7FsmzTW^YfFZe0^G2<+#2Yyyl@zIt4Xi* z`E32^{^7@rw@g;v;x~h&LoGjIdV1J{BSOVEP94rXoBKp2G$&_?E|~_^I1P`ZvrjM_ z_CTlfAqW($k&sFHEKNEW&wG%flfrixb&#P)W*R%3DgwJXO6%Sb;u1Xw>IeNk>~J+l zr28=urK32FwEp|PL+_rgWM97`SaMu}LTLGfieo$?u_{ekU0-S5)#%$E=41z*&g&qr zlJ92Wu70>K{JHdvcW_a0h15C5CwmDoguk~45;$(GO^O5_D$iS;9_6RgEk&8H?XdYq zIc+!`Pt9?6eP@1xUM+vZvTjrbTlt-G%44uA6CI1?I5{8}=NeWT|1()_MQis1re_YN zHA5aZ(P{tpbDnMtVT;w+y!dIA%03zUWDl#Ipo6 z)X+61G_yQbZxA!CL2W4Dv`W`Z=&gBFyJl|Zv>IbkXx;fw?&=O~=rB)Z!`*w?+k1S4hU2Vq27TGIvefnN?mXJ@MmpvE zEW1$Fa1^zneXCBh#8Hq-LQAz@nTz}n&XBP=RyPl zcZ}^Eu(OofQpe0OnRf;%^*^LY0|ZU*2})E^6sZj~|69R5V0rGzV2!}-Lk!t=(-os! zU=gOJ53nfM*=zJ{rjZI2(^|c^mNrHcOcd*tSY(e@SD&knmtXM~zH@DkdU?LHyDd-Z zdSiE9RD0^_(GP6PV;bJ2+43HlyLQGAaC$}!vN}4Js+p-?>rJvNCZEn>+dOt&2&b>& zpdh$TQO-S;VQX&DAYLCWwdqE;aZPaH`=&G<&U?3KJIp<+O`VKxt z%%i!f^emvOool=l1-JhtQEhIRH{d_~DLvQ_P_DOez7){KygQ>ZEjFm2;ACV{f}&VV z$n&1lxHgp%3)p2b0?q@AbzB*S!$5-AX<<&)Ra}}69EfO97r5P1<6*6*B(R?k%2bChOln*O_|v1<9T&Ktwg!SRj$T#Vh;8coR>KC5-W->& z=;$=7F*14DEqEy;36}O2&nsIxD0pP1?i@VN!`jf%qhI2>Ce33MU-S%8F(?C6yftb! z6d7+AxaN_Houhrw3FkaU2Ad?c1lig)8?-4)%zqMZ$y8M5yIt|Ut#PdXZfFJ1?I$3mWF|Ia9lGoDFy6bOb~J5w*0}c@MZ|Kve=UaXkP| z)Y+|tsS2P;X*P(V()v79=Up_<#fG~ovU4#DVcUwGGqgnKY6Ol+C?v)gsSN$KryiHs zyz$5Z(UnS_MQDFhN51Nb)#-qGk@db^*}RE$vr{GY5C7PAzBQkRoFh6GBRsW{9kH%!r}#WeEpfhQR5_~W1%*^yV!8TjuB~TYbvm_ z@=4;2PwA0jG(_4)vpWMVP8%aFthDCcnQdtK_uI5I{ZAiaFT2sRB-?>Xw7L|9hGIyj zvGuL_0oHt|UyTLK|1&v!-eD-ROvx zSv2w_PnecMl3e!p%#07l6Cd=G^cdMTezI_^jHB`>V z@~;vmkdN^ecj;bb`b}kP^9uO(b!=)7aboI(ted>2Wr&6eN9498EX9aJEo7F&WBF^7 zvjew3dGvrGW~E+JUYlyDrM-IlI&U=N!mSPe@{udAp)stZ^O3*4@#(nKOZYO{RCT|& zX%h;cOJQ1c+e<|J(xnh_;VyalIc$IGUppF6&&-?Jv>Mw+mhsn~O-7rk2~|O|ZJ;0j z+TZ>E@sR@!G0@YCMh72f&1*uTb3ey%{N(Rx#XK!RYA`-7Yh1{MOXAYy98AjB8kOV1 zwHZ5%di)dw;tH|xC5BgR6)=xLr&YA!_{)p3zg(2~n2e*h4V|FPh0EJ{tJE)7C42eY zc>IKA!YK}`+_Ga$S~jCBQ8mNXsG*vK=pm|K-YIJHPR`6lyf^rw&ZWnv;+*qNs-Oaig3JL75W*BvD}>6B zh>(Cl0wzF^kc0qX%y3`Y-cvR7e!u(2z2A54*ZwEj-Fxl5)_T`k&-1*`Te4WIU|Nhk zxYI^X3%&|vBa3U^nz@O<`dT`KzdEH8@3Rn3{ubG5e)f?YkydO#wt00@??a*0+N2co zEX(YU3tn85r26&F`M(irrCXeARwCUk9h6S5%yku~)F=^{y8{U6y zeI6IpxaUoGV)*+yeZ2NM45$mfHNuN)gsKTqO`jnI;I9vFazneJiw0S_>{sH{4Rar~ z6ks~haF&6YysjqZGs*gdV~(k2@`<8xk4@=^)tISKFvI3JmQK^(A_ztV`WWBRe@k+7 z*m!UlFV0RCmNsWH>_O2#GuJ7S(y#aV(oKihMWn6XByOvvQ$T=7-R@E%Ae(QHF)X9^ zU+@XxYzgXqztPBm9QSlaIwduakExw6hTzP2y&tl}v6R@UdS&Od_2*oazGLltRO_9B z%Gvsr)F5wVnVlcAPEiC@LRJ>$Y;Z3sd=E%-E{81F22!#wE)&r@5%Tm1FT2?yQXiJ{bOG|0G!tcImr%JThm0f6T%elf@x#CRg(2aqnJFKo^f@1 zk-WXv3iK|^dOZz&qE09uJQW}t1;BuTUCq1)B(2C9oikrX`_#2h6ixTZCkm&g*4NQo zkUZa-x!Q@5Lbs}jxk=gebSqrH+&Rr%U66iZ#npFK7Wm)P8=MacQpjrdX!#FI=zUu< z@G%r`55wB={ps-$BK7v3Wv0WsUD4ZC zx}yy}bY_3~X(Fv`N1d|hmSxF&f8vfG+}Iv|SMpuaJryIBD%Ayj3p_0%sD<1&!<$pa zU|+ZguU$EN}I=!H^L5579oa_c<LOWot`c^+IPnu3KwmKVhLn~B(?7S11ZP!ZPa|Z@c#85ki6vv1$rfr zCMvI8X&Z6O%&hi74AYJ%O1kD`l;RSoO06vE7&@cq{}x^bTxYpp!d+j6rutTv^P1l>G$!!un^W+Y)^ckNGW z>YiBjFkAw^lkjb;gL;Hxk3_AO`t@4asAWa#ECXGI)QZV-5$@sKuhhx}BaB@aY|89( zhIS?^W>@PD&Q7CswoPT>*!0l-XP#xUMp(JLR|4DnNZ!(b6*ill?aWCr1=jp0TU>?D z6TCnC2+2EA`TDSgh+odjqbB{NGqW+2eT#04Bwe?>gRktp=A|4ktNakqdZKcP&O-8$ zKan;a;#4$NjYIoHc$U)}Pb4(-seOCTxq`EVy~`5Afm1TDTd|rQc&HgpGBgkJs#S%G zGWazjQmn9Lhv@PK;dgt4l_Fu2iRj9vJI~x8^9U+2Cqgyq5Uqtq-B%bUo8{&lf*3m5 zo6m~5+xvPT*v`U0P3VTEn~FJ!;zh0^m08c!VBIo$bFYqk?T%ItT%Va|CI^t#jwTBT z1n|WaoeBu>iNh(`A^4R&bdMvoa~PFMOg+AP2bSWm#;B$V9^wg>0gmgYFw;KOeoZNK z&!-w~)|rTjpAu*!b-`L`!*{?NCd;5#LMWQ)jo!P zc=#7B_L-hia;XmLbFf}0C$f@HP7GPeYrzdn=0Xa0R}yab)!7GEu1~Vt&UUC#jAvfE z(=}LtsdDbm-OEnAM?>{;|x7@n*MpcNkbY z+sABABrl9Z6qB&Ey`|YP#Q~k0EQ3by;Ql(tgk0U{7KSzHQ74uCoL=NY!^T3PsUvR` zgKlavx`O7myjx)AsENAV9Icskkhi3HaCgPhy~G@7O}AUPG_cnc^+_*6;;0P+nshQc zpfTwlIjxXogVE{bo$%KnuY>8R*4=t;TI~GM=qJj@#R3Wd3SV% zrI}IO14XE`dnB{z!gdB-fT;!JhUr-+UJ1Nd8OKQYcPK0Du?4K6A9c$y?)3(bTgS#*ywbx&}k3(p_8I5R&s=bp7tDoj=$- z?oG{N+5A;!{zL(GQN=4kU}ZYzlt1yqKLJ&MsNlapVcmD&+_`NmkAXWvLhnlK(nzanyt8mcZS4?~zT2AJSf)N9CP+9WmO!4e4&^ z-u`XXjL>TEMnOvf=h892&LAZHlNG!WYfA+R~1W>@HgD z^?|lroHs)(PUZZqbuHXm&c(NbjOWCi`G`UU)==jVlsJgM zcAnuQoQ(NV(I6Jw;H7VUe-4gwB^X9CpS9y=|M%2PF(%l(@WI_;?dq%!Ud8m9lEE zxPD6`Yj^P=ZzbR-kDL*#6KAZU_a0?v#~$HY%oR`eaG#~f&HU`f`V3VuvE-+F*Mvu& zs2R$2B!&=b=4bRAhY0}X@^p`zTm%-7yRbtoX`S(BS!SDq^xt!Sm)bA>lGg&R}h77L8|+LKs>+*`G$&R}{q;P-Wd*aRvZ&O|!{i^+C0zj~zqriXB}%s`&{ z%%#n|{ct3m5qfjXqO!p{d{p z+<{_FW}`~)$2@r<$FJFxaf3n)?mBEA6+YMm>91gL6%+mZB6of;a9yMi7+HkxPqSX&&rn~c~l*ec>l6alwQ8|RIFIhC~EeXIrvqV zV=zW>XO&^I;|_fzvs{WNpf~ZFi*GQMKQ85N+T^^h-)w~^TK&kl!1d0~{DKtCOygq& z*7_H0Njb@@s2Dl^=n~7HY)hY8$SY<&lf?t?C>OK^^{h@XHVUbW{!kMDqKyWfxXn6F zq-E!pl@%0PNJ<W~gIwm?C2ORC%78i{_hU!ipXk z>>QoJ4)IH>?sSqKoU%v{k#@b;p3~55W+`~au$5J9V%zOVvijE**8i4dYg$~p9mCvz1FwI}8`Q(-L-t1D{OVh_mMiRce#J=wg?dw#ud(K_%Sj&P*3K!v062p)m3ZFwlxEP z#rf;-P{^iA-pzP^$!h#(tNBq@^EFe%T(VL8Ik&GfkS6f;G}DBH@PfevFkoRdSA~h5 z1tiRv{GVX|)QaC=|2i#_m+D_UQU>2hr1Oo;z0u$Z5GAW(HE;7S5dRPnAKIQHv+Rp4 z(Kc^LUMEcU3;hCsUj+oUp)@YqC)vItW=Qf~F3@qbz*y#1?Zt;?(pm9*?T!#XxAsv! zCc}|1jKF$uY1pO`aCn7>8`>7M1EXrJ)7Od55R&phbKFU!p5gl|Hy$(ItY7U=72rZP zn~JS+5rb<6Zv62c!kO^_2PAKNke#6}$v(ho1|B)CKQF5wC5Y2JUKtngGeR;ipPteSimzrbU52kmCTa%Jdm7nAsyz*;|L|`b8n}a;uOgK9?2}RjstkYf9 z!&$d1wn1%n1si5EY_sNG;JQxWeqQg^I0>?%fczDqhy$stbZc%{Kr^{HHIg~YwxvS; z%64G%daK;p%Oi<2vB@c`X}%@p^H@o=wqZ@7BQ-1+6dpRHO7=x8r3gtEbgQkpaysIi ztOjrQMf80A!+iu+qtmJIdZUS4tqq1UtF!w#f<6@S(X@pHhEjwNT>JtgX;G2fe@%DW z3L)AEH!lqe3c^rmI&3TK+F84Z%F8_=j-`xv5P1E;MP3Y+P5K`y5>tw)E6Ns^1_I*% zqL@eWMOB3hLP#JT9Kul$+-8*)uS(7q7MPR7aQ{D+NfxuJjSbzAJRF|U`l8j1HaQL=b9vB8JvYrE@kh&kVI#hp}7eoJp=F zl+y))E66Z!BTTp;`*eL-yPCW^680Z;V2DkV|Hrzp`a4&~z)#mtG1UAsp;S?xW zUz*T2{LtJJG(>HRS+aiIhjohONZ!-nxF!h2IlrfbZ23j#i;ODrSqlqp5q)(JhH{Dz zUWv4~5B{!tKYDTjJ;RKFBQD$C5^Q)DguXnY%YuacH{W|`-j-7fLf;}*Br1Tt)jcQ~ zf%VjF)X`rW#azAh@&wzY31WP9-|J5c6)pgAg$fDF0Nr#0RaF|CyV$MLkN?sr)!c}O zC~?Qh*>S-<7u8&?*a8dHt>1HD20z1qY}j4YNoN|08XMKq(~6WuZ}&M7d3UXhL6=_w zSV_0#@Hi#Ga9cB`w<+7G18_Lq_s3A+&N;08;jey{|K2~-LhtW5f(98h{*}PJU&2LU zogJ)m4qdaAU1r3ZY)PK7xo^B2RB;PRU)mcwT~q!gQ8~u?dg+4DRLND`EZLdKn)A!M zUO+9Nri6-IAx+|?g<;{KtzRPT5lyzo`k8tjwaW|#M{IfNsc@&kkc{{U1+OEY8W+t^ zT^D~$%kj3K+g)^Rb%SR>!-Et2SihJp4imROurw8DH8YdBpwztHSnO-ip1Z~+rIjbUJPy zKoz)(Sxs8-$b95HQ?S7$P@|HWjy_kGksE^O;UR>HG=^md%3Gr|o7aQRcjPJv|8DQ7 zu{5o^pNts(kI#>j()h9CoDxsh&hd0)p*YX0VTb>0qbW1)d{$Eag1_fueWud=$S2Z? zE|Zc(F-a0#738g`?TXvL6!fXS>hF_kW0LK%*=#ruWWF0Wa|$mdsIk)#8gIJ5Yrv=H zFgj?~fV>^V?D@YZnO!VZ2-^iSwUSkj@UafCpUpf?ZzuhId(@%nz0QN|x+fCg~uc@rp|P|3*s*ZBMR5nvCcemrsj+%{|Jo^he17~y!lHG$uYzCgx4P>Oe?M} z<)O6Wslprkh#wbZJE~5UvhI|y*|&D&M#W&AoTyYBnp5IJ_KSO@Rr_ z5bT%gET~7&v<}n`q9zh4ScvXHf)N1gPn9z!Z3M3ecR41r5y<0>znr&Zh zOYCh})A;bh;~ifVF=7FP>MG3+4$#uuummVQ4YE45!fMkMXc-tavw9heSC|Ib{fm>J#6el68zT}gNi{Z@PHJjjA zPqJ6AZ52%>ie`V-V5()=dayaDm*YLsIZ1{%wNjC7ro?|JX}XD_1mRx)GEIj8+dew~ zV>b;{#OZON08~c`JUjM*NnWF4(iYc~9eGw-Rn*Y!%;R&<^_`rYK*_99%a>}09krh8 zz%IGb9Sw$v>1CEoQ3+iHfGtjSgBy0Nha6LiLB*4+ZuBqC-J+~1AxMSYnpI2BC!SrEN z=as*-uoxU8hX1m%n4Sf|bLSR|GasD!3G}x+{gZpkP912w>xK?W&QM z4&2*TF}1yqJ^i;F*4$89xD!+}0V1ZRBEH?4DNx83u)0_4 z1_Q`jslP1K%i@8JJ>W+)B#)r2*_>N9)qNL3fn*&M_YP^*v|y$qH|gGjNZL+ymeY*H zUd7tg+T-JJ2pS!#Y*w3jLV(YER;FjgC2Q>*rl?(vnq)fcpd}?9=%8!5JqP4jIL2-Y zrFj2oH;HI)tud(>^`31cOTcIV)b>CzbqK2vR?ZIX=Aw+_J{jVa^n~^A^17Sf`u4}{J_w%~tIo*CFb{AB!)!yt z1{RAr2-`QK#A{|oFk<`aZhf*IFuGu|t-L;SFUWRl2mS}Q1N zWn`bG^{t=;XxSOK_CtW-CL&hBp6yWk1}ue^HGbJB>iGGIbzpOr@8;znN1eH%h8=m2 zaIt-lVW_0IrkebA52aN8PO)kL9Bl2s2b^xPfEx6i`-65DhO%7e1p)ElWDc}><+E(= zn;l+kQ=3QiA}tDQ6lVXI{>%lUJXW^0r>*TC%L{bAudUn|iuQ5ur2|!T&kt>Q5VC`5t-t2~zI1rPPUN|1SoR*!??}u;g&oyIqwpckz<* zh>Jh6UI8PQbZq_?i&wtgKYNol + 5 + + UMLDeployment + + 520 + 155 + 100 + 30 + + <<MotionPlanResponse>> +Item_1 (Group_1) +bg=cyan +transparency=0 +group=8 + + + + UMLDeployment + + 520 + 190 + 100 + 30 + + <<MotionPlanResponse>> +Item_2 (Group_1) +bg=cyan +transparency=0 + +group=8 + + + + UMLDeployment + + 520 + 230 + 100 + 30 + + bg=yellow +<<MotionPlanResponse>> +Item_3 (Group_2) +transparency=0 +group=8 + + + + UMLDeployment + + 520 + 270 + 100 + 30 + + <<MotionPlanResponse>> +Item_4 (Group_2) +bg=yellow +transparency=0 +group=8 + + + + UMLDeployment + + 520 + 310 + 100 + 30 + + <<MotionPlanResponse>> +Item_5 (Group_1) +bg=cyan +transparency=0 +group=8 + + + + UMLDeployment + + 520 + 365 + 100 + 30 + + <<MotionPlanResponse>> +Item_N (Group_2) +bg=yellow +transparency=0 +group=8 + + + + UMLClass + + 510 + 125 + 125 + 275 + + << Vector<MotionPlanResponse> >> +MySequence +layer=-50 +bg=light_gray +transparency=0 + +group=8 + + + + UMLDeployment + + 855 + 155 + 110 + 65 + + <<RobotTrajectory>> +Item_1 + Item_2 (Group_1) +bg=cyan +transparency=0 +group=2 + + + + UMLDeployment + + 855 + 230 + 110 + 70 + + bg=yellow +transparency=0 +<<RobotTrajectory>> +Item_3 + Item_4 (Group_2) +group=2 + + + + UMLDeployment + + 855 + 310 + 110 + 30 + + <<RobotTrajectory>> +Item_5 (Group_1) +bg=cyan +transparency=0 +group=2 + + + + UMLDeployment + + 855 + 365 + 110 + 30 + + <<RobotTrajectory>> +Item_N (Group_2) +bg=yellow +transparency=0 +group=2 + + + + UMLClass + + 850 + 125 + 125 + 275 + + << Vector<RobotTrajectoryPtr> >> +MySequence +layer=-50 +bg=light_gray +transparency=0 +group=2 + + + + UMLDeployment + + 1240 + 205 + 110 + 65 + + <<RobotTrajectory>> +Item_1 + Item_2 (Group_1) +bg=cyan +transparency=0 +group=3 + + + + UMLDeployment + + 1240 + 280 + 110 + 70 + + bg=yellow +<<RobotTrajectory>> +Item_3 + Item_4 (Group_2) +group=3 +transparency=0 + + + + UMLDeployment + + 1240 + 360 + 110 + 30 + + <<RobotTrajectory>> +Item_5 (Group_1) +bg=cyan +transparency=0 +group=3 + + + + UMLDeployment + + 1240 + 415 + 110 + 30 + + <<RobotTrajectory>> +Item_N (Group_2) +bg=yellow +group=3 +transparency=0 + + + + UMLClass + + 1230 + 90 + 130 + 365 + + <<ExecutableMotionPlan>> +MySequence +layer=-100 +group=3 + + + + UMLClass + + 1235 + 180 + 120 + 270 + + << Vector<<ExecutableTrajectory>> >> +plan_components_ +bg=light_gray +transparency=0 +layer=-50 +group=3 + + + + UMLClass + + 1235 + 135 + 120 + 15 + + planning_scene_monoitor_ + +group=3 + + + + UMLClass + + 1235 + 155 + 120 + 15 + + planning_scene_ + +group=3 + + + + UMLClass + + 1235 + 115 + 120 + 15 + + error_code_ + +group=3 + + + + UMLDeployment + + 1595 + 215 + 110 + 65 + + <<RobotTrajectory>> +Item_1 + Item_2 (Group_1) +bg=cyan +transparency=0 +group=9 + + + + UMLDeployment + + 1595 + 290 + 110 + 70 + + bg=yellow +<<RobotTrajectory>> +Item_3 + Item_4 (Group_2) +transparency=0 +group=9 + + + + UMLDeployment + + 1595 + 370 + 110 + 30 + + <<RobotTrajectory>> +Item_5 (Group_1) +bg=cyan +transparency=0 +group=9 + + + + UMLDeployment + + 1595 + 425 + 110 + 30 + + <<RobotTrajectory>> +Item_N (Group_2) +bg=yellow +transparency=0 +group=9 + + + + UMLClass + + 1590 + 190 + 120 + 270 + + << Vector<<:RobotTrajectory>> >> +planning_trajectory +bg=light_gray +transparency=0 +layer=-50 +group=9 + + + + UMLClass + + 1590 + 135 + 120 + 15 + + planning_time + +group=9 + + + + UMLClass + + 1590 + 155 + 120 + 25 + + << Vector<RobotState> >> +trajectory_start + +group=9 + + + + UMLClass + + 1590 + 115 + 120 + 15 + + error_code + +group=9 + + + + UMLClass + + 1585 + 80 + 130 + 385 + + <<MoveGroupSequenceResult>> +MySequence +layer=-100 +transparency=0 +group=9 + + + + UMLState + + 1125 + 230 + 85 + 25 + + halign=left +valign=top +*Build* +ExecutionMotionPlan + + + + UMLSpecialState + + 0 + 235 + 20 + 20 + + type=initial + + + + Relation + + 15 + 240 + 70 + 15 + + lt=<- + 120.0;10.0;10.0;10.0 + + + UMLState + + 245 + 225 + 130 + 40 + + halign=left +valign=top +*Validate:* +- blend radii all positive +- last blend radius zero +- only first request has start state + + + + + Relation + + 195 + 240 + 60 + 15 + + lt=<- + 100.0;10.0;10.0;10.0 + + + Relation + + 370 + 240 + 30 + 15 + + lt=<- + 40.0;10.0;10.0;10.0 + + + UMLState + + 390 + 230 + 105 + 30 + + halign=left +valign=top +*Calculate* +individual sequence items + + + + Relation + + 490 + 240 + 30 + 15 + + lt=<- + 40.0;10.0;10.0;10.0 + + + UMLState + + 770 + 230 + 60 + 25 + + halign=left +valign=top +*Blend* +*Trajectories* + + + + Relation + + 630 + 240 + 25 + 15 + + lt=<- + 30.0;10.0;10.0;10.0 + + + Relation + + 970 + 240 + 55 + 15 + + lt=<- + 90.0;10.0;10.0;10.0 + + + Relation + + 1205 + 240 + 35 + 15 + + lt=<- + 50.0;10.0;10.0;10.0 + + + UMLState + + 1385 + 235 + 60 + 25 + + halign=left +valign=top +*Execute* +*Trajectories* + + + + Relation + + 1355 + 240 + 40 + 15 + + lt=<- + 60.0;10.0;10.0;10.0 + + + Relation + + 1440 + 245 + 50 + 15 + + lt=<- + 80.0;10.0;10.0;10.0 + + + UMLState + + 1480 + 235 + 95 + 25 + + halign=left +valign=top +*Generate:* +MoveGroupSequenceResult + + + + Relation + + 1570 + 245 + 25 + 15 + + lt=<- + 30.0;10.0;10.0;10.0 + + + UMLState + + 645 + 230 + 110 + 25 + + halign=left +valign=top +*Validate* +Blend radii do not overlap + + + + Relation + + 825 + 240 + 35 + 15 + + lt=<- + 50.0;10.0;10.0;10.0 + + + Relation + + 750 + 240 + 30 + 15 + + lt=<- + 40.0;10.0;10.0;10.0 + + + UMLSpecialState + + 1015 + 225 + 50 + 40 + + type=decision + + + + Text + + 1060 + 230 + 35 + 15 + + Action + + + + Text + + 1045 + 265 + 40 + 15 + + Service + + + + Relation + + 1060 + 240 + 75 + 15 + + lt=<- + 130.0;10.0;10.0;10.0 + + + UMLState + + 1100 + 565 + 120 + 25 + + halign=left +valign=top +*Build* +GetMotionSequence::Response + + + + Relation + + 1035 + 260 + 75 + 325 + + lt=<- + 130.0;630.0;10.0;630.0;10.0;10.0 + + + Relation + + 1215 + 570 + 380 + 15 + + lt=<- + 740.0;10.0;10.0;10.0 + + + UMLSpecialState + + 1785 + 310 + 20 + 20 + + type=final + + + + Relation + + 1710 + 245 + 95 + 75 + + lt=<- + 170.0;130.0;170.0;10.0;10.0;10.0 + + + Relation + + 1710 + 325 + 95 + 260 + + lt=<- + 170.0;10.0;170.0;500.0;10.0;500.0 + + + UMLClass + + 65 + 80 + 145 + 310 + + << MotionSequenceRequest >> +MySequence +layer=-500 + +group=7 + + + + UMLDeployment + + 90 + 140 + 100 + 30 + + <<MotionSequenceItem>> +Item_1 (Group_1) +bg=cyan +transparency=0 +group=7 + + + + UMLDeployment + + 90 + 175 + 100 + 30 + + <<MotionSequenceItem>> +Item_2 (Group_1) +bg=cyan +transparency=0 +group=7 + + + + UMLDeployment + + 90 + 215 + 100 + 30 + + bg=yellow +<<MotionSequenceItem>> +Item_3 (Group_2) +bg=yellow +transparency=0 +group=7 + + + + UMLDeployment + + 90 + 255 + 100 + 30 + + <<MotionSequenceItem>> +Item_4 (Group_2) +bg=yellow +transparency=0 +group=7 + + + + UMLDeployment + + 90 + 295 + 100 + 30 + + <<MotionSequenceItem>> +Item_5 (Group_1) +bg=cyan +transparency=0 +group=7 + + + + UMLDeployment + + 90 + 350 + 100 + 30 + + <<MotionSequenceItem>> +Item_N (Group_2) +bg=yellow +transparency=0 +group=7 + + + + UMLClass + + 75 + 110 + 125 + 275 + + << Vector<MotionSequenceItem> >> +MySequence +layer=-100 +bg=light_gray +transparency=0 + +group=7 + + + + UMLGeneric + + 240 + 65 + 765 + 460 + + symbol=component +halign=left +valign=top +layer=-1000 +CommandListManager + + + + UMLGeneric + + 760 + 100 + 230 + 335 + + symbol=component +symbol=component +halign=left +valign=top +layer=-100 +PlanComponentsBuilder + + + + UMLGeneric + + 45 + 45 + 1710 + 830 + + symbol=component +halign=left +valign=top +layer=-1000000 +MoveGroupSequenceAction/Service + + + + UMLGeneric + + 1365 + 180 + 100 + 125 + + symbol=component +layer=-100 +MoveGroupContext + + + + UMLGeneric + + 1375 + 210 + 80 + 75 + + symbol=component +layer=-50 +PlanExecution + + + + UMLDeployment + + 1595 + 615 + 110 + 65 + + <<RobotTrajectory>> +Item_1 + Item_2 (Group_1) +bg=cyan +transparency=0 +group=10 + + + + UMLDeployment + + 1595 + 690 + 110 + 70 + + bg=yellow +<<RobotTrajectory>> +Item_3 + Item_4 (Group_2) +transparency=0 +group=10 + + + + UMLDeployment + + 1595 + 770 + 110 + 30 + + <<RobotTrajectory>> +Item_5 (Group_1) +bg=cyan +transparency=0 +group=10 + + + + UMLDeployment + + 1595 + 825 + 110 + 30 + + <<RobotTrajectory>> +Item_N (Group_2) +bg=yellow +transparency=0 +group=10 + + + + UMLClass + + 1590 + 590 + 120 + 270 + + << Vector<<:RobotTrajectory>> >> +planning_trajectory +bg=light_gray +transparency=0 +layer=-50 +group=10 + + + + UMLClass + + 1590 + 535 + 120 + 15 + + planning_time + +group=10 + + + + UMLClass + + 1590 + 555 + 120 + 25 + + << Vector<RobotState> >> +trajectory_start + +group=10 + + + + UMLClass + + 1590 + 515 + 120 + 15 + + error_code + +group=10 + + + + UMLClass + + 1585 + 480 + 130 + 385 + + <<GetMotionSequenceResponse>> +MySequence +layer=-100 +transparency=0 +group=10 + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.h new file mode 100644 index 0000000000..61737ba275 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.h @@ -0,0 +1,42 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 + +namespace pilz_industrial_motion_planner +{ +static const std::string SEQUENCE_SERVICE_NAME = "plan_sequence_path"; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limit.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limit.h new file mode 100644 index 0000000000..199791f76c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limit.h @@ -0,0 +1,159 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Set of cartesian limits, has values for velocity, acceleration and + * deceleration of both the + * translational and rotational part. + */ +class CartesianLimit +{ +public: + CartesianLimit(); + + // Translational Velocity Limit + + /** + * @brief Check if translational velocity limit is set. + * @return True if limit was set, false otherwise + */ + bool hasMaxTranslationalVelocity() const; + + /** + * @brief Set the maximal translational velocity + * @param Maximum translational velocity [m/s] + */ + void setMaxTranslationalVelocity(double max_trans_vel); + + /** + * @brief Return the maximal translational velocity [m/s], 0 if nothing was + * set + * @return Maximum translational velocity, 0 if nothing was set + */ + double getMaxTranslationalVelocity() const; + + // Translational Acceleration Limit + + /** + * @brief Check if translational acceleration limit is set. + * @return True if limit was set false otherwise + */ + bool hasMaxTranslationalAcceleration() const; + + /** + * @brief Set the maximum translational acceleration + * @param Maximum translational acceleration [m/s^2] + */ + void setMaxTranslationalAcceleration(double max_trans_acc); + + /** + * @brief Return the maximal translational acceleration [m/s^2], 0 if nothing + * was set + * @return maximal translational acceleration, 0 if nothing was set + */ + double getMaxTranslationalAcceleration() const; + + // Translational Deceleration Limit + + /** + * @brief Check if translational deceleration limit is set. + * @return True if limit was set false otherwise + */ + bool hasMaxTranslationalDeceleration() const; + + /** + * @brief Set the maximum translational deceleration + * @param Maximum translational deceleration, always <=0 [m/s^2] + */ + void setMaxTranslationalDeceleration(double max_trans_dec); + + /** + * @brief Return the maximal translational deceleration [m/s^2], 0 if nothing + * was set + * @return maximal translational deceleration, 0 if nothing was set, always + * <=0 [m/s^2] + */ + double getMaxTranslationalDeceleration() const; + + // Rotational Velocity Limit + + /** + * @brief Check if rotational velocity limit is set. + * @return True if limit was set false otherwise + */ + bool hasMaxRotationalVelocity() const; + + /** + * @brief Set the maximum rotational velocity + * @param Maximum rotational velocity [rad/s] + */ + void setMaxRotationalVelocity(double max_rot_vel); + + /** + * @brief Return the maximal rotational velocity [rad/s], 0 if nothing was set + * @return maximal rotational velocity, 0 if nothing was set + */ + double getMaxRotationalVelocity() const; + +private: + /// Flag if a maximum translational velocity was set + bool has_max_trans_vel_; + + /// Maximum translational velocity [m/s] + double max_trans_vel_; + + /// Flag if a maximum translational acceleration was set + bool has_max_trans_acc_; + + /// Maximum translational acceleration [m/s^2] + double max_trans_acc_; + + /// Flag if a maximum translational deceleration was set + bool has_max_trans_dec_; + + /// Maximum translational deceleration, always <=0 [m/s^2] + double max_trans_dec_; + + /// Flag if a maximum rotational velocity was set + bool has_max_rot_vel_; + + /// Maximum rotational velocity [rad/s] + double max_rot_vel_; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h new file mode 100644 index 0000000000..796dc01c4c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h @@ -0,0 +1,65 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/cartesian_limit.h" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Obtains cartesian limits from the parameter server + */ +class CartesianLimitsAggregator +{ +public: + /** + * @brief Loads cartesian limits from the parameter server + * + * The parameters are expected to be under "~/cartesian_limits" of the given + * node handle. + * The following limits can be specified: + * - "max_trans_vel", the maximum translational velocity [m/s] + * - "max_trans_acc, the maximum translational acceleration [m/s^2] + * - "max_trans_dec", the maximum translational deceleration (<= 0) [m/s^2] + * - "max_rot_vel", the maximum rotational velocity [rad/s] + * - "max_rot_acc", the maximum rotational acceleration [rad/s^2] + * - "max_rot_dec", the maximum rotational deceleration (<= 0)[rad/s^2] + * @param nh node handle to access the parameters + * @return the obtained cartesian limits + */ + static CartesianLimit getAggregatedLimits(const ros::NodeHandle& nh); +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.h new file mode 100644 index 0000000000..9d7b70431c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.h @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "cartesian_trajectory_point.h" + +namespace pilz_industrial_motion_planner +{ +struct CartesianTrajectory +{ + std::string group_name; + std::string link_name; + std::vector points; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h new file mode 100644 index 0000000000..4a5a57e55e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h @@ -0,0 +1,53 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 pilz_industrial_motion_planner +{ +struct CartesianTrajectoryPoint +{ + geometry_msgs::Pose pose; + geometry_msgs::Twist velocity; + geometry_msgs::Twist acceleartion; + ros::Duration time_from_start; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h new file mode 100644 index 0000000000..ae2ea66387 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h @@ -0,0 +1,225 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "moveit_msgs/MotionSequenceRequest.h" +#include "pilz_industrial_motion_planner/plan_components_builder.h" +#include "pilz_industrial_motion_planner/trajectory_blender.h" +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" + +namespace pilz_industrial_motion_planner +{ +using RobotTrajCont = std::vector; + +// List of exceptions which can be thrown by the CommandListManager class. +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NegativeBlendRadiusException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LastBlendRadiusNotZeroException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateSetException, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OverlappingBlendRadiiException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PlanningPipelineException, moveit_msgs::MoveItErrorCodes::FAILURE); + +/** + * @brief This class orchestrates the planning of single commands and + * command lists. + */ +class CommandListManager +{ +public: + CommandListManager(const ros::NodeHandle& nh, const robot_model::RobotModelConstPtr& model); + + /** + * @brief Generates trajectories for the specified list of motion commands. + * + * The following rules apply: + * - If two consecutive trajectories are from the same group, they are + * simply attached to each other, given that the blend_radius is zero. + * - If two consecutive trajectories are from the same group, they are + * blended together, given that the blend_radius is GREATER than zero. + * - If two consecutive trajectories are from different groups, then + * the second trajectory is added as new element to the result container. + * All following trajectories are then attached to the new trajectory + * element (until all requests are processed or until the next group change). + * + * @param planning_scene The planning scene to be used for trajectory + * generation. + * @param req_list List of motion requests containing: PTP, LIN, CIRC + * and/or gripper commands. + * Please note: A request is only valid if: + * - All blending radii are non negative. + * - The blending radius of the last request is 0. + * - Only the first request of each group has a start state. + * - None of the blending radii overlap with each other. + * + * Please note: + * Starts states do not need to state the joints of all groups. + * It is sufficient if a start state states only the joints of the group + * which it belongs to. Starts states can even be incomplete. In this case + * default values are set for the unset joints. + * + * @return Contains the calculated/generated trajectories. + */ + RobotTrajCont solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_pipeline::PlanningPipelinePtr& planning_pipeline, + const moveit_msgs::MotionSequenceRequest& req_list); + +private: + using MotionResponseCont = std::vector; + using RobotState_OptRef = boost::optional; + using RadiiCont = std::vector; + using GroupNamesCont = std::vector; + +private: + /** + * @brief Validates that two consecutive blending radii do not overlap. + * + * @param motion_plan_responses Container of calculated/generated + * trajectories. + * @param radii Container stating the blend radii. + */ + void checkForOverlappingRadii(const MotionResponseCont& resp_cont, const RadiiCont& radii) const; + + /** + * @brief Solve each sequence item individually. + * + * @param planning_scene The planning_scene to be used for trajectory + * generation. + * @param req_list Container of requests for calculation/generation. + * + * @return Container of generated trajectories. + */ + MotionResponseCont solveSequenceItems(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_pipeline::PlanningPipelinePtr& planning_pipeline, + const moveit_msgs::MotionSequenceRequest& req_list) const; + + /** + * @return TRUE if the blending radii of specified trajectories overlap, + * otherwise FALSE. The functions returns FALSE if both trajectories are from + * different groups or if both trajectories are end-effector groups. + */ + bool checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A, + const robot_trajectory::RobotTrajectory& traj_B, const double radii_B) const; + +private: + /** + * @return The last RobotState of the specified group which can + * be found in the specified vector. + */ + static RobotState_OptRef getPreviousEndState(const MotionResponseCont& motion_plan_responses, + const std::string& group_name); + + /** + * @brief Set start state to end state of previous calculated trajectory + * from group. + */ + static void setStartState(const MotionResponseCont& motion_plan_responses, const std::string& group_name, + moveit_msgs::RobotState& start_state); + + /** + * @return Container of radii extracted from the specified request list. + * + * Please note: + * This functions sets invalid blend radii to zero. Invalid blend radii are: + * - blend radii between end-effectors and + * - blend raddi between different groups. + */ + static RadiiCont extractBlendRadii(const moveit::core::RobotModel& model, + const moveit_msgs::MotionSequenceRequest& req_list); + + /** + * @return True in case of an invalid blend radii between specified + * command A and B, otherwise False. Invalid blend radii are: + * - blend radii between end-effectors and + * - blend raddi between different groups. + */ + static bool isInvalidBlendRadii(const moveit::core::RobotModel& model, const moveit_msgs::MotionSequenceItem& item_A, + const moveit_msgs::MotionSequenceItem& item_B); + + /** + * @brief Checks that all blend radii are greater or equal to zero. + */ + static void checkForNegativeRadii(const moveit_msgs::MotionSequenceRequest& req_list); + + /** + * @brief Checks that last blend radius is zero. + */ + static void checkLastBlendRadiusZero(const moveit_msgs::MotionSequenceRequest& req_list); + + /** + * @brief Checks that only the first request of the specified group has + * a start state in the specified request list. + */ + static void checkStartStatesOfGroup(const moveit_msgs::MotionSequenceRequest& req_list, const std::string& group_name); + + /** + * @brief Checks that each group in the specified request list has only + * one start state. + */ + static void checkStartStates(const moveit_msgs::MotionSequenceRequest& req_list); + + /** + * @return Returns all group names which are present in the specified + * request. + */ + static GroupNamesCont getGroupNames(const moveit_msgs::MotionSequenceRequest& req_list); + +private: + //! Node handle + ros::NodeHandle nh_; + + //! Robot model + moveit::core::RobotModelConstPtr model_; + + //! @brief Builder to construct the container containing the final + //! trajectories. + PlanComponentsBuilder plan_comp_builder_; +}; + +inline void CommandListManager::checkLastBlendRadiusZero(const moveit_msgs::MotionSequenceRequest& req_list) +{ + if (req_list.items.back().blend_radius != 0.0) + { + throw LastBlendRadiusNotZeroException("The last blending radius must be zero"); + } +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h new file mode 100644 index 0000000000..0f60b353e5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h @@ -0,0 +1,150 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_container.h" +#include "pilz_industrial_motion_planner/joint_limits_extension.h" + +#include + +#include +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Unifies the joint limits from the given joint models with joint + * limits from the parameter server. + * + * Does not support MultiDOF joints. + */ +class JointLimitsAggregator +{ +public: + /** + * @brief Aggregates(combines) the joint limits from joint model and + * parameter server. + * The rules for the combination are: + * 1. Position and velocity limits on the parameter server must be stricter + * or equal if they are defined. + * 2. Limits on the parameter server where the corresponding + * has__limits are „false“ + * are considered undefined(see point 1). + * 3. Not all joints have to be limited by the parameter server. Selective + * limitation is possible. + * 4. If max_deceleration is unset, it will be set to: max_deceleration = - + * max_acceleration. + * @note The acceleration/deceleration can only be set via the parameter + * server since they are not supported + * in the urdf so far. + * @param nh Node handle in whose namespace the joint limit parameters are + * expected. + * @param joint_models The joint models + * @return Container containing the limits + */ + static JointLimitsContainer getAggregatedLimits(const ros::NodeHandle& nh, + const std::vector& joint_models); + +protected: + /** + * @brief Update the position limits with the ones from the joint_model. + * + * If the joint model has no position limit, the value is unchanged. + * + * @param joint_model The joint model + * @param joint_limit The joint_limit to be filled with new values. + */ + static void updatePositionLimitFromJointModel(const moveit::core::JointModel* joint_model, JointLimit& joint_limit); + + /** + * @brief Update the velocity limit with the one from the joint_model. + * + * If the joint model has no velocity limit, the value is unchanged. + * + * @param joint_model The joint model + * @param joint_limit The joint_limit to be filled with new values. + */ + static void updateVelocityLimitFromJointModel(const moveit::core::JointModel* joint_model, JointLimit& joint_limit); + + /** + * @brief Checks if the position limits from the given joint_limit are + * stricter than the limits of the joint_model. + * Throws AggregationBoundsViolationException on violation + * @param joint_model The joint_model + * @param joint_limit The joint_limit + */ + static void checkPositionBoundsThrowing(const moveit::core::JointModel* joint_model, const JointLimit& joint_limit); + + /** + * @brief Checks if the velocity limit from the given joint_limit are stricter + * than the limit of the joint_model. + * Throws AggregationBoundsViolationException on violation + * @param joint_model The joint_model + * @param joint_limit The joint_limit + */ + static void checkVelocityBoundsThrowing(const moveit::core::JointModel* joint_model, const JointLimit& joint_limit); +}; + +/** + * @class AggregationException + * @brief A base class for all aggregation exceptions inheriting from + * std::runtime_exception + */ +class AggregationException : public std::runtime_error +{ +public: + AggregationException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; + +/** + * @class AggregationJointMissingException + * @brief Thrown the limits from the parameter server are weaker(forbidden) than + * the ones defined in the urdf + * + */ +class AggregationBoundsViolationException : public AggregationException +{ +public: + AggregationBoundsViolationException(const std::string& error_desc) : AggregationException(error_desc) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h new file mode 100644 index 0000000000..fac49e9106 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h @@ -0,0 +1,163 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_extension.h" + +#include +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Container for JointLimits, essentially a map with convenience + * functions. + * Adds the ability to as for limits and get a common limit that unifies all + * given limits + */ +class JointLimitsContainer +{ +public: + /** + * @brief Add a limit + * @param joint_name Name if the joint this limit belongs to + * @param joint_limit Limit of the joint + * @return true if the limit was added, false + * if joint_limit.has_deceleration_limit && + * joint_limit.max_deceleration >= 0 + */ + bool addLimit(const std::string& joint_name, JointLimit joint_limit); + + /** + * @brief Check if there is a limit for a joint with the given name in this + * container + * @param joint_name Name of the joint + */ + bool hasLimit(const std::string& joint_name) const; + + /** + * @brief Get Number of limits in the container + * @return Number of limits in the container + */ + size_t getCount() const; + + /** + * @brief Returns wether the container is empty + * @return true if empty, false otherwise + */ + bool empty() const; + + /** + * @brief Returns joint limit fusion of all(position, velocity, acceleration, + * deceleration) limits for all joint. + * There are cases where the most strict limit of all limits is needed. + * If there are no matching limits, the flag + * has_[position|velocity|...]_limits is set to false. + * + * @return joint limit + */ + JointLimit getCommonLimit() const; + + /** + * @brief Returns joint limit fusion of all(position, velocity, acceleration, + * deceleration) limits for given joints. + * There are cases where the most strict limit of all limits is needed. + * If there are no matching limits, the flag + * has_[position|velocity|...]_limits is set to false. + * + * @param joint_names + * @return joint limit + * @throws std::out_of_range if a joint limit with this name does not exist + */ + JointLimit getCommonLimit(const std::vector& joint_names) const; + + /** + * @brief getLimit get the limit for the given joint name + * @param joint_name + * @return joint limit + * @throws std::out_of_range if a joint limit with this name does not exist + */ + JointLimit getLimit(const std::string& joint_name) const; + + /** + * @brief ConstIterator to the underlying data structure + * @return + */ + std::map::const_iterator begin() const; + + /** + * @brief ConstIterator to the underlying data structure + * @return + */ + std::map::const_iterator end() const; + + /** + * @brief verify position limit of single joint + * @param joint_name + * @param joint_position + * @return + */ + bool verifyVelocityLimit(const std::string& joint_name, const double& joint_velocity) const; + + /** + * @brief verify position limit of single joint + * @param joint_name + * @param joint_position + * @return + */ + bool verifyPositionLimit(const std::string& joint_name, const double& joint_position) const; + + /** + * @brief verify position limits of multiple joints + * @param joint_names + * @param joint_positions + * @return + */ + bool verifyPositionLimits(const std::vector& joint_names, + const std::vector& joint_positions) const; + +private: + /** + * @brief update the most strict limit with given joint limit + * @param joint_limit + * @param common_limit the current most strict limit + */ + static void updateCommonLimit(const JointLimit& joint_limit, JointLimit& common_limit); + +protected: + /// Actual container object containing the data + std::map container_; +}; +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h new file mode 100644 index 0000000000..84bb1fc79f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h @@ -0,0 +1,66 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef JOINT_LIMITS_EXTENSION_H +#define JOINT_LIMITS_EXTENSION_H + +#include +#include + +namespace pilz_industrial_motion_planner +{ +namespace joint_limits_interface +{ +/** + * @brief Extends joint_limits_interface::JointLimits with a deceleration + * parameter + */ +struct JointLimits : ::joint_limits_interface::JointLimits +{ + JointLimits() : max_deceleration(0.0), has_deceleration_limits(false) + { + } + + /// maximum deceleration MUST(!) be negative + double max_deceleration; + + bool has_deceleration_limits; +}; +} // namespace joint_limits_interface + +typedef joint_limits_interface::JointLimits JointLimit; +typedef std::map JointLimitsMap; +} // namespace pilz_industrial_motion_planner + +#endif // JOINT_LIMITS_EXTENSION_H diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h new file mode 100644 index 0000000000..4791267e92 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h @@ -0,0 +1,101 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef JOINT_LIMITS_INTERFACE_EXTENSION_H +#define JOINT_LIMITS_INTERFACE_EXTENSION_H + +#include "pilz_industrial_motion_planner/joint_limits_extension.h" +#include + +namespace pilz_industrial_motion_planner +{ +namespace joint_limits_interface +{ +/** + * @see joint_limits_inteface::getJointLimits(...) + */ +inline bool getJointLimits(const std::string& joint_name, const ros::NodeHandle& nh, + joint_limits_interface::JointLimits& limits) +{ + // Node handle scoped where the joint limits are + // defined (copied from ::joint_limits_interface::getJointLimits(joint_name, + // nh, limits) + ros::NodeHandle limits_nh; + try + { + const std::string limits_namespace = "joint_limits/" + joint_name; + if (!nh.hasParam(limits_namespace)) + { + ROS_DEBUG_STREAM("No joint limits specification found for joint '" + << joint_name << "' in the parameter server (namespace " + << nh.getNamespace() + "/" + limits_namespace << ")."); + return false; + } + limits_nh = ros::NodeHandle(nh, limits_namespace); + } + catch (const ros::InvalidNameException& ex) + { + ROS_ERROR_STREAM(ex.what()); + return false; + } + + // Set the existing limits + if (!::joint_limits_interface::getJointLimits(joint_name, nh, limits)) + { + return false; // LCOV_EXCL_LINE // The case where getJointLimits returns + // false is covered above. + } + + // Deceleration limits + bool has_deceleration_limits = false; + if (limits_nh.getParam("has_deceleration_limits", has_deceleration_limits)) + { + if (!has_deceleration_limits) + { + limits.has_deceleration_limits = false; + } + double max_dec; + if (has_deceleration_limits && limits_nh.getParam("max_deceleration", max_dec)) + { + limits.has_deceleration_limits = true; + limits.max_deceleration = max_dec; + } + } + + return true; +} +} // namespace joint_limits_interface +} // namespace pilz_industrial_motion_planner + +#endif // JOINT_LIMITS_INTERFACE_EXTENSION_H diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h new file mode 100644 index 0000000000..8b95269266 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h @@ -0,0 +1,153 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_container.h" +#include "pilz_industrial_motion_planner/joint_limits_extension.h" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Validates the equality of all limits inside a container + */ +class JointLimitsValidator +{ +public: + /** + * @brief Validates that the position limits of all limits are equal + * @param joint_limits the joint limits + * @return true if all are equal + * @note always returns true if has_position_limits=false for all limits, or + * if the size of joint_limits is 0 or 1 + */ + static bool validateAllPositionLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + + /** + * @brief Validates that the velocity of all limits is equal + * @param joint_limits the joint limits + * @return true if all are equal + * @note always returns true if has_velocity_limits=false for all limits, or + * if the size of joint_limits is 0 or 1 + */ + static bool validateAllVelocityLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + + /** + * @brief Validates that the acceleration of all limits is equal + * @param joint_limits the joint limits + * @return true if all are equal + * @note always returns true if has_acceleration_limits=false for all limits, + * or if size of joint_limits is 0 or 1 + */ + static bool + validateAllAccelerationLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + + /** + * @brief Validates that the deceleration of all limits is equal + * @param joint_limits the joint limits + * @return true if all are equal + * @note always returns true if has_acceleration_limits=false for all limits, + * or if size of joint_limits is 0 or 1 + */ + static bool + validateAllDecelerationLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + +private: + static bool validateWithEqualFunc(bool (*eq_func)(const JointLimit&, const JointLimit&), + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + + static bool positionEqual(const JointLimit& lhs, const JointLimit& rhs); + + static bool velocityEqual(const JointLimit& lhs, const JointLimit& rhs); + + static bool accelerationEqual(const JointLimit& lhs, const JointLimit& rhs); + + static bool decelerationEqual(const JointLimit& lhs, const JointLimit& rhs); +}; + +/** + * @class ValidationException + * @brief A base class for all validations exceptions inheriting from + * std::runtime_exception + */ +class ValidationException : public std::runtime_error +{ +public: + ValidationException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; + +/** + * @class ValidationJointMissingException + * @brief Thrown the limits for a joint are defined in the urdf but not on the + * parameter server (loaded from yaml) + * + */ +class ValidationJointMissingException : public ValidationException +{ +public: + ValidationJointMissingException(const std::string& error_desc) : ValidationException(error_desc) + { + } +}; + +/** + * @class ValidationDifferentLimitsException + * @brief Thrown when the limits differ + * + */ +class ValidationDifferentLimitsException : public ValidationException +{ +public: + ValidationDifferentLimitsException(const std::string& error_desc) : ValidationException(error_desc) + { + } +}; + +/** + * @class ValidationBoundsViolationException + * @brief Thrown when the limits from the param server are weaker than the ones + * obtained from the urdf + * + */ +class ValidationBoundsViolationException : public ValidationException +{ +public: + ValidationBoundsViolationException(const std::string& error_desc) : ValidationException(error_desc) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h new file mode 100644 index 0000000000..79e9d3ba00 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h @@ -0,0 +1,104 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/cartesian_limit.h" +#include "pilz_industrial_motion_planner/joint_limits_container.h" +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief This class combines CartesianLimit and JointLimits into on single + * class. + */ +class LimitsContainer +{ +public: + LimitsContainer(); + + /** + * @brief Return if this LimitsContainer has defined joint limits + * @return True if container contains joint limits + */ + bool hasJointLimits() const; + + /** + * @brief Set joint limits + * @param joint_limits + */ + void setJointLimits(JointLimitsContainer& joint_limits); + + /** + * @brief Obtain the Joint Limits from the container + * @return the joint limits + */ + const JointLimitsContainer& getJointLimitContainer() const; + + /** + * @brief Return if this LimitsContainer has defined cartesian limits + * + * @return True if container contains cartesian limits including maximum + * velocity/acceleration/deceleration + */ + bool hasFullCartesianLimits() const; + + /** + * @brief Set cartesian limits + * @param cartesian_limit + */ + void setCartesianLimits(CartesianLimit& cartesian_limit); + + /** + * @brief Return the cartesian limits + * @return the cartesian limits + */ + const CartesianLimit& getCartesianLimits() const; + +private: + /// Flag if joint limits where set + bool has_joint_limits_; + + /// The joint limits + JointLimitsContainer joint_limits_; + + /// Flag if cartesian limits have been set + bool has_cartesian_limits_; + + /// The cartesian limits + CartesianLimit cartesian_limit_; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h new file mode 100644 index 0000000000..543c701579 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 pilz_industrial_motion_planner +{ +class CommandListManager; + +/** + * @brief Provide action to handle multiple trajectories and execute the result + * in the form of a MoveGroup capability (plugin). + */ +class MoveGroupSequenceAction : public move_group::MoveGroupCapability +{ +public: + MoveGroupSequenceAction(); + + void initialize() override; + +private: + using ExecutableTrajs = std::vector; + + using StartStateMsg = moveit_msgs::MotionSequenceResponse::_sequence_start_type; + using StartStatesMsg = std::vector; + using PlannedTrajMsgs = moveit_msgs::MotionSequenceResponse::_planned_trajectories_type; + +private: + void executeSequenceCallback(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal); + void executeSequenceCallbackPlanAndExecute(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal, + moveit_msgs::MoveGroupSequenceResult& action_res); + void executeMoveCallbackPlanOnly(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal, + moveit_msgs::MoveGroupSequenceResult& res); + void startMoveExecutionCallback(); + void startMoveLookCallback(); + void preemptMoveCallback(); + void setMoveState(move_group::MoveGroupState state); + bool planUsingSequenceManager(const moveit_msgs::MotionSequenceRequest& req, + plan_execution::ExecutableMotionPlan& plan); + +private: + static void convertToMsg(const ExecutableTrajs& trajs, StartStatesMsg& startStatesMsg, + PlannedTrajMsgs& plannedTrajsMsgs); + +private: + std::unique_ptr> move_action_server_; + moveit_msgs::MoveGroupSequenceFeedback move_feedback_; + + move_group::MoveGroupState move_state_{ move_group::IDLE }; + std::unique_ptr command_list_manager_; +}; +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h new file mode 100644 index 0000000000..54c0af0f61 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h @@ -0,0 +1,66 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 + +namespace pilz_industrial_motion_planner +{ +// Forward declarations +class CommandListManager; + +/** + * @brief Provide service to blend multiple trajectories in the form of a + * MoveGroup capability (plugin). + */ +class MoveGroupSequenceService : public move_group::MoveGroupCapability +{ +public: + MoveGroupSequenceService(); + ~MoveGroupSequenceService() override; + + void initialize() override; + +private: + bool plan(moveit_msgs::GetMotionSequence::Request& req, moveit_msgs::GetMotionSequence::Response& res); + +private: + ros::ServiceServer sequence_service_; + std::unique_ptr command_list_manager_; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.h new file mode 100644 index 0000000000..b2f13f7a84 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.h @@ -0,0 +1,101 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 pilz_industrial_motion_planner +{ +/** + * @brief Generator class for KDL::Path_Circle from different circle + * representations + */ +class PathCircleGenerator +{ +public: + /** + * @brief set the path circle from start, goal and center point + * + * Note that a half circle should use interim point and cannot be defined + * by circle center since start/goal/center points are colinear. + * @throws KDL::Error_MotionPlanning in case start and goal have different + * radii to the center point. + */ + static std::unique_ptr circleFromCenter(const KDL::Frame& start_pose, const KDL::Frame& goal_pose, + const KDL::Vector& center_point, double eqradius); + + /** + * @brief set circle from start, goal and interim point + + * @throws KDL::Error_MotionPlanning if the given points are colinear. + */ + static std::unique_ptr circleFromInterim(const KDL::Frame& start_pose, const KDL::Frame& goal_pose, + const KDL::Vector& interim_point, double eqradius); + +private: + PathCircleGenerator(){}; // no instantiation of this helper class! + + /** + * @brief law of cosines: returns angle gamma in c² = a²+b²-2ab cos(gamma) + * + * @return angle in radians + */ + static double cosines(const double a, const double b, const double c); + + static constexpr double MAX_RADIUS_DIFF{ 1e-2 }; + static constexpr double MAX_COLINEAR_NORM{ 1e-5 }; +}; + +} // namespace pilz_industrial_motion_planner + +class ErrorMotionPlanningCenterPointDifferentRadius : public KDL::Error_MotionPlanning +{ +public: + const char* Description() const override + { + return "Distances between start-center and goal-center are different." + " A circle cannot be created."; + } + int GetType() const override + { + return ERROR_CODE_CENTER_POINT_DIFFERENT_RADIUS; + } // LCOV_EXCL_LINE + +private: + static constexpr int ERROR_CODE_CENTER_POINT_DIFFERENT_RADIUS{ 3006 }; +}; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h new file mode 100644 index 0000000000..13ae41e67e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h @@ -0,0 +1,139 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_extension.h" +#include "pilz_industrial_motion_planner/planning_context_loader.h" + +#include +#include + +#include + +// Boost includes +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Moveit Plugin for Planning with Standart Robot Commands + * This planner is dedicated to return a instance of PlanningContext that + * corresponds to the requested motion command + * set as planner_id in the MotionPlanRequest). + * It can be easily extended with additional commands by creating a class + * inherting from PlanningContextLoader. + */ +class CommandPlanner : public planning_interface::PlannerManager +{ +public: + ~CommandPlanner() override + { + } + + /** + * @brief Initializes the planner + * Upon initialization this planner will look for plugins implementing + * pilz_industrial_motion_planner::PlanningContextLoader. + * @param model The robot model + * @param ns The namespace + * @return true on success, false otherwise + */ + bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns) override; + + /// Description of the planner + std::string getDescription() const override; + + /** + * @brief Returns the available planning commands + * @param list with the planning algorithms + * @note behined each command is a + * pilz_industrial_motion_planner::PlanningContextLoader loaded as plugin + */ + void getPlanningAlgorithms(std::vector& algs) const override; + + /** + * @brief Returns a PlanningContext that can be used to solve(calculate) the + * trajectory that corresponds to command + * given in motion request as planner_id. + * @param planning_scene + * @param req + * @param error_code + * @return + */ + planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::MoveItErrorCodes& error_code) const override; + + /** + * @brief Checks if the request can be handled + * @param motion request containing the planning_id that corresponds to the + * motion command + * @return true if the request can be handled + */ + bool canServiceRequest(const planning_interface::MotionPlanRequest& req) const override; + + /** + * @brief Register a PlanningContextLoader to be used by the CommandPlanner + * @param planning_context_loader + * @throw ContextLoaderRegistrationException if a loader with the same + * algorithm name is already registered + */ + void registerContextLoader(const pilz_industrial_motion_planner::PlanningContextLoaderPtr& planning_context_loader); + +private: + /// Plugin loader + boost::scoped_ptr> planner_context_loader; + + /// Mapping from command to loader + std::map context_loader_map_; + + /// Robot model obtained at initialize + moveit::core::RobotModelConstPtr model_; + + /// Namespace where the parameters are stored, obtained at initialize + std::string namespace_; + + /// aggregated limits of the active joints + pilz_industrial_motion_planner::JointLimitsContainer aggregated_limit_active_joints_; + + /// cartesian limit + pilz_industrial_motion_planner::CartesianLimit cartesian_limit_; +}; + +MOVEIT_CLASS_FORWARD(CommandPlanner) + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h new file mode 100644 index 0000000000..a89b3fbbd3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h @@ -0,0 +1,158 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/trajectory_blend_request.h" +#include "pilz_industrial_motion_planner/trajectory_blender.h" +#include "pilz_industrial_motion_planner/trajectory_functions.h" +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" + +namespace pilz_industrial_motion_planner +{ +using TipFrameFunc_t = std::function; + +// List of exceptions which can be thrown by the PlanComponentsBuilder class. +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoBlenderSetException, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoTipFrameFunctionSetException, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoRobotModelSetException, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(BlendingFailedException, moveit_msgs::MoveItErrorCodes::FAILURE); + +/** + * @brief Helper class to encapsulate the merge and blend process of + * trajectories. + */ +class PlanComponentsBuilder +{ +public: + /** + * @brief Sets the blender used to blend two trajectories. + */ + void setBlender(std::unique_ptr blender); + + /** + * @brief Sets the robot model needed to create new trajectory elements. + */ + void setModel(const moveit::core::RobotModelConstPtr& model); + + /** + * @brief Appends the specified trajectory to the trajectory container + * under construction. + * + * The appending complies to the following rules: + * - A trajectory is simply added/attached to the previous trajectory: + * - if they are from the same group and + * - if the specified blend_radius is zero. + * - A trajectory is blended together with the previous trajectory: + * - if they are from the same group and + * - if the specified blend_radius is GREATER than zero. + * - A new trajectory element is created and the given trajectory is + * appended/attached to the newly created empty trajectory: + * - if the given and previous trajectory are from different groups. + * + * @param other Trajectories which has to be added to the trajectory container + * under construction. + * + * @param blend_radius The blending radius between the previous and the + * specified trajectory. + */ + void append(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius); + + /** + * @brief Clears the trajectory container under construction. + */ + void reset(); + + /** + * @return The final trajectory container which results from the append calls. + */ + std::vector build() const; + +private: + void blend(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius); + +private: + /** + * @brief Appends a trajectory to a result trajectory leaving out the + * first point, if it matches the last point of the result trajectory. + * + * @note Controllers, so far at least the + * ros_controllers::JointTrajectoryController require a timewise strictly + * increasing trajectory. If through appending the last point of the + * original trajectory gets repeated, it is removed here. + */ + static void appendWithStrictTimeIncrease(robot_trajectory::RobotTrajectory& result, + const robot_trajectory::RobotTrajectory& source); + +private: + //! Blender used to blend two trajectories. + std::unique_ptr blender_; + + //! Robot model needed to create new trajectory container elements. + moveit::core::RobotModelConstPtr model_; + + //! The previously added trajectory. + robot_trajectory::RobotTrajectoryPtr traj_tail_; + + //! The trajectory container under construction. + std::vector traj_cont_; + +private: + //! Constant to check for equality of variables of two RobotState instances. + static constexpr double ROBOT_STATE_EQUALITY_EPSILON = 1e-4; +}; + +inline void PlanComponentsBuilder::setBlender(std::unique_ptr blender) +{ + blender_ = std::move(blender); +} + +inline void PlanComponentsBuilder::setModel(const moveit::core::RobotModelConstPtr& model) +{ + model_ = model; +} + +inline void PlanComponentsBuilder::reset() +{ + traj_tail_ = nullptr; + traj_cont_.clear(); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h new file mode 100644 index 0000000000..14143a0ed8 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h @@ -0,0 +1,181 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_container.h" +#include "pilz_industrial_motion_planner/trajectory_generator.h" + +#include + +#include +#include +#include +#include + +#include +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief PlanningContext for obtaining trajectories + */ +template +class PlanningContextBase : public planning_interface::PlanningContext +{ +public: + PlanningContextBase(const std::string& name, const std::string& group, + const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : planning_interface::PlanningContext(name, group) + , terminated_(false) + , model_(model) + , limits_(limits) + , generator_(model, limits_) + { + } + + ~PlanningContextBase() override + { + } + + /** + * @brief Calculates a trajectory for the request this context is currently + * set for + * @param res The result containing the respective trajectory, or error_code + * on failure + * @return true on success, false otherwise + */ + bool solve(planning_interface::MotionPlanResponse& res) override; + + /** + * @brief Will return the same trajectory as + * solve(planning_interface::MotionPlanResponse& res) + * This function just delegates to the common response however here the same + * trajectory is stored with the + * descriptions "plan", "simplify", "interpolate" + * @param res The detailed response + * @return true on success, false otherwise + */ + bool solve(planning_interface::MotionPlanDetailedResponse& res) override; + + /** + * @brief Will terminate solve() + * @return + * @note Currently will not stop a runnning solve but not start future solves. + */ + bool terminate() override; + + /** + * @copydoc planning_interface::PlanningContext::clear() + */ + void clear() override; + + /// Flag if terminated + std::atomic_bool terminated_; + + /// The robot model + robot_model::RobotModelConstPtr model_; + + /// Joint limits to be used during planning + pilz_industrial_motion_planner::LimitsContainer limits_; + +protected: + GeneratorT generator_; +}; + +template +bool pilz_industrial_motion_planner::PlanningContextBase::solve(planning_interface::MotionPlanResponse& res) +{ + if (!terminated_) + { + // Use current state as start state if not set + if (request_.start_state.joint_state.name.empty()) + { + moveit_msgs::RobotState current_state; + moveit::core::robotStateToRobotStateMsg(getPlanningScene()->getCurrentState(), current_state); + request_.start_state = current_state; + } + bool result = generator_.generate(request_, res); + return result; + // res.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + // return false; // TODO + } + + ROS_ERROR("Using solve on a terminated planning context!"); + res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; + return false; +} + +template +bool pilz_industrial_motion_planner::PlanningContextBase::solve( + planning_interface::MotionPlanDetailedResponse& res) +{ + // delegate to regular response + planning_interface::MotionPlanResponse undetailed_response; + bool result = solve(undetailed_response); + + res.description_.push_back("plan"); + res.trajectory_.push_back(undetailed_response.trajectory_); + res.processing_time_.push_back(undetailed_response.planning_time_); + + res.description_.push_back("simplify"); + res.trajectory_.push_back(undetailed_response.trajectory_); + res.processing_time_.push_back(0); + + res.description_.push_back("interpolate"); + res.trajectory_.push_back(undetailed_response.trajectory_); + res.processing_time_.push_back(0); + + res.error_code_ = undetailed_response.error_code_; + return result; +} + +template +bool pilz_industrial_motion_planner::PlanningContextBase::terminate() +{ + ROS_DEBUG_STREAM("Terminate called"); + terminated_ = true; + return true; +} + +template +void pilz_industrial_motion_planner::PlanningContextBase::clear() +{ + // No structures that need cleaning + return; +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h new file mode 100644 index 0000000000..f9e69f4791 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/limits_container.h" + +#include + +#include +#include + +#include +#include + +#include "pilz_industrial_motion_planner/planning_context_base.h" +#include "pilz_industrial_motion_planner/trajectory_generator_circ.h" + +namespace pilz_industrial_motion_planner +{ +MOVEIT_CLASS_FORWARD(PlanningContext) + +/** + * @brief PlanningContext for obtaining CIRC trajectories + */ +class PlanningContextCIRC : public pilz_industrial_motion_planner::PlanningContextBase +{ +public: + PlanningContextCIRC(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h new file mode 100644 index 0000000000..d0990c3913 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h @@ -0,0 +1,65 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/limits_container.h" + +#include + +#include +#include + +#include +#include + +#include "pilz_industrial_motion_planner/planning_context_base.h" +#include "pilz_industrial_motion_planner/trajectory_generator_lin.h" + +namespace pilz_industrial_motion_planner +{ +MOVEIT_CLASS_FORWARD(PlanningContext) + +/** + * @brief PlanningContext for obtaining LIN trajectories + */ +class PlanningContextLIN : public pilz_industrial_motion_planner::PlanningContextBase +{ +public: + PlanningContextLIN(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h new file mode 100644 index 0000000000..bb0839ed1e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h @@ -0,0 +1,144 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/limits_container.h" + +#include +#include + +#include + +#include "pilz_industrial_motion_planner/limits_container.h" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Base class for all PlanningContextLoaders. + * Since planning_interface::PlanningContext has a non empty ctor classes + * derived from it can not be plugins. + * This class serves as base class for wrappers. + */ +class PlanningContextLoader +{ +public: + PlanningContextLoader(); + virtual ~PlanningContextLoader(); + + /// Return the algorithm the loader uses + virtual std::string getAlgorithm() const; + + /** + * @brief Sets the robot model that can be passed to the planning context + * @param model The robot model + * @return false if could not be set + */ + virtual bool setModel(const moveit::core::RobotModelConstPtr& model); + + /** + * @brief Sets limits the planner can pass to the contexts + * @param limits container of limits, no guarantee to contain the limits for + * all joints of the model + * @return true if limits could be set + */ + virtual bool setLimits(const pilz_industrial_motion_planner::LimitsContainer& limits); + + /** + * @brief Return the planning context + * @param planning_context + * @param name context name + * @param group name of the planning group + * @return true on success, false otherwise + */ + virtual bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const = 0; + +protected: + /** + * @brief Return the planning context of type T + * @param planning_context + * @param name context name + * @param group name of the planning group + * @return true on success, false otherwise + */ + template + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const; + +protected: + /// Name of the algorithm + std::string alg_; + + /// True if limits are set + bool limits_set_; + + /// Limits to be used during planning + pilz_industrial_motion_planner::LimitsContainer limits_; + + /// True if model is set + bool model_set_; + + /// The robot model + moveit::core::RobotModelConstPtr model_; +}; + +typedef boost::shared_ptr PlanningContextLoaderPtr; +typedef boost::shared_ptr PlanningContextLoaderConstPtr; + +template +bool PlanningContextLoader::loadContext(planning_interface::PlanningContextPtr& planning_context, + const std::string& name, const std::string& group) const +{ + if (limits_set_ && model_set_) + { + planning_context.reset(new T(name, group, model_, limits_)); + return true; + } + else + { + if (!limits_set_) + { + ROS_ERROR_STREAM("Limits are not defined. Cannot load planning context. " + "Call setLimits loadContext"); + } + if (!model_set_) + { + ROS_ERROR_STREAM("Robot model was not set"); + } + return false; + } +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.h new file mode 100644 index 0000000000..b1760aac2c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.h @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/planning_context_loader.h" + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Plugin that can generate instances of PlanningContextCIRC. + * Generates instances of PlanningContextLIN. + */ +class PlanningContextLoaderCIRC : public PlanningContextLoader +{ +public: + PlanningContextLoaderCIRC(); + ~PlanningContextLoaderCIRC() override; + + /** + * @brief return a instance of + * pilz_industrial_motion_planner::PlanningContextCIRC + * @param planning_context returned context + * @param name + * @param group + * @return true on success, false otherwise + */ + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const override; +}; + +typedef boost::shared_ptr PlanningContextLoaderCIRCPtr; +typedef boost::shared_ptr PlanningContextLoaderCIRCConstPtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.h new file mode 100644 index 0000000000..f3215fd869 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.h @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/planning_context_loader.h" + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Plugin that can generate instances of PlanningContextLIN. + * Generates instances of PlanningContextLIN. + */ +class PlanningContextLoaderLIN : public PlanningContextLoader +{ +public: + PlanningContextLoaderLIN(); + ~PlanningContextLoaderLIN() override; + + /** + * @brief return a instance of + * pilz_industrial_motion_planner::PlanningContextLIN + * @param planning_context returned context + * @param name + * @param group + * @return true on success, false otherwise + */ + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const override; +}; + +typedef boost::shared_ptr PlanningContextLoaderLINPtr; +typedef boost::shared_ptr PlanningContextLoaderLINConstPtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.h new file mode 100644 index 0000000000..52ec4f6af7 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.h @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/planning_context_loader.h" + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Plugin that can generate instances of PlanningContextPTP. + * Generates instances of PlanningContextPTP. + */ +class PlanningContextLoaderPTP : public PlanningContextLoader +{ +public: + PlanningContextLoaderPTP(); + ~PlanningContextLoaderPTP() override; + + /** + * @brief return a instance of + * pilz_industrial_motion_planner::PlanningContextPTP + * @param planning_context returned context + * @param name + * @param group + * @return true on success, false otherwise + */ + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const override; +}; + +typedef boost::shared_ptr PlanningContextLoaderPTPPtr; +typedef boost::shared_ptr PlanningContextLoaderPTPConstPtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h new file mode 100644 index 0000000000..842c350f19 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/limits_container.h" + +#include + +#include +#include + +#include +#include + +#include "pilz_industrial_motion_planner/planning_context_base.h" +#include "pilz_industrial_motion_planner/trajectory_generator_ptp.h" + +namespace pilz_industrial_motion_planner +{ +MOVEIT_CLASS_FORWARD(PlanningContext) + +/** + * @brief PlanningContext for obtaining PTP trajectories + */ +class PlanningContextPTP : public pilz_industrial_motion_planner::PlanningContextBase +{ +public: + PlanningContextPTP(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.h new file mode 100644 index 0000000000..5dfc44da29 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.h @@ -0,0 +1,71 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 + +namespace pilz_industrial_motion_planner +{ +/** + * @class PlanningException + * @brief A base class for all pilz_industrial_motion_planner exceptions + * inheriting from std::runtime_exception + */ +class PlanningException : public std::runtime_error +{ +public: + PlanningException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; + +/** + * @class PlanningContextFactoryRegistrationException + * @brief An exception class thrown when the planner manager is unable to load a + * factory + * + * Loading a PlanningContextFactory can fail if a factory is loaded that + * would provide a command which was already provided by another factory loaded + * before. + */ +class ContextLoaderRegistrationException : public PlanningException +{ +public: + ContextLoaderRegistrationException(const std::string& error_desc) : PlanningException(error_desc) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h new file mode 100644 index 0000000000..e173bbde37 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" + +namespace pilz_industrial_motion_planner +{ +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoSolverException, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(MoreThanOneTipFrameException, moveit_msgs::MoveItErrorCodes::FAILURE); + +/** + * @returns true if the JointModelGroup has a solver, false otherwise. + * + * @tparam JointModelGroup aims at moveit::core::JointModelGroup + * @throws exception in case group is null. + */ +template +static bool hasSolver(const JointModelGroup* group) +{ + if (group == nullptr) + { + throw std::invalid_argument("Group must not be null"); + } + return group->getSolverInstance() != nullptr; +} + +/** + * @return The name of the tip frame (link) of the specified group + * returned by the solver. + * + * @tparam JointModelGroup aims at moveit::core::JointModelGroup + * @throws exception in case the group has no solver. + * @throws exception in case the solver for the group has more than one tip + * frame. + */ +template +static const std::string& getSolverTipFrame(const JointModelGroup* group) +{ + if (!hasSolver(group)) + { + throw NoSolverException("No solver for group " + group->getName()); + } + + const std::vector& tip_frames{ group->getSolverInstance()->getTipFrames() }; + if (tip_frames.size() > 1) + { + throw MoreThanOneTipFrameException("Solver for group \"" + group->getName() + "\" has more than one tip frame"); + } + return tip_frames.front(); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.h new file mode 100644 index 0000000000..450f89fea5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.h @@ -0,0 +1,59 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 + +namespace pilz_industrial_motion_planner +{ +struct TrajectoryBlendRequest +{ + // The name of the group of joints on which this blender is operating + std::string group_name; + + // The name of the target link on which this blender is operating + std::string link_name; + + // Robot trajectories to be blended + robot_trajectory::RobotTrajectoryPtr first_trajectory; + robot_trajectory::RobotTrajectoryPtr second_trajectory; + + // Blend radius in meter + double blend_radius; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h new file mode 100644 index 0000000000..880f2b777e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 + +namespace pilz_industrial_motion_planner +{ +struct TrajectoryBlendResponse +{ + // The name of the group of joints on which this blender is operating + std::string group_name; + + // Resulted robot trajectories after blending + robot_trajectory::RobotTrajectoryPtr first_trajectory; + robot_trajectory::RobotTrajectoryPtr blend_trajectory; + robot_trajectory::RobotTrajectoryPtr second_trajectory; + + // Error code + moveit_msgs::MoveItErrorCodes error_code; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h new file mode 100644 index 0000000000..ce647b5479 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/limits_container.h" +#include + +#include "pilz_industrial_motion_planner/trajectory_blend_request.h" +#include "pilz_industrial_motion_planner/trajectory_blend_response.h" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Base class of trajectory blenders + */ +class TrajectoryBlender +{ +public: + TrajectoryBlender(const pilz_industrial_motion_planner::LimitsContainer& planner_limits) : limits_(planner_limits) + { + } + + virtual ~TrajectoryBlender() + { + } + + /** + * @brief Blend two robot trajectories with the given blending radius + * @param req: trajectory blend request + * @param res: trajectroy blend response + * @return true if blend succeed + */ + virtual bool blend(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + pilz_industrial_motion_planner::TrajectoryBlendResponse& res) = 0; + +protected: + const pilz_industrial_motion_planner::LimitsContainer limits_; +}; + +typedef std::unique_ptr TrajectoryBlenderUniquePtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h new file mode 100644 index 0000000000..233bdd3e31 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h @@ -0,0 +1,178 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/cartesian_trajectory.h" +#include "pilz_industrial_motion_planner/cartesian_trajectory_point.h" +#include "pilz_industrial_motion_planner/trajectory_blend_request.h" +#include "pilz_industrial_motion_planner/trajectory_blender.h" +#include "pilz_industrial_motion_planner/trajectory_functions.h" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Trajectory blender implementing transition window algorithm + * + * See doc/MotionBlendAlgorithmDescription.pdf for a mathematical description of + * the algorithmn. + */ +class TrajectoryBlenderTransitionWindow : public TrajectoryBlender +{ +public: + TrajectoryBlenderTransitionWindow(const LimitsContainer& planner_limits) + : TrajectoryBlender::TrajectoryBlender(planner_limits) + { + } + + ~TrajectoryBlenderTransitionWindow() override + { + } + + /** + * @brief Blend two trajectories using transition window. The trajectories + * have to be equally and uniformly + * discretized. + * @param req: following fields need to be filled for a valid request: + * - group_name : name of the planning group + * - link_name : name of the target link + * - first_trajectory: Joint trajectory stops at end point. + * The last point must be the same as the first point + * of the second trajectory. + * - second trajectory: Joint trajectory stops at end point. + * The first point must be the same as the last point + * of the first trajectory. + * - blend_radius: The blend radius determines a sphere with the + * intersection point of the two trajectories + * as the center. Trajectory blending happens inside of + * this sphere. + * @param res: following fields are returned as response by the blend + * algorithm + * - group_name : name of the planning group + * - first_trajectory: Part of the first original trajectory which is + * outside of the blend sphere. + * - blend_trajectory: Joint trajectory connecting the first and second + * trajectories without stop. + * The first waypoint has non-zero time from start. + * - second trajectory: Part of the second original trajectory which is + * outside of the blend sphere. + * The first waypoint has non-zero time from start. + * error_code: information of failed blend + * @return true if succeed + */ + bool blend(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + pilz_industrial_motion_planner::TrajectoryBlendResponse& res) override; + +private: + /** + * @brief validate trajectory blend request + * @param req + * @param sampling_time: get the same sampling time of the two input + * trajectories + * @param error_code + * @return + */ + bool validateRequest(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time, + moveit_msgs::MoveItErrorCodes& error_code) const; + /** + * @brief searchBlendPoint + * @param req: trajectory blend request + * @param first_interse_index: index of the first point of the first + * trajectory that is inside the blend sphere + * @param second_interse_index: index of the last point of the second + * trajectory that is still inside the blend sphere + */ + bool searchIntersectionPoints(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + std::size_t& first_interse_index, std::size_t& second_interse_index) const; + + /** + * @brief Determine how the second trajectory should be aligned with the first + * trajectory for blend. + * Let tau_1 be the time of the first trajectory from the first_interse_index + * to the end and tau_2 the time of the + * second trajectory from the beginning to the second_interse_index: + * - if tau_1 > tau_2:
+ * align the end of the first trajectory with second_interse_index + *

+   *    first traj:  |-------------|--------!--------------|
+   *    second traj:                        |--------------|-------------------|
+   *    blend phase:               |-----------------------|
+   *    
+ * - else:
+ * align the first_interse_index with the beginning of the second + * trajectory + *
+   *    first traj:  |-------------|-----------------------|
+   *    second traj: |-----------------------!----------|-------------------|
+   *    blend phase:               |----------------------------------|
+   *    
+ * + * @param req: trajectory blend request + * @param first_interse_index: index of the intersection point between first + * trajectory and blend sphere + * @param second_interse_index: index of the intersection point between second + * trajectory and blend sphere + * @param blend_align_index: index on the first trajectory, to which the first + * point on the second trajectory should + * be aligned to for motion blend. It is now always same as + * first_interse_index + * @param blend_time: time of the motion blend period + */ + void determineTrajectoryAlignment(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + std::size_t first_interse_index, std::size_t second_interse_index, + std::size_t& blend_align_index) const; + + /** + * @brief blend two trajectories in Cartesian space, result in a + * MultiDOFJointTrajectory which consists + * of a list of transforms for the blend phase. + * @param req + * @param first_interse_index + * @param second_interse_index + * @param blend_begin_index + * @param sampling_time + * @param trajectory: the resulting blend trajectory inside the blending + * sphere + */ + void blendTrajectoryCartesian(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + const std::size_t first_interse_index, const std::size_t second_interse_index, + const std::size_t blend_align_index, double sampling_time, + pilz_industrial_motion_planner::CartesianTrajectory& trajectory) const; + +private: // static members + // Constant to check for equality of values. + static constexpr double EPSILON = 1e-4; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h new file mode 100644 index 0000000000..f31bbada01 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h @@ -0,0 +1,219 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/cartesian_trajectory.h" +#include "pilz_industrial_motion_planner/limits_container.h" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief compute the inverse kinematics of a given pose, also check robot self + * collision + * @param robot_model: kinematic model of the robot + * @param group_name: name of planning group + * @param link_name: name of target link + * @param pose: target pose in IK solver Frame + * @param frame_id: reference frame of the target pose + * @param seed: seed state of IK solver + * @param solution: solution of IK + * @param check_self_collision: true to enable self collision checking after IK + * computation + * @param max_attempt: maximal attempts of IK + * @return true if succeed + */ +bool computePoseIK(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, + const std::string& link_name, const Eigen::Isometry3d& pose, const std::string& frame_id, + const std::map& seed, std::map& solution, + bool check_self_collision = true, const double timeout = 0.1); + +bool computePoseIK(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, + const std::string& link_name, const geometry_msgs::Pose& pose, const std::string& frame_id, + const std::map& seed, std::map& solution, + bool check_self_collision = true, const double timeout = 0.1); + +/** + * @brief compute the pose of a link at give robot state + * @param robot_model: kinematic model of the robot + * @param link_name: target link name + * @param joint_state: joint positons of this group + * @param pose: pose of the link in base frame of robot model + * @return true if succeed + */ +bool computeLinkFK(const robot_model::RobotModelConstPtr& robot_model, const std::string& link_name, + const std::map& joint_state, Eigen::Isometry3d& pose); + +bool computeLinkFK(const robot_model::RobotModelConstPtr& robot_model, const std::string& link_name, + const std::vector& joint_names, const std::vector& joint_positions, + Eigen::Isometry3d& pose); + +/** + * @brief verify the velocity/acceleration limits of current sample (based on + * backward difference computation) + * v(k) = [x(k) - x(k-1)]/[t(k) - t(k-1)] + * a(k) = [v(k) - v(k-1)]/[t(k) - t(k-2)]*2 + * @param position_last: position of last sample + * @param velocity_last: velocity of last sample + * @param position_current: position of current sample + * @param duration_last: duration of last sample + * @param duration_current: duration of current sample + * @param joint_limits: joint limits + * @return + */ +bool verifySampleJointLimits(const std::map& position_last, + const std::map& velocity_last, + const std::map& position_current, double duration_last, + double duration_current, const JointLimitsContainer& joint_limits); + +/** + * @brief Generate joint trajectory from a KDL Cartesian trajectory + * @param robot_model: robot kinematics model + * @param joint_limits: joint limits + * @param trajectory: KDL Cartesian trajectory + * @param group_name: name of the planning group + * @param link_name: name of the target robot link + * @param initial_joint_position: initial joint positions, needed for selecting + * the ik solution + * @param sampling_time: sampling time of the generated trajectory + * @param joint_trajectory: output as robot joint trajectory, first and last + * point will have zero velocity + * and acceleration + * @param error_code: detailed error information + * @param check_self_collision: check for self collision during creation + * @return true if succeed + */ +bool generateJointTrajectory(const robot_model::RobotModelConstPtr& robot_model, + const JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, + const std::string& group_name, const std::string& link_name, + const std::map& initial_joint_position, const double& sampling_time, + trajectory_msgs::JointTrajectory& joint_trajectory, + moveit_msgs::MoveItErrorCodes& error_code, bool check_self_collision = false); + +/** + * @brief Generate joint trajectory from a MultiDOFJointTrajectory + * @param trajectory: Cartesian trajectory + * @param info: motion plan information + * @param sampling_time + * @param joint_trajectory + * @param error_code + * @return true if succeed + */ +bool generateJointTrajectory(const robot_model::RobotModelConstPtr& robot_model, + const JointLimitsContainer& joint_limits, + const pilz_industrial_motion_planner::CartesianTrajectory& trajectory, + const std::string& group_name, const std::string& link_name, + const std::map& initial_joint_position, + const std::map& initial_joint_velocity, + trajectory_msgs::JointTrajectory& joint_trajectory, + moveit_msgs::MoveItErrorCodes& error_code, bool check_self_collision = false); + +/** + * @brief Determines the sampling time and checks that both trajectroies use the + * same sampling time. + * @return TRUE if the sampling time is equal between all given points (except + * the last two points + * of each trajectory), otherwise FALSE. + */ +bool determineAndCheckSamplingTime(const robot_trajectory::RobotTrajectoryPtr& first_trajectory, + const robot_trajectory::RobotTrajectoryPtr& second_trajectory, double EPSILON, + double& sampling_time); + +/** + * @brief Check if the two robot states have the same joint + * position/velocity/acceleration. + * + * @param joint_group_name The name of the joint group. + * @param epsilon Constants defining how close the joint + * position/velocity/acceleration have to be to be + * recognized as equal. + * + * @return True if joint positions, joint velocities and joint accelerations are + * equal, otherwise false. + */ +bool isRobotStateEqual(const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const std::string& joint_group_name, double epsilon); + +/** + * @brief check if the robot state have zero velocity/acceleartion + * @param state + * @param group + * @param EPSILON + * @return + */ +bool isRobotStateStationary(const robot_state::RobotState& state, const std::string& group, double EPSILON); + +/** + * @brief Performs a linear search for the intersection point of the trajectory + * with the blending radius. + * @param center_position Center of blending sphere. + * @param r Radius of blending sphere. + * @param traj The trajectory. + * @param inverseOrder TRUE: Farthest element from blending sphere center is + * located at the + * smallest index of trajectroy. + * @param index The intersection index which has to be determined. + */ +bool linearSearchIntersectionPoint(const std::string& link_name, const Eigen::Vector3d& center_position, + const double& r, const robot_trajectory::RobotTrajectoryPtr& traj, bool inverseOrder, + std::size_t& index); + +bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p_current, const Eigen::Vector3d& p_next, + const double& r); + +/** + * @brief Checks if current robot state is in self collision. + * @param test_for_self_collision Flag to deactivate this check during IK. + * @param robot_model: robot kinematics model. + * @param state Robot state instance used for . + * @param group + * @param ik_solution + * @return + */ +bool isStateColliding(const bool test_for_self_collision, const moveit::core::RobotModelConstPtr& robot_model, + robot_state::RobotState* state, const robot_state::JointModelGroup* const group, + const double* const ik_solution); +} // namespace pilz_industrial_motion_planner + +void normalizeQuaternion(geometry_msgs::Quaternion& quat); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h new file mode 100644 index 0000000000..8348850687 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h @@ -0,0 +1,121 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 pilz_industrial_motion_planner +{ +/** + * @brief Exception storing an moveit_msgs::MoveItErrorCodes value. + */ +class MoveItErrorCodeException : public std::runtime_error +{ +public: + MoveItErrorCodeException(const std::string& msg); + +protected: + MoveItErrorCodeException(const MoveItErrorCodeException&) = default; + MoveItErrorCodeException(MoveItErrorCodeException&&) = default; + ~MoveItErrorCodeException() override = default; + + MoveItErrorCodeException& operator=(const MoveItErrorCodeException&) = default; + MoveItErrorCodeException& operator=(MoveItErrorCodeException&&) = default; + +public: + virtual const moveit_msgs::MoveItErrorCodes::_val_type& getErrorCode() const = 0; +}; + +template +class TemplatedMoveItErrorCodeException : public MoveItErrorCodeException +{ +public: + TemplatedMoveItErrorCodeException(const std::string& msg); + TemplatedMoveItErrorCodeException(const std::string& msg, const moveit_msgs::MoveItErrorCodes::_val_type& error_code); + +public: + const moveit_msgs::MoveItErrorCodes::_val_type& getErrorCode() const override; + +private: + const moveit_msgs::MoveItErrorCodes::_val_type error_code_{ ERROR_CODE }; +}; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +inline MoveItErrorCodeException::MoveItErrorCodeException(const std::string& msg) : std::runtime_error(msg) +{ +} + +template +inline TemplatedMoveItErrorCodeException::TemplatedMoveItErrorCodeException(const std::string& msg) + : MoveItErrorCodeException(msg) +{ +} + +template +inline TemplatedMoveItErrorCodeException::TemplatedMoveItErrorCodeException( + const std::string& msg, const moveit_msgs::MoveItErrorCodes::_val_type& error_code) + : MoveItErrorCodeException(msg), error_code_(error_code) +{ +} + +template +inline const moveit_msgs::MoveItErrorCodes::_val_type& +TemplatedMoveItErrorCodeException::getErrorCode() const +{ + return error_code_; +} + +/* + * @brief Macro to automatically generate a derived class of + * the MoveItErrorCodeException class. + */ +#define CREATE_MOVEIT_ERROR_CODE_EXCEPTION(EXCEPTION_CLASS_NAME, ERROR_CODE) \ + class EXCEPTION_CLASS_NAME : public TemplatedMoveItErrorCodeException \ + { \ + public: \ + EXCEPTION_CLASS_NAME(const std::string& msg) : TemplatedMoveItErrorCodeException(msg) \ + { \ + } \ + \ + EXCEPTION_CLASS_NAME(const std::string& msg, const moveit_msgs::MoveItErrorCodes::_val_type& error_code) \ + : TemplatedMoveItErrorCodeException(msg, error_code) \ + { \ + } \ + } + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h new file mode 100644 index 0000000000..09055fcb81 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -0,0 +1,291 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_extension.h" +#include "pilz_industrial_motion_planner/limits_container.h" +#include "pilz_industrial_motion_planner/trajectory_functions.h" +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" + +using namespace pilz_industrial_motion_planner; + +namespace pilz_industrial_motion_planner +{ +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(TrajectoryGeneratorInvalidLimitsException, moveit_msgs::MoveItErrorCodes::FAILURE); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(VelocityScalingIncorrect, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(AccelerationScalingIncorrect, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownPlanningGroup, moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoJointNamesInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(SizeMismatchInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointsOfStartStateOutOfRange, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NonZeroVelocityInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NotExactlyOneGoalConstraintGiven, + moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OnlyOneGoalTypeAllowed, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateGoalStateMismatch, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointConstraintDoesNotBelongToGroup, + moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointsOfGoalOutOfRange, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PositionConstraintNameMissing, + moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OrientationConstraintNameMissing, + moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PositionOrientationConstraintNameMismatch, + moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoIKSolverAvailable, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePoseGiven, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + +/** + * @brief Base class of trajectory generators + * + * Note: All derived classes cannot have a start velocity + */ +class TrajectoryGenerator +{ +public: + TrajectoryGenerator(const robot_model::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits) + : robot_model_(robot_model), planner_limits_(planner_limits) + { + } + + virtual ~TrajectoryGenerator() = default; + + /** + * @brief generate robot trajectory with given sampling time + * @param req: motion plan request + * @param res: motion plan response + * @param sampling_time: sampling time of the generate trajectory + * @return motion plan succeed/fail, detailed information in motion plan + * responce + */ + bool generate(const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, + double sampling_time = 0.1); + +protected: + /** + * @brief This class is used to extract needed information from motion plan + * request. + */ + class MotionPlanInfo + { + public: + std::string group_name; + std::string link_name; + Eigen::Isometry3d start_pose; + Eigen::Isometry3d goal_pose; + std::map start_joint_position; + std::map goal_joint_position; + std::pair circ_path_point; + }; + + /** + * @brief build cartesian velocity profile for the path + * + * Uses the path to get the cartesian length and the angular distance from + * start to goal. + * The trap profile returns uses the longer distance of translational and + * rotational motion. + */ + std::unique_ptr cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor, + const double& max_acceleration_scaling_factor, + const std::unique_ptr& path) const; + +private: + virtual void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const; + + /** + * @brief Extract needed information from a motion plan request in order to + * simplify + * further usages. + * @param req: motion plan request + * @param info: information extracted from motion plan request which is + * necessary for the planning + */ + virtual void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const = 0; + + virtual void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) = 0; + +private: + /** + * @brief Validate the motion plan request based on the common requirements of + * trajectroy generator + * Checks that: + * - req.max_velocity_scaling_factor [0.0001, 1], + * moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN on failure + * - req.max_acceleration_scaling_factor [0.0001, 1] , + * moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN on + * failure + * - req.group_name is a JointModelGroup of the Robotmodel, + * moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME on + * failure + * - req.start_state.joint_state is not empty, + * moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE on failure + * - req.start_state.joint_state is within the limits, + * moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE on + * failure + * - req.start_state.joint_state is all zero, + * moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE on failure + * - req.goal_constraints must have exactly 1 defined cartesian oder joint + * constraint + * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * A joint goal is checked for: + * - StartState joint-names matching goal joint-names, + * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on + * failure + * - Beeing defined in the req.group_name JointModelGroup + * - Beeing with the defined limits + * A cartesian goal is checked for: + * - A defined link_name for the constraint, + * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * - Matching link_name for position and orientation constraints, + * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * - A IK solver exists for the given req.group_name and constraint + * link_name, + * moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION on failure + * - A goal pose define in + * position_constraints[0].constraint_region.primitive_poses, + * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * @param req: motion plan request + */ + void validateRequest(const planning_interface::MotionPlanRequest& req) const; + + /** + * @brief set MotionPlanResponse from joint trajectory + */ + void setSuccessResponse(const std::string& group_name, const moveit_msgs::RobotState& start_state, + const trajectory_msgs::JointTrajectory& joint_trajectory, const ros::Time& planning_start, + planning_interface::MotionPlanResponse& res) const; + + void setFailureResponse(const ros::Time& planning_start, planning_interface::MotionPlanResponse& res) const; + + void checkForValidGroupName(const std::string& group_name) const; + + /** + * @brief Validate that the start state of the request matches the + * requirements of the trajectory generator. + * + * These requirements are: + * - Names of the joints and given joint position match in size and are + * non-zero + * - The start state is withing the position limits + * - The start state velocity is below + * TrajectoryGenerator::VELOCITY_TOLERANCE + */ + void checkStartState(const moveit_msgs::RobotState& start_state) const; + + void checkGoalConstraints(const moveit_msgs::MotionPlanRequest::_goal_constraints_type& goal_constraints, + const std::vector& expected_joint_names, const std::string& group_name) const; + + void checkJointGoalConstraint(const moveit_msgs::Constraints& constraint, + const std::vector& expected_joint_names, + const std::string& group_name) const; + + void checkCartesianGoalConstraint(const moveit_msgs::Constraints& constraint, const std::string& group_name) const; + + void convertToRobotTrajectory(const trajectory_msgs::JointTrajectory& joint_trajectory, + const moveit_msgs::RobotState& start_state, + robot_trajectory::RobotTrajectory& robot_trajectory) const; + +private: + /** + * @return True if scaling factor is valid, otherwise false. + */ + static bool isScalingFactorValid(const double& scaling_factor); + static void checkVelocityScaling(const double& scaling_factor); + static void checkAccelerationScaling(const double& scaling_factor); + + /** + * @return True if ONE position + ONE orientation constraint given, + * otherwise false. + */ + static bool isCartesianGoalGiven(const moveit_msgs::Constraints& constraint); + + /** + * @return True if joint constraint given, otherwise false. + */ + static bool isJointGoalGiven(const moveit_msgs::Constraints& constraint); + + /** + * @return True if ONLY joint constraint or + * ONLY cartesian constraint (position+orientation) given, otherwise false. + */ + static bool isOnlyOneGoalTypeGiven(const moveit_msgs::Constraints& constraint); + +protected: + const robot_model::RobotModelConstPtr robot_model_; + const pilz_industrial_motion_planner::LimitsContainer planner_limits_; + static constexpr double MIN_SCALING_FACTOR{ 0.0001 }; + static constexpr double MAX_SCALING_FACTOR{ 1. }; + static constexpr double VELOCITY_TOLERANCE{ 1e-8 }; +}; + +inline bool TrajectoryGenerator::isScalingFactorValid(const double& scaling_factor) +{ + return (scaling_factor > MIN_SCALING_FACTOR && scaling_factor <= MAX_SCALING_FACTOR); +} + +inline bool TrajectoryGenerator::isCartesianGoalGiven(const moveit_msgs::Constraints& constraint) +{ + return constraint.position_constraints.size() == 1 && constraint.orientation_constraints.size() == 1; +} + +inline bool TrajectoryGenerator::isJointGoalGiven(const moveit_msgs::Constraints& constraint) +{ + return !constraint.joint_constraints.empty(); +} + +inline bool TrajectoryGenerator::isOnlyOneGoalTypeGiven(const moveit_msgs::Constraints& constraint) +{ + return (isJointGoalGiven(constraint) && !isCartesianGoalGiven(constraint)) || + (!isJointGoalGiven(constraint) && isCartesianGoalGiven(constraint)); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h new file mode 100644 index 0000000000..080239a581 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h @@ -0,0 +1,107 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/trajectory_generator.h" + +using namespace pilz_industrial_motion_planner; + +namespace pilz_industrial_motion_planner +{ +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleNoPlane, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleToSmall, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CenterPointDifferentRadius, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircTrajectoryConversionFailure, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownPathConstraintName, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPositionConstraints, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePose, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownLinkNameOfAuxiliaryPoint, moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NumberOfConstraintsMismatch, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircJointMissingInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircInverseForGoalIncalculable, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + +/** + * @brief This class implements a trajectory generator of arcs in Cartesian + * space. + * The arc is specified by a start pose, a goal pose and a interim point on the + * arc, + * or a point as the center of the circle which forms the arc. Complete circle + * is not + * covered by this generator. + */ +class TrajectoryGeneratorCIRC : public TrajectoryGenerator +{ +public: + /** + * @brief Constructor of CIRC Trajectory Generator. + * + * @param planner_limits Limits in joint and Cartesian spaces + * + * @throw TrajectoryGeneratorInvalidLimitsException + * + */ + TrajectoryGeneratorCIRC(const robot_model::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + +private: + void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override; + + void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; + + void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) override; + + /** + * @brief Construct a KDL::Path object for a Cartesian path of an arc. + * + * @return A unique pointer of the path object, null_ptr in case of an error. + * + * @throws CircleNoPlane if the plane in which the circle resides, + * could not be determined. + * + * @throws CircleToSmall if the specified circ radius is to small. + * + * @throws CenterPointDifferentRadius if the distances between start-center + * and goal-center are different. + */ + std::unique_ptr setPathCIRC(const MotionPlanInfo& info) const; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h new file mode 100644 index 0000000000..2d17768cad --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h @@ -0,0 +1,85 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/trajectory_generator.h" +#include "pilz_industrial_motion_planner/velocity_profile_atrap.h" + +using namespace pilz_industrial_motion_planner; + +namespace pilz_industrial_motion_planner +{ +// TODO date type of units + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::MoveItErrorCodes::FAILURE); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinJointMissingInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + +/** + * @brief This class implements a linear trajectory generator in Cartesian + * space. + * The Cartesian trajetory are based on trapezoid velocity profile. + */ +class TrajectoryGeneratorLIN : public TrajectoryGenerator +{ +public: + /** + * @brief Constructor of LIN Trajectory Generator + * @throw TrajectoryGeneratorInvalidLimitsException + * @param model: robot model + * @param planner_limits: limits in joint and Cartesian spaces + */ + TrajectoryGeneratorLIN(const robot_model::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + +private: + void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; + + void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) override; + + /** + * @brief construct a KDL::Path object for a Cartesian straight line + * @return a unique pointer of the path object. null_ptr in case of an error. + */ + std::unique_ptr setPathLIN(const Eigen::Affine3d& start_pose, const Eigen::Affine3d& goal_pose) const; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h new file mode 100644 index 0000000000..09048557ba --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h @@ -0,0 +1,94 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "eigen3/Eigen/Eigen" +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" +#include "pilz_industrial_motion_planner/trajectory_generator.h" +#include "pilz_industrial_motion_planner/velocity_profile_atrap.h" + +using namespace pilz_industrial_motion_planner; + +namespace pilz_industrial_motion_planner +{ +// TODO date type of units + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PtpVelocityProfileSyncFailed, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PtpNoIkSolutionForGoalPose, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + +/** + * @brief This class implements a point-to-point trajectory generator based on + * VelocityProfileATrap. + */ +class TrajectoryGeneratorPTP : public TrajectoryGenerator +{ +public: + /** + * @brief Constructor of PTP Trajectory Generator + * @throw TrajectoryGeneratorInvalidLimitsException + * @param model: a map of joint limits information + */ + TrajectoryGeneratorPTP(const robot_model::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + +private: + void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const override; + + /** + * @brief plan ptp joint trajectory with zero start velocity + * @param start_pos + * @param goal_pos + * @param joint_trajectory + * @param group_name + * @param velocity_scaling_factor + * @param acceleration_scaling_factor + * @param sampling_time + */ + void planPTP(const std::map& start_pos, const std::map& goal_pos, + trajectory_msgs::JointTrajectory& joint_trajectory, const std::string& group_name, + const double& velocity_scaling_factor, const double& acceleration_scaling_factor, + const double& sampling_time); + + void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) override; + +private: + const double MIN_MOVEMENT = 0.001; + pilz_industrial_motion_planner::JointLimitsContainer joint_limits_; + // most strict joint limits for each group + std::map most_strict_limits_; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.h new file mode 100644 index 0000000000..2d71ccbb4b --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.h @@ -0,0 +1,215 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "kdl/velocityprofile.hpp" +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief A PTP Trajectory Generator of Asymmetric Trapezoidal Velocity Profile. + * Differences to VelocityProfile_Trap: + * - Maximal acceleration and deceleration can be different, resulting + * an asymmetric trapezoid shaped velocity profile. + * - Function to generate full synchronized PTP trajectory is provided. + * - Function to generate trapezoid shaped velocity profile with start + * velocity. + */ +class VelocityProfileATrap : public KDL::VelocityProfile +{ +public: + /** + * @brief Constructor + * @param max_vel: maximal velocity (absolute value, always positive) + * @param max_acc: maximal acceleration (absolute value, always positive) + * @param max_dec: maximal deceleration (absolute value, always positive) + */ + VelocityProfileATrap(double max_vel = 0, double max_acc = 0, double max_dec = 0); + + /** + * @brief compute the fastest profile + * Algorithm: + * - compute the minimal distance which is needed to reach maximal velocity + * - if maximal velocity can be reached + * - compute the coefficients of the trajectory + * - if maximal velocity can not be reached + * - compute the new velocity can be reached + * - compute the coefficients based on this new velocity + * + * @param pos1: start position + * @param pos2: goal position + */ + void SetProfile(double pos1, double pos2) override; + + /** + * @brief Profile scaled by the total duration + * @param pos1: start position + * @param pos2: goal position + * @param duration: trajectory duration (must be longer than fastest case, + * otherwise will be ignored) + */ + void SetProfileDuration(double pos1, double pos2, double duration) override; + + /** + * @brief Profile with given acceleration/constant/deceleration durations. + * Each duration must obey the maximal velocity/acceleration/deceleration + * constraints. + * Otherwise the operation will be ignored. + * Algorithm: + * - compute the maximal velocity of given durations + * - compute the acceleration and deceleration of given duraitons + * - if limits are fulfilled + * - compute the coefficients + * @param pos1: start position + * @param pos2: goal position + * @param acc_duration: time of acceleration phase + * @param const_duration: time of constant phase + * @param dec_duration: time of deceleration phase + * @return ture if the combination of three durations is valid + */ + bool setProfileAllDurations(double pos1, double pos2, double duration1, double duration2, double duration3); + + /** + * @brief Profile with start velocity + * Note: This function is not general and is currently only used for live + * control (vel1*(pos2-pos1)>0). + * @param pos1: start position + * @param pos2: goal position + * @param vel1: start velocity + * @return + */ + bool setProfileStartVelocity(double pos1, double pos2, double vel1); + + /** + * @brief get the time of first phase + * @return + */ + double firstPhaseDuration() const + { + return t_a_; + } + /** + * @brief get the time of second phase + * @return + */ + double secondPhaseDuration() const + { + return t_b_; + } + /** + * @brief get the time of third phase + * @return + */ + double thirdPhaseDuration() const + { + return t_c_; + } + + /** + * @brief Compares two Asymmetric Trapezoidal Velocity Profiles. + * + * @return True if equal, false otherwise. + */ + bool operator==(const VelocityProfileATrap& other) const; + + /** + * @brief Duration + * @return total duration of the trajectory + */ + double Duration() const override; + /** + * @brief Get position at given time + * @param time + * @return + */ + double Pos(double time) const override; + /** + * @brief Get velocity at given time + * @param time + * @return + */ + double Vel(double time) const override; + /** + * @brief Get given acceleration/deceleration at given time + * @param time + * @return + */ + double Acc(double time) const override; + /** + * @brief Write basic information + * @param os + */ + void Write(std::ostream& os) const override; + /** + * @brief returns copy of current VelocityProfile object + * @return + */ + KDL::VelocityProfile* Clone() const override; + + friend std::ostream& operator<<(std::ostream& os, const VelocityProfileATrap& p); // LCOV_EXCL_LINE + + ~VelocityProfileATrap() override; + +private: + /// helper functions + void setEmptyProfile(); + +private: + /// specification of the motion profile : + const double max_vel_; + const double max_acc_; + const double max_dec_; + double start_pos_; + double end_pos_; + + /// for initial velocity + double start_vel_; + + /// three phases of trapezoid + double a1_, a2_, a3_; /// coef. from ^0 -> ^2 of first phase + double b1_, b2_, b3_; /// of second phase + double c1_, c2_, c3_; /// of third phase + + /// time of three phases + double t_a_; /// the duration of first phase + double t_b_; /// the duration of second phase + double t_c_; /// the duration of third phase +}; + +std::ostream& operator<<(std::ostream& os, + const VelocityProfileATrap& p); // LCOV_EXCL_LINE + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml new file mode 100644 index 0000000000..d69a541779 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -0,0 +1,55 @@ + + + pilz_industrial_motion_planner + 0.4.10 + MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof. + + Alexander Gutenkunst + Christian Henkel + Immanuel Martini + Joachim Schleicher + Hagen Slusarek + + BSD + + http://moveit.ros.org + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + eigen_conversions + kdl_conversions + moveit_ros_planning_interface + moveit_msgs + moveit_core + moveit_ros_planning + moveit_ros_move_group + orocos_kdl + liborocos-kdl-dev + pluginlib + roscpp + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros + + + rostest + rosunit + cmake_modules + moveit_resources_panda_moveit_config + pilz_industrial_motion_planner_testutils + moveit_resources_prbt_moveit_config + moveit_resources_prbt_support + moveit_resources_prbt_pg70_support + code_coverage + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/plugins/pilz_industrial_motion_planner_plugin_description.xml b/moveit_planners/pilz_industrial_motion_planner/plugins/pilz_industrial_motion_planner_plugin_description.xml new file mode 100644 index 0000000000..3a6f16cf50 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/plugins/pilz_industrial_motion_planner_plugin_description.xml @@ -0,0 +1,5 @@ + + + Pilz Industrial Motion Planner + + diff --git a/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml b/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml new file mode 100644 index 0000000000..0ff208f2a2 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml @@ -0,0 +1,18 @@ + + + Loader for PTP Context + + + + + Loader for LIN Context + + + + + Loader for CIRC Context + + diff --git a/moveit_planners/pilz_industrial_motion_planner/plugins/sequence_capability_plugin_description.xml b/moveit_planners/pilz_industrial_motion_planner/plugins/sequence_capability_plugin_description.xml new file mode 100644 index 0000000000..9daf67146c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/plugins/sequence_capability_plugin_description.xml @@ -0,0 +1,13 @@ + + + Motion Planner a sequence of trajectories + + + + Plan a sequence of trajectory via ROS serivce + + diff --git a/moveit_planners/pilz_industrial_motion_planner/rosdoc.yaml b/moveit_planners/pilz_industrial_motion_planner/rosdoc.yaml new file mode 100644 index 0000000000..119e416808 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/rosdoc.yaml @@ -0,0 +1,6 @@ +- builder: doxygen + name: Documentation of pilz_industrial_motion_planner package + output_dir: pilz_industrial_motion_planner + file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox *.md' + exclude_patterns: 'test' + use_mdfile_as_mainpage: 'README.md' diff --git a/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limit.cpp b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limit.cpp new file mode 100644 index 0000000000..3d67a9be13 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limit.cpp @@ -0,0 +1,119 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/cartesian_limit.h" + +pilz_industrial_motion_planner::CartesianLimit::CartesianLimit() + : has_max_trans_vel_(false) + , max_trans_vel_(0.0) + , has_max_trans_acc_(false) + , max_trans_acc_(0.0) + , has_max_trans_dec_(false) + , max_trans_dec_(0.0) + , has_max_rot_vel_(false) + , max_rot_vel_(0.0) +{ +} + +// Translational Velocity Limit + +bool pilz_industrial_motion_planner::CartesianLimit::hasMaxTranslationalVelocity() const +{ + return has_max_trans_vel_; +} + +void pilz_industrial_motion_planner::CartesianLimit::setMaxTranslationalVelocity(double max_trans_vel) +{ + has_max_trans_vel_ = true; + max_trans_vel_ = max_trans_vel; +} + +double pilz_industrial_motion_planner::CartesianLimit::getMaxTranslationalVelocity() const +{ + return max_trans_vel_; +} + +// Translational Acceleration Limit + +bool pilz_industrial_motion_planner::CartesianLimit::hasMaxTranslationalAcceleration() const +{ + return has_max_trans_acc_; +} + +void pilz_industrial_motion_planner::CartesianLimit::setMaxTranslationalAcceleration(double max_trans_acc) +{ + has_max_trans_acc_ = true; + max_trans_acc_ = max_trans_acc; +} + +double pilz_industrial_motion_planner::CartesianLimit::getMaxTranslationalAcceleration() const +{ + return max_trans_acc_; +} + +// Translational Deceleration Limit + +bool pilz_industrial_motion_planner::CartesianLimit::hasMaxTranslationalDeceleration() const +{ + return has_max_trans_dec_; +} + +void pilz_industrial_motion_planner::CartesianLimit::setMaxTranslationalDeceleration(double max_trans_dec) +{ + has_max_trans_dec_ = true; + max_trans_dec_ = max_trans_dec; +} + +double pilz_industrial_motion_planner::CartesianLimit::getMaxTranslationalDeceleration() const +{ + return max_trans_dec_; +} + +// Rotational Velocity Limit + +bool pilz_industrial_motion_planner::CartesianLimit::hasMaxRotationalVelocity() const +{ + return has_max_rot_vel_; +} + +void pilz_industrial_motion_planner::CartesianLimit::setMaxRotationalVelocity(double max_rot_vel) +{ + has_max_rot_vel_ = true; + max_rot_vel_ = max_rot_vel; +} + +double pilz_industrial_motion_planner::CartesianLimit::getMaxRotationalVelocity() const +{ + return max_rot_vel_; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp new file mode 100644 index 0000000000..30dc95ad41 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp @@ -0,0 +1,96 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "ros/ros.h" + +#include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" + +static const std::string PARAM_CARTESIAN_LIMITS_NS = "cartesian_limits"; + +static const std::string PARAM_MAX_TRANS_VEL = "max_trans_vel"; +static const std::string PARAM_MAX_TRANS_ACC = "max_trans_acc"; +static const std::string PARAM_MAX_TRANS_DEC = "max_trans_dec"; +static const std::string PARAM_MAX_ROT_VEL = "max_rot_vel"; +static const std::string PARAM_MAX_ROT_ACC = "max_rot_acc"; +static const std::string PARAM_MAX_ROT_DEC = "max_rot_dec"; + +pilz_industrial_motion_planner::CartesianLimit +pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(const ros::NodeHandle& nh) +{ + std::string param_prefix = PARAM_CARTESIAN_LIMITS_NS + "/"; + + pilz_industrial_motion_planner::CartesianLimit cartesian_limit; + + // translational velocity + double max_trans_vel; + if (nh.getParam(param_prefix + PARAM_MAX_TRANS_VEL, max_trans_vel)) + { + cartesian_limit.setMaxTranslationalVelocity(max_trans_vel); + } + + // translational acceleration + double max_trans_acc; + if (nh.getParam(param_prefix + PARAM_MAX_TRANS_ACC, max_trans_acc)) + { + cartesian_limit.setMaxTranslationalAcceleration(max_trans_acc); + } + + // translational deceleration + double max_trans_dec; + if (nh.getParam(param_prefix + PARAM_MAX_TRANS_DEC, max_trans_dec)) + { + cartesian_limit.setMaxTranslationalDeceleration(max_trans_dec); + } + + // rotational velocity + double max_rot_vel; + if (nh.getParam(param_prefix + PARAM_MAX_ROT_VEL, max_rot_vel)) + { + cartesian_limit.setMaxRotationalVelocity(max_rot_vel); + } + + // rotational acceleration + deceleration deprecated + // LCOV_EXCL_START + if (nh.hasParam(param_prefix + PARAM_MAX_ROT_ACC) || nh.hasParam(param_prefix + PARAM_MAX_ROT_DEC)) + { + ROS_WARN_STREAM("Ignoring cartesian limits parameters for rotational " + "acceleration / deceleration;" + << "these parameters are deprecated and are automatically " + "calculated from" + << "translational to rotational ratio."); + } + // LCOV_EXCL_STOP + + return cartesian_limit; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp new file mode 100644 index 0000000000..52941969d2 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -0,0 +1,315 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/command_list_manager.h" + +#include +#include +#include + +#include +#include +#include + +#include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/tip_frame_getter.h" +#include "pilz_industrial_motion_planner/trajectory_blend_request.h" +#include "pilz_industrial_motion_planner/trajectory_blender_transition_window.h" + +namespace pilz_industrial_motion_planner +{ +static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; + +CommandListManager::CommandListManager(const ros::NodeHandle& nh, const moveit::core::RobotModelConstPtr& model) + : nh_(nh), model_(model) +{ + // Obtain the aggregated joint limits + pilz_industrial_motion_planner::JointLimitsContainer aggregated_limit_active_joints; + + aggregated_limit_active_joints = pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + ros::NodeHandle(PARAM_NAMESPACE_LIMITS), model_->getActiveJointModels()); + + // Obtain cartesian limits + pilz_industrial_motion_planner::CartesianLimit cartesian_limit = + pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits( + ros::NodeHandle(PARAM_NAMESPACE_LIMITS)); + + pilz_industrial_motion_planner::LimitsContainer limits; + limits.setJointLimits(aggregated_limit_active_joints); + limits.setCartesianLimits(cartesian_limit); + + plan_comp_builder_.setModel(model); + plan_comp_builder_.setBlender(std::unique_ptr( + new pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow(limits))); +} + +RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_pipeline::PlanningPipelinePtr& planning_pipeline, + const moveit_msgs::MotionSequenceRequest& req_list) +{ + if (req_list.items.empty()) + { + return RobotTrajCont(); + } + + checkForNegativeRadii(req_list); + checkLastBlendRadiusZero(req_list); + checkStartStates(req_list); + + MotionResponseCont resp_cont{ solveSequenceItems(planning_scene, planning_pipeline, req_list) }; + + assert(model_); + RadiiCont radii{ extractBlendRadii(*model_, req_list) }; + checkForOverlappingRadii(resp_cont, radii); + + plan_comp_builder_.reset(); + for (MotionResponseCont::size_type i = 0; i < resp_cont.size(); ++i) + { + plan_comp_builder_.append(resp_cont.at(i).trajectory_, + // The blend radii has to be "attached" to + // the second part of a blend trajectory, + // therefore: "i-1". + (i > 0 ? radii.at(i - 1) : 0.)); + } + return plan_comp_builder_.build(); +} + +bool CommandListManager::checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A, + const robot_trajectory::RobotTrajectory& traj_B, + const double radii_B) const +{ + // No blending between trajectories from different groups + if (traj_A.getGroupName() != traj_B.getGroupName()) + { + return false; + } + + auto sum_radii{ radii_A + radii_B }; + if (sum_radii == 0.) + { + return false; + } + + const std::string& blend_frame{ getSolverTipFrame(model_->getJointModelGroup(traj_A.getGroupName())) }; + auto distance_endpoints = (traj_A.getLastWayPoint().getFrameTransform(blend_frame).translation() - + traj_B.getLastWayPoint().getFrameTransform(blend_frame).translation()) + .norm(); + return distance_endpoints <= sum_radii; +} + +void CommandListManager::checkForOverlappingRadii(const MotionResponseCont& resp_cont, const RadiiCont& radii) const +{ + if (resp_cont.empty()) + { + return; + } + if (resp_cont.size() < 3) + { + return; + } + + for (MotionResponseCont::size_type i = 0; i < resp_cont.size() - 2; ++i) + { + if (checkRadiiForOverlap(*(resp_cont.at(i).trajectory_), radii.at(i), *(resp_cont.at(i + 1).trajectory_), + radii.at(i + 1))) + { + std::ostringstream os; + os << "Overlapping blend radii between command [" << i << "] and [" << i + 1 << "]."; + throw OverlappingBlendRadiiException(os.str()); + } + } +} + +CommandListManager::RobotState_OptRef +CommandListManager::getPreviousEndState(const MotionResponseCont& motion_plan_responses, const std::string& group_name) +{ + for (MotionResponseCont::const_reverse_iterator it = motion_plan_responses.crbegin(); + it != motion_plan_responses.crend(); ++it) + { + if (it->trajectory_->getGroupName() == group_name) + { + return it->trajectory_->getLastWayPoint(); + } + } + return boost::none; +} + +void CommandListManager::setStartState(const MotionResponseCont& motion_plan_responses, const std::string& group_name, + moveit_msgs::RobotState& start_state) +{ + RobotState_OptRef rob_state_op{ getPreviousEndState(motion_plan_responses, group_name) }; + if (rob_state_op) + { + moveit::core::robotStateToRobotStateMsg(rob_state_op.value(), start_state); + } +} + +bool CommandListManager::isInvalidBlendRadii(const moveit::core::RobotModel& model, + const moveit_msgs::MotionSequenceItem& item_A, + const moveit_msgs::MotionSequenceItem& item_B) +{ + // Zero blend radius is always valid + if (item_A.blend_radius == 0.) + { + return false; + } + + // No blending between different groups + if (item_A.req.group_name != item_B.req.group_name) + { + ROS_WARN_STREAM("Blending between different groups (in this case: \"" + << item_A.req.group_name << "\" and \"" << item_B.req.group_name << "\") not allowed"); + return true; + } + + // No blending for groups without solver + if (!hasSolver(model.getJointModelGroup(item_A.req.group_name))) + { + ROS_WARN_STREAM("Blending for groups without solver not allowed"); + return true; + } + + return false; +} + +CommandListManager::RadiiCont CommandListManager::extractBlendRadii(const moveit::core::RobotModel& model, + const moveit_msgs::MotionSequenceRequest& req_list) +{ + RadiiCont radii(req_list.items.size(), 0.); + for (RadiiCont::size_type i = 0; i < (radii.size() - 1); ++i) + { + if (isInvalidBlendRadii(model, req_list.items.at(i), req_list.items.at(i + 1))) + { + ROS_WARN_STREAM("Invalid blend radii between commands: [" << i << "] and [" << i + 1 + << "] => Blend radii set to zero"); + continue; + } + radii.at(i) = req_list.items.at(i).blend_radius; + } + return radii; +} + +CommandListManager::MotionResponseCont +CommandListManager::solveSequenceItems(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_pipeline::PlanningPipelinePtr& planning_pipeline, + const moveit_msgs::MotionSequenceRequest& req_list) const +{ + MotionResponseCont motion_plan_responses; + size_t curr_req_index{ 0 }; + const size_t num_req{ req_list.items.size() }; + for (const auto& seq_item : req_list.items) + { + planning_interface::MotionPlanRequest req{ seq_item.req }; + setStartState(motion_plan_responses, req.group_name, req.start_state); + + planning_interface::MotionPlanResponse res; + planning_pipeline->generatePlan(planning_scene, req, res); + if (res.error_code_.val != res.error_code_.SUCCESS) + { + std::ostringstream os; + os << "Could not solve request\n---\n" << req << "\n---\n"; + throw PlanningPipelineException(os.str(), res.error_code_.val); + } + motion_plan_responses.emplace_back(res); + ROS_DEBUG_STREAM("Solved [" << ++curr_req_index << "/" << num_req << "]"); + } + return motion_plan_responses; +} + +void CommandListManager::checkForNegativeRadii(const moveit_msgs::MotionSequenceRequest& req_list) +{ + if (!std::all_of(req_list.items.begin(), req_list.items.end(), + [](const moveit_msgs::MotionSequenceItem& req) { return (req.blend_radius >= 0.); })) + { + throw NegativeBlendRadiusException("All blending radii MUST be non negative"); + } +} + +void CommandListManager::checkStartStatesOfGroup(const moveit_msgs::MotionSequenceRequest& req_list, + const std::string& group_name) +{ + bool first_elem{ true }; + for (const moveit_msgs::MotionSequenceItem& item : req_list.items) + { + if (item.req.group_name != group_name) + { + continue; + } + + if (first_elem) + { + first_elem = false; + continue; + } + + if (!(item.req.start_state.joint_state.position.empty() && item.req.start_state.joint_state.velocity.empty() && + item.req.start_state.joint_state.effort.empty() && item.req.start_state.joint_state.name.empty())) + { + std::ostringstream os; + os << "Only the first request is allowed to have a start state, but" + << " the requests for group: \"" << group_name << "\" violate the rule"; + throw StartStateSetException(os.str()); + } + } +} + +void CommandListManager::checkStartStates(const moveit_msgs::MotionSequenceRequest& req_list) +{ + if (req_list.items.size() <= 1) + { + return; + } + + GroupNamesCont group_names{ getGroupNames(req_list) }; + for (const auto& curr_group_name : group_names) + { + checkStartStatesOfGroup(req_list, curr_group_name); + } +} + +CommandListManager::GroupNamesCont CommandListManager::getGroupNames(const moveit_msgs::MotionSequenceRequest& req_list) +{ + GroupNamesCont group_names; + std::for_each(req_list.items.cbegin(), req_list.items.cend(), + [&group_names](const moveit_msgs::MotionSequenceItem& item) { + if (std::find(group_names.cbegin(), group_names.cend(), item.req.group_name) == group_names.cend()) + { + group_names.emplace_back(item.req.group_name); + } + }); + return group_names; +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp new file mode 100644 index 0000000000..5c800573b5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp @@ -0,0 +1,181 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/joint_limits_interface_extension.h" + +#include +#include + +#include + +pilz_industrial_motion_planner::JointLimitsContainer +pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + const ros::NodeHandle& nh, const std::vector& joint_models) +{ + JointLimitsContainer container; + + ROS_INFO_STREAM("Reading limits from namespace " << nh.getNamespace()); + + // Iterate over all joint models and generate the map + for (auto joint_model : joint_models) + { + JointLimit joint_limit; + + // If there is something defined for the joint on the parameter server + if (joint_limits_interface::getJointLimits(joint_model->getName(), nh, joint_limit)) + { + if (joint_limit.has_position_limits) + { + checkPositionBoundsThrowing(joint_model, joint_limit); + } + else + { + updatePositionLimitFromJointModel(joint_model, joint_limit); + } + + if (joint_limit.has_velocity_limits) + { + checkVelocityBoundsThrowing(joint_model, joint_limit); + } + else + { + updateVelocityLimitFromJointModel(joint_model, joint_limit); + } + } + else + { + // If there is nothing defined for this joint on the parameter server just + // update the values by the values of + // the urdf + + updatePositionLimitFromJointModel(joint_model, joint_limit); + updateVelocityLimitFromJointModel(joint_model, joint_limit); + } + + // Update max_deceleration if no max_acceleration has been set + if (joint_limit.has_acceleration_limits && !joint_limit.has_deceleration_limits) + { + joint_limit.max_deceleration = -joint_limit.max_acceleration; + joint_limit.has_deceleration_limits = true; + } + + // Insert the joint limit into the map + container.addLimit(joint_model->getName(), joint_limit); + } + + return container; +} + +void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitFromJointModel( + const moveit::core::JointModel* joint_model, JointLimit& joint_limit) +{ + switch (joint_model->getVariableBounds().size()) + { + // LCOV_EXCL_START + case 0: + ROS_ERROR_STREAM("no bounds set for joint " << joint_model->getName()); + break; + // LCOV_EXCL_STOP + case 1: + joint_limit.has_position_limits = joint_model->getVariableBounds()[0].position_bounded_; + joint_limit.min_position = joint_model->getVariableBounds()[0].min_position_; + joint_limit.max_position = joint_model->getVariableBounds()[0].max_position_; + break; + // LCOV_EXCL_START + default: + ROS_ERROR_STREAM("Multi-DOF-Joints not supported. The robot won't move. " << joint_model->getName()); + joint_limit.has_position_limits = true; + joint_limit.min_position = 0; + joint_limit.max_position = 0; + break; + // LCOV_EXCL_STOP + } + + ROS_DEBUG_STREAM("Limit(" << joint_model->getName() << " min:" << joint_limit.min_position + << " max:" << joint_limit.max_position); +} + +void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitFromJointModel( + const moveit::core::JointModel* joint_model, JointLimit& joint_limit) +{ + switch (joint_model->getVariableBounds().size()) + { + // LCOV_EXCL_START + case 0: + ROS_ERROR_STREAM("no bounds set for joint " << joint_model->getName()); + break; + // LCOV_EXCL_STOP + case 1: + joint_limit.has_velocity_limits = joint_model->getVariableBounds()[0].velocity_bounded_; + joint_limit.max_velocity = joint_model->getVariableBounds()[0].max_velocity_; + break; + // LCOV_EXCL_START + default: + ROS_ERROR_STREAM("Multi-DOF-Joints not supported. The robot won't move."); + joint_limit.has_velocity_limits = true; + joint_limit.max_velocity = 0; + break; + // LCOV_EXCL_STOP + } +} + +void pilz_industrial_motion_planner::JointLimitsAggregator::checkPositionBoundsThrowing( + const moveit::core::JointModel* joint_model, const JointLimit& joint_limit) +{ + // Check min position + if (!joint_model->satisfiesPositionBounds(&joint_limit.min_position)) + { + throw AggregationBoundsViolationException("min_position of " + joint_model->getName() + + " violates min limit from URDF"); + } + + // Check max position + if (!joint_model->satisfiesPositionBounds(&joint_limit.max_position)) + { + throw AggregationBoundsViolationException("max_position of " + joint_model->getName() + + " violates max limit from URDF"); + } +} + +void pilz_industrial_motion_planner::JointLimitsAggregator::checkVelocityBoundsThrowing( + const moveit::core::JointModel* joint_model, const JointLimit& joint_limit) +{ + // Check min position + if (!joint_model->satisfiesVelocityBounds(&joint_limit.max_velocity)) + { + throw AggregationBoundsViolationException("max_velocity of " + joint_model->getName() + + " violates velocity limit from URDF"); + } +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp new file mode 100644 index 0000000000..7bdb1d983f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp @@ -0,0 +1,182 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_container.h" + +#include "ros/ros.h" +#include + +namespace pilz_industrial_motion_planner +{ +bool JointLimitsContainer::addLimit(const std::string& joint_name, JointLimit joint_limit) +{ + if (joint_limit.has_deceleration_limits && joint_limit.max_deceleration >= 0) + { + ROS_ERROR_STREAM("joint_limit.max_deceleration MUST be negative!"); + return false; + } + const auto& insertion_result{ container_.insert(std::pair(joint_name, joint_limit)) }; + if (!insertion_result.second) + { + ROS_ERROR_STREAM("joint_limit for joint " << joint_name << " already contained."); + return false; + } + return true; +} + +bool JointLimitsContainer::hasLimit(const std::string& joint_name) const +{ + return container_.find(joint_name) != container_.end(); +} + +size_t JointLimitsContainer::getCount() const +{ + return container_.size(); +} + +bool JointLimitsContainer::empty() const +{ + return container_.empty(); +} + +JointLimit JointLimitsContainer::getCommonLimit() const +{ + JointLimit common_limit; + for (const auto& limit : container_) + { + updateCommonLimit(limit.second, common_limit); + } + return common_limit; +} + +JointLimit JointLimitsContainer::getCommonLimit(const std::vector& joint_names) const +{ + JointLimit common_limit; + for (const auto& joint_name : joint_names) + { + updateCommonLimit(container_.at(joint_name), common_limit); + } + return common_limit; +} + +JointLimit JointLimitsContainer::getLimit(const std::string& joint_name) const +{ + return container_.at(joint_name); +} + +std::map::const_iterator JointLimitsContainer::begin() const +{ + return container_.begin(); +} + +std::map::const_iterator JointLimitsContainer::end() const +{ + return container_.end(); +} + +bool JointLimitsContainer::verifyVelocityLimit(const std::string& joint_name, const double& joint_velocity) const +{ + return (!(hasLimit(joint_name) && getLimit(joint_name).has_velocity_limits && + fabs(joint_velocity) > getLimit(joint_name).max_velocity)); +} + +bool JointLimitsContainer::verifyPositionLimit(const std::string& joint_name, const double& joint_position) const +{ + return (!(hasLimit(joint_name) && getLimit(joint_name).has_position_limits && + (joint_position < getLimit(joint_name).min_position || joint_position > getLimit(joint_name).max_position))); +} + +bool JointLimitsContainer::verifyPositionLimits(const std::vector& joint_names, + const std::vector& joint_positions) const +{ + if (joint_names.size() != joint_positions.size()) + { + throw std::out_of_range("joint_names vector has a different size than joint_positions vector."); + } + + for (std::size_t i = 0; i < joint_names.size(); ++i) + { + if (!verifyPositionLimit(joint_names.at(i), joint_positions.at(i))) + { + return false; + } + } + + return true; +} + +void JointLimitsContainer::updateCommonLimit(const JointLimit& joint_limit, JointLimit& common_limit) +{ + // check position limits + if (joint_limit.has_position_limits) + { + double min_position = joint_limit.min_position; + double max_position = joint_limit.max_position; + + common_limit.min_position = + (!common_limit.has_position_limits) ? min_position : std::max(common_limit.min_position, min_position); + common_limit.max_position = + (!common_limit.has_position_limits) ? max_position : std::min(common_limit.max_position, max_position); + common_limit.has_position_limits = true; + } + + // check velocity limits + if (joint_limit.has_velocity_limits) + { + double max_velocity = joint_limit.max_velocity; + common_limit.max_velocity = + (!common_limit.has_velocity_limits) ? max_velocity : std::min(common_limit.max_velocity, max_velocity); + common_limit.has_velocity_limits = true; + } + + // check acceleration limits + if (joint_limit.has_acceleration_limits) + { + double max_acc = joint_limit.max_acceleration; + common_limit.max_acceleration = + (!common_limit.has_acceleration_limits) ? max_acc : std::min(common_limit.max_acceleration, max_acc); + common_limit.has_acceleration_limits = true; + } + + // check deceleration limits + if (joint_limit.has_deceleration_limits) + { + double max_dec = joint_limit.max_deceleration; + common_limit.max_deceleration = + (!common_limit.has_deceleration_limits) ? max_dec : std::max(common_limit.max_deceleration, max_dec); + common_limit.has_deceleration_limits = true; + } +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp new file mode 100644 index 0000000000..66c5b198a9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp @@ -0,0 +1,157 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_extension.h" +#include "pilz_industrial_motion_planner/joint_limits_interface_extension.h" + +#include "pilz_industrial_motion_planner/joint_limits_validator.h" + +bool pilz_industrial_motion_planner::JointLimitsValidator::validateAllPositionLimitsEqual( + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + return validateWithEqualFunc(&pilz_industrial_motion_planner::JointLimitsValidator::positionEqual, joint_limits); +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::validateAllVelocityLimitsEqual( + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + return validateWithEqualFunc(&pilz_industrial_motion_planner::JointLimitsValidator::velocityEqual, joint_limits); +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::validateAllAccelerationLimitsEqual( + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + return validateWithEqualFunc(&pilz_industrial_motion_planner::JointLimitsValidator::accelerationEqual, joint_limits); +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::validateAllDecelerationLimitsEqual( + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + return validateWithEqualFunc(&pilz_industrial_motion_planner::JointLimitsValidator::decelerationEqual, joint_limits); +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::validateWithEqualFunc( + bool (*eq_func)(const JointLimit&, const JointLimit&), + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + // If there are no joint_limits it is considered equal + if (joint_limits.empty()) + { + return true; + } + + JointLimit reference = joint_limits.begin()->second; + + for (auto it = std::next(joint_limits.begin()); it != joint_limits.end(); ++it) + { + if (!eq_func(reference, it->second)) + { + return false; + } + } + + return true; +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::positionEqual(const JointLimit& lhs, const JointLimit& rhs) +{ + // Return false if .has_velocity_limits differs + if (lhs.has_position_limits != rhs.has_position_limits) + { + return false; + } + + // If velocity limits are the same make sure they are equal + if (lhs.has_position_limits && ((lhs.max_position != rhs.max_position) || (lhs.min_position != rhs.min_position))) + { + return false; + } + return true; +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::velocityEqual(const JointLimit& lhs, const JointLimit& rhs) +{ + // Return false if .has_velocity_limits differs + if (lhs.has_velocity_limits != rhs.has_velocity_limits) + { + return false; + } + + // If velocity limits are the same make sure they are equal + if (lhs.has_velocity_limits && (lhs.max_velocity != rhs.max_velocity)) + { + return false; + } + + return true; +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::accelerationEqual(const JointLimit& lhs, + const JointLimit& rhs) +{ + // Return false if .has_acceleration_limits differs + if (lhs.has_acceleration_limits != rhs.has_acceleration_limits) + { + return false; + } + + // If velocity limits are the same make sure they are equal + if (lhs.has_acceleration_limits && (lhs.max_acceleration != rhs.max_acceleration)) + { + return false; + } + + return true; +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::decelerationEqual(const JointLimit& lhs, + const JointLimit& rhs) +{ + // Return false if .has_acceleration_limits differs + if (lhs.has_deceleration_limits != rhs.has_deceleration_limits) + { + return false; + } + + // If velocity limits are the same make sure they are equal + if (lhs.has_deceleration_limits && (lhs.max_deceleration != rhs.max_deceleration)) + { + return false; + } + + return true; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp new file mode 100644 index 0000000000..6ee51df0b6 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp @@ -0,0 +1,78 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/limits_container.h" + +pilz_industrial_motion_planner::LimitsContainer::LimitsContainer() + : has_joint_limits_(false), has_cartesian_limits_(false) +{ +} + +bool pilz_industrial_motion_planner::LimitsContainer::hasJointLimits() const +{ + return has_joint_limits_; +} + +void pilz_industrial_motion_planner::LimitsContainer::setJointLimits( + pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + has_joint_limits_ = true; + joint_limits_ = joint_limits; +} + +const pilz_industrial_motion_planner::JointLimitsContainer& +pilz_industrial_motion_planner::LimitsContainer::getJointLimitContainer() const +{ + return joint_limits_; +} + +bool pilz_industrial_motion_planner::LimitsContainer::hasFullCartesianLimits() const +{ + return (has_cartesian_limits_ && cartesian_limit_.hasMaxTranslationalVelocity() && + cartesian_limit_.hasMaxTranslationalAcceleration() && cartesian_limit_.hasMaxTranslationalDeceleration() && + cartesian_limit_.hasMaxRotationalVelocity()); +} + +void pilz_industrial_motion_planner::LimitsContainer::setCartesianLimits( + pilz_industrial_motion_planner::CartesianLimit& cartesian_limit) +{ + has_cartesian_limits_ = true; + cartesian_limit_ = cartesian_limit; +} + +const pilz_industrial_motion_planner::CartesianLimit& +pilz_industrial_motion_planner::LimitsContainer::getCartesianLimits() const +{ + return cartesian_limit_; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp new file mode 100644 index 0000000000..6a6b84cab0 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp @@ -0,0 +1,294 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ + +/* Author: Ioan Sucan */ + +// Modified by Pilz GmbH & Co. KG + +#include "pilz_industrial_motion_planner/move_group_sequence_action.h" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/command_list_manager.h" +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" + +namespace pilz_industrial_motion_planner +{ +MoveGroupSequenceAction::MoveGroupSequenceAction() : MoveGroupCapability("SequenceAction") +{ +} + +void MoveGroupSequenceAction::initialize() +{ + // start the move action server + ROS_INFO_STREAM("initialize move group sequence action"); + move_action_server_.reset(new actionlib::SimpleActionServer( + root_node_handle_, "sequence_move_group", + boost::bind(&MoveGroupSequenceAction::executeSequenceCallback, this, _1), false)); + move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupSequenceAction::preemptMoveCallback, this)); + move_action_server_->start(); + + command_list_manager_.reset(new pilz_industrial_motion_planner::CommandListManager( + ros::NodeHandle("~"), context_->planning_scene_monitor_->getRobotModel())); +} + +void MoveGroupSequenceAction::executeSequenceCallback(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal) +{ + setMoveState(move_group::PLANNING); + + // Handle empty requests + if (goal->request.items.empty()) + { + ROS_WARN("Received empty request. That's ok but maybe not what you intended."); + setMoveState(move_group::IDLE); + moveit_msgs::MoveGroupSequenceResult action_res; + action_res.response.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + move_action_server_->setSucceeded(action_res, "Received empty request."); + return; + } + + // before we start planning, ensure that we have the latest robot state + // received... + context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now()); + context_->planning_scene_monitor_->updateFrameTransforms(); + + moveit_msgs::MoveGroupSequenceResult action_res; + if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_) + { + if (!goal->planning_options.plan_only) + { + ROS_WARN("Only plan will be calculated, although plan_only == false."); // LCOV_EXCL_LINE + } + executeMoveCallbackPlanOnly(goal, action_res); + } + else + { + executeSequenceCallbackPlanAndExecute(goal, action_res); + } + + switch (action_res.response.error_code.val) + { + case moveit_msgs::MoveItErrorCodes::SUCCESS: + move_action_server_->setSucceeded(action_res, "Success"); + break; + case moveit_msgs::MoveItErrorCodes::PREEMPTED: + move_action_server_->setPreempted(action_res, "Preempted"); + break; + default: + move_action_server_->setAborted(action_res, "See error code for more information"); + break; + } + + setMoveState(move_group::IDLE); +} + +void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute( + const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal, moveit_msgs::MoveGroupSequenceResult& action_res) +{ + ROS_INFO("Combined planning and execution request received for " + "MoveGroupSequenceAction."); + + plan_execution::PlanExecution::Options opt; + + const moveit_msgs::PlanningScene& planning_scene_diff = + moveit::core::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ? + goal->planning_options.planning_scene_diff : + clearSceneRobotState(goal->planning_options.planning_scene_diff); + + opt.replan_ = goal->planning_options.replan; + opt.replan_attempts_ = goal->planning_options.replan_attempts; + opt.replan_delay_ = goal->planning_options.replan_delay; + opt.before_execution_callback_ = boost::bind(&MoveGroupSequenceAction::startMoveExecutionCallback, this); + + opt.plan_callback_ = + boost::bind(&MoveGroupSequenceAction::planUsingSequenceManager, this, boost::cref(goal->request), _1); + + if (goal->planning_options.look_around && context_->plan_with_sensing_) + { + ROS_WARN("Plan with sensing not yet implemented/tested. This option is " + "ignored."); // LCOV_EXCL_LINE + } + + plan_execution::ExecutableMotionPlan plan; + context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt); + + StartStatesMsg start_states_msg; + convertToMsg(plan.plan_components_, start_states_msg, action_res.response.planned_trajectories); + try + { + action_res.response.sequence_start = start_states_msg.at(0); + } + catch (std::out_of_range&) + { + ROS_WARN("Can not determine start state from empty sequence."); + } + action_res.response.error_code = plan.error_code_; +} + +void MoveGroupSequenceAction::convertToMsg(const ExecutableTrajs& trajs, StartStatesMsg& start_states_msg, + PlannedTrajMsgs& planned_trajs_msgs) +{ + start_states_msg.resize(trajs.size()); + planned_trajs_msgs.resize(trajs.size()); + for (size_t i = 0; i < trajs.size(); ++i) + { + robot_state::robotStateToRobotStateMsg(trajs.at(i).trajectory_->getFirstWayPoint(), start_states_msg.at(i)); + trajs.at(i).trajectory_->getRobotTrajectoryMsg(planned_trajs_msgs.at(i)); + } +} + +void MoveGroupSequenceAction::executeMoveCallbackPlanOnly(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal, + moveit_msgs::MoveGroupSequenceResult& res) +{ + ROS_INFO("Planning request received for MoveGroupSequenceAction action."); + + // lock the scene so that it does not modify the world representation while + // diff() is called + planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); + + const planning_scene::PlanningSceneConstPtr& the_scene = + (moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) ? + static_cast(lscene) : + lscene->diff(goal->planning_options.planning_scene_diff); + + ros::Time planning_start = ros::Time::now(); + RobotTrajCont traj_vec; + try + { + traj_vec = command_list_manager_->solve(the_scene, context_->planning_pipeline_, goal->request); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM("> Planning pipeline threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); + res.response.error_code.val = ex.getErrorCode(); + return; + } + // LCOV_EXCL_START // Keep moveit up even if lower parts throw + catch (const std::exception& ex) + { + ROS_ERROR("Planning pipeline threw an exception: %s", ex.what()); + res.response.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; + return; + } + // LCOV_EXCL_STOP + + StartStatesMsg start_states_msg; + start_states_msg.resize(traj_vec.size()); + res.response.planned_trajectories.resize(traj_vec.size()); + for (RobotTrajCont::size_type i = 0; i < traj_vec.size(); ++i) + { + move_group::MoveGroupCapability::convertToMsg(traj_vec.at(i), start_states_msg.at(i), + res.response.planned_trajectories.at(i)); + } + try + { + res.response.sequence_start = start_states_msg.at(0); + } + catch (std::out_of_range&) + { + ROS_WARN("Can not determine start state from empty sequence."); + } + + res.response.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + res.response.planning_time = ros::Time::now().toSec() - planning_start.toSec(); +} + +bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::MotionSequenceRequest& req, + plan_execution::ExecutableMotionPlan& plan) +{ + setMoveState(move_group::PLANNING); + + planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_); + RobotTrajCont traj_vec; + try + { + traj_vec = command_list_manager_->solve(plan.planning_scene_, context_->planning_pipeline_, req); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM("Planning pipeline threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); + plan.error_code_.val = ex.getErrorCode(); + return false; + } + // LCOV_EXCL_START // Keep moveit up even if lower parts throw + catch (const std::exception& ex) + { + ROS_ERROR_STREAM("Planning pipeline threw an exception: " << ex.what()); + plan.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE; + return false; + } + // LCOV_EXCL_STOP + + if (!traj_vec.empty()) + { + plan.plan_components_.resize(traj_vec.size()); + for (size_t i = 0; i < traj_vec.size(); ++i) + { + plan.plan_components_.at(i).trajectory_ = traj_vec.at(i); + plan.plan_components_.at(i).description_ = "plan"; + } + } + plan.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + return true; +} + +void MoveGroupSequenceAction::startMoveExecutionCallback() +{ + setMoveState(move_group::MONITOR); +} + +void MoveGroupSequenceAction::preemptMoveCallback() +{ + context_->plan_execution_->stop(); +} + +void MoveGroupSequenceAction::setMoveState(move_group::MoveGroupState state) +{ + move_state_ = state; + move_feedback_.state = stateToStr(state); + move_action_server_->publishFeedback(move_feedback_); +} + +} // namespace pilz_industrial_motion_planner + +#include +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::MoveGroupSequenceAction, move_group::MoveGroupCapability) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp new file mode 100644 index 0000000000..ce6f789eb4 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp @@ -0,0 +1,105 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ + +/* Author: Ioan Sucan */ + +// Modified by Pilz GmbH & Co. KG + +#include "pilz_industrial_motion_planner/move_group_sequence_service.h" + +#include "pilz_industrial_motion_planner/capability_names.h" +#include "pilz_industrial_motion_planner/command_list_manager.h" +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" + +namespace pilz_industrial_motion_planner +{ +MoveGroupSequenceService::MoveGroupSequenceService() : MoveGroupCapability("SequenceService") +{ +} + +MoveGroupSequenceService::~MoveGroupSequenceService() +{ +} + +void MoveGroupSequenceService::initialize() +{ + command_list_manager_.reset(new pilz_industrial_motion_planner::CommandListManager( + ros::NodeHandle("~"), context_->planning_scene_monitor_->getRobotModel())); + + sequence_service_ = root_node_handle_.advertiseService(SEQUENCE_SERVICE_NAME, &MoveGroupSequenceService::plan, this); +} + +bool MoveGroupSequenceService::plan(moveit_msgs::GetMotionSequence::Request& req, + moveit_msgs::GetMotionSequence::Response& res) +{ + // TODO: Do we lock on the correct scene? Does the lock belong to the scene + // used for planning? + planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_); + + ros::Time planning_start = ros::Time::now(); + RobotTrajCont traj_vec; + try + { + traj_vec = command_list_manager_->solve(ps, context_->planning_pipeline_, req.request); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM("Planner threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); + res.response.error_code.val = ex.getErrorCode(); + return true; + } + // LCOV_EXCL_START // Keep moveit up even if lower parts throw + catch (const std::exception& ex) + { + ROS_ERROR_STREAM("Planner threw an exception: " << ex.what()); + // If 'FALSE' then no response will be sent to the caller. + return false; + } + // LCOV_EXCL_STOP + + res.response.planned_trajectories.resize(traj_vec.size()); + for (RobotTrajCont::size_type i = 0; i < traj_vec.size(); ++i) + { + move_group::MoveGroupCapability::convertToMsg(traj_vec.at(i), res.response.sequence_start, + res.response.planned_trajectories.at(i)); + } + res.response.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + res.response.planning_time = (ros::Time::now() - planning_start).toSec(); + return true; +} + +} // namespace pilz_industrial_motion_planner + +#include +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::MoveGroupSequenceService, move_group::MoveGroupCapability) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp new file mode 100644 index 0000000000..503a1a3c89 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp @@ -0,0 +1,147 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/path_circle_generator.h" + +namespace pilz_industrial_motion_planner +{ +std::unique_ptr PathCircleGenerator::circleFromCenter(const KDL::Frame& start_pose, + const KDL::Frame& goal_pose, + const KDL::Vector& center_point, double eqradius) +{ + double a = (start_pose.p - center_point).Norm(); + double b = (goal_pose.p - center_point).Norm(); + double c = (start_pose.p - goal_pose.p).Norm(); + + if (fabs(a - b) > MAX_RADIUS_DIFF) + { + throw ErrorMotionPlanningCenterPointDifferentRadius(); + } + + // compute the rotation angle + double alpha = cosines(a, b, c); + + KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); + double old_kdl_epsilon = KDL::epsilon; + try + { + KDL::epsilon = MAX_COLINEAR_NORM; + auto circle = std::unique_ptr( + new KDL::Path_Circle(start_pose, center_point, goal_pose.p, goal_pose.M, alpha, rot_interpo, eqradius, + true /* take ownership of RotationalInterpolation */)); + KDL::epsilon = old_kdl_epsilon; + return circle; + } + catch (KDL::Error_MotionPlanning&) + { + delete rot_interpo; // in case we could not construct the Path object, avoid + // a memory leak + KDL::epsilon = old_kdl_epsilon; + throw; // and pass the exception on to the caller + } +} + +std::unique_ptr PathCircleGenerator::circleFromInterim(const KDL::Frame& start_pose, + const KDL::Frame& goal_pose, + const KDL::Vector& interim_point, double eqradius) +{ + // compute the center point from interim point + // triangle edges + const KDL::Vector t = interim_point - start_pose.p; + const KDL::Vector u = goal_pose.p - start_pose.p; + const KDL::Vector v = goal_pose.p - interim_point; + // triangle normal + const KDL::Vector w = t * u; // cross product + + // circle center + if (w.Norm() < MAX_COLINEAR_NORM) + { + throw KDL::Error_MotionPlanning_Circle_No_Plane(); + } + const KDL::Vector center_point = + start_pose.p + (u * dot(t, t) * dot(u, v) - t * dot(u, u) * dot(t, v)) * 0.5 / pow(w.Norm(), 2); + + // compute the rotation angle + // triangle edges + const KDL::Vector t_center = center_point - start_pose.p; + const KDL::Vector v_center = goal_pose.p - center_point; + double a = t_center.Norm(); + double b = v_center.Norm(); + double c = u.Norm(); + double alpha = cosines(a, b, c); + + KDL::Vector kdl_aux_point(interim_point); + + // if the angle at the interim is an acute angle (<90deg), rotation angle is + // an obtuse angle (>90deg) + // in this case using the interim as auxiliary point for KDL::Path_Circle can + // lead to a path in the wrong direction + double interim_angle = cosines(t.Norm(), v.Norm(), u.Norm()); + if (interim_angle < M_PI / 2) + { + alpha = 2 * M_PI - alpha; + + // exclude that the goal is not colinear with start and center, then use the + // opposite of the goal as auxiliary point + if ((t_center * v_center).Norm() > MAX_COLINEAR_NORM) + { + kdl_aux_point = 2 * center_point - goal_pose.p; + } + } + + KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); + try + { + return std::unique_ptr(new KDL::Path_Circle(start_pose, center_point, kdl_aux_point, goal_pose.M, alpha, + rot_interpo, eqradius, + true /* take ownership of RotationalInterpolation */)); + } + catch (KDL::Error_MotionPlanning&) // LCOV_EXCL_START // The cases where + // KDL would throw are already handled + // above, + // we keep these lines to be safe + { + delete rot_interpo; // in case we could not construct the Path object, avoid + // a memory leak + throw; // and pass the exception on to the caller + // LCOV_EXCL_STOP + } +} + +double PathCircleGenerator::cosines(const double a, const double b, const double c) +{ + return acos(std::max(std::min((pow(a, 2) + pow(b, 2) - pow(c, 2)) / (2.0 * a * b), 1.0), -1.0)); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp new file mode 100644 index 0000000000..a7e592b204 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp @@ -0,0 +1,173 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/pilz_industrial_motion_planner.h" + +#include "pilz_industrial_motion_planner/planning_context_loader.h" +#include "pilz_industrial_motion_planner/planning_context_loader_ptp.h" +#include "pilz_industrial_motion_planner/planning_exceptions.h" + +#include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" + +// Boost includes +#include + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +static const std::string PARAM_NAMESPACE_LIMTS = "robot_description_planning"; + +bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) +{ + // Call parent class initialize + planning_interface::PlannerManager::initialize(model, ns); + + // Store the model and the namespace + model_ = model; + namespace_ = ns; + + // Obtain the aggregated joint limits + aggregated_limit_active_joints_ = pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + ros::NodeHandle(PARAM_NAMESPACE_LIMTS), model->getActiveJointModels()); + + // Obtain cartesian limits + cartesian_limit_ = pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits( + ros::NodeHandle(PARAM_NAMESPACE_LIMTS)); + + // Load the planning context loader + planner_context_loader.reset(new pluginlib::ClassLoader( + "pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader")); + + // List available plugins + const std::vector& factories = planner_context_loader->getDeclaredClasses(); + std::stringstream ss; + for (const auto& factory : factories) + { + ss << factory << " "; + } + + ROS_INFO_STREAM("Available plugins: " << ss.str()); + + // Load each factory + for (const auto& factory : factories) + { + ROS_INFO_STREAM("About to load: " << factory); + PlanningContextLoaderPtr loader_pointer(planner_context_loader->createInstance(factory)); + + pilz_industrial_motion_planner::LimitsContainer limits; + limits.setJointLimits(aggregated_limit_active_joints_); + limits.setCartesianLimits(cartesian_limit_); + + loader_pointer->setLimits(limits); + loader_pointer->setModel(model_); + + registerContextLoader(loader_pointer); + } + + return true; +} + +std::string CommandPlanner::getDescription() const +{ + return "Pilz Industrial Motion Planner"; +} + +void CommandPlanner::getPlanningAlgorithms(std::vector& algs) const +{ + algs.clear(); + + for (const auto& context_loader : context_loader_map_) + { + algs.push_back(context_loader.first); + } +} + +planning_interface::PlanningContextPtr +CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const moveit_msgs::MotionPlanRequest& req, + moveit_msgs::MoveItErrorCodes& error_code) const +{ + ROS_DEBUG_STREAM("Loading PlanningContext for request\n\n" << req << "\n"); + + // Check that a loaded for this request exists + if (!canServiceRequest(req)) + { + ROS_ERROR_STREAM("No ContextLoader for planner_id " << req.planner_id.c_str() << " found. Planning not possible."); + return nullptr; + } + + planning_interface::PlanningContextPtr planning_context; + + if (context_loader_map_.at(req.planner_id)->loadContext(planning_context, req.planner_id, req.group_name)) + { + ROS_DEBUG_STREAM("Found planning context loader for " << req.planner_id << " group:" << req.group_name); + planning_context->setMotionPlanRequest(req); + planning_context->setPlanningScene(planning_scene); + return planning_context; + } + else + { + error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; + return nullptr; + } +} + +bool CommandPlanner::canServiceRequest(const moveit_msgs::MotionPlanRequest& req) const +{ + return context_loader_map_.find(req.planner_id) != context_loader_map_.end(); +} + +void CommandPlanner::registerContextLoader( + const pilz_industrial_motion_planner::PlanningContextLoaderPtr& planning_context_loader) +{ + // Only add if command is not already in list, throw exception if not + if (context_loader_map_.find(planning_context_loader->getAlgorithm()) == context_loader_map_.end()) + { + context_loader_map_[planning_context_loader->getAlgorithm()] = planning_context_loader; + ROS_INFO_STREAM("Registered Algorithm [" << planning_context_loader->getAlgorithm() << "]"); + } + else + { + throw ContextLoaderRegistrationException("The command [" + planning_context_loader->getAlgorithm() + + "] is already registered"); + } +} + +} // namespace pilz_industrial_motion_planner + +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::CommandPlanner, planning_interface::PlannerManager) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp new file mode 100644 index 0000000000..175a7deca9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp @@ -0,0 +1,137 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/plan_components_builder.h" + +#include + +#include "pilz_industrial_motion_planner/tip_frame_getter.h" + +namespace pilz_industrial_motion_planner +{ +std::vector PlanComponentsBuilder::build() const +{ + std::vector res_vec{ traj_cont_ }; + if (traj_tail_) + { + assert(!res_vec.empty()); + appendWithStrictTimeIncrease(*(res_vec.back()), *traj_tail_); + } + return res_vec; +} + +void PlanComponentsBuilder::appendWithStrictTimeIncrease(robot_trajectory::RobotTrajectory& result, + const robot_trajectory::RobotTrajectory& source) +{ + if (result.empty() || + !pilz_industrial_motion_planner::isRobotStateEqual(result.getLastWayPoint(), source.getFirstWayPoint(), + result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON)) + { + result.append(source, 0.0); + return; + } + + for (size_t i = 1; i < source.getWayPointCount(); ++i) + { + result.addSuffixWayPoint(source.getWayPoint(i), source.getWayPointDurationFromPrevious(i)); + } +} + +void PlanComponentsBuilder::blend(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius) +{ + if (!blender_) + { + throw NoBlenderSetException("No blender set"); + } + + assert(other->getGroupName() == traj_tail_->getGroupName()); + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_request; + + blend_request.first_trajectory = traj_tail_; + blend_request.second_trajectory = other; + blend_request.blend_radius = blend_radius; + blend_request.group_name = traj_tail_->getGroupName(); + blend_request.link_name = getSolverTipFrame(model_->getJointModelGroup(blend_request.group_name)); + + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_response; + if (!blender_->blend(blend_request, blend_response)) + { + throw BlendingFailedException("Blending failed"); + } + + // Append the new trajectory elements + appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.first_trajectory); + traj_cont_.back()->append(*blend_response.blend_trajectory, 0.0); + // Store the last new trajectory element for future processing + traj_tail_ = blend_response.second_trajectory; // first for next blending segment +} + +void PlanComponentsBuilder::append(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius) +{ + if (!model_) + { + throw NoRobotModelSetException("No robot model set"); + } + + if (!traj_tail_) + { + traj_tail_ = other; + // Reserve space in container for new trajectory + traj_cont_.emplace_back(new robot_trajectory::RobotTrajectory(model_, other->getGroupName())); + return; + } + + // Create new trajectory for every group change + if (other->getGroupName() != traj_tail_->getGroupName()) + { + appendWithStrictTimeIncrease(*(traj_cont_.back()), *traj_tail_); + traj_tail_ = other; + // Create new container element + traj_cont_.emplace_back(new robot_trajectory::RobotTrajectory(model_, other->getGroupName())); + return; + } + + // No blending + if (blend_radius <= 0.0) + { + appendWithStrictTimeIncrease(*(traj_cont_.back()), *traj_tail_); + traj_tail_ = other; + return; + } + + blend(other, blend_radius); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp new file mode 100644 index 0000000000..9a071bba58 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp @@ -0,0 +1,64 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/planning_context_loader.h" +#include + +pilz_industrial_motion_planner::PlanningContextLoader::PlanningContextLoader() : limits_set_(false), model_set_(false) +{ +} + +pilz_industrial_motion_planner::PlanningContextLoader::~PlanningContextLoader() +{ +} + +bool pilz_industrial_motion_planner::PlanningContextLoader::setModel(const moveit::core::RobotModelConstPtr& model) +{ + model_ = model; + model_set_ = true; + return true; +} + +bool pilz_industrial_motion_planner::PlanningContextLoader::setLimits( + const pilz_industrial_motion_planner::LimitsContainer& limits) +{ + limits_ = limits; + limits_set_ = true; + return true; +} + +std::string pilz_industrial_motion_planner::PlanningContextLoader::getAlgorithm() const +{ + return alg_; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp new file mode 100644 index 0000000000..0234b989dc --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp @@ -0,0 +1,75 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/planning_context_loader_circ.h" +#include "moveit/planning_scene/planning_scene.h" +#include "pilz_industrial_motion_planner/planning_context_base.h" +#include "pilz_industrial_motion_planner/planning_context_circ.h" + +#include + +pilz_industrial_motion_planner::PlanningContextLoaderCIRC::PlanningContextLoaderCIRC() +{ + alg_ = "CIRC"; +} + +pilz_industrial_motion_planner::PlanningContextLoaderCIRC::~PlanningContextLoaderCIRC() +{ +} + +bool pilz_industrial_motion_planner::PlanningContextLoaderCIRC::loadContext( + planning_interface::PlanningContextPtr& planning_context, const std::string& name, const std::string& group) const +{ + if (limits_set_ && model_set_) + { + planning_context.reset(new PlanningContextCIRC(name, group, model_, limits_)); + return true; + } + else + { + if (!limits_set_) + { + ROS_ERROR_STREAM("Limits are not defined. Cannot load planning context. " + "Call setLimits loadContext"); + } + if (!model_set_) + { + ROS_ERROR_STREAM("Robot model was not set"); + } + return false; + } +} + +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::PlanningContextLoaderCIRC, + pilz_industrial_motion_planner::PlanningContextLoader) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp new file mode 100644 index 0000000000..85cf3b6fe9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp @@ -0,0 +1,75 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/planning_context_loader_lin.h" +#include "moveit/planning_scene/planning_scene.h" +#include "pilz_industrial_motion_planner/planning_context_base.h" +#include "pilz_industrial_motion_planner/planning_context_lin.h" + +#include + +pilz_industrial_motion_planner::PlanningContextLoaderLIN::PlanningContextLoaderLIN() +{ + alg_ = "LIN"; +} + +pilz_industrial_motion_planner::PlanningContextLoaderLIN::~PlanningContextLoaderLIN() +{ +} + +bool pilz_industrial_motion_planner::PlanningContextLoaderLIN::loadContext( + planning_interface::PlanningContextPtr& planning_context, const std::string& name, const std::string& group) const +{ + if (limits_set_ && model_set_) + { + planning_context.reset(new PlanningContextLIN(name, group, model_, limits_)); + return true; + } + else + { + if (!limits_set_) + { + ROS_ERROR_STREAM("Limits are not defined. Cannot load planning context. " + "Call setLimits loadContext"); + } + if (!model_set_) + { + ROS_ERROR_STREAM("Robot model was not set"); + } + return false; + } +} + +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::PlanningContextLoaderLIN, + pilz_industrial_motion_planner::PlanningContextLoader) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp new file mode 100644 index 0000000000..5c72e69cec --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/planning_context_loader_ptp.h" +#include "moveit/planning_scene/planning_scene.h" +#include "pilz_industrial_motion_planner/planning_context_ptp.h" + +#include + +pilz_industrial_motion_planner::PlanningContextLoaderPTP::PlanningContextLoaderPTP() +{ + alg_ = "PTP"; +} + +pilz_industrial_motion_planner::PlanningContextLoaderPTP::~PlanningContextLoaderPTP() +{ +} + +bool pilz_industrial_motion_planner::PlanningContextLoaderPTP::loadContext( + planning_interface::PlanningContextPtr& planning_context, const std::string& name, const std::string& group) const +{ + if (limits_set_ && model_set_) + { + planning_context.reset(new PlanningContextPTP(name, group, model_, limits_)); + return true; + } + else + { + if (!limits_set_) + { + ROS_ERROR_STREAM("Joint Limits are not defined. Cannot load planning " + "context. Call setLimits loadContext"); + } + if (!model_set_) + { + ROS_ERROR_STREAM("Robot model was not set"); + } + return false; + } +} + +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::PlanningContextLoaderPTP, + pilz_industrial_motion_planner::PlanningContextLoader) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp new file mode 100644 index 0000000000..94c108ced8 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -0,0 +1,301 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/trajectory_blender_transition_window.h" + +#include +#include +#include +#include + +bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + pilz_industrial_motion_planner::TrajectoryBlendResponse& res) +{ + ROS_INFO("Start trajectory blending using transition window."); + + double sampling_time = 0.; + if (!validateRequest(req, sampling_time, res.error_code)) + { + ROS_ERROR("Trajectory blend request is not valid."); + return false; + } + + // search for intersection points of the two trajectories with the blending + // sphere + // intersection points belongs to blend trajectory after blending + std::size_t first_intersection_index; + std::size_t second_intersection_index; + if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index)) + { + ROS_ERROR("Blend radius to large."); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + return false; + } + + // Select blending period and adjust the start and end point of the blend + // phase + std::size_t blend_align_index; + determineTrajectoryAlignment(req, first_intersection_index, second_intersection_index, blend_align_index); + + // blend the trajectories in Cartesian space + pilz_industrial_motion_planner::CartesianTrajectory blend_trajectory_cartesian; + blendTrajectoryCartesian(req, first_intersection_index, second_intersection_index, blend_align_index, sampling_time, + blend_trajectory_cartesian); + + // generate the blending trajectory in joint space + std::map initial_joint_position, initial_joint_velocity; + for (const std::string& joint_name : + req.first_trajectory->getFirstWayPointPtr()->getJointModelGroup(req.group_name)->getActiveJointModelNames()) + { + initial_joint_position[joint_name] = + req.first_trajectory->getWayPoint(first_intersection_index - 1).getVariablePosition(joint_name); + initial_joint_velocity[joint_name] = + req.first_trajectory->getWayPoint(first_intersection_index - 1).getVariableVelocity(joint_name); + } + trajectory_msgs::JointTrajectory blend_joint_trajectory; + moveit_msgs::MoveItErrorCodes error_code; + if (!generateJointTrajectory(req.first_trajectory->getFirstWayPointPtr()->getRobotModel(), + limits_.getJointLimitContainer(), blend_trajectory_cartesian, req.group_name, + req.link_name, initial_joint_position, initial_joint_velocity, blend_joint_trajectory, + error_code, true)) + { + // LCOV_EXCL_START + ROS_INFO("Failed to generate joint trajectory for blending trajectory."); + res.error_code.val = error_code.val; + return false; + // LCOV_EXCL_STOP + } + + res.first_trajectory = std::shared_ptr( + new robot_trajectory::RobotTrajectory(req.first_trajectory->getRobotModel(), req.first_trajectory->getGroup())); + res.blend_trajectory = std::shared_ptr( + new robot_trajectory::RobotTrajectory(req.first_trajectory->getRobotModel(), req.first_trajectory->getGroup())); + res.second_trajectory = std::shared_ptr( + new robot_trajectory::RobotTrajectory(req.first_trajectory->getRobotModel(), req.first_trajectory->getGroup())); + + // set the three trajectories after blending in response + // erase the points [first_intersection_index, back()] from the first + // trajectory + for (size_t i = 0; i < first_intersection_index; ++i) + { + res.first_trajectory->insertWayPoint(i, req.first_trajectory->getWayPoint(i), + req.first_trajectory->getWayPointDurationFromPrevious(i)); + } + + // append the blend trajectory + res.blend_trajectory->setRobotTrajectoryMsg(req.first_trajectory->getFirstWayPoint(), blend_joint_trajectory); + // copy the points [second_intersection_index, len] from the second trajectory + for (size_t i = second_intersection_index + 1; i < req.second_trajectory->getWayPointCount(); ++i) + { + res.second_trajectory->insertWayPoint(i - (second_intersection_index + 1), req.second_trajectory->getWayPoint(i), + req.second_trajectory->getWayPointDurationFromPrevious(i)); + } + + // adjust the time from start + res.second_trajectory->setWayPointDurationFromPrevious(0, sampling_time); + + res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + return true; +} + +bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validateRequest( + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time, + moveit_msgs::MoveItErrorCodes& error_code) const +{ + ROS_DEBUG("Validate the trajectory blend request."); + + // check planning group + if (!req.first_trajectory->getRobotModel()->hasJointModelGroup(req.group_name)) + { + ROS_ERROR_STREAM("Unknown planning group: " << req.group_name); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; + return false; + } + + // check link exists + if (!req.first_trajectory->getRobotModel()->hasLinkModel(req.link_name) && + !req.first_trajectory->getLastWayPoint().hasAttachedBody(req.link_name)) + { + ROS_ERROR_STREAM("Unknown link name: " << req.link_name); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME; + return false; + } + + if (req.blend_radius <= 0) + { + ROS_ERROR("Blending radius must be positive"); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + return false; + } + + // end position of the first trajectory and start position of second + // trajectory must be the same + if (!pilz_industrial_motion_planner::isRobotStateEqual( + req.first_trajectory->getLastWayPoint(), req.second_trajectory->getFirstWayPoint(), req.group_name, EPSILON)) + { + ROS_ERROR_STREAM("During blending the last point of the preceding and the first point of the succeding trajectory"); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + return false; + } + + // same uniform sampling time + if (!pilz_industrial_motion_planner::determineAndCheckSamplingTime(req.first_trajectory, req.second_trajectory, + EPSILON, sampling_time)) + { + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + return false; + } + + // end position of the first trajectory and start position of second + // trajectory must have zero + // velocities/accelerations + if (!pilz_industrial_motion_planner::isRobotStateStationary(req.first_trajectory->getLastWayPoint(), req.group_name, + EPSILON) || + !pilz_industrial_motion_planner::isRobotStateStationary(req.second_trajectory->getFirstWayPoint(), req.group_name, + EPSILON)) + { + ROS_ERROR("Intersection point of the blending trajectories has non-zero " + "velocities/accelerations."); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + return false; + } + + return true; +} + +void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTrajectoryCartesian( + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, const std::size_t first_interse_index, + const std::size_t second_interse_index, const std::size_t blend_align_index, double sampling_time, + pilz_industrial_motion_planner::CartesianTrajectory& trajectory) const +{ + // other fields of the trajectory + trajectory.group_name = req.group_name; + trajectory.link_name = req.link_name; + + // Pose on first trajectory + Eigen::Isometry3d blend_sample_pose1 = + req.first_trajectory->getWayPoint(first_interse_index).getFrameTransform(req.link_name); + + // Pose on second trajectory + Eigen::Isometry3d blend_sample_pose2 = + req.second_trajectory->getWayPoint(second_interse_index).getFrameTransform(req.link_name); + + // blend the trajectory + double blend_sample_num = second_interse_index + blend_align_index - first_interse_index + 1; + pilz_industrial_motion_planner::CartesianTrajectoryPoint waypoint; + blend_sample_pose2 = req.second_trajectory->getFirstWayPoint().getFrameTransform(req.link_name); + + // Pose on blending trajectory + Eigen::Isometry3d blend_sample_pose; + for (std::size_t i = 0; i < blend_sample_num; ++i) + { + // if the first trajectory does not reach the last sample, update + if ((first_interse_index + i) < req.first_trajectory->getWayPointCount()) + { + blend_sample_pose1 = req.first_trajectory->getWayPoint(first_interse_index + i).getFrameTransform(req.link_name); + } + + // if after the alignment, the second trajectory starts, update + if ((first_interse_index + i) > blend_align_index) + { + blend_sample_pose2 = req.second_trajectory->getWayPoint(first_interse_index + i - blend_align_index) + .getFrameTransform(req.link_name); + } + + double s = (i + 1) / blend_sample_num; + double alpha = 6 * std::pow(s, 5) - 15 * std::pow(s, 4) + 10 * std::pow(s, 3); + + // blend the translation + blend_sample_pose.translation() = blend_sample_pose1.translation() + + alpha * (blend_sample_pose2.translation() - blend_sample_pose1.translation()); + + // blend the orientation + Eigen::Quaterniond start_quat(blend_sample_pose1.rotation()); + Eigen::Quaterniond end_quat(blend_sample_pose2.rotation()); + blend_sample_pose.linear() = start_quat.slerp(alpha, end_quat).toRotationMatrix(); + + // push to the trajectory + waypoint.pose = tf2::toMsg(blend_sample_pose); + waypoint.time_from_start = ros::Duration((i + 1.0) * sampling_time); + trajectory.points.push_back(waypoint); + } +} + +bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::searchIntersectionPoints( + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, std::size_t& first_interse_index, + std::size_t& second_interse_index) const +{ + ROS_INFO("Search for start and end point of blending trajectory."); + + // compute the position of the center of the blend sphere + // (last point of the first trajectory, first point of the second trajectory) + Eigen::Isometry3d circ_pose = req.first_trajectory->getLastWayPoint().getFrameTransform(req.link_name); + + // Searh for intersection points according to distance + if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.first_trajectory, + true, first_interse_index)) + { + ROS_ERROR_STREAM("Intersection point of first trajectory not found."); + return false; + } + ROS_INFO_STREAM("Intersection point of first trajectory found, index: " << first_interse_index); + + if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.second_trajectory, + false, second_interse_index)) + { + ROS_ERROR_STREAM("Intersection point of second trajectory not found."); + return false; + } + + ROS_INFO_STREAM("Intersection point of second trajectory found, index: " << second_interse_index); + return true; +} + +void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::determineTrajectoryAlignment( + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, std::size_t first_interse_index, + std::size_t second_interse_index, std::size_t& blend_align_index) const +{ + size_t way_point_count_1 = req.first_trajectory->getWayPointCount() - first_interse_index; + size_t way_point_count_2 = second_interse_index + 1; + + if (way_point_count_1 > way_point_count_2) + { + blend_align_index = req.first_trajectory->getWayPointCount() - second_interse_index - 1; + } + else + { + blend_align_index = first_interse_index; + } +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp new file mode 100644 index 0000000000..5d37a4d4ea --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -0,0 +1,584 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/trajectory_functions.h" + +#include +#include +#include +#include +#include + +bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotModelConstPtr& robot_model, + const std::string& group_name, const std::string& link_name, + const Eigen::Isometry3d& pose, const std::string& frame_id, + const std::map& seed, + std::map& solution, bool check_self_collision, + const double timeout) +{ + if (!robot_model->hasJointModelGroup(group_name)) + { + ROS_ERROR_STREAM("Robot model has no planning group named as " << group_name); + return false; + } + + if (!robot_model->getJointModelGroup(group_name)->canSetStateFromIK(link_name)) + { + ROS_ERROR_STREAM("No valid IK solver exists for " << link_name << " in planning group " << group_name); + return false; + } + + if (frame_id != robot_model->getModelFrame()) + { + ROS_ERROR_STREAM("Given frame (" << frame_id << ") is unequal to model frame(" << robot_model->getModelFrame() + << ")"); + return false; + } + + robot_state::RobotState rstate(robot_model); + // By setting the robot state to default values, we basically allow + // the user of this function to supply an incomplete or even empty seed. + rstate.setToDefaultValues(); + rstate.setVariablePositions(seed); + + moveit::core::GroupStateValidityCallbackFn ik_constraint_function; + ik_constraint_function = + boost::bind(&pilz_industrial_motion_planner::isStateColliding, check_self_collision, robot_model, _1, _2, _3); + + // call ik + if (rstate.setFromIK(robot_model->getJointModelGroup(group_name), pose, link_name, timeout, ik_constraint_function)) + { + // copy the solution + for (const auto& joint_name : robot_model->getJointModelGroup(group_name)->getActiveJointModelNames()) + { + solution[joint_name] = rstate.getVariablePosition(joint_name); + } + return true; + } + else + { + ROS_ERROR_STREAM("Inverse kinematics for pose \n" << pose.translation() << " has no solution."); + return false; + } +} + +bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotModelConstPtr& robot_model, + const std::string& group_name, const std::string& link_name, + const geometry_msgs::Pose& pose, const std::string& frame_id, + const std::map& seed, + std::map& solution, bool check_self_collision, + const double timeout) +{ + Eigen::Isometry3d pose_eigen; + tf2::convert(pose, pose_eigen); + return computePoseIK(robot_model, group_name, link_name, pose_eigen, frame_id, seed, solution, check_self_collision, + timeout); +} + +bool pilz_industrial_motion_planner::computeLinkFK(const moveit::core::RobotModelConstPtr& robot_model, + const std::string& link_name, + const std::map& joint_state, + Eigen::Isometry3d& pose) +{ // create robot state + robot_state::RobotState rstate(robot_model); + + // check the reference frame of the target pose + if (!rstate.knowsFrameTransform(link_name)) + { + ROS_ERROR_STREAM("The target link " << link_name << " is not known by robot."); + return false; + } + + // set the joint positions + rstate.setToDefaultValues(); + rstate.setVariablePositions(joint_state); + + // update the frame + rstate.update(); + pose = rstate.getFrameTransform(link_name); + + return true; +} + +bool pilz_industrial_motion_planner::verifySampleJointLimits( + const std::map& position_last, const std::map& velocity_last, + const std::map& position_current, double duration_last, double duration_current, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + const double epsilon = 10e-6; + if (duration_current <= epsilon) + { + ROS_ERROR("Sample duration too small, cannot compute the velocity"); + return false; + } + + double velocity_current, acceleration_current; + + for (const auto& pos : position_current) + { + velocity_current = (pos.second - position_last.at(pos.first)) / duration_current; + + if (!joint_limits.verifyVelocityLimit(pos.first, velocity_current)) + { + ROS_ERROR_STREAM("Joint velocity limit of " << pos.first << " violated. Set the velocity scaling factor lower!" + << " Actual joint velocity is " << velocity_current + << ", while the limit is " + << joint_limits.getLimit(pos.first).max_velocity << ". "); + return false; + } + + acceleration_current = (velocity_current - velocity_last.at(pos.first)) / (duration_last + duration_current) * 2; + // acceleration case + if (fabs(velocity_last.at(pos.first)) <= fabs(velocity_current)) + { + if (joint_limits.getLimit(pos.first).has_acceleration_limits && + fabs(acceleration_current) > fabs(joint_limits.getLimit(pos.first).max_acceleration)) + { + ROS_ERROR_STREAM("Joint acceleration limit of " + << pos.first << " violated. Set the acceleration scaling factor lower!" + << " Actual joint acceleration is " << acceleration_current << ", while the limit is " + << joint_limits.getLimit(pos.first).max_acceleration << ". "); + return false; + } + } + // deceleration case + else + { + if (joint_limits.getLimit(pos.first).has_deceleration_limits && + fabs(acceleration_current) > fabs(joint_limits.getLimit(pos.first).max_deceleration)) + { + ROS_ERROR_STREAM("Joint deceleration limit of " + << pos.first << " violated. Set the acceleration scaling factor lower!" + << " Actual joint deceleration is " << acceleration_current << ", while the limit is " + << joint_limits.getLimit(pos.first).max_deceleration << ". "); + return false; + } + } + } + + return true; +} + +bool pilz_industrial_motion_planner::generateJointTrajectory( + const moveit::core::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, + const std::string& group_name, const std::string& link_name, + const std::map& initial_joint_position, const double& sampling_time, + trajectory_msgs::JointTrajectory& joint_trajectory, moveit_msgs::MoveItErrorCodes& error_code, + bool check_self_collision) +{ + ROS_DEBUG("Generate joint trajectory from a Cartesian trajectory."); + + ros::Time generation_begin = ros::Time::now(); + + // generate the time samples + const double epsilon = 10e-06; // avoid adding the last time sample twice + std::vector time_samples; + for (double t_sample = 0.0; t_sample < trajectory.Duration() - epsilon; t_sample += sampling_time) + { + time_samples.push_back(t_sample); + } + time_samples.push_back(trajectory.Duration()); + + // sample the trajectory and solve the inverse kinematics + Eigen::Isometry3d pose_sample; + std::map ik_solution_last, ik_solution, joint_velocity_last; + ik_solution_last = initial_joint_position; + for (const auto& item : ik_solution_last) + { + joint_velocity_last[item.first] = 0.0; + } + + for (std::vector::const_iterator time_iter = time_samples.begin(); time_iter != time_samples.end(); + ++time_iter) + { + tf::transformKDLToEigen(trajectory.Pos(*time_iter), pose_sample); + + if (!computePoseIK(robot_model, group_name, link_name, pose_sample, robot_model->getModelFrame(), ik_solution_last, + ik_solution, check_self_collision)) + { + ROS_ERROR("Failed to compute inverse kinematics solution for sampled " + "Cartesian pose."); + error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; + joint_trajectory.points.clear(); + return false; + } + + // check the joint limits + double duration_current_sample = sampling_time; + // last interval can be shorter than the sampling time + if (time_iter == (time_samples.end() - 1) && time_samples.size() > 1) + { + duration_current_sample = *time_iter - *(time_iter - 1); + } + if (time_samples.size() == 1) + { + duration_current_sample = *time_iter; + } + + // skip the first sample with zero time from start for limits checking + if (time_iter != time_samples.begin() && + !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, sampling_time, + duration_current_sample, joint_limits)) + { + ROS_ERROR_STREAM("Inverse kinematics solution at " + << *time_iter << "s violates the joint velocity/acceleration/deceleration limits."); + error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; + joint_trajectory.points.clear(); + return false; + } + + // fill the point with joint values + trajectory_msgs::JointTrajectoryPoint point; + + // set joint names + joint_trajectory.joint_names.clear(); + for (const auto& start_joint : initial_joint_position) + { + joint_trajectory.joint_names.push_back(start_joint.first); + } + + point.time_from_start = ros::Duration(*time_iter); + for (const auto& joint_name : joint_trajectory.joint_names) + { + point.positions.push_back(ik_solution.at(joint_name)); + + if (time_iter != time_samples.begin() && time_iter != time_samples.end() - 1) + { + double joint_velocity = + (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current_sample; + point.velocities.push_back(joint_velocity); + point.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) / + (duration_current_sample + sampling_time) * 2); + joint_velocity_last[joint_name] = joint_velocity; + } + else + { + point.velocities.push_back(0.); + point.accelerations.push_back(0.); + joint_velocity_last[joint_name] = 0.; + } + } + + // update joint trajectory + joint_trajectory.points.push_back(point); + ik_solution_last = ik_solution; + } + + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + double duration_ms = (ros::Time::now() - generation_begin).toSec() * 1000; + ROS_DEBUG_STREAM("Generate trajectory (N-Points: " << joint_trajectory.points.size() << ") took " << duration_ms + << " ms | " << duration_ms / joint_trajectory.points.size() + << " ms per Point"); + + return true; +} + +bool pilz_industrial_motion_planner::generateJointTrajectory( + const moveit::core::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, + const pilz_industrial_motion_planner::CartesianTrajectory& trajectory, const std::string& group_name, + const std::string& link_name, const std::map& initial_joint_position, + const std::map& initial_joint_velocity, trajectory_msgs::JointTrajectory& joint_trajectory, + moveit_msgs::MoveItErrorCodes& error_code, bool check_self_collision) +{ + ROS_DEBUG("Generate joint trajectory from a Cartesian trajectory."); + + ros::Time generation_begin = ros::Time::now(); + + std::map ik_solution_last = initial_joint_position; + std::map joint_velocity_last = initial_joint_velocity; + double duration_last = 0; + double duration_current = 0; + joint_trajectory.joint_names.clear(); + for (const auto& joint_position : ik_solution_last) + { + joint_trajectory.joint_names.push_back(joint_position.first); + } + std::map ik_solution; + for (size_t i = 0; i < trajectory.points.size(); ++i) + { + // compute inverse kinematics + if (!computePoseIK(robot_model, group_name, link_name, trajectory.points.at(i).pose, robot_model->getModelFrame(), + ik_solution_last, ik_solution, check_self_collision)) + { + ROS_ERROR("Failed to compute inverse kinematics solution for sampled " + "Cartesian pose."); + error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; + joint_trajectory.points.clear(); + return false; + } + + // verify the joint limits + if (i == 0) + { + duration_current = trajectory.points.front().time_from_start.toSec(); + duration_last = duration_current; + } + else + { + duration_current = + trajectory.points.at(i).time_from_start.toSec() - trajectory.points.at(i - 1).time_from_start.toSec(); + } + + if (!verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, duration_last, duration_current, + joint_limits)) + { + // LCOV_EXCL_START since the same code was captured in a test in the other + // overload generateJointTrajectory(..., + // KDL::Trajectory, ...) + // TODO: refactor to avoid code duplication. + ROS_ERROR_STREAM("Inverse kinematics solution of the " << i + << "th sample violates the joint " + "velocity/acceleration/deceleration limits."); + error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; + joint_trajectory.points.clear(); + return false; + // LCOV_EXCL_STOP + } + + // compute the waypoint + trajectory_msgs::JointTrajectoryPoint waypoint_joint; + waypoint_joint.time_from_start = ros::Duration(trajectory.points.at(i).time_from_start); + for (const auto& joint_name : joint_trajectory.joint_names) + { + waypoint_joint.positions.push_back(ik_solution.at(joint_name)); + double joint_velocity = (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current; + waypoint_joint.velocities.push_back(joint_velocity); + waypoint_joint.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) / + (duration_current + duration_last) * 2); + // update the joint velocity + joint_velocity_last[joint_name] = joint_velocity; + } + + // update joint trajectory + joint_trajectory.points.push_back(waypoint_joint); + ik_solution_last = ik_solution; + duration_last = duration_current; + } + + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + + double duration_ms = (ros::Time::now() - generation_begin).toSec() * 1000; + ROS_DEBUG_STREAM("Generate trajectory (N-Points: " << joint_trajectory.points.size() << ") took " << duration_ms + << " ms | " << duration_ms / joint_trajectory.points.size() + << " ms per Point"); + + return true; +} + +bool pilz_industrial_motion_planner::determineAndCheckSamplingTime( + const robot_trajectory::RobotTrajectoryPtr& first_trajectory, + const robot_trajectory::RobotTrajectoryPtr& second_trajectory, double epsilon, double& sampling_time) +{ + // The last sample is ignored because it is allowed to violate the sampling + // time. + std::size_t n1 = first_trajectory->getWayPointCount() - 1; + std::size_t n2 = second_trajectory->getWayPointCount() - 1; + if ((n1 < 2) && (n2 < 2)) + { + ROS_ERROR_STREAM("Both trajectories do not have enough points to determine " + "sampling time."); + return false; + } + + if (n1 >= 2) + { + sampling_time = first_trajectory->getWayPointDurationFromPrevious(1); + } + else + { + sampling_time = second_trajectory->getWayPointDurationFromPrevious(1); + } + + for (std::size_t i = 1; i < std::max(n1, n2); ++i) + { + if (i < n1) + { + if (fabs(sampling_time - first_trajectory->getWayPointDurationFromPrevious(i)) > epsilon) + { + ROS_ERROR_STREAM("First trajectory violates sampline time " << sampling_time << " between points " << (i - 1) + << "and " << i << " (indices)."); + return false; + } + } + + if (i < n2) + { + if (fabs(sampling_time - second_trajectory->getWayPointDurationFromPrevious(i)) > epsilon) + { + ROS_ERROR_STREAM("Second trajectory violates sampline time " << sampling_time << " between points " << (i - 1) + << "and " << i << " (indices)."); + return false; + } + } + } + + return true; +} + +bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, + const std::string& joint_group_name, double epsilon) +{ + Eigen::VectorXd joint_position_1, joint_position_2; + + state1.copyJointGroupPositions(joint_group_name, joint_position_1); + state2.copyJointGroupPositions(joint_group_name, joint_position_2); + + if ((joint_position_1 - joint_position_2).norm() > epsilon) + { + ROS_DEBUG_STREAM("Joint positions of the two states are different. state1: " << joint_position_1 + << " state2: " << joint_position_2); + return false; + } + + Eigen::VectorXd joint_velocity_1, joint_velocity_2; + + state1.copyJointGroupVelocities(joint_group_name, joint_velocity_1); + state2.copyJointGroupVelocities(joint_group_name, joint_velocity_2); + + if ((joint_velocity_1 - joint_velocity_2).norm() > epsilon) + { + ROS_DEBUG_STREAM("Joint velocities of the two states are different. state1: " << joint_velocity_1 + << " state2: " << joint_velocity_2); + return false; + } + + Eigen::VectorXd joint_acc_1, joint_acc_2; + + state1.copyJointGroupAccelerations(joint_group_name, joint_acc_1); + state2.copyJointGroupAccelerations(joint_group_name, joint_acc_2); + + if ((joint_acc_1 - joint_acc_2).norm() > epsilon) + { + ROS_DEBUG_STREAM("Joint accelerations of the two states are different. state1: " << joint_acc_1 + << " state2: " << joint_acc_2); + return false; + } + + return true; +} + +bool pilz_industrial_motion_planner::isRobotStateStationary(const moveit::core::RobotState& state, + const std::string& group, double EPSILON) +{ + Eigen::VectorXd joint_variable; + state.copyJointGroupVelocities(group, joint_variable); + if (joint_variable.norm() > EPSILON) + { + ROS_DEBUG("Joint velocities are not zero."); + return false; + } + state.copyJointGroupAccelerations(group, joint_variable); + if (joint_variable.norm() > EPSILON) + { + ROS_DEBUG("Joint accelerations are not zero."); + return false; + } + return true; +} + +bool pilz_industrial_motion_planner::linearSearchIntersectionPoint(const std::string& link_name, + const Eigen::Vector3d& center_position, + const double& r, + const robot_trajectory::RobotTrajectoryPtr& traj, + bool inverseOrder, std::size_t& index) +{ + ROS_DEBUG("Start linear search for intersection point."); + + const size_t waypoint_num = traj->getWayPointCount(); + + if (inverseOrder) + { + for (size_t i = waypoint_num - 1; i > 0; --i) + { + if (intersectionFound(center_position, traj->getWayPointPtr(i)->getFrameTransform(link_name).translation(), + traj->getWayPointPtr(i - 1)->getFrameTransform(link_name).translation(), r)) + { + index = i; + return true; + } + } + } + else + { + for (size_t i = 0; i < waypoint_num - 1; ++i) + { + if (intersectionFound(center_position, traj->getWayPointPtr(i)->getFrameTransform(link_name).translation(), + traj->getWayPointPtr(i + 1)->getFrameTransform(link_name).translation(), r)) + { + index = i; + return true; + } + } + } + + return false; +} + +bool pilz_industrial_motion_planner::intersectionFound(const Eigen::Vector3d& p_center, + const Eigen::Vector3d& p_current, const Eigen::Vector3d& p_next, + const double& r) +{ + return ((p_current - p_center).norm() <= r) && ((p_next - p_center).norm() >= r); +} + +bool pilz_industrial_motion_planner::isStateColliding(const bool test_for_self_collision, + const moveit::core::RobotModelConstPtr& robot_model, + robot_state::RobotState* rstate, + const robot_state::JointModelGroup* const group, + const double* const ik_solution) +{ + if (!test_for_self_collision) + { + return true; + } + + rstate->setJointGroupPositions(group, ik_solution); + rstate->update(); + collision_detection::CollisionRequest collision_req; + collision_req.group_name = group->getName(); + collision_detection::CollisionResult collision_res; + planning_scene::PlanningScene(robot_model).checkSelfCollision(collision_req, collision_res, *rstate); + + return !collision_res.collision; +} + +void normalizeQuaternion(geometry_msgs::Quaternion& quat) +{ + tf2::Quaternion q; + tf2::convert(quat, q); + quat = tf2::toMsg(q.normalize()); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp new file mode 100644 index 0000000000..8069004d2c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -0,0 +1,332 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/trajectory_generator.h" + +#include + +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/limits_container.h" + +namespace pilz_industrial_motion_planner +{ +void TrajectoryGenerator::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& /*req*/) const +{ + // Empty implementation, in case the derived class does not want + // to provide a command specific request validation. +} + +void TrajectoryGenerator::checkVelocityScaling(const double& scaling_factor) +{ + if (!isScalingFactorValid(scaling_factor)) + { + std::ostringstream os; + os << "Velocity scaling not in range [" << MIN_SCALING_FACTOR << ", " << MAX_SCALING_FACTOR << "], " + << "actual value is: " << scaling_factor; + throw VelocityScalingIncorrect(os.str()); + } +} + +void TrajectoryGenerator::checkAccelerationScaling(const double& scaling_factor) +{ + if (!isScalingFactorValid(scaling_factor)) + { + std::ostringstream os; + os << "Acceleration scaling not in range [" << MIN_SCALING_FACTOR << ", " << MAX_SCALING_FACTOR << "], " + << "actual value is: " << scaling_factor; + throw AccelerationScalingIncorrect(os.str()); + } +} + +void TrajectoryGenerator::checkForValidGroupName(const std::string& group_name) const +{ + if (!robot_model_->hasJointModelGroup(group_name)) + { + std::ostringstream os; + os << "Unknown planning group: " << group_name; + throw UnknownPlanningGroup(os.str()); + } +} + +void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_state) const +{ + if (start_state.joint_state.name.empty()) + { + throw NoJointNamesInStartState("No joint names for state state given"); + } + + if (start_state.joint_state.name.size() != start_state.joint_state.position.size()) + { + throw SizeMismatchInStartState("Joint state name and position do not match in start state"); + } + + if (!planner_limits_.getJointLimitContainer().verifyPositionLimits(start_state.joint_state.name, + start_state.joint_state.position)) + { + throw JointsOfStartStateOutOfRange("Joint state out of range in start state"); + } + + // does not allow start velocity + if (!std::all_of(start_state.joint_state.velocity.begin(), start_state.joint_state.velocity.end(), + [this](double v) { return std::fabs(v) < this->VELOCITY_TOLERANCE; })) + { + throw NonZeroVelocityInStartState("Trajectory Generator does not allow non-zero start velocity"); + } +} + +void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::Constraints& constraint, + const std::vector& expected_joint_names, + const std::string& group_name) const +{ + for (auto const& joint_constraint : constraint.joint_constraints) + { + const std::string& curr_joint_name{ joint_constraint.joint_name }; + if (std::find(expected_joint_names.cbegin(), expected_joint_names.cend(), curr_joint_name) == + expected_joint_names.cend()) + { + std::ostringstream os; + os << "Cannot find joint \"" << curr_joint_name << "\" from start state in goal constraint"; + throw StartStateGoalStateMismatch(os.str()); + } + + if (!robot_model_->getJointModelGroup(group_name)->hasJointModel(curr_joint_name)) + { + std::ostringstream os; + os << "Joint \"" << curr_joint_name << "\" does not belong to group \"" << group_name << "\""; + throw JointConstraintDoesNotBelongToGroup(os.str()); + } + + if (!planner_limits_.getJointLimitContainer().verifyPositionLimit(curr_joint_name, joint_constraint.position)) + { + std::ostringstream os; + os << "Joint \"" << curr_joint_name << "\" violates joint limits in goal constraints"; + throw JointsOfGoalOutOfRange(os.str()); + } + } +} + +void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::Constraints& constraint, + const std::string& group_name) const +{ + assert(constraint.position_constraints.size() == 1); + assert(constraint.orientation_constraints.size() == 1); + const moveit_msgs::PositionConstraint& pos_constraint{ constraint.position_constraints.front() }; + const moveit_msgs::OrientationConstraint& ori_constraint{ constraint.orientation_constraints.front() }; + + if (pos_constraint.link_name.empty()) + { + throw PositionConstraintNameMissing("Link name of position constraint missing"); + } + + if (ori_constraint.link_name.empty()) + { + throw OrientationConstraintNameMissing("Link name of orientation constraint missing"); + } + + if (pos_constraint.link_name != ori_constraint.link_name) + { + std::ostringstream os; + os << "Position and orientation constraint name do not match" + << "(Position constraint name: \"" << pos_constraint.link_name << "\" | Orientation constraint name: \"" + << ori_constraint.link_name << "\")"; + throw PositionOrientationConstraintNameMismatch(os.str()); + } + + if (!robot_model_->getJointModelGroup(group_name)->canSetStateFromIK(pos_constraint.link_name)) + { + std::ostringstream os; + os << "No IK solver available for link: \"" << pos_constraint.link_name << "\""; + throw NoIKSolverAvailable(os.str()); + } + + if (pos_constraint.constraint_region.primitive_poses.empty()) + { + throw NoPrimitivePoseGiven("Primitive pose in position constraints of goal missing"); + } +} + +void TrajectoryGenerator::checkGoalConstraints( + const moveit_msgs::MotionPlanRequest::_goal_constraints_type& goal_constraints, + const std::vector& expected_joint_names, const std::string& group_name) const +{ + if (goal_constraints.size() != 1) + { + std::ostringstream os; + os << "Exaclty one goal constraint required, but " << goal_constraints.size() << " goal constraints given"; + throw NotExactlyOneGoalConstraintGiven(os.str()); + } + + const moveit_msgs::Constraints& goal_con{ goal_constraints.front() }; + if (!isOnlyOneGoalTypeGiven(goal_con)) + { + throw OnlyOneGoalTypeAllowed("Only cartesian XOR joint goal allowed"); + } + + if (isJointGoalGiven(goal_con)) + { + checkJointGoalConstraint(goal_con, expected_joint_names, group_name); + } + else + { + checkCartesianGoalConstraint(goal_con, group_name); + } +} + +void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRequest& req) const +{ + checkVelocityScaling(req.max_velocity_scaling_factor); + checkAccelerationScaling(req.max_acceleration_scaling_factor); + checkForValidGroupName(req.group_name); + checkStartState(req.start_state); + checkGoalConstraints(req.goal_constraints, req.start_state.joint_state.name, req.group_name); +} + +void TrajectoryGenerator::convertToRobotTrajectory(const trajectory_msgs::JointTrajectory& joint_trajectory, + const moveit_msgs::RobotState& start_state, + robot_trajectory::RobotTrajectory& robot_trajectory) const +{ + moveit::core::RobotState start_rs(robot_model_); + start_rs.setToDefaultValues(); + moveit::core::robotStateMsgToRobotState(start_state, start_rs, false); + robot_trajectory.setRobotTrajectoryMsg(start_rs, joint_trajectory); +} + +void TrajectoryGenerator::setSuccessResponse(const std::string& group_name, const moveit_msgs::RobotState& start_state, + const trajectory_msgs::JointTrajectory& joint_trajectory, + const ros::Time& planning_start, + planning_interface::MotionPlanResponse& res) const +{ + robot_trajectory::RobotTrajectoryPtr rt(new robot_trajectory::RobotTrajectory(robot_model_, group_name)); + convertToRobotTrajectory(joint_trajectory, start_state, *rt); + + res.trajectory_ = rt; + res.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + res.planning_time_ = (ros::Time::now() - planning_start).toSec(); +} + +void TrajectoryGenerator::setFailureResponse(const ros::Time& planning_start, + planning_interface::MotionPlanResponse& res) const +{ + if (res.trajectory_) + { + res.trajectory_->clear(); + } + res.planning_time_ = (ros::Time::now() - planning_start).toSec(); +} + +std::unique_ptr +TrajectoryGenerator::cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor, + const double& max_acceleration_scaling_factor, + const std::unique_ptr& path) const +{ + std::unique_ptr vp_trans(new KDL::VelocityProfile_Trap( + max_velocity_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalVelocity(), + max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalAcceleration())); + + if (path->PathLength() > std::numeric_limits::epsilon()) // avoid division by zero + { + vp_trans->SetProfile(0, path->PathLength()); + } + else + { + vp_trans->SetProfile(0, std::numeric_limits::epsilon()); + } + return vp_trans; +} + +bool TrajectoryGenerator::generate(const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, double sampling_time) +{ + ROS_INFO_STREAM("Generating " << req.planner_id << " trajectory..."); + ros::Time planning_begin = ros::Time::now(); + + try + { + validateRequest(req); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM(ex.what()); + res.error_code_.val = ex.getErrorCode(); + setFailureResponse(planning_begin, res); + return false; + } + + try + { + cmdSpecificRequestValidation(req); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM(ex.what()); + res.error_code_.val = ex.getErrorCode(); + setFailureResponse(planning_begin, res); + return false; + } + + MotionPlanInfo plan_info; + try + { + extractMotionPlanInfo(req, plan_info); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM(ex.what()); + res.error_code_.val = ex.getErrorCode(); + setFailureResponse(planning_begin, res); + return false; + } + + trajectory_msgs::JointTrajectory joint_trajectory; + try + { + plan(req, plan_info, sampling_time, joint_trajectory); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM(ex.what()); + res.error_code_.val = ex.getErrorCode(); + setFailureResponse(planning_begin, res); + return false; + } + + setSuccessResponse(req.group_name, req.start_state, joint_trajectory, planning_begin, res); + return true; +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp new file mode 100644 index 0000000000..6e1e57f0d1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -0,0 +1,270 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/trajectory_generator_circ.h" +#include "pilz_industrial_motion_planner/path_circle_generator.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +TrajectoryGeneratorCIRC::TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model, + const LimitsContainer& planner_limits) + : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) +{ + if (!planner_limits_.hasFullCartesianLimits()) + { + throw TrajectoryGeneratorInvalidLimitsException( + "Cartesian limits are not fully set for CIRC trajectory generator."); + } +} + +void TrajectoryGeneratorCIRC::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const +{ + if (!(req.path_constraints.name == "interim" || req.path_constraints.name == "center")) + { + std::ostringstream os; + os << "No path constraint named \"interim\" or \"center\" found (found " + "unknown constraint: " + << "\"req.path_constraints.name\"" + << " instead)"; + throw UnknownPathConstraintName(os.str()); + } + + if (req.path_constraints.position_constraints.size() != 1) + { + throw NoPositionConstraints("CIRC trajectory generator needs valid a position constraint"); + } + + if (req.path_constraints.position_constraints.front().constraint_region.primitive_poses.size() != 1) + { + throw NoPrimitivePose("CIRC trajectory generator needs valid a primitive pose"); + } +} + +void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, + TrajectoryGenerator::MotionPlanInfo& info) const +{ + ROS_DEBUG("Extract necessary information from motion plan request."); + + info.group_name = req.group_name; + std::string frame_id{ robot_model_->getModelFrame() }; + + // goal given in joint space + if (!req.goal_constraints.front().joint_constraints.empty()) + { + // TODO: link name from goal constraint and path constraint + info.link_name = req.path_constraints.position_constraints.front().link_name; + if (!robot_model_->hasLinkModel(info.link_name)) + { + throw UnknownLinkNameOfAuxiliaryPoint("Unknown link name of CIRC auxiliary point"); + } + + if (req.goal_constraints.front().joint_constraints.size() != + robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size()) + { + std::ostringstream os; + os << "Number of joint constraint = " << req.goal_constraints.front().joint_constraints.size() + << " not equal to active joints of group = " + << robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size(); + throw NumberOfConstraintsMismatch(os.str()); + } + + // initializing all joints of the model + for (const auto& joint_name : robot_model_->getVariableNames()) + { + info.goal_joint_position[joint_name] = 0; + } + + for (const auto& joint_item : req.goal_constraints.front().joint_constraints) + { + info.goal_joint_position[joint_item.joint_name] = joint_item.position; + } + + computeLinkFK(robot_model_, info.link_name, info.goal_joint_position, info.goal_pose); + } + // goal given in Cartesian space + else + { + info.link_name = req.goal_constraints.front().position_constraints.front().link_name; + if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || + req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) + { + ROS_WARN("Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); + frame_id = robot_model_->getModelFrame(); + } + else + { + frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; + } + geometry_msgs::Pose goal_pose_msg; + goal_pose_msg.position = + req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position; + goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation; + normalizeQuaternion(goal_pose_msg.orientation); + tf2::convert(goal_pose_msg, info.goal_pose); + } + + assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); + for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames()) + { + auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) }; + if (it == req.start_state.joint_state.name.cend()) + { + std::ostringstream os; + os << "Could not find joint \"" << joint_name << "\" of group \"" << req.group_name + << "\" in start state of request"; + throw CircJointMissingInStartState(os.str()); + } + size_t index = it - req.start_state.joint_state.name.cbegin(); + info.start_joint_position[joint_name] = req.start_state.joint_state.position[index]; + } + + computeLinkFK(robot_model_, info.link_name, info.start_joint_position, info.start_pose); + + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(robot_model_, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) + { + // LCOV_EXCL_START + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw CircInverseForGoalIncalculable(os.str()); + // LCOV_EXCL_STOP // not able to trigger here since lots of checks before + // are in place + } + + Eigen::Vector3d circ_path_point; + tf2::convert(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, + circ_path_point); + + info.circ_path_point.first = req.path_constraints.name; + info.circ_path_point.second = circ_path_point; +} + +void TrajectoryGeneratorCIRC::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) +{ + std::unique_ptr cart_path(setPathCIRC(plan_info)); + std::unique_ptr vel_profile( + cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, cart_path)); + + // combine path and velocity profile into Cartesian trajectory + // with the third parameter set to false, KDL::Trajectory_Segment does not + // take + // the ownship of Path and Velocity Profile + KDL::Trajectory_Segment cart_trajectory(cart_path.get(), vel_profile.get(), false); + + moveit_msgs::MoveItErrorCodes error_code; + // sample the Cartesian trajectory and compute joint trajectory using inverse + // kinematics + if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_trajectory, + plan_info.group_name, plan_info.link_name, plan_info.start_joint_position, sampling_time, + joint_trajectory, error_code)) + { + throw CircTrajectoryConversionFailure("Failed to generate valid joint trajectory from the Cartesian path", + error_code.val); + } +} + +std::unique_ptr TrajectoryGeneratorCIRC::setPathCIRC(const MotionPlanInfo& info) const +{ + ROS_DEBUG("Set Cartesian path for CIRC command."); + + KDL::Frame start_pose, goal_pose; + tf::transformEigenToKDL(info.start_pose, start_pose); + tf::transformEigenToKDL(info.goal_pose, goal_pose); + + KDL::Vector path_point; + tf::vectorEigenToKDL(info.circ_path_point.second, path_point); + + // pass the ratio of translational by rotational velocity as equivalent radius + // to get a trajectory with rotational speed, if no (or very little) + // translational distance + // The KDL::Path implementation chooses the motion with the longer duration + // (translation vs. rotation) + // and uses eqradius as scaling factor between the distances. + double eqradius = planner_limits_.getCartesianLimits().getMaxTranslationalVelocity() / + planner_limits_.getCartesianLimits().getMaxRotationalVelocity(); + + try + { + if (info.circ_path_point.first == "center") + { + return PathCircleGenerator::circleFromCenter(start_pose, goal_pose, path_point, eqradius); + } + else // if (info.circ_path_point.first == "interim") + { + return PathCircleGenerator::circleFromInterim(start_pose, goal_pose, path_point, eqradius); + } + } + catch (KDL::Error_MotionPlanning_Circle_No_Plane& e) + { + std::ostringstream os; + os << "Failed to create path object for circle." << e.Description(); + throw CircleNoPlane(os.str()); + } + catch (KDL::Error_MotionPlanning_Circle_ToSmall& e) + { + std::ostringstream os; + os << "Failed to create path object for circle." << e.Description(); + throw CircleToSmall(os.str()); + } + catch (ErrorMotionPlanningCenterPointDifferentRadius& e) + { + std::ostringstream os; + os << "Failed to create path object for circle." << e.Description(); + throw CenterPointDifferentRadius(os.str()); + } + + return nullptr; +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp new file mode 100644 index 0000000000..a91c6e943a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -0,0 +1,203 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/trajectory_generator_lin.h" + +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model, + const LimitsContainer& planner_limits) + : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) +{ + if (!planner_limits_.hasFullCartesianLimits()) + { + ROS_ERROR("Cartesian limits not set for LIN trajectory generator."); + throw TrajectoryGeneratorInvalidLimitsException("Cartesian limits are not fully set for LIN trajectory generator."); + } +} + +void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, + TrajectoryGenerator::MotionPlanInfo& info) const +{ + ROS_DEBUG("Extract necessary information from motion plan request."); + + info.group_name = req.group_name; + std::string frame_id{ robot_model_->getModelFrame() }; + + // goal given in joint space + if (!req.goal_constraints.front().joint_constraints.empty()) + { + info.link_name = robot_model_->getJointModelGroup(req.group_name)->getSolverInstance()->getTipFrame(); + + if (req.goal_constraints.front().joint_constraints.size() != + robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size()) + { + std::ostringstream os; + os << "Number of joints in goal does not match number of joints of group " + "(Number joints goal: " + << req.goal_constraints.front().joint_constraints.size() << " | Number of joints of group: " + << robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size() << ")"; + throw JointNumberMismatch(os.str()); + } + // initializing all joints of the model + for (const auto& joint_name : robot_model_->getVariableNames()) + { + info.goal_joint_position[joint_name] = 0; + } + + for (const auto& joint_item : req.goal_constraints.front().joint_constraints) + { + info.goal_joint_position[joint_item.joint_name] = joint_item.position; + } + + // Ignored return value because at this point the function should always + // return 'true'. + computeLinkFK(robot_model_, info.link_name, info.goal_joint_position, info.goal_pose); + } + // goal given in Cartesian space + else + { + info.link_name = req.goal_constraints.front().position_constraints.front().link_name; + if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || + req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) + { + ROS_WARN("Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); + frame_id = robot_model_->getModelFrame(); + } + else + { + frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; + } + geometry_msgs::Pose goal_pose_msg; + goal_pose_msg.position = + req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position; + goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation; + normalizeQuaternion(goal_pose_msg.orientation); + tf2::convert(goal_pose_msg, info.goal_pose); + } + + assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); + for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames()) + { + auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) }; + if (it == req.start_state.joint_state.name.cend()) + { + std::ostringstream os; + os << "Could not find joint \"" << joint_name << "\" of group \"" << req.group_name + << "\" in start state of request"; + throw LinJointMissingInStartState(os.str()); + } + size_t index = it - req.start_state.joint_state.name.cbegin(); + info.start_joint_position[joint_name] = req.start_state.joint_state.position[index]; + } + + // Ignored return value because at this point the function should always + // return 'true'. + computeLinkFK(robot_model_, info.link_name, info.start_joint_position, info.start_pose); + + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(robot_model_, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) + { + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw LinInverseForGoalIncalculable(os.str()); + } +} + +void TrajectoryGeneratorLIN::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) +{ + // create Cartesian path for lin + std::unique_ptr path(setPathLIN(plan_info.start_pose, plan_info.goal_pose)); + + // create velocity profile + std::unique_ptr vp( + cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); + + // combine path and velocity profile into Cartesian trajectory + // with the third parameter set to false, KDL::Trajectory_Segment does not + // take + // the ownship of Path and Velocity Profile + KDL::Trajectory_Segment cart_trajectory(path.get(), vp.get(), false); + + moveit_msgs::MoveItErrorCodes error_code; + // sample the Cartesian trajectory and compute joint trajectory using inverse + // kinematics + if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_trajectory, + plan_info.group_name, plan_info.link_name, plan_info.start_joint_position, sampling_time, + joint_trajectory, error_code)) + { + std::ostringstream os; + os << "Failed to generate valid joint trajectory from the Cartesian path"; + throw LinTrajectoryConversionFailure(os.str(), error_code.val); + } +} + +std::unique_ptr TrajectoryGeneratorLIN::setPathLIN(const Eigen::Affine3d& start_pose, + const Eigen::Affine3d& goal_pose) const +{ + ROS_DEBUG("Set Cartesian path for LIN command."); + + KDL::Frame kdl_start_pose, kdl_goal_pose; + tf::transformEigenToKDL(start_pose, kdl_start_pose); + tf::transformEigenToKDL(goal_pose, kdl_goal_pose); + double eqradius = planner_limits_.getCartesianLimits().getMaxTranslationalVelocity() / + planner_limits_.getCartesianLimits().getMaxRotationalVelocity(); + KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); + + return std::unique_ptr(new KDL::Path_Line(kdl_start_pose, kdl_goal_pose, rot_interpo, eqradius, true)); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp new file mode 100644 index 0000000000..45b2157e4a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -0,0 +1,261 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/trajectory_generator_ptp.h" +#include "eigen_conversions/eigen_msg.h" +#include "moveit/robot_state/conversions.h" +#include "ros/ros.h" + +#include +#include + +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const robot_model::RobotModelConstPtr& robot_model, + const LimitsContainer& planner_limits) + : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) +{ + if (!planner_limits_.hasJointLimits()) + { + throw TrajectoryGeneratorInvalidLimitsException("joint limit not set"); + } + + joint_limits_ = planner_limits_.getJointLimitContainer(); + + // collect most strict joint limits for each group in robot model + for (const auto& jmg : robot_model->getJointModelGroups()) + { + JointLimit most_strict_limit = joint_limits_.getCommonLimit(jmg->getActiveJointModelNames()); + + if (!most_strict_limit.has_velocity_limits) + { + ROS_ERROR_STREAM("velocity limit not set for group " << jmg->getName()); + throw TrajectoryGeneratorInvalidLimitsException("velocity limit not set for group " + jmg->getName()); + } + if (!most_strict_limit.has_acceleration_limits) + { + ROS_ERROR_STREAM("acceleration limit not set for group " << jmg->getName()); + throw TrajectoryGeneratorInvalidLimitsException("acceleration limit not set for group " + jmg->getName()); + } + if (!most_strict_limit.has_deceleration_limits) + { + ROS_ERROR_STREAM("deceleration limit not set for group " << jmg->getName()); + throw TrajectoryGeneratorInvalidLimitsException("deceleration limit not set for group " + jmg->getName()); + } + + most_strict_limits_.insert(std::pair(jmg->getName(), most_strict_limit)); + } + + ROS_INFO("Initialized Point-to-Point Trajectory Generator."); +} + +void TrajectoryGeneratorPTP::planPTP(const std::map& start_pos, + const std::map& goal_pos, + trajectory_msgs::JointTrajectory& joint_trajectory, const std::string& group_name, + const double& velocity_scaling_factor, const double& acceleration_scaling_factor, + const double& sampling_time) +{ + // initialize joint names + for (const auto& item : goal_pos) + { + joint_trajectory.joint_names.push_back(item.first); + } + + // check if goal already reached + bool goal_reached = true; + for (auto const& goal : goal_pos) + { + if (fabs(start_pos.at(goal.first) - goal.second) >= MIN_MOVEMENT) + { + goal_reached = false; + break; + } + } + if (goal_reached) + { + ROS_INFO_STREAM("Goal already reached, set one goal point explicitly."); + if (joint_trajectory.points.empty()) + { + trajectory_msgs::JointTrajectoryPoint point; + point.time_from_start = ros::Duration(sampling_time); + for (const std::string& joint_name : joint_trajectory.joint_names) + { + point.positions.push_back(start_pos.at(joint_name)); + point.velocities.push_back(0); + point.accelerations.push_back(0); + } + joint_trajectory.points.push_back(point); + } + return; + } + + // compute the fastest trajectory and choose the slowest joint as leading axis + std::string leading_axis = joint_trajectory.joint_names.front(); + double max_duration = -1.0; + + std::map velocity_profile; + for (const auto& joint_name : joint_trajectory.joint_names) + { + // create vecocity profile if necessary + velocity_profile.insert(std::make_pair( + joint_name, + VelocityProfileATrap(velocity_scaling_factor * most_strict_limits_.at(group_name).max_velocity, + acceleration_scaling_factor * most_strict_limits_.at(group_name).max_acceleration, + acceleration_scaling_factor * most_strict_limits_.at(group_name).max_deceleration))); + + velocity_profile.at(joint_name).SetProfile(start_pos.at(joint_name), goal_pos.at(joint_name)); + if (velocity_profile.at(joint_name).Duration() > max_duration) + { + max_duration = velocity_profile.at(joint_name).Duration(); + leading_axis = joint_name; + } + } + + // Full Synchronization + // This should only work if all axes have same max_vel, max_acc, max_dec + // values + // reset the velocity profile for other joints + double acc_time = velocity_profile.at(leading_axis).firstPhaseDuration(); + double const_time = velocity_profile.at(leading_axis).secondPhaseDuration(); + double dec_time = velocity_profile.at(leading_axis).thirdPhaseDuration(); + + for (const auto& joint_name : joint_trajectory.joint_names) + { + if (joint_name != leading_axis) + { + // make full synchronization + // causes the program to terminate if acc_time<=0 or dec_time<=0 (should + // be prevented by goal_reached block above) + // by using the most strict limit, the following should always return true + if (!velocity_profile.at(joint_name) + .setProfileAllDurations(start_pos.at(joint_name), goal_pos.at(joint_name), acc_time, const_time, + dec_time)) + // LCOV_EXCL_START + { + std::stringstream error_str; + error_str << "TrajectoryGeneratorPTP::planPTP(): Can not synchronize " + "velocity profile of axis " + << joint_name << " with leading axis " << leading_axis; + throw PtpVelocityProfileSyncFailed(error_str.str()); + } + // LCOV_EXCL_STOP + } + } + + // first generate the time samples + std::vector time_samples; + for (double t_sample = 0.0; t_sample < max_duration; t_sample += sampling_time) + { + time_samples.push_back(t_sample); + } + // add last time + time_samples.push_back(max_duration); + + // construct joint trajectory point + for (double time_stamp : time_samples) + { + trajectory_msgs::JointTrajectoryPoint point; + point.time_from_start = ros::Duration(time_stamp); + for (std::string& joint_name : joint_trajectory.joint_names) + { + point.positions.push_back(velocity_profile.at(joint_name).Pos(time_stamp)); + point.velocities.push_back(velocity_profile.at(joint_name).Vel(time_stamp)); + point.accelerations.push_back(velocity_profile.at(joint_name).Acc(time_stamp)); + } + joint_trajectory.points.push_back(point); + } + + // Set last point velocity and acceleration to zero + std::fill(joint_trajectory.points.back().velocities.begin(), joint_trajectory.points.back().velocities.end(), 0.0); + std::fill(joint_trajectory.points.back().accelerations.begin(), joint_trajectory.points.back().accelerations.end(), + 0.0); +} + +void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, + MotionPlanInfo& info) const +{ + info.group_name = req.group_name; + + // extract start state information + info.start_joint_position.clear(); + for (std::size_t i = 0; i < req.start_state.joint_state.name.size(); ++i) + { + info.start_joint_position[req.start_state.joint_state.name[i]] = req.start_state.joint_state.position[i]; + } + + // extract goal + info.goal_joint_position.clear(); + if (!req.goal_constraints.at(0).joint_constraints.empty()) + { + for (const auto& joint_constraint : req.goal_constraints.at(0).joint_constraints) + { + info.goal_joint_position[joint_constraint.joint_name] = joint_constraint.position; + } + } + // slove the ik + else + { + geometry_msgs::Point p = + req.goal_constraints.at(0).position_constraints.at(0).constraint_region.primitive_poses.at(0).position; + p.x -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.x; + p.y -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.y; + p.z -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.z; + + geometry_msgs::Pose pose; + pose.position = p; + pose.orientation = req.goal_constraints.at(0).orientation_constraints.at(0).orientation; + Eigen::Isometry3d pose_eigen; + normalizeQuaternion(pose.orientation); + tf2::convert(pose, pose_eigen); + if (!computePoseIK(robot_model_, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name, + pose_eigen, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position)) + { + throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose"); + } + } +} + +void TrajectoryGeneratorPTP::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) +{ + // plan the ptp trajectory + planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory, plan_info.group_name, + req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, sampling_time); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp b/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp new file mode 100644 index 0000000000..f19da41b8d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp @@ -0,0 +1,451 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/velocity_profile_atrap.h" + +namespace pilz_industrial_motion_planner +{ +VelocityProfileATrap::VelocityProfileATrap(double max_vel, double max_acc, double max_dec) + : max_vel_(fabs(max_vel)) + , max_acc_(fabs(max_acc)) + , max_dec_(fabs(max_dec)) + , start_pos_(0) + , end_pos_(0) + , start_vel_(0) + , a1_(0) + , a2_(0) + , a3_(0) + , b1_(0) + , b2_(0) + , b3_(0) + , c1_(0) + , c2_(0) + , c3_(0) + , t_a_(0) + , t_b_(0) + , t_c_(0) +{ +} + +void VelocityProfileATrap::SetProfile(double pos1, double pos2) +{ + start_pos_ = pos1; + end_pos_ = pos2; + start_vel_ = 0.0; + + if (start_pos_ == end_pos_) + { + // goal already reached, set everything to zero + setEmptyProfile(); + return; + } + else + { + // get the sign + double s = ((end_pos_ - start_pos_) > 0.0) - ((end_pos_ - start_pos_) < 0.0); + + double dis = fabs(end_pos_ - start_pos_); + double min_dis_max_vel = 0.5 * max_vel_ * max_vel_ / max_acc_ + 0.5 * max_vel_ * max_vel_ / max_dec_; + + // max_vel can be reached + if (dis > min_dis_max_vel) + { + // acceleration phase + a1_ = start_pos_; + a2_ = 0.0; + a3_ = s * max_acc_ / 2.0; + t_a_ = max_vel_ / max_acc_; + + // constant phase + b1_ = a1_ + a3_ * t_a_ * t_a_; + b2_ = s * max_vel_; + b3_ = 0; + t_b_ = (dis - min_dis_max_vel) / max_vel_; + + // deceleration phase + c1_ = b1_ + b2_ * (t_b_); + c2_ = s * max_vel_; + c3_ = -s * max_dec_ / 2.0; + t_c_ = max_vel_ / max_dec_; + } + // max_vel cannot be reached, no constant velocity phase + else + { + // compute the new velocity of constant phase + double new_vel = s * sqrt(2.0 * dis * max_acc_ * max_dec_ / (max_acc_ + max_dec_)); + + // acceleration phase + a1_ = start_pos_; + a2_ = 0.0; + a3_ = s * max_acc_ / 2.0; + t_a_ = fabs(new_vel) / max_acc_; + + // constant phase + b1_ = a1_ + a3_ * t_a_ * t_a_; + b2_ = new_vel; + b3_ = 0; + t_b_ = 0.0; + + // deceleration phase + c1_ = b1_; + c2_ = new_vel; + c3_ = -s * max_dec_ / 2.0; + t_c_ = fabs(new_vel) / max_dec_; + } + } +} + +void VelocityProfileATrap::SetProfileDuration(double pos1, double pos2, double duration) +{ + // compute the fastest case + SetProfile(pos1, pos2); + + // cannot be faster + if (Duration() > duration) + { + return; + } + + double ratio = Duration() / duration; + a2_ *= ratio; + a3_ *= ratio * ratio; + b2_ *= ratio; + b3_ *= ratio * ratio; + c2_ *= ratio; + c3_ *= ratio * ratio; + t_a_ /= ratio; + t_b_ /= ratio; + t_c_ /= ratio; +} + +bool VelocityProfileATrap::setProfileAllDurations(double pos1, double pos2, double duration1, double duration2, + double duration3) +{ + // compute the fastest case + SetProfile(pos1, pos2); + + assert(duration1 > 0); + assert(duration3 > 0); + + // cannot be faster + if (Duration() - (duration1 + duration2 + duration3) > KDL::epsilon) + { + return false; + } + + // get the sign + double s = ((end_pos_ - start_pos_) > 0.0) - ((end_pos_ - start_pos_) < 0.0); + // compute the new velocity/acceleration/decel4eration + double dis = fabs(end_pos_ - start_pos_); + double new_vel = s * dis / (duration2 + duration1 / 2.0 + duration3 / 2.0); + double new_acc = new_vel / duration1; + double new_dec = -new_vel / duration3; + if ((fabs(new_vel) - max_vel_ > KDL::epsilon) || (fabs(new_acc) - max_acc_ > KDL::epsilon) || + (fabs(new_dec) - max_dec_ > KDL::epsilon)) + { + return false; + } + else + { + // set profile + start_pos_ = pos1; + end_pos_ = pos2; + + // acceleration phase + a1_ = start_pos_; + a2_ = 0.0; + a3_ = new_acc / 2.0; + t_a_ = duration1; + + // constant phase + b1_ = a1_ + a3_ * t_a_ * t_a_; + b2_ = new_vel; + b3_ = 0; + t_b_ = duration2; + + // deceleration phase + c1_ = b1_ + b2_ * (t_b_); + c2_ = new_vel; + c3_ = new_dec / 2.0; + t_c_ = duration3; + + return true; + } +} + +bool VelocityProfileATrap::setProfileStartVelocity(double pos1, double pos2, double vel1) +{ + if (vel1 == 0) + { + SetProfile(pos1, pos2); + return true; + } + + // get the sign + double s = ((pos2 - pos1) > 0.0) - ((pos2 - pos1) < 0.0); + + if (s * vel1 <= 0) + { + // TODO initial velocity is in opposite derection of start-end vector + return false; + } + + start_pos_ = pos1; + end_pos_ = pos2; + start_vel_ = vel1; + + // minimum brake distance + double min_brake_dis = 0.5 * vel1 * vel1 / max_dec_; + // minimum distance to reach the maximum velocity + double min_dis_max_vel = + 0.5 * (max_vel_ - start_vel_) * (max_vel_ + start_vel_) / max_acc_ + 0.5 * max_vel_ * max_vel_ / max_dec_; + double dis = fabs(end_pos_ - start_pos_); + + // brake, acceleration in opposite direction, deceleration + if (dis <= min_brake_dis) + { + // brake to zero velocity + t_a_ = fabs(start_vel_ / max_dec_); + a1_ = start_pos_; + a2_ = start_vel_; + a3_ = -0.5 * s * max_dec_; + + // compute the velocity in opposite direction + double new_vel = -s * sqrt(2.0 * fabs(min_brake_dis - dis) * max_acc_ * max_dec_ / (max_acc_ + max_dec_)); + + // acceleration in opposite direction + t_b_ = fabs(new_vel / max_acc_); + b1_ = a1_ + a2_ * t_a_ + a3_ * t_a_ * t_a_; + b2_ = 0; + b3_ = -s * 0.5 * max_acc_; + + // deceleration to zero + t_c_ = fabs(new_vel / max_dec_); + c1_ = b1_ + b2_ * t_b_ + b3_ * t_b_ * t_b_; + c2_ = new_vel; + c3_ = 0.5 * s * max_dec_; + } + else if (dis <= min_dis_max_vel) + { + // compute the reached velocity + double new_vel = + s * sqrt((dis + 0.5 * start_vel_ * start_vel_ / max_acc_) * 2.0 * max_acc_ * max_dec_ / (max_acc_ + max_dec_)); + + // acceleration to new velocity + t_a_ = fabs(new_vel - start_vel_) / max_acc_; + a1_ = start_pos_; + a2_ = start_vel_; + a3_ = 0.5 * s * max_acc_; + + // no constant velocity phase + t_b_ = 0; + b1_ = a1_ + a2_ * t_a_ + a3_ * t_a_ * t_a_; + b2_ = 0; + b3_ = 0; + + // deceleration to zero velocity + t_c_ = fabs(new_vel / max_dec_); + c1_ = b1_; + c2_ = new_vel; + c3_ = -0.5 * s * max_dec_; + } + else + { + // acceleration to max velocity + t_a_ = fabs(max_vel_ - start_vel_) / max_acc_; + a1_ = start_pos_; + a2_ = start_vel_; + a3_ = 0.5 * s * max_acc_; + + // constant velocity + t_b_ = (dis - min_dis_max_vel) / max_vel_; + b1_ = a1_ + a2_ * t_a_ + a3_ * t_a_ * t_a_; + b2_ = max_vel_; + b3_ = 0; + + // deceleration to zero velocity + t_c_ = max_vel_ / max_dec_; + c1_ = b1_ + b2_ * t_b_ + b3_ * t_b_ * t_b_; + c2_ = max_vel_; + c3_ = -0.5 * s * max_dec_; + } + + return true; +} + +double VelocityProfileATrap::Duration() const +{ + return t_a_ + t_b_ + t_c_; +} + +double VelocityProfileATrap::Pos(double time) const +{ + if (time < 0) + { + return start_pos_; + } + else if (time < t_a_) + { + return a1_ + time * (a2_ + a3_ * time); + } + else if (time < (t_a_ + t_b_)) + { + return b1_ + (time - t_a_) * (b2_ + b3_ * (time - t_a_)); + } + else if (time <= (t_a_ + t_b_ + t_c_)) + { + return c1_ + (time - t_a_ - t_b_) * (c2_ + c3_ * (time - t_a_ - t_b_)); + } + else + { + return end_pos_; + } +} + +double VelocityProfileATrap::Vel(double time) const +{ + if (time < 0) + { + return start_vel_; + } + else if (time < t_a_) + { + return a2_ + 2 * a3_ * time; + } + else if (time < (t_a_ + t_b_)) + { + return b2_ + 2 * b3_ * (time - t_a_); + } + else if (time <= (t_a_ + t_b_ + t_c_)) + { + return c2_ + 2 * c3_ * (time - t_a_ - t_b_); + } + else + { + return 0; + } +} + +double VelocityProfileATrap::Acc(double time) const +{ + if (time <= 0) + { + return 0; + } + else if (time <= t_a_) + { + return 2 * a3_; + } + else if (time <= (t_a_ + t_b_)) + { + return 2 * b3_; + } + else if (time <= (t_a_ + t_b_ + t_c_)) + { + return 2 * c3_; + } + else + { + return 0; + } +} + +KDL::VelocityProfile* VelocityProfileATrap::Clone() const +{ + VelocityProfileATrap* trap = new VelocityProfileATrap(max_vel_, max_acc_, max_dec_); + trap->setProfileAllDurations(this->start_pos_, this->end_pos_, this->t_a_, this->t_b_, this->t_c_); + return trap; +} + +// LCOV_EXCL_START // No tests for the print function +void VelocityProfileATrap::Write(std::ostream& os) const +{ + os << *this; +} + +std::ostream& operator<<(std::ostream& os, const VelocityProfileATrap& p) +{ + os << "Asymmetric Trapezoid " << std::endl + << "maximal velocity: " << p.max_vel_ << std::endl + << "maximal acceleration: " << p.max_acc_ << std::endl + << "maximal deceleration: " << p.max_dec_ << std::endl + << "start position: " << p.start_pos_ << std::endl + << "end position: " << p.end_pos_ << std::endl + << "start velocity: " << p.start_vel_ << std::endl + << "a1: " << p.a1_ << std::endl + << "a2: " << p.a2_ << std::endl + << "a3: " << p.a3_ << std::endl + << "b1: " << p.b1_ << std::endl + << "b2: " << p.b2_ << std::endl + << "b3: " << p.b3_ << std::endl + << "c1: " << p.c1_ << std::endl + << "c2: " << p.c2_ << std::endl + << "c3: " << p.c3_ << std::endl + << "firstPhaseDuration " << p.firstPhaseDuration() << std::endl + << "secondPhaseDuration " << p.secondPhaseDuration() << std::endl + << "thirdPhaseDuration " << p.thirdPhaseDuration() << std::endl; + return os; +} +// LCOV_EXCL_STOP + +bool VelocityProfileATrap::operator==(const VelocityProfileATrap& other) const +{ + return (max_vel_ == other.max_vel_ && max_acc_ == other.max_acc_ && max_dec_ == other.max_dec_ && + start_pos_ == other.start_pos_ && end_pos_ == other.end_pos_ && start_vel_ == other.start_vel_ && + a1_ == other.a1_ && a2_ == other.a2_ && a3_ == other.a3_ && b1_ == other.b1_ && b2_ == other.b2_ && + b3_ == other.b3_ && c1_ == other.c1_ && c2_ == other.c2_ && c3_ == other.c3_ && t_a_ == other.t_a_ && + t_b_ == other.t_b_ && t_c_ == other.t_c_); +} + +VelocityProfileATrap::~VelocityProfileATrap() +{ +} + +void VelocityProfileATrap::setEmptyProfile() +{ + a1_ = end_pos_; + a2_ = 0; + a3_ = 0; + b1_ = end_pos_; + b2_ = 0; + c1_ = end_pos_; + c2_ = 0; + c3_ = 0; + + t_a_ = 0; + t_b_ = 0; + t_c_ = 0; +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md new file mode 100644 index 0000000000..4f2abec293 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md @@ -0,0 +1,62 @@ + + +# Acceptance Test LIN Motion using the MoveIt Motion Planning Plugin on the real robot +This test checks that the real robot system is able to perform a LIN Motion to a goal state given by the user. The test is performed using the moveit motion planning plugin. Note that before you can apply the LIN command the robot has +to be moved out of singularities. + +## Prerequisites + - Properly connect and startup the robot. Make sure a emergency stop is within reach. + +## Test Sequence: + 1. Bringup can: `sudo ip link set can0 up type can bitrate 1000000` + 2. Run `roslaunch moveit_resources_prbt_moveit_config moveit_planning_execution.launch sim:=False pipeline:=pilz_industrial_motion_planner` + 3. In the motion planing widget (lower left part of moveit) choose PTP in the dropdown below "Trapezoidal Command Planner" (see image) +![moveit_1](img/acceptance_test_lin_img1.png) + 4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle to select a singularity free position. Click on "plan and execute". +![moveit_2](img/acceptance_test_lin_img2.png) + 5. The motion planning widget (lower left part of moveit) choose LIN in the dropdown below "Trapezoidal Command Planner" (see image) +![moveit_1](img/acceptance_test_lin_img3.png) + 6. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle the select goal pose. + Move the robot about 10-20cm choose a goal where the configuration has no jumps. Set velocity and acceleration scaling to 0.1. Click on "plan and execute". +![moveit_2](img/acceptance_test_lin_img4.png) + +## Expected Results: + 1. Can should be visible with `ifconfig` displayed as can0 + 2. A -click- indicates the enabling of the drives. + 3. PTP was available for selection + 4. The robot should move to the desired position. + 5. LIN was available for selection + 6. The robot should move to the desired position via a straight line. +--- diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md new file mode 100644 index 0000000000..732ec8ec4f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md @@ -0,0 +1,54 @@ + + +# Acceptance Test PTP Motion using the MoveIt Motion Planning Plugin on the real robot +This test checks that the real robot system is able to perform a PTP Motion to a goal state given by the user. The test is performed using the moveit motion planning plugin. + +## Prerequisites + - Properly connect and startup the robot. Make sure a emergency stop is within reach. + +## Test Sequence: + 1. Bringup can: `sudo ip link set can0 up type can bitrate 1000000` + 2. Run `roslaunch moveit_resources_prbt_moveit_config moveit_planning_execution.launch sim:=False pipeline:=pilz_industrial_motion_planner` + 3. The motion planning widget (lower left part of moveit) choose PTP in the dropdown below "Trapezoidal Command Planner" (see image) +![moveit_1](img/acceptance_test_ptp_img1.png) + 4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle the select goal pose. Click on "plan and execute". +![moveit_2](img/acceptance_test_ptp_img2.png) + +## Expected Results: + 1. Can should be visible with `ifconfig` displayed as can0 + 2. A -click- indicates the enabling of the drives. + 3. PTP was available for selection + 4. The robot should move to the desired position. All axis should start and stop at the same time. +--- diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img1.png b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img1.png new file mode 100644 index 0000000000000000000000000000000000000000..c0a55f478f5b837e3d93f7bd4d0563b925d593d7 GIT binary patch literal 229896 zcmZ^~1yEeU(l!hsKyX5EcY?dS1&8477Tnz-xO;F27TkinI~#0qcU>G7{m899_a<-E zKUG_tJ+)`1=XB50-A^~+pXDWx;Bnz0ARv&WB*l~Fuh`F++PDn}u{e39;+sk3MX?Pn{$weSI2e)5kj!MpCj# z^U>+^-_fi{)%sSU6${8DLr+>8$1~T6Zf%X-0>mSBo~~+6m#NJs#wue&QC7VL2A%gm zTSG=U>;vmR5b&ZWCVuF4AQ1a!Gz5ddDr$4pwkMy|e4Am`if%JbS6UbBKX!FN`V5ao zq9MZAV|dSzyEv0 z+iY_o2lxkp9;|7~HWL*v(e|gB$(p$rAo{k3SNO#L{_T5k(FGq0;PM@e?uBFR!E!Ty zKu-_f>%8%jC#T@>zXS;zY7`=kCq=cVW*DPFg%3I7BeI$;QvGLtu84q3xZpQ$qafFA z=TrEmv}9CW=xa1GO<00K4_q;#OxVP{ys~Tw!ORmJa_dJtDSyu&=Izn}GK?ZIsOZ0K z%>D$N8Tj$BYIi+B9)G^<0|j*Rb_kTpfHK|dmUaruJS>KBFD{&hCXnr1$}cV~?~|_b z6hM0D=s(Huk>AHseWax|9-b*?+A_RnShe3QZ0`t+?pyXusZKG!E zWx}foWCLZXv-Ycx1O8h^Z-i5T1x-db<|;U%^6~|lv+v5oEV zLxqTiR9)EX^gJ*I?^;dD?Mjn~ikw$yN)tm8kPX}H(E&W<%5bC%ck%|Q%a=>*OZPgY ze~s2i&_fdx>{a_R_Vl4teZ7ODcWeeFVs1N8??7qI@LP`LKF^Y?Ta!0|cLUlFXR$SY^g6t8=(H!^yqhG@WIzU?MelR!1ukUI4VI!`v zexNgZ{JZlOZrem+GdPd-qncMD0X7*5?jsjQ^$j@pVjbk9j9@bNbcQYKHJp(C@(GKU=4AA+Lq9TcXWIQ zyE&uM4!COl9(7cmvQWoixF4*YQ-4pVh1J_c=)?Od45x9yDOoisKB`}`$eV7x2{Lei zEW5U1j8>A3b8OiCKKKf4Msj>6Q)*I$o7+iVo;eJ{a!!lW#P{1N_qF$BK0uJ_v5<#ajR zoi7CRu*5g)zFe%lH&L!vXI5rR;4)KSHYfOW#p<3mGLbT{h*_v=i2OJ#j^84|+46I= z2cm}k(jA;ku^ojwD&Vv9{)$%f=B94>IyRMQA?4eLyq|Q!RZxCiC5Z#SI+?=s!4yPo zcSNUNj>v@_hBN(*j;q9}+Y+2kr#X9ZQRo|pnLWLGNIexE8D$ggkr=?d?e!1b*F%$6 zHhY_@g9s{Cc$ngRv)vAYLBn{qyH#fTQ>PQLg{jpONI@$-h(&ExFj zaV5oFv+e@uO8@-w>2hcd^!a{`Hx@;}E8eZ_*2tP|WIssyZtmoA7zz5!5)Dz%B=#yZ zwS5c^k7URB%N0Z8jSi=>a$MuM6XsvO49u`-Nt`BKxQ`*ZF=$EbB;UFGF*Sx@D7kL$ zvR59GP%m-Q?ExiM-xA8##^qkbcpqboGWp_gP`*$}jC6rKu|y3thi2^keX`sJx9sQ7 zpUHb?S_D?wcO@T#B(4js-VsanzziRItO6* zof~dvawrjqlqBTa%C9*im?}m5y+-}L*t`?*NJ%rJKzfuN%0JZ2c>X+H$N*PRjG+8be`q|gr z_0QHgM{v9(1>)mjr>L6zRiz@a2>J|zbh`HHUfN*|VH1~+KWTe2Oc$+@{H2L_(03aR zQMB)Rh=O`c)vWK0g~+>;NOYTb@-buy7L&W{%g!umGITPG@x40@t*yGTB`NWgavE1X zi)V*V>Hy#FPwcsV&er(fOAn2+uXchpmcDO{y|vSKBAjD99*M1dfs()wT|k(-sB7)W z-7A^1KPez}42jB(gZ7T+PnwR|hbZ{w1~AoObwZj{RBqnr@fpT@ft;*h3+&z?1b=D8H})@5i%`TAG=cKV#-)GUH$51NSgE+i_P0Q8l@P z^j{ux?l>f?jQvK56!yc71VqgTt=l*#-AAx5(O5xJ?Q+N zRe|`9Y*{_MT;7tHoW3gJ-FM-LOhS*w+4s2-pPL98hO4H5*)jnIe`6oeJlL`1 zV@6}JM?XiE9Qk0h;s<)l`3Fe`fjp`Z7QL49sM9UQ&|V$E0&C|$kfdvVPOyxHWAZp!?|b>Az`zh#?VG!t zrrq%VZH#Rr$Enjux1aD`-&NOWzq*Ms z_z31`&{a1yBI4Bvf4_c^=CoNnB#&lRQB&J!ImH}&bJmsuHQe$E92 zI%BSnxo#To5kAbw!NoW34z#03M!oHsQAtI!_jV-Hq}PE9AvX!t&YHubX0n|=GV;Y1 z{M^IciLdZJg(CJb4OXy%gqI3-O{>S2p5q)EW$lhR%TcsEi#e-RHsK3fg!$3w@|~BA zb&)FV_+o1czaP7x-%tNKJFe6xCq#aUq`2E{qjFG?ltFDz8@NwGw%MLb6AERgD;|NX z0HF)_`L=UsTOv@U{_LT;eh=+$)S+Lp90eLMbsg8Dd%66KgS&3o+qLgsT{+Nwq9{3p zhxv5bWCU7urqK?NQz>Lb#5PMUuByW0#YU_)JK?z(^K8JyZ|HN;1y<@d${G9IZAj3^ z#7h_c_5s!>W4nV$C8{}R&}0XDS6g9Eyk=Sr@@BGxU7rMW)kJ-{`a+stwkk^VqG;z+ z6K`SWyJTLprOA1v57NQqMqKS-aoW%C6~^u|9Z{CQFlPnstZ#E5klWOTl#-*4<*Ucy ztI;if1p%kHIYSo>I4N^n(6Z(_CpvTPAacGJ&6)XPkqV2d(q2ZGN&JvzYu^i5eD}f6 z2`0aeL|~6d=cNyRq#}=0QLfRI#E4RIKK#S0BZs=khY(hh>yh+F2YWE&JYDO=*e%zx zqvkrbeuP`{v_F^`+sNi!HNOLsgU)yj@LONLkZXP>WEm^`)pFP2VI-h>La+$rh_pmSc&YkbuPT*?o;h<6((`%k+z<<}qFVagLXOQ=#D=MUPMVR94HiJtt+>3)o> z#5Y_wW44Yjwf)TnNJ9BpVI)B0*?+^ED{5*{PSEj|^l&dJqHxr65%=uHrl}Z3vV!_r zCsMFv&5?%qoFc)) z6kv*`l~GuqA^S8%wHBPt?tOLPcQ=?1`VjUtTr;Qq_@lnSEqaa|PZ0mI)~e?*{W}lx zv#?J(0?iMWmG>79k7vK$Uoj$oa3oQ?haNRX9$+5N&Sz2?)%NQQJ~hu8+vr_P#26=S zC`v!J)_B=?pyS>C+*0n9g90pO>DJk@cu30$pF3V|jev#JSKdUFZOf&PTvxdoCTp_>jCQJTCl$UU}oq*`b?m(K4>Wg#*8AtbsZW< zPGmy@@oh-_px2gw7cJJ2gvq}i^v27j%qGSt>%W`^>~bM{Xr>6MC8^Xki%Z@a8qMw$ zB$QmW#KnltwL##u=_IJ))x?MvFQc+kRxQ-at=iFETHXuj04d1|ENQjfofuXS-pp|E zcGQ%_#JaqDJJy=uvd57P1+lLt# zj8@PoQ^&gV3A3S*P;rJHAJgFv_TTbbS{kdUswpZfi+;pkE;Rm^T#DIx5HQ;eFm9xd zldU69Ak-V_z4e(I=S7OyT8GgIglz1fInUtzKSa*E4v?J>((pJmE2O?v2h#tUQr*15 z#*3KxZ?Gb*rAL?i`%Hxm$>$3x{pFg)#9Y&5iQ)eB*J36Tb(pf^9%-ABf7#Ld_Y2gS zEBuiE+ydVy{huXy=;5J7(J>o~jf)w$Vis1W|CwXK zbg?a@pgslqQn23zKVs31oR${BSPNl+&H&fOFN3`;sHM0;qZ&1KaG~0kK0EdM3t>>> zS;s%S=+Irnm#oyPPZ)`S_q^E;-fH2hObp*d1ZqWDJoAtH(vnKmEl^p}4=|^-%_^zW zrThAJZm?FH9H2Q~UhFS7k5*>Z{uM5U4cSibrc@jOD+?uJ^|&fUiLv{TnhLfWoaGE! z_3yp7#tM$zn0hrE~#!fsPP#`5(1}SN`^a&$R zBht95Q2<+%9G4{plZ_*Ga00q&$D=|fT#f&|;y`1e_Xt=ll zEjmi&_X`EoHf9p{{0sNe(%u=*yAQ4AoBvo!tMC$()S3w8m%5JrmLf&*f(kneEp1xj3k_@@^x~fV7#VtGPo93?sU6K>i z)QUQ6G#IDM4&lcei6hHUG^Z7fpC&>Y6MUf?1Y+3SI~1Nu)7JtU>i>~tsdPM`!0xF< zp!@*Pkz>(sId!7VX|=S-<^RC($)UmF%+kiJVytC{f>qeYthheymg?`vF9>RMF%k}> zJIC(hffi_8it5~7Wa})u>_i<5bW_zbJib`b10ALwyajc!EV{WuT2MN2;OI|o)Fm18 z+OYi-5)uf`(Epy0v^8WJh;Rs$lx?(Sch}?jdg&9H!<~|$FJ!Cd>zQMB?j5-YzI*0; z8NFoDPur_ax!?QA(LPX=14rVM!5w<#+nN=yoj@VebltGj{QA+XHId4wO4YTS*=P2C zxg$DTeIT-bM3G;=Bg(9nTe52dW9EN~eLW>ue;tzr!$m!kgW@vc?m&<*aa>7~Y-rL0 z5mU_Vsuzks)IhW`uw1=^pfEn8q$=*~eM&`itZ8~G;>Hf(j;ZR1G_oo3IPU3;{) zZ8Nlg&eO4Zrqhk))xoqmEMM?3o!`UK8khT8Y&X66qp?J>8;|zVP4`5edX*lp8JaJ> zRcZyvd^=}wHv#K*rQ?mhS)G9?H^fuxj?)n%%KSL%UvX*tN&WA)A3eso26bdy5!+9N z4nLVE3d`fXFZ2=p&5<&ZfiFVm;o?kDSs^*yH5t~US%d=fz>`IY}E1LBvQ8lwGOWs*f~>oCa_d@=q)_6@BOX}%bYx}*j2rtJ&EZ6+%VAl~@s#~T+pP{`jdxdr zQuZ5!5FO^0{>weqApRx4Jg*JB;L`yB;+wz|=3>3oU_pM2*+>$#iK!{%*$8db+4hH* z1?xU+rx!DAnt6cYO5ow#GWINo~j5Jv{%{xa+}=(x|E7Byb3H{pFOIpN&rK^kwryhCPC27u8lE_8R4+G>bhwPM z)%ASe^%s$NUp)(R_?%Hc?9$YwJvr`^FS7%Fy~+FyT7y``~fVkF`HvLsqBn zRq>puEYBSbH4wA2>;%|JkYU)#3nlDv(GI`1&a9on9u|*1ze@1oZrA5(Ut1c&Lur>R zv5~N-K;dP^{K=kk*@ZE8W_0!0_o|GDwfa}B!+|#t)3m;w?-p;HvlEQOweyoYYB}(( zN8;|NQ=RFeoVLkc2>F~n63jx?dE%FEkfHgQG&u2e^!Be8bD{f-4uH*P?VbpzXP=c5 z;QVQQT>~r#;u0iK;mI&%tUOX$?tp@VMK-x9PfcSwH-OOdI0&X(8lP$8zQ)zcZs?Oq zxR|!mw9PxS4?S0wdX&XevtqH5)Ix}<&=;+f_p}+l3W1KY(Gp)sSD_InMm0E`DbaB{ zdD%KFtF(5N*VM$B({HOuy4i`HlmDKGFl>r)UiQ+M zKK=4215@sk-3od{v7;{|p4ozsf9QWnR=GC%yUafN+eNnm^W7i%U>t>I zUlf3EQ$+f*FaFI|j?>7G9(CpkiQH=Yv99aD@v~Kl9tRvOqOP&r|NXZ z9R!C@)gi!R9pHAcSj%FB*+U?~1Rk5_Zj!(RHlpee(Rbd#LA3i_Q9Uzpj4W~z?N;L0 z)i$B?APdISJ%flnO~KagTY9#>kNh1U6@)i5e4W6G)=gHpx86D-63FdTnLPk`e5pKy z1~WKTP-iUFnznewbc~|1-=IY$MaPAq?^9xE05ZN;{|^4jLSe`AB(Q!@)^EVS1_n`M z`b@SWyx%C(VE_l)_#1}}?!$2%3g?%$PEJ_zv87X66EK4NbJ{LdHsi zypuXdAheN=)wE+b=j7KY=x|TiOYM0Eg{ev?Rs1Gr3heY_pC>$fY%ga{u(^#Xm}Q3ZrK=F0)RyTLkR97iQLY5hk$&Q86tgVg1| z+(mWplFPJ*`_Q%QiT3D-$LJ9EGfm~ZV2%k(My62p&I8gryK^_+)C={CK|$Nq+F4Y3 z9MP(a&~Lu#Uh*%`?lAQzg~syQ6RnLeY32!o=SPCE-A`w>MJqQbFi2cE@zH}TJs$ai z2p(q&x3%`$nmSLBz9y>bXbkYxA;c%KcSwGGC0N2+>!*DQKxo?0=4UL}#4CCjNl&Dg zEFG7Lz7`4%=B~oV_MexOwb2c1`s-Gl`u__L;#W$>>9vo5LSt-n{f!|pSr_W*oPT-& zcrI7HKpP%}ZmS;VPtEa?Um0{;AW)G*4}Rr|CfZohLqkK4FMV5Vfhoph)Et>gCZW`G zM}oyvV|^((SoQ*xA%dSsel}v_osMeG=?x8iG{8rr&0ep%1(?SYy=2$5R_--x+wR@$ z-K<}L>WhuQd*vWcc=cz)cbafHU${b%2eSw31xE(UJ=3(GFCbVyq3lL=NC~un>>i7) z-BO)RW0So3iKJ!fuWU>4=*f|p!gV%FU;YfI#j>`^-;1xT+ zX=kC%lv?+=Qv!+FwV}c0>tIuSJ_`y3zr}d6Dw@0%r61H~6sq+}djiO;=Lz{ZK#F&| zF|&7FELWxicZzmFR7iQ&XjuX_nYzg$}ontOsp)4M?h6@9vh%nzAj#*2s%h#O?| z3N@_>N1v+)TpK5;K8Qs+X<5MI5qD}gUhVpy!+iT3vB_4@`dzd;; zrf@!*4gW65ymfQbHKqVeJp1l_cEQ4f?}KdYg|Xp?J&5d`@*t=uQET^($pp(Sm#H%^ z$1d^~4|P>IS{3hjKc&L9xEDN*@t)9YTk8306!s&o3*w90z$BvU?&TxZd*rrbgZoC5 zp0CE(Pks9Cx1(>#c-eLAXtf3H!hGU6xAO16a1XzD@xJK4?bCoV(9xM&p|J8%>*r6Y zs_I^Jrv0)eBG@jur2h=AHwfIWr_Ig%5f={|y&*W~H?Y_z zjBY-`N;yrK%vHAP87bEuyS3lWlVHVTKTo3Tew_@Rq(IZ7B}%L%X{d+n!tRjAkTz3f zqK~}Kk~lXvx9*}f7Mb%E$8XXQ@dWMdqT;(5lM%vb<++$yfQktC8bqm!oTkY+B~oNa ziK&yC_n1dcIBOr!g765#)M) zfN(yU3dUh2v=hixmvNyHljjmyBcnvU_ft4`G@vho!3V7N3#Ui~k(X_{v*Ufdy*yC+ ze0zjikZXlJd@CIedtV0{vVOk0N3*;Kp53Ps;PBEB*-d^bEyc7ync>7+ss0+#gfb~h zos}rA*O2Ea%1$`$w_xoJ&A8&k8{6Is!N46j6$DLPbD$YBZ@YA=ti1lOj5 zaYU0?-t_XOXnD=vSFcseJN*`O1^96Cj<>;-)}Lg`ici^>KZd9>r}xA3%LtL(JJu8Y zoQOV{Z158fpR%8A9KTgY1J8{W=TAqy2orCpn#^wWw+lA+Zlik_OM14ob{8A@knWq{ z+w`o~)_S%z`m9!p&X*@5Eq>Q=cJ)RJz5iJPslJ4b-NIrME^1d50yLfomNBBZxVUT@ zOK@8opFd{fT)Ac+|{FVaKYZz!-4T@ABUGgV6zF>VH1vM4WL5c>1(zyOrCsH zCPmj2i7?SoX4r-tR3deMP@+Y&c6kn&q0xUFdb)RjqV_o(5w%*XrO;bR6_5JX35UQX zeT?2mFcJiI9{>v0)ko*>KiNj*<(ZZlAEh_hGRGZH6*?sXL}5_OhSs|gy-wIiMn)jM z@mvq$Gr3yVS+$zcDB@jisrFgN9p{Qxf58vM&>vb+*t7p-8Z^uvV$dM-od!9c^dieq zEu}D57gbW}#uTI575b}O7~$3p<&i?w#U$)39f&PE2OZ|!ZarD?x)4Ll`QVjZ#a2o9 zl({<<%?oD+MZM1gS*runp7K3#W%*W9aH4ZV>1M2IeP`59F7kaWbK0jSBoHGoRl$Y`G(4!iUqob9Fgm$GM^x z?xorg8>-go*-N8K_;;!mMRag0hbTBvR%eNXyi~O|mY3|9RC-rzyHi!(EE4lP|I82Y-;7>rHTN&bBQE- zBUeTW6#x+&9DF^fwIq*H@3C#D&6R+BU2crq5FOM_wgFDN7Y&tc{kR~iu$S%>f@Z;D zmSB1vZ9tEiQ+rFe9uM8leROe&ld0Z+G^g8dr6KDoz^zi-!V+$y!eOQh`}j7y-XMLD z%>^U=TEd$sD$zsY;Awnu+O}KUu3elZ+2}O0If|c{z<(@@70cg%xJ%$Qgy}!B`JUc{ zlz_puj#&fP4sUyqHjmlQf+l}w(n?N)HlNAQ<*bo;aLYel%{r*~)06VH-3Hr95l++p zfG5UbQl{xhAa&I|^EId{x(z_QeHnXV4Pm}s_uXEA=b(r@SfLQ~K*q-#e5Gf{Ns#T7gFM=oK z_f-BhWuSlUYg64$uf4LI(^x%+rhUP}=(7;OfA31XX=5@m#;vk*B5K$zgp8{(A$a=2 z=%cU_OE`&cT8NzG)kuZ@?nA99ou1-w6G=r5)~OFMS=LeW?FJ{8`4r(c#Yi}ob78Jk z(uF37$xjaQ#HuX(W?y)o0=I z8|r7fX_+0XjS9U+kPZ5njUGmG#pC#r+5Lg!lvX6j9GG}D{aQKEvLewD%WDmB6VwtO zCS;gJpLCe9cYNjx!m-=2AMzK;OjojWYMo{uRBBw#2WfjB4m8|_zdiq|@28M4n_BZ! z2Hq2WDctU>xf+X5G(AI%WaJST^5sc+4yC9E13?(kw9D@ITL#Hz1Q}u3dc>b{Z@@&Ftl(f@#qR@$q)IQB4gx& zZ*_sqef=h;bqo2~d(ya%Y-QnPTc6qVGdV#qG_O-vx30(Ih6pl|UK}z$SrB-#N`C#6 z_jJ=uLDf(W`HAW!W=-LA@?+fF#8xc9j?Fg_r#@dFsysqy5H@IV0QsqOR6P!kEx1I&{`uO4Axi058G4f_ia%bcm{i>XOIC1|^ z(vuGxGz^1p-{Rj%!60X9xoJgitVxB+$n6jHa)IC}|mnIEaJak>QBLIJ1?|DX+h zx{)r5;tdbCQ!rl)2CO#_jRvdd^y_i+vJTkTx`TAIzI=MDVD-3gSiYL;Jnz5QdF3yd zU%$3`N&kiQyip||8LgBh2F$ZFyPsjB1|6m_!XZRMPKKA8oeqE%+8MSTjyuvJ$OI#q zu>xN_i8*qX7{FA`g6&C(i-3&~8GVCokl%QKIP&v=`kmjL_gK7o&fuA3peo~9FFot7 z;-SR8*O+D13cm+(%*W^8k~3=c7TQZGJCn9_#Hcm0&-s6d#-_`)0-PsJfqzxChRfsrid3$15QPb-<48 zW)eA5RpxR8beNX&+iAP>6%IuvaJ_Z@U#cnQ`i%oNK|%1G%Xm;uQXSf;8m8rTzdw`P z37gBFxZA+~D9B_6I)TXNOfQ3~qm@lXq-ZYgCuWo}Q(DAmi#=KwGX%^QHvTlKB%#aL zHpfaGL(*IHe_CvZIHfZlRJE+|7UL40=ORpHnw|li=UYTC-&{`?Z7wyA=BkRz%js5p zuQPf$enzp!Kmm=~LsS_d%RyUQ@}f&>ME1D#={EVV``raT+tp?ytjyO+;qU zP1JOow!W<+eC;SBVDkE3cJuyyyl&YmB@B8n82%ccRw_sbY^2k@^4R?bd1QRF*kUFk zJAL7=e*))PGY~*NU?NRyvx02Bi@3#=w#Tvlpuwptci`8X{Z~dWCbnHR-;q-B>tht# z0n@~c8*6`+m712ioCP9gFXs=`T9@k*%iS~|A{LAUHa3fXjYvC5b*VSN`!u&R8+W+B z&HyRtlG%e+gI1)&6l8vl08ry>SOu0tniA6LN91e!_~G;s`JB?X2j|GULvyR~pUz=R9GsI(0m_I>gW)P zc{hVl+Kh&XbyQ-r<+DEIp#cZVN7f_Rd!Ul0ro5}#&1rhmKYTvJ+^$fqdX)g3c0*F0 z=&dxYVoSVj^Q%QRK#jFOlW9)=tWA}I_3(U@Q*G@mqd0v|G85;{V@*xpS7|m&koEl z-EQ&!qdwPf!d{5W)LSulf`-LWe{!M=m)pZvK7Q)D^#4Bm>EDu2*fCqc|5sCsA8(H7 zhK#ZRH$i)^Z7%3?)Bag@f$){6Wd-mhay#u>&l!+?iZZyrI?(q}`Yq|j$_`Il2411| zZMGPx;+R>qo5oZ+zoDM2mJZnxII_iTa?bscUZ zfx~3a1S}dQ@>#Rj>;7z_`&Y-R18=nzl6uF3zYE(br@H_IxG|as7^{eup4KwmV)@o>#}gAg=&GR=b`*N~%?*uY7GSvQ4xcGWaOQ^sd{W`dNdiJ^#5)$iQk;aX{m7 z-kI>Ryl6upUWDg%A5hKwP;DNIzkL66@{z%H7@_3pcae3-O^PO&6m1-`OUwOwRviu| z=(zoM6@pH;|SxE`8WY7_t%jUP(gSayObvB9UbxwHdwWVlw^N2H;WD>7& zVfXylMLFi` zI1oHv_8~etI!YCxX&fo*>IOUDGr_D~y59c$19&C~iz*}wO}H%R)> zggsp-gqzcDDNbL8A=0CgwEAgdJG#DaI>-uC5P?-{N@!B?VXGZD=yWHFmY=w9_6beA zRK2;~II0AEsqotBz}>(2(m8!xhE92+&IhB&IhRNpcFD{HZV_X5+_uJ^WU%K?p;zyY zWsJO#$nnl-)vJH@6pnV?aN(_P)R^zC$8e`BhoWx)QxxBi8T01o{5~oiTVR)s@1~o5 z;oCbq=f8SUHo1`TIBmNisIpy2y-u644(iGA^vV9KRf#1P5*$PrxM11DGiGedA92eK zDHYnNwhADTZ~84EG;kkIV43v8flCa?usZL+xHS!$`)=VfEUn37v>IGPdpgrtI;;M*bL%of_ z@!9lvJWdMQ&q@dQy?{B31kZ7j?^&5%aUq_d=a#QrPF*_Xikbv7a)XC~>iI16@$4%R znmttCmc!<`yAOLI@V_nw>}e$$Twv2sX=3R$7i~QTto6Q;-KDC<;VGh85c<9#6=Lro zcq33t)EnB~W%J@5I@gIUQRPb?t=?NIjz{9g)o%z@pSHq5AYUY%T9rA8(`+ZF%gOFeu+8VoxD5ou{N;0o4oC?#2`KvqP)lPoIkS zI6DHlL=<-S(AtY{D-0gL=M_*P!p5L~sxT>`purhQM!+LBR(;^0ZEYBTk*W*~Uj4M& z(U|#pFlIYgDTz^6l%B+~7B_-7aa3hGg^&eZ##c=dLk<^*!x-Lbw0yge$zdm%|7X?*cT%mfp&=I9BRy7PJclo^co1;1I< z-&b6w&Z(|2+bOlQ=S72~iTdqa7f7P=td*%JGx5vo@qOr#+We`EdYh$r~2NzCnYe}$>wy&W&nbUuIw*>p7}!+kMCw&YGL%0=~tR$zS? zRuH0VoT0!8V2JOrC~!PWR^0zw!=X4)>QU$Z0=D$-660Yq=sCxqBAfXY&mL#8v!rk- z?*aeepf5M5*#7QYkF%<=8Y6P%uT%;1_AZ1-So?du2m8kY_-v1B55n|BWYR2q27=om z8+RPdc5L3FFpnzRg3+e#pQEy4WqRX}oU0}t;xs;5hHww^0twIY93Ak#rMFO2BOjs4 z@gD1VAQECp1Iri>0AKua*bQu29)j`SvDlpsia9#Et$qD3T9=KzvNW@cMo|!BS-h!( zgkDhq!I#WJnbbF*m6UqcyMxmBJXl_OjPP+1}umncv zU(UG>i>a8HN;_Q`U(aeSt;U-9$sxvTzypJXBdw_TFm}Hl4XCkRY>HP91b;hB$K`Q3 zncTk1<#(NU$@%|@&0YfsxX-Gp{T~$gggbqo?yjr+uBy2Oxi{mIas)qpq*Gt?dj`<&>}OaUHA{c7WzUN2~3;iBt-!;mXS{ zmYwe5eotN`=R%M^1NeaOhX#d4pWpp_@w!8wKWoSe8=jwL|C#dDdurP((ENt43%Tzp z6@PGy7pQKtaCrR9;=e!q&jlNn=9vkw@GC^R(R1EFIH#f8Mh4+s(DK5HeBzMJye|N2(a|H{Pd=J{DtNSY});m2)# zO8VIG?Fv9vXEU7_aA{VE;fC}ohGTLTk59-{0A#;zh-Cb$Cv4wl9_bceP!U_VmP6Et}1iO>=a2_e{vj8b45$>LcJDNnDvsap zM#)Gsr9Z|-n@J8!v10LajS>c5dm9rXl!AhSh{))N)gR0$7HDqUs40b8lYbtT#%vxZ14AT3WI^KjW(Z z2@{nPbQ+O|bJT3Vj}DnS<1|OTUcOmR)2nCjI4x!Upvmum&ZrPc(b{FDv~YeR;eB@! z5LneLdDFq&#&MzRE5#Do6!cqt_}t!Mmh6@ru4zmcbge7$3%}sWDJXP{M}EumdlckW zY7=wY3Znt8O2tD%OtBk@!>b)v@%iF~HMs2t=q104_OkFzmjP!{D0GL9H9C^?Z;O@I zzx?7G2kHCub9QF+Y4$DgpsCeynXyt4h%e{H>6ja7;?>36A)J~gG;MRYxU{4y++eb6 z{WeHy0gDpqBw9SBTq`JP=!E2WhiKYAoj~zDG-pG=uV3Gah``?%{@N*D@rj6=(z~+K zK3F!cIE0xlJtYEQr6gkvmTKm(*gP{@^QVV|Bdz`7VLO?R9VFbB+{aM$w$ap-&}6rQ zpT;UT8I4zwZG*G<6_N3PY$Wu+)D|6hYT{@%p22rrikH8~Nj38nYrYIcgQ_;0th z^cX;M$D@#F{{=Qffv096cikp_E;YF!Ss|2vz?>J#!gEmQ6W?bzP&pR|V_L5{hHWq$1lWTgHerwlJeqcvHoX7#CPX|DJ-4h|8{N` zY*X74!&SSV8|{u~3{EQu%BiTR1k7a-^x~>b;Q5a~B!-n6I2!x-H5@CLywVCZFzS4q z9WKj(rgVhUBssdQ3)kR}0t<@iwp#)l2Wo|Y48f(uXY?T$ z&Dz(>TNGo-R7)&p3NzcRvFcSoEtvp;Rn>I?W=p4=l~cJ+Ms)cI~-I7HqrZMdTN+=PrM>Z(H4 z(-2?Kntv$V`NLVBQ6d8({yl0V@PZ%YVz(JwY2(=8Y}cRRH2A*8hrE&Bis{l%@S>B#609oO&koH&rpGWMo8o#A)3H0K~dyC))%K<2Msfk@&$Me6j(*+bmZgQLAcct@`cunU>N zhfYFyNXLQ38>_AH`KsIPPQwBD-7)@Q5_fn+eAe`>`kdD5W z3+NXgmv;`kDtet1Zyy$9>e|0B8N78MwInz-6DzKL z>tW_-e;~GcZ%v~fHez6^@6KXn<63-8IT6Z`G zxA}pbI?H4Rl>KLmaj6=LMYORozM0YsykOrR@(}*?9*G=2StuebbJv%Fh5{`_oRw6b zRn5MNtbvCd!NE}svy0OTQ^`jML`DG5L}y5+Tr5wANgq+aX1})K?^Zn>r|#}<=huJD zZyNn?z$Effg&2;T=#7V$SDoz&kLT^_`0+v=0=jg}lP8MdPMd`)CwA!QZ~}ov7bWJi zZ{XN*rljrlgqT^M3HlG@%Q4@ZLZM(v7kGByjI<_zO1~e?doZ-^UP!bJGCfq8rjnD< z?}Gh-hhL;(`Kc{^xqn??{0#C7j_|LV)i$OrzrNKQznN=iDz?KSCvz2!@|_90NJ8`J zq7PiXw0p`AkT=C5!2|5)qxr7AjXZKRJ%>Rx31;W`bE7cin$PbL?au%g zQ(wUahlhtB&({RKA_W##ln!4J6qzm3=K48e&xq%At~7D$Rvnm&GI(ALkAT$ihRdBV z=zSv$hJg;PMsuA8(o&(1M@iYsxXnt)#R>Bq1%P{#vzBT)62``I`)fQzgAnPWYS7o{ zW2xq`yu=2G!-mo~R1Ho{<8SAvIlQmoQEpELTFUv0Mo179 z%MZ*=nR?VUnaYD-)rhYUzg+|bgUlH*K2Z?enT`riRg}KXOqJ1kEj?#RQT8TFuzi8V zPL0B1XK)u_R6F%Zf`P%L!i+Y$lX&>@yl%g0tB8|x6)o#|ql>(!&(q+~RC?ci5&a;E zhy7_?ykEoO0sQpQy}n%jm#Lo!47X-^xT)?z^~~ek(u$2i3lpT5B$i(0@zSO5YR*=Cmj z*j#H-%RBvDdIyS&rCJ}<#V|`>-~6s#SzLms~tkraOvhZa$tX zO2~tYQr^wn*FC2qw9ysQRJAm;VH53m*?4oF;Xm8U7tJ@7+Znn~VNO@G=Oy=?2Xd}y zJU;*nNb$9XixMmumB-gfFxLO8C;`SkRZX`bq05b;O%Je}-v@@p8lb1Ux_a-bUDXH=Ada^?SDWzQxLN=7t}#~x%=QPK)o@L^j=(CJPK6hB!$rss)61bsP6j<_jp(~CgH_IUEfN?Tk@%X zvo1y=ZvG&A@*K)7*fAaer>gO!1P13zc?pGFEQX`jZ+(L%aNI>USmUvS3&#o^02G_r zX>T*a*1F^)Mj6`^{Ir{wnz+!J)3>ChWnVa=nVpqm7ipPO{j`tX3&~}3j-e~gXz|^q z*iX&ZCYkf0yjM;TDa+Y4v~5H{xK)KsC%=zjp^1DkDk8I zPway{O%a3ef^c?jZkeYxlewqn``rbVS^G_u`JCUJU7X_ab8#&RV&}+Vq!Ub+DNTr;EwYyGY!W?a89@)>}+kE@lim9U?^! zZna1u-@;*KsF}6Rt@-cuFHQQz{bi8KmNof9pJfUYdnq@03)+owFWm2nFaZHyT?;i6 zCueR*IW9U%^LbVYa+wp`6K70SoaAa`KJB}hK1UT$>e%=$@ne72cpq%b7md@bTU>uW zm{*qvlsVX@csAG@;BdHY3_sX?cks}1Fe@sJ?oZ>b{e zJRhv)IJ^C%|As^-930py0`*44$XBa2vrc#^Z^Eb_kB>KVGJhWOY={N`NZD*gWy30y z!h~?JYqCg_Rj^58j&ZO{6WMQ1Qqi-QtL*$D3csGHPxODqsiN>9 zQG4GcVQfre1MZ#ux32pl%mk1swltO>ijSyhXsQz#{`?Ar{(vM}-Yv9sNa~{A7V$9< zrfCCBivOxzAkU|-T3o4X08Ge^BlwhlyU?>pnFmo#h5a6^IK}wOzmiWh$-2ZF?c%6x zLwAEHRCHmT9IZy1*_MBg3NI>J{O^QJtVMhrfh3BO40VvVcTiV7g~S-Yz~G;LKaz>~ zMB=$dITbpPclgl}M2>&`ZFziR(V@V}V^t%dxM?cpD`uKM=_DHYpvTmn(AxMr0=U8D z5TTQ+lXjZ4W>4b1hBkWWoRrbqsl+_h>yb*q+mW+hbZ>p~ybd1Tb0OTuw1VULz65O@ z8=RDqaT|BCQt}Ic-q{Rc8&nq|szuw{*DQz&qW+M58o8iH)Gdp=3CJ-R@|7Cc-KQSD z@>+Mw#`?ziJL>~00hW2zgmz&&Xpluph+oEv)ziZoRScp77!M;~@)y6w@!C<|GBb)} zk55cagUS{AF6=QgcdM^FW>Vxib_o7jX-Br5`cm6wwsI!SRSn_-9Em^Em*47b@a|nZ z7kz65lTlGx-+KkhOfdt;?sL~Xs~x}@`*}6kQ~~e=tgA!pfKph!7H({0{o*<~){QWc z8g!E=#y?5@(@2LH9r%{Z)P3sOLIQ)M=E$gYUyx%}XBlPWdkI+Hh(kS3L^dOz?FJjQ z!-^{3!F8V;9O?VflKq#Z@I~kn6P<{plP|k?eE#>y`ij31iHeB<#jdg}l6s8k z&NeK*v)Jo9UUS5Jx>$n{u(i>>ixS1dmp@Tom%lZm(CAUUP%#Ilm1QIg`5*QhGZ(Bl zC#QW9HomE><*?l+E7Ich5!ZHq^BQ5j%D!;~0;K4ZmoH&m#!`Q5HApT$syKv*HCMaE zqe!)K{m1;xn0D^Lcu`&fnOZ=CYdFK&w%ugQY=oRS#QMImm??!by6rY=%l5oO@OC!O zjRz*fpv?lbPg_fHL?8X$h~clhq~1hu&tp$kK;#}CSF`QR|K+rAONVL74;gg-Ze_-6 zOtA0f?J}C{njP;fg!B8bdLi!@FDfd4<1iiVObP{o$i~D=_xfTn)_so~F4}Oo?c2eC zMPP7kRHbLyh(K?5U)%8a0z#ePE&PN0H;}h6Gd6O|2}Pgo3VSnn$#~T! zDYMjVhod=}u+4w_5Bk2xgyQ}I0R*I^{hK3MB*JdIhlhtBn-43}ASAU1((`-!q40AY z7-~Ne(0AOvk;hr&tfmx^Vz8OUY-ID5*5@BNE9gQKVoR#Wyzi+?8D`tse|_a){cz(l z!RYGe_MwCu&%OEGSdT9#!13GMwW|#&v1QIfxKMQ8vC_e(fp@fLm4}04jtBF}Rc?$2 z`A;P5seCi&WQ@m>AQ;v#bQ$BlTxpBhsw8LJFFoV}ZOm|^6NL2Y(vWYl$)12NS5O>Y zTdV!#LbHK|a+3a;$ZUx6q(8uHUvjeeGKik)`sa5kAir6ZHgyfDPiJ0kF>k+jLxn_?tf(Xmqf!K`h<5CWqbzTL;)}ha2vJtB#MOS&| zubWB+H6u>$e(~%0M?CWKgiZ!Kst79P^dFq-5jJ+y?rFSXIfnz``(~%ahD`g5tEidI zj3DEIn<fMBN+> z{^mYGe6VtDv2{%wW;-Um7=FJ#K?iK7Tv{i4#P>OF0p#Ag{t`YqFy~vQenk)u2JMLB z#!KyW4vSEBS-;;CDQ+v?zLiR;NfJO8k}^0juWH(POF_Z!rzH*1dP3piKc1`m0ZnT| zXkED!<8~Y7Vf!AHMvEF{`vWcFglT!NYW1gSY$JpbCen!m0!m3|g ziKwkXT00f17ebTB)u8Xq+7!cckkKN08Yf3oS9vD2*dh1%xK>gvCMN%bC_Q=`W*GRmWcqfY=-65@?z{5L{}vxK)dUxb)+%rP3$3i zp7Uk+;gF$8O~t&E7%K16j&8`HM)iZSo5=>N6J-bLlqJETigHJr_hA(m)7bNj^Fyfy zy^;{x`-l#~f(1C0hb(`8>6pL#{pQnsvuE#`z|A_K^h)XBFT-tct);VET{PzzwnlR{ zr>~DxO%1gBjIC;(G(@<>x}*dslp48GIJx7bM6hO|+UiQ>tOrR-VHbTsgN(ys3+GW1nd z)@Q0|#d6PJ>&8R`Zue*&D&U}M+n%2@Emoy%2S46~2va4NInkACpRKJWoF6ak>L(^h zY*sinzCTYdttek0aI3KsB2yk*PcqujbgMTQF4L{`20)yX5{&b9nePo1+FgtU6~7Ly zx)7*lVwVDYjcys!+@^DG%ieZU_SJI#IR581O;9s(R>r|xt=c(*1wg1QshE~w@^@&4vw=WNp_Bo zTLcD376-j{Ppa*74yCEAeK%3#hhb2Dzrk#v+!!M#`(doc6fI1hGcOt5Ra_Xw$2PbyEh>7mqBu(9y;huH_L6J3X2 zvz$zs$32$bk~-fXh8l-~6DzI+&|L@hbT;x-9qLOI{|{CC^>PVeM4&(0^?{ev)Y-i} zW~xmOmI*slYdy)a8MwcxXO-|7-#v}PdjsYXhI!{eop1fcYp^Iq@42fVIY5?{WCM^h zQg?Q9@;{Tx^hm){u$NEFN0XHO1I7$a_D--}QZjksSAL%B6W9Fw2I+~|Kr^nFb|tw^ zVE2p)t0>|1xLq+!k2uV|CW^q{hxLubSC>mCbtj(B9exO4xG(~)i+5APW>6XMdmkIQ zSBI(x*ne+;Q@%Cs&MlumzhZv{GLv?KA%(7~#LK!wY12cRG=D5_ZLRn}r0n*%MFGv< zfM%~KHolGcH%|b+p|b*(CnUzn=6ybwD%Mblly>~AluX~*-`D!8j=^bg(Gp-i9Ku3j zsg5_SAYw}`z{6WDKYeg$z#Rt?_5F-1@5pRy%z>u#uytRs)42BIQEQFKGs{%*yw+n+ zBW@qiC9VRchS>Q=@`xQ&0c(%&BjNDa?#&ip;mYJepIj-2_GRb73VDYg!CGe3%#U$F9wx4uZNo4tl-CA3U?EL z5L!>x`h${De^+7E!Mf{qn}sLp^xO6pVuPxyTb{?}USXrBcU=l&8;3TF{dLH?=N^40 zX}->9aRMgiSYG5UHm4WLUqmdtK*z?;S@)#rPB81{-TEjZGN&~q%Vka<`UDPNT>sku zDpMq_SkU-aJivm1Vh<7)D$oP2K(T-9*~q?2P%BB*$I8v~?7ThS4 zIQM$Yx%~y16_w5BNNcK?;WYgwUbTl(r}N^v&Js)K0$;6G$ld%>&o)hOgEfQw29hVv zfjs=UYV%&WGTkzeNQ{L)Wow1<-9sp?g^qs8l#}UAf}rh&$|(EmFU5pV?KH?{UW5D~ zBYbJ5^6W{j#}$?RHMXAk@T42y8%qCwX#DQ9Lba8c%GGN3oHT*!R(!~qj^JzidPEI= zugJvz=YRYpP#htO{zVJ_JW=NV zEp^iX@Q=$Ol$Vs9oYERHbn&LoHeEW5I`-H72Xhy7e`k*HqQ1-hYAJDqiy)-pA`V^` zxaqX3KTA-)Mhj1D%iWFC9==vc&#vq6Q!2FI!~A`=AmU_%527#25Vg}n++Wjzel{Aa zn$;6IenfzokE;Vi>%FeBE50#iLh!EMyqyu7dfpmKAu$o>pb3)Z)L@Z*0)=L<%*1a* z9@xA1vHU@Fd3kv#r3eKgJ4oSqWBo(1zy@mGE9?;(K(}X{=J@cQL_Q72qnl>RfabaW zG*J()w+M(^>DS;rHCggv02t?=vs|^Gm#45X5e(gp7+Wt8q3h^gpg$Qs)uL@aL+Jr9 zQ#f6(>J_N}-d-Q>W)XoDT59Si`1m$mJOe{R>&3MTdR@VUAH>9LuTD(SaVew{STvn> z%XGDaL+`creKTCJcpe!6@jCzObs#yCo|c?6}Tw+^(lR;!(nJ~DR-_E<%k&HckF7nog@vR3%YKUzj%M?kYc4#xh>7N0%5w>6pVUyu%*2@(>0)z%$jp@r3a^ zFi+0@K2txel@@-cQYT{7`c<9lbhVWi6dcTO&;SKH0&LdAqhn&6_v<%Q8tmv1A_T#a zkr4wUBM#7lAiv|jRLC=CVt^4=hUXn0YxBh`X0@uio80&q?e)EWY$~C;t$5NK?Nxcj zGoP^_`XV7L=KgiRQ2Kxs;_I@k`Q!}U-LF2P2X7JCelQnkTD0=mFEWw9dZPl%ZCvSlI{=q31UmnPrN z^y+N8E4fs-m1A&)Mf)Gi|Be8MC}mP%uxW@$e8&Y-o7+I>N}Z%%@5O z5p_Lo$Adh(v(+I8kGBGc4p1Hx(jZy7geI7>I& zSwcHG_&k{6d{vj0;h#DKY8y<}4#T<`M74PF*9*u6+fOzcGr*JPp584_0ChVdW~^8lZR zk#eDYLx@eVNI3iuEj=PK-v^8~xt^ma>ZJ+5`|FmLEj$vN;-*$J6=SU8&(B)y3UoUl zI7vi8sY$T|Z;?78HeJ%|P+54$&QKH)azgo0RVA5#?6~L;WBy??<*vA5;oaq~ASPRz z(wQcnXQ;mE&<6ZeYm{RZiy_iaxLF$y-enF&3Kk-w5_WsD zaZyxa9D1D(zh`C9tJg3Q&`NzXoA?n*#uM)4<%O~b)9O!n+$xcJQv>iFF!KzxXu?PL zGK?-5*8KQk4b;uaoJw35O`!V;aqPm|s_xx~qv}#W9~GThswn_^6LwEUR`!)2)1l`U z0RB1jvP~>5wT}W2^~!Ae-J!3(Mj2mTU;nCRW_LL;SszI1KrFt~%^{*Cu(h5yf=GZp zP4|o96z)b6HA6Mh21b}LnO<)j=V}?E7hVuZe7$yN!rf77m{fJ9vVoVGdSWV53hh z0Bk(5E)D*odgd|_!hQ1X3wXB5%<&5Zfe+{%?vTCx7VkmaG87%5%;g(Gw!FhT*d3x) zab}#&_lp-8Xj&P62ol0@HdrZtDK^C0oSa=1GL_ujGF&rucZ=Lgd9Y9&9dS`4F_vL_ z?&J)Aut!u2)nB6i`xgts!cNy0PZ7GI-_H@Q{L;V7>o4E)_&*zkA`;MmDR%?&capS1 z!r^UW&@?jh*wt!vQVlr%ual|#SFGGlFUiQegvWw=7YLlTxFV){@QSkUw^<_gN10T2j|Is+nB-g*)QKdkLIZf^^06_QyBKg zJatU`X;xk0p80#kua0VMpr{gbpM|Dzy+jF7FHN8h8jElpy$F{naBK_3SQ|V=wt}A} zWmKAROymO;AqA7S8rro8Z#<_K<}H{!)(UlNpkPi2(Vv^NJ&8$J-UA2x)&i)cEkg<} zvIN;-;2p((8b#k|YMj400>8C<=xI#iPQ?hYHAewb0GSp%lf4&r*C2B{T5ubdj#tcl zF{q}%aTa*^^^91hywiQmo|O2(y7x8dfx#Dn?o%d6)DY3)?nZzG*`B%5chBBauzh)p z10G(z-hY9k7~6aHT3$FNY8jgw>ccG;RswBT@71JdxQ(~g<>SnM=EBo*N+^;^yFVm@ zvN?)7W=d~%SEo`}cO!wS;N;4MNc;SU!ctRoEojxhz}RY82Hqy^RDb3CG6|-$JdQ0` z-|@hZ0pO@&Xf}&kQP>y$wZ&FU>X~AR#6Sn^bqctEktT@}7wo0zF`LkJ|K*gANJ)z% zMhH?f>S)bCvO!TP*AU7ox15OuRE~GiQih3sblB4ZBg}<0*Knn*b=^*fzp@-DWzgSL zRi)l@a>mA2q60!y=zwYD!561rFf)JE`>>sqlcnZApvs$H(Q_083`ubJ7)s^y^LyH; zS^AEiRS&&(ty`AIOBZqpF*3eAlxlEqN^ov6d!|Dal1+f4fyA5Gbij)%sF==BpQ*2O z!@dBGI%c9LNwt$}eS^=$2=9UVyeA$T>#Ge)w~IJx{^6}*WOhcWn~lvTEt4jtyL|NY z8Bo@EJxlk4M%OjrWVPEKBDdnMk{dL}!B)vpCfHOyWf82uQA#{CV2d4{`@p}JQt6f~ z@$}ZsVQ=Am^&1eW{jOTke_}(FzR1WYzp&v-SI8q=T#@q&L=E89XwIk1_TEZ{_#=ZY zp)k{Xm=abBIT1S498Dh3o077=yV|q4eHn_zB_X(Vu{uX3c5(5UPpiNP1M+qtqKDV+ ztiFf|zXAytDda*^j0Ai^EYihiH481!ZV`-dEEb>h3fXsZVzs<)BJ#QtO5tGSh@r;} zKYCvirq2c8$aIitPjcpn!d<5{IxDdoQsI8r(Qmals5`GZK`szkwee#3HnKNH6+n)qozfl^ zn#%6l2J@{uj^7qkfshNZ)G$FiHWwA9Q#N08pjhmW7igy&g9^oCGmwX{a^1-xRqm$G z#T{UNjamy;rX=0@DL(m|=mvS3C$oK?+DHk$T!lCX3qPnmMD|dy+-{rl)$?AJo->KT zQ2aX)>jV4$g&3Xd7bpKXCcM4?=KD`JSkHN%1a!Pb2$)n=RXe-7maTXZg@rm%Ag$sm zqEG1l`n7Wf%H0XLV826yN}JeIYbu(#0Y~>E65}BvE$-Gf9ub;o9Us6UFXgTkqHu}F z+8#e4>iDhM=#Ywwr$Zp+%5z@!R+!Fp{Clj+7x9TzA#feKx>yXDq_f-zq!;kQK23ON z6%JonP1{N>Zw;5YX`|1pbKKAvwioT6&F&b$@ z>ak6G@3+yMt+_%XRT5K`+`FuQoYqH$r8lEcXxef|fbCK%^61zYpVbmlM*waQ^tRvM z@viP3E7#DP6Z+8~K*u?6&p>v5!vIKoE(@s_E;1$LclNej#51Dhoj$RB4)$u;V;C90 zSz1%^e^7#D#40!>(c0p{?BP;Pn37&1p^p0k#tXOT?R35qVo+X9F-|F8si$q6>ZOX{ zff&pMzwb0a&3Tu|TS2I<6>i{Uh}yeUNf|9J*piuTLD)=5*yT^xZsXHEpy1{@VBc5r z>{*g2Ns)?kXfe`sf&8pT5@mdtzh+9=W!yRwF=<qVY9d<3Mh5B09xX|E#iF{?^<1|$|#V;)SKIJ*+)`STcw z;@*q=l53WbBtYT-6%$*$JQ;3N=}iN7x{pLVaEP%5*v#)u#!Iy`5EOLu`$x?0-@lh^u2U9A=(y5F|kCQHzby(UrGFFM81NYQE!iS+jS;#jc`zL$1asUhvPvgUxa_e-sAtRgu88(#(nJG^+E z=*@MGR|z$M@7r(sEbe>d@ z75ZwLEORrZr%l8>~876m@xs0w%8i3tX{Tzh~7BKo=9q(!j0W)`kaf;q1~GxWQ-0Vj{1_q+Ve{q@YaPLNXfZN+D|lBeuEG4x&jRpEC$URIo9syZx%u0F zO5;qjPD_pZtdxjvk26&DHUgU;KZJ5%lur1jr>6w?o4J zfzFcUa1zEgiM+QXdH|q6{Y4ITYuR!PnRsqaJsbPO5^Y~q$1mUY1If6V5K*B*51PAc z6Kwt~n8uE3GY91flNhQ9suU4Di$D&0Ut#TVvE4&BZgCfstLXq$gE+QW(LayEq&M)# zS)*9zv$(;K1$XmNa4-f1@dZYM0z(l4flDma_t>zLPEHSzSeft8u*a}ZV?~PW!xGi? zWAyw!K|BFJSq37mzMyQWFrQ=SRW9eUmmu`banOBH9T+Ks%(Wk!v_l~nu1X9*urk4y zSy4q(fiq3>G68_q`B~fgavy>dN(M}{4@%P^|A_#Kj}W&MspwS$=MEd7R!iewI7^;) zD(vly8XhtDSN#wzJ$Pp)+OI7+yCubO+xXA{b-=k@PO6e?yNE=4I8L6KNNaC61tUP`5ICYo0jTlH|` z!NhKNT&%V&D;-tZTvI({mOO&Iv7oW#&aDCGp%z6XM8rEwwgq`AyQXGuRy)T1cUVxe zA(0SLYT_OxTB8*-ydSF9d7e^tcuUi0ude_72bv}vf6b-*Ia5--w=s@aZ7msaMn+x( z2{=Dr!`b$$%%_Br6$D}!{}BPVg~z0&#lcSz5{v!)w{Nyye2w};P~;5|1jVj$#wpKu zb;n(u9w!%*aXO>FXN)&>9}UarJ<8UCaA?nVz4pCx+pAYTwGp@oj*2Ld`D1-Iz6kCm zfA@A*YyKHDaxNH*^ACVSkY{c)d4wM4pXYzY*gdb`aBwlkJy3rZ9qHCs_vxQ^Bfh)r zxm3r9ZrNA=bRXnc;kNO(QK&HloElAUNXTlp(;C^8R~`)h&xQ_jmviXRi=b49S08GD zY6<$POUqk&)_ca;;r`&&+OLa;$LxsWUB@PsBn%aPPrkY6)Gp0GVf0@g>;{`q{Y6q_ zq?PnpHB$(Qx$)7@aj{LjnY6>(1^Ok)m^DP37xA8Cv{@{yj#lP;+G4m%2J_Yu?*X49 z)P;z0x0gXt=%c1%*MLEdjAy#AobCk~y8p81=e@Ie0f(0ufKt$3TCCrTa5VhI!rQ>k zj#D95s%^QhA!B@cufPnnry(YX&Ebx%@@>?(vg>S^+b!- z?@dIZ+Di2X|4@6Tv?=crwg>LkAY;a$-q6V-!HJ>1Mq^OUzO?>KVlHy=?|=8})n<=y zr+2a|glMc(TV%3G)%MHT^+^J~AlHuRw*TZZEIsRSHMi}#n_hI&7W<*C7WrtxoXg3| zevh{P%7w6^gMjOo#{hv#ePVQfy!Ob6EuD0OyKh!LLN*NydxoSV8^vEzIffT+N9||8 zT6yVm!5*I!hBf1wT^_y^{gOVD_$V<|?Uvh}EUpVkOzbZ*0VEj?XJR8DvT~&1>0e;? z<(=v(?R;f=NPv}E7Li+8qQ6=TI_wl`+L{2dQPGv^%sHMvDLYcVYN?NFbm2}DGx}MZ z`c`~#>-pqU<$qgG{ktyG|DmT@FrOKMg{oiB?mPUFeFYpL!!WBCvffcogkgE4^QK7~ z4?r|vA2%clr>jf}Qn*WX6tj7zCGi$qsbe;Zml6`cO6?l2rsTnw>bXokn^{CicAW0Q zmzf@V#3r%tsE`5D5F)ayMggy{hN_XuFyAW%y-q8K0+GSZDU`ju1N738p0y-Rv z_1Jh&{_nMV^-*CrO2NvCZDC=dNWC_Ep~)5CdS>zN{d-o|CPUStP|dDLR*GDp33;=J z0r#Qt3JKueJ{q7v3_Ug^{SyA#jvV*}j_5u(IM2SR`^r~4YflsR=-}te6En4E=eElA z1d3URyMa75icenb#^3qDJf`?ZeQ6W~4v&Y$tmi*pSL{?AGxkPxwkbTBUo*P1*9Y8S zn|PenFO^6Mxk#8B>~~w=Q0#ck^2jZ+Mr`aEnoc(0HzFVi{j)(^)KA9)W}=JpDfccS zkWqO0x6FvB9=(4rQB*tqB#wkvXXgip-RZZ->x0MGRLB*YGg9~V=y{_u<@^rVrL4pd zgver=qBiZ!e%9JEaMh&^u#{BmfFH>Gmu9@yk7I*ga9Jgb#9!#o(U}T~wP7TgS8F#B&8uzqFv?Yn1 z#*8g5dU*n^n-=RsH9J4GvN12b))hXZ@tx$IIv zb&86BBUak&@EqLcynLp7nHrLGK17@o=;BOsIM=*U-z}Ep=E%tK0Y~3dUYI@akAy=U zvg-VA4afG6^L^R6>QuV@`$)?zPmllj1W`T=lW!_MC9n)6vis)e=f5kqt*k#34+uCs zJD{CQl8y(2yxc=qU@t@5NH+A3w4!x+D}soGI-RdYnmy)L1d$I}Negv(m0TlDWitPr zZ^G@quD#cmyzYTgW@uE~zVsn4i@I-`*A>+HC=0f$b^7#+gIntkyYXG41y!O&Eif^U}FK-avUmK zw7#nF)<=&VrgMh^e^9-hTCD9+-zQe~u1W!z44f*b4@s@MD9o~-v%7IIoQ_|yjmH)s z#&I`2Rjd&sI0=<*Q7f*mZNl&sg$|D0QPt(-vTp61MY=fXPO>!L=sai+qsre+>1w0& zq{H)^jd$pg(r9f8b16Hxp8+4&<@dLQt!5fv#3-Msm%g$mZfTA144Ky4nr19@wZ8zCvX$!Slh#gcd1=d_Vei?9qH+L4?-J1a(s zfJ@dRvpiT4{}DPo94G^AL!z~TF?UtIt;hGzXvi+t=tHqnsZak|;+`~G$)|vv>i(Ma z2n8l6tx}q{7BNAUIChU>)Eb*7$Qp4OiV#G#eIesc3FF-E_329>GJ@+aDy$N7IrI6> zNxvXP-T(T|$$Egt?PtrQuc`8wpuGbSdkPoSGe>t3XTZgX5k#aw{U1l|`B0hV&NKC5 ze(Z_{^7GE;AY_Z_l-rh#zF99W<$S6{OJbu0TAnyK zZJ|`8#K`?(L9b1(T!&q~Oa-69*7jUyZ#%Ba0(zENRV)K_!8L69?Yw88tm>Z4rLam1 zzcpVSQ2)r_zuV*EIq#BGHX#3g^Ixw=7yo@X1#TG1Z{_me9?99{zwgI*|KImxP>_O) zfL?zPT5n7*6iWUp;}vu+gcKMW+6slPt}m9AjUn;eBXG|e=f4{>QG7q~5vwEQ6cxRT z+dh(jOytYO^!7e{MMVWtm-+C48Vrf@HaIC+=G;3M$XV3v>!fW?1iFrku2HU|w`LT>d|^x7ckaNk4BB|q76$#K&KUkd5RtFd3OKgE3om}}*oN|^|_ zqeB_%XbLyTAj|Z{TQxL(Gdt*Vlk}J=f0iy<8VWi{j9;UohC-pB5+3I_NrL%^{7E0s zM(2eo&HCs^;FF&H8O&(~$A@k8(KN2sJ*onRRLaN^m~tuy?ZTL9B#uXz{&pDyl#?r* zW~a}{L!92NeU!Ox%1{`gm6~A)mFjlzi9_{Y1uy9*6Ac6l{kfkZx{2mN#Qr`-Ewp)Y zi5-C7-lI-M9fQ)3m`(;bB6l&Pgo8!B8pKQVC|A(wqov;D)!6`|TASUj%H}dbR@HVE zTO{xGE7lKqxQ`-KsV;1xoJK`AQ60ht9ZcRI&A?;e9zKCuDqGK&WBE1d|gS)lMqI;@P6MbOtgt-K~ z*|K285T(cuwA}^K-@RD6H~KtG-a63NRXXBfFOloc=OY=ud?Rf66yV7}P;fxp8tAHH zpKRw5!-D-d8^Ax7z;KDn=6vw{@$imxmqxc@hM{nI^jlP-@*O>_B|mLM%Y621BJA6} zHqxP*0;%I%l9}k5B;1>X!C+c$THz*l3!3dE zHQ`GGJ`u`@)?k3g1o)`$A|5T7qr7j9xw0W{+0N;pqfq3 zX*OfhPu%-34J_VE^fB1#qbhoX2(IlX+AVEIO zBP+wFf}*0L!s7?*fM|?ZD_3XbH^@1Gfd-g-9Fd_7#1D`MHD}ulMSJVvdUR9tx`%i zPJ#IwJ1e?K=ACbA{7LC&8kqL|iE)NKV>}A?oXX*73-kuCUS)+uVFR}M_~ESgOM6k- ze0HcAlwZH4H<@fEoUzn&rLWW$i!J56b(?-6a>i4#D&PCa{DL)fM>x3HIr-gH?5NWTCbX`->B*E1QRV%?n75JusELvdmah#8 zePGINgmk$;+8bF;qr#Dk=d9rTy$_5p?TRt-GJ?zxt`F_3554>L#L&3Wuar4i!S#$F zzLp>>;7dC*cP!jk#;7ej4Xt*kFEZT2>PR40HlzRGhER2~_%)6}S;TEPJ7`m?2E%^+ zmW1n&CbbOWe86pJqaMcc^v7bWAYqRhGk^$xEJ86Cc!tC!HrwbECd zd&0?T-Z&ayr!*huy;is6Vf0hoZcqD7z6K)0RGH)30a@bfaPkLACpn>#)4!?QnhD=a zr-7na|MDf=74G#>$Bvg6?o+^ox=LbWhy7cImF zi0-InAG1t3R_b(FH>M2hNln@CqM{{q-R>4Pwk+AoaPCT`HFz-Hxe)}EOuXzI6z#l| zex8Zu&giNecoFu|Tvo1Viu8)-vJX){U3~ z@?C{`9Iiqq>hLN6L4yH{h*oLq(nnvXm!)|5Z(-kv`5T-4o#)g61-koEaAu7A{_)2J z!I+<$j0lfTEW5;xTh zbzd!y9jrLcXxzy0wdZP4CVUuI-x&s1`VPJylFR8k9%t6UmT;4?f|#j5#d3R?)8`DQ5f4wcJeZVQ}A!rEw%HD*PX?qZV^Y z_30THy@`4CDw=$)@y)1p6EY40hrBuB&J{$PVntIpvw77oDpy(+97bxzFd2@! zu<$8QqC-`~8oe6iIxjlV2Tu~Pd?0dbAAJ8^dd(*ID=#Zk&G^*j(q06Hr!6}JMH#9u zz}YQzS)6y|P^USvP1u3H$9zxncyfiJYW|*cP|NgfCA#5(qrdlr5`V1oKTK1yb8rk6;Taz&}WQ>=yc2sJ_2gM z{0YmDiT4wC;Op*W(?m}GL?{VmZnwy}Ei_oNLF4C?T&F3DFQx0q-m3AX-)11Ysg>sW(zY(-Pta%!d(}LsM zhcsOlHDYtN=4nmrdq3rUE;iJWrB{KDxCdUQ1^1%A$ulVj zrzOtC)nIEdnDAt1|QZT8WF(??FX4?Fq zv4{bg3np-f8MLMnpgEGYcA7n~3R>uycg%}C8R<8Yo~JC>!%x9}EmgZk<;>uCR5uzJ z0y5g(`0ZbMJH9O)-kIDX@R_rsjlm1&Q*_=2ejTJxt?JFAcfbbE8iKr8eJ9}-a-Fu3 zL1duD%h|_g9x(cqy5JTMfwxi<6}-qQrAHg3nM=nUALm1LC|d!{k^s+jes0TP-BNev zZht3Tv|V=yzClZ}z8G-n7;&{ap!fbcrbv(Now;g_!6N}OtZe@P_S*-*RVf|VP1zdH znYQUiFWL5r@;t{f*7gsXkMDSOTDM`o01Q4i;c(G(da9ACGflLB+6?pzqDOXvA5x@* zIf*$#7x}StyB)p6R?J@AFE7!&hJqPcmCjqe2uoKf9LvKT`{#wZS4b5@lu*r%R%|Ps ziZ}S<_ux{nNX2xQxf(TI@I9JQhNvA-H6l9vjTF;msyG(4w9_+IZ-JJw(^7HL@e}d* zPqad31Q;c2xtbqY_t&An^)FG^c}s^Qy#qHfe*voVR|r3iSH85g9Z8JEC->b_d-fx| ztyCHtAH9sk+RolDvG7BWZ(9?C=G@a<$Vz6Y6?~8prdeNRNk8U5oX>snC72&2u~)qo zd%_sUc$Vn^$e$t!d{iFWCX_fdcsyo1=!)gAEgC}VUDq98%Rlhp)CjvbqrxKpz1%Fe z`7qo)9#FQbnp_K%ns}$htHM%wJsyU|{WHBaVd2~UY?K>^6TY?Pc8ti`lO1E`)gsSN z{u4)?4|$0!r2=T!WS>!zAzf*STDuSll%rqxT}p$YDu9#HnI&i z;OO9VnaqYqitf5O0`Mg6ZE?TX(OIaJ2ZjM<6{b5n<6`7mnEp4~TsHSp6?CSml}&Z;Zun5<#&NCW4^pARv63jFxhe3mWC;EvDtcrrme@VMkUomB zi<1x%tos41x+Ud9Ldm9B$rnM$Q#R1VrYd5{Myb)*_i{ID)Yfo~=C6-Deqg2N#zLsg zn;hN!2g}(UM^TKhFO!LV1-@puqy^p1DzI2@erwC(zZKaE(_OJK3ZM2OEOR{0a56osG+k8jC%KagEms z9u25+#tK&w)Ieqck!Bt&k9wlqUlh|0wOOy*A==#ZO|k3PF-pbA**h_KKCz@7>~>z4 zQet+>0O=!%fh4&|+KecsM2-;c!2?5kz=!==P|J<@W$+i^f;QNNCJpS+ zqsOgZsaj4uBti}<*C|&iOJr7IdK6J#?^@37*Rc{Mm{}j zwKN-&4DjR=WdtJbU4&!b89?_TmB)+93D99bI@y_qv?IkI{jZwQbb{nb+MVeza(o%8kR zS4`FEUlRMMo&+B;=ScY>k)+1IXTq7|Z{>c}a~l4ut3kfMvchY3ubpCxA~~F-iW|+% zh6(VZQvXjUxAvOhuZCkb=NykIP*EPwr>??(X8JN@(MKqn!8G3<%LWGu%KS~M#lycx z{}n0U`tosKABfh(*ND@kaVK_booSVBH zQUU*3-KAk=&|S~kx@^IcvB^vh`6)_TeFU5jo(!88?Z(oO87a`tArSAjweCRT4`1v$ z&GzJT!BhOfV6W{I7e5@^DHMX9Moiw+NBJ}`ErBY@+$S!OoN~-Fl$`QL`{)h+uF%aR ztO~l)<19A}L{#D$+@@`d&bDUZIEjX_9Ib%CO)9l^gh~DSC()D?@Vy#h?9KOaYk!ZO zLRpni&`9*?2}d=O_;Q2+;h+fpt7yP4`1wrM?&8aVW&go>diT^bX{Tvz3)QArytMP< zp^83ENCuP};?KvBx`Q@idLy9dz;1hMLoDWCu~f
k2U~srxdnu8J4EfJ@G$rh06O z*5uGSW%rmQOp4YYh)JuUp_8;r?rg^>%f*v^-wZfknYK|6ezYDxF19QK$x zTh(!4>t@_C>+0csT7i(~T5mZ-W!L@nwT1Cj%O%RknC-s*ulNHH7BFJ5H5bT?AWufcSvGd|9><0S-Pt8 z)(t>ny&0ybCWpQi_BXA?zEG#hB=_SUj$^R4KO0`rFW4qpH;Uo7zC0klk4kB{J$`$Ll$=@st>_ycJ(@|z{Z=KhoDR1!z z#jeb&a9Z_X>6}?rI3t;DnFz?unU4;e7wB=+7pPn*{rt*8=0e#y2#DG+!q&=ucqnyN zPnSFPW8uVaF3101B6Z_X#?k3pT>g7)Hv4{#n&&t@dKn4$U6vYDmy(wA7v}=&Qfj@m z*FyxxN7)}brZ(?o0Fw2jZEoQi>u$Y8rRP?{&vu1^e>!z?qAS|=c!f=u6*HysXOudo zuJNBa#RJ>w=$l?!Z!rpASz*sb_~GfIH~v4$-ZCt%ZRr+<5Fkh(xCBXp1$TGX0D<7{ z!Ce~~cMI-r!QI{68h3Bp-M`M>``mNBd+&Rm=lanEIORdLYyd135!VgBe@lU4!A>%@JKt16zd`-5da-8}fWveKM zoht|i7X2o(ri<7=COQt3>{%rio(zTfRaAs1D-}W|(6u}7!n!+kBVe=8Tw{;X_T>wF zSizml!R?zkP2zv#ZF;>$lU{q@NwN=UzT@C2YVWHk1LDd)_hc^+PacL~Q1|irMl!~G z#^2Q>t|g?uN$cSDFCn-$L!1gkCZD@-bvWbhYsq3>Aev6P zHJR>7u<4@P?Ob~MVfZ}HdJ51fN>#f*JYNLFEz|B%0AX++lD)h*a`&$+F<8jIcSoKJ zI!Eb9U*gsn-hp{KRi&wYh{&=PgfB~59y7T#nbkj$1 zGtXOTD3Y(jn6nhz98u87iHo5WdZp|zc>+0o$#Pb^tM}Zw-Y!DwAW*$nsjm4t?BcIa zuXQ)GHd`KMIf?+{6(%b(tqy9et%$TF;!JF8$M<+}H7E9>X?v^vilg@hDI9w%eC(%vrLUZ_&Xk-e{-F zJu4T)5XpVxjo)RdAv$C8Vtm1JGCTHN;h5In|0{yk>$P(s_`-oLeq7%_)sRUzNl{;= z`#=MS@1x31_10NQ<`1ww$RydB)7;!|Sx;O5*`*3do-9n?s$z22pC+>2Ew4a!bMDuO zh={juF(=;XphX4J^utFhUpkhha(2xcU2+ETTOMF_bUqNL@IOaidRtdmxWM}ydH*Vk zW$*{NJ@Bx#V#^Fa1yi3DiWkU^e%*gJT_R@MFRz_elS(``TTt6?gw`#yuR7D|zU{XrTXH6*&;L6DQ#OQM8|Y8{@_C%P`o2O%&C2|nz9@6Qml=;z4ilZ^ zuqGK+BjxuT>=~G&O!fCdQn;pTfcKZqHJdSRC&A;kpLMdbF>Tvxe_UBwk9a=CP6B=< z?s9Dxi%5hQH9#pzFV3%6@bDiu$t(%*+a0hNK(OuaLwujW>jgB6268Ls#GryYc(F`% zQU0=;lYS7H=fy=3Rq!}+uH@WJ3$8A_-v`Yep?=w6)uz^uO6d{fcWxQeyIkPU6%tW? z2XJ^y_7^EwM`Vd;#MfJR<$hOHE&b4K3$ACg_OtYcDf?>Vkq0tLDlqbhYr{*X%&*ia zqAIgsEx@TQkEx&GW99XjCE^}o{)!)LCdT55U z^8S+9Ue`%d|BUdgdY!B^FjbF5Jo*~#dFV{?LE_ln=eSM)G*V~{zSz1(MAl~cT1m_L zn*dWdaBhyl8emS&dL`vv2H#sem#OYW_*R8y8ZBWN1;%G2hP; z-RXM!dEH0Cwl@qeqD&DfcD_qm#@bz6I-ahsuy^;zlDK2ZL!3%gPWE3i+Bfez3h)X0 zP;>1mDH?2pR$kT5UEA>Qtx&QnNUUd)w@qN1W&1%)Gp^*{ToQ_<_-4;`7gV(yDAe9k`>vQ)+p5?&8cm!l3b5{o4NB+V0t5k zl+60oH-R^C{B-AM7)G8s4!F~U!n7k#Rg0<#IF|0Q8&tfTe*OijvVa+g>VB#@l2{BS z*r+$x`*rhAHS5J$kNwU7bQh&eYt;$+a5cVNT&^qvHyaUW2bn#VyMh|%v!~Kf6RqBE z=VcEsR!lJ8t@a|$Xd(Z&pts7`G-FQCPub)5z5@&oy!Dj522Pp{#@XsyvOKLXFnzUF z~I)p1}W zXGQlT)}3-nFOLc^al*Sx-*u_r1W*46`4cUn#`^Tb)4RSav{4`mxa#U3NiC-ozcYit zGg0u=yKvDjw0QgnYVMm5orI%zZ4A3P3XLu7p$!Wl4Yo9ROEzjYWLySPzHxye#GmY^ zOAT9<&Bm=mlO8x7Iq0w}VK%)1@42Z^5=CDT2Mt7F{=i%K{}Z-UG8#XB13#kMWB4|O zRqr{;(fZUI0YIW!x=#loS)+Q!I&zb-V5o&gEVT`_@H>tVzh5#llpzg25VXhNDBA$PeyltPkTxnu_ctY#(un9Lh~Q zbfxC0o#)K++seWckHQhk^Q``WtlR03SRUWhKL$UFJSh$nn{Nbt@UIx7bEwffH-_Fn z6sLdfM`{6#EztQPj{}Q9ukRV*$(ej|Hy(D^mN60Q{l`u66>KhWroAbhw z>5&D(cKdVzbsU?X)mk1=0+sVQYl@&kb!-Z#L#3A|R!n#8{Gb#x-(FaF{on@UfM%iu z+;Pt(@Wnnf^}RZKo*%~iV?5aakhMQzxnnm^p&|c4D;EuN=xXrgV{kH3dd+#~DlI%s z$m8TxZ_3wXZF0;OARTHz?#4Lq*$AN%X8Zj*Waskgg38R;?OY%H%`YeOA?9Wt&Da(X%Wt(EkENR#*f1fzizge6PtmhJ$m8biHBHE#2Up$=jYq8 zwv?^bf5eO-43|BHKdY#tuU~qJNve}@v^=xiwMb+ZqX}yo^HFeMsB-et-2ZuE#X|aM zpYSM@;rnkNeiVTbU(+l|igo})veV!uj%QlZQY5)+kV3<4_aukUS`qFS;*_Y!iR{>W z(wIv6`LGR`x+&n(Tk=;0@5yWT-)qNY*tcoJ>*r2aKQ)mJ_Kd+}96~2zTduQf5h>o= zwVn%&Td3Juqp3@mte5f6p|_XBmo?=GzLozCHS?i#Mv!vtyFBjlx2P!h->t0{!SRd; z@GgY4QsJy~#%W9)712T1I=cyXZ`;A`8*07Fl^*o#nEEfc+BhYfXC-QZzUaWA1-PrM zJ-EXk(aH&Txp*>96pB~*Jy@%m{U?zo)NaGDrc+4|bKHq>Wcv0iHcfx2wq(m_|NPm$ z{+J7EmS+%!SVy4%*+sP%7E)W^ZjyUXdV7ecuL)(MJ64}e3;Zm=tr=3a32jR?6EBO^ zW-#<9d=4lkoM|PQZ*}7Om=iKpY5=P#nJmrdljHdmsfBag2sDE~1iO#r`5fbFkg&t$ z&|-L$MLl8pGFWJjKaERGlmvMXzwTr|RL7l5F{jp0{NDdK{p?N}vx+Y&lF%1BY%boj z$~@@93GmTA5vg_DO1c>Pna#zPEu+m{mVvY*HF4SUV>T{`SLeLj^Pcj12cf|<|C8$~ z^2urg?(yJK4fE%W@U0x6K_lqgRLV`T=l&-mtA*E^)=_^V?{H!}TPl_JnC8Qpl<|vI z{z%FwW%Sst9~@D=wfVZVwY>AO1Rowxa0A`-ATc25x;ox=wwvpyz?H3ds8I-7Rjc!~} z$w$?O&Q8m{rJ-ij!;OXJ7G3Z$;?KXL$s`@OSN~M?9PvssVKj5R52|0B>*#*BE>bD0 zm*u%uue>@3oWEqCmu0|%e5zVo{f)qApx;;vZkfq z58wdOg*C9xuET6-ye4zr$+a>D@a?2^LK!uU8-_fx7?-E?Rm>Py+~&#@BTScXJK;P| zK~LS&j^MR?Sg)4^%lNe{i_z|`#tN?$ z{tq3ZDqag}lzgO%#e+(k0!2S=0i`_py^FoEjyk53giDAsXAu_nP=x7e6&aDuS(d{y z-^ku2RbJ|6#W_IO9S?KR%cB^h$t**iURSY@cnF&e{jaw<&M;yl?3FHyLVO?j6JeD* zKC?sb9LSTZ2=$v@SdJvhg^#4Q`6+FAZa2L@vct*hOmTG0D#3`NaJuMy8uZhGx9KVu zVh%vD@X0_xcFov&{_}^X#2sP_FxNW4gctP8!HUwSU||~gxo^5F_Z(qFy}#GY@t@V- zlEy~KZP^R84pG+E@^yn%6S+;Hs{`D>g>|NN$+eF40FNmX+#28h@op%ReflYcFV4PS z%JON0QcfB7pGGBjt<-n4uzrUIE$jaW0y#*Y?Tqs8n45>(wH9K|zOt~yW@cuVSoLj< zqW2ArS*l`kB$fC7=p`#&x=g$~P9Ub#sT>-8G*>ClwVyjt9Wl0GqgFQWYRNvwM#2t- z4PEf*jXq0EX@>vvgwbIgYMv!Tni25zk1k_WjS;zv3{WE-d9a%ZmUltNZDXsH7=>qb z*ZRT$o8>h(Xj`V;;`zQUUS*T?ZwU3v7d0soq8%$zRC|_4otx+%O)RyScanU8tdOUJ z{vy{5B7$RRj;GUI3qUR{M>ezLW_|@laetj6WlWwS-}p%$AfhW##MPKlT0hsUi1lrOEDi=oYpDFZnfse|XtTgUKxeh|a5S7`U^63I7M~8(hHT zky4hhB;p_4aDG)tALmLSrulrwK=Qrz@!Txhx?5Nb!b7a!egatlp{FD_gCWtg8K~)nkWWFy{L+_nQ zih&^oPKV@}_Jb!eI)6>$yh1T$cUnL1d{n=*Sw zI$s+pX?psVkvCIRgjAzi%V4dGlIE$N_dS>4cU-(jcKfCqUEy^J?8vfElc#?}sHLla zR9z$!k~lZet=2pR(6F5zB6P)df}~}mFi%Jci=m-mtF8yCV>fcZ)^V}@re-8w_UZ$4 z5ZssG?)?m_6S8%8M@ISTQ{*5mFnZtLLFtte<-(XFk_gmP9O*1!%a=lQp zqXJkNjT&!$A(9|?O?PSZCr}Eql@3uo&BEFA%KR<#=-To~ZB4p-3GpGAwf~gwHg0k< zP`*vV!*>06wis^OIW4Y;peHH;vMy(R6eCQ6qQim8 zUnH*dPld3N0`aj%GDJtYspa(Cuv}mEC4nWz*DWzg_j=ww;*ylLXdRNDn;{p0>$Ke} z`;R)8?Cx?OYsG-}Tb8z-54JDwG*sv}ed#=Htj-CvsefU;x6c+Skfd=BD!17xPio=3 zkRFNC02K(sKBek6T#ZA^HUQRRM?oC@*@Q_9d5pGfpTHk7+SntJs;M%n{h>J}P(gIA96>EuWm z`&&}~9JegG5loJ`HlmVCa8`~^PNKnuC4;@*oa{>uvat9KxG|TdTsFsWyb|qWU7Ww- z?&WnvjAlR4IEWUPJrBny?SF;|nQKu$`8tvsj9^LJQ+XpemuaY)!+*aYmd`WOvqB-N z6g%5pG#ydBCkgDP<=82fiJ4o=2jmEGe1tORXqWrgJCcE7PsIbrut}tvMxM5oEA9J+ z1FysB8sPSCs8zV>ah)iV6_Pfgi!*OA&w8k_Qch&%x7eQJAGae^L1f(E=`_`xJnZo%Z|t1@wNyj?ebUGm)0NYDKWed0_HgYctm?{Yce=J39vYSTT##yQG1?JJKp^1*1ex2$1)h3SJwu74>1sP1xC1_v&_EX}Xt zt`v8!Ml#rMXitrF$1lC^W_W2!WMM=EN}O5tL7aR@IlzB$0qz)S0WX4E{?vPFBi@j; z!{Y_pJG}zZ&9OTe>5UHRb4^ZF3V(5{Wtg-MrI$OSsbr%%KFNIW@M`Sl12+p(G&-yx zazycOW2q~*^p$@E>W>dfl*qnJ|GcH^=2GtJeuwaU8#$7}lX30ohh{nQt^1FB{@fpt>X0Bok%6OO-QvyVduBETxM(pRDSq)ey>EkCdg9ZQ z)=6yFBms|cMz*^`^BOsvwI=*}u3SuZ60plO3HO)7crJOAw0i{(mRW4c zkj7Sc8%=J9L*+%%d=Gwo`I2kN$)ga zEL|H)i1TyY6HXP#8M(ZK&>Sjb!Vjy>w$J$n2Zmf8Xl(sHA?!?dWKLSX)X`5E$qn~p zJ$O4}*c%xb-%bP(ljYOYO#JRB(9BFw?!smj$ZxA>~T-aDQQN=sF+etzvCV$S+5w77&+xi z#~BLAYDWdq!b4!#O`@S!H(GY&s((hxB=YPzL7t~gzuOBwWT$0CaiL528&P>%ztur~C&pX%rgKJ%a1UgB-M*VM=JK>1C_dEDyW6K% zPaE(N{eJS4;gim$&Mqm3(Chp_HPz5~1|A(z~B82~QgBA4RP+{@S-91n$)Gv<*yjKk_-a=XtgXl#+Sm zR^B+tmwv6(@LcyL^h95+{n{8=qCZIlIEZ-t#aGMA@MebU!8Xs4Kc4PBCP6$lM^=j; z+p{fLc<$iCrZI{BD|v*xPg`7T5cd<>|47m^nvy2PDBn%mCs_$m$Z=woBEK=H`@P0| zq~pWX+Ha?rEk;`eKQCB|?-$DeYP-b;pZ@HnxVAIskh*(qGxl`Hg9{*ilIV+X$W=)? zc&l>LAz*4pxGYmnm}mN=4`6VgA9%c}O+4S*hMB|oM`m$?Xumvv_ z$(PLNlPUPAj!Z`5itzEJRRWW)A-Pok3tVXU4?KIUPhZ)Ons7yVfQ3BW&}-;zXEUN- zXp_t@jt?C^JM2<^oli4!W#-K9*cJ^ZVpdxNbG$wRVb;nrWKNAT6b?(}a2L7~CirCq zJIQ~v7})jwA3!Rs_=ybwkjAVXXbx@0>Q36U@EO_?@Z z^?vIY&Cj{?WM23Oxgs!$ph>hm1a>Y#66(>MG( z$Y^vty#}%;OV-Tuy4LYsK5n#(!eQnwT}AQ4xsQp4HQY7jt~EUFkuXoSx)1711H8!Yd!YOg zI2bfp>S*nhAa|s5TRKfdzG<|6)r4%Qi?@RoXNfi%8(S;#k}87{Oo+svkL`I|4Y=;N zzHkZ-Z7D%mHWJ3$%jJ=&&tH{`Fkki|wVh|45O{MzcM^;C119sXL$Q{l~}h8qyAm`v=Xhs(i#W zuc4P^9>0_5_czK+Y3Y|+g>#uWr@QN27IzhzV!CLeh)Z5_&J5AYcu#|@?Zn-p=h5J_rr(&Wb(ip$(} zEiBO>Z|M>VsB}&+c7u|QH*7sDq z@dLpD40=^&x^!kXTD6cx6Nk8+H*Z4&(KvZxvjrCAl{|Bi(G8rtm!aJUg!5r}i3;T)g8dr&sWQMd2SH_AbAL zW$%I}6rNtML!P?4{Rf(7KL=e}is@vLuc#NZ30xGDM@Br)^?qS`N_Z`y%G7JPgm_I$Oj515-05XY55 zl$15nG+Il!HeDGCQo!jqqShNn(G+fwbG-N zyS(OW6vG#^4!$6^7~?G-d-7uJmhpNZNvW|e?Xt2AII+K|IyUz-;s_gT~Ynn~AdVNU4 z+U}`V*y=SNTYuNGBvK;lwz)p?2`2Esk^IJ7j&ly6*{Eo$O+1@kfnbiyzh`fQu)WY( z`+IF&TW46}B-!`Q!9qKqPIbKJHXbc9N)uLSkKjo`=o7-cg=uV6{R*((ssHs)EJ+I+ z$*D7hW<1!wH@n9qDLrJrwz2}V9_1g2Cz)J8U;05DI$~SoPi`<8x6X1!zgVe25Q0QP zG1$9XadSQEuo{=+38)o>CAgHRb*U~iz7Ksvae19sWXQ0U7!*Vb=~y8pFE6jmz6I@8 z<6(F;meuk!X4+JuE|KCD+rGwx(R9bkRYKL(pAnfaMi<`U`3v~CBR-^3Vt4tnI77X^ zT+5AJjh8YS!dgk{+0yIN3#qZJ)2$@s0fSFEArF`?G#v|@YNRz?6AigRtlh?-%vs`7 z-L|{rMsC}B{1q555=?@M(e=?8mg8bxl1QGMpWe|u)R6ygIM|a%Tmt-Ib^yk`c^{L= z8+zNe)^=wPl}6J8B?AZl`?e%1L@{GKIKK2z;?lIv-5G!G7cf>47gB&2P5ZMahxEJT z9EBnTY~Hdvp+GfJAX=j&#u=1Upgu)}%-FVVt^gBKulQ3p@fywct!KGDoD7lR+jgdq zTOIm9du4V=-Q@P?$^h%!-Qg{c*uE`qUS+0s*_&gMEg|;H6+CA%RH_(`g#15g8>&AX z*&m?m^J`K+$jZ37cOP$rN7xP){*Av%5<+g%7^y!rS)PC@$HCdVuU)dK-x5*pQN&U& z;J!~x@%c7>*|E+0Lt=eHRZE2$Db)m~k<=40oAcJ#$SH%Cj1FYShmb8ox97b53?66C zOUxSwa{hY$OLTSO^1H9fUy3z8L;U3hFEO5>oc!pr1;rtAL50D-Sdte9LJ1wvUR8}) z{60s*5nU74=_W_8;v1LY~qk_K3VTy3FRW@++#i8<|SX zO?w&4Exqf$^m+uSH*-R)O8erlvo%&{7IHyxhW@WoSJzJvtXwwML5AETDUHR9>E6K3 znq|%4ChAF5mQ#phXX^{9u-KgMV=FcU%Z@sd`@10$Wd4@1U?VRwdwTCT)@}HTQ=pf% zPZY?sPSd;kQ%QE->B)0YqG}JB3q^}N@=937GLRA+4lZSFX>CtJC=YlQcF)_uoAk?E zyX#My$Z9J@fA}7MiaS6^L^0#%`RJi`^krJq5GgvnmKHW+N5zxO=(2rjeAn1TRCG)} zFbyQnPUTGLOnvY8`Py7)mPniuMuPD4OQd!@Y%~s?X2;yzEm3@p-Sa#gctmyV7BFtJ z0w-4g6pP{-=MAHiRWSu)eE_4^*b+#OL$`BS`pAi=2c3FGsT|BT3Yf@|)Jv_hoN)GS z2P5kaES!`epmg=d&K&>!l2L)5TfIJ)qD4yTfaB=2;vv<&h-TYmn$61Z2D)p_VF%)< zABni->?G%6`K2UYgx`ky0iSZ&h;=UP5)jQvA(`uJdk{~>KH({35s4ybkVrLZehNZ# zyML~=@!s6rBx#S~pL1`mrFJOKEjk>%!sWFbH#nVqg&zY%Q(qRl=5ERYJAa+T+;^*| zij}Tr8=%PrwG8aH-`Z0x`cW%3!RGEth8%lE4}0 zj>;p`V7Fc!`6KBK(f2O=o+FkYAI$yC{aF6W54a^H7WICyeX4F=Sg83X^oCSKYA;tJ zsw{{obzW9Z{>KNb%FOiihnO#4*{-Imv%K7Py?wF?Y~0*Z(^WStOuB`l^1pw|BJ(qp ztcsy+=XYXHvZ%|m8oUIyUP#rAj=>o_F|WC%_xB_MPB$07OP-j^ZPAQ+q{cP79yS)J zo!=OT6o2Nk=U^uC@T24n=b)-Pf7lc)S*}P^XcpS_-nBVS<+m}8ba9yf>UM~BqI1vL ze4C&LD{(Nk7iwjEqLedWqXG}79e}gMr>%2Gs>2Y_tW38(LUUaHwCU#jzDcv6u}$ZZ zMOypb>o|{XvpKb=QDtpt1c6s_0y;v04|rge{``{yAYE+pMv~XFI}Za;`Py24e|jrQ zKoOk2Rkl&ai36gS#CbTpH)TvnZJWQgKjC?~Uw(V^G2yIWhqx-Q0+ad|&mK7u=dFR& ziy|enHZY1B(15PL^wMsBo#&agLcv`+hdghyI|{i~KRg9Z|0%orNL zrgVUg6U=4VfRD~g_hzqL<+@*Ii-Q(%th~-Rg5urfvk7Pcs>x3Vbmim(tdA;3y;?s2 zoEqxFb7Qft?8O*l6GvXIjgo1uk|6QQ((nU^=&y`HP}P1>e@(d1yUDIaE*E4mA4#;Z zBBwVs{hndJ&DgMqfzdIv8|tOxmrulz)A42Kd?E2*>k~uL=XRW|(CdT#_)nJ<{y*{b z-jhNzF)=|}Qmzlvw;Nt;50+>(v*~^{9e5zTU9Jw~CU#3L9&-&AG)W)tLGm0oeVz(} zDROQby08vQeL2fnc9No6Y^Zv8R4Q4H?wsDaOaiGu!%eL{F=uGr5?LRoO9H*Q>7>W> z=J%qI8K5by*XMVqRBy(dsK!phOaT=}({iMxy&9YnRJ*D3lltLp}piRmb!RzQpvw=n47!T6;6HzSE=J2g~uI`d=K=KB;%&Q}Yr z{o?!kskHfKt)X=k?TyO6x3W;sFH!X~+_1MXT{xj+NJ__BZMn{#lnq6sk6B++zGIr) zDOccq$;va&RI7-pi~La8sl$Qy4XgV&_y>PonC_)AG{RT^v(HD~hLVy9{{H@u&dWTx zh>{G^yg=+>xwNrJV(azNYwL=$WFsDaJ-)~lTetwrHAN|U*>Sln;FD2-Lw_W}7F4lu zn2A#B($x`lH8fvsF1SgFbtl@9zJK3BAJpHg1bTFP1DowKXU!D5MB@`1KjMIy)2mJ<5g z(iAZN7Y3$!VEauOlcz#ZKo`D*)j}1ll9F15uHXPODX)W+xLyA@GUw%9nI#)k6*~7s zaq;vXi$IKt`?1MX*-V@{+O|N&sT$k7^qvus%)VWFyf8PLd0W4lum?!9tGxmCqj{x( z)=!~}Z-i5)e-Pxgl%O{=`|X^-PO!SDH8u6#=Y}$)1;V6st3AH+orFROJp3%EBUgJ0 z`)HJTnHY==JkVRcdwSQ7cLke4AhWwY)s)3zC;88rLmO^^fgK;ObgP(9Vc2ifu0btp zjB1k=ncGUHQ1dNv>eVj{8=5VB`T2S(yrrQ5XVyri^zDm+DRn^S^{dL8iRlpeb~|XeQy1} zQXTLDW2PH12;L_4{Mi#JYwC30(n^YZT3$O6#UT5QlF<)NftcS9dE>ap*0lyN!e!tZ8B6C!lvn|0yGCp$KE}xe4SrI5+aHze$vBPSZ%AUX%E&k8Z&`*6E z8J<&`JPAzj1yxEaac>}!)VkN!hrn~mtE)>Y&vwr0R5F|sB(ByS;|d2L8x8!*QZ1cP zEmcM#<|BPXM4Ni!f4R6M>amDd*(Rs_OMn_=y3{=MBsodsPy5S43Kv24sOblYYHhc+ zVAieyU*L1j_`Cd70EtRkteTkH#Mx5PM?s!GH0lguN@#qQ% z%5u8q$m`G<<|B?UATZ~p(^|XB|9iGNxmG95xWf=sU`A=I4O5-R}NeTDD|EI`f*-+ z9Abk{9urtsc*LK6z1@7&>WQN$+nFSm)qS}45JCj_|7~Z#;7gp%SGD*`CNbjyXs~;! ze_au|=D-s)EY`(0o~nE^H6`2M$Fi)?E|vI5+CVpV%* zKSR2@gzW6>x?1stSQaT#_;lCTvuYFON`U^wWl6!M`EUhO-rcVvkKzFDrZ87FgCRk6 za*870`09@N8ye+}xTG9?SkY|h>f?L4f!~(HyG;Zx8CkqvX_vfzM%Kg`tS5QnHY*t@ zJb!hcY{MCNl0-vbvwZrk7KwU@EQw}LK(JOH^IYV8ajLx&b|c9mo(iA(vn?-iu0 zaE|L*V^5x^&(x$wQz+nuL9|~O%@Pxbl?1;DGYYV~m+$H7i(#Fq9HzAF4f zCMal{zb`3BTs1xjy^);sLC0meb$sDwUq1ZT5aXmSVZ7)vVia@B0}dVATgu1#!)~MF zAz%}gxZENqVWp)qM{|zueu0)^@>=*TBfy8_;C2*d$`AkI0yM@%eHf6q&iP6G0+x%w zcVrR}u|O4ai4jov=QBipM6XvqN9i)TsnQ@~OUFM(7jQ9s?jX5pFqV9SYdacs^+OV5 zR;p1OCtWrig~kWBrA3QPsTcOaa#V+-@IACf44YS2{BvN?x7AbVwmSI25}XZk@7#a+ z3_;|bIcH$%#4AnuFT&jnpp@0|Yx%jlnaew~Pre|~5P7}T^=hOG*v2&wd$IvFD!xpp z;V!v;E?Q5f;$Z*hvS-lzjyYfgZWmoX#OBw(-&*9?`gfLi=MP)=be)DSYBY#t%BAxB zzmVDZLBdOQwPjKYO3d;z!h#z0aER~q8!fZTnS}HBG-FnVM3$qj~Na0GWNxd5ZYu?8#cLiw>V_vvUQ_PglbU4;@5r5 z7fsS%Vb~E&Y_A4b<+r$fSH=*m)&Ow#(5%oDNhBLzDLN&-g=SyG&x1cqjcgti< z67QpsTW|ec5u4mKc6ut1r2P1{24gZNb|K+&ft>!sRrOv5!B1l$X?vTaOYz2MZw~Ai zjW?42ctFFDMG?V2osC16se2zPion1PYSt>ITd$HKXC{w@RibU(KW|u z&=W}%HBW$S-EpjWA2I3Mo(bWVNYW6;bQg)OH+?j#^X~_pX*h0w%tiK1@;sFDD?S$g z;~>3zGj0?E*%BlO zqo>OsX>fGZ2qN8TXk-NI9#(0fQ|0)&+04FbbQ`=oMKM^sQ&^hFzB{^IS=!pD3w}-o z33uAicy`84o5J&$F!H+?y6!1R{$63H>G($3sl3HMB|eVfX^4}@P9Ghf zPkS&L-dJJVB{* zQ$If=(-!VpiPlgI`%{=>uR{LS8BHENJ6+N^q^<)udJu(8Mf-0%fmZDf8|gJ-guy87 zGdR0Z6j8$6v5r8j^%#pOlfMnOVW4%kx8b37-b8TvQUr%CKO;hejWjqIfej!$PAXUK z2h}NvBbcurHLi!+}1H!MUC_zYT6CL0^Sqz1o@$q6Uw6LV`>}kVp zI`8uR#FnPxm^3oir8~Ly9YFRlLySPa^-?L`(_s55ka(v~QQw!EdEV3FnIZKnGsyBt$OaXzWg$Mm$kQK~oT<>Z<(&XwNH%4L}W-vM$9T5o$($(5U0xMdV z6!1h7pTU(z%)i>9ak!?t!8+^$>~be+-aMB)!jIeji4-zeI*2K8RLC;1mI`6<{5xtTTEy z#?$Py$sXa_6q%=#g6Dy@nTp`*`o!r?rkqj7-pSbl8<105_#l$MK^q{rVm?QPs%!De zvHEfjk1~hcdF1^5A1}4?ll<9yv$M|4(ZY-w)>|PF5}nT6UAM_!b*x_+)N5a!ss%AH zbMfpMg{29>cAhqKCGAckK9ahi@#fxp>B1sg^|i^eZy zBm;Bx|0Vi;u{d;i)7oHl&YPN?A5MKuYD}*18rIAOmqSG0G{@;Cn6RO-h==5|2O>Zw zOL4J#LA)_a9r#stk`yBgrRA$s)_QbH%3tulsfUsR zgi!EfhRSqh5e{-e{4`z7FyhIsHTNwaVldU96iE}fq^MmAY$@m|tALA!v|`g;tTDw6 z=cB?9#uhKoGuZ1S54j=e=4s-mw9wz0wd+euEec*ocG#QCz~tKw%rVtu<5(&7VZ!{T z6A&FG+zxz}-y8LHei^~Kxo@Q%E-TijA9n$6;3bsXYFW*$526t~8_#?%Y%uGA@MkRgDYYL6wQ zRVFbP5zW_*RSYOfrj&6W%`_-CazC^QyF;vbC5?smHl%Cdz{rTj)mBNf%K5}F*%hAC zmsT!{EN<-{F#BsC8;AoS!bW+Yz({*bnBEA$rkwkxCqvJClLKG5n>INAR47AuhB8i@ z+M5Gkz*HxPAA6cpVfHCz?AV{uZ&&W;4HL2hK6?smhSu0}FKRV#`wO`->Q*vkYvTE4 z+H%?jJRLz|PiLgc#nmpC*hrcTF~<4{Pd2t^w~5Z4WHqtg5;y(XlQX$&qd9sjCd0O@ z(L?5I#a%&;R{6Q)m+ zMHGMUt9JzgGZGV3GjA59Dq33HZ@n4sg>~N1p6W6w3q1+YqVd9y3$H*XQ%#@=jDIQs zT0_`-k^Ob!-0HlfJZ|s$M$&WzpAH04y4$kWnVNr=p*fqWQ&3k%{GQ>&4e^>3>f-jW z>jbJO^+-7?4;3Vf-81IW)rU9Cp!~xiowVIf!!;ch(7CkG6L?+-^7BGnckTf$Irvv# z?Nq?2sx0s4KuS3t=`K)SOVbhGtO`s%vfX%EKc~rU7p7EaW+iNVmYl)#H~YKMf=gH9 zNwGdYp7gdUoO6e#FWBY4UuwgIiA?aeH-zK1-JEK;P*=fMxLk=Ce5&28_3dx>7;kYq z^KSXN!feyDZi5epdc@N^EAA+j`S=1=%r_;786r{=R=Y%7HECtTcu)9!oEU9vg00?Z6?~~gyY?Z{# z?u#NU3cF?77VFqbAcsOF?%QpgP(TC6-rxp8o`YlRgm3JvcasaBKE%@@Iy{gO|5&;H0J?0rbPi=w|GWDSai`l_<-4+ImWFos!W;YN?+5jk*UPt=U^}de z$Sa4G8msURi?jXcbv+RRGm#+e%6&rsT33B+EM~^zM;lA}s#jC)6gJ>=Nq?YYz`x=$ zk!Fb^7Ymfrn^-$2nRl->mXS+M`b-mi1*H%T&4U4fQgvQ;XM?IGEP=?Clf#Nw6yMGn zry1P|xL=T2bW@QeheA-Li32sU3c09!^QmzN7gKniXM3`wN3RRI5d(mHO)|EuJ-3Qm zi!GR=r2iEl_`MS>c^m2N$83o91;6%&ZL4)H*v~_@n*m)jE3^JB)R7ZOk@{_QDB8rm zYXCoY2@YHH8sKkUN0s0}UG)0#DzeEKQO9;dY4yf+vfGIP;>^UT9g;9q>FR7cSGU2&74Z^)=a9dq}ejz`k-)h*;`x5kkw5N0$gLQU;`7dm3#789U zFQwVrLBvGvxRb~~3o7WOv2d!7EH>)eXN$L6iyodjbc9Qkv0hqe@=Y;!x-?BpqKS@n z+klR`g>q~Jin1uJ)I^GT1N^P`_vKpH^a5*=$O~THy+6Oh%@AF#H{*4M87xg zrS8^1(96E!GZuA%Yv?AONMic(k-n}fmk)`*g1`2L$TxD#a?XFwe_!Mu!Wn+65I8iv zE9@urN{&~@Jk>EYNP2&_n8=|A%HP?6g@Tyzdo(IxY=ij-wUBg7>r~h8cZ%Ttfl{bO=fIB-BzDSTmN( zJU(gBYxKllH(v0J(@a)ROmfesFOKp0M9Dbdsi0lvi(NG$xQ37+$nYFB$WvJ*x2M9R zvIH{{#o885!1;w8xWW}ZD2*cMzEZ}_5;Tsi!~WK6u0Bx*D_ZY(gUp=$2X25(WGH1l zkga5QVAk*P1M}C3QU@J=y(8+~M%^_*egp{s!9cr+={hpH<3!p3^V{+KHFwh<(!cMQ zX%{S^L0T}h33&O|2}3~=8Q7F??DUN>w%}vBCN^?pebCMhi4OA%v`u%|U^oQ6iqx${ zRP0vJrC;AIQKN*y(nkWshutQ%6|{f6Wk6u%FdkiNfOGu{bb z5N_gQ@p?GlS*+!fq2H=^>D|0jciX1s}= z^D#EJmsk==m!20mA#>08X1q8W0tZdI?VEupomNx9Vz-bgzu&KM(mw|ds&y8q zal*G8WT*V0LGS9Njru>M8!DpT2@vb~aBpk3`<}Voj8&#@6n>Jx{f9YW`cQwf^BWx- zTLuAbHQ^WwUfwsVL>9fMHNH!-0U_ah&jj3Y8^eB27lrNlvwLo!O;q2zfeH1JU#;XT z5|mF|Yr;-EU_w6tVDZ2R%8$RkR@Li?yzOh~wA$a5`rT^+*{unq#(ak_z6@pxUUvN} zdN31tb6QHEeIP?+fcd9S&{9%T%myv-YikByCNib8w6u~^i#4$1*u|6psLCC%a>8XE zmW~sKcvBy_V9iP&HG~REISXyg>d1yYb zgS*1fOY)$L0oIg{%_2XS*SCK>TTqSRiuT(XqS>C2D9H!0QuU@C(ytHaRkugoO>|rA zt^1jXmFIL4k?u#JZaOYWhtJ1){??eF*Ws@g2(SUYoqn;`CmHf+7V3Muak_6d&=MrOmZrdUR|bOyZK5^5VmnbTkM~{2F$cz|XxKmfR$h>e4NI?0 z{-!Wox@$#!bi8AU0;sbZT66k-4dsfPk8s`eHZDn8V}Je4*Nq$c+B5$G$;L z!#O*#Q~$wXK*QTqVRziIlihLYxJv-bGupJHl&j`D-O-?X>wc7>PRU(S(34H^7>0Bl zp)@7CoqtjdoygBH;}rI$FiRixLtZCrsec} z$P|5!^fqr1jYq~FlgBK-y`%E5KsJeggF*5q}Gnj$$_Fd*3KC*C-lS#7Zl!gL-(^G}ocKtam_SN^ zLhS`2>pHyMpzt35Sbl&NEpxayRN#9#e2n8fXX{r+@O}6qx|0nao>Y3-Z`NqrmWg%r z2XGEHl)qM(LkJC)%FgK6<^6WlqVpg3Zj0X`cpn`NL=Ip*1n*pmQMTmaic|x_6qt209lPxp{&}yp4txwzlfz-LKef$JEaXUPQ??=wC_V! z23{FU;wqCdP{*PhTNehXAU1BV0ujG7+bsu{mow|se*eQs-LAM7&<-Y_#g5~rN~*m6 z2_GXOWaFzlvF5?_Bin)V>MN0Ws#7wUG-ts0B{+c<9-=tld)^!|nS)i%hEChI%uuFt zYyW^EeZ6sN6eKJ785-R%GbrEugUY0FZ&&mOf*EeoMYC+ah|d?5mMuQfhPhO3(W)ZJ{w+-Am*kroB@BAxqm^vKke$?F{ej_+u>>Oo6 zHvmVb(KZ}pWbrW+X*K@yW-Q=_aQ&6Y{kO#ZH@(UiH5NUi?^;u*9-BLzEKP$v#hC!+ z(kDbpqA`^cOD8hPkE^z-*=G*ZC;+5`X!t}4`;FxHMmUKBTqx8MDOKe%;d z9XO=nYz_(SypT400gN${Pro5LqRLQ)1jUtFY2I@nt=7IuD0h4|yD7|UiBi9no%phW zIIfSg^9Y)%!2zUUA#5}?mc!p8q*I7SZ_z*tJV)_R zk^9eU$gbb!4M!k5UEA3 zd?j0rQ`!r5F$UZAWLt#RxT)HO*rUFt;U7%Zh%$H&qsz>;*f|JvEgGq)Pcj&A9Gg!H z9urh|n6j|>xZ}{pa3%f_1L)$!#v3>NW2q$s?$Rz)0%b4}i}cj*e+!>aUo|uaqKz*G zl&3D0FfZdWzp%uwp8)K7c3ezA?JEil%HRa3iC*}MitqK56UtJVk*(umPP*G|a-R+c zvhs&PIyQc+^2#Oj{H<_ltdt?{>A!^+k&9l+G4e0*ls&s^qEaE&`LllIWFCq>d8<97 zpaLhSzU5XZwLoU?<*A_E%5qelE?jmG^0V`9)=qYsLYw-4ij2fb!-|dKb?!HYhhyBU zVMB#FBau+MAvQbDCp;6A@Goq%EZ#RBtJkjyTgsAi-3qEZnfdHack-XMFi;mMj zNU4_ar94Qx z*z0YxS*Td@ZFDTo@&1AWkvznT+SpXS5rJWg7IJCU=wyl#K(Nw$pDdJ*jB@*{(?;hT zAlHB=Rf9CQ57rs&Y9B9^egBjY>UW~R%>H%kt3Y@o;^)PvZwGQrD$E`+rF+%+0$s>2 z4X)o8!+cA+MPk%=C$@O=CigR>hzOo8z9HFv3#HS@+heO?G3{+5Bu}X9#Y^;hsH{`{n#CG-;p82vp7oSr$6~ZwhSdKm*e@-OqFiYkMJ{^ zM((%N;9><2*JYo|8hAy5om{~<=gG5>x=X0l4dX!~1^GJY$GMGc&r{enE_P@a#ORJ) zG}e|U2BWrfH?0Hp13_zw(!A^tHF@H{*atJZ)VOOFQ9zrXz!Ul90Pjb^d&rt3)O2)Y z16M)vP39=bT<2dTFY@8EGa$LxJ@VqJ2PE=OE47ga zxuRuH-?pD7H3nmm?>yf_*(_Zxj-|hzV+uXp>0V0I&n<^vE_}6u z#E@SQd{w=gt^5zW>WqwH%UNMO=DBU0)wgJUeTPiiRP}S`#0+!79Axf-j4dL+T6;0A zMS-va^45|@yYbo7lbTaF9d{F4F4rTHJ$q^uOnH=Kw}eGJ^I{M~sDV3#F0}O^WK7W^ z60)uLo%_{AT6b*WJf z{^4Lf)bt+=COPLI@VQ#a79v&A6pqxPZJ;R(yY=rnuGh4bDOY8r%~~y)*I;>H{SZfr zD3_usfF+?H9v7(ySG#4iu6JJD1T;{hpRYbI7!%RWv60vtV&!MYtO*9D_!IBM4cv-UVx?gk> zd1;L%?1|6pzd%0U?W$0z6b}1W5MXuSa?r@45A7m3z%GYK8gijRl)JOLtFSAV4!2@F z(#6APs=iEP;LgA^Rt7C&+jbg_0Ap?PDA{0?dT(wNiuH2+K|-IY>II> zy#}gMIyDd+H#1Ei=+hne_fy=DfKca5gH)2R?%a)qZns1B&F;i;UNG-{+h zzpdV~&teWR$0ul4+6hSc(BDZ2MU7(Av-*a5r}vMCeP8PA$8S?J)!Ka+Uexw?tX2YG zXnO1I6}OH%-TLlGr4^IgM5BCjdT&}k zPZU1(sakm~C{E4v^=4x0pJuD^kN|yYQ#o1IsGyj&N{XR)Bdsxx(TW3y!Mvf;RyS&e zHwX*Jwsu8F^B)3;2}V3~s}gX@C7yzZ6}RSQ$fTX>kl`2X z_PPJ)ESM^hx)Ri`RZ($g-5Jzo^rCDVB-;-DR+~EiqN;c=ht>H`4)s`Dj?rK5YbRsd zGQ&tE#e>XdN}>{QHIvwTUYuwAw@n07zWMCKV+UNqGOgazE8JRFQ)Uz5-@L38wZXR)1V)e}KEWEH;)*5L3#Me$C!DeI-al@DU5`7OLGlK5iLYynE z=BV1CDfc>_r;-xVVoG!wYc=&<;y=$R%FEUY@*IeKOQoskiB@~s{CH9JQ7}9&E$#eu zoAVMq5fM}AUr)S3Qvp6-*#EqZd8^P$cEX--X!@$FSUBBN=#@RRyf6ldvZ>rbDO&Rf zJl6AOUEkIlPqQZ=(RXIx_jL8*E{S|-JFtRRrOCXmK(&YqMRm1M*0y_v5E4hjs8VPr zX|iywZ#2a@4S+zy3Cyg3qQYQsu3si9L}kR=Wv2rR> zD$oq(ld23T2oBA>ulSlZaP=vrCBvI?V&Y6yo-oVVf@+S8U z>{Ll35GW6@_#u>U?p2kFV)TN)`o!wTVn|iBUW75tsOnmOds_lw=`*2#Ms0P9!?@1u zQ!<~PHzZojXYY30-$Ij%aR$97H8NK_k-8}Ib`^B?w3gfmAm4r!$A@Ub_(+lAb=%vj z-35X%_O+I5I@aOX={K>zpB+xET&m&KJ6{dpRG7ihJkk@4Ly;GkqtG+<%49ht5gCov zo7S`e9oftuZOtdmwR2a@+YIR$#f)H|=W-rG7}rqF0RSfA9+9RhpG_IMlAWkXF~`Ro%lHOCm7|ty6q#&x z9y7f`2qTb)ULO^Ucb5rLgD7J~=b*`%_*?#V%f;x}12C`Xd%j-96oKsI(I@3N#l+`a zm?Z-(Ujv~K+r=WMbp^lOOX9-C)A{H76PIcqn%ImABXhkwj8~5;(LbX(&JX5&97ZJZ zgpHY+Uq^ohXOsRc&g#4J!i7wQWP8EvVI8rkGDO3`mve9}K#b-RAV&|$u$a|Khgi}@t9~GjKU${+@mt`@zpdRI6tBF!YfDs`Z`PqC6???Q+(KjFMx> zJRiUxl;w%EAd9dRyJYaciIDO z9^GrM2L^t7RPhVkt$HNpi9HpCC;l>!7{~VnwwzK+LRyL3Fq=Ml5feaQvTGa^t;wI| z+k100h+@hmM*-MU)W#k;u?sn5B&%7C`|qi)TPF(s<%Gamdh{t-R;t|z2!K! z`sPL?I*dH-;H&Dqv72obG3dEGqunk9qwUo`*yKg1M@`Q>H-ctxTkr>sYyXtfDAwIJ zrS7cuB57w4ZeC)+!D8YU~|Qg}6*jDmva9*%o*Be-&@J7jmLib!GmkFcfuff3(mtNEmeY z^NKHidq`amtAP5@S;D-Ty@t?k=n_RN7X;4LIAY+Ryzmu$skC9eO{$HU8}y zlT~?rJPMu7?9|eWKQ?#gta}SYhsU1pT#*w0H!wjtNc}VG{m32_;LDLJQE3rXKORy&G;HI@^ORgq88j;&0<@e!UG$1Ls+G+VR#+`#Oac8it{XToxE6k>jfd3oiAP3SvYfbix7G7c^rDgmb2&iw1 zPB}tWFW$(i0w>o3;y?F@%gt-TkswDnm0GPw0pd&F)|B_zH9wum9U-0u!Z~*7wXU4< z*zsg73Rn1}`727j=Pm(=;44cHWkZ`*2OgGoQ*Z}$7t{qOYk=hZUckRy1y zFf`#k3Z%KkaRmCGEo~ab(=FXR&Bqwxf$(Zac5ikPzlUU^4X-%i`9gw3f0fs?jNUFk zQSj4l>)PXDdkg#OO+fq8E8aJ=pu9A@W``D-M1i}#@dog23Q<~v+ajO+$yduUri!I$ z3y!fNx9!ti3Am7<}H!_ki<8?d=c>b!CBQBF<$GZ9s(=retDPNE*K_Kah&A6a!^EL=3Hk(fIb z%CN*=J6+_*;H=ttKo=zWNXlf4irAHH5 zrtQ8ph*oMAK8ZF7F^B?9@%2g?j3P^uY#N`NQpN(*glcjGB;tr+o)8-h$dS>=7_4d; z_kQIQmpg}=k+`);kkM;H!Zc8r2)KrD91!pFS+G@REkYs<4GlIfE{FI1#UMcTwK2J@ z+fdKFRf4oTOE4@ejIOZ310j(ck~s*c(tSbJ_40s$L!GF(?f*Kzl--3_qjDy#Oz7?U z14giGL5`xqkM{WrV|RlN`ccgcMK^2wMY*8VA1-P6(2 z7s>Nf>Cr{G)dov=kJ(~{CePvQCvxk>UFi&+6eIQDu;?_OKu)ah%cGiaWwwu(pXu0L zR-GOIJ1HXi+66wx1?!Mx#&A9fQx@{*T1mb22Rb*mi(Qcx>&Z$+L9D6%O(dd^rmSc1 zO5c1xj$+^lHu{JOk02i|o=i}nT#ugG9Wg6f5BRA$?%gy*;je!J@YWBWIV^uOuHa4Zp{}0TSe3mlg z?ymfD)`6p`+>-Ioj7p)vKWZ5_`MbuqPZkeWT9}Xc_1v8O2)fiVTJ1g8n2SK&YK{qe z!JYlxQJ4lxX#xCi4aEag2E~>=l+^WCyj148OT)0Mhp}{*>rg`V-}&X0GxNWG9=`K* zT>tIG$F1~4VXR{Ig);bl&3AER>z#{8Y9SqP4%l(O*J|hKj9Ou=+XGa#Pl$5YTb%8S34X8yj{R&7u0M#Q!XHIX8$K7VW%uUf&a+7KUZ69;Y` znKo$!!O-5}lO!>awhLE(V$x=E?KPXCyUT|t0$^O5(g+2QjhU4j8aQQ&jq_(Nx}df4 zS@vWfXVmkH8jWLoa_{I=WGPQ+q$;oGLz`IctRY&Mwu@=6Nv2^Cog_JAO6e5Sdm620 z*$HD}{2cy6buSPX4jq2In|sM(D56_5=8pJixb454?8Y_oy}?$0W*|xU`-hHKg4I#h z-@(zpEUyL2M}JVqDkD1j#}<7`bF`l1daYMe={-$4#6#y(qGmE-5miQo7q~uFK_a{C zpXOpE%HFu>keHYKo0$d>q2+qFt^4}l^fy$ek(Nf^==R^N%5N`xr0D`_-8JjIH123N z?7JF&O$X(IM_&R&Vg>>x!TO6g!U3Dijf}dUOaLC84TqolWH9w17*{)LL4tAb=l7;y z=nO|Ve;L9*_!UirSrefXT1p1lbm@()jI`^B#x(J={;C{eHEmDrDTD6|xxa1dIJo9aL zB2G^=aY{4?EP+Mu&IY>bIRaQY8^K_*EqBXeSbyx3`qc{qNWWhDD zV~K4h%vN`2yPx}|(51!fCWn_=3o>op3Pr1*CE0az0|HPXPQXut1o1GcxxO^7wP($@}W}EC3OrzNB z9+*K@pm&EFjNN&?)jG>tV5;nP{^!&?{GH2%{3^HWYBWYRHcLomi0#+&l_ve5RDJQE zfzS*Hwco%|*F(lC>Y{QP1U*>fvd9?JD|S3~3pT&mtr=;7O(}`_tTGk7+mQj3Z+Fcw zjAyDG!5)cU?kM{wKFI6UHxxgrab-fBlWFp`zdN+sLH%hZ*ecG2?5p>t$LNydmJvF% z7^>H+C&BESaG>RzNEFPWEhFS6j`%-H|6YHsH;;%q#(QZMZR|e}x3LL7-Mx(ZcLZwb zHrJKrY!r++zx3u%<`-coVOVVxGFhRlHCDMX|Wd-OvQ z{FBG}=w`4*{?<9#cS%nn--SinoeA<{5itiH^bigh0ivaXtQ zKv(-@?bl}}^xjDqHf~~1?0KADkMpG6eZoY0U9BCweVuT2^51V#(6_(@{!ilF$nB)s zE9~*+L&_Yf;bHpD&Oz**PFGvHa(9;4S466Rv_oc@n(|1PnGi^&!4&i_6k>t|`YWn% z6JYBp^-!C1@b{_McPU&Z3#Va*Q8C;jK!uA@!?@I(~P@3SDgE9swOZ3gi!_A z2AOUfOscC|{4_Lx@-(S-atP;Kjyv6p@?+Vb%^UjxjZ(&s(tTlc*8V6N$cz53GHqHJ za@8Nrl2$qy+OLCxA*9inT^0>0!fJNYtPFVAIx86cPFpR%+kC=!Yypa66y^Fwt1PEi zC-@vx@4pD}fcubSs{Xk64j3xyV^>icB$}w_2r#{kFYJ-t4#+QY! z=xq|oJ%9JjT1!Pi|L&Ygm42BJ;!x9mPbztwwrMhJ($X;Ut>lTmPDM3OOTA31tOV)n zVqOWILo)3Ejg2}G{R&~@YQ}gD_G!SoL{sw+ydhv>ho5`c9kARYR_6hYslLY{F_CVy66+<0P>EfGehC*P8(^V!EOL(r3b zj5Y8mpAba{)$s@XhlOqUShD8AsHma2j|rpS1ZgjbCR=f#;9=IzWD?r@YGu{vk4l|> zmOD7rowsHw4cif2*mom2_&?6a^;*3#Ofs?T8Xq1`7%)L5R}1J7MtPY_FTM+5yw5Qo zkp2-|Vb@8*{3rFSPf=VBl7!wb1My~nBqEQ6RjK1+V_)%{qb09L7c`||uuGTAzO%|y zqSj_+m9#z}Q3;Q??9tg&&^*y%t5v#v_nj;)AXNY1fDl-1BMnZAYM7E!%8yL^EIt8R z2=&mied(F{uA??C!%odB5_bEL7cHsY*7?2d^>3@+gC5$XRgV`qta+;ngsj$~)jvvLqOz7yB^^a_9 z^N2tVl=E|@mLN{#vr&Q}@CpJ)jJx^W1t%IN7p9{ctC2$tQ@V~Tyj8T{jxjCv-+87CzE!-2z|&!} z)W50)vci{N$sj#-y@d$b-%Qc(=x;xYsuL*&<~@HDQ<;hGgn#rc3C5OQsLu0B5X1=~ z85udV87`!DOr{nC2o{u+*q+yv=S*wKw(9v7Wx?p4>(0Z!~GU)TD zTt6~?T`|d^B@}8B8t>cZSvz*)-j1@)h#gv7P=8*%=Y*T5fl!WyV8|;)cdHGW_ppK= z8}-{I6G28b`YY7m)6mZx;u$Oq8Mf{$Y0=GU2ZmA2GnTSB=z3utJpC&mjq^ z_HJZ%)f#Gwosvw+eZ$c{@Y0<)dfbJnbU&{al{M93zthWbEk5JBWXMw#z0x3$sUkns z-z7S&H3@^Pq+~@Q9cSrrkmM6=*M*J|M8vpFdfi2+yI)&&9V?^hn(Mm|j$!b+Y_iC4nltCsbm1vRFH6~Y4thuX$WjquR#=k!om!CX?0L6hca z=Cim~XGN8eq(SSzfTAE&cf+3(#>rr0cB$eiiyocH&S!PyLwvc0h{Z_IpQR#Ok7NJR zYZNL>Ig$gf8C?T`Lq*R)EgX}!k#+XCti52zw)P7MmlNB1w4b@RjYZ!jf~mg_s`{bMy{bS-3+ z0_7B#kc-4zvhdHXfwzUl;l}muzh2kgwocuo7=RKAQ~O1%#afOJP+>uK*6WOfI<@4I zptP7Mh>hMm`BoHA}<2#b5`Iwzn)EetWthCn3qW+>w(j&X4@3#2Zby%|_nI zwp|ZFA#AT%A_<|PM)Ln04aY#pp*WJS^@ywgQ0{3CD*B-U2uV=YqtL~9VV8SZ21Vi| z<X`JP_RA%9A*cuP)cfoygq8~| zqRmwUR3K#l2c`AectmDGn=EE){8m1NKRVPa>wia93xUX>^qcJ`aI_72Gr4UKAhdY- zVHE`Y{Cxi>pkKCg*v0eqP@){s1rJ# zRjm1sM$74--_(W;lslR+OG{R1_kD^Ff5@Y7>Yz zEWCi~(`0^kCM@rMG}6X!q}9}4Z35Ky`Rm;>(mq)*n13+SEoSV1w*dK(qb?KWEszdquUt96}PTxOa8Apaci@ zOdEhpD*z&@gmN;IBhtbQ2UX35CFwO)R%R85aOsRKD6qB$-);Kkot)TVVPWxkUegZ@ z3>Y|#&~Uej24MxvzZ(b{_;!Ix9rM%WO;CHogQv=@ulC$Kjt*C7+;Bu#SD=;Ti#xPe z_vYxUR+=OD)F+&ge>CnZoB8sIVEFMpOxl-p2L85~nC`OZ4oB9~Ce*@Tajwk1mkjni z=9iBrrjkT!JHTu(x4W z#KBKI-B&Pz74NBiUe)_5G1DQ+!X?-}b7{r&UjLSFl=5hGrVm1?>-=%m6{IHS(QaAF z2JIJU@T({$^$9j`Uq39-x8-=izMYVUJ@p6}eKUBwn>(=n>lX%N(D~nPsfx>ODRlRb zcGJ!+E5-zMA0sg;VOQlX=1kEs-?zBDS}sh5~D5YL^ZA#LM8FPS%Rmop0KgHQ*m z<2MD#F1lW=k&rGK98)kzaQf>K3*(_Za)sVIUKeJT7nGFKFXzK~FY^#(UBCyqmX*6z zdH!9YFI|A7M<_{9P7@taM@q?Rt7y=D{au+2u4_#%j9kO=_;?0q>)Y#7on93rZQ60s z(A|_LjBv8mF(p~mAR9dGBYO2v4`;_5u{BqBTIaB|-v(bvKt+(I=2>qkIOm$@03O_Q zP_&UTI9!&O7Y#)vk$uNrTM;&~KfZgVB-6?#VLOj0u}>jEkc|r;^f+@A_qAnjR%IE$ z744ha?|s{cu!GtbMza`AwekMiD(b4;xyA3somdbk)OudMg8=V%*Ux#c-_eXz=RU5| z@w5(NeAUEb)&3|IGBonKYis0pLUqX;#Mjh-qnL0_;^lOR<2B2@$Gf2TkgT-KNcFjK zh$ArBjpPu+&^HYZsj~pldQ_v{A?1LP|Kq=t3cZRKfVsKI#Tk=us zm3VklMN3|5_qIaKVLVa7ztEpxWksJ~pofYMjvXB9lpFb?1&kfPg69V(Q`PY??<2VyMzJz19i!Wp|^D+;Gy|o zUkOC|b-^nrmhYOjaIx7v%mmHXliEodd1ZM8o^D_=;@*X7t%kXM{f03EF81QqFL=Fq z)Ew+K@?BXlC0-Qf3DVFba1?aAm@FvOy_sM;0Xr5*W-?H&=?m+!Aj#z<{$e>j|JN6a zi3v_*3Ho|}@O7i`&T@HcF5L;!xJ$Yg?_hasj3c0A!7V@N_~ z&YwR^M75|^-*M-^npL z0?tDj_Br>@x~EsLlr_a|6St{cI6p8kzHS5TJADY?w!i+0C3un5immhAP|QWnT+U!8 z>ODpCrb$|U|Lwv``;XZ5l2^KDD;4hg4a%O5dOykuCw|X%Nt>GKc#GgYIOH_ERuU^K zS&=D=0M>tK^3j7c{Y^=Rb~1bH0PSOoTjyXU|8di%LDiJ8nL!=IB0=oX9pNB_H2>F3% zeW;fK5m3a0_`b}6z>f~ENcF)M|(hgUfAYP0oj}Ow?bjPfjfb9-SH*l98>^>~0lV1iz|bmH8t>?+dJM0@)~=lFT@i!2jo zhNbbq0q?o#Yqw>R*MRl4JQCGi7%cYuwRbwbOXW!hxeHImA? z3=wXF(;q?VR~ESa{OzJx;S`YT1s(D~Zh9zHzWH`Z&E=om)Qh^}X}2ZWrlR))8T|Yf z0f|9VdR|)Pv2*WiR*&{Ol5cMCq!$_=>jaYRLhjwSNBB7#9klgTg5Z)Mrzx{st3n=& zTwm=QuSl0TJ%P=bmMFybqow-BB{IrQM|0h!RzE_iLrztR^lGS$cSY7Gk;2GPLMd|H z{pc!u;Zt>y)^i+EeW6H9bfcfy`e;`dQOK>k{bo>ol={CaS==#ijFGg+n&gTZxcFM$!oD$J(yYx=q;@X0zeo-qYMRq*psq4=Sj zLSb^~q9f(Yv%2sm@99dE*j7?E^7yxfz9_AZWEWqxo=T`D$~YKqI%3KGlZeL!izUw^ zwCNGI0`9s3PPL5ML!mK2WUgH)_g7?hP74lD_!#+ZiqvLjfC1 zk$Zj3Z3I?oIFpoIp*0ItH_`aqp!~Fd0(P$SOiWRbLOY>GC=}(V$84g7?wimSrJ_$e z5}wybLA@qDChV!c=+_oIJ+GZEEU5cOD?g^`^5UPWkf*1oIW+1???dCdu1If0k+x0e1cyNnzjqyMT(xUX%QyHv>&;9NL)&o;}vyl;xA*==>Q1;ZPA6dW3$N&{vTyF zG4>U2f{hl*J$aLJarC1)dm; zlS87FTBAe#$C?UC7vt)u0B9->c7H z-MlQG`9W{pt*UE2AyYG~(CAA5(zNPz!wI2=f_4ThgQQd!&g3sc?WUW?)H(oz3xbHVnuKO$hac%L@bV!R&UxXZ`qYKEVKSLp$VQcu2)nkGi<`# zaM{HHMIyD#?Q7wx;5d(Gqx86r41X_aAMXI2YAw^BZ%% zImTm-^u{ctx@aiHd?Jg!tA5j?fQ=I%6QOLMx9BI4`U0-!qpU_rpWB*H*3NjK@>n}TGPAy-Qsp2oxxP(rv95(rN3LfME z^Lil(7Tzm0TE9wx6)?nV*!5LNBgG9ES@KWwD^q(S3-rm2$6bQtaYDW$>z}?pLQQpg z#o1_^SOXGY#Ye zui8l{0fN9ubEFkPBBee*)EL_v-1C=GKbMog|qvoxgQtTL{F2D39W7O~2lnpv$0p8R#i zoYX;@a-kt1pYZ8%kwYRwo)@Yj+yr{3Ihec{$CKvz3{DTBH>FMa;(`w+CuzxTuVV9>=_(e}F zY}9wYwZ*6K7H80|pnAemW2QW!74FBMo6Xu(qY5b0oTG2*+tyictzG8P*M%#jofDf; zoQJgoMy)j0#Z=)^nOmU*A$ zhf?o)Wg!y14||Ta79-ot%g%_O*|;^k3?_be7S|-@IZ%9fS~+sb+GiO4?U&~(^^;G# z>P&{qPl+OVu6ghha_^}%+RdZ#N^WbXnHDiHN7u5$Mh?K(zmGyK{0RuG_!)BKqOO{V z?!RtrR=SL&Y`F~7H^VPeJ3)#x{1j8}BU=iPdn})9tN6N}sUEXwI1#99y~_9w&}7Qu z7guKN{z*>)XUrRw(Pcky}Ua7wO|pEo{LX5I=rnMfbr5C$}!na1yiAQ@b_J{fg~BW?_%$^8@4rvMaEnM^QDT3v?Wy^@wz6MIVvJ80iD{ol#x`#-ov1P%X8iH5nIMF@y zH%vdP(Uru?BbsK>uL@1q9tkF2qmsQ$cwN6jq{A4!vQ6f*TI*U>xKZw+k}fWa%hrFN zcDj;AMy$%ENrAC#P?vlgiTrtzwxvywGST$q#_EgF3~q1`zgfoSlt~K~Aa?|4dVREA zCscGpO>Jl>H`4~x7t5=2Id{zqsa|EY2J9`2@aTjOvMoJUbQ^qoK&Ma{x4cdzktM{& zYQLq!^f!B5!i65x5x657?irpr#y;CA79`usQR;T0-$X4jSN3vYaK9clo9;gURYk)8 zL(Ke zKUluI__fv@OM`6mD+kptJ}#r>Z2wQs_N*+N$v*_hJ!8B-{qC$EC5CzL;t@yac09|w zNf14bCCGH&VB$?lq^vQqKDb-Nco?Q`d^2D=kvSmmKe20(HGX-77zT7uKeB#?D!rn2uvM|^djoqvH$O}YSpT5el=&!lG2-P_R@8nlvLEIPqN{o1GNedyLlbzy{5?ssRF4Y ziA|Vyvd*VUm8$H;4}_+ku5HCGodt`My6n(&d9yvOK9V~>4()FR)YtB9rHBvhTIucY zVpdwZrYPb&2XH!^olK~_Mp6aXpCH)fd42B&Y_a{iL%Mjs1ty-vkmazwzyNTw6QvIGxO1KEB<7ST){93lD`Pd?t|@Os=HeABp{{S(&dB}HP2U^ZUs2b?y$ zv^~2G(F8Sb3kYMGL(8(b2#zJH`u7H;)fVadrMJOUin_x2+%DB2`J}@{>KSA1XOq^O zK<#QfGpx8KWwq4eydFap{%!jPQAJC&?8WLhC~0V}o6`xLTHzLy*U#<;W=A ztUPYKEnZvJZE?;LR#Kt=Z0wc&t{@)$ds5$J zl3TB*{3UoBo=s;>81p`fLa6ooFVm^|Nl0!Ek`Y@g6%5qnFMp~mp8K2`N|rDgA6l%} zbcG^0o_2p_Mz65mLuh(n$@vg^b^yYo>)0{{ujSfSAcWANGp)g10gzv&3EL>8}%LaPSs(Y{6vgcVv2{$CI;kQ2S}g zfG)pv%w%(ia-r0k(&2C}=Q@hk9MG9U)lg$vb58ZaAyjwEwT>&=!Ithc-YT)j^zH3t zwL#!^fw9o1YiNQw1aHwCjX{_tU`kgi!1T_DV^gZBh+2?LV!p%=x6b%?ooa=D=nUjYQJufJ)Hyw=hj#UfLi-CF-C?>MsTKgB-4d=s zRerpVtOCrtbWG6xmTIa{LD3Yl2g5^XDz~maI{?Dz+S}KHCI6JK>Hd0)e9uTke4Q5p zOvSbUeEQIZWD$0W$ag6=Uy&I5VFL)i;XK_}7l-ztx1lV${CYZ_YC`FDl)F_#k_7G( z;xt{3I8Tb1BBrV9sojsoZKsRIn}eQD0UIK}R<}{ckHli7%?;0OVyT2V00mLu`1M_v z#rDznA+@U3a~1ZMn5NLD_)SRSic;5JM6denVJW!ahLWBURU2OZ1(pZ3r0b7D#m#q`( zev<6I(nv5c9JY=y-!SYgUZV1OZPX3&htJ^*1fIAe*h_Ec{L+(@Yg{n8PlVDJEuF+T^;ZEg1?Y_a&gz@ag%*P?7@^t zCTv{iipLjBW@W_cN!#C!+jh{t>}k{=CgAMM2(xkE3q;n@6KwU$XxXcE1Tu8i@n_xK zk?drNav}ZM*~0FAy}nes_Hi!;vkNH?>q9L3f&YV9{KK$jfa zMDr-PQV?^}-Ep4-yHdX_?(|{%!RzflIIe=6a87UfIn*`ti1a!XfeXj?ROc0+JunZO ztb)A~mdNSIW2ZU#+fR|%5@YA|Ym@nb)~4s)&C_a*3w=QR)^s=tQ6n*ju7^Q^qtiut z{z!o!%~On|j&n#gLs5I^%`S_uQ>!PK-TTHI?t1o*cH9@^;b}s;Q@P;j?PjZ%fB%!( zoQDM}uFEN*s>aW$Jr~TRNJ#41RNA}ggb2FK0%V=t7IDI<3#paQj#A+LFj#l{u)@1n zY$3am#LnGsVYPXO30`e=m0m#EK{YPVf+NF1_D?JV{Gl9B@k%TFK|&Ni<-;J;*70wN zqY{au15^9z7j)oT{NI9-n1YIH!Yh&%7}mQbrRLR@Pw;oZLb6n)&iY4G)u_~tq@$>R zF&~R8;ryF`nlClb$o7VIH9;hMpiHQOkI!{Do;3X6Cd%Y-VJ_ky*janPZ#b$#aVGdl z?1OSg<54w(K4}<7^>w0OKOPq8!(9(%EB6$>J*JnLGm-O_@Riv!!Gnv?NIpN-_uWFE zU#j%SW=jci{AbC6BCI|Vq$Ffu$Au96W=#r6*E9fc0@r{Vo{iq}NN)Sblq+aGHeyD9 zd-613p9F8*)9OL#z>U%H0mc4xgD(Y&g7DTe@KN6HmRNCaT6aKN;{4|h;_=Onv)3O} z=1q0gkl~J$pfm!Me@jDKzsm*NGcs`Cvf=gaKkk6*2KY^$nju$UCpuyyr_CaOV9A^vgiPl`rWjnp0nz+>O6B0&T;@<8-5r zM^>aMG(?=Y^4?=#^^#__w4LgO=Z(uDb??9gheH?8*#>Gj z1-BU4d8dB|Q53*=N4@7C|_mE4wgA5FXZ%J3C)V|nm+7wgw?Pby0J@jF}JE*QCAV2b%dGn_klT8bu#l@%-*~g|NIfs&0mljvTisRVw({aw}YC5&o)%hhilCxK)Zp4IiC<`EfL z9WpZ`+}kU|0o zMJ>3o1E)K8*__izgsmi1a^vAa7o#fvT@f`>ftf82`VExRJN1+kUYJy`jA43pH3 zZ!lXQTLUbGQ!FZ<5A*0KcVfkq*c~+HM)w>XihT`^p?zzyPUTH1WV4*G(>zad_*LKl zO}B0V${Ug0V)q15alo_!)qk8L)P3kDIe3b*L!gBeOCx_9r8j^Lba1YIKhRBTcv4GR zm`#otxqnQZ$yIzi{7Vvm&V6JX-pHQp^=KMi?Hw3l@fKl{o-4NOIU=Yk(&1zx^Z#W~ zh^8~jE$UofT{if*W2{Qu`ZAirkFh_*E}q+RvhXEqMbO1D- zRc1>;>~1oftK`RfzHMnye)^D$qdTv@y(arwUQaBHzoN=`r5@DO2#zzHv93w+Kc7~A z>-1pEm1Dm8Mycw1Aukh??TkNe?-hsVQK{=C>7bpB_{_i^v|D;lDcL>-52{W$W9R%C z()fO8;@F#m7)qjjt4i}MV>;26mriUr9`6|zSNWrz!Yt;h!*p-KiybNE=S+69hC!*1UK*@$I|i209z{EQMBf@aP%^lqITDuo&GZb9n6)>O|v3?@>+ZteZ1 zQ@UN=#;JWC+L8dH0&eep0(_JDO9e-hP6g{A27%j#Ln);^-rg@>g&zy%TP9xX!^F1R zTdl$aaAXk4)}!rcRj+jwxq{_!GnJRdYNWs=GJY`SKVGhAiGpV{Cl)Zzyx zdkY6R=Z^!tm)_lP;2sG#p00_0V_?UnGojP{%s^QLf`awXQyI2>M z+YcUE;xr|$WaRA(K2FjVrV-Wu^O`S>09)a>^ASrF-TqD<858Q6{xbIDUC0XdS&vj+5giUZSTB;%^nV8c(}pejq^BfIjmm8 z&4n6gG64U)@LOm7v;GzX1S7mF|GvATaJ66>z5e*5)iK)>8_gX~_h7J&K`E_37;`xc zD}Lq436Wx{O#Ge1^S;2#SwKeHKx~N{iDJhk9)MR64!^#o`&@!4p#YNTuB)a>V)ZFoP%| z@JTtIK6%vdd(A#*ac^PjeQ1~0v4gt4%zTs^#+w2*l!2V%x=0APwu81aDB3e!CqTIw zBEvF~ry0@(LUByV-Kl?FgK=C`>uLl(8nj+?>Be8s&aeN4uq?`?I{^Aj z8@U7&8>T8T>nq{ z^h=n3Y1~s|Pwt`HlaYkYHa%$KVlo6IvM^-#d@!-|fI|^*r7b?!QcFF+4;SB7Vm$Ip zjM#%n-WEX*1b38I=F!Kur&~q1xt6V*GF+fd%}I9oBf2A+PbR&UqI+(}%Ry7hC@}H+ z_D$MvWFPp+7r3((8()Lzji9Xau~xgJ(e&NrSJzV-XpIdg%9@Y-yJ*m&UJZc_+{``brbi>IJ#tq(ULWtxW)w<1J}&4cUDRLF|R&gz=KGaU#P!cO#~G_*1Q zY>ch@a-T4+{W{Y#VmP(GdGj>i{YdNdP8bM?DP5z8Ki9qyTJmpVB~0Adyw67O7(T zToOC@t*dXdqK{Rw-(w|%B^ETgsHwqIe#^By{eM#rp3|eg;$iJc5h<@;qs{ zcd69EarhM-9jJ7WcmA3>wPG!w^VWUq4hXo0`yHB><2GC*e~w3{77cp*vsnIPalp$l zq%Df0*HB}+m(jwtt*;0C_64sZn_<+b=}cQgn}=)-OukH+dG&4)Ri-G;#O`kNdI1qP)|gAZt8sg$Qut z=60NozN@i|!BI4k&YhI=B(K7k{TUh;cw3wbtosw{q>(^BIwgWh*r*K;B!gKuho=@# zbTLP^wq~N&yhQ!{=38B<@rq3% zO!0+YM|%mdIxYTUvtMA>5JcUxi7l2wvNKyCE(M~|9QmThkR>fIYHI%T$xs!d>7p7H z_3*vvLh}S@+~^zs)}4m>yvmOL{tGSxhlcNmsB!7~W2Z(87XRa+sp3pp+=z^gY$Lw9 z6<2`xZ7>b{*NADKp)(}$1{(7Qb~<;)J-1waqb$3knhghf-C(&Ym6o3J4(<-AP{*TM zQaPDEcgcN6b8NF_jwWMTy*r?L{Yu>kx!LXZQ@B#HtPG7ljuy7rkIR+{WQ9ZtQXf0b zH9M^}D}e@^XYZd0$o}@DWRLR^Nk|Q+5P{9*R2KfH%aAszh75c~#h!b_3jKtogfe4B z-vLty>c{hxr=>jq@VSZ??P~Jnddt^w6iKm0iPvVw*t%GoUyuU$;yN~6^!+1q&DnKo z|DGzNcCKQC^%oR9bg~gO27*0i=VDTEAW#Dbni&U;I^R$wU5p-$KIdwr_5n#@u~pUN&y|m1G7~frBOd#G zRACZ2Salg9G^8jQa@YyBxgTAgOXEXZm;3RGh$(aFx7NSsb$up2*}Wz;u-ja+g#BU- zg6kbeB9F}fR5nbZmBpbHQXhMjZuxh|`4Ns`Gwh6Co4%L~f9AHfhI(s?fm#dO6@t@)L}sk}6w10;!?B7S7OU`h466w8?)Mnskk88wEd z&gKiS(x1d+JX(Lbi6Mn62!l-_3|ZOrpT30|gg#2Q5^&JHSB zdPsSjz8vRp_XgOo5T%a~hC6%=w@(WlUbwJSm6l)@-rTC=Z~`iFUO`fB)JY9}ePWQ3 zMAu<4Nde@ZL0=G>2$RlW=>}@UsL{nNAVO|*FUFTJ22w%ixH}A*@&t~D5zs=ABWIB! zB-QVD3gXDXAd!EK+Iyd)q!!a9Oi2-ei?7svRXbW?vyDJ8LEQb-QLzAORqHj{Rm&o% z!6s#Ob_&F$5cZ-v3=rmvxVmbdtlwzs@^SGesmgK#jqik_KxDY zzk9ksKJT3Yuq`kzxw^PsGBOA3kRA&F34ieAjVMVm$JxK6xNS?Z^8P&*0M)#u4SQ~> zkJspi>8{!_R`frn>}t>~2{vjrpQipx*bWpIGK*1tYlTt@Z z2-9%#!gQQ4?L$96NTaP?>5b$rcInWi4FvMwUQ+L=rtZtWQzogwBz9M+JD^%Ul&>J} z)>V@-BTISm#9i3hGHX(9NE%*u*FWN0bk$EOe}X!Bt_5reT+t}e5^nL^mBXl9E8SZGE~{wCn>eE zofjBFQ57OaP5Us#aLd`UMhrcSeSe5A*uiZ;%v970|T*JoCe#zD|LuFB9 zs!RfvDO4{++)u5x)d6Ie1FC<8l+aROlLwJ)p+VKCc*jojP*7X$P)BVsg`pzUQvQrT z6iFfgEErqy>{yX&$+3PAHtA}^A;YI%yegV-)Qns2TT3V_A*{#kd;yQGd8eQveoBO~ z5AusCyVaFE%$h2p)MNc&|Dk}-xW+}N1N2jzOqp4U@g{rAJHh9O?ik0l|5*1O&HPkV zN3A2xwZA*&_|B(^v@Y%}q=Vt?9*mAEC@GyPFeI0bxWs-QDT#pql<b5fH2fU zOT=#-5N7`lFNv_DM z0P;esJ$$?aq#>;f;lFNxlq-9aFd>oA5zjnhdO!S{x zcDIz2luF9T32pdQcJU@F?

0=Ojy_60sTt3EQj-PkkN&5(F=n*iHW=_ij9ag z+O3Dtu_T)SUQ6_u7HRg>+)hx zH@Z&au2UC=Ox5@|I~$*PECP=ZmE2dQe{khWlUa7!PjNU%^$#O}(}&yJaYyB9EprTn zv|Ihr>3TyI`iNe4aYCVxgB^MMNiq(zZXekj{Ip49#wRl!D1L?DFl>9pSFJ2rW9Kyv zlot0^jqETwFz*rRA-?zdqg8Lb(Yi`J<&aaxXA|Wya`QsmRdp5bneCC@KFO@pe}U=DZ^yD&^N5kjI$)$Qg#xU|73I%fIDh+-nEU;YigBR zELr(m9!xu1ehXO&8Fm#J7L;c`Z&1nFCnsMBe=y`4FcWATkS$1H07mBg@q6o+5$v?E zI^!kb-t%mgw*qfYl=BhqMNPq+wn)EE@~ZD%MD?0tzRJXCUuir(eISH|VL3RkuQGu8 zpwk}^;~0z0P3=aMe?;kpi~(~r@(*RLj=Ljnfe2})*B@lrya;l2Dqeo-e}$G`k982K zDP(opWLdS2i@#KsA%f;if6jb;L}kZ)f4%4c0QL2^wS7sg>=z?d`#Ws?jxqeN-LQ!$ z-k0%*o_Z3_6-NiuTchLi{G+_MBj$Gx2~N?FuX0$tcFtC-Y}{a$H-&RKA8^!#-wC<( zEsiq>=Pd)AQ6_Bs4w!cr9K$1#KTu7Z-}x-kG~Pr>@DW{K5rPC8>B1U%eAo6BHXqx?yc!;g|hu< z^U(aQhZH}B#2kQkxmzs_nbdrf8kW=q`8g8>bh7OI*bK#4J0G+jE~2djDa!A!lq z66**SA7c0Uj_>ovGj(ZKxdAcrf;c^`J@7g)y3ygtQ$1?*Ijc??+D@|Z-wGniUyneO zspz;0i#6pnqjn%PCiNeP!QtN}#$2{?ATlnFSrDuM7F&)7L<=kxyH6}MRY1rv`N~7j z-59+5T#t=vIboa`-hwQ-yb}axUJkYD;Y84Nzb@-x?cAR9yd$qJt8QXQ+sL^}fV3~c z7{(e@+QH3`Ta8%bLea3b+ELYoprZPy(X|pPUi#ULj|r`!(pgZPC7vo;tZzjtGsUKA zBYK*NO$uJZN>^;AyyPK9^r*9HmCk^WKE8d~8DBe^5<7ap_^ws$mRjsUss)mFfdMC& zANOJY96IgxinsPp)~QRv4QK173w=|pey~HwMJW6^1jPSt1OznMKhTKb$YZb7Vo9@V zVSF52H5Z2n4MZN-l-QWSF@#!5EAWOQ4S489gnyyY0p*nJ2kg^EL$sC)iP`Nb!->n8 zR*uV+Fy1Crb6-fZz?LvPKNn?)vryB@#gxuWEtu4|!TixsPuCndcJ1Ajwz$mub&`1c_zBS+=ZkX(L8mI8@9OUI5KbHmfM^02Q)n>n}M!U%%UFmEpA= zijsIjGAYQwr(lLq5t5U;ic)&oe6`W0vy6cHTHH4xJ0>>jONy{)4UP~9kG3vml z0Y^D*g;vunvPC+5s6sz6uIf@mO4DYmUsAXdG6lGovRT*8Rj+1FeiCLFuc}|xm5(eP z$X1Nlea{1jTGiOQ=W)``ZH%%Ft2Zd;v6u_xUS9n0+AJ~f)@HSGGi0bU8c#K3#|2#m zve!5CW<5`J8pZd^i_t-YjKiqx2_UATd9pmdEtyFu0g9u1-@N{ySmy0^5nssjn&_&> znQc10Cq&8~$Zz+y(U-K5sBU}25`jodZ8I-q2}F4vO4mrxTthB0n2=ox$X6K6@Q=Ah zQu6)ac;pDLwb)c7A5t)Xeqwm`?Z};nz1(ZakFp9F1Ve|wF0efWa46CXTgjW;FATSy z2D?risI?)T@q9|{)7inyaaiMR+wq5btDt2+waN(ivCtdq}TC$C6wi4OI^<%T)?aew-M@|-=qOG7(f7R>LcxXIoZMD{d|SsO|8H1oZvq8e$AUB{O|k!u zwf74GX?mbp@oYe&-a8;{oF)Up#`rBD5M0?oA!(qTn3lE$+DH#l7vw7Kyxa61D4Xo0 zy@YAf-M7--hyKvaVz=6**qI~zE!?dZ!o{2Aga-S{Xu{Z`~E%9`W-Qym&8UN zNILF>dPu+#2ED57WYtvOk8HfTT)yXJjag9u46`Xp9#*f`1%meH+L@yH&LdI$!* z_w_&Z|44ALIrOgU?i?UjE!DC_hfq0fhP2QriL3TAqeGXCiIOslxvN}Zb_Yow@8_Z5 z9kTPf-auK3p^_A(9iMxF3#V)QgDCP<=2$&{e&OdF-hDiR8Q!n5IX`dOjixvfLi4pJ z8PU43|3#{&B5?%%7g9YW6Al_79!LFbNI9~N$wL03zrKyZ>uEfwvxg&!Egpq}`Gl68 zA~BPq)$z!2>J9iq*ME|rYK1TJrAr&b#IfE%1LQ|XLI1g*MraP4^6iU|->d%tiwcg( zU>Dg(lW^Z!36})Q5U*qe8PN6fUymbJT%nT0rq9Eor0VIgFGN+k;(0pogEezS=k-Ib zcO8#ocr$HVcMBlXL&C2Z2ORzb!Y2-v?yNImAL$$$hZU++f$@^Ri{jh&KgAC_Ap7;r=ea~C&bix_aGpE5C+;KhPscB z{Gk0Ndx{2QR$R!bCnod7R!U7)Z!v?O!Fw4~sej(@r;duW<0|A~5lkwAZ=;fG$a8D1 zxHv(O(%VE-ukvel^qV0?sXO>&TRXd&xs>Nl!!9NF!r;0ITOiwb0SHp;5@bnK1=Ehw`#N1_K~wY_a4b5k6=9N^BiZ59V}sd zDt5d6l!fDqcTVyD)B?QtfHw7VI5@XnN_=o|Vu#H3|DR}cnDC|h3*4zOmb~Kky(mjW8{cqI}@Wp-60_RZoiB0T4K|pe?&zkzlZ_{^iCX_)4nG(hX z)!3nW@>WB-EQeInFjjViA)~#_MbS zJGypv0^AkhVnlGuh{hmg6!iUxz1oDTbs?*t?0*&C`+%FgK4b)USjYCsv&4?zGW&3bq4vKbE0Twv88g_~}W5 zi61RJ;)!Hgk2DIqvv}z<4E3D;9MsNpVIf||>qc#=2VZr&pN-V>+EU7MEA_)`L-moa8k4^$Z47DS{Gy!mn|NUC{P1np0!NZ~ z$e)^bG0FIxaK3sS07tVz>i#Q4-0o~@WoE)D{(F09)q}-wu#3BH0#%_jU~_u3KbZ|E zga0M5E7Q{-hNLTDGI0D;#yo-h&sDIvtwBeI2lml-2wW3B$2m2Z*6tRp7v44_ULumujA5=y8Z2iV2yc?L|c^;ql{wnVa zH_0#EVfwp^o__{E8$7md>=vFr1M(F4yrgfVh}JU`y5JJP!6Be%nP$KZ#w_KuN~%;& z3|(jDMCA&?#0pZx=GhTTNnySpUF6rSc=zkxO%gYNCfq zgu^Vuhpv|}4b}Bh0$YHP5JdsEgVwOZjYF{}#~tE2x%$J(Xl3~;s1%~BPdioJ)6GgH zvrOh?C8bC9he31Ltq`0{mzY$I>%g3QFN_H8BrV-4R>`6tww2vtE_akeU*Mv+iO6aSPsTEdOv*(73G{7u8b)O z!MMMtC2mZ>`q+FWu^gf2U!xFKLQzyGQB+9;0&aWLvxRcJr@?(E^3Bc8n^%SlTNa`5 zJ?FNv3V_kBP&6Hbt7IoB68W^i3y;rb7p)ucMvBCID>#fcD!8yUY59ejS!m-8ydfFF zwwCufK|I2=%Ot4H<+wy5k`&I5#wTgeyjZE3$4Bl8`SSxOtltliV0(5t%0StIpUk@N zq%TJ}u{3}Q791t!k(fMmzgBI#daW!ciEU+x7oaS9`S-_XAMt1yBA&b@6AMjYMQHYt z(Wo@{!h$wdw2Z2n(xy5Crj(g!WFX_9@cq+n9GU{-g>j?HHX z@JD{b7e|zDsG)2_YfTQU9#@?U2ce{JVWVnJkQ4?Y<|ay#e1iv;zlEKu`M<{g&T~~g zRc{E_h_wpLua10YL#GCWnz?{H5FU$TCb0Ici<&eH!2qbVB@!*}A~()s2klMR?Y=OF zhvx+w(36v8)^b;29LCzfKqex$u1|7yLBsPTCZ1^JM6As$3AJQ$XW5js<}I4PVIFZs zUA<9pO;lzuOmXLge$=rtc4gUTQOMItiBkyktm^%Pp;Vvf1&t^Qx6Z?yV)8}xRk>sJ zxtywY(t^J;;-ndXO5ExxHYBVSE+cMO3{K0+@OTR$Q2fE=qkAHxV4JA6D0?QZqa(Ty z=#=4VsLqLcU#&hnR`nF+(Zfj+6N*UC{v%GHP>~|>l2bvGalhPS$zG5t&4PYngU}Vmg%GSr5x*KKXbz-6?!;8?NZsKcT?qTSbWskIp{P7NySvy;} zq&X*me@z~bvw`^Qeq$D`tb?f2eE;sn8(JKD^-ySoubp;s|IGt+ zscLzSb6f~NTo?G9_N}FqK`Sen*#%{##?w_v8j| zMZZ<7@)8D>@#Z)yPF0Y+kB^R8ZH_&usz+{c0%JYPf}@V+C*&2FYcKIv9~BKP?!71Q zIan1`n7F&`Hi>owNGc5jX;2I>{#Ohx&?RIMg1;7^!>N`4d^Ciw@-ViVe{SeiP{|cc zmL1vo(a&m9T}hA+qn|s`aon`(`X)%DI=6XaRjuaHYzm{>wq@MPRAVyx+lB|?!*ZF8 z4BK+O_}Vs`nJRoah`t~xi>WHBW&RC6Yk6}h*LxiDA-x}o;Ud`r5&Hg-qn4yCE&f#c zC8r`><(<8Os>H~XjnH^5Vpq>ufzn$u_F;bi`!WPz%dJ03YzSe-=onN&yV*%dtrqLGqx94DD_O!q+3 z8hQO>8O)#bdfwI08F)06w*g&dZ*+L?Kxg)3PP>Q0sx(HM8j+gIW{qUPfE>C8rm9-$ z&}NW_cc|ClH4c$ScuZCGWRGEhS@Ez*q%?ot;$i(q+#%5SHE`Nuu=$auy{VIsu&$k} zeOYehdKd@6Y~$MIW>%=uaa*>uIMftnjObU(Ge@$fOTy-P9VwgTow3Xa1Xeu%nz^AT zl6#|qh3+Q*!g7~_oer~192Kd;A5yH|EVgcuyp<*0{wtXFK*hhQFW!T^$jkmB#+uMZ zo+DVN2+}oPWZ&%J>41lu@c)?&tl_MCPOf^xoO0PGj%~V@SvZSK5lEn~gcuZ>&D1*W z_Z5wtRCJf)DS{h|&UEzUEGDvC2~{iufjp;tqPlXLaRNPJ6gQE0qZvC~ufbxX{k3Ry zT9o_mSJwq%k*LeY1F09JD{r>9FF(sFuuQZyU?U~2cosj#q&w)@30eq{OdJb!{Ddr7 z7!h1lG&E{uRiY(13U!*kccX5;S!&a|*-b!guSxdpicJd3QJ9Kxf_Je8{NRzZsNVl1 zvIEu@+3{kWZS~sP^s3!~_Qb9_*y6!I$82F0OuhdBpeFyz0t!Unc8A{tWg@@?Co#bU zF;fo{^zK91g3;m|OoqAg^dCr??KgvsFw>JJCoy;~M&gx!{AfXxq=oC#6a($kSW7$` z4oU`{_)=<3#mNt^;Ee?Ic9I`(;~>qxt}-h|WO1k~I{9yR6PqFDuU&ap2X$H56Qcw~ zwIPZODa2z!tkaAUfb$*X=yt?Jwx?Y9G4=SmWIV((_?Byg{zt=vp`;N zr2S&!$F~-BRm=;TOosNa{LM`)WnY+{g;;N1syvq+Tk-Usiq0oT&!jsR*f=7%PuQI3 z&%WP^A$>xS? zA|;7?4O!hT{zU~A6crq_q`{xI2Lqb8#g z%cwf3o>r!uEMFxR1(ds60fwN>2{tMUG7nX2Gm~(3^2iuEpVzOGUdbhv|umFzqZOSpD=gKjHdL#K`qL93;dLNX6=DBS+TX z-t%C;o9FZ%A7RHFklYM#3HEIUtPErh`vX43*wKu}30Q6cKE%Ljz*s&4yYxUa;3$#C zgZ-&bpoRN0u_mVt{o!rfd9!Zvh5JMT#|9vjD!0VAf9#<<(KmaU1}%#E8fKMsvCD>p64 zov~66Xi-N0QFWqDszruJ-a!t~Rio2T^G1T4c=}teG<)$HXid6cbc<{XH=3}n;2Vty z(WRc6&iz{vIzA#dOI?#kT~)Ji>ad>DjP3SY<5B9=ziV4Xej0JjfXD=7Mdbzu$0O9LMEMjb+6OXQ$l!_RWm&A(MmG zNz23_{t`y=UeEvYa@_N1Bz1$oqhiD++ZFI0u;B!R6nN}|Su+tC^fYJT%bC`H{-(}} z?0e_pb)LcMhH$0rV8mNd_KaW3>Ne&{A3fcR7)*N6TvidK9URl>RDM<_geQd>I@=4v zWyMp$Y49*cJR{f%Tq^IaHg&?+iJDBA1`>Q$vSWqV&d>=%ddy<>9^ol5C2`{*EWQf0 zpfJH;pdhj-kqwytEsm0Q7s;E3*tR)fH=5?7*0rK)kYAyHK}$)QYdp)zRGuz(vWgjP zIT030H~YsBcgvKJH^>+p~Hu$WZI$&Rv?CFf5~;lSL&$)w8TMPe;h zq&@@;(WhtJdv{3`DMuP%g|bN$NG~Tk$B>$mqpQ#kWZ)#3A2;Ye99C+nd7+m2zei7$ zyI|Ps4cq8g|MdzSag1jbWn56i`N8{M5v_GG4}bj}m`>82Y+&%pdG7Csvu&WveUKXE zH*5$r(YPJFe$SqwgLUa^qhYh~#hWF%z8#BRpOND&s)t&5?SBT1^rhCC;x#Dt#BC#w zRU!nF#^}G2Q^4MWZYrVOdokuoqtHfWPU?#XaR#Z8m%9TX-&vphS}BV!)2C86Xo*wS zBU`CSihImBf)%n}HTqj*jL9n!BAWx0lSv7RXGwjv3d^UxS%4E|%)N@Nw8YFx+1X5r zdN&sp&a1|6rb3Y!dZ(68sS{~QC6b`;;1Z_)#K*N#5V?i_-Geltg~n7rO4T*IvhwCq z`uq>*4Pc{Zf`(l6jRXN-1+v?(R^7quh&Wt?>2{|X$a|9#{Yi~zDL1qb@rwsdoEzi^ zTNVAs(31z@=0G=_GXgF8AQ4inTw=qk4X<)P7uJU$$!Zkvh1n#>+`^w4=$pokN!rwo4n$#YitirJ9q__ZhuMo zt6;S1bo{r8BW?cVS)uN-o5k_jZ)rQZ+mYKNM~I4oerV$cSHs}JAsCV6pk>)VOR%#! zP`7tD&VzCJfeYpR48}g-8LRn>`0O|!^apcipvm#GTvAnJoD}aThOo%Xmhki8u3Eq2T;uoi_&JWWme`n~HO&_v`uE5pN%78a$?)ML;E!+4X zON}hUzPk)LE(gRZ3?ff98{RL|&&O*|t6OGZZQVYg6r2tF2S6p+DZYuob>M)mAtkR) z2OK$FRBm0FB0z||bG-BbJa!7LdCasVwBb6nz8en_y>ZIRa((hSncUyXrB=2-d$X@d ziI{OF+pulHeGjTrHVzUJiJB88)W|ZG=BUuq1R71g1Mz3GP~u?=+s@<~MySYOL!zR# z)RMS_NOmW)Z&I~31)36Iq5~feM!qw}HN>0JS$>&jFWNd^CA%B4mN< zTNw~yPLN*SrSJ)00#$%8HWo_3ee;>Bw_J^nN7`$~XUcg77m}F8D<5UL?_&Ot)@iN2NHkMA)yBl~^#D6MN7gWI#?e`*$O?cwHu$qPD z*@j4_cxfgJ)eVOGTL1?>2501sPiBla;>+Vb2XAsDfmjxIn=TeRYtOVjFN-Za7QK#kOtRwpFoh+p5^MQ?YHMV%xTD^{Myv zz1`oR{xZ(5oH3H|>}T(_=9+WvwWRQ;%VmOIwp~)4-ZQ1!cFyeTYPp7=%LH6= zV<@gSJP^?8#hW#R~;7W;S+j9K2 z?#`pGbrC=e8g~`n`3&ZBj{)`V1A2Tyzd#*%aTXwU+*TBznl&A4wB08p>IiG~VZpH@ z!_nu<;EPvJjC}w$lmjK^9E-BRybulR7D=8f3hug`>^^U9ES&5BjDX0p#j=ZIh#yzJ zRv>Z%HIspvBkylihm1(WgjtrVu%&vzH~$L&z|vMp@QIIe1*zH^mRQuTtAgvdWuKCl zvYMSSJ6l@bSZhfgBPa1$_i2N24G?1e5r#8vsu;%`u zX!>TM_&%LocsCHx{&rsny6wU(_3jo7y_Wx7F8bz`{=`9hJpT9>lK~Bne*k#Z#De3`qwzIW`)QL~61yqVA4GPWa!Vjds`m4uA_M#(asq3|_W5qQX|{9@AiFU)QG9 zJXS!5KctV4%{l$AYrKEE!Sm;yj+&gK;q-nPUFj>>@0PYp09uS5%xA zBC&ManG@<1!1N08Q9ed6Z)TY&HI>OX$HM(>h_PTi+G*_GyG3@O_ z4j!-!%{$7}>8~vG_W2oUL#s@>x)RoH0-dp*!FX47JG-?ZS1!{8uUOynN#2)`0N7DX z6{NB>J!vR$`!JzV&Uqw8THN>9Jb1*k7`Tj;x6#rJ&)%TkI&C?iziTSol`EWjc;g}- zwP&M=2tW+1iQ}t<10^I37%=aJka$?|eHnXne7Wp+KOx{h_w7DzDw8){4_aPU+4+Wo zaHenA&(*-hJzIew9+ zHS^4S7V)$mAMeDaG}^keD12+}C|^B~w6M>6N49~OiX0)?0$ ziDHA}1ouwk1jik(gx3}@@B4!6+rEudt(9S9uox}%vq50MJ_{F2=aJ@h#M!Wgw|}h} z?5kr)zh<1TpYfy}j7+?hE#S-#d4zjomwtG#PQL`?=%efHAr(^7xm| zjeAcJU1)gFA)F2r6sT987JSMA$Lv5WChX0Kn66E9oweuH$)X6+GsWdP=x_ou`r^A^ z@;W|G2swUd?z`_d?$paB&s6)Nr(9qz@C&Yg$OykvHn+X@C&Sz~vE`Vz^P!nz@p`SdUVW)|}PlK#1kzAKZY>rRi z8yn7=0O>D$LpN2EFE$Tc;-u2T#s-3@urxI>s;*0-BDTq&?K>qbE4QfAkfI+If>E%p z&{Yk=i!Er|G^f?bZNsmqF%Czp_6fIXBGz)6{nZAf?aS4HmcSRwbD`cpdoTx*dMX47 z{JLFCO5Pzbi=r$$SfuW#zRiY8Kucx7;w~zp0Rie=9)EM6@67Y(C zA|`a%EA_k(6yK20Vy}fCBPC!aF`H~d+7B~#*58wmEop(9&&p2UI+l##L1)hXve5o= zFx_~t`ucFgzZONiS>%MBV0N@wsvV+Ll}ug{%%+gjal zc%8iPdRJRr)*Hi^BkwGdoM|vXr4Q!4Xsd;stZ2Aa4j#_&%b~4XRj0*)ZSHx%!Dk9L zBKHuy{q;vsUR>Tk=hPM})h}zc=L6KQ`;w9Gu^@JK<-DqFVJb(OHMYUhCT;+(v_8mu znM<`QQmM@y?-4H6f`kcH|LHPHttMyGphS1-PcBy!HNO|8-^)Ii+}qPdlnGC?7vrQC zy&cazUHsrZB`^cG`(s)3uW!oYSDiuZCP2LzB~&P+2tb%25~Fp2%KT71eXHWO1jv!6 zVM{bH#NsSg--CsEm-=F_$Ub;_gx*%(jreOhyBP$bX;DA5s`wujso^3=MeSmqXzDQB z9dqNf5kvuY|Atu3`^SU}XknCftgTi?71FXM^OH%9xrVcl^4oYWAAvY4j)@k ziW#52Uy#GQomC*@IUUPR09xwJJL*Et+&+ zQ8$LgEB3yN#Buya{kW(wKlt%#c601GcKmye;)b6+YhHdC^gaGv^1TON1EAhS7{_=XG)%G|y*m!L6k-`T4kzM(|?dM^T z8CngWvme{S)B7}eZv9q28QWH73O8sZTtWi+cr`yMulF9ooPR*AV>QUI4o<#FFf1qTJWN zzA`oR!w}82U&+x6xz32gQ?E27^dIEQe&6dXx-Xnh73ap2pX_(`=458M?Qv2BiW;%8 zsjt(-^z|2iMHfsrr(Ph_PE0@fzoxid8deLIWT(ODZ++2UhE$#`=c_@`oR-X<4kp%n zCf$Jb_FlTER^J}7KlV6-i&C?Do#}~4Km}K59lUtbio+}Zj#A9CIE2TcfEc79QntMl zF2?rj8`#iigZxzSDNk8&;or4)vc-fnU;V^juJsa1AtTLfTx7ABHmx>bpd#th@YYCM zG8QpcVDjvY2^8#LrY8PsIkb|@={)0Th}jg`)&FDPeO6lcmrDm4DX zm(YF5*Z;WN8Ee*~jQsoB;s={ET?+`w%)7OtRi>Nh`LD3}Fp^&=piOhIeD)Tr8j8)g zar{@0H_TN#C{1=<%(3q+EFSZZs+=}zx}9QZIej_K-vNHxkuQrXD%x)2+1|&q49YUb zr;_|TXAN-qL!b!yJ(1mgb=zJ39o^q9SHFHvy!7BZo&ZI=GsS3(r@7ueZtxz$S9TtM zJ8U<>&ztG@;{LrD`f^Cjj>rNu&2fZti|T7XmSf(9MVQ36|8V{) zy7~xceihsL(n7X%3(a|k-D1Br8(sQ9&ZG)RHpcq^skaJyxV@={+3M;eUvZ8_cFTL8 z^$dIHIv)|h!yJeg9-I{af@h376F1Obaj-6u3e`relIsLDDU`KXOc16dR&x0Bb$}oh z4$&0*ZLN?hy@HCA@Q+*^G8_yxW<<1LR{tapC2D5H69;E;i??VxIxykwqSE*i7Pql0 zT(c1Qlwb{(G4b&B9K{byx1w*VBDI$30;63l)Yg%8XU@qRJvcSg3{qUav_m^y8MXPU zh|==@T{q8O|GeAwx6r4D0KzEqqFi!?msm$s2#Of=@Khz16prVmb{8-7pLhrnI06_I zkj~{|IplNeo+aVNZrAw*#l0RWuGXeaS6*(V-a2hKHZ~q=UKkKJ$}G<}rRzm6wdwmY z8=BstPZ*J(M^Sy?(j6HI&+gZI(RLqdYz+>0rE!&NC)lbu+5&|K5W}ndG;SZ|GQJSP zp3a<2rY(_kBB<#jQKjZHWcfX5l>0bu-%gjtDkwBQT6AGKNOj8 zhFUzByO}EXoX0t}B5LTTpOreQ+03k|f9w7!@szy!)RnK{ODL<7lh?($h>|}Fo|@M9 z3GjgZP;n_}MG-0=Yd^T{+GRW`yP_pkS&Ux46mzL)Rbh7^mSZsl#l#L#@h$Ty-Mzoe zYks;O-2R3j+8$*DEF~KZO8zVXjy zJeKH;Z~m;ph{RA!T9AniNbm`tP%ZHqsOg*c4tY#S1Xz>W@_r(PlM<0Otg;L+H=eV^ zh!RkvSw18UqsY4%HL)b>T_pFSEk{Z5jOTWX%9!Zyalo7KH=Q1(k7?tfI+rIR(Pc?) zNk$+tFtPqE6PZYHVjBcl;d3_5k9berl07uS+Pt-^37R@S4Yu=BRK0VU4eV zb8ayuZpz3J5R@bYg8&!@@T`DO0aSd#fHVOD0Ya&i&U3YI(zOVNucIhS_uJW)M@q93 zt4-O9FTY+BDJ8MqRBKCkW8_bBOcB8)>7dn5l6y%>QAz0b3*XpRayi{>|oC7NQ%`C)N*>x#w1zc8Divqm5c9{OXAl3?)ib5Ar7Za2O4Ru4th;Z~`^P01bDLc z&TVi7m6bp`^o=#OjenL)f!Gq!l8W||;(clOIg-q)I1i3w2*m3$u9hWfFK>UZ+o-&+ z@-p9xKA@jbrbCO?{nc@OH1Tt21JflveSW5GUSy#SKXnyWbKqQVsrwNrfqI07oS;A2 z6RtHjOYK~20fveP%vzcsqO7bU`gnA`&MCYup%qKIzgdpGcuA(HGxQgWebyYkZ|D98 zfXnWU)!ZE{UGU#PP1a0JAtbix-xXb0ilHO4NTkIP%eMw>$(PZV8hgHGYD7rNC@~QU zK$QE>+hm}s^Lv@j=GH4bAo!O%uST@&SP|e-5h2taH&f1DXJCLUzAD3=yG5RqDp?>!TK@PoFbaD_vmR+9LhU})QGwGci+JiCh)S4 zK&!)ZxK{|GiaTftHpGlPWH@2bvk1Ma0-t-L~11}Y#< zwp(J^a2NwvlmsA!BL3IO1UzMbsc50`H=wpKph1A6mwJqb0Sg2yS*I>NfRR4_>bFvZ zE;e`6Zb^`rVY9%lf=<7!T!m?lThP3#oNmrvtzx_^=f|F(h!VX#t*fS{nv1U}%A;8F zL?QLbtv;@42f$6#b<2#D-;Li_)XB-f=tZN^qIQ&YBxVU(j%G$S+bWpPvRkic*PzD; z{ZjXr6uVvuMpTv)Z(mJUAhiRxrCkQA#Pxet1H0)3%~DL*h=~CNNkXoRg|yJ%PPv3g ziYwU&|JSpK6&hPeJQgr4%{(3@MKy+t6^InfE*$8(k_IkGOK^?qu%4=Mr==&w7eN&C zk=dE#6#Rtm4fB{eM;SBXR862I<;9~uYDCFWiI=K}ih!zsV(EaYUvaXQ z%#<}D_4MM_WXfnh;7eF#35?OLg=o=OzPHtq13G>51pLRc&koc9C`f_%G00P;s(0{x zZlJkIsVXZSaDe7(;gOLRBP0TNA-|}mdN_e_tKvq@KM#q>In6NYLW{=TriAF9jlHdN`m^2+)CP z32{y~YIkMCfI)%*OAO$PLrN}+*I@Oqe?0}fUo+gp+b#p%$S_r9XBLM`5lx5`KW5L zc{vb`kXna72Lc%_O0Z#A8VblC`Umnk6~OjF!$4fKekPb&D!Ja@B3A>_B$qkI5KIm;CY+W0>GO=uvN#_|=QCl_hT<0-F$ZFe}2zv-LJ}`(T&Vmgj;BSEu&3Il`$`+3kmL zTbzz^6z3=aLFSs%+lLd(i5ogQFD|4K?n}GuQyFp z&ZN4kK@elQ`gaMmf;gGS*F0V&5Gz7hpDtTg?+aqjhMY4^f#x}DTph{U@HHz66oL6k5s$6CN1{5>(lMs&Qrpi_Dl4S9ihaxj_#qY)Mdr>-Z7xCoxcW?d?~KIXmxs7!yLD8Mk8(JABR2Wozk*vL400d{w^@P zd8GHtIz=4~bZtmf`+kA8ef!++T*WJ?w9t!r?N{SC9^s{s$6a>2UlA3*!}!7KeW&*9 zH2P7$Wh58maeBpH@3G{e?RR0>{WQOQd%`Kr*-g2f9_pZp*ByetaZ7W~e?bD~ZfRz} z(wSSoMl|c)w#C%A zH>FtTk0wl)j5)OtLIBXnU$Gy=oJhe@tcwB6UTSo9>3H1zD;YhHmvc&z?^Yfdi*QYfZ zPSaG0f!%(9)bBBm-QjomK<)M~n8 zt6IK8xT#mEzW8f+IJbk-^`(7-&FELVA>4pwt(o7wkK*h*jw-do5lU?jH=dSVh+BRi z%O4jz->kcD3j!b{i%Mhi*fMvk4o0P!7RDpUoShJRh0H`7$d`fyy zH8b|VVlE9-I35F=LV< zYPzN>V5p2QxKMK*$a!=C#Msv79w4RnuMoYjbkmcgtc|aclS<`k8n}rB8Kaq2MrPx8 z50-cM4xF>xrwfz+N#}My_OWq|c;)xQ>vig2Vcm9ryM*@WrTBnJ#Yz1( z{&0!uUC!{hA$Ons+cR-S0alq359^sfVCWzp+SJ{BR}cMmjCwtMI+p}H8Gs}qi1Dn8 z5~WNRJm-DlH-R#K&NSJOd;Szp?heLeh6K^n@Ys^vfKs01%xIu%i<6%!y-<;*gb3E=Kor9!mX$)Q@kIfmga9_FB_dWKmh*7K>1lzn%Zo5)kz!ffQ_r{a zxzh}-4K1{bj(s|M3K+*|XC2mG_^Y1?dCSp?+q2UU{D(QamkJxVa`y?~mwt}Fpw^{N zQg={Ks;=fC4_v1|99%yf#HJqT2vq@HP34PYn*;^1)bkW=K2u7ZUIAK39_2)RhBPith4Sj)O;`tUw(G4C$B6s*B91QjCXrk*W=UQEAbtfVEO^C6vG+b=C<|M=ez5-4}1ls zL;uP^kO}yV`#NytD#|48^I5f#X90GD4LC2Ea$O?&bw~U<-Uq&&-6_dcRS=$e)Ze%~ z7Qi7yl;Ag{p?>-#d;ZwZ)E8T6e&XNoxm?ut5)3=@syUd^R7mKrF7gq_W@T+EwfwT7 zH^>MQ&hD^R?l(7VEi43p&HkIb^U12Ip$q4*fHXfa}u0>sWOCcr1kJ*yS5WRv38 zZEE13GeStfAevt;ZU!P_0tyUc1*LTdJYudYBo9}ptE-T(u%GyPES4(dT>EO`(5R?g z#9AHgdEECR6kfSCOC3s#m9yqSJMD7$>jk2ldfg+N+Cug8D)DRv1z0)_Evu@-F34RS z1yA_7*TI4s(^o+NB6{T$!x}O@R$FCxY8yF#VGV)MV&0ob&F8vc0UQ5rCboM#*GTrF zArEMsA1!N)_C}w`AVEM-P?D}BPNyTSJv(pN=vBA&e3Ad|#x?O~Y}#pMtn%^G0CgRX zGi3H5erPr=7@SYcC4}Z&!xT`;)z9L3; zQ(;zdu4~dE7i@dL)!elVjF)E+Z!F%$ouAdXr81bls;Oq@tpg zWGrK9)P=N4q3lX2nXxkR9doY+rLi5d&o9=cXSd2-;?9grVui1Pd$6NJ=DUp-hN@rlaZx;(}R@2K%4#!!}y7u9i zweUF9F?w5C=k}~c)r)K42S9| zA4t6`8UaIT4D8c+-T2P8yaS)Nd_o-_VPCv`dGO%Hc;pI8GJjEs*lco;$z%`-DN>B+ zOG%dbq6EcDwld1dPg**E`ZU58{Vj-sw-$nzcLtK*L< z{#tC5zy<4c0Ipe&Zs`W9Sswoz5uy9pm*0?oFyTn6AjM_j2r>GlELxmnrCgBuu)6Lz zca^->BB3UWZTZ;kMPR6iAcpQQpua+5O!x2Z&}3NLgumIHgZJ^?VwU6L;>X8m@W;-K zr)t)xq;1cMDqY9eWSjV8Z__QJZPzJnd2DAyEL(?Uc;}55&-cdnNp;lh+unBTW%Y9@ z_ge`Lqpqj}?NR0Q#b|nqU*0D{rngyQS)oa$1Gg9V71y^r{9F46^p}E%9Q@WA zlf=_LQAdFZ@vQ5)J5StX_wk_7t!%t~mp$Hzcfj4_h*Qt;-}43^8vdOK@?Ggr;S$@7nz*_ zc=p@fBY254NFLqjvooyX$+YNcehx;X{wsOEBI9c2{OoCTKpGr~tUe0@Hh8$@DmRd1 z^||vb{b^*1eHcqSem~Jzx&Xm(1#h6DxaMbOnPpcAn8>O2 z`8^@^d|w(q9@@1$9Bxf$ww{T6T~tt)Hyz2hJy>8+ku^N$;9K7+14|t=rpq##f;=j6 z58YDzvN3k5)BF6*bO(Y@s*i`>eg6*ju-?Y#jVNrl2H(fXMGV%r zdGFpSt$07;x1P;`=NUrbcU=gm%G{E931L}lJpQQ3Xn0+#_pFB}*HL-D3-{sCzMby5 zIZ97u{7yV}yg0LZnV-Hs-YG)Y&}%&dOw-)H9q3qN)8d66EUAb^*n;jy-cBmyX_RA zQD0*&b#plj3%4o*iEo`%IVL>c)KD$v%kh6VAzqvuV`VwceYM1C$w_HjecMM!O5pp2 z$sh#jNZUekddhJn0k#KA9#7X<-Cuuje722!1XoCf|I4xY2epL?K%QKrSZ}n(mnoH` zG}pET_hh*c_2hWd!h736CJS>h2~te_f25n&li6IV*Vme^JEDffWV^u0zEB)4l5^CM zRKWE_#AvbAZXYfGGx2x;4Un*^*fy9DF9~Fh;p^F$o)YJrXInu{L5dza;DLWm*+NCI ze#Z7=Cc9Hd7t`2q;2dBOZ&`~pz_ZcfXDfxTwTTe;Pix_g04+v~c;H6f0Vj{Lcwv z{-up7>tP6Nm+C&VtT*SI#+fg#PMKyH4j>cHmVSEKoIUN)YDsy?dzdf6=c%+bzoO;F z`Y&s8jV2{#ED70MZi8EF8fAJ96aPoFC_<|5;gEO!cl;RK9T$;-J-e0vLwEOMkRy7A zvT_gI*VWJ>U;=J=r1kPyt$6SP$QxbH(ejIxY%)R+1Gkc{N?u{HpQTNNo`aA;V#3>i zCmVBnLW)ZQZl4FK%G;>vF&NMy(Yv<}4 zK?9${l0yg&nAZedK(`U;6j>@_$o&_?-;W_)#uY~W-OgVu4F?SZz>Op5J(kZZb>#R* zQ`zDKzexEfKn+k3m-_zqOswD}W-Ia%ym9_T z-zaz6Nm7_nIUD__ixS8p{@_VnIZpKNaIA2xxyi$^N_}ONb3U_0faF;rG*>}3lLWEF zqZJtV6qzK8bda;gb6OQ0>pzUl40+ zbJr#6ti~h>_qJ66f2+{uJjIWXh(zcq%h=BsY-qj~yn~`8E3Y{bf6$*QuWa~GfYXt& z(vmPaqVJ$U5|M%aVG$8JB6~LmkCU=CSSAEGb^c2nW6g4UTHWv+#(nwp75alvlIyx= z4=;%!CROE2gBlOGbIwIUMhv9P8c}|RA*MK^a*_swJ78sj`5DzRdu3T)Sm2{z<8KVz zV%?V7sW&fixM4K|ivc2#9TfY}O`H*B^XS>7tgMP~^Qsg-v#Vj?eeMnXf^w(yDKizKV2tIqJ0~#*%20G|xMl(U}VJ3$Ll7P`d!*4|Kf6;PIjz z`sZI7M}i8y1)59>3Mk{EWWmN&7HYy|5+q2@!Nel%#DAkjq*PVif5@Cs2~gG$TDvnF zsoPxKUT>^kOQ`+`5NwQy_=sf}tP#t99U&zB1jbl@%RyEmn%fG{@>G#9o%dtZ(CVnJ z5k>*~4XDW|q9jJT3FE8#tHu6XDrLx*BtfDWU1<+UUrcD8}#4*Pq zJJ>&~d@lgzL{J0cf(lOsOp5|l*St*BG=GZlyTTm@BNeIkJ`Sp=w`O6eVw_(r_nO3% z1j3jxb3gLk855v`qbylWxHG65^63B+-;*b#vYJ11z@}h7bf~1n&y0@5oMy0!nj%>q zTOA&}bF4WeBpOP{ltyPLJ}q6-cuJ!ARus^hqvHN^vHMng(r-U`l3oU6!0(tT31|t^ zd%vzXzx=k&T?+mn&cgE8qU*$Cd_hIiUT&J(qVx9Qx}$))$wyV&64T;kC)g1%A>E*! z8=T7PO1XFu(x9)vcqYo-NBYEC!D26VZa1*+n~N4uy7)O@o)+2}DjFZ$5CWk0Tv!ke z&Yfuc8o4)AzdphLY_z&o`*t8zEGaV9hzI3W4#?779 zk?ebF+&5*og?BfbGd9k3M(bo&bIuzi0L1s;3Am=Y^PE6~u?|&Q4N;CY2$5{?0t?gQ zk0n`HK{7t>BrTcgZ78oIsB-He^V0;X^pui(fW?6l3P7Q`6pd{xKNTzfKU|5 z2`T;tX`X2mDau#hLz?H(kW}bBmP0s$*gu8I4@J=)WmEw&4cg)*d#569@R9%veDMx8 zaU8Is>K1_iqbd-37f0&6p2@P7dFRb9`HfzBO0wwOD6;lg7W7!AuA`(?3uA`7L!NZb zO(W&NczWNIXVDg@AFLA^CTUO!fS}m%ae?ChG(<0xZz32lB9h>&lk2bW+^UU65`?0UqDm}GBr{}u{(^WLH5CCG_UHCxL8Y0#Aw_HY5IIRoy7^m>4D^{;<3qsUl}Y1Bf8O%QJUR+* zg_a4x3h8^IrDc&zYlRjke@F#DV1{}!v@A2wad=xn4BdWB44?XfdK;co)mS+m)q|o2 zEJc-qzx${N4S4cxlCVfh&J)>O)o_!Ds~NevOmm{1rh~WKJV~WwSo8%`T}8`W`Lh)3 zEq25+2s?S7@OTLkfdI-lS_CshBjjqC(oVh`QcG%RKgF9ynKB(1p0U2--PP4uNl!&i zfK}mvA_Q^ZhYYhn9=igH{pu7WB|i1|M%l=d2$5i{Z6T4Cm!~reZ^ZB=QT&hC^<+(X z5q66CxteQLomSLD_x{_%hZZ3wtfnAiBcmG8@hrk{-?v`KGusRcg=iFJ2P61xa!4SZ8u&tQkpVv-c}-FG5Yny=$BTUF^FQ+ zM1&{=b($1Z{{#8A4>p{YPdgJB)=$CxqgYH(}IjOQ2 zT^$_Xm0BL-d>Jk(zj}_npSUj)vz$L1i&aVB%XJQ;a(R+RCsoX)wiaROhtR%029aN0 z*I&_Y91f!4?G;p*m5M#hDe%keL;w{LilOVx9PhIIC9nLgZ))gU7hj*pHcxJ>%mizq z+SnbJ-S&sZRHvKO&^(r$?r4`@hK7`bKS7d+krFd^v)z7jYL3}1pR}Wt=Fb+%_}MjNNGX05A_tS5TJykY<9R z3oOk1E1QT34LEhY{qKHC;e$I=^)!oNlG+1WYPTc&mm@r{Ck_fa-C}S)#S1l0iaUrU z6>B?@!!HRsD}>S+h{wOBH|0TIdySyPG8k%-EcwW%zjMrSn1Ql&xXSYZ*|{&9xV@{S z16sbVM?P0i?oOmhRlI%G>{w12##xjw3Y_)g1?S^8Tr+-ikt^%3&GPwOgv76(^^JWD zX@AZ|CYaWuEK!L2y2F>1V#WQR7s2{1h0pjZ{EVp)hGOh-Arvt=J!_xIiO7Oy!y4>( z0NKVuqOgJi0btr?;m$oQr`MY-9}DDoJl8Xp%g(ev=h9tQ46M1Sd#c9F-4e{aGT43P z--B*oeYkPm^1GzO``A4E*#B;LrelmTdClH(n`(nkaKN1Q7Bf**Q*kqMQESnpZNj6q zAen@L83EgX272m;1_pz{s0H$3gvM@QfWcq@)5T=F2)1!&+rseeWitk5j3a| zASheZIji<{QxR1)G3^`~ANI*|>g0G%fB_@k*1(Lv{f44RNCo`F8jIZQUq63u`(}Qh z^nA&`sM!r7Y#+IOH~39lcVFQ8Vt8h5f53lB>rUh9zrN&t5r5tL)lc?(#(wd3ZvZLx zFHXCtPk;G*Wq$9PTvq~AjS7*;(C>Ht1AKt@{3h~y$#0_Py^ZkuqUXEjMgALjc=7ud z|7Nl2kp4H%)LYExEz0AnU+DK9{+HA}uB~kQ{o9?{?|>T_iX!*palPG+?N>Ixw@Ln^ z$!Xu5>}+ZhNhy^S99-O+(Jk@Hs+#+?%VjgClAzS_y-Vv(Y1cP_5*`kxoKg>x`L{+8K)+SX&C5_$#3@UhTA|Z;F`yAI2 z{Yb=h9^!m=Of0nA@ozjCTgJYEBo1c8nj-@Y=7L3HM3xLS&x|bWe7Uh_lv#=RwjdFm9QNiXW(B?cUvPw}_cI{C$Fr|F;0%Zx~EoIX8 z@I7F=+j7K2AAP@S;Ppn5iNOOP0mfizay|3q%0p=rNr3WJn|oNT)*BZ!8~&)MzrJNO zxn*Qree3I1$C}?Yn8{FD+TEP)gsj~;EpNa;itd_Q)Fj&ba?PrtMcn+jXzjGBY; zIQ@O{eH*JQ`=6X1`FD4gQ7e-=gbK1Bbcb5+bFIu|%L{L#XLuO@nlU>{LDwK5%(`lN z(Q0{hWy5oq>8{21{knVRQcroRTn`rptVY{M{^jYK>$=O$7dboNhDsqLA#f9T#TKnH zA)b-^NFt>xq2A4PY~l?D`&oW6C+e$5YvYXsu55S^*Z^mbIhxQpj%tvg4ID1Pm@krk zRet5`=;^}yop0V3;hhGj)8}SpP4&k)m&}J%8dJVkE1F`QrZ9H}XN>$*bd=G_W50oi z)?v`Ok1STElH#1XD0%qG2p3L_RFrB`Q$ao4qINY(9?CfXWEXf6IKJGv1(2v9d$hVV zan|!eVuppwokm3Tk-7HW?QWW_-xr3H{mn~LRU%~PvkeB!%2mJX^vt=)Uoj`pmybY1 zF-oF>j5Cd3#t6enhwo@g@FFwwOfMD^5L9$^l*AvO^~TPVwH>LqOMK8ocWlV`zFR51 zvt?FbP&KAz(NN>#&~pcko0Z$U zJwI;oKX;jR^k8FR|HpjSPmDl_>ApA@D<2^tDfsg6^71qcpBmqFRNI>_q=+nrZ7HPa zSj8_b@v~Ggb^2p@_K2$#O)ts?63)S7OV1r*U5&I~)b2 zu+gzF#V1?QcJX^&q{1X{28p1Ikc6Zq?vVtYYJRg~J?GP7(&I|qqWr8BP)?Jaj0*SI zn&KiJGPYO|=^R#7ZyEtXvf@2w!X$dsT0;YTRp;Q2f*iE0GNQdC4n zU#lSlRoL8?w-q?}uOxSY%$Fn{o1SfB?{H3cfsZY>*5=j2CTd>8g7B!~Obov7KydS- znaLRO_mMUCD9ofK>{8-xZ*9F+EK`99UryLm7Q4pX{vv+JxE!hSeOD9~! zleo!4j;5F;gC5$B0$zZAY}^DlZfPbXrj38Mm|HDhF79eENfVqRQ^ZlTt-rGs^@lfw zBbTobee}whVx>#e_?vNbIV5WFR;Vmxzzp0g+o8R2moH-mJ&G89l5PRA62aoalnzlCswAM{SC^MGW zMJ;kwez6H2c#cy)R+vNn0CcgBKc~zmzP^K1IoT;m5liENVUoM^`aV87BnPC$pG@a_ z5HKD+ZD~9eah(s$|6`|x;NkkD_z8E!OuFrHEd6h(LF2YZj+yS(=7Ome<4e>fgmZ%H zILA^KEXlL<;uJ|M9L=OTRG~&h(M(7L&Uk`zpu6ffifG(8uLiB7{2b5TY$jLdr)p)N zkP^Cr99s|@y|O3=kX!`X> zn4H~!k9jo*9w+INT~LyZ7;+-_maIHKWYn99rrA>)k?xX!tB%tb35;40$06YC(PD#kLcTV|BtJ;pY*`sboeIpPgrv(p`s#!F3hW#oG-5)Eit=#dPLTr zjB)(tsh^t3B%?azp5WFK2MwVwJtx&E@ztE_Qz&tfG~*)zeG>`Iwzd$jeYj-e{L2%k z*a;-}6#M!NJ6+G+_ue9XwodAVWU}({_W19D5#j84+oA1nl%wTDO(~DmYJ}rxPnT)= zY5bZ{`aEE2e`>6EI&{X?5V2)T&>d_FgildLP3oqYGnSN_3S4)A9%V_bBF*UNh^xYh zaZMG5a$<_p*Go&S#4T{A-pO26FXX*6DPzP)TWTsn>#(v7%C!(V(}Sqx#WpssW~GVi zZQT_H#Tp#=OOPk$LoZ>h<}aqbUwJN8C1I?Vm}m%O2`2MK49;MeioL(wC$1#+!z3VT z@CSGn)*>X2g+Q$;TO@v`1x4_U-!3~6;z9~4%8hp@7t5P4$vLJX{OKY_txCl~Ozgkc zZwgA46RkQFX`gqkMEN- zQ!e98?z7hODclnpjh}3X`mXv$gF6_U)S(UM9#GiU19N31$C)5TI0!elASp+>VhJQE z_S|)k+kZcyRb{-h;|-X@oMS#P-~8f~N5SN(6Qmq`oQ%`A9w>9U4EU=wk-{mD5D#Su zKE-`e2MZ*3ZN^$%@+3-KySrj8ddUNbFKtzQG!T;423&%;cwmT-%m%B^pmtj;AUhQF ztuCa(K-kaXc_>4r8c_8`yd4}`^*T94Ar2Z!9T4lXj7BGNu({*VTR0wQ#U^|C8I^F3 zjw2t3`JX_%LgtHSv`fE<-)N3JLx*O}9xIx(wu&?rHZw@Pj92xt;&%HFO_I~BQ=$8* z08NSHrQf^f*d9i_rYd45{%O_!fjGlEM3Zqm)SnQMKNT$N!0=u(wD=FnJ4uhrI8>j=n5bpGYRilu{-Ftw@uk?g{ulyKb)ZHv4dB zOO}4x7&mwDLqgh#%fEuS{OGy7Dah2%F z1Rz9i4Ji%o1a)K!*@fwuPgWTegGS3B9VI)dFr>3XOVuO8Ij`79L7}%kglnE=#DpbI zOYkMl@--zbaRqKFtMrr@2LQfPVe<>S@(&~`%L2P_K-+YQqY}EN+~2` zNfZ+kS>&MnZ4s5Ieo$FHE)Jv=vX2FN6c8havbh#-5L-b&A?tyq3`ux zm^5*3x-TfTbQttENF@Jfz{R?qMJ>+@_CQqVRJSQMarbR|Hai? zhDEt{@57=foq~k)kVBV*gml-?Lr5dt9V*>1bW7(DLnEMcgVa!h(&B(LNc!IUd4A8{ z`~MyX-{uR~y4Q8CbFFo*b(wo_^*Sj)lf5i2BDGCbNLw;{r3jOi-A2Q}-@SkS9-oi9 zGnZm{(X=<9r1g|-zDPNoWt;9dyGP-aIu$@ZP0yy@DG7Qqev3(k!U@} zy_bgfB8WCIG&PLJ$Sp0ELuHsRuC@H93Vl@j62z#uNm>nuHP-CclOYd}rGrLP*iUnw%KPGAT66m$PaX>hG7zHHZYs#<)d2BY?@#Ca{Wac8qD_*4 zFFU?D?6~P+X{KI6tw+hA=0nqP0;H%iFI(TK@8NIJ2bbrGmEPEMy%+I%( zsvCU{^?vP?98V@TK$Td31Y)l2%iGoo6c5k}WZ4cp%bG-*Anw!^s!BpvSX~F3{4I=_ zwv8Oj){|S?uNX^?`0Tz}mdu>bOWFKy-lbP$r{;S8;(5`@`_EVA$3~rpU6)QoeIZBW zRQoiV!L_+#!+7fD40r@WLADLbuOvd;J-Axcqeqy~+7*=*5#lkP`)p-Zdy`>Xzjt~| zot7M0JiDj(oL(IpeZ!9A4qT_K`*eRPB5}{2kobpE_-Z@tPhW8dELRc@I4>>QOiO_{e>t3S$i# zI?T#oNv;aUKM6tQm}q`s16M6it1vKI4{Bxc=G*AV4Hil0zM~BJ$sb@|v2(!yU(A9Q zwIse^8s~q!Oja4gK4ZsH0?twC_#8G1L71>S_U|s*MUp$bT=7IB*G8n_Qsf+Uenj-c zGarF56Z@*pU{NFbIKZ5kpCu3Z`2Bqvx16!#KZ%aW#`K=T70-9ESqY89&QoikS#*SX z^8CtJesH{$QOuk21f7ctRvf1Ehj&+z%NNm0?(e16zNeUw{hW*bIZd*gtb}1+W?)5} zqt7M}UU@O5XO77u-$%bsHV;rTfPdFDc6Y1SU4E3E{-pltlLidnjBm#yY414+?sqY$ z0yn=;KYzFwe(NBz^wBU0w#j>zJ^T}v+O!?U;oYVbPNVPzFHu8Lg^Q64{ur;EKQ+-z zG<-xL_kw0H#q~%69h9da66sA7Ta@#r=5h|X*@Y7=TN;g9l5FlMp611Zq?e@WGLm9`pxP{Dnj~rHXsP*d%vOcO3s%$%(kUO8JqlpzWQ1zHMAyj(P!D?sfa}qy1%p^VrC?yS8%$p_GR*KM#&Tv-SmKp=$UC9H z;GIfU94fS%TxBVnoj2Ycq$wU0OPGU7#mod5lR}K9+5&DG+WSek`D*f93-wTkoU={* zP^o^idbGea#w(ib!&@{>GgXnNI<{KB(vP^T@+M_UtyJYG2MX^!#iZ>tJe_bh-sDR( zrPHx!poV9ys{vEWu^mZZYQRaANF1HE~I}=!-Eh6;MgkXJp z7CKnU>T*Ngyr~ZARq+-%GIEe3s1VtyYTR-V86-K&TxfA73@b?wD>=#~;`QUTW3r09 zy`wi40AOL`@6INn1QX+(uisxCCEFAi0lpJ#DcM0WsBFuHb@)4DIX?AFM==}anTLV~ z)rDb*L#1D}re45vJcRa*1$ZMveKyB^&hEICC%B1pm4``;J~S?Wo+-{YiFg87Tg0k2 zSC20u7IuD4ghKI<2g(|T+^yqZTpaaZq;FdVbbsY!^HG6yS#dE{V{Z)hQd4~p;`cX^ zp>l|uQLUyNg0Y&w`|`*HEtmYP{>rBQgXXJqGB7Jh6KKtLg# zanySU#!`}hRD#1cB6Nwfw#;2G|Nfk%b|>I7&#(^W43VD7%J#KhHHmWC**IaT3d6-! z16JZ&;_6k*|7igV+LLk*?AxtY9ax}3n?2{xXqu%PE_y9E0ODX3-- zv_lye#N*F4?vZ=Tzc%r588Hy(C%K?3&_PV{MP?B;`6y{GD+eQ5if-;)Dfve*G)sNTIsjKdnNnkT{)3-tu7?_j47DBY2IW;qH#Q#wsFfL(cn^NxsQ4&G6NBQ=qVHkHW z6@aQe@q0$M9M?oGwX}ZXiflgXzNlRu+D=|0`%?mnOIkv0ZHkGr&D60{xN0~=p@RqZ*<*~Qr)6(xl1*(gN8tn)g8^NDDybJ7{r3BH#z5JAG{WnL%6srEd z1in+Qh=@~~-OobNO1so+@4X&-LpFZmPx{qddIpXo88@LqFTLtC)~4b}Mqhry z`n)Jw^Tb*p%RYB&|0<7#P~`*FXENO$BZ8QvAn(zsUsL-nIexI)q3d(c6~NO5HD#sXrxd z7y>p3sZ90M(R|LpB!=)7WL<_3CyfZ3&jiFR>uHdGQ4r>Od0!}4LxE%7%1h5B6p`0^ zVkm-%^rX6nCN7P}Wg5y|Q9Z`~|yePjq&ZN;gpxy#D)+ zSTnBdhH=6BOVwE9Hgl+3eUp>8Ju1YAyHbmFet&koLW$@}&~A`0hx}9Bo41FWtYVM1 zB<13JEfB8}~j+rH?Wql!%(O zH*NKgNwxX4lWUFTx54KjI+63>CGteeW644pvZIL29cslXONr z+N@C0!w^E9{c0VrO9?NaT7P0N9`TVDeJdf0_6Jw(723<4N52@qZQY$Lp#3gBS$MG6 zlu&HyQW$ZL7TV+UejS2`I<@8z9=!j?cAK*vD&GDN(BC{=&Yoh*?%nP zM|aV0GY{MojLhEr2z=Ka^jtho6KTniSFAd%*!O|zW|iD05M4PLKXF}ov1}Quxn8+) zsYYHfr5H9HgP33$E9T#ybWBxu$ij(MD z6KRPGdHY9hf1E`74jY#CJw84XSx?e}F%LwG2RdjTUAdt-^aQbgW(z-N*;SlWq?RWw zi4PNFH&}@|@Gje8(68C1IeiOuQs#4-j7=gjeR6O($-rDdTMdQwW~^`ds7V=w17&98 ze<+uwD+=j!mELfq3cv#w|A=XC{GZb&SIxPQjMSG*P3t}0^{cUMtH+j@BgNsfV+c=k z{4LwKLdl4iN(kv!`4f3tIs4^opBnEUNl^X4ZiZiZ|G^LKxxo>vDZt+e%}JGfAR*K% zYXWfuF-?`-HXqJ9g#kpsilIo6h(eC?n>ow%!@j-Oe7Dta)jb9K$|zS+(%PFHiy_Y` zW)&5}MX`x=yaLfzxjI(D4>&_aQB!pSCVEb>{_1EFIv+aZe^pW)M*a}9b$Ian7qgyU z4SpKDVcNif4IHZmiZevMuilk!@kG4Djm6O1*A|_c5M|^HY7`P-B8BJT8kkwpSJJ=u zuC=%m+;k-$F2f!P!G}|e2Oaiz8F%l7^QOip3FM~3o?04M-k+$GaiJbqRyvWIGK^V3 zV9FX102=s8^b`Sj3Dah=x4<+W`6$`^n1?M=Fdo91h z3uo75yCdjd@AM0{$i^tHMcU*Fem+ z~X7AW;O$qEaxG7uLgI+Ehv$wJF*3=X5P2_^m*4>ZCf2)3g zvb?sHmU4*BBdm;!n#o~_QE7{l6j-1*fqZG7{qbcgVn%0Y9fDwp8o`d^-=8D$3CyS= zN(%Zu333Lh$=o}d_Cd#OaQ>)i<$}#uCp;onu#||oMiAno2n|CC0;wfiuKDY7F129y z$dmZPKEjx;kdTnGiPY3oiyKF_tl%$0=k141e5}mG~$f_G_gS8_68pZ&o1%s+z@AC%0Qw-O28aoAOhB8qWf9$Cma?%V+n@ z^U7PC(}z8=@-8%}R!M#uCt=}_$dX(De z&?H#v6GI87YH0ZW%7@DOAX8K*=kRwMt9luT3~;;IofA3wC1i_qg(GR$X}<+Tsedb1 zPr8L41&lHK`3Djyr9sV+t0+vMgMYmxR~z4iE!kBP529O5ff3< zl0GGoc1CtJYejjLbGU-plP8whGRv6xSXMX)+j)T=-G{`zxq|~G>onCY(idZ}U_(CR z%=HY$zNaC?I~FJFAt#>LCt8`s)cZU4`+>)s3?-tM3vAxmOPAo^zyOG=BwbAi(4TL;~z6NG1K zs_dQQN?tRub+L>{XlO0`z8<0i3!}HNw!u=j24__I(1%c}6&tQXDWjWYra9?!xGVb7 zxS1QN&Ef-wb|ZSq>SU&UIblDMa#u@e;4BbZu8gc|J`;T@T#it}3CJ0)K>rh|0q)8~ z2jpAoDf3JF!hcGVQ!wIlq0drdRSh}8=FIFVn_*$-Ifg&bmD<74S*tJEKt=hJxQ+`8`~I3`(i9G0gtCCkxXa9q$9SP`=PLMlUZh>l-(qC+~QG}kQ zm`cJ9mVzPR@kHd~npZNDphtvys?sg{;u|NcV!HuO z&?8>E0juu;4^p?eto=OJ&x%o|6VsEGsa7glBqhLB4kkI?lEs{&PoD%+OaToLd>TBg z2-itURuhE?5xX-pX*hOiAcJxwg1oU%+i{xM*LJk?Vq~!k#~iw4FEZF8eAh4n2@52? zc0(kLzl9<{0Io$q!vJ?xU++?Ikh7-VMA)o;74vuj`aB5r{VLS>(Gh;I{(hoayfacG zfw|MGcnV5cxVE+vnwao=teF0W=0)dje6+QeW%`3>ad?TzP`5rNdO-@IT{8Z->MCf) zEG$yVU0K-~?J~2RQRtmp{$~D>AS-6Ui#o)A>_8Da|gs7kv%vay-_m>ts10T$YlaB_AzIhEV1>JK;il8 z4`X$*XA>p8lI{lEAYCAHD=!|Bg(Y#Qp32zaYTW+W0j4^X44hV|VYbzVm zVdufV?#fg$w=>?_C#RGuyZ+#>w9MrP+(2RRL%4<|1zN-LsS**{FX zj~eZ8t)PD1MIWl9#4Wz#djV-|>J0Rc*XM0?OWDId9Z#MH%#QuTW-xn2)`4Cg2*9xX zcR;)yW=&KXHKhQXte1Q+uJBP4(wC#0XL z$Z04Jw!HQV_cvw&xj5bt9`@lX?TRf34H@jVckmXqByWk}rwKyG^81_YW0za<6$EF!E7qX4sw}l_2wtG`<9KG#eVfI3aT-vP+(Ql&b&~bSi=*S^ zD?c&4poGi7tF^F8>G(R9TCyjyb=115q4_VDd{qy{X#`bxCQCmQDD~-y>NV%-R%P$B z#ZuZ8N9L<4Uq8P3p`M$6IM!5EX+JeR|8Q4)Ed(X{ z1o_m7{LDr+Sf?<7gIDT_w{a#@7MEZH^Tal1I3!MguDN9jnv*@AVX6>I>3a+ll(w!; z9l@%5IA(ETJ(=|v^h=Kcr88dtzMdWt|7mS_xBA%9-tMbOn;s8ENGV;J|*Iqg7tdvB@cUh&+xwR~{0)RmHt`17WPC_T#V)?l8f=bmwj-W zwEA<1NO>d>aSy6VihDiaWDwXvwewY9$1Qy$h@O%8G9|2!G0UkKZeyaV!qr`+Vn(f` zh+T(2mKI%*p6ST6wGj;L?nO5zs`SkJH;175Pt7*5g}7SWS)1+i{>rYAW3ocwq?aBF zo5v3Sm7X=@x$(G|ofC?Np8l$2KR+~es~D3_LCq=ji_swyW3svN97!68?+Hqb;EARD zY_^Z-H)#i(lc{l7;wiD-{H?}Bzjb3mb{)@if+a=iaBSt1txN^Y@tGLHTC(TiUR;iS zGQ<=yqZVB((E`Oq%<+AQ4t&9CPI#1Oe3U1PtQBG6q#vLTGSblf$Rji3FE6KNO+EQu zrEu0mD);(mQSLd>w7jgBj^&|I#4`&VBj)kwFxSgv{k1uAY-&g;Z?&xgjUXQ^9K|^m z=Sb~x<5-6l^uZ`y^D zx}IsuRC9}DQ-RqGFvs4Z8>pEi>h%XOs2_vCg`P&^8e#I zItV3hv7?UE#~vLO6l};3eMz~d>q{GEkLm3{E{{&F;aRs~_H*w{+Q28dkhgIud8{dV zw2#p|m62V}dc;_@WuZ`{fC&AE7ZI$Nef*Rp1%PcS-+Zx}nRDIyj~f359eDtBa_{NL z^Wo22wAAwlOkrZ)%I=437wbE{d#i{o2fxXWOG`JW41PMwGwpsM^@s#vh)y$0Lbhx% z28ucW!_r@=A5g%|j-DjG{S$P*UAMY{oTSt$ImXX@GOyQo?N{o+PD@6*tt!SVotgP{ZYdN+lYKBg>-pD4P{Fb``^qu7=da zu$OnKJ#GDhu+tu7SjUAh*@cr6yM^mY{Z-*Wz+34iN_f6~Ma5>p0*Gup}wR`mS zTp)(EIdTMJ2I}FV{LB+=RTNroA^9Gy$sY<{jPTG?FsI(lpUl5FB{uRiFrtNpubIKi zQ&>d@k5mdi_TJFqEl%OAb*3Mmx7P7e>HnyP6QB8#!D2g23 zMBxwJ{+~XptvPc1l(4-RFYwNMU2GGv``27k(D1kCVav$q@njM%v_udynfC7kukW{a z`$XvL^CdyA{dL(*h=QyKXj~ZZc*$8LA;9h&xn>Juh0}pyAj%K3vz?9)Bq{=|`|~N8&(@EuOE#TNB;7YXgd?a$Q!>NGh+k*+A6Vn^~m_E z-)6)+>Y`J=(5W2_Xe%p2VU@TXDHjH7RT2p&ytImXv#9Lb!Lb=nX$|#Nyu70Slt1^* zvV4ZKMcXC2Nz134V$X`Yb5rvm!m{v_l5Qng`RsLDb4|v$AuJ;`Q6DX_;J}EeK$2=5 zWwcDjLf;KIR_}m~q3YVFw~0MwpXaOfHEMvy8DUt)Ks9s^{I4`FduTV`1ct!Fy?xm~qxj#_IhT3diHAUej_6)P^< zVCBwA6bcg+xXi)Vo8OS9fvjH^dY?OMWQ^@Dr1xc0g1JuoHSwW zEHj?r3kE*NN5TG*1*M?G!_?GfYoCup^YQpy0=?w%@nSb7;o#@fN{V14b{wStAdli9 z8fz%|Qn?$%U8(DPIiB$L?Z6!5zbwX)TDB~{wc%xS5ss57N1SZU0zYX0g zPg45->^1#sN2KffH-898+1QK)@-L}|HDUpkgG|1QC}O*`obi*}b+?|6;HJy`SwOn8 z#o|lyWG`aKb67HGvj>S+QUa7h%v{2<1P>X&fcK%oNl_MjFfCx=QRq!ts!8Lho$QZY zO0L148`-ShA0YYCqKk|_SEsY@>jZI@@zt{5$?x~4LbLUtgOpN&(gq|Xcd z^63XBaffN0mS<_jHJm9$Ho}#x;K;tn{_<16ez0Hijx|gr4a_r?rJvWL{Eq6*mu?H( zQz0bSf&810yi71wUvdv_ff(A-Oc}M&m9U9{gke%r5q>{@?u;I!rR$WLU4TW2#EOm>2`FwtoTHuRnW; zOHWC~9y5_pYljX}+b~igc@8K}s2$iV%BZueSP3LgH;&GZzJ~x#9O6!oRr$NV;VQ)pn5>!Re5(v`x3Xj(c(zB3$HRt4yW@lcI*7IcR zHD)EiqiXCU&-0bcv-HxdTH?wC`|RW>DKw7;)8cYg#GOqHH|q|mk}mM1cR)@*`5eHQ6$0hS0PGX>xH0HPEGS^b^9g?58oumiWuVdpFvC7P{FY6!A+Qr? z;0w%)0YBpLW|0l&d5PN>6KEwJ9ok_JD-Uy&l7l#F;%Q7~JqPl8uJpRwnh@4VQl@8o z?io#u&svfoWMH^hstuE) z+ZJtCUH#u4^cnCV2{qGmN@Sx!jmBy~#DQ2mp~<+FX4!dty}huvy2cnS02DSMZ{R%8<6!SNsX__8 zdLkNrag<|TyG|9i92K?A56PE z_sUrS&9dF%8Qp(Ty~heAB}YZRx`2yBbTPh11pf5g+C(fEaNhvMESN!A|p0J(bvSC6y(j5g+GV~&^M!)3I2d3tE1RJ$km*nz_ zlTVYIovMtw%A4?aWlr26&|rUV1qYjl#OnE64w09dOPUkQN(_MFmL>`m@0HNgChZ`o zA%{$`Bejv+nT=^4J+7-M4RmW%A*R?+EPeFB_kUV|6xk=mp>aCc_bmZoc3Z`xS$K&$ z@F-)duy*_bz5b(CWaV1{>uZ^z;VJI72G%G<*go)F1{YgFdYEx|6i~$BqTH?gf2_d% zPEl#y&&|;OzZZpNTWKaa_LR-%G}j!)bDKJ#jmSvaXJUcC2x{jX)v0g#Lg3A*Zd$VB@cwCOnIv27G9{H#)(=^fsw+ z4d-N9G;um+@eQDxva=Hz$B9U>Z)vVZnbbOa{yx1t^gxYL%Cip+@J;KDT&ETmTLCEa9arT^ULKAdg*e_18$ z%wF~oz$A8UHG(x7qo%Mg8pPMuS3lA+d}lnjT=#BQFfamL0&cW3U}J0}KU*#F|17{9 zo|`yodx>&7%q+T!aV`A_zf-ZmJHS=kZNQI11LFwFijJG$>Xubi`suPt7Cfe~QS`4l z4O2>4Z@LVAp^YF-@F5svnQUcDF3XWM{`M21lYV%+pTzxj?fXyv$hRlIB6VKOeR8g4 z0tOROX+!^XW-~F0pIVB! zKD`}Umndm4|6|kt!-g2xLS16+bZXp6TFXEOL}b5|q-p~uW=S}~1uz?%pT0e3bz^!{ z^vU?&{GSx4#2m4fM4ay}jq$Ao$E0qhlz`AqNXJYgQpM6>fk5g}{N47_qt?lj)ge+Z zT>!tb8{ALR=QzULK)%Bfa!F$?pd5PsA=f3$N>Qi<){sX^K@5B60)wfEBJ!?&r)aoK zh-8~(TUiLjFB+&As92hQ>KJT7cv|!+U|3#U45=!|>X{eZF9j6vz^RLi6ihe~Qh zXwo_>iG#Uj90m5q1ZHHR(|(rEi$fg+{qvJ<)pvQKhQdXX!TP?dxz6|;!+MQqXQWgz zj%SX3Y1Ti=@_o8>2)W3}q@-&tv{ctRyaJdRnFIl-@UuM*`YEUX@A&IG04SF@K!P<> zk>;@a4C}_Fgv7p-GSIrGG{RbF>T2!cLfi3p&rn?{;q5HX)oc3;{6YTYH%-{OWyum+ z+E7CUD!bhCAIy)VAMZZc+z}$SyH#Bbw=%D=cLJN_lrX;YpxG6F-PtvA!0zA)(U*}$2rTb0GMY;sVf?@`Wgen~a?xFl&GwRB%Aq_lIXuVTrk^+2s%7 zO-T~zb@u`Z<2oweoWGmK-VF_Oyesq2{7A=)lINj>Q>Z!E?~077fE_CB6-)wZ&$KU2 zLynsU->6A&Qe+7B{Y5W?Y4m|!pH@k`DQou(-iWLa#ZwlnWGp@i`;YE+H6dOp341xm zUUfV8@0>FpMFZMj`>I20Ok!7!lgKhyi_+VU8b6XI20rYU1e(~rrDj*Ld}M}z9~i2R z=Kh6|#J0K}62M07|4fVjRrfa4dS*-6b-C6mC)?FqQ)QlqGpxV+XzjFjQ?GSxYb1QD z*p~u`p*~WI59hzB`Tpr6YTXb77{yv-)%^3-OgFnwD@Wq^*_k5P_jOeeM_NdSH!C%x z)w-A#=xVt=rJhFyp#*xtRLlBUj&!T|Qj;WI-TwD)*HL+H8+MhYmzg|l4o!MM$<~27 zrFSFORsg<4tGYLTgkzV8a;vcQnoo6JW)v4>Y@qiK!oh5$iASo*)%3pkog;*=Meydr zEryAaCzU~LA0A%sI zSHVpG%HT-L?jD`w`E+wYPDm}%-i7INo4cD*Fk|&G`t1W?;Cji6+{Uj zluhaTZ&b|T{Vd3I#n4g!e(akH-TdhfRB=Y{c}kw5#fZ)|B9XI7xN8AT-X+OdOw4&M zRcc*B9dwWKfG6H3jdrJ@>$!a*zC4)_!M$-nV4I-8RCjPDOqnJ|t`2AvF7uV_fE<4o z!CZg@_;{$>fJ;_tor^aGG0mwxA0sk>H99(;IUYAz?`-XB;Q}vtG4GQ&Z0UK{f!1%r zvKiPXgW_Cs^Wf4W5Z$A?of)Ec^52)6HOWx7y$_V7dDC$^5IGSNYsDEKKb;VJl(|-do_cNsn&5^v)dXeJ1YU3$qJn`F117uXG;tO^dbH z1u!ayP8^AY&>K(-BGqp12Xh3Fj%i3xAl6h24hMwAbg9EkbpvwG$%~I97(pqDQ!{DS zPB~X`lqhjzyK7$As359;ZgR8B)_6|%P=xZaRIUcN=2WK{RG_DWOx6I)%S`|r>vb*O zTkeAg^}je>s$yutV^Crba=VVo!lw$ED><+D*&&rnm>Q#{tAtAROgNv5Z82h z3IzL5Ml@h{Vz)8?-yL)c3fGSr;GX!ro&JB!$y)ATl!*y=mSn*4Kh^s9rH1kMVy|&i z!n!ytg{JgPgHP>RuFz37pPda60j{54TTvVaW801F6Y%4&nI_dAdj4KHRJ;1=8NXiA z>o+B3`Xw3<)fE=ZU6zt*Xi^0!>}_6%)_mnepK!)OW$;qrE9U4jP!}z39uiTo(y0)0 z+OoEm@pp$n?N^w2s8ePjh6=hbpKM~kn=Y~7sT8KkaE#-4>*9p>ZaBjec%yUfDk-^0{RbXr?u5h zxTa^dj8ff(Rwh`J`ta-Q541$1R?gxDR9aHObEBq62WJ)_E%6B`MEnn6)IIO zt^sqa_MOH#TtjX3a`1u8!MQc?>4lFuS0yl6B7Z!JRYbv5J01N<%GH`!1_loxGV#l`}LL)ip;R*Vl z62grSNv|OPJwH7@Ar_MlnO$GU<+=T8NAgcagV*6(?uq)x(25GI!H6I1i6GznKrOeLIBY|Opir~oP?KcXv^NG`@%Swj#hVUgkB*Cza|ox+ zL|!KmrTdC2k^D|B?A&UV87Y-FCT(xq(l(CyO%U^_4O#w8unk(6$`FN%gd!*~ro4tb z+^l!tPeW71FrunKX^=M?&7f~$B4qkfGr;;jWo&Ss^2i({`JftdygBDWx z*y^Ba6Yhb4YOfEiH_|rp9hYQp@~@N3WFJ`EZ5WE z?tw?*^%QCs?B6r?7jCs3dNgR7jjr5&sMbg6y-SP;{1s5tKdurxmrY;VsUU%Zxd}Gg zC{E4_^a)3ew~69!|9m#3+aHr;=dF5m`k|0A2?oC(JZ%wMyj^iNJDo`_| zGuWbKwy1fvN>>U^r>avBi=Nc18rv3&b+FLBl&0Uw3WYW}@=(*)54G@z!~z5(!b&eA#v5v%TAg+Dx#K7NtEaD-N7yW|fI|@)g4uyFz!D4EX3788;pi;@!`jRE}Zfhu@Q(BEQqK^JL1FFBGH%juVof^rti?CjsbgS`I!I|*E~l_ zG@*w=Do>OnaVuX)t1sjJQFqJiS;}d14%=j$27As$gcvC!>oz)SwJrHp*?tTvI|K;TAeIl=sLh4l#shFa zlz;2mF`iDKEunX@?OSE@u_RoVO5UPU2=pM^Ty{3G8gvuNQEHQ)a>l#kcjjk(Ql&K5 z@O38J51#oY#uBavSAO4M)?j74TAR@6Ws&qgE_BzvNv|zor>yog^sCo{rYGs|W)R+R zcyWt|z~18WGfJ6K8ds0*NsEbc0iB;>Z)bBp4VDvDClk8=Nj3wpDMRsZY7g5g|Jy*Fx7J|;tbIc7Q!=uT zp$U??1Hjz8e^jw8;0$&F`#jm8d{TpRQ(nt7=iic~T96RfkH9 zd2e&Njd+H6x+K0;N{XMyIKVPGyoWi<@%6&&rWydVE$LxN7@_|`pI3jQ51|u=VCw#S zMDx>Eqy2D$f>^k#3umE~b@kymC1a0O&uK*yG+I4?Mtg0!50Q^jNtHMSOk zlL!Gj%z84PE5uUh^ZR_`X;sJN`_V28-4x9>Y$b|JuKcOsAeHvM_!tX(Ul-JkFjJ$F zhvLSxRU!7C?|{|HB;n=?h_mgJyj@km3poKV>|p}PgN0Da-{e6yBa~420-H_>aKnj4 zq^3LQy_MUe3`3ml8?j$L`XcJ3AtNutQ`w=d7q}o9y1sXK4}@vB&@>TfxewGmdqvCM z>0KD~K&UVF3NF9*rL`WIxuzx-hmx9?tY8a!Ra>LfO4};Gx9L&&khC5?XULcq2G^zs zZuWDy{60PA9vlRsza7ZoLEq?F%`?Zf5P%Uq&}WN#Zx8yOl7*e-|LUz~i1BVP8J990 z+l$1oBnGv=u8=e1-&ZJ#6o?08Tw+sOTYk*U35T&t9EKPW$JXXBn1jpya)a)BEAGwi zskToDQWW5ax}xvxr!UR%Ygr3^rf3DSpvJhi6yg-DUwylhFaH4)?9jn|X#9OrH%)!C zgX^U#QN<)sK`fBW;QYbS^fb1EQ}u~fp-#DlySxJ>0d>McnylO2nV3s~Vbn@vHx4=U z0Llp$Bko$nX4Q7qn9$Je)+&`)A^^WYbKCzpbL z8c)a0v6)k5!wD@u{uAFVbThbLFEG(Z$K;RiziR_}z3%dy8Vzj~8O!P`4_2d9uXKi6-Du z!`-OD^^TWQp+iFxcf~r`hPCw&b}+{= zG+7z#@fY3jViI#JgNo}wQSAYtm_Uhd=jbZHcd63H#5(C_{>4G9`tYEy>tabAv!V5$ zlWZhhD6_IT84Nf#Ls>J4W=}3m>6Qzxt|;4)dh^#oq6a ziD@OZY}V%vgg?Z+$`aPmc~B@Fc`U9HGM*`yM%qDA#}=&|p8QT5BFvxtncSIEpv}En zx|s;Jr_yZWB;GSqt6LKpmSoY$XvspMk&?2@mNEA6`ZSNw;?F-!yWivhsR;pYN|y!P zLHSqHIXbMV#l+OG9m-nSR7%152`@u9bG!-K3>L-Hn%yw1Wa3ER(;3S`O|{uMd!_r#K;!h+#+Lt2U&k5imxUyYj`1{!&(zlxA3JZT_8^Zw?(n^d*a` zBfiRmSvCA@VT<^GljSgjlvP6IGL|=`c-4yzT+gsg9QZDNsQT-p;t@zAt@gDx&$u$3Mm~<%^RpFUfypPrf@lS8LV)Bsz7{A1=bX6fahgw%nD3+Ghi6o^4d_U^x8*n6f39wf9(d*ah zWpXaq{U#ddxgEve?g!7L-M{+iuD15AUk|YRf!qILrQW}6pY7zsx>SwlU>l-WMK`c^ zda!OD9N?T#^og%x1rM2%LAqzcgvE0aNF`E>%Y;YjS?%R8W^%m%cKEWq@kR|fzol;A zj3I&3Z6_C1MokBPT(vAhBdR$v;FYQMrDW~R_XU1j0_*9jryO7szye;s+fSMffP z$;l0FKbGR!L|$9^cgqtR8rRkBS|)qL8JvKOR1uTSTh>2M>93U3Na~+3fkMm|mH<5M zX0_6@@IV~FDn@l^{YyK zRd*gU9(S%wI8GIt`*}0=Rj;N#Io1cyay18c1hT5y)WdJPq?i;)R|ySm*zugt=CPzh z-965%^`F4P*|0$2fEW9u{1uUP8Iwrc!&jND`Yi$p?siGu_HLg%Z)dWfNa~=~BL6`xuT1Ax&-ra# zURtpsM~MgUNymSDaxp1B0yYz&8?~rIk6(^_s_d*l*S?0WWYFd0V6pF)$irj&R&>7` z&embn+QP~~fkB0LUY5v!51J=8cpe!uIB0H_EX=Cv{F?2J0My*7dYKmsMEd3EkvrU| z(5pkeJjJtDabB5D7b0k=DB7p!uFG zDva%o`tv{mQ-2wUVPUiQQX1Jj@W7U))Em(p_IWJL_z?N0l$AWh9f<*<$NtF`dgPTI zKmnlIn=s4xlk(jM5-Vf8XFn&B99lCn>#orY>H7;0UH`%Yq5qJrjeN!w?LAGt|L`sPU(TR;n9;U#j|I2fFH@&hxP6P759BA6(nxo19Ek(ilHiN>Z(Fa%UKh3ape*JX8=g z!;>--g+sJ6R3ekdVkprjUS{Fe8lkfH-8-tDf0R*@j2iZHzPYA?>wjJFT+xeqqyiU= zr`z&w?{RFGA~(GKqDd(cmw_t&%F)^f{HoL-sf5XfkJ$1sC00~d&v$A4P9_~?`tPgJ zg?x|R=H(D!)5w;hCH*yYzKY2wAbxfE(Rh;fTk+oUENp(;jUjTosHOpd;x+RU26eHN z%}Nh;aG%#gVHNKGN7YwGRn>i8tEeC?-QC>{SGv19q%Yl#N_Tfj=LP8orMo+&rMuzs z9-m+R-!J#W9pj9#&pvzAoO7)$I)M!18pEPekClb^?bSg4G9!30-NCDZe%~?(MC{1F zZe(69jEdVeF>yw&2l_dD&Fb~QvO^!iks%MS`mQv3#eyObBTu)5U!s(Q!p*)CJzvnntimXEQ1zGvJ7;c(xcQ(D~s_i&qBmMvAS1A2W$XEffhYX zn={U$vNG=4V4{-RD;U!+-^is5dcje@^P4hJDSx_clwoeO&0dL~h_z8BGMf+Hv2NiZT_$~om@*N= z8siyU-!+Gg`|w|L-h4%yB$Fru zbKG6uV7yB6fih^p2eA8vWCtff4Fpy~>TncR2n*X!<#W84j<81F%PCUxF-BJJhAub= zBC9$fZb;m_y6`FleB!!t+VU^xnlf}Ghjk6wYY*27#s%TbRjvdQ8u+iLon7hbzf9nC z4WKdLlqvlDd1f^KnW0V3G9=Dwf6${ec_o?r_ToQU0MB+2sfF}gVN$UWpjJB~qF=7D zdv@pi@1jkJ4}o$`kI5eVF7)pfM@2xcDI3eI{~C`}*MY6>7E6u7^my)TaPG&xJ=)DL zX9KX5pEhzB6XaLFy5XdCh$Ic+>2jB-7MI@mXW33ebW%d%20bhDg=z8p25brkO&BX9 z-@=iJvPTYVU+Tx$*0Zq5Q*F9O2gU-cY5z93?Lyi!w^VjbIxq5Hds(n(r~!_pG!1Rf zT#M?g>DQ>L56#N$OATIhBx<bP@x0Y0yNBZP%=L;Z$ol~VJ3~9dy5Owhbj?F!oia!MhA4xs^zx!q#ejuObs|rJr zt>31Saex2n18L9#!qnmM8#%yNyI-M-tYoaBic$U78-@y+bJzX*_z6kRZo|x4^W!i) z2#K#7Ub?C+cr4Gc8j%j6-fJTwSEkv9gyU(=c-DObr@?M6JB34xCdBEB3DIsq{*jvq zAzEHHc(UUPjo-wI4;vDlXdmilJ`9(UX=6!Pa~pur#A{FsRl+07t%5(+CcRJc`eGFn z_ce^3JVQ8e>$(@LUUSaEMtWFk8@)qHar9JBj%xB8Ki_Fe&F`s?VX_tPTAq-l?%wU4 z_rHCGJ?i7Cz)#Bd>GZjW=%4Rsg+3pxb`AZs@nmUU`|iY8IZ~`X~xw<;hRpDIrxvc4zH2XfP_2i z*fa!*bdKqMOp6Z5;dn`%NzcQt_*4b^e|Cf|0~_1g44hiZ7&E5}P1}vt4>qxx zVLzsJmf)6GvliwX^K0E1kou}|rdGXl4Hj0er4Fw7u=&V}$BziI^1Eg7d495{h;h|g z%capHTr!a(glktMPR$-UGh*P7O=eMLLI_f9YWG#kxYj$r~GJ*-;yvi@1oz^s)BioUJAPh)E@p6L{slk z?&Wxclh5gES3F*+VHWYdOQi3T`V#jTyyZ%c$)l57|JU?1GZGJ9)3EfJe4(jnLV&m9 z>HM43R4;03z~c{=Xq=*%SnUX-=a6g6PZOL97>K ztumP4B%rs?!qEj2v-eniTKKaUl5`>B@1A2#)%jALWy-$rQ^%$k!5St+{a`THkjCHI zEv;8QkK5b#f}HzJAAwL$Iw$i@+7VS=j=;iX^Dg@si{!sRkWCzaqw%V)AyH#G;OCD} zI&!E_C1$|qOVK)C_j;1z>WyBl^L)aoB3|5d0mcM9I_+aYhPwyyxJRp}yjMy-3BfqK z(s-fq4I6BZnhR1t{_d2! zoB#Np=2(d&?N4Al%4w^#JK(=iZ}T)+(lVKP{9^rb+T4!m0DP)*+TE)Af1g^O%vS4E zzQ*pKwdxDkDs&dZ$zi}o!5@(F2nAvt{2~=-|FYGyTs$9T-vaf#)Oj%AeJcC;#9@h?^(Rx6=PLnoS}p{JBwerj%x% zd&a-Uo*x}`$1t+z5V~ZmRcl8e@dkqp2@9V*(EQ7{#sy4{qH?ln%!ENMar^Z1De?DC z(-SRgLd?hv5eBzAb>F6bvlNeKnr;3eWvir0aV&Wx)wE}Hbk0>E5B)KRhAWGT3zUlz zs2?Sg0bcGTL@bRgNW}Os`v;N~)%@*}X)6#+LyvzvaXL9&2<$j{j<;#RS8L9P=pkVL z#s)o~E3BjZn;u#~mnlRmoci>rwDiNS!-b|f1u!X|&=cbJvDPTM%h5OCJPd04Ly`zE z&6EemSzaukIlP`Q$e&2Ovw{Qp{QkKJv#;X{2^2ZBk+)j$08a)`-cq73EV{T9vq z-lcl$=_4RGK6@x>DF&0pg0@M{g-lf+9mD+Yvt_jMKp$9v?o}bJ7dNkl*T;7lwezo* zFjFS1w$km!%P;{0y1%Fr`0#1Wd9WpYKnn{jr0UF>j7MsWQH8mPh!{lz5u}+NN zKlXqn$J8^_P2_R>B@%}sxW6m{BC4^SQ--Z!?ycWT1nu2;6%!;{{;|3KC2Cfn(L z(pxW2`EQWsU+V;{u3hqPjOs+c$3r0)jk_eU8)ZH}@jm^SZfrLzG4Vx1Tq&r@7Toeo zr-VX!+6R^{qApAPreYUo;`aJWeB*$T-D^!zf2-7Ki!rC80l39Tz~Ek$aEcCHe{o_Y zVGCwqgA7ScS4P|1QQ6JniE*Tpa)L}7RS&ar;9Qc1@Eqtk>~`m5_h})A~7!gH)_h0(HYOD4&x$ z8u5n`t_5@sLlSeQTR`^pCm0BfftxOsh&0DL76cajN}qwY88kWe6=mI)sws0{QQ9>nwk-fbU*|G;=lp3LL8>GWK9|4!t%$OOxWz!b-=-IMFykw8(gxJi3Ay076N=c5%r<}ENYn!Q2B#L(LsDT=`>7iBf3zK;oUzG} zR>LZ;3bwB;{_+ah>j{UIaMd~QcJKvUi+TpT#UGrRWN7PDnxZ$4QL0#1U{C#@XrFPL z46;Abmb|CL__5s|k}O9cNr!WwOA%^hX&G82m}dNaVtkPN{rIdF!YE7LT4c#Rsj_Kk zWE9K*;87&-0$7vHQ9lM^fg&y6mbyXwdLT|&UhF33)noCa3H$4YUrm=*b&<*)D_14jwq4%mcQl71mq{bimqrsr`<6@85?E>^coM zzA|Nq@Wwu^Ugnj%%#g?0wO}uP#X+?T;fO5MrgrQvW5(_;Pp>h7XBG;`A%}kX9?N- zISJ^90*NI8sSNO^7I-}BK4H}B)Tevs4@}@h1{D$86Nkh(x|*({l)`@8Ngp?vwJ2IK z8r(g<0shpvF!0(P;T_r$w_8n*Lj@E^b0qgfPQ}*Fjt&Gp4HdxakF9lx*Dn5s!j6B2 zh||uxE_5gzTm`DBgwHbMkjVAPRE2e4dCJhP`97jG>qT$GiMqE%6&BJdqq2Ez(@?=H z%QW`UXz{IkZR*yQ+JDlA6{#yVlZ;Q{nN7!RgwQ$_{#=>^lYJ))(Pig$WM{hfc_FkH z1vUwu&8^pRQmjo@oh zSqxk}VKG$PbiV-=c9YVNN|msHh^7%6UoQtT>3=t)AZWX6ey=_wX}{!kY>>d`$%E6xaljid}_Vw<gOgPVng`) zZKo<#KBk3!%_X&4Z*!adjAP!)QnkuOx5w+(@BQc^i{?OQUmnD(bWHkre{b*JEMh1=i zaNhRKYLpV^bE~_J%5e;4AcWl5)bj2r9&IP~O;7~hh*bE54 z?7a6shx6(c&w%i@`93_wv$=DxsY2B&HeWcSNWi=+=(Kei2!)Eq zr*#_r4TRPJQ+oQ@d1xzUEHyWb_Etz^(L@fq>82C+K?Kfy&sn5rVul!tq+yi zChtez7io#4XzD45RSZOrOA;r@y&dWQ+mKesT(J5ZscIrL8TjhUrPKYMq=>lsW5TEd zKe2{cb51y<%6#2JnZB3AXm_&+ek#g)lZ#R|XipkJBSbNW?izQd&W>MO6m65M@|CbD zT)u+1S^dWeI5I5o>)B82EPO4qilYsHe5VqcO_y*W0$1&Lf!FfVds=!PU5h5RmQneM zZLzjO^KsS%b+KF>jK=q&TCa+P&3El0*4vZPtS-3POxMt9lbMoFC|k~lUIj#LsKJN6 zWxlhD_D!K$VTzl@wdJJ3+MHFZd?{l=-1g)_+h&Bb%;O&kX;8dkm=KhW3?)>gOA;fvhFx7ihygMh5BBI<3zqwIq(=e_=r z`>gjorqq(gSwh8dtsdWxx;eLc{e*8JYk7+)_tW;B<308-M2b;7f(c(w>J;d~m?r;(2O7!PJ}dkDfwbKK*txHNW8=USq6m=T>+jnCN8 z&%9p3a^AC-Hb8h~L0;&s&?XqUT;+d!eZFbm+rEUleobY%5)L8xta&89%wpF&b}$>v zseP&lu!ZIcK>Wx`KnALv@sMGZDEZ@`h{p#~@msO*^tSVQB2Jzy3Wo$#Iiw<=muKUI zEtTMt(@ABc?ueUTBVz}19d6jSj>3_qBz$ls4Q%4R;A?4w*bLLHYjUf)2bq@D?^>ht z>l2d|o-XO%bNh{PzZ?;EobRmf3}$~d?Kz_NA8YLJ&+dPL04J9iW{^muf;ufTn~$+N z`C$3(f$#H_&a{5oboRatgmtp4Jd_)>u(dE#ba$r{5ZC|8tkm!xcfI{bPji)Tlw`11 z)M98tsK7Nw_CB4l}HgGv9>Yv88+>^N|Jz=wE4$NUTZC5Y-J`YMqM^Glg6k8ynmC zUf{MNPTHLDlYJ@fx1d6=pFV}cHxmm~pa)1JCaEd&`8CrJxD-{yMybk(jaUCjs77e} zMhjP}h@RM6{$ypp4Tj|I4&^@PW!k^DE8!*9{{|%|&!y3G)@{mM({pUf8+oac__n;V zq4PMb?M~&_nb=DDL&z|(KZ}< z@#)^L$KZ1=Pp0(--6fog*THuCE&ne4}%i1iS2sX*W-<5y@~Hk2`nf zqVWfnqPfX6zaLwqbdFo(NEKK!a!IDCUzHk*luosnA|` zj}z-!-|!3gNCLrMFesp&uz2r~;c)rdcfhs^{@vCL(>GF7Q$Y(tW%V#|RzdO7%QM>o zmhTc8F751eYz>STeT2aQvuiuc$2wu|=0G6-%+S}XLe@3$Tk<`Kg@IE90FZVw z((^MO{xpuc1;q{njLQu&k;)EvAS!+WGL`U>$tf3_m|Ybnqi#a`tkF(L$Cc^$*UX!0 zGbZTdZByLfFWDDnP^wpCr{~cadV*WhR(nIW`+~Wlw7M7-dbBkEZPHA#0D zMS`?zi%cK5U7d6@u41`jC@=8eCi1-W_VaAweUk`kE{*X#?LzT5-5S40Vip>hpc)Ht zk5Lxi9ms!rnAgm9XP$3Q>38E}Pz>2}JQJDMQZId(U%hcsj9FG+h?Rh83+wm1`HDto zQ^Vn(b-U&j*4fhBGvE2Za;p|debc zF&GwWejCi96y6^DQyD9#xLctynNk+3SG~6+guUt!4SFaXu6oAy&*=iaoZesBw|8?x z*E}DVbhXM({Zn2pU+$@e|#iJlmZuKG*@BKqJRiihr|K}N8KG9E_E%ag@ ze#f}emrYkc=q_o;`2eZ{M}O}rG_@4H^OwD!8!9YG(=WxBr@a}_BT|Q312N{<#qK5c zy+?zHVBHtljWb_?tHlsr&eX6R^67_o9PtIIk83_(LC;n}A)V*=0$LMaa{adS4LmPl zDw#Q|hd%$_M)$d|W@X!PzuzARXdlM61P_vm=L6_54ta(!%1<*)Z9|4n{#AV__IHlZ z>#EFxnD^Z4o=Nj-tz+L%>^9&mK#w>%xZ}0nMfbXffvU`ogTAsMtd4KcUr=8xTN|4L zp4sg*KlUcPvh#}-?S`;=-O)%o%lbSPqTCzZ_z@91ehDo+OliK`4Uy9nIg=r1eNiUz zKV#v27g*bN{v=*GqF(Y2%Yh~`YX>^{<&fC$bJ6DKEWbV?pUh$+Jj!LSuDk>FHJ=?c z;>$)qEz4x|1Tcb8p0I7+= zY-II?+B!VfBeeP1ew885K5}}HU6OUo|CqOB37_Pi>UxED#)YFW?#{#MLu!&FYn^N5eo%!wgvVYUi=>idQ>k6+`5P44n6Y zw5MGc&>VvgZe%*JUDhlpLst|8=VJqB>C>k$6|KkB_-%rzi2Y8N^>AMpq0l`ds!RLq zoz|CI?&dq#1RrA-s0YF5)0db1&U@@O_szp6@0WY@NY?I*^_LW%&}A_@}>D#W(Lq_C<#{TF~CFfI`+a*t05bZ^5`A+ElK4?w3+lj$~>$ssL zqFED!BG|S6it)R7?dB6tJr)zoj&v~g3in;vDT@U$?YUA8Y?N->%aE>~M|Dpwa}HI` zN@v%z&-wo209eT`I}<+5`;uDVHhcFfzq9KzOmZ`d;m)k#)0@-7PGBDWDw;GE`tM_P zc@@Ic1VT2Kl>@H(23y7w)4gw;kq}>yowst+B4T>z56s|4Zo?hPal`x1M&KRxvbSZoct@rGur5JQswKnRUX6Q^M^;Yt?ODBiAvTJ1C>e7 zVht-zU<&qHvH5Jv(8=30p1RW|(8f2sk*TB4xhQAd%L~!IIH!+4zPc5&E~EDyo-hW; z%pISctANcp!+Kp*;IfBQKy_~}#NOe)>(Ug~FJLhKJ@t6?Bk_(DlVsmas@EeXQ|B3c z_sdc(!6gIq39&XxSm^Nx2C-GhnYg3x(7sIqx(=hrk(c@m?u5#n#g6S^7r!YhZ)@Ae>`eH zb!v90-v){wH}|&y?s}MDpsM0_v!Jt2=vS%VIs{B(Ila>7`{DIIelGncq;JPNlll;( zoe>>*1JdppFuCF_E4MjV!uNgyVH58j=<%CNslC5m&ldWk8<9~Fd#j*E2|bQ=BFEm;v52fM{j-5GN0!9Lu}zx_fwPwk*T=GA0)?$=XR@9=!1G?r)NO#9}Zu; z!e-5*(le*q&Y{DbhJg*Y! z+i!al?bL*Eo&xg8;RdogP3mZeb&@tOv*3sQgsalnX8J}r1Pk25klsdSXZ_Xwlq)cDgr%?t8Q|Iq^6PD!pH_u&j5O`vyNiKd|DWPgIOKA0Nw(OpU{!i*p3` z#+4}^hhgbcO_!D&R2^T?WbO9_yY_}RC z7dDw6g6sBMiT3xwb;O^~#m)8tAts3;$8@Up3e~8Yg&@_=$MtEVrzD%XCQR@&AX23M zveKTAFH#W%a8Wjloyz1X^H*jocl=pV?qqBY-o;x3Is{&wA?wZR;60xNJ=(12`1sO* zfgWK~BSc|<8xIa9o;nkixktp+Oc>@_(JPtWJ08J9`zvY zP;rj~l;;JZQ87eMw5A^0>jK5g|5M%E(PCKNHT%fR#kj|!&=BbEi3BHr(I4d$YGR&iIStOU$Tx%=t|A9?91x%kFHU1+4aHh?b(Y*7~^}@HLqv=%2D-F z*zidFuY)FX4^Y<-?8zxxEb@}}8$sw@{mJ1aK(o#f)$%j%qt_kjG7ZK^2Wb`S2&@WO zj@mNc_BQE@AKn^xpN2e@-||s=lUsOP$f7t?e6BW5N`p`REe?HBG&t(;@lPtM-qwix zEZz|Nu2W)KtKxm4-!?wU?rJVBaCYyJlg}kAaPfKG7m@gN5Qo^8-AEytd@ma-%~0N6NkG*8*|f@tB3gwYRcBp@|#i-~~{H|90Eyh@`ooa^zU1PVG`N&FlV zmPjN|j;BzT@E=^rPmw`1Z?-5PP)7Jw1!8{topacc?aS?p>-^=}B$;~mB2AZF13+XW zmOOzYXA0;9qDX+YKk`C?B%M#R#HXv?s`3oc zxsa9iWC&@RniSPn9W=SFiw`{~KTLBJ--vVHEgI@km2|&6x%>Gqrh#_7PhXJzZ-33% zdCj5X23Yxe*@0sE3c3bqkcqb#bs#eFwhf2E$AJpV$y)9|lhh=bs?z29cCrfQ*=zg0 zC)xqRkcv0#%@7Kub@u#uuCz{d82dyODkvS+DQ?SG;RL9Ldt-*aZL4K8xBrc-(3QgF zeKFDJCPn4e3$#uTkT1Dl)yIiQKd7t^ROx|ID!1L|Hew3ddj5CaUfhHp{1g~gyRS!K)OTIalxoj>RUK!@vny~R5vA*O@9a0F{al^CR6ZxTCX0-piREVA zT{p1v=Q_->RtisY`=yIkwRcH!<45j+jOlQG=Wab)ZeBwNa&|O39LiKz5x(m^xZR5Q zu(@%|zVLRO&3t&UNmBH?CnHYvLnb~rto&_oi=*uL$cpgWd24grW2f&Gir)!tG?IBW z!s+D`bMdJ|F-G)>pT0)RbPeLd&-Lk(AB0;PE)@;P6i3I>$^bF`w@3=!vj&ghOMQv(ZMrQa28sD_6ZezopE}*81qAQhHDa zF2-?Tm}}Z*pU^A$osrwbX$WpxoEv+a)ww#eg@~1EP}h_nV5(j`GzFl(#~F26S$j$! zE7#fvRXC))2Bnwvj2e);bJDhFKBD|Eg>9ShZM4)BT_BMvTX>a)UsSSgVa!C~ZXLRl z#q2(M`g!p%gYtvgCLvd%!d#E+&7b471b4yQu{Gkxa?4doYF$$!qgNK+d)Q-A?xC{G zmzyC0KId|;vm&eS+2}yRj-LGmmDbN{NotQrbL-NYy+1@9>t5L_I+r^pLp5TCN}K|8 zJMuW!GV*NVD*mgVFA*>Y2z|+s<8mC%3_F{;0>t(Hc*}Mz@}is=d5Kc3XXcbvj4SCa zATw&#+r8M4W%FbLlxtGo2J5BWLZv$nPi}8l+HJIl=RB;3DbV#HH`x2qb=w|QpPc*^ z-rxINb?30C`b2g#^&=C}7wa9gFx7ECwh0K+1+ zVPbXPf+sf66^jxY!6w2J(~VH~B@i^&jFPoK8y-$(C+Br>h-k@0->f5=?|QU9w{uC1 z$7Xq)%}D36Zy`BYME!Hn@hBHv&ku?QYDZ=DbP8-kE`8*U zj>g0t>7qXE80}B{@V+M8d=YkK?l7e2Md_o^bE@nqij&tXyeLFT1%lmN_Gf`x=sUj+ zIj0-TxJX;2QtRk!A&D0z$nWaDI*}xgc!5gzh;jBJ&N0l=w%;UC%j$K+2GjzBgx-nN z3QwVX>mETd(LP_A5Tr|ze_0d@D5hr z8g3>Twn^}KMv8Fs_uF?2tGsAu^Q7C#8=3O>oZze}GG89F4d1a#4t_N3@VIy#-Sz@s z1%0A?f>mHyDTcGgOfP)o;mlm5v<;e<=sWQ$6gin|QV&uUIB&-HdD*0I^YTqCj@~&D zl1PqPp6ietq}56?`>~C><`d#x*`-Aq0AZrMZq6E}NKr(3zFJq?GoPwjbO&k_@&=O? zB8v7mQn6eWA`H|(oXwz5aU5tt#+5{u2<{H>yjVo6%(r}Bbe(O-%5_48dHTiXm-6c6 zdoE~Ot;razD+6z~o@UM0H*A$;p|9SJk^n97)Dj4ZheE=pfQ~?(7HCEH88fSFahdO* ztC+V`+jEE<|J2!k_FYyegL03Th9tvrv*AZ{mn)}pQn{SJo^glYnNWTK^?0EUz{YV& z{)Jtizsr=7$`-!8@90VZWso{$J5`@lp7_w5z74U&tiJLB!zt?ypU z?ONHvyH7BG-nX>m#3+=vf}35z#<>sAt9zap^)UWHFW3G{efG~MDfdx^FXU`LXBul? zPBL(2Wg9zrG5qNep|q0-b?xKjBw8+ekC%v=^YL}qDu`-V-$L?4DA!X@46ThYFNaRn zmsI{V$&JuvgYM+BVOh*1sNEbcmru5m_Z?;0-CWZrGl;jpo}hR<^sn!QIT+LBFQ-*z zeC3Bvr;^O}3c5$!oYY@AKWkqRB4R?1S|8G%W*4}Ait)H^xpf@=y|sb4lV!i-i=uJA zk(=Uqp*z0Cs^(5)bn%eUboi@P%Kz?j_S5c$Pl`#|R_leYnp*MtkglR{ibeK4S5^FpWzltBH_dO0WL)5yua!8nsOHkZZ}TgiqM&;qF@ev=7&4;8K|=nU z+S-bTd3`nH2GeEPG1Z;bw&%FU^AXkY^O45rSmiv&XTr9_Y~4kyYi4};m60fNNl&Kb zep}VTwf(Y|7})i4kDGmLkMqZMvUe_?k^8Mxd%(Ge=39biHbv+e zYH`uY=l)&;@_?#gOv6UX%GTT0CwIT5+pe>7@(NS*jrnC8_XpQ&nJ?C- z-@9@eG-j;cFa3~tMcWhfS1`R~IX?QM-9`z1lZL=dhrVAcg-s<&$~HxX;G0aLhx+-R0Ge%39V`iRUvsZzaub2UW_onz zL7+Q=)0f|pOieo}20qy^?(3qB%Y)@KYFa3#>MD$t3d+x`iz_Yt_Yc?|u%~y@YyA2x zbKdMTZI?9qJ@oHp%i-vC5a4UTl5i5J-6Er z{qW0YEjcVX`n=YG4nb5+Dgyo0zdF?6#QL%7@?@6TlBtsZNDY~z%n~C)HQ8XNL8T~B z(7hXXx2K}DU_!nb86-4P;&opjORZ&bizUaa?X94$6{(CEJQ=qB#rwcXnVx;~kU@!e z5R5rKG_W+AWHH!^I@v9K^2C&5nbsMS*sL&A>P)zn>3o#FhHXcFJYhYCTAm1uCULT% z5l^?~Rr60K18#!!GPWYHs)PS!e#iSZL54Pd<86!X zbHaC8)-Ua_4>0A5Orb#6UMk;PnD1a8c4l)oe;rC=bo8Ge-2BYk1#SqPPlESn@jmV5 zsP!3JEGr+`cBP+-Yjf*m(W#n0{-Iln1g~sKY|ilc8jUGA_O<-+NnozMSIhmdEC*%4y+;bCPJqPs}liWH=TMP ztxaCs-1gwYkE*^MNRNQopt=4~TGXmL1Gs7QAs^aTr{pO!>V5l*0N_QYy55o&| zCgLbB3Ysa`Tjqs_)DPMirUV5nwWjcb;+;G276OYaADy;5y zNfBEYEtrSm2CXJIklRDI1u1+D(#pzTH{U{#1<4}_Mf8BNYXgaApKLgG7Gil76JfvT zO!BAd=Q^yRiL*d)E$gQF!U8j@}Y1?e5%3r z39dwzpkH&_0z?K>RSarXz0C4=<=C#o9=};w$XYy6G3=$==%NS9^+j2eAKNte}fOQ+Fhx;AWCKyFF@IM z{IRvP{o}G!(x#H9sS|hm;i~D!@mKND&E2;3o`~%otF_0leQ-N=cd3Um1|#)law6i} zgLhRvPzBuRP0L0dG4uP6zhnmwH7XE1kHd{!Fbx{JKLM?Zj>niUql7#TDF6gjp)zH~ zs==0MB46-`893)9Q#jGZaV2@6TRN%1iZ{TkXe#oGM`(&za8tRRWvZ1DtW?%Hii*M1 z-uvVIN3c9>Qlq+hJ8t~Wf)XPx79Z`r;#}xOnG~Ebz(FUP)G`EeNaW@k`j&>n50Zr< z(UWXlv&F2ieid5T-Ko`moOaMs%RW|@L%uX=1~4nJZl70D=Z$AqSJ`Xp-4~ud&e|AK zC)sP=mm%4^geU@X>1NL{cAl06>O5%j_^XFoASn^)>i8rB>VRU;_W^@YFb&(saiL2L zZ@Z?Ayhc_ouCWaJw!UoN$H6HM=4GS}3%ChaC~WwcZYR>n4>bd<{$BWS7^W_JgGujI zM2-T)<2PBS*9DQAsrmgV>|DH_le)3<5G~@1CCqNiXTwJPIG}OY9tHxX0_fzQL`7*WWNN@}0e z1y~}<<$2j~@|$-aSYO4Duo^JmmJyM@O(3$ncl$2Bxp+0y-QL_ z3%T`f*?&_tDrsXjYMvk(l1(_&>qHuegVl{8hpZvWl_8Hz)zq5 zUDBL*o1HId&?lCz-|D7X9y4I6S{`zc+b=Lz=~llU0Ba)V zUwf`r2Hgr=tkJLa-7o);`Aym*p{jN&1w>?V7mG!MlNyM!ahpM&L3FdPSLC#_ei@}G zZ`CIO)@3TL^lF_nO~ujk2}1s`i=S1g$0aVEn-2S+V{ujFJpra1QroP4z7SU<{ZzuV z7A4_Dc}8h-X-NKby$ib}w`*N|qMZq_^$ad-yE?QMd#wbZ>CFv(w^!TFEaXLfbe;%0 ztL3Q-V=?)t)^rb#I&X7&q1fv2s6rc-g*<-dU)S$XaD7UvPZURg#4Z9u!>Xb0>8H7e z9wBvRghDZ(n``gku>1kf@A)gV^^27TaMQ;tQl5b z6?5~-;RcB`81Ft!VyWG7&490_sf3Ft6zqG072P_D>AWpzl(o@7)L`ounUB|!El-#{ zFz3*vMaW7_Yi6Pzt8cdTiY%?RuA-~aVLMvWfPL67p{)u&(sU2@W!CvA(rIh+Q3MNO zP_N?gdr@=zB4pfU!qMzGCx^89BlZx0?65Dvoi?J0Ay*;1i{I0g#jRubk5ak(0eagp zZUMUGg-(JVLWak5B5BqE94M zRM&!Ai$Gfo2?XBjm|~0kR*{`oWy7Tw=2M~R0zc;z0>1aHZU}EeE?JGg7aYkYi^{Ek zh@aA;4W-ZHgky)dmyK_BA}N-TD5vRH9YDgkIHld8NuHTV;&i7^6Z`b&|3Z8=sOav%iy9~art{ADh+FT`7oEVd{WHI(@1BejN!)E5f1mJ&VztT zVra$x(at?_?e|Z86-GdYe`x8@M^Rz_ctrNZfV_}2KAkb2((znL;f5uEEwWBg~14U*_;81sjx|qhpNd${aY8ju8P&FC}Gz2aA5v zibk^9rIAdpz0_A2YbZ@2cZ<~IT|K&Sb8Sd+cs8|NyoumV4QSLniW%x&_~lU3w8!6H z-nKPIm_w`>xl@@At_xA3uib3-AI%cOGzBInj#aUS7m1dDw zwCb@UHkg0af0?rUiHE^7Q=S>>y_TF*wH+xPNMnkDs0vtV+h*eNFu%4oQEL`DPFFRW zVpo#`CtRgELho}uH%e`YwCB;e!XZcIuN7wSNq4z{J+ocG#o z9M$%6MaQRXxF{o0O%U)9DN`D(u}tX0r6zQg+4$^7@yg!SGcDyaRUzx8i_p#F4RsCA zmJ1OqXe6FWP7*b|c>D8KV9-XYB$CNfa%E;8D=|v~zbx|#VRDw@{G1wQxd|#^NQF|& z9|RxSDf4}(it>p2u#6!@PslwIm)Wgu$_=%eJ}=zmTm3PPf0VRF{LX%b?6hkFMy3ro z7tE+~-&0!-JOUmEi_=%}o2Ta0^&WdXt``)u)=84YzAlUR{~iSq%4oYcMkw&`XWiJ# zE1g`@fcJD~JBF6@?Um@D305?!uKJYK&PL`3%ve4jVOG&Nanco&1GZsu&ABSR5pD>k zpY+Sz8g%=saVlY{cQGy`EXpJ|gLPSEfm7Q|pelGq$2hVJCaY)nmNhWeu^=o8`H~#g zzTBN3j~!KnseKu!Ue^5AqY`zhAgeGnACQ=$hrO#%Mf3>EKTuJIVgy7Jcn0n7TM_wQ zoic#u;r}zgLYSc6=Rg%S93pi03*1gG5=#2Ft-xqnfv7Kt?dL!se7BR$7fd>GXYtwJ zJ39Mi*3uoNm>xMH1)i;1koV)KDPSyTis5RiXpVZPo)NubZu}{nRq(BK-L4)zDOb8> zA*u={#27mycCVGZ*Y7l~%=t8G*^-e#4+>K&jsb!B2Lr0iPWm>uzb}X;7H%eV^$X>q zRPmX?k)X<>W8~ldn#}!W`NYC2hQaOwDU;m~UwV?-(Vk@0v2MvijA~>?IrhEoC#iuw znj7Y->rG2L2s{OF{oW~Uuq?7&BXKxZQaVvSCX;g_S(LL-RVv~#ZGW(g4Uc1GH=#FX z%fiynMv8hDRoTKB6a#NtZOz(ln2pl$2%hr7P*}U=Uz795=NE0oLS&Z|%mtRzS`5J< zxMnqQ0CIZ>ST$oM#YZ+hDON!~$U zq45?_d>%H=GwHo~P|-7oqaJ*{0AgLyd44t{2);ity_vdVDsTQnna96HL3;%a08&K>hk>hwY8M$iHc z7xrNvdi_~DEQntFcxPtuo#8M32^zbV#qV5v!ZTi%LquN~O*t<|fq=Vq*@(q;L-9wX#{7Ad%~DQd($YmEA%mm<@)&7$Hm7 zHRg0eUMz^BEvfbVE^7Pp5 zzDUj#u#Kg#A_KpMu`y$T%Qf={Z2Y6QMW}(wxQw#a^wF1kyuQiyb^)IM|B>~UQB}3? z_OPOY(jp-(NOyNANO!}gyF6UJg&P{hp*9N3x)3xc?bpO}$obx;9jQ9P*mpvF` zt##MD=bYEPuwA?OJ0G0kpC+eJsPXC@F0-PhKX?Df3vgtNbKHFCIV_ zb7>@l@9}8NVIuxjqsxLvKIqAMy)&BaoR4>WFtW>YdrNLE? z57yTWd`G=Zi%2mTj9QNyER*+&?=0D4Tyh8|=^Z>S^Up~Rh*F(C7BpSnu}aS52`G+4bDJ{Y0p`6aY&8u!hAAKkzl2<4>6$)AVtIkD%1Yso@6!X>Q} ze)6zs9^i$z(+36ztj>iid&tb3+ca4~7t5lF-jcqQuKo(x9+PB%RV%5|Nb`ee>%21c z7I2c5U6UkXi}7L1q6hx^TNdsN)b!ZiIF4-+Yr^os{cY#bna-f|$(BoG?1=RmOkl~5 z;Mi)BdY>A;_~#xH9xzxCZlG-T82QZIme`$|3zGQtat{xwTX?f=y;(=JOUGsp6oGx;C1kmLj!>Z`FF$DwDcC zdYZ2-Bv!iDYqcgMSkMJOCXfb|DUg)MfjUZj6Sy}4OGsM=CQ2f3)2n7#Lf4i5B9zJ1 zzgBl()oX(zD)zk{aZu-XuOq9&-k8lT~JGz&a zh-PKjfcnl9lfwLY8-gfA`!?smqbTIq*`1z{;O?*21ZWdNXdAid+AhT7(_J}%T?LNnq=c!J zpKycY$|H50)jja} z7iD~88s@i}p=(_BrLG5QzHuI7yhA8fw_b7`$eccKNl3=f6$y=!p`+}pBDC4#Qdf#p zCEt*IpX$q*qYk{wlOPjm;)>;;nt(_qnQBHv(4Q-=GS8w^e|6+Xb#RM*dusL^ zibDOdntZgXv3!O#V_7HzLZ4oQ`fu!1y#nK|SF>I%sRikvNeEg7Y4^}{i(aD1d$UDI zyh${JFjz0l{XhqkNj-6$u4fSc+|d8co}E;8|^)x*Wgiq`z-AIAG5Twhnk~ z)WNca?3o?GWrJg9{p)xhF1}|X37NO(&&SyhCl4Vf+1vX}O1wKULL=vu8I-!~gH4wc z+k>P&X?_pPPbnJlWiILeVlV6Qg@pEdyFa`#ove$)`eVFThsz=Ld-YI5V1~bW2Ye2E zR5B#(mQ43ePVq9`8;xU%PKyJmE<7IbTuXariq_} z1r#YuAg0BdOVW1ZpRG>|b2roNUN;Kk8K3CHystAhNQID|*P>-wtG5PE&ioz*NNewn zpV5TK@^waKGnS65^?I^WfbQ0dU7Cx7AecJFFScz3WamI;ZY4+jw zq|ob8|3r-bywbVFK3?xxv^&f*cO?7=x^gLhyr`uIYxoSyD~YrU~&*PGHx<|rU2h+@EpJ_r4D8}$rxV&-&*O+-Xo8-fo7Kw zk-nW7uP9=4GuMBsA$(^FW7gxsi`rrF$E-W5!noyAGHlJS)wq5pF3Y4sN~Av)Fu~C` zK4WvMBaV&9pB@SOgySmOx>ilI!l2W1yA!XxA36mb-e_xv74fhBtiHdMA?=Juq&-^T zc0XR*^nQBwNnPl?9ci;3AXwch?bW$H)?c&NiAcx&XZC7}&VsPXQoN6wVK+M;R)41l z;$mtgo%J#M#5y)Zo*wU;9~+=7{RUvlj#D$>+AqrEyt|+$7R^u*8f8#`iN$08rcaWS z=z>_{aUoW$$^uLc1anM+Xvx7I17MZ5haDo|NN%>P8TO{$+#TYzTjX4X?%=$Mmu4i` zZ?ciOlAr&^pLbVZbC&(zSQ zIQXJ{%8UK8EzkJQ*8-=lC2 zn5d9fthJ3!01ynk>l?Q2Ht_|^z)S3WuQB;{hMJg$xT9{puVLq;9rN6}5o}hGpXXD7 z=9v(CHRZjHbYik93WfijV1Mq`xV@4MI%^U&Z-@$T$g=-_$gjUKvdY(ca31acfOzb) z)$`E1ta5PxM72Kd2(M$la5=u;*59V4)?ed(S~$rQI&-@%bDMoJXoh}vDg4N%&>9AS2BCPC51ndW^8p~C@{9duBIDrj>ZMXV$G0yBad2>ul){R5v8!K!930)vbX8S zJBLzltLAZIPBy_O_a9IAS>9*;-ukG2Kkf&as?5w;K#PG49=CdoO9=P+@JoN`KhS^Z zyx|^S?ee>96z+8~!-2k*l!s5xb@Xxr+Gg#$-(`^}Eny)~lLO6uaOn+$e<+dAk-!mk@;Wol(c? zt=3?RZE0RG51YxDVdOAOf@$Op9&eR-MgVh0K9&T9mL8gW&`hky*u09WV8uo4!6WL} z6v|^bo&15ojPf@d>|g5w6vH_<6t_CE+$^x_d4Eir;Cn%+^ha>N2^S>r@W{L)PisdC zd~j6j0#N@Q%WcY7G!lHO=}hmaHYID#OUJCu_uIVpUt5{La0Gpgs?BPpY8Mi$fH2v= zKx&v>TT*#)nLBBcfuoa_+NXC_?Wmo;l{=k2i@)o-a&KoC9>f+C z`S?cBWtYZ7PTZqc?{|!eny=%Rbl%i_@P=83gq_?{V2pbwF^{>zn`7{)JI>M{S_gKT z_jE}&RlJ5F9XDtl54V0JTF?6Dwf6W2%Ta{FcM;wOZtqxBsf{(+WG2Ri$4MWcQ3e3B}P^2YrCh9%$=!@2O)k*jW+BQ=2@H7%sxS@>M9TW z)b@p*Szd*?Y}JVAi#Sl5-B_QcXMY9w*aR}9bJ*=^%i8<+T;=eo;Qr(1bqIYW8#|my z=ghnG9jpW6frE5GkKE_H^rm95`p6BQjMUSQ-Dp5GP}Ma5WhAbEq9rkE#NhuuiR++m4>cvVg&{iL*?=w zhy;9X(t|t?;E$cLT4_FA1X?K`2&@GCnDJ8J)_w5k1u4Wex2ah-b&SwcgPnL_)we{$ zxyo#{>kraH7o~D2pA%E~ujL%zkoV>3MR^6vj$kzPZGg{!6>2ZxiP(^&^M>kJS+dX~ zMWZ&VA;rCzQ(K=~-MAse=v(5`+SK>8AbqhaxR7ItSDcRG((dNG!>34Mehpn6I9h$w z?Rd(=xAQjmc_kBb*1mK3(A#>&{*?HUpI>55wAm|gAd6>c7y3je{J0{RLk{m>RvmxL z_CBsYafJUtdV3j~Hr$Kudbm3$%LJ6nnfu;wqPtwfCpyoRRu&ci$StuX!YiLHzji!r zhTH`4N2DflRB6_?4?(Y7uni6dihZ~1 zQ3OK%#aIU;AQU0}weu|b)}Yy7`9`!a^*W}8z9c6^G(j3dIudWmf+GXtjM_N~&cbic7JOAC;ttj?3br~4&uDzeML^N!BH#sZpMGD$qLqVQ&Z@nOhrN~-)rOr{*< z(-b(Y7)Oe{2Q@P}6Txt?8lsX!G56q~YEsDdx;zzX6Kjl@)`*UNewigvI2l&t|BRdSxGWmr6XoS(Lvw3<=R;t8;R4*A@62bcez0}pr$8A*{Zq($R z1x5(-D=5~dW7(^Yz=wo;er!<<%aD%!4{gpD3|h>;h_G2*bzgM|OF+>wy!Xd%k~^== zA6E}rJq{#<|2rg0?%ZA*t6FWS3J zH>!0b9TUu8-pNgWxrk9JNsY5Im+C7!I{e=Ld)L=NuqMv>+i{4Ze}fPsc=Ztl{KjgA zPx`A<#n6e*RV|IJ`9yKS zp*mr05*U>-$P~iQEwILDufIv^-0MltC%hYjr|kT#3Cu}hw+TV7H>y7d8SXzxH0`aC zo^A`Ln_gXDV?0BI=-j0fu_zd^F#05h&V5ZTH9S?9`zpb5zBY>n?mSQEPjLN~h-#Wb zHn#qG$-ReIgF6EKNAURh3ET&%%?`dA!z`raNR|MypNk zMCXL#-}5=DTZ^z1t$e$X(0(IA|AAbb(Dzs+8)0qrImAz)5n2E}G6MHmd}aswEQkVe zINu9q*#n8T{MEVhur0~+Xa-oNc4+#7A-8%r8s!_PJ24U6gKCw=I=Ny2ls(L1JotoB z*IDy4aSU}yqlmbwxow`47IJaT0^X?W+YlX*cC70w!Sjwevzb;*bzH;z>l~nunQkST z^?MOl6}+F)3ihrhaeQpBQ<9BAC3+1wZq<5!9AsTby4fTY{w5>V3c`z!fKL|d@o;&nd_vu=|hjCmd3V?N`*g{SA{y;D3ctVAb(9}9oD7~>Ka zOw{#$#gvoyMK!D&sXJENW4xX7>Bvo=s^gN`X*>{-NU85j&6l~a91Y3D6JMh{-Q%2} zoh?H4wTb-Ju{n;-CDi;G-^WfI>sgSM_FnnKpGE<9#;2E5gzEtgKx6O922K^PfiTUI zhBNPULUY*V9Q*e!7G`0D3ZI{wa`TVCn4c69tl{%|bXX>6L=mB(XEWC5*WnyCs@Opr$cy72wXg z@MSI-;>jl!JW}>qDV)Q}SvkTf3*yN}*gYa`Q=DX!fpSjeHBbb5X+~Y0u&GE^f^9=e z?{w{)StKu=xHa<$1z0U*mh}JL#DxR})Nv$z&*}qLZ>sygAtr`6kldtNj_7yTB{J4{ zi|_{(V{em68CQ7i_6TIOb(=!ML0%)qp$Pne0BS}lyvLpzY&wO z=J(Bkm9DY3&X0VqbHu{#hu^c<8Wm46^1PO{o1Bz(%;$Yqy_z5Sv$uh2^Xy-Oj!5wf z2LiX|t6PV?{5K`q;g(&m7(ZTvvp@An#u)6 zy=Zt=h&yzPEbC?o_VM{If0H4_`lxvpjHv25?Kt%xW~hOqfQQ>D>2C1tikA>P79*su z+uQKC!)rCXC}nwz*Xiye5o`r0Scqgf`I=Eiu{9GPTla0D85rkyP$_ZBik$lHkE0#L zpWA#3>y6fpS&bbhEMd3X(odY+Ab+5TBXipM{18ls@5BqN6`>VJhe64X)KAk=64X{- zJ`q0Mvf|RJ)wwa@CCL^s0tFYQ<|&~VnwSaB^0rtud8hF2?V3e%pd6T|qQQzhC<`XS z#lhd2O=$Y~Ad`r7vlcizQdT&}$=`EY*tvqe>@v1`A1uu(*S>#9rh2&Mf?8+NL@bMh z)>C}lN{lyf+-$Y@e$B2i-n+Wf{HK2BLGt0t;`P%!i^6bfLKqP(Iojc}X zx(DLD4t-d+_rsHhHr*RHI;;ivaaW3?pW;2IZCd-mKkDCRQcIPvxj;WZJV>kzWpR!o zqCtfMpD+dlA(704LI zu8*CtZtMYE1HphQO0UHm*mhTm0Ta1N(i*U#H`LA(;hlv*Tma>MCFFE$w@>X?z!3;& zy{g!%@Zlxw+x@b7Ss)R;)PtV z!-VXiX=c$BPO5A@-u#p!l`sKkWvKOX+V&jEkpXcg#e-frCzL?V%2bpxd*t2i;YeHQ zWHHNqO_LlL8$=7Jk#5HoXNfSgVlJw=wf8t>6E<9yT!DItmt@wafA1~&Pj5k?HBK6 zn+yX)y+BNjY`5qS_C$H=Wln1qtqTpd$OSWTBkpEn8S8)96{`PQRa;+gJfR~UH)UR) zW=X8HGC^@fLJLH0rhh3qqO>L@{(>dCd^AofsYd?rnX-SM(p7GnW*ZDDE1;?5W1zbS zAYCLnPDt!*Z!CHdVUTjEo5!Gb@y_$s0p;_Rb%Dr?0m;~ca(f3_}aO#8iNqn1W2m+pIjz7=nv3O`KIE>P?z_y)uvTOvOuCt0ydokF%A- z2Lsot-zt616D`xiA(4qOZ%Bl1leYEabJp7_v^G^ec{eUtKd%;J8WqbboB&ehc>k@N z?*~Gg?-*FJdSL^k_jfqD&$&2pjZyf%kJWfLq$^(homVq>?iW`n60TC3oEJ+(A=%zp z>c)hdmskxR8Kl0K5_Lr+>kaQbeE;c<^xc*Qyio7O3^N-$9R*Ts8FW2{-LMBDqZDea zud{H*feI}{s>C?$De;m(0n!ry*2Za-Us9BO9%7KDEiI^uHgJABNuI~zaX+n`FG{G2 zBO=+Ruga-awekVBo_~GuQ$81RB1(t8wICv|pKXBXz*5IFcP?R`yH&DlqYQqSiF)@)qZ zl@Dl=QK90)Hl$^LixWK!w+ud(!C&CQG1Ws(XLe6D`A)(jjp_)_|JbTLTye)trKEMV zQLF33KHXpH{$p`yD(><-h#?j3?+Q@RInZ6i3cubVPa!Io#L<`&do|SQ_RRCVpK9Fp z=*4_a?-ujjK%j%*o^m!_d+idhbEHu}HD~jOMW8NsXGr$_-yT^hl7GSrL#?G*g_<+Z zhK}YuB?2)w(qa`It><^iwm#u#4Dwargj3h-y6LbvMNxwIRFLbilNL@8DHGjl=dYH&H#b+3x=giIqJ~>OOfyKS5v2i z$g&*mC28Pnos`f?tiR8GA(yKDonp{}lnA8>?QC6qr0d+(j)>XqZRRxFoP70|3M0O2 zq*}^H!2F_Qn2)6tF83^i@j8|u6h2!XlxsEZm939!l$SzKcA6yUl9jIY8&syh7*97F z2x0gjq62|9loOt>-i_J;7u1slja9K%QhC^Z_s9J+^LXDm#ai93nIDi(I%eHm*6(8FhgXrWN9DSxDdPL={VjHgqD(?dqqZohzo;D4=BEW*S~e7nVcV) zgLVKfgpXPzUa4*u}ilY-X$%qV*aY4woe%qN<%TLHLe$Qu> zjxWKKPa>D2e&>WsPbo?}+5T77I)Lo8otT=5(mTz!WQIHv+pB_N_cOjPw7i)`L=ObU zTG}|RdBBM#k?O_*9;dtAky4$94QtajMR@Xe{#oEK!;oiP11J7ZmNO_B)`K4Cl+rnEIY?o)-8bS1Zl)w^2<> z(VoAgHI)SYQmcKSefwgrYKiVnE&c3IRT*TmFFywj$xomf8qRCc(Ekx*fuY?HphzH+ zPp1qdQY}@e`pY|F!P(Cnqx|>SY??mDu|_Yfim5kLYKN6C@P`lpQ4ty-w|~bH1*$j* z#CS$;+v2cRF;-Pcusy}z(|V5Qp8oRYGFAHQ@o<6Q)ey7RPCcSyFO)~EDhz_b!zCp6 zhb&Z$$CxO-yya_6?YqMiPx?%3CbCoe2`csWGi>oT}OGq0oAJ zQSS6BBp?+Htn(RMHD*{xKE#lyj4kqI`|o1laDUr2&5!!%L5uD}9s8a9QRcrWAKF92 z40VbfQH8zAuRT5@M}gj9*!qPRr&D z$E=B8RD5swxil&ha5FMyLwT}Y-Omphw%r4Pu~zLwP{m37MUsFB-V2KC6Ob?16Qo+) znu&i!nIB+ldyb%7haRIqWciQzi&J;|>@%^ds#ZKqTPM}5b_j6(3`jzZkC$?J5HgR$GRr&LUU$j~R-Get=FRTilDoUEVZQ>kS z7WypY+OaMtr*qZ7lcjj_=+V=v$fzDJ`RGpE?YVO;T&U>`bOJcy?0?~E?+L}M1OcD% z!dXAQyeXEX9YsI{bZW}Wg3Fxy=so~bdkEOv(qS(s$hx*XWB;d}S>Q)3ENMGbAhFR( zIR8Ih0AR|Kr2fv#D2UgLW2oeT&?8&ph{l1)iGTLx?lX@Peb28N+r(LaU|D+)7Ol^B z%aRPZMz((37?!-735YQNdbKGHNVY|{r(QdN;Jx0SmmW+f4-=3w8-gnj*{oen3Pjpa z;{zyTD3;CXC?WNhX}kKMSZ!Q5Zz7q1h~F#DiuNmgvBt(P%2M(#48G{=#>J6`<3Xr$ zn)j;#8`Y}qsc~aok;pNICaNpQvy;V%^Zf6f_NPsS+l`2EP@w-vyC_f*RMu3C=K6AM zS=^!s8S4MGy8)A&^&3S(8&`wlTi(Gvwj*_MYBVRD`02QB1f(GzVV1Z%44{)es-y!d zYxghw-Q|*KCg@<7VpK?v=v%{Fl5)C0pj9r)Q9O!Td@69oQ#s(_&sL7PHMR1ATm(lpIufYM&&Jz-fUyuw9N5bx|^ zTo^Q)##3(r=3$2#<`5#Z>we%Q&%2pAA>GV+gr=L8@V}A5R0-1qZo` z$7wAtKV2)^NXlmR_cq9Qdh3{m&y8_RfLQhDV_zyM-KR8y>Q^>7zX^4YBa;(%%N*R> zT4%m;eoN^iL_cK!w;E|1R{lym>8t{5yy7kjq8uKk3N@M`YP0P2SwQR{MkQK&P-dj` zki)YawAw?AoFswPOpea`W86Ib&pJIch?-6_8=JEUTF&;DfmxEoWKuwUFTdm=1y!}Q z5lp^Xp58bGAmTY7h<<`Z%d(B@@S~l7mT=Rp5<9ppZtC81O}^7fb~Gv0AZLSxDq7@K zRivc88=JD$PH$$>P$~Och$H9MP#8jk;o$$^=y6tnlqWJ{(rhx%t_BOLO>Tp3(8U8% z3s*x|b0wVj3qw6eO2YNo%>P}s$^kj~3_o>*-jOlu8j17xw)8b9$+Q#X)sxqw8)I7* z0ubsW+Kgi{S{)O_w{A=&+{Gfh6k9q-D;^Z|X`(iq34FVa%w@l@D8+~gY9bNj9IrUD zIuSS!NIiE}mPz9+zie-)9d#WR;a7DXb5Y*m$FC1N(3r`+^p#fNtmw$yXn!lb z<)ag7T~r)w^S?e-u$z>PLjXJF_KWz4|LwLI_|YgJheC3scqR@LKAf`8;{;Y&;#Y$K zzTa)h58FqgoCX}4m8@FDh!;}gyt8D1b+-)PM9L;}sjW>=F?Yyu6Msl=`|FNMoMrnlaX!cLwz00V1BoKApNQ3%=b7VE4;BunIqLm5bt2Y2=E=Qe~S#Mj$$^^&$b z3_q+4WGD97kX6KP+iGRE_j^$U$EtSdFO>@C03|FvXT}m*DcJ*PzF12};^GnuwbzJL zRLr)%KmO+g)}u(!Y3~w~_=O3VZgLFhu^+3pt&`=i?~N5N01Qm3`!s#OOu{AUGv2BB zG-VV&0svVB_K^gVnGTf#HlWtc%Xhh*d9fa zomKTp?cdSv@iA8DJ^-9~zgLW2av$o(4;1CH)v>m5O&Mk5=jerr{0c{I2fdq#8s=>q zFC)Ua5pZ{QC(rYWNuMA{Y_z>3i5!Yx92y@XEs{5iB?w&SXi8z60qpw1sTj1gKrc{O zW2pkQxw-uLCZ7JGE9_tRAuhfOK1*VGpv&)NeMDwRTbIC#I_0{S3NW2(EG3>y>q0avJF3# z%R!_gt%pbKh_yW6Sqsj>)Z&C7)y^J$IWExvb1*@?Gfy(jETH-}QB zZcaC&8>_U=ZRHHL*hZ@NR-1KXI+iZ<@6r{_uYLa}rmgm7g+B6%ECDU=^I0Hn#fryY zI@NDjB-&z~1!b)3XTuV52n>d;d3o&h!TuL@ROd+2EHpa4rDdF-O)H5$n;fE2`TQPC@p();LS(vZ zr*`Z^x`TsP3g^3)^1*UZS{yq1bC0;$uSz5(@a%^_eMth`SA|Fcpcu?@JNH|^Bv29Q$emC zyV_PYFA@1V$3l%T=##QX44lrsFHN{Z9Jp?Lnq2=n(T+UyAQCPXbJ-jX`L%?Kh*yI_ z)D&-4+gz|Z6{LwGc3Dl7K~X{Vj_MvDGnpXc)wWRA2VA=u(CJF8&po>DMZgKW@Z8q) z(1$uG@qFC-qdqk{HP^$Xcb{Tr5Glte9O7a2O+)A00u94XYD?p>6aDooIKWw^VL%@e zhb5v%SX6^}d?=Nm{LSjOD#jnc{eX$=DPhAQZVg&S%@YkRtnc;+xmIK&NYv28eOY+@ z_@iQe=aV(@ikI@i`LN_b8`?`#?sw;89}X1%M4!uu`7t$#Fyc2g4Ur7zOzqe@zm=h$@MN z`?K1)S-6D!XVVxL2Cfh?Tp(HeBV`gXzZ2pHtX{>zEdKc%OIx>uB--1}bjPr24SmDw z3lbo=8PjNx^IsF|&sRvtOBYUq6oE@0S7gO$sm(*{_SHB6kD|x0Jd9BSZA}vVIT{u7 z=P)E_P2M_44LKPpQk>o!F%?BvbY72Dm@P149R~3%zx-{`k*Q2em~s9smP$Qbg&I~p z<^tF1U4nP#0d;LXj*uw`de&Q%ZhdTCKdOnbw(ysM7?Zuc!+7WgtS#z_bOB80Zorkvit<5K{;I347>TIH7^^+K;5S3>r`97rjL%p&J~a>_wggyF=Kt)U`~+zT8sG-LOOm*#nNIV93wF<)hSZdxJ9c;7I@+o+GR*kc6=mIP?U;q>Imy? z5x^A>AV@jl0Y#+hN&;H(;b|66rEets4{M#-k%mm^gD>upb_8#nl|jeWnp-kbMJ6Op z8I*ia!4>>B>0YZO2`HI)N8VQjB9OJ)xxSJ)SpDR&MZRcJ!_Yq4TAhP~(f|y$&i`ky z$9QvT#Dy+^@WmbW@|^s@Up=tVLeZ$BZOGEQwv#w@;@Q3%M~ik4Ej}B}p_*!lD0ar< z&F^7owMZIoqN$u|qU z8@vmQ)jA+5R;(_D4=EcKn1wbyT0NO=S1}+gncIC|$VY~T^$_KktNmdI6w%=$cH3@g5skj1xr+U#F z+wT<`)z6pQXpKfzpob+bBE&z*TYjED8@u4($v^Z%k$AzT2%yQ-sW-Q5LKb^nrs?Zl zT2HE#*5JPtgZ2mJElLaiN?$&kpLA^I6;B~W`xV;AU`Q3yUSPHS{Dg~%!w_mzX)ZXT zHRf7$-O8^p?z*zKa4I0^)q@xwN=`>EsiumFnIOOW>z5pqi@j6W%&nG|;^pLdn@=A_ zD|_u~>IuXo1(1TQKjOCV3#gm$d@KtZ2#Jg&#vclOox3Sly1r(1k?DX~SW2h49ylS` zhl};gMjLbjXPvR=r421Pgg)c^~x(FJ1KYCEAx_nUFQzzVdX+JBq`h5U|Esi zFlGx(RIS${h)=)2YW?}6P0&lID7iPC*EPq##@w@ASNoBX!An3=uJ35NDlgPkYfuJi87qYuwMesqA7g(aP%IP+`=VSgWl5uAy0ONK0&u$LFE=px`5TYI?iy zI3nG!+s>OSL2i|)?F3Z$tM65MU+fnpWI7Y$T{#l(Or}-d&8stthAyKNo?eaf>bP&8 z)!BdD9Bj-KDvkcSUOIm3EJsJ3#rZQ5ciR3vxbRt|Rg>pY=IT;7{+-Vz6ynql&U=;( zpYaiMC;q>}vCtc|lHN1USf#m|<>QlRvvH}^ zOyhO4FoqBRbRYQ{h)vBiB`I&$R6dA&Ad4wClZPLfmiwD``jiioamosb^5{=)Cccz2 zM6x^emn-kA1JNV`XBt(h!F&87tcS1hUk3Ae<`JI1((lXJyqg<55vfB|PExCL0aLPQ zRZ?n;-qNRD9&-rk*QD8sAr_xG9||V0!gR;-yJ(4?D3$Q7SKWU|xaxMZ2ehnJR9ct+ zF%x>2>tW!3dY^S1`y+1_zkIi`XOx`92ddey>m~3xvVTPKtVCZfgS1<@>1Y;(Ba&;sF6pCJ%A^AEHwR<}Fz zgn~#|ozQm|-!J)n{E#R1@J9GKh?yAan!0@m!JiDS0WsRq@WE$0)8r%CvU9dL`1@i& z`G|CZVgQs!l4pCV;EyR%$0~=DknqU_x*e%tdg-g#UWQ4zMmcfxC#S6Gkt_opEm^cL zrzGJ`q($NHsT`v;R|i&j8h$fL?H^LMyg4(<4$MMRC0rJo%aFJDRFq_Bqw)SD+m_QV zYCT`YDRsgkdZQ&OBxnR~&YXe8aVczCH^#V#8${PSaQ z5VOG-;$xez*|PejI$Q)`3;1Tr_(-IG4Na=gfBm{~bc58-Gj?0uEt>xkd2BB&T)ibk zPWV~YP*N0JnXau4xh@2mITO_G;3FKkAB|(2^&Jm#ARoIfTnIXHm}l^&>4jA{E)ufB zL|o!#t5Gr+2R1G0TBGZ2=#9CvSDf%vhUIrEF?7FG4!b3a@~2+@c|KF`(tNtAl-XM^ z524~5bgiQEnITUZfD~n~g(zLaEA7sFGE~ZVi19ONKGnBpz;Q>*csHJ?xkNuC{!9Cn6#l`gMVOc{eFgPy{3KELkD1FLv4y1Dw)nZ@o=GEaWo$ zQj@e<{B<^mEV^KkW3<|gW1rQE>l6+sXdHM^I0>@S(g?+I)t|X>pfnn6N%X1hly5Zk zn1{ZjJInQLPX6sKkaJNJo5Gg%zGCp?`H0UlJdnVF+A2`?<~7;}{Xc*3SZJ=^RCD09 zP@}ntY$B4sEdPyx&F`h8c4%Y_gsSPQb>;0%u+^7Z#SmLoHWEihCfOEv9}ey>R7w3M+oVRe-`R|E106_=5p z;$yqo@xmv(-4(xC0yU}?hwAG6xqo915sdAOKEaGvvR1lzY?Xyt_#2Z_l2z?*8g?Qs zqHh~V&KjmB-U;)0upfS2ZEqp30~cARwz#)-8Y63yFT|dfNAg9 zc6f>%WM8OH#5|njB;Ss)CxmtKhS<^4nR;=ZjjT{?Oz$*d%<7Il;u%395*`-;?aspM zL={J?FB_ExuUyiVump<}8?`>zg}lmsCa@#kzt=34Qz5pF@`y}hywSu0B-~T=O1+s+ z+-3AgWMA!Q!oxGpN-(p-=QP6YUenSr^ccQz;Nx%}^?fmtv*0hKtM(3`s?c;{TU##T zx1mvr;tqz=AxbX4;O_U5rb&V#h68_1GQYa%(w~APg#=7eWg#cdJT9CB-cgU}>kLxG zn4xt6@v4KrA|J|wUey&mVLtuxUrjiaR2n&9M$ON_*_?Q_fwJv8q(CBL_W0KnRAs_hfRwOz&sbET(dhd+>{`#+E1~@$JVki z08r(lzoHfSA+t&6`0GZ5r^~j-!71`EASC7$V-EUxy-qA>JBD#YxzK=X}*rj=M_iVltv3cGldEzLe_w#%J-L)9avMxR6 z*k;2|{QP=Ly@l5(+Q-i_{4Q@J2mj_lDg0*Jb=XI;z?=C<9;eQU<3>tk(M z{Zo|tpL`{P5^U?aC_L3(?iuZTmzz0PN})dV8r9h(I$q0fh(45Pc=is$UEVOPs_%4i z2gTr)8^(}kSU#pYLr-|Y5z%Tj{>+7KLy=;Hv>e2B2QCaSz3(sc`1lVRRz(;`iU%1q zfn0E=0-HbE0;l&A-!`I&c;hApuC_}$J0NAukfP^hW679&2RtTN6jD;ncNwzaEa^(6 z54WRY1Mx@drN_)JRxw|@h85g5mzi&Tt=|_CDC|^sZXeToE@UTj2k}>C89gsO(q%o2 zu=31~?}D8|_o3O5Yna>+3g4@in_7jh)Kr z`-a5RX=9h`7$uGj#4_Pn<=>y#ku}aHM(p7W@XUl9thw(WlONYX;`L>e_f73d+sMO=orBgf-(E0NnMeW!rUGYtBYS9=xs+MX*e$V zV0`y&@IAdRcn!Eyb4SD4!@l!yGgG{!MGu;?MZ#25V0%IN#P;(^nO|m7_0&ry@1^uq zjpC0lgWudd9GW*zwGpLg&*Wwv=*W%v78hc#T?MjS`g_2w*%F1jW9@<)M z?uIJdQlYrij(RaVA>4D-fd{6jQqRZw3!+%YUPZr24CtQkV%*&l$3lgl{6|74b6*79 z71TZ_A*FD+piwWgeG)LmS`J1vb&x$tVgTE;f&ddh3M%!b4H6$DIryDMyU`l6akv^y#`!hRWXgNR z^FS?2{$%KtxvMzJvGHiBtp&ASpRK5JRXBI;T6;;`LsLA&Pe&HRJT!N-OiMqd&x1R< zsEq#4abh-`X}^j9^ojK0Y~HmvanXLZMK+?Q-rngsPalgwAeNr`l3Z7)^hm^M?bXqIvd-Y>aRe6{CR5aqqHrFp= zrH#yxG9DmHw+A2HK?+#(tF_!$@EH}1)BQVHd7f*;U{^=QG=1?RS2}eI>TJO?jFlC? z*Rbbf0naAa>fNI`k0;hBbO3^5Xb|7g*Oeuis?O)lw;npF2Tk%mw&ht?YUr>t8dJcc zvWMU?Lgr`3#=PyuH$Lo=-GA7dt`0d8h*#n3UX}3QbA3JBF`w!upNwV}Nr3OK()3)- zt0_wDIFnC8fv=>;*plZ zx<1nqz4Q5ym-wP{14Czi`AxXEkr(mEyzW5<-cn`L!U zBq~3de%fRQ*!#VkS;$7BWz2bYLP+y@ldDCSfSJG@4e8eP`t*oBIXCBEG-;>7J|7*i z!g{^rP;NV%vlF5VNz%(3f~yLhy`H~2>vTU2@V?7(WaYkA5TzX+*i8yu{CNMn#=it@ z!Y@4iD)k+UAFGo_%*N8!lEf!>?e$6DgZoatt zSCq0|Zfw^_&w?0wf}!7PQ{+}F4J;noR{!G#ugRlXCU;IirC6 zE>63tJfg3V*B(BrqOjp{S}D6*zW(?mM&kV)KTIOK2kmpM!=e7b&zJ^rsPW7QpQfg| z9PI8DkxaPO`IB3Dx>}{eP7m5+^XV-8Uc7~3Rc+wdJ87`r9O0p+3U&*saAui!e%uyn zmCv54o|ie>nc#ERwxBLj7IUXnD_sT{A*4oM`Wukwx|k~p>^)y4l~#$EFrrgfNjC*( z)e6HNVcA8gl&=Y1l9t+;i|?m}moECN%Ds`BaoDG`DF5gAR0{~mDa49+!8q%sAJfFXvejqGCQ`=3QTOK z*yPI?WWrwKCUP63;_W8xJa^;y2!e~gvd}OUqTXAcGH?~@>-SG4S!_)Lf1SS{X4JWm z?z`S_^JLxq86Ch>v@{rWZU-9`<9lV_67#+!gdZ_p?Od>R(5o=aFtEy7>~Q7cb9Q@m zD3-aOxM?e$>8}?wP5fwv--=GH1zRw68d9FimqhmPGo?@!vL~Hb0`91P|B_)IM@$l~ z&Ny+)kcc@v_5PkYM9lNUPJVZ_@k1Z|Q#TR{w=!QT@jG=0OsvBSWM;;?Dwo-_a`oZn z{$z{-`chH+ls#yn00(F~4IO5n{f&*mi!aLK;VtpRW=&fNKepa7sLrL^8buNa5+Jy*5Zv8@26uONcSvvx?(Xiv9fG^NySpyj<$Lyf z_K~`E`9)DcukN1SW6m)$rp40B8whhF)8l?`IGTJ(O|G?qtXlpoV*M^rS{#Lr8l=@F zUBolus0iBMpJ-v+eqc;@>Hg$a@cooriG6IlC+lb6h68m6PORKkiqv~$*2QHrx=hPG z6@X=c@H}*zWAkeDro3*<6pSL=g0noN9|&Ad{DNoyxyE)!$L=QAwmC2Lc3K>q5s`*r zwTS1Bv=u4F6U#io;cFwKHwn?q^~>%6k6FFd9lNMd^%Tw9`s_%h+gY*0K=+{5^)PUT z+T@$M@ts3@vD5e{39$ne6}7%CT(2pzk={hbUyMV{8J^Ee-gm9Ia5yPO#)t%TJ z4%s*u6aT{yT8b^kK+#?G8*EzOYtqY#3%2yh=XH= zRd!!YF&V(Gm5U4JsK~E(EbbuuWJ%SZT;AzdkYVG86MtFU6-=KY@foXhozm2W`7XLn zGmNs{k-eD7pg$jG6kncP4Lt6*@T5C5``PlnMaG^YJgg}1RJF7r?eE$CJzO8P#}p0Xd0IoBq{$iqYAN? z0qRpna*VWGaSHP2Tye2im&C+We1XLjcVRI#6(uElAI=bmUP-)^aQ-(rFBYTh_eHa&iir&vh~OK&3?3+uLH7! zbq2hZFQ%p%n8juKy2ZZ6lzS_~HK$qiMm5u{quo%$QeiICWMLB}JxtIoY6+$>4mWz= z54Vg_-)#CkWIkK0sE8bS4co;WI!v11s3X4JxE9i8im9~VcvO=IG25OAw^D?*Ki(Si-C}2a$;N1V|_lh^aRe;dbJdn_FFtinnA7RT@ zVW?Y0pJB1aZo#{1xt5hc0+Z>8B^x=hQ`Co$e1Dt9eRIhYsO3bxkhS;`{G>~nk09FL zFr~v3kj5t8t7Ee6&cjSi7q1=ggGRElpaW6DQd`8Tvz}W|k3jD|+y*Sxq5S579_xNb z>^Jdf+?b^}O?4O2*l_pZR}Is|x7%c`>AGR-KZ~kU_gXw0J9X#WS5tngk0bKoJeMT> zNMIQUg7I0PP^WGdQ`3SG`LR~!S?5MScF`3$+u9kR9~Zw->s70-As^h2$Mnf+T>vfv zC;g00w?Wxn9b-X^`1Q}oU)w_*s~3C5jnU`SgSR&##i;gJt_p17urPkqD~nV>sm3MLhn)D zKe5ZsU(ZtdN;14;Y*b%4DgNH8Hhk3Ih;Io|5{L-i*ds+}P;Ws~hyqL3;5zb#+jLt* zwg{@j{@K&=_AxAOc%2Y4y68naeA#VzIjY#hXa7Y*9ziIf;&Fyd#X4FIlPvu5va}-g z?1rIvhl%Fk)`@)d_SHlR4)`SMbFGA)8#Pr(TdfAE^)@iTlYY6{sr2z&DYXezZ=zCM zb%y_+;ZN~flc&r2)rP#wJA%tyJuXR+m_ts8rlJC_DXQ^y5X{__B~(+@%x6d4`jUNK zvWh^#nNcb%w{wfvg?>5q>iX5k#6C`=&g-ALpm{HE@bs+jklRqSlS$P6U!<&yt-5=4 z-sA6)VkI7s`7zUZSmHDWN0oiy->2!8ED~&thG3v4I(72rib>&{2jF<=dC|Vb$nvzY zr#uZfvB-8LMFw78|D&{uunAj4W$d z>FI)FGJ1gj)Byk3`e288@PXPM!ZcEPuRW@efmEM#xGD`Np+Yag)s7N|loS$iX0vF4 zaOV~yCEe{P0zW0iC+42>-PK;$MRR`QR~1NJ>C`xtEHM`X2C}EKE7{P;c^POWlyn&-I;un|n3{5Dvs9R{E45m3{`azB@?;cu@3qjZUryFVeOAw*D=WcA6I9QV1j~l%J_^OQ zd78z?2fkr8XzznNJzfTHNwq1r-GZaZuHT$jJ$YX(aQ@t0Jb~%c@zMNgbjNc}-D9lS z-CuWtbbAO)jlST~so=vI{UQ6fWsGf7@Yub9{Fm1aRXZpm@1zQi)B5sy^}3t7b+rT3 z0XD+dTNhb|mB(#;*CKdwcv@^kaLE-Hd%C))! zbprx<6UN@V(>fSgRrA|3ljWc9)-Yy*Q|N2xmDVefO$j8_<#(FifLECEXV4)u-S)aS zsOSE=eX~|JYcKO?E#;SN4>nV@G z`u3j9+8t7rlw_AsRYx6^GJcUNm^Mzuy%r#0W^y34h3bk9(N~NAXhA_wCd8ULBW*F| zwzTl&SBNv55d4%=Y7b|}c$AECJ9bHDOixNjnwZL#)|>snYb%2%U09OmM2)ax%H>mS zYpWT^Z6YLng?=!%h;S#Ovi+b41W=`SfJ|-d*~h;ZfiiJ1b;rdAR_Q2OIyTp2lWII3 z752UALj@+jj&l`22RIV$#i;TcUPe)?SD)hXrZoj?6G8Z`IcDxe+^#j92BHled1A6> zt!1)Sz9-Q;<75}jkPiqCZ96~MbvdYbut(7+oe?RmG%!e)X|zM64`m#t#tEw&AS$d{ z1Uv?>aG6X@F0}MCZ}hklf{2wxJYQ`fi@io6D}<;zUMX1Wg!R;oN2wG)+r0!EEn5L?&K8iKb{5 zGZDDvE3raU^GckO2UDnT;L=!C0nwc01~ag?AH75xcCOvc3L}j+qF{frNln%Br*$pY z+AsgKf}|$G_$2JC^N+bfqLsmt3AF4lRfXB+GrXV~QRVjFqUUI<^xaxgJ|P1Ywo5C+ zrRFQPWnKS`m7edm+yq>|vK4Pid5V!rkzSP>jeEUJjyKT`OTjxM_gK|ja^~JEwek0+ zV39K~sGgYwXb&%+wGq{-B$=DB$B@)?@}Rgap6eBWn!h*X3Oh(1zZcw#4krP{0|gaA-^~vgPcF)YyDpSRzDgr@g2S{N<>ktej=I@%r|HiPwl^`; zIR@|bva>%Vi)2M99?UsgUKp=M3{7t9km@r}knL#Zn%|dZiZ4?Txo+X#z`l4mTg#oM zx>hB1V3zsnebWhgSAT`b?eMt3Y3fpDbrLRX(hDRgHlIEiR&%{#QvJ3$x3 zc|q<NmM%xHbqH02E1E;e;7 z=?f}gz(Ci;; zN@`qJ>mOW+G8jx)U_QT8O}^U|RFi;OlWEF`5jDzQKnbp|g|+-^ZrX2@nVi6!-+l7| z)hR3jA?ZSLoWf!GC%IqBXa;9g~BPI;SFBI^Ly$K*4nt`;u#FcGFL)f&IsQlshGL)dN4)3opm z337l>KYJ*DPi7=ga@QF_5_@Vj_K!!e;o`ike2SM%qC&I)_b<1>>bkV7oVc2;=wBaz z7^oS1=q`=R(zqfXh0i#Rwb-=^L*|S>K=X_wUk_{MOLP1lw})wINQ|eS`Jh?@d-vMg zofT;yun}oQ-u+u0bRA)eodQR-PEg>plBpw|mZwxR2uBz##H0o*TCs0 zViW2IQ8@DY0TF2Tz7@iyd2s^bFa3gy8_2mc+6<>3sKZ7 zduOkqdvxI=1%rQ0?Qw985%4E5eaTXb9l#cQV%2%Ts#@yWW(V-qwsV-F_A?{p zEH3X`L;0GyL+qe2y>_^S3JKXP4*f4_2hA}{5r8DPx0M=fl<;GhTd1Z*vZ$UPu@^l3@;WJ_jGQ#HYeooj^6 zSaFpbzN@-Ri(2A`%6qC8yv}#*hdX;5IGDJIXNy65svjVr$)8+G%X4Gx=EYv_+iv7AGaDrz0O@?XChk|Cec-(GuyuHAQ+cj%v5($6)$$>jlEAKkc;fD%OPbQy6(kq&~H zqm27^Kv%oeRHmSekY+tm#8D- z1)BuOyeJYu+5JK0D8qd;X4lh{uW=&xL~BQ;BMk}$6G1_3NTR#*4iyWfe2F4=C9dz1 z@KwfLPz_`Ulbqr`9hS6Q!(Zu^KGwfTTP7QxQ8YWB`PAr9g1ndF;@lg1aE6Kbu-B1$ zx@Ih*rta1_nj6PUL2aWg6>&JxTi>c&vAj-$K8$uo`mNJjuIK{zfI6AE1VimMH)SWO zq2WEv02F0)BEHnZY_&Z|Vd?sTpjp+tpZkWMt@!6l;6W<1Bb<>khNlbplq=GP$}$zBs>Rto+Nw`MB?Ge>IkHAfvyTzj1w$Z-IG= z+m%t-u2@~4>7{kO%B6+oN@R$>z|B(fa*rD@5M4K#IuKLBQlVI{*zE4O-wVmjZiox6 z#P0=(W(@vJLI;M-7jw4_j!(^U4;djzi!@#nP|+}7IsVL2o^e|sT*tL+TROcg#jOmHCRk1n&H+smL#=XBi2}683sSz0(Hd@*T;jTN=wjqoK#f zTZ;~B!--8QWz79u)Boe4y}8l-2$6h!6-`9sKxW@1Ut3=~r6cy!eHh2{Q!Qm0Q`$mS zs^!M+{EpZd4u7kG{uj~nk4a2V-r&oju(;}YmzsX9o=GoCjFrMd=BGd|xpnU(+4U#n zT0vZKPEm)qj02{IIGq8nvZgqT6XJAnHBC%L>o?wE8h*)aMevl?uQeR}lzhD@-#FWL z=!4l}54Km~{g3<+C!>t2$Od+9qWxfKML(7fe;fR^Jvy1}NJyGW6`^~s^bn0wD=#Jj!3URVK#9# z2lEg2k9y}ynQ3THtNFLM(hG)>U$voSv#$T1Br2_^y|LQzaINs#Y)=mkMWiF2m=Z)* zUTRfFD~wggbCzML0|huZ(o?m@;QJ)0f?lW_qh=(&EX#qPB7Bn@C(So=Tlkj5^arT~ zwwGO_2agLZ5E!ZkqEC&E%gm>rg65FI)FnbI@1TB;FKS5c;Y zA1KEplw{g3fOkG%QSM?m&A~nminnZWCKoz0u$414F%g$jm<rW6`c*4CRop|y(M zQM<%EuOyBCfN(Iem#;{M^6BBE4U#ry=4d-i9mMRkDF5Nb+9@sh8$`WER#Jg`k6s`&XPc_B;T$o z($L9#Jy?|dYi1{~cK6s%_PS6b{$7u7ol~jt`~*iNgeMRBF*r;D@V5AVd^J8?#EC31 z@HXGF>UU}Mn-wy3krN~3=FP_-pirpRtw8Y)|ASqolnW`zsx{7v@=t}TI~HQo>f#pN zTJiDG+s8<vCE5$<50{6A&1M&d#wpWA)-5Zkkl@e)JS zgPs#<4K({c@6D43Lg}TLTqAI>R$)$wO8)>4_uapzh2_yFvkZ)p{C_{QC1zSV#H=Jr z4HFzdE&?Hue_DdtCTkD~9fYJU`~xU(Pbqzzshd1h6H*ETjEAIK znxn1fUc^E5|3?Of%SPfv{(%Qa9RH5y_cHB5Aov^JTBv8Li(4o>$Z!e!*`p9z*>aE-y&Ln93+?fl z_{Y$EsfD_VD~k9hdlw_bAwfBNMDZqbRjiuwRKBCUMCt!lJpoK2 zGWE8a;G{(t=ag_`dU#Wgfwy7aP((}?|1BFIoZ z!dLNc+}g&@VRC>^=Ht`Bq9MY{grBdk&Vi26*i(^LV!3CmGM5~jk4vZsK)wqKdrf2m zy&Fy!FobhvK~UM93qC+^D8b-jEUhL<_USDqTgwR|kmFwL4gmv!DmU>c^sgG8?A;iUpq}ND@{LSPN{GumcYSDq4T5k5K97Up_f~VN~ zA+D`x&&3+MLvZ&H6AHzM6a6LTR7@C>wZY%gs^91_jG~06&}fnYP(6ro7+p~!3$!%a zl(dzOrS~(7lW$AW!2jRfvo^lm4)|voA-u0Ru|b2XD)Y0_|LMOpyLDRo(NgE+riPiu zR%SXGO`O^+F7QbntYPpUeF|-mQZ}f;Rc^5>EurXb`N>U3fM92~=PM!|5zdH1*;NuB zg`rLMME%qh(@YnX_ z*{P_uGDEci6H&s&g_!?9d|gbw98IqJVe8-QWJmAm~hIIsDQmW_f^oiUeIu4f5wX@trjwVDGfD0`dm z%`6^xMl#t@UCPoI{wO^W#VQw5VH8a%U3`?1n#t|O$~}(tH{%7=&|K`R!lx;G@&BD| z@W<4e_LI80l^iMwwMd;#dyRsUFwqrg(N`!*F;6=euWC|>3varHMoGA@O3MBC!t&RA zKI&)wW$@CDkO~U(zR_1Bh-lsAPt=q`Ji+m|G7=JG%*>K+bSyqV1?TY2Izk!SAB8sS zANUop6Mvwx<2E_(7o7lv8+8~K9BcB582HVh6;ywnFd!NQMU~Iwj87k;e+{YpNncYd zP_lH&9$LJrK>4&(aM}hr6p2n7v~bdQax2kb#1l3a2q+ZN2Hi|V+?^hmj81zNWVW2AxC~7 z6}BuaiLn|GGszS9Jpk!5l9O9ptsb-GN@Xmek!TEyPl+reQmBevAb?bmOOc!iSZX*N z%#VGLlYnW`D7?zf59jn?_>XBS27UsZ(toxwEATryNE8sY`m3Y_)FR>j-*tjPHbo*W zLRkDPUyCUuSWwifov63U#7Ljq>m;Zd`GUMBl|J@VF-V2+Pj=cKQ)mcQyM0%eq&~)-q#TN&l2MQX9+c{ zJIJp;2Ho~=-2lWJ(AxMqG&&lKH=f()XhsE}ZCOGPuY`fhgS5@_o#NMp{Ev`ClK_jF zZ^{MScuZQktW4!RzOQ3-r-W;}o(Yn=+p{~tF?gp~Hb5>{tE_mK#4 zh~JFARJAfTJKyY5LQ+Cb`J=QVgEYcOjaa1P=A<%y>Tf~ip02?wJz~Zm;=*uIIOfXY zfptZ2@3J9pCQ+>Fm<$ku^ra?bL>iFK`q=`2H(FWU2q`gE-^-x@EHx42|B$6vM}8nm z%LIf#vWNoc&&j1QE6Mm+TtXE>2%BnQDn(WOQ@#9ZBabs?tD5{wtw6ch$kGLWNQ}(* zLA!?qFu`9V`_<)%Qhoj-*bOOG;y55cNikXE`KP-(c3}7EUjxoWKxCP$@#yvD8q?`& zZ#*x}4xjOyQ*U@KT^v@g^3(oXV6fl>*!TcLNSm^u8VbL~B5G#_FK#abv5LgXZDZuh z|D_wUUu@jawc9A+NU8pqhe#Q)QJ?&!5PQ0@7$BN*$TS)knJQxJ(KTl_tbtbyqUBh! zW(&rpj#KGOcSl_2ee3*QtI-LzMXq-^o4*%O74E0hzwja+mslH#|DK_}0Vw0vi(@Oc zr_>Vde^qhHjf9jmCO%$?=rAFvF=4J$Rk)U3Xhs95h9u}!qtN;m0&QeKErbf*CP*Y* zk(_hg92TobJUZr7v}(AP0E0C3b8)|ke19(ybw?RQp}9UbCeU1#)3LQ6AH4tVYEr`H z+(+f|9+1D%?t1avW+5PasC}AzirRnP80yILi$M z%1XC6yshx#2xc4>hwoh`%mcO|?9BUx`}&|?Z}t84>8+>T+;n6-j9K8nhg-)gE-ntC zTtl=cqI*r65}Qj3Xyx|&X=i>wz(B!{{Sted)7UtqT#v74nAqc-gnCTxn-x9z-A`fp zDg&|x8U`r@@4-$}i6ND#hF%lNhF;*HgKPo0%CkK6k+Tus45=rhQr4V|dKD$C7x!koCvMQe0FZ*v;7oh5&zXuwTFi*-iMA&A&vv^3=Dv%Iy zohcV|IesI;>YeIu#VArNIqe4%Lk8{Q<(zOJN z`^8RuioWl&Y<=5{DqE{dLxr|;Hn_3&K3F9{w!5ZIBYJZ=_q-!}YUx^J%;55VCv!Ej z+b5Q_GiP@_miaIDGxegxPM80INRtk5L5aO*bM%&LR4K; zJSKAc%<6}qe(MtZCqDQ@By{}+2hCx3P#&XSC-yQ)BWSMlY&2$`K_uC%?dtv<2Hi=z ztLAIh4*!DE_Szi}y0UT!3B$6zNP$2Z)!Trghk|Ocj#CGAcbj%QClkO z|5m~Mt6nP$xT5kO%p_e<)MA(C^&Xst7(XD&2{KSiKtc-Qyonuc_B4qTk~xj>+ShMG z8|y3mnIP2|Z+quu;iA*Nh<8_?sR~`!v&fE%%XLp0?c{-iw&q0ovg-iuyThfr4c3en zTlYBH0zL-kxF}+n@0Mm@-^8?`C**ZwWfU>4glEFG-&zAOI_-P19ua!i)1rsCLa}KgpJ&%aiShySko8 zqS8WXvVY@?h_Am^Nr4s}iOhP3m@=#(Ao@-yETKsKjf+HB*xz!&@e{poJl)?en~}RM zmkI+qLPE-1<+Pe?>;f$Db17!)SR3;WJ2rsy2S4FgET~SP71!;yglr-)J{-8nKk9xk zBw_ojJq3l_~$8u7`f{x*~gddZT<=d12#Y;C-8t6S`kVuRS-8&&(F*yfJEc zzg=+3-NueE|G^hsBz&k#8GJ(i@3GX!0j-^6Kg6ffG0rw5e-HQ?`;O#nJNzy6|^_WjYNM*jvB-W|q36a{4oEE1z>8@JN+ZcHk{7iKj>auN}?5>+f&fl>jj{+O? zspvf#^7f6nSikz4tALyYr$V$4G+IzR~d|&h9C)e8=}>;#v1dx#eo#DivN)mxswG9E0OTjYR)1x4}XP zClIRkPwFMF|3>H}EsU_B`SXyf7>*7IkqG19)Yl%Os|_VYen-b_&>9RksbtDfg7|=j z?~9ZO2T9prOvYKKV!*$oyvc+#@ja>flpJ*EQfQF+l$YxcTc=?CX19BE)No5 zw!25#X0wl0%a-iL6aJ1IKV6`Vhpt^8YYFB|c-g3E1J0N1tLo$15KOLjC{-DPc~0*a zcu(}<&J34ET~DFKPQ@#K;VQ*Z!jts@^l>ngTWVy|+%Z-SH77mIFeE=lu={$uY%dMhJ;Z1Ga+SSLmwLS*QDDhsf1Ve`u0QjK60?h3R#20N2Vx3{BOj<@(H+FKO#Vs zwc-;KQQ+gZegUQ7P~#sKzvFs2$SL4uwCkCPd+Jq*_tbr__>FhZBn*XBBqGbMBEQJQ ztg0@5EhQ5wMTZmUhxvjW0Mv&4QGzOmj=BCmYxkcWBwS378e2IUNAC{nor1we69~2d zvT5B72;!U)@0c6J#eA8v@cAGYIsLAtlq04~?a+5yvfJ5{eFh;Dm98&ACjVRC0^ z@KoA{>+>i!zMcl=x{r}4ygdUFxuyb; z(|nTmMz`I1(|}dc;_#;1k>x|vRj)sCwHEvnP4E$E*|axDe{180$op{zoh_#O0XAzt zRfpE4On9VIfo0Sa!F79_Tum#s7w(?B?=$o^KoR%P4W$OuOrs{!>2&_t>1R>Liv+DX zWzh;rs(7L(*zAW0nu2A;0kxDiIM}ZTJ~W+nPYz{1X%o&;5zm<-x0A#z3b?^iy13*w z&bV3lShy0#$1pP}<34lhc#DV^qa2F6#du-k9}Rm+SC>};zY@fysfmz;cbj`pZs)P% z!^3rH7U+O|*{m-(B3pXlK(<6er4?Q_zCXu9tqJCiKWm)g>l%H`WxS zsk0ca|A7JGpfilKGV25JzZrcH8Cx2JQ8=l52Z^ErOG56eJ5YL3>I3p?R*Xi$9w;EW z!*A9$FL7YYcgwC5<(PuOvofkK7>SqmbnzWoHPtMLOS<>vZ-xjZuiF9pQ-so;AFfl=*?cpl}&q>1U3lkiZoQ}W|8 zOUE)w_bd^5@&!G_qCZWP^-hydX7K*V&yWr5B$f)pv7U^G?+S1Wj#+CV^SVGbX;oZ0 za)wBJBDzC_BXL&Pnq0Wdq1I_^R5e8YlycuJ>$ukbF#E6Ta*y|qHn0U)=DAm>+S1Hu zKbVzY)O9PSDvXO#3QMjJ%zQ5oml6~y=pPNlhaTV3Rz_&i4eS8WFl%fg|Hc2|Pz7e6&XD+{qlNg#ZZ9!SwxoD$E{lI`@4siE z{g^dAkKZ>bXc+-6fHYDeO?i{GZnT&Vh>&i?PO47qRbxXw+b1XM_4?zgS=jWA>Dj#| z@A02v#IJ~d$sU348%nk4pu@krz@iF-h5$sZmsSk+7!!&gGDQH2QuV<{7mY74gzX32 zLglq3gLSAi=9s&OBZkG+Iz<>H{it}71$1K?0gK9-urYa!hBY1JhKJUFO~ol#*jwMcq(g}M1gJ4 z3e#9EsVwi~Y`-xO*G{DW?;auGi28OD2^h}Lb!Y$yDc^pwBWAhb378KR7|$z5mm7V2 z&{6zpxLl(QDN!o&Cs{Z;wHk8o4!1s1D~TjAM`JXVR?sD&VeNj`e^HcGRZxWrBax=* zz3F3IvA!jqT(5UCIr@qK4sa$u7Wj{%6X@K5`HV1o?EqUF`{33qQJDiNQr{( z=>lZQ`h4JI$^PeLfs1|gArJ$WA^?_JEADVcyIqtUZGvFJa&gJbZ6nRsLNufNK-`tH zEK;8bwTAxPO+z2RvSD1z4c?nr-R3(GnHkXd%_W~LD8g62FK7JNRRgr(KZL};GXCfI zs<`YJ&xmZQTgNGylF5ecR>Kv4(I&>)b98sc!nONlHHTB%&>&rTaYhwE|wJv2L z4VrlkC0rp51dSM-2D5?-Ic!7i7S*_Qw$J1V3X5fPr2!i8RmO+v-cJHjF78ah_>v~T z+~ZzmV&C1!4@i8M@8VyZ_#|M|+IMx{>=u`n#Pba*s}Rd|hI{aq z)jV!!;IHKWi7?3!;fMn4R>59b0MSy>XC$P_7FRT?;wc?Oiamt*E3f66IYS9bqwzXS z4F_K9f@p?|Puv0Zp=MSc%ALaTlh`mzFOIAwE7a<$svFm0;eEJa;s-KF3usR;#LnQaYpW3*beBmY1j%o-TNcgB%zc1HHq70XsbHn()$Rc}sJo)?7kwbl^a$!Rx|A_5P>qzo^{ zBCNKyworHVh#;^huj%?vKEH^py+bR$zz)vS^49l!M^387qF7T#d&H}t_rLM7v^bn{ zn+b)C@4<>3&vTUke@AGXyMf4=d_~hK@oEk43XU@R_k(HA?Q7S)(yRgfy`dX0e}ZVo zGbL}!)@QAsyEO1|=5v6D4fc%oj}{28R4A@;Is%AqWgL10{YcC=**O?|4$P(8nhFX& z8qxw^EgtXn$0*QkX+#y%@)tuv!hkZ+U3`L$qAc)fP#Bzb$fPX5rw#D&*o;ALtpib0 zlxr2_8AkLxH1q|zDv@hqc<6#;kS+6DjG_cxW zQI(XyQw2sgw2{fc_@2YB0-><6}a zpzQ(lhBu|EHLj<~D*wCR%T-sJ>#KRvX#s1R7~)(PE3tn5w4SBaxqA0!$wa#z!;!xy zFS1o;wiUUH*Jb*D8=AaZ{3TEYi@E?UsTmZ0Pk^Zr9Bb%ELx5o_7 z(9pg0`X0Jyh2_%F8-jRY<(jI>e=uItp8tr#;mu=*SJyd9_h3urs4d%waWXJ;CQ9R7 zSYB^~aIF1G#{>Y8ORE?6XK*13u$^=NUY)2|EUtJIP3+Gk<;QVM$I0D0?CmT~*261+en8RDpJydbqd?Fw?U zO-4aVC}s}0ie?JEU!6QmrOqiS!-gTdBCEC^uBEG71XC?sXQuJ?jGZLa77{`e%@nrz zuxGqfzRYX|$uEz8PhAEz-7SDNYmqnl(7x3D1K@BEg$VXHHgh%Kkc`Bc?-Q8Ml4@xG<$k`Etnb=3M8UizqX-?i&}#D;R~8Pxc8 zNc`fl;}!^kF@C@q#@l%?!5wuj_eI&O)0ka%oPM6Kzr=#TM`6X016)V_*w}|Kvp{FC z7CLcNu~NBxkoNqQJ38f3<_)#ZJNTLY1!c_w!TVw7W9w#vF5}-v>X(7kk-sLb6-}24 z6)!`0536xeHJmU?ut5_oK2aT;d9AIqutQAk4%b|d$8pHj9@zqj(Omvmgs9!L zJJpUOrx$JPH|0&C$g8hBUiJGEWhI(&iX2yev+SO)WmeC@@z{ABae`QMM$NExECZmq zq5iP zwJiq1ZXw-v8GC!iwM>Q#gplm|V1jOYK!L2a(N`EJYDXD2PFv3vNzPW%71!`ZyA9-r zr+b^dD9a%Ef~E9}>BbA>N;RppK(n#;tGQC!UEhI)DG;>vfc=r_#>CN?rs~~(A+6q4 zw`;}Hxcq?Qs_$#->rA8LjNw7eMU}GF{pEzi$d0EFzkWYs`gLNM>)Djz#p2ygI*T1~ z*4r$ytDWQLLB{%L*L3Gg*Yxi^*!QdyA7(87o*#7Vg+NNa`Q*7wJ!09P!)X(H^Jl(bf<0avSML$%_ATG8T2+$l zNBeaz>MP&<-Lnn{Voi^yDt83I@0v*Q6+(c;7xDwl)0TDDA-mcjCQ(ke`&<0x{TvJZ!Uh@bwk~1*T{xz7Fn5r5Z|f z{)nC#y!$A72K<|kPKk~mGRd%9&;^jo(c6oLf(Q#d`2Ptq?l0@fKxSAP7Q$AD?Z57e zX4*@Ke+$HsV8)eaDMXK@%o0Nu5GabLD63F?)G((>?6MfuV%(-2$BoD@S7q_J%Q@I>h*d^v2?yuY*B?-RD$}8iXwq;bV zyK+Gc&qP~o<%sfim)^$ExGy0Le&*?gUJQaAyP=|5O$TK=wwqix-hPTi4j1Y=oIE8!Ja|QNtPg^c zJQMk#P*6}AwVKjOz2DqS2<+IwXL2oeX($`_>TSl*yG8asB~>;Lm8*|IVJ-z7$$Y%` z*x1ckX_+p`bJCS-dK=mvw^;N&ClAD`qcn`ITg>U#d|BIHAOzs2Eat=6W%S*hU_}Y; z+%VVfsJ%uYN?`&Sc0R^_@4TtZmUU)m>#H%=Ll}8`avgsiVtMm;E1(gZcUZV`S4({J zs(Vm|v52xi@jSRXUWHzDyxQ{TsvSkzcvfMnY>%`p*GjEW4Q6Yb4O2}15YUR23J<@_ z(mVI5Wv;Z?oM%^R-17k8h{7H5?em3(G7(ZqHUc9Dr&J;C2@m6%)alLTWK3ug# z6$q=tyxn}}_2_Gp@;(aic<41p_INq$j=SQ}`0U zM_*DuHCQe4Yk|nV8c3}7&?gKA5w;gmejvmQHt+*e7sju)XV;&bLSz<+A@+u};P(Ym zFiKJW;F0*#7r12ss;AKZleuXr+>|_U&4Rv4x!108gDfnSrjQ+f4d*G%dvU=XF%Msk z@!07&mx9YLFN>=5!MEs=6?vM8#(beI=`|^YHxP6Gp`(4>b$ytvyw1=mah1xyOk-z5`NP5X!=k4hm z!iHeH>h~@`u@F?5oQCC+D*5O0ZU6ThS;io8zlzBV!>ob^S0{%?7g2uAc~@QJ$2& z7j*udj6keOw@VH#lXIQ0eVj!^c8RBddpfW(Yu|{Bx_ky&kQb8Dg^_#=n~t|>t(tam z$FCp=w+Bku4HxTI{!DM!kGMapKi_TGFFf8~b`%jHb;eP9jit&vAM3P?f#3Mnm0{Gz zWRqS4^q-o*jjBxyHLq=}UPqV>T6fsGyn3tTd~}lC7n4jj?e8Pp5d0~Rdx@9c=C?$x z&%X)XHf|a2Go0D)+m6feKUKYn5dLB7usgpu+;Pv`4h&F)aB(@JceR*xT%Ko=y>0_P zV0(MW())qITkPb7-wY43=qwq;1(Z%qcS4_=E&-&rtT^>R`giwqJJlGz3l|x#txuSQ z^JAB$x}A6}7?y-uLvl0js4|+p)dTo_@&oUmD_eFwkoG9gUmcOf4MUi&BY6|^qPPQr zH$Y4}uP*dJ!k5povk%}c}OSwyQ9oPDIv zFKtMNU0{FDtGnGfocH5Air-Q5c+jWeeKc-;OmM8Ce(yYXo3Wg%kIswK8vQcR5zL<9 zjR<*$f7yEAFg+z|Kh24C5!i!q@?I6L1Ksfcb&!0w@Y=V7Js6}kvL zbMrm1tVsSM{baM#Z=b<*93LrFr+Z~?Arz7gN#NuqkMsD#Prd4bLx#_okd*Tu@}XX@ z5-qx!wYK0mS>Aeke#@a)ceUo5Epb;Lnc$Fb2&A=seFosewVIeY>cp)9=(>ZP(sa}1 zcy8@IZUS0PjcZ>7Qo$&5#jCo;-RV%_EP?T&a`b5pM2dY6fP)usrMx#l?qApFAAiNG zsNn$o)*;P7%Djr61*a_Cq}6F@EJnIhAx= zA_g0?t!>bBlPg#aKWn}+twf>$srL_R68Vhu+t!VXg#f;7Jnz4WwtNoc)LBn#AkX6! zE$7pTrt7kU(}(}Z)mukJ^@VN2geW0`bR*I!-672kB{jfMGD8SMNFymo2+}PnodXUf z4Jr&FAq?FL2uQc2@4?^uJny$Y*77gcI&;q6cU<>%U-#bfp~~l34tO_(t{FG^*7w;T zNWBm`zs&srwNhX5P4!z;Y+LP?c)46#X(H#zd`7I2-D2ct0$)oeX_AVZp!Z!!cct+{ zZU%=D-z|wHWT9K{Kn>w2*2S?`PGSopewazsZRBtKitT0URaiPU`$^pS6ZK~zruyN@ zhV-)5uLIsRp{^3YP>D6k;cT4F+56XTC}AyV`Tm@yA?$Gp%dav)r;0%zUk!5m>g|=i zEes1u}AKn}1<4op#mqnR6 zJn`70L!-SP(OHWFow6NCqZZcpHe-h-J2xwG;J*Z3s|S%xaBhk@Iak^`9`Eh92d_=? z`y0g-Y)i-n>IP>0^;!I+F7S!*iz)RrE;WOxQjZy$j{$YyqwsT<`6e^c*x4>YHoWi4 z72Scjyk~`E@{FzT%cDmTqNINs8(x7=4 z7t)3#u0__DMBST3*F_hP6N#jd7Vvw_>yvu{kyckXZj&by1kv(h?&FOoHdr6WAxlGN z)z<`P@+}d$73$o^k!8+VHHzPZu=uNNj-KN9esHh%S^aTf8A1M5f`6qSU)aMbD8>!%J#LP5+N#5 zqj+`EfRU*{ep>mtBZFY^Q-OVeNTZs=XTd%Ui99tdJRCg|b!Lxea|_AmRWUR{l&cXq ztLAl^z~CUVH>k89eoFEvm3;Mv-0%AMWvVQ=Umsq7^}?|dDlB|kI)7iI7(kgCF}6)i zty}dEcr!#0x_-eN>~n%h%0Yx`y8Np)PL0cy}uW`m*bR*wm0_(!czj*f9gVR8GrYHqwnBPW`g8>zgGtGf|- z9@u+rAG4}Ie!;c6q*z+oePTHYv7aM))seN6nbYJLl4c;V##)mjQUNx=vn@IQSjVc5 z4b}C(mwvs&M`)eszHTTD?zkMBR%H4!%XyB*_3-VPTO0v-46x~5Wsg{#be-;fY|P%f z*-CF}SPO4Ek#n_IY_(g-v0AQ^aK7XiAUY80jur8sl>Yu)dqo6sun{!J`D*UVFMPcp zsV(@lPx?n(J`vTV^1R$w3QTg%zPRVod2WAoTtVXYHGbqrVP4zo9{!}?L_j&=6HBdx ze5-urOGzTUrZHSobzJ;A-;7+_X|3DzC%CfFB@cBmuuFNNSumO_u_`&%5 z*|c6=d(GdTj0J4O94MMCI5wQW=ey5VF@FbyYo~%eRsV%;lwjeh;-{%EXzhl0u7CdV4+BeV=WJ&Lw!&hk``*4ZNg2`Dph2#B8SJ%t#v7 z%r|e+-m$u5$FClwxiRR0xXO%Ua3YReXpgzS_ zq5BW2J}4;UZNeKLwvQb(EkBK4s?qU1{8aN}=={mSkKN>qlhAqBrV)7IDv7FEcL8ss8L&#oX7JJbaG#XmxevZ%g=GJ=wclJIjhV)|)*XzgB<}9{7SHi9CPQpEs$7>d;l9j+p9y#?gXOUjN7Gi3 zeT>(1mR{(G6t5*SKv6sQlI)fVZfa>CNbPxLdwKE_YF=hf?kw7=UsWr=-&yL5?nQXY z27i`J?Jx>e!k-{C!j0|zL)#9?{8_c)+dmh#RwdH;^-}(dDCnvWVH)t|&rtO(LkV#3 z8Y~=l3E1-(0dCSz99dFFGp5h=Y~WQ`M|$J)i9fmMGm7GX2Ufg4yJ1!A)V{HfU#FWA zo~*Y%7u30b;u&Yda@>C}&CdzwRsA%GCvM=%&p5(2Q=-1Z^A`RFLb!zS%`Fi&j~o*G z`+8DsWAhbVI1#O}D--O;Hh1j$PlxdJ=I8-7l;IWu2nKSO4=KgaCN1O9A)X>)CtV~U z`4(Kwx7xyA7_}E{P{m!6bwjf@$6-jW%wXfy&!pP7& z1$1%Fr-PXx-aoVPBW&`PVaZ+_U%-yf-gVUIUF9YW-t-i(jbVSha zIaw}0S`@jC5BS*~e{dBk@S`!r^)R}!V~r;rE2Fn((_X&iPP$@4s^z|G4h|38u0e^cpw{Y{>K>{v0|)tL6XAL+MmbI6w0 zxvS;w+7UZ7D1Rr{FNQxjh@iJhA;Q#;I5^-JiC9E|$70-~zeC<()pu2Cuwc`%PEhO5 zM>^TwG?;T{m-Zjxc5=bk2BAEu~GOxUU4GrjBV&ea7x&pH|T+gSRX-b8CMHhtMojdObD_pbq)Muj=k(nJ6 zZ=PDG8>==j^dm-cha-c6r1FN_b3NkTgM>EWL(=hZOHV?~bpmb+iOPZ({nV0d9`%sk zu@kr3G3`R@DO45Fbd;{yKa!x-|3?*JIwuuM=D6VO5S#3wXU(uEt)px2Z<6;hzax41Z<%GMz{? zWX~GOt}|x}v;<=v`5o{HLR`xy7e?PHZ6qGa{p=QOZ-45q**8FpTQwuBLgPDL7-pUV zgg7AOZ}~;M7eV4_)G4M$#6$Jo5f^8#SFojifnmg}cMn#&8)8ooLQw~TXd|_|9ZJO$ z+0w)iV!9-xGDsCV`5rHMQlHlSUb(nYg={+ui;@<@r9Y_@}C==ujzz7XJQUqsd?|Uu73WL)d$rEDb|Ur9%hvS!sU(7(kmf_ zpe^fw6A9x(HGy|5^*SMCSWk#;} z(ua9~3m&%W7CGc1i7Zb8bDK^I&Fg+Xk!|zF{lJy_U~QxMT}ExPh0>SPPe+9kspsi3 z)yu8C>7xeBI1>XgafQAIG1V3~YnYz&!?pAq1%ZoBL{p)@jSDJuH@SM9e!}OdhGe0C zfcOkWxc}w7=ZIwTn>P#7lWdZ^4xMK(lJAV!o{PNATkjNB=oKUrS8fi9&AmtMIxZBZ z4)!{|^T=zahv$=L2`FWD1KSKJ)>Z_58l-z24sE%v(5L^+xTETTMz#IwTi@$=s^8!~ zuh14h6|iGAe-*C#68H7MsPwgin;jtf_bR_{T@sNpy9#a_n7 z+LV&Vv3aYk$f?72v+9-TEY=TQHhbgZf#B&o_^<-nH zgj}gDdCeC34wuYFkW(j#SS?-;II}P11tiVA+W@21vV<#ZB)oA`(lXRcm*>l0brGL# zegBB*Lm)T==v51?Cz34OJmucg*%*psI`3lD;mh@AVaKBT^0bs+x6a@2PcD6XVtDxS zY$X~^c9E$sugeeC7@I6}E0~@(wzX4F#FYWxhrsVvx)rAKyoL}2Dtk?yJgD3F;QdR- z)w(HdzdIr*MPIh1BEE3lNA!yZk|K9_B@~MG%utc0-xk#bcav@^G7Aco9?|AJwUs+Of#URMEL!E#o`7 z)RFyrKhDvUBmIXL^YYuRj&4Vt<*mIx3d3`YpM(BsL{U@sNr(^U*gJoWuU`#Wnnj8X z&-=&ob5H}Wo-yCjO08FJxVpSAGQQLrWiG72y@yHCJO&ImtK=QU20vWF7oLhj_!iT%|prA)A-(`td# zGx5L5`+kOx8(V5f_o8KB1iD(mhS8b`&)jT5Ag8~J3})uCkinaG60vhPlHm~8&uGA@WN~BTn=8 zLK;5`cUEh*r_D6W2V9hwc?7IcvQ5h4s z+GSrxE=rND>G`OM#7Hev_dB>SMfKdYhwTMLNKF(>`SgBf)};wzv!-QN0R%H`wZ(5s zlRLB|K*=~Qq@Da|Z&OQtH<(7DwEg6NFM4}xF&=>R;P$($Sn^k^>9^-%s*N{Z4C#S+ zAJ0XzU-$)OVST(SXs-K~o%*d8 zd_BoDJTQmtFQIH}QEhbCv(> z7}!OX%(=vVb2gS!3o@{LV4#_JlIGnPh<-hR@Kv06{Y6H0PGho3vFc4%+I72gT+Z@A2M9BWtUAL52FY?99*JX{`iRo`k7G zPDUYwz>e^Px$ZDsN98w=3Yx@jFjoLztGNrlc7a`@-7mpZ7|aE8e07|C$;UACOAV%2 z{1C=3Ugj67l&@s`xEvcfzvBeed#^{x^X5dYucf#%;!s^7m()M&PNM&?23NC51#suu zW%jl7{|v6Z%@GR1=iccHo1Ox3oga_i^Ot0T7M@fTVupe)H4U+UiAmvWqJ=5YMqEn?MIZIfV zuD$Mq2^bk%>V;!ew@HK}f#tIG*!$FuQd*igu3tEO9|N4j4M_6h@`g(X+0eq~np~^% zey@g(*f3z7R0xO%Yiky*<`!D|nMk&l3@kxnV4XGF&JmU0NhiVNI#HtdbhaTn%6hVD zW-wFCOs8-5D*zUYV=GBvR*(PpI@K6u0Q}woKELilyc*_c7SiQ&^q44LSAdxQYnPFN zowd!dPikUFDV;L}*|3VEz2V=DPGY-@TsR?=JnjTi6Y72Ee{4&+1cknZ!eF{(So_PsKc*3sGoU zFHeO{q$yG{j_VloP9ooEAc3EZMRiB9i>Gm+?tqIalXh>CoZ^Z?Kl^=o=FFB56F|V) zvpCTvSMNAsz-Q_{vS?X+t#&HG=})rinm`Jx{eM@C>XRuEvY>$rV^4{V#4%}gDeqVS zMo)m4u})B!h$T>0!cGN>;7)fpd0_~)n>q)KG%)iXryo?PfnS%#m(R+~+S@n^AU=#= zt$v5l__9Sg*S{hT(1zV|d>tJ7U)845N#wMK@InB6LOwL6G>{DbeU?+_vEMz!{qe`M zX6~`Q&z{Qfo3z@#eV^0g$zemMA|9SFQ~Q3Jrl8LV<_I6PoV25=_nIicY6pyyXsX); z(1$$owFADR_4!s~RjpjTgiWx=zhv_y;eVzdQC7*MK=u(y69{Y?t$z04ZoFM(;41Ve zk6NR}u`f|jAECN|ct)tJD8ElX5!U>jKab|!B%xI$+7@m>SyW?z7_YAFEks=S zdTKLA4m*7{F?PrZ5ACQC)W)_FqJ9=nPKNlbMDyZz;%7{>{;>T=s!|a?xe7fcmSdm} z2Ds4cuR^*m3`Oni>38FI&v_^U-+0-1rGDRB5vQjFml#90l=|6(Vj;-gbKVs9kya*J z9_16Xb8>gB_czj(?~Ut=vMlI?^8|7O4|?Ns8Md&Ds{ak9+de=bRoW($qG@Y*Q5IS4 z9x_=ktdE^vLl78SBNqM?&IhRKxQ#cblb9_gHi}cjXSVyamN)AMMq!u=Mdb3Jsz}pX zh|wx;-eg-mO7x57%ZYdp3>6D06L3;dlt0#E6Zm*Mmhjy~Ru^8?gK{I~O9Rhx zLh{RmYJ?32&&T1)EY3%Stcpc+Sp$ji*M9RQ0>uGFvR~+isb6X+LU-RRh=zSxBz}2SA2vL{K&V|B{ z6jyCV&G}~*+BFs>Sh|aZ@^L{3{wxXSqEDrfl5uqQUkO+^s1@)tgo|^AtIJ;?eVVQu z>}B^%O?X*Ot5bQ6g?)DC+uP}sw)Hxjk51d$36%hGUDz}$TZ(5vJ`-Vxy0MhH<|5~@ z7=ZNlYhUV<|FMM27ZDzEJvOA%=PCY36)@7^fJeQZibL{-UFQC(gQ~#(TXKw~c+r?R z(i8zfE(EHf+;+@D%$N1vk)`NUDChCGL8UfB`#xt9JvKYUelvl;)(kpqQaQhxs`onF4il)g2)`ZvG z%~qBK3Fqk!1{7h6f=M+C_O{^>aZzc?cs0H!g~3ph!lI&mvtpIE z{|Ou&_BiiWI|(en5PvVAnhYoPSMl%~7%RS9?-V)-u`KaQrAxNF6{B4RJ?X7 zj~i@XF1DdsiR=TEnenm9yM|2_&8^vQ98o(AVUuYPP`*O$gIC*p#M7t=v@id)ujxhJTIH` zYiO%ER~nrey#^~b5oz*+7XPCKa5C@997>rNeugk~vvYL&(gF@WEViwj_99c+wYtqq zLZjN#PirkJ_4v}%uASPjYR`FN4D|>B*>`f|7l#MTSv82)(HKfDVsVs_}Y;Xrr@766h1jm0*hRgU#{k@N0Q zUNU%wLi^zFp*P15CQW5aa|FM0sTLBBYR=Vhc-xc|O-4WC&7`+^mHVZeD)ny0z|=S# zcK~UosF*_I)Z)V7E+McgiZ02Ae<<(S4SFXf5iFc{R90HYX6!<<+@<4FH6V`E)eVkS zvPT^p*@<61amElnZ*VrBk}$FN_&a&o4+N2vyx+j0-m9fDseGU@_fTv4 z=~#>b_Irm1k9!dlnRpyZZx>wK9@U<=pg0L*kVRUnfSL!Oc1pR4n6vDe`ag@YbgR@^ zg(tbi!WJ3z31wr`Ba%Ya?VjGvmC>7kzFX)1$U2c|Ep1SnanyX47|Z8hk>2AWU8Wk* zJ9+H3Iv*$6UFXnO{*=4TGgzPZ%p{>vUfT@i2hGQ+aWh3whoYk#2J^JW&SOotrnz)J zPKK8$n{Gjt;AKqY7Xk_6j;rVQiPa4we%DilqOa8XfaS~>)uTBfi=|hAew@UjOKvQ% zm=e*-c52VN>W;wvU@OsP3Q#xypBfl^*$ggLD%MW~q(I$a!q&K;tO%-^?boPGT+OY< z4Z0~%U=gYRcS(EyxYWSA>N&Cr5FIGNzrz$z z1z>ym%#!m&C~vL#Gw$$6qO00TWJpa{%Wp8>9VlE&T48L;f%L@zh6)9O^@{U4)L--dhrAtzN%qx(ri7BRtc z>9-e1-=2PtKSu-nReOwffYA5@-AXB-hN)^tTuc~g+Hsb@fzauSRDAhFock;wzx~dVi zz7Iq8m>9Fa4=+bUD-x{(t-R)^B!tpB^6?YS6Jc|WF5g;QZ~R%k;}biz-`xt@QBzvm zPivbGN1@LX?g?~;1vlHTz7Crb&Uq6rnd)JA={TGr*T?_Y_o0;;3hB@9_KUM~SY|-t zU?e>-eJwWmyLT;!mYYSZhls@a{W^zB3hw38s(e$h~9`-X!tEiHS= zL11>%Bliz<)<^`snQj&0uOr!A#6z4|1ZV`@0&zVk2KPZ3Ofx$IwRjC<(3UjUhT^%xUJOBeAj^WyZzh#p~hq@2ask zvEQ1{N)?s|&aM+rj02MD1kS1L9?(6q{>li|9l3^$BmFBP!;8}Iao#S` zO_T&f5-Z(r>ytO14&ERXfC;_e*St^UqPYE@$hom_WYs@*KR;YE4?LMLckc(No&C?T zQp~1vYiN7Z|I&_NOMAwk6q(ZYyG)V`0gH?8@<*=b=JXtS9{kWcLX{

on>7pKuTK}a}sTJL?ux?5{K;^e)P^~dEWwI$1nUxiO7Yl1+Q^3y>h zp0rF|K_httKiWw*Hl4ZW&1cY80JM80!~E-t_uU&e9gLF00+WJ&R@CZxfp(`*f0=h$ zo!A9dl>e4ih5r3Y1D)oMLT`snCtOxOY_ug(aBL!5CKC0-iuvIL)VUfQIQuYGHDsV& z%O_j{^KuxQ!zdVFw`uh-lqd?YNq}5C`$f3mFvxnK0SV0v<7&RsbZKx%D#_odDu(Q| ztSMxS@`JmsE*jmWV+4D_mrcaU7Qq^Pp%~5mF`QVOH;XxHRK$nI^swVh zR-!S4u$c$>YacEP>boRAGlX8iQS2ys)DuGt`|8)zF)+*9_wTjXLJQeXPRPZDk&$f` zGxi-(RC@&$T~YNf3b%GHWD$}>00ZdsFli)btsb6 z<8AYWUa9=jjg||Qx4RenC#>X3j5B^T-hPuOJqSrRrw?-+tA3EK#mMQDwB(lYCy8V- zBQ;l|2T!3~zW|ERX)b_?Q|A-HCk}s0xis?kOhyT`Q~6nEP3Fnona-IHbY;i@h0gw5eaT+L&zt(ow{ZJk~KJK+nMgrNb8Q*@7_AylA_!}|ew_HYS$5JhHpbNS8( zT$CC|P@G3q$$kxG4XG7pL_aK)c$gWeeoFBml^Jtd#lqAP?L{rDXHxUpDRQy0_L;Mi&o|lAy;hD;P@m2#lZEu6w4%zZU>qU0 z?~pzT)B$YUNf7y3$WLw{zUkrOs>a4N*MrV#ZlX8h5(o z_qE)5u!c%NNL~~J`aNfW3Mrr;VSTfhR9!?((I0E2)z-}stPjK=JSJCX==u9I+9~S7ArEg;E0GFk2!e>W>91&C z9Rb$Deg)6IPgTU|pkB*A@C*Kc-a^eCUfCON7D-|(&4^w$t{w(b+NF1BgsCew%Y8GD z6PIh+XGAR>UKvU}oe{xWiuRKic_oM`B5;@1t4Hlc6h6via;ib-XT`NUX+K}pj=sJ{ zv1h1ZyWP`BnNOuVH1(p-r>*Z}%IN;HJON|~cr0q%xDecHyI2zu>bjgT@6w8k(?jn~>!#fv|M@$G%pyzrSq^qS zUohOPM(8M}&0v`0SOnBYK0APxNnH69*>3TAhq#sRpcH@Ji$oFkYt*%;|A~4!l3m;& zuMqtuPEx+`1omVpB6Kq}f|ZsY?XCwgQa-tS0iMLg9F|?e5z4pu2BKPUTPO8bF5}Qf z5c0(jrJ0n#t6)GM@A8+n#vE?B5BA9aDEj zu~ys@#Dy+UKIj?L?W^Ws;j!nFmdoOP*mjuA@QIdkN$n1;Zk*%j*8Grax5n1Nw%E)o z!?(p11uZazpIm*B&B=i_H7SQD^-V4wH68J!c*E?rQ%45;+q1LNG;uKxR{~ZtuQxKu{-q0P$z*BDoxoyZ7_W zikEm>vO<5L&4=m9JId_N+>cbNcOJbD3hLrlKgE8D`tH6;1!Dh+37Q!=Xh;SMlUTA{G!u zV3XMBD_Ccr$eR6>KU!W`XFMC4HJ?YEWG*Lx^p8XMEnSqNkMy+0*}$LnZ?16M9?tw@ z+Wxa3Eo3)D33+J_AxZCGU1bcy@BNX?>DCwB3i|s26Lfn$n9BfY-Y=(-a?wcot@_rs zF7^d~AcDCy(_AD2?tw;!FHcUo#R@P|_OYW*q0<9PPJIm)`RnW_xC-n(OT+T8yA!0U z?Mi-mHanSYG!4ZW;QmQ$Am~|Yn`-vlsPEwz1LY|3>J{33;75Nx;)LhVXQdn$7G=2Q zVuWwmWfKrlo#1%d-$RhN4N#IuxE zFI|kGkBon;h@Qk9gY23MbEQb%FGUC&<>YupRFkXaMBC?#hzaKl<1wZYWPdxT{Q?12Uxd!_F z375eko9()aMfbN)v9yTK>tp2##&`dg7sXWKRGDSduutuKF2@hAvP&`~ z-;VWVaCEQm$)^P2?T5p}oEGKAA=~OFYL7YHp)ZQVgH{X2`P^=f^*P{%q+x{{U|SWk z8v*ai<1T-nZZF6rMAT>>ErnU$ZRG@(uR`UQTYO&VC(6DIst9Pd<{zxl^f2E*8eaMo zrN*lXyj)XZ+~s;FlTcy1*VTNHxV=p(P1hJ9>5}s&Be>dUmA0jBziF5JK~yZ0jmk@= zTRxP6#6h}ciya@kvUe#KRC1+w&b9WuM7^~|-51~^2uGcsPh44Aimi#>_M#HWmjgT3^*=(;PP75V7sg*opui@k}r zeq6-}57{ak#67MWr)elPx{?1pb|Z3B2=z;H0&&6v>Q_K-wqx*v$n2-aKY`bD_P+~z9`DHmp=JXy9=17(r(W=x^~aXCqH^;; z{YjpS%`90fY1ZmWOm_a%*2U#`lRM_#X^MVSP21@3UWs3gt+MAxnD4G)Pm;==WItQ9 zxs6qq{6XlrNv4F32ZxsmPGa4D1@^v3nY)jQ@f#}pwF{emYPJN1-C7^XQ@v>-5AXcj-u)+5uZU1kV9+jWn+Zj-Y zX)nY+05lZV+y8^5^!!QTqpK4$IiY7zK=FGOmb-3C0*XT4JiFL0e){pmp2?pyyhm&5 ziv>klb+a=c)MOXY7kf~$jb9Qc(b-3?Km5m?qF}prh{*pf;vT7oI7bTl|NPqF9^XtG(E8Wq#d|c@ZsGqZQj6%XpZ>1}zPsyp_+p|LFh!g2(xqOT5$iKZ^(di~#s=G2ZHL z#7u4|X#n_D$)^7D2~47j6!?_O4?B332BmRd#Te|S{}W1x{l_EwKUg(a-|@d8gKU5& z<85^_)CB4RbJ>Uxp>U@R2Q2PmR@NIiiK*~gm<<`m4{HI1qP$l@L4~P;T{r7|_pWvq zJ6{s5G~+T_HrNT9aq8A;*ONNQvhiD|NOep$diV&)b4nUXD_6%PzJUJQhTHmhsl9rC z)uM<}oGy47Efa`EKpQ!|9m1n}(OD$7i7~#|@qqlk+bB4+(aKN`VZj0sK2W^>{2!;5 zIyb+OSkxCwh`Ir^_axEx!VK&h92SOUcs+)GTS%mDzx3F({NibuUd34nHq z6Nyv^BW*_I6N$128%Ko~D9nkP&9V)yB}vI31zNn&CJ4Hs3MM)uZw6*XkhfD%y*grq z12LCfbaI@U@@#hyHQrbY)H3QRs+mqNtQD8os*hT-CAtlW%(rN#^-gIsnA%{(%joNK z;^EA86d_+LE+72~5j}TLJtc3>5qiq{m|BTrLjkHj_zKm6Dns?~uD|>vR9B5)M%R$J zL_;%03$A6XO)mx7H8=CRsRCkuz^3<8gRN(d%Dk~z!T5vFK{uTyE?UziuB*=ndk0b1 zN$ffP?z&TT!RKWkIc~K7z;G{K7zITotUCe5M2s;?5P1l>i~rWAmw`JNTY@VZ2_%cR zp9-6HTowvVBZFi#VZ8f|FHjn(TE!P#A^ZSDXx@V+&9fi_u~RK(GE04z3+Fi)I!O5= zSXYkNZ-Ygo5H$iC?;whzovCH`{zOn1I^3yyRrMflx?GUIyq$yfh7!|?DoHZgDL_UB z`35D8idwn|#m{Ul!kW^`6-wKJ%RAW%<2Vp~TrjHjja*a)`I9#jm(-?e`z6mJ~OKc{g?ef>+-!U6dn+ zC=^kKLA-{3LUo1eNp$aDZ2B_TUzSgj<|L6^Y7D-UqQly@TkP~F&bj4qev4bSb5Z7r)*vy+_9& zIn5|XA(t=pcE>Yw`>WMxx5pJY>q_ik1$^7_BSYkjFOSm&Jup{$Ll?p{Aok(NTeU9D zGM~0Rd5(lS!4iv;Sdjm)5s))*&ZACQOwTs{CPw!OI+>FGn0u8RGIlo}QNPkAp(uVF zVT$(bb-xGodUh&{HXE#2VMHDxel>@iEwn!shB{^NWs784+(4?@pSsql zmPQ}%!scov`yxq@ZsNO)yG`-Ob0WxLu-)K1cjpy$;ThrB;Gp?N96Psl=a;nP(Wzg!_(%sQS8j=G)bxRuVJ<4L8Ym+NtR zqUQ9WT;_TGz&9LKL!gDsPY7f>&qN?7a4E-5}1e$c+)k|)Z#ss`` zU?plNwxYKWw32h)DyX*w{Dn+&VM!DKovFHtf*?0B#C(kiK}Z2NnApM7NTp*KF|ajA`7_%*{GPY?%t~xth1SX zXyD2jNe1AG5`r3a==SctCb5csD88FGM~b+h;oa5TQxO5bN{G}_T)VVU4m+o<6FZQE zUL~dF40#t?Yh1AGZcXXt$)!kX^WpaiA?}Qf3V+}?S|mF>9=Iy|IbyUo0r99vY=Ag@ z%MWr$+K9W0jgUcT+2o}W9ryrtabLG`d7RdMtlTIS$9fw6DktJ1dInd1R>n)`9}X=+F4CXEXDa2@X3 z9;wceVYj;L{n=c0GI)Jfh_H;$+J00VAW@&v;#-1;Tl*TSe&wFt9i|Q99|K^ieXd9j zRE|?*uB5@HZ%91j)Zqd9NW}vqb(Ird#1|+**XV2E=>f8&4~FsoEE#;hAtT9ZK4U$) z)g~I;L|OtD5nmASOMYQ*^33q-RQCOBsN6~uERwn=>Si#l91lN(C(#>n{>9?%eNLvW z9__MnLY}C*njt~0fVsG0PL_kbzo}oQ`c_rAPur{}9kIF>x9PvE6QrvSlcEMr_Ks^& z=4dMHT=NP*$)+#lH@6<;(Wcr|6@EV+P7Zfge@Bj>;mP=nDCP0q0(A9V44J7Qg|x=y z45CwdIUrBcfAw;)7zUpUf4ah?;Bb2Wok8h$qZQUvs*}l%vvW8}&pCtHkx}XAb~1p* zo(m!LCD^yWgBcl>0j_AImZ;a=+luKg3%>;oOWW*c)6b1=R53hIf~F9#*f-uDXm7BL zo3SemSvR};N4wz4qcm&I1!a+w)O#_$Xk)s z8lV0l|41MKx=W0Hin7&A)8WeQX=xe#>@Mf>d`M4Ag>!`rgO+=v0={7(dK9)PtC7UF zmdHl?BM9y&$U9(zuXVh6tf)C|yO63@OL+T^;{FR9Cs5#&@NWht@*9w%8 zA+_O)aTn9^y@`2_n`yR9oI|4Yc~){oU;$curGIB7zzU5?D_xeEs`(C*Y?_1K`2`f6 zIU7x?2hMTt`PZy$d)zb5M20fL-|N5cmTKV^ucDZLXf|n;GS|r(xMd%^&*Sk$OWBZRK-CRv?W_CG#rJ#8_K%UvMA zYf~=4pRu^cGnfSlGEXy2l+)!;|GU2wrL5BK&-8B+@>zAvGzCT?K zm7UN`3U|G>n)fyPg_Gubco=){K(gbkav$?KHS^X#)}^$<>O_%3ER?qlh1RRjKXdR8oz(}^AT_TFA*(15kz6MIkA)) z_h3uOFW4Mm=~oy2^mE6*YnL*guO$J>~}Da~kcszENaqgZGu=T(lkK zxRvH4K3T_oPp!Y^`P^`6KQH$q(F<~{tcko@G2YUCz&&W)@4%fs4SXc(VG2&uQTvx( zy+f^x9m=0NF`Ysj3C{KVsc)K`T9H<9%}r)o>QC4lytMxvqo(mqChqLbs^`(E|GmnY zqYr*5^<=C|D@|vs(RV$a&g0SK;vGY7{BG=Q4MZBMi(X_)@uHqKbH(8Jt2lt#H>KOO z|KaheMt==knobYmRqSeK_TB9N+N>u5BB`KVWHxsniS8%jrkf2+Hpqd7-=vljJ)lrr zm0jH30uz#Aim)8dQY~r&f=&V&?%@^_!I92vf+hdAk6v~GQW`5NBo^W`gO5njk&DS& zk@M8Egv~a?kFPUG43eBM01O1{f&0N#O>&X7ZCw?wM0`Dd=&cUlRM~F!uItjlPZdQj zL*ch+!dcN}R4Blbz!*-p7*U3xChnwE+Qww~Ed-Lu0RHwl_wWQo0jJ*+Qzbfnvquni z!&4S7DoICNG~E}dbEp!J7s=xgC$Ib>_(uW)Id_|g%LvwvciAnp6b-35UaA6|2Iii` zL0P0}qbpn*)7U_kB-;R8A{jC>!rQ*sY~iF{PYPj*#qSvD7LO-nGKu>u36a~c%$wvP zn9KWR=Yx31Wesy_p}h4?dgK|&>YLC(f3n5|zX+1%m8GtM*FSGwoNA+-gQ9Gyt9+Nm z?_JV*LoQ-wKj^rJkvx7l2;D_-qQ+6eMw25Pt60STG@%VZg`}6T5a0*DdUc`$_(gqH z6*@7}|E710v3<830RdOXH5OprC`M_Q*IAoE${4nM>-%>ffM|Bhp{nR|BFe)D6qN9* z0QM0ThKQg|VaM>JT76zzw8H)5#T7CFKtqwYwj_BvxrouEf&9a<4#4NPfY&A z-Xm2dT@S72WgXG)p&SR~>khV|ATg{G@lKi+RWg6Ia8zDxG| zN#dURXPIJFCAsaPFpkCB(zA^j{vO@~vxHqu9&RE748x_zTtwFn$YL#MMngG_qwt(N zCgdg-{mionbZnP885G9RdV8JQl28qt^4nb^%$UH4%RRvATRNsT8SmR2Yf6D5B3<&_ z&N2y-xQ#(3+rJ508z`6+VjuQ;GI{>8@Jpl4+CKUU^yByt?;Oh)mNvomhxb&Wr)=V! zay-kKw*+2YlRlV}yOOG2lvzN)kU zT4jX-IYDD*mTRVQ)#TNlvrni0_2lqT_$ID#8FA&*vo#8*M|63tSuH0XV+j1so~0Gh z^Zq2TggdM#e_l!n#74E@$arEK@Tq9?%w3Xm=H^@};ZUpKtC_x0E&Z?DOBV@oIc%)E z2`z>;eALy@QKwRbsm50|>s_oAQta+vQM0$xM7~ZG-^QFdHTKx2uu@1^J&#^mwo&b< z!@jspWbr~RC@RwdV<9S}iQbC~e!FnbCf8FO`M21!kPp$)nPJ&!y?K?r=BF1FRddGD zw*+bw%bJt(yUjW5{t0oRpQP~|j2jcdsC?^;NcP1Xgf91@*`xxn56E(lI z((6tRUDHhdGD_t3BeMQWU5s-^F?7p}PMT4(q<;Z{4{BJxVB)ds=^RB*Cv@&u8O(Ll z-a)+9|A5;%dvfR-(2C%+hYsb=%}wSays_Duol3E}b8n&3-RMR#FrnX4L|L0Eu@aS) zCv@di_TV?zl-6g);-nHKDZS9Zp0bE)`#65MU+!Va`RdB)wqS992N(SmycZKM@k+yDMCa`CnI zU1$_pq^6{sMf6#D^UriK=oY3EQPQ!b;t5P6bVVkN^Ut|i+TYsN+s!nn% zHg2UyG(?wlLt@;B9M4gTcWI?*hf%TpxilOh(#qV{_U!w{A*G=zFow!6ju&N~_bg4!@@TT^p< zGQfNG$JuwD;~uahzxz|IN%HZD{@Ahz3Iu+=s`-9Ve96=zMSyi%$jcFA9b!u4VovP3 znOMnVyzGK{Qu=-0w7TGOYUKOWh{x2WN9iNWRf*NUB!MXLgZtbugJxzu6qEv>OsOO6 z&4Gb)zv>^qwcQP-Mhd1zdU?Jb9id8}`?!7Xq?Y$oemYYQj=T9#MQUFIy+@R<;S0kt zx{SOtSV)5j=B72i>Y~={)J#b$Yh+#fg2R^J#IPc(&3xZR^&>Le9<+| zgT7zE+|qb+vEC4fRDfQ^(Z_kglDt>Quub>OQNQo8hA}4^lB{v2-}=d9B(r%1FslqC zHBr=vzE#<(R{aCC+Mh@;mm!t5>aZ$OZJ%J8b^=g%BUP)P76i9_%50EfM5o(ukMAk7 zP|x2!xSl5TeeDjtbP4={O?L_6SR6uObUGLKKz~+vX6FlEd@d)i>$L$&saD1UjpB)s z{y>kbQ56b$2>N020g<1=46{l=(hD4TmKMZ%x zF9eX=fl7s=lW@WDY=-f8W%G^J4N3>?`k=N-5e-73zLk&~z4u(`%~A*J+SK#=`T$Jr zYFc*8ch+>qGq_EL(X=JAYNCSTOQ~03){7WvD~78mi0LBBbMjidm!=YAIUCa9HpO*p zw~4l8YQOiMhD!LV$dpe!RguB0Ni3vgvg@cw7vCKt*IU5HE+udjf?v;`lscF)Gq#wB zhq5GgQ1(ibtXUf!ea_JGLvfJ&t`3|g2TklVfQ~=%9XK1JqwnCMClA;@SEsah|B})! zAajqDK!RLs3a`>8#+t$g?f;Wkk9^-Sg-e4P8F@mmC9lE zgxAqyxq7Bo z;PG0;?_$t=IZ6D(s*>?iTNAbOuIomh&C4Mi*5VjG`TAwRo_=nSqb%g7tJAUTR*mac zylZ&__)Txd7n=o!4ETcpVcR^Wlg6=>FAQJNK%j%@Z#J4!wtM5HhbHH}n{+Zw%!$;N zk0;{A=GT%@EuRC*#`p#-K=lhBEnS_!6R zmiS)Tc9r6w?ZzOfBBPid{^gA@Yg0zJJD>C#?ZDs3#pt=^qe^-Y>ZGxHYT!3^q>h$d z{{`NY>Rw35p#V)Z+JQq89KdWOHEP;Nvc%h_`{#A@Ema&nx_J?+)80aYZ-2Z)Eub>b zB6DWy1daps2pY+to5z#HeQDvIG^ER;7fIu0*jdOx)EJ{4izg@Pe6{C|9P%K|-1eWpgJmYxrfxmV>-aQu=! z2O`~1=#oLO!@2F!elcY6%z=0K3RBLGbieW~tLnlXe_Do(8 zcQa81)d+;V?6ch9=K)QyLTbP^Pu0=C6GK;~+?pRs-?ig5!3|!&c5xAXKlAlDr3M*|(?Wy&^9xh& z)+9hnD_fJIoln97G>kUC1Eo&llEBxy6!9>TepQhFeN|q^F6G-;v!qM>iWGMp*4$0! zYZT5+>%9v~r`tD}#e3@42c&|2S%TD~&mVb^!(K3UOK$v*Vjh=O47)|@PhvAjp5rs6 zpXA95oUf^C0mylz#U#y%uX^GG1`ob1ljwa?W#P_leazl6SzNhF3AV-evXG8nu^DL1 zXIfhO%j55X(a}ljN!KzNC`4g+R1v0{$D#$_j7B<1O4u8qrQb`POD#%WNP3Oflh#`~ zZ_Xwslm2bR`BYKUd)RwHo&;SPA}I4Xe>l31$KZjIEtIpC#o$cJ1Yg(XWsG8uvU+a5 z7m=Xv&)H2T5J~rq-cXXY+2;~39j5LWc&wpavv`plc+1IYpyZjZj_8 zbA)a`|LFdCwiaS#pe=GGC3teqj~azDR~Ix?7nIyxG**#NtADsdC&S31{m=B~Ca_3G zh~q%`!`T$R@4b@DClkP&=t=Sn_uR+sb1*&x+00;m`%GD!b=!J>&vZXA-5`;!c;I@V za}SEanqwx(sg+;$OjaAV@6hlh4JLZrC45Hm>_VJ7iFbHcNfrlrq!RKL0t z?R>)yuvp-8`;ECc=FY~)5MOw4ue(Ni#(`eWfT4N)4Ys!y!4k*HMr!RV09k_S@0V>c z%%EGKiC;H*{qQS^xm4@67?BL|UlYZ!CyFzgXmVig!&wKgd#LfwzY3|B$0WJZvuC$z zi(*8H+MzA?MpGirv+DIkqrChtDy7e2iVZx)v- z;(ykh{qtUTTKowqWT?0`vDL!ftXN0VP$CY1*TTv04E?+%e{cgh|FKTKV;;eJu>%D< zg8^X;L59zuLj;^l7Y`daD00hKq@a03EUB9gErfC{+k5QLQtF(U#K~L1qTG<)p+uhO z)VfLyyaGEEYH(@_%F7f5{@(EkDlECl^8k5(XMlQD2@;f^GIZFi$_T|r9p-MDzhsK{ zHz3dXI5mQERly_Bi|anOYEPoMUBQI7p5mQg(j~&gK&*@FBfM8k!G`SgL2^%rZFLOPg% z^KbBTXyk*8&8G^}A8qv@+rN+HHr{?r-0o?=<|pJalm{5?ENSO=PrP21OXY|*&^k)o zV3F7G7{9QDqII)s*IJ|iNz0mJOcrQ#?XwG=E{E;Nwb`=WtJ$AFA#ez=Ak5T3xTBxJ)zf_a@2eP1gvo%__$R?gz-q{qKzC2_xXdovPY-L#%2_$btJ6u#`J)w9lawySuiaU8F^Lql47*AlSem7AkcE4?`FpH7bR?(o>i?8Inhz zgX(ePhrBRu`0e|BB$)+z+6g$`ftgJ;NTRH-$^*Rb^Ow6jmNXbJn=nsaHU(#caM*?7 z!+?`3_#tIryM+k((?5OaqRgkZ+QO{VABS2bGr>L)1?1&JGr$fCM2v7tE&VBju&`j5 z&e)`g^O4;GwDCqmn%+XRLS*dIzSXeB3r1;1`hCYRh*r>5?Dbs$Ru^gcR87c^|0*q1=JkA?7or>wKt6=rx#;pu8Sg%ypJO>Bs(YS9aX-XJ;0HSPYR{(NGNJ3!+JqyyW5@sxTD zB^w@G5>9K*V$KoF5!}DDB7!8HGe4eW4T`cD)+0B3pjO7^qFyH8Ktu8&JJsc6cj9A* z{_C;IMA0vhRWKw;VY=WurabH8j}Jpi(n`e1*JGi$HNH6Wj{SgrLOpv+Ypi}bX4@d4 z+V*9p&?eieGDHW$waNstoL@?DrhLt|St$e)^4+Zbj&^%<4{V@(?>LjLn3^1ModFhP zkaX-@LftH_)#-b7|1VGK>yTU7q3=sEr{w#^I?dlliwL4J~vgvrYr{#`a@U z*`Xbj=i!N3(3Jo`IXErclpu0?T4-^p%Nx6}c;OxNi?)V{Lr=TZU*RAfp(lZ?M!Oq; zET3X4>58eM(FVK@_-j$!RQ$uWznKqeLkAZ*3d8a&u-dHzn9Hm-+wU0FJ4r?nWXLnb}v|4*9VqC zKy6v;PmW-1UKR$30NB=#-N!*u?P-1Hkg`=tkBYhN%J281Af$r8jNac6buY-Rps0@w(NqH(P}NlrBj^e{kNx0fYXL3ClfEk1rHH9h6PArtCaXl(zZG!F zxwxVR&F_oM2At!@7Zt-1eryfn#*emsgfS~m)y%AMPCf5$h<7Q~t$lsR;S_&WBa%T4 zn*6~#55nOxAN>MmQyMSIMXKWA#cW9Pyi6lJ(yF+~;uB}k8!rj3-;>~5aHQ(7hJo_L zvePGKwM=0LnzU4xjM11jc-ydhES=P|rHjN_-D581i5DQi%` zi>G!JaxfdBK)m>;?IB;M@KYdfO7*LbAIDljg>Q$ZYkrggk4*wGg7`%*AO<{^taAbP zTZsX`eaNu}kDi9%D_=4xt;Me3|!oPpSd0h4ZJlrx}>qa^U9f#pCGm+tSgI25|5bR(ohZI z^G2%cwt2mJl6L;Clc$ABHwbHtpNcK}L;}xZjNlkQ5%*1*_g%z#udKU?8vG1AH1-#b z*D4CaTt5+C8}O<0ddn&=Gpyu3sU+((hqY-1OjKIwe_hhZn}a|MAOJ|nYQSog&$rfG z_Yg+ek^RFb`C+cXae3e1I%xMB$0N!om9F}6q+BM|PSI#L6r4e_IQcTD20;yIB%3By zA!waQ_H7Edhon5>^L~>H?OB9)rKSI|c7bwgj=T0C@Nw8-Wz)1xL16BK{yo}rJvFe@ z*p`aEBBH%TRH1?*?q=)wCbYO*#_4jqk_@koO+umC^+CXA(3#Zyh}f5@x3)2|+IaI6 zpR+VrT`wvBOQ?~t`DPGofbW7^NIq0M4zGBPw}GH1(md+jc%)>UB)yy$s4JvuJIaqh z(jb12S@J@w>|I2Kw8)Y$J-7PW7K}z4W#(@BlkB2$4_I>^O(qt=tMfo6kYvdksJB#A@}kYy`qXr5PHPD zcaJj#xPgFtsy)5j%pg^x$#nJWR@8p@8NiIc!-0F;7QuVRn0fS77A662clm-5VvK#U3G*xnT z82YM?TX~2HIoJRlPhY2AXJ?;-g?$zK^U7ZmacUX6F_o0x&Ir+#f~U&gn$=Omr!m$|vVw*!y=qmsC ze^9$|BtFas?)nh_MEPj54<+Y3tGT*!qTiX45bq&Sq4m?LTjCQ;jMlsIu|G7?TA0h+ zwk|+Ei7=sinGW81#)F7oizAWGj6VBr;RBKAkY5z-ItgDxhR|g>b8)(V*5Fvll@*dm z7pMWi7+wjL8AqEXoTLEqQqfOsr+D%AJApyJ)-VPRH@(;kYfT^Logom)rEL$dA$ao) zI)I4zSwN7uH$a@t{FRiiBz^M|dfF56%@5v1G?e~jdWL7?DIqJVE`cX$mM@lM*s;C< zVQr90x{A*&cRZi}nvS~BDjn?{!L-4n?&wG5qV^S_>?@TsN2^7#noLMjHrx__vmWJ_Aeuy1EQp6k#|@UTKhD;2mJzRp??Yuy;+t^~ zQ5`7JR^fk}dzM2*RrT!8JgV4X#8bSd8Nulc{gq=8X;RK#l-T+&O$jTB`^|B=g5jDx z1Se&UhA)$ixeD3eT*MUek|G^&;sR~VK?H~+J7CmWBVX~@S3Le%fI6s)S&p_jQ_An` zI>OZ^mQaKsaHmD1w<_b;Ite%k_cU$*q8OVgn%SB)pMc57C;P9JE8Gx~N6*qFgRF)F zQB;s{QH7tRn+1O%vPPE(;VzO-w;~P5Fnmd0bZa$qtv#VZ=?p&uNcp7u!VUlFS3SlK zstgxJvCI1$vYqhM`(Xu3)AD{`V|N8W^4JJbmyrt6c&g7)-(J&S18)pf+6lu=d+ni7 ztB;L{zkyn$oj}8>7cX3_eLvzu;}k_DZBb?ObmM$g zFbw80&Glj2^t+6z`*9QASTPgI>np@Cwy9{k_Xc4S@=K(;GaJ{}v+rdD+W9izzo + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + t + v + + + 0 + joint 1 (lead axis) + joint 2 + joint 3 + acceleration + constant + deceleration + + max v + max acc + max dec + + + + + + t + v + + + 0 + joint 1 (lead axis) + joint 2 + joint 3 + acceleration + constant + deceleration + + max v + max acc + max dec + + + + + + + lead axis + + brake + + + t + v + + + 0 + acc + constant + deceleration + + max v + max acc + max dec + + (1) + (2) + (3) + + diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp_movement_1.png b/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp_movement_1.png new file mode 100644 index 0000000000000000000000000000000000000000..7226ec5e6fff499fc02d1ee3eeb3607cbedbc34d GIT binary patch literal 100740 zcmeFZWmuG5*fu%>0+I$uhXT?eg3_RXlz_B!=SX*hq=+CbNJ@7|cc*lhv@}RJ>@@>V zeaE}^@%`Gr-Z`E}7-nYO>t5@M^E|I>&BwP=BA94|Xb=blQ%qDy76L)Vhd>ZzQIWx4 zS`xPDzz-zrS7P$0;NgO*=MSFWu@F_ZhCr~jVgKLC;7>P(K*%9tLNDbVVmGE8UE~jH zu6B;1J8G$)-GL%#%M^%O%HEY%vtZQnFDtjmRoVHa!aSyG+Mrq`np=umTB76xy-;Wo zC$p&-Mv|vvG{MFF((IJ$uXwldVLNOPYd3d{4i@CNJNKmwcAfZ0m!F1h$A+D_JmcfT zing!SY${@q0Mq*Wk< z`dyJ7a~3NRN*W)JV``j_I_E5m<{Eu2-Vwkz=9mo$1O5v>B{7yr`#n6o^rqn z!Ty({2>L^Yx7ylCp`oGSQBmpNzdxR~Y$JJ;ZyD7p7xikk`@qzPr1-pUUhihKq|^)3fQ8H*@~`h0|(k zluf@d9xVlhV03hJ47Yur-PY7ev|A+nKAVd;s?g!TK@Qh7X8TwI;|r^{YgN0Vn3c@T z^785~ol;X%jhlQiE6=v7N-P&%n@*L>SXz2&GFI>PaC4n){8+N!kXKX7X>5GO{`~n` z6E^=4v-^p}wENYCxR}_gV~}pba?X#Qfg=5`;`igMpGkRDG*3E}oJgzwG@|pDZ1FlD zu1s0?DkvyKfUQn-zZi8Nu5~@zZ{xp|>E%6&jdwdW?vCf>UL#ts zOxuJVZcP*Io=56u=H$E~dSQlfhh@t3XxeGk6J`H+A$VPN=2Wqn9X?Z*cLjeL(@$)- zKZqBzDExi)b!Y0`3H+Lx=ks_}V)j#(e!F{nRARxrhhH(8V6RTcTX<3ed$?Gn<`2=v zKRw8IpI&#SrlL9^?E3k0Bg&DO!$QFQq)Su&?b}2uiAZmr=dPzVjPD8rN=izcPFK^N z)vL4`K4@f9xEy3_M(8U~l$xs64Z&?fpqN4nKDBys>+PDOwO`KL!@Eme2fJO&>mSgW z`eN9j#a2t5@z%eb))~bHfCmh%pYANQK^NOYPFIqHhlYkI#VQXcpuj}f%zyDr+YCxN zt!1VcJ0ESi#fjK|erDJcKDfGZaGaz3Lr+<;*RhR`j&6;8gd{H`JDa;B0`6OPiT?7* z_G1^m6Bj4}Wtj~X9 z9!a>|r*>VU*X%F1Twt@D7rNN8uUn6D+%IW0^VO!Ir_UZenSOLTq1T?Z>)LV*?p|9C zq{hY^`c^@~cB&1VgbT+io-)q$a9pL#EN*RWZGQrqzj`)vRU1Ui`F%aVPJ_Ajc-|x9 z9(=iCIS^nl3j#@u`q9HB5xNf+?8xc;XrB5+oqHV`I(kG%NdNlI@v(zs0-t(UB;#PO z`=zyVWNJ}SQ9oaVa74M3vP9qFt68Fj=bEQxy&^@|NP?DTW4qI{O!S-N&#_k@v&u_3blzc zbIoS5Tx=LV3O1dtRDh1=DM(2L{@4T2m` z@XWwybs$xx!YcKNWK7D*-ZDKqy8_Yk_kAF4WJfLBU&N zV`HbIY5NlGCgch=CRbo*{=u-TL=W>FVDhBBcF} z%W>(c5>X}#t)EXD@oE!0JH;=yuP*arrPAXa=I^AYrTMDqDvx};(t=M9zZsayQ@yqz z5|z^T(VbmgFs}r0KhCn1K&43UOP6Vox&NBGl2T54($)iSeTXnxI;{L$lxBK!`J@E;bsk{i4qD|m^`e_mkl^J~&vlV5g8iGus-V8VVUef~CnV&bo;p=Y z{DY$Jw#+vsAdpWKmzHMb=1KxrqNArD^u;79)@neg>bg#Du(9s8j6J^P|FUe_?wcFf zJh4z>EWcWiZC1f_6hIQ~j`(L#e?=q=5Bz6klK81wKBfeIqf?s}QMJjve`k+vMV~lzckg z>td_8-Qb*L*w#3TXTClZR zBR}3{SB#39dOh%256716^yK3=u^cj_!Ra&WNOz}MpFLZ}g*1Lwg81WHAPA(a;6WY> z*Fo;mmVQP)4FVe~XjXHe2+OGFd%dIn_bB9nDFfRnbGo`XIv7m2l&7VowTnXiYhfVd z;;`$t+m5QIw~;!(3Fg`LH!(2@kB`p-&>~l@LKb*cVHFSL>G#sok*)g>pT2coTB3??$_2T>a+0 zUMWXX)zwMzX@pO(^!370ga@B8@NvaFHRHc` z3$xX2?%;WL%>_kC*|wtUq*+PHrmXXs8F&^lsyg$4{0pC}l0=he_+a!CKZ>~_9IUbT z!&$ExHDa~F7~dfmjZ9b|mb0M3bAqev+qYWwcfj9kz>et@YQBq1?dkC{flj)z2^Fk= zJ*yfXwk=;)^P>>%=+F%hXL~(99!1N?Cn9AS6Wg<&Tt>!U*(SGS(LBi^F*`)h?EY>! z*1Bq1AL5SytO;@&tgSqf-ZpouxjG;K4S=}J>S{I8;I=I**kX`pIn+p+m^jIO{%oF6 zX!(hds4OG9Yr~P}k@j)_-G;GvmX}t$_*B%v3N9}37)=F*PT)gYf99<3%{CW)xL$Ap zZ-y>e)kzeS_fy|}eP0ZX9s8=NTt4bxPJbpxKwt8sN=LfQ-k~kv(n&X#m$jEYvJ6*;JJ5c8dP(#Ljju8M&mOrZ&>g(%?=924iLI zQB+e)n@&aW>J-?(gCIXSy$yj}_`TzWKvu#)I+*y&+A=-+m>Ig#PfxoiO4jR&O(&O& zZP(vzPL#;|-b=S!ARQRW++LlshO#;CB7Y{ma9vtddtDAGXb=G2_8sv80pfAlga)6Z zVy=q*-#fdphNoekYy#DM3#K?c#KW9l_IhWh!m8qTM!_Gy&YoI%OUu>LA>`R(2lwkp z0Cyb-q>uXc?b~)y5OTCWdWg`P{Nbn4(%>Luw2|!EOY@=}rqs0f%(*7>h%VbyR@Ssm zme}+hQiyBQ89F57yC(R!`wi0Z_^PKI8S`x>)Ht$}O=5UpHwc45Ls=OQWT%AArI_r+ z#eTb}JFrC-7$Vn#FE&KScV;Hl*HJx8t z51Q70X8f)J47@J}g=SAv!R#0f62jH**55Ca8WVHC+ATXgY#OR>OT%tk^g8gka%$^- zYN{l_sniuJ5WD${>tF#{;r9iIWm*_gpd>8p6A%y}uc5&d+@^b-#w^rwbFX+R4_;;x z63u+kqj_eX2F9tF-V2XaGqV2(yRq+(gijFaAl&OPU+)21zL?S@Z!{HE`}R3eMX8=8 zu8aM)C97_QS05&~SmEw~&jv>eXNQ|AHBKcOZYTPi<3B{0q9g{>rAGlsPX|zTQk@hw zzW36ReJ0pL-lPvbJ%|CHx$K;XM_i=@ByXU@h>alJ38jm`a%vgR;Gb-R{ZlaX=TsM-HY zUZ!6)hVO#nGuhRbq94qZY7l|@+aTcux!nT6d`@#k03({u)hWNwa-ntAI^ir9Falm8 zI)uBQA{kW;Zv3Zy2RL_#F(1VsgMwg_fmmei=`bzfv)@ec9ph=;x_=8ps~VQ-uHD+> z77HpdpUbRUY)_XJh_;+Bwm!5E$p_WG<@#Tx5 zeKh1d5^RDXBZCQoY~EbX4joyk_e8ARu07je0&xRUPp+lR$Ie#W|A5ZOe1s%V}>62g(0O}dV#cYe! zpNU>%m6w-~9(;PpxOVWn=KJ?oEiEmqlF#(c?wlF-O zNvd8&M?cfU5Ay#lBO_zkx%{Z5bsaXk);AGhVc|DFlbxBsU@XCx-`duOvvvQvn)VTo zS>1&lE)GscImhvM8;Si?f3_kd`=WPWT!1YIlTiKOUDW8#}ET7hgy-6V9PX(&QJPc~q zcPt#8*>eIdl1ZaR(5|Uum7Nu`u-bG_y^hFGw+QbD~C7v%S6V zSye5Aa4e%Za{FKAHWmnjioe{>ObL9Gz*x|W(}NmPYAx6piK>R_nK3bj`Z1|l*^@K? zM$ytH4E~ubgAL>46aqj()f?CwAuJv+ELY#LHz2{>vApLxe+xn_XlYq^W2stNCeSm{ zvqRpLvR{>N^XvG8fY9+)T|Mn}aIMl{nrou*;APTN-7UG-t$0$(R00klW@Q~Mv>lp~{{TVVdGEzOb0iPtws(zCG4?_TyNVB|G zl9^|=(Eql<$+3nVcrsI&b3Di>`ugfLeiRKi*+C$jLJM3I>^#oeT9$%j_YYV$8>uC) zt5cw*eZcN=th8hoMZXa8#vku z03@$Mg&byOWW1G;2>~Q|X~$s$9_H|e0GRUi9R&sh?iFC<$ie8m*w}HHFIc$()i3%6 zb;x|`74~?5XLwVN!LZ}g{I<4s3ENG8m~3qo=pSi+o!W8)9}JJMO(G5ws=5|eAYQ~b zNb-H(utIiB%(5OZ&fJS1=a-79DR24;w9DFI#(64FfXqz<_bLxhFOO^Vnl1`f^39Jo zY06orr*&exv@qZqd|9iO=O`{2r*(}4Adm!qejl)Hu2y^9FaF{s2=F!IFE;GOE^&1Z zDSc0ns$Nw~+vGoLoPP1@nE`Ql+nz5P+7@B)9KwGYjP`A?0e#d_>UPmL-hg2cxg;mp zL2VoeaGq>C8?hGOUuk5T?fJ~E&3Dczdc@FCpb7^{Qt%`0z*3>xhNee!Q;kA zW_Gs9ojZcSn!_g5zi(`OTC|{?xW*R(fGfE?%+Z2bC8Yk&%37A3>E@n@;BBJCqcVWn zt9`L*l+Elv0stMUUGVG?{C)l;H#)ojTjw*k;i2#jHVKf*iWs`G_xCdi+1I#XyA;a$ z(pUR07$Za1<6as>KBf1oF3U%GVI!Y|sJQ-Ivcykj|Zaj?anxe?c%5Ge>r{T%7OK^^vcf?=$Pi{63BV$i zvSQT0KtOmxoeQh9xyw(kLfygw#Dx*?NgKP&)WS4Kb?YD-) zY2}XB=^@n9Ad%nPTD=t9aG?_=(2u32+>++CoMI6X{ED&zM*muKO*X4Vokn2w|57CY z*)RBT>#nXZHd$U)7Twa)5{#P4rAw7mQG(k@^uX4Ec#GuMXa3$R$*y$^gTbT6*L`T;8`rV;}(R%jL99aMLutxy3eFa)57+8MCmJQq^3&`f&>Ae z`}gqhJMSM{YqWjRH>3+01jeC>(_j6$*y6)O`)L>yQp(Cd*vzKf0ftu8nMeJ5^Kf9F z@ZcK<^`<9pzG~g54SeWbyV-@7*>Cm;U~ysMk~aOBB*^sn;9Dho(L&lW2L!(tz8KVoJksj@N(PU?nj-|g&x znEHv`Y^oQ;)xy{w%Bn6Qe*X$LZDm=xFnJ}sa#NGgm~UQ3{Feu9+xB+i8u>%RxZ}B_ z(4#<4IfKj;{I!RW(|Z?$>;H@9-Da=jSbE?h9iH)~z#S>!ncCzy`1rd5x!b=d-!D%ZyPR3}_TN0-a{7~)S2c&s7{{-Lota7%NJ#pNC;-oWG9{*H9X?-FHi}4FitWUAb z0G-(S{rf4!Y_z=U;?kqv*GwGMn5J2^KZHk7Q$5N0UaQg7*>!M?tJEGB&(zW;VQE>p z9HA5*8lFCmw4|rj1+N*BaS(GD{n(DY{7X{1oCTo-g4A}v4Eky`P9v5e+u4h5+RqpOu3ZA+IzTsgMOTUqW zs;g%&wkW96sf7~v_q8&}A2q~Rz@WiO!}8<$kGik~4U$!ziRpzvn(`BXL39_0LNWC9 z$gItYXKfy&({}4jj3a`*xU>@Fl5XdKx{9!ollw>wY3^3Sj1l_frm-~*&vM4^GY|D zG7O zlwrWMz7B-TtnSZQYLj0u8R-gN5b>RwC(O)u@6*tHVXC0CPG>FrvEf?S%kyPF3?FM)y9ixz)+rHDo3 zdx?=zX@0vz1}+^#LL^*unVAS1F-YlZ2b;y zaY+rcAB6%ZrxK7`T7W)B!^a2YsD3gPRWDT>;wt@yLRCECsznPI7cJ)x9}qGN-hzr) zed+DCJb^DE+A}V&;UM*5!u87i?B zTkc$<8yPFarsF=-;f&j`BpY_&%goBMx0oHvmwdJ#^GNW6wx?Lwt1`y+UTmH%3zxK_ z{{h&ng!!Q3nM_yO{&gjk{IZwq>_l+r?y3Q_`Owf-8s=@MifO9OpRtdcEC618JlyxE zW6ra)vwq~ZhWVfK;$kfatu^^o%}XkqCC?Cw)aGV4=5PPSbk}KB8Ip>}|0c5<>f zJG=hhIwCD&ck=mpDh#p4oN~f<__(~MM?hdUtP%0-Q0(*PhP}9+*^%48nJbooBcq|E z{Cr~!sUGbtxb#vk?IaLb@#oK~l$;>F*0u&+d;}y$T%4|;s=1-N zC}nMJA)l|$wJwX!LgPggJwEC)oakM-(i&laGTqPt# zq>~d*3ijdJ6WHEEJYIgfMw`u$x;hC=%>2U~`M&-_O-svhtBO2%?KxKvcHT%)lZhwT zXygM)Xh(@`<+8H$xkIhCXpz>sb+lD`LvOM`6h5w^jfhxyk2o^bmlknB*2G92!!Fc3SIC3~? zXh@9ksZI!t+etHrHfYv~q}71(c^%Al&lnkDk@JJ6=TqGjbSBh1J`g=-pHno|o(TQl z7oh6fic8&S1{op%UntzM6_Yji?I@x|_ceWSvGBPP2jt{JR$+L@3vs$kB*@${4{;7qGrmQu=c~G^F)bs}pCnlio`c4@Z5Afg0;piT-kLXF?H5vO>Kq(t=fDTU1> zh$gi35<2VG6+@3+kkw#U7;9cPRW#j9fFbN$S1Aqin)?sX!@r$T!PMGl$6Deb{4>LE z`eN+t3ALy74PKsml}n3AO^A3JW6nfgNrI{6_-CB(ksufX=g0t4?~4KDXe3wF@ERAl zP_JL_E#Rr{A+S=w!5cv9Y(cOPm-zOWBIatKwuBv@@c7vD=%TcA_T=NohWdK%I|Hdv zz<+o55GRSq4gOtq{hLmH!ecJIcHethxu#_>(s2?iTK#TAu)??KW1ubRva-(gRs){rCTkVp8q1x z?GiW=gOHoR)XkrgFQ!w{6Wml_O-lgTrw?Z&-q*;lK zCB{H|6GPb@PeMovGFU7k2BdI;84~Mq6YZ}nYEShhOHEHNw%zj+&KIdG7lODc#a>KV zc){9NV9h0azq_9+p^`t$QLl=0PeDYbS9B5v%4u3!F04-4+FGC6JDTF{JqIhNZH%Er z#PLc<8LoZg{|KN3&oL~?iD%|9In*{i!X$c#fsTXggN*X}b11-&EPQ9!5RThAshW@# z_kZSu(N)Ay^$B8O#h_j(kIlRP1vDu}L_VCFf%T)D_Sn}ZgFctf_)ou&+JY)DD4okX zI#%$I_GU5=fRb+gpEK^^sq@2rmY3vj`+n6*gyN7Tqz4ABcD8>7Y8`!e-Zer?w)y#>gnG`L?oLjO-vLZ-cC<|n;@hx7$)bnp&<)koPeID zyg?~$MoKkuG0@Ml?s)D1Q|vq%T8kKfB8aIRmzbIPDsuQyljvKgN)e{fDNvk|9mx3Q ziO?1LMh=*!#l|FFwfn~C=4J!*{{0X1n1Mvp-7&bZ9f}$ll-|U?wi1Z{T>x!;iMeb< zL3iOgJKy^;7SICag@Qft78hKQ*QNGhNr`L9;bDFMNGbPX?aa($F_4Sp0qtK}Qcn*o z&Jx>ImX#HGsVzQCl)APi@O8HR-fF$4mVaV0#j*F!4)C>s_7KnH3>t?zZhXFLT8AOzRRq9W355v>9*{FCc-2E zah19!4k}W)nygvgq8$nXqG19FDy9tz>gym%p6h9Nd=;sO_&@6C=vd=8%wGvI;jd&c z2R$#DOtfp~7vp&HXIpW*oIrCzz<7s!sa;oV+bs%rmpF@ji(`FCyLQpFFQHs|ly;q> zYvK2~^(h_rPq^yzhN#*zK1Ma!bQ-tPb`U@IBVY#qB{$GEu?|J!qzB<-J{=DYdB{w` zts28B)!7qSRD@7QR+kV@s%(*%n!3GrJ5j%EI!(XsJoT^s1fa%`8NLz^kc~Yz!-eP& zz;+LV==nzZBspvoL)l!8|47%k0T&0dwo3au&l`s=bmt!wQgbv$b3CTP$b{%{-paZH<_ zz5*fV6V<3#?i9oK_WD2EuBP}HE45=;2j-ny-H+a<-hV(`xmGlbLNi=Ka(b|BzwVp; z+IX=qJbq_clPhJd5^H*&IoY|Di_7(NRhKMdao}DI2UD`)`S6SEgG1*76_qoY~@TRz&1F{V_wz=0z=FI)}Hz%L?#-3fdm8triWWs7<`;NyC7KXzgPLc7bEZ?Kg zN7>VYNf?HKU8t+L_j;Q`HC&T5%(sn3=Q*FHj_47Xidhv?t~N-z_xm`8*=kHE2SBAl zf3S<0k8H^rUr1EWoEGNfAt{Ge5)XLoU>l2`I>afnh#UZ_TU*?9mnD4V>@ps?5e zX1F|MO|9eV8+FQ|-D)Gc7r(!tUy8ZAYURO5{Rr}Oyc<`6WaDVAwjTY}hP3vCkCzmeMbjs=`{ z#f(>FX`emgxNLqPD;rv@_0nSFkM?Ta)i)qJc=84}zRG204!(3G{^STshW-3|Gz>$h zCD3GdQrY+Lsv*Xrm6vQ_7qvv|b&iHNa*r1zRpMJ^ip&aRz1 zClMpY9girEc^58BNPi_&CUs|#P9zFeuycKJ7IH4aHqo4T$!^D;hPPS$RAeX)Mjv#ls1E;czQg`uVJ)qOAx4}1`g{U83-9HrBKezJEk>xI1r z+Ur2C_P+hqAk#?l1VF_o)U9(n>oy9Y44Qsn3iexC}RAWE_a+9S|YA9aLs7 z;cRzes~4qoKxRu|duYVwpe$Hm5zlDxvJ7kuyereMifx$M%OI9}&Q2yFg)Lvqq^ z8yd+`POhxvl8on3hV`cR@E)mwR>r#&&&_`k7x@uDh+VNM%lATKRcTwwXx9CJL@+ci81GQCU>RwgO5EzOR2wwabCAe^+YyV#O`xyS@;jT-I`rYzNx-) z!2J%ChK|WV9jyM&=`z2UwwIDd2%r%^pCM{{RamF~=#7l+sjnbWATe?JL2L0Id&(}_ z6Z_zS$P?sODAOV`mGeIqq{n%H4Z1pC?0BKJDL+b@o+Pz$jir#PpslF+fZ~|xSc7n9 z3#V+Jcw+?4_X@D#ZA^ffU5bFCdlvWE*hGn5AU7iS@{^5eG;Bs1^hV}Y%T~^#(K0eR z)PjoK%p~c34x@=*l$vr{s$Pqh9lY7ZDPR>Ql#K%eYmb}Xsv&H&RD9ho(JfcKH*}j34oJKZVN>Y2oRNZ&IsuIKpW%76To%VJ zpiJjk&~SP%VD9!{Onfd>A|*Qy4GC*^7lSHTN^y5ZZ%IAw=xV%bZzW^x>}nyOJIu6* zno|2-6V^*4s#_7lcZkL02($#~sK3qm zlEH|I`uLLo>aAB#(3K6gwu8~{2qHu`Q`)mS^7T|u&-K(;&m<7IP+Mv4ors3MNyscR z?>sl)qWMW1zWqhoO0@qXIk&IjF4WlP>!8kJpjGJ_2WHb-7rp9viiP2H!35-B=6t%* z_<<&EZzlucG39DG$Ev;JbiH6KVy?H#=Xy%Jcf3$VdPDExJ{-O;cpb*-wP7_>f#jcG zp9%)^on`_@`+gl5rrEYGA%)px8Ln7_e^9@$r$7>?qs#IToC- zd&yq|@og(B!zeqHHs!`g_1QhW=7(G8-n}LN(HVZvfW|=U&E^{v!Fe2OlR)A*EFzKZ zUfeC&qvox(__3M>UHX~`IvU2eZeo{~QAY1n3lyEjk1`5!3VKdQg^k2_wzZ`{z>MeQI9umZbY$gi7d1JIjZw18DZm#P8C;8k`v0SR@CFVSNfZxsQM$ zI){ggZddF2VE%HEqoWIz;;XPT@B%K_*o5fJWFD#U~AcZAARXtCzV)_L;8G? z2_njl-~w(m>A0C$%-NfS!-&o72d`;p82_MC5}&9nW$$wHHxgJjM&Ap%j9C;9Z90)+2Knf8tlyRGoh&=hp$;zneEdY(T0?9%g{&T+)S z#DMEh&S8d*hz}(#4|}zNvJXX)mw(VbMwX(|7B)#-61psR18-de(J{G75x*?#lQ^>9 zsM*o?9x*=I^iJ{)U%}xdk^1%;Tn+-+#5)r9NTbk#c#x%lr!QOCQm536JMPoJFR;tS z!KI3`kH>vpzVeusG1s_PJw#K6h>N3I0aqnqESQJ0=!wPM7tIrmVFJMI zuim3lqf&&+M;^y*vk;4al*+WX^N!Ic!rGSpBqmoLb_CCLF^I6sqW`D@*X%2W9YG)< zAOM;#d3!&-8-CLlUyQ<1Khn_W=6RSzYW8uqKmG41j%m#cW!i7oCvX?Zr95U1l<)MR z4bAzs=vPXz`)yV0jAC+$@lZTetw!A~$!qNAJD--_y3kNPd|%*=xMT$Rd?crA4lc?H z<97^&$7HGnp?10b@dLbcT=McTU$m;1U>EWgE^YpziUf>1@gL)6@wx>Gb%GGKY}!Eq z1Gt%{iquO_&`SWObz+tI{S_fgJsOdop@g!sqQIOVi)&ivi-@itt;xXoc5`+zDD#DC ztrGNLAp?p4w-sxZz*w0#2Q#sBS@Z}oxZ2Vkq?`0JO3UZo?MQXw*ityJ{XZHlP7rkm z0_FNGVDz4#oxlk7U)x2f4+=x#($Zz1mGNbeMn9Lhc+Qsw5F3chMNzOs)vHu~5URNp zvYCYA{Ce|a?JUccPE}L!S(m3_i3pWuM0*aazBfSt0sg{^!IwWXpIUiy$1KltB+;2i zpmm`kqET!xPSPi=?s4-!;Yl+)p5>5WZx(6oI!?M1sS7eb66|OKY+jgiwWxhIAZq>b z0w+btk>nM`$lTv|zyA_Ou{rjLib@Pm!>Ic`i(56$;7>C7BZXu6-MagAOSs~o{Ys<5 zpAE;^>3uf?iY16iarYRT#;aeX_j70@D4lg4#aU$fQq16mKg^g=+O&5|c{YGXLX0c) z=WI_#Gfch13f~*vRE9*h^$8*g$Ss6l6r_?@fyUyFj*5L+J37YGT(orJgfDtx#%I5O zO2Hyg`91NAHDA1TADblHlp*X*$GwHOYQN`{v9Nk=X4jTX4yBuu0V&8t^us#7jTMAR zNn8^s=xAFpbD|x@lKMLR)CY^J#U|@TU|+-VODUAD_rQQ5MkG^sw_=!ng0nBMM|T%X zN)7|mdF2u41DuG)%&T~9#gX!(x3m%Y0k!yaCHqWCT)z|*hVjuiTNRcObp#-l!`%C@-cjcJbpe z@nxmsm4R!~#e+@S>EzJP`*nLHzR6Pfb1SKYAkq+qWwD@K#lq4n#DnB(0|X+*3;E0n z9buRTO~*He{6WdBw%8^U_qoG1FGyuS-#`g`Hr}re$>n;ylE`Mr$%$K<{i7JINh=nx zf-{H5;ZH=E?jPxSSN zYxr3i2GV2@jLgpeV=zLOw?LW;kBArt9k|auH11V%*@mV`{wST-F&<|HA!xOqxw8)( z3b_7W0FhL1?9Ayf2a^bcjEPg#XynvC9MzVi&WkJ7JaZi2-PL(Vn%9|n7y%d_6(yS>fZvAdu^v&_4 z^y%3?_wN}!$f{PDP%SN!LQP66PW;__JM}imzkMKoWgRMWcD-Zxu94bS%D`oL@<-(dQ8L1BO|PD@RNCySkyO*WZl@`2+F z-JvADHy8K^otRRXik6R6qkpcS{TuzyrK9DTi#et`-Nv{%G;8k;V~!;o4rnd5wfFGJ zk0y(|a9@HH(*YqFwWGIp6db3*bGhd>XXq)MjH86663*$Cl3RH=W5J zdxiqz`^7A<8yhdTOeS^RmpND+6!AjF9LQ40-e9^TfC@WTEiMi!<^b;P2lR!8Xq(#4 zFm6sz)$I7P|7eJ_7cct->R>7_y*Az(&kAIgY%6m;CJyKdlv6W^t$b6)SFy#om$jH3 z8l#LA@QZmMFWp`&ZPB04<$SLG<RZ&KO&4W1tof$@Ktz z3a`Ln_V2D|hqOmNv!*aQc3ANh>$yT|nyrQi3ZqS1c$s=Ib=-Xy|BMu#!Kpf$OX%IEa*<96=C zgV=zsEKHJS?9WPiPLv6M%x-TMYU2AQWfaf|`v}vHN4&S^6!CLrGrX;JV_h z))WUV*TgTUrun9D4lbFG$q%513Y+<%)T6rHHU$&OAEMBUbw{woWF1T#HW^R^bNgNj zwV_sZVk_U|XH<9#aU{Hjm`xmN`czy{vu7ep#@5Oa-Pw%a*=stKi3GLM>hgJKzNY1PWHNqn8x1U`{#L6E@d z5d~#sg1*9#kPt>SSnpt;jq9nX{j|O*sDu-lzk5b>zNR!_S|;X&qWTO5i4aj#Zh33z z;QM~B6sFBm4If|<3!{wYlLPP>Du@E*p#yv>e~Owj<*S?y0MddBINUUfL85-$6!63a zIo3WN#UvMX-R~E%s!+o;>+kPB{r$~+g9%T7dlwv8gmv(N1md}{IwV%P#kHDSmQv%K z>V7HL+5UypU5#s#Ez{n7wTC-3f}SdWbju{T*8)Fa3olnHj4QMEYCZX$t9yPf+0#3)CAM%55W{7#J>pN2m`l2eP&AGn}hL-R)#4(94n~$bhg7974dkzknKX1bePU(iVVA|4N5AY1a*Do#Tn5 z6?9|l72`W{TpZD>)2Q&T9CE#U+!DYtY_}LB9fI1miP~pbf|;`Q9^E#|f)=q| z@Is`j88|RdIa3onXkb=7I4T0_Lw!o@&kuu zMh@F427EDPrGiQ%V<>%m#B^KVX(N`Yu1ySY1rSj)Jb&{s`F;dP)^jQu+eGpBsU}aa zVNzTMhQ1*F%`cQb>E0j@KPqYPYG(?ui?}YHPCNyf!T$u54Z+FjVzX&3@SOwTyt)jT z`*|THpw8PR6X2qke4mz|Hq&1ZTkJ`?b^YpWtU7~ zp9d!%y(O1;>jxJb+Y09iI*!F#M8MYWoAWh;oCJ?7}HeZrIi! zjBVXUN7GY`W^Kf1!3dO@dJ9$dDNnKtAh=5*Qtq0-4-R6$@&=r8LW+qaMIrU_0${;V zmD6Z*HO+l6t*lad`J-Z`plMzoALKIfPH)kv#^*Pthh9YvAz|@k^U_^Z%I}H=(fH{| z2Opr52pxTjoXe=%2j@e;c|rsjIcmdz%S#?#^zjF1)89?<9*J_>i%UeQczSVfhnv+X zbp9;;Znr@Fwed&wqkrPzb{7dT?Qo7|ddALT#EbN9_J2?jKH${6r4sebOCEd$6K? z8k9YB`<yv}3QeV0gMWCN*e%8dY-?(=iDj^sdb>v} z6Y>TPR})qTr4rWa_%HKozn#RP%`nIdy9p=fi-BVoKtS>k02(vIi1S*u1K*L+2M`iK zOK8RHAt3;EOSoTEf%uc9pjfW8B;K(JqX`mBAsS9%To(0PCaOw8D&AagRuZ@B*7H0i zP)v@u<_Jrzze;s;DyXN~hPVv$)NKw;d>S!PSXNN6-KERD=uj~|TX??dZeEuakR^3| z{$C@$36~IO&=&hoY%H!K1{*@2k)vtOm<*V?>tzMb!zaMGO2QifSRT!@Q!?ToWV1Dz zeB`L;Y*MBcTuqu!*)iOfTAnmgga+GXZVttsmAe}5FfbtSb^%8EwQs(wLx5!+9+%o; zrnO+&k;SCxK=%s?K|d9309V&J!0E5~5kq`mwP|%LTYK8g0XDJF)n^K&g*neH0Mx6= z-;Pk$2Asn{;Em)gIfzU;$gS=7dW1f~tO$bi40dt?_RC62SPGGOaVb0$8Sm(`SOn?5~VnjJKnPi6f1+lNrX&X(NdmxwO7Nyv(!ijukY4H=Ax7C&JPWE=G^7VuW z19*8AEd_zl*nja|pJP&62X0Uvv%j7eG{HnJurbq5(>_d7(D;Vuj#Zelc_5L zy#uJR@y0t^)`6{n&1F=38Z(R7dE_>+!5mk(uE3CahnU!5+r#d{G>Wo=U|s&bK~70^ zdl&tNn_F4wm39>+ZMANBNlxRJ@|ba~$Do2_iUCnUsk;t( z5RXXRug&M4cFfGo(#p!po!#BAulSf=f^Ebj=$RcX%~|}J^9voNk8@1Jecs*^$;!Hs zSFp(}p54z2z{sp$nv7xrZ-R>vP()s#C|jaQqm@;dz{Y9tID0HG_!P)^ zutzQ>uX^Q)0d%rXQ>FAL5QdWcQk=I!P7ks%iDE5%7qzsT7FaO^L#T!64pQV(4bsJn zUZMDvl4ji@au{(az6jaeedtUjmiLRP1!Om-E;Qn1tRMvk)|;q=aAQOeB1&qBuqVo@ zjupi^K0sZWyKOwekSRxPJTFB5p2Z*#hrrmrrJF!SHhHq|A@4fH${^J|t zKVw`k2hQ2C_F6NZ`OIeplDF%mI7$_9Lr|=$k``EU)yP3NvV=l*=4OhJw9(8>X7Ox= zY#(6_JD5B0nmb7}egH{<)qDaWYhX^y;P8zXDnm}87}-q&QUk9sa2Y~?$nE1BbXyEE z9z5ln6y!ZBW@~!0&Km6K0$aYamgTxuB`I<7aSu_8h~eQ;*VC*-&fZ*|5H|35RaNmS z>=cs^H;1H0m2NiYDVBdBAxvWJEQ^TdhP=TAKCQw*V>*?u9gr-Z<*+~3$}J|PSkQdw z227ixB1zjq7V9*ed#1$+^ePJ(tDYA_oHcgEf=#I8M2x+0xa-zFlNa!J3UmgNleN7| z4HeDcp%>VC8_8i~+ifafiEqKXaP&xZA44MFXSCA9@hO=Blvb8+HQA5(~ zNuroT6lIrax$<3h@xGi z_XGghXUT%ywzXR^@fJ*7Qcl4;v`lYMPPd}F6~U~f2YY*hxzGl{Gaz_<^Z*s}0^IOj zaUO-=6AMJDAlpSHdGCB+Kt)z{i2%}|N1l%_dk*5T(T454*1$L4$f`UYF>(|JeteZ~ z5?~av(N|+FJ~As#uaDN;Z(Nc0(o?HWora}VC7TuPBHN^09xe5$bKv@>Onm!Pamp^lK4tS*H=(&)l+_>!@E^ zMP_R*W~iunF?8c7?ygH6r>5&$<)T2G@<-re)5D#v*q`Q6KgGX4uNJdcjZ|NZKVscY3ktS)1OJI9-GCe*KxvH8Y+z8QF9hULLL#3LH#RaT;(IMd(z6UJC$%%<=q5{Vq^P<5e&go%9v{YPi3pw1VUFgkuq)8~LTS5YP{n<5k(33ks8W=CvXl$_~#Qub&q=vjDj>WYE~UXDhC%8UqJ;9~qh2*w`rh8KEZe z-Qlnl-rXus`0ksKAewOBA>NM&nP|QYAvJ!%q@)WO$fr%;~_O%(#G+fM?A99sHfBd9@(uI-aRAn3p3AxA>_kX(tZ zlC`4MA2fOP8~Q2^i+P9+ACS!D*5nNh({77L>1tUaAMHDnBt^hf*+=XotytUzb!|$a zulwc(w_mL_=V&924onWsN#OcY^E}{@j?k0@fdxa!FSpwgF$)FRF_^AX<_ny z5gKb9#u@DM$ zWXG-hstI#&TqO}k$`~l$cz!pJ^R-^M654!U4&nDf{I{VG-DmNv2J!IFB_-1oFs7P< zE}<=E;38jg-FyuU78Kq04-Y9w_)unWP)=KYa@N3adv~i1(3Er)8mG0l>m#q)hDzI5 zy8)VfLriyn#T{5uG`JYwaQ{@Kju$ZGt9QDUE#CglBvDRxhB^OC^0(|(X`_r4F18VE z;U@e3U1V#3Y-5h%w~Yv;4A(~cUylSTpymG@sl)kab{vaU3KWdHBrk%tU$k{ZUhYHL zkdE|7%LckO9~?;;nYAwTZ^ZxCC4rLapQm|2+G5?eM}9VmH~4VnO7e$qmvA7i5DgKA zic>;@NYHJO%ra^mJhn$aQ4A}V|4Yt*!Ma7ptxUiryF0H2Jgl_ItC02Y4j;ug&<9CM z#+!|&glh^C%OX>UGZ9RJ{NQiM9dHfB9JTeCTZ87lxsFBB^15{4@4~5o(#GOg2!RPQ z3o^^;?YPrraa~bXAf)+ELOp@BY9HrEP;Iu}@ZGkZ1wPQu8h9GL$;Y#O1_`VNX$MqP zZQCnt3%zZ-8-Xo4qTwihmH*s7exhE4HQuRjs{e7ECvrO|hIuc1;i(`sa}wt}GBP57 zKZWZ&uJRDkiGJY}8>VZM05^mZ;r8zo;@dO(&G#^T_FF1?!#uO4Tk!y*R9429AIfkZ zIZ|;b02?4;L%o?0>d)F7$oEm)%l_bqC>LbS%%EjVF_JtyF;_2Wy;ldSKwbAtVsPla z{4=8`Z_&b-7@{-k)ifG*HgFm&Q2@{Xqb2ix_vGpoB613a7Cby1r~B4i_Aa)C_s7pI zT)hOb_&h*YCq@XxRj>%Oi2p=7aO4F%k6Tu=nnq+oT}~yr(z{E=Ur)JDHUR5B{W=yf z6<*+plap3nBH-s|L_6K*>EjdGnczDpgQ5%aLz{&wg0ltXdBvAd_(aLn;kF500FU#Z zb>Ogo$Hdf@oTS*l5|(H=^KN1VYQKJi+{6Z{Df*5WiM{K)r}lEofdQC2gJ;b?Pg_g_ z@#p#9a~fzajA@hD%Jk6Vw{)B#qGTbeG<<72Ae74r%B_g&nlw)aM;^hb;l^{YF7O3} z>r))~?$9tW$hE}WXSMyM4&?N-A+7&d-F)-cD18wiIIGv0BJ(@J`G+{G#uVnMcRp{6KW`PoM_zt==#86XE5J#r^{A z;SUd9>6ewyrKG+O4^JMT;@0`Y1pv%M7o?~+c!Z!?gY-S?L(7@*A%pnqP zYhWl(1GCCnB>QJXT=S1%wgca zA>mbJC|wWyYO_oq+R#bceeC6s<6IDg@UHKxRVpZln%GeBMOML4F7y`LWJ*d&9c)5a ztk?SCfjMV1teIuQ6u@PEzxb2@#^t*Z4pZ{0^7)Joay|b)zD=tB_iyk60spsuzh&`4Y|`JCF7)X9 zeFKy9=AXrS{L2py{(qg=Im@K{Ko-Q?p})u=&!9P+SqR>M9J+3v!EtEV8uR;-Z@NE; zU=e{of_BpI4Q^{NH|FewpyVq;BfVL5-Q!jjUta3&=AywPf#ZvEzxVqZ-Lqh( ziwAD#9bxyv(?FU?g2XvR@Kh|H8NAOb6lc=CKkJjv9LkHUB1#(n!- zIBY5iJ3mx|YMz;YB(sj6SUaMuGGUtg)`i~7*H>z|%n9el4FemX3BxNF&1hWhWNPpQ zselFsBvil?S1$G6bL0UPH3_(tZdC`PCTEZl2FZMzOlzWd<7&!F=X3dD3;&#GrRPa^L<+1px{la_uDoguP;O>Dy*SsGRA&0NbJ z96D|BA|N!Kx(gth#LgUG>1q3 zF75gmVuuAL_$SQJy{x+7a;$k3U0-Fi=Pc??e z0U9M8xDGT1MAjQN4PdP$+IfBisKGo2KH)P(Mf=KmB5vEKfgNyUdlKF+<>23!=S3)^ zuhp4GEPzUP2iWoTg`AeIv*MpF$FT>9*QQ#JhPM@xqlXB0ANSAXf#U9Sx|f zgZKN<+`P$ko0e7#BDfr6HwQ$x8{($VYuvC;hq3ATS73MafPudYW?`ed{IGL5TY_hY z-xrro5;$4_0m$bZA=ap6x_y1N9sK=%^U986UyhE%E3JjrkFQ{rN+W9iT3`Szp#k|0 zoVSH%FD)=A$f}+qlx2J1c!+X!{uIzq6kA}=fR0iJ8-(x%AVK-|cZO@YvRknro;z|1 z*3D77l*Ii6*i}InWaH=B5_}OUeK55E(C~I58xH{b6!ps<8H{G0JwrrI+#YZj2)iKJ z%b(5bR+X}~Erd9MGPFU)VjqlPCUQ&2&LFWoP7~I}!4VND@Py~U+I1s;MYR1{YrEN) zE5TT#GL-L2j@SBk_v*x^dwE^JAm)}waI9f7wjtX%p6!t{u!`sWxR#zmfTyJ8#{#Psx%wVR_5S?14(H(@8V2QPAl)8=r02i{oSgAm%y%?&pLDNyn$FkV+f^i%AWOO(Z85>&-&Jg5GC4J+ zzq>w*XyX&R?3>Z4l_`T44&d3DzETA;N@a7K~PB?Kfrp`2T5Lx7%rDXM-@;yx1agwi9F)+T5z&gdEOp}J3_rBb~#|6 zKt!NsrgUp4=eSUdg5pLjN{;d%Q8bkPM@x&GqGAL{y(6ltg1oiJPA>qF;eJLC8I*^$ z$kP{rdh4*E1;z2e=VAaTe-oRSI3qVVmt>!tf`Y=w&o88^;_)5i@21tTNkAU>2&7dO zVvbJ;>=qquWTSBM?TZ6;XMsZ(E6MXX&a6WO)S#p5m zk<%$5^0#0CkYWP7_#z=lyl@NID}ugsy)A;bc+eOcZN=jR-y*`sX-P5mKWgTXPoBJT zUm*1dHqGh_(RNee_ONw`pMw}=G(&dNL`YNv@Gnm4-?&m&<)6qq6WlVW!}nbSG@8p4 zJjHw@sMIZuBTf_A^*elU{zj)g^Z$TS0E{a z(w71rQAL7##r=fIdFhz8|CioD@CCM&fj~2P>hOZ=HwZoZKJV{4BK93rl|$NR-#@3b zGF_170i81`!1s-d7xTw?x&vNd?QSh2&pd>NfKM1+Ad#<&S$aYlLGsCyG$-4TrPUUB z^y>xX2TwqI7TKnV9O+u_10b4w7EBgs>_$0*k`&?#IsnuRj)|ccA>eT=Jb#5a?bag? zWak86Qz3izK<8c}Je%75l|O?G2*Mutum>~QwZ!c(hbTl~pBvbo0%@EkpPft11TFO_ z^s0ANqrbS8eP}XTl}t*Z4CN#TY2n;JQBI;nYzD+i%kD6>`Lq^>%ZGoLn*TdUz=5e^ zdD6Z_iqZqw@$ahei#bLV3~-M@uqn-NcIiK?8Dt1rF%h22wC*;rm0T5(JqiB8Z+{2q zgsB0+jB7mcpPSNULug4v%>VC1hTuib8)#BFCIBD8BFIQr9gags5*d1T@Wy;8)O3QD5y^7r;^X#WjRrg~a4JR3yy*!&`1J>#I_<(uW6$9_9*g~nFXC8riE4L%vqd+q;r!H9)Cq9R*vW)onHOrB24v(Ldu79?$ zY%J8@{G6g+e(BO-^27uQT-GG~g zL8TMchgn#CBdAoaf@V z4KnhD{Ck_F$#Zi~f#gZdvL0B@+n>jWy+YG&B=e|&VDlCYEv>?3A^!VpuMWy15~Czk@hgJM-K*`txFpKAVZ zL;|ClZ9z@ynPi*YWAQ^!dMb0)(qC|?LQxcQID!F2IvGy}_#|!1rOVN2K?Hw{_#BpT z0Knw6zO)&aq@9^Y+gj#i@;S@Y`!b=e9MAAA0ii2T75gTs=@0$wH|(YR^Y35##j+$6 zQ|z!6-uHFmrE9Z6xnH{r9N7*uSq7p~x`Gz~eSFZ}D z#y@{scCdNbXh=Dst}ZC}y=6>bpmThDyv3Pa6MVjT1IR#8E)XIT)gj|qCyjIQNlC}# z=MSY@KI7?oha25;cj%YI($#oHVx9X$z;8-fSq0dyZ?rl(#XPI}Hv*F&Aae0o6;@7g~wX;`aP+Ir<_7L!0=$M$0#eW(l z@iasRy0?TLo}-szz`>oWo2Ch>8U@);c38^No|o~cNlEjjdOUcToJwZwV$XVrdbuR^4C;{7i%EJBkFVCHk0>bY+>xS#D`v$%O9Mb;G{o2Iz;Qt9>>`r zMh^bL=wID!(3Km(DvD4IemnPuuU2xp%R}9kKd5|j4Z00^4$wP*l_6laBWu+2%OW^9 zJyZ{Nk;@aLm%9Qx9QCCG`vk=e4QAXU#OXvMyCtFU^+VMXks3$(WwK~XGzl_RL_3~PKR*H#*l})5l z!N=c`Oi4;gDp`N`?pV*4N&S-2eyH4EyB(BQuxGxNfQL2xw9Ub4YsphY;^>sgvbg2U zuYjQ{_y#E{z1Tb{DNYg%zSr?f`ir}JXnjKiG?FBoYdVLbs;cegR>@&^x9w%Gcim(Z z_jQFdtUHXRmr;m`? zxdiV43&f2zwY8oQ)`f$LW*?&#z)VbzPLw~wMnAcwWTGE+dCL$eA=Xv zvd+$rd{jPutBSjL>Xb=9$R-6bv9q6u7$i0Ds;=lzzd80Cz~L}XD{3YtCd=g^qM&-| zsca7oM$-{K$g3;r0?6#f-?FjWuit^phzzzFyVt15pArNZiBQr_Qu+^HyDn8Rdigrw zN=UdgPI>!@3sqATN|ydwb8xfYFuqN{blu#dg_DygK)*J4?zIj#awfMP$uezSa zsP8?X`d6{B!&wb*Jv83iEdY-9W4ZiDoS*+Jq-WERp1lAB3`!8M0JrXCG{8z0udJ*j zjCS)Xbe&VAxlD2xhE3V^Gk>wTH7SP3?aId?at9aZv!>W6RhMKq_#bCjlcSwbgg1Q zkj0zr?PtnXoi_>Y5so0|^GNMPGg;~TMNBG5N?c(jS;>J_L63$=)8^<;44$Bh(*HdQBp#9`ph&tCk|HVn9s&1yEVL( zmD$IJ3%qp=Qe*{QrG=U>X=|UJ%v@G{FX>y5|8MW@;q}Lq({XV04pDi-@Jgdmit`6X z5Qy`A6XU*rKp5@k)e{z}^u*1LXJVn32rjh$=j_RZ_3C*Gb1&sxVzOPZHOe#Ey zcj4i)u4%|5Ny#rMp&c8G`Di>iw7thW3AbD=KG!k(U~MWcOa69HKz7SNhkA`13KOW0 zkb(Ia@!BjA-VVA5Z&9C9kjgXxSor(FNz()dY6pZD`CBjIYTlhC3lX*}GB4E~w7&%N zv-&#*Y+x#sL?$^2zr@7alRUZGgXK_RoU!F)3Fw{6Ufk&isU-XQl{2W9$yr!(>tZ2s z$S_8NdAFdHPXUsGCELnpzN@CS-<0# zwdG^dv6~K?+|Aiq!>u{f#N6BwFr^V&gH?o{?&~ETl>uhs6V!IR-@c__V=GYa2aKf! z_S)NlO(QSmCnO}Sz`DgEAW+n_c!|t1>BiA260@;t`@TE}xu$cZoyvUQ48;-&76$i9 z;-rp`42C#4!cUx!-$yA+{S>8m)JW^cM<|Z46%Zl2DT*s#ags^mM5^|)6e08~KHoJz z0;=wsSE1e<#eq)5`Cg^i8aA*XgY95cpxzh;GJSE0WMEq*C!gyiWt$B9vKa3(Vh{{W zA=9^lo0|X&pWXAm12GxhA`=1Y*Q%e#%LXpB#~rc6tzctCV2YsJ4)O-1C=mX;>RRy% z6=;A&Vz;r4jRhDUj!jL~09@EI^D{1-&4d_e4ePM5p&AWpA4dj#wJHSUmbz0rw!;r)FNcE(31LlHoeq(tt;4zrUV zKoZgYAeTJ1@4k2zfbzIyuI=pZelZ<=2%BeC0;H`_U`!;Z=9EX4c#BjAdSd?CFSwf@lK`AqSY9F>JDnn#{%Zr{*1dag8pY7; z>=dfM--jDxds==-r^s?G`K(NWs;yo4Z0jhmt^K{}){k~k+&c~rEFL_4$_@@uDCy4F zYRY3+)%{W@OFKvd-y<#v3>&~Yi2$hc3kYBl5-z~Qf>)IIA*seE%>dg<<|lQZrkJGo7EfX#C^Pk2gq~GFT#l48BiXXd{G1L zK3z%THWyd;U@-}DVB~>-)BW#(fvGn8_rUL2qAjL3+dfv^Eh$kv0|$`}dMQkiOuaES zQD5ga<>uHF@%CE-4d&;1Z|oa{!4bm2|H}-4%e}_IF-uO-^yyg~s&2=OAF2N&+jR(f zWGBP#wKz*b?isyif)nJ$NHq*%<20V`cGyIbBv_VYa1G@dz#@_x8 zcHt{)H#Dcy_^X4F!9S0&i%oJQ0bvElZ?l9IK=b#>U5&b59Qnv|C_yH5#nyfo`H0yw zSjMid1@%8Tzg4_4;rgH7q6+=^$di-v9G2Os3{@KRD)r$qykwaMv9+ld_-p`mQ|G*Z zLPy46jCPY?mdg>#moI@t;87b=E^j>Kr}q5M8c9$qclHy1z_09@+U>KxzRd2tj|wxb zx~Zuh+@!Q#hAP7kXp#1YM=0>HvcXSWp!$W3xPu1sA1*+s{Vh^CaX2u4wY8Xs2EL#8 zLw-UUPRsMsLHIDYL@aP8f}ZAfJ?;zTEjrH3$&KLtN1`TWb5%cfAIxJ;sonAVfmk0%)C zk#o|+rp=be_Ch&Nmi)H(x94{??v>BYNb{#)$fe`q$Yc%;7tN)hiU`|!uT4uJ&m~lH z5QbAa>OW)p0;m@!V@lKI!>p4Z6u4@lm>1#hDMW@u2}TeT6-u5umBa3G$Sxw1$%O<@ zl0@@xoHzeH4%BvkKbuYnT6+qX&y|$)wWal%B8N#YR4o0Bo~NG&VG0u=`!C$^@D(JQ zeIZ>7j|3+U{vp-m`n}FqHaD$L?krt4KEVkV#r#F|sJD@k#V~fsvdZbI(jg&qwIicH zVS5f24&;iSslXMzkNx)*L3EWjFkn$#d*lm~x;<-@4|!aj{BH@MAUlQ8)+QSeQ`Yr2 zyn64A_$D!H>w8e?N>Iqrwsml@fXZpM_2T4Fjvk=XkTT%x#Tl!>qYDt_vNgS|pn!Ve zLf9e&&ObAZ*|x%{+tW!N@3`kCJ@hg9jg4twr+$vkCcYyb$q-Y%#NtOJg(yixE?$R zU6|Ny0+zzyH*Zwp;~k$atbn@!4|KUsteQh+8b);T_pq`OMZ0<_XF(YntWrI|tluHj zLBZ#?WrPwA5RsT5C|miGX$H04pC2BaQA(mF?UV;lnFvV)P;}C8;HcZX9$>fVdqTq4 zF9+3Q^Ok>%PZS)PW~(;4=p!xFEYGlzEE05!oYIBwVI?~OoAU*mEf3ecqMP8wz>1>k z+!^CeaVW(M`10E+L0(wx?YP;igNy~>^eO6*NlBDYWFjv=I^^=aO!RfZ0N8wHyX$&N ziBzPW1F+NVaeRwhPg&Wm>Kc!LaJ^F_9m#a@iS!@s)~3DvNnU4~cxho@OHPc+R_zH2 z4GnLOE+=K%qIK9(ywQZlhIh@_=W6oOf35?TAbSARynbe=a5*|U`WP-lo>(y+s)>Kz zs4usoyE~NAYK{^3RuB?s<~Rz#B2@NSIq$Hi}#XS8^YTFVPG9kYSpgN3#1mzf{V;WEqgnPln!>fgI ztU*qcK>RM$ZOx9?T~v{*tu4!K%IP~OFJD?j>0mLCo~8{6B*~Fa^Ox&lcX*qek{xxQ z*P$lGecz`OO{cEq6qs!@8xs_m|72*!)1;($>}xxaqv3U;!E)(;_Ufe<_R8ui%;eOb zO(4qQaH|r-KGKZN&d#82oG-bGjdcnisy`=FzA=yVmIfgQhaTK1y6+1gBit40FQ2bp zJ*}bfy;gy?Yw^}CigHzfnqC7jnYWV*JH5=Z6ccjss_df9*2ZFWjk%aNZU{~{t34|} zOpoGq@d*q2sDBr;$cs5v&CAQ;H!^`dc`O$f7wB#9F=`p^%GGN(FV}{ut+0dyL%Rj+ zry;DXs~f-zwH9rtI}5}VXg3i&eDuioEeVe!JA97%3$zMEw6;El9N)(<7O(}_HlFqi z9Tk;#Yip|}Q4J6>w4lL5ARnr&m;?krpiJ*;s0K*`R5s~C`hl?qS;(Wc&+&Fp7fyQI z7?(nbyf^K#EBRH4CeGo1>dbDI!?L2OOS@-u)s>_%Tl+AijWK8_N4?Jp#Jp~0j) z`r{Surz}jquOF1!XU}~~Yy(LEOipuhcTYS8bRr&GM&I470%NvFw5ewkwqZ?}#Le-M z#+p;23^>)dXy3#qOI;-5__EC{o2hN7t{xzN-C+~czKPWkmZqEh(;3@+97>l)xKjB% zQ`p9@e3fPe4uWpd)yV0;e@@Bw^$aTU&5owGxxU7etVsTsp{zhIi{yV!dsmrtp9VNR zTYxV-rJ3oceH&bQk(H zYRNdi)|K5^IT)Q1{2Uo+Mf`g6dx`TwT5a6ZG`Ho{SE#-VzK}&Km1kc1?sj#EM4Qal8t9hRZTu8}g>H^|RceDeXm|&0daJvRN0Zkk`mur1^YqurxiT(G# z@BK2pOTb3I;{;7f=te!5m6c(<)`kz9UjZ+c*#F_)r;)w6Ps-HInXqk{lvg zn_}ASR`V_6UG+!qNl)za8+%s69(C=;8HwW{JpJN@uqHnukvGInUc864X~IDdthRSP zLJ}GpQ44~pkduDUKY7+ub}&QY{(1K6+=6fLAAs@oO>=rfz?veTK`MCeG`--F$oD95u(MM1th<-KjTn=rg9b) zRgT>S79y4oWJI?(zv6iW)LM0am&(x|bX&~M)^MR)3a4RE|0p2aI5nnrW@750J~cT- z#mh5Yz2S(H20EudC@`~IUg*m$Vr+g+&3XH1-eUSD z4{-z=^jD^5AAY?rm)sov%4v`O&BY5@epIt@FGh_LCgs=*e2=0@QU!d!Ov|y?;?nsn zSM5j9AMOos2#dKVKDotlU9q4HlZH3b_o#W;g-%i4fB1Xt+?dnT5G|RuvO53ZcTXM5 zLtm&}q zZ!CLYR4ir=f4|elFkT8!u`ThGV_4OPNpSFH-n>vlcz`zVi3IbXdVVvJ7H#(S!Gp!}cdVX=h zk@?LH9TnmGrr*A@+aK<~X5!)LI@j8juCb;)Px?7Fb|briuEWhLV7+)*eS~jbQ%@sd zBV=!zMsh6`#_2kT?iPz+Z!fznTBPkCG`OUVJoKBqF1NimHnxAMB`(W% zT3V60@%vHP95oQy(Dqup_Ld_I)cfzvZ)?^1@YD_tMm%zEf6$)TwY6C7on@()z=nrH zaqmi4lN7u81TTas!cSZGf(ax}V%dqu2vX^dr1rpEaRu&IIEe6KG(er>52aW63}9OM{wf-cTn0Hidl-P5eOM>KJAp@lko~R; zfO{`vhq1pCZ>`>yGqyOzC+K0tWWJ`oy~y_Y*%@>5wDM%5dsV}S+A6KaKgTME%gVA` z$~Ow)*$iE&q}1EuztHJU9a&o)7Ospl@GgxpVN>>Li;0Y@+9m8~Q-5XnaTk6!Uen9K zt3;IQ<5z6k=zejML@IHidL=xm_Y%>&urkIV=&8>_ZW8`rYMfes=q79+zJi zFME8oVjxNi1V8&pCRGHY)FADq!z(1Q8P?u3Cyv})!NZF^I4zA<3qmwO;G|*z*cT_4 zsagYgl>7t-LZ*2JR0q=j80THRIPj_-nVFfRv$J|oU1-K#v-b;M!HkJIVSeLmWqZmbB&)>^ZshA!}wc|l}2WdXK@}H+q(oc zZs$Az&)vzEyTiuT4{WNNTJv3zZ-8(eS&MCUuG!l87HP96WMcF}^9aNb$?4B4l2}xs z-q@5U**vBMFB5iGDOkJ6^Fbzc`Yp=o;SRz;O z@M zFlJQ6JhxCa57=eSxqNfKAve^LWXtQqg^N@>lFPVg{DPJ5#!3HMxVNN;#;8v$=|uz6Br(JB)cby%&+2n> zozA(N z$L^D}VI`v*BVZ*&+M+;ZItdBU^TxJUVueNb0`D1V!yL2uu2yH5Pmk{LS^@7;d;920 zE!eq_&&cG9ae=<|5TMcbzRVrIm)2HZ>b0x45Em|S_LSa0nw_7cgqG1g<_K#75JW#P zjJ-^<4cAPDUWI|bIzyTP&T1(1JHyt{&^XCx;|v5;hwL^N9bSI5%1kmF_srNo*QCAu z>(R_GNnh^EqkYsG{{njIA@hdd@-ku8sTuocRPcL-a(5CJ_nNn(&0Og&Krr)(L_--E zyrFqd;uggVtRx8&*y&q9`DyTHJW@@(6g(LE{J3L~hAOl^h?46VGx5s+2L90$)D_O2 zFMC!0{Oc;-o40wYSMT5JvKMui;CY!WRWW$*Lrp0@!osrjv$WHmVx}geruzo<`C@CB z!wDsp?TIZTi-R#vdx9GHf(a#wUa85FFzpQ=pWq-+rh+q1UPT~5q!7dYMRsC>kdn#l zG(4ANrL71WC#EnSqOVr3;>qU|TEIB&tfhzY;04ggPBznBCU%eJ8f}-TXu?=3Cs`^& zh0N_!sFj5Uw$;_Wk7R6jJcyF7lG8faA{K50$HHXOJ>#8rt(!l$bG zTCGf@>JYFi*X`|}6H_s%B_&9f2CpB&#}5*~k{5tmc}`56%hhKkmXIj-DHRqx|ArW& z)c$4t*;LsF%7nBE=A%4w+Bp&E{PCYFo6hazeX=r`Pru4(>3VoD8m=>v7@1a}l>EWf zwfFf8qLGmbj^M!j=?=@BR>Pz48m^-BGG7kPZYVXQxo4=N!ikMEN#A=f6+?CcT6%@MkR0d8O3 zTcn@XQ!xyOjY~Hwn%ExYXak$s{^G!(M%}wHG>@G|?fG$1Ne!8)flst~V^fhDwUH6FPQJGKrMJN3+Q!`A8-Iy{K)mjU4)}dPP4wOkKYtf->CAcg zW$(j+@&{sXHinIN!^lakO*cA!l^<24DVR=pZRnrhk)x?Ip}#X5g60Yk0KH1t_ok-E z!a~5;pS3DuUc2TOD!0OP;=D9711r;L|3HR_O z);2H4s}ygsJK3Y*Haus=JDG%DDkqZ=Y^Ee&1_Q zn@((n7nb_nUK%5;fU(o3)yD<#na_8ny^Q8cFL=uMM)dO9nggNQaCx@NQ7Vg}ZBhi< z@jBwKp87M(2ngnmXIW32EWRKy6%|^2v-oYcEp(&464~9czMZvST}rJ*IeA4Nl-PQ* zPE9pttNJMO6ADh_y7ZP0_euZSnI2iXe1+%2)(~2KeZuAJq1!d?{w=IX{4+5jU@a0lpp8mPk^nQEV5GohCe3gk7-j6mW`1c8j z?dx_UD`8y=du?2u(9nR~-|S|zl6v|B4UMh{%jME6y`a2X`s0E#Jy<;ZSv{TTCo|}0 znbX;diW|vlN3u|p9UG~z^9z6FMf6$KOIYE*=sr+zr%%QKF_uy4dz*)Z9IF?HhN5S> zjOaGzhlNj`H(mrfw~MglT=TO4p3(?=f7g*2s8`EnYO#M6YAUcysZFh-90XP?KPFVu}+WX?K4kX@5ZE z26h{I?{uX-(_PDJJUr2}ZIh91{iIMSVqm$xzkktk`LapLsqxxJmMl@6GM1+p)SQDJ zJdZC0OjI-denMxWKaU&Nk}Iph&0D9wTE6Vf;n-<1INRx*S7)R#FreR-+^)lt;e>bb zB7@n;H$sPIz|fIb%0&c6313(+1WvSQj&{?p&CMxj<i(85PwC*rqVB3|2Na zCLtxd&BXMx=DIJ;+Me3k_I@wFr6s%*k@j*OuFK>yP+{J z8^&^xn_C^RRJJFUigZ&pq|b6bJxdD}{l@vDT|DRZ{E>|r-|rbEntieSp4pUo&us8l zR-k~MsHtpeaPB0k2%z~0zLk>sc^xK1#Z{0~gmc-@16cBWtIt*qpG=Ir zd9Q1CTdhEm@ijwYQ|I!}uNwyPwAdoT_^G$gB;WlXYeD=uqycHlMS<6uwHZOv;Sl)g zSs(!jdj@|GW74AMumU0b^b`{J&)xEuySlVc3r<2!f`fcw5ky6z=xl?4hCC-joL z1_Fqgtzk=pdVC#2Zb4b%^iGiWF0ATfpL}7#yyYZdW z&~)0B?9K++FQx;nkFRUBoMcT@yxrc%sAb5iMfAz9g73jbx1J>B#oh;dNv&TuSo-qz zPzt1UM&kTOmWsV?w>4;6c&{WUcctP-ah_W)MNL=Is&6DGWzFj`5R6aMtZ$@cDsQWi zwuI#D4sUI>`R4w@bxAPVN#8 zwxWGHC!frIzT9P-+@+7<2|artN(r@|3Z~VMGLQ1(V=}MDd~k|HIk|3 z*M3B!bE$fS0sPG$kKz)iah(<0g9ZlPxOJO{{J%#;P769Wf{3Esz2w0&-QeZrCAOOg zAnk97;;0%ZvY72JU?{cUpuR_c|9&*|K}DWAbKVrV7kGAt(7A}PFt|apl|`>DzPb?j zMj#6K4hU+&J@Gdw(B}HW!@|yYuro))|IS)1Dx0oG%`^LPk=&4`(ptFQ(7V=NW*WVp z%aF^U%*4BhHr<0a^ud>ck;l7>!&RWi=JLp<)aNVv;AhnHn)IV3Nx(&|c&G|vxiK(^hWo4Xl^dgjU2q`-_1$Jwt@p8; zuj90+cu~J|T6(>^h<)J(SI9e8lzDs!iORlwiv4ICMu`}7sYE`1;&)N#B#wL|N#z#R zIjBV{1wr^*7wMFEn+uM1e&{(9_b`X#OeZ;wa_;D<6^c9e;q~EVgsI64;*mB#MDPHe zyXaM>mRCtNI}=3lptWp!rIy6ka5zWjN;IbxQZ5@wPKu$vw*COH}~#tv2L(SV65FTmx_4r zBqmUMTuVsU_jw!6(XPHAAGG-Jr4f*nIk=7G6td61TTLa#VdIx{aG($tSQ)RFr~7M;{2q@Pv(p|rJ(IM# zEUwh%ayet73{8Z+y{fgG^z(nO*!jn+4y3eix`$nc_0}wY$PYW7si+`lGn991%+|sr z!hkI+M|X5uDt9;Vhn;FQ+Qmye7H%%bU%9=dY<}fl#e1nUj4w4#pP3&HV?<{><)Lxf zUs*r*@olzVJC{xNy%xbFze&;^jzt|ThnnB_mZL+fb5xF|GwibY#_IJ>f>F`sSnXte zdd$!d2+>0dDcsCjE@o)NRH(?|PRx?R^2c8{_hWP0exis&D=Mk(9BHfjeu{ zFsH|=y~QH$y#+SUvtsMTtK(BgH;vljFG8ae?P?DYKd*CH(*i5jrXn946C-pxl|fq& zX$uh)RRC{6mV$t)11u)s!U$+;MMt+G_e4$KU{!cRag2A ziKAD`z9ntlZoX|frv8|YM>+i4h=bzKftKQtqTzIj(U}!mj{$@8-`dy6StFJ-+eED{ zlqTR1ou0?#-U@9J+OcT47~@c)D^;gNQe!S}Yz^%4q;Crg$upocsPjVUpUKn-OIT z;bfJJ!*y=PcQqIUp`2G-(-xGq8GOcYaBZM>JX})_~w=sza5V zQ}$jDJ@)gc^jEM1E)F!9d%*0ge)fUy}xG$j{SzfUvneKI9Ku&o|pgeO`i^ABr zp_n8=W%J;ax@lBz!=`EXaMRg*Lq+&JiN8+?@9Ng~k88#9>&8SePPqp!hDn6lb1odQ z_>D5eB#!Q&?Twi)`(J)(g|_+KrXe!tg+mv71f_VBc&z-Wzon4U*ZGR<<{p`g+J2T1 zp63|B>Ws>Rgyy(kD?Qh3-pi?SJ;O}RwNswmJ2R5Ayd_zCbmwC!ef8Ily>WQJG0ug~ z57MNPoPMky!qAvF=lb5@%)41A<$HDdSx>nfd27bo8(Zii^gVd__Abv~OyAX6F(!6c zWiirM>%j}PcUdn=#By1d$x&C^H%Wb#xP|)dWrsA6Lgcq!sINXxnvI63!geYKs8`PO zIuxp_y`MhF-t66zx7wwxx2QEQdGK)}QS7CX@ymz2d%Y#sWz*gC+I<>daaepXQo7@~ zbPW#`3yV=3{aV??_SpBQskq$xzg`=8UQuFT;4#F{HMG;%!&CbfK=!MV881JY3s`iO z5V?gu@Y^3aLC%G-?fU8Z2e-_*X_*5^qR0n|0PDzqz0*5PH3V1G_ys zEzREW4Nqe%(=576?)@-y#_M6O1(jQ^(-EmNy``tZ(Dh|TN)FnWv9GgDvatHAY}-wW zO)mzi=_>xpVWMf+3Y?b9upS!vSZijy@N{!+({m|JDvROAZV~Q+&Ct@UT_EpOo1b;* z3$X??_Cu-xEqUtYYn$-k7wq8OmXJV8Kh5f4*0i02zz_+}eUr?(>Aov+w|`B1@W?IK zPxu)25VZ;$)&z>lm+8!4CUrUN5kpVM-u6F@45Z4$OL~uv;9vf^TN85 zh?#(pS@zXS;d95O2?$L+pnd&aOpKquKUfr~(=uyRo`pibE|?&1Er-no@5sm-F8eD& z0gDBL);BI*ytpt}T+mTC*HL_TC-k5%$ob0R4d?m?v7trxa&5_kH6qIoL#A|8DW~Ln zYmehK0Mq{)XXQ-mw8-yhStkxX+VV(c=e1yAZQqx7Xu%yqQ*Bm|I?qFFH8V;g=AYPY z`lOw>!+J|&KQo6Z&)#iXF)ZBWn_ITa#rD1ZphT;P5Q2tYmmr0mB5_)smX?_8_pwe| z6tUvkk0-9h6dmFWNfev@AKu&vC_{mR@vxS0=c7wFC!Oem#+^*Y&`ZM`Oxa@B-TLfSqG% zh6hf$@YmZck;-WLmVN&fiw)*Ui*w*Nmz;nKhTkzXBzrdMvJX2hWWF>3+SZ5A1n#$x zT~+xMkitQi@bKhye*u?04i^^}Ql@ySDk0%3o2M_S1GF09zE=qd^iaa}VtvksKd~kT zwo9*Qy5)cRn$2MB%l|v^k9JPcgZ9rj+N@d$sOHz~EDdGV`?^onOPenEjhuAj;&d+l z@;=wq+hQW2j`x%= zZ;wO=!p8bz$HO+|Hb))XYRic-yRs{qjJB=+7w^*IG!lLkUx60-*k7a9&Rp6>V(i`u zCobs49)&?uW+VjSfjS_?Q!ac?!gO3ZI0gJ3NHF;RwxQgYUg%9aq2qaS z0a9{ua!5--n9j^b12GR4)HHQ_KWhQZeZV$ILF6bD8_RC;w9TS~gv3ds=m;tJ4-m*y zj{kPu|K+ZMGr0cC$6v`#Us~eiLAFbb@P2lE^n`Tg8;Md{yP1;DGsJe=cX*0so6QRL zf6&`d=Xg;(skGv*I4oo3*b#V0MN0LxC){yqNm%;{`>k_6e$;$7&*p^_p}_S#x4KEq z$XWQ6jeyJ`o2^NDzvd^*vDb|2*mjOSx3x_MIh%n%)1q&p9(adkV6@zytY)NW=aGz;9NeeIYn1 zsu`U2W*n|TnHAFD#@A7*KZ~*h0xkpw2EKm%8lkv=s{2TSC*=>2E9ijn65j=Zccx9fe{74hGzGn-v|s>j=Vnuw>DCrNsQ zyqK$!A@GnVIKtk)Wr)*iwC-fFTlZ@YSFW6Kf9?&_;iwx!o-1{+EVJtxsyv6g9D-F> zv^ktRY#qDpc+(>0^vKI@wS~ z(j2vLT(`-Tb!OD7!S(vuSZGG&{Aqvb@48kqEu_vf*A4CR$G0YpXI+~wL~oNc4Bby# zYKu(^f(l@XifilEldp(r`C|u~`;M1u`nKV{zFUHTq;&aUJ4JZS6 zd2lXSScX1fAY}1Oj`KX?npxc%!3|6x319>~zQ8GvA1J`Ecd?i5oykNI{23FuX>|E^ z5kEQ2ZT*@Dc=3;Fzehk`*Ugk7G_$k^`w|^5;I2wY(ftIDJx~rwP}{&zf6(+P*I2m9 zb)}gF(%mjg8>0u0uI|W?Ba$4eP7^ z0fx7VaE$S~*Jft|w7IqD*80Qn2x}^_D8G`*rE>d-^gZ6T^0{br7ItcabV=AoW~(v; zgE=L%d%0?hvvzHM+1s9BSkk;}63~BsfX5eZ#(X|i{`q|Q+u&PLax<|!-=legMF(z0 zu>Sno3E{`Pw8Vez;}3oe$;K4FUK$z;5Ug%EqfzE|c0lgc*K` zHOy&@3B}{WT(GWS^KdMk5=x*BS}ChBR7@u!abo}G+el@u zalOa6J$SEdL~rau;P;4=6AkGA^hF=^N|KLgk%}6WofMf=s_Xq&pQ882${1+~_ z{bbF*aYmo)`g&Tw??G$8$_lp5ykqxNXjN6sHx0^&Dno%h*BIfrI+x=3A+iL$gw?qH zqxk6)mn?2t@!wh=Srs@8-<qC?)P01f5xABbim0f%Euz}Ug{EEKSs)Y^T!pho+294q4%g1Mr&ZJK$=FR<}(SIKO zMk;-2tZ5Tgr@HZP)Zy)qS2Eewz=9!gjCPY|`uS8)%ej z{J7v&CO6al@cZi7PeW z8iLl8F9r{|fXC@J>7zu@&R{CgyPu0<%s2jb8r8&Jp~(7pV6) z6j)8s=mCA@jY|87isE&to$0H}1z3CAO%(HyH&Izb4opj%gFe1_U>UX)h{xt1q z2HNH!t(wT8=09>#5SEfMqxtx$&MchcZMS!pt-+?oWr5*=4Ovchf#ESqtVs!&yf7s% z***FU^MLbwU-d>*avr@t0gUxjYIYpOsy)S}0Sd80N$uuy*znhLL9I&`j=->S?7zol zb@cvz^&m=PXAE<%aIQIa%*lRny2txAk$VBB#|VK`uacIq#`o2TM8ty%o zA0NNYgahRiZma6&r&;pC|EW&t|E^9!yJ(~Ki_fe(vy|_72;}5OznmF+mf3H8ly-~O zjbHRvaL%M=K2!VUC*x)(XILgpM2Bl;uRaXkai+)~S>w^Ha}_SzUEDEUw&PpdrK}^w zUB60r@B59dh|h2C6kN0t$?-(F4?VKAT4+}#DKaHJR#|5;#V+^jozZ?<%=^+7XF$D> zQ&@!pLu8-c&!ufSe%GhP@JP+7HO_0S_Dc^@ceWRue1?rIdWau<4f|VB!^4~ST}dic zoa7>U?WO9bW4B}eY=F?HJKt3B=cj9AVYZ_yxwTYMBB!3ho{>1bRjtpRlw45 z_-ctNR%9}3%kI*?8l%s$ip0}b)h{jdQYDi)W3;zx%M7i}C{GNZMLpfK?k&0y#`FqA zG`ed)vvQD#bSeH1olANEPx`0et2^ld(6$Q~kRr#So&D^Mg$chi97!!1oK%wH6uTph zQ~t68@tnuyM{!gFao;jj4FY4LB)GJPUYOfIyQ>HFsQ@~&kjQ~%Aa|0>pc;JJ!1k}{=Lm!AIUHOxc*@VTq>=uDa2t7KYY z&o9H5oJ3s(Bbd|6ewYi@VpZ8blsc{BM%sMPvW`()(30s}>3Z8`V53f#U7;KQL0Sxq z_$50={~|ZHs5gR?-#9EATU+$&To{Nu9b`N-Z_)C5%`U{VwO&1Se9gB(;TmdV;Y`&L z*6Ql$jFPo)owiBt74}Ii8p?5Kar@K971;c<^o>Z}9MD#PkmUeOrJW1g?}3tdYw`VZOU+`3oPCmU)RW* z=XSNE?}feo%;G|RT)94SaPYP5Id8dNz|!s;@KV6Q;%zA?mqeZS<$DEQPxRghU8(aB z=5paMW3b?3ton<@Dy@c% zyb!;}TkY3`?;07aW3t*n^s>G9h`zTK|iQ*IZ(gG(_-^drEews3V~ zlzfi7mmE6hlT>6ck6hH$uGPklJRbxV!VSmEOd6(l`)3R>Tco+UyVHqYzpk#>4%?BV zP>2=q`KN0C?gmAd1`teQ5Caw(n!+BaReybp_uKb*)Z-mZT07fb$C_0&%(}MbEnvIT zQCsiW;i$8~r0SQ+Wx~OJ`4&C@(dze!`tN?X{OZkabH4Ze>1=CNyKSSXQ=N-ZzhdTf zHQUs_%ie8$zdqcl@-}#xEpMa)xLS2%*e(aqUg2UYX%yj$eFiv@UkmN%l>a{nN<&x5 z248Ds*S?uNzs`=-`qo-rKN0bS)!n5wzYB%VPbH2WN>|6-G;c4dI>gDkJ#?ux$&Hd- z?~1I?8{so1<*}I!T2Fi9`}Mref~2b7WL;zZ-jxME^?*fs#klEKj`P8dte@f;_{-lk zn{PlzMltSxB7;WO^R+)EaVf@?K@bIC=&w>zZoYi=3TeLsY*ymV9BuIw;ZB>|=$My} zK-eAXz{B9}j-8+N)@t29^|^It^l$0a;wPC!OO159d^zRSMmqhikCL?Ij#zA<(&X;& z((vt+E_eC91MHUUyK>iuzf0b zcPe1O!v4{h;-D=pSJz!lgK6Dd4jYEMIIrID6RPSS6SUF^YSyx1tr-t|U~=l%?w?W) z#b2=8|CCS4!Zj_|Q2eQZNDKhweO{qC$0x3wx@dLV)3IRQf_;ICHcyJsDeD~GX2pER zx3WuqD$Oq+aH{g@^}Tc zDu+C?VaAb(DP)ST*3Mru@4UyZD}3?6i+y0_U}vp%KW{a<`HStsXyvkTR-4TQ=hst_?gth$t|h3H<)-OM-C5SIQ@DP=FW8xkXvc(Zq$6b7xSI>iJbY$LWHKc zz{u(XZYF=RqRYUh!uu*42WzK-u|!(AS65D_igz&YEzK-jn+zBebh3V%R_Rzi4kc`7 zeGu3}vDuPiX01N3fZxuV(8Q+DnUwZ+vikH3>eWrdj+q*&T!})w#w2XK#*eNasa-Zr zdX)SQt}2kkQE_Y-rew9087=(GoFB-_zIS-9xlWlhN{g6X_gh07fzIUunF-N6kxF|{ z0Z=lS*N)N;oZ>j2Ybei0<1}K&u3j9cCE!0)o4ehnp-B{5xwdaNpjVW-Pd(Ii`*Ug} zCZ~LddlGfsDan&jX*q5>yYF47)O*D~?ik$3eUdQ5B*om_M`=)S{P7U0m{BOXiZ?Sh z%CZ0X0J}-#W0~tM(N7lC(HwI!{8`GJ7(#VYF+6&{*ZBKE4y5D~KECe8^2msYr8&-b z=*z!C6P!92Z`_j=TBt*J9b_dlammTYa{^J#uXTT@G`U3!|s#F~c_^;9n)7SS4$sE zxJ4ZqI^+nSD=1)bF)3W_t{mjA zEd8~hJt5Bbe%5xiYr~+p6swK!iYONqNmMQ8RYA4&`=YM&be>`@9H((_`%1-}pZL|M zGIx39+0w>7CTja(B}#^1TG%}@z8YmtN$$MzLW@1Z7n%(dCVPSMS^u`_U@x(Nvwi5| zBY#pu$m%^V(>%I?DMJSRnzlfy&*#oN<=v5t6EBQi``)p|B5M4lHl_iVuZfcBElw{J z&NW7dg7%p;@^B@U2cHibrfdUx9?HI?{P)-wT+*C#9%EkZnuLq@#`5tUs70O83p~mX7TPTsHpXsHt|D>HR}S+u<0d}8^Mj#S|?2FhCoGRwDKDfDy`<9i3Y9r z2w$nd@qty;1Q6m}fj(Hb@J{5+1D}P@2YS0 z>lv*-xLVdzz7=`fwR~eD0K_V6NMWs^MyHU0_dnJ-T^tB}Iq47|O!*U$GnD1teUhnC z&<;*S0br>rj*s>clH&ZQUrr3!4w%r>(_4ythwsN$KMyERB4@|Kie%`NGnu z+{$LTwO5Uw2(P-|Iw+jyA|rK@F^M&j*Rr_#(DSxS>21f`b(rvF6l!~X0k|v8^oTuM zV|ZtN6hNUroIr^pG<50rQKJ}xY)Tsov@v{i zF+9|vSZ3``5bzi^U1h$~*9RMfX zhH7Dy4v=KRH-7?{(8NDmtwau1DLv5uu#JPP@l(=*+x`HE=wYZIW6DNeFY)+iSMD_} z)(1vJB)Gd1t6PeCPJc&4v83iVm&udUvtu(Mao9r$gb!9@ZHBe+SFJ4g-h=tJ3=jgNV@Jom7Q% zs>~;N@%u~7H|bn0y(~nAR~=b#+sGcl4J!G@+cP49(YP~yI>4i=!G>$*zz>&gzIg|5 zeDQ#X>-+8kLXcT%87NX6_FiRt^Q+L`+J(@ChUxE1m3Wc6DnpCbV-RWtAuXBAneUB_ zvdYTuuet7MF(Cq|9HOG4Kuw~ktqqhrpNvQafu)VMw@4b1H5pPh>Xq>h2$!_D%NGCR z$CBW(;xti2>G%Xg=dUgOsjSAlNjiob;!F?o&0R-Pd~#zE_YQB5(oT`LC}l4@(p@h< zXtJ(jNF8*_zU`O~YP|WDdUd{0%5a&LfoK+yEl^h)96} zfQYjWGc`3;Oj43*mq$=g6XZFjVy)_zqily^l}q^o1#z{N^&-7w^i4!Js6kMX&%t1g zZEh_zoZ++ahpoYaN6Ce;x!iFEjq+m8y=(S7xCNh~Oj%rnOJT#{Z1BBz-)VU$JzAaH zX!99ESsi8&$AyaBB#=A8CzG^5^T}4bpglOE^CavKvMaVCq0|SuR$D31?THZTdwY8W z%h&BF<1^=pujlot$>~C&I!mL%aSl4XWp#BEsOEfFDt`dqzIY+R?&-c6Q=?iW1~mDg zDMMx_60WX)tP&?gu3mB^c6jmdl`ZaIs>aBsoJhdCpz)V)cHUsop2z59(K@VozV=d! zn1^-RUCPx}?(Wb_>$x}L3?|1QM$?xkr~N745bv+i`igbh{VH2ji%OYoCKSYy9M-1| z0TNU?86=($MK7DB!MX^edi-UnO#783w~dzWC534AOmJ|U%qONwg+^I1M_hvNL(!rmdNALN`(L>Kphvu zJzPf^ALVb}pe9>de4S4XX6+SjIUb99j-xM{XxC3rp6EJUU!uPw{~@Mm>2aA`eta=1 zg?i~di&0wfx2HWH)Wu>#=&Q^HVS9QMx~u(5d0`$i&V^=k!JtJxCkLMdrRN}Di5gX# z1dhRIuM^-r$}m9-VDVo|N<|ei(sAHHl<*R8NdQ0yE!sAoNdAy@Hz*)~dDx{%yd}LH z1Lak$Q&~*hH;@+^70n>(tw#7n&*FL<~nhuEEfgk{;oTbej7&jZEZpMYL!>utO2fURH>oD}W&$|kKRVS}Sh1xKj zE@NXBOzWrACsB$d>9_Ltvxba*WV~QL?wR6YUl&OnNG0~fzod2OysXjK)3a4xr_JOF z@@OR%h-bWsgTyX5FOenS?zq@Un^iqyR%vZqo$%uv*M2DfoeXuZu|PULSB+sfRU zh7*1=vZ@M^%mpI<<6Y_9^(W_^Obx#n)P8_%6Fc%`w_(l3eDDj8FQGec)8M;Qb-Se? zGfK_Py`=CHEZ8Abn6H7CR@ZzQQIFq978K)inbiVb~aHkO;BWG=hU$b_-%$)%f#^kT&BBo48a z5#W;~q-4rs{UUv4RayM)I>U|T=^b$vQCcZw>}b@T`i^x>_tM|b40b&fsw>xWN=(TM zRejQ4C5ts8p+_T+S>dSus(+C&S58d7;DAOdp8RI>OU-Da>!WpiOqwBKnyyqHA4~&5$=duH!Y-Bm z3N@ro=->+g~#hVY$C{`v1Ma9(Mbwk2OQPQu})I3Y0W=w7y$MY zo5&7PCUT?H$zI);=gO~R6hCfVB3&$Y@H)Ya zEfY|{KY4Y0!F-_R9;iA4g>~Zol9l#5^vUj=!$hn1$uvy_h=;4w_(21AOSLQwc`!xz zqqIiysAoNKwsSq9Rs9vrZ9&N!1ak%WOzx%DG`qgGgZcN5Ob)wr)GzFMa%-qv7Znc| zh_XjXw-Vr76$6bvO*f`Y4fTq`>C^h^t6vDP z7V*+twii3zNbw%2N)gS#EC~Huat2?O^jmb`?!Mkl5q^%% z@vQxWN}I!c$Zb7uM7)ior&B*)=y1$`_k_C>1*kxc@lBQ(N&j7n49)j) z+VW!F=UcCFr;~9lzH4kt@C?8GxZ{gt`eF1feGvzTb|02*+yD8u!PQs$^V>DLxmnsz zb3~303^oV$PwmY%;V9A1u3cX&u?p}nw|>xgfl5h5?ddXEM;-lu;zyeQ7K-&bkKsap z;hCEh0{7^0=y*1V=ngxNX+cV$D$I3$&$P#g=o~ez`1(tvsGGw3 z9~+E9eV$lrP6Y0B$0csMp(9e=MCbc1T~_kayG7eQV^!pe-C>N2q{Z@1UdvrMJ^pi@ z-SYH&^2(MjeRdK==|-v^k@@NAoWuR$WerN^6gLaoQ?58U8ZupF-J%Fa>)rsJ)`P-%m=(CI0I?q+558l?gF)Xj}OncdR^0oglkoi)Gi>A6HDj$W(Uwpm6;WS+Rd8dth@{>IG!p@JY!sqQ8=Q3|*udd8)aveuds4WJ4$9t(k#;&7Rt9 z@|Bd%(llm| zc)pjrU-7t5eRW&Y7IVZcQDdNeJ8SaVG``V#QcLMl+QRb&g}vd>rJ~a9r2K*l! zLVW|d3L%i2>Otoch+ zBdi0WJ3;4`%K2m}KZZ)|h3}PLPZugFvm)qaTDhmGaI>dg3gdpC zw&%F5L!$%M?3E9vwQ!#6k`90jIjN ziGpDZ>EG7}_Dgq;I7%wsMxK8zC-?Zr2ho#d+aGc%Za4{_W>ADixxVRFN}8cGQu$fh z?_y1>h|jrCl+OX9f2;7nl31wYcPCQ9z!AkfeY%wAJj%3nz_N~aS>yuWbZy;Q_*5cX@UI@kjy$`_GK01xAL9_>~cl2;ymvU8HmowA%sY0#|-)QHILYmp}ws3QuYb2o<{ez?Ge~qBRy?ZZ#8Ri3| zY|%^;g>;kq6Sc z_fML@<6M9DchM?;frKbPpSyu@0NTwnAfu*Gc6imS97r2+4O=2Pt_Zr8$J&pd%hasW z0A(geh>T(`vbpc204Z!1gg$3hlGJ3?)Gm^<2tN?i2KM5 za3>IZgC@cLVqv`81sHbi-z%i@BsZrbsKOf_IvOr(Ea+0W?=7xqrP`S_wn>YL2Hv}u z(I8GlmlK3|;G@^jIXr^%C>C&3kxU{`8He-Q!F? z_9#X2hJ&xDn5Qoq5z#(g2Yw8d@zI-r@XMFg2(O@@WP#_2lhxw7`ug;c7yt%gM!T#X zWfc__buYNF(a=cFPz@Ial@d9XqSmgTDDwi}^K-${30k!Y17<940({ZhRLE)E?LI*8 zc^Rau;^LNn#B&1SooEnrJ2k~Xg0HCLG>ZA5rDZNU%$vWD?23lfsJU#nP)jRyGMv$p z@*A4a+WlDh=HYr=mj9_0(_wh7ojB;Rz5fDZlnO+66d@588cNJvOH8z#obk(_Rn4zSfK01mkXYXV6l)Vw~&*#M-y1bBGy2uC}qCP*=X zN36hH2n};=xb~C#)Ti7=*fcCgong0Z!pEtZM9&b8Y&rwLee+SD=J73#l z#{d4Bf6prm`{_HNXS$p-0e6tNRB9Y@`uw|x{^jO_9ra^=BTBxtybWy7qm)hTHA(h6 zImhP@1TpZssQen_#flwCfs_mBbi&88(po|w>r>j8qr`v0BAH<%84rue2o&H-$2_l+ z8d<^AYktnTfS%<7zyuF6tn<|`PTCIqmnge*wio8_eDHxYvU^(S7%nI_N|12LnDHIv zBg;8Uy}9=tqAt-Xzc}iqg67dHyG;?u3SfJho({rWV$8J1p?T zB~$6S0Z~vVwyP^BnYY`Gh>C|5!%1wY*<>Sml_EL)eg{cHhhLDzi2ws2e~YMxA)I14 zIXUwnSVo1gVN&-Dff8Up=utsRK+GSs=~IB32*lF5L1Tmi#vAbF#p+cerNQ70!@Y=k~5ZRBRpIdfi}2duvR)s+p`Bs*_s!I}u9ll`~Wj z)_0J)X`xuQh)e|7|9<{qV4ZSV`EsU!DZ z(kV~yy-J?)d37eS_>M7~?DBAy|LGJ>2(VG+WWQ}5;W?Bt)>*j4Fk(+c%=_d0_E>(o zryt;}GEwL6m+tD*aVYdxsA6GR+B`svgwadHyXvJt=M#+Hq8BNdAal$6F<6nEg&>lg zT@^;i^y&8&BO~}HBq&I+GMMW#NCaoXcG1%2Hp-|OcJ+w{)uOv4n~E^<$hqB(WuC%+ z(x@pZYtiY6O@5{PXEzGWSojYeU4#Ky}}XWiTxj#li`#JRU9$ab)um_Vx?i+q8P}%zeP3 z!TD!)9GnZ%UoBVW4!0BOf$yKW+wMMs(OGGh!udU%b&s zE7u$bwU_qQ2{6p#6PYs@$`IL}y-;+NQC}|x((7)A>DIM4?N!GuZLFhWkY_^`)BZJz zC{#)qz#P!Ik#`Ikc<+vP1480hgdd8z?1|WoFsWO7LN}@bv z`r-A8|F}L#A(l{6B@%>2+fQB)$y60D8m5ooGi1VdTowM4mV#{oIikO62Zk|+a+-iR zcSz-t3KD4El6RcF0c&S&Zy`}hBe1_9Z)m+@!|H3jV)=|yU&T0?;lI6*jUC7BuRaoD6Yu7>@W@H?crRUxQQe4`jSXGN4boDxjnK+BGb4_3H6r%#m>THJ zXHoih;Wmn6A(O{v`M{xeXGf=Y_Qx02tkNc+L(dd+G8wq?@Og$-hMM7FU-^3r4D)M9 zRq!U1ane{J@e_@JRJw$5QEChzL<~3;F$oD>H+GZ)qJ0W>(5x77f3qrQyvOYxg%4#{ ze3~$5714C1%hX^3t$8S|56OM}AcS=u<}6k4T+My@V;k8(8wStOr*yokSlGeetFoUc z9Vlt`1z=)!SDrkPP|inp^IN}?9u~IAz^O9>^-4U2?Qul-B2S*kiM-*c{2^n^yRfn3 zQ7k5ig_Ys5vny)Qrn2k-8HK%eF>(g_T#e-!1NDA^n<8tQ1%(_DzuXuZ^O-$HigPxq zjzuLS)n-AJ>YhYh91+2jiRPAy9|4rvP%v9e-zjKn%o)bk9P!qqt^His2rsq=2eG#+ z(wtwDD`tFVZZS1G(XJhM`EtS~%3noHH{TI5zKgZL=eQxe(2W%; z`6KLTZ$%3-^iW4O=54gq?jCNa^-8SsN%SJG{ErWuvr>ZdIZsC*kc6zjDT%cD_*-jz}kl^4re7 zE6Db`mRqm1#p8sxOW<;>u(e0nust0$HBr{Pue7(`zP;?nl`*ochs`8b_C8JvS$0bS zh{dJfwL)*MGI*hqe4buPN3GBqSeT_PQ$Djla=g^bo9Hw0M19b7zB6qg)C6Q_yUcDm zShkG&_@ujPa#Rrc=Q9lG*ZY?GWYc6TNG|Uuh;}W5$l-jEg|0*afu`gVNS22dmHhcl zRC%Pa-80lEZm%(H4%Pf>zcjWuhQfy<{vA;u7iZ7%2grcLfGAY)3vU|!^vLs>{|@QG zi|P^C=;|^gbsF7Qus{p1r11c1jLsce3U5+ViL9aU@P5BxqJzXjjg4>(SZL4^4V>r;0i z-PPlaDu9}^*MM}KfVzHO$si>w>yNjx=uHg$t&|Ipe;9;TaBmIQT|H3ANCn@KJwl?( z73iuHgnbAhp)|tez^GdI{oKzJDcOpdD!&t{mYAqVki)p+?fm<)&=r&71#xhYufu-C$zRkH8bj zZJikxrHLY!_$gI(dO_be@&!$?*$dz)V{?03Ul3;%s_8J~coq*btNay+NTNQA%0zzgPf3 zRRq^*|AbGglVOco!0GrpfqNhXL`eY_8KeUdicxn)M2ih{VEzE~P||>DS!T@)@^79O z$d~e!(T*B&?!j{mL@18GD~I|0|7&L&z$}K)k~u$IQFd^6SfJNTh&Z26kqj?19in%0 zJLVyWaQN@BECjKh{&)R62^#s533xj2b!yG+XB<|Feizq>wECKZiSgggV(juiy`JxBvG3A7As&XVMER z1RA(yF~a}Si-%Y@>K&cxPr{%zS4?_S)PrE4I+}zv&IDr%Z z4$$4XqkQAWjZ(n+ZGzR~V67`rm)Ry50ogcUcImLIVN<{IBuTi^2#6BR~#y zi#h@)8+RLL>-ywd#YIXvK)|d`Y?Gz=BWiitmG& zvZ=-43K>iX{SBm{o(GiLON?u4YhkiJLe!Zdl2N zDFAa;XjavcBj`t`_V*CtUpe-J6*pWmBqkWb0`OUt|T98OUuI=*W%eO5jML>{UJg6n? z9D&J4R);lRNX&_YL_iNDBne1KGeGGa5sy}>a;<>9m<59Gs2Ogzqw>?Zq$z-QGKTyM zl0<}L46qDQP0h?KfP_@$S}b}(j%I<0Lyqbeps!khE#wNfB_KyYl2-;WgPf*=DIg(hK?)nm6M}$77w{g5(~RQO{(vsS zg6DngIDlXQG-Z}GBO-T<$f$#AvnX5~3%BdiAU9fpe9VLBHqf(e&i<@T8UixtRAk!* zy#o5}XlPPrq{F|-1TZTg+3$Qr?($gf4I6A{QyOusH? z)FucjH$m<+vuHSXU<;-k3mx;j1W<5aKy=m>6~h3$!py^oz85tQa5Ip@oKn+eGZeJO zYz9nCgLY2VZQ6lU2xlZrIP!;q3yB){|bym3@Th4$v$lx^TpM_}-i6uplvTiH^ zx5#Gp$49j&5K$Kkf;!W&y7hDP-4tW}HRXXI=+&&tZus@hCC&y|y&0e`wgBj^pRk2) z&IleqNtTKUqACDxgr=Fy)&lVJLa4~@mS!@v!X!`~F z_2Clwylei4^J-TK328)2*IRiv0r$&I>L%QF%XIKg#aa_XmyxQYs%N`mwniq5J_9Fb z;UFO}ro4@)x zmN?Rtvb(`lT2FqvB>eDU8eBdi4h{rw3`2V#XnI_a54Y(V84>uCU;UqarZLNY0d*cu!=v&SL-rT04N2Qjpz)f%T5{>^s+^T?2TOcfk<*pF#UG z?q0tOOK?$70cc9^4`G6Q4$ou`1fYxVVXp@tj7qt zX@H3hdYozO;_Q4||2r{60fx|<$OEb566KDL4)A;`#Tq)w9jp-hFL3gik?;iQ&5 z2}w!7Fnq^3pr*D~kN9DU_DisNAidtW)N}^9VnDArZlu&6eyy)>Vm;py&7C({DIzB; z8wfm+EVZl~Kki@PK5#}4r~OF)6(UD9&h!3!(f#e^9^fWRot>S<%60-bZWBl)j6v)t zQoyAYWGaze#RYV55Y#geYS|(_KiJGYSPh#^4B4|U&Rlz!V5GkPE3eH6=;B}n1`6!| zx`ji`Dg-(3ZgAE?${NFEy@ksW>}Mxz1!4^xkg-N&;w>yJ9N|H9LTO#yaV}9zT)YET z=tk`Y!DmxVp;#9#*v$&y0D3=M-8KS9G2=lFC=HUhJeHGhU^9+KHmBvPW7iT+Sg7HB z&%2>a2GsJ*9HxWHLAUjqLa_FM4<~>|Dmn$M^%HPwIsi(EkB^^#Y)D{SOM%&IbE89= z7ylM|M224?@y#|203^WEAwYv96TBfr?XAR;GOxZ0Q@01am<-q!7V_Ez^Co;g!NewO6ZSS>|4##&4pmr5DtY>T$lZXggya$b z@xeM0*+@P|S}CwjfOjJ%BJu;Ext%Z(T-F+C5&3$sG6|sMQUQKX=~S3%nsco7ix)5Y zi!I`pz#uLTl_|ke?|@}5jz#>dz|YsW69y*}lF^98kL0mQgD2Uv`2tkhyGmk zeQ>btb3mX69gE(*`$8&~PxN0-w)+(wpHtQ1Cd1`V;+)ssYB()&D0%(P<{>Aj#s)Xl z{v)g!KQdlL@OlBttu{7qjy^g1|Lid|mCm3(Yyl8g2A)S(kEYw=yv)DeKL?Y!9Mk}l z;0A9)Djq~$zIKiM=L0L3GvFs8=;V`9z<+T(SZyFP4F+olgHQK6(30{6DA@0#x&ZVe zb#s(&L)8Pw-@e0%u6aPP??VccRXmLGryzju4q@g8QPr{{u?n)Lp(R5Wq!*FGX9rkp z8zdZ5yTtzYobKY~Kr@d#;q+P*A8Y~cM7vNG)g|@>LfUjTo%#^o5pX8?SHI%H4(#8u zy0T&}+U+I+6&n?zk0(~1LSd!@b|^Gl;ZYVk_4rRl-~&BGx*cGTmDvA;su~HZMW%Z& zr<=***!8?Y!f9h~Av+yho~c$MGoK#?#!AA%!rc5)s8}XOMqf3I4`Tm4BSKnN_)};T zd9r~?l2POiOI|NALX zcP;=xZWG8Rt|HYu7R!~pY|X`b$a;Y#VEYZ1lz7kj1%0DS?-^=p_UNdo z3)OOjG-8G6=yI^I^8_iCI&4v-6aKrOn7a~`0dngdP-LRy?BZO$$v5gZ?*wSsJO9r3 z14{jO`w^Lyu#iwO1;x0DH>S7GDfoln;zj0KR$XEbdU~GZni@U=!dTf?rzJa6JJhIY zN3x@%V{8X7Qn;ZkVUwDs{Q`;tRx=6Fq;r#^bWAOd0)m>6taq=lKTyf@96!-{VUz!N z78R0OY$Tpq#XM(GSx|yYfRZ8LVxTjm3B7pkD8Rk&+6|{29KVUAnh2) z^A@r{ed1Gh-4%mUKxbspNZ2(uwYBx&+}sTEwzwifqC0DDrkp0)#)y@umi_L7;+R3& zMBwCSClJ=d|_CU&Z z!J``$>$t!1n5bxrl}kY0nBIw(5&7dj(k*FxanKuAq1NR|4rKc6J|%^Ul~ zgs=20ZE|{LAtB1b55A@wcBtA+gI6bMN|n3->C%w4~JT`e%s zjDgE8JFDC>n7L$9gG_)=`1sJwATg0iQANc$u`N8dvWENBDZY0|6AZVT21xX#I@tk| z*;w|=dYd>X(q!>nyr$uhBXsZO;^uuX6>N|p%4op5Jg?6uq@FIld!oATpK`s1E(97^ zc&t-E(k|h{hwB_>!)nme(gBH&rTHIVjv!JgLwD)HGEr1~$z$WF(AOsj-3Gj3{zOD1 zx_W(afz3@~pXr!y4luKrWtUn#?6m%R;H=*>ZLbBHDm z;2pZh>wGrB8|yO(|M#*>^N)uolfpYUXU4$okg&HQ8XTvIoxBkka~OZ^X+4#}WCMweZ^ML{l@ zgQF(sQZ?*i%_N>5p|l9$wK0sdZT1*+KB8nae+|2Yx5kLeKkl@XT3+Z(ZQ-$B5uKVm zNFii;xbUm*6@Za6vYrQiS(|E)4^Tmv?l&fb8O3729?=O}$VhVgIm8*nhocoU@_mR1ZFc{zhyqDS}3SBmj9F0jK8?ihgz>>I#_!K zE+=Da$kyxX&Rz&tDGICH6LXvb8mZg;xWtZ^kGZQ@sucjM1ZeWVMtjr) z9P4Z(e9diU?f{Lm4J2#>AI!=|8CCwk)HLgF^MkH#24Gwaz}R(*x`7RVWuJE6i-$Ham#|F6#e_UU%J(rWyJz?|0?axhNe3?f>?z z?868S9zg3fEqeTaX?#QXmDS4zG+3-n8YL!iwx7K4JuBEE{~6o8bVU~>cK>(p(yjM4 zY(?9@1-kWYk9c)GBMq`c<8JIH?p(f{QFqGFFzB{~p<%ZLJ`o8}_YXn4TGVSI&5RK= zSm*Z-01)U5wAI!9=~ttpnXVaZD7KjRUv#|%RFz%xK8y;I5+Ws~fFL2IbQ~#Z5RsAw z0qHJD=@5`^5CQ4#@X+0P=0Zy$W#-}Am}eSg+c7fU_d_uhME&s=lOHJvSJ^(=7J zCMD#1^;H^f*S3J{Ap@7I9I3J2wOS1i3wuIJn%CS6CT5FNJrhJoqN1b}0@$1N9p$cO zIly$1QR&MX`4b|d-LRXXL43p8n>L&4J%=M)D}WfBzGYUh(C#XKz7@P=)^M{+y|f zuI}H1CsPHcnz~zApJu6)RF9K2B!P9J!lR=x@a@t;n2O|C4xA`-sX4kGuQ{yD%()I( zEYLK|fBi z9QD`o!z?Z%6uM`Zh24AC*DUublz-LR>pMfpvB=u(S1e{$ySn#zx_zzVvNp{)Ca48! zV=HtB!kQ)alBQ{E?nY3!i*|Ih} zXTFVmK0Bwo$8tSAKk~`Jp@3U0?M!q03ObIVY8-ru(?qZ$Gxk1(lA0wF6bh{c$nlg_ z*DGF-N89YqBqE{XTY$_sj#O5-)+-7FH#Bu###%(6!THy(*165ehEfcE{(D|Nn5m_2 zrb{s>b~+PR0vF3HDL*R2)_B~jZz1s1uBSjkoQ1<%gcO55tgQ7g0v{|6j<%FZswoT4 zDwTiKp=!STGfR5^6B1G&f@e$&dQY&Rg9C+~6Fa78(YTy&8Yqx9{S=tPtQk`OO3R?R z1J?iVe@-jQtcMGEeOdwl^J^XtKL-T+So(v*Ra`Px(4n8$9W1Dftgkx)A#3v% z)uU2rkGp^CSIUm}&j4;13kD4m68g0Mu?P43y#Ggq?X>Ezb?{}5)cagtsGU#oz&^$F z@qZRL>u!OCg==NRJ5Gm(x9n@~dloXoCf30wG+%;^$&}O2_#!b$_4nt98GpEYyizex zO&-S)ZhjEMr?j-%*ES0Y-rJLx?y;==`BUTUyajCB61ILHL0(Gflj7oMEJZFCUGd?* z)B5Ho>@__yGYPd91!dA64!GtqU`CG17bv)F40-67!5^XcRoms0-FG&$RYd`D<7Z(Z z08&J8af=rCf!>9MauHGbF{C2RrZIeAJ-ek~@n>zt9s-g9NE^abB0mztrqdFq^=Wus z#QUZBUFgEi*`K2um^1%B=6ttSVu9aDc}BNTC>HK@C(w6xl@w08+0|_0Y|;ORqq~EVJcqqHc`u%88r(vXkug<@|^9Pma#?ZrExssKK=|d zI2M=v|9vbdC^XW4YVq^)KNl8BG`EY;_x7dvAYT%lovqmuOhJ)H#YZZ`4@TO2nkknn zs>kFhV=Id=hzw_Y5jfL#)XLEuot;2OXRr97D9_^{*Q%2}YMyiM=Vqe%xJ?H&8a@F@ zQ6<8^M<7YpVV$00hmMlc6UFoU}R2YY16% z5J5=G&XeTl;139}v7`dRyzVO2BDTjG5)x1v>Q=rAQ3Mo>Ey1^M2_z(e2L(&ddp+^1 z5WC#mS1$F_Xny(cC>QcAUG+h3R-e!BOLP4y=@${!_WQ^%5_o48D~;r@e=ME>W5Z=gN6L#V1aL_Z3p#> zF2G`U+^Jd>6BnNcaOE86rEZ7AqxGhr5YD)x|8M~WJjaUl0>!h0nBTu>xmJ*Y(yQo@Vn_cG&FXwy4uKAOT!oz}ht=)+l;8a;UE z&9eUHR&JY3ik)r@873}7`|=C%J=y>9%1aKwD>ny%N|hxjW?8lp8Nk~KVo2}CcS z_R&y%e)jyiOLy=AAXVt1pmB>MJ)|IGWWsX2#(RuQ9E_yuxgU5$dAsPmknabIy>C1r zL{dlR1Ii3h)U&BoI^TrqV4(p>m;{cBRC24L+VT2LnWnCnc)%|HEoB!maqk(uVn=0<-?YQAU+n;A zp_B;(UKA9jqVOlkOiS|ueH?NhJ~V=4*gphPUFQ(XtbHGh@Cf3U;n{uz(kj@RE5KWy zb8%IPUjph@Buv98Z=eXM$b^N5cR)?~Z(0)iDJv}ggd#CZB9Q@u^@ z!@(;|Q3024-)OFQ8=G)x>ATX(M^oA&TP+1cbU}TYj3*eTDND(`$Nlxh#>KQ^eB5T7 z`Zr9<;JWY_7zntdg1}W6Z7G~UCU8{w#bn5o_jY%cIEa!K6@W!^)Ar$6dFtwQ(jGkz z3DkbH{t}rvt{mj7wI%`RENBUEx4+_jgBjG6*akX50JC4!mF%f+tM&;eyzBy>_-bX< z=`^T@eDm$AFKC%jsIp7-W83|UMyX76--Jh4*L&5O+C@?{w zR7e8&^3c&T)5$+ulhLh)8P@?V3q-a+pD;(cj^AG{)s^Cnjo-MX@Dib##l7Lt#?|DS z0Q6${V$>Z1she77nLaMkz=HG}2jBs4Dg?hszc7|)=Vc4Ow~P66C^GTsw0?2XNG1=Z_eOm?<5|Vw$@Q-PJCd_~J_%W`!CP4uM^?Vs(0F;j6 zMibQ6&rp>^>_mw*~l%t+cUsr;o}X;zE|!%JYpM z{UO`&zBb^w96VvG>M`;bCAZD1-SdTHR{j;X75?-NZXZjaC81fTs1TIw^Q%_ksDT`Q z-uxb4x3F;B(5SVDMPVcmfGRGLZ|`w=K%R_=h;&TN!vo54A3Z$tOefroQPCyS(u9#7 zTIn1P8wY;JfUQ>Ff8c)Qcz$NH_xu1Wbj6GNrO9VP7F^x2b2py&Tpg{FhCrrjG<1!x z9mi#U8GFO$g2EHYAXUK$El<7w0Eej`Bnm|DLkj9qEXR_ zJ4W-|jx!hTyYKA@cXqk`DlhNcdP(tnA3VU34NxyUmZNdg;AdIX-FtMv#xy1{x~{l2 zxwYluWPZ&O|LDU9wMhy!D^$+J`~ro~&ypJD;n%p^zt=br2p3uDO*FPc#MwsxH6)OK zE!aeTP^(4Dv5gbOBXBoHdeD@alDj2ZlIj=JDqrx5l@)}i^P3>ll7L09gB+AWyQ#_V z57lT{fPl1V>cagTyw(uk>&tm_H~G+S4w$lu@JrY5e0!6M-~QqEAx znvxTrN#Iq?H+jj3z{6 z6%}<*QKf|?CN9W{JcL38Y}S|~1_y1xi;AI}%wmd&1Sv7MCr8UJafP0Vp2rhxF{cO# zt~;oR+VK~McPNdH5)dFDK5TScZGhV}=5Hw$`t-kXzXLqDx%+AZ@AO$ux?!CmUAL_Z z0R^-C>*r7b@WWrc0MYwC)jD!=F{NlVKTiPZlUGbu0NTOSOVm#{vd42QSY@j+o#F(F z(JyIPp2z(2$yEPD<8W_$7nSsblWVyz!EIapv?-ICoxSSDK|NH!vS>B;m=we(A}#nN z!L0DjYs7_b-cKbaKR#vI_j``_-USIrS$(f+h#Sz>LSD%KuDhDbla-fkYBO1BIcHH{ ztVivQYxrC?0nA4x`QH-*e5Ur@i819-%lzTwFfWx zUQ$KnH%O+W=KW##>+8%$ADdw3hX<&nL=x{$pB)NbKis_ilrwF(UbqbbdT;?UA=sg0Nw=8c9&j$))R1a1YL~CQRMY18#bH3 z4J2V5r+qbNt12M|W0+lewz1~A+j z3&l6NIzJ*11qA*t{|^rSCuc7%F22hL!7>GScz8EAOVziv?XD5Ocp)GmF{E=ZzQ>lA zHp>?LKmaU6MowmB(em!>zF08i-JYP}BYph%886BAPo5sz zn5545@e*UgGPtC+*m;yz^$+JqQ98S%F=-EYcum2}S0Wvcn%6ozV zALh{W-{VEX&4GY|bNkm(l_$`H$W5mR5u5b7EsCT3(|j3B>*$MLfJeBj&q$8CabjDG zK!yx9IaVT%P&8wI8NJQs!;ECiSy%Loq>YP`W$@3SF7v+RTAPZQd8Sjc=t_Jjm^+>Y z=7{O(U+^%R< zi~}Y8-=~tOTluK^=T;^10axs@z(kwX{@SSw`Y0|Qj_i-*RvuH|Firl9RaAmYVTAq}BzZ*Q1wFu8^$!gEivo(pzhqGRuFdJ^UzNEW z^!l~-!DpBvN>@}>IjxwqsVlu_6KVBDead!cCm22mI(?P)8&9_1E3aML++No8XJCE& zSRX;V2S&Q;KNkY#Ix@obQDM2<*@R+{k(E`e{}~FZWuC?y?t;+pxtxBR5e|ThcDB?g zdcSM+Njeqo7CvA$Be^<2H<{G8dJDe+f$G2UKfqBei>}XK107KdO|EnA@1I9RDzoSq zno^5lzIXq=8a>w0#xl2A*^O(cfVC>|m10fiOU_+hFbf4VhObRe+FnJex|aNS&d^J-aAdR}}9S)Z7w%XT4^HuE)v;o?7%(t4G?3vd-x@B5UL2NzRXBf#oN5|MM z(?R{5?cV0CO4h3ay`h%)!pW@!r+j081Rv^>`io5-rosJ6`v3Tqv|qm(?{KIIo&3za zyp+PdlBfa=t%{~J@%*5nx~su5(}t9d03;9=K(9L^d(2JuoVGX6aA(}e^J_3qcI{2u zEW9W@>+5&UF55urD8rd#WvX4#)Ee|721TJtA;MWx5X6^@H&F3Wst+p}Z)gb%@y5ks zlLf=rvJ@srKB-}wJfc2G5aY(!an}@ufLMONr^0$vrO;vD5=%U-$DtEs>MQ>Xcpd=8 zbNpYM1C`;H**S|RIJn`?M?n}5^6SXhP2AHZ=ZEFv>yk${CR%@T$^RyCH1?z{<8dDG z6eY97(H+0UsTp$Pp31!^$v#r`}%-nX{-=WPYjq9s!DNEg*Hh-Bz9L&Ne8q z(LDSP>4B~HeIWN#^zM^ty7YeM!o|XUiykiZ-8G4UL5f@~Ray;|?n+Czk;Di#lREzQ zV_$V+{!MPJ-T=RcjolA`L>$rCwf(l|uX@S?>f1w233e=e3%&wRceQQKtox8b6J=GTDr>*o;`1H*VEW<>cfW_M&}WXWdfS z;q7wkSl)Yk6>&!0#;rc$ueo#U4MOoF$7-xajw(BQl`^!Ov$jQky}y>Z;9Ux)pA5(U zKhoilPoFce4<8Z*s7D0@ zMF9b5Aa7Rr^VK$W5r`zH*-zU_7XuE*GRQAe;5jk;4~IQYjHbDjc!LtNX)T5*pJt36(0)$l1@cvd|j8r$f`i{>zt z>SV0M(Pesbs_cxhH{P18NVaA0`f5jQuL9kH>rhWeP)E7U7%x$Rdw&w^MLy{I zrrF-x*J{XFq?ICTL+)euw6jpANiBKN7pQ{6m@1PD#fKammYeFTo;IYoq<70|OYO4o zo{e~7>~O{~QM(B^e;FD<3r@3AdNF%?kp#V6mxR^XEw>V@(-V@MT&m(!%+bQ5Pau_pD+UQlLER2N5MhL=BYPq@ z{W%qUT{d&?o+aYLlb@i5)A+Rlg;Dar<(9b<%C6sf&LpCCxWx%#8v8{+7f3SyU?F8%lz} zB?iZ=_mAM*Sgt@~+*+KA(TWMVoT!LTO?fwpkd9F(PGpOgc#}P~3BMB~kB=AXiU!Jr ziq-6S0cSqt`S*@rG_XU!F?0b~UjqcZ5^zob z`cC6Myz_H(zhJV!0`ood*`ngd)YA7ea94UffBaDFv7B;tna|45x^hr1M4*;2v5GD% z6qb+xAOa1hY-PD}MXv(x8zYBTNcIH%*E2h!wY!U&klqdToY<&0K?*!d*Eg@-{QLj(7ixN&I44s|F+rF>!tz@>swu8-bWk2rlHzRIr7W$p7GSs#JasDWJcm2$Vqe%_oa=k@>zJeX)^}yyp)#mnSEG zrVpG-is$oe-1cVGr@DMi?jR^m4N=>4qM&EFByZ673?gorts9W8vFp)V?G@!Z z?51GA*R($H7o(JgJi_8bW|OTMkjWvp+S-5ky1qb%bAlVQ#GM=&Q}ENyR+Sa1hwstD9z8N_lbNOCUA^V z*Phn*iwKEJc(jYfKyK~h?7DSz3HbE!3y!)oytah2Qp_3+cOIxhae7C{(K^@lUV=Xk30gb*?? z+$KAF*5nRHqoAO9_Md!RmHwsjTZgkR_wO$}#iWMkOG2RywT_^B-lC0kuY)Fka(cW! zCI4zDLr4w>0S8C?vq8q(Dh$Vca1f(jK@C*51%a#aloIYeJNre#PsS$~D&b=6fcCn2Dt+?@S_y#i z$T_W?Do2qCzZy+d5>AB4il%jz^pCq))OH_C!Y>)dpczFW%0 zbZB z$M;n$sudCUA{IQjPp6Rw2J*K_G0`x~9~0QWu)D<9(Fs#fd#2GxTJQ>U+WQ6NgT|(Y zGw$SiYn{rQcgYv(;EsHzCx90G3E5oA`2l&L(@orqB4Y|74*h<jafP9)9H)?6xD;i_Z0(*tpP=^6Da|V_pPvJ{azoX#V=8Ha^YsJ7aD~1rm4% zhXjv_zy4-GjgMM3xyjx38J_}8_5FdC@LYGE%m^^FT3 zrBj%z5O;WGp6T?yW_y6Pz=6ldBql9=ObOae z(B?j-Ffd*Hfu3G`w+x+CV|#pfo2=xPnoF6n(Rv|2`R!0Dm!Uh8U3}tnR!!AGD*Za? zNPI>sub$l$eg|z&N~2uEiwBJJRpd#FMG7Ho)zdmf{osOn!pOL^Ah+y%{$hAI=8588 zBx>qEAW>L&{JEqKPH0nUD?F@lkBLD&J3HebxmqTuJo-S$j1a|S{*p0MUg>k8)w1R3 zY<_ES`q@SU74JuhLi6a0CINwILiR0mj3%wFsJi#6o6Ia}Vv}&=b4jFBw$*08ZrnZ! z%F-!J_i{WhGEp~$6ATDkG?!LAk{wUO>`_m!*m-v3O7O-vDn!oa2C-sTP0+9y-G0zO zU4IQ+FEEBQL3)!19JwMQ>Q86mpC~Ue|NmK@6)5@LmDlR`d+{P8YiQ|JVGd4SgCE3X zeAd3eN9`RMLW@SwKtiQ-tM6CG9yE6}y*DTs6zuDRsk=89Ld!JUJepSl3Pa+kA`?C} z>Z&Fah^v~U-BVNf(5$IoEANrik3-7K$(CJZyjEkhqd<*$4zAkzX)d~t)16q+vxgK(9f|p`*v-7G`@F}L3vDo0a?epC{uIaer=*M}&-B(~>4>R7ysfK) zS5M^4IC#tgb4&AbnHV#5iiCE)uYawjbXME*|pF@jfijP z!vnm=!pwisyY6lwl6>InaBvJ3cgfeenN8zz3c<6&PEclE(xPfOB7sZh%Fec}%)v z2ij_XR*f2)=4JT{z*BYyPf^JGaMTqChP3!rFuDd}-(u&-yOHwalT%~GOKGjX<<=Mt zZBFOntt=o0bkK#xNv_hrmt`d>oNS?&os$#GZA%CA>t0Y%d4WWv1P~fabm4Ir37c=$__`-ji=aU*{?(cUU|}52M4ZM{YtbbJaK}bfy*gKRb~yf}RAtnzpmx1K z;#|1?pn7^B$Tb~#?8=c#CSx3RSvUhmL19(;FLBBt6bo{N4S_(+K%?@7E@&-NbGtrv zKApB-T3U*UkrA$K0aIKI?jpE0ZY4sWa1RfW=6C7{(Q5ZcMwrdXyu3=S1Ud9e8nP7t zp$?2DG&E$^LikRKj!xh)Eo)C!gCU=w zuz7XfSEz%8LFBRRO%O9w!$CzOGz;B38bTazB4k~X55US@&3R#7kg&FBa@ZTARnri3 z`;LEJFx@$6D7t3VxDOi9XMJJR=L`ax^Sk0WkTYBd-Dly8p!c$s83T~(bp`3TKCXUS z+d7l2i2Oid8u;-&2vkTuI}3x)5^M zP8G82*}Yy)fYb@X^1Y@nDS8uOiv$rJ(Sg)jgzv6ThZf7XFYbO~G|kFnB~DU2Yjh1$ zyg-J60rnd3z zzxC0dq#aXXkV+{kzJcn6gh%2@H}*Fi6HN8?BUi3%bgAa4JHb_B;Tr_rkeeVWTz0jQ zuh%2SR{xY_ip~_A;C7KfG44f- z51n1a6&*b<%4ErNLFI=)4S_%tgJWiGf}P@3(9CY`GbUFAjEir{Zd31)ZwNLmb3kC+ zf-~_^2~%rdb4G8fOjho!^_JOXFp&500m|&|d6IyoXXW8APpoYN0~zl%<71(#D$7Yb z4X#ba-W_(2nMB#uU$!}p>E`4<@9c~QqbK`V+WK4D7MdzwGp?*NMYFD9B@eDo_BN|= zMHH<3*;N2Xvn#`Tp6lF(k(Va9*Spu7X610y`uPIm9hanp(D%ka(9fatxsVSwIe8yo zppUPefo@mAH(#)s>aJKhrYwZQ?fx5@YxX+{a1vBKJ8?oJl2749nC*44oTwHoo89HW zu$VLMX^5R%Uz_~L_Tx%pS(>yqJ_h!B^PK*3oOyzwNg9PgPE2Lx44MU+`>0Im8?dA? zhp=XUf-AFHJ_QBdT5B2(yPfW>DYf6nTjJ66^@`?0NiJoXK~49%qUGbG-p%*h-0z+6 z*L8aXO~KmQC z3>gA*sf4n7WbgABR&25iyue!B=~X7-M>dCywPxT4wtP7%o8PVH_)TaaM+n8y^0UTfdly$<)jQT_S@d}ay3dNMCLJU=-i1ciJzU984T z43F$*dn|jz$CSO_an6D})AV~#U{xrY9_~gA-}3tqhqd+#>`2iw8&L8aIm%HC43S}d zbdblA3J9DoYOB9aek3bR_w`ob*FkwKC%(=ZPkU45a75AFdSW3yVDNQqwUt4jEPPt$ z%AQbDGdD?z+{VOL`0FWx2ocEsQ6@P|zLRx9^u~(CVm5neN-IY(T zxt5er@tLm@&staIRk6KgrEq$u6AM2+BqQ-H*sSHgR!cUw-)bWd^?X8^)p=;UM@wmu#>%Fsg5yhX! zxoX`86lHcDpBmUeJm>eDQ1>2vaUhG@nV*luvOZkDT}r>|yjWfADH5nX+SZkljC&Vd z$e4Fh3mLWB9%+@^oaw*%p4!UoxL|cY(i*Z0kz;eeFg2ka9fl|hsu1P%l0{M zB)v^OT)6f9!__ofPR0IBNwwj$BLlZ`SjW#AvU%H&IJbIGblhk48jkF{eZsRrdWd{6*m)D zzPeNrZ!FNYh#eanyI5qZ-MdfjdiES75_u3sIgkxTs|lX&OvAzU>wOSAB{{ATQA@`S zznlbIz2LgKy2_4sJvOP0-Z_?`BKv-xp^%?+_`FHv0^rN>X@{_zv4a&@Gx zl;pyZeABd0==Bgbd)o8DX}mNeF8ra54e8+{ee-;8gPB>03csB*-`3Gt`xy@J+_8_H z?%IhPA(0DhspebH7}!0=`Yk*z^yR?M(@cZCfdq4ya(difAEOL2n$`Ng@s|CG-hSiZ zJ*uRMl5@)=|9nVPq*Bx2?zVQj_`WdaP1TLs8zRxS&$#sVhHH4%2YCX1`t4m<@6V{p zIhW;EpIh3X8J{_xJuF$+lvpV!V)+%FldRunxu<;Hr)zV10i`=RnxC~sk0%E>0s9p% zW>R43h>A(OU;RXd@!_H$rs=S=m~F=tNReV9s= zN^2YXc7pYI(gk#4>D+Qpt@hfLWwOg|h^IQGfRY~qrTyF@yNt0podqp;Frr;|d&M~x zEl1k^-M+*ksugFbN`AyRj~V;@!PHre~Gs`ue+*Z6qlg5%W`_UW7AB>2e&x&?bjQ(eo^QnBOi)n$dB>m{n&C6YfLUvVM|y$^!?uOjez4|jK;r?_#TJWAni z_T6B1FkWV4$0Hcz=eGWKK*cx7~!x z_LSq;Ou6v4?vNp3j5Tu33UR~E)cYifX;FgP6t75o z%9q;cc(De14T73c1(sp`sSt%;vFz{65=IXkOnRBTY%!nzGC*L{&gVF)8&7PRI=$vR zPk3+iD<{i8KZU!qVbO5XqLtH+cuwiIM75;GFPdP*VzfON@wn@^P61wx=F1ZPacPYg zHk+iHJVY9^qY;a~@dA!`;pD(H?#~tN!_nU!!6jhQR z8t7G;1ju?d>G$e(@u1W=FP1tjroj0DE-ubg(&pnwzrhiro5gk=TbNtMIwWG#dKsCX znAN%KFGtuVnXDIDlR~inttqp#O-_ZhDJqel_x9VSr5-BS_py+T_vq#>RlctvFxG{z zWLk{l)ntR&-kG?Dt2c}4{M%C@uq}m0eMKrIVwY#~Ckalt=mB(UeQgOOCqRITVQ;rS zX$Dt}{$-9^QXddxBJNQ#ygVsdO}_oCZOP)aut>ZTAe6Kkv{LSQsPO6Y=O$~n#dm{1 z(e;>@iNU0DcQJ$`u3+$FTeoj#n&E14c5EmW*-#;8}+MIJmd^bleslDL1|~A;9xrqEPO4YG9w4g=NsKT(REaxOoDB<9bA3sgMa5r zvT1u+#0!}?tiD!GB5(UT?aEMKjHOX=`h8O@184G+-SiAufidJN^2T!!^;z1DS{frA z*9NT2xcs{F;yb&uy3;$@yO4$q1D_wNPy5*mj5#7%wJcUIplJ39%}phTQ>(Vy?!m_{ z3ig*t$OgiDFsGe^i`iWxk|efZ&@-ACZw`$GV*IeWQ@CP1eAEFRhH&t=yEaI-+sj#a zi_S|12C6r2N^ii>1xT+D7GENIq}MSlZmMg_uNPV?oQOyzz5e!#vC8ZATGND|Sv*9Q&v)_*>$&6+2+RuU)3s&p(k5+P{ZXT$FvJU7oamLOU3-5Is zRITH$s{vv2{{?6W?H;z5rMDi)7`86lVz5z9L8g?GYi|6u?LK}aT+LTC*vicOaH#M2U-bI0s`+nK&&O;P8QIF{{6f*?rUtpVjy5w_<#VUy-lGhXPD<>F;d9Y<8j zltZ~v99hNo)L&eI_tosLDp{f~=n-3#>%dT^#zm;MRFVmx&--W>5w<$661669`4m~E z7U>RBEl%x1jju>T9D?Z~^gj5C-Fgb_TEbLHhxiw~< zRfmb7R(-EtMI}kW`jOj~m(;{fP&fH|c5Y}qfy~3rH)LGpl5~3aig32(w_u&?-eSn! zR>>IuZ9;^q+4JM|jG`oaGGP&%*l?jiA*@hVTRQPSPEJcCLkrHB$i_@HKv?Et;r z1Q{AoDxx(=u=E)pKy06`tsH;R$XI=0l>i>P#?dmT>>pbtt?B_yI+J+?|A1|u zQVXi9ex;ww@I2y2-<8>G&F8ec4l44AWHArY9%t%$TBfR^Hnk>Q%so1%^#S*>GWag7 zK-UnQ(~&Z+5_wIJD^G{5%{ISH>^5A3sSa3%X7iuL+0PrX@~D>--^)@*Kd5V4Xs)4D zC!HcietXn{WP4O;TJzTeN1mPs+d?^$*a_5bIJS^vbqF1=&KA#y5fz$10Z&QuTTg4} znbQ3^!yI=nZ^Ee(i5GQ5tP6fcx@+Bxbudf8wEiy2;;t7j}6x6K?yG?zS z)J%%%o7M;foBn?XwX54%^tEQ?S#$v*sD7d&{K{I#^ zsgUs0)P6S}OooN?^jgX%aYwSQ<+^g|TJp@OO{w;F^)zX~IA zr6=9{^l!CwA=jpOJKuB(njkrxZmCw0LXJ##eRkutHr%l_40f^GIIy@l2)w5w;zK*V zvBGpfI(#$rg>Y}$-eNJR&=8kF<-TrfOxVy9bo}~e|Gi=Pt;#g7ganlY%T|8C@hA!J zK$R*G5nlQNS|~ziQ{iqa5jZe7A3hf7m)%v%N&3`6c?SfI^z1>?PUC*6=ebfh4cGHn z$(cGYd)KcSc=FZJwOo$Q*FCVQ>y~M?;a!TPvnJd$8L1a~u4tS}rJr{Muwp3D8a}6%GW-i33udLTvn9Y;1BS@E82;(#mFb?R(1P|9@ydmpjhk6 zBue`aj52}YII|&3$+Nce@#e3(l>fWNdp}B_v-1Mv;LXD-OKJb^m`_9RSxj&&#Kg%dN2R z&2OGd$W(|+4Jn{&Xh^$8PRdp##zX$Z#?E%X;nB~jaqGI~bsPx4ayqX=ZAY(HS^s>v z)br%#&ksyp?!v`sQr=_Bm*#(vtXJyC4dN4(o1Fk};iJ;7B_5H9<|-Ydm^4xj-fvlZ z)7T$B=D2ryL>`__Js{?2L?(axnabN+62ULvBDJI8Zxy$h^e@R?Sa?dtiuKA~mDhFP zX3O;}2LdREB@v&JU?pB-%SX#Bm-m@9F@L8vH5FlB$xR-b2@UCj@zm+hk9uDn%epN` z*vn{lDX6v+#x+3gPqbZzJ86@XJt!}qk~tzKIX}*?Yy{P;nbm#@&m+=R`GW<%d_W=H z76HDk_MG~}kRvpDKPF3Mvb$!t@wUW%*HSL({I+fOwI8$9V{ANWk}~1LAJ1ca1G;;P zM;b7V4A6L-C%dgwfyg&3*M!HkN+o_omUkFXV2tcaN1CUkI!2wg>qEpB=_`?f*|1>l;Efe5^HD!D%A>UxA)Pv9FXs{vxLhqV zPl^BNU8gE&x*85n+e@BKQG&@S1ODGumc@5p0qOzJ-Yxiu_tLcEkDRwR5smE4NrlzX zBc;)sEVRpG@*wneMi9f~Ju|Bk0p49aY)BZQ#W_nfqz=n(^A2T0zJ4lUOiuS!B`{im&}9G0{`I@+ z85^y_=*g(&(Z#Elb(@WykX=PN$Iuz*iRpRl_0rg4e`rYXOC2>Y%sZB39~M#>2Sij6W49Xd4;mOlp)RRe9wm;<<*b7BQH#o3SV@{5E1yrd1C!O!Qk? z25n*DKpW9|b@WNxuV2#KmK3r$IJy{^D3uPM{vga%Wdd=m%8(9~*|=W_pCZ41$AGI& z#07_=e6JTm;iGk#1xc&N`D%aCEzK^CJlf%_3%jTM+4>WmyO6o9%j(;BY z*hjDZUG+9>MX!4(v1@l2LUSemVjX7ez~qbOcKIh@F!0S>$vrv@W1u_RHMp=-RDJ$D zaHpzmt@-Vy+=pWIcn-~M5ZWDYUt*DR?s2pvo&IKMDu@&mN@p{6PF-wUk1+eA0LH{g z$VOkBF|fd~d6e-(5csN$vRUjD$u;3T}Gy3T2aB!k5pZEYe7sLUC ze}%pt_I&CAL;jL)V;h?aepbPcJi>d=hj(i3Wb2LS>G|sEA*0CILqUn9yDcu++;R$= zPcK=(EqtARV>dEe%4VO#80+RA96`qN(sD6UA!7YEmf44jezQ225fkwb?K}6?Yk3uU zW|`hkN>Z||2)-ClhU8e$8*0NN~jIm$!ZvP8hFjcx( zw+%Cu5@GLtj~u-eH5g0}ANidyYb&o}7mHcvR=HC9k4?r!WM>gbj|>t=ZCp0Z8n4x+ zaKCq$BLMI=U-f~Ui~SnUAE)UWG$Udv@r{Hjpi3J2L|rX6`Zf2LWr-{$Cc}N5_1eH! z;IGE#hTWYUHk{CXoAPsWpV)5L_>u&Z;}KupMnpy~7_mC+a2diF*Np@=T+n z33)wXsZ}I&Txm<`9o5CK$gdV_p0>TKuR7%C_AFe=pg}<&_oefLSpIj9Idc zLr>_XTV8eh|Jr-YsH)R14D z0@B?e-Q9Kf4|V3hZhX3*?ppV(ndJdays_UM&))BTvR}Pol}J1#*LZOr7u9mV5;`b) zfb6+TL4WKRuOk59~)?!C&3 zZBko{o8W80?Rr^vuBZEWAl>d>i{j7c_Ydjzg_|&wp|*U{Wd?l|whd{x5VXOwerH}C5LW`bY6X7v^YC#^6pk`rFMBR73%sCVV!5sDpfSX?x6EDGUo<) zHRt+Op&8?BQfT6W1x^Q7+hzSICPurd&T?64n^msh(CJTt*aH4^-DgoTuU&z>Y`^s( zYUlmVjx$1i!0))w3jA4b{ShH#vf!E>uuA&k`(SR%nww&OE{ve#Yzq%9BfWG6M%`3l zTD0tE*6ocx;@Cn3txz9r(gx2Tdh@+egmxRV#IbYXiY+sKy84}0^9?*>qEy@8;(nv?NuOJ14n(vrK+j-kwtbBPh8d@MxYv^%5bJ(ZT=ybPV=3vPo2UMZ@q0M; zHv5`lXW!_lf8UCEufhFf34tlw?M z9!mw%qAB>KpdU9hM(!s{hCyGV6p}Ww^FEJ0aV;p{{yNmaIgt5F>@!Ie72Bat(9;XD zp0p18y4(~;qo1Umg1LCQe_FNSUleYAfMePIj%qMf)7+@bEgCf0*%Lr2*DyE^+9EzM$nX*(?{1fK*(q6Q8Bz=BCBV4T< z)51tY$IYW#bKB*Mo8SBQr&LvWRkR{Bw|J;0@Y)SUf*EnRThtY^F){Pel#E2?=m#?|fG*u>xw=Pi)?PY60?o_YUi36O8AslFBh`L3N$JtYjV5lTo@_#*`i3%X z$osX4UtTnM<_)v(W-VdCjuIr^z0Bers|_u&f%~Ly6d~_{wEf&rxzpK1a4qjj#0s^S zPD?k2!XA_j4v4!Q=c(gQkt+nyxpB<#0^y=w*&IzFu|-Z@2W3wg&U}z|RK{68{kVPM za(&ux@Se?tq|+UkYFI?hSkclaK7F-Nw*WfNVZVm~em5;1lhXTLV@PPmu~y<6W06>< ztK?QUvLty-;xK)4qkh?>qMU*63aW}pw;BT=$yG*w=v7VL1O!LZXU@FN@xsMHvxO&4 z$A$v>mw`3t;nestqh)nmNeN@W5?A2(Gy#;mT*}(IFf`cQWd7?fm^y0o9ndANC_s16 zw&%EK$=rWH({P}UYg&s?*Y_y2;L^^j`}F7wG%{@0>OtsNxFWNfci<|QJT#chaA1Na ze_W9dTwGiV{aeCV6Poo*fW9fO&!Rs!Vauuk`ZGuZuR)GOW;|1#_ImaHZ2YGdyhhxhm12@$6Ll=9N0vsC|QW!HB9pdNyCa^ub(UdXQQ1{JS`z(#6`Vr!K z?xXU@ls^&}*2DNX#YWuoHC0s&H(SQbmlIdVlb|7IV0L--mcW8zR+v!bT^yarXZ#&S!xz-~DMDU=x52m&x6g0nl}( ztDYdv2H8GEiw~$8T}Xs?eiXw`FhH$XFQt-zP6q#Cc2=pHh{$j_DD59YAHMCJ>A5KU z<_{BQcfL#4D_NLjGO5W_wkTH)eSZ7(YPKpaSuf?7#kWIu86S3v<41GE4cZYYAKiS- z26+M*853KCt^T1pH#%7{St&01xfs}f!hZg2Cm|!#-^MQ2h5S9}287r#oL!z$9&M9l zp)e#kAt#r@Vfco_DZ9Wvjl)vsi@^~c|2}TtqF48;^Sf*7XYCE7Mi-oADp3yw=5W1V zTI-+UaXV?39YIY)Q#*4SRpHHVR{QF_5)fLpNn&cv6i1x)8meDZ=fyUvb-hlf<-C00 zu8OtKx_?!_JZ-#v!E}@By2gNZF!;V4ZV}Wywi7Ng>z$9>@zJz^d<$Z0QCYcIohNsH zCY#K~$k?xOjC{*(q$a>z?WCvR9~{S0(ya=up~UT>W+m+nLEK?K0}E z-5y2S@(cT%fX^*z2E%Qj&fv)^%1STj}U{!~=V=%SEEoyL=(VYuIb zOaN3I1>gM_SSK?Wn>JQdQ1BulNs2wXnbD=12pcq?9hhCI=DJwO&V89lXU6P)YQWGE z-$*ZOT3Wk70J=x#){x8etbRTv2}vy@{Z5&Y+o_t1F2C0vjKLSuGu{ELkxjq`=inbH zOAEb=I{9BFfs*tt)~L`Z8^Z$@X$IPF4i+7)4;Z!d6VIyY#;tr9OQ9&YZoEgMb#cCO z)k@e%Jwh;R>YJC{Tl2SqUi}F;Ih3s{gJW82A2fUPm-mvUXJ+zU`B0W2!4Aa)XM1kn zcr+i*aFuCF>B-mZgk z&U^pfIXK8d9hhBaS@H&Sxp&mF?U~VQGYRdy(^Aj^pg-?An-bYwk+N;Qo)7|~{0znG zG?NqQrwE^BJuK{|Twg41-|Q(TyWE{=YHRmwqM!1{-A^e@A$E}hCrov%t_7^Wh(2hOpTiPFD*ikJc z7cke;lxl0WFOhOpEa3`UopU}u4Agg8UgA66?8(OayMV)VNvEg8awzdEOHg)&QUs5p zR^Hbx@An^WJng%=yIV?j{>M~c?EWs=X=G8Wu*NbJ8%MWgC__DPdeWXR7{cb8H?y;y z#06jlp&vdhEDB|Ufi8PjDX_dRTN`Ix;g;*KYX7)c9Bo1S%7t2@Hu4RLhgCtIQeJZj z=2^;j?GQSRxw*C4)Pl=i^a$&o#U2QrHU`7Yx;0mucraK zc!;)g_2tIlHp2Pao|K*Zd{D=9+Y)KJzBqs|;39%vDv{Th2ryMfjw9y{9UC9}-ASZN z@<0*V$ESV3O2%U^s;dHmRQrP+O7Z(T4nmuQ4Z2|&y`35)i6kY9n~^fZ7y%#st9Poe3orK5kX&kg=-pi( zIjkMlbVNWz3HR*ZImjm`byX*cIs}SpX$`R|DrS?M)^iqhT^5|h$bbS3y;G^uU#8m7 z5=unn$Vo*mK)(oiNtNDw$^JqatwQOtnVY7y1hci5j& zFs#CYy5e`@e};i2@0j@u^)?t7B!Ec99By5|fes3Mz#t*=0*@#Nj}Sx3%;&;1y-2=T z;^6zU_}gvgho5Z5$UZWq$j+B#DueQmw+iTn9`!@Ee)bcu%+ETYV$7$T8GycJq--Xh zqG>x(SpP7ts_U-^gCC|A16&ev#y)pSMjQ3-BIr(y+kdJWRWtwg8lA^pnu?;$nnoR( zf}~YYq_4&3+U3j-{_gho+0Y*hAOf15&3@WpdIu$-4-pW#Ka;A;C9al>Tm^;qJTfjs zHHCGgX4=p6=61b#b!Gi5?MeJ#!hC{wphg`mxO%MibF`h-!um!sEvx5~#g-{B!FZ8q4wU8ZwssB#}y=+1eH?*#+9Sr-!6{s!7Sc z;`3(|hjh84#}h7mh^r0O1s>V1%@%Wf($bxdgq50@_Dc5lz5>^Gsx00IL;<)beJfrfpz^&?vk;SXc62t8RxcXNzYnGm2bOvsPH7V&t!OUJbk0UYC z-&5M3Yeb0Z0&IC4^8~RgUUEa!{4EZg{!t+gU_SJwy$XzZzi0Et=WNn?b09WO(qyA~ zEWg5N&VgU~Oic#9ilW9?H~9lqRkfsBvE*r*l_IvBS2h-J-7U zXvF8XcX-onRc1%$@d+C>ViWW!R1FoA_;i%ZKYK5pM=f8t@N-W_Tc@Ial)75h^JtoY zkm4iP(|hmy5qp4Ixd4+9dIEfJz+{#OU88~LF%d2w&t{h|DapzCx3$5GB7(aZlai(P zw`G7aNYX#b0#Xmb&6V^b(N^=Bhb&9#ldIARYKOZ2KFbcir$w2v0k|W-OHDQLqd|_a;lo$)Q`4z z+{38|jXcm20WaYDwpG;5t0b9FGDP8;yw-YDDD6jzmP-L@fEbHAh~Sk!SWX53n!MBC z;1=@DT8-D?Nw~S&DWH9?@6u6Fyir?sjhyIY#qWm85}bLhuGl7#kielO*L;tcd@Ca7 zEI~Ke%KPWftJjZxx=m;N(PqqBV(l9!%PW7BWsNV=8X77fLJ_7wdKwyWdBM#fp`#rG zV6>kB;wDx;N)**9q_TevGfj*2$*azkfQ`^-cOW%P!^q0G=yN_;H} zJ+^l*csi&Dx}uHfmyKfMyuiX+@7^ZO#=}|G{Cu5>Nl{aif%5^Lq`!M|(bI&<>C5SW zKKl63kX`%BAKZWC#$|nXn`nbj9oyG%4KgS^+*ITys>)>bXYGlC*GgCG zT~u-@y>w;9?Cz%l^Vk3|Rdr5OJ!8Z~#Zocs_1Y11$Mjx4gl98&Lt^d_lGEd!x&mSv zR*Z-#TES&kG&q=ogHJj^gGsISTXHwYRXFO|t*z%98X_`#dsQ@<)DmuC6Rvf;V#Q7V z2L`GuDaHT%`52*M8}kDxu6Q-H)K3cSkPy0pi6=chDO^|oK1ZauLo(LjNvkcFVv})& zCt7Y2puI$9C0b$SrqvC5h8v$hX`noxz5lCcn0C0}!96fAMpI3U%tP7j?XnR7pTgt8 zy%$)c;|TLbG>^?as62nsrijSz0|z_k2xVVOqLic8UOZA(b>tE#`r{TWf!$=bW?QLns3;$_so1%0LFz4~HCpuPsZ-*l}Le$imcvE03n~psxK|;-8Wf zT~T!SeTx8nT5*fL@8x6@tb6rC;XOr$a|08#HZ1>!G}?i+vqY)(fYBqal#3glLs z^>nfHK!~iE8LxZqxZF_hAOVvD9Y|B-;zR(6P=zL6*T@k9uK(O8^JhUI*-gt^1JGo| zVnpwFH&)3@se6Kn#yvJJuIin6%>fO$qc1qNmqM;2FQnv=6f?+`Qw?T;i>R4Nx;aPF zX90Z5rGf+~Zs^(CR_4&gcYNTLZcsP>&fiXgm-j3QX-H~jW<6W@f&FLP+h)xm%OsZ) z)9j`v*aZO5MUC>GU{TP%-5ok5ry)EHP`!eN|4jkDffu-oD`61FI9qo!cG+Hru5htm zZ9it9hwWK<;%Lc3L*eMGxUi2BOS;oc3zYQGK zHTE!yB~I4@(4+mdV;_CzVuUxAZa7pWWFkMcB_9hwi_bu{IWB$Y<#@^FA-es>M>W77z zT2x&8+B^R?dS7BM`uak_6O)ZHegPRpsyv5zH6Rj6N=}|D8nDdRd?C4`FH{|I+BaKy`B^E6!({V%pK-r=jKR1B1CuP@_TW@F|7cUiVWl`^ zR=~q-n9XG4lYT)&b(HeY7Z%`Bp?K?$9Ytc51ICHyKjY-KwV(*Mt||k7wXa<>gY!}#q9E?Mvg5@?nD~n%FpX>29#JR-Bp&~51E1H{@YZlvE)oquU zBD1=Rsh?;TCamFi-v5PxtC2R0@iZMVQN~vsC{zyILJfZ%n8)=F5j%l409JwoZas3< z^7jx~Eu-wWhuR?mFCNj`t^fq1ns|@l)(x@nVxu|dEgOgEcGcQQKh?a#`)o3_`DnJZ z?rHd^%}IQof56G?+K`&0I%O~;jj&&=t5AnB1)ZacNRM|!Pg;)6Sf_-K;DCXHgY+RG zp@{kT;xhBSdw7WZ-rkk1YxSZdigMsq&1Gjd(4n?}bSP4=0M5vt;kBg&a@ePuNB+G> zc8;2X1uGq#if?hGk7&)?=@nzHwufkx7=D@0+QrRX}U`woXC<96E3vqWWe|p0q|` zFp2F-vm?jZZcCk6XK&WsPj87A>KA-k%=m_}vq((4aIsenD;nogD~-sr=~IlKwBxYL z@ug|&g*vjUmN!&)497UNDR>tAX{Tl6ow5q7n|Yh=YN9A_aBzs$)<&wTsy?z0a?Sm9 z2x$(;$Od-I+LKXWd*_0oaI7WjK*V=ykJhPXfBNZC z`4V&UHKB!%=s}Bnti871?2<^qYK`Ku;L%?k&`%z&JO)ohi-WbFk$s!`LRH8rz+ zQ+i82be;8%UH{v)jr`ud3lsJs8Hp3s5|U{)J1f2z*}2v+G7y}mf7Bs_+D`bN*v(~U z%=eu?s^+Zoy{w(C9BRK>J2lqS%pk6(@t;vUyz;4+n{M#zU_&Sd=~b9EnMD9ffBZIt znfw3wwQE3~3t1nSfOD&-nPnu`hxKk$<0f`7@VLgA8zDl#<@~6<>)MpV&Hm zk+A7RU{(Z?U^Nx^(-ERiNW@cmY$zg3RMmT3IdA=OYa|iQo?k+eh6stiCqr=1mC%nB zI{jtAcsOxFF)A#cX>oB!Y~k1{Dg|w=VH=e+lvl`40VeoPe+5LJnYi_gBC zLe`h@f7bU8ZZ!vrk&%VPs5AIHS>s4Ga$cR^jPd)>$pwgezkH*M$o??d=GjMYgu* z;vxgmi+l^p2?_51FUhqO4@xgDFYv=X;21LAzRf>wuII7Y1!=QF$}cYoJKB?E%yo+& z*Anzu6!DEOmoEzefnQE{E);{oo1pn_7oQLi4)D2s1i~|j4opEcK>qFjDcP1)9R9dH z5&bL426-g>b(7k4DQOH5RkOn5c-D0(dErt)>=ELr&jRjtl;ERITKAHWMZ|@(N4qXp zhMH&F@hrfTS!T8XG2dZz=N+laqrLL1z5SEV`jfqbA0!L+m#g&=b_^si?)ynOsP zIo4J{mDH~*>ndVsn*V4%lhfPc-uZQK{)l%1_p*- z2PaD{tf?I^vphp3nF0+uT4@G`Y@_GQ+B(|o)YK(aa%h`q{L!Dtw?nuOC;;Q_`8&9{ z(TLh&?$@~v7Sla82OyKBUn!zr?(|i5qqohx)8xALZQ^sikQ-eZ%79Yp8K7crhc&aZ z7eER9T&<(x<^x9B(bt7H8PKll4@~Qb(ya8it3nX?sVw5mohl82)9;_Z(A7^#E^EfI zUj6E7Q~%KV-Y_wwP3JTZa^E9 zZU~|vy0SQ&;;wGHW>tX%K)0EhAN1&avHFBEp=7#>7so1)tM-7;IyjX^%8HA7#>h~w;<|@2G!{-tPrqpCrduR=a_kCDL8S~l z3drEF?rkc``S0r)G?LEFWq?qnrAh%yp;o0<1g~=5z|eLX>g}|3FMt^F0O1r`#ikw| zhSooxP4cI(LRi>EBe`f&KUpVe>+ORoR230Dd8td-@X;&RI+VyZip zE6;;M>2UF51w75zMLfo#S?mB;y-}d89l$AjU#JoplyzcRbY`>)N9|ieLuhH}${%4+ z70&8Hw#6PP?YMJHTCzN+Wpj!zEAs(AjLjA@bdTT;Aws5EPE5 z5gX$H?HjY%Q`q%g0@lgX38g5LA*K*LG9GIm*r~+l&#hFH)}oL0M_6rnwxsy%?4Cij zISuG>aU$Q!?yb{Z<3LrT?JR+OpLcisyS5YK9han$)n2|VeYh{fZd?|DYgwhax4>R+ z5pr-K=P9shm#+zFIWj>lKF1%0d!HY0qg#Yo#3io@-bM|7f5HX;fi}r`n_$OfPpQ&U zD@gwHh)eXVD6AC5$6L03xYcOLRUBM@>3V+O=H2lA1`8qu`whYVszX;FAKjbvi2$4# zTSy}VLdEf(`0}VY)m$yzjPpOjZ{ePj^4j%N(PRL>lk(l_)dLJz^|b=$`9C` znfJzg_k$zLr93?!0VC_OEnlOGsw(a5M!7cWWh*6X>*t;aHc;Pp^7+L;vyN18az!?s z91OZ1f^yxxC4SRQGt(VwP-(D%fe+wD0U)vF4lghl(mpBhxvKP6XqHV}=qYho*J;5S z4PBpbZNo5y3=^E5Nf(8-E7a6-tE<{j$3sLzt2Wmz)96r7Q%lDB^VLg2{?w0Ri)3un z5aJHl54+ewsoI?k2fQZS6&Fy$?|!MiU&n^cu;J24*=&r~?FGl4Ioz}~rN{B}22h}C zgcVp{Nb$V7`qCBpLUTHwze|e5cNVRZ#Qo-|@JAP1%qmULu)RiZFSZshl3a^KU}!Ku zfIx&d9YGZXrrPUoZ0vZFE4{jl{hJ}N`mczn^fF=Sjh3+tzF#aiR8Yn7dMFd7|FRh- z7|_Gl5JZ3FtCo$K-7|n8jS9I2nXHS(Q5;aF=oeydW+%Eya0-Y?)dEGu#yy9FP&?_} zdMqf8e^GB(bdikrA!_Bn0wpc2b$=Z1Z}jw18G!p|mDR_^#aVvf9Y{VYAw+pHAT@-J zJ-SZy7F_w$NFpb3VxsH;c2}la&+ZgCKI$FRSK3BH1uChZYBYZjGn;V-f{DSm*u{bU zP`t)%+4+ypS;Jfna9?Zcm~_iJk$pxLVv86kvP3LdL|j1Wi$I|MZ<4h}QV_aUab$YoF{+tu{D%n|~9ANa>lPVdPZObkJc1Ug|<9JUkR@p9P1WgC9{ zPyrUJ%^DoynV)_v-9X$ExuHemD~5segNvFbR>7TXTowwT7TZ`+&OBzg-rs&p!LKJ$ zn}_e`_HUuc0uV5LbGAeBEGd&oXCaheX%(6US3x^7X3f0&Zre-26I!?*YGBMD5ZlTus$(V&lRnlBA3O^#lj-5@4o7jsg?oYx{Ns7 z5c%~mv4Jr-mg}nPxgY+V(O$Jg(`|5CiyQh2sWNU~fqbt3R(oGhAqaHvhcuopo zWd8N28Gv|prvw-kOlrKbAmbWv5(fSGt6%s#B2-N!p#Sh6tl7U0XI{WW|K~yY7#Gp* ze;%#ZA^cFwtfOzQtk;?14h~x$_D0fzSc>$D}0jn6nVT$qG z8N*LVz*@0}p30a$i|!ie76>wmWBBCBccXj;HZ~4L^0aL6CobD;2GJ=*|;=P>$lMO;Fn!Gg=YEna+6uN*4&bsf4?3C^5R zeDWmGvV0M7vOaj`WPk zZO8F<+;c!w)d^L~_HaSj?DKP6gm_skM=xvai|;iGVsGEDyN{k%?M_{L5O!A-%Cl7< zg*o*jJOR>AS^Cv_z+PU}b{@+)4*mqW2{og9O|=YFxLm{zHQdB`ClLo16nC0JxIjQe z)D3;50gOm8j&b=0fTh6Kqr-j9hczf4U*GxTqvc~DRQ~8N-)#yR3~6cUb_|WoTmREF zC`3GA(>p8OoHiHwlrz+Gx}lwm1N5EjDqDi(CF3x8k0d$Z%pkEWw0Ef}b6Uh1c-1IrKCW=UrUBi84z@Ttzye)NSVX|$#9lVX?x98RE*#83+${;cuj%x z>f^)B9Iq}|8)|g%uxrNN_HqtfRMr5bpOfbXN`4iX&S#xAXC#&U$2P7Wl$0a%@aG_hB7ttC#-o2 z3>!%u7W-8(LGRzk-Oet#2j{&NoajDGE>^*(Z=G+7o({~x_4KyZk7DNjfI(&r zk!7o9sLif5vRKvkdaFP5I~5na*He9z1VQl(%xQsF9eup-tJBRDWaj}mAVK3XNF;Io%PP5?Dzq2JV! zV0EUz^f8^V^jdWQyD;VVWx~kF&Y$)E3|t-aoj!Z+5WJG{J6b#d|K}YUr*x&;b~nT_ z%gwJ)0wM-xKGoG;$VG-8U^#;;xWfCV@lJIplmJ;I1d7^C*@k6Dee6R}}DxDWN>l_YxJA{`BDc8)5}6 z>qamF$Isc$nUhO za~LETA3(LM>smb>qA|n{fw6+m=I7sVijmWg1ouc$0g#1d1O;767{^+2JrW`XP`F)ZSt=ev97vhDH#Iuzc@z%Gs*7}T1@=HB|Fd*DMp;8646;E` zHc)e`&~)bls_OfgHE>?XHLupx)I_YK6k;LQXVMVFK*sv`Z$!T&jH6brL;wE>AT2op zVxHSKZ~g-wxqu2-OL8J#1Vw|wZW#;|sMcjRRUn83i_~7OpVb_D5RT$`sU1@3Q9vs%Jl@;s5_1NRzmq>`$>V%IJKYaKQ!Djdd z%)axdIWO}AcbY_Gg>nGo>CV!Y1n3mq4xn60`Er#uXyWu;<_To{a0n5_?*KZIQ9xi2 z8NcG@KB4E)nK%Yg7QIa2!&&*Vb-3UHjU=y8;IC*v$!viV>@%l~Pjvd8M(7<~Ekp^b zYiOiqWS2<;i~ZigdP^6~7IMui*Ab-;$1)9(OweKdYPW%ZAM09SyFnSD|6VUM&BNeaz}~tF$ik>`YHhZ3T2e(GIc7h?)bHh8Sj? z4y+YwuqzQfHu2C2Gso+2ULOiLU$uQeImdyCF$Xk2EAPqc5ZIFSnVy_j#5zFni`m*l zy+qxqQ7l@yDEgF+G$=sq;yap(v}za`(TD3N;jEug&QZgaZst&%4w)hKtnuZFb^jkm z`A`O60t+@gS~j2E0hES18EH0?4Z;1Tb}8^#CJ$jPsoB|;VJ(rQa3N3(;+{Hnh2qSL zGi^%8EC=5ylmTSP(V*44z=uqQyq!8SpKtH36`FN1zPrNu&xv_N{)V`YQ0ab=RyNfR z85`Gi5|kCI?SvF^#puM+5}=4}!EBg;)rtJcLJekgC^PrN%^osN zvmmHq;ee0@{0Ot%waGnjFzvy+GF=)hm5@C3@I6%Mt0FiS>^Fzgl8&z1Vj-ebXb^P* zx4vF&Esx}sl<yiP-iSs7kXd9vtbJx~v;q-VxO=Wc!@c){P);xojktrJ)`iBir<1Fy~`-fYFMokeN zF*#QCN~lyWnC|5CwE28@7L=!rib80-3B5whVa5=RKW$95r{^Npj(WiJ5Y!#DJu_=5U*$faQ@%J8?SBZSHaA$-;w=J$ozhV z$zMnj-M^1G(f{+CAO8C{c=G?^%?GmJo*{7y!hb_RNEi(6Cj=K{f}Y^Bu>t2CB}6S5 z+j}7Bv!IJYYac5ju?Doh19t*2J5B)V!50H^XxZ_)fF78%nQp$8Iwj8>2S%8q7M*)Pze9bOQmbNLH zUmXU>S$A-7FsHM*{$H221iBdtVcms zKKEMyRSw(+6of1aBNG#n^8uJ4)CAPYo40Qdju(f_-v)P8;;k6JIE_v5*PB-_wdh0J zN$=AqlZm{?uB3i?CRFso#W#OlIHl}I`ngl$O~Io#p^i7-skPl_VpbB1h#*y*8%@Id zil-aEdv&!#q)x#+3@iGiO5BC4u zRW|DV?d9X|g*m$ATqC>9PHZNvaf9|94@{wKsK{z0rg+4&ym!6dJZCb(Ji8uT!_6Pj z{M;lYB)g6JM;N%EFxh7u+q5>>D8GMPk(-M!7S{v2F*}qcY(Tn&XBys-Gid#VFh!7V zrRtutvT}#l-i=?9KI+v0N4x{h{veoMx=KBD_0X3uUqX(B6}=NE)0R~ZWca1phh1b9Re?2r~oLLrf&O{(a)e-=s?9a z0du@u=@vwAhDh_ro*aGjVR}Y(gU9wz=+l9sB(Rk!k#qq@OVi~6$5FUyI}E}kncH1 z!N;*a-O>VmqWwIOPll#Sb`a)qY0F~-AP!2Yq#X4yK1iYZt-^(#HVf@9z&$lOs2W8h z5hjG8iDhMch%6>Qh-|O&*r#5ul(p*1Gt7UwuMLz4bpn(at<@V9SP&qdYgBOf9+5?md-B81EV98X3xosVV(-ZK$u5w%IK*QDU zL9gROB)gAzF{Jw-B$0=Ohi4E;f(^-KK~~YDy^46tT?2jw|0)au$0;~1nz?3%0*_Fr z(;?{lqAGgym)M}v8QEvePY*?r1SqmWY2ed}iG7i5-c=-`;0xN@+mnsv<18L>)UMjD zT%H8Kpapg--*&RWVx;OY$RMX`Zw@I-PRq`g^CMlZZg8LZ=*wo&`w2A5$BBLLqEtYH z)Xx1S8_q_9XhZ0_7Ym@pByxlSsR}l13`tU;caPshE&q}8QRT(7lo)AP;5Wk4QFd~O zUvvQ3=%;EU5D!-`@@AL(d=uriRH|8QshjKAt!X?t)0RNDC=8j!)t%=Tk7E{IJp-*5 zy5X*1U|<05&pH$Z8bVyf3p13-X3;6RtFG&y3O2JnMLt%?eYMsUD$qgPLa1w_N#*KZ zPlv(rLwi6lXKH5Fz}_NY*KPwUZMVtFGY~A7Auzzx^uV%)2p{jZ(2S_VP}#sug2Ve$ zkq9(p6V>rf9l4+@2dLNwfW@#uov4bsdP-W_?8Pkn|5VnF$~7MAhxIat)VpKEUSWV} zO@h5;N%PvlJvc4^_{SJjXKf22R}f@DznWo;!38ohvOK5_hyrzl#>hqUCBbs`fxDg0 zE8i)%;n@nx^+ZX6AMnF4RJVJ<6%?o~IIfeRnCfrmRA3-rNmkF%17-$kEkIPD-adMZ ztc8EM!eXe56H)YOu{>sDfQp!MO#$y+BcN^X!Galr1KE_#&BbN3G1C^#Z8eN`3ZH@d zrK{Tz*H0Dp=ruwN{l2WMOvic5r#9idP%nOz5Y}&}L#PWWq;F)Yu1Iv;>N8H!F1Bo% zJGO+NR|4WGl>-4VD;%7hhH#H&_ZxSWVy4w#3G2Ie@2H}JKqaWYFy;0KD^=RjfJo^o9K<@;m-jxt$HKw8 zBrFU_DZ?cKxOA~Gdk239B}x5DgVlI_K~y9FL!q6JRUXJ(28ZegxPogSEBPAb2WyQT z5VJCA7iA_off6%?R>)O{i&mG-Aw>BOMj;=p?Bhk~a@-6c(=d~W0$fo5TR8=^fin%Yg`iw?Y-&lavOc~K zz{|6MsCDSRTn|PuQkUO)R0M)$&xTy^Hz2-~u^scdY}A>mm{NT`F16J@Pw@V4BdTmM z@%;nRhZ*pRVdX6*tpa+VdpAIwh4bLh1MM&~EP)he(0Tk_Z534WCMGA-!Zy`v!FC{U zDFl!y03-H&dL-t#T|P1y?{}vG>H7?OcgZc3Svv$A6Ju}$uL5*sW@feqDC84Bi$I+8 za+hJ-0WZT2Tv{Z&)&!zVz&zi_9&MzcF@S5{#=$%H9kh@VY!j3bVfFzj zz7B|&Jf^d!$GG>?BiLh^WVz@F@IF$qv+rmZS=3V;FA3t9(L%4N~3pcKh# zXM|`)_{}X}Vyb4`%R29YROJaE(oOzsQW(&UbPyjjfwLZiXp!b9-f(tfx%?_Hk~$Ek zYL+`bCA)Tw9nvU+z#%#ddy+d1(Q-3nmgGybp$j;&ieMw(j7bXbDMHxl1Jg?ZtrN~^ zt_E$YhAjpqoAv`^52(3^?3EFEgt~m^)t^lIRRV>Dg{7m<&jOjj0)|r%4k4Ax{aW8? zgh2#(Nv2jIBfzz4jEszk@_kDsECgl<@e=~6K}YFA)9^qbm`IL~zlpfWFralne~*H; zv4trA=n_<1Ch76)zd4Pge=P?J)o|h8EXDu-^w*L8|HeA(v1o?5p8h6J5Q&T$EiQLI J``*Ln{|7mkyH)@I literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp_movement_2.png b/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp_movement_2.png new file mode 100644 index 0000000000000000000000000000000000000000..5158e329597be231d579b261c9a056d8a4b07b05 GIT binary patch literal 117675 zcmeFZXIzt6*F7A@UVuSGI!IBJDj*=8ktWhaMUbk1B3*hfjw8KDmyV!-^dcZ#r9?n_ z3pD~#q(xdN0rKA$I&;s={meY?m-pL~UuF_Ra+P!T*?aA^*FOH&RTW5&(jG;jP^5}i zWpAKRhZRw%eanXr!Y2V5CyU@^zugtZTZiG{e%RO#{(a=p)jM`56!|^mKaxzT3{w>9 z3`$Y0t+a{V=G3K%G{a4t=PDm?A&s;_EQTM#(+rq!|JLCHcDZNeW zXXkSq>Mn9W1^Av>vi-SHfc3)}JC?)yA3eM8bFy6av9xd%%g^e}R9Ank*FC&iZC>kT zUlDQ5iCr&IRa0|z<&JrN*3M9jt0(u0a@Sg`HM?nCItg4H@+gRDIaTrfEehp5c82Zu zKSb_lKJw%96gK2Wzdz3Ya`mSlZ_gjOa^lBZX~mzzf4oIq`}NuWAMdZ9_>JwyTkr7y zJ2VRQe*=P;!~a+!FPlpDn+`d6 z3-95`isAKDE-Nf5-4Cs;Lnm8e(ADdsbPNm(OHX;-ne!?aD?tv+Rh4hh~zF8(OA|1cc*xYV9|xckxGmrYbm%&yUE zE!&{skkrQetKn?=u3uk07k9CJKRd^}=TPQ%#I5$huP_s;Qah{8gdr@WX z)L4%xG@3|b+Ih%jP~%?a84b7a?sF8?p2JMJWY7B`mQAr8b3aD!bib~hN)VHjWSbOm zn_9xvUi{m;eKrI4V;aUA0*#i&8`Skj*X9C|yHOSW`tqYOzq)||lP`m#R@G7?-4a@A zmv48{VE1fZ&DJao!op_Ig4FbHCsqAiR;C(Y^2R%4$b;3qwz?(I584vSyDKzvv?wx? zT+MQH%6LYJyX$B#f;Ex*cD_D?6k(u!e3yU=t;E%cJ!p-KvQr6XTUtpbiZ8;vKNmE= zy;+5#8r&iMy366Nv9V_}$54CK%nnz25-KVgeKLXHTN0 zpa_-Pb=I|O7390$9H~`maR<3)7M9&<;&!slRIEwk&N`N88VOxavaZrSXKH8G4(pY>MON1_W(2 zX%-fih^G2q4;*7r4BzMs_3W7&?d|QYZfp!;s8pC*n$6B{^xEn7YWUI=G0~;O+bZI? z+{BJ{nNFml6SDAw!LIBtnND=l&)4AKso5C6+I=aSTc=cE_5fUsqv~GNq_)-6bI{Or z=+&@k73{AELp_Ca!xF|zhpYmvy$C}R)8ad;-4qlyTVFHQD|SD!h6a^YFIX-uxA3Cj zU~t95cJ0BiqP+-ZUUbO7jI&E^Dpsv_DY@A*jtI6r=VNVdZgyqxA`-Anrg8X$vTD=r z^flT!GCzyqw>Ey9$i*DI^5bF>6B9cJ257(&6ngG#Ie&Y9XrgMRtwqEHJQ9qP&3L^e zu2Z50j^O8ad8z)S(buk5OOMIj&n#BV=FT}d1Yu7HpYj+dzKosh%$PsM6yTJoCAC9og$|7UeGlxmuUCnTYdEQ&l zuwGY1_0`oW*puCDKgA34))04a3UTQ+xa#%AWdVU$W&P~S_Wf_|zjl3mP+T3td#8vE zyk|v41qt>foZ>wFo=ZzPgE$MAzM9Qe>&C6MIk(2*0Sot~K!c!5DigKDb>gVNF2*QS zPDx2ApJ1QAd|gMUvZY=8v-dv3wb|Z%c{W`y6_@(KRtgOp0yY+%YUAzZVe$AqJx0|C z>ypVN(V@>9hk&b7sxB{%!7J)o_l1mv6L!;W!H5~yeBwjb511q~q{#;d9Ab1nljOEo z{+!Q9-u^%i zB6)bSgt&_%G8UV(FA65HDdy-^3W3WmV7~?+`P!{;wtbT8aprl*pl`e<_xxFEvq>$r zx>_RS^0?pIt=a5}V+I>nB4exxy$xq$bWad)wO-<2c}sH!L{WH*TUxv6xG&lz2lBd) z9kcn!>^r}_vNz4=BK(O2FQPT#cf(pAOayS|%^Xo=Q|!v#ts@iTN^+fjy)h+ROH;ei zz&k$X%M^lm_nbS$<3%kzfned2YXUQK0&tJsV_`3MP4}I88pWyEc+Z#N^v!f-|2lFh zaYomf7jHMG;t$b>Bhh(+4!a>w51)yk+aw9|nK?PbG8#yXgGUhk?Qm}v8(2{23Wqtv z55FDh?CAKJJ;~W{bbTYfb~8RRJ9}tf?+>UQwY)p7W{e=+N!0f3NKcrek`izn}75!O~mO^CTrDC46r9`ODx& zknbX=J$9PQ4}82JEghW^Smsk6itoR;8+tO_`^2}UP%m-G-KEg=`TqWXG>u6DMMk2d zwywuougyfT@aY24@89+wV`d{cJKJ+pEjU6|^a2tXJlpTodU|Xx$J7SFYP@7-X694) z&QAAmzuxzWTzm}0o z*Fob&3o5{`rf`~#B8*M3f6fu-7GhZ@CpB$V%Op0BbIw;@uU>vDR6J7d(N{9Eq#Q1L zj-@%AtvURqNv(V3(x<-Q>Y)S?yYWiwjumpKpiz=9N?6qLE9-(vedn>Kdl#96v-BQe zWg~Hmb(rgG0N`OX+w)re_U+RYdZDcto6T1S+v8NocoVnViNm<+3Z_CEBNrE0xrQXp zxUOeS^~AYa)-z07@;2AH-!ssin^Zh;zmWU7dd-@HiID_BjHYD(XJUbFUo_8g_nqV9 zkIhsL&!qlLXjV=)&q-dNRO~vbIzSi9ASN_BJ9|{`;C16wvNHx-2cPkJJb5wRIdk#S zZ)_=k!;k7n3&EV7*LpQsS;lJ!i-En_nhLV@(^AAu48V_ZPJ_*B!>)6=h;u?PnyhTq zv;@xor0Qkn(VaynG$5X|7cZEtYBr~y^WVSb4zO-ya+*hC?6=FKOrJ3-#T=XyR00o9 zRUA+8o1t@)k4q1eU$q!mSa-VIhxHT+wL3%*84zz@Q0|_&+C2mRAFjamYqjsHHrnC| zG;^3Sy9)H6qqvFi?ANQ^53v=buNFVoOJAXsS@T2}X0iEaDW{uck6PupRxUM*%P?5O zbv)b6@$O1-LTz>ld%J(7L$L;e8w#7ttK_+zt4&Fv*P_m={pS2>ZtYVP6jZv8et}po z35EbLI$=(7$zQD=0VTvW10to0iV70eHs(#F=#W6;wDmUFO7Q-DKC`*3Qx~;ry~Yj@ z8Ax0@g;SI&WbUSN$8%!{=^-JBN!>}U@;Xd7Cp7+XKoUxh>2UyDDs~%Rw)C1xA2V6r zE4E+opzGpj)zL$dMbpV%H63SBa`JmNgbLyUOP1C5+?>=)vRi2tM#BnE5OD0(&}`61 z`l|Yv!EL_qDG$kc{;h-diWQok=k1xFL|W<5q*+#8mopcl%QYQbi))o{aWs;^l}QP` zvonXN8>6jl+deljJbz~TMi`sOpY3*G&=A#ez6W8cS^8j_1e zkicK$LgLz$7}O~mk9~MD#c)}d@Z&88l2p5)M2hLeM<00F=P=Cfg~ieh_!mJ8?IU-* zjf@n~-fau?Uvyh4>rOq2PfpVx8YgKFOYjk@!1@o74v6+h2lfONH zrwtAc=Cd4pqqXWUx72n7$7L^qdbfCug`YC^Do6NLj)=VVC`C3(MPm*Dk5J3?itY)q zpv~}82h)vwztx944oEz3d0E8i%)QVM=B+j9JF>Np18VS`E0@dicbZA|i);<@s)e6d z3jZi8JJ7d&AC^-krd@W%1|WLD>~2ju81ddpAv-0(sfen4Gx?gk@b!b6u)tA{H{(=T z7p}5eDzRFsu?|HCzu^wHa4R*j$Yh&r%6mdNf?hddv}&W0e(vNXAzES6bFw_&sF}5C zgC!y?S3JUh*^gS|_U)M)6^wyh3Mr`)DrwaOhe~w%>OAgIPX11*c$?+o%*+nWzPFB< zR~Yw&oy_qYWe#|Jkmq1ga?{EnTg4UnkrCdnqs#OyG}s2M_6<~H#lCY-X!fDJZ=S*vT<2!vXoTK_9QR(MP$|0@v4;hoUhn-YC-RDz~Z`ykIK){hmh`&$IF5*#7xFM# zk36f`6B#Afdib!#arrppPU3?@E(N#weM~pCxAMRa?05>}i)Ebi@r#5>8%tV>5k0<< zExRVC920kx(a5n#GGngkl`R@hz&5OHV)@sT$xgBEncVwD?ed}y%6Iy5SAX)p*V)r^ zYvzPEOPclZTDGs+<;)x`&1QkqN?jHTLZU(r_Lg)6PfILjv>01aAvo4o@{sThrqh6JNTYe&4t#HT(DuB{{L z^IUh;YplyPxV)7O$$!jjcoxuGiZMXl z`)SRo7Ps!y4=vYvhUOZoNB9TWP^t^Q=HOu0kneq&mFcRZ(LdL>9u>inmNQrz?OS~% zPX0}a+$G`l(YUxRM%Aa~Kq@^>!Q0L~qk)Nh3=hTeE8H}1Z8xpR|fuV-6G47DK3`o9trgrsS z0a{0U`Pj)@x8-|!zF3u%Q1tdvOioh7D)+N0lzJ^#SLOFVP7uKKzt@dm)1yP7BD1hW z$?*@zj_Dnk7X<2#`t)3XekdSw%I|hJzi<$y0v*qw_ugMqJ8i8ucc^dOT5{#V_M{Z? zgk^q-oJE81<7Rn18b(YgS2-_*R))QSpZ%*^E7EHHO4F9;qwI@op9aKAz$R{ThrIh|Okx!6p~$T3cP z>nq3P)NlwGKuilIe;SemA0#C|s`nGbU{dM?Ww~=%hYX6 zEnFYoL}rVDPRurs6I%`v$N{%1-x_u41z5mOAOA+PbyTuyNuC%QpPBQC z^s;@eYHYYzAC}OWR>?So+UXEO!X7dp-eG&|3DCUe_x1EHDS}P$7X5V!%0sv6XPkl; z*Q?sn+Y-LX4rNI`xiNOLh&#TMW7-3lvOZzg?(Uk0^RUeq)BN z@`6g`ugCJMPcKbNIa$v2TOZ?}=bHc4r?Z1~TAdjkd#V`0GP>EOcZeQ8(qB|GZ{zyc zz=A?`WE2e+Wl9tYnS~BFO-P6qb?$1rC3D_*Q$2XvTBwEwG1XERop6@d%Bc^`RC*XS z=mZrkJtT_GnT2HeH}B&-R(hgA|8fOy=r~ zn&hxN-hCVsri9ny+Uerm#W!S?afF0RDrrVH6*Qd<9n3Eg3q|4#EJD($;>XvLs~NdD zwOP9I>{{>_T8^mNAG%-`lH(uX!Rcdd$f^-1Z!wJV`gjjr*c$JKHJZox9XVNQ$(|Tl ziLGXEzN?9OpT>c}(rQ0owCncRfep(Fhv!gwn?r>5};B$c*=5a(6uf5`Sq~kSy?sjalg3p*%vrRYdX7%<|r8} z(E45yscEe1TLA%p-o|_t5^S$w-}@dp$&!_IR6vdTML?+&>Ok6`%}>tAD6RG|3c)G% zks2`#nYFZ(X}e~(@{Jg*?d_kQ+wJHn-m2HBb>q~pC9KU14-YupIHB%vRfDUICxj+g zz?N}BUGNnL|72a&i7P^Mh4<>-Jt5aAU9CZT5+)DQgBw-|M4nxyLvSX!39_gN@o*!OdOEo0|EteeOE`&bg+nqSE$&`ycS zM2`_@sm*R9WrdU(HpLTB6P#(b?)QTv94y)jFU8BxapqPtVwYmvh$r%^wy)LqZC`nC z>zr9eq+Dx#Tc$*ch?%@)P+UNNs!%0Ahfr-eoge0Hp%VpRaftHK44DYMhduGVFP~8k z9!`L|_|sm6qrmBEx1aE+-fb3y;> z3%9d=`^baZ;&xIuCrkB1aT`i9H|@C=F|KvOX|tBuX;UB9h8VFnw_su2q3iY4P)O$& zn3rNfZtNfsPXYBh*432#`fip;whlbfX*yx6ARVjnvyzZ~lg(_k#tWlg#pu(n58J6- z5)^D${n~Bx{?|RGWqz#rg_4Jd2SU+2<=J{KEWT7I|)n@>A4-Alcndzb9nh!8BW6 zz5x?08}Qr^i7iNvA)SoX}KVYyB0wbE*5Rhap|3<59Vf1dd(lbeu?-(L}=H{ zC}*d0@MEdPvFknbw4Y19 zJ907=fU7Ey9whl}Hb9fsvqK8=7LuLuoZ_|szr2nHbp-~>;s>n(Lspf!?Q=+4KlsKJ z$vhn#9Are9P^hC^Dy*dzO}w8AcC>EZ#IAF524bABd6e{9ZVXa2PTyWsxH?Zu9q+QB z1;>p4jXe(h#mUr%yEpA#TIzW*s5M7qO6G{NU+!c`&rG_(pg8dE_6M+~ySOi~B()K0 zLmgPQljoGFdrF)}95m_eJARQM@%Qrsq|hKTop6!}+%3hz!UBAa@7AlIP#1Nzb(|QD zF&Z<>@e72SG=+6CA$m_|Sbz)I_2Ook3)lD+Z>i;l%r^BFVhQUlBGHbH2hQ96deX}x zB)E2%+T2R_q$!~fB`0^H*H?4h)m@=AOim-KxU>TG(kKx%opIi-)DrFBjDT#AgH9X9!<{;1d|pv&|^3oyMLajjq?MuhAx7wlS2I1)53z>aP-uK;!ufw2K#i4P$%-&wxKsG+m(l$dx8xP-XOI5ugLp&=dDLl@n@ zL#cNkS%AYyDK8g++J|c}fCykmJBP8g(!Vjkc{Fy#ivqQrYCV6p>&;vk8314g2EBZl zotkQo^}1uNt$HP7vJMWimX^e!w>Dx@lFxFpve;zEDzi^*$wMfn^t_yU4o~dZ6+t67 z?9M>Z!V-gyW{R^%=Uit|>MM5Xu-9!6n;f0p6&Auu)LkCkXWJc0tB<^bL-|D4>@9HW zJIL_$^z_J@GZD=SU%fgD{A^rYTq5>!GiRme0D0vYzB7&Wj~I;ES5bG=jlZCypD=bq zy>R98drTq4A%-_@V^8Da{FEPTOv&n7)$ZW0#9Y4~8=%~ynj1X$x`cwMO(NQ|MZP5< zI_b0Qx{dmgKSN{t852pI1AO3^PpVUpb* zs+HT`Ko`!6Vg0u%Dzs|u{!-uWdAjhSdcDVb!x~g}bY1ZS6y%cBOC2&4p^~&JO3|`A zK{SP{3Q_^iQ?Y*7ojh|!?566cT?`sZc0s`D`#H?905WQmn9F%PZ6X!;&^!?H=7M2s zoR<4Uu)5dG3n)b~78)zod+nOkgPS}HDW3{Z*igZt0A{vcvz9hk;DPK;yBFh})anaA zaGo0-zAO(u`Ef2*@=jr$4C*5{ z*xK6Cs<1b0+xYPGm=S({pnCU{!(#b#qs01fgg_~jqB8UIaSn?JTP-|df3r=?$jGP% zm{cen*fr+A9S?Xe<>lFi$VA8(mqv4V9jod6zd| zMNGU&?;fwGQVr5E8KG6%mQt(7tcL`*35!m&OYPWgPd5;0)|g(0o@M9q$I@@aJHMO_ zbGBGEvRQf=Xw)V)T^|@zMa`M7Ph)PaPglFsWNp;<>CV#pM7w;&NHf>&`bF}Uzdflp z#830+QllB&ER+*P1z`x%YHB2*^4mRE{P?|6mwk$P^G~dDGBPp=&g1?zE!<`GP;Y9| zOY&+HnXd18rDW6^Cnz#OW#DeZrTg}xkOk&vC^A&%nOj%{0SSrxu&1||^v(`=X`7FZ z#%Vhx_=kGkw3g8?myz?}u)u%-~8alw;9u;LzuP#hKrI=rVk8{(fJs z4pqFcb;#?R8S;1T+>w<{@W2n)Kz*?Rip30G>rP%&6ck8~3Ke1jxO~F#t8f70W&tDx z&pM~Euv%qdE;QJ;@N<$@r6nXjIhQxtm)iTdT(Eah8jkT+iF z6_dy>E*D@IFPxZ*&M1=|iYuKN?uxb38+cxnnecW+WA8HWs0<_}oO8QS{UtT}b+TU@3{)q{*_2aB&W!pfZ$S>H2j zo$k{EQxMZ6Hg@;&X3asL=qN-- zBE-hvB(i=qiyI{(Sd%>6|cLG{&gIqzS#;V-|RnXtLN2$-}O}4h| zv5x?R9%80?d&zXk(%%s}gR41DoT_)BvQU+V3X3NXRU-{dFgD2N#y8t+C~Z_fO# zG(-73`KzXk`gDxGtLcAC6?IVNzs3#f^l(QAMT5{t7pecuTCjlv4(LJZ*uSFZr#y#X zk|E*zvhjz}A^-j_@tRM=@+2}||MSuJ$o8qWM4j&(&EBoP|1S~;0_S$hpUMPJUi`Jf zfvPiH{?hcLFn}=m)W`q2^pPur$Nz9~+nX!@a&gjOAgu{Rs&9OzT`v*I4G3{Wkcyt3 zJ|dcsKNs<}q_?k+Os2O$<>t+s|1`|eKV(Lgj-xKqdk)!d>XSdX>PGIe;S(*<+=0G~ zuIEQrlGazaW#9-w>Cmrj5m(`{xcM7TohZ% zVeyqu0yaJB3Jo)zX?Bf}S&Z7bLjiS1A9N`6o*QFJQ}Nb~nd)idp9I>24jemO56K(; z;YdOC+FU>St<3~dq?7%z1~^r^uXeZj%>-Rq7~YZ`TJc01l@kUM>E}H1M=I6&T|D~Z zg8dfRv(r1*B6}~j7rvREF3c{pRI_3DsI|BujmEzTrW-P>-Qif@m`1sg|KQlS>X?M6 zD)wWt4=YMjAXrp6;c{XBM!o+@R+Vn8;s+|V{flK-&m}eDs+vvng$!8@4GrziGjVqp?!>@)H`JE4B8z^;HFxe7`_GfF7mF=<%%_FeM@kg)rUUX(L)27nK_F!W zlPsgdwQ4k4l3V*z_cx54c^_I ziIO{alR~k`k`Cj!*|(my@-}_0BJcY95R%^#7fkRS-nEl+bj3^EFN{ihL_ z&nqo$^m~4LhW~CpRKMj?($mjr>$qj?*U{E?4Vgy7GJ(9FOMy$DpO7C_yl62yyL}e* zF%*KQdFGaU+ymcB#s!tkK7R9OOv3$@(?$7XvpyPUu_$Jh*5J%@&Na8J58cZlB)H2Q zdosX_t@ss0bzW0{Vw!6od$as4%eahlW3>Iq42+!*Nuro}Uk;$p_5oFq$;h|UNp7}n zI#td#fIdMy6t2YMA8%E0!6c`!kPiX3pzafy(A2e&`vk=#6N}y~4Mc+062se0Q~U3T z*GvqUq1HO_Pk%hq-mQQ@rf8sVW(GsACcbpneKpOCA^SFWAc#YFN-a!-TI0P=+D&_M zpLpjEp&y0Mas14!?ALQT>|WqZwli4JsjF-DWk)(tJZ1{9!d(;Q!JF=>>eVt{Kh zpl%1$yuIjTf-bsWY&i94X=#F~mXnrNHGn-?Tr!&5O#xI?zioU}FauH2zVR>xd2}7Q zW_=xb3}_Iw=vJn2Ora7&e2iY(9a^RHGaE5pB0Q4YV<#tKO&-*PgvaAzk_Vo3Y1qz- zDtTw#rSR_ijEM#+A~Wa>S_*9^dqH^QG#-&$#k0GC7BlKjH)x3FChZ*F_TAWBX9{K7 znJK>-9*~(y%_JzOP)}z~Ph+sz9F2MNRl?@;<7xCu#hCgpO$NcfOuLV^S2J*-X`#C| zAd@0e;M}M6Amegs+}>nYuv;Nd&0F+$Uaw5{|JogcZjQntYPN*SYdy6f?y+t(%_u>C zicPesBV`I+oAaW_xwH>no{Xe~M6DL+qFM$`eV6MAk3OUU2S(m#gC1c*)7v=#i|h%4 znOflx$t_>NcGytXkhelYR^ptJ#LW!Dp3HXS;sQWf=2ibvkw^`?nc5R&qg&ea&Yw5= z?tKseh3TEn9(!H)t#09Z^Lz$+!lD>!V~B(Z;7RRa_mw(6>(S7(Qb)5}nri-8Sx?f^ zjBbI9GC2rbDj0cllJ{>!!rxMji=j7?(U3w-fO0ArVg%luT>yl5`pKln^~2M@UUmI^ zuEQ6RA^~qwjVZV%6T~P{VY01*-MXy{m2(8(KEGsoLw_9+_@w3K@gQKAK zoqc_$5!F)yxKSX#5S0bQN_vmgPKo|JgIY-ty~NnnB58oW*Us7oggtThh?FlQ_T4Hr zopMw^Dr#=Ec~y@6r8|VYoq5%eHA!I=G1gQ&^Q$SvCUdkc3|OI3OL3 z-5F?5nl5e3LUNp6jn_t!KCAM{Xe|&O!@RaiOzS;*e_#o8hq6MI3$YD|^a+Gz!XXlP z45)%b6qr0NAyF9QOM!vBHO|t>#Q99V_HHe2XIZkbqER3=E@5T)$T!fsCqPe5$D+b)uk z?CFYxy3WqR1p^9Cd~tFx^4CJwwb_TJrv4Xhrtc~2V)PvE1knnbWh4(&yLU^hyYq*L zOuds35{AU%EJc`w2b=T>J%ep_iHUOb!HVR8U~}Kpki~BwO255NI>P51gdo(5+aPZC ziGH@fgO{jAwFeMp)zTa)-UsNjEKdkRIm-;lq!|-}^R>2ZE+BNh}`jqat6`U9QXM{A@-vn0eR7Qi@~m~a@?g0X6EJ-Un0~(4l%hulJ3vDQ+y8C zk_3!(?T+C*rf^Rs-NIX2<;&@#E4A}g?O)C;9k$VvAxR0D=KA=r$9VjRYqh(Ykufo7 zM7StiyV3iP2Jta=X(lbSvA+gi@^&^Wd;Duga!9OzNh3HR-`B2wfbqHiX?$Ni{mjy2)TlH zThMbFp+r=Xi1>OOZXkr*{qsJg=rP|}@#9GHCqVu9Q@i+zGQC5OI-l$6Ou`cQv2}W! zQe67d5O!AxQLU|u3VCd;Zh#8;ly&u5UkBB34}|FWx1s$X@p5v%C2*x6StnErv20bJ z_8I)QED}N8f5WepJS+ztxQ~~3ZDn|I{L_#F3;zR4>bM1$y?L`rkHb~?K{4gWVsWEG z?)fCUYEX%JKb{uHD7NwW38Bgg)cT=-m4DHztcEg|9F*nS90AqWBUGf_C}?Vgwah1mgig1{iNVfYWeydlOjY+qrwJW{tZ`+s0bI0e0?=6$s;yZz{#` zw9**7eRL1_s!B~ufRuSI&wc;5C))4%pF_L6hey8?Kjea6*9P8wKKnh@#+3i>!9BTO zHCa*u*edbejR}znrmYU&ai~iMNNtai?|51Pv8o&|#7#>}o0^wr_3)fblQ!H!)y{%7 z?|-tEKRb`gmUY34Z}uz^k^jSE1CeDX6(X@?GFoRGR_WrAt2z4BqMjg_U$4M7u_p&X zFGx}$7FsG0@%iQH1Uny-pMKW6$K>t)J$?DBA1(|BMfBoi;_hZLQ6yb8kru%O;%hxO z_kInptSo!Wb1$aGFO)AL@?J!2S!ywG!eFZ_Cbg10m%Sbe*6{4$*huOs<{d zk3__c5$tL_|8{fD$MjGaFCr>xK+n9PP!i-uX&QIB;_zMLoD19ru#4ZH2y%7*CP7G5i?u@na7!e)asUc_?OfSr1yxWFa+p$Yk7IECG zp^QNKWw|)mnN{%El#p;n&26NqyCkeBbMzXJnt*s z={Ha3n)X;PT)>hyxJ2EL3cG-tuJT-rmp!2vetck<1eJoRimo}$X(;!i#k<;PkD4mz zZ%!Y;bXA-cDzRuDh#QReVXNxs8xZ=e-7GMvepCNs31x};P4$SCn$lDuvu1O{Pcxw8 z9wj&%OfPQKq*tATuT>tuxJ{fNae1ELkjS%`A*y4-EOx|AE5iADh&^kwxAeE;v~8GF zL+^d#zn%5F=&Ge~>b{65+4*srQHGZ5)IsYq3Px2r`Xvh`Jz7s$BSY8!% z>&J!0aV0KtS!ZdABm`VPFJb-tUhdjup$6I5>YP zk_|OOp%l&41bSfj;JVD0gV!U6F(OedlU#ZwZ}i@eMN#J4pO>(Abi3cc>0;Sne!*l# zla`^!y5n@Z~xzl?pU;uJy4d zNwLs?mOW^M5|p(<$u$L!zSiY28JN94q-!=a@2pdv+uhGN?Da^#FVw*A+kGic>WBo!n^qGp+`hL9j#?W)P8Y?fuZ6$jJ3%a8mJ+LK#4sBPSk1f*ViJ{{ z@C|Nfo*-|^lW`oeXu5Z6C$ncgU+wj+tPcT5NUe}pL>8{=3)}Jp&D&K)HJFY&oA)LX z&qU|n8C~4mxwgX(2fDU&53?~ZY+GAp*Mskb$OXFZ^$(^nW&c$szJ7fH(QcHW zQD%O_-ia*%f}G3E%bEb!VphhBsLHh6wYp2w;V7i^eR*FVeHm5AtF3RSFw#u0liOX$ z^J@7_@+AAswUpDFvHQ+nL(HGldsI&GCvQfZ_XntMx$RGB{75>Z2V)MFm82P68;+RU zCP5{nx?;Sys%ShaN2@rs&YXmZ&EQ|Ea>wuM};$&8svw+qYJBKYP#%MAs#C zB72^MTeJ!$Cx<(KVYS)^%;&|sg8pySrCN`uH4H9DADWD3x+ zQ?_=%pN&S$(IQR($h zSo(6XgRrGG=gTg zR3+AWblnz8XhiL2LTKp*V9z8B|h|E&+0k!DehmLfmI=)5=HMlP#bY4=%@1Ct@&y_tP9$VP{ z`!(L;iD~I&{0UQ}=QwaS14Nz(^Wuoc0qefqYp|o?xlzY7#Yh-oo> z`0%QZj?Ug+fahodY4L~qs6R-}Y8`~P@n)fFMqlPXC zpgP>5Zd|La@T9qX_wH8I)+;YPA~G6dbHO2&TOOKBI_oc;CxUnbAXkoSE0{QgO??R|9khX6{>8yr#m&fh+Nu+xQBtWC&=qmsA3 zuEvWcq)Wy6y1K&C z)6==`6ulf-;>@q&NA#4HMo3}`0;Oeu_iw~Jp2Sjt~{ap{g9jf9T-iz+Hn?Z!y+4jGQrUJvk5Ot!@Sg8Lv+ zXaV8TbGg9NC1S`8AtD3b+HKo|$EmN+2*&W}#Zj=z8#e~gYy#mo_*Ep&M$Rb?YW~m^ zuCc)9O^bxozgTH$OuKoC+koOvX73b122G{!%uL+MCsyoY!p|x#C*s`M;ieXj0*Ak8 zjFgYou=%>>{E##*e2YUG;KPlfCr<2{l;b@3`Gx#Y&( z5(Si5&>nJ>*8DGw>d>#J`3FY*n``^wHf9n52X$dE&q23w>C&ZJZuDll=H});$7yc- z`&4=}j>VvUplnkJ)^gmm>{(6zm8~3?e?TuD5{5gFWdO9`(E_}9Z|-=+~D`i z>0k#0`s3m8D*~Iw0e0m6!+oc+kw}L=w6y%EzyE(77jN1Ekd?VZcW*mzVKHNMz8O&O zfS!ELRa(&YG=tyeH|xm^qUK{kLMhVP%y5bC`(;Yw5ZeS2@4)8vv{W*vH(h4r+8E4*G7G%XvwI?SdC?y}z0$JP-!$ooqZ`wzgd%lxEzcjOkiiaQL ztt$AoQo9_L{{>pvKazWXd!p;U)Ub&`OGX2c%#K_aP)W7>&iRBybKSY|57rF$(Cl5u z3mF01FCTfNyxI~(Tv~KSyB0+?BF?Eai;XUYxXKH7+X_w6epYzns5K~i%7NN!hFoR=nY*RaC_(_jM`W;#$M%WV2Q6SGXnpn@7bg=k@#4X2pQP{BMZK5HASc@0=TtSGo7P_K8Yt13Yh@tNu zgG6yi@O1W{z6`<9F~DJ-izopIWs6&M?b zt*YqgXb}3RmzNI+c4Mp{>U)A#FaMU?@f4r$Ii>y6-!tnc3zLW&+R2tcbyPz!c;HQG z=>w2=%!MY}cPW-bX{o!bt7w-fPQ|754B2O*e7>k9OOV4O$l#|R-pG42Lszvg$J9%w z+I6Vbpjz}!r6alxc6{(U3zTJu2l?IU?;&<*ZV8<&K{Brk6D{vPC6EjYvYBc3n=d2| z!?4C(UqhpUtML@mwegIq-x#io8Nw_x!T9SjA_=H01!W2P2zyBm&N2 zyAX9PR?xg}D_@vLG4k3gP-&L9Ef(CXTZU~|xFGJ|lZ0Jl(gD0f39tzg3}n-Y^y$-( zb9SIhO3voj*1Q-l`08PT7V@xY2xzVKd>wh5`q?VZpxknMEgfsUP`I^~Ct+agap zwU(U&O1>?mxiD6=Wbo#@HW}~(^^IaO7EQ4J`oAtgeUTNAFR?3;6;82wVbbPT5CDgx;jkf>u_jXI~pHNbx!#znaE3{ z7cYv7WIU=FPGkAgt7M*s0eTcsuD5mH1Em&J19&eT?H?dpx`b|&$+MyB^Z zJvS!qlvTOaRS@(X6@|)|nFX)>>0rP>v|277UUm5Iz}RspswGN~{xtWX?dI6#G^d#EO(y7!0J(M;IF<9t@Sug)W6K>^=hoK4Zm)r+s%81HlP!mM^tJvmgyD@yOEHC-v) z)30fQP^8ieH<2j(=`Xn&LP&{?HpbLox)#X;shm8&20F<3riCtU^Ha0Rv%R{p)~A4F z`+J}UUxE)rUH98VK12Dr+?(CW;R;MDpuK`+n`^LT_~|*RoUd?Aq3zc|l@@Xyk6*0M z{pldrfU?nDVD=t;{XN?;wX#wLRdER#Zm8mya^3P%jTJ2Qjq9OONjX~TBKclK;}5?X z=U0nj*Y34TM2t8&eFf9=W@!C`q(OaceHh?%FHkWRGzFlbkW{pvwOx zz479$_isMl34R`02_z2T(4g)6p>$dMm z9DhZ=zIA%<>oX0vbFU@27u|5jyg3dc6S_7!!K@pe&oA@2^j8XLyxT;L;wqUTbr^m!<#T&cT*exxiR(!}tu6tgd%_NO!ygZX){ONfX z4~f}rZj(dT8U6(6R{bVIrD}eMHf#9d^Dz-}fugmSV7sl`jzL$$AMcCUN1k(v33iPl zJ?n?udF%P8tV%M>z$$(o<;wJfpN+XuuFIq8b~we!%+yp7YA>*(M|VfZ^#y3KPwD53 ziHOmktvbB4G0{|XKWYVR75Zj9NDN1IM%^?SK}P9a4?{k($BUs3aO>Q@AjPG1{erZG zyW3hX>GVRi)2r=A^%PAsCKl&q=F1H3qxbdzag7F)1;cK*FlapXfmF`#z%e2BoeU|< z%hvsRoRhv!$W>Lv3zygG{hLS*vB~({vIZIlg%Z872OFVGhGZ3gP?4IpX}YS>Tm#pC z#cb>@p(8Z1Ii`F2H66X5U%If_;mdK>PcedHJxzYWy8$K9*iQc!Y`p3RO;>H8yk&;b zHB%}tba#2RT3||H`2N=~QJNgnk=L3e3AdWban1_1qb_s-KfB=)dwSPC8+tQNnEZ!@ z{^xPbv_bd?!&=i66pVR^y{S29TCY~N80J)ZO~p3nQ7GP@_En4!I(4RRgvt3Xzw;x9 ze$=Y?A9UDzJd)<{iXkNF_6Zb{V70nzPPXxeE@6yhiVWo@6CUs5d0EoU3-a6nPnx(MWyNcHG@#NE_1@NhtO_X;FCOVTs4;r@323}9~4Z&-#Za~#K|NczJi@DIXm9j=)C}E469%Ix}b;An}yhGdtySjDsXPlb>p~WbO zE++Dtj+az9$$xOe9wYBfd+|;Du8S^^Gl;qekO9xgvg`GF*a+z6cj%kqWNxmc?`|Ul zWbYBf2^?`X!?+=UwEcCqk|VadXkWb!HZH-Y(TAnJhH!4*K?|(1?Y_UH$&gKW$d>E2 z!H!YMpf`U2<0Aq)A=&M$^_##}UcHRLlH=C9Q`T7$KV3Kaa`0AroM6*RyAzkVh3;?8 zi{;HOa}bc{OEin7lcET9WZA>k?sQT$MMm`@fJy$A2X-6+4GafV^SvSvP>Fy}BuPLd z!ke%sS0bXEp%JpsDpA;vO7YFn3ggo5ae*7#ytg#otV%I=oH1b@7W51>(=~M}IB`h_ zG3X3jK7#*>T9K;%mCD zD5muudQ8kuCC&-PizHNvVf_&n#ns68=GZlQvI&>D3g| zUpDL>GS4sZ8pRaS1(3lWQ-nk&{BW`D5P0bvQ-x=Ae@)usUtA%Nhoe|kE|lhuB75vX zJBo*IThaQc+w^3N!RV|Dd%~kDfPDu?ke#g{l9r9QAnmpAktnkBUVA&Jn1>k5!Lo zHDzpS;$d6{N!y$4iZ{EX!0no^^yZ&S-Zq|eAQzp9)%}0ey=7FEZ5lPKqM(8xC?zEb zqI7qtpp=4&lypl=cd3LTARt|$ASy@-NXJd5gmiaz=XYG7GtcA9GwXZTdjGulni)sr zuIoC_Blh0MK5dj_V;oH}9Pe#+a<#*F=(4OyH#3m)bcZclrrau=D*+Zh>6q;?g%vKc-a zO0t3l%s#a%dc?=vWekzMY(73wMh zH5GU*Hjfw`KQ@UyW|5(HFJbb&6|I+bRY&sFZk8AJoT5e;Px^`$>Z6C(_IAvkC9=7X zWk*9~_eOjvukUpT(=+_Ht^%}3F-ZK~LbMmjis@9yhd^YAFk2WaURYd#aV+p#7EO`@ zZIZmn_krFHWygZaj-HhQMq<|Y+U}aEh1mr`sJsy-U8IOT#!m}TAD3hSE1hPe&sN<_ zTOE9ya%Kt#HiQnS0_Qebq&nc66n{Osy?tgQROKHlo@W{5PY6P-_v4*pjmBUJpyWj< zsWP581M4f?`wokPehFTECPfGfFMw0*5t7`*;3QyhE9$ytbZ0J5r_Kc-F8_ z1Se~}`bv`Esryo+cQ)vQUqQdMoTYW*XIppoVTrfSs+N-zgO^Yq7_)N|MBn0wTH(c40iN? z57Dt7SHO={>7U^qH}d9|mVbhS68;g~JbVW?p{jqE?Z{6K!O5dHzww`#B_JUGZqoit zae!&UMBKZ7xPnr@K=mK|S>L|JgJ8a)p#kFDr*)pAcXRgp1&km4_1NSE=;YBs+AZ9m zq#qqD5p)%)Ul5+p0)nOv_`UY*{v%e$bJ$rxo3#Kro^#L)=G9k_wi~emwLmBYFxhL! zDO&rA1TnD6>z!T+WmFD9U^>L%5#e7D2i9;<$RomBNE`HmX(z;yfun!E4~8Y@xO?AJ zQMd}k?(|MrfK@BN5L9lI7+r3gUX_mOoZm#?Q}Eez<^|_N1j;d9 zvqr}ukD^ALp1rE%lNFoiqwamBmk|I~LfM(3GeUiDTEW1;;8%h|dXm2shCN1o6CC@^ z^j0`XT?O*6Mw6Vr?0XZY4liFL?!s_~oe#wW<(UVKhUszWMAo2r4WM%!^nH}$Y|+ox6l7Y>BU39&AymALbJeqDF|Mi`}A z0p;5S@Jsr@11Zm$zZ*x`Wq{Hf-d>l@05luvM(lyiF6gp^)ctR$4g{pu2voO%_=7@z zmk30<3|})RZG>=!(k8ZV$mnQA9Q(wDvS4P|LBE| z8HPtQM}da?=!}5FUKsh8AGB?9$Y}K8!!70yy5{J|{}B=YANz6J3%EB_Ky8PE)2)Bu z1bA^rAmkkk?+LEQz?UiUI~2+sGY)N)5f+4B-0JQq%Km8ymeiX7V2(lfRKvNOy$2m7 z{KsnqYH0=7f{Gq5;mK}uhe{cEvSlMLt%Poe2cr_`ToFs`SzvqZH}VJiE_^V_K}tzz zAEcow&mW1#r$wIHFLS7w>STfda2H-N4DSC#PVwHHUQ<5Ifdf&_4(en02$kv*NGpiQ zFThV-e(nMCY}5NBt_Z4mY}F9Nq|(43AjH2kg&8aW*t;R{9g)R2+oBvbRCR=cElod0CIIei+wi`vCl^KC$JsMK+yuPWV&Yp1Tnw{p|a}gvB0dFtve@5 zM+Ud%8fwgMT;KH z$;?*}F_8UuI3h!^Xpx}R1xj4mX#rVvh$js)NuXj;Lo?$KX5_I@)6V==YFPK4tQi^x>-h8gv2~V*V8@h$OJwQNZq~OV^*wU)qH!7 zoF9?{J8(2mpi%=JrRus{Nn&`pkSrEy<$pJ=u1@gHn(r`VIe8gBZ0UIHR8itpwrl+hZ!`hw2#>r5uvv%(F^K>SKN`V|T@$SY6vg4l#)f1y)1;h(DuWRTK1PY; zOej(qiw59*LZ}b9M`x$LC0MLkeS3(>H)GvY#^I0$Q{ie0GLbbSg~vVMWa%a4hrE+u z-53}A;bw}&8!RH0%iy3d)}E{sge*ZB!L>&SHHU14nI;leEz!VHG>Xj|?J27HT|H2R zWK?V@Z}9mLGDBC42x9u|uZa2G4Ma9?PtK38L~J_ZjWB3~_-$`Z$+$)!k_+h5%>DSL zKNSp}GwY07I0x@;9CMhKewZK=ZbyH(O+u!Knzn>@!dfG465i4)?q-2&b4*akn` zy&!6&c7+w`RRxPT_TwYE1ejQoU~}4BybH`H!XLw3b+y`Z-dURog}DdQrKpuger7<9 zkOmKV@sF@*A#=_xJ0i$Z&rwjGl>Ze}Q(%Y{VJv>_+RphXgerrnnQEjf_jjJvJ}kI8 zkeis37laIJvjJjs_-(yhH@$59K{+ULxX;tItHu#nVtWKBS6bWKLy+K=TD7V6ADlw#dfGLM@mCL$sikT=%Dj3wA!Inkn_V^t&RrSv&Qs_+2{CQ%Usb0nMc zPQH7NaKlhMaW)BVsJOuW7)O2qjK*Y=g^M%E=zxf~9fb^M0=|#7mYrwzh?S|G;<@jTqcdw-^OF+w~?&H#L2jmg|BUFE-7x7l+yCV1I`lVh4b{#LCX| z;KLsS-e2qo$VnNqa9RWe&Z+pIT`6c-+{i|kswkS`C4!I@i9nT~4;#-P6S=+UZQjTV zfL+@NmI5sxSplMH|?$i zuu_515<OlL|W-;!aO(f|?xol4f^57mlD|!nK z4hV-MYQA=B-)N8xbQ79Ini}NboUSU!g#*;V2d&O1lKvkevgw(5;G|%QI>-GFqJuS< zz?pz<5c?E>oEZ_DwZf*L4zANP&f9A#KG-Qv1g|#aSc8RzqV_x~ zVk=U^>=#Q=|LDS70O8sQxfWT~$Q-tuGLCZ{ zj7ugRu@3(QNV;)u3#l0N!MaQWcJnf5bhwf95JZi%q?w=uhTWTxl9JN5g4hn*V=GrQjPuU;R&45yW^$4>zHkhb0^GHv}Am zr@?stzwk)Ga1w}r0(({y{(^f}W9IL;HvgO&_$y54z{c&zdzE*k-tYFy#Ml&4;qfqX zl2e`1F5~|mi@E+wBjk|yH-!Hu!aX|#TYGpzIiWM)%Qs#g4$Z;B0S(HNM#RgefI@)hdhi!PXE;HkFaz_BK8=53tTeO@ zqaqa3Q=gF+$@4!Vs!PpWBX{=(Xb%PsLg6Tk;AIrP0=05aYWkyT#h6Z&xc8Ym4T4-c z;tzA834f~C5(nI~MRnSRV!=zeJ^pwoyF3Mj5u_O!+kt4{>nwiUtO+ev;pMW_V{>uJ zFjfLKwta!0e;v%3U7Y5+zcz8{RIi6JGv&r-@K+w}71sbU=0lkO8sX2=Ru1)(f9hZO z*|=-RB(D!I?59}p`f?Ysg2~yar?=!eF1n&d!r zLZ2^Qu+tYTWojk=LQ&!EW>-_5$y+kh!4jRePqhgmS7oH(V3=?gN{BeM5hRxaR~n^? zmV80L6Xfqit@J&uTUKOaiT&>bU&oH^hIXjSq!bH5t{+8FNMj3f0An6VvCqOg9#-pz zEwLPjW8#wN?Y!|-KrJc8#Q|kX1U(+q>B~I>@~CQkhi%H&ITZ*9v$+c_i(GMG?eMEdqWUfp2awb*AWp`v0k_?>3wlzYL#L8(*@z+Zax!D&v_6Hd*L6SZo4ieDm zAr*v)%RbW8Ky#@B^fba2Bh3hS&A4JnW6d?dJn`>1i(lgl3ZOO4z_jt3R`#b<>xz0}eV%24KBUH%`zlaW1xvm)HMF!)8q3_p>_h!ksF48rpp^CV zCGCvRnC{HwsKaSPZvcxF-WYIoMLJ99uCZ27YcjTiajxf-|;hLOLO^yN^v2UPeFPJnW~7OCB-_C6jW3IO~amZFFy4}dcT zh{kcMBUK%_wqyuL@JJo>O?S)l`-3?+s2dt;6pA0`s+7}&UK6(p1rd*B` z?w6jPqkRj~023L?AYI0N1pC%1aXU`TZK@I)!w(f2(iz)BOOe0d2?L!-yMEYU05+a) zbQiuRClKKS`-}kej&^DXO~lH~7qBa(y~}VEb#~68?d3g3^af0OXmNz2V5X>M#Ge-8 zLPGrVO00q-a^%muRHQ7@fbsHZ(_*X5A+JOjfbbXEeuDq;1eCX66DdhN{6+*ODB@F? zw)QuIU~Zpxtu5a5z)+@>dvzf;%I5Jh#X-RCiZHF~wroP$vhco`u09gF-3+0W5gJ6$ zuS48^Gfx|u#p$Vl$>*S%Qv1z#z+H9#9f?j$e681t<6LlkWT?QV#cyj@K|uq1OyC{{R9Z%66>AmsM!#~`655xVzpf}K2QlEdKY;^bM_Y0N-P{B7T|xHQ zv-6lK-xngDqyz^m8A?75G4RnHOO=hTZJqwC(jv*2Bs=jf;$^sAIOWv{J%fM{%+ChT zX1>3_cAAQxy=lQ;?7_*l<^avrmAsVP68-V;t^N}OHy4^L2ln^(Z~I}KJgGs14*r(p zpCnT_px@6-lsb>g{Go`FYGS67HB5l`{Rgx0^s0B-V%|Q_Z~F{(8wsjLN9kmh&E=qC zU2j=&=;%cxNm?yn`w-GIP{w>1;1w;Gnu?`E;dQn1U`mTKk#=m#Y=bx9QOkD|iFq@X}Wh9AYk$!h0V zS@n3{%TP?VogrYjuXsx`)@9;6uEJ+CZ||fwZc))%0uO4@lw&XzOMUe!0k6xlBB-Ss z9b5*AoWURZn$=8q7CpE^dywbo)ZoF1d#|Dn0Eb)w00p0qfRIp3RP>~nAL(@~Cukah zM*sXHUC?p;JTC4{Cnu-4k%i&|A*5rC(+&?07ZnvfPeP(RxjEaF>7&|W42AwBugW;{ zC#3)SBh*)WLCtRZ`X^;uoC~yfSTt@3 zO~A{{#N>xf#+PQ=$5)0z^~o1|!=u{6+aKgx8sm3QV|&uZzHr^Y@a55xmF_STy?jYh zYMG4i&6)P`vy{)#-5bWIYT=5&LqKeWlWIO8m|B#H9Rr7)nv(Lk7wW-N;~(#ClJZ$0 z2TJ0T@>`6a2CR;Tit2r*+Y9gf=8z^s^4-cf~8Rx?BM17UD8=ZR3!mc?8EG9)Re=w_W{UQ z0?j!-L*GR*vIa;EmGIv?4i0?B00yN3=1l`S{8JO>E^&A_M)R)#hs5TxYbUh3C}R43 z2t7)g7J!_!^82G_Iqa8pWI``fsoilF8LibLT3l&*y3+6?{fcqtF$z~Ke%rF*{ZnIg zgo0wuBg@T&{0wSXuDi#5Nl=9ww|od#ocTt_?DP0?dq3G8tXgD~`mB(uR=NkxDuR%S-o70W9DEy~aXWybo*?TvHE}Gm zJerN2{mj|3%T~lx!?$4vq}-LJWl%h3R-h^UCdUZl!=31X5X@U1PGDyt*n z!cd{(^NQiA>Gd*y5(BNNt4+v-6j$uJ#j)nH5^$r-qi^owl3jrwAhfq4ugk< z#O*uJ`qXxvmzJI$?EQ#8(`!r7mWornf3|q{g!A^b%ak5jCYb83=OVenZ$`i(yTtiy zqyKp_gNyQjc_rIxX-_FtEPbma z@tK=5Xq>iajJr-0I+Pag>zblibf0S`eM+hm`25_gFWf>o%0 z8cP^g1D0g6DkgK`N$Z)iNVBqcH`A=?>toK%b*uKyd@@t9kKpo{YK0}ey~dt1a2k_6 z$3#MA!^(P?v5o)Fn1_pd9Ul&4T(LSsHLMG_hKYHs_U1_Ck6iYGJ+4Q|r<3GMla+S5L&w!0I9p|1rX!wSf z9va7<#y)X;xw>7WMXeii|$&%)z-T<0&KL z?sg2q(u(7P`T?5)=NU}?HO2fsm9f$Kk)zA4>oIO5+ABpbadZ8KQSx?x^a|jEqyyhkD`{1=@zWMYN&1lQnbDRon={-Xf2P_J# zO8MHC_==k2IdZ!aEU|($0v$bq z=-1eomag}2Ig3a>EabZzq2rP&xPnE>t7TgJAc^3`3s?|K9BbIP=E2*ZM-e36_`%_; zSHR56!W5opdEF;7Z#%F0yDO4=p643oIOmH`t(mwu3)+PkEOEDOoe<7eFb$c94I>;0W~%IH(WlHM2W zq!6DBq<{X`7e0!&NB3spZ77Cp;;q(?i<=`)_tw&MF`f2vc|SWPyi}ohDfm-u)_MIA zf56NYshrEaiJ2xl%VVKWTkpXJTV23u5;8(}htFbRDfr;)?^Q#0_SXUwf4RIu$%Yrf z?IlL;&ZMf*dNqu<73;3bXNMPS8k@UAx1Zs`OQR-m62%m8nl4_Yxx|(HD(TAJ*J zOtc}oTS{?1)=5qyJx}-L6{%GDUvVJU>L?C?4=wfY5MQ22iXFy@`8ewt2DzKfoCXR# zj2+`~_NtY21${#iE|=>m_U_`x*RGkfU1t5Tm-M1R{efZD_aOOd7ES{#boc%3y53jX zn@?AET-q5{^hKDMx1;}l>yO?tAUu5PUkl8$^~``1nn_QY;6_-|?PO()mGSVZ+Kh*D zB!Uk2C#IU~pB&I_)aQGhDZYDI%jTrN+>BJNSS)pl1dI7gb7Sf5`t`SSv-f(W9^Sz- z=rYim&>!K5N>E6`+)m*u`}A4ll`5rnTN~x7s&zTLhrM``{CJayN>uK9Fm4KSkF#GX zVG&S`{_gW$N^ zYHMSh^o5EScA|re8;7@-)SoIWjLfE-T=Oscw#Bl`)u}qw+caXdRxtpKL}c*{h0KmX z3GHB6?8G;JA$f|lo|-(@Yp5$&{dRU-n)s-*C-7R|GoEY=lUt6#S8x;FcT-SvPmqdp zc7(LHy?)B4qBR&QJV-uccafz&HZpYr$zX6)uO$|mmX^lqlNG})60;Ybij)0)t--;J zUVao>OY0To#{QZ$el+oVi4R+g&#sy_nyDLKPSYbZ&fm~d`RP||BZo?Fx>JqJ3s zFC@S0)Zm{hL||d^W=5N`3cX9nbE{eJjfr)ju4SOEb)bRu8w1NX2Ig-J8m^)&qDDdl z0vYUe6qm(mLrL|Sc(-^*c`VL4b9_%TqD&^B-{*3@WV<%xN}+2hrN#a_ZTk^_RH?1- zqcXGgU3$LD$Mv2(slkKjf+e*&gQ1!irui~P5I^KsAYDTLy*pIgu}}qOMzJX*9TVmK*zMHm~1K7EB{>+ut_{4`_>?SN4+~%CYWj=@>qJrgD9Q zDI?E%xal?7w+$SsPEC)fDA}Ce-ZEDq`~0v9GH2(~fq{apQ|_)bSgx*%8>p!Xuhu5Q z#ME%&^O%yZ3B4X} z7Zzim%Qko+hsD2xc?q3|^!D=$G^glZ@w-&A*1h!4?o~0?7h1Pt8r6x~v0qs}E9s=q zJi5uVd4bL4qf=?0V&`()g_^_`58$CC$yvF-#(&1>TUWb}A|( zlut6KW0UchC8ge4=mT{IV{k4;5JVG2Z(ncqOdJ4`&^0#YC7h3Jly{xs~VoIi7L+68%8di-ocK z*5GTUQ$zmAJ6_A>4O;~d7H)n?Rm5aveY$x0&Qq3`s>+%1;W!JAUg>{WW5GwGYe_Vx z*yfxQi>-A2bWyRU6-6h9jRAG>YckB998|Pz*?05toHuXp{Wa9T@kv06V5C}+jOSKG zGR{J_4dZKK?MD3^LtW`MY|_*6Ns09If}Wab`-2VyChDT~*+lYl${s^c*?4kTamcX} zR>mLQ8s4uQetIKQPw(=3TQkdn7w_H$#`3VWU_H1<_#&-Gv*Ik#xHG*(y|2BGGHoM2 z{u`bt(fe9MJ~`~47Ba^(&dhW~4yZuaj90}{{iHQ2YwpXE{Z|nu+%$ZjhC3S$eN!Fr z6nl|tiAuqGicC3H(r1=epkQ2?FdY#3K0&@Rk^bO{jKbB!1mAz>u!wl`!uzebSswDE zeghP`C7aE@lV!i_tm2BHO=JUvR=;nv93k^6weL-Q6u&v+zO1@n@a96ftO4U>ew~+R zQjVsSwELjOWLGQJ?OqEXg_PYv8^TNbJQgQiHcPSYLjQFAIuD%!oO*Dz-Yl#tA3r;7 zfI0F`@TKNd1L1A%xu&eO)QYBY+s|U7=DfU%UHUAVC#Y#kpW{UI^tUBV{OUu?m7o04 z&6MXG_Zy>AjW=?p%2TkTdtx1@ppa3pF(CFV{TP4mSCrh%-r%mT(-(Je_M~Yaoo=}P z!nj*ch(}=e(Y5zR3X5xN#!I1BywLVHQSeDsPS12`D}E(4>3FTK96W)V&flWsdHKfe zys3eeRB1Np&sw<^tFh2W@&rz+xCKk-@d>5gxQh#}B2PXg(qBN9L_mf#a!svIF=o(F zQN^qR5b>{uWP+Kp6ieir0h&>Jn8H`Ht`k5{ZJ|@QShl=;<*!x#47bfOhulQiurB|C z+IVB<#q9mIS5dZg>?3`Bt18*g-`GSC`Y;a;-c-!NR Vl@>YvQVON_+_bMg;llpz zh4ncUD3Tsw8#S6dP*&Wv!ONwP)3rCgaA|j!R>q0*^(C{P?{>b;7c9+?>{?GUGgZy- zox<#e7xzx(oU%MBFkv#2W1%V zh5154!?wj$mXy0$Mko=S(gOQ8QCM{$Lyik9-3#?PEnYnmk;8RG9u4-R6WaBAHUcKX z#!}A@I0j#a(Qaz);6Nqi=y1|hu1MhZrBi_1U^17`#@=|2zPMcXboEnWQa;<1?cdvn zX6SjJNGbvJPU3iz}%{yzVcI)0g>lzqB=>ZmT=@0s$ji&iLjOZFS?-Ow)Vq z*-4e(eu&h6j(NfOiL~(2+I!d5{Z*5e(1Y!RU1?Rzdzm2IDpKL}eD|^+ipM!dZ|8&F`PVXDHxfD!S>cd?2!9SvIGgKGa zgDKqh=V`ArZ_j5Yb}MPBm!>6o9#3Pmk0gH8Nsck< zFXz$iBdX6oup2H}SkWcvOUJKZ47E8YGWok4TpKXki%BkEkntaU-}dSF zLV>pF2X?!(+`T&=?kx5@zHzpn<2ela;EekK^0S>goGQ`!%|Nym44NkG0Eap|FHC}B zc>;d5_;imh^YDa$jL$zlK3+;b6}fQVnHaD>-lHddtC&xal8@b<>^}K~Nbr))+_Q<; zjg+Liq_y1V2F)SQ-jz3g;J3HlO2Ufjj1d!7Y-rv2s1?cUqqLH&Q%F(v#edme!vHVq zQcl%!DaN>ZbqM1))-q#`ofw-alV61vx<-sWdI%%?M*QZwGF|@m<&bf0HELaGTI-DC!CpQf9MYOR_ z%V$9O%IGh!d>S;bL!gKm*<2b4%`gtQ1y@o)^_jOF@!Gjuyl5;*0Kl-w&PMa2nm@1@ zz9sgvo1!_5y>1npV%eTrHE#x7*?XQ0@+f1rzGG!T{zjstf+?v zn_jt@cXg;oCm_n#`TWuNZ`$P@!{yCMPG;EM+k=a8`t-H$#ngzfHcl9al;=%SYEc9z zKfJ)!!wm#~{e=~;-VTDIb@3M>HY)ix;%uxl`b_&xV}@Q!|J*j0vD%oFdh4#OE{2}m z*Pk}85g4l;!@yZm?6cV2e~A{$a4i()5fe_IW9er9k0ioJsV)yvKNp#Mq$$30c3fF+ zkx~o{0`>GK$PCUtgqG?VK~ZPNlzO;4svpZv%9;rjaT;2SZ~cNIKK3a=MKBwjJqCI1fGcEm}d-= zG7~K}#vyew&d7v~($ci`xj)WbuPd>L8>6f{G*wF3YlnIYwiKoH3wvjxSnG^jehd%S zf3+oGdeQ!M=q0U2W7PY?GE?rHg{6<=$j76is%Ulc^>#GKKZnbF{?a8$EiJ8p5n6xn zajP4f04$OTXoZQj&LZa>JY3udlkHhX3h&>)KPzZj6G&6RM6&*$oXbVE7MuSw&IK_y z^J(L%^Rq3xW`id0q3)g$w$dKKE$@Ldp#+Y@X zE1Q0`tJ5965(|VW_XZR$Ro(VQ>5VLfvu`ml)T;#Q9=I^bCI&vtoRXzuSta+y2?sdNd9DWl=24iDh6?t|btG$s>9bY6IQWj<+sk4N!A=RL&h z>V$yka}>@e%?gf-`IkYlw6}qhVh-L*Dm{nNK*gP__m7o%bnBv{O$x1+m+z2cQj5Li zB^TnZ`>EujqBjoqe|*4LPcCoG=HM}}DC(1L-xDDw*IcYbWu8eQ8lQ@*8DrumblLI! zW}jITCHVZuD5-@>T9Gwp(P^o%o;&t0IG$9xjiV9$a5@%kXj^7@eWxP?`rFh3Ez{-; zcqaf`+879pjyVE>31yI)S#JFxiUUXEYjeFGMZ4erT(=p#mrI}5)id_JW zblQyDO8+j=kjApI`3gVgQ|H@-`iTiu;gz=ciBA?yiEL$_$n1~4tlw{P!7kPvS}`M2 zk#gR>Q;0>v6;|W7G2j(xhVJUxo)V?ZbJ<)=!<}7^m9e|5JJ_=CVIm-_!LbzU>*wsg z?W;dBCdOvh*ZQm;YcbVTuu&Lgav6nj!9Ur7s@7B9)w$NwZ7+e>4}#OK%qM?PUg9>b2QILy z(2+Y97D(v`(Ah#DsUwV%)-O$m<09E%D}e@H)6pX{@2gRUl$vl}bcOn7+&@8b_uS^8({RN=iJ{-F3x z_xRWBk8EvfF`?OApInZ;zJVE{raGj)H`K1&(A9;9#iw#k8b>_Jis~9`^}RFa*Us^h zIDRRtes#5|FTZthKkhYANYxDILR@KvTtV6sTxKTsx6{N<1I7fh)2`)$4bP^24$Gr+ zklcv9>U??$XKD56*>uHs2C|>z>Sn6moLD)hx!otOaodFm&d*ZJk+B zDl|C5O+9Kc5OcQoJB?S#NEcCDoZ|KdcTV$U*M$>kf~pzS_K_qj1Ub@C3U;?IT113f4aN(}y6~Rm z*>2r=yP#McVBqQ)^%URB(#j}6!wC<YKfCCVFB+ZRJ(U`sCQk-H+&UJ=U5xGLK^!v%dK#7ZFk^{_3rZ!;Rtk z5!+r()0Gt%=_3#xsSFpcJ^ZM{f`3Dc;Ag{^P;Of*nyXJN)v{zLR;K6t$OKipu1lk8 zBKC~Dp-V0exki`luRq8|G2e|ZUg$hU^}u+AZZySII_FE55kDSnpc>9L}Neihx} zDvER+{mfK(`*Dt1J+&aGmJ~y2(K>gmCqiz1S$tYqD%uH7W8!QgOrqEse{&yX*|g<6CZ`hBkDW`wuV_0r$Ik>>;MZCIeH7;n4X`-G|_QT9;d2KvZqDf*yu|`M;7~yfQ8&Cx%DTqXJ zu~=QBrnXCF@R$9gwxW~ zkLq@cm6(6T&XTCu$m^G-5iyN%wfprZQFYHgqIY#z4YjxDu)o_n7V*enH~`PmLSy=o z-5jU5Mbi?Qg5>|}=H@I89Dr$keGKdMI9kc{ z$9q`X>dr6Hj-x)cu18R9%=HlY2+{=_-EoyHVvikI->uwzv9-HirP7_!S65=f(OX(VfHw7@ z7^8@5;2}NZ6*QnKuiks6T{RdWrqOoolWKmOm-;T^N?RujoeC3w0yzptl3HNryCL{>5H&b?S1LQG0_ zfR~DM5IA-qp*Y~c@ zRfRV*0TKl|O0VJB9pAyJ#G%1bOd=!V#C}SuL*DvOXZ4m5;hn@khY8qc{AS=|GPE(U zNc2oUr&}cCt~%X%WDs}aA zZCHx4U0y^BCg%9cEj#kjVsex6H_v&J1{(T1_sDJmvcmK|etE0`pL*8Dvd4F-HBM8+ zZl0INY4iOZ<&XbGiW7imuyxfuvA+K9vuDqKQ}5SJ@@-Ks&MalLp=80Q zxn}6?cXcD*Y{_RpGR5CR_Z-&y$Y&o+NYoUqUh6NUl=B~Kko}eBJf*2=dYy(7zu&&9 zfH!(^GzffS`eT1S{7YE;PrXjcNet`~qf|s;R`F_a*k{q-^@he^f_0i zepFq|C;qW%bE9Zz0BBnr^3uCJyfp#cW&PM#-<)D2D}jk}d5g8WHpAocdy$#nt@AzJ z7(b7SkXtI8vq{FyNl%Rs_VPMMclyq34^iDv{to>$N~#Kehl>QvSA`A9D>nv4BE#=J zJU`ks__YYXx_Cy~)Wi{k`$eByvHwnJ1+NnkC|?ftaCUd=_&`9hRRK891& zo33mcceI6aJlx}RJ%BxGUA}emWyaKCsW{!E$(tG)iUB(r8ME;TU*G99XeOwKsieOe+4$_) zVD7|V>_GuX96JIp2G9o}0-d^PgybHrUHprVY1s7hXNcn*PmUezr4qmH%&HR@_j7xp zNrdPGNQC|M%U2jTGi1n87IuhQV|>PZW4~G%IG_npIGx;C$Ni?s9+Iw2DL~3VaBTKL zKm|JG^tVD!h@~Z`i3_QIYXQjd?U!z=jZ0>lJ^J+H`HL4rI=Wq$WKFdhL2p>1&f+_~ zeatv-wWy&Jz#9EFSa)LZrZYvzZ3r_T^suxy_ovgGcuP%E$Gh}<0tBad9ps)g?L5eJ zV$(YAyWNZG$&uGAf6-WQu;Vx@(K8a#k*ai|-yXwe)-6s1s_4v)qOGesdU<0sOxO1s z{~niwVwi4-il$)wMRZlJ+R_R-sKKu}muWG`U3+^Ue|Vhb<7(>FTKdH~N%5O0b`?SW z*t9hUaXQ%-om=u_=acugQFFG#maX?T-_qN*Pvq&XCerjPB+hX@zCA4PR&M~5s~?my z4XCNH65)@N=$`l|sipV3NTRV^qj)`AtFG$$2>xsY*+m$E3FXsDhSQi=;D0Lzgx?O~ z7B5!ftemc6#$jds7QVqb=c?j4Ad}R!*KSC_;rxnG_0ogPf%Ds0h0kp`yES=nd-Kj` z8AVcln~3%5Q1@~@Ao+OLolqSPw-mNC`j8?1S04k>_5*#j7l9-4B zX#Ik9TpZy>fs(xvLtpOOPJH(C3Wj0NVn540(?!dYbUn?J=L#@mkLRF%Dm<6-*-$kl z^)p^x#tw)y>Str)3JTl2URp}VYh#QbOutcDm54{+gP*3A-YLDayXs#-k)vo_A&|W} zk|NL^{Mf<*JI!)pg~M=_%iZ1gDNl=pxLs!wiXt>Km(|RCrriG2@s}rT_?0$res?p8 zeiP&1v_I;^9a=ECaIN@&rD{(@LPXJ84&Uht`cv7^I2GlE$4(|igmRI6*cYyrtWMo8 z%p;Dl8^vARDE^d)6xz7C!+-L9weJza;lrZJpo9(o?&cL8oe~nW8y0K=tLF<=@!GyR z4yA@ih1YK^H5>bdOaG)$XmOMhG6}joP*uB>LUxAO3a8!Md)OzCt$_uL)Nf>ToSg2W zOLd(T-R5qavjh)mRihm`CYSPQY_UuG`)g^B(T>r24wbWzo{Ej#p5z&68XsvCK-Zm# zoh~Z+xww()8>@bAch_)uM)|{_rEsxq?9S$+f=|JxM^8j0T~ARwj`GBJP0=hFZy0Uz zAKuK#Jfn2LeayAZlS28_8}m9}t4Yq+{r9%d&r&#*(wGilk5r-R!|x{%fL>Z)^YZ4F z>Y^;dvUsaJ__ImU4d}y0d7-`;cubtkG4_?NON;#dOuaKf z-w8`tu$EW+V{o6ch+rpTP)d7JMl~k0$CP@;s$0WXjyjFj#$srvR8B^r0ny>cGW-efJ^F`EVKEK_tjdyRa zXjJ3uEo)^4mqd@N(l`$f21%_uOCg@B3Ldj9S+mu>umSpgfl7q=UdXN!fR z(!4cSjVv+ZN(r;6Wp15#O@g8Bd}?hvTyT4Tr*vbOAo;Ll%boq3?zQ*2kWdVq3gMWC zg}X!r>6G^w5s z!}+{sG{`=PT{~dTZ8djRNGMcCB{V6Sf&a`Il+Vf2l6-2UsHbm2ZR0zCl1aN1Syv1(>)v}AdVG;)W==|GSY!{4HQpf^O@ zG8Fw05zh;?Xs?!x$5NpvrbzBc&6j3H?st4a97V*}X8Bd_uMy=xv>Bj+x3c<{SO=p4 z?~5q)N1ooei}jk+0R77Pf>)9APy=_IZrzG&jDQ$9{cN35&QrP~R-dW#B_cAsQq8P4 zEpewPpU+pg4qooKK=@G9-2K_+CdQK=gXfI7im8(LqjJxRs`cYG_C71qqgTH+2o;aH z(|T7L`?K?UL}hxI<}+VT&7XF8D;G+&Yi?-m|9k@Xi+EYD#puOI#`w<5x0`>`ebLS$ zC}t=f+vUt3#X{TP_NlkGtPbP$qKmXSmi7fZx!}v$)futSNUzqfYPQDmJ@&$LmA))C z3{D$l943b7z34bxIb+{{kcl$A_$!_LN14@Y0DA4VB!#9{U2ktf=t5hG(LrepTBQCE zX0H!oh*qj|t5s%b$g61~7sz_04BiN`0T;=ix=E8+G@*G_mouX%rns#>)DB`SP4(c= zDHO$P=cy2T1{D-sw6MG&(~BJ#87VF;O{Jv7FD|u%AIbQlprD)zjKViJAJO4G<*`ig z3QD@sX?)Y8k+Gr3JU2!1KEbn-<6kIiD$uVtV#$RKg_k)weR)gt@mOc^K{1EHvhcI# zsG7$9B)4)89xLyH$!mede(^1-MvF}~`eHPj>2k*m3R6C8xru%Md|UmQ)APnXl^ZHi z#BVnD9oZ+rrD$<3=XQ=9ok4zr&)<|vxr%=R9x|+0omn%Ft_>%~4*sCD9N11#1v2_c z^9iBNl7h4Lz27rsB4|F+W>}5g4J?kniRx@v?KQnGg!9l)j;h$q>`UL?`H-0ucJ&*t z#;nE(PLf+#;sgfC@|-lEUU`1I3`a-^e`y6%kX6EjhSQojF7C?dGgA(a7vuO`Tz;lM z-V~zlCbK%C)Z25pwA{`QW&;$S3)fZr)hKHA8lW zbURiKBfad=$tSRUTTW3QcBcjIhl*)Xsb3H1x4JDV8qYz-b4>UE13#}l*OZ5%#lH7wL~|^js~u*#kI>bHe6w7`IU&9U zZuJR`8T6Cb1woR}y}~6hxlK!-vI?;Bnh>F5SEXNY_j+F8Ne&Trc}2F7r^lu{c$-Fs zX=m#vLzsq8?xmZGokX+i=6Me4lEJ6|)wG1F<`2RdMjdr+pIEVyINyc6lTnMaYbrioT#`7wy!l{`*)eK@Ve%vw`+g5?Bye@#{)`A3PLAN8Fh z23*)tcy%EFQ^Cx6n$>mDJ9I}fZH23UR@4b^ql`@>%`FnVH|-b<2*W zXuhR*3f!NX!*eTl{v2KwER5V-FQ2~U;rjD0TEpcgRgyR-zts$sX?nHNdN?~LL7~EA z=ai7^{JeUq5x<=`OSe8eDV~}@>1M@{IP+>Rfse3@3#ZT4))_{t-i{nT`6PekO4*MJ z2jX`}J99F{_r@^|cZ<#dw$Vd}GMl3d>ewLY?q4EO@Uli3rV}`Bzru%bm1h27YvSWQ zsGYlWgxAs~gCCOmBMI5t>u-fguK~CXfd?1nFEwA~*hD8Q9D^zw=Y5W(-p4PJgE#A8 z`C3OTKk7X38AhYZVfGB+;{lnSGk0m0p4Y5y*$5uDKOcx$BUcyTVu^Fh>vhS&N9D^U z{2{f%3U5k=s_FECz208{Dn!C9^M~cmw!Gj>{9Mh5kWgu;lgb32OB|3MD!pXw<~(eT zd3*IWBlh-Q?yUzq70*)5vhZB7WrR&LgDnm2PVmjMDki z{~vE}9TrvFwvUdA2nHw!C?Se;gCHd#9nuUP0@B?bDj^^sAl=;{-604=cSuW@^w13a z?m>N?*Z2E=``G{NJ;!(mGt63R?sZ>%UFTUP_pL3&viXio#pYNdjPdpX(FE*($b|jO zfYwyAcch9dYqc%O=CGcn`x?QQLU+C58>M$-#2zZtT$7H|;LXjBUNrR8cbd>Q%%9 z&nE>fE!nfPY0%INLDzDfw0fC)USAb%fJL(3@TX5bLW=H;7MBWzgx)XQysJ4&sZMR}jW1}z%>T}NZ`M4g}-Q7@sFA6CiYUXG2 zVv-tCK$H7~*tD9UB$7?OIj_=guIM-yr+fPr<#y%!FIOBqwSOqr_>?m6F06So4Mp!_ zA6r~}%VDJ9s;MB!l6Sbe`4eWs+F+o%!0P24y@mJGw}p90)FY=`t?|~&yLb+32b(pY ztaX%<@L6vbvfq1TZayeGF!AocO7(_h==T^xVq-f2byO38{^f9nQ<_bb>os|yBdL0q zUmP~Ry@o`lpar$F%)xd5G}j1fM*;wfxyLyn)DM|!_pg1ST(YD9(rO1A3Obujx!`Q4&(FlieTElc7m|HF+zK-epUK6fauN?b#6@yYLU9$tenNOv z5fP0U7lG;fy1L>PUzWeFc0FY~lwrz!5y$b;`EGL?oo&G|K9FOv<6WhD_lIix-mI*u zX#&&NI!C5`_ntJ(;DlVdqS8<)kHRu%UQ$_qM$P3U6~$4#kauS-;U8$rs;dzg93Mw_cs* zxEDtWCq(@@W<$*aAc~e?-vsy0{c!>c*!xSVzV~>HK1N0w_k#O>=T7Iy0H{e500Mi! zVND7K@$>=hA7Jy1S?r7)%vYg{Z6OL99sNXyLJ0}Q}(^YhIB{N)aqRa(zh$d>b4 zUb`H#>SIxY;0SRZiCHAA0EmLhp&v*(uBMy>^I}r=jGF*wgqSJMkH!c z0;0Ut8%4~|{M=_v*31*dzgy&He@+nb!DOfVQeRIjR+by8P(ThX%n{*xPp)7%?z1LY zkYXP0^LdY^nzjAw`%J()Z2@MUv}Eq8+d*fc6sqMY4aMP4E*BoT?y(?eJ$GHPP*V~R zj72f)-2@;~D32*9d_f0=e5&vr9bH|C7Y=}j4M2+fgC>mowN~V$q+Qw+Fk*{(o1}Nce%P-1AKc&Vy>VZ0~p2gg)FeQHtdz7;& zks&L`11;`awpD$cZTuLx`JZOe?p?_rm(548f>lR19;UW$_XD0j!RXZ6-hhd$otL*k zF0Hvfu!zt(kKq_Qwy(Uog5LM$?Pf8}=~=A8?>u&AkEINRlWp=xJO{}i_Nw2!fTOn= zE7KY@ygBY7+wEKkJtmPOkN=5hfSPx^`K9J`$li*DAA7ualtg(VJ#ZF+T-#9=cC6b=iU$RaNbk9AEO&ZV0K$ADn03~4_nAz z=O}l3HpW$<{d4O5#rgZs(&FjH*0@)kGS53{CQwgl8fQDMqzz|@n&+=Quv?!a9(f>A z<~(F>!`|o05QWf6BH$wvxtm9O}w#A*_&2|KDNAZ!3BHtKI5X zAKoB8n9q`m=fs)RQWtGvybV~l+Wl(ivaBsZ%dREib9fzAobn8p#(Prz84c zF%|(dkbHh~8Omsu*tQS~`t9b%)h<$A-gu3GnbZ-Ni)7agYtwOC0%#7Lf@+n9TCwi! zd|2)m-DM5JwgORea}uD{y%S(MI~+{f22$hKZ#dA-Qm^7G=bz)LZ+Tx@WH0@HEiymW zvP_JdKw2Ip{aveKi%P7|UzBN&j!@?3v0njOP{K2^+C>J8j315Mn>S}I{izad-P|9v z7jQU7VVQF(^qmTXU%P?9~Q_SG*bNP=sHoFsvm1VL|Y)* z+{o@4r8~hE7WyS&o~!&*g$u9t>07-ev;(?ZLnCez$J*kn( z%pqVL@<=Z6eT2TLw28!IPmn7@lE}Ka`w(>DT$*f*;hcEHmP&(-BVJ%<&B_e*w9x!6 zJ^pBm{7j`Ie&mPBFl5W^V&Z7{E$ry8Zsz4UGFPn&RR&!B9O}!Y#`mjaLBd z`&_xIfZ9Q;{h1wZS6W}}$-8KqFGL>keGwP-6DHy0f$MXDL|h|gy>|2`&Q8d)fywH~ zy@yKT{=ni$^L{N(+!d8p7b@AMsjL$D%DGXn1J&R>h>coM1`Dq(~-q}1ulI(p%gOb04@!tm6s}F6KGBxJZaL z4I@H(bA|s6(@*}ZAND7%CY-}pjO6!~vqr>c70hrZUvd3J(fdHq|&awUQ3J{a6}E(#v* zJ=(%>J^Hf(9eVH|-0nPzU^%!Rf`UC^1|es3h8uz{plDHloH4WW{stys@{@H2FwFY^ zD667*17HB8fg%{deA5LDtTksm70&y{0NQM3;9z5d)oFJ@s$sK=erDlxcEhhhO{?BY z(>J)=zrM@+&okYPxUkS&=pe(lOm7g%S)VCyBX%_Jd;Z|jDPZ@Z+C=IeM&(e3_ZOwEq5F%4V}#oC{Z_`cUeM#;vWD=T@JXwZNo9TCnn~PoCyj zj6Ew>5RxnA7@I!c&-DHfM5Ms+~XA zK+;>0SR6Hqu^3X5um!V-91=R}iq@QO%zWwPEKni+_ApN2c3pDgt}Y`v`M0P-XSKb& z%SnaX!dgrGf1U%0r#dxv9#^7?fn^61rs?VFtHhwAc4i0k;TZzY+ZiAY+5!Aohsr5{ z1;+x~uMj^Ccn1Mw>il6905B5jz^j4I6+v9zQ`WEU`LmQ!Yun0EVrux-a(T|MW?D~k8R z8*kX4{n2-xSWzB2YTT$+}tC{q<@YqNe7*3Sez!{b01Ceh=q<~=VvrOEDoj`3X@jb?;*cPm*=I) zAAR4uG1$&|fiSu_YS42=5TEZq$~GTBzM4#(IkUsYreL&RE!UtcNgw>k)jZMOLvh`~ zRp6Wnv77#$$l1ko|NPlO|A>Va-3V8#RQbWMExW)ep}G9Q&Ze)(Su(LjUyX^ViR8q>8=sMl zU+g4}Txe-Zmr|V?UR3Tnr|ge+GC7|oo?kR{jrcBrm$)(Lnr|V`FkUt4F$^hz&(5E^ z|D0Ukjon+X7>OFWW8aEL-~?@7!3a z#5>Y_l4w@3CUN*`MABJvbd?nmIX#58pHP1^v+SkukC~QLg*2?WEj2^LhlH?fXS2!b93sGqS<1 zrasGK%^IPhH9}DByp?*=>ylyY-aU8{q_}eGEY3UhG^W8h!ra{`NKtVn`w(+Fqb{*) z^xWZG9@CvyisGDErTsegeV5mjF`d4v8`8JRbcDQSwy5>p6TTlfgvSi9aBsiz1G^`; zeX;*q@8_%^7oiAuB_>_s4Pqs*ug=f)Yu)z^E@q&=_LK1`d+o(bfs5(s*Bw0L&ZhhB z2p5<4;DERr-=_4CcU+j4O)n?cs!tnIM zWVHOP^N(mOc_11;(4{ zBdC%R4G&NK{t;kb2Z+ygO5pb6bJ`kaR>U*u`s69*KDU_u>2T&xQ_%PJ+Ko~REg10{ z&vJ=tm<2~xakJJy-xG@%_bM9V6HW&}(J+LBXGYoQscAm!RA1>-k0MwaC@<6&nEu@1 zk!N~%J6_p?pQ0lE^9#8*!k=|`5-Hd5<`(N&H20&Q4?Dfc*>_Z(DX-tMcW|0?=$ep} zEKgoI^uHCpq(8t#slN zu`@Imh#c&&9HPhhMBmlOfPZp2xW~%&DuIE-AQHr_>pHq zaIJ`*xcqzA)6CIa)Iz0A$ayV}ZW;B6arl%mG_RX{dFDJrdsK5`w`iEUrhDnosi7j? zl_x{Re|kN1e^7XJ9+lN&JgHS0e<9yQC7a6F@H18hIiYIq&;x*J=%wC_KaTcrJz#+6VjP)SlyMUJOLI(wHA4}+Hp z;W*_h>u1|F;+9BrI(n>aj&j583L36rDCAXBMcgV9?HSPgssfx~FpTJy20-hyA~l_} zNsRyq$#|tzYVGu3;Z$C3?%+_iJXB3B7eGi!fK&{ie9nCHz^B||rr_m!RJ&u=Ub5)# z2fj`d#M2r?Y`2g*;rBDrX0=mee#8$`s?>{oBvEE6h_n5ZPO*;Eb@I^g7hEb#&AQHI zk>pw{DH=a?I@`levYV5y8yWc*sj5krLo3yDY^%n-Y0|FTyXT(lcktxq%qjZ4h4w2^ z&3obD$fcuoficE5uMYXd;CD+)+&US}KXg=mkFjWg_EMo{%Dp0>RQdBr%TKe@olk9T zBhYu7ogD+rz5=<$C0rXN6c2o!D0xdD*R*2Z_HoJ%Un*%kmYM8Xnq$N8lQ{}lC$0h4 zJYqMl8L0&R^Vw6*0M_tbK*QM;@*O}T{}M2-6#|5OAtGLPbpY-R!0qdX@jrbc4QL@T zsZkPXg~3dkeeS5%Iu-#K>?cljn%&XQM8K%j!ozD=%Q-rST;jwtJMC?e7IB#kb>DZL z+Fhj;{yJfL;fFM|bXO`NDlPF4QdT9l^>3{aP;#j}-sFzq@X)j9Sa zZmMx(Xz6gy%aD+K7@<_=BPy!?hKyIM@P%mY2b}iCs|P)rRX+_)hkRLCb1bWde(cXY zCY{z_XXfiamlZK#IFPZ=eCeeHOMU2%OIYgI7pKUjR##>24W2KkoCaS56IsUG z3Fgvhik0e|{hK)b{q;(MiP}(qaItrS{6q6&!`^A^Tk_OhSs|{D_0w$blhrcJn|h9C zW}u$lncixjTqGEqFVL4<5*=w+a-EK4By6v5VgZUMaBkoeM8NiUhswNoQhbFHH6P9? zJSNc>eAVZSWYOxmzHH+8gM{=-lEhixb$|eC1+B8x5$L4G1{WS#(nclbPeh!%z(JSn zm!=}-FOIHhNb*_xY9=hT!~Ti?uGCc=k(gM7xj`7q04U?13+(KokRK#SzYRK zI9VO+hlW@E2IRV5y&B>TA|9>$t~?w!j+L5IrLt-7aW3Zf@kK6T;~!Ya)A z%1yBQ@0S6=41}++t+R8Gz@qlv8Z#sc&8C?UtT)S>&AWG^xdcOp6sCu&Sz$X3#7f5& z9b{rwVRNb#OQH$kd2Lrh45}xdjmyXn8W}cL5T5$P#5}rn|GU|9jn@_H@60zFHvBq! z6w|-UDrHv0#we^>w-0WXw?B_1H=oETDk`EjF&-GEqB>bG((OQdj~zK1RQ0w#rN5$D zl5Ikh(8yeIV&%P%T*zQHlYNa&o+9#~9dEC3nYx7FEG}yf!Sa{-s+e`_#B@AAl(%l5U$;{Lx;TbsnV9}f7mZx_XL{F zmL?vGFF~LtfDM~w&nVUk7sI7;7k8C-4Yq1%!>7CtX(boKZ`@T?BNID|Cz^FyyhU@N z%{n?PwB$=4N7s$f&w`Cj_9Ih8Sf4ys^Z?4mBqxVc0)9;=M6P=nsnTbGKU6wM$yg5TEP#oard^>znA>%&msOKWOUy`vjKtnFFYlg) zFMkvfgXugtSuNjP$-*lpPR^kQ_eRiOLG^0la|MQKf|%USgs^A zl#^R~-IB8`Ib-SLEZ*Y>&TCeN4}5Z_HUTNH;i;8d%@_H&;c>G*DsE2IYc=RxWiv6E zg{H&vw(L;$1{uFd(wwyxYN2$Yc9n(CnFV@1LYM&uGxWN~wK~Gz;IXeC02w{O>=}i8 z9rn1`MPe5xjgFQ4EcO^IfLmmAGTvr=WH_|553|!jphN4F(+6M@WU-$a>0Vx2x^0Lq zsU9;_OoHx46HpQ1RoF2q4hND(6Ps{ zxP0XsI+TTVRk=nfM?{%cJn-X-dGv{oz=IGLrY33BJ24<5vvY1LH*4UNz6G~hVY*GU z`eq_-*Z!iAT7$l7yzZQogCu4DLQ>zhlwRr;{M2Jn?HZL@(|FZ zs_(J8ePi|fN-4Elv-`gNd?R!xaG-wTpp5?O=3w=GmzsI~{$ZL1mo=PO`a-2}1ILz{ zEd+I>gqhZ0p1kyzY#BSg^_Trzg(2 z)vCejmT#@GuvjQX)!Z1_P8bTcSCoPOEzOPFYnyFe;qMRS%Bq>{moUq!(2!eC#sd7+pau{1gs?|rSZ4mO{D!`V1W`caoRGe%7fw2e2PwQ3R8<=Yh*yNN9WMq_2h~i=oWS-0ZU`D54b0*@PsohT zIa_4A#Bl>*m+-|LEuPbvVl|D*!wtv*!3!%}?%I&p7WGVem5SY?3;7wvGE=8dBjlnX z&eWp)=gJ)qui`Azs4JbbYt{8Dm5ffD)NO9cZSBu&lXU2;G9P`KUP25#Gj*DEA)Jm3 zPuIh^p;qtp;dCL}@*8$$(>=CKnmv(d|A)A#9m_P~EHv4JkG%$G6}o-!)4SO~unKnm z4|S`0RJ4K*GJxU?JeE_6(qw%v9F^tIS%q%6yYls`^m95Mz6|Hl9LI_{!qE1!oa18J z;CjOS{Q1b%e8G^*i%bFRcrPjp_V|!kOh3=7knttK>aFiTva&p{;vkh* zeuG(}zHNcqu_6{n?2qRDCH;m8Ra?%ZP+XjJS^FA#N%1unv=G93Pdc4Yw8nWcl=KV| zhv)BpwaKvb>W#j7_kgy>r~(SqDg@$n1Gt)fWTAc_*KwC7EpTc39r8xs%kmvcAE$K9GKj2#?Y4B@=Pqfn)8Y<{@x?L}W+BWiJ2 zv^7&wak91wd&0TQv#L0@D?j_eIhfhJy>FWsqC`to_f>RL!Ric*gi+Xy zd0z3a7+VNmV7l#eM?f=sKR__EGHbS3f2fMt>d)Hg#U8Y38)* zMC}IrJBNDa{y7&HZdzegVe-r1h={dz*;Lz>#$+HDk?Z3j z8J(hsA(A*&{B_oLEj=e^q`p8*tiQ3XO(ya>w&iH0onaM6(>`qC z$>wv^@D~ax9lQ$E#-c;3Uj%P!qoefTecHbMblk&K5qRiuS_jG4@TYN)^d_CD2}ADH z&mv7~BP~*GM6wP62N=-0SojeUG!Dv|9WpU=$c=Rf* zb&Hd6zk0NHQPcJNqn&=G+dPsUi$Y5Or?^qp@d-jq0t`GV5Z0~}5%CB^OQH=hp;d1V#v4Jw`T zZv!J^{i2VYkSWm;gcjg}36Y0!Zpo00<^R3vY~O812 zC4`+)zE210qcZ|9)lJtdy)a8IP0OWA=DDrIbh{bN?jEE`9^^!=ZEV656ARo;QYbT)>p?&6zo%?Go6JxR_!k3? zk~6FtViyczAorh1r(2$NR%Pv^kw<&4Ac}x*nZt-BzYZ{&guhHC7Sy${c4KU@Qsg6;Jbxg#KMziyLq@f)!$3CL?^uy(1H=vFb?9Vn8g zZ(^`AC=9o{UCQ>G@^h3`UMiE^W@PhhEc1C<{5%)FBbPoCR}g)};2_r$<=<6P)_N4$Z%R2|ep`>eT|vQEAxmQ2uIpDs@@sn9{ED?7WNu3|FfiGU9!5054wHHZf3Aa}(IjW4g?<|&%^ zb^`Dlmr7&cwVgk4x2`2WH%iIah{^yS8MUfRTE1>Y&hD|e)0AoRmB;Xy+*rHqY557` zvlNAg6>kiezDDEkA5mUK(m6pq-Mu%SL$Dcd3n*#Wh4Ucb@^l*23ZOG>z#oT(#pS>h z=l*>|P{o{@7J2pRqhf)2c6k7dl#Fcu%^3JJLh4LtC+6rxaCkEdx>Kx!<6a`uf{APL zt?ntyg3psY+AEJ&s1pkn2J%RO!$!oj3<3y*Vwrk)K}7h|7iNk<5BM1`RZJi6J&-l~ z0mz33eTm((03d({VEYLqUCq@(iEv5eGz7?j|hbLQf~cyNl(xlQIIJ? zpm8u@fS@+n#|L#|s-|M%)DS2i9i5cW&`=h`zQ+K3T@)~ga?8^9Wh2S;lIgE_wgU$^ zl5~O8a6pTK>M&KkMIV;4P2fGwshyj1t}LWX@*#>~rSPYUeW0`TXZx<{WK3U=tX+eS z@bi3I4LNH+VQz8u6t4eyJ=E0HpA!&3`j@!0Y#M% zjbdIOAd90a9G!T|dTm2bU*D%11w!6D$*1-CQ*4r$#Vfg5p12NDXm`pf+$6w1oQ zadK(E+Hlpy0~d2~FT74Y{}_A);!8c^f@uuHvu}eghK{Zdd+|!k99Wd#utr&a8+-k4 zGyqf^(#ZGCfaF7Dr=8+xU{Fv5DMx(I$l-*)pI?af18YvM3*G8o(AKkXxHDO0!wRs$ z5rCboAMn9ZbjNcY(&hrX4L(LvDT$%92FZbTGm6E^nPhIZpR!$955mt=6vC#XNKYwU z*6rdkGW~IJsa&A;Xf&vI&10ll*8AjrZC6$WK*~qX;uRFJ=^W^4nMT-2=-)u8iA;+- zTxw1}-ez$uK86X93;t>EQW5$6`}YwumY?=DF)=ZmlS;a}skFEOKMxu&w#yuMbW~_@ z0g;)@@)S0ZydKFpxS*uFNwIbL0wst;S$~@MmA6mW8XiA+sM*Z$_-nfqBy^#`suPGo zw@NJZ#4&roLTdE$9eHF#TFLuSemC!pUSWq~e^Uy*Qs!mon#B8*^a{Kfl*^HKa7q*O z0ZvuN16coRaHBaVBe#@=g@qgKzJ2><2v9k43JNw4s*zd))CbS#Bh{__K$qaMLmiM* zB~W`LGWP(KEhYe^&K-91=FQKMk(sF?!6!rwpvnkeNd-UAP*KThY9`#gbqf&$dfYB6 zms}%#eMz;oF93;2`_|ETnMoO7h{h-b^xMHa&e_4`hq_BV<0+UYNx=yl^Pv*-tJrSv z@U0tea8qKxXIT3(O#;*|X|K=+u}Uv$PRpEz^bzIg*>Dp38Hc zS3S);`P8&^+J9gYQx#l}EO(|FoF0J^%y){D6`C1vWth+4vo#Et=ir`zfv>;{`V)|a z04FE9QyR{{524j}z=2AsDh)!-kQm`L!WVL+dm1=^tH80z41>vjOB z?6DX=hjkihc6PQ{cXzjcaPVPGFVGQd0IIAHNr)^p7!1-4VE^%dt!NP*aZ?$;c#kM< zRkO367i+@e zClNsIXutj(9~=}7v))tiAjcFKteEW+6dXF*fum7a?p)Q)jf9HWFDQL5r!{VBHQ9^- znD}CW6%As^R_~l$o~c=fUjzAh;MH73Dr-;{opwJpqf;ps?2KfH99$g>PFbTMUK( zipzbpOqsS=S?Z$b1qX);avk=kUA9)i>V1OE_k+R8eSl`R0+@q-ZYY8y_IV#%K^OaF zIBNDQbDPLu%r~^rlQ4H#92L%ARf#{M2|2A)GrBd%GPilpznzruQlR-5L8;J_zQ`5( z-6}YyxR@Ba-9yMLud27_8Nfdv>lQb@BU-xn&&?(kV7EzFAns>UOTLp5v(nMnct%`ZmKB$w&_%Z!^NYha=70c zP8LPLcuhK01gD`;roC5UA;+3(gTM%XC+uEv@Y)Mf&w*wL2iKf`@g<{R??~|FA*g%Yumj%UtHu`)tqYe{Fh5U!jV?W&EmKWv%zy!ToV|+=M018w!C58D&h3M`c)Vx=v>Ve)z^^FRtc1E)Oyz!y_KT^vscyZndU3^%vE{F?e>W5) zF8u*cgG?l(@eMMXfJc5ovALGKVCMoY%HUUnnM3<4{E^w(L7wOgbOvdeNynypvmeK< zJeKLY^E+o>^uVXz;hY5ZaTZeb$HwLY7_r5L1+QvCurerq76d_S4moP&vq!7hX6&ZG zO-1QgKUmu)#>_o3XtCaOpk5rmuQwH$e$>8`Ophcc<|F=!%uZfL?%>xXhhxQvZErGr z4>LAx^v8TYFqz?TY8+k}+GUUn2Y~V$8KZ$X}34oj`Cvsju+Sx_r@afFKM_^zxjqNkDD{Qb)qT!0=~xo_0No zcx|3U@jz3+SBonTY<;7{QF|JFLGaN9!`;}haCSPTp~WX?zqfhY%hu>Wy#T*!0{mDI zPt78LE%)dCtVFTWex2lFe5ffPqRE>QoZ*@_W#d5OY*?Cc?vg{^z^&r<3{3=1_Lutx zqL+``h+BA`pQtG*Wz5YBcK7ze!@{uFtih$ch!gwf&~FCM1=w zX2>7Aribd2wI}FpZF6KcXJcQ0rogu$){|Q@QwN-6y<#Ju1LZ%T9WUHfxOKUT&No3M z4<3ywJKkhc<{VU)nlDNmgcLwE%K=5*kmR+OB+peiP1S6YrETyY9Nws-A%oHh_cBic zN)aR`S>RxBOpwyP^{a+-#jEgJ05aBuKP`O32r z7=M1wONWA!%F9&i6NK1^@+YQN+$dKS8)$=x7$3zq>lJ;0UJ_MqU#9()e=&UCy|0l^ zI}37<{qG{^Fvh{xrdCx>167zOIS&wTXPJ=BIk5&j#9p&bzF5=>uVpo5A20jFBX?MN z^1&L`FRQ}Nt)@m*Ym4vx1v6F^24`hU0JD4wH~c{e_$6yfQSX{E}<~-(&KmC;}>-Z z@e}^gjIFo=KaYeguSpb8oLXf;o4qAIWSWI8yNJdLDww}hj#nVF-k_HXQa85?pecUu zJ~($sc1ZD<8M07#jh82>ck|>`3+8>KAFrVoWFB1ER0>irK26kv3Nq;cYe{UJ2XZiRAh}|jK*p!Zsk2~7i(d{_E$}uu! zHm_Esza5+%0!pimXQbe1Az?_Z+;|hqod%oApHDed0yjYB@>C{p@h7=+%ezHuQ9&u=nw}i>bwKMxyk_ey(;;NW|NgnNvD(y45P@P>UARo z^op$lK<*I$!tIi!13vzv6gV^4i{Yp9=j>9FLFAHCEYN5NgSEC?ky5=b+>Lto3J|~h zR2qBbwuTCC)aWKYH!xE?ezu5TZJ4Fcsc+oH0K5AMc_E*aEoioa}10IqhWqt+jCZWr;tz1eD zUd|hdFI0Q5=*t_2@!|F_R{+u|2tVS@^Uftm4$6^loRw#JR79-(ob_EBh>W)d(@Oon%zU+l{+5n<$BAy@){l)U@PSc3w46rZ|!TxeKW@9{xl!+CXvbmC@(G}PX{{CuAGTmVTk z%_xpiz5iV|*!x%X4mn{k@q%WLW*Uzl7{#0&wilC)m%414n-SGO7keW*2Uk|%_FaMX ziwospC-61t{r8N_y*a2oU0#3FrkBc~`%Hzx>NEf`nvJaBe|? zqP%?Az~CU4YmY--ZEbBuCqM6f0s?~HXXaD$8^mUvEkloyWf7oK%0&9i-OX$!7aFI- z3L~U`RoePK+AH6=V5Nw?i89?0j4Emk{EJ=3?(fe8R&NdH1updBt#JH(;&u6}bzO|N z0pn>&wYjY=+{uGGb8u|Yt>xRdBMl_;Cz?Z%u1+{&!c=c6FJWSa^_ceq0{h6vdO*f&9Jat2Yo_SMjE#-gwyRoOg&RO`Is~GBFCI5P1T%Puj4X2HoiBsT z{!DI}+1YcqlW=M1-d2oEq_4T-lqR%JAswlXN6TvNhulqWzlh3<>*f$Yw!V4g(tbnw zbb*UoX9^-N)e?QlipXI=L0l~)oP>%&n60a1I6tT*k}rX{w9 zC7aC$`q_dzw(&DoQVU&qtJj-B@6z#BN7+tOVD&+(Ng0=RMaKDzf}{$s(6t738HR?l zVycsErqjdsJgO&#F1-J7PM*W6-hDrwlMe9SO7m^EBOZ$b9#>>)0WiDdlS{^=^{0x^ zFcdDvbpbWPh3wGn0W@{@)<__6b;xWfAbcDKldt>U46d_z=FfCnZzFZ_bnV#I5ss32 zG8kw=(i+B37yPFe(c?54bZ&OMyI4IR#Ko_H3c&`I5VWD)#m=&o10l|X3SDZaQb7vd z7uny@i#pU}vY(7%hOmZ?qI>ygeoghCQOW{Yc-C-MD&VaLR=RkBt`rahH-bXU65L$*kk3h+`%JB3Z|l?gUZT=%oKsi7paMIGf+i&xn7i#LQ)3^G-J3N%q=26xch5e z{q6iXOj`NUDh421*ory7wo>`G`^(Or@tZv?6H1p3@6cGru!~0cgQ{Ok@O={?@*5Xl z-I4AiTw{=RM;aSl^s(u7+<%sDE~^4@@i&NFWpD$8YV0>cwI4XodgH;r-XInY-2_C@ zBkOMS1T%fDfdq^D&T7F^_7i3&U~nfhx1d_wVahHN%xS}i5Nn@+%#usL z7csgu@VSab{vhPww^@apaDaxNS53xc9Xds}?B+$v3-*cbtdnsZHppu{A__~}-LDOu&f)?LYq)7uqx<7YKid~U;!IMy6Gl`tRSv-{JozhCZjqu%h*pW)1(4vpF0gEl_|c$zSS&9DQBd+EI!y1c#1jJX*#_~z=%B1{s6`y9oWgm z9zJ~Nycn)zdv;_sJTwHjDC`Vwi$cCCDk}q@@6t-~B~`}~fG7zcF=U9$7=x16P>52n zVP_QG_)wNRF_Z5TNlo6~ z}qOy&V+2{(WIjuefQ9hi$}Bp`STGDpVoM?<#(D}ICsbI38f1gfl3aj6IYYWJq1E= zmodTSzk5>}G(q|EdIQSu*L&>Dm7nH`BUE#Ix(HI{FW3u0MI=gHrp;SE`uA_w_|TZS z$C%oyeaZzRT)j%vz`6gaa)6O&!x@*O4$ZD6vO@}}|D${PObriHOD!_maR+(N-TK&e z{@V6U_5{fL3lS|=rp0$W=94^LzQ1VNDa$YQ?qI53I=19_?$N9c$gE@7`Ek>OlYmrq z`>JL6qi44TpbSb>ycAk3yfli<)2M+O&lK^tYLo>UxIcSd=C2`)I3OHFK1##b>144r z)VjmJf;=MK0rV`o#lSiOzcyA z8$#X$TII45q6msob^kGpi+a>rvIW3C8p%vQ4(FE7*kK4hoQP>!wahWp4gvQ6Mp;mg zV{F~-kjp~8h0}WWUTN(nO z34Jucru;50L(mkYdjeNV4Fg>b)0jOzXZtTxE1W1~tLxD(O81U>H`K`poV$F`;%mZ- zW32V5?Xf|lExpCC9!o=!}+?$^6+5f5Fe}##VCZEwo=&%G7+0_tu z10UxZwQ#pc3{E&MS?U-&k*VRfy@vsA87j8x6Yuh5?}J&N5}ro}VXSMD<=?ii^AvA( zhU!K)GaOw8bt@W|eOAbj20YL{v3BIZudf2nG$T|*+;%(RJ-*{2-S@ev4@Ivxj7eZnQwNJ7d?-OTb=q z`w8Xc34uzpQFFkCctVJ{omqZoQat(g4<-!>C_(${Bt9u%f7T?U@jRz8fzj6&=a`+#`l>?d>q7@aurms;Cyux?%U)S|}Eq^TxU&HTI z)vsS6@LRtWA*iGLb3Bk8_@VxHn&9$}Qj(G$|MT5I$4lI;zv56Nu`SYL@XCDm?&{*w z5;ZTc*5zd)z01o-(#$NfduoP6M&kWr48UMb&-K4* z^1mbomrnIhT!ZXp`0qdZzjs*tz7$B`mL=#hqX#`(|M5Uv=G&=hX!cWy5u(UqITkjy zUYTkc;DRctM-=OIzA01XQU~A_E?+>&{Lcf80W2CeB{lT`V9o+_Z`P6tFH~>?$bOuH z0e9s1=>A*)L-}PD?Aq|vc(n?1jeqVvXiR3u``dp<=KMixO9r5}&i?X6CIF9tXZiGa z>*CyE3Lxv`M*?5-dXe~#E-*mX|2yMw*;ryp|5pSQiR_(ZZGpjjs|Ek86-Z}Wd69Z*>Ob&N zryo_(VB_TEq}1B0nL!lHz>t-C5HS`MdLY(Si*&rfSP}l`!e5Ek%LB}Q{Djii2~ZTW zIBb?|UAO~UQoxk%3aEwr4BepYp^gP~6l92qUR}`xg{$h-4A8OUlNLdo0mMba{Tw;y z7Qj#s3Q4pee$vcC=FxsVKPbRJmPYOXMtvd zvw{kpmYYNz)`EaJL@)9%92BdNtH-TK^XEvAYS>LIzHQ~(ApfU<;KN2gvXg9kckNmmnGHjV100bxDZ?!r!W(<8cI@5HcKBp{=PsHg{SfSm*By|LJ> zDS)b~7!wmy-8AMur-8tq^J966Dd#Jy&_-au=x_rj%^lWZfS{ruY>6KZKf$!kaKi<{ zB0M4@rw9-h3V-ZFmfC8z>eaxB>g0I;J(L&zuUzf#SNa%~k%fwXAAA=7i|Y9I?WF%r zv;2Sk_Vz`I!?Lz)#3?b1xAzNWr6<+qE%VAq9iws;qTDbledCyzTl$LhMvrn+!}V2- zt3F38@+U_xwNEk+=B1h1e4WITdCM;=Yo%+jbO<|RL01>nPI$P4s4l4bWPq4_J%gH1 zc!(@ufJfugzP~i*i>s2qyr=(P-=5?L?B1ZSpsBgJJ813;AZ=;WDWZJxI0XbQaesVtC%)g04 zP07qGH#P=7X)W?(ov|;{UH^(fH-~;0X`&%?0z)9UFUgDK_Noyy8TwQ%sAcC z6B14-%b%Q_?0kVZEF5>V%q}fusoNh>Gaku{7LTCKmP;334dUADOA!j#B_zPXAp`0Q z*&{KSA?dsjBBD@kssb*K>dBLrp{4E!XKn!W1#lliu3o*mu(eePq|gxLJUZIi+U)9L zSWS#COU_`3_Hb^OEb_S^V2u&M4OpDpXF&FNOo2I0yaq+K8P^hkrj*px)%5~~dgDee z;J6Z(lpOwW@2LWSVZve1R0m&ZI87|9;CTJgKlaSrea>~cIuOfxV1kT*zmTI;n1CGQ z1|(|Y$g#ORm69P)dRK?(>*=kAh&)0jRut--?ezbQ69kLqGo2^&C%s>J1Ir--2lupNwAJ6w}9fX@uM)$Zfqq=T=ip{}X>{foA0xoI?jB^b(-Ccb{-hJ;+fA{%`LAq=oHv1&J= zfUbya(0@Sve^~pQl&b;oq^`c+T9bl?Mgo=h zFcgeMO06)P?3|ob?~0-u1Z&Fw`W31Ed_7P3D)!b#&gT3h$?#y%BqjKq>-iz*2{axO z1tgSFRQ}6kFxUg!m8^yx0s_)9zVxfVsU9H19msxd z!YL%e02*i6Y8B7pC^&;d)UEdNhghf zqc`WcON@mvnZ<4uXI7`-Vw;wR8oSxq>U6;sFv|X1}AA zz~`z~33aQbR?2s1-QfDP37)F${-T(-m)B|#`^@iBs6lB*U!Sz|@n-eBEdcY=f>e zxBDVA0ICS8m-Wskz2)`qVwixs)i+%3OV9z%RS56#cMy<}ih-}IUbiLSJO+ctK*Q_z z=YUsvT+*Jv?|vOySZXv#4o;s6s)yA4`-CgnWbt=z?}+QkGz?J95ep|@A1R0foB{gY zude`)PXEIPjaQ@P9KbG+E*UFSD`bdA9s;GN2DKpNaH03_-#4^7J5RdzgolS)H;>=0 zk}~%nz4IicpfCZTWLUcSbWIKj0Ud*1Aqj#_4(NsO+Un`9~cXoFV0V&J{@%*xLHw+I2Y5^kRJYcvGk&uSKxSq&}2%A=Y zmco@Wpqi2(E(9#Nn!>%ZkNpC``|$%OV4IMXG&ZIJo;z6O@^~fqtV>^YDD;~uKJWvX zV1}VFu&R!+q~NN{WN;~%1Zdk=0C-msFVF3os$K?;--YcS2VkmXwlnI36>mWXK0zhU&Y-}!U!VQ44stcndBQq8kKZt!=RRKf7_evp+;PoPl==`DMAAh#TBOY%M8vN}xCW2szlVPKV@t;-nc2|$O*Rx6Uc+8 z13${djS_1&ap2j7f04BsI(@f_BtS>t3J%6d)P^2cpVtw<#0Dq)#fA_BNYqoAuIEU=Lh4wDp z;??Ci3pQ><*dHjW83S8}4JfjK0SCgI2B6-Lz_SB;+7Ducw6(#kJl)m+ShNqTlK}^n z2kbR$wiJEh#_!^pnHe=O!UN1(&jPgp6EF`jmPtPVSOXI9+`w<;fd^h)g5&eeklYtU zIaJit;)7Yz$}%#cF!Lapz@@k`TBHm_5@w=!cbnjq>w6ynvwjY)0UfG$w%?kmR|g*$ z1)F1F(^<4P2lC7c8t;HD5d)XK4Ve_xXM(!LO7#lA5)Hf5`;M{z-b+U?K952 zrepKYnB0ENtPSjm`hoiaa{xIL7f`cVlfvWU^MR66a&V})xw(0N2dx5s|DB0RNXUcu z3pT$Xl190Alfz>&P3+z-1eE5$<$x8i0x=mmxp){*GEkO`1WJ8+>JUkb^8J$BFio#qk>C7`J6=zLYNE=4v>SpN?m~-0tdD=S;ZSki$O8VF;w&T|7158ttR=gRSCkSd&Y z1lX5DG*FEn-oAi+5K`zJLkdAX22*Xm6TlIIXqwzW1Tt#?whDF#vE3Hg&KQg5Y|2%1k1AgKt(;yWs!jU?5H9u=LR~U}#98lmueQ3^p5_Y7VI)974H$VToZ2577wJ zDIGU8@P*9Asw}`v$OB>~Ipb+HM3mlI270N0fH0>MJ3H#zpPqv6Gx7M+dS{ep@4Y11 zLWV!{4=mlWS1wSOn45EK$Hq2<&eW9S{C33M@30v!RWdx;N&JOclAC(`4%vuGA&+ zW}pzH&oyU+@K-bA05Yz}&q&0qf*@0QyBK2uM|z>uTK zouea|Bb(O9KaxXocCH1r&B5Chf-8V8V{l1Y7ar;wru>E#ZD&Gz?G zKo;xFq5K#S2MwGb4eTu(zjAh#ASSMkqrhb=v#3=&xNi%e_?ux&`sn4lkp;9q_e#p_ z?7;=qf~AixLVNa1yc4xn8jwAL<;}MLmSP}uQ(I7cILjzdM+f#-L%qUO_T$IwTZ0zB z5+{kvB6>axpQFXQmKKses@-@lhqpbSB;k8`aInyxF*1w=?^R8iPq#RC8NwPNH`5Xw0u|>NjEo< zJ1Y&!WAo<>KD3Cq`fh6Jem;}zSV;k%_h|yOGdzJ4Bc7M zPcmCb>OntR@8|hE6<_bw{D3oub989Ya!q8z(PZX{qZr?k%cA_q%6xm6q(o;{en?%H zQ&(1e2-9_^%v7FL?yDM~%Z>A{Sp7)4E^_rK$&v2H8`G|mr3w4JgYLSE{G5@E2B|k= zLzdyRZdhldC(3CIc>OPK9YB>1uub5fhkw43(d_HBX4H)Hqm1FaqAAhY;MCOpK6f1# zS=neJ_3BRpyeDJY(Ogz9Ee=}ZIYFr9+SvN(Y}tQ)yYd`@*c3!R_r7#I!kTX-HWr_L z+Cs8xa`*iyZgnhyOXpd(L(jZp>FmjT_u4_yXKx1dZ@kRHg@qFWCoDMbN2K(W#7>K_exoUgQ2 zy|T$na=1lPe{Ou;bI;1^`0baO`h3lkA(M0@^Mve#p`PODMr3g{NRA2oUryYD&|C)GHM>{ zEaJkM&9{q{Q=Sm>;A1BLAO^-T|ZTPe7vFDzAU$brOTpn%mb<6P{6D{p^;5`u$5v1WMv8B||_89VtkA z^J_TcG9p#)Nk~UyL!&^H59Rg|N92ZzRBJ8-j`Nf>oyI{?-f>M z->g}!aUFINN1CguDJB-^$_d;m824=iw70hhV32q){XGpN12n9xieSQ_tcuEagPz!o ztVE~IO+WB%!p{hgPh@M#Y%aEd!RU?ucD&ugyCLLM!(tr4V)YUkt*|>vZ?FOGVo|YE zLQ;wKn>O8vc35h5jogArkSuG2Bdz=^PrXIuQEB$_>JI-7*a9HJa9w5)!cK!yDi#(6 zK!TV6F>gaVqbnd(KiI1^yZCI11Of7Bc^$FWs@nTjY*2D7AgBhD;9RJSRh&ER?=^y7hj#{ z0AY#+iFtSMJ=AAdSO#~#=5uO`aU|IC%!_9@VxB@eR(8`>f56;)V#!3)B>hx9kqf)3 zsQhr%0CyU#C9cS>;TOc?(dpUPveLkT#zmk6mpG^otscDHFIdbY*s`bG_%MR38S>v^ z3t(B)FpxolSY#EXi2%CFg&}GH#rRI}gisfBe*F!SD5*bvN_V^3?0I&uEGEroWS5%S z6=SDdRxb%E=cdi*WVGQ*6 zMtu9GV@C@-z)LEMcD}TxtfYY#vKULFkp7B-p}J}olc+Mc`au$6B7jg83+gTbp;AOp ze#MP~_-2;(Ghz%*s;l&ty#HG=1zSZ7+L*;VFLe#T69Tn<`2f z2Mf5PMp|$}O}P3LL56bj<}Lv+$ACrX0LYt#3ntV~ELbPWiz9d-wG3Qun*`oQ$i>Z{YtX|#Gvx$RKNaw85D2yaH}5PQ zNbku3X5|W`1qN%u3Ydxj<%D$f! zX6HtEr?A%%AU++Q3r~Xbo1sN|X(F1L`xfhJ5@x*37tW8jO#bEz%FzjQN1I}vUNlza zHT~74L&!jPJ~5X_R!r^lc)hTxuu z31A5*9W3{h>vv&!`}o8XfFPlX3|vl%M;b+6&Ms=R`4JpdNjyF*vvPAaI{Q>Qh+%et zusK-V7m}m@PT~yV419>5^s3LstlBK_()eO6YRCMWvK7Of4MdrP8Qt%9C{CfQQq)j= zZT*{h_nWjg_mu5**!(eR(hJmkq~vW9rk#{T42WcVidl_9UlX?GVJe3&N!r+TscyMD zHJ+Z_h7Ysjcgd=UNoXV)@1B<{n+HQfD52}nrjRAJG!OA`4oUr<&$w6sIRL}H|9rJ? zv0BvGr^LpMqM@)Bm7#bOe$K^plZ{4B`W?1a2pH`Ft5`}E>og<$&A+R{o*<*qD2Yl$ zi4t&qPk3}@HC!{dMHhstpY-{|g&o*GZ#x#@U`$e9_I9nU{@&auepMafG<9~H39HS= zMPh=R{nOU1LN#tk75!g;qN#j$5+33q|1OrLWAhPY3dt6}@FlbS`oz%=g8%VWkCa!J z)lyA=ny*40R;d_9*{1b47vsD~r!93B(+#zr9e&m4+5Vr1>uBk`(%b#x1%_nRb^u(7 z{flY*lfD8X&WC>l$1>%O?lARQ7lTo#rEQ@GyNgTuH!EIEq*1%*QYuHK-lJA%+%aawvtROCX?>(rx2xf442Z!uK1Yy0+| zLX#CBn2;UF3Q;J}WpS$;7n_;{VfboG0D3NObw%A}J#3YVxsFuBNy^&o{QQNd?DN2) z!^FfA?N&@)>9#!jD< zYDEcBoUQ+ixQ_n;JjLq$gszQJZhrZxRSp7mmu>(pPI+?>v#3 zUJW60OahBlZUO>?o9oJP|5-}=9K5+Xfa!LVTh*gwQBc#7OWMv9kTf%P2YbFu-dhM# zY;XprP_JyrmH;9X2>UGk4kx4v(35a+sliH~AVsl+R@4}*DM{Z3i+CjWQH`JjaW}j`QfeKxRs38V*1Kz zi1UriTLB*dcAs2_;rTl(mwx_Vh4&Dt{qtgP+8&G@VG(WOOmq^@m3iZIAXyq-6)i}D z+Ohje!P-=^G%eY0k9r%(25#3bTUlwd)KYxkt2;}Z9D2vDXwu8bk^p5bd(Re6)Yyba ziKz}&;NqP0vx5xyhv2LUdrf)L(nG=(tX(9}rWS$9V7zIi#*z<@l#HlhP;_?M`PS39 zmJP{Os{w^|CD62>2rB&`eo_R49Rm^1hmK(SNzWbZDf6A4MKoXA!XotJVB4mU z+#=Dp015vtdua@Q9k zF*I*-Q0iW*a(^L2mMf?~{jLN_us4qvRrluD)drt;#j5ju#$~J12Bn`+Z*|=ee?e3B%HMF4a-VO>2H%dVA zrLjmkc3zIa4&ub5MUm8v(}b4-L?ec~0UR96WL9Cp4vrC&XhC((1XM^60Tc+z)klNU z{8XSUfKL975iCy#3?}>M?4Az{qyxN>$i0kR^m0SnfZ|+zcb~GygpEy^`KLGe7ssA9 zY6r)NYB&pz(zfMb+h(Ztd7pcF2!9kB8aTw*d8nUq?>PQ&tU}i&C1vV<9swAQI3seT zus$L;akeUz(Vi6-isX^^sU751-2g|1jLw_cK_K2c$CDO)r3K&$Ay~BmRNeFhUIHOo z0fJww;|aVpDD#5VfV;L7S$Q-LGJ_Org+r**P5s=iHk57Y`sH$@?FLN*cSijc@$|zb z0Xe3GsGvm4$~|$xY{Z0bB=d%U=I4g8sMKI~J8JG}xS~y0xB5`F3rQX*S!9*eREEtt5PhBSS;fkfD!@~jyiDe+bA+}r*e4rBS4wP z+z#L2xx}kLTn4jz5GhVPs}SsQGbi5(yQIFQ0!$I`kHZmef&l}@Q&q*|ufU!Vr-3<2 za*t}bR;xMDDn(x+;ZSjx#LJVP=BCb&EV))Th}-VYNu?)pOZzFUBrDg(hU1P45PqM+ z%YI#zLZ|L>LX+af8H-o6qc+nT?NQ8Sr`|QOJbIqwKJ4;~+OjpHSos%*^na$eN^MrN zveJ__wGkMaB`_tF6f)!@LBUyHkzRHDx>#YUu`)6@|I_u-hgSh@i+*Gh;?2GiX(AFN zBI1g9u?fFf0f+`Oyv4??3bq{D7~h-Oe=7$mvyF^!R~So&@ETw0>)~kmj2FYq0fgNH zR`qcO7rKap!N65k3$X0`6`&~89}Y+uv_od!X5&#(=HvheZ>yFDHdPlMC#HQ^2s5tA zPO&S4rg)N|<{r09NWFX5gt=FWcw1Jvtx-561A_?)u5W4%dbvTH7enbnlVEyg=07&{ zMS7;Vj?O?)V~x3IOO59S%j(9P0N!V5_RQPL?IG;hasi5M{_x`UC-lPlZL7h?wu&Vx zR2__^8>q29W%>}GJ{gx;DlAdgP-cBTfnyh~J3yWR;A@oKZ zaN2B~zUlS+pte)V@8pqryTld242)NmiDRE^g_-KGTUG^1o#pFm!h}*)?ZHm|_N~36O|v7B&0cTpwfwAC_=BnZ~=yt*tzL zycgiiDphITs7Qtz!N`sBqNCw{_jT3@4Hc7ErHgyuy&XiBTW8O6AC?y@vS3+0u#^PE zhcx$_3n-WM^4A7X>Zf57)yM^0G+62I2v&Xs0Zi0J6{y;SE?nBav;WWN27ojGvhZ-> zJ-)CdKFCcKkJbTRU~hD^-iSx4p@V;b-92g+GL?D0Gdj9l4>ei&>C5VV$c8*tBJpjG zcS*Th8%*p16*`lP!+z@%{#=;jlnANN6nU(9iFWx>;?` zRtnZ}v7@s@BsGC-k&MvJZ8E0&< zGc#~?BJuh2en8-8-2?4aPj;ws*PQ&}81s&?LsAG!`Gq0%%!#ur$J|#bmuaDK=NihT zk5w~4Zm&b~^NafKog2OHxELO*%$3SalxfL+RDV6Uv0k1ZJ<)BE=0n7y@c!oY8Ox#U z^qTF_Y4J(qKz+D*4`r761T)|uZg>)L)M&xD~h zBv3i>ry@O@Xw||UE_BHo3!@$dz-%Rlg_JjsAj+vgaYJCC`;AD7A5_uOmgqjnBby<9=o%qh#BPg{7HEy!F$gB|RexnIwu__bcadI*l1x20)=SVn$|(huMSx{l2JU{-p zwOs*&iNDS1W9SM%3Vo6kZ)^R3Yxj&ct1_^mXh7HH!Ih=^n>&DS(y-DW~ zsNV_TJ@8V@n`Bq|Btm#znvKtMA)|4Q;P*`N$QQZX1axNMsuh>|uhUJwNRuToI=5My zaqbCLNyl)S{{1^NER3E_CZ6Mme4cxJUi*Ma=gs8N1fGVRq;u9<9N&hG#cutmYn*&m zIxcHE9Q=^SCcUOcg&Re<8X(m0DdnTfuvoSwi^3nHjyY^V0weHL+KN-_$no_BIrd}q^ zXI^d|!_A;(Ip{*+T2MPYQ%bQOTkB6&-sO?s4#{}|)WTRUrNeeQ^Bs=S@25uV3Tv&{ z8Oh1_2z>*&(5!@3Qu}ho8G1g3ddO>9Y*nrOj8iaFBWOhlj&z52SKQXFe(woqsd|}z z??0#*lrAvBFo4bJFYd*E-d2Kf6=pxFRYv`4&Zy$JSXt>6Gn<^iBV8X32bGN>2d-~U z0$0k1%6ZiVLoQI&gfne3-d7jwsx>_HTTnn?d*>-cY-KR*7Z5n`kK-Z}^g)T-Yr|Ww zJFC+=CswbIItm*$sNdc|#Uodx9a!vm@O;8sKcRCV<(scUit>X8Wr)DL@N@?a1yjT4 zW$%|pVjgIGmAbObg~-EM>i@g`JCwna=I762!-2WEfkI6X+O2*BP-hrfwc&S5^1Pnh zadiV4km$OdEm?0tpXQFs>n#yICk0qxPwWz>{5=Pcna>3`=Al))(dI*xInqS+HT3d9 zBbOi?2=0dDvm#O92 zk~4mnJN^w8heDL!+;KZA(eY^|lgzc1^t!MB=4OpYni?Xs5nOHmad#l=sJ~);f|FO> z)}C}?6yfa5aBW1r=dsT(bj--wH}hY{OZBI$LtZu<_pfL-q%hdITTNF!=bO5=S|8d7 zSwrdF;VNo5L04QD#-&!u`3|@QzN#*?##L1*E?->A(ymbSGd{!+z`$RK<%3($M|XRw z`fq8dD(qk-VoF@W|nz2zNVP)0Z1xoWQ7Lz=~akA2l`4=k1(4oOYRP;sV z#^N$PUtqxG)&_c?+!v!)V83_oL%v2%08WG2zIQYi-6Q!=>qiAKdr3E<fU;J2Zg@m4srv%RZ*t_z8Xz9$BBe1Xuw zJ9Lng(};6o?0uRz3w=slW6k&`$Lr6Wm;E|h?gowPz1ev{W$qABVaP3ih<^|!1hZHm zul~WfRtf#Y4oc-A&z>E>_5T$sJ8#;iimc77(Zgq&8?~Vq!Bd(4t05?QZ#@rybOUK2 z11~;L0b{T?=evI3Tw{I)m3GqUc`|An^NeIuqO0!09BzeqJjvoFr@J{Om$^K6T-M{B zqT_3QMaPB!-Ncao4vWB5HP~bu$ZXpk;5)lNFt8cUa(SM|V)Jnu!T$+8i@1<5=4QPa zTO8JwMmj7IE25cEp2AiI#R8e*k6%Fz*uP@qC((a3n($LkqngKhEXK&jl=!ID&_imgUf#f;hq|vD%u=fwTIR zzgS?^{ku*T4K=44*D#;EjM(V_3^B=hhEIF6t!a5O6IZ8<+U@?@bD1~AZ@A&y(I7K- zLR<{$NY7|C$Yj$Q4IbYI=g4|DoBNQE<)DW+a2egfTze_`p@HG<5*U^FLy znWrkE?Y>>$_AF$*h}oUf#=kQ%@ zIX8Zx$nVT>NdL{l4E+qS&SC3oCiM_rX)3 zW>5dJ!)3`!d&%E7P*bg%`y7LI;RKq&X`Q~m^o0MuxnI|4P%NvyYJ~*Y=SSh>(oww$G%fMotG`7=Y}{5HhaHQ8kpgw>@L$W#jRrS4-^1M3@eA(e zOKmwpdvc@&iEmzlA>^F@fsk8L#l*-((+||x4(u<<#j$xiHy&=<)Hht)vN{)~Y@cl! zwz{gkD?nWqC32^|+7GmuH1o!TBrX-!eu!aJzhYa--pqdxyT5_qE9HKszt)Yj=81|( zrA-Xug9#Q9%4N7kDx8lD0HYM2E9XF_cp=T&iV`^dO*(kd`JI|ZY}iXcdcEzBI|hirWIr59oJA#oR%=4vG1+IqK|2N5)2= zatpnouX6^#S>D15!O`WR$f0VB5uJKyS_!g6WU*Nsi+%t`lOVPIqLS@G54%3kfK*_# z3d^!5eE{b(>z=IZlg5D@`2k-dEZv5CcE=&5T)=)l2zeZ3;iwI$pw%3CcF(nZa2ZT! zVU@RWovdC{PLefH;ck3e&qY-I~wGpbLqoR-zqn&pQ<)TzZjcXI-a?L*8)_NS?>cc>7ah_mht4yD( z@i1t*8t|tw;}{mM4b8sKvA0;;s>~P~`W2-56qLVpeOo#k6B~1p@^$U-U!WR}pzO%a z)adunl|=mbIN4aSwpdAF6bhLN{f|}t17(b25D*jq(zyv}u|h;fW`&hmDN{>IGq$I< zx=d(QUzS^FbDW%Q&)VZwt#eM}Vy2ypUgRB~;o~qg($=4*F1CA8eJAD98n0v>Z9}7S zS~>DPYz?Q;e^q$BgM&gCAt12XWsXF!?iJ5IyPBFdanM3IU3Y9Seb^)kITe7s^w z^u<)OGwxkur6Rbzu?Ua9=eC3#7g0D%5k}A#4=}v9!z@xZ`H|@m@^S>0*ZK+ zI7U`R6W9&XK~XGnfxL7WmeL-_N0V8u@W&mo`mEf9Fa^PI1VJ6)OTgl_sN3^7-k#0_ zypc{uq}y68Y7~s|pl1=RQYN}SR{Xx`M*dQ|=BU|#i}9L&;>x_@d~-iae;)&9vO)}u zGTZF&`0}ST%M_e7c|kZ|(jR$u(krWB`Sqc1PKTl7??ML#6GHpn7(E<}*tPqT8I@D_j;egujXzS$bp zC9p}!tAB_k0h)bV-P>h!y*OsQV^mYt+;M1no;|@9PvGLTbNi=k0?llZtf85wzdAW` zE_0YCu5awjL`$gc8Y3b>CZIk$ojQfgmQ2-~KVN9;z{Se^eWSmzL}g}sdAXOD$ZQPb z&j|~05GQ-0Ao0I|zR*^5udV@63e6{jEe-+e>N88st3Zg!}Z``t|&8oXJo0rH0PW(Hi>%Q1rdo9)oZJusYh+-2j z9HJA??@f*-#cH{&-RcgJplDVlfSg2f@XD(1jOPO0OQ{~*=|kmQJjfLM|F~~ZsYn>6 z=DjMzz%jB2t4e{QK6uz=F=H58PAj4bHP0E|E&&=9tMV+$s?e3$ug=11I&G6%JqIi3el%Fw~41l&CU<2j9_6$nq3yJq%2tO@4K=UtD(+y3|E;(fdnJu!OPh{=Y1c(pTydbNkMo3)tiWg$`BycB`am83>3 zrM4KOFBjY3n+q_jYx)b~K55}(>}}-79;}m`9~>q{^Onhy|+?(&RR6J1_aRUZ*bNze8cXp{X8e=eNWTDXsnls8BHa!fnmlQV{^V ziYh89h036PwG1?fXtWn-)+&LE+#P#~I4qOFF!eYSbsL*9z)*{bk9Y3;0{|hD`4<1` z3NS{4l9u)z=;`b!2R*%OxUY1D3eCWL2N+O%kZC>l%X@vY(jw6m(7W{UrEuFpLr6di z2Yo9H-KCte;yH8QoVIt7Ke}$;Q+_*1Dj6rICQ6m|`eB6P>(C58_``MKq9@p?vo-Sq z&on$ZES;^Azm#AIW3;1MNk`D(;n1Z;_Gj9B3r;s`JG<)Fkx+jd$0m8<$Vce-T3V|2 z%J&uA^Kmi;B$qO|OEzy6S`ICtdrj$b0| zyDe&T+DYi%Da!n9U!VV+#k3=I-EkTS&T^~|?z}uZ=|@|qbZ?y7v5jyB!!I0KwBGHx zzhz!oa3ORw!r&ZO0iWvypwN znn5*^BA4^gUE3Mu9e>kTUgohOdvsOp4~ltdC7CKB)lvI(VO+8H1fbgb5$o=Q#I~n` z1WP;Ym-WG)-X0#&-oj++tt3+eNmoi3~c8sXEaU{m^)ix+{(XaP%sE81SEd`pUYS_*`ZCR{fK?j z6%pTmJhzB^c_~`D#cs+LfRH4tHKCR0e8gYJ>BdPNncJpWRD)Py;al(gCeN&noSM+P zpt)k6{?nklwfm6TnUv2i(e;o$M+moA16|B!Ux|MMdu$RcDY+4j$VMR+vGt5>Vg8@~rM3Z`ncQzqq)ZU#&*kUc-oTDXyFV7HCoBppmx z^o`bRlo?shLR=1INh(l(UowVY+Mc!46H>rEp0mHq*?IKnNpgD@ub(q>@L~0uDNfCU zi#Munf_`nw(pNrDQ;kC11RoK{M!mh%@_+hk6ttH%gBC$hm7;_IBzbmsZooEZ2Xy%h z2ne{0^MI`gu)qi03n%}HXVElOZ8>RX=!TRv*^y_WtuF2^tNx#ViHj9t)pYi2{7==Y@8ae0%c zHAVuDU@Lnxhy9@%nOH3pbS7G_{z<{zk&u+k0AmppD>@D(VejEU54eZQd;jKwsP2kw z%62q{XyK&MeWmqm!Ev@3Uc=$3Mzq$K<8t<76XhLtl}G%GS`|n=wxfI9!f!NcZ659w zEaYvKIx@%GV$rJJW1kt@HREDDf`~vlI-Ba@`)!9^)KQhAFBCkl> z=+f+8Woa;v0t*E4%Y;+(>{smvly_jApr;FrT?wj0hes1|m;?=Jn;Ybhh%dhaF%g!1 z-x)FB(Gq8o==l0&9%W!{)XeX5JfDN5e9!m=b5t7TRL!%X`=l*KCXcUtK=*Y(7JnN$ zKi79=+xut1fMs{MgBq=gAkB|#Zr@zd9;LhR`zH9$UVkuJ=u@|@qzjY+F^TySq4}8r zl7)Jt3Y75xv&Np$9<>Uzq$ntbgvP6qm>~tU2M(xioLxu*ENI&lIRUHoanAo6yD5h;Qm(JVctK~%w z)VEq-`yGgzR}nQ}v6|@Ob9r+axH2Pcc-|E~FUvE09K%Nt7Ctv9jaNZ;K{O>TWAl`N z-2USjy1yD-g^FnDq)8n^kGWxS>%eci+{U)T4(J6+q`2^*d)iuMA<_0d_P7hPH5 z;4Snp>%<~h=bpzLIr>6)c;0Z7K_xREXUrUVc!-*keUPTS^%}Rpxz8V9aMZLXVyE@& z!avy~ZqFkmO1<^N%B?;G7N(5^D6aqu9a`9}R6P|K@=t9lGupj8*j>-Fyt&Fcsk$cu z7vjeEO}l8bYd&TYC*2qo>MHUlpoo|}^UdT`#M;p-Qc_w#bfiRm241`_3fVKdSWn{| zSw4v2KXm%la1$h>Nr}yKiqLiA(l|jsnAN;Jy*q9=!(ZP%r*o5j<)!)bZHxQMI`=OK zfAE{{LtZ)YK0!mv9a7R7{NihW5nsC`!J+CkRe0YRte2Q@KxRy7P*ST$=VoQaIX(mHH%jo_C;mGNW zzV^yoq9v)Tlh_mfaMum@%`mhlzNj~-rDs@7%+XKL4=|iuv4@POA6LFmpvII`H6spb zLvg26ub6$#6dGyu^rAV5RQRai(@nN)Xd=pFrRPMTb~wl3JTVHNT`1h-!>3auW6lrF z-&3HJjq1;8%3|<%NXS}JA#RuO(?F%1)iW02O&bR}4GIT4P#qnSkdT-9E?wwNhY`b- z0>v`~9^6b_uCCx`g+IW0qAVh`E2u>%fsVdL3+F$mR8=3UIl7%nTJLr0xK~!+P*4;{ z>h-jl3w!JzE&#AA>!NKYUc2;<#U2qGlVNMlNrHGM0}N4UF!y=-D@3%hzpN+zu{57* zO?O)}YLe3peDyfagS7*yu-|;gXd{rxGEGwg_ z%9<&fvesJ0fj-CB^TuZf_nECq7wWA4y!}*F_1$r8ty@ewOY${DL_*vT1xYAPEoZj2e@zGS0KL)n;Vv(a1*`j0X>{t6BGwq{*0Gc<7L$H@_qIiG6p- ziu?Fb;5>`7aRqn_(P|oRJt6{<^G zH7~I>_AhU2HYvThr_p(#si_Hyzn%&UbMHBbXN936u6y-=_maW}zLOuFz?B~d=fKZD zModPAB?FpAf6@odLs?Eb-)Wd?tU6_Uh$@w-S?iC_d(K6`%_sHxH3y+|v9dWT-}hf& z@*Jm5Ey1p3rWAX63Z*$BBHJ@8Hl*6C7raiiB1JncZ7X?23xiotj}Iky9F^G#-K`(O z#d-u>(APaJDb?x1+UX`QHk=~-)Yzdv+wl3m*MsLpecz0DACMPSa2=nHs7Lhch|WaL&$E4(x- zlsnzK)zQH-lmaiEp2^hxK{vXm&F9i(lMvOZJhoI&5C<2xlQG=?CpkKK!SUC=@hPY^ zWq?52CDJjpmdLd(kud47DfIoWZK>C;Nl!g^Fb+wFc&QB^>fS!u2@3`0S?N;qcSl^a-&AF7{QvH0ilun`{#kmuK;DhSR(CF30 zPy{u~-^e#5D2arrV{QGj4Q@f!2PAL3eXY$tkk$viB^+yn?lj+zbkxK7mbv_awfqU9 zu89t@NxqU&^mIwQl)~$x*;(pRNt>ZHDaXR7{lqj&L*m+^Y014gU9@HLKgVxj;aQC_ z6|nHXoYSFCkH{8S1stNifYz6X1s1{#KDG09^}zd)1&{vC( z@NfDWA35t03W>05>EU(cM0Nf7_>RJhIP4M1cCZ?O!j?+8H$tqSOe)#XNX{S&3ckbB z1bUyTshwR1@kgkQ-?Ov#%FM4UhSuLcdWeY~&dK=9-k7?P-Z>HPhP8Yibp13Ld-uf6 zA(2yL3+z#v5?0}0tTq<&Q%e${4!xGb`g&?Fbf3JSLfGrERQWVfypd!;>W@D2BD7f1 z;^^LiL1N16myEBvKcpI3GRD?4Jc@-=P6>3qnHE_NdzWe+`0}`1g$3cScSy|%-@_%Y zlMp}?@Kfs$R)%?lB`t>(8>1kKBrhCh<;)fGys zC{KxZs{R^0eD4{TX7}SR>7ia-)vp~ab!|U9rW1XJYY;xg23? z*r`sVB)sDHT$~JBtF2+kOd)4rtJw~YctFC(DaM$pVU2_ip{-DPZq773cFa*%v5-72XCJibxw(RF`PJPW4wyZFY5*)WHOTwCs=AI56GpU=@O6z>28qjZa6or_3gda+2_X@!-4n?VLZNC|p?=={_?(}?(@#w(4k-UCQ|MX5Ha{rg*ev-@5 z0@M(Bi_-AKeZaLbH{DQipGj?>;7RhxyK9|09b0cS+j4C-|4i}Tjw=4;VZ}Ny@U7up`m_%{}v18jH17?hLx6H zGoILilA}8Gh+eqqh4WQ&WJ084uIh3hF)FAH5rWclq>hTs1QT)nMpFlWIBDaF z-O3nl!=hac3T1vf#)8NqCEOOhTS69?M&4!q6*b z>X7Q#Z?6s=ci*?cOIzZ_1}_Sc0y{}VOg!+0JE5bqv$)`=U^ht!ENmbWyp~bUJI)(F zU$lIM+Xk4jVHne`mVB-fp`Rq{2bd>pf5+Cw2w}+kPVo6tLfsTz@uyI3gD^kS_$GX{$Hjx;eiSQQjVJ2D zme-5b9IrX-5r$-uR<&zcIQ)%sVtP8x-i4kroxl}A?VOOpQ;*qSugSt@N&bD_LD!%(yOguF?QDEhmZp!<)Q1Nt}ve5}g) zgqJT2si18-#Y?!oi{M|sO^RamT({9B_I65GoOrb9D2q5;Y&vmEsMNN= zR9!28zmTdqkA6nTmpc>eWOOLKwp=c@52)K}QrJ2#H<3S|AxmebsJL(6$IQdRuu*^y zA|jvUdMhpaK}XMteMy0V`jB|Z}iW@%d~Ymq3FZKj^tVLR>$ zg=#GQg+gBszQC@yOsUIoxt&z`(@kS z%(8--8>=OH5YtkoaBhH)Vyfg+ZG6VbiRTXRvK`iSqBxo5R@5ln#1P{q&_cSFpaLnKwN;wz>aQ=o0Bx(4?m?OF}`B7|l7* z7@UH*p;=-VM>zfu=}s5d^Wu00DjPi#y%nvsewlg=H7EJ!3gWRUMfnX?ie40~tjL%wu}hI=*SJGWu* zcz@gLMDJ6>R{ic^pZ%wh^TNxKlqFkuVm$x<2&`!ZfdnLAdDI2tMS#0;q|p%+l{RpJ zz;e3^5$RL52&iMeZ+=Hx4F&GhX=5m0&ces_z^6wM=p;T9z#v%E1sa6*AvQ_t`eB+W0X z@shh`D#myJpDoT59Z_AwSy^~2pj!_q3;p%r7x8dMhFgc{<$~@ibaBm)<7uq6tHwM0ftV%A+G$>=oMh5`wMvJbOE&|3$M_>4A5z< z74g>uh{t<+ZWik3G@^!YA$&>^Tv8$Dy~2Ts%osZ#-mW>!QBch)zeAD+h9@Eq2O_o9 zHui&{h=?W>^jzvB^I|nD2^j^k3SLB>Z%T?~xe|6{^eHk`Y+O$dHAAvxj(icKRDp@p zKGjPuAz*fEJx>l|66bH{?G#GB*$&+s#<#b=oSAoI`mRgzMJ+0jXHeqg>Tfhi3=oCX zFmQXfCQA?y;f>NgVTtc#uhH?DbaWCD}Ev~Bdk5Ecus#J^Xo#))Cxg?ipPuYJoSV$^xI|2L6s z)o!8f_VI-c^Q-T9_HXi-^8R%bSydUAA(>&|$bWng5JZQBv`=mz`t|Q1{;lsnLA+XO z1idex4Li-#;p(h}NH(Yc^oam0TN?;lnp_2QRzqQ-9hnODQGi%jHz10j}&V?j^WZ$tW=cX3>211OC6R+RTC9NA1 zKE+&$m)q~&O|)a&?m3`L@5jX4*J^l)ex5p6_SKM7yGFuv;n@c=ny!=s%QcNNe8zmZ zwoFF00{E*-LBjYXRUOZsjG6hr584O)K1_q5AL14kbV6%2Emg0=Krd zOd_J6bG{Kb_an$qe7M?(dq2|)jcz~uDv>XNZ`~e*1(BNbyShQDZGF51iV}W)s@Ck` z5a|kVy37{^@B+TRXPsJG-a=WX1Eao1Bbc9w`8=Vq`=P=KZgFZGy~@j9B@EW0<2qyE zg+W5Nc4^zVbVrp*vo~tdFWnFOWe8|@?;2aT&apJG1}w#-1(u(V{cg z6YgZYLbtegi2P%YP7DmGFS|K7%@Wu#`C+Qk>~d;qwA>X#7UUBa-Rd=I8YS;*{Eq}v z+CCc|KUz2E+ouH2}T}e82`H4}}f^W;lxL zmQ*)*9H=#El@-?9!Pl;)Zj1BTdc5dydC$|$q089{iO(LI51)5V=s}1aKf7~tB;fQ| z-Pz=Lx!!bl3`@4(t3MSY8@QCJ{3`OuNevVG!LRcK^7fLir^b8}XjGZ9nr#h~sw+nQ zct9bPL@=M77tvYmMtYMlz_F>cJ!Oe}d~7*dx?m30HQvkDNG@!~l3AWA_GPp&8Un&8 z4kMY5#lj*S=EXsCVzp_Oh(rrUkv$Kmt?N%sPche`5PkC(pql9b!GHQ`Q|@>?)?sun zXKqrpBy_&Qr^6i6-=6Px2OP>U)myL5g$8T09<-L7i4ZU?gdhUnI+5VsO;uFg^;R%c zV+fo`1?7@FcbYKK&xXxEzOSr&4GrI@u48Z6UPyK~Obff8xBWuM9vR#0a%{Hw2Z<@M zykWsDCb8GNw>PEA9Dk$Q6hXB)sfyKn^=6?l@q1DqdWawL_4QwlRB|52x!u`OPccvC z7s$y&ivyK&LwCmHQ(u6 zY-Wp2ZH6jgO+g_b%v3BZ!{xrB5b#&p8r$S*aB%;@YTl*w;dhpdv71r}TG*WB6?E8p zoGNFUFJD@1E@t42mL5E8Y+LnrC)@6>4N*l0lCN(Uj6-j@;t?-V4HhKn!kcV-x434H z&mx`Z7At%-@Yxuhb`@my9WuXtBf}w1%IVtU>Pj4}{)71{Yl`WQ*+ri-)VRqi@(Vii zg`_V8oJR_7+1h~*vNeC0(=~q7;zdMffqL;b{oV4 zF_6Jg9Bt@aRe<5>=+)n8h8k>4sVy5wtw5$>M|I82_ktrAMYhA6yhDnoMK9hZt+`T0 zHY8D97q4n&(>;>dPk@@*Ik`OoanqN$IkZgJz_75VI&d+7$@E=-ep7?PejnhMsRv?G zg*>IDV{EaSO$%M!SC`_th4*`9&%fjPbwPiti26g04FLxS0m975y8B%NF;d!Y<{7la!Y*mxG>Y018ZfxK4FxyBXo;E~76Ra`vxv9Z%$b$Lwjd zzcGHK5SdWWL1VjO@dqDpMD8+R?#Gg)#9bg8b*bVdnoE^i;!R2qU*(kMN`l+|bbK1_ z#KcO)z>BvxKA*;6Forl_z{K}(r5n2sc$qT*^(i1aiw6YR&9iHL^bNK@EX33{e=%|9 ziLUWu#D#G2VTv2^9;~fUSeQ5O7`^<@^~j_%M4OGg8x7mh`WE)Z*YAuPQQ+(yJNv6@ z&Ys#ZEPiIZJf;ZU?l>EEcizs=s!}KIBH~0)JYyoJTmfn383Q|`exRH>5H=zm$YG=?28KSoO}lcD}G#ND6w(sNcvpZ{47Yo(Yfo#}@Bk#ChIZDfQUR?53Rq7hPCE#zAH{*)IYwtf) z4jk{yq&3#8xDT;3ziXHkFx&kdZ0?*`l6Zb0ZTBM_Cq@Em&QoQ5b*wBzg!!^?+MwUG z+iY&MkA7E#Pp|Oi_DXCh`A`LynJQp4 z##@P&;X%1OXsD=ibET3mYNmlI$>C=4`6Fswoz7j;$3kB2$G8c;UG`(=^85kH-;+H0@S5FJ~&T}l%2G&sh(6{zh)9h2Onru3U zgB`H$?`rvAZVU;?dCNRbk7nZEh1Sua&Q&HS&RQ_iZyhWzbSs<|6!E!ohAx-S`Kn7- zT*qrV*`4SC!IK-RLqx!(6xgX#aPr9wG`pU*M9psGGY*n}@II5lz;hIZ&swZAmmv^< z!{tB&aBS~LNHTS9@hzL$03kIlhmBq*z_@kEE2@76jNn^9WVG`Fu&}mqft1wE)}*x^2U=8(9_BStk^k2ne~Mz(?FqP?I0&*@_!IeJC)2|80t$R-VPVZ^Y4lXJ ziLWi#AV$2s&kzwZ%0`rq4MFB3M->}E{fd8FF?g`+TPPB?p@9x1Wk%Xa&;AW78On9? z`~&AK0p4Ka82w6|uU^VUwHJ2Dq@c*S9wEnMS}m#`y6u>F3mw1!d=Ac(l8FjhXhCgs z_nb+)EuRtfyl@KcyO4W@IsVFr7^RsPr4vu^qFXciXq^{nyZrZ?{uXaGMxEcU2QQjq z#@C7d!+fsG+4tJugHFcm6f#KQn~KU>KmoQB5P?(G0ABTSqY;u9$ar$PA2WHW%Hr*B z&kU{ci3DWXL(1Z4PIsK~lE>xw0Ppg}&hdXlczyN@C#$m4%(BwrPTnchp3AdQ9}K!X zD+(<&PyCT}mwtJ>MJ4noRVAWF1k{d=)vuPdZvFJ&`EELCHFB~4mB-2}f{E!%&CHR= zq6#xZ*n;I%2uW=+s7-BJEQq#ug$)e$EbsJ))La}6ZB+fy8E0f;#}e`w4rrvSPo|V@ z&6p^;75S?4q7EJ@l;rRz>uYtGj{3Nmo*vEb?SXT9Wqv1du`Mo9AGGdrm8r<@6@fl* zM21tP3}&T4p9; zJj^t@L;d}OC)wBWqN0aA(s!A!7xxs2RLE_lcuDMwx_FwK!tuYH;EwPO@?m5H$Z1LX z&EZF&z&WdOy|e(>9szA_Vq;@t0K*CbQcUG~y-`$SjtUAb^IeHwm*lQ>G=A88-CBrl zcI_C!{G)TYXoc=_d8T3>_IanU6-Zu;Rhuf7z1fxfd%6A`ZQT2N78I0TRk>E~4t;>S zZTYnVM;h2bgKeIG4R9GwA0c?Y`P7%oQ(G`5C(dU;*VdYLJ8Ns(jc)hY*IzNvM}Tg( zB_U5=k#P8txCF}FOT>zpm~^OiXsF?yP>gnAys9Om9n8<#phzT{mt)<(*z6V+`?*+S zs4A7~RD&NXi{u z+rtmLKp6Wi#23L&awQM$V>t6&-96Lysft5WN+8BAYU;8Oh+cXQk3s;nD5s`@uJMr5 z3{uv*)52iRd9FNf#?-AzG>3$fBG6M<@B`0#e|ndTzlUu;y@Z@GM6Q5sYM^FLAsRD~ld7fm`9YC;3hkV`;MA(yIq`)@U=@K%*E26U~N zw!Q?)Wv%6qch%6wW;R)=F)umjqY7krj1op9BwThumc$Qx17PNB=6SgCQDhW%;#W1|C|*ipi?miQQ9zU#!-nT zq+)K&q-n@PvO`DNZL~Z75B&GAi3Y-ub!ZvcJa7Td%@)C=bdhR>gE@YU1wr3pb8XDv zP1F|=5l>Egmv4kTyqm>U4mN){SYM#>3d)SqlaUPo^jPmxU&P7^l^IWc;uH%ziu;r| zyaq9&-?vvO6}H&z0cB%eN55T|8i%_MI+@Lft4O~&KB^(<{r@W@&!J!xE=9{UsO*Y; z>Qgjf5z!gU1)5Vx_<=g$u;b*^wUQs*>$#BNX%(qoaqavy*w`naeMI;96S5r>&Xy_j zC`cMSgh;;#k(NL~H7SOo(IyB>UvU7fOGd^kMlO6_I^M9bJ<{h=)&rH#8O6?IvajI1SECh)qk*bD+o6rFZt0E`e3-lk%C~5wl?wJ9g0z|4k zCjdVHZ%KJSX7u|Frd^*cfR=&w?5nJ@#mRpfv>Tig#J+qSa%uzoA%1?ixOe+j!wXtk z3h{KI=5ukZ-rjj`cPol0Srjh`2oIzxXeo`cB)KcR65Ik6>^CY!_w2I-TdVpFx~pd%P$ZR1N**|Kd;C+k&jpf zy_whF*B6$IJc69nft65_L1C9OoG?|~%8JP0kkDtLg(#l7c9p@~TYBrf0!vIc#-CVc zWLLMjWnBEYW-wc#HJ)y%I4cXUg=oZ^HG;RJXGQmV{WTW!jlf5Uy9<0LXC2X}Zpu>y zkC`$033n0o6?5@yJzn!Eg1laOWv^51EJk}AfPWRDU}`p8lR<&$cI&Cf z%Mv?{O!)@cdr9arvs<_9dki~N1l$jIY=Z$WM0#ITKHr`mfAc6badk1LA&}s(uKRLBiy)xZ0sBrOo8qN$XQvr0;@6lV?(fnvP#+T% zRPmB18qX0Ch($^^E=0ZIL8ZrN@xDvb&u^es>FSpbare58h!F+2)pe|l=vfy7cD*|rWlGcZDON%kbP$hU)RRvjXKmWxV#M@0sut`K+OmselPT~Y z9a!h>Fz${7j73(1$UkNo-{rm>8E4+Se8V5;Py9dY?7sQ2YIc5T{DEEus zEis2S$Hx`A14BJpx}PAwLqdi!CF5GMjx`ewokta;PdaKy-rmlR=_^d&5>zgF_}vD_ zoz9z!IiDutXgI?Hto`p5zAQC6V(9R1_qVaW>Q=_7fA(##qLOcEm6iK@dzFK_bxGOl z96xK3u-Cb!?xMHHi;7lKx1(KOoAwSpVtlqvSHOoy=E09PrpU+;072Z>j~{UyT=9B) z*Mk0M=ZINCw?u)ajQ8`e6i1^}-L*U?+5pSOIM0Rim*xTw%B3a?oOKr}6m9>%N{_f%phO z77JSGWg{XoGsPIdZD>P6@>*1*=vV8^amsH7T!26jDoT;51;xPS{aW%ddj>cfD=0GUZsH!cO^eb zGtGU4jww^Xo4|vg`xOKh{`&oIIUCxN-aXqj37`MnCzi7*_T7JNmPc0Po2?lIFJY$g zDadAB%?a!n(&fIRv$^CbsND3Q&)8z~5s2HQ>KpaAdJ^6O1`#$a1X8GTW~FN}1+NNbF`(md1`ZEr==U3@gel;=2zA#e zVe2v?modO=*XC(I*B2EZNw^79s9VTxw}pddquRX{jwV_gqCQ+Z3!T@aR~OzCGtbqu zO(+UDIBc8V2UG7Xfd)HRvK4}Vx?J$m)b(30EgOJHq;j2A#(zztXnb)1;|)d#?jGAd z6^di=PF@!I@S)EBV+d>}oY?!sROBhZx^5c#vM$;fZHWyx-~8ya{t>#Yd#U z##W`#i-{SYGSUMVknN|jK+%i4zM76zErEDsyM%e~Ka%#P)@hv~+E+)Opb0`rIb20P z??`mE1H0DMR#>8`b2AnDdm5UdbitRk!qDgE%$l{$-BlU5cJx-9RR}M4Pv;;;4H9n*H)Y5%&#KaaRDv%Yii1%X#RapmH}3y8`D-Tw+07PiNp4jX3`st~mLW z9KAIVe0B{&Jm3}w zFQB}{2Z-UJ%F2Ya?`>2?u^&p}@aKMNg9UXL;K$2B4$$NdrIPx5`(@vB!B7oZQ!64O z0E`ehE>a?Yg+MxVgf52)bgEp=0C&*J;Y~H>B zvZeivyYdh31wS9B1;U?&v5N(fW&!(FK(KYEH&`W-GH|?x_8RW-MseE=W1aVL-yFTH zC7E>xO~riPyYK`P6In!hUa7?NDLhk{Gz1EK7&the1%Jmri@x~F-~pi6@#dTV$q zn%52xnys_Q$Z8*`)oternN&R+Hg?_ryr)l@LkiPSZ@4s9`C;wYW*)Eand!r)AdGr$ z!l?ZVsHMNGrBG5_^N!`{Y zjF#$_S`;x+udjFep)mRZv@( zj;>q3gF_heOFcm&@TJG`Zb}q#j3}d=GZf|Pw zZ_a&|wUnw}P{lJ}o_6E1cwlD`zw))5hm@8+w{#)^ee6$dX~1|%QqoBX6q$?b=^1PB z=k4-~_N8KZ!+WBNeB38S^(@`DGqtII*)yc6X}leJUbg_7F9(Sec2DI;;*W;;*VFwg z3*GC!yfmFu7N7Fgi(^GHn)-ZX>&t=VF37bJW65R-{|-o0UUTY zygv2BqqcFjhC44>ofY5Pghg3`T-~1Y0iR%Sy2jiX14rpFsY!du{kG@sZS*H&pW&pR z=bV#et?k#F9RWmfDCG(lt*D3VNslc-#3aE-$aBPul^UCc^Z?IDGY>px`P^mJL=6ee zp4;99=4^FxVCM-T1A}DNm_0zcDl`}u-XSGn9GI#w{Z7FlJlgaUtlweDsWco46BG(> zvycRc0a`ylE(lZ=bN2URk-%^8^Z((`;<)F{>22Mlp!mgONq3>B`77baV1&BXdg~gl zM>h@Kp#v=5`8MrBbli(6HE1q}U#4Yksg4TBTnNMxn$b_wS>oaUfm8a|006VgnUO-V z(#n4g0K`)-b%u*MNS!IZa`)I0*qT9)VJ86Ao`4*N+yNU)dhZxw_;VhqL>YOf#b%I1 zDh?)|sDDS>T0sH#Ts_mi{iB;e-ARH%iDwz}$pq#F-=5V6<42E&scLF-pKk#4a@y18 z!#!C+x&kmJFDX)RtfrZa(WU8|<@Q^mINkP;8%-fZhZgnj)Hf4}Xxrhn7Rb@G0?7fKBUlE?OM^&LKMvl9I736ska_0v zjc~ZsNvopNMvKp^#=z*0a_a%9(v}rme@8qIA@{+)SgX@PH0&2?9xv~2*|Zw{j3ATD zT7e6%{BIZj&&0tI;L7)J1Cty7T!2A&DeRbOX~DxoS6w`_6Z`19sQxwL84E+xPoIG2 z0j}iQqP8q~!(iPQ8u}Zx`<7_#(%f60ag*^pjw>Zz{I1Xh9E02(mU>)1 znS(DY)1$gbq6EIIKRv2MVz`{p8XNDqUMvdnm(IjPOIW0u7pP`J<;JYyHpIi92J2{b=;_; z%yx<0$Avk*vE@yx`eID1eKN)q&e@vg@Zn)%m=`XNaZy65i7cnoezYzH=^x7Y-Vi#%)`odREtLX^B}U%MJ{e-8v6t9*n?7kiz+i~t57-+%Q& zA$X{lERkWdU9};kZ1EK~yz4nW4M3|gf%f%v+P^!OZt)ekJRr)L>PEwM@Lr-9lP+zl zNl3uW&KWdQV0riMUI>69zxmuc@w|l$yZxc-tcM-H=mvcQC{6KVM8tFcE^qQMLuUx{=tuz#=?toLND);Q1)47tSjM%rf#}Es2&Z3@&lCePn z90c88-oFHfc~wr$kIq4(Cc1sl@9d`LwRT6+BwN?Lv}{^pI&YG!Ir$4RFmUo(=0IX) znq0q1#MkMP3mp~~L@KYpf|v^er0D&&j$pc^nXUEq}_G7ZlXn3=&H{&>ybeH>lqdgT6-^=Ijr zv&S`TX)EDRm8&aV_RTl+uSuTMPrTUY^wVCmTpE6bjRqSrg8Kv>2*>aO8n^rj6OYGx zM@=@dCzm|vHJP6SK;j6vXzk%4j#hV9r#WW!QDIl-ZAfD@LCo6u*WILiG0bHUWCrY41LYe9fe@*;dD)Fej3(M+V90iRuQsJYR#nAx zx!b;$oJxN9Gi>;#bl3i`D&D)3{yD|>6!4N^VGS!bHk)7G4M`}(R>)&2&EFS^{4Jl! zR@C^eP!_l7&UWZ7>K`+H8tpNBe<>%s+;d{&B5guy2}{(^b<{yPfnZ0X(7?hv2Y$VjPYgr*yy;vs{DsiSB0noqEW=%EKJ zF;1^Ln8n9-&>*-CM>0R}EHu_W$*u!-=7Y>zn+PC36sb9bJ9CrOO(Ys7CZ8pVS1^FA zUmOl|l{)BXuFw~Ssa~~)2kZO#L4I_x89-vQX;rgVJ5Lu4mnz&{+{}J1I&mq_gnLrk ztq99#t*}@@Xpqy2E0Z25EZ-6tN%Iu^=rcKqOlqi|3UOKM5N38Q$QZsp6mxKGK`~7l zEh-b$?G{zkqy&8a(nF#69t86aw#fAj zUCD^F>eD4^AW)V;5p_IgVzB=W&pR99vW|{tmy|4Hnl5dBiU4ISc*`PPa1tgu|SzBd9@KSttK6ioNB(S#Ke>f z)CJ^}Kp4#zkQsF+&!?%-IS8Evsbq!l*bY{2C#V%AvozE)TwVOAtR5Qsnl1^a z?x7$Mu|)T;cIox)_uiiNlAq02OAI>O=kvPgm8@Et#T0!a5thjMZD`)7ellETC^n_G z*3c~O)&yw3Zr7me*gez0f#iEWkbsbjsLEzt6v!HBoB*}GTw2u{C4$#i-F0H_Iw%mE z=@wVKs)6Fjd}jHK>U~=ggq{@ZW0*yx8G)yl4Hc(}sV}KsX9r)(on6(3ymnl$So3_0 zRw!+{Y8lK*GCdkEW2tm4|F(EnRw=wBnFT|$elc6&R8fh?Hr}@6IX);LAb)#~dX`4` zC~qW{|AH0YNyA4F1+4Ycw{0D{d15|rVgdo8phR(hl_ggc=b8mw$Nj*&2@&ZDnEsCF z08tlju12#<5|`t_J0jbS0ckn87}Y;BphwT~CR{B7$aWZko^kz!YKvR_8f^m1|>j7#zvDS5vY6sh)R^{T&a5FUn}-MyEUl{y8d0^1b|V;#iarKKOhtr zYwBudR{R2$&`qO;*Zt-L3Tl`xz!Lz8HACGeR&LA#cAyd_Pu3+Zw-`Z95I{Sf>yL=P zV;3;Cv$2(%rt#QI-u=B}YAGUz=e z?7mEa49bG7B%qF?=M6W|Pc}AJ`DxKSnd_x!*mvFI)XN+)094)FOy3wj7R#htXXicf zg^KUZ$O?QLwY&8dN@7@dJU1TC`?WURq+F;-uW;DJqy)qYEt}u@`EL+*&$wMbsl;`> z?gBF1VI&OUnI|5AReS|;ttZj)>W1CsLwGxZFnHZ3Wow}5-n(L@9*qm&S9>XQ3qDiAbx9C61$M<37pj0hA-s5v=j1rQ)1K1%-`urtwZ4deWI z7Kuk=mVUC(uS$SGA;XG1n@!puk$;|6f`zs-qiUYApO=_~S7CnGm4kE4n9Pd}N(zD>v$BZi2nen$Z-hKA;d11}0JIlgwCE+lb zxZ42W@tAsFDo;jZ78#k%YMu>PJ-Z2`M@Q+JJuh7ruz2)ax|ZJJMw9ou9n^zIs*5%j zEo&k%EQ%BTI+u|kz0z=<^Db#p-^ClBOd>7rpjC5cOt@hk8?YLYTP2OqM)C_WbZz6DrY;yDoTE4F2 z+e8IKTJ7m}>dSB>g>m(R`UQvD8D zv*Czn?$SSgB4B@OJNywh%`*dc3P*)8zZ&~OJ-zUiJfhoHDpwBhDb=PqQyS4ip^-=( z$caz(45F9}@+t%Y+BmN!zza0-Vlvvxa-cnbT-j?odu=AtPw1f5@lH1iCU zdEuo5kNlSGC;VHBac@7k;&5M*-$c7YHyd{Qo|dhJyejgLoyF>(NrYpmMz4-X&$dU? zJT(?gg?_%tILgx;ww?L{WZT!KMNl2ZW;=gbZ$3ULw>_E};^XMgF>)$%qm zG127G-zY1Ws!q(?gxD?E%kY1*JD-5%B=h25jj%<5fCVDw5QjPmxyC9$UCvQdL_`XZ z^J5Ybl>wRGB5-uQNV*1CW7(OdZ?mul?gVv z4_oOMJQ@KVqD%lv{3YHPv;CvtWx+%C6!R*ZKv$FVXsRQ&h19Z*`65k~CZB$pW}bCw z%1yH+T(j-2YcQ4dwYVrS3JeTU3qM?MTij(L2%oh{luwU3qQW%Ifq?I?rRNZU&*JGc zl>5HY%3myY34Vi9AP&t+d7Y~F^kDj%e^Xx;U}olLELH~#hI`yUU7L}toV~q%*D!4y zmz>=qmHQruK@++}pBG;cu`x{~mZQGPy4;#`WF?zkwg^@e8Wf&oMW3&Gm7M_L)_OcL zf28kGQGd)-EBH-D1%Z56a9C_t7*NA|I+8Cj_-vk^CKNo)!JIu72fIuVG)63Ugb>2A zP0F+rKO8T6dSQfoEbVVDpml__9jlnVi&|bN@c4GX#;I z!iqsST%_4mtRrA|K}f>BiI0DbP&tEK_lKMp?c1e*s=1t*D)lh*xmvLyK6?ATt%ixN zLU|x(d_eU4KyTR&@5o>wr2b}SXDb2vzlaQrE7T5y`~y{cEEaCjARmls%M0Ej(l+61 z*#(iLlC?&-of22eqlUp)uIS?`#=@kS%F##id%MR&6U7pB?>~mMVwO$B)fTio7YRVN?QG7n z)RP{<&4+Ig5DUWr&cCK-wfG1eM!mp)T?2nl1~dPiyS#9!MRn=q0E@}R+DjDz1l}Ia zovt8cayzS)Px7u!NoH+}kGl7#Lt?nNqhperhxWwY#5d)S=z~p%cg zgveF(sfn;c9JBVffs6fW*0X%P#nc?uth}9QkFiCGiJ-C6gpk0P5O^$XK%5? z(0hnH3-G^0M`6ma2!8h=DG9exrc#I@beD5Emj~22cg?Ic6ko*-)N&0E%=0Y^#r{~J zP|}rJeQUQ|FC^e{5ZRyYy`HafsgW(WXv_=c#kuz(Y`7qvLArDcy|~#O^?anBD5Wh? zdJ5j(y{lgzhT#6KxdC{|1O9DUAiRVDl2G;En@kQ^>Rk=YQ=IGAE0qV;~yS)Tv zqewty*^-Z+?$RN#^QTdBETEZJu0SVe-XV$aF~;R0hJ+;huqArWValN~Rp^-+|0w%o z(KEB+uX>~osS@LAJs6x>PLraqc?Er!@->{;S_Nyn;ne!l!8?BnsZ}En@Beg|*M6DO zqz}U-Em0+h-8jVv3VuhmGp4)qB)daIpp8E2XBMjZz+AxXyX=xz*IxczTAtA1^ zm8%St|Fw_5J|In`sL0B9DU8(I)*4@)Ehq?oi>KbPCpDId3Z9_IEjnoG+o4eW{oyHB zWHpakbXw~Rm(H?7rp0{-b3u72uOk=-7@VVk`gQZC6`~Xg<37y9DUG7n;rojB8RW5x zhphJwicNF)WGNEc;au6R4^axmkx8tBu6&K^c7*>Y&|^tc177uXqg3_f z*Axl@UN`-2BdHc|-Zu}i4JhT?Lq}tbmRn@zi+hXd6tfq5-z}P?H?ksj_QPK#DbWf< zp#CUvbZN>$Cw@2Y(LkPNw_x&~RiRLGX+Ho?%R7^M6GDppiHK4iX^dICNC4tU0s#TR zWF*sXb!`ocfWX`yC?qa-e;peSCwDNKvTc-G>%=SMtSOxM4iTZUUQ&Ch5s8AYXGHItNg((hbxox+ka^@;0 zcH`{!EtG{3EmYHAtm9+$ZR_Yhk*&YhjOhxejRAH8;uoq_^9w8br3XD&@GmdT4@d$N zX$7Swk_uxN)^q2ZSA3`Q^rVymy{Az;DJT?b-RR-l!`%UDMIu*DKE7n?_OVaI+@@vq z7R}Zd& zfJ={j7DNf5&mQ8Xp0042JS;FgOeuJx`lMOk&~mj@u`DV-o=WI}W(h7^Z0 zQfo~#nYl~|Cs-t3u=24xN_YL%&PKpZ!SQC){nA)DK(uhwF-^yUuz)DwE?-68WPNrb zwYq~j$MxX*WbyYnn(tH!c5@k!PTw=x!Evv9KJHJl3QoWCrL&5G{A9Njn-yhs?xf7$ zBa!{ITFtiDZ}P=>t%bXX!72+Pw=CiT@dR=1-_(UZj;IzbeaPM(%PCO>@ig_-u}}?O zfp#rV-qVT+o>**m;pcP&AbMzB{U~dRN@AAYh$v!qQgTK%@)RVRuMix2WKjp6c60yW z&n$y^60_wGpsWt|dJYBgS3hPo2b@>C@KXrM59G1oTJA4YkqJ2E>~|;Sot)}SCW{MJ zIs&LUIA%a}P6}ktYdE1Ghy)gX<>-;=Z~d)_E@aQlL}_yUsCqC%GR9mI4vUCY-SGWP zE~% zcUQsVQUD6CUt&XwZX%h@jb&9$50#57&KuN^^bZONG~HG>??IryU)#7I3DG1cooow) zwV!i$Zv{9AsjIFCqB2t9@O;HGjY9Rtni|WcQjlgE0Zn$5Mu!qAIf@3N1}F%xOiEHp z(kWWjZ&WbG|EImT42!aR+r`IL5K#en1PKWV1ys64L`rh#R8m^HLlk*HK~kiZ9tMW) zR739Gz`(dg2?AtvjIV=(e}+OS4oa-^Q|_64TG8-^;PY z#F`>Y4wApA7a0k>p19-yhPJW}Rs><#V{^qJ^|rYvl(IUYWGUVH(4kv?Jimf7*jqE2ti4 z6;z`x7%|TTFhr-zbtmfUzsP>9uW|p|8)UF4o@4IjB#pG|R99lf4CXb=KApzcPWV!_ z-8|d!Q$YNgJPrEQc6GJ4O}wvlnZ(XKCyUUa@cS|02k00<_3|Z=)F_T@YI^#@wDAj` z!xMx8pfEjrHJy7Qi`@m zEnoSIQi(c_e>y`Y>ZLQ)6j>lZfWHUW#P3x0Bs+uM!%0_=w#p2~X1XnPp;zLJ7|+Q^ zM9U>V7iD3EC~gq;k4YBL&w2h;#vzYj<99%{9{6u`RqJ`vg#`9vLVHd zVk_l=V%#oW92<)?sA&WS*tP|XT^7aDa$B)ZN9jLa`S)abdCQVp=_-0>wl#-Tm5x>4 zpH*t%A5lHfc?Mw+^<-kyfuaiJj6#2N###kvbjd|WN5dJ*Io(R5OqXtGJpAM9u9LY+ zrp(r;>+g;XS%L{nzJs7o3pKHjX*{ygYW&{bBj)-Dd}3XA+E zQV;*Vw5ge8*vq9}GC*WZBO8vZknb<=E~*mP4?eGN$pNGpU!K+A2ODXX!uh9?K)w1ivsxW9=KDfmtL9mMmrZzF zG?z>*@BG_R@iDoNsyP3H>@AajI+{5ua6xTv$Eqh@obtEOJTUJ6mXzkUc56z=?Nd?G zC+~eYT0gnUQ1WqClVq333dQE&TwN!7kK!n{dG zqcV?_PI1|d}An@f&G;9}SFH3y% z56I<)!|_?+7VZWHNw|tdfxZQ&`ucj%w!`~2Nd0F${`06QNT4qlMx?UzKN&a~Iiq!- zh@2~e?YW4pOHT3#O?SVOMfY;@3)Yc3Z|fpkoKkX%^RO-3hT?nQ-C+0SPhz^9a}*K> zm9m7F1dN+olU-S9@389W5D+`>xi0o@?}<#$jXvBYn$+PhA&eH*&L7~Y8ank=TTmh1 zl3Uev?K$VRX)9pgJs7)G?sp2gBCO7L+MM0=h>KkndFl#^_Ht@c-mvMG^ltr#{pjhA zl*Iva+&&e*&RiWeobAS@m)TG2t=uwgy&@qoRZTp%I8@PHW^WqFuB!!mFDo8D`_u=! zp;`UwLg&7v$x*Ut<}>tqHAito)s68jt%GCk%F-;bvyzpP3aJ}Yl#!9aEs!wDcSW$E z?W_ihETRAzy+}f0ZhKA^sGMq= zR}~$DwUeHk#l;j=?zNln-j}p9|%r|B)EL6n)s*RCgak9d!AD-Gd7V`wqU73 zk6z~FW5tGRJ7balMb@(#R`EhwHRsCZ4hV>pJmwz9SC^MPaOcKo+-lLN(KD>UD{GZ0 zYn3X-+!CL8EqtwEc}%N2Cvk1bTb4>dDqD-CSnL=PPD_@m2W87Qa(*RQ_!X}{pz*)M@gE-Z)F9_ z#Wqp-`3%yl!Jo|@#GA*!PO0cf^02T%the{jPoeDT-pT5Nb>k|UG!|pi>l^XTTwASz zhz@D8WU9D-%CS>j4P)(LB_nT~79-%Nimkg>Gz%s{)(R2YY@q+<7(!f)=jIo!G&fxT`GYu zO4XGYmnswlb1*qF;=w=t&0j3`?)BHWc;il-sDfm+FEt;F^kwIb&X)L%IYC2z^YjxlL9cIfr^FT(toi9* zx9UNbhWpw`qht*&mbo1rRUZYsW<-*?o=$ato6k!XD>jnV;~ z9ZTwNktO4=H%Zi--keR9#^~^HsJ^FNdCWu!j=pqPiU4B7%ZfA3W>0H%DSfPop+NZDIQc9&W zZKsc0Q3{K(aO$a;tC&%Vdg6Uw8>YXGFJaquS#ceZH%j0;!1GAI_ZDR!BfRmZqK~dL zCGeCwetI|T$RpN;vr04bw2@5&%JwGtIn$wze@HmbeNc) z)v8GSSdCX>Dj&?4O6Se=%+^zW{9JzEEdQ~iM*U~`tzN_(c@g({L_cx$3s1^Cs?qG9 zrhWGs%h(9x@VFNdR^b+jH3wgBKj>Y3F|)J3NxoxbJ~+qe|LXe}XZ7ujqks zzm7h%PTGoZ)M4JmyLZ{_^v3itDm#0Yi{-3cPTt+YC!Q_A`|(ZKSu_xGNYQwVYHj(R zJX2eLu^w9SphC1&-(~QZ&t~*E zYvzo%yu|*>|4eX0Y=ke%d9s3DtJgN3g3T($?!W{fA?Of0&d_;oy?2<)w&okHblt(a z$`NUBiWO9}ys3)Nm6iAGvblYnzM}tlz@mSEG`W_6Fp-Zb6q}uL??8bel;%b zmA#(n>_?h06+ya|eGRwrs`pIlZ=-aml5_jc#vXlzI?;cW>>;6|&xI$$iR5nV?Uyo| z{PWi)@eIPlqRP$j+N6|wl{@m<>@5jweGAkyZ5^q&0ZXCn6lmc$C=f$D5p6))CQu3C z@4zue(5=P^CBj;cfmA(V+8(b!4vIMv7}50E%~&sWm%1n1+m!)F=^sb8G^4$md1zR$jho+lYKg4gL%4gH14Vg zRIWkSuC;vQ?DjYN@=A5bw&P}e))m*jmNO^J%=z47TXMMBVX$suraHT zIiq#!iQlz)xsF=RgYN80T%@HEI)kE+w=VU~D;IE&!eonj=Cd}`yIMZ*KL-ewN>w?R zw+PWixKWfM>2fj^s+JzS>>qse=|`27*)a1RE3~m?TYGu%oP)qrVRz{0$!axT1c?a{ zR7p}j0}0FLSw20CwJy@T=rc|A+$C_zdfIYQ?@8+E!%nMxTQ-l-V%!Vgf81Z0ILBrm zvAFg#XC@pJO0gtAw3SQV>JVM*h&Fix?=xexUum~FzwZx?W?7$QOieAbE>Ws1)m9*^ z_22G?b+Y~+Rr5=Z>c@Yw+>dHWu4lM+anbS%{8Q!;e0P$c=d2c$+fL5W%j{m2+en|! zqO-j~a;?<5=+}2cHKt$k3D(TZ&zm*Z>*Co}1gSj~clO<(`tz{5bq?j%+zZ)t)?oqs zUp6^ zP?x^bjCCjdaSQ|dnCL<1VwJj9l~(zVIkwX!%GF|; ziR_Y;ilsNh9_aEp_Lsg#mbI$6K&gx>gOZsU*Na-Lmw~x@J?f7_ZA%RKq(A39tzJ-S z(j?Q|(Z6BgXx_$xr5_{MRvzNZ$N$^mC4*j!cD`dCC_|U8tIh1prJ8kS$scCO%x4h|Q-ESp z+GF0EU+R5eCu7l{t@JtgEYju|z}G*b+MRkk9}aUAN(^Fz=lr>><03wYDv(Be4djjpuo?>n6n$?V&N&<=VKirC3nLO5dh-G7dl#&E6LEo`?lF)Ht+0@_Ba{ZQSqDs6f1Mbd3f5$Om zDUM3og+4<^3Q{u2NxCN+JHi+{+?S+-C^X|w`;@HuK~G}o?*FDkhjQtf*-I}f?$k0) zGs^7aZ6O`LIXEk|Y@24r`d-!7@;wjtON;yApU7n79Jyr&9m3aI1=Z5Ul$N0%ouk)L z&?fU**u;?n25i(rC+EoE%gDptFfH*1^%K}W=N8f{m$Ql%dS|!FCjg6nEx0ywm1=hZ znFTH>(mVH_&lzk~EXjx-SUOT{l|$DSB+o@W!7~*J9O{DAlKd6~^&vdXPLpLa!h@D7 zI-k8zss^~HCbW5)xa1gxBUAo;dpdGa5wR1SFXu64XtTA)jWdM;L>}Ewk13Z}D_6O7 z%=cZal11iql;#i&y9Wi+pJby_LNS__d#@aG*TUc2s-Z|`m$7A5suX2|qx646-SO-B zM5YL)yjv;oxep3y!RdR24ttNxaxF`wE6A1%v&IWB%EiZI>t}L>i%%W4u6laMHWHEM(qocp}*)eoC zR5FEx35;9cAg1HXk(|1jzg&P&Xl}~9B$w?>b2VK+jaF4U!m|3^Ir)vaM{|Ov^I`w4 zcHsD%vnt^nu^BF_<66$00#v%LU9(br{v|y@5sWMX)f=TS7l4%6>A6YENQb69$P+l@ z(P)t)ZBxTmwf?3nEoign9BTMw`4QTh|9+@@-I=P{zoEp@prbEOWj311sjS3=?d<8- z%zfdN1iLM8F&w%X@0~;U4HF@w+s1lvQLU;RfMA?SK)hf!Jk@dwZ^zGXxEkJLr^<_6 z4dFqC-~uafrNiDgGFM;xeU?$&`zd)c^${IA+C6j2QU1Vg;CPeIC&A<;`hGS%Q=2OF zi`$wKMuo);?}81l9oXAW@4`kw@t-Af*woyA1$}g4kAuC<*`Dm!OttLH_I5=H2?>mB z6#EX46taUMrjCXmy?dTbzWhQV9)hKa?l6DDVUa>&mDn}DIdYKVDFtlYNQi{ zDKuWW0z{RnS}~JPq^6o>X*_68oYaa(BRBlk3ao;0kFGXZl`&T zg^?Qlh5n*WIQl0$_>|}^KmG2bAqrhYE`ooEN%_1{-7gX-9iIFW!_sdH+h8eAkyCXA zh{}HvX|#5XptGq(VI&A=o}OYz?*!}P0cqle5?S{RDcPuUHERCYl5UtN5Tg4tQ9ol( z;cMY!8p!b4bq4Og`sY0bxA|jG&6n8^pFz9{!xom``E> z0_WZDtWWp$s+Q;*J2Gg@*ZLmo|L&ZeDX&JA7KK2Uqu`rUG~im` z2T--zPb!FNU#|gmNZ)Ham4StiPs4e{vm^mn-$;q>`@AnNm~A@EQ|P*E0^q4? zB{r!$@8Q#9gv89A=S^X*9*DC24qR_f(o2+Gq{Qr$GH@c)y&9{t@-@FxTpWg zq3ehHcyHxsayuUpWV80c-EZ z_wV0@DDT4Js^b&l3QkXO%anRGVTqs%2>&e4pdMuC0-J#9w!(W3QtNexhy>hv;TxgS zdlhxZSkvG>XZi*$9UiW-F=AZQQrxmMLVUd;H1w8iwze#>5UsK4?jrI# zV~w_4=9BGkIu!)Aj&61RVZPRV&3bKwvPTT~RNyQh9XVDztZlmnpS)^t2&Z6xOdXrr7vQcGd@`PDZ#(J6CpcN42qW;ZRI z7!vaC5}OvYh=|^|Z{IF490w@T3Q`0>q@6^n&>QIs`J#_X1TGFTxa#H88PJiM6AKqC z!~=XSr2_n4mN~^Ybf@lXy**!pr-OP1T6rDIV`}M7nLPH(9Gcm1+Bl)jxNbAV$T!E_ zh(`4gy4F*$orIEwpWl+T3Ll4p=Z-Ob1{X#VwB9_cny8}-SCw(QEiHI${=Da`g!sJ& z&v}UZvR8)&eJS}jC#C*)kMyO|a2cl30Tf9B$RdB2cR!E7rI+f}M!Y}u^%aOMuT#&_CY z0+QTTq%8NGEPuS;Aq%P0BYwMkvpsS^h;ZA~ZTy^b9LbAO?7lRVM?`NzaZUxFccv@! zR&5SrONQNK*|Z83b#&s~)_y*66xs@~Uur&yin=>=M+xnyquL#(hOS7T?@1)yYYe>T zSKH}G-$k7X9>~0tDz^3WmydaboGv(2hD{?fBzVr^0m_7pNTSDS)A#2vfi1=NB`aslGOl50A>EVTS?@l!`Qy2<+?p0usM%=mqAOljsG`OSX0h@qvWOAJk`F3V*~I5gLK~6!sfa5UefL05!%!*Dod%jFpOsA{}3&A&~+#Ze`lU z7h9Dy-Vuvsk`y93P0;ya zFaA|@&j1^jO@2;(`|QjMN>JNOIoMaHxJ9?QDlM(76UX)B9pkSMg$id`VRxBqt+E(> z_T@HG=B8PVHbAGd@Wa2Bodr z{4I=)KE}bUcO!+NmyOb-t@t(-ySiI>S5*b+MLHn7pO`*KHlw_=>)1}bGLd^cq{(slc z%l`uD`7ie;F=9zc&v7^_nXRtm=O%j%7E*E}LR)T`I8W_U`eYYKLZ{i7*|~UtTPd); zCQ5YPH0Nxn>&?z0v=6Bkr^*=7M>77K%3yR{JdRm78iy)4yG@8+Xe&f58)&x?Y4VX9 zi;+Cm5uj>cy|2Zp&c2!KV8-H8BlG4huR>q|e~sFP z;VdDG^J&W`$8g$4T&K|*3%TAD(mr@TSLN~r9<#pN80c~=PzGPI%~pQG17My^+<;K% zDzo+m-YQi_nuqwHs8Uh<>7*75J-a+$k_FKOtT|*NBKzMRV~HwscNPWJ_Q5cGy#Gcc z#7y+)>>j4Px+5NY(y~J#(mQN51^t2S*?L3GLC9HYp2pnEI8h?{-p$*sZmlMh+k3{s z&KN`T(aN!wx*}YifM3rU)qVvtX!#dy>=cDDXup+vw5}6$-uOVb$MNN>lVhxE$!{&6 za*5Y^hK1_}UAV3$6@IOZ{gXOw9-t-%DH-X;UE7}_7E=LnORuSg55DsZME?k=jA33e z&;R$|C)`Fln73~$Z7$n6B|pu{8_+GYU!f6Y2P{64gTr%`=r{c4hF4)L94_d(?sWAsqAOhGr5p|Fr+EQDfkOjDCD(UYryic8wA9`6ai7OE>@ahAC-Q-cx8TPbzZWyxLwh#jhcl=&Ah$V zb6SM5=zj*Km__`rOLvq$3gbrQt@;bUDTwYFIi5v{JTE`u9npDD(b-AYuV1`vcR0$` zz_N=-=PUHKwb7H^r$Zwc>g%PCopKZCPBk_vrC?@vydDxk3OyOvxWf!^pk-FjJFCvGX|4DzR= z!oTq!__mTn-uqg0o1nu$woC+AU+KG7ug*$$fT;ZqdEcbbUVU%XyLWxNZ=tr%&z?lm zms%Qb>l;)e^KLpnUH3J0uKdS5ortuLzcyV?+!ZfV_#I#2Z++5@jf|LCSfqJ}9KxF-ubVnv zsh$246*->LaueT@4KMG2Sf_&vv&4hiGtH<2wS zo%rolj@@;T!3G{rh06Q8C&)PTbbuw?6~k>oaO)NdOO|5&HI`R_Cw8Gy^3ENfb65Bj zzce1v7y|)}P9TMKWhl`jUja}D4P1yqU|==73ELCZijt)`-5f#TJ3j|ec7xJLca|o4 zY;25Gzq-=EzyJi{6ZLxLfZnp7PV4w=`y@((1IeFJBavZYN$H7RTF4JVNmU5tPqSCH zSKZRI<&Sh9ay(rUa1imi|m4D_@ezePTN zYk}Iw(iXuG`dopkNc7c!)uzT&KU=BI@Iudj0LXIF%%sJ zm0#)$B1&^OyO`XQ4JO(fCOR%?`{d{dkep%fmjftjzFcg8Y&*C)o6^(}#;4~U1dfLR z+!j5PeJn%3M@~&ow*c7Iyf!eBa;ORmJI) zy|l^5mxCLsX_3Z-W7r5GNo7?8=ZA9_+pMD9ssf3CJhS9&Pqtqsbj z`zS{hYX3jPzg9F!u~ii*m`oX zTXTSh;f;in6}GWyQeE&lRk^!Z(O+Qt8n!A$!(+jn^s$#5TK0fAv2Y6ShzB~%sGtuz zm?h?5RR4V&50M8?K74TSHGa>Z$CXrRUx3&)+k;cJBtR?SHt%9CvKml3cJgfL&uTEz$u9~E@Y7o*w+ufcjKTg2=bTII zIvF4b1<1~9QqH4R`0zT062_|5ZP5A{419ZgyO8%oKG&5Ln4-szGfrSoTQNXM$b{9p zvon{I9eng%2!b)f(-g==v1cFbEqUKQJCU@@*^8o`s&Sh1gZUK&woo#34QGP|7u%uz z>p1`Y7VI#5S^a|l)aVE<#Txl5U`v6kL&@HgkT4sm_MEnA>r=}!kWi33I+~oRf=R0L zCx-ZY=cI!9`1>I`(r6u}f3^^tjw>hdvz*F(3@ydV>=2RJtJrTL z!tzJM6afxK8vvCwCpb04p4a~*!6=Vji=3)7IRDzbEkW3AB8a!Ez*K?TW>_c80dQ*U zy?~1dg!ZE?F()O3K{Z=THEix#Gx&CS1i%Do@hrQkrkV9#{a(m~ionN!(G4^`I>LNU zU5gi)V0hL!I7D9U-o58QZ082nlxn7mG#pxgy~gSYsTf?i2jCrGOT7ovjo-vCl#H-T zBX+4A(*MEZ*s=l~D6OKU6$6_g(_sy<@bhafRc$69Fe*H~Cb7`64+Et4t;|wGhbx4z zPymXk61jK*)e5JDFb3ET2r#e3ukre@6v3rRG$ZuZ9{rQwXUCxj%WMslm@MYM=fwcm z>h|s1UEoV#+h8K`c^|q(nYsI5ao}=qd#-n@%K%FRHo`7e!=BK$H=iPZVLnvu2>4;5 z$4Y?6_GI{KLR(pTBeI|glm%go_c7p{A?&?@tqnjXc94QgBb(CT_yUfZAF($3rlN4; zcBCo49V&2p{zhB)_cCFz?9yR=@#oFa-M{I_?>D@j{$`#3{h0j!+Q+rB5a8xQC8?Df zf~gR{+Goj=ty8v=uP+K3R(+PmtJIGkJ#t+XwdlFQ{kZ)O97mCfP7em|ZuGPl=tZk8 z4Ogj_JDBZxyE{`StGX^7!3)v{-X6_$`1CDFOw;^=tZg=#QHJA<^Unfq$S4}BDj%n~ z_fFODSduDJKevH}g?7{U{UA)Sw376P6VH>1RCRJSE_}-8lCr(`aCrCBShkJ7Sm9>B z`_k?VcvvZ?hs;$F4!PVAY?4nWNbkWox+O}thB$8W#(P0|$lF0Noe+HrOJ z8o3X}TKOhksi>;L3DCipc`P;dcY15~wGEHD_s)t6|H<|qF6nt~PjPW^rNHpT@h+eeb8FXj5=u`~?~`_Kl2D zMi4rpxh>SH@lzbrrlPx=?A2T2<8OO)opsr3@GD{4mIK9X0Lw~ydU{HG!ftI%aLDqo z0xX9s%j$Ra;Qw~k=c1B>1y~`dH^cUCy9O#}Be)9)d8Qpz?wetzU4U&2daSmvkP_7I zT#&q?bo&Q5)i407Ss@*2KEo6jvOd>4eM)poY-eL37XJE2LE@g}%T=>R@HufHWAP&; zT)qkUj`Of9)5zYUGf2DF2tl-)>B4kMQj1eW?mi*z!r3gf^IY zl`JTE1&CqiSx=T``0VT~Lt}QyaEy)DYOH3lr6zK}M$T_1zy#X5FlbH~DTYSK!aIRB z8a7uAiYYUIp+dCWcz>PoYcwYlxWHBrD*$!Cr9m#|79{w zOfAF*J;IhuRcuGK=PpBq`VWAjC1>a6?7>NsyU)$e=EH3ZfyCEtTqp3)?K-5H;4};G zef>3P$fr6l4I{P&iEUBa>Sh{*`95H*!dZ>=1}*@VBr1TTnHCC_D17|L7ZcsDIX4|iRGWHQ>njh?!4WsWm6}jCYx)0vLHAN#<)Y}U&1cN(t z(rz2`(Hx#LvWOy3U=h?ZIL(_A@1C7&f_1V2-p>S<$8Ovz@D)2?EaE_TnsHtDNC5Hb zClZCptb$fnS1*WcF0DeVUxo0?b*`I3&av?Yz?AZAoh$5=zkWuFK^iDx9T3tH5hBe4 z8}HqLwWSeJu=tku{?sv7#a~R;lk4Bu-`}h`5WcRCgMIe#>iOjjh;hOFux}39w|Na+ z&pmj~@cUs$5AyE54>1hpzXtxmtQV(y|21`d&EA3yqcFr@%dknfwHug3m+=7b3ODxk zm`PV4k)8%aG&>9#q7-Ksc4@lIdZ^rFiC}B9aA*0_-v>*AWBZKo zTfeqHl2?a724o8HaC?l{`z@4h#@j1XW-uFJLm&K(o#T7g=f8i9Sm-8Xe@*3%b72gz zyf&?n0im_E2j;ch&A|v{Hy4LC1rousrbs@Ox+Y>mI{g!MD%&?#58 z+A4^wF@(pdAhK=L_!jzVtT?_rr-lA2PTwEV?DC^h|7)sAa-oibx-Zp5hnX}VY}s<8 zS{V7l4BXv>i1_hc3)r2{*Z}TU4>9Ri6x^)NgSL{$kroVQmX=?|PEi&1-a03;H447= z(ry@Z1AE*Tb|1FYn8T;JkLnOGz+-n{WO%v~<}o%VCMM3YM{{?6AS%zW!31)SGDrkC zjhiBnQ38hV7jT-fEvw+AS^1rz42CT`54={w6OG0{1@KQrpf}utqV(KYz?_=NYqshU zeV_Ur-xGxSFokd!YX0Pqnu^L~7h0j^qaWeR25>uvZpi{yb`8E~R&3c91_!8^Kn!pB zaR$F@S@I{?B>FHVdfevb=B^j@r*hIraGS+Ifu5u3*%HHjV7OX3>Pu(`S?TmQ;OO<7 zBg!9zSo9`Io3b?lAy|^b=#~Xg_OWAY&hJ1C1)_Flu49`PD&)D-6<& z_Wcv_&clt6UW6mzOhM?^y@Ji|uC9o{z(B_2SR}$d}hP*0|v#c?q2nZ=6 zt}B!Haq^lko7J0D5IynY(=d3IS0I^Uv$p-Eq^hny4bhT=nnNR(HgSI~eKoqx!z6JU z{G)|*7%YV_h>`<{D}#ejSYHcoXh68=n;$fMEzAQq1IDeeTfJx-p^|V=g zCcSOZlXXL{l21BNTauv>!hONxZ%Gh^$t<8+bN~)bC{%#cqtA$2A?LG))tCn5M%j4a z^+3;4G#*c^OUGWkqU2?V0$Bm>4e+K2aE?DEGjjhaXm`ECgVC$Tmv8qct_7VF8+~!#~1Q2_VzUhX)Sp zhkIKAKWhvQ(iO(t4x;-sBm!;Ry*izi+OHV4XB50+ptNU$wJr~t<83ee)+-v(YRwT? zr?6Ke$j4sp=A24VKB?agIFFmSadeOssVm^3s@ETYeK)f!2+`xT|GUJ zI479>s`L!n&9?Ep4vW__Y?8x!TNVc%AWI{U4qz@hi-Q0Vru^f7V^>i$g%aB{?gUQHpn-mCFJkn?mqPW{{WwS^4KW z9!g{e5=c-HM3&45B%o{vNn{2x|0niof3-Z%`}Vqe^@1k%eeQFg^ZkC-A^I1q3tKns z-n3@Tnysc6&)Ke7vtE16nzacVzX!khUH9;h;6L95*AC*Uy$euUS(P zx0!!sJ@{Gj=0(S#HEXuL5&!#LqksOjHETpUrssaP4|SapY)G_+kSQ^wUJ{1mB0`3- z`A@5n-^7qBR1FL?DbaD$*f&R%tKWr;<+!mgy)qVI>v62aM;>e@=(3 zizw5@mi#cnMbGfS*E+kU9u#nwLh!Za$Cr2geTy0M!;xXH zXBRn+lqm*6SW&(%xBe4So)T=CdMu>fNO0~Z$>L`>iN8KSD))A0%Ck)^2L65Co=f-VJ7pi6&jmj_{D)U>+qK%r$(7!x8OZ%|* z01uIYoqNQf&nr`024n`R^pk7HsZOcvTE;dlc~mfCTGet7n<^XidR=aR_R=)wROk>L zlO}0lLmAAVF9*2uiA58x>8HeR`pD)UF8FQePy3Uhy}?AB9lA zOpHu($MWl*qg#$NjU2o2p$<*t-Wo;yHOw_1&-SWm!y-P1L76Y#(Ef-N7)pm*pW=Qo z;=O%U|3{(RM2qk~jBuk#9e!FwRz#?0AcWPJ+KRJVPK3R5s=5A*K`Y2&48AZg)hLj# zlPIB@8am$}Z>8197hAG24o#_Pd%+?e;o?Jt=eNpNX$fm{f2~VAS0N~OWmamM2 z=aMb$M#6XgUa~1?+%NS9s%GE&`N+-F#+=vl?cA~X_B57fntqoSm_k1tkGWl}N+(gW zvxs?jaeXCydF-;Nu(0jywt^=#O}kFXmJ5pEbz7A}d;)9JjMcQ`P7QCw;*TXymLPmc z)<)sR)rNoHcxG>ryW1dof=QmU?YUF%Wp!isDg#O?D^;j||POV~AgW$5J4#G}W|q--L@W zXy>wAOIi}H-BfYy-#1;d&91vWvp@3C<6Yb>-~5*5PZ2EbEqAFr$e5SK{bCUFVaeu) zl)pbR`-gVH}K` zAqb*cNpkl*`#Pbt`OO(2^Ga0&+e}~V$RhL>=l|BSS(E@XSlK6gXLM}Xq@8F@eIAm5 zF7?z`f*$Q%_Jx5};8e2E^U3hP*IWe|`0Ic)Wz&>&|SCV86cro;0~ zp9`D_@SCakP6wCrW%1;bj+muBF9G?P z`(ICWdc~cRQHdd;p544Bow7LH69g{0`1Iy$$ya`A5~%f_%SJg@=K`t4F6p(>IyHv% zUY}pzVNv~{RWNLLw}AU*E9zk2%1KJxInT+ImG@0@hM%o|M8sWae&+YrnCFrN7{1Wy zfa|4&rvtNHmFQ%XJ&`MKp>&i)O<`z2Qx zl?)o*SfH+hiRjjFtvMdr5@9kt_gJ5@aQ#I2*8kk#G&z1pigdE7Mpkp?!N>ymh^qGe zfjj(;3*)4p*KSy6ECX$=Jyk`nswrPyj}#dqHyE2=xFS)8VDz$HZ`N9{$S|TS1YU$7 z8*X>en{I6&LS4h&W{pLUG*;h|GWzK6KHxjlJyOfZW4T96ASDGHbJrCdHp6md1-H2! z*_x`m_|zyQ_-5+dd)LV7P?)LXj|1?V3I-vxiO^KCE8A*%u(iUlB*wS5JQ_87XM)pr z{`yYt=C;&b{KNHr1lkVg49mZF!!M8e*o|y`rt86{7VC1=v{m|jdY?}jcP*#vL!3y; zT;~a@&@G0Yqu>?l)ndV^n?r{1_t`qXW#qWy>u4NB({q=TJch_(FkhZBOFQ;NeUo9lwMUXu(;jI;i$qhg5w`MM#FD>qD^C}VwNWcRV-ujn`Fy25!BTL`0; zo4Qk6d0Pw-zZ;CFTrS2^$W3}X?nqjYv}~03VpqoGw3KyiTE1L4OUL;dBic_@K;3-8 z=w3>sNBi%t`0Ze3q3z;0ox>*ZnG_pVwjy1Cs!cz2{SVYc>zE>B)4k>i=V4=N?XqxS zE2i3-?FwJqz$Ri(t)vW{=gdA!4B;@lnrnsq=IeW-gS?)O5JOm_muJWn{~F;F*nvmk z??3&_TIMd&(g^hFaL}HOhrWzmCU|sF;qRJmxp0bI_+b?KmmG>!T2RYmSA>dtdqViK zdkCxh6?%S=h;DP-bI&DmWhBcLldEV=%|Siqc{0vWRU^ZM)nJi$r?C=>Aspn}PAEMR zQn;Kk@J4D7_9;ST@5%9;?XoF0<$jYt_9=I#2Pc~R)k^=k`slB{wNF`FWv|M96?GL2 zOE;@0#|l1pST4+CMQoAA&BNg9QP84sn4DI4TiU7X4`HzmZe|u_PMaC0gI84ZuA5j0 zDI)4Bk=eK*EXPm8{fuIVdl!sx7pJR{Q1TSc#8ge2Nj_(T`(g)P;nrP8>tDbqgMNLb zZ(RbLRN#{z?C9|-=BLg^ZChk4Jg)5Uy0t-Si3c82Vt%2-o#ws&E|V;^8A(7~wRPJz zh|aJuGTVWfW$VI0GYh*?Q2Zf2nQP(lY|-^9!B=&i-^OX~ z*=Eq9G*ZwgSGC0K=OUo4MvaD{B16`alYdB%7O5LgTFI5CzrgZ}ilpFs`r{-uuV?u;|664VY#m~E0LP5BQPWX-LDWS8_H{?%lY9%jcN;E>&doG~$ z9&Y4QNifpHknp`me@v74h?0NARJv;k524?$7yRIc?O^|39ZmMWp!-hM!7~ze(zZvB zsvV#RxxPqd1(iz;ZtgEW80lR)*E-hhOel8Q^gN-w+{a??<3j=7`|J*fF7HL!a#+RC z_Jo>L1R9et#M1NYA1ykZ)j=G9lUmbG zAWC^5Z@6?n)Id*R*n&|b%6oPmYI(K?OLLCO#xD(b8IP7r~3)?4ALzOoWC zS{kZ(w4xxw-wQi!;d47L`WVBcNO`|N;B`8PUV7&ZO#pAjy=#aMZ0!+%UlO;2Q z9x|qNc@EE!^XZNwaDL-%q`O5TmK07W?hd5b0}$+1;guY8#f=6Ga;qUFkKH6`wR%uP|JfMdDfMFLc|*vtgmaU7|wt z0=IWAM>y!~IPsyV&`9^lqhxj!b0>!oreEdcFDxQ)-!}_RKAX>%UdVKuSm6{ZSD=e2 z<7&vh9Go&iLlUK?jYm*~3$MxXrP>(hPh(!wvkUVB{8lJtJn=7w9yXb=VJKOk9^>N5vDTTjQ{Z9&}k%z8$cOE4k z-XxRa;8K0%&E0b`S3VyquCuT3$ct5=Wlp3ojyP8O^aXNwv6#cnlSWsU-ePByQnRe& zr_}dXu!;@XU+7JU2jDqU<(^7EqmvMpEGVe;Ex{MZ`+iKhyv^z68>|lYG=R>FH(tz-4WHpV5uj z=f@@NI<5CDfIh$vJ7$iwcf%~crPS7^9t)^p3=ynZ>pe3ibQVK5D`Gx~Fq7oz^cv1m z>MLKJ$X(nSQVcA-`uWhI+UY8&>ybv$(_`MY(4=E+r)X;p1?nYXh4$4P!UwZsz+;E`H8+7u(J zRSzzz$q+YyDzj|#-RTH%=|o;l?Pbvk$s3F}{Ab)h@cjA~D%9^yq@&-XiJlcaq)KuM zF@(OBe*eS2ID{sOsls`k=~Y{2C!0yMg=#LnYnDrgO(tugRYK$bnuj$%@n%5phXN`y zz6gcQ^+ZjdaoN>T!G47M%=>F~tXb3NMe#}xr~SD` zY!hF2?tbe~L=r^EpD_M z+xpEkLk?}ExO)7_gAYb=M%?R>?spVt%+5^i{mR~ca=M-V^T=;?>%fH$uHAWR?spWc zdV+>0g~;B&vsrHE!<1(mNbx^}zY~98&AXlA+t%+)u|8GyQFrDTopRSmJm6m!^6v#w zwJ3=j72d?kg}+pnd=~YU5B_>%d-(S;Ktl1Jn;TOpdi3>Gy*=&n3fotmb#C&$I(ALM*eg}{D?=-%>_)fDZm?JnQzbV_I-WqpJ0-Pw;FPOiQJWhLmm!o zrLRo-vI-5-gVL->dX@DN+^IaekjG~4)`!nb_Y_S$(f4PuYB{c=zLv*2^v7Q5)$K-i zY@VXU&lMOmIugq?CNYgg_J_;0{v@_@N#dF8+|-g^Q^T9hVWl5PCZaMz=`ILfP%(81 zix3>3?=in>=GI&6Vz45NHDKM0QV2=mQ7OW)QYKoMCOqqae6WK>X`cpGa zHbV(;7kwnlnPLS=Kbe>8U%Ti)%`unbe0d{9`hM%~DjDlj9lE}~Em$$f{w4XMk&xdd z#mZA6$Zlrd|GF@KjZj(GvKxn9=V|ZSCTiTo`J&@rbs-(yrEzIFm*o3O8abF0R~?4b z2$3?Xrdh5upEZqM`7m;S)1JnSk}a=pZ6F1;8|ok0&nB^rsO8>voPJ-t3gJ=RspJR09{mn z6uCH*ft=64yfLhv^h37g#8v5tjt*IpMn7lQTNoz)ZWmdAkbcmpgsowGcy?>(f`pxH zLSFAm>!YAn%i z)9*lXUGr<{={cKyb5Spx?%i23`6?5hCQE}Y(QG7Pmz1b_gpK7RkZgC7Qk6e?&_$2E z3)%a2o-Q(4ctToX;b_%3!|66kgunh=qiFU_+2(N04 zOSE5~vu=EE)ShAEgqzQ8vhF%n-MkW*v3~PTWzsG4%TMZd2)K(uE~F5I*VBNzSkZfR zyt1(!ZhpX@Vz{CR#`(u#Fqu~dIPd{Z>ius=YW~RGt*z!pWa^0t8|2_vTs4KhnCZlS zAI0iVm8r}DG3ExqAvhU}ds zzky1>FFN8^;{4M}m0vTG#bfLJdxe)v*qu`uN9bz=N*Pza)^J%ewRJV(+J_FO-}uEj zQru{HC0Z5ONuZ1Wc^$+&0p3g)+VQWQa%O$*?`ACntk4sOv?`3sze;>F%`Qj*et^$B zPZ%Tl(~zlp*KRL$kWb!K&{}YVN}rK>3@!@#!ang<+qXVfJ36}NYgJZ^6IbQA=mRwm z6Ccp{FS4)g(c_}iBZdQvqo4h^FJF?dOR4WZJocWrv8@?DQuMf9)9xWH$?5pTFmdp2hEPs;YffTs2Np=(DVO3gH zJ$-qZ$HH5`O~?caXr=$zR6V~_`_DhPIY9775(dMINT2V@OFJ>;(Yb!Z*IJ+G7F%Z+ zMI%x3&03VI$2wj>zc|$)t`g0(3=H?stoLz!da&KqCfUC8XEyF#0pn^y{Mnx$`ir}O zZGPPhDBFy_vOK2>AIMd|jz@KgH8o>OO4YtQYbM6qU}MGM*OEvpQ-JD_9Ej?Q;Ri}Q zxDg@&r#O6Z8uYY5^XCEZzETess9T3+BEwe3a3o0?Rpr4G`d)2&kPW-Mln3tw(E=J( zlArPbX*_lNhcjxPKcpyA|8y=+lfQQ9XA)(Ar>Dc|#Ofqo z@56f)@YT`!{N`4&$WNb3%wf&(2EUA4-I=^WDdI!SE*;g`Vz*3*`N{$KJS6_L+E0s) zBB6lZbRc6!+k=mZB-@Z86@x~tocKI@5YMm#TKROhKP70l%#VQ!kltjxHD9@RY zM`68=Ue$xODAHDWy{ZdJ=x&9Zb>9I!;hDJO#rB+6@S5%}D7MS9g0IYtVFW7^8Ej(k z)NowU$PZ^W9?JOHE#7&7Q!2GMeCe zO|4^7%W=P_dVWua9Ijp(wtIA(UKt}9w{dTF&8n4K52%wID>JjNLH9~rLru-+Sq?ql z7Z#vzR7#5>b2C`Kkp=^0#x;W6i_xtb5$^s7_k)FXdkjKu4Aavu&WY96LmQw#Y_>Wv zmRO(H7PIF>XYKOvePNlrf6bASkhmT8=@_oJzFXZI{cG=jfM%WFs&#h6!Hq?$E<3$G z7g0bJ67|h4y6x;e@0S$;Dqxd)e!LyW<Pt)n=sU7|<1tj|yUoMo1-x0oa3p<_9t zW3?jw$Fwj;YP81YIM2P*(1(R!P1h0Nu;Y)4XZ0eLp#R$07VtfOZ zaWz=QJ@||x?sxIzQxi{voI$tSESn-7-IZs}8F83i-W@j|rG)vC;mzx^Hm1jw70hD< zi&RW4F`j`D2-w)To(ACb=sj6+8D5S)@Y+}oREugUiG5c zbLjP?mN{yk1Mfcz!=4ZTacxM#jh&DNFiO0^{Z~Eo<;ce|*KL&`*LLcTdl;>cG~Kv& z-M=ekVqu-K_}f=kef#m|7AEBea-h;b2pB_7SGVrH{X;x#)kgDPKVB6uLIaRnhFzNA zUx^-pqgFyV!xyxw(@0A#RhFR8k2ml|T^Wqi$&D~5^@c|$s{T;@h^ z5WEL+>-!fv4v+f$vleSo9S!LG4R+$mMJ3e;I!TAOCG$+ILesX9&wKgyLC)D0XfA?uld#r*RB_w(;opltl&=DK=u7 zf!8n{r#OKh*QtVe(cWkg z&-jzp{WK)*rhwmgCt-n96{HR=wup##?7eYmgv+m>$RjzSge#1@_5H8s!99pa+#8e! zWi9O8215q?SoJd23zx>Sj2J@{3J%qps*f~JK0M2qXic36Uzq$%sQvWYWlNr0cc$0oR5e0JOku0-{8})Yx#P}Av7UX z$19d367mE>)zcMeZDO*4>@+=~!e(5FQW%G2g?)`X*bu<&v#f) zgW9e*+kgt4^^>+~@vZ;Ri~VBE17J&1O4fxqs|aah7<}b@OttS*UmZdk%8O1aSwD~j zb)C{AFJGlR_pwN$0EG<>$gyc;wRe{)?9ht1_ky8TiwO;Q3B2~ZQc5fiMAl=ZAx2PI z|IEat1o9S?51-mmFfhf*%au2#j06i^Xf^Gu##CM3WYR=BiXT`mp`)w~Y3&F62-deN zrtQ&gdX%|lwel?7npHKjP2c1F4lV5s+xARiHK{Nxde{!^QIGBau7T1d2|Jgn`}foi z>$Z8!pU|K`*{VdkpYfC#o3`jhSV+i;))+RCO>u9GwX7MCf#~}1FUX{*J3POc_cYLK z*lyx{?4BtxwGj2i=ntk`VnpD4$ENZ!mLvk+J={xns=RjnWaO^aF&vD=F_leG!Yn_7 zX(^x3BF!A77^!JjXhPi7q*LSbZ+*Wt5Qbh%g2|cXOW0kds%c9ZExS>HqL02NF5aD6 z|6N=T1#YoGkYWeW&Jy!aqr9s2w-vC$+Zhv2ocn35;rz_{yxFWmbJGT}6b_a&ggNe# zO>u}Um^PQJxw}=KTWE$>eOxeE3vny_t*cNKz$G>*IZq}e? zr*Mo6+SWC>Ktvj~h_x1szf^M7_(Ab!4;qdbcMgi?)>4<=Z)5$TBNdQ_pBgvfPrs=@ z@Ytm6VNp?q+G*Y1Y~meX)i0Ajrbyog^m_hGXYW)6jDmrG`ShyTvCNaz1BERzyjoH| z7pdqvWH(eDws4Ext^2+LPVdifN{a8d`t28XMeUNyOSedWTx`)1eM`4=M6aWeEUx98 zcnBw8ZnOzLTp#V~Hcgy4o^}#3*XSVp5@ozh?XH<^kQZ{FYuw+dF~`UPray+q6n->7 zvKH8qdv7Pg|66q|ZjQh9wh6KsAJ95p9xSDi!48?}e{{(llvDc$t2R@)M49r+jjjpS z9jTrBwZKK~->v$L?e-Ki+O9_MKOaVHUNx1v;`VUczI)Zz$|ZtmRRM6|IY%Qwr`T9> zZv8lLV~8f3Gc;b8sTGq2D#Xw{Jm$O&> zY9eOo?eI=RCE|D)uG%<*QomRO&vD|vLPlF}9>w7q`-H;BJytMBKkV`aJ0ANNLj6Bexr6C8W#z8$) zwR$?49yY#<5H>$v?=Ut0X33?v)kEDmi&DcD~ zIe~*=0zjEX;#|WEQcncs8Ze%h(dOHY*!AvLkT&>d1{5oe!7>R)#8F0n4g)g{^`^v-Tc?To= z*rj)h+3>@l#W4TbVn8;?US)i;BTFVDyPdnp5Nl&oieaPMhZgytqm)9#)aOl<6ms*< z!#4pH3Y?#4=k%n&Kz+K3N4uYyTePlR<|vGlKToF<>SM?wXpyT#S)WQX?H(4(77ALe z*RLDzlPV5rOy@m8`L*suGGDiUowU;Qhjsi#!VoQrp+{0t!Z5cPGZhJ+Dy#+J0GpJ4 zL*M`mcCNhPIGnJs0-%0v%^qE!MAA~fub`-*uA2zpV)coj@hUN}f_fBQ)^X{EI1kSX zbAZ!fJhV{8wl$jRHFW7y(e`SAjo^|1<3f!L6gO7*uPkQ3`I6$LuFQ~>Y{@{f0x!gn zsX??|QY)ODt6m`hw}#;hzxSX|J+U?xbhoA%P%YC=_Gnx#e^(1e1Me^QX5CXSz6mgZ z<&7kxfl7AZi;2TXOD)%NMaPm?(5r>RN7b}@3~BL~BOOJ*<&f1^&Nh?JyGftf*c9H0 z*lRpXI}ourX2Q|3e%>du>h@sVfD%O-cOH%QRKSXL|8quHfi4dR;zqRf{n$dy#-YVX z-@w_Uzz^TN@}^mTxgDtJdf$8A+KeL};-GJf!Cm|!u<{{p9=n*1PbW$#8V zxe+awc;}hl#wNft#qz+PZ1=aX@baidKooSZveP&Gvp}*11&tU#Z(L5Z#ByCYZ<8}* ze8ts?zX}B((v3VZ7fGe}W*~`qM$7$M(?qen>s@it)}UrRM+Ji{p-E~@KrAzL$;-eH8MNx4%jW?n{aE^ChQFwfY`mvME78{sh;kvNGLoyu zMZ99(wdSSzol`PNcjb(@(JywLF9 zZ?f-#Td`|xKPd9eh?$Qah{+FoqX&`fL3%BZMT*wA-xljasish7b8xt*!7q1x!>K6v zxH*IFB#$+_u|3xGjU60AP1E)*7^p%-6(NB{Jgrd_FEKx$6he$fVkeFLjZlwCo}VW$ zFvvw8`H>z zp-`s#+W9;r`J=;Wx9Q`pnvUbsPkB##4ZZH9Qf7Q9uKOSZ)+o0M-VDLz3W2=ogs;{>MH z7TLR2X~jKfQ_C+TMOX4Gt)azf9DH`e&t0@DCB~Wseqh+Z^;r$5vmM^ zcx|F*I8D{9{wVKS605phFjr&ZD?n+OFE5fYISfpz69yG+_JR`fdE%AksH!&hBe@wH zGCP72=11^bV}t@<-^x&%{OQ)oaLBpKPFtd1%OeRU(f1lA9Q*=Z@R(-ZsxJsFhYBXQ zbs#~)F21|}&}Scs`DlV3SAc*+9MwSI?k-kO7ZPBD{1Gjo4g|YTTN02$Y3USWHf=0+ zHyS@trK32<1_{S+`)1Au(+Fh^-SM$fk7jA#qs=%c0%@upJF|_%LLfiJ_*b13aw`oO zZ?))hPDs_tjR(}UDgC8%q;zir!=qecDMBN1)6?;x8LIp@G*T1*7SW>=SJ}>bu0m~g>>qYy<4H$4so;dNCG0CC(#G){+CgnM5P+A1oh;B&EwXBA^#AC-IFD2HMAJNW=X3c+M zz<5VAb(`YOWiK7FjP!PQo8~U|j}CjlU(=PAnP2xGh|cDEX@T(YB3C0?7qw~=npwKJ z@;Y}DzGQ7>@h70D=mIkf#DN2S&70X+hWZ(p@Ol}$Uy-Q?IzskBypW^aInYcX$r*wTrI_^Nt6LczZi;lg##KKt*g@9?@uLmu^)fZ3=6CrDEAv`+Mc`TOYJx}ePSHsq zLULYhP+A@fjxMuQtp<0c- z1QqVKdzARx;l&fVLcsfr#Y&C8dz_-7Go$wf8p-MQ6RXt?4|a`kpupEXF5-T4kE^cq z8wmd{3cv&B@%J5|tp18-B->%9mg|g_0rMDT4_!5_Zf1ah z7aiUW2?O+z033**y6-k9Cohe^e6uZR2rH_UOX1Lw832o>IZ-fwiWMtAtQwB0YziPo z<`MBmg)h60<60-mgMIZPW;8K@l?EZb8T3x;WYx-dr{wi+e0lu^$X1#^9l$Ps-XOMS zk$}>ZgaRJPD(&RTVuxR{rpxXfDL{}d9=hOuV=$mm0pmi<{!2{u1j-U!%x;Ec0Nl-3 zMJ#O<*4nw0kAx`U>P}63-o^SosE{-`&#hL)=^@J$DF|A7EDQ_gfK(kc!5tF|u>?poJPSTnSZOZ~$a!S#lPtI~M5Z^}n4GbPBo#!* zh85)R?rx}0tcw7tJZC_}`RZcixLQ)sjqIyMaUO{3P@-lj=K*zts#|TR>JjAJRNj{& zDVdD=xZBsj{0W{9Sw1-yAZBtf5wO;IvzmDpkB1x6x+j1T90ZIC%S|nldhdZ(fR-J9 zuuI4J#pMeANk1$Q&B9Kgfa}dg=+kcA5^GVg*z=`9+=VI3&On4LwM5xKRvyeuJE9F- zy$KI$iLG#zW@%>PX4YL$ga`=idF|_521KW`%@ZUAQiyUdAQyYb4y@WT_Uk-N=lXKn z>6?`fpLYcxIz(7icgC#T)E>7Sw-Q%mU|RjlFsj}m=Druhw*6Z?_*qP7_U;08_FEe+ zQ9g2|2TGDJ8^W-9o%}6eMZt!OR4L!etp@OFY4y%%X#M zu?+jEF$Y*Ok3ug1s-^_G!MBp_`=q*4K%sL6fp-B9q`0fVW+lD>(NIdeCs>u!yN_M} zLpa z*u>~Oe()aFE=1gYA5rLdYhk8#x-G3oASS%hpm0pXM*YUKtDmC%8 zC==a7E^9X$NDJ;X#gMu$x*74myn*wVXN9$rEiar1+$F|tAS*EOXQw~_A@!)oJ%2k= z2$Kc~91~vzG+v?NK{0L5KumSMbqVe;!KAS=*1cSUqIPI7`rA)34pV`COStmpdy1>< zvy*t12<&rkS^&g&7uiEYcxpvaj0qijW@EwO)pKO!W>({Z=z+jL)kV5Kd0cmDzC|aY z(}qq_8MfaWHdE3_1mgT#G}yVtCiiy=gAiLq{CP3Z3!A3_aefr1A+Fk56n-hJ*H^6A z083DSGQHSjZVtK@y}!a&PTm*3l&90D%WJ?FK3cZ)}&2XO%cqtL1c$hzEPlarL50tFx!h0uhPlbMEcB z-0TrhnPzY*e4ql1;8gTfM(kg#5GaCHd{@@5rn~>RY(alUx^U}+@W-FN#AeP<2zdr>WMD$pe znu2V_P~j0RAc%85oRwPfa4EYsp6XAYiakF3`{<%s1NqDA&E(6rjU8y>NUQv)=EYHl=0H2f3GuyZ~No7@$ph;ScfwT_7`cz4tOUEXQ zgxJE)^5~mDu=Il=@&i=#{Rd~L1-2gwRb>&p*z*#$NlBpd0EieAwI}d*Y|`xqR?63s z;@^)pC7|V%m+U|1=XaI@F&E>=+a|BqqQ&6%>+1qo&tvLSUtT%;)qd0#A3Pd8cLq=9 z7Ro)kckbD>tbLK*Jt@*D;$BVhOlwrrmQ`?qq~9x4XDUu#`r8KN8v;e&I$xRl zS+L1MCL=kM1q!=;@~UnVct|_z$V%?)ZisAt$xkY@V^+B)F8P)R)e4W=gOqbLSgR^V zeeyA93rbW~HW>s@e$uS^BtbC(>b87kLwt8iG7!ctTFvN0EZ>CPIn?0=dD0I(3K=K=FM6) zAN=#&3qa4;L1_Br-GkEYAlUSvwiDzhb~e;tm-|LtM|v8LbCQ5FcXo9h-ku`6w_vWU zMn&x`5FU0I|NAwrk9_?0Co!!FT;=zPe|`JmmR0@ahp(mm?yBuBrh|Kx->ja_{n|ir zVq-W4#;KK_U&uhOl*zcA&=i^=_K|L9I=<4~&xt;eJ}KTDUJGp@$5`178HW%5nT=Rs zbS`cO9Vnv-C`X`7vqlrFt!mUycCc?}W{zOKyxIU7gsIKY*`bK}m{emd2AJD=levIXEIo-yt3C`42xP{d2T zK%zmc-eJ;Iz@ldAxHoK}JfDsS7Iw9!3$g0HCh;8}1!|}>_zcMO_IYJ6son+)VoFwM+4-9+5r?z()A!yGO1p?^IxLanZIDGBW zWn`lQa@oU{{|zN|c5owe22tejbB-lf?-+n|BEK}#CmF!04$&0^pc|^atd+GI^vcH@ zE&TE}wE%Y0C9*kq;@~m82~e6oiB8*%<*7jzHhct`k{BM3?EHXY(~fVHWdKy~6<$=t@@KEfW~y0sZ^Z-0 z0N@6MP)c3W(1e{On^G`PD8B@#Fq>crG>yX7qk!=(LShPku|YkTu?Xp{Kj)u%!qU;e^U5r`M=_r^)nV$&&s(zC| zRvy<$CM+0_D<2t&@HbgME{;cW0Qcs4AofC&#tvje43n~*GG$YGzmHM(qdrp82J^(g zna+MA=Lf25L+!m+X@)+T-wz_7= zxVLNvrsIB&*}pRl0?~;FK`Dq1_pBxMfj6xw!RlmFoP3CwurNUob+7T@>MQQbRq^)k{f*S zwaLrnH^Bb_K{Ije{?7y*Fj2v>Z_-XGkVr^u|K}F`nzgxo?Zd|9^MjgKGbAVG~m>opGEYb(4=s{#8Sqv}{;hsW2oQRKBlX)`(X)?FI(wIa0m< zn+LWnUgrr;ck<@|uysjNgNSo^rD|UxnzgAd5E2L>%*zx$Xre?h^%yIC4efmr4|-^Iu*WRO>=9R|;`N?-=?0zOHf~!tHPDd) z7*xEq@aHVEMsNY5IER>Sh$4zH`639Y2aCfg?hOwDz~JT>3sD!u?4Y~A&P9B3Rt)i} z7u5Hj;hBE%z6({rg56dOu7E)w18Mn1>^sxa^Ey~z1U1)Eo|<(*N*s)&ST#aZ2P%9g z(!-|@QLHAY7Nb5+;-K=DGO+YQT!_H|axTG-KzyRGu7+7TKoi54664O`SQec``iqaj zo8nb4=!LjaF<;oGaB`U`Hg74hpexjzTc0a*@uL$fL_$D3WWIXN;$TYA0-!sAlvcna z1)N*L)*jAp`Na z4Qi=``_*lhv8|6S49C z`9mEmU*nf!wr&{V%0w}M{^Zx-?^Z~fd-OhcY|{bKX)u^5CsH*{09;G7)Y>kje9F^^bLz91t2QJ=Zdp>U=oJ3@>JEd^?@p+ex@RWl~nB537vRp ze6J!VyXY!5QNy|FgZLOL9@OvnFkA2fqi|$@<-xCiVFasnQ4t`D0so-!$6iE0<%hJ4 z=icpiRhu4Sy2lyzisXghR^d~*lO&U&TkiqG?c8)+@)bZ-{aWTJ{UOt@{s=i}4T5L3 zOH+Houm1ux?nzNq7qU@|5P)JaE}D3j$;Qo&H1fux6vN*U9I1z@4@laa zJZFzxU++1IIn?26&M_PSMP1~Q|1|LZp*x#eUX#@q=S$cgO?`i|V7GqJve+c2DS1DnV3PY+&OF#@B(aa8`?cTUbS!DQ)I+CzVbWE?febLG~Kae_6XzlN}6>%chrHkKASI6 zcn>hfXVJm#A8{A<1e^M9oA!6SF1i(-?960uLARd*5(*0}dJZF4 zkth6+@gZg%&VDa;3XS`cGiZveRLoO^)uJ7C zq&K}inCUR}EL&bj-9ECwI2I9MiOG)M85|2R&5ED>dUsBb@SxqmLZc!m$HM@a-X#a^ zkhBmCXxezYUz{d2kd^+V@A&C+Xt{&jj9`aX`5!qKd=~skko~bg9@%$jI&%WIry{7} zvfU8m-ff0hIVinraU{|a^yh03N@Wk{0al zv_a##T;gp8wxzX*>0t^yQdt$C$-pCdd5;^nr|_h|IVboZ zX~9lWP1|(r8w34+i3|U?f# zR?-=X7_r{pSl?NNdy)gu$xF`pHO&}H&Nvte`Y~pe`T>l;Z_i$Q4jYa>K>L0BeqcHc zlzNz7Em+7}8bnZCqLFO??XW><(yM+^}``C}@^mlwu$l z!^OuO{D;V_WGDFSfZ1)cAAFpc32LhPH$HxfHR4Xzlh8V7ZZNgN7Yej*1I}04U3eIR zI>sw9;sGMX=K=+lH(zuwSR78uystUO$pk?T7HB#11KEn!!=>Q#8X5gWrKi~aF9`rJ zHZw&LNI)X;(!&d3s-P8;R=)|kl*0NwhT*}(39tX@S%rq>9rGWgp+T-6)7J^+kOLB1 zyfF~3t4@Uzo95$rgjNT41`tiIN6g zzy59)Bpu;eSm;D5b#L5;1bBcqrqh0^i;fiuFe~7%Hq5$X)OF?0ptL+Uf7!m&03b}-H*qzlJ#!|3x{gbe#sc0Icy;rH|AS03Q^@K5C2%Tl!psm*Z{U`< zJ{c<}bA7aQ{D#D*0>lv6*LENgD7CMl&T2q*TEySW&X4&{?Q`|@j?D5#bC99+F60Bn zl6(|0kOuyOPjm%XO@X`q%@u45-QCO-8>Wnk^8;tUdSJVGHnI=mZ-eIjF>ipNrVSQA zYA7LZ_$b6p5lIUWB7lSWm2x(lm7|Cz%ozAQha#eHZUHBIi6!ohfc67Xzq1kx1g7|b zk0)d$EmBi13>59wq0ipkvTK<1UcG?&EZ?B|_5?^@KfZJpD1W$BieQeawyx1NRqQA4 zhzrrCt)~Y8CP4z6%3aj^K37e{&oj_+OSam@7{noaI!It+fCz%AU1C%58SLHZyIao` zm#FA!=ESPca)E%&ZU3sWTJbJJLm__C++$tDhB9XgrWOWEtg}DUh+6&}CD<})mun%p z$%1}Mv>YdTcijOAN_EbE^S|vzE}GRpnmNZQ#fpSqR%^T)Ij)2>&fAZ?y$>p zLEuWO4_*8(KkI*qr@)MhmDg{3;aYog77&4r1(#OO+CHF{R(npiUF)5JVFf(^puDf} z?rLa12iJ+knSiZ*9c$IP$LRWppEzL2DjW6bKXZKD1@g=78OFxqt6ntuuV!V5`vq#5 zmo)((ZSmQFZ!GrYdfTZB67xWnTWVgrVcUYADb8ZuI5@ub>H94|J``JP*_r>fM*b_e zT0M~-1O_}eP`JA*vyXsys%RKq1hcfT~$IG(Y=&&_NNe7x4W-4b2 zMUCT}!k8haK@KrRB~wY1mTHvKILtUD$ILjD5QDO zKUwiFdBTT&HP`zuk0X=7zZL=4WEVH`is&u-4BnG|s{)?2%P*H#e`{mq?45t>0pp>6 zMJDC2KmRK(1#wmHfOo`q4-QV>24O*H5W;i-amP$E1SQsOVm1O^nFiGHwCN7n+Jl4`66i4T!Q{-+ z`3@ks-iIe&S)LGyYydoMXgni|X^eV`*hC{&vSB;iAIyo%LqLwitJZJ&>v0A?G~{tKY(-;`1k;ED4whr+=*#N2oiL*0)kaVQxN;x{TS$*+S|D@1aLAVM4XHWFIGY zzn)Qup9J&8*?2_@sASHzf2%C&7w5BYq-0C8|LbY?|KHmmRKlu#`AzQ>`3x|)kDwldr$vp)04RE1Ceq&YU+^rvCPr* z2P885klAV&(+vQGoqTA*8B{Pblwc1z!FXh`^b`2JUX{FS&%RQ^>oQRKIo|^r*Ip>j z=h3H6g7E4<*hYiPS^%OUfZA?IQWr9iqMp~k_yYu14^i#l+(~Tt%Bn}hH^gpEXM8yW zNnN&dvz!V5$SY{>61EEnx1v$)w~UZkhdycQ1hJTGQ}8hj)Re)1_rSO!vJr4R><$gY z?9IOR97lbtu1U4HVn?5OcOH3cLtkqE^pw?*DPM*FDTx&bFSZd3`u@RKfr1>cyCWZe zVA+5x=_RSqRaDY_7|ih_>k>2=5x2w%R%v_MU%%`EQAVQIgU~N8 zp$`}}&e(nY#@6pXB-kE&pO}pnAdH?bcLMi8TQ@yHY&Z)vq!4Vu^WI9qxa0AOuFdJl zcxdt4gC8h}H*INnaVTC?3|^kc%mX^UNi{w%R%OYBCAV(ebK>SicLDz>Of5>A&s8AE zactxw>Dhk*l>bu&lMq>_xZ6K1!#5QvQ?CApGUag`Ocgy2)7n9U`Cwt-oijqyfE+e} zc+2!y99gOEn{_uByaJ19o>RTZyBI4 z@r)P$ESv^bIT#2tFi1>1L>t&*#0cHm%ry*)Z-~60NOFtMP`=tVcJ&TOxUnbyfKZ_CH zk~V27?UnYi7aFd=`GQBwnQ;YVY$!;*H*h-j-D!XB4MTzk{JOurwg67RG)cwqkZdHGgJ#WxcsRJq9ws^pXT1V5+rQ3=yEs4=|(39O)s3X^^nk3kP$@o z;e!h9?AG=FMkvYkgqdkg{!20EsN*+cb?tC|7Mt;Otk}coz3J=)J9sr(i}f^eVfaQB zt{~>l#-DZ9+!|IwwA~6zbV}M8+9!V9@B01)U>cr$4s=VHMStx7V+?YFs6MzE;XmJj zzv18Meb7*`*9SyxuYzKsJ|!#L;mHLkmgajafR#E1hE-c07#Vn+aWK8hzwnJK5*z?s zW;K#*?-d~uXYev3pD#VG53J8ci^MUWam@xASV32+be zx&hlYTJ=X2AAD}T@U;_h0Z_jZ&`H}J1fn_!pJ$hPnFw>}&GVvEr*A?Cq@2hFwk*O? zAT9qBj3z3fl}%&Omh(Xfs)ry z|7_B5yZHLSe^o%W;T}+izZBlOF%HEIGr#VeR|Tt9?7GG9bm0W#aUg0Zf19Qlq}#9B zY|sTD%hPU{Wzdqf9%US_zq&4T6THnO>`DHIuFNyC2}9&})c^>KA<$x_mYU<#=X}ji zs5{nnGt6}k84`j=$@~c7ivp@o@CXC6hvkhBmP4J=k0J%n)$P%$3^${XZ6;s$Mwa_R ze^Cshno#|6ph@R^>o$0T&+VXdo!|O(m&(6?7ysQ-b*LD8kFYidvdpf1J}2`{6un8y z^Xe_cBdfGo?ZZU2OPK}esuAC1sXXx|r@(kwCS`VNpx@p?8b$RphO7HmRmUx>3}QoF z*VoxA6QEijK}KaecK`M9mhn7CC~+4Cirn1VNn|widhxq9_&zV(J#A`;%$5(hh9Z-Q zsftGc@%M)S8miB9W2Z90eEUIH_shON>b?tgu;Bn6T(D3%0!)KlWfGm!$Xs7-vI*u| z39~6$|dKjb~J1fT{I!theJ<-R4c^tc<#Ui7|Ki{ zeEi*q>sRbP`-fXkiR*)3PcGl~0P&{#$H8NP<}8ns{JjeP!crR0b8B)Gp{PUXIC~M8 zb(7KgO8KP2zyM+$jD!S3qWT?Q7~g{sM;kVSw;LiH{<&h={?N7HANG%jwXNyKcyp~p zJb|jZ3$qLK&~L?O-c7;H8`*&c%Tce5_HH*Idy@u%!72E%Fn0!kh0{o(0mJnlE7mP* zQb$~p{_%va6PN}n!(Io%9CFDYew|&G9DtB{5l|Bfw4ktF2uTtWcOjUCrb0GAXr|BJ z*@9V)6~wjyXnry<1ofz~(LR=s>t>6F&XZ=My0=h$K?PfW^u=xWuvv-y^N6wBKOX(X zJNG_;NLKhT{Ot)q`+|M-L21wm^lBGqtz4jcbw)gcG#eD-Wd*(CM1LlD5cr~PN9QyC zn%`;wmi;LejJRZ!Y8En3}yIyK(9(rKuu~%p6Mwc_*2L+>Mow zV1yPFgpbRQeBvV?5OPCUhnP&0Y&JW&^khreDl;#YLjwA_;=BFxhVlMkB$f2Cn7W+@!QBgv+$ovYrr*W<0@Lf630>Q& zwj%}^<=W+w%dEd5)*t`)d~V;G*1~*yDoUBEL3n*3+k&|dBw`YGkF?%kIj5wh!3@z5 zjAvc~h*aydce1P#Jm)jyu^O$0@AjSn*-ABJ!Yc5E`cq`ZCCdm5Zbidm1E-1}T~?l$ zf2HroWh4JGkNn?{%et!f-ltNHFm8SL;P(0a9>lc`DX%tjx@;QXt0U?XJ2%#epQZd^ zpnv1R=MnWO80ntoI|pzPh0Fyx;85708!0k^Bk7zZc>#RcAO|b#((jI_K~ggS-*dc9 z+$yr4;j#o{#Ny`PFYx{muacVU+coCMEp2y}MG^z(;47rOmFdj>{shyFtjw0!iZYA8 z24Dst!i~+0FuZRNFb#ssie{6ceFW02D5IujExi zP1ma5GiTv_BDQP)c%KYb|b<8gvrNR`Ab{j;~eDNrAhZgJ<@ zV{r8*%xKXGhP{97@%F3;KH}Ln>*1b){P>(~7FMfV5iP)}*<$mUu4Fg1YvvI6nn>9C zSxeDL)*;&8*A2j3%8VdQ#ayy#z_7hEVyU> zinw!BBKk13As#p7i<5+Hcg*<1UA=n%gi_zaG{Xns@xpQNGmKBNi}0Q9h^^5<+)oPX zy53_ouqqMYAq`Qtn)l;Yqn8va(^GWRP(tSBXPL$Enjjx6`TLt{?hXwT2m?5@%%GCe z3#h6F9;h=v*Y8U?Ycb>|8no$-1=>WoeAKvs8Y&23+&MhPY7`k(p{ODL=+4_ShoQ9C z^7Np;hv)bny=QsPN=}IktA}%O({XNzFB#Q3aY!issb~4Jkt>VipmmU{_*3aw~UI@t?x%9(bZ=GIKe{E328WsEYvfuO#CB!aj%_eVdJdLv+t2P=W^J@YTuc0E z-?Ub7U0V8Cnu^9;q9F5>#0pxhw*SjLuJ2Xdh1}q>LbfHBLi=MoMwhu75b&qN$A;~~ z@R}Qo?w4(R;^#0?6kJ83Hc}d&00l>Ho`AUL*kJQa^D+6j4FwSu}~&#qKfbK`ry5AU1XXUNhzY`F4joMOh;%L<+tU zqR)8}XF0#F3u3OzmzJS%tzyK0Y(5g$2ot~WdXrB&15VD$Mas69K!&0BOC_xBLvGPC$MgJSU&3RRlRXrw&dQ~X{HOX{{`x{I8uX8Dz}6;_tsd8{4F5;iX`1a2Od znKHPsIt)JxZ%1$ci230Tan#WcMNSVe*NfyY;k0yx(Ze@Qvqf0IXTtDtJ`$b7Ua~TR zadxqnpGPU=Igv(0bkR6Dy+M4rAi!3FSz`AF4=X?3iWK(&f9i`S6HB0avuCICTe|M# zw@9iwEs+Nv;qKSSDh1(*)AYAH;N`3}10F&GFA04@DfL~UXn{dA2g?s0`?>if* zPfOL3h3~bPlN-fbm&NW|>L}Q|rKu1^*TR8>VN$3x-@n3Ymvqet2X@F@Y`EJ_AHd? zXj{wD3dCEE%B7-O7#)pNj)VP7o@fttA7?TX+@rBeeYDP6@qA?_%N*56xl>YSdiavq zu0Z+@+?4yl72-qv$6kv962{_d6!)IJEiXVv0zPwr7ln>_F1Ob)NFTs>Y6038&r?T?Qgd3ov=&Y6_cd+UH5cX%-pSjwWoA zL+7Nd4uLl3t24S%eEM;VB-FBJn+-`;Ey6ms8r?^ z$OPU2yEZ+4`cflVc(l`VKQEb=!@D!S-WWje*9VJiXM|HfwVfXK-a>51J}k3s%M!;M zjH{}V6+Y|`M{iOJ^4+H4u5S06&k#%W3H}R3nv5o|@2ljC4(SAe;}tr#eY}~$h5E~> zqP4J~iiI`iD5gHKP_~how-k)mprhFFo;j>+W%!`jw@6f`51GwQPonv-TPUDf$}=bl z&wV@(pt`$gY7967X(i;uC$a_^N-zm*X9|V(PHQsB2M4UyiRYFja+iqP?^iT8Q>DBK zN+*4Dq^THhFEcHAmUStOZZ(jHoo8_Izp2dM8aHhuGKuWZmL#J|kB(N{q>Y>x0}>uX zKEPzUgEf=vr(XDpPVW+_WA|Yx5q$&<7OP!!nEqmzsArzXnyhW474Zuh%v=!;EB^>d zt!s=wpZF_T%5VJ{Kv3dpI?9?nIz_{_k+=>*}+ z(nWej({xMLcqLi@4)ddtSsw}pc!KA(&X%5Q6!)~nbmM(}13E=ISRJN2K_8ziY$7!} zqAUxNx<+}G&j*}|bY5Hb$o!8rK#gwVEQk(zB{KJb)mC!PfSLq;C5lPiY9`}=(avu+a^90P1qTB z?Q-cUB3sbr9z1g&v{9zNV^-mhg@|g(q^WQ%Pl&RZa+x+5iixqz#mIw8Fy+69p5Mgzt5eqnH@-IEh8RPg((y$+*!BegPsx4_*>( zJBS@mIu2AMN z_WF}b!u${82H0(!A_I$$%u(h{F3FNalo?-J&Yg(#EJi+5-Jt;aQ*OZxMwVMUShGG9c~YG}vMDUQup?o-;i2wumyJ zS4?h%lu)P>hJOa59+25;!sIgkbBjom7AS3)7k%W=qrA3yqG%OUN(l9?3r-^&ZOhYj zEP_=wJx2!`k#Ocx5SylR03R6hk?=m>p39uvuD{W2pn>DxXxB(eH<_3`EA**uYyIJ- z@)Cvebn3_H>5uhCEf3|Pd5KC{rSx!-rip~@HjN$1DZb8$S5somnU_S047ohh^!l5R zYjom{F!{VN+^X($L#A7brw;gMCgJ|>fuSv2m;(Z`JDoMI4Xcm(TpK!0W)Et~mD~0^ z1$XJmB;mBmSre;5^6a^0n_6v4(9yVk5iWOG%yi#o(W|Z??16GC*@CXFeTlzzRZ;yN z`LkmM;+t1Ojh4wJFAmhPuVvLV%>H$9x|i40*cw@eyPYX!2&2keo=-a3HR-^N;2l@^ zk$&l5_hlX*rxC%~bWalxm5XUE)C+ajn<(1>#b0<4YLkU#JhXZd%%yM|lb&=PN^WOU z3!cb+qN26sNM-{+ph%6DwLO|;YH~6H8;k{5v5%z$x&}I<+?ZEIequf^+n|C(F?lzm z-6qJ+%viVIYzgFLSUq?Iey+KY_Z`T2AcVx_!`%B@AIDaBNAKqI_z;|>GKX_Dg7819 z1PL{9MGtLC&;s5SJ6+$yn#@B84j}caHAT01CAG-@o}bAm@1r*S*&`k57>x*mX?c;j zgmRjkFU>Wb(DJ{e8`(%9S^8~n-CIflkG4t zYhtW&G4h+Y|G!trPpoE};BImw?*8@l?sqM;UkWjq9&)QC?kZA3@{qDJwkRj?rnQPg=P?bUBN=Z!b>344Og6A zZ8K)RCh?nf`M)z=V>3LJ)j)$lyf}zzxwLA>{;`+tu>!f}fECpMaG`TDCf2g736$^d zaB4}KTgnFNindS0Dl|G_mb`O^9n1uM515kk#pr7(9lpLNnA2BQ?&B_RN}MD8H4)+7A{Bg zrJ;EKj}O@RA^n8w?CS?1mr8-*`|fp}44ZPbt^B}OH`r)Mm$03tH7nd}7pc>Vg%g#J zujp`vYVE`hD|#DYYf=AMhO^_NT#A``d3w<6Vu2 z9YXVNrYn1T3s<-C0BUzUUu3{A%Y^kc291;gB&)z@`cMnZi9MWyMiN6e$)p*?JX5K< z^(8{IzMR!zz2H=yhoV9(Ij0-u8^F}Ec~_*>1tHV9Y;rOrA^%C4S9#-ek{fyNEI6rc zUniO|xG}};D{5U`?t<O6G*KfH)DL37rnq>Q{T5PtgL>-3L3`?qn^HR!HyK#7i;YKT}^Ps<0$0 zuy2KDiL?R3oWQK%tj1}SzbZBpm#X(~H57uD!<*W<p}SGb@cClHT*atIG)Et6Z~idmn;Tfqs9o*bvTiTX-` z6BDWk?z##wHd-_~?IA7esrMFnIO*ZG;cOHsJ_;QS5|uj^4e=|w!}rVJsDDh9Mfn-S zuY^ITiMkSoYd&^RzMNczz@@`bK$A664ly~o6Ry{z)DZjJCPuRE+e7YKQh2#mi^L<`6U;M zndS0mf!?!hB>P^Ih7XKq&Nxg~yX=|s_=@al7Bf`3=&px!BT**zw7<-ihZrcA#c@tv z`G6}w?40FpQ3=l}U}0IdQhBovfO8iwBlAWPB*(!8$&C>Nb=un0`Yz2YK=GBr4Y?7$ zl$HQLod;L6TzZ*sxyo1LB)6{cIxu3MX8FMfM|&Y@Gzh5ekJ8kF03Fl@w7`ZWwBKw} zf^4V>Yt0g*l_85Jv^s}x_T_ybNs!NtcSA9cToU56O}})%u_?nprLL>sIq+R^`YWsY z&Vdj5f~XzwI=bMGw##tp2`-ASQv?k0T~^85ry!qT5_Mp%D?@I-8D^=ImhR9fv8+)z z7gJZ*V(G^2*b;RH|1%DB!n|Doh^Ti$D^2p5)(rR7vHJ3II2V&U%Rkh0j(MzabXEkI zF_CGXGN$up!{mB@nR&q>UCh>QZ})}Xg}|{-+3z^3XyKT3!a2m7=Qv+7?-}xIxIDq) zk;eU92|Llo8bv1hL6)fB??{~BF#%Nd>yvZ5H7{MNZ zu&f zNtg1rWU~jAoFK1aaacn4LhRFTbuM5 z-!BmvA7TBae*gNNp-PeotOps+MNSxPFs}MKqQ)lMycjTgQW(%5tkho|yKbJk%PD~7 zbJ`C8!{T1t3feSj8+mPtTbX@)EL+Zu@(3o_3p6+OCZ_B7ec>*J-3$raEJ!JLf#n1yw4~hu`8rEA zI23n@Q-4*Zuhg;*{IE?!X$^ug57M$|+Hpr6TvD(aFlw2I$Rp@rb>aJGy7jFZhRlgs z0ToYttj#)KEv?9k&ik_jrHupjIa}=eP21;bhea+tfv`K1IIRhk^#>2FrPv=sz{c?N zp%gIfuQa|#PzPXV>KgPEl*|R8AIBIR5GMIp&F*w(3EMfQ%XHBu;BJ1ZTF#JN@)=_^ z<}&*C9ysLotk`yYW8If(?AWgd+=w8|DtM?9CS+5TYnNZa$V;R0V7-FcUdyCZ?havx$K#xoB+`n zDAQf~)T@Tk-*!%NxjNtHVRF3X_^Y1R%7IPQ`%8xoAL>rSfh^ns26h*YbM{W|NLKvM zr)R>kPvxf${G88p=T51 zYucLIlkdzJ$?eOvN74G9!|x-M6U-*FR)bbD`04&)^1&!C;esAe4_gs(;|Rb6h+c9b z?~XYZr%Q^7&RtLoDpVT+M%SD&V?kpLZt1W7*E|Aq)_v-0Ia+7#Y*=!Fz-(np?`G=Z zJZb9o`*qjReja0v+b-;Z?#=~loP!>XR2Hr2V$YV z*GHkh_P!kIy-Y4*iLN6fN9K|ZEHK2YH;2{9X3j>ol@sSjU`N2LauJdac^gr?H~nBs zr;VMOa8wKOr2=bhWlMg2LRY*cRYj9#XP%q8>sHf2nJD2z#OBvm?aY8mh7!&?RKI18 zrF56SH1)Evq>t;0V{&q!AUTEIt@|~XV2;)3-USUckADuV&zUUpnt~4cT(3BmBAZ&r z7%33v6%cG6YJWWSUH*zC?)2wvnC02wo;Oh~64Rv@>-G}fr?A|n;sh4rOM_Y;U`cCu z-zMci^_!o!h4ljZH@ruH_7goxaE@nagD?AL!-|V_vZ51Z1dI5eh|2Af-j;{WBXX|# zZ^!#~C$xW=QAp%}at3%_GCQ`CvHOfW(e}k@cv0x*v$_X-dvF+y?006sU<Z>WS!W**54l*?-$DFtrLJZ2-8=e^zjjCUsC z05aO;ogfa<1~Rat4Bz~dicsy}CYTol>iq13?gW_Qsp@PMt2~BlzRRgQXR~_bGx>eD znC7B=xJ}*WPRB2G`t+3v`EjCzu8MQV7dC!!E)k!0y5&LM!|ol1y1Jx_ zn4DIf;#^OmIC3RqjL@U2TJq~;KUG<}bwnu=v=-;xZTG_KS){8?wYAfw!xEhs?;vQ7 z16$hmUGB(jnp!9(P*KgG_Xuiyj_B@Qgu|$RH)-#tUu;}N*lEn4RHXk>(E!wobxBaM3tr(|0f;iH1ExwVrZ$p=y~K zoA%#d0t;!n!HDDgVLtqRy8rdlsn`(M@OO}LILSE7H+;%4VGA;NF0j27Ezc&p+~|Jf z0Lv6ELBXx|j3{A+w#|AblEq446|r*6jlK^x83%lcwjCWTnseV*bo|3tsfAQ*dJSV! zH?AN}w!IY)KB8TR^|ZD2<)iv?jb_#UnR7TD8jI{HS}C65Mtu;XLNOYU`{ry)>50ia zp7?Q6{8H5=5`8S! z%vZXl7Jc{gO6vBHcf;Oh5n2><*_a(tK)Q+ZNd#ajfg0}nHrICu`0m;dwpnJm z?t}Cm|HOQB>Nxv&eiI-v3g6i_B$bt&RGX>FDKQm_9I<<6f<$#)`dwQlOVmc6M>&sn zwkjhp%QFF`NUh!cAvvWY8o{*FTmh3u-Q>SkLQka<`pP(2OAtX>qdL%>C z9wtiG@vDks4N14lnyi%c zg?fjaFso<^?DRh0cM(aP5aJ*`{*5JOoQduzw(og`C|5xx>8UI7_U$<$wnE1eL4#mk z9GpE)i5H;b#aH?)hFD}rwnIai!xK&k7Vt4!83|G(@B;dY#@ZLp;}$msp8QzG%5~_Y zy|63zI0`T=!}IKx1ruA#fz+y?Ke~^SHO+;i=4(qEhR` zt2}?G7^!M~hd{=LBV?mH2|FgWxv;JHT=~$OTGr$X$C3}@pK-QD{rCEcW=VD_`8r*v z-IXg~9J_^UKJXpqwcVK$D45pvoNF|1v>=goIWa}785T3~d7|^2Wq-=Y1j+85qoORt zw6f+&S!-F5$Owz)sJUE=1-7yqIUw(A;j-!UCDQw5qE?YAkzSM|0QSKftap;s@2?0g zpVte_$3{_)U=Zrki2WVYlhyr`$Xb>suyIhu$bhcuXVogzMjv#CNF{rSsbI&%Q&|dj z4nlh-P8wx2A?-g%AEG%LV$`|^ocwk>2rqVGvQ5@mah{9G|wCx1dZJb4xrO?S~<9&gxlCkfWNdLTbJfCDVkNpUX(Fo@A>fXAH{(7~h#T7zZ=>1q&X@xKnF3~!y zEZa~83OdWdo66V6!vH7}M4OIw@fix;Q7)H7J7|v0^V?CD4k;(KHY(B-q>E_m4M-eS6sAGsN<_$g@l@Q%g#=jPe)VF%I* zR5_K~)GQ&jcVyR*@!SFOt!pf=s+$oE7kn;iRAmft@nUeX2^g)H>{F zs)d9Ou}NxR+0<(QhQsqBSV3+YZ{uMYDq$hsX*kw;A_N zpbnrKK+y1Kq%DbLc&{yp=?>h@=)KdHXPzissRWA2ryi3Z_jP4^foa4@cCP+x8hnOS*+h@p{-T=@2$K4pr$3S<9YWg zV>-1mHNtugq0@;I;HR~Il*(JN`9WoP=nvbr@C4{n=!?&>{gQc@Gy;ZMu8PNV@dxtN zM$l0<%QqIKm61-r)n0fh4o@v)$YtI3AII&0TMh%+iz~}%lh)X5(bn!QC=;$pxvfvF zk$pt=Y_}LD-1Ei1+0h8xe6Ms1Wv#&D^!8d_OTyYOC4YTVzpz@oWk3aGE5kph8weeJ z`GHc~4*F>4zF0}%c`p|o?f&v(`lcm+pZ6=cr5)Qa?Su4$x}B;>&^Xege|GLpHu~-$ zJPP%X3(MYXaD^gsr8g>fR1T=T$%o9R&ox2mF{Q~5QH}d$VtFYzJ(@LqjydUVdeHWi z(gF7(7}Fd9p0!vY8IjFl6g!jisz7Yx*Q0xA6Grdaj-4io-;ChcJ83TQQ!&#eH~}OQRCV) z65~Y%-lPaGf`W`K_=d$B+y`jtF@*tz!}E#zzRpydY>rpzpB9tQjD~d0qKid<%6E;T zv&BD%E5`JfPcJb^S&L(`5_N^jMXKLFk5Si{vut^bYN=%kOs@#l?PX}79~IkHHhv(U$GeQvFKPl<;f3C1C`Kup`$h6hrm*SJhi8VZR)yK8 z-oOeW#OUb)vbiZEAse@&yT~vw1Z(Eu0n+0hbqZsAOMXEO%{gbbpt=$xs?VwDoK<)Y z6J_Z9fwR$rqq>nJ*;(6MvI#EPqf$$y+PeF)xZ6!UAH&G37ZRP6mh96qwr)`I0}TwK zHxJM;@sG)91OJn#&9330$8Vkh5LKDnCUmq8uvWIQZ1YX`OgEAj3CfB`6&h^l^b!aO zq=Pc)4|8%igB!+pjozE+#}QrihUg2T+X?xMO&{6xG97pe>}#eTz?%($LH+?<^L*OD zoZy?mO3U#iVPhD>OX$VqnK%5>#TnC{FfSgONGX3XWinp1{O{e@|3199&5y0qBBBcl zSCp$n^7-aes6Q{(>2UE5$;#^M1iq**YYwz4p8wnhpbt@5O1VyjQ&= zlqLt4lW8dOghH{0gI8{aOrNmOdD{XhO!$bKueHc-Au27oDloPS( zE@?o^+KBxPGZ7BMdSLq1cb$vM#-GG9(pAfj4bnXv^}wAHuU0fpmrKi>O~fK?Zz7cR zkMDIIN)z1&uL5jdF@Wtm%6B>x*k^Khwa*{zg~YKlGt#!elD5zI?={f>fc*!XDck=( zuj%rzjd6mLl6ic=2vCgL%8!u8DxvjyyEgOn4pjwi3ovFuLjYZYI;(#J^3pQsdV$EwYU-tPp-$ao3G z&(18r*or9--7E{vzO%2$scH(UZi3F_S4#;>pJ-(j7}wmQu(ZC%F))jZNQO7@AwhS}m8L5;~&6~c2Sm@to!pOD^? zp838dKRs3~BKzHH#eDiGQtLzq_gQt&En$W(3O*gQmVcSB9jJ2~;2%Z!6hg$85$!$R zHt)~In7|OdueZPiQeI@5Sko2K$#KMrqLl)l8eB0|k&symiY4q&a$q_WwF-2&H-fhY z`Q{YVQOPY=>t53IB!7Rq<_88FDLGyv8S8R*wpc)UM(Pq~(Jy^d0Zs!?MM9(8sfwW4 z?FX=5L+qb2nIA^b6I#U%ZCG_Co-i-6=bAV6Bi?YXC17d^R0x;&GGMIb6M+pRs_5E> zPLZ`l+ctF7t}2(>wsmpy4U>DWh=)cyblyy%Ge++aXw2ZteUksSsp((dg!$BPNoEDA zv9N;zX5U9^*L?(`588H+9$>@djn5XT z4T5!Z+No&Rrd|T1xixuQGMCy&VtTP_Nwm~-=LTl(Ef%v~B;9IVy=;1kp-y5%Y#M<^ z)Y{SCQuN?~imJw$cssXq+>Pr6BrZrBzQs{UZSm9 zwPA%$+ru_lHbbr=c86(>_(8$Ss`?SI4%|1sMIEDAByF2tT{U~BF$AL?5xuKx=9M?+ zonRaW^f%jPW?<^AwTs-@+~rP!9H?`^`8S%=rl#*@ZnJ%9aL zeb4G<>)g7;3N=p1uiBE{8=!CrmX_Q4G)c{-7rZymG~sXkS^P4k(qaO;t$U<@I9G(n zY9Jb^$1z(o68M)<)TlY3|L=V)OIF{ok!VcTjw3|H9y>j4y|8RLZgFcOm&jPh+_HjH z#ET_B(a{+bksO;O?3$1H_0XlqU%#7s-TZPz+>BD#Mu1k0>apk2pc3)U3y*eE%xYNm z$RXWdSw(a71m!{# z5I)1nEM%0s-50DiDC(Ls3T&1AM(9N&)l6k{qS;7P>`THLVE74Y3IiMy_q~*Lo6{V$ zPN0=@a?JV52G^<$r~I@XxoQcC!VbgkLKeuht@;l zGMST_<^y7~7q5nu!^&fKMToC~yVNh&%u?Mfn0z$f2BQuH*NFx({E9X~!bob7t^BEV z={0Z@N1xv*S~v?6+%KjF*crpQeQ|yM7tb$dg%Hh@G0~=$TM~aD`b9Or^p9B4ymnpV z2JsoQbpsD>d-RLjG^Mu(UN9Z&8$uhfzP(Oj!}`qN<@1khEH;mrW!7ib=ZepymH~;M zpq4#NAE;sCeG~ccF?j*ep_^|PrVp}#nCCIzm}4IZ?&-0W33Xj3ncesxAlGkNpvQkN zT>XTS@3XjX%vTN1u^}jhF}AEI*WG-&_CVaB4XB?t)E-IZB_s$(*A)IZnSpv#Yb-R4 zv4xnhb4{klrp$>e(=puUu1JauU-Y<4Wbt%`T2=mgykEtMcwpcZed>_RGli2fgEl7e zD`IG`JUm=eYRc}YBU7b@n!SksMsPIP75vIswp7zckWg}G$STDp93@V!ayf3fVLj>a zLfiX6-fT_SOy-wsl(GBu*uV^JrN9=X5>II3n9S#ZsEQ>`UQ1Rao3rU#U@i6`X|7p8 zbixZpd2W5KUj{)Pfv}%PIKxVDc&z}Ft^$n#NO9{tXS|Rt6x8xN>fM|g&@ZuANYM%- zv0Z2PEt_9VnF&9yg1M%pgozK}_Q{H$sY->rn<}g~8#|PY-g_jaB79aIB{Z(y6Fzkk zB55*wXqQ*GXeVqH*%ggfAGWBzbQGEgN-ewMIEl6?gFXQm;6N>Q=j!-aUczNe9wt{Z zuapCQMNpUszMOqT)||L3C;OBKn(+DZ-#G?6aciE{C{ zD)CTb+nUaG26lOpt^B;rnP2Xjjg87H|NZEwsLi`KXC94s836VW=MD%pvE8~07ru^O zIL|-H>4p6}L3qp>=1HM_W6RjiAsgW5K=Eb+#OvYQ+IyNBMMtZmJ*Mg1>FrZPdLJqY z>fLL&c1l4Ga7%yXEFU&pTue?U#;P{ql&Tnx;l(8y(kp(sxM$TmgM%k~N6-8At?S#} zA0B$zhB#*^Lu_bCXgj%bdtkct?B}m@59Th><)Y9(AztM#FZ?xkAvi1uI`Ol~!l~gt zyHabH2CTZ(z8S`Y7}IQ2OBX7bt{Eou3I>i}?Y*J=EZe~jI9v!!t;2oYUbR}9QiS8Ud{B{uAe zY||z7ZaWEUyT*A%C$ZR_Shdw$DwhH})nF2l(_`123yb*_$x$}=_o{^wof;?cM7pia z+>6p#*hr)P2zFZ6R05GSt`7{SjE5>QY;b*vW#Pj`bROJ!VZ+)t;V z;5&l8($;DSZ|-SAo9=6XBAa#O_Yo|###`|M!5{=cj z5L@$hp!(iBu^J5QhxC5;0tZiCBHv<5kmn!gmJ<|~I7L|O20~0i#96|&l`FGXNgk#% zzJGQ!e70FiXq*Za=i_h1%nBD7*%;s^RRNAx1$MpF9?|-b&#beOx<1!4} zkc7ic)&hs`dFmetH@l|6SE8l{w&j%qo*-F|x`pTGG4t9JJ2Kg)^IhXm-Tmo?pC)L9^aDYZQRT=ytifKJofXbmNyfDbzu zURbceQ=|Roi68REmN-t#zm7xxNg_+*`%Dkd)bnxzb4yu;%nS}vs5@shtaka~tr@Wd zCetiu^4DY}reH8M*4Ks$pRc&8IDpPej(@rFA)+pB{aEy2j5)0dtWlYGVh8I4a!n!$PnGm^{7?pb5qyEuMYzgp3_X*+Xpm}lKjNtMXDy@I&FUoZ76 z{fb2Ali7Dp?;YeQE@4UHw*Mu}Gd(#uOiG0+XaX7Bj@xrJ<<5@^=h)`vt(@nEVZ#>8N+Td zWRW0G9L&Rnhg*?Q<<*6%+fW{B4a|Lcd(Z55`#ozPHaF*iiSR)TRw?lO_O98c{gBvd zUfd--Lc-kZzrN0eQbCUhV+D$dT$HHh`j%hnbK7S7dg{F(8i$-Y4y8YdX+=Inc2?kV zaX6&neTr3U1zGNcov`L&rm7K=EZHrkMiyPb<3B8>1lUc}?`CXQUoO2w+fuU@y}K8O zxj2YspK?I+F1=YX>Dq^Bbv*!B?w2j~*pCbLP}v^DcFjg!_q;KD!+RvD1(77|vkOTk zs6}wL%A+HQ#7>ch?iTo78%<mFZVzXhQb@dKq-dS|8C^}*ID3|~lLLv)E%woDCb*sGw zNF&gSwO`*@U6@{gKE07r3xd6w+~Hi#=AP@XahNOMpKq1}Q{g<*Y;g|dJlnN){7;aM zbuqegSxiv1K$N-z(k2*axkwk>XBcADiY6d#I^H{3E+2%lj$PciR^c#yP2Y{sR>`gX zY59Q-*s=@0@p~_%%tp*M%F`p<6^RdQ|AyX9+hZ7hLQylA3!FXK3x@3&(ZkQbZ2kJ{ zNG>4D)XXLdm*^fp^1^fvkK5iv?SCc?E@0D<@7r45zwzK?iT~SiyJFNionI>JbHi^< z#yf`Kl|;n{xUJ>Xi=KZMxp-*QQ0QzBSPeoSyArOuB@*VNi63MPe8cp{V7L zKnfKM3H**<3mDBgeey^RPgE_Rm8fGazHr05*fRW&;e9DbrLX=RIDANUJx=4)K*h1O zyvs`edbkf)lwipe8p*#y{hSSQASP(msq z2Y{2D?20?ZgXri!D+&4pRYL(;Ikteze*PlN3!5k;8&qR{O^1hY{8PqQCT!NCKj(irB3h9FB%DxIDC68)Xj2oE^d*rd#Kot!HLifJyz=a z{k-WQ9j5Pqp0$FR!3|l3e!2&pJdKj!KNKlqRe?OwE#M^?RG4PPGP;6EWRfM5#qPiR z%wPvz38q@5HXhC*;xtm_S|~%e`3#STAcj?oAQ*Gh%8$Srz0j?hkv7~zoR}VtFH+2w zlS-LAswDQDg8j=qI~l^fYJX5Y+fhw4yKPc z^n(BhkG~@vn%D~};u4A}d-ep^Dg*~)h6d)0(0|C_N}o2^pQvl-Cdbk?cuv!M@?ijJ z=MpoV8@*L*yzKT8`tz!7m}5=fWmY7~!$(0}G3k^f>tiJ)Cs4+$iE4Xi2S~yuzIE^i zDXuwqJscyED2>!zN8gg3YN?U($(E%3ToYg-7;_JVc!LO`L()5N6?mHh(CV{WF}Q9y=R`iNZ~J4kQy)TwAAcAKhqsG|#O5y5 z#b#(32opL>l%XlHg~(Uz$Wq-Vb!}VO1qVoka);pS*b5kU$={vT@G`bN~3Y+9FOmKE3yG(D2Ea>R?!Bx2jMmGj4wV7yx4^EUDRJcz0HX~I6dUl zr@1=aB7YhTP&li~O-j(WL30B^tSr5vhdKlT<`^g!1&FlXKFo3V8JFW@UzhDau=vI$ z=qkC#0|K$rN;dXV#M-`lh4{8SX<$Ey_N@96>O~&cyhh2d9g$;_*{dvN1fm*-+#q;q zzrkkLXXIohVI{E(I?r>5Gly@2*8r}p(620mc}$u~mWFAQg9w^Z40e5lI0|-t;Dg?R z7Jx&{=zkx0EZt(@F;;~+fOi*F2tI;*X+lpe2!FC4R{LGp_k?os&n0R^3b7Q|6W-_W z3ZCjprLyA=wl`^s``se*byj`(f9QJCc&OjEeY`AXYq3P4tTT+58AGH{#%{(=mNbUO z9zxVuswre&#xR3Eq%4srOZMd>*=C|7vZX8|gc>0!{jT}m_k;ic^>x1=doZr$ypHoc zkMlTEJj{OFfIx;mvf{Y%3X7Bk?TMvIM)rzk8>MG#GSkrQ>v?` zr@R9Shps`TU@jiiS{gO?9R6=s2^u7^ry8I`{VbEcFh`z(s1Us>b8C`Q{}|05do=du z07^zls#*^Ok7R%qwv`M(1$A#}z_)( z#|qCJdlw8!gRNjLQJyU@1}qKdi2xa5%zG{e+km3^G5$5Sbh|qU7rJ6$2Tl8iOe3&3 zvu3l-#T@k)B}$0*+&I*Q%!go&8M`zUE<_W|;k>oao5><~3=fZKiph`L#d8&>AqD{mB40lg%z z5%UqycJEkm*gX87T+25sNXELf%tV;#tQo?@YO)(j%|mCN^id*#{s)*9SF%5%qz5DU zpaG>Z8jMG*@ootXDYK@eJ zae^=#2>-rZR>)yn$k+Lh=TCoD^cyV@*A9Wv_a)s5ho~?%fx*dVa22>2Z9KkXZJ)h3 z{ZtkH9^zt80f-Wif!R1g3&u|<4ZwMo!7c|uUj9O`;p3>S+wpA=!8yf5HYtII zr%XI8g}uV;UjCTliPJznC`vP!7c24C=JF-8^JdZ@e1%)jTsGtIz1U)AV|Ln1oezkF zC}Bh8sO2^B5j!kuTYVy;^+TI4^gCd=ux*Gyj-hIA`X6N@t+FthNM?;)XpyN|0dIue zDc>0vXGrnb6Zk49bXq?td;2D8&uzvoiDXXntQnlAR>c+h-9ZGL+xS2S2q5*!3)%Ai z&%!2INradKt`$EI*0k*lK_^GWMb=h$@iEFq)_U4%DLQmBtCP}v|8&s9RoRM)X)%|p zBx=@w23Lpqd9lJ&wD7kNH>g0I==*$Kz|4hEpUEyT+;=892&jF$z_|FH1O10$#X`SYR6vWyn73AS z#BOvF4!H0@>y+|aE#8MC1Q=MfHojGosFNB*dLQ*SvBny3s z;qL_SLUBLD3C7lQ0BlKGj!@Z{aeiq_nPg_PeUp7CjObKiaU7T%o|=W6hyAyhK|u{q zZ>9^d;?Na5a%=mrU^G}vQ{U}}%Ln~k(|O}N6$_@u+133xu%5|4=IciKvHfa*EDt+n zo!aQ}WYOBj8nkDU$*JUIa$FguCJ0g6|IyEBNnYE&pw$2U^0;oQd(~iLB6wd*f&ZNI z@X;J0TG`JbUFQ zNHnOKRa-{LjR=mlJ`@&;m;DiO(FNUo2!x0VEs!?0%x3HX<0uJ6fZ*1f)ov%|--|VW zEa#&*WbXoMd+|b z=mChlA zJW%kbkF!_9Xx77njZB60+nU&^Ajnn_R1+3J`kGn?i)R6=2=r-bK)B)&K-1^SP_`!W zWzIJ4)`ezfZYVXgvgV;ypwzieYaUXb2egduh}vW@?)o6i6kpa<-qe|@ETbvw+K+XoKkx+<~xtz3v5gJ|0tjhQ`#j#MQ9bR3v{#2Z4}8r z-gw*Un>q&Fat8I=)WRnf3fG?QNjv`Ex0H~P<^lr>l%vDywc1hJ&$n{6GKSm8A_H8CH1q-4yiZ?6L;N`gukjh}QVr?dmMQf3ihj*+dF z!D0b3yI^205C#lBzH_|mY6BCvwV3(zX1mPEYu3JeISbkcQv_Q^sZl_TGAz>QmIYy1F zoaR{jT0H>2Enfth#FMWe%I5g8U#ujx{%`Yf7M~uyc)VQ?6}CC@c8ZR*t=P-#sM2e{gOnktc1Czu1tAYK}&2rP;j;g~T1R&RQ zz{IsZPy&FHd@4aBrBEz8C);J@ZYNid?k;Ci&!mcwn0D0^GE|&!-p;OAuEsSkicl29 z>Ww(6`cQhWlQ@-Q-u|rZtOs)Pu*R+0a`3KR-h zg@ZDsLien4mH43Bd~!AUigp)bkwMu&%07b1SyiRD(Kd&#`%AkESbaQy>a30y{M}cj z2O`b+U>mg6rRR^T<3Z24$$-Na5JTf`UI6WSE%ap1dw8uwl|6?oUFexmHt6P*=eM{l z)uwiB3S3g#BeKg4fk5P)_~hg*&Crj{kHkEFj5Q7(#}Yi0po)o@^Df7>r<~s}p9?$0 z!$21NwHOrq6(}tM>fA3fPm!Y+!=5@d+lQ9+0gcUAu3|+j$inN@t02fzZ_@yyrb!df z^G4TQCO?NBAU`8#7c6n_3XkOr`!em&59M>C$L!MM68dTgN+aax~8$&9nnwe}dltDRZI7vqg?8@k6I+qlalmrd-8w0FxTQiKQbz zO7Eu`COM+?#S?#pyc-Xt{@QqB8NwE)|Fv8cZVcPgbN|2N1*D+nm6Vun=I4w`7WM2f~gQO#QubSjYJBB@bZQ0a~$t4c_%ckz@bAkV$X2dy0` zep~zOoClA6?QgE`tup;@q)R?5Bl27?hN6$}?>jVei+;}4GhiLlQB(^$n^p1zvF zwfZP~p%CDL-f9lC3fBNvK=jyt+nKPj(|Kj`nVsw|XVekT#n9c+jSCuGsLWei`p}xv zJaNlY9PC<|svMYoR5d3Q^I+wfIGdPoOnHblsERB41XR?q7Yrs5#Mq5YkNGSgAZ@I&!a3&u= zJB-n@ck}HE0QF<|C^8l_+^vhIkL>#S}aT3JswuBTtk} zYAa{K0)lsZ5;SC{ft~BmPddAUexQ9-2spbXvNj+cr(LaC?6b=BD4xL^b5jax+0zcoc7J!q0*n0|BNXX{`v+2o*QitSZux8mxsu1P5ezF@9gv+SfT!HqXHtX2Ut&VLI(qXF>TqU!!PFnJ zkj~9GtK2@0GmP#g?bUFv!N)MduBOb8^hLPHl7O$nfvG=JXrK!DC*rs*+1{VpUdSi>;KwdlS`zL4KPy?4 z3fh!TE+mkSaW~A8qj-4%12>)e(2C?!$iwJUQhkI{{ju#T^spNa>hNZ{ph>>9NdJ!r zr58}RIqK^!nXC`SAC`wQU8-!~ra1=thJ@MgCcTg;h{o+F>;Mt;HVIloIrC2}uK z@;`^u+iqd3)`aNvxE-6BDTC&yRs`V9z|O=d4uUj1?|#8&NbyFh5{ewhUg0z)7{A<5 z@!)i7+xFiE=X<}WIJ^fX>~X-&3CK88W1&#UA&3e@0Rpd8PJg^UT;wv@`aWu02^M1t z8jIx2u>mD@MB|r-9KoO#(C^P0Y@{r?KvnHCOUtYZE|)#D^?!HD@ksHD>fT6sZ%`C@ z^Xbp-$Ks%jo{ebqn2q4kn(eXJm14{F#k?r4Vz$_?xKKtAFK=JHOXT;~r79U@^6egh z$-0CDd4NLO>I?b$9`e+d%1iE){QaSR=2t4Ww{?@5@=j!@ly>bQopyfgk}o`{6h0x8 z0<6V0-Bp!N+BT(M-2Bd#stF8SU2gzFM{eS;Qc7p|?d{kKgmNRNX}*9RZ4XpwhqlTk z8bEwd0xlbV`&PyGh?xIR;AH*vT0PhPwXy7_zYFi)J1SS`_AQ(6*N-u3k5FVIenWv(67LVZ1nOGB`{TN{@E8|qvE|NxlSM1?E<>g=~@%Oc!FvYy! zl4WVqmxxEXT8m+tQM3NI3&WHHJ&qUhs=|jqJ#0Za zjCAsXIb{{}h!Dm3F+6Ijq#2V92uDT{50-};3R!vHK3mNg>HVE&WAqU?=dL%Y2|PMo zaK zM_llL?W4!#DUZrkLHhZvad}=R)5fOGhrHqPQ1u=e5{U|E?dQkHWMs|3E^HEQ(G$SA-S8^*pNt=hA?`!ke={wrn&P6Ecp*!Mj zEKYd2CA1SxVCA9N1rIXQ@4b$^-DNn;=tkzGZU{IlLy;=V?9U7c#E32*ha!`0-pZR^ z926=T%qz!Kh^zaE-)VMH3noC}9y;_^5n{G5tq0>~YZwDpNYoQfQ<2G{?o|<@rFRas zE>*;B)LAJNq$wX+&f!yS9yp*o9y}H|-u##%fH8OM_~&GI>9mjm?nIe*M)wZn(Ilzp z(I4R5>gQEr;>o=4Bi!PWG~X%^LvXxMA;Y02k`4Uzo+i0)dESrd_42lm4{@_GvJhoP z^9m2GzDFi?z0K20e8p89!~P88-8kMoYys!US}8}-hu>M&ZcHMb3X*!2Z5uAnyn4p- z@b90rNe$1ds)$0i;D;LuQqcq5^9^~oD2MqN{I7*+lZNiTw}guh|NTQr2s2l`yL#gh zH4EpmFFR|zEVjviZK)yT)oFyvADd619oeUZB@8oYvZzgjc&3L3-zocdlMfp_UW?Kw znHjH5M}kODo&ASS{2-Hwnnl}M9YSfD*9=Rp-8a&2_Ka_r=eOm8N}n~vcL3<4%>NFV z6aRm02Eo*?AYMeTDJ#WD5uSL2=C)B((mWwIh&m}{$ukF$M(Eo)m0Dx9Zj>rcqdNJq z1xHCRmQ4mN>eu6`TMqi$60m*Ct0h|ABV1A8m#Fb{i0yWSeA6JEe!qPv8ZuEAl+aY~ zlc1y=6QPOipZVHoN{s6)r)EE@X0c9TnDujOUiFK_IdeV+kV*^}kEa-flRZrLDU6ai zil*YQRVsqnCME_@_l$wQ391S9yt6D)uz>PdPI)hhsJgYCn7j1@7^9irKW+V|;=oJA zr2pgJd_yqByzMki3R5|rOrG<|*F7-b%IF2R#*>9QOe4pO^J7scdpGgBSFiZTmzvPi zv`M_F1DL6Ivye^>HE?qR-@JdbgUdm0Z~MTUu7h18$kW5)wVp(}=d~NS$SS9&6x2|l zIx^4O1u$LdG#>0H$MKAy#C8e;bEdfRVjaBlilnHrBi@M?`!t)FvX^^fQ#|lkh-sHlg zHGT^`-h(KE3~CxL!^F9QH;c&LGMSuu0qZ$Pv`|$ok4GUogTvMvi@U>%J+i!7T+|cF z4$fj!S%jv80H#!unF5zu(!<~3>BHofFiFp@BxxScS^gM#B7!N0>yiO6?Hjpxg@G+k5 zJ#K>^94bj`kJgE}MugEiuiW0Kdw0y_-uEN#_J^ls_0TWz8#l_4nhQI(Tcuyx9HB62EFVQ$#{D^ z;q6X<^Ih{g=ImSz=|o}C?dIsKDP~;I=xNqMl$zR&KQA7IENN-fk!mRGb2qSp?!z zBLM7g(&k&YS3<<#F5k9g5HI(gr;r0D8{hLYtlTfi-=coSs7c{$EiCTlvu0OM5P^TO ztZTSM7_-ncBl50PbTPrpccv5BDEEIYgs_54&K{owHGV#U2KX)~+H=MgX~gzM=*fEr zY-2onin$Of`oV)$B1D*a8Wrt#ip4v3d9OOj$;ut9nvJ_}o3(@9_C@};JD$#Wqc{WB zq~Q)0QeWuLuXl6^;tui^G)KB`UQ2}{Zq4))#S7Y_H~*d(k-Hg7O}N!7EFnd^TQ&bwWct zS&fcA@4q-c9kgH0IK{3`!pK-JVxNjAzzP5PAYi;EapRyc&FHx|i%Wg$ZI1WIU|uv! z2<C$eUK5;o^mGLTsQF$zdD7lT{GInk(HoZnaek8+PkummSe!cUeGBpQ z-+o=qrpqsX{oI0&0Bkb{Kv&fuAJc7R8={@o`uM;eln|Ou-0(M+Vqa1euTB*Z2qc;oD*7=#dF3{BbR3iLxUi*-| z$K}W3v}3Q+X*tnJ-oQ5y>(1ozbjYH8p2A(+IZ)C1`Al8YstOKw!Tih2g@I#VU)Uem zWCaJVKh`FS^Pzoe1qcF^3@D9BLB^Sv0gFGXE^{q@{b8?AlO& zX5Jr(topmTz7sDRV*bbA*8^JY`KKiL!q8A z`O6sseZb}7SA*)P1#}L$c(xoibS8WBs`~7365LR((C*7;*W9ZN@Ukks{RZ9~;uNxB zuXfID{1xSD8`No;o2k!ZPIEIq%b%otwfc0X0Y~~eaKq)8QN*L&;xA)GUm2la^GAQo zjHVtFrm`FlMe1cx5BJfH!`E5S9fKJew1Va<(#U%szv#IKM4M{P@%I&1A*ssWf81n> zk8HmV>{rN2KW>k|14S0(g1$;%QBhodw7pu-x8qC=hQ#9B+~*wC6)QHtoBWeoT)(Yp z=`W3Ndlf@kd20M_>!cxG7NG-R(2vQi)x|S z&5_W(rLYlV`1Pmr6*{3S-?BWiJ0}`E)3-moQ{dynG9MbCWD&3fw-mRw>m#?SsI;s#mr>FKB&`ZrFb=f%FYezO}=_8ckM?E45ys26yv zd(yjOhMjgr6vSh|dP;MwrRVAJ^E09yalpCxgZWAWVJos~;`D-1oyQyLI**@M>pVUn zQ%(Acy`_=4`WP-xX@otgG@hR3h~s1!KKnAl zge~Cq@?bDoxPCm$d)me-hJYdd{!Vc;+NXXIzw&YNYA!gk5+>N? z)xw4z7pmLv+>5Q3fI&Pr?KNT^eEq2f_0W~mX zb2@D++@1juDUSFKFDaMB*6$O(n}ZE8t+_-+V4HK*b9FK4(fEb>%i|7>m;A(*C(b&^Rg2eSNK(^)7V;r6No;Onl_HM6{F0BV zcJN~hA!y+b#uE9lZ%^zBrmOwP@pK_BsCPGq`OTVBW1%%-InR;=i7>ix#KtwteJs}7 z+`tWvHQy;9z-F2MYn{Yt8bsn#K?$i-l;4$3^f8YQtjA1+Ij2?=Y7RiCit+o4ZoaiaT=MSrd>rnm>XC9vYP@gJD=%4BP0xqlcZF9TGet| zSp2Bk^Bx*!1s-}1;hRg-(*o&~{1o}(D+6M*^`*So7Au@hY(G&^&X}Ova6Ohv`^KF8 zbzzRZ|bwbuWs|v44Ses^_7Ek^9(hsF%2pvqmFLVucRx@CNwm&=fv!B`I zmh!YM550#(gnOi5m_q~@S~%OR;TJHFTm+uzNdwIW?x??z5O202?A@#HyB@o_`0ZN8 z7+v?SXy#hJ@xV0gg;7(=hO}sAvY@A{EK-8tik>pT8BMbq_Hs_-3ZaO-C^8j?KPbWc zE~J*0jq~0YEfjKA`C>7qC+k5kJuc6+Jm5G<+>_A7Aq=&+){?QRzpZjsZEVUU#9L{J zjHO$-#6CL^Z_0!mh{zk@Wo+IrV{kEZt{(`HM(X=)Wn(ZboSf{f)qjhKzjx+xqOUC& zdMd&>NwDbbe{My3WN$CNt6eF*-yXU4*$&?^H0(4t?Khd01x=Pil#4dL*kN^<}vo7-TRb>Aj zli8wu+a{Out=wZ-z>3_>_JnTY8qhzxj0nok}nV|Inv(DQ&x%w_MP1 zL-#(i|MO-6ZYIV{EWD>-?ft66qL~$nnL{&^k6;u0m zxg6&j7`#~bm}T7C>XA$R861qqVb9~ivyZmrDrUsTH#Y;35*>mY`BhvyhzqBIAXr+w zabKq(ZSZ%=Vf6{UELmNrJIxB&SsdHJQtd8T&7Nlu+)m2|X1YO@xBe_m`nhg9U!M#e zX&4V^di_$H)*ha(8x<)h)z>#_BwDI*;lPY@!LCT5f}{k;P^fppBF*D!AQZYUd&NyE z4COs-Wz{IuLq?-i2bc^(TBW5P_;0jm6+CDcnDxkO7xBr?$c*`Nm;?L~uUzGY#yK&> zaLDiai>+>VjWaW2tN{3;gLSq@Fr+t%1c}qZ>Ae$&`Rw1#Y*Yvdac#YN^~LIjx!6@M z=GgT^K}%C=@St25l5e{S|*v*Ivj17Oj$Vg_`oCit=GG{#%rL;rl3 zKB3$2K4i6EJr9eoa3wz*dV29frUJ6}h5=p|)J<^!e(X&b=L3sO4mIV>!!&5A!o;a? zCIK(2=JC;N<5802Rz0wO&X1305RUWqZ4WZ`rxkDrp(2y(<(t3>ru@rUv|xhm8wTo# ztU%!q+|IAczl{;E{ctnfP#0kOoCjQ!b?%M?M$Sk~f?C_}X4)g;)C=8ZZZ0QxKimy4 zMR?3x-h{#{0?XPtB0l_u)~3cn0Ecl!gO@D^i~krIl0|~e%@7Asiemc_2+Uy`;-Ey< zJ*bKf>4ONV;pwUQf9(iG`|b#_@i5YDqyp>S^_XRIm;@851_}jpOW|3aU&clndzYp@ z6$h>?7%jKFP5yX2L*mvmYHs)sQB8>3aPPQUkm=>Tu1*lP4->c071sUZDgEn3(>?T9 z4z+Wf8C$RgQMfEhUX+$+f{%?xi_A7looiD>@MoZ|3(_p}$LHqe0BzS(D8$PjihOd? zOXDyNxKTIfud<$ZpIBqrocBa&LQ%q|#DzgfzDKVYOw+43OdcwEHjHtXp?&R3ni!3z z2Vh}O@k~mz9FpjIcScTz6uQAJudo&v%M!ANZIjn|35i)$Y7=n6^<=R|ZZ40Yz&~j_ z3mGIsII2Njv2zM(T&#fu$NIsp$GRVr!4eYDD}5VcGoT$#g^Y7nY%DvSiK6&(e-H`2 zIB-w_IirjPRavSbZo^Csp-kXXIThA}HfD(~-0{xkbwkTERim|&a_tedK?Ye1y&dILL zgQ2+}&oU6VHc3UinBPKsVM45y5~Zk;t2yg&ale0GK6#8g9=6@p3Bnr;1W=I6_Hi-Q z=EE9Jme_#D!0ls%_gyjCGX(|JBGp5$BfN*#I`|PkznXG+o;HR`>VywruvN${0UJRg zd%+R>`Xq5PECJFXNb2=EOzWY)&ehy5f+F;L%4S0tmwSb1u4>Wc)+wo<5A!A@|GwHF`U74~u1v40V;e0VJ_o)#K}Zbuhtdej#9K z^z?W{kpIQ4ru?c2pMka@LUE+~L;`EH~!zDBONG8sHXaME_1ssZzrnop~?Hb7(uF4(Dguv z&|=Gf%Q4Ze%J7%{QwXM6^=Km^%R`8Q8dJt zsY3yXdfxeU#PZLjvvR*xd^=Fl<@BnxktT7msV_#C8r~+~YNqD0hE(!cC4r&Ud2N%n z-`xWp*0XfQU&Ueip6e4~X;HlD-$<`lrI;^TZ{5%%@e(jL)5%st1|BkpX}9YI;vt|v z!Rzuwj5dEg!T^-V1RzjQx`G}EO{2*o(^Gh`H?+l<2&g+e*uLKMTpm-Dq@j1fSPje) zPfq9M!4}GOmqbTeqO$bPq;zNJsuFCgC`lF%_ND=x+&#MAry+dZw{12mi*2I|`BXO% zjNv4enli3R6wPXWyzyu%wv%we+<)Sj(ZyXWmW+OVAOpwK3Gsedb=V(geHBEk>THsjkxf?x*Z!Iw)nE~2xdv{YrNp)eNNLlF+)uO>Aoxuzcd?DJlkAv*`D zam4fG^oWqOqQXZ+Xw-JT;!%sW0jyEY`USD@@F1;U>A4e~3IPGqa2XLQ|J3k^ z%FT#2tjl1Uf++Pc2J^rr7i&BVfsBPpNz9IZD=i4v_KKZ9HMf;voE2eUF!=9%6zg1T z6~o?gg&J_{TEhT?YZPHH>!E~v_Z6dSS$zWZ;@12JKi%_CME*-n&!1Y(Yq@Tj*J*%@ zb)o^{P_RdaLSs(OF@*HOK?~!cDkXgqtfv{=Q?nQB3Ske80zKDbz#I^<$kSfX!zUye z5c*uQK)DP#*Mg2~7x+vgIB+}+2KD0;DZNp9r^)+t!9H}rq;9>G`1pxh88Nw}O>mA~ z0!W*&;GEJ)h_o>~k?Qo)`~Seu-(j#NlzbZX&DzfFx$}u!PO>ilOjX-|7&>~~5ihEA zJhd*dor?>~vgw>lM9ag}4BYpj)H0~Da8BYV80H*)|CN6Aq6J8F?P3U}vz;EcEFnH# z#*wO2=t&7V3W$QlwZ&>=Q+)gVYnD;NkZBa7QQ(yaEi}H~g_W`c8jr<&uVPXss@t#s z$+Gd6K5NUxTueQ*(aA!I&_G|&%VU;Wm$e0+%w#Kk6#`W6dcF#91I;e2gd?=^5#=!c z`Pbj9Hj?Wnp1G4;y;4k3NEo-V1=)kBDqeklUoj;^PU|v>_nLt5O}?CV!mZgNMm(>q zHX!sp?Py4&D3ge4`lQ~5^vc`y29A8Mco<#sP-)~STYJp!%Vn34-2ugOpRD?3O=D@q zf>A|$pEi81Ex?>qYDQ1v;1I4^opZJ}!TCzV5s>?<@9F=vtzX!5>{#OsJSdKoPTeP~ z2Abk9sX%2;H7=)8Q*6NjZcjxe{f&H`TE97 zlw&%JvO>kFiQTN=S55oNrdgcV9XtjP9j1~)#qX*cI9F`F{x`tCqtQ~JluAw25TWsy z$SXS05dU0yY=$|IPDLvtV;u3Zn|lZHre^Y*u2;^tGv@-jb6(-RE1b0l=zSSYixa(* zPbNPm(~t2m6!IensS)>t)Ih^9PVS%}nUcwg`hJC_DgEqFmEeL&_GyoqoOXlY*zDX$ zc;o>}bmE&ryWnxK<7KmFUOC2~yfneL(| z4H`EOJW|=c?$cdJ2!ai=BA=ZwX}7|PV*Gfp_7}PgSXGaB7*@Ex@cJY@HX&;1=?c0- zpVv~MIV~-ov3HB(o!Zq0jeoY{HH)$45}VrRAdb*x;s@j=^Ycrcf=;_vMQ@Ba)GX1^ z=s}dc2+iXbtz#od%fN(r>KiP}5swx-LW`+_&jf$(6Wgxe3S54YfI%Rp)ZWNV+vLuJ z^RIxI^}2-qu-AnP`YdG#1SQ!ux`Ts<%cPoI2Dw&k|EaW0RfNK`RVoJ?2z1U#?E|=} zNV;hDc5{>0-!s>*Z7W?TjGN{AY<;;jUX2O{Wbj48VGnVaIyNaFjEY@Wy?|NA%RKHRcGnp2ea>QcBLGD zlH&T(n}7FX{$0@-m+*M#JVADkaS6uUO2ZHXRO^)Z-|BID%20}xLKAzI+dveCKU&YEER>K80GuRZ!0{z&3)3NB#!Y+#t}$B@L6w^pA94DS!z zw2Q2%dlFun>h-yMyAhKAHNQJy>e!bHOvmrzocxLOHJ>gIMMUov1B-tzFP z2)bAlQp$2igAjl{_Cp#jNaHc?gk+PI(^G-;S8rdB(^T87wtEb0w2#bB?a@ru84(&I z6~h0o_3zY_l^xjQ7$kAIEs1L#??31-qdLhTW)F;m4-eEha>Kk=B-sO&A{dF zHwD~oO8a&`bH^RFMrL_X%vMTM+aaGE|_&v*m3wzzqGx}U`uW} z-O|O8^7?u_&+p}JG0nr&j+80!&%Un5W{3B)F^tCypc|Mlo+bVE8yG5$W}&iZ1?|f6 zs^`{!Us|b~itJLUxWDlE#I2cs^&5m(k!)O|BjG#2Sn$xj7z|AAaR;kX2nHGnS3&&+ zb3V+5FQsOpG4x5`SA9?D8$M+7?bt z(va2jTW3}?V&bnAR{sRH@EB2A-PFw{F7l#K-&W3M(y#88H)hYX8u{TEuv1)+t`sYwN?fD7`uxZ+9 zGiE7IuFl90sA*axnbhgzE9Y<0<5wEa8bJy<)<*YnRJTR!Fa-~MIQ9$@KABc(L8ZP@ ztNN7~K$x3H>5Yn%wqrk}4=Tt6=JU>jjaun>OX*=enhGTRko-|WIlEET=D$Y7Jo3IZ zt}>S;adXoAM0ldaZ}eIsC$+&7A7a9afNpBBO|sBCKfT4rzDuAU4`4NY-?(TJXMZa%1ur%Q`b)#zb}$Fj;A+HddOf9_ek%X? zeFb<+vvz(Tz4Bb$nC2%ZO)nGcX+Sht`8_EZuvT(!OPP_Je{3<;c%P@TYC6#Ng@e6W zn=2VC2hXf-V@S@$_2*^|o8OfVjVMf`L}^xK?Jj3-y`WM#);v+&5NS@IFE2G@L}Cu} zVbhZf*fq?#NibJia<4&&dsHDd$t)|*EK3#v+%2?Dtodn#U6Mt11K~r>{ho?2{bLz4 zMofGs;kZoSZ`$|2OmQQ>-SdMD{Ug+zq$>xX0^xFa+hRqMOZnh=15x+rDwRTR4%=FfF@NG7W)Li-&cfrn3PI0BuAWOFXkai=E8*_7Y8@ zOa<}``PIUFGxof;jSzggN)>RRSns{~6S0mvO(7FyQ^>TqG^A}2 zAe}5s0_wkiVkH84Ku-=L1#0TQHr2#2{NISBR|WRRjKfwP3EhgIwCD|Z0NtyCz)P#$bgcI2V4z;SDnB9E4Dl0#m# z5~sXdaYJ~>;}rIecIRkb-wgpIXm0G za{8-Q+ge=H^%3jqGNj<3!bb!*-)SLq&{l)-A~$noAmzuEou_V~8fhe07-^LAz4^D@ zQzca`g*7+6?DnMd&ixTSRr`s&ZcymFZ@C09ubtt6FXpJ$3zH4$1txrq4D zRxvX)&)H1Cs%lUaq$Kk)938uWnC%Pyi$?bLnHQVFo<=aUy+Dm@MB?R5X(t*bO%tkA zyR5#)eS69E?=NM3ww&h8n^^w|@Lca+G4sRVhKrylE+(4n@g20eaq0DuBUOkQ0qm)} zRWaTn_%5*g)R8-mxY)S*!ix>%=UmR?J7zfd4`Z+~u^emr2C8iXH?-A?9CThrG|b=Q zW(D6+Qj_4J)9!-`j+w@T+Y^jg$V5mgHPubL!F9m@P&{BSFuqn_WN|ZFquseZl@TrN z_s>mfuD!iEp0Y3GQom$q)04c3fv0(=KKSRJn+SRAne^m|%aT!BRFcGIMNl5^>WNio z%k?|CtR2(5Qmx`BZ!?W4k4dk6#Npp7p9E0w>U}x{<9%x`HqewB?HBI%#gZuDQ}&uN z!X)WI)V`ql?d!(O=btmy#c6R^LF(ZIj9|v|c80tsoO3Ozny=o_*CUyF1<4mf_k!C_p(d7d^)?bfSFgQrD7W|w2riq*%_8YF2hlqg z)5`j!hJeX42*^SL+~#TUl%-c5(yAdGw&!sH^nwI7 z>Fsq-J0Ql$N*!S_yPD_o?r^AG#m! zIwAKfazB;+Gyw&kx4HqJyMKjeMLxXMm0)=Y>jV0wDiT2dWP@97V*Z5F3inLny?KJ{ zv$T2+afkPfpU~AeJCM_enaq>dlL*+kUvUW1D_$8-6RHl@b3m0W~p~zdL{4 zKMy>rVz?_U$3ImGZm9m^Ncx_yTUS_8S+DW$s`3^jv;5=pH-jT%uSp%Mblt{P9x13# zn)6^Ra@pfQ24kRb{hHw5bfAS!R75V#L7I_~H}X0s?&}+P8dbSMH9}Zbqie2Ag4c5n zwPHJx={`>=LNl;mI2(A6Exda_Rof!2`HVVHUw(9|`+fh>*}51fP|HAop0tk{h&KYS z<{AeM!kk^g;U#p?<@;fi&I&SjK`S3HbXnQDqI2K4Q|16E?O`QXv`~fswzuQ{?~Q_h zzO%6ntj(n0cS5M%Y`G&;AFXg(kmu02xtR;s4q^pqz&a~l+!G(ms&r%*(+e*vB4K~w z^2kGyr_-W}4+9dgO!La&DnxIGOWVzsfsSU#)}~psypWKA1)sTp3FW?)s#Z#*#rwLi9qXMez(|8~GnPk* ztjD?z6wW>D{WH;F8r4!i;e2YspLTXAdF7TZh+uK_{dHLl#$gup3~Ek&`8Kw-UO?gp zK)Rf18D7I!M$VQx1Wr0QQ&1m`fQ%d9s|roucu6@froy6as*F=l1pL0Zudz2!idozo zawCf*uJK*az{Qhe?fdT~pCcq9;nCUWPx|k+yR=2N=0t%SaC&alaRVIa*b*!N)9&seioD^xo07EsmF@g;>&D8s#}C2LXi6HM{73FNTa_pbm`Nkn!L26O2fJPdU`(?h}syzr&;{zY^sG4 zh)4tn@~hsh&zp#Oek}8)^50J)nyiM80TYRw&7j08E`(tK;+mvfGgQ$C-@5$o-IdXZ z%~`;s?4$^QGaR8YR3A#8*G$WZ!z`fs&#YpGi>?A5kAS?V;t4@28PIj**T{euH#@iU zXM63~hPU^n9zaHd=SQq))e(BhSVvjm>=GeIF&`pmdu-3GkGST?V+1KxNxYk&LG719 z>TLerB@rdctPh$QY7=|u&AV&%)V%QnH+5S?o4P4JlV^6LanpN~5tpko0JTCN zjEOF4jOyg6YiO98+3U0ciRM5-eP@N~^ipmvXwl5)L4J&ZCke$$&sYtS+df`iGO$A# z4^wD!`#tXWr7bGtf%u{9tarvAnMIH_=L(tzG9!x;x9W--3kiOI0|8&1M2=n>(|(`& z-qA%j(m*{DbgKA4r)o|u<_#C~4mbYOiar>CX0j}_acHO zHu1ZnyA88`=-5VFj{#8BDg@Qj2rxQG{0o;b!t6TF;JIyWMYscnU?7KY=iI--H|uec zpYi1C0WMEWx-v#(O-O!w&z*=m$PkgiXvuic`gNS8-%S-+CZ6g?gbp1Ov@)N?YdS6WKUoXEg za^7JxPfy(MqvnUF5w~uZ)?fQ08subxlT`tideHHO2WLgak3pNmL|(*+?3t!pA?EM$ z7C#rCn|#U7*ynCjr3|ovyl1nkGUD^vzoBRuz&4!(# znFNQfVB0D+LtHFJh895nEKt0Am)*_yMD61yIhun$+VpTh_iteghFb9RyiuN z-j(U$%tRqy7P(Rz5V>dnda$(my4_swgUEK#cO2l^p7B6T)Iz=VrEYE%!lOlxSIc9j zABo57aV4v_Y9!EC=>bn)bPs1E9D^V1m%EJKi5nJULK^l81CM_BE&_-7bgzYLg z398Ej*bnVnN+(aQ;dd~O^;ZjWwL*%QMk@2gXau~k6Ac-b^XuSbx>r29Z9pkw!nkf4*a&EqzC&+S_+I^VM;!Lzr*Xhdd9s`R55Q za(No_&;-&AduE5q)VU){rj85hO#RE7&8UT!Mf*;Z{<5w<_@EbKTIRrW^CyDCR})b< zlNTNe{*x;U`Af}7OVMXX)okGA)eo``;Dl>cv2yARr=#DREhV@wFJrUa|NY#0yfvTx zD2uRl18pjrYxSR_=b5^%o%|;`_>75Q`}K7A{x}n@zdfn=CSq&_kb>J#As$8;q;5~X z%EN`M$^}1}+qtgH&%~Ltdv^(qq+87}#BE z*3byO(vwbSG5SM1^-m2(jlsFcRI>29PhD(cG26^Vqa3S{&e1w`X$e*~j9b#Vt>RHI6isicDlbh5#?#-KtaPA!bgjAzlP2m@W&#f5`&^ht8a+1-r{Wtx-& zxF1A@&asrjq%u0j%*%It42X0{xvET~LA$}5_L+DFKkLmPMvtW6pCmp>tHv&b*m#Ge48i{Tcq zR$XW^KYOIjto?ceya}o4%6f*j<`9tF{63=za-v)?jphzE-{?`g3o^VjhDAh{j zJW5yf`gXtw=-cehgrg`htG-(LCHOkn`QRbZeU9oL$CCKns(-4yy|ao93uH?I?-G^P zr$E1y>jeci+b%`uC&roFW!9h}Hk|#I%L>Uw8UkiZG$b-1 zXS;^b@a4?k`h$i7oSMSXsa9ZhZhK=BQ||uie!vCa@?RDj>hdLVadGOVH+apNofD9O zk9JkV&)vIr4WUS@?T#S$yIa29xujY!3%$=oYHC>h>VMx5uj(C0s}-P|yjt6AS6P9}wvSdj-DZ!M2jTmWfwJGYWxGMcNo?DV}>T`+}V%G_UVho9ENc z8a@s^yrVFAT0G>~UDb}G2U=uc_4MWFb&p5hPu<74)s&k(uiLuH(Q)xaZ&#x>*W$(X zksMGJ-jbx!kM@wp6)LYcW#AzPRiE_!-KWy~rTLCrxygDl`O~t;^|*>Ol5gIlAHx!~ zpMytw*<(>ttn=gU+yZ1V>dN>L1HLzZltz%wE6Ubw$AD!e*SsGI&XYr zrBvrqit+*Qur+u*@%tyvWLwIi)_C&Cdq(to*rd&Cc*VuV1pmA~j;YmTAI{(1KwX+3 zf3Kh}X2s-(O75=`t+U1mBOh5?)EjHq5Cq{iQ2qC9kR;Fw(M1^No_i7#Gm!VO&0@)~ ztIE-$r0+%GG-MO8m={nKJ3utrD7-=T!~;`Lebu?6;c3AcP?nM$ds_V4uCmZthBjHb zMF_)-dGx)RWcukMDc-x&m)Tqg@1C0?kVfO39j1eLWKi~cOenG4Eiu+wPg_W9xzucD zUa~DM_)s^c5P{~LQqp?k-pw$Xl8?%)uU4kTjJ}yKoqC&i8!Jjt&I{H>v0@_wPzDx@ z7#A=GZaS+bwnf2k1FhnT$D?2?w8Vm>Qx9#-Oat>qA(`HBFQ?SLpSz-=z*k?1%3HxX z_0HHiewUixcSHyCEIQGdxB+R{cta#+s0A76?93?H5f_O1p5pCxv3+YL6V&1+B{@1V&;W!x2JrLOKqI;b z$M{aH*LMv}6Txcb<=^g^H^d*UsKi@H|Cs zhOSxTsj$?6ddP)1I5s(04jbG{HJovuKx(Mk;szr^L3nHbl7lj|an#k=$YGLnpQK3a z)zvT<-jGGGBIdP$p@0+4d0ENxbLx{-H~&aeyNDLbRJk(9D%Lta+av9kUs<|V;#eaj0mo^64FppFu`YF2hLOmXCTlv`=g{5rlufW zpX|#2VHx@8YckQ#dbIkFGrr_8^~i%Ar>e9Gv(FwnJ8m4jlRk6SM)t4^1n)<)Sge-N zG-aAAe*zgz5E^YMp|$k|5#4U>6dx&4a_^3bV83nZ7g-^pzN^)|xA)f7p$e(D_E!(| z@XTD!v7dha^*L3|PxbK{an#~gpKF5l_=sE?kLKgei=Pt}aR-kI6u{+74zbr1i7Dh$ zwOs!5pTE93;`Z);^PqtUc}p5%$qL6{*=IJG^e4B@wdiRo<$|&m2NR+e+-jVQ79r&~ z3OmMvZSy@Wn}MhO6b8~(!0HsRx{BnNHiIw}C#I{=JB>Ip!B>Lv$S}95+g$htvi6}P zjL65IT-{3yVfHZ%FH!%!m(cGhn(5;}vJ+7}l`}J})uxSHULEo0Xx)*Th7>FRql=^R z@?l^$y&XJJ(aGDVXM!*c7m3p4Pu%&w&dV6HcRlSqLKBbBwgBz?*SPIvFhG+6)pxz? zJ#z<*)W!rQo31_@F756HCU4Q?W@~x6$wY;bYz7PGKwX@nXoJ&KWhJpm#dL$r?E41H z#`pNgnB)3f;X)L5Wo%T8;*3yDvZWp&$6YFe(FVHmfQvVo3P?m7*n|z!n<6wsY_08B zr&v2=F$zKBK}a^aE5=4gU%c5f1+j1nVbn}JS^h()$0Lt=)XZ z4SY;el+Le3^kf!v3$}tElXh`+3t$C4^$m2PorudMmmW;UQx+pOi`VJ5nM4!WYZWcYi8*ltwK5&2n~ek z3Jcmhez3@(U{qVrBH)6$a1|(pTMJU8+2*~Sl;lqrn|{IJg_Y9j9(PMY4CfY!JtvWJ zCLQokDv|K^YbRRiKq`&Bxv}@Pqw( z+pTrB1vm;8)!Xe%0IRTuu<5gg9{KWW3zMyZWzIEx0iWA$4&~x9Wv!ELi{GATX!DN^ zR{PYkPO^UgoT)Pf|DObfdOpJ5ea-tFS}45mg84h~O7!297O*jGpE>6A#DzcOA2v~s z$i{=-%Qktv?m!?I%#9J2=_s^~$*N)Z@$io_M-+@*iA*9DV<_#&EABh(=X~I>mq!tr zwst@(pyyRTmlPx6o!F`T#gPxxEoPHG6klt|8thtDpcG}|E4;~IPM4FXpI&aE ztVDiY@J)h?bsH&YA~gCznSDQorYh z)mz-|7PAFCDk%b{I~ONKtcB$vKyP1jN%s3Sag;#ewnXsrHD;@uzb#CXPaST0BIisz zkJxHK!|c51=D#CNlILr{H^vSdWC!~AmdN9|>t|kWH^3-rEl0nkn?Vf;Q$YO@^?!S# zjWO^UH1Re*dD!Rl%Dgrmvu9Cc!FwBKRhVwdl}ABQOA@j#l$2-)8C$t5NfJ!vwXPi( zb}z_-%u0nXcfL(2W^w9M+;;c2xlih=fUh^Ds(_ci9G=pJO$$nMYblrFsa13)#ZH+d zOnff zXQ6|_IBG>!AxOqJ$zE8DoXz1mL-VM<@;X`aeBLMg>x$6gAz0XybwQ*o1+vw6d6HQ&~p{uBpZu!aCzQH3s~GSNCv zkYjCxPlQ z;8kApE%;A_tncBICAe7x#_6c9z5^h^ez!0y}+oGW<4RP;@e9;FI?sH zZlqALjUhT9uD0_XwiyH^lcztfP8cfNoe(m1G~px{JMi+gc%<*FwO8xO8X7bRZHeG{ zhtv+Z^q@yZ2zxG?ftRO{4vO&KlH zYG+GP*i&WD++{Mo?h(Z^xg9p|GwNlADAW#tUPs*_8HldaqShN(84RV541v9h_A)`ugy+`T}Qe@yUp_6F*+Z#qI!6kd23KY(+AZ z6&v#CuoKG8`c*)%T$)WvO5#pJ_rSx_5Gu^+R@ z`l;RyC=*@%t(Gp@(dD;35IZJ+sxiDT2Z3B;~>t+6-rmiN0cdzP&P zD|_b>v%wCW-nxRl z=d=hrd#w=mSc6zNmJHCw86eRF}+q3l2-kNO#=5B@ZtV?SF_^hbfm} z8cs(_ z$hXktFBRx=y^pKJLW~p^MoeKob^*e$ZpmP8KGj+S6Pm3GtcbncP;|%_)^>j7{$ z--`?D9oJdE>ABY)GwUJ`7Hh7j>uuhwaJHk#O=8Tjz5 z+1l$y|E%%?0-gelDP-~L#^hV4U5?0-r2rb|2_gwJvmt=7(~}n;?CaSa{J5dzQToo- z2MM#$tp@6`5(Zo19;=(q^;^aO`XjXtY>(#iHNSdJ&($vaeK-Hoo<5R4fA-bFbBUU! zAs+pG>clh7?t6RRo*i$KZMGjVvI-YuKFCHV>p~l9(|3nwWpWK$4W=f*K>*&HDNo$}t)Byzg*x?5$hLBLz%ZQG5P(u7_2jn)I#%M+;;6#fT# zXV|Ze7`>v0a2y}>fWAwt@{LRvKLdk7cdP*=H}60u3uiv4U&?c08gV-P+LM~gXB2K_ zUd3$`Y~rvL|Vv3yO}5qFy8tJpwU0Q zLu)Z@ec7@;Au?uW{_SkTXzSQ)TkNaN&Cxf^zIE#+xVk`n`2IamuL~0_-ZgHw_b($0 z>dn5ql1lg?*w?r`zPE99#Ilk9iP-r4xr^6n|LY|W?)Ez$>=?lloc$;FS^3r6yxsdU z@t*g4J`!D1HdzNba~R@Rw-$h1<@&_OcZ$y308Zw6|pkJ~!+EhDE2J1-KBCNep z2IGU$V8NV(`}%X^`TC?K?`_!aVJg>x5B?mIDyq+R>c&&3#rIRJ2eD#*{HRe}U(^ke z()-Ud#6x<5YOW=f-YGTR4qC>fS#^3*A>^!> z?pXb>tD3>%v8(OY^DFLC)QGtBh95(u zFoedE%y8kR_Z@pAt#h_&mG+=setC4HF z8$vXQT7K!(diTy`>utfl$jD*uo%fO=sQZ9I>_!w3_Aw!*%2oCHlDV>;-l)m|Jon zPHgoy{>`!WO0$Rw#f!aXR(_HpD?rG26sn{D@S7WmVH|g+DWy8&FuW47{+CB+!-gwp zg8!yR*Eo-<*QB@`XAkZ+Ci)v3Dpru<(*JJhE6_c5+~tX(hG~-U!K+1StM3^`Cv%t^ zLAID(glWa^A0;J|Cn|z&b!UrB{%V_P>r`V#RLoG4WN&xEMS4S$?6c!ogy(L9VcOs^ zb1Dt`7|f_YxOfs0zg3RbITNh8z(;<%QtZUjUgVwVh^u{#->WNsIZQ)eF)&Q!p{0)s z`PLwlf-&)TW=jq#K-Q6Xk<39E)LM zk=pt}xil!{(;I1tSaS23JhQrxYXdD`i)eb=MmJzF`L+9vj+K@e9zAh^c|!6LgXAY7 z$m0PU0`ZRBfP;>-6+(woMVfPhe4@NISD}`(>|k5D8K0a@=vH6O-Ry`$J+{joj4Q1f zCAsYoUtXn>b52X^Lw>k_E9tP)YUs%9P0@mBZiZz2`PAjD)(?kLwW>ZnT=I@~mL$|% zo__&4Sl~Y+WPsV!mi`;~CBkGJ>B8CxPb_MxPf9o1*1u+Vtp%8%4mYHBP8dVvm?s4} zoq@I!?J!lA>NmqaaB*hpk6%}&r$?XwRDve1yXRz&tNQfq+f^;(_4x}eIrpwuXjKpc z&$IDXxgeotR>~klsBuNU1NpS(A#_mkG&1CyPQrW|hgkVEfTbG-hXG2@=9TuF@*ydQ z=Bn=CvT7Nz5RXCkl;9W!1EdCEmlkLRZQno_sQVOI+IUn3JD?3@`&~F3{9a+5(T!BA zR{6l^mp!=1>Z~r?{x+28vdQ>4*Shzit(%i+Q$boCkW1gv$oiB*--d`UPfSZc+4WwN zr?T-m7YhoSb%*WjkTHivifAM}bIVi(MHb%=;KX%jbATcUE~}p4z`UVUxvf&aT5UF< zhHNI0h{EvPW=v6feqO8?VybY3l64Npuai6=H@t0LN>Cj1nQW@}Hw%5;m{4^oOv%`fLiNySD*o`p^{*7m1F2cLr z35=>vyMC}U9sH{V!X4oiT@xrixi%)GrEJk9*5Yi;wVAfKJ2?o(V8}!i$ z(#i$ROTn5WlUV`DENG3)Os(q!T{|;Vt>k5g(Q;q<{?t)O`*7a@0N5d@4937z|0S+rgt<%vv;;zF1z< zdvYps%_3DZW+bMWKp;)Fx>iAy18m?^`2&Fd%%y{yv2}>RgRWq8$$UT!_jPe<&C#DH zy>^^QQBHKQ32WCZDg^q*-pL1I-AD!S2bpy54Ma$WF)viYol}YPbgDu=Mzp&3bI7@g z4*N)2ffWAd_YX&5PV$Sh9%3YoqRXNZ>v(`&p=UrYd5XVD=dgVZ!l)=ee`d-O_dL&M zTd;ssmY2GX|JB5JcjEVm9kNGQN=;`?H&_P}@{(V9VreGt#6$x_7m(HjLe3?;qK5LIWeZFj5zaNG2cOW;hmg_L<1Sg-4sP-<=zu&1Q?o*e~9F_B-jRiZd|Im&hJQw-mS<*B0j#8u739IZ~pj})5 zB9q$5pki`!K$2|2eHM|pgTAZ~-K7wvI|D@B22I2HgBE`zXlk1=_lHS=(Px^l4x2D( zcG>GW9-nI6#WV_2fw2;UOk|hB+k_VE`~!!_q2K$y3LDX>VyHpLdZdWd8Ih$5erjTN zKIg@SQ+e^>RL>^cg{syH(dD0LHV7UficcMORDy;q7nVt%Sa7hIHgxG>kPcy88Ek8u zj!6D&L71i*2Z!@M|9#$GSHnpD0%;ev5jA9`VkQ8QrFhj=eDk`weRG>LbpqkyGTCMA z&n#FJr_c(9>y%VDUw)V~mfWw^ND<~rX5Q720;2m~Td1u5e{XEUbtMxOpKZlgBi6v( zVJ0qK<(AUPTKTwfELn&aP9ApiFH5-0TDb}x(-fA;NJ@yLiPfLA&bhPhgZgl2z8j-K;_+s=_tgeh1!0jNPaZ_+12x)}w#6P=KN80y9%D zxn~5EZXJnc3c0asZjNKGPI2B}TEkm)1J#mcF~eLp@kNaezh5NAdub+{hKve4h4$`| z_{ehKj^ht}Yfq>8)<@kc@ol_M^gokiy*JtPT40Nwujl*8m%KP>&jU*l5E$YrdiJT# zr#{a-@L}{RKL{+ZFK@pWdJk02caH4B9QJpgYwIvS45$}q7sFRnz8Jt{$0V^SYVLCK znWboxv0$|RACOuLpW*af*A)z2eEQ)5(vcfjhekehPvL84$x%}?f`esZ$b(-$W{y4^ z6k9UbdTiRtgfx+!iw+ovWe$tQM9eh=g2pdZ$+J2qA_!SrbZ6tHAk{XqPGKC*R(Z0+4ugLOkZYmkG^jQ z`>x*(Zm1u#+SlmK3sm)$JsfFX-3+QZ|G|gRFNGgQfCk=nf5Z@@udIA>t>jL5dch^v zx%_;p=q6z5aC6-};Xn40LLvpU$3|8tU#_hOz2*VYy=Q#zVsB}s%p;`3L-ikZk3D|~ zq9)26xZd7b_1UQm2QHjubQ%0wQm|1M1Y_V`8@f2KKR7tsfh$L$-Izh0#1RpVux%DX zT?7TrW$%#J3ie%K_qzFuTyy=t%JRa0Na~J|M#U5zR&aDk2b=__3AYjKyaUwge!wIh zTG?4~mtv~J>6$pzOasPyx6D)*&NAz&406(9am2$=+M|*Ilx$t(bww%OmnBOLB|26D zHMG@-?sp>|llD(?)&JcgPbUCx@6J>b?hxPr0<`DzZ)h&A+v7_Wb^vQTAyhOO9>V*4 zPjwBCG-fA!Wlbk-Gn{|(L6|(cn+|_02ri8Lblzv~JM&bs>DiS)uePvTWB1~xwl&5d zzvBBKhT&WHpy%$rlBpflH7CNur&_+Dr(6Z2?ek;i0LxvN(N5eKM_Teu@aKFa`2Xzx z!1-C_m%l}tlj5}i8_U+qM3W7Wg`pv1l)0s+#pg5<|3L@|uP}b`L>?NXR$Igd8 z%8=J&nwsh+*y|vSC=Kq2fjDrHfb~c^e-=WqHoI~%2>qPJWHf(!`TAtegN#m+8GhDM z#^#^dng6ae7}q$QswXLMP7Mtncrt+mmhb1ak3Qv3=sE%Z{7nP*(7=1&!-*@yq77JClYOt0C=qeyn4_Vx{YlR6^^GJbwPbmG9=?d5!~ZvRk4ro5x*S zm9!4$uil4ec|IRK+Jpe}<#b>LpSo zTV#|;IbqTgaK=$g--mIJ0pdX*87=P?myz8tpK1ZHbusI!%Q&#xIE6!{$EnR}sR$M&9B@hz1 zq2XfV@LETQ$)$8Aof;!x)PYNT+nl@vWZF~KDR?Rv$N&*>A4f! z4cZ_4d&L^#hm#UWi*^Y=zlN1No*eI%@;Y>Awq)t^Ce!q68F?e+Uej*%VOqv6qUN&B zW?KI^NgWHkqwM=(L(uDkpg;um9rn-FDPA>M#3!ayUPyf?k@ju0WOolBZO;~BFdBD15kb=No%jR;XxFcUzj7MIN zJ}BxOIUNxMR`?0f^@L9j*(<|OZ+0eqy`?zUoBw#UAh@tB3z+R~vYUIchKy{#34V`g zc#^KE0{qMWJ=5cuhjx=@B9)voduVRm?Cq%ZgrO!;GukP+;sJF!3=gyZe3bch0X0`P zr#?*SFJ~)IGP}(iBl9rYi;M|VvZLK(`T!4f0yIp>fCnAm??e!5o4@-iD<{gsoP#Kp zUho(LlsUE;V{!Qr(g!MddAgc%04L!($4N&^@BM66b&55UnR~yWS1j$I}amBLeZdk=uWo)@HlB`%iUhp60p- zXwV6hv=MQTQ|inP7IshyBBaGpC;Z+m7gXax+PW&azee;&b|FR8=%3!!`}aSRV9dmk zbrm25NiGugoBeV{h2`52{H-r8VI~K)bwxosd>VEmvDkyaGOP8$ck5 zz*vNE%YOgWOt_D>O7hyk5u50G;MA(6RBiCzzp*CkUOI{MJ718VE>MV)+OnsdnzgXF zwF`UL-7{q7xEGFbHPgx2GwY_18O}s)_fd(S>WyljR`8*rWeqmZGK|584;{Yh8&crq z6kbS)-;&x&jG~%KEXW0SijDp_;8On1EEiL3F|1!^_DKIu@g%oHd=n?PP}1aG^5^+z z2$2(iZbc^(xS-#(JiTteJh@A5ZtQ7AfgjWGp*ygI+;m`O%j9Un*envo_yHeHPDN*g zQ(*KulJ1nv)vV5f2!iF~9kc4$a~J<_3v;^dw8thvk7y2iF|T@>eB^@kuQk}iE{)iKlS#|^uy{bv%dD{TG0B> zoUSv}L$xwhK8(C_)I4+#`Zb5r!;O=n@@(8(G6RD)auSVJ)uweS0a!nO-AjZq&`9q( zP~TqB>)}H}@v2g?k3-;55k<(Xb&JFdv(+K^|5q+VshoCzxr35l%fh*(jSCbP4Fuf+ zg}2Q?6hhHhzS^LHExnOkUfHt=v`>}$Ek%A^cm)6n{oUDrDFw(NY~r?2{9Tkqp0apRs) ztb&wdOi3;XZDd`?3+mK@l*O4LOg_~RrlNj3-dG20DBiN8a{RQDI-a*n+E^(cVFWm` z#DMR&Gw>C;o!Q)1w*CT3{?FGI8%O^am*l@CHWQjRdQEXkgw$)(`kCq5U*8VB`IfY! zkgl0?$%)TTjovx`zPaY-SCR7MqQc8t#tPQ(^1lHKgFIzn_DgV*`IXN%S#q>GQr6Nm zWV5tWfYlRs&0TaEr9}(6)DsY^qBw$A&4-8w)h$}N(h#y8%ncp8rBC!5WNU-(Hl)h8 z>Hdgu0&$zs@;cRbXZSs;qnrzOl6uC?=1Ql;Cm}iarOR0iH7-b<*p4Takb<%zg02ik zPizv&udcbM1{H!q18L0_|10G|r4A^~=Cg$PNKFaID56ew|-nuX%P_a4GSHCs#;jgI3g|07= z&u{K)pJ)ZtDLMBF+ng#gm=|IWaKqWJgV+>iLh)J%{HyMW_$*?KM$(oH0fvrHRDr;Z zitIgG@3iv}-OA&Di}w>Gv7%kHwoL`A-b)@hsM0L|PrS+h6jPJeGHso$8pLMRuEN16 z4)J3Bw$XP$ZQhqC(IS=L{8=Psj-e9y`-@ZK$LC3nGp|--jF8KDNP*zv=o_Xmm#b|f z9C?L6%hPXQUP|)_f!jba2L!J!HU`soGA|}wK{2^G>`LoG95|IrpEuw4-WDuj%9nXxL==s*edrmxa8th`qN@)-|8 zI3x>gw2fRLGkYJOA;-b%YT5@zW5Ama>>szo8~Vewg3*}6PBn$I0`;Na6%(zieb!lG9UijXypeX&i5liu?^P!m1#T(bqQlNy8@Yt8;fb z6hSaC`ncKdo!p+pq0BXy!Q2b;mOk%wwx9Xy&;eo~Vw-&60Bd^AeDCT{qG9CdHou9J z1qqrerdy_VSxE>=yDPIxKXIKUhE7pZezcrj*uQ7~efy*90)Sk#E3O{<6v_G>_q()I zlh-|A$?D`e!qT4~=my%1MeA9rJqb0wF2Hv679V;C`OLmD2q72=uQUyVR0m>LKPL>- z{Y%-u2wG5kaZYOre{Jyg6P%Vt7%r`wgLE>!+08BO$TbCPBNfeSSXWOwjcq|bnTG3Z zkw>DVInRGbMs=~G+EiKEJhiw)QQX0tk0t$4ocXoc7w#kuF5CKxEIN2>?b(YqRH@6x zw42Qyzt_5#I?W<4j0`gHb!nD=TRFkLfWv`bp|djEoX5%@vi0u`^@%o@F9iA}n<@ZJP4|7`&laCo}8shqYLSL|`Pc7}fs4yWo)Z3uC9Z$I1q%TD!KPTFa!QejcG40D^U`_XGyYGFn%|JiDiNS@3aInsj+2d9=Bmzw%uGLT!3 z%zTWEx&wah%Gkz@F(i-kq!I*X%bUXAhcu4M(`b!_!9K;{Da2!|>O0!zR`b8a?uYk~ zdo-s@DO1-cZ?I5pNe9Un_IeTe!;p<>$m|r8zb5G}PcOZf3sB72*O%u0S6ubK9|Z?R z5Jnp=-`$HAws5AiMp%N~<)=*-yWJW;@*FT2+NjyfyL-%s@dP;?W10$)i;&^aI026HSsDFDVYyF`EGtNR}vw5#_T@B4MHa$fc9USNYII zkIzgaQBz;1oNDqvfOIltMcjJzTudqgFJ^FWST-Oo6V0>Lj7wJGT~N6$MaV_- z_gRee>)vXD#|`*np32vb(AeCs%XfPxkn`yRNQRC0%8a`^bp^D+ZpeV*@^ZUYWmVL#PIuT_g|w%>KjF~k6l%v zBq?W;d&-iwu(^-pow>k7xf5#s*{4VHL)QEou@DQWEN#kLTA;PXxV26}zCveEp3G5j zDcwbJBv7dC*6^uQXh}Ii$&!SAnAwP&k~X81z_iu>*KAYuRKaBF)utm|?l!GeH9a>s zH%>wB6f-kd{%*&Xl0T7?Ir-19VHdhrQFc2R%4g5-vK)}sV4_Mi4TsAWqAg|B6azD* z^49ANrS7^$P)>G?*=!Z4?jo0&`L&qBZJ2p_J$>0~?lq43+ioriVd&Glq@|}Q=YbxU z$UO)%jSML#pj*64Ib>BPOn-YKhVr}l>AivHw-bvHGYe29Q&Fk zqdQ%dQsRDG>_gr1Ol$8#qEXwbv8RvwF|p+=tHy8jnfr7MxBgn$+hh6o^wD{lzbqVA zqYPJX8QwQH@NgWMsHn;=^`77VCew^s?sA46Ri26E5=qbIRbKOa$Sd0y?=q{7)x>T2h_x zy=%Zq1f6^pxp@WG{3bfIh~~rIgv-rny&tQ#PZQ+rU%9vn!Ugq*Y5ca;^5z25ce|i7 zl9V6A@VQ)fdV0r@LcfiqgK$Xap_@azO`oWRvY*|v6Xz1R-{u~&+uyO+=CCud4+rD+ ziYaG|SRdpUQtSl~r51A*P5~M|bMlm|*Ul<16{x*_7b85my0_mUL`7DcK>U2t7d@B7N#MiK&?5$0ZyFgeG{5%lVFm&o(+Z5V7ROx z57QCFbf>I};rmwHX|n0xG);ZvhFtx<`NGkt9$v|&B4HyQqh3xVFFhe)3<9QZ znLLv*`oQq2sozxL5LOM=>L3r1x>^}R?m(4@@p>z@u}~}2b3D(q$YBi3dMAQY zHH98NB@-o$VlZX)e!l~j+u?7S6N=MXBDryrwx~szrR|={Ay!dM8nWj8DHh~?sU(Za(+UtQUA#Gq{Y``{t3ZQ z68hL|Aq7HP#lP46B}~Zq^Z6|T@XP^C1w+4Z{sJE{q+n1uaP;3wa$TX+^>@>;xJBgp z^4P>wYGte%*l}a}L={JWa&JL$mD7`Rl9m+OlbfBav-WSpb7ctM*V-c3U*uz= zU5YL%O!sWx*RuSoKaA*~s-XBK4uB`1${#7;oI$!?1Y^#~JL(tL5(AgQ*;h(tsMwQ- z15cZ`t*$J{(6u58EKZ!kF31JNUPXz573i^@TNbMngrF1#I_TH;RZAI76Ba%a{oK=< zuO~L~8$~93bTGkh-P&z*+a%g|%BQe9lk1Qq2y+{vk8@@4RtKYh-B|29;guh!q9{4I zI9X-w9}%@Kr>`ZxV|4wdpAH))v)1de2qdDDRuwN@3IX=D%&UN{2Indk165U&sl-$s z&-MHJR|vyl*H+^7#DA*|URvc-7z!6T?oVoN)ftC^Q2MR<52F9e(k&leTJGZ9Z8hf{ zJ6+E^mWbBhZ0|p}yD$qQzn7$jV9S{Uj&oNQ3p(O*`6te-Xf5*Ee6dlE4JFl~urV{F zqRmMP;MBpJVus!szt0QL`vj7ywWHRCwFhu)ibd0nQw0NEIDb#qxhdmSVdDrNIvY(T z`mfn5)!LC36t)^Ql>BOZccoZKjHaRdF&I2b2Ztj>)F8_k@#b6-r2fu#P7paFIOb9~ zUQUSZn8&qi>NbX|j_pGmn4*FQO7#B3|G~NK_ci<;C%-__DPYvu;?S{yolX4XCr{V? zbtac9>3GJ2iC!X7sD7FvicOI{ih{hE;{^ZcG0a4dbS{T`9Pc^tc*UF^HWg3l|MeYV zsFIN3NXb7ZhP<91WV8kK*j0S0I+myIs=9ar-O~Ww1?0@@mH5!gih=P*7e|Fz`Aii9 z4vRHY)5^p~0sr6%2wTX7JY=~oA=giu)1)j>^V&^M4!HnWqo*ebI#bl`lkFn(q9p{v*4owBFhu(VQD?C55%lS+))AR~>po zGg_RByQ1vlccEO-xY_fa-@7_P#a$Q&T{97n6uP7}Q(Tu!MZ@_*x`vA<-;0lxZG<;2#xQvv0kL_^k|mB<=3N-Fqw9TekaW{TxRW@9*(ADio>=Ww%Cw|g;`o+BE4`uRn zG=)BLcAL?&vFtD0uVO%S9lK}?4j((%V`GPUlzxnv%h4srVNe=lA|n=SC_jTG2AjvA z-!+xD-+O)fXW#?nhp#OXykKDA$13NN$;pB8c`D}c+p7hBRt8LSvY?Vi2X-`xxWjXI z{}*sSgX7|piy}QO?O!CVgVN4U8!nMwkQo;F_`|c2 zKkDi%9vko$)H?2Uh300S#PxpO#135kv1F@)NU=OMAhI}p@uG!;H5e6tJU@NQP~`iE zjS<&tr31H%ZT>x^75&Hl0mwp$^ zI~PICwx9E#wMbpYOsOb9(M6Su>Wl!>?vbNEOzRR*EJkuTx30=e^=9)K8eQ(h#3)^E z%J!hdrVHDHSYK@>o|>vv9Q5|9S~%!LhJ3!&!LCfoN|NKQ!RY19Z6iO47B4z3Rz2xC z?BQmev$68d&JK8IChlsDzt*a~cg|*xVhZ}oE`g^N7FyL>sNwowFDp7_%qZM~Rl*-F z9A?=(4%z~iuAu7Ai+=#A4BLN}``70E%cedLmS_93Rd@FH3Fq(zAe4eKo5P+}ML-oH z2BJy|at2{g^bA`%(A)rpghE7RAx#+Hi?YnItjsblz@EeM7E*io%SkKSf^ zLWa3UqU_UDZ}X+C%AE4|J@sv^HoHipoB6Q&dyQam(7F2*=iY3F5U+iRf^ z`n<1Sd7J;_el-R3?aH$VUZP%N)ADV172RPvIKejR?o`gqltYFqRHSz+)@lzIA~2WH zsvjO9`_h=r`g(&=R#gDUyKo_LpqrmHlf|un{-lR!lxv!K1Si*<@ax^nTkWm8I&PVK zekvC7YQH_)g9=MJm*VB<*_vY4B?-j3Lt>}}AD}INpMC$+sB=e8GD(z6m*@OdyD)w7 z=l*CvdHSy_CpHVuwo}?4Nbm}%xpu7!7HXqSW~Lj?5|9fvxwI&-M;n^D0C~`B%1_U$ z1`Mgol&75K*KwQvRa&e^)zZ5$<-;-ye&yTKTK6PIq%I3y9GH{7d@uHJQXpfwH~)@#gpEt@%*OAjW9?^a zh2zk+-7_x~luUU%P0@kJTArgA>d-UJE1SURvyxzpkF75spkM_{iqAqUO=#~=lKi91 z;pQ|#4%66rR6+EzJ8_j!1_165jfu2KI_vJTF8zjt51kVZwKco%!G}{# z@{JS62Nda2F{`Tp+YER?D@!BTVu3+!7rjb4Gc3?2R+rZ}jYrW`N`mgySZuEqi z9nL9p*LWV&jkWTW3XgQ z8TQatu<>xlvLp!&VT*4@RV+@`AOGk91cW*SJkyfs%rn1!d;weVxB{e+4K_mnIs-3; z9ES#Yo-wbJR1$y5Zt>qFsk@FksEd2^w=op9c(m5^OK+4WB_!iiIYKR4G9eN$7%f=>$dp zl+-g3x}kjASKj??cp7*X-d{QdFBmV;R2h|;>4gp?ESYU8R!Gl3w9L+iDk%=I(ffa? zTj^$>r(&TKNrLyD#{I&HyYK(}at18|J{CY*NutDd7MfQn{iYL3tKVrHrR0)REq6p0 z|HzkfI+mol!Iu!KpBDPeFf=OL+}P2k_eU|goEFX@qVC|SyD@g^z-o$bCWv^}fns7g zhqQjQKz;!bU}?>9mLG2tr48O{Ua48>I|iP&{izu z`d9vy)&|56ZTnYc$2)>d7jL7Z*T^%OCEWD#Rja4~y0UHtBQP95l@YS#3=^}AV3%UZ;91JF!nhQtNZF+JY`d-H zjm}?nSv!%74jt`a7~#zy1@Gdppl^BGD1*6{@-azTZ1S~J=&ixcdApZeH#v%Ct}@J& z16@*iVE5$Kk$`l)@(?WxUPy|QT~M`x7@Mur`L;MZ+RI8&{}LiI<0`yo417vryVU=v-bQ7>Czvu+7a%G(te6Q-J|5@yUcY`^v0RC8itlk}B51>@weBwm z`$x#%1fO{MXk^#^BW3u)a8ACzP5`=zI`i>AUmC)oduKCJ+v^OX95Al(1WxL0{I=45 zvM7*$=66~y+fxitI8`N@n_jjDJpzp-Cn>|6G<#^+;ru6vd68Cy5#01$^+3x8) zu2*;lS7kljnIRT?vRyk{!aP62Kk2YR@)RFaz@ZgQSp6F65}yl*EAARWOz;>j=@7BU z?K_wdPL)<)|IS?O(Z?lrE9s}CjVd*2scPvY(@apRtyj^??Lc8cRQ}l3ge)=Lo(hQ+ z@`wh5ujXh7wz*U|OeAvgDoKu>D#v>?xCiIP=jvx(gUnA5{q~5bitDXsV2l10pMXy3 zF{p90E#w5HB#5|EOv`UuLmXSwlViLc)To9l3AEY(s?chOK=B4m3zf-BPtlW`gnB8P zV#muV2F9ZK9?F}<#lIFAAVpd9pD%e6V4i0)ujJS42%*vJ9r+T-?1*Be(SIw&KEHWt zpv1=xb+oph2&jW`<`GNvmH8lt>nZgLKG|njfT5=kbx1rO;==V#3xYa5IQ8+;*N|6? z&e8nfJ|&n5Ld~NjBOexjx~+tXy}k~f?~}*-gjQnvTF*{tshgduI)Aj~qE+k{;iQ1T zypQIecMF)(r+f7w)({rjG&HE`h_X{nud3an{I`!!P09XhZEspMD3|gnM>A>N!*?x)#rwPzFx;^fWz3sC-p*MIlC9f9&9}WQa7sm z5x(YUHj^24xDi>A$GM@Ey$)^Hr4 z1;@vobza4)`o@Y6o$g6z*&TV2N?}1!y>!Df+&eroNFY8!FKbU3-1Q@P(F2N{BBaMU zY*Vi_!u;7#RI6QS7kQvN%2ik67ZsX~y=5p9Sq?3j2l(gc{gs!uo=Qi9iZ!a@`R_$P zvoy5*Sb0~#N127d_U|vG`uE?Ulk=hvt(L+&vpz(zNT>Cf6_2|(_4Qsckthj5K|gx` z{=_t3X+6!uRYwgXEzLabqv~tNAvM80L>yNmnsjMBt zV3l?iXYveQza)LI62WLqE@%jt&y!Rwq&&`rNE9wbU+20AGgI0D>7)Q;g8>N~0^|+? zdF6Yih{@LZQDF{(H;tvF%X{``@o&LSQ8hgYB{o*sH$~KQ?(wp+swp?y3+~IYt$f$r zZ040Dy}dqB_NviWURH<-d?TDS1({jKu+C-dFyPD@wq{Mk{rLWbXyxVKgi)nM~|Pbq-pb#?T~h{>vst^l}~z zE|l>A69Hny$7mdG2EUM5f}okC196Mk340XAIAWtE6U&WnlUt#ldf~5rvoN18^;AxD z#Ywpq!r;BTGiGvW2J0M6lNHMk1L9H+9B_9r_7HKh%@g8_hjtmT>y_kDoj^$@0&n?+ z2)XSGt`57&md!bth7#RRj)NXMwpFjKn!4#U znNLhE)0+=uY*lRgr@NVh5VUhB_1gcuwED~KiF>4XW(6cYENBbGA2DBHIrB+K!L7il zudn+r5zc5jxDwe^t1E|$R1c(8i)va0Bf=>|L$TDbNg<&Le*Ar`Y?F{79f4Oo?rPRg z;*++7^r7dOV)4F>TWfK{_>4D-E{?}K^1=>d?Kn7a_vh63s9NG+;mloE_-1JJTdxF~ zU8rg`Z=%Z1;tITybYo&HY*6`+Z-W6m(1ijE&yl{VTdeF2+a9VMZzON$5;f}9Px^3K z>J=~d8lK*rE;|VP>SzG%Ae3UlH!=5`h_vH#sqvVC4~xXujyS-{ zedDCg_orb-#Uq=-8^tcOPLHVKf@Q!MyT2Le-ZAxu=_Z9jUa>MhwmfF_yp@>W-6|{$ zSh@%@^Il$wIKO=hVr@CR$96(!v50nZLTL3WzZf2?v;P~~$x24M)^m33E&rOxhzD`| z*4I3#=c~X^u{qjEX6eXJowzh9Vd6~k^xnE*`MP5ROq8(0lD5$~dKO_X!b2zR%i#sU zb?8#~-1MEb8WZlS`NuLNH`@U=mB8yv34k=(G+h8(s2tot(x`oDiRyD$$JIMvzR}+{ z{k21!!yrT#0do>Uw43F|BLmb`+Bc`AKZ`u-VR3Go-jOI1TU6b|HD@3t!hXVQx?^o} zQ)(wzbBIW=KE|i)X7wS*dR(_x-TsFL~`PJ~QDurTb=-I?vU%arlSm6d}ki2r6YSqQfpw==H-B2KcrvuRJTHCa9 zy_5@yyEdm>#%+-0R&L3`Lvf=|N3T(%T8FY+Swcl@jHpr3lc)QWs~vb_(&}AAjyU=U zj4dAzoC}!wsf!l+LsjJa6|1=?fVW|0+R>o-@8YT!<~LG0Mk;taToluDfx~#;u!;o_ zL14&Wo@Y@b&q9dxp`+DRpWNA6m2`&m80*5tJoBm*Y82Jgs-tiGAW?6{e|u7y%(7O) z_&;88H}@}|2Fsaqsw{CI8>UNK=1$w``!lCM+DW_REIbbxFT!ZkWPBSJbjYlrS%dHS zw3<3Ss+Aw)ce^uS&?wa4{>ERy7&tJjjEQ4h1iu_DTy{wDFRE4lf%+%oeCQ0Lwdcn= z*S##2s)8~pk*7K8+{amcI|o87{uNfP{Us0>34)h;&`AZuQFrt>VQlbF|SPg{fAX-Y)YL(vN-fx^)&KbU<>1r(I8lD~3iGMi1rt zkLXVwh{It{0i?%|ZbT{7GT?k}ey_>4Ay5wYO->tNdy^!W&4thGOhZB+%ge0wOntwF z(HAmv6@ETv0_7rUiQEiY<>qp&$+^j0E&AXC%?TFW=8G1671@n$K54D;$kj=;aJF-yNpDMJkaU@F``hxmVla_<>&ptc>K32}#8E@vXkfWR^Uh%rDoE zs54#qH7o9(#&8(5wkZ4gBT%R}{ed253*;NKKl&G-Nq* zY3lsOlA7;=d6)zv;h0i?alQXm_O&B|LSR1jtbr(JE%+z`e?E_v4@78uUREF_r$wvC z)5HEj!SK6&u;A;MW+gLraQ7l1`RPiX8_TY4Jo<%VNsF(W<3%{O8BKq9&%gftC3zy>Oyh8~YxIVYqk@72(eq z8KRjE7^v~Po6qR5$!mOQYX~aAWsJjC{YJf6azQ=hXx-udjhqMiSmYrOW@>ModaaXe z7!s9b?&v4sW`$0D3@Fl$3E1d2DfnhsSTtlD6ZJW!5JUe8^Z9V?ucmedwKVWtJd9wk z8my03m~HSnz3F@_OfxhRb;5kS6_yhM8wMsDKlWsC)*7okN(Ni!kN9nhr3^_ncKQKB zI(3q4<3>E<^`x>g>EY1#?A$JZZ)&Mj_4ZEbt_(i@&cqnY4l=}9Lk z984i|jq*t~v|1cRXyNgz)F|64upP5?P)HHmay^aNx?t7jp<$Cb&V~a^2QWKZReucc z?=uiW4%VE_OcAtTA(4~-jbR}Wr~!2-f^+7LMhjxvrWCw?L@Vb>Esc2%_?#I2zwD9` z?lCP*=Cf$dTl%7mkaZ=M@rs+Jc-9piyk7eO;gy1=aT9}Fc@&PD#RgAz?l{5DMn=mj zxbXCG6=z|m=*)BV)xmHR<~UOf77lrA)hA^0ObBg&b(gmGRFb-tCw4|5^4El4d8K1z zmGai5(-)LYE-1bH?77ZpEWS5DNg22?OY77(z%~Q~mel90Of!<=UV7;hi{_X5W`*p} z2;*LcHav;j^;{GgZw zTVW0%eSjCo6*MO4x##CheT&P81(OdQG>wUuuUr|M1o?=Dg(ug^O_i^Bf1qFd{C{5= zh|A+=dx&z@RlSh;3XYJtyO}#0!-RvVmrEC$$5N^cBh3lMsgF76NP4&80n>I{_K-8y zM*6v}g}7(nxW%^;%X~W~(eJHRMZ~HhUJlSLrK;G-aYM1a&Age~VlEFzyw8{#*tiR) z!~{7dr4L5KMorbhJ;hEa}$d0W*ELxmAO zQ}!A+n)?YJ2Q&GYmQQm-1A)E!o33HkUcG}*8#lH;|xi=WEp6YW_sxck+AQ)k|u znrDzuw^BRp@=6>Z`YVk=Zcu}b4kUW%(OfKlJo*K4E$~s!OP@I!vM*%njPrA}TVamm z!z6~^L1uQZ>xR0@6Opo&S9aY!74QAJ9X0o2Y9p(0v*5Vxm&JvfuOkW~AKaz#8fePp#!Q25X+M4SxrEe0N%a#oZFZ~>27WT&FtIVv7JO( z?5v6#tJ-hq#PfI*Bj?39sf^d$vYJ)BE}!Dl%0QjzGNui5!Hd49F;OEIjE@u8M~vnd z{Jaaa>|*%7Ytm*2-o@62OY%`|I?>N zpSgT%+%?iQ5MH|AcJCotS{Z2G+W{qq1~z}5cIq2U;7gpLjj9HZu%u4IX7e)#W{ZH` z6snC%&n2G7c=V!h2|X=wKL&WX&3~T$Cd6RWoMmSo+z*`C$**G-llR~umP+YasJZ~Y z#}lnFfky$Y*T3_x&^BU_B9soF{hDyLIJ_|eua>@Y1Emt4b;@KKDZQ+%B_@qcV zbv_%@bSuamw^PVmx!kKE#xJ2E-%J%(5b{1=Qz@-F-UC=0V0Hk`I;&1pMtSI+ssc@= zBTV=Tr*3{U<_{CtX;dByHcIIyfRY;BC0%dF$!%*eH=71s9` zTr~`#K|P{kX>d7AaZ|!s$9#Iofv=wXoE60muvS8=ZhJE@6fuoL+J{yyBe_HYrS78( zkz8`$k)2i~OnSeS?kjdrScjwjM6Q65Q=i{ED;_vEd>F{IA}=OAqa%%@uiL%AwNRlk zV;_W9HNqjI1NabJ5*UqzCi3=tMZm~CZ#PxJaZrfxeUCl*oo{7j^Q^L93*zBcpr#CWtA3OYcp zz*-L%1gV2D!AlYmwui3AqF5M6n?qyL{1*5+&V^8z9(;W@fSr;s+xw14EmX+df5O4M zP+t%Qk`L~vB6QnGx^)z|fPQP7E}q5w+pF{DjeoxOt#68DlNAnk+aYKD+`;*K`N!?7 z-3YRO&zmZ=GmscKg~?m64Zat;| zplvq8h+LJt@<+Y1X621%K1v0*)oRqUUUNY_*! zjut0XD!VOw6>an)O7n}uF62dn;!g0Vhe_b=?mk+ZmzUSUfSnL{V0d!9bykfkuE&cJ z-?oQrQp@}{-0Ff(Lx5e9^@!?`jl$y8R8Zmm7R2{$K(Ud_7 z+OoQ|Z`}uu?ZV)&g)KzSsN@Bp`;za9`luJr)+Thh(y6|Th_ds4%t45zCrRai>t420 zybp(?g%~R2uAmq57MP#4%!22lh3>jN0VmP`=#6JC)&`Y6eJNkH5aLluZF(#OCO+ySi%OQ(m}%X+S!!bC9dz5LM9_!TB|3`1yvE z439kXKuOslcO?a3cmI&`cr3~Lg)|U2#5vpxSwCnW%nZ>o;XflHq-C`oyKMFuf|XG3 zQ-L+DWy`1C2aNkWx*IDJzQZcP;vC%r0lu>879A;ygYV3%dS!>1{s3}sx{#iPB%&;c z?xbpw_x^D?rM~!}lBVtN%Ho@@ga&Q63XfX4@+#h)0)~`MD6kXk=I{JTxc`B!V>Ko}#9Jp~>{k&$LG+YICc*kMSYo4Shu`)dY7iU5X zRa~ETOSlR))5{{Sl^V4e)^r%mp+3judXcurz0UDtj6R&yg>iR?fijK1Gdg@kgjY+K z9|Nz=K0C8QF_0At5pVB-s|zIZr9$`O!zY{kyDoq`w3PAUn7E$Cg_JwPx9A`$*!UqW zY3+${m3~WRC3lp83m+!R{I5jNab3ezoJP_;4w~503XgI7cIeSe?S_FK@<9`wG8z!C zOl?G^)fPPms$BtX4T}^L$!=~_@ec0DLUa!5*K&WN;^@-NwW@Mh;k;t~zT9)&sc$5u zD|Dw!EOR`Vc?Iy^dgyog=~bJAh|2!uC|NE0?VA$97f*$z$5)_Z68IL2cyx(ar>W95 z-1$SWd1TZjU9OdwY=F?&zcz-4Rjk%$`#Qh$_HG}YkQ!mai#%E}q@AvdZ&u{T;CCAj zDuEqY!O;oLxZfQCfo?Yb(+Uny91qM74AM(58)G?t(108qroX1@Sh(2m%p)X2<=e(J zAm9?@6W6c<-Oz0$?i02t%Z~6A#D{|xml1HCF*8B5^U%ZUToJv=$%(YGV$*4l`3?7T z#o~V(7W+9F$N+as$=e}N#zEaGlL}M!NP*K{l_RGjn$!KtcJy=Q{S9gwE|gIga)Mdyyu@>5+*L?z?xi#BWgy^lysy zyk#heEhfRP!sg>sS8h5v--V&v*>V)d`CE}=C)5nMLJ)6Jv-x-4M~upEx?pZ5{Q)U< zeV*fCcdAd!Z<{_3EHhi2r0ahQ85A4(w@cVtWEly5izzef08PmGyG35u+2KO0*Q%Ss zY0^XVtf&4OEb=Kl+qm%!;2nt($Z3k5lTw91sPbPcp2LEp#nV4l3M?SS7ku3pzK*!f z)m!u_kPXS70ABR}=(N$zq@ob_;VcJ1t+pNu*@zgXm$d3_kK(t$q*V=ht!8q!ewOf@ zQ93%u!ks_H5&_%oc#9f8B3*F=g%lKybAdF zL2XwE!yD-M|H{enBCpTG)0~AZsL+aN`0XR`I`lbcKVRE)M`ooBUvvS_g>y+wkB<8_ zR}7Ga@Ag{eb%MnDX_EpgEmlRQcJ;2tL4FBVurs!IBQDkWrsn>7gs?cjugnBU4#R{r z(MAk=o_QC9>Pd+uk_RX4E!Ss!UfvtM^?2RSSAD=XSDKwVlk`x;h>35n0c4{O)0$Y} zq2+*>J2-RNdP#>McV0_-Dzn~E&h4&fw99?y4f*@p6#2xH8n3}LNVVemtsY-H#EFNe zB}5nGQ_uw#&q3MaJK}$5apWaTS?wjz4gJm>ll$3?A*n<9w9g*gAvJd`ypc}A{FTTa z2p3Vm(R@wu4aBF106%6bZQ4lP3Ium{>5!O=+ZvNx-Gyj308}jaa~TtXm7;Fe0G?Tj z2#*RjCd*WDICR$0sxOI=E*pbvPIX?6s#U3x)T_^-d{}@OmeuZ+RI{~-wpHOdh>6l- zinTLqUbWhuf7%35a&6`DJ{1Q`3y`Uow8nsqcm-{rYI{o(*%PupK(NUR6;fNTKe#pb zufMi>6v=g?f1`J-z(K_#DD|R zT>sh_7;vid*#E4MZ2*#1s%R4Y;~owt7r;#qW^I|S7cc9wq3kCfQ#&X;L51YT_vJE} z>7BvM7NIrlBzQ$X%Qx#VLwTOWD59L5`BGmTpU{LgkbnwpKhUm? zRLQpzl>)XmeGt&JRSRBa9Kh0rG$-e61o-?2Cc8h8sim|9TD4QZtJsH~V&9*T;nKo?Tf^l`JX?o#j^OvdTonq5M+hWsFKi8D~2NXH4lJU_?1ARMh{XVDlgO%6$ zd?AHm2y_(R{?=PqJxBcP!?Mx@1-p`3a0cE_Lk&)S~~p`p}Z+L)U|B?-x+c1nfj>KP##F%gB_S2#K!0{9}B$&O_4rLs4Sx66mr zPi#x!aih%({Z|j-<-k`!)}>ce*%C@M6Y!A1(63tE?9Y|L($ZdEz9<@0%V((0zcYaJ zW>b^9A)3s1hGw4dk(pPoDl4H2(V~O?uxil**~=!SP5Rp zTg_|V&+2n*RWdG3tb!3$0=_Nv1go}*yq9_GyI}Ycnt>zycG%b@Z5O4sGw6M_nEbOf zTAz5A85j|0(GU0!pJEM#yERn|Po=k3U7phKzC{Uw^Gg7^_|CFAY(b5O18*j!C3aTj})F z_ph$^7Zp05=**TGf*p(Ex%BaL3p1|G#d$zrnmG-c(l%5^#j`u3<{U|s!rWZZ=M-is zoQ@2nSHqloj3!fx9J7UFHaYhQuH8F)@pn$Sju%<3oJ?jtb9}!1<7;lTdR3XBs+C%5 ziEO!t-@PgwA{mF5^WD8{T;W^}#PnRb^W85^W!qkwDvKD{&`k#3O&8}R+wI-4OP zU5)UVBO&I=BYw#p(Ux>ObBn%I)3yeZee~0&q3hCSH5++PyN|ZBp-QXrv8mQJvlm*N1=e>$j{%N%@MPhj*~ooK&>?{tAeaDF z-Hk^d8#PjF7x*4qA(jYY&X8&@$#0o5NsET$5!0vZoaTDP`}r6N|H-kb}tw^bL(`C**jh$E(d4`ko16Sjra+gg5Z@3U#%9_6K#h zHvh>K245Ng%;T#I3&H;UJ&H6fFw6u9aW_DCpO^IwC*@0DcBm2O>@$O{;~g- zhp1K`KP>v(dRz({Tj8?}m(cy60z~Y~{8?2ETSxR^ z@c9F*4jG||M2`c0IFB|Tr@kSx;>FFIuXZoJi?~zwlIf&sX#L?LObvClvbh%3Q@~as ztX5>z26UX<{ipBhY&n56tpF(Yp)Vj*kMz(4eQvfYHLRZ0^rsuxP|KYncNa1I?YH=p z4^93xGd^9Xem+(x6j15lo>)(h1bsSaaqd^!Q6T8j}dFNd>WSH`&XPZgASOk|1VtfX+;i-*aDe^?AbXy9ey9uqK1 z6SawuOtmk|uAc4AtGL>*SX|Q`n4j=^*vnB1p|SOP zElx^M*AC_&#K%0L0;d5+YmufFrX3t0h~p(@W$XmGKL*XGK0s?NeCfrHBK{0g)d_rC zB`xW_2UvFY<(kCx;Sufv{qNqt^u^EYo<^CTf!D8)Qo^0luK4=Hqjk454P&d08wxuqJ(Ie(%BseoWBnh{hK4YESGA*xqqe04b;eP!|~I4GV=t&QiFTB}omW zPeB|G*188#w?z}2do!YW22;U2>#~h!jR~8>f<=dW*J4c$Q%4Y-OvLMOr7|y1%)p9D z-=7{~n9~x=58yO39^X?8{;l;(o|X*-e_jtt?Ln%zmE|N!M+f*b5V!p_7y27`X1%~R0n>jBF9PyWaO*)vJYd0g*+75z zUlv*eb!PW7r|ip5>4dI%bw!#$siiM)*66%?;@|@b0oJE9BVlzdrf!=)b#p<)HQXSl zztVZlQw^OA1G@=N*I`tJpI(h@QAuyN(oD4)$NqD=5k{01W@i8|T@!tP8r!q%6(wpg z{Q(x8ivbJm5_Re~nwcf3PqiC%ko(OXoJ27dZWU3-MiecPPck`9OTw*AK?W_T)QGng zSqYF>_7=uGkVn7SiV@J-0dw$^8NXBB=4od0XN%jYUV}5J2OB)_X_S-}iVI7&5?xAJ zHxHKD%=HW>EUgagmK+>_xw0$o8f{%-9_^=>BX?Z$z!6p%fNbqgnbfGeI1Wh00mV@4tXnmVOLZk=IO(>YC84qU1e@kLm<%-KUoq%C6ws0Da6gzIIKTkzgoq zp>1^&9)H%~UF_rC=~ukEny!jCwSYS=;UpNZ=BO1aMt!SMYbfwttmRP5uN4=FOZa2Y z|Anb)o@|S@LPlIv(pKz9wC@` z2~Ufxh=3kZorHqhaK44Wo{GTeV7+2uQ8b}2w%p^@E4$E5JtZtAx-#q7i@my7m%|6A z$0OZN6azt5@9426pod}x*S*u2w$gA;ggX87f9VDwI>G$e4KC?J`r6N)-Wh^<*q9c5 zBqsC^?e?=_mZ_ba_nSxbZa~q0c z)`FSkN@#g}Zp&p_`F%(rS8i`NfKfApKx`k=xYktB-E`A9aZ!7J?Yetx7xNF#pDZD} z+$vrzl^rOb``Hz$fZ(xAcu1Wu3t! zBuh@)5ABy-QKanN#XBP@SHafaV#SB5AWzipq^95N&^#OHjQ{;gISJmanluig;>YnO zXmL|9jA~U&smcXkzgH103-yS5wJ<-v%_S!B0lM>2X#qT9{7gppB;b>wV?xvJ!g}3^ z2t=rxg)na02kO&yp0zm+LLi_-+RhB+4+l9unInqZHgC=m+uPg0vIDkDR(U2xn#Tz@ z*WuLk<-tCT^zXFzj581V2LzqI|I_l-_oX4j+t{=5+kBvTgo;CkwL zr9t}noj&?oTzA-isEctBJlHZ(7^X*1yI_rDB!P*EKxl&Q6RUR{FHP84HbcIuRHLVN zOd?nQUNuBOtWm|eU}Fwrf4p6_`lkyUYIw{bi4P@&ONs$u0wO5wCzGL?tnlywKLDJi z0QiF^*s%F91_6*uGO3;`g^yX1nSf(1qC;{c zJg8WT$*yOMSIdM3%4hE!uK8F)@xFEdufqm7l~56A0#+((!xfGbV)3D_)J+2UOvoof zrm(^kdeU~`uNzi-mpqR`D13$H6e9eGC@Ddf)udf>FCMH2ukZn{?$;U?Rtp0qXAP|Q z#Q?2J&q@erQv&LoN@Try_|tvB{-4+_s?W&Jez@^6uf&lz0%E{(^b1&*4%T-Bj3GbR(P!CFLr{niM(D%9KxZ6*r2muM10x86AjK;*&}d17RFok z+ACt>fKLZ5^H^LIya!?fK#PL#d*EUGkGIADdBH=RV$Z6xTt#**P#XVs;=>rHQ%OD} zoMR^>H9WJ{7c9Q+IoGuQ_KSES2)Yx|lvZt8Q<^_wVI{GE?EbP0i0e&N=^C4X+@e zdU;=!`XJ==`YmW2#Necz%%CH^_pd&WoUOAAP%|mxx zA5peXDhZDaN0qC@T|@3G)Tb|?km-|3#zR~?V`AS8s|R;RZ$;eU{`fjV=uHYsPP6nG zj7hMUjbR~>7r<hd}qK*$YjYk|(nXkQ5Qm?NAW}V`2?WdMT+|s0z;n5RQWHQC=RO zsuM+{eE|g|PSru^MO0@b+)L+H7W#c=G2%E60E6!cE-Nvyr@jS*SQ*I9PY;Dr-gr_s zy*EnPpooWGe^PM#sQhMcpXId;>G0nB%l5V%<=4O7mCO?wxa)GR7^w}1i!+DlL!$<{ zipT$yGO(h@Lk6<=ER91=6VSI}5+R!D)v%xG8*?oqr}36n$1mY{6S@joETc0=J1LxD z%gaU=C76CBz_8%2(3bM>9Rn}>39yQSKi?WZe%nkg!OkCW?Ocd9r}v!i`ZD$5Fw7YM zoxu}-quC@eMB56=SUzxh0L7R}yZA%_Y3*7t`}7U?G$LEOL3^nbmlN-f+Mea|R*`#A znr{s>JT#|Qb4deo{JNV>UG@vHV_Jrdo`8abURUJ1E84Ky?$@PS)#XH9*1I=fnTG|- zxP=E}IqvE8whFTnc$MoguunZnrlyKJj72kh0nHe2Ep08`1`VsgnLpilpeL969*dKJ znQBEs;V77|S0o$+lC0-&v>8Nbmyj33n^rAlP>A(Y7CpATv$0t*KFozmazO>GolYM| zV1N~xDax+$0tG(8t%>t@zukYX=#sF_<2q8^7C<%)8nGh(oo#f|bv6%^Nl#A_lOGA( zB3hPr2JNjbJRjW*CU58DS1MDcWQRKgw^M)Cf)3zp{^q3e?znPg&?dRgeJRj(XWAfe zu6ty*9g{~-pW(LTKD1FwHsAP?U;DlG!zTP67TA?3XdmS_K0g-5=g(Sh=}Pq0ri7Q| zSJiEwnf*=ji=YC65l#kMX|@bQXY*H<$m!yvHeUJX-9)T(&8 znxA%`rzJ(bF#kpap$e+hnVqfG(O{Mdnw>|`;-M@F3NswaZ;=#-({fOBCnU#P43N)?v;E{(wx2GR9(WhU;8wr zLcefH>GX0H+p@m~)|AXjTU)ygb!H;rmM}(@d^GR)a*5M;qf@fh9V~Ie9$rlU8hP%3 zaX`F>Ulc#OZ7%4beF~psZ*_)>#RNJXBV3ZPQVOVw5YuqrhJ6%bj`JIGkqASQ^L$em! ztxtwC$Y_Vb)Sv|u^1IbDeoNH7+&-j9^7juqO%q$Td7rx zfg2YrWOlbwgFf>L_)~Z(dyB)Pl;P3swVdbbHZqM`3rUFRn9SoO9t&(5`V{Y98(Q+vv+bk5Zb&+4gp!VCPdl9oriS zp!kih&W8Wde2U|P9cgz?Ip}*?-OjW8^2H#EUm#X|?*wOc=Xp|OQn-Her<^t=aly~Ux? z-TBdNPpi`14Rp|08N*6)fle=Kme$~}(Y>dMwAGqCJJV4?6Ma=D!*kTsvM=m#wrqYc z)5HYM=vjJF);_0h&xfQ0ZMOt1&F-v+|Iu5w^ccMwsj;>ClXgo zIP}Kq9O2*vB}JrVHltqeT?mSa)bAjkx2rj8^xqpyBUN!-G?~i<)XdUQ?%zGF=ZY~5 zhxJp@`}#-vidkhzFbY&T=@pNTo_=Y2wi)+97*^rL1`@S{7))bAT5Qc=#^}j}pGl;% zUWWu{mkAc%Q`3_0%mbnWU|#&%J-p8JvIeXJB`X}1eI21kb*ySoAe0~CqM01GyCP;$ zeV&EjF>{x8P>6*ECQ-{jSD2Y9eHTX#a<~2mUx4bllR7cBSr@dm>ed;wL$0fY6_p_6 zU!uPbbDggb80%D8>`&M!Xb&tY$r+NZ-ILyEefDH{aAZ5dl}r$~EnHhTYWp_2e(vq~ zyrc4NC?)FGZO+0&Ouu+3%kF#1<@RJEI!Gvh@_ZD#_i#sfXWJlfMIhClwHCe?1j`okuvj z`Q&VE`iA9apVYDSPwsZM_r=@paM&*W%(3*MtQApy&5n{se0NV+yNd7GrJ{qjh=HNE z9aDMtv?Ne*1udm&(?8n1HZ^l1386 zSUf{`~-(Fxqu{`@;PyD{tWmkn-E@AKl{io%_MG5fw!7 z&9fP)b|sg4*beu_QNG@r5#aUk9rZ~ZUH1)IJkdn1dv(%}I1n znm8Yg_2qS^#qJGT zzr=cEa$3DK^k}v$M|1pWb;F&rjcTXadujLbFqU@o&@ESq4tv)-?Emd>@DBSrwfMu6y9aK3$b6$F0S&<1})FKS1blCRynU5Lio7!-o+Pk*V?4MEARAo z1l1R*hsV*i$n(Dx6;f&p`XRYTXusbH|M94Y<#rGIZlLnsSRL8$&ic6W4wD|&pktd5 znJ8BY<@teHHYb$luiMi>+fU}CZGT$s&G7ElSxRxoiMG{k&DO0f5>Ffu>^2CBjbu>& zyKZMydD|y4R4`HQx_yu?vF-=apYpj*?B6$bCsh(D@0&i&f8;X_EAK=F5vPV|ntP5> zc1L$MqdVKbj*p_&Cr8zrXDum{8>=Hv)NJEWetQaS(b5tU>d0Tw*j)zvb9b}Xh>IMro!SB4;D0oOB7ZSOIiu(#r;1x4%Z4J*Jo%;It4 zG0`RhsVxsRmGr>~D~$1o5Nh^CnAiKmlNn3Q8;rE1j*-?wc#+{2CIW;oSwL%oFFytd>RoN7mAE)u=!tNl%-k?6YXvRj+ovYP7nw^yx*8!H86wwU zxs}RLgwc2!IKV)P$1q5;5Za`}nkG_40t(fjMORUvOoD}W%~f@)SZ;*L0wJ)4msnCU zWMDBf|7G6R z{Gxkce-wTE&jcqEjFJu;8+*cg>wanKm6HSRcJaZeAr%&9N+OQ|kM~@jCDjH~0P+zOj0l zpRcVJ_JE#D-6tTh#wr*|iJYnx)Ppj92$C;)@k35x2<~P_wHg&j@r`>j&Rda6`PH)E zsJxYCU3{XM+4kxFEWVmwV>ABA$;XZ)UJEz(*8a`y`MQ5vNeQoXWV+U+Z$_@0`yS6L z`4RZa|0Yk}@L?&YL&;sAof7J=3OP7a6~()NFZ{;qv90T0)YUk@TDjE@TD)*b(V z&{7maZ-?~|Rezj?mFCM zW3jl*NVtp6`1rdLFzg9aWl?!G?{eZ}*Og{NkwSQGlNpKKYFs}qG#23|TjM3H{T0u) zR0&I>(&Imd>z7Wimezh@ij&Z-=}2G$L3SY@GD53`+l9tU1z<3oUArC0Jc zx3@PGe~=?%WZi}c!hqL=4#f+R4c#K6!zU-fzjJrmiT+XjVb0APup*J$AH3+Wn?H?f ze-urg-jWVaXt>cck+HP97)T5Nm~GSZe52Em69y5xEj1BxZ=?$lbSe3yJG^tEIc#m} zofzaB2|a@LUUZU745|kmk{_F#Kj5V=DT!GvLUTG;vH3;3d;G!(cKo^urQzRaz)IT> z`u!pD;#<#H{MOiuCPBXf*z;JBwy{{%34Z1)f~X$xcml0X(jgZ7R)!e@L5o~CCX@+= z6);QS>nAEt(vtY%SmnqmaW4&;#3@3P<2}^?kl1PQqcp5yRlyf)ByJ9iK-Pi|UvXSG zI@jqN1pv|zG5Z|j1cm>C_y!G6^yZZvP7SqSlJy*UjroU?!%)jM>Q-()<;w2Yb1fjK z5?4|WWQz_I$qS$3WG8&IaMbIr9qoNp6!|W)dG~o{`9{iHU`9qi7Da37MP5I&J zCUrgtuvL+G>Q8-gjSrmC04qvWs#E}z%X8pdPsMV>K(BRH68#ux<9yZ`;V;O^;y>4` zgA$Wp*G#*oT<}fF$uXV3o@X}M`|hHqn>R3neXo)-u5tBMV5D(A%H*W=5Rh$Tp+2GY2IFMy6m3Z6L=i~Bm-8!2tbqg#+ zmFfVlNwNpk04YYFo|L1=K7INU+_QHXuL6SJ=@@g{{(oG32{hDU+kS~`*~XUaYj$Ia z$Zjk#vX+D~_GFh`VzQ2%WX6)pGEw&I#E>;2OJpzmE?d_4KkEJd=l#BO=5#up%KV<+ z^W4vUU-xxg_j=oWksZ))p_jpM$dd>~Fm;4lTho&hxR*-!3#bBLqt1@PBt2lfb3V0H z7QqjfeQO@t6U?M-0)~CO0UiEZEmuK%6fi%P&UgU}5==G(dH%X2UnRt|TpV}WDxIqPiV4H!0Y~X$#krF5mgij5Es${p{%Y<&?a{ zt;cY;2bG^o^-#*jE_|mPqwKgCnU=WT-6XCM?qru1eW0!HtnqekeJNyaJ zYb9sTzE(QylmV6+%l3>O%Z5&41(y1a_7O!dJu_zfio>5(bx*X!1(4hJx;O?7*v8Us!a^(Bf zeC%?ZH^)+&MS94(o-zJ^9t3?a1-TWZTvlYnQbwwRqz&S01l61U1=Of4Eh@4#DJoBCmJ8t=|zZ(iM&u}9f*VcxzLh6LIC~B%5%uK<4)w$o4D{IZHvv9-d`1Q* zoK!~Y%Wnirx}xbSNa5A3yPe%vAUl_v2nIz&#DDYRIGhM*zyCHUWpzU2ELuFwzk5p7 z3loJHotvd(_iPUUK0FTm9-(c@gENETFzRw+=X811dt7AC^YWZ5jI3uqF8JgM6AYd_ zu|%P5@LMiyBhfif7Z)Tqm^BCnI`7(LNW0ueYG`U2IXeqeu*qYIa7;mtwAgZBo3R4- zKW7>8t@OWSAuy6mTohCPIrje_gDwE^bj$x}xu=T-L$7WSEm@TTSEreKQ@}4TPfzD8 zq5tbT^!X|6ya{p)tZ{`@R~??P+^x!nls@1zJj+|Hr8c8dglv!on`L|Rln@8Au_kNg z_oT6Vyr%6%GhM~s&O~ffm(&F9Q<&lC3FHq$~i>>-&sH$nz z$T0RlM9e00Po^}e&oTiqiRo-%Gh8J z(hnH1RY3wIOa9p49_`}3XMJnh@!~uvT|g0(BEm(%v|ykK*a8~>WhGWsE4#Z!gtk^d z_*MGygAv*!ffhsXsMa%*i8UP}&~Z7SK3*6+)Wk(;xY zTpm=}Vw_MK0a6s25NO?9z=K$#9OJ{6nNi@6_S2X;?;;sk$Y#C!eW#qHf&@O|@tXNize1Yi3^Ng&N830hx>g* zz7q;T2Wv;Enn`-R(2nDn6Xm|{XP;h2y#rC|>N*29q&`W{c{Ua;Krk5F5(f1mTo~GI zRcr)5m8#Ctt%_FZBfQnyUko+JgM?iZ(i^c08>lA4Xu!*U{Pz5^@cy)u9CU$DR zW%%!VSRM}o8($(q4Vk_TkpKg71tbj^P>$Mgrb~l-&>NTho8Ok2%ed^nZq6n!`unx> zsIBYEx=E`tdY(m1ycROGX@1s*I1sI_tV|B3GeH$B+B9EE>jY~tqZdK~dY~Sp)T5xq z-=983K>LiwqI1xWkB5gOrBf5c)Xt6+>^6eq6$p#3F8T{3VZJj?S(Yqyd!d-3;w81?*;&WMBiT%J=si~q7pUEBx!?wU;eMG4@nXY*j7v*y~c5B2tr{EICy zV0y9X#hwroV}n9J)QQz|e2$Su3G`#|CP=PrNEE)c3atFkoYx4G|6X}sLiyA0HoF5l zV*QrEr)eUHCtK=bXvcqV1D^MbgG!KS50!Uw=J=&;cxkBt+wk$Pnr0h~LnbBphgJpQ zim@)1+mK+IW>hJ7S=EJ1Ln%Z!0T5h)@#qO~Z^5_TOw0d33jcM&lEYlf3AM~*RM)FZ zjVUEGOx4nHj_IhE3d>u=km&HNS($dVl+&{huID`X>cVaIRfAE;h}mphxwG#v^{iz? zc=*-TpXuHdDQ2;vOiK_7tEU1)&!72BR!oLo*1-|% z9F9~Qy=>Dnit#oR*efm?_C7!8>INxzj_B<^rPEHD?-9;@J~kNI^9a`5MkhzK{u9aCXeP6hi zh7t1f_ZR18S)~4lxR?>v4kMjd!_IW6_E~Qg{jS;VC9n1+$!fFvN z@Fn0IykS3Jip>=!2&+{^$}W;o5`Xl@Bp4vhqK?$w<;9&ZUt0RC<>xB9z;sl@FUZ<- z0DtMf2mH`H;IQ;@Q;!Bgj`)MEa|`%Vm-X+aoMHDOI-A{}_9U$09RD!+eV%X}#5ecE z$b@IR)Y)^a>UWVVhh|Xm5)tV{G)uOgMA9)McEfdkg9Pr&%EiS=!^;J+?l(i$eF)8o zb)D8~-}(G(7p6#!<BKjLRWx|}JsyJ= zG8M63yR#M7Z90CDkXhW>X)*KJ2I%bf2gyrIr`9LL;H7fJE|J3FoWkn1KC8oVNn2uN z3Z0-W06`UyJ|R=`_kVgh=KKIk&||$Z3OY(>r`O4QMA-jm*`+$?xcYu{u`u};^vW(B z9vp8r^}99wNf-HrpvSfm@2K2tVwX<&Y6Kh;vO2<1+bZTm; zlc)ppeZa4=wHB11ZGeg;=ZK0^@@ZPt;mjD-2OmG?k|m?HO~Gy~>4>41kDi1Cp+I7D zdpFMvS%pM*C1E}(KpsDoyhdv|I7Z3wedm7f;Y7CShlP~HM6ICX-#NKFQJq;s^}cTL zQEMY5O1fAmikk;^$ZW zO~JuaPn(|p@iRkiO!!N=PWU;z+P-9FCS-bDX?*bf<^lvK2g~t0BVzt(xW->J7ELqW zf=w&Bf=%|f+NK;Wr!fw%f>KXG%dfw}aWGTv1oIk(h9FiCB>ABMnzg22sJR~^Zf+z&#~SU(L&mIWjX zV0^i}-SClsPD;@kgZ8pkKaAF#jSshG^)}-|03jcjxP_6g^kbzYtmv;%dq&S{yqE8} zO_l(l8Q-IDwss*-v|N;|ORzcV_E{hEU~q?7a;X~D1%jc|*ht%!>uIH#lh8s;YX0oWb=2F^0Ty;1D-OG`4M*Po!HIyRNgX zxG%D;9`R9Q6Q6uZzcG6#GyUZGHEHLiBUA(5r#e2|)rm42VjEGa%OpKAQ+}b-F(~`< z>Ux#nT(dSD@Gq61?bM9ww0wFhVD{sqAT)-uk&33uJ0%+XgHH$hFXiT;brA#^ z4TR0)AcL*gOhDeEWDY`3MHj-buyGyN>D2OS9w-`?6OQivd`x*djyDXHBb5e&>pa zSyik?Jj?RYMKAImMuZ>gj2igE_9oM=idod9h&CQrR~;z}&!DR?&?mwn zZ%Myi^zFC%@&o6JT5h2OF$e8B>YB97<^ZI7Q`%Jc8UgKqArBvj#!yRB zYIQfiy2MKsNo`bGgCVBhzVsa4m=kS3?0Pu8H{8-<(jh%13sSnt8uy z->p`khrVYF*dJ(@TLp!hh}7EU3lu?HIuB|WHOZz=*w#x6selcPc|4m!I*^Pj>nZ(_ zI-{|cJxv5tKl9WNX|cYO z-i+!R8ghFFC7-^?O!st`Qm&LzIMdoOU81!Z=4Aci@;s^AngBqvK&8Bq>hTc}!zFlTz+9&qXAPw2{nC)`sDTuc9%k1X)LZ4; z92k=8$dC(K?L3;S!uHuO1BCu*7lqS<+1`MXrqeo0l)&l%8Qd#WbMzHHW&c~gL1=wf zPHdUrzwBIli*S2uVsy~!_iop}g}d1q7lfGnA*7D+Ixjse?TMP7anPtRkz`cRHO%K_ z?X*3Ay6)yq4rh!nWcfDmK0XhpX|HM?Jplj@Vo5S1=sd47h>4xVMm)rq8MA`f{;al; zFU@c$vcpJM(nW*F3zlUi(2dNySev=7w>24F=p=TIVjyn24rhpB!`nc7&*N( zE!|lH+{%dhF#|y?2$gKHpL(_@I?X9*{N3`lxnOH)(O|!eRuLtk_a1$FH}A#S5eQO4 zye5(T7Eg7_;dy=6m5k3kI-nG+_K|r}@5uh5>5$qYu>08f>7b^RM>pr{5uOeUBGjwk zQ~&sTt?WFx>~&!|Y-cx=6>?u|X%6YVlzYJ~NMkDCeL82w0sNWupYWa5e+79--S06c z|Jo)OJ8(a%8n=Ve2O4pGk+=gYiQT&ENEu$aQvi-V_Kh3A0S9U-(v#o~%#S6NHu<`F5 z4cP!MGB%Xd^st2dbeTk_vum4I3@5=`;%@K^RS09vf3P5dOOIKx1D;ANTa9GJ0yPVt zyYqMH&$10gi)&9Ty--f$0Q%Uxi9Pf<#R%U0{2t7X0M$|t9I1Nmhzw+u+!q6Cvd3>$ zg#+t4m2dCv7U;cFL=YI!9?-dYuAcnet6vEoFU*b?oUBwAOr*ce;!!-#pEwGYIz@jR z-lLbXPxTB#P6w;ZOZ z57LPl$oF_P0Yo9|K*}%$sROF^>BF0oJph`LFt9&dJeMjHY3VRsdQOC+2@)9X;1qqk z>!QbqaA&);<>%5Rl)w88BZ+m>U)FwSu=U`bLBSP_%1gm{mX&+&l_`WQ=Y`d3UB7J` zuSS&1Tkao0dg{=VOwKzi5KMWG{3d(wesZbJ6A%d#rU`c8>d8MqiX=O032?lIu=^}G z-CuJHt8k#e;5gM-?HM+b9x{4CFw#T8l2e>hj8p5zrCI{d*{sg<+){`%Dl-(-FcAhw#?QStqLu_?w04eE<u=#{E}6_vmdSRYyc%sZ?~|1xm@m24dJa8Mbk4sAxw+G>=}vRpC$<3MS5!iBid3{Sd+0GM?^wz&EU zny|s?O8eiW&qF}jJi0XuKUe&d@%62Pruz>FI@^xuOLEg+lswK~2l`z8Tf<^%G@=i} z@7n0Qn!T0M%NQw{2{}wHp8v9HGQVG0d^fuKQnP>rZA#;c=cA$5qJMVS&pM%$_D@fw zU&}bfFo@J>zJ*z)Gj)nf^9H0um(n>prh-^E$LpDZ*L%Gvq&WNTeQ`E~SNrQX6U%k_ zZ~(FPUzv+ed>+Ue%6rqD4#-Df4!9U{$V1c7|0s4i@!{R~l-fz3QXa-UKsogCWxiXbovR`(tb9 z90lx1bcBid)gBNqY3t4LZpTF73r#=pk;CD-D93TlJiq5JkFWn{h1~8RT=}!7%2PIE-`l)k7!B(~=~w7QTtTQ*0xca1W`^s5~H0}EwG zOgwa0j4(6UCO9*|`ilTG3Ktu?EF>ykvTfL+ycQ(d@(dy#Qa zWi^y%O0?2H6R`gz<1*d&J2s2~<6yaHsEaa^i;FgF5F1C>8_q;g6trpiNxAE3Rs3EM)yD7a(@)-8s*KI}VL}^EL=cdv# zk#yfFVuqsEuh~&!Ll!Oe?q_|Us*HDPbXe7fzm>MS5@tVQPtPJJBhuILJzM1lS&2zF zuRS27xy$TC+m*Z#5^KVW{@FhquIeJ^pa#9G+b-i(UpRoXuk{n{4;eKj_!JVjmx^Yp z#|3{=V3VkcNH_PNf58%t?!y$%rK8MABaoYw=ha$r0scu~2jxo8xDH*_iD}(zcy;0G z->yJ5|C_e|fH>h4M)yE+@!bh=yJOf#`!x9vKZreqJ8|W@=6CW+ODPyUK%?U>Qm|23 zjw>mlQzt}t4h#(<15MU*L?f!MRa(%b5O3r)3RL$)yxHuWlr$7TLZ47-uB8rK9{?IDrqU75>QCm64hY_c zXvu5`?5%jvf$ygKBZpAOe*fBeLO?2rPWX3v3`$xR`#K_Z-s61|;clSXEzCP33I&%w@=V!(HwBn{|Y6z3K1wX0k$ z@lM`;&0Gz8po+9+vlcJD5oULZ0dwIQL>WODQ_!0X*qe--dY5fI^-`^7n#kb0J z>QnloZI!^L=%PG*YWCka=6?|6Mj0#-jgj=IYcxIY^@A+SH+;g*uOrnNT*>j17u~zH zzgDe(gx~)jp!c$VwMJ&6XBIypV-_gT_PopDQ6MQCh9slLMm0$BE)XsL2^IA+SdY+V z@i6_EZHAD3?|bR`EI_$D&?Gt>{YhNHoYx{z*p;DrBk~p2k!N5rtOb@Cp1bNhXm@X+ zD>J>JN?cW!b>INK2g8qeaZD7^`Jba&V;>?SXP7ZLH5zuNwh`Y7wSIon9@w+fUZ`t@ z&9K>?xenx#lEb~&-A0O8H7&InJhi%;Ly14&!}6uX=Xs}yYr$qrdI9{zi)ZjDm>=0O zqSO@f6pe>jO=Qfmw z7TGrZ6;~rgs(3W8?`Ckjqq-zuubr;&AZLqm`;IRj=Sr6vVrP(8C|1X^$} z#>fV`iT+eqf3yUksc^yUN@^Ca0CSGcr)XDBM~NIwg=^_UsDXfrw_O&iU+EaBv3&WV zt#lx}7X&t(ToRn(fiUU4m1mSFrv5_8N*D7`b$c~EDL$Tsy%BR&zzP3QWpYn>fGsIP zH$!@UZPsP1!iokhJh!9)TA-qn7iKMwqIE8AqH4lAVSFA&_qkREUg5)jfSTs~(eEX$ z=KVKh&(Gv4wG%ocdYO(l(4xy_#`EHBLVFF5v>W~fgzfuvcZXDrxwxRZXtXG(;74O! z*(Z~0f1Ki;+>KMI;BDnn z9&wM-7(zKtu1atlay`tA>HnC09~r;Fx}NgZ>%E#${kQq;&a5Q)mubcc?Z4wZK8hvV zEwuJle!Tihzgq}G%cEJpJX|Ke=j3}yUAjbvwZ+NEIj3A$a`ot*J6TX`YB4?gPzgESm6*`f17aGs>+Xm8btfE|pOyV3f;J7974h}mPLGstVBnN2}5 zg6^(Y*pc=p1x40q&eJv_QzUMd6}$%!G=M?_@(xjU<=ZaLGHHqLrOG0s^g5ZQth#D< z?rq3GIU|>-%_I#bF}1M;az$@LDh* z38!}eg7g;9evEQ7;tBxtN;&`e#cY(0J}9X#w@|ypx+q&mL`FVl^n_Viv%tMLHKlB= zt+~9H2f|fgIN9Il?KF$%1{lVR;~WNy4HcODEUwPz)2U;G9L_i3{^b9ff3)p?uaU03 zYfg)yfyDKCftnrTUFB3)K1Z< z29>+j4N~CD@YcEPZww_9;do|MK6Yxr9uUxAF64Q4k|m?A}&Slg8m zWaX1Y-1Yq}29thd3kge~m2FiKzx5uf+t5Ce$hr0H^+nbRU@JNPIpupXr!TMDNNy^( zxYM+LsvzPqsfG7Ey_7>QOS-ttWtZ=zsBrtk?Rlmo^!m6ja;(y6lbVL+wzzm1e6iP} zhTQ-Cy&_NvoHGf)O5!|b+r@1<;=Qv)le~!gGq7BNvV?POjF#{bL|_rUaQ%;G;u`j~3;=Y;9 z>6LJ?eI%@hWw3ycVr4A;EeP}NYXjii`MemHDbjVhcK2=m=y#>Ot0t&3@?x%8x)2bG)Yv7 z>+~?G$qn|rDKD<4+-&yw#a7tlc;_7>v%{VPhv+2agGk(Ot)5~w)M#xGcQj>v{Frp{ zZJNDq{mFOp%SWrlR<-vF40$)ozG2b(@ z#1&sWnfM|x;p}w3$rNTS=qZudDuuZ%r+v;5$Xy>8dx}VlN35NQ+*I&Ri%TNWmn=2#YEVKp?<<8D<424`e`+ zYK9D`tDz$GwVhwHvTz*!D#{Wrkdy}OS|CQsbPnd@?R4YqldP;sShc!p2NdaLo+^73 zS_`h#lT{>SkkNjlKt1%Gu=soPU{)RFsP@&8rYt}{YT=Nj0tY_)_|gouszJ778LO8- zLPCOXHWUnyT?6POt1Z>pqSDzi$>$zIoN$L9Ho2jL0aZX?)TMi#@)|l5Eo51^4Gx@A>_sED%`LckQZ&=Vga67$GCVMXVdz&WS`ELO~sPsrfuIIB#IZ zd!sQL?Ab}jbb&2eN1(wVNUv&Z_2xDNurBR{A$t|1vujg-gOxwp`3JyO=|F@c zOm}^5i)eJ(qA(`I-P&v!jgnB5Lq#ls*9_3Rh~?@)DsZ@ zauwakGL{o6In0|opO5H|g(=-(O~IdIuPxdAw}pjG9|VNW4R~agC*TRWpv}o;T@`-t zWabnFVJ{b2uE&4Ax<7!vBi3|NMLcWE3R7;WX%wG`V^c;%x55bdciXe!wCIUWe%Z4Hm4 z7mZpNkF>~KsT@(>vIOwP#s1S7-g$W`&~b6RWKc*<(Ja4NyusI78emVF|sQkx4 z^nfo0gIx@-GVT3Z)uT`$@+R2wp>+L>C7?~~?bwZ9C=h@~ zwi!F1n5Ly;A?8LT@Ti`M%J=#&qUY~ha~+j~#wr~x?I&f`-%I_WytdlfWHDM7XTiOw z`h?5cNco8hM|vvzM9Ia8rs<$Nh$Uzc|U}t#fSN(#4I{F#gg(^2zDZvT}z*y zMJHSQ?E~r~XPVv4vRJ+5A~%S*G(lRIkG)@U#kruxwpFjK$; zGY})7rNKDicJu1nE4HIsyoO~6aV4#D{BSj7FVpagdsNOdywK*J+<=s^JP{pmD>af9 z-GoTi-c^rwvUv4C=A87A%$UF8(mkOzE^$n^P$juP`8%VK-#-p8 zWeifjTG3+oP`vi`1hh6*0p##;2Yzkn>(zsAyrT1+Ic zHNzDFp6pox>Q?bfz1t8Ym@OA6H4h);48AmtZvTz;i0M&uO%fn@T;r5JRLGr&girZ@b`>C}L}$+_q@i^gPz{8QKtl(Q-_P(KrPHm1E9YtV59T1WL>Mm*Z(S_+ z%J!I3i8mqNR1Q<;i;4F0EHVwWeOx{$*wSmb3*W>c2^V~)pMDCip<%scn4>|loE0ej zIH)g$E@1KNn=7jhk#Wp!d6iCX6{6i>@JP&gnCtX^5{syX1a!PT;} zIA1!VudhI624FyUuS08z@_+vRihiU358rSoo=~Rm+EO}BZbB+3{P~AL^CX}6Hd~-(|fDW zNkK~o91netq?h@CKQ~A8w~s`RI@}`OG}w2wun2yrTj2P-eW=KZ@o3?_SmEl?Y}?{1 z8|Q9Wl4}s&?+Oe%tV<&GSKp zd*jG!fx(7rdeeeBkVLns*Kn@WyQRjm3Y2Y4+Wts%%Ia_+2; zF|A}h-S&7iP$ORHI2rxcEGvNPliZ=YYXxs{T#d$2@he*QtTx}3ls8kusmySra77Es)j2q_B`!GEp>b5m1O^Rd7%IC5j|i5 z->B(*>CtfakC*?lwL9!?`b1vU^=*k~5pQ9B>Ii}!x2p?mk3IXbwxSu^+gV#hgW>ZP zr?HZoNiT4%0LzC;zIgkizhIrIvs>AsixWx&nvaAyBpDbB&h--nE0B&YTRK%x^O5ti2zGZ&UGe|ECNlu`!vXv+W3r+7^W?IVA$Km4S- zn&~fJ6MdhQLF@Ti6m69B_@@dAf2b|$aFPAu;YP-%p&A2jherFjYr9`YEM*CHcDrcl z=IdU2MaS4A?QKP}ZwFY&`Pz38T4(1hx0dCYTUI`zPnYnAP%z0yU8aD~gFI_W+sUn= ze{)+Y_$=<|_p%!4)|Qn0F8@1=_Y5Nr8uy6!*v6VpPK>fT#5oHUZV~L%f3h@PmE@M} zvKKx3IsPfOR{m|LKMg!1=%_!^cyv#Vv*W1E;Z^DVT?;WWtgOwoyt=1zwoi{L3Z(eG zFJS(V_N+a*K1$!uA>`uUH|c8_y&Cyop}z`izxRN8O?-@y?-ciYbMlK_RH1&?60$R{ zsXPy2EiOKgm4OzcADj2n3;wqFBA^sfH}YfHRBe_~+t~g9Tsl#}0XCRq2)E$es5t8N({-nqB8x z`i*X}#~*(AlOKN?f)~pBxiw3B+RLVW`|OK99inYoYM*Q_)YtY}9*_3=(?%INDuwVn zcwJIv8V{QYBT>CIy9wDFs7jQ$AK{~3AGOx%z_op4T0AV@EpR$$aeXM;|M8S!qh7;x zYEws@3adXydkUf@0Hy9 za(Ly9)%~j?77d+;l!VhhNpjPh9n**QVhrOL5 zj>|fAtres_hoa5|Mxfx66#aN$X?!~NKHuj>xiHjBrtppn^GL3!) z0}|?_-kH$pz+u+E(Qa)P%d_J*a@oX11m4|Mi}vbo*(*d9&Txrya_gc-oA=<3+B%$k zd=b-;-ue+0Vfd87IY>BZ5-7|8lVZp!Q-^005GR;kJIUeBdn13jjmb8s!vXTXtxv@+ zOl)gBTn;}%PK8;vke1c1ti_)`Sr?_>sC6b2k;ik9>2rc7|G&63_;puv$xrP@+rbE@grHcf?SRX7T$|{vGRg6xibP)?O8o!?nbtE_Oy{j7-k~c4k$V z@2}RJ1bw}cvg|8K9M4>v8*05~SMHii6Y({=rEscqO6&K+k?E@LEe9TVj7tQF0A^Aj zbWT5r}%Ru$hm93^?A#C@OR6RA3v4WuSgI~VW)s0P+2 z_(1Zy2nI-R%yqs7l`mR0&xw_dm=;0{ihJrwoOtC!w%!@f)+ib&&p9nNaw9_f@hTTq zQ__W=v`?$3)>hccTwNb52tfO$NL49<19JUt?-IcwABE89)>dpP(2F%1*|IZwpd}){ zND|=+-}2X@&yiJ6zynUQsO_s>6rT%gWg^n~Yz(hXWQTg=(gK+w*P7f8j z324g+`kkrcmLMWPl|;hv#B%H9%*|_Lo2XwQh009RtC^;fwZc@JiE@4qHJLgOQTxOY zY}Dy9LPBZ|Nu1u!14=;o%*?Ry)N>R_L>yHD&*{CnN%r^xDoT5k+;93GMale)@9 zNW&`J-`dh3`=M5MUS4C4VJ@=h55dbF)pn8|v5$bw>aX8kHD8mrPAz-+w#aw~;4O+N z>b<0;^{YjXP8|fc`BIJYWXn;Hy6Bi|a@hE~1dp#yknAdCtW#|9bVO8zgsIjSTYMNZ zeXIR~IkTL?tG}bx!HF?=I-zOjOO+CP@PXpq<_M-GL)vON&2(gc;-OB`!+gy^WyQ|{ z^Z*+@6A1hBeCbhe2YPm_I`DeQtm1V7%g1>lK_z|E``8YNC#O4o&RysGTwQ^OqgM@9 zkseGe?RhLuc);A@&UWyY$n`{ju|EDJ>mrcDh}2KlugjPkY+nN!hiraR?vl2=+duf> zagD~F7(>ii%;(g)dxar;NG~^yEPx zu_gL25l|Oq?Nt$?te6=V(ZLU)bpc+LpaZN=&tY7w}?@%#4kRd5l#GR*VCT-g`w4)X5oyUmN6T^f&nLKg4qwhH%VwQ?|TQ>-9e>)=>c=}e~Y|IanSt}*jbW`7k$2z8cD)DM8PKYwX!$j z3W1R=DGzm>L{2OTe}hSO$OX0v@nXI(YPOdCkJNkhzRru07N%m<+ej=h7fLSwxLQLa zaMPz}NuCB9eDa%oZ{^$b`jmvL8+A1Aj5Q4Dle`~;7`th|s4T)0vkxUhV^ zmd(!c;`P-|U(y0@G?XKK%K`{zc#3c3QMqEBLEPC3=9=w?05}Ye? zk%zVTxr7_H%ors5SEY&N&j1+m^WnkNn@hd>&RGek@w66m9&z}S^=g-0Nc^*+^DP`z zgVEB6{TQ=^nD%yFd{?V<&~bsVpl7^ueTTX~cYAW?8VA9kIq_@V`Avq7=+%clO}^u9 z<=JyL&b6Qh&$v_mmJkbcjDK++Ex-W*2=xoHyaLzC?cVR0evuUe)p4j8keSlPE-?Wi z%JbMt6r_up4-tJ z;z=H?5Jt!c2WN6NDx+*@#*KHHE)Oby%qh_F!C!N#Mbu;+&yren==zgFAP_&fz~9d4 za^8Xzta2&p_FyJ#E%{Bzk@5po#dCg13&bF$kzmI%Yfd3C#ZRm?kAn%2rp9`S)Vl=! z16T-{cJ{wuc^w-WET3t;CI&08>d}^GIikh-g=mCbW5dzp`7Ewy+{eBl7h-eW!pCjY zEV0Gxaq=I@agYn}Y};X?8%KD6d_ryTq-{&|Rtv>&~A1wa>BnqDo!n@yVo>!W#Lh zkACoYQT4CiztYOt?E`1)Tc*WYyIZ$r{row~2U2f}aUA{n6JGli`d0U4%U`xQbA!959jH)9HD72HdpP!Mda{r+=sYufoZRP^+I{H!X^ zt4x_wkdA~ow{CIDULCu%c%>0Iv+ zPX}Wma_4(|rOLz-c$RsH2PsYSRQIVxdBF;1-L4$R^WUBJb}#1IW(f{Gs7E<>cuW27 zSdHou^ANC&ZP|^(ug$9F0C>gY)PJ%6h6&=%Hz9;(oRf(*BUWV9!1`EYC{N91g?Tf; zb(f^)w#B2tShhW^de8vvzB1r#j{#^CVV$Xi29OGj(Jd*JOI$oPPfN$>KA9M^omRIA zqlRROuse4*0tgVPgxPOX6D`fPyP)l5SF56mD{~8paB3qJKwV^ z${`-;`>4ytigJTSrOTs8pw62+zJ*27{)(n0V2?#Of_CGg2RBkv zlV7mdrB#!{C66F21F?;uv`EC}4-zVBBdpG~2B23KPUH^_`*FOJFul|_9W3%|cWLS+ zdjJrB+^+X&aIxr!5hm+tz?+EDd(AcQKU%z%C~mK&Zs6Lj55VCdvd80H|BX*sUvFy5 zLkFV@EIM9HTAGud&&5kg617#*(EYuPZqmk`_W9b;f%AWxJSd#;_vJ@Ew~wFLOi6U5tt)#tQs zn1gi?J@!9KXAMBulcxd!HSsz`_QQ!g+4(Owm&)1UL$U#vvx1`%0!kkbOHdHSlGNFUW7CKB92pXsdAoW1FmJYh6)p%|`W`_&3 zytX=9JeNtbrZ$tFbMb{2#aQ25fe`F8)8{NVzsUK>CB@5SR2Ad8m~-Tno%C_`p>va8 zqu}bQ}j1MbeWWzwQUuxk&m1 z(5T)(xkT;QU>X|Rbp04!?UWy`bJWRk*KTD0<8rq^reA?bRmUX^_+VQSTZ**Z*TG$y!|Ht{v5kWf=V0C zxzydAq`>Vj=i*Lqy^01U8*J;ViEx#!ivw5HHU#Y@-OJQ^F7W4gwl$T0u*5$B$Yqr0Hsgl3FRb`Z^vH#jn;)(v~ zra$I{hvA6nqw>9)e=YeF(2^H+#!7t1x4z}dyU7n<-$=!tFO%q5-7f;c$GW8Nywyw} z-P^qUsQ>jf@}3%L3VDJLvU;405q#cW=Zknu1_xAKni$Yae$ZC~%=28f4Rb*^6=q>A zxJ_eY{7eRK*f-Qx)h2E(He0T z$yool4#0+Bqi>t+Typ#iYG-fI>eF+mqB8yp>F&#`Nw;9U>Z#sEX@UXF!h8I>-tSW1 zF6_NG>@HoQ8?RF&hoc+eG4C`0(c&=|l-DCc0Lm%Soa`YWeBR^2pV$nJ-6EiUeZ~K< zWkQSX6{H`Sw(G)a4*A=Qy*QI0$kQS_kDd(C+zlE=KPntm35yz4i6^N2cod@+cImP9 z+Z>kuz^Wq(pDA}U32fZ|$Jbj&MY*O3a z)O?D;N6h?+Z;0k-JANAZh@OGLrLV|^)txfNm({wAf-vztY4;Qs{ES@Gpu>rY1^Fd} zBsBZ+qN5!-O;+gtmAgwuDkQo3gp8U=ve=<4c)rw=x&+@frOW3d z`?oUaiZ&hg+BeTNhu!j?0{mPTf5eOQBzN1yu$0?{l~3QA|GEf4P4>}vK#hVO*vI$^ zyv0~%nXvpK=O<|DSlU5vuxnwIf9oE#5iqLqMwXMYvZU!#^)9Q_m zzz7*8g;rMH?m%i?}5Br*yZqC$gD7VQ$YV*HQ!euVUU@N{ux(KJ+Ui(alZjSjPu?RGl349(Q9aM5t~;L7l}+JeFJLrup20k|+;~8F_P_YsF_Q>EE{O*}kptxq7*g9B_0@ul0hg@s3v?zi>+n1BWyu|H0f>;5d zpwCb02u|OeJG}*g<9y#%*uX$;I4z8fEh1NeE3IW4#CJ@3vkU6D+5UHO4cQ6e4^4ly zd|WA$AoA{4RE40&f$!h9-^O#_+=9Cq(A|U~QIu7bmOC}bka4?Gq^Z0Mv4R9BjIec~ z zI?(SFdQ8~W0hJpF-ZNj7(vQ1@xhsgL3Vf5jt_Aiz3cYw>ljrEPwFl*mjd~u0hdUd* z6fnp%D|B?fv;?6dlFymN9GOHdnzREyv4Z=9-UB|si=`t8n8lN$R%&{14}TmH1X_k1 z-~abd{344{hAv}F6n^ILCU-0M=^8T-R$4%MMaTJB_f2>B!=U&!*44DqpT5zkxG?&f zw7~>oIC4`5Lefuxf#Qf7sSYaL+=^!Mt)k#_JW7K7rj1Lhy6J+~a#uN}1=D?yQ5k$- zTh!9?1iH`SB3kuork1`IhzC$M<%KEHZ+9(rNzmIPm0Fs%i9fF4#M~o9TohM4QZ%gf zPsgaJzrbO~7E*-DZY$I`UiuLyWNdl`geA3f*cObK)5&0vx_>)EA1X*HhUcMKtdFm!; z5`L!V9qP~eyDJz+U=7O|wxpbV!*&-)SukBlka|+XiGmGjj?0k6b%jz_&R{-d`WQ98lKI{pzBU zw-DD=|I^Jl^V4fcE8u8p5r&9qpjPljw|k;0?vr?JDgjq$2A_-DB|22k*h;6{Jqa&0 z3%2_57$P5Z=D)>zLe3!Li?h(E&%oKU(P7~(nz>XiLnOY4n!NSUJKrpu$ThBd3oK0! zrC!>ISc?c?S)|ZwZgK^-zwj7+88!hQw=4uEX*(-5mGB40k9)|+8E>@&mhhKpf=ys9VRd;FwsJ5>0fln0qV?7^B@SJ^^>GtjUVotlR{frj;H zA5wm;nW2R82F=1s(k2v5SXl1ZD`6AIlip?^8IFp3`a$GowSwpPD-*-dw|=s{Gs)Ax zoG9hs8@B!gwtUPBv<1T!QN#l1;(uac7^UD>I=Qb$j0_8CsR=7gXqKOG6)~IPx@{^^ z?qKKur^|7zBfe!#^@^i6B&U>UWM@m^qWqMPe*N{Xtcn>af5B6AYc+e1vA5(}THwvn z8spQ|jm9%_%5CpkfpbvoGO?Lw7ZJ9N5AP`QUA{L_?5jS&(xu2*z!(au;iq+^nb3zq zpP*_jeRtpy$f6s18)CD`DFdKuy@8%y#ahi&&59@$&|hIAkeJbe|Ihw0_?aah+ z|L!+obpIsQR6s<8Q0B(ub!YN}(bE58LDTI#dM_wPjwtL~cgWVB|JGH0T}acKUn7_( zueMSH`;g`%3W7$!SJ~BK@U`kb%GNcuRl8w(T>FvRFRebF4h%On@h5pnHnD<7c(%=e z$ot)czF8r4Yo$V)0)}A9vaoS(*`wt#&nv-%dwYjMsb_urmp~ZJD+)taa-0}P%FLxP zK57EjpW`KDq?zvnDoGtZq@K8gJf#=XGG=~cP&3fcsj^+XKPEcXq!lGP0#^G+pJ1#& zSy9oux>Re9Wf9V;<;&mlplNwC=Pd+7;i4vJdvOBy=>LAA#ey@mf#LM$kTkZVIx`V_%wz^$g=atZvr)<#;9XhY1TKI4;!D8*ZuWEiGM9*PQQ_+)s`>rZ>eg3wrt< z7l1lB$YXpn@p1WLRRs^k*e6Z}3(;ZlaoDL9R~!@CLY86rwol!SU(E2Y7`4%^0l8m_vrn zD^?pyL%0H;HZX4Js+@WE{sVD5nWr9f7b}|@= z+a#1-Tb$Dw@?3XdHvCBi7hSzOH&S7A5IgF_wHb*=t)vdw3|QWPj~vXZ)&}7^J`h~T zbhiZ0%5=rLZ;UU-?GG3yH7ph?#5-a!)%0K_YN$g=K^Ty?8=5jk7TkEkgY*m|M)cbQ zRnE@k_+-f@wqUsiHurELYC`M+)YvJWZLd3UhsZ6{w$n`5fEN`%9+Oc+^qLo&rV$5G z0}k#GV{2Sesob$l%9&5OV%!oS>3d3edPJkxvmaa_|1wG|6|*%r-LBRIDn6Wx4awG>M2YH-=}p#=o{_x8%U9=v0|CL0L7yRg zBIQ%Qnv_@b+xyvZb}jdOaYpu`xYP%8bWZ&b7Jqf5%W_}E-5ClTy>M%&k|oFu^jf|* zm3Dqx-s;ah5p>_{DsP?%Yw6#i?=01Ma)nMm3NoITr4StG6TJ4CXb0T3Xxej~FLtRj zMsQqwHZ1AA!uxRPz_09JE@$*=mcHxGXir+Qs}KkZr4MqQfU^CQdep||#MUM!pIr0y ziCY7ube&l~V)EX5p`Izb2T6I`GJMkfi9rlwCC7Toni&L^CXegQw|UpEZ>m#@Luy5R zc(|?xB^yk05npG1W`fVL0A%C>q_Y(AB zOOtY;|7ceDtey`wku2w#?a&f0!VYIq!4DH+M}apPU4r0Eq9a#wIRGPUB3cWA&9cK& z-UGJl>WS|uDQ^xM7pvcZ8}mn=a>^eHW(BRl{qac=mXM4Sf4n&>IhnZ1mHhv*%`h^z ztjEOAD*oQNW8&-FXPv0zO@+DJ`ziH$u4%(~T5Ix{5cj<$IYGOAhK9)Tl>^0!t)r*L z`7fVYbxI$Q9-Z+h>#mi(BC<=F#?d$Zp{^~;t;)`3O|hVk1v z@na!@amu@6>fzxLzgH}M?%>Ki9o@|g8Ts;-d=zVy@tnxpKi^GvjdECPR{xF9`^znf zwwP>)WVyUK=qZEY@SQrUb5CBst2M>A+Es%1-nCilM{EA=T%IQ>H%i~Hi&a;_wv z`RSo)egAl#Mh@FMSty%lAg>drhi>BskYQJ8aiNU%V?tjWLx5bU!?3Go01tA|teQ)N z>-a~+DnC@Mv7_ahby08)xnBL-wr%D?stg~0w+xT?ctC|kzZx%~M-Oc09~?_)rlW)d zA-F^2y!bKxOi;SYvWK?(G~W5)Xm0^SVfw@NcU=w1_(n!-`tE%kdXOh4iwjAFEQm7c z&^lIeTT&?htpp>jhE*SL<_9Nkm`bf}!xzmfVuat|iE`31i28BX8r~VaIOJ$1xJt6K zneLibS@(jDo8CL`Za^>@-?z}GF5M!@Ehn)j69v_uYTFM33_j7Mhx)}CUg(pTdUa*y zeHl&vC1&@y4nBE)`S2DvYJcZ^coJfw5HnH;9_KGV3QN6siZn#tL}EnEQ2oRHgXf@6 zJPv!ZP3?3jcr%WKMocx$M}_vQSjqx$MB&H8 zktc{lB^?$!xy%c~6kg0e6~UVr#zFs;PL19Wiw(&Je=~~j+pZ!AqaQs4<38L}`eW~m zoy;&OeX!TCyf*PeHmck{tnAkDssj9ZM6z_?USICE-=ZK01pv+rNqz7H!Fodbk#(LA*%4_DNqvxN zt3#R@$0Y{N%!ScZUpj2)H>sD~M^JZ_%G_`lM@vIAuT#gwPb-Y|3BaZdch%tSHA0mB z*!ZO$@sDZZkH-0;Hj87=n~vZV9cgEEjLG@hr-lrr>fvo}IEk%#DG#&K$pmgB1HHu} zi(+C!6q_!X1aUAxJfPI0dv*G3A+XZ`zFu}0`n~@fq9ikPMa!PnJ6!8gEShJ%68Ddc z8*ujDzpE8l0|d;h`KaZ%&x=2E*u6h*DvA@8A)RP*GFaZiK3IH9IXb;Vb?QNxGJ1RB zwz+RWL=Q#TF|_aGe95whKNGiHTZ+<`Stjz0xq;SUN^U;gGSTaI@A&;R>(0B&5#{{2 z=qZH?8}CsZrv18Qg;(a!_t)=L0UoZdXfMHj@EzjW;wsg0omO^vw8uD+;wZ|82u>r9 z!N}XC>^0}NK7Lt0{`b+#*?f&^+5v9bF9}Qx)r> zE7ejh;1b;ky)y%zf1nIUL%-I^M#lj0q+#trIuTdBAUdXmFs2_-p+GF;&L549HW-+S z#oQJsHIwsrwH*O1m*jUJ zHa?b$$A(r{x;zXLLzmt0H#p`hF8Ps~njjv4V?ztkVfP;j7~5JfUQSa=5r@9WsHppv zXFH)8{t6MC3KN=HPWk!Xq)nKrGp!N@hJ+RRRny_kU6Z)Ve|AJ}Nqecdb*kxPuOu=9TurT7pyDp>N0HTmlV3{({q@D*k3zE2)WidJc z5rtH4Byxt)Lc~N1WGAv{>FH(km%@4r`}$N5)f)ken)HSD03#Ml#M~0NMboN$C3?=Y z_avFw>jekf`E~4dC?=4{N8^|$E z_bqmoYStUGJ<$NvCX|-ShKGi}hJxA7M@LCmDa~pE0|dUU?9Ar1!3QuiIf*7_OE6&t zdvc6JzQv!vtw_$uSMu+W%V(m(C|W+lgMQ{(2KRhHLv#+~L%6rrAAu9l=#v}(wMOF} zEM)u*HH_%y!6r*i)kw|}`q+@!A3w-4!JvhZq?UuQC?Rnc0|duSWIUe_&!!%isNr2I zJPMCheU}hCH&7jWX*C-A^^V)nrrXniOH2<+=tU+N-8>Qs zc-sR1!Zh(VC&52qC)%C@pbt0XT_Pv5+PXA0uklPIZ$Mg4} zcy=9Cb=X-+4c&7DgQJO3pIfgg9-rySMa6#q!Gk2f?~3|9jmDs9%K??X7XVH_i2Ab~FoQO$sBtIK z+iO^l1Q0E=Wx{gBw|5s#{p0NWKS0I|iZ^hcP>FFP&3}q$xX#990`n$Rd@VpHoF?n4 zy0!ko3sK+?0Se9K=!;kYJMZ_=8-5x$x&34wf%`DikCi|n00VRM$@%!>Es`j`y=^jO#8mPI$SS2eKzF;o@0kkv|6p$$lHx~Fp%z_!)S4Lessnz6*SlL9 z%$8%|{B8nmwj}64zEX=p)DyqfiZNUQ0*8YRRyp2TR!Cby#9sHJ6-56W-U-6-3IQX6 z0UOI9YasM*@44xpJ0`isz7+7=uVYcyFz#!FRa+eHlH+926Dqw73L-8?rOa%(WFQ7k zW*6U??!AXIOdVJw+r<1zSn;5ZH?zP|Z;w=;U_+N#U!`S&mw?RJssO*{#YGZW*AuGW$Ccn$~L8M8OR4 z9S~SsMAiB=Tp4a<%XTs{nW5*XMR=}^H6P~|jU|u|HUvvJe1lk@lfd-td-F6_Qs`Ri z=ry%ciH zS2Jn|6-}+{3Ed*~)V~P#MJn-gqNg&P4JW<@84u3t!jX--NfY(q+wtk{D6pushl006 z?lS|tmrXE3Lz+fqknLGbDc3#Ton8$+BLRfXOIBzW9M3V+(gISAPe0YU-HFl~cMR-vA0ViZQ0i?27ig{^vFT51RvM58wj}rrJsuc{k7k7T7 zl#-7^@^xi^5>AY~f^5R81iWP9A8V65H{Dq;N<=@&AsO{0Rv_32b;QLpYE&`w8TwmD zeaDJF;SuE$^IMxzg^Z*O$Zqq6pz=$Xj8G`j^A@Es5XzWiTOTeWAubILAIkCs9(jhx zkm$6X7qs6(hduq5ezw2bQ{!L#w=kq%;Z9z&uFy7FA@}FlvRd0UV$ArI-XocjO*fT% z{|cOP+(cleV`Am61s+itFnIeT!qY1&F$!52iKe+K7o~D1TJKW74y68q=j2zLRR9X71bEjmUA^~6Ta(Sk{u|tx0lx) zRMDg2mUCe9rOosayMLE9RJPt}NV@IB#^?d1>w}t4X;W0u0oA_O&*}EUKOj!6DX=Pz zK)nIdfeI}rlEXvH^f-2ZRtsJwquHfrQsC50ly%3EkUBz92P>zD#*O=sGSydd*We#^ z;PfPYLIamDkbG?rj>9{x+U*`k=kD9ghxt@V46@U`((m(8(DzVAe$|@Ea1DLaFb?SKqlq>om+M_GLzB-xRObAj+PkM%~C;5%rP$J6lIeChfGI z@v(-^w?|W)?+l-g$EcV4yhdC08Qm(0>Qc<))6gi|e1SP)Sq~;I6?<^)!hb}d&Ry*2 z45!f1x(vP8HSz?4;iVP!d8OPjav~yZh&DrYkOx9l3DrYIMX1i2GHfKy;w1(p@vNjtcY9wAG)#jsfz;23YyRko#}sYAb+?mntXs5xyy{ z6dZ3br7bM5;V3@E6bu+`BzwEJc^&ss_*V;sujq6UFuqadOn;b7WCpJCyoQE#dAe8# z@>g_{qT!n<(I`lc&DzMi_HwrXMrC`noP5GKBzFwThIKqWa97xXs0Kr+w|;(S+RILW z>^kaUL&=*~y6-OSuT`ZSzA*yoe+H<>pYNH$0@tZ)jX+sE&dwIJigMOFS^#pJJbdNO z%zHMZD8EF!x)sF!jXLyI+kwIV|L;9{1n-HVp6Gthb}}LB^863RQ}@|)iTNP;c-A>^ z{i=X#Pk4LfXe0~Dx|MC{r-qECwJ7^blraAq?iGU{T|*2?`tGN|h9l(;-6~Ajli??y zL#@x)_@L5Z>4u0%hLE@y=GW}Qgjkw2Z1rcYFtOs|n2o!2h}DKKmm!0n-t!F)@}2O`; zlci7+TjQ)0Qrj)KUg-U)MR(=bCHV(GgmY=GR8KqM)<%PRzc#mXM~|Gub{_M|%t>7swcA0hmSR`~6510VqJoX1r{Wnq3b=H2P{uSY z5>FYnz2^H~!PHsBEhp&Z^)9i$BH>ppzFU}0+ z@U2JOR$ZPyW9Yw|DZ)|Vv#1ZQN>7!IWk`zh?f76ODaOmq@RfeV$hogb=z`0SrwnVv zsLSIhz88BcH@X3tXvuya7M35F1+?>FE{Oy>%ys`@_pXlUqvE2^`P%v%TU@8sGQj2JEdUZ>Gk%>r|6mT@%ZAUC>DMpM82S4v)YsP=MzbrPCa9Zo7nX`~i}C&VK?(^! z%}2l4*x0aIh6xft(Ru|IPT5(x@(2=t{_gyMwEuiDH_+Std29W7HDhOG2JVskRh99v zJc4W>opG!7CHZJpb*{FY>{O9WHfiQZD;NoBt*}g_VL6DxrN_g#k+l&&obunMxa5ON z!~tpLB)~klvAEm1ThU*Eki{V@GBk;2)?o<`ksJhyH3{8hF{wSlL!9WKVuEynja6|v zRbE6Zy+RRZ#4QDW2QX@zHCpyFpr;q~m9RX>qZ=euPEJ4sC%wh}b#yM0Bcn(a2Ek@P-0gFxPJ)9pz7gSnX(@L#$SkGe}{JNk)Y`Mr> z3aqXOOB_pF2=qj?q?#$mrdGQ=PVydQ18G((by&59e6_%N_9TMVNUF;d1{VmlV~@_D z6N;5m;Cp<`+mo8GVECtttCGfNy!v2g)=XN>3U^TkC&IwwBDXpp$9mp;fN)HjR%tX#j}!+o;^OH`o|c`kLTB?2PJFezmhMqLk@36Z~XRXf9=g5B%6bFmc{F@FxkG6J;B4? z?u}mk@5Ma9;C)--!`aaJyH%~f;%&_T7-J2Xj(8%6&47{NIdNMQ zv;0J;lR)`lZAX4qNu#Xp|8()sWwNVCon+qLXbJ@=ZLA!+bp*Sl*ix2>FieR{(T9x_jr zx0$?Ksw~Zy+{U7@{TOBiSSqTTE+A+EOiA#oC|A1;|oO>=tMTH*y4E zch=|U2^`yh%=5U(i(*h5qNiN2Cfh5AYM?5-Nh3 zTGN~?C#VCT``-*zsoWZQzCFRJu!JW`<(tlf{6+VxN4QC!oj7|5M7GR5i@k-7W!pnV zE(|@V-ZKB)IM|^Ez&{fFMB`*L9=EZ~#4}NphD>94~%p(jUCOBWe=2 zcbN49Z?JO8Mcv8P)K-~$N1gkNPbf2rnwLMFbt`u{|Is2pxdpU3Z*wgxu7uD2S(tgB zHz6-lD=!$d*Ac=v{_wY&T+M&IxT*asjLH^^y`NL~a9xYT70=U+FV@!^MK26at%aq7 zvaXnQJ@f`j%IS`t)OoB7hF|}*CIGVDipMtyC;57dwC!&Ajo#W>-o2Y#uF3xJk!a1F zgw@~H{=1{ugzP2?VlqUVyywd4JaTbLN=1klx!~3n^TjWUWBW@l(^Kahql~T7;{kY| zG&hUF@XZqM+3QjPeT=j62T{yVgj(*O%HTvZXKr(etMeh<=hawxVqWm+{^&Js^t@($ zu+z!XP%I)mH?%E%vymXb`Y_2jM$TWQrn=h!k=rui-OjOfekA||f6ncANvEJl4&_v0 zGo5HwX6BFh^X8=yZdYywte?6|A|rD$g={)_bnsK4BD{Sq-(XAfboghn&>Y@2vjvSu zhOZX>@b1bFWYKZHkt+Y$MJ0#reLJ+vN#%RGCiZ<4{jT>;v-23=r>P`Jqd&jUAEPS4 zffC1^l^Yv>r(I>SmI?CgHndgx9vbZh*;98h6fnTc62CiIOGYQNf?Q$X8i^2 z8JC5My-#1rF%Q1gmHma%o0E+Wu);@9sMsiJ6pE^Pu!RKeC(-YdDY*TgBe5+-QQgTP z?VAiIGWuZaiOJyQT=J+>Etn3!Mb?okZe48Kf5?FmvSaC1-A<=fzSrISKbyyjz$rJ2 zsuw(1&Ld1>7I^O|xBcEmj#5mXSv_48&=BWCP>&Rl94|;TBy9aa_%oGwJNM6gz}^=> zi@Q`5p5|cH_QrkqlsD|ZPlnmo0G+xn%9(2?$E1yMD0NXOAq^d}C#JM3TPt5Ys$sTa zW=O^Tz~;=YFqT_rN(fBShw7*>&wYxuHrl->h`)RIC6?@3Wx*wbn2z*IMxZF_`xMC5 zlLX0po2qIfX{^7X$gVage)bsCdNm|MtlkKp&W_~~tTtx-D@pUW^vlK)EqhpYo$|9! zB0tv6K@;lbbPQJzODyv__*^tf`S$ zTvH*;$H&(JRam2DFAly&?slYWmhCn1M7KYsWSAN#^YmvB%llIs(ka=dFq$BIT8ve! zp`C`b;a%(WT_Ot4ca}Kr@pZ+OuSgIpEiU8*o}ku?!q1ro6JxU>Tr8mp;i<*_owYAG zotXNAs!Akw1(#b zZmhZnL$ac55th@E_42=$FSxN=UTF9edo6wXz=3B#gzb<20vqd~9F|9!j`CsWTA+wI zCQ3K6-tL9-&{D2ct@j;Jv$$_8a}D+z|E=qeV;zjZnRjPB`+ROSfTlcfl%sfkwr4XO zw?;NgVcK)dUJzN4KwZ43ZAe8$L`Wz<{--MC?$un2u&BMIDwDzT&oV#49dxIE#c~LW zO2;3gJ=4_jaJ&UGId1yQ`^v%6lTATxTzb#0`iVYd7OmzN|Ci zsD_ezJ|e#pdU^Ftf3g4MhKx{-rca*hvHnwj9gnvM$mpRvsM_zIau10XYWkQ<_+u3! zq;uWk&hf#AtIv+_u5~d)uP7*yOm-&a-E9FU{R4*t9HTuPVR7`5T?mH17Nx}Ymr{zd zz^&a$a$G^s)&dg4MWwvG6aW?DC(hhsduu-^?_COp^Zs3SzcH>e;{K<5Y}jXC&*?fs z0c-!|vkIY^`tCsnG1qPTOpnckZaN~E#1SL5IN@WB;q}>Ljc*$h;^|v-3h~wE&P!* zW$@d5x?x~a`K9G?s3RX zN_Vr^b1&sz{Z>4DPPcNVSh6k^TrLV?~JSED8 z;r88@s-&7_NgJ-Sq58k~#_FBLEU%Sc(H0H0LD203bUf3yt+*q!>l5lFu5wTh-j(~k z;L40Lgbe9~E{+kBetW(pM*7H}qj0#RK0|jw$g*wnd&0Z|Ht7X}k;nSC@z%;m@&w6FRU%KksyqG)A197n_Id8kZYb7s zEa>kZGLLP4cl{09-I^~ev>m^V#CkZ1rCudo%W8c^%S>79JqAKa_Wa1CyJho4h<@~iAmnr%+xWx9CgUv9Rq=F;ci zfx7k)nWIJtUT=*K3(3|1X()+Hx!x8SrrQ;6zMx-G5lsHv9uM%4$v=@Hf?Y?w?!XCFbqPhnn1?=5ekA-G}v8!#>qeH z$vpcFQ(T;aW7IsHw2jcd6JzcmdUUQ{Dyi8&Pg1;2CGT9Dti(#T8qoXp>uvL!e?t5y z9`Q$>?bs{xEM>QQ_hy!_oD0!(l=y|q zQzffqNq#^V&kbqNiaCtd(hjxJ9=zm_nhfzu!q@G_xUi;ig*`g*^j$=B_fy;Tx!qlVH85P8C&1#EG& z(jKSJ-%Ep891h9|1!HqUXd-@KPx_b==ENs%8k!reqoIq^k zQi%pk`unY!Y5ALXKA4xpd)Ou43gJGDcKJql9N{`lO&{!_d`d%z9kep{<}*ZqHE|5* z0!H4o5#gz~CrW9$R*ppOP@&U*gFuWKZ9yvI@PQBRkjLU+*YzXesp{YoY_T zb6h;5^T!X?KV`RjLQma1(9+W%WkJFvJ59eKCcF3g?2);aS_lu zH@ZsRVDKpOBC7Xpt}+@zxra=eJz^C7+1fn3ZwD{j;6KlY*|v2G}a#@1Z?-n$gAV<-chTEKEHabMhh|tS5eMT4UQp zK%sieV$mRs!y;DE+O8n<2M0-=m!@@jMHKaQw6AXY2heO7hO7^il1&)PTz;F?efcej zk6)L>*LM_ah_5xh%U1~S4P>ku8(Gk(o$BqSTdP~6NhV|4r9}JgPQ{CWvaVMkf-Exv zozR}k^*Q_I&px+sBMYK@NVkcU|5uF@vnH)=g-*9>KN*jhDxh=_Z6A(+vo?>{(H#PO zJW$8nQL;Li^{>YRG2xxmjea|0?jew5Ch0iX+bQ6yXzrIlddNXC`Gtock#cZUZ6H2n zdzxpC4Cr2o4~f^TTlI4nBhXOmutP;KccfjR$Hs&>GG+>if!OwFdF1nKnC`HfT7(Z2 zdj8d*uFmpV5=Jsd%%~QIcmv)|>}{a9N29U_pv}+VtCG%+_v} z3KVEqmb*t^+wVTx4`je#_l5+e7g3!dBAvZt7|sp$$fjQ#md)}GI(tdC2Cu~5kjnms z@VsJF^pdFfGz??dsh3B19%Qpa*lQ*W$q%v#L+yY+yJcIcY)w3ZT}gQnxQ_&e;4j^Dgw98zbloV3OG(TnuY$_SZwJ{ z!3EBh6_t}K^#86%->a_$bq?(Qv#p=RUJ$ly9LylHK{uc?0}YlNQbz10(uw^kwQBBN zI&AML?8IqoAP=%JL(>{S_+YY$2k`aJ|A>Exx{wh%;hhZ?cHPp(aULr++6!IQ-fEr}*cgT)f z-yq9;u438bDTe6$5v-t2I&u1-(wYo#%5Xeu%&%wB!{?DLW;MqDwTrDrE59|cfDqd> z9h$ARblRy^PF~0V=RX>vFx4LqrU^#J+;S*Bfgy&PfO?Kk?)&IAf#`(gJU1808i}6f zL*C5?qfeS#Xm{J0vAz(r*K9JES4;92!uQ;OG9R_uVrbur+rzwQ+Ksaa&o>Y>fQ|;l zWj}0m@wAmWETFB!zFlCEd}!v<|4)9T27p^bH8iN2Wa$qD9A@^11H56@7&vRz!yU4H z*KeCzbNfb85Q-M&T~Ipq&qT*SomE6dxV-wT-IBVBjKArSurYu`G z@@!XzOr$vSrsWhnNoNFJii{ZP{E-fO61z`-1Clf_56%5fU4V5-{Iv8B|J`9Jk8aM2 zDNj-kvnuIv1ntFoo4I+e)VV`mHP>uGJ~0n_iNs;R>3u7WB@^B>^_HQUak zw}LwE<8aWc%2@b;mkAF0EwE0TF=3OKX(x#d+p#PPCD8sHMoScTEc=SJJ|77<2@O39 z)!;arHNAra$`5RAK;8)z z0oRBQsBr1;#%!_UWYKm?ML{K>FZ=(yH%c*L$Z5b*9ke%pH^>gnV)gMY2Co<9qMu{g z_g2&T#*3{jDuRHMS5qvLt#;nAt%l$CU$~QcZ|i-Y>A8jBlIZQZPJ_||Le5LDI*T$rzZtX+;pi$l+glwxUQ@qS z0!lk85X_|S-x)3U#Y#hS_^@_qAYNtYKUe+bfZB?NW%lkQe0RDUEP zpXxMFc8?#IcCOu{$JQiYyqvSYYY4*49LtL0t}LIn=Z;@|pH(#m{$oA79V2 zXkGusJomCJSM1A&bV0ILk2%GmhyMJ@VAHE`u7bc@S#lg=*$Rpywf}jq(Hd@e*A|GH z2p5)yE03NSdO8dbM7|>!vy1Rj6CkCz?NDiMotqTHhc4^>TqDfCf$P6i{MwhNqBEk% z3!N|{Zv~f*5srVC3wQbQ+_W+A`|^kHYbo&A4v+Ey-m)>aYqHZo>5qI|&CUvS-nxE@ zto}uxh2No<3)#J=$5XtzK3Cak2HI1eTmPj5+9oA2zy>gQe}JlUZVnH%?!|IcuitQc z!G_*$MF$Xr*k=ngs3!^)_P6JRQkA{;D5_vWW9e==o`h#ryu~}1?~*Pl))*Y{MM=9V z(z@I5a2%Y#z?NZFgbz?R3|Lm|NDH#3>Ms2JKi`Eqk;DrRLRl9>G!L00h9*)HWvxHO zfoVi^`myTWL{rWoBv&a2{gYX~lmw7gEmya3^rl2RS+yOLDsWNYdm{<5EZz=~|o-=#qr zuNB$yM;!OK#lfm(H`HKqa}TpI*ZX}v&3})s=Z)}=YLEP4&8Fyl-{+TroTd1g`)w{g z91ils273L3`KhN$k$IT);M?tC4~8O}KuW@7v4TeAnP(lTmi=;PjAyt=>Aw6D5{)M& zOJ^Cdf*iScJX>>(8z*OHhjW2ESt<9gvgb^4Mb;O7zAUa;FE0$Z0x9G@fk6}31XS#c z-3G&P+&tMqHAVZ1-?c`~+Y%cgzi`&cGzeL2pdv*n!KYCMXB~9kjteIvQcN z*~oDY`J9_b${R{FnppD}-=0EQ(-`P8V?(c#f^i&ilD-ql>|_Zpq!ZN_;9|G1Ear+> z1+Rbza#;=*^S+<6hq(a46O@M{V3zgwuQ6r%%#Gwp?0IwSLT*X%D;%TKf6$DLHA;8{ z*cT-2&$gix5%T`N)aWTUV`h2i`+y4>I0%wxyUb!_jK-1G#@``jVs z4XnJgVHerNV$FU-#A&7EhnoxEVRMZEb;EO-!!PS`mR?wp@pqk`e1M0DMbCP(L#Z*r z5Szix|i&=MW^4@*vpVhr(6r?VUG&ibT&VKxw4Ax^?%*8$*59!;O*;uQFO> z6A3RHr@NNZ%UYr9{^PIi^K-9HtQuQW4g0U(JZD}k60kLNQA83FKRC~GNkgBld9~+m zQq&_x1hF8J{b0q@>EI>pa8~^J#EUI&4}cF!HPF>*{H$kFzw+(6a*PGSBO>0~)IRID8MHqR;C)}A6YXcWNcZUecHqAi zKG%&GyY{b0d|(J?e%^dD=nXjQ%W{{iDplCx_YeB>;29h_d1JhVzAw}d}r_@^ECUUkF zKO|l${WHg?`+uJYNP*r2R8WpG;dYmr?Uc{;9{F!@Mmpya`RzIKZ=qTrtUjKBXdF`= zQeG8iw_yss;c>`gFs_i+Jg2j-xmcr1xtx;h%K#%XTF|m*xeV;&ANQZPDNU>ZIWj%; z-KV3$+%1#u`2Za*&ov;pp6r|SVI4*X(a*brNelXF%QOz$Z1QDhhoFAR9@Vjl-{>5R zV+DCJX<(0S%`y20f77vjrei%KkCw<)+{&WLRRmp(`-#|DEq%PQ+C8>fW!1MdUw_2K zmz51yZv|ezzAS*GEvS~Y*dg^|~L3s*^0HTxU0vIw^MaRBCx(b&J42Rc-7_rxc z#0w+%_x>9@e zl2p(FYp(LAyD}P@8bnTNBG{<}%mw`(UBf|(j_eim-Z4*NaIk}qZdNTP`osxsFvq?+ z9V8*a{kW%gH~;u7PS+RM3IUd{_m{SR8F-!0e37bxV4$7-{xxYu+TB9p{`iy4;g&R+ zTVoV8@I1ckt+^O)nN|S3fF{}TWWyCW>ZN^i&=5Kf>Ip$!$80kGY{M$~zlCb3-=Hm< z$iKZ@@?6~2!TI^QYi_^YUCA?O?mY{^uaneX|1JOMcF(i*+0vm1d{Gtc-}XmwbsJoQ zw|{Mx$5|5k5;(r&+3t(An1u$i8&+4Gs3EVm6M*eU&CHD_u2#6gD+15 z>SSw6-Tv(Se|&v)K$PqDHJuVlsWd8rAR!9U2r4Qif}&ER2uOFgiiC=&2t${YQi61i zfPgYIGIaOQo%-zoyvKX*?|%QBf6h6~`##U}?q~0{*Ip~Y#(hzZ%l{6G;@zhtrPP@( zxXv~-ZOy^#yz9&L!L7i>*EF*O*voM}VCl5D#DD~bll{u1x`6Cb-HG!LDv1Q@vAsoh znqX(s#L}`bR5cZD9P{92C{xGRVwc^@&$~;@YWSP0VX=_;$OB~`*h(z8f{kn05j?>a z5vnsflDY_Lay@Djbow0tY+MhndV>-ae$&gYr5 zi+&QdY+qxs{RJ$w7Oluyn|&uXeZgP_c__!)4MwX7JX~zu|CS$wPiJ9 z!lnAA2>?NG0}-&gY`=>Zq(J(_^fojn14wt`VIi$EYe?W?d5!2nhHAr)0R&EbOOZpx@6-oI9Lu`!!t|zDtUkh z1y30PMeQYk&87haOoun-zV1q~5XpsXR&FkrhCcnZyVV?2Fo0O}^S_c-d-EtVf}m}! zJn02fEBNzVTn!Gq#h9!dQ)f%R9dJ##POTlZ!aobQuyB$ul7m~j?UBRNdkyH?mu%l^ zUysNU!HBv+rRSDs#}gMj@xWYAtLP2F2_%!DNiq$tI9ZpPA4VEC#Uw>!js%B+2-&Ai z|HCJw^5Q|>c&aztduQhz1^P+Qe&>y+sJ=q5ICxsK@JXfV9l#z>jF>h9ztF_ApctRX zXpjtNT69G!5ZKvJsT9|ufM*&UHu(9%$df>*^OuM-4Hg)XCk)K8QRY9hi}RxClL`&? z1!=B?93Ya!@MY^=N8eOLMEH}<{1o|iT}Q;K(LB^~+!pU}fP?#AjgL5y_UaV&E|^Ef ze5!pDe~zYWt4~b7B}2?A?k8WE$)MQ?C8QT0*kD_9orL2dC^FT${&e;h1AGQK|Gr6&7(M9tMj@4PO?VbS~ z)$&M-QZ-@v(cwBkunz(QKcp|c#O}E8h{q&$;IzyeaLsnfr&aL&!!B+ zr9w=r7In3uAg8I+^URSd?aJ=Q4o67IgZ8D17}W`+alNXve~?__O9&Y-4*~MmBLqeu zdKp3nKUXE4`unQq$u8Hgc?&;3<$4R{*;{DD@Y?KH{}=MtiY_y?^CmOM7J53?E6`zV zBCpJrz5D#LlUByvnt7_Lh=)-PA)_aQ)~Y8EMVt4stlWMc3%gr`sop;66G7!g09t7= znn^bpKz!$j1I+nhgIL7nKY#vA53)=qCMK$kwvV_6%G|`loMyYn9&rv7+MK`)`JS7; zpJ)0F`)t_PK+HPWfkSnJc<%H{7q7TMnrqNVaj@V?`Sq8|X%_ zR(_5acX)DLiCLca`}adXZh6h2c^QzzM0@vhe9Bt@_2^vW-`TQ&cF$!pG|(HP(wKGY z-ULV;^_lANmAZL^-HV{ji48lW$1qQZHWqeFevx7oF}cvLLO1!{VdLfE3VMdD0`T6t zx$|uf{It2WNWzl*M8L@JL6_@I#;b#GB{Y~q`6N+i{_2(f%(o8Bj(%(N`S}@wf8I&Z zW$}zEL9SatCj9_!(EWV#A^Uqxf)oHw)CscmOuegR1;eEVqyvy-Mpw-(4E90A@U#0d zkbNwxERfi!p5!M_j*ISe>u2($>hpj()ir}e+hsvlN_07>dNgO@bc;IC#K~%mwvnjI zT6cVi;?%6~^($@ETe`&2I{BV9bL1a{JwEOtIeJ)5oe7zu%0gKeu5nru zF;2IarBaZ9xQC4pU5^;FN9eX{g}FeTLiGnNs07v3cKAw_97c|75&u&9+;XYXD8$36m28Vq(^?HuaRWLN5_Vvct%JpmL#4oF3OzD4C_89&CLvpK3_d<(O zw@#zviwz~(6UsZoL&{S)e=)VDnaNW?8P1KYJ{^n-nME8@_HGJ3DT9OiaGGlN7PaM^ zgt(NGpH8=y$S1tJh>Be>LCwMf<}5f5BRf0hD}E=B52|wVIRlm2RU#u0BD;LnND*Pg zN6?m{RHTBi)b9@{Fo8qh4*HM5VF>7tuu~S-jBLP^$(MYe1fHoqtmzBEa)JI-0TdNk z6Hd!NB^wG8F4z@35 zttuKiL&M{!a4_>NO<3R+l+bDa>T0XoHUvO6$M$q^m+yBd?&IbF0&h8QZV&ucz1}he zya9O+C}MAn~P0kMW>jd>Zh zA9(^<|3L`1>YG-6hUwp|dQMfeQ?)*z@%`v!BRdrG=+yN`$ z_|&>#aO%4PwoS_)w}16mrCxeU;(myf zW+r7}A-U8`7Nn6JX1jgY3k>|ISNbLA7q-?1+ywJXyWXyw{3IaxNZ{68kHkoQoLW2?_i)GOm<9)G{QK2tY;u~K0wX~9ZOoH?~K5;Uc zQh1mIl$7QMZ8-NDz2ki%MpnKPG7>OQnvB2KBp~5_Ks>ZDAmEGFfSy3&zdpEw zM#MKA#7AK7UE$S1=48R89ugcpcuL6d9tGRy4eYxprSF*wXV7fhxx>FGEYoG^lrXROvX5$hozrejN({R1W14wwtAl$XBcQz zzE~_moX#yhb&T|XARS#e;RW<$pjiW-1Wv#l~zBe*E3#Nt-E)T zw-NHk$3sy>6VXm=f=w16&3(vL3PKc2zdMEE4>-bfHZ3{Y&bY}sWYjp;9qqNj6B)-W zmdyM8`pTABPob1shjN6ZaX!YjIYrINn007;6>H*yP4HXd6D+G!f6G(rG=15RdS+tD zg4t@!fz5N_(p*rGguT9?ZZUnB3az@hBN};VwV>BsMA0hWtS2NUD|4IB=O1jXr9GK@jMNVlWil zcOBt~7-=IO#wz=CWoQ>1+B0}9u+#A&;$E;2yu#b~r^YcH6h3rwEB4bR?m(}-7E?&@VQMR_|#5vPnBP!3YkE2`Pw4lPQzQ zaMXJcbJK<+wPCWND%spd~S}jab@AxaNru@ zWhTaTrnvz4S53+;vp>VqEgs|YVV;Kv-*OAl8-H44o*Bl-2fVVCraM0;hEX{xxi$70UP`*R3yEzdHa3B8Hyh=x^!vIZCTg%?;)pu80nP3lp z0_pnxozz>l&H!eqc|qd>xg zV0jmSz=7SPAi;q%o0`JuJu9&!k}1bS0oR^qrp1Vat7XDpMf>m7Mf{G@Ws&xuJ@XF~ z)TE4f==E!-YBf6;;JyKh zqJwiNh1v+4l!*yPv0Sv~pkkMa@<~6R#7^tO%dc13IaI@JWxwE|Zppll{KeWw_ha@thm72|u9m?uReI96lkOM2|m5l$aGXKvgdB_@hc!JG9Fd zUgdweVn|VrjpzaK8}nEi+B9*eIUSO>47l0RYxmnfX$j8RT2d&pq-4y}CtW;HfZxM2 zNKXlQ)e`n(!Jgi+epRQ8dM1y^H^YQF&N|nJ%X*HI7XR9!SXu7U(8H3$M8V_V{eua8 z#4b$*eia``=SpgegI;A-Y`m0koRnkL5i2~aosR=Hv@y%I4a}Aa7eME%$T8#ovCs*| z4K@ow`^aM8jBY%rS_7ZDqq5*4ebtsp_~olW4A5~?;TDfdvIjRN9j#{l$4|j<89Xf3 z&RDoilM6!GeR1AJTIpb`!Vk7;$Ge^HUe1RTLT_F%w>IJ=+JRIfVx&ysgYyIcRkDZQ zD6ST}W@#4$b!9&wJkh$C-(zXue#IbrknM6lr!IO-gyWo6mR?eGk|#e(}@{xFn4dE5DB6=5VaGO6Bx7Lq?Lg-?;m$l3Fb z=*Px;?HBbEiq07ZxK3()QJa*<1WIPQBTwo1*sr61`leo^epY3n#?N_$cFDVX@dgrS z9X1voc*X%9Tl<<_ZN1-3FiZmyVDL21x%v#6lmN(Q+MFP4xrbXI4>!lQHZ|!7hE18# z?)?VRf6norSnggl_=Ga*&?vyV?Fm;X74@FqLdai!IHQE$^1e>aQXxTbF$N9YDj5%8 zBRv4a`klm%0%q}UdAd!WKrvhvi4$4a8xIEn+_VB(Om){)ZzL#?(g2I)6s|pbiu{dqF!*|dq)pH#Z zr!Z^w4twfiXVeCb6UXtdqu1$hZPFE(gmM1aBV(|D(u|?8>VAfs4am#|m2(+k&X_Tt z1j&MmRS66<#b31Zm-5c@oy5Y(QK8H!@qn=S85;N3zx3c9V>1e3lm0QL$ma5lTNx=g z(HK3#?BX)?U>uO|MKnV%Q}-aWWGKiQ-F|s|`4W%o?ZFMaImU2Mv`VJILKt&@E5+h3XSF)!I%TdvL0zvuO)`%4gg!sVWJwaFa%>O@Zv%RyH zD~;N;U8#G0Y}!-8NmM~M`(zPYISt!Igl=e9>H3@`WWXpxuWkJ-J?WU3i=O<7wW&N; z@c$f5a*i22&qWu=rUX-yvD`--nk-xbVSaRE2}Zj%*=c!RymSX&$j3LQ@iMokD`Ia& zcjpxyBF8z(=`fB;L)#Y;n-60812yNq#9&Fct6SL1`bmomA=PyK3!P}oa_5o<_qcR zBS(gLE$R@*VT3ooVwX|z(zlxGoAr!mC;Jrqq8wmAw&wDctF7w;9`&ncRytKZ)8TLB z&)z?u@|1L~4QkX@L=epAS(tl_F&HzSPz)MmZ_KyoZe}g8!qvW}tsjIz!@9|Zb?EzP zwv-17ZBZ^WbZdsMuznQV9gr19h&r4Rbem@-LpuwcUY)!Ie4lZT$%(Bufp)tWsn|tB zOrc>^?=;){Rd(1wNA?j<{Tf43_LGXQdYO^J%vh`Y#vU}8hr`K{KtjXKq8g4t7rqte z@M*c(`mMd)LDzeb(BJ$S3;nSon87GEta3GGKJx8e(hn-tXpj|rXkHDSt~RA6S!~@+ zFFAXzKzzd=<1v-w0x>{<2=N{nVk^6rOK~_ka^+#Bh92?j1gHl0PXI z=?NiD9*%`)v5hhDxT1A<_@S=seH(jBv{Q2nyO(|F>FU}x1sW;TSZ<113qW~AJC(~# z>Y&~j?Er$<>H>oL`5~4LyZ-KO8)oH`Bho2qF>x@5(&n?fJt>T-vIetnz4eM%j~~A` z-&yoJ%}eXBNj;_o2sxl0t1IhTA1W}E<88t9fqnal#d%7aiLH0Fuf?%)QJ!nxYggtZ z{hWJJ4Sb5+nC7d3zdkLRi&h|=f>}PBe!n&6%_JsW+0HcnyAvgzA)JneMG5qx$tuxr zFX<&T<_n-MaEaj4o$+yjaSDLDxh@c-+(BxCdb#vV_=pKFJ^cp}^z~ zS-)UYdepwZ)z`wmcyifnuh((hK;~$X0f2TlBQj~L*`aNKAWo; z{SwbM3d~WYSx35~bjXLh_n;%exx|L;>TbqqL)^r}X>jj{QbAlxqV!y_0xlU5(8jUI z;Br4MuP4tntT&PzY5DN8^KyfjHnfbRSp!_M^<7R0fj;B$n~33~!sB8=$7%%$=qxnA zJi|=#%nh48f+i&5lLWhxIsb7Gykwk$PDSGtb(Y=}34G}v2cJX;kf!GHSe9rEv=ZD$ zYuWGa;Oh5c`#EJ+i7!qaK4h0S9WgZD!FPEoL#!X6{?Kv)RbEzVUbPh&we{ounU*4v zlrkCP4)ihmCh$VvnLI($_6S4sGF|ZH@e6Om&fd@VPDJ1I+nw)Dq6DyF(NVuA|NS~; zPmS%2*bz|#yJ)fJy6_#;X|^>1U|AyEc(T|(a{dw*>%FxavpKU1 z=U%+nSs^}hxbNFy42%znk63Sck`xyzJmCuTfT_!!*DUE_(0vtH+2DVf7BJiIx=fO( zxH-lYGm;~_Al}Xeh`Hjb3Er(5GvD75x%W1E=EsAKP4@6Uw*#&#rmteK*iFMQhT{>$sw#cfHzsot6}=q1XS(# zmzC!RO8nSdHaPobrh5+~^Uj$YjK4f)KR@1}Q?vDsTXit!d*f^muduMs@%F5ymYCE} zz}9D#b$wPp`J8&u-{N6UDt2h=Qh%8psq{|sQ(%WVXKv%bJ2Si={$D~wudz1A>bXo^ z$Qmeo5atCyP37L!+ehf2oYanf$BG`G?bKJz#v=DKKe-^ zzp3DtuMotLU%qgLyp2Lbfick-HT?+wzNpFoug-E1ubVA zeGUSRbHSjJU+GJkdsc{(>xvGDmp^Ic7W4sro>fJk(!+;tK&8=0Q5_q(#Kr!Ii(SfD z=+#l-C&bA{Tv{P74~lj=0_y9mOHW}KESTDBH&5g>shKjg+oswXNSdD$gUwYd(c#lR z&daYbQ!Re%!?vYZznNyAU7Z^Ght|MJz~Li|$Z~9N{^FhcLS8aqS{XWD!)*^Q61Nmu zHCfKBNymA4VO~F?cUtyb+?)`>E&mjBx|UAxGoQBZVmdi5XP<v3ISK(QL zfY50b7PYh5=TqL*mcCmbC7nFNK2ogrFn!?5*Qe>&4Z{5jQ#Dp00hnm{Be*^zksXd0*R~MNp5bEvqFYD^-&bcWY^!k6NiS0rxAhO*C)is zAUzkdd5L4;iF{w>H! z0V?}I1tKH$gK`<7AtPN=?58_;Qkm<HS3W=aQ$!SFhx!NAzM<0UdmeLZsJ$U^!Y zP|u+d3C8LAvoLkzS27wr77N15{A>JfiOGCSG~0VEDgBFK7V2S^K{?g#*jEXc~rB|D*^M1K=~Xp~jL9$`bOPI^5o9QAAlDmx;X zuh1Z@uH+DUQ-^bMI*hzFd4fPgWFJIvpb zs#r6#yS>)xCHp&MV!&e}*vylCwktQXe0y97^8h3r8=jI1iH@_WqPcpg_E-1t1Ba!~ z!#D)3@aiLO#6Ke-d;9PG(1^n?6~@+z9k4o3930}LQGt;PG7jctCBze;c-2yY2k0;N z`xTyL5V+{s|Dvq2o%omyH;9bHQk7GOVLXY&U0R}XK0z79NdZ4gyVF6tV68apYo9h| z<~w$r>#t7G-iLDK$cAQ?UY`=8rn(wTN0cGRlDmY%0+sR~f+;E_sA$Krf{?fi7zbD& z1KiCmZ~^C_=QpkuHUgLU{m&U@gogo?RtY{`pCSOj(2u*G%pOJ|b|8fBo9s&P9bNkAhA?LH?%%dZxXtIp?zpHGlKA zANAvEy?h(R^JQMvvL0*SN&jft0m8YbY4@+b+}>_n#$}cA;{m9bfq}!1707V5rUY#) zzi)v!KKShIbd4IMekKR)d~%2iZyvDXFf`D7f6EfzXATqL--r)tiYIEazr0~=qCq$A zefraU%By-$4){T*=>Ky8MxRg$3=+uzqW$2j-y$Cv2$C6b3(Z4liCcXCD!!T?$#K+2 z+_Lv#XxO#kr**eLm%~LKaf6E;0w=xG!9o0IBog-n{GKeG1(Beo6)-Oxc;sTT!nO$e zF!6rpc`sbhiV%ud$7G=nh4}Px^HNGAw|^m59#9@VO4AM`?~^AVbK727#7%X9*VezN zO?!V?@1Gfw$^V=Yk#^jl5!nx^@JWdg8^H`t;0M&oDapO>`i8_`$PtcFwrJFpSzv@y4RJK#O2llz1t zk>Ee94Z5+k{N~r_arRjwmxtLvK(8n~pwxbOUZlkp3sP`*d$iO83IF4juN&h=Kd<<{6@M(L z9Z3O<9wuL1#IMEedFm$V5?^8LuMN!#|6lNZsf`;qlx=O`dg{Q`&v&rw!(QPZQVnl8`R-o_v2rnESeps{t`{N zonF5bv41K7=vduZ{95A21NdEik&S!#pnG6cFlVvoluF!;@vRjtyUewhd|Or{_bCB3E6Zh?&gQA@a%;LLWpbjj3I zN=$Z+F-A2MdAG@N2G^YUlL;!E3$Fe44JN4HJU?*$RUsG`3w|LAt{`3Kuv^@Ktk!&m z@YAk<{Ue zwD+!mqJr4E3UfpKFJ-nj2n7a!C787glN-x~=h);HvvuO-T_8WbMTFp7#0swLO*pEn zzkvxlRb!d67G!;1zdiJ@LluUI@SwVMTqJ(t44G8%=P3JEA04GAB zRCVMk)v+DBnTQDkVV@?TFd-((OaOOcuYpzYjydW-a8g$eh&(<*VmqrF(6_GC8J zVPm$^9jN$^x4!V|7{5Rfbt=}iGzO4=d9W*?_iMUdaLJ0NqiR0UzY|!Fde3tNFS75H z!Knn7A_>J?u$#(P@rFQK-&rV_|3oEz;yXZu{HUM`&_e@A4hE z?1Y^^n^V%by>ctnu8t4}o=h-}tc~v7!JPKLJ=R?_ z=*Sq6p{~}GD3}cT^vU6#-c!+fXi?ojIL>#xKOwl-bn2Q|yY%+TyLORNU1rkS(G_0a z0fWm0EEV{)$vjU*PQPBHuaI8Vk?rTHw|bclc8BOmb$cMr+iu2_&W+5?^Nbj+`^{6^ z9h1hB0fiv}%FeaI&xxDsSr5NqH=S=gu<&pR$}JXPdDZht`*s5k4yjFmb9(uDw~*N?JNuH zD;jW#X|ijI36GAB4)1dtFEBQ`MT{)gPI0|wiac-fR+{HlQ~jCSA3G(ergs-MMqbam zg=}7`FL!Fn3@Ps}+4h^KY;vPf%2OjH6T-*4cha47sbsF0j}uionegHX^9B#9{kMs4 zM3?JUp&-hh(i{ZZlh?luE5YjNh^Q`jSoR3eGm9{=JCJ4?-G<~hBY_Eb(wYi;x$xscD% zW~X?9qO7$U0Y*_&)SCLj-Lkb97O}=qwK}VldJzK)SPylE&1(x{5w1LM+S#m`b*Dk+{eb_izBnJkS-~;ZW#)Gpvd#nOm1o><$L#y|?lrBIrwDT7U`9k}6!SsK-?--E7n66M8}o_Kcn`VU4Jh&+p} zJu6EYK1?4u;m78hn!(49GQU}&XTACEc6}JpZ|zD7tBQWrcP;3xl>b4dviwE8{m74?Z(e5di*y86*s8egh30_9ar>lD8dHozLj z1QmXj&p&@V2`T(~W60Y0{CY&=dVkfe**1IvWjr~?7Akig?=(GMbwc#$pdj-jE)@Ta z>ZoaAs>fRv1O;r%4HxflcP86t(G z2#{~dgxFmNJ?$p4d!E$3>v!*4DJjg_eW2uH;M`p~?JnzG$7X($vy2=fw3=NXA3orrKUv?;N@J~pdO`I zxZ&o_>r0Hh&*Zy`8oesRGwW{AoN6n9x;)e>J$M>Lag1d8x{B#s;`)l~SA}gWV>7;WNvZ)xs!mUr%8V`UvG!Z`XTC%axu0N_P zzft9rexvSSYr2VsU44TX@yIz9Y&TO5~NoykEZ4Y@AP6x7o>0dLus#G}=C5k|{|sS-OTuxF&Zu1Y))~4|Ax$!t&6f$o_Idt3;eYs4G{En$W zci?G26qL~IUypi3A#_R{Rl3ruREBTVlW*}9udN`+rxk@Ec@>B!lKijrmyc#EcAHpx zXfu03tA|5<%)%v7Wpil^{ZKbc`1d0F?+wM}@3oZXMP^}*MwPj0k;btu-8!O=<1)0% zY>PIRV`&P;G*o%5mPD<-j73~9MSh=GKF2N?WfgJW#2owK#`|3K+bgjoubnIhj)#c+ zYftEDFm0Oj$*P{)8P67Fy4e$`B{tNp!_Phq)s1Cpd6nN?U}l^)Go3eJCNc0=n64&9 zQt&X;jJbOAmg9+-I>SFTFkjolPXGzPffi9b^)vUK}fV2ys}S~ zSg&WY8exJgzCEm(hz)0Kh>W~{!7L)TQKD@v!EQuV>hsH~uEY{>94Mc?P-LfnvlYmV zZcb*=6dtYDPj{P;QWv(bg|uA|X}XyBHhgF8WUdF6QF0(zC8Dzya--ao^iS(a%I=%r z2J`mM-E%w!-Ept}02DbF(x>5|6u_%7?xHwQcsh-Tmkx zA?HnwS9gSBwU@QdVfnbP%@7`LI-?)2jo8WN_Fag2ND3jvAeL}i1a#shhEzut**(5E zc@Wx+n?1dH#rv4X6x#wFfBA<49boRY((&9=&~2^Emc)|cyr$#$A6plJqTAC5PLPkH?FNhs|!{oYZt zCbe}^h=A{usr#F*?OqI`k=fvI;!;eT`0o|dI<;OXZ;Jb&YocC_{5^CU+gGDfC&o(2 zhLS$~S*0P%!NlOD7De_vPNR=_xo+!o}(B=vu7y#R_u$~hYXN6jfxB!!@^_r3e7>`w0Z z$(fYXltOvsmH6XBe(ZXP|K9S53$qF$t511Fk=yZ3@s;BAmhW`6GzPpd=m?pgC913h z${|<(#0$#F;+f)Kr|^x2-p*gn#LLw!aWsO&n3zC0BX=RX4)^TL|Mu+H&f9hS<%`9a zGpV1?_>@qo#EH6W67na;qtv{%+6GBvC7!e}cL#pwWqs<8 z`#u7fkwV|KS}_6LA-uM}03H0~60;H>l)V!d#$>cNfFnPfcE^ls+oL$->hQbG}Asr0_U+ z?R=?hn-zrDkwB&qpmCHJh$ou=7Eka)i2iL+3-NN@H(NcT- zWabLlKnfud)`!IlIoWv2(DPlA5qxj$J(c-%iao{9GTmg&LX9^vPjTYdQU^!(n(KS#A)p%qSwdu;Y{NA zWa8^{4yG8<_f4sf^t_TL9fmc^b;PZ9eG0A6Wk#b5l4T>o<(k{}gB(Q2GS^iB(Q*=g zJdTngJPol0b$=ua!t}TkVXQqolA+XX@9??4+IR0YaaV{2=7q$7&Bmaa2K21FD zw}ac%6iVyc%0rLTp9-RS%M3oHId_{R17*i(DnZR$*M=DRltj<8p1mp7?9AwI1GO0L zKxGo7{(P6oW(}tYd~tGUSHzZl!|%l1@U}dt{)(kXF^t9Q+X7W zLLO}?pR(~ZoT$}Kqgc*8y7mS?Ihl$SS-whB{r;bmO@r6P)i<9@NF}82Ja^BoA@~#e znQ_cHjK15e3^;fcTkRUTo@K?!ra$e_Ax_-HLn+D34(nugroI&U?zOWD4GJ{9R`seE zQSMGlJ>%>tQAb{um^b4|4#2Pll1bmWv14j=)iq47b>O^NcXhgc$&Zc_JdLcZiSw!Y zP3KK|>t=fMzjcK1+C7QZe{HF&QXmj*5gq#b<&BicU%LXLXCuT!46Y6sV5_8sJessi zOp8ETCe);@`27^FbSb{Ar2YCar1CyUV&tg?CR0YH0W6`MfQEB83*wBu5(9#Jy?p0q zmgoCBP2XdGt)v{AcR7~4e&??DRHV$uQM@)?LJ`=-zEcAMJFmTV418wss{i`z|5+S{ z#4&EW`oviF-lg_yE&7~JjGXwAI8G9Ck zBz;To=Y1T5`%okG}H+7r0e$KI8az0KC3xIX9(Py6dI5$N4`?_~g>|PCnjvAu8 z@Zz1-F+2_p9p}v#`a6{@u9NZOAkBOpF!0Y9ixpy|1-Olb;w>X?KA!$Gz&>~0w$G$A z8_#aCIUa(v<>=Ga%75}VQg8D&bL(9`L`ZE>%j~bw^4HWyJi6Q0%kal%6|98UUms(Y zT8GLvx9pr%*(NtL+efhv1Yb|rO0zu+elWy_jA**8MV)WB+98#@bSzn;mO3=^TiRGF zwkcAiO*L^$T}|n|u6J5nv*Z^m<7hia2D^F{G47XP0?|zc^NRIZ>qFDJ8+&itW+tG& zl$OyZMJBYaws)lk$Zs5+{Ke5qB__m2twEbwR^xfOOy;!>0+0FB{od@)996?#ikwfQryn$LX+m5F014qb=^VuLU;E7kAA zuj$o3lgZbzYyL$aykxZ9X~me`?h!5gl=!a zYZuQ-k^H$gerz15UbX$JH0POMJHJ3VhV-T_Y~=vlR%^XUpKBu|=L)NOU9lOW z-gcKD_c%tqcq4Zd4`;hfOn?S~?#e@KQk`p+n%*uo$BdCno3pjDJ3};%yW9Cxv`l|R zO1wue>?@k6Poi+e+P#Zbstb2h-h471tEC)gBoktgn5JL4O2v$X)=;<;cbQ2opY={3 zr(dVW*VPUG6G7Pzk|I;+sD-Xz(3+EJOTnKqv&&dP|JSnEY4#y@Wedi(gn+u2U7wb% zu`2M`9XhoZSxxnV%#TxegYfJcei*LhG3ZRYMorI7C0}aUufjv*%Wvjh+ziF%$)Jzg zt@YaRlYZ45EtA<6@sm&^<@IS&B!sym2yO~eBq1p^KP}>;CBSK-p!|^%Bt8*kXYQSb zef4Y3sp3TS<@C+}V~0f|c~P2bHwOye4!b`60YdLbdq>Gy_H!_~&&$3M-@X?dlFPsO zcr7<}7riqsRzdCxaW!2lD@J;36IjejWI|vfXL|q3;|DkcIqz^>#c+m_Qb_UT(FcUe z;@I?DG;d1O!yuWh$IOUdOJNl&mM1u<|GFpf>KLayV;f>*ev*WwJw85Qm#Z#Hg#4t% zpNAp&sp8Szv;1P!tgP*;;QsaZu=XIBrKMt$EK5%mQx|6b&Sf9CeN)3NS3n^^tdrqq%%Lh{Phq5l^1pUjI? ztL$fq98?F3Uo;l1KrLLcJ8L!j%54;v+}M|0?X%`)L5P~R*WL1;nv(}_Wv6;3O`{j1 zai0t684*FpL~43%>(Az^ri^00*sNOrRB@+Am>y#|j?GahW=pn`#4l<*h%6TeNokqf zV>vAkkQedl{Wm<}K_TQob#=8|(>s|B>GgYiK&8O4?+9carg{aIx=Q#)C62S{%T2Na z(`_lQ-?OlVa$|JtUX@*M0+(lg;?g0!<>42CqPoTQ!ceW{rKu-APomA$DY6aisMyG- z3&tL7LLi|hp3T?wW5e7B35KA4>b)rUdRj#R)DFzT7*&a^5=l*cc*oC#m_8m3Qy%## z(tIfM{?ei{>SRhG2FDvsG13;-Fj@MtRwyQ8$fjwOeZ%gP0!;Lg6boH1E7Oj26E#ju zbRC~&Dk4bsm$OpO*Mewoojm57(eb`l=IXEv=e%1JM*2ee?xa7)WD%(NRuUoo`Toa?sU$!z@c#Pl-a}JId~n}~!AC2J_{R zpdY1R6%@Z}gj-PC`eGvj!;f5aj#hZWs?6vi6^KnIXWWj*@ z1zbHKaFgXA@}Mrgwe+5P=5|pJM*l=v+I4?a4H?!3u|6#c`@}EMPH8t3OGnMY;5fZ% zPin3Bx+=xF#E=40G~pM^L=Edm;eHn)(vz{DCW8rhw^BOzMm3v1GH)05M-FWo0(yM? zAf7G`{ge9>%H_lJDC3V640uMqOff^3(3GvWN_4N|v@U6tMLbM`-YA3cQm`IVp-PoZ`J@B`19)=&Z!0{lT~AJr~48TY~>;H;gM)=vrJ zYqrBQY2HiGc~N;utHa*a!%13Y3h3R|6a|wu&ZSlnl!B$rzZ{kwmOIRR2U4D8O{Gnw zDki&G6_*s2e1APsSx-nV#?y4C0HVz388JiVOnSJq_;e6&S`D+G*DuAfk<Z*XILQ%WF`5f+CUE|~=#zwo2gRvcynl;ym`Bi-YTY!=Hr|E}Ej zcW*|H5X}_h)}bRZdPZa@JO#aq(KCQ*Ao+>tYJ0kVR_5#_@v-RZmp0O-uJ%b+u9<6Y z2KD$w_^}lZghqc2qdBK55o>jaUpM4tmP;&bu1Ra*8oyspmIbaOuUlF{VjIhP1$mQ2DmQzjgzJHM7Ue)HHy+8Wk(Z|S`G1I%2$PJ*N$ z(e!DZZ8WX$la;hh#AfwGdCDAtX+8FjkDB?KCo*oGkpwYwtCs2+a z*H4}L3+MRrwLmw+z=^u#th3Zq14gCd#kMP<@+u1SZzD6d%^QAjJlh778}t+dz}&Ke zn=1y|jwy0}X<;cP{UZ_Y>&@?e%yC6LcQY&2$gJ{-rCe`U()4n0EpYQpYU7Le$rsw4 zqngXMzF|Nd!_GM8GnGo39Fvlxs)5-)lw}otBr8s|R8ImbOrRE6)(>s*)Q|M5eZc6OWU!(XR5A4pu#NlC?I>eg$@B))Z7^qnv-Q~h~|=2|qHfK{o) z=Vp`|Cb)f-C+|?PWy-YujiSsKX_`<6kDZth;w26}-*-J@ea=h=J{|BBF55&t|fUmIJyS4iN zD(ve#=Z;4etELi6j4pQZAM9v#wGRpMEf!9%2VPS6zm*F(h^VVArQ7=)1MVix0pje3 znE2`0G`;Y0lbqWJaI=3=AsmQxGLCGPVevA|L>%O_82~QF!TS)jyTQ{ zD07JY?`I%F%2_V#qn5nX8~D}41IGT52N$b%9j=c%cyQ1EXL^tKhE|WDsOo>@f_Pk< zC?Z?GgLSq+(QpWC$SCw4GzRc4%X^cLQo|d6d`XNfaWY>OZMFG-&An$>Q(G4`8dN}0 zK#w2-0-{F+ks?*PjbouIO*)8l0#ZT=T@KhmMIf}$B=n+z&t0-U7Ora zT}mp(yl@I^V}7t5kaw_z3%@ShtS@D%aTEUhQ)kYUs+L;xTEaXn@+er5|CdN91SZKj z8(dhjfPB}=+3nTW+%?A1%Z*3G*S|qR`?_&f%tF; zJYHYu@!I`aXI^vbX>;Fy|j6)S2zhn_TK0t%b@ z`yF;czrW|V-yZ~9t;tL8m$Sl|EQ%Zwa8@`coCn7fYHrlIoPZH6f>9$T&~iO{DnDuY zkwm@L%X)g1tU_04=oDPG{W=n%-IX_i5~XOd)$3|2Owa8jb+U20{&za5@N=BK|aU{&Byo~^sPqb8kZMUE=>zB`Li8Pl6P_iCMNKB zi>CZnj=Z$(aTqcPl1`l^VY4=?YkxbqrbyNuV!XXU$TaKuTWJDt@~44c2-dtLZz8n^ z_ZUJ$gN98b0sGju@It{7OK?nBwHWM?$c|$G)qg3)7_Zz@X!&%W&*LKC1J9BGk-j-A zPacKl3+n|Bu&aORq+YkV6^m^xMbyBkR&BaFx7C#8$0g~1iZ35mDsvf-(%Hl}XRqYd z0;vgZRDc-;$in9-mTu72!Fab?Mk)ohqs^DzOF}pVfB}DldSS0D4FjZ64EiOi* z*<5=H%2X5ogTs;cd#yQVxSY&!`Zo8OmHT1?u9O!&PA$RYT($i?wg(tJL z9BVxrH2FSn9Du@vjV<;y>qa*)ksdh-CE7+Q_AkBX5}z7n9BCB9Y_sYy)r<(-h*<5@>{)3pfbT$7pdpo zzBZDJ^mKkLS@&EKi%F`QK)X2i6xmTO&dmXAC9Wzmt2;gbIC5xd^AlM-K$L}@wTD*3 zyUjSgANBJ&I&uv8JSE5SEIBov~`p%9rR zU_D)BxW2md(r#!m&HgbTX=PViZ)26%bW(BcGbwyD9~Gk3*EK@#n{Te=Un2)U?lqJc zODZMkl#LfzC?)k+`uFXrcu^Y^SpThYpgPIzMs2ICTW34*OWL`0M^?qmRU%w3Puxv#c0Hyl>7wv)hF{GJ))C*Cd$g9yJQwX z{w=*OIp-pz;Ho!#%3|%lc(F;@Aj_kZ@nXi^8{EvO5Fk^5c7#87!~sL4%#@ASIJm1w z9?YwA=qN~>-{sa$Tl8TNV%4YE4AfV@sCqS)3^cFX{>4i81Z=t7F0@gV5v8q4WGyVb z>HXhMFjyrZuq*FDOK5Q)$Q&BMj07sP-dNiwf$1qFs$=dlr zEU6wn1g{KS@mwmd(Eo60Smx7);dTaDRXO|{o8Vm{63Z?nd8QV$rs6#lat)@u%{Cg< zb#@_bx{C~((56L#C^pd{m5Ycu+Ob1%+hCr%nT~tzgQot}QLu=kK?pMZYg=;!b)vUA zx($80MGYuxdiKx#Ra9HWVu!_O&y<}&3~6=Iw`s*9WDe6X$%^Yo15B%m!gC27@uyaM zSx^ugpeo%Jvk(r%a1)cJ@zX)ME)ID_3x55c>H_0sbHXbPFY5H$c-NGor@5BtiP1VH z#saXz%fH@{8dx67KQf3@SJHaef zD4P7oozVHMKYa{gRGVNu6Q)2?fLEge&`7>b*YAB41Q{4*M+s;VF_JCAx=J4*N#CqZ z-^v}GH!8{BbRWs7Rw2aoB$dq4kTmDG-5OmNUio7&n2_3svnpK!UK2Qg+xOge8_Y>h zkQ+Vf)tAOZ%E^o>ZAy2Nt?7wSZPCC>$O0akY_S9?VQD*^5^DP^1i7QH`&b(_PmDDQ zi6Hm)1c4fJR<`N@O*w{GYR4r6bQrLXOCW3t{tBD@prm=%p+S-iS$PQMZ0sC{*_O|_ z7Z?e7$%EVV@p8t+H=LW;4#kUa$R0dV&WV3FczdX97a}96#|dpJk2iWp(7~qXZhOxZ zzf?{T$uU5kac|Y)|1&bc==gG+L8u}CW9~H8*a5BGU7_F#h=O&k1n%TMl`#{!PxOTg z15TM54^ZWgbqZ6Vczan0MBV_X9+3XSdak!hk&$-WW8)lAdf+fC?C}1@5DkXaV#xbt z#wEhVT~b4GQowhPH~FjT49-j&;1*hd+G|O_E9gvqw!Il3BrM~Ax>O)wYJ^k{KwJI? zu;nY!?7hJ;d*#E(29`%`e1_$q%qFA-tD4oUyyu^hlO4c0!Q*>)tRWffd~f;82uvn$ zy4q}R%>U%x!hpHTZU9_xOkZ2u94_a`u_sAChMVVb7m3Wd^kGiG4Bl`E7V168xW7?6 zSryw9e`;Zaai7mRQdc-W>!I3d8o8^?j&{adI8BEx=+Pta#{1(r>+dY;8G=uyKOW@Z z;WnreM1i%4j;->4g7iGjiZYFyUl{hF>jmsjgHj3ISsxmk6i?9Wy13nDO9o7sIAwpd zY3-`AZU0^Qw+8+V6XZL@6cM$460|Z7odK&<0V3Z(o^QaJQ15mR20N>?$k1Ro1TsQ6 zfH+14%5;mxpd_AzvW)_#Ywki=Tw#u6JMoZB%C5CwN*-1K*!@`zz%;4ZFREhfTljE2 z*}D$}F_C5x2s2)^L?vyDrnE1Fu3X<+XX;M5_JIq*J09-{?3AH_)V+-0JP0!2-ROs3 z!Ojgki-h9B7%0zTpThJX-|mUo4hz-lV8qwgqkrwbmpQ>~*m}GNtUY17U?Lp3^@1CW z!&@7K;LqEFwu@JM0Njt<4A4-@@bPEL(6v))pTcH*0P?F3+FOMvEVSUS(EAl23xyq4 zg|?Z6%_?#S?3Ua%t+(u(xp&n3A0I4!eLU_y42Vt>>_3f_&oJcD&Nwnmfk4fdq4^iM znYu72BFAsli=yw|1b!5qPK$$I^Xxs)mze|K7v~n5JF65xlBH5Y82lneSbHrQ>O}Lx z++CYr@5gtG9F04#gi4ihsruN|bb)2kYaxWO93tA1IHjIrXLjqrz>$C#Sn+3B{MI`D z6WX9dczFA<^YkQl^*vw!urGDsK~y$v-Z_@*dfSP}X7<(OHTv){&lKn;xoO5lt;2ml z3#Y)Jo*IG?HYDkD%4ruJL#VU=YaIPWdlL9(CdTM%+k1|D=Wy;jkRSl=M+O=Iz|z;| zs@+?Bhl96s6|b$I8rl*xhVphQb&ep{h`li87&wC6 z4sImU8>jK=(GJvbur27-c1gS8q^{|^B_;j>;_aBk7Nl_f@<6PxS9teC`LM{LmG_IY zghub_&X3poy2J{;33R>^msTzjf8}u5!R*hbi=nx7#QPQWdPNFD{ELeqQ|x7#BWb47oTn52D7Lur28+U z`(Kevid;dv=(r~&LBa;lo;0f+p(2*P1?Uq`gUXvT`FhQ3#jp2{&d9bT$;m;=jB)d{ z*=c@3QdGEZX@Kz-hLjHM5j` z$y$z4hxqU$UkmIW`*S3^&WZp2xU_blkoe--e*PQP_GKW24Zex?`}!?yuGcnN8^zLV z)!)`Jv?=j~ah>(sH6{;X6n_AIbY382&BOl9VBfhR{$d?1)bR)4(3QjpD#@>LC9o{u zQvIlrxEYFDlc(=h{gsF~z+ET^D3>a+rl5O3T}#Ta(5Ifh9*pf zaXpWl{^~FK*dSK?`sV856qfSzG{pY*fmdI1NqrBun2~Iff;(<}*Z$)uV9DF(BwNTY z%h{4I%Wj%lv%RmR{y)~;pLLxsCvyfJTJmB^&1;kmKrW+BG(N^6pp&}S>2CrkiPK$q z;|%v)2PLc9KrG{u4MemU_AL51L)V!ID2yPAs!zY9LZA)cN1gZ-++1tv>LwLZe(Yno zYrfI4p!IOxb3i@epY_8Gh0_)gRPx?}<_*2qBULWDI{_)=<<*~?&C=%Hmr=>i#BI${ zv3OS>kLdRW6W3L7a61(!suJ8Rlm-@nsA?08F<~MB{o1Q>sTSkWdPHiWiBs?fHI)@F znIb0^pI`-)8TR9^-tD46EfC3;-(=~?k=4;CdeVCSD!`PFKWdVPo+LOor{#(8g(X#K z%+=VF_Cr+&y5PFhA+P4sIE92rAi3$|_sqGQfI)A_?#(e9%y=-ux!Tobgcy09!ucvG#+-c6GoD07JyHNzDY z67)<15@-AHGRIOfrj?<+#>d+1R@;+S++}FzbKmgMzdR*UVGx?@NqLA&IMEo5Wx@!? zi|QXo0mYk;EKtaOYEC>o1gQGsIg(a2uh$1k&TS`@UfEAbzYw4av{fwM($*W`Rzp}BS)qH}W?bsMFK*9*L{t|k=Q z^{Osr%*W$k4u$K46#`p`*((@9`#IB)fpT{Re}8|Rigg`KCU}jrZ5Bvm>&rG4lRE+n zYk`-~4%Ui~bCnLy`>^!8v{sx5F%14x^@7sC5tF=<6V9QzJ=fc=IE*jYVLZn_;a&$$ zJzHb2bHm+DRgFwkr3~i?)#beO$xM6Fq7w$5B>#rm6f5S!;NUw`7v)P+E_dJk=^m%W z)w6-4_0li5decMzGyDM#4=sKU zDL~-)Q-Lb+NX72TOv@yt^)=wgiz7_wkPX`umHjN`JA7#DoGSC7cw0}vYd1gQm2n*c z=ijuH*fhBM1A|J;wamXTpgCxTaR$lkYt~&l=hl>X$%(&I&3Crft>~aiFUyQg9-(5W z+6T$`pBY4_%amLScT-$HjG$Um;-JfKou{%gdu53a1)9L6Qj`#~1LdRyUPACrsiDOx zrsR#_mm4x>#g-_r%gQvk)rzB7vUHDc^~J#ciBcE0H@ixCtr~bu9aA?qlhNlw<=xG7 zHwX9fSZNX&%;tEIO$dz7ZP=MMjYxMIE=tLIl|PL4s$K>u+ta>Z?-tb7YdxODySd`E zW;XlLU~CY0mIT$^xx1xahMZ%+@EPPKQE>mT-K~^c5oDj)UXtp+*UnRz45Q44RC`=6 zximGadh7LGhW2K)|446^3eB$fR|e(($0&|(YqnS^E<4JQ3;X)wSDtlt8&r4y%oz2L z*Op1%wEpncnp27@&+XhOA0e02GIqtiMErsOZ_8L8KZ=`xvw-ACX4buCU1}4GU55(7 zpM!CHGnin%R4_%q$r9HVF#ABp7)aMHT^l$8Qc!BjmEwUq zgEv#%;{0JZNACFl&tU<0&q!0DE?FUkdrVp?noec_OLo&-mCOcup%l@VK1KuB%z&>kx z)@>V|(%hdmn_72*p?=%qyiC}p4nG8mv)T#Z@4T#mTuv$PJ@8KDXRds-PUZVfaK_Ga zPU!CTf|>pG&5y#N!^iH!ptbsm;Sl3LwHtp6cf3rbH<#+HJktXqdey8I&~XosXJ{s0uw26Far50JVmK)J?qN#oW{^*j7~U`o&$ z@wxAe-^BBU@s@p;4%uW^LQq0;kcG7pBU&#N}FAkY7gnPp02>wQ%B_c{AN(l^}efNIFHyM=MR~ zF&z+uc-Au4hs_+W4`+Bg`VM^DDaENaJON4pQ`Teqw%%$9D3!lMQ}5)}C z-y>4AlVo*J$8`c(Gx!zUK)`h`9~affpjK(*0Z+gG&fmoh^KNw)S-1qC9q$^X=_i6$ zeme6~Qj~x)a!AJcIlD{XzwZXDD{!7{OQ-6xIo(2XHKf#Q-~gpGqVh)*$Np)Fxq-%w zoj#Ck1j_Zqm#6!V+9XP?hm!K!d}`Y6vP4kjT)Inb@aO4c!a7W~L&lnix8|%hShAZs zF2kElB$xeG(y=7hIs(|qQRZD#E`fW& zEAJfM*lJUw^>tpd-UlVy&gmpM_)QiowGW`J`f!d{(MR?9Ndi6N@2br{6$W5gwGPIU z*_8Jp8o>8D0;7!z%ID|{?;dcs0E1SzfBoOP?Az1M*)Puxk;YAbf$w^#++G`yQ1O3x0SFY3eD+17h&d}BtnrZl+8ln)`X zzUDVNVO~IgMO+Pkx4fcvCiky<=aCUKw%M4wXPDRi*F^d3Vx3n$2H;g0NRYydY~o}^ ze$0MSyI4Ho`XT3ue%L6)1=5t}4Ny}UFVGCBRd1VewTSS>f}-gFl;EWJ+O;f`N>`8b zc%9Qd(z77p&?^>9QmQlb2fZ+VydPH@4$I-MS)CRO$OFFS`paEwXP_K>$y&eR#g&+u zvRZGLDL84$EZgS86JgS~Mxo*59Pd4*slcT5>Qp|y`afoU2fI|G`f2%*g(EiPo*Sjp_9Ya$f_=@z7Cpd>ch$7+z8Ugw| z4ZvhO?;k;R({n7IS5tW_`lUx{p81H8!XoaSu4_ObqN zn0`oust}?bgN^}fx+G%tod$i?o?NtSFCB<-J|= z0Jup4Cu}5uX0uyX+Fx(?J7qKDZw-b`7Fkql0sne7u1I5DtJjco4YWuulx7wFmzw!2a*g3l!y*K3xl6yj2CB zudly|0rGKyk|{3gxTYFAI+OV4s9?w`znN-F3^EI_B*U0B%=fVN>Hs1xj}J#`1Kc>1>m$iRt#t!Aund4&c9)yk^2}Ld1hsS_wlFhe`;{6 z?kaT}`sCeg)qR0ewX=IxH`Jsz5a|1RIlUn>h7db4=`jYN^0SPOp!n-WC*}cBZm=Jr zh0N9N?BDpP=7e9-AOLT@?8jy9^z39<4OoH6wnPKP`&_^nC5$&a)QVik^Y*UYk^c^D z7`Fni);_}-fe&J-*FFw%4Fg>ja zDMwBKO=Id_y~yp&@m7JvwXZz2B?{&{2S8iftZ;wDwFc{c^|g)VbBY`w_AB{yuI3o# z$8}|C@|dXye!nf_+AC#71ZGFz45X*IX60@h>Q61-v>XI2llulAjfAlZmLRg6+E)jE zbgK_+KdFV_D-Y<=`LFP%wJ!Nt+eghrN;1b~cuoa8>+BcwbgiD7M(w#xk&~q^UGTO6 z@3TPLRCBT#hAk1A1O^kGEt7*PH1*HxBJofwAHeP}Poc%~cC(hx$#Fe$PN`TG2~6co z;j>IX4;(|p3XC0#g^QSKyWcE2>9=UE%(A8?$Y2t*{$7BLBw(jNUZn=*RUDs0#}Zx; z<3>tO?=PzIEb(UyeI$B#d{N#j?%F=}|GYM+V!}*Hj7=koT`rzXe*I-FYDxOY>YJ-Q zk6im-ye#aYJwsHL3Fa@40$YJiVM42IZBf`%Lh-c4S!mXb!1RT)f*1LLV3GiIwxZhs zLxO(ZG%lf__W+s>yBWNpl6o&CBxo{3nQ=3uYwe(uif&3`S#$# z?aY*z6L>q_5l(#z%=s0_A^-;NzFzaN4s^esYT!Tyv6-{aN9w>%EikaK4Pba5*?MOC zU$zA3HQ_$Y`~rjlsosHAPmQi%0UL_)as;Iquk;!HP55N>>d(gr(h$^Eljy04rHQl8 z(q8&1-a(wEiTZ6O*>(&yQblEq}HA# zn2fuAx{s^=4Q!tldXZ%y1N94`*@0qB#W^?$m2w203+!#(^W%;FK9mE^DNgoni8Yib zkA#PWY3F9``MU$ZZ*a+kv})lrPILaAS#8^RwtFj{|2CAXEvYh1f=`@5l`>`RWBK4o zA8qEeKp2w0&-#cPR1e()oWwnz z->%uJC-h{&I||;)^?@19W7vKVAP$3_{S4)LTMzm7GD8F4`(Gi_8B%dP`Ne5)f$Q!G z{rYkU0I$BFo3Msi->P< zb9pZdp4%x(@Ii>!_-8nPgRexslLjOzy8J1}Z|o7+@$`xQguvyY^0W8bf0cnEfR5R# z4pj%dyQJbJO>W5p8rH{c;AX(59Gk7{FEWg4*1pPMK`$kdp?+-}2o&_C0r${9($`zc zg(96&k3p3JAY`_l(-tAvS8$`Mcj?Ksa6qtWwKKTaLq0QjhZ#cJ`$g;4NG{Zt9;@-^ z0PRa67vZ~;EwUcuf<8|Wz=}*>PwIOO2iZ1*w8B?u+N19uw=5O<<7%3Boj`#dUSX&jv72-(wa~&A z;Ljipb+k^Mu+*m(fisf?d8>^EnnNhxpKrA?zjGWH3pICz9YBZ=M`?=&kdoyC3 zbw3DbP7Qy>asncrpRLM6hY`Hq+}kKvHlB?>FxXB0B5TA9c@d){U91(@7YBG*TTF;-*G&IQ)f*%Q%a9iDxxg@S1(*KL)=T3PC$vh=VLnw} z{J*^dB{&iof+%Kb0cB0sr*YBv?Vu5%3O}E$ISm>``+38A1NnWFTjOJ-AAJ5V}kd$ZGc3LQSzQ&1wL4-L0RFD68 zP@BHuni7xz%D60)Km9Zvn=9ot`ko!*&?blhT}&8_ha`+nO25x*q(y6@;!qrq1j=E8 zLW&V9O@*H76G%^i{uvb%#hgPc!InBem5d(@BxqwCq6Kl~8G@2lRd1V9l^X|@ou5}y zqipQgtI9Ap-#q|YzJMqU9Bi^wVzvIvC|CmT2XC(eb!Ya!92@y(WvnNjgI?P5=f&%v8f0#Zi!{iIOG3O+EAbRq)GyZ{*5LIOB=NzeU)YnfHK#>GVm z>b|&2ht45Vt4;ZU)BEUv!OlEG0@QyF>hoU6GO4?(FrbL{%$Mtg41XD~bwp3U6Zfge z2}*%^P3Mx2>tN2)AY#X{hd{K=*@HMGM@e$8`rIvQ&{?`nl*wzKh_JfEM4FBT6X$D9 z5HNJyXsThwr$YVekhxP+@i$QL%BxoSVw(!SAtl|^D$oDlCQP-DtDG_pe(r!v$BUN< z3J!6;=0)xQ^L}%fNUd;x`v9W-t(QP!nox;#KsJuE%>s9kEm_LF6F);Vq*JSrnL=Py z-D9BU5>V|O)jL%h@;ibia%kk(UaOzXb29g_U(grU@SHyJcmVv$VXS=Ly2~`3k_gU! z@mp??g0dVofaJa1BB3E=qQ)*MySV~hoI~|M>hbecK4MQ~i4BAjKLx@;^oaw!uxZUJueB)cl&+4}Yks zVCrBSB>s=r+mC1*u}xbr#3k30G2E3LP_(y$c#0t|KxN_O;2!H710$nYsNOJN)w zR`i03?|ca`805;5<_#fZS9z_AEGoIcf3w~%4l*(id<+)E)onk2IcpD?_%JXE=qfIS zdd;-OpHMC=wEQVZbUeW1zsmXJ4Z&c3{F@&HU;qE*2f=V*Ps*T0w)y9lIJAH^|Cj(< z!elwyJYYPPWM5Ah#omdtYkd^`$AN=e|9^IB-wqTO6XIwOI#4#&*OyJ~x*s{&%L_wK z2>f~C5v{VQON~ig1I8ZMjJgW?oPeEkcaDAJutz|c$1%~JQYUng4S|dyUz00M2P_@8 zE`v?=ExuFIuQt;5TJs2@0`a+|@u~IFvq4vOgQo9S>qF(kw~mRM`KA9D@rwyS7)gV( z3_=yZy_E)j6!g)HAw4zFsR@RbF=ijMW(2~rNEOMn_@l+-?$ z{Pv}ZB{9jZLgHY^xoi&mFQFSi0K6Vvm0wgb2%w&>H>0=uqs!JHe>R(@c=p$_2aP+c zo*B@oDdn7H7R@nafgoYXb?lJ;+kK>32EA50A|&}HCTQNRUL~eHRrmS$sYj&zh7zoX{!e0KVLJPEjDI* zZD;8OwSD8tI#)eYE^@1BoJ%dUx!``~ASX0&&fF#&iDU7Sv4!8aY2|u$C)l?<$ifw* zEr1glaO2sH>Tcm$0^!}c7ENRDHI)(ngt)!d!Bm`(Zja_uz}3k{bziu4?!t2? zw$W``{AntFwL_KM&n92)mYQfmyP*R}9}j==ejrnK2-)=5hsfqk0s zrT@!l?pNf8w%v=ln6FORm_4t#O{bft9vfT9JEWNO73D`ywBSY`m>NHG4(SdolXyl> z`HP*L1exN<-wOJ+;?Podp{R5~Ohq?H(b?)M&Hsu*T_@7}4O&9qV|9xAp=tqv-bJpE z>c6CJdI&N0lw{SWYWrS&!*Q=xHk(x08ej3NZEo5Mz}L%1NMu^M^@?vwMUZsNBCnU- zEcp+?AFpt`#FTrqzF`!oe0=pxQmm%YW{q*N#v)02@>uoCo{h^uD{(Nm%vYBQNOC@@ zKT#cu17~a5M4kD%lR6&H&!$ygOKMAU0J4&C=#7EyyyS(Yq$O)})z>;oQx~b90ItDZ zRjLxU_AWQFkR?||~XLDp5TZSI!DDVhfpre2~&U-^5;`r3q#03B&9k*wUbbc8-c z;dn{bph?udo=X%aJ-HC<(&)mv%#3P^54@>Z1y03XVM<_oo+&FIr|9X@(zSEKbm4InV^iRlygZ+TN89ztf_!I)Wn zys06z)vTD609n=24Gwqa+3luMQyI+#=2ah}b{FT*<2vHw28`VBB8R+2jm6DemoIfG zf27k+0FC&ny*A&8PE9|?E_ZeDOyoUhb-m)ZOF&Ogs2=ZJr4OsGy(v=X`BTVi@$;L& zp!8Ryd~|0&WAS^&?ttE)<_JVh8>$;i&$?nUrn^BOhU1ZPE-7T5Yy(w+cnMR-<6))5H)zOnkvBS6M> zn(hMcnPxR%xLwvX-9%SXD&bD0F z$;oYczymKUr$e3c`W)fIJ2-KX9)){MBGb{#ARL8uTy4DaDe0nO?8b53m_yc4l@o4e z!IP=BTEj&A9}6fmw<_WBzO*SYqig%C2WEz_`Jr9;M!n(Av9*KqmF(j+-`8a5erpvc zv|9M&v;e~0(giwo5kivY*3JgSZix!+_P~S3g$z(9=;&-)AfvjqX|zj%%ZIaYNI}A~ zsowPPmHF`H8Sjs!TC9IqpX|MUuVO4_MLz9wF3PIN%7xQf4sY~5BZ3M)m~r?bQ`hvk zrYkJttx0F*W!4W$h1XfhBP*I`CU?an_^?PlZR9$4z+)KA)oVnHiqUozS?+e(KU>PS z-}^z%HX{YAOJRQ_ZABk_W~vJH%>txxg&5M

hD&i9kd` zL}GN}d1CHlELi!89Lk8>cmn_BYW8sAU6V9)l}75R`p~W}w?^h{^F=O)UgN|4S5WI1 zM?=Ag+>*@QcaG+&_LJvg`=WXD#eyJQgJI`3{F={|qiSA|ATy6bK0Re5g^B=HZ+Ig36lhNShT;YCPuY{oE zH%oO-Pj$yGf(V>q28;wQ9C(d0f2r8X#F?_QJ1nQm=`^~&4}Z_&oSunP_FkH411%V` zON~`b6kuM9g63N8c9c-D)4=IJm!vnwY9~2ey=C7Q%bf($lm$%`5g8mVh!*FOLwZdl zpkoZ(GV22o6s13GE}(R9`Vd=~xtHz8M~39tB{5e8TgB0ieb>kg-}yZV^O`$-o!wB{ zGa!noYyWvV2E|M_)r%7gJ83bfj}MWyBW}LPNtl1lOM(o`I#Y#+ZK~yfMuzL24{gZb z%#eXp7``{#`Run0uWWEWWT-(o3e~G<9`;)O2+?f+K1Nva1r?)td2q9-*K6 zZ}6u(=qXU~h0!=$5wbUw78VXc9DViZf97H`r@vSRd1Ow|M{?m78JT@hfnQ>)!FNF z^3*}~l$Av7%Q>9kmW{`Y^-bS1lU!iF-MMytcOAfZ@1&Ex92W7(1ZZ$vN{Cz2Y zZ)5Zu5sP<(QaN%C1>J(qzb4|ay*;xHALW_^Fd4wuixmNTtk;e~pl&lyZ^;7#`pXI} z#5W0OyjD;|lMhTAr}Zgyy{tHw1mWR;NWm#aOc5p8gu z0OuR}ZAXcI5M@nGRf$YtgNK)^n#g+TeZY1q$$o$JCVx5@KYWiD z7ptc<8Atfo!XGmATa4&&ayJ=ggtgHu@m^8}AKP0Bay=@rx6hMX^*68vZn*tbN@Fxt z4%I1l+$|VHeV8z^pQ^OUwlg4!LjY$duU73wU(U4l{z%f-QvTARuqT2PwjO&{t_K#g z`()P|1$C%uBm+=j7-@1gPzK;13~HS|8-u=6k`O?cZ>(=6Li?+Xwq62#$yjdc0O>ix zSaL{Rm)_`jl2Pn@qf~(5N?80rPO|)=Nb$T-4*EXWcqe1QO56;1MCl`)bpt=B7_>`# z2`x9`xm$?hqR8J9GWU_QjBPNy%bN5+GbxThcv8QH-Z9aPqcXEfy~ORXts$+9J419* zKmNX+)7#h|QN*~%Jvpt%9dG+pLp6j(#r_*3>fZf_cOj~oYNQUCvnadklaS*Jr2Q0o z>;O2KEw&j;(A_Rei~w{*sq~hS?$VUrW3BIf`3zHmzDeQ7*+-u{ZwKk50P%1bX>0nW zPra*tL{e_4&}0FIHsLNc&tyT^_bJ~+4T&7isWXfM1IkzD9#DJW=wcShU{zV?84U<8 zxS#pSW9{=IrndpJX0Ds&dZy7hLq>^k+-Kq7ub&fV(q>j@h^#jr8p8{Y zSD~LOZ&h7UZVSyx-OqHjSjVeWL99h#^^U^M-yEwd!v=Xc;PA5E-OgMtlDU~Rs$?q< zUM+&hJvIs92}9T|fOS>?wST7PcONRXOwR}#Dh)dL$4xb;hm6MDA7*W&kXoLUeO3o5 zDHi{r-X`zMufQTS2zwI1V$4I#)m1Gs4O{=|(qO_bm&F(}{V-Msc)A3J@l!OeryEQeM1o9Qu$G-H@zp)_`-TqXgok2uB-4a}m?W0>fg- zG@39nvgC+&n^B)mZuLD2TcRb}2nexhJh8akXa^N-#Yvc>oSgxG??)cloik;vfFVwA zyNjjx|*!}KK`D(km}`GL}$YUjJjiOXfR8HM8n4<6>TK<&HnaGo{Tay z5laZjBf%V)NX+{6%)ORjIX1Rux#DKNzx=iS_)1vzUNL1v#Y3FjSKaq_gM1k^)0~&P zzm^D@Ynq_)^OWsULWHCW83X64+P8)^wuCLFBZ(Nbh26Jb6ggK`4i7?6JUMdpj1!Gw z$6^hRVW9aD=Ojodx|(NfiU;}9vj&(mxzpids9|@8VNl_45VHaLu4I~WIyBXs$31v= z69wI^>b%S#QYcL=7gMib$fgYKjU7M_rU6hRt|FoSIk8RqC4C)EtxdOf1)@F*;B{Y3 zzSbYil8KbCdMUqjd2>?VSpQu?u#0|a-|yFO&YWSp(?{5CrXVgK_He`60W0`gAA z+y{p=2Ao2yN0f5Kf~<`CS-)hVReG{#eeZ#m3(yGzSU^TcoH(~=z~^=i8I7Z_u9ev@!th)tfv8{6HsS8YFt*WGs-rVf5dSaoGL zdyLttd8;pg?z&J_0bk5=DsWr!z+mqU^NZ41bt+;%2#QBQvlEEKCZS2)Fu8Y-S-j9` zwMxuayM`Z#o|pV?6R8P}B1RkaPb#x?rO3c>;@P_SMZ`A;thW14x`tmy)`<}5k5!8gKi1-| zb!yhHmgEK*)$+5iae7~Sn#xZLMSYKDj`VohpDM}#3X<`{v7pGV6mRAMyy2@#te}1&Wo07xsOx@Ha`8#JwYuV zcO$2Rm~9>Sl)Wc`+J7(7UAbcl+K+T3UXFIP=t^9J{i9u%dNJ z*-Ts=sYmNWo@h*aQml7Tj#`o3&&oxCKIYCxd#OQ(8=Q^Aw0De_s4KDxk z?=lxA%dd;fnpk0`_JXwC7`86VT=Nd}aIgfSz@|U{NUX#)qZQB-1v=B9D`!OWP|Njz z3c8z2zka7FXQYiyHJ5U)o%6p`(%>d0Y}Wx!4sPCU;AJ zFP3M=k>iGPfesk5{@#9{Qdqf;ra-IB#WoUlRF*M*(;!??sovuH-4;t;sKyK7jc%BW zUTuymXlkHVp|j$-n`AZYZ3Ek9WccRysaO5Zar_m&@ufXh-`1SL6)~}!b$RX8pB97GyLk1mvAX&tou066y#S z>&%~-Y3^t(qiJ6D5WBY$DQ|uV$4inykmNg>}qG`d9sODXaNc za}i<2;LHx@HE0rA!>yoQE+uMH03_|Nt)9G1Kh@&uQ$);-*?Yg}4d)ymSJ6{biyDic z%d~}!$)$r}h61lAQK4$b6+X@ef<;*|BZ#*|4#&UdyZF6JQl0nU(v?1oOR{iXk*x0nZ@3T809?fcx)o zjhgfuC($$%suYK2?PS+qQYQ~z>eJnCi3<#_5qeM4-|(YINWXUPk=EIzUbd%8C0|U+ zm|LaZ*bM7TvxU7hLfTtgm~iFz6a=epoB(HKs}UBmT|dzozYllbX*4>vKuR#2CsV7k zxclBw{P3vE?(Z=EZ^e3bO$Fv~0X3Y)@$9HHh5qQu1Fu6tM3Rx$~Vdxdw9;PS2cFH^qM#NX}m%Q+R-+nM{Kn0QL8Qf*08E}bdfvU zY-b&_kXjfsa^gzFkykGsd|=TYbc<)*^A!X0m;5mel=8 zo#;#;huxC|QSjFuo4@PigN%eTNoKZ<^?G~_@pOSjKT4qVDNDYvizK8*)^q9QI z6sExT@)U5W^K<%&sja{R-@D6Bq#CzgB&P!Gu4G&g5xcP&yEYXa4|Sl}Y$6)mF>76* zJGs>&YBAd5Xgczn*rEy|E%ONT=owh~v!`LD9tEd+j;ODe7GI}Gr5L|TOSAtVdxAG1 zLO^yfosAsBXg-@D^7~GlUK_PiCSQh@`ZqPu#kqGj7np2ERB5O2F;J_P^y=*NMu|Kn zB{;SW3ah*pdHqBEpL1jjDQRGJRc@3%8}nsfsQF~X6XCu9D;<3yYI!(8NG>5))i773L$os)Oq%}enFEm)y<_KU#EV(_Q| zO)(MMS0K1?>G(h+026%TKmioT8!;F=8yRp41+MnJ3G&OCA5ITdaIV>x(rjXQ1?m3t z0+FY;>jb){0Heb}flV#NBg6HEJifvlqc+7KLf0KjDf?;4Y0=QYv2lB=EW@OteAv8$ zDp=p<)LWGUII-4}cZ_Y}Y<=THQJuPQ%owS#xSC!{(Nf(Jo?bSWPD-q!wzRVbd`wV3 z=Tmi7T)MM*)NJCYj>wodYl@6EjnrEwBE;U+yGLXjoSU8Bpr@`4Q}GU1Iwb#$|0Vda zraS%LM(Sy5`dt3PaWTvomtayb`s{HsYsl_QL01?R{bb5*)xTOm+o*mn+9oD_H~iTA zRZe&!nnucvPN^l1LZ%r~gt2&2kJOm*s%|)6qzDN>1TIufdD)LS#8;z^S;5T4mD5yDG@AejSu36zX*49=5sij?>4eQCXnyI>8 z(b=S~{l^J+0E5%{Zp~7}W*JL@B}sWMXWri7yA@+Ko^wfJxRmp~VT!|u@ks?^F@Ik- z4JA>vtOPM0`3evC=kQx&&N=4IW{2nvHZ`5eTv7KmPql^;hoW`q%?#t>&o)7xl?&Gy z-D-MCT+tX^9m9YgP|{9hjC7$ndGg%^08ucq&pJdnMxy9;OkxEg-?E`8u+klJSxMIgR)p!6}0?d3B^t1VY^UU97Kb~O**yMj1?92!$QLLH!M&A=N|4E~JyrDG<8~w3n-1{OoCYRDtMiU1D{?ZEhf>xexB7)+TUqEHh+~1p zK}$Z&a4L`FGP|hCilM)l# ze=3!!<2g-8-VQW3l4#CzD?`^eeq2_vm=8Fq)SnscE}J)reVHo&~TNa__*B1(Z5!n_m)70r080I=|6 zp%GR`?fr9`R0T&e`ijOFFu3Rc`2-dgjlLWK`+~kbr;X2tsTaL1vLYqAlRoUtep_H8 z`Z<1QmrrBjML_K2iXjg?F5~ERGXzq8Ms^j&s-*tt8AVT&Ip+d5u%miTV`!W8Aa?HU zumm(s!xs}2NPR#YV9u?R;vAiO3E_qAqnNC8Tp*o_LERc7YlVmVChL?)36^a?nvu~k z+(`oB2Jol6U8UR)q2P^?c1GK{hE;5~T@UsUP|6zfR?3*x34zA06O$5SSbMeAYa{du zxOD0l@K2oTSXdnj^q*010_e%w#&F-_Jh$+`T~P+9QO&A~8uc#Z?@ts`n;Y`fLZq-o zlNCzp{H5o;Y#}XTg_>C>iSfn?MX#tlr0h4apVL0^i+x1arP=?9eSO_^zxB?B!T-M6 zE}t@}#w1Z_M+fVLq2OkQk#1Z}ktE1)-yEaR<}3PoO5j{ z`Re~yGIo(p`&YGs%*ZyB|L0{`m#dUOQ%`us`Ds_S@%(b0jj^e@=WI7dCE9U-rI}Ms z(1>K7i|USZ8%;Q#t`D4fhP z{FfAr)BYd)!om_%+E$%8KhG*917Gw+f5{PJM|16N+YAD$?5)Cq?(-O@Ed>tEa+Ta- z8u1d7GaWm&0qM`i1zb0P`ZKC%9dLC4nyr1Q{n9BC1;En1^`3WnFbYo4>2;B4Mm9qs z%Q+AJW9;Lkz_vnug5!JtP%=@EdasU zI4`??d|rFkwlS%f%^>T|+H+u;5xtU?lY`mb&i;Kur^xm?_glPMG4RX>|MEB%Ru{hX z(6d@fG&rV4a>_h~Z7TH(m)_?y${sa#nZ}c_x=!lwgeNV$$zoE)TpS6q59T87hgLu! zX$?)jty7VAuq>v5hy4c%fQP-6Mm`(kPN7uY8TOrQKPZodw3yX0_DD;ZLTGvq7Xs5u zTX+-IFAQ7W9q!NZ{%xkVi8(*01&S~xdjFf)L@CL%6PNgSe5~9jOKI78or>@1CI6rC zZHxY2;{$D)jCVIhld~>~(D9xFVIL^2Qjj%c-%k9C8Bo5v#VU$rWOdK0}rc>HFQJ!FWBf$nH41rOTShc-f#Thm?ra&*L!z zYlplGmx7=(<-1A1_TQ(bSDK|Z2kfr&>&^*KYTo{RP*_z4&4v2YPB%OJeRo7e^ogyQdM7|!_f)IQ@q^^W~N+cYyaPm1c*P2|op9UC$^I5>26ZZ1uO_z=K%|NObb#QYE|JNCcG z-~S)B#lm`;ZcM=LOyuTK%FnIm#g`~la%4q!^-BDOXk&BJUw@6b!gZUZ5eGf#;m@v~ zgK%+lnWUEyxq-1^7I?T*fWl`ZhX4}52yjUBaZU%zkmS=|n( zBv;{~*7C;b+D=nb(>c58Bj?g!!k0domo!LP)v<)QUFokQFlO?B7oHoqpim7gNv?m^ z_IXcznG&UZizOvPAL082;$vZDOVNX8N4?a4wD_lI&5soUgQ?MEtgMCI8r54s&BP9} z_+hh-%jtL3?0Qw6cWzV2jv-)WcW`tBCmHgj*0;9@f_o>-6iHtI7)v}Y;W`29WdiYI zE3Zyr!QcNCPOQsOoLJ8d86{HY=jT({EINZMl2)}W^(%c9ITG0l1;L)y6;((zE|6p6 z3a;CackuqNh~!HJzj1flB$^$n;Vt^g{*VzQ64`&$R-NEQGc6Q$ClTwVC@=4~8F`D# zOY*J*!CK1OZz_0%-(+$Riw4w7yF}NEf+Ewv#rmhHg)grHw-#Biv!*bkrFu@lS!O55 zG&>GWzuM97K%`Deo@0KGxkeR!;u>O!)x1QpftIR8IU64O-E-jtH9)o3+)d5Q%;@Op zWe%@TS!L41@GC5+C$X{A?V0}`!mF_vhF-R}0c0_jB)hi{u%v|*mNmDm&&4N zfLG3E)iJxN-`%#HNcIJzHCEO_OE|~AA1XD>Gm7NyZ$Jv){8+8#-m<(DbNHFm*w|=@ zgtRJjwr1^oz5jJzC4QZ6hxuDelW!K$2G;)n;p#1*qWr$F(NRQHQl*qG0cq(L$w6u9 z5+H%J=@-m|YUxM-!_Kgj;?P2OYIXO9G9$=8Z z&X(t}x`_|lu5+eFbluD1Q>5$k{^DDP=fupp6TdS(FMPe!f zokNVIeVgfpfz_4x0+9YE32s@00+&iX6P;^RHjM(g%6&xx@j1h&8fL zyb%)QV<-k5g*{a+S#LOT55cTs#1-;`hil9FkDX_A&y3Mb7aFU!7%hleLiz$)UlLJ61$ z!>$EZfW|hpDbUUT{CZ31TiMIv-T5NJ;*RoFh$C+TXL9eJUyE&}b(!ApcU@&=eA*Am z{I*;&hTB7l$wJJ89v!rM%fpb-eG#|1i%m+Dv%lrStXB>=C`R%nlbBVaDOjMJY)MfG zQN|zmG&!QG=AIwCX()=Nm57V54%92s$<|SUJ?Bo=W&=bq#@)lC!u%oWtqHUl<--_r z(HN$I{qh%&2--K?&)TY=2YvZ$On|s50zvkhuzB&Ln+WLTFT@pU94;VA)i23*7Zufm zi!9Zznsnvmu>-RKehgw@V1MMbKxKXXL+E`vx-lfOvbv_Gs;unmldoSQDR|aahjUL} zb8P&Rg${J^KbLB2eMqPF3>Mb+`aahhWOe?wvbl21)d>GPMX zv#~+%kVVRaLd61Q>~t#U9ctzxkF=>W0Dpdi2@qu& zy$bnbXT4gyye!2u=2qdJh489(UW#br9qH%@0hzvbP%kksJe;ehU8GZS7Ck&XT<^L2 zO-M)xxii-ezP!`ROVq~@q?MFz!QXTrqlJY9v4Hc&j*gDX=H_(2e_Ju23>p;Kym|9% z(;eNqr8M=ym0A(3y59fHdvs+ZI7o)sP6@K2dwBfv9Q;B{&n32*e`C1`X_XNdpRy53 zF=j~5;uI**DZpMC6pYc)(Mn*o@bH@;r8azw8a*|1i;V{R`(xbQ-52YNL88|`=|tdu z0QTviR8dv6@%1$ViS($$;_YjaYx#d|K=x`Iac|*?xtO!EqI?0tb45|kz=B{i!Gvj+X1KRbW6#Hpn2QY-dixOJ2+h|0yhYE__;K#BKF9gAmomo3LIF!Y#S71~uqJZCvjP)h#jU zdjb_tIvC1+3d^di6TN57oNbo5Afk`|s6~3{daO>9i7s`gH*zJ8Id9_48aR7Z4}QKw zIUBYGZ^;PGidnGeQ}X$(<8M*0UccvOO6Py{^&P!PG@WVk#Dr*He}A4y3DEgkKYSls z2cl>t3t#!ASCbQ(+XQDB86Xf?e};644{EC$HQcKr)WwmgY%BvmN0pc~Jp^X0FHTBM z2F7hZC~{ccUFd`jC4C`eB1Y4uq@+xs-BSx_ex01GmvG#;3LuUdT3KHHjTs86=7@cb zHg|GEb1osTEOwbo<)~kTRQ84vh?*PRQjHhaM+tJ9~Q@bMu8(yRp$x zW+*>Df4$?}J7#R{?)bQ22Vj)>pi7q))-lKFq@XpTrsVurjJQBhH`wzA5#MSLSX-YaU)?7~Y1 z_Ywn7{=9v>cXF@}ONe$&Ou+qcDBbB)FSRWaV`*h|u+^|_x!HAkr0bJk6CzaY59QaF39={Nf-sUk!SW}82uecX$B#~ek^foDZ@oI zF}Z?8!z=*ypyKgriurQdo}VH7{4tv*+UNd6=<#!N;P!l-0)Vu}rY_*dn_J+^^QRj= zr>2e^9v+s(Ftu<*VXsSP2PJ5z@YBB!N;B{Kjd%qxN&UOHm<2$nzN1Q6j6{7IZUES8 z?y6Vk9J;xqvr|x1)QdhUA#2Puz_@uEd~JNe^M^F6&F|>{oSqT_+6rUl;+`2D8ZvGJ zYPbt_*lE40s+&nmdHp|Q^wfX}4W)cCw{g!^C>1B|Xp*l99XHRNy3NBc7X|MHMxb^9QhCC zkgH6F*a^_xR&FSy_4`Be$cKr_inL?i<3~DUZdDBp9Oh8~LSua4AlDd{_h$lT_?V&J zcnK<|fRQXQRR&xD?E$8^PQw9$fm$OdD3}ettYoouw-l6iN7$aZ(dM`Vts(FCOH#oa zhz+UdY2qH-wu;MCuyh2CH9-NwYXV)Qv;MQQdJE%kMrmVZrPyc54^(O1=MB^S_WXQ& zPOtIz`h?*L2rqBU8EOgbQ?d_-hgooPKxUaIlFm*~fgVVyCvY*(y88qW7v2cK=aCx| z6`X;UUl(%)NsEA)*dmyr0P{b8`Xst*{-y>C>NYh~x7v4F5t(djHXb?!e`RsHKYhBF zCLJI}wRvB@ta(AaS@upeaL;Pbg|_kCq=cV{8Yej9f}lOYVw{3Twx(#Qurr3c8zIOd zIk0}eqQ0UclNqYIF%IHAUD79~s!DKkV}ts>g7T1V%Zaw6H}hh2lD-a}ud6z?FTB)yD+o8={XxFACX?h^vo%qw8^o0050HUhPXEY?!JWv5sx7!|pFV0M zJBzwBUE77}WGF-OpFf-^{zVaOH4TrdJPb`NUbnHeU2pYULTM;l^H&VM26Rv!i|wGs z9SxZ+_Bv840ul*F`N4B}2{OcQkwXH9bxOvnDqVH&*g=uPmz3gt88!+w+IT_dHB@Cg zhCJF`z*#WJK$)hm*Bhf5?L_Jt8qj&Dmd3`yrKKgN=|h3W{eE$5wJA1jDdq_{?gdm@ zDbwr@ulPM|Me))f>{y7Rv`j~UqJ%B!8NPer5(Dwu=mA0o;wg?*uk?A5B)*(= z7Y)*bWga&IM1NZRf(SNl4~*dl#@^sa5K*NFzcnd16T%B6!|@i@AR(U?M8LyR z-ssJm3gfuLh2nKfI&q-4I3T=czB0_@R?z~iS6$|D3X6^=Y(j4})PpBbb7u}g2|byH zpBek@{J=Y@c0db7M@OekI$eP{QXf|@0CgBh48ias}O04+14Aq0F1MZ`oO&@kiT9 z`k(3n2!8cy>CY?~B+$e_Umw)#F%abgDuXWnS?Vj+QVPx6y(mAw-AL2tnS*}-g{z=} z34N&S>qH1L7o8DypN>hXWe$2Cw8-_3jwS)*9s3*Mdv-7;eIUYZRQm-Ww<+epbSwyu zxV}vX0y&Yt(AJT6tdv2lwz6X>8l_M=Vs*JM1t6HQodT7zGG34p)#xT>-0Ukn(j6RK zlyb{jcLG^$LPbSoOo!gEk}lB9L$;ORx2pphf<e7^w1_#2ZjB3Ld+_jtKodG*DPXF^F0DB|ub>NqQCKVX-t#)22!3NNFY35};`T7|2=zmav(t@7O#@$^NG&_Ji6ecDv znb46dT>Echnb?gzh>wpyIfG;TemND2!~kUXg-esjNjW5EB~&xTGmHNDP)R3!(~Oe# zxEg+(2G8@D@lwpdoFd$odJ=IuQ<=j70%1u6rKpfl4(Lam>o^DYO~$vbLB5zScGm70 zky!;lzEBKaZD8XfULo_VVl1sSI9&zEqw&`Tvnz{cDp1gQ?B*Q%#VgirXBrqAOYYgR zSX){uBr0dGn}_1Y=`>91WNT`CRaXap)SaF6(h!JuUp3PNS9rzai>4&?iUAA~Xa?%y zjPL#>ZLKhl%ovUM9qUG>kAoSXZEG(t{f35yPITnzYniO9H?bDkn%(d8?}OFKV8@)` z5Dm~t8$XigzPr2+)5U#S-HZWC2iAn6DnedJMV(d-76Ap{ps092zvH7URT9rFN|35Z z>>YqDVaQ3_l2?6wnK#jk-wH zAcLX@;4b8g!_uz7JtMO@ePMdxzkAi(3qE3Hb`hlKKQmxlVz#hleo!>IM-MM8X+t zV2b5lClKB>s5|lQ!>T1ejlC9#@%1&MOn*(n4*m5~Blrn}tv}#~21PkQS=-nL1Bi&k z#8<%beFN}CFRqxV3$L$W`|noz3WoU#s{w5DMaUHS>t(g!&~>loZL6A!8Worv6lMYM zy`FSTGHfGo9}o)g7fP#!g)|V!vKpjmdL-=ZQ?b@TdYw+dQ8|5meRms(A&~1T7qLuz z>54y0@39$0As+|$&?wpHL~9v9NOd-gOd~rb>z3kadygunB1E*8-_g!jU-cP}Z1Sy#i%el!WDYo12@3xa{os zM8;!@AZ`)k7GKUM#kOI$0`Ar^_ZEo6O$^BQ0W-zV$_nkQ(FLg{^Tz`20#GEXo0d)WuQ&n2}Bc6e*OZUz`WWKO$8PLIuovLP${0mmY0{GgR>}8;PY;q zE)o$yApt@MsPo?emQiN1fkQDMU-Vb*$;W|~jWon;Y)tq*q2FFND-WvuD&)nr2}A>E zfvbQ7SjEcc{a;M=)-cEVm|+=CsZ3Ua$?L+gb*%owZCuYhu+N;$r03!8p1>TA4Y6m{ z23%h0x%d>^ul@Xtu@aTneh#gK=NGU%D;W!^gx_$7Dg#MJiNoDtA?{5jJ+L+<<<8Cr zP*`v*NVBh*ek~>?95D-p?AD-NAW^sq*}3zEL{wZn7aWwl7S9xMnY0&`E@-xtehAn? zyJBL*(4pz?fgr%`!Pfw^!4iPd1NZ`u?;QTg;-vYltzrO%3Ux8D zvFko9D4)OfHB0Db>P`((d4R>5_qiuq|7>Oc4*dWtpB?h=-^#4a7{H4i!bsR`g zKwZ%(aC;$8Pz4B7C^6mMI02;fW*GysUH72uB~sqRvI~EHU@0K|{9Ui2xE_a=^gU7s z&4j)E)}I03XrO%pQ4I(&K;r@j#{o0gOi4I2_#l)3rb1vz74({u_%xdt`=;hm8U=qf zN#y})Ylv+JPeo2Qc*N2J(9r^?h?EVT1uhj5zjvUQ{X#b`WI^&A1P%5;6wiSlkc_H8 zO99Yjb+XEKpec=zl`2|+T$w!b7dUJp65|`NeP`4Rpq}HZm&usYC@%r_-pdxvWnLd& zmSti61mx9#ycT{iaG-U68bfpm39(o%fb98ER>-=q=ej*32^OX1(0rUDg{m89+Nstc z1yXyk*~cnIE_5>?>n5Adz<&K?%rVV}@;Pc?m)FRb=3342WWA&aLw4;`Y;{U!(j;pkVSW@K})8I@1#t%9)Gz7yo z2SBd|LYOg-UV%p9oxOdAT-9QMe#=$Mz{=SAlwPFA_+J5XdrDk~%tlR+Ji^Sg69`>i zn0OGoK4$Dd*k)8_A zA%z4k+->kSX|9yzX^GaDU|_TpGrDE@eROa z0nXE5y9v*@|09eJl8E34V0S|Y`op%iqK$zSwabc-QK41}`jW#FbqK_-HRcuJ|1~Zb z&>WhrQ2>?iaYiYFh5sALlgk5&e-PN)XSE;7?mbB~$s1cYFsPTkc-t4Q3C#G#T$TXh z4!zpC@>1F&peGb2p@0MVLpS7-Ee`v+;F9t6zqf!8SO=QOMClrVn|7HjFp>e z$IA+_b7N>^WX-)kvBz)Nta&_vt50KVz?1 z;)|6h6|Dxe)#l}$FHF=5F-r!LH{PKv5$hA+p_3H9H~kr*bBVgBSAa#aK(XVl#+lCa5@oSL7wyZ% z1OhwWdS_EY3{ryNkZk!*beBe1rE3j`FZQD>lB@-<#J_ko>Ks<+8PvPXBML6%uPfNO z-ARUB*M-1%KaT0O^(watcA0u&_L9?cl4}>ogh32~JF6}ECO*Cc)=K#pZ0%3Wp1D4Y zwbQOpQP=GYH)FtQi$1k#N?b4jHk;GY74iA+xa<{q;NG*P(z6dy=kKswsP#)J!X8Am z;=I#TdILAtwCKUU&PAfVcnm1h;{XxPuq?vxvl}4jC<%_ME&SJh@s=S!^_5yj_u za2&9TKrM4w=zS3gILkbDhe_Xw6eLJPn=Od%mW}<*)8iT5QYPzxlheeDpyzD`QP(;acFZ70)c^f3oX?u%A!C|3O}q zX7M~};4h7ZW8*2KB@cAr>4K6sFQmjPSaP3hh4JqK;4aHwjNgP+nP8p-c#s#5$NeIQ zW-D1X>)w!D1A6}SFvajCHVBAfdbv6htfa&mM1RLBTKx_r@P`FmJGzX^dcX>tb3pdF68|Sb%>JE-T#_Mi zoTG-Av~6q{jS86R>o0q-_b2$7#SylFe(FW3_`PA8Xkid#J|}wi9!7^JB1EzmM*=AC zfDR87ZYlsSNnqM3r#2B80vzV;3jk!cb_0KM13$`}0W+WDkI%$%8G?VX>ryl~73H3Y zRBVFh;V7-y=poySSwRJHy#x+^{OkKgUH(!5UoKM^TSaz!H$s2XmlEP*Gl(DuDY36Q z7CEc>w55h||1yOcU=9EB3m>waICbQI`Y= zd_M$7%P|IdLK3J|Q>H)MjtAcWe4f(z5K2VBeT9JOof23CTQ>#)LiUYNSY^R=BK!xN z$HjK?L}J*FFFg&Ig|8fdDHX2(h;vE>V60ci3ctR9rw$;2k{LzT?(W=E0ooXZao)e>@f&uycIU0kW|2P9()< z67}Y)gyjBqgr+XP2DuK}L$X8=6& zk*tWtkr&FkcdzmIp3dO25QiiMUCI-bSHQ-QOg^_27{T*4@+aH5Sa@oMl=5^GjIAqr z4?2Q_%U$M1(wtqt0=yJh?OX={8_fLgzPXXzLOJ{r7?|ECF$ZFgZz74lVyn0r zc%Y3rZPN9F%;f*BMG4YJ^{x!_A3AIRucXo6=zsIx`{Gn~ro+IGOIW&%jdknNg>P#I z25^&WR}=`bm$PyJQlbn=>PpyX<~TkJ+9g@w(qHd@zeH~S&%qg!Y!4wk7jCOC8;Bn6 zoSBgr>#t!!+eLBwzwh@X!}@pr`j-rFG)d?adZ`WEp}nn|5&cRt%&)Kpv3a&LzWpDs zL5BGKWrk2|*W)7;+%?RtSM5!W9Ix!GzAtg_Ux01=B3 zxB%GOfbNi8`wO5l@QP#U?FnfL0|@m1ICD}=K@NcgCY!Sorydk{Enrb|X+ipe550H~ z_|OicJBlD$=|$}yfZ?-$0e@1zKw(KLhPEN>G9k=BFdkNz=aQ4#Lof6Fs%#iG zWEb$?JqEE+uCWqi`Ck>@49>R-jw=E#Z4ImV%4|LrIOy)kxjt+B;)P zL3&LF9{XDImJba$cWx;(bWnMDY(;^Z3;LpRLV?*_|Lvfh7PtjP1)>)gvSuU-Pm+k_ zE6yDJAV4U2ax#Yeai&eya-`_GuK9IL@fx@vlFR_sx*|FrMjeCC0f~vt0mcrkDC#nk zGOdn1J4Rg3H50fG>x)IA{Ff2P{gxdMWJ+wN2^}H$>>m%{-pn8c=D>EWEnWer=Bhy83!%_5g~d4ec?Ir z;fIM4c%IGiV|KQy2PXmtbm;D*4Pcads?%YVuJKYE6U%&|}wcI^2e?Ap!~ zbZ7My2=Bv>k^bYH6hGNrklU(-OBf_2HwW^9dfzxASv{nZbpS z=XI`H@H4*vW$9B%hwt-Bec}Ns0n05-MIjjZv!9sLg_D) zl&CU=O^(rNp0Y#Y9$LaV%64Fx>RWc!rP}wTY_1R2UOmFU|9tx{WVub zi+L&z;Wt?%G zjh=-pk8Z7nGpoQy`u5WojYnHh6)lAU!V}tlE{0RY{@T(96`1F?D1Sw|?NdJEeua8+ zWOl}p;Yk_K^Mv>DjOl3Maa9)Se*!JHSMyhRNG|svb z|Bam=!G~h~7>2gb4y1E4j(cKg{SPFI{4s^^O3q475{b>Bx(7JY2gwJl@JA;d@Z+0j zV6Gf4bp#KPH^sCEm}HdrNKAzX!b#(=4`rvcRft)l42k@0IgRJ7T!zdlb`91Omf6)-(@BQa+pnr8 zUPii2+3QbO>QC6aP4yO%avXeqQvB&1N5rEpvEQPoSTjd;b#?c2l;3VWuS@^$?bPm1 z{0LF_Ht`d0It3$oy@sUyU6+Bqt+pc1hms~YgR{w>^L1wR-M(?Wu3tWgMxE3+EH~Di zC6CLD_CE?cXy}GS2{9k8Uu!80F`i=INzkZ#Blrtq9D^9&`bv2nDRTH)Y`{N-Fxa-M z%V~`ML*<~*0UPX%t$rekCLU84X40-|fz)Sx#QPv@DEg_M)s;7hNS}`VU`)Pz%7oGD zA)c)MNF8}O@Ur>kv8TaGEB{%_^r?I@hnh@Q*a&_wjFEBZ&aYp;7F@;L2&Mzq7(NKx zP?LgzU;Q;eIZLHfrE^|g@o;^3)7T{}z???E<@ZbXR&M&h^q{*pa}oqA(k+(TXh@vc ztfHWqTHSQNQ+KuG3&z&;lESUM0vrNepWwV$Pb3jr`?L1e=`0(t7 z{XNrB`VRt6=9N|?#3XHx zQMXS($qNELwqFnYAP1WgFQwvpj}xpz9yvzt=t<_{`B}b>RFU}QhMS`EaxlK8zO*JZ89cPMacQ$w96_4DM$e`tMd*HI%)dqSoZf!*!!mxs`}9INf>!V?rb zBcbBgtgtWvkLE4LPoH{SckP(ICqIS-5>X4)GC8!g+=D2>1OALz2M)fiT&Uk$whiz} zUYCk$wzGRoQkIuCJD{?N;r+Be_TY|P?^4NUiDagjyZutBJQ?TFZP_I*=wBrZj^mCE zDoXV*3ObRHt~04H89jJ~DF3d9<$2Xx;_dqbokF}S+kWA>8S*x4B4XKmSqpf+wW6^O z+bth6p1fnrBX##4KiQNu;%7?vQ$6^8%A+hBLLb%7$8b%Kc=_Lg)co6R=i~JtbS)A` zoTve>2JdC3WcLWa_}DN$Ble{!o)06n788)p3KJl^#Ud}4CAue_uHzBcp`~`0Tx>;P zMM(@oq6A;ix+x6VkfZaLknBlTrj`m+O7S>88n$0ryPfBHgWVhr=g{9B6N>u4HjmC3 z;(KkH800=l<0ct9?M!Yx&FBQNuL-|>M@7?@KHbKkU`uk*Q#AugpG-RyxkB$id2ggy zxbtjZW=l$PMm#v1=`H_uV~mEM_?@c>q=|u&@4wet5UDu7?9BR2`!qQWa?4}6^p($T znL#K4T>u_MZb;y5Rm@Ym<*ohD!2&ODgBZi81`$D;Tu8)J0)sw)fz zQ#LY^n3&MFu(V{0B8^i(H1IOAWJipe0b{5{p+45f8dM5h3E^-+_`8&PN%xuk{65Ua zJ?XHmb#W;%-RF9(C95sGo-2dVLLp}DRNHk_TFT1#56k85fAsKIV*L`KblbY#cUxPm zjssjWaeMwAjn-InopitG_k;DkCz@D@kTbZd4UG^WFr~_?to#AV4u&XiSCeZZTgVnnlZV7ItSxQn?7Fr@8822lqc?_^O)= zg#7+LZ+J03Ki_MpxWo>&L#}p_I;3!?YAJ`$#Q)t;lL>pnd+@|bRjX^j3t71Xwi^uL zVFHv`eAI+%YHD6ViVpw&-)`<}BxZFrZ%!v^&ozZrs$K-Blt`MYrsm0qv^4EqeWt$9 ztKDL=?#c7+4L{27`>-)GItLQ8)6=)UW#TO@3w2iix1|Z``DfqL(^JY>5vMqEM9|)9 zPfXuB*UC!(p^jNT6N?mh0GzR*vM(DEw$X?7|9G**wf8kR;aVB&eIKuO2R%1>-gRg< zrH3A~ao_Ce6mXl@1)1V3mgf3&Lne1zOD9oz`KX3^DSiR2pU|(Q zjE$n`z(C04`_Q{H4^lt;QEwgJ7E5|n=4VJG?y>Qu@ON}}WQ+}S>)aQ+DWysInoi3K z!9&Na^zY#Db79;#Mn?3qb6wYOqa3W=i9T+BoI-#4hHsw2j|;>5vB~`kT}}PyQPNPZ zL%_F5`Srz=FeQGA&kyRRYU0XVZ_4~&a1C1>f5XGe?$R(($CRwiq5DU?idaKqlh@#n z)o?ypfb+_nwV=sNIcG$~t%;9m@~ZthcN$PX7uct^#lWEB0UK|ay`(qE{rfr^`s{xz zI3wHIt_s=xYn`krfIfRR^k&*8)G*04T!CxVj=Hq`k?{H$M#VX!*>!WYsVr2> z!A7f$^3EM)RF+AhNnNU)sNrjhX|s%8hpGBtNJH9BT1k%dP5~8y_V38X=9V+rnxyB@ zI;)Q>Q`PxT6ySbeznUSH)_(OYeR?x6q3jy^@r3TXz%6a4vh8BSqyKL?i?eK(P8ex4 z1vw#|AMxgsfoiuLQz>WU>8bBGFzVU(3Tguluw?kDFNMHDK6h_9+jv!ESJ(FrTIKk= zR1|7p66N1;Z}H8P@*5+Hz@|0@INT>Bo{K%~U;Hcm#I~+8yP8+}KRbTK$5&MS=wOR( z*JWWJ8Uo16vY)}im$tdbrhIk~KdPENb^f!V>D!4@>?DLS{~l74{5 zn9?cov4Pqdpcd&q{8dm=`d(vPaAm{%U>Q}&xHaus1=&jB#r`eM(_m2efL|7~mc ze0=7oPoLf-ASm3EO6k+dq#x!id0$w3GJwKpSJ~LHzZH5;BX| zNXwmuS}s4L3951As>v`^(PLRG)az;!JYGro1nwsl*+zKpv|1$PTaq}-oJ>Xjk$#gU zA&**ys;jF2_@{qO&^F(w_`#`Rq7I6(nRlqX@L{rbzx39(Y~QAN!}G%ppN0Z&u2Y1m z9rW+Fa)-}PH9zy4-uRRxRQocf>-TShv>U)skk4A0a#?QseSe8oC>s-Beo^iwQbAN&3)RMb4%?vNU#iO zXBv}i-44n?Os8nxX6zx!vVM;E#t@EtK3ty4bc+kUABIZL$Se#XilMdxMAX4UV^yNB4+#c~M}?bUvotd=7Iczx{BJT&=kuUK=6RCKZf2jf;#rwDhCLRVml zm$iWuEnH0J%BK>w`Mm5<=VxR2SKA+zOV3troU3t{CGr75`AXq>rgOvgWc zc=y-$V18d%;TsqQ(nFwfGS4OaB4tqeckfYNC&hF}j?HxA&6(F8tTwD#7A;rg(k`8^ zbBXmerc1p=V^aBD5PN^#D;`&kd2acgvoY43c13E@n!W4o{_PWbTR({7{aQuf2-a6` zeP*^Pp6JvMOm+yv39Lr83&PY=Ta7cX4>Ns_;+mq^S*VtHW_T9UP^|a&RMqR(cNCF> z$f~XBI-R=ChA=LyD56AJll69Xo|Yj1!5nseep$dQlsIO^X;C`eRsx__zO4#;JoL?V z*muTj$++3LJe)Bl-abgd>Bq0!p_(F^QZ~KgdpylZzWuQ!^V`qRR~9~c_Ogj3OSc`W zIV}8BYijYhk7CwR8}Grdk!YKz9pgRdU=S4Jt4|3nfX z@AO=spbGb;FeJoTX^QOBd6<#hBGK9CId8SlE>d}-$xpsFeJTC;3N>bji+ZX#!uivD zVxzG6y|?YTjwlw{Fy7g)_r6IcJNHXYo%t|Q4EfFnR(8pF|JG*^b5<08+%_z>jLvz9 z&x}+203o~@+%d#2TGtp600}*CQ0z~{%oS{py7c^-b|O9NLpe0}pk2-_Qp5wlr_0`x zJb$8qywMK%N7Z6Bxt~$~y3ca2H!w`LZfmf>8mIK!tyl`-w!M$6g;8v8_rf~u=Z-f7 zw!D$fCpvC(N-C6gE7IpWC+KsjZ_>B#!JRheU%hdnQ+)m*`a6TJ^ZJJfTqQTo+0cfh zikFJ_aN4grI(!}&NNE!}IqaqMQo8Xcvi9cw(OypQX`Gfq#?hHyKW_QLn8#5Wefat| z<-ksi`^s=ogHg+)O`6r*CX}{J|K07+G46|oCmiH zlxQT9=|YLy^viZ8jOL6f3pIrbz22|;9}#CPFTTV@EoaF-*dO_$1uexHC6_`kOW2z> zf7HfpC=L7*W_G&&(OTkT>dI!dx>zif&ZIAeO1JcD7Qu}yt&O^8Vonb+8-H>dZOT5j z=ssKiEINGjP?(NMBv|XZWOqP}#Al)<%&}f}z|IM=@a~_Xy3@@Nb%fKi`=~1jLx{A? zVf@<}+)buWe$%W`{}w*8Med0*pBdD?)F-nu?87NkPh4gPy)u?}zhzTN!WP9GEFo6v zUA7$r4BW6JhepYO_FuOBKmSbE2Fb+YQDKks9+$hAzIR>bwL2ZVuC66>DZ=pOb07EJ z*@XR7Ytz1|*nt-s2)=)w@W5DhVOVO|iH6kYe`If<$=N-^S6DR%D-ETYO6} zsoFW5z8pVnN10u4H1O1i$7~#Df`;QWseNzudrI%hg83!3-sJ_>8FNE8Ro#47wRUwP z*HrcUUsHsz80as3f7sxlHHo5}8Y0pwK+E|FDFa@aL?F}d9h0;(wKod4Bex`XEZ;LS zawX2)VCcI3>X$v|0C}^QDSESzjiZuh#ok3THjW5LJJe@Vc zSFd$qpld?09%k)Rj{HF1dljohKdRA+X*N}!31kZ6jnHdx6#S(T8h3xw*b<)MLjKDu z_=Vgb?w_bUOV)DTTN=ItbW~48>-=`4LQPVR=}|@{pzufQpp4G{#?RC}sM&3Qr~d~3 ze(!ov41HtI0q5Q*yk|#$z#N(Wd%Y9c*}O@!b>>eX>+bh^UW4_H-BN|`v|1riOPa6( zAxqD{u7ODVg}K;)Z+@Y!_O=vZ+STcGw){6FkStQF5x*W9{Z4V&n$Oq&imaQ5UKf4l zJZ92aN(^Up`V;RvQ^9e(p7Us~Q%h8uGkyTwdHcnA-_vd4r8Ppo+S8R!ssFe?rX6#8 zhv2nY2A)3?#k`6ke$+aeWdGL3K`q-x33I0L%RgaM29P>Z(0ARoHo@vLmCu^%7zMST ztdCZL?{^lioBJkQP}}|%#qsJMRf|uGmwfl>dKrgEaT=k>dnqTDe~wdDjTI};9}xbN zSH1F{mFP>r_SSpKKjyKD_^+v+K6!Grfog+r@w4?d?NKeEVKjo+#o_2(XNJCVF5due z{(d@7sQeGi%xsbNV3fZZGx&FY;@~w z;Nnxyt#bb(3ggdprY?Oe-KS<(&4Oy@f}LDBh&k;v+H05k@7B$or%)z~CYY5`9hsRa zo*yh~BMV;IAo8UU-_V3vO$QA8mRyW(r;jJ+UdoOV^*Cy+%ym)fFa z9xbHZJUxu+*6F=E`G=*R*zcVOe5(bj;1xMKjc(s-=xgKMxW#P90GcAL`G%9pr5-I3 zcE+LKYzZKgL{n8++1dTN`um4`>IHTmm!B!wf8>47)Jt+E%k@%aEny+hbNBP~Fo}5_%Xp?^aRg&~~?w z+LdC!bRwfkZ0D2sN6e_lBFkAMzt3i-K~^=_;K&mdxy9L6eY2DMRvYKyjNuE#wW*OO zxCH|V#6#kTE;RMQ+{8oL+&b=o(>87=BaZwHF6eiT|If5;FyGY&Iz59{L z7?+#SB|5?CYT4TLM2hiBGZyKA&mHl}(;>TS3sEsHf%-kw8*la|i53&${AOdLDI1b@ z{svI+KU!s*)b_R1#fVajtZUMymVRo;Xi4XvVGB^j`{>I2pz(8o!>L}9+ha4`LTKwV3re^)W@)7`or8=3bx1C)1o;9Yt$kj@YW8S+4x8tyQ zlCn2j;!C9hQS^Yx&iRZHDffB4^HsC>{4po|%KP^yrX;%VhYx>o>i7NIBxlK{7+Q+M z=ZIx^Nhta%o7CQdMMSEsnWqcX_C6KEwTkpMRP4n+x%f2_($)R7I9D70QK_-^^vu})A z-t`5E9b;y=J(cE6&PY&!L_yxxW@Q-dp_kO=AS*Fw2Ha#rNKYv{F@ek{@j`TiJ>6y-B4;8b? zvTY|sAju(G)C!;Jcq7znyYQ-W_14c@_zdXaT_cq2t`=Tgl*KPR;xP z)w8zr5|dw>3V51rcBPxFI!UQ&&$Ls_yc&OGyxJn7+U>7;J%|(kD8uc%rq0aK!qseG6;(o;M_J+IR zZ=r{NJlQkENKEOj2l%Bvsa5TIYrap%?(^~HJL$xgx9@wfv6{N`s7X8C8l-BT&y>dh z;Yczc_)?dT(5vV7Z**b+{D>eRd%8U!|0dIqwko9UqdvJ9X-@A9p68EUrB4*~3)ADF z{>NUAQOocn!A^ytxSQ8#7WaTm7d(6Dx;YrMH9a4mcZ2E5M&;%V4y!(=UX?BCpLcLT z*(?t+!!+!>P0tG+97QF1(grsB@5B$ zX6ePYvU@!sX_X!0GthbPLO+gRJ%F0wS4FKA^m_2py3R=%%cH`yA02M+ikZ!ZriAGA zW=k$8LFA3X?+=jy=MjgM%~L%%;7c$&tKD@~3>2;gF`0EHNtcD9vUC@>GSd6muBuRN zPN7&~1X44lO}e-g(4KY9k9q{_xu1A>nAzDn8`@*>T|9AiLuueeH`zqx*ff|~eRh;8v%1HQMM za*wWz#gbH)j{NQ1OxM4XM>n^U9yibniv|?3WW0d2Z;+6vl)R^xQVj@=y`CzgtWGPc z@TlF}a=(o$wzRfW@L7}7jX1aaQ}x}S&j>OaPMrR-r%jYu;)+9f9p!!wSD7n$9qF~z zA9JwVqKHqbr!xK0tnf|$Iq_+Y()QggnQ8lK=l$rgkyOfWnSs`G;d=kt+U+z918yBV2Zj%(6i{uu`_B?$w;c2Kif0PkvW|{1nNx?H|__b}fje&WwQaPCF)Nrfq-1`rpCI5^}nr(}R$# zWIjlE=;qodImO)QZ_!VW_o|2|Xq~R+9^Sgnk-@?bf589n(-zH>vb_quqwe0{Tx&mT zZ--wqYfpNV%AAfk?zFt)K0_~NKm`q0mrtUSta~U(O=Um2x$V;b&}<4G5LabQ^J%|j zwkeiE!}+J6ga@?bzN^gZu%x1yTQU{jUKIq~(3@$lI`uGpMgJJR6}#&>Kltp(iLzZt zbv9!fZ-|A&==ffi!_C>oOo!PKCZ3l)p(v-(WcMwV)ksN{p6xo{+b%?&}HNbqIcCK6dq~IgrptYMMA0$({gSXpxUC>?hC3 z*aZiF<3LO0bDa8c#&}&$I4va(30V}V>ZzrY!ZFk;+ax#~(xHD_8hgha7U!A0Jiy0ci|do&W{`=aveR96aRf<>Ylfi@izx5ef=x7iSkQGkB7&%03R zZdPJy1-8IX11DA7ui-(j70HOTq*@Y^gN`b4c&m0a(m#3;3zJP+k0G9KdC%GqS!?<4 zl@3A2C%JhT8LM7ycZE%DbsnA+`0fmmS|(3ePxDG&d|$754uIe3jO()=pKio(=>{1HyoKkB(>`mVk0kxAjUVd?2I1kkEHy3 z9_V|~(!Y=K3voUk32<_c5Ps;&9JMsq`P6go*yEGyl7Pgxb&AOQr?d@k`F4ru9WWU# zTW*a$>r0LCRM6x_SyAix$SAbpqub~Vsi6j{!yC*PlbV<@6Cb~lrH9?hNVGvr&(nbtOv?cj*WXLZ6k{8CKttTvFpmy>ZmXXkJC zX06xEbM6y1;;RiV{}@X44&zwws4TU|M!?UAp@ow6HHUiSrJPio_2(%L3U14X$g#TB z3Rd=Z1-YrawSwo2G5CZKL$wE~hhz-WDvNnu$vZap#)Dv@0gG&^YtFa>#QJSyEi=C- zK9OutcQ1|)65Js8N7HIRuH$vU>9Y~LH=TW}Tc>}i2TaT!vcbfbQi85ymDfrk@RU&5tAmgn4Yl9C#p___kW-{4RV?~Dl z4_R*kR%P?V4TGSFq#~WtAPv%?^r5>`y1QGY`_Qd$0O{_MZV-?zY3c4b?|txjp8xm0 z-?_LhFVDSacXnoWcIG#;_x8>oHcO{*+RW5g;clf1L?O1ZydvY^h!~0xlH8y7%UU@| zf@TbVSC&xL(%P{B?VwOxeg`8S-bF9@qZ9bK{=8Xj(dh4JDoNOJx_{tAMd&Oytedw- ze0FlZ7mepJUf{wRmfi8hR~8!BE)rc@)mvQhn9fThT%N3GWK%;=oY?N#j`+?EzwfxQ zO&JVePMi8~0JV%{rlP`rzkIJ1`ZuYUc1;9Jr;X3SlJTDO_g%fcthNhyj!sVaT(+pd zNhL2vWmeoZUzO6B>K|z5OVBY9IhY<%gRl4$Y~k5gu_ z!xT>g+pB{X!&#OA0NeCjT@wo_bEjYl;gG%PLp+gvZD6!uJNl3N2D8lsjTU2FX0VF| zm0_ZT7E0=_ojB#3Ik5QySMXy(ozZ!XZ4CozT3tVIM-hj0iN@#?*YyK37TS7viMO(` zehH9P$iM?F5ZT7g9R-5v=n5LIK%$UmWk{1{nR zGqWL{;u%_xu39ah?0N`TDYP=a)@5PJ}m1w|1H z3`FAFQ)_2I6&4l&$?SblDqHeW)sWiFr3W1!XAIf%QUJjxxqy$J5e5iQIXGD6=e*PH z<+shS_{Qw_CuDd*0gEK>gOj7Woc9oUkEzMWy)|(XP2TOYj01_(F{Va=eLm+eI2U8a zp8PX=7jq@X9z_NMCMNGnecz!H3V7xJta2NGk5Cf5E09tG9PYxnZot8x4Rj;DW@s&pB@12B#87}#%# z`12<~Qc|RqRaL+QfaInmO3wSgng5-HLP~91Se|rT| zZ-Djt`BG!40OdvQhkx_w&%V+3CmI3OEuOMKsfWd}>NNzob()rryb=joDCr{<7nJ=E znFL=7Dbs|8F#w$&4h3ogVq32Bw0_=Zb8!4K__zTuI6n+>irs`MeMl0NwIxq0rswOw zJwyOK{GI4408WUVl2eTxfBhdNx%B;c!`sfQ&f@W-baDA)0C%(y0r`$=Yz*Dd1ECu^cRYrOhj zQpEwL^?ZX72yvt>a18?LxULGxT_Tue6aF3ZH87?Ikg(lQm)_c@3=9c*o*)8x4{(J; z`T>_{=cByE!0X2XqY%j)#RTK~lZ^ELN4Fs$m>7mr4zopSvJyAn|4Su-DrhRYWxkUB z2>T`QYE{@|qF_}Luf?URzWvK-K&5u0HiLl*5FQplS0D}cycwxbwN$EqW%XT!%Hs&O zg#(%is@TQ@B{{di+q^^B~4p5sxK}D5NU+)h41?3gf`!hZ6 zZ-+AD{B-}+nuu>X0wYlnc;O9y`e&~M6+ZImzyv6-0oY01?)EY}>Nd9lsA&N94A{rO zEdio3%%?76*)1p~pOqi?3bw9j+1YcHw4GnZ2CefwARXSu!hWPo252Afk^S_fzwjCf zIN$~=+xe~}0!ShN)_o5qF0Qu;Dpx>__jxQo+Q$NkW+&(POb;+*xUhQFeofM7~U zE*lHTw!ls+ zfaPy^13XCmIy4j^g^Ly%@(U(s0PX|I+|C5vYZZHrjPln%ToS}(B@d#1y=Uce8f_@3 zW`!$?8M<8heY$~ZNGv{QB0#Ij7H4KQV&-z8cCrYEPVR+jdPcQ|KTCmXXR6tAAx3(< zGp~QL+vv?8LAi*OF%-Ba5xZI);Dw;ydh|q}Nqe-1SWM%+f{cEYAb?<^Y_a%lxGsou z>qc=WNHYrsuG$l9RVYT7Yu#z;+!*8~Y;ev8|%&|7WF&UVH!+_AlUVoAAJv zX{AsWV0>J7Sq}|H^=w>VBMrWvl$?D`h&;wxD(_?2t%L##RpE!Aeq zEQ6P>elzCHU|o&6AUtQco{@5zQF|4_&K6Rc!Pl={=kG>Jm%Ucy<`_R=mmFnJ zK9A_1gYYt{aDbQ5mC+}GkJ?W*>Jr}p(YZ?K;b0n=4%K{*LgM{J)p}3Bh*b?KV zdEd16W@UWtPLTQq`qH?T+G%)x^KS}f-l~(&vfHgk$ve^xT_vF9BK*OBEqNK9 zxsee*K-)K%^W&U`{lF*j>C?9wF}fX!bWOoi+Yz@Ot3xAAZaQNmMwix`w_MGJ5Q+_9 zl4hINi^5;4%U^rN^hYrg(!qsrM-h^?ru*8U%0!3$V?!aI1_8Kc%Y2S6!mNlD(gHn^pbqh##yy9(%X zb5=PgLyQ*+(QK_W#Gr(;*QUn7dFt0Z&?eOXH3Tn;z>ODy-0kQzLi4Wd$DyLP5$Etd z>Kr|3PC!Z6K5B?S3Mg9abUV1}`XuRH@s$I$vae=exANf%L=Fv%5&pO(qqN3(4__A= zE+ZrTt(y*!M1cT%y(K*?;yUe-E;1>amT+@6=)=Br>ijABH6p=Is1IeE?}+GkC;l4; zi1X3I0sYR881|914iu)}9ZvFQ>&P_h%}%NT>kr9$bJhLx9`813#j`w#I)l;4=@#r> zYO{Ck9v2SAr<&ov!i1HST8lIpfteJi|`tDt<5w`6 z#Tve!eX(iU_+6JV4@NkU*43w)O@Ay*R02s8qPoj-|1B^DHk5DPAB?4~xtPh3Mu7s< zL{1Gxt=!2XD45@i?-|tURRd4m$<2xPrhq^Kgz3`$B(p@-s^euZvKi?}k~~y=Wqi8f zes$V`+q!;{no_qv9D_*2cpphp^dknjhQv}LMzZ@F`73f6PP+^3E_0E~4~qufT2i<| z{U*4w6y?8w%j2`wZb(!k9QM9>AMXi=;mEJupy+k1l-oD)IV~wGOWxkUDNTu$HJ>-{ z*l(Iqg2NQwO2)Jt8&-SND^+sks?D}= z^7(-ae4Rd9I^_HNu)1QC?g5wer(ececj1(>_xv;DC~VgoWBaR}{8e=%K?>TR7IIZp z-pyHXojoIIvEYz!wh2eTs+%W=i10;JvJLE2IEOy}YHx2zx0+N ztf4URswn;ci+;8F^fwHZi#TVR$dzdDfib$;$O|oqn}Fc&s0HZ{-=}I0C%<$jh23et zK{c40@iO-FZ$L?2xBh$!_{=Y3eH3riVyF5q1wJ^5g{jE?>+b=@^+}LNXNp#+J?7CZ z7QFy{gGlba!o}5p@DYQxmiS+5RjO@$v~9-*N%^d4gHcAnb`PfsgJP%Jxzr)8Z$tLd zp1=zF05!xRsFYH&(FBYGgbDDSEE@hx?veBH6;%XNjIaOe*O`8MQ%H#o^A;s_m1*h5 zPXKp!GFuh0`L7RKrKqn8el#=?!Ruf!?-Zs*tM@XY4pUMgxtJyBU+M^5bgA$Gs(ppu z$kQOfM%LR$r@H-KCprJuq^f}EABBp5(qMDEerCqT5wL9XwH4HT*sO2&GQLlF8a$CYa3nm)61imYz0 zv0f;jn|ttE4`@h^q+zg^k$uU~=5m-kJ>>jpP8g+H|0!B8Rd%J%ilTB^E3Tzn{G_bK(bl>8=@iL!aAS&XcDl>lhQ-#Uys-@2s5|IW*AjkzCz0YT zjCx^7`aLF>d7(>W@EP0o@t`EKMe^|@F+>c|>o-o0j?O3V0coZRKoyjnrbB-TK@0l? z@&hnq8>b;OTGb3#bx&*AxWqN-67x|zIV$)`WCm|PTYTDXe~#L|z-;SMAaDuo1q~rT0n6fE)IMuaHsi(hmv3!5x#P0au2#Q;WEUhpa6Yz_ss5 z_hD6+_YA{G-0MoY)(@wVz$aFfu&_*saaQ?jhfl!%wWXi z7b+A^u*riVHKvqVZ_fZtt-x_p^+V7QEB1xTjo^DxjWYI?#7)CAS+sByrdps9^I_mU zrR}Njis(CF%QJR!2v@47yL*L1N_4~+QOSK6__dEF?hg^^->eL8Ko+qe=HY1xO+|plL6Kl?emht4%TPXKUc@7Ko3cd(}2S>p{q!j&eYL8i?!V)`l zv1wwWueZ9`N60-=^L6oE1g`!~^7ZaX#q+#-e+QXnlh+x<{$@0O-fS6Mg0~4SO$?v- zF3h@~s@q?zjCmdEj8eCHn(|6pOW5z{aIsY1AN;XAVx>9s?a6Mtei&Bw@Eggpc0$6O zbfV4a)$a=-F}y8xKPx%BNdYAZMmj7yACSI^YH&h6Y&&|9#XKkalC$IEM7{C&J5;MJ z!}FqP``%9!DJN&{jYv;S1qY$G1ZAU{y*n8spLow!W-4%<4t~0SLi@4Me^@5iAMw;` zwt9jP6{$vkqyF@v`}(ty{hpOU?(Wq|PJ(ELBQCD3*QZ{gtJLB*TQR@k7W_`9&g)J* z7D}3(dkw$9Z(bZGyKgvm*!sqXN>sQ1FrJ@wv)_38Mu%&k#Xsh@w* zGl{nmutOpRMy+p?bW59kYE6I_X_GTC>Cs>Vx(N|epruo~^j>DeU4X2Gxr4x-1I513s58FxLe1h0^t6&m*rx+U>!FMV%zU+xU$gyjRnOo#> zdyLPp!yiO=NGQp{H`~k}1{?*ZD)zn9qxK{Cag4h>l6y>T)#@8KP2%;XoQBlYd266) zIzu3s`QZ~2Zu;o&i%C)SOh)h1KcaeC8GWENyOo$e??fNL&)ad8v_5x&a{JAreRg(g z@X~qm8~2m<4S#BjBwZ6Ld?uJyJNL`goyL?N9*%quGHX6Rm_3Bgdj-av-Uc@x?}^rG z$lLE8neVB+BxHLqR$j-~aWi4F;pe+LE#kWRy&U~2_UO5pbLoK)UHGSnS#RL(9^B02 zfgidwm`;Or-nFaF)Mv=>5;W-hSR=jRI91=)Sv;S87+Y?u%48FU&WLWm1R*MRHFgtz z9rQS}HZrI?U-^QvhDSX6%+>VddeH9l&}?nnT~V6+Ul{N1Lf$~v0s4d-Tdd2SO)wmJbuzA?o2TaO=O9}$^4w6aI~F=FCZUcwMMlF8t! z3zV?M`h-+|W^CsBOwT)U@%8?~upu&$qr0EoIgP;vRIO;bn0LO- zCjv>@n#gm%4g0TDVgYz=s&r0K^)U-H4mejl+)F%vZp}+2W2s@Af8s_-O!bKqPxaxO zy9P(>SwhZA$8&-uW_{C*op*S6>AoklbJ!aOwpwety@fe1`hPE_duYA7Y!!+{_Cq)9 z>Wjl;A=|M?Zv?5wQ+gbO-k zHrUC%^Pn<^=P!_2t&Fh}Nh4_DD@Q=%T8=emMB5)#aFpK~JP*idDhgq$RS zQ1}^K?$da!tK2U*xlgGy5HZcUemwuSHPheB@6NjH$C@UenbC(tn$fiw$DEC8U`Vr3 z*{_pGva zlVHX@VqJ4q*DS`uaLVF^ccK(uw%X`RgVR>M+CGXDdwun=0VD#<*!P&ZclV3x#dWTg zzT;25TYBf98C9`sy*+E@6KOeKNJnR*gMG5YYnRQ4;-v2)-Yxr%%BRe4XPgr9#0EPU zn<2#)ua+QXuIzHs*l(jjvn3#r=?(J7f3|K(ZEWryBm$Stjnf% zHTZN?OxpXv_WFV*vhgff0sanbhoCwR0^>s~|KgG=#zNLH5w(#HopcA#b3X6VVpd@RgIff_UET(%?=tSh;(3&;k z9ELjgb`E8aE#ez%`9d{R5!_Jd{{N1HsEyQF?oo}AETw%`v1^?vHcu4AVvegk=^~(0A9=6^l=vVF_};;3d_ec z?YxQ|xO*Fly%5g92y+@cvWLo79Gc)++-$pR&m=|!?qB1WFd;FZ?@lL;2QC{Vyf&J# zj&E43XJomYuY4DBke@CU&0}}6Cm>(B{flCoK&|WMY1YZoN3O*Yo(#Qqe=iEBwgJj| z?I-)67S``761_ix(5sB9w|kRgS!M3_So07wIzho*uXg&4)=c?=Be=oI0+*#ZZ}atC z=ur+chC8`zHon9Kk8v@5@*n5tEYXvbi>}ad?y`sgWe%R@ljh%FP2m0H%rePp8%ft) z#${Sr@w|gvP-OI%g6=QzWiF%Wb_M2b z+uq1Dd($%y@urg8vxi(PCc)E9A6+@U^vq<7F^yG8-DCYUlti*6Hg}Y!$OD1n7%|Of za9=CvOd@jxVo1jYpEokn(r;S8LB0d95!q_WimS7So8QDmS*lVe7!e0hY#HT z4(5#KcEE`=Ra9U}0$z|fIa9j_j;92jr(}S~d+48?yNXGi&S4XY<8o@~^}( zTsbmFprESp-ZGO3lx;B`o6fQ#dNE43oFCCjV#@L(c zR4{9(iw;Zk0 zS9et@PD?Hep7&MGZFdHawRh)nlpsq?PLjl$O?mX!uZK*!LSi$&0$-jdeuJFFiK;`K z0Eua1zSXJiR;12vd6zPVL^QOMoxQI(!CMp_c4c^Bw!Y|u7T$X689s>g;mamZ#z5YW6j@k9;JTc1QQJaM7<&DtPQ2Sj8caE<-q7F^ z(*5yaQ}J^`+C0+t3?uY{Zz(66>o%S~83!>k((?H>(|X2t$68aRi}pR8kxxIomy)8$ zmQBM4t}mnn)qJh}+=tjiNd;08G02Ay6q<8L+1l!Fg~hv)x$wpyN{;rC2~gzJ4#rS| zz}5IJR3XKE4S!qH76j#4+unQ^sg~kPmbAeod!#u2`5TD{yoLir4PRfq3?90e=Rj-l zPSf38^Ene{+)hqqXFnBaiiGkb5vNLl&Ti#YJzvoP7n5}m^M=TjO9S7$6Ae{hNn zA!-GH{eU$iD31&_DaoQKd|d8m3Ev<)ja{rD2Aoqq`bncl;*}6o^f!YS2>uc`prGWU zn*Fx&8&RAjTFN8wzx+A6$k^H+$B4m~DrBm7t7Pf^$Rx-!GKURTj%Kn-^1nhhr7dU# z-UO0=$JGGL9u1U%0h|?v>4UNExd{*3Un<%$kK@ntBV%O*A6Su@rkxP@z&F|#7&1a zT$-B0&pYk;Q~>T%PBLN#n?!bhB-yhAuD1dB@)O{TSGai2`sRhM+cyX#C|?c<+}&;E zahXwPk(AMC*Fz*iNoP?tn3R~v<};_=EMUThMB=TH@MMS)S$%jlbUw>;($|c&)Qr9> z>10gA?Nn{kj(h*N9K`w*2ZEMRZZIHvFu2=MFSG|r8}{1=lxNDK?^MJV4aO5NDpk!r zWnQ#BAQZ#!U#{?JSkb+j&E_vD#1Cq_fvPn+x={Uyg-Ee*oDY~P(r7C?PnNuH5n8qM zNNf{J4KB{yk`>Ft+-t1Ay$Pv0+xI(~uh=?0s_TAN6>5)$V&>Bqtj}16lqF3keY$Mw zx#X>;?qbGS1GQ>fc@T=)xSVd0{)W~Ga7Eiho$ykVi^Iy-HxubbJBAyc2H;H$71KvT z){pCr*FCvAU~V~he(_k9Is_0c3VvK6rZ}}I7(6pa%f>LUrr$=|`O>dGV>GPxqhQ^HKHWJ;EteacF-9zqso=jJr8c}%cK@>ei zgQO#EVYh-hjj}a1tw*wZew2}k`&HXN^FWKX!HJY0q3{puqv{o~B~$Q58ueRB+Whpg zcp)e7!rR?p6K0T$V9rb`%gI(tUc8u{P*?oMHsnj3$NJ*3egAhH7v=)AB=)rmT+JBp zvqPbw-P31IlEJ*t;{+7*g+q5HsF%&OB3cKzsgMV@pNBCr#c0DPsVB zn?PlZ?JZF^{{4KcUS-55=2q%HDYK^N*3^ZlF~1wWoVrU_!1P5*5iRt73`8GpL`#1p zREC^oV0eGGJavvb)PfSY`=G6gsw97eM(Z7kWa)h(Zr0VO&9>I$H5}d1nm9ev115kZ3Rwt%bEX*|Cj zV&jKx&K?XanfK{p%=%pgZh1}@ZP1QVO+vSqiA?Ts5<-%u(Lxzwo5boTLlaeaK9;28 zSSYWDSKD{(j81T$GSIyUaGpDxjTDLI3u`{hAf=-v28#Gi}-)fMK>n=sVR*GDQCvJW;?Z+)bkJCLJg5<-QmR#d1QqRI22D&8* z%2Ito8fjL0`cu}aCm*WcdfjX!cBWh=f{LzIkK2x?gK^nc)8BI27!O7w^w>Yu_f>!+j@pdC@{=WJC#fCu zM5(w4c60wT66|r_CZzB>bll_$p2%zU!AwGO6}2X`SQ?vA<7P?t*qf3Vy2ag)0#f;O9!@j39-iis+H2=-oBJjGTHGa!w1dX z=yeyiQ5%~qO*GdQj}}V4&*q=xWhEN5i( zM>NyQi1soBD%P3e2Nm)NY-w1pf8anixR14$))d^rY7KE&nK{MynExxm`+gu}!3&yE zjmH_3Ga?w4XVtbE$Val8n8r7D8>T--O(z_8;AQ^{|8|@|wQHbEf2*|VVc3Py%YOeQ z52a&y@#4s)ANl}wpuhOAR-DQS!l>>!E;SQzPrXy`7{xi#QmQ*2w3$5Al`B&+`WLaG zq#>2HBt`QQwUa`5lM@+?9x69EN=~7vlHh71Da6>A%fAw!v@i@5~g@(#T zDkB=xvRZ_#^~+_Zcb5H;2dWjxo80XsqF?uP2fim1Jr}cqd8+1FDC)ig6!JdO6JtqE zOR)kV1iYW0dr;)I@`Ck$H^ugX*|WI{?1t9k0;$gWm`o0k^cB-l2TgrDvcU(=v>Ah5 z`o+16zCIRupzW0~{!C8_@zJ%*uey$}+WSQCoT2CZtS<1K80JlSEK|+X3PGsW!av}G zP|ZhEt4a;WvGmI?w;ML3MYnm)X%XhB+xmDo}K4gLHyYdJVNM#F@F1V3g9_Sm0cU$WdJ8_z3#Yq z43^s>a`iV&m%7f-&+vanmqwD+bxP&6+}bGx9d?q4R6YMAohGJy{U_k^y7rHHwFObk zlp$@CGp(OZG|^P+Ee@AATd->IR*dR{z2n4%^Uf~|-RJe2r!*dS;ig1)mlHhSrC-Dh z%H9&>;6LlcML2#Os`o%=-1)3K$!IVywhJ!VaA} z7x7bCQs!VYDj8m{CUgx>FW0MLg_LF6%M#I=uH0fc4F~2sbj(ojd|bd^NF^0MCXnio zEvbn&(f1p6#5q|HG~)RP_-B9@Re>uo`s(8lGef~gmzX6B%oc*O%}&7noK~q<^vgc^ zU@DK^@JD~aX|0=x5Arfg!1<|Jx;H+CU+t%u$=BXD2-T!gS-=6V!|Ct$1~(jZ8%@nm zi%T3;wTt3A%38n1M4=vzxi6Yw86R{VWq>uVCvh1H&KQv;Z6;GTi;c$ylq7e%4Uyc; zX4%(xeC}lV`8T8!RqsoH=tNt=_5E@%aC026CxNrFwm@GA692$p38j?EdOd}i;Gkt62> zauQ|-&d0j14$R``Qk$Cv?3&~GF&HoS9R8FxolX{&ovJ;2^knqT3LFkP2_K;T{q{b# zV02Sa2hk6@7)!{1&C5I@7_aPUSD3>DycYL;y1g>87Uml|e->0OtV)~J1~j*(D+$w0 z#sEr<>*KWb?i+T7qOcUV7||Tckfev|5=cDd6X6a_saC(lqOugxG233J9y=$CS=t_n zy8B2ZEEKyPisvF^!8y!)!G>dDC}NU!DT+WEBqB4I?V;n|{bZ+&^%y)zOXP>#duCIa zA}S;-e*3WJ*($RGNRi@3Ici%ijeV^n-#^NN9KvFoxVtlw_ctGvI? zN|{+kidtBbAMum3XGB9fG$OrymzjT98A>z?p}E)^AB@ho%N0QfZs0AH9IAU2jy?CX zA!8O6zgAahFw`=|S1T)FQ;9nPf$p^!*j=rq5HdT!H5(vzrs)^^4r}cay)n9B)d|W4Sw(JrQLs|OWZO4x9FbB0a z?|l(qvi(G_;o(`HU{-Ivo=9J@GwIgRikri@Q`5LLR+jVeo&1%z$(IWOEye!LK@Lla zP;Xw`uLKmwOZ=pSmb%!#SLQxRS zcV{c58CeqWJP`sPg@S5jpM+^lG2kc0Gh;$xGM^L~6%Fz+z=QjC#shm~%+w6yMVzV% z7b+Sm{2|mn+$ZBj{f!qN51PH7^sCGK?mO7l)-j7&ifwPHNF*jzBeW!wLwL#n~P=Apu_159MuQhH^Sln3JTkBLp5GkkY;!3B zMpJn8z2GBMP3ui| z{V~|r%q%`0gcr+ZEQ@@l%PBMeU$j!UDug=N`+8GF@URwFAkFkAKbG32{7Q=*Z>4n7 z1JU&^5mT}=+BBD7l>YgE!5C7s({zMCeEyz33EMN#+k-*>cq{k2hUWtE_>xtU^s*Ot zA2y|ZjxJhfVP0OafyeHMiezzvp!9>%-D^l%LbRHVL~>`mpzgyZcdK* z?Ym{GTQ|KgEOl3Iad1wS{A{;6%}H;ZEu^azkprEUHu|SuAUBc-=T{qR9s0cP%xVjm zfAB@>mWcRjb8M~GaEXjl&EwA=WcpG;So~B*E4Ikj4IG!Gty`FrIkQv)8{FP z2r`XlNZ11}f0H8{Ra1_Jhx!U|A!c?f!tuXHKgs?PBH&(BU zW_#=VM1V2nt8bcBn?nOxpwWd^ZB(kYWA3aCAF08_l9@BS-1m5EakysXjl*TLxr;rl z2A*{S_~2`ti7aUR%u@vJo$u{Bmq(%p_wHt;Geta|y~G3BTtdTsB=|isD8WBBj-(qH z_T>(IH*re6IplH@Tku_WuprvXMz;w)W|hgbi@%79^%+S5gbeh>W(>g}kJA6^(N=mr-^s^3vEDuVV!_qiaQ1dD=^Nyu)I_ z@1BAIL)l{S5NsjLP?yR z7sF0G2ktom`%#zj>}61s`(57Kl-C@2gT7m=AMHueTd!V>Q!fopL@#vhS^>espW3a0 zJs$e}y&7#g$=!~2AgD|~EnS=1&C0#qK#GgIx>|KmO66J&ri(t_glxnEn?IrFz5~A> z@$K6=YWQJs`%P@KlOM#<_J{Av%Bjs|dNX=i9^8V*-wMDf{VBiHxVZnb%;f}p`qQ6F0BiCt(nY9xd8pJ}OdrB_LTXCwm<=|Z=i$=@<0>9!!Cr4}DdG!dP z(W|q@Bcf6fCU>2^a_yIz!}(BbKZ`?{o%es^-c(dnugf$$i#4wqv>T1X2PMc~W{)x4 zT#$-N<%oYtL!sJDVc|@W_Vr_LpLJs~OrlOer_egvdG`wOcXq?=B|B4J%L`qt^t{J%T z4y=MCnxmLKR(x`;`T~vIjb-FC#aMF0eV3pa(0trH${`JYs%S! z-)gHwt?~yTkVlj0w(Zkk=sDY-uxuc|*QwBmFy|PIV+J0j8PIA9%>wu0P~%I&aQXQk zhMK!s0OlHj$2uOz0&5Xl|D2ht4g z1LFX$C+|);M9bu#9$toHzls#MYC;7#K^*nXct7+JT!t@ytE80i*gcG&+#W`>k0YI& zp2DG0)#Zq)8v_u26FJo+{@<)Y^;7hDdJy>RluDS@PkxdC_Q>SlM}MbEDAJ#rkdYw* zBu!vXHUB`zv21>_=W!@4xzH{Lx(T-h0eE9srzQg?CR4 zPtt*05*~qlO4Zm$jhrw2YQR*XF{I!~2#5!Gd9H}>gAAZ;)gmm9B7)FYKEjrscv27i zW9H6_UVu2aFiXI$c>hp~{nz-D1c9|_fHaB%%XFzEh>z$|ETFj;kJPG?iu5y90W9+Q zheg``FnAuaV?^6v0jM1S&)2+F8b=>}4`8_GtRnI?t6%U8;;A=5E8&3G~!L?cRPhlyNQ1Pli zLiUof(-c4-ssk)aE-8;vc~>O(1#BWxec=KK1WhSn^HEPIZ8Du;3v{v=uq^9aqYH1g z;)}UOiwuFBOW$9q7Jlf437v8>0*En00>LJ_u|PMQTU+~pP{1n0e^YM-seetuDLt@^ zuj|ya-;Tf0qU!&ri^#u9D?uJF^TpRCR5un}&H7g*3dR4f3sM)>_@R-JH(YgPu%`j* zb4>4H{M?BP(*^6HF#{8>qDNnT@k{GDUZnVRU^Xl>m_R^Tz*HsjsCI0g4Ir9cs)0Ch|2AuGhk*1Tb_`rYA#BE%p9yrw3Wh8oIAj8jFuK+#S zCkF=n=WkN}gM&lG-!=F6C4?ZBe=P^Ysws$|;Li?j zD=(=5zu)2%N0hwXG^wOOMg6}Y&&ABDq^yUzb+8y8xGjTIQwQW0o8n?)L73Kp>qWAL zc$JouQk&SX6^<5(9C=+zC(dEp#@|1*Vb6ed;QD7tDXDC$%gkL~inZgb#u#SQPANN@ z1G^%$&aN(Au5<49;^NecUKbyy%sd!0E0b{;wQQY;|La9*QDvY~KJ$02m)`2ywb*-h zaXtHY`51kW(%StJTuCP27nv=NTn+Z!ENq6I29>-jI)0dlgiqtJNu?Q<-{e#1dnelc zHOR0&A-vh~RHQ7nRQCeki{_QRRu*v$QZ00F+YE@dVJ*>{o&kM zoLa>sBkjd|b~ZLTCOvJN8p;1AGfz$SikZA-8vbtOHz%a@Cb1xzzpKlNY^z84PaHaC zrD=P9^Gz28|HTy1RE+TMaIwRFbM%#?tQR`BHEB?-{L&EErPouHI>?L>1#ueV>ENdy zE|>KDq)L$~#)0SRC+2}aPV#@U!9Dpz_O-gYx)CMzUtxfK^PbBhk#-*<(Hvancn#3V zv>g++(lO-9d%pTIc)k8G?_n{i5GCOCoO$u=%yb$L2Ul^3pF|LfNb-y<&8Rf%M>+Gs zYL>kBP#sF5GAgB+i)r6dF#EyRfNIHS&o!?%P!=4v*qd(`9filu-~DD`;=WXWR}VGe zd_g(AKZGOP!%{rcudyFW8S|&>OSwC;zRwL6urubmP4qSP`e~K!wPEij{YUIh6nBx| z&{hI&8+E~H0IGSc%7s0q$)5(UDkn^Z#v>}d((y*E3aE!!h1Vcu2xPqZDZ`a1brlMq z!+Of?7*$Jh;C0#QABa_h@z~S5HOK=8(?hC+t+#*lJT$0^iC9!Rk=4(FJ(a5SgkZty zJ4XyEawoT8^79+DlQAJVqmJsC#tkq*-M4xFMV>$3MXtqMwIU-ygwaW8qgs`}U4R$Z zg)(6sP}XS@?7Eph+qETp^aviMTu_Rf)H3{q9ok*Y9kY~$_0T9ZOD80tKkFQrmLxTS zPP{#n$N$Ti5e06X@2B0-dsjafoSPj>x z);j_mEMCoU=hs{Ez`OgEffrq6X@RO0xw(L>?%1u*yt&}eG_hCITfZ5tezJ;@yRis< zcZ_Sx!oS-ysNy~J*BU;u)ey(swF>%lm=%tt0-%%4o zwBMz-CG&edAQJhxrog4_LCgLdy2akt87zCa{`4GvW7zKl z?;`pw3Y6fOHBR3%(+ui@FE1#NDx}XF1z{|+NV9fD?=R<%MxFL5-qVXo72_>;p43`3 z>^p%j^<+U=BRP@oGo^#;^2|5m7H)nd=q&GL=~Svay~tk}LK|f=jgDhB$tvA0esB<^ z+Mgi%0;W<1UX=+oQ7dCjcbJ8%Ol%z$satB-m!7j9Y!Hz|A`67im;3#N#Ns_$siaoG4O;Ja%)X1@vlq@hwQ`;0oEzNwaT7g($j%+^D3N(S06-YPD2a?)oT7brf zXpCtX;iD39~Aga zbLh>uU^ds3*rnsg4#$b&UmpmPDb+h9%|YaFKMO&RVkygCQaNlXKCq!E>-4)H@0Cbe zx4WW>Bj%@V!zlsm=jAH$7>Ke-p=MP=qGMFuZeV*u*_(d+S(Oeif4ky2uPyZy1pP%_ zO`5#yfI5}3z1@4Fdx85R-77DC8+MKW;|o5vYc zuPrx!VI3oFq|r>fJ5v|Bf<)&xjx1n4|Iu2uUGUev0vT zp#+FjaM;I=8dxRz{6Z4MC2wl=a?WZ7k1{kFn<7&XwYV`zoc-10k!@}1=ggYkU;2MttXVRF<`Oav?lMFiBo0CcA$io=Dbt-4OBI(W;(2 zF&QJodgCI_9y`Bo+%uCLcJIXBwDSV{#WfQ9(qtC-L$#tP2N>S z#a!$DktOn*Stx#GK;F2#UyV%x^J2_pPK)6neEy6Po zZH;WHiQ?bWNm`zR%^-Cg&aE_XaU$P-K%B+DdHf4QuBextR=vYY<`cdzZlCPo?BgWK zu8d;}2(Ac|iD#uKM69dmgavKHf}9k3mPB9w=aEp%nIUEmPdY;k%)fYYKQ_J^4&D4Q zFSia?8t=>%7Y_%QJ-wH>YC;6gJ-ypm?lJo!sw`!tRJ)RoGM2ITGi}V-*f-m%JFf_{ zW7jonU)^nC?2VG_ab~?k$GYyi+8t3_azt~;q-u2|_tKIqF{aCr|AI*}dt`BC@S`zb zBU0V%GCVN%{`Qd5W9Y5_P>z!+v}N>{O@A*D4I~acDRsW|>c(Iw@^FCks;%VQ zUsuX~6I43{Q2csLu>lG%*3c@n(s8_f;g&tJJP*jmw0T{9nyQ5uVeX46wGFs#*QXkX z1|r3Uoxz@z&qP6QdlSiONWz{$d}lSe{kq- zy=EefQ^Xj(U-bMXYxXz-H>C-=Pp}mJXJY`=81!j+x>7LXE?G+6&667$fk2fBH5Wms z@Z-PPexue44`4^)x0U4r4TX<0Uk^W?HO48b?f|E(O-=K0p}nk7XfA~Xf<}9B^?8=BsD0lw;l?yi`f2CxTU4sAe5ekm3AjVFGX%&( zL;wRdrBX|nSmUb~bkbyc3FsvENvhN&Cv|}#^3k*CJ5))-vAu$y|1cx!!!tU;9AdDS zF;D17x1-t}nEW6<=V7%k6TFslI!uC<-3qg?TwlTp7J^5CA-ew7>U$!22q%V5>3XYYH@o=P?PzP@zydR|#?;o9_bL(IH z$c6NyIgCu$D8P-A+Ld`{$)(H5DMkDM4cz*N9I#8w@lH;-;7Rma z83*#rWFvnqc!ZFMlCYwpq89jFZ;UCRwyu7-RTQirqasnV){-T^@1gCprVaCIU2CiK zf|O{`_nmM_mO-pB#}Os?SqL?jy4R6U%t)9 zGt&SUE$xWQ_sYO2}SpxlprzSZpab-*LUgX#NjH?;G<8huaa{124KpXUGo literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img2.png b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img2.png new file mode 100644 index 0000000000000000000000000000000000000000..84f533025e9c2c01733930dce723e8484befbc2f GIT binary patch literal 280710 zcmZ^~1yEeU(l!hsKyX5EcY?dS1&8477Tnz-xO;F27TkinI~#0qcU>G7{m899_a<-E zKUG_tJ+)`1=XB50-A^~+pXDWx;Bnz0ARv&WB*l~Fuh`F++PDn}u{e39;+sk3MX?Pn{$weSI2e)5kj!MpCj# z^U>+^-_fi{)%sSU6${8DLr+>8$1~T6Zf%X-0>mSBo~~+6m#NJs#wue&QC7VL2A%gm zTSG=U>;vmR5b&ZWCVuF4AQ1a!Gz5ddDr$4pwkMy|e4Am`if%JbS6UbBKX!FN`V5ao zq9MZAV|dSzyEv0 z+iY_o2lxkp9;|7~HWL*v(e|gB$(p$rAo{k3SNO#L{_T5k(FGq0;PM@e?uBFR!E!Ty zKu-_f>%8%jC#T@>zXS;zY7`=kCq=cVW*DPFg%3I7BeI$;QvGLtu84q3xZpQ$qafFA z=TrEmv}9CW=xa1GO<00K4_q;#OxVP{ys~Tw!ORmJa_dJtDSyu&=Izn}GK?ZIsOZ0K z%>D$N8Tj$BYIi+B9)G^<0|j*Rb_kTpfHK|dmUaruJS>KBFD{&hCXnr1$}cV~?~|_b z6hM0D=s(Huk>AHseWax|9-b*?+A_RnShe3QZ0`t+?pyXusZKG!E zWx}foWCLZXv-Ycx1O8h^Z-i5T1x-db<|;U%^6~|lv+v5oEV zLxqTiR9)EX^gJ*I?^;dD?Mjn~ikw$yN)tm8kPX}H(E&W<%5bC%ck%|Q%a=>*OZPgY ze~s2i&_fdx>{a_R_Vl4teZ7ODcWeeFVs1N8??7qI@LP`LKF^Y?Ta!0|cLUlFXR$SY^g6t8=(H!^yqhG@WIzU?MelR!1ukUI4VI!`v zexNgZ{JZlOZrem+GdPd-qncMD0X7*5?jsjQ^$j@pVjbk9j9@bNbcQYKHJp(C@(GKU=4AA+Lq9TcXWIQ zyE&uM4!COl9(7cmvQWoixF4*YQ-4pVh1J_c=)?Od45x9yDOoisKB`}`$eV7x2{Lei zEW5U1j8>A3b8OiCKKKf4Msj>6Q)*I$o7+iVo;eJ{a!!lW#P{1N_qF$BK0uJ_v5<#ajR zoi7CRu*5g)zFe%lH&L!vXI5rR;4)KSHYfOW#p<3mGLbT{h*_v=i2OJ#j^84|+46I= z2cm}k(jA;ku^ojwD&Vv9{)$%f=B94>IyRMQA?4eLyq|Q!RZxCiC5Z#SI+?=s!4yPo zcSNUNj>v@_hBN(*j;q9}+Y+2kr#X9ZQRo|pnLWLGNIexE8D$ggkr=?d?e!1b*F%$6 zHhY_@g9s{Cc$ngRv)vAYLBn{qyH#fTQ>PQLg{jpONI@$-h(&ExFj zaV5oFv+e@uO8@-w>2hcd^!a{`Hx@;}E8eZ_*2tP|WIssyZtmoA7zz5!5)Dz%B=#yZ zwS5c^k7URB%N0Z8jSi=>a$MuM6XsvO49u`-Nt`BKxQ`*ZF=$EbB;UFGF*Sx@D7kL$ zvR59GP%m-Q?ExiM-xA8##^qkbcpqboGWp_gP`*$}jC6rKu|y3thi2^keX`sJx9sQ7 zpUHb?S_D?wcO@T#B(4js-VsanzziRItO6* zof~dvawrjqlqBTa%C9*im?}m5y+-}L*t`?*NJ%rJKzfuN%0JZ2c>X+H$N*PRjG+8be`q|gr z_0QHgM{v9(1>)mjr>L6zRiz@a2>J|zbh`HHUfN*|VH1~+KWTe2Oc$+@{H2L_(03aR zQMB)Rh=O`c)vWK0g~+>;NOYTb@-buy7L&W{%g!umGITPG@x40@t*yGTB`NWgavE1X zi)V*V>Hy#FPwcsV&er(fOAn2+uXchpmcDO{y|vSKBAjD99*M1dfs()wT|k(-sB7)W z-7A^1KPez}42jB(gZ7T+PnwR|hbZ{w1~AoObwZj{RBqnr@fpT@ft;*h3+&z?1b=D8H})@5i%`TAG=cKV#-)GUH$51NSgE+i_P0Q8l@P z^j{ux?l>f?jQvK56!yc71VqgTt=l*#-AAx5(O5xJ?Q+N zRe|`9Y*{_MT;7tHoW3gJ-FM-LOhS*w+4s2-pPL98hO4H5*)jnIe`6oeJlL`1 zV@6}JM?XiE9Qk0h;s<)l`3Fe`fjp`Z7QL49sM9UQ&|V$E0&C|$kfdvVPOyxHWAZp!?|b>Az`zh#?VG!t zrrq%VZH#Rr$Enjux1aD`-&NOWzq*Ms z_z31`&{a1yBI4Bvf4_c^=CoNnB#&lRQB&J!ImH}&bJmsuHQe$E92 zI%BSnxo#To5kAbw!NoW34z#03M!oHsQAtI!_jV-Hq}PE9AvX!t&YHubX0n|=GV;Y1 z{M^IciLdZJg(CJb4OXy%gqI3-O{>S2p5q)EW$lhR%TcsEi#e-RHsK3fg!$3w@|~BA zb&)FV_+o1czaP7x-%tNKJFe6xCq#aUq`2E{qjFG?ltFDz8@NwGw%MLb6AERgD;|NX z0HF)_`L=UsTOv@U{_LT;eh=+$)S+Lp90eLMbsg8Dd%66KgS&3o+qLgsT{+Nwq9{3p zhxv5bWCU7urqK?NQz>Lb#5PMUuByW0#YU_)JK?z(^K8JyZ|HN;1y<@d${G9IZAj3^ z#7h_c_5s!>W4nV$C8{}R&}0XDS6g9Eyk=Sr@@BGxU7rMW)kJ-{`a+stwkk^VqG;z+ z6K`SWyJTLprOA1v57NQqMqKS-aoW%C6~^u|9Z{CQFlPnstZ#E5klWOTl#-*4<*Ucy ztI;if1p%kHIYSo>I4N^n(6Z(_CpvTPAacGJ&6)XPkqV2d(q2ZGN&JvzYu^i5eD}f6 z2`0aeL|~6d=cNyRq#}=0QLfRI#E4RIKK#S0BZs=khY(hh>yh+F2YWE&JYDO=*e%zx zqvkrbeuP`{v_F^`+sNi!HNOLsgU)yj@LONLkZXP>WEm^`)pFP2VI-h>La+$rh_pmSc&YkbuPT*?o;h<6((`%k+z<<}qFVagLXOQ=#D=MUPMVR94HiJtt+>3)o> z#5Y_wW44Yjwf)TnNJ9BpVI)B0*?+^ED{5*{PSEj|^l&dJqHxr65%=uHrl}Z3vV!_r zCsMFv&5?%qoFc)) z6kv*`l~GuqA^S8%wHBPt?tOLPcQ=?1`VjUtTr;Qq_@lnSEqaa|PZ0mI)~e?*{W}lx zv#?J(0?iMWmG>79k7vK$Uoj$oa3oQ?haNRX9$+5N&Sz2?)%NQQJ~hu8+vr_P#26=S zC`v!J)_B=?pyS>C+*0n9g90pO>DJk@cu30$pF3V|jev#JSKdUFZOf&PTvxdoCTp_>jCQJTCl$UU}oq*`b?m(K4>Wg#*8AtbsZW< zPGmy@@oh-_px2gw7cJJ2gvq}i^v27j%qGSt>%W`^>~bM{Xr>6MC8^Xki%Z@a8qMw$ zB$QmW#KnltwL##u=_IJ))x?MvFQc+kRxQ-at=iFETHXuj04d1|ENQjfofuXS-pp|E zcGQ%_#JaqDJJy=uvd57P1+lLt# zj8@PoQ^&gV3A3S*P;rJHAJgFv_TTbbS{kdUswpZfi+;pkE;Rm^T#DIx5HQ;eFm9xd zldU69Ak-V_z4e(I=S7OyT8GgIglz1fInUtzKSa*E4v?J>((pJmE2O?v2h#tUQr*15 z#*3KxZ?Gb*rAL?i`%Hxm$>$3x{pFg)#9Y&5iQ)eB*J36Tb(pf^9%-ABf7#Ld_Y2gS zEBuiE+ydVy{huXy=;5J7(J>o~jf)w$Vis1W|CwXK zbg?a@pgslqQn23zKVs31oR${BSPNl+&H&fOFN3`;sHM0;qZ&1KaG~0kK0EdM3t>>> zS;s%S=+Irnm#oyPPZ)`S_q^E;-fH2hObp*d1ZqWDJoAtH(vnKmEl^p}4=|^-%_^zW zrThAJZm?FH9H2Q~UhFS7k5*>Z{uM5U4cSibrc@jOD+?uJ^|&fUiLv{TnhLfWoaGE! z_3yp7#tM$zn0hrE~#!fsPP#`5(1}SN`^a&$R zBht95Q2<+%9G4{plZ_*Ga00q&$D=|fT#f&|;y`1e_Xt=ll zEjmi&_X`EoHf9p{{0sNe(%u=*yAQ4AoBvo!tMC$()S3w8m%5JrmLf&*f(kneEp1xj3k_@@^x~fV7#VtGPo93?sU6K>i z)QUQ6G#IDM4&lcei6hHUG^Z7fpC&>Y6MUf?1Y+3SI~1Nu)7JtU>i>~tsdPM`!0xF< zp!@*Pkz>(sId!7VX|=S-<^RC($)UmF%+kiJVytC{f>qeYthheymg?`vF9>RMF%k}> zJIC(hffi_8it5~7Wa})u>_i<5bW_zbJib`b10ALwyajc!EV{WuT2MN2;OI|o)Fm18 z+OYi-5)uf`(Epy0v^8WJh;Rs$lx?(Sch}?jdg&9H!<~|$FJ!Cd>zQMB?j5-YzI*0; z8NFoDPur_ax!?QA(LPX=14rVM!5w<#+nN=yoj@VebltGj{QA+XHId4wO4YTS*=P2C zxg$DTeIT-bM3G;=Bg(9nTe52dW9EN~eLW>ue;tzr!$m!kgW@vc?m&<*aa>7~Y-rL0 z5mU_Vsuzks)IhW`uw1=^pfEn8q$=*~eM&`itZ8~G;>Hf(j;ZR1G_oo3IPU3;{) zZ8Nlg&eO4Zrqhk))xoqmEMM?3o!`UK8khT8Y&X66qp?J>8;|zVP4`5edX*lp8JaJ> zRcZyvd^=}wHv#K*rQ?mhS)G9?H^fuxj?)n%%KSL%UvX*tN&WA)A3eso26bdy5!+9N z4nLVE3d`fXFZ2=p&5<&ZfiFVm;o?kDSs^*yH5t~US%d=fz>`IY}E1LBvQ8lwGOWs*f~>oCa_d@=q)_6@BOX}%bYx}*j2rtJ&EZ6+%VAl~@s#~T+pP{`jdxdr zQuZ5!5FO^0{>weqApRx4Jg*JB;L`yB;+wz|=3>3oU_pM2*+>$#iK!{%*$8db+4hH* z1?xU+rx!DAnt6cYO5ow#GWINo~j5Jv{%{xa+}=(x|E7Byb3H{pFOIpN&rK^kwryhCPC27u8lE_8R4+G>bhwPM z)%ASe^%s$NUp)(R_?%Hc?9$YwJvr`^FS7%Fy~+FyT7y``~fVkF`HvLsqBn zRq>puEYBSbH4wA2>;%|JkYU)#3nlDv(GI`1&a9on9u|*1ze@1oZrA5(Ut1c&Lur>R zv5~N-K;dP^{K=kk*@ZE8W_0!0_o|GDwfa}B!+|#t)3m;w?-p;HvlEQOweyoYYB}(( zN8;|NQ=RFeoVLkc2>F~n63jx?dE%FEkfHgQG&u2e^!Be8bD{f-4uH*P?VbpzXP=c5 z;QVQQT>~r#;u0iK;mI&%tUOX$?tp@VMK-x9PfcSwH-OOdI0&X(8lP$8zQ)zcZs?Oq zxR|!mw9PxS4?S0wdX&XevtqH5)Ix}<&=;+f_p}+l3W1KY(Gp)sSD_InMm0E`DbaB{ zdD%KFtF(5N*VM$B({HOuy4i`HlmDKGFl>r)UiQ+M zKK=4215@sk-3od{v7;{|p4ozsf9QWnR=GC%yUafN+eNnm^W7i%U>t>I zUlf3EQ$+f*FaFI|j?>7G9(CpkiQH=Yv99aD@v~Kl9tRvOqOP&r|NXZ z9R!C@)gi!R9pHAcSj%FB*+U?~1Rk5_Zj!(RHlpee(Rbd#LA3i_Q9Uzpj4W~z?N;L0 z)i$B?APdISJ%flnO~KagTY9#>kNh1U6@)i5e4W6G)=gHpx86D-63FdTnLPk`e5pKy z1~WKTP-iUFnznewbc~|1-=IY$MaPAq?^9xE05ZN;{|^4jLSe`AB(Q!@)^EVS1_n`M z`b@SWyx%C(VE_l)_#1}}?!$2%3g?%$PEJ_zv87X66EK4NbJ{LdHsi zypuXdAheN=)wE+b=j7KY=x|TiOYM0Eg{ev?Rs1Gr3heY_pC>$fY%ga{u(^#Xm}Q3ZrK=F0)RyTLkR97iQLY5hk$&Q86tgVg1| z+(mWplFPJ*`_Q%QiT3D-$LJ9EGfm~ZV2%k(My62p&I8gryK^_+)C={CK|$Nq+F4Y3 z9MP(a&~Lu#Uh*%`?lAQzg~syQ6RnLeY32!o=SPCE-A`w>MJqQbFi2cE@zH}TJs$ai z2p(q&x3%`$nmSLBz9y>bXbkYxA;c%KcSwGGC0N2+>!*DQKxo?0=4UL}#4CCjNl&Dg zEFG7Lz7`4%=B~oV_MexOwb2c1`s-Gl`u__L;#W$>>9vo5LSt-n{f!|pSr_W*oPT-& zcrI7HKpP%}ZmS;VPtEa?Um0{;AW)G*4}Rr|CfZohLqkK4FMV5Vfhoph)Et>gCZW`G zM}oyvV|^((SoQ*xA%dSsel}v_osMeG=?x8iG{8rr&0ep%1(?SYy=2$5R_--x+wR@$ z-K<}L>WhuQd*vWcc=cz)cbafHU${b%2eSw31xE(UJ=3(GFCbVyq3lL=NC~un>>i7) z-BO)RW0So3iKJ!fuWU>4=*f|p!gV%FU;YfI#j>`^-;1xT+ zX=kC%lv?+=Qv!+FwV}c0>tIuSJ_`y3zr}d6Dw@0%r61H~6sq+}djiO;=Lz{ZK#F&| zF|&7FELWxicZzmFR7iQ&XjuX_nYzg$}ontOsp)4M?h6@9vh%nzAj#*2s%h#O?| z3N@_>N1v+)TpK5;K8Qs+X<5MI5qD}gUhVpy!+iT3vB_4@`dzd;; zrf@!*4gW65ymfQbHKqVeJp1l_cEQ4f?}KdYg|Xp?J&5d`@*t=uQET^($pp(Sm#H%^ z$1d^~4|P>IS{3hjKc&L9xEDN*@t)9YTk8306!s&o3*w90z$BvU?&TxZd*rrbgZoC5 zp0CE(Pks9Cx1(>#c-eLAXtf3H!hGU6xAO16a1XzD@xJK4?bCoV(9xM&p|J8%>*r6Y zs_I^Jrv0)eBG@jur2h=AHwfIWr_Ig%5f={|y&*W~H?Y_z zjBY-`N;yrK%vHAP87bEuyS3lWlVHVTKTo3Tew_@Rq(IZ7B}%L%X{d+n!tRjAkTz3f zqK~}Kk~lXvx9*}f7Mb%E$8XXQ@dWMdqT;(5lM%vb<++$yfQktC8bqm!oTkY+B~oNa ziK&yC_n1dcIBOr!g765#)M) zfN(yU3dUh2v=hixmvNyHljjmyBcnvU_ft4`G@vho!3V7N3#Ui~k(X_{v*Ufdy*yC+ ze0zjikZXlJd@CIedtV0{vVOk0N3*;Kp53Ps;PBEB*-d^bEyc7ync>7+ss0+#gfb~h zos}rA*O2Ea%1$`$w_xoJ&A8&k8{6Is!N46j6$DLPbD$YBZ@YA=ti1lOj5 zaYU0?-t_XOXnD=vSFcseJN*`O1^96Cj<>;-)}Lg`ici^>KZd9>r}xA3%LtL(JJu8Y zoQOV{Z158fpR%8A9KTgY1J8{W=TAqy2orCpn#^wWw+lA+Zlik_OM14ob{8A@knWq{ z+w`o~)_S%z`m9!p&X*@5Eq>Q=cJ)RJz5iJPslJ4b-NIrME^1d50yLfomNBBZxVUT@ zOK@8opFd{fT)Ac+|{FVaKYZz!-4T@ABUGgV6zF>VH1vM4WL5c>1(zyOrCsH zCPmj2i7?SoX4r-tR3deMP@+Y&c6kn&q0xUFdb)RjqV_o(5w%*XrO;bR6_5JX35UQX zeT?2mFcJiI9{>v0)ko*>KiNj*<(ZZlAEh_hGRGZH6*?sXL}5_OhSs|gy-wIiMn)jM z@mvq$Gr3yVS+$zcDB@jisrFgN9p{Qxf58vM&>vb+*t7p-8Z^uvV$dM-od!9c^dieq zEu}D57gbW}#uTI575b}O7~$3p<&i?w#U$)39f&PE2OZ|!ZarD?x)4Ll`QVjZ#a2o9 zl({<<%?oD+MZM1gS*runp7K3#W%*W9aH4ZV>1M2IeP`59F7kaWbK0jSBoHGoRl$Y`G(4!iUqob9Fgm$GM^x z?xorg8>-go*-N8K_;;!mMRag0hbTBvR%eNXyi~O|mY3|9RC-rzyHi!(EE4lP|I82Y-;7>rHTN&bBQE- zBUeTW6#x+&9DF^fwIq*H@3C#D&6R+BU2crq5FOM_wgFDN7Y&tc{kR~iu$S%>f@Z;D zmSB1vZ9tEiQ+rFe9uM8leROe&ld0Z+G^g8dr6KDoz^zi-!V+$y!eOQh`}j7y-XMLD z%>^U=TEd$sD$zsY;Awnu+O}KUu3elZ+2}O0If|c{z<(@@70cg%xJ%$Qgy}!B`JUc{ zlz_puj#&fP4sUyqHjmlQf+l}w(n?N)HlNAQ<*bo;aLYel%{r*~)06VH-3Hr95l++p zfG5UbQl{xhAa&I|^EId{x(z_QeHnXV4Pm}s_uXEA=b(r@SfLQ~K*q-#e5Gf{Ns#T7gFM=oK z_f-BhWuSlUYg64$uf4LI(^x%+rhUP}=(7;OfA31XX=5@m#;vk*B5K$zgp8{(A$a=2 z=%cU_OE`&cT8NzG)kuZ@?nA99ou1-w6G=r5)~OFMS=LeW?FJ{8`4r(c#Yi}ob78Jk z(uF37$xjaQ#HuX(W?y)o0=I z8|r7fX_+0XjS9U+kPZ5njUGmG#pC#r+5Lg!lvX6j9GG}D{aQKEvLewD%WDmB6VwtO zCS;gJpLCe9cYNjx!m-=2AMzK;OjojWYMo{uRBBw#2WfjB4m8|_zdiq|@28M4n_BZ! z2Hq2WDctU>xf+X5G(AI%WaJST^5sc+4yC9E13?(kw9D@ITL#Hz1Q}u3dc>b{Z@@&Ftl(f@#qR@$q)IQB4gx& zZ*_sqef=h;bqo2~d(ya%Y-QnPTc6qVGdV#qG_O-vx30(Ih6pl|UK}z$SrB-#N`C#6 z_jJ=uLDf(W`HAW!W=-LA@?+fF#8xc9j?Fg_r#@dFsysqy5H@IV0QsqOR6P!kEx1I&{`uO4Axi058G4f_ia%bcm{i>XOIC1|^ z(vuGxGz^1p-{Rj%!60X9xoJgitVxB+$n6jHa)IC}|mnIEaJak>QBLIJ1?|DX+h zx{)r5;tdbCQ!rl)2CO#_jRvdd^y_i+vJTkTx`TAIzI=MDVD-3gSiYL;Jnz5QdF3yd zU%$3`N&kiQyip||8LgBh2F$ZFyPsjB1|6m_!XZRMPKKA8oeqE%+8MSTjyuvJ$OI#q zu>xN_i8*qX7{FA`g6&C(i-3&~8GVCokl%QKIP&v=`kmjL_gK7o&fuA3peo~9FFot7 z;-SR8*O+D13cm+(%*W^8k~3=c7TQZGJCn9_#Hcm0&-s6d#-_`)0-PsJfqzxChRfsrid3$15QPb-<48 zW)eA5RpxR8beNX&+iAP>6%IuvaJ_Z@U#cnQ`i%oNK|%1G%Xm;uQXSf;8m8rTzdw`P z37gBFxZA+~D9B_6I)TXNOfQ3~qm@lXq-ZYgCuWo}Q(DAmi#=KwGX%^QHvTlKB%#aL zHpfaGL(*IHe_CvZIHfZlRJE+|7UL40=ORpHnw|li=UYTC-&{`?Z7wyA=BkRz%js5p zuQPf$enzp!Kmm=~LsS_d%RyUQ@}f&>ME1D#={EVV``raT+tp?ytjyO+;qU zP1JOow!W<+eC;SBVDkE3cJuyyyl&YmB@B8n82%ccRw_sbY^2k@^4R?bd1QRF*kUFk zJAL7=e*))PGY~*NU?NRyvx02Bi@3#=w#Tvlpuwptci`8X{Z~dWCbnHR-;q-B>tht# z0n@~c8*6`+m712ioCP9gFXs=`T9@k*%iS~|A{LAUHa3fXjYvC5b*VSN`!u&R8+W+B z&HyRtlG%e+gI1)&6l8vl08ry>SOu0tniA6LN91e!_~G;s`JB?X2j|GULvyR~pUz=R9GsI(0m_I>gW)P zc{hVl+Kh&XbyQ-r<+DEIp#cZVN7f_Rd!Ul0ro5}#&1rhmKYTvJ+^$fqdX)g3c0*F0 z=&dxYVoSVj^Q%QRK#jFOlW9)=tWA}I_3(U@Q*G@mqd0v|G85;{V@*xpS7|m&koEl z-EQ&!qdwPf!d{5W)LSulf`-LWe{!M=m)pZvK7Q)D^#4Bm>EDu2*fCqc|5sCsA8(H7 zhK#ZRH$i)^Z7%3?)Bag@f$){6Wd-mhay#u>&l!+?iZZyrI?(q}`Yq|j$_`Il2411| zZMGPx;+R>qo5oZ+zoDM2mJZnxII_iTa?bscUZ zfx~3a1S}dQ@>#Rj>;7z_`&Y-R18=nzl6uF3zYE(br@H_IxG|as7^{eup4KwmV)@o>#}gAg=&GR=b`*N~%?*uY7GSvQ4xcGWaOQ^sd{W`dNdiJ^#5)$iQk;aX{m7 z-kI>Ryl6upUWDg%A5hKwP;DNIzkL66@{z%H7@_3pcae3-O^PO&6m1-`OUwOwRviu| z=(zoM6@pH;|SxE`8WY7_t%jUP(gSayObvB9UbxwHdwWVlw^N2H;WD>7& zVfXylMLFi` zI1oHv_8~etI!YCxX&fo*>IOUDGr_D~y59c$19&C~iz*}wO}H%R)> zggsp-gqzcDDNbL8A=0CgwEAgdJG#DaI>-uC5P?-{N@!B?VXGZD=yWHFmY=w9_6beA zRK2;~II0AEsqotBz}>(2(m8!xhE92+&IhB&IhRNpcFD{HZV_X5+_uJ^WU%K?p;zyY zWsJO#$nnl-)vJH@6pnV?aN(_P)R^zC$8e`BhoWx)QxxBi8T01o{5~oiTVR)s@1~o5 z;oCbq=f8SUHo1`TIBmNisIpy2y-u644(iGA^vV9KRf#1P5*$PrxM11DGiGedA92eK zDHYnNwhADTZ~84EG;kkIV43v8flCa?usZL+xHS!$`)=VfEUn37v>IGPdpgrtI;;M*bL%of_ z@!9lvJWdMQ&q@dQy?{B31kZ7j?^&5%aUq_d=a#QrPF*_Xikbv7a)XC~>iI16@$4%R znmttCmc!<`yAOLI@V_nw>}e$$Twv2sX=3R$7i~QTto6Q;-KDC<;VGh85c<9#6=Lro zcq33t)EnB~W%J@5I@gIUQRPb?t=?NIjz{9g)o%z@pSHq5AYUY%T9rA8(`+ZF%gOFeu+8VoxD5ou{N;0o4oC?#2`KvqP)lPoIkS zI6DHlL=<-S(AtY{D-0gL=M_*P!p5L~sxT>`purhQM!+LBR(;^0ZEYBTk*W*~Uj4M& z(U|#pFlIYgDTz^6l%B+~7B_-7aa3hGg^&eZ##c=dLk<^*!x-Lbw0yge$zdm%|7X?*cT%mfp&=I9BRy7PJclo^co1;1I< z-&b6w&Z(|2+bOlQ=S72~iTdqa7f7P=td*%JGx5vo@qOr#+We`EdYh$r~2NzCnYe}$>wy&W&nbUuIw*>p7}!+kMCw&YGL%0=~tR$zS? zRuH0VoT0!8V2JOrC~!PWR^0zw!=X4)>QU$Z0=D$-660Yq=sCxqBAfXY&mL#8v!rk- z?*aeepf5M5*#7QYkF%<=8Y6P%uT%;1_AZ1-So?du2m8kY_-v1B55n|BWYR2q27=om z8+RPdc5L3FFpnzRg3+e#pQEy4WqRX}oU0}t;xs;5hHww^0twIY93Ak#rMFO2BOjs4 z@gD1VAQECp1Iri>0AKua*bQu29)j`SvDlpsia9#Et$qD3T9=KzvNW@cMo|!BS-h!( zgkDhq!I#WJnbbF*m6UqcyMxmBJXl_OjPP+1}umncv zU(UG>i>a8HN;_Q`U(aeSt;U-9$sxvTzypJXBdw_TFm}Hl4XCkRY>HP91b;hB$K`Q3 zncTk1<#(NU$@%|@&0YfsxX-Gp{T~$gggbqo?yjr+uBy2Oxi{mIas)qpq*Gt?dj`<&>}OaUHA{c7WzUN2~3;iBt-!;mXS{ zmYwe5eotN`=R%M^1NeaOhX#d4pWpp_@w!8wKWoSe8=jwL|C#dDdurP((ENt43%Tzp z6@PGy7pQKtaCrR9;=e!q&jlNn=9vkw@GC^R(R1EFIH#f8Mh4+s(DK5HeBzMJye|N2(a|H{Pd=J{DtNSY});m2)# zO8VIG?Fv9vXEU7_aA{VE;fC}ohGTLTk59-{0A#;zh-Cb$Cv4wl9_bceP!U_VmP6Et}1iO>=a2_e{vj8b45$>LcJDNnDvsap zM#)Gsr9Z|-n@J8!v10LajS>c5dm9rXl!AhSh{))N)gR0$7HDqUs40b8lYbtT#%vxZ14AT3WI^KjW(Z z2@{nPbQ+O|bJT3Vj}DnS<1|OTUcOmR)2nCjI4x!Upvmum&ZrPc(b{FDv~YeR;eB@! z5LneLdDFq&#&MzRE5#Do6!cqt_}t!Mmh6@ru4zmcbge7$3%}sWDJXP{M}EumdlckW zY7=wY3Znt8O2tD%OtBk@!>b)v@%iF~HMs2t=q104_OkFzmjP!{D0GL9H9C^?Z;O@I zzx?7G2kHCub9QF+Y4$DgpsCeynXyt4h%e{H>6ja7;?>36A)J~gG;MRYxU{4y++eb6 z{WeHy0gDpqBw9SBTq`JP=!E2WhiKYAoj~zDG-pG=uV3Gah``?%{@N*D@rj6=(z~+K zK3F!cIE0xlJtYEQr6gkvmTKm(*gP{@^QVV|Bdz`7VLO?R9VFbB+{aM$w$ap-&}6rQ zpT;UT8I4zwZG*G<6_N3PY$Wu+)D|6hYT{@%p22rrikH8~Nj38nYrYIcgQ_;0th z^cX;M$D@#F{{=Qffv096cikp_E;YF!Ss|2vz?>J#!gEmQ6W?bzP&pR|V_L5{hHWq$1lWTgHerwlJeqcvHoX7#CPX|DJ-4h|8{N` zY*X74!&SSV8|{u~3{EQu%BiTR1k7a-^x~>b;Q5a~B!-n6I2!x-H5@CLywVCZFzS4q z9WKj(rgVhUBssdQ3)kR}0t<@iwp#)l2Wo|Y48f(uXY?T$ z&Dz(>TNGo-R7)&p3NzcRvFcSoEtvp;Rn>I?W=p4=l~cJ+Ms)cI~-I7HqrZMdTN+=PrM>Z(H4 z(-2?Kntv$V`NLVBQ6d8({yl0V@PZ%YVz(JwY2(=8Y}cRRH2A*8hrE&Bis{l%@S>B#609oO&koH&rpGWMo8o#A)3H0K~dyC))%K<2Msfk@&$Me6j(*+bmZgQLAcct@`cunU>N zhfYFyNXLQ38>_AH`KsIPPQwBD-7)@Q5_fn+eAe`>`kdD5W z3+NXgmv;`kDtet1Zyy$9>e|0B8N78MwInz-6DzKL z>tW_-e;~GcZ%v~fHez6^@6KXn<63-8IT6Z`G zxA}pbI?H4Rl>KLmaj6=LMYORozM0YsykOrR@(}*?9*G=2StuebbJv%Fh5{`_oRw6b zRn5MNtbvCd!NE}svy0OTQ^`jML`DG5L}y5+Tr5wANgq+aX1})K?^Zn>r|#}<=huJD zZyNn?z$Effg&2;T=#7V$SDoz&kLT^_`0+v=0=jg}lP8MdPMd`)CwA!QZ~}ov7bWJi zZ{XN*rljrlgqT^M3HlG@%Q4@ZLZM(v7kGByjI<_zO1~e?doZ-^UP!bJGCfq8rjnD< z?}Gh-hhL;(`Kc{^xqn??{0#C7j_|LV)i$OrzrNKQznN=iDz?KSCvz2!@|_90NJ8`J zq7PiXw0p`AkT=C5!2|5)qxr7AjXZKRJ%>Rx31;W`bE7cin$PbL?au%g zQ(wUahlhtB&({RKA_W##ln!4J6qzm3=K48e&xq%At~7D$Rvnm&GI(ALkAT$ihRdBV z=zSv$hJg;PMsuA8(o&(1M@iYsxXnt)#R>Bq1%P{#vzBT)62``I`)fQzgAnPWYS7o{ zW2xq`yu=2G!-mo~R1Ho{<8SAvIlQmoQEpELTFUv0Mo179 z%MZ*=nR?VUnaYD-)rhYUzg+|bgUlH*K2Z?enT`riRg}KXOqJ1kEj?#RQT8TFuzi8V zPL0B1XK)u_R6F%Zf`P%L!i+Y$lX&>@yl%g0tB8|x6)o#|ql>(!&(q+~RC?ci5&a;E zhy7_?ykEoO0sQpQy}n%jm#Lo!47X-^xT)?z^~~ek(u$2i3lpT5B$i(0@zSO5YR*=Cmj z*j#H-%RBvDdIyS&rCJ}<#V|`>-~6s#SzLms~tkraOvhZa$tX zO2~tYQr^wn*FC2qw9ysQRJAm;VH53m*?4oF;Xm8U7tJ@7+Znn~VNO@G=Oy=?2Xd}y zJU;*nNb$9Xi_9}8*TxlaCYbFlOG+o7+-xa_U1eVn8~LPV54t1 z9<;TrGRysjI`AKN;sN~Bn}exhQc_ZJcHiA)(VC)kP}{<^gC1~?uMIdACn4_oRyxUs zSM!TQ83Iv@xX=mN0+&GNL@11^&X2-2qDaFNOtskw^HmFLr3yQ{g&lsPE@9>Ejc-bmu@JQXH}S`dRA0FT;lg43s^l8Xv?x& zgSN@{Gm7+y=K?7%S9heYxUkw8_>=H7{XPTFnIb7jHS64w%dR-S22Ndy6cV1IHh1Zg zhO)c-65NF?gfd6duV&)+Ze*JQoLmG9eHKlnKC+n9aUrc`3Mc|_7NRv=<*kaW4w4^ua z6fS*qt49L)5(BM7&Z%!{D-tulG#`)(kw>Up(dCOG$Pps)S8omwa31Gcyx$dOv;_ov zmFT8S&fJi&UvyR!aj)SOFr{>)%vfl+E7r-s-}kg2L*`fO+{~5!w!aJ92RZS^W3?ET zHJ%R@HWb>*A8eBo4E2S&9&Q6+2fLyNk8OtX;xd_^QnwS#2W$#EvTtQ7TpTpQ`Z#11 zn}=?}w_R^Zt_Vy{N+Q>Nb}p8B*LdSmm*1J^%@JQZtBdXhU;pko^So<}g3xf<`XpjM z(y(UDk=KFmMFXf&u|&bzE_5Y-816g?Z7dm&=ZCFjYv~*v!?ozP|o5$)BCs7?SiyJuzn< zf*x_W3;;l+M0C1A{brO=B?6=x0eQTdRStceolTC=WqXo}j;%^#=fP2UxZ>W^{f<+` z;X>l}zQ{mX8OH~Gx(05&_Jx`7Am3RtnZKz#p`xLwO`-q$6$sq{alDduRNJuZMWYk^ zG2p7{xR9C@tY56eYphvTV`yTVoS%sIKI?X|cZs49qL~5xGgv9|iC4dqPc&(UM4KH_ z$gIP6Lr7G#(Ov9qX20@n{xd3^xOl1mBxF8YCne&^AgReyhX(|NcQ=wtkMr>l{q6T7 zlY&bqRbckD+5jSo8y`#P_WRySxI_}eVUx$2X0}om8R(QuG=I}cG)m!5sr?q}lkV_f zCYQtb?q2TtnR2?lDfc@1s8MsWW+GE5g__r+H3GMzXAkb)#^iYeET;EDsGV^Y%TKHv zWdjqGnx1`|c(Pjl06^~?hcQiROX1a`?H%jZM8$D`oqZa`@F&!*OFYSl31~_+I+#7D zzClWQcj`c6Anwk_;A*H%p##2i^bQJSi30L5d)5B=VVx!#;Q^GJfj3R{U~ePgqgcFO5d4uCH5V>-!tvTy3<(U)WTZDh`y#nRE#D4SA0cn0VblN z9h`J+)r@APnf+qQ6giS6ZawE-h4wpuGq&?ukcBef8OYFp$Q7xic0C4Y=J4`5?Xx$2 z3box$iX`78^$#-xB2-%uP79x@YinsV_PQgpihTk0HG>tT(cE&7k{P>pp|C=B5$g>m za;H62k*nAK3(%*acQ$Mfn!+1vNJMxdob_VG)Az$aN7i5Vg-}9L5@7!0B|QBX1_E8H zG8k+$X$tT<@hng_vA4kHg0-x^mYSZ(akE?6ytF|T9+J%4dA7{c-k!O00Zl;$VagaW zWO+C;2hCz`?D)?S@fu_nvCxk6-;jep4l4Nv_kYd7)utn^~EiCKz%!0Awm& z^+?NnF9f`)sb_cECoR?E2$a(IdHV*gz3M@Uc>E-&lb5fc-9XvDx*D!n6jv5W$Wox+ z>RYN=v+-^IW?aAEV4}3Jm{cn?*(-*AecO4mbv9Pf3gU1NE@MpRh;P5m+j2VZ6u6x& z^yY@j)9W+ax@4{=yP=Ndg3&?@m$jSm?zvq^iwS*V66>}-_*75(w+tAkJ|f!Pi>}W2 zj|=qQh^(M^tvmC~LO60qv`ctY{i&z`ZX>j>XVS^>gg1Y_@@XuSWZCz<;d~tfa}pT} zU9yd+kE`*^9Odup>2Dv&Eygz(*}^?2dJ7Rrm~m8GNiKbVpC}0-RUvL!i3PAF5RUyE ze|dC^%!3HLF_Lg85qoPJYP{8SdvR2Wml1kVYW2)kmBOTdD^mDZjg;063Nr0(-d4%Z z^u?aGfP;da=PBu6Uq8e2*@7TSsdv(yoh=Q+2)7aKeRDsycC@6Xvl7q-F2cTS9#S5y zNs2ser;&ILMoge7^Vso4dOwm?7% zR$s6G>_WGRnPQTTKzKG%eKG`Kxi2$Wb{S4bb^Rk()|SsQ&TT5dC7uR0_?qsQe2?H{ z3VQm>y^qd5!O3Zz!_o=UpB+ZEHWgcZEAe7&b_YLa_Z(NS(`@i4!4GIuTF=Xe?H_(ADdD@D>}bMWG53G@Tu(4Dn|Du>Osm*k@x@x)Q<`#IF0SHc zy0XK8gEv#s!?K3XBgqyjytzMuLAdo*r_WV(etcdQHPM_brimf}r5?rJb!3c|9U}i4 zi}>R6ljvae+Is8yb+psC+)~W_#!p(?4vOUs(kHwh5*O`!+BROn#s=qtDz&fhl0bGl zQrJnddtD>K6x|MDd%|VyW!ty1>2<06sDiR42Ug#kcSOj^`9A8&LG+%HdxlID7>c9l zZ3=FvSD@W)Lw%jZklF9g@qGyG)(0ao-?%lpY+W|f5qD9@1wZBfQG1Lzarn`UX+%gf z*o%O$l0hU}%psP%c+XFjnFTR}kP-fo-K_f&me zcZez*>A_ArYJ575RBA-bHzaD*jZdrlheVs0nAq6jUhupAfESKRz^5N!sv7}#ai$c?Cuw8Qk4&k77tJZ4zI zR;=#9Pc$t|^!tJKb_mxMBo;zi5F(4>#E8LCy31sdpp|o}3 zCtO%V2~jRl*~-sJ-<6QqqnE#D^5bc+MC&K?IWsYR9rr0kDvhCnEswxJ2RAVD`mQ?L zSW{zTruMaD!5MS|NQmcskK(HV3a@qQ{V~&OU(tT>?QNtGRZ68hZI%An`g-#D@$#;5 zO0x8?YB#Xhc~(Vr)gqpEowFdR`p`zI*`}^{qsd66VSNAq;*pvREZk+fH&yQNG!sw> z9$NFn)6BuFuBsS z-&_hlxGclfrRc-)0_Us|lz-w$GS#Aw;&q#x4SS-9#oxWd-+pb&#ZA6-z`97jXu@C9 z?`X!BzhFR*s>V1WM0j8|EH-M|uXTb1E0u1!3H{K>n*tr!Dmp3kq{3M&F3kcZIuWON z#3wGLH=yCMSC*n4al5+)1lH&W(xcetd!S#%`hj8|A!+ zuanvQ;-ag|d3fUV@l1O&39s6B4m7w7`kzdLHHGbgagkSc6D z_A}!Ow7bMsrqGc*-%J~IUQou^!+(cAGQNAW1z5Z?f7Gv7!R{vBi9Xzpf?WTD9NhW~ z`Ha)|^RHKBGkne5(7ku}Qz8%A`{$A=KthXG$Cb$9EIMsQ@kC?gx^fXkKB72Jqws39 z^**`g(~9;@J#X<0;a?g+X#+Iy$$?yHDci8lB2GxI?FtqyUyDgD$e%YZnBT18CS3~k z;Mu{so-B9MmB%*^9hU|g5Dm|L z`%f~1JkApN&CfCXU$i=&UZ|@ITl?D`13mKYNwVCb4lTQlafGDq>uNSD9D#Jn>_ONe zx1m%P2zsCGCW3JQizX_)2pGt=zA$C-{bN5c*Rq)&XP~)&Gu9{!3pkulC2(=1?0hY- zSt)()|CD1}6_Ev*HSkDps*L_L>n2IFm%^ay;<~{G!{7o}t6tE@>e9$5(`b_=o9zZt zD8-IA^7Q+!d!d>vn=nF2X1?^TRSMC^3$LvWjMJywEpCzpoHjMa*xsm?;V74g8Dacc&Mnuf|xVRln!1YrA33i-_(9x^`)V z*WmXF|NK9Fy&5ywwHCAxc(&aIcNS*B=hqlHLGqSCJ7tFjx6o#^{3{3vq9iy{r4rsRs;3>*I&Y<;H2mWxA5;3 zXZ63SZaM(I2}L;blA?=4PDh?L$%5dQXQx@`{zk}9!II&h%n?ppuHr*Wi6d;dLnVJWq^?L0`IZ|-E(9dnfyV3f?H_BQ04V@p=N?i8P|LiUBI2rz<_{&Oo?X)0QaAx=q zu&JhHBca+UPu8Fg&rsk0H36!r-JN!*eVAv}Pm+yavS z`3I4Q^)0=tdK6z(RTV`cOb*WulKb5_d{ZfQT(IgB@{J0m-2-O2J-#Pa%Ea>Rp_wwF zAv2yP?Bxj%w&PNJ7`&e@b72Ai?fgTYmp)llIx8dI@ZBiTVUZBk!0-a~+1RPx>y|U5 zUH}uh`}LYpvG$+kC4M&x52U=NrhbNt>)6dbI6S;jR=;S}9f2<{Dd}`|Vu6ZHE}P7( z>%LpndvI{@X2Lg#D#4T?_TVyvnk&P86$SkM@|mJw3SuV17|pMv1qwQc@KSkX2BvUm z_|GGZ2X(q*mW6Z~1`KVrIhz?Ha;0OAS64dTKmHCyO$bS5QQIRJym6%ORjO$`jl=oK zX|7{k`?ngPPhUL%x&PNMb5u-{uC@SV2R9b2^yT^`U&zNNcnA-~Ig3RwFG8)=)J7+^ zu*uWgHa*B`@MB1gs-;jfAJ6(<}q$72lAUse0ImB;H>e`Ik$W|{TG4YB2${IGN zt;ys$`b;el`=`~!m%6w>3r)Y4#y<-%tk3j0{U;1;Y+7dKn9)2*beBw}6 zVDm`8^FOlLJ~Go520Lb>V`Ap_HdZsrzu+65@)~n$sU@D4rNz?cX5KpHc)^%e8@yTH z=*a96nhzL80t4V{j3(Q3Bg&dn0IbpKf+k#{HFSta8+NBISt|D)9ur=%N#V461v=ZM zDRQ>BI@|6}s}O2qA6jMB|F7of!oeXD3L4tl-ri7?%g(O_5O)o5WSIMNR7}hUT*>mR zrpg82bv@q4L)^QwwUKa(w+e%ez*SG9-u4ANgS!r=Z031i9$}Dj?JArP3S|z6Fl^Fr zRBZY%M|H9D`Z6X4X)dq8-gmNZMjJ6{2Vcgj=Qa&dkAD}r3HEA|&tA{7VQ!P3`z3WZ zH-XoCr@^Nn^C20YlDdI5>Xc{1mC2*6y9(^j)+Em)y>m{EF>o`Uym_~~NE%sKh~@+P z72X@SXOf@4?ARF5==s+SUY5{1zf4U@>4Oi~d=cyck6G{hEa`Py^68Bt2Dq<9b0ZLO z+mc0-a3XzylT9#)*+0Ttdc@|4g^o3Qog=9jW%9!Y8kSeAeN&nfr`B>*66}-C&sv>} z4Lc!Nsf2sDscoqrLMPKka2b8VgVnB<_v!lD1k>M zocCrEO~mi=P+^IH@tcZ|QkKhhplZy@<;n{>ua9Uy4X(l5%o z85h;xG`@I5_nr8@S*-zsn*91dCrjXDkS zxCW4?sPl_KGWhfvpaPKvzV9&@v|elMjN`#jM`-CN@jqlE{%HSG1^|fl>eVaty&kxE zFc{5!r;!wFNOZ~4kTMjc4J&FN!#=Bg!x{N#$TwN_e@C$~*cY%&C`ZY6`;{;la z^=np*QlM%ZxV@4=K}Rn%?1b7Q7-R{>*uckevO65I$B!6khJ)F!cTc6J_Bv!JWPGK~ zFyJ5IK1!vBae&hd*DoWUOKdozGeYJ%N!XiQo}D5O^KXrs6?$L3ZTfliW?ojSGg@td z>f&UBT^wb)ZHsc~QI?S2Lgh)VqQk1KG_D8%YLj z1-j?+g8nb|h-;<#P3pgYxhN#$etq#A&Kvr39bqdi|3kfg+n%TYZWIbnKm(?HOsqs@ zUJDAvw2v=jl3IOUtJNmaf#E*vOjQrDa%ZDdvw(7LYpz{e+iB~o7q9=ygD!1{Tx#*I zf5dyXl?GE3nW)B@JJuVNi0u`k4`YA0frAb+;Z`r!v-|dO6S7&=FCWGVwFC!*ueiuf z2NIsUrTnn0E%(X!Gh)i4ddCH1Y1$8h)7btJ_{f*$3$8j#Fax7l&neL7R`T(7Sh_+r zA9MPc9K*P<_yU{?rer_7YaIhVrxxNVo;=oz@@^t$N{%$1n{+x!NM6|kh5q3JXk@NH ziZAj6*r1>tmH#%1zR}e=e|rQHv3cwVB=(_V061A8*^=8bF8ZYfEbXpC=63X8j?A5u zOuPxm7Pb@2u$b!^$r>g1`-DANse_GxYmx&KRsNn+Mo8Q+;nMDAs5R-Hm0GS}-zmtY zs?`+-r_tztfujVcd$xKW7&>w#t2grFEl(CaeXoGER03@9twYrW(_dNm*-Qz3V$>gq z%%*6G<4Tw^n%y<1F*MvvrYb(U@+8zh|E9d$9A9s@7E%nfUy+Bk%egmRdAv%68mvrU z3N&^;GNlK&X_#8hqF0yn#{{=JNy-waluHkGLf@o=iWz89DX>BQD!#MH-S?`eyo73c z#0i3sx-mBgdg4v;8pWn47R8ku3|sXiPd#<0#5>nLJrG=7SPD#6E88}l4ftxR5YmT2 zEHpJ5{U&D|gQPnlge9)%X1=_M#>F%9SACB=NI015{smRuK19!v;4mcF$9Fh`_v6Rs zVBHE)Iu;|;`t=?KZhu3_CBzJPdnnuF)12(lY)N217@3cUrGvne(tN;!EufM`-T-N1WlT*!C8ekpb4b5GY18KHgs+p5+{S)bcGr$ISAK!Z06`RZc!r)^O$M zIf1PLa3jx9T&MdQf3ntl51w1`(I^NXXJ@TpuM}vmnzD{C-mD-R9(2NtFL)GEPoZ{8 znsR#U?Yg%pR{PeD#AR2j^xxPJssGcbXAjtLwL9_&Hn#Bj1-u4uYwYX$oQ}R4`lKV1 zEx~Aud#D;l`U^tT1uGOKKwoSLggwVqz9ZFxJq~+=!tAWvd02x>v~$3IX~=M1^XCixPv19Rj@*9gOIl0K za73DZqt2VszEHyUrJ9s5V;MhVZpeaX=4{7yKg}yGYVFY5vl17k*1zLpU$qtb0z*H& zBQ7d~&8r<6)Nq`%EuaB;!OvXBxX}4)NqIW`*UL^M>-~vh{VbqeiPYz8#9@pApBIqt zJ{DwBuF#KQy~Xbq#63mnfknSiO$v2SX8Zm05t4&AORx?Wzft=N?;&A$-!>O%7gAQ7 zGfF~{Lb~Ad1O5Mn7(E)7CjS~Hoc>Uz`}d9*WIWG8J4N6GCQVJvuI}y?dmeaUp+TIT zUfFlT_oyMp^>fAQJ;~UhkB25Tj-O8*sA%Q}-F%LSfy0D)Ty5>#!Zh&)fq+9Eid}nn z;S#rl3vP1Uv54i^u!g6fYZ%2U84p_<)ZjWv4CC@;Qp)#8m;r6WXEdmchvF!t5Af0@ zQ)qY%23uXr+{&nGi;=!*Ujqv{FsxyNDl4Vrc;3nC$P-z()f@J%U##FoT{=?;7wT_x z&ny%XDkjbHx~*tbM}n7dkUQhLtE@>y?)EQ83Br4EJz$Z7jIvd2)jZX-;Y_qnqGBxA z$g*glI`Y-_E!r?{M0Kv6I$FH+-Hu>P3htMHpltQs^_6UZ{kqFs;I0*~CcqAG&cxFW zE7V0n^-}Kcg=g}DPC-h%iTAM@;^O6$kU`=ZOLzAw-yJMpz1lQQbia=WbsDy|8d-YJ z7DF_T9XtBIjD6jjDsgDRo>eZbjr|hJ1GDby^0*VES6@p9rdO>tz6MYA zQN?mYOy(lQx=fJ2il*>X<7?~1n7ErF_bt~@#7haZ=Hy%Bw@~1BhtPI72KEdpdwUJK z^p_J5q*@S{YIsDIAxsx5&HAQNBt?hlrdM7jZZHy&v}g2Mj?SRl7aSOO1XGZs)m!&% zC+BGpyznPop7zT)-6GuodDpU0>i3lwj|3{-=*5*u8HBFvz#~?k_f2&xy8es?LC#%Ja}jLM4}Um2$P@H>WU_gkJpO+r?pT$zIF^1c~RBZ1u=1Wk}i zW0p68dT*(hbBJk?8B5E))*lw`8wkR(XBiTc^U9coWhja|cu5GR%xY~kWg+;i& ze3KHud5%-FU;^8BW}kI`e)A}0=EnCWss9g&obM+9)`J`%gYY6>Mj*o-2O62_8@EyR z*s-r+CqWCv%(zOjjKk^LY3?cHEMzUqo?hJ%^ZGOLSxoU}Tx^kN2i}9k@_Gj!Bi9AA zNihQ%&_09%$SO0sfLc0^FY{OAZ?*I)>G0XBd6d5tq`W^L67UJpfTRBwOtABQei%G1 zQ<{M#%|H`gfJs_od`=!i7HxdKsIjrGR|7}?sHlP<7aE>5l#QzC49+CJe**2N^RGN6 zl@$boEz$%|CzREl*J7tAOm|1n+Bn)ds&wS%b+E|op7*Uq2OTf9ImWqjeL8U}g!f*j z#E=RX95$o7p=fzrK>G5ScVh z(~AN~I3DuM9yM=%z(n3z~9n=xumPR_fQEkaVP;{gS)HfRT{(_IILN3I8N2q!w- z^y`h8w__pMpa8*23~X+mIg4MA=Ca#^x|xF!8+Y0FPKF*=V@-_yk@{*hIGcgH`Qj~6 zL*XwmQf3_ZC?votgQx&e_}`Sjxz6>z2oj@iM#Ny=Y;?CYa-A^rTtM|c7E`&XY58pDrE_iAc-{+ z|7AeKnO69g@I`~qpvHKFNw^yZ>~2XY^?VbtIL?imy@7z7WuclHnx4pVA(13qjn@`Z zL2A%2x?M0ZAD<;|c_}kbZtZWnoP7NhFea)@`sT#v- zHJbF97U0vRLqa?C0vMeDTGyhk%{*wBUGUvxI?6U3!&gKJT9iq==Dj|jB=6zw` z&cKpg1c^hDi#}Fkpa?W_q6@SpW%^QjJkQTmxu2s?x;T7=Kg;JXL_dYTA1_s58N%}b5m7Nm}+ zIxC@K!GES}Rml&qKR;{VSm}rRgfgK&JBH+F5&uR2rACQbOErvYZRZY~7VMWNR5{AY zx|H{J#!QdsL%x5Ls5lUHmKe~Nncb3QzXd;bMIN;F&8UsLpIiS}nbw6tC#e|>_ot`2 z{u8ac9F2=It^o(N0MjSRacG_fg>mGJm!2CJ63( zDcydg}@e42{@Vf`1t!JTI=OC>lT=fBtg6=V0ie#_3ukoZWOApVm#xZ(YOKMj9( z*?Xyt7T>zB{r*1Ot=fC@DOeC33Cf74GsS0d-f8>PU04+X`gcQzxyw1!_$9jxh<`tF zv1U2ynrEvB9m_q#?8rdGT7B@+;V~P$c-O62BNa`9&yRO5KBHUrZy5dI!EP||wO^)w z`eZM6R?8SkYy~{}F(LU2XD0LTc9CvbCSe`k=0&PE4P_PsqqB{v=ye&)Gn;8^ndgAl zZNZa}Vz-Z8Mev=jTlb(zoxER`kfPxQDQd`y#D~4Jd4AVdXn+d4-(0LelW;VmY8_zW z?98EDAltsu(3BYjm>OE`Pwm_y7)?C%Sz}%b32{TSE-lLq@|VPXvAr_k)3UU-#vVXHp6f%l`bkhgZiv z{GGnZ?nuJ%3Vq?pQcWk-v+I*&Iswidi|vrf6=+u8(^@X4b8n;g<}J2EC%qSA$#b43 ztNXqB#;X@XDz5xqs!sv@m&Qb>AvhhMer_3LncRJ`4-|B4qTe$m8Qm;X&0rr{x*ccC1vi6$W!|=g%sSG_P746TzNbnUZEd z>N7;7mbSa0;`P4k1MMsH2oZ{2yO<|R{WBWFH;X4z z4mb$Wfxg?6E}8yrftSuzVW5)FJuQQ?=tZ5dS+<;<5-ht5TuU#6EjMyn__Z(#6Ysd+ z#jG$s_Whj7wxdA`$b^%~^1%H5!KRv@)D!alNp>O9U%R3GH``@J31C4t;!NHL_Ov@3 z|LnW@sOq0{Mfpy7H%{5!o^^3?u~fT0X0h1|;B{s#DkjF_)oiL+8l~I)iG{qt*8D|_ zuL;*7aFrNv?-CDCCR#W)B~gue}us}oBtf^#SJ zMm&`~_}M_dn`I|2ca!cuLVc(B#)4>+`45jrBpv2ITvzW@A2amDb+s!$n_oA(b1??o zV4C}$H7=LS3VKRgne6vC+>r11&vGj+vBYlfnOaOX;ez21gpOdy3Hkl;pryppeEPko zFk}pt^(7}ZuGi?FQxw-hH;EvAN`+Wyi9*Hxo)vDS=QCtd)$G}axkfF2PKt2a&PiD%D!9M(z$<)@VY#u z3&_y=vX48=p?RrcH4UQzo>Bz_K8bY0y*5>l_w^9Q9u`X4bXKut-yKUY`| ztxeb9cf-iM9hrkUo>$FOtx!W!&xeV=hIx9>9L}|DHugxSdb=^ui(?sEC<(C@{-tn8 zLDoF}$>G@kcD}FBP@6%!fB(s5%g^^;PY@SKKl!%$eKPZ43R_T7QITkwQ%&QcRA}ho z*}?0%RJkNT-rlxcNxROa)! zyg$9~8#?-&UNk&X%ZZ9>-YYAUb@guBVFTWQt*KJvJ3Vny zPVh&(E5pB|AITH;62`24t^IHvfM(&msF(x=#R9v>Hj7$L9QXt;ID44^IBI`WE$}z_ ztPy({{qTMw@I<51`YBUM{Y$VB&(2vzo=7*Uv(*HC=bNj>P~!9+$EBU#FmN&FN+L3A zys@Ux);r&?jOVV!A$E<Rt?c7cP zN%p(xsWP1efyo8AR;{we`ew8siG`u@JF14S1uR=TXP-P>4JVmfZVVo^L{k;*rgyhf z_|f9{%_h0_%IdT=M|)Nt+|Phu7X<^Y(QDb%agDrGpvk9a6?X6a>7YqOhZbDXP(jzD!OFCWY2Hu1EE73JbPu=dG@GH86#qN9lEzP8H(#Vz#A%n!)!Mh%wNgdoiYB@?!{ws&d^#xzg+2i`9WWA(u!my>VIvuZo>!1b!BHw zr=&nP{{1kT_NUWq>4P&1@K}5t84V+2)bjH3u5C!4%{8AbGi`ybO0m$Bb#BRv1} zXogGlNRKt$(dGDG;yw~EcIAHiPZbwGV*Z^7_^9S}h3o&#QJClccV8v(^%jS| zebm1IEleOvKtKTDF$QD2^AetaQIQnTJc06b0JkwRop%$yDGL*%VqCT3o)*~IOjO4BFzDgJ zbWGDBl9~-`b`RL;s__~YJ>wvfkZO6}uXn>XDYwvt-*)WHp$29HiZQGycn?%&DPIh! zr4`udqJuUmor3JWiT} z55er6(S^IyM-#`l#6%k6PKZ8MfUKZ>XHLbM}$+(HizG(izgRT$mI!%o-H?eGPkg)tJ6|f zT)?FLWXpU^toI^xTpF|AF4ACXpnp*3nyNQIUC zulMt^bWcYh#Yw+DnAbpCylqY8I6)&Hu!MoAnse3B=zf-FnSm%Ms>t;CtlHU!j+sW9qW?%r*BhR(C5e`?{0xg(Z< z0GOt{@R*g_xH%v!q%1c^n~&hkwTWEKxO2r5zV!{UYEUe7a0|iq2pF@Kj+X8-rm{DPXiNVT#|w(>L8s@(ZgD9IZjY4iZ9usfky`bRVzq`?S+fqxit{AqdFjpb3XClXp2+YF7t$Z{ zkobPZU|=tNU%Vy}GJ7}Rk88^Wd-~?fbGT7>rH&duN|^hq@)0tL_edXpaeZuepctjvLcj8bZMq4PyNq)L)Jg{0vqDwbf1Q1KRSlxRL35N--cFx$XDOxZE(enkJGLnXzu6GK{ z9C(kgO<+2%ZwFgFXZnSZr0gw_PXV|uQ-HDg6fJv4*qQnCHE@%?sje2rSLyhJ8mQGF znC-@DU$I_t-orT&>nL!`1@=6oZy?C+KyNO(@1Gl7n#labg(wcw=qn@~7+w4N?CFq0 zhvS{Tk@d2t4L7RUISUF5;eI$Rv_d+aw#GwdtaD5#qQ^&{hGT!8`=yd`jFX_ z!|CwIQdlVn>2~hmE^y?cP!U_EKsoN`FV*xrJpGLk^+n^ZCZ$1h!h@wv_oO_LwRJD( zVQXb#_9kpue7@#__jPlfKE7qJN^j!!XEQYIkj$?V$A#t}x)3cBscq-#!Jsua%BJKl z7EfT$^+ora;dep6t=WlhIt1#-pWpj>XsgxzjPlFuJy6ifSE2)exXyw1caa(l1npN- zmIQlj;|I;yL?5OSKCg)y`|0;r2MK-GKOxwY;YCuVTf4(t_J%5ZG?V*W3m7!j_ z{VwhlZrky^P)rY>^2#V~xFNMan{XoO^C0Xq2aef*-gF_6wt?6gXP%HB&(K@#1pLc7 z5XcWXjV`Ydde>HNPPjNSe65`374%;PXd_F5UhERF-4rGP8PQ(5K!=3WSY2%0WC|Js zrk>2XJdF^NQ-!5hr6`Yn_~ub=a-9X}T9SC_fv$`wN`fC$?8QrNQ<(4O*)4&|lf2yq zs(^iktm7~i^CfuJ3)s@0b#V`-x5UHldHP$~ULW!Hih#M*yk@POPoA_Tv^G4;MBwpn z4@PQXwx;c zhEmy7m4Wor%2450y;nK8ei36uu8gC};Z3+=qI~VTR~Ys4TqKxpkq)1nsl=MXTW0qo zOjp6{a?G9z12S^%ce?oVHr6yb-vt?C1Kip`-uvT(O8$Si0EYs0V8G`iA=F_T;?Bxi zVsLd39@cc_Z2XjQn+aFIP}0SjNnI@Fu!_3}7HOLGW=X7ijioh960=`iam}@&;1F&d zT|H+&WV$4q`M!gwCx($y8D&GyuJK~JvhOnB5HL@{OwaYnWs#C-* z(YC7|XK+jsL*5tztAW}>sPCQV@2C|}ON}qbzV2EJr0`|+L=iRwql^4R?;O*g_G`op zXbFe(WWw!>6khi?l;tqQ^m-O%vh{;T_cYJ1G2-KMZgFIbzE8cgU-dDe+pu4bx%b)o zZgE7V>LX`KmxN!X2NC9YFYOB`&*33+fnz-ud${h@C(o*I>X)0o6Q4XD5#UWv_N)M2 z-j%Ch%dHjHTbCpPu49c4ZDgh)bq{^am>|gI~ z*=&Q}I2UaX?Sa$(z+A`k=K?}RnyG$D4~S}AetqCAJ3AKH0$D=fE8xI)m_lfZ;6;6j z*eU`SmAtL|Gq2zj6jt|%d@s+l#rFuJ=`xzEi;SiDFkpQZ zw`!~n$HZE7HWEWdi#%E^RoCmMvRpr8f7Dd`Fj#maM8=nTVH^F1%qi z$>Av|dT%HG>hp>#<}#O>Qzt?+HEAdO=9u(yH8?f6ue-ua7h3_(}Brf!lqViU%n* z`JDGAjvnfdKEtDh{Emh%i6u{5><2fUji0CY*CRHEa7rCgQ^o8L8$`Z3YQj9?RsIH1 zDd1Ny?MZXM&@$=luXsANnDHJ~Xq%+M>G`ujOxdo^?qE4s%@kKs35iIjc-o6gS)lNE zv=BgiSoO%hyLB7=vlmzNTU=z@L{C6iM(0NN+QkWj&!k+%>@5s)d?`<5mV!}|qtG)``#gQ(7Va(mVf-z@78bSy^`pBs z1t|npkaDH<8**Z6^$<9U30@s`Fy9evNg)ovXZ1{49#!udtEfhwW?xdfFO*>2UKihB z_D%P7!mWpNGlXf~mBSg9ykT4k8><{vs3jY*tC1S4O@xJ)zpezdcv4&wHkaA02ucWB zI@PQE6R@Fa*lWdR-40`){uZmwqxV>Ez&0h#wzoYq3s{X)r7NY| zvoYNw$`?k~Lm(nh3c7&o96e?HJ3~W5I4c$C1`xNMV-fb1gnr|+o!757!`B6!cUe{$ z)X!DY3<+ocfAC}Dz>Ey-#shQlUNp&Wa(5;EO5)UZmkx8-ZnfU~r2i>b{=JNOBP0YO zHnQP}IqE+iYn%=gG9ujU0CGqun3;Z{%EyJq21@pSi`vKrdqh2eYY&Y#p9V|FkStnn zX!jM`tI0MDXVu^z%#pj6=eE@s@Dn1{ezlKa(xz6_YNxWN@&+#`l~@cN7xLawgDf7N4!rUzW~DUa-UGA5 z&suy<-i=&pmg)XzbDyd!@tYWa_Iy1*)5?xV^PH%x4*}cdr9b0~XHVjKr<;WXb8}u@ zq$k{4t&L6Adsy6PYI$?SCco!f>{W{l>kBgCtqsPvQW)L4#B#%tnr(13680MLRBU!- zEe`>ik8G8d>NtH$8sZbacsu4By@1CLaCdv7oP?DdVMRb7z3J4EdD7~bAoI}ou&;AN z4=q+Rk$qX3w>KNE4fOX#x-Qr7?Ql(TW7y<7C)n`eohS9PG zYoSHYv_gI~!=8@rJxD71Ii9Z&d9a}Vvm7SRBZ(`T@|uBw=X1P^g1lhzOLfw z5?^eOf0iw$1xBu)pN^+CZu4VFYB(^fWNxsu%8wLuH>6VZRMz?_*C8!;*EmhGHRL*x zM4#o0LoHQ~5sKb5_h#pr=7l@MqhIxe>ZY36jy%ZJ;+0lV6SMN$IAcXu9VDyKT-dXw zspHP5=wCrQ7YCjAH};IUq|6sn`@^AlT6iY%*02BtGj^*}wYvuM_GJR8PB`qj{D^)H zZv;O%b$xNY>e3dzx@UHzc&&`#ZiW#us?SyuPMfr5MFcp*bG>(pDN$=Heu+;~Ti~`t zSR5}5C5pL?GQTrlm2-MS;e9ETh}}HoWS5aUxBc!ry0ib_BC7u;sVa<6G0I`Fkj#%@9y z3yB)tk}7&#OraEp;IEW>4pS(`rLruS@-U7{~4?S2iJ;=t2U0ppm%UF$uQW z(!0xYX3vZ@m_=jG<;WLg>0!_&uXDjgAgA3%$S-gI|0sLwsJOmmT{{pYSP1SC+}%AP zxCRLB?wa7xxI+VtYk~%McWs>D?(Xh(oBZ}Z=kD>{bH@1A-)q6@HEY(adgeQ;P}p_1 zv{=0@Nd~kZ({5I@2CvE2zG%KVX^hb12$?rAlS@3x(-|i+4WI(r7X8G&gXu1xea14sV}Z zuz^JUGuOjeqtOaKzoLp?(*?2~5?~Lxg=f69j#hGup2@HyPE4Nm>Q2!=GNTw6QgV+xHAf^Hc{ujgrXdVJ#;UGM_C;G$&kl292*zn1@Zo$fFaaR5!>n@HyeJsL<%;v9 z-=64yp}APe=(%1kZDzkfdki6cRD$x9ed9o=bUI{=WdV5ym$BG=QW#k3 z(~uc6+t!*KFngmc1g+ifJ(yB?N<3@G(_kjN2Fu58sgAW+l6d=6mCQi&gsn@7^M|%C zA|zz-ZS8LL!OmYDOE2nRNMh>9DOSys&@+CWb045;A+votTPs15d z;UlaJ6N`Y;W+HoXMp!%x-;byP9kGp8y;3-iwHL=|MyNf(kvA(wQN)C128Sk2VVI9r7MDGpZa4q!k z-h(XwdIMN)Fe0FPLNL4-*ix`++3vna+C~(azkGmUt~%+Pgt4H`W5nf;(osx66c$XE z_p0|WPW?Bkb)$713ajoUiIfGUQSkBzf2To5MOBcFqX`9pFnoQ*I6Bq7Yzhrt`Q{8e zMh@l!%WdwU-=m8(1&P^LKMf`Tg*LSR&@+&Xh3d0^&;+sNvCNMojczA?HrL^?fNe#A z2!vrx(tRW>zb?~8;82&XF0eEkF>R4H*{FgDOed)i63e4reor3q3Q8=@`822K}5S)H?~Ns4+8 zOcux-iNOE<7W{`zobOs^(2vOGw)~gGxRW-mN-+hcfGrc2(;&eG^NQIuF~wFxRA3>R zWYKY)1TKG+p1a~|H83Y>Sd&!?TPL^ZLpox4GKTE6cEt>ES*53T=H9|<%I{`gt=R?E9)5NGRkt$=$ zdp5+j1hAYxK?xkEC+VtQ3$!Qlqb_@n1c^ObyZg~*Hz`dFzlrNQ8F!v%yiF5j9W(QMr$U2u?3OLQ2L~@4{&A%1vm_GXUlO(R&qELW% z;8gN$%?*WBA7bz^cUOXLa6Eg3Bi%p1#6&>6CI)9LLX1JNJYf+PjH7WxaZ*_nJ7}s2-zGC?|NmJuMN-NATa%9KkCk zF}xAx^2{t0`$gbSLr9~3freNWtCPQOp1v&rqVlAh%K2b*_asZmti1vrQ#Yd%&|EjI zHycveg6moJ_acm}rG$;WiQ0OEiUN1t?b(>~JWYdNNmfSjQ7akuG4~gvYS&*_ zjQDEp3&;sh9(U~1$>&#LPA9D(j#bSrqf$@ z)HiLWb^dv9dy;8CEao8K$r(M2E5GnYkio^0PDC=>kmUz6IQvM-7ZPG%)E;1Bdfg|~ z@?fFlrtC=9p74r3MvAaeN<0qScI@DUjH`sbu1+^`eqaw~lXnI7O2@$wo#5iMvI2?Q zzxjd(J!EZyDoJL!NASduuX0$F6sp0@(#-Ya4y&JI*W~htKXN8;M#g2K5F8cRM>OX? zUbq`IsvC9=6uwD^AGO5%!QE-{UwA7g0tZY4Pi83(;uHIbAi2EYI1)7{nOr3u zFGF)CW$Y^s?`w(ocq?Tl9G1u==TpGZLw&uYo@fzqUk*38Vi5pTCY|y}v0iEbIVDt5 z$os~(_q6cy$f0F0uftH}R45^tIX@2Y`k8BWBUEYq&p->Vh(4+t&20+0PLPS!7XZir zOTb%WT0RJvq;>o!){?nsk|ek7_+i1miv}rQ#KnB2J9_@4ldR3_FemKIJ93qRbuvdI zHMplk7{aAgoh2NFTVBs&K#@W}V-vmnJD#Bh9ui)A$+{*oDtNaVE&QUv7hX@sWzu~B z*#I;hiZq~BIP%YHoStU6u5~$;o25Szrmt7ox76u7{|GoxtGD$pMgh3|GN0-!wHEa`O9cE;k%m+x_2E?p^nCD)uXJ5s1SS*w&^MO|KTu zp-uCwH~D@nw0ptz<4pmJ&X>*W&1DfbSf?@U2;A!QC=4pLEihe##jZ>?@n~hvi$IXP z{ZF+jCETuB7Q1bi+8Ofw$Lf3Roz=I6(RJ#um%E~#Cxe(ckcjlHA^J8pHQxcuEUFyW(^eP~-ZWxl0= z?veBLF2b|j8sZ{BLJ!z6E|P?Ct=h#2E(pTd(!y1;S&%duPLVuGn7=GG(8F+e`(@Fp z84PuL=r?88K@O!KqwP2z?I~?p<+WaLJ(vCXS*t+NG&8KP{N2L+d#N8mzZ0$a`HtO- z(F`p6jQp|w)1pGXdiC3l{Y|ibwb`#xPNjNFmLNs~^-~8t%{UhOy%o<%?<~IMy&#aO zUnU?BE;GXL+$uZta5_R7mMCP=gHMdbjOw<5X;M?zx4;gcPP)iwX3G1-otCiMF=*JToxsKIoS>|*XO8R5UwLA|#t zz1|T^RcWvXWWUiiJ9}PA45!Pm>6+(b(eXUx%@5rUYT*PQ5K*Qh*Smg!LQkecIvs*% zSv+{npf%Z>OL1<>e-}f{N-`sFmDNSY>PyOiiI9L&W*hI*;xL0OJf>n;NKQ00v=qzQ zO}D=8(qslI)u@re@UO|@_mAF9Yjq zZ!6xs*UkOCaO9mG)Tp=Ww{;(wol0TXg5VS)iGG z4P%DQppSYLzZ*mJV4S9pwkI(dZs@Mr-iUiJ2vr}?CmZL}mZ-U)Ax#Mhy_3_=Umfvb zEoSUJPJAR@d>}Lutv70qMXe$YW(3MK$ArPPOq=1!XzEbeW(dSyCJaw>O+HX`muWpJ zz`Y?2;q)~t2L0=- pth1=c)4-;q;4JAWF715GS?17fmrk6j71L~>Y&v3xktRLwD z*qr-~mlt88o`39V)%2J6Qot>;d!W8zBV*=Z5lz{&NQ+*M1UdXp%ka0$CIlDz#n07r z$4R%CnbMPVSJeD@ZT-_Q?PH1Ok@u`sE8%fYz6?4itm_ZAAK;E5q3c}N9UjK=H$LS` zDREy4TrYffN8eD&37oUDfL{#NP*HaX#VVQTcj)}`AU*b8Z)4--h}`0gQ=*x})#x^u zNIjeO39R%`zE+R|j_k)rd>{7bZdBLXOu~6$yc|2`3GCmov*mM{pg$=Tf%qe;uF-|s`GaY@ zw56kHLNe2x1%aDh+Qt&yFHP+Nv1$-*5zyJ$`R@Jukz>LuzWs>Rs!s#fbp$HW=MkUOSr z7FH5#r|kZmwidtc){iH~ud;60xIx~~1M_mz6Ayx206fukqbqS>26G5K>reMyyzbIr zEbEqfg@UhFWxTk+z>y^0wmETI8fC+G3(cLgF3TP0)7^u~!bIC6h@-AF^ z(5#b|Fqyy6|D}M+m;wVW33CG8h;GNgh!K-?aPq_>i0qt2iIld^9t95!(QLTTLtx&h z)4SE=v+&53Ot({H(`G;EwfLo@*MW#>%3A&ESQ!T;KY*T$=sKSGcPy$#wjSuO9FkSt zW~wu&$zmt|sZj~JA1wws7f0a!9-D%WF1Eg&2SPaC(YVH?(YClzlWg0wWuJZ(rs(4x zr#nG>MRkrO|1C9wUYrX7Z?Zrt5rj!)=U?X5Z(8pKf(mwohh$>?i>pVh0)Y(@1Lvi9 z!@|ipKm)Y?r*l(Mi>B`DyNS8=UzFCJJeQL>j!f&E>Ha$I7%wr(>f;9tw)rrb2bL*~qQu{$UQ%u%dh!jZt2eSNU z&y`i$jj3QiO~mx3SJFmAJcKtac~r2PW6Jb$w9~KYJui9%)GPh?y`H`NTGhoMF`jn~ z-EPH81dZ5xU$`Tx^+Vz!8SkQ$km}pF;hVcLW(8dm!z*+Jz=>+{H#@b_I$zGCv~N+S zzBxyjgI7sP7jJ?6EgwuQIkp26`Oeg(xFhDq82m#$pFhFC->sQ8DwG)j$sZpAW%Zh4 zRyxU~-e^A!pr0QWAs?yZ-B{0^R_mksbi_{8JN zZ!-G8L`H@x=yB=Ibr|x?wN4z9%S@R(g6S3uCC}W4Its4*zhJ7<-{C}!srHi~)f!;S zLv~ggok;{i6U9X22&h(hEcX{bmWy;`=Oj$OTiuM-fyxaLFmE^^p!*Nx*^TaY{BB{I zVSCB8&B3`}TbC}D2*oK+HnE?f5m;vb1k`Be+h=E((wZn!#(GRQYPh|Dit=z>PwN`z z$&XD>=BdFub0)q1K+?jQc>zROE6GP(dxLucyXJ*DZ+&hvq;-M6@)QP{lgAfGg6z-X=|q z*!2r4CAv(#PQ;&Et&pBBT4;2t1)qHoesW!XMs&OmJo9|sVx7NskiXi58L`3d@v#s_ z^B#tKFBE5W&t}*9%+aHRtRKF*V?myL(WRFbRv$%Ve_htg-V!ukBxOM8d%IEOW_Fl= zh{P`u2Mfv9vp&JYy|)ylL?!Vxpg&P#BG zYf!Ot4*y3?SXGTm3i^?wKZNsT3;$E7OR0v&&YQ77dc|y02U}#~3bFHt#=Rl!E23cX zVji4A`HMuoe6tC|dfYc!|C4-+hoIeii{M^>9jXlWz_gF z>x35<`x9Yw*+7veM=qzBT+Cyu(9?5|qa&{lKSdNVp^jjMQ;1-9f~xhuwE(wJB_1#0 zRbupeIs-Ac|06RN%QS21%wTp#)5{~ddXoF#jxpzv{#jn;i@hNv3aZZ*R47rd1s&)CY$c{tsnUELG8%+zD`ntzBmE$fRd?O|E?-vQ*+ z_$P0Ee1(=7>OaN5-?CEIR@nDi!x8vtaPwgTCs&G14bp2vY$8rr(@KtkV@}3;1MXDz zmV*>jc5|a*^aapT7J0WoVw6ZM1^o*SRXK2inc+ zaz5k~+K&mWEnuf|CAH-g?9a~tfaw~%vK#qFAI|L0XG5Po*mb}*a;aG0mBIT7yEV$s zy7-;l(za<2Zg}kA>&*xoUW}AGga2LH_K?X|f_L;5vSp}x&e^Ju(~DddAGGGs9*?}&$B*z}90`^J zT`2UC=gurC`sB)dLPmF#oF88$m>xMY`;jDAiU#nNY>Zp)8OnM1J8HqM|2My5OdoUEE1L*DeMudi{_WT-WXHn{lk|o_ zOo7?-JfV8pm;%tkjkjO`W3+v>ws*B-XFmUB>ZW4St5fAakh#aK}JPg<$Co={_2jr!$F_gEQX&F8`m=^xTn{)}BC zO4)bH95m~1YNXNYj9zpoM+BY)% zcJ5&Tk73G>=V?ozwMjMbS9mbRmCk?4AJ619t>7|6$J3g|p@5x~G+)W?M8g8hIW@c( zndv4=WF+;+CBzoog!IZ|en)?0*k7WkICKxp>OQjWXd-ZSY|wB*{Jj7P=KEFlMb-`8 z5m$Yt)S)bsBzC~Y!b;A9K<#mbG&R08>vG=Bx4;2zy*$G zJsDNmF5l3NPKXAdT1pBbPfz;h1VRM9Cim-Qb2p=~Sq?qJhW>07>PI>K|BX>k6*lmq zZsX7VN0~pE3~Peybn@v9_!T~nWlYi~^{~QH+3jDZ^aMGBZ&$bp?ku}7@0ngg8q2); zeye)_3yJOnsLUePHka)~5Y5^_smg-e?RQ07_Upp7Hf1u~uO`MFli>8ggMT0!C?R7{ zKFGP9@91xM+94DrzCg2;JS{}?-#9g{ev7iqtQ=xh6qFK~Zv^hbk;V{w-r{Q0tR6s1 zkcCJ|wtH{BLBq^95CalqkD=TRD78hrO^qU4&B}%Akjo_1-vV3n5sI3J1g>xsx2qbw z0p8As-k!aSNi#C3qbhFRijNG?>o4AD_I~g(jxD}q?x?HjHHOoc;rv;+Kz%spl5iVz z=TNC*2o8Fyc*nSZn=^C5v-*uf*wZOvHy{U)e4*S&bgk1U4*f6brI*agmI_ClWd{9R z>kjf6_vg+yC9n5V?9sAE@_I-i{KKsvXNDQ^3vK_gIw93vju?c$>JzDt*)fXkkzAZK8A%95WrEm`g}iS) zn)0;hh8zu1M1vVA^Y)6GIe+3mMx4SDoTqDnbM);1L z(a{g}*^HV?l=4?sHT)ncajxy4{G*)`H=G>F;LeY{Rl7R>k4N!yP>))y4xLUgIi52p zBbbsmGLu?29qA?HM9y!z0NWqf7tdWak#S|=+M}L?6)KGe^+_NYR}K!Iny}oUWo#6W zC0PzqLnxTQOUr%6f?uEMoJRqk(WEEn?U(ongDzcyZ$Z-?;CDNR&OdnsO(WLF;+&Wa zyQfAuH+q-ENRs29d-ORBNPTZFz2oDE|AnZDY1TTKt#7-&OvQY}8~SD5-q8WX~iLHB#>en9u9PB{9ihCqg{lVP}h$MEw z(v8ycaT>Sp_A(Ffh$d)BaQab{NXlN5&PuWyZ?DaDg#yQ|cYXS+0fKunQ@0Zy2-&zT zjkP(tKi}F%hCrJ>E6-hhV&lP3oW>Da82LqCLgU%wOAJx?Vv#WVPXx6PKF#=Q#v^t7 zo4eA^>`0Fd1nT#Yq&Xdc$pgn{Es~?ohdTLe{pZ@)DAQCgz|7##5)D>|X@vzW?-yER z&&=!q559*k?EbZ*{Pr6>zr_f9!AP#WhS#Za@ShYp6slbBzbLXEy9g<;j&9UX<6y3t zH=$*_F#_qTHw3C|+~joC)V37mu)VJP`|A*^i#R!jJdBl1s2wAp(z+&mxfyXnOtNWv5iA^j~X z8;MowW>>8aSzp3yRc>A3FoH+PN|gnnt*LJvCJ=r);*ZujZWo0PJy)@C7A17AEcw_P-^L@THw5xh&0YNi$R_X&(bju+z) zVTtOUxgrKT6wa*0ZWugcEL-6Z>ibu}f{u^ECT#i_A})6ak>ZzlUM|@3PFNJJcEjVo zuZv6KH<|GwO^Yd#_XUQ(X!R89F{-%MP*SEzVo9bTcndvwDK3hUQ;U769B9z?XjHjN zDL844e6b?>4cS0H{=q(rsgpvSUKGt^Qy%JvGXK?M`d!{AT;|S>;;`VE=7G{L!J|Q2 z)8i)p+f`4G{lp>EuWJE1WwL|YJrjkP8J$PhG2AAQF-@XBQ{iOCz>YGhmGRhz;luFY zd7Nf~^}%>t>BOq%W(Wsv(GUo0c3PZlBO_mRnhOrsG6UCDoCv1`PqyJ%O#L#v0^ttC z&({9*ce_}9S*sVdPHIVGhbqC-ojHi=lvjRci@T6sEfNc#H7^aHnPTsb^NtmFx!26| z<2b&CkqKfl`Ln>K>s?C+C4Z(Z;6Pj^KC0a0gRl;)EH`6TP32>?lg9~5^}Wqz*c}6L zM(HFA7h&`ai7e_39gKzlkf(j7hR_mG8rPw_ar(VN=Eg1C92_B`5GSD$w|mTWN!$E8 zYB35%H}+@_?4O^?VBf|c6fOLa+u=1T+3W* zhd+*HycA)~JN@^0OPjp)Ln%G`fhVWm7*;n$8@9V#3pE_l*!DPV%Yd@9NKfGvqzV!4 zz^W5DqoGF^ zu8UhIIuSyVxI!}n&HYHpf`>?kD8!h6OA{)TT}jG1e(eS)H(lc8nA1?JWR^_~dzAIi zUq37+Z{_s{C7&gNPRHu~WVPyjrR*4~p~&yT0>(Kv=ekAE%NHhSP3+pPAgjm=qy&Ro zIb@s3yFel-_c2Q=dyqn~VAKnuiSU8P7BZ!B%V@p)ObE*pOYkZ{lrp__TTt~_x{s`C zB7*h_G`isXv-M)hYIa0Q$7k01CBqcO?Awep!~2v@93!jP{s<$8Iek`F$A$D>m|KeQ z*zLHEVdY=hZs3+5v$-Ohhs}F_N(d~cWFE8lfgp5ijQr!=0_M5pauB%kwZSry;AW;o zLu}?)utJkz_K3?`it-MlShZM{UNs!6i1Uoq1yx}ha>GIG(TU8$KDW86-ZVXAw^u>c z4;)uVc_Z_l`UBi z{H^7Mlz=}p?W*a{dFD!``LKp(rmb{nupp!_a!jyh)G;|@{_On~?c!&6MXA^Jev(l? z(SWhClsY8x5ZLUtwWxqMk|WfrEX)*}n)8o(R8lubgpGjm%k!|E9@F{^pj019Hlrh; znI*+Vw=_V;nOh!08-*&wtg~()X?xtr#-|q5il5#iN8}q_wWn7WsQV}d(Hx1d`?Ufz z5J+64GR}G8j@tcPEJ{&oLCzqY|0L9@G){8egZk&-t;@_^5t-E)$YRAb;&oP3a?Owx@= zQlQq3c(gGgTtJ=fZ%f{W@c#66C6me~EJy|y`_{ZWTfiIw;Mj9pu_+R*Wd+w2W_%+k zflM-X#>r7m{d?mCQ0bo9Bz)fwI7v&;I)C~oQRG9wIOO5i>n0u|$hCkFJj6QUa7Tc# ziX*4n3*z=zzlIZx!u-9u0sz8W{`h-(;==Nyz1S z${t5-dkpa^5f2tfyvz62-t_J}bhodYfgTp7jlK9U2p$6wOn*JkirZrPHhy(Y>p3K- z1pELO2y#5hZKa~{P@M6v7Do_WugIt*;#(FYCWMWRMIe}wKQuehU@n|_-#Zokmt()8 zYS z2$76qWm`TX9Bi346w(T1xd`8>l z4mo71Wsy?r7CA!MOu7%9|2#k1kBgBwWx}fRA5PLHy(ft_C{!encs1}=R zaS+>UwjVNs3_N!rvZGzjw__!zGr?vylBcAmGL!lhGEq|&8Ss-I;Ez^>Uyfb zKUo0EHCb198hEynpCk}WQ`@hne5!*|$a9VQ|8d6i4ghHz-CE%OvT;jqF(CzyX`alO z`aWSmDag-nV?REZ0n6S;3&8LR4YT<-4FNYPjbHmzQI@Vl=MQ@a(t!_9kfo(oM1#== zv_fJ*8bFb5G5v7odTKtwMNOT%0=jM<-MMH;%;-nMrO{uJ7(Lxw|8Y3$5hviYd;k5= zL)a+Tne^9hzCTY~gqVkGOa1KX3e?0}0?8Rt*DL3#BM;+-byh;X%=D)0HzmxoDLNuw zO2X@puY+-yv!P<(+S7(-s)%=b%Dl~7t}PfuzWdYft`vRr5PnE{d8`D2zD)dFAQIrp zLzT-$p#%s4hSs(RchpxDh`?>S`=8cQ8Xk>060HY@s8D~8?+ax#99tc~NN{y6d!{eM zmLNEEEY~%(ybP)U_H{B2&|TJUB>%B3NIl_IG|1_qEXKI6EVLc%8n`F8`wwix9&zK9 zFAdX+{q5NArhr?ycObHiwJ8xca&mvKhig{1b7LiCWjszxOerZT+Y_V0<$(Dh)Y;qR z{6lkxw~^VAJAh9e-peH1X**op>%D6Vjv9`*)bG=CyoVM0R)z;yZ&+^tI`t3z9+JS> z;rxq?je-}X+~rt!DIm?}?1^wWZxccn^@?m&pTKnqAe=Y6_w372U9y{vTttuU#^X}| z=wk@L)<3T4dM@cai*Vt@XHDGT8KrkdzG_lB*UpUw-UBDsd_?}-%dV@p|esZ51*D1irK z1o2gX#r?R)ZPyXIviOp z7r6hfwuDt&=?`9~Ns7}qdceDxN+?An$<1m=;op-HzEN`Y)qPS&;{Yh-7`k#s!$2BD zlCrVIp0vGWct71&81^96{W6vuUrFL|pa#l&W&E0H{Zy2}{ikSjWpAD#Ly`36CYRy1 zzk5qTVPkD@y&OpI1kmVQmqS3-X32n(>-3rC2)_f_9p6U)osp>pvuT)$Vmm2`7A6D; z>uHjW4&c~dKqZ82c~om!;S0TYp~lP4!-N!3R$7zx55psjrcD<8!mNOPd35)-_Bejc z4+wv_{AazMTn(Rv@3J2RIcVK04MI!IXRgVz>O=C1jjrmSv^|$f@iFucFKFjGPWLIeEx#O9u~S?W+Zp0d{8HvWxIz5+M{<;k#q0{M(zZSbzgPSD z@!AFP;41&ex8PGqP0QCydT7S)pKrK<#Hh10>~~dT6aWLKww5FO4H+Keo3Luc`gRF4 zB8^zusXJ+Me7u;cDV5DaE#ucfilpC5UYJOHsB3j)Y8PXLH%XpgPu(Ndj2+^JuxQ4% zXgN_P1>n}>ZBg{0O`Ai+TL!%8bjlsCk-m&j- z6p}bHP8{6!`N4&#o3WFNX)8zEu3>E|$us+c6S?HcVjsRZH13AQTn&`$L-G-Lm?N~w z{-z7v5(9if$fx@*!0%)1J}22)owYm`wjtB?U0h`zD@BYvcMhA!cJO1;5~#@Nru;Y& zgF@GVw#M^&Z~k9r8^U~6w_3r2NJWX;kWyy((r{jmN@zpf^~<_SUyLrSD!R9|Cl@+p z!0d%TkV)|rS?x%N6QH#DSr>(Z+h)ES(rrG8#o%+OndCffXCY|bhO}VG*6?&cbdl@M zBCn<`WXu#v-##N-gF!^}IE6iCORDHFTV5f8gLe&|#7}Ag zEC!wYG%6I=T9ez9P*iA%u4kghiaedE2g{N1P20nih?*{VW3;Afz`#t88XdVAU-Ei) z%)i4Q-iR$e|2tD0e&TnsE3TbCN8>zAZb$qPUz9UPEf(ttwCtsoCpICLBsOf_?&N{`xh%4*Gj<^u_;0Kz4 z?vJNei%m*W-+$OTQtzDPt}^$OLc*Vp=(TqwAy@`PGIl%3HO+VDp&HunQ#YU9(iooL zi{uBfViNO88fYps=AatuUnsa*oYUkH5lSZZo<&y$4^1%cdFhCDh67y5WSSz&dy(C( zSyq;BsIu*rBu<`Z?4|rI1F($GHuUmkT9;;J>WxH1gIb5CM zD>zh%|A`r)0%SLf3tbwUsKsvcxvm!G+tG z;=@=qb6t&kN44Go%Vtn7C8EbaHuC7}2M*i9m9-qnfgyV!?QnlsWHssvA{q5Ywsry+ zQ)m77tO=c7WIygeY+3H`jj@Dl*jMa=nsOGmRrjJ{tCDG{xrGGX-f@n@h!oYfDUf^T zDoZsP1JX{9HhgGER!K>zxrKl%_j-7q*?jJPy{aye*(3ay3vxcGFiCh%M1%QzXM;~r zu&}V0n>dP!`cK-j)`WwJEj@Bi@b1Pdjo(`q$su@s7m( z$80Yxp#z&6F!BRvToWEW;*cKP&h?hnzNlB_iDqQB4cq)V=hm;rs+Df$sfABe2|6+v zY2MJ9VenAF*~<88r&y0HEVOW7j^%Gp^u z@L|#J+gwZ8DWH;4iZHD&q)d*=PXT8JJg`^MvxMYlIPM5Q1{&U1+~$_QhYHEaXk0HP zAaANxk3=HqwOFnPZ(mwcA^%y1r9$}nVPUac%em>YI!hF;i-iPEu_w{VPl^=J^e~f+WHf!)CW47h7Jv=qQVXdno{ zw6?S6^Bt?A zG7$JLw*Ym}$m4nY!Oc9$`1#*j05>=5xk@Q8IszTU4B~sNd^aHrfRwB(Lzm!i+Tt@B z^RO34f%|YP2R4HBhvdO5a4h@b4@*`j**fU~|=s(j^r6Sg4%2lbc--azl#g zw}|+9cGIYm)ZRWw51ePke|) zH@>&FB4cFS@Vl!d>*!v3>~|{bK%~jNY{r&^bh$}1=n8txcx3Zpsg1UcR@knd&D1fL zLbW88r7S_(YAuL zhHH(LH=QUeTxdSN=%}^V(Gs4dg(Fpr z#7-Z;^o63k?UkNf#W| zBGLagq*MXidc*E)4v+r1c)_I&Wrxf%cf9bMvQ^R4`}Do&`pjj}%FYv!KW;KH)V9%M zr6HHo%DrF79PF3L#*hPBK**XP{s)V?u|>JEbHz4^S$9%YBTtM*6!(x|TT3bJufKBd zHl2ms9k9y)%`yIFvXg!FVI8Q8;K%~jw4$EHzJLQhtrgB6#`Ex_j@MfMEI57-(;YM^BqSk5aTKWpJ^0czBR@ z@4$W#H7F%z;P=;TNoi?E4-(}os!1--_TC#I6NzvFbHbxP=hgbcCqRM`r&I!mjZ^9d z2HpsaV0ooBQ<}{a)2*b3q=z=9#ivcd9Erp23AfR+$(KMSyMF&12~5Ys8%Un6 zD71M$saF^VE-125?Aght)q^DRg+~8at^E8pO0YY%bMoB=TY)C};Xw)e(RlqlKmY91 z#9n$hOS%+PR;u4yZ>^A}CDTj14(7O#Kp5%H&&OySW9IKbqj{Ne;Py3&gl3BWe?V}D|C zrvH9%p+4i!_J{XC;5Hd3!9fNM~ z{~)dWBV`h?C+$jDJ{5^qUqr5`V*dxkAXZE>MU1BJ|-%?Z9c2=VlS*TgpVP1`Wx}HGS~K z!iNn<)W0DM#fN$%VggRf&XM$mZ^;yfU=0?FhkEV4R?e)eT00n+bv%eiiZ7@SC-6L* zU02sY{@tja69W0=>g;2%~D>BJwiJYHT+QN24A=QxnciW$bDCdKm&eY`eEnhG{S5C7T zJ(68hRR5G)%1Q=dzv+T)^1QC?nNnp`cNy@3^8YbYe_B8=;$2gOkoXrsuG8 z30*Fzk7j~(m7x>W-u4p(gmg4AS%(YnWF&uCOymWOh2#@IfwVGR!>Cv6dM|ZC@Ivu)9R0c%y8=5d?= zT2QYl@!6FbFEC)!k3t6bIYeb}iCXWqz5V;Ml^WKpLDscW?S|Oy?rxs8@fnW(VkC``ooDsiH`#vE~ zxQ|nMk-kj@bjpAvSdUgk8abkZ(*%t(Y^6>P@X>Ysz(9r@uFt>6Y&bAln*uAGGEBcc zBwrSKA-Eu$_m$6(Y@N+|>`kzx`0Fv7Uh};@>b=f(;8C1JxgL7XytQqKx$S(Z*a8JM zH?pP;5+BHQvAmNa>Wfx=G2O-h#$L9QzWP7#mJ_7gukvwY4c3^5cMI2~fso90n?z(0 zq;d5sO-<8Pj$3shWRnqcHTUaqqN`Zf_;^vOYihcWg|a^VVm2IQO+t?Qu%aXLWue~* z5*jH%ti80JZyIL9p;u!;x~3SDH}i?lfg7M?)zjw27|kGqi-3)Y;?SLWpXOp(vTDyH zf{PHjz-m7(h}=Sh@Bc9+H4_ke>gMEnFcsRIh}4@Dq|IG`lzx!IqJJO$Wh50mey31Q zhx1N>^s3Q8+e1#YQ$~r!ywJx6cVo|f1-JCpS%7&+TQo7hX~MWq)>LB1c%(8Px29dz z)sVi_ZIkl2l`Hha8zanqoVvUboWW#{bCh;TSx5eR1@|C>B}>gVs%oZ|3^(6s-YWIS zN8E(%V5Jf_?&HiGQZIra4Hw}CW?!gsuz+GF{P-E)~M1Ua5v+lIu=b6d6jG&u+#UDjf(lz~?4*4$3dS#B+5_I?OkH&Xo4?6t2~P zl?7|Hg9n*EWb1OR^}2^$Zw`*yZG&l^J6f+QIuqL0O7FiOVGYV3h;=v2&Z_5w3Cc@x zhyZk>^PGC|2hY0c|0;;Ge&SiMULioZpu7*06k>I41Eg;<9A9Du>qT*+3lJ7jePYyk ziYX+a4=%dODu+1zCo$Aoqo|^d$g1m7P4?D*_xF-SfRR4kjp7!?Mgwf_ac;OSB2*hi z!Qj+h{KKe$4nkXbpg%c z^=fDxIZbXrL}-dXvt@XH9tG@_uCh;@xMUPQh2Qc&a20c5O&KmPJxV=*#~KSf|5aF& zf`-=h`{;FWb0MXe^vGXdF6gpm#6sdiar-;hYD?+LNWWZ<0P(6vuima}f6h`5T=OfT zZ~5PKMWIvtz~>dH>ig3r`NGp;((T?$-MziB2?^fU^^j8u-MkP^=z2U)>K7&G+phE5 z?!_%;V4J}wAhyonDra4j?CpIjSoEJ#E+LX4il@68wHP`U7I=l)XbxHRMt>lEYTluu z&fKHXLa8z8{P3G?SzjpI_rU4X4g_6)D<{j?EJP_{emUBL1ikZQ9^(3NvD*x-9zVE> zjBHI7+rSQVPNqQ07YR@xU=$Q-}C_J^g+vtlubx z9`g3i==3UhH^St1dV@qtsHepNs5FE%CDlmknl5WyZNqC}Rhu@_lt15KuVcFhM-8cS zgsFawziRjHiyibG7Cw_O=Oao18(dyL{|ycllumb>2zSbMQ5_MCUlZHw?zWQ%M?MM~P_xXmwFq5_w=4XH`2o5ag=R#H>;@8#TlYB=uK&b{oAt0kKMiN{d z^ksnQk5*&h{a1m0)rgXY6!$GoUzSJb0Y2X-ZqzWAg)&f1m zkNLn#zyo)&Hcvc%>&mE~nr*&RJ+;f<;<-78m;!KA3kdoPQ`dZydOd8!7^lUbeK=yTbFRcS z!!IQ&nl+Y|OX9&J-z66nPWn$So*`MhEYPRZ+FPA1y}_=`#4IYXukmR2#|iH7Zd}OT z;YR7!74OB}YLhUJ=DPs3T-z$9MczGw#;HD&r+Nb120I=*=KB77K`yhiu@Hh}{AWy( z(Y?=D?Vg@UCnL%I>r-0&Ovu@$gH{brLoJuGQSCm-8>tBwi%xduQzJ%6p%evAL;~Db zRI%aLYkw*i_F5rrZd(>sjK`vlrP1X6cHsajo#J{rIy%l)np8n!ok<->Nwt0#P-3A~ z5~{s26lNa9QGhGD|LPeJL0D&3O3*SawFXz1uQ5~+mA@OK8)x)TU!VR;tCwtsK=S0I zig+Zx7zA1+*P6TwCu4&-SJ*M?V2$QLPju$|KfJwVP@G-U?~4S71PBn^f@_fA7Tn#P z;O;IHJh;0BO>np15(w_@I=DLw?2G$;-nVL>I(zSPs!pAc^JQkPb*jaCs`kaI$6!(?~Zx;PlRA4D;!9cb#0KzRLMEt;6FRg8T$}BQ}&Q_r0hBd zAL&E*rZnug9iS=8+}JTOK8WCYX7U+BJ1smcLik!45)VmyLSC#5cN;+?CZkQ&s-Bl> zhg)X81KYgB;L^YH%Nz%8+SBZgPbqVrRui{Je0D`TQJ8!-&vxq3%`wd{hutd*t0G># zb^=+qgbEM?n(Jgf$=@5C`o1p2YlWygVa+M=k{sSOB?;&-SlXOQ_5Bptc&Wm;@ZImN z5cY%sc@HD5bGM@VZ;Jet%udxi)mQWDCZ@bk*dqbrqoPg4q9u9s^lTrWRGW#}jLEDo zoE~oN%;hLc>RI3kSkD&Rzec4YBlG|JSF-)(+56%8usSQuy(5Y}yCDVZWsBG81G2wiB^bWI+h@H>73Ohpoo%uNi-RBQ-C!QH`oU>GK zEEeW_i|i#X&Ql)u1PeI4S3$v*@&)JP)(QJ6OAj9%c#hb}afn=BV%?k?nf{l7N87F6z7}Gor+<}W z7aMGKej^VE?y{T>dV=jV`C za98{wNntAEQ*U98t2q;u(dai$*fNGYaFhy2DNXZiUnJ+&DqGjED+6v!y78QJ=nzbXi$EKWyt z{>DSW5w_$%fD>=1VP0ZK$4`*t^3-I&`Gm{T@r&+Q>y57MqphhF!3tiV zV~-~C5dsn3^_kF#Ea;kG$|u>Q`+0rkbl$0s*%dSL&)QE(O<`oY3=YajalY%loaKjJ6~22vwxZjT2wIjvtSQ!p z7q@hL&H_x|FMGJckmP#91<-<58HsBNxA!vo*Z8mNp|(e3fnFb;5`z@$E!n=_#~Z}- zvi)-XoO#+7x0g87SUN z$;ZEh@(*6WXH3KG4DuqsIdw^0k?3F_i>zbjA)HkQow9O8w^aX}YcZ}>5B@xi>vO?> zXTA2)qC!Pg_~GTBo{+3T_L3?9@Dz7;ErZ(DHbkC1H^i!Kd zm=xTpw@yB!VsK`ptfdVFCEW{QNtHjwdWmozq>|RnUh{nhqq3Q|4;=Nc=8mfdn#qm5 z$cq-Bft#y6`7Re>vY$$zK4I$RlO1c%R6UQUD&u~Fh>?RwjQ5|YHIq<+sAIiWxGLI; z$cj`D<}dDjRx>i>pbaCfMZvs39RM!!lj!7gmqgR~xmN3WuOtrMSIbFn))a&EASueX zuaP66PKQqf9O|rM%{Jo|B&Q{uT=9&iotc_Clu?I+UI@t$@WM-K{<5hCGbr;^M61ls z{gN|Rcv=+5NlwmnrBoKbv5kjtOBUOV!bmC-sGpzSCxlea`Pem=$#B}AhC;=TjcUNN zZ1=de-rdl!lGB~WDZKEdt=`(_-R5yN*XABziHSbx0c-*-E$!+mTAcEu+3|-u8{hMV zrPtKjIB$qbY)%Xu3J|?UEXd|A$bRt?05Wj1V_FQoy?xa_3z&8Xg5w__XA2K^h!-m8 z|G>rEl=r&0R^PFWbY0q?bW7BuT@pXinUpieg{MI*bTq64X_Xz1z~qlk-<7p=$NZ;H zm6e@t4omV%0a^-l>@$<&%|JRm(|z{ z?&JQj@C{1@a-poTCGlDHMryp?=M?=n6{~g`WkQn?ve#ZLrjEdfHh8GGa_X~lP;1+V z*TAL%FZ?-e%&$}Ns_z@~j2xW@7A@}*5>35hIbU=?15qVRLapJXjz(WZV(8BjVD=x2 z5K_?~6NZA5`i3cq{KJ%W>LEwj!-_OrP9s=>F7}`1<~ip6d2Z$Ks12kwd&Ui*5b0Iw z{`X&JWA;VcB|VGthMWNpVS47itIN+<3yi#z32FMy>~!;pb7z)e@pui_HRGn-n{ZSb zpQYx0ugz7=QMRc-Xz(*rjap+HU*%uKQeaFQ&ax^b9tTDI$N1N3bt!yQ&CV^g_*p?O z<}#1Emyc5^^5J23z@t#W`}uiQ+iz*9`80gQ zvsLzcNT;0Ib~;22CamQ?0lT{4KT-E7eMvwaR@>*L63PexQ^xcwpQwV;ggZ;ieKC;F z!?P4C1}Huup(mL!Lyb`j_3{1PYbLDFBvTKq(v>Rd&)j0ASAh2KaF1jNPs#9sNxe~$ z;d)7%wdWh4jlk$;i-vhE&#rnOnvG~`W>b=D$!%yG-7^0E0XzDcMcWU$R zZy-y#gNE%EYSQZJT+|%HJiZ%e3>fN}pO{UT$kio=mX1?s&ZT>Z13SXs_sD5xa%OH( zF8q$(eP-Y$hZ*G&O4(MEQnOfiGO3%e!S5RGWn?d^xrTVkDV`*!IcY-B&7?0+sV=;v z41tMSh6#b=rI#dk-^GGBv^te}Y^^pRG-QN&I~z{6quTA;Gt}!AWv0q z(higk1SGJpw24^ft%k@lfTKO~D`_x~yFwWwAoroVj8NqLqq>K-H92;V8kg2&n#bs3 z)qy*6+{LAKw#)xHg8x8J;u%C1L?<;+KtYX3v{{TWu+_*7+vMy?=-zgY+*puGD3CK` z&-`{{v{4wZecKg&j=2kDze>`2y{Fl}&yrI(vK3xV^*MxEk(x4X+-hU>UI2*DBo$e3 zBQv=?;{i{--rS$ChRchpVkApL4kOrD$_&jow57us2 zETM?XB{O-xr&jw~;$h4=manK>;54t1Ja6|1>%NgMzZ}5~1ftQZkiFNG#HQyB+xb^x z<5$|GyQe2CD(aOsAr$cWWwBy866O)0!Axe*=JmN$z5MId-h|8Ec&>|kbcKX?srvL= zXbXj(4WAd*z$8zHfk2~N>h6sQ6z;QgQR3`tlABC+5et}Nemihn=(Bi_By4K!z1Qqk z>)p)h(WpQa+;>T_o18PGECoII=h7{Y>a>>fm}CHcE~+eXzi$w zF6Ps%X|mnnFr-*)Y{n@d$H3t4?%zexEWY8ctzhtp+y5fi|M6=AFEn{Y{?>H%_HgBy zD#9+xe9Kya<#bK3cI(WNG&2Nt_u`^+h0h0)fJ;^WRsWXju&zjj_inFnf#Jae_R*P) zyqdA6h`&e9k-!78pH_Bf$4SkiT=NSVf);CS++p1DBG$8fSk+u)eZ4jsbDs_U~qjJGjPT(xZgKFG?p~4$H*$E98dF#mj%{ET8R7&FGecw}TttOma=-3L zJ6^!NZhddpt67MC8F78F6V{XEA`C*gxqNQg;A?Eg;Cu^;ABykca@?c1w$3zzOvRi( z%(^Y#4tr?-0XuuRzom72oh>R_unmkKEXE<~0q6J$--s(1vJ^ z7ZQ^&S5;jKy$x0c56@CEtR{AL!foM=Q=nSN)usyG?^cZ6S@b(h66kQ+{;MivWsqqZ z`o7n1B+BB|4LY9A-r2ivN&KHv;ox2A^q2n7Pz8}|RGx;BAMHr#KgnLbTC1oGcBB4i zqSh8CZc9dOCBbHa2_-cy$0x3|G*FG}WhKQ-iP+^d`_1Xn*6|8|3?%9J0am@Y77*t~ z^%2(x4jB8-CKg&H4j*|enf*ruQh{-ks|!NPmF~}fSiaRwR1e<*gpuSqoz>d{K&)$6 ztvdmtwnKjtb#x3F)=1kU{73~Ik3>v%QOHil;7_pGz!4%fh(;N}y9d)kuXKg(fjF}SkT6Kj7(pQr- z#6m$k_S>2BRB1+-T)pA5nGW?c$K2<6tTjJ1>UrO++T;p{XE7G?uI>ev^1K-H8|%>< zuAA~iZ29oSOdvxH#N>{#%C3I`(&G72O5*Y3UflNq?7 zG{R`Gh^P%$U#zCrvLp7TMEme^v!_j&7c8@TNeBx?&O9QQ?-iCAO?vfoY|E}}fpaTF zisz$Jn37_5zgx4yD+*jg^zodCc=!6Y()jpsNb6?yO$d`LI6B45Nk^qW`S+^}{U;(K zhPvHK4Hdoz?!_ldSSoH%V^PZ;2t?G7!?1vi=yayksmkL_r@4+nL#*gACFMAv`Dmy0 zvs=4vH=j*7j?!Bkhc#`@2-Qf$5Tg?F;U)~IKJZFVO;CJJTC_txVK8IESsz|SGnxE` zA>ut%Y)9h|e{?{_w0ESPhKez5rw_XdiDr~|RHn|oqkkCr8RHbLXeAl~1D~gZmIoR% z)YmtrFg6iP7l%rAC<*$aKPq*BlxsC*)_XzOK^$hTcxa`2Mq@538r-#?|Faz0ta!em z<3GvyWw>S&3m=u=D-XCSFKq-W@XRceXI2@m2)tuQ`}L5ck4}SKr4Q3-7?1 zj-1>yF)d9>S~|SZU8G>TUF#8!QOCgN=H^3=pR8Aex>zlFlss4>`_bIwG`qthJ9#6{ z_i*z10(m9-`rU~pV8l$ ztplzEvz-F%3rugT_}%N(a6q+gBN{uuJ2cEC;dz}$E6X0XnK$DPmH8LD<7{)P4Hx{Z zp2`uT{v~+du&YUjCq}lMRhK1w;pXNBI8Ch?ZF2p*@PSL<_0uKPa#xltVk%tDagc(cWK3j^N*f48;)FCqmAQ2rjb|aB>o&zL6`z#kb zcmGfJ-RGOI6w(_&k9v^FsBpGs5lJE=*WbjIeXuG_UaXYWswjU*8Y}N!rtCu$d%D-kbvPvu8i2!3AC3ED!8rpHaH{)@EWLy?ixh zoGs}%J^7i78-?_Xt!CSRALF8agw2cKZ%v`6FaLKr^JN;YC&r;JFGM5SrWy_ch z(7Md32XDfaj$OX*XX8Tt3g9mN-emp=%gV?ol3Q>GfZ%7dOI?oSt2fIwi~}wK620DZ zkHXMug5g}?17YaLv9Om(Ldh2+6lz;`cjbS6H<;T7a)9!jF+otdze=i?!k)ZS#jjKf(e>T|6ZH)e)wRR&fQ>#|M6!K z>wV89>_1~_&TWw?|4adLfcYhH`RO4AUnU4hy2X>PYGfNK&%}^p59y)j&B*{Y9%!Y} zzW)6)=`O8*6$@%SDP#FO&Pah8JJhOczuX%U*qD^$ZAM=DuL6i{(ct(5_w3mt*}rjTRGRaTMi@XwPmZQEzkM&@&=vg3@bUK? z87kLhzjAi!#alshVp3|7Pkj@=4oStMf2ZM8H>3}<9U0Vq0~`3IyqpLGlNCW{JqY&h zb$w8pK{VXGO9mwBOEiJCbUFWGs*!YUF8{~LKi{>B>#Sy`c~ELsOm)+pA_a`qtG^Cq z)mfcWUlQYv@#@ra!-~!wO0FT07+E}8WtQgrPu%b%<~Kpl!!82P>v0xM%kyz5mSO~f9fE*$f#4`R|yZ)a=QKPqPKiAe@Agkg~>kS6CEp^TM$%!f( z%>MPjWBZ4MwU_LVb64y02r~0RI>ewGqlyxjK9VxuX~FXK8S!fAdnU}cwx_3*;HQLyJMTFuFvzmJjgX%2k3q*# zsF$xKy{XK!$k1tC|Alj7Df_`_8yoTN;DPusb;HGK>j$m-WBa3l(T@y^qD&aG2LsAT z`DO{}XVJ&^uNTtyN+lXR!vD6%K?%&&kdAqI^n-#Z$@^xjFM8>NBum zp%zh-k>=6!>R2}149q|MQQ@8LvQ{m(!rQ=J#OP5lu{Mrp=$UW*lMQrxa z7=_1fMW;vbN2%%3mN@Go>o`v4n(G7Kzov=%CbxjD`Y8us{;cYEKu3Y^)MUaF^#OU@ zRFHVlfE}HH;OA$$O_AxzYhmEjw0HaYE_Q@0`cb(;`zMCX!44D@P$VKQg&|3Up%733yL^)0_1QA&4wBhMC(Yp%lVn9kpI3bRX_#9 zNU4}`iAOtM)!|8NrTtT<(LNxM_@;jB{KIQJ$q$dC<0@felL_F6y1%|3^w(643@ewK zVys~eGwRKRsQF|;tbZ|P)^&Un+9<&4KlG;%%HnaQa|u0_F9_;hTqBsIqz&X@ z*fxk*quzN?EeX!rjroYZoWfo}wE_$szOIhIz5bQ9WN#sNe8xt&>Ka?3u`Ky`K&5Qg zno#kQ`S%Ke%F$;2bKNhz!D=+`0meh)5UO!@%7EqT2+u?3k14ZJeXZsO6F!oq-@R;q z_4p(YgJSSCX2?%Fm8{$kr69;2gRfR@lOLJVH^Vn6=DMM~-_V%XD8EyrcQ65XKZSTz zjL}Vu1#nyp8N)v7iS3HsKW?uilp@W(Z}cUx?J4i5hSD8w>ybjyKjE{eNei(`9ZVvx9EovwBJt}HOc0hXlA0NC9WYCMZB&AxehQ$6y zL-1kFQOR1Z_jaK%;%cU|M+*vRZ<7LvuEAk4Wv(P~`Cp{hU+Z6_7gJP~R_h#aldfiv zNzQ#sy5oh`?SZ$HTTi$7NC@rmC~h9;O$K&+?P zc%m~oIXTwypc)Ql@=+yVJb$w^mUL_`f2ih@w#70@g&Q67Z_acxo>y6L~kR9U&zm_^RRO`taqELUSto8p-F{0!*B^u9-e|9UJ%e_ZL`8MnW}iO=0{Y{?D*WR06gnF&yb`TCI=UboFygZMEs&1=;k)W*gJ zk9znWE>Qj75`o<7{_mI2{#+ImG}})Jb~ykM$>pJfL9C#6r5`$mx+3h26%REdvG1Ca zfk8i8&5c|OV8@fu@4q@3Wk}D#ij#VGzpRLnOd8%Zy;5A91-(FCNG~0QeiH;Fg$4$= z0qW~Bv^8lnfTNe|VUPg1-$ese!^c#n-AmN|j-eqD{{m{w{|oiIv!uIGo%PhjlJP*a z6mL?Js&~a`RwDpPTzam~(P!@HK91^XZ4&`(0i`*2*BFXf&H7lEOUoaJ4e*#r&B66f zrfgFhP?N5@XPv#HO7(JoWS#f>L$Wn5=G_xnVx3@|U0Zp=KISr(-FIGKne?<<$lC{e z31*%9WgqKNtk7sTOOINxpv-wA0p{8gqK@8w-;8|@K|KMAE}>4B7h>?1^nAZydkuSr zB9(uXY19)zREv+7l9H$3DQM-j+@1ez8JUSWxc!cvHj?QEaottB(sPrs9a9>Jg6rA<8cBkB zN(^X-TQk3wyqm`CC=n@06%3_Ba1s7cEmr$kxcE8hgetu@yd|WO!cpQsJ zx#&D+C$&V(7r%`71uZ1d!Xf6*l3irWv=P6TSIONL#Yy5Fed~9Z1xr1G9^dfqrt2b-OwH2W<^Hsq6}naXX(cD`mS3cYr(13_w&pG;E;X!?d`fkORb^Vb<0E-T z!qt#PgbQvteiXH;6 zj3C3qZ6`Zz>)u5g`3mL@@)}%xv37T|=1Ana51yiNNBHaTgoi+Q_SRA~;Q{VY)pAPV z{5X%hY~CkTM$4-8Bk^_^xNn@lPWgmKi!-KXyZ-k6d9Opaxfm|9c=gvug-AT96vGdR zog;!&mwORl+~0QGkf-BsgcVQi$=nefXNPN?<8<^&EkoABzT7ys@wz@Th-7pywwS!& z*3mqJ=S|&7Mwf3ggmN-L>Pv!nq;vTc3*LlPu6MMZRJf^%c0&(up?*@=P1 z_2Mo>H+&y?X1k@J7tA%-OgixohVR2^qnuhuO$f8ToGNupdERS_!!0Ga?y9V&|Bi#a zP{b$0a^0b?R&eHL?~5gdcA>=Gz$3J;N~pK7)ZL8OM*NUk>`&Layz}YDQ+AP8jrk>p ztjKuE)Y!30E5-l7P2y7(VjxeQ{|9NUNa4JzZuQNt_<4SJWE4Mv(S-t3p)egGmEUhY zlXH))*DVfGe#x1QU74r--XT$T651yg&4AwWXjXV5|3Qw=JdHYS?v%FOo;4>bAum>q zhCbx(&iMY=M*1R9)98?Ykm5HwrAP;dyd_~z-czbhiiG@P6=P9=6y!D-{kHGZx7J!T z>jAYZ+)x51R1_or;oyWmA}_l18dNegwIiV#T$eQSg?h*v7lJYG0q-bYG3B||B zyi*kRilZ=Rd2*mzgyBvp8{)zl6XQx$V;<$styZL$D}kS{fuY%<)iTc+5?gY#aroTN z;e&%fa!Z6M@%FwkVj{ff2evmYtDA#p3Qrf_x$taW&lF#Ev41%I8IzKD$c2j}?!Z_? z17F8=(Y*IFS;&)$Y zon|7=?JMUCX0I_>Qut@7OlB~7233@anU9_q|1FbTTReA_A2dnyL+Ef4%HD?`;wjf8 z2%r~)7UK7wmBE}8Ih!feq*gb$yvD@y z1)X$a5(yDA>-?Bu&Df9hiQ@qez#q*L{K87?|ibER2-{uwJH+ zRTb^u$VP3bF>1aDIc6j2V2}cd@?yyaG8vi_sg~?&l=f6>bxT)o&Ih!`#*s&mukf-$ zwduUVQA?_;IcafQcZ2C#93}*QNW!&K1u1Bb;xdM~wQc`hh5L!*!%K3_XU1@(A?nMj z7^8zCN+c-rl9>{bSe-784#=JiCTzCZwY37~FZiz+iV8_WYo0RqMvClhrQnqYWuo-@ z$br)7H|)|pa6PSz2+gzQY^#H)&kK+b>G-~uiuWg&9g*)3@9w^(b4;xo%gVSX3dzD6 zRNVUdtI@w~Ps|bI8~E2stIDKvx=MUOt!9BXx$+HGD~*}3b%M=bn^W`Cx}Xg6@6o!P zO?Z>I|2s_>(WvF9R2<{|3Zgl_Qc9bR=Z+x0`>}8az8ktq=bcQ2ht2P_S7)LQ*(<}*sn56n5D9fsaNps(p5Jrx^*=PPR; z5o2f=;)ZvZUsr`P5uDO0tssc~qi{DkCawO@UXggENQ<3vt(%BxnY{|#RJqWh^ia#j#8!NfXkia|h z%i%pb=ZBi0zNqNhRa4bEdSud~c39pHuV5FIcX0{AU+J3WY^7x(=$Y8)B^~k^x2^Y| z^Tcf4XZjb75U7~ikgOWqP`=a=$RG1^{OW65fW6z*3rAJ2Jq@h}^swMe#Fu68Q3PY} z&qlIYHHk%omOBll82xMA(~}?yqxazi($aS{L8h#k=s+UZ=<+?y?BFsk-PX@f`Xn3- z46VW%R)|L7#ZZ_hZD`g~@yQj!nvIExiA(zK*QdoR%geS06tKX>2y8#km^#b;d#^5X zjw6At`vTb`y=jy6zvVVWxC;BG`s?FbL<{>W9(5y$OO>T={w5=gqc46qR&Is<<}9#X z!_G%95)tkxxFtIXZfib@d3KPzvqA+hd{)0;)e##C)!M>SHcn>^? z&Eu%W`3Yv$oR>NxnW$9l?L|*kM^k1A^pVhpD7HN{S6r`L6ucfw-)(1f)|pO=|46?* zCPCIn&Kl7df@heKHdl&ct}m=%Wkieq^^N>B@t2%V;qxsz6i<#gR?(z=60RD88g83p z@}^vvt8_FkLAZ&;)AlMTW`vunB{xhkQXEZncaAn0|Mn?chb4YBl;nrRSpWWn0HK0&C8&f>w6PSf_MH=z znmcf+n@9Z>z|}5OrkIc4CMd`ougqRrt-P%aX_aL;D@ z3pz(XSK@RIrB%~Seo&0CCI`fk5C?}W;RjBq17Q-2Dpu&TeNUCLNVKU^T+%jAtf|OczU^Xf_3g!pMEB)OSsvA=^2xz;l{d3-xlS~KhZh995o*DS_AKW-q z-sQ8kr92Gnx{LTLL;o0A>myo9Qbr>SqWO%>i*Pg^pFi`i8(Gx~+oFDtX7#S3;+*sy zLYc`((Z_=6=7FL>?K~>>9vP%C9wNnzjmy;^NgL@AG>#Pqx_6@hvdPY)SbV8Ze-gmt z&OCC=$)iZ4Ml&9pq_P*WlZ7R%Dm`xLx(79~qi4fYl-G4QvCWCANCTBMFvl^q2dAZ3JV*NBX~>wYMy5%tQ5bD!mvL8uj#L3iuv0)QU3h zh(w_0ITXzn)fP#;w>AaJi)Tl$Z@d5VMzB^xW*+EXM(SOXpmJpxAlNaoz2MBf3gSKod zx<5kf-Et;2mr36+Z!PVC3>qOe!iZ#p@{axPAYvcBNK=&A%bs_5gsP3}S(*xoG{1Bk ziS3?-;|_^YE!GG0X4Ys4&r!}pm~YWRTaG2(*e|PAYY%U@%eLk#*i6{NIx&R20|h~H zP^S_|B?Y3U8rbe)RA71Pxc6JSN1P%HA8s5e)TBlb4UQ%pEWuRKOP{yPsfaxVbCAcp zay_b&%0unO+iiEgJ}*n_!sv{f>pByAT95^bmz!U2kg_pdTdUJj|Jz(c?fDJR|W!0Gl% zMWIGh5T<9I#r*g6&U-3xj>ko=X`BzJQ;tCN#iTNP^-&o4m1sDhqzjQ z%MroRcy1Hjaa*y}7Lzl)CF`~GNVzre2t9-)A?n8L|(wb4m^}bmedI?qM;SWHh*eqwo3$ zZ|zI4LWUo^-Wk60hX9RRG4`@VorpcG)tqNC!K%As$Kj^woLN^s(j7je#)C`Exy43T zl%@pOFZ>Uf?MB2?26{rf35@4c&qL88FEH0AZ9luqkrb{=N5-)eJXenKyk^}uCP&qV zS4v6EgqaVxz6ldarG!`&{gE@xRRN{n^TsbHHsc@RPaO2)g{e*qtU_#y46GK8}d} z>!d!JHO+OW%<+y?CCXhu!(Z4^ts76wDA>%VizRA?`7F7++8B&&C~1jzeA7IeoG*Xl z#z*ghW`yonV#*fhzX_m~Zl=1X)2D^J`&A%4L~X%lPw4!UgpA&h#E3drNP;z%4O4}C-G3f`1%`@9D^~G9vr7~nXX&Wh31@VVw6RJkoP3^ z3nX8I(08R`l~@ZNlr&+uvxCd4`Z&rc2b!=83N^Hm$Yk00?9g<|57#~>i&55{dsx{T z7r$Nz8^D0EpR9KfPn%TMT+SN=PRUYjveJHpaf?x^FbsmDRCA;2YhJqfYz%73V!?V4FQkw&l2g zthlij`$(2ngvR+|jOF@~f^1G>>a*%lz^r4iL)jx&Ao(2ImZU4Zd#OtAb`_r6^Hpj) z`b;Z#Svq&N>w=p{NF8_q&e*f{=D6^O)$>Ni!kWV~Rnwa~!?VZ~T&%BjaB7>xkE|qZ zI>8Gv9VQR-^y8b~6owtT7o|vPgV0U$Qny-aoK)alu1vfA7WKI}{w+hACL!|t8Ie%r!#hle&lB6=9ncbq0 z4IO2nwKY1&cRV7x+1Ph}{Y2peRp^ago}E3I`XK*f=2VI!+_7F2kXldF&Rxv3Y7C_GI=ld-~xO7NJNn24yT z45uqJUlua;S-*9?L>Nt&Z+&kHEqQKnyo?#(d)VX-snC%Z1a0*)WdvaddN}E!vvIVG zvCS!anGSJ}QHwmBJ9dWp@Yjd!BM*7!ee|5OzVhV_4Le*dlye;YN+;6dW|Qw1*ms}O z2x&??`%8m`42K-ddRp2Hl4i;jl#$dFEk4pYTa-77@jow7;&3y=bv&Fjd><;dXU$QVntG2PnqULwS9LcrF=V_+Q<;ytOi@6t&3Ys33)Qm{i_`3qqCN-gY4q%iy>|p^S@n z)Xe+qgTw;a4G103Ll$;MMg{8)ol1hV(96{VVwM^Ql7$864S%Ze^U2j^@DXCYlp!=_ zNkB73K$4-M$NsOVVIIf%P#(usmmuf64K108RJ>3P#r`B$>mD4{@jzG01k|cWk=sI-cAnBA3asVyihC!hW7CsnoN8pX zYJPxw)pZ(h?As)Uo;t~nb%Lwp9*mc&9v1P_2n-l~ZLss$u?P`nLBP*Y+iEg~K7S^N zHP%5S_#s&Rk>=N1JG&ErP1w$fG~C}Rt+T-fyrv*mCADtBCVkJ-;7=4|QekxZcNY8` z*+zv0pM8Wdg|T9cP&6Q>)`lg*2xZPb*m(u}F|d$xqr;i;ylp80^&yj!d5%_jM56^+ zMly%9bw8kS!bOZN1c|gk7n{+)03A3r{uWM7e+LJJOJ}eSZ=?$`y}c1s-7b>9AgV;p zc~s2|oChef2Hf4{bKG}Heh>dU&Et~HUTt~u=ADd%HDeIvQ2AGx>b)%;O?VZ?io)(? zCp?lRQbxVtDwjTLuT-7^4Uo;@z5@PqXu`we0!*477VuPbh;s68H7fPZz&AO~F0>d? zMSF^qW0gF|yaJ*-gZidbp+Ky%RYJJl5O%&i)WFMK@q$O0h}C_wN6mVjK(h0BgKQ>7 zIFV0A@LVX7QN>o%G_qd4?{E^YQjj``M(_cB(jl`gV{9y>zv_Gdp>Ixy0Er?ZKCaW9 zzMZ`I+94a!nd!8xnRy{>>E1*HCKr-J(Wj!d+M?1|(|p$hgF}pJHr-UAn$qDzCJL9B zlr#X$QeQvR$Rdup6)RT8_VSJr?^#{xsPSD=a5-K08F{0OIJ9$Wkl(GbBS}LWbSy0; zMJDj9nHG!#91+olcmQu@#TE*vGphd#obc}#<~%{IFOIW!+pvGpsG}638*ng(u)d@? zhTG5DiDK{tK^y&`IVJl2ZUNj-wOi|qdMvQM3M-`tAJ+fYp^qukjO=9DpsaXjadrUJ z>sHXqr!L6I_2|^vI(W+AcwE6oaD5nEk5(WOU0Cx|x%$w|P|E<wNj;#W(3f@&-@( z&143Kz_$0@7k~PP3eaBvaXA|*qjZh7atTRr6)L9eqJS1c%_d!0VJRV4II(pSnQVjw z74S}l#zPZk$A4?VNYxeOJVgJwoIBgD(kkXLs&4bR-vI`jovw)V?40(oI4lZG zUau*8E3%O%k3vEwd1q55BhOD_Jwp_z6bv=`xn_f#GJKAX*c?#_>tL>bDQK>CgiMfV z!Jm`dnD!Py=`V35n4S2&c~f%kShM>}1CgvORJu@DE%KAHLb0;e^#GUf_eru`z$Bge z!Jq&aP4|V=Cre&r`yRT}2|Bv15<2UOUzpwTiLQv2(HlsJ4-S(KD^fCO)bu{2#d517 zb>=^7R|+X%4ZPA7nRzFlc?aTp2b)U#BQI7(h!%>a3f})+x7@PWOcz_6sU=)&4Ug$N z-QLzI^{maB^HrxyW^f%r09p;gMReSUIk$x<%Gf&WlqdMq9^=1dVVZxxih&PPukR{d1`(q1Jy8KSn# z2ytWrj!+Y4GNy2P{yXaHt1S7e4GPWCYW)q^fimY7?#vURA==6d7q*nv(}s=Q)|dSj z@4hh->A6Oxgc7wa?Af^7t^NziSQY)5@6p4TOPV(utQ-)(>Vphm&o2MF>saGE|=B z1Rbhv$A-`I{V7HL{{LX@t)l8`)@@NBA-KD{ySux)yIXMg5InfMyAy2U1PE>k?(PJ4 zKO_IzYqxVw+w0zkdwH44(~_J5{uQi=&*vylX~`pH%?=}p9g+m*8&(3P^gKoIX7!u!NM7%bQoZyYr$v3ZmgX7}sN~^vnOio3D~fY0U~o zldkG)I5EK~%ppb}sv%8PCw(?V9ZI&EcGhDHI+TqMum&$0at!h4_x3ku9t|PVX|YBn ztL>hujQrkuU|-@gL?eGr6d?eM-}w^#7O(!$awIYP;<L^jgZFw6N--ENvMA+7uC)5tF?h@k|*+y0Y`93-6XMje@BxW)6*GXbx^1^JdM-> zneI_U)$xHY_@6^Wf88UzlDePHuC8Nz;3W7^P*RF{f)*NRY@!j{zm-&FnRUA5YdYwt z{3C!wdPx;U2nJxe?GOR&BsgE-*FOSFBwgu8v>(Hz$mYE)ALgc`pkC&1bNEPYf9$<_ zH+E^i9(niuj5>m3^@y5p=zM#@wFm}D)9EgY;vfH#0Uy>bYXIk14}A)0%`+XV&s+3lk%N#S2^l8CNCZ?T;D?Ul%!_JssU>YH zGwYxb1&&)cNKy`l)=LZmgQ-y4lUEirW)1PujB2fy8yQAGZUDSwS+_~~7d@xU{KgXZ-OaGJ$ zi9wFYVPza-ru}DV*-E27*CiA_#jcLdWB6NnK@JSy2jm`fi%>IS(rlXS+yi6hF)HiIIH`n(KTf#hC1sh z26r4K@O=^DJYORt!cG)ZHMwbm%n3vNF+^AHsha#9a~H2Z1+NFh;V2YkV3=(W((hqhFN<$RJYja3 z6fZPj|90#%RvNCT3JX~*!gjG|CMcf)hU`LECE{w(>W_& zXcRyBlayqSh)4M*FS*=Rtd=k2$al&KO zNBcpz@nwT>@SW1%l*0rQM^owi?n;-Q+JK`6mU2Z1G2+`Ahg@iy2*~%Cruyy9n&MAv zP4$Q8g?KYVXgn*gaXD${`(lKudP=}OTEtZ&As>E#gFG2KKH6A0>6f8y+cAU~;al8V zE2aPk2<48*UF0DXLC&Q4D(>2CQT+?go98)t1?k=nOyP+dv)``z2{F2B2r0eZr2H;p z=-$o6uSCm{3YNnTO9KW#$2;T;$5JP@p#lVq>UC;y?GDKNTLN#-R@`((2GT_sOux!i zo$tU|&lXE#^~yl5%nAyp<+h7_=D+_NDGdSR`0~FYrLY{6cTOjR3{4WRwzC{Zyu{y9 zX6COZVkuwF%_){AVoO{K>AlZS0ITfDtdk}Ni4BCkD)NCOaea8~@iY@C116a2mIs}~b_KCr0? z#tmM7w+;i?I&C@_lJl4?XZz#U9wNXJjlcLZ%*^W-DJDtVKDXve_YC(cj71CuPqGA* z%TSxb*+R4nC}d4-YzD(L4&yGz9%o#B)@{v<3h44~mas?$)6kn1@MC04bsVhN^A*=r z-Nt={{+*eo^M)3}zY!*SE&TzuO!<={&WK|$7Mr__uV&9;z1K6CFe_EydyIeDTyfso z*wX-Bo2o0=Is3J6!R+t<2gEU`kQT)qU*~yDb)KC-I(f&Pk*4r)wr@ckPm@Ap8Gb7$ zbPDZ$AE%rp{pi+U981SC&teQK+zB4BZ-^#K?)i-r$EuKaslX|!SWb*%X1lcESB22T z3&99?K(5}?^YZ_IrcS)XR5?|s!l2&QTbzqMG{gTa`DLsQP4YP@M>j^ZN}_^(ow$6? zRz((ogj(fLWnoqBWZR!E9dHyi!$Lz36k5m3^_8{46(C|7M(y4cVoFkyOMfiUb8DwLaO=dLe4hi+KwQ!)7{tLZ2#@{N4&j9FnrFj`=@LuUT<8N!peSkxdM3%Y}yn2A;ypU zF~(`HUqv4=8wiW2JPZdYni?>cN2kRH=?OJ6cG#t3HBM zOa=&GF3qGa?kaR8eW^q0DgGXV&u9a13>YDaD6Aqh;*cr{!7}}kjyCmwDgge zzGgV#asZS2%D0>q(6MflgAqi>cfq)!??iL5mnLTPip6JC1O%XE8kz! zp4$T<>i}5?)F&bzAD{l(s`YZGhWGh5hyr?P?D4V_vO?EgBmkWEW*Itm%c+W>3(OO zH^b_@kBR?UK?EyVL2guYrLXEt2x!tsV=kCgev4ziNMa24K^9Mg>Q1A z!`~zgoIeG~g`^3}s@i<6 zc0#zCLZu~-iK+AG9wY5zqQr>wW(S5*!mJOsQV37l{rYO!pP#3G4J1Lo-jq;xDgZc>CbgOk?}7M? zzsATXHZ^yYj}ZOab=B-dd0sBfYXhEKr)@@(r zgdu^HCe|T07^Lae=|GXVK>idGRM_cxC}^Iy(w8|k$Sqp4@}*!2;Z_Y@M~&@pqAv=_Mh5Vy5%Bz5bM)h=#MzI?yC% zk(yO|$9rRcL6l);RN)d;l$A~lXF1H&-gOufT3X-?DQFdM%~YnR{lJ-S@<y+MKp%uzg;m+zCR)SXld*1VqiyD-;X{_Q4_Xed!+)t!0F638D__wuXYSq!)r3 zRNebbeJ`)Z1S&MvJHo@pHe$>+oBWGUdP?r{Eyd|`o)JnstC=j8Z>g@~^W<}X)W$kj z!#Oi;e{UuqdXGz?<7{x>ut{tHKd200KNEgG$Sut|e|>+XA?Pe8#Qu^~CbqRzE*e_p z2o?mc;Y{O$Xb6~*)c*^vfIdMc3Z#Fhx6Uv|-s|Q5GD}ze^qtzq<`&F-okA)fRYwO9 zp4ZSSyJi1BHK)_>-6guJ&?dC$Zca|bY;0=F^{^^I==0MW>tx!LFeDNtwEv%h46O<+ z+I0a0tro?-BO0Lu{9l=dWY5UDa|z>BR@{D@tUBKkGuAb|K~~d=cLS{@WVfHDOuZxd z0YpQ)t^dL0SDrh)vm7v`SpABhLPP^AoCoufS3*zzf~vR2fkf{yonTnsz;|wNsn9^k^ioN#ppi%TBHAeA|7DD zRlg0_MsI9{5>RjP$E*Yu8KC^ZmSV$wa90^$(Ki?}`@cax+3^g4E(I0NcavphVc5-Fi`c5XqqAp~fdTsSD93Ce1UoQFMbQclGEeJ?O ze8^YWpSpF;cpu@6rP|xnu0b&f7nq8OZFW)xHV&K-iJ35*2;HGc|To$o(;Dmg#LLuZ_{nm z6pzEihk7$|9m4mO(+281HofnT*1&uJ^!tNVj`2((1^>G|ZPbpWYKJ}dF+-rsg(_zP zxNQ^H_L485s(PRc4c|Ag=2X0)d)MEL$qBs!`m4%&q83)fJCr=_g?A?CTa!-@^#4Bq zBGC&DvrgVaRxhbXV&^Pt@O{!`JXlNYHsowOC(|cwo;JrR<3YD@4^Fc}=C%65Jz&j8 z&lyE>Kw)WJ!?qW^S*a4~C|MTWfhx3{7C4V2*W~WGh8Yf;XUw6**1J3in0D}Ht4$>{fTLM#@G z(@1A^ug7b{HLgz8$M;L7;s6%R) zHNG2aH@BPES;f!Ch^o6yPd9(gfF%)Gwv5`Yx(F&By*F9++F?&Bn=qG7n-h0GmW<1d zn=Z{AvhTs)*w3*hI*I*?-Io*F;QGQCAi$M*VGL>2qtn-NLrv+&pBwIY{oJn z;FdnnL8q_mofZa%AUKV-W7t1^u9wXb4&2TE7>4i?^uaU|anxjUbFO-sEqf5j`r)yN zNe134bFNN6CLcG#UQ|E>!vBC_FI?pJ&E4qQx8=r9A|fJ|^YP=9EN?>fa78i*P)Q)L zVeap-aY*GhY^MT)O>R;!KiMz#d^WJOo3E4z{kxnmbAP@VDN@R4If$e{n--CkSA@kg zvvDb`DXlKgcnP$`S13vB=XT%|1g#7hcH)2%*a#%msLopq6DDjhcVfR9d=5Q3k7@CevdNAG=Q|EFhmn;fEk{xDl9U%`_JnFpoc}KN!=hT=nknX)(;4e_ z&3Ftl-*U^J5h*wkw`~pBhL)KvwGYvI8~VTM`edC66_t);qDbOUm;=C-Zk8k#2nCKF zt?7MdaO%nxIfoSThoodTT%!$A2gWZeRTLC{$bG!ndEA9siBvJei^L(! z!2X`c0sGq|O`IWRVifx8?iI#!i}0$L1eYHhZe8~Z4bjItWVB2)|c#X;cG&v}+t6aBJ(O6NQ` zTBReU8nLv$SJ3fs`cIbSbq>bI^?Tc1e*QGa;e&2)Mwa3Gxe>{*`T+1p=+k7qk%D#w zR;8dxWWR1)N+Ya_S90dyM~yLDwCDVkoChd!f}I`Xp+ zIdx!_H(t3tX$e&xmLwW*mABaf&j0Fnga+zafT!7|Jf zL>>h`IX<&BSQ2S4=Pc!SIGWw<@xc|#rf0!4ybd|*lfCR5%|(?a=bK7J!jM{T55Ye_ zzuL3w^R%YH&E~5hKq;3yw}k6)ITUcTO{{JdI97$jkR&jrI#B%HWm5U4axL4$*H1R! ztxQg6S7Jd8`GFH)I7A`HknkI75TLG&>T>uY$Nu|u3jv3yZ$SD{ z+VZU{)yA$?rA&sr*VS zBJ5DO_^s_2y6^|@F>q5~U@-Ba3rD|E^ciKK|1(0MZ>aLvOst+2(v>4`tEO`snYUIe z$5ur^GfulHYh8zc*whoF{&L&5HeZaN@`n-xP@9^UxkvysLFr-3p<9Dl(%?m9Xx*%v z{5N4VnxQVcOlVgfKrg!$RLpZG%-UA4%P>97-rAlibgQ;XbAL{O!^P!Uk&0dzoeof7 z%C-Q-)$!keP?{%f=Yw*AZhNL}iG2mt3(SgI?*|zI+)zG3F3=Ug)ulfIRzFmw2>)6Z zZ7EHLg+3zTC~-6wtA%w?Z?1_CXY`l}ABxwkhhVl3r-XczxbRW^NTEuQWn91~WRXxs zUG+;JN8%X+?d90i?crLjEig&>+}x-Ns}6ptFTeF(=Xn@5Y=Oat=uanUB#hw5dK=mj zP#fI^W5*TtXy$`*{4qaYwdgy2=Kf^$K$X|&5WCYIRq2$Td|)L-N(z_*I{{z9S>2S$ zZ5Gpa)h)>jGsl^hHm2j1r#xQNSx-(o2thK3)At9reaq?QFUvWZTNB>&dLRB{nU;rx zLYdhiTD0FL_O<4?iCk42&Sy7oWsBhO+0v`?yp8Y1V}7blU)o^{koc$clx7EWKRX=! z=enwGL)}4Gk`N0u+7vGzjV<#RmE%webR1dRX5CmKzONvqIo4${k`QJf8GE9+h-<3+ zW+${>x6&_76>JADL{0yjHb1){V5XycKN%^A+h)o%R<;3 z?x3SuZ}HnR5MfU^@z?LA)}o5S$`vfO`B&9SoZRho5&eZas6}94U_O5Ow0_r>osG}x zu)e$ayI)_aU2~T+S(f6AFeTD<@uL(~8L1` zA3(-@!cOISeTHCml4N$f5?YP;THalZ?&~&ztPueI447D1g;!#`zE*|BD9-2y{9@>pj7{+x~-&2FofMtiIJqZ9{sP7{ewDH0};pGnED+2 zqT=&hM?$J)9+vbmhUQ$k;}R?gWFNXJ%tm(pZuo;CG4aQYn&^lxh20R03cH;{zbUqON#Qa`Vcuqjc=~V$ubCnq#2O0MoJptw> zMBihoC=95rS8TN@j*>xZ_kyW+H{`wsMU|pIr@4Z6w^rE68sHAgo18J%IcE4)p&<6}0E+=V6s_6klBebl{ z&#i|@m4Sw~^4n}!bMdejaB~n{84PZBKHDS!kK=n({|B@L)~kyuyUte;&?%&&Q`z~B z^C@w%)WJ5)c}26IR%RH1Vk#+RPaFu*^bO$bpyk-Mg#8_W7D6h(SCAZw0p_`mzS zQbnEv7P!f8V>1abG*OUHP=qEYqXW;LV1_3y0ebS1j8&mk^ux^hUt8Q#kX09HVaVm8 z!;1a!9&VH|e1N`Vj;MzTOV`VANP2$k@p5N_3d3su>G+%HgKB}pi5)Sb{7k>~e@dT_ zHHAe-R%C9?j%4K;r=IiLJZkPoSsb%QyweaClH5Mk?i?TM_QmP2;Do`yy>ZBP4P+uy z3;;v*c)l0k$S?z-J>ET#6#+m56m9X(mUu3$P1kDkr`A%>RObTmvld2syKiXbpSZ|I zPXh7?5f%_%f8KqVg#V3kFUX_6{K}JeZaownOq9Rue-`wwqqHa?4Nn{dFV2-$P3v5B zClhF6i>6oX(4@%cX#Kp}g7G8xt1#-#iA*100`2~}8cl0r>PP*9nnqtq84 z7|nc_*f8XedzgR=Dc|e{d8LG!jI658jw+ovxW4dodBp^2@1maa0v!_&zb*##i}MGJ z1kRblWMEdt)79S=&q}YLV$Mi=wC9UKLZYy0{oY^8s?*c}{QL0~0kPIUFT*ZyZ^Mma z{g%jNmafSA*GBKnsagwL=}8rS_i2_&=WEYnlRI7TD+VCD#X{GMsbVs&b8*Jl!prMZ z_B1XZMz*QXsAP6pYMgvi$PX8iK0+lXvR>SX1NWCaMX7H(T@MP?+z*64IHv@NtOqp7 z`0qF*VE>_EE5-W5VgN$6{n3;$z~Sl0zE_L@V9a90{)u8d4 zT9;4p{hh5(Vm|0~3pQ=TZ=Psl_vTxbykk!q&t5Mv+N-0iBEUT`fpp=1-=phn-Nldj z)H7JlTk+EF#^@+eHsE_3#qQL09V(WUAs;rK14Y>-weh1f$sn2WK`xD(Bm_hHpE|7a ziI7CqS8E;zBr-TXp=tabSQ%ITFY*pXN~|Dvy4Ivr4>+YMQ^8_t&i+Wz{xQ+ymcEsG z-Fv(A8a6^M^A}{%bW^{=k>23v>+ZbgfClcrwQZ28SxqS%X}IFz5?f^uxI@#G-{Hib z8O;0V;rRKP!8ra?(TjF+dQ%opQH%5TEQibNy0{x|pWKw7Ji$Tdh@m}eU~S&mi&Bd- zKMmB7ZOkF>A8$#w>QJ>ezYB$y*wbN6FxnaW-v^!eTV4l(C)3=mmI4PSF{U2(0-fAD zW&!vpVpF!XY(nOUSX@0*dUT%G3!&x>;LTRd3_ko5jUP-eSzdT*{f2hevs$y0VHi0g z{aFPx}4ukH|s>QXY zsO0NTWe2jed9<`T<|-cxCW_gcshhe0XsYD7&VR@IT_MF%K(f6|tk1emZufl1iipCS zy7-Bt&C_kuJ7r=N1(kcIWJW}hc3)#LIjZF~bIxYzc_~uypBhUuU=;ydl3bNt70%?13m&$`M&flQZprDiDO9;*yaw-@gy4prgH?yk9+qho1T zJ$Q_y+9zMK>PQo6m{gq_V@icJ`?4vGyoPkJCHL+yZl}4s(+ON+$Xw zG1K9*{;6|?)yoM<6#vUUO9&77FS~i%fcC}M?z87eJzw0qAxzfWis99fjYH^F#=Gr_ z)|l)g=1gI*KTcJI;>yY<_JIvN_piE$8pl8T2G6Jo$c|>#^3J;-l7;g8QYa~x&-Yww zB;2AFSN%WFXFt`7;k3HZ2qD;tPT6{coRLb_f@@qHXUab+B|4KV&J-AP(D89qwuwCLotPgnmrqZXOw4cZ-w`<% zJ=K47LOShJV-k`A8K;>p_(H3ws;a47UeWT#ce|;^>a6O zUTI#3+o6@A)>c9V><2gug`C{kgR{ISq_dKI%)RQ7;dkkQW@L=uiaiV$CGIxRcR>mY ziuYadigwHv$p*46w(G+g55r!IbL`ygJ617Kf-Oz{=l9>+YZPI&k9pUBE^MZ=kE<;< z+T?w*^sQ7wwan2fFL445$BPeY#Ii@7$U8NZd!;l>pl(-Yzv6^*r&LbBkw=m2ICj5X z(Z9uYD;P+Daex~1xn`8EbOD+6p#{X#=Bm}{ zuY3zU7f1Z54o^91kt4g@tv?tpCWa+(OJGPsAlt(^Qzv5#O-?~|UpRxuE`=H`_uVXG zWd_XF)R%o^$!SJ4r|0a{fo6SKi#JE5vbn_6O#LulifS2eVBp@XH6LtUEc?I>Od)Q} z2W)@aY3n~=RO*%V>a;s(bn+T9xV2O;Ha4`x2wGY?hiy&RVX%OSGWVx4Ndz)otm|;` zfSaPHK21{0Q(9SzU}OZ;_n}H3Ki*m{Co6i{F0igtA;`l zw2KBHr|81yBIq{hKW-rmG6;&@TsV(MR|H*8h|y z)&=srrnIfImC^ntOoFmfex$r?meW0daGr2B@OxxdFj=>N{n6O4)#vs^e9r_gSNH7h z{eA34ZqW9%!`GtLNQpLF0DV56UR0(cpvfS#j=-kfph~hxvgo^5k;;M5q=ioqLU~xX zeU)qq(6kfW^{OzgC9^*Id?2igySQRy;^^qGQb>MC^`XYuURQ+|s4!{80|ee=ZIm{I zT}sGOT29Mkf;@iKwOaPH-ThK+uH^3Kdp03|IG3RH_%YYY*4{tpjNUtd&ip}!m<7YF z#a|tRw7m$tJLB(82=0D#)tI&|Zb<(_$YRO>tCm~lz zk}Re;fng)@)xXCu;D|8B{M?#jndOBqQA~Z~=XzYOi#5(F>m|=Uv`xSp0B=qCYuj|L= z1^zL+Bi6Rm3O5U2uYv+l*LE?_;Z=(Jf@v|o#%oH$JJMDef9$< zA;rCSSYp-pUWCwZQ?LczexL!>&BJ4Ra)Ko(i!y71mAN&P%`>}1SarWSv7oq;h%4^+ zc=)l+fhrbl$i3r>q*R1tP~X?F%QTBj9{3&hFq4JVs_FJQI0`s;c=oQ!260$)PA-6! zB_;nfLfW@N;K7_hY?v7s$bTvyWF9e@Q@0!3#AVf=AE{_YaeRpTp&d5vxC(0*etD)62-nCVR|w_}1B zb#2J;<%;L!Nh=^vNdAqpuDvyU$l(PyZ;faxQmZTT+SK^wbgM5Q?L6>E?j`@)=4QpF z)Uzl4AX{U}Kp(p&((!G4>#Ju^x<6K~$}I<|{#HZ(j*DpDsQaC`?)9|sXesf_4%c=6 zD--$c&GeVy?{<%)TAo*(LpdTh3Bd1zbIP|_q20FLh2IZrCM&~;g+dP~MgW`A%e2MH zq05;Seb3h(FccIW9fJ<IiuQ{E ztHYwWi^K2XG??Gq2IZfgo_OwG;ObE$Zw(b}(Bu#(`eP#y1r5P>2RQ;eG|!2rWs%#m zbz&`#hAP_UowuIsV#{0>*RuVGl!zCxPy{3_p5@yXHL)eCk_3ZhK*eeuE1}O0Z(~e zgJ>IjuYEDtYR^66MDJ@K9S$%VeJ-CLPdxe43Q8rv;$&ZLBV84Q3yo|9Jl;R%1%5!L zSP#Th;xE!mVFLuNB8lLAN9p1ENa+&m{eonj&z{-q_t-;l+?x_(BdIV76&KtHo3sbm z0FI*Ii_y!DkYDHyPkvEcW8Q_X;LVET{?ThD5zR>%s4j24`|Z{cVF`>i1OEAb_EWGZ@g(>?Cw&totCR7BmTik&n0em{q@x2e0ZMlRAp_(@JoQtYoNHjnyk^?5lv@tghBQCHsl~-e?@FL<&_Q!3&Iq@)sqxHX zXlb1|n%#P}!M*kdZLl|wOa~<1k64j-L-4^xJC&AvA*u&W4Fl$s3i~gaWz$#5cOsO8 zlm*6=csUIIUm{S7BeJb;?RIImUK-K>L(gZIfj|EE+-)&f-3Nb(M9kI7bHMk{BOV^7 zkieF~8FE9b$PTKa^=+ksE?R2Vtz$-}edI&=C$p$3m|cKco_wTA-3EjZfY|HV-iD%? zp`cj-kkL~^m^N$^1_KQaniHA7LkNc{v5x$CiC!m@XvA+fAW)XPaGQ;4a=8K_VF)gTD4VJ)i^ z5sR!~Bn+&`lbDk=pB7*!j{5w{9a5ke3_X7X#^L&QEr^MW>nb*xjWpwv+8w=W8EAN} zl?gtMxND=?Xf)5o-8K^Sd-CJ+y47#V%Pjff55ekJB9g6zM6vzZ{L?TUp2LU2X~{-u}aW4r5=Kw8|}C6QUCh7DL_tQdRc zem~xTtd*E@lN=tE%&<}S$+yfnWvTv9d3DlAnua})V%8bGC$C$Dy?8H zRoe=o-FVl#*j7=#)@!X`{aqeK^7Rj{$40x@%jo;=q%tJyp#&Ksu)p6D@<^IuKeL%Ig_TxPv&RwG@e#PKKX zSw=L*t&Jnq2@MSMX<6Q8Y8k%>1>|uB_$6xvY}u~+<;IRG`g$Acdkr0y0A?N}PLsasPc^wU^nswY zXoW^TI}}U$2}-PWb}E7%0_B?}IOyhfu)gR^l*$Ffc&upxci9)3cAWm`CvoJa8>^&#*q1wOwtH^En1|42ObZBNi$X@OGUJsT{R|ZlbzmRIaQTKP_4cIoaO=pgd0Sso*A?dYa_7(#rK&lK(@&!qo3nYB z_Z}_B=qqYXebml~1W&wR*+(x%-!U3m-b=H`QY5!O5UkiS7o|${$ ziII>F_rWqo%<1aVZCgMV!^7Rs9>e)al6{=+*I1q47163Ftq9 zojXbT3)gPz7waIWEn}#=!>(2DPNFK;t_4KcN%9>Tv`Pxa(<#*ZWx0?eQ_I4(qzpUm9 zCe3!w6rYtz;&P_MgLj<~N!AQKLj zYBggO#d{xDL*h2GPGMG6+oNTjACXf|qs+`#y_mI>zglZ-=k58eTqvr@8zUYQxI>5NV>+kY4@GBe>{)}X-iwx ze=&0mLjZIloPVz}P4a4MY|m+H+RxAwsCgnP6*TGG4?mCGzx^4el#%3xV9+a4UJ)|J zbt)T+%3usn)H(L~IsY-SW5=1kh;mckSOd$9;)6`y*O396`}%xmy!n{dN8)LRWq&=g zQyG%o{p~<8I%OfyC44HOTL#DyQNB8{7j^zrzUKl60<6voLSIi9r%Im zxfASg#sy_vfJZ_7=sbZ}E$WcweaqepIOO~wkq`qhg7d>}ix+X6=aXbW=9BOhc0659 zV946#c4yG8thx;6s72QA@_h78W7OHnW}Q=1%#R%WN(PYej9 zb8th5tz_LbtqQx$O#O?m6lJP!E)I4xpY`0{+`LY71fg$T3lCz_aj11nEtdOF>&zaw zZ{l-~4ROon&Zw=V2qMHz=@>o%_M=3NY)(ohMH}wjlEDWl;9Z)Ij6SAWQl=&qTs zjdx-sg4v{E$s#u1z>ZiFfu>q8z{BO7{i+-yx57Ue7oZUEFvFOy~NhPWvuoU(Vry^O_(TXmGu+%X0!S! zlfe;~00$hyX0rnVD|A>2W&96vJD*4kisV0~(+wD(K_Z1X^{ccal5Kx%p+6<%d(_5! z4F1|&o(iO=5+>*Xccu;trU+Rzb#$ZlfxxF}V(EchOcjth@nV=$cCm)P5d^_#l`tE{ zik7#Bxw6d{)4bCLyV(M!6QAB4<2^>2PFm5yApRHY8&+DBe3-S@Ae_X_iFkU*#Mpam zrs(Vny)ybD^B`&z-i916lilyrH8bSV><2?TaNR9?lZpD1_#-&!Vl~ZsD6V<IIa%KH{b}n0<`d>d%A|gI&qhMi~0iB61Rpbn*3oY&y8mTlZ zlol(+z_FlL8{*572Fzza%_Q`E|oN1Ti?)STc~`MN_ZO_JJH2s+izv`yWhU| zDmQFo?S@7I)_HbWY8GS3DpUW zQl(6qoJeOAV@^*oWmk7H;bw0CM`fzM_F^??*;~FhSj%nmTLit9_HE+Vkr)Y*;-*Y_ zO0~DQs$kOPEI^|Ge+ifkPPauKpPE?T@7Y;h5=2KZ56H8RiCL*7tu@RE&>FnCzhOzF zl1UeBk=cr0_KjwS;v!NSQl~?%{=_GEoSZ&gTda+;V816Ht4Y+CU76o&`ixLyI}-Z1 zl3P$vvq#|0>m?zsx#9;UA?>RIO>-*BV6gJq7n4ickaq7j&dub%eZa;-5Axzz~+|yC;L9ZBvp-qXMFlVnsLBy zDy6|5HCAl7VK{T*rH?awnX~%w1wLaOT}(``*-TMHXttGrp zzfIZcowy{SF5Ne(MPHj1vQuwmYN`fuvTs2+dA;8lx;y1t?Dh}$7>*pyRAF9v>Jyb# zrgOA$1njKfe=#CgLjdeSq9gMt@TC5=Ir^6#vd%*|cv6NX3!H%#eA{w!7buYj*;#dl?uzta}Z>7^U#NAqH)DMvwnU@_O5W zN~jyIBq=OBaush}8!6nm%X^1j5kdgx+mfg@PrWNo)R&J3oL<$IE|Mw=J5T)n>6JrBv7m&6u97`# zK5|k!vcC!}ZLS1awb%#mqcz_w#NTytyPqsPzHA7hYym$vn>|`zz@7 z#eg0>9P6IFhWJ7gnrKJA$8CsxWb|p;Ywqjbv2>o8KQvAeZ?%4Xs9mjX(9*OiTCW7t zY(w*E?Obs?d1Kbcb-PPZ87xJnCc(K>U5d7OS8^;mixu)OzAN0x@Mr9~bqFUe>Y^i2 z>J`98)pDX=C^a+a>NIbt+;oZmRMX>tu{~-x+@AoCdQ@?Ed=t7}%fsJ1lWH6XH~7G} zh8P^mwz3=En{4b`*#@ zgJuZg4*HE=!O}dRLEQX4*WiP&60Pn+al3WqdiQ>EsDGu}W4hcFQvH=0Zp@q&Kn>DF zM>t&C>>Hp*fT0fZ2-pB#s`5$xbuE09R~zm6pmx@OLB*c)7z5Y^qKATEZws~)d{F6rcDhX~&4>(d+j-*0dfR5Yn$v)hJT23UwbT8 ze2fcrOB$?7bW=sNY=4}u8Oxw(ORsmTp`%q(-XR*3*IZHD9c@p@#r!T1enw-QHkO_L(B`?DdUU$tMwOhZl zG;Hq(hn6Tsd~tj`badG4zQmUx=JXO&@*l|ZI$`=VXiF(XZ`pPm_%3neesdMF@QUf^ zGfDV#K)7=k_{y61c;K7fc7XRX^4^x-6ENeAZN_yKI^~y?C*YNRX@hl`@3>IHf3H!u zla}j2Y0!I+-4TQIeq03}q3?&W@lZ52{IVc)4L;{xfBn8-!PP_y?L7D|gAV;lfmr3&-CD6FmGcs=hj^%J2D_?gr`Z z?go|aM!H+NySrPuL`ve)A>Ab@ed+E-K#+JJKlS^*uKSORweEAzJZJXI?Adb;)WySL zQ!Z2wglEie$4BXRe)ov)o-(>e_j(;s%9LFyic%386y7>P8$B6E@ayY3HA z1YCj>O>-Xz*4u=Jj)b}gq`Mx~m7o`z&wk?4;*WV?Eyl_H6#|;EQD{U#!J1-4xFD0& zFg+eJmv2A0!;{}C1LgMf8q*hBaC}UBZ-# zg+OHWoMoZc@f7sllJ1*GxmHKy79iBC)*KNkga|l*LXUL*E#+ zI_l;Z-uq+_3FgurwV}%uUh6GxWaR5_{D%x9m9Fj|U|F0){N>>rPw59R32ef~4Ss{& z)-I~izPi?H6jfPe0ac*jhH-+~+4m2VQpdU-QcyM3GIe*BK?xN3PuEtpdqc0oqvIy$ z0(+WJqp%asPYJ&>A^A_dg)cfBC^46wSDC;sd4ZctKc82dnfWqh|tt%!0A~gM8^;L(#jocv<}xV!dx|fp^7)`vB7TQ&vgnMu?5>>Da|;7UXe3U9_z zB%cJ+M6f<;Z=M4cm9IuP73GOd18gc1g`{qVZd-Nkv`D?{rCl>r8UKUpb4$e93fM%lvtPp2NJ%- zhXQnFkv6>wjNqgnV&Kyei{I~8;3h@So#)l()S8=QBDBl&seq^q)W}3gfQ$ull7%uA z7i-+vFFc=Q{cov0Oe7P-MVPT?qkd4>CyGo?Pu81b#P<$WSpO^p=c?Cv(Q0^!4=I=# zvnb{EwkIxQ6>MBwp|`)pkQ3|+kK2R>aU}AQ&q$^f*Ip0Lwh(C?$OJAsgM(=zQ%22G z%I4!AWPU(kmib@qOb>plNK zbgn0kbsep8EMbYH!rSkoe#tUV%&bD#uGDXR@8@6N;iV~ zHrmMhqTsU0c71HtH>lP3+g*tmf1lp z18M0|_7B;gbC#LANg_>m@*`^E z+yP3xMcyP{MKo<(c;?s(RFunbuF1{iV91%+kHKxYTNYfl4Vmd(hL@S<=H_h50oUqd zucV12zG2V59k-_oL(Ms}P)Vd7pB7O*bmCD`p3vB}@3u@i&@~lsH5G7B&b$j*q4{`W zKdXz#Rug;YI_UUJ`+D0obv0X_s{cBO?);3`fBtdWcR`6c!E|Nurq|Q`GP>J`h|R12 z&C?j2gYj|qyWt$8=S7in8l`I_!`D-;!_U`y&et2SNsC5+kS^!88sF!gqt7F58E=!M4Qar)&d99D24vy;l~#Iwdd``#)Q!9M-sjMP-xG(29*#2J^(l9Mct)aZ zH|+H_@_lCj)6M#>=V0Ty(y!b$VKvTk;u6_tGJI`1t1VZ)itG~mH^=Q7wLzx4PUB_IJN3};R}tL{VxvLo7YQ;?&dt+5x+;B?!V*Oo^${5w_`P@j+!rZztT!c-KBdf$u^ZVe>C{S`~lswSYK8E1;xvX z!2a?wj6-~30`h8lg@j6M;itnGQJTmP;#)B#x;H433ddp-%=pp5S2xPxtbsru$ZFL` zDzr&2ztK9xI;ELetwe;s39Hv>R4sk(H17KYBMqMq5Vo&dBwv0P+owiVwHOcX0#)yK z3PUExf)9uc^>yKZH2FdZ_;1dFcz-|viTKp!bHUqCvcQ{_r_EBK*1(Rrv7tlDkHZhb zSHS%^5F}KNn-z;nH{pS;X=VoEKRqOl{I^#gV$o5YZx;gxSkSE)q_A_FE*?5wCTg~2 zJ25|}f->|c1+XIp?zGeOC&Q~LSiP>1y`PB!gJVGmwTYfDHAee2-9Mon1Rm+7&mj+~ zy@z!4T@2aargiRDZ)>QQUnSNbG6IK6HePTy+F!Z_{3xX{LnUm`mA_rnbljEv#!r|Z z@_`E#=b#VkNruI&}%#qH?(Z*vLr0oT;W<7YUZ`E zQeP^&eh3qnE$yArFFqQ?oEfS!zV%Q^M|-EhT<(5i`YSC(XO?=4pNUX`RjMJF+AC1} zC^%Sg1xS&f?yabr<#Cel-mLP%D(hMNF2+zZcSRx{PgA(7D6x(IOF!UzF*4#)BQ5WA z{>te1d+owC1xR!<-VeDxeh8!o3hWsrG*ZEV!3@JL#<76E!k)AI2X;6KgdC{nz0VWj~|-qWbDk=s|2nD+T~34IvQvC_ssP+ zE;Lq3L#A~uANR-1OGgs(i~*X+`J*u~2pT_7P*`pwi)qc%#DcwOL!`0tsF#*RsV2{2 z!1NDQ6xH~7f>iE2E0Y}aYaDTjT0FnLpna)WwPvdzR^LkV0%w2!$md22=g)IPb5=UT zatEXkJBb%@=Y`ELCcXBmDpaFTb*Ovffl3qaviKw%}PW35Vo zq}T~?#X@ugV@LXUedV>9?EaupGO9R>o(0u%+?k$j|HuO~Z*JrLJdC-Cy`C+uo))W3hj(@Kx!@=Ce$CGN>w{Ns&Sd7 zm*)$GrDsM6-TBHCQ=!u(ebOpVt9)5xyjip}OyBCZEC{MpklNE`<|@~sp0Fns&qcWB zL%6(SSYg$2{eE5LL6Z_S;=uUab}u?MMphUxt(7NW07S?7x{=NhhLha1>6pQ5^Z(noXl3L~FM~*m zbpV|%6>nt^xWv@C0(umGPlPkTF?aa-iU*z2mSn@z&l+8!hv2<6=&rbFKE>LY5*NAx z=&WQzw|)Zo)f!Z?*=Db{$FO+vaWy%=N%q)utQ;CrN*XoCCnRLtC` z##jJ0RW1MMRxP&k)##V`8m1~;pM+oKa3L3REW(M-e>qay>z^lH?7Pq%bfQVxWWnHh=pE2HcN^SE-gm4z)4G>@AH+7>izl+~^m-~ih!p1PTZz8h+2(c-CfE3<@b;4mxqwUz4n4~H&N!;}E^65!8d!eg1k(Bf<9{Gk z{vGY5lUQ($|I(W01du%wfBy7d5si@e?S{F_>Q^L6J?nGRq_k#$Iah>97OhmJ(b02< zV#XJVu%`W_|NUtsyn@P~VMQ}-mk>cy-Uhxqs;co4!v&Z*obiyCSeA9lfo;_1FBkc% z$0bo5-=s$m#xkJ>ZV)7WyRmzCuEr_QRc$-R&1K7(L~6WUf*tF#`lqEiAK@O2i$sG* zgwCfA{>P)FPw_ z`EL-YdMkPDerST4#v7xvc%9WI`#jOK?b3GDR%%)2{R(&07^(57ELO}F)j@iTQ6A^9 zBWfTG1OllQR!nO?(AbL~68b41tXa0o# zDse=2qCwFtcoH-yVY63BtyO_~qQ>**=gaV}RP)FH-=2Tp!ZW|X_El@D~m7(YMWtrt$qFtcxWZ&J}$!L8<<*q-z8;A@Z+b$aGG2w$B7uEdbKhipdD2^HZTkg{(<(`G-UtYZEy5b)V!!oNTn4Nw z#zk|`@ab442q(xs71$na2o+}Yn)KQj|7l2r zjddA5RiRQ9&iJ@?;pUnY(s=b+Xnzzi!Vs{<-&IJ}>4$Kc=Sx_ql7V_OLt8=b&$YiX zX27wvm}qo}A2g*-?c5vt!GxON-+AkKUCI&*w0Tyn-#HoBxEC9RXXy^Ojvjxt>t|I1 zrY&iwt0~O!1AX^e#D6L`kzgl#)Gq%YLfMIcFysuo;&?$5Vd)SJCFCxw7!q*(1qjR1TfNkEA@-j0XhbYU zxbU%xgr8HD_9tz#e!R)?AP+-lp~xf?n^0N%VTs~AZlLF&|1U7{e%+jW5|2FBon44D z0qJV#W}LWquxfdB8Xb9bTXU`}eEndvqT3WC)Y))`C0CkI>~(03o~&>e0SA1cm27i8 z5H@X0Ul>)i1Kf)*T$=3d37Uj%o4AaM{lmkPY6EU!7RAn#{DGTwXB;$Ld;tadUu20n z^i8)&LY#i1Wuf_Z=LPCGVl+f5|N9);`^fNQ6PgSPdp%!hE^0lFPYSfD5E?A!K7s4Q zBpGY#>_Js~Q~BJpAR|OzuQ)VSX*{$OmbL*y;mu9+07qEtZ$E@lk7++9Vcwul{@3N+ zl=ZzJcly>`P0)n43qB&XL1_61bMnVV7mM^q#!*7672Yz1)KPc49i4V{ykou9H@{F(LKhza3+Em$; z(e-O>Dv0ra0%H zQ&Lrcpe`IOza8T#oXlY4&yIK^R1xFFZ6#4Q5J1OlI&9aqV(t77Ml5uP7o5pDO8QopN5jmvu0Wt!xUNtjr zavGq=iWeK1SJ#6)@19z&j+SZvZ^I!zNUzmM(6=swbPrs=v;Wc&HpG(}k}GQ}G~eh~ zZ3{kv`G!b-;?Hpoeaft$+%^rbE~KB5I0v8lTykLDEayT4m}JltT9)odghon${5*|t z7x;K&xfyhaOXU0Yx=V0PN#Ha0Y>AVL($=mI;(5VC-W^H_GroE2tyYzNV)dXbr9QP1(9QH%wh3^w{PX+3eH(=27-#AO4kcjr!t z(f3?(HGC32FW=QMfg^bES>f3D+PWx%@tR<(-=mzT6IUPc-mgM?<-u8YB%9_zvB0g0jvRy!?d81U#(v7rQ zlR1~*n>>Ug)(jsgR2s@EQEc#^Nq0jEskt|hPF+@5BY7&zn+5B6<6xpbIUpm`3ejsw zKsEi)ui}>H{DoqmQ#0hW>@F@o+Lz8k&0-6TPIMwkIpuC$d*fSpJDTlePK;;o!nrCk zo8idp3~l2-3~_OXRxojeLR$Ls+hiZP@LOjWG2wnFRZM*w`|Z`Vf8=_HIC5pbJfX@3 z&WSkligh^;5Jzq8E?+m>!#C|1Qiq9++?ComVOmT7i}dC{3U_tK#rhI-(etxCCyvTTN@(8(P@@3kV)ElN8LcUI6?aH%7-Uj4yRk1ziO2#KJ<@Du<< zZOhOXBI4rD`A*yR^x=&K+1u+>5$5SX!5l(1zr(R`u_FIW6rsKFJ4Dz(7m>=!$td5Oa%8A3J;{TP_q_Hy3{Em=$XDTC*P_R?4) z*PQAUJ6fDP<(<#hh6*=MM}8jb4{6q8rc+R$(2SXirVhXKUwVWvE?%R&A3{cTE~Eil z&no$Xfvau-`3|jQG08uI5zORzOEDm|GI(PKJGFvZZB)V9f<>&9L)$7sE)cmG!GnAK z?LlnR9$ordfynO#7>QKXFT6hq;TnF>lL&-n`Q@|roWFz2{MUk~+S=qu+4r&rx91_&Y&UH20jvJpwdNw4e#-3Sj^E@bu#b#KZ z-w>G0r%D|Szlxt!w|3$vKrPJzCYN(6dmb5CcWJjsj+X!=ge5@i(9QY>Lov%ajpzYo z23GUL7LQ?>PXAXVD*u4^T~l#cErnFenZWegy9^ ziwP|W8f~g~^qdrFh*gs+&W$QLBEtZyWtVm18#PVhrj_h+*<~QfGT{-w< z@J!;`oDbNe;+Pv>W%7@J3#9vexUlHCY*9blGdRfLGqnewGSC~BlUZi-BBj{DILe+^ zLiV-dOJiLp(s!Nk)Jle>zWHwEnMoWmxg;PGSxf%A-;0%KQ%zo9a)2p<6)djBK7hh* zC@eO?A28)`amMSzS&%dpeTFZl66)cr(R~f=m-XV`f^1FC`7{>gu|C<6ZTe+_evRiI!BjU^W$6B1k6v zI=B0QFnv<8W!9)-D97%L!AD8LvJ)I7Eu-7{2*~Oc@=QBc3^z*EsA1=8@9}eoT4|06 z=a62bU(BQI(P~cKFCkOywa1!Om3v`AFd#^!SOJaS`*U1vWYBNwB?^tB$- zR+gsG7tC-qmENQ>_1bFqtIVg&q~M5sm%DoPc zpGsEGR@VMj{eG31v%{XF?4C3Jd9OmzwKaBrga)h7mCnoOF&o`N+K!&dtUTDo!K|Eq zW*iP$)K$!u6l}{*_8P2HjStJsyf>@0Z5m0v?kF|Ox!iD-tiMqmNa(_MioX{Y2kKS_ zFy}$VdH5*0N2@mt7h^EZy)S$%17q_WrL5%iw}C6VE>=6(Y6Ze)T&|5RaHOgujNc>! z@!>7apDfuhWfVA)UQILo^?vs????m%QwRB9UoaPCSg{;$CLmmv<80*>l29Jpv2(4M z?bT-1d_1DWjpevna+cyEfP+|c7tpv28HQhDs*M#Q!uzr(|Gp`d4!h@@Codiw)|AHg zxsKzJNA{{Q_aherODKDbPV#!>mK_-O=F8Z#!yqqW!N=e=3}=dr{=;(Ng+l=X{`a^( zU2?4ON7>(d{7CNyhdC^*I=xeM$dS*J<@VxpK)HNEzQ%r95A{30unFYR2Q2r*N0x&c zADZh@hsgcUy$)AphGkXba0CpNrkuaA_>*8 z%@tTPzi0Hw1#q{%V+xJ5bS;7kSiaPh}I4{V)6?A$SAlC{L68 zx3PRZoH5Byg`V)k!BxH;OaIMfn9X)V@52~n`5 z2K!#17K`?NDl8n>J#0`fgXc!N@N}9H;Qv{X`8WR3=DglLJ2rjoCkIrqQ@*7nq-f z6|ZNs5tk7Ow5?|iSrG=x(t5)G?9-tJ%m!!)eTuUKi){>MOv8^`G*Bm!vcM;G#kGe@ z=axaRYbdC?b*ASycRY>oX|?GSA*P*>Hq2QW2Q$hVO)jZwKVc3IA=^wmMU5c3A)X|!E#gBs0GazA6*FrP$EJkX|!RE=M=(w<( zWn7B%DYaF##=bJuiM`==FQ_zYko%;PiU>Z9iRfKK7B^hh*`ZbQhcrSIisy&ew6Nvg z2?yxw)wHuA8R3ZJ%&Mne35M4#i5$X5eC_6fFnBV{Y5kRjWsDg<)LgieMvd~BPduuK zG;A|yK02646eVd4K;||7U*@&1K?Dd_BT|~|wG+|MwI;sH0T)bGwjC;B#M|&n;a7W{- z#+_qlhI($baNPEf)O!IUYF@6K;i|~}^=XGm_Pux;w_(dG6TXH=o67m3Ui!}myQ6iZP&4L zBqZhL{$me}nQ{G=D@q)mfW~i43EO~>6SrpZr-PD>TQPe*BG_b=H&VK9ZRXoZ8$%5> z#W=Ho-oL&0%3Dkw>wgiYxUiOere#ir6ZdAO8yGe~_wY!`&jGc)iVtbp2}ur}F&WHC zgJjBmexTDRR!Bd=ipzEja^}Y-#o~3IvfHKaI$zio9gjRsb7!$Qr--+RuJ=aKT$ud& zR}1id;aiMLov8VI8#UlbN*~F<`#j$Pzfj3&jlChqic`i_P{ z+=Kt>lPAYQfWhvpQ<~H)HNZa70F_vU^DoOVGwlcT3kw0Dq0Um6Y!BtZjRkliO#?i; z{%i=P^nHNgr3a6Hc4jcy{#Di(Gwb+qRoIfT|vuPU$S^ME+z^z4dHslC%b?PQ=|9 zzfet(C@-+DOb8q)M1Vj`^+9Z;TCzh$tb*EP3viKWK0X(uD%S915anNU9inHs84fRm+E^%^b zB(GpoHMrUkLX+p(G4*##ZQJH{C*$j^x zHD_CCxld0ZI!e4RXvLR6;=z>5srX1@l~rio#+Y%OAI-jn{*y^VcBn;> z-Rll>iE}W{q;}^{4&wtF6+GO2jo0Z}xH*=kRfg6N?J;hqw3ObH9%K^5hL=aFv5`IY z()L1MBz-(UT{IjRC?q(tceAH5BMaI*`50nRQdi|B@X3xh7shMpv|JydTx5KPJT7i7 zH~rUqN^&nE{|@!@4y+RD{(B?;EKVNP?IxQ|3CDR2?_pM0794H`_HGR)K)7Rg}ZXi_Vy7L zC1Yi=<=K8qrzT~t%kAv)t&cp&#Axevq6j*w3^^*y{~om?8ifh>g-FTqz)~&n6S`S>yC^cZwK++YP*9$@n+R zF2}}|ndF1#gwz&!?_|j-Yxj`}NFBs7&e{-(=^{>iwiYJ#+4-_=xhVQSM5f$G=1p7U z<4~!_xHgoixO`H(QG=tz&VBFu{?&cUEDheAg3q3PRtxcf^AFF3PfSc~hY!pfXIr}C z>lj8)0ySPK;@h`r3zZPViERGfYiow#R;-cU2js2x!xfHvadINy4P^(@5dKQQ%PA(b z6lpI*2Go4#p;Xu@bSvWGAd8aGv`!>T7hbKqq#*=&_`2}-dP>^AAA|+12Dcbq{N>SPO z7IX!POniDyyyzq@lzk8h=yVGn&9uTKaK-h5McfAu#q&r`-Og+DIgpQjsl9Fs zw+Bm5K)G&`wm}iIPqx>U4{K`3M3kdal^K(F1#HzYE7$6(3GJ?A0D9SySr3)s&Uv_G zrukdZfAI@q9Zxw(eif>&>%|42fGus-AvhK6>5upsi0QO{r1y}mF4?)8Pk;1F00dG@ z0vEQNq$bi&mMKG2<9jad^D(YJ=)nmHu0LiJL;`z^tXOG(Hxw&sgCb zep|B3nSTh8DX{P!)3Z5QI6uD@{yxn(x_lcwI5IK5S#ACsY*An38|=$*of=oya@-s# zRN24d1}SAgs71%CM#$0DuKm!Sh|~E}eeSOXaTAvEQZvN*{L5yMf;z5IAGyKSKh#L4 z%b2MDY{m1Vl^>A`0z-X0(6npByWpDsZP-9~cJOaA*v_=+Mg(qncvSeOia?M8giyw} zQw3UaW#yE~Njl?|!6aYmZ(x0!H<=T~S*A1GKnc*M-@=%yK$uwD|1(6!NLpBlHN$M# zeKd!bDZf?qPZ5dbS-8Z5xF0SAJuD?8f+%heKsLR5rPq*kDkhr<8 z(LkcksYp3Jf*juZ1)J!+PsO0n;NDm7^~$1c^-MGwUACR;CAW5FEeF!IvSv+U)>Ojw zN=p_Uva_IYFDw?P@ylgn;=SCLQA%GJ(?X)+;d9x!^z5*;@;j+`CI<4C`s+VyuHNkB zf9al>&kh9vrQz;t!~H%-Lb0{a#zic7sC3G>!hT+wyF1BRp2nK`?ELW0@yS*pq3rqU zVuO(o^BNN(C>SCrJH%!h{D>`REOMd^?7NM zHG}zicMAA3&gb;FK za)5`A+#7GoE}+sUhyCP4BdT2j`1dn8sHtY?dNq(u;^)O{d%CFl=frGOf&c z%DEbcK@e~*m74)V&`)thQ}J_0TDGovu@#WEORpD*KqB(m5Q=Nse)b1#Aww*2)woVw z6Q}TSUtb!KftsaN2k{yitL0ZEX0Z*@I2t${mwzQ5%C>9)L-8Pqo$S z4!-&Mc+O2_~28vd-%plUmCrx7IXmEAj2lq z{0MdS;6R)YTYRm9CBmhG-geeDT+hZJu*}g<5iw}BrIUMP9qnTp%iM@bIy6SZR%mdOeS}`5UgtrcUnFhz1rEs0xwSvvDun@JQ`Y}F5YDP`a+9o>c z#=$5D3H#wcz2xfsS;qb<@xC0CRokItLUTPFTupeW0Z2_-hayd(S^Mh94AsckgI3C? z^yp$2qGFHBrbRa^!hC>4x|xNGovDAOG)V&nv(20ZftF2w6yo&{_twWHh%Xi<6jE|u4+n<{X z%$o`y7!*Lpl118j?%od2Nv|gP;Hq*U|_0Y+(B&x zv_qt(TG>RChLGFEJ77>kOC;g^{W@Vpm(1*_BbwBQ__k_YbMc$=98O~FB+2F0g@4+z z3t{`1$Emb*r_Y6tfFwI2TJz*g0`0Z}?l75JiVn3=ww)`I5k8_Oawno`WVPpntglZ* zC;#|LClL|e{;~15ev10wkOhcR|9P?Ah%NP3KSFrgoBGbqoWM2vKHWN-I*aB{wt32Hzm|3-qx)Dt&^EdVYes~J$AsFFq7iyYrUsiV}5`_ZO zHyJj{_Y?E`9u_usWL93@m4w{TRlS&AY<&5gxLSF1*zq^1 zhnfQ;njv{6ZW;2NSz-C4@1PBx^{DU(9c|?hKO3 zg5AmjV;F-AvDC9F^;3K}V)X=Mr?%iT-`d6|GmCnd#|M=41s8%zs6#JKr~sEcHF9ay zx3AArSi%UKM64#@$}%i1J$Jp@4lnDJM~xz9_=75cKGWf0up^)WV5dqErwneBC(N#% zRCB3vCT)SVHxU{A2~&ol@o4CApfC_oITX z7;w7Uz+~4V@~Ww0?S-?f#7m(Wi)eiE!tD}>W+mI=Ivo5lYm8HN2>=KL3JgH%6KFKM>kXqj;b_RsXqtch8 zJo1p@zuyQsLZABQGzdgxE2)zQF_SfSLMq+GFRXGA;Uga0u_?N?YUPO@icu)+Rw?}b zOw+bi4ps#5A@Ov?ghR_;c73WxEIs;YbZ%MlPofelFYniwX`Qi{K#S$KX^l>_6-{S& zcE8J!Q)-*#v4mA;PIV|+{g(Lr;KWS9*rb&&oQp=dn`QMC6{7lsvO48$@-sXp1gH5@ zsxvz0{lMqc+C_F9a;!-r4rhTnib;3``T`_5a_;7b@Xum=m7`EFMAiO*cNmk5rK5LA zLXsy@3wOJtzr}WKAa4@fedg;op&qlW6o?NvTUXCxlEb^+jT3B4e9VqNWA@J|B59ZZ z^0*DGMru%I>0L&#i3l|mn38fwXNgAW!5|obQ)nfjv}~rFllp-6J%v zs)H1mL-~z^rTSL^H z56r05x=9ip9mvEQ#x*u62>zh;O<?h7D)dMm)=UJ#4NCVXbF%>6&;W{16o)4$Acwyi4^ z|It7wy~$m9)UqDzXr_anl^vq*5Is7&_J-z68wJ9ZHJoc7Incb+iW+%o;Oa|2Z`OGs_V*;PZA{%v!ddXEyHfX`UIUYs{CQO~Cy5 zpJX5vPTp0o#j^Sq=xFfx;T1zcPkVLiIc?a{%=q#+fQlhkJmnG~H<{^zgo5xGm{dW1 zmSANEa!d+X(Itnz1DVehlR!|S1ahYPm;2)Xd0BeKHAA)4;r5BYP0fEF&M7^LFDy0& z2WvQk(r=UO?p9F8Tv~}2YmC-3RAE->WVWryK`_Fzs=b7p)}8Q zBi!HT?PkjxXol0qRdRgeTJCGA*Bh&%xTl`PXOv<{A+iyL2UBQk{idIA zynfv=w4!oQM)~V#s#W$AZP-z(}|eRLIdD8W0l*rTpYzzIs4+obYCCv zYnOZRakugGhMKD5b-+?vx)|A)U9ssS<~~`O{Ht%E|FZwI;;u{U^6Of=KUSm@y8wci zdC%;WD(1f_%pvEU8Hz1a-!qj~Y_bUjZPj;srF4Dn&I7B4SCp}1JEC?9Ik;!4qa#QF zKnYlQs@({`?cOHC5Sr<>$R~ZMIBz>`%-GenleRl~A|JH6@RqEFn@rjoY-YvZKfb^w^fQ$6o$FbMu9Xu#(`;{x8h)0M-*EVUmDb@qjUk4=F!o1q?@ zC z7`)~P_OH#;Is0Lsk0?nzNOGN)qAms`*{9<+DPuhgJ>#*Y`FY`opz};Vr5^#1_b|<(7DVD6U z0?+tXUa&qv>T}rg{Srk>((=@}sVt-XslcBCa+1_vNb}A7qY{{yOFDNnK;P_9HysleHrfX`yB+_*eH(g7}+Na)BC7&$T*vtNLz^Gpt0|`6NKkuN7 z(-PG>uHOCG;*~IBzent`v=%U^`Isakz|~6&6Ps=8zL3h(D2d3$S15tAic@vWpLf^J zL!85uDD}v;JL4&#xnqw&ALDG-@N>=z7!g*Y@1MVYAY#tTZes%~0E9k5?)GuXOdMUz zZxsJu`fnf~N?zWy|MJ{EP=!C0D=5hc4o$%%HOcs)+fZLW-Vd*?u-4tbMK!zt5(n|f z$0BE1h6NX5nu@YzJ`x7a)z`Y?+}hZXTGowO;NtC%u#Bf3U1YSqX+tVEFk!3Db7wjw zTfPm3iB3NGBt~&)k%9h!;$ld#b_FQrFzb)pK}x%m&fgsI zi=OFu6+q|Q4vfZAh7?b;0KlGoa^M+oz?L+|!XYH|rxyBS3FOonE43iax++cvkGF&{ zqYtfg5(=r1nqvq&kO7DlI$@I-k z0uoMpHnVH$>g(eQQQ#sYobwo!O12m2KQ_M4E`Qo`R-BM4Bnm(jL!Eb{;vZxP{Gk6^ zd{-`bVO`*dN=$Ddrll3BI&*cBLiHMx?6Kw&VzEbf=ZM1`sMb?QC_)2HiBH@h2?hIS zCvsTUL~QC#kLpR`MQWJ!n6B+DOM^z)F5{0H?XZtHsgSv*z3Lxw(6VORYXzI<**gl( z+IZLFmlk*=MhK%}_CJ{3UR$biK6##RUj_HPAuwJ6y+aS4ZBfl#^)s&|cY~iu^$EC6 zB}*Os`EKwn7ErS5h55==stA$TW(C=Ki-s~(I94l5hjr0i)PbCa)s55!u%rB?!ohbb6 zq0gmkgZ>DNbl5|nj%;>E(2?gF|*GLR5IE3j2y)Y|(D&G`U>2sL7ni(2M7;05D@ z4{(;New=29DGiQt8}>1r2Gn#+M5TBLqs`kG$RNY?$iZrA*7_)`@46Ut+LY)8-#=U8 z-y-shK2^3tT3HBrP&PMVW7cO$K;1Y)SrspR>jjI|%mcgWT}AF77tkd4=s*T_M<(knt9E@*>K#hFl=kOiY zYxuXN&wXD&1rb$&cA1hQx+aqE_!_AKak84zgoRlIOC`B4q zFmTEUmpVCP4!_)!VwYdgUdZR5i>NLu=*A?T_o-PZ8+?N>xgF(k``q&{_d&Cgh+M}- z3@%w4_=@ullT=TlrE^WdPHma18I^P}fg@)9x1wx$$m;AWyz87bdblSa_LFAl1vL2K zeUP)(B(Es^iDfP^7jyG^g(t^cyLF9im9>y|eKNLPrJe~@;wr`iX_rcys3%~|9F()! z^-+Yod&0y3{HG*=0Aar^}yM^;S2v7c{?nyw;v}up6{p3pk_&eXW>m3w&eWyl(I<&>uc_<^0rLlcuU2 zoq`;9tsG^HO_|tz`mCU1xgU|T(1gOjS{A>w_IM=fGAln>3suUZcCodM>e?R z-#N^&=aoXP&W-#|ks!y6_jJ3c2gM=%Co7iRuJmX?Bx<>HUO6tW*LQxjy6z$Iw*sQL zZ*gY3Ev&VtEJLTgq5WH(P5^e!obsrPf5$s};K?Niq z&?7SO_vpXJHL#d?`*+jQoxV!H&VecpX-v79 zODXX&v*&Zd^Wx+5)lJ_m#_&Ti@k+49J9)@Kp~ctScE_Ic1v^!{wwX*dykqse`iocE z?geBIf-XAb4gn3*ZTg{+XFNoi74o5)5YxJbSbFk2A!ihU|z5PR}h4@7xlV{ z2z*3(h!@rO%U=szUh{AAKaM_=fLfoNpMM?uBrSWK7qVu{*6^GDu-4#RTxj8%2HJtg zwLsfVgsc(3CyB7pID&6?(FSMy%2K1cflI-{3UEh^-kdKTK?M62D$~UdjAAh4<$6ED>Nuu3%ng_f2GDQE=j8D6Sx- zClw~Zo&Mx3rqe!zZKSUEQOp4|7&6+rF=1!0oZI&hd*z(bvXDcy3$JA0j;VJL?{T7q zVqBJm*(cO$T5uX5Zyt99w>FFfdP8re5$qmkWn<4=;TRV6vkwgcar=7F0Hr|^oNowU zTJWUXK!ug=eUaXpF=n!|J27sh(kS9x={>ulRko=d_>mHAnHU3Jv_h2_k0DJ!|s)r*~!jA~&@{@!N9(lf9xv-D2SXjk1f;x&xG*rU-Q>|DxJ z`w=N@Aibgsrw4m1?jf_2%EUnbFK3SUVwOo5>{ESe?X;uyb@IbKaXT@S zN_8ybIb%Z3orf|bORcM+QRb?!G2zgy9PDF&?7QvUGzJo<638qPUd#sT|7Km;#T`m@ zFv&_-D5UvFAndp@R^*d18YNlTcH{fPov^uW)J?D>7kKKJ_%iYd9{5&+{QP1hBqq=; zsn-5%@R9KOp8fW=Cz11)m@3{bEowfYoYD0&?lUIvZaQ%Co(dy&vpDA@vz&4T|JOD9 z^rLW1*_9(STjE9>LcCg?-hm^f{)k9RXG@<{J@iKKDIX?-PZKzT&KI#*CH$lPA&TD5 zn8&B*RFU2fQ=u}p>C(%Oj0mZKkgXSWB1+){6ruOMe6_dvrRU$t0u?9tT8#R;5Bhng z8sI3ZgHqlfYF0d(SoiUO(}g?_{N~~c!GPqz`#FBAz7IV9bk;BFyTu9Zeg|fR^l3BW zMUTsLe$Q{%GwV`scSl4ZIEKr(Lm)T;Y8mcJCw%4JEHv^78EN>U7TKSQc_$gBrnW zk0F1>o_yip-@RoVxSp*tPp@o!J>u{mAQc zfZ}56NT<^J7deMV(wrCu;d-7arDO4L!r?sM%%wac(nBKVa;JQ{^g8EL=b)d_GeXYM zO1Zvi0Pd8F_Y|Rks-es}|5P@dJ|p{M!p{e#0Ga#srRS3|!kl%3%|5?4B}*+Q_1F)x z>D!xfrNlUo*T=m1i^Z#HgBkRdAbJ`|gm`8_bdtxO#|~$z5w_`amLDg&97S^cUXP@g z3Jgh1-cxu@R*`5-IY+=jn#)_|+@pHHPGvrS{oD(@Rcd$Mw|k2K z9X*U?8BD$Wbag;ZU5(^Jl zf@Agof9#rKL2LA`R?QYT*vwxuy-8T#)=T|jFZ&=QL1?RJdICb^COx(s*3hjD_{VR3n7o^(Jtq5VRFkS z!ZY9+zPj$6ciO5`A_)QNm&SQOji4PGD+1FAW?x=26ox$G--6Z$|BY5rlqd-nr-~QT z_V*`(05_B9yt$*N-3bA8E1})`YEw0ee(dZEn@3q5?d+I z+MS`q!xM}Ql?*I}Hr{h|u?oe2Lg+SBbl2N3(Bj(!1FP?;f2~osaP=cs#w4~eC6C(+ zKG`+5if32^k^bRJcKX=I%CyVq>@qytWa`V)yj4C0UPETzJ>zB1C*zn{a?bmt&h8XH zkx}okp5s01(OlDtuJoryH$*{#i z1DC=3)6MN(4Do!>Fzvo?LZ0tjfY4?~u>0%7UtE9kHDD{$5_h~9?@3~`dIM?rXCfs( zRhMhXo3S%<$dYzI&i&fmJO6Pns3Q(*@=6|UW4VuAX|e?igP#elsBwbbhHfwjkS0S zy&ILj(Oc$=YtEJ`b`V;DV4<+3q zL`C*7u!)q_rRQz`SwrsdVN=?QF*|iHS@OmZ?35~m)Wx_U6CUSVYJz7oj>ki|{sR`sf_`CMX4uF0IOLRHS?YP$iV`OZ z?C#dqChOAhHIyoqWw=P`Sl9uja$^dnk7}t#nUS9PzMtdp@6JjTI~1c_ezh7|SsMW; zjx@}ekp6NK;{QlCatqqLmS@0DC}{$T~W{SlGA}H z$M8wzBoUsQoD1yAIR#^8tFgxR2vD-e#%v0wlxrXC?N{ldG70iy=7~{weQz881DVyMO~n+mY_pHeONTluYculX99W4_xLMw zWE9G_goTD`7lqemoei{3(nf3lXII_&6Upa|q<;g4Q?OQ_R~n!~QVV`^zrUs%Nc1_Q7dL%L zqwK+Pt>IoBS!!xrr|i)qC`%nv0?9^!C#9wBlM`{r5^j3GXH_DJ(0-N)XkLi!r_3JmiG{> zl~tU$B6)*?>IwEB<__M&77cwnu336n-Bh?%3vZ-RVQD+(+%?Wt3}WQ?h9AcdOVdU{ zPcX{V!u*?8DX|B$M97r-Pp8jD89R2@702f>7%Wadp;%fTLh~bSyar4cy25F-3Y4o^ z6XR8!4J4avsIhE6A3gQ4w8Tn5bf-G0QUhllMy*P9gR^L{O4UMhtV&}Q4uwh8u^5}4 zuQ^7a8JCo&JmlHHQDZ?%&vG^s)pdX<+p9?EcS)!}Afj3;7Ehf6@+?uKugwVw z4mU{LM?nosLK7KbVI<(;nKpDMH$kmALAs%cHT*CF+Z-#!NVM~ z1jo4lD1U##3g1L$j0lMG@mAOrKUF|i;HL8INw;syW1CLR@A=Jeieu<=&c70BdT_z? zEb%A4qNcjyYe->w@2FhJj#}bCeNr_FYD?8TKX*%mvl1~(-PhHsBd9LL(w(s3#_7B7C?;k^{tLS2g@l~L=xvas5Xg6(4 zRz}5HKR%&^SvO}+>n#sBaX?Ga#Ukm>cnLd-oX5YD=z@J3^#ym1eFoQ5rMMN@L4Gc~ z*{Qiq^`4SJ3;*)dfbNzP*^ypVhVCcMc~p{uO_@oA`Ho-E+2F=8Sw}B6&jozaGvd8u zk1q?dQKw+tE<5yL{XSaCexO2| z7wRf})X_2bkwL>bAuBs!l;d)eUNlrKjw27TX$Rd|oKPg%NlWhR9@R%_WRiFB8vE2N4|2Wg>rb-3oS zaYD#0iBP@jXKyM^zI(z+Rs29{Y9(b$yc2JF!Y-FoU<~k!=!&xW3C_d{P`9>E!aWN@ zKGNniGHxz^?s90+Yp+UN-5~w=kO*!+HZ*)YE7yM8@zw8UA9C#Vh-^P8kX&!kKf^so z&(y2@edNO zjpnfo9`RXG^AD;?Ai?1-Ql&&p@J3JVnDLLJ^~$-orx`bc#P1}fQvp9qB0 zE`jyqISula723T<{RjqB5s+gJ{jDQWD|`6hvn4@1Sq8l|UaqOwIAtx&Afhpl!26d? zs-V+1C<)j9uxXO}T|u>`y?E%@6b$7M#8T($JD(@h9Qw+ML413^aQc=UD}N;Bx&wvu z#dJS$woe zD=l8()ehPCkJEPU&UCGhU9G3Ztyjvx$H3NSxjvS$dX2+}9t+_;UZnUrAEpOZJGiBB zJo$M;D7*knyj2bBK?>}7g9P1kn@?R`x@mxJjDSl|o@GEBo-7AKfQQmKRJ3s>aAibX zA>GzH^I6KXgQF!Bmv`bQ?{e@DZVhTI;ndYf%cJ*e=i!hrele@uOey2;)1}?@p@yJ~ zC|qMes8ySKZd!TpA>Ns_5W z1XgbJ3PuPQes^EniqY zU6YcFZHhX@dySc2CP{oc=lhEdmasIsMO>ya4h0T719Is^efmXyu5r$98XE9N5%G@D zNd*jrJ%SK>^oV-~ZGJi8?MxwkY)|Yi0}1#BcO7Mg9R*U}B5qz1q&g49l{i@@k8t=% zn&|UNKiMZyo#Jn+^BZ!RmNCSMdmfNa*-?wA5AezL#3YUF6O_fjd_FdtW_7t`y?lF1 zU!Eo;$yQd^yh&k`;>nBikg0aEl0J7xNgO?2s*xo)N>viteNAV)oeH6^BZ#guB2#cb zofd#gE6|ATjg`5-j_~s$8dg9TFggNtg_B>LuK^Gom#O29dw1^Q)z>a(gY(1 zHDg3dl&$m2J2a=e5t|7Q52(9KTLP~hE+wM^E{Clo7A zT@TZOB~L6uUao+<=V@2=MSdL4@u$-|mZ#jyt8&Nb>yx^^;lP`%?AVVv$D8ucr^{O@ z#%~I2J|`APR2aH+Ny(prpXXb++xhQ1$WKC_=|Y8Y83G~rfjba3#on*4A;!`i{0O>e z%`FcG-`KmKeS`+5NM-CuB}Ql9als;@`&f#5HLcNI?JgaCRj_BURV<&ccA1{O{fY%# z5FPGMFge>6Ay;8)StSZS;~3KQ4W?$Nw{O?T^9ziMC*b%L8z=5P_CqRt9%m)P?2Y0^Vzbi^?EdGG1OM(4A zR+NzmIeQaz6keAhYu&EbP`DE890Rw5BREl&FRplJ#tLf=KJq}M>v1fj!xLX3r~BV) z>J6i@Leq8rC+y&WmrlS(GJkUC1oT2NKcD$RTGfz>;4KBPg^tL1Z#G-~3)$kt5d0qg z>7|2+s;wtp<#`pU_Y;18=PNj4pKtC;VLG;9o#cS~N}~hA@Kn$6?WO+Cp3%oT-K_!E6hxzk>asz}an8a7CwdPDA7A4wr(tGNb!XriATRkTU zAJYArAfC^M=r-oB0O{E0epU}KxaGtz(RyBm2FA80f<1eC2#APmUG-nmAfv=VWogcF z5StQ6YmVAR$}yR`4TU_Y`2Y+GSE1v?2hGFpXM8#7aB&{nEIWFdjq%GVcf1@e&R)N0 zp-3#fRyR(KN2#DQ8Rq@N20J2YS&$8`vVp$>N0&OeL#DY}!%GS2kLWl?^n&kuV6`^e zxVUqWRHoawiLHC|6%?ehfOx56op@y&ca~^6;m|M)=)t>iyfbIK)19g4xtgI-cIIt9 zGQGKVJa>+&J>4bKEcFxA$`qI+Xh;;2B}se%P%~#uNjoES0k91n^shku-IcD~Q3_mL zm`n(`a(rqEyzp$WUpuwJcG2NL2J7Gufqhu~TXc0-fym(VK7>p7bL-J3&Kqk=Wnz~v z@Qoqhy(-Z1H$$M`o*1Rk6Z(P6n5PQ0BY(Gt#>QVomHUmAt(0=FJ-)tRt-u&21-D0p zdk3j5TnmUB*&janKpnphCva{=oDxN zAnkv0H5Tde9_odPm8Vi|cXql1+9>T%^GR|PjxqAnzzJcuNenuEpXO1vL4gmKbn|K! zLibYr`#|6622v=^p#tTfs_y!vWuF0MJ-MFxzsr>nyi6@kaK7VxU+cT8YjcBHjAQ$Hz;1VD!#$_SH+)+QVokAXO~Nb?1tU84cTIa}3}0Erhd z4``wHF+4q=4_qV}#8t@i$BRm!DI^kfV5`r!78;n-9jPZ8VP9`MuXyOVAW^PZ&8`6C zMnrCM_osmOHn_*h8?CCPTS2P*9G-8No)OkLzB>!wiR^$OVo(#6;^Op}4$c{7d)oYi(ZTz(>DIy}BG=oOEF z5Rn|~jz_@|{3xtN{&wFPr<&o1#pk=u~s3xKjM$!x$!o z5@?|Ak%>7lM;D{<@ug2{fN_0hohXH?6i?9}BAzod(b7jk%%aEqekPlpN1O)P`uk0v zFqK0&l~{KnMn`Krrs1)7u*bDl6L(O#y=-71k#ZeuF zNf2?W?sX=WQFQ;KNF;G6!S)^>aKDoHAr@B^9#Hiz1V#QGT%@Lb{x!k&yeK88s)x|* z-sJlM{C+K!p+}=X!39^PNR=*fo$h$0E&DM(In6t9&(b-01#y@pjU#FJ4VC$qmbDfM zk*y9S=#f%~uv}4Ft>u_Aw~Y%9@^B67VNUzQkt4fW@h?8_M8+RYx8_XqM9tNUyW=S~ z&dW4t9-$26{MXlu!g*n#h#A5|XoP6O19uXml2V4ml3?w%8cXcHU*OL|7486W_a$9( zysALbTe%o>QWQ!g0@K#Sa{Nd@yc!nexw6s_W|4W<@*=gOn7G}q^AYdx*QC24(so_7 zpWNaKAsH5l^`zX>uB-DvmwolQCWza8Rty znDCs%@34or2R=+PYvEzuXDbqf_XA#>I*6UkHJ_}`b{2OB>Hl)&L{;k7ltZ!9q*B&$|N z-sGZk*oEa2l{oj!k&E9J%0HS7rr|V zhtlL+UyA{9!TA4y&O(PrFw{ZX>JeR2@*e`mX2{Fhu6a*Oiirr?(#z|(qe~7;(qD9- zQSH~=zhw00)9*$l3Tz%JQ<+g0v%EB3eGfWIK08rxL(y!B)~HO>(+$;*Eu2xYbd_^g zLH*^-Oax@fFL7$*ndab$FE`t`(qh4MiVX{2UJO;aSB+?f|4H~&k6QC@l*I9Al+ThR zoGD7O8>wueI)#Y{FChgETa-8gdUUT_aK*7yoozh<^b+KVJIyNYoboaTR@ce4trv5f zJQ&tENP;5iaiA4(0@7_|xR#gSklKZsTzYY;GYF>gMA0o(zCcbQMKku`wF4tJxG|1O zmo^scuT2==ZVeF!#)C;jP!mh9V`sqQL&aRB1=ehYN~UD5agTN_42i6lqbFIXy)$j|Pafm~;_v#=y6 zSE^fM6!}yHDC~|Q9AtlkFDv^>ib4^F+59v4id8{7qk2hsDAwux#1~)C17z2~=jVGf zci>1w0*WoF*Y;g`Psgx-!}A5rpJp5j3DaaJw%h*%8xMqeD$&`xqrsKj?SdH?Oq9fm z7q91bN;rDFi*SL+VOIYkhw&M;ZA1>$!{M0-ur#)ATHzJzG0<%h|KYv{xHhzm5>|Zh+=OwxC@=_7uZc|rgwqAHP!*;GAv!pN-OTCJ>&!+N~yoi zW|t$iG_DialXeiRW(Rr5lMBA5hLS9uZXZ!h#!wNZEKPh|Nu;E_u$h*@I7WJrLFw+m zooXgDJO!;rdMcc{e#lI-h*84WU9kkIV(LzNkPC;v0qbKveR4EMI2jFkhiTZ);!Hzr z5teVVoG`v*J$2p$cQKkjJsLHC(5|&K-B||?WXrSGsL1@Gu{{McB=>J(XXa6py~D;c zrh;CFfpZC9PR|F#yhjcbx>twtFK5$b58RQ4EByF4lp(x9FvT62+;cW>iP&|aNSKb- zeHR|k8d572^aQAec!2Yj$w25L0Tt+2Oom?LzT{`N_o4#;f0QV8@;6*QBBY0 z6Wm!=yoN;mVW%dC!7J&ASD&kIwhI44KRJEf-$JKerIF`-xZk-(fQ`F*ajw+v`)VdB zg$y{8k+D-V!K;K+ZuMVxH;AUATkrCRUveI^gkIYC`5)rXKWbWT<(h+tsdm_JW+A}@ z0dyDURunS3(}*IyT(7Tup!3iePP@H$s%x=Q74SI((TR6nTu0ST%XVb7cDvjx@^gBF zF8JeR9bEcJsc@R5!S*`)heyAd`BhvlWJ0KI7((sZ=j($ZAmYMY{kc=#u?b@^ehI1lo%2 zn-aOW`OOpK2WyV`DMn10(aI|npypVjJq)-t;b?0oTq>hgqeo&S%CI!CryoKe9PVL+ z-@6QhX3g`|i47%axOvhF^J*bZ%7R_*wL@1vNX6itcU6+LhfC^4OxwBohg1PokC>x8 zu!7#E$+D%KZ=QDN(SBeEjXEkf!S1|njxE$Qru&;x;-eSD{Mw8TAa;FdZeezqOq7Ly zSH=3Q>0o5SB4e9i?^v#qg9<%n{$cRW3^(Q(1(ZSm~J-n-{ zI&CV!rkseiIE@53YH1*`oQJV>DqA}N2rcI%|AoNstqyz#*LygUlHpnmp95HZ%{%sC zO!c6kxQq}s0~Q7l_3!63C0B8JA*jyR=7v`^sD|wz_BG?Q`ssSw=S3_U&YsJ&8UsM= zLeUYCz`se85VQ&Ux)PWT%sO{SFCmCy&Ho*2A;Pan$tP@S9l5fS*-4%GTh;FvPVrOq zG;aOJ&>eh+#w9XFxp*g)ENN=Ns++wTwFF6D@Sd$|B|2LNoWxpx<07{rNX%$%sN)(Y z)BN50_L=oIY`9NzNV)yjY%!1KAr{RF63EMTZNPMU@Y6D}1HPmX)?*Y>Ie z6U;^^$WyccApR>|dVqVBW5B82+eY83C+Gc}CQG&ZSJfP>fnn-94@ODe@Q@9=y%K(^ z9RDdwXO|5SBxr^>PHUDKhZWjUi*54-#VHmW#SXX@U)PT@g+AKbAf*q| zNm-!n075b&hKGlr7MYg@yT8H!Kj><>TyW1$1W@OgzU0+frQ6S@1)T=t*{bdAQuJD~ zvD{!lm+zX=K-wuhG@Pf!Ac}!}X!2+RF<$x#;+Oe)iFsGbIEFIZ+|`Sqq&zU$FLRCw^Hq!d_ejZh$&N1uRMzOc3bgk# zpgkg#@70k}c34r8^7=J1x_j-oOZB5zbVFzYr;dMrkSUJ#h%NWC@pwx@Re-?IBT_Sd z%=lT643uXc;%z-a^*wg=O+2Shw;5Qa{_k2ebewGI09W5LgW&UiGNOEF=cBNHJ=@K9~p`hKMkX$?WZ@R+=Nf+Hyhk&ar$5k7Ht}>%1%aE z8ia!=KzLy!Hk1at)+OVYYE%l00)G?LH4Ak?EdC)a_>gWMwzw&9*G*s6L;IipC^tCC z=*~(La=u0TSEl$dvJ01N1Te`eAO((bC!^r5CP>B&`fI0vG*h67`4AeVJ)Z)|5))y6 ztjrXGPwYTT)s?Xw<=cvpQifW)3aG<^jfcm@3hUnn4!POG-IqZgDF@cot?k}hBB z$3civRbh!U!RWo%v{i?ZW1|86zd#O#=7*tW*#(N! zNK@SCqH|*{JjkKu1+Og!3VB?O@mjfFQ|51VbwrqCXlXJ^q|t>1C)VQ_)0lQoQ__Q1 zL$XbWDWMe>H=F0)!;Lb-O=Amnd_oCXd6M7_reVoH?u&Vb!=w4VcX7Bdt*bsZ!QfNuK$D=ozC%1DA17IC?&iV&l)~6w~OFW%q&jv2>md z+IqFU@ggFw@^H-3R2+RUZ`vAe^bn4%PQ#Yb#6DG;#RJ!(a%vfNCbd3)R%n?GVDA`a zM~^>!_dt~3w>4EyR{>RxJhT9Ifce(L&I8 zGmV%&;ySa|4FX9PI<0#RXcKJL+_Jl4L{mJ;eMvkcR7XQo+);az+-4(WdW`~U^b?IW z1}PM{y3|ocL%9&J!5h{EukL-NTdEq5n{Upi3>)u0{Yc*)T^0B2TeR-QwERzAUqz_o zRN+i;rtkJ;5$@|+o`(r;yGJ&=B}F~t+B(C=7kLsxX^S%8sgJ}`x-*&lKQ#~2$|^Pb zfNMp=X{3Gv_h(z)WvwZRgR=k?cBziHzoqGb&cPbB`Y!r0ND2BySlu-yZ#(-o%UZP2 z0hrL=FOG0pMOu;a^m6GQr3R&%dZL)aGzqzZpOdF?Ca9=yzeVaOJ71f->UvV{y!i7b zMtoSj3%NqS(@&gCXF8IHrpRAz{;o_n-1Q86^yF$DycV6^yY}KIOc-$;RAo$m9+eUt zkP>=PnaUNI==JyIB3R6q_h#g3h&EttT)T8;!@+5^VUj^eoP`esJ_6ALl&PYRBKJP7 z4U!=x73}6LDktRktLcE9m@UB7c8LsGbf)=>l5YYbsP1jwmtYTckI3^c_bV(2-jp9H(1-t zsDgv>Mxph|!humVkf6ir1M-O`e9X91BKY zn_@eoEqHsL?)|uf>gx=v43CbPmf`IHXc!}9Lz4h02{qlv8Pf*)NGVUiHNt(}!@T*G z$xF{~coa6vDutwY*D6xycKU0$VWpj}s)_$&c4Jb*qbX2Jlx4}Jmla5No-y4P^Oo@w zi2)oEPp*NNrWfK&e>Qbut4u$@ZDjB=WyN%02UbgVNjKXbg32+>iGVK*!lcI6K?Lq` zPn;ZEPKlrHnA{0lTGbvVwM3<=pvC07%S#f|=fbeuC|*K8{JtQ=F~~;g`TcoGN4FcN zuVOx5s#$XQeA7e=e?p49OrDrvuP$W~IDg^f%y#1?nEE<5*HwuXGhZpz*h*!tB|prSU!*P| z$&wD+b{{ly*21YM8JAilFK8h^2rYT~;KH8!ZSMGwEMLj0!Y}RY)F(HX8HHJkB)RrV z1X&dd16ftLercAH6&V&yQ_QN2Fjqe-8q){&iz3y+P18?!tFP_$MWeL-ZTzSa^TiM> zU}OYs)eAuyli+N>WRq?NDlvYvsJbv?z~oe2>_n7b9O^IJ$Hjud$f6V`iJx#zEw1`` zwhVFI6F#_|tkatj!rAdr16RicHWL|};auzj=fzA}7q>F2H3j(Gy0lwN7w9+4vbQe7rl<-T$;*Rnpf5&7fAZ5~8 zv#BMhX+}#~=;=*u-Q_E^9o(L>9rJd<+mxC{kVCl)yCdh9%Ycji@1T@6imx^kicLQa2%7Nj6Y2HUu(L^Tx_b>cSR$GdR0 z_)!oZW&CuJ*I3DFc_&|T1sR7eEOgC71icdvrt_Vx3uy&*Wz%r-=Ta@k=*IifIYp+e zTmgAyYv@X`Dww@K7$+RMoSYR+nrzC%fn2X*dUmbfnc;Tzg6#)sIW+g@Q2Yo8^ceF@ zOi{I6n-V{TR>7|xh(4V!0?*&G11`c!{(bzL+2i8ZsGWM%Du_F%dzHBTouxr=g!lZjZDu1yaw85#AC)v|x zG0{EA5|&iaJr+#NPT1~80+1e41mKb|nu|3D2p8IVwXIKYOG-DZd{#0%LugD}3U(x4 zNR~GOX6jsq!bkcUS=PoAsleo)>(PTUB17P;w%cLixQXo!lfdLyGUPEXrV}CWS~LmQLUAQ z6Qz(*6~mTpZrB$Cu7ri1N`iiE;x?0$PdJ;G$=lO4-<$I;=)r46>ek5CaE@Q^d49Qg zeTt&;(?d(xBO`i-@qIwxD?H!(e+FXv$=`lXP7{GOh-svOw(|DIF$@%aTPFtA!>nzrW-bJ}j6Z3VX?fySI>|N_a;<6!X z{dCWWMQ~rbz-eh?TB;189zAHmIcdMsGh1-9_RGXl8qPSQ$X*`MWvNVajQ>-Lx=k&EnfLW zRk$INVSkCHib1^8rBXW+N6OCNrZK-6jhKGGlC*IH${wXkp`wm7Htc}bWrv>Z0Hzqi z*%T&a2wScKeQ^NblT4P2WK;08nOx^|8D!QTA7Phnr$uE>_}7grfQ*<|m=7&Y7(6{z zKMK4k43%wx^O?Ogj@$%5nK*H2iY=&m@zMbikr3yKb7vZ?Bqp;QBl`6xtvSk&JY1wl7lObM`Hc*YbOf@ofGVcgK4AQ zxQ0y7RKQUQL~)ml)}uEuMdc#(aZ)mKVp2+yu#8I~IT7tiRaFj;Yj|d6%YjoJWe65$ zznt`RSX1(c<$t0+bL%N`hYpR_r@+|?0u#Bwzd!y5!;awQk^yGl(G&DitPMiGpo=?; zS8CewRc4-IRr2P`flSDow{i-T9GGj;o01_xEt! z`mA(jS+0ZMb0cyr4}@JAIc|=OA~hUR4Lphtf)UnfI0jji^s+_Dq+xV5Ji4)w$;`*if_nmTVvOx^{4Pn7*0iwc!$j|Fp&vi`aI`+e{a_97^4UwAgntI0c)tvpSXlFHLSV_DypV6A~aDlO;S~`Ddft*!3)bJgSHh<7p^huu3m5?-yM0r)TimU3itR~ z>kzLh4v%RPw(v`m#PPIOuVlMTR6a}~u1%2=yr?#I1=wVXcEz>+VNF{|Gd3*=O+gVI z8ePPZyup+OY2Tr#+DE03&xDKP6e&CFb`9;DbWPgN{?@C^p!NX($D(3*&^ay2GMwfz z)?_*5YNHu!uRPR}PV?nxDr$zcjXQP-wKVyx?P(4{iRm!5oS>?B5ILMajaDj!!JhcH zuq0*xJ$+80KBLjXP0dD3R&lY}ysZWki9mWve}LudBheMsAjn!rq|1upwN}dVV0Odz zUQn=kUN@ChJqZ6b<21$I)A>m41F09kUSlQ5#YkPcP-`+H*ApYx!34@?%xb^W{TS^V zwI%&!u4vFLDySFDsI$&6L7N>nYEbTo=O`{ghr zMS?|5`n=LmBn}sp5EDT*@0qw@S$h(D?8?B@Ys_E;KI8A-{5clxIv+AK#kOlBKE;Y^ zm^#FXfdB(lVZuB_N$?n{J`uKTmh{2;-x6(OQozF8NaV+{p=(OBAmK+Yfj$Exf zc)P+U*mB+2^6aJrQeoE8XvMTFoqphyO(H`ynuCcIXLgKtp{5WJi>NWR8lG4blt0o~=1*L; zF<}QXH+{5?(Yd+mX;#lUYZP;LmtjO<%uf-Dy<^6e=Av``)P{>1H)F{9SuPWP%%c}uV8g!g1bvSVzaN2pi20&BaJ-BsV>eSS~ z9JT2s-xevz?YT5y<1}T9IIOgEW6{MV;Yo(IiI6P0Q+>?%ryKrr5MpLpg@JN3RMM+F_qT z=cQis^rSHQv!Me*Dh+yR>d9?N>yKjCXHnN%MW?%m7`z*v5Z<|P6iDnt%Y;y3vIV}D zMNkzbMrw$bDh`|Jpw42f&#P|9uI9_mW~RX+Wt-B-zE6`stPKGzLBn3xzI8p}QwskY z(I5WaggG##m^+K!W{s2_5xi0|Mix>3(R|I%O98FAqGsMAXz)74nNw>r9Nj0C{6lKQ zxpDf~tPbpQLG6x+xrHG9K1^e6Fg{YxB@TXW4}jRQhQcXl^h9@tfG?J{6pxB^PWU4F z4m|n}Hxn&44`CQc89p`mhvAkvC~ zOz!)WWB63XpVI%JjfA@mC_L$X5&rPv`%&VFhe<=sV{D)Mht6E+o4~&RJ+Nv1SSTX8 z^P7eZGu{7&h-cdAU?B z2`#WKu1snW$0S2>8NiWpD&Wb^#u}02hJM;wd5o`{w8tql8p!UnXeEU|X6eB+=~86U zg?Yyqu)Z$q_4q#*~^Ch zihM@E1^O+40|dGZV?|#jGZt3BV*|Ujkb(A6W$=86nKQ;COC!6}%0YY8gO$>FS1N4^ z;UzF!hZchIA;^Lr1&ClzLqgrb8QL&*kbipT4U;FZ!W0MAMD|%TSflDdP1Ntl=UAqv zZVAwm^khhxYnu5)LBvcSni~jdhvq9(yP{P9EVsMn;q(i~xhxI!bNvRHb$R)07stlw zPdW?*A*Cu+*mO}4{R}x-cj}VR>H5~=CVP?_4wbP z;&;KvLpwahvVZl!Kh#nsyQU;biCC=}K^whTs`g_88n;Fg6~4mVj(IlpInAjZ7U0ib1dX`hdu zts;8_jAb#IXga4+N%^_?fbfkv%v6ulF&g+`3fs-yBOdpwu9Hq`8W~ANXO`mymgx!( zzP-LX;~0l)h;qPU-3TzgN}U%6qDK*}79t*fKrUl}L*phpub-8I-Wy$v@a7C=-JOBL z%&ITf0Rr#FtgZe2MA9hpyR&G4@tCFYlwe2+QgjY2<6(>%x0ZjWz9=Cwza%;AHiU>mg8vM_Rt)M5gs_*6>|`jJ7U zQ-fvGIr1)R{aMack)|TG>fz8)R<+Hd`9lMo1>-fx~qh2`#aN?y|F>=2swU~8bw#JC<;z_ zaMKW8XGMQM@mB;{mc}+wC?4Q6W)$ZNC$4vK?xK(_r$d~OcKYt7!r~Y*2Svs|Ov)=x zJ1s1FvQLb?PQu}}NViq>(9-~ls+Y_bBF))5Q_@wje%n!3r!DEz(7KjxKW80YKB1F-wGmPAo;7TWE&o(Hs8uQGjmK81U(uo11I}Z?jnS6 zTE#{EIcydCB4!!{TQ*&!LVW`SkFR$fwNwbg@r@tUS=PEFi(#5Vb{4lkNQaG}^LQ+( z#n|B(N~J(96eiC-IZ64gMkm6?Tm9Q;*JnjtMfZVkomC#gwm&ZLdEL-#@ucS$v%Pdt{aSvCvf8EB~HkU-@RO z|EtH#v59`qnBvPXiyhhC{^_5R?qBz>_fv|_=WzWGhhkfe8vvG68qZTN10$Ln{nIZA zzV2sYE4}~aRL_l7N%P;I&8U-pLWQOdyj1WEd;IzZq<(q6g-D-_vzLr`QtRI6#oi8S zDB!5dn^wf~EvJ++z4??LyZD3A<{bfz7y20W`ZJXEs-#qkAX0sRJJMb|w?=nFQ{pZg;lG7J(#@h&s8Zx->kSgV>Yt)>7mDmunY#lckqd7DI z7$<}?^hFK6mk@rH+h{Du!!M7Pic`brfRDcvGDN!?yrB8;c_|ddb?xMFJ$OA0^7=bU zW(D@^s0IPmMVW6tw{b@V4(x=qt7!}EGk|iGTI_bMS!KR_IfAHo+j8j=0$s3N%+ot; zIL7G8IWRq=?vgr%WAx7gH>Z+)&rhf*&u@AKLqWR1dq+kJoNqQ#@7-

OY4lh6nR5`-(YgQ!mscx=JkJw{d41y5(gFTBL^GypzCTg1}s@>zh4Ni zGzx;k%L@W>e0v|)UlFR>c;y5D584?WCi!Ad6HP8VT}2ub`VF)4pPwY;r0NtIFw&X5 zDg;BaY%iAzYZcxS4r(po>zpgZkwvOEeJk?TGioA&ywl?#TRyDod$kM#>&s}2I(LO% z=e{1k25bk!5%D*FTG$ZwCQn)o^CwSQ0?2z8kESzG1%(KMP{~2a_gcV6)z3+h@44*8 zkTo#G*9qAe>-iu?&Fl1VPlS||4H*WqT*I$#oJ@>sVSfaWzTMaqDIOoPFi`0zF<4e6 zyVP{Jv&|SFJ1vU?+#d@7-|r#qr99~wyD{Zuv%?;Bdh`p#PPfH>uvQG_@;06MhP>?X zKd8#k(hw7)t3`&ZZdrU?#Os443$N4&1-6`^7^bGD5ZUic>|Q zhOaSD!}cTGJshJvPUJltk+GY?jl8oBWG$wVrl;Pv-!7VXS*q7ygCVu$3|MTHKC9gP z8C+Z?l|T$EVtZSMF*pqFqSSU{G@QfpN!gG4-rO6e;z|hlbrb6Qf)nEE!bs{8is|gl z=?O_%?N(OA7)7pAtPEsdYcS7?&5(gOW+t=kAi@)Fd9;k&jj>KFA$+=B}#mKc7+oZ|-jp6<8pEgst ziqIEQ^+GMDPoP@KA#u)~QTL5UGwR-05Z1t)1E0llNXTKgFPDl;wa9cDQ_pz$^8lu9 z=yj&w_Y+mI2`YS^$Gwl+1Nv5r|HD&c;?*{@N@kXc!L1!ZPpO}01_b2GYkFd>+eU5 zbnZ6q1oQRQ!iUg|3cXXYF}PKw5pz{WIWAvMyTyOp4U9^_hnlnt^`3Zt6ru(u3*MBl zWtvD_dMJ%cDPN@0V9=|yfpga9a(PtwH~1OR(&f-#uuSeRjoBT9jZY zJ-#~7kxjQ+!?af9+#Z4WaA){(@|dL|kjHvjDfxa^tn+4y%M8TRYy2xNEh8%3@^el3 z34Z|}b9<)lG;IT^apkPzy};ZJEgiAU=6E*-Fc!I~K9Mv5@0zU^-M5ZoE|u8g6Jy>&1LlTkG!Cr_%)vqd_)$w&4UB*3%>I9p+DX9Clb+87>~(Mi)zS zqf1OZJ6gR1qSv!>-mfq1-d&%P;>|rN4e_#%xk*r6ylZXtHZz+}>7tewBHrz%cFiqs+vmodEc1b!XQtBzgK}n_!Y3Yx7Qh?edrC9h0v*?wTLj z<)Gq=e!;84!QkGjqb{clhv(m|FBs?3As<3RVnmfSST)bmdtWeq`WMNt#ZRt1zryW) z!d}Zu4^W>j_>~s=|{^Tjt8Xo|QgT2H`o8qT{VZc~E(TAA%yPc0~ui+bVWcJI4Te$+Cj5W8H~JR2(iRzLq!kzIG{Osm1j5D_dLV5H~-N`QEGh6?uGMtTc`EX?5Vny zx7vqGJ?xGxCHUSm=uZQQVKRcDdC(r3+g;_Y$IA{s7`pTy{&pHWYIcP|47N4dkC?84 zTuC2#U8xD?NP(1aFOuu&Q=oyQ(GoWfusx`!(~FqsnBFrOwX#l#Fw;!SO5o5rDhv+! zO;~KvaYv~Xy>zT9HtNTxZ+Oa65o#zLrWTbNgtMm=T-KMV9o?7Hw~|0a;Vf_GW_M2> zC7IIOf>4WPO9-IimGF!Xm+stk3=9$Y-~m?ox^9ICVz|}7pnlhYhVZl2ey}k9aIn4u z!+rAD07oW^YQlo)1j&5bez5iO6I}F=p&-sV`|({v46&O!LZEt+<$X6p6d|b#z9pg6 z4Q;6^o(hO7d$e(}5lhWWwb>TjSj&#>1e%pL7bX-8(EDcj=&*)BSKjP%p^k`>7}*t$ zlAF+qzE)IWottMQt?avQL5o5W9{)T2OReqp&T9iB(=sg%tx8`~b_Fj$(ipw71lG(j zP2?a;>$h;k+Qy3}1F)5{j_d|AbRM#kl&heJ(VU0-WpLjp@`nmgWCucTL$y zpq7kPA*&{{9IxK4D-18h6N9aZV5;3Qm!4l7HfJesJ)&kJH5fi z@U`G{ey6GKuGws76q*Iwp`t54_HgZGI~Swl9{orDXkKKRg=VGQcBQ<-km~3Y?ENdY zxqiB>$olcumjxdm>Vn3z=9G&U@wH#0xf=thxH}aRm;4ehm6F!W&zK>5{Oj+MUgPvW zlK8FmplJ~@4s1~MnmBKRo*!@}zv}D;`mH|x{Aw*S;@fj&^SqPt?(fCtuB2f6tl^2S z_l%prMMI*O)IeLMO#XMCW;@%KIcA|h*$GLL(L5pnUd1sPMyb29YcDOg=fkODd>ZFd zy^lN}|IYJ0hMY?b#~KEl6Q*C}-r1dsD@iX2U;ie#TVZ<6n>#VvOzI>od21T5NqOjf zx@+e~^;KZ{?1?ituJ_4cw7@J7^>I-V@ffGfLGSLH*x`Sau4~=ki3*nTvOEF-H#RlJz6!Y^3C-v{?TI-dI(rtPo?a;D z{$A)F%1F03{N>PdKM6@%QV& z^Nq_xe_^#ZuKCa!JIN&VVYq~gH=-C*2I}_r9S>Xu?{RTBCJLc_+iDCTvhsef$zTzq^pemwB=QON)V}YV<--A)t#wJxJ z2Y1_s{_I`luwC1C$~jMP8>~dYR=shKT($J1!?~=C^OKHMz|rYkQM7F5L|0ea zctIRuIf?$4!IB~vCcKQ%mKx?s4%IDx$gN2lxjeMuO80~lK%YAj9{+(i1J#Cb8KgVw zN-){@=dpnj;w14eUsJ2QHw9?IB8EG+8z(0jkBiF!>fI{JE(`a4Gj**%!EZfF-RV|8 zA253^o-ktGWEEbo4Fq+zWQJap<&wQv#?~z!7>s&rT%6I_k@^3T+<%hv>0_Y9J^fUF z5qdEe(>t`h?KZHB&u|~Ne!L+*Q)4-G-)eghXto#kO?u;L)7SH~;#Knd?_T|zxfa||EG5rO$F=Ehy8UZgL0Xw$v<+z4^=RbpUm^2ps2kOo@ z+C%(bYS%p4&?xt$p82MO{~8w5i4FFM547IteV@N%x;s?<%6|HY_j2Wjj_`YKg0Saq zocM9(e`LRl;${>l6HYcKht?dWN_*ct5NHRZMu6PFN(!x}-jy4ufyaqxqp2)w%$ zh2!x8!#&ChjJI4u$ndz#mK_iF!u8=%F+8EoNb*QKqn>1HHs&hx-2k!{O;lI@Y2k#| zh0WcOYxuqE4g%v9da-p}^egtftYe%sObY=AXQ2}IHY*m<)b+yegg=^YVTdPZI*myH z7nn1FwZkZ&zR~pxzYs{nUnlP5R&XVMd~v}u(aczsW7!4!MCDZCVW~$OXrr3cgcQcT z*lh4;OgWC5*o8d~YTz`chya#>izcSzZ8!CC!E4Hv9>4uWDeJPXW18B$YNZOJ>fmTpS&D~NV4951 z*aA%rOLAzzqSdF-7wLazD*3fHbmxu#eI2UM)0rod?=z*ZIuct3tKUQw*y;l-Cfx>M-;fzd)>8uA6R55maJx#h9zM z!<5a@kJ*e9y(#!6#)>!b4A6XIE9=a!J^-Hy$@gO*KSo7VV)H+8G@R#Kv;V_UNA7et zX(ur-fGaua=$UR=6p5!*y>vIB8H5NV!WBCUh)lN+M#h2*0ododYTNACc1lPZqgV+( z*jAmJEmeITV{O`$_?cVH15NI`dMzehh)&Wr{j;_ppK!IR3JNJXXZ!B2l&p_9jqj(C(2hHzsGMusOeaD0;g<9c-y#0A%Zy-F`*v;h@Gv9zBf8X28-%uu2J+0TLK3 zF^cQ)W!3<``-#`gg`s2^kGX=M01*@i+c9n)GT<(LEai1I>1fq6xsaLj9N^B%xFYEm zhu2DLFR!Mh{T99IjrX)3q|c%c54&BFJLP+9p#duq^L(P*t7%I{}!DQQy}#uf1d<2tWx7JX(61gG+UiVNjYUHNI9 zP*3nnyYrf3qCn5vyFG#I+?L8T?y81JpuXMz6dO!O1oQfZ_VpgTR5QRP`vG2!2 zuTJdOt-sNM-5mznDh2zZ+0DLF#g))1x+F)IVn;c{ zjP_5n=7Yl=jnO*3tNU+SiPJ8{(^k~QeHMcx>O>QdP*@K=Vw3Cqx?ljnV!|lLIuUEYcQ~)$eK(f5eUrJ$zY@d>fvM@>6<@i>V1rkM5eD+69Mhv9G z0;xyq^1L23Y;L=h5WtaeF{d=ogVuLse>?Zfg2RhXS}U=_1&~oRy5_C@_V^SF&BD5k zStkh$%FMt$%w>e8oW#2P&W(}KBV1K)3(#$#3ia{ZoliIx_4|(exqQU4yRk>Nu=m%W zbH6&m4-TV7wZs3g6}t=_H)oa?4^|^G7rdU)Dc$qptN;TrqJ{rU0rBZajaI5R1m6>Z zqn)DY^7qVc?d79yLXMF8KS@rl6Vw%GL-g@J&vD}=Qd#@HkN7^F$I$NI%NpaDn-}sx znYb~167h-(A2N67>QD+Y9 zL#PKXK}Ke{J%2HO!4S=t!Lm6F0BOPq=~3c!hE<*MUQ2v6wn+as>zy3`TKqKOz3_H# zL=RHIq03Az2LE?}UqruB zu_?ClIpvW9Tt=fP=xV!p>d@GD(YRLwU20V2jhqD6@Ky$bKsFkg}O=u)8mb#8U;&_p77&xEmAlpTh z$3t6*8^&6zrDTbNMre&~2!#XaU)$tB`Knv!vBC+_fS|qCTXJbBghfzzKvoKHIJ|Rd z@H%1B`QFLtVIy8}gd6&G749=rcHL>LA&Gw}=n46e3i*4?${l)DqktS=Gmb@k*B3IS zIek(umyzVafG4*;#|Zr5=7%at-aGn(!nG(X{|{0-9+a^G0YmWe7e(3qhtNCB=f<<` zt`n)`Lte9O&uL7Y-u>%ibg2y6WqqEY+s)4xSsm%%z^HG>PHk36mpaECrz1i)Q=0zS z*Dn8h0nS=OZ+uH|H@^L+gF5rhU^1Q7Tr=9Z@;d#Sx}$hef5Da_`9i)Cbjiod;CGxF z`m3Rz>X<`Gk7V_#)5Ms{19hX%iRt67RLv0Rd24S$?l`wy$QdDPn495095oykR{@b& z(`{6Cc@~W}?!WoACH!9IpMN}QUm}v6py9!vU9LG8B(q1tW|lIJ zk9&(naHKH$McbfXUR5=nd3=Ao?xXc;x1NghtAN&Ml81rCPa&FJpGdehnHVh$_hY@} zDBajmM!cVBb)a-o!hv1$0D!UK{Jl#IqW;A^Ia5=I@_+=LK_`io0zR0=T9Tu^fpMnq zh`+K|goCU-HHg`;$~TL03`lSA;pmH#%jeIu#h-I1YDqyzQ$h?RRi=#ME*M ztk}e-)H}Y04_3-LnM|uXAUkB#=!Ri{BP?U;nelCjYO%Eu>DU8obWjgel+}B89euBt z%IQ6&M2;PY`)U3mlJ4cgN8+sE)ZKPrefiGwag%9!zr`?D2$+rgxa+Kta=v&= zO+l)JM=S&1Y78&^qm#Lz;V-m***=P%gCuU=s&e^2I)7eHhAs)A*{6S} zM($Aa?L1myDcfTH%S5H@tE6#e+X)iun#PYGP3^EZz7;N<`tC`_pdxX$4=s*1XGq|G$D9!zx=diE%{Z{<&`Q^hfDH_~1$5KKW!aMm;Qn`LL z6lE1Sk}0Ju4FuD#uxrxvkv0~TX&Cu@PjeS36fk+MohgG&9}cj0f}5cwbma_X=3Ksn z`4nd}?`M4!9hz#T?&V|MIya7)cjR|$&guODFQXh}rgEkM(Q#uZj|J5ULhf6G8KHoe zEfNy{i_jnvFj~*mUqJ8O$KL7B8yVYFUsm&`0_sG!GM{r^wk5m!U%y=F`+w)?4gIex z)Y!jP951W*wAPz?v2>6Py&4^0Mpk>rltMhkrqrBXxaXyM_^pBoy$j(drWJn!n zD@caP(Hj|wiJ$r*F~&s!131J?gTbJZ2#6lh>QDa$>qjq^Qr7R`OH-EV*gsNjj=$z< ze7E7fPS(|&;ZyAIUi5V~_)~Xdu>sR3W3uILw1!swX#NEbn}U*Py&8v+ocXpxMfc*_ zVW_=q)jF55qdil(^0fn8s>U6fKL55x3C0-ylm#lS)EJH}d&j7(7x5O&S{uR}J-4jp zV|VoS$l3n!hwSS=-f!q@)RlX95EGW~7LHMW@o9bgI_t2+bm8Lm=nfYp4{IZB%YS}> znj$uGrgRp&Z3{#TEtIS@cCA>v=w@qSlR#}L4uqK5{)81BZ#aLk{h@Al1@8f2fRdFt zWv@koFRPjJP+J=#c8F~^P&DpfY+0lD!1_^4!{lYmhap`}x7?YZ*!~b)qWsbO@~Rm=EoO8=?Y6Tcfh3LOoBo^UarvZP z7nzF|?x9n$zkKlN*vaSb%mQ*!C^7xt4^LDbQS0pApNh$Qdc~iEBkS__0?i&IY7bD` zUHVVW{%e}hM+GwQKH@Zj4Y!V7&bSB_?ryYlLmY~ za|CjW*B#&9R}7ClCviHo=})X^99{VtJHnX@lHCZD71%YyR7|{7 z$NUbsR)*KC#DK-*tOG7Q6L+BJZI7H@Ilw)LU)b0Ac7FdR{r_~m4&d()LEoVrbJ_(@ zn`O8jz4x^J_Zio8&--UcxLK!D;`a7tx&g=Z?r;syEu0G-P9R!AK^vc3yC4p^`6D^r zbesBL*jmRDaXw~}sCm+{f(cJ5L;(>z>;%&zgLCNp3Rw)y(}MEt`Sp~)W<9dINwT1M z+kauE`WhLw6(dAU;-yyH+|q5i zUQLT7M`a8A;_zSDrU$e97*6#=*1nc-s0_E`pO%%$SfA+NAF4_d?8e{)C9V}J6XsFRF|buuVxJF_!PzP!hyl_NB*e12 zARqiH_FbijuPwE4Y`vU^m4K3YM$bHvN@+WqzS76$ z-4}^$7)Fuzr~N&bSQ z&m+Vit+CYtuCmel#xMU13UP3Z(^02E9x|5tln%iqqRmxo$J26xn^4GM6E^TKZuJ!9 z8X697A?}_bON<}H8J?!-^)p=l?;{q!I&1+n6*2rGmiN$Mh%*(>b|`=+^Y{Nx)6!EEzY{($I)Ez0duf3^%6FLIYWv08qsnr0-HQYs20 z4eJ~>7l*_nZBiy%8UUI%1+5GhJyJG|t&aSHN)>BRO5>eJDy_hWiOkv&0S4nGRb~A4 zD;plJBr%P?D8Mig37nVPkB%mSJ@haiK_NDre*5&wO8`8$SnNDwC{#H6z2UL zD*1N7p)BwsIy-;4i_Q6!HHo%VaYRJc*Swj5JdNq^KP$`2{SLFIMK!1MwD7ddH4`gn zq$2>t2IjuYe7SaZ0RbYq3zuTOk_A;K1!B2BuDZ-VXP?xa&Ffr?y)}6XI=p;Rh;U>) zNcP~Vs)`UZDoNbeEstx6nrQG+(U0XWORflNS~r%B&~o$)qGj4KP-ai+J1lIXJs?}E zNV{OZ2bA)SMdAy|R=%f_mKBj^sQJ~7ljM-1#gfCL{NVGaKB8oZ2k|JQ$$-r~YLpH= za6QEFJR|S@-=ew?LzWE&Y{Uuw{N03CNs%3h^=xmWN<3@PYYlg7nYJXD8V-*Z}cyKN}gle$yeDnO4s82cDQ7!^vb z7^V2^vJQi_Dx?4wb!H;ziWwsv9QqKNG5C1vTyaBG{)tbuX3%xI)bveS1NjfI;dbUQ zSR_})&s!t3xrQ5IbR}#RDQ+8?)|0Q1&Pf9;0put$zccH*ILxZX((GG~I~3rK;AoPB zq~>jLp7|>D(8CTK9*n4!S=g|ZVOj(zI%=g!f8#KI&Tx}rC9Sd_&OwQ) z$N_D1Je?E#1JfqH;+FFD#a&+)_j$i+DXhSC4^(ebW|gBhG_bg+{3=4v{@;^;AIL7V zMTwN*nVNxqsQ#36wyttxdYp-R;5Kwkzx}gkgg+~0l%qJKC0i=M(jD3{-Ol~HY2`ClzCIw9CG zU~Mq#k%OL|z6}vErlYNLRzew)ELDV0o`+wRQ%ArsLi^fTHFsCcLT)@2>faxw;Ol2t z6dxr|Aks{3&rcjg$3!nPFZ^~2G1)2)s^aj)ln`S94SM}NKl^BDD!&d0_#rlohAEoJ zoicP5r_eNSU+PY)Z^b5esAR6UvXkv)tn+5tJ5@!W3TEU4LnO=E-9GyqVsG4-T{2;= zOlGl&UjpDrm)dH_sVq@-w;$buHW>^9KTcns4OIX6)iUG(Bj^O?=HyiGw-eE(7}s$5 zzi)uwk^j94l%>`KN;B1-TEjpyCd(?0v0&^4jBVurem!Cg=cL1iR?;Gmi(N~1j}j%} zo2%Kn(Nx_gMLZRFbiY`JPkXAhX)4At_m;#CR@ADwnPQRx9c-)J^SqnWt1L|HT8O6t zW(BG^gk5B+hQEu#_8K;3O~p$2q$|*L)k&{qiKcjg(!rHzFrNNgJZ2EOjDy_7@8;te zT}0(~R`5Q;*^y1Xtfa)|%Yk*{ve3)l;<6reLULS^kC0+i5d>PUe#@>kgJNq+q-M$2 zgO+rRnI)VG`>BZo&F}{j#e2+rh;$296r2W6Y=_ogf5?*&S+AuJGaSktTC?xC1_yhy z#~zNh)nmOF6L7?wx_8?PYj=!UaEw&k89=-u4?K;2<6kEvM{yPTZCh=D!sp1R-SMaw zdVaM>Soh1nPM3mS7>kX>T_|-Nue9ecCtWgClnz-|zF9jtIRSorQ*RkHNPy){>uW8}ih&%ky25Do{*5vtYD_mZ znk_v__EIW+uJTOQ#i%WSUOYx^e83y2;AjTI_ti*zN$A_wZA06RTyb`6O%R=Yr9pkX zT5uSCafc|mVXF(o@J|xsyd&)O%uXC1=J59>fmP63>xG3%epqlCs*Y7&YDgiP%$PaI zyq=KDch*_p_Ij2)Zo#ETQ%B3TLL5L`N;8YdfF7rkm&Of%zm?1)N1KD3B~_4`4^GkZ z0~;hh?`Q!Kmm=GyUhUf7%hfca6D0YOKvWlG1`vgC29Vl?1g-f|%ke%Tjcaqed)KPl zHd$9qEZHtSvN$#PR68&za_BFG3}BF7TQvQyJd#tPa*?8WU;L!PNSU@zK-b3@*@r)T zP-Rkn2p7x3D&oqMxq^AIlIoA!anPrW0O<9~(_iCQjNjn2gjPfjS!R=;_uW7lCXV~y zQIBMGDNFq>kwI#o zF(P2$pDfx(lV&Eem#zfRrWYTx>cXtJAytQ4HAqk{jC~^IBMGu_Xx*`vh1z4=-#_q!fZi0LCf^bd+DR4@^3|; zEvf>gdP?nB(sKo@R+AjC)p^Gu2EXgbp-)s_(&45M4*WJU_$9yQVAQV&i)2Nk42LqV zxy^Z^mCBeuXs~P+B7EM0To{83ROh^z2{Ejh&_6pj7+@{BTQyL&8vrkUls!LSV;x~; zR9Yb`|EIeCH^dXi{5Zx;Z`P^M(_);cT53+ev}u`7;nVuviAcZ9(7?;Zkl;yUG%V(g zQd$hBMk4Uz z9?M*A3~3mcO_Z$6Y?$O^L;yhMw*UwNXxA+ihN_seVflqE` zjE4C22T(hPjPcVpWkS%OcHOr$b!Nl3HvBJJCGL9LtqY;eAb!Q2v%ECA_4H2(we3z+ zirZ}pSo*19CYPn!qm|K4 zwM_cV^*?S6JtKt}d|_$U6TpMWgl#lvK~;1+46Kb#gO+s3O@OXk@R|n@J#0z4sishD z&CK6GygFF9wV|8Z!KhbX}MYl+;y#V5zk&X0$kj;m2yS z5Za?ueHF2}+FyM8jsIk*sHDCWA7rR_}`9gZqb z4#_?=(LrgEZzK|PYKncqiaep2z|OX0V@}92;nQ=^Vnb*2N z;-Sc7im-yjNPuJV=w>@maj$T`qvefz+v45FBcb7>L$pY`8Ev#BttFT;5D9~y?baI4 zJ4VZ44n4ET!(iRC-y4lezM)u&!8n=)s5}i!f4|&dCHiH5MGQ(jv0=Fm|J_s%-q2^2 zGg9CIXEiOIS6cl4BX^>k4ry%TiOtPzEmaE>JNGhJ&vjM7O+fY3;w0m0oo)5viH}}3 zZDg9)RZbBNQ1<~3qSD4(i!Mt?tSrqBW3>mks;^U2Y7ZBg+Uz(cW}=tUrd>f)2qH12 zy=UyVm_JL?fbK2$b?uJ}uVfw8;);p^N+n>z2?W{nFfha5`CQ)I;&6OVk0aX8rlUD> zVfshos5+_Aud$G~;0P6MS%8W|rrPqc(>LFPG$7!Ns;Wit4>^!$;{APX@u!)Zezer{ zE&~`=SXl>=F6^>_dkL_sn26M60HLsGr)1Y!9MP>+UtZDQxjTVNdS}DpdA^n;Ro{A8 zswl_`+&uiymO3EabQ@~4*^5yHR3@G#xA6i+AHe> z7ix?IIAB=@0Au$k!lBSGh)hAIO@IHU*?=uD1z!ZEYAc3nvc&+0s)3@hvNC%rSI~Vw zzxB-_iy@BZp{L3Usv6@62K3+fhUbrL$Pb40-Xf#hAu3@WeH(rHWa^XRokCZ0yN!AN z?DtwKX;+D(6W)F7&Hp4i==`4)*L2 zmiZNfQOOtOp@n^K6dySTeFKP$FXkj;L%AYiF8N5FQTTq8qDilc_jF8pqTWwh|mrJVU1U{qHsxe7N9N?Y1D(>GXFHq z3VH0wTJxu?fZIx+R#6!EP)mQmo;G)4mN$^2*5yx32h0B+fsJIW3G|UpK}NlPg#yD$ zO%x!f41ykC=+MVYzFdO^av92zSY;Jq#`2GJ?+*t)E_OZ)ujswi z#RBQm7aaYXnC;a}pM$+7NPhdOZhIz29iki%=>bviWFcgQ!n5^T8I*VgyH1C1WLCLF zM?5V#yxaCnJvy@T z2pSYY0Y;crUn`955~nnsw5Tb(_p80`gKiZ$w54E{V_??OTFhq2JWBq{VEtYeulPh$ z8J{TXI2<)Usvegc_XguBN-28gMC%p{xRnTrR}Cwy@kh|LX`iSnqNRO+;S2N7S%4kh zK2gS4BNLzq9b|Wvze1mu&Pcp-S!<6-BuMrzFk;K-E)U^a?OeM%>mW>OVBrx| zL(~4|_+WKtJEhj*URm=~x0Oipw%QfNd6o>z(20qt-`R6-!|%53T4slT8&k^7zudVD z`agV^i_(G#=oLlitvWD}J!1S##{fyk%p}STK{Ro6H2G(E{X0%-jK%;EYHd=50eZ80 zt3%TEyqH}9uNb}U&Ltg8#6xJf7+TlCX_d~RD}pg#gn7AApvoN>ka1(26Uo}h0g0Q+UFcINY?yV2X|T0BQ?Tr!v!H2m5#gc(ddSp7uXZm zq`<=?Jtm>yg|_{3le|z5l2-99IO+BNZ`+8Y8(h?IUisAf>pLkr)C&Ko)pRF$K8&KW z3y>x|k3SrK)WH~2pNYnJg(*f^{QEsoSN#y_%5b1l_wDocv)8(L5 zih=apzx|MKzhT`E+@1u$d@Y%o(18v*XN3)L~^ zb8RIr@}IL&;wLFC>Q3Pc8eEOpLJ3)cJVusJe?Y)an(y-LdkM5~$>d!|;<*ZJPhoyQyxGymFrT>zZW)CFvn* zVDgl453}kb`4$g} zWLQKf4sNGXt6H^I#m!}(tVD4C{xMw*#j)CY#`+;3g4poK*V=0Jbad0eq1sRKOM98D z2~on3N7h{K6!bFKqo;T2$mh?%4I=@4wauG7pfRd_c#x!#zP+nbaZ?%4CL~!DF%MQq zsYD5?kW(=d*`%Reg4?Hxb2Qhr)5w;RRb{O~8c>K-mO|XYH3$R60!iJz*0TGzZ-$Ox+`tQANOsO>={u@@u?LZ?XNkMX|xbqYks6bCXcem z9@JNvhXS%+G0@~jNW3Yjic29X1ZId*P5n+b6D@Us7L z-u{&O$II6mgc`3dis{8y%4DKAXhL5V$Yvy(EHx&g%>xJ>_+4}u0Gp#HzzGL4SE853 zdnkc}(cYn@rF}^p#JBw{Q>xI~K4d9uJ|FWwN%9?AM&*Awg5@rM0^be)$D%suE~yQh z1y4ap4HtuSBUhH)#Gd-qWPz%uDr@pKW!uPeG8ar2>+00v`!Ey2d*h|OPk zFT!eZkC(&PNBA=EgY2*WKL_~`m}j^yt_Opo+3m)%+N7c2;6MfyAL_w}K`$bV9WIS; z1E~l`Gxy@U5dig6W}C}4!G;7>R3mtH0WfXmZi9*L4pN4TqvV7vM8K@bnRJ>x&d@H<7gFcwHuigqg>dR z;@%P!V)+q8aUu#pqy%#}Oin~)>THKxBDemrGym*n~R@_tEH9&9(!7Z2X z@9t)9=4SH8n?I9%pM7>evP<*xO<}BL63G`fF<)vt{IFj*3kE6JVXw#oY(S;T`QyJn z#diFrdl*`!(DKE%%1K%08?fK-zLkBsFQsqrS$xk-q>^J%!`WPF=l204rHmcDX(4nb z)S)H;N=w73!{q3eDTG9eQizzDC1p9FQmV0nw%MQ>Fu(U&y4~me$gWBm%e5bpck-dlgO#VlWU)xi( zWRUy2H)o?(ZMPiwbg&`<9mb-HEwnE}wyEs`@B$i?z@HanLB~S#PyX{9MIQ!GuuA%H zXiwYlOM5LrKTSPRz9NlEaQ1q}UhrvSGu|gtX(JNl{ilNU5P~w)i<`BLC`B&%xUopw z&&+K9kU9Y4-flc8c1vG>ymvuGB1tP-%KfVqUD)^D2P+F@SM);jU(zQbzI`?8k+({S(VPG_$XMV0^<*E8!FrwSVkxV>g^uwMuw% zDhnLkl!TBNss53a;w##c$!P;Y;|K0^+sQVHjhe?vZqKxRi&>bczv?0o3sWBr1!)(lm_5<7O`n=f!WwHzjn)p2jhV%z z`ZLW1cHYp#JYkpm99lBY@mk&1{h(<9QHN!(}k`1hawP_RkbH9B>D;Lbhne)5)JqG3CA4kb zULg22JN#9uYwNod0{Za<#o!>tq?@-2A?L{oKB>qGbbA_CrReLGpSGha-ybUJOzP-i z4Jy!5F~(8K2YMa*HrV2XN-)VV#BtbO$}1vJ4@EPS(U(sJ;jdI-B!@1O7rhGy?1ahm zc=XR~l>Q%`!eYhfKc%>iSWu~Q=pE^7?Ol=VJuymd%8(UMXFQ3L zEXmIE_=7v%#PQ8QDF~ManHb$SiD5a@-sL>BRQFXt8kLzGe+%bq zV&4M%~a8!A0Y_DE5CE#W5c4x#wvl#ZS;%WtA6qRF63 z^I*`CoKe6b*b~7gpvh#cQMvnCBroc@DOWgiFtSfyx)0qDX%r%XB1chIdC zN^qKOOpq~?2v4uVTF(w59CuvW-cXGncvZL4q?n(q2^`9BlVhI*8w+N1j5(fE&aD&` z;faX7f*Gbv98Uf2>WMQJ1|({R%s(ZvX-rKp07}@4U8<)2QRVq~3V9J*-9IPp#=K6G z$bMh)@vjP+mJE%=JCR||S~=9nzMl4J51#5x&XH&t^7((YT>&!r-QFaNYt8we8x4~b zIcpeItaB9d^(QrmI~X{FtyuwKBt|v;lE2pF?Jl8vgQ%mxHR2(qYLA+HMGb;ac{W`F+Ev=wZx=uBwP`GnaI$bXn&Q`~oeyy<8crK-TW)k1#NKhe(_N2h#ruVFRM1+;w4TnxAHG zMYLUQ+LSm)Ps(#s7Wa93M^EKDKIp-BEi4n~MY9(5dJLMTzPYP)wW)gcL> zaDfEG9b5tt>aShw*1ikUpuNS^$rPp;Kl6W6${Xe;|8Xyxkvgp+#&yGv(3AJmkMPvtEG4UkL-rhvEL-zA zL>)oKa)es@f4T&wx3#S4NpqX*h?IKP3A;fsBQbkhZcXSZT12$HmF zXla$3ZpI3+$7DnUcxvhqQxc#C-}SS|qqi)Eaba;ih*G0AJH4I~LIf$F$+ z-zqQf-yO?ywa9R`e4ebn@wvJo_z%jACmD!J$0;^S9DLQ@(XP*9O(>1s9aIGBO;lru z8wa_|@Ge?C38wCUY_I)Ap+>JsD>~fDn1UWbRUMV&+AUk*k(z>s;9}Gb-1n1-f0)VI zhsT%k^mIF8ShHmPEz5h8O`1}Sq8QoN`CWP>TUM}MUcaCRSNWr1V|DFKf?j0on?dcF zQoLW4{Tk*2BLdbX3q?%YOpZ78c-c=$igsH03-dXJRN6K*2zV#`s_E;oK$qYoag4#; zq`8y@3CV4H0%C;BbFo;c{nPSyL6YC!^jU|6BG)gBPW{xM9i|1r?p7;~; zw|O3mg%z6S@sKyRXYta+0RFX9R_&C6 znr9t$zr>ib7&7rH4p@x{@i66p(&3D@-K6Fh4D2dMr&DX~25 z7itU8w+?OO#{RPEhqj&~v@DH=-{H^4hcEAgO@52L-8S@`ew912Y%R_8NjdXt3hMhZ zL3M_(DL4NhCLuT!wb4PHfGVMfODdgie>(n^9tiOAaxGoz0{$rq_Y0}_2 zE8T9@`;ie_Msf|TTM|e?HF|1xRt*+IjiBGXvM+!Ly`&FD7U&9jJ7pl0ve8W~I!j`U z=8fb@3!wMrIBg7YNzZp6HL4O$Q;fnv~1 zWj2_LkSmR=s4O}++4XKAnSI(o3!R{`+_H-L*L&F}W3G;{KON1phWRBl`Yh=oebZbv zj`jatGU-WVhl_VzADgw6`6LN$#QV`fdZ@ z%I{R#dlq-=3gd;3_pCLBsRh1ZvDBL0R*<~%5N^9bE&%tHfA5%Ypg$CML+yE35_i`* z6!gEEt+SXO(b#h#@fKTg zoz6U&Z$@nO?B$L4%V3R2Q|S+snNxTDz^SU%r(_u(iuA91GT%-epDx2U-4~DIu28r3 z+Xc4gVBXs?Q{P%e3o|GoNPgJ$4q#)=Oq92S<7`3y3(avk5MLXMv&%$^xC zY5)Ni(_)`^ry3I!K;wS40%M~M&^X*%aeWx6U= z1p)u{Bu;bmOjovvTFGyj?w9JV^eJ5m!2{^K?5oOFfyv8$NQ9ysBRAh|$ecVk{oci4 z7b*$cpvRDiS}aY(zsQG@!K#WU)a#YG_Gaehnskx<10!%<&9__mZCCs<(Ts_juQRM$ zepjqp8fbC9y01aTtUk8Z3Ivy|u6!t2MtlsIX62Lax!orU+E7657i51~hL@0I0_EQ` zC8I^?gdTh}q39|k5u?{es3rPx z@-D`x96m&0zq*2PFRYrZ4m?QS_qywP%yx8)xHe|D0ZyHo7Cc(`Ikd4yE%7`6JS51a z3%sb7wCe9-a}cc`mzNN=1Tmn*b$lURL$T3OElQEf#~Vz+=JP;SvcO<_DPzZ@StajX zLee~+K$cif)%MLgD1z^-P@6NjjGf>MgP>EOAFp(6Mcq%%Q`eaj7SfmI2^<7nEy^j6tPm~1OyfP(cN-PDo|04EQ@F9!LR$tS$yY{XoR)N1(V6Y z*7e6Q8r#-SoV?UCaRGQ`cVRo4F<~{SOBFWlU!hBC+=Z+^4tMWIg3l}LMV2Zf- z^!Qm3iiiP`Xw3M=rR-EeiY(P~;5htZ-X1=bvI@I%gzO5#SYgD4k%KPU?Nbg+Pi?c2H;(+Sxj7-R7Vry)Hqhg1<*mdj={M^&U>$}%MCUr7*hpJ zHihLqb0SAqemxCDaY%GBO^a1kcKBpff7QT^$!6oAaK_PYP_49 zv}Z%VqupP!IOL+vn70<#DMaq}wCXn$5`}Kst%jSBweBCbi#NqN)Trjg{S(k4v-qa8 zX3Xg~CkM_SImcf||3INCprlGsP}5Ik@l1_f68OI8r}fXU0zD-kGQFB*aeX-Z(FR}} z-KpbcB(S z8eraK&*7!^?lt!+NumL)>E3a_^^~pQXH`^B?XrSkB2S4(^j_jwyzGoZ3zNFtuRsN1 z&u=E+TPrWJqWRA7nNZU_VbK`vZ0$ao!8You@5PMtsjjLDabL?~5l|A>O5lt;Lu#y) zCZh^G5rk2|NneS<3!w{^$x~rj`ZwI)fnkA@;$K5JUN0|J0NIYvrL0#a8LyXP`T4(5 zbiFc(E)LtNJb%xJ?GckO2XX+4vJ>QX^9~m$O3VA0eshR>)n0SP*4pHKP2F^9+|&J0 z!Nquw$k17DvFA3o_Y1S>)ICfk|F;=z(*M~IxHZk%@$ubjdL>YFjQ8^~yM>1N`ugd9 z)m?8+E!R23bV^sujgHm#$9z@bfKE?$f{Il3)m3ROJevO^x%?0KKBsMa47ev`GEpve z>EHfg%egf_0k{KDevD9qo)MI-b?1mrDa6bv80MwGcJ& z7xc|LkL+brADZD(+K^+)t@@z0&Iif@&{GrW<`Fo0Tl-9An)Sjf=H=vh+(*C;uF1%g$eA4u*nDLFD%unmZ6ND4^RGl%B$x!d!5wX8mbl^x^bYUQJPuqiYXpP_j<>r|;`i$hBCiv(e5gS_^Q1o0msUo!!p@Iu zrajOve5W5{LzlC1uTFH`?qkwtcV4*(SBc$x3tYZ_-Te^%xnx}J0tSB&YZ09u&u@4; zqb$CiB?8}L2UiOcqhtrIicfaH7x(f^AK#brA%$LTTyTo{!;Hk9N|^=3bXpRUvhkem z2~3}Mu&U3vdsc)A3kEHn+hb?TeCEx$D$k8iE{-4C|CT^-_J-T`?ZvllJPi+-8GDN@ zU$}jsq$Y#Qq7|d=P?aUwPaTv7D&iQeJFM)GSOJqmM$oTQ=AYv^t=HLAt=sGg4UI^n zH>I6-0q5IA`V_u4{@rhnM}gBhDGT4P3Kvf zbymb{*_#Z;dASBiAsj5H3y*)~!z#*^Z3jJ}id}`vSosy#T3RRSJ(70chUlzyRs5in z8NMWfy#P!l7ACVc?&x|5X<=#R5`C4ZSx(QFM3Pk*%Gj!Z6;@pR$Ti61( zkPEgi-!ZooQ50cu7CG$oAf&vqYvedDSM8RrpiUvyPwPs z_v7>aaH5uaE5MK0x&Ei{7hF%FSC3oD9yJA9-4ZAtZo~zDXOnh6ng<5J`4k0sCX7by z;fTGD&N;gnr)GKf82)GQDQm#5LZeTKwWv&5_A1`lxU0)H=$P0b^!WER7 z>@TLKr8QepR8jy-Qn1ZUpiu~>7xj9DT!H_*jwRA8AScEtVF`)!u6@Ka<=>#EAHBho zKvI0Kt@x{#{!nm)Y9NVVMj=CahTG>oGC~ACqmUB8y`6Rb5Nc1B;*_hViI&P2Ty`vg z;K|U_=&I%MYJ1(UIruGga$_Z1g(SD6dzjbc8Da-j>EG=!g-W24N9EJOcPE_yPw!D4sZe`JiV9Qmr7l+J7Lk%ab?9}!uk7lJvnk3ZS{oWQvJL&Qm$WhzTGI%xf zN^k4&YRU$iv#2kG`KB4j_=$Z8i`Zws_c zV5(D@hZ<*H8OL?)+3b<3V(^6zw)h&Ov+TP8&fkgk& zmdO<09jEyHk?x4my+-~fZw$l)ig$LuDWWdDM!%ElU8kwPlcF?dm_RM>-{R5FWh0fQ6lz%l}^fyUsdfE-l-N4 zsEE~)!fOrhXOPvnt{R-Oi!2QY3;IUs_ujKuyY`!}-DdgS9WUrM+Bj*ON%4=X3P!zA z(Ce;_dD?tQs&WpWXOmWsw}f60 zNlRF@a!u_Eo!(QQ?#}ap@Y6{`hD+aIo;Sk-+>5nmSu}yD$C9E4UNC>c7Jfj8gLj&H zwWjMXsl{#EmV@f_^qT)^5hwHU9^j(LXW`P{Urm01nCxIQvX%;pj|d#p;SWr!Q-fnt ziZo(bORsDHj*+8S2?lJC`karlsxJ+!SJgs<3)bYJT9mtO-0Md#2e`w(U-YoWo2jyi z+&9KI`(TEo-?lgdVIlVS5Bwc^7u$`t2quE3B{{Ac@*#o$*qlM-!tU)Tc+isn>i~Hhz{DGy%=2Kge`3*vP;eV~=4|L>*be}ki z)Np^PuamaU3BhiG9-nYe;&W- zeD0^8{`+e6(a^@LJo(rYz5=SwGCL%FiPD`=3vwA%x&7xhToS;(^!y$mZn86=+Zp0+ zU5*}((&(=4eGuV)WDnpzZL8ZyF85Fmyt3*BtY_Svrvo9#AiI7nK6fxg9CRHJ6j{OtMaeBb1?d(nf7d$d;{AnWR zT#Cre==WiI_^CJi2etTFI{R>4zx7)6N-m9IbSupKlK|Gg_U-m5{uG46l}5cJ5(;nQ6>ibvk{&C6WIBP z#On$d=AJyF42(0aHXLK#c$6iC(E2rFO{Wp00Lv+>-Br-6@_acn3KkT#X3B5t7B52z z0^)|k3bwB?w7_+Xy3B1iSlax9n;C(&1PE1c1H5|FiddPht%R+7S9>|*DJcTkIO+Ei zucVm~`dSCh9u`uU@RY9`yB9t`lLy17Dg`59FD-qVR?JEQgIfxHM)!_jheJ zn@3kNAnwhhiUoXz15o=@mr5hERpEM)Cj7Hghc!K@t0YBw@lEHR(e{cQ3v)DhjZgNZ%n?oEhej&LpiDip;Z z<*;5E6@m0+`S0%Qi1<$B=%t_R;Vgw2Bw^wAAjS-sg4N zjaOS6>=#B-jq3l%>rroU&$a*g+aWX$pW+T}akD#r=g`qYu3f7b#2TZzUt+0`GA>|y zsQiQz5J&24G%n~Y=7cGCd|^y>46@A~j@AN)?axjFh=3h8Uz)#8m5JKbY6PfZv%51v z)EuT9^u6)u*(Ry=`idrm&fu5NYc1EBgcJGIP&zxBFK4^_7gI6aQ~b5hfQ{=_pIom6 z!Cc^(EsLEB)hqaD&ix<_MS1sQS0`Jgx$+(q4lbiNXgvUDz&e-F%{blJH~Til?{%Hl zuQUl+A_;SUcHtele$l_;5;j5A=>+oO?K zvs>TywUxY?sIt!wSq`)^&Nyju@@=<2s_pTrTs$MgP!)?Rou!HBJOZa5zes*NBzRga zRV!C`g&~Aa@gUa?9gZLt0V~hg11FvN_}v~JH%Bd%R7~>rFg~mS#upjl>OoGAjo!xu zId?|@;E3F2Tl64EBnLJno|ES8-ui>WH`VlMlg?lY&T-!6@abBLLQR44L`d&6W2bd| zW3cqu6q3yz@I%~FJgyNWm;jp+2s&ic&<)Iv*B8ASSQP&gAr5~c^a6Bes=@nZ%5c~a z+kwk7a7;W!0Y(j}7%P)Sqle=HmiOrM=6r{@M}z2R-G>z6Rz11Et|uhrMs}-}8{yVl zwOH4mMrl6V6M7}13(X7#ztwEb$xag8vr)zGBj6pvw)yqaz;SEkYak&tW&K+AVx7}L zq_36Mc#&K;m5feS)_aB~ttx9H=KijOqR ziBz(HE$=8sBk@r~OOe{OA>B{&onzqiB6=!w!ZDS0wspZ;7(&N9sf3IEGWiLo}QcMk6LlfbX9GsEDNcV{-8A+ zETjnX6+LhDN1sjhIeXkMZ?h{>Y@@*T4bD|VT6CAF(pb&W%rf{yAdt@;)RX0h$@(Y&2%H3lh=vV!0_B6bR zJ{zd{x2rA4M$)DCTov4ph2{8MO!Ztq#Gf=V3mquY{;lMcbkPnfjQ=7svJd|9{HG}Y2)fW;NB^gpfQMZq$o46H z*h|~n>G6VZRP8GzkQB#pp((#U)C*uiKGdC_WYGJWXcE%MZ#0TZ;U@&igF8$Kdx}kJ zwQ8Q5K3;DPeFH?~VLbYtzI{3sX;=A>-u<^@^D1uJ^Kj{lSn3@qAD1){{iN#R*h2p} zNw*vuxf+nQz1|INKKjTG2^hOy>ew}?+MabYty zZOg`i!=6QsJ7L~8%WB2tt_^o!)MF#$IjV%GhjOFa@_C0f9V%t3pGf3sR3toPexjm7 zS4aLe(~mC38d}}@zYz>bg{yO_xKm}(t`?*5{*qsz3$q`)yw|rIduOTX_F9X?#2VUz zwNxqIafSj5|MTNm8u|FU+Gr1{W$qD7iJQ=qrQIDTl2d~inNi!>vc{TDf}U}?GTIX^ zrx9(Yuw&m5ZTZ2v_u-$OPuYOt{N^lqG{=qXs3%nF=OrGRlMH4*jV#oZn@vS8nqzt&! zk>%2WzeXGP8Kbq851wKLhe7^aMv>O;RGvshrk(FxVF$5U%A=3|OH9yCMg?BH3A~fw z2l-73>rLm$IK8mKpHER^FDmmmAEITwv2CJaA95&~l>^{f8mrPhB~6lyVT4GDcbc*R z+VfcZRlnF50&1%fx3PbWAqi-o*V%_0mh-0L?1|9yk#*iNe)(2*KCZo8W$%geTDOn4 zuOa*HPOUWVyR%dJLA z*rT54q*mx5VC%!~0(pz9eTExp%gkfHb>HdZ)<-ZeCt&Z*gt3np(fYlL z*iFqjbiI`RB41T-s$ETkQkv}QMrl$YztO}^1mwHn^R%-m-p}6SLql)k!|3R_8Sy7o z($2e}h9WN8hPTpmao&38eBN`q__o2RtCcK2E`o5GKd{ZBGrKG2BGVHgB61}+jE;#qsz_6)q+0k!@tCkM%`R>Q&ncanu#Y zQ2j;Rq*nR~qRvP+H6(AzpH2Is>_%fLtEgGaE4=~sfKR3SsbS*yZ1QTI*Gq7h0Lct2+xrIogg)_Y_oV&syZn5d!o#odn?Ad|^&sxz~rk0h| zVqg=|ztOZuC|?A8(uYm?iAJm(#S~)pTy|VYN}9>KGM^Gv{)hXQ8r^o8ELeN@z8K#( zw?`UcD`HOK(0M3JL54Q0_cq%K^m&-@%-E2lFJALkA$=Plgcf0h0p1JK}}y6lIDLRMKoQ%U+#aL z?uepT_(y6b+`ODFvIyDIEb6O zqpIP^Ep5-ll9XdU^E;IURUuvI2da_+6l`a=83c*$o_rhHWQvWx9ZFlGE@@L3rY`m4 zp95L&VKb*0m?*24SRa=w(`<#uo!KaKRJiAd)NZJ#u{NF9m3qVYqx+a<)Pn6 z!)HFZ8DHzZov!?GusJML9Wncsuzv@`DYsdtydQO@+$(Ud-rXyI=_|Is-FlBca#~<= zW8GXFhlv~Q5O?Evr20VBxMFIq4`xsmbbmXzA_rC{3-TyJ7GE96_Eh-pF5ZipkTts7 z4pq+P3I86;P!u5E%Bcs*2V^fC`g}1wxa>4Xu5-F%tjv*bD|H=XikhBSam)(lwA(1c z9{NSxK+z2Wf9AWGQqA<;U(Wr<5ryt~tz3h__12Hr@4or^8aAu)`1})rY=PTw0-9DS zOO-_(s5P>e|D9-l6;IOSYkbP{C+!FI_&5rm(<%}RH(ziIWmiU?Ndd1N$-`Ewg{~~D zgjGabZ($DO9>mV8rdwyj&EHf+vx@C>a8^OTPv;kx{t8$nxOp9?n5>#Fa+qVD7}W=t zDK9=RJ_GcZZi?z*f~U!K7te5kxFc2|WPAP;c@0SX+g!lH#vbHhr_|Ha5puiPU5l$T zd++Dtpu*T3-Vl|-U{1r{nZ@vt`^{9X$!0{6+s&uge(T2}1QO;&A~k(y_Uj-7GU!hl^bwJU!n zNHBy=foU~|!ItjX%YnW)U4e_Y7y3t@Pg(cXGw5VK`}Fq(1U`GjtggnvgmCW7dcYQ9OI~j}*%szw$6n1vzhKIMY7`38tm^*Fw+EsJ9@Y{uu{r9*}J0okbyD0zcE$HuV;Qjjj5?qB`_vOhEV6s-O2COZrCmPr;7}=e+xu2eEEv zEpBs`BTCREWcXOn7U^U~r?1o8%JoK}_--29v%hj6n9BuV84L`rS!LO06${pgAC9+% z_pTq)x%~wYat5XjmxKJHs?J`JK@*Pv)yfJBJL6P2llFoL(kV09>NKme#*4&k3V?dT z<9BDqtYJBEUjf}s9}J-P9z(6ry+ilIy;W_`F=60Uli)>*Qe~!xGVgQ%zW8H;%)G1V zmarqgYdbD6o!srXoy-RE3!0W!=wTsK?F1jr)KY*Ji=|}<2XxI z995Pgt*g%G55qDFb>5+JChBBd?Py(&!i=xPJy1wE4Icwt9C^=u-;sw10*WYwewjUQ zo`I()M2-|wI$?8NBCA5#+^PkV6Svcz!yG-{f@&Kcq`cPTQ+Id?HKePsqR z&uJ1)m0m_fkL5V6_fhN=9k_%y_b73?Wa)$wq-0m671hA8qwuZ#$ z)h9-|MzhO`PT&mJ+4J+CwN9`Hp>m|uuvc?Qx$9*SRQjN+hq|k_7$Z&YGD^t=jLN^+ z2LQHa&#L9B1-xl;GWyF6G#v8f%}^3^WtqRbhbB8%4B9gvFu2+J9_zn6$cSrJ_#d+=pHGEq|A4l41&bNSn*ZVDjZSmF{f60Y zsDCr}>Z(BXV(b)fVdqcRNVlL#2vu}0Kf?Ei z^C+@d;Asu)vteg#owZ{p9%@QB`O)vPSp7uVZdWAID)(mdu!u83z?@ZJ8x zrR4jnX_zEgrHup2aX%!Rb}_h(9WIjWK+JV-_(oShDui-mLR=J|HFC6viWv}#p$MlV zqwM2^_?q|3R|%W07-h02FOV`#L z>$~<)FPNFA!f!K3RAd~|N!zlgT)fT_u8)qjA5bf$%EO{I_$xha6?Z3E+y1eJckeT9 zGBM8Tje<3AR6J&Fogg^Ucgj#;)B$HU{i2gwSCY%$p=`&#z*{2}Y^8H~cZAQ>;HUkY z3QbDwAQ`u%sQXywx_;GRr+8Ckihe{!27P*^fJ?4OY9Jh?P=vrF4xp@&5g!E?1|B=l7`RRpwZI; z)*9g4+51h*C9yTf_6V`EAe`C1;>zjF?}(xnIP)g@S5|%I=rFme%CP;K=ce#nKD2;L z*S_%2VCXE{cW82f>$zy1`&|!(Vt|jqZOm z9faLoQl72B;&o({1tE%ouBw4E1GfSB=RN1Jb&zSG$mZiLG>=XY3<6sNh1hTXSIPq? z3&iFtK9N1diC>nl!CZ46XN3g;>L${zie%RXPU-Qk*;2h>EhLvI};<>~% z@Evrtsq`-!O?H(GVd+b1!lf$}KX$5=E52rpt%zb45ZdFmPy>MNp4W0-uW6^wXlM~@ zDVH1A+_!%>w&Nj`CJ1-J`w_X(*9c(yX8108cnVSthiQm$aDHDgo#`EfL!?|D&DXA1 zA=5v!H2VhxQy3a~3|3=BUqn>>pZW4TpA`t7Ze{!r8gP<2pE;e{ZW!H@Rv8QJW<(Uz zWVL3VmvRtG#vB9gpZ;CLq+0$A`WCf_Q5(c>T!`F?tx9Vmd*!-v6S*HkK>LwyVOE&s z0ozs5=WP)^8*sn;(H}Zo`Cu`3w1|cHy({*>xFyls%cT(h3nP+9Z4RI5VF9mg0kZjs zmj*vWxkbKJZT2V3P4cc#RLa2c@-zRSuSC@|oD(9KNZoD=Q4^KFLvE`XY6|-28Ww%F z9^kwhik2S)uByk|E}pSc|4n)yO8F^5Oa_2PU;mQdichlk_CE+spk^Wo!E3uzx0Eju~e$3zoUsP71_%YbfG; zs~z%5le++;jB;uqi|8V5T&eB3Tdwsbj21u6>a3z*JGv3_;oQjx!bh?SKDVpE8f+Bn zOEazBseLTiZ#EUR6662Ot89nXgL4b=6L3w~ycVS-nmf0B8eCnzoPw=AC-~Vc{?Pin zTg!hqllQEkA0wIUjvpX^zl+ zpFN9qN6Jjh3+)Q9QFxD<{pU9gUokcIhwmC2D&@-u%~D?~t%UM|^>71?dF#^XgfWtg zi}Zrd05)G7?4Jg4KQ0qjNenYB&2*>WfePhYaAO2gha42YW0Tu{I3yX>6Nr~t;NGE; zVA(Naq*1jJrBQ9q@2l6Qcb(B>djC0Y^&gJC(p%TM{5s@McujAngFpP#9ui<>RU#%T zEl8Ng0iMq47c?DbaeLIrTN?GjuH&0_azOA*WD^NYMG2;MZ;S)%Xo*c8<5-;;Wl9Dzr!C?Kem0AC6>F@gG9xBJe|# zlmLAe)YDv41?Fis%k%bU`(ykj=3`BV7BhHtxOj1c-uHt<0@tyPBNo_C&&eAX_&`5q z+4WYE9WUXW7awtc(ck$}JumLv{@OkX4GugcV5l6W#or`3oJVN!s>v>}wv+h!kn(b# zW+$Lxz^(VU&#dfy?5(WVhufFi1bv}nC4ad2?2>z0+h3UnXA7EVem4QV2PTpj=vg+v zCKE8C&TqNoy0eoK-i{vZ(#t89vVF3@zXscT{9T)M1q!_-eEvZD*wNLsZpEb^3eiEr z?R`5Dr#H(4(9mp)IRooqII&f{DH1E3ooQfkSpD0edAAzS8IPHbk2`D;@9>&r7+|v6 zPOOnVZXabCz0@6G_!8!6(w_H!wE$;clJ*yGJMbn*ft;%T_h|T^JCg0bSmJ4?$}+OL zMJ^EzYDDPG4b|hzXpn~WxoX!=SqwDSr@GQIy@|K{#u#SKjCLevp!ntwZH<48{AzG7 zafSK-HKE3^vKE68XrR{)%OD7nRf1C!Cy?d&tjcfXS+bVf&$;I-2-vOV(_-ezx<*7k z8KEnQ$BeqcjGH9)zwf-CsHo%l!{C|dgx|{5B{6d4pU2$4g}Qpn>-_~u%3KTewx{6( zyQUb!IxSjXw+4e^GzX(c1X@zC%NFiJ0G2o?Y!eO=d7cxi@q-emAw3kH-{Q z;ji0S?#PF%=h;eVaCu=517C!V+$=!fX?Jfy@*lmZrX*S(ToG|>ncv5LWqOQ3-9@fL4mmcEC%rc(Ze<70Lz=xqv`80)F2OjAV z&|O%n&cWsNq~26iuQi+dHSMF(ARJ+#h9b<;Zh5zmu*MwC^iPJs!G2RA$}6(9CY4Zz z;czEgJF8QGIOZdf`9dSo-)Z_CJ&Vq>0foE|^Z|Kw<9hgi>RDU@;>?*f_4(=F`{ui{ zJ_O!v4je>iRF=t>#z2TaaT3h6XiIQ`ckE%}X;679WtEBeR3&?^aa)X|O#X^@;vN!s zp~&_$(UnNU7Hn!vzvYyYT7K=`P z{xr3*cM0wg+%>qnOW>~T^Ui&{>Q=4d z2lXMVd-mut=NLU()JP`q<_27;zT@KL_&3y4K{QM_F%AktRUedYNt8)BR5Ek*XsNN% z7ia1iT*rFygSbgrBUHTcn;&9P4eaPtw&So-USFt0VTumZCDoULOjdux2MDxTXmGZU za-h@oYc5@B2qcg4k|pBAs;*L;TJy{@Rv*sgI9V!m0CfQ^K6@LKh4LV{B4JlanLHmA z0dlMvX?<_Dujy3Vr_IW2vXy&PRhJ~G)li0eaaa_GKXh)0sa$?jeqkU5!2x5fVd%8o zC-&MY?3DR_F(7oZ&IW)+JxRvKjQxr zyWUy^v7&sFPt6=d$ob4#Lz@dO2sG4@%S;bL6rUL>>oKBaSeel6*3<@*^4Z#{aR%yE zv4;zWZu#nuZIDkJN4-|)8y{9w3Adjel{hAwI%*KxpuYS7S7L37{$*5~ywzLW!;TZV z3|iHpVfvx`rNKGhWaw7zujCSJd`w~uCyuL4k}}A~HY=&CPo0)Zew8Jx^01(;mO+em zcb`EBxxD32==R58eC%6$cN_&n1tu?8BFKl926ew zGLi_ViKi!2CWo!bN_~;&2DvZ_bo^C^uOdN0SIvwzqhiR@doQKNIUcKTsNk#w8Q%Xt zh;-aTsE~%GmQll&_~3^1z}-4P-qA`PO}xS{wPt2*gqvuD=Xj39H`TumI-J9*G8ZlD zZzwRfsfmU*GP?}U-wTfT-aOnKf1z#A2Ghj;KHrNvgTqiI0+DXP&8y51XF;NmpGUoq z!UU(A2q&G-fevbK&vBG8;}0>`63=pxWsn+nbgHWY;wt_@%Cs{JZ{{%?9yQrrCIO9v zy{s~pGCl=d z;Z$D4blVBdN(QIF7cH}X0b+|JkoZia4}LaJk^<;ZIet-BNf+8G7DycB@-Rz6gHDC5+8@7R}tobWaA;@nAyoHDLeOM1z+} zMVRq$_@Gg*vzQ8x==V8nN?8xQDuy0#bgrNL$(c2^(EBfAUve^!>hk7=%QzxBWpkmV zh_Qe^t`=--$3U|!1ThRnRbdsKuPj@|O$E{QjMbyn@zr3c!+mua)#geVv0v&K_!JUv zqJjP8y0%K_&s_0jVT})8)wNIbVP-~xfIt_^#Lnv~Y&;GVl`Q2~MgmJRNy<69NgU4; zLswmg^`xGFGG|5!IqY$5lo{zwmh{YO^e(K$*a`ULZFcbCW=%dn{KsGOvM@zKYgE7( zeGOsp-AUoQihGF*Bt}5@!6a{bm~jfAmDnU8iP+0wd6gKNx{`RY_(uTh?g^| zTpLmdH>a1GG}I}baizPB2OX+K5=+9U5g${aZ3pOh`Pb)-&QTEQr%c0n8x<8+D(Q!B zA5oly_(-D{;e*j=%yLl2)9hO4dDK?AiDi`%N&}SV-$6E!mFB2SF(}CEz{--CuaoQ` zEQ?A-0~0kbeEzyN8$gYN1hzC+ILoMQJzI@~HDr2xK&Gw2NPv^dl04{raHlnqtr>Fo zObM-?si}!U9ZQ+A0Qqip(?s6!X}xw0}GF^)`}lLOd5W)MOl^UCAV{KiTu(6QxONC$ZhSuZYLGE0^x z^wg7FMQipLRy(ZZ7H&5@m|@h&qSAcg2@z$y#@xrW4wbY5PJPod2-REg$6@jxPgGXh zL6EUj{)}DlC?h#88nZwtP1`T?lD%Gf@eA<)>w4-1dFwDSEvcl!b5uw41ydG)6)O$j zS<{~GEMuMrSXk&3R~H^y=f?m8^ArGk92(0$v~h&zUVyZt{k7Wu-Amw{hVwAO#&YT^ zpy1?rI)79%rR>RK1r9^J_aI*rBj>UJ0+kJ&KUrBEpMnXbEsf)DD$GAxJ&~a}`f@

-qtt^WPjW&X8Eha>Z|SzQ zmYZ($;kB>!2wHJIRMOXHnV0(lX&8<(88iB>tm(vpP6FS3iOboND9i1k???>76t8e| zE4G!wEXcIR(P?HtA+69dP>ZS)p?qC0mMu;L9*ahyv@_wh--9if5ea%Hk`cshymcPz z8CEu{KX_0S&R^vf5WFT?;5G2Uqha}3Kv}XXKeTbH*s35<0 zQKmmc5g$LB=*GG%!Y1aSXlbbQQEhPsh5k_EmM}SLcd>zwjy=WGFvjjJcV?+J^pTu~ z+A)z2ZWMam3ziNvW!`6Wq#raylkt2k${yHJ>o_M9MiEB}u^R;n)uc9v&i^miBZ0rr zOM(|B_y~HU2BY9`0^<}&e1|psLs3sLT-sp+d+75@Q!)7--IH)b`F3tqBz4oK;rVtZ{K2s@ zQ9TARxJdx4w!7a9S0WAc(F(%~ZgWc{wbUjJjnQ7t@y{frmLwP2=(VoMh*@eeR6O(~ zknMU;k;SK(a74@6DrO|L`1?2A4M`Q5SuToCd{eOTdA#*x=R40Cdgqoy8mSI5Kjth- zYX<5ko<5U(b0x;6g{LX4o&DDJj=eqx)mbZFek?P-Uwr@E(uJx)J52OS`#F@ zXqA{7A}C0(H~D*fjxtE1Zn36%ECUQ*Ah*96)j&!Kqd@+9ux&_ZsP{22I{z+~=h;O7 zOe}85BTM{`*q`to7SBC>9;)3t!Fr4rqBUH=SS3HA)e{*2i<2Omvg?WJZSaZlS;Zq( z@tckMLS@|{7$$F3#(F*PR=BYS=5F@Y5WnuAQNfme!cB@E4eMax4||B7n!J*(##6Q z=jZl#&pW%hzA$N=0O#I$T#nAx#D8wci9OBGPO$w) zl+0(3xDJVU)6-osG`Y7G3xdv6tJpE&ilX|JEklS}Ns^&bpkmZbele#kw9+V*C(6wB z^^si`-a%lM^hsEg8H6@bC1Z$p&_)z+z?Y=n4y@5sO3wVbbOGp6uwpPhdS7s_Sg@op zN+=pBpt=29ME@Qb@(}R-C)rTPbcEfY?!Qv@byZ$pNZ@f|S*YyjP{`a@ik{%qSwgZG zqrOej5^L9*9AZz1R3a{LwGc+T_wgS6$n5Am5h4PKgH1*LNe0z+WOhf2u~A7AvSy5= zz>W?P?&yc`E9^o+Srdr21R?TnuEa^%4TdWwc^#uK++S^N)ZMPq&vdcG2z5C|KCdo660U}t89xG z&?Dzu+6UBI+gKZjja1v(*jD8IfIW?ns2gYe{cCYiU7OiFbTY7T%8s2B{zOh2ue{EV zKjVyIDvzp5gINJm^5L2QuqfyV*W=9|v%Q}ZhG7y77JqiS_VoLiMVc>LHK?J=7;iw8_xN+s>=DtO^DTpHQ z-Y-u1#%cXl0}@0bv<^}k=N3GXiEWRVjh=RTpRL``L?p%^U*FLn4J}ymHR1R7F=W#e znY!U2ggiYx?LPh|5eZ~ILlIW~A7lW&avdGfDyBf?n`m(Y_9K0G%OZ2sdKTR9uwIT& z&7j*rBd)5ds?cT&xg|A-Z)*e|ZH)_E;eii!w_4aSe7NT(@7qsLGb6{9b_MjPDd{iAnd!Q+)P)m3K4xUZoUYN((4cQa33H)Hl7Aup^4U@5!W?cR?guE)bUX#gF%p2kc8UL8?*J5T|14og zfzm0M(Lb0_Ng3T^1Wz$#tyCcQs4~Smj&TFSttfveN3^xwh>U-Y#eQ%;2rS|>6HQ$r zrEc;f7!$GaLKd+QHnVMHk-mp0?lL8f|F$oUN&%C={yRbq&%N%O6=E@MA zXG2D#jR_$4Qh*ApZWr<9fEA#FzveR#A}ri6$V5R&_?j7sS3?Kn@RWlym=;^GQEeH~*fZ10&s-V> zc$lC@D>CTvz+vsoBXtc+(q_05DQ(SwBg+9XFk36JgU&d!4t;8EZ8$xYC9AB&-uk?X zFCjHiU_8y`!8q)n)!C6Z1<--TMC~FY9d(-XAI2LW1Do)Fkuq=^F^ERQjFrO6d!v~> zu8)hnx0T$>Id;Xr<&b+(w$Ni2J8cDIcGFCFAZ@%>)%)_j_$9z9KbpTq3K;VJA~~(% zd9zZQ$ZhNBYhf$;Y%C(zZxVNJf$j17#ERu_?cP<+effvhNwZHWWiXanaBC+!2?&r9 z(KO095n0m5-}J=M)k?vISuHe#H>7s^*ls8AbwI{4ybSs*0G>SscYh8E%>0(doR5XmtP{w}7jn)Pg zU{>OZxol9gCblG|8&Jy@{}9i-MH=JECe9-=sY#x=!LnsuZn zv(kp!f(V&hC=FJGV?P~+sR}%Q;h_=*r-oNehJgsJ%(LG+O8fu9+xg=i&AWTtwtS><$7n*HO zA>j!SJ$RR%vP28=KDL(Cz6GXblc<$Bs9zY^@)(q@cU9)|B1-#EO;c{0#61PN>T4O$ zHcq(ItA`4)=6|~QI3bN@apAO5b;KwMqDWwP3ZuzBIgqCb*xGPAXJlw@2~2y*qXcgU zx8anA+}uP2UAP-PYA*IhhG79*&H@yN{vTHN55bXmLW5velDk^T3uY+6_@?@9#W6Z} z8oUb2#SBdNwk%@wUhaYbXhsLiT-p@qENVqkk2`tZyeWd7o2_ zWoplb(C7CCBr&+CnxKh7ibTzg6+=|xBQ@DCMmsgJ+sPR)TFDtg>~&i8&Ffg!V?qF; zkuopL+o_1(DM|lQ zZG41qV(xDqjnxLdrs`V7EeQdm7nzLaCM!Mplwf*l0b{)yYk%&p9FZ?(tWIWw<=v0PNaNG-Oge`07 z>2G2r!KNVASYyNmQKxMcC=*v~B0jT1D#vLn)6lCDo6A1cNzhy7gR+tt++AN}AtPoomB*xZz{GrN)c! zqjdafGgaQK(c2RqK$X7m;?7R|pL2r(*aD;`8s!x&GwZT7`6iNV?Ia$JhOxKzz;$uO zP9MA1EU>T_pK?@8&BVmVllO%z~UrIrP-+Ov+drD#Wj;fi~z^eg$JKOw2agSaA!` zHXU)eooj6!LoI#ya&eByB4P?GNRps(iHoRTQ_~crJkehH(^kDCO#@!W62P6h2C4~y z{?NmgU9mA3a~NP2&tS!XjLZ`zSZ8)VjBU#jC~5{e)G~hyER^pazLB`t%_~W5`0gA? zuc-PpojZ)_pwo{#G1RGI$rZNGKW<}^E_Sp3ufltxTBgo*xH7vXA-n=_EONI!2L`DC zY+o&?q~&Ty^zr7;+_=`SBE(o&jC(|UpD}Llr@QC*GqFGwt3jsLvk!imjs}1Gzwep! zBgs}{(@%X)Zcn%=OvWk)CgP8}L%`Ap7`>si2I?2_#X$iw>1Kf1c>A}gQ-ea!NC}CK z&fWe6zKn54f5dMBe3qh#d3)98 zT;nQlnt}U^v99R9YNc59IobX)2!JL(X5zp?JD=>#ZZ8aE+cVu-zJ3)2PeY>E@@x1=({c(C(A}_513|Ri_Rx&*Up25C7 zXZM)OxWL$Bm#e^Bgj($BQK!h%dh##4!HKrJNL|%?-1qf{G$y0}pnep#csl?YCnrd& ztGcVJU=b_71Wf!s#|Ezd`B-GEX+e-ywRR7uZu9djnlYW^N=+bz0a7!lE##~p{+F+s zDe ze5Y@<(CyvE@=ZY|;!JcbBIM<)HrsEm!`C;AF`Ls#aeVv@NUP(S7IR8gW(+%LGK~8Z zE2f$jUurhTJ~o@9*fX1FGN3BqiW&JrLXuBC4~|op=aFVsxwD+E_r2eq>Oe0Y6EfYq zVckA4>~d0UC>80wEarX*3c1`t%21mR6zf+NubS_6WOC;Rg$x|!0f~%n^r<;q0cqe@ z%g-mj-VP4JJRMbd(H9i6&+DxdPCa*!yLT_lHRh5&Rg^}I4*SH|`&_`%zIY(&EyK7Z zEVSqmgvV1%bo_x6aMgxb*u>QT{-@>bCf?oBZ~yV_uGjPJ>5XTdvwGY4<5!Z&(N9_{ z(_?pm{;AF2h`FvQ*AJc`>;7g!b3fJ;5wz&F5?G!z#6 zNL{~uyQFO^nT)YLOGZ9NEV(6liSSpv!De^H-3f2&%Z2FkO3|D{v*7ThuOl*3l~?RR zAq4<>?)@9-t$z*o>auNIb2G8wKGM7_WtOB!r==DYdNjo+#%dNaIueRdeNo-N6>Sg;LeV zKVUT0kiI?{a*N5(4mH+#vS_SvmY2e4NA)4Y%pc*V?3Y|p>{Ru~Dh*aAw));Bz5*u` zU`u0zM+#`;_eyY`f%kh8anb=uIfo}|84M@i+Qqk_w5pnmrn|JbFzdM6@-kpF3a_e` ziO<#a|A`pjuL|C}xSFeu(2ZUnt)42CJnFLE+d6LJui~QRFXRA%l%poV=i^ihqOS8QF z@Z8&7rr=?K2JSI+d0{>E?+4D@Kb#-;Uvunkgi2PfCX729hr;7ijD=~%9JLq_sbo2+ zT(V}CVh zJP~I4niyM438jCEtFPa*5*{Cqt+YQ||1S3K{X6;X=(@^7Z_z+mOH54-W7j&ey}K}V zV*oV0`TVp_q^<{j%vC`)sXEdWwv~&cs5%Dh&Jkjn-~a-j47GB;Iy^L&}C#B z|B>S}_Wk{RHns^64dc}h2sw*S8N~H!yRXlVXyO&2bFP2thdI4C5!R*i6f@T%J5SE}57T{Y91JVZUU&C(rgcSn&1~G^v*2mKC?cL@K zzgJ)F4~x(hU1_&o&d=9<#}!&#+M(EP{@8vL7F}goy-Bpa6ze?w#zN{Px0K(#?QT!j zsrdj81A#3z*tl9B&ei_XEPje(a_lGY`}30f?oagQC-_ID!S-h~r~c~f{+$i9#@h|@ z+x0;OR|V$JA6O}Nq@oEe?>EeuU_&|Mk9NTCv;V-z&0Vydn(j2D0;?lq+=cnL{aXF{ zm+1Na(Co1`Hu!IJhY!YS1K8k%pd)5c$XJltZ^Ouvv|-*YGf^FTs(UcY#dSYCCW>Cm zC%&bPCp=%T{wDA5xN*M8Fe!GIYXon{4M1!M&}SGi8Z_FQsOW5qHRI?yZVWyp4J>@j32t)w#_wm`%=08I{)PAeN>*=x*LP{yve-_ z7Ta>s7O6%Ma{GA4yFFLeG~%lJgu`l;_eG;q4ggWyN)~2c2(MVDmbiOO< z?yc!1(HEYct9yxVXQwd?>|Vryn%pFoxCHY2Ts$~FbTdajM|Vvw{kRK!iPBC)qxP}Q ziV&(rXlvE@YOkL)@m)ZlITjpW2Q^ZYE5IL{hs?DW(KOvdV@){#rOb)aU_mU{g??dl z6sKB(hU>oex{=+MaQZJLY~KiJyyTvp+2>fa(Ep_7C=V09SmdwhIJ@UY_q1e_3;2Gx zx3u)CHl&eL%y2m^@9enl&ZSyO3Yo(eU5-CbRraTilf>uvO$Ahura z#JtbGraBK|bq_|{XRsHoecjl!E{{0nF5GNAUTtpK^<=2jv#*wn!4?zx{><)u;jD2p$D3>JOqA`>*R|_)Ok7AUuBEh;R}(%Y6L>|I+PLK7+9vPO z;o-Hw*f215+P0Gq|2#5iuoudHuz!Ha>zpT<-E1Q9L-U&&QSY%c{mVe8dvV!~MCty5_wMJeV=E9uaXqu{%{5d$7(>IV?uM?(kTc__6i?ml zd@v0F-O3S@%6cn5gLt+LkjFqhWALztp)rjcqwMZtkE2ce$6Y z$ptTj!`qF}|(AtmK` zR*Kl#)&bk!Z^F5IkCJAr$jTc==`=FpHV`^>aT$U;?MDEiR0s8rS_dXmy%&m7R2ITcuEJD5d z&;^C9q1Y69z?YMPY`~ zZ(Z`JHMq614AN6enq5QnF7ggh%a}O@*Q{MXESqjPQ4WABMv05n0!;S_GMcNd^lq2o zSoK9e%{SAp|2fzea~8jSXht-_&e;V-jXa9Ca|??!&TmZ=@Ua?jA&S7ioM-;fAv#;m z7p!8IzF_w;9^3XXHLlfCko@of$;$!UcubYrpr^s=>_!15C-Xjp#~{G-E5Q8Ss?v0E z;5jPt;xffGw#6D^&D&op%ePl6tNsZxrgCe%R<mKJj;$eulEXk9k|vCPczeezx>jFBmrM2 z9IgIxU>B#nLT7n{W+hRkzGUvbQ`Ct|3;=EqlD0g<3zQarj(5{b{|a%zh{ikN^wRe=y)Y zP|LTUq0zq{M0cjFjsh+2(y;94vJBA=W!pj{r~YCg`MiAB-*|=;d|INao9P9bXimNz zDXG#<4DNm!D$5?+TZTB3tl8%@_@Gr-OpaS!j{ap9JCNqM3B|_ePDZDbdecCcbC^1) zG&>ro5KAQ_%Yxe&pj?sxhH%u+{qZo4!7^;doP*pJ5&k7lmLHU%1^gD`=KGYZaI zGb5TMA5WU@C=hX`m_*E<%-vM$Hty7Igba=kQY zINY>FG zv#Fu3HT=xc6_ZuAd&k7@h0kaYF@83I*X)FK;ZR+uPpS^&C$GgQg^NGch(%s_u=1=7 zL84nrl+tR4r*c+^F;&-1>08b8q#OhIak;;ZzKmU4G?7gW6q^dH-`xEM`>$Flbi>w}DPRL@9}$EQrNZa4M>%Vr zIdZw~6)DyC6l_TMchi3zc@$$Irt8X3>>_zbgeLQVrcwbHP?4(d;%Lb{*s%_xM zr0K_;n@<~;C=St&ytAbBFpp4|UhoiZF^HBVl4l=np+I8pviNp6L!Y#J+{V zykmyP4}Rr(9kaikITkZ}6QjJ|w*SjZlqHyA#Oz~*rNVEBBr!of44vgj^heO|4Qf(f zQK1cyCsSzSHE=)1=tkk5A<$gt;$66!6NEEx84PE`AspKSjfbO#D5?7SQJi zSee&8C2sAhIPX*btATF+5zdNxja5uKJ4?D-{;OWbpSv)$%Q$m1!u9zx6Ii55y`PQ! zQfC_lR6uxX)8t@YdzXDyA}$|OC2BfT)vmSRQ?9$ziIC7;6J_?j6F?Hchm}AC@Wair z^wyf337x#kfRq52TaksPT;MD(eVg^>vD@6wn>3*6R zu(N!Wg2t=$)e?)Q1O{IPJs8VO>RYs+3_(9tbUbg1DAqqZcr`bnA0}e2`#<o2E}vf>2@7bN{h2;4{c1~zocN)-aWhGl*P8qq_Z1j<-y-1DF9 z--Rwz2o)zhXt7F61WUs$1-vysQ?O7`z2MZL^bAy4RXLt@3R&pD83Y+?`8z7Fra)=C zYfB8&QAezr5%?dzkT(z#&~Q@Gf(OvCqHy#NXUIF&@;Df@cTKjjsI^vt>(H>_ZiNjS zY)lApRv7BQ0LJ}oF%R7YN%DuI6ogZ(@w!OBq?2Qw<7<_hvh@*P703*(C}y12aHMNB zSD|%Xkt+4;UZTr!#UP!}l2t zETS8~G!|1Co$J_JUAztPF_t=cY<*YoZxaO`1VJDP`s9d?@MU4)^Xe+!>Z-UQq!j#v zA_4`W`;TFXoPUhjGrxY_VieY znEmuIA!8>^w3E88?kjEUWl`094DQ$E4i%JORB39Ee464^qvhO>L8G=~Cp6&5LSGM1 z!gYv4Gn~&ezkL>+B3%EyYRtX6)_oY&yHQSzj}9vF4K(;TMibpY_&2Fs)F`M<9;mT+ zkH-2*2nx>(H}O^HLgV3>|1``qBAO|~=VE%QDYlk@)n-(vI^d#O_~^)hKc&odl_98A zhI{3nb~~yKKqEm~)qx@9Z(5kB61&es#}-=ZUDSn0yILftFKr(y#3yRjz`5le>k$+F zuT0`kciwW2^Lgy)MO;=Y8Y{N)pDIEjFt7{P`4Q7UZc*%mG=o_7P6KBDlGlm#&v-B4YcwE4B>h$JnbCgz#*0 z;BoN?xv&vkMs0K95EBYTZ!12-xCl3#X+Qx(M7~*37l&s>j=7O0{}vZtcOss4H!(&mtJ>ZFosR7q1lGB?_P zW_~>_m^h$+Q*K41V)*MGi@Mc>jTxUsBz4Gx0 zE+eO`1>Qjl8d#CXGe(e`K+I=9g?7Tre=+|Gg9`X;|FhZwwzhFZD}L*OO)y2$=ZNd^mt_55iF8m($}1`*(mEJY%WiZvdbNMnw~8In5qFX9$~yHilcBnQ4E#uyv66=Rw#vR{mttWuo6 zG!{jHrOS3S7S(&KR!-HUciF?quS#zl2CGzI#H7T6Kf(+clh2j4q=k>oud>9r|5yE9`rFztG=q55`8&J|Z zuUj#Cjg5u*cB50^O^i6?JkK>=8zs80sJt zQv`7!X*l)CFAZHVEf6|t9Bls2QsSTQ=&0cVU?L_`uq2;E9$X)et4^}Eh}1UbzNVig z$|~IW-logCyQrT$w?%|TcO9?dnGKm{=& z2q}GKNe<;fF zIQQ9z+6WC#SR1aW0DmAjR!P2!h_BQxQ~Vi)9|u*ZTvjVzD28ekgNZj-FG4MyVFC06 zpd>=iPW0?XX~2yeg=s9lb=ebd8qH06bor*Dm+Wd6h$Aw*LNN3;|#Gs{k^ zO9iXh-kkiB!4;`dW41H!qvInOfC#F)Vw9EaS!@kQ8NknW#%0PNscpSd>q{UP9sT4> zsB`oQ-V3VwkmZ{Y1H>szUeo3abjN2@bx6Zk6cU3Ynfi1D;Di5s3=a<+m8eQiIym~I z$~JV>r^;GtVrr%sqpEQ23&~#~^Db!+fHoIPHYx-Z!bdfLUUtrAj`5+jI>e%4twbGF z*BaBO$-KD3lh@C}481g?h(eJpg66x5H6Vi~R+T|ry|6k$A=O&*5T)QXM3oKim$B;f zBs?z|>13!Ukr?@6Q(Z9n3N41cZ~|lK<&B2Es||d6Jk0hsds?Zji#sw*K}RREl^n_t z*<1rIej|SMUsUr-tiUBKYH#{_v0Q6rIigV|NUOQfR5PBfXj&g1S(q=30T$2)qoLSb zRgR^P8@lSVQ%YEocp`m2TLVsElrc=f+>$a!GJD4FL1nM9CDnGsf^%#=>D+Mok^$q`%7(P;+9GKyv>2%UPJ38P{_ zO3T*;3k`m$lq$aNN|RQtcR)%-v(e!b-eOge?^T@M*0k-PMIHO&{|r(jQx^y%CA^H# zm3z41zGb+2=Yzv*Ct|=^vQ^Ljx7>tvFc|phP79LLr7FX3fB)z$vkId$2ZzT6qI~Ik z$P`Ojha>3fzqN02=)3#g7)V^}Q7tmVeefkBArGt!Rt*II)D;~(-iQ1&1S~~|Qgjm^ zQAVQa*&M3u+fi}Us+ifQA%8@%k(3xdI*!omzGz!+aO1xA~v~!7E*=^_l|7Lj;bIyYn9yka-$3=Uem!T~Te$xyKF7N#d=T7yk?gI>k0n zqpqh*te#d7J(NbqWw2U2l8bt2=wq=AKSufW1g#nRC1Q4dht#A3y)L9mUV#ITU_zwn zdtplkk0|fJ_UR>)+^W!c!-*cKs*P61u~>nqc2>&k~Dk>uHG{UQAbo~$b2juR~(_T zM|1piJo9MesnJa2(NxbgE6sf5Ih4IZD}5I&d(wQ-w2U6PSmI7DCr%x|C{BSB`5;M@kEEu7 zUGpDkyx@W=>3>L&E$(Yohmv#%AwE<7_r|uOpi$eNAGZ@aIFq<(B6J}vKeMk$j2YxD zqCqQ)No>turP&iX#WYMPvBaCrZ$o7Th%aHD0tV5uQi3;vT9z6U9uaNR`6y59xugw~ zi`w`VD3`SCXPwz{DjXY!j6AL7;ANJY^876eps#-HQr+CF;ax*Sf~(uu9JYm6E5SKw zrK{4z{!QM{B62dPZoP(&kwT>aD93kT#--+hAnJw6P*zk_t%cvdjD_U{kIW9YM~-h~ zCk5!L2p^&T`{?q5jeOk1zSDqF9U;-2woGsr@tMfKS0gA;O+v?#nd)39^ssiNOAOVJ zP$~<#yp8{fJ=h36ln_a(>fA+=csu%>M_11ev{ON5j@K-c8MdtjrtR(U9C3i36L$%) zx@Yp%S6WIZj^imirkbqh(H<)sWP|lMyDmecYgafpH7HkFPhK^2)6?^PLJQN_<<5vD z;EX@Stl9_mvr4Uo3=+`+fL;9!c_#&>eDNKcz-j?34tNK!;7Ls>Id3sVfkg~= z31lx7g0pV)d%d~*pkbJOI4A7@Y}S{2pJ?_Dm`BaecAo+ z4}C8@Lb-f42=ls0@=cfwaXpXUwI-^$f+FSdf|i!fSKw=_Zatr-PPwnP^eWwC@ml`0WFVgp8R)m>$J zHQs!G2uYjAiQI@0j=3jei0xd9NRSzT+g%=93A%*qtMR%5^y-2c9&}QSY{&Wl8c>gN zC1;zFC~rU=z1~_xU7U0)q%WO)cU$~wyRga4CsB$%HI%W0+8PzaJ0g;L-XfOjofIIe zJCa^8OG_5_7B~Ie{w8D|mDbce?E116E5)?iwldYwI(X-)<94s`cq*ZAnlzEW-k?|W zDH{HO+5O#$^_N2$$JR)KC)3q;<61P{(fW&MEKf%%x?he5UPW@6Jn9pNA)ez>_ETK$ zOhhtmvwBJBZvH=`2c2_GH*aD#9<}QTI=`=lWZj1FYEElNDYvz>UDn4szavh~bn>#3 zuMLeqZcg|pbc?-W<}5WbY>#@`?{z^DB)qM_+}+`Hd5nI!=saaSIX!K=t?tn5G#ND$ zJOzT9yS$>p(qcKRA6YP;>nFSW#aDVw=57O|>CYlh+Hb! ziclrTQp+RnL`Bq26Y)I}>VCl*y+C?U2^v2B&$7^XTwf#F_O6|SU$YWz(NxDEue zDcP}Hg#}u509z5Kg!Y)Fl4G!^9J2pZxzy$>?b^i9y|q?{+3yR1Cg7xtkfk4th_JoF z(J6ZHh=`y(kVsG)hZVAfGB(_JWmt9>4Ua{=)oiYKhAQ(0EK#};Lwmh7cim~Td>-u!CI zHn7Nw=#AQPr=-gD>WY9FLVpxZQq4@Kb%`?@cft%@V=j%QWEqg zCks_QUz8X2eKttDSM5e>Fh&VRC-3t4?9oFzZ3DfO|+wav7&!FIV?Wgy9 zTo3bN*qyK=dAHn4bBm4Tvg@>UyK9Bs_7R(H7oD;5gmja${z}|_l>RTCVB3Q^-^0oG z8w_LahxHnV!QqFJV!`fbm(xU9Y{8sMpT`azT?v$6f)ciT1Qe=5;xp0#nl{rMKhE*O zJjaB(+S?*UZ0ivi!;KMHMxf73$Y}8ceZc2eaBaFqj!jy|2%;lZIxj?_;WJ-IX#Nkuxg~#Z<`0V3cVUlK;OYKks z{=Z&;FxyHO^c7tpN+Og;{Q75q*$I;D`_bpBLf)H34j$Eo#mkF1I#_W2!TJl&zuC$y zCZhcUe;4lohvKwOQWddf^zljY>AS~hkA@#M_}4Z}+*?M(+!tKH8BVn|GZLXec-YeL zo_d&~JNi47P@FweaFah~PM8S>w*q3I@@gb*_p^Ej>zvGV^Pl!(H!YCGIS8f?_fc{oA3K6gWuGCe=^S! z`kU-`FlC=R@tpYGJ|%3FBXNd33n+14&D1GL)>MBpF;%dBrT^Gf(|x!##eX`sciJ9M zH`{WA|8W0dIQ=WN&M%~|`PTqLs(!!m0@2>Abxgj~^<~@h!}ycMP#RFkSPZ-_yjn)E{xQ0 zp^JghJnqor8PS&NHfKFb5xUvK9;H( z7vp%9+SS7iQb z88!N&Qo4Xv+;N2F@VX6Ij;Y^?kOfXY==HEU56oZxY!9kf%bSrKU*aY(~Hxq1ijboSFDi$wFP!Kds+sivNL@>%6(rTfaTJ zrMGP$54{+c%5C+JB~*^Zt2`V%t^U)zALe}Tnrpn}tJ*0K8fA#XodizS=4)5II$!xh zXt{4IJo|lkq^e9tyXtU%+ep#BEdJrQ<%9Mp@W@e-!Kz6tU_rF`dOhWQfhv0C|JD)f zaI5e>7HE2!|9DJ0gztuX==YFxrGM_3>TI^!i+ou_j>UXIx3r9*Ft-Y3pSoex91kMV79{*9fya&6$uh zm&+fq!LQdCZ^+?2mT@ zJ`^RV+s;<{;V30fS~NmvAl2w3n$CC_J%Gqqb2hoj!^R1145g7+^q-Buz~j-J1G=<` z$w7qJbV7?p2C5y~+=l!z8(QPa5{c=bbgQPNgG8wn zaU-Mu4`1IDopCocnSg@|H1@ z@qK&mHP>8o&ILZ6P4peES9rz+%2BITNesxH~7@W1Xf>?}^il&08z1VA?; zp#W)rFEgt7o!+V6t_oB6SC&!=@5Lzc>aLwU|9;R!0*tK_rjZjtit*fdealrTFJylm zeVz-EP}d;qSp{j)hBRqa)cQ0N133;a_e0gtfExA^je7k9t163Q}3wSu5av=8E z3&xLR!7J&m7<49;J9ws&l=z01BN50wH%2@?y=@*Itmw8pKL&C4=n!_jpT7G3u~c|n zO5^bqi@m=ifG0W6n)WEbbMHEb`gCu^?%$}_WT*wY?wYYQ>Oa30|BK} z!aKZxIIx>yV1p*Auw6C=GWr3TQd*)Dgv*?RCd89CYeScB^*Mh13D3UM4L#}O5FB=! zFN;t%bpYuVY16BJKlHY9@Tm}9_Wd5tTt9#TgUc^(HUg7LvJ0?T*@=2d@8G`55z2Z9 zJJVHrWSI}VE^4d$K&BaR$Bq6-Sp3)&czr)OEyZzRu!wcM&f3`8Z+5}I$)MwQZ;>@0 zaN%b@)%49vc-@l)mf{NjMtEVLm~uK@blA#>CVP6M0K(f-rplH*oDa_ymyc%EzptUd z+F^TQrs=PJ6}N}@XnSBJY&pd+&%Sr+bQ~nsx<;?}_u}G0wA=dWGPq@T_>$fA?uOZ@ z!CY&kw1iT7-r>85z~AC2Bd2?ezZlnG`of8lF)s>vC_? zV!AC{WA~fxLLCcRa$EmFihRJ^*!b#?bk|owHOa*ZJa_Dfh0OuOT(5|;C{-_uNz(Bj zsywb*MRjG?_dNcjTT zjOV<-BOtt>pEklyC(d@f+Ve#ymI>2A$C4c4RWvrZ?pem12)BU{RpUvWrnJURfa1yF z{v*X$-UKQ`ue&=FoZ}bvua+1r@%lE<;5==tt($9Y3|92_wz2+Om7tkFsbci8Zu2tv zYSihtcoMB`m^c2|>zR=}M_&Pob)F_3fN{+EM;W(Z$r{``e{!w1Cl(i3Z0EFS>3m|x z0pSqT(rVc71-hUK?4h`z3%Kx77PXJ?jE-vdwR!tQ6H|YGSX$LV?v9(ikGgL?-mfEl1-n(oukTPb zn@UjTa)tH-F}`nx^fc&cPF?tyufc)eQ+2|`y>lj0FXb+(i zyoJIA@dTkM`V&ROR}X%!iy_)Uim~Q7Utw}Q;YPCj_9{O1_@M6ccsLW!;^o{-kcq&5 z+nv-f(Gq$(g`m$ZM=1QBGUYV3TRrW(c3rgL9lF_h;QW|kc=#Gp*4FX-M)@gil%x=c z&76(tivzob-n;g~>lLGYn=9hOr3d~c-!b`y&la@`#qRY*;C3ty8)UWVY=2}3kF%Jo z#SOLd)WpG0&5ius3gtPm7!%Ql!|z}}ljs|)%q|+Ax@|<9J-OG6fv#FK4qhEf~u4TJkJbD6cXu^~YZ# zxC4#)Goe~t+%I#>wBFe7vsiM-XK|)v$2;u_UOEpw5%CVX{Y^#$xFU297>bV1SkxTu z{Hn@HkGU8M6F$e{D;3?bn>;+^9790 zFP74ww8~&WA1ilaDk#&6XzP`ZaeGc0BWo_}PFLfP97NKI45e(#JbZchePi5SFg)I( z#kX!p>*!0xXt8x-m5$LebE4;bVx0SW_dT{2A*>1X(2e4DXc$&(D(L;uV2E4sxs z%Y$@w^1kje?gbG}fR;=sX(f}KdM<>h=gHx(_coo-!ekU8k0_*q-j|=&I6H?~}vc8@%V?9{pm%JbP zf=&Y>QS(Bo8-nx|w)37v>&0}v*3D(f)1eUI;SjLppo{03wCep;xwLtsAjOT}b$`gK zbj&?^enK$o73V;s{#zJD-aE=+-dkZ+Jc5CI4AFPPj_;HbE{{c(a|WyQ(T_(D*TqBx zeM4A+L#iLIQ1N;X82)SrZ+iE4iq4MKF01?654}X`cU!9ChMhs~sN}bpgBI5wD~RAgV;_5d`yMA^@x1pZd40Sf8rhm{V{;hHgy=!F8oY8#5qM%e2zX

)n6vWwYN5;KkB{i`~ioctVZPNE?mFH1&fWwyR@;Q=9BacJTL++pUR_ENh3~qw{xZmPydY5|c@RQ*?Kbj54Y3@79qCnFfRWmoc z?>(QdyUs6p`E9j12h@$_JQU@+&>o_#8zw?=KY9f-JApf}L|cW0)!N1O!FhD5^uM9I z)@@x6b{UC&Ix#Tx+!ywDc{EhvF+EJ2I{5+wyw4=4r%ita7wrNJVB;H$ZgWo6iLN%+ z9EW^7FRq6?_Y}FD2SSn_k2p4W#+{tHzbhV+q>?t#A?i_K^S{06z1|h`zhppp?%g4R z`07q$J)6zBIU4HCO%P>SeQmCFwbo%VcCDj$U?3N$w(iu~p_u1B*O+ov6^2-ctEbte z*~I)4TC9SA*-*_Pae=8OW(CLDW+KIf6(BKih`%|eCN z=#TBf0UF@{>;73=ar@FD{s#Ap0q`GabMnBLle-1I)j!tRF zvLY84ZZM@5xNV)k)t;mleBVyu@hk-^vMO_1CO@v@`~BzVH&_E!$P7$YNLboLE8S5% zyl*qD7PlL3ZPoG4s$F{>x#V(~+uLQBS5wS-;z4zTv5`}vhz?XRA zOJ+Nktb|D(7%--viOHWssCJxGkz`)&kYu8U3p*GP_*8&X^@JEIeGXuZZP0V8zuYF>hFiN1g;QPYMC z2Tbe+v1ce>5v^kfkl=r{=w^S)x%$UMW;+}WKLdjXCnj_emrf%k`CtyZ)ZR!IQW@A@ zb0B~1x8yI~x_l!do;+Y`p8#-?+QtBrd=aA@u47dzr`i-P4~2&p8m`0fm>gFR0fsE%nGsCN%|07d=*; zT4+V&Lgw!(ZrFUdeCR4GS@T~b|C=F+!E?Sa0F@_fHNg6HaW9syxT+3Rzd| z4{wj{ueNVi_?%6+D;o9uY$-|cMB?!BWmR7me)rGbg3PW+$hdd}WZzy?@3=-A=E) zJng<)Od~s(=VT6|gp@FcZeBNU3QZ29QL=WA)l_($GO?GmVXf@-Pa}{i)e{{Cb;TYQ zu|yBXVVnglNewP`3laH$r=p8Dbxrx{CC-OVtJNkzG*$~Wxz$I7w2c>UFom>pcohT| zo&=kz-MX?Jm}Jf>R{8&OPtnfE{_cRlsySFeS@ZKbT<*bBv!u%rV&{ej1Iu9cqcG*i ze%hLQpMa^NP&$b@#gtI%m-H2jN*P-K?b6x5sZpx)ckhIqUgJq@t*|=?TqC6-rgxWq zX3R)|)5-{sUQs&&pQtu_*Tu0mm&;%HyviG|TiufCh1NBUkVYVV4r-x%g7+5tBA)y>F?v?#|TkW{W<&b|6Tv)30mzYZ+#blEyXQb#+Lsu;|Qd9*P zpt6JHPDo*Tb7LYCbLe%DW+Ti;pp4p7ztQ`D0)chbdV-9fo)LNLyn?$~ODUykFO<~L z$#M#?GpJh$Hawzy5W{|r!8*FB$*M%4rOBd60co21M86{2O?}Z|lW5T?`z*C%G?atY z*ypyyc7B#)bbhWBMPWOzCU$kOE*=&qEykN4vbEA<0<>4IqM^h3k|oqU`fGpyr0VvE6>2zLz&aYj54VYhG_eFiL9heBbNMv%B9L5k1Mc_wg2y+Yi(D zeLv)11>o9@(J!B7^6^|2?*MQ20gv@jN_xwN${_HRn`5|FGScG z5|Pz6-Y@nhhRFLuE9kYtZ47%abSa|HaSYJLg_k!z>LV>lUA^0RykTQZOWo&P?Gcwk z81{!n);K;|47Jlmt8CW>mS{>Ia}d7nJ@GAKVxx9FM zh&`wsF3 z1MW&-)ozW8qQyN!z|Q0*;i$0Tde7nu;~>Gkf9LZAzQ@L`=0)c{OWk!(EK$YRV?vW` z54ert%8n^FT_Kj-UBrs5w<WzO+QO5V0C-Hqst={$juvqil7X|;tf`7M+*K+gs zT{o$zo%m^Trg%K_ZcKjR$b1 zf^><8WY(w0)4Y%VfE;h$Rz1dw1TAE;_EvI|*c<3DhNa!P5CIV2I-=+>*B>mEy`U>r zlG)|PWy0L86?#iZ5^_Abus@^hcA{&3$*Z?d;UB9V@z)dhMhY^axVw*EtZ?n4E@^8H!}=rS0TkjdxNKwrhD$xJ`59a z{10dNAiv8fH9&f12sUB;?)fg+;`bNf^*sPF-}t3zgUs_ftd;$YEd&I*XBPBZ?b1HIgg@+l3`Y)1 z<)GN2tWmAedIY2B45W>E1|K=lkC7^jyvNx2gaq*aPAo$a{UxuRxtZDLK%_0Z?h|kB zDuhDzx#JJfUI!V`A(Y2!!h`;c6D5_;^J|kUz3+FvgBJXUxFqaaJRgQ8Hx{NUqTZ3T z^LEYX1c(X_>{*U7&$-fsY`^=6)`yLk$9^io$x>x9F72UI7ZMxcCD@%%bntwtej1z` zciqIGHV7f+$Dm#p4u zkj`C@TtJpIB=J)AE7GXa?CHiMo!#xZN}wlLNx~L)jP@zJ^!%;J$43UCryk>z?T15U!RKc4ooizqucU=eWh=9Mc*(`<5 zw~V9ufn_|=HiW$Rg=5X-4SY*_2qVg5lFkGBY1>`~@$q9~q<|I&V1c1mdjVc5#wpC_ z^xM@SBDT_g7zepLw^c_gSwv$hASB`~!_iJTt02g@5>aPC0r)2cCwk%wp8nre1Ks+S zk*zrjBPyCjg`$6^k2N%%P5mH*zr~zd7uP7;ma9bje!EH@{A1Km&hn!$>Ow|2)&_wf z4YwFTvoQmx*b#D!KS;M(TPUSx!`|wVrZ)pct zO^!cGQGPAzaN1`cukpXN0Nz7Xjh0xy?;7tdIjN++aD(cXhh{Pm^%j%J{qieNP=N4W z0%6DyBlU=gMx!$;L1utBFIDg=GPb9d_}MYNjqM`Uk54bwYkHK}u%MD~64P!q*pEg_AB=Q!e%Tor3Agj~7FM;b=)VxhzDL303!moKE+6uei;Vxrx$ zrH&|Y27S}HNuw;jlcIr3Zz&m3pwV2q;EYOf`TH=f9%+Pfkt>p5=Yo zE9jEZ00Jp^8t?49z?9cfy2a^6n6qNc^4mB0V6kxu6qhS}f>t8Y+Uhx&W8UcWQ+E_H zlZ&4U(vlHQH6WFNWcp%W6J3JJ?!eRVRU8|bV5Ly-=!=P9IF&-u!=eE(GC3WZFTGlt zXn`}^MGq~9TRf@0Td3HgKAgncJme;bZt6~)RfcrIMKzMjrOMExV)s~pED@M*47;I$ z=Wi+jAIP4x>f_=Wm6!YEtej0cz^?fn`S-iGotm!rbrrqbsjiDW_DaX44#wem`n{Kf z3wHxTR(BUV2&2n_cIxGvGBUFVK9Tf?kb2^@7J`4fQ0Q3|=%Su5?o{XaL^+pHmw>5{lgW2TB8Ma%WKDd^njMQH7L(&q$TaKmcSx#m}y z3Ww4!YtV(ci+rBZQCbP7@K2VYLz_=i8{kc<(%AcK*Sq>7vVDuk2v<{(dGB>vHsgw& zg-T)rNooL}Hi^YK0}+txA4MeJC+O(tr1L#w2-+vq)F-!5Pz(YNHRlq542LdUpj5Q` zb~6{=`7iXh*Fcyu=i;B3xw@dy{M#EbBmyBUYqME9vv8n$c4f6zG=vF!&}9rZ&6&G5 zD#_=Jb0+9sMl%OY3O@{!uf-%6$Hx3N;=KdL(%uPdGhbN^RD=?hN@IQ6uuF6t#Z!4H zFkmZ1gOqI4(MbxSVydJi%Xh%dZ5Xhp6MX8B`Kpr-ayTkYZ^KhvLHUg|C*^2GJB&O+ z6+z2$xiR-B_Lup;q*?&_;5eJ8O#p5CIRz%I*k(~xQ^+$d*HHLKzDRH{S26!m*ZZG2 ze>AQR^S|sf?&wo7Zu-Q}OYZrN^`eJzHViD4z4oI2PVQ*XZ}nvOnTaCEpOHR@RfRG3 z!&*^ntOxGVDW6742Tg-iG)+HC((y~?%Ye&N%B;unx{6t2D1_H>J}Yb4M6Y9o^>T%G zIAC)%WR?|So$K!6$?~2ijX({U>Co`9J{jAM)c9qVsl|d~6*-B}#YabrBqe?L0It_S zs5*E$D-yZYgXCW&9VPf5ug?(|x;3G?X2hMm_`mXxzqBegV*4}XGX~2}SX*=zEo{6K zm#Yw^aXzzECBj&pSVEPNr#1Dxm%<`6+|(aW`@_26meR&v)i&%^ol*jAN7?W%W=G+F z!9rLtD=}CIqKiQq^ty`*YqNl^cGq$sI*tI~p?}GEw4Cw!qkhSA@$T1*?!KYw&$W?*e>v1(_qm2`~0#4Kzrl@gjaY_jJR z6dhW#K7E?{(*iPnYvYK;7N~kqs3NunQz24WQvNTMzKG$!6KPA8X6R)0&#n^vdi8Ac z&yP?u^nL2^)77n)@8*!s>=46A0Clk=KGx!GbVy__*6FSW%)X3ONBSHO5Sp)8>1AGn27D^Am++o>Uf8vFSp(#u&Ewy5RTX0>csFi~?u3NZ$QDbZQ_UX0F%MAjuF zPlwO7BCQZ?WZh?pW8yz^%WNM7Utvc_3@#amNroU0q=e#{wCFc{B))0eUf1?IFDiRh zQ4(R(Z~c>GC&M}nnX1Vz#YH01{KbSosr!3J4%iO(s86es{+TB zA_lxb3h_b)c54BT2^-?FFLI-hZTX?~tKN%3uDtLPuhAw=Y$Fzl$`M09Fh3A61dvQo zV=BVw;tn^X+g(T$73%upSIE|9p)jwgoLfL3ZdT9EWoO#g)(DT!-eM~@BFDQ9*w4T} zp^lIXb-e2^cI^OJ%Mkfkz&D+V%XM>5z?I%M<$7`gS#c1Mp9hP_xLVM;QnZX=EyYHilCIW3GcI82M4*{yZc zNh9d7<}!RBf8CUIyByOV-OKp1;X@5M`uW8tLMb=2y3)L?+y@k|Biwt(dX z!gLee&%){5RKtfcY>V{Dh!H9Yo~82lFr=LsAN*d ze1VLvml6sZp1E&Rz+TkpcHub2A9%w!F@qIxsI{Um|C<-8uP7@E2S|JVGs#a=?F=^c zp$mzCCu8>4vc(63mBStnQvYF_5BW6;yS4b2vq+v191uioAu06w1-bHvthETbjXTX3 zG$ny=pH3I6{w4oTyhH2qhsEd`t zRuYyRvxo0%s^Zp?LP?91^GhRPE?1>w5dzQ$alhF7X;XEbK;`9)QBat7v6B^ELMaj~ zQbGgNeQtZ?v8yelR7Yg=xnQ zEUBAN`;>yH`8~tP|8lzLwh9{6~Y!cqra+C{(1}$i9ezRL5awa=8U(w>1+5Tudl6OkoJU{HyJ7$bXL^GVRiB?)iy%bLq_AL%3$K~u# zIDu)a@ARgPsq~9EhJ5q@kK=@a7uI?){LQkf_ovscN3+wENU7K6cJ_vDaM8$2hdBk_ z`gSY<87zn31y~zuUZToIZkY%~8MBy+F{wgRQJYk?wShmGa*T$q z_PjDEVlVJtlm^;T6OL#@6nhnjMT(Rz{u|0~6jVTrTtRBtD6PQo)im8Yso&`SDs{2p zlD+{DZWXWTwX7(xbglxbEZUHr7vk^+i#+7#qUTvB)Z^)`Lk%Os5p1|zcF&)ZfarR} zvO>i&8E~7Yu|=i>EQh|DdWW4sHc8ukB*7p16D5zkq5-?hnj6NVn2xkKtoE=__&&Tu zUYn+6?papDGo6o4{%={5Ugqz#EAZd3haZpGp0=o_wvY0!?)?mR%8{x=qOfjmF9{XT zo8Gq_9=wi)0AFlUx`aVn0A@YiGwYTIq;{Ec@7dr5wFT*UQnY8D`y|n!j}v)E<8cUXZRLNa?om7 z*&DT|j4&v2WT^T>w;0Tl^ zs`1+{_L|9K<1PGfB@q9nW}%*Or1fJK12>YI#+IsBIUP8aZ$ZJ(+O&ZG9XO+pxbChky4?Jqbn+`kv3bn-~ z_PQfC)OudjkLzyqfI8cn@K$@uoArlz3|m4Vjw`D>>0oryZfT^cs*!&o;IgND?FNJehJA3tWmg zGnR0$LF90a5<(w*^a4EsUq(YIlyQX7jZ(dZ|G@Huc2}*0AD}Qx<-8j#{0_QShZEHq zT0nzhXeDq%hrIdiGb=Uk#YCaIsy|H$gqKjm1n0LSh&~g)(d2I`KN5a&eB72q9{^sE z(DP#3IN=4%weMlT!nDfDtx`CP)@9?v`f1%p{ibDg?3Im2y!dgOjlcOzS_<5E%#e4- z858yP8%g}@f+4EmYTFkQBvx58EkrSsa2PxkeG!G##OVGh^~4#v_vrPSVB|cHF^ewC z${|iJcwQFUtj4O=(L;&)Pw+_N&jcTjQ!>1fY?dd=+5Q_;+(vrVw>pN?sCC>Gx!BZuDJ#Jh6_g!uLbfe`p)-mlMTedDNc-F}X~F zeG=u~jo@;gJgVAY3K+<1JqD#&f_1K3DYNU}^>y?-%nuy%T&GkA$;hMj~bR87iy{&`Dav8zQE@)|66%|;nCaznhv0eLkgM1 znP&Y)31E2}Y7B}ORs7tjgIK6RKtSgEO?rSVqM$L~QhJZN0n%d8<<{INJ(aetqBZ?>0POfDQLe~@mOy(@|e$C zALy+3U2W!e-e#lkBaU42BUuXN7#td=NTK}!$h&x$G@bdBK#s;eH8ebK=W?LH_t0_Z z*VS^_8og=C;qnc~boqACrBAmwP}tj?*G+ek`Eq_$o1iTWr{u23#(wW@wR$gV0L6!> zFOHD0a4;`ANIQVugEzy9(b0plZ~ZZtrnd}f!we(P2Kw;zj-Aq+mu!23@h`z_SZ9=- z_k~WCPx+h|KZ15W=yIbai)c~5dp%8&e0ACpPd!;9{gI8;DXdvVyEB_9w-W&txR3dK zxMGV}MNj8h#%Mx}IwR?fZzGe`##fOCO%I6)$C5*mRfdwHlJUv46lWygj|bM8;zs9P zypBP{`h<@{l;jHh_O1r*e|gWFtr%To3%i{=b+mfs1V`{5%EU$_q=X%HG4}9#!w%(% zvzR%|=+bZR%hx^@A(GcQa?oy83yloCa~&Bx8V5v2tPi@L3A5G&AD<}%mzfiH+bB=@ zxOq?-=`x;f(Dq=Vxo*cpucoK5yXGZdr%? z_MMKt`{t(!R)if#?=s8K~y??Or_YJ;xmSIY}_Vi5swg_k5D@sS?bb`QMZ#0 z6RDLYKbJE7{Yl-hU8=7&gIHleUiZ4-)1+W!^jBCIPiRxKq*JSo74jB;!pq4Gv9#+5 zmZt6)O3{k96F+m!yPfYLP)YlBrTT`w&2XAZI(ETSUl%+3aMrun1P`UPIZn0M16b%d zHPFO|(&i&zk|2}X-c>i!OV}=!HkI~jS5?(-NG=)2_5usHDI_T=eyTmcFZF0 zpTA>%ciqFc_p(616Y`2I7xao<`;5lDSGvpX>-t%cVLX~E_pqI~1@b-2rM^L=W6WSL zYygEgE2O$3Ob9gEYXb-c5oJ0U!6MPAj~iX7^u3*S z<8Leq7vD@lC0Ox;vQVS4S<`;KsK@hivZ8RwZ42U2G@JHW>%sS0QwCP?tX%zGd7;0s zRuipTJVv_OIi_q(Ntx4l8?NUP>|R?b9@ob*@9lm*$IVS+9?Mh>e(EVNH8iW4R#O2G-z zmr+zs7a3496D2`QgItR16ZI~dDs2p=?LjV3PSZq_+sRH|EkQ?_%`JyL57gSa5^*p` z$LR4qsX{q^>~FK|#J!4&^1COkFHSFh7Sm&myCz@6xF+`ZOAPkILP&CFweixm-e%Ocs*63pzX#Qux_y7_z03n|Ft%4Kh!OXYSc z&Nrb&M}zGnLsKiqg&)b#k2S=vJ5rC6kv~2y_bn*NekCn@^KUQ%Ti#yIjq$tdWih_t zS$I^)bv1WFd7f8F9%7zEKx?dKu70S+%(-5`f+s8cQgcgYSc%|7QvKv!u4uiE5N1ah zZX#b}9_cS-(jt+3C2}kRnPm13XcS6oX_caeQwDFhqVHlTMJJ4s@X63H;Q|3`xndV; z2Y=l7Ce??24MqO0JddoPKxfo|Gn`0E^~DX#^MPZ&JiIbnaIs z$w6g9)eD#CZ?GkqadOn@&6OwMfbb8PwecF*h(RUA@IhlCVXb8>^PZwZk0way*gaN2 zkUkdkS^7QY{gI7aARHGcMpAB+i~Jn4fWHtM1=4x- z*K-C<#m*(DEcq{`UsdoBPMH!e&CTF=KcDk+UeF1?UZDx*G^S4o`48R#KP5@ky9=|t z^{+#ApVBGfo!>{|IhTGLPiV+x8uW{CzII!~C{4C=W%sm4(S_hGYhh8^^{AYDiUw_^ zZ)@K}tw_}t5(P--^gkZkdFEriKbig9bRuy~4MGqPMmyF!W6@kL%Q+Zc&s`0F3B}@TAwb0G*b^h1!$>xMp z;)jUD6DyR^I@%aG(r`zHB;J}%)h$DE#LGEyq`DMBuJ19`0E5= zc?K2mRn;c^)k>m)0#V7HX_wkd_-jjyeWOr?b_ZS$~A6F z8G6HtBWKV_IOx^>Z!LhtNN{5dD1DHlkPXFb^X6vP;i`XU1OGmvo~N!`{;31Y^L0Pq zYCI5%J3tARRcCOA2uQ9v)2qTyftW8(zkQD3zX>RZo@_}dgpEB|J+9o3h%mo8QS9S> z8@LEmb}&ch{bm0xfk5vR*;OLKg=J(s{~c-o+c8%=c66tj5*355A^p4 z8W_GJ;E}$}l=Rr38P8XkMXs4Facw4KHhv00JVIdWGBXMjcQ}PQNVECN^G%80U@=E1 z+|MOxVl>`Gej8ywbs=$=ln70w#gf!>V z8MekOI(9f{@m#D%W}46$)?772+s>X;pr(lSnUzGDP6G}VCbNnp zn|mSsg+-7_XW`OjF}J}1&S+@TIy1b=>iTW^%tW#fQ$(t_S0?gO^Q@8_mQ^NF?aZ!U zYy^ZUP^Cn@5>bRfUMz=Pfm3z%5DWjFkc5jJDe-nIAJa*-G z^!}bUo>iWttQ;NCEet>GN@7M}gIP(M%@r;hj=2mPK64$9yJUVioB(h4=+kRW=lFwv z&tppD{n!W@k8eE#`iWAB+AT#gO;gxuZfFV?ld3sub+jFOk`fJRYH?({%$$^RYJJ4V zJpHT?V;=b%#6n~X=9l)o3>t}xi4}ZdYn*z|zABj*_j#6hfB%Civ*{D%0OHBh%JC_* z;?rRjT}hsaJjS<~b}K9rzo81s?;^a%dO=2SRR?q1x!dCUt}Os>pz zx(ScZcx`s88wt}UZ_Y>6#$ZL7&|$wHh;+M*WBZz-5iWNpmkSe(JRYV?0JNQ<`*gFM z_9Q5bZ};9YtnZz}`$Z;M?kjAl84AbgQj-ym;oBjNf6Yt=cNH`eMjYqDl^emtW7Xt( zr?}X`WO_KIxX6%*3!d~YTGD#zk7>DTyeGMtTpzpo!7-xaZVNk zH}-5_KyZey-}ieG`cYHaUf?a3wp`htw+uOxPCHL091vIHHwb@JXF;$z7+oV}MIq9H zJY?#gpAv>gMJ+XsGJI1igM0IUE)LA^YjT{>MxxWM(t8axDdT4(xDb*C*W03cXkQ zqK27%^kTI{nLK)jx81fcdr&{zZ#76!RdjyiktaCwb&)X45DLJGyRc)D2v00&Qlm$k z{v?Y`Y+IFNw|zh=5ZOv`x1cnJRpEye2|f^O;vJNtCplaB-nI*BAaAoDYNuQPG;TO< zL-HBaGL*?elG8v!U@IO4arVebs!NasjBf0lC9QQZgCOj&*cfco0rbyQw8OGf+*D+D z1Zi2+*mN-lVQ%Y$gsjzZ6bv6Q7A{lb?>%j9HW_Rd?mk@7`IcO$>xp1chy`V3v@`)- z=&<51Npr!bYuH#>`#+_DT0F!?sWxCS(HI3JtwcB2NF5BY{+)I#X+0faQ?v*NF&g(( zs3$+YI4X6)yV4|>BV?5eiyht^l)K;&@-~LoM5_G$1v?Y*ebN0$9JF&qhbenw9baGu<%V?#v9)gHL1<}k7@GgM2aWx;ns{b zi|dQNUb_>U@N&do#*pJtJ^!DnW!==E9NtKHu!=TTPF?N27q_w(tR4T7?4uPVuWHi( z1)RhnlES%#pv_CNtw@4>p~^xH%!HsY8U#Ir!KmM^LY}CtL|x+AuAh;nJU=fkOR|;e z{uuFkK!-2UdWoiR#<}#b1u09SBkTPoacTZ+`ln0tQ>z_97Q|PU2nB&?u=+y!r)!7} zEdv4}kU2L(sg9zT*3A6;RSs2I8k>)BC}GHp()aBisS$F%2?#L~HF2qnNxR00u4*urbxDQF(wKWdTq5n$93{Vp zX5gBdR!1c$m7IxzIS@vG{ou7ByK>>v>x6xB&(W`lKX>;Qt&hD9JX^ri=szszs%SGd zRr)&cJD$Kk54_8-PI=0;~rnb6cC=$Dc5}5+@ zC|Qs6J7Dzv4_slf@sj-~a$nL@UBT5}-1|{0b2aj%1TzQg80`-+Y0yVRjYDnDI{%^S z#I@ieKRB8*1>1_kG75O-0SM_z!JESK@b0yu|3lSVM#b4Q(V|Ikcemi~?(ROgySo!K z5ZqmY!vMhv4uiXUg1fuB-O2l%v(`QQgFp1s-Bq<~*X}AS%fvro9ZW1`>*`97NXLKc zEl)nxxn`>x!R7Y)XY#NV6`aUDB$bGJwD+xMEZ(FgveCqjslsSd=AHy@YVGr`bE}%9&VULJ7m`X}sVR2zWB~i9Px(uZ5l& z$ZeJPPLmtab7)7ryG@GZptGJ!?R*6n-SXa;Dv{RF6GOjlecyVlWA@aC_OaFG#r1pF zDBWeWi_^48?4mZAryK;MWgy(*QhK8V#EvbD0P@*2bOc}MH<4%Q~wY46_OC%MO# zj=N)!Gd)1sCCqqdFa*NdhA}rC!Sj+2N%sz4xsyFzcORh36GuAKtdhmX=YT}`=XRKo4p%UFC9((*%jrWyo-2y ze%F1v-Q4n6WyPH&edv=3mEXik3sTdKFLZvv5&qXEw@LRebcf1gCWta-(UHjjPK~_kqhkLgER$m>6NE_6h}N0S=o`pTih-Z|N)7?b%cFSR zEZLnmzrrI#CpoGtxR+ueY9$@Jkpo5hF(3Vz7)(m(Varx}3lk(1BCF*Qt;Po%Aj7J+ zT>Aw<3h`+yc)Xf&{hdTv_gHSqwj!0d#f~$3Tlg-6lZ)% zb)AmDs$#O7wv3;1Fx;Wt7Kb5)!4b6f;Xs=$`7!&I`STWML?c>j9 zdl1nS4~31H9{kquUO~iWbTeZ0Zg9Pew?p!+miSpDg9JZa?)+~`dRE{FkBr2 zF&i}`Pe*q}6H>ZD5P@!V-1>opl8(-MBt4v73Ok2rl8}fAU}KaauTjQ9l>g!w6%Ptw zAM~Qm|52(2O)QvW64(}dqMRwyrB7lUowz=3+V+$)6;^}N^|7+SbJG2JxA>DK?{nck2dZ0)@omGX~GnNkpnY>pojgwqBbLVp|!{S00oWkLhR~Pzly> z_xUXS?(Y0t>UtDeba)KY-l_O@H zU638IHh4ZW;EROI1Al78+YzP-z3=&X6+W6c!~#!8FrBrv)x5uYg!CKgqr zK;o*bcuxra*(;23j8=Z6w6?bPy~ggGxx%ZRJh7dh3FF80Tb(xo*3l>@L4=hvx z?_soRiZ=Ej>g>(k zed3t)y)7M6-nc#+cY?iRrkF!WjBw*BLUg%YgHV6k}qz{e_q^l<@JE~`@_-ayTj4w*!Y=< zy!&6m2Q1QF~D%xf5~o7vfL{Z1uUUF@S7 z$;>V>nLu`f=$781}h1m_vOlbPIw_=o$LS?aNT z#~Z%#DCEtA&a$p!o%h%Kn|7Nxpq?(H{JL+NGTjaDc}4*6lcn1k9tdSQTUmRp3`a6Y zKZGl#mK3%uGr-*r;s0So%c{)j!F-y_vU=3yJ2&^bPItq|x0LsbfOkE^@uN~?Y9v&> z^{s5$W(=VZsN6>bQeKQH^J}y8tA3A%#H(~TAd_z@NI0HlcN^ny2kLN4HV}7M?_p!~ z){4o*_JU3*@D@(E1fc=P5`|xCR$Rp3BQ4<+#QQ=G1lVk45e@pL8@<=0#37pHhtNGK%FWUm))L}& z&}7@Ifz?5HYDBUOM`ps73cwh+4Alyg*)UC`+aYYVoUV*o+h&7B)vC$LM;Ue0n4i`n z28-r;4k6=Aw%`4g2hZ)6SRY5x%2ZDAJt3b=_tMm9FkO=L6Wrg?xAh_?3X|_$I|3>o z@5-xmbtaN{>$PW%GevtIM-Ul4a`Ps>3?dKrFn)H@3PXC%eCPuI$%lCOd;1n43sopd zfd0rU?^uF$%iJ~YbSum^L!^m?Sk`3Yt6)loHy=jT?$p1jXgQ#y#H67Jmmga%77Q7n zWn@#2m!o-Jj2iOw63jSbiKPVwU5<1l>qpn;#2L=?Q&Ehijrs&hdTchU&`q(FJpOc-i2e3eEi`98)QRD>fM>t(O%CjAXD)j8 z;Yj}|&XzX%7PcbPOk^J>oP2JU{cGIwS=V)sWYVZ6`f|!@eW#3no%HlfVQLcJ+Go3+ zFl*CwJ+CqK)<0lQCI-@3jmLzr)v2?G|E8&SgEZW&ha_PCT`O-hZmK7TKWYv-u2jne zVeBTb&!YLb{qi=@`=+4gSF$l=6kgynF7k2Kb45QJ_sQeqNIWf4aMuY9f z*Y~Ch-abv7#F?*X?reOeW}kGQh4fuFz*zST?cHVXMEl*8DqHXeeQE!8{u-uYM-HBc z*LD1X(`FstkJFwZ=d)V`+{C}o->NvUD72~!C+3ttDn>l5vR(MrqN52;OBXw6&;SK0 zH2024swwk+c#!0&kguqmP^xO%E=$Pbb4e|TNCpdp%>~^>M@nZhBEh6*BYR);5a)9^ z7-(yUeBG;rTs10iXc$S+DE`K~6Q5~O3>-1mLYp)?EF>%>+jbs1r34)&D7+tc!Tgy>jRw;pUQ?shA)H*Q z-YQ_wM4DKh2G>5C>+|K#HWVTuTZ}W$Zp%UT@)s8`Q-zF%=9&v*I=(Le84f`z>Y?f5 zlX1J>Ka=LQe;(7<#W=>yF1CUq_XqP<1N>RPEGmh=xU86>ruWq93-3P%a!=X zMlT!=(?P?*$A}X`vZs=Ewo_MbU$>}u8CZ~x>V9|pr~PtU9#~wgs;1UA()GeP0B1_^ zeIrk91pE>BZ1&BOJw0u@`xBW1EpLJKzQ`>I3fc2b@oVj{WzYkd@S&zomYdz8>KYo9 z6UL6VEh&>ZfWz#qzG)9OfS~spA1yJc21Dj<(^WrSs5!{wu*DrIUOpZnmPvf5ieFH8 z!c*B6KTZ!2t=u+e>OmFxC!*mZE6sY^sbR;!VT6saahgM~HioKyOgE04%~no28*^S# zAp}YczFeFt@t35-Fc~Iiq;Bx_G0Pth)ls@1Eo=n${+L8r{W2rw&wjL+zGRVwz~@R@ zOrm&Zp^sCov%H2P6E@O-zD}c$nlzO@))Hk~T?n@vvdGARn*KuNZ5)iaPjE_G-#D^q zlSXX{R2tAnX_u}NIOU%nqmh7KXJRErfTtSmDj8bTLKAftRaMfix3frrms7WFpJNglIy(Eh*px1TeGQYM>;y2fDOe;P*-xpA zJBB2~ZZt{e$XluYO+)D@K4fvIoz`8~y;E0n$4Ht0=wspFpk+lAG^|lw=-&Gk4_HpD z_{olEu+XSo4QSVoPeC6`x?+W1VWf5NDoT-1#AqJtZh%Cai*r!KWMUjbvw;grK#c^` zZA^403PV@4(;SfrDmZoNnPT}*o*(_3k}nA~66KLb@RBPDhiM8;vRv??g?F;YXsonm zF1e`mRAmEXJP{{4NPH=y6q&32hJr|uB}?gu_ja~juD@5VBmsOLtf^niR2s15QcktC z|5Q9~fiqE02Q1ZY8D{gtEgY94**EK?(Q+BG)mwoZ+I0cN!e*0+eIt2!PKs!Fgx2kA zc^A;yMD<2$Te2^^TnIQ2zz;B!W=hCzA**1N_g{Dv*x?Js2NkT4eO)XpthUP#L+645 zXI*Y9WI)ERy50t1j3R~LdK{CLpU46SwcrREO-|;-Pfcc_(H1EiVW~nrz{1&3WJd4O zU^OcqD->7@a7RAp#{_71Z1Ym;J79ghe7Up!PD!aMkB*r2DWfD`_mLIw#lh7Pt128^ zy^Ff1X6@zlGy+@Y6Ghe6vJ>b671Rlr;Q>aE->VIyx7%NBd62bY?Q+f5R00rx_Y@@%O1Ztdu@*j z$BbDL{#~E2#%kz>Ea?&`@L$P48P>#@<8UuBQ_QiIO@L%UEE@3WQ6XYV2d&vwc|xYZ zmZ^aGRU`4bh$JQ_cn!`!<`RrxVs-0R&d<$<#eYSQEf0c2^mfOUR9r47Lu#Utli3_x zjcE^-?4QYPB_^_qjtW>7${ix8?k5d z=$6^tH%L?eSlIKd_gRClt8-iK9DyG&lu}6X+YEZ~JaMt|zZ!8A%oUr_wf?D8E}R+_ z<;uoq(d6Zp6nqAk#JF`+T>EWKs;;gMRV?3&Lf)Fb=s&56%BfP4WXk+3pPFIE=XgEo zxikEPMiY#hl~4A+rxQE{rNtLq@Qiitc|>r77K(o~$hW|L7K0#K(V+$QPd7ZWd`7Jy zhr4(8P_%7g2h@kh6)JVeL<*W1@L2=~n_y)BO#M?pQ^CEUqC+4m6FIdbHfJww#bXs$vR3{{yoTu^v7yr{##0d#7fLOy4(Nh3rU_|3y_yP%1dv zl4bp_$M^~<)&y(A=klwB5?0b79QdgnOjGo!CNL2h%uRD{b5e*dj=3X!#OS=`EAD(ZNR0(wwH8Q?_g~2hNHZKr%`4)bO31u9AdW~64Y11yHTBwY1&vIM^_|=R z>a0mQ|I{yoWY6S8P0+^D8JCXEI2HT$ToW@hC6x{YLM+Z2IrFo?)8#k`z;;c8StVvl zRDF-%?(xL`yVw&!D4tOc3d?P+&^uh~lt z{C)qBJJeyDlBW_=YYwQvQS=!3do6=|-=hV;ti4sR&AEpu@V|nm*i0@(-)2>6lDMQZ zeHbxi-vt$6V>XQjQr|S;L7@ozgp0H-tU-RO2S_fSw!0VtUGe)Ckiq&V(N4Kd>CsxZ2>~F-lJlrNmSkUnRR2vcBI+kgPlP0e zZ5iAw5u^qQpkT3HrI8XVN-!CSO#!aHf>%wm4Bre=)yVIVSGEhTCIGo3yBloSnd+wg z&wZJb;Yp0xGGf>^ZO;~2|CI8HiZ))P7&s}U!X5bM`;8CrnZ%{F(fg*eWfeccHCA9q zRk|C0W{iNF9(!Z@KHawcXj~>VdJmzXca@El0qs8 zgJB9nED6P)re&5KcEDl_r%aS^BcoEDDUOQRe|POLjgZGV;W>sL)Ay5H!oMi=-{L)! zM7pcXGI=hc{ui1gT@!em^#HR%s;PS(#*!z)Qz_Q?KySoVCL&VK!nE#b&R{hDWh{{e z)8HcLI5+f9W7V{@#gGMCvyCknY)wX$OhBsBmOqPi&JsLVRSCGusGMi`$*GNK<;P_9 z6|swG;G%g6WrS|hFB`NdZTF)Tv3^Vm#){afd{e916Y=FYHc9{urZ3lBm&1=#JwUpgaHlnpoM;OmlQq{IyW@Z^|D4R!kDcC0dNOd8uz%Dpf!Y z0y@%)@Bh^TkW#i)n;hbssBL;2$xx7$Z@C=FbSI%V7x3SGh3c&f-(n3jpP&Bk z`tZS@(aP!e#+fMIwJvI!@uR*encGQ~!{n`WpRf*Jg05N+^}`tH+T4kdA}J{x#l`11 z6vq0eJ=~ELXnP6`BixV0;7t}18bT;=StBK&Ow?3xqR?ZsL&&JCo9NZF@2^5@PZZ_g zLV3$#jy5vAx^#XF70-3J@o{ebVoZ>ekyUl6I#UEcjJ$XG05h=nL+{AH>>nar!A{%m z|JnK!-2cLHoP=*=QW$lpj004rjUm9_@wk^tRbJ&}-pU>14ps?ej(z=eAhvLZpJYSJ zB)|lbx6nO(il3ZD>D=lbvIB6(<13+L38f%xEK=`7i>|dX(P{dQ%Nj~_LT9mRgBR_H zHOWR>Aa4oZ=WAJHhhG!$#IAEBH0#G#5Mkj&f6Ak+#!In+^v@DiIHWZ+uqY{`|GOsN z|9@-!@3xqt06Qc`eo?Y8idvSZGiB1MtB04B*UyESs#c-N8~>GZZncLor!g(G!Eb}M zZW2eQXe&XZr!O0D)ENA`(rXGn>d%rnsA6@_8c=Bow`a?d&hyno zwAl`!H&#CjQ>W4jS;49Z8ccq1)0hfZc{)rY2uc|+=?=EALzVu*`Uz{yIgUgC5;`Tq z&k_?{CVfXQzwYU?`v$j)|J`el4T>rN-|)pqL}qyES}kF(l*zuXl?@WVr8vxz=UjRX zo5-VE73)k-DX@@|wC)IkyWK__#P<*iG%|ta{Y+vvW*eCitYl-@s~TOq69u^<33nGK z7ihc8lnXV$ED4tgcDh>sIj%Ty074Q#j*gD*S9!H3G@*vV;Cjf|9vM0Lk8!1(X^)PM ztfGIYi-L?0{`SPD_D(P4xID?-!>O%HW<&!3QI>=h1 zFZ|@;K*b>5L91Hypz2^EN-s#x0%wk#V1uX)o&80LI-ARdm32@2q80H>*~^>zlVF4I zKi2_$^0&%jGJEN4jp9Fk_(P9dyA>R8C@5^8My{`25&QC`U)L zlCkJDRSXT4z(2m+^8cKr>~|X}oq9_w2xyc7-+05bwQRv$qa>BYr0r6zynxpRdpNyO zuEd3QbJ~BPJNS ztwdD^hjd_vjIi;t4atf>`um1Bjr~k(G71NaJ5gT!IyxB>RTx>?jW@V|Ok}g9*qRKj zgUf6kq+wTJb6#FYCKwJlXsxJ&qdgJ$AT?qt#1i$vX-8{udv>S)OOqEnrmRRnfY4Yv zvsUmwT_epW-|1gXC1K;^mG_QOg;4PD(T_Gu09Sr5n~ReF<4PsAzdFc{6lz8`)Q9x< zyGRd0bv@Il({dhgO}6<2u?(0c?7JnW?hmjucz^O6XN5y5(le_b%t*lXeZ^%|teWNE zr_$tFmz&L>psTQX$UhbbJHjR^DzU-Nz!VyA?k6luFC$f?3IG6>L3mLVUnwa=tvQz0 zC;n}nWKw~tt{#bR_htG)N-^;9tmSV%1IK@{6;oE+Ud3~>yQhjwuC0Ss!ge%L*HgM1 z^9!?-e~6vJt=%t(_wwq#$y0&~d0L>z90#xgYh-XOr$rnfGy7HsS|o#_Aq6|S2Ik)z z{QoG#q#t0hRVZq?W&W31DQNMa0l+;#zk(BVyRf99#GjQV( zKwG=ESU*ur6U~ydSmv6Mv#t0b&iP+Ujtwq2aFjG^6P5JHaGsB9W;av@$Q71YzYU%i zU}9sdYHLeqXcU99A9zVnGVV_KY7$sXk~kl{TQg+7Q>uJ9-5hfcd+sCLAPA6aa;(a?)uT+aDDTmc3?JNcMT0e;rlkjo>d&rAxSxRp z?g6xBlS0Wec)km~2_EuK7duV>?k6#&qXUPFeK53Iim~{#SZ9GyZ#iZ8`gpZ#ZpGb| z_wQO@MgdDXB}Zh`zlJ!1NJAxxgful~-Di^%&<*AiKzfPx?k(Rp6w}8MXZ^t3Kw;&Q zzBt2HeSo@xS9ASXZ)(Oq#tUi!_D9!+GAOt%94Bp*{17 zo4BhnL#qHqtLH^y#z*JLSHMa$mIu#-y5D}GIv3OpW6YpNmA4~{I zNt@-$j}Kjj2V+T!^Rm-wH;3%ku!VCH0VF@hh0S~ckD=#XuVj5M@!IxY}N~uITRcA zu}Cub`QD5nNkL!Ur%eX=of6-1^^hqo1=kTpC@)@x=2l*>Uh99JDo&w;hWEc9!9~V9 zgt~+$DurIPBu6CqdcJ&vn`;Ybb|9A*@(n$3%pBZCAl?t_`{FSB*R$XTO+R4k-JBXW zK+1=e_H3b0vzQ#-9Sdvk_DmQxKXNgHMvO|Rc}?&|tzZ#t1!E}7S6GhrNOwMVPdLq; zM$~n>fL!+Jo`0RW%w$n0a?(w7pqb>l=&*MlB5GMF2C`dz3Zr_jdU5TtM!2x@JuKYO zJB9b%pXXhfdX9e%TymOXpgoZ|K7SwZ+H+gn^KxEv6goW@nXfkAA5*?O>=Lr7g3?)8 zTQ*+{tl6I(`pb45bHdZ?#mP?66`#e7xhv#L+o#59KH;_MHRHYxM>rrkN1bp#X9u1g zRIHzTPxwHXp4XmJwk9$30;Tz1p!iL{7!25R#Gour(FPdndJ@T!9v7)#paD!dS}fnO z;v*$i;!=)0k8s7%StVa}WvHTx;T%xKl4fY5V`2~~Ww^xt15(PQ{|D9orM4NG_FPB9 z$$#nYL6mF0IAiyO?X)?#949cUd&(#O=}4?Qn*70?W~mXD zFuypRI|l^r+!30!Pd2z277Q^?T`m+fEl{-YioSafcoF(t^kvV?*$kBjx`NY&2B(LP zh`_fu&SM2%;HMJhW_|W^aZ_y9lI_NeR%cKyF4hn z0>?UYK2}zE+$JVgb=iXShbCghf{CS!{Di3E zBL?JSO#r6JlAPeWX_(bU_C#G2jjo)K*;HFvG?j(w)so=q>%0ILG$n54ZLT-^LGHJ& zYkm`c)H=oOE(4P7vv+*NUG}iFJ;egTJFK0LHV}Ir*qdbaVAb;N0Xy%n#REhH9eaaI zDrLb3;r*)0{x9vB21lR|lGa<4AF@@MQxICKJ>YqV87@cOY+fV&*1wtICBw^?is5sx5|`nb}QLi52lq;ru>q;Mx&qRI?`SJ zeiKk34|Z)!R0r&91n~Lczdh_G{bZa^SS@?|4-%`m^OTpD)6nWP=t+#6PCh}(H*+}? zFBej{3;`v`is#f)(9=s+7Kq~{0q=l{-AQ}qlaJqaI#dnHatdRensp%*^yCP6H3QkP z9mpUdpx~3y|LltljJ&AyX;c9P)Jp}e%65JQv8hvaEQr$GyOC?mxmLCZB8aZXjuBW)Q zeCrYjMljq7vU1v;93N_VN+3jZ!?;lH%7#hDbbCC~Vk?;zKTG3lvjiEhD0Lz~skOY1 zLX2>V-0US4glfNERJ5LZaEIsk3m#x2q*SQk!aUJuNBS{b}1kUpy8n%XK##rOXZ zh!Y?mSDxA!-dJ~axy;nMDkbZQ0?X8oK<8iY&Q~&K8;>4R7P;bKYFfivGhv*Ui>;<9 z+k#O(-o9{esPr*5=ZS4#{-S@vSy;a&HoaA!d~*K7831 zy3{B0i>5ZC3R+b(;{MP>K;+yCB|ok~g(qRL{Ca!B$n`l^wcNekyP(GYT?FZf$5>?E zqpP1hwR5bRIeeVz_r7`~;FA^a_>e1WR0BL8vTCt>24xhLxK7GK4A0U(o*%({vzn3W zb!90V4(SyGMTw@HUF^i}xHk;D68reQ=w;~z7zeaU|k5tP&)CB$I z^OvBQ$4?#q@b^Hj-06eTzclc#844-|rC3IWYVtCpo%Zcfy}%9ouHsU7VwyN5p$@4q|w_&bd6Yrjz( zWM7*120LcoEp{l3cojUo+=P#nvRqtCx7-BPEZ+VeL~G9OspUYDz%zP(AukU%n(VMr z0wzP>GE=a9a9Fkr=Z*eAA@vvt^bQD$zFJrtZhe8q*Z-2iPcP$BPkX#Bc)pqP$IbhF z;tpI?bD0z@M1W?g1w~UDZFCRS%Vl1J3eP(6_zOj-bGYPbvpQnrFc~b!VdV6l$g8fM zyx*$!Fp2%q&Fc8Ozqc)pa!(2N9Dpa`aTUKz4%kV_KfV6(q9yP_r$&^st9Q$~Y*T_xh#&2L#VSb(gbB6LO5Pvq^@0wO`nE`t zv_FZgevseJ!(Q-PVO(cI?H*tH;@t*xK1??IfAG3rlg6Ckb$>EHBQp%}=gdt07)&j_ zw*#onEO>D2m?_sgp(VDmxBB+b9hYDyPEIB+Ibo8A6&JClyua`HdA)G^8Ti2agEZJ5 zLLX?x_;W!_f|2Kr-}%U#?Aq1rE-H%S&6kUq&vwZjU=%lwrrLBgi_|8iYYNF%`;2#0Ba39D$rOMM(6#9$W{?S%7&Ulmzd#z`{EsSMbd$xT3^E{2 zz{+(ev&_$|11Pt_yK!Wd-v_^Yy# zf|1eTAorA5%9OzlO#8X*kN>CC{#V!iUursVPFV&u(xQ?q^5zeM{+!}`K&y7l&EksZ zGme$*<*JNGA3}NNz&O@BEBcq|priC-V8!S|;;0GMu`8OzFgTGWt=Np(O(w*`MCW^& zs7r3puSGG%`Q8oVaaDBP2dF8*EByMj>QXZ6gyUF|1`M>hIubX~Q1sV8T~W@^7`l5h zhM@}@29OcMsTl_k#BAM;(skVaXf%ecjI9u#Rq!5R-`q*FzTbGd{?q;fP_aFC1LbyU z2&Kwwi%+)UoJmP=p9w#qIjCw2wt$k-zBuEz|LdKDYi;%+x{c1&P+blHeXBu>CzG12 zrw5PLVEh<1MKFMCh|{$K=&&faNEcxw#}P;WhoedyJ}ELWgYi$d{icvzc*fu1LFT6^ z0(qX-?`5YeXppiMKlh9#Oangpet*Jy%Cutcii$^I-pi#QT>j!~R%rKhx!3$_j4zD# z$Lu`nlczO6nXO5{L?II<4@lxKQHyGC?%+*su?I?6$5Gk7z zy<6(7gGdSa`ujGl0{ZsN+LD^@q&qi~MiYKZawx)kBy+#ppRan+(RV%tD+|9zywvz) z;@k_z+f8Lk-0!0YAp0JHl$)tcDI{oLo`nV8jt>|NwUiiz0EOs2VXR&w=OtScXG+-7 zO8`4jU<#BSMJ9%{T5a?}p~hmQ^-Rao8Fle$@szK0N{s#}0fE*JX8 zS}p$L5(dtx!KB^U0MznBs9Jb_hOmAhQ{U{G_gvlZ#6)-L4@PbHZ4(l)ua)?nzK77n zZ%zDa@2*%cSTHPFT_ZUU0d_SwO4OR8kN&8KVaOj->)(c?Rrdp51?&B}mlT;PNI#>? z9TG`Y!Y+bxJMw&T4`_XIy~o&pe)A`< zaeRRWc(1poG2V=ids$M~vrip>-{0FtBoyXLAP{%nwfn7*`w?@e(wDq}dqS`kZwU2v zkc9GI^cLEljFGdEu>fgkmV2=pO$Z3QzLdkmf9FhXfAyOsfCENpX7_X@8)>Ee_-vaV zI59d(8l#vXs;a6Uk>xDdoP%}3lB~cqYY__XfH%W>WS5Wtiv}BZyLgM5dimV9pz`1pcSkeQHt-I2fWU_w9wm|!Jy>`Et0?*b7c_>64<>k9xHXx!zbYaaJ`PL z4LbvgjT{8y*iVG5M8DBV=dpYOL4Fn)5ZS`=a+NH)N$E)p#{1Va zn7ED0;X%>l+i7_JoDum49a`Q(G*45lcMd;Y zNd%9rJgMPzy}ujO=QB404&))n+MflT@XEC&LcFcaOM|Eb1jn+cGqsbarwnZ9<0jG; zMHkIitkZu~$b-FO!m!^W2>XcMrgzTB;xoKs1v7kZ#8PHzsg*%^wL8*L<@opEfd^BVe{hHK65^IpCci7t82t`j z>k$mK>a_$*R^?CEQ=Kt$yU}F%pIq1F0^U6__IspfYh?Y0_gNAIyg77278@J!0pS02 zUH3lZz)}5uhgnCFl~+N~jjrhVj;uz@$cLIM!l7-v97%UcZ@df>DE3s&nG$}#`YK|g zfY&=sks5mw{VUR5H-8tVbAwThMFJ7T?O$d-*M9Dcw>uQ}^VN9ck|Bdq=Iq%lDxJb(Z7j72K>d(_tg(s~w^#Hai{Cg?M?BSDo;y`;H6eg*Vd4 zpBxG9(Rop47A=FymfD4%<=<(ue|CM>BL3Oqc97eDn9U=0&Xy7!86$f(ga#2SaJRqH zZZ4R)g@^r`<+=UmAwKf9mjM`hi?I`(?^|!0^t0(^tx9kyQ@p7QvPW2SG>J_m)jFOi0`xfvPc~7 z2eUp4DLEioso1lxblh*tpmRlAayUgS&ydaow`g1K>Oz0qZmMgt-N*~T%zwmrF2U4x zY92;``yCb384xAC&v5X7&K)4)y6?6-BAw7;BXkqEEYqsoyL3Rq^!$QFxJ1Kj_G?$% zv3;MCVL@POO`0-PS|UX5@p91av8$ipo5?e--;LgqQpcn;#rm|;I%un=J;$Gb-3dP! z309+ObL{{;_|ZbIOry&DM1MW@|7rnl#|p4zqV3J{y_u%`Pfhw$Mq-YzlpoU2@8e># zxLED}GJp#`8N6;(=>|TuPft%s2W!9Pd*yDZ*CykOxrg!=qo)jgq7Htu*)B`WlEDOF zmgbZaZyJvA5^}doKMH7=8b7970SQE#s7*>E1`xE^UmttS6`wK8g#AMf&a-4U$6d@J zx!5jJ9~M&s1Okcn#Mrt+>QN4Id<_pK!{4&~=RW~r?ot*g*j&X7?d=CKpZ9dV1CI#W zxlcLZ8ULK7K|4PVtH-3R6Z+~i<#pUsFw;?mt>vYs$lP#U*eHDtK;eqizLZmk?+LH~HljYZ}!WIHh> z;4h^5bQZ0Zv^z7xmt0(;9BY+P|@n+wXK17@??evr|DoWGK z9rs_rLnU;+|4XZ*p&_ZMi48uXPc%|6hMZVg0K7S+lPeW8#P^N<81 zSJ zjejEkgaD`W12@wIehQ~U))w4V;G2NULc7$;O1fR%8+|U$x`kC& zbnCYi3`{J&EggM6OEi=TpRC3o%*7v-!)e}~RBQz^70 zjcEyw+SLdQqu|QmI@1Xb;Uoo~6RqNZpx5}{v!VFof3gQ+D-}P??8u}RNr`fylY`2| z+4wBiLQ4Ws^G(;pgq}9V_nB7YSj=#T_UV3w{L-(F!2;Dk&fP7rbz>EBE&ebsiqUn3 z3~cj+A5ns6%<44#TWRI}wNV%)FBQyamU>&D@h2%ATr40`2&s$CSk-?!*m`l^7wSG* zk`gPvX>odH`1_FEJHQ=Fl?>1fxPs1NIliLR81p{OL#bytqG4*g04P_Xx2bDDR9_dys91i(GwJ-F6AOFD||M`Is zCs|WliFhz0Cf(a<#>X~}FkJ1+pN_QR6^d)+bLngfGg}9Kg0ms2>W*&g{naa@bsB=< z5v}#{gxyaA!U4T@C-}>u>KLw9Rs!BTcKv%KgICl!!znHuF2uW|yCsTQ-p@y+pD&;M zJ_+32&guK#{kc7wgZWw_pHKg;7{;35xtJOA?Ia+Lm&#?(*-D%{+`Q=Gn-`m4fajwC zXY-aosxU2sjP7%G?-K?W%S#T94$t%F$zi;qM)vBx#!RQHQHYyMS(jTvf9#LWQ}ySS zUimnZcdPSO+DIKIzaI97DbXH<>uEO{+;kj*pO{0n^3|5t$HZcT= zQL1bSF1tE00SBKuxk(0GAP6Vir+$S*^l5ZOOj!oXwW4%On4YUBy}v+Z^A5*Rh;F!j zj0J96-2~=!4*wv5>NzaDfcr3i<)wDrN!=58xgT4-aae@2uBA@m=0dB7T!gtQ6^`93 z;5OP%HIicAx$IbbV2cFTj_#LnGVf*x1TH-UOLh%+Wxi8LH#lBic{y=`QFq{ah=t;E zQ>K&iAXc7Z{Ys#(p)oMk^|}mP1CQ>iU2$N&@zBavDm|>lfQ9_hBRaAU?j5cIn>Q6c zFw5dIW+qnv^u{6g#V2SbBRJdUCu>u0Wk`idpS|@j+J6}UXrBZ41_O=6qL0e0aIQZ#BFjM&b3r^0y?pM?Xq!9yy z#v$UO0fYngy~pIMzs*%Xn!cA$I7ZIBaMWOYrRyRK--a9Ny%v!K67f%5zN7HMC6D16Xo>y zkeAmRxTLn~W&dTM<}JsD#L{_geNkS{Dod5w>B=Hy($xbr&tII{OhaWMLmRr2|7<&Q$bu z&?`gk*%PXpBT1}T%?}~($D{CKOk~2~AvK(vZ;B3#^qQ6ZwvF3&K6t_b0@(GDB46{H zf3FZ>RvGqm_lU138dpEeU%kyr60%)I-0N%#Irm0V!=(fNgWMt}TOzBDUiCS(hBY{n zsxtEO+^Hae6U?3eqXVVoH z_2xxCT+(ZZQK#El-YW;wDU#5U;h3o+Gy=Z_Q30@_>V5^NJ^6C2Q!>A~a}#=c{#NN- zJ^5i`Vjf=k2oQT-tSf@qxW>jtb1SQhQo*)Bv^!F8PHOy--#QVh2XL;f-CTJJw0>9O zukGRt2clA-FtwG71042o@15Qfq;sD$t{WotZw~W#+rueMVc{1rW0fQcPhOE~U&6gI z-zL9AMU9p9MKR`!UHH6`LP@$!N-c;rS|sd*U|DcNVzCd{$&tFjKOu8&Gf4#^^LzW- z8~n!bAsqOm^#fuugEu?q>Z0CnPpN(9=I5Z5BGuBDl`&gbx+~m-+S2}@?SgWl2-Elx zdWHQ04B5ayrkjNYRzI19!kUG``;HV7xO|$t%u={~ud#8CF%ag^el=A|>73-61Kp>F(|>rBg~eHb{4O zcS?hFcS$!$_g#3-`EESde?Guod(Am|yzdy61J}njMtSx}TqD}{6m$SX;0wXBk6Y&J zvW@vRj(63G`;Cim`Qo6w>48kUVHFD5ve4ZMj`r2GVIzy%IkC^nJr<>0TF8*euf0po zIFbo1GF*J8md zkr$_-cS?Gd%I$4nvMJl>_Q%~>Z$0xzBW(Sg8#C;(7Met}2><`DYfH_xG{uP?8oqfxPLPvj)mx&3&()_jy)bpI<1pF0#Lv1TVLQ76St#X_R4y z@uY2^QD010m9)EsCAlk($>f~Hyw-ZQapc*AQU3YJHe;Buui9L1@E~`K%qWIa-%Yav zJc}MT%2OzBB---6sE~FN4kOm31dS%ANSp@}4@z8>C3aEpw_9tvk`7 zteHSVOH0bc)fhmw|YxI8P@V2qEGn@ioGXiH67G?`+H}8q*B}@EpNzuVW+Rz^MWMbQn|^>0$++6 zW$rd->q8JJsR=h`s6%9wgVrocw!-w-Ow``#pyww+HWY8A7%KUc;vkjIeHV zj0TGZGW~nKc8xq|ZgMBD1Wf#nT;uT>gH3I2MYK5(RVACI)Lm93Tm4S=wsF|0nJ6Id zViW`p_*(7hLw99_;ph^VEh2X{40u&FxDQjS3Z^LEaL7UZhOP>VicxeVj^k`_H_6_h zRJ-~m>Qz+{B_*`59OB_s`4c>O&A3ISUBl-DG!yC1D4@Q>QkY*>^+v48G34;k1GxhxtjSmNDb`A0azvN_Mj4>3 z+^%ab{PhcOI%7MV&Nsie%G$sru@n-T9q4-&Dj*f~gX4RxV&NLVo&rc1#5bXWi-tul zDN)kY97^G^2FwYNq7>$SS+Zt#RQyEtQ<5d3|KNu#4J7yB$_qXNDC^<+TlQogzj~dqleg(9C^t zip*j7t>~6)!KHQ|F6N}q{yvbcZ^Yr7L~}>_>bQ|YtI<$WK0C7=so|Bk!e0dNY@%Rw zdVLz$9%^Tn*j?UokqQMS{gGn|KEpGO;`E&#u^P?WO789By@cd8kCOuh*?Q*1H%Ph0QfqFO06+Fk4B2y5!K9I;LKA-56%|Xy zzE6LGE|Z;}hTM*hkVR#9OG;`Cdtf-9Z}6^R-B$gh#K7b>oOtb?*Ev~PS<@ktg3ncf zBt7B{#V6{Q0$X=+a(!pFTr=)7qM#5|4|H8P{{VH;Pu~zXJIhBq?agkE6H{c`ufX94 zv!&|W!8-j0F)YcWUVk?|@x4c8ILi(Dm;_mzu7D9OS`H4M;O-RBpMB0D*z{gPf8w7Mws>Y*=Q%S}(tJ?IBOF&ZOH*@+sOPap&a24}!I-(|shN zf2sy$=;Y*~q^+Ghul0=^9}%BBMu=F*)RaOXQ{MG#s3u}~r`v$ppmvctStdz_5;%Jr zpu_CU&zMNJ>+S699Bxa11Pj3H7fxINuEc)|(?E<~Tv-XQP5=r7ptk@Mkr{g?NTOU; zSC@c;g9F$K5to@+$;ZnFp-A*504+?_6AA)M zt9PXRCKzb(01+XLv$J8gy!6+SkBoDoY>C5SQc?)O+Kc$aL}g7)kd6*MXR55exRw?! zzsFTpMMYFg3$KahT%S;60+eLpFvie1N_5ZWFT2Lvk`l%c-gLRQ9hJq!Bq(qOjmsbz znO+NaG(^tPW0aG4cHBrnX@H|+PF*(tHM>R}C<$E4ehOS;p3sC)A+X5MpHY@v)p`N7 zm@`i)-x7Da2(eIdwe?kPVrU|Y2l{85UOV^giXe;U&9IKwg&4pS6koD{%|}OF!cZZR zMBt5~jN)@#TVWckG0eOvS2I2^5SsbVTM}db==nZ>G|NUM3GM|B8OT?F8wMm8Odgw( zB4uVqX=-X3A0Mxvtu1Y2MC$112zXyZc3ekpd~$Mf;^CSMUR7|OSy0p6or8xO5WWYA zyn}*x?@hd8-}xN8d;*}@-y$e4ug}0KO&p2A1PFWIr@X__jpvS0_t#r0I^wjwPE zV2HeBY2T2$_s*@-uZk8EADR3Tg!IaV5M4+VX+nw4hRJ|{(J>h7$ z#bD^mT-Igy&$UuEt=g^FH9ANmAZY~>)1Vwv0mNnv?K&`vF)|4XBBvy8EFOU-SQvVd zn5#J%B9NM9p_1LT%`y`Q5k^IY{G{OXp$0o{M48?-9gn*^EWO&Btc(OoH1Ud2WxrW0 zC?P;nQ@QN>Ef8I~EJZTGXu`0n|39e9EF-HgJr5jJ)> zs;a6F)(q*T31J*uTv&zEAZBb)Mf9-naKOfd_xZ1%4OrFp(<^_X&Q%_>iUI|633++7 zWjxfkPp-uL-<6uxc|gFS{G+INBTG5&L|2p%kYN7vl7BBhXw=;CCD3dA^6M5Rr_2vr zjV@}zlf`g{fll}9AIP04BGzwODV8!77Y1?MDTCgbw&-x%^x0q+N55E}3rDr#z+mfCuHNJ2nR0jS(NKvJ_x zN?!08c~E`kcBieBDJd!YXZ#~DiF1wFuo1b_HUsHYSfNZZ%lE6bfMv#LvY&rjWTo=gu z2*=660C&QzF)3b+@SoxXk_*tXD=IAwFK#Xh^_r~cX+ZJrjE#>M1tjc~Fsq>8XGvDx z))-15tLT0TMRE`+A73i4ROje)aNmYSD{ZN^%K)$>afyi%&d&9W;*{j%Ri3w}f(<<= z|4Il#C`SqCmO9X@1c0_fJ)(lZLrnV;KEj>b;q%V_`@1d!6jfDT?wqk8au7D6cm!qKTCq86@-4Iayu^Q> z|E_jXh8lgUPtrnw!TRbbp@7ToRw7hS{%a;j)TmMQhEp@3yDXRFR8(6RZW`?#92XZC zK%ete6@Rk6I}nBD?fUxx! zFq!kxs-_gn4Yt`86_~0A8964=gGD3lfaO8MB9iyg`OMi$!0qPK8T6kZ1eOnYn$21p z52Y^MglJZT&zyx^)=Fj_`j1JAUK>thX6%%7CN8eTbUs(3daY^=dRsJd4z9fEgPbNe zJn2Nnmo2=1@@XgQHX&6n5F-XG?mHDJ#2^Q_L2YH|0bPXQXIRW2~Z4f z`-IhRX;ccO$U!g|ssxvjmq{9Jh{iJhgfYLOA-KtH?>n;2zw@H@?|P@*_%h}v958e= zTbSF!LtJFFTv=kn$>)vmQ0@4v_1oo)l!wArP<-wlz-qYMzIOigSsyd)<;$j6cngR{=}dnYU1L+xExTCp(&c! zEEK&0h5~%2_-EePFJcIS@q@Q{-WBZ$YIF_B0BXSn=StG;+gaG_p$bmUFElmc9w`%CghHDt_a1etz^9>{AVh=Cx zmsUv@-Q2$m`2pcYn_eZOIFOY*`E;%UFM?DHEKWZL%$@-=wnnQ<`Dsbu`I#?J<>ar| zQ=GJ#G^Br17MCwyB``6OreHhZpmZbUj$#m@12jf3A>Ip?uwW>x#gyE%hcsnS#l+0MWZ!=;Q1HDFFVY;S020F}(#{5rec zy7stue1?LMeZ=4GlxI^Bv(aO zQJqRHDtd5b_-Cy)eoar=J1}EMI6{k(4gVE>1Zp||?qS2A*Q}PK7!T(p-bP1|6*r&@ zg)OAHq5?@w(MTuG7bkTLHk#{1#>-2LaG?#i+<>1&&BeIIT;i*^cLCTvvla+ z2#uU2+k^8?+a`{_OQE2q;6a%K_IwEouySQ(zjRe)snO8~)bUtwBe~NV(ZJwXQ^$f; zx_GICPL|Z5sOh`mq2?rB-V8=@gJqoz&#zR*l}&+)jE=_y3M%!R5`1AbHLREndLx1G zpCmzYc0L^e*#C)jyShZZz4?Lgr%bP%ms0E~<4{7p(0O*^bks`BzC+=ESO9Bl$7Y>J z{J8m}{F)lH%K7D8caQ7CpnnixIo?6^KkguYus$RGq zE>yQC{ju$gn`VozeNeUGxG{ChybBhYd>L(f7}vh1 zJhK1%tw*CJr2uQ8JuPUxm8OZ?%T}RzU$}t3K9$CGeRQ!|WNkHb|2jRFnvQPk@vM`I zkul12IDy6GSOW~u2w@*98yx;CL^%x&34oJzIaw0Z(Me;Jf@qPhVQ|jR?fb1@akd%^ z*xIl#gu|nwTMs?B4`5pg8k*3$I`*)zFhGu|!$bi8%cWlUre#pJ6lsiYTCOq(ib@m7 z2f%P;HBpp_w67_-a^Rem*i9S@w?ms%0}`u-3Zwgy^@W3*TnI`}G#r4)!bXf^`grSA ztFk*WJypfUAV0xBxf0!SNvfnRBE5ra7kEPstaA2LRMZQeG{}+{7JP0}LeF1(pqymX z-Pap1G-jy=rO%rcES?}CJlvSCrp&6S#mB{`)x1KkA{Skky6-N%{;l^ybp7lB4XHKO zJ9B+*zZ3A|bZa2=_QI`>m_{3P5G$+Z@$~Yzss4Pd^MF~q{t~PoZVmUo0e~c?e?wJI zUm}ud4b4KMH0RY^qy(3zt7@uVV2y8e9GRVOk;q@})_vE|wklD%xGcTBoGAUUsANv+ zJQ%9uJ*>}7xtzl^+`982qwNewKvPrvSM0#%p8Qt9*B_g_3IR#D>5od_IG)o5V{Kcq z|G5i%AT6=sOx3fluD>daOpnEl?32<=0K!8{C5f7g=VZ#y35kh7%&}yjyA(nW0AnOt^5xz#s`REK0KkOUT%L|E101VAk{b?)lGt$uu-6IQ)GV|-E*n=|3LO>H*!n|%94OhCA_VQGBMFzwvz@W>c5#%Aw1=)GQ8PVZ7h z4(YgyDLwJC#|H2ZPe?px3zmn`5n}3laN` z@8zmj>H*m!J7|7siVRmX!T3W_&wRKM*qZ$oaq@9T@#^Ywh1_swDi0bc@#_1wng zJy1XJ%+UN77d#7n!&To*K>sGr+er-ia+2dPDrtlcI#E)aOmj&eBPmvBtPS)4WAmAz zxTmC&vGG0OBV+>=h*Yt&4xAU6v5l%YB#LRM>?PYWosJaQRx5ZZOtjh$*t}Ws#N zMbFPv2^BQiHFG~82Irt^9uSQ*UJ;MeOMdW^I*6`%T5O7ij~n*!JW)QWMFNuaB>W^m zN_v+U~H0pW9xeMA41fZ&S zIX)Rb++Wp~xHU3t%mUy9)k!-D7_4?=o=7^zRH=^kpeV$}a+nm# zQm(bqxTu#P_?0i3X+hi;F7HmKKi!E(J!7I$)7*j(6g{Hn- za)e^74PP1Gs_uVK+#u;A5W0qG1G2+exF z!t_sXsEUe;f|AnIq#^uU@J9?Zl6V<_kUD}o+Hfx|C;zX_`8WqEY))X=SWsXeM)Y?j zPTLCNT3^-#v&Vu@*dDzWtRsFVq+@U;u6)BCTH@6V(S}<;P_arj8h@51y+&q&1`tUW zi~dy7=dT|+L%~VAd{?U+Q zQ#0&NTnzB8=?W>1LRO+5Vn_3>B3-UsoKrrL<`!~bn&R=%e9?8%FPbTTiK#4k__|pQ ziKEDue3jafgg0?3* z_Ph+ez38nn(cPQbtWusbttWA!`vn97x@&)XW>}{j}ggf5(m)Oxeb7TDZCa)=|=-wOp zsPvNiEcnyD?RD+ZJaQ8Mo)4j{r~fN{azsQX!`WI_xqSlbMxb<)r3UYA3hA1U6B1p6 zn?k?kHS(;W8;&9I*7m^N20G4A=l8-4`*!E5V|5?KL~3p);;})Rfq0$-78!$Njoo9Kr)l|~}) z=mbgUdb3l7uk5B57k)*ELyqGYAp@4x|9f9DYl9Ob=kthhvShyxf%>l6(v<^Vi<=z! zBnY>eXU1xRlTxcn1&=2HX+~eU=t*Mj)dF&nDx+^<`F7Tc)_^{*mUmcAZgy=&21r=+Jt*<2w1V5lgF!u%L%$04N z$h*Y2c5Ueh@DM;w2+j|-?6_WxPU)t(d8p;(hx;q$VuMkg@q2F#kKImW1&VjV?=K=V z7f&-qrO4@xMUb7oew|cbE{Yy#JovJvWoxn`_|nyv+r7;5O^9~Zw>8dnuWTo&^W&T~ zA>|F;(0TTSBw}`_H{|fyb1acdhWvWlek9l-swE6h-X}>yKXWWUbC+$i$T*4jUg3cv zHO9hr9ReFfAn<)=u=T}VT#d0QfWw*&?t0i}M>?`QV{`+xXWtkG##Hh0^17|rfy`Ng z{L`N1sP@@w)aBr-!zFY;m9*dZAn6f)30bW+0^^_Rc_ypn7OgMfWv>dj7FVUn0Dp4n zp3mY`$#O+><-J6Il@xb_0tuV0z6f=v#D}H-JT61(_5m`dfS#M3vv%ct%YB9aNi3?Y z$?<$8(OdGO)2uXJGnIl;9QqTt%b;p#L*iZKyyv@yoc9c~hr6Sxi*4>~?|}N?Taovg zB{f&S)5q(2R$US-3Y4|6_HzM@8AMl?_%{UglXjsA#cx)^N?iSx&SVrA=+Hq01UoIR zHOejI3?_)YX`eon&@46tjdnhd%v-BX8t+5(V>W%$6)MferWCrazzrEEo1LmkchZ3= z-gQ2vFQ#~J&gZ9W0Lqm2nZ7&NN)!RY}iQaFCi4I1CBPhD)u_k82mWuDD#pke``+W z2`09v5kBF5V9;{`P)xS)u6irn!iBM{(GV32@&TsSi@1I@i}QaA7ptf7DeOc{CO^}5 zoUC6g9p7r!_`v?ugG34^f?)Kw6_5=sTC2;j@?dSS?{SfeT8lWbHFznwR)K<@&K>eX zOPKi#){xcIg|o=3!9~ONStA(vZ21A80cTcrr1f3h>Y2~w|-iLX4 zTa`<9``u50sQ)PteUpgjchK)S-|PVzQH6DN@l8$Kx(*Q?>sCV~#glg}+4Y_rkq-|K zf=$rXGcz+)Kv7>e0t#T8ts3J4X#r4O+8s-m21>{^^uUbl|MY~&LS3Z8*em>_@@k7z zWkV`RifIwRuh4j+11*aQqKfx!Ze{8h1z0Hov*UR$TO=(H+f; z(kgu^79YySp&8?X0^?MqGdNvrL1ipq>xoo zk#9x&sfT}FCWF5Bm|A^U`83l;xVW-ldS?L0{5oXj>SxMi_N?0Y7h0ji>ZePhff&MP z0!d7?Po^o#vnq(_dX7};h*jd_es9z+3m2Itd$P(gw=||dlwa?!)nhZMYdyPiS6)K} zJX0q+r{)?8c@V_`qM{m-^cOG2+^F^tMP<32M68!C-P+8J@J&~?hCc67g^ms1mFq*b z;Eb=(uY@U~9jlGM@KL*oX~HcB-nkNmI`DAP~q97$L@zd40O-867nMG(7=~JkXsStce&k z%dM!WP*uIO=jeYFdJXB%(N-f@FQrE@>B|A z{8V>lGpkYe^z&oRq*z;+i*Zg4H7&So_FZI!H5G?rHv*I|x~ig)RSpLTo`8QNJKt24 zzcf<7-m=*f!}vqxGV8iPCD(6M(6(1;VP6y)_4Ub-mA}a|%*9#nQ1Tn;5p)~Yl8x%-}fWQ{`Ip9WJYL8k)OlsAFI-> zd+wAs-8S?@5GAq-tVODs64omD&_$OqLb_Z{9t{QHQpDnDF=TgQ3(LJL`Z{bY0n^+IcjPt%R4~8=I@_TT`bN9@Mm`T zI*m_TzBbwYdZT{Ds1r7)Hhq;d`XïW?1mF7iBwkt5i#+K3Ry#(b!B{_3?6XQKd zVnXzlw@1J`MU#y%_w=fVT9S2xhM_y+m&UOZ{XNy!T|ej%uVY~)CV^x_c#g7%ZHnFbn?Eblum;BVeJ5{Z zuHe_@_^yrajJ$rFi$r^at~8x|6;@Q0^={Rz8A%)_=qj>G;UxIPKd( zdC~2e>|?X*(<1b@=J18B`C-`b^WT^Bh??7-vP*>f{fpM*i@(=tv1hx>=H@jNJ5Y8> zF@X`HzYPZgnWiq7nlLu9Ymcb6fIJM&r zB7-F1!ru^G0J6*8ml+!!=^yIrn@7%c6GCI&hS1~?^ic3aNUXeYC^>25x0&IO9Z*2amFrvAa-+veks?3?Wc~X0u=*5Wy z!c;pe3}?ZkP%K-=EPh{p+?fJSo7$K8oPr0vfd1D@V%5DBh6^{ea&~>^6$YNjLZTAYxG*hQeqitoC zgve)j{`^5Snthf&5MvZl#qVQE3 z_9sv9N%NhCcGJ7_Ge1j_hWoWm+&m8V=cC{MKWiohy%qlpjIvz^lgHu z9hbz7?Fhui&ZaE%qhW|i|8K5O)cW!{Icxw^d%E=~)A<-|w3B&x7#kJxC*tABV(Z!G zr`F;>mB@Z<0S*nSpLonyhQDMyy5H3=C+)JteBLLwOTSPL9uQlByvS{SsrEH&+ruI3 zxc3&h(LrkeLt-buapb4d%YjK=T2gX%pQxW&hsS4z=+eqyCLm1ze>4W@v+%hb|1zpp zP*=wy=nP$6xch{GT`!>|uG;|g+ z%k-cWW*j@FgI5BXz@jH-LjAMBKSe_Upq1C;<)x1RqQ*p4ZZ75e` zPF91Bm?(J8uuLC~?ifQdK3>EwB7>ovAej^={4g{t$&af=TRPIdD>DS}C>xE5l{-ZG zS33=R_680zABUKl?o0MqbRQvH2~HtyT~|B~awX@-|BS)M)+L=1x!qOX);eAJ0=)zX zm-ZXi0`H_Q=kb-_y@BT~ZU0Y_jJdHRb#Fzq{cg(qbn;8LEMooa<*;`mi}Aes=lfq5 z9?v56yPl`_myjxHuTZxHnS^qRhxb80D%!#ss9vLgj9X}qShS6POowCzL)F-9437(% zJ_Ux_F746z+=s^I4m1{*3pr#dSSL8n|3p^y?PD9^X00AgL^(Z_N?_3J-IS2;KlpaC zD)s;6r{oG$Jze!EA;2PaZ54;#w-q!b0K;VgV8De(djydnVfdmbG0Md9!U5WxhzZ(Xh6Lj$0804nm@1AyR}_5^>FNoEz3hsI-Yv0JDzjJ{A+Qv+BH zLqHO zeEG4Cq@<(>+KmhZ94V`4SB?T!UnIAcw%C!ys@83Zvr6ggp?oQ+6-HSIUZW|DsG>L& z)FNvQ)AzF0v)kK&9Zwx%Aqbqr?PssgG28vO9j;wo1CNjQUQd^pPnU-_3T)a+KQX5? zWO|B{n_F&?X9%QV#*0^n;h++N4N<&otO$xy3N~Mt9@fr%?IA-Bh6hhMG0U~T2s`6y z`>sG2ZyjLR&edg6o;SX_3(z)>?nPRj)mWS`p{sI-AXWMwa+i3$?96Fprp%VF)u#Y6 zsb2rirrkQ7xT3VTCs~pGY$;4#s9L;veH~?&ecDoZD$mkhMFd&yl`J;zvvqKRLDJv0 zN*22Q@=V`ZEa})+l*xw|ApjQ`5)!g-b^}?j z$*bE(q;P0UUBZ4c#XAjWGDYcsspk7OCY>xFW=Wu$pbA2CQ4!^OT-*#l?N|qX^#WuR2 z)^_Sh`!vIg#w>VZW39WxQ&Dp0d%o9Ras15e$sNp}=h|t-Gl4i%Gzb?(#E}9RC4{40*O?mVH!`2+GQkPP-8U32bV$Q81kYx)VIM23bI-?EB|ii*gd+komFfA(Y6 znD-vLzyd=-7p!QvAt;<*6)k!BuL*YRm+}*R8qHhb zs`R_lbjN!i(!D|@803hG`@Ivg*GK!iAo5@rC+LJT{g~>;oGLG}bz9DmPG=X}`V@Ah*8a7G2roA)tbFa8#a9Ht)3a1>*)@t*y z6XEQI(UC{LLy4Bti=Gjpi1o7mhXvq|gdFRv)3{`_W+XW8^h_m~)^imiL94NuG|_hw z9M_wY46j{A?vz?M{Ve3{WWlRI4DVSAd#I#W4O=*k!CG7PCA8$3ojOf+TzQul%&bEj$JF7P}%?3dw>v?nRK5-~hj9q@IB;Cs>bVD8NT!JiePGpRt(_`x3f0GI=HE zpw!?2`_pfejhRgOU^lq=tbKGomTWMjTDiuS3+4MJ08rhGb&!%l7w_#V@H)(2$ar>uPG^ z0MHk;R@KDep!uWn*@{^u^(KqP&O*q{8-Qooc-)HdKtW^?(f7agL=lTLi_5kG`90F$2v#@SYP9 zw^8k_IO0{?xAmFVzK*uhR*4mY`F&`W$8bS8?Ke8=RLgfB5A(-Genm4~GA$R4GRK&^ zzNz2f@OCB(0N849T}48GoN6O=FTT7qr68#KRrmp1wBSRe$O zFR38?F1B;8vGlbsf-CcnHh%9le*ojaP2qgAtCRJJ^~l|m;OFaNtPtYr^`UP>U+L4+ zJeT!hJN30?-&q=6zK*T#T~@6n(vs)5JMpHQd^_k*S@|g{CKfwGlP?QvS73>TcT{n^ zMfdx*%6bn;X1ep_=DyAL;gUxOphh(ERphIG5f$AI0a&FI`O=BM9F;XRu#qHAT<%~? z-O7M}Z0I%?rg`}QJQN*$dm6yE0qY8mbjD&~V&4J0*J*YDx&8gN_3NG-;A^U@tADiX zprXbqV6613&VrrDML1*XmQSvZR}-If`4;Lf%u9}%tmKv$Q%8e`F~kRYMbY^3@o(OT z4t6}%BHxZtejK4*EtHyiT}ZJy;@y>ATJ`2JJtRnDbzWCr5L}VL^JQ>Fj4s!0wmr2? zK9s@j=J6%$=5a2ov`i7b#vdm-BhsI5y{N39`&q4gW_~>5eM*)vGF`E~Hht)pVOU5^l7#`%acP7 z!8Nnm&2PM+(elLA%BI?HcqNqKsDBGan~ z36U(CwgK@dk>E-C`@(Y2@chi%o=MQdK(!>ZQkP6Iz%@w04jx+jJeKmjZ;wG>p}Qp1 zbrLeFK#*sK>#1FbD?a*IXJ}+JAbtX~08?P89F$k*2~cLDLP6xtQt;{7v$DU=@2k#~ zcN`=+I*GhEenL7KXgAuy zb)tFR#g?;&l_6!8!Y#KIOUNG|l#fz_pNg#wNv;Mw?8O!Ktx{k(ZC;koe-xX^<{ct` z`{?UNGlkTwW7wo)xNyQDtPA(#yQ1QI>@r~ZJa#$m?F21A$HRO?EC)QXzS_ST3`V;- zUi@UCf9TWd2g#Hx(9(^ zM~6{8>+du$?MO*U#M07o!v~Pun{EVBHnv)49lC4GDGV?vY2@euLT*#jg71#iM$)|C z{ES(xVbaSSU%{rmh7|`xeT859c4Ok)k+WX%4Qs=~<_e3gTU=W-83s41!JJNzO}!n5 ztPKYhqD_HwjlmfR@)I>JJt;kX8_v9%Xr8Uo&phDl&D27Y04%nR4LxTn1Bwa|O(6CF z_-xp2u_OD^?g_g_V{GwJv1qdmfvho{nVp>-mDz~PV!Ef=Zdm{}w-0QN|C|QAPn({7 zmaa9hz@e|NFY-l;6fm=54)u9UO;X;}G3c=_6b$?Uv(0qaF2|B3?Q89d+N19)SIutK zVR(*T9PBc0L@MQc^WD{(LXRIiZoDfgySld5N`Iy!QI)E_2#Gh^jn-t4lG>)5fdKbt zy?5;~G&~H9zb~IFlWbiD?n9I~3Cha*1R&}FCcJj^@%s|69j+k~;A5ONj`Z6Mw^Sx2 zCH-2FJZb`NLBgd`%uFWR9@9DLe6JL@Sd#T7DN{Lu* z)!=Jh-#cnIoIN(2u%VBy&&}o)RrwLAm;XAtp30S|G}Ds?LSB$6a9dL;Ktm1{6;)VF z3_2(%2xut7K!8d~r>r&+Ud|_WoHomoxxBN8SvWZx=5B#sc=zpWjo(3A88#N!a+Z{Y zjgKUebvSkK4V;;&Z>BmA^5MUK_QEw&%vu+pUUBwk60|FI0qnfz8yVpX+0v$`Bt8QSwlgn|$%EVMT=k*)- z&RUzmC50U4FM^d4p9=%Sf&YX^)>bcdXi^CQ&(!)@l$*eMb!bCP$-$(p>o7^;$CZ(pU3{pfIV zWkr>tofeSwk3gadk{>UE$G9C+qGw>pZ)?MET#f`dL(GT(xdt$50OtyzSpg&bfrtFP zyaX~{XkviG@}mbeNen(FB?UnUKpqfAQ=KmWUrXx}|A>s2H)Vg{0x9r0oA>Z>cjsAdv=?kcmIh>mpeD){upX)mi(@n~@k=94oL=`xucg?1 z(1k7(W%=_GfR^MDz*T^4_CosD1=9Y7%hQ*ORXfrd|4TQS6ev%?SGSOi;bE&0&I*g%dPlKSX1 zN0r!p*(W;Vt_i|1ExR}2B0szwmvdaC{R@d@9p!M;4co-N>$BmC+bp3e>)7i&L%QyRb|=sy^KOz{<9Ppxo$ zUXO(Z41;ab_)&fpb0#q1rIyZKJ)H{o=4T4{_XSlH86#uI96a#3{RMBz3U3Pgfi&Rf z&!5#ci>z@$TAj@@fb8~S3atmN)0GYqxKIRA^1B=mI zL?jXbfN!~KKAKy1ij0I^zHG~hhx8)=SRVukA@H$}^=9?)2?;f}%bd2UfD++`Vu+Mz z)jTT_FTShpVcL`b#Jh#CVnkI^+bY&0iHZtr1wW+4WYQ9{@_;eV@O9>iAi9n5znx3AV zlM}Ot84ekF0B}mXvfXR6l63KP5K_{>DG-MQwrA&Fd!K)mnFd@dn!W5i`Vq$=;8VMU zKjNW_0@B7FyTMsel3+uGF3BmR0Zg@JYd!O=X|iIgJ#7l*;>jE!OcTYzQn5?wDsS`= z{W}}|b$UOF+pCY>_g1U`nCmEON{LDnfbBb}GA2k_z58P$_x#6V+tbnj{o;04mBh5^ zF~`bRci%a|2Ux-t=lAl0UbI62DL<(n1}7{PDB>_I_H zG$qy$8-06^?I3OF2O`;fpfo}z9<|%I_X^$64?i;!R!*;36+fuRu~|?V;mi}RG3N@= z-<7#^sIuoh&_+S%@j3^ULKeeLt#yvg^+Xqa;rmRt=WHu|QBjBW?=BP!RKK*WI@+wZ z1f`_~{isi4&~B(Q9mZ^NJ(IgVT}8g*rb`11D{>&%(9rO^l{gSZvsna+=$7l0Z%l1E z%vcR+>FNSQVN|o6SeuM$84V3L%k(=lWK%f_<~%XqRA$DFG=3 z0Rd_0Qo2*RTco=|qy>Qi=@KcC?gr@w0qM@6YhZ|hnfHLd=Y5`ct?zr+_s2JD@ed4h z&YXMSdtdw7*WUL4R3gYif(#zC9khR9=RxT~5#qjfZp%=8Rlw(U6PzRL^d;FdD@|(c zk8Pvvdl}02RSvx(i-@)a=j7!<8ZMmaz#GCzYX6iSMrt$lv|y3Oa;LsvSB|i(kwowX z{iwDgh$Xn+C%fI9(`j$XV3CE=PI-~ZbaiPo9Dix#{D-N5*HY*?=E`WxF8j$j3Z6Q` znaRI%a3C{)FV^3T<2<9dp+N=5m-qGh61i>qHiU=(6A5tLG9sO_QA=wdoW5Blm9ID2 z9&IVWW+iHMy%R(MM0N;Cc(Asl(YBmZ^6t*>ZxW12wWK8tK18to06x{^rwFijo&y*JeI&Hu`0&Uj5Izw~y zQ+pkH<<#=;-XQ^!)%@?@AARcV?EF2TYDoDKfNU+$u=qK^Jp|uByMv#_4X58)xnJ6w zV&-9VN|4$R0b|h?_r2^v2}s&2eU+#1$+6|zDZ8?;Tbmgtc+LD!^zvGVw&w7c5e7_i zjQGDFmbUze5x3m=o1CyDrr|R8?5|YxMlEHvcBj&RtW~i}p^L8AI`F%C)>bT97t}Q2 zxRUwq4A1F!ryDAR3m8H6v{J#lC5f-3y~_EztjX!6)QL||XxCpxVLhU;Lyw#%p*mD8#@ zSbOaBZ;7Sd;%*Ms^D1r$<;D<}QEJ5&6pk;tH)ljGO}c($tkh7t%1-^l0iR%Q35o{X3r3&dm8Jb4KH#<8aTsHH4XDOvb~S z3%t>O(pc`kKaEu3bMX-f?%_)gi~#=8H4*~6)F+z2TM&@j3epf6yHACnTlY;Z!DlLuuvh^nri*1*bB4-Z;m3&)peJi_z|o^HPpS zhMW?iV^FJD)h~2_MON)9QF!yYm7WI=T;8Ud9<9pEMJh&rJ~Zjdqs~;w>mBcl22%JZ zAUX@rH!WI~@G;K0);*E7S4Vw4?W1P0a|16|&IM>nu5q<3XX+hoNhqwB`V6_F$lRt$ z7U%imTk@+@;OB=2Z)m{VMCt;9LP>KgtmgV#sy87L*S-06y$XNN4&USMEAemH>DA#j zDdwms`s*dnM?`$>mE!UJrT9DVAUx2uZ-o@(cXQleuERK}-8KYq6zvvU&t8wIq&j_C zx(375r1am#jXpdu8>_SQ<2M)dEQg%@Sz-IG?M21Dp=hx~-fsP=83oiP+?IYJ?1;ES z^MUV6$3$MJH8~5|Dl~C!-AwKfsP&A}EL+Ym#GY6b7B<@2$sAY-ENI>JL$A26Cll`? zPf1QU7$?19G_g-+E=)%i7d-0=sE6{tC{-VA#E>`d1kUF#y1nNz&2MbE7+!e2QiCB8 zlvnda^FGJZI2#K;ucx=)P5Mx*U5@@7=v2+idnxF?7Xql6Gzyi0b~VcJD$m3DP*XCX z*EtH%_@Z_8-#iv2NK0>4UoZy(j+IqtTnDL&+X<1b1g1?H0A*U$O4GktgiEp#8Yw+SO)pQ`p(LH8cQ$=0UIDV~opjR;*ilXo-J( zPgQ>SrCW-`(zm5Hi*%+{h;y@|pnD{5xZ&=z0(TLOp}}#rAlMp;(?1{BDk(YtouM@D z4VEq!jAHl&AIIyCnBp$}(VaWCNvpuVZzXDOKl_e#T`#e$+L|NrHjcqzkQ+iIj-U9P zLgniN36kP|Gr7aRlMGb151a0!*&qeXegI7hISPFTrSt=dQ14?+C|vB_hts0PXq&h z>nAY=&a>fFYF@bC492jI$Fo3kqrOEd8R1mAuo=I}*sMy5RL>#5q~nt9l-acO+zE1y z%~w8QNOLyd*qUR=A=8zb>JEdk=Jb zhUX}pV_T#2*#@DG*esc^BfQe%B@hjgY2O|;GpFuUoP(#q_lf6!#p_yHS_CD!Ha9o* z0%t72qnSbFN|P1<1ea+MosJv>ia?EpJ*;9S_4psUhtG_$T)%z|R|I7d`8FRQDRz+o3y3-d@ zNgmRJv)RJkeb}#VnR%!Zu+;?rb^14p$m5i+r!-H)7!J>L(%<17lF%kzI&aN zm;w)Yv$rBpe|p+8CyIm@Jv?SaD(9Y)8G;;leOvYD17&69s8%Ja19^!hFswSK2ez0Z z8p?RAxOOoUm^bN2y`c@D?01*kY~W4y8u zD&buUYX&uVu`3wpA=`tMLoNjpKYi_wF(k`K{T@h3JHa4u5KbUFy)U2%44x(8P4BU5 zgV2gDftO?3;Gxc7tG~H-V5H8*=XEIo(YxTyqH7kWVP;bsIc)|iPvLzrM7(z++W8PH ztmP^vT2|;}RUQz{IYD-J`M)Hctmisbw@MgSX>_8L+aYt(!MiiTZlyfdv)e9{1DMN# zNs%KlUEA%n{_PH{+YU)0fno@F8B2TP0Yl8;UT^&_3bpS|b8xH1fwE9aFVaF%FjXNH zPHx*>pDp|3n-;QJ=lMiw`410CH)UB;8vfia@cprlpW-IRevm*Gvpqp0sg^C z#%hW9c94$(pr7xKzX0<&S&q#+6ES=~;`lQd4BTP(eTtA>m;esk*b+NEow-r;!c0CMDVQ6bLQJW6Q(&x)wFozFHkmHyECK#^(dvWsY=EHDE{?sxbe?te}aSkENEH9AIL*h zEZX!iaexE>{duz3%frpB023;WXwBDt3(z(K0)}ij8^13+U|cvTuoozD2~~@+158TW zR|#Q%nq}~PP~qp6-66S=Y-wYCwonu7*sjAjbLZ{XC2S?VH8P+lxImSp%0hxkdid?A zkVtT6EX~$w{r2hI7sqSAc{<_h;ok4A%0-yu_6^l4R&o1`R{jlN%NVJpT0}A&7;zYV zMqY0p>~v$AbMZU*TX0}t(y@cn#zC8HXRnP9&V%ScATXuM;%NQ#zPaPcHJ@0;dbs1& ziVO|JAH=@XxNzmtE96({MZUu76<$c~ot{ahyjt=C-q-{EiTY!2}oYxr#j#yI}Q?x-WfU7XVUSfqFIm+$?{3U-Nk;Dv$DT?W2CMvf_2{q~e|gh>pBDop%;iR&_C{`j`N(L_6UC z%y(MfAks-_t}@EWGbOui_=px6DQ7~RNB?uz^g6@x{mWRh_j_17 zP7Z|?e(0OPl7FLFye&9`(lgo3l7Cm zZsFMs1>-n3!WDkE6TEo2YTj}?;8J&l2VXScKYMz1vw?s5C!zXEVC#JupgX~dh9>)m zUEbo7#>iW31R7fz84=4E2cTG6lW>Cve6NQ}>s??8kRs(HW$@0^MXE|E?p4oAr;#fQ z-Zt3F%N3Zmkx>Qqqvku+5m%`+8K?WIK!00Foh4w9;r|S>(tZD_i_24FRA~51w>e?@ zo^Z;h8mHx%XfbzMk}X!lzUQ>GE>{4@G?kcGqMM-LthV-o#<70Exp||Xnx9Nthitn; ze88i?@7mk|`rBjFe~(~P*2arsQPnDmZM&l=RwH1}RhWau*n919`r|adinlLS3uc!i%f}r880!&)T{0Pw|`U7km z2HbxQg>oSg!1;~$;b7>Cm@`ZQVvvHXi_ypsFk^91~WMw0m!3#}t3 zPuT~^;6u<6ZR(i!*vCv=JQx|+{{%J_{Xu+Z^TSC-QPL(+46t~ZH#9!UhyWy^0j!NI zEcZFrIdCf8r+@$dD7G)SZ$qutB0>$|=7HCQ{7-pxp!!u|_1s&S9*b5V=>ae0Fg1KF zu64}6`;}zwxJ&W`xDWy82D9uzveR?uk&v3&bFxN<)IXpr3skc+-`LWZs7?s3-G63I zSCh^wZf^aB-LAi6+3C!H+5vd!5!@s78dg}n<<#-zw0z<{EX!eO&{MZJ*dLqMwvUe5 zB>oRuPMdcB<#|7dS}f$L0!#U_Mt5O*^^T)RAE5sn>YK-*np0IkB@fuDiIru0z>}Ts z`#6%8m32RO_-|4z?wObH=H}}X5<#F4AiFc${Z>X%nH1FL{)2T3n!YlZ?# z5B!-0Af26>H}rgXfR95eVHti$0y>m(I09-4)xh6Fo+3nbSjbJV>mmVr;v>iRr^WNN7=dL zhrK!dqCtN4bSXSh-*`P}?{R`=8`B zDPZdo1Ao80j}ddGbap;XDVW2t1y+^=D3X|7oq+7Of6Ged@l+@)KGNuG<6ev}7$Jp|}3k|UX-rB$ zS)fJ14(iIex3oO>Q{xaV!Um4gTKo3Wi_sCoIWy8pn}1#+%v3cld+Y?1i%%|%3=|Ed zqbZeu8s@Cs*D5@#9<8L;I)gV8X-<>cy-gWXuNRsya@CC9Jqz}P$t-%G(l0`GmHj@I zyAg@K)>SrgKgq@U@>IC>Xr_L* zZvgCc|A~Pvzu#5ARwZsd)N;vz4=`!{n(FZT{gr13h#UC#2&DB6@Gx+{(vF;I3VV*U z`q_v@!m?VT;UHXtw;EiWhsAScX|g5(+w_?JR2a> zd^$A!NL`U*J6&;{L;~O$lwGe7U;eriFZGEDXOG8SVRE}wVDD8MM!uFead~gai*b{M zQfu5uspy^^2N&PX(_Vm20Z`sN#j7fr2l^nI_a=OG?mbr1eHAVoVApe6a4*9_b1x>X zw17e?P)Wtbq{BZJ5IrMp8CaZhZW~-u@%zjNd(W2jH(Plv zk=RbJYdb}!Q%~*2Qp@*Mna2d;M|#R4OU9mvl%4)HJ;MJ2De2aaNX5Vv$64kiM~G0` zX-a~wL|f>0u3a}yLkNsU^mazz+&1bjppE!T==vPUqX^MJ6N`FXm`cE=o-=+mcCQXV z{xJTu^r3=DdnliP5wDq^0vXjKRBjKBg3y2~>%}aGeh#?V_dChvew(6E+DN>7T2(eH z<5g&-ajM(5U;= zW^FdwEUE1lwVZAFH*_6K@wqTll(ACdXKWb;n6-)4vDtWF*4EUR7DyXybha$I5$eE3 zLmZ5o@ar(j3zARn2k-J2JalNH@nt48^g%}vndoil9 z+p=O56?JuVx4R;S{3%EVKfmx&&jE4npM>yumAm0b{Mt7>*$9^6Soh6Tx{z~o6Z@)b zuJ5Xtazvgt@S1RJBx6WLO7wA5;TRL2^9)++W5_UgR7^PZP=(!PHE_xZ9kep1zV zBi3he`RnrPM*|HhLNMxuC1Zl$B-pQFo6jK8aHXwO(=NlE#&hJg*&khPC)*X-v!qe8 zo5}j>NxzJA71^zw10Act`e^$^iCrSREsEqSa(1;LKyh5cxAv{tW{!7L{uRbi z^!rYo4zO{lWk0!pf&D@*U`dA=|dGF8XVw!l!*NB;jWSDz_^J8HGjzOSuzH zNrx}RgioIf$t*SOEEkgzqw)WcisASTpf3fv*>aoL* zt}9sMT1%VcBb+^n;n89*m)<@*YHtGaE*KGfNML`X)etEMly1)ZEox=jECa>G~i3uwcSX)I?>D+ zUqZ8(*of8F@mWA_n|DL<_qja3Ln!}(zKzb2)v5kr8(%FHW${d?Ag5CAUc~E1xMO4fR0en@$(A(S+v{F7+lAw zyBn6gzYzj5@8E$wlBlm)zRCLFVyOwHToyxf=(4)eJ{-NJ{iho`j=&%SUN!m3(1VJ5 zyVU$LC8k@Ily52aS*IEk9$AW9XWemwe+M$JCf*B{l87G1;TP=Jiqx%egQmtuP}Pf| zulSxcw~&N7nS0!IQ5~qS6)Hyf>>p|{>Hs>O=6ZP=n7O`P(uiB;B?3XS`orMciF$KU znv$69hRLd15}O#wuxw5;*q2M)Z{Xq?u9oHi3kgep|8V!yaL^-OzPs3wPq#F4I#z^J zwr}nX7w+k?wmmAp=Bh6x)x8NB_1`q5(5Kp8TT0mw_TebaAqMGWFwj*RV@ZwuzUhx1 zC~Qq6H$Vo9y*6{dzE2VT5(rOKKTGi+A|4gEG`RmMScDRUdd;e>m%3)!t7Q&2bsc?P zBIB16ShI_9EniKwE!*fw7F};P9<)Vn!>TKr#mfwjpE5mWlhpygu;{s&ZTVQ(9%|E? zSiKXZAP{&)%+_{0wsGHVyl@!?4(GR^1U5F`m%E0&?aBH9)h z6L9j51$?D!ySLA13!>@$p6Y{vMJwet%(Zv2Gyx^ICJrlCw46xVEHQVqK=!O&?LRtK)Bkc|-kkOV#1&X;^mPW0n z$vX{Qs@je>vEM6TWSiG3Xr!O(mV@6{?nbH1+C8nl9*w$k%k~w!xgqV%Oz+4^w&)B{ z60)uCIX71nV^kS2mNV7DR8VN*+HKg-MO-Hj53znXUgq2E!MBv051M3>vC|HN-w=ky zWOrX8+EOtJMMesP5rdTlm=+w@Sz_G7R4vpVv_!d|nv`efoaZ|@Zf&yYoZrldCiYx~ zW}mDzmqY$!)RXzmNi6!QFVzP6t~g3ez9pS&hhb~@$gF*dpwH2eF*D>D`o*ztq<5n4 zlJ7cboVBr&=$Z=YXci7UhTVFeUkIP~t+{X&8TJCcCF`352sS`>hsFd&E3t(0@0sqgdz0?e7bQ(r@|M5_Jt#1)82BB4G&E(C z5Z!&o-~@g+I{c=-{nQBG>tsRxEA@3Ns%lW*04DhdL{dTtyLP)YpbFj4VaixnaE(YY_G;yCXco7~!C@@DH9? zpO%!e&3&Foi&9|Wt~Yryh8dPHIVU7#Df_IdSPM%36+5bGB;4`rv~ch`XDWN9I#A`= zZl>>!YNN|^GNkSjbt&zrxsVLf-+P`L$aSH+)tb>T={r$u?b%d7e}qei)Tut8T(8#P zC*U2_sZ0~f(r(R|IiOtRO|Q>CfTFw}zmT;WiQTNqaZJ_5G|%_6W-a=0mo!>da1sYE z6joeV@zRI!UB@5|(@*0UWi=@GvHDa)DzC=sA;Ge_t8-WqR~{ArBrT+TX`T@Nr|DN)z%?XS*~N7uJ3dC?7> z<(2wBbY=Q;1slY2?Mn#xm-Bz|^5w4t2(X(3-QgFh+|7H?taicp#w$H#NzN?)7&N_ zv1ExVMh>$xez@-nh448$BWcWTnE$>o95U#$nCrNGMU2tb;&`=h=i`dvT*jH1YNZY?db9Gy9kdxtt6lr^Jq6vX% zX3(tRF59SK!m zX`wJTq9>QDcn*0M)`{=GB*-EU+rAm~Fa08neLkmsXf zfsX^TIWx7}#lkR@6<24m899%OF0U?!O)33{#@)nVY)6q^kv7L-UBQmzA@oq|dwOjx zCGYK!w!R&2;nxVu-9@$xVSXf`C)4gYxe21%OFNj+O6A3d#YhBH9DIHQ``O^iK2*xe zJj<`(FBFX2T+4WQ+@~=Z5h-xy;}O4jc9x(DFQ2ci@s;L#M6x%p7H&~omkkD?E}xv@ zZ(_lFLn{~(U=GT|| z)mO0Fr6;L_YK4Y<_F#PaQS`*DY+YfPzY*Jsibu3ERd@G(flKs4ow2o^^-X!B&*0cC zq@Dfj1o1o3?G34Q%sgWps&92`yI#5c?N8f=!UIbqAA}P1T@|Hv-;u@aqjakDn}$05 z>5dp>qc5_)1dDZp8>gsclB70=|3WCP))O21848!RjCP2OM{x>?_j6tqxziUIysR0Q zJR2&RvQpmOt|bXTU`+t#f4$e%syZi#_iF(}*oQH6zN;n%rlu(v*z{83k)W}{ z`a6&fX8q~0K049V#Qx;t!OH_Hxlv?bkng0~zSAux>*aiSaSjGQ5IR8H67s$PK^L@e zsP({Ypyw4@qxND^GMm&uk@S3wJ^xX5#o`a91L(eow$^vd!!(YoIR7zrgcHnsfj!o? zbg~MEveD*z`faHrwiG}3Zw|5~>&^$|qV8)FH2_nY-zu>91q(>_Fhu~+fbntTDes)lM?s!e#MqbEDHw3U@pEm3!iE zo)!R;S%E=qm6J@Jg>W%bY4ygQ6d&SkRs2c7bA0thy)WUlPLgWaj8}0(uHEQ|HB!D6#-P8 z9(VDxiNhH4{b^Q!;L;?`6^8Ib;0V%s|*Q)Ksd+H^8%cSpyhITvnE|PR@^ujMXFB(#ZdCjq{iQ*C?xiCF;C> z>IG1gvz@jtC!na|@ujss;sD9R`tgJ2W{}PTbXteC=zhh4-%*XfmLsrnNW*8EX!$H5 z_kAiO@4jeNT=J{G*RGg)v3KfM1CBe^KQeRvp3IbWReQoT;qtI`Lc)fJUw>lQ$D2sA zQeb{qa{?Rddk$Ag?p>bjhktCH%l#>AwW|0JDlDuyf2Vt@ZlVm}!U4fQCysF|^*s&V z*y+SyJ3tJolek|G;t~%vzzq7cPzh`tAFlPL{rnsU_@vyr52dlKITjyAZ>4|z8mzLF$3O4$*w8p=);V()f zfDNuRIhx7fJuUG;O9?kS((MvrZ|TdzoN+N-apL%Lbu?l#93`3{rhM>aKf*ZYuIT>| zcIqpX$H*4n-;Ds_Jt^FSOPe!{1!%IjRRuWj@sXB}CMaiuse67}A9k2iaadn}j&*Mx zq+J}Z`p2xV`p5rU+Q=X#Mq~Gt$SsvbU$@GHXEv}^@4->7rFnr111-@72- zij?HG2(rhG{^quUR3-pz$=Y_OeNP_Ou>7~)?|Y6~o^IP0a9LwA`yr$AN<#9uSXB`A z2l*`)FTcIJ*S}^FWz@+cLlTVc!dFxBn(5w`$N1H5=Uune)^^2>RRbmS1NAh+n@ihnwhbH6|}Gg174w)8ig1eD|YLrU@)1D~g6DK<7> zz?&yJnHXulS{4#$_0IMKhwb;f+D7{sp)~cftKTNOS3+YxsA0G~wed>vh|)D8t{kl! zuRTLdX@~MJ?X>cHYtQ~tYjcjFnN#SUCzrV)@m+vBdUAHQ@@q>w<$XtBAh(KVDCXT2{;6Y*ffrj{gpiZ8R{W zZyElLHJ4$s_C?o=|G)HVjbBXVL>yjBqId)_K1SGqR*{{PRiT>?f?tf#G=+>TonR<@j)Z`}|ISNoYn zPQ$)2g;uQQ8_M{ak3gB^E`#(2&QOzes!zIuo?}a0Vqk}yVT`)jG91JRO^T1+iqCsq ztushTAcwgjM(+}oBN-PD%TVTydB@oZE2*_hq1=C%Pk)djcaqHI4DEL9z+Bb^Nt#o^ zIN-B3siTRnGaAE_M9$BXDo33$XvWnR7s`W?wC3I32L3&$K9ciz#{#shCO{Af3I^iK ztUm6pM;}mlOVVVmarbqBn7@$F zNMGZ1$orVl7)z+<(5#lgH_P}CjU6J_PjN#hz|lg` zFjrn7yk^|aCZ`*o1YN&L3gNpBRcij8At9BKA5J^9i#)&RV)n!ztxF>!^~YrULcdmA zDf9odSc>*-K=?Cmu(c+NS&)?=&glz8ZaBtOguC+-h#XwqbEr*+~oL2k^tyI7m@mf;PiRAJ6W zma;ByWf4ST#2a29cs%|)mE^98T@H$P!}H*KFTm{|#;DF%^?;)It2w{5U;p-RT8;R` zD~o{xM~?8t-xa7rWq2F>xtGm~>338bRnp@*)M!rIMAzmB!4& zs3(D2)C$rc)lasZeNwJfB|N5(?M9=YR8}~T*~5~YaD1qg=7_B@tpyT%ksg9l;`9Gi z3+$(w20r+cT4s2O{=sUO@BdvDTpfRlAAK~qYK}h6J*=mmh$#QZ00sJbJj)*{Gw*G2 zKcJoj>7*y*b>#lkMZRki(B-@o4KOf_D0kfJ2z;5ZKU+m1Xz8)>cJ8(G3js$zaijTb zVds&T6PdqPgU)LlxAwG`Eso}b`FX|>e=54+IL}g)Ue2Ab3T=zT_kAMjbFuq{g6^LC z@f0~ruP$vJ(u6);|8C4CO2jo^Q#CP97FKW_M|+t5;J|{bp~)iD_>GPB;2*849|yKS zZ2WovgNgr#$ZCitK!A`upx2~83@#dV76#c@4mXPNoQbsqP9{38V1siI{@yM?RFa?E z#@fB+-krW@kpjW#no^4L%U$&>#r=Y|!rs+MRlhzEW)3D*{df~eAfbWRkP}PgPQsT} z`#HA+uKhk@Jj)!0420axCkV=G(GG&mJWtWH`3PIr%JuA=iZZ zYFXB~7jeyvAK@r|XT^zSo?;3s6%ve=F(S5edg#6k%Uuc4;2>JkdS)?Vgg%HRzav2SQIive?oJKACKq{9)Gl zI>hr(>Wy)=nXir1bKO+d68e$Z;bnj2-^dJsot-Eq8hBF5Kg%c~I$3n;#sFHHIDZvE zOo0EeA*LYjrFgeDEXUgK5(9aEV&6lrJh{r0*cWlaKfvFrEJtJQcwA<0HXjEFu8tpX zJAj8-*7OO$j86xok9U{zbA&yW_^5fKD($Jh{4*H#etw z_#RzgJ4(QyQzb8SM^jsqwuCyOgWX+|tH9s5ZA=Bu|KPlSxpGEXpYcR>*N7--RrdCy zpH}Vhlj66_dnu@!}MgtSC!?c1< zU`=jVUv&fTnD*oRCD5}O9_<`#u=7|ro7V8c!l+wha9-0;Qe}hPc5FrfQhz^I2c?`Br z6By538c~>R-!rb?SyjPf-Q=4*e$)K!w_Z9Xf5Mq^WkQs4;3iPbo#b4dDpHstVn3Si z`kq2BYc^V-8kldsm??QqA$vQX{|P4GaWZ*@uaoRW#Ew6<_LWg=b0k|3axPF>k!FWF z|DC_QLEilbPZf&ubn0z&QOF5*!ehZ#!z?1b9p5T_549-Q?iz1fh&>f%;??!8Gzesy zJS|tbenp-m6Ou3Tg&2dCEw^uuaE{5`oJMzMzOYp?JT29ES3n1v)_TOKTMRcN<;++* z(_vK4TLV~p*$vD3g~|nqA(da7%n`((kNNqf47|L`eJB88%=3n`+TqwrQL|o-VK$P8 z3H{wZ{q!t25=d9gYUi99@n&{h#egpU>x!Vfj<%X(xsh0_->XJ7jx#YHB=s>F!g+ih zbJ*7bhA$Wpss~!ZtdOTwyu~Dw8Sn}FKqF~^w{O;l0C(h%QWx0%JI9Dx$HU=jmJip^kN5g9lzf>^Js}7e>*T5 z8%$);RokumIB6u0%h((k5H6QK&$%AM9eM1;d5L4+F)w$_VVz7=0&RZ)~ba=5)ze1MMY6^3i?Z%$|Vu~AfXETbRc)sSQGH5TR6-s zU~{PI8wT1sN%~NjnC?Cvro1J&Plzml38X;Vg>G`)mzV)Vj&yu~!iCCvM&udxqie_Y`Z0Sxp%Glezow`?Hm7u{LrAnBh#UMTnWbe}Q?Q^vO z30Xmi#gEwQh%WKJ-+dbbVaB#^QB@@z}TIbk|u8qbSnhtpf<#kA|N+4W6%PY1dhSj=`#U!s${4I!2-WiiPwM zs{^QNliPme$Qb^HDu6@(7X9c%4p1;KZppyYr(2sdSC`xixexv}(PriwK+C8gFrW568GarX0Mf2_#}XGkd|#21?*hgOg~kDj zh6kC>SE;REvUllP?f)TJ`riTfvhIuLF!XDzFyb%GeE^&I?rSVFuzobjEU9=+{i&I&{$m` z4wPeDbdJpT*glT$p?qu==Q=E<@b+02eJq?>xa|F@EQrTmcX-da9x~i19zi|GQjSZv zthJDPe*prJc|MSrA3K%f+hDvWV4`i4OT7O^PrTT&)S|RjRQE~ZOMSb|kzdUZhP@7l zjjr>w_+C!oPy|qvvo3`rtqxm4ro|s26A4XQLe>)B@a4AX9x<3; zCB}ZF>@wMAJyEy(JykS^LB5XNYOk9;1v2A-b;|f_vdF-t>+z`K zW0&v-V6|vzMWq48$G!N>hZk^)dXv(U&xkee6;b79mV_ zI^%gl@FJ^1%FRUjZQ3C+4z%<(t}p94#d{yB`WZ8QN-iKND((^Ys0hUCu(ERa)>bGK zX~c$FReXD1pyl%K{_sMvJNvzEu-g+LJ@cTN^`gMA{igbGdRmvi!in)fxL}@F0@Qtf z`NmFpzWr}L$!Jwz#6aczvYf?=h1mT~NRXl252c1tooFRS(Ty5f!T zpZLMXU-!NwqWnIsIV>_DD7ttdl~;LUpbE-Zlp+D>H1%U)O^{$4)V`KRmQb>}?U3WH z4c&Z!i`RkG^c1h+f^sm?uzj;Uw!Xq`P5LG&&-_tPrt-___^u3|qi2H=8W@xGf1@LL zaVX>a^G+$89CSON6Y!T*HSFBHf#3Tf2y}9V=|@jouiG2y!dqKV|0+U83n!Lu@hM!u zCyPpf6VAT{3|!^}&c8s#u2@rUBdMG)TMJ9pXMSXsEUNdDaIz)&^Tr_jTGKL^0ZmMK z_wQGW!xN>Ckj|-n&@qFK7)k=(!B-c1!g!9X1tyla%8Hju&#-j$tDV39BXIqa3+2@h zXC-640ujaL;2t~6`WO2}mu74F8&jdiGl(nmr`DCMWmnbu!LGF?Dktn!t=AW<2ro~c zPLTSK@=g1HO2oUHCwRLCFQkxmZx=BO$l;ZD9hw#O7dD3ag|WmVb0STrK)hWPUvjtK zhS5B=H(P39YH9eu#Mg532FzQ&9XhOaJDlZAMqF|g$Jb`0L@=J=v(NI$Qk&6ej& z-@5Uw?LxBgajdey6zDqa_Q>cFp|;x79*}?IdfRvl;z%Nc5{G}9Q;aqCuCWl?I+ci{ z-&iP{1ntq<%bqKZ3iI&hC)_vh*&0u9(4of)u(&&4UbGs9#!#3DJIsS~L4L7(@AY6| z_hLq#$H*cg+cwVWh9sJd^RsO^d9Rc;?pj8|k3CxQrvxCq&Eb{KvlXp4U9Agv6;;&L zHtc=fQ>re_H%4aPECG?!amhg0SDt7?3wEDWit>PzBK<+UcJI<-W68aQ>3CuRuB;1N zb@dyhxU?pn{aFD1gFql)Qh?yVfMHDm5Rm>xZf=eOqW(}2uHw?B$@J5V*>d`95beNR~=GsVaIZAVvNWM8J z!=Jg*X=B`|>Al|zXzhmZ3yH@4sd_0sCw^468|`t0s zUu+EQhRxccTUwSj`W%pr()ciCKK1XOZ|P~d_-(daH}|~+r)xUBZ#gi;=pBHwGnq{0 zY%fp6_uweR3QpMb?jor#l8wX>SI_a79m+dRR;n(^c$NnxXx!)qJz6%LLG5d#1xm1> zp5nSV4RloL_}MLDwx$3wMwu!5r50{iHpiH(cefhHF5EbrVNwazFgWlLOYY&EQ zRFfA~W2rBdZrLeN%Tri|`Fcl7l8nunb&@z$nU4Jw% zD|m@Hw7L@%_p;^hTw?mQ2&d*Gk3Z=|G6ma9o0 z|8UR7ZrGiML{uVE$(-zb#S=|AWufr6>uNIm#FO%K7Vl-a{v&LHJDOEX#I9l*9D9ft ztg3|`#7fH{V4$-xYPqT4A&Rg-hiK&`XltP-SxMGAM)uhJR%?|2=8ZFlMg7PSj-VWF z^~{n-r9T*2l-#SI^nxI?;smUSlj{VIzMVc6m}~wZ9Q)N!(r7<{-|_1DrPPgDTH5Z*y3*)$IkV&c!Hqi-1_BzcNQ?cY@35$f8Gp%x5L^Mo9MR_9+Fl zy`YvcR>dn?nZgJd_T0WkLm_@r%WF2PIJyLJ*w$KWrFskeTExHY?q{V17ra(9L zVzm4Qb>h|h@L0BAe@&z9tcDeRi$ow>BV2D(8LJodn|<0639@|YNxH>ij5Ba|7Lnbcu3e}fBT>awA?rN$qdod&w4Raw+ z_uNv$dO{{q?pnS99r7WxLXuH}sq}BnFd@$q3yABUxTrkiO~DhHq|cZD!oI^Aj}l&C7b1|`b%uazvUtJQH3Y)JnawD zro#0e=cp$3P^nD~`XR$6j|!*E(a(!G$U|U)@*OpjR?xJ=|6lOAfwiwh6Z#7Cp-o z__)6yCZ1WwzlcJi>(4RCzU zy1pLiScjlrYDMYX_1k00n4Lgg5*imaXQCrzGgFXb7sPl^{V8}Dx4B~?tE}{6*%`~+ zIc|1&UHn-fqVIw+%mle9`-#+1>)7emc`Z&`z3p|92cggT4;ck)Hw6a2h3sg9&1FaR zcMR946ScM~f{dXiw3QRD9+kX_a**)5g(E%NVjZRv1el4k7l~<+;UGVkFV*6)Y+CUBUJ&S>*R$Gzum%;&&#sFwY8B7K}pHv`r(!ohPP2 z-B1#CQr}Z-KNsh(8Vs}-inem{8k>z=3qT%1N0<5cZ_hGye1vSUn!vZeO~yce$e&nD z>;A%SBj+vQ^9mOHfZN7;nHRLQ?yUZVmqy$+2t;@tIC{ZhtI44wC8M$A21+NI*_tuZ z3~|0`o*|2Q?XfEOEAxsV^$DKXk!3dIci&*~tzx*|s|mTIf{V?Gur{hR&5+Lps~+d} z*8hvJw}7fL>bgc%6hsM;?(Xgek?!u6?(XjHZY8CgLnAFMAPt9ZI5damKKOp$|NZ~H zR_@(w zVg!lU{NnskVdf7UXVa8cn?5SxX zwMQ$Pbq#4l`F`cIhS`tDPw!znIUhCmT8(z$~wN00}KH=C}WX!bZLo2^H1V z8^CA|C84;WzvbaDS2^PHqC``JEMW}Bfsu0m+sOl`yYIPLtG>%e(l!uDjX7do=fmrz zI73d1a%?6hq~SyBU=U*1JM4-fx&rm~%Eeu;Ty^D7>dFWjCSAal?el=|S)Dn7yy`pV z^_)A@@Py2AH;#5BeqkE@Cba{K_x#A38)fH_-g)Mlt=+kua^^?SAC=CeJqGzOBizm? zZR`iZpUbGneEwZgP04x}#<=G(V*X@jFO6RlenX@xDP>ghaywZ5lU{Hq1HsIz1F&or%yV*dLM?4HzXyb3PZYf9Z+Xq3Cf@) zse3zLC8}zM^Xwyh9@-k5z)12?cvs|=wG$yZFKIRu>Vv*spm@V6{p9*N{ZDQlZc}a8RH7oa*!VacMZ#%#1`SjSt9tV*n>dO70fc+FA!=-Go3h*!F=Tx47C%oFeR9zL?+syf*MB%1o2kgrVI*rM1|#Q^oN*T7TN?0jz6qC zbWj9^UsUf_6h<{a1mDVFK|d(Y>5Ihe+*V{>p*TUwnI5K4-}lwKnv7*#D z`m{A{xH6zq(?s4k5!^2rP^dsD=y&$6Yu*!8t0pQxINv}_N$;mBg$@=v4hvNYS4UXNv>nlq4Y9-6sdUVIzJXU|-R zXRq#}Cb^PUO`J;qNdi>rJ^fSyU@gAo!-l1q{6=_%7i~~r$WI}t{qwb32H!DyZZ3iW z1yKV#zt&p=Si0I>d+3ZG&{Gz^oekMqv7?3KHn{+atUK@KW9~-G>?+IEVP3}V6W<5K zUpQZ7!ob;^_5nrYW`gH*J8=TWclV6vF}+_e`h0bsj}WtQ)qS$&V7SkpOoyT~yft#m zU_l4tqR7i)gl+KV6E$s&rn|hW=hu5D#5L?Nwt@-C<1_8?)WKkoK%dEpPao9idCd7$ zk|6OUegwS0fY`^Und2@v8^_d(UNgU%g5Uh|ij>)7E1+DtCFV zEV-4a#xO(B4&uPCkDjPhS;yig8dFe#5Y`)Dh&PdGA)7U1-1Ds#U=)}#WM0i%&wz0M zw$Cw)b-3Y;wMT_ZY_63i{*@FtbFmzuA=${9no?8O>{18dO-kThZ2%WnG4lTYEn~B! z+|H1|Kp!lIcWlgqHS?`AmPMKp?}^90zDM}*MkZ`o??R6sc(S%bwxy) zemZE6-LG3fItr?GfWPA)1K9!NnT}7+e}LD*07W3GopJ|~u*9Uc_=JS26KA1qK_z-X z+%pe~tEsWKLoS*OkBA`!d=D>(%OGqgF2YVH_~I-lKp55RUPdO`-tALYzzQHiub?w? z78d0%et~RXfIpP0w7kria*JqrkHl+LiDAQ^CfnTzy23f5j0XU?V0(M**%>{81Rz0#eMw)~WO6qzfF&|DUagOb2tGl4hb{$RO8 z<1#j#IpgH0Wupl^o3qgl9p%g?*U0$`@Z^GGd;7{oyJ>v-9g4?jy(9UBHdIAaiLzHtXgj7#^Tf&R5}4=bJ5*8<&R8CjWa7YI=GM zu5Bwzz42l%yqXYj0;6tQ+?D{@E+*ELNT zMR&jttny_Fmec!3o9@w8J1kVDoo;psY}TWfIND7Z#JaVX*>BfapO3Wjc)ZsBU@hX( z=FdyemegJQG}zoBbe5Xuepuv|-sp&I(rtt7J|@h^*ADli6Dx~Q;WlNy3ZIeh`GFWz ziZ1H|7IHeF2WLXHOtE0SpvH+wIWfJ8#i?~9jiZ+JlkFZ@C<%(`jTClCJHW2@Bz9~+ za^-dPG8cVcIx7zxQE9t3Uyrp#MZCjGXjs@?YSY9?5CbM)6U8RQiUpx$%ABy1`_Ug; zTr}v2kBkP3XRL==YyMics;NiLpB^JyTX6@&wXyYDANGX9jT`kRDP!^0Q%)e;1m!9A z3@{Ed(-$DBw>x7W!#p}55ek9+f&haCn!DA74nB8!!)eZ>o0XZEwoo@xvhV3A@`Amh zDP@GZ_p_({S<0HID{){&HS5=Aj+$xJSkE(dI@6DAB$mhR--Dv72{yZ-Q~NHDNy`bz z6M@jdv!f1%dZw%(v7>ZwRgn%@~K=Zj2kDv2p!SyMqI5-hI?T z&U@htq)ab63xwJnz%SuqLhRx30f;NPjk|<}oDfDChQluUV_#>cQ}w$MMvx#s`km-ah18$8|BsYz`8hoB>r( zjKLQxKN+rt`$O^eD^Z$57DuXgf%46BFpA@-N}i8bJYpV=9(j?0pG@;!gBrfjV8!2wRB(UGTRXxB=7R|YBq0Z`R50F z$U_El=aTTo=^!_I&zrZXOUxe6op^2Ok5dR!4{|$CgH_*!zr(*TTFS^VIGU&!#qayh z1c$9J*jtc?=>+*Xi6DQ<+a#E~aplcyf$j^Vzg3DWPyF{SW&duyW1onChBJ&^Ho9Kp z;$_Ver`bs$a(W|YZq}f{??3Li2zy`u6pwfuZCY6K?Go_JC2k4B^WK+XJmDK0@GNDg#V+POWIjmZc?znN z8#5iJ%()-WWE^p?^Fa6+F|_+xEBmrtaD}x`*2>*F&9sN~Fl5qu2GoRG9*@$4l#L6* zukyNnx6$UeT%PgnVXjMR%f1C2i_y-hrJ5us+gahcl_nWYIHn%6Pd+t_&yP$fW21FljcrM^e*pLk*f5Y}&4u*^W5Z3?LZF-O{!>5x-9azSV-8U+A& z6c1be)4l~d!0){)BdOmz{n16aB@{cy3(ZLl zf4MWY*~eSHu0qI0pmf19E^Pl)qdM~EM}LOOA^q!D%9z%L1`2P9x2sKT2!y@8y+w&M z>Re&P)!sd6JQ7xN>uSTHEy7aALEa>n!A~FJOun zsu3}lecoLVW)AE3Ib2N#S-7m_1^a0;bY8r{h<%8j8+er4Ip5JaeF*6zY+wv(@1i&# zj^=mnI#y+nNBCK%R@e{Dw0A;jbH!XE37n78UrUNACp<}T+&U&oGgu6}KUqzik)!2B zI~p(k>;Gk1(fWjr!b+Z*EsnS$-*HCdi#wW7g|T=QjFu{m#T5@suRXqReGs1hQ}ur~ zhZ)#P+d>jvNT^#CQckT?>YY~-sZ?0%(6XH8HL>~(BZ!$Y7tVKD4|Fk*vDgntvyxCQ zq><#tQw5^h-?is>W@MCZv$dh&68M1p=^$MDjwF_a#=NRJQUMb}KgRkZ zJ3uYyxu|-W?orHxj(X}A%hh-mBY%+2cM{JKVTUR@Bwm~6xn=WKT^-|2wFm70p*Y7$ zM$Xj#U<_>+RG$Vsp9$DRS($TDkNg@z|5y}VSR zHp2<0sQoU!r+r)*$khi<7o;j4GG!w85@*WGdvfRfrEC9RpY2U*_d47p8gmAIXUXMz zxb#a@u|(nfUnG;A%H329UxxvhiTKm7MAjrt%R z%7%vK-BK>vq_0N}b@#LS%61{F|FuJ7$m7|DSAz_(W_$_NkmzDyX2(=~3fUy#yhF?H zMZ5%XRlh!-4@&|rhsl7Ii@Up=>HP7Jm&7=;b$&_sGfINk7eHPiv2v;-^0SeosOd&c zkU!2}1Qv?eUF%K5I)|y|^=1@jA%Lqq$tFwcy&b-^b;a&%rkdfCxa;}6G3g)Jevkf? z+2n>#neks(UF#X<$nfs*8Gr@yn7;`Vj&pRv9H#TM-o>p549CoN8SNYPxjEz-qdrj6H zdE)L&n`2qgtTz4y6KAvSb4|m?lvY|NQ>bD%p$WOcMj71+5oNIa8cVEL>W*H!TD;1YY=7l zTenwoIAcsZud?Vj*JxBK>v8F|OC`rYdRN`mQ)z<#QHHnf+{N zKr3iH=F6ksoTn)-Iy+ZkkeGHf_>=(vJd~^eNiqIXZN%*hF}eY?Dl{%Vpu0C}-;nqd zmh=f^OZKO4e9zDZCLGmK-ieU?arweI9R210T}QQ*laV>O8OqCbcm>37M;9`j?4*Xj zg$pkaMn&bLN!B(cy)SMxd>LLp^hod=Bgx^SxhZ%J;Jj-vQm8iAVbtiU`WW`P<4_!& z*Ak_c*521r39S~kh~ds!VtC95E9n^IH0$h3Ih*0kuB>p@rt#YjN_b|k7JR;X9jCQ- zw)a--+PUU#mz^uAszBR`b&XC?(mi;yIw7WcL~m=RC(XlVc$!beoH&0Zgpc{a){}$v zHEfZniGf{2+r`#;6P&UHApsVYYj*PFbdi+R9=6}E?!Jf>%J+>pw|K7Ie|1Sx1$&gT|Mg$cckd0JmGZpo)n4kIEQF^xqqzCS+$vhKSY*NoZzwXe0In01J91Vw%i>Z1~HV1 zO7Lc@!xZvYyhL^q$o;k3RbeQfk#ZUf(|j3vh&9!janIXiPM131%J-!U6>I zo`2Qprk zZR_o@&1SJIuu7kQ6cz>p9Ssx!lm4aWTVz}g+lsoJFs-KHfQD4#%`J)^y0vheANl-p07OtB8gFZCLk-Ol0i<&_(fX$0C0Su3d#NWy+9{3v3(fz_`7Tk3K50nCUV%wd(+l2) zVCQ)KCRsRyl^y>5dt+!-PHAD1ZPD8wboOa0AA}$l`w4d3Aa{Y4?&(evF~9d(}n^|r#2bB zFAPCXZ;EVA%h?2y#^gR;JUxcrAVF85kD>{%VnuvY%GfPkU#G^ljcl^|-F`~d87h4O za$m%MG$v?x@DdP8cktm@M50MZHCCw6I@m1MG1#CB2?>cbn(w0q1Tr!q1jWab>id0f z{u#g0<_hF*35t_kIk7mqO8@&nfp z$U2LmyhPvlXWylGd8rB0^j8J;s9lf8WHr(x>!#4dHxQ2Ms>jz>QvA3;7e=sq@(Pe%#iE*6%nH^XHAv%)LzTWpjcVB+6jw zQa}A>Kqfo=z2!oXG*6z`+|CszAxYTgt+UD#&t-$Di=V3ugrh1&U^lnSsNlitOCd#B z;-zd#Iq}8$`QHIa0IA0Zaoj(QmMYHK74HcVp^@(B$5f)3EWPh2P~mMhTb^1` zWNw{lh{rM$x`4m}xht~CW=X9q)YTA+0$>VI0MZx=NXh|bn1+!tuAxB{I9v)R&+s;S6o`&M72LTy ztS0TW@xJBAMUE`xdvL3sq;cfZlAhxI*K9@wk$h%cK?DazRW!0%`Gv=m4V>8#my<&Z z2na}Nt;u}l=B1Xl4$PEcGlIiB`!rixX6*yeTs+Rz&&F0d1A8H)X`v&QT zPSsk@RgJ^<>Gc(lo<+I}xn|c3IaHlGJ|p?!bx;IZz@ihAOKco2j(_X9z5-5ln zI_a&Z)f|;peUrKI?l)2E7apUA5Ma~kO%Wz=5>u#>lZ%U6C;WE>qWS{^0!kk)jLI%= zYqTmV^cu69;ExaNYNo!IO4ZqV$jSYm`6ZDU+6ghyq=!=^p9|-_#u2BOQ*w> zrjI$c8@$C393tKdbWWq8>AD8xpd}0=Jw&H-5mmfpLa3|1t~0%o%9f*)Jm#Ifpep*I zlw!QX)l6UhMju#snLyHz5K0Bm#r{BDGSkbk3HTqpwogt0I(ID}Rt+&TXUJ&Ksa0@j zi<(4??Wi9&WkMH=0K%sr1A1p%tA)+9=gd)|EqzcO1vI+51jPL~=t)B&1`3~!7s)3( zjY@)O=tPl3RTCS<4}}2{HXiQFzEA-6MWr7tVj{q{DM~spb1y6ume~FHZa^3+{aRM$ zb||L+C|9t+`21C({?nVFK!4%9?=$V{@&KE2#e8x#WXv-CLzxAihuq5xfY+3OUa)a_ zFK0-?<_p8(o#Vv~+g3(WBm}fhn{iA|Nl41+54}3v``hwyeH{h;P=wtCcee6}TV66L z7|eta8Tnx$H$l#H$99q2_1C`h-{y{;4%3%Q?a%`g1?5a3WvliqJN^|Q83hz%#K4LM zA|(J}dSP||?BM3@#Jh0FN6JgqBsLk~)Y3u{#|FGNcY+yoEwbYGfSVJqm+RbR=M7Ad zg!O7d%;8)er4uGf0=?z3+J&$C%jcB;`y4=$(`$=-bmImqY{~DlJ@cPFR`>^d>r564%gZ28B(T_w@}Vq@;x0eo}SIgtq61As4nJ50dm8MGr{JrIOk7k!ywp?caCFMepw^|^9qSV`#$JrT$mn@zl1D#3&4{dwJFu_;F2 zbE(K-v#hbn;g}8NFF$|@OzdofFa%=0pTl;N#C4X{} z#2t}VZ7qGpF1}#{3+I-%++cxP$a=e^+(Q__8L19ep#^Q$xkSlQz<9ox5)sgcvYY@gk&S=#m>rw zoPz@iXw!~SIra0_oXA3<;nqyx)*SZZw5y`IKQ~wAb;tPjtoFo@Mn5ci?ZIz05Op}s zA4#LPs2M3MDNEv)6b2V61m4a(CYaG1+g3cEG&+R`#|b^IyWx-ap0TP|{zNkp;tYXi zrLixDQyLmoJqayNoK9FTES%;vo@Ok0pnS|(%LlGY)Fj!UF^HOLRTW_p-vkMOWCDA8Zcl6C*#>OR+F_jSPKyJhTbx*Z1K zyUvE&qg1L3o71z8tf!_y3JNvIy+sOj41IXLDXT!3V@gaM%=VF(i7NcAtxOsxs~C_s zluY&f(`V`0WB>&Z-r2w*T`HTK2d9EgEgJx!zhX~>?T?pjg|JtKdt>SeXr@2xp{o^R z)^auIbnBE(b9+gLrn@HCqfe&z-H*?*tM;r<>%uL5H!M%=CUki@T%m{Z?|Zi9$Ky6B z>xyp|_B z1za?{+t@S0r&zyL--ag58faT~5~bi^bPF$+i@8lshbO%d(m#{%h8 z9IeZn@F|chS{T@GKZ68CTXSS^GT3fAU982-i(WvAfh8&$vq1|iPe43TYxu~+;{y5W z?*5jdp0W4_aW&M%YHx*dI~>G`5R{+)waKh7DWfIJ6(}^|!FS>MYH@_)m9o`+D15LL z1RPAlFLu9{K_xUwD~g4sr;SeFSIM@|Nev`|a_!FyRR`EAzIOVssn6rTwSOmZ#`QX{ zzRl$)_rEMfKL2y63)e11TjdQ<3Q>&;*#db(r~+nkS`;R!3X4~`!}Oep#MFCh<<}#w zg>~j>IjASL;;&wDEN1%=&OP6LB8<(F$y2W)wI0v%ZWl=Wn2Rg7YXG$ZBDhT4*5~mo zLci_lM+Q9p;90d_II>gllj!%3CX9v;hh55xR}{HjwUn-`pOX9b zI|4Op);L}D-Qk&MSy(!KL&XIdca{|kuyfZQo6L&WAbeaNJf>NV?T{ZpyBOc?+5N7% zqQXr3Vt0Oh8Lg1jUzi>T1Iq8&yCu(VEAW^TkTF(%;hM4ke{+MkgnUTV+_Q~mvGtx6 zLleB_kdA)o3kD_pu!87HZBD1JX;lrf5_}J+Tg}+wXPQX=4~0l?f}|19xo?ST&YXt} z(B#7br+z3Ey^V`#n+tH)IzV+&o(qBV@IOa41CLyOth7ueBshO_YK{MEm z`gh^jl`*c+b^iF8CI#)k)G*;}_PIoL%z7f^BrY$-BU@Vx0Do}!4c5ZZ`l@@ST5{8`S)NMsBH+p6crvlRU)_B$ z#Qx<2)g7SL<(X=HW`eYn#0IWD4~FK$=#L1=iE+#uy@!@-@moaT^x@CB4qgagD{x@3eo{|($H1HJ&ogr zI^k}JwNy^O@iBGi4s);;A8z{l_i-fQe6Cw}T5X&!gxyu8>t;qV*MDHHbhm`Ezr~Hd zSfLIwpcipan;Jba_2AHSQzLN{P;&#IN&n(AF)1Km!Ylnx-H4Jh=Aqdei$w1_6WaN3 zfo%*ufqfQ^N9H_zpu}@be16YH#g z#HC|R^c}R%VP(T0JfBO!PY-jN$h6QDU%J(Mclhk@ytX9^B-*|CCJuU+1cJs ziPIFjJ_31mo=xz?op5ZjItvV^$Q0kV9L57X9y$$JDSmZ|dZ&34J5X{-;h9~O9BYCc z7V^E)TUz0T39!UZD2k>ekScG(ce^A?2CpWVd77NB3Q;~q3*H*<&RqrFGPpipB7peo z;$z)9habT$tkPv)%0aD95H%)&x2F#x^Uw&Cvg}=(;qV<6ua2%>#5snJ{`jO~BhM?V# zjQ-JEoj^iFWQeLelz*O3r=>X(U)ZU3Gb_#A&gQL|Y3lGA+G!>!v8Trf28BzEoo4^s zBO)hn(aSNWR>D7!>|zzngzkxJFCT$(g-T6B-|^l+Qn0OUy{O&v@Wd4qV=+ats_2P) zNWFrzQStG8ZuFO^zDgGbxRT_P5?$C`?-F+YO}S%dlv|aFnsl@G_~s?oxeqJHmgIh1 zS%mjJ$5uWHO$Ws{@oE z>{njK@}BJV^Ow?6lkGWGx+6C>)iF0hwQoO*+zUC(U(;=abgO#W>=Rl1SVgYLYmX3B zt>~tEoZDz0Cs^X3Yu20Ke^X~^*ET1wW{cBB|CBS9F~f>!?ozGar>^PLYrEK<0b2D4 z`~6!2?7NH4VFRwD;3IPaG1fwx=0)$IbzgUQE`3eXPJKhx<*gm7krNY)?LIs&s21@RuVLll zC;oF1PrnO&a1LLJFeV~tX9nSd*OdK0_UP~NALKMxh&?>6l*5m#<*qx-L;|05cyF#} zG_7NJ!l*IPLvzO9oK^%e90qCa&cP?}%QJ%QUzN$WZ8CfP{P@27Eq#11tKCbp8fN&B zyx2*cv`JdN-GBKxoXDt z9x?MJIgzmKq=!rLDO%K;vmb3BMZDK-ZJ zNto`)PtKF|oCsz3E{FF&a0RwQZf`G}FGw~HHC`)Jsz$K`B^`^`jYY(&5?Z%yngS=N zoj3b#vnS&NJw#+>s1ZRE+nLNk;bpU3kqk}a%}Mt=5PG+WpBH)SMYP|}s>co4oOguO zzjqMa>!W^2(Vm+< z>#{6$G?K&Px^+ga9NQc%p;}(9npi|j;u8PT61`Trs4(2Uoa72ysIZv_TcQ=7l+TQL zRrWsA$K%zLNZ<8FJMW-_*6CfLZ;19A;mOc-2}ak#R@bWfP?j&6V=|@% zHb>@s$(P;;iqlc*^7||EhNW6&9ITpt3L@F9v+w4pii{(*8uA)l+*nJFz(gTDiot(0 z_nmSdcsh{-dzf*Uk?Dy;;|oL+8CM|jZQGps{hwo6YYCz{I=s-AJxFnWR;u6qaxXJ`yf@6+Io-f@yEv)YZ+G`S z6g%^tspQwhDR1RSD}1?!0kc3LG>l1tIt5Pe|LtT{3gz*@qvyI-8F)Ta8At>98@*35 zL#DoSI4!8h_71*!*N?Ef7}49(MB)Znjh&u|F#<<()3u$x4n@fO6}@gkw>y`p7&m6D zQc8Q&(6L7@(DL(dqy^`Wi2$}!Aus0wbOaA$+j*0a5=31Qe@1!tFnGK75Q)$k`W9+3 z<@PZ{1$?!`&}1`0!B#71)A25+>AhU@4t%k`GB^ey@Y}>z=N#~xhQL;p7JIxQDq{Vh ze1O?$b}%?31OPqyx6@*N=*q~{ZpZRwTWl@iHym3@SK2^nQU+5e^tLu-*R#*pl$Yl@ z+chT6Cp)7uGb-*)aZdbNf@R@W-Zn{WB}-RtNB&^WrSYImLa&zJ*nU@P+|q6SdF0nc z_2z(6s{+jl%>dC)6TS%gkK$-Iw6EX(X;&uUZzHGz*E=?yZ;GFs?K4_Qx-D4DdQf5N zIH8;MrBoaUj`B1loD_EqnniIq{5h7tsG@YbjoHMS<@f}{dGG6!r!FY@&1X^QJm8P5 z`o&G3;tiu`g81rvj4n6Ejp!%NBPH9;YqZ68*zxtb*b^N?fwF$}j7x~~5#fZxMVvY# zrL{sKMh^AQ6D{q~Nzg;nScJvLqLra%<0(WRi*L>I_0Al2V>h{it~&<71Ak7ItyWY)Z?QI+PqaFUF9)YFye~*)=-dIX%rt3uLL&74N3nf00ks*Ix5XV|+iZ-o!$D0A|^SfF5 z;x_(-j*_7n zOxwJpsEj*1%)0O02)F$G5V}J}XY=e>*IV>0JRt5Amv%yOKyw)GLwLRD^x@#7EKHS9ju_y&&nPL^n^?K-xxc^(F?eKQ?>jQ{_ic$NKmC zA%_}!?s90`{$lIXQ%6;}Q4X?_L!nC8BEWxChC1`)kdf1u*51D~AHS3R0T@lK<5yIN?=%yv$zH2NYSNO!%R?y&jmZg#ws5SnI;+d~QU=m4=<|!&&l- z6g-*tkPciwzh{0Mmf)I&2eqz;J96XwstDKj*})L8GMY}oiW znIYKIAw>RqIq*w9w2VFv^hjY2uX)2_>#*%gcb8wBSohyF?{&Iij zZBVW=;==DpN*g6q`iL{~HcM&RvQA3rrT#&rkOqMMEt-Lz_VDl# zg+)b)si|5o;OGU;&g8|W=_fothPl4EqmH`}=-Pvo=ij3of&jyMLwu|62KAit!U@{| zukGvWyRDa&lohiA@%vkcVgo(5SelDH07KZjdwSj=6gca+oAG3ykNr}7ILNhloXH#dpPw(&Ww=^dE%gJ;#jg3tctO6;0d6Z%bhC1IdvY6btdg}V} zKSO3R*RmT&z3+6NssbH6S+gB?Bk7=G%sK-5a(BGmb{3>uEN|^oTB-u^^)I%7c0Smi z?4THVB9>o6k(`8PI|+_tg1j7bG9#=wsDRpy9>;3_J8!zXBweRyMrc^$8_kWVD z?%ZRIHDh&(VApV>Uq83beTCTK5CR){hwO!E1t&;HR=U!x@fuv&9zcq~$QjD2Xe9p; zBuY8n!jc_xpGv#h^Ki4}N`PFGrgWW9GRf#0Xeq*CF87LhC^$Iy(@ef5ED%{ ziw&;3zz|2Lx`lc#%v2K;?3vG?w(rX}rN_R;KpYHBHbA0p-cH8@cC0Q|HZ!h!} zD1RT=InffvVb<{&qLQ=ThTF1d>Os1vZGQ8d>THE5tycNXPKa`GairN+8mB3h5=9q( zCf=k*{JKz`HR|~>@#_wCkUiGgVq^5`^x;DWr7GP0sc}L& zO6m(v;lB%H9CmxhTKv_l)5ijLfhNmxg{>+sDaJ1s=j#vzd^~_NNw9R`N|sSkO_AV7 z!^aQmUH`KSn_yJ|r;`(sJntH)1_JN70o1>+-<8sktRDQho z=ROb`CcLnfZ<`OI6;Zq)3}X+pc%NYhat6s)Fiyw+nnkqk@Og5Mk$Hgc9};H*Xc-Q~ zn7#~M4dxAc?x_;=KV7CR9-~qiVStaT9@5UB9O;vKLy_V>U>k?X^nv))`{gg#%&(M z1IaD#F9L20nV=6m=kuQ|%pHLNK$fQsCUTr_jP|@B0B3NeoZ^OkYF$^>OV5*!<7Fi2 z6p+8s5h{yMZar`>C+jd{=fzupIct*}3M0h<=8{B-!%h?suO!^{f@qX%(nMo(-bhlX z0qxd3jL{V7weVYl3`xJWuFOBsXrdSeSY$ST{6OREueQ`-{jx78n!IFdL-Kwh$_onZ z$_s8Ui4vYospqN{KgnuM)tWvBr$^XnL7)+t3|tKg8aNe9AINtR9!lo8VB+He*+jlMjq{ZeDAMeQ+n&p@oEaumw0;&muzdJ#Sui|W4jh{pLqtUIPrMg=S~OW>$&Yd zYu@GnQG<&<1H=QTgS5Sxx;&E7;*xS_gVKZH-Xb^ej1V$( zUEr}{P9f77BVY(?kno@UZbdDwi4RNm&SCCvSdHKr&x9ZorkezvaFt&y9E1516<8z% zfee{|@At6a6J6u7|I;HpJUqk8N$*?%B)@?E%OLQDH01+A*sCy5LFBb+!loG)D-yv~ z_+(;8m;J!PZbDX)GOE#MAhV)QV%>Zt?Wa3>sw_Xa{=RxSW=z-HccrL(62iSBb0BVh zg;3kA(McB(4B;Od7f?AmAFZHMtJIQJQ25l+(t?1)7XIs%pH5ZpMM`LJaAKV)`ij?O zS=}FQ%KJ}UC81S9C`@8y`yTtNY61zt z$DL<5(8ydgUOpUO>u-nGP`79Gh5K*CX3nv_g)W5nK z*WSLY1VSyfMK4WMc~Mr8xMa|fiylyf^$LCcN@8JADtccUjwkpIkZ+JoQT=zwCTr(M zQ`XK+CL9=q#c@gY@L;{Qy_4Gdq_LBO#*MK(M!ugxIHlMSjRXO_`XqFV6qE{W0YnYy z`=eqZ&R%%-%k0vIj)l5Z1f-2YG7gn#3fsSY=KE1#m!+o9-Lf!${G`e!{!_xDQ#CQS zdR;_(5G`k@0_Oe81|oC^gN%Vtrc@LRp^+<528KQ(67T{;{l)#i-Nw^v_|)-q@__u+ z%k?B*v)(klIAW~$y+&Eu^uF=huWr%T#!6F(i-5TU@Rmi~IdV!$iRe<1f8W@Jq38Ui zQtJ5UQz+2^I0pV7_gKoCYD;5H4B1RBns#qnIS%0IJ#};>1zH=fn0vPgmYckSTi^~a zms6mvw8#Pfs)CZ!T!f0?YT56^=`R4j_)oP$auvF=n)fr69v zb72Bd9+3lHV4=?pLY+e7%+X3U|5-T}f={8yRK5W$)s{x}xA@c7jSJS6(=@6yJUqoU zF-ij{6MAye(yBmb5I{tUL2KaP^csngh=yX&lFu@?4UoQ9&uilAl8l17_pk{eM51wu~nH za=S9|ZvYQ4Jr)vtKeli_e{MbOi2beTE@m832Z4}khhCC-1kA+g>%y6d8o?X>_HG_>X#y7HzlvZ?R$kB=6MY=Juo ziu2Z%eZ&O75v2Cq9RR@jut<0nFwQU2W&nhq+@sp)r7ghNI9|p!MWvD2ukU9=!bB90 z=f3yKCs8j*f(n9=zcE9vdTK@i!ae*d1q>1&vK!x{3f=H6Gv#kB|R@ z53B_V{(nwzI3bt(ZMFBelgw34iYA2?nOLEU6-rb0^>*?3Qc@PtQlSyx$L|&KlU}+K z5>yUny7M&*AwFKvPYd}bSI8;{*^5Xd9se12Ou$!3i;Dl`8JV<7g#?O>^e<|^>*A9i zvd+BzN`?i{-Nf}5Tx!C14Ofi7hOm|{ier-bXr--WV1UXcXxIPOdXOPCwUy-~f{ER~ zH6|gPnpjsnFKI}Em}3{E7Lov*%rDE`0`2_b{5?E8y{o1Nx18ig8 zs-H=ZG0CAt@$m}h7Z+uI@k)p7aGk)E>XZP>|KIYM(g1{71HZgJlPQ!4xP?pf|1>EF z7W4;n1{)dq+K~TC|y1TnUknZm8hgL+oyHUEkOF+82ySuyILB02Tf9sva zdi0N(GiT@Q^V#R@txskILepL@Gp?}!>PUq}x$1tj%s5+0kn+CLIhppJ$h^ZpbojgY z-9m{Q*&F<|X1A;7|7s0xz{G>8dX!kn-^w~UAtD82spR>;i`w{8SQQ(phyfQMcbs_@ z4Vi{^0DQ$R=XpQ4{pkwOw}UM--9xsb8~AwjU6#tka=!xtu|hG0?1}zF0olLpVOb zt@76x4S}RTHvCEh@s*BF_VXX-753FUIX#%(Hd9gMBPjb|pEYLiEkBdTH9L()OPC)W zAxCt5x^RQuBO2o61)b?sMo)TOScAN*{#>~!sN{1Zcpvts;v9SZa$DTi%3X)a{d&QI z5y5QwfA0gx+2=mdigE_I!=e=6{!{kcy!$dEQPr_4K0z(spiQdYp;pn)4KH{891BRR zsTEm45%8owFD{BIc`>$jzmF6X?!p`R!kU)P3j(tlQZH+{uHSwPNymVSzDmAXjhjeP zDD#u}&l@-2lD!99!f%=a_Wv%BsbnzB^89z+>Tvx1&{!g|G^TlDF56j%1jdmn3OV1)M-2T)yjmc|cUod?w zAGC61l7LMqkRNcg-#n-c zt{ow_t<@c=tNj>Dwd;=4@-g?(xb+|qAU|kL>#lAHnJZuMM;T8Rd~r)`f(Dh!y`1ZG zd+yz}4jI#*t&@$-%2HVFwDfp5VpQ#Aru;(Z<+!T!VQ^7H6oihf3f~)P9;U}}EvH|A z1UH|$p&35%4$R@FB?5_~o9Id3>)27(=}zJWye&_q@j24hNzRap#|b5`Kx|wUv%y~S zXMoUJXxsEJSSfdZ&_QbW;w!<0%ok8p%9jEu-dVg-{V`Q|ocqLL?Pl4f$Hc3kHja4N zLgG)VauV8=yb`EK0emSK6+cp51?^k}3e#$cVt@QA@7vl4nzEn6b?d{f-p%&|L(4h3 z6-@lmj%8SqP6BfgeN=Z|?JJ&Fw~)*H48Ha^p;?ZrD>R-jvblUFc~_HXPDqfNBE+Uz zctidu`gRQdAX~Yey99&X$5{zt&fnWP)U(r@JT`3&$U|XP4kQ}-^e(#SbC$Vhqp`M( z(7JtT8}o+J+P#Cw8SCv&$Y&ka6srv9b8M!lFX?e|>!6K>Hq#H^nbW@3=g#(m5qi#Q z^Ei1UJ_`tlNLy`Q%%n=nBs%-*V61esH9}Xp&fzX++!HnxC%D;(kh0C_M8C*%y&Q5U z&eDV5EU(<%hK+!zInxykZ8sOqe8v-oSBKO`PQ5#P8cIAEFo}L6HYTP7uw@*Fo3v~jYrS^I2sqSoG=G?7@&=$Ds{C9S1*#XC@wqsE#& zqH4QY5?^v9zE@lP_Cg@;FViakraPzIFb4q%0ZXOqH~;LK9q>+7`N;!R3hi5WW12_G zFGVCo87(-je!4tCB|vs%?#B#?R`IQf{m$>Hzo{%uU$_jWu)3jb=5WMcfR4!WTHCOmZ@}PvpQ86pytffb zMr|$#CGRaM9a!Ma#>m}gwJHaSJGi5>9x~s5QGfSNY&USH*w`1j|`ua&le zPj?uw-YX~qK%=lN<{$~f+}FaZ*iZwPkWsBKnTMaoD+i&+O3@Y*$@SdueL#1S=I(X= zLFSFKZRYQj4vmk!R2k}KqoKKxHw}|r&Ru8>wyWuHojX@>d#^}X7zni1Hk)JzJMOqD zXk;d`2b+eTQj`~HlhYPsBvM?hpizRn%=$2j=c5LCb1i36F(e*?#vo(0B0#ry7cIy$ zr5~RA5lCBgLr1JTcE|alpC$A|CSNN08_d7aeE(BOf9Q)9 zyv%!kS7^)DXWu0L$G&Oaqz2eX{wf zX;bL$3XRj#l{8jtBUkap=16^9#f;EVIzSC?;1kv@^CNqbtT6b4x#vHBtFa6(S;XLb z0Uh(H^2@>6fV8e^+wB-jEf&pU`}`8x9HG4>L}%xe0cV8&{(wp>8c4npLgR0?p%MVWJyp)x_tV;4qh! zM05TOAuOxpg@=u9MNaPK)__*C;t}>dU@;VTF<1Tadv_VgbRNocsC#Rn?+9O>&^NBV z(U?1PuAmyBw%ox>W72)RrxUFzH^>;>Cgx{PWY2NU0;L(-w-B+bEJLu!1ZgG0!%L8? zpuY5KhroD^BkKY3oEhE>;>&rhFt6hDZHr@xdor#Ee%(;H{v1t>RS603K}Hhi7#m{* zfE2cU!tsBMn*|s*9}5JrzS-GiH9V*&1?I0Sk8Fi<_#)z=pYbLBZ^&?w$Pn7-sUvCT z=4+M%afh7~$j&m<%N$oa`%=9e=nYb!)`#air?0pJFSlR-P%OC-DOxww zQS#TSG)fDO22a|eM)H1VP$m40ST$!xKzJn zBhQ*?4Di=cZ|$cEN#b6t;FBQ%#Jdm}@(JUB)=+R(QK`cs46t<+`C#7i{Y z^D%2EN?3FQ5a*9`5N-V$@wb|khXjs%}nn~s9&oDZfu{^&y3vIY?aLDx<;+B6IdCxH~ zY`**hL!T|U=AydzoGn98qRyVj)Aapv#uM4+1S8>3Knn*D&W+Y&0=ci~yEM8xcI59~ zzk6P8Q0TorgSo)iXC3GI+=O3Zdhq78o)hUdUn4q(eKL@5O7m_Z4&pr##D~9)+*@hw zo5)ClWI5@GJb3Qh5+&DXe z!pLI0Fpzg|KTx|GSV{mr;tDvI_edsxDreg>r((fGxCdJGb?9ZshNC2Ht=r42)Tzqi*7~a? zis?$m6+<>M~6z}GtF@WXko3cs$-aU_n6)2w&KX0CI=kFVFCM#KmBR(#{%C)O-frXQx1HSsP;M3 zKk#X!W1R)hD+J^E+sc+qjS5)}@GSnEVnMipgFryQfmW#q9RFR3s4Ou}2o=@y+EEj@ z@Ru_`5)mo<$X3F#RnF$i3k*#7qhPr3nlirE!U&XAijVs-n6BZsH zlm~G54A5!~uWD^-Xb@9|szQ|~%WHOB(CzeI-hhMqS=d#F`Q7-n{1+Us@Q}Dl{}X_T zAOSrwdZdqhL^mb=`uunA7t)<6i}v{S7)rl>?L3pzFE?Pe()%(rvJIytRsClnDy#nB zdt!{<+P=B=)Ol=bDtn$$eKJ~M0zD5404Rl3p+(XEGkV7jbBJ8(hp6*Ehls>}IC^5B z(?0s^V$f%53K63W5%W$(s0C2BVQZAhMo}p`5HNm3Q2|#&hq*p6JW8Gd{Mq_?)8y$# z9Q+|_d}j3Thpz5uA|_aA()3=bu$^6&WVBQ~b9fuCpVz40X5=*TI!oBqHXK*cIM z30OStZj_ZA%K~`m0uHDsR{6g&?gA|bm;h^V5~){sa0XE4z&|o(LF^RUDTQ-7PX4q2 z6pN?D9a(nb=cO&JI*ce0>%OC^Xie2v7b% zPlb0X7EymWA_ef948+&Q;e?tKsFeKgR&13<+9ILy`P@G~EvoYFdvr2I`Pn(36_TFB z4m7W#0j|k+00-(`JIMm?wrc%Bn)#_R>P(LQ2Y}Oa;=}(@Cy~OWuf6wze+k?A{BiE~ z);oOc5|!AY`QIo?@1dfuBK}UNKkrlJnH>kN#RN66veX{{Gf)D}fdiKVcg_Ez$mnBW z@t;npKsiPucru*UZ{Lo=fVAWP+b|88Nf6ahe&xZ!qqp0C7H1_hB?tG*4QVqlB7m&!wtVKr_>)%p^vXh!2J_&>LOq%;qrBF1@b91BcaDV&Pzpmlc_6MzLn8e0IdeE2tAw9Kkz918zGej_gH zQ{^$BPvMi_&vogLuNJ3KsLK-C&s0qhYtVD^V(2S__>GLWiAdZdV z68tOBz+MzL%|FQV~zIuugmf$nUlQ2 z1OUR!ru)yV9X|XClubDbHgAE6-~-?)KpL#wtuYXP5;So?8d#*HivIt?| zrLQ4aHffP)QQ+jCwsNmRrjqzOrn2yl%?jY9q6@EzqEQ_z(emRzqoMwD9IPLtRKov{ z_LKk6UI6(|y1)@y@w`Hy4+ZRhM4|N0n-zu&^_`1~_e4fU<^wkr-2DG;h!^S`2Tfh3V|ExBvp2 z<(Yv7h#sJvlCtmB^xg*lbs9ijWT5&KAg!pvoiutzBBF}1Z@>nqtb8Vxf5iKJ{vj}K zQWX`Kq_AZ8DqkQ?WUOAp3mL`q&xg3PF%uG!|zx zJXG{hiuf2SQB;X0TLm9;Rbvn7pTXR6kzn7x&9W3C9!eH3Gz^$j|9{{|=)52P(aVLs z|AO$S6rfU6Kna?IL`gl>+%x?I63ww z?gAX~YtwV!>7o3PA3!qxpTVI}w(BfFy!#_ia|vMYy)#?$pLd*FufR`g!0y^{1PhJ5 zvVsX3NYBZj{qaY@tYrRL32R<0^&ip^dMa|efxa8re4k!hBEOD-=E#)>xc(YmL}2(R zv8-AuGUO4oq~c>tVq$SUE48NX!XuzP#s6FqoksOqbpIUSA2tI&inNE+sx)Osj7lLu zwf)GwK`_0gM&}wH>_3zK_ha6>@4>dk{ak2rvn(iFd03Z|11t$66#lUI-_QQ{`*hhj z*5^W_8}AKS9@>HRUtgX$kWE%yF!a=&o3e_4*7bmp&8xO&t-4VXN=lW0V!i)&Ws@3z z#&=mEpcx=`eqaNKfY`$%kOxu+-|aUe%h=T8MP06mp5>%TtRGYi3>Ieg&i;I6|3e)M zTNikr*TXn)o;2&TXTr~Zi3yevA-;ji{>SBqi!&-La`zeavrex zec4(Y9S1{1?EH1)qIxA@k+IPCJ*O?Y*_l6FAHz|j+J{V4FDfPZ_X~QNXfahk~Q7#(nTenTq_NF3nZ2&#C!kSu71tq$@H6~r{9#B~lkTh7*d5xi-U z*Vh{W+Q6VKRp_`nN2e4YSxPQf`V9tpr;Zn?FL^HgSEDAS*?@zzp$GTwKF5O1FI3Sw zk0EVLi%!VbS$a3%e46R<2{_MvTb(KTS?m?lCdY0bNC>VsTd68t5wq&0xLS`xJvZ8& zJzye^T^AAKKAj%T=XV5F3u!?JLAo`=Wl6Gnjg@l>r4$$M3l-x z>R(EkE%m5xPgMlK)uzW@l5&-ATa>$AJ|c3y9eLV3G2J-NDV{sIZjkqkr{;aO*qWCe zzock*Xspn(-vTNPtr0D2$AtN(EJFBP%v>YF7aq1M-d(35-#IPc6ff?m7g_eW^4MQ+ zls4X2s~F)PF{T(5KYvZqefmW|;2Np9wPxR?UJ7A#=g4`0q(aBRH@%?D%f1()yLN5A zQGe70bx7;TLB1FAJTOt#+7qx+Oi?tLmltZ>@0MPa(AOSU<>oPsF{oJ`XZ5~3_3>`` zNsITIoZKhv`+-@J-P2G6Gyldy21Z5gbVf>JXNxHjf%hdX4&2X5^@WQ3#S=dvkY?i3 zVMz0HI}Q3V)ytJh1JhiU7j;u*(LP4NtIXnr&#Nht?xzDhYdw9EeA+mI9pmXjHYFsbl!=LU?H4D{rD2$x2dJzJ#fQd`941o(Liy zH{8iz%3)dxUzc{T7dBJOB5&X0NfYgD4S`76wzse7wXRR4AKT>twvLbYHQH#%~hCJ7&C0bm?I4G>2jMNK-#y_8#8K{3y5y3a;ny5S7MKv9B@Ybgd zI$#YQOVIw6xtnQmAZeMLeq9XJNoC2hXAhX6X-qYv`hMQda$aeEr7SUxsGdU_^hZ;cF< zuFRfftYWNC4 z`FPvK{&?;Izksv}@yrqyo(Gg4!*aA48E3c4x!5^xvsaHiUoc@B3C~q$WltTJP4B0b zFD5$4CFDW0;mKZ!vw16>r!V591EK9wQNUirw|y})o|Ih}l7oXnG4drHBT zaZmEl)sam|$yh%oYTcmIn;C~XggqXrzivwO-toEZKJiR)Akkq$I$I4% zF6okZrc2eTs3#h-C|Pn@I78i*J@YX=`O-nU+FRGMb!IUQh^;#DIt zz3|tab#GUe9Wy+W;d9sAMRnRzCsdPVFkTdi7Rxy+g59!* zW}DXudBSm6Z+yaV4_9g3|5JI0Eq+5p#C1k@7QL3N7B5XW?a4_J%4}@B^}L=f_NTmu z$JdQRuUBtH6%yU{n9p;<75Tfx7)yLk1oQ+5G6P4~+q*&eV=Rog~L`PFB6KK(wZB~D(NkW1lr)2$ROT&{$@ zzH(E~y}VLT(aZfJogb~DERu`}?1Uz<7OzJ1S?U}(>K%DQw!QnAUGwZZrDHIs3f+-L z<5}~vZ!}L-M>vop+UB?ND$EKfLJwJSr!7JQiSDJ7v-jWM5Y~#9Y&4w4$xff%~SdB)L|sFuShc5 z_>=fucz(yskiJtq3D0b_Kj!h>!5Uc2!$RP29mjIJ(VV${+1Nk>r4t}M%Zj}bS@7F+ z-=fvRjtY%q=%1KrkgM!nIwzCoyN1IFmMz*}*VBMUNVs0>sQmiFS50iildRh|shZyz z3}xy$Rbvx>R-BG5+SFT0stsp%?Lo*P=ltja!46#i0cnjqHTmcpa9+rqozZ&>qWSI~ z=F;QdL=Le|A75fUtu9_4u|DkpCIMDKyGgB5kK%_{84=%461+0}!LO!m`rG558+;_b zQLs?W4n+_=$%s8L%3vzFV8RJ}0lI|3^jFKK! zGE}xUA4@sJG*G;s9od;kU_4xI7rEu3Jm$SfVzXs`CwEW8KIz2#nIx2NM%HMZD5H&j zTQJ_|5_{$FutQQi1vdwEX6U)AjNz=!CvwNJ;6M+%=y`T1(H?DK)tffJvD1wL!9|?W z$jI_=vtny!r4JnZVR2ac=CVhjzHpJ%CVa1~f;(m@Nuc}2^YlUjpR+1%WV*Ijjn7ZM zPHnXfDe0R(?$Ypht~sGJ?!uNC(L`h7Tunbe*3%i#B}})pf^PU zTT6Z3JXoCM%(U|q!`k3ifjZQ@BO@4mCTO!A`F$RdY1pO^iswqMjmO5|*w=eT({Q)c zgn2_J08{E?41Uy7H2X+i7wYw%L-8uOueHneR6`#Vi*;A3lye7?OAI=LH{tTnVw`%@hvt-WPPbBXTh z>~Hxw1EGbgm!WLwl&Zt6zUG!@-`^GQ>rIt#q((lPQvCG7Xmo)C&oo`(6ER(C|9sJA z*qI3F9sp)Se$e(17_m)OaR+QX$cGfu@-7+Hij!_AwnK$~obe zK&;@Xw}Q^Fwr=N)2+KBbeETqVvHWtbmmyH;TDD$gn!b^ec1P;d7J7+dilQ)m0-<%N zeTK%&_Yj2UgZCTi)}JL$Ho#lUWdQ9GjtZNRTDLxkTFVC3LUO$?;WUB~f|4&MJ0^oX zY=^?P{09%ul~`lEvVy9quB)n|mdWye_5z@?`Y^rU-ej~G&k8lyC9Y<5obwrl(vLdb z=^1=D5b1XJ)22?T$?zG@%|wUX!qv-=hd0@>+DJ$uIKBINmECm65<)PzYp=>w`T67a zv8~(gF&56U|2~(={i#?R^ ze0*_a)rQ#&l=v9lPQ}qc-{Z=)gq+W*+_05L?7R7X;;o(zhRUlup8H;?-~MVwNArWZ zgYnfDd9&3+Sp2g3j}deyS3W}vewWlM&-8IFZ_f^Z^*8f5Okq!3_rhI zA9HgBJ9w?$y4IRbc#uJ&O1)G3_y!y@+wTMAPhVmKsH?BU@cDuHYOCN>YUMvz3dZl= zjFYt7=)5QV64{s{%f@~2V8~8nER1sLegfj(*(~oH9&Tp7IHoa8_eu|F3`Z0>n7Z=h z_dajO+8TRoLgDK|_ zn(}$`{LBpR)bco&?r(l5f2)`TmE3}^tyb}u2_Dg!&sk4coy8kByyqlV9`QaW;IAuc zAo+vQwf}Z@i^g#8UeBo=i5p1qUTgC2OYN)tG8}@9r)zk*5#{rf>l-2L*Z=<6ttg2- zc4nO-NBoqlrf1iAVju99|DpU+Sbx{#7cK2x`Zo0XL8G=AXOn1c&9b@eS}z(N&SOA? z6yv9ROV5lfQn~pMm5L{CJlOyjgZp5N4~R-E=S^Sqb7+5wM_ktv@f*?>3Oa$@hIi@; zy}TT0V;R;YF`K_gCJ|o+^uYJ@^W_RnD^P-J|rf#+$2hu}a`h7r*&C&So9hH|SO$*O5V{XJkDM<3HmDjnozy z3x6rOS7C7{J{Ry}?u~64i}B%y6&cenTrZ2Ltd3qYCvtsTY*;MLZlBUd7Db0Q_7$R| zyGyhWsm#CGbqvkl>$vigPI|gh8;_O8TJD%R6QaFJ$B})J;FZ8$gs34b$RDrg9$~-g z{;Vpoe+_!%Fom~aB|o#0^~x!$GPW_DpS9)GuA-n1D-H43=0krzMIgX{V%Un~ShQ(E zW|LvZ6BNusADpw&sSZiciaC{8==kY{{;4UecFUmCXe89Z;nRl;MsKVevIAzktjXlQ z-i90F)XLn-^@*EAk8Wtpv+MF`&=o^bZmWFw03>uz1817%b`15L9mKD=-##s3E3@lc zi}tpdLb5&2D{na+DRf(&$ha>K+lp!|ijz2ZwOEboO4K--j8JuL2Hq$`@~&VD4G7d% zn`7)C_)abzN*{=)itnukg;I7RezA7r(#O!H(fl2yLJy#f{t0TlFNF8ms=MiIVOZ=k z;j@ov+v>1k20UstrWgq{XCgRYn$Y4%D-o{~TAY%?QD;o_zJtHtv zJmGAZZtUV~Fbd_L+YCDT-7p)BJCV!2-Ch?Yu@>W1r>lq8BQri=EA?P8kZ>0NEx#qa z{;Rc^;EZ|YX*yWGaf@+h$E&^?lyW?E%u1Ts-8Fz*wyyV6Q5`*5W;?tL^e~{k`NpD* zP^d;dTW4hTd`wuY-{+^20BU)^S&d#zw!P7n5;=A!<_dH1U;-+ss(iB()nu#AE)_`k z$#F-UfGf82TswV8-dTl^6ZiB(l7Q)y!^8@g@z1;T+0`rbp%c=_{P)9&c0>nv}%K9kZOWTiQmz z=TCJW>Fnz*L^s6T3G|yyj2L|{m)2Y`7`nQto=EWz#cdbfbGbC?j zN(qqCn;vgXHCS?(hf}C1fW5W(q6~}7&Y`2q!-kegD7pWDEP<|V+==VKP$`uWahK_V z#-hHEbUV+2l&U*$FMR|mBqc5&dyWeV4WunyFyHb#`y3^I(U;~fp;6{Ywe&p7UyAR2 zgW|4IdClMf_(b)b2kz#HCbnZmlz4Es79}b|bFSeh+JPI!E0MhZV_~Ute#l zdLUq-lDP&2V*ZQ5(sQwJ5>b~Ao7VVnF%s%5(RJu~OPx$|$Sk=e2x^|)(4}D@>y81I z^N;I~9SY4Yq+$FjxVb)7pDA|w#jd+PJhWIVL(2X@H5BH z7N%n$IjCMNd{Gm*-!2GUamG&~oNqfL=gm5N;^>^`?aJE2so!5nrQ2ejDAylm@skE) zo#&V9WX)jqk+$7;M*8if$9Q0)k~fm&eD7du~Bw6nk~qrzB~E8-H?-l0mzU;K#z#HY_6 zu2ZFmA>-pS-XZalzVuxTO9+JB+!bh&rNqcCviODAqG^!U z7~DOyo%vv-jFu%27Hkc0o~(R{z=5g)MRnxhkO!ZN@zq6(_OOs5`lRLUFL;D>PhOKe zT*<3dZho-VEtSlI^g%C^-41OaIxCIzAWm{r+S%hu|B(LlB`2?C?|dx8DQFaYOvzDN zf2C<*&S}s7Cl%*(Z7Bck^?>XLR-EplqpxX3mE0RrEkN;ZerCU?266S%IXm?F?-QBE z9*r*MfN^ z=J2B>ZM~%x$qp)#Yz;d61AX1NT;R7$;-opJugMIm3yL+JV7)@dsteuIOM-)NK-Wn= z&J&(4n=|WoXCvq3W$=MpKgxx~np=YJ9=Mqc3-Sz)@I_ejMa#A35Ry|IVA}FK*=8=~0L?V1c&!^QMFkS^k$>x0i&3wk%;_YDy0<-H|Gc;N`(?87LzgKnt*RAxgn7IiG47YJFHE5*=P|Kjs$UYPSV0q5E*LBMZa4ixr8u-I93#QC%o$2W@jm zd-Dagt`)H$H}`-kZrh;0pvzPxKj)9TRJdKMBc2IMAb#?@qUt*c&x(NUQMKLbUPIW_ z|Fv*cW~;2cz1&1Ra4nEg%mMCw%_6D(hHh9-A_J=q{W|DJPawvgwrohMkAGGH{PZIw z&&_OXr6z2>n9G;oa`YDS+GFY5R;JL+TD;xaHT`vAtw@F#hfC~DZNo)hTD0%3ymhc$ zW4$s_#zrY#q0yv2GBXw5M1Lcu{CI;CS*^7_s{GJ7a3RiU=e*-|UyO%mt!2_o$M80TUc#WOv8#4yXOqqJ(9atC#FkuL2WOH%J z9R5Ck3u_q0=A)yV4V55HFg#^V#LcufLAUw8+@ByZ5UwW8Fy;DaRr=C2!a7z*;pmb- zhzpFvX(IKg3>hqc@6>8e50O0qM?at@H{~s?2Qb0x|`JRp`KWwb&>|P zTQOT7TMkg?eZ@7)3{QetrU#P;);`Z=hHQ+*uN`W_B%b&6yH0Zt_lHfW9*WmXF=N9V zM3(;;D>$*oG7XmCcUi{wU_3H9k4!U8S7uV|mC^1Kw~a$$Y8Z1EByEiQkfcH-Ez-zc zOCyKh0_O{qhG9XfOo&!G2-^z(CbVA~0|t46EB2xm5q)W2Bky(Jtg&);9|!Ir!=^-M zgUNF+R%8f9l2Ep)6f&HuQB7dIu1N~DE8)!kD~IOX*sd)EgpM*7{QbD^a3Q`UMnSoc zFL97e7}4vec7-;-XfT@1pl4d1NjzS-r3MmPDio>UFjx09$sOSr(2K_Pa2Y`j! z$MM)C8|4YRj~Y3$-C>^EjiJofvY%1sRh&lXC|zlGBp*!eUYXD?USeS~#Kh4sb3 z#Yqt@PK!!H>XObaoJ))S(!_-t-!xlxUK8>r3_}@WvN!3P-mYHMX>}|y?UCt>m5$X4 zIIeqShJU1L9$)p}Wh?NXQ;aBDBhyrqjo>(~p{h5~j}|PeXLaeTQR@!oRJN!Ap~N?N z1nQ4RBJmqyB2{^yoxN`!jTo@L8q&w@E4LQvY;0ldtQvd+9`bBm;!weN(57|t>o;?A zvtV%^9B=V)q+T?jOH&zmd z*+shhNOrTaB_%}}|MQG|5ynN=?*Jykp~{ux278lEA+?K2GT+8EOTnQhf777$5I7&r zo&Q`(B79zIR`4(=aWA&VfT#N_TfN7=;QHN~@pyAKe0yi~N?bp92orlu+h&aJ=lq!+ zEU|7#{qj_!=ieC(8s2zYl)kz!Vu-fdCd-6-r>zs*N_`Xy2D(il0o*88jqhq4piRSV z)(}^xO1bTIG=?$3V&ZtJ(7N{UKs~!w{&jHoi$hFk&~DVfZ1M#MR9Nk(Yosks%oe9G z(wgPM!~^*?6f61@X1$hWI^BKP(ljYS{DjIpbvb)H)gQLi8X7lo;=5af%OpC}>L@WH z4d0|?;G=02Rv(ET7VZrud-)xx7LGcrv4}~Eo#etuBgIFvPaW-Gec!J(U+>!1)@-;; z|$Y>vRqiWJPr_+ z5d77{Dx3BAE8A%sN|KRUQPj=P!*9A@eu7lJEvI>8L^BJmnI4OP@f)8*t8qNpx zmco0t+ZewIWKEU|(H>97q|n)4FbJ9>eQ$JM{Mr967I+8wBXK^V8)7B*?AHQ4ITp#@ z?YX#<=iP7-%Op;>d=)iloCHXb@)?&EDtqJDT)Ex*ne!ZN8mk^R&w02UNl^(B{QxsQ z+3u@6+kQB~8j ziT7Fy1WdzrOJsqgqobPqM9*Eky3`aHk!i@8UHPh6*Oo#NVqIT+?YLZv!*y~xcilQQ z9N<$SlWK0oRVxx?Wwb|L@CNQ;1_Zh;io4q4H=N_Gs-5R2@^ty{Z%(97hOKvJuZcYU z2-d2`FD$Ns`gPI~jD=_w(7rs;ZHXO^c&3sgjE2JloBah#R`Z}ZA%Ps1<|oIpNQK?a z;v(Jxogp2T`|VoR!!~^=0ZOJAQ`(m1_<(dQ(Y9plNcwWO^@zj1%9ETU04bA6>)mm5 ziL{~hL87_#%~lA3l+d}_o9Yj{(2URvI}jWoWX%*Zo%~7`;CV;4gT+-?>y`LxFs*p0 zHEM<4eXLkmHq^Jf4*CZ+PQecRDH0{CsgN-2`8X#W`fPoxF&97XWDJWNSXc+n`Q>VS zJ$^>tOk17;6AA6kY&`|ci^&jdQxjM6&0vm+ypDzKNmp{kF?W1Ay;EcACaR3AuCFIj z=~7qRqmYB`Vsn#Ab&k1~-OQU!yS<-J3rxW!u#~55p38A0nhq72-I=NosNM70AX1jF zqbBB{{pEC6JVJBE{Y-`#859eiBJp)W_@?1xn=3W84AU794xMt==@?kDK(cdS_USL| z=(^Iadbado`*ex?vwZ9?ID4u6_7vHdHPkGax_&Y0iM3yBchZSY$A~WLcQT8) z7wF-RWGdr&Da$?%EDi~SR?Xo&it(KW#k)_dA z5-QjaydSLpA7rLusQ}%%DCpB1qNHf8=}d19Giive7Uf5_hqt1#dY0c27e{bbyhh~1 zEYB^zBE#_}SD1~X@}*g2-E@vY?iMdLJ3vHyWBctcy@|Wg-KUG5v>I_}MdI9iiI=2x z_mPFCp#;AUmWEG{E2&d;8RBv(2u3MZF&%xDf1zMqZ%-22AL8BXO4&>vyilb@w2u5N zQyo3^OILC(>M@M?#&};x>A5x?ab2U;JKGeiizGC7fd@PDWuCdi8y|z#q$-yWf7^)$ zC1^4bQEa|Bjm$D@Ppb!`|3Ef!P>o5`c881e>>-m>|$_%9ZbHT4{FT7uzS zl7tE!yt(Y!9Z2Vz{m#?gOy91cA*e#O@Dtutqm5iFNVe?Zx(DVI>^<_CW40Aln;B7y zx{aG08l{(HkECv*j7*HMZC_TW8Kp#2Tn^l*X}G_cF}dwRB&XuRRi)GKfAEV~8E4wA zqUbN_6HuzN5wT9`Ogc2mMU)jQnbND8&*yZbI$@=$$!~>Ed%gmvSh)>u;as@g?s5D1 zot4ydNydA@8;>KxBkLZKir1vUcM*KF(F|`Tbo6t{09d0&_}YsE&Gm0axHJVAs0KBo zhdTCSmJzV(c1fqg)^Aw+G=7oGh>n+Kv5Wg-MP7s_CCMEkPlir`Z`(hbI$k9w&$@)c z(IlW556pH)wBdG*KEA#0nH_L!oH`f?p}oG?TRfT5@*%Z;oJy3pHL}$q#7kMS*5z;d z%3OuTYeSO$#BbyjK$y2>AT2>RxzFg^-4`=wE@!w!pJIMt!`<9ZpI0K(gzv2W@Psnx zk$chYIB}vclC#?>k)LCHqfc#W+T2}r@pd-2ka_b$f#oXS?aS@Xa1$n1>TRqmI@+lH zAqPJF%zzSIZ`M0nx-MrL*7j=eM)sLnbMrJa`jC+9cG0r$S`R!MfT>cFZ zjqf(%dd8-zW}`9MH+U`dUo8BvSHvRTaX%nCD|3(f5J4L^B=Sy{f_Ov$@|osMq>!AISWXZC`Y{zV`$4rSbZ#Sn6$a zv_0|XNL64Hw7m#~G9TRKiO2Vv?eEp}&q9$T99A#Uf!`yiBgV|G5*``sp{I4i;HuM& zpHhvpVVsO4o)(Nqik{5Ii#S6l=+-9sm|8T&-)GX3GP6QNbXy0~CJ4Ey&fxbLKXnw| z6@LuQ3N8xze6U?fNfBtHZ85(iN|s?N+s}g7mh#jnoQ+q$G&|7vB>GMCc7xc|ao>u~ ztZ==wCaUrANowW&|AN+EPct`DWXU3^W37}|RknXvy;Q&$eCSyb6!ceJ7f4^cN9P|- z92w*CbNhIV)s1wezx`A8k;RykjWkry{?aWU?W&n@=bUJoc7CMB6O7lpX8v?Nlhjj* zDO#Rc2amc!?^;nsZY4 z{1Go{!wRA#78Dv^P}ymPH&zvftR9GVm(&U%ifI3`n|>{8O~Kwed$_e+P+VNYS{Si0GN1C&YJ(IcB1!jlp4*>P zewY9BlY1exM}Dwp#4Hd}@4J1!IkOTHdO8V^VkdcKFAiT?-M}#k?0gKw^p!NH=1-Zw zC)$}ge>0t(QDTq~tbv=6d5B|TfFyAmON|;UA|jrIw6?@_vtEonM)6l@sAux%K!_QN zs?G6C*D9^Ct&i@R-CluWKbfJtSYpaSSuAzyZfqxLV!=pXkg@2g+Trg%*%Q47#fHEJ zOrGRxJdH`kbU7xpcx#%l?pkdbl%94hU|e4umVRgOSTJFtmi%F7xT)+?)tl_Hw!S`( zwrp@5?41euVyN>N8I`p&L=?N}PL7`k#ExoU80uSN0I&*>8GTIrd<(67gqPdCdHuag z_3s^T4_4QCA~XD~mL&@C$3bF60QgEI*w}S#~c3}!-=}xkg)`vb_VUmVsLGeX!{4Ab>r#dWX!r5H0%e7hcUiu z$|-o{6S(P>og?&_kA_9Y!G?!_8eQP0?-r%B3~iurG_cD{{!Jl+P|4n`?H=%4%(-BA znY$!^sOq22=((w7lWt{qVcT^Xy^d+Zd!Azopz^OjypAsR(~TXT?<%7^m9T%d3iRlc z-wCdX6#bXDsiiJMsett(_hToAi}Ybob@rvI)`Ym-|T zcEwd>W6K(9J*#pW6FUGLDA7_>z0$Fg?e=*Nz@ryI7K1-WfFE@D%CRU)9e}io zf4JaF4U2untD}^D%Y1m;Z7KZCxUmHezLMWT-jpy7>(+ShGzPytZ<{aPrf}}DvWZXV zw|*t)Y{^0jCw?oo(>C$v*1kPK!E{XAD82~+m-!gKrZ)*btapR@OHQynB>i>e;=RG4 z@}?hcg%wLG>Sd_&AC;Rc1rMlSSAAI-enzXE0si5KeEbp$#7~kd!a1B1`}@V2_B2wj zsGrgG4#F*24ug(9Rjspq&8?VH_liNY$g`CqvisTZH<>j{uj+Ku(!@yUDxLvS7SE%% z;VHXEun|-C_%ONz)Qi1j^({>1S+_c}N=r_ksYJu}WZ*Ib$63d8QPk3yk|aR8@CgY8 zIZXbjvl}eKiF2CHWptvfq6ns3YIhX35N>+-l6J_8gOmIj1+343Nh(h%qFb;bDK6N1^qoc$H|S6Dv*e`DE_$m#k3c>6@tCySx&Nx>Ou$=Pw;y7n?P zb7!y+wCgJOPS*uTAyfwQO6Lz?T2WoBoLMui($e0fmq+t3tBTC!sutAP<)jjK)}g%+*e|K;$18+-IuI4?)o z>4}TLI;`WZYZYx_^apu)7jqqOa0iiwC=bv{&hXCQp zns5*j_=B;1bH2LXxnv4YqmFk*Hmt?$Ho9&!e6`oI8bgdBmkUE{I?qng(Ih-z6XH~G z0a7bx^S;nNw$jxkfu#AI-nZPlf~Cv<k=1s10e?0WP~wcT#l0hxUhj_)Zh^T7Qlwa3Vq zM{k6`oOW`JE4j_5iurtg`{g3SeJ!MdxA&bYEUmXn3D{cra6i`kZsUAfC|(rN*p?n4 z>PXU5YsPa10AN6k*Y2ki+XFMN3(DLR6(skVjV&!n*YIu PiwGCe24&4aTuS~Egy!>m literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img3.png b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img3.png new file mode 100644 index 0000000000000000000000000000000000000000..41804fff3b15b9b58ddafc7595f936030dccf7a6 GIT binary patch literal 253603 zcmZ^~1yEeU(l!hsKyX5EcY?dS1&8477Tnz-xO;F27TkinI~#0qcU>G7{m899_a<-E zKUG_tJ+)`1=XB50-A^~+pXDWx;Bnz0ARv&WB*l~Fuh`F++PDn}u{e39;+sk3MX?Pn{$weSI2e)5kj!MpCj# z^U>+^-_fi{)%sSU6${8DLr+>8$1~T6Zf%X-0>mSBo~~+6m#NJs#wue&QC7VL2A%gm zTSG=U>;vmR5b&ZWCVuF4AQ1a!Gz5ddDr$4pwkMy|e4Am`if%JbS6UbBKX!FN`V5ao zq9MZAV|dSzyEv0 z+iY_o2lxkp9;|7~HWL*v(e|gB$(p$rAo{k3SNO#L{_T5k(FGq0;PM@e?uBFR!E!Ty zKu-_f>%8%jC#T@>zXS;zY7`=kCq=cVW*DPFg%3I7BeI$;QvGLtu84q3xZpQ$qafFA z=TrEmv}9CW=xa1GO<00K4_q;#OxVP{ys~Tw!ORmJa_dJtDSyu&=Izn}GK?ZIsOZ0K z%>D$N8Tj$BYIi+B9)G^<0|j*Rb_kTpfHK|dmUaruJS>KBFD{&hCXnr1$}cV~?~|_b z6hM0D=s(Huk>AHseWax|9-b*?+A_RnShe3QZ0`t+?pyXusZKG!E zWx}foWCLZXv-Ycx1O8h^Z-i5T1x-db<|;U%^6~|lv+v5oEV zLxqTiR9)EX^gJ*I?^;dD?Mjn~ikw$yN)tm8kPX}H(E&W<%5bC%ck%|Q%a=>*OZPgY ze~s2i&_fdx>{a_R_Vl4teZ7ODcWeeFVs1N8??7qI@LP`LKF^Y?Ta!0|cLUlFXR$SY^g6t8=(H!^yqhG@WIzU?MelR!1ukUI4VI!`v zexNgZ{JZlOZrem+GdPd-qncMD0X7*5?jsjQ^$j@pVjbk9j9@bNbcQYKHJp(C@(GKU=4AA+Lq9TcXWIQ zyE&uM4!COl9(7cmvQWoixF4*YQ-4pVh1J_c=)?Od45x9yDOoisKB`}`$eV7x2{Lei zEW5U1j8>A3b8OiCKKKf4Msj>6Q)*I$o7+iVo;eJ{a!!lW#P{1N_qF$BK0uJ_v5<#ajR zoi7CRu*5g)zFe%lH&L!vXI5rR;4)KSHYfOW#p<3mGLbT{h*_v=i2OJ#j^84|+46I= z2cm}k(jA;ku^ojwD&Vv9{)$%f=B94>IyRMQA?4eLyq|Q!RZxCiC5Z#SI+?=s!4yPo zcSNUNj>v@_hBN(*j;q9}+Y+2kr#X9ZQRo|pnLWLGNIexE8D$ggkr=?d?e!1b*F%$6 zHhY_@g9s{Cc$ngRv)vAYLBn{qyH#fTQ>PQLg{jpONI@$-h(&ExFj zaV5oFv+e@uO8@-w>2hcd^!a{`Hx@;}E8eZ_*2tP|WIssyZtmoA7zz5!5)Dz%B=#yZ zwS5c^k7URB%N0Z8jSi=>a$MuM6XsvO49u`-Nt`BKxQ`*ZF=$EbB;UFGF*Sx@D7kL$ zvR59GP%m-Q?ExiM-xA8##^qkbcpqboGWp_gP`*$}jC6rKu|y3thi2^keX`sJx9sQ7 zpUHb?S_D?wcO@T#B(4js-VsanzziRItO6* zof~dvawrjqlqBTa%C9*im?}m5y+-}L*t`?*NJ%rJKzfuN%0JZ2c>X+H$N*PRjG+8be`q|gr z_0QHgM{v9(1>)mjr>L6zRiz@a2>J|zbh`HHUfN*|VH1~+KWTe2Oc$+@{H2L_(03aR zQMB)Rh=O`c)vWK0g~+>;NOYTb@-buy7L&W{%g!umGITPG@x40@t*yGTB`NWgavE1X zi)V*V>Hy#FPwcsV&er(fOAn2+uXchpmcDO{y|vSKBAjD99*M1dfs()wT|k(-sB7)W z-7A^1KPez}42jB(gZ7T+PnwR|hbZ{w1~AoObwZj{RBqnr@fpT@ft;*h3+&z?1b=D8H})@5i%`TAG=cKV#-)GUH$51NSgE+i_P0Q8l@P z^j{ux?l>f?jQvK56!yc71VqgTt=l*#-AAx5(O5xJ?Q+N zRe|`9Y*{_MT;7tHoW3gJ-FM-LOhS*w+4s2-pPL98hO4H5*)jnIe`6oeJlL`1 zV@6}JM?XiE9Qk0h;s<)l`3Fe`fjp`Z7QL49sM9UQ&|V$E0&C|$kfdvVPOyxHWAZp!?|b>Az`zh#?VG!t zrrq%VZH#Rr$Enjux1aD`-&NOWzq*Ms z_z31`&{a1yBI4Bvf4_c^=CoNnB#&lRQB&J!ImH}&bJmsuHQe$E92 zI%BSnxo#To5kAbw!NoW34z#03M!oHsQAtI!_jV-Hq}PE9AvX!t&YHubX0n|=GV;Y1 z{M^IciLdZJg(CJb4OXy%gqI3-O{>S2p5q)EW$lhR%TcsEi#e-RHsK3fg!$3w@|~BA zb&)FV_+o1czaP7x-%tNKJFe6xCq#aUq`2E{qjFG?ltFDz8@NwGw%MLb6AERgD;|NX z0HF)_`L=UsTOv@U{_LT;eh=+$)S+Lp90eLMbsg8Dd%66KgS&3o+qLgsT{+Nwq9{3p zhxv5bWCU7urqK?NQz>Lb#5PMUuByW0#YU_)JK?z(^K8JyZ|HN;1y<@d${G9IZAj3^ z#7h_c_5s!>W4nV$C8{}R&}0XDS6g9Eyk=Sr@@BGxU7rMW)kJ-{`a+stwkk^VqG;z+ z6K`SWyJTLprOA1v57NQqMqKS-aoW%C6~^u|9Z{CQFlPnstZ#E5klWOTl#-*4<*Ucy ztI;if1p%kHIYSo>I4N^n(6Z(_CpvTPAacGJ&6)XPkqV2d(q2ZGN&JvzYu^i5eD}f6 z2`0aeL|~6d=cNyRq#}=0QLfRI#E4RIKK#S0BZs=khY(hh>yh+F2YWE&JYDO=*e%zx zqvkrbeuP`{v_F^`+sNi!HNOLsgU)yj@LONLkZXP>WEm^`)pFP2VI-h>La+$rh_pmSc&YkbuPT*?o;h<6((`%k+z<<}qFVagLXOQ=#D=MUPMVR94HiJtt+>3)o> z#5Y_wW44Yjwf)TnNJ9BpVI)B0*?+^ED{5*{PSEj|^l&dJqHxr65%=uHrl}Z3vV!_r zCsMFv&5?%qoFc)) z6kv*`l~GuqA^S8%wHBPt?tOLPcQ=?1`VjUtTr;Qq_@lnSEqaa|PZ0mI)~e?*{W}lx zv#?J(0?iMWmG>79k7vK$Uoj$oa3oQ?haNRX9$+5N&Sz2?)%NQQJ~hu8+vr_P#26=S zC`v!J)_B=?pyS>C+*0n9g90pO>DJk@cu30$pF3V|jev#JSKdUFZOf&PTvxdoCTp_>jCQJTCl$UU}oq*`b?m(K4>Wg#*8AtbsZW< zPGmy@@oh-_px2gw7cJJ2gvq}i^v27j%qGSt>%W`^>~bM{Xr>6MC8^Xki%Z@a8qMw$ zB$QmW#KnltwL##u=_IJ))x?MvFQc+kRxQ-at=iFETHXuj04d1|ENQjfofuXS-pp|E zcGQ%_#JaqDJJy=uvd57P1+lLt# zj8@PoQ^&gV3A3S*P;rJHAJgFv_TTbbS{kdUswpZfi+;pkE;Rm^T#DIx5HQ;eFm9xd zldU69Ak-V_z4e(I=S7OyT8GgIglz1fInUtzKSa*E4v?J>((pJmE2O?v2h#tUQr*15 z#*3KxZ?Gb*rAL?i`%Hxm$>$3x{pFg)#9Y&5iQ)eB*J36Tb(pf^9%-ABf7#Ld_Y2gS zEBuiE+ydVy{huXy=;5J7(J>o~jf)w$Vis1W|CwXK zbg?a@pgslqQn23zKVs31oR${BSPNl+&H&fOFN3`;sHM0;qZ&1KaG~0kK0EdM3t>>> zS;s%S=+Irnm#oyPPZ)`S_q^E;-fH2hObp*d1ZqWDJoAtH(vnKmEl^p}4=|^-%_^zW zrThAJZm?FH9H2Q~UhFS7k5*>Z{uM5U4cSibrc@jOD+?uJ^|&fUiLv{TnhLfWoaGE! z_3yp7#tM$zn0hrE~#!fsPP#`5(1}SN`^a&$R zBht95Q2<+%9G4{plZ_*Ga00q&$D=|fT#f&|;y`1e_Xt=ll zEjmi&_X`EoHf9p{{0sNe(%u=*yAQ4AoBvo!tMC$()S3w8m%5JrmLf&*f(kneEp1xj3k_@@^x~fV7#VtGPo93?sU6K>i z)QUQ6G#IDM4&lcei6hHUG^Z7fpC&>Y6MUf?1Y+3SI~1Nu)7JtU>i>~tsdPM`!0xF< zp!@*Pkz>(sId!7VX|=S-<^RC($)UmF%+kiJVytC{f>qeYthheymg?`vF9>RMF%k}> zJIC(hffi_8it5~7Wa})u>_i<5bW_zbJib`b10ALwyajc!EV{WuT2MN2;OI|o)Fm18 z+OYi-5)uf`(Epy0v^8WJh;Rs$lx?(Sch}?jdg&9H!<~|$FJ!Cd>zQMB?j5-YzI*0; z8NFoDPur_ax!?QA(LPX=14rVM!5w<#+nN=yoj@VebltGj{QA+XHId4wO4YTS*=P2C zxg$DTeIT-bM3G;=Bg(9nTe52dW9EN~eLW>ue;tzr!$m!kgW@vc?m&<*aa>7~Y-rL0 z5mU_Vsuzks)IhW`uw1=^pfEn8q$=*~eM&`itZ8~G;>Hf(j;ZR1G_oo3IPU3;{) zZ8Nlg&eO4Zrqhk))xoqmEMM?3o!`UK8khT8Y&X66qp?J>8;|zVP4`5edX*lp8JaJ> zRcZyvd^=}wHv#K*rQ?mhS)G9?H^fuxj?)n%%KSL%UvX*tN&WA)A3eso26bdy5!+9N z4nLVE3d`fXFZ2=p&5<&ZfiFVm;o?kDSs^*yH5t~US%d=fz>`IY}E1LBvQ8lwGOWs*f~>oCa_d@=q)_6@BOX}%bYx}*j2rtJ&EZ6+%VAl~@s#~T+pP{`jdxdr zQuZ5!5FO^0{>weqApRx4Jg*JB;L`yB;+wz|=3>3oU_pM2*+>$#iK!{%*$8db+4hH* z1?xU+rx!DAnt6cYO5ow#GWINo~j5Jv{%{xa+}=(x|E7Byb3H{pFOIpN&rK^kwryhCPC27u8lE_8R4+G>bhwPM z)%ASe^%s$NUp)(R_?%Hc?9$YwJvr`^FS7%Fy~+FyT7y``~fVkF`HvLsqBn zRq>puEYBSbH4wA2>;%|JkYU)#3nlDv(GI`1&a9on9u|*1ze@1oZrA5(Ut1c&Lur>R zv5~N-K;dP^{K=kk*@ZE8W_0!0_o|GDwfa}B!+|#t)3m;w?-p;HvlEQOweyoYYB}(( zN8;|NQ=RFeoVLkc2>F~n63jx?dE%FEkfHgQG&u2e^!Be8bD{f-4uH*P?VbpzXP=c5 z;QVQQT>~r#;u0iK;mI&%tUOX$?tp@VMK-x9PfcSwH-OOdI0&X(8lP$8zQ)zcZs?Oq zxR|!mw9PxS4?S0wdX&XevtqH5)Ix}<&=;+f_p}+l3W1KY(Gp)sSD_InMm0E`DbaB{ zdD%KFtF(5N*VM$B({HOuy4i`HlmDKGFl>r)UiQ+M zKK=4215@sk-3od{v7;{|p4ozsf9QWnR=GC%yUafN+eNnm^W7i%U>t>I zUlf3EQ$+f*FaFI|j?>7G9(CpkiQH=Yv99aD@v~Kl9tRvOqOP&r|NXZ z9R!C@)gi!R9pHAcSj%FB*+U?~1Rk5_Zj!(RHlpee(Rbd#LA3i_Q9Uzpj4W~z?N;L0 z)i$B?APdISJ%flnO~KagTY9#>kNh1U6@)i5e4W6G)=gHpx86D-63FdTnLPk`e5pKy z1~WKTP-iUFnznewbc~|1-=IY$MaPAq?^9xE05ZN;{|^4jLSe`AB(Q!@)^EVS1_n`M z`b@SWyx%C(VE_l)_#1}}?!$2%3g?%$PEJ_zv87X66EK4NbJ{LdHsi zypuXdAheN=)wE+b=j7KY=x|TiOYM0Eg{ev?Rs1Gr3heY_pC>$fY%ga{u(^#Xm}Q3ZrK=F0)RyTLkR97iQLY5hk$&Q86tgVg1| z+(mWplFPJ*`_Q%QiT3D-$LJ9EGfm~ZV2%k(My62p&I8gryK^_+)C={CK|$Nq+F4Y3 z9MP(a&~Lu#Uh*%`?lAQzg~syQ6RnLeY32!o=SPCE-A`w>MJqQbFi2cE@zH}TJs$ai z2p(q&x3%`$nmSLBz9y>bXbkYxA;c%KcSwGGC0N2+>!*DQKxo?0=4UL}#4CCjNl&Dg zEFG7Lz7`4%=B~oV_MexOwb2c1`s-Gl`u__L;#W$>>9vo5LSt-n{f!|pSr_W*oPT-& zcrI7HKpP%}ZmS;VPtEa?Um0{;AW)G*4}Rr|CfZohLqkK4FMV5Vfhoph)Et>gCZW`G zM}oyvV|^((SoQ*xA%dSsel}v_osMeG=?x8iG{8rr&0ep%1(?SYy=2$5R_--x+wR@$ z-K<}L>WhuQd*vWcc=cz)cbafHU${b%2eSw31xE(UJ=3(GFCbVyq3lL=NC~un>>i7) z-BO)RW0So3iKJ!fuWU>4=*f|p!gV%FU;YfI#j>`^-;1xT+ zX=kC%lv?+=Qv!+FwV}c0>tIuSJ_`y3zr}d6Dw@0%r61H~6sq+}djiO;=Lz{ZK#F&| zF|&7FELWxicZzmFR7iQ&XjuX_nYzg$}ontOsp)4M?h6@9vh%nzAj#*2s%h#O?| z3N@_>N1v+)TpK5;K8Qs+X<5MI5qD}gUhVpy!+iT3vB_4@`dzd;; zrf@!*4gW65ymfQbHKqVeJp1l_cEQ4f?}KdYg|Xp?J&5d`@*t=uQET^($pp(Sm#H%^ z$1d^~4|P>IS{3hjKc&L9xEDN*@t)9YTk8306!s&o3*w90z$BvU?&TxZd*rrbgZoC5 zp0CE(Pks9Cx1(>#c-eLAXtf3H!hGU6xAO16a1XzD@xJK4?bCoV(9xM&p|J8%>*r6Y zs_I^Jrv0)eBG@jur2h=AHwfIWr_Ig%5f={|y&*W~H?Y_z zjBY-`N;yrK%vHAP87bEuyS3lWlVHVTKTo3Tew_@Rq(IZ7B}%L%X{d+n!tRjAkTz3f zqK~}Kk~lXvx9*}f7Mb%E$8XXQ@dWMdqT;(5lM%vb<++$yfQktC8bqm!oTkY+B~oNa ziK&yC_n1dcIBOr!g765#)M) zfN(yU3dUh2v=hixmvNyHljjmyBcnvU_ft4`G@vho!3V7N3#Ui~k(X_{v*Ufdy*yC+ ze0zjikZXlJd@CIedtV0{vVOk0N3*;Kp53Ps;PBEB*-d^bEyc7ync>7+ss0+#gfb~h zos}rA*O2Ea%1$`$w_xoJ&A8&k8{6Is!N46j6$DLPbD$YBZ@YA=ti1lOj5 zaYU0?-t_XOXnD=vSFcseJN*`O1^96Cj<>;-)}Lg`ici^>KZd9>r}xA3%LtL(JJu8Y zoQOV{Z158fpR%8A9KTgY1J8{W=TAqy2orCpn#^wWw+lA+Zlik_OM14ob{8A@knWq{ z+w`o~)_S%z`m9!p&X*@5Eq>Q=cJ)RJz5iJPslJ4b-NIrME^1d50yLfomNBBZxVUT@ zOK@8opFd{fT)Ac+|{FVaKYZz!-4T@ABUGgV6zF>VH1vM4WL5c>1(zyOrCsH zCPmj2i7?SoX4r-tR3deMP@+Y&c6kn&q0xUFdb)RjqV_o(5w%*XrO;bR6_5JX35UQX zeT?2mFcJiI9{>v0)ko*>KiNj*<(ZZlAEh_hGRGZH6*?sXL}5_OhSs|gy-wIiMn)jM z@mvq$Gr3yVS+$zcDB@jisrFgN9p{Qxf58vM&>vb+*t7p-8Z^uvV$dM-od!9c^dieq zEu}D57gbW}#uTI575b}O7~$3p<&i?w#U$)39f&PE2OZ|!ZarD?x)4Ll`QVjZ#a2o9 zl({<<%?oD+MZM1gS*runp7K3#W%*W9aH4ZV>1M2IeP`59F7kaWbK0jSBoHGoRl$Y`G(4!iUqob9Fgm$GM^x z?xorg8>-go*-N8K_;;!mMRag0hbTBvR%eNXyi~O|mY3|9RC-rzyHi!(EE4lP|I82Y-;7>rHTN&bBQE- zBUeTW6#x+&9DF^fwIq*H@3C#D&6R+BU2crq5FOM_wgFDN7Y&tc{kR~iu$S%>f@Z;D zmSB1vZ9tEiQ+rFe9uM8leROe&ld0Z+G^g8dr6KDoz^zi-!V+$y!eOQh`}j7y-XMLD z%>^U=TEd$sD$zsY;Awnu+O}KUu3elZ+2}O0If|c{z<(@@70cg%xJ%$Qgy}!B`JUc{ zlz_puj#&fP4sUyqHjmlQf+l}w(n?N)HlNAQ<*bo;aLYel%{r*~)06VH-3Hr95l++p zfG5UbQl{xhAa&I|^EId{x(z_QeHnXV4Pm}s_uXEA=b(r@SfLQ~K*q-#e5Gf{Ns#T7gFM=oK z_f-BhWuSlUYg64$uf4LI(^x%+rhUP}=(7;OfA31XX=5@m#;vk*B5K$zgp8{(A$a=2 z=%cU_OE`&cT8NzG)kuZ@?nA99ou1-w6G=r5)~OFMS=LeW?FJ{8`4r(c#Yi}ob78Jk z(uF37$xjaQ#HuX(W?y)o0=I z8|r7fX_+0XjS9U+kPZ5njUGmG#pC#r+5Lg!lvX6j9GG}D{aQKEvLewD%WDmB6VwtO zCS;gJpLCe9cYNjx!m-=2AMzK;OjojWYMo{uRBBw#2WfjB4m8|_zdiq|@28M4n_BZ! z2Hq2WDctU>xf+X5G(AI%WaJST^5sc+4yC9E13?(kw9D@ITL#Hz1Q}u3dc>b{Z@@&Ftl(f@#qR@$q)IQB4gx& zZ*_sqef=h;bqo2~d(ya%Y-QnPTc6qVGdV#qG_O-vx30(Ih6pl|UK}z$SrB-#N`C#6 z_jJ=uLDf(W`HAW!W=-LA@?+fF#8xc9j?Fg_r#@dFsysqy5H@IV0QsqOR6P!kEx1I&{`uO4Axi058G4f_ia%bcm{i>XOIC1|^ z(vuGxGz^1p-{Rj%!60X9xoJgitVxB+$n6jHa)IC}|mnIEaJak>QBLIJ1?|DX+h zx{)r5;tdbCQ!rl)2CO#_jRvdd^y_i+vJTkTx`TAIzI=MDVD-3gSiYL;Jnz5QdF3yd zU%$3`N&kiQyip||8LgBh2F$ZFyPsjB1|6m_!XZRMPKKA8oeqE%+8MSTjyuvJ$OI#q zu>xN_i8*qX7{FA`g6&C(i-3&~8GVCokl%QKIP&v=`kmjL_gK7o&fuA3peo~9FFot7 z;-SR8*O+D13cm+(%*W^8k~3=c7TQZGJCn9_#Hcm0&-s6d#-_`)0-PsJfqzxChRfsrid3$15QPb-<48 zW)eA5RpxR8beNX&+iAP>6%IuvaJ_Z@U#cnQ`i%oNK|%1G%Xm;uQXSf;8m8rTzdw`P z37gBFxZA+~D9B_6I)TXNOfQ3~qm@lXq-ZYgCuWo}Q(DAmi#=KwGX%^QHvTlKB%#aL zHpfaGL(*IHe_CvZIHfZlRJE+|7UL40=ORpHnw|li=UYTC-&{`?Z7wyA=BkRz%js5p zuQPf$enzp!Kmm=~LsS_d%RyUQ@}f&>ME1D#={EVV``raT+tp?ytjyO+;qU zP1JOow!W<+eC;SBVDkE3cJuyyyl&YmB@B8n82%ccRw_sbY^2k@^4R?bd1QRF*kUFk zJAL7=e*))PGY~*NU?NRyvx02Bi@3#=w#Tvlpuwptci`8X{Z~dWCbnHR-;q-B>tht# z0n@~c8*6`+m712ioCP9gFXs=`T9@k*%iS~|A{LAUHa3fXjYvC5b*VSN`!u&R8+W+B z&HyRtlG%e+gI1)&6l8vl08ry>SOu0tniA6LN91e!_~G;s`JB?X2j|GULvyR~pUz=R9GsI(0m_I>gW)P zc{hVl+Kh&XbyQ-r<+DEIp#cZVN7f_Rd!Ul0ro5}#&1rhmKYTvJ+^$fqdX)g3c0*F0 z=&dxYVoSVj^Q%QRK#jFOlW9)=tWA}I_3(U@Q*G@mqd0v|G85;{V@*xpS7|m&koEl z-EQ&!qdwPf!d{5W)LSulf`-LWe{!M=m)pZvK7Q)D^#4Bm>EDu2*fCqc|5sCsA8(H7 zhK#ZRH$i)^Z7%3?)Bag@f$){6Wd-mhay#u>&l!+?iZZyrI?(q}`Yq|j$_`Il2411| zZMGPx;+R>qo5oZ+zoDM2mJZnxII_iTa?bscUZ zfx~3a1S}dQ@>#Rj>;7z_`&Y-R18=nzl6uF3zYE(br@H_IxG|as7^{eup4KwmV)@o>#}gAg=&GR=b`*N~%?*uY7GSvQ4xcGWaOQ^sd{W`dNdiJ^#5)$iQk;aX{m7 z-kI>Ryl6upUWDg%A5hKwP;DNIzkL66@{z%H7@_3pcae3-O^PO&6m1-`OUwOwRviu| z=(zoM6@pH;|SxE`8WY7_t%jUP(gSayObvB9UbxwHdwWVlw^N2H;WD>7& zVfXylMLFi` zI1oHv_8~etI!YCxX&fo*>IOUDGr_D~y59c$19&C~iz*}wO}H%R)> zggsp-gqzcDDNbL8A=0CgwEAgdJG#DaI>-uC5P?-{N@!B?VXGZD=yWHFmY=w9_6beA zRK2;~II0AEsqotBz}>(2(m8!xhE92+&IhB&IhRNpcFD{HZV_X5+_uJ^WU%K?p;zyY zWsJO#$nnl-)vJH@6pnV?aN(_P)R^zC$8e`BhoWx)QxxBi8T01o{5~oiTVR)s@1~o5 z;oCbq=f8SUHo1`TIBmNisIpy2y-u644(iGA^vV9KRf#1P5*$PrxM11DGiGedA92eK zDHYnNwhADTZ~84EG;kkIV43v8flCa?usZL+xHS!$`)=VfEUn37v>IGPdpgrtI;;M*bL%of_ z@!9lvJWdMQ&q@dQy?{B31kZ7j?^&5%aUq_d=a#QrPF*_Xikbv7a)XC~>iI16@$4%R znmttCmc!<`yAOLI@V_nw>}e$$Twv2sX=3R$7i~QTto6Q;-KDC<;VGh85c<9#6=Lro zcq33t)EnB~W%J@5I@gIUQRPb?t=?NIjz{9g)o%z@pSHq5AYUY%T9rA8(`+ZF%gOFeu+8VoxD5ou{N;0o4oC?#2`KvqP)lPoIkS zI6DHlL=<-S(AtY{D-0gL=M_*P!p5L~sxT>`purhQM!+LBR(;^0ZEYBTk*W*~Uj4M& z(U|#pFlIYgDTz^6l%B+~7B_-7aa3hGg^&eZ##c=dLk<^*!x-Lbw0yge$zdm%|7X?*cT%mfp&=I9BRy7PJclo^co1;1I< z-&b6w&Z(|2+bOlQ=S72~iTdqa7f7P=td*%JGx5vo@qOr#+We`EdYh$r~2NzCnYe}$>wy&W&nbUuIw*>p7}!+kMCw&YGL%0=~tR$zS? zRuH0VoT0!8V2JOrC~!PWR^0zw!=X4)>QU$Z0=D$-660Yq=sCxqBAfXY&mL#8v!rk- z?*aeepf5M5*#7QYkF%<=8Y6P%uT%;1_AZ1-So?du2m8kY_-v1B55n|BWYR2q27=om z8+RPdc5L3FFpnzRg3+e#pQEy4WqRX}oU0}t;xs;5hHww^0twIY93Ak#rMFO2BOjs4 z@gD1VAQECp1Iri>0AKua*bQu29)j`SvDlpsia9#Et$qD3T9=KzvNW@cMo|!BS-h!( zgkDhq!I#WJnbbF*m6UqcyMxmBJXl_OjPP+1}umncv zU(UG>i>a8HN;_Q`U(aeSt;U-9$sxvTzypJXBdw_TFm}Hl4XCkRY>HP91b;hB$K`Q3 zncTk1<#(NU$@%|@&0YfsxX-Gp{T~$gggbqo?yjr+uBy2Oxi{mIas)qpq*Gt?dj`<&>}OaUHA{c7WzUN2~3;iBt-!;mXS{ zmYwe5eotN`=R%M^1NeaOhX#d4pWpp_@w!8wKWoSe8=jwL|C#dDdurP((ENt43%Tzp z6@PGy7pQKtaCrR9;=e!q&jlNn=9vkw@GC^R(R1EFIH#f8Mh4+s(DK5HeBzMJye|N2(a|H{Pd=J{DtNSY});m2)# zO8VIG?Fv9vXEU7_aA{VE;fC}ohGTLTk59-{0A#;zh-Cb$Cv4wl9_bceP!U_VmP6Et}1iO>=a2_e{vj8b45$>LcJDNnDvsap zM#)Gsr9Z|-n@J8!v10LajS>c5dm9rXl!AhSh{))N)gR0$7HDqUs40b8lYbtT#%vxZ14AT3WI^KjW(Z z2@{nPbQ+O|bJT3Vj}DnS<1|OTUcOmR)2nCjI4x!Upvmum&ZrPc(b{FDv~YeR;eB@! z5LneLdDFq&#&MzRE5#Do6!cqt_}t!Mmh6@ru4zmcbge7$3%}sWDJXP{M}EumdlckW zY7=wY3Znt8O2tD%OtBk@!>b)v@%iF~HMs2t=q104_OkFzmjP!{D0GL9H9C^?Z;O@I zzx?7G2kHCub9QF+Y4$DgpsCeynXyt4h%e{H>6ja7;?>36A)J~gG;MRYxU{4y++eb6 z{WeHy0gDpqBw9SBTq`JP=!E2WhiKYAoj~zDG-pG=uV3Gah``?%{@N*D@rj6=(z~+K zK3F!cIE0xlJtYEQr6gkvmTKm(*gP{@^QVV|Bdz`7VLO?R9VFbB+{aM$w$ap-&}6rQ zpT;UT8I4zwZG*G<6_N3PY$Wu+)D|6hYT{@%p22rrikH8~Nj38nYrYIcgQ_;0th z^cX;M$D@#F{{=Qffv096cikp_E;YF!Ss|2vz?>J#!gEmQ6W?bzP&pR|V_L5{hHWq$1lWTgHerwlJeqcvHoX7#CPX|DJ-4h|8{N` zY*X74!&SSV8|{u~3{EQu%BiTR1k7a-^x~>b;Q5a~B!-n6I2!x-H5@CLywVCZFzS4q z9WKj(rgVhUBssdQ3)kR}0t<@iwp#)l2Wo|Y48f(uXY?T$ z&Dz(>TNGo-R7)&p3NzcRvFcSoEtvp;Rn>I?W=p4=l~cJ+Ms)cI~-I7HqrZMdTN+=PrM>Z(H4 z(-2?Kntv$V`NLVBQ6d8({yl0V@PZ%YVz(JwY2(=8Y}cRRH2A*8hrE&Bis{l%@S>B#609oO&koH&rpGWMo8o#A)3H0K~dyC))%K<2Msfk@&$Me6j(*+bmZgQLAcct@`cunU>N zhfYFyNXLQ38>_AH`KsIPPQwBD-7)@Q5_fn+eAe`>`kdD5W z3+NXgmv;`kDtet1Zyy$9>e|0B8N78MwInz-6DzKL z>tW_-e;~GcZ%v~fHez6^@6KXn<63-8IT6Z`G zxA}pbI?H4Rl>KLmaj6=LMYORozM0YsykOrR@(}*?9*G=2StuebbJv%Fh5{`_oRw6b zRn5MNtbvCd!NE}svy0OTQ^`jML`DG5L}y5+Tr5wANgq+aX1})K?^Zn>r|#}<=huJD zZyNn?z$Effg&2;T=#7V$SDoz&kLT^_`0+v=0=jg}lP8MdPMd`)CwA!QZ~}ov7bWJi zZ{XN*rljrlgqT^M3HlG@%Q4@ZLZM(v7kGByjI<_zO1~e?doZ-^UP!bJGCfq8rjnD< z?}Gh-hhL;(`Kc{^xqn??{0#C7j_|LV)i$OrzrNKQznN=iDz?KSCvz2!@|_90NJ8`J zq7PiXw0p`AkT=C5!2|5)qxr7AjXZKRJ%>Rx31;W`bE7cin$PbL?au%g zQ(wUahlhtB&({RKA_W##ln!4J6qzm3=K48e&xq%At~7D$Rvnm&GI(ALkAT$ihRdBV z=zSv$hJg;PMsuA8(o&(1M@iYsxXnt)#R>Bq1%P{#vzBT)62``I`)fQzgAnPWYS7o{ zW2xq`yu=2G!-mo~R1Ho{<8SAvIlQmoQEpELTFUv0Mo179 z%MZ*=nR?VUnaYD-)rhYUzg+|bgUlH*K2Z?enT`riRg}KXOqJ1kEj?#RQT8TFuzi8V zPL0B1XK)u_R6F%Zf`P%L!i+Y$lX&>@yl%g0tB8|x6)o#|ql>(!&(q+~RC?ci5&a;E zhy7_?ykEoO0sQpQy}n%jm#Lo!47X-^xT)?z^~~ek(u$2i3lpT5B$i(0@zSO5YR*=Cmj z*j#H-%RBvDdIyS&rCJ}<#V|`>-~6s#SzLms~tkraOvhZa$tX zO2~tYQr^wn*FC2qw9ysQRJAm;VH53m*?4oF;Xm8U7tJ@7+Znn~VNO@G=Oy=?2Xd}y zJU;*nNb$9Xi_9}8*TxlaCYbFlOG+o7+-xa_U1eVn8~LPV54t1 z9<;TrGRysjI`AKN;sN~Bn}exhQc_ZJcHiA)(VC)kP}{<^gC1~?uMIdACn4_oRyxUs zSM!TQ83Iv@xX=mN0+&GNL@11^&X2-2qDaFNOtskw^HmFLr3yQ{g&lsPE@9>Ejc-bmu@JQXH}S`dRA0FT;lg43s^l8Xv?x& zgSN@{Gm7+y=K?7%S9heYxUkw8_>=H7{XPTFnIb7jHS64w%dR-S22Ndy6cV1IHh1Zg zhO)c-65NF?gfd6duV&)+Ze*JQoLmG9eHKlnKC+n9aUrc`3Mc|_7NRv=<*kaW4w4^ua z6fS*qt49L)5(BM7&Z%!{D-tulG#`)(kw>Up(dCOG$Pps)S8omwa31Gcyx$dOv;_ov zmFT8S&fJi&UvyR!aj)SOFr{>)%vfl+E7r-s-}kg2L*`fO+{~5!w!aJ92RZS^W3?ET zHJ%R@HWb>*A8eBo4E2S&9&Q6+2fLyNk8OtX;xd_^QnwS#2W$#EvTtQ7TpTpQ`Z#11 zn}=?}w_R^Zt_Vy{N+Q>Nb}p8B*LdSmm*1J^%@JQZtBdXhU;pko^So<}g3xf<`XpjM z(y(UDk=KFmMFXf&u|&bzE_5Y-816g?Z7dm&=ZCFjYv~*v!?ozP|o5$)BCs7?SiyJuzn< zf*x_W3;;l+M0C1A{brO=B?6=x0eQTdRStceolTC=WqXo}j;%^#=fP2UxZ>W^{f<+` z;X>l}zQ{mX8OH~Gx(05&_Jx`7Am3RtnZKz#p`xLwO`-q$6$sq{alDduRNJuZMWYk^ zG2p7{xR9C@tY56eYphvTV`yTVoS%sIKI?X|cZs49qL~5xGgv9|iC4dqPc&(UM4KH_ z$gIP6Lr7G#(Ov9qX20@n{xd3^xOl1mBxF8YCne&^AgReyhX(|NcQ=wtkMr>l{q6T7 zlY&bqRbckD+5jSo8y`#P_WRySxI_}eVUx$2X0}om8R(QuG=I}cG)m!5sr?q}lkV_f zCYQtb?q2TtnR2?lDfc@1s8MsWW+GE5g__r+H3GMzXAkb)#^iYeET;EDsGV^Y%TKHv zWdjqGnx1`|c(Pjl06^~?hcQiROX1a`?H%jZM8$D`oqZa`@F&!*OFYSl31~_+I+#7D zzClWQcj`c6Anwk_;A*H%p##2i^bQJSi30L5d)5B=VVx!#;Q^GJfj3R{U~ePgqgcFO5d4uCH5V>-!tvTy3<(U)WTZDh`y#nRE#D4SA0cn0VblN z9h`J+)r@APnf+qQ6giS6ZawE-h4wpuGq&?ukcBef8OYFp$Q7xic0C4Y=J4`5?Xx$2 z3box$iX`78^$#-xB2-%uP79x@YinsV_PQgpihTk0HG>tT(cE&7k{P>pp|C=B5$g>m za;H62k*nAK3(%*acQ$Mfn!+1vNJMxdob_VG)Az$aN7i5Vg-}9L5@7!0B|QBX1_E8H zG8k+$X$tT<@hng_vA4kHg0-x^mYSZ(akE?6ytF|T9+J%4dA7{c-k!O00Zl;$VagaW zWO+C;2hCz`?D)?S@fu_nvCxk6-;jep4l4Nv_kYd7)utn^~EiCKz%!0Awm& z^+?NnF9f`)sb_cECoR?E2$a(IdHV*gz3M@Uc>E-&lb5fc-9XvDx*D!n6jv5W$Wox+ z>RYN=v+-^IW?aAEV4}3Jm{cn?*(-*AecO4mbv9Pf3gU1NE@MpRh;P5m+j2VZ6u6x& z^yY@j)9W+ax@4{=yP=Ndg3&?@m$jSm?zvq^iwS*V66>}-_*75(w+tAkJ|f!Pi>}W2 zj|=qQh^(M^tvmC~LO60qv`ctY{i&z`ZX>j>XVS^>gg1Y_@@XuSWZCz<;d~tfa}pT} zU9yd+kE`*^9Odup>2Dv&Eygz(*}^?2dJ7Rrm~m8GNiKbVpC}0-RUvL!i3PAF5RUyE ze|dC^%!3HLF_Lg85qoPJYP{8SdvR2Wml1kVYW2)kmBOTdD^mDZjg;063Nr0(-d4%Z z^u?aGfP;da=PBu6Uq8e2*@7TSsdv(yoh=Q+2)7aKeRDsycC@6Xvl7q-F2cTS9#S5y zNs2ser;&ILMoge7^Vso4dOwm?7% zR$s6G>_WGRnPQTTKzKG%eKG`Kxi2$Wb{S4bb^Rk()|SsQ&TT5dC7uR0_?qsQe2?H{ z3VQm>y^qd5!O3Zz!_o=UpB+ZEHWgcZEAe7&b_YLa_Z(NS(`@i4!4GIuTF=Xe?H_(ADdD@D>}bMWG53G@Tu(4Dn|Du>Osm*k@x@x)Q<`#IF0SHc zy0XK8gEv#s!?K3XBgqyjytzMuLAdo*r_WV(etcdQHPM_brimf}r5?rJb!3c|9U}i4 zi}>R6ljvae+Is8yb+psC+)~W_#!p(?4vOUs(kHwh5*O`!+BROn#s=qtDz&fhl0bGl zQrJnddtD>K6x|MDd%|VyW!ty1>2<06sDiR42Ug#kcSOj^`9A8&LG+%HdxlID7>c9l zZ3=FvSD@W)Lw%jZklF9g@qGyG)(0ao-?%lpY+W|f5qD9@1wZBfQG1Lzarn`UX+%gf z*o%O$l0hU}%psP%c+XFjnFTR}kP-fo-K_f&me zcZez*>A_ArYJ575RBA-bHzaD*jZdrlheVs0nAq6jUhupAfESKRz^5N!sv7}#ai$c?Cuw8Qk4&k77tJZ4zI zR;=#9Pc$t|^!tJKb_mxMBo;zi5F(4>#E8LCy31sdpp|o}3 zCtO%V2~jRl*~-sJ-<6QqqnE#D^5bc+MC&K?IWsYR9rr0kDvhCnEswxJ2RAVD`mQ?L zSW{zTruMaD!5MS|NQmcskK(HV3a@qQ{V~&OU(tT>?QNtGRZ68hZI%An`g-#D@$#;5 zO0x8?YB#Xhc~(Vr)gqpEowFdR`p`zI*`}^{qsd66VSNAq;*pvREZk+fH&yQNG!sw> z9$NFn)6BuFuBsS z-&_hlxGclfrRc-)0_Us|lz-w$GS#Aw;&q#x4SS-9#oxWd-+pb&#ZA6-z`97jXu@C9 z?`X!BzhFR*s>V1WM0j8|EH-M|uXTb1E0u1!3H{K>n*tr!Dmp3kq{3M&F3kcZIuWON z#3wGLH=yCMSC*n4al5+)1lH&W(xcetd!S#%`hj8|A!+ zuanvQ;-ag|d3fUV@l1O&39s6B4m7w7`kzdLHHGbgagkSc6D z_A}!Ow7bMsrqGc*-%J~IUQou^!+(cAGQNAW1z5Z?f7Gv7!R{vBi9Xzpf?WTD9NhW~ z`Ha)|^RHKBGkne5(7ku}Qz8%A`{$A=KthXG$Cb$9EIMsQ@kC?gx^fXkKB72Jqws39 z^**`g(~9;@J#X<0;a?g+X#+Iy$$?yHDci8lB2GxI?FtqyUyDgD$e%YZnBT18CS3~k z;Mu{so-B9MmB%*^9hU|g5Dm|L z`%f~1JkApN&CfCXU$i=&UZ|@ITl?D`13mKYNwVCb4lTQlafGDq>uNSD9D#Jn>_ONe zx1m%P2zsCGCW3JQizX_)2pGt=zA$C-{bN5c*Rq)&XP~)&Gu9{!3pkulC2(=1?0hY- zSt)()|CD1}6_Ev*HSkDps*L_L>n2IFm%^ay;<~{G!{7o}t6tE@>e9$5(`b_=o9zZt zD8-IA^7Q+!d!d>vn=nF2X1?^TRSMC^3$LvWjMJywEpCzpoHjMa*xsm?;V74g8Dacc&Mnuf|xVRln!1YrA33i-_(9x^`)V z*WmXF|NK9Fy&5ywwHCAxc(&aIcNS*B=hqlHLGqSCJ7tFjx6o#^{3{3vq9iy{r4rsRs;3>*I&Y<;H2mWxA5;3 zXZ63SZaM(I2}L;blA?=4PDh?L$%5dQXQx@`{zk}9!II&h%n?ppuHr*Wi6d;dLnVJWq^?L0`IZ|-E(9dnfyV3f?H_BQ04V@p=N?i8P|LiUBI2rz<_{&Oo?X)0QaAx=q zu&JhHBca+UPu8Fg&rsk0H36!r-JN!*eVAv}Pm+yavS z`3I4Q^)0=tdK6z(RTV`cOb*WulKb5_d{ZfQT(IgB@{J0m-2-O2J-#Pa%Ea>Rp_wwF zAv2yP?Bxj%w&PNJ7`&e@b72Ai?fgTYmp)llIx8dI@ZBiTVUZBk!0-a~+1RPx>y|U5 zUH}uh`}LYpvG$+kC4M&x52U=NrhbNt>)6dbI6S;jR=;S}9f2<{Dd}`|Vu6ZHE}P7( z>%LpndvI{@X2Lg#D#4T?_TVyvnk&P86$SkM@|mJw3SuV17|pMv1qwQc@KSkX2BvUm z_|GGZ2X(q*mW6Z~1`KVrIhz?Ha;0OAS64dTKmHCyO$bS5QQIRJym6%ORjO$`jl=oK zX|7{k`?ngPPhUL%x&PNMb5u-{uC@SV2R9b2^yT^`U&zNNcnA-~Ig3RwFG8)=)J7+^ zu*uWgHa*B`@MB1gs-;jfAJ6(<}q$72lAUse0ImB;H>e`Ik$W|{TG4YB2${IGN zt;ys$`b;el`=`~!m%6w>3r)Y4#y<-%tk3j0{U;1;Y+7dKn9)2*beBw}6 zVDm`8^FOlLJ~Go520Lb>V`Ap_HdZsrzu+65@)~n$sU@D4rNz?cX5KpHc)^%e8@yTH z=*a96nhzL80t4V{j3(Q3Bg&dn0IbpKf+k#{HFSta8+NBISt|D)9ur=%N#V461v=ZM zDRQ>BI@|6}s}O2qA6jMB|F7of!oeXD3L4tl-ri7?%g(O_5O)o5WSIMNR7}hUT*>mR zrpg82bv@q4L)^QwwUKa(w+e%ez*SG9-u4ANgS!r=Z031i9$}Dj?JArP3S|z6Fl^Fr zRBZY%M|H9D`Z6X4X)dq8-gmNZMjJ6{2Vcgj=Qa&dkAD}r3HEA|&tA{7VQ!P3`z3WZ zH-XoCr@^Nn^C20YlDdI5>Xc{1mC2*6y9(^j)+Em)y>m{EF>o`Uym_~~NE%sKh~@+P z72X@SXOf@4?ARF5==s+SUY5{1zf4U@>4Oi~d=cyck6G{hEa`Py^68Bt2Dq<9b0ZLO z+mc0-a3XzylT9#)*+0Ttdc@|4g^o3Qog=9jW%9!Y8kSeAeN&nfr`B>*66}-C&sv>} z4Lc!Nsf2sDscoqrLMPKka2b8VgVnB<_v!lD1k>M zocCrEO~mi=P+^IH@tcZ|QkKhhplZy@<;n{>ua9Uy4X(l5%o z85h;xG`@I5_nr8@S*-zsn*91dCrjXDkS zxCW4?sPl_KGWhfvpaPKvzV9&@v|elMjN`#jM`-CN@jqlE{%HSG1^|fl>eVaty&kxE zFc{5!r;!wFNOZ~4kTMjc4J&FN!#=Bg!x{N#$TwN_e@C$~*cY%&C`ZY6`;{;la z^=np*QlM%ZxV@4=K}Rn%?1b7Q7-R{>*uckevO65I$B!6khJ)F!cTc6J_Bv!JWPGK~ zFyJ5IK1!vBae&hd*DoWUOKdozGeYJ%N!XiQo}D5O^KXrs6?$L3ZTfliW?ojSGg@td z>f&UBT^wb)ZHsc~QI?S2Lgh)VqQk1KG_D8%YLj z1-j?+g8nb|h-;<#P3pgYxhN#$etq#A&Kvr39bqdi|3kfg+n%TYZWIbnKm(?HOsqs@ zUJDAvw2v=jl3IOUtJNmaf#E*vOjQrDa%ZDdvw(7LYpz{e+iB~o7q9=ygD!1{Tx#*I zf5dyXl?GE3nW)B@JJuVNi0u`k4`YA0frAb+;Z`r!v-|dO6S7&=FCWGVwFC!*ueiuf z2NIsUrTnn0E%(X!Gh)i4ddCH1Y1$8h)7btJ_{f*$3$8j#Fax7l&neL7R`T(7Sh_+r zA9MPc9K*P<_yU{?rer_7YaIhVrxxNVo;=oz@@^t$N{%$1n{+x!NM6|kh5q3JXk@NH ziZAj6*r1>tmH#%1zR}e=e|rQHv3cwVB=(_V061A8*^=8bF8ZYfEbXpC=63X8j?A5u zOuPxm7Pb@2u$b!^$r>g1`-DANse_GxYmx&KRsNn+Mo8Q+;nMDAs5R-Hm0GS}-zmtY zs?`+-r_tztfujVcd$xKW7&>w#t2grFEl(CaeXoGER03@9twYrW(_dNm*-Qz3V$>gq z%%*6G<4Tw^n%y<1F*MvvrYb(U@+8zh|E9d$9A9s@7E%nfUy+Bk%egmRdAv%68mvrU z3N&^;GNlK&X_#8hqF0yn#{{=JNy-waluHkGLf@o=iWz89DX>BQD!#MH-S?`eyo73c z#0i3sx-mBgdg4v;8pWn47R8ku3|sXiPd#<0#5>nLJrG=7SPD#6E88}l4ftxR5YmT2 zEHpJ5{U&D|gQPnlge9)%X1=_M#>F%9SACB=NI015{smRuK19!v;4mcF$9Fh`_v6Rs zVBHE)Iu;|;`t=?KZhu3_CBzJPdnnuF)12(lY)N217@3cUrGvne(tN;!EufM`-T-N1WlT*!C8ekpb4b5GY18KHgs+p5+{S)bcGr$ISAK!Z06`RZc!r)^O$M zIf1PLa3jx9T&MdQf3ntl51w1`(I^NXXJ@TpuM}vmnzD{C-mD-R9(2NtFL)GEPoZ{8 znsR#U?Yg%pR{PeD#AR2j^xxPJssGcbXAjtLwL9_&Hn#Bj1-u4uYwYX$oQ}R4`lKV1 zEx~Aud#D;l`U^tT1uGOKKwoSLggwVqz9ZFxJq~+=!tAWvd02x>v~$3IX~=M1^XCixPv19Rj@*9gOIl0K za73DZqt2VszEHyUrJ9s5V;MhVZpeaX=4{7yKg}yGYVFY5vl17k*1zLpU$qtb0z*H& zBQ7d~&8r<6)Nq`%EuaB;!OvXBxX}4)NqIW`*UL^M>-~vh{VbqeiPYz8#9@pApBIqt zJ{DwBuF#KQy~Xbq#63mnfknSiO$v2SX8Zm05t4&AORx?Wzft=N?;&A$-!>O%7gAQ7 zGfF~{Lb~Ad1O5Mn7(E)7CjS~Hoc>Uz`}d9*WIWG8J4N6GCQVJvuI}y?dmeaUp+TIT zUfFlT_oyMp^>fAQJ;~UhkB25Tj-O8*sA%Q}-F%LSfy0D)Ty5>#!Zh&)fq+9Eid}nn z;S#rl3vP1Uv54i^u!g6fYZ%2U84p_<)ZjWv4CC@;Qp)#8m;r6WXEdmchvF!t5Af0@ zQ)qY%23uXr+{&nGi;=!*Ujqv{FsxyNDl4Vrc;3nC$P-z()f@J%U##FoT{=?;7wT_x z&ny%XDkjbHx~*tbM}n7dkUQhLtE@>y?)EQ83Br4EJz$Z7jIvd2)jZX-;Y_qnqGBxA z$g*glI`Y-_E!r?{M0Kv6I$FH+-Hu>P3htMHpltQs^_6UZ{kqFs;I0*~CcqAG&cxFW zE7V0n^-}Kcg=g}DPC-h%iTAM@;^O6$kU`=ZOLzAw-yJMpz1lQQbia=WbsDy|8d-YJ z7DF_T9XtBIjD6jjDsgDRo>eZbjr|hJ1GDby^0*VES6@p9rdO>tz6MYA zQN?mYOy(lQx=fJ2il*>X<7?~1n7ErF_bt~@#7haZ=Hy%Bw@~1BhtPI72KEdpdwUJK z^p_J5q*@S{YIsDIAxsx5&HAQNBt?hlrdM7jZZHy&v}g2Mj?SRl7aSOO1XGZs)m!&% zC+BGpyznPop7zT)-6GuodDpU0>i3lwj|3{-=*5*u8HBFvz#~?k_f2&xy8es?LC#%Ja}jLM4}Um2$P@H>WU_gkJpO+r?pT$zIF^1c~RBZ1u=1Wk}i zW0p68dT*(hbBJk?8B5E))*lw`8wkR(XBiTc^U9coWhja|cu5GR%xY~kWg+;i& ze3KHud5%-FU;^8BW}kI`e)A}0=EnCWss9g&obM+9)`J`%gYY6>Mj*o-2O62_8@EyR z*s-r+CqWCv%(zOjjKk^LY3?cHEMzUqo?hJ%^ZGOLSxoU}Tx^kN2i}9k@_Gj!Bi9AA zNihQ%&_09%$SO0sfLc0^FY{OAZ?*I)>G0XBd6d5tq`W^L67UJpfTRBwOtABQei%G1 zQ<{M#%|H`gfJs_od`=!i7HxdKsIjrGR|7}?sHlP<7aE>5l#QzC49+CJe**2N^RGN6 zl@$boEz$%|CzREl*J7tAOm|1n+Bn)ds&wS%b+E|op7*Uq2OTf9ImWqjeL8U}g!f*j z#E=RX95$o7p=fzrK>G5ScVh z(~AN~I3DuM9yM=%z(n3z~9n=xumPR_fQEkaVP;{gS)HfRT{(_IILN3I8N2q!w- z^y`h8w__pMpa8*23~X+mIg4MA=Ca#^x|xF!8+Y0FPKF*=V@-_yk@{*hIGcgH`Qj~6 zL*XwmQf3_ZC?votgQx&e_}`Sjxz6>z2oj@iM#Ny=Y;?CYa-A^rTtM|c7E`&XY58pDrE_iAc-{+ z|7AeKnO69g@I`~qpvHKFNw^yZ>~2XY^?VbtIL?imy@7z7WuclHnx4pVA(13qjn@`Z zL2A%2x?M0ZAD<;|c_}kbZtZWnoP7NhFea)@`sT#v- zHJbF97U0vRLqa?C0vMeDTGyhk%{*wBUGUvxI?6U3!&gKJT9iq==Dj|jB=6zw` z&cKpg1c^hDi#}Fkpa?W_q6@SpW%^QjJkQTmxu2s?x;T7=Kg;JXL_dYTA1_s58N%}b5m7Nm}+ zIxC@K!GES}Rml&qKR;{VSm}rRgfgK&JBH+F5&uR2rACQbOErvYZRZY~7VMWNR5{AY zx|H{J#!QdsL%x5Ls5lUHmKe~Nncb3QzXd;bMIN;F&8UsLpIiS}nbw6tC#e|>_ot`2 z{u8ac9F2=It^o(N0MjSRacG_fg>mGJm!2CJ63( zDcydg}@e42{@Vf`1t!JTI=OC>lT=fBtg6=V0ie#_3ukoZWOApVm#xZ(YOKMj9( z*?Xyt7T>zB{r*1Ot=fC@DOeC33Cf74GsS0d-f8>PU04+X`gcQzxyw1!_$9jxh<`tF zv1U2ynrEvB9m_q#?8rdGT7B@+;V~P$c-O62BNa`9&yRO5KBHUrZy5dI!EP||wO^)w z`eZM6R?8SkYy~{}F(LU2XD0LTc9CvbCSe`k=0&PE4P_PsqqB{v=ye&)Gn;8^ndgAl zZNZa}Vz-Z8Mev=jTlb(zoxER`kfPxQDQd`y#D~4Jd4AVdXn+d4-(0LelW;VmY8_zW z?98EDAltsu(3BYjm>OE`Pwm_y7)?C%Sz}%b32{TSE-lLq@|VPXvAr_k)3UU-#vVXHp6f%l`bkhgZiv z{GGnZ?nuJ%3Vq?pQcWk-v+I*&Iswidi|vrf6=+u8(^@X4b8n;g<}J2EC%qSA$#b43 ztNXqB#;X@XDz5xqs!sv@m&Qb>AvhhMer_3LncRJ`4-|B4qTe$m8Qm;X&0rr{x*ccC1vi6$W!|=g%sSG_P746TzNbnUZEd z>N7;7mbSa0;`P4k1MMsH2oZ{2yO<|R{WBWFH;X4z z4mb$Wfxg?6E}8yrftSuzVW5)FJuQQ?=tZ5dS+<;<5-ht5TuU#6EjMyn__Z(#6Ysd+ z#jG$s_Whj7wxdA`$b^%~^1%H5!KRv@)D!alNp>O9U%R3GH``@J31C4t;!NHL_Ov@3 z|LnW@sOq0{Mfpy7H%{5!o^^3?u~fT0X0h1|;B{s#DkjF_)oiL+8l~I)iG{qt*8D|_ zuL;*7aFrNv?-CDCCR#W)B~gue}us}oBtf^#SJ zMm&`~_}M_dn`I|2ca!cuLVc(B#)4>+`45jrBpv2ITvzW@A2amDb+s!$n_oA(b1??o zV4C}$H7=LS3VKRgne6vC+>r11&vGj+vBYlfnOaOX;ez21gpOdy3Hkl;pryppeEPko zFk}pt^(7}ZuGi?FQxw-hH;EvAN`+Wyi9*Hxo)vDS=QCtd)$G}axkfF2PKt2a&PiD%D!9M(z$<)@VY#u z3&_y=vX48=p?RrcH4UQzo>Bz_K8bY0y*5>l_w^9Q9u`X4bXKut-yKUY`| ztxeb9cf-iM9hrkUo>$FOtx!W!&xeV=hIx9>9L}|DHugxSdb=^ui(?sEC<(C@{-tn8 zLDoF}$>G@kcD}FBP@6%!fB(s5%g^^;PY@SKKl!%$eKPZ43R_T7QITkwQ%&QcRA}ho z*}?0%RJkNT-rlxcNxROa)! zyg$9~8#?-&UNk&X%ZZ9>-YYAUb@guBVFTWQt*KJvJ3Vny zPVh&(E5pB|AITH;62`24t^IHvfM(&msF(x=#R9v>Hj7$L9QXt;ID44^IBI`WE$}z_ ztPy({{qTMw@I<51`YBUM{Y$VB&(2vzo=7*Uv(*HC=bNj>P~!9+$EBU#FmN&FN+L3A zys@Ux);r&?jOVV!A$E<Rt?c7cP zN%p(xsWP1efyo8AR;{we`ew8siG`u@JF14S1uR=TXP-P>4JVmfZVVo^L{k;*rgyhf z_|f9{%_h0_%IdT=M|)Nt+|Phu7X<^Y(QDb%agDrGpvk9a6?X6a>7YqOhZbDXP(jzD!OFCWY2Hu1EE73JbPu=dG@GH86#qN9lEzP8H(#Vz#A%n!)!Mh%wNgdoiYB@?!{ws&d^#xzg+2i`9WWA(u!my>VIvuZo>!1b!BHw zr=&nP{{1kT_NUWq>4P&1@K}5t84V+2)bjH3u5C!4%{8AbGi`ybO0m$Bb#BRv1} zXogGlNRKt$(dGDG;yw~EcIAHiPZbwGV*Z^7_^9S}h3o&#QJClccV8v(^%jS| zebm1IEleOvKtKTDF$QD2^AetaQIQnTJc06b0JkwRop%$yDGL*%VqCT3o)*~IOjO4BFzDgJ zbWGDBl9~-`b`RL;s__~YJ>wvfkZO6}uXn>XDYwvt-*)WHp$29HiZQGycn?%&DPIh! zr4`udqJuUmor3JWiT} z55er6(S^IyM-#`l#6%k6PKZ8MfUKZ>XHLbM}$+(HizG(izgRT$mI!%o-H?eGPkg)tJ6|f zT)?FLWXpU^toI^xTpF|AF4ACXpnp*3nyNQIUC zulMt^bWcYh#Yw+DnAbpCylqY8I6)&Hu!MoAnse3B=zf-FnSm%Ms>t;CtlHU!j+sW9qW?%r*BhR(C5e`?{0xg(Z< z0GOt{@R*g_xH%v!q%1c^n~&hkwTWEKxO2r5zV!{UYEUe7a0|iq2pF@Kj+X8-rm{DPXiNVT#|w(>L8s@(ZgD9IZjY4iZ9usfky`bRVzq`?S+fqxit{AqdFjpb3XClXp2+YF7t$Z{ zkobPZU|=tNU%Vy}GJ7}Rk88^Wd-~?fbGT7>rH&duN|^hq@)0tL_edXpaeZuepctjvLcj8bZMq4PyNq)L)Jg{0vqDwbf1Q1KRSlxRL35N--cFx$XDOxZE(enkJGLnXzu6GK{ z9C(kgO<+2%ZwFgFXZnSZr0gw_PXV|uQ-HDg6fJv4*qQnCHE@%?sje2rSLyhJ8mQGF znC-@DU$I_t-orT&>nL!`1@=6oZy?C+KyNO(@1Gl7n#labg(wcw=qn@~7+w4N?CFq0 zhvS{Tk@d2t4L7RUISUF5;eI$Rv_d+aw#GwdtaD5#qQ^&{hGT!8`=yd`jFX_ z!|CwIQdlVn>2~hmE^y?cP!U_EKsoN`FV*xrJpGLk^+n^ZCZ$1h!h@wv_oO_LwRJD( zVQXb#_9kpue7@#__jPlfKE7qJN^j!!XEQYIkj$?V$A#t}x)3cBscq-#!Jsua%BJKl z7EfT$^+ora;dep6t=WlhIt1#-pWpj>XsgxzjPlFuJy6ifSE2)exXyw1caa(l1npN- zmIQlj;|I;yL?5OSKCg)y`|0;r2MK-GKOxwY;YCuVTf4(t_J%5ZG?V*W3m7!j_ z{VwhlZrky^P)rY>^2#V~xFNMan{XoO^C0Xq2aef*-gF_6wt?6gXP%HB&(K@#1pLc7 z5XcWXjV`Ydde>HNPPjNSe65`374%;PXd_F5UhERF-4rGP8PQ(5K!=3WSY2%0WC|Js zrk>2XJdF^NQ-!5hr6`Yn_~ub=a-9X}T9SC_fv$`wN`fC$?8QrNQ<(4O*)4&|lf2yq zs(^iktm7~i^CfuJ3)s@0b#V`-x5UHldHP$~ULW!Hih#M*yk@POPoA_Tv^G4;MBwpn z4@PQXwx;c zhEmy7m4Wor%2450y;nK8ei36uu8gC};Z3+=qI~VTR~Ys4TqKxpkq)1nsl=MXTW0qo zOjp6{a?G9z12S^%ce?oVHr6yb-vt?C1Kip`-uvT(O8$Si0EYs0V8G`iA=F_T;?Bxi zVsLd39@cc_Z2XjQn+aFIP}0SjNnI@Fu!_3}7HOLGW=X7ijioh960=`iam}@&;1F&d zT|H+&WV$4q`M!gwCx($y8D&GyuJK~JvhOnB5HL@{OwaYnWs#C-* z(YC7|XK+jsL*5tztAW}>sPCQV@2C|}ON}qbzV2EJr0`|+L=iRwql^4R?;O*g_G`op zXbFe(WWw!>6khi?l;tqQ^m-O%vh{;T_cYJ1G2-KMZgFIbzE8cgU-dDe+pu4bx%b)o zZgE7V>LX`KmxN!X2NC9YFYOB`&*33+fnz-ud${h@C(o*I>X)0o6Q4XD5#UWv_N)M2 z-j%Ch%dHjHTbCpPu49c4ZDgh)bq{^am>|gI~ z*=&Q}I2UaX?Sa$(z+A`k=K?}RnyG$D4~S}AetqCAJ3AKH0$D=fE8xI)m_lfZ;6;6j z*eU`SmAtL|Gq2zj6jt|%d@s+l#rFuJ=`xzEi;SiDFkpQZ zw`!~n$HZE7HWEWdi#%E^RoCmMvRpr8f7Dd`Fj#maM8=nTVH^F1%qi z$>Av|dT%HG>hp>#<}#O>Qzt?+HEAdO=9u(yH8?f6ue-ua7h3_(}Brf!lqViU%n* z`JDGAjvnfdKEtDh{Emh%i6u{5><2fUji0CY*CRHEa7rCgQ^o8L8$`Z3YQj9?RsIH1 zDd1Ny?MZXM&@$=luXsANnDHJ~Xq%+M>G`ujOxdo^?qE4s%@kKs35iIjc-o6gS)lNE zv=BgiSoO%hyLB7=vlmzNTU=z@L{C6iM(0NN+QkWj&!k+%>@5s)d?`<5mV!}|qtG)``#gQ(7Va(mVf-z@78bSy^`pBs z1t|npkaDH<8**Z6^$<9U30@s`Fy9evNg)ovXZ1{49#!udtEfhwW?xdfFO*>2UKihB z_D%P7!mWpNGlXf~mBSg9ykT4k8><{vs3jY*tC1S4O@xJ)zpezdcv4&wHkaA02ucWB zI@PQE6R@Fa*lWdR-40`){uZmwqxV>Ez&0h#wzoYq3s{X)r7NY| zvoYNw$`?k~Lm(nh3c7&o96e?HJ3~W5I4c$C1`xNMV-fb1gnr|+o!757!`B6!cUe{$ z)X!DY3<+ocfAC}Dz>Ey-#shQlUNp&Wa(5;EO5)UZmkx8-ZnfU~r2i>b{=JNOBP0YO zHnQP}IqE+iYn%=gG9ujU0CGqun3;Z{%EyJq21@pSi`vKrdqh2eYY&Y#p9V|FkStnn zX!jM`tI0MDXVu^z%#pj6=eE@s@Dn1{ezlKa(xz6_YNxWN@&+#`l~@cN7xLawgDf7N4!rUzW~DUa-UGA5 z&suy<-i=&pmg)XzbDyd!@tYWa_Iy1*)5?xV^PH%x4*}cdr9b0~XHVjKr<;WXb8}u@ zq$k{4t&L6Adsy6PYI$?SCco!f>{W{l>kBgCtqsPvQW)L4#B#%tnr(13680MLRBU!- zEe`>ik8G8d>NtH$8sZbacsu4By@1CLaCdv7oP?DdVMRb7z3J4EdD7~bAoI}ou&;AN z4=q+Rk$qX3w>KNE4fOX#x-Qr7?Ql(TW7y<7C)n`eohS9PG zYoSHYv_gI~!=8@rJxD71Ii9Z&d9a}Vvm7SRBZ(`T@|uBw=X1P^g1lhzOLfw z5?^eOf0iw$1xBu)pN^+CZu4VFYB(^fWNxsu%8wLuH>6VZRMz?_*C8!;*EmhGHRL*x zM4#o0LoHQ~5sKb5_h#pr=7l@MqhIxe>ZY36jy%ZJ;+0lV6SMN$IAcXu9VDyKT-dXw zspHP5=wCrQ7YCjAH};IUq|6sn`@^AlT6iY%*02BtGj^*}wYvuM_GJR8PB`qj{D^)H zZv;O%b$xNY>e3dzx@UHzc&&`#ZiW#us?SyuPMfr5MFcp*bG>(pDN$=Heu+;~Ti~`t zSR5}5C5pL?GQTrlm2-MS;e9ETh}}HoWS5aUxBc!ry0ib_BC7u;sVa<6G0I`Fkj#%@9y z3yB)tk}7&#OraEp;IEW>4pS(`rLruS@-U7{~4?S2iJ;=t2U0ppm%UF$uQW z(!0xYX3vZ@m_=jG<;WLg>0!_&uXDjgAgA3%$S-gI|0sLwxVoNYZ!>uC;Lc8PcV{Dk z;2PZB-QC>@?h@SH-CcqQcL?rov&rwAd*{r&_s(b5-)pfKtGlbKp89qbBD2=6I-{2v zU_|3N{cc@-^p<$@o7#u7mIyVLu%6N7ne9R0G3^&z!*P^nE>D))4!*JB5S6YF5n0Pr zXU_zy$ncHy?)`wTzI;>ULT_om6aSAKh#YA!g0CahdBAI=qsso9>W5r%Qd|d}jchiB zkqSBZzlQkd3L%Jr*?oxDy3e(dtC4rRl9?~Q`sf2LjuHbyaR(h+oUGAq3-Q{H8BM^w za66te`9miAJF@UCRUqRr9_E;9c;;KjWDUFEh2-lR-50{iC3?oTR=%LP^;(ySr|ah9 zv5|}DZS#ojGsBmI#&gs!^oY88r0f$fZ4oK@?sfwW>9FI^u}WK_LqANfY0to+VSw=5 z`7MLtg0b30d{~iyGu}kj=(V;xSH)x4Z1En{do#mt z6j$q+{kIzxZOm6F&mo_m<-j~7KiJ}{-*S0kX=SUmzDATDL~o8jl^M3%RB7&1W;NK4 zlfuvL`cXA zM8luDrw>OsY;AMlV!`59TjtxvCuqZ&H?_&rPk^n&36ON+$OZ`OCG}^q0AcOIyT)`) zK=`gbXfW(&KvUceZ=((O8sG{NN=nD(g4ml3C-nwI$XBg#z0tihz4VVzhE$!rt9|(7 zA!A{&It?YyF7V4{6B-Kwf4r?{sor-htciZq882Gu^Jb26p4Z$6{Tk=e5P`QanK7Wo zQr*@{yCp()joW?qkT$Jvpe?@JI?+$P2Fc;K`z-CnVUw`*GH|Iu$-b2v8Gp-ic}Dxh z8dSCF4tsadH^g4LrTda3n3hy!;@~VzR;HsNzn({4aKX9hR?Q@Z32ZOkFza$V_}oblS-5tDW}-Cfn~b)s!lBRRkJw#?M-UcFRq(F& zFi871s&%Dw84IiH0Ypm1rIT=S@HtSRA|uO)$5VvH#i9B73bFJkf7{_5z46T(w~HJt zjIOeL07pXQZw(T%se2hsjON|e_(e?vmmDY%y=}wJwQl9tVuN}$9sH?>(OpQ@iVMfmvN10zTDT@kvjm#EH zobbaGDF**y66U(q9Q7lxys!ExGHtIxsZd5j3bdlbuph;{qF*<@B_!ExjtVS6Q7a^? z;2*DXa%xQOx@)DI;vLFb;Tlg45tWW_K@Q=$M+v`6Hf7l4d7QiI`#4*2IJw&Zq0(^e zoQbDOe30^$Av_51Cq9^5J}=VpcM+O zO!WAao?>v(|EveyVR;PjMY?^0%)8<2_LBH)y1A2db$_ac5kyBAJDbR*fE<-SGisjF z*-jgm=0eW15I0?aBuo7ZOrS(=qp$M%^;SKqKS=HbiTAJSW$0v>#zm4dNlyzIP)lTZ zpKNE4+9T#FJB^#4e(3=^w09SI&Uo`t`#o^FQ#wT?kx%r5I4>6}g!MmAo4~06=-u(L zd1RFc{lxR9z^72TLV>G|)hygH$=CzNk$F(f<$W@Lc#*`X*H|B%QZc3is%;rH8IQ^A zLica@d*VknkV3`YMeRO=MGf{o99Wugyvz^2ldSG~RC-5>NplIeDYrNMI+vdqw7AM$ z%Ls|~?hnlKDVH~4_GcY&EE{SiV2Z1}x|1EXeeBmm9wOX$(0|wA9NQ2 zO;BY)CqnJsNNgU4z=$ZMd&3CXM{02|6Y{-qVAu#ayM!bVJ!VvL@1&QodWY911!Ax$vaC3#v87KDgNtmSfz6zA`bg=WW;sf=bijs9&4I>~p?PEV zOM%h7S-Qipn4^FfC)6;u!jd~48fP;qe$iY#21okA+!HZhkc)xVaDFEAPn!ld1wEtozkeBUYkRh(D(bseJ3NF-W~@es zIl0R%pE3HQ#GxEF^xYWGWwgVPjR{vRGHIxR;uJ9V>3p_!FS=Fg>eZ-KBxrrTthk4p zS7|pg2xxz>;|7EUYnXHE{yx}AlpmM!k=-fayuy-5QaH^8xU`L4151kbIG4kEhY0C| z_xr2Wizhlw3+)A9{*<a)OisgBx;mKKvq-v-*Y1*<# zW8ZQ3P_&y)4Tr@Ye`(SiV|COJDM{8K>?L7Vxzy&n!J3`Owr)9UF7rPoVZEFI9sF;8$Hm( z4`Gw5%i)W{u4>}YB}t`Tu>7%Blt9xC19GoDC0&weWxP7{mw!^=@@=JH(`h}$S$eD4 zma2lSvlL#|+rRu~yVYWqZxjE5pRrYI)844#PjD zTr|3)O?ENz3Y4XKM1rUf>JSwpXV|3|e;9VQ63I=$G9mtSdnaP#1tK?2($ano90T zHyAB^vY}J-^P|I@IC~5i)hOON_36&VTvdabHouWwb8rbv{32u%+Sx~1Xa>|eak~8j z`)a)jw}O}0KWG;p383AqbGC;L0x`DqaD`k3c-7`}cn<*mx0PmUNER=@97<)~u^xAw z*4#$Wq4ZOfeY=wbxn1*ujw`m;O2_O58Isn8aUFRFQ#XeSKfGakO5w|W>o@%cDCPy} zQ=OL;nI@Gwi0#9jL7h6|pOdU|O=b*1w0J7#wm53>3^oVr9nS_80!f)ee`!pMy{-slr-TSSmj~6a$0EVd13_xxf`?t*SpDJ+!_v-^*5v#T7 zP)9^XC_6pRPm6%j*OR=ap9?~Yqu15{fJWp_l^O0Mb zOk}c2eVOr}vt?^poM^xlpfe6}M>AZ7@2nYfQ0VH>rlF+9K&|9QjE1+7txbsP4@g@1 zMaw6iT1P4{GQ>X2`7M%$EmsOgWjKS`LTu^JCBKfxo3Fj7OuYp6FWZ1^#G7afOu9ql z8@PRFf=AO7LzMkV!O&w5Z8rMsqd~|zI6k>pUv~vfc=RaB0o3;P*+07z!rG0Q`|Y^^ zo?LM#0QC>b&!rvw&Bl206sP#X4RkxRM+YK3eg8+s(cxn z<}^CDIRa^+EG2thcuzAZGtK2=1l2!8TbTpR>Wr?l2?LtQkrr5B>bFj`yqTPa4c1m5 zqh1{k)a!@KeMz8KnB9@zv5^6NFu$5)dZg(9OQIBRk6AdxnjzlRVcBaP)oJoQdY1Ss z)eSj!LFe#1WY<*EW#l7c?Rt2;y)TXC8ROQ|{U_*Ckn6hG`+$SCrpP5cwoD-l^#PSzI!>E;z{|j3Eh4`x^PFJec;iPWWTdub*9=BR zC|5_E43^}VXIvlVS4Fk++l{^Zv@Y3mv9*Vxp*B+qm5Wo%3NSj~NF0UIO-ra)jsaVn zmv8e~;e&rv!-PExn6nji^gz`cdC6JIGTqei7(FTyT;s5q+<#;B;Q}dM|BOoF{)!Pd zO_F%8UhG;#>EgkWhX7zbGgynA@`xVZx3=PPo}oS~;Ro3xvfjz%hNYu<>-5!=ANWK@ z`^&()0m_zgt#7Sez*uDvw+QIz>G_C+G;wOQV>xf5(#4NEnEJ!2)zm;Qz=&|S1QA#Rx;nxKPAj81KlxixxAj#FlYVi zyrb+rTR)wYu)(-%=?Z#7cl7HW4;&D7@#gr^IJq7_NMiz`XTup@E4RIxw3U5g?@;jl zs!0$Aj+}_%?3oaDrjs^1n5ykx^m-tfgYHK8tG9_IpdnY8&nLc;DY$a!N3lp=MWtYhzY}t!IYWjIHF4ixzCi1Hi$TbAkBs|H3;TS+JD?gxd<;J zNV^T_5pMhdv~$Bg_$2zY%V+By=KpT!cdXzDNxAb_qqFwTTiuH%jWb5?5RIyD>93aB^k-{ZDowD7P9I$7@lzaZ2k7S5*n(7Sh9o5+Z z{#$AUzOooHxWfRdL=YyHTX>z{v}17)7+1W{HzpD5Usg9^9vIy$GICjgGtQTS6|IXh z{BmjZ*|fFq_F-mmYb{0hL}DX(_2m5(fV8(+Qf05azHXLj@*}K;GG6cUD?4k6@&Y91 zOSdXwR|&P;$-`OKoq;#I<)PY$3Cjf83HRwVZ>C+w^vlW+QKykK&;e8QVDI~=H${uk zldtUsFSxDbPDbaavIR5*-!K+nZhDZ}CGOp{&Kf?+X9-Dq@qUgb`~}+na~CUXtfv$( zUuI$kGHNIzBA&vVSKX@_P0%F~}1DfOv#S<5hxy2;cb=V_e)TGQLh#96eJf{9(W0hvv7-D2;o>IYp-klfex@ z#mas3aQi1iGnT!;B(4h;G4_bXDH{J!kJqn|Fb|tXEi#q5(ZtVBfs)#7G3z}Cz~S&Sor696@)FfmmSTLmmZP^a5s#6yAN z^_;IT&`4&2q{zgd1~-c>4=(hH_bR<2Yxc$#U_W^qfeLC&?1vMOhcx(4m4EIp0gf)~ zYvshtk62cLQg+g>`QX;khof=Brdlrrl$rYEbRKx&1+*KWCz^tADlf~i`Rb7|G>`vB zOjuio{5j4sPiGA4+b-_cQ0EF&)%_1sfz-0OMz&T6gw;ZqPb~*y8aD*N#AO^76)FJ*Dl^8kQuu7Pi68PXm1XB+CYkCMTuFVi!^UjuNJPe{GL9u zZnagoK52u<9>#h)asGu~c8Rw7xaef*JKIFcK&;dgC%}53ackSbqZ}}qWIu@65tOt% z-m^O{ld`(PZX4ginqE)exJ{hp{NNxb2&^FdKTEL;@~cb-Jo6IZ*C!30bIv%iF~8#f zs2nNvV993{l8Slm;C*@Rx3lBaw-(?Ytla%gxK@bzKyxJK z{(oe~LRrSGJ(=`QDB1;pn-{51_CBt?d>coCZp{*HCK8?ISXHoyL=)6c!U; zvj^v7z1+&ZGao}=?wo0oYkUwp1JU%yWQv2z50*Q5SliGfU=1EQ`*0^pDo#}JSpw+? z<|Pl!^?xsQQmTWUZ!K~jVQWlL&MefV|G&s>=AcJe`f5Xn7{GJjO}bylhF-Y6p^~{t z-?7c^H*zj$ja-JUV1jV#<;US;LQ66`c2E>|W^g*YBTG@+N!J|wzJ0*ZzJph6 zd`(+M-`y4{?qUcr;>_Kwyr=Vh%8dj5>nv?X&!wk`^U9&vihhRW{~bWyOn>#_#+7Mb zApccX1i?t&P;E0{0gdOU%FcxzJ-c3Jq?^$YVi|G9m|lJwJ!Nm8J>o`YV>U`cX1zEm zL|qInhV~GS(H*+#@!;+KX{iG>vv?$9iO9=9pc2I+tCNyP_%>gm<4B`zOUj3sMB_P; zv3=0KN=|t#6;t5V+uLXpM&6b9iw|qAz{Qw=JG17XrBoWm;QHv}jP)j|fEI2~pSV@} zlPeDM;O$O?B_~?ygYN$>ZF|aMD#tm20Hq9-E?GNtum%t+gWNT+*T+(OdWN}S4}PWT zX1r|nEr4|Kktl(sHEkkd_~pjG&z0!O2&l%xDa-=i?cYo%*J$;55#1dgB)&fJFcfVGlLrilhVIbDHU1z1wH8;cljl}Nu{pY>DF_fCRcvm`wi(wF6GSyq9g**=US z)%l52OtgEi1=1y$2Xda?|1-a%&6sjNsGJEte@mO3FS2XnwdSCQO#Xl;Btvg>nOHY( zK;qrbjbnu$BvRXbE+I2x+r{UnK~nXDrOy+ zKkS~j)@3-s^X|(^Qv8%oX}tSpbYwI7*lHZTHr|skcy7v>R=rBS3I0hAC7PN3a-v6u zV=WIkKbEbVF`LU#5N%oEKWU}xR${$T9rCwteN&Zi^#Uc^;*R3A{cvqTPt?~|FSt=wC1T%(%rMUwXLNepuS@qsQHm7jvPLaosw@u9fb2D>H+c88J8|Rq zEoxb^&fGCi?Dhdn*OjMI&zpCuPQQY0!`E^&DMhlW3|HPZlL*&qF3vI)ijuz*r8w|R ziUSU`V!3Y2zOxkrYDL~sP2W({cX<-Cm>dt~xPO6}&3&nhh>7xCsL>I@;gMll>&c;2b0`r{-^x$LR!rnI!kamy>%Kax`&kFJJEw+SYQ>aswXWy)oi(hsLr&A z(6TG9c2!K#kMA^xt0dLOZh<*{Cl=kUcuscBs`hY2%OGcdSmBU=OZOvTtpKSeX{9Kk zEhYv=N)Eaa6i5%>>~aDAVKD0DL=skiggQajSCiZW1cTwtrmDpQ&2qByae?G3vR@Hcr%i!iE1N{U^h5 zwK!|dLTX)Z8G)(HS*qlIMkq4t!|T-kAg96mb#}Z5vtIN^y0?&)O3$GpC9i)W(L--C z2MdRg8AwG=Abw)s>Md5Gcb8TxTcjQquhpeZLfXfAKLU zUEi>gtgLM}Au>RFxNN)4>&ep~w(Od|yRm-209r$WHM?Y){CLqh@jmFmwnkHLFbG2S zf%fn|Z{dt%Ly?5f!#?v*K%RHXmHZIFt!9fb_`jr=o)YW33M}zvnbeCN`v@280zL6^ zo=9TMKO|42wc$g!$2;PjXcmO8H2kL;d6oXK#K8VlpNM_Qjgjq&oRWnjew6@?cH^)` z$KXyE*~3Lw7g0bkIbG>z5xel~k-ut^W`bt+A2ioQ`XanhS#iv#yk7V2tp(~-V|IFo zg2A+;1qY>Vtl7A)QAvxh-JkdJ(S*sEblLpI6JO&b_xh?o-6AZ+O>mt!p`sq^FzL6I z%N1^HsQQ6I;(V)7>1S&>c4#TU=)PmYhIOO=my?7=uxE8f+a7z!JdXv$33SmriCOi# z?u>F`0;dnX-g}>zS1w)D5U?eoyP{tBWNP$BbpRlYD+LWhj$ftQJ~fHMkRk=DAry?~ zsqVI5%B@3p$sq$nYuFzI@hu@jw^s{KF=)Pf@ZWQN-3%ygi*>-`phY_Q0wT zASL0oUx!8av+w=2S3*4DzYsMc#byt^#eMI$xtK3FV?Ry0x`)9mU{_Y=$i~QqHi)6$ zRFP6V#6Xvf!(h0N?#@?n=3^FFrLA zsM8Nfw?7&r4xIjNni6$6*287xzu3t{nyz#;NIw`_uF7aTFSCr{^+t){k(C?Z&h^xb zIlOsN*mZ~Fw-RB)6UkQ4{Ju1f{*xkyf>mk%7e&@)=KmbsNHyuFdbHTanb^M95&?fR z5CT>?ZFs)nj-)Xm6I`RdB15IvOQbz4G1j&@$Co+7vnq@J~XIkbo} zekk!}36xa?Mtk^z9YVGp%I`;G>;j*`kMk>Ipn&NIi1fEBOe@w1+J%nPO+2*ntk1xc z%TJP1#;X4%BVj9eqDFl?!eGu#E!NC@uZro9uNahLdc4{Vz8l6Pnq>_DVy?UTYaxOB4InSACn{y{illCi^&p z^RSb+L7E(o|924qtjOZ`wb8G z!=y3f@0$Udm6D@-{WB%#nLQ`BG3oGmsLqNFcG2t0+ppV{Cu*aX{IopF_F!Iav41_)szS#UV+~;im zZL>+xBDp=C8LS*fYvCxWM_T@sDgH`)qf{vTw@F3#!W?s7yjQHS^P^gUAIs@2q(l&% z;jd*jEw2VDFzE{opeNr>%ERL24YB7fcYQ?P616IPAQvK`b!Gm+ zf|>obQrd&rh*chQzVOuj1e<$@=!`yd_=gnc2mBQ{*&1E2nk$&sy)GrmWYoc(6i{Qt zKU{hl@ZZV0J_zgM_Rkz|Y$1BT0_@q-w^I>q&Pj^YB!Z%jz7hk7g)?_Dq9exA8UirR ztTu5sP=6^JB(tpWo zR7Kv1&*}<`&cpqA7ILtnv=^l!nzx!Fz^caVCW|bvf~X?%+Vw`@T%Z(P;#~yj>SJVWD*?Dqp#Bxt zuiZB@M&pyuR9wa#-!hF5jTIN1XpmBSu=LGiha>bs(@8)@1sl|RVR1FW{ZIF83?ujY zUNgJ&l;sW45>&zKOJZOZDgBhGBP{Q|0fOVDDdcPW^=S0^_hz$5yt{>RRiTAbo@zCk z-zRJqVx$jfWlCjA)JowPrK}f>&d4(Jpc{@FPR=Bj57|vzwCAZo*@^pPt{5P>A%Dtc+w28x?A3GANpPzguXWUqF;a`~F!yFxVnBaz z$_=Ca%dGVplW|p#EGzNQU>;CkyCQ=#k;mI1&qo)VmiLc(R8%XEp9zoj+v~WsHr>{OcZCj^WM+3j8$+tIRz-k> z6T38sHu6>rS>)V-!uI%y?XT@h)!AMXC&b&m4d*we$cKoDm}Wh5 z!|oXz1O3qE{JHR8(~c5I`)ULdDn{AHtca1SgU%paX5(wtuzpEB-!?|w7kot~o`CYF zJT#mzs1O}4{VOr$&sW-g{Vuk!LdqE-dc@mywS#{)Gn0S<=Qq+||6xd)*IN6dI z4pgD~+mg3Ad^n>^&akEx0~Eomt()9Y#7p9P^JOj znyQ+@HB!9Ah{kp2yBeRU?0s@;9+q^;O8!bk{6b(|CG4U*KIn{@V_P`TWf4;}lh2!w zkFS$kWqP1K7cYg|9%}uVXPlKAdlg=u_gV-V*~@op;;yauF5qUGkXOAfg1+Ho|63l0 z%|N39>tW-uP~~2~?L_t0{}8k)%R4oSyr(ii;50Y_Jm^{&NamfQ+~FbA(ZEFMp~@?jI{OCy*}flJi?k29VR-k`S8z!EOcw|jT86a_g^a9Gx#aRTzDVH4^kD&RhbRdp#V8#?yGnXA*NP!MH*=du6~;C~!DiyrRM zO-@0Nm2A~s=ueQ0<+LZ;v5)1vbIshJL=cVa`bPP*=ISSs(OLMGx7j@zpRi($*&CY*&Cx|$GAJZ=5g>0#jXJz%W4=F1*jqwU^lyg_I0 zn`Lkv_i3JXSg^Ho;4-b=evNyR`FA8u>(n7Mf*j8C118Kd{6XF$>aiB0lut)uUT?SV zw=DSMxyX)R3MkUXwt@kFgMrX+~Tz4Dh0~Bu`Sa2rR2?_Xn zKQRcAp;^;y@1HGPjr%HCWxjqUI7+f@%fl(3=`kIZe>Rp!V$Unk84!Mz$-2TT$DZXC z{@W=Qv`!F9k2b1_KwtJZhu45-eST?=NU`bZp7i7NWV?b^wld#5OxrwA2!mcb%ghQon*R7 zMEVBqFizyOE=#C-?fpD`CcPBTw^+3!?QrBq_&r@jy~AL2=Ox{HF5%h~6K+*azU~#m z$m0Q`xv`F{qi0{f!+%pOufa@cTb`LD{McGgZ6;Tl{pk)adu+^yjlp}B%&wx@4|-3) z>*d*aBo#5&-m~x=RN3x}~6g`Y}-y286gw~Pu@QGT?jSy8bvTz zUKh&ke%FhNhc-Bg&!q(&;71vo2OV=RPe(pFQl)h7QKB+~9V=N0>i)vMuk8`X?K7%> zcfO1KK=QUJfcx+#Lgqc)i-MxPW!`m?_O9ECZ8Z**+BX^e(HzQmMJ}xk@lkbf+TDqhUO&(MXoxa`V`AA?EY6b`oa65C{D!~*^HYOy8(uArKPsE*8j48%dotoS zmujDzjmB|7tszQ1I&(5cQ*O+V%IW{`j|NR8OCRgcsMA*`xK#X^PyRg?6Cv&$Z44lk zIkxOQm}3g6A%>wG#pXX7Ly0hXBb=PXo;>y^{%L z`Jq;ELDO9L9Ig5mh`=nHR0rwax>H&$wV5d2a>Fo${@PlH&OrP#Pjj<6@`Fm(H^LL z0UMX+uzqe!ALbyQzv+jAtMkaHZZ8FW9|q9(QO#gLwTMsI3gLVBQaPzPzA50tfU52c z8gyEGdF8GtJs0_!YpFDO#vkuw}ARUeixzx zWvRUpYBf}?6e2+h#YSDCmR?I>V;N#)M_ue)B5>dc>ch`j2`jOMaOPqFcz{yf>c^#s z%-*&T&N1x=>}@P{Sees43}oOWfpURDflYPl&&c)5T!F4SeqQdb(!RpDD1G_C&z9 zA3P?{aTHJk&uF!ob|o~cvQ?%nXB+nwx*{k2oy@EG_VXlX&9l87fNBG`_&oOUbkPf< zk`7-q*NnUXRC}FZad9y(Ki}1z{aeX#Eh1p5`s$(3s#5I%SKkUp#8eEHEe*r`0spvW zT25R-f|8apzi#%mIr%q-Qn^RuQBY)2g6GldeyrMJMaX@{2xRoYUH%@2Q6yzDy7#un zicLVoK!eBaa%MEnK-{z%3~EF~g8oLbFLslneu)>lazrOQ$dmBs4!X%opLuEL5|jgv zOh9w@GP1V)=(=9H5!%?>EDw(w#R;Y8@1v$)PNTDL+5mN0sd0q0xu2za65Flw@zR89n%^-T|Z9p4GaN ztT#EVr10h9{)_MEoEro5J{(0q-aiK-LGLXidrqOai{HnwMwN_Ul_}uQZgS%yqz!g7~5U3v&7Y1TLb)K!_4H) zDOkswc~bm_H#SSJ2;0XmRw5PkxHf6JM`p@+lobu48jS%G3SwlwU$4mf)zL*C;g7zv zy{y}m3qw z_9`B2K}+<*6PHaH$c-zj-5U*qM3cE9mw7`{;-^&HujjWL`tc=#fIIPfuCQ*k0xwQ2 z;ZwS*Ghg@PAFUOaxAHvlpx91tZa%^y86Itf-(^Dh!}Tc3#=x220u(u$_!n+o{qb1U zmCHFV%uX9-%bxc3xPQJHhWt6j>kx;rTzB5tg@eWvWBzSXekmlA^V}@P_zy>no+1HB zzl?tzg1ODP#Bxw=JN=WTddXx4UqT)?g+t!{R=)iww9V_(*H4YNA#`sh06KP$$deuE zHCHb}iS^aKSAh`G(~;kcG7N2jP)8T#ZH(SXqw|);jWiKK1eH7Uq25OPW{_m!i+2-f zz?NePr&Atr6n+PLH>Bn?F@3im*r2Fk-?9mFiXuZgV`)jf{I)5XaQF#_Wt(0-!Xh-)#=4rfrN55#G0*8^2?!@P;Qs#G_q#Cb~oN^s$5qV;F4wc{b$UeAg1y zmw=_FCUd-zvA=hD*}86D^;^KdY!v$@skMoXCupWVd7b&DB>dz|&5h~lq|^3L{U1`{ zw?7{O2ipgrOupvbKmR_6adtI3t#~B(EYx2MDD{$LtUy%8(1RlYU3R229qxZ*W=`B+Y>SDBUAO=w&moobD?G->X&txsL9rJ0DGu3me{U60=ar!5|M420utkM}DV zW0!+$nJWr44N#ugItHHe<6QGD_>%1n1;;R~a+xMqB)@4O=%U-^{DwxawqN8~siz38 zV3*nnMVi9Dcff%+iqg-z!7$Q{kqp_c=tnV#N*;QSx^ zs>@sL7nfV&JSKHc^ITxBS7;3}J3ot0O$Q>#I9lW*I@2W!G`f*}EEd@wqIzlk>zRpu zu*Cp^n?+K9Npu^m7L5rZa#h@8DwjviHOP5LIhFz@154o;6@x>6o4)2L1S&&_z1Mjp zA|`$~EUm=ma-b+xuDrz-56XLvF`aR`*eEN+w%?_8z_meyfO!k$J=ubVg7enu-m<3E zxV*PGZKXic*HFFhwM(>=Ht5@;M6gDKb|}ystMqVMhH0y{$4^n+%@6rsLYhs=m^wK% zn+OQ*4uiSctnb+CS)=MAI^3^N;fZHNw!dn}+VUY{K4{j4tg^?8V7>r4uUq5Tc4bJm z+VGlgTHr&#|E$FXya=YWTsk_v9MI>TsC36#(_W|Ca5&%8@ZDKu$a63q^Cr^1`10|r zOpwXX9ZEr$oX3~h%#EUb9j+z>&9t_07mtes{cu}E4rNSk zZ!MDFCa-xeTQ~enjnd~;V#v0=$RfH zOacwB{B*R&XZiNqYb&ggR=$^^%~-m=rd7eedjc$Err)4a`BIhn0+J$GD2ODG z5de~Eany;Ite=GWKi3P=YUa}^I@tQt6RM|Ehl43%g~!i4i`nkY@!Nq4fudt!F*_d4 zickJg3P_tTaY^!uJ=;0Mde~ z221rGlYV{V*OAC*;6Sc*{q}*cRCS(1p5i?m$iadS#445)RVe;}C%_`=owz=JVI1 znJrJXs6T8HSEZ#ATCvacMMvn)v%mjiwwC+i`347*_r8}X1XuSgIXXIS1UL>H>M2`N zV@QWJAzgs>c8HV(@L^W-`5)=&8oN&zCmIuF%JwFnUt!ePA_g4v0BLJ(q2+z{LX87` zMo|U|kM6*ya~C~pJn70r#&`lwQPqJDZpKkM-=6MVyars10Qg_e<}ZFU1|R!HA+(f` zZYH=q581B?e?)wg);YvtXE1%;E<523_Kp1C;}ME8X!W)?^^V^rWF5!e7M|9y^j?4o zA!NiHL+pUOXdQ<_Z|40N7_o;@(H3&T?KH(J*E<(=tJZ3?DSok9Iu>1&QLJ_`|54oU z4jUAaPe|FGbtRxoBj{=ymS;tswW#;0Mx`Mpu6^!H*7_5U%?T6di@N1*7W#+7wPNEt zl?1IOsDIsHst$0X$xOXCF@eaKsp8!ms8@PU1AVc>fEb%+-cIgBYe&9BA#u-Y713tH&|X@NRXro)Oe?)J8EEJ1uP_%ZNUHF{ITWwekz zXkwjaOv+kQvF9V`2x?|JczZ5xE37em9=w6Z`(=f9+b<@}Zb(-|dko&Y`Vra|q`+o0 zW#Bo`StCW@qYr1;>=pOSPOhS4=Nm?sq@)>pkd(JywI|+ECs`dOM3zh9j|Y3z+u3hQ z@d2^FI_RD0+k(H)_8zxpaRtCwuTP96W$pPof1%kWdR=J-oa9dCi@(>p05O}=oQ-O_ z(irLBSXC}pnysKxwuILjEvnQ(=n}X1)NLz}2$tNpbn8%rz#Ko~sGD=h`L#{7%I+W6 z8PuD{c%dT0XFj}>(X-`Ma9ODR$<3V)^mp^14ULWC5)=1MRzDTjOtN`=8=TnBYH2sy zHIHaE#Lp)s{cZ7{Ok&F}tBaq0k72|qY3se|$k?J3skOITbcxLj*WK=#^OPE>eM_0T zb1yut?KR5J=)Z=Cus>|MF$RI%F8i(`eSh|^zNzj>?Ak1T{C%SO5FBO41-+ zRV5aI_u<5UhcJKt;lA|;x&G~D8!MS&G##Q-oMplV$Ns!R{isfSYZ4bwoZzR{@<+mWTW6>7{K|8DXnN(w1{{wI`O5EI2)VcC4(p$W}o5FeJ# z?BEc<@-)Scy%1EY>ISDq@`D55j8El0W@~j;0t#3D{d~w*2FPd(+w?znZ9f*molz#u zh?1{a&c1*3fyCWT1sk8dB2(mh7u zt}P0l>NjYwkD!vuW?6uBM3kVT!47IwYG7cH$#qKMfo>~Bt7)ED$QF{VFZ1~v{Sc3j zUT#4H>WAzDTUmW^PU4V{*3~TPzg%wAPw2pTy1~$jhzJ_RijXGRDaJqNn}Pws!R9X^ z3VyAL6@sCHjZdVMs_CG zZ`3_)?R-_NIo>2Upm|*NMM~ZmkWbPO$P^Z} zxasg2Q15glT0fAo#1WosGq}5ZzfYGwa!QwL&50}JYd;O}ltR~NMV^bZ3ex=fAEN!~ z`+HF8>LX#LfSbl2O?yQjm|tHC4(W@Llr2>tUWh)v|FRZ52{Iv2)XvV1^)zKNNj{i) zGcLD~7Zr6T#A*M|@LY=B#`MVK3{s=u(_wyIY+|aUgD5SJ@TO5FjwgBLS$%?cm!yLo zUW*1($W$(Up(YPs=xvzSW<3n~h3cD7nF*i}?!7cm%;&<5DZXMgIq;am^Xh_VK{Y|5 zIjldlnu;$J^~0PiFyk5MchA*`=9bgX!}i<%j|fMF%3DMohul6|;Idva!;W$o-R`(c zrcApz07sv_=52SsxmdM7t=JW%4)28}-2Cdx|7XNwZV&T!v6aYl>+9I1e{j;Utc+9H zqK}vpzFl4`=8$8cl=G}mYw1bxmcNHkd?K+rTL@stzE8pcGF>@#jPmkp!RkciDrUv3@V&Zm z^`-g}HH_m7SmM2Qg;T!@xJ&deLsG0YW|G=#?TpiVZ#N;1O(B|29pDmrT$43!>V3;~ z;BquGUl)!JKZt3|`pUevRh~iI%*PBqV)x+oFhg@c2<^1H7-F#$>Fb?2MB5%)o$j`R z*aq}QvG%@z7a8?Ae`K*-NK-Fd7g>i6v`F_rygS1iaB5H233m7)g7d6cHGLs`GM;TOFhV5xiw6#y&I=FScNJkey8qYH}9C2 zc-H&Zsmy(=DU^00O8b6%L;l zkdl&eZYhzHCT!>v1tmpyko{*KgV`~w93IgR8@)pDLn)?JgT{Xj0#3&?G^>zZRzw zHzTN|mo~q0^L*0~e9y2aH^B33yfm0l?Y=4xY(I7Xzc_o#sJ6fDZ?J`yqNTXIdvTXi z+@ZJ@DXzuc-3jhRi@Uo!6nB?Eae@U26YlT+*W9~Co@XX2@A4vFIcM+lk-hiF6qON6 zNp>^`)*$8na3cbx;l`A+gU+O1y53V5PhHsRJ`X^`1I28y4h@5PVcW@}@#cL)q41Ld z`CYrBuV0adeBIK9l6?j$chw)be)sn!pO{Ycw`pEHw96SdrUM;;K6)svX83UZt-$qBJ9;WkE+pMKiqI5geN``NP1u$0q2>p&|Qm(R{uq4mg|hv*9qxwiZ4&OGLFY>vmfnNz%5rN9#3R!*QB9Xsj=^l z`D(|9_4E+Y#YxKNTV7YMi??lvo-yN{0U$MDueLO(`IH!U22!p$!$gF$bIbaa-Uz|ZZEmqH=Y zw&U4N`dfe}g-xO z!M$iDp)KI$eaQ2pd8PPs!YWmtFlxMpc8i{nV}`Pc?+rf(iDXj5 zQ5d9E3pA$_bKWY7$9vk}Uu`*il*Qw5gNJ=IO~}nIf6b&q6W=c2Thk5b71i`r00P0W z{yw^VZC9G7onoc=T-vRCBpI0`3`()G=!xpyA+Zwtoi%X&FCoDpdwlX#pW&15vZ^VHECr<*?3A3= zXm7j!DSKj)(!p8FvbBh; zZH91kVrsZd3=AM`JR@hf!pET6Wdcz@!<}E&0B0j5i?1- zM|!og^o|qg0tQbBx(N)bZw^1DL)#^EDz;SjbF4Jl$l_MGo%9&AT*eQ%U01jW2v*Z2 z?=j9WGAv(ydy7&jP4iIxhc)Pch@fZmrS^15DV?&}W9^fXiqY2b=?O~!bi!w1kW!Zo zzY#c zUue$g+%OGtEu|d>+&A)fy&D2<$LGW8E9m0?I(+<=3H0z^sftidh6)JN;{Co9)noZW zy1h0Ep~OKtSm5%HXoT&dxbd-_wc8zct}#^;nrL7rM(V==id^~&k5${0;rIN?D7?8K zeh6!@G+GmDfh7I(kCu}S4ADD(ZwsTEIFB2t)^e`|E!1%;yFGD^A0!4{&{5CfLJf8n z-k!7H=U*=ScdP6llTO2ELTx0wr#raV9N$r_bPa*n2iYID;60jRLfZvRaGXs3s4*e1 zfU}&dd?m#))H$LUx3?CB9iwRZ27&^p(#jXm7+?0p^-p~$vdClX|ICHI#VO2WoHos9ilEC@`E$6(49YPk6JeJN%MI@fw&M$M1y}fgeDrMC85)mLB|FMF+J{(|g%9J$&az1-y;>A>n-2&dco8EAxmpU{8}Rj2`nj*lqqFNcWP zT5}nZ#3YmB;i2Hwg!gh@P4E2v98zi@-=6)K=ys$CC6bl8OW2=)PWD8 zN${CBscg@qN(Ovbde|o4(2bL&`yOL8Fwiu)R~ZxkJ)+=I5Y_&)&(VE9{!z;^qHrP? zPxq%GB6bqbt7%D`@cGJy>+|y0k<{ zCh?^Xu9>wjF)Lnw`>S?Acz9ac8szgP)_{|#FEVv&F=6m zs%HC7VHC(xj=D4Mzm&Lplnkb=m3nn*H`V$4cWi3TVANd%??)RovqyS0s3QTS`s~J> zBp%~zVl7KZqKyS^JolGiA9vPI;v1^o9Cp{AXG2~Kvc{<<)IaZeO zi)-d+{g^#T!R<;qv&s*3DwFR-%2CS=CRedRH}z2nJGOUJSnx2%9ZNUtd2QqsD`y+n^7(@6UjIM$L6>! zVs{SrCO)h9>dd#j1Lrkkou!SwXeoMkt3n2YSmx#F-7rgph7f2Q94h!HhZu>fU;t3u ztD`X(nHjlK>Ql}Pd_$E=-zH`WC_`j1<~dUA>RW3voZnPexoa2nRq=;#5U}p_ILYbv@Oa68{dDumGK(=sU>c9k-b|5EdG!8}fJl z4s{E;v{)VewK}C1e@ZI;YqAS0>%$c(h5j{znpEqzLq_sFxhX*e*VL|S69Rs8vYgZ|F z^X=#_9pYUVD}jLf@)vUT`q(n)#2~_KDL}odC`NAw$Qz2$;&YAI(nf?G0)~gBs+XZv zMw%7h8Cb$N4119^{hu(vK_PB;Kk0%K$Q!0`lV6z|v`H`nRDyTD(r*{*7>dX>j^mqFA26gqi6SP?Ic@}zlUCRF|Sn(*yk9Isw1-?k{quk@O&=R^nPYepp$$OH|u;0^*lnh^NV#F`AR7#evz5HJTTlC{dG6y zp2}5HDX_(5kR@z-_AkCrra2qHff^Rfpu3vL@(6m#QS6sS#>Pi8`(C|$ec{p3yHI|K zP{`}I*Sf_-_#9l7zG$cII^RO2-dGR<#^H3nmv^j8{wr`@T#@oZ>jTdVn1p z_Cf1ovPoy3W_3?Z+2d&JAt%?)*fvaYsvsmYFn-E^E(P%!IqcL

pi*Iz z)|(fG?4J;TZq4%{!E}d>8JHYeNkzw;xVyT@W_Pxknw~H)W~}|aCVTJBuFFhuCR#3I zEj)FG(8taAt!%O4FQ4O-X0mxvi%CovA$V6kAW?VCj)yI47^5 z300%Tgx-{aoo5)TG4qeLKHnm5UA(qM!GYQ1PM|%V*@uK5L%gY|2px82d*`v2PaqQZ zlQahN?;$fC=NC_&!PlAg+=d|2pR~nq7^YKY|6jG_WV=?Mmm>jI^61ZUK{Q;{a~^Ry zFQE5rMxmfT{lpxf_rIksVInDf*q(WA=X57jw6z#$*d^Z2oBMo)nw9Gw2*V`=`NU#( zJgBEA_v@P%LZ~Y+9=rJSYujyl77mWYphH|CuNT1wy#*4$2=YFNTwX-V9Nsh|9OZZs zkU0}}XugTJ*WrAe{Ea65T2#Hjg_mBcz2V?rA{w_kvcJNL?00)CxzYdz``-`KO;CMF z>ZS#ew8xu3-PMt%;XuqX#x?Zh&HpbRLS@X(v?15+`B84QIq+$k>9+x7yIMl;aCsLd z&kt#3_1<)gQ3%2pidGWy9G-7;N9<&UkEq8FxLyUWUG{u9p+AvMky`XT^2rE40WU^+ zrKU`Fun6>T4U+jK9eAc(T=Q`&wlL=O4Ux4PGQjCNU{$TRq|Wm{{4{H0>7k3k(!at`rqQc9y(ODj=5}>S{!NY%C*ELLKQd^Z4{hT1+QXB zX!svB8g0Pwi4xJ~IoUlBZ_oCQHnY$A%au6Q&3{w)b<>9D9D6?ls^*V+n#A+9Q^i;A8Ve*I}E3$WzL!( ziFSq;NQMikQcMPoxO_%4yTy&+;`toN@+)7W%HTelUgR(rIhWJF&KfVUl%vM5S#6{` z*Ch6Ic^Nz);ZjyrmD6JPDAsI{6X-^0%uCIPx?#{M>Us%W-W(gVrAIv4N{>zaPqR0y+b&WgF_9>sKcece z37c@A@)t)K@_RWkSNxIcdgUkYj1e6>fmoI$PrfxN}}*LtBn-NvEWN70SbN^ z`-ObWe=`=hZ|YBKA)BAvT<}WdE0q~$wKB*(kQ4eW9L^}AUz6Z^5EBj zMhH$Z_LLM9fC?RF7^MPc+%hV5HnuTE_5CadPC=;1RU$n(uY*W^tN=wSe3kF%C8hF` zE78L&OQCX!eKrU&ou6(e{Qnfs`3X?Nsp;UZx;$f1gqsRnWw7NndxcXv#8>8zD1)#r z#^N1qG(0MMEjaCQ)2t3a{a=yjT7Th0S9H~EuW$OrFrOPTjpv7b&g&aB7MEqO>;#(L zdVQLlPoY|65R=Ew%M;xiRz@uUl)@W;cDlI>#@kKvwtv^5GxT+XxE6_z?XWaCNGD;$ zlBnlOQGaj#jC+P|2mXV67Ve*ppHt*)j+FT`o4d=QLl~Z`_8tvUzfOj~jLxu7|CsGs zPFu>xM)-yK)EU01AktBASoc*K`h4O^f4v^Ncwr>{;85^qO4SG=A`lhd1Nfv_RNA6& z-qWU*Ye7k9S13ey($lLqtU!UXVDy=oyc1UbCKbW)Zw@FQ)_h>kgf6mZzQE9+F zP!_?R-j10{59hbzK+QCJqG#W}={4L$LmYi0i{F>y6Ehr)q*Zf>wtx+@9bRC-nZ{6M zF5S$Ve0C?$-GDl^pCox^RzjD|6=-TN-4v4Kg9uur(z3FkTiY^gGtr^HLWV0r&(c=l!dWF5qR%3k|# z?OUSB%(sWA4ElEP1tvV}KSXD*&hl#e9tFjPx#9xptx&QjzTM=n*p0UQeA2Z*iQsOr z>%(6f;vO1!8w0Ft9Nn?wDu3C_JUgGqOX!(QzoW3)e9UT|E20!%<*9v!#jaXY3>3aC zPU@9DF*EU^_jO~19vLiP{zB}984H#Sf%VM|j50Hq{_%q{7l z(o&`p8^}sXLlGXZqxT5O$;fzDwS)1?7ykmBB%;-k!6qCKxJr`lt?16oA)gyx?An^Y zl-+seYa*QEgTfUvw(RoS&B}EQ(4bPkG9CFmAZIgXXUF6Ul%ZUe-5IOGTNyASQiVBv zB6&#_!}&Y8Qe_tARl@vS$v%;l_&xVODT9BtfOd-)l}3=51Q8RR^J|#T%_A_VS!T68 zJz!#-ieyI#J;*6mJ|vKYst zZQHYgUTFl%f6wr2m-|%!EqAz>hO(knnTgzR$k6)pTZV-b)03@#8?Wzi zgA;a`{#iIvWe7z8mBEGhQaFcXKW~?U;Mr4cB67cnJs|8;k&=p~oXvXxG#r-j?Rva> zPX-yDuTgX)eFn6(0JH%+vQMAm#(`rceQ6gX)89xC!Y#zwK#y$qMFh?D&evKjfluoH zrR}TssXKMv^2P3*<3-D`BVX*+*Yz;pzA$xAhDz9ukV1cR`z))q5;X5nO8R zM9^S@b}5TURaoQ(pL7DXRZSKGH-q~wK28k<7ltB;&y$h43j?_mAItrzd z0q2~3xPDOPCpl*hDZ3#Jy1(n~L0Z5?z-2dUL}W&MMw`yBf;s%5*74!t0>vC*B~?{1 z{S8*NH{ovaP*)D3h-C_tBbGHB;dp(?eL3mvoqP<@nvu>kx`#`7AEqB zvdi~T&@;i-WMxmGKk?wAj{r%N&l~YZr>h^O1Y5?DtIk1^wc{*n0BhnaFL>1hd%`yv zp@>6W{#?3zL}p{KxMlZOthvC6Q1;*TumC#)&SUz*VW-KWt`*t2xTDr+7 zn#@;fW1(>Nmx;O(bRU*c5ao?-@^ytd)!Q)jC{t}Pu1Z328;WhOw{q!u-rxKInlEB(R7 zmH>4b7b#>#45%#IPyVlj{r`PHO}B4=Cb@bhc(}lU+3G@l)HKzy8;H83wgh0_4h0_; zx(*-nMGv@MQ}iI4a1|ex`H#CDGsNgl=NQ|um|^I--`m|eXazi%PRBS1U)^eCFu0Ze zNK8sn(zH+X7WN)2HG|0OyJ^7HM+e23^WAk6JR$MV2-$u(wuSgr-C@l#OkQ?GFdKcw@3!`s;mY+~|y;PH= zqfqBZFe#%}v_?b96gDdtOG$iv4uqKHq3u&P2Q!dzj3yPA;}YeYRH<1FcV7(=Fmn0s@)}ry$LoXbtv#UJcuXR|140_2&^U6K^bq=2 zW^|1DWs<95^d1(-Vt=aN6ngbfRi^Z3&zjw`4a6UTyn>tfAqf$=z87maz+c4mkQAS204Npxi3XG7dW6a0SZ>OeV z$yFqaKS_UXa)R17E}aKAe?cRS+gtx$>5{NTXvqh)eEQr8+}Awm2>++^4dcEkrlYI` zag5}PsULRnF3x-ihVP$#7KBvwBU?yq*1)>C&!`Lvo9`c3@y+tIr}lLb6+lQT3q2E{ zP87SssTcX(qb@n%(h|erpU;WL8Mi8$>qE%$1vc96f(cSCS3*sht?{Fsr#i2|aV}^R zt$%{V84a$fQJAg7rv39d&iMTfI4=@kL0G>hrpyDEaZ>DaWAD)ST^>UWTK}Qt`S|#d zDfLGF*nF88VwnRV&Kuf9Wb_8DXHC5xE?D*c&37KM8pdx*xXO3z2z`(Fcr>^J_47xU zy~yLr9sYL!YL=d}=JN+CNN@Plb4|#&-Zi$Oyn8U)_?*=OfK{*Rm!}H+L4T*flL|b` zl+Jj%7C_VFeeaIN$}jGUziRm&)0jiS7^Z9wb|5YGXX0d-RPr#Z8UKU*)w+U@+dZpI z?4bQ(_bokm*_JS2*R$e(33rsxdw`ZUxyQ!bWdX}*y^!?>0VPmiu}`D&d``Iwz2|IP z2Fhp~oo%Ni6Uy6#ik6_@We!EQZnrY1T7@YWanGf}q_;i3yaMTlzT@W#TL0dbRy_Dp zM6(jY+2Uj2dcC=}s;0ZI(5Gst1g*8Vu&{u_xOnpn@R6E8@~DoMu+a~ct!AzRw# zwDEnr^=I}v*QxV*7F3|6_PJ2X+^CC{sI_&usm#LLf6&jRH<|_^4Yc^7;s98T3nMK_ zn`V9=5qIVLv2g!|FoJiz5o?Py^DkC?XmvFpo#qHgU+HsQwVT&fsTTj;$QtLU%s;g3 z4f@4uBtWdR4^83WN;_$E^SLe?^H7dU#!4qQFv>=_s;^`P%* zu_<#%F6eN!a&TYlvLrnV0P z+1>i!UlDLZ?wZq(%lkm#%tO^;kVP z!I=VgrD}l^`1b(%6!=QI(c?>@b#)D;R_ZpI;+8oMdVEz{z^TZWe>V(5_Y0s)bo!r6 z1CZFsAf%D^F41Na7LI`;qiBB<6RM`~a8aikS*wkFjXLJd>zGSleSE?Wq!$vP-e~8{ za%5L5%?@X|Ym)3QBx3n~V_fx69) zHR*PY*Mb4r%=Di&|B=Y|2(cg!%nU-z`HR(M<{0}Nl4T~S$H^T%Js>+H6X23ePHhG9 zYWPI^)t*%*?$c*i6Y;sNWU9D(N7^GgFICRicWgrlDNtW~RS?0gdPQX^#oIi~`{^BC zBB=m}Y|31vT13_7Lp{i69CfitOacN~=gUK}rr1)ZWwM5@46z9FMShy_Z(Fd*l>TwKy7cx|4D4Xt7bpK*z~i+6UAe0bhKv zeK_`kgFL^twIw<@C4!D^EfLTW+GNV^+cA)wzJr@x&S9A}Jsm6KiFXsE;9o48-gIvS zEI$DQ6~q5(xG1c~cLPy>63+Ey;$%CPHYrSQmUQD0*|G1M}nWWTC#>Xq>f5OC))wMQ;Ns3&T;>TMx1D)gX zWj}WP{Ya?>adX5p)+h$+lK+iq(sH!3V&)Q^3myDKP+S}j1=U()L7(zWf0~^B=({D< zjv){Ou(d0US!Fk?TFTHK@owfHTTLbNv!yZz0Uk*sA5TkF>iK5Axrco6x_NhDy^1(c zM)?eCBFnmVB2re)erx(nF(2Cy`wk$gt9^!Jn+VK9f!>tK1|OLv(%tj7+sz2a;5g_H zs;2W6&mSopwy$&8QVyv3zf+t>rj0)cWL~nL7XwIgSwAhWX!?J+&0kODWh~p9sPN%S z*X7P?S$C{~P0l}Of4&jye*xjy+F87*zaQc#2@)n}A`^s+1Qg;ti-^LLiE%o{wE!u6 zMy>jS-cJ8;lTTa}aIjEwr9HwE^2j)!1~(uVASoyOMdr%%FOb!ur(}q1q94&EO@ZFW z_Vxn3*IBxFDakQ50(J0jq3n49k>J>j&Vveow`N#+Rl)P(v(NUPU{HDbUB(T6+#GJ0 z^=7ox8^)&=;Wzb$^`|R=#Qxy1Jz|p^3>MII%%=2MV<7s#d=(?`^4^%y?a)C;AT9>+ z9hdCEfpjcuqXrM6>Bn}i1cXf#qmee$=?(bYpOzpteRojTlN6SaX00V$b9>xQLVkzTu>** zWCuQi%Kg6M$;$fWNk)djtTSy2X{^)mrN(JoL}@2*9KSKxw=-_Cd48;gVZmM0;`!Kn zFxCqYGWC*@@*(JedwqI`1VfP<2p0#uLy?FQYdn{ToeT!4F4v)6ju1F%L6-Fu$~?&V z^?Ha<1&5=F0$x0iJlPm3MjXfBIw(qP+0+G`pra z=zJskzJH=LRYJRu1}Y=eaoB<92jO(w(_DYR^xvn1`iq5P&Tj$dtdPd{_rkYiNTK3< zv3CoAc3}wDMwD9&szFQCr*DWaS$Fsh+{C2orn$;Mt7fxNSY5nhU|mZFKc_RhT~}jJ zY~zb`GHktnv^g}i%w~5)a!J@#IY1jRYe@dl9ob50sA7M~s^X2a9bjoC=o=Is@A#^- zRrPl4QZ0(8A$`H|W$4;=k{RCp(TshkGpE^O8UM^`2-jryf4l(3B7#@MQV`9+KRa;USk;}FDv zRVvq7x(j#CUFy`s5JtQf?Qv*g zq5;y%vg$#&rZh-!l(h9zQXeO`g4#)+k~S{F2JFaHb}ra*6(26uU!dunSEs486b>M* zV+I6VbF#$h)+;x;AyMmX;)tq7dA`wVN3q&hyLJm9s6|Cm_Pj@=5#+y~A6jk&b2s}SvV61+5aQ18!}iII%{w2KJlr77d}D>bsxfIgfKg}Bb@u)Ni{Gve z0G=g0^W3pAk`U@;fXy3Ozxh~~B4_H+?>UaQ05AXGI-Yq?xfz7=qxT!?A8Oi^I-K5c zy2Id=kVl}ff=V1*BB{|6k;%DM_w%=fzFY(W&Ki^zV}q*&f>_7%mK05}CfhG<0^psk zshKw8Zn0v%BP@YdvT1z``%JRNz0aNdSyZ%WB8uKVDBflVYy9d_U6`>x3f8`ns*Cc!~*vI19t1Op--nx21`9JE~|$Z_6GQLejY(; zu>!p_?0PMy(`nb{<_Alt$C|a9Ll|kk#v}Fbts@D>*g=nfQk@jC?WBTso;rwDTQUG> zu2*x$dhj*b6#J3;j2S-MLpEjZKISHui`F)8nBbyKN8_Y=qurmtHyQ2i_V{E7=j<(d z=&Ax78akT0ZwV z4Y}}L0+r$qqq-xI@G)X1Gd?pPUBvr8zi7?YZQF#4ANhhSbdG1d5;oxe_|xNw;c&*9 z@elvC9AXsH+p0jZa_+iIOs*82(?5GGEeljU-|$B41R~NMm&#&^wgN@S*Qc&-baE1wK^DpD+(GtCpEe0WQ+@8XeN#`~ zH%hfHb_f}A;c|Lh(N+X4#kMXpZ{bN>#jUi1F@+9j>>w%iOP-oHds+QQ{(Hpr-(pyQ zZlHHIm(ft9@39c9rEfeZTrqII%}8cg?QV~57qbNMJoKF}rAHUY23}G5xOa7o zt+rhwXM)L5Z;`~2D4oN1&f{aNe-5y$U8vp#%r1n4KOfFAV4_G<(hMYUUkR7%-Y_gG z9`9U&_=cJyUu3n{-l+?F($viJ*%f^fozn{$45z#~k$AIiKIXy&0b;I)>h*P-@<~}n zyMr_Ynx1iH&TZv2ONQ>=!68Q(fgf0;SW!rMm%a>cu-r$x$o<8R@z|nLKa1VE{SoV^ zLje0H*ri8unQxmx=y_Lvhd(J%N}K1>S9U3J**_FVxJRB9@q01Yz>eK4e<+W*$|0tl zM6>naGun&ha8w&cX4Mx#ezXJ+q$x(dsd%zG42^>vP25wDyF!g`M3gtwEm_I@#BMoF zjhmxmDZ#&b6dqzpY8J7l@<%2k`qmO%wLB?z7O|C}H8AqlQ153w^LCnW=kuf9G-!!Fn`^_N$ zjFrRX%K^&UGr`D)Z{L9DCwFJyhLJnufP zdM4qYt=cP%16aQdi0yvyUL!7Jw0?SC6`HXa`H>}xh<{@qu)?036*J|tI;17*n(nFf zjbI^dY}EpM+g+7e=R(%I`_i}@mNjhUanO{FB?_;+jEu$_bj5F0YYi<(gK$J$hi=Y{ z)GXU4Zr>AgU&|ORl<(HT@>1T_UqDAiRu2h_=16@w+I*IPvna>o4&oHg)*2f@Dx7$m zG**)n`xWFVZMkRVH_I=(Li}+bjJDaOW=Isu6*UQ~(|saHI9i)R5$9A)mOCNR`mWM! zG5g!^lwia99J9v|?>}G%4@vX41kf@X{;FFr9wy&hY2sLz5Lv`Zvb#d}b$* zNeC62s}_X|$pk+%_$|6PQjRGL#MM7fRNfubnDi+U%8YeeA_wV)jn-{-i>xJS%0Q=H{C!ooWPzs8FbwWovhW;xkE7JdTjnrWUL?C53BZ7 zb8#YH=gg9LQB9oVUn85(*`uXRm0zU=Sph!+B|~rgKx`d};UsDW9;Z7stWK~I|C7+H zsWpQk5j@@#hvQ%)^9;ItJ}QL-aI3lRExz5?l+I9g*>>vsets zr|^p6z9aT(ypRNS&@IQubDg8sQ%`mdbIv!n0YkL6%_^l(ETblo&z{#8Gk?YhZ>4mN zxOB=xyLzd&d4gCOt=IDx<#E_Lt1o2T1-?+ z!X@jEO4r~jtocb$@2H_(KbxQa({zm|SMbHoeND zxX%2Xrjd}27FQQK5Ze)QY*W_>R;sjW3D_q&B`cGCm*JXb4vD2CA4_?R;OiE&cgiYv zI$gyFYTRr)tJ?_EAE-o53zHsAno z4K0KQ9YjPx-_-Au;a!2@-QZMlTtOQATUMsj&!wx2x@6?vugoJkF>v|* z2EXr+p$+4WCh`Qrpd_h9*e^%+C8rBqneqL8EZJv9j&nkkbL$`Hn-u2wM?&^xU(W5s zWU+C6o_S9sby`F*FO2r}?47l?<%38rILA0FAAPjOC0 zm-@pm{GW=|=rBC6@;_~KQW$zo93;xY-8nSYJDZT0fW9#J3+{wpt8K?We-I|D!T3`s zh?YIp?4vIZv4cXrQbn!UbNQB?gP8V)?@R_>@k6%W2HA=qB_l)NY*x{FxlbPXqm=b} zWy`Z@C3Yp{9;NQjxm1|fywCQ7s>&U4 z4?1sR4F$7fW6cCR#;N2N((p>UuaYi?M<)$lZFQVFGZ~K0ez<#ujkXLW`=wS}vz#P) zP;wwzpaoqtG1?4ixb|d<8sMLIsvVhwQ)(%NU}z#inTWJEG!;;H3HG8_z3u7(Kd6p_FeiKRbjx_J5d1tRG3pY`n8l7|uOKs#(mXp8cN&h8+o?yA1iDCwun#q2bR zxv%+$Sn)lRs0Wu?>N?a>3Y2lZ^dp;_z5v&G6YY@HrMqnyA#fX>>sq!5DdzJsT{JEm z#8O>OYn(!{WKFP~J^F?>b0jQxYCj7;sq!Z@HSCZ6^!gAi0c)4P7(nChO(wQ>52qKH z6_^0IY;yE`WYN4{GcnES%z9hVM+%}G`pFN9HDro(LZY3ND%++AH{g!dQe^D*G7~mB z@x59%^M%Qh)%}Js6WC^FHPXC4>qfu#*7h&q@DUS(yLVeiK}pk}6?lAam_?^~cE=-R z?1xQ-^CF);A|7em8p%h%i%x6$*4PS$A#Z=N9W$`%!tjZ|<*oLyfi>j*(;)HG?8IsC zNL3&0pc6P?G21;AM={z|s_ipRQ)nGes9t0Zq1kSz=Y`d5gaiaz;ru=e8Ab}3*()-+ z$qLKPJ5J$F({~bb{}IWj`qRsl@1Ie4?XJ(g*B@1c9uvHYDvOkdOwEO#6OxYG?5!3D z?@K~?tz3LXnNS|rd%E2L?%oSRb4w1o3kB8=1vgcw;JbVG%oa;#S&}okH{S)pWG}6r z#Aa35L$%Ub7atgD7O#fygEPB|E%p47C} zzS1{yc4{{UK#dZjmF!>b9Hn762EN~S1gxqEsXWtK!XQwK>nRhwo=c|G<2XBJWZ<{6 z9(|5CEZ1*{YxwAY3C0#ToNChhn~0l>9)S*#2irMfoo8ukU_^E$%>nPyKUmOh{8p9K z%%N)sdr=%PuXi)VR`rG?=bDa0ow^Hl;M3DaSMH>7H!xF-oEAR1m(t(_Zkw{~- ze%k5$@=)|-yrA=rF>PoRYZlD7+9JPQvwpZ(es7g6NE4kXFAFyJ;ZqkOSc!n#jc@f# zLV|aj;MgA*Cyi{Yjq%Htj^; ztQ-jzMk$%}f`1dN=~T4EIG-i`QF6y2bVDN3TUA;$Z?xXnJLLapOD`r zs=2vYwgWF|xOaOX9~a~+-F^u%;Zw^k?GpHJ4sEJ`vbSd6)dAR6k=g;#^7x=O&5&_J zc^=gf-DGUApN}WluA!+O78q5b$aclc5Zmzx13UH;>l^3F!}_Ar*-XdQ?V^E?gqJ8e zel%RPM8?bVVrX)g0&%hTkb+vR4J@3#I#sL=_E{j`-K&7uxsTqrufykD zzMjg*vcV>WwE%aHU<%(;qx}WzfJfI~4$(*yrzSa1SzW5Z;~nR0$HuVSojz2`YM4;U zB9GFE{ys=mILVN|M& zdtC!%>m_Tr%&+j+B0V}cMn`LkaSc4HeWg6G(sr3@3ESI4pK-ku5>)7?-poMRWpSJR z3Zt1DZ?aNZZ@WJ2w1UF2PtK85?liKiRe~&Hm2_k#KN3W0*?8qnym!%vH%+N(S820- za;+eQI-KEY!V6EKAB1^B(Wk?YNqmFDvtSA=m7&acLrt;opf`8BNJQ+TF->Qm4lHD= zI5y?7ZyIURaOw|_#|B51*&vPX6)(cPF#-6g>eGXKc$I$@sFP=S1w^oBxBjw~ER$Xu1VSAi*WLlNWah5*z}--QC@SySqEQI0SchhXi+b zcX!wG7kO2gPsKgAs`p8;%}czH0;e^Cz-l zj>bCuZ?Ya0V$fsIS388F>OP8Jv4ErNuZJgDhpBqpdeUAS`DVM9Gp`wN@rmNU62}tpEhz~0ROBR@+7iW}Ou4|pq0Idw3 z=gBA1d6e;NX1eDaTCx8Cpxo$SJcVh(4EC@rJjYYIpXV|^bx^~zY$I8hE6-7JhiN3H zWyz2?5)_yR9D)ogbrS2}V ze;d?Q%BMp?NJmEMGiA|ddHRBiDp1mm6O#FQ3bBD-)UM08k&3|#0|)++Q(@l@4A*sV zQx=4m!!`S>&iuI4xO$EzFk`t^fU?l;PLYz5^69zZ_+w0qCI+2v9twb&q8G}P6_t8_ zH71(5mMe#M>;9Y^)c6eW2kG0{4Z+bJ=s|HAqGF3W6fzM3MbGUI@yA%GMTz4?ux>RSJONk|$ zF!Iv%1$~$Dvqr^r10q66=%127&D8W!kfP4j4HF*KHGjmxVei zJePY}&s%BI9m-X(1`cxmeW?uarDGXff5D^cHct+?Y#N8zhLVzKpOM?cJ|hyB=)ngT zEpqRVet?sHIhKDo2tR(O%+5op&%!N>Tccl1nfSy7{bR(~{*9^P;cFdgdhnbLu1V?K z_1L+PA%85lO!%{A?2l%rRQ!p!c!M!yg4K2BosZ_#r&Cm%=vt7tEIxf8Q1a;5TzN_g zj^Rhd5hLr}BPmAo;x;HzM7ue8aek^J>ifq7(q#0trK-8$`)bG7{Wgcp>v32RJ%I|$ zp;tQH_+#xe?}_WB@BV9jBvf4fAXi`E@Vm&95A4b$(}ukDs8DsD)n0~U|AEbO25F!L z!|k3TGkmcQt?uR-+sb!WY^fav9T+*};*u1)bx#RJNwlDKZq)LvMfq~dw9&phq7IYm zc^%trsYc~#WjHx|j?&<7$BxU?uzz)y{PsP2d+i+j&)9rUqPV?0#6;HT! zv7{#Xr{ZaTJ9vV9CFeZ9AsfCb$xEm#jt3TbC}9XZZ4pYh==f~FQCJ8b8^e-T z$9o^>?}wtQ{f%B zmd(`DcWAbwla{Lu3Mk@17y3aZh<%u&l%Vd(6orywE($y&rwU-kDXoIPIN~jP;E`8? zuSv4A$Uept<&PjkTlMnmDJ7vw!Dxb9%&~)0({eV;MYb{yS6P=J4P!NQhm>-*vkyk# zZivs2&}8Z8jy_QFjv_!*yUU~QxvQq6ltxn$P$X8u6>!u0`VmTn{gC(EPzsUa*z>E| z_=9iCdwTC)msiBwJ&!}kgdV43jHekQ%oL<+k{djMYR%Ljc@z^f#Waa%ufrL;JFmuI znnHuk#i9~7GvyXB4F*k8$0LR&USqltk#}#XM&Hz*SK`GLe~*`2*9fuq92H1{|H?-v zStECqp}vSL3Kv;2@bQhh z0G5zr7Q<7P)2?{c1dhHO6Dt|;?YEh$y{wgGH0c4UwUzVF{PH}PknXjSH)(HB_cur* z?`QlKL+?YgtIS4k>_I&4LA0%zAyI-j-;Ge`>#xQuacy;ZA6c#2pD^*fIK`saBG{55 zH+;n!r+GAu?(M%{PDDehHT|N7_43)Jw2memZ3%cR^XC4-*JbCzF7fM12-r2mrs;qtuwS&hTZ~LAJI_K#8#pcCI)7OK^wm-?L zH-TuS%5ty3%f}Y>yo8>aU`({owFmEe^D6RPfFz>b(ncrDi<$-je5g$oy+>ixExd6cySHeUJFAoi!WT{f`&? zoYW`_v?*y6Jbeq=n**{fC<5$aU#S9$Br9kb$bhKAn7KEhbJ48!nk^2wc&xHk@?;Q( zbb1aYO*(k>m&ha@1f+U7X6q^kQQm05$HzuGAA)(lpAh>WYP*93tA7t)dI)=`Un_o2 zXwpiQBRk}XHWa#HOgR7n0f^v=zu4~$dnuf*vhzH|(jtVZDW#B_(nFA;*vh@hxaaEi z+mZ4)q?qSlwjNVn<8u+6E0I69bx1eb7T9a2%7jhaQNLIIdc5@s81o&8lsXfVuOXit zu3!FXpI%(C!A9o(9$Ih&s1g}4A@jxb=H#Q9xvFtLMRry_*98Ws^4!5wS@OZA3^AlP zkfN40+HZDA%R3SzFBI&=SAG=#iulo|4ZkdEs$r9N3sMvDUePi#*yguW)S0~mw!ur3M(TC zDB}CZ%q%9h$N)UrtO>=funJsgFJ}fHq8%8{oD+=?+2W)Zzrs6k5*#>Ch9R|*LAQ5+ z4p>y>suv?vX`LFO?OVfUkDdg&Q=~bw`VbZ{c+9K9a;Pcyu5=z8s+Hn?XnK4-BnB*K zq3NH1Drw>tfBoOJ0NUym;%xIKW`C)v(;C4Qte`TKE%}mBP##0(GpFW~P0bo~ocK&V z_v?*b!Kg=I^?Vue<7hw}iFOa{ke|hporKhFN6dZ)o{RCu7(` z5IK_9J@iroG8&Qmm82F@tN2~>_DhW|)A?&Lprk1FlM*c7s5cgK@gfppw6N-IqoqI) zBRQ_DEF7)YJ7k4+;ME*yz4pRjR()S}G{_Y?4LQhTT8MA3(?;fDt0E)(lbuM2Xe|PO z$&cxs^2=4oB+n1mSyFiHF4oYfuidfDY45*=$)CWR4j1<|ajfxmv@ll7;dx`HuoL{j zFRipjPe#HMMi)kp-#MpwZ&2|1y?dh+l|G7r0tP5B;sioUHVDOsFy;hi_>Dx*|BNk% zaOF)g3i{_948mOGs)f*Zo>HsQQUeY)oD8d6J3I%OQl3C9BB`SEbC@#462e&E5T3ZU zWuhVDm~8DgF-EE*+;Ls`lA;oDBU@R8c*7DeHoZCTIGhZ0}+dp0%bWG7bxBOqGaem7(^XzZzuu zKRQPq4G0bcDum~|2%U$2uC< zt||G~-gs?>fOyFG;KYeQ*iC?!wO6sPZRk{)IU;sdQ!GPw&pz^=TL~9sWI&V9_?>WVI-u8A*fL_RQc0VPt!vRpxSw9MYDD z6GeFo+jejk48e}ij{keod_s2UFgMo{`EYA}6Eo=hde_i>W9%AWeVE0GB2e#xDT z4r)emefZcda+-{2JmSMj(Q4z0m$&DQbNZ;t34u{Mi}UlyffI0U$|`Bfv!0t&lN#&@N~5sJZbm^MS}*KYIS0}_qzDB=>8!1vB&Ao zkg`rRqT5G6@!i6V`CW?lg*_zK6_$G{`wPK|mhH#`QPu%+%;Xl^=buS8$1r(f$4+&K zzHACQJ2R88WiI-lRWP4G<3^1o&F`ea6BQ2e6F%XMvg?EPGUW)?-z2p=HUpmLC&jc! zUqm@i@}UNSn6XhTFb^fCP?Q8BLY-vK!1!g{1g)J9@!g&S!Vu3F zm2Uo39~*gT%Zd{JsW5xxFux0Mw}%by{>u|G^l zpRngrJU=+i(W{Ry?&Z34IAi0M`~N_zFnP@DmCPtM*qXJPAY2~UdhGnd=0L8}cfdcL zEM5MpdVPwje2*)Yv%a1_M`~xZPRU@UTF-*d4j#th9howT;;&^IIl@0^9hkC9k0d(l zu9Tv|ebiuY-*)~6^OZ?8n4HwtqU9*1+2J$%t97BX0^b!<`iS)biM-oD7142>%w z(#|UdS)C=Bf03XDs?uZ3vohGeD-Ais644FqE&}KpDb9An*3~FwjRybU0BED}1q_+~ zk1~F8RbIolatoM=Q3MbJ#06iR;EGdX6)i(N7nm610+Ny&a=36y96e(;&dGt(`vw=1 zt$gvIp^Xxj!|KzQD?YMM(p6SR6d~Gi0zQ>cXZ5A;{dr z7*rAk72o?0(I<&XsL0`o!l*U3{j+K&bR3dps_Y*-w#&_x`t(sqMwCg;(B;DQ@r7T$ zrr*T9(XD#EW~v$*U8d*LzKo0PmUp~Egk|N8HszRpz zhWd4aH`2IXXVCMzkF3suZfs%;^Nrbw#F@@Bly#oYL|}QU@ds8czkHg%rS~fn+aiP@ zDFHOkzzei>wm+Xw(8jNL7XxETd(85kK?W(5iG)0jETg&A*DD%4!2{Mqre7ZB^re}P z-)UGtb&vh_nsXFz?MZoZXbUNFjXhV78pRHs$Dg;vlaZ$~A5^|!5+hb#H+-KUkEFA)y`KUr`Q zaUIth4xr>4|AC36<4;#`M(#lOE&a!i zYh&1eVq5s-rDyc+eU`ZlBNG(#!**Lv!-w8DiHJ|#h0=wBat;fYwSELZ?tkF@6SmIWTaAS-$R>cyo_gomIaVOeO#M-A0&*}nVa!Jj5nI++y0RW2n zc0m^qf9x4%@$RfeOZHRg;_hwX(V4>AUg@xPbX|Mq>9$Qk{@)((0WZ=I5a{M;`d!{S z48Rs*VvmVY1l!>V(r_S;Yh_zVqt+!ZRi)6HOft*Xq`5L-O653J=?aK(wM5fw)vt$O zF7@)UhVrl+p~C516^XrRoDXP0exuZF4}-s|?0G4Ze%t3mJJpwqPm%Q*600h4U=w;3 ze-{c7-{)9QH|Z6(+M@G;0)TnI_e`buiq`^OC8_WdGLYdZ1>g?;nF%Fl2ypHJ&K*mY zm_gA-L>C>jJQ)tTu(t{2E}QydaD{%fa=$^rma{kzCHw*N`6uieb$jbI%hk(P2M)R{ zPZ$lhj_w^W>2!KL`|%@@1HJFMhg!JQoJod`VPYd-5d#Y8?8mX;|_C150B6!0*51{HzB!CYXI6aZypz?)|OV=v|_VC^@AH2^r`O z84~>AKkn+-BSI>o0)&)r$-dV`KT%X~ewEHAR;g$F$ODXv_4}{*Bxc2!)HP6v%j$EY z$<+XZXC4s-s^6)rYZ2%Tmz!eiOO}K{6%XSaW=<|MyOtEk1K4n|$!y&*_E$PBAu&x* z-m|URo3=S0ENS3>ZPBbSVE(L!5-*3*Vs)HZwpA%|w;gD8h%epLU)W1OdrIM`sikM< ziQFCLE0o#)5rf}AAb0#sAZ!4`w*09~c^~dG@LLm`!ajEY;VFSZMGu+4LkTJKYwGO0 zB4qAm4($?M`K2m2TQe7nL%0FW>l~kZ6#1xO$55!ae|y5CU#l*sgweWsY^uiu#NGbg zAvy7l2m&EPB)}giEy;PRCY{eOM9alg#$AedtUNakQ3fq-e35(_x)~u`*(aY!7Hmj` zXQ8Yyr=F;?ME>BTRISgdF?q;eId4$AShai#wQc$7Sgl75EPI8%i%i<>4x^r0l3F{( zT4`u5i=q~Eb+dwzb7xzbToU!4mCDsN-8s*@1EqE9-eNFno&Qe==C*NEG9Z~n1}yos zB=>0&>mIZG%ra=J@^tbs3tm8sL>sj;?3dVP*uGB?HMUJcTUhmlMCt4vrnq{w`YS9s z7IRF-cC`Du3ahFR6E&rA@4lc4DJ4RVn|ep8xDJ%f7|kL*kJery`mM0ir}#B`lvuVh zff??9jQ@E?UcSXE2xRz5=4)tn6?J)d^&ZQs`_b6}p=n0LUXIo*&DCU;iF3t^LTqzM zqMgGihfuA5SVHF8#yavOgQmtw1ByUMje-)hM#nR_D486g)0I@c_uhxlXVlo>8F{_- z@wl?-vOuHMj3yA}HZ&kEhpqVRNJNKQqppRfClGn>%Xa{|O`K3VhruIVcYTmQ-T*{|zxLKO6gd_lAwl>;5|d(UtA< zjyfS>Bl_-}(MU5>-#t+sNm@p&@b$r%p|%!NO3zXT9RSe-f$(N5gX3@H_f>UsY}ntF zaD$#twGNkm%ojL8KRy3hH=l8%r~Dw52kC0y{C@hBRDe8I%`(9 zXXCdLWxU@axsy};g@TS4A|>bvj7tb?GrAap;CrWn$7Z8ufx)9xks4y9WF~U7$u4PF zG9e!M%>O!m&SDbtEk6WAHvz|SVo3CqAat#A4b0;naPnbmf+dKz-kb!K8hi8CxGmEk z9lqt7oPAxsrSrrdvS$|*!^n^p ziKzZia%+vSUnT^W^H)Njbv~X8Qx;PeS)RKJE2xr}2fWof(6$gMOk!NC1~?YE=mbCn zY7Ebu92O)bcJ%+5LCzVX!5ALzTa;pMGI>%l>_OT9hc-@SR}<~oi!8JrBa{@_1vMrP zbaQvO*x-O~{;Lft!30AfWT})}Z>JX^t59}#*HXnkWBpXJuPnJ-^QzxNuuPiCsTa6{ z9IC0aRT_aS(jEBrI~n6eZgPQzpFZ_vu6Rs$;!*p@k8Wd!4cR(Fs}YMi1yI%==1_%_6crGYQwD5Sapr?>`=am@Fu2Lx z4SLVp`K!vV_wK-Z`ZsUqwV(EFA-eFA02BiOVyajF&kT}vzB|xQgmWd9#Dq0n^Q1_ zRUkM*N`KjD&_APObucCEW}WT=gSl#^F;*@o{8tu0O_HF}?LSE?-mA5y1>S%BK-mk^ z-ujQ>mDNtmur-sYGae|>ZBL|vcxoIMOQPf+-$TgGd4IZ-=#Kv%TaN7Rsvkm=9=UgA z#*Dd^p)uZ90Y7s}{4ZP~kC&BK{G@ZLVj+H;%fM73?i^p-13VjHBSeQ=Y|(iPlNCZr zHs=B6qO7{2blwmJ$*ev|X_LZ8KHYl==i;XGsfXv>twR?o0|*0rwIgsw&hghQA!f39 zM)ds2gFzq(5YVI1Z2;B!nqK46P@=wpQ#+GsH8pZ%u1>m+MvMwy(GK+Lif6z_7xVF^U*`*<~MZTJqKx^d0m10#c1uoii4k z)SJ8h0N*O3AsRAk&>8dRE`8>VUD`FU54%%`g8C`0#Kkp$ zVKATEgbb@JPCsh5pHuE(z&Ft;pL_D?;H$l`?8AbIM7O%tqT{iRH%~$(w*)Y{j;gCt z_>`r$y7TuBUFXq_$)MsIjL5D{RwPP1F_eV)tlzLR+=h0=UUFqIxYZ7v zYr}xd@%|UL{U6-Wq*~!ibr1HEwO^_P3#qBsxlvoa4G>45n@u07(W%K}Ab8(T$ReAj3&pb`zM(gm!^6UYzSVpI^3j@I z2tU}$QqV^zQg*-05RAOz=a&53hmKEFH&aJS#Bg%@o+65fT$v|=mScROT&*o%-*U`r zwn`ZQ0YISDw!j$K+LrK;)bjuTh;F;pjK39eo@eZxmYO5(-sWcB-l$)~@@K4)nR^_k zP=~8p&o^2MIKQT3mX}2k)`lf6lTWP+dM>&?bHqZUHPxlc%Q^bNR5JZx2D zMhtYeDF08MJ6Ui_Kz)d6!ZA;Tc|ECn@AZz}`h1GnW-pw3fdQQ^W`-=RQ-t@=5JsoX zUxn4-I|s|O7#wGc4djVhG*EHl#mLkx zK8_t<`E^pH=z-vQo-E4^Dd<3@0psOtcSsR%1tkAS@ue=--~S2xgaT03{y}j>$=RTb zNd+PGx+OoWUIF2*eeO8yHU>N^p9Mw8eR;%m>Pwt!2$f+)jEj>i1L(SJI>=V=JFt%Sm=dd3+7+!d~D%U)}l z*tMvz`(q|e(VHspxP~_qH8K?FuWB3%!+s4YS_c5wYz&ILVu2fxs9$C>C|+&O8==!` zai9%Wrl$kz7Jg(?7UQ z>5N!FE112{-w$MCXlC2z>vyrz67}Y3CEZ zkc=81u3xJ|Rh!>!rx9Px2;@D!uEsS43%Yvs+*>Qz00H~|ZX20ySun?a$yu6;S$*tu z2SQ=be+8d2n5g1#)<*(>H}5VPGTJqrRDbzEN%-wazsKTfuOz~u9_|)XTo~_-oaTGq zFpqRePK?n-e;N9zivjyCw5qIu_~X0Lqd^F}KWK@$jaHp*5tzRO*iFzp2_tQmEjVHz;VjT3Dls2U0?I3*Mit$g z#>Sn}yEt{g=Jw-a;{{}-+Xqgxc~atcjP!(p{UBTa4toYht3Kqj#bv70_0<@XQ0?Pg zZcF236l!-fKDTne?tT~x^d65oVHdvp6>tt(pOZ{~#^!;%X1_U8Xloke*NjFANMsk* z3{pcPKr(SEn*E(^GbVXH3_b<@kon6Nsk_bO#bE?$uD_ovMGC$zcUYyj)o7bSoqPTQ zw#M#r#^c>+cg{3CS-xCpKVUsGKfa_z+Gl=r`;{f5U9jE`;7`r4hL666pXR;*QQ_BY z!&hajQX&adgKxIfd=U;xCy9x4O#$%a!;I0SJHfHP;zj#*yz}H)k`d(XLfP`{#I0m6 zcuZi-x-@OR#NXc|1u1++C`8z47#&^T%gIlJ!(!}7=ZB@{P;*2K!in^nfRa=r_mii= zoM?tlDE{%ICt1=6iF~+(IC_ukimvP=0eCVMJNA47N(e^4y7DlaV(Km6jD2@&;HgxK z5z7vn{IZx!RJ|a|ldf&K*S+-;dO8OcWes#=-1(iS%2=An1PO%qU?WieyzKFC3Ptg;7)EHRj@m~N6YZ82ndH;V54Zs^Wuxf&z+hs)5lsAbPQBN>wMg0wFD8 z)Bv+kAIa=8lxNWYAkj{h6W08>3aFSZbUx=5+C$}Wy@Fb9eCHP*s{G`Q<|x*B`3ViCy@xuX`WVIJ!m1;?VWhq8H?P4^UWIP_%a3j65Qv(|| z!KMLbZ(^4p}lI3kG_aBDc+?+_sL#1LVuaFh#f#p7|&d!mXc z4>(;Pf;z(ET$F^11yo7mm;M zHlSs$uVy%kwm(nyr2h#DEESE!=|=O!V^>)3du?KRb2MgnMtJvPh(jqOquk_lTOu@C zRGE0&^i4<>X_OXrAf`!l8uXuK{ zU3P3YB{+bm}AZ8nYOeBst2Ky9pc6-G{z@pe?0C6Q)3g+4&@l z+C$i}*c86=Wfk_mNy|_hQ3R>e`-;i?XI-ICQ`E|K>mxIsAMR&Ni8WRbPt$V)I6{Lr zvQU66|9y`dm~JXEAU1F|68W08I@+Yb~eqoj)&hLyjTSAQ{xYnV!Ks1MO4 zNC?Tva-hcwev>=Ij9{#T56fe?X{ zXJ}o4dIv}Id>%rX2ejO2C}h%U>%qUG^?<%jJ!c?)7AU3wIuF!H~idm6iS)!z2>Cu+qq8uJ@}A#OUeRkKBnPsxFjSqS&%?1B3Ic zQFg;pE6#t7Ug+h7N3})lH?6R)i+M(n6+7DbhLfT^<2BBjL%s2Bznkp8dMMYQf)u{2 zd7m=?q8|3-Z5T}T)Emne%#ZdYE!t|{j2g42QQr* z_J?1+4-V9Gu;CwkU8-%kzdpB3-i*9@5Fjik@lotZA_fzK!5!@GZW?M&$5J)0LB9GTteR9Q%u z?|ez3apP{o-k3_6{7}WC?Cv1vA7`1r)|&o)V66GB%#NdJ4nLH7s77{}zmG3QuwWGw z8~Aas!*4)7lt8+-#M|F1}y)Mhw1bt=}M-Cj(qE$MKUFuW18f^pS1xXW*AUUkDRh{ z5GI1%M7Bu8O#$!&AF=$M1BIg3qo0*tCsI^@L>XzwKdg0^CC~WyC@jxu_kH6GjQxx8 zQ@t*xT;7iH2&7JAbeucqr7t%~gI^Xl=)f{G1u9A|J9m#I#C6DcoTNM!wmHkkeokMi zikfFzn&a($avpBMFZK0D$+_vXlTe`vsOzLi`ez4J(SL>Sz_R+#VaY_m3bP2J&$If8 z)Dd;6*Cc`s$2FUIp4~4TaV7TH&Fa&t%pbewehUmjFpj?{i-=fH1baB8-{Rh4h$H2o zS#?6?$bBqH%{Zs}$<$|uHzz-UOx|yhCJgG?h)4uijDdD~7LvlNh5kFO(35O{j5}B1 zDLD2-Kpa>?)Ne66B1_|leJC0jgB6m-q2=>O$@z}oF3(T2)hjq|E`AuV1zQU>pWLv4 zaTFO}oIt!{l119mEIa#W=e+O8=lVYfL8b|gQ!Pq2`oMfh7C*Q}{c1857mOeWKd-MEjXUA^v4vhKj@W{@UXLy@ zdrb$}(_jHQt@76s*;rt!9sl=M)54NLOP3G@k5?%QFL8{-A{cog>Fv2_l?)Z6rs}1k zEfTRFo13PrZ{dq$7KM$NRL0e*Lc{rYi{s)IB1^!}zx_;rOd-M{Zlo(y6blPo3#wNH z>azvVNjlA^;NT^i$APdXg5rPwKNeWBa6m(eBo~BBu*Qfwq86LdP|g8}+Y@muM4x9D zH#3uo;Lznwe(NN=AJQ%%zCJ|coFtR0V{M+z0vx>!J2xrlFRqk9Ye@|$E}a^%fH>;G z(0)RGalE;ycmPVu(LQ(RRncjY1)y&y0uhYCM9mY3(8CCoBFhuuHg_vg3gzk9YUS-p zlm1~(Bw2Ze4X3W{0-DgJIZ7tOo%S{}drkPij^Sq+fk93AS9VU8pSdguws_-f>J57q z)97pjzo&5SxAFd1QGa61F{U94FAp2R(pqAUsap^X>gzisv#7?DR}yHztcdM6k7JE_ zj9t!{BTyVjHXln!+IdKs7dELP0gYRQvvRNORpCwV?!*u#;R9|xikX8~I0U)|$=2gK;- z#PY{cmR3BmB=OFI+l2uMmwVrS+oNU^n;A*I3cZ!ooxA?#y{Yx8eJO8x`WVUji%J^l z?W27P`?2K$?G$aW8&qIGp9eUvUATkefGgTf$RE|Y&jUsPV5~9ToFvU0enUff+L035 zZ0Vw&|IuAFnxfe;%Z)3i+uttr-@-}4a*d@$*?-1ei>R~aOmPuqXVwKa2kICLk3_>2 z#iFbN_xSxz)}$eX{rY_I>Cgm1^e~Zv)mrZ&ZPCQVZ?_k|@!^s&YU<^YeVn;#Ymrh| zU{T1IQm_M`(xu}?niH1m^*?fb_IN*tqh!tQ_{;I^EATw-|7A!~{0>9>u2G;y*3r<( zvqf5vCDqe|spWPp$kFm0k8N%D{QJM#%s$9BxwD%zZ&*U`%#DW=vT0RzQ5n;k-WbKA3tYoH!!$wKgc1%IIEqo zP!pF%|A1xPIW0M*(4T{x`S&~h3#B{Q1nI6|$@Mh)yZy2Cd#?tan8z*jMeMkDR`o4eCVxR*M^I9hJ!5^? zBYtM*nLx#3zwjS18Lx^vzU8HLHIr8KhW&agKg(+IC`m_Kc%Sw*0yhF$Ia(qf<<*v zJGh*Q&>6jros1asd*1f<3>Evs<-CLsleRO>+imXay&jH2%+Q&RIP<3p-BJ5z+jACU z6bUU^7JS@fwWx`Z46UJ+v={!R5kqEUr#JGgS=+va3ixmB$h9t=uK4$*;jR}9=eAo( zdv^zO@r&$B^X&s^boLzQ?uFkvHBFu+yp4LphS{zAWt#76{9CeuInw}xxlHI=j2Z zSoTQ_Bg&KRN67k%c^KRsc4(K|78m=g{UIl$aq^z;H0~II$eKNdrD-K#|gv+Eo>Y0#Ec;YH5vG=2H6? z!8ku823ErcEdrb4IpR`MI^{$^N1H12>^#`iq`FR&O7|MhczG>@bdqk(79Emj(K6@VmQ@%15G| zJF>(k&u5z@u}JQ_LTK015$oES>{o3tTN)%XSID_2?7?5y^ zp$pfkB4cCUG7nZWt*9rEd8^MUtX4WqlMhAj-f&XZw83?TNOaHY!lG{vi5@LKkdvb7 zkq!SzJTrGkAlj#&@wbXqzYNDJ+i-n0+D_N4Onqvct(***#?iD?X@Ny0zq~fy|8Qp2!+ZtEj4?@b?9b*)MBX1ibM-6%#`^8b zaVvE*ZN61`da~n>Wu{Xq&FiA(WN`h(5>>#(`}DfFzlwcd!lFzr$$m8NvjHb zHk6y;#3vCBu@LIsg4qi>$5IrFR!yjGUoz(fYip|oU3oZDVOW&)oE+>YuI_r+t<~&usv0`pCgK_sVI0K*SLuWpwQwvd<-BDdRa6BG6YF0&EI+J1Ylc` z#?-N-57n=~es%0)BYfU;eo)BCe7#SJNaVY7qijsVn?6K0(`>Ra0^7&xv?jlDNJTHh z@#ywjc<)>mpig3$*xiJ+-M+q=IT5&`Dui=W&Fw1N#FDQLqsejq62-|AoxHg(ecWSM z-|o8Z7(^T{P*{6}bJnyaseuRd1}2NkYJ9~x7Z!8k`>fwI;t^^|K4R3C%DF%H_n#VE z6!l`6=ht(JhugUB|*Ir=})laomL@ zAAV`gE+AIym7aXHFN{?GQdNWNQo+m2V8_J+IoOuf@AkDgNU?4fcQXIgdlXup%*=!U z0e{4DDWn9zmeBfNSDnk1tL4em+@^w*Zh`^r(Fm)*7y_93r_4Zs9riG=Uk?RU{cWx+ zl-CQ}E#ZqccT$?K(_|~}?YlE>cpSRma0);+h9I;(`S1gyshcata`Kqibi2>=NDfrC z63t?_q;%F;cmx4asaD zythm00pb3|G!0VjkTUJW<&l)8-LT+`!Li80%E(CKA$qZ&TJDc)i_%4D5{mEWZiG&p z*(&QKa?afQ>23*&Q}r~`?a2RvAN>UCw7O(Ziz)8Q8FgY2B7uodp#%(90`Y0h!6SL+ z!pW;)T(-g?Q%e@z#B=Sprb;|cuoHzf(# zd5U~+!!|*&(#mtCWp$jZs=M*h**omXBxaLyri(V!o$At-dPkK7ckQ|#@?p@4OTK0kk1S7@>-CUW~A(ly*FF zXSLsPdcBQwc)S;Jg0t?K&YB;P=sO!C7(89*&!pZrqBPbEjvg!?bp>%2zcRGMVyU$A zn+W-xGQ0+Iov(|Y!t!k{P{GmP%XeHH^041$zHf4MS6&SEVgQibDlBwz2hdS092UJ9>3l-l=h6 zNq+jI@ODhvoDM78V! zyn%}Y9%sD3*)w75)T{cTWG0VzWMIsQrGRc4)WEMzrm4L=cge2-R0~Z+$IDud^DUtW z!6Q~|``^BI)XN;4f9Q%a=JoRAdkYeHbqo0QipV4M9VmJXic?@T(_#p~R#S#C1MJL_ z=|W7W)vHS4n@18zMbwr6PxOw!_qa&VR(E3uCbVJdY-)(hdcESUa3hA>jilu|eb&4W z{9Ejlc3+(C{S`h-XK;1>?KAA}GB^6*40j$Z&pTb}RS4d_Oiz;U&%buS(B1>z&&4-0 zY8Z$*wvse$1jtoy=r*p`-)J=|pLyoB?-&!tcl?ux`mR#eMkGc331s+2EQ;Hkc49cM z(!k9$$QwPcJesfG(HC>gNwo(CI$L@BmHXLQ@4m>itqU)KG}=oltE0$g*4-D9`jfCp zw$Fk6@+vc;jDf`zn!U4WmD@?q@5nl@pM3DVZzaJ}bV)-vpaASdZprtV=;rKeH?O

RB*h4BIBCp%m}8wM`E1iCE{tbIn4HBeX7>;=1J2nvxE)8KBjBquHs)r|7?_| z3#9g;Y6TPTMZxp5q0|$eC77o>8$MLXiYQx^0m_{bLw>v7*S$9c-`%42Kzh*Rl5!YN-Wjg84-u51fa`g9|fA$_RQ zc%|#_I!{a(X!RS&?LN!ot#|7cZ0P+qj~0F1jmE2e^Qz;0OLjM;*pavd9mQ=fI!7Zs z=l2Odw11n0_4FwJN$!ux=uW8b-f6Dij5Fq*zSboEAUHXaAVzs19+B1AWF6rLXoRl2)9rhohF@ChfHXj8IHn%33~dzU-?k5|#;&dg9MkOsPAK0pLX&frL5L%77E(LSUd9 zE0u8Ee`-^Ep!Q3+(=FaS7^xcD=uiYA_yU(I_r@VKs&ql!y7?ZASf02?%H{q@B;A$@ zxF9_7kBaV&#GmH4!2gr!3bSxGuRc>aVr#gY3zw$L&L}%1iNPN@8GuMpE@wkTYqBb> z)59>3%l`UftrTX`8;vS}mb85;O+#12X_eW?*7f}bI!f{eV+Am z{pi_UT|KI5j4JR}FxC{14Rc$b_Aaaf*B{yWE661p`^jA&N&)UTp$2<6fMqaXTIA{nEtRIy|E4l&> z86&Mh3B3l!P`?lCeV_+b4*ip`bjwO{)p85)6cx!2OG?4@3sAwp&AR2)bBro7p~pEP zyYAAl5)i1}ub`Zy$#wW732=oY!bO3=!jl>p!VR<;VO{xKTmPFU%Gc!09N2M$WHB2j z7#BQZ@O2p^%u=E3( zUer~JV1}r_2}pFoZ!|FfO?&p2?5JY2wi@2Q>?|Jbj;8i;3f)6muJz0WS8ch^}BCihz35Vvm_ipp)PiXDa8ucemLm2ovZXnzh4PjY-+qu zw#vD~|8ei>`|yqVVqvtrW$#InZ2_6J=E~#sLXIAZiEeBclUe*K{;f|#XlZSzg8^Z5 znLFt~{BzQGpN0r5!}@qs$LrXnDgdO#Qiu=o#axVN`SqbQ+vSs^oOW|y5jY|y>pn^s zoq1+hF}>qe~xWqpFV{_yb(3a6EfaUOel6b5BD^X9yrQGHI zExXA8gDHnfUMuXgz1YY?Pj;s}qL5$l?{|2E>z6FpYFsQ%NufiplaHSP`#_dyxVVIx zcj|+hxbzJTZT(5@*psNY+bnSgkv#9QcROK!dgi*jVsm*wf4{1>%j)~yCL)fjp)+9h zOPl-64)&tk8x*0KbRUcR+3L;xGp4zwJ#?eewMjil=r%MMjIJfdAwz;^^1>oyTU?e0 zp=`(sQQ_xvB_{13GRDLOW9wi_Vx`o=E> z&$IN`Z{VN{0@>T@?Ion@t%HcZhZXM5Uuz8}pPQ<;ZU|@1v*Olxr4DR3j)-nwE(Y7Fhu`V>C3UU6*u37J$7%Z>&3e~QLS-FfJomMI@+jbL=03%XZa$_m{jBWY6WygZ z;_{}}w^#kb_TAfGw$Sz~vB-(2trqJPK7BCimNd9BMtg|T&3ePXu6i?2fSliJZ_N8G z55wh7uh$pzf!ZIV&O1Wg3%iPE@8XI+*?=g^ajP=3hvt4yIg%W}nK0k6u5Q!qepv3X zk7ltaFD*%zlqCmOP0!8sj{oMO3lu5(k{vKYNzD-jD{phFe@r9?$V(D56RVUf`P434 z%IGY_heG18>~_ncer11370@Zkveqd2urE_=VfRG|nAE3Rh`N%uy<*4TXagdr)084# z`rXCVHW@<0c_gvhV4wEZs0%{CxW^w-vo2TVE4L|E#!1ab%=?_nh%Qf6F9H(}1a)B> ziNtcl8PcqpJK5a}I;6~RT#=?gxN)6))KZ)+7jrG~j|`_?b9t$>s*K=Z4AoE=s5ur4 zx*RyC0cNyNL;hK<0$pwCTV7Hw2)ZPl|J!icA?XKlA^UAbvDVj`GKHsEpmK7$beCfG z2CLDgO6Nuu2-K%S&?chmud$@Ktnk>M+_k-s<{NQobp^>6eU?3c+jJPl3v)p1c+nF$ z8BN^5GSALLmIL0OQ1(g90A8W4$`tTcR~k>L*Qcv0?YEYScd4O9YHNb4KctTMV(aS- zjy@o3<~5fMiQ1?SE_Zzp1u|*z6%;Hfb|f7C;{u?P50!q82#z(%?qJi>Y>gJ0;D4FP zIXy#cYWmx4;-6|PF-0s#6u??|1uHmi;*<&3Rv${)nd17d307$38vBw+Q=rT2GOlxe zz_SCT9^9qZQL={sLo05Cd$OJE$|dLbK0G_TnFs@QUf zX@O2`-tGUc?G3EO*2}R1DxArLQZ5BMRVJ`nbP1iyJxoM<&Rx&wwNfGmwyB?FE* zPD`n_m;u8n}ds}sH{G;@(f)AbdX4WFqeV6(nmvvx`~=Z)hff1o$LxS zpn|G@qFfQLQCL|1R}T-om7?&F7|Jg+kOV;M8`xr?q#y^XON?{v_y&f+^cP*z9`aNm zGfhYu+9a{ul@`oly!zx)CSw(6U~Idup8c}uD-g^woc7yCFtV171^H@(u=o?o?!aYcsWaNkv-4L91AD>K!1ah`wo>=udJS$8~TC?7?+e$6XrFw^(lxb&* zbW&d&C+w?z{g0v`v_L^+0U7wEf!sSV!0e5P`6J3TyounVlYX}pKn*>fh9Qbfj6n5M zp+;2dX;dG9l$xqe$LmvDb--zA zjSWv+yUVTO)Y3*LidvS*x?( z2``b#7_5YGHG(MMLI{>-FBmmIdgIR`I+`09H7kLV;f(@IJ;jpq=K0L~`dU`K~`K7NPia&_n&UZiai|c`Rg77XUi~K_XItW7_>?>YfbCEC*fKQvkX2J zc8|`miKK(huuJ3>r!5=ZGthRLoy!bCv1?F7%h z`^V5t_+B7m#hB!WDe{jh9k`T)qSAcXpz6;Fr*zWP0i%BJZc^u}Ab*e1NprF-w4|9z zS*ohK7UxWwLzUqTjh8TR<hn{0NN$5lV3R&rEyXdOZX3qtZu=op47g zT$lc>3+q{Lexgtu50e($tWhwsa9QhYBbS^0B}+jdJO%_8dq!p z@;{JSRE~!D(`8BAT5ukKe4xD`Xl)#1e)2=0|GmpNhl_E7eK~@2IyK1(7`?uJw|h&Y zM)blS%tu3SsBgWv2%4tOdg=Y65i3@sVZTxOd94<%z76&BTAEAG8z*yWHNu)J-WIJk zGWxIvNfApdGL0Vg3nnC{iWsM2YQmDJ!bsRn)W^z+8&g1s2WxO5l$5GWoFL-}lv7j= zHZ>SGD3c++y8P_IT3yC~%)>Z_TSlT!sSizrfYdSbm6aW@y$TY!tpzn;(+5AKvtb0; zH+Xb)F|$pJi7V#}R0K0j4@==mZ|a!dMw}-7e8v$<$H)XOkk|eZSLUn@VRnE_ie>mM$!yQS}{} zvk!py-5pu4l2K0ilt!OwUP+2_Y@3Sokv)<_?RX&Dm1l)qILbhQ=zC2^j6yA67(+ha z=An2D30f^Et_-r~wLsIOQUeMy>@y1*LY<|OqQbNVdyZUswSsM;@dhf}9uF`z!9M^^ z@o@pm-V&1!b<*6{r3_Qy&@wm-k22OmQR{>G28$N^EZD^*-bt&NmjxrJ(l&@OB4C5- z4OXWN>G?OykgRiSazPw4$r@%c`%0?U8|^NEyh|}-f(i>gDf){$IQ5uElW45bW!4Z+ zv-ofN)pYJj+Dl5mOIMCS34u05%vW|k=z}1KS$!DJ@-PGzlRC>ku_mxD{W%>it|wA< zy+3|vE~zI2WP(eI1cIlKR$jJjPVT8qhZN>TEhM!k?ICq5#%0$DNRqiZotH4tM!GTS zsh!?6w%M{j&WMywK~nqdkk4c(>H@qW z%ldq%m%^z^65=431d$n|*GlRH=#3v@DW&f)SJ#hbd1d585a?S*nhCjtx)f$<1(>1Q zQD^f$hr6nMX_Wp%+i(64dKn4_pBt6sR zaq8Y^6%&lml>Yn>1za_md5aNiy$KRDPZV+=!E^V^Kq;C}^tJN}waFw?Ov( zGx_w8Gtn3_{aWrx1yD|lQ>r4ob)r*`?b&TMN_$F}Rz;BZ2PpfN@Z#Y)G;zn)LlK9F z&o?{H&aSAk{UqqZjpl+&K8QWFY>~!O6$Kn5akMtbT*c;72}Fej#R9X z#z_m&>6hh1IB>*~WN~Qw)&Hn;Q1ooy|K(cQRNV2+`NC}%gw@Er`vz&NG zzuVmOB@2r|og#M|hfQTR?zV&$LIrZR8&*HL?~*Vg&=zNATc;X|Pnmtek|5w+eSz8b z%BfvUnn$WKWDaBO=zO7^TYIPG{DBfiFDB+b2ox(PN@wDWi3scnySg(+b@zY?4dQSD zQ<$q=iBe6?c?h30&hHs$bDRs0RQh~j3<2Gk$=ppu=Z({bMaBg&1@b><`8HO5H-v(` z7Rr{uPs5M{D(48LK*s_c5ir<>NyqguO7g>698 z#|ONC9_T(!Cio7h05<}=XkTmmGy{DOH@KSJyWQ_b^)xtRpvC2ku7uDqFvl`nI5YFZ^(4v=4Mz$U*MyBieJ1`4=hcZTw7AGOgXj%8<6d z_qJich^8g#3_!n_6m}Y|oPl348{bfk!lC1h;xR=vx35BRtboUL0Z20c=Z^G!E+eLX zk3cyX4P@2cQ`jCVVc4f&T7Umu>Lp-sf7yMz8QMC%t;hKK?SLE`sVN9BKv}YPmM-L2No7a*n?Fj@;7V!Qu<< zT89pjV1?C?m$5b(QQJ1*H))2njec7`p-Pl7z=4%8gSjXncKsrXV688pFBiU5d7YZO zEsQ9z>Y+Es1;VAVNyWsnT((Rm7S%)()-ts<7N2NCARQn=ppq0SMnyA*?Z8?7yCMG{ z+7J^iBmNc)-jelwnhj8|v?y@SwA7i<#pWto$Rn0nSituWUKSn-PMTAa&n*ae2w2N3 zJ-r&#?w1lH*i%q~VKc%r8g|YoBk$8?HUPnNWwZT6&cLwYMirmr$}>0pTq<`Z!V>@4 z#pMRC_Ew8TKO}0>-0ipd2Vx|ZUUW!gWvvOp?M#?1hS7A>5~O8TnTO#Q-RrDm=XZpV1QjM>q>-gr7?>U!g>r$=U>l)roq z6o;5td8}%sXe2k9Tr?8^Ou`F(XJ|dy7;Yo8#mY@%dZreuR+1+yeUBHa5tSYX(Qz&ELHZO?1+AH0ex*wS!}-I0uzce=1j_n+&ma zb}&Df&>bYC_tEWbfYef_5O^_C&^2i3$3%yo$dTyrS)!k^@ljK8+oGA z{{OI#R$TMS@I*lKkU18+CLFEMEPH&K20PuK&pkV!gA$J`;1!b)GSK6&HZP)f$!tea zf0xOt{DKX8Ko{h5d)!|+$oH1P#%%bwdGTq;7>%+tRt5Yg@K7pZPSdTLv>;P4@$RXy z$%at{#UdGjd^ROwH;DzPFptuv)K_7P$%;ymI^K!+ABRc=h`@qrHPwW=ZSL$sEV6g+ z<0Ft^rZd|;2R}5QGL}64$s%CS5q9*C5iqv^6OLIg6Xl!696;B`Sp2>o9n2_Jy);-pC|Hrqp4qyQ2@ZkruCmX%tGC=b9R>4wRKmD3fImeNub`&mwA2xXTq-{Il zq2foCa6B9g*2m)JZL*Kr_05`N6&re{-r(#SJ(Ri{$T2*v4qU?HQUOhVY8bKhNXs>^ zS#bemIS>)=1hX}0fqXy7RGx(?4G$oa-nAZ4uNb^J)KU&7Oz&qwHVg?6B68D}3|Y&` z=iUdHL{}C|X`^ntCL8{2;5#1zcKQ&56zO%Z+bu%cQH@v+IHk)z zskZSi8rQ88(C-QG8G?!wKM@sJk zriR}l$+BgGS$5RE1Om@LZO5+v!M&^Q{c`sGNZ0)NTqC@6w;Fqp$M>X*xC!q-vEs-m zaKE!RA2a_Wi{I-V-WsAf>$w7_5hNV(6RZ5wZ$m1NN8QlEqbRLUPi1BYG<^PNYhIMH z!M)wp!THm6cP;1A61;(j}4A- z4cYj~qO!DXHkmgHog*8*F;YqwdEK0?Omnc(JKJt&Q~tg3)%gze{wo7d*Ei`ewseV! zi!5k=#>zC?6D>+k3u`B6BkPeV5}Xl9uk6%jr&OeuxLuNocQAzpGH@h>fwME7GJpx1 zX3w$yrR&*YieSZ}9zL%n&QC;5um&s4j~+WvW{BgFSe!nKXu40h`$2VLEfOB4S4!=! zFir+-O;^IB6|tcr!dV|*qLz5Jl6JHu#y@xQo%GYzbrGY}Tk~QW2%hyq{VbC*g@Le` ziE($x{!&*N9w7-J$+OmEDqJIr!En>d}4_*W&3wcpBRbO>D_yeGUI!{^`Ii_5XO zDt}(rslKdE%-$Vb=SgjOHCz*bvdHh2_VdNc@?kZbYnQOhj;U4emHKPTVGe}=($6qq zfE5=Vnlu?g4UG%g5TA#_2^qe!6rfK$O^pI7bw0i~Sz;0UY8XJ=6AsSH!FH_>~=m=k9Ogn@+xYJS}#65+7gG-VqlzXh?MS$M9vtYr0u_;hHAmR zKsU6cX>NP!p#`n}VJ2wuk7SI5;`Dgd?eO)Ux8xRt`f znS@djWC~va4s_TQ&CC=hfo<|zc1V~LNow>D)1MRUq`I7;88?*UpB(KD>8FCl&MqJM zXm2g9^*iZZz{Yp7eg;3GV(Jb%T){GwPVXOC%(9*wl4f=VjhCH-_P)QZeJQU}Xc^4S z@O<6lanOZfnyRmU;9P(DS=YWyK#t;Wwdc4mZRWHlxtog_Ph3P^@p?1CHjxj85_&Qj z7c>=|9m;HfykA|pE^a#Td~$ofzx&?uax9>jmbInEEztiR-|hE`MV75PrCtvToJ~+M^rEknyzSnz1z#Ac11%w>0ySlIUGx*jq z0bRZyCH*!T1l>j%hIL}CJ)`;TJc6;gLKX-)Cx)-T=yp#Q`^7m$TXGPyV;Y?3so4+e zsn+CYXP>o2RYZ&7Qxbs|K| z%anAsxu6Az3`0cNlo>7b8|)aA%{I2`lIp_}-yY)(* zg*6JaXK)+@D}5{j6~Ox^HF~`|{0g7yC-f!gos-~`3Q4R=x>b0h9j7c}e$IT^pNKrH zg-*le#-+vb!^+0B2kHhD73g#KcZ-M8e0|eb?juh`THh#AAhWUOmdi+-cu>$`6j;Hr zOROl#n}!o#;ul)KhPgAWb*vdKRWBuOc!$vhXyB%j8Wf=RRw)#M+l3r=Ie-R4=%ldB zetI(tpEC7-DBVhmb+g9GHFb^lO{Ce~9|`*6u79$aI9^_muEVPHR<$dao9^6I({>zF zu78)~KRF(f_~CN3$@O9tzpEeZNjjUb&4Tgj51Gi5*M#F9$bina4Qxb3TOyO9APc@6 zw&cH1T}oTuSf{W0cF=TjA5ah5$h3ANE_2= zK0fAZ`~aJcxjkUk&Nd&ndrSPwfW>lsTBxmi)D5A~mKSjbxb8Kg%(C6Kq~wYxJ27ox zn0&o0ux&p5YXld%Xy_cMo=)y*&T|xy)$(c!erOT^+a>Th+8j+?SyJP zeXnrqrtSW<<1H8JcM*qDs%0+iW)Cwv-GAC3;W!=VQb<|l#3>C6PqDn)f22UpH)~k_ z>oR5^iRPP1O`B&4Wp>BoM)y{FIRo)iHZ2Wo4WYh{$r(dQTO|(ZyWvf&I_niZss7hJfur1 ztUhxHJucZ)*uG%jJs(lrzPR7v|L&e#H~IMu>4HOGEQYBy5xY%y=j+7j=CAmpi?b(A z-zS#+IIbl=O^=NvJEuV{MP03q%>K^0M{mdQBDaMsSoj}jBGXP21TGc;n~=@aYZf4t>kVV*1m~pwMRB)l|F7dZPC0-bwpQsL=VYgPvKy3+vf4sifDs!Ox#>eR^qW9RTQX;{O$68W-WwaI--l?zbAs6PY(oNyMBIXDWmWrUd-naHOp}&Z6>#9-ZCt4BKSXGM7 z-rM~-uXOD0MU6mJ^R(F3@)}L;UQB++x=DDk*&4baA>8a)&s6gPB(?R4F|Q&zNaz)k zO5m>R_iE2Jy?6ESKu)0GQyL)u&lkJ3~HdBh*u_NBwvsR<4?VlSR1TB)(MY=cV z+ib-Fy)^LWG+q{a<&J&(-L`cjo#lA#o0jB-rxNr}J2M~ZoGsTudH=)-0R2%~bf~7> z4Ar9aCy@+RS#7^n7SeOD7{CB05a9# ztH_CY&)`*~U}_ak7P*h)@(?nCIzC~- zFUy@j+Q68Ov@%&R!U@O1@A;iULA@oW==I=dBq~7lV|Wc@a0|C^q&zLn*m2DZgSh9c z#q^pbFUEwleFnV8r(iv&wAW5t<1HUPaIaoNpA}%-emZ6Mc0O}#d|5{%(#>Y@Ow5vY zO4$rdat>D0TaoZS6R@7m`ndKuA>o8Hf}dK|(nC7qn&`oPRz6*zw2STxS)lxaQ>yPr z`RMxv0|W^!P^O&{UaXI~49fnx87K7m?ajv)%I@U}vh(d2uhac%*1KZO0~u(+`8Pvm zcpb0cXv{#U-=MPrB(Q}2$|!n>WO&z!R2o@ye6n&3pPm#{(gL`2iV4_bD=b{HEPn)v1n?A+Z^|Ht%kZKVV_wu>sEiLp8xLE(mMy|xF`G`v(OO9@*(7S;;~9> zp(3pd`$%o6yd7c+t@~N|$7`UDL`+@NfwvPb1g4W71yMA5493GvX#~=5I9@0Mb8-K9Fu*84J zepHOs@`Z*Q`;}=G_qOn$J^SP|#pbu`p*=A{?2~D;fkVicuKuu`eL0<&2_dKXYOuRg zu|yIlC&oK4@K#|+?Qh5HRED|oDpPk;UX$t4=1dY5$3kE@z*fYRWyJLKD)g||mRT@O zSn;-}oB~te^~2uL!HGjDld=Es{U-bRJ?mZNJ#45N#88vRVU)cenchE9AZb61@i*|? zVxyrsO1(~b6*ublu`}{WXs0AS|E2`t?AbV1o3ddEMgShAF)FhL{rdJQ^yBwi=n4y4 zzADI}oDLu3+ljAGrUN5)WPPwdd~k)zi~rjY}C zNY)H}lQXx0xgUISEP%=mO{>Y)V74sLf01hv`4U!rOehfe6HwqB6@)wT05~y4_5K)! zku_(@`qpRZA>W-mGTYL~JD(|Ey$%?ki+#8!Ry;T-Ru1ZOM0ZAU&8^Asd_u~Zn6{R* zCia4|`2xFipBPTwI=)S0JM!(eX_B}@n-K{lvb739G`_s>WjsWtt210xHJrMS3won< z&Ul+X<&+)?*gic{KIhj2R*l6857~AJs}|2>?3c( zs`(P0p=DURWLN^ z>EW;>G#NFKF5F4tvXxMN>z7e*qR`Rh0Qsy$G)t>)c8ya(-yJRt&QYiSx+KbIGoP|9oYZk`^;r^Qc^~ z*XhsuTV@aA~?=vdPnzJY*WJa~>w4 zj(EKn7fvl77nLNSRP72W)K(kKN8}!e%_S+-1~sbf$#3TS8c(2d@Qz<@%?-h;zQXKi z9KI$6vXl3g_g*Z+V@^`NqQ$>Z`?~V0)00ljMQ@Q!`xTde=%1O?<2ExJfY3j*1p zZJxN;tM|&cUfTvlKD(H6XAyA{q`z6Rp_2U)roU>-bZ`sJDVG3qTc#4A*D-FNe(yD( zM+%Ht4qp1)Z#IpSyp9N6J@3Bfxf(bpA@2`&9rg#uH<7+S`>bjGQ*?%Bbp_HHMZsXP ziUrq7C?vAB23jZ9Cp9cd5V^S~JV+VVdXqxMD#%Cl5Wp&$1<*M>RfA z_(O$t65{949k#tlt$+fp(6wlJ=gxf8m^o$Lmc$%xsdh$r-ni2T_gf)>RD9ZC9K*RM zilgo>7aq+uAwUe>-*0>p1b7gD6I$;)w&i(xKs}9w&46=of`fUDa_9KU{Wf5`d)d(D z_lp&zS0Q7SzS=^YI%~;+Q^7(1yylba zZbg6Lj(y=y(6=say<{3XXda@nQ~`G03u#tX`^q@xbRae|igkUtl4IW$#ahOTtkfD~ zoNmL{4p0|wO671CWifZ^0*3f?l)rDVwpQ1(ZC9P1jhJ?XQR5WJ(dIYiHJVxV+_5F2 z0fYY}`~^}7N_pbPNeci5tYO~By3n`26C>C4^U>+AD2J~w^#5y5D;kMhnm~Yedj6=CiC?DV=XpffoSbsWF89S;X zG6#hs-o6dw$uM+C6J_Fn+e1wlp&4c4bgt4K$!8-NyGX}+=C27Lp$sx0I5TwLX_Bii z1BX2NBw&gzHHpyyvn5@prW(@!As_!^B#6>98lET}(BPT{3S@F2;|Tr6T4Yp_6ZSBM z^Acf>y)3bhpH1gGB=UDku$I^YG}6n^7@4`nxcsr0GwYn6)7ePVmAm&bWU!X=8c<`* z{(!)?O^c7Sh$KNG=BKuyRtHg0!G*gRQg|WB>J@Vi9(8zZ^1+`>8;mxvs$A1&JUr%S zpyI$yW{fmlEYm>WfswVgUunm=#(rA$ya#rP#=h_tTPV;Zcqg`fcroZPZZ2vfw*!d! zHn{~$Y^7$)s$WJLLuTWppDo=!mB2lw7-)!U#!J*!-L(|Uh~qc+?6Z7`o)DXOF63>; zq9w6-A`*rf?#^`FI&_RvooYs7lJZx|{Zm!*t)|U1SEW8R1|d}ejXgnoSzbmqw@i$9 z4w3XTs|>E33_NWx4du}`lNB)xvX;r66!?FfBT@-g1WH*W>3Xx0c;JZAowQW@U8}ItLPDA}}>3MRE-IY@oF};4AeLhHdat)W30LEf(}foPwMQ-$S9q*H1}yeQnnRsnp>@h3EevcLY;<*s>p4f zv_$ahVzo1KErOvkuhaNtvQHwaVLdHX3Nmca`P*^FsuQ_dTJfDR(H{y3s_tsTL@AI< zqGsikvveJM-IQlBx9KrLj87eYo7(NS7wV<=dhP@p=OIQ$eed1wMy1dNRvDuTi5g^d z&@?~{zL3)X9>Q(XouG20xgHXk(cga5=QP|pM}Ysi zPX_c44A1>a@*? zU5$V3aAWM`?#Gi>NelLQdj`J8qSG?4UZ%jIiL$Y=dFM7ARaMo2xsM36@V(2MjjF8? zu^9jT9+75J?}r_1e(!s!f^FIdV+!&!gGh9ZV(hs1b_2&MW5xNh8(xEZ9?j}(kE1=( zXMSTZ#;?tcG<)%l1M@XD?3TwFGEDH0)Tqp&Ql4uxF1RyC-Oc{crs+@`v1(;m;o%7d zo~y%SM1=|bMU6LQ2;5?g#CF7*of1_@SH*dF%u?|lwF{Kq5f}w$5AMV14;RksFMPCx z+Y~mVN7<9!XD}P-r~=$fVXFbp0=$_X5AA5s8=6xG&X+R%@9F99JnqjYrR0Xql1Sj^ z!lhVOGd<|u_vQBN#7R90+ymD;rz2Pk&LyA1(i))58zPTvYt|n%rpx%?o z;-)Oca3>O}JFrxro{^T9rzN4bXfm)m+P*Pe9h+x7etB(9sF-O?GnP{3Q?y#zm9f%t z(W|>&MN=4$Cm8y*x%UbBHaGV*oZ{ho4aT+xF?w_o*!At>hN;viZ269kd(*B0O2DL; zVah^rtAKnWrvU|T{^cnt!^}#R&H)Z7;lM(4H1pQGhUgNdNCRo z=+4_lZ*PqggccC4-wgLnR?4{s^n`fOm1zy`YT`M&el`oe}m|zq^t)iFoF$LMRF@^o>isUB1 z-3x7wFn75xw>h1J<$WzV>3}1kq(B;{rfN8yv@xZwcK;Tf9ct~jZ>c!h;%^NVbM)h& zYH7(fHCE-&99M&dk`fm>pxoew-!1)&53f);%5Qh)Y~P+1@3Z>aLy;hlKkDB_BsJHMian4pxiI^W&(+{tqzO$iO! z_t9Z%ET#Cg>rylxL8)n2sWp*%K5Z&qbeU#avJ=>0q+G z`ZBY{+nB~B3{)75;yng0%}G;PeXQ<-WCPYql}t)nOlqiv8l2v5Y>8GSSy&D){U2{D zf*On;Ej8qTi0k2MiXK1G5@`+PmtQR*GZwL$3gja6Ant#p_% z2-3+B8HpBPG*isPIuf`$7?Dr(YLvjLvULJURyeAZ|Id=N{H3G^A54rB^M7s|t!{-2 zPOJ>c7EKDp{iB~AEO%&#hILe2Fw!qRXuB}#BsU`jZV2uy(5xbXJA@^nTm&23(U zh`BAM#NXw{j(zR%P0xEjuB+jhNV5aIYfM)=T*X_dz|TMzW#L3beD=9FYqTE|dP9A! zC^#b|4A>c4GD%RYYnvDQ#G=3lG76ag?UgM886|1N+&~rc0&U-${1;lPLuyYRV5m=r?8IJ}fDnmBrmnkr^wX@i=WhG0{t_rE8Yv#kf$1#LB&$XT#3rlgpiC zyB->-&cF3Ia4Wpkn6CM`saZ#atpdM_X?Cp@+6YlZe;CHt zrpVgc&fRM{nrn@3^)L7%KHDf?L@_;6Nb-GTaMHT>4?LuX0{jF6{gD>E(k0!lRF;I) zt-oKb4XjaOSx3#)bTK(|_F=$F=m+8<5g(D}pcQBYo~XWTz12xa!0wLZZk!Hl|F0Wk zJIzao%xegDHfFuEC4`3<)7V?BRNVja+wCR@Ccf zC&MiUY8_%4(%!Dulq%TC*#^Y2no6jr5ZoY9`TYBKxK|ti-}eLHUUyM zMH%^8{rqR8jBL*g3Her)?fbOp;B6NQ_;I@iC**0;=TAH&&MAIfb(J*fqKm#AnVodUKyEUV^lF@pkj)J4Li)YTM^8-v) z;%QFDN*h_0rYOgh5+AGs?b~Q6jC;N$#ZISS#9#6N4))7tiOl>J*}uS3l9E8m+fH&< zN5gDr=ycp5YkEdS5gAf6I1YcFgmm%18N%()w3XYv3tPeRs?pny;O-gu{qG=6U-+EZ z$e;W}($O;Fsvh_amDIC8h`1#$KNV336pzPcaPeKy;vk#8I9h3`#dzq>su(BsAq+XV z&N}WaY3@89J6^J-@Ru;dUBPVZvV9SK*L3^#_0PG8&&T+`*PH3`O6K2&S#%F|xmXut z*N~7g5GX*~;-W6!Xc5C^6t7+EYzz{{!gTxNyJq+P{X(bPhk6McMiKJBb&vDu&9yDj zspoyIv@Q*PAG*t0E3N7J3d4>t&H3r$DQ+Odss=E#|e6TPf0MI6#F z71z|N;GxAJY&)#DbmkW*z1cHc8DBhbZTkya9ZB?1t8n9i(eTV4SEAq6n~^`uAqLvd zqmt782Y0{++DkUuD6vHd^55RV;1haz;Zu{vOn`eFbTY70qcc?dmQGxYi%DYgQ(RI2 zVc&iS9CEKvvk9|6ax6o;lJ0~hk*}E3+4S_;bwxwvH*4pj;~(A`F45Z#+V)DIoAiY! zNDkx_Qi|JOE@Ml$rfTC-q~u>^#G`M2*0UD5t6?XW|GbG`KdGAr2DG$xpJYZK26#)$ zBK9teP;CDs`TOFv1^#S4P!{2a3vFyBah!p(o@q@mQj=xlGa52b_R*)({ZL_IrGe& zoo=%!sY-D$bFj)u(QjHQ9kn;+z2|jKc3WhzLHB|74Pn9U{6*nInSkN1rDbMesq<)PUu|ev#k6*U@ zbup6vy10Y|_j4~_lEIQS`Y0gjgF_7zn+OhlT3Y6KBp~EMvN%)i(yT27!A56 zTsw5f<09v9>N__S=4_$NtgJ3Jc);K6D~1O*+ansN5r?|j_bCi(5L}RlgE)eaf6(T1 za~Vxh6lPl@;~5#0^iB(0koYUZw|^8DWlN5ODC1W2dE`8=o3?x8*RBWP)WpqU|5RB@ zu*UYbi;Sc^S=|EkhPPaDtlHg^?5EL!l!OT`0CCX$>y0^O0nvI8ey~mbos&OZyo+_L zniausp}H*%t_@W>y;SYHylFS!QMHEjoL@aFvs9n>oEF$naxM7h%h~HQo_m@;u{Cc? zipliPEr%EfXvTH>YNE-@sJfU>A2t3!cJA3;dW$Q_BCr^z8tjXui4)w*%e-eBBk8!8 znfa)}1&)KEn_Boy$&{${V8!`zYI>3}H0q>3s>wuAJn@CGkl&iHB{6FFVX}1SC@ngp zKq>$`eCX(QkqUkuAug6XXwylq)0q@79Pxh}PB9(j#-mCAuE~P#atKv$wWxD}q~m3Q z^ETdID&_&jj=U2D!{tc_MQRM!{kA#R9!Fxf= zg@HLF15Q$cu6Jp)jU6txJYboqD%h-ztO)b@v4#AopZ_L5*y16V7w+XA(K_aM+ClkZ-d+wL=zmfaZw7h(#VORT>n$=!s6u=AH3QoQ zxk%To*TxL4a?Rj}x18aXN2JFY@YL}H#a8?tCR;-x!>)&}y^ei#J_2d#Ha(tC3Y>>N z1R{HS{9zd*VPnBbI%1yBn|x43yQG@CZ_tJwXOpZ&jis(A_Uxe{<;g2)5g2P8V7K?g zMaZ$J8>JIiQl0{LrASMtrdBw0q^f*a0T5902;iuE!cu?`lZy2JzfK|$RWS(}32_jk zZOR`u_- zpEezJ^KS2H>hfb%*|yl?u@xUM#@cme&M`>BhiRTH$%={7gNBu7D3YvJcqd$&vsC5# z;T!@jt`AyGeJ96s8eF+#A-!NEyY(gu){(f%TbKA{bu}iZF*RXDR;*+cGu#qD^dh-QxX%#qFXk z$^cBk*%mVo!oicEMVetH4AN5{>2e=$gC;H6WOoK%%KVDOQ zGTc^`c-p|R$QhV#bhBY~Fet?@$0H1BP;TqnZ-1c!sJL55-wU=W9kfMj>BwWU5;Zep z4hy!CTpFD(Y+~bJW`k}gvV1tow6KDggQb48eg7NdRI&fd;{>y=6(y_ySF>z6JvI)) zbP@Gs*6qB@7v88L0Y7o%qM9%{8csL}0cAH!q^rf3R$73db!mT0*gtQgwjh)3nF*^9m zI*TOnPV$KLusN3TnA?YsD$$|^seH5E`vVV;xJKZb6lM*Es{hB-JI3eP1>3{1ohFTK z+qP}nw#~+OY&(t3#Ojz26;r3ZN+5T~yR_E?npb zAhM_qmaNCDD-1xKV{2EDce%{-O0;?3P1Q5SB?#zAD4jnW`{pc@PuhT~U8&KIxeB>Y zL5ikQLGm9QxQzNgH~_Javh9#=`1Nqg=leXLI<1!t{Nowsp`X8bmn;*Iw=Hl{>v(40 zrtIn$ry%DmBG{)F(-nAhY_UgxS4|So&drI=PpT$@tT?Jxt9Odf0&@j;8B756xD%68 zv~bWqN>9gF!CPq8&FOPJ4aKa`A}gQTrjl)3+A!?z$hivPV$c8vice}})M1A=qF~_~ zF02&T@&36|ntpqDy>MGfCl=RFkd1I?4yah-t@tQ|-1ADjvdz+t!@*Bd2S&Wtu#~0C zbkg#;j#2(5kFw}^#wAs~@XHbD>iG7bER8})Yw(=8A|jYkVc#e+pSd;ZfJCv~M(qDI zBN)m5_@rL+fB$`?Ms2}-bW93F=U3n-yN4-u>#r%^aT8ny=1TBEgDgDl!7PnP+)m## z_8K9@1WQK!#swB>BYOTEqMmqa>0B~%88jtjklT_w8qUxF@`Vg@6hfmX+Zz8knUg{d z@X|87jwAh^YlgKH5|WM`S65=)jp_JiYP;rmam0c9D-gJB(5gx30PCB9G;H5wwYm75Ey*Tsf&$ zyI+mgi;XxhGkl++JxY_qE6(#Xhr>tq=}GE2L+iNY6vsk6PPv&B#QpwBQYi67G!&rH zk?X#`wyyYt^h8gPq}%N%DL&)^O4bMH-h0yGP*cQF-EYpS!DUF4E$15d?u}okNEr0?=-|?QSIxLxUJTGQ=7@opw%WcR>@@A5xdZ z7jz~pg6)0Z7#gOZ?=Lk@q$Tpj=zivZ`YWX-*&D4l!^)`(J=3(!j4=L&PEG*iE2aZH zr6n-it1cLUEJ;sROa$(``27y`jv|Y!CQ4Yf*#9;25GRs!a`IwNk|O9-wl!3ig%x@V zwzXg5QXm(5f0gCx%auY$thTx0hZR3-{?`i-?c*OY9u`JdVpndr&IT$QNmzJZyMg6QI}9OxIGJHfvUM~s>eFT&l) z`M5}E6{B|wAkpD3>3Zgumt*8izUYEr9 ze-wl3CbP4X%uzG|#XTR-NyuZ$i&XNr~fUMsljVu0~(tknD_&a|Dy#EJ1 ze@n;{nUCB|jG5XXPbXhLRZAJ5;n;M!X%&VauPCmc3ex&QLHlGz(_ccRF~s^K*y8u- zsDq|C)UKyWgG`oiLBUA}c2ZncwFN$z0?tUL#Vv>m(S+gT-`7F3PzXQ|Pu{|oE!NlM zu>Bv=*H0uY_`iRMyMO43xY!B$rRQv6w~wk3bjR-H&XuyZAj5-pV~c&vg~x$c$ji!} zxRQeI%x59DwpsVFi3Nvq{BE6ikpb7*WV!>+OH=bu_q zJ7QBv1MF67Uzo0nNhlO%rTINyk)nzYlrm8hgL>D&I)-0Z=Ka^o8NmPHlLM7pbt$oK z;=q-8?-4UI6bPVLFbAfiwCKJwl>PfXbv+KQBG|CW3wup4<~|U(At4763qL3=!Ll&h zWaSO#f?2a%f=)5B)tA@uGqId3TV8YXPc}K!Z4EE+xp)0VzYW~34pyhaR%Odi^ebGe zHe!VwJ-UiFhUqoJfr%Vh`mgdhawW+=h9viKRYt?p>EM(6XORFN!h1JtgWJK|wrx`l z>*d_oEByAMvIpVSJwlE>LBF%^>bk9mcY!HRBYTv4f6jwa*h!pKJqZTgN#9?e6CZNG z2Q*qs@xkWXe;JI;dU9_4Ik&d+dU}&2e3(DERge-_D2{xGX8Uw5Uu}tRYW~e>MbGj; zB}4tzfLHLH0U{}Jz+K`$q?aE4Z~I@$QJ29nA2C1jrzG001d6;v&T^}pp%4et8T}t1 zy7_Z#9DRY{7t;h~r~n44k8KTe^64aHX~FcQa+Jhl@bsxsdV>|-GaZ3YM;ME9A2?}y z1mkb0?mQK%^h=Xa9_$_}D#wo~yKVLJk(|KsTy5*}quxQoPvXY{UF^-uNBQ23NZ~5$ z72`yO-5E0Z-SM8C-u;*7`Y<&C2=?@DJOtv_z2nJG65{pH+0}i+p38utJV?b3e*=dO z?ljuas8e24-s}hq2GYL+SXt8)%&I5Rt%dd=u39n-AdH*{(6|v=v~0Aj{Hs)d!l?eF z1e*3jeKB1tTkat`C5*qkOS}KnsrxThmR{Xmk^jXhM4=oAl-ilcPV;~xkGOwBC6}WC z4AojdV73u)5R_X0iy&H}v!75~mt@We1Ge_58KbF_%0i@$D|~mPDW~TrMXs9?L&Z|P z!<*%z07KR}jEWUEsrV&%f>9F<{B52Hg%QZlG} ztDR8yzN=2Zv)X|j6Ok(W1npjzxJDkMqx;>}ogXAver}OP_kA`zh0WWXRtd^c{>>v- zPL_sf=Gc)QL7O=up$H~BM#^@xopXT^#LKMBYQvty$jEm|9d~!}^~*ZB^-#q{N56Un z$iYrk=6#SlfgPkt5orc~F@u5lzri0l%&AFM23;C(<~j9`Y6r2e_YJzdN~KXpcJ+Mf z@$xs}c_TEzPyj-9D&i8gYLcpwD7(S>#bInh0&7f1)BSCaL=U=2+aphRN<)7HRJpB! z_>V`4;o}_NRCR)N`e(oN88(Z!3dDVv*qzRRmYHd!CCqwNypByELNw*jq@Q))9bi zwAEV3df`#fqR7-0l;1Yb?i{`2`eWh4ND2Jil9fAs~j z&^^$ydKe0mwo%VwxyHHqApm!7p%4!xM6YtCg1T-`)$Ph_SpH|n#&(is@AmqB=Y#^e zn_}>aT7NT-hx8$7yH*b#YeU6(MLjz!E3Y}HB>k^vsChuKAeUQL2?=A`MZY&t4<#WE z?-F@xLY~mBc)+HD8Ib-LiOk3=8^4|gemP34#(=BN4*rF1GBG01Y|tr9&9^9|A%Tx* zFDV((uFMaJ{L=-QE>sOOG;T9ZN=;T~e)XlkfybSR=A`1{`e-@z($}-{U}<@2!?!BkZc)Hs2BPGl);%IKqmqEGGkCH9$(eGE+JO}b6csgL}Qoo^o!Smqv zJzvH*%C3Xfio$lG2%oIUv1T8$8KOVsWW#cKv3LR~B z?1X>D(xNwz>Z8lD_VG6e_%}%_=ohm98CRe{vU}63%eVU1p*bEHzwebS-CaNib;B}j zsZUG>6iTBU+Gz8GLx}LA|1FP8ZafUn8vH2m^&M|nT=A07#YLV)DiSk_c4m2`s!8f< zRWi72aiFGcoY{ux;`y<)gtVlTHd=}#$cXXt<502RRCi!)zvrR%0D(Qs;3aLah5=Pk zD7al(D%t{8{ffROT-VkhgJi`|l^OP?sIqg_&1=yv$VW*nbH_gi#qGOncAo6E^sJp- zr^0Ee={-NDep8he--cRlyC3s5c+KH5z4&CCdH(wQ1@<*ac>G94O@1=rlm2c`$;3+Z zn8WqC=rhF0i)!dDYNeP-yFT*SP3v(lNpaj?E4dlu$QEs1IR7eS)^@ib;A(??m|myJ zj*op*js=OXJwd-&e9V}zGl`4$*&9%s9#j`x4*1bcQWafjIV%9*4sOFrN(^CAXu&UD z%uHvr1WqMOHUeA;{^g;p%-6{C*3c#~;17XqpKW5uMT0C4VLvp8ZjT_;7|0TeNy4tW zhi7!No^79t%=w@qOQ$O$mKBl`lf$#d&lcKan~zOLney%HIcrK*W4rLgZN=tsjN1e3 zhmSs9*q;eTv}_B{hj3R0jF_sNa^<9Fd{K3Ti=m|ZZZ8Zr<*jV2Qjl3Hw9G>_o$w69Ii0D;1d^*h@j%$Rjt|$WUIzi(!Y;8i=n%+C z#-oEAUnvlJC1I~b>MmVON^X@Obr?*mCgm4C$*mjhhftLsjk*YZVG^UMpeU4WUdN*W z065>nHie&V`(j?(=>AdXGA5e8ncYX8IgjVx_5}(*9%4=>=!#L`q$g3TH~Zk#nwwAH zjY}gG6Q~2+Eq zl|s8RQj(FzQg7E#1;o*jRCI|M^Lq{x2-QJvmwddj<0>d%iW1@m>v|v{<3wnpx0DpK znJF)38NTq}Aw6g(!oI#kzxIH(Y!f`bXAi+@LGrjBuuU6rM*KZ)yW0&sYJH&$^WGT_<)#1R5^oR*> zo-C#_1|Z#m|G*0GkKeH%U>ZZ%kboGSO3_0WO-t)VLw&=n1e!O3KBmRVN{Djs-|)VL zWlxtxbyuuF|AUsnoZJvEt&s;L9#I*WWqIO|XazfGPfjkARg)y6#xl=Y1=g6umo5vN zKsjLRpW7=FM$xgXzB`QKbzMY@&2OB2be2#&b^6+8SznZxedDICblJ2*zzX%g<;~J1 zL`6eg$~l6!L9tom6aq|+g@l4*5u}JTX6HjIs93~BB*zGY#Hpz5R7;v2RZjFylKWtL zy+hylKpCXOj2)x|+aTX>Jx4ENb*?iffHu?2LL9eoE$~rOAOx9a#8^7L=E}@V^CX< zybuPt84IqGLVQU8I;NE#Y|7FS6{w%SZoAS6xz03t_{%th{|)Z@UvYz^q=ehGfRn$0 z>48rqw?K3QiO=r;tA1g;Q#I^0O=#1SVv2e+jhF7{L-h}@0@?3%<-5ibsFg45q2c}W zv{-t7=0ogt?2MvKA*rd3-R7oLTu30jU4$vz8h=#cDLwlV47yjjE??p_SpWgyCku6@ z%%Ih#ZnF=gHJwo6tgc#KvVUGaQ|8ko#Z0a_#6jqT-vJVRo$%#x?K?ICl$W+aUDgTg z$bpN1WTVb%mc<-b)EV2BoV0!@oil)WEWBh|kNmPOZ}g+(oEc$DVA+4{;kTX6dfBNk zqX7|k>q@#b^0(k{F7=XMi~RzGzh8tq(l!s+t-zoIBrI}M)VQd%&HU0AL0#8_c0mKB zx|X^zgAFNE*@yAQUDC`#RqSv!i%+am>Lf{2Dl?w$&W=j%JgAVmC;MM7Cy#E0fmP7y z9w@AbUCKU9at-%QPiBSE|KAzjN18u(J99gcJZ)O3p9xX6AR~>*TB%(vEjBVISCqlb zVL`2okeo`QqtQao5t`C$!jHM2iB++(Ri+tvL5mni^>3i)3v}i_-J#=6pB#N#jp)%z zpy!qAa^Z&95Sv#ms)$>;e6t4sS*M{C0rXMT*@MT*j{8NB2^|uG>CDUG#G&92$Kg=z zMt3m-bKb2I_A`NB3NLzIJ|93XAguT*>eaIvfc%2E^rENaOttiG13shs72K2t zr^X;#tq2pqO1V%eio*_d-IeEefhKsr4VPdJ-! zc0y#e^X)fBA9?qdVYzYu5O;i(Ywq|dG^(j2q>fN7L|@ncwVYby#UZl>J)j(O+;}27 z^A8>m(;X3Ct)CJ3O%_7Qg}pwZh|&43&x50}LID6!CauHO;&WaIRBT;_2U0z!M_X$4*o5=)g@OX1^q6y``i>fauhtblaVdX_i$cjT>_q{9*bTiFsF}j= z$!c&)43aL;j6usAa-IaS!XLF(2S)wK#m z-gzK#-H0AoBY0C<2!v+2K(Uv$J|@C}*JIO;OYKJHUl8mI=N7_x z@Ayuge6fV?t{MFOwpXiMbWsu7uy zPyB79-#Rzq?LAc_LeSH)?%U2xHppj^0eO~{l>&K=)+!Uzz?(rGDN0j5v=#v0rCP8- zfV_G@zJSN^`yS9ZBoRjtLX?wmNkjtw50Knqs;!D}$1L(BnHq({1Tw?XsYtwJK%EP< zHMd@$4}?uc{gbKOD(K9YmJ#wRuR zl_t@Teh@+YgZDjq$HTW@Hyqkc(x$c2)$6}1C@eIXxv30)lex1JPBLgp@;bSaq@JFL zf|XFab=fbhfkh#uS|y)#tLR{N?# zn)TC~!eq;%0ywcU%RQLoWTb#tLm%kF<)B6_Iq&oGJp|{2dKY;RI13PGU z9L`E|6oDjg#N|Yo{)10K46L{#Vf!aTE((Ew*U-s{?ZLkDggM@Jeqk$no8*bVsB4$^ z@6I=iB?UtEvtqk!1_1oRef$Gdy=gAMu9lv&o5cWCtTdPsz+c zu~<@e!g`ewc2%@haEn0q`RqN)17G8bfcENsa8Cd+!q5Es_=8wcx@NXdkSEhGR`5oA z7(OdHh9t8@2DmDRNrA z3S2No<3%=oAnEfx%?#;cqt1*Fs9s-=Ux|s1feJN~d z>4(i9tNkx3XqU+r$LQ2mI*5ysX*NW^>HHZ-9&`GavFS?LIX)f;)k5#X%*=j{jp+`U z@kk-{=Ewe51bW=Mw&&BYkKl1bn0SYGV}ZWt-N3&g|4knqebYz&89_NAR>F<7DG&Of z%^yacC69@Bu|!9GzB3hCFU{-Q+}XZu7%R9Lz92PGL2h|GZtZ^-3R6mp4prB zeC_=lG4fTEFdJEF>6K(X*I$eXGjRDezVyzFod)iELP6oj=eB{OGxpDP+3}Az-=~zJ z*J3A2Yv>a7NEA7iamND0RA${t-i#`i+@SNYIWoJg3I}%s- zUy6WAZ>S#!+`u@H{P_DC>hL4|gZWp3tl4WygSrg@l_U~Kiv$yLdZ@+B@&fqeKQFXP zkD{r^@w%*xL6|3ozbAA|y5xoEY5XYn)^XJC8>)|B9xb7B=5g+E@%NEw)Z|WRM|ktf z5D{rj&@0Z6)y`EMUrp4+|`0VP2=m9lJMQGDot?AEfw;*dP zrfr{+R>JkN_2rfPzBpyLx$oo_kSNqAe7Q*T>rA1>RM`qlqrKa~Z z3^*iT)i-g*NagKedsz!lxw;swA?x~Wat4XwTc4qxoN#mc?Qj7Ae?-s-0MOl4Q9>@M zve;H4zMIy0?`iWHX-SJ>PB9z2;aXsWE}lw+Bwtg?X<#@I-3T2J0230KPyhga8%rrr ztNN5f1fv-lB`wS~<(`Gi4gZgpxdXNcL8f=>*bF8n7#$UWDJ;ZY?d&y>3s(lTWnT=S zXkfZfuk%XCoV;iPt8epLz_GwI5}l2t@zfCB_!eqQ8-}d^tC($Cs>{nyEP|h_^FH{d zDo?9!bZ!3Tg>Wp3XdoYl+;M8gQ ze?yQcx{WkWzLf_LqWhk-)9n9)gbB*-*fP0&_~nABj@2mF!#9^LG=ApcF@?=tfoYPc z!NCx~mzilXgF)jMw`6;$8i5n@yX>KV33IKAe;;(VA3V9A7Wsq)hE}jm@;)fFUGb`; zEiy!udYp!!z^3369E@n(k56!TlH4rxAzu+A&K9ipOI2HDQQ6*>sUbko7PIU&#A%TF z0`B%9_~jh!;m=n9&`M*2!VEN?DvDwLBfGVe)E?rEsV;(i(#CaI=HOc&O&)Z{TAzgs~RFt*IvL&c~UARKpwhpi(bhl2PMv^PwWt1g3cz z1bsFEP7e{Y^0+XMk4>^;(r%0v^&h@|jxkw|cb_ckFdb1-b=4DeV?}UJl0Lr4`BwU@sXNlutop*FE!dykG=&aF89^Hr7a{9TRUMG&ajgi`*oB zvJh+2J@&BMwf>lZEPW`*XQ@)AU)in{wAhzKx#jm9V03-@c~ob2T)0(xdTK*LelB05 z*+fmxScZYnU_J1QMy0NKTRw*~KLxOyQUaFN6idG)$caeSK6I3qm*GycB~t~x0~FcL zB=Ft%;Qw0?%fLWQ9?(AmF2ev9sLn=OA^!~_S$v&wk9Hs*W>_DhWO}g&gnZP6GIw4* zN=pCG6FQf@e>DzGR@d~P3eXi3`AgsoQGQF+&kn(u9I^vWZkCS#=-f!ae_LqY z859&T$01I*6<>KDU+>>P9?}H^H@n;{{sBySD4Z=3Pc>mZC_X4?CL9POoEg4Q#Rzl? zHCsk{3Q%}}RG>eGD_)VD{Hb?jN}bM(fjCvgN|ALppVBUDF6i!fa^A)=DM`IKKolVi zrv|ZJBVh(UF9R{}a=Dn3h4s7Yun!x*-ioVUk@xTAuK)D{&=kWU(HT_BN%|0!R3-zI zC`kR`xYtG=HT31H(^rd6E@!I4AdgR62?E(nLf?#XzfgFD#l7OEJ=`Y89{j6v-VL(J z^xdCo+WiHkT#U0)fsg+wK=}}KU-e%Mm>Ph0bakowr&Bg3{M1FOlv8Hr z?5;X(Z?}AT;1svh5KgGealTRlvh->`^|zLy?Z(L5i60Bv*4CKFE;UB68lR3yY;pwr zGvC9VOO@7WCIbn)o1&_wH&_G7!$OUmsE3-C>!y+?Bkh_k%)$-(W z%*Iku##C~Gfw}jam*V7*1r$ekA$@pNq%0>UNz25tW4GU1zLmDsy8|zjW@^@Q z1e-1F^ahH$D_CP%a8=m1Y}Dq58O?CsI}`8I>cKsr@Qb)m_5m zgk5&aGb~I8_Tm1gY>#?vNOZ5g&#s^jUgM=bQIt-MTsC37)YLLhMuikP~Cp)$p8aIpAH}mY< zgXa?_xN0RO&{QK#vQRzj2G(!CuM};EMwr{IuOW6tdk<1;7ZOgNe-1)@bhU1_bwnJDCJ;XQc*66(=H;8|PQ8g9j3jdH z-hevfkqS*8@aQeK$I{hr*%EkPp5J}#|Gyis!tJec)yDL_=Je3&K!KJr zS*E4h^__(2XED+O;uT~%xsy!M+EuP~O1|;m!a#GkIvOU>SH3eub9br^ zyTqeq%Pn>I*qT+McVZ`MNu*0YQn+U7*p1HK!rv8#pC1_q+jZ~hTwz_~`Y1_|D(f`=n?y{(z|*#w%Ya`XKPT9LKEMi`TmKe@5O<~Ddfc$WM*1KG$A-^c1xmaX2t&U(XB zmW1%rQ?{zcMxjg{?f2tV@$3S#p*JW49Vc$(fHqB1PI7&Qt$-g#H^4~I818iR03EMk zz@zv7b`L!XDvkYpV@oOhL$PtD0`l@84lJfl;wHOY*1~#U%;C= zWB;TLTaStiQVNOk50qDu^dwLclJMe60xL!&CxesQdTTVaqn9$^J})5<5^NUz>1qOQ z#7!E0ebC>|FaMttJB&9Hm?w%zrIfa)2*fyIb`Gb~bAnTUN#8Y-|oG!X58|viyq9R*C!QGPcf1pGwU90RyZmeH9S&Crp!z84HZ?>#lrkJ zOdAS|Mp`=S48$`ObcAyDwkE^P_F1b>Lkqj~X}toa7i;)WM`d^K&GxxJiQNuQ@esB< zx7VhVu8@gw(ps9-7pA0@juoXA`O`t{p$Y=8JiO~c;{pQuhNk22_!GWGAfBBFUKj^d zX&S`PxD#&RET9-Xp^1Exl__-oi-2aNYWwU$ygmXuUeu~CYhKM1en9jNti6V>!vAx8 zwkUuaz2>lfmHEGPXA8C@;^{R?!iYAO8*k*kNF8qQTX5xXnU`vb&^S=mLBn}4zi=ETgh z&i@B?oMf-hh_&8 zTduDw+wg~hxi7>%`g+^e_Lf~*$|^f<&85t}**O|Lap2S8hoLoCA2%)IoqfVQ%!Zo} zt*zFk0UmVgktfdf5X1bqH(8**@>jg{nvgGvd z^WToiM?^2C(}jd!nl^J<5|(Ki6PCK7mbXI$dNS+}#lG;r`w99oDzuc`rUlu8J50L{ zhoEGlQi0cKvCHX!C_$FNA73qe&fZ4E_gn6eSEF;k3o3I#?3aTgHq))FymOJfYrgU7 zBy$cmf?Scb{2Vx^3b<8Aj+hZ?kuLBNeES4~MSF|?@m&m9qo8u*{~uG{f-76DPlHw> zhxFDx18#)fzd3pVEocL&iNG=AewBW3MV_an1252BLPx1RoBX4xjBJ%#7xv;J#XMgO z^-}dj+5l4LrOZGe{{3F?ao%Jo6eSHuN%Kula+Vyp9Mf5s_tG?T{D{ksob`qpDI$g0 zxWKrO+HPj9p?Cj_%-qDK0k+yc-H#5qebd7h8u(P392Z)si@CHVH+Jw<{HQ1P$AFsJ zULepVg9nzY`L6y@1rXQ$eWe5i8s+%KJ4tYt=k5@+8MBZxs3$l%X30n^lnx5`S}-Cv-{hm}}%KJJMU1~2g@AY3e6%+Be~FwJ)4Ppb!g)I+-^HuKPn;Xt^EloPU4 zzk|{H@c^F(^cnyMaB)aT;nGp}xn@h}_YM7vC1i{w_}n@c2>CfCc}JTTyYScuq)2F3 zWC}TmKhcvZoKK}sR00XShbwQ%i~u~$nO&Wz%f$%cW>X+_H29CR&s^AjNPF)_=xsH3 ze#0Dp1D|(Tr5dmJ7JX;jt!Y>%yF3;HEPQ+r=1iGDB0)frgcFBc#!KT$^_Zlh8;ET< z$*v9@d3hO#=@@rd=O?>Fl)RaCwo?U-%H~6YZ1=k+j zS$0rAPqXdZU3M5fE~VjsXf(i|s@wu8m7=xbq}-Cpt+@7sit2h zy#tj3v1iUKcplD5c7RD`O)QSr!B#x0omBc3*iNenmkS- zw{kMQ-R+*V^>)pBkSj%{vAI3So);k@K6$dPrlNtxJF(x%_ADUGE-+-ou}xbq!_?b2 zZ(^??T6ZvFV5ruZ)rInKZ}3-$xQ2?C;=`BKCSdEbs7}}rPsf(QQV!%3pF&K z@Otp(Ksb?A$x#X?FzG-x_Apb68x#&4uI|Gr!dyH!OpOp+7!cqpD2>+8r87`I2@* zjK6i*a{T)eEu6CReZ)r?FaiQv=r@E|>Pn=~Z&zY`t~BF8r{ z369R)jb*)Cv0Z2ln3%c6a=}H15Jf8|o*ht-bd0B->(^fSuwd~-VKDwCS0KxUVL2z! z%q6qgl zoZ4X<-<$Nt@?s^szfE@OlPtATO~i;y-7Qbb#C$vcR$s8vaEke;cC7nO{RAzxTk zDX;z}<}CJik7(7R%yEs|;b6sylc~XWEp{uWo!ySW2R6}2A_r&XL_>TWw}Z@4J(g~f zSKl^`zIXQf>FL{~Jdq5882QKt4x}V(#7`z^d7DbNa*Z5=u3{`YU4BW8lScWA#BV0z zRV!uzWQ#rt1rVM1)mG$bK}@Jr?*8Lcs8}t@ENPWQ*h5OlDNYxX7pOy3Uq_9|`95N> zQO+!BGJSbe{n5z88PFOO;UM+s!T>RlZLoe4F8B(OgyEav(4*iW4Wt~FBNv*_i_O(N z)4{n@D&ppoYM$iUAkc!Hj8Qbv6uktHb~wG>{c=mMwehl~OZ8bBZ|_X|%~sdFU&4!( z4!~UJ?Dx5R#38_oMe%-jg9QY1`RiLMFoLqYBUHvL7@dXJ(x+h%Ha*hO%AYl{?EDxO z_%OQuCIh031$cCU2u}E>;vPi_yHi4dm`z8UELch9lTB&mMbt4fj(drx3Whz3a#_)+ zCfOOw+mJeY3|o8`@y_II+m4&D#%?EZBff>h!BJgm%nnwvZ_%!^rZSCE1h%CauQM};-z##y@}mfQ(GRYOpBU{vpk*^5 zeKO;Ag2jB2%xd6K?}f}D4)A!;8PJoen=NR%#mB+9s;5Qb6MCSv6!uuBOYfyHTP3Gu zLhJ3t9=#HU@B*U8P2_8@wL2mGWQ7$*$O!Xjgz);TVYnYp$VnvGM{yg z446z6`ol;Y-D#nozp0_taGJEJYT-eps!W3Ec1?sE@*2O3gzRydB2@(Fk3D`TyV{%z zd!bx~yCmn4w>E)yNc>=GLC~eCLhRuSqqmYkf%~Ol-+r}vX8Y^QB>E7S%K_mfEgOT_ zx1muY>s*Nw3nAjkG;AC?=IW*Di*CB-;(GB(Rfl3AA*FHk_K2ZhMP@zKV;YOV2Yvc8 zO7MMB)$aYE?rxxd+cY}k*wpZp))B+&ZB<|rvu%_lI~OB2tfv#f{lg7*H`-ODRyA=6 zH=RX7i$i3UnKaJ&lTHkPC6%=nm!|M=;;Y`u|Ki+wO!&MqHn&Z=1A930RHsU)SXywE z8u1#@^-nenZ{gf9_AJyhFf!tPFWIkkqAfa-NjH1qcoy_dp41GMJUz{%NzK^!r7+%6 zKcrMG!Cj6QC7i=Y$sjrkB^i`^Za!D3=FX!|uLpOg_g;*VerKmSs0AC~jZjt{twfbV z$`ZS{v=dMwrgZI7^Cz)s(Vqjn#SVqqm?cW>VnqT(mcHV>a&SSW6?>`B2M6xc#uu!8 z1)4orW-!ao_{~%%@|$@I6|BCX2SgCEth|;;X|v(uXkw79#{wIe4guY1!G=bNnLJgn z>q^LQ{YcThl)&#TSX3ACGSO`#CM*YJSN!cQz5KN0t-2lQPJBS-QdYJ5m0QP}+)fMb zf#TmF#=mc{y?5w+Ly!EB<53-LUm5{$FnSNL~sE9_S(FXyu8dZ?Syv?d&W3v z>O%)hh4OM$Dr0`^$STahcHs5la?Up@G5exHiEOih`E~fUO}FI%Kgywehp&8+ASN|4 zE03{u7iLf3?fri)-Y&R>$e1bbFky%^qHC%!_Bbey0$Zhm>HoCKcVL-$$zZFAcptw% z99?U*fA16@uaR~6;d>e%ruMO=QeA1fUQlGl%GvMdn$;ij2R6|)1>{U?k6U_Lyf7!i zIERO{1$|-s143(7U6=9k83T)^;;A%DBI8G5f!L_W61@k9C^^fYLY_2FzFeBzLZct%@2}mpGJuwu{6Kgu3Z_pGg^F#(Kd;@`JS;rQE>|xx$;i#fI(iL7(p84P_* zBZL$iAAch!G}*rd5crzG+#r}SWPD-T_t z!$t4nl>U{f**8J^eKVJOfR>rBAyg7sU?%=Y(@19o?}&bs?%?)li7~q!n;Z0;5AQ;Y z?1RV9Shl-ltyyslph=GDM)7c?Dx%nuvIU3qSdXr~@^C((bFl#~#cN%9+2&^>dS;n% zoJr~kTWkR;ZQ*>&;#->3OL2l|xqAyexms)T3$T#In^e3h!5eiT2A&Gh@Nifbu>Z^f zB<4+GFfOAW8;$H|VrcWDSg-6d_Vr`D#^!mH<`NxZk>I@arx{m8$JyE!~Vj1Kx;rYf}$ovSP^apnY6Eb&s?`xT3z zmXH$)U0Ze&8cTni%T@XG6=Jf}c2@CuN_iv69&{npybel5V|kf{t0z8|xD^P#C@5$7 zr!5VAV*7d52zT&{!8|{(1zz4RPq$}UIbH26haYOVYQxfRvKUDoU&US6QClk+^-hl71&n zxMr#Y{Zm(F{UZEJmwl8F+a|YTbq90*=O;orNTlRE@Ri-nJDY1$2RT$O*VDyheg)u- z5+jJ(ZQju2tJJMJ>!*g2#hVsUUC_C5-ovBIwb^PFN$dmz${`wysv#PgG-jlEoh5iO z>*V}P;{lVta*l-xU@Y1a`SE1ZZ&~dOo6s; zQ5(On&$u0?vHkbcP0e!+Nac16fnY1}%tOk8*Hy#yw>cg_U!g5B$m$Qq*O`3e?8jiA z;<@3M;>WP2!d-aZ3q<$~8epTc?mqB~(C`Gh{Co5k>hl#~$$8;9#h#(}q3-Sb-y2@M zK1`!0!GYdhZyvz3=vw{?wiXR5^(sq;?LqcA9%Y=azA72+D38wJ%xPhQQ85Z-ym-P_ zp|J#ikaDZt=J1Za_Fz~ThpyN_iK3mQ3C*S zh^<`k=x1?xNB5cpT*6oG>^*uUg>$;#k+0xuP^JXT+V8lVgUZu96Ulr?u`28DyC-#D zeQnF0~f|~JBH3pCx0-@1S>l( z>8|%o;qY-ecfm;~B&y;WnNda0H^5RnxPLuFT5|EXMy&%07pdj>3`%XYep3bJF)-^WY z7Vm6PCPC$b;IKKmILkNTMWFM&chs8!ubcF z+3XU_vW_E4EWRH<;qhwpnd!*k_boqqSDU2EwTmnQBC;~tuQ4uN3H-FaF9J$I9gCMk zb2=hDS_!k5$y>aWZP^$#U7GToZO(~2O62>cXTYC8m{af5t~zI*UU7)_W7uc@M>=TA z2N)9@Lj3(Z=ta7f4Y8mx#Ky;Y$+6k|(~ne0oFpR7_GC)~uZUdpF4qs+FF|{)n*bnc zL7+ga26B7W+_O3a#CxlT?}XyUh`2JUZev42W-`L766~tM1W=-_D^i2wA~wf0oeJAcZt2Cs6-&qJ2Tq~eUonyj-g(|vfit?(3Z0+o`$=mWu4_75sy!=E-)3wR69c(Gfx{;JAu!FehxERz! z5BOrHS~7muCP&!Rpg}psOI=Z)Ak(KEx38*;pAKZ+Z&&n7o^2z3`_?)zYq@m8^0A=e72vm)lsz`7YoC?P$hW z+O=0dJ<1Npu(vwKErXR>q1Kt62HF<(o1?7BleHm9Y|J1d%L>?ho!)D{QB1Qx?G)X@ zFWmkI@@0Ki62q49<+VqX*Vr5)d4;oC4gbo{IKFXzeT^a=d<_`%J_iP7k_Ntj?S8XvYT}?m9zJS}TqpAphm=dzm#Xe>m*x7xc2G+!=s1 zcsmrlxjNpro=sukG&|{7OzO#NE|lhHr7lHV6MIQ}iu3JMZII>}VGsE@Lg(Xl$s^3; z9sCHk#MW0Fby2hQu)wyRZR&0=1-TYy2=I#K?CX)bQ_VjCUajK`5mJ47e|?)T;lTaN zRS_XML6_}1Ko7KtX=JCIhR@)xz&%yfv1fLllP30K$l=GISDAP3bMyqIW+5)|2NJWd zGf(g1lHnW6fLPH_z#FUM<8yW0f&Z5)pTRm6pyUSRd+tl_|Hsr@hDH5FU7!*wCDPq6 zbeFVr3_Wy9H%KZS0@5`@r!aJPBMn1JHz?gmH{9|6zVE&F1JCeGd^!8K&)#RPwb!{l z!WgR7EHy~{nurrZVOSlz;Qf2Ekr0t#*9;q*-mLlK1^>GLF^=Cu&Y$UtYi#4E_z&wZ z2;VP0jum~u$5ziO;~CL?$ljecIPbe+A@ezuT1zK4I|WnjPDaXXo+ssoPyPJQw~L5K7RRZaUGD6nkdkN+Q3yHes7A<$ zIK(dyzyG|TpAUQg_pxXY##i+9Wk|Rk=jO~UC$+C_eXJQ?-%j5ogL@HTXH^vbPYa+g z0Zl)W<)V=)t(z~#r`YY^mHoRxuQSm;c-C8$aQLEtsmWR$<*@pKv3V!rwDRfTTa&=^ zy#dD45d-{2u*Xon9epH`Czu?utMAT4dvDumhm^}90;c~4)tycDv@`i;%!hm8yA@-g z8TpIqO&hh&HU+%7O&&sv|1Vkdg(u&WeGT3m)Hz+L@}|%z05NzRjD2R=*Dnj)6fiRT zK|nH?lKd)F1DM8?t|g;EdFUZ>Acu%Mpc1t4m`cFEp<5nS8Vk z9h(i$#Njzc1&-N85OP->;;uNVM<06RttYr(ogWd;0&4}p*Dw!E4nMfVkr27YLMZW( z%NH2tZHxOp&G+{|J%7I6|4~px#bmj{x5OP)`9kNB5c(ZiPk1;dz&cIt1x#r@MG&s~ zEJFDZ@Jwv-{ETmW+k)zV$nJl^LIzenGkN^*6Q&p;f2VtgyzYG?MI{?n*Z#~(>Szvjq21qHnMZDNqQCEfF zoN%$;I^@n~HlpTA<`g;^!4>8}RDx{S?u%@H=tQxYD8~sbaY-o2*I<3)v+tJi;W=O_ zgc6n&Y0!JWmlVc_PxZlRx465SVvYrwSS*E^iqTa%}348c&c7 zni}MH&-rG%;o@RoVuH$MQG#EM)cyr;+Vhg2Pz&WjHw6|NV-8z&lS6mC;HgSB$eW=I zg^;^U-S`Dlb=~-}j?iHA#

@_zS6g84H(n3LY6FQR`I*8PYA)EXtK;UIroR5tTtI z^?G!TdPDa-g%b5l=t>ka5RS;y9^yf4>ljHIYPp5Nh;Pt2jX@KYubmBp=h+!2D~XE- zB^4vAei{d@vj-P5l$gt;A4?P^W+C6Jsf@f)Q>2(h2rugL=2gfNs-y^IpiU3{{qXBP ze4KIF5Q;qXgZnmFXpUic_^G)dQ{fIcKpu%h_tS;s`hA(L%m-{6bITt15sfj;Vo)dN zh0)MZXiU;AxH};O_jRbrW`bGF>NW|jO2za?Y=)ew=TYQ{J&ZnLiTcEEzM8#pc9##+ zhWNO=h~bg4nT84+*f{+2pK{6{41dC5qBp%23FJqnAu!>=D_gxW(c`C`dy+&Lqx)0f zS{@W~R(NdF?qx=>_UPtMm-C0x*Z0*1k)C%Vhkxnl%ijiPw?Eq^yVy1_a7J|_?MXq6S7k#;*5V4UDkqMU@>BdN_I7wuq zyuLJgeP{#%4{PcwMtZq(k=2G#C>flQ2_QN-8nELlm4x7Tvtg>NZn`Kdd2hLM@%!X! zVqc2h32$tk3|9R3pohR6)uP8EQe3VBdHie>Ze3I~-?0jZ2!vw4?Rt$~re;s<3g0Zb zkV#bxoTL#S#D2?i0ku{}K@!bs7@c(u&Pra z8?0Y;?BVRYc^a2;9=Vtq8oKK-wz%MJEXK|ckyw?%LP_drz%MM(M2~QrQY1@QBe7Cq z!+aBgetJpfP*FTnR+3_jni3b=WkU&Zi?HVkrp(ld^F+SlpFz^K=>g$34*boK0)DCk zXZ3?-J3>z8bxMD~XWTR=_V8!$Jh(n2zy$zP-E)FB*ii_zmgc0`MRZhC3|QbXr@d-g z8cggW87;d&r@R`f-@cTj~)0z;AYgplX@Rdnu5`4#fA{UN}yfAsY4mBz;j=a0!NF+-`0vBDqwgpY{^ ziCsW3Dq0ofxD3ZElwT!4d_eE%IkwLg$^P<#+xixVX)pmVS){j zAfNJumy!}@9E_Z-bJWSWDknsdsj}XTgHo#)sCky@m~T+td($<*7QcK(ma}9c0XMY1 zK4d7~{!YwdEoO_l*0_6jhVP{0b9>l2KN~j_E8G0ss%^u&;B(JAANIZF zx)0ar+c)w-2^0yU(sj=HN~6PO2GI#x6vSXEag^?r6|w?QiN?37#^cy7#sX+n)qD9v zwMKn5hvmXW2gF}?4bAXtR(Crc*V4t=v+1Kvi{t0Q>qBvnqAFd_TaruHZS>$+KU;QC zdXA|1amdWR&Nq3{L@xD^nk8c*mdW#}T42Ao!yBsuELmX4Oo=g}0WdzZqLZyB3Sn%A6 zUi2iO6BQs)lVsu?53{UT{&;B9<)8K*(=(NtNt~Aa|0;g zrZ8-p4`TBL4zsbmi!K5%l?k1yne_Ka-TgYZ19xKCFB8u!&)3&Vm1#D~&M(d;8?#ju zoAWp17Uwlwx5K3EUyd)L<4T2J`SpfGCyW!lZS1i>ZSPMqS?^8sv$$~AqeuULD2ROul0!Rzl(pYl+d>7T;a?a zj6BScMrXF`3ad^u?Nw;E+g$@%FX;aVsld3FTDYH{o)(_)jeR}SoFD(nwYyjard&g+ z-J{u9X(N{ztH_wvx929Bp+a@w7cl!^wX#<|GNs`kJ9`d4h1lU@p)oBJHDY$=N!E|B zPSgyzDcpwdAGAMzH5^#bp!G~Km)#M92iIVw%ZkU!wl%i)ADlHl8FYiYd+yUuB3G~? zy~nvWGBNI9)sE_k=Ouk zBP-Dt)i0%KC%iXQPLx)2D!q12zcAjz)w_UsxKzPvC85=ruJ}Ho^3ZJAQ|DMV%U=s6 z=&+T%d-BQ;)3{A`e+Ya0WTCr-T)1`4h3vaPY+7n+?xF*zLc7M1bkHR{uXwX?hRMK4 z=Lu&5lWMrLk+p2IXLR=ht}QUvBk;L70mPaor=*n3qbrToCnyraefW*9S`O65nI?a! z9taU6?t`Ggq{Z_RnT5!&ukT9w9hRE9#WXnq6owj~v3e;C@8nWs8S z2K+vAE5O-3TwHUpq+o0eFi~+ zOh$V0NpPeMmfq8V1(R1y+cCyPBu0ng=L>r{OEX+sxPs|IU7?<`UrTA|mTK%Qqa3Oe z%yibhn)KR*zS$3k91C4c?+UuvqH=l1tBQ&R0stVq)CJ=|9pU-A?%L-GZ-c-wVL5dh?v27=+_4M>DV59)8Ta60dU& zZ2H`P-JUTuZg^k)B`=!AK+~Q84ku1ENsx~?bX&AAU1WZQydEIB(^aW{e-v)jAmP94G;on7J1gn4`(nZO zLso6cE3l%-f5@P!Q|jv{mI9k>H@T0{wWgF^de;cfP3}f#mbIJMi|gRtWI~3a{5g(O zU&xt;^V#)#|L~VR%rLazT=fDYuMi`tlQ#-1?PBnbkd-(=IKn`Eso#wy9u^HEBczr6+_dZCk;2V83ha=)GMefdcwl295FkmI_- zO8OXk^GoT{ABOkSp^_Q5g{Dkmz9dC*v7Y!+nGo0MfJGDW?7C6-2Tuswf# zwK0iD9c9)js9*_(1ANtpYns@{CupEUvvd7SLdiB`Gq2gks?RC>e!}_CkLsSmaQBvH z1M2T=fF+~Ayu|2X2TT)_x^K}*DXMV;tVos&s(j#hUxu<6<*?i0qDTTiQf2)};Vdco zSZ?#dqF`#GK#@?;_qO4|Pd?1!S1)ym@K^H<>0f(Vif{vs-3cW53*L2Kzl8H2DPQSQ z_HU88EJva^IL~LR&ObPhpFQpx|NinUmk|+gVfnmc^33y0_hy9~_aWQTN1du5_v`PE zzkj}b?|oeU_)PyY|8lPr61e)Ir;_1Dtx?9j>70LvH+o z374GRE@!ZfHPFEvan%(QYX_buw0V820CwgM)sfRi(UEI+sU z*xdK%Afx@|nN*2-C{6_UGN&GvbL(P!9((?rmQN;zi-g&qAc}FaGVCFn{~*mlnefZa zY*zUfzFc5D?6Vo&A)yCF2%LpmJoJc-_(o~IoYdR?a)M{@u_os6mglh#{q@?@Z!%{} zu?Xqk$pc=#4uqS636&WU`-hDkX4ylI+byaeM;=c7yB?}?X#4VOw_W^?_Mb3>_iM+0 z8+)dVUr5}d#Kn4N?dSbP$+?aGy)gM+&}+*Bv1|CM?ij{)M7FXJ42@V7mfzKV-G4ms z_wvs9d`@_)?)?6pId|7PkqM+;ml&ffB#ZV-wkX)by_XdfY8yc(tdh#nm1*%lM6 zC5W}$h#DMdRv_tPiz@P-2@p3vT77eFxkgEBdh%PMhZ8z!Mk&=9)lFlE;;w0m*1)DWd@pSU#2_1F*QMNI0M zB~ZJDeAWF65#L_~@}ggUT9Hm2iItlD=H#FlVdC@Mw6maa)(=oAagctA`^9QZb?y6s zmmP7KWZXI|JmD3(dPKifOc0`c0``I$VgUirT6U&;E9x_~fr~hOusJ?s5u&-Ze?)9< z6Tn4&ptjB5LK||lod|((-6hz47gLRBZcHkyYhay??EzJhv)I=4rKJ`Siv!*Yzk%oq zt81SqehdzHpkQtI9qM@S6eYa(I9i7@sh!w4*Fbk=duX6(P@JCQ>1yb-BUq2~VYXchz9_ICXTikw=MZ-?El`^Y6w`E^e7{yb0a>uY{kHmaMtIgnlHX=F2;*enY;!xy_bSFLK< zo@P+}{2Nwe+hhs4)QvXNBvjFSCAELJzxwk}s!l29Y9t8Y9B zy(@ILlb~jGy%QOX>l|5oimPZ*#%&7o35qc4wc2v(JI?w&`0**NtuyWlRH9j|c`rNx z$Gf{f`xH;kCTYpQYEN&<83bYX$O_9YpB)TZ$jQIws{NM6!v=ig$Y(r22~4&$)g!2Z zNSlURE)NOp>`MkE7-CLbd^Sp9j113{U(G!4}v8A z`D02p_yhi+I5oj*!)@op-rkL2a`0&Zdx{XshYU-+R(1o23z(XLVM9a`Za|UJJGyk zC&<-$0OVLkUdXP+^dtEwf2U~K*iJ$bve0So9H(lvS?7usk4^0y-8lg7?mlW}?ffIf zn)dk^uO#|szONAZp0-u3%-!1y&;f>sEI@5VU;;&9vc$=Qj<9oLj1GCwEvk5_uRY`7 zw?0X#Hm5N&&3CmKpXIIOW~_QNhoVm<<2FXEf_&p)0@f6f7afZ3aUE)0W!oOqns5V0 zkQR^PL1x9Wr4?MSNyzn|UbCWk_2?4VTPl1t8m{*3&vi?JhHHa&;-QBe+8$J8Z2Mv~ zj9P+>7XEx|yOX_T!Db7^T9Ew2z@)n=FyO>%gLLdknoA2jAc4dwt7>Ftm=~($-d;|# zva(h;H;3^*nm2N%)zpZV3l^{lS`n%z=1NPMc1rN=b8haG z=Lg&b?r5#yIk>dBL0s@9+2F6Rs?X>Ir1`+dP;dL9RF0yKEe@6!r_{$Qj14FVDKQP# zAD-1gS|e4CV|$RaSQ{Yz0svE~dJYCgCuMLcDy5%jIWzcJi%g|Zq=1_#%}bUfg$c+N z$$xsvHcI{dC!dg?(0&L9IIxxBRf3p*G~;)DGLA@2(9Ap0#c1F+tSEDQI&3m+MllBv zF?Gd0JDtrcg9hwe=4nLv4HyYa!EjMv%!t0<@lrWyjDx;_M$W+wtnl;f>AU~v(Y?cx z&Q7uLN_SdmFH};!%vMmt1@hsH7jDIZQ-7vuC0ht~AV_ zfUVD{8v1v)NEC)ECc8$s`SZC!XVhazfdXhg@vsIwBZ9VidDMDKbXMbKQ!VZ;4Q z&nF*6Vtljh8is%WrUC)e$E=FRp;*CiSg=9=7{mM3(6;LG}T zR*EQWp|N6|BhtnvUrA>mt(CXe0+rcI>wV3H2D@w!&V(zm4m3r9=Ri(;qw??qB0h(f zG&G9_d9@>F+c7jK`)3;0&#;Ae=sSaCt>F(+HYH=*6rvH96bsfMT~wPobGuG!mOmF; z;J*pz+M@UybmCeqR7^$FLU|U%^scr>K+%AL-{B#;;^Uy$-tw+}{b6<8%NhPm=@oRa zP-3IzF?K%?xjxi2W+_&GK4zvD=$|PAYN30JuJC~PPA(!N6gqCbs6k_~sQ7yLN1ok9 zi%@3Tx6>RQvm(lFaWRjI0TB6VCpBNy@6zoas1bRVHb1tCeZx|i%JPyy2=97#6R_&u z`aS4-%RgDHYAqjt5^`9MO+%^FimWhr^{*zYFEDGJ z+nlKv=}x=Q7N`s+&jf+0gbo!xnffD4jI4g9Otzf1k@&a7%2zixN>7Jg#Cn zgD#mWPqOnKFaVQ*=!Ax?vR{sN+3Ud5hb*6q$Foh0MSj0y{`s)5V(qd~Obo0+7i&Q^ zrxCjbU_d~?t}dT`z2Sc1Ot8iE*M`7@R;Qvjg^tT2i9YGz91&Q_4FNH;7bJEJbWU|D zzNRIqJFX5tH0!^2nA+F-h7xN`YVPAjAd#L|p8&P-Gb1x5Io%u%RqupPbiNAhaVS!* zGvrY6&MZq=Fwl1D&}II4V8_mnyOSA4{8gX7$`WlD^fbDEv2MhI5 ze7lSYI3Nk#B0ool5jhwA8@J+IPwiy)gEareD=a^bcE@E;aKdkdg0iqJ*n3sA zGPa9H^c*@ADfi7Cwvd5hZ?Ha4^xw=CS9RY~2VPon+gz)5sM*_z*;(oO8O`hQvg zM!jo4F`kbB8o^Kk++M~!c4SJp_heXe?6*U&l3yt!2^Z}uK+WUgqbT!hY3%u(9mcH- zX$7`HMvMf|)cm}hSWeNZ6Kz4?#I^wTpj{5UbUTbw5o>t9SI{MvDYNtpEXzpcbPrB zFM@C6Dm$5dKT-v5sVb6)6m7I0P}+q+r89XISZJn{2Knm2L<2{iN>NRWc*#C!i7b=s z^AiMx#6xBCgv`Iv13MFOwu4bRAIlas}7Dt6T|{Q1Rn#a6Ui zcGE!(~sMrg4n zN?zV~)pwXVJeWW3izy8qk;r$3fmE$xd~|;JyvYMX_U`#@ z&^HXsl6#!R@jizl_>@y!c{Hty(OWty0Q!ZS@8=^%38M3jBI8z-8Thp#i5I)$X2Jxx zxCTtuNlLaFz2s1EpzG|N3)!>-Z)#en5?=q&Ld!^Bg(=ei%;gBX_%Jl%25%kM>M9Qw zbyrX7DnfkgwKsVhu4cQY`6$>%dTJfn2f3ytf48zr!snm68XfN+9#DNL*Kbhs!xu9w7AAv;W$ z-Lr_{WM9qgUTQ)+0gAVJt zH!QFU@+`25XslQ?CEx2*#|y+uN3`%L9$&DroaN*T*7F9xN?}v7tc- zuL!`rl`OAeM{aQ1QJ)P8>04?bmdO%I^wxKT4lJgO;|y{sDpG7t105f0>fTw9UBGS6 zSvRE@AiDaCW%O|&Y5ihWgi-eOlCNgY+N~HWj=c9N)2$D4CkVv7f$^h)pDjL;1<+t) zs|FPp1plazfju!6Sq$7U2Cmg=xeY`3=2e88-C?e$tx!6*vJyAeGKppRo7lkR2*)qRh0nV80H}8%$H&XCQv(@&mUs#QfUksz=xG&%bK3!It z0t-{qAUNufG*ge!6W__W5!x;B=Iz_6#m4zc7iUikM^CgCvm8_BWvWRT2#inCn!{Qz$o#= zDRdUdW24vgjCwF830KalsRb1_mPs{Xng$=-MDN5jTm9l04_XiJ*=_!#y@{FKQK(~# zg?}T*+_&l`?F=9oLpv@FcvWx}UzvHA+$0h#svhTHTD@{G>-vp^ZLqQLM@QZ&LF> z49HZ1+O65Jq(Gh_dJ&S>BN$yC3woeJ3=)P~W`)S*(UDEBKjn_Ie&R@eMGBB_OavI_+4$|dBRpbLu3sQz z^mmyroVT!kVh%%saf6r>)RG~jlj8#9=BV2E()f%FX1Re8Eth;OjYtY?L(R(oCXeSi zsCFS+?q0!0wqwCE1eC=Wfntl`QcE3yL;M;`FR(0mLRcYdShv8&j zJaDiW*X1!IPZx2VF{jP$Upl~iIHf$ zs$R>oq1b)N_v9{ZRP0llX&pMz2eZg1$lJ4C@6Oh$Ya`>Y=0R~Brp>8JIG9-ZI*3cX zQz(tUsh%pyNX~b?Sx66!DK~;2adU2=@ELA+>0-~Ts`{dz zI*N(a`oGFUpSN}kcyO>13?J=2E8uuS_<2V9)5Qo)hxY&77b*1NP=m?+R0<;=+R@i7 z?2rcU;{&9(*?ZLD>EkNj{+ODwspISk0DNYXW`eT`4u%VG{NOTC_T>g)BJJjpmeit>@ih8Im$PI63t2G+h}BJYw6C!dLk6>Ptyn)4H>BV}Fo5Z4h)s7)6eUoPn zFw4`7l^MnYc(B#=D;YmdC_pJ3*YGsp7 z@6oCv9gMG=afKsnK{#KnK*!5(&<&}2Sq`}xrJ zU#7ulyBAwMu9zKntcE{vTFAhR$$8~~fIxP2P~Mc}a^qO^E*G!BLdg|T7SVZNOy`?PVa-7?u|{L_LnQnuvhNz=`__ z1J6^uA-fe@hPas7g~LD*q}`@bJ4^&k5@N3Nc7udx=%MNt+2a?tM*3lDwnJsjA(1(H zT%alj8GY3$51b&}?b3SO$+JfqUk4Dm2AxKbdQ>6_o65n{f@p|*hYBVi+gzUQ026T^ z7X1kRN`07Bw?Zbg(p9mM7Ldu0*bo<*tUpdry^y{$oMy`m_JI-3z9eH)Q;OA+Zw;{h zo9F+Mkj*0D{!BMact68kGYIf*qI1sew_U+%9~{Kmc42R$Cq$g`G>uC?WS7DcA4~ zb!(q|3?#7@OB45lDHd|6X947E_V-NzcwTV;8XZ}h3Lw0W8Im+%?C^*hCbCR7S*@lS z1RkJ$DUyc>M)h?wNXtR+Xa-6MZB*Mb(+w?5@+wh(^vRae^<>IVW$06}ni1m+GM|-T zAx3ES&;_$a=zjsYVI<5iBP@c+eyOY*vv_5Ck6LYr$8>5lcgb)q_^nFjQI>c|A_9N| z__w#Pt#+_wJ*j#dnMaJAL6M#ER8?uCcAoJh(nOFqkMc;mQQ?Ib9lHYhc5O7&I~j(s z<@oi1+1IHFhu^%U)X9#E8`w8u++)EydmRjAY7i(19V*W)AqUqey`IS&u2vLYJXMou zQK2na0pomiHO!j$69t5j4YPbNW`j&mHJI)k*dCodi9DU{zM_>BE4P?#CTaRFpGfDx zlFKLwVXV93etjpaZOZTdRkQyR$du1;ep@HtN=M)vKA?2RV$!ny-JFw%U*H1^kUvBD zC;^)}39UjqO1n)N4$d?lS6;z!{bSDzPK>s~6z-}}EZs)s?nc|D<>pF7yb7%K5HRP2;K)W&<%$N?B4u1fc=yXK@DUa+!hj%YBjyIK=3A0+Nu|h-I`&Dm05` z-3paFa%*^$%HF=b`w}QFtJbx9UqD5OeSoB}hToEcDXkA7uPOssi=1SL%k*P&rG-6C zU?SWNOyF%r3e9EU?V%g)t|Lz83kaZ<0idF21Pi{?<# zHAy`CMnN?op8tHruDB9-@5<)s-sYMZB+Y>URSpe@!$fxfsYK$d!Xja3%(28 zb6wd!_sZmiPxbY19+jruzSQ09bSIMCWUVvsl$B0=p>&)0g!$?lao{V21gb71Jc$In z_vBE<1W36#o9(=QsDu3n2f_B(&mk2kUbNchm5zFi+ezg~(xA6Yz&vR+I_f|=UOL9e zW-cnuw>!Q(7nS~gS~^;VFGIrz2Opd{kzB*O6LP|3f1Xs*vXo6;82iBw;Vl+<@rmLk zG~By=P)5B{@8kp<MIsB_PbD>B=jd3pH^LW1}*3PTL1D%J@RX0=-$BG$0RUJwf(VYc8VAjEnL7^p3Pvr>(u?S&Y<)fslqeIn;uUcA*3)ePEJhN!*L4`F#$W$|5sc0p z3xQ(j>ChqM8Q>(El%j-??mZ^fEsMhMJv^FHg8LzsdMX1g)cA-4EZVo^}dmwVx;PExT@NA`VpOdGZZ?w3I85N+@QvW>rZhX_-=k1sDl|zXl}HD_E-{a>$7YO5;g|k$ zZS`HP3cHXXT6T2#cS4O8ad+c7(!d)w~IrPZrhRVz+nxh&wdczj-oilqn66vLdTW#yLs za8iKLum26je=id#a5u$Ht5_qp+n?MPiDcBd=)ohxDe>-43_*W%T@zC2P_tCtG68nS zbDCVa0{mzjD}7iEai+x>V^!r98cNY{@yln6Gt+j`&b|9v&FW2Q5vl3&>_*yv#JURl zmn);VMG9R6pTtHut&IAr`f3hoJFf~!Sf*1rQ|z%a!fB7`@n2UgpmBrSs>Lz4X%=uQ zntXeQ1(0kU`HNOjT5}mf~nDz3C~$H+nBu5GG1}7$kZ^weFvqqFsUPDGUIhU5>&G=VEKPjvpV>>E(o!3riarHc&MVa9dP3DGX~S4%<~XD-{;FxbPw? z+wE(_db1OL)S~tXgw6_>3}QI5hw6DB|Yj%#i5$e@~@?f$@aw%aM#o1NbNN7lL> zN|R$y#s{f7#EW;j0)Gmi)_5u7Ohx}K@I{u3Aa+Z3Z7Onm>ZM^KT(pqgIcNU&{}#WJ z$GA6#G1~ZHxkk|eAuw;`z7BZ3f-j%n7o;Jj`V*W6m!z6-c%5WSXCQlui zo;N_$Dzo(KIz>|=aGd*}E>ycrlQ}7&wtV}{89%tUU{{HqDr)-(z44TmDcqDW4QL%p zBrx6HwUc->dc`!L2P=e)H0@NO1JW2DBo1coxg4PE*J4Z z0Cql8NIVRq3pfI{2II$kV>QYC@m0}zOIGpH z{7!~rcFX5v)OFPcU5`sf92zUlf*6Q z>p6@;S5J>(f4jN1Rx*(eh1#Y;zrM+7TQk2qttKkpOU4z81&{#{EQ+B<>CJ=M1Oq^XtqsU zphhh!D1H_*iWF&SRh-c?w>T3P<@J9Sb=uz+lv*!|VQ$IK2%;qVAO=i`KZvMXqi!5zNdyXVauu7K%juf;)n==vhw~qmHrdL4&a>QJ>|i3(eQm*x)qwAuR%59< zQZhu965?W~+Wobk9-wnCB_TKzwxNU)Xs_0SJp*N!c!I1ZHS~>CVKLb`$4bVB`b)Y=EFG z=9y@^K5C~cglRsXQMrh0$xySLW#6@kJ5g&p*cmgCwaYgF12g#QPr<{%9;7H08>ech z>3U`NN9@bfZq4AI|F`Gbwfvrl2pibmqdM&_PjFI6bl1mQ z7dWB=SF?X|b;U8qxbh~i;6um7HB8O6(kSa64Mmq>(w1XW6>5e1h_u8QdQo0aNA6ric$J(CbIE^%|k`i zlgK{SG@81LUe2RfJs}CKmuy-uK^nsq)=Zg?D1&AA%nfUSMqvJKEg!pfF;KaTaQp0L zp_mO!80l)a&gbt9F=J^wuOME0@V1%$mW8bEml26^0W~XJtgcMTK+`eSYVMsdiPBgW zo+|jT+NtCHd4gTnX4%D&xbCL%eW8JFO;9SYwh~F0E4SOWQGa!2*_^gYu(EP6etbk@ zllYRyaLJIS0fK@0bt8dPx^sT|4b-7u`*YZEAy1LLww?BaV`~=wmFz|B?%2grpV94F zr{7z6tNa)X#DZ^X6J-%mBeh~@iAPP3UPt{~_9-LJg&kz}{>MMiqd#PzEZkUeBm^OJy`jCPyRmzI%kvcnKStD zbH;>JK|&!|4qRb`tt(YA?6C;ZPr(Ywxee^GwE7w5>hc2v1l=fQ)Pt|2_jU0V(y@vI zc`o=UT1}I2pNRZ`w6KvHwDYM-Du_6<-jHSm-_!z5V{56xG{pa>Di*R7R|`A!eserO zG0_5Xla~>S&(dFAvx4QU}l&eXt1>#Wrpc#Q+&Ks2VT`z!@6=W zCo&Smvz1tdfqIDvZ*W4`p}O|4rv?^G?UOdr9>you+7W)q)ROMS z?K)f}v9kZu0?d$#0rtI2!$9X&0oW^>XYKTNP7hMXZ0)JOj&6F?cg~n>XL27O_%WTb zf6xEC#Q1r4OYDEObD+r8sKZD+66Y7hL9yP~aoQ2Xv|cd%Kk?s<#Fo{l;%jz!>D~yg zo6FbH3dNeS^Kfe4r&1L?BeJ)5jQ;!6-Ag3fJ`NPw(7@Nj$$C%W*=9t~z)+&eoGId! z2!kp7Qwm7U&CR8zriwF#f=aP0?F}07T-_$pc%T@#;VmqP;%E}dH{V$q2tu(!fLuj* z42D_C=JM>+w5x|bdQ8;1DjHUK;WI>~v&Xm=f|F?j zd&uUzdWDnpNanO|`pYOrg@C6}uj~V+Yh|NqQu&5=8aq5LzNO*zRiPHiY+xOi@o?13rCObk;hT6|d|70R!)V;Cw{qvR+(PvVffTRU%Y?Glc( zqP?%lyZ?u)zYL2rT$Y95AR%~gcXto&&fxCu9^5s!yL)gC?k<6Wpuydp;1KM6vew?; zIp>=n@B`+Wxt?3Py1KeLYxmFMRNpT+dMw4(0<3KQ~IYai5fm63z2>@f|y#yCXjT z9enq*wPDkEe*|1LgAS2@mkT%YkQR=`$qEHxbfpJ?~jcgmuq#TXSX07-NL`EJs~XBqxkLV&+0fVQQdu znhA?p1&RFNi`H6Kk>+@JCbypXyV}$e^svbv=wq%!;dhETbddu%uC#NX=pKcBbV;*}63r=ujZ1wb9YxFufdveQxN_sRlc!}URB z4{mza7PMjt#z}LkfquF&rL_8(Q7-CZbXb|zYk+Xx5exfEk6X6!ST!k+eo+4plVQPw zCx4zM{%Y%`(epiFlTH;gJ!(4U;MJRC$7swYKPT+YGOl@2uLCW)R!s6 z{@(WE;d8sMw^L7s??&KP6qS1ektX#%dU z>KcyA-ss&q!>JTP}~Q`sF>}Y#!_?xxg{}ne*WX<=*78D}tD>0)bj_mn)xsf8L zrZ8CIQf)dU{+iFCiGt5m$4dU#-e$MgO|e?y<_5q9D)1;fj#UFgQ-$;pOPZ!+-@rA4 z_;kL`F1XW@TTmVR@I`MUYVN~#`vV1(BDWY7%#QU42WAsHy6;q<%DZrPq}x}fjmT69 zRMV(sllP&`)mT5Lf{r({;pG?wk#W{e|3fN+P61ooh7Lz=!D=?@U zPPE7VK830gaAoNQeo^1p$3$(|uOZ&Wt{tugQony2s+LSUjkMn6x}q#^r|Pp`D0gpP zektvP-6mq!x0TM8} zX?tl2XEbiPr;VMRoqV<`=RlW;*wWfA#n6rNZ6I0INxC7u^-*DFag zqL!8vR+uwCV$?JO8>AA|qoQ_`R39t(?dEW5)$KZ|X5^flm^>y8e%%gH>e{)1N|ANs zN5Nw9tq-X+6>cOYd=*+OYxPfU;jbJ`n;`}U*_4sQ2 z^6=2p$6*<@A|Tj7#Bcpadb0{a$C*VTRVa|T+7R^5N!5AF{`pTh5LCpOX3pkll^ATE z+8cer%R}gg3NtZ^TC}w1HMrWwng})JG1s#tsU+B`k@8r)tjlvKCtcLvTb$Vv?hRy= zs1#-4ANN|Rt>x^EKe_ua1ZeB9U?iKY-|V+D7`T*`P*BM#>Qhjezb(N@C&qtC@G8-_ zPmZNa&r9v268;2ih+mWxPv)idDGY>W?zJ{S=ZxCw8VUJb2fIgrXo^1RAEh#dYh68lQO6jjhQpVMITAQiIi?_>+OO#Z_ia1UBO_~{-G=Tve!J$n8>*=IpERJ3oQ_)_5FZnn z#|^F@-!=ZGcC!t(Q`~VM3#eDN0xdGYk&e(MUqt9NWL9{->Fw|{8|9IG_0^@Ad`2=dX0klG4mdtMXi-m`04DbTD^Ul?E>`rxP1#b_AFL(QJ9Zuz!E!)N2 zhGVqT5a8o~*R4u)*o^Dg$LeuAK&*3Y&d@Tj==e=1ZpndTltNeOlm@PSv7FS z%JVyBk_|gR)<6c4zMLQhQnf$V>yITcC%^O6Q?W>r7mG*1(YmL_NrAnl;?$$E||^yE~M&&K21H(`ZCi1uyi zYOC|*vYzk|rkGa4?Gckc7IB((EAy1{in&7I`!MF!6R#^k&!ETc-~~Ql6)u+I@~y*8o3|H^HoiKdC}{?>+^S8?9aW`Af>i zcbRp6Jo29e(k6utM-91@2}bfBUNJ7$uppvVTw%Y>bZ6hsz3gB5MU`c@Uj6zE2#9Vu zHeY66UuKAJAKUW-s8`9R&(R5ZvcC>?2IMSG(!fM3UiBUJ{-hov=zZz;n`d$|BP&M% z{Y2)h>xZ@~D)Nq`>G1+pL_`&7l>O8&g zjp93bz|9LoYwwL%mnA0|&ofKF>+m7>`i#N<5QptlW-DV7gM1rT$RnepPear~aVbf| z!w9fj+|SEe zv(>OsM5V#k@D^w=V*qQxM36NtP{;(chKVE<#ENhy(q4CMb)0wl*>Q%U_npu+>7bF4 zfH}a<1oFw>w}bQ~6^!K~e%gi~kQ)z`qoN47lFx{7C9U~o^sk4rN>pfm_{Iv6fk-N4 zUiFF`{j5D}^E)Fa%Klgb(PFkQI5iPeL6(XaM*2mYJdM?f09^t#_6EtFzwO&>YjL!T zA=Zp?Q{=&|MbZWQp`yTehBbRyP2zApXW-L_(uVq%kW9t^9XLDMSqA3NlMo`UvGR|_ z1GTNQ;rNGk!PlSeb(A0Cw;z%TyYsmmT2-l|muV-DV60^xma2X1JaRVc(34AjUCecd zS(cB6iqRv7+0bSuh|-Nbt7~0rCgPdVNoKq3s_Vhw&TaWo!4!d#u*shMn}qvnzk;AX z8M4sT(dDkZ_1nXxR?8S{2(fnqAv~aW#N?L{qI-;e(CAUJy5s$^QT>Ky;@An zzIc})>rIE4F4@j!kZ%x=(MdXBOAdq{}CGKY~EMDW8aV zN0%O65sAH@k)J!?^6cj|-aG_f`v!CP{wUZHDXZjWS^D?fF7`zvOlZ9*x>l4xYey{I)R&c7VZ3*7+JIhV0x1=`3&f9;;_sntm=6B|t&Bc4^ zgF0uxKc;Hf9#7se!uaWclL4E`Mt9jzEq;AuXa$|$<=us$w3R(EKgh@#d+~Bz9{Owp za`f%h@M>@7%SBc$ga4(YLvNm+&q*JVyPD$l8FkO(c8Sk&i}fyUK%u>bCGypla(~Zf z{}f{W>H2^V912R$I~B292mh@v-JlsIQxBqfzZziEV!kd>Y1^dY<+|f1bB(bLVEFa? zBYkxMH77hSCUk#9jGo~LX2Is=oE3*v>*_z0&c_GdD^{mlZFL;Q8N9va_|R=or=d>U;9f|MBt!w8K>WCc{Ovds5NEmr}K}?M{KVsvh{>4cCD_*F?XY6Gz#dS2%|Y zRl2~~Em+0n`|D1UV+2Lel^nKzt2O{{eEOax*ZR_amUX19?t5}6ypj3?WHNC_k->rG=Kf25o1 zY6Y8Mc{!##JGO@G6<6yun_Yw!?^+l!TCc|5qjP{P(CA77$bxYJO24AlRB$v}(uYm0 zM4T-(uJ7r1HbGl|t^;qzB;=ypB0r(gUeWALw;|QtXqqCeZhV_-+UbS<7A3o7qI#K7$l%h(r!gR7CPGY zeo3@bksMt;oMe+Cy260hRwt!amRU8Js3Ese_C_`kIT_=Wqw%gH+cnMkC3lY6sxdO# z;|o)=-Q9n=3a{gzY(D*3_>-1G&)P(uTM)eK$agwWqy^0jJ5xe;v>|G1UWfRol1b)& zukr5zNOQ@mAlgn8^4kU|hpI&MIWx;p)9!MY8r01J>Qn;*3w!%m?{0+>l|q#YfQK9} zv9W)~1$tFgE7X7uEhi^u6tmrNTKj$34%qX6h^5#Xlth6D1C=BRg%@i=V9$P*Y!yRl z*F3F*-lAisqgtaTt;@(O{6(Ip3PYKRKbAG8YH?R++d%-RmP(oz`W+u_munKBJ0Vo%jGr6Ad|i&~gO|Sc3OOpohwbGRdnY#}U+pmo{oZsXP{YoW zM8Vu^(KIA-%@j560a0Xbp-med|@Mfl^;w~Q;RoMz8h4*BKVTdDyT0$OEV zEC9%LJcO`@RaDRpu$U=-85^lHZEhVHdPIh?rN)gH9;Mc{S_O&^MfFMP?rv;Tsa!}d z%3c;o$P=ka5t*3oyv&x~gNQE=j^8$gSSKIsvk&gB6Ui90i%D*X^>$_ihCA}P(2@;# z;XTd+_wxu_3-yc^H`0fT86p*f{0KlOcrYWW7%c*%==R{%fj^J^H7nGjT)!tL^hFfJ>}HU_ zT5)6zIrVC3*vjLC!IEZVW&-x+e_sV}Zf^e3Yz$xkSTKOO{r++Q(`te1Fo`16t}E$k zWRgldZKmcoP_JZwod!tSRR~XU*d&U=)$@j~E;)kHHRnMj?9^h$hj9$iuLs=K)fq*j zCOwuP2Bxj)%j%PME*+dC0**SLYL%Y%P9raQMV;4Kp-Dw80-+O0LxpX>v1Tzq4c80E zG@B%dnuLhR!4@Y#8!JT^nIfi^i(cx*e|7oUAO0DwMq)mzSyY;LjmJ%8cCwfLrlX-@ zU}Xgl9i2UdJzHi))=Y;C^5=+$%5h$CZY|k!)w~!>VQx6X4|BcHpXkA}RAhDeKXw$L zf{5T{DX+z*M(^fyiMD6h|MXUG{OmcvBVO_HE{?G!rM$_nEAHK!JKR3-qL=%m?cZ~) z(qQzlu)mZ=o=GcLjUgITKS~a`NjEJSmP@9yw{s(<%7?d@(37S=YuMpcPfWagL7q~# z@T(9G=wAgW54DuJ1V9Za7EaVLLhk*vJ-kOBPmh(9FHrOJc=f_13>}stj3d}oF&cg_ z3w9L#e1uF=4s(z1G>PxIYVYQMxlJCz^@7u?AR~;fhKa~Kgclr{JMk%;`)PtFX?^wF zgHk>{IecpHk#J%W+6)zfw)-)ZNnB#$A}g$pfAHpe^S4t<@zdFt#B! zePxXzyT!r$}szd*_ z!Spj|d?t3E)3akU`l>9maD6|1Dod-at7~dw69s_hy*;z5ymEyJQmR89kDXPVLYc}= z9B58l>yD}9j|r}Ew+;R=ZKT)?otw z&D{|yGvv`rOz66t58Qlr4}-^ex;(_$8eC=fw*zCQ{bYmkhJ#k#>iImD*10jc{>@Zw ztvTNN&8@2dO?1}0AZW$mqTM!b^YuFa`mmXJ3yF`eTJQ!_@IA+9>Ur~L=?61YgjKw( z0PigkBIU)oqdF8vFTzh!Ir%2{_pOcZr};o`^we7Xur}oCo#p7iUB#f-ok-l#*J~E=r&e=c8-9=%8WYQZ<4C^r zCSUyWDaY@{S|c!b(bep~^qj#QZ2$NyTdW1TDdMHqR3~Z#*%y~*`?^u1sOXJoY$iSH zlo4RV_LLP{>!fIWq%Rv_p@=F%712rycHpU3QGu2DZVYa%t*s>sDVV`&!Nx^|2|TC8 z4QuJLPBRZJuU6O$KS-Vd7sku3cjrBFv%CBIR=^{!7S#M zbg&P~J)H4NLzt-bX;c*@Bz*XtC6ES`7)w-)`3Vn47GhnocteaM5;fS;r*v71+y)jW zM~8->|4E7g?u4^TY5PF`cNtH6ha7qEq&!rnNbETo`gF}@(TadVDT$nh^-s(Zx@kp{ z`K3b_+_IY#qe^=r3C1vMnLvEY?cQ90Nl|=37aKl+ZXwR_wrGm6WqO6ij~)}FHZJ+( zxCFDwYb)E5_Ih11WcG!?K`h%iSOt!6!`F>&oQf&gcXXxsVPe>ZGLRcwVrDhE1L}wI za&~30staiJxFe(bSZ@3IzD-*` zK7I=`GNEqmc|UA>Xn}k7achh9@jr4%6MX*ic7py{jAM}TvNsQEG4{fMjfqU_a0wI8}s*1^Plm!-e!H;9|kWe-d4jO&xnQW z0!TI0M_MnwyEDp0hUm5(QU?&c-TU`EEfODswNsB7-6(e8c~yX-)jzV>*Rgy7pa1QV zeC_U_HHpFtB$csKS>CB-6C?7GRFVH4_U>S*8NVL*&%G3h9Q0%bn)~NLgGlq`Rs$O( z7*=B(rMkuAhiB~Wi}K$#p&veGWM!Ec8$*<&Fk0qrra$10vVs8F7fp#0D7Kh*F?#M= zD-=+=fB-pzwN5dKVjllki~TCl+8=5JZa^f3&Q(Zo93-qt%@rxIJD&XdYMA$pKEu3u zyK#^x?jC^wM9bM)Tjo_AWi?tYfoIifR);W&@@(NfZp`w5H90B3&R#McJ@jA8Dxa zkb0`X4P`3tQKs?qj_jnRzh!98q1XJ36*oiMrL0GJ++&7>?LmL;7q9L|zTfkPj-e+H zFey51gqd73=Z6KnT4heW=g0?3t3yTB0N^>*Z!Py-$MZq`NT!-|@RmoF6f;MZRO@s~i zsnuTURUib`Ap`Yk1Ox;?wTBlAV~nE$erBVR5$)Zijv^~gk#PKZ_n?5GQKeo)Q?z)h zT%*Co@YyDYG^&V<_GyBBsy^V|9{^|~C>Uds)bPneoV2tEsvdwuPhxYd-$*H9ltUgs zvX(43)93DyyBYqyQ`gtdtRsLi$i&2ibfyX^bO9!X_y1}E&Rcf=Y}2XG4X}vY<{~Vz zTuLo}U!6R<1Lk(UWN_9bdy`f_k5{wix)r$xE+7CMA`4+8I4pua14|j}7TpHl=Z=Ao zk#W=)=i*2$A6R=qb8)#xU|e@tq>{_8VyCyL_-!-fBv+y`mR#jdRFZQ044BX8DF0fB z9KQAqGnU2)bM4f>KU*HmWVZwqiNkH{Uhb#7#%sV=FNz@1Hdkq>fN9~k&Pcb5y(UTu z3WK2t)Rx3_+2Cb+Sj#CyhyfR$pe{_Em`=%>B7vqzqC@Q$>RYvseUzidM z9ap!sDzIq(@*YI{#*UHMY(yjX3}@CJ;z~A)b41mi!;xUdNo$SRg#NSX95-|SwEDg< zf93hUucu`;JeiT3%ZjbtZFA(?jc3m%#OKZ$@5p}=OpO0i1vL=&k9W%djm%8d>q6wa zb=g~W$DEzei(6P06zR%Lek`_dqYFhPtD$!57rG%>6avknHf12H!XztJ^!Tbi%EC1 zgWlc9UzMj;orOjlT+aYckrFEoy!5kV!19w6U7uF71Y>S)?tg`zw}k-Or;v$Cs#Or} zNUGmYF$AmvMn6bI)y*9n#n5__-#_~S-TRq&c|<&6l7sc;;6QduGLp7(%aR4$=B%NN z41>C?XzAoK0{N>pJ??o2F=O&Thl_!oJC-wc9$YrT4QObU;Ds;-aDock)0!Ji$ba9f##TN~tUHs$INL}_W+tZye>oI5CMg8P^_lkiIFD{~I z?;b~Ih?DV-j`JYGd`^wx5$ztiH*cL2CvD|v&`na))6(}3(r2bn;(*=68QtEM9-|)Q zVP8wB)l=@M;-4@_ zg6(vN%&k5%&vteGI%wB$U?9)LVjSblqVTlYO;77pqR0AV&O_q=R2gZ5$BwnB$}H>d zg#WQ#QBLNHxzMiW0{Yj}sSa-YqKl0^dX@CcI#s{nw{av;l`#raOziBcr7t9u#k&nG zf#wh3gfSRyIHZ{ay$La_EyN~FApHt$uG%|MfCdVskb#F-Id%nj2H?+tycbyV(#6vs zO-(}+6LO^QO$&A#1TKGFwd_{M`bl7w=t$(*ZJIIrrq2~1;ZR~JKIwZYn!3{RXjjt0 zhinI0xP*czOO^@nkLzedPPH>hN`xb%mYYwve*Wm_Nl*a+-gbb3-)^-9i4-~&WL}(8 zBio&(nq`i9+OllNsSKJqcbEFr&N?WV@{g=w*!zkhF??^MsvDsTk^2jBMVK)dL=yGg z-CTJlSYy{aCFINZL>wxgsX~g0p()ILMg2&%Ot5rQYTC2qHq{#(?H2LM^M^_ONph=d zC5DG&j`m`P=2&PZMyl9)>z~)^6gATdP53?g~0PV<*7cCs;%|iSu znq&9E?*C7y4U|kEm}-_z?<6S-=riFpRNOk($-cCrI_Q^Ry!sQ+J_Qm9}?*O zifEmM7G?_soEm*R<1b${Z8-Y@jS5X?^)CtQq%z86L{aPg z5`eJL4k%nT+ydyf9-%#Ova_PhyLnQ28W@L=Zb|iUd;gr%1fH$l4a*nzXqT>nvcr8_ zRE>f`|FbwXBcSW=DY4dx*}O}26~H9yHuzlMaQE_b1BAANgM$xIDS+!10yd%w=kbFn zU1|iVWPD{MOMVS5h*haBA5#2{fMU#3MiLOf??iiwQb3FRG)gE)DKc=}+nr*8X{a7F zst-&-(DPX5Y;oKaYmw0QeHGIQ?{T zz$uedly0V-l;-2aS<2aH0d0HU7^5zXSoM3wOC^|s9fwwlZpcV@MZAKu9_QS+q9Hr; z5%y*bb+wgMQKm!HF+^ULZfkuNJFyR{2LHGP>#6@7JIq{9J3VbLbl6PE=Kc7uB=7Q( zN+M`QrKYf73;3e$|PI{07BF$==|W2#AlM{|JBsu@mbkUJmbN zWxsBfzAkzr{?C&9=T}#eQwet?L#MQK8aPAe2Nq@tZ`E%)P=%;44=(1Yld|{kJ10Ai zFcmbMM9bRw@)T!rA^8-Z_ARn~i z%nn7&&A&tqOHwV3gxB%w%%y0oqxRNX zNxfbj+HDWhg7`6hf6lkm;}_oBc7D|*S^r9E@Ai?I%uw5t^=WnsJ?Z~m7QK%R4EV?7 za2Sx)3?KBp7A+ciu6^4>fHsB_jj9@YB%6%9yzvq<$=*!>Vq#DVt^3t2exI>o`SL}b z;(FW<5m3^D8xo<*l3gwi#*=t?GR<+F2;w6e*>V-ij#ArOTEbMKfB*&CDxXb8vj2Ex z-joN8M}wI_5y^<46G4$IOAm3>6<&-hlUd8$o<#wtjDaG7MvLAg_-)};6lRd_`C;=X zAR7KBoH_0EPx#g!2+&4GEF|E6tttpkS|gKVB}#M!IyI~~OYJ&2hDR_8X5R2X? z6JLw9!hnSF>HDXxX}Gokb7q8~1Hw6}aKISfTu)yS?99vNksgQ{R>Hzm@uT9WH>^*o zQHdk*Xh~8v6VmZ)N@fe1iVO~gJMo2^{aMF1<508s!>l=*=6{_cFT?inhw5?v5y@k` z=lh^OjSokUT{8Y}arnQbF&E6A4KWstW8>o>(x)5@`t(QFBn4aj;=LRa*Whu=%kiD@ z)JHoGywI34_fFu7FQPFqF#(MK#^&eMNLzQfi4v`ty0sOg(#*|xNVTz2YCTG15go)^ zSO44cKu=Hq?~+qtlR;~7tV=e>rB+}-g~n~x5ze}I-Zmrm7L^X;9T#*A)#Hgh0?kn2 ze20)o5-v+VP|+X{p^!IiGBB~AGTZF&1MDdE)GWTp+pjcfRAHhi*-k#)90JWl!_F5u zZ7zk)OyWf&}aP9alV1Q6^JD3rVoD7uM>p0u`!2LKsF0 zg#O`SiP`!43<5+L@>%d5v#W`7Hwz24Ll~` zEkHj6kQs5u0Y7SV>2JOD)6)4ujLGxagIIABjAMi&5{%E~HC5bcpkig}_YIIl3a-se zEk;SJsBU|V~VWAMLX_hIV503 znso>hhaHel?|4BKCOx4jfGFhT&O7m^>%MVr3{yO=(cFcul_9r?$1yaXSeP)4EnaE@ zP7Nz0WM+Dw1fRXNgw5O8(;r@}c?C|((M-ko*kVe|@780w2c#)dkAu|I+kdk?RnKXC z3^=7_VVsvt(H7rXu_I)d@ZuQq}nm6(PQ`D z(wK0q!DYyENe+hJ`U?`QHAS^SfJFi;JNq}4D2Gzlu}MnNEOvFMu4S&)q<@hpUaY8zG2Qt zTN6U)6?~A72~s4?rbNjiUTNk?O5+e#qT9_nGOm-__Ev|ykuQh&6Kc6V*=aZMg9abW zf~)y|=;j!1#E;<(=vBL%;%W8^HZ&2bx@ZtE?KY8u{D87N* zwkkwc21Y~)Rse^Jq>(`kq#ZHdK;b%2v3ZG&R-LovvQx=`(`vU2Dx0H%4Hm^oOQ#33 zQIoEBn6qkMU}sXuT=@bBfY;f<;Uem=kb#m}udVOw%x>nJ3TW)bS2znksaUp?ab{&) zi`vqjRwI+RN*F}XaNhN^%dV(=v^Hlpk}4tb!(h~{8p zG8=u6C1nUWXaI$R2m|mIX(hmH$ZIdBn~G)&JG+>Yl9CW31^TU(}c=0!BbGBC%JcjbR&dWD#~v? z7UxRdZ=$%jgc!2Ler()T{aO9%`N8%487xd$50NG9Wg!x<5BT zMdP-Cjehn@;#iT1OHa@o#2vu5*-n_JAKYl)8Lxr(3L7TjE|8bbbphGZTL6Ulmn*Va zRg2fd)^~ycCr{Gj|FZ~0Q|e|w?535W9?+{3Ex`aJI0D*C>zU@-q+Ze;>W)9^Q%AU###&bv^~V4oxxUHuw%YBxdKoW9lKsTmovvTr0D7En*&)W zw;&C-u2xCiyTUZLbX3y{YZ>|FP`k=2(pQyM|*U06^hPQbR4$Gocnrqi8 zWxsW}1(F$`sFc(O?EX!v&Q%DvY4%QxiP>%Ac63+=!uxK#(63VY?4<^CglsOygMm7+QY)gs`;Sc84DUJR3kguMPJ1n zRKA(Mw6iBNLR{&n;*yEEB*@2^>@BfRX>b($zM!vzF~+5NtqBu8QXPWvBU%+lv#~y9 zTbFswN!Rx^gNNt>dcW3P-?+ydKg6PJG#~)A4|&6Tw9Xiy1=PU$v8j0U=DKw4;diw3 zl=nik%sY?2`TzezA4xTL3!avtpsv>Qn&*(0GVnnAv8L+mXY$K_v*bzP<^3$zBWG=X zxrI6P*!+3L;Ih`I&Xr~Ob^{7p8cI|YK0y@T*D)!bLRb_Jj`R)*0U?HwxGEJGGgc`J3mU-SrP9){uiSD! zw9Mwqxra?6e^+$|Mmggzl~>XrBo$QhR#qCVB3D*!Wzk4z8@a8r;JMY?mz+`A7nVv= zCraO<1-`cPnB#+6l+}^s=X@f20YVTx31Vt}C0(>onC>^9PhssTMY2p9v!Q~zI%aBW zD&S$OshO_%SsOcG^C6<{!rzpXPN2ex~!;%IqG%w_QgMc#ZFjD4; zk_NK68YaOilGkZLy^g&oH}bck>ucWHRpsGW$f|AlWR(+6LRj12aG{V!>eP>Ndeo*f zu8Ym%qU5)GX?m%lrjqyE#QWf;br0M;^^Py+4SciCMrw@J6K%bRlMS70s1b<}e9X1e zGy9Rm{bugMefMG=q;tpcz?U9bYiD-2r2BZb_2>eE`J8d9NFogb{eQpg{u#G7G$eho zT`Z}J)h9_yIn`IW0SL!6Ed_&Psln~5Ikwun_-I`u2?wo-L=YrGmqk0%D#-#JhPx9 zL%7+d`B)`{))mHbTIf(yLq=6F6SR)D);uOk+)kJQSdjux8ie##wkR>yi=@A;1x#!6+&%g&B2{E}QGV_6i$BwOgek zKu!eX?%lK-JmJ3nd<7NGUg{u*iXL^unQM1Li;+UBtE*RQH-@QKX(TF?Rn*o}rGfxk z@h9IN%{Qe0A*(D&&TI)~43i%jTv(hKEx%nhC3nP6Bxl>Np^}V)YQ(fuVJm8vj%XnD z84p8-FwrrgZR-BiKuY|6oW`Ak^%2A*+l}g_gT?K@d??{5s6_d_em%0#-s^$2?tM&$ z!zry=sf~qCXlSJ&bsJ)XxnEyhl;bn&tcTiY)B$#;I$Pah3UDp#?CRXRXe<@{Dh+dn z_ek9c)oLc(w?O^#Gj9DC*pM-#Mwx$WM^EfuA1I@GUr^ez;fMCscG2$BkUPPOj;`ln z4HwQAZ#xpu|8v^xP;iks@M2XMumrGkYER26Pa@;}u}!%NO4aDROwuP9X(U7fekvb_ z0ncmCnMbq`g^(~85sxFmWhimI=#pEuW_Mh94~k6AQ^3>xoT zF&^@PEgA!}vwt);CQDNUms)3(%Ftk#{4wgk-p2(|ev*($3FSqXfcs7(uq_|@b!|F- z-~3007&pOigPh3^w2BmZQOWN!1}25i0&4DoSXQ>Sr<5oUld{K`Qpb9?d=wBJ9UbKr z6{OjdDyph;w$HA-@@$#KS_}wr&6Abu|j$OA%zv+H3 zL`zeMQlODyMJuyO867NaWvn|0O3C-5qG(`ZK*yn2RR%+r)6_;Q)0D%8karOzSEWdz z{d&oZB+sX+-Ri(e?x3f#Y2zWip(5D|dN39hK&*0Jd^n86D56%Yq6xPiUPlDGBVg}u z!l!i_6cX?(V*Gg|dvGa*CX`??fr@mvra^>V;(?`VfP!pc3PQoHX9U*JZKk$u=bHXm z_xRhxrs>Pj*!)u-H+tu`tTG1L7)GR75@ocISzlk+TNV?^|GbQ_w@b4fH#XE^VPTix zt`MTbjH}7%WP-rN$QXY_gM&{el1(f&(_J}NTFyhoV~0(SykXVVWyI9hScZ&FVa@H2_p)N*WqwT_pYvc2`z{banB6 zXdxasDU5fFu@=$5CrXES@+;6_T>oW3i$npbC7xf5{6K0=4f#XFkS<#AfnYl-VIR_o zfQz|IScxo{92#!sJWQ7{TrQOneTKC7ZX#2w$14kd~T^q6yOIQq^&ZHKWcCZ$7 zXf}2MH>tyL7{Nivbz4QQ_oC`_)BgHe;>>s-A#S~G(wLc6k1y`A z>&Mtwd2zBCVGcOhB{@9&Y#bqfvP9`(24u!EH9GrCn~!HZk9CK;ql&j_=P@|}Z(iBW zuT(q~F3S!!Ha4i8PURYrD8VZ$-W&eAUmDhN7h0+dm1t&NWQn$@@nV&Qj6MiYYw*#C z5TCC>zqY!d-){aIyqMLz=1#7d1Y=^M0I%tPL+ixJ0<&+FJGNd1iV%-are0})NvH|) z97uyK(;P_Z>a?jK;7YBt`#Ly8g$4&ILHnTUWosm9LF@R_ry z$DdnQS5~5mXw)<{eH4CXW#M&nc1DVmrGO}b+kct9$(En!;e>27sIs!WGRitD?|Fea zMJG+7!JkI5G}N~?v>ch_t2j@3F_{v|Q55}S1$?=AYw0dGtW?2PeH09uLRiz0`>-&k z=ze--%6cL^24J69PV3O7F4cGSefy{Z_NnU77|>_1&teiV2H-3<6lt@pnTPF%xuxz) zC!%fLrPd;_t2(ET20 z1?+*%$rE0z<4?x?A>|tBZ`3~6%7zFeVV?T@Y2*TD)SaBWMNHH3ldRAitb}rK%L)YC zU#ml^qu$)?f_HZ!_w=5m(RRCIG#D<;{frKDg9=semEK%9X_B~dzI>QDeeBW;~4y%ggQ12`6eE>xg5TfKvK-+B4|*fq;n@G>&aZ@sRvQbki! zN>?|#6>u_$jf+DDwh%6hQDFMzY>WNQ6?aWdO$8 z!ipYtF4xEfj>NyUA0IXW%Fk5EW77b*vbCkplq+R8!rfOYtu1e19FmT71<^(A?5{wN z^AO(@&td87Ba0TJ3_yzp3aewXi9Y><(+(MKk5pEIMiX?Uv@RCESc5eH@cW8^B~H(Wauj;J}6E=n9Y|Z2DmJO5pdv z3#wRgR7iOzrI5bV&+BW+xe7@+S;YoZXiQUN%W<7y#+}BRg#@+D)wy`)@@lC%HazpE zL6fyr*QH!r8K?;{DPpZU4nH*lMcd^_*VsPRZqXX*Q(8@l-31EAATGUrl}KfpVr}P> zGx!AUs}lT@0KtcFjn~5mmkg|A2P%hqO=PQLv|Xrc2ihD9zG0aGFo6e9Y^gPXh(MIz;^N#D zhpzdc0H&vIx^6WJ_>(bCUPO5%-Iy8LAPY^ZbI72CGtWnP*&^!#YtlXuWmmh-4>ZLp zoU&;Qqy)&S>gp2q_Ka7aT?}>|?R+ImG!mjm=QK?WRg1jS=nd*o%_=}xciaE_mJ|2p zL5nuB>jVUw1yo9NtBu-PCgA%VyW$P9D5?KGMVFuBXH{8PWiA4-&J$y4EQl~^s;8>i zL(yw8P};N`8%T(Ysbbg-_XF#utVEz$tQI0X>?ViDDgGs#)1CzBM3i9^6PFNzXUQ;? zwDMQO#EwK=PYM|5Gkze8KA6hKZli>P{EGq$w5(2Ckg1O$A5;oYRvB#17orvv6&w{z1`Qcr&jcHexp2Tyxl>E8qPhMO8@3Oh z%$8Mc-?>vLx{OJyH#6(L=9@6m7K)?#2he4B}YJKDekn8fF@E3%&B#2J-f7rEZYl^nLO3?Kh4q674n?hjxe45(W zuTK&dI9=5L&*@_PYojUD^Dm7ggrX$~9W*9OgaszH0q_%eV*ejm?;KtC6Lya_O&c|A zdw(Z8Y-LSE(6E?PO+s?V4e&64Fv2Om!T3ILS44#>Jo;`c-`6SfKFlfY7 zFk>D+R6s5dO9*scMJ1iiE>=N!I0J;h3oYU#W@cstsBj4*#*FX*`n^z-fX?^I09r;W`7=92IYHViBYgX7 z0&sLGQ|;|-B)T&$kc&%(6z5TNIEWA56_`;vat&X$3c>(u9XPbIQ_WV`SXo(9i^3{T zPGAy~je&3AcFn(BQ%Ws53QM0dL)Bi8_~R+~=fzR{S!A$tV^1oXmhYrhmMM({I6jZc zb5lIJNH@@0 z+Hjm-xYwO#D)XbnHfage@z}fwCq4uN-an zZB7C`Az=KbkKllGbkL~vBO#MsilN#VZu=3TW&mnDUe641fasDkkbGSIf0K`p&gHw$ z3;)f`7s^;*X6SEAou-GXD}wbW*VoU3bk0r!XIP@tX+(vDATK%|@V=4D-U3jNAjC*5 zFlqITjbFori_6Lkr}Fx_A~9h*))0aOyT54>$H6>#WxHt^4#wh8tBKml1aK&pkfiU6 z(#7Uk0<{`Ye29~5?;RxqPvLk}fe!+afz>7}u6&5{FM%mh9{xtRcWuUU4BMS653|y! z(c!@gWSRc~1=;*qK*l3qj)sPY8X*F&%WHupsMd;wJD@QDq?aSd7!+@hy^X`pg1ND3 z$$1n7RHai?R2o;^z9nG-RHw+({InZ86fi&PsW#a^iUc8Y1>1B>P7w=RglA06P;Ou^ z^=KEBogc&>z3;Q{JvbmrEWH+;wc=JdU@l}%5vx+c^$ATX1g4(DB`tl$S%sHD(|E6C zkYWPr^G~-ueD*v(ub-IGM(G&sagir6z;A+N3HtxiMaL*06`*TNXLy1YYP7k4r4sPz z_gc3u(VLzXH8=r5gHijF&7L;3laKS49EIT;z_h~+OI}ERgT8|SP27T8gAweWvyBrA zH5H3rts5NmrmlP7&)sZcOmlo;?w0Q>CyMG`Zl|8BkJ2C5W7>85QU08Zt;x>X#%mXW z%URnWyAtYU*n_?ekS!{vJmF?(=Yw%tH+_qQ6S6e3w1W^brey`j`XxVKq*+^1>R}NeY1HI$s`lVgRpY#gg5yjPN`+ zBGRtMt8a~-p?u&1qT9fdctq$UAaQ7*k)(>m-n;CE)>ov=P#@)=(Tb}nEe#hY@&g`l zBSN7Wad0F}7SU8u;ptWgZ!TK;7OdThU3htb;a!MH2^#SRCb~+Cl5Kc3rqOz4dRmYL zn>1nQcPSOH1bZ2ZOG~e>Jq6|HB!FsPG=II5^yH7C9I6>9Ntt4oNhv-KmF^(3W@spm?uFBsGHXloyR0)Jk-%PZhj1T(D;&kqD)DG14 zF#l6DbXD>rC$s9gnyR6YHJJ^nmc423ud*f#=w?rZ+kBt>1SxjWoeR7yI#Z-v`{elg z8GY3SZK3iB$2Xlj^)UG>LS#$}cmcH8AXK>*LmVIYy!QK`(!){vldS#cs*YREnzvKy z191l4{Yq-z@w$M{@i~kOk^aWitBsG=4gE`0XL?gl?<4KcI+04aeur{v=%ha}ItLuQ zNfJUxJ^LP)G(3x~E$%avQ4HEl5Ue@C`JY>yEOc7H9+1d0X?`qBCM zy#uL>tE(Xrf8K`94##^X0Tu736MzvKvb^$1;`0-IB3!q9(orR=X>(P>tz6n z;cSMbwKWOtEI}r7l!&>H$PxYR_tPvq4q?_MdjAT*IHoLXRG8XuRnw;W*G$!pV8m7XH9O*;Zyu1_ELcd~#h`6pyYe9^o1rrLL+|-nd}5{eXEnFcl$lhQ zWGyixu_-jtKde1RJKDK*Oi<1StXYw-pYrJl^d1b<(&`ame({IG!yJx``$TQrnveL< zKS`RVb6CpF4*wv2Z zxW4j+hOs59W975&>bgBU_~m4RnW+79X}!v_z^(gTKuhM5?<F zq7ss^bWrB(Mqor2`dPxqi1Sxkkgg{bD2>7F3y)Tf`x}PF3%BpvnGg&3@*%Qh6~(CQ z?k?ze^)1JFt2Y~c$A+NJuk>QmzQla?i&tXv<*y4tXQF&>rp}l5LQRejO19VbDW3yN ztguJ?Ofx62l&A+gotZ(;ZGTC9@j$qrhkVBKD;(f8ceIXU&KxV3fY3#tfrbEnxZtMW zVBG$L(mFc4vnKN$aMOM$a?Z;o4|tyh5L7FC2WXX0CwD9-|Eb+n-(k%;)xMrrB2g9jWTuoj$LT=Gh(maLMy z1E4)+C4h1ikHfE$Rmx1W4rc|TAH^)3zh6K^elU&X*uZJuu$(4RuesAdRZ*BWA~yXa zDKt(m8YLRPiq_QBby!?9y|U%FQ@@1W#!c`^G>g>&RvPo~j?GRVH-^a4a}*}l&%tWF zA=Wu|$Ur_H+`KK5a*(>3Mph0Kzl2N#dOPI+1ZdRi^60qt%?)|6T4-9QKl8+Pm)2RA z?(NQ%eG%irZh4E`jg&ozl-v14X7y&&vp$`3E+1*X{suJ2NW50`O;=lk6fY*pxT zIKiBV}+T8Q#;`P?H zO5SWa#&Ci+c^__&#ybSe?eQI;!6Rb4?iv&!QRM8s-L0UXf7^PgCSA6vdzsK}I$KKV zy!pMr0TFus;|XMGvkj`@VSkFq(N`~&)Ia;mYbNte&9#rQ`N*mrOrtvbjsR+@O46u$$X#mv}&Cl*4Z+ZyfO*(<-2$Wb7^LZ#hz?xT?VFA!2psoh0xXP34I#%o` zkChh@Q;h*}lIf8rShu^lmHMV8VABRL|9|Dwp7_;#V9MW>M#0!d&VO%4E+&0 zvkz^9ClDJAFt<0Cq-o!g^x>5{7`&lB=phRciFr%N@O7pQJMrl@56Iv+ig$E-dSPzX zpI_H=$f4k!#X>Jeg0yN3Cp#!s5#8i$VE`D;??3Pzo; z!)GKGub2}MN46Qr_~W9}xM!g6#@nYV@62u}qND&!`>unQ$5H#Yww-57>avUmj?npm z$HVTIRM$s_Ui0;U*QBG7qoiRktN4Zgc!F-)oaR+~oAoDW{IS;~I* zCCmDR53fh)*}Bc~(Rm;8W@EU1edF_l3V{#HB{=lB!lrxUi;A#q&h-BJ0i7EE6;^E& zYZz_8JE;a`gE9GK$YbeNZbqp%og;g!* z_jNJI<`)Ic-cp;*fgc9%@3Gku>=r)q>>G_}Or7z%CW*I4^OwQAvo&`oCjTm(TMfj- zE6Imd(1ALHg4SP0*NM$;jCE6M*dEqz>}OmX+mGFxL`|vWCa)P9^xncL-|{?W1v2f| zP1EL)*X!-Syd_S3|Fc7C(|&~p_YKsivXLG=i@Rj$TZGS&eB$f%t?6}#gG|eZqG7sc z?GC+3`wkAD_N-^V43qWDsMBqnkVP!Xd0>QWZlGvqEu`hFlHAQ(<41J*@2lTcc2Dm4 zt8cV9-4C7Iy>HyFAKx$YAz^7fA~V3O8EPrUD8j{APS`2h9xMoN+7&Tj_?XfC;fd6Vw#iVauhKOx>punG-ImZ}% z`IhpI`y`MQ>jk9ED)~pLW*nEQhCQdUQg#y!UD985YO>-aWHZEXwnSK(gFzi_WG6 z?2cZaR(wPjJizrbYyJf%ptZ)*+nxL*Qwmz4B=tM4ugd~AUi;Ao_w&33TyY=t`4~ng zSfxnifqAH{VY~qa%J%K%=llIMoC6!h5ZOJ))|X7zQSk@xB7)UJ(C*IbVfYdYc$2e} zu`<)qYKvCO<*p6qb;gptOar{T!}LE;vYhE7qy;?%h>a+M1Obe zM%jCfXItz+BkX?Dsnxnt2#+tTtE9dr59xX2WEl3NMD6e30eH}evwgqG@OI$V(wzoCD)M*9hO}3Xyj~9XQq5&QA14Kv;Z2{85VqtJqjzrn;I$<8IvHY}Gy8-P z5x?gq^=3&DEVpx8g(In=NZho`VFP+Ye^=*!*i3WZMB^O4`(Pdivq`_;EIe!EzgS7aKHYjE|YDvb+(&T>|tMkOM- z^pQCcvusyK46S?F&+_!SpBCKf_h^L+5p%JNa9%k67E*^UTetI^=yE^)@nyXIv0*f^ z`^P*)b=}kT^Yhlh%~?rOtoGlsx{IA0K3&Q8Qhd8p{tCI6XFk{s&^k}4a^%+Kgic?W z3hd$YuYq@Gdfz8cNaD&=SsC}UTL_G+tR3u0KRaVuD(jA8veO3cw}G-2<16H7-UkCR z6(jeHtG4F5V>C=YQ05M{lQ8J5nZvsv-U5d|^}%hwB}kgnhoq#_@!o}6PS+Q$iN<2L znz?fvFF<-55t?sFk3vhUGJ5 z{z7QpUBf_{>6}AP8nk@X120pBjqfMZb8E1@ewOUna5jIl_DPv@f#X1(wGru`CEm6Cc!k;WwWCvXrXG z=gpCf^{@K$X0@-`F>N#(z%@A46xGGURe-3+_8)TEj6-1$ymRV8#Q3Mg=T2bS3O8y@ z8#_kI@X=Gf?tgHUbA+cUnaK*=uW7a%2dkBE-8*{OPW%kNnAqA+TevzH*1hZqD4QDH z8+{(LZLac$AMidh_^Xyy?r{Tqt}UK(Q{si1Br{75-U=|?uXUPrN;mMP=Y|@=duCPJ zSQuY4oY;$pWu;g=j+j*PV%yd?bEh!$jj6v(sap zq%`qk;V`FDI8C-YEhs&o`OPuqI;h*9RPHTfr3wsLQ|3DwhJc;6V7q(#U6;L9%&*K77Ta<2*Fl1C6j#{K1Wb=uNPkM(tdUlk7{7KgP5Q zT-?2`1-OpzXDjh>`CT8O`CHHm2v04#CZ8O)nCW~3W|Jri-sfbnVnO*z?YF&9NojSddD zyJ3t4HdJD`%(3`mX=5)TK51cFN!-pGZNf#~F+%Cd*Lj0e=?wyhzm*wWaymvxBMsCI zDCysBjrW^UPtz77T`*abD8D68NE;0Y`eCMQbd_sFu;2Dfy=$yH$B31K3(10O7I+;B zw`Lnbqu(DodrPOCEQW>T<&b{Y)YOQhZZ&_2vrz|MS9_Swc2Z7dJzRA6);^&tNQv9khLVN)I zshcR=*pE<3=y*4y@qT1LqAlNjq^93&-bZE#&c!I-9LoXqIF9i4hNA4D{g~Fx?SD(! zQqz)I=Cuf#r1(1@JnyK**^r+RcFh){DT6709$MPLzE{wTi4H|3XXIvvw$WzI=BRnU z-@Lr;t$19;+w1W9u>Qx~l0D0^(?`&jJf5O?FR;wa%&I_ECmfFBr(>*Hc3(l9P#6`% z=mL*s9eCB6HQqUjh4sF;?Qh=uKz5sTIEvCAhTP8Ag3y`q8D?DquC21y(@}2;>aI|M z)C->!5D#!AXkX13n4F^1!OAUhm`xl!msPN!dwN3=#6M{1R~GuFa}6E$C_ z8=E0w>iCwJwCP1IB@6%V`oVihih_F|n%7BroUr$bO#|C3dh% zD|exp&vyzF$n2b=+ANxVh&*Mn4;s=AeF9$aG-YlyK)Vqk^SKcD-&uLRPB^o{7eAwU z2>Z&A-cqz~OrJKsME^azXPm8Y4_kQe^{9-?xyJkrwtRaYCvD48V`F_>8u!R6(j^z=q0$~}$N02eo zQJxOkgEqw(xKMtf)6Vp{lAr%V0Pv=BluA$m8t%3sKv@7t%0_h>z@4ffjIH)jqLr=f zgI2Obj$*lUzxWrzu4>%8QtCX;#@^o$tc#5SC8VLE#5C3?d9fIaf~0}bhNY-1Ka|Uy zL=_!2aQW3ndJLD=)EU;=BJ#=*lxlyseOiNpuLzX753zTybUT^{nQFS9?1kd#16Q8a z6<^w#Wx%<=|D_QwU9Ol(go=sKLA1#A7DG>^MnGomO}`dn}V!D#U^+We8YM% zB{zHTC;CF^g*47WO1kNlA7%N{|M352e5dBy>9K#OH2X}e5RpiE8vWqz$y%&VyVbX5oF8vJr~qidy|D48XF#n$?~RL#C`2*GR~7$bgaQ$ z;{x-?%M&$BoC^icX>WwkG8Afa5q(<0+Rm{o-jpH?rzEK-}JH#+oG(<+k{N`t1JC^dCgG?=Q(KPINlr!QH1k z-0L@*RxqB3Q*Q71LG@{dShE+4i;qX^7jCN4($AwvPrZyI?xyEoxL%tT zo|aJphG?E+>7eRTK+!fsPP7_=KPpB$uPsNlpl8f7ch{ER$6Z&mX5J^0ZB^|XM8xbzk6T(V zn{!?l0p1+1p~M_sIa|tBJxLxn&sDH8!E0yEbnF={_w6APuPfM=ZoUm&RIgD|px35# z=Mrl|!zE07dQve+rSyCr{HX zrQ(aaVmO1ru^!QiAw1$r_!|a(_tnNbT1bUYPKvHuHhA=v*6|#-pE5;flrn#Zr;{D* z#-z$@8w4~Pwv(|~k=5nv-KhTh@mDdGDloQDqt77Rw`CKp=*U1SiSi*mEW|IWLUV-q zCGy@2LRGK}j_cm(hQE9&@4mCAJb(9f38y5-7wyGA-wwy`?C%c2ACH@P_)R$l4v^G5 z(1OggS@*m}GfBSOJm zBvfQcTgHS<{B?4lpL;feSy4?}Krs=FQ#aDYV8TISnVU)OV3w4@d<_lV+vo9Ppy1-Z zAB|1q;)$pMr*Y0@buZsy^JttV82!ImvHn);g8UpgVHTo|7a%quS9@Rbh1PZi4bg7L z!vByv*n~iG1dn>USuTs5rk!);{!!pCKsDFdPC}}aUKv4HqPoVvx;DsIEKr!WQj|fx zRmTj+3P%**Q>wz1$$v+dLQg8@b=ZCG4f$E-b{d?a&ageAa-mN6W>N*G(~0UM@w+F*enX#}esQb3)|0DYWHWK7faZ zP@MasV`OrrWAluxG`;(!0p*2l?bKW4hv>ZA?RUa0mwZybA$D zE$f97&m-dV_cJ~>PR6VW-CL1hHH!N2H|QVS(M@uc;T$?E_mNracPY*ZkXn@;VQJT;vNIpSiz8PSr)Bw?aswz`da~g6pwlU8pIpIu zGJoQ7pO*-qk;?_`d^sE5*&E^n&l9HOyFEgk34XV6N=|>X&({VU9q?Odz88^^97m4H zsYFwT=WCcZ-EM6{`sGN!B!xuQ@uUJ>ugkNQn$O1O)uDOtxi^c@1F!$i!FyKD>yC*{ zIzzNALhX<0GdAo13e65M5#(Qo!6QSC$L8J^Fa~JX4>VzLJ!h&&`?@#TMtj5q)lq1` zv$L#*@Ao&A{`J>H%r)O3)TPd|UG+n*${z=q*X^DXmN+qzF{Jk+Un%Lk9+eGtMC_)Ir3 z%<9BdM<;8DpWxuwimW|HI&RRR50tdj*!CU)(&ERDF4oVSrt@fou2HcTiZq|pxDyeD zwHFxSApsOW_KzZaVqT#Wr{s?!KH+lROct3LM+wg80P7Wif3;VVNT}=Q+*5@P_ZcA4 z2*OlJhviYM+uq&X0a5~k8qBk^v%^+dfR=T(-0)9-@%OLj*RW3jt`G1%I7s$7ais(x zltp@LWv+wU+D-I`O*+2?%7{=27_aHwD)NkAfF`PsAxL`s(eUZ_LDU2&F8#s{r>QhQ zD<_*L^d36OKIv{OXw#)UiHOMz6%| zn-&al{FX-N=%mRw^@aqkVnoKr;K0F`WBUV5#D^Vw>o*JUnX32!k^Bsb>n) z>ku~5{SRp?m{w+YoEg2T_>Xb01CBjD`C%WE*?wW(;yj?6L5EAZ zTeb)}IWe27{4Sshs}Aj_g?b?s>~x7eqtD8>DgNH(N%@6K?-7gK{_>Fo&i{7PAyQyK zpqNSlH+=20)xZ&!Z8XRx)&@^9e8J6HA-RK`e3fq&PRi}fFk`VfSGhEUiS7AG%35ju zNW_0jDFc@8g-~jR7SY;*tUIH-VNv-+sqd0l!_$xz-f@;t3v5 z?k$G69By~7$C81c^#oUn3Cf=qnQ`2*R*6E(ZWm9;>~2DuT!TPf@gW$QU5h1GQLY0f zcR3IZC{FOT9FSYLgFiCZ?uX-(cL*{aq|EkqbZfsBn%F|!MPbqq%y9>zg7=ahm`{~_ z764>eg>MCkvi*cU_>l&;1kM6U+q5_VB2d=H-6a}=1G_l>5SqKXe6N-xY( z-#K2p?9a;#m{VJjW1)kL_zyI(k>kv|*MbLx75^|=I%2{55WdwiRD_6rB7jB)Xm%)2 z1%X6kyTD0y*t$??8vj_O6&fxvl9&<jDPJ_YCUw&OqX!q?Dr))i)6R;8CvqQE zqvezs5nFAbfDr9NB=&~D&*m6xs*j1+)|ravi zSSF4RpL-zt`*0r!ej=9$=MVRv#hv>&S&oUMCQ4%}l~eCg61>NQj#ayio{|@~-wmLN z1gX!nHoJ>#EfqN78jvvG=2kBml%@AZXE1J@eBeS5DfmdK01{*>r5KUe_*)=pqJn3aBk4RV8H|K@ zGkAh@dHHCFg?3bw6?e*)>$=t>6usT2;Fxw51CsZipOTh7a03r{x)Cn)nGYb4*Gd@X z~uz6_e2eQC147;ujKz9ufC)qD6DnqmryLdhQ7^@x(+rijLUW_ox(^Vt4_H#8f} z?&1nPUhTbPco>JnY(&Rw+n%lYGsYz5FUjsgZ7GX?ZxJhU9{3&ZPE!=0>S13h$1}sy zrX`8mx8_$qQkt;RXD9lMfx4K9JwxMT#+E+%+081BQb zw?1uciVMU`-gDdY;l0lEzP{75k>j(Zw^mCG9J@?=;`Ri>CCT4RWwJjTo4oFD+F0-; zd#5W+-a+8&eYDagSajkZ6fVRTM{c|vh;n_oi2cB*9=<_ z(PNF`*gaGX&n-FJ+md0WcJ{0R$urvlxvsJ;hi|pq;Ta;TxJ+m3y^W8o;bLL2N>syC z%ab)UYO6r#DI(kyr%tM}Rm0pz+qRIV*>CFmXS|*z}e8bjP zDYe}zU8El`nD;mv6Ae1L;FfxDaa0VbzIz%&M^B?48G9PLkoAkY$}cyOPtQ+$)+d>k zFHByC9}+QiwsOhE9A*zKdgdLL$FA9(aAy0U#;2b3aTtCcBtzS7R}Lyjmq+M%wFiGa z7y1}obNnr4G1qtav1*N3` zjSirs_lZ)fOKwfE$Xv9cQWO#^wc@L&obCB15}C4K->t?gDkt{*;ZrN(EB6Fo=sw9` z!bDp^ZVS#?0MnC$6k$ft{!?zXD9zU%7prMBmBiY!Vwmt?KRk)aP6rg|m=Xgo!%~{? zdmCm5opCFTZ}b=WACql)RP3{&b>#D|+u9MXk_DjHEkw#dv# zM1WqN(F=ghu2nA~zkX$foe@gg#NotWKd8^JjK80=jIU2pU2WmK-Q_smE-Zi@t*(t% zJ=89ks1aMj+Yqm4!&n?!$g4NyH!2Kt>}HtS>ZZ%XeP_yPUM@xn&7#fHHuV(+bxoFve~%wa6OQ+ zy5dZ$DzJlxc38QQtCn_s^L#&(AXg@5Bd8n7dz826mIjCB=^HlU@L)HvgLHj>5q_v5 z!$RSvvHxQp{;eq~Adc2Ando$n=*x`L_6_uYJwl##ycLJQm-&w!5dOA{4ooY z+%_kQVqI+xO0eW=1}4V0ZmY6n_5T{Kdb}~8<12b(mJ)uGp!g_|d6o0V zJp;b5!Rx$&3XK1KH2G!8P|NtDwWL8B{A^6|u-f0-1Wp^EiMoyLU_bFRwR(psCC2^XhiVVM5EJa7`bn4YRR2DFsA(E?{016Lx}(3G?47tC z&8nKT_Djd?I_#L(Y$M0(Nrg+>5FwmmiQX!4O}%U4Gl2M68b?X)lT{auC6{>@nIcmx zXVAl;_1*w??zYLd`g{;#52L{PZWMgV80q(tInsjvn($)sp7ldPB8D@$dnfp2s;tiK zKHJ8MUk1@+cnHJcZ|Ei4A&4;3X??1G^&Yip^%7nfx*B{yUUTqE2GjF9rs+`Ff!&ix z(kjQv=l8b-F?;z?q|`xJLnI@wpAZ$vN4j9bIOvPLtq|}~#OGfOM}CH>iWna^+~Ge) zd>Y#Jb$>qpb3_3VSZszfz^Ka`^-ZizUnC^!8MiZ`IV4SPZA=%;=jn=vucm$B`+CP? znk;R?q|x?=n$~(okP?;hC67q$^Jiswfy4+kdm$7b4|+N`dq@EVg{n{tG}IsH^bw){ zR>`$!<@Je$@~wr|>DEjqOPwIT@yxWLX*TU?lCguKd?O~u@w5cTcoUEHnk9$q4PKUmi5mvXUMtUIo#c&_?$65QVM}6JV+RwQarVK%VdPg^%nOb; z-k?1(IsWD=vV}!fVRdxZFjj`KVS-5lGXn1zVI#T<$A`dFgyyv|MQm2ciFVDIvDU_>(h=YZcm~` zP!se<%5<#?4B_8sKU2Z`PP^v1w0J`i`{f4fR{XL(SvP&&dF)@yK2%hx;~R%4em7{;nzCu~}@+Q0$YmYqWgc zxfmbnMFSY~UsP;|mrfjZG#5%7sRhlEa9@YQk0lCU2Z|O1nu()m9Z7-~|4`A(OEnZ= z(xf3#_8qdyGo(p6BMnW|NIJWo5lQ+R$MB45%6gI~t1KRHF<^@jDZIOYd1!R5$;x{$ zdB}9gV{~rN-qm01D?8mFYO-ir&3B+o@K`+i@Ts$Wf-OOpQnNuD+c#n)E?C|8;-VuQ z*sAxU=Z_a<8f*`BUOVjpF6-0wd7N!0NRt85NbY76CqgM^fIO4gA2^?Ca}8C>Q239c z0LNQ2ztGr!l2Ltw0UqE10*Y(p3iEU_e#%t%cZ$Np^Yfy{#zs^ps5K;iWB=h%z5T)+ zb~7%azPmQy`5QDk!dl$#LkA88Gg#QATJLAoSCMj}5h@Y=*P@$xCs^@1HP+M}Wtagg zjVBW^^;*6@B#C?$uZ@+_2fpx&{TvChjD1d8;CvOUwPmGW7hp@Gmcu@8SA!tr4&~tZ zuhQXu;>eVJe&lXz_^;e*?{DBkVx)_?#)mQZA;zLxn-1*;E1qBK89BQ(3&hemx^k<7 z)6Z2P`-5Q2FLD`@Mpc@P(eQg5ZmqM|PBd5u2WIMr+}0;QYR;m>-1CV0W?N(ep{{s9 zAYf3}c3-3L47ap!4|Q{p1tUcQN&;8WN)k^im{8$OBmv9?@J0ZJ3h=~GJTzNzp-lWF zd8R0oi~MiA_AsrO&33R?|1=cIQzSN`(agBs8`Gr2!Rg(HN@t+gmBH1_)>n;jv&X`ti;{oTpBKq&pD6UY_1D|+T+DN2Fqa^q9_L0=T@WRTQ*<`!a$ZwZF|E!%c`giYv^GS8Gn){_B6K*S!)fEsmH)l$#9qK5A>}C3 z4+@|n7q*xdey-;rFj;`OU*?Z^`aHacq)U zRD;1J@VhR>T)d%{JZl@5HhLg(zS4%C!_l{9Npv{sg;~d z6);Y$kMzXW}(JV`Dk50IaB)&_hV zN`@KDK*#Ke!TOu6biw+1faHXsuGXVgagXgeIJR5tlA0`l3Q!i=;XrelDg}vD%Ij%0 zm`v4s?8`_z)=@^RJTw){3se-GugOst$1CssRV)l2irtzBVEK@;Zmo%s&&@YI^S09I zktifX!N*&PZ6Q7r_94I^shR;Q1nU*R8U?Q9ql!XA zkp;Vvh5yso(6&b_s_=a{3ALz3eXvPfea_mJy?m)bw2}Oi_{AlP$ATXAFcPK8m3c?F zYGt>FWOp~KdG_e}dkc!-Y0EI1k0P!6X53S(U}!E8&>3-tjBX)dss!Tx|5Jr{3nve( z%?iLlV#tUV&_QEnBiGWyl0wse4ZCf%PA7}rnOs|wp+P6cK?I}@z#*Gjq>)Yvo9N$5 zAPu;R6mBxX0N$L$(512P^GN=K1TeHgaX}wz!~+BVQX*!k3MnzcbzAy}Z^jYu!+X?t z{8B7P+-PhNb-jl}>o90A+QS*rNs2CYO)|FQas#VPrZ-%7PUH4Z{OOw}@GC&>&8v+n zQ)Y7@S^jwf4nQ6~wUG#$e&(>;gaO!e;-!T!06Rbk2kIoP`FuBqe~S?_^-B zv+#^HyVVd{igaO-1o$i-H0Q5BfamQm7X)u4DPi4Sm3 z+_s(rZNZ*D0+9t_2oc?mvQf{ht;;p;KHi|ykffF4o@;*<3?-cV5f~V!TS$<*n++>C z^dEZHQ%eIV2ao&NDj*^f z$YcORr(o-n1M~8E%zjJ$V5Zk|*rdX63hoL|6;5pam{Mx*b8|j7coSjO@i-R$%NjXj zBV=CH@nm=#&%M=zVG~42fx$pCpD#zHOP-#Rfykb@wLfbMjk`m^YpnC1=VF1N`~Jh` zA2GDILrO4|k)Mj>L7q3|`?>bN-UYI61_|n8V`s37Th4bhCJP~Pc{RIAz!2WIOkoP% zlLDRlBG?Ob4iNkPm%d5r7b=!2lODCwO_|jz%N(&I0>2kl#S2j@Pd+9t*8}$>2kz?0 zD$6#2kROoI766uUs3yyR$=C?;eOv$YTsVbO2~#a5%dvr;dTHLR?E|Lstef^5=y>gH${df*yO^8cP*ezIvj~wz|lc04{j_k3wDnQL^k}l z$pGCzHW2Atsz5#p%^{2`~bodPDGKJav818f6J`XDbqgc&6YNTvZ-f&+Je zv(d9HDF9e!5k3GWzn7aWo|6F-M`3IqnbMfy8Xe;nk zG$XWH0dQhUPu4m-RLgax4h;plU3bGLLM=aQAADX;{H{S)dXeuZUK60JP{suzupcXltbuHy*tTAK<@pe0&TOB>2xI z<+8UQXg*-7^x?`4rw|qRARdz7kWUjatVgZ9S|Q_y3V&<~NFnz#M;Lj))`chzV4$JQ+=lQ$K^Pe?9=hv* zjtBipNxztUoRG?oyv`i_y-_+{iV1hD=$t30W`)ESV!}A!(TIu%C1GfcR?vg=HDDb& zDJiZ|4UydW%2xmetKQ{og{51$+U)CV2l!Evunz{M{!W(6(%dGS3WO2N&y+@ky8&7A zB>>srHi2L9Ukv{X7?8xok19RJ ze-_xkImU}eb~qqO#PI;sOn~18ut}Vt?ciRNXL`gewaMEoU?kQvL#l{UOe)BQ zub56&S{4aRSQ!86jlBQz%$MS0JSjssfzX0QweSt|V1Q zlB@Nmj>U^O+sIm63?v9z>xB6J$JKna(=*uhJrwspRV262OeLVs)JB+z-agsaaU^h@ zI|8NqhRs|t48hnoyhJ{x^6@rPQpHb%UL;C{Iljg6S2%xpCiGxY+b`xq|HA)gqL4%y zM>`iuD5_wj>Q`~N|1)dIZ3Kk7>v8f9W*W&V7E8;3n~k?O&m_p%5?LDYwnZ@U zuMShlJ4Qa(*`N75QEbZg9FRih+^+;PNF>fR9i@DROclox-Iwu=0O|0j%u6^!p7fbrr@mMr|gn0m{AxPoPC zG=u=b-JKBJEjR>%26rd8yL)hVcXxMZa3{FCTX5HTJLleezV~l_&hDlt$$i$ z^z`tiz@fm-cpx3C1LlAMJs()0Vk-kg(xUIImfP~nMz)-zle_O`QjD4B<^V)FOC<#k zp|9J)sgz8?Ou%_gDS=+0%9hT0&}Lnd%aIKML^gBv`l9`;Ds8Uq9ie|$Fx`GyA&{)_ z-D?~if6|FB6e2HF_H~SA6Ql9}S9zm&NdIp>26ok8MUa%71oN}7>D`Gknk14ha1H_f zK36Wplx?M4a)v2F0e*4I*@#LVS1L^z_L?I_(g3d~LIRl=QX7xLEQd-}1X?-9TdhPK z|E?g?eaE_Zvxpr~&tF@NkAvpPHJenqOns3FRa6y+#RA@p`y#Kxdz(lf$1Y*leOM95yb!|&6Cp1cJeyKB|3nFK!GT5+9c?*^% zf(VUr8iK2qiioZK^d6Pt_Rn5&4N=t6;ChRQC9!CdV2!2n!nX~s-hV~=3(nq!tunuo z%+NMDXRdzoxsWtSXAx5ff&6ZSzxXR^$M@Dl6KKx+)mWsMbHo_~VMMp7Xvd-V!P$X$a^$O$>Sf%qlLF%uH=)QwX z=Y));{lnNDW`%`D+YnbTdB91lUZm(bMFT3jn6no+iRQ@Qt~)62Iu`A&$p}_!QFyT| zduWqPmShcwY=Kkjnx)sdddmkk-u8^hg^>v2FzhmGTA#L8H0$-x?8RCGvi$tXM-2D* z)-dV+-7Z(4dCAdbFVH%|Z!G}kRYwnR0AWLUppNHAF9fp7t(wu-e4seQ#9hN)vl*h; zI(DqpE`!D(!h>qb8f1HOoRWjCT9C6`KVek2yn|~w)Wb{go7h^vBF)F4fu>i- zG_6*{RM(R%exs2{ow13K;O%$>JGgt zWiY~O`@!>t7&wUff5VX!T3Bd!T2^Sps~pg6Ii4+!|Bqbi#x~suNzPv$T^19IYtx0urzK7<#yx^UnYenHo5`($f{1fq+Kwn~lpxbT-a{o(t>~eBt zIX;P}GzmF+Gc23-Hm$G6Sj{|K8dnre@(P!Ltg>!-fjPoSppAT(Xw!g_tuf^m0rjD`ZAe7LYjDX+lA5aJ@a^*n3{_dvt06jAQQlSUcY`iHyfq z@Bv???%^!R6>^8tc3+l%z>mgO6Wni<{F)XSv8%1BJL$&S$MeYjzjcBBBLW+?0!Gv1jJ@!Zt4AgIiK?OUfKlKX8sy#h!F7+LVykuWjcF!Al)2{E z^A7d>;q=?J)?_z(Nc#o@rzX+Xs_rAHOZAeVY)LG&A|C9j*~$Xn&}iiXKfgW$p_RH2 zX--XIqKocOVsqyXvRiXg&8sh^TPb^BB$=UKVo=);oIY~{4^pAJt|;^Uskg!S*YgwN z>mS81ZQaje#J>hK1R~gkp!aV2(U{W!`5@{KsXkY86!2383%nDezn{O-Ws>O?{7(kd z-}`JV#GvgiRlWR^#pk%Y#=TLO%bsNXGrl!IC#WUOTgdDQI*{i+yoyiWOlcUT!V1)U zV#Ax9bshMiXo0vw8x#y@l!B9ZL!L;Z%wdfsKk#e%~=4#LM20G?6tI=f)0xRH4tCF)HGI&{pxlTN>nz zPB{E-%U=rmTcmMi_ncksr*=o4VdqLomC-tVs^FT?rbE@~D_?cCgrtlf(I9?-k9jO7h9fK!6ROTFI!A^tntzBkD3`PYFqoOUBj3zjdf=>F7*eU$#o?S zQYl~FPbs=w)et6|rnWy1LVdejg(qNHUpG`>v>!|Sc4>I}sYFoW9#s40Nm+$*)g-T_b8d-!} zer4FD32Qt+wcz#Q3^Zo|Vg^7EzjdrMXl*>Q7Rwdt#3*f@+LTIheuQdRXiW-JL&`p# zDI)Guh6WvO-@yrz0>!DK7^1f6nJP=#0130angag^2syE?EmXwe(@piJ#kdB)WAm_D%(huxx=_@Y& z%uZY7FFUm@*tAeu4D9E>?EDbDtyA#%W#n7PH;F=-L>rUKcN2NBKVu;o+C; zmc$pKuOI&7A^y{({Y?vqF-Q-HdlBW%1e7a*w`)uY{{P@eGXMJuFUZlH%yOgt5uO3A zILX|j&ZuJcp!=!OY|2u3sy@M1JZU?g0q<(fUe?muY{=0Y693Vm8g=W|lWC8jf~vGT?5M8D9GdYJBAK1k@vpIqKlt9j|qyc`vY#lmj$ zpKDrtf8Q9 zL(B}OBy}PKde*1>mjKc6f{OhI^r+o&){SvCj+(ow9_YAVQ9MDcSnHz?@5`g>-(E;B zALC{8+8Qh{|0j$Pm|;l={vA%K%oe9isUa?f%$5ZV4cecxqeC(L}svn?!t2wR!=VcNUFc|y99PQCr82e z`xM0jGCd!%97Z;XK*$f&rRZ4}1JMLOjUr_TZ;{H^FQ)pL8BJN`=lK{%)!S;_=*3r{ z*ROa}AYIOuuwQZ9>G)3ZhNG&DbzrfWB|WAzAfV&y`o1!0xBNHs$80sC4-TwgM5{?w z&$tn2|1Juq^&lSkd|`zT*{fCdbVZ`$aHX8fEZCx>H=6%W&$`axab2a4&UG_&Jk&3n z``$=_$AY7=}Kv$~zAE*T3<#<_K)&5cy7Ilj%DErLnKcq3(w_+u1 z&k6LaKTw2JfY!*oR zSFhA_LkKnY7!48=a|0Rg$2K4{JI^NKMvS_4BL2RChK_npUY*~u_EOc z_7KeJ3>;+j98U~*sk=fC$>OZJ|6f0aGh!H*lmtZw+{C3&hu8aKYMt(hT1)8d7+N2! z%>W7Lb`}~;fI4k&hYb`mWbx)HB%!+Ht;P_7et?QLAw!>Dq5V@s9USAmssheGZQXA` zVL}Wv!zt0{EDa0QTPdM5n9KL$s2ETd3IbDBjQZ*WYt&@Go&X{kSdRPdp-Kf^B+|n@ zp=?(3jEIOP6v3QGuk61_;*Z(k46D;TY=H&9TMjhy>y2YQVPixdDb1a`+ItR=Nr#b$ zGT$mQ^#-HM2Y)E4xnS!)`!v@^ac|s565CH_$n#prx0@R6G&1f!k5!qtQH+e}K0#nJ zR%~3bvi=9#gN_?Bb~S$z;5>MY53(Yb;k0|q(hQll+>#V;jiJ2H0KX+9NRIFUsXB$* zi3X5uxY`|hC=2@}nGJKM0f9i-w{xVig9H%mSMq)1jE6& zrRD;!(<}#q;QM=L3aOOeM(S_I5-J-={3o`4_XJ8}uj;WH{wf116_~5{yph*z#%Bad6Yr`9JSzGwi6KbW?P$E3x@;Pa0ZH{faCyzgTn z?Eip&K?wmd`kWF}ttPya`T6<&09gV^f?$Ow(vZ)SW2${@`&ws%?##AXFG>9fB`Zr-fR2A`$m8#94`B^#J zx0l3gV)&w50Fpvr&xXchUKH=F`&o%)+FKt>9=qboUB<<$_&RDJ))4X8D=W!Zj`V znf`pinV(q#fc6)9q|qHyQuzT5l67X+{TC`(vHu0V`(?hy zOZ?Wsz7P}=XJ!!xRuaHW03HxP7p9k%)Y=3-lhWE~R^=6c2rc^O?uRf(tHR$l5GFo!Q%*9Qq>Gu_NNVsz zuT`H!tmKYIH>o=`kHSuh>RUxHr}CZQy^!GfZr!?$9TY>~q%! z&{Wjg{67FXFT0DB9KrKSCM1@mC8Wdgn7Kmd&CfF%NeG~4GG+1`2kYnn&K(3d@++zU zx0Ea&-87S$G5<}8CFM(K9vhDWHZ3K4+0urx|Mi6(ANJ&|qibWw!Njk&Mx@yfV^^Bh z8dadDhJ96$BDk3PT%HR7v9hh;Sguv%F91jHlby?ra1}yqtb$!mZ835C&Q&55f=X#R zvI4}eQkh3`wGoZU&wt(6u>%mG9Ilz()7(IpAZH)L&CM-N5fA7@lf@3p zBwJ1PRs;-26H*t&6l$S?WM-l%*|*&Aj2<6MD{YyNnn=U^cOMaTbRE)59bN_N{yP-`*rup3XG15$~8w&ZXj&XPT5lYV-cf zEv##7`n=6*m!%}g0l{#e^lIn*5EH8@P6_cq8}CucLwwzh`z5Wzc0Yz(iFA$uWm02@YQ#N(Ui; ztSp9a|NRi?Uz^!&_J&H)f=qXKd02uYz0dZ-{c*3nJg8juH4sIAQsTeliNO!sS_S}D zo+J25DoP8CAxzp40D+Lgqz4GVN2$@Nq@m*~N%|4rIvM8HYG9iDD9aHzh`6%xjGQe|!bMwKOGtr!fxGUmc$SOR^^SayLsv%} zX{JnzyWH;%DFc_NqYwr1baQM8x_r2K1WIWG=A&o0p!L%ZJF^PUG;v^13C@lLca)@# zF_j)5d_2THj%Z)4sC6AY@HSYnO2dQ3eb|%{ABZCLkahZ9`I@H5~{-4nd%_YMo=kWPhZ=4Y5p}Qm79nP$uiR+Sc zx3;{)ZFt!J9sApE41xM01-qOWboh&Rujkcxg4=H1AIx#W{8Xh)7Ld8WISbSM4bH2& z&t|<4w8HEc@+L7a(T6JBW-P`CkpA30l^X1nDW{6O{N`y1kY8U)RMs|aYRW2-fbkbW zfS3CPL>;tQ0(2Y_zKZT}`b8(C%b#es%LruVKAf^*cVA+DTP_zh`3?Rzcr@|zM49*J z?qg3wRw?dRsy~~3f^}1FwAtWOe|lHw=*qrOxUt8F-}*Sdi^H^@qtMzaMv8y+YyTT$`Fxl@B%x3|k5vsJUU z5aHz-$1`rRHbA%L?+(t0jG$K&4pZcXjS;*<;qefJ2cnC#rbYYB`pxYi<%g>uFV1j+ zc?WV)d`G+tJm>mR@?Q%$ma z*Y-@GJxC_UK<^ZbmM{(bCNUEjQ}w=a z?pjp0%RWE2n{cULYs2!#i{g1Qu9D&n`F1v&!3NhW&iH_$+j8b;y;S9~o_(%9qO0<|wUE9fLbM5b9NJs!c4e&Us`nWj`b0wYwT9~-nMG@`RoOQZN6G*TTAbhL|?Zye=|@s<6aOLdh>b2)pd9N z0)ERxX4REM{MR%vi=zp_F>?YJO`1X&5Zj8{+R}izPh@0dOOkRvNl_oMEz4?BM|av3D1=-7^8! z&oBHwZ+C?1ay3P5vvT~9aFp-P0yCLC`3ZfCDT&U!;B1HZCK+bY;n~e=GjMC>{_^hi zVM4a{YjID7$vy1F?oM<@ZvzdK4))~QE1$L7wUb8swJ8a=HH6Lk))3c43Tyy`$J;tE zwEd#qnH$*viNh`>aH6esJ3X74{Q09$x|QsER`%NgM3&7&?v5hY^=kzTq}34crRetZ%0{5SvpQq!wH5zq zK5@|&ykc!h!ePd210Ejec&Oo)a(?9%E4(;Z-F*E*y9zIuU`JnUyD@NRHy&{Ldnm{K zWIDt9gstgyS1(JWK9OKvitm|#|L$rw1v4Pe8k-h;>)`D)sn`*_pd_u;zxoG^b4awT z^bS?`p6m&{mASQ<>`K80*|FT2%SO!`(OI`g16m;1vK`a@MPYWqnSGeaAb)BWDkb1f zrvk7?pe@Fgl*AnC25u@oK0e?f7~m1}Z>)ZyQsd=@WR)e2njLTPj!G@fZ#UMSY$;Zw zZDrxtDo567UIMWYo@Ao2jNjnk-u?pJlN3*z8g*$x91)6`i;9aD^fU7rl2*P$uxSCHhi?lVYpV-4o2758|w*(9YO?=l> zI|6jR6zdzTF!jmt>AxF(1-p#XlqUUy!*ODL3vkVPeX2FWuondePjKV8I^0>pjiolO zB&}(qLC>EU)judJmGh=n1wS6=BSYj!L9QX`xIT?#SP_h-v2IARwfnt%tI%WpjZ1l8 z40ulIRK07xJx}!0-*i#>$kE#vag7ooa@D0wdk;H3Wa8}bk_OlhxG`ld2 zFW6Nc(~J+6I`%xxMD+z2QtVRcqrJcCOEsUi7zGC3uzr*Mh>A?N-oD=Cl{kHhzPXc- zS6Bu2LTWBTpumaPWpHHaIv`F$qEPrrGu&|sv>v{($$Dn|#GfOoKqtk3t1UIa2VTtN zN!EUSz>DU8VE%(ZzxNc6Eqt$j?qz_PP7|eF^up}zzKNTHIO(vy$JuJ7dUG$}8IKJS zf>Qsjw$;V=s0tU6F73q|>-Fq=7TLt0BbH5$;LPXUyAGM=>F$64z`Lv`ZLw>p)g|&! z_fWUjvN>@`M|&`B=Ru4tfl}Kw5GIQ^xjvs(t;vxnwui2M%t2T9rAYT`%&9#51+(X} zt)}c02P&}ROB&oz2;SD7l_MzMOpf*N$Dg;kQZ{mFmTJEKiErS9(=?=ml?R!x6%Ljf zcVv}!CUzqOHxezG9goSnb}x5}ay)P5f6~x;kKIFkx~%Q?0+Stf_E4UrWv|lD)nEu+?p9Qj7XkHmg`@%EXI`63 z^~gk(aVeytiJIPuGO_^So&^+h{SjR7AJP$SeQSK$^Oo4v>K9y?w?~$!3V9w+BG!f& zMS;2T)H~QCjfu{0>w|aSr1rOHH-*p#45l857~YiN?JUp%T;IX_;U+S0T|<09;r02aVKrz%m}{fj)N z5JC`GgAz6(i=?i4l%q=FUnkNTIqd5h8x(E#76+Bmy^=`0yM#Z;`p^`8m8q&j(K*(b z-|^OYY6|{qprvuTIqNtKY5DT1LL^|${bBtS-2c;gpIhv@KH8>m_r~IJz5|+Ms4vy4)KlwI!yVKauRNwfOOX9NjgQnAaY8m8N<1QaP*Dkf*xyX5t3~X9hmQdCo4n zz~KZ%H~d;_DPc69rr5yd&|C8T)bmxb+uv8pr_ZrGYo z0YHe2fRFcS4$&J2@MS{Qp7}mxRDGEr3h8|XIn%ONXYqJbFQ5Pp^O#Z!8PFb9M*WlXepom~pgHhs zv356%CPuny)fY`W8M0k$+s7RdIt_cYL(d&gBx6X~*K+jch97aM!GwSGf*FjDL0$pj zn;K%y`vR`{A6(DqUe@~xkl%2UGhNWa51MLjD`YQYpO6?|t-hAd+|03bwK0JP<1Ld>{#=zi)?!n866izw-=zU{A*#>{#F5tF=ePjIQp;?J3~@&TUo`r z^7Usa_y&g^#%^v}tj2aj3yXozA32+gu=AXYLmhJ{$BaK5Nqd;|=f+_0A6=;wcOg8ozfa@TjX)e`QjljlLln`7*v zQH-R0+Kod!nV@qDr3y9VlU7E$Dnpqz4RBRXfUXZVTq4DimD9Cip-wXJTr&{r&F4zX zqpnI(f^3wJ9%&>!$s?L@BBhwa>6Z>qR;e^RU8wAHO$bEs9bM$vf^moGwa6q1LC_fj zKvlSF3lzwc9numi+-S_2zA?uDWe3Z>L8--sgi|m(Iw`o(wkrL6>%i zTSEue{!~^gQf1k$?;9&Z4}yvA2W%TpMaWD>6Nr|h{3G#Z=?2e*!B7j8y4#an&hah#UgaC_FhM7elX4QL8Vklr_y-b+7`Riec%tQKl`p@ z_ov7uvP5FGA3b%%zk+nad2FD9#}EB+E==3L!fVCdZ5ON1N)+TL|VHXY%MD9*5jOK?w)o(eYYGTZrYcr)JW6djSI>s;aJ zye=|JILas!t?ViKIL-iSLE80KgR?QiVuo{KjXSrf_h#s#aWy+1KG5jTz);NW%^PSP zkL1%9#v1__-QJ7UT;r4Cr;}chfF_*@6V;BodOlJDI7qCld;i6mIlk46{l!{gWBLh1 zG282oX!~PS6DGz_t={naYFCeLoOk)gFge=L{7PQQ z(*87@44m_>9j9ZG>O;Z>WwL610 z_Ny|WD+HmzC?A;*<1qrO$}ksyq0q#1dy_YLI#m;k;3#p4{d%Irp-Q;!B312X!;U*%@{=9rZpqK_A8gMMX`Q=zddpv*PX+dT`%ATMTccUq zH*(IVFB~R1d2~clQH5MNd$2=#KjY_g&P-Me7VPJ%UQItGqUQBRd_&dCAs0}yL5@lc zv%7q$+9@)~+RYUJ6Z^Dn!&!rF&OG+AJ(}PSQU(z;Y@@<^YGbfAGLG~;+kGH+yQz)^3PtZYj`2E_>#m^8 zXUvFJ2iJ2;U&?T#Ky+zC<}`fsaO2y0zeikqE%Ke}<=;TnDHg$xg-R_rQdUMzvP3&j z98FL2g^2b1L;K;;Uy__3vooRNJo3y@4mY+Ny`D1+)aE&j$Fn8^d=-YbOvZ2gSuMtB zI9L*qyB^q*q}Y53wC_ldG{sxL^`fT;z$SGzp&InovLR3TySRKD5$|I=7$^EAQ#t#G zw%RC^=$_~g_cn_DZ}hBJ@N)YN(@0%?*UhHGIhLJ)E=xU)#n?)Fxu!+AX6NTn=8vC@ zV~x8F1aTsISW=k#AdQ0t;InX54DeU|>Bh;}21SjhA;;AaOa<9aiel8r3mE3RjE(lO zVrv;)xJ=$d#!o6v6w2F`=TnvMyt62|xpzbzH@d#r3`V~2TzCv$?f4cPYwsvQB7t6f z_I~Q<3hI^qrR_NK;3sbO4&Xb#u-{%wIax(s&zo@xNWRmX(=*(1*MEhwqLprWLG3!> z*%(glI1{+?61d)coH7Ar1rv8V)ms3p)sFR70DNBc4~WawtY0qBVu&xIZq$fmlZN&C zFl5)V#AYaoYS$=g;f7irt-yJD^|$L>}M2IKHn zJ1K5v+#;W4%#izRzeaqdDs}(OW0rnKCg}pM=n3h4B;$;VVnGzC0_hhaG(r|7br$E} zHbXyBc^eBNIfS{nKOip5S1^nGqF;EPQwslHG0e#y`Q3kkCgBuK63(u8E&BVOaeDbz zVxLzDSc*WDhe9h{@hE(7?8Uo}cN~1Ph8mdoac1W%EApcHF?&OEH@G9@{29XvMhD!< zyA5f~7@f53|MT)#z!P0}`xT1V`!V~9uQB>kF%tguHJE7YP#JnNDkKHKSG@duzIiCV zS9)X|v-Z+`D>L;|}ld-qhIH{WJd&r`9-HooBPQQWlbcbbiwTWIG~DE_BF za7j9q`HR42S8^(uE+%(Dn*V!f@zY~qCohLr0u8;?f)(bFCJQ8#*mNP(TsS5WKlku zR#IcYHB`}~P0ocX{%o%eDW5MWpK_<-s(}c%5SB7mJH5pfV}6mO_D(6@qcr3Vgr!L^ zRdgKcPvzd)E`lWB$;%Uq2%C+VlM(%G^IRagTgVrYTpU*w_ssQd9U5`ATWkPr3XL%` zFovKMl`9}kp`$0emZMj$W9BY6RJrjg zw8nb=Kyq!2pUsTFD#Gs&fbtLzgsa8{yXPCE?4@S%yI*~ugx|gq($!-wXb2f}Mr=P` z+uRB_VbqkeuR;0IMnLyEi1_fZ>qvUxodXLT%9D*VTac}Z&`FzE0$z4vta*=FrUX5&b+ zW2X7i!iRDz+h=d+sk^Y=@p8F#H`DWB)jRBBS^Ed-<^uS1QsQD)v+Dq4d;9>V2Dxry zOloUFW3;ZJ>Igib>?bdAO;^7{dY@ikJYf^R;dNq@wGEr7fs|U z_TkcCQ+GF6;1Sh(yI9~y7+Ld8r02TDruWN73FWO=5ZKFeeG&{u{Ju=Q{|!P(Q15`M zz7;z)fSq%+9enkT^)YbszzaMSOnH{lNzmwTQ*bG%Ds9AdEe#Y+1()9MHy2*Nv!84c z3{$!9k&rsyfY_G4qrceZk$I&9J!g9d$!^JcUxG|F6_IPfn3l9W8ry$aiT54^rYN8s z^@yY2&w$k=a^Qf zB3Nd(uyIx)0$oEh8p&m%!gvaHFUS>)D#~)CqfxoE3?!&fc-$fy69#`SNo7&b_AT*` z860u5*GXhUsP8;mhRZ?0DyMpDi4ADuMrIdnPb~({;y8AD_$}&l&m#z=4aNTbp)? zVo%ZF%V^h!aP7a?{~1NnH0C_I0S@-%9=6w2Qe~g!W|NbE=9)Hn0LIR_GjQ4+xd~f2 zJF1juY8?(`X4nd~7+CQl)lxwH7WcBV=5*5<+Wju9#YNm@H5W-00!9p%k3ciN4b zPMNsvP2i^9GG!8Nw zNIoHO>E_pC(wIXHIY{`Ku@rqLZT*2K!+QDi(JVUkh`<+qqE~NFBR??@g`R1_t-&wj zBzFF%BQ5dyRz$40U*i$Im<^5lNqlqXSI10n>Awef^37wV&oaJM}Y1bu}$N zBp#@PA)vkujww-b60X%mEu=}h2TG+BUl5WRyhtGnh*0{Z___qt?#cn?yG4Rg3}35d z_sq0ZSw9o&yI&G`RpOkI4VY$qCF-A`N%DiC8c;QBX~!whc1Dn`-Qr@C_WgT=0Lc!E zDF#apeP2pT$D@dl!5a(Pb91%BZ^T26eW(7}UP-!l^i_SyOI%2TlpiLQ`1*eNGnvc9 zMqjM~^2j}g&|3WYsUS=HE+y9sq(JL@IdF*o1#wHbrL`Ev4H>N6MV#EVTc=JmVEc+8 zf)FgMwK#w%+3>7AGq+R*c(a$0B7`INnGLw;&zo=q+Nsl_Sde}^5t3`jH;!Q_ zMK_*&X^HZ0O49C>jj_|?a*g3!5)-EOq{ua;IE#_s2`KX4rRuPZeUr&13LB=coutd@ zgd4QGRJ_!dPF80r`q{@9z`WI}?QVv(cp4k+@o6L`?AtG2?Etf6k!Hb@gpz5pi~dKM zAY+4A&-W>ni;tvDn6P9Q8Hd6n!G*xMjooBxHRS)ilP3*urM4y%lH_3_J$C~3(8kCp z(7bVUW(Mu$;bQ2@Gp|2R<7dQV;{aw|lNg6)9tzG!S^2x6f@N-2V+euq*#{Rn@g(XL zqV-q3l_+7;m%o2~u>3x*U;8(7!lgF!iBq$f1#8#`miI7Q%8`|ixh_+aDSnA$P!wN@ zSRm2b7G3k>w~mI%I$l$A40f(3&4m zS$vL>v+omiDrijV`6bw>A!1aX@;zJrgz4xZ<#DIx~9woRVL)gIdO*z>R;!8SA#Bm}h1q zGBGCLjI8X$n0GCG^Kk3tH1u%}MuKMAl-GPXvXLhyb+;d{Oq$&j&++lIx$s}Myx$Sf zgx2u?4FCc`31E><0O?L@Eh01$x@{YB@uJo49LeW#FIPG_rPVQB(=1vd){-U5pFiTB zLnz&Y%REj@VgQ3770m{UU!s+ds%EyTn)ySVe|+}EfLm$DjjTB&o_>1dZ=W444pciSmN?4CyBoIAwRai&ZWYxJ0`>XvO_bw1TBzA))VY=aL&) z7|R!N=yVnNQ6SplW^U*wd6DecNK64Wg<@_To!-Y}4d$`^pNE{cTc=I#*q^V;-1Ib6 zxQJKafbaW93$$SWcPy}_8kk%I0#qi$_1wL^_vKFC7DEIO=8YpCpPXdnQ-=AL)7dd< zIg%g4jPWsYAua7&9`<>$zDjAEmqzQC0ECoC8FbDXCC`sT(^NS`+C476sOyt4i$&Sn zelVOc*-FmshO5DM+7jED3CvGJ^>zA<$)Anab@SkS}6!%Im^>+PVn+oSqs z7o+@M2&?0P{bErO2cMJ3CB&s|$gTa0;5;vNUF>UCF-ONZXLGWhHF2!M-Z8V1(T9^O z5r+MD9*tD7;UbF!$7tUwlp>#jzgvv0Xxc@CnHhoR9^_rMeyYjoh5nKT6oire@${LI7Wj1!R|KLJ#r#bd;T{+Db?X z3;M`;ObrVQa*2pW=WG%_*yhm#7;KQWY& zMITAH*nc@c;s&*LjvsFN?Q+iL{fx+95m^;7q3!>k`Qy9wToE@nvS3QpHqVq3vTf^g zA;H-zFolUGuh;Z&Xc$LP{5sTKin7F>;8z49E~LOr&#`TwFyV~$h&P;jmCm%$8G zH7vO1q!FVXK@lBO%4ZK+$Ya3_GeoY?lKMT}>{Z#%eD!lLD(85Hn!b5fTll^nCWeH9BCjY=@| z+JX)_<9klv>PGM&i~UsOYUts*pvvtMdgs%+l>t+bP3l_Ow*IdVcQ_!aAPFa<1;rA> zEG6~pY4#wR$!@>HecTelh*0X(vHnuMEz{m^CP{@dx9oRYK<%l8){0;3#F3jo;QcRF zb?l>%RaH^~N0m^>ne{4kT>03~mZ&~oVX-PzQR@b%Wi9H~D_4hZ(!>OzHLh>Lq4-0q zl|qA&MtUi`Q``9&{BC}OqbzsFsXU;(|CAinYcv6$3>!cwJ5g=f!V;|KEb)=H{=8XH zSB?G#Vul1qFZF+={%o7T)0srcM`5S1sH)NOOYNzaiY8wX*lMdGZT?olt7Knpkl@$u zbD^g#{Kkx#Noli+;28M)NU)H#YyU{01vNX(RkgSAgNOIWX6Y6eoJDS0lf)W#U_l^$ zH8axYXF>Khag^$5%jV@+3j_Ked$@ZK2GEEd1rrh4Ih;Bs&XfXv>`~M^AtHYx3p08ICR0Kr? zq(qRVyBnn0rKB6AySq$68kPnD>0G*7fu*Hc8l_VN>CpG0pYQMW*}r@qc5lp_IdksZ zJD~K&mu14JjPz(l00~8F}(L^%|)+;EWk?zUuwbVy3)B~-hTEuCu!8(4l?B( z2g2tLt-&&AZqRg!M|AYU5IfvwULnnkmNXZXt7dQ0HiaWDJNP3lj4rHu;&i^Y zrN;M~Vc2y1YVjRs6E{@4uai zjvRa%-()}W@t`y1>54n!W^5RErqQE$7a?lEr_5nhF{({Fu)DZrK-I|t!}ETfy7TK8 z_g4V84(4$U9j;;6J-Dgwd-Q5-=I1yW@P=ZxtOB=V-^lo_5*qba@OUhdO;Vq4hKGk)p3 z;9tz!I-Ic7yr%bEKY6B9QdOHJ`YkHNlWqJWpsD7CLQ`Awl%4kgWeuKhIvL)qUe(_e zMQ^%oxCvhKpDE2+drE7x#rUHgL&Qzh5{g`w97OIYMAy;m3aO zkVdq8YDu8%MiF(V5-xgV0O#wdXEDx4MxuN4L5A^LKbbFXY9IFDxRD^4X-mpYogycW zo~vbQ_lYW)FvnPiXrvdl!tv5_AL}Hv2w+V|E1ckqQYZ^izrLLlWG`=n zuAfPZeX#|PYe37q*s96zyVdVD_em12z?!lXVL8ONBRMmC!gxW(AL{28?1Ox%-o;J0mkmuL!N(-i}XhT6n0z*L0pKQ@!B6f(%V>iBSEpWV7q{ zas1u#u&(=8hT`AAYSO4geBv&O>l&vKky z>{za+6T=C=+KA2m={XNY+lMorC6*k#-1fZDLxbxAdoOd$UYtzL zubUY!VcRuTHg^BPvelr1-+d&Y!aWi{9zH9RSkWT(HGfvusqT_IpRHUaBq+|%2p7W%bK@3?&fzku4xyi8}cZl z!SXi>6Bay<8xp5%AZ<2Eh%&q{Yg?^prjW?9C%!czj5%w;;>BXxCL!7=1uJvrIk^+L z{37bBTBRaUSd_s7x|+!V=h(Cnl2U^7PZ1^?6F0n^c0ILwDKH6iKP$MAqHhe15Zg`@ zW>~DCN%%EXO)JoPl}ZIiujzBGur4+?Ih{@9!n;l{HoP#gw>UKk3#yer*#{H0#MTO7 zOfxqu(CDf=RT6Thu~w96ZwRP|=~YvA`mYBP#+;xQ3HG;U3nJ$(!2_^(WkL32a->ZR zw9O5pHotytpG@kGA9azHWe8(Gcxry>E7seVexY?aocLAYcRsRZzeeJN*3M3Y>*Xlh z75~iM^JoL#e9{U~Eymm=ij3S?q0aVV zMMfImZW3>etWT)ko!D6P4Yh2w60=CogKdH_EQ~gpoGorHS!d7%@_vgTT-@IaA%8|k zhyD;}@@?|PB&So+$|w)w=<{B>b&R53xXJqpb|sb>rV;F8d^(2tFQ)3}8M*i@Z+gev zws5XQ;(WBMwgnEDM<>n0L+ad&K3sIH%vmG+K3#mQ zfq^(fPrj|!HipGmy|lMqh;&OLq2=S5krK0~FL$6Dzj=yAFp1_`atz}K3Z(gjB1n%Y zqQ#mp1b4E}oC-y0-N7%|uXkl!!feiB4oH zw-C{v({A0LGDmB@V*9f(aI()OwttWON^7x;Y+B!d;YJ8v+4x(Gi?hv)-wbcIjqdYl z+tR+r+%fy`Mm6&9#kfxYYuPfOex!u}F5|55HHx^&yhO`P%P&-Ov*t9tqtxuJ`r{G! zh??}x+~g286V6pwkJXHV^!i4_FwXs0WpvW^CCR<6Zh?3qPV4z10^`w_ulGnaJF@i| zM7!EkC2ll$1p{y%r9U%uuJgU}CiejU2u<}Qvnn)YrkX#o_mfw5;G?@k(wV%VRmd(F0Y z`u6qbq2F`1r^$QTe*`99hKu`09^pwet7GRHYCGB?rMG)sY>@+0piEO@E*t3d5+5FW zmIWX*F};)|+M{hoGc%I2;#gHGG5nNAT#ng2^k0O|cPP75Tz1jtCX*ULoxb~>^$)|< zYP9nqfCa^ z5R)0B4mG~PE&tm%?%i8GgN(IqQL%~~Rh)V3oP-#?yi{)%n9T<@`LDVj_eJG*$f=xN z+^TN(9DN<(&_N|*8`r^5?jifNE?OUaKMXuzsC_?ZuMY&?>w^r?38K@!29QltK1S%S zPdWcx&Pi$%>|1^MGo_pI?Nhx6N6{m8&9AO121VQX{Pvno4dDmg92f|C(_J?l(Rx^Q(UxFv;mf&&V1@s?H_9!tO$?|*Q6L&m~2uuGMVGE>N~ z_(3l%LMhjzFzrqB9*qxW;e>_p7pSo-;uk^akyG=(xqKQV^#_E39-cg6ePE3t+W}vr z*JV2BWi%e8Oe|5A(`?Z8x>sCIA>~jKV}dU+nLL@_&R9K(mh6omc-W3mPVFqr-BX|D zJH1s{;ijHk%d$*o%e>hFVoH**{p$yjgQnDy{Eeuo>0t&((Z4xF}1rT#}-r4 zD1VUPONH)QQO5~;>=<8{&(CSj@a9&yew-w7RPZKCM}%8U%Ka1ETGM|=9b2-mJUtOwJxds+ZrLDzW-Xt!!t^1 z5{Ww`FCTehY@X0ef-eL%TGh5Ui4}7!&5auqzf8d>QGosQ-b}-LoR8RNW)Z5*sG2XS zbQt>aCN_RS$@>hbR}uHl@`tynjXH8Z4{Sgqb};l=n^(`@OhwrArt9VJDSd%q;>-zB zw~6YkP4JOgXVzd%GCgC_?`;WD7>Dv=;rBmJoEG|RdmbaLl2sP!<{mRHpG^9Z7S1fW zg)>JwJwl3+CQfJIqEw0L`+V*mcm#5Ge$57Ez04R;Z}69x7e5<3q8t@Aq7q12@5~qN z(!hQBnekoWdvc_x(Zl3C20=yi)BM-yVGvCw3WwC+REw?5W5@3Njcx;1%zoZtb zoj7?XO6xxBwAINN*BGmA`W4?%yOu*{lL3U^arRaG9a+2A_hM96{u?7cojf?+8QA*- zOwl^(4`tK68VZEtx`!PBUMDXJk$H6(i1C{u6K}6@?eFPiB&4WUh^3$-+202w|}_U zAY*|}fy}K1+j#D!`%8w%uQA7y1>QrRzZ=%X6rw%oaYZ{Y*TqpQNe_&aYp!5HsPab9 z;GDfverR|&wW3~Ee`Nz<-mRD=hw_IK4)=$FPsGpEzFoi*?d7y$ch27eL_E*nLF$tme|?X4+f5KK*W@PFxbTwTT7$U}MwN<4FLi=kU2YKw>&henZj#|50=4C_>MztKShACPb?MJC zN?Zzm68P6e&YVWxRftkTDAOVJ_!y~;53aat@KD7c`$f8j3$u7xu```wDPAKfjQ73? zgr+IGQr>EVh-0c^tfx3ldM8zYqM4X?j#C1s%&%0xrFbS;tWmyle3)x%g&9(TqZY*- zj}6;Dv!|Hi5qllD+?_EG-o{)qkFkMKb?#RBW5AjW70vnPd-m6!oNwALA+*tPacx>OtZ4Q zez5x8{L80qlT*RaqW2lxcin?C^^BGGmfc)~iG9E$`lPpmy9IQ9Gse9$nSt^d=8B3+ zQ~SM1SCg$WWSPDomMr^vcB!YmP)%{>7Vp&JZkxml8ODAdqfC~A8g;d)ZqfNd*;(`~ z^L9QNHd&lN!&;T;IznM=PaM_H7g#cMfb?d1fra2xmz(LmjGi2#GZFF8eMjSYD1Id~d-DC?f$>i3HDO%*R` zhiS4q*#`77O1=)YtL}?DB^c6Z^%#9cIEB6<4s?r;-@#zEq^LI*IX)4UVb#f9AHN%` z!Mc+EEVTro2ow{-geqB%nRgZB4u2@x8qfKaY%$~gxq>a_-B1Z>x0#)7KB!8*<>9nW zHMo-3$Bld#lfU`{$|oF=dblCrKIfJ!7vGzswV87t#bnpTiZ}Za#K9ltu|p;6*DBP0 zsNL*y#E~X4mMHqz=VIbWny=MutOfDiTB4beHPF$@Z4!mGch^rYbxwjCwRfm$6Pp;~#$QE#xKSA4JSw_;%T4A{p23GT=P!)5 z3x;2;B3{(->s^}o$;K+{uBeAqXwfL7tZ2lBeNR=gDD#BFg|kC|X&o zA^P<_jIvi(>qFs6CU4XAEE&9BjyAeqHL9>0(vTr5H=bZoCUTcgs%G~3GX9AOYof@` z?mJb9)kQ@^6SU(wo_Az)m_~f8PuZ0kv4bx;NjGDzwp8T~}POXg0g+)zPFU3WMs? zlSxy(o&-$s3qPF19D~v0xed8XHfF+sENl+{BAAY3v&9OzSp!E0Y-d-{0Jc^&hA zwE#(=94T3~h1_%-JiFKad1N{s*}V zHq#FZaL4=3uDMOi5a-Xx(Ibx+8~%v5H8_`ebfjY=m|4}z;WSe-^HjBO2j7NYuUyqb z_<}vMI}lIL6iD5^$AiuXo#6V*pfWyP@2`rW$ylM>&+k0AJxuTSDqh!)9eKe%-M^+(W)Q6KNoeGSTX-Hiq{ z^#SvxYPsZMZCnHFffUS?X@M^}U3Q1Clxd94SMhdpomJSf9m7Zt^o1Jh<8%luVZ_mh zH3Z8cZS@;{3T%V8G&u4xwqt|n9gl4L;v1Fwj3RAH=;Z3RyBm!1pC6Wb2N&QBMa!qS z%`+C(KnquOzsuiU!ZIAs5p3?E$CRI{E@d*+Z$Ordh{TcUtqj^s9jwSaSQSA zeb2YY9mpw66SAp-AZ8-@f&V+S4VdEmcV_fKDZh$EtStaf&k?MZ?ER;!rmqiFejg_1 zd;gs3P%k=y9Z?_qNGHT^b_7nT@;wa;P5lCg*y`Bwp66ke@k%U~abutA+6}N)?1oes+>kr58jVV6UEk=B zzB43TQY%C#jZE+;YUg9~HP zIr=X&&liquI$eUoGNEI_*c4Hk}cn zEXZ%e$zw)qh{2rrMRuQDk(jSHSCLd9VJKYLx%DR$qb{g6;ru7TnOXcgqY-%6N1yX9 z^>#9G^+tk;^duJtg!DRy-qr|H>4K}zYF1dxQl&7c)m&gE`X0= zpphpT!a+m2AUbCk13%%C@_8&12vtgd=@k*vH*&ItW0K*;-JU1fpnBEC8&luia~Uns z%>7@z>tC+FCzK1LEk zk^G<W!4!txfUJB=y-Hakc(SP{~6k}SrCzOG)mkU8O!t|`Y@t&??z~vsIikY!w1ycR-O{o$~ zD)TRx#mB!%Z8E{s-q*y|B2hO{oKB3y=GGUOM2CiXz8TNNniZJ5KIBz}>HEE7=RE9v zn`&uwQ-(Lp_I9;u;s|{fd)4cECvWPLd4J`Jvn5Tt{gKEjtmTp zE!;xSPjM3ODZhNP`JDkLAze?Bq{sRf*q}_NKN5*&MP5?REIp90?`nUR@V3Nc`_Tr`o&Mp<1FjX_pAj8+&urtkY)b z+iAko7kYRIi`gh^NG-og@OVvtby%dBtQk&neeW=`eCH|jF;5RlQ7LipZS9LE>Bbzc zNs*;l5udgD+ks|p?BkaLWVNv5Y>-%#Zx2lb=WlM1+A0xZs0Ziw(kd!RT?iO?xsBEm zTAkjOGmtl74*M!<;ggZW-Yg}g)~VQdnD)$YE;BX=WPfk_Oonl?lAt_?&mso*=C%e| z9Udh=RsEo~xMtI9+D(x)wrLkO#6{IvEct&^ioQ-MsSG1`3E;G^LgJ0X$pQ#u41$u$ zA^zBhJu!76qeltI3V@B-FQ8bJ%RHl$G^xCsTU$Bvj98|0_FBk_@hq-eu-MgacS`C^ zT8PMvdH8Wq<*%K{*=(G6Pi|r4!)Xn3c?BhtxOJQ!JS51M8;dL45YGKBz-qDh;W4se zWrtdkMT@H}6aR`w{12zi9PRQ%eaa;4$YKd%dmbr6BZpUVHO;;jWT3-MlsA#eKR}nDx^nZ@Cq(KP| zv_Ub9byEPMwm!=E0xBPmPmMNqXqH0XSk=k;B!@VbQ24 z!XCYD?rSgn)NQMO|LNxtUz~Zy^}K~HJ=v!whGCn#VUbzG;QXyI?XO*8rnk{udk87gHwQY79FK=mnIN&HTv%ntj;Hu-cZs1 zX0_EBvR>PTIhW3;Mtn5V+Ra|kxq#Eujo#pp-%wt&Q(%EINWtK#oFW=uMz+opN;jc# z`n}}OcoHZbM%{~`s4Rk8DzOe02lb+&9cKfkI4z9ClGaWhs)We`Zz>YfI+VX{ai*)S zs;)&$jepCdE_KH^cMf}Gdpn6CSb-){$Jt}KTZ_^$TD34f z2ZW8BS_h~{>1;=M-U+9IQE5bz7DRj<&g(0Bjc1D+Jp1~&-KS-BIs6W<@i(6om>4Qv zUow!Mq491DF+`M=mw1d+3FIxYmVW)N-QPnz?iJXz_U7#Co7K$sEQC24+XhS5Niby+ zo@Xr3IqQ7NBLG4AU;mpAZ2thDcGRQZOXx7ao&a}R0KWfZu#!)j!=_5&{^BlPF=yZ* z=hcqnKvm)mX*U~oB5-7+u$FL`h9#LVA*y^{ls3Z?=0x)t#P`sL4&>-uo3ExQ786gl^xCNr(h$aI99^q;lR0uwdo7~YzRBif zpF1?;G}`B0ac{E!C2}5N&v;!RqWZx%Y=DKKvjz4);4=Ix=}6eo%~HkL`S|XiKUT?! zvyXZwmb15dCyUj<5-``0*toZ>FUSqD2zPSL`!E0O%zHb1Jo8^X8yXuLqkEHo0}m=* z$pH-}A5?fxZ4G65w<(#xGMih;m#{~zQ|1RPM8ZwVY(4vrOLyI$b-PSQK0-J0!f5tU zE;*f%Q9baIqlX!i;t4#|(q<2MxXAB^tOXX2+&=eBwx(2QMD2j4_t%7rf%F#Uyh5`^>9REgFI#t_ez%6eD+yd#8x217 znSuiS2Bd^Wk)Ues^nK{qaZ+e1E={+#P^HmBshb_eRV>MaUwGcQJy_7^=L#oN(op8-;Tathv>UJ4ybv^N&QiS$h(sGsrma*} zrqL*RrSq0BJh3)NG!@O9jfoYUzighXFUuNDamjVtbw_q77Z)(l=wfzJ+cuh}X;!g5 zVR@~Iel#RM0-K&d$Gh>Fomt152P090+c|zl2mZazB05Ss6$c21c#(#q(8*efo&+Wy z{lZp4ISV`MiLjzASV(#^!YlgtJbL$vQv&;C{k*@UkgOg)vC}OC1OjPiHLLpd-Tc1a z4am7hnIw#fNb3uORUtk)Wx3F7mXvE*(7qNrHj&4sqp9NgjE-B&SI74F5u6^GotlA0 zuIRbX3Qs#Ipp>`ryy3yNTC3-+r8Enry(lu$SwDjWp2GMkYXMy4;C?nZ0uHyO+2>W{ zU5GY$#EH{uXIJq3{Gd-uqwwOOkFQ!KHYHNsbC$|%ES_A;hJTlx(?l*9TxoUEZ{Vzs6pPU61T7%|b zYPt+Qj9tw3)hO6!zNLMOsV$9M$t)Hz1`>@JfNx)+KD zr)Ieg+VVOg6008y720sKEGC)^zH^XPvVpgb9@#d=`zaYB3n%l6gM2mk(j-IU7o!*H z87$ykDJ!Rlb@@oT2ED|nNcq(~H-AaEBE`t}oNU#>i9E7IODRJ6t_O!l9UimYmB)Ek z_dcD>yv7I>d8%^%+X$ZAO)!O~=frm*#LyOO8tQ%Lvv@Mj{0FwYHU$fA>bBmTxU$k{ zH6+DI5J`*%>$ZiPYK4i;$cMuqcX)47;oa;k3+BvX-Dv0hPrd3=j>O7Lt3<0BsYR)^ z&23>-KF)p+rX z%C)2vHye1j{*yFPv`<6nYDyeCw1bBrgxE!2xpgsaU1ppe%C%IW%2jP41&t@}*)F;T8+BcsYWUv!B~^=WmplQeFg*|LG*GE$d5OHd$)Wvr+;cT#{E(rR)9S#MmxkMnO2){?pM^ zMmE8**roSe<<4YI#uog;k5>yoSB*NhA##CC%Y2)ZpOZ&gOtny>V1)Sul0)Zc)`mEk z2Bt_&HMAJF$+PMkWU@d0yG3b30`KlOm2xl|{%0)$O;auyVE)sU8(x zoLR504ChXW=iUS2xoGoFWjss&>u=%ulv(rD5Q8;b5xU^Jmv5lORTM!D-VaW zKS@u63!ysRG#jn}jRkALijK0bVI&F8(6F(SNN4M6Q3NYHHXR3*Zb@M-%gi%z5jH9{ z{xQ29P)jLyhg7Lyj^>LT!+!^$c#Y65D*@rCAXNvIx`^cz z_C8%#J81p=_Wt&Ry5}|%P`VUDD^ci@0TOG^B=x0O|j=oLuVzQG8q7%%* znTC!2XNNLU>*6!|KbBg0hrBfp(=_H7`s-$FWl~z>+`{yh zwPy{wwa3RRq!dZX(3xvRk^D?P?n@snQfPAL_~MjXB*zBbCk8&)Oa?@18II^)Ru{!2 z(drl43BT;{D>R<@K8MZ|pn`?^5^B_y{xa)1@fR=@;y)7Hj`K1rd2sjJCrm1{pC`dD z@Ha&M=gSZA$>RnshIZxYNh0V784R@Cm&#GNTCFj0t3*&+{nou=8*S4~xDqf}&}#JK8EP?eG7b`PPnxPEsw({%nL zPFazjCtLS?7U_P8e%aMZS0nfG0%9+p;6#|`G9b%PBV9|4hLgcN)y8{`qV-ueO>D9n zEmyv3p&Z`bPwRr+^`i;5eCS5EasPR3`{#T2hb^ks&1R3&MLi~z`;PpQ9TahM)k7#$ z#u@q-v8hG&9LAH}JNWc3~jvrc##D)%H1d-MJ1USY5*iU!V!>BD5X0~uS#U=9j>z1o8Dz$1LF?#QFt5c4tqu-Zhrq@>QM9CSBhjeM-K24ngY;o*hW zjRy)wiry~5${hJ62~W{U{&FEwOEDR)Q)Zm5-*vNZ#Xy2!coJB2`gri07u5e}+0S2Z z;LyA$n#-2UAA{2=)`TbND;J%?=9~1**efSy8y8OeY36Vz2C9D;=x}ZleA!lYplWUf zJ~zGZpP!PP&~}vL&V=;oT=1a?#O?dKUeTO_17o4i({xjY=Ei&Gv!rrJL88;oUla|3P@LqMVUUesoYZ%Vr$G z39q?8strjYvV2s_ju~lEaoW>%@#(KO`JJ}?0bx%!1|lON+LqJRbq^s$e|LxDKO}^> zd4GJtd51JCRT0U{EKph&6cg>I>~5-X)Kk9|6M{so%m6{a@@7NR0;FV&4*+MNIGb)km_vk(H#p3$SkI=2Oudkn9Z=&@(z` z*-6b#KjKv7td(u?glC~KJ80Sg2FI5C5}+5i!t}iwqxWcv0j4^bg8h`yPBA6+yBvC6 zu3q}=pabHlfCwmDtFRqwf2rTW_GsgFoW;jLz*Jw|@ zC!;C$1nKmCeY2 za*G*wz4XK&>C@Yq+bQ5|lyC+b3+!&#$>kL!L&(Re|-mQZg zwQfBz)yyY#EJzYYN=6hHIr7lNI*9YQ7R@Kj`JBf2)(Lof*7P*#wiX5nWF~6;*pSH< zsXX#fvynj&I@)h(oD}@9OA(q=VZixy(CpL-%bICKUxg`SgR}pLzv|#hN#APa=;japDqUpIwsEa4(x6T0YHgvSBrNPP z|C{@esJFGypH^1?5*~1$*=$zmh!w;vmWH|9(l^Cw#S^N}Vxpk}k<|TLqKE3NiSGR= z2oesnhKFgKOrcFQ6AF5*Y!fBb&w>c{YwVA0Mp=5IYk8oyfPs8PMZPzT{SlP%(E7^( zkIIm(fQtmvNFAe5#&D4sbuxCxm@h`JOg3oVN)Xkp!J4?*=s^79{=2W=__hDwK*~L5 zUh9Qb?jA<&g+Lld{w0+m2~Y^c9t`bCDxy$*t96@&Lcm2zRj8nOqPj+%3>|?e>#s4L z`S8`AiJ$hpN{QaGuG&}BdpemTsFILNPU-XOet*@ib2LBLCAvk%=s>=D z(MsMO165R142*hc=ZYtzn~yBgi^yXVrwsx`ZA zUhE$+u+5{4{#Og|4Kgg`DcS4#ebF3Z|KIpO(cFbRulxFfEPuv$d@)9^qK7(PoCLY|hl!0$=7_dh~sWy!)71jFt`{DUx-_Ik|)YJrBRz6?{zH^wXuh`vB zOi1`rw&~WmZP{hA-#W{~%bS;+OafkZN0yZZqi@^Fp0}2PVS*jBsA{m^?fFDyO&r7+ zANn<2gp60v*Vq>iMurbGq@UguzxH_9`~E3q!BX=0kT<)$d`y6q8BnF?uyf1jkEJ9h zt5j%@&&;HNpGS3f$1Biwbn}DJL_QTO9UN4&cW?k}UZBtF85ntTk8*6Y+HVov*%^RB zq2l1VD;J=}!Ava()=WJ_qL#3XPl99!_dCtpruyYFufyYmj z0y53m6F~E9O$0X|AKl^!O-+YP2m2Qyv-R^B@t?%>@o?Y;e2xoo(1~sL!GVE|&X7l; z-OG(0ySl^((x8XD)bH)@yG7bXNIk1$mM?2PFS46mh)OJz2T72Eg;K6+3?)7!NU9KY z^;6Gc35k*Z%MOIIj!8;+Z{NKEiRR|w$_5X0_EpXVr@o_Pqlogv^gl5abzTZ!?pKB4 zkBp8Ya5{#`9Gf#|>m226>TbPfcggRbnwq-9c6(VLfF_-CdYt{N3%P9iIzz@#|1aUL zWoMpLMH>8&(r%oaeP3g$#7#%J6%`c}02>uCaQS_&H$^8afM|lv)uq*HzCi=MA;Hb1jA>cA-(2cE^9Vbn|RVK*T%G6Xc zP{MfD9z=gA3_P+S=yzHU1zW4yx)*FDGIXyE>*U^K-p7=bE!}&STyJ*fc*nTSXKPI= zl5A>Hz~()|D)lwL(AGR=g1jWSl|ctsK{X8r{y*E7CHU9{2jR)tQMp*j>N~nV6WOPTqwaQQd&d+rv=xCR6_ibky7Hpfw7OU z($WIxY(&+-XNX>Syu#ffc=03u!amqs2N?TUXJ=7jM|LZnf%?c!&B1$+Rqk> zuTrBMzjpz+TGJ~B;FEa2UM23;;ewx3N+*62>GtmHV% zH`dtuYCpw?lqUiK0?V|`+wt?KEULe>{Dv+9Fk<(O;mq!1imqCw+KOX1Y<#2< zB2Q<1_LggOSpa?(8I}bbn*lmiHI3uQB`Vr#dhcK$nF~aKL^h)Y^Lde>KMOKYQ=J4gnxPFF*gpk1TaC zZ|kTrWiU?{2`!a@K)0swzkwn;FuxUOZ_9$_VQ6LPF&smGa`rd6nJ zLe7tcjSCTcwCOM|CU;#_b-dn3utWbg{Vb&QuDZbCtUaiFt4KUvQ>f9X!?&1-v=1qU4 z3{e)e`-B}Y2!qG%6&2S$YMlaz6^NrVQ}n`<$2TEA%m1NJG?az;<2n~T)=g@?>%$?D z@o7X3!wm?rM?73Q`2Lg8G$BuJQBjH@1ma7A89VloxL^ky?f%q9sA!^S5{=de)m9nQH9F~q}n zXFYB}ezO0~i-kW!&u{((lyYS&*iA@~3Z9_o)!|r5TH3_HfeXLGk+`^-Q`?(megkT~ z_p|8jBaBm)AJ{TT!xkS6ww6N#FE6j#(Ut~J>fr5#z0LfLNnmg0#6bsVmyw^DAdr^0 zA+|zwMuynregr zjoxYAyBfEt>Hb3_7}sxZ(Be~5Oz3^E(#tm=2^b6@K$|D8|C|8RJ}NB-hV*J=`dW+j z2@#>WcE6`1!2NK@#<<;o%n5XvRFv|X;=$pw?|Udqnob(iB-(BxsWvZur$)N@YjbgN z0UBw57CBpq1^nSbvr+NHXviZvZVrx2IVf=9vpggm0`UPy3977~Dz)~%KsB84e~Gj4 zp2fU)7BjrF zv*Yi$?45p{CD_#JvocXc0fBxLWM+pg|Lm1BSpc}67LacIUKOHK10Q})zh1iXp}(8d z3kaj=NyAXDyFo)K6NuY7leLrwuWVuPU5}h~?^#mB=Of?D37g&X+C8ssp~C^Lp^a(F zj0w+Z zf5*=c`D|0u5XvzVVoc*0nVi^?sAiyU6zGTA5o3%9|8kv{ufkB>&> z*{rRtVG}k--(PX;vVMpoD5VDcqT^*k5OBlj)nD=;-RQFza^_??2ATxe6{qKacSRhE zA7icq(zQxvCc8TxK{1ZJjT0ySJfsn&8JK>HixtLapWZmz+bjO(H&B>t6a{W-YSV~K zje7CK0{4fV<`#1u5XV4T1Tf?U{m%Ir1hOHU_Ll^e{UbrQ1Fq44m>^6VtX3_hT45Y8 zJcY42kJ;OSv44T}2|9Xi&VL+aXf!rG$eFC}~fSBU?2WL03u6N%JV14N}BQViqg}zGWT-CzT1he&m z$+RzBz&k+3w0x9czI)pIKsTEsu?ad`dz~ky3A%5+eY@t+UAdq<90@XIFsl(OZaWJQ zu9*Ky!tw|ZmL31Ig93F%Aoa8?;6NZ0{UCQz7{ft_fP5dgBj9Gn#HXK7rr9HR(8fE5 z<%2!#n`8MkwY5dd-BH(>jF9NpWvm#)3V)>EVxo1*_coBcHk1~JgoMcYC_o_RI_tz* z5-{QW{}|t!zsOJta`lJ^+_Tm)KvKMfOXZ-?#<)A&X%xriYSYauUyy{W9r=rqPm}s( zDA3l4tiPX$0b%T8Mh5!u@UZq`Pa2P;oLN4>JTO~VWM~W+zo`f<1oECT!w4azhadG1 z==fiPb$lcK_?GkM51M1#gi+n*I-2|__xu`aA%Vy$q%=q~L5g@{M|Q#p%q*x>oNN5V zAH@4Wu|)ZFK!*@u)i0wlsh0ZsLUTN(hy!9Ow3+qwIPV-V_hUOgo}2t7?4ABtR1;6z zBPi)D$OA2^3a{M+ad4Y z;{Tfd=M|8e0nz|jq;(Hux*Cjx;ItP~3#J1M*=&|K!*T!l|9%2Ua#%XUnkDFMT_dAC z8$qmyo=-d^mtnxN>d`he9(J5c3JPxiZLO`WFhB_0h;4oxokyPlE|CJ5D@anys+i@$ z1c2XvcOSG+E8YHa_6tD19AHU+phHIzI2Tnv8_V($Z^z<>ac= ziBsE@(+W zMG%oL>68ZP2BkZsrMp8=8j%udB&EAsO1e8P-Q95E^3R3W@B6>AT+5|v?m07i@7eJ@ z`<&si0G%VMw0@O9XJ@a8Q9-J@iNDmfra2S*4(|Tff5~T40T#RdB?1CZ1b|hvEm`qu zJfIa|W)nV9&ma1SiwncuvX<{V=5rSeaRPqNIx};#d}Z$MM9hh@W|o$+k&%%RWaADk z^Y(^@88lQDZ{xKVfD@~txij6_y8@5kaeD(MTs7f9HQL&{HK6GK4Xo{#fw4Zbs(-O^ zZZ?20*f262e{(W1!GXt6QNuX6zlUegaGFcQ3}8rnL8%+*L@$oVglPSY1+jYQbg{uJ;5XaMhddVT?P{1e-3jl?Dmy#-ic8YR zVpUw}x3`cQNLfG$rUgf)oOTlmbQ%lvJHCAtFfgW?yr|o-cbz^(+DjH#V#Y;z1x|#G zm|D^YC3=fk|IwcTrYcS9=;a#Ai6}I4OG_+Kke)yCA!3e<9R@|BPbcObY;k;BJqZ*k zV7ye-(ozD6zk(d^r~~1+14zOR)P6`0kDQ-ZX^xLhLtu^`3#dBpS@A}rX;kb%ek77M zXuZ>}+ml;n$MMpA59-PBkp2;|Z@d0l)dZ2Vy%xK<$ippV%iGGU+a|$6Ae{`96IQQe zP=ULa1%w;)*_Vj4yOd%5K0M@JZ-e-pC7F>LqwOj|DVs3R8A(~I9yIz%3GOmAJKEci z!Dle!z+M7~791%~0}eR*HT}cSdPM|=#*f~-L8JFgez5i7A^#p4uJ6{97LcI;_!vMb zp`!ZJCS&iaHOE3=dF_}f@;2pVhi~_GBJtI4NK+dU~_c;ZpdD-I59` z&;+cYBN=EHWYQ-m4ghuedyB|`e89S1V#5-Z`4PFVH}in`+ZH1Enpa#8Ax?80@c8`% z%uhKNo3BGQs4cZXyIv9l1)+&=1Aj?U!FDS-jyHyZTRHZy!ub*jN?0^<{bJP#BUUt_9}Z_2W3s^4&BJTM@VtCwDY)y9b$t64 z9E;QDu+oLCRqMDD?r97Perr_?Tl@c>2G0RfR4T#@XXDROL88A24#wrOTMd~z{87oX zU-4}TBvZhai?CllqDNjZUfJUVNKAa0sQ~=r=mgBz`53}3*J&GKv?ZE=-S9obr zR#8DA0lX_Zx}U8r1_A=|mL-+Tw~A$qJ7Dq>4PJ8zaQr4Z)Gn+)jU(Ft6|L)Ptnu?e zVbGC*zFe)MtV|{WP6jOj8KzU6jIogK*+&F~r^>gja4rAY77Kh<(Cm6X12 z26wfHsO}Z}^L;&db?FDrLkK4Y+t4GgLt9(>$C?Z9U1Ok*!4=&VUHd$sxl6Lwfq;Nt zMD%5w$K|na=-pz4;I{=>0lWjx9Pe1;Z$(K-iN1;R@%msvA$K+S#IW<^nFla={?k(= zWIlj|eF4zRF@i+^aRR>Fs{VjL6Tod*a(x(+nAy@!T@2d0GZju4KOPdvd3hlXVWuYOJX zhy+a6-G~5#6|51#n@<#=cKbxJJqe(~0Er#mJ!J3OfoY|5)kW5PR|MCAcS5miM$XgR zl`mv}fG~jUQ=C_ksC-8x*UzPg4?Xn^zLh~BkR-lF80^2z#|h+sV48J8%=sZh<&OEk z9fmR$aUI5XI|IZfFov;4m9T{0!7rjt>^@k<6jTz2Jn(FdPw2Ka3m2ZY>mHkfg`PD+ z?ENCJNEboyLk3I#)*j&h|KXPF zOM+%tPCDMhif4F=!~fR4C0xZM_?R-K&U&85x3ds=`A0`zaY z8%7P1J|HS@5dTxW9KIPw!|aFjz?r#NnHRkps>n!5}G*)x#rPOKv1@w<;P3Fp&wb zX2b+|!v=hfNa4-~a3e+gaVlYggM50;4FbnoH-L-E_s4-_(ScN&ya8$NeQmdQ`5oxY z&qx!;l?IeL5_SF{3ncP6d6Cg`SEl_G zFZ2jxhP)mO=wnseCyv`+Km`kQOa^~Q3(C}Y;9kfBMaf*LIZS@`_MJIPfu*)QmNh|@ zW!(JQ7a4y^7ZP>ndxU&}@7b2MUafkL&$D?Q$K{<~;3vYLcLGe!vGCS+z?yuT`;-m% z_xSJHwfx{%YZd>Pe=E@=yceu+(~#b|?g`eSY~G3Sv=VW=3k`na#z1)kq8Nb!LLhl= z(%mwrp{GrN9O*JoGyqz0aciQ#WP1szjAm6JXVR$+Nqq`IL7a$2-wnPhYxKDV85%}vJ zday@)i##0#o+&q&}8`_RD8qI`aW9xyl|Si zqi2d}DI(wlcN+>TI|?4|bV54~Rv%ViPXd4X4&t)Vf1^>zjyU1;H8j;y)2n_w9#Qx2 z143I4YZP5vOyQ3sz|CEG5r94M0x7&%{yz7E$Yz-3KoGt?u)xbMZpcYS{TvXs-$NkP zmH;BGw7k*}OzU(2Img)Ue*t7-SvnJNVtyjl-!m1dpN@WhIj{Es znHmMIhNlAe;pwB>+>tg&ZlWdIenIi&{`WVaR{CEeYW;))(c?3Gvo4I7u$GmSm=kt)K^W$>Z?{pHxubT(pR;4+d^*nGchv;3m=so1imr$F`~tk* zW1KJt=K;A{YV6~>L$tYFJ*iGZ?{tj$P73QCKWP9G!E3nN@y)Jh{e0bYwpaUW`En$V=B(yJ3#V&gDn*#dz8*rgsl~1cNQh63YIFM50b?DS_~zokZEmoE)>pD8 zmjhz)`fYpk-J=A+qjP;1(a}EvFMhe`jq1&gQ^v+6M+F9iQBLsx7;%EOH^XY+WW(MC zl>Z4>D}t_Yv?TeYV-0+tAb?EZwEy!91a!)n+7XA3U?V!SHZOzsB4`fv9KPA?to-}Y z7Xa?;6E#n*LTZ|r31i~(MNTT|GDZx7o z6f|PEoC%UFron#_eiI~Q{m;qNyviYXE?NP9mw}Z25&6sMhanX}49I&3sNVi9=YS}4 zWQdH7w3znbPjz7TSwfiHuC`9!^1MqYLO9eTnoLmf!b1!nh(42nKZ15b=Lf4Iv75@^NrTZ@4sh0dQkD?O4cuzlmVeTu2hm zc>+2W-F|Q4EvUXliuTNJ6O@o`K0828o*|!>&{eOQiGNG>%>A|%VYyym%f5NJ`fTYF zoz{SHjE1e!fRxu|PR^dYwtnw)*Q_aIZTaXWxw_QlIm1<|=h>o6v_=HUGp>V*v&~ws zeeRRpmJgj;gI~k0<6v9H+GAUllAk1kdh4XT*0Q{WbsQ$fr-B>Pbgm~TPgKUc4$~C4 zj&`j^&OG;DMU`{2jQ`++9zhDNtg7V?eWW>dv26O!KRYI`8kQ8 zjk~MnN(Ywjd9oDjTE8x7B*|h7$s&aD!zz+w#chMTuE%wT#rO{1)GJ$qSG)yv4o@Iy zdpAFv)(1@T)=>~ZvXAXPQDWg{!0(LM;se-po^QIJ?wJ7xEGohsg5L{l0jW@=mWk_T zBCK-liNw(WXRn-OMDAA1?(d8rmY%C3*cG1Ijps2n>XGWXIwL23WQhaZNhOBDv=?Sh zifojuoNV;NG+XUbgyRfXe<&TgqetrU+xXAR8k}>}f1s&lj^v3)E@4LVK^acc2w^fF zm+ETDu9EusJ_T1RV#i+Bk&xj~x0n!fmmpE{(cN{_o0a4Zg@AoNfQ?eMyQS3w)}p-cQh_;a!wNPdcEazh6#bSuNAsqN@7T`kcX@8|V05q5NU?Rgnd08nfmc4Ehnu{1 z?i)x|_RZCnSLPE5IrlS$b>qeh6t%B7$96aS$y;sX{Mr06b$eBZ$C_{-eKnZnk%z(r-sgm7@v*Q5OMuYme&W!nYsMv4UFMf zi16~+?8TDYdN2gmxSrox4$l?p+bH=);MqoQf4Twg&}1QuI`RCx!#>SbBt}`i_7NFr zT*8elFLrf~O0ZX+*VTQ7%?-HgyO^FUbixnKbvZh!S(O@!^SsI8`wiKvGux@KRN<{> zt6s}c0t=u2g@jDVx&P?6Q8)c#wjx;oQV|0@M7B`f`ulAm!W553WKU#xpcz~lF%IPp zYRTuJTj~)Rm7b$@vPtTLGlg2iL#U0v!ZffYe$3gzB*;fbq9Nzx9>=8*-dxLibXPL0 zt1dNer4e2PTaRpApEpqUTj}s!vCkELO-pQ)m$obACX0>8`&oU0kmP>jiSD^q_#&KG z!cvOPYOPkaqwxd(!4b=4Jp@+8cku_?%*!i<;Z2{RFWPw7Qu(GojbB zpE(FflE&laheuv(Z;<<5({V&jR}2HlysXOk*LXaV?q4|S>nCDN6LD7NTaNxY-9VN% z>+90cXgpi};YFMQUshqCEEN}wzTS9u{A5c?XGBDTxo^EY2`}`S$WHd=B0Y&0KiU+3^huuw2{{HA=2P@vJXP0H#RY2~J?b5BA|?3jGb_@wy8UL)eU$FA8)f`xcH zCY^ilo9oLPs3_FYxIw;OMwG10!5#MV_>eC8W|?ryzmP9G_HT_gq}NV_<+5haHHPn^ zJNd&2>WwV_rN@ozpowt)4@%w>{w*Dk_4+v1VPq!O(;24?jpQpckK>b9mpeB{zL&EB z%Pu>~yceIXhTSED2s8#_dSO32p^hKCj>8({yuw3r2eBP%)M05}sb2fxb@Gucaf~!I zrEetkxolY9n61_|teJU=5ne7i)WbCR^9ir$6g~_qbw_{mT4UJ2X*_A=yGDnHULJh) zh<7(2DMz^Wfm2EWTxVIa^h4axGF?5zj=Ad$E)t2Ljq0;rvp?U-6j=6Lllv@ZH#^Vj zv=qWmch(vbE zVA^xv_$1N`N^^3&@Fg9Dj%2QEO8IHZR``lR31E*j@ane1N(F!g?w%5NZwd0^B;bbk z?lN+rfuK1&m&Ml+Fvh1F74*s~&R6R^n16KKtA#yV+iiF<3*4qDDOZX8L?xJE{Yu4M zslQ@@1~Oc`wwrO2To3(bl{0t40ppSwmQYZCY*-fcL3EfT!1u8aM?p%}d&J81YfhUb zQB4l(lyL`aY@FRET@Eyjnrf|q$7`@q%smxzv%4KH=rA7=%%(2?soS& zJI_4$k_%hHGlOWuDOR-zho1f>kr%!fvsW(}SxZ8K_1m4YfBk2TUJulB7jd>BoSIBT z7+hqyza{0e{QN&>%1RMe&9 z(`_yajhF1X$}>X)%Ba*8bI3%VhWLnMqBuS#7ZFa+z97^G9&BEL=*{i)%E5j9!X%`n zb4*@G7ZS+}`y>g1pPEuPESY=EmEk|WTXedtBKaG6_6C;nol{&}JG;L<36L2fEKF18 z+q)KX;+rB|CYb8|w-)h4AR`0Fsd;zKzP7MB<<|(X1l(;$?x$j0)zzHQ$!tTmRNu`J z=v7|Q$Dm4=R{Mj=KmlzbC}=(*C=~BTh1|MAeLrPpCAF>&fB2a7C?GCb4_7rGe3r}{ z*ALOVcdSImID9poS|Xg+)I%IIoYw_{euNEIgYUj0QSlZ_#sdW+7jMa+;E4(c8Z(%7 zTY=UCXP&roY)sQYFp*ie^nq+Uk{7w<)&6$xB&8%Ls`ZgMInCbC&U}3-oGg<_L*ceX zN=EkO?cktrWgrC!GZy2g`cTt2^Xa&bzSrDT3{HBR>1kXj#git}Z+|!*>;OWymlxlZ zEGOdmq>Mm%z~2VWMA?$k80x6!nBmu}+^eUand_|+Kcqh|BU)E?O;oUj<>is5pKZx$ z);h~H1O}A7BM9@~nXZT^Uq=7)hjjh>Kyh1=uKccTv0aCwa)GKWq|APCaY_$q(0I6O z#mnZe-b;tMIaZmV;NXex@j}{jFIub{DoQ4{dR_#;%1vx$@UI>`l(J zn6jQ^tA+#wq#GCV<*65k$m7Vli@~3EtGx5Oypb=;De-cbr`B64pFW zAU8Z=96X!(^!TO6xQM7|_)so7%@4y;IxEzJ*I0O7DMPft0`tT7ddzEi{bpwHnOWA@ z&%O%_tE1h#m*4UX3=p?a#D1CJlVyzz)bj{bkKrO`6N3T2@RnRGY-vs3KvG!8y8CdjB zv&wK$QiR7;W^}ew^|q*xzrotRHDx>?yJ&%QiXjo<&3+uo%bXtIAU;L>{F(kt)hCLY z?3MRaSx^8Ha&X(wM~!l29BB*n+#~eAo1|bfOc#tIoPx7PhU7^|({eN`DvA1Tn02rX zi}tTI_Ia~5`$uoivo2wQAt9-Gd1Zm3)V(4~5m?)0eCYw=Ek}h>NUVdjBqT~4VBc+u zVx`~iJMrMs%6BDsc_9M>19*j{s@glEeNZ>7(Rw6mb#vNN&-jj##*mL zA8)I*T*f-&<*?HDunx0kXU`4t7qRAsk~?pbPU?G&mYuoxS4zIu_`qpO{oNcJD6Lr- z><01S4^+V8@Xa|)?HCv{j50ZYN1gMubkO?|QF+DgsiEba*#lH{5&Jy9e=(-5`Z@Am z#24hAyanUY&Cxmz{fFRH68K-a%7q0swKe-zN;Q?x{jcD)g|R9bDVPm5!$S9p&QX{f zq`x+E{Eyf5sBX@SXpdp_h@prJ!M`CP&CzDUprg~21Uv=BL`DKLe5|)pnL9;)UzvoF zhOGDAD&)k9fT8Al79pO9kHeW#frFLZb!hT;XK3&Y7tvx9L6-UJ(oEjgLLtJ%?4-db z34L)(nG_^}kxwS9$x_jbo&}$$4vgjvHa2b7);ADcYoU-3x z#<@@-!X};I$Fg0lHshtboP@qM*}2G0U^MlLzo=qB6zVIx8Z=$7tP`lmhF;h(4NX!S zq~@!GswYh_ev4($c~_F(00mw=gT~-MfBf*>>aH)DnIf|5F}F{+afp$L?J}ZbXLQkG zD6+G2Hz$wBdlJmCDXR|*N$#-oc~0$16Z*!r?LYdVmX5}hJD6v`i$WY z9gGmwCeiKo`U{UP+E1P%Vp?32Hx+FyA`)cCG=Y8gc!!Jf|!> z|2GDkqmG&jr@%ym@@eme{m04<)-y)o&9+9op$+>_p?H|#MvHFfjvPbOa%dnp&adSL zDH=zNku%=!e9@;+Cg9DP~~j* z*a)Wj>pmlX7^QUDs^y*UGoBo`+W&K0dCrNxK83(7DV$Eu%=>)LUht~1YPE~hRP0Ud zp>pKK29(F)D0!LJmeAP#$-qcwk>D?Z!eL8Y%}SFlThICEEF)_(39LO{cUqelsLFE0 zddSHw`8f8(sn*y!JU6s*a|Q!Pvl3___atG97iN}gzUxdgSIdd^>?#Kv&wz9tC=%!D zz8w!t@vreruN^df@xQ3w@PTQw{?9Af z%c=28?r$xTE#6pmq3+!u{T2Tc;r1ca!Zby2&=wQYoL4}7c z>YDrFwOy7?7|YH%9gown5Up%=oGCk6_HgUErDcUXTd1jyDD!4T?c~YTY|l~S6jsNF zvxe^XQdG+Kb*Ydmwff|q;S@7wh3GH+DK3*p4bwk6qS3x&b)tK;P=#!@`16!4syixg zzCKn8h4&M+lQz#6-1|@E=yf|U3ipTPa1N4}mU5K6#+=-m%1=EA++B%h+hrF{4bcyq z_)jJ&Nk{~!N?7U@6likJk*!T`9GA5YbQ;c$UQ_CfJkvPfcZXhf*YTe*RMdbE67oh3 zE-~l|OFn-%KedlNoP7O7B$xfe`=_n=f6CEyL|%D26xxnX7P=Brt~^d^tR(c}v3=IR zYGmEf5;Iho`Alr{kBJaCNqHD@23@!g?7Z|dd%<8>x~01Khqm=ewNJdiqUF0d3u4G@ zamWgH*GpV%AG4)@`x*>-%gvbY2(c)8){X)j4~(@rHzdkE`o5MdtyNw$6yc1| zzD-KwQYYs9%ALT^vRcf3v9U?Yne9PGSm&lDRHE!q2QZ)7dfp#8U(+jXF&y)%V^O`d zB&OzF+`UGKwxeGqw;9fbHq+`~Z~9lA*4`XS%sKY#HzYXkKTpDSecY+sG}Du%lh~c# z{i3wQ8ixYoB}<4GA#Wm7!OgjrE~?k~&|l@?(r+b?feu-4W8?y#BAGGN%gIFHL%Hf7 z{_ZU6lbH?o;(7$qndVMkg0)65v@D@E9m3LWIqhpp5_H~OgA5#y{SJ6d|$b6*|iy8x=0$l~sr6;`HUaPSZH+p*8gPPVY&RjLhN26O-HhWsN zRbHDe#}x&O`C`T;zq@fQtBi>bwhe~+IHvp%4CkN$FJ0*>lOh71_zp}wRh`siLagTZ ztr$8Op^SBidyr;Me&eCnPlskkRjV(8E}~JXAbIc2&L?NiF1w5K;arW=b6FHazj^2vPb0lSU0Iil2Bk3st~eiuQ#ze3Ff732~*wmX>P z+$ZoV*Nx1u7OiHQZW)0rp~r$9EiLgu$iDNoXz`f<9y8=1>U+#J{~WaXXJ5yYEVFfj zSMSRf*!f)d81m}-4BsQ~8Aq{E#P(GPdfqsqT-<+3*-m9NYn4kC$Q@PYy!RFgOJI4^ z$#9R<&k?ghj*X_+1Hbiv&dZW&DgAT-itp+%=9}jUkM?Xel^utLNyi0j)zNz|8oI}d zitD)6ty9hF5+z_Y@tm8p#!Q#83!`aJ{n^X6PF~!8#I<}z2fv>Mu%oJ0oFXfWK)TG* z{IJ$0o5F%E7n;NR(>?s9ElqKP)Ed6In3X)@U4Fo~JNmNJgsHH<-j@avN_Jh+@zUU^ z$fLI$4Z82)isqWYSzr61$Pw>Z1OL~f)RNd?{&0+b{j+SBCdehvT3!X3@H z?ADV%hnm$DGbau9ho&!Sk73a`S&}7Fxwd`X@tncnPTYQ9Wr%D@an8`G1lgfUdj4YN zI|)f`ECG)td%+=zqor!I54z4Ox)W>5LrT2@&q$|I!M{}(M(u=np@_$F7IH8)h4X!1 z!>g@0Pi!YFp+X__m6R^_^N&_UFJFko6m)u;QHe2KRm8lUdnd$j>4KF;*_Za{kwpDZ zV|vapznrwrAu$IoHM2p@c_VAVGEg^y_IY+9-|PM=l5<^n^y3A&26Q*>8WQfaOlKJ<`pavYisRbvKSRTq3@xa#k}sHU zBuv*Tjd{$OWKy1JEe%VcLp-W)6qu>i%Xa4oGa-f)etJqolaI3{&J#P1_}}b7ZGN0Yk4PY1LD+fn%nxG?r#0H@GU$e0D3vME5;9jh#xNuA56^&K4?r@xE`tQuSI#kFfBy3>~mH~es%S7|BVUd|9Bll-zZ+hAGy&67y zfl;Q`Jc`1)ki`@J;#gMMory6w{^(cEIecMwPNkpqM&93;#;MZIZOIIe;6>y|<&b4? zUgQwp0WJ2YNY;Lrba+oN`+ru4&f=HT}!IMs;>=U!JJxHWJ zbNX5Pi}P*n&`EP6Ufw`-KCW=?EKTuCf}M@HAc;=Mp3QW$TqMgjK_F>km>yB2Q>*_pRH~ztH#7uxXk1H@g<5Hxi!dnmF`ffps!Y0J$u4VH!m;98 zuVH1)2|cRng1yAnqgS5fZfm;|g(acEo%BT!QE}c*d;@|8i^K``owGbHkj~b^8=o*3 z^}QC+FHa;B$XQpSVkvKoVRkJFSBW+_H?vZ>C=`9K-mXkBs~hjtu~FntFa zbEOs(sANB9;oS{_LJ(r))F z*D3MSMhiyC&!ax}y6QSDYv`uFCoSCwee0wm6DP{-tFPCw+E^rdmMOQrn>=^QA;|k; zs&Y?`R?WT6Q=`r9==$e`;Gg26;5vB~kfpAv9`WT*5GtKURzrRjBO&VmOjZA=!fT56 zLtdyEmRP+&CmL@2;wU}8+9Z#(Pf5>Eh)-0q)mqurB;-k<3x0xe&YQvd0Y zAEQ?laH;hgEYvDLb=d87cbpcH)?!KM&~6A6nmRVxu71krvWd9sw&U6-ZF6{i{4y_; zKk$`bt6p;(zm;YiIg2Fj(ae$NRBgjFP3m*Z?69IJg_n_BVK3$QB%udc%qbQi9jxn` z(VFF43sEHTUc|F{DJdp~6GGUqn%M8kNHVG0aZ2?NO=AAcl>NZm@}U!t`}r1 zkh)^{JqTW+ay_8!=R2hMu=X@Ht@?8hdy;YNU_8xmYVrI>3yWksqc<8i7x4+9W}Vvm zFZVp^0)|=&S7U>rBfn4u70S#{lpncLYgAnNdt>it(7?8H(U43aN0fmZi_Kp5k?kOomxsZrfT)@kdxze0*!! zPu?)zhL3|S=E8@|HXAZ3mO`Pj}2|llL<{a?6c{?e14JnuDY%%eNIGFdxfz`x*)*0bu%Rabk&rV zBc2`nfZUp-oyy%BM=nh(Mnd;H~Q`jJIn)r2MdP0==L`}01OX^~`%Ruh%}e9s|9v6|xQ zcSo!P(!7HON7xQ1Ud*~~ND%Nz{A#RwI6r;zPyZXetf4i&5peS?B z9(awq85WxxWX5F9g?vqwMsnGa7BmeX`=7|25tZylt@ekCwu?j9PJ3A6cgxOBB}*a$7-gAsYOV=6gXPsC5!X zzP(zLTly1rNL)tQL2$)v_1fm%fg+tR5DZ&P)X@g075wj9(-qlT_l zIl?S_R3?kCwbgV~&bQ1+wDqypn>fN(jDeJ^Iz8Phx=*B5>cZ1Fink1TxR=rCp%&EMdK++weprFUOnhoHa zA#<+K*HdMtEd^S&9MC))GU(TEgz)eW2FBMC>WwfcniKyG#9TCyIj*FH*-A=Y2Ujg$ z602^D-}OG132ap0Rq&Ys`LjK{eEH<9vsyh|T)zjoqds)3QJkgxAD08OSSP50f*)US z9#K9-e?6yrrl!;K?NdXCrK!7d$7YWz=X=N6*R{Nvw)dtsddA881H@@ob%;S4)7!rw zet*EJQTgNCnH~E2DV_ThI9q0FAwBHdu~cvv85Q-n%0_>p!GjyT22cFvjZK&Z_4vm; zk!Hy)Y&8Peo2sSuLh>+?x1LSSElC;yHDA>p|csDZEc2OR$d^GFw>7rXgyuLdWuLj z1kw*8^0h53zkv*L4$c$=jI=w2{5s7COKz!*@=-67^my}~=iQ575o`niSCOdB^*;$F z|J{;O{~!VZyx(6qjvezH6YFDcdFiJBx+w4|0xV3-h01(H?9T}u$GW%=K?MJOj$TJ6 zwN)c)C^e5t$V4+$e5qa;N8#5tZP3C<^QuC5t`Eu493o(%>iq1R^1X1c==6qkNsH&g zfSU6-7nA@PASHib04pYhsWyki&d;6pb?i8J71b zCcw!~Q3b%Mx(s--y>XsH;?P5nEC^VzN5zxtbEP<7G#LqV(3fwCr<~1z%lO>6bWmhC zEb4jff_VQaXPpSLBr!2DnZr)|;dX|wsOV6GP)7>XoLMGveZ8hoN=06?6)cKTCi46g zrD}EsIzw6ScCyexxLPuw<&w@d6c`$Orp*6ww*V_IzlZs+qSxHRQE&v&utCSE#Z`1U zQsuOaWFemnk0gpPm@GKWcgl|Ih%K8KFsCwbz-mceDV>$S0 zlLtdnfjbqB3kFBZPv9+1K&9*aOJAg_>(?)x92gm{eOlY7W&z9C4O2W1Aa#gSZf)@ zKlvtfo$&)E|CVk9B2CC4yYD|$0=$LU;&ooP3eckf9TO8u?QFprCkoa7%syMPW{%dgP6;z7o6%qha5g&aozk?=1h6~@?Aq<}- z|1A_%N#bpOYioK?X~q$o1lUi4qwlfag9p<^+@Sb#O)A`r{jZ(E^leUrxn@Hr%Kzn{ zh3^Fc37U4gQ@+l*E-OR&H_1sLVw>&BA4;DKL4U^_T#7&bg4pvO_YIxjzgsGUTXNhg z%HZa9#2?E_vuh30@PG0vm5*NjtN>S(?K~(X#Z!*xJ9I%TDHH#n=VVLT-kejAy}Mne zkTq8#h&CMy;-pMW5^%8~K~Mtbo>3krN~lx(v(fDYSxQGYX*={Zk)m5QMn|T@l`V{? z?dbReEC(23B!jx{}V0}(t`-LBXklw&@>Q`DDsu>88>yx(08w-b`* ze@yO_`zBySZ4mrh29Ddak^}pzos|XCdwnDI-t_$4P<^->dx&ArM6u@8jIAW1b@4%b zwkC%ot$uq__iMt!U+J`AZH)yThPxk4SE(Q0#) zRhNS+d1ItEc1k@C90^#*HPv@TxLU9R;A+tay8*BW?Qm;M9Gz*wG_@uUy}_`oFqT~_ z*a)V&SCin*U70kMXx-Agj)J30OPd01WDa1%^CE-xx0c4F4^AE67F4x%&dBV%baY?K zocMLSMuA=&$%=|GVbE+qMnSP)ziMV74meJPsKi92m2_gva6iy-@MYGLlG`o20a<+D z)AChM=+T+V&%+5GM}(cQhVa@xiL_9}pCSg0XLxo$1VUV>UdE)qzwC(Txn8}eykhyQ z>47fkTSLY)27>;-QB-xYhMdzcJX*W0Jo@_0P#Ghc%#S(GcmCtLJlTWAS z+Y{~}rq4xRJkj_w+1na>4V!~3szOX^2s|I(IBj&Mi)wgdDc2nk$+_42(0#Qe`toyG z2-gL6!*rQ~o9EeYvcx@Zcgl@`k^o!RPnh+5lH8Nf>x1=b`tr%r7$T;3{MB~<&x^5} zznTrv@_E8e{BbmvH~de7=?x|mVw_zZDZJKdoAD&vyKFSsH+U|;uiD=_ZL@PB4DJ3U4a zWcQ-jx%UM^o}S*%jErs76JCZ{712Km<4lZr35_yqWBv`sl)AxzU#4Bcuv_jE~ntUuJapy_UjC5fJaQ zLy+1+_nginFq_3I6(c}WoUFY*vF_e0Ot)E(+kz`!u1H1VveNxXS?JTD0BCo}xLtI! z3U>A`)sQ+^^hVc#U$|%(YSmE*sfo(?K>m@MwA-;gmw8fK{&-_^O=?@ub=H^9+0z%7 z4pz96C);8!*7hobj#sTh<2=o&jr?o5<&=yKrF*jG__m-uBCh81xK&lCnu{*~B|%bk z9dfnRT$4rVAywC9?1|z1_17;X`%#lucTl_E^z>&9T;QPOPDY$Oi-n(R++>O+V5HR-Y)mQm-k7w6cf^hO=a6KSe~KVS?`?!EZlJr`LMIfh zYR9-r+RieZ(DJ$&^xPUyOPybPneS^??&iPZJfpkGclushiqEA|8(Gws?u2Tnto^dB zPhOfdgnpxQ!f0iIdP*Q1*?Ld+U|Q~QJCD{;x!UGstz_>d?|7=$xy9>u9R3NOw6mmj zavZG8XxBM zBGeH>1|eHsrQr-MANx)IhqQ+ggF=O?JX_N|-0We|gwhMm1JJ6aG6LZdwkTG?NBO_; zS$QckH<}f~(JT0`keIQq{0SxH#WZA`1jXADovAI&RfSLxH8YW)MgHN}S>-P+p`!Rq z+L74qJ@NXA<4DVs;kXtD@{>17{h@_&$Y7Iv%Msd!J60rN;94ZQ@zI4`t{Ut*Uix~2 zoj%SHhtly=gu5!d7cyOBU*@oUxzx|&*;85L2GC}R8BUZAvVhGlc!^bWF!V_c?_|#6 zHLmSsy(T~F3o7F_jk6=o)u%I17bj6+l5bDv__>3*8JCaks^_ncs^=4iab2z?@JBN; z-<7;2aI~0N->HkUCQFm!{E_DOG*zVxLh-|Tdtr{>QX^XG7s^k|F0;QBq5JgM`m{y| zSx0I*AM`fp=~G6KT8(v2xmm zW9aW+Tr_7&{F2hr=b~`=ztO>X8M3n~vfT7PHCmZ#s7{2jMaO(MWhw8Tk+zm{(-mdb zVD6Syf?|F7C#?xAPFP;c`BkS5mHHudrS>uP+xQ1D-S_JY<}aY%C@Nt0$uh+rGv$$c z9U)8C=wWs;5xliHoNhG{>KB;RyTI#u!QV%YBTHG<)AcJdX`v#Fl$%w_Ky2g6c}Kr!Hc${aAufgM?Dk?Sa2AcxS8bbU7=}Zm0jX znyJ&W1CQ-+tbu)4+ruhNf{*}bSMslm1^Mxt+F#B%lQkIlg0yXZMVV{29AcYwq6emW zR3dUL=6OfUl3LDBg=d$QepZtgHdWR5qiM*d$fj<}e(Xw+#1Yq0Ti)h}9MesfD#RUf zXxo*axPkXfaIlZ(trZOgZ3B*2Xk%#b5Xq_4{_Um^$Rk&jxTGfW$e&Vt0)p%i*TqyR zbc#7hd}{^z4PhPnS^PmuY-8z|?63dDE@qzVgNhZjy@I;rdoqjH$NhnY-nV{wVr&ro zO2>w-mUp>tCrU}=BhkP!7IiSORXgZOhFIPA5HG(GH5=W*0ilo*CAcVsml4iYBp&|3 zafLSpkd!cdf>485G*Fu4i-*VkYsB>)pD~k|+Hu?l$3UU@x!(o--Uaq9P z0K76_r2y@c9eAL~+H><@Oi=?J_e#pj21s)vw$%!hqAcs2q z1=U>&zW0O#Dsks^K_fG=I{5~7?xbZHF+00|PtU~@9zn34@VE==9Qcy(K~3_hmbT>P zf0k6zcx#)Z20rkq@B$}C=U1j`S?=lGYHE{14HX$gts_Qk+Nu4EXuKeZ@DjVADQuC)zl01}K%jE04p}T#;&#-xu9tRXi61 zup(ec38%{B8=&1=yE>HO45*pxJt+84XrGGVdBb>vSbe;olFeGOx5bZ@Tz{2_S}w>_ zpn4fTpZuG@L2Q^Qm+OtRSCtx{;X&i!P zWL8ogC+w`@>>TlMQLKT_1}bd?k)wSL+S32?k?%>ZV3VDZa8#@bax!-?s^7)`oBND?|h)nFtcavy;odoU27gi zSN2g@Kuo?F8XR`p)yHh(U(+JhYSN>$lV!&H&AH0u<_JN+UJeef!IUkCdnsjv%$sq3 zk#u7Jzt~5d^~6ziTq1%rg7!OTK3u@-ZH6N2yqos%ux`AzQmKTM7aC7hK5a&~ryOrykeODCL+nd)V?yb3`iF}+f zZxWFW&vKL2g1@Go{uvDZ`{zrN^RqKH*O%acKGXaC&)R(v+$?V;!%HJ#k-T;23!yN; zCK>tuSPJ-}9k|QTcjCgdY{i#<{U+)SpH=uT*x{9+^Q(Q<`G(?f1BRy1ys0O5ko~ps z&JtH5vopi@_7XRx@$zdHr-Mt6n(US@nJTxDKZa8T0`{LgIShIujfT~*Y^S%VZq}kQ z8t~ShA}UmQoFwuT7Jm&s)a@XZia7oj(ob?By-ro)`W5G|LJsRIw|euMM78>wU|Ly6&jN+RKryfJ9|J+7Q4N%iR=Qmd4%)m_ z#pY3*2gCyYI8Pg@sYsi*0sQl)RM`(I;;Dov+xb2PHQ20lugnKY8WQQd-!uNCi@r$K z6{+cPK-^iLw^wb`r&wMT{dHUzGs1J#YFqm38)TK$L1gS47>PT!J75V%J!&9x#p2{v zf)x8!V>42>(@$(lZ-~h)-S$lsTf4zbL|Um}>qv_)ti_Jzik1scTu?p2fnp1XF>|qi zS9J5r^61+B(dOAlTlaEtjF(mNxTAxr)`~mfO)sG&@v?2Cz&g}$@99uYKRm6{Dq>Ji z)V$&W21&V1sav_W7uT_*<85-2Ew*uc&O1DT1`*!>Rh&20*~Gi|EP-}+yqc;t0-$;~ z9`ly7#&u$3ojo8BRqI+pQm)q!F-1ynM&yJc*&J_;sj<2oPG+e!50SSRc#Ecc~l(#wS7BHf63iZh>{H1X@S}cX`WEst+iuZBH!6V~=5_Wp6j!P>e2a5U^v= zYL7$3Ru3}-S#F;*yBnNfu8~7}fz~=J-bWNWbPM9lm^nNqHm6&~I5VRoMM>p5bpc{b zmjkC4h|qk{boZ;YEj;c5#ZFCd`_ZG!28*fver!aGXfC? zW3B1uFjTsWv+aX3XSZUs!MwM~40CtbQC=q$+`^-j^Dqrh>k<1_#xjGt@{FPgEp(nqG=tboTXe|*3`B@ub(*aSf#I#EfznA{rzF$!bPJFH+%h|?t;-P8T5_b z)is@a1AADo4cZ(w4`q8?uN59Ui#K*HJu7|r4792#IJGmQ`sXZlQ zcVQ8g1@?y+KWl7{*UA~SH~pUZ0M?^$`ihaY_YZ0KYUA^f(KbyRAzUbD0=Z5Zo%7wZ z$>eB4$U0g4Si+IMFwE+#eZ4$+cm#!n5|f%f0Pp65tyI zG#r9O7$;*^a8Lcs*rdco+dbDtsg)C*Mt^JzLwK{(D^G)^1Da!z=4g54R<-L{i}GQ= z|sK`_y_2VPT>a4ECLi6K$rMN2uZ{Q zZUPYE`|x+BqL2jQP0Lo+Lo;?*ii&&;I*p;%(s8U&f-of{Excd^A9 zV-`;PGaRj=gS9Gp+JUZ7=2nAiT)xl87)1D4dajqCW%Qft-zvLPKPM5>WjeIuioW^wqqMbFt0K z|6Yn7-)_YFkFy`l7R|p{HB-LuMd<~+MY8}L7kunv+>2enZzG3z?Lcgrx&kjzt;-v9 z!J7MiC9bJ@bTHSzd+z8mxclNu6N2 z!(W-k6Mcp4B>#q@?6t857NI-%QZSUfaGb7N^=J1oqSqHhb zYQHt39N3$nd^<0`pQm>6k4@q~H23Vk1CUApKt|$bdofeSU<3-!LrRRI;HilE{ZK8X z7*a@5z?q5a%fpANx5oo2d%1Jv)<;lG;+i|~B<~Q3c>LoV-Vb`+!)xI2ZezRf)s~Q$ z`oG0}6^|irype>K8jq2vk0w+&4e|e37e&& z<>DfHm|HpqEa(_dQP$nW7!STnNbNKa&f7Rjds#QOM4lBO{|Lf1r{!RAUD)3b-4`zJIXfJhZLL=krP{#h;|0-kjrQ9drk?jq zh97v0K;A*SwgyehD~nTgfq6fWB$f}N<0O>ZR_ih|=rTZ?M>$z$doCwcO=;N(eheA$bf%yeyJwFoRfZqelcPw+3*K);? zp~uQ<=~iJ{W%2qdq1?vl8{f3X<(d=dcr(_j@?8_&eMyE+@4dcE!i zR0@E{0bw@|jvp=6Bmgns9)ug|{E1}H!hv6l(x)g$m2OqgXdAHL=peO7O!eihRBf6y zAzYlSI;ytZN4XCTF^w3Zq>=dxiHty=0cz^LdX=A#?ABR26wHTanA|cw;Hm9(z`Mg5RTuKpl7Qo|B<96vrC0AT+uCq#a+NOSRy0#!pZwT0w!xaT}V+#$Q%{Q3g zxKbN5o}B#uRSiLO>Q@9MmpnX)CH}w{&N2gCUwLa*Cxbwbfj|SQ*eMSGSfqK2_ko3T z{rnSOb4Nag^Kyrz3A*_AEoipGd6LcF?FY5A%zb~}ri#UfpK>>qjnkUXicyPh4|&EF zFE6X3nRUW-g)Nu4jTuYKk~#Jxtt=pt^IjzL;OY~~8hZ2A9FFX+5HY9iCVEU_?P6VG z4q}0Pf()iD?NY#g5i#4mCg9;R;Dd;aU*Q@4{$=WPC#;nIR(CUX3QGv*vAZvF)KWJy zK~2s}$Sm&I7Q*^}37-W6pt|qHaHr5ve7f2qnkuG|Cuf2N3`K3=+sPliAnQ5l=g~@k zY?%0?PSwh9;d4$9$wREngJG4TA{)#ynAUT$i+P#;{&QxH&J~W_DZEEfYnLh2Dd!JD zp+3R>ijlY1uSW$}@H|o|TubhD3EDj9i8ysbWOpj6Ded%DZJ54aH!39KKf+skJ>gCY z*h8Y$IERyOH>7sb`xQ>vH$IOOdOWNY4BxM*wb4PsQq-B#_{9z}mlELE4z;Rkd#0dh zCl4Oj(OSAA8$YyS+3L&p*oR7kOR8a54l`zxZ}0V%oE@fD3-I@@qcep#=+cJF&$SJb z;O6utaO{{l(`PX7%FH$qa_>`|?VN9ev#2k;)r;EJ~%RyuI3z zWBucBCQQd>$w{}rB8S}h3_sI%%v-;``t2agLz=1scHF-4+pK`d9_YeZcw#mF6tVTY zJM~LvB2*I@#;lWF>tH^HXysRLIKghlpU{RlK#Voz3=pGP7r6-KKoka|IWv zb~F;T&zI{AN z^z_Iu=Su?LEHp2%)XD`qXB@p}n_Bf$gQfOF?*cf-sVNdpf<0A+UXniIL!UJj`^QoQ znO%OL7~^$Sk*0jy_>y3Yre1{g2kBY-IQ?I4qS0vv%C)!{=!@uT)X z;0N$W@}S{$@82N=a%sSEApHGx%oD)k1b(i8fd|e%Et9cU2~&>2?44MG zdJ9Me015`Mgp2`hdOF0==qP-$1ms^8bnr(75mTatwV&3A%S3_L8;p0{)TE@2AlG#EDi_mHvC^03iuD%r;Qp ziwDN_{x9Jjw|0woBCF_STzR#?b?7s!OZz`)6AFp!iTwqn6xY*Q?0C*K9NzBc()_9D ziSWqI1MD4obzTpU&?~Cnqaw-)e?%w>h>+sci>MMP%@L25 z{||JU2t5RXFD|O0zw-iQ%;6l`j>&<1zPH#X-{k**wixC2sxv_)DE~#-=0G9ItAuiz zs#*^!RMU~aAXp7)DRc0}m;df%3{OE3T}UIJclkG6Jpq3+$`?3;C2|LlvmXTAuL(XY zJwDq$xE>n;=^C4F?i(ML)FP!P2Wz(b#|i6#OjFd2m6g3L=gajbml|b8Bv}>gHQ*aK z17Tq`Ig;GRj#1h_K-zvUEbkL+Z#D54U5h;DIUWNFY_`;an({&4@88#!#tB^dVBP z^>|}oh25rgjXgM>AuV}YAsP2e*vF`4aoy7bpd325pL+J}CK?RX3A|Wq;N}wpcmhIN zHKCe|!%5AMJ+?d<-F-ddWu8IRfiV!6VFCLK&cqB1$kO>?6EI|Cp%Lbs)q>odOAdJz z){i>qc%((O8 zjGj)mS=J{nyjDD}?dms4nI`7z*9d^tNwHUBQ3m3RcJKHW5Ad~aFs;WTe^}K&@hz9Y zXU>O+9CCjf7>koyxi6B!XaZd@EHhrZz33IiZO_K~`PeD;edu4Re^vwc*{fV&n0v|! z4B{(HXdG!WS=FENzOiI|O>iSlaZJtvA7?Ke<~A@|k)rQBb&l^J|LAQ*MBbG)o}D

jJu_hNZC`G%tSO2n3B z8G;YaV#$a181F-px^^ZtLnd^TgJ;USB)QS%I z=1KQ*=CRu(^`z9E?3(|cDWkaI6|%mHXaAfvrybuigcO7$@6~q5o955_I@3lE8%a17 zT;OY(M*-c9<>`vNqiyhUo6$FpDd2fl!0>5A+ z)uIX)t1g@TTr)R((eltF%8IPE@zT!SEA$wQM=VxQN{{ukNSsxjeDR?rqE`i$%p(G? zGg7Q_)JBX11LETj(!wo-p}ob?=S&^VAm%JVDFX%6Mem>#imuGfKC|2uz8dPxZ!uoM z2?`*i(V~p*d9C*qGGzNa#LZHfH;_%-BstHR9P@5C$h@xv1w>Ik2bS6_QhP@ zMhQtYes<-Q-FrZd>5RYqFk!)ct=(yj+Oojl@oDGCA4 zo4g?o6F%=fTR3P)v9k>6Ah=W%1kTLK&hm>tpow>877t7B{O#-&xPP=g|B;Ke!P z3`4-B3L5ENP-?6+_jc!8@GD0SeP4mhhDsC5U7PVu>UUka2IN>2OOvQlOuwMZoH}=! zH(rl{MERb=w;Z0k&zjy3)mPOA`zaoGwn(@U%>f_6a8}(iem`G~#8<0}^H;=LgC2dK zl?uoF1v<92b-f5OHxsl!Ht2WKe@9C^_?p;q&ZB^~!1O!d+}67oN!P6fcjpP94`sER-yFD$8L=DcGRx&;|RmEwuPm;cBal(v2Z$P zP&!cpk~99Czm-NXx(KDVu5ueLTw+n_p_pP(&iZP0Jx(LvObfb2-{pG_C!auR6ko3Q zaH@ay5s9Qq>;esgQUP77nR4bU-jBB_=8U5w4K%0uySTWCZb8+*dnHh*B~yS`hSUm8 zxW+~xaOl-wOQzWwYd$tl6t4)iX;dWmM3nb|UT@or?jaj9@`Za}bK)$NLlQz{U!I1z zhDI6FZ;Ib{yskI*zdv5Lh#JZFiuTJ*yEbt4={`B^Hk!>%-F%OCZTRJf82TW_gaa9c zZvbfVtBp5RQG+T|Ey%CJ6g>q*8CX}Z#PBve!F(<>am14uRdVQ8k)go7fIXmJ_Z3JQ z6;yws&xGzI(7(kMRkcJP16`q4*IjN+I}vinxy%xmIKFpWI=fQfqcx$Us#EtCaM_Xd z)%;drJjE<_dU4IF<|+5S9(tzWxVpaDb&J#EWLs3WgvIuJAIfx3YgqQ8M0E34{* zwAs&+#S`c<%jWT)2}G2sZ?WqGOD8#PxgIfJc}lw|n)h5z+ks1oq-O#JZ*}iUCXV2G z8L#!Qo{OdmzicYPCO4Wxo?pHGb-!HjTWJUbHv1}0Y@Sb_9z;a=;Sr58_7v1QrjcT- zE^x0mu15|*$kt@d1%*xp#9dUXUs~C zHO4_%B>akb+*ggzsuG zVD|3e8RZfDa21k)KkQKOF>SBF7hhT#w-za|Ap>8M`J`ykN3?9*B2>IxPjEExgbsIg zIir1U1Ry|HJ~yWj3}Bcp7uk`(RmOGGht2D=6{r;3B}2y5;vSXi=oz@`70fh}IL0QYOQ<|O88w4N*PYP&?G zdX%vxW257uQp42~E-0ZEsU?b{`C`)TWfE=y43ncXPntVl)`&c5DqSk4NxBv9TM0t! zxhMg@%$;*_sDl(kyifGgA2)oU$kB&QW?WuSs?k#Gy@-ao_R`ZKHJxU5j@HFuC^$iK-kp!GahF1wyEq_8G)Pb0D zZnkv5mr*U98Cu4t&OK|Z7ZDAl4SyF?O>>VCyK@dpF2yE?8Kgo74vy)M8TY&0t}#g} zyGttb^b7mxKV;CZrq3BCC(Og?u)fK@o^Fxey_1K#V@~H-~0VI(e9~J>-j`uQ7`OvyUEjkH5tyKIc2* zMY*?xwrF(Gch^KBbE)IK<^C!41HNg{>5B_J+6^#%tYkyOSi{&{NbOq^k8RdxO81r}nsvbeaiT2FKsoBw&LbOj&5$0TSgAu=PL(a{$dyF2 z_5V|~+;Jnuycm*2^73ax`y)NZ)j-X13>xjdkD;lOc>b}`d=VFo1JPg@r}8is-vT$= zHGzP}DZ(3fLFv{dF(k05MC4=$WP_rGI8uGj!MsQOaQ>z38>ZO#R%-Y`=iI=0IBDfn z2g~s7&D9&rPcXLA)<0}}_7ttQyFyo$Ps}H52kVbTkT^$mqwX@YP~0bv22>iV0ba@k zU~JF@bw?&(Vc0D~oU<2Qg6YN5T>_^_z6X<83btiv{WLC`DHkUx9$Qxqp!`TVW3?)4 z;Y3i>2*JgL^Tf%l_`}_ZlQ<6QT+mQi6HnWv9D#SmElrN9K7S?b5Uv5%u|chmESNOe z$J6asQ5Cc}6>~eZ?2fFiUC$&2iv><2Mj`~}gV}Uv(B*+_m7PhjP6#c>m|^F}VzO6x zM4xr%`!-j5I)=^OiSkUaT9WBmiENmt*YM-vTU#fTBK9)Ga}M+r7rcZa@IiXXZ%YwM z4OpvpH>5Y)ta4H}>mx?scN++Hh47)t|<)=xtlX3+tTJK6dYxqNr9fx_dJObS= z$TyiX`(N6+%VCXMeAOq;0ovb`DUGlgz0uRC0Gp}S@06Dsm{0xpq}H`}s2Gllo28nf zxKb}0ZA%)66CJo$bxN8-L(g`tiaQq98eFI`su zT<+E}*iFN)&ayqiPPgdEc)5S0=g2&F--qioeJBLw%{b^PY_?^_L0B?+S(`e3e5D&W z=9v{DaBV9k%(%fZwz+6YDHODnipP>#!O@uVdGd2{h6!O9T$l+|Zx znmd_wpNy6JW|2nrrf&VG6H=kp_qQy`Y31ep)>27l&hiLlW6S(+yi!MV4lot3C5&dr z1S?p6!aHA>eWQv-Zf8~=O^tGH=6P*Mv~j&_A94zc=~KVaqbZtHw&yNuuBWx{9bLAlCf|-muj2G?nF3zkh0kd z1*`vr^m*uKwD%?}G}n62V&zJWFDFt@f@dpSAN9RtO<8?l5L${0W@pb)s9w1!t4=k| zYUu&nwwh{8!c+H!41N`l!t)D5)(g|ML&@*^#cbHZ9lN_JkTY@L^;@2L_12iY^0aus zqLL%uwscT+xk`ek#s``-r_u(9^Lw&84YWMZKRlN!{0=-j6Lo^ zz69~pDVxGHubY?t+SrDN?7UvM7N_m<&?y-YQagbcu!AEF4)%TDQNIl?fPt95-*w=b z$Eoy#$DomSAo}r5&)|M+u^+Z2W4|~Z2g+@xH70SzyUJ@9*`iB$cNGNvoW>qN^kg;C zz_9(5_qXiRgA1z8h?VeAl7{?#<}CK+DbgzaQ+&Ob!-jG;YDN}z-}ljHCGAv?v|r~u zIvnL?qa%9U1|OTIe@h324ypcL_QUgo-z{VJWE`5MTw829H`OEmM*ESoreGr{fNmaJ ztl7ymO0;q#4MU{BPo*|-iFM=4(a%k7T>+^Bo_v{fnW}Z!ANlpCow?T&!_5smg6JzU zgoaBtI&G;OQO~6k&|@@g$G5M@?&>?oTz!GR$~pMG!5Qx*VqII#6GW+3>+`n;SMXlw znPFC6#M&S=Wh42fWOj)y z*^C*n%WsBHsNkhLeh{Ua0@ncj`MQ^?q0-)Fv<2M1^(AZt2=RXXdOp4F&MLe%w)Qb8b0$XOT^_@Ovd*r8hW*;dx|ykWH(PzX{-`s+cKf;2QAbq3 zg9?dX<{RyF6Y|zhskKyz)4y5(or;TniW)f2^uRVfJ|2B0kJ@j?f;apHJhRQDwQ18m zWAejSL5Z|;E>cEKAU@3V>s1!#GZ#!Fjg#`p%s0IP{iPiN>3#uBEc)~HPM9ERiB8C5 zPxD_*&!3O1RTXDD00k8b9wwIWP*KbIW9VZ#w^1k{17c^BTh=KQDQHQtIt`^H`OH5~ zH3_PWKh~`n<`w9Ka%2D`g$RDuxaDRt3283ZnHRiIxtK^MHDG;jC-95rd)EYJ3v=wp z{=VeQ`_#(Wd(4H1uXk5)d^`-PZA2ik^1&iw0C10?6?0Rk!J94r;TbfVREk8voAKO) zA)?JKB!s(<{Q?}m>i}NwEq{g^`|KWWwSXQo?OoFJ{dUM9ATKNop{%T|#$p*+nu5-V z6){~F%>Ew+E{1tW}kkI402O1gyhtaobvmUd{ zAp#u=iH}B8Y2^0S)p zi{A_USjK!?Ok&A_w?A9nJWL;ZZd5&Aq5kM_3OK_ed9d&l<+4Sg?_wtYvXi!66>7{B z%PCHMS*SHH4`X^6LQj7)+{?W_e(bnAa0ozvhwlX*m_}YiGU6{NeEyyIY%h^BF^{3- zk3bk%ScI~-FzAlmWn`y|d%s}COc=B+A^idB8)LT|3@%oolluE%17cLNTmGX(jWV#X zHr?jG<@2Of`vtsQ!TWg;=kUIeZ| zIN=3Uevv=RKTeuB^BKbnjS3hTw{uJkDg-(npY#3_0)qS13Kx}MMt`1! z6}o$_xC>AtgX4|+V)hm^Fpre)-zWF?+c=KXkg5PIsh7n>N=gB=w0#+Pm~i&iZKZl?!Ic`|$G@y& z+JB6(82j)4cTVF#acFobPf?LLYU9S=TnQ@oo4><%c_*2f0X*53+4sNvx-Ir=z$^7C=PdxztLD)TFoEy>J^85L-;1@ie69jyGrI*Y0dD#dp@bukr4_54w46Q#Lh07t>9M3p)_(ajGtmk-rq%OvAZyMD zp;T?)G2hVuU^WBk%bwQMbj9!^A(06VM+BJ(SZ@>2o|(;4qQ-2*zmKNVX(L4vPPO^n zF5pB(mE@EL3{*tqTY5HBORMKs>8Es{K9%>n?!;SUafP zasT`H?`}vERYRok`=ftyfaM!A*`Rp@^RoW|dCHurNbFg;yv*1{0_a|Ott#M~a`C0& z`=%FpOu(SVR7qp}-7;8zpPwOPNQ*jU@{@Vol9Pz$*=1O;CY&P4OBxkmPR7hzaFi&6 zhL(Z;pnwzByImCjU|rI0a~VlODvvmsnSt87ZRP*p-T!{o%#a4i5`V?>?~{1=BukBY z%_ENZ&>72xSPQ9Fy0c|kNsa7(Zu9qx9eeXc_0NYx(46L(vJLxujcpfBl$PbB7EQwC$^DQtk^IH{$3k_izFO;%q@x^un1mub)`s*HhwLABKDCW$s`-h#$yVM5aJ zWsU#ukS(yF36gpeMC@Ao9n;m&+^o>7&EL|}!kiL{a?3!~#Uw5zwY^&SJ|_~Nv_W)) zWm)@W1N=K;%w>E&um}hs1Dx;-HzZjlQJ#!Z1rngicED5S^>qLbzx1$%@s}tl7gtw> z@{ht!$Kh*R11^I}oj!-Arw$5gVUx`WA`wVtw+0%Iez|@ldkR?$BJ&d3V{J`m`x&GbqNDR3~M%XAGAI4 zkOgFU38(poEU@7UpkuFwr2&cfuyzT?4fs#)(w$&yrVsY>QK%Vzfh+{tSU9=Qd@}!| zO8(7ne*m+dQ~u`42?yamL9qLR-pp|3K>RhyO&1B<)oZ5y@c9nwa}!qZn6Tl7B}p5G+}4N%|`0X|<0L3cAI|S{EmN zxL%!idhhei%5PV#t5i|lt;d$U>M;oe|L0PQM(3ac<@0j5Lyq&OifeH`tC8kk3>7tF z^$N5zt}2w=;@nknCs2Lj$X{iWe%TqFx4cyC{W%&*Q%B@}E@J?YrfHa1D+x;!Xz_c2yK{@iA*2O$9 z2Z;qrtvPT*@pEG@XhHb)ac{-Q<8efv17D)I-7hrTne`z+L(=x%J{?s&b0N9fKRhr7 z6nmOO75hP(f)e{G?qIbtsYJc>>z1{^<&F2`_QUY%le|%s-s6FF%A-41-(<&@Rz_MT zUXNeI@_=+O3D11m+MbtYy!3)syphgxdmy4pC+_OUSz?ugJ}=|W2GfI!qwe!uqMQI7 zTa}9p@s>>kl}c` zwZcbNnJb+2e#dx?m@}t5_iWS??#gXx#s4jnU9xdN1-36l@K$e+rKoMnZ;_$nZuIby zSY-#&Snh@iJg0P|YDjE47Z(x{*{7BblhDc_S_v9$W*HEtdT2iNXu*51DiZD!dL=<- zAZWkh6?FSWso`-O^}>2BV4#2Pq-fWJ(;KOIv$}ml>~T>?UnDD4bwU=+lP}|2>c^XO z^WPpAw0n#?z?s$v@RJkJ?X>Y$?f<^zCmD#GxU@O3PDLgb=l}+yMEN6(O7klt=zXL$gjS6 zEzem))_S)(;P{d7#|%~JWXdGJxzn(u05)%ux&erUyxq?iBQ;+El*JzHfNyddoRn;f z=)nu>?Teb}dra3z7-&m5J~dwW&1OG!N)v)!vxS($5|>cv!_FIAt7&WS-z| z@`6L~ehl-z>WSWih*SAw#%cC3sre}1ew)>p`0<3Y(-J1ol2*guC^tZ7R~dFb7xT2Y za2~$`%)b%oZ3Ct5xZAVL$=+S=uOdre>wV^ZOrO(R95+g>%C~wuO6hp+LikLKPvzx+ z!|oG=x4o}3FRuUCjdkl{PN~=ycD>RXMvTQfo>qqnX4731jWlx1(_dfaOoE$-iz!iU zh*wG@<4%u;ZrC7FjLM%~76L7%WpMlPM|#&ige`yTkGPj;qYt+Gv9#_Wdeg^uJIb*q zz~Pg`o1rc9HM+`xg)u@$W(_@yWd{Z%we;-B(U%13I-wrdyq;)AggYP60>yP-W}Qq> z@qRt&ZflC7o2g0E1mG92AlbgTjkcY853bW`gvQCg&i!?V1`^C4SX0vv} z9s>lm_Oqj0&hyU#GKHV%$@~f(&yy=%ka!~(`e8I5{k~3}86OeYkg-dii2jyd>dAk^ zY5&L}M)?cqXO8pi(WRkNN(S9Njlie?Gis-d&npQ2X=^TFZ#sH1rjz3;p+iN$Oy}zO zesZFjb*J^?)HTRb=bxb- z&5CJAtQ_;`wW3;^4}Y`Lknm6|5rCwk%IEx0tW@BpyW*Hnrw_qbJYMt3-mDoid&G2& zJ9J&}slSOA&%Rm?-uCC3LN!kIrTMy^FAtW%rsP`q2bO}fU;YE}8jz-35>gc(dyZO8 zq{RmDcF9GLjYoXBTb}jFqdeIH2@|BiuPpTpBzGQDuUQK+o@+rYTm)4ePhYY(h!=>qsk|j)&vE)9XqVoaYa)SU+6$V(oOpWQoDS6qR;& zW>!R#87B=eCFY@#B)l0OA#lm;*M5jkG6$u?d4ApN_&a9ye(_)`DBg9}owxIM%&9wqF$ZF5 zSC3{E-{d@KkQwBsZ9^JAH}1bBT)1M<@e&A_Ip+UJE66(Pf9b*Rik@FLcJ}ptQb_UM zcBVA4x48#u3ZYQu@P4t{tyA9Eq9J6R^Yxa$=sa^t@*S@P`#tksQ)ibrM3toofh;lo zs<5c&E3qyWR$32!RFu}CAx?d;FgFHBhZ?-Ri%*^<4kOFry+Fy+HkJ9!<4uq4 z21=U-8q^JH(%v^It!Cphl7pD>aM_XTrwwVtp{ZvBQwOLr>)#q4*dcy#=2Xmg)u4gl z>q36!&_nOOU)DAQP7M?l=u!NJ29DC7tbQv)W3h0b+#Ab7TvwRfe*KD9(ik=vY-6K+ z+#6}AAM2B+lYHjRxzg~%uC3KFy2gmd9vOQLS8h*fM!1>cJH7CVg?nS#pLPi^%KS&a zA=2hReXq?%H%T2`+K6*5r7o^ux|Pol*X_lG_6jDNhZ}_TzQd)ZJOSqw?I2EY#CqfM z?D*x*k%PGk*0Yq&X=^`&#&faIa4YGdQUnZ1M4=%}YT$VOPb%T8)XrG;afj9dT!y(2 zHNh~(f~Vm+?@8v{*OU3PU5Jgv)LzEzUcQck9)s5)fAlnHh8bK3S1-CK&g%~UkGs3$)h1g}ltwvb#%4rfHX!2U92YL!qQ$IAgPd*8r|l`o z%#c_?X+y`nn-v-*Q%y(6giRnk$I;YiV}r1#^1UReFx8d<^eDD7h_2pMd#{1UI0$00 z$T%Dr7cTC`T`}WlG-%!jQ@ok9A_iT-N}qY1v7P!8Y?jlU7WTvN+62SYPHfYl8_yj! z%%{ea?o-bpGiZve& zaA>7WW|vIq7LmL}5f=Cqv7Flp>mxo*?o{dTlb}_1uzx39&cYdOEpSfQmDcStA-nyW zfz;CJVzE?RBaz;vam+wcB&4E^?I<`>dW)AKGZ^x&;`LfqQ{XPG`>F95O(UbEEE?yC zzG)37#<|#B>#!kUaba=!dd!T-0eMPEW<=c@7i=p6f8sGmD0<26U=>4q*cMf8B+jq8yH! ztD$cS4~gxsg}@2jvo_2d3#17o^<$z;b`KHTPCD!m6p4&+Hh-zA45Vjs-(}T?v!|jr z<~?6|CX&jnhJ7|dx6QDo_+Bv!b6i~9;v-UiVoHalk=p#hLP z+cC{9W%Y{-aXR@4@jR?Q(4mYth~N~#<}{i$CJ-g*l1KG6ixHZ20;Srl_uWqUicI=) zfZgT7r1{B*rs>mOoa%TLo%G0igDj?e1*^A*!Fs#J!YMc>baC2Ne%1?KUblMi2(yP& z

  • QFd4t^Pav!co!kP_5oY#~Dl*k=DTA;g3~poKmmUVsm*fxWNNQq)wnCSfss;(d zJvU|d+;H;nSI646J`^>}f1ka~ORzru1?vJezqv}-cOWKso5SHey2 zEGymAM|C^dv-HM8gFr;z7W>QF7D}V%5%(y>SM5_bpD00^ek15Q+e3vMEoY;`Y}}8I zJiICB)N81}2YjII=f~G2Qo%@_X^zQ8to0l3O#=MMV_A?+hIdIw_HSUk9;}dQo$#r; zEPfcL5h7xH;K|t0zC9eLz1bW3VZr-WSdwFH*0*5QEA#3JS)X%cGffS2Cg%V{c}xs+ zIwXp^fbURO9dxg|ZU}E3zB@60T5_iY^FyJ$61Mt2+ur6-wwrL=ka_EMWwU0y;M40RFJO@y&43J{y&8U`8D*ogS?IKWE1gzMMMm!|o&7+{;ai&E zrCUpT@#9{)*zwzcez;x7u(o1}c2el{KH5YfpY7N~)F;rVaaY}ujiAb=&y=i`;prQK zaa4}SUKwKxk0V9fQn$O+%yg<#87ew>ihDn>dm>3pw{~JgefsR4SQ_uw;mGrh*4Yqa zpKoJTRyD+Q%f(zuEh#3tuug6->XWgVLdLZQV`}aU1a7D^Wuri%^*ot|>0Gi-N+>{% zehXIs`xLGj?`nIOIs1mfLVpLWb1v#coYnR#HeHBKMJNk<2dm7>Ak38dLO8L;}heWj(Y<)dG_ZmZLil2X&tSvw09n`l$R@ET=Y6J z;I$Jwn|18d*UBkS=OBw#hiK)NI)7$Dcs>-WvR+?@$JX%1`JMMNo2|qw8El6ZQ%Bq& z>a+|;r+U|(?%$p*r!!3~98qiU&cYst)0ZsTp%p+bJ}-!wT^E@gTg`0w)|c}dnnZ8E z?%z1^b;jo9jLCQA>#VrT1U6}Z%~Q;Wgs<;g9|p zaE+x7GGr@GN0aXcy~49rJTq|a@xNg~D)YjP^Q!XfB3Wb}pNrUM35}K3EK8~4yjhdE zH_=&J`PBjxj#(-t+%+!hppE(?J*@W2%|*wa*Y`~O@^=KL)k}3a;^7?VFvZBt%(rXC zHEVmt%KF$CPKzQNzv|>e(V^ZoA8bmO*Ew>{7B=pBPR8RV%j-YeOMii5IF6REuJbJw)iqD zUN_AtmGReorNPg|RqiX7Kir?R+vLFF6ap9z8rR?}Z$Xot;makS9J1}*2udqN(#xcBNKlJ{ zf+tT0@Ap=(IiS)opKw2a8-QlC<|IZyyQ(tK!T&h&>h$Ntw;d&TjR1nUG-xu~fWp%A zlxW=cH)PG9*N5{T`VJyMS$LE!)W>=ZHe?Y=-VjHWCq-*9=)<`0xW#K($zt9G(MB9K zp(GuHbbAMRO(%+J7`P;s_|mp*Ya78odFZlg$gbJvp-nc71-w1c(jIu3PZ~R38x!bm z56`ze8q*l8?#XGPphq?kSS-cK{%!|1{LH7iddJG{DU)|_6FDom^x=ob@n%9FO)P4o zf4P@OmfS1KlR+FU@vKMw(R7TVQGze3P5gsWdXUi`$w*;frrX-CJP>cJM)6i_+q*LX zmj(C7`TC)cXX~(66iNlp_Vw~Z{9)*e-Wx&C=qZY(q~%UDQRl1M9uL#Fnr?i9g;z?p z65Jot-2iJgVl7TAY;NYKywGH>nEx;8t549!8d!FzHQ2alySp zHjvm!82_Zq*bWkxR?@jaDfrre~aG01<=&%UIjm4_%++?wFmaYj_4hX`f zHgu$2))k%vZV+fAmxk9ALh@+!lyC#z%!@;QU3PG z^E<-%(?I_CA^iz$*>a%Y@Kl`0r-#uRK{Q)yCq~ECcJT<^(G9X2XZHy3{Zt>+cQt+w zS-!D6BJcID&F*lo+I{7IJVzQJW6}6)JMZ~AKjJd@m%FBEHKW^1jqO)oUEBgy`)o<{ ziFhuTyS(OS63Mp1=OrT5EP^vR*;r%Sfsl{I;1YEtXLc_+mpd-Qix+ zhpLZigrI=5Bs{d+y6vBQS!>^i9DrQYPQRPajGm(xGH%Rp9UTRK#1Fw9rksw-(zzm1 zAkqj3fKfR~F%5Dn)y6KEvnlQ5mSYZlH_-+tt00RsEmvq) zN>PY--c@$A%%UO2L|69y(NHoKr=?6y^kyB$zC}~)=!S*9o+!|X?KSQrDN&74Ex76q z;`OlPR$$pKI*2=;Ds%tTbwZNOxhqSzq{zqK>e%hNvpwH+r2;xFup=C7xPZxyXIeJa z<;NAq{1^|2T(yLU#;zP@Z9W$3ZIZQw;;@Z<*o(Ai!R38;CY~MXYES{4JsFh(mQJuv z+d^(2jn+(jnD=YUX0l0J=VKA_mn&;ahEnBYsY8R@C(IOzAaJJcL!V!OFbs@XpK+ku zni4|La<073_`or@Ci{hP0eMZZDzrJ4wT|SDDn$QpB+FvC$?G^nz-Rf4#=i zfmWm0Dz*v&la-iOI}Fs4(MQSv^TX$~Fkhsd+u6;F@?A9Uyd)KTeVOOpGU~??{hoG@ z9>`XNidL-u!1ta~cl{-gQ><+fO~UO+$x8R%5Z!GcLtTJngEb=2EOyo-ZI0P{H*EXCX( z>k+3RSgOkqN#3ZjzD0}v^WamcWo7@3`;iAspj94Dih4UOOUp_NABi^PNx$61@VYnz zHCMNiyeV{6CG3`FwYX}~Cvna+I-07O`MMBXJB6F+U1!G~~Di z+WAUY=ALd8-Dvt&B#x(D8zr8Ocyzl z(t|sjxAzZws%@>P(ze_nsAF@mef5#g zx!DrNf8cynx)sj#Xha1jnV%N<8!Jpf_(h^{lg1+L?Vc0X-1FVr_^f~|9)p(Y{QIa- z&C4E1($6^klv_ZvV|{<=bJO@*4i{8Tt_4Ieg@^!a_AtSz>fZaa!B3Hpu!kb-;Bny*HcIgUTUzxX1{WjQ_@_fQ}mLA#3pqRAS9ke4~eo7 zFg;C)soUtDvi|1u7Qj&qO4pwE_WWdp@^g311fbm$`J8_DA zW%p=}a`Kq07%(1c1Cp;CRSNHIuy(xmSG~Eqs=GDAjaoAG%e(#3IxqfT*Ts0V-023t zu8n6}va1k63Y1Xg=#qM=Da@UN`ci&WTS%Bth}D?sobJxqpSPt>V$})ex2$b6Y3&Xv zQ1Gs2iv*Z$?eW+PyM?kSbeIkgT?(GeD!>3bRT#!ydi0_4lDJA}^y-;myt}5ZMYOSC zR4o<0GWRx66>T=B)#36?N!7Ql>iQ;MFC;(M=IMv19=Dg|cg6>x#;&4l`}+;8LVil& zhy_CL;Bxh#zcw|d^S6*7p-1&B;tQKDIAdQ-1_t6S?&<+<>($K1zbPr@7}iG9J>QY; zWu3V5On+WUO22K`zi5v@Bnq_Es994SXb$Qww)8x*ZU|z)GhCDXmAIzyIAbT*k$mr}Ke=3(DH6dn_&frCCZt_=dYfZd4D$A$ za8KY0i6!iR>exP8s-ppXTWI{maxd85i9-I;?PG(FwnLneCQ2O&;$JsEO5Bxi) zjOX5VZxYXr(!)30!^m3&59k=TS~l!cU?1UF(uzmWuiZh@yFKQwil;lxhxTt-isjG5 znP@PI<6#H!XcZ1cUke{v`lE7&tV(70-Gx?kCAGtEn~+u80QR3StDhyE&a(E9vm6mJ zwR>$jO<0e_^9PC8s8j1AS^O@SeQZS%^(l3!D!IK+2#zZDrT4}tQi1ODS@opqZKA2k zyOAd+Gpo{{yYk}W?SxGpMn0xp`NwFdY4%Z@@zv7%sa)(112YjdcE+l54bU?9Qeba@ zOHd!`%!0^_LSH@q(uF#D_hi5R_8-s#q=%NvqUH{!d%Zd6c*9t%rs9rz zoXwc^S|DPJjL;|dN{~vsQVlZqpE;MNm|JF@Us^qhX7iac^KR@>T1stWTo1g;k0QpO z&gZw`mhQB}mh?VdaAteq@RohA4FAkAMSGGpt*-Z6mB^lL=@W%UobGyn>Sq+!`}3$1 zPJZWWXV5z`lH#Y*aFOwol2N6@hg-AFY{2g3@RR0?`!X4gvO@QRhrMcICwI=PBv&Sg zR;QH-KdTwIq08mXna+IZ{d72M?*pf9zdi6I1@ELC2l9r51lm)T`k{V8CaS@o8>+Ka zPu2ZQV7)rw1Ma7``qB<>-R3eq;8cF;=(p}XudmfvU-f#cot)@ATfO9kf3z-W_Ed`+ zsXgKCdAE4LP+%F7-Y8%h!8dnpGoEo;oAL=d#%f2@*KpQtKGxU&o->XM?E>i2=bdtl zILCO{T)p^;@TomHfs~{?lH2;m`Xi#{k6wh}9lkvqJ%XLPxkwprm9Ye+3rO~9_Yv9! z=~D7{S~kcdpUd8$%AWqfM`TbdAVC&Sj5DuY7`3zt2plP*$M3!K zS%J*8bIP>~+-Hs)*c+aYoC_|atOMKA2HA>T2kNseeyL12TVIB6Dp?vK=t#8f=HawI z7A?cLcszMonUQtgeH0trS*1;|N*;qhe)A_*%Px>8z@c5G5b0Gg%tEb*I~w7W3i&qs z+|T7d{InX(5HldZpJoV$sWZGT+{sR46hU<)c%46~dgjU3KlQj<$}JMp^;?rE^>V0~ z+2)ye6I85vSlqF$^oBANTl^mbKF9aiPZtygm~CC_ILluCVvTm-Q@aP?F-yrtve>AX z6AZ~mv%)3eTc02>fD}vXtd5J1w=Lb}NRbd~W{NmuII6A*ougNif^uspjk|s3=_Hko zy7AhGS0jFCq?{Qx98XAHYWh-=N7^k{i3xU85%c1noTQE3TYn2cHb z;A;hPHczSig*C4Lw19bZNvSsf7%AB)#pD zJ4qyK8QppgykeA`4UeWFFk~f>UdA(rG^XY8J)OUc+IZ*m+2Dgc@ACuvoz{D24QB(W ztg#yl?gpOqj6Z&~C)(e1Z*^VgJOl)FFM|zcR&aoOSwUW2((rnwF1B8B+7nueeadM} zx{;1;8#yaLSjwcn;bK2lye%}XhklkWPf==aD9eiWw;}SEPkmDQ?xtpD3gjF(^AOK! zP51arEaN^09@BS@N_xNYL`9W+JRng}^=)V)CbF93MA(K(G`L^)nR4zdlec}wbPQSde@|PLLp6~UroO(rY?ejN}^s| zWxeFg`WNW@snThC0F(EthytQ9``yY@vsIIZ>YyPXin@Rp8>zqdFsVo2F`_7w+r~)( zxnt3~#5j9N<=YuN=2T#MruJpUh2c|p^h(x%K6;~bmwEt2)5W)f<;YhbDOq+rWe`%+ zuQzRyr2F@zJHPchplD;>?qQHy>6Ez7Wl9<>j4f7>-5#`HFS`GVc+VaNrJVgsyRXj& zHI;wQRn4J+Rjq-`eByE%IGtKQCl891u-2@l#R4Dvc+S0jxUW6Or--d|Txj9~{qQ&} zImL6QXn-CLQyW|wmM71S8hcxc6>j3|bGvsVoGr9mDUAIkin)K>BD0gxPaBlZdHn$t zzq;!=Y`r3$P_laCtPS29>J7sY`GuoeHMc*yTn%6Ny@@7w?MA!u>Z9cW;l}}YwI8u- zi$9T_)7YZni5EH^mS+}4MEVsp;!58b!abI88Rl!hM~>R@n>#3vTXuaUep^PMfP-*Z= zsHtBbkk-3>H=hh+!WjK!q{=PjZaOouEXZR ztI2QMH(2e{bX}VGSQmzUGpU@o1_BD)6VR^n2C?hLgz93<*QUk?#nmwL`{s%VO&F=O z;RqF915)PxjoF)WDbtu_C5pfiX*2ZTOrn8kN0lX=Us)BVo!WX=eQY&b4n99Yyopb8 z?o);@P+(wFl>4;Dx?t5e_;!0r9$RUku3vWT&t!^)@5^gl z9m5fd0DzGkgpI3Oh$46OC|T#BBDpplezv&XY#}}4lf%m9`Iepow;hN`5073a*d%*k zDTOhC8ZAZpw-j$482d#4!`XzX9AtuC!N1u9rYIP8C2T)5-C{p2!imQrK#6S`$J*9i zuP1Epn2TnzdB~s24D&^Bi}JGq?YF)Vib8l0k?zBBVjW;LSr0La#K{vQ%ivR3DPqV@ zn*HWf<<`g&y3!LYDvgjGrGNk4`4m@}ul~b;H=?$m{vM5VzF&Kyh28#itr)S^xEyP8 zc)i9`>s5i9a{l1n?uh^4`?IOV&-62)=i-F?JQtfN{p2t8gm8UvWzOz)cr}>b2WT(< z^}cvC=3>Y}o+vRg(t{t-bj%95MREq)_H8dY9|5#!gCxT`=^l4A&Da!FjLZ-B^L38l zpsuaogB;(QHv*nyKu;Hu%l&8N=zhGr&ca+N9Hr!FZ;Q^G1_i%JW%JXP@M8&Q3{BcB z4SJB9-*y-pd+#DX9lmkoj4MdvIfc`BgW0?ek1hL@n>t`=53+mHpaK?j{-n&Vgh>+O zQMy)gHg}o!QFImsjQYOG&FXbPz}|b0E&Fwm+VuDfF9ny`QLEkA2vNGA3O)AniwsS^ zW(W4Ak#+&n1z#M#q)4=3a;2(Y`cjzi+_~KV&Gt+LemOMn%=?7=H$w!jh9B1MEU#tF%0) z-MTck3nc~&*d{uO>dqAQq>dL!3OcxgNRoYG)wr+$aQ;K(BD;PUp3S!8+l@9iF!8=? zpg6hA^)cI}0#Z_y*Pi5dt@l)|qDbbSl)3M>K6`i8%?I~u1}h8dx(G|k(^ovrP$|k0 zX!ZYP*T%y+u!c2Li66Q<{i`zIi^i$<>~F>H z6S9TMeaS_{x3OwF);2mWtE;3ENinv-r~K4lpd5kQqL`yX0(Z{4O&8axKYCw#JNm zu4t9I4am3O(R#nFcM_5Nn4KPrYt~g#=7S{{VE?@!{Cm(uL*P2kJ{lqO!oU%p_NkW! zzCQ6!o)Irdt&eL3cNQS2`y#qACUx^qDGGLl*NJ$c7+Nn|Z2P#%hXAOW@aTN^WG$vp z{-jz|)2AresG4sap#m?g+l!0ndB2gLEP^@W^b!f8X&7YVDJ5YqEkud>Sqauo^-2gG zN`Y3r(LHssu-Z-Uq->$?8j^kMXK}QzdK#OZ%4|`3^q)2JWEP+_la7)^4Q+J^sbq^B zmA?3=Hsfa*?yTq9ZNp>{n#?AHZp6Z1vG;yNmn=Wu5ig{lb7>PD_b)xeGG;KTy)Q-- zr;-)d3*8Wq0>n7De^%`Um6c`9-Fc8xI2unrSoqyl9yh%$lS}WaXsQKwKw=!X`1$q3 z$Q>{`LwgGxtD2>;$bBni20w(n6LOmoS}$SHp;?ud0Y~ z)B8-c9h@R5^ELt+q&bZbB0^d2Vl?%!oX2)^vAM)J>fdk}GR+Oh>MVU${9)fyv$ekv zu!fwxd^i3`y;-`NCyZ!)LyKkVCPMG371&E+2MnTX(sNMSP9YaOcR`>igwU8hlK`^B z-#u}JdZRNw2t`8mxDM7U04WXW4+gHfO%NQ3^rD!mZFOZ(V}X=LvuDsY-8hD&zMN$q zM;!qYsF9Ip;9KwND?Q<62aJQHeF`gtKBq<4bnwNXuj~#qXeW;=^Cd;-zW>aA%}-dT zC8lxK3qj=1z9((9dh@q}?X`U@qAG2hv3#U-Y#kUg`!_ly(3DHR#!I2Gh@^b7*k%*H zu^q#@Wg*LLHy?3 zsgt=U9c!dcBwvbAap;6kkF~N&_IR2<+?n4%hx+lfNt5wP9cpx~?F)OKH`GzR?XlJu zy!^$jKUiEe#Fu0x=i(!hWgyp=lSE8jsmZOuYT}jb{+b0mU0rLM0uMWK;(LT;g~9TPvu$n00dsp**yYui$X`dKw~yjRqci28zU( zIu68itbly4FsgxO1=@4GG%ANjphjdpoeBn6L=9KFIEHMB)^JNSJT*FGrLK z*g*yCd%dxKKr0S(`mdc8$k?9xB7N@;B@C8$j>{=aZo4A`sS=tgzW!|+WV(3P&r6?f zJk2C{73cKmsxqmS3LNd@C*y~|@>N8?6Mn?Dk-Fq~@!1q?RIi}#M7&z$@wd%uHsdu% zS_)9}4=0s&9(TH7gvf9nhnrUF|JbMiCT1GUTOxGOLrpEvlP)5(;P+pF^PvQL&30J3 zl3ik)yn`}tc7=fwMA)CQ!;ZtSH?$@JMjX48)0E*28wx@QxTpbQAC%V22*p1r>r?(oi4T2q0ssr@Im zeOP>$HB)c2ZRc$f$+41IJb3EWjkDmS;W1iJS98Vj(+AN;rhAl{RD0Wcy%Y)7~;jTe8139d?! zmB|Hqr!+aH5DT~pEL3XMGBzFdukiXGoB2Ue;+8r)HC))TXTqC4*zO{AOjc^sYXMDA zBZ)!#R+f8{6hWR=^tdB)>rRSacyk$UWb`>4t`0zja#wz74~A0t$An^wP_rZ>?PIB8 z2clN|34FgUt9nXm_5-UnzX<^RLYicIT5$2mWQIz=NpGY7&)EK7l5og;Y`9_DyE>%%;@8(Rh}DswlUD@pdMB6^Nk>n%E^gjYhhaB@ac7d8 z(HEblrvFO>8&=6N8jV9+@(K-fbWoH1q<0Mgry_ei){-w>Ew{FGcs@dv&x+OWc!pFY zrbF_yS&cR(f*_~(gmJ0@yYyKx|25S=j+Q5@X`!dUmi$(hG*Wu8p(As%A6({(!K4s&8Rba)n*aZo771H*wcpeG-{pxcG7{m899_a<-E zKUG_tJ+)`1=XB50-A^~+pXDWx;Bnz0ARv&WB*l~Fuh`F++PDn}u{e39;+sk3MX?Pn{$weSI2e)5kj!MpCj# z^U>+^-_fi{)%sSU6${8DLr+>8$1~T6Zf%X-0>mSBo~~+6m#NJs#wue&QC7VL2A%gm zTSG=U>;vmR5b&ZWCVuF4AQ1a!Gz5ddDr$4pwkMy|e4Am`if%JbS6UbBKX!FN`V5ao zq9MZAV|dSzyEv0 z+iY_o2lxkp9;|7~HWL*v(e|gB$(p$rAo{k3SNO#L{_T5k(FGq0;PM@e?uBFR!E!Ty zKu-_f>%8%jC#T@>zXS;zY7`=kCq=cVW*DPFg%3I7BeI$;QvGLtu84q3xZpQ$qafFA z=TrEmv}9CW=xa1GO<00K4_q;#OxVP{ys~Tw!ORmJa_dJtDSyu&=Izn}GK?ZIsOZ0K z%>D$N8Tj$BYIi+B9)G^<0|j*Rb_kTpfHK|dmUaruJS>KBFD{&hCXnr1$}cV~?~|_b z6hM0D=s(Huk>AHseWax|9-b*?+A_RnShe3QZ0`t+?pyXusZKG!E zWx}foWCLZXv-Ycx1O8h^Z-i5T1x-db<|;U%^6~|lv+v5oEV zLxqTiR9)EX^gJ*I?^;dD?Mjn~ikw$yN)tm8kPX}H(E&W<%5bC%ck%|Q%a=>*OZPgY ze~s2i&_fdx>{a_R_Vl4teZ7ODcWeeFVs1N8??7qI@LP`LKF^Y?Ta!0|cLUlFXR$SY^g6t8=(H!^yqhG@WIzU?MelR!1ukUI4VI!`v zexNgZ{JZlOZrem+GdPd-qncMD0X7*5?jsjQ^$j@pVjbk9j9@bNbcQYKHJp(C@(GKU=4AA+Lq9TcXWIQ zyE&uM4!COl9(7cmvQWoixF4*YQ-4pVh1J_c=)?Od45x9yDOoisKB`}`$eV7x2{Lei zEW5U1j8>A3b8OiCKKKf4Msj>6Q)*I$o7+iVo;eJ{a!!lW#P{1N_qF$BK0uJ_v5<#ajR zoi7CRu*5g)zFe%lH&L!vXI5rR;4)KSHYfOW#p<3mGLbT{h*_v=i2OJ#j^84|+46I= z2cm}k(jA;ku^ojwD&Vv9{)$%f=B94>IyRMQA?4eLyq|Q!RZxCiC5Z#SI+?=s!4yPo zcSNUNj>v@_hBN(*j;q9}+Y+2kr#X9ZQRo|pnLWLGNIexE8D$ggkr=?d?e!1b*F%$6 zHhY_@g9s{Cc$ngRv)vAYLBn{qyH#fTQ>PQLg{jpONI@$-h(&ExFj zaV5oFv+e@uO8@-w>2hcd^!a{`Hx@;}E8eZ_*2tP|WIssyZtmoA7zz5!5)Dz%B=#yZ zwS5c^k7URB%N0Z8jSi=>a$MuM6XsvO49u`-Nt`BKxQ`*ZF=$EbB;UFGF*Sx@D7kL$ zvR59GP%m-Q?ExiM-xA8##^qkbcpqboGWp_gP`*$}jC6rKu|y3thi2^keX`sJx9sQ7 zpUHb?S_D?wcO@T#B(4js-VsanzziRItO6* zof~dvawrjqlqBTa%C9*im?}m5y+-}L*t`?*NJ%rJKzfuN%0JZ2c>X+H$N*PRjG+8be`q|gr z_0QHgM{v9(1>)mjr>L6zRiz@a2>J|zbh`HHUfN*|VH1~+KWTe2Oc$+@{H2L_(03aR zQMB)Rh=O`c)vWK0g~+>;NOYTb@-buy7L&W{%g!umGITPG@x40@t*yGTB`NWgavE1X zi)V*V>Hy#FPwcsV&er(fOAn2+uXchpmcDO{y|vSKBAjD99*M1dfs()wT|k(-sB7)W z-7A^1KPez}42jB(gZ7T+PnwR|hbZ{w1~AoObwZj{RBqnr@fpT@ft;*h3+&z?1b=D8H})@5i%`TAG=cKV#-)GUH$51NSgE+i_P0Q8l@P z^j{ux?l>f?jQvK56!yc71VqgTt=l*#-AAx5(O5xJ?Q+N zRe|`9Y*{_MT;7tHoW3gJ-FM-LOhS*w+4s2-pPL98hO4H5*)jnIe`6oeJlL`1 zV@6}JM?XiE9Qk0h;s<)l`3Fe`fjp`Z7QL49sM9UQ&|V$E0&C|$kfdvVPOyxHWAZp!?|b>Az`zh#?VG!t zrrq%VZH#Rr$Enjux1aD`-&NOWzq*Ms z_z31`&{a1yBI4Bvf4_c^=CoNnB#&lRQB&J!ImH}&bJmsuHQe$E92 zI%BSnxo#To5kAbw!NoW34z#03M!oHsQAtI!_jV-Hq}PE9AvX!t&YHubX0n|=GV;Y1 z{M^IciLdZJg(CJb4OXy%gqI3-O{>S2p5q)EW$lhR%TcsEi#e-RHsK3fg!$3w@|~BA zb&)FV_+o1czaP7x-%tNKJFe6xCq#aUq`2E{qjFG?ltFDz8@NwGw%MLb6AERgD;|NX z0HF)_`L=UsTOv@U{_LT;eh=+$)S+Lp90eLMbsg8Dd%66KgS&3o+qLgsT{+Nwq9{3p zhxv5bWCU7urqK?NQz>Lb#5PMUuByW0#YU_)JK?z(^K8JyZ|HN;1y<@d${G9IZAj3^ z#7h_c_5s!>W4nV$C8{}R&}0XDS6g9Eyk=Sr@@BGxU7rMW)kJ-{`a+stwkk^VqG;z+ z6K`SWyJTLprOA1v57NQqMqKS-aoW%C6~^u|9Z{CQFlPnstZ#E5klWOTl#-*4<*Ucy ztI;if1p%kHIYSo>I4N^n(6Z(_CpvTPAacGJ&6)XPkqV2d(q2ZGN&JvzYu^i5eD}f6 z2`0aeL|~6d=cNyRq#}=0QLfRI#E4RIKK#S0BZs=khY(hh>yh+F2YWE&JYDO=*e%zx zqvkrbeuP`{v_F^`+sNi!HNOLsgU)yj@LONLkZXP>WEm^`)pFP2VI-h>La+$rh_pmSc&YkbuPT*?o;h<6((`%k+z<<}qFVagLXOQ=#D=MUPMVR94HiJtt+>3)o> z#5Y_wW44Yjwf)TnNJ9BpVI)B0*?+^ED{5*{PSEj|^l&dJqHxr65%=uHrl}Z3vV!_r zCsMFv&5?%qoFc)) z6kv*`l~GuqA^S8%wHBPt?tOLPcQ=?1`VjUtTr;Qq_@lnSEqaa|PZ0mI)~e?*{W}lx zv#?J(0?iMWmG>79k7vK$Uoj$oa3oQ?haNRX9$+5N&Sz2?)%NQQJ~hu8+vr_P#26=S zC`v!J)_B=?pyS>C+*0n9g90pO>DJk@cu30$pF3V|jev#JSKdUFZOf&PTvxdoCTp_>jCQJTCl$UU}oq*`b?m(K4>Wg#*8AtbsZW< zPGmy@@oh-_px2gw7cJJ2gvq}i^v27j%qGSt>%W`^>~bM{Xr>6MC8^Xki%Z@a8qMw$ zB$QmW#KnltwL##u=_IJ))x?MvFQc+kRxQ-at=iFETHXuj04d1|ENQjfofuXS-pp|E zcGQ%_#JaqDJJy=uvd57P1+lLt# zj8@PoQ^&gV3A3S*P;rJHAJgFv_TTbbS{kdUswpZfi+;pkE;Rm^T#DIx5HQ;eFm9xd zldU69Ak-V_z4e(I=S7OyT8GgIglz1fInUtzKSa*E4v?J>((pJmE2O?v2h#tUQr*15 z#*3KxZ?Gb*rAL?i`%Hxm$>$3x{pFg)#9Y&5iQ)eB*J36Tb(pf^9%-ABf7#Ld_Y2gS zEBuiE+ydVy{huXy=;5J7(J>o~jf)w$Vis1W|CwXK zbg?a@pgslqQn23zKVs31oR${BSPNl+&H&fOFN3`;sHM0;qZ&1KaG~0kK0EdM3t>>> zS;s%S=+Irnm#oyPPZ)`S_q^E;-fH2hObp*d1ZqWDJoAtH(vnKmEl^p}4=|^-%_^zW zrThAJZm?FH9H2Q~UhFS7k5*>Z{uM5U4cSibrc@jOD+?uJ^|&fUiLv{TnhLfWoaGE! z_3yp7#tM$zn0hrE~#!fsPP#`5(1}SN`^a&$R zBht95Q2<+%9G4{plZ_*Ga00q&$D=|fT#f&|;y`1e_Xt=ll zEjmi&_X`EoHf9p{{0sNe(%u=*yAQ4AoBvo!tMC$()S3w8m%5JrmLf&*f(kneEp1xj3k_@@^x~fV7#VtGPo93?sU6K>i z)QUQ6G#IDM4&lcei6hHUG^Z7fpC&>Y6MUf?1Y+3SI~1Nu)7JtU>i>~tsdPM`!0xF< zp!@*Pkz>(sId!7VX|=S-<^RC($)UmF%+kiJVytC{f>qeYthheymg?`vF9>RMF%k}> zJIC(hffi_8it5~7Wa})u>_i<5bW_zbJib`b10ALwyajc!EV{WuT2MN2;OI|o)Fm18 z+OYi-5)uf`(Epy0v^8WJh;Rs$lx?(Sch}?jdg&9H!<~|$FJ!Cd>zQMB?j5-YzI*0; z8NFoDPur_ax!?QA(LPX=14rVM!5w<#+nN=yoj@VebltGj{QA+XHId4wO4YTS*=P2C zxg$DTeIT-bM3G;=Bg(9nTe52dW9EN~eLW>ue;tzr!$m!kgW@vc?m&<*aa>7~Y-rL0 z5mU_Vsuzks)IhW`uw1=^pfEn8q$=*~eM&`itZ8~G;>Hf(j;ZR1G_oo3IPU3;{) zZ8Nlg&eO4Zrqhk))xoqmEMM?3o!`UK8khT8Y&X66qp?J>8;|zVP4`5edX*lp8JaJ> zRcZyvd^=}wHv#K*rQ?mhS)G9?H^fuxj?)n%%KSL%UvX*tN&WA)A3eso26bdy5!+9N z4nLVE3d`fXFZ2=p&5<&ZfiFVm;o?kDSs^*yH5t~US%d=fz>`IY}E1LBvQ8lwGOWs*f~>oCa_d@=q)_6@BOX}%bYx}*j2rtJ&EZ6+%VAl~@s#~T+pP{`jdxdr zQuZ5!5FO^0{>weqApRx4Jg*JB;L`yB;+wz|=3>3oU_pM2*+>$#iK!{%*$8db+4hH* z1?xU+rx!DAnt6cYO5ow#GWINo~j5Jv{%{xa+}=(x|E7Byb3H{pFOIpN&rK^kwryhCPC27u8lE_8R4+G>bhwPM z)%ASe^%s$NUp)(R_?%Hc?9$YwJvr`^FS7%Fy~+FyT7y``~fVkF`HvLsqBn zRq>puEYBSbH4wA2>;%|JkYU)#3nlDv(GI`1&a9on9u|*1ze@1oZrA5(Ut1c&Lur>R zv5~N-K;dP^{K=kk*@ZE8W_0!0_o|GDwfa}B!+|#t)3m;w?-p;HvlEQOweyoYYB}(( zN8;|NQ=RFeoVLkc2>F~n63jx?dE%FEkfHgQG&u2e^!Be8bD{f-4uH*P?VbpzXP=c5 z;QVQQT>~r#;u0iK;mI&%tUOX$?tp@VMK-x9PfcSwH-OOdI0&X(8lP$8zQ)zcZs?Oq zxR|!mw9PxS4?S0wdX&XevtqH5)Ix}<&=;+f_p}+l3W1KY(Gp)sSD_InMm0E`DbaB{ zdD%KFtF(5N*VM$B({HOuy4i`HlmDKGFl>r)UiQ+M zKK=4215@sk-3od{v7;{|p4ozsf9QWnR=GC%yUafN+eNnm^W7i%U>t>I zUlf3EQ$+f*FaFI|j?>7G9(CpkiQH=Yv99aD@v~Kl9tRvOqOP&r|NXZ z9R!C@)gi!R9pHAcSj%FB*+U?~1Rk5_Zj!(RHlpee(Rbd#LA3i_Q9Uzpj4W~z?N;L0 z)i$B?APdISJ%flnO~KagTY9#>kNh1U6@)i5e4W6G)=gHpx86D-63FdTnLPk`e5pKy z1~WKTP-iUFnznewbc~|1-=IY$MaPAq?^9xE05ZN;{|^4jLSe`AB(Q!@)^EVS1_n`M z`b@SWyx%C(VE_l)_#1}}?!$2%3g?%$PEJ_zv87X66EK4NbJ{LdHsi zypuXdAheN=)wE+b=j7KY=x|TiOYM0Eg{ev?Rs1Gr3heY_pC>$fY%ga{u(^#Xm}Q3ZrK=F0)RyTLkR97iQLY5hk$&Q86tgVg1| z+(mWplFPJ*`_Q%QiT3D-$LJ9EGfm~ZV2%k(My62p&I8gryK^_+)C={CK|$Nq+F4Y3 z9MP(a&~Lu#Uh*%`?lAQzg~syQ6RnLeY32!o=SPCE-A`w>MJqQbFi2cE@zH}TJs$ai z2p(q&x3%`$nmSLBz9y>bXbkYxA;c%KcSwGGC0N2+>!*DQKxo?0=4UL}#4CCjNl&Dg zEFG7Lz7`4%=B~oV_MexOwb2c1`s-Gl`u__L;#W$>>9vo5LSt-n{f!|pSr_W*oPT-& zcrI7HKpP%}ZmS;VPtEa?Um0{;AW)G*4}Rr|CfZohLqkK4FMV5Vfhoph)Et>gCZW`G zM}oyvV|^((SoQ*xA%dSsel}v_osMeG=?x8iG{8rr&0ep%1(?SYy=2$5R_--x+wR@$ z-K<}L>WhuQd*vWcc=cz)cbafHU${b%2eSw31xE(UJ=3(GFCbVyq3lL=NC~un>>i7) z-BO)RW0So3iKJ!fuWU>4=*f|p!gV%FU;YfI#j>`^-;1xT+ zX=kC%lv?+=Qv!+FwV}c0>tIuSJ_`y3zr}d6Dw@0%r61H~6sq+}djiO;=Lz{ZK#F&| zF|&7FELWxicZzmFR7iQ&XjuX_nYzg$}ontOsp)4M?h6@9vh%nzAj#*2s%h#O?| z3N@_>N1v+)TpK5;K8Qs+X<5MI5qD}gUhVpy!+iT3vB_4@`dzd;; zrf@!*4gW65ymfQbHKqVeJp1l_cEQ4f?}KdYg|Xp?J&5d`@*t=uQET^($pp(Sm#H%^ z$1d^~4|P>IS{3hjKc&L9xEDN*@t)9YTk8306!s&o3*w90z$BvU?&TxZd*rrbgZoC5 zp0CE(Pks9Cx1(>#c-eLAXtf3H!hGU6xAO16a1XzD@xJK4?bCoV(9xM&p|J8%>*r6Y zs_I^Jrv0)eBG@jur2h=AHwfIWr_Ig%5f={|y&*W~H?Y_z zjBY-`N;yrK%vHAP87bEuyS3lWlVHVTKTo3Tew_@Rq(IZ7B}%L%X{d+n!tRjAkTz3f zqK~}Kk~lXvx9*}f7Mb%E$8XXQ@dWMdqT;(5lM%vb<++$yfQktC8bqm!oTkY+B~oNa ziK&yC_n1dcIBOr!g765#)M) zfN(yU3dUh2v=hixmvNyHljjmyBcnvU_ft4`G@vho!3V7N3#Ui~k(X_{v*Ufdy*yC+ ze0zjikZXlJd@CIedtV0{vVOk0N3*;Kp53Ps;PBEB*-d^bEyc7ync>7+ss0+#gfb~h zos}rA*O2Ea%1$`$w_xoJ&A8&k8{6Is!N46j6$DLPbD$YBZ@YA=ti1lOj5 zaYU0?-t_XOXnD=vSFcseJN*`O1^96Cj<>;-)}Lg`ici^>KZd9>r}xA3%LtL(JJu8Y zoQOV{Z158fpR%8A9KTgY1J8{W=TAqy2orCpn#^wWw+lA+Zlik_OM14ob{8A@knWq{ z+w`o~)_S%z`m9!p&X*@5Eq>Q=cJ)RJz5iJPslJ4b-NIrME^1d50yLfomNBBZxVUT@ zOK@8opFd{fT)Ac+|{FVaKYZz!-4T@ABUGgV6zF>VH1vM4WL5c>1(zyOrCsH zCPmj2i7?SoX4r-tR3deMP@+Y&c6kn&q0xUFdb)RjqV_o(5w%*XrO;bR6_5JX35UQX zeT?2mFcJiI9{>v0)ko*>KiNj*<(ZZlAEh_hGRGZH6*?sXL}5_OhSs|gy-wIiMn)jM z@mvq$Gr3yVS+$zcDB@jisrFgN9p{Qxf58vM&>vb+*t7p-8Z^uvV$dM-od!9c^dieq zEu}D57gbW}#uTI575b}O7~$3p<&i?w#U$)39f&PE2OZ|!ZarD?x)4Ll`QVjZ#a2o9 zl({<<%?oD+MZM1gS*runp7K3#W%*W9aH4ZV>1M2IeP`59F7kaWbK0jSBoHGoRl$Y`G(4!iUqob9Fgm$GM^x z?xorg8>-go*-N8K_;;!mMRag0hbTBvR%eNXyi~O|mY3|9RC-rzyHi!(EE4lP|I82Y-;7>rHTN&bBQE- zBUeTW6#x+&9DF^fwIq*H@3C#D&6R+BU2crq5FOM_wgFDN7Y&tc{kR~iu$S%>f@Z;D zmSB1vZ9tEiQ+rFe9uM8leROe&ld0Z+G^g8dr6KDoz^zi-!V+$y!eOQh`}j7y-XMLD z%>^U=TEd$sD$zsY;Awnu+O}KUu3elZ+2}O0If|c{z<(@@70cg%xJ%$Qgy}!B`JUc{ zlz_puj#&fP4sUyqHjmlQf+l}w(n?N)HlNAQ<*bo;aLYel%{r*~)06VH-3Hr95l++p zfG5UbQl{xhAa&I|^EId{x(z_QeHnXV4Pm}s_uXEA=b(r@SfLQ~K*q-#e5Gf{Ns#T7gFM=oK z_f-BhWuSlUYg64$uf4LI(^x%+rhUP}=(7;OfA31XX=5@m#;vk*B5K$zgp8{(A$a=2 z=%cU_OE`&cT8NzG)kuZ@?nA99ou1-w6G=r5)~OFMS=LeW?FJ{8`4r(c#Yi}ob78Jk z(uF37$xjaQ#HuX(W?y)o0=I z8|r7fX_+0XjS9U+kPZ5njUGmG#pC#r+5Lg!lvX6j9GG}D{aQKEvLewD%WDmB6VwtO zCS;gJpLCe9cYNjx!m-=2AMzK;OjojWYMo{uRBBw#2WfjB4m8|_zdiq|@28M4n_BZ! z2Hq2WDctU>xf+X5G(AI%WaJST^5sc+4yC9E13?(kw9D@ITL#Hz1Q}u3dc>b{Z@@&Ftl(f@#qR@$q)IQB4gx& zZ*_sqef=h;bqo2~d(ya%Y-QnPTc6qVGdV#qG_O-vx30(Ih6pl|UK}z$SrB-#N`C#6 z_jJ=uLDf(W`HAW!W=-LA@?+fF#8xc9j?Fg_r#@dFsysqy5H@IV0QsqOR6P!kEx1I&{`uO4Axi058G4f_ia%bcm{i>XOIC1|^ z(vuGxGz^1p-{Rj%!60X9xoJgitVxB+$n6jHa)IC}|mnIEaJak>QBLIJ1?|DX+h zx{)r5;tdbCQ!rl)2CO#_jRvdd^y_i+vJTkTx`TAIzI=MDVD-3gSiYL;Jnz5QdF3yd zU%$3`N&kiQyip||8LgBh2F$ZFyPsjB1|6m_!XZRMPKKA8oeqE%+8MSTjyuvJ$OI#q zu>xN_i8*qX7{FA`g6&C(i-3&~8GVCokl%QKIP&v=`kmjL_gK7o&fuA3peo~9FFot7 z;-SR8*O+D13cm+(%*W^8k~3=c7TQZGJCn9_#Hcm0&-s6d#-_`)0-PsJfqzxChRfsrid3$15QPb-<48 zW)eA5RpxR8beNX&+iAP>6%IuvaJ_Z@U#cnQ`i%oNK|%1G%Xm;uQXSf;8m8rTzdw`P z37gBFxZA+~D9B_6I)TXNOfQ3~qm@lXq-ZYgCuWo}Q(DAmi#=KwGX%^QHvTlKB%#aL zHpfaGL(*IHe_CvZIHfZlRJE+|7UL40=ORpHnw|li=UYTC-&{`?Z7wyA=BkRz%js5p zuQPf$enzp!Kmm=~LsS_d%RyUQ@}f&>ME1D#={EVV``raT+tp?ytjyO+;qU zP1JOow!W<+eC;SBVDkE3cJuyyyl&YmB@B8n82%ccRw_sbY^2k@^4R?bd1QRF*kUFk zJAL7=e*))PGY~*NU?NRyvx02Bi@3#=w#Tvlpuwptci`8X{Z~dWCbnHR-;q-B>tht# z0n@~c8*6`+m712ioCP9gFXs=`T9@k*%iS~|A{LAUHa3fXjYvC5b*VSN`!u&R8+W+B z&HyRtlG%e+gI1)&6l8vl08ry>SOu0tniA6LN91e!_~G;s`JB?X2j|GULvyR~pUz=R9GsI(0m_I>gW)P zc{hVl+Kh&XbyQ-r<+DEIp#cZVN7f_Rd!Ul0ro5}#&1rhmKYTvJ+^$fqdX)g3c0*F0 z=&dxYVoSVj^Q%QRK#jFOlW9)=tWA}I_3(U@Q*G@mqd0v|G85;{V@*xpS7|m&koEl z-EQ&!qdwPf!d{5W)LSulf`-LWe{!M=m)pZvK7Q)D^#4Bm>EDu2*fCqc|5sCsA8(H7 zhK#ZRH$i)^Z7%3?)Bag@f$){6Wd-mhay#u>&l!+?iZZyrI?(q}`Yq|j$_`Il2411| zZMGPx;+R>qo5oZ+zoDM2mJZnxII_iTa?bscUZ zfx~3a1S}dQ@>#Rj>;7z_`&Y-R18=nzl6uF3zYE(br@H_IxG|as7^{eup4KwmV)@o>#}gAg=&GR=b`*N~%?*uY7GSvQ4xcGWaOQ^sd{W`dNdiJ^#5)$iQk;aX{m7 z-kI>Ryl6upUWDg%A5hKwP;DNIzkL66@{z%H7@_3pcae3-O^PO&6m1-`OUwOwRviu| z=(zoM6@pH;|SxE`8WY7_t%jUP(gSayObvB9UbxwHdwWVlw^N2H;WD>7& zVfXylMLFi` zI1oHv_8~etI!YCxX&fo*>IOUDGr_D~y59c$19&C~iz*}wO}H%R)> zggsp-gqzcDDNbL8A=0CgwEAgdJG#DaI>-uC5P?-{N@!B?VXGZD=yWHFmY=w9_6beA zRK2;~II0AEsqotBz}>(2(m8!xhE92+&IhB&IhRNpcFD{HZV_X5+_uJ^WU%K?p;zyY zWsJO#$nnl-)vJH@6pnV?aN(_P)R^zC$8e`BhoWx)QxxBi8T01o{5~oiTVR)s@1~o5 z;oCbq=f8SUHo1`TIBmNisIpy2y-u644(iGA^vV9KRf#1P5*$PrxM11DGiGedA92eK zDHYnNwhADTZ~84EG;kkIV43v8flCa?usZL+xHS!$`)=VfEUn37v>IGPdpgrtI;;M*bL%of_ z@!9lvJWdMQ&q@dQy?{B31kZ7j?^&5%aUq_d=a#QrPF*_Xikbv7a)XC~>iI16@$4%R znmttCmc!<`yAOLI@V_nw>}e$$Twv2sX=3R$7i~QTto6Q;-KDC<;VGh85c<9#6=Lro zcq33t)EnB~W%J@5I@gIUQRPb?t=?NIjz{9g)o%z@pSHq5AYUY%T9rA8(`+ZF%gOFeu+8VoxD5ou{N;0o4oC?#2`KvqP)lPoIkS zI6DHlL=<-S(AtY{D-0gL=M_*P!p5L~sxT>`purhQM!+LBR(;^0ZEYBTk*W*~Uj4M& z(U|#pFlIYgDTz^6l%B+~7B_-7aa3hGg^&eZ##c=dLk<^*!x-Lbw0yge$zdm%|7X?*cT%mfp&=I9BRy7PJclo^co1;1I< z-&b6w&Z(|2+bOlQ=S72~iTdqa7f7P=td*%JGx5vo@qOr#+We`EdYh$r~2NzCnYe}$>wy&W&nbUuIw*>p7}!+kMCw&YGL%0=~tR$zS? zRuH0VoT0!8V2JOrC~!PWR^0zw!=X4)>QU$Z0=D$-660Yq=sCxqBAfXY&mL#8v!rk- z?*aeepf5M5*#7QYkF%<=8Y6P%uT%;1_AZ1-So?du2m8kY_-v1B55n|BWYR2q27=om z8+RPdc5L3FFpnzRg3+e#pQEy4WqRX}oU0}t;xs;5hHww^0twIY93Ak#rMFO2BOjs4 z@gD1VAQECp1Iri>0AKua*bQu29)j`SvDlpsia9#Et$qD3T9=KzvNW@cMo|!BS-h!( zgkDhq!I#WJnbbF*m6UqcyMxmBJXl_OjPP+1}umncv zU(UG>i>a8HN;_Q`U(aeSt;U-9$sxvTzypJXBdw_TFm}Hl4XCkRY>HP91b;hB$K`Q3 zncTk1<#(NU$@%|@&0YfsxX-Gp{T~$gggbqo?yjr+uBy2Oxi{mIas)qpq*Gt?dj`<&>}OaUHA{c7WzUN2~3;iBt-!;mXS{ zmYwe5eotN`=R%M^1NeaOhX#d4pWpp_@w!8wKWoSe8=jwL|C#dDdurP((ENt43%Tzp z6@PGy7pQKtaCrR9;=e!q&jlNn=9vkw@GC^R(R1EFIH#f8Mh4+s(DK5HeBzMJye|N2(a|H{Pd=J{DtNSY});m2)# zO8VIG?Fv9vXEU7_aA{VE;fC}ohGTLTk59-{0A#;zh-Cb$Cv4wl9_bceP!U_VmP6Et}1iO>=a2_e{vj8b45$>LcJDNnDvsap zM#)Gsr9Z|-n@J8!v10LajS>c5dm9rXl!AhSh{))N)gR0$7HDqUs40b8lYbtT#%vxZ14AT3WI^KjW(Z z2@{nPbQ+O|bJT3Vj}DnS<1|OTUcOmR)2nCjI4x!Upvmum&ZrPc(b{FDv~YeR;eB@! z5LneLdDFq&#&MzRE5#Do6!cqt_}t!Mmh6@ru4zmcbge7$3%}sWDJXP{M}EumdlckW zY7=wY3Znt8O2tD%OtBk@!>b)v@%iF~HMs2t=q104_OkFzmjP!{D0GL9H9C^?Z;O@I zzx?7G2kHCub9QF+Y4$DgpsCeynXyt4h%e{H>6ja7;?>36A)J~gG;MRYxU{4y++eb6 z{WeHy0gDpqBw9SBTq`JP=!E2WhiKYAoj~zDG-pG=uV3Gah``?%{@N*D@rj6=(z~+K zK3F!cIE0xlJtYEQr6gkvmTKm(*gP{@^QVV|Bdz`7VLO?R9VFbB+{aM$w$ap-&}6rQ zpT;UT8I4zwZG*G<6_N3PY$Wu+)D|6hYT{@%p22rrikH8~Nj38nYrYIcgQ_;0th z^cX;M$D@#F{{=Qffv096cikp_E;YF!Ss|2vz?>J#!gEmQ6W?bzP&pR|V_L5{hHWq$1lWTgHerwlJeqcvHoX7#CPX|DJ-4h|8{N` zY*X74!&SSV8|{u~3{EQu%BiTR1k7a-^x~>b;Q5a~B!-n6I2!x-H5@CLywVCZFzS4q z9WKj(rgVhUBssdQ3)kR}0t<@iwp#)l2Wo|Y48f(uXY?T$ z&Dz(>TNGo-R7)&p3NzcRvFcSoEtvp;Rn>I?W=p4=l~cJ+Ms)cI~-I7HqrZMdTN+=PrM>Z(H4 z(-2?Kntv$V`NLVBQ6d8({yl0V@PZ%YVz(JwY2(=8Y}cRRH2A*8hrE&Bis{l%@S>B#609oO&koH&rpGWMo8o#A)3H0K~dyC))%K<2Msfk@&$Me6j(*+bmZgQLAcct@`cunU>N zhfYFyNXLQ38>_AH`KsIPPQwBD-7)@Q5_fn+eAe`>`kdD5W z3+NXgmv;`kDtet1Zyy$9>e|0B8N78MwInz-6DzKL z>tW_-e;~GcZ%v~fHez6^@6KXn<63-8IT6Z`G zxA}pbI?H4Rl>KLmaj6=LMYORozM0YsykOrR@(}*?9*G=2StuebbJv%Fh5{`_oRw6b zRn5MNtbvCd!NE}svy0OTQ^`jML`DG5L}y5+Tr5wANgq+aX1})K?^Zn>r|#}<=huJD zZyNn?z$Effg&2;T=#7V$SDoz&kLT^_`0+v=0=jg}lP8MdPMd`)CwA!QZ~}ov7bWJi zZ{XN*rljrlgqT^M3HlG@%Q4@ZLZM(v7kGByjI<_zO1~e?doZ-^UP!bJGCfq8rjnD< z?}Gh-hhL;(`Kc{^xqn??{0#C7j_|LV)i$OrzrNKQznN=iDz?KSCvz2!@|_90NJ8`J zq7PiXw0p`AkT=C5!2|5)qxr7AjXZKRJ%>Rx31;W`bE7cin$PbL?au%g zQ(wUahlhtB&({RKA_W##ln!4J6qzm3=K48e&xq%At~7D$Rvnm&GI(ALkAT$ihRdBV z=zSv$hJg;PMsuA8(o&(1M@iYsxXnt)#R>Bq1%P{#vzBT)62``I`)fQzgAnPWYS7o{ zW2xq`yu=2G!-mo~R1Ho{<8SAvIlQmoQEpELTFUv0Mo179 z%MZ*=nR?VUnaYD-)rhYUzg+|bgUlH*K2Z?enT`riRg}KXOqJ1kEj?#RQT8TFuzi8V zPL0B1XK)u_R6F%Zf`P%L!i+Y$lX&>@yl%g0tB8|x6)o#|ql>(!&(q+~RC?ci5&a;E zhy7_?ykEoO0sQpQy}n%jm#Lo!47X-^xT)?z^~~ek(u$2i3lpT5B$i(0@zSO5YR*=Cmj z*j#H-%RBvDdIyS&rCJ}<#V|`>-~6s#SzLms~tkraOvhZa$tX zO2~tYQr^wn*FC2qw9ysQRJAm;VH53m*?4oF;Xm8U7tJ@7+Znn~VNO@G=Oy=?2Xd}y zJU;*nNb$9Xi_9}8*TxlaCYbFlOG+o7+-xa_U1eVn8~LPV54t1 z9<;TrGRysjI`AKN;sN~Bn}exhQc_ZJcHiA)(VC)kP}{<^gC1~?uMIdACn4_oRyxUs zSM!TQ83Iv@xX=mN0+&GNL@11^&X2-2qDaFNOtskw^HmFLr3yQ{g&lsPE@9>Ejc-bmu@JQXH}S`dRA0FT;lg43s^l8Xv?x& zgSN@{Gm7+y=K?7%S9heYxUkw8_>=H7{XPTFnIb7jHS64w%dR-S22Ndy6cV1IHh1Zg zhO)c-65NF?gfd6duV&)+Ze*JQoLmG9eHKlnKC+n9aUrc`3Mc|_7NRv=<*kaW4w4^ua z6fS*qt49L)5(BM7&Z%!{D-tulG#`)(kw>Up(dCOG$Pps)S8omwa31Gcyx$dOv;_ov zmFT8S&fJi&UvyR!aj)SOFr{>)%vfl+E7r-s-}kg2L*`fO+{~5!w!aJ92RZS^W3?ET zHJ%R@HWb>*A8eBo4E2S&9&Q6+2fLyNk8OtX;xd_^QnwS#2W$#EvTtQ7TpTpQ`Z#11 zn}=?}w_R^Zt_Vy{N+Q>Nb}p8B*LdSmm*1J^%@JQZtBdXhU;pko^So<}g3xf<`XpjM z(y(UDk=KFmMFXf&u|&bzE_5Y-816g?Z7dm&=ZCFjYv~*v!?ozP|o5$)BCs7?SiyJuzn< zf*x_W3;;l+M0C1A{brO=B?6=x0eQTdRStceolTC=WqXo}j;%^#=fP2UxZ>W^{f<+` z;X>l}zQ{mX8OH~Gx(05&_Jx`7Am3RtnZKz#p`xLwO`-q$6$sq{alDduRNJuZMWYk^ zG2p7{xR9C@tY56eYphvTV`yTVoS%sIKI?X|cZs49qL~5xGgv9|iC4dqPc&(UM4KH_ z$gIP6Lr7G#(Ov9qX20@n{xd3^xOl1mBxF8YCne&^AgReyhX(|NcQ=wtkMr>l{q6T7 zlY&bqRbckD+5jSo8y`#P_WRySxI_}eVUx$2X0}om8R(QuG=I}cG)m!5sr?q}lkV_f zCYQtb?q2TtnR2?lDfc@1s8MsWW+GE5g__r+H3GMzXAkb)#^iYeET;EDsGV^Y%TKHv zWdjqGnx1`|c(Pjl06^~?hcQiROX1a`?H%jZM8$D`oqZa`@F&!*OFYSl31~_+I+#7D zzClWQcj`c6Anwk_;A*H%p##2i^bQJSi30L5d)5B=VVx!#;Q^GJfj3R{U~ePgqgcFO5d4uCH5V>-!tvTy3<(U)WTZDh`y#nRE#D4SA0cn0VblN z9h`J+)r@APnf+qQ6giS6ZawE-h4wpuGq&?ukcBef8OYFp$Q7xic0C4Y=J4`5?Xx$2 z3box$iX`78^$#-xB2-%uP79x@YinsV_PQgpihTk0HG>tT(cE&7k{P>pp|C=B5$g>m za;H62k*nAK3(%*acQ$Mfn!+1vNJMxdob_VG)Az$aN7i5Vg-}9L5@7!0B|QBX1_E8H zG8k+$X$tT<@hng_vA4kHg0-x^mYSZ(akE?6ytF|T9+J%4dA7{c-k!O00Zl;$VagaW zWO+C;2hCz`?D)?S@fu_nvCxk6-;jep4l4Nv_kYd7)utn^~EiCKz%!0Awm& z^+?NnF9f`)sb_cECoR?E2$a(IdHV*gz3M@Uc>E-&lb5fc-9XvDx*D!n6jv5W$Wox+ z>RYN=v+-^IW?aAEV4}3Jm{cn?*(-*AecO4mbv9Pf3gU1NE@MpRh;P5m+j2VZ6u6x& z^yY@j)9W+ax@4{=yP=Ndg3&?@m$jSm?zvq^iwS*V66>}-_*75(w+tAkJ|f!Pi>}W2 zj|=qQh^(M^tvmC~LO60qv`ctY{i&z`ZX>j>XVS^>gg1Y_@@XuSWZCz<;d~tfa}pT} zU9yd+kE`*^9Odup>2Dv&Eygz(*}^?2dJ7Rrm~m8GNiKbVpC}0-RUvL!i3PAF5RUyE ze|dC^%!3HLF_Lg85qoPJYP{8SdvR2Wml1kVYW2)kmBOTdD^mDZjg;063Nr0(-d4%Z z^u?aGfP;da=PBu6Uq8e2*@7TSsdv(yoh=Q+2)7aKeRDsycC@6Xvl7q-F2cTS9#S5y zNs2ser;&ILMoge7^Vso4dOwm?7% zR$s6G>_WGRnPQTTKzKG%eKG`Kxi2$Wb{S4bb^Rk()|SsQ&TT5dC7uR0_?qsQe2?H{ z3VQm>y^qd5!O3Zz!_o=UpB+ZEHWgcZEAe7&b_YLa_Z(NS(`@i4!4GIuTF=Xe?H_(ADdD@D>}bMWG53G@Tu(4Dn|Du>Osm*k@x@x)Q<`#IF0SHc zy0XK8gEv#s!?K3XBgqyjytzMuLAdo*r_WV(etcdQHPM_brimf}r5?rJb!3c|9U}i4 zi}>R6ljvae+Is8yb+psC+)~W_#!p(?4vOUs(kHwh5*O`!+BROn#s=qtDz&fhl0bGl zQrJnddtD>K6x|MDd%|VyW!ty1>2<06sDiR42Ug#kcSOj^`9A8&LG+%HdxlID7>c9l zZ3=FvSD@W)Lw%jZklF9g@qGyG)(0ao-?%lpY+W|f5qD9@1wZBfQG1Lzarn`UX+%gf z*o%O$l0hU}%psP%c+XFjnFTR}kP-fo-K_f&me zcZez*>A_ArYJ575RBA-bHzaD*jZdrlheVs0nAq6jUhupAfESKRz^5N!sv7}#ai$c?Cuw8Qk4&k77tJZ4zI zR;=#9Pc$t|^!tJKb_mxMBo;zi5F(4>#E8LCy31sdpp|o}3 zCtO%V2~jRl*~-sJ-<6QqqnE#D^5bc+MC&K?IWsYR9rr0kDvhCnEswxJ2RAVD`mQ?L zSW{zTruMaD!5MS|NQmcskK(HV3a@qQ{V~&OU(tT>?QNtGRZ68hZI%An`g-#D@$#;5 zO0x8?YB#Xhc~(Vr)gqpEowFdR`p`zI*`}^{qsd66VSNAq;*pvREZk+fH&yQNG!sw> z9$NFn)6BuFuBsS z-&_hlxGclfrRc-)0_Us|lz-w$GS#Aw;&q#x4SS-9#oxWd-+pb&#ZA6-z`97jXu@C9 z?`X!BzhFR*s>V1WM0j8|EH-M|uXTb1E0u1!3H{K>n*tr!Dmp3kq{3M&F3kcZIuWON z#3wGLH=yCMSC*n4al5+)1lH&W(xcetd!S#%`hj8|A!+ zuanvQ;-ag|d3fUV@l1O&39s6B4m7w7`kzdLHHGbgagkSc6D z_A}!Ow7bMsrqGc*-%J~IUQou^!+(cAGQNAW1z5Z?f7Gv7!R{vBi9Xzpf?WTD9NhW~ z`Ha)|^RHKBGkne5(7ku}Qz8%A`{$A=KthXG$Cb$9EIMsQ@kC?gx^fXkKB72Jqws39 z^**`g(~9;@J#X<0;a?g+X#+Iy$$?yHDci8lB2GxI?FtqyUyDgD$e%YZnBT18CS3~k z;Mu{so-B9MmB%*^9hU|g5Dm|L z`%f~1JkApN&CfCXU$i=&UZ|@ITl?D`13mKYNwVCb4lTQlafGDq>uNSD9D#Jn>_ONe zx1m%P2zsCGCW3JQizX_)2pGt=zA$C-{bN5c*Rq)&XP~)&Gu9{!3pkulC2(=1?0hY- zSt)()|CD1}6_Ev*HSkDps*L_L>n2IFm%^ay;<~{G!{7o}t6tE@>e9$5(`b_=o9zZt zD8-IA^7Q+!d!d>vn=nF2X1?^TRSMC^3$LvWjMJywEpCzpoHjMa*xsm?;V74g8Dacc&Mnuf|xVRln!1YrA33i-_(9x^`)V z*WmXF|NK9Fy&5ywwHCAxc(&aIcNS*B=hqlHLGqSCJ7tFjx6o#^{3{3vq9iy{r4rsRs;3>*I&Y<;H2mWxA5;3 zXZ63SZaM(I2}L;blA?=4PDh?L$%5dQXQx@`{zk}9!II&h%n?ppuHr*Wi6d;dLnVJWq^?L0`IZ|-E(9dnfyV3f?H_BQ04V@p=N?i8P|LiUBI2rz<_{&Oo?X)0QaAx=q zu&JhHBca+UPu8Fg&rsk0H36!r-JN!*eVAv}Pm+yavS z`3I4Q^)0=tdK6z(RTV`cOb*WulKb5_d{ZfQT(IgB@{J0m-2-O2J-#Pa%Ea>Rp_wwF zAv2yP?Bxj%w&PNJ7`&e@b72Ai?fgTYmp)llIx8dI@ZBiTVUZBk!0-a~+1RPx>y|U5 zUH}uh`}LYpvG$+kC4M&x52U=NrhbNt>)6dbI6S;jR=;S}9f2<{Dd}`|Vu6ZHE}P7( z>%LpndvI{@X2Lg#D#4T?_TVyvnk&P86$SkM@|mJw3SuV17|pMv1qwQc@KSkX2BvUm z_|GGZ2X(q*mW6Z~1`KVrIhz?Ha;0OAS64dTKmHCyO$bS5QQIRJym6%ORjO$`jl=oK zX|7{k`?ngPPhUL%x&PNMb5u-{uC@SV2R9b2^yT^`U&zNNcnA-~Ig3RwFG8)=)J7+^ zu*uWgHa*B`@MB1gs-;jfAJ6(<}q$72lAUse0ImB;H>e`Ik$W|{TG4YB2${IGN zt;ys$`b;el`=`~!m%6w>3r)Y4#y<-%tk3j0{U;1;Y+7dKn9)2*beBw}6 zVDm`8^FOlLJ~Go520Lb>V`Ap_HdZsrzu+65@)~n$sU@D4rNz?cX5KpHc)^%e8@yTH z=*a96nhzL80t4V{j3(Q3Bg&dn0IbpKf+k#{HFSta8+NBISt|D)9ur=%N#V461v=ZM zDRQ>BI@|6}s}O2qA6jMB|F7of!oeXD3L4tl-ri7?%g(O_5O)o5WSIMNR7}hUT*>mR zrpg82bv@q4L)^QwwUKa(w+e%ez*SG9-u4ANgS!r=Z031i9$}Dj?JArP3S|z6Fl^Fr zRBZY%M|H9D`Z6X4X)dq8-gmNZMjJ6{2Vcgj=Qa&dkAD}r3HEA|&tA{7VQ!P3`z3WZ zH-XoCr@^Nn^C20YlDdI5>Xc{1mC2*6y9(^j)+Em)y>m{EF>o`Uym_~~NE%sKh~@+P z72X@SXOf@4?ARF5==s+SUY5{1zf4U@>4Oi~d=cyck6G{hEa`Py^68Bt2Dq<9b0ZLO z+mc0-a3XzylT9#)*+0Ttdc@|4g^o3Qog=9jW%9!Y8kSeAeN&nfr`B>*66}-C&sv>} z4Lc!Nsf2sDscoqrLMPKka2b8VgVnB<_v!lD1k>M zocCrEO~mi=P+^IH@tcZ|QkKhhplZy@<;n{>ua9Uy4X(l5%o z85h;xG`@I5_nr8@S*-zsn*91dCrjXDkS zxCW4?sPl_KGWhfvpaPKvzV9&@v|elMjN`#jM`-CN@jqlE{%HSG1^|fl>eVaty&kxE zFc{5!r;!wFNOZ~4kTMjc4J&FN!#=Bg!x{N#$TwN_e@C$~*cY%&C`ZY6`;{;la z^=np*QlM%ZxV@4=K}Rn%?1b7Q7-R{>*uckevO65I$B!6khJ)F!cTc6J_Bv!JWPGK~ zFyJ5IK1!vBae&hd*DoWUOKdozGeYJ%N!XiQo}D5O^KXrs6?$L3ZTfliW?ojSGg@td z>f&UBT^wb)ZHsc~QI?S2Lgh)VqQk1KG_D8%YLj z1-j?+g8nb|h-;<#P3pgYxhN#$etq#A&Kvr39bqdi|3kfg+n%TYZWIbnKm(?HOsqs@ zUJDAvw2v=jl3IOUtJNmaf#E*vOjQrDa%ZDdvw(7LYpz{e+iB~o7q9=ygD!1{Tx#*I zf5dyXl?GE3nW)B@JJuVNi0u`k4`YA0frAb+;Z`r!v-|dO6S7&=FCWGVwFC!*ueiuf z2NIsUrTnn0E%(X!Gh)i4ddCH1Y1$8h)7btJ_{f*$3$8j#Fax7l&neL7R`T(7Sh_+r zA9MPc9K*P<_yU{?rer_7YaIhVrxxNVo;=oz@@^t$N{%$1n{+x!NM6|kh5q3JXk@NH ziZAj6*r1>tmH#%1zR}e=e|rQHv3cwVB=(_V061A8*^=8bF8ZYfEbXpC=63X8j?A5u zOuPxm7Pb@2u$b!^$r>g1`-DANse_GxYmx&KRsNn+Mo8Q+;nMDAs5R-Hm0GS}-zmtY zs?`+-r_tztfujVcd$xKW7&>w#t2grFEl(CaeXoGER03@9twYrW(_dNm*-Qz3V$>gq z%%*6G<4Tw^n%y<1F*MvvrYb(U@+8zh|E9d$9A9s@7E%nfUy+Bk%egmRdAv%68mvrU z3N&^;GNlK&X_#8hqF0yn#{{=JNy-waluHkGLf@o=iWz89DX>BQD!#MH-S?`eyo73c z#0i3sx-mBgdg4v;8pWn47R8ku3|sXiPd#<0#5>nLJrG=7SPD#6E88}l4ftxR5YmT2 zEHpJ5{U&D|gQPnlge9)%X1=_M#>F%9SACB=NI015{smRuK19!v;4mcF$9Fh`_v6Rs zVBHE)Iu;|;`t=?KZhu3_CBzJPdnnuF)12(lY)N217@3cUrGvne(tN;!EufM`-T-N1WlT*!C8ekpb4b5GY18KHgs+p5+{S)bcGr$ISAK!Z06`RZc!r)^O$M zIf1PLa3jx9T&MdQf3ntl51w1`(I^NXXJ@TpuM}vmnzD{C-mD-R9(2NtFL)GEPoZ{8 znsR#U?Yg%pR{PeD#AR2j^xxPJssGcbXAjtLwL9_&Hn#Bj1-u4uYwYX$oQ}R4`lKV1 zEx~Aud#D;l`U^tT1uGOKKwoSLggwVqz9ZFxJq~+=!tAWvd02x>v~$3IX~=M1^XCixPv19Rj@*9gOIl0K za73DZqt2VszEHyUrJ9s5V;MhVZpeaX=4{7yKg}yGYVFY5vl17k*1zLpU$qtb0z*H& zBQ7d~&8r<6)Nq`%EuaB;!OvXBxX}4)NqIW`*UL^M>-~vh{VbqeiPYz8#9@pApBIqt zJ{DwBuF#KQy~Xbq#63mnfknSiO$v2SX8Zm05t4&AORx?Wzft=N?;&A$-!>O%7gAQ7 zGfF~{Lb~Ad1O5Mn7(E)7CjS~Hoc>Uz`}d9*WIWG8J4N6GCQVJvuI}y?dmeaUp+TIT zUfFlT_oyMp^>fAQJ;~UhkB25Tj-O8*sA%Q}-F%LSfy0D)Ty5>#!Zh&)fq+9Eid}nn z;S#rl3vP1Uv54i^u!g6fYZ%2U84p_<)ZjWv4CC@;Qp)#8m;r6WXEdmchvF!t5Af0@ zQ)qY%23uXr+{&nGi;=!*Ujqv{FsxyNDl4Vrc;3nC$P-z()f@J%U##FoT{=?;7wT_x z&ny%XDkjbHx~*tbM}n7dkUQhLtE@>y?)EQ83Br4EJz$Z7jIvd2)jZX-;Y_qnqGBxA z$g*glI`Y-_E!r?{M0Kv6I$FH+-Hu>P3htMHpltQs^_6UZ{kqFs;I0*~CcqAG&cxFW zE7V0n^-}Kcg=g}DPC-h%iTAM@;^O6$kU`=ZOLzAw-yJMpz1lQQbia=WbsDy|8d-YJ z7DF_T9XtBIjD6jjDsgDRo>eZbjr|hJ1GDby^0*VES6@p9rdO>tz6MYA zQN?mYOy(lQx=fJ2il*>X<7?~1n7ErF_bt~@#7haZ=Hy%Bw@~1BhtPI72KEdpdwUJK z^p_J5q*@S{YIsDIAxsx5&HAQNBt?hlrdM7jZZHy&v}g2Mj?SRl7aSOO1XGZs)m!&% zC+BGpyznPop7zT)-6GuodDpU0>i3lwj|3{-=*5*u8HBFvz#~?k_f2&xy8es?LC#%Ja}jLM4}Um2$P@H>WU_gkJpO+r?pT$zIF^1c~RBZ1u=1Wk}i zW0p68dT*(hbBJk?8B5E))*lw`8wkR(XBiTc^U9coWhja|cu5GR%xY~kWg+;i& ze3KHud5%-FU;^8BW}kI`e)A}0=EnCWss9g&obM+9)`J`%gYY6>Mj*o-2O62_8@EyR z*s-r+CqWCv%(zOjjKk^LY3?cHEMzUqo?hJ%^ZGOLSxoU}Tx^kN2i}9k@_Gj!Bi9AA zNihQ%&_09%$SO0sfLc0^FY{OAZ?*I)>G0XBd6d5tq`W^L67UJpfTRBwOtABQei%G1 zQ<{M#%|H`gfJs_od`=!i7HxdKsIjrGR|7}?sHlP<7aE>5l#QzC49+CJe**2N^RGN6 zl@$boEz$%|CzREl*J7tAOm|1n+Bn)ds&wS%b+E|op7*Uq2OTf9ImWqjeL8U}g!f*j z#E=RX95$o7p=fzrK>G5ScVh z(~AN~I3DuM9yM=%z(n3z~9n=xumPR_fQEkaVP;{gS)HfRT{(_IILN3I8N2q!w- z^y`h8w__pMpa8*23~X+mIg4MA=Ca#^x|xF!8+Y0FPKF*=V@-_yk@{*hIGcgH`Qj~6 zL*XwmQf3_ZC?votgQx&e_}`Sjxz6>z2oj@iM#Ny=Y;?CYa-A^rTtM|c7E`&XY58pDrE_iAc-{+ z|7AeKnO69g@I`~qpvHKFNw^yZ>~2XY^?VbtIL?imy@7z7WuclHnx4pVA(13qjn@`Z zL2A%2x?M0ZAD<;|c_}kbZtZWnoP7NhFea)@`sT#v- zHJbF97U0vRLqa?C0vMeDTGyhk%{*wBUGUvxI?6U3!&gKJT9iq==Dj|jB=6zw` z&cKpg1c^hDi#}Fkpa?W_q6@SpW%^QjJkQTmxu2s?x;T7=Kg;JXL_dYTA1_s58N%}b5m7Nm}+ zIxC@K!GES}Rml&qKR;{VSm}rRgfgK&JBH+F5&uR2rACQbOErvYZRZY~7VMWNR5{AY zx|H{J#!QdsL%x5Ls5lUHmKe~Nncb3QzXd;bMIN;F&8UsLpIiS}nbw6tC#e|>_ot`2 z{u8ac9F2=It^o(N0MjSRacG_fg>mGJm!2CJ63( zDcydg}@e42{@Vf`1t!JTI=OC>lT=fBtg6=V0ie#_3ukoZWOApVm#xZ(YOKMj9( z*?Xyt7T>zB{r*1Ot=fC@DOeC33Cf74GsS0d-f8>PU04+X`gcQzxyw1!_$9jxh<`tF zv1U2ynrEvB9m_q#?8rdGT7B@+;V~P$c-O62BNa`9&yRO5KBHUrZy5dI!EP||wO^)w z`eZM6R?8SkYy~{}F(LU2XD0LTc9CvbCSe`k=0&PE4P_PsqqB{v=ye&)Gn;8^ndgAl zZNZa}Vz-Z8Mev=jTlb(zoxER`kfPxQDQd`y#D~4Jd4AVdXn+d4-(0LelW;VmY8_zW z?98EDAltsu(3BYjm>OE`Pwm_y7)?C%Sz}%b32{TSE-lLq@|VPXvAr_k)3UU-#vVXHp6f%l`bkhgZiv z{GGnZ?nuJ%3Vq?pQcWk-v+I*&Iswidi|vrf6=+u8(^@X4b8n;g<}J2EC%qSA$#b43 ztNXqB#;X@XDz5xqs!sv@m&Qb>AvhhMer_3LncRJ`4-|B4qTe$m8Qm;X&0rr{x*ccC1vi6$W!|=g%sSG_P746TzNbnUZEd z>N7;7mbSa0;`P4k1MMsH2oZ{2yO<|R{WBWFH;X4z z4mb$Wfxg?6E}8yrftSuzVW5)FJuQQ?=tZ5dS+<;<5-ht5TuU#6EjMyn__Z(#6Ysd+ z#jG$s_Whj7wxdA`$b^%~^1%H5!KRv@)D!alNp>O9U%R3GH``@J31C4t;!NHL_Ov@3 z|LnW@sOq0{Mfpy7H%{5!o^^3?u~fT0X0h1|;B{s#DkjF_)oiL+8l~I)iG{qt*8D|_ zuL;*7aFrNv?-CDCCR#W)B~gue}us}oBtf^#SJ zMm&`~_}M_dn`I|2ca!cuLVc(B#)4>+`45jrBpv2ITvzW@A2amDb+s!$n_oA(b1??o zV4C}$H7=LS3VKRgne6vC+>r11&vGj+vBYlfnOaOX;ez21gpOdy3Hkl;pryppeEPko zFk}pt^(7}ZuGi?FQxw-hH;EvAN`+Wyi9*Hxo)vDS=QCtd)$G}axkfF2PKt2a&PiD%D!9M(z$<)@VY#u z3&_y=vX48=p?RrcH4UQzo>Bz_K8bY0y*5>l_w^9Q9u`X4bXKut-yKUY`| ztxeb9cf-iM9hrkUo>$FOtx!W!&xeV=hIx9>9L}|DHugxSdb=^ui(?sEC<(C@{-tn8 zLDoF}$>G@kcD}FBP@6%!fB(s5%g^^;PY@SKKl!%$eKPZ43R_T7QITkwQ%&QcRA}ho z*}?0%RJkNT-rlxcNxROa)! zyg$9~8#?-&UNk&X%ZZ9>-YYAUb@guBVFTWQt*KJvJ3Vny zPVh&(E5pB|AITH;62`24t^IHvfM(&msF(x=#R9v>Hj7$L9QXt;ID44^IBI`WE$}z_ ztPy({{qTMw@I<51`YBUM{Y$VB&(2vzo=7*Uv(*HC=bNj>P~!9+$EBU#FmN&FN+L3A zys@Ux);r&?jOVV!A$E<Rt?c7cP zN%p(xsWP1efyo8AR;{we`ew8siG`u@JF14S1uR=TXP-P>4JVmfZVVo^L{k;*rgyhf z_|f9{%_h0_%IdT=M|)Nt+|Phu7X<^Y(QDb%agDrGpvk9a6?X6a>7YqOhZbDXP(jzD!OFCWY2Hu1EE73JbPu=dG@GH86#qN9lEzP8H(#Vz#A%n!)!Mh%wNgdoiYB@?!{ws&d^#xzg+2i`9WWA(u!my>VIvuZo>!1b!BHw zr=&nP{{1kT_NUWq>4P&1@K}5t84V+2)bjH3u5C!4%{8AbGi`ybO0m$Bb#BRv1} zXogGlNRKt$(dGDG;yw~EcIAHiPZbwGV*Z^7_^9S}h3o&#QJClccV8v(^%jS| zebm1IEleOvKtKTDF$QD2^AetaQIQnTJc06b0JkwRop%$yDGL*%VqCT3o)*~IOjO4BFzDgJ zbWGDBl9~-`b`RL;s__~YJ>wvfkZO6}uXn>XDYwvt-*)WHp$29HiZQGycn?%&DPIh! zr4`udqJuUmor3JWiT} z55er6(S^IyM-#`l#6%k6PKZ8MfUKZ>XHLbM}$+(HizG(izgRT$mI!%o-H?eGPkg)tJ6|f zT)?FLWXpU^toI^xTpF|AF4ACXpnp*3nyNQIUC zulMt^bWcYh#Yw+DnAbpCylqY8I6)&Hu!MoAnse3B=zf-FnSm%Ms>t;CtlHU!j+sW9qW?%r*BhR(C5e`?{0xg(Z< z0GOt{@R*g_xH%v!q%1c^n~&hkwTWEKxO2r5zV!{UYEUe7a0|iq2pF@Kj+X8-rm{DPXiNVT#|w(>L8s@(ZgD9IZjY4iZ9usfky`bRVzq`?S+fqxit{AqdFjpb3XClXp2+YF7t$Z{ zkobPZU|=tNU%Vy}GJ7}Rk88^Wd-~?fbGT7>rH&duN|^hq@)0tL_edXpaeZuepctjvLcj8bZMq4PyNq)L)Jg{0vqDwbf1Q1KRSlxRL35N--cFx$XDOxZE(enkJGLnXzu6GK{ z9C(kgO<+2%ZwFgFXZnSZr0gw_PXV|uQ-HDg6fJv4*qQnCHE@%?sje2rSLyhJ8mQGF znC-@DU$I_t-orT&>nL!`1@=6oZy?C+KyNO(@1Gl7n#labg(wcw=qn@~7+w4N?CFq0 zhvS{Tk@d2t4L7RUISUF5;eI$Rv_d+aw#GwdtaD5#qQ^&{hGT!8`=yd`jFX_ z!|CwIQdlVn>2~hmE^y?cP!U_EKsoN`FV*xrJpGLk^+n^ZCZ$1h!h@wv_oO_LwRJD( zVQXb#_9kpue7@#__jPlfKE7qJN^j!!XEQYIkj$?V$A#t}x)3cBscq-#!Jsua%BJKl z7EfT$^+ora;dep6t=WlhIt1#-pWpj>XsgxzjPlFuJy6ifSE2)exXyw1caa(l1npN- zmIQlj;|I;yL?5OSKCg)y`|0;r2MK-GKOxwY;YCuVTf4(t_J%5ZG?V*W3m7!j_ z{VwhlZrky^P)rY>^2#V~xFNMan{XoO^C0Xq2aef*-gF_6wt?6gXP%HB&(K@#1pLc7 z5XcWXjV`Ydde>HNPPjNSe65`374%;PXd_F5UhERF-4rGP8PQ(5K!=3WSY2%0WC|Js zrk>2XJdF^NQ-!5hr6`Yn_~ub=a-9X}T9SC_fv$`wN`fC$?8QrNQ<(4O*)4&|lf2yq zs(^iktm7~i^CfuJ3)s@0b#V`-x5UHldHP$~ULW!Hih#M*yk@POPoA_Tv^G4;MBwpn z4@PQXwx;c zhEmy7m4Wor%2450y;nK8ei36uu8gC};Z3+=qI~VTR~Ys4TqKxpkq)1nsl=MXTW0qo zOjp6{a?G9z12S^%ce?oVHr6yb-vt?C1Kip`-uvT(O8$Si0EYs0V8G`iA=F_T;?Bxi zVsLd39@cc_Z2XjQn+aFIP}0SjNnI@Fu!_3}7HOLGW=X7ijioh960=`iam}@&;1F&d zT|H+&WV$4q`M!gwCx($y8D&GyuJK~JvhOnB5HL@{OwaYnWs#C-* z(YC7|XK+jsL*5tztAW}>sPCQV@2C|}ON}qbzV2EJr0`|+L=iRwql^4R?;O*g_G`op zXbFe(WWw!>6khi?l;tqQ^m-O%vh{;T_cYJ1G2-KMZgFIbzE8cgU-dDe+pu4bx%b)o zZgE7V>LX`KmxN!X2NC9YFYOB`&*33+fnz-ud${h@C(o*I>X)0o6Q4XD5#UWv_N)M2 z-j%Ch%dHjHTbCpPu49c4ZDgh)bq{^am>|gI~ z*=&Q}I2UaX?Sa$(z+A`k=K?}RnyG$D4~S}AetqCAJ3AKH0$D=fE8xI)m_lfZ;6;6j z*eU`SmAtL|Gq2zj6jt|%d@s+l#rFuJ=`xzEi;SiDFkpQZ zw`!~n$HZE7HWEWdi#%E^RoCmMvRpr8f7Dd`Fj#maM8=nTVH^F1%qi z$>Av|dT%HG>hp>#<}#O>Qzt?+HEAdO=9u(yH8?f6ue-ua7h3_(}Brf!lqViU%n* z`JDGAjvnfdKEtDh{Emh%i6u{5><2fUji0CY*CRHEa7rCgQ^o8L8$`Z3YQj9?RsIH1 zDd1Ny?MZXM&@$=luXsANnDHJ~Xq%+M>G`ujOxdo^?qE4s%@kKs35iIjc-o6gS)lNE zv=BgiSoO%hyLB7=vlmzNTU=z@L{C6iM(0NN+QkWj&!k+%>@5s)d?`<5mV!}|qtG)``#gQ(7Va(mVf-z@78bSy^`pBs z1t|npkaDH<8**Z6^$<9U30@s`Fy9evNg)ovXZ1{49#!udtEfhwW?xdfFO*>2UKihB z_D%P7!mWpNGlXf~mBSg9ykT4k8><{vs3jY*tC1S4O@xJ)zpezdcv4&wHkaA02ucWB zI@PQE6R@Fa*lWdR-40`){uZmwqxV>Ez&0h#wzoYq3s{X)r7NY| zvoYNw$`?k~Lm(nh3c7&o96e?HJ3~W5I4c$C1`xNMV-fb1gnr|+o!757!`B6!cUe{$ z)X!DY3<+ocfAC}Dz>Ey-#shQlUNp&Wa(5;EO5)UZmkx8-ZnfU~r2i>b{=JNOBP0YO zHnQP}IqE+iYn%=gG9ujU0CGqun3;Z{%EyJq21@pSi`vKrdqh2eYY&Y#p9V|FkStnn zX!jM`tI0MDXVu^z%#pj6=eE@s@Dn1{ezlKa(xz6_YNxWN@&+#`l~@cN7xLawgDf7N4!rUzW~DUa-UGA5 z&suy<-i=&pmg)XzbDyd!@tYWa_Iy1*)5?xV^PH%x4*}cdr9b0~XHVjKr<;WXb8}u@ zq$k{4t&L6Adsy6PYI$?SCco!f>{W{l>kBgCtqsPvQW)L4#B#%tnr(13680MLRBU!- zEe`>ik8G8d>NtH$8sZbacsu4By@1CLaCdv7oP?DdVMRb7z3J4EdD7~bAoI}ou&;AN z4=q+Rk$qX3w>KNE4fOX#x-Qr7?Ql(TW7y<7C)n`eohS9PG zYoSHYv_gI~!=8@rJxD71Ii9Z&d9a}Vvm7SRBZ(`T@|uBw=X1P^g1lhzOLfw z5?^eOf0iw$1xBu)pN^+CZu4VFYB(^fWNxsu%8wLuH>6VZRMz?_*C8!;*EmhGHRL*x zM4#o0LoHQ~5sKb5_h#pr=7l@MqhIxe>ZY36jy%ZJ;+0lV6SMN$IAcXu9VDyKT-dXw zspHP5=wCrQ7YCjAH};IUq|6sn`@^AlT6iY%*02BtGj^*}wYvuM_GJR8PB`qj{D^)H zZv;O%b$xNY>e3dzx@UHzc&&`#ZiW#us?SyuPMfr5MFcp*bG>(pDN$=Heu+;~Ti~`t zSR5}5C5pL?GQTrlm2-MS;e9ETh}}HoWS5aUxBc!ry0ib_BC7u;sVa<6G0I`Fkj#%@9y z3yB)tk}7&#OraEp;IEW>4pS(`rLruS@-U7{~4?S2iJ;=t2U0ppm%UF$uQW z(!0xYX3vZ@m_=jG<;WLg>0!_&uXDjgAgA3%$S-gI|0sLwu(-Y^T|2l03Bk1!+}(o( zcL?t8F2NmwOXF?{7Tn#nae@bTch`o~$?u(+@BPlqT-Vuu@7=w-*IKn!-St!zDx2Pp zHjA$nU|8ol^KMOh%(5#Avih|rqLB9 zp=g`#=96R>9l4&_z4z^NfZ!yR*lWh`hN-{etI5PUUQ z&*fAZu26dW*ARbSCV?<8r#Fdu_jwLVHOih>3UkF*i2l&h7;y;HckuDWsXFbBFfZ*m zK;lpHw_`shVij_~po`zq2D2O!;*5GkX1{h!)NqMh$iA#Hek7S#WMXM+6%0vOt92iL zx^6xm9lii=Sx0T1nSu`*&)gISn*qB8@%At8GdS##&x8oI%4Q z@V)(1Fg^TQM{p+L`K$KQY{33GO0YF4FT$BV6t`{Ip976=+K<#8tJZP*s(2KSGtrxF zcY5fR`f4q^|8~8ijqM8KIqbu;5|p>>8%HATTV5YryPgzi0-d z!orqe8usg+-W=g`w#`OLg-Ty-T5lDfU=C&9)TT~70XCDzA<~H>2R;YkY(;sOjog z?JuYd8;^wBX)1emK~y%A)L0lC`?{8+dEc$FDv4+?_H(h%k1fV+PIo>0bAo$A6yf|t z)_@j!bz3XLrUd0RLHFH5#+0#%zVuG(ct71LEO+3}v%D|2L((F6;8KT%Ycnr8@s|Da zjNyqRq-wF%I!kgIgl5S${Cky2&x)lHtV%s@wZEuX6Jf@j0Cq>*}4V&INwtO0uL z;K|MpdJ9-(IL@bcMmV+{++MtH)#Z8cp_4eeX!QuwLTx4>6>~|0+n6&5wY!XvI3kp` z@Q>cZB;!9&tp|vH(QSc|n}GZ= zr|6L)V3q9yG}?Qi)(|m=I`Bv`P+&{v7acudGTeaeomK*CA@kyR>cnn}u!SzSC0qw8 zL?8@jn&mHH^<|YN3YV&KeTgmF;;12))-l14VI{GFsqYb4GLCx<&}&U=VfkCfxIwGvb&M`@~XfOEw$)|ag6k|Mst zGsQ9|LI^)qLw~V~^WN%?1QOfcSACb5a@L_yDI=%gvtz_|9wEG9S~I^TA>V0^2`<6V zDx$0q8mn=2ZA|UDYh|1i7|dDb9ZL<9luv9y597bbh`dX+WZvL^oW1HpoGCe+*lB># zXt;LECe$Q5Nc+rwwi;&+P3*5IEUj^{Y|1mzaF;t^jFhH-mhhxg1Pz-+9jCGKK^aHh zyE(r78}lU?itjWlRZs1fuPfyf`l|PMh}g4@XCO^ptJ3t?o5bF;Dc41Y`%DoQ4y2}c zp96XPOTKgbRQv^NO1G?WJM%hbel$yqqI=D$fZN7FEO$KJ_EWKu@ngUcAZ61Il{|qP zUZu#+!bni;5!X--UeU;ZQx7soNAxbA`0)JI6gC|4u?y@{7MtbOg zT2j;dR40@49w`sSDT0E`OK(Wh-d*%L%MGIbAIIrV?G%+7stnT{vOfJ+D)-s-xp0CDr?%%N) z2-LflP?DX!9@ys6E^i{7&pHy=*R@KZR96HHCpv2TxUL7iG5nj9(>f7+Uv)=QD-pXN z4CjL_-YY^fLY-d89UcauNU7y}BS^T0YY8rs3ViVqIEi_>#AHytr!}Uk9pq?Rm`wM& zo%;j{GJ*x}%YPHS^3Gqh)ql{ZOlA295FB(j(d_i)3eR3iTDPIrlcATK6U#9{}f)6l3MP+4YWvBdpf zf5D3mvNu7Mr!hYwd1K1gIxb5J)MNe7DhT9?Xj)*?;tWDKaV2y`!DA-ppAb4kw%|Hl zdYCY79CHm8yvstEu)=cT>M{LKypUy%Mv625)SX zlkjl!BJW8F10Mu++<-D~k8p0^--k4T5qqf|-JJ$4ATEn0hu>UCz|iPBu&C;ce>r4y zh>{7q-&?6(I5B9NZ!d%jqH(NT)~igrZDBQLCB)R1St*3r#6e<+FV8>qmWqQ^u7;L} zzAc9${tpfxY)|rnkun#INMV%oFJ|wjy4}-Av5b1Gf*)D4{03D1FdKwwv(lX4oLoaL zACT15-~NeP4kMds6P7%`TIsF1>+>*ApoMlHQmyv?47A`&>07Qk~OBjSGam<;M%j?{(@n4%Y=rwInK5i-B@5OU%Br;_{ql=}pHaf2bQr zK*ya$Vm?EB?~+2m85?XfK1-kZj&tCDak;VRhQ9xza)^Da890B43tu7@->wdgNLHPA zK24@~lj+w}f&D8^mp8@Cx}w{++p9vXaL$uBQFwJ(F_@IBJ0RL9%YB(Vl8Ku9SH2K= zhg|hqCA{7SX8T=?ORlUgOenNvMDog)-f`kij4iW`<7}qX&S!vk_M<=OSr#vf<6yOd zdnLJTOcL19rnnG&1&LBUqQTUMbVy23G4C*m@KnNfXw`*9@3y$>@{7nGP+7iDR9klq z&hv7Mo9GuF-Ely(U+bgN1{#&EhN8D7$)WJ^3-2%1Ft^Wl^egU>)Rka<(-wv;>XDnV zn@sJ=G#M#+a$r;q3}nO^KYNUn)G6LP_3zHZSBW8nd6_V|ALNu64Pe-)b906dfiSkrNR>S1x0=o8Z@mFbqRY*6usTJstqN$IB;drl_@N;}qt9ao$$l`gpr3goTxV+P7!Ej_Z7zj1a9r3jinH>(+yk~rW@U9+&B$x%} z_yxn~L>XOL=Y=27MM=XEhb?>YiZPp0-ZwMOXbA=s+edwDaaN|q`ntZjz|4ps*k6mi z61IHBv-YB$$|}akEdDxnwnx9d5mY)U+r8KN{_(Rj|@@Ari8v$?Py$I#!* zUOeW|T5N3lLx#!MlXwir5F*GA4>%-#k_IX zXP~z-lZ{R}VXQFreWq+RhX<1{jn56AtfLvZB4EafEhK#9Xv0)iXP{OV5xe1acyj}$ z`VE?1!Ox`=AH5?D1O+mGwt^N})0Qh0vod^8t{7+LbIGscvF2-ET1#J%y~{SfHnI)O zc~-+gs&#@sOp&80>Oq?Rlu-E5hc*Xeu8|OQ1APBH+|N5A7W_uk03Y5241o3<_2<$Kp=NVJW$II+&<4is$TSQMs62Bd5+74~Fm0+Rh`*&F3GnhDztfag!;`BJwI00 zA(Pc**q9fW1MT{u@&I!9Wi~JLKiJ5GDO5;HHZ$6CfIV4`pvNi_X4RDN>agskj`lS5 z9xF$BhW3W)Q(@=O9BkKQ%4PH;OYK@@qH_Sf?itJG)BQX6Q^?VEq4xnFb5)gBxkgGn zfE>?lx3e2&mXVHcEU#M z+jdQ3XNB{2Bq-p@etahIXM0iAI=@}tz0c@UJeOL17#wV~l+n02#i@W|42Z^8Dc!Jz zi{~D2bO4Lao!eyRw4wkN0hx2OAU)hbM~1l zC$U7NW_wF~cLOvn<$9v6U3~HC5N`3Ur>6%I4Q>3?Y})98ST4}xem+~^=MZ^9XI zJ?$d$CvnE@w;u7vH+)W>cn9w!pLPTt{UU`6WxttnwrOa zT{abI{PaIHDk1k{#Gw@6@;yG{kkit}H#KoX2qz-7dtxR{y9X6u*MT+f{EHy@AkP%- z8S)=g=LGmKsS)(@LKtY98B&QLLMpH5x}a&><{&tscu#OtCO)XFZrnN;*eo%8S%E($ zn1%~9#25l!ntiZr?Yn)LUf5hsGdz)5PhC0r^9pffUF9HFcwi(L997PJgf8;+G z`l+kf6d zWsUu$3J!QWZXm0MCMxPFvU$a;n#BT3W`MnmZbScN+2>o6lFQG{ywyKdT?|u_csAbK zulWdJkoX=7cE@zQOIar6S(Xw|gMo?M-j6dc?v)r@qb&wb*NMN`Ylzhqy^PVhN1au5 zjj{l(11gs9fkW-@Os&{=gHw1fG^Dtq7AEO~!o6QU!y-Ivn6)TW8Uo3lpMque+v3)G zNTuHBfQR2-9+#q=XyD)3ES%TruH7**=&$t5US}&>y*b)z?0aRrOPU-C?dkr^?f%Jh z;)#)z6rJDe+L!Y|c^E-If-t=4OGsO-5)s5>t|c@A-XJ60E}AdJtV`HYbE-;ihDdyiAZr8)ZD z6}wJH*C8D}+IV7A=R(<(A^H|zr2`nK*~WXwMn9`PU8#)yoMqg6e+L!g<-VEOJH=fT zpOwa4kALC%;r1P%og?Q8h`Lc;guL+v?+R|i2Yu1z(soq)ieL>K0a=r$VOc+Y^v|_z zXgp__5`6$=qhb~;b{Gutr&=|of4XQ~3T@~Ap`x@0>6j83;U?seX?zB%&irzpIz4Vb z#IKa%Hv4BG=I(Srdb((1(5e*+`y=IY-UuT*-3DKHzwEFq-a5+PY{QP*67>693S#(< z!J`QzT0gSdcf7Fo>!KJ$uJ2irrCs&vXGS!|5Ifvf4zRU{OqEI*5(V6Em3o*T7ahO- zB#{UQ@z;NRMu11N5}`mRdk5MmwmrBoCflv_jjq`pokx1-X9g*#F}@c`OcmA;G+F+k zzl3jO$yo14qVlk96(nRQ|55;L4?G-67_!s@7t&-KQ!#oIMiw%xL!M{~!LPh5#}lka z$JRamA2DHV9r}j^mwba!T+tnZ&*5$rnwooWCWGk|^UNIWP)Mr9E}vQsMs;q8L&?gx zaZBW{QuK?=r;VEM-e~_%@-1${F5ex(M?N@$`!$JI)46(`uOL`;up^Gw1IfJCmw+PK;!1tSMPGbM2a&A zrz0d~X{={wOd)M$naeS;g(I_`sd0-e$L-;(k_cY~$$u}!GAXFC9Pr6cdb>7Z^8DkB z2M^~nQEcUKsW*E8hnQU4bB6%`QW-OLt-rAGRs^W=oMT)5}|5xAA{#s=Dvp0Qg0ttAnY`s z2&c9ghnhJ!r|kWt-aCyL4!(1vPp$Ds?F_~=9#tp~DL+{1{SZm=wTg;8nigz=27; zy`hq=$=IdM={I^FWQ|-#EaO0M>m}mQF^Mgu6BopbJ2N?**;b@3Z`N_%;$dtK{j+@_ zF#G|pctpC6ECD+$aG#1{q{y;&a`Kx=gcw~hv=rt;BzAZBiuZ$`-@C<*_u0k6S&O8;Cc>2%-Z`B#{NlF-Dji2UZJTob zWaK)}$t>+4=PD)j)pQ);7e7C<4FqKmvXA~8dBPW?!d`5;AX~W%Y|z@sPK)4dj#d=Wh z$6tDWNYSO$6cI6aqUO7Nkzjmg&l!3v!CX4byIQgv0RgxF8(|%Mc9u-n-uO4PZhBBp zzvp(@8&EoP%9Xq)-HZXS&99>p1#_mY_I8rxp1h%)W$L@(FTFQ-`5^vOX6SSkxLg@= z1F_%eL2Y<^W|Vo8W{;QJP6>6=Mui;u`iBoB3HC*3ELT$=jLngMX73XNEf+~rAqdxnmkT0FYJ60)1?iApqYDEs= zqnSSJpPF;yr#P7RzVj4|2oF^Jz5hMGW5}9xJE)uvJ%7!ZnEUC}CScFa1e^MXP)vcz z>@vA-&V<~roeO`-5Y~A2X5-*y$Kvw(s?O#DFUgyj1yYUU|BD@E@n`I6eJil1C`J#jgIW4*3Cm3!W}({%k6dK)q^Tjgi! z7t*w^{1eiE1HE|O8>=sz#eiCg*L2I*_nABV$vLbphf1G*L0Qdytc!|^@tLnNkRTMW zNd>@glD^!*Jz>%^bF*Jau-@iVwA0N{=C@G64=0?h<2)WyE7Y13D32rkTpt`CgIRc5 z!l$1NTgGfYh6^Et|0~75` zxs0U2l!Vxlhk$-n+|SrA^oJ|t)yJN}Kl)B=x?2fdotibBk$)~h4)a4QheDf%h$OYb z6uK0Zk|d5e*w|@5u*~2fdiZAd3+OkX7%=!(_ECJR3H$5zn6U=rh-9J*0l2`4A7JBJ zyVX0|iD{A0b1O*!l)0IJ{9uT{*Yt6-a^Y?Q?uTRlm{AaGwZ=*Q(0^gnbA>JZnERxQ zpb4gTrej(O_PRxMhMyFKC$nd0Q~O!qDD4lg)B8hQLHBE1gb!A|SdWaaVJ($DgFn@L z|Aj;k{V2_&HnvwC!jR3o5;Bzece@@+IUP0y?QAP$cK=Ki@#2iA_5cmF88DSw&7Ek@JS{A@c1cl0OWd4)yvGv;=9G zlw_Ch_8SbWB117CVcsO_!?03k6ih}8(fW@9_-?tJ4+c9R8(tz2i?HA|E|M-a!#BYD z#qj$J#JEgj(?-g&wwE-{FMg#HG}$Piw$C1i^jCc%^)W9_u`7B~5t-~o z1~S@>LpB|t?JmlPi>@xxZ=qC-r5_}mBCm)4s!5uOo4I1?uStz11Y&X$*iHp}@4Z?J zwP{D4j8H{F87K-5O4~Sc30`7S7GAnP>=s~(Q?eRz2971aB*^aeRlmDMnNJw!J#l^i z{@8%kxUF2NXnkEX5aJRS*p0|P+beOw%K=9ATng9i8-qTcBrQNaYqL1^IK$?9&!diG zN#4oKXy0{bm6H*>zUlSbeaE(Z>8^!>Ckx*d0~S=MF&;4hKrpTxJOUL_m0|nj1U_?` z9HfR&D4~zG=e*@71I9~k1q24u{ty_^q$tB)Jwnxxxo!~R-mz;gH@{ib=47HXqfy`N z1jp9EiWnd*>80O*-S9)e{k3mWBFVoHH3{`b50lM(ujp*tNBq(67G2##P&P=*%d?cD zl!NPJ@UJUZuSt`8u}e#~n%)h>d;~xvr}_BC=a?abalfNI=s)5fNwpwQ-x4ywK2*9z zT0T+p9`PXO2%mVG27o(+ssyO~<8;=L-ue1$Zz>c!?H~>stbgM_keR)o_Cm_TYj0`D z*UN=*hz^4`dr@Av`OM0VsW^u#ur&UiuAJJtHGl-7@Wlo&1Wkvu6M^Rgw36O-{|9%a znctJ191b?<2V^=QfyjcVep{x+T#oke+666ivQlKKU4fWD;pLhvj&ll2*uJkcDBd}F z-@JIAdU1v}PKvtj@B^2l9QdO-3!DEejiZ03$l*{``u|Ch_1T0z02^s10yU2o+IW)N zcUz*~-VB66RZf|nuX~~Cj4OnetiGmQaf!H1N|+V+vGCk)ScYxQR3nzv>(o9Q4mx?_ zBK(NPD&Gf^Awd*AFrmaHEQMw-OG_QvP486Nnd!m7;FP81{nkzS69fJHa3fV{2UsI} z=W=)nb)s;x%Mv)dDD3vgc_)-yCyd{Z=6HqvAjG-lQHaA73nBgODpRU8B2MAMb>k1M z{A<&1sgx(EXyUd1l97mILP@i}ZE+~KrWSiP!57uc#}{mxQ6mBUhJbbR;f)jbtgVz< z4fC50#~-4R4J!BU@R*?!q!r5i&^A=}j?+lFP6QK;PP?Vyqc6409Hq&9YbyawZ~dxF z4<`C}LX{dV1Onf>QCx3bv*%?)4NDnUdI&=}5i;U?0V0F)}JK(VF3A9j_LZhxFpJ zw&+)D;-8QJ`l$=sZ!Fz(lC08LZriHxK-9%AUUP_r6Yx2Edx~TH7g|S3-}z5Q?95JE zgYMV8y$(}G(Z6hb)2);p+3lY$!OHGAxsBs8g^Xz`-GvGV8zxSSX@iW{AuKPZ7x(iV zBb-0x^I8vf6Bm6XaEF>uP^-uCY!?OPrpJPRtbqx*sp3pD%YU|uz-$(n?Gp@tB!02+ zYpBo7T6CjH#3r>plMSjIUvK^>rbk}+g*EX?dc9OE^0!4r~rO7RKhR)3^TU`>@dlUfEZi@iKDVw3l)Txh)}zaK1|UalrXjI*yZC22!uHclzTj4ZA#h|Jy3~} zGkCDYvg71_u9Wv?GviRknJYT=I>Gz2O?t+ZJrpa4^M+^{S+T|ts^$veWw%RBHuXJd zI}Oqpai7;H>+KiHt~cVw1pU*;>zk;4F92t*%&l}(hja3uS~4LqN1w?6Wa8P|S-_~# zjD~O6XLcI|>+gT3ey%4_TCT)57G!0$A5cYKgya5l?shMH`bIE z8^Awx?VO6cM1=9FKqQAOj+}+~(;A$Xh@}i@?kTpTrL~XKCHG^i(~x8eG`aSLj_x2_ ztt2U;I79=q!j)~FrqT5N!{D=XRr>X9k>=fA_YzIVOx6Q-yGo!e&0BC}^#_G0Phjnt zoH01RQ4@VVF{dj6m{0KiEbL%eZ8t_kGJhpam_v)rQxRQw8C65!rR$a0txyeE;=a6d zrW+*?jVCZa+&1(!jsN(q5h@9m-?b?v>b@jJ<0qYFXAeD+)wuI;>on$VOb67>@b4~` zGxzfPBa$x?A?K4#fwJ070aEr1R8V9O5#Odbwio(@-d8P6)0o5<)WEldQ*9Z&d2fGA~l@3ExjwelEU+e!@W zA8dTZ`)lXbipBio11+z4hiJALs=4aCD?M6z53aFw{7{rJWI73JXy8G5FD$G?dF^-K z#<6@_+im8OpR~OpU4$!q0VfAnQ7}zfx*!SMo1nN{TEf1xUylISzBF4!6W-02Yl_XE z@>gro|32Zgk)n9OEK@5}qf?8-F6FpjaYI*_gWPb`aB?QIbjW4lu0KZyiS{a}1%eVA zDeh#@sBYjIP1x6r?T9>5@mAfbyW;q$Ooh@e+h#5p;;){YPC{F?1ngsCM=5N+AlzGB zN$~|i)2y5AUFNLSSd3|U=h#Vyhw?-EBFBYl#hj8c6#ag@p;;D2P?Y-9-cK?n7XuhC zOQ8#phrni!on;05@qB>}WkJUHjQoGpqmp|0Lac-oqAz3i`iz_Neia5#vf16=+L+Vb z^eVo|xN^xuXrn;2n9Ywnh})hxzV*3Xtvc6t{Df?)x8eN868#XhIF>!-PrR0o8Ul$+ zRi-%3Jn?!!qmVD!m^&8-HEpXww6A6mp<;|<+_DsfHY5k(GM7lVhT~)U`IhP0q}l3;`mUG(Ua@1L|y~|h`4{K5KR>p(BDdBPiaylS)`&&i(pGUtz9*vF%4sA2lCP>H?ddr?h z?Rt&!D3Od5OCT2c>TLTC9(y`8&O?t0(!^i=Cj^g!2&TVW{z%+mRGqrHrSTr+R|39+ z4^D78E9jsk_fnh>suM>N*{sg4A?95bBO!u|k4GY$mp?W?(_|`{M;n-p{mZfcplk$+ zJV>O0$ey;E+9O)J#f;u#`-_&Kq~d*QYd)@g$#TJRMdEyLT_w_j7ZD`K%%v@o&wT+$ zGF#A(L{PBvlg89QeI8-jCug{|WBxIYPk1YDl?ATF;L&{pHplPUitoPNOpyp^*F`Zk zoa~9_V>=8qs&E`O9*b4(_B)POfBrW?tGcplqr{(31_YdjMnQ*Mi}O+XrKxv#i}my> z(ENKHoX^@B_EQ&2bJ9_nufsS>!!Ut(B<7KsU(fK_-Uv&>Vwy!}+YbK8yRg!Qo7C zLV@X~0(khvT7HI*KU4j%i2~dRrBLV|^Z$_J`QQg>8_ie3YrA#tMwzCf^g&2iZa@E* zN;+v%u7RemrL;2C+rz^WCOJh|I9y$<;=>aETtw*7(wY{d;QA_!Dn(JDysy7u5uudK zR|P3yAdOF_*iK$_aBOM?|Yph1K%JL$X6 zcdU*oT<@hzVEFBOr6%=OGha|L_PcvW3a6Qd?io5aGFy{dbdMZ;HTZ#Wl|G}l$E2hL zI$a!%s}rdmV`1!2b2b7QPlyvfk8EBw3dS%$GQa^J!5Ot@y?6H$=ubRS>|>s51HG8= zQ@o2RxARvc-ga#^9pJ4QspqSQ4y;Q3E`40Tp5#%R%K$Cn(~Ti(fvwd}Gq%UslxB*B z?02{)URm0q&sia0aVf( z$gv*FLa_-@1Z!xf3(_d@w-HF1nV3}hz4{g^rr(GlEbVU$86yE~UkpFjbybd#pd41;4uGMBSdyK9NA2ok1oKNli`X0#XoyDU*57Ig09AC-m z8FhOAp-DC$**%hb+`Fc{@TgX8#a9jJ^h*&Ylw*yz*2*jb@#{rMRQ>y3VFelI$O?1zJNZgu2kWtByfiZ8bs zf`_60G^}y%&Mbm{o!Rzil+H(Wp%(hBshGiX7HRM0t+RIpGisY?c#zmQo%|jH*DSjI z8(98nyDlnPq9KO>Ecn7q(>-v(c3R4yi!C6n>dfUfmEwC^a17o4SWRp3k}o-XZuJ2v zHa1qs#DrYm=LS0n0p0B8aF#@5C^laZ0$%J$(jVCKDtv>{z!iC~U4oOPa(v%~Rww8* zp38$rvQztEQu+*|=&pP<#%mamP_+5dzL)p-$JB*CBu$*%$GW*if8@_2XDm(X5nE}U z;v{PNkhVoAQMC^~$!sbS*jSQT8fXqTc?l>dB8@3sn!UOo8>cgEwH$(Mmk)OD;rT|Q zmKHIPe(?nT1_yb3ofNz;m{_T!^{6%sC*GfWfd0|sKWAgIk`JTfJ~n%8^&VMQE57qZ zq|p93;CM;vz5d}5%|iEi3wfi(=Uz58@~acS{ba$d#B@3HPu65Lc_kGac^88ia_1a@r&1SAvH|S>jtpP%x$q z2hlk+t=|Z-($PhZj>G_kLw^*ieiiidu(LC7-G1%3d88wMK`O8XXxk zP;F9KveXGuK|lrIW_P(y>eVy#519o!!+W>m(Z~t*3DJqU#^FQ%cSD2JUC*PYfRt5!NJLN+wMI(V%`2pmcaN z=|yR_!HE|h!ff+qUIJrn@I?+2$TZt zi9x4bm;;8_7N?X)R8={`D_PZ0g1~#~P;xi4<@~iVm>j(idyA?&Ri#hiurQVI@jg$t zrhQ=;8uUptS#Jwg#rP%7V4mCnRE1uIMhjh!GRkVKJzv$qm^1{)eriP2k_U0MnW%)MpWDJ{S?4v+;2g)Dj)Vx0quddXv!8@ zBH0m#_2-ys)pO4t%NZ89=@v*Tk{sbTd5{j zQiddnzw=tsU-?`^V4wtlYmJwNp}Wh`$aAS->+9jyk6FCBlD@tq4-XGSJWd)QWF(91 z9*?d09i0LE>Ix<`#oV2vs1scdKgfcZhaQUf7H5$68%yH380c<%Y8A5$^)2pK&D-9> zm9d|1f3T$eXudeuL(=EYnb*r4*7h)nu^i`^_H?q$!wMPqjTsDnt69DOWo{R011VpR zExfeSO5R;F0)=5`B_w>`71AHCMy>5}N_tiQlG{EL9pVJ#=TDKSP zB%v|N5gcV&XGb5aF~};dH4Cf$DNxY|pms0Q8t=~Em6iU$3XgQVWDdm7;PP|cbzV>X zWG0AFjyd&ye>pf|{7Nw>y4A|E-V%)-vPE|fTrWHVZBnj}PfozG{t2LD^AN|im<)Fp zLq7r)ZWc&ifr!D;QF023{-xHBN_IK+_o=tUOnlB>v%~XvE=zb3$Zx9`$~0tl4RCR> zzo@8)elf%l6T7YK?;EJL+Ob|{Hqod*)}GKyQ5|s=>vKz@aA)_WR|T{tvEvJLN$ArU#%fxsM$*Pgr4Fz zH!Ubi^*TU_qwr9IhHfk;Q3z7jrbJPoHCbUZyZ)7o{A17&rQlc={+HCY_P=R`q@>jD z*AmE-{p#+>fYPTXLmdQ%hVp88VHsu?k|(h3smZ}C@KG={v7+pX%EyBtj5;=v7E3BL z-2O5re03H5?;9bRA(OxW1Eq?kR#5=_juq+b+27_oV+}U#PV<>ySxbBq(>Lv2Od9QQ zVeb1p-#HfklVvC>`Wzn!T8EhLlJsh)A>Spg9?%rGdgOyN9+}cKWF!aPRq^7(Nc!>k zN*Xx%qC3=+sAw{+{^!%)>46fCyR@laz4iLA_qM`}(Ndf(Hh`da^*ZALM)g~5@-&wU=13poI_(s=!@`%;b)-L$hTO-BJU;k2) z?(D@1m(bm-zQ^yGu~ko4+V{0bGcc)0)2pInkjO8`kNyiq8Z7)+4wn6w4+l(Tpx@8H zowhV@3^Ffz9_>C|yG3p+Zf56(<|tgUkN7Hp#n3_Nx&cuPɗd<) z9D_6JX;jh{p*f4IIQ8^v!M}#FBqUJcf@8d#4qNsSU|bLnZs-9GE6J!MnVpsZxa?ox zt14kY>5?)d-1Z%1lFS$EewW8yRnXEjf|YRbLGY+X*ZY%Ecr4J71;<0FH}|-sZFf?0 z5_jvn)r|@1@1#!yh@ai7IP1Mx3~G0s*_Xs^Djj7BGld5ajER0f@K`60ZZ}k-Row?` zTVkJ{Wc{;9XtE&hBr(8~fK2HFEZT~Qp_!Fn(4~lNE-hrV4Jj*qCys6O{`j8*K`~-w zgN-hv84aXUjp_6K1;j5bmj%-NzJgoJK`8>&g*LZqe;qRSaxB;oXi#o-zkBeY_pxU~ zxcop`&6a1*KlIBCr;MuMV)vPcUb`{tc$4|Cu`_*%>N(AK2qTr+=(Sbh@^?-2@uFLl zxBq_OSHM>$ObHjVk9{3J6H5*ZKR(kE^VT&qYMr>!th`5OHh5fWYq#b$o+j(^%mk=S z7(&53hmiQQ3=PEE=ljuePd4$AcZ^|f83ovHNS@3_9bCyX(^m; zuk7t!UV5!L|LhWOkD|l7NbzbC;0UR$PhDf%NtZbRXtl5%Wpz64YX5^N0{7;^kdgWT zG^y7Dd*@$3*k@NWQ>sTI&teC)fDu1gmI_pLY$IeU3Jso4F390LF;pF9C@Ko&_H1q9 z`e0g0PR?|>$q`~gJ-nj|?C1#6S2vy-1q{?%k1X&{W-jaY-`Fb<^eSs>JAhL`P2@aL z%e_>VT7p=}cU5LR{Pg$w3a6gOc2i`x=iNO;?$58nmNm};Q)Cf5FIV$6V|M+i2V7H> ztU2HeJ>!=tDZa&Xmt7rQfOGK@^#~moCI+qu-a{sR71YDAmMh?ne)z`~1Ezb8E<1vt z_CO%M@o#&d;52oL_nYn(pJ5MrDIJ^>Ao6YRXmi|`V3=L?G(Qnzda)-DugfgH_HvG( z*Gu&lN}|1`*|#3M75YQ^EwSH+V#@0_PvH^{lty4b)?46dR`_xLWBEQ*1WE{%V^A2- zq?X~EHvjw#!d}}fnXO(_i>NHNxG^?uDtV{uk{Xr;IkR}UbHu_zy54_aWsR@wc#P-{ zLa6n;u>yWKk(>gg@w#xKk9ntmQSTHE^_`#eThG~_rOa0RaJGj{|1#HOsxZ14AhXFl zkmMmqZ@!QW8^Vx1`b*7FNMfHfi1VgXI2nbjKSp7 z5|5**Pfue?1FAxJe$tO=5KogcS!p&nv+ce^XMq)ZDV#~O&YKGfCfjtt_9P5;KlAhP zOE+K3KHzz3&rU0;EjH|}-R3PJBPnvOwbvmL-e_WyebtkK-$&Cu1bUT1F`kHEX-eXmj3=9tO8K}OWihI6Z z;9b>y!kM<$?u+!Hx|#`6e^+yYP-8i6dQhFI{Yg{yHYSIA{Btt!wD;1v!_4fgnzXC` zowSUu76)Q~`i-Exb|jDHA*ar#woA3gGCr?Ql*S*FNm>)eStya+d_m)Fh*M0q-DyT* z&U`0c96_3E)mPt=)EpT(tzz=L%qC~s3VWOt0eF;t zhvPMR{b?FaN*d6meE$1~7@h0cYVj~R28?=l{??U~fU&jclkFNJV!r3kg3_7%DMq{V ze&a}IQqv*y&Da4_I8FT3RjNq+V=2&fe>+k6bnVlt(KWY-X zn_Vj1Qr6`uZ^fr2aK4%LX!YK07+X=Z{p~jV4JpBAp8K^RV|;mO<=a06h%dOXi{0z= zi;2{o0u|ILBJuEpq_|rf0&mFEuG%C(pmC}NAC?cXhu@Pclx8zunYPv|LAdHkhF01% zPRT_N2?ICZ?-ILqW`%0(*E;^%_;Utls@=ORJhZs!pcz@+M zn|!JE3x-khfmlb{9u32jmKI)z9$sUY>e7ZJWGXS8(EHqngn&`&fbDb+Ye`vI`n#=5 zHwN?FQq8)ypTuKF62`pEIb-&neVm)kB{CvL)1hpnH$K@uCPdY4C7C%P%y6aF7q+7aii0+e_xYELzUI0t0`aO|r zq0U3SFcu{6AB%hO7B<#KJ+42-zp!RjAb}ptX1!NGO{RacI~8JA{GE>(t%Jh1m=16IRDW(bv z24aTPE{nIjtHF0HKRr@??h+6#er0`1y|}a?@kZ~7XWW<|e)`4P!m3*G*BB_N6~F;G zG^4+ACXW6ZGREN2sV}}xxlvl=<5Zl@Qz3Ssg-RgD*U8naPeNcM?SYSL@SXz4e2>EDLYBsin#6&Fnvk=Ktyy zE|QKZ5=k_`D`U&b#}7$Mv~37?K9U8cY7On!HEa_QRSv{%fJIZ71zJ^VIBGGGR`d@H zwD7I^`?T-hD?4+Rz1ArK?+#J^Rk_7)PX-&9w6UD6MAap_%+|nPaS$P;03!0^S(Q9@ zs7Qyc|9T?Nnd`p z%tu{0QvHhId9O1wH7jUwn`h4qIh@p9W7SE1a1=P@PP|FoT85O0q4N2U=P`l|5J_aV zxF>h+#q>JcBHh)nyYq`YfXL9~zwqW!EPS)MiXSKMn$ru4a>bsrs!>ZtRO?gk*!`yN z`Lwt=6apaeFUSQ;0SC2W5UJ1oC-(Id19Kj!~eVN&v38uv0H2Vh zjE#XS{Ry6!$0_yv+@Ty0$LcUYrAigXPuC7E>2M1eg`{hjL2#J->0;I6<(b~;VrvP5 z(+%RN&{TSV3e&i;?&z2-!CWA-{TcA@4^_d31FkA#J70=I2V6P+ECYS0&&Q$x?D_Y* z2Xg>xL|zi)PJ+_y!zuIL6wdfW@wYB!S9jfbqvqHzA$^* zHQqkEE$w3M>t0&H*8V$HWrgEV8xNx8M~2RCh12D>L}vf`-g|O5V8@8ZON8&o6ro+q zRbvl@=T>W}Vh(a8-o0jz?z!Ze>q7~!zNC(`ia-AAYvCbNF`~My1`L=KaV)1yOYbc? zIx(-Pk&zJ`N!rK&jR^khK{ZUhUJsq;iz1j0ZD*uvb_Gf3B6f>+c@jXg zzxXY=M(~%~Xz{UetUJFlrM^6=2a(`C=8-z1;p4s>k^6&(ou$&FS&1I3t@yZ^ql-W= zM^4@Xr;%5*@!~p`usmR^YkWep)`YUr$Hl*&X<54T{BRlS&&et-Lirpom59opSbgb; zJo)~uaeW-P!HqTsOEy9!voyKF;q;Z}IiLj4uf05$rNWKh23?J6?K1g1ba`bpGUswM zSM7SDH2?%CK)Of{->vUcnta_C>V4;5alNmT0|WCqEV#BX7TZded8#woeOGwdwftIj zmya!sDS6^WhraqMkuH(ml|E+f4A^K^@H11mwX_iuRidhBcP4eawG_ouJRd!- zr$u0lR|g~lT-oH*iL!{54n-?JS=#^qyGyy;8I>s$X5B}Gzq5?VY8-JNUp=53ZzTJi zc0IVxFQ3KQu6CG+=-_*-r<>#y>~e13v>)wMTTZz|tq6w9A<FrFffU4h1Da|jrTkWEf zryTcgBsET%7l5}Y8#{)l;)W8*(sBNb5TIb4<$im6yUKP+4UTp*fsZ)B`@lWlKq=pe z=%}BYX#!UPzVz{ncT%gv)B{)vT86p7u$(b_SAn3EGRT7g!mLLJ2Mu0dz_K|4Su-;# z1JQ&MO8D22?Td{3-0btaCf&5+}$;}yKC^^!5xBL+%B%ud4A7- z&dl!4&c4_)yyV60KHb&zt&dcdvT|^^R?q(KyW2E)U~kvY2uG){6Is?n9c;%w;a zR(}c&cBH*I`(rJ(H?5&eWli=f-a3ks61skls^#N{V?q?L()eDK=k8QvG_BcGdr5lN znRg6^(vXz|>qkfCp!lf~Nc_0r+tjnYfaD*#-kXWf2CTG#FV4tdO1Z*?3dWUO&SUiA z&0Dx!@!LNsjx96Q-NERBo#Q()JzwwOX}gY-w=5C=U!kF#;N`Xm;sc8*Ml3^XnrQuU zrdnzcYTiRcX+LEfjwb@H7a$Z)&km%W%fCq)m`pSt?+~vgaYEypl`aTBL zHn;(&_vUn3H@sD6e%W=4oMi7UHfQ=$7>BMlD4%dy>Pfse{Q?x6XItNj4nNpfynW;% zye0W46YJ?G9oTs~rqxUyU=&Ho+Ng%}JD!8N=jwrw`0A{hxsY>g>koXG^?EYH=_;D( zchwSG%_FuSVv$%G|I9zzhSk-ZK*@3+Z1fAqoaQev1nmYBt0GJo{xZd$O;o@)qg}dbX(oqnbJulX=Z8~pPnuSn#>;4AbA)Q z4$}Bhy!8T#*uEE7JRFTD4Dqj-qW2&pr-;$!F_0Z=#CVNgXtD8uvS6VNK9!M;D*|dFa~yT-x)cZp zJy`JV7bCQT#YVI)$NAdu;0dodbJN4=s&AYVq295^b#71E38A24Rv;bmdgvagTl$uW z_Is(@8Vg@^Pp1zhr;k7P3oLTJ|GCRiDdj(Vi4*`8`Fr(#YQ0Kwsw%m{mT?|&>SPU{ zdIw{CL}^S)_2e)G2!t@VlklF=ofYiUB<57>G#{ciOuo5oNe=Ie?AqiP3yIC^)Eq< z#H}>~w-R!$HZD^!G=B+$ZgB2cR30#34o(+Qh8tP=$(op2&-sHe8&AS;GYSd+hYP_S zoiQ)Y7>8WY5E+`h05I@f)=btecQI(hCS+SUV!aFakHsV*xWvh_bNK?ARViG_kR{RY z*tv5ZB(qSh)oaZP5XZEE>EIK7UBzPc#=jM?ayAB^w4CEJ9k<^CG7P0*^8RV)vFwEi}0`U-_`YfF)6w#IGsjvp@+l5(WldaHA?ntcT0OMmmtp9#ig zHyhMwwDBUrVo^n9szWzBTB={9d0eVRa`k&=8-Se*WgCv<&$F)IS@4bdK zr$sCM2R{sKmnKw9n37mvbkM%W-vxtC0xBcKjtad6_mFBb5ww$FB0}N{%trHSdDVX& zgmc<9-y-6ipITw%UpeFaSebn;guNE}=vfLNOsy9xNtl`Iyf}!^mi^w&YibHmydiEku+epuACk38c*R27$xeyjP>Hxi=d8~X$h6-us3|p!B6?t{H;Oqb zduikL6};%b{4hoByMt)D@62oP}5)iPsaM`-H4 z-byqwJ730qC~}t(Hw@1P)ON+#^5jI&(Kg~*~hhWQPCBgoQ$-o zL`M4kI)%u0tkX9cvqd8OPbksk!9N;uKRay+tJ8TUHF*koK$b1aZXZ=-7<0SE*%H%l zS3xghK3ThVII|>%FP3cIxcjsy$dal-L^!6bLRSSN3#yMytSue*0vM^JcMekR z8v;g%q*Jx&x&&gP!ZN(~XE)`{e|G8M z)LyIE*;n(3ef;R33M71K>4+A0k-{~B%?svJUVV@K+w$ZZ=A&k23{0%A{UP&KgJw<2 zHciT^nvzO?;VTFJKLEBZ3qRyTMT+n7@^QV@ay2i2v4_;Zwzq3Or|FniGF@|YPRAg&dI2PJE$pr!fq%qlP7QYPJwl%nw|u|?@dZ%o)VwQMnsZ%{6&t%nX(si!a3$47s#j{uv%6F& zLATW#cA1qzGm&(1kF3U8{`TiE@8StdE;jr-yRd=Fke;PWziLkT^r>E@!W)!ls$Nhj zJI0_ZTJ2~%XT@sq=tENg3}#Dp6n0tE5rEsNK<~)YXgWm86DtrHjkyjL&W+AiYmeU4 zmBF3-{>uNs&G&rxNsZR*Mo#bJ$#b2bnddJAxN}h({N*XH#AswN_2!3x2-C|ZCWty7 z6VRXnjf2E|;w#?l1MHi_IjR}qs+$u@=SMV?Ps-cJ7W<@{j4=JTp9S{HdD>_e)BeK{ zTzcNpNG9?pH7+wj4RU05Nza({!OZKETfPrK>m-^eKy(zk1cXS=`u6qA< zIufqUo4KLJ9w$#w=x>P1*l=WChIHvw7^M{Qm$Iq36KT-|Cy>rFruCr#k`k*`QL#l# zg=(;2>EFQlJ+OqJU@cc@)rv?t#)CekhOhpzDN}gWnLXC5KP}Rl6UGVJlkF{=^7+!tPfJ|jN zd4s8)5|Fslll6}%tyh-M87W)2oNw6r-WH^}cX|U6&5lLti5dOq7D)-R7TFHxKDmIG zn5;BS*33yEhm3^<&4xqP3|*aeNg9DvE{zFF{nMQZ!w-^;E?wy#g_1X{A>@{phoB(m z*HIvPvpccv0Xy5?$CLS3%)K=`WqLiaa(zqIOM26 zN*p+FoX+y#gf_Bs-;X6Xum|B|urK{4kH<{H25MzY~NsK7C`ku*E~`R=@O$&vc5 zIHglvhhej;GpH&aDV)bMNQ5$CO5kLGVY5B@CF5Io~B6m z+}z=rI}YbhD5Pk-tX>O=Yp5;hskv$WM~rt4Xsk^cuK}<*>B5_j^XkHX%Nqy^7Sb%@ z?oHV-^(uIH=GE1fltu^Fpm>S~+{BUbNkYNtQiPC74QSxYV((uJL^B?Y1JrhjOM~w2 zjl##$$2zmJq6*0y2b~iCoB#3x%!jyw_wC<#D;~Due4?ipNgf5#MMZ##xH!zy-MRVW z^2~+xiAb2Fp1~{ zqrp7<=O*{(vEXNLF4Jy$MX7m6zVG?f^OL6wu|J~`*vx_Jm`y#O8T-s=BFJ0wq97%m`OH>l8tQ6D=ha7Ox|G}dUX+GH^;h{ zkop@3Q66?HAF7kz@`Hk{ZSRdS(N4lHlM_mAp=H;sjn}r3{>DbIcTSdU=)G{v$p{I# z{NK)qup~VT3o6%qq+Owl)h|Nv4i-|r!VX3zQVc8=tBlp7%F4x28hhZc8dWSgk}<8e}VI~J(IVL zol@Dglh*|H|0W(Ev4kY3M?^+Y(eX(w%;Y*N?jT^6^^d?zV1B*0+O4 z`UCfQ_D;9Euj=O~6YI7S%U3H-ZfrQ?+c|O`_I1(gu+CJ)4A85o4K^~6{a6ZS78Oi+ zpKZVT?SXj(cGvZcCG9OY8;j)8wHqxQ-9k*_UurIf^jaHNp}FKwmt(R!WIP3H3@=-- z7`(q?q3EKyyHyw7_&*>2vMkN~ayN;~)+Dsb2IN1{(JIpl^^=vRNY2Gd(keaJ3<^$i zo^q@4$C+dXGlKcblY$z>=qt)%(8XuQxz zmID1RTn2d{d*3||r4Vx@<{AiTdT_zJ9{t|>j##SF5!2;014|8@C|Z*wQl<;pfVfhQ zg;wWW$vn(++0s_sTYP<6!P9$otlQ)g?93SP5gw~L#!}6fkF}b67>uJsO`C;5%?sF6 zb((E{1Ys9rxHpKRrcdSDarc29S?P@oo{k0`ZB03Oaj2vEc zZmUsiak8mZ`ziO_+F*Fod#;y2S<>cxmVg@qZPK`=)xX@vC0TZQ=>Qegj&sL4=f*l2g-#B z+Ve_YpyZ?p7L^7O$XZPn-j%&r{c)~jl#tB7O+Y))Z8EAFG0y1Xxw5%G-~(Yc-iq>F zT76n>u{RKp!U-7sO8pl2Ing3HLs|p{+MS~U6JN2`o;96qXHp63V44eqOUg|T5#;Jt zB~=WE*IWf_?>EwH9B7fG_=x$k061{PsBTC_Ake1?Ls-d7-}@WbQy8BZYD>IpcCZTm z;x{RU0RO}7m}B4yUXUo-++&z0>y8c@*bnvRx^DLNU4t&c>gK-|;j>O%7PfBoE_=VU zvDdjw+Z#l6+WdCQ zKx{NIMv}{v-!J`DOa{HW>s^J1MdP)AwV7zVR9z8?vEc1q3Xvc?OI1T@kG_`ZHre-C&xG7Ra6IXa0yaEdkhzCfB2^YhzCH&muP6e~}? z%zp=+s3tQXyBz~0m!67nj>`Go^);#BL{f9l|Gzj22HrAluoIUY&h%DHO-6W!AsLFk zVP|Ukh4xnM8&ydpgv|d^PhTUDS$rbeeU#Y08Z9u^vWh60|8}Um08GR+5M|*Mc;VYZ z1c6r~RXk}?#+fJD_TyD~UM?6A7wJC@BWNBacs;!EapbpY^nic|LAp__s9flUQ$d(r z5%?D~bv|UW8rZM30(4-DXyGGOS|y*FptKlZk-3MmZ!xMu2AUgotkL1ci>DU+O*#tA z?d{J)-zok&!wzA+GSKrlyKyurttkr&Q&luGqoAgy-q=8jcY5;aA8tJN*h*g|_P-!H zJmpCmk6`P_BKEwEP51ToFR|4Y|JS^Gcze?0FiZnvv3Ck!qSG%0pj(%i1p^)uvTeel zwo&as5Lor4^fpsjD<+q=>dPg?9iTlv-S4+@{k&Y6SJ!%;`2R#7DB}FJ+@bwerKVQq z$mtu7P>1=KZBerh_Xww&SkC0)jy-yNbN)R>2O=?~rJ0;^?hfvk+Jp;=bBgWqye?aMP{w`{pCBlTCG6U;-Cx}{;!75s6s^@Fb-H~fS;nJ+=~v7 z)YkS=9A`$mWFNwE8|ryEB(pg@)o|7QxCwY$mDPvdv81ims>8Z(6)xo8^Zql|jg(YC z=tm(Sj)RF_isTlTo6>%aqJCs_M5gdWd=f8}tXLUO`ct!!`NM$_x2z@G>#DqWCL z-8%wQV8GAG4K*EjFgiUy9ejnjgbl!z)NZ)H8uYA;Y)hBw*GAnxeYwm#_=R1m+myYF zFYaX76^G}&(HEF|F?yU{xaoqZcbsN@;`tYs4X_R~##vh8B9IT7<0$m4+QSZqdkkAU zHyPa0tTB@fF6o`$CVD?#`4=6oaYvMxXwdVmccHdd;5gR(iZU zct}8JgmPST-u}z5OwCSl{9?jK1MP2?d-p?HQQ03yul=*}fpMb1pFI!=^ArE&rIPRK zZ==*xqo>~XO@{yHE}HnDNxXWl=>gK}XbIb6tx5_M`9aJoLj!U|Bovw1;`gT(1A_)L zB*4jB-D5$!uX@WY*_lI~$VeH_7eo9UenXqeuv8fhpjaJ|sXp9~((!(*qm^MQ zJCcac0D_@g!d5dd)A5CS12%fAaK}DA<4>{>3o~>4bb&;J-8v5o3(KUB-&hL$_37%O zFODU~dQcQHp#=`)?_{FvV`sxZ1`iRC+nh#W3&=8G`@P<$ zH%YK0x@NE za@P259t?W_C8IYmb?EjDQV-*D83Eg**X2Xqdg?D26FrT+IiQ7H63;%gpMMc|t?FNX z963*Snkc$OpB(+6ndGp>DkynZQ(rpkrlx%V|q zHiXPy-@iRB_AK@I3&O+0o0ys^X=?ro`}mork^lb;K-C&qX3Oj#dQA%vA^LNegT(c6 zYCZ1dNHB}0&hnJ;xP;ufr_|?7FB-u!~As)l)=OpAY)2Lu>r7SW7nOPMS(GXz^W_x8w6{vs@hXGxP4`%e(-S3_TVi(4uM#z} z3;sXn*z@0SrS|KRx+h^Gu|@-e=ggz5RZx`H@uOWyjI6=yW>`W zwt({B-N^Po^=qGFtY_ak)=AV=o-`mc3)FQ(_sT(&Hfgt_A`=4rR9 zXRXu%U90+;-xf+#xdB=3-Fmz0XL|Z*xlE4fA{p$zl-MooTYa2SDkGnFhZ~-M7FU2b z?_!_oE`cPL(?vtkxXzV*_tOm%Wf*DuZv@*bs`|E#&Ng*)y@(|XIf@?o3u)(nJ>WlItZ=Y3<+7Npdp=Ihaa>GjVEzettz0eNz(pu#iu zHrBOoB$ew`kAM=+bw<4KYON`cL18b}n4YIOJ|)F(vQAaB6X>ECleg6@qTtqGhXRdf z&v}=sCTT!PeXBDb2KF2p4#g1abi;Dsj7LdSe`4nQo)#pUa3qRrMv+r-(UL5i)tIE> zdv1CKl`VWZl(I2J3?I=1U|>Go(<}%H%)^?6WN-BU?O==$PbF! zG+Dz2<&9FN>M0YVo;qBmt?7Y9g?u}DM2dH-@ZDKA?;3h6MlKv#59di^w%2RGzoQrl zr*x}9+~+rckDah7=8%N)fh&I?y#r?LH)a^(@)Qi<^;Qtm`th;pv!UYSm$1`pi_Fd3 z_y7s1yRTIs`us;su9Tk8r@bwpg_fE+JegJ_A!)Z9VLI>^nRZd}%x^se_@&C*iyFgC zGZxFC>;jnI1&&k>2`BAXlgB68xJF2po9_iy;-w_}$E4t$yG2OkcsahC9zPuUbN=QU z>5t)suUY&)H@A@0GLrwQ3bt%`oHlt>%Ikm8cF^qtpa$f$3R+(K|uWkg+p+ z`)2R7=E_}vu6B^lm+}DbN}APB%tt4$#fAN^ZG`4^=+a*g7Ib9x`@GaiAQz6TBCCy| z>#Q>zg-EBDHPXvz1YIPZc?-^8r~VuKIVL13o?dE7>O6nXQSG*Z^z$J4{nO4(ddGon zPR=vJU|QLHA#VYXfy81vNgBEr_?HYxC!amDwVAw0JSJRZiDO4jW2suek(derc5H6% zubv6>iN+^Q1jC3=)k?e>DK=9Hd|{zyYrRpa=HNfxs3|V+{%1=P|L9M4oVaQ=*|g9A0c2Hdg5-cqoDJf~6U1^H zbVO6LzNuVW#~MeWG08 zLUsx&3}-GqKVA}U*c={}3$Gbe^ox~eI#%ITWDz=3V#>n^{0lZ9Ojet#5XkwNa@guO z)$ge`KAvD|LPL}6?l(I=+OVNY*xB*-^~qc16sEq{ewr^g)UC;jgctQmH`e2cF;eqo z4yfR$3{WLuP*x#On?#O@geTY{JMiT|{dZ0aJQD)>RPJA7LNTS)8w@Raw3D+cGJ&|UE`*d39i380?mXtmBRCk>x>xV)wNZ0@bt96p2<8#CPBjf#zq9}`3IQc&3z3894-?$d$LWsmz6_ONf)UZt z_Iy9{yuos%i&g53XXT!qR*RT_fZm^9=&h&cXBB;aXO{@@g_@d=?R5+~Yd#;td2O2T zyd6WQSNeKlQli_LcnoW+J?5D6CXe_FJV%uZ3?{F{ych6?Bafz>Ic;%e(fh8G7lPl` zHgqQdGH{sq#_SPA>p5--M4v@YA>irt)yS*X`{)m9LpokIE_OWqt}POAPh*@4`A0T1 z9$*csHfgr`1Y2mjb9)H^oVXjk3JV^WcXuxp+kCKRDl#^CXtw?z4sKcf9Vr0qk8{7xaVJ;fBI2iYt0vj zLKc_pn^9QUhqtU?PC4ushc2=M#s?aa7hi=MlbR1()XFRsV)zq_L z+&@`X_ZOk>&#)~bU^rjAr1L$T^Q`fJ?N~_Pso>mbhiDk~e{96%&Ky+h>|ke15?_=z zJ_|0XgADHJo^15ojQ2Y)_+U*0lNN5XREppiHa?*5!ib2i<|T6VcCzlNt9tV~e7SgX z@+p!u|Ic^Qu-5$6;uO7Ew#CZ1vx#>4l;k2yQ--qE zqWFyM5XeBLe!3pPkN~5!0HfqRun(WdPaAV&plZrGfASG9APGX9AD7 z^S;QTI``Z@DnI+rce`0|YK|+3oloXUzElgHkV9a+y4t*NgvjU|U?n$7FU1|6$0qn#`TMk%Pp+MMORdNx75iYb7i2O_?Y^(PA7O-u z{Lk+oOi`3))w1rwvqAI6ZZC}eMi41nho8?fU05Ex<|2i zzFmk{_GWiK@}h9(%_&f_>1Klaq`Jx*c+8WNwuN{LLB0de+9Y3PNh#79FPw zVjXmJ{8#z3G;SO2mlyA?-Zry(zC!p|%vgT9<70`(H}+A2)bPt3s(RxYX>A&7rBy}( zI<1cJ_PZN$a$h^6#10iA@pJc1DHGr;$46R%Q1b<93maU|oIAk)XHyI^g!ak2GaiV; zdIq1SK3@-8)?XHRzCC%EjS@EAHgzFt*2i82XTEgo&4otwSVL?m`1SSm8a2Ok9byni z_e8pj=x---R+9+j_WAF34UWpeN^x3SF1?DMzVnT`A$z{`w3~MH#9T> zXZJ$fq;0%f!TqZZ7|yrP*7oE0A+Hv{IT7)jN0w{_r`!rVCAvk{TU`FRr*!?T|M+oD z&3>pcuI2?r=c;d~fHRI#xr?8lHxFXB;IHs&T_GtURZf`L7r~($`-s#E?CFHwTdlX@ zC^`RlOGr~gna7x{arFMxE#H;F&2fh`Pxtj<4ZZz_twT{0tkFjj@;#w1yqTkXsI->d z61bw+`A$oqp|S|u*79Ybp{&IS`e}_K2G(PeUI(J{sz;a!&-wT9jjNr2lsow9mlVEh zBv26eb)n|_5J3Ifw$-Gq$sguki?h`XTj|&1I$Zlv(p@%*I->-9J^Y63S%#}AA`W91 z`lp-k<>Wa8Cc1>s<)=Qz&bEDF5-4VKtIV2+FgHW*c*XRB=3yKZ-mh`?xJW+_!@12ugcb3&In1d819yBCU!wE>)@vdxPPUIp;a;6=>`6P5sJR_r^wt-D65A%$G=Cm6H( ztBe^MZ3*j*KPBtfZJaa&`c z?w)Da-9SEe8iLo?2v6|-l}#dYWd?wNEuZef$R0%*3@6;FP1cuEg`hQAZX3weq)x)z z-lxge`;C4c@A(}HYv2>+QIqJ?@W)mEH8rEyn&P8@^p&a9W*;qjj#0x~6W_&J{{Tz| zciSP-zRk**C;GsFZqG3L9L~M-0~wK)SA2Cr2@`E^YGV~`y5J6kypB4vv*PDieU{ll z-)E;ep(!a=oH^OZ*Ynd#e|v#gg3j^nJEA<>*GCMaX`0h+# zA!V^8AH2B7iZ_Sh!B+Z2IO@;*I)@@)zk)QDPf>mJZG5&+Bh-SgwK%@ zhU!kfAI4W3Gb39*kjkcy83+>huDQG?#7m;T9OG|HxpTKX;03D@9b$%|Ngkn27JZCu za2h0Dd(;JePyZr}?mb5MErv4>jp7s~hIpmYz7zL2bSomielOhXb{?7U&GQuvHv`wIy9bR?jS z@X5d0!)LOtUM+2#w|*9e*0i{tdKzEebN)cZLCdb-$KpOjOHfarfA<@}l%vEE69t4Sa*yA&*@64w_wn~HS zth`Wv=J5^+Ab|q`oc>Pg)YazYT*air3qgg#QVVsd9bf9y_)n|8e1+!sq5PDUyQT`Dh3?tN zjfHz+mT~T^$D8+T__CpfC{SPerTpH;gqNbF*cjkW=GabV0x2quKvGcs8LbcCa=hu| zl;+Ntg|~bD7cVPLuAc@D-B&%=fa~aib*2zVf=X>x;@}{;``YMO^I(EGe;92z zP-WAav8tooo=-#7P=1~8>gMwZ-mUh0z8n;wyNsRvJTCnh8~W>0H2V%rZSgNwT5i|2 z#tGTUaoT_ZuHMMBa!|&Ui4Yw7g+{Tl`lWyy$>1=KTC{6s)EVVhBdC! zaQ25oVujj{J3%!nJs`4M+?@HwwE5!L48AUgf;H0Wfux(Sb zhR|Xo>P-Cmggv8)?bPL1RuD)%DvK|%n;wdp_$kbiwK??y2kzjF&v~esW(G~}2Rx~; z?OxD40)mD}>RBkO3?Fvm0KOXA340K-v%%ghA@)J)J?-iw%xr=EEGa0@tIx`{E9G|V zOhY6<8N!fSQ@n0Zx-0}$SQR%|I}{I*0+>KiDO98sF!&vF>h?UNc+pX14nf_1O~R~# z)@IF?#L>!KI4RDo36egI>DV7@u!YyqE^3Ye{w z#+)&#srExLU-|p|*Fl7|DO=+m2u#A9W9ct}5W$eO?LPL-U9tw-!#({&5UUS9FWLfW z%<-5NAy*515(m3irpkGWdx8>m?plbT9>1KC$e3BzK9dbetB9#&emcJ^`V-+<&4JOV+Pieq8BV`RgQ(Ig+77Ko z^TMZbQr;3fgk~=FRv+Hu4mVfDs3kN%kj>84=*}z7=-m{IXVQ-T_ji??uv)t?KKho> zYJI4k$cqa{5vE`ZJAY_XNyf9I(}_7(g9ch-R?5JqE7dV&ifbBiF$WuhFFwO5 zNwSZ3wM~spX2fP=M(Evo+wjir+t#JSRU4h7TJmB_5B<%K1@sS;d^CDx|@=}HKn#( zU?8A3o3va1^~F^}{{$`R?WqjTZZ_o;M}y~->7&8y&;CuFaC;Yp5QVee2~Ikb>%e2p zCU?5*1iiKSgx=m$N|F<^e4peve?IFb&3B(m?uaNEZ9W-?>KR)bXIkggVM7Oy9ZAB( z9Q>SFKEB_&Q@4?`AENgq<*Qct*rXywuW<^Md;4$z2?=Sn+nbEq$2v*>spj^!a?u(L zB{j@6v%0>n=OEzGtReNCKWXkb`TjwRraprx@kl~R&+1AFMp#u<&aPiOJqrqHSUs+u zA5qC0aC{Rg2zO{($-o^-LXKQ1hW_SRiCi<#xau_{=O^{8%i@{9bidPJFMMk~))H`q zPAt*F@t1)mbrGL@^G$(yu`AFi4LMjHO6YvW)c1ZiWr%K1Su*@3*&z&uc(Gh*{4A$A zno*RC=B8)-!7?@cKrqe^-OlKta8Ne8%xpUksGu`7`QA{!K$$!F24y5bN@2XZbRJXm zQFKmKj9tOCdD2vkW=KHl+n$@rZfkVj^s)636H2wjeeiqpATls(GV3c9^`Wjz;Jcdc zsAQ*@B^8?LR=~P=C@Nlac8Ba%ylzrqH~D%0r82EPZ8(?JY4gu5+l=m(t~eZjwXH;+ zOES7}7!7FTl~9%K#2b4Z3)qu}NV41PesEx4;dBwxli6Y1V>T_5nC^RP_t=Q=eO#q* zzY%oXOCRBaNF^^ZM)En#KrmKoiteX_JtHZdKX@nAAL>-br;s!c@os(oyW?2NrS8xE zwdsx21|5y7TV!Y{mCzY!SOhspPPaMW@ZVLt3&l^SbXZqp{;JM@Ql* z@vyzV_l(Y$=1<8fWce6MpGzpVxM#}4-O23PL^zU$Yye(BjJ-TcSNdRq{vP|*fyhDY zBF+7~T<%FPZ814g7nrL942OFkcVKY;k=}igX5)z~ds1m5>unDT>p4r}`E6ZZhLEc3 zJKT(>)Wa42Z4;qre`_&S&wWTTJ`Yo;@#oH{VJ>?IA7SzzU1vYeJ4ZVCt%-+99d&01 z8n|YzrzoGjpZO-&nNo{ln}%VTvLA!|*EwUsf%|nf%DkNcohrv3KF`7o8=4;3r2?4g z=XJHG#(dP&>`$M>;kNYU5LggEF?8Wzh@6D);Yk7cwmO6rWT#a+vp{8>ERb8MDj1QG zM`1f9tSV}2^EYgDzARiz(~<;P$E;QvSGYcz4;+;SxozYwH2anTC(aMJ!eMk)9PkR? zf9!n^{gOhD0%h_q{KXe%V@T;8E}!lBx)4~UpED>}n~&*T5Y>KIc|b9C zR+wC_?N$0&qA!1QH}R*S)Dwv;5QbBt=UeNc#dk5@{b|^2|KU#z@2X<0#-gb6VwU@be(kp=8G}DyT_J4Y z)m{b1ojj}q>E%>w(sPsjOql)gx1cr^L5jr+W?NE>Jk^w$>D9=#$AE2z}c@uq18sUGToAOCKJ^oPA(fZI0YmK9dl$Kx+| zl;Cl1M9m8sd#sInx~bf>!Th_D%U`_jq&x9N<1x-uF50P|d6}&&SiAMyzBlhw53VQ8 zR<~JT9y>j^IKc)7TVui#ug5DYhqd=_5KTJfEFBdt!b0{wq!Q4Q$yl!IM;WiGKh+~t zkdcI5Ula88gCqFs=?Wi}=1kCJBUXmy1)4Yj)0^zfj9OTGZ{2*muK`FC9x2(qFQ5W- zrW?5_`s;DZ#6phmqrvG-sj<`2O&=-+A;D;b+K#1?@l)Pj{o6!Gy34)3IwOwo4o!yO zsMiBNQTu}!#;l-u?>C`zJZ0cC`vT0Xo}Ql8@Mfn`A{xeuH5q?P@BkHzG}20_`D^Ju z*y6Rk9+T}XFzi%If(963=k^NP+-+sd>^;NjxEEBQp5kO{5M`lI1H>DN-dpY_9uKKY-@ozwWH4 zjkMn!ddnr#EuzN+@-&ZC!;I0M8xCxAs|iby9Kj4L*0&2sH8z?pL5>I^KuIB?{&7(N zHCGltGfIH67;=l4lExwAf6Qz1=P1+z7eobFhjX4@vlSt7|5?H0V~LpxBS3-YJazfq!IO+#_Ih^VA@ z8}$GNRtUE|BzfIh25{;h7J$zNoMI&UzQ*Vc&GFQGP$!IV9I%~82;?XpC9 z2vNnygtMMTK7DGriNDj@!)spQ^A?lt@$LoAW0EWR&#&$qZB4BE4|!~0YSz~Yo;+1@ zp5;oOHe!YBg z)hs9`?1Hu^+jSjx?&D3S^RdK}J3N!9S{z`9713@-P-^*GL(q==xZv+}pv$wfJD;gn z1Kvw%pq0y*x-(WuOfE%-2Lx2!K!p+81fIpNN&1m$;46h|lWB!Hn15j$3fXd#Ms(d| zvzf4>hBi>2ILCkl{`dId0&2-vIS^D>KM%f+Oxo%|yo#61LZG7NLA-t`nVoK}i-?M9 z5SmzTK~Ta(5HwePZKgjWw(Qq-UfOJcSZ)q3g*u^hYqVL8`1VccL&)0q#E({wz_RLQ z0P>;cyD2@OTJl=x??$mN13+GOcQ zZ9(x)mu*YBXd@h){OKf=k1Pp3EGoZesVWCfO z*$l9Oz2AD{WX#N9U;+d3lM}nYaf%xwA0Ar4lZ$nkR(u_+Z04BAzjgZXsR+q>0@P_6 z`1b=Wb{3fKpO@;swOyU`o5|zPY80$yh?^Gs`+RbkGG%pJ9b&3)<`pCyTK8SE#^yDjb!RqI^txF&@N9b;;^JwTzV|&q=QfTOEYkyqEC-2+wGw8RmoBg$6`d{Bp zlzc*#FNij8SJ1tr=}f!hs8nj>AP!7Zj*O+N8wH!EbDbZ+AG22$p|ES1n5cxn?w=g| zj0%hwp49^mu&@xH%GE+?v1|@guGZRSF6Ik3xIY{NcBRj#LwzP+@rgeeL~Mrd5?<(M z`MiblNvleWM)eq9WRhbVSc(%$jZb8fc=K)Iz{7F4YegcV?aXl&cmlK8D!dt*Y7FXU zDXQ5g+^c8DQ|ym4HOahr%4n2X9|GoAACm3-z|(pM+C<4L`|06p{F&kBm^?cr%&xU} zxM36TattQ9U@-HdwGB#EX8~AfuaRUufyg9dB@$77(^}0I`*Zx}HRkqNCN!ZU<+#-X zP8n>*ir_}{{CydzxNmr||9s;(bHinWwsQ3Xcn{J7*91|iOZtmaiosinONkE~*XqXM zewvrx2tyTeAye#U9jYTljHVw+ipnr}z?TW6LD#+00GeN$17X$9GB}bo2LvFr5UL4D zXv}l2{~WLRSKt{kV!wJn1E3deA&Y#`x8p;u`_Ww|U#+SM&Bnc%)09|U<88a7(M9WB zh7j7U@%!TYN{4Z(arzttQaO@rAyVo%I41W%2LuMgB~v3)e2fSa0@v1LlqUXwm8(wD9{J25Wgx7CJl10ByW+lTDig?s<0{|EO}7L%P^WIMrAaD zQ~C?4QyUWkluTSJ(U=)x?tqQ@!AHf&2a@6N2aR;mut*UL&#O&!^6l`4BLX?8U%}H! zar_fmkp0&llQktrJoTqYbpvh^(XTASp;klu(92~9~pqcMH=y~*vy9tFjXj>|4o~JnM8kZ`1EN*`> zc{#ayRGyIP)@ubU;fGfGPve1AazIz!!wr;^^{lt4?I&RL@&oneZyrjlQh4f0>i1{5 z|BSx?J9(hfA42)%CEl#XKB*G!6^{;NvM5SlGrOi}{h6Gt_w&xDh5NnW*PK&1J>~rv z;~LR-ynkQLf8#(TtFx}2b6 z-ltQ7fL-5&+a`oZ{qT(?Zb9RwuNHyM4SoGgKtG2&GL-dyPW3@&|BqB( z)ET!Ua?R#c@zbs$k9c>pC5x-PM||y?e_WmuNnlN!kTQ%?+Nrs|G!?bz+WulMrp>p4 zw*yiMeO$&)$kFtAwj*jgXNbTVcGNb;%Slu>duIbE7-j)K zKq`0BQx3Pg;-V0-yaWLNwlEWOwgP3K;o&gBn&g%oVTFazeNZ?|~wvMoXC zomCV42A=OcXj0ZRd9rdwG3T{)^0e?atz-69e+ONBvngWsbw^ArjE&2KfU9oN! z`?Tn^dE&4Z`U{qBp?^>8$DtJUIC2LO6cel5L7U&8g-vU=cwM^6m#?re5B{mg)Oeci zhW^{(Kcv0P!zbAu)3KF}3M%--$JCD4GuY?V>P9_7G)`f7r`q-`a-R*!$7Tv~o6!wI zH&*=R)8~1?HwJ;NZ@iZ<1j*{hhQpPme+$inQ92_3_9J4vFa7zivs=!#@8^Ou=aq*QZaPo+yd? z3ufFMP8XGs2!?lWEfFzfnWhn#Fx=ax4g;oeHjRe%Y~c*r%cgDSV-pJcR#w!KOkiRY zKF9;9qP%03{fEj&^oPto7;H7gOhI$gjTC{H!n$&e+`zkY*9j@dlb?SPQpWT_pAhto zv}D~dJdG-sY=6fJUUi`LUipma?$GP0tMIWagh7US(UDWB(sVU56cQSvWWQZM;FXf~ zclM#!UPXj_GE07{b(Qy>G}(rz=OpYcOs%uG$vdT$k_O{#iymJBg0i{x<61Ib_zwU5 zN+Ge5h*ThVH-M#lK=M8a{k7JTndrTe>^_f7Sk=9#sxR|X&y!53(WUo;J*e=Dz}&}4N65UM(b#uglkFPNt%i*{4=KOOl2Mr zU4%WE@Mly_$P+M1>&bRW?UJr?E(l_;=5*Nm>y_#DFjFZzLXU zpAUSbk~kgu%Do5O_d^ky&OF*Jxi3@wgIl`^a8e|qYW4`^dkEGfR-76En~GeTaq*71 zhBe~V$c}C%R{;zKQ+Nf;h4|J=%R`FV_~m5qt?4poHhH_Hjujy&cPStyYBg205kQ9J zNv>l{gn-SS0}5&gOvo1~jUXxH1%y*Cp7PG4yjm9!Qj;jmhAblJE&{PHFvc1HPMjG-T(@{dJ`U~s_!pVGpWzcCWh8- zFE0=#i06X_I!HULhIITcmD?|gbU#i#A}{Xuri^sWg;xn(&-RvjI^Y>;NRN28ir>`@9_OY?TgFNH}v_Mo5DQeSh}RI zvE!swq>sgRTFnO}%akwioyPoKyMs(ugBH3R$5&E@w|}_^^#tO+dsdtNC2(aa^yTXz zbh)cNM7}=qch&s3A(1b_n|!wVI}nk{XMKc>B^>G@4E}Gw9T3RBK`{oFAmT%J+otg2 zeV!N{-CZzX%vWt9VPPTm1?!vRm*Gh*mxK-mIG~=$N@&{{0q@`m;0cdLLT36O91xt2A-H_LC=F4gOg1nP0 zH^KT`q>;8(NehbCUDYZQ55klQ<4zGNDqxAMT4?-gH$u!W^@h+GW7;sT}t0x1GG429x*(&g%ukB&Sv!B&%j4z6|M1z;cuh}wJd$JKQ$IwY9^S`z~ z$n}r{PFQCbV0LZAy;TG=EK*V22*E@WJ-}l5%@+StM1?(SYfyildVf2`|GTw9+`=?fU^f$PO4QM#j! z3IP?{qXo%(alzg;Dm>Tt%G^T@LZLJOu|u z`q&ec-$$Fu3zLFlLdg@#{7KwZc_mtK%1-QCa&yM(D>>oaf!MXo*osjhwros9BZDUq zU8BH^7Y;3>$SXu)7jZ8q;>X0#l=o<>M313QY{*ixwR`xStL(RA^V#dl{woqP^-W5e zA26=~$nb01a`^ZhOj%W{Zr{KwbxAQD;ac|59K%^2(OA*Kl8pRu`0diTrXdQTB}Ct< z@B7CQ4af095y72Qbqe*KJD!Co{kerOY=y~npAnUsR~0s5at`iFK}^vn66hjuc-U3j zPW?>TM@!xFp}&+Ee?<6ZERA&b|6V93d!)URX-TWvr_{Ws?-!U)LA#NJhP!BrOc#hV z7^E{A+Ejumcf#_oKy83?1c!a+lBE7QVW6;O(MF(-rhp~0ig{TY z7ZsH0F$LCypy*_Gnl4culo;u9Ar3Rk)0?sN7QL;4jJ>=|#eHMYRu zG?l zysqo6sL5w4twj{XQA$r*>>ZjGh~QF}T#n&TTU@TE`Q%Fs&D7#nQ7Jt=aX|(gBkg?z z9LZ|DVI9}MGi3R9h7bU$lTv=x`SsYH*m9r(hU{?IOd#!d^i3<6wy-w2!^3R|&dLzB zn7HK#n5m>-ww8&}Rb~;L*RhHt=BI{3{%lUdu8_|gua9!L;ul*|?H#^ELn3Hb8b5m( z{Z>4lZ~7i=aptR)V9&H8W;lq3Rg5{AJ&vp^THU$PwNpBgj$=Q^8zI&2oKY^>Ks9{t zzw-=yia{u<Wn!dzNb2g}c9UcU0lkVuL0fK3`2tO>k71 zC1MbYa~KeFt7er|`P?Sv7^$|1u;O0$;_jB;ALOhW5cm?mgSeTp#;{jd>j4&JZR;)1H);DO&y>g z(6cB|+@0S4R9|}ojvLB@bK$Gnkpj%&lkWb5X&>xP$}R|ioh{SZmagw*_6GuQAx#g8nU4o@|E?3 z%h#j~i65ViK3#GypD!$1>_#>{UMu`=k%v*@J_RYv4cPYr&;|Dqt9S67d!X}eMIjk2 zpQh#6%jTsk;iI+eb|;yL=JwG;p9B!y2+tB(HGb*(3D$OZ=-sFx2BS0q&>zGM44r>B z3sHc05R+4pb^Lx_;&5uCg3+a;=}ut9_5M3w{7)^SfGZ=gWIj@wB=FheUPYFijUh?x|@YNEzpo8(xBKJDX$y8W$f$V`#TBC%C)(|cM;O3C{(pI(Dqv%O~; zMb(2mJ>pAu;CO{Z(Xc4+v*y_8)7t#14e~j(fZ~$t3ChH`hr^7JC_Wd%iTY9fLJ&H6 zGBj;9H0+B6OBF|SdITZ4VF$()UG-sU@+ev{f@?5=wZ)>metG%M#MNXx2PO>;qC-XxJb)PV-mZF{5-7HkLnO*_#~1hLHe|}3c}7J+3GFfL zK5;}cEIJZH|HO^8k`=jG6eN?Xgez^mbBwsVI6rgBQ^6-P03qc{mW$B~8b8CYR%iP6 zZ(DeP5*bL7DtD!BXBg_2y*5z*00PHC$ebaI6D3ZEHBkl+yc+FFq$F(t78nwmn85h_ zsauDQ|5;6m*6GUcH&#HeRM|xqSYcq| zr1b9qC`T#Ku^BRrPm(XVJvk8TGibmiP*LR$1uvn#u)&lDvttO)Qld$RFIQEm3MTj4 znwoHjlS4$Qr<W2l9jc7v;N=01c8Nh0m=)xA2bGZ zAB;!d6!r?4FRTpK0qqU+UNr(qO|Ba<%JxslyJZPiz_T>)2p9P9N!0HdHcAOX5q2B5 zV5_mCs6y=Am?e!V%~7%3`Orx}6_G+V74F9aG($;+rk89< zB#kN*y`ht)Kk|KYWm8TB!$wuUTKqf4IH{Jt-mRE{0xU%kmFkq?v>HHfI{Am9-1z~Y zw?Y<(LF{KzTG1-;WHSbZ!EB6+3msnGA&eR8>WE%02#C2?#FGVM8}{Q}46*;rT{*=d z7M79rwAZgJ-R_WY6FP&QW_bwlnla`?Mp8jKfiJ!m)*M|EUk1>-C~x+|hsfs7ZM0xW zh6jvl?YCCB|1r0v-^qST7kzD&uuEpp*F?0Y3@oW8OT=JT@-FeAi|ULZj8@$LTvfU* zd#$8nxX3m#>TsJHg=oWBmTQr!2~X8kYVR^9*UyP9Fgx43;6?ZUKK@Y*VUfTDI;@O}{G9EbRnyOuU)x2ItzVO2m@yBEVAjU#*PL?hb z-us4QS*%)WbvMK%9J>5Zlii~=$O5 z(qjl@frMvC$${n-JHhEv!jeky|9iso=W2OJu=;h~^bkclGb`oHz(nYKFYLWnYrJc* z&E921xm%urWReC=a#q?+hPnO6t=&-Qao=QEpXJJ5_`aZ>(=}(Lf_%r|X0`8c?j8=l z$md658O$I}b?OP4K$J!et=_-keB!``7_MbcKwtMJj;${9Yz$AXozrIN7yKoi?EGyFd~e-r-IX zhMG?e-~o1qa+WeG3l?t8Te>{oGqG;}LxP1nT)dW{ z(gv$+T&-tZ)Lt#9=qf>>g2tE#vz<7z0-{cRj*x(U8;tM^3gkWTe_tXVB%+MGd%ZaL zvO=+AU2T{jBA)^mx$)fc${7bBr|Z1GhJ6ClJ6hsJ8qq_4eB3dRy5J2}G?hBp8a*G-;h=<)ziA|QJ!73T<&fc#`;0wbJ!VX> z)Z~Q)*hLxk#=L3P-76V6|0E9(obMQWMef`&Y=f;H%);S&XC}m1#J7XAHzkV5anGt& ze&6?M!nMC^#s+S^*gxt)4hY;z-~c!UW0 zi29Ito-AyudRX6tFghEMq_(O$T^UXuXzfqstw^#nF%t*@58&( z+nWSK@fT}Yk|4@sZ{kweC=%f-Tu;Hb zU_taBc}dkLICL#D_Jo}rU%E1lz3#?^%>=i^Mvw_f*Wu}o1h0o7W|q)+QbP>VMGLP` z@HVtYjgif3EsjLBbKu-KUvv0=orR8Btk*}7pjEooO9j35`hz6-Z8gPVM{)AxlJId4ho zwDk{f=i1`0D)w~*^zES!>l5?TE00IJBK?1;xUQCrI!EM9j$c$-O7qYT<%&j~(I zWtw6(@A}{SX$-!vbdIc|V}`$9VnrADC9Ev^x|1p8c_4i}QV!3@pb{xMu4T;`k-hZ7 zzArYUB7dl(y(prQl(j1_d>{Y0`AROz{MhN)!mW%*Hq%%Xrev&M>-h4S$H033#q~o( zd-kou<6b|XUC0r8xkMS4*$&nq=6tvYWks2-N6vDot|((?f^S-R{3lN|gPb$N#iv+L z3;K(T=}Fi9a=vL=6!O?vl1v#=Li4?F!Irz$@$5HLczt|`qXQ6e1Hq8q%9Jcbt;3J= z^gCXe1$@%o`=HSl#vT{V)kcC(#Ua& z@8tn>hM^2Z9wmsU5O6@s3KA1lxaB`Em5F@Ic&Vi#dQ@Q(&i`-4>+IxOUiP?)ujl20 zw&%bNrah|U@{-8V;1@yp^m^vKjivm%UX1jRXMZT2%{0B^8P!7R>6l|!va`0syUb7W zGE~&ZFn``$#AxRA8h<2$0aThC&_)6gmPE|7?7@%BA{2)y&9vi>y~PPd=i{r2ew!V& z43xmwiNd-NI(E!tQ1jQs4enx22=~HsKG?Az`6Bk;A;YS)SO_2%YP=ULW#4#MYS`P~ z-ageqx$acchg{(@B@_Nuzw(yG;&kC3bbYzDV6I{%`4ORbe|@^S9)z6~cbj!d@j*C` zftXfQ6;KOMveGMs@Mo_Ymn-jVksY9?H%9pHoD@?nxm-$eeGL2-yZD-Fr9sdAyk)mg$K&X}91Z0a_ zz5@NN%O#ynI&&*txxdzDM*AVORK_|veb!(V-*0uRfs`GG?x%q(qpa1C`2wrdXX;B} zRu7vn6e(sshD=T}V#82tN~>BSJx!#OtEHaJ-b>E_4}?=!oL{)l4kVJ*0_MMc>q=xoT9((ZM|F z3#K}qI(9QAL+z!6DUM#Xwb**RT%oSbU&gKsOTjxHD4qH1S5D|wlvi}#`U2dk4w7@^ z4DJ6rs`rX(BtQ$PQeDTTL^U_peq46p;m#&5mxS#XiH-DL468d?v*Vh_+`#J9QY4|`h_Kwh=EUIh@kwOvSm+PFU#*0Ig zId3VIt+1<;O(eF^&i!pKU0&})>MZFSk~xFqdwrd3mUF@LwrH&m$K{bf95OK{=uBLb z2l|}t2Y|`z^r!RI7IBL>l??InQ*h%S_js1>_b;j3U7pY%lZo8aBopK{n=m~@abE^P@EQbnRIo3P}-vKX=5 z?{nvGiS7_01TqJl*P$OR*1kJtofp8`Dahy{K{pXFwvYXHY4>OC=aFB4iODDw28hCy zMgL-1fP<;NFAJqHBkGQhi1=4!CAN>o?|e)K06JOeVL$&5Sf$om)F3~TTU1g4mt7R6 zsnbsO@>-jbE}<%ss-vR-J#VNff2?*nvqbkM2#u_&PXJ>AcoP+i?1(=Y8gY@0Z6Z<< zs2=>N&)4%ha7yepn3A8w?l zXQKap;>CLv_KqOG2>;2j>Z$=hNS`wZ`h*OU@H80zp*OU`i$NwwAWcgUyR)<*0z+bj z-Y6b0VDzo@_Ou`VADavS$9^@6qD%U2C|vSb$suRVZS8$9?C@Q}S}m z4!jJZ;Ng;NeZ4DYmn7_wR%k1w+m`?BY3qdp;bZ~OMpq74W+3mC)@I;z+_KNFwr9dt z${0X9OM*)=hb&#)C)jX6EHiCC^Y^|=1W(W73HkXJOSQAI#&SofU!QV%T!xWZK1PhT zRbwM2f_-wzxFu`m=S)#!e9gcN2Q&KN8)oehSU^4fF&mLgx^Cn=D?z7sBPigtBU4BJ zlOopaiob<_0^wLPcvL&oYF$^olR2o--OAti<;EAD!oeJVrKdn+#WU6I?9)Dd&d9Iz z3)aD->q96c1p0Qj*A0=F0AVXvs)~k>HoTSi)(~c`VMS$GtI6eNNc0^MPi^K4p#Jkj zlb-)UK1Kq?@~|^EFJ3LtEQXBA@S`G-8s6Jk<1fcP7$TfbEY@$0rI_x9r;(vL%^FSF zk~ypP4lhMEC8K^#D=Mw^rpl;Ndu*MU*7BeR4QWCoq-1w;u}1ra;6{$qEd>lj&PaoH)D1gUh<@E~B9) zUkh!%C%CwAy2Y}}mx4v~D#chA4kf|>{65Knht9Im!)RohDs{G!y_I%0Rc8Np*n^Mn z1(58sUmIHytxfHbC{Bzr#7RU4W|Nbvszm-i*D_)^M*J%R_{ZReYX5OyC;^0_j`Mby z4jONo8s%ok5Pz8Ja6)MQ4n7$Sl#$bFVf}0^6jFrMw8^u8eXZRq7!S%!`z5O=EGmS) zVv79|eyu9?taRsRqLp%C$2Lt)A4Ml9h$PckXrIRbsNG=>y7MT45drPx#9C+XRnsvU zupv?ZV?(6xAwF7KT+pP|(Mo+Pe&g#j@X#oezz&oV;z9UmPwsq8W!qE=Dy+Vjem@jJ zQuNZd(z!IF=Ej)iF<`>_Edyi!#jXy-a(SnYMaMUjDD^{~Aun9v6SE<WX@G#T|Dz|P;Yxr@69HVOrUW4K_itBVl&qzw zw0xA`G<{pj!`V)BZ>614S)L)@c~)#qE0a}mZEu9F7%XcZOKK+2E$7m}80oa(aeyw@ zs5&K@vea{N(ZU3bCQ6DdmbaXaN898G2Z)DGJV3ow_$i}21ZuXR9MDFWuKZfv@EL-$ zxcKIWo11A>bP4%?WZ(aI!Cv%#sfn5zmIvIAiS?{MtxA(0M84z+vK+q?PUmcYubh4A z$fe)2XvNq?xfJbzP!vVN6oq`v|5?&%rpWSNT7XdK@XfGLxWDw_aQWNF$fB(_v<<`( z@bnTgF*Yk_X08dRW~>=fKEARPX=DjDF25gqvSTC&m*gmh$mGlu+$Roh;!u=jDbRo+ z)Bj02P_V(0(~{1`sHn7JRR$lUNMd0Vnqf9NIBTnq1;tvaibOwWgK{YF&^R_YIZ^?lw(+fglrW{z!IE{7d|`h6ChXP7WZ^lRK&t9j5RdHP~9 z`yzL|NSYcq#=LPRKGIF>XkzB=>8HYm?*u+*vT!6crW+imwm@@fk^vKPzug*UnCgz4 zmv_!6l?iv=v7POtGs@eCjK?JqeCuVz#dmeF6B87lXV=viU34Ozi_hsDq#e=!?c$&E z!;;@f-4#&H<Qv7hXXuc& zVExsi>CL7CdVYv+NNND+-H5Nra!m$!afCrd?KWu`>R5#Agz!LYsgipNkFu^5uC|m^IZY z;d9$ho;k1_@LTf~<^T!oHJ>2KMoi(hPbU`3KB|3DWzD-t;>OqO~Kfha+l z+HK%V0iVeOF;t>H$M+NUfhN!XxmD8GK0g(BcX_=)$4P{evKvftDzI zP}_*jX0(yT84hROc4M$@^iEv}R`qrnDJ3s3OCH2To|x$-J?8g$sWS#UXij0i70BF# zDp#C|SfAMJPZ)IfKvxr^9sTO?<^%@6ec5S5H*%6BPUAR4bnS^$X*nLDWFlRYaeMI~ z?6~;R>2iLf>oF0-)mW!rY@GiwCJ1`5B9L1ezPQjI38@l@-?X480ZyOdC)o;7d%TR@ z(jdu!?m!THKA-!E!sGq`b^3X0Vt`fnhkYU1+HI>f_twY;Fz7QaKyQ`3Vc3U`r2jKY znCN27R*@9CwDes;i6}X}wnVsZ0TF$fVhFdvG_|C)I&at@G#u&7VYslWniN%sd!1(xz~$ZYQCt#S0#l(26{ zw%vMTxEycW5p+8uy!%OM&UQX)j7i&@dxPMphWrcb?>^X)w7xi^wouw}et+=+FQoG7 z-=Ws;wP})QtBp6@`=gAGOGhIxEi!x!fIF_8X%5@ixIsH+-q^D~=t$DW#^>N1jxb*v zI{V;GAu!_#;3eQ4$U{Bv@!=%YTH%F$LV$(Ka)*k%E z#69U?Oc~O*$_LsnsfeJOp#GO%=ZV9FN?}^Q=o+&w3l~WPo z^k_;d;n#sL=T+JW(a)Xn1M3|9P|p$4*Xsj=eSiK1&_hae;rw!f_2Dd`4~N0quB@)k z#~6WNSN4RHy2FL5eBn^mX5(+hEl{(w{ozsyXzdgGsn_ujACED}K;;Mfv)^VW=c0R) zBX*nhGOwH0io7GOS^8EJDyuJU;RT&n49P?dq3CvUZ&u zi$?u~1JS{CGw0Oi$Zul0qjJ6zP>eX)7_xreQ zmTh01=djQy**6(mqEOew*otDKPbpS86M|{Dw?!kfOzMc#=%@4-2brL%!ckQs_!9%K zv?>+L-pU%$g@g<6rvrLn$R*u;ul*bIzqso?;SnRS`ihB*~v}{FK zbsLUe%!DOigj=TS9lZ4A!;N}4PoUrM{d}ZT&&)3iOvL3Usr5tJVRz=(W5&7Y&SIfY( zNaUr1&Xx$t`y_6khwut3+4|t>O2=+5$GmzsU@Xnl>fxXTJ%%HN+k_*le0(>>*Ej9( z_b-)`YnGc(^qS~$KEu3}M&GW$gB&}`R-2~w(m)iWOeyi@!(YKch@GptCJM3p#J8vC zq6)_4)0{=mDxU*vS6#M_R>5b_KYj?fFAcQYeV0>WFG3-u{}MI5$YoEA8}ks|yBr#3 z)*ciOp&>XNZK=fV31$ZAYn#eSkmto^`5B5Uw9hiSFs8k*qNc6J;*I5?c64moCm?iQ zO4>WFQO`+!J@ui4VL5Pm5%L75g>TZ2a1xdgDFuOi?Cj5TxCo>t z6YQzl+^!^Kyt=hP8YTC2X48Z6kwBxWvIBi!>SN|0Z7Osof~^iwm}ng|rSx25stp-$ zUAqHmF_EY7`NOY1u9M{vF4qs5P&>o5eGIt^fyjLu9Tnsmjm{vg)sbaeSb0z;Yw4t0 zw{-dwJ!2r67t_0-{#m)v(bNqniRzpunW!Gw>%zDFi<~L=5ZrCYU=HD`^vO%tKt2r^ zBWMG}BXj;v{qXhx!guuY4&sgM80ER9A8o(;G;b@z)$Do4R%iGtMD-8hHg!o+g89Yo zIGvAIw(9d!ilcHruF=yIMi5^)U`Cl)138lCLUe~Zd|T0!y-(LK?R@S6P9A#>@@L=n zzeL-{N-9TIU1lR4pWrDbPG595(TLG4^M_lLQ^eQ8kpm_(ve0qPEdY*ErqPz5Tj+aX z5u|3hrP!y`U$)Rq5VU&gT2=5I=um6MH%0bSQ<$1?XG|dk9G&ADZUG z@T*$bX7O)&6)X0D2%=o7#vmBuO&fudY)3cl#_vgI9bka(ON|WX4Wt_=Jzjo)5DN_x z2bYo=vDAIJbDcCf-Q2SCjS{@&1YLfbslEPF_uFwqH%-1+a;GwYp&~0atVPdiH`!0hq zY~L%Wkn9gJANxg{GS81c^ajt&b+AB#ud5-I6=8b*Q@E4Ds|;W3cvKhhJhL2+Sw~;5 zRW(_Idj^4rJx(p}bvyUNJ^g6!^3!&-0_(w1GZ&i`@srF}D(NfeOjRDamX56DWe*OtuphyB!{PWfe?*WUz1I$K# z9mp@e5q+b&yq7j&*5-yq^_G=}(`D&4gwP-gdNgSyWI((HCE_ndLeVq@M_0w8^-=W#Y9wztFZE;Qg68wBCz#XCU<6$91{C_@m4C^V1WWOV= zUJ#$lJC7`e-oc`*$M^R8PuQ6!!^X$7PH8uW)oaI1O178PJLtT87DUSp9?1gk`zY~4`@;1 zmKK?V6-yxUSPWA{b6_Mxj8O#wMI@aGGG#!jo;(Co3wJ!Etxi^;YLauhxMXETxMN8} z)R$+Y`d$aSB5F4+iy%V(r}1GmX~5#|k<4ArDVm=9h#3P5#)0R8XM zYQiB4;Eth8|I>Vhl`&TB8!zbpiOJ@QwIf{VcR1*Z;bm+MqOH2SJaVP) z@7N<+y`Ir1;XTq`pV}tD+a2Jz8BH%fsyzDVXdO=YGc3Ix9=N^-`;TBv;QlDzzNvYroc$ww%iuw2my@xgmwBu? z@HTOH`tamh@A#3nc=B0!M(}Ts;0KTI`-s{20@~rZhqYwdpx8V1%o_-|BJ<0{ zGF|r32-~iBbwLjwS9y@Agk4X-sKI-E25)PKAJ1+_&&Z%HLF8=--&ZW!IX%Aj@~#^5 z`S(%F%gN>&v3H+?j?-D7SrYl2@taLQ#;#-QqTLWn`foF!={kpt&4x5j(%^|oX?eN% zd+*yMc#`Zmn$cli;SB23{fZ12wRqop;J*lu*rdwCgTob?RR)xB z;Yi8%6gsz}7};o%6uxaHT)A!j=ZJx@EgHkDeTRkN`9$Jr9ocScDM_Q|I#%ZKsj>0( z65ACtN6K9CW3OhWYsfi=FWjTqJ!5$wq|WJ%=KS#(`w*J%4nW853!tk%(Yf=8iUYkM zpHYP}pV6oX?+nKE>|V|tq4m2W1N*JttUX3^XaZMrF|}`P8;)mt$U=9z>kb1t+&`)q zS<$Br4|b3*g{=e``0K)Yf0>>yi*P*|GHQIBebX1XfJ^SUn8~br_Bj8Tx^P?ho|5{p z86I=a?s{42*^9GoN$2^n_j;O-Zc%1ImfAb&YA`UTQ@w-j>J>EchTk34<--s%R*^i6 zOA?5a(mwh=>H75T;_melD7*{O4;nPR*ePG|iVk4Mjunr25V5EDh8%ltJTd>_xKVv! zVvc_~TRV|wZhvPk*rRmEBB&ghwcO@mb#na2gT0>&2(dKl%`jz>>53YiU8Yj*l;>i8 zs`iF5qfg#_&k)mGtYT-(EPUk?CRbYJ@uc&Y>mfQM4#XiGC)w>^x|fmBU`B1p z_IRDT)!A@))vOM|!GzU+zrxM@=yzCs&WIRuDgPHc7SZc8G1;xR4`a!n?e-{S4~~I+ zx*-e%6Vp-kSV@LoCeS~Op1#r6b$y(~bzMihc6Dv9bUnV-pQe<0;r+1V-Csm)HDGQ^ zrqh!C8kgJQ?V5Hh0ZTNn2zSU6(bu;SuWdIqr2|I;=iJTF%N)zij(rP0H^0AKU5oVf z?DnTXiz3TUTyd;KY%6Q_2Iqj7$BL}ffKo||DQES5qiA~F1;p)IZ01LrA(U3EP9?Rh zX|?!!Fl9h?eU&?VfJ)MTnL(8lD`F}*`3+EEZ(a89#GK8Y6#?lvlEBpM>8Sg+h$ucYnMdub0sf;r7fr#01lQ?T}?2jfV$kYr=bDnH} z$%rp%^=CRGG^nE5MzqEvaAadHlPAz>^tj4u!btSG&?*G5BGyaGW-sn#YEnFaSHE8| z;-j{KrLW%##L-9ZKyzdQ*bnKsLL8a@#-piZmfurVJ?)?tu3py)!7x81T6Jl2LD&q% z7kv(BydKbvq$R}^$7-^nr-jQK#UJ7q5}sKsL2Q&d{Ht6*qb*4~ubhurR>^P8U5hCK zPfpTxm#-pYL7CKGPhX_#xuz_Plp=Sp6%SZyS z35zj^UVPubWxxKr4**(n_;dE{mq>Wn<~3~OYoI76&`i zSHN}M8QV|jrv=+Ew|Nf#d^y>mV$oboQoN#aBsS^#FSUtGxcXxMrb@&2)gtOsDYn>5 z;_qr4iMsf-E7^^ii{Utkc^&0A4bU*ab0e_tES&3gcAyi_?X?>Ewp+1pocOyMR+k)5 z8V{(F0Bq}?6bv^P03jkdVE^S203xj=S-#kzRTpeDM=%F8E|8I7*BS&rE|~Llb8tXU zNzt6AE{GHy7;jV&wLJQlYLe^HNGDyb2|qvIR&|mi=206FkG?RkdU6Uvq?}_`$LSW! zySj{w{vNAp{$kH&mJQ3C1}j9TH5K#~xHFKk#K1yk|1E?YO&BQ#7&Kf7ArORv^xt{c zP=}F9s@uDPtC#ZnlrdmI)}43sv;G9X-rAy9=*L1RBetr1@DX6i2Q+aTnb4%0%^DkR z-!{Gq>ZOC*BDnwB)|{TV4AcpJ_dt?JDm1Ox>KP1%!V2j#6&dOok3smpKe-oqKqHj~ zK`qJ*OEI9T_Q?6|oe&|Xw@ixmH0dnjKMRp~#(TJVTWPv!V$>Z~LaZ}Z_;*^yA~WC( z%@=0lneOf>*uimNYjF%-J|9$-X*fTEJ{6i)XrJ3I)(E^n>UO$COie3)&lvx%l5cCR zA@p6Q-YPhbiB8Sm_#0xWg*CeoZODBal6g|$Kp9(!$qU+a!xVf8D|nl{44t?n-V4}r zDUJTV5Zu-%b!r0xwo#ou9LcwLib%2-4$&q?BQ8l|?w?K{gdA0#LH)g?Ts5`VDb^*; zcy;|0K378uHO;=*M8f@3<6iwl+)*;HK!d^OLb&5>`2J&#mX{2Px(1r+a6|3^8 z%gUCdr6>ckh?dm19E~w00qa2!Rpv%Qkb@U#Zy-!Ds%ZwfSI=yM&Wqmb>T zY~eu%?rG;l)e543f5`oRU|f&!wy$}r-%krNA2RgZIWv3)ZD056Bf`V!198gI#^_Hi zH^y!z`1|~ziH>okzXbQ-Romtd8wC7<{9{NzAT+3Rw__+t1z?$Q&B&ytmEawUt#>gSFIFy0dA02uYYEP9dsP z1iS$pGfTx;*yjYJuXP%K0|H}rq5yR2X*>)aUwW>l>pA7jrw_s)`sgiQ?3xm(uq9+G^?oD%={~CRA{jpFS9rGJ zsJC9*dgf^9T6?BizgO_EBiHAvZ7$Y#Nzf>6FVDIak9siKjW_I!WzhQc3dkS?Ydptk zJJYrr0M@5cExfEJ>U#}yqVep~GwWk)59;}Maj_^)x{MbRtE+iHNmCC4^M`JKrTGgA zHD9oftZ3N#e0Ro&_ZRG?+Hh*puCe8*EcgHRp)AWN`yScKHX{V|us848+}w49beuzj zZ#H+t)2QzxJngYQV}VX{GWHrU1=7}&*R8BInA_09vLFW3*?|_3`M1GL$QsRriOVqZ zZ@rhAU8w+yB3#;U3)sLLWv40?}!x+pBTm9nHE~Ia(&j zV_A)0I2KhAu>9~Ut!O+4QsLLrJT>KolJ6gA`;$D8&*VQ5r?g9%7mRB)35k$G5Zyu-(zC#yH9UUeP*>= z&tiR#$%3-eo5bBd3OF)p=p0bF-OmFNmYrp7bIU-*%#FOlw2xS=i`w#2K|$H4SPXKq`HTi0jXTkV~5JjjM~I#SQD<{ z=qq$~D;RSW0R&#F{LW|NO)kK~p8qUN@~aFRE>%#5ZuzNVz~&E4la0TeVP7?u;Zo1k zHaynpG2MMF;HSSC)5FE7l|DGK{#lrDLH7W!LH|b#NnObJw5NS@SM9VjlkVeo_CGM-uBp%?|}rXA=YU_Dt&ZjNF{W7S)D2g}cgSl<8k^*KfvMlbd`LQ0@g!_gsusECF^Q5i~8u!nB&o&x`r=X3E6+Oa{&gD6;=P4_7(BgP8*X> zjk>4|n;GRBN!{6)YnUJnPh%=!Y)~@_2ztkkPh%E^2F3z`E7kIl18{@VV7==A&z4Cq zg>ueM!R)qNR)i7Vi5gyL+XfcFu|{yaxqw#(CQ)sl24P?&rwrOB2Nz-zqppaW*E{ zP)j2#^8}->K9)pQyYg`6kk@Yd*@IKDLKt&I`8_c$!c`t5+mxCa-sB5DYNTPJ85O2+ zcgVGbh={a?6#=;ino8XRj+J9$xeJi`d_iQ>k!Ga}33NuF^Jfr`Y7?E}-9suUJXELn zcPRnWt++7<{cRMv1^D5=>}Pe&@j3c8ahR4$XPt=IYXBP4eG z?is3F5F(Dz5N4ZLAmJ^a@7uqXN@0a(NKC5QSrW`q7sCl{qfWq(ka0dVJmRyx9XAXn z>hXQ>5ls8?{I&khM-cOy_|0kG^M?;GW3kwx@`iA3Rl1{gi`4>C~~w6GPnipyGxW36zp0s1?yd;caVmN^=3|P$opk{^*H|WE2v| zDd)f2|8sk{DP+<{jaT<7K+)rT0(v9WreCnY&MtFw@E7YU$&w5erssSxYs*>E#^pWK zWdIICfc`7-w=2E3g&=+o#NW@s7=e^O3V^1XNX|oY#z)IHY>*qym@6nogBmmLIf*~l z2FMiF`*nv@8wRTw*Vk2#6+So~$vkSZ#o$!`ObLmlTk;tc?uej8>!*=oFaAnD=wQ%6%p^va4 zCQCss3N^~I$W~&YIF{qugnzgIk@Y>6Ke=x-0>|Cm+1?Rbbbvs%us;}5?V`tbhciIo zi&cwdbWR@YRHp1?yy9he-uj@rq%J`|Fyg2l$_~y~*qvUYK5yN%#Ti-t zjTks?2Iir$B<}T}7{SaUUQ^44_J^)6n^?bi-gq@?nSnEGboE#>At25@x#E3Ano>>dTHUE!JBAdExOm5;E%Ayt6u7{d`mw$?bL4i*1Ut#N zRdKS}4_a}h-QY?(3xUfoPr}j?=ZjP7k+=tdvZdPpjd{~0` zx>L=+nl}gqTFqLI#zrbcys;Vbhu7`sPcyMx;@E>>qWe{V0hHR~H?!3c5Z?zH%G*v& zxo*V#FUAUcLp~cV3YbZ5_Lj+tGxy@gsZty3%!=ucJJlc*m|j-tpF1KC?{^hPdBKNQ z0rvqKDaC@_MoOU~c0GYydv5yCgPc;BeGZ9$RhDV$pnO9Pwt8**$^$^QCms;{Pg*i% znk_ih_E_vw>7ytgP$tv+`_=x^!WY&pcyP3iS9UtE z{?_}GBl1?Of_D-WDNlz)r$>%xakdBJ1(m<>`iQW-Q5nCMD+x=|@(OD- zR#Q7Su^44xs#K8{BAtm-Sm09`asj%ArR*&a=DV(Qfkdc_YQS#msO z+>k`$XE|$Q2epa@H3;;Y_~rYEa-1#g!uy_MWnWh=Hu~j- zSVZc5=oN5&i>rxYt+lE`X-VmzmK8(LXH9%0?OjX+l*+DC+KV=E1U2JXD&UD$t8&82 zzgzTpVDVp9`fb=h%$G;?1r)-=sOyG*dcOznF&z0#B`!s=?)*v&{jA@=a&uc51%}1d zIq7o`;_2pGeLl|%3QYTqv!hIu;w76NGX7-|vq;CLgaJ8oAiHuRAA@if4{Csml1 znUSYPwRYNR8FXhucKj3fksY>!-(ep8{(^p%^yXt*+wK$p!~Ws}zkkcK=S%$orO&NR zam~ajy(ihdyJd`1YuEUcGxM+V5Ikv;Gs7e~{J;9g*-GmmV@dguy@s*C1HvB@oux<~c|jvo$}qrIDa?sYG9&-GdO|7=EYUSbUF~Vh>W*g)R4` zn%!D8O8pGffL>E&V4H-~qRA0OJWtcg6#8qrr$I56efy1+Eo_~&WczTjCJPeD6^ZAbKIZU_v`NbPIS0j$B zD8Kx98=ZYTKS@mJhG5L|(c~b$#p9*Y{c|^&VeV<-+S$q)^X{izMqw-e@N|d9>`$Dh z1^M$({4=9`VOWB9Y>NJejG&|FRG*~E?0aJ<{o%@Wt~KtI0sqs@tk8z8%WLBL3z#}D z@|Big|Ca-1zu!OQ>Uw{ywMK3VedZ+df8+-l#Mt_W>pA$u6my0C#FB$n{@{z)pM6d@ zCIFfB8BtF_^>6>K_fi)!--4nHveNZ{PB!qxebmAcwE7EjuwioA#?VVw-yY=_9C@T? zgtYKFkz3PCEb1-a6zkI>uEWOtY@g+VQBh_UD|_9v5a*g=r{Cg|SpX)n+|D(nORtjM zMTv%Q10i*9zB**HwNs9$H`C6dXRN98l`Ho-21}& zgy?@mm_07Vs-6DK6Dw_|=l!l`ybMlT$EDJOl|VvcbVx;^jK73&9)jaknbH)`6bF7G zv+UQSb*As_6sDUU(iqGr=SmmIIO0;Oeo67nLVQR4hmt_6QA(8^oxw92oG>9C)~40S zRMV_IaASGL0~fb{vg@%fb1A1j{m8P5T+BECaZBZN;OQS_a8CwKd1a((AOy#b`XdXP1t1ss=-x+?^r@&qoeejc*u8?@BAc{pouJ zWt$o=zDxD2k2Ei*opGZU7uo%40y~!;YsrmIfyk_vExtSlrpcR#zDW+1-)bKxJ1!0g z@TzGqcx~oSb6`3T#T01v7Hg6o{m-D&z5E8aR@o>|Uu+Hz&YNYtZ9&xk&5 zL@DeP+czL@*s+}dwtv8^ynPBdZS}|9A2{TH-q5^(@)+qqT59_e>hFX)R8GY09~+&8 z=dhdZ{v1jwoIWXQM=$R^6Yg5#UqN~q1D7j3C_&vS}YY0tT3z2FPK4jgrds$by>++slHd->h9L#^?$azed6 zilsu;5Ffr;oM4x!!a3?QVJfla;7fO6eN~t_U1m}5hzb|@I#8saWa(xPQqk`tq>EGF zr8}+>|1ip`B_XlT@Y6UK+A%co!a_005SS-E$Dk1}D*xd|mKu@9831Ri8+xMB6p~TC zEdW9b%{C=pSbL$*JcX*Lwd!^qgb$!B=}`gEfGkVB^$1&Dd=87v6b1 z3#o!{|EACGC08}%^5gP-r5g9ya}#>}m(7kbJ3OY|r5EnCrn_SP>05q(4=1kvjK#>` zo#U5>@WRn zFD|x3diiv?=9T&byAJo94)Zr>J*Zk|ZN+)5XNN*n`zgo^T#rK}UFKX1pQH)5qU)8g zynhM$)eb^gI4-((bob*HdfuNfG;KuNwO+KWRloH7HvRU@=J3qf|7f@kk!$bDUgXn$ zo^cz~sY7q$6T5<%Y0G42_kRU%#jGlcl>862mzTzrRaDG3AW1#*IwzFm^kBe|72UU% zLAN-wgesL!EFjuDwd;hbleo<WoEJaF2S6Gt zJhULT=fkLinUs6*E2=FEEnT5x9e;tC%xZ#&RFOg)d0e=J!X4?PGQE1y!7ghVZ~WA^ z!LgiS#hacKRM%$yoUCD3Qpf5le0Ha(u@j1ZK@yWdVrk2St*8U zb3FpvA`rQsIakEU7uSB&yx`3*>f*nTuRI^-65%@`T6R3&*jT!0AFN$^^+bGe(w%bR z_#?90^YlVb=do^V?zi2$^OxtEVFp7M`_AdQxOm(q&z)dwzK;tMLeoDlw@CE;hbdbZ z0zgf{-7z*!x*NWzj@={9a1AnfYz1WQJAgLf9niBrN0Ben#jE z<*c;T13TNOU$qysY5BZaxv=`k%W`tk02`NDe=)wZ&O`b#lSQ?-8u%ss!Cb2P=QA8p z3+Xl}T4?yCzdap2`-WL_jJQ!&{V%kPz$OR)p!-M*@`ZH7gVt0eSrT^ja^`BECfo{p z^2r6^y0X}wF*wXj)Fy^$`=|YlRJ!jRpUUvyv1AFeR`xVhP{zr-3PJQ|WC*WE`WYvq zW>Rq;Bz_$BmQ?hG^-to!e#n#@%s?uoNW9zNzOk0>-TYDf;KbrYNtK7_~!Txls*kjyN(-y&aRiz21l+)4~i#CRo52@(y#)Fw@ULXwU_-6+`}d zdf|YxdjRvwi$q=7ePNY|ekZ+d>O2E-FsN-;Wc|KWBwAeoi48}--bfXA-F+-T7!8d3 zlX2J-l!xct|5fP9H<5fI*ZYx0AEwV|Yzoe6N`Lj(EgtE2D3QHKkd_+KW7eI@YK#8| zz85zlbX!?XpX61m%hX?AKl2VJ_b*I!^ zs-GYl7}`mT84sKaUOXTcKlhNPiKlV_h zIItpqjNEzo3`Nk&CTLRYLU^aflu_LFTT0Gn&SWPf=wth&33eKX=|}vV_Y=@4HAc)| z`G!PcYAL^k1VF;|6V0ewB9HhCIchT*TvI`Vq^R{u5(+4VvRVjiD)Kvjoe~YH)qb-( zaffx|Awa!_PYumI_B-=lK0AYR>mL>SiG)tVFxtn+sjK!E1b+{nOr0VE_jyjYLa3dKis{wiZ~Y{rznm|G&T$3Z zD72n`WH_Y=UGi7#N};z6L`i34o#AReKRo*QYp_5~M|tB#T%0WC2&IO&WQ_jS>1GZ9 zEsRuZeSWkVovV~2B62?zCd^;^d`En2ymN8CQ{(Uw@83Qr$Gg&9F`P4?X7-t-7z`K*o_;}hI(7>+`7f%8{HA{r^CNoz%7$3RmQi$wK|#!dx3 z7%-Ihv9IEqQ)Okz{UP-CP4?jQ^QQ}!3;8Z|5|P$23Zcl??1PN)r&n}H!Pjor&58!! z0w&Hp!fXf~nZEwAV(|rLJM`N|5F!M8LaPeP8w>@d0duRw*>=_J^UM(TZg;=RbbfA{ zFc8^j_rzZ{e@zrsQ16O3oi`07y|=?Zu)0bbY4#pZ5?A$y?6ocM;G(xLLSnU%oOBpgwBvp?yw z(KL;K1;OnpnSN$0Gq4ZWr21Xs0VewV%N6wM`FZ~RWxdkW^BU28PpH2n zgiPwpa*WrHhIC1*sG`IFdu`TdtmxZxJdVsB9w~)<`uY@KP)^$^s{KQU+`W_ys;^!? z4F63qV=a2P%dArmZ1!dlS9`zE65rkU@!FP9!>$m_e5=}B7Xk7m+EgG{ zl$J)Xzdz9rIz0!%$3e#lL)V3c;3m(3|KPZA8Rru_D(`y_>ckjPd3!}BgC9>(Tv-hY zrnzR>*hHd1?O!f+AzB|) z4jt9Je+ECD4bj*)-M-0MC8Rz^V|Cw_$+-RoNcVz~&)z?G;v5mJ_S_h#Y&gPz(te(X zxjW4*oEkUnKx;akCku6r+xw>!Hyo|cYC*NW{xr7T30uGO#YrOC;E8|vI@Iwzx?@}f zU9eM3=_y*FWfN#wsbk!k*jybZcvo4lpoM=($H94z(C{wT8VbE7*N*~5c-Nz)|!X|!U zmH-k&d%`discGF7o0iSC6a}GOH_|Fkj$u-cXjEYO!LKakzC*S{3Y6vVQ68{i4W=i` z-b*3BYf$Bk1l)~m6~KNs%>L&|rcH%x_?|A2}2<-6E@yI|c}^>W(>7%Q!`g1FQV zY1Z#6BRO+tQkU?lW0vOXjQjfCzL}Agsv{i$(8BV)YO7!vbLoRV$?pire0PSb8V^HI z-D1N{;9^l;6LloE34N{LI~~3k@a6f%J)kB2vTg8**yoExgLjJ^`@b)110a%sDu8@?2l%y_l?@QA5vCr5DDqu7z9j0oje zt{L=RKuA?YpCXr|1OaQzk*0>*%&f_48}?+g4W_bQs~!E)D6vE`^N_DoDLNfS{;67K zQ0AgC+2cM=Ka7|Jv!j7tIi71ESVn6-ISTsx^ZW+t!E;Gvx-@Pc2A{+g_*q!1hu_IN z+=lM`g-RAN*KfnhULV2k_1nhKh63}u|_pN5JP*(`Xt3GP!m}Y9r zd?nWUPXb;Xy~PaDIzCioEvoAU@a^N$f?`F(fLc2i7*r7ZLTnx!b8{UdD@_rUS?4nP56uavS5P2Q_jlw5 z#fp_i9I9cv(aO?ABg0XHfC7_dbF?k4J$lbAjXAZ)2q~_Sws1vA(_b zXtO}jHLzRlq#K#y7*NlfT1t1X5F{dZlYXsw@`{0)=`V&MYHDai9%xRz?|Y|dkBP;H zj*5ny!!c(bcox}KX1LVBJiS@kMqbu#DR}0QpheX5_jm-8ypfif1oCsd@bOUJUS0z~ z?Sy2rx*xuYcV)2Xly*VD+h3&zf@4EMc49TbqG1tzkwejoAx-x5>QmQb!;S!fvp?PJLI)`~F=?%4zSyfi=X6aK=?c490i5 zDEAH?f$|3mg7H9Zfhp0POZdQ*HD)eGqe0WzByXj-DW!|c)*7UP?BP*m(PwY;Tez4^ zn}n3ZWUWSAFo&JN&b!Ojnt=D#e9<@t6$C_>bLGK!|O?o!eu=!o=xq(Q*#_i)qXZr!QVN7li|c(db7u=?zIL3JzZfI0DY z<;CgBb+0~&4z|nFT0uF|@@UoUoe*2d+|1H(oXTUWYpRvm4g)pTC64dXzK6n8)Or4RV%~M(dpKU5nD2Yw`jk$m~t?b#SaSWOWbsuk-A)lah*iA*6lQGQu1%NL}eUau5+DkPoVQ#&?rsd;w$>92&izYzDi%N(P|UR zTQpx1ictDT6=>AxDbhq15V1!lC<6C@0^gdI4yc%zaB$aWKz8I6A*cTTFff44z5_OE z3LE!IcsS!!0SeIq!3y8{X1PYzt_Usjp%r6`Mo;Epvw@ z@@j};(o0SwyCG|<^K)t>b9LP`gqAOTd&rA6K*4ImSvf{mW}w?fi+ z_q#H_d|Lo%fa5q+5vtvRQ9gk`$b~t>5-?N|*wV_6r_Aj3>Aw{G~_exS8*e;)cfc+WgYOR>t;vd^|6h_86%De7%$$A;$0grGpoR&XWI5 z9_-v9QXk@oLG2|-CA~Gtq0Ax_LSHC=)TE!kK9V2ZHqW%zSEz>f^V&I3rke@p% zU8V`JcIi-PmX_SHzKL5}TMy|0icAze+NtGC20#PD-#|AE(F6zt{?sazogzv2W|DGX zQ78i{9*F~`hv`4eC)IN&*yigq>KHAbgDYsuU^ zj3)ELag_B;3*4dR`_L}50%J@vi-ITWFq%n)=V3g575u7CCQ3Vv02^{mWVUb@Vk(H} zxjDp$Wv|xws;F_q7kye1Y=t<|47C;$FICWBBE}6;kw(8GVkP?Vik|xi3WfJTNUTes zCbco}2JT=3f5^-AkSi!mTiIo1qS9=$Ct;ZIr^^3--=gQ?Eq(6RPfLQ3g)bHu3tx?2 z2cV4@miB6vjY=fDmeTum=M0sQFXuJ}+f2+3LT?i!HS!_Ct3EaGrZ$kRD)#=v1@Ng` z5v!Rrc)z<`WF@XXn?bZcKC9=Zi6NPk0b>OlW*dTnJK8|BzOirZ2(>!DOdnnn64oe8Gj~q$apn&C!v;0}OK|fsrkD z+RRv!2YSoAqfR#~D+;9p#7yToB{4S1-tsd#cpD5z717!DBWY4^J+uxZFVzEm+8%6^ zdd;%`$Q)x}MFz#07JITl4?dAkVdkilHw{t5ytcf2w2JC4eV>h!B(NCiQKXRmrPBYROVokX?+U}E4l$5 zN-*3TfTs?yvSQnfnB*OX(qRBb6pUv7tC_j4dBW7gnkEiqR76C%`IM(7h4a}R84aYH z+Xj2i4=TI7p(TIe0m_g2#fl$;IZxTHRvbjue9b1WDUjC8sSA+Ds=EfJBIl#v0RNk* zUtM(koI3FO6u7^&c74k48D_;uM`M}*Ak=0W90NdC03zmCi1OA;5ufti0~~yucniEqO^rge@9{2tb^c5xar$dNEkj&jjm;d$1Kf476d*$b;V{w>^EM%| zxD7z2N}VW^X>$dar5^)rSnH%h zK`}Y`PgGs-9~k(<8I9uhrGrc zeJ~FuWV87RR}nRWS16~UGK*nvM-p+ZFtg7KH}OrU>8E-6=}&;@xwceYaXu$i=GRi)AED0COjNA`;eKse+lL#a z1dLmr6XhFG-dI3)k1ZBkQ=#k5^8dQH&wpH8Ao^S9Jo)_Z0yr8{y6PXoiv)r|X~sJi z^);;hU@O~Mjsx$d!NW2uystmMTAO?z9R2>CmZ_FCP9{z=E;GCxuav*z&_a5i~3!I+CNwioVkO zv@T60TEHYakzSg#Jf|=u4RBs4dIBUFf?WOxxR~S-DE;5ZEF&yB7~4=IUt*3g=;CV# zd#n?o(;KFu-kdPhCC?s)k_*C7fMvAOvSBLiBLd#?PdZG!V zgzfoUo$_DMmI$k zf{=U!BgLU9&2t*rbE$-f_v-94yftPFYwc_(3X71*g%adr@i26u0h?UFy3|`E+^G?@cq~U`0 z*x>R))Opw5kY5)?Ta{veS>+CiEntc5EGf+uT1GeCqfn?^Q_9ZUS`%@t*BP`#KO1ti zo;)xE_{-hKyZx`KvXYl5a-DNRir-|9o!2jY!rDwH|QdWT!Hu5GSNgK9R4i1xyN(74EpIgP$%HZRM)u*68?d zf-yY%^bq!Y=u(KmB#)v`{x>Jr4Y7=)myfqSh@^(%stv6&i3gC-cCPQe@Eh)5fg%cf3`FX+a=+hb zV~O2K@nVCX6lM!*m)yViCvEiAHOyL8;P6@apVW@YJnPTn9I~SDEuwUjL<+n7+7t=< zt6)OczO#KCL8L@34vu?QZz(V)p(#U%1;PglINx!jsd&djsooOp`N3$h1jl(&7hRI_ z5gHG59CA{nC$B+V-J-S_+v=i0DAKoNLCE0~lfZyIv#afM)vy~%b%kHG0CPV_5(z?P zN;=1OT-@()2+NV4t6<}4{QFn8R80v>50eXt=zwRGpucNhGBO~Or|Fl~e3Vr2pItD* zYTtdFh|;EJOHk=o>e|Gp{(5_Z0*OO@6>NJx!c4!Fc!f7`4D%J&#flk@}Aj1PPXy4}r; zV&g|^5XPd%8vwv)XlAqV-i5(Kl0sv*t<{OX_H%UD8a~dw+56Imf}+Dm`TmXi?&)y5 zNAiPrM@st{PfB}k(_)4Cvv@!PRrt{m#sM~ zxyEsK7erYzyMLw!kw;gp3l~ymmA;#)m+bdFGm_%dc7A$U{)KeY+mhm~^bSujxc;%Y zp(@j0Cb`##qM{P06B9q@j3~TsDPaL@^+t2kz%Ii-v`vf+IEGbWLC=fWpt#+ucq*Pk z37gM8dUNr+vbf>GH49HXqdUd#4BZW(5O>R1+4Kqn%3*B-_-N}5NA((crzMu=_GiDY ze$>ZDq%d<~ew}F!D2vtVg1P1(v!kmYMal1J%6*&qYEK3D&UvFLEgb_Q+PhFmN6{%r zV!jN$HAzyA{fZFj>3(2c1aDL=mo#{8SiyK=^3g{~|8`Z)gEy6^3E|K6dT9PmvE{hc zB#+>@^zTp*46Y1d(G6C|rA)ebspy`%wD&Wu=$(s;N^BN3C$5TVF!#+PNr=u=uO3;U z3v;Cq=AA0i7;&XJ33Kz#^PpuAmI!x_#Uhf*cdh!Z4fA;&S5GtH8`YTXlq{DXih%!v z_2OF)8A4A6#z|qof<_|gl>Sij-e-2l0R35AL61#_9LC*G9jTS}`W^ zkaaGVsqOF}*P*q*#{qYnQG5(5D}Bl(Xj}mv$RAZN`~UOtabuJ(d`_s`t2Eh&xR`N8 z#}0Nrx(DnVm1~jJK=?@{Axn0P$J^)oGS1{5wRmD)ujis&Qpp*ou^U+W+$Nx=I!aIn zzM8SdbB3>PZ?rQoTbjc|y$VHLmOS0dP*^ow(hS%sdwZ6yPLF$Ug`DGoAc=8UDUIHt zB!9QWL?}9z78G|$_R;+g;d$wFN*;Z@{an9z`q$;SP<$|Ob5a&CFvHQ`40Lcs0nBoY z4ak4#JbOe$>TwzwL5pp2ST}?>ipk6?&aY!@i*{uRYb9K6ZBFWzguOwiHpSCTRrCY# zDaWeRX2fEF%96L=v-Itoa}-OWIA}(q7|nnfA9Q`*!1G=8*NqA`#C?%*P(uInk|q@A z<+NQ?ot@*V3kFsKa1$)?IWergVT_gIff?il{oqed`Q>7E99KxdmLj(IE@X-+P)Axo9X(|Zi>cB?$v_+Bv1nq>Eu+G$@&=Svy;%UrP8*jz4QW`{CvAfH>v zAb5K2z{dB6#s^_+(EP(|Mxd}KK%`dE42oTA_;?0;nb)7!e17!9tlZRv7|UjK$U2-Y z^yzY*#O)*{RIw z=(&#Rv2PI~YB|}AIKghrOxwxkj|OHMd$o1R+u!e=XosTLspYjP|Hz^x4~?f9%v07H z_IqnEbaTjrqNg`C)0B~7YdaMT#X|bY(-2Ek-~B&7$E5CaU?@6hHD@13CG-mS8<7$^ zjVZ~hn^u{I3-RVu!VFKVfA5bUq)`4IXHXRbo2LnB>KJLV+$&IUzgoe5G6K&(uD&f&XN8$1eGOF6t&s(?AO zxnd62RP=MbNC|sOhW1WIBqE@x6XO}CZaLfh%H@YEdQqjfe%YMPHvQl{U|bUyAv7`w zy0s`z8IQGyk)3uVXq&K;lX9Cw7@tBZty2JT%u9v|v3+Vt7@g$i0DA>i6@`m;Qf z3r?X1L`OnIiptOpPkBLLB5z~G|9OEG#Ijw-wy>N;l38X`A+z#8TPPZnRVBQ5r@&6V z6L|jP!~JX5bI8@LwOLxUOk-rL%gS?d}&1uAbBIR1MqDmTyuS zmyy{*gJR~>)dP_!Dk%T;j-ty4%I{?axm9Vo^hgL6s}w^y;-N0Mr@{J%*CeFuE1gb{ zuDYRy(9f+=kC5#4&k4|;gJA)tz(#N-V#NWrrzS={%Ms0m| z$g?}t(S_h@#PTf$*u28+bEXU|wlE!Is%*dLYEzdBUy>IcNCz6zD0*Dq{u^B&tlxyh z$xOW!XIl!oLIBNj%F&M?F@nfLyD?5iAjSd)5w6qQlmh%0@n8mQ=|x&m|Jfd2@1hJP zIv_XQr18V=Eb@Vjj2WU*C>l_rZt38)%s5!vJOW%n&Qx#DWx<2%{Pc%y!so{|c?TjW zvc-n3bVjt-xad?3Da#*rEp`RnJ}FkR=+xmg#0JGQ1pbME4u${CXkeH&FeS*x#b9e( zhIhCvsem<`8g7+PT5@IIACsVF<7(7>T4B{H7bu*K}TTjt}j`c#n_Tnz7D$C-oA_uF4MtNIRU z%6f@s8**DM{}>+^-b|w3{O|0^5Gfp3{%GuJ0WvLrXH^j>ik3r^YgX)G)|aETbxPk8 zhaB`O?gsq&EN$y<)nX#KPP(mZp^_fNIz@~nX;^q54ON4(;t=G-1IwT0oT(D7x7y8g znv1Tv>rkwZ!!x|tG#mWsrdj0E8(uFV9QyVON@uF?UyMuNyD8Fo>Hu$75s_+GBQ|1j zo@{%UGj$|bzAN=Mp ztMqKLQ&ULo^5$z2PVN@`ig`UnS>tzm)T7EOWR~Yjrel}p*Lu@>Mg_5C zmbqp@eUZy2W_oR*#l>rSEwtpau!_aY%tnL7W6tB7i@R=4;I2ie6D##P(vbB4r;7OB zscH{sAW#n@*Ih1FF>hQQ4D@!DF5Hw)H(UBT4Y_PvNI&?@87Jk!%K`Jb){$Y!SO*g` zy4x@L4u1PY31g0FQR~zhUi)DR7wvD?nHs3K`I$vg8Hge^at2K9kcfTv|J)Xw?L7cc`v-Gofyb&_wI+}HQ zZB|l`CPTAvN?AU%KVllOYbmJ6`_nuahQ>YrfOY1;|DNgCR*l%;CB)bup?>?`&0ONn zhvW=hw@qc=Q*afp1(}Tczf*v1T3-cp$o-h z1LPAtGu@-i^4Sb1cnC!rkPn%+R$Pttw-kPkMx-rqS9|v1)?IcFeDg&;ql?e~?ZXtE zKAJsk>*rOm)UFV(d_k#`gNG*y+`+^@Q^gt?|2tRi+6WYCmORwMUfM-9njm3JQEr}* zvbRiN9*S*G{a)br!~(G9kHrZ{y@}kHg0zZ+pi7N+xL{*&Fn1P_U?07hRJgXe8rN`Z ze^X=vLS;o~oMhASk>%^hM@#J1^p6nO2PLJ-n7VY_T35Dy@k8Pt2QpvGkbh2&KD8`Z zCE&3~T=T}jdO5za5c*6>QP4QFR&7qAp5N^wTnv4l8<^Qq3hUEc#0Cu z#*WG$CQZP|n|n$)xXY22>H2WIdmn5)1yeW7olxg4RP1-B)$dS-Ko&71_0bw8-CPYP z>cg>>mH=UB6!vQ1aOJ6f2)zEz`Osz!_HHPOZ}3R;8R){R!?_HWk8iibmsAqh#pJUB z@*`S%7|p8_qR(@}7mchn`lwH8?0R^u2D~dqu|^lono?s(<7)8yK6iWIv1y!nsW+lY zfA2!^kAAT}_J61#V+k;dkcqA*^9EysnF52ub4`7Oq9})P>kF2YkvWF1gHGB!Yq(N} zBNa&OgvJbs%J(S`{6Xqc@R&Qf1>R3iPN}z>tjkk2Do z!7wm^(|R85Ocor3@ZTQDC5wl-+0|fmw<1Z<_0~a4ds}0vw;J^IePXuPi6c15cJ)ZtD%Ne&;%|@;nc(CL90(TCXP%=mAD= zDeq|+K3=Eff|xdl8_@Fl2L~AHO?I8J{J!WQ^cO-TU;B(x0HM0YIx>(G6}|L~->Jm_ zY4dgLR3EuE+nmfk9?g0maXuVzVQ$EhU9bi6*;GBhqsUmkOMEp(nq&xZB;WT%ShPh- z#G)r@=&X22;{Ak7rKK?BB=D7XM#78caiFz-ytIO&nno2v>P={O0^cz`x+amh2fiZx z6B;}vRGdgj+OiT2lYD)Y<$h$+;NIe%C2LdI+c5o{51=_ORXW{~)r|Ouh0)q^UmudO z`n_8r+Tc^B99i1kjEb}8g;+OmF&%vTXCe?yVdAf2pdYhwG<>)DZHx~9BOH&`RxZ)W z)o65mlRm$YqIMdwVXgM;s_S$t7{9>|c4Qv)!T z?(Qx@gS$&`cMTex!F34k?hH2Qoqgop=YE(UFwa`6ySlo%+6Au?ebH_c_{ZIV*#6k( z$6Z#DdThVF9$X?Sh`gyt)U29^2@7wt>OotrJt$6w_Vm_&L%x zDY&3GAv&F8&RU-gtYyOd0SGDBVO=bcyJ=6jMkb>Sy3ja$|5cun(-y55z05CNVgVq> zPMT_od?uX$NTJj#AyA2{pKa#y#V~$~{h2+fY|Y(fU@EXA{@dgTkc?)ow1on(Ttc58 zTW4E2HwQa~qw~ykT@xPu$GSi)GK`TQzELaT{RPza9bX%jqT<82QB~Ud3iE*ob!c#a zB$n$vNtW-OaIfJeVET~E%-|zH=U8wiUWaHRlg@fIZ3@j(`*|c9P1aDeY15Jijq(ol z>KsFjo-Bc}$D-W|Ew?7G8+#3vESUfY(MS``V?Hbr&I*oe5s zCv2mhlZPDz#84y?+oWD-u@x9oA|5mTo=gC8cn!%b5wwRxoB? zcM-H(`9=QF(k=+%w& zn5gw0{$wB-2G9y9$IkwOB~v<;JQ&8;wL(!pj8eyM;l?4#&;o2J1J^cTUR<Du? z-tEme>CiGlt~wh?Mt|zmR2GZWlb`_bh_-dL2C2pPvGv1W(~J$OXWnOBy)M3-?>jSk?tgL>EE#c7=`{UBW-&fROKKbwysQ#w6JfqV(ovSz(+9cg;`x#jjSs5o&6tiGH=)*2DHwbZ09K4D*bpGpV7F01Mn`wFX3}Oic zCv{YrUJ_$k;ne8LmqdBY_m`Dj-tdCxDWmZdWBn}lv|n<)^AoagKD_N?)kSTB%*3Fqnk zpe^HFOS0E0j>WYF{owDu{ae{2@k$Q@8J)Ul$Ii-#JLsEFhsIe1(pWS`^oa&SUkGz(T=g&Ry%&_UBVUx{sQIR7!vu)2B zeP8Y@4rEs*L7Jvh;faI_NUSyXvu#X&pbz<`qW0XPC99~YVsdD4Hq6=HPO|)oA$q3B zXc`Dbs;LPc;2nC;PUnKx{ZHs^m;4_>GR1bne!$1g6nw18+Z!@oG$L!2X}+b0T~nw0&&4XOq`Ye)a4uO@Ey~+N%L48~{I3D!gV*isbH4H8FNyv=e=k8BA>1m(kjr!^cnA5Esp71F3|?peQ8DCdX}Z_qkvUrPODBYZ z+oLzMCqKu5Z#7nFCu{?Wro3Zb-ZKMq;?qp^p>xMy%fy?O2i=qr-#h-`{@DdcdVpT+ zb#+6-2lkcfU*Cvx7+N2Y+6M{?_NFIxGv#rr(o$GmdTva*)Z}~oA;n#L&XOqEU?_x9 z&?A;!Z8Ug`aQ;sb5J_lfKnPSS14%J_WoAfGP!Mwoyk?zKC-zttE15RBTZ-w~AUoOH zo*+nuV*l7^$_-66txcaxvykV$oxv|NTrk6%5`%C#N>P8Qb*3Nf?LAaU`Rwn(fDJVv zQacw|k0OsR2&_Nf0=v;3?}Pwib3YjdO%n;9iWXm`;?HkYsAS}+8q_JE zkgTTAxkHKw!NOu(G)=2yOH>_no|sSfr%#P!1d;7cU@aTTY?dC)1{r7d&4>|*4f$sm zrijsH;FBVLJzn2IfRc83J+EM3nFK>_uV-h z4KB6^Vpv2rjh^L)cg3WXXeQ@cxn7agG&~#>D)COBw^D7Ip6)ZsFyqF`9d}aLW1BQY zPgWb}-Dd`=;CEc~fqtWsGXK#P@l2RQ7^1ppR4UA5O(`6*9hWyQ>E7iLbz;t&PxN1r zFX}oaMDlT0CLoHB9CJjrY3m#w3FoLNVmO|=I6*M0@wtfpEIoi%$}w9u)hw+~3U&gG z%ANl;!s-a?5*z8N2rKnqu}HWsNewAn9mt`4PhOz2sK$_0Ugq7VUy`Kbw1p}(G1x#Z%0|T3AvCU9Bi2&5tjM@1_~({mD3f3HjGCfZb&y}p z=wvk6f+U@;PzIw6KUJ%tko-btq{%_b6HC$YXkY#pmJ~Exh`siam{6wL^}bI&8igawX?eww;L~93!fy1l^0zJ!jdie?Ff;3_4H~ zotobdJxHCMz-1jHP-`Ti=Ql>($DTWTSgwIAuF{@}p1AQ&B1FnU4G=+GvOAAoNWP5k zjaQdC5R$7vhowJ4QBAYq7{@5WAShQOKcPO@kc-`5-QRx;dZXQ+`Qe&3#X7(ctC zp{V&F?c^xCa+Qj)w`@Z?^73__e@0~tBQ&72#qN#1k0$M6>NemZcsKMr5L!KbmX>C><;o7HG3|;%*>{^>yH4}c|ckPlJ1`p=?`+e`OLn^t?SB8*8&h&KuA zd$ZL08;>!!Sz6dU&J_iGV5E^+-w^zF^&|r61F{?8d@}q>x~7Tmb;HPKw~Ql+@wz$Mk_J*mdXC4t< z{CG}|C2?c@=9y!WUF51!3+A-_`)C@cYtBNgFC+pH#6CypGCJc@Ni&T&&sb;FM!#%n zo((_Ptfu+@%I9rF33(TbAWN{kawd_=Gr;CMef)Ow=usn$|2;HO*Ya;0ZL+rwt|@nD zXj|_QeJF3|4!G_yAy<$E$vI$X?PO=5QgfS2<=b$Fz?H?GU-qXLA_ka$#=iTq`MBm@ezRtFhZmoG&Vje~$86FR<{N~OU zDO(maTt6f6$RX@Y{MB>wp0_6kVep=%<1@43zs=0r zUoKGIsSseJdEF`KeKXr%uqS407RJiZ=rT5(^-H$ju75;8qL&t0HaYMZ+!p;Sy3|(= zdvEY1YNP~tVw>H+DSqmP_1Mb8+r0YxqS5yd^+NOX_A*eYA_=)9Z2NQyf$=YkNZH0S z8E3!giJ*k{y$>&*@qA1NVxQTf)dkBEzBliYwjMX4wXOyKw`Q8qq=*n?@iCy%<><6q zF{`sgmBZB-M{``_IAqXb3=5r}(!H3KpZqxfaX-EDWoy+|f_^PfkQaNU-6E6b^v*aC zx3?)e4(t6+ny~AkTAto&rC8OGK(R&VacbK%&3 ze^O&R94752KS*s!F+I%+#Vq${*q=S+L`^XiFCWiLuOw?H$%TDmXD;c@-qO)uV=??? z$OSlI)1LoYJla(z;zov`mh8j^%G05;*f&Vtm`OCx+hC=_k14%(yq$m=P3m#aV5IDD zE-uGc>?ZhLV?~aAFXVz>yO4){Q#pb_j(w4=8YNf#xbBfYF%Z9@D_(R(D@ek1R^;KK z(rsY1?(=uQ&n7n>N<^We#pcDkyB`$tVby=AsoJf%8R}Eb%d{Y&91x*k&+3rsdQP9* zl{2N;BbgG6XUhZT1*54o3PXwSI5(VVyMufi6{8LF=4#b-ntSk#$*zMud^01^NCEu< z^Vp=C6?Zx4JClhlVi|>plj?@qwhyAozR5_R}_PsX{|kKfkc2G4QZYE zRDK|ut&MSN^`v8jv2E>7XYi{Ff%n4NjtC(zepRw{ssp}|AY^G&E?vsw0@Ycy=6+8b9q81(x4$Txz|8)%K;@M%8JpS*`$l z;yJ?7!Bn}&-OJEqdXR4WeN0hXr~I1TXP$v?fR=#N&Q7N4ZWup6{Mmb7$13sWJK0qd z2{!R(uvWfcOyeiJ^eNnTe;X1~U&4wgzosaUur}{){{RGUUjx!GbP$9o9lZNQ{^>vU z?;xQ|^1)Yw;$$&^nRhVj4P;vYNFvmsI*L`f(M$8CFZ~K6m~ac8cERy-57$rFFDsh! z7K2*Ma&<>Ybp$%7UTU%m3XN}U&~o5iZHd_SXm3W?VAFde*)c7gk`=?Q`5?3lCh*4# z3z~7BeY0=-R_g3FVDpmi^voC}}`QCJ#I$qCXwwEDE9v1r^;pHC^s=jJ!#8|un*|6v>zDqhZ*GSsw6 zF4b6A-es>t&CQlb0cUmd;BmNCSuvRv)(tc|4Fm?zt0y|jdT+l%*p#}(!d-6}1iDbI z6xie)ZlA|13^Y>Vj@EVPf04Spiwhy}^zPugaq<(SKn&em7mVprmeC)x&}q(-alIvW z%AaBQ{^8-CExUJE_?aZP`a|d+OKwJfjS8ejBC)hdr32gNx5U#kht}EN$45EM!o}VS zBldV=RB-|=HR=23ZNIl}UF8`8#T-!kM6yL5)v6_A+qW8Hef^7XZO2%8QZ5(Y=cNakF zF;XPao#ocZq*T>#5TG-(z!>{?UVgiNrvX4o#Uyre<9rYCKoNL_BPLC5jGaphHyDAA zUz=i!|1s6uZ|Zc^;ijM7<1outV)A*bV%bQQM_cXOp`rDD{<|Xs^Co4=%h51X2V|3U zs24Hl2VlpA%;Cq)>E4oI;1Esz%bERViR-^=N9M?8V}GHcwPqS+;hp!_bs9?BkWB4Lo3uv$uMP-#u#InSdK&F&i% zJF=pf=XUUF0TNz!Krw&g3|vovTsq0Vb@9(rcGyZtA|#7p4E@%)yw)+-_h|2yTp0{8 z!4lJJzHy~wU7S*lJUz0~1)je1)BUGO-UVkp;M2gLi`x4*xrzCjS(>GWUyEEeFe+I| zj$QDL5&E2yIm?g>Ug2ZXy>8l5AGZ(IyyCdAS-^7z{phlNsVg^0?R?f5`KC(1N?7aa zz7ajGDweNOl46n>2U>Wo2J7-m^Uw9QFvcuWIh|-*?X@Sku4-`~nQojSit@p%DCk3W zwYvBdVrYEMlWhjoZ*^ff^{ld#md`Cy<(j4>>r5o$vb5!RmXW*T(I=x;yrC*`DYxZ< zfm3q5A&MMbaS+njTDhhYyps*Xy+p94TN^@rQ`8CfW)5G2l}G-F+(bvyZC-XNf;|&S z?uM;Eq(aDhpJxlV&INp8yIc_mt_6{8$w@0Pi4eCF3y+pER7HdiRPZX!{8de*41M=U zz0s%zi2?lAvydobkLT>(;7oBUQE(hZ8`=ZbaaJKK3n6X7rs+gTH%tbi-Qu2371LV+ zCqzen5PpV`r2ZoRJ-&dxqhr;!+Ppfx`yKMRuk*O`RTV4OUXMs}xBK>-^$+$cM^(|a zXEb){%V3lE+RmKDQTOxay%0q!{EKXS@a1C28&b4qSn%(?cezOPuPg~)Eng(h1t z2yzcwg2ZK^Y#T29W&IDuO>(EztF2OgWX(P@_Vc4LqYU7~i{mY$%Xpwh30IM2RBO;s zCAAPN+@!21O9$rWNlHg~41~(b2qMFkGl~=6t3(K?rHRhLq!;8!m&DP67~LNt>9e9V z%nY{uB&hCmeIu>d&M(*O&$VeC7J4@;bU%@)5ZLx|>3zsaC!k znLjIPWO+Ki7hS?Z7TsG>FWp8KQYN80%3tSby=1@Ai)bQ!;1P&q929w+9dUdX{%;E# zvcXiYIuzS$I$LeEZZC)swjOREm5fDzr5&+%U#wJxY}0?j_nIDQbwlIw~bJE{WN)mx+o`6 zjMWe~Bd5Za^*7GR^=8Lev=|;PNY(X0k(QKvn)MU5ZcO5m%y>5DO1m$dm- z6LKo}I-QsgB6E`b$Z41Vlw`aoh>y=771=@qBFm|MM8K3jdmCSRc{E(#y^8N5(JE4M=*nF4`_dEtNi=Ad2o~ z-r^ndcnqips*KRb&Wbma?C7~S7&R^kG?|0OMn5uIriBy)3(dgOXv+6fX85h1>t*M2 zFDrT&!%SGvrMNlFIbd7anGfFi)3XiUnSW73s+3XT$y%_Tv+L-#{w;7ip@tte1u^n)qv0iN|u+=`-7*KHr$iAsVNwkw!dG7 zoTiLFOkdk&((r1}+&tamFlJkdzC+)^ytuz}z(DUwD(t8BbTu`J@_$g2gOK&W(>)bh z61vpJI7>Mc3f>7Bi{U|P(q3RZ2W+BG@obUlN-K`N7JR5n3$h-CUY#xS=_@*vHF06i z?*g;gs1~&&E%RX3#{B#X44r-{j&x-@#~SUY9?2v>g`^-hUGNh3a93u~oEu81ZN1iC zxTeHD0Sp}YSK){XkjU;XpSLYgDOe(NHbxYm59XN41_?X*5Fr8fn zzqRv3%hHVdDJLCFz|9g0>%9)iZ~mVZqS{JcR+f+59`};-yZ3!>E&ikJYj-b3IbBQ= zl#%JlRp=j;dP^mQtSh|f6JoVAJmZcCf0Gk|P1vt%oO`G}7yG7Fc$Xkk65|6V7AU|z zeRB3Jo;K?&*}7I)*#EME@Qfco=+)*{G<3pPvp%>YE5Bp`Is_k=eOiC(%Fp_>n(qCeg$kuqB|citTW!9(fH!UQ?$VE~JlY-Ef_YC&8^OAoC0L`p4AxoWaNs46|)IEn~h-myjP4mBOla=sw=s2_LvpZvB) z3HiD_``j(&LO=hP=!?`9U8y%vli+&#s`qi3Wx6p2KYjd;vz))<882B_>eO_1fb2z>ZY&eX);ZEWBQ!rY>wbQYzU;^2CpBA)9ts zjzpz0?(8;vVRE)4uJEr*g+`xc9&cRvAO`dJ7uj#2P_8v6-fDLX*U0t~ZD^@lDtB3F zb@j=DMx*&x`GI#Vex4xR-GD}tnK4tS{Q+1&LIm};Qs-3(=e)6*D9mwjq^e{THO8cu zL>iw(v@{vk9rK5X&~KkNKaKdSP3tzFym)$Y-X#QZR;0Zs*fy(4Z&=~}9);|6N8c$; zHd58YK_oBx_ckVuIlldKAyXLB`$v%=cx}RZoWC|D2Uji)dbHxwb)?lkcYupntNxi_ zUc@JRSEs#oCPR28-uRcidVm!cwqE3Gw0E0mVSK$eJqP$9oxD)E4{5KT|xB7S+PNNE? zwQ^GelsQfckZ`k5t|GbR!0)%>wPu4@u*G3QYAK>Cy=AS#PNY^u2t9}-l}OBh7;IQA z{+%woV#mB}@(;fUzkJsTqw%``J(|du4SiBvImD{YFlwCMtWjTGUfM>*0mf=0^zzld z<)g%O2qfA9zG_i~m0s%%EaA+BYqG!m1iW7%SJUjjOM@w~AEV&^)KzN;5S5k^&bJbz zo5}`7`I`aC(1d-QnzD%qBGd@K*E`KM{a)$L54j;zv-ef58F>Z0f}USZXW(zd1T>xN zVZ_VWTrI9d5vW=qNew2wdS)dA=$wrf7F2z~sdc)ft&XA*4a~x@gSlMUyor92gin(E z+b(qHpJb<`Z_Aw_X#0(?P0nSrX{B1F%8EN(P=pg+kkI_a8A?uCVpgGM_ZON+(8?wZ zmW4k`$)B3)9S&vgG7}fS&d(o_&-NTkQ^cgBQAN!s2!$@hrC}B31%js#C?vQ=QPEsMxy|*skezait^Y-}Q-n_^_Pr&c{)8(R98!RTX&@7K6OH0#O0N>KVz}&@SK% z6%Y`+_Y2NLg4?Q|gwKPsYeFphFDIyc6tiG7N7^>*kgY+}Ob%mTJ$=9{Hjm#Zy-WY( z%oM5k?+xw3FSi_GT)9fD9ZuLdU})nu)kpM#lzu7svlV)-aL$Ef$L#Bc21I z>bBt}iV@tkWC@|)sOW+GKRKSeWx zEj{G8K#Q7&s$A+lptomE0(oPEE_BC)hdhJJgc`@5gVVaanR z0{q<+Kh3!7e5e3gzz-4$*h@rm(BYsQFp>P-PI&Dm=J)Mv*3{f@$JUG@>fGV)7~R!a zBN~BhIk%?p%sqSa!tD`9pg8=PSk`RFgmtdFdOq~_R;D34g_lM@afE3higOrk5ApqQ z#}ztjym1M;-9p`5JpHboFtSXa8@LqE_Z{Tb=eso7`+4fKaNZDQx!ngsu(iwpdeF{)!{znO1(3zMzmnif$o2fU*#8SR3I!|n*a z_M(NnJsJzfykkKv+H9kt9`ztkBd&)CcnO%;wvF_x{=uvy>n8r7>-FSysm%^iWQF@M z4fuwQ3nr?Xd69nbI^6WqqFWM{Uh3SIJWE4#X1(+MGKky1Lo_^iMgo1KZSBJdugTTE|r7M|;otJ$Ub8 ziov7?eDO>QSrFI6D88Ss#)!37Kto9g6nxT4w#5=L~J?Jyxy<#Z?X*ohc_-z zZBIRw)}-q^Ws1o!*1Dw+RM)IhNQ+`NLz8+dP+lEx{GTf@$%YpuGI!{d_8BOI4M|jx zx>$rKYua5Ogjoy)%Qi1(tr&CL(fAyO%o9vC^`sql+Oe0G0QOqu+qe8HuN%zwKf?&N z-=8DVXRxpnEE&5*5v^&H{+Ai_K6;lBGofI%VtZzNq`45^3mOx!g? z?>JzTmNXu50mV?&#b=!`*sBthh{2}5y%yt}R%aO6mMnMF>W^(~J+xhZYNM#8w{wwM zvz&?&reKCEM11$O7}O>@4$_VyocD0RYH{VukhFBvg3uQr;a90jENg=-d^$-MoswyK z$^1=|9~Kx_-XfV5)+0;w7Xs1{%ByqN#s+^F4twZ(G&g}(tybX)87m|s@*eSd%*AAB zsx?~-79%{wxl*b~V2Cn*2uz3?)wRDD7YclCd&h-HkP+J;)-Ea>^^!Uk?<5;vKY}}@i*TM=e_R3T|OQ}fm%||U}FvTnrz!zMA;^1Zl1y~(G#iQX+{w#~ z&C&o$TR2P>y96DC)igA=Z3?zjsR)}my9Qb@8cQehQI0rU%Pp$m$1)m#>%Zjjfa!#f z`3*)U-Z>B@4MDhAJadV18;l{ejA@UIA=SK zaT|lq47IJhEA?_v-c5h+UN-M~X^@+;&mU1NKhaortxbyq9{1#AV~36EZAW2GvQU z4ZsvGlBwJx5fekR$w_Wk2pI-P4s(!#0x8ffxN+8Tuss|pC2_w4!>yCUgHxgW}Jd@e`no>)rVZ}BF zCQozX4=jClglUD;B9Kw{^urH(-Zlu)MspcVJA=aJ|lEk_2 z-fSIzxJx@jR4IP_h~rej`Z2t-`Jv&*Hw=7w6!9yZNtWvwG^MM1|-O`E~*30 z#WB~58b6dH=Loouk1te`bYas~6#h6XO&!1oqgaLGeJ<#`sC;KZRvz|{NA{N~M-+rxX-A)HEfwkQth>w@@fc&?(%Wrv?-{e%6!#dJl#I53F zVMIvts(;h^8$2En>_g9-Mv>&+OvTgsN5;}x7*uzd0s17J1GaD+ln2mot|k_&Fq|_` z9kcrS`qCCX4=*F*6Jxa(a0H$_SHh7}>gMJa=H}gF)qa}~uWv+c&`Tb~x{0eZ4#b{m zEhbuovEi|M;vVd>>z>E9|tRgD9(8)*1Fll7C@qe&17di5DPgsRHr}la_O`PA{(+-tXl_MOu zJ|s%~hUXS4`B3p@PcAWIE-{4cgCQR>Qyr9ExmyWqke|2&MS{3~&u*YqpoaQY#%3Sl z(6+9Khmj<|;-tjz4N*STAkq&1$f#G!7VihKZcH|r*!?d50x@dD?y%FyY|Z3}mWF4y z4~O<~0;7a&?NUQi|ImTUQmvfZTbbYPW;Y-`5ZvbFqA04$_AuA>cn9@)=lLw?rL#9P7X(Rs4qdCH4rzjKXl+;F4si27g0j-uMjR3-bBA z$hLrVI^We0%gxa!9D6x%2%U$SeU*`|#XquUskzBi|GpfT#!3?B6s1XVbnl%0Q|L;2 z`R+>1uEp+3tto@c>1xgH{jP}?@sb!*Mc-%0cKL_mm`EEH77cgUs={i?O!bSgXk3i>qJkVq zs|vr*)_bKTEMF$y5>bSGEs;f)Li>?9Y$cv!f>j7n-D z%%+$3PIh-IDPJj1vHs$cfDUa?5ynm23{;~;E*&#!q8+rX8(0bPNv14V)GwU+QsNSH zO{Ko8%TqRoY&DQX^Bc-d<8PpEXd8g5q6jr)5sKIo!C!XESK? z)Wx+?jMS{2Cs+m})mj!DMpoVL6g{ARyE?x(O$#f#s9ayPo2?6#{_M4MN4HiMPN@)N z+lAAlN=xoNCH?%2Xcm$2_;k|!s_!Azobx!;wryZfKH_$jGfKd@@zNH1C&S7)e>q>k z^*ZU55=ZtIgvtSh4bPwm(v^XuNr#EJ7H9hhf}ea3O^%>5>23re3?SZWYcxEG@L=uJ zqUjD1Pp~c|GPIH44xF@WLxREr86(uE3SSp)cV&yLAsORfRNJUj6oOWvv{TMPF~YOx z5ER`CTGy}!FOC+1`043QEVi39>^!uAl9&5mKayMQIkO4<7qfNITkFwuS=@Pm^Yr-SJ+cMJKUIwyC<_P4ajFyo zn8PT4fm-TqL~ELY+3!=-N%2h#&W((>kp<+~j5SqQJloX_$d3nMr7%mMj1>DG+0p-~ zrn&<#VV_D<1+6?+Vp3sJuryMT{nI)T-H|~((uFQ zCVwwkHNyTRe-$gFk|RusQdvo$Rn+6AtW<33mXJpM)m*^fsIGSn-1qpI@XASmqLk;i z{@V}2D(%A2A|-E`H1H5AMRs*pN7*R~q1`)gP7F=u`8KYOaPr>Q^^SvoA+x;o`UM^! z^#0x*BTlx+h4{#u*0N6POFDtN_h$NN3z|Ia~e&XFhm8A3=Vv@z-EW60<$(`-nQbq8~;lYytc7Rr#HX?vDGd z`dH)9kb2BOBTfq^?7fcf-bKD`)twbF7d2FX^zb{{0)}k*{iUq zOcK-H#T#jpZ@I*PvQnwY*a0TQ_*!oC%X~aFjk7#dz3^P8ZZUoVG1em6rv_Tp1iEwc zC$_rb9FidP=C=o`Xq7=R4r(L@r%!8%Fg-gx9VUhJ*M1+VzgmX?solGgVxTAh>lJyh zHB^g95&6xLF;dNh5(X&5K*uL;2daw{gx@$rQ*MW8ZoEWsr$xO=b(qwbjF4->QSBy! zBj5^jy~QRVKtk$MbzKaXvX ziOQB7nlLJdJaSqzasx@0aqMv1PPCBm35ZR}VKa=Lr=FL4L3o?b%T+J8kGzHfZm`Oe zu4e*)!xOcJ`%5C+cJOW^hv4hB0J&#V!-ME&cys{KNWQ@Ev#WeFCd}gkwWsTmDVpoM zlhH$BtNk=>n92{cd+)dN_;jzw@!qizE&-Bu|2HNB{x@bz+zvbp_U1#x0PrZM=q^*h z%REnpJyp=ipTSqbr@^8#f`p*PPLFN#x5szi<(}@@5pZ<=rg#`?lVi^WgJZrq8FVlA z&|5t5cH+4@w=z%Q3Rs1A(0o+`%k*Y@!TQgynpDr&!l^XJw$oqL8C#kW_@s)ExjOFF z)O2tWuJZN8nkWe=2x{nNIhjZxAj(x9G1T3I$S-DuG^dy`$SiItPhj{MOr_KCLC|LY znU5F4_5PJZPb-Rb18N>kcIz1L zi1R|FFRPX>#o4DBM10~r5A73lSleS^;)SLLR>u%HC(+`ZZ(~7>oSn2^Q>6Za{y2<* z%J}>**qNwkW6!o5%1BJ7f2BLVWA`d&r|E7w5c9zQ6Zz-OE&vL}c2hdWE~#I&%f*+i zIDzqw=eO$y!I`XPzRi%yqM`{s|1@>M8M_;e8Yw*H{R`P33($vOQdo;muw&ViTu6!d zrJl0slC4P_NN{Wx0zb&0Qd&6<$oS_B$ZEY;bj*`;J~XvX5I%lxncC7IgZ-(hr*-s= z4m=3J3Q2vY4I{}*;ap{NPTEU9f;+vLNK$4f;h-hn1w3RB{r+=E!Z+BO_{qvN^0Oq5 zhG2bB42|dY_AJjaC8F8~v_d!Hd8W<|SQ#v235}GP+9IH2**rCZAAJebE~462L2Lm( z`e&<bfCpJL=DOUh-7$GuvzcO{5w{%l|jIB5r6v;N%-%*qqs#w>}2VnWWtqBV^L z$RP<*To>nF_8qC*Fc|w@WdY+|oHr}w`rbbAqA7fwr4&R^PM&3dO{CxfzU{rkC zJPdqteE;Zp`SkhyI`9s5UT^F1;SIa&YW~$)%L~HW3B2EWpX7t>-!3?O5mowdNvIsh6V5_ut7|a#m&kA1$O^9!prQ16C#9;0uRJ< zG$h8Ew@idB!?|qk3O02^uYVNwIduxhO^vbF%$SI}m zY2_;p3oT)u^coN3Q$s9pP4qP2@0-OviFv;ZDf~rzrF_*6p`GnZW8X(YD_u|j zPW?~8hDZst0xpE&oX_kceiz6v@T9v<@n{Kpe$n~)k95-Gy~CE5b>s`|xR!*YFv>T} z=jOLJdjSV{g*JeK78_r)P+9-EV?*N-zA%N{<;++6Ir?pK>Z7S6^9JN^ugj zag8bXe2CHe6ef*N+#8Q!uyZzq8gZ%)o4}Z}--y3=rS@@!)a74rW!#daN{|~{b=;)PMV^99 zx7E_G-a?*yZrp#+Vd_QwNoFz+GR63U=YtevgPQ0QNsRY!B|AKVT2u+j`@G8KVQCO45hsoz#K0;O#y}Y&HRHhM z#k!#giWYwCVXuE%Z&xr4NS2W;>zsY}J3e2@sO}y#r;{o?oIes8W*4qM$_{AOM zy+;Ne+W5+9I$1CNS z{C;EYy$8T+mtcoeoQ>9HOW|7u4leH=YODEHJRWy+A>D10!8PiWPCD)6QiYSgvP<8i zdngH`U& z)&b@4F+MpWub9j^hEj^ibAIozq(i%cZ($T?HAqH;$?ik>FI&WM`2wSs9!d?qcm~4$ z!S@?NY7+e~$I+B6uL@{?b_i+s7(77BWoR7OP%&3#heVs+uR_d2^9qI|qj!@*ww;7U zk9o~;$J{Kg2eC_;ra|x?sN}PZJ)WuDJ0-+UFqjIC*@N)Z(nk03fql}rD}q|2UnALf zAI>KXZF%*!ZqbBy{JuB%bFRnzni_1Cl@h|M8}W0y$8#Am@Ni_I$oK_(ip<>QU>tnL zM_AE(7q;(<-?M1Z9U0bQ2ydbJ>aa1T5&@$TmHdZ3xRGuCm+(i-fn+_sj;~}V7K%?F z0hwxeT_ovycae$O=%<>8hv07e)mr{QyeahXOtdB%HSTQbn`ccNdPlv;iEq~Wl7FI- zHav`IbUr0O+XGnzeh`}$ik6`ApH}LDv->xE2?ta!Iwq80mc2z0MCq>na;xsUG>u!; zn3;+Az4o@81E$WS;oNhU$x~d!?q1a5H`+q34hmNmt0(=tQ-vmVV23TIWJm8t8x|~0h_71*%NM1@Gb%+HX zUGOR8g)~fyjMmR+aYOGLqc>&@IRKGL4s3JqkX(=(g#Xm|wAWod(!EP(DrR-(>T>CmE zQCZ-kqzWagNxt8IoqJ5ctm_%d1XG`Dp+6$I`7}5_tuY<@xA^^({8*#eGfvvo=;CkO z6fGyswCUYQwhBhoxwfOuBAj=PKD+}VJzvYM37vZo&rNsV-J?D7bTQ}?&=dKUa#;TyYumi7Xt&QExFG_2+yhJY|a;5}3^**I{s(7C{~y+Z2`rIh1| z!;jfTskO2Hg@C(s{8Am?x2w7VsdOYS3p8F<5+(`~yEgX4o=)e)Fd+nYpC?+$Q!2=J z$GaX@oHzJ9GcBaaV1D*fs1TS+bWl<2!*6L;c-1dN&6I{ppsR_qvgK--4#M>)XL}5e z)Ht@qwa_6YNUjZK?K&8x`hK>W;4W`7F60CZWvZo2gaUfhMgv4Y*G^M;3#rsVNiJVi zfj}>_M@JnXgP%I*THA@;o0p}bu4`Wj&gnUq|ICRZM|=xYxTt<>o`oZu??@VA2)eE1 zrWRq6F`j!|q8^_3BH=p3H4s&cXlgiscSIdkz-rdj6T=NxV(emtUxPvsqR7nDicgSH zWYChYN~0?AQ;KlFrIY9X5%o{ek+;qFI6Sd4nb=M`n%K6TOl;fc#J25ZV%xTDPHa2> zp8NTI-*>HEeb9Zy0>wLh%n3Wwi zD5Z6GsI;MbCD*UqC{!+o!vE6(@Ecxp`^j2HkxQbu#S2@48fwSu@5Gr+-p+Avsxff- z{JmJ8*gzmQ;;RDN+_hu3p}lPJaI||MVx#kBA!Q0NBLDN?WA@}qScNKec`HlkoSpN2 zBeBN{MB_iIA9-F(y`HkV2e!TFKmJ-XOIkX){7AYGtAyJttlKNd0?WG8bzFvvcPY82 zMOTH@N@J?qW=U%tr&g#iHE9rqPGggrJ?fdzp+;F~mOf+Arq#6)U>D%2!4eg;qMvKZ zF_67n)N$#2S>PV_y>*lg@_ZRk62q*7)+o_EFug+6HJW4rjSrLaN|uYn_$~m7rJ{gtTdk zVDaO9uH@>f)e|I8&e#!S&(ZqHgu(8&9A9HZxcP1GwC;{JPtSpyaZ$iq`g$n;;TzB1 zdu`73+zSzq@&K$dX!X%_^gHr($A=6JWmdAg7E{xFlD0Qp%^&W#G-!&&Je>0PEnT8I zvsVc#WJb3A=nb9Tx_EQ5JxpvY1(~Q9%+->L{a0n8T1c3h#D0jS%S3M$ru&|?Qbrgx zQodO#qZ3rCcF%?C!|$8%#78LbU#3qSF;U}1V+Tk>-MEOC1&`Y%r@aoP=PCv zJbpIKHg#UI)>t?*!tUl%SrKT=KB7f=F&l7noq1*wWJil@N{kW?I3r;FIyzHmxfwgG=e> zUo>;w6(W!)J||QZXtK|Kfe>->SX3y`z4FKNm*lZ#W54ggSx7ZjoZXUs=V^p#u;r>C zwrp6HGywdj`y4M z=bB$gnvWfF*9&O1_A9ay%3~QJOWf~SOyBr^uANq3>}=?E=4gYU;>EjkZ$CD>uB(@i z(>rph)kj&YDvC!k<3w9cF8!o{&WMd(K&95|ES_;;Qw5m#PF8H)KTg0zPh&wMZcNvh z(yx_Z^SkPYiN~y%@e29eSm8;px;9_)?n6xd#C{Lcm&`w^TSyaP^pyyR`!xi$Td=C0 zyhuM7pHST1qlF_%z%lWmM5IT75ENw1CaxM5n^_wP!kz!k4pEu9zYosi$E93C) zn5zqmQD6{p1&OgdETxH3$*Oai{M72hr5*8v)uTutI5_w^_|)t626Ef;2BX8yBGvS9 zF;*R?eS=@wmGQVzCq#WQdigoK!VcaOUUp$8%2Dh5+>~${J$54((ER?Iv4w%NmIRk_3^6Gh~OE#UY!PE1OC!OP8OchtZ!eB z^pt~1J24aPEh`6`D3AE&aC$8bdruaX&;*H|CWnk0k2F7r9{$x56~t>E0gn3{9s#m@ zD;jF~pD!zZT>d@dkacU={@)aTC(0CyRd~?+KBt<6O@_>Z)t7vlf>Fpn847n;;i6ol zuoIs5!nhjO@zAsBYr=PXPdkN!g6mG}k=lfsqyAuFLpPcPmMypGoR56Cjj? z7+5l#gw#@fH{5=TX@+yOv8|-_*zsm_O+*rjdAJ%WMvVtdz9)Uf7^KUR1T7exA~;cy z0J*M9&gp+N9Qj`0XjB|R=xCMm)uZF3N-*dfHEpUMH7@J!9{~{FhQ>yoiLGVi1|+tZ`?vsZQq~3dLld2@nNRI3 z`0VuIw(w!Pbxh<0P=>F;s|GSOrulYYJ)B^8xTFI|0pG zJ0E%gAu@sHA0R#+@3HWa-}^$Y?q0e2&++_a0eTk8q#=(8*XLYfFKMo+AM21X(ji5~ zM(bO6@K%aYF|(X#zs`f|or6}78f4d7b8|5^q=22`ZSyjU+!z=Bewa4jLF+QGgq6zG z+Y(wD+TJD{v4mXZ==CTyq-Z3%UK*Eat(2zCWc$i3p;&-7@`SA3Oht6u*Q<&nUgZ`a zx_>=u(tJNZ3bQU!PK8P3t_ipv2Drb}O+U)n6hvzK*iC~;cith>ecg-XL%A?${1~Vt zYkSz-;KR0xSKE!+UAt^lL~5OPp|*Is8yKFx>olahxm{JrUQ_7uz&WvOK3OTSf)!1~ ztnwtSx4k_Sqt}L`pZq(Kv#v?5*)?+?v0b_~jvBM7`pj$n!$zdNGeu46H#iXgw7XAm zzT|elru;|lv;mBT|5VdFTQ87t^iSH0p>xbxF*;_=RyL=j^%+B_LkMo0=He<=2QQ3) z95MJZ*i&!-`QTALW)WZ1K(aRS|;g z>r?1^9H5huD^WOf%~a~i%sggbc5jibVhF3F)$AmyoL!h!MpShLiLH4?zKL^Fi-Xe8 zPAHWpW0k6wLf}_c3BvZ*1aesa!3wYi|JzfbczaW%mwQmJCHu$4tBfpXe3CWlt^}HU zlz-KfV=42|!ps^9&fG8+zcfEee|V**?4z6Us3+NyVF)%#a&?>Gq6*C^ieZAS?OP&P zj7@%NApQjf(S4;jmRr2T@oX5e#`w506y(whmus}=uRyH`gQHeQDNbC(;Yt0U`ivPq z)$y%FXAeYEnJ=LQVma;e83!xhEZIrx5x4F`USVA&ua2(6-(GxOi7s4UKJVb*@!f+r ziv))WzJaXsy`b)If3o_%-f?8VCzU+QeL=-of8FA|pYXkjz2`yt3pC-mWxkbx8iFtO ze-V9cZsY8Ky#!p{`My{CN`tzA?QeXI@Io4YyK(Jt;mNYI+$B(xE#-5h#8z_X!C zq(`7ah|yt=;8LB88QE3|(j7*1c4tKJLz02g-X6~8+>rlPHLx=R;-S9&U|bm2YHCJn!}{*<52`s-`P9_p)P3F*oCKzuX?8 zp4?xK*D=8btiy#8o|t}7-2)dV+?P2&@c<;b-1h2Y9QPL4*l*t7NCOv|3HDNWU1Ax! z*{b>KaylF5JL1j{{le&TXsO!WUmg9IW4Z^|hqD2YM&_YeF;=G2BZugu+UuQR2kTUS zww^EvxIVmfJpPhm0C;;37T3gUwX0Tbgo+6~!-ePwjy5rTEn=SB0>KOy-ap-!)#Sc7 z5l$H1sJ2UF#xBg7UAfMHJS526ZVRLO@YHVqcl+CeGgUMaCf2?94t$1Ni;eT+CCyPF zZ&+u7XA?FD6ahSei{d2CI2i^2DtA_*8l49yJ+o3pIy86~&thtxP_nYvJRFQ}xO17^ zTN5QI!jNS$SB5|0Y6pkcfbhtVq5wUEA|!KeOTd(cl)qxIjn$hhXWU+Ime7fFIT{%V zGDH#d&=DbsvTyyZ9wkfS5E*+TIOn3H5QM;KJiPkPOAuw)rZH$U_Zf+Y)^C5F=^b|& z4IilTt{5=gkf@P@lab&t1!Qm}{lqx#p}jP_6`*m7G^pPes6Zmde<)x(a_P|3uSCkP zAyIR=eQf+iAL^mo>J$bjp5qeaF3PNMu=3|unzb7?SlAAi(KHZGO|2WFXDH|-C>0Tx zFt}{SF`5r`k-d@;xC+NQPRcqT%UUhwthY-AAZrT8x6`uTWRatoT{Iq)jm9u`G9w^p+uahtkaq>|Z2j4-|lP6Eelyw23&!^;3x`fwN?)91j^-luJ=bpzZGZxl#~YSry;v3Jb*dz zF_NBHe(XuABm=1g)S(__{+txk5Mt$ImMSWFbBBGF=Lds##k@xbXVkPzA{yg*+@x=4 zlC~%%bs)LqQ6o=#SiZ*Xi6|^3;Vh&df02Q4CdKJuY>|p^B{C3I!OC8fXmv0Z9{m*H zmksRTTxDpQHQP|iI{b%>m#bUKpKgYie{bb@zKO~1%==rt6U(f}oZ8?bhaZu|fjc*Q zs=iR%Z_hFZ%?E;+(H!i|jY$expWm!uXc?;=AT)v>T&TxTe6ec3fQ{( zY4CRiZry~*T{~1KGLb*lSloKW_z);`Lf`vO=v2OTp<-Hzxm2Mv9R=;oZjX8 z(2keH;ZzW;N2zz!%u+K4l{?Hocv+rSYBkdSede4*6Qw^mBGuVEi`Q^r(g61tT2qRV zO!g~06xm6#Tx6^Qo{SfopfFI8@I=_(qO2zxy7xD;n_r4;JD$@e3RQcxmmH%!tlA}c zBG{Q79sy0|N{EC9P6@4K)+2%OycN&1A{Et|yOotqTjknT3S;Fe<=n+`@Jz}AwcqB~ ziVGo>Bt_*7ckka-Citjml6S_W(@G5Th+4ZU-o-|xNV`dVu7B#4q0#7))td(!ry?$* z+e?EWLJ90e3%odUBa^Grq58%|g)DroF$S zOSa(;(EK*LmQd}Q+4bVUHEd0>O5?5xaiwl&RTx8 zG@Y%4Z=AZcFWNCwAP!#?VbTG&xrddzxZsvAx^=8-v3!W;Yc%WjcVx6?8;Q;P!O9wY6Z}QF zOmZ__yfFig9z-Qy6p<+*Alqv5SECfh2#Y9MzOhJ9<%`tj<50KN7NeLWO0RD$N+%vn zs=vEriZx?{IST!TT2$848VeT*=7-T_V)S;LCCrp7&zmG2t%ne{#DNx2gnv~-!ys}?FW{^`(vlI&;`mkP3pP0dPwVK?c;?BovGCJmV}Om zgz1l=DXva5%>*3@>^J(T!a~A7rkMJ3Bt;j{=FJ+<*~5!=7fbe1cGkMuCYR*xm42>k!kGdLM1~r(QLVn7qcw&qa z&bYoOQckq~9>}wuA{IQQ=EA1<}7ZqyU-hJdd*~-VV1`B5-AujaYgs2q+iH-akZ!=dc)OQ|o$L*x|J*7?cN=yw7 zAh_jm&Z|9vm!R2NzX3#9SxGeAzlpp2>~K8trj6U7H4Y+dH9Yd}aKB}j<9(pqBR%;T zkr5%GB*l!)ZZJ^vg^EKV!?UbW;i8mfLJb<#!MWwhS(fV<^Y(nydSC$}G912_G*1^@ zIi%5}4zA#7ziBI##75qUi&cFFTBhuWv5mlJ&q+u|)@6ud%aO?5pM;XM86X;)f$?-F;NikpUUGY$ zjA-O68qjMB{)wRbenXt3Mdz(p^$Eh=n66}rtX{9ztodrIoUv<#f(22i|0vr^Fr^ZO zirQ1MdnZGYiVC;YFl5eJgI1JzGZC*F6fW^ZqD*bU>}G3WL$+q6P)%z@L8d?poOjkt z9BL~)BKv0H&jhLx6oC@9LrVyeR_o4f715scaqD2n@&Q347=02Q1o-bk!r24;ItA3$ zv1^13ZgV?tpxY?mT?0;|XM#)RewA3}9^(S1ZqiUF#*L)b5`?!Jv1zgD=Uj}zUOA?B ziF4~#n|ZdD(I!_cZxcgj#`$Urh&&Cf4Q()Be}6k3O))VjANB`}c^i3Ubq+pVan&)f2PJ4 zycqTB#LkM7=6Ph1e*okmSMf#Iw770HAVv+nW45vJjLj345^;Y-M5^i={1M!N3x<>g z&(6hl3-lA)k)q0LM1m!Fy^{IP}*Mq2{n0>hgb+dH|E8Vrwpc>#L zW$GDY5`*0dUE zNMnA>s3ny@6H7`?nvW>dlX@I#KcF%u*P|=cXcQH=WSPgcJ= zC68#28{H}1Z{G<6J6;7>;BsDCBeb$JNTT;OJCYEGB?FP3eR@V#C<+Wwa4s=jf!Vdh z{=e+H<uLFpqTZ&foRp4@G{mJG}qnMm!qr1KF zHf+6a+K_OVWt3o!t9u|$;bO=V*9Si5G#`)5$#7DU%Onr0cIEVTXG*`T&NJ`^ZrpAB z$I;vcjm`Rje3~I(D^IBzQR15<{hh?0?s)*IXipK#fJm)kY{Ll(wdo1EO?%KuR$ti@ z?%FU@%!L7w;!A|llt{p+rGv?oNS5fODJYP=1Gd+!!4$JrIHkxXZ?YI#kt)gyFKzmb zBdLpf4R;tRGJ`TbW;jRbX4-1iMAzVr4!>n{pxUQ1E})6P6)GH{tk{;E42}8vtM+ak zFck0&hO0Vd12+b&hNAx+!4{lV_+d?eu#I*Vsd&|LilR{x;R%pN2DZGrb{plmZ-We7 zH4{}%f+eOCIv9NL9!#SYD(H9w_RY^R*pG^6=%uqv`tKzTV z$+@SR?e32^pCDWdJZT9LzBA(_6tYUis!(&6LPG79gIyE~((u3L)?!s+nePK9q$3xo z@o|tB9mP$m6JT-&F_&ph8=unR`3T&Qr!oErAB|No1XC=XV-oCi-XZJ>Yl<0F91n%t z7?HT9aKQk_*{F$*tE82nUD{JU#@ry|3-?4lj^^oTmuq7>w5#O|2cK_&h~z2 z`wl6Fulsd6r2B;R&k6s0v5!v_ZP;}aWqZ5jd8K_J)Z&0B_d`N2BlvS8on9T zWk90}tQm~Rjs&vxS$v96|6h}otbH(XqUYl|#`R!oW{n>y*xKn?slAfF6a9-+0+_C{ zOx+9sNvAz!E(a^PMh{bn1U@URWdHDZbj1n=m}XX9E)+g(!QRd3r>-yWEgt9a?f1!T zLTLYA(_8{`|8q~jy&Mylr{DJ1$2Qc?=i8Ir`*Rre)YZS{&okz~iU+8olH{f5^PN2F zTKc2X;!!y^NhR4d_{}j&R2G_L+AIerh!bV{#vc`5SfG=CQell?goQ!Cvq;rzteG4okgM$>{6&^F*w1(^&SuX zqF5=(6dW-gTq>Z+ric}g!}WL`46e{;>JnX`K|su`rDA6f{nm>hXbHdd4u{kQwuL*= zx|jR}O+wvdGT+9(%ujm$p#jr`AlL~n^#dM!DJpHZ$QjVcAWnH*;bB2yBhs`D4{H9D zLTM#%=!6Qn%>P+3L_qSS^k*YHu*LA-OQh za&d?-QlN@r)8I;VoEO+E6uCrR!GJgE`~g_Q+h&?k<;k(Bh*P@V?yv)RJERRcL}9N3 zd+xxqrpS^T_4N|G`EYr{vkv6=J}3dw|M1qIq+DHFR{C{n)h`oiG3*}qtvKuQ!^!`7 z<8T72>+`|4&-UxlwmRI-*}w4rj#YNis@3g7wQrVWQD4<&`wvTYTSj02NTwt3|DP7X z3;|@Wl@RF2pz^SIDg6=6f`VmI$t^2aqqs>5N$lzYEv^A(&enYe&>=SpLkb9dAnKe! z0q#%XCtG3VWT~NxT;X^nY19bcUK?e}!?kSiq>NA$iFObeWnG!f{V2VpW+mF;=o05@ zf}PpDk-zxM2}PtmW9EDyvWZ2vO!e+zy0HfX#*l^2mxr=&@fE(oP{fj7V4N2}`Pyc3 zi_XT~>Dzz+*H1U}1CCfJY6$UfcxwkLzcsMP1u<9qMj_hlM3t- z^X5l_j0an%#^v+I5(rG0m^FtRLpw^`m@w*Yu9LL}Nd9wMWr1#L#})`txuNU=7ULp{DI{u@ z2bcgApQVeeT5`7@8*xm_^jD822?Dv0xa*{Sg%$Agp*SNZfXWBxB4MSU>z83Ry+rDW zR^C9$dw?GULPbt%Hy!-8EksX*%8y@?+5(S*K6^PhGM_ZY{@2>|=L;Ec%Z!mro^^qi z;h+N`4+TKeWN`X(aJ%e#wFg1{cs#%8LOIbAW@%huej%ii28o6XjM;CBaNr^~Yyyxi z*{50L#B};BPFx9F2L`44>9t>n- z=0501R-kfJ7|Wmt28_;`$7J0Wh}V1b5MoeTXUY0W@P$a{-bXEKE*bS1G2H$-xE$*_ zTPz~2p&gs;MZOpbFVx@pA)*pWvma+BLmB(rRk$fL$GTXA`>Lg8#iWctKQwdxJM!@O zHwqaW#~`A^|L$;T$^olMBd({mw>Z@A+4KkuTKQ7tXB^*s+X{~@nE3zN0-(suGrhD_ zqU*8o>%5GGbQ2<}fA@D%K50dC{hArisOCvD&26hcdz-qj1m1#rXfKUxPmerHPlCTq zAq9jERl2%8W4Wf8C1F)0j&_{s+86c^(y)qNrnVUqt6Dw|rEu9wV;f98+)tB3vQ4$w zXk=vLP=mofhm;~0X>Sg7rN{~P?L=@L!J>ns{6c=hpq)MOj5#lUws4SCtaEEdlOj7a zczG}eXc|BKhAbBB*b~nQaQ?VzQtz&o>@Mf;QIViK5`K!1a${P7sHGm}w+P=z0xOBa z&}hBE;}qN!i39Ui!)^+D#!5Wtj{OTZyQKGNRV zM&e)J#bZS(xDFE+O?=h;1>*U1>C8e3EIR)srsxM~|KHuWs-KL*KK7BeUUwO;9*)4n zLT*ZufAFx7umP=FPSA=U!IR7}-7KJ6SLJF}dBnoYFVkJ5Mg#lTd zna;9-T#B|v%l&nwpQ!QBzyPlTD(Ocm8$sPvKpOK74y+af|2Q~}tU#5-nwkyc!pfI* zRU{G+WRf&<(sQhO(CprD^OWKC28BcmkgONw*a?>x4*Gx!Bt$0dMJupcqY?yGxamqV zNt3s@q|4EK3Jx9ye^eFzNR+IISdoxY`g`1j)D3S+GBX6fBmGF?S$bsz7;p1IRck9$ ztJ4PN6G&{snbMYHoU=mZmm?4#T5i~kA5v6`CM?wR2zP^$MXQB{#m6)p_}-JaIN!VvQSJ<0!(hV|g)?4h&iMHx{1zrY}s^-5U zZNp~?lb|;^a_$Zg=KR+697Jb#a6@v&djj~fV}eQWaZVN0Dz2X?)Cn(D1s%&1BfI}6eauCn9LxJvHRdN)!3-L@C>da+ z(k!ochOnj)6wH&rBAaE21PGQ*Ja{1CQV?WQ6qcMnC3hkQMze4M+6^UWm;4_rce1v% zqjSN&DSbW+vZLWPXtF=SoeH~ag*HQEZz&P+C!#nNLUXCSPe!}ZJ#9j&;?Cq*Vio9Q zDI{n3s4FchAR`NCl#C)O;mSSI+M)Nv_F%e5WU!PFJ! z!}TT%wK}xla16y^L5*t>jUN%~JQ9nqIbeyddE($Iz>ITTjDk*Qp5)i`3P*#M70bMm zyAP&s?|Fbzn)g#y%(D;m1u+&Y#kjhv_+iaj6^0nBJwYCu#m?`qeRIOdzS4FJ4LT06dzGuRJcJzyHX zq>`$*cD;{s#KF}(kzFc~$ZGCXLM|ILVpE}hIRMPnRB^jX`cVqI4E;kQ`zGi1J`zmY zPimRR8UoXp9r^@Q4|^KURIF7xms^8(&t z`-Zm3$6)PR?bK=b)94n#$e)1+P zr1j?Ht_oDvvHr@>=MvRxzvo=C)y%2LsAWy}iUAOc;?gj0FZp-I)OliwW?oo&kx`&k ziC&4(rK&>o^zAdbs}=I=;FtQtLnmSJmdg*$|B+XDe90^*L|KNL3PvQ}l(MnF3wZy} ztUw=dw#HMm>hO$m{f$01tq+a=##vF11Tm3e9E`)z=)m82u5`!pr+;uG^gT(fy|r zxT4uw$hqmy-!6p|K_Ei|FJ436mY<;wDx#NQ>AhWgmyXX; z1*MVDd-AT2OgN`i5~2_zEPdSGQ;%5jx%z7abIjX8Zv8=*2r*#~X>DxH+{ zn=8!81%T@a6fT(KjnNb@R)HB|FXOy66`Q4o&uicq6x~g0LQCvOaZ`pON3pVw{elx3 zZ62qz#-xA(_EMF|GpAIvbd$jB3s)ESYVyxWExc7K1P+}Ps;;;$6(K19zr_3BcOckN zkYX{s!$6L;76>(#3y&H&ojtUX?5>@O*(hs_8fbk{pc$H6#!~DDp(!8Rdz^DGGFoLw zROG55m^-6L`V}rV)|UGL7>1Tmnaw|XBE`)U=fo-1EQT?`?DHiW$bpxBE6f{69%Yta z#_~34d{wQC#s}4^J+=F25_>%zb^B) zm4Bq=n{<+{sMk*6(!=H)h=#=*{S<+mLT+fn(s6-&0ZX8Uf+auCEU1`7jCK~&G@oN=cD6XNW6psQJ8WisOqtVA)SnY? zTxd*g_`ne)NQ*!54yJjc)L3w?0cLdIxh{W+b1!&z!7j?{AR%edLy#T6?pI~5sjnZ{ zo8>pu2o^n9k~BsP2CV8RqFD;~A}3#nEE1eGaLTmLD!?yU=V+KEPEQ(?@C31At2V)m zng+zGZ4bOwd6XcKhhT{}`eJH|MQNd?s}ghKB#gW;h)R}fT)aK&q23pj;u+aVyVg;lc#r{3MOF=TqsIFV^vx>JeUlL zjHrpZMkSiUA4&|zfv`MuoSd#o0ICB>5Jhf5tf85aeBc2wdHbcLh9VO9ZeMCO(4241 zEg$V#5kc@0;;{d-B=EhD%z%nAO`>zX=IVzyyf>FEVuPChlyRo!G@~`R=A0vuS(oJf1EisM^LDWE%uqe<*;vWZn~uDCtv-fJ)rnwt+-T%7q4lpyH=oJQsq|(bG5@ zfk?Nos+szIqd6G_Qq)nfM4t%9LAB2Sw4D|_R_U+OU&EBBQbz<#`m;0H3vmsFKr@{i zEsskYKl=+G7?DRvv`Pq%AQ4gizkmdqF(vk-@BqOcKH*h@77|@h_K|0Kct=C`bO}@D zJ=*50Co9V-)3WB~t^XiqNdeQJRo&&Gl=yU3ztkjK zXQZH-;%&H5FH9sHDIV6!?3nq^Tq`IU>NQF3=moFVB`1b&YrWsM*vDmX-n_=@E1k;e zj?-Y6<}6cft^o@+EVAAL>Er__9uffE5Re@~A*1>lUkhVz z0MkGW%gh3?iT7iOb8kR#Dg+)eD^Grdelrdk{l}apFyQn(>X?mGRoTR1@b#ZAOXyyB z*E}W19@M`5i2q;U_5BjhP0F##GC5?HQo&?<*avnJid3EuM&iO>$8Fzh<^aAkd zSS}nKWKcoqV}1B|Po5%(utO>MG|4x(C&E3VTyg(^*6eC!aBDe!`fMq}0|q5!X{&At$BnZruz*mbGI5sC2m)drBoMLIeoL; z%YY?^N{9B7mX$`gDY8Dm6yyr)!9J9$w{VUjg`A9uC~Y@0B`GXOv8u6xk_;r%_ww{} zk{v3m2zNK;tBT==#}KDedaM&a!|n7UXOCKl%zT{2 zi*q0U{wd6wVE(&HrTu}BOrt)n!xLD4X^IS>)|;*@l3eOhJas9HDA1*>)@viYJ#`F>j8IDsgi;8hRRJl2 zk~)84J8)q}U81M!Vx*Ou;KpCJ1d$PhaaXYGA;bVbbpFbad^cs6w*>Jh%N&CO2ei)> zapZDN|8QS3HsS(1S=Jy_N54*M#?S}%_^zpvNTO1q)nCeFD&ORXcO%F!gAyZkgQ>Qe ztO3qRY`4^X`$%e_BuTZ>;Lh|-KNq?&f2$_4q)w*1snB38Q;|g}#lk>Cv9XJQhi7Q) zQqYr3bu0)M=q3QWb9V7Zg!pCmeh za*g2br! zZC*hPdXh^+`lZBA>m2e%-u-oelm3l!*y=-% zJac2wRcTZ?v@9~YA7n5^hXVpl?tOB9!!2*{^M~f^aOgI0^DJNZi7i`dtNO43C&Hrj zFVSsI$qturzOJ)Mf}AeiTSmseS_raH8V&w}r4i{5Z;mHOb;1V8(G8F94F-#DFsA07 zG%{#5TODC%thRz6gA}B!$@L^Gna-H|HA}vYUsjd+G`fxI6sYNt8$m141Y$q8$RR&O>;Cd?{`n#noCK3*V?mvo%kKt9CW(q=WD3pHpuxm z>!tvkxGFNn*_2JxX4k9ia#7Ky`K@k7oNBuZMoYol?akTosO~pRK6gt#-s4s`%cNcVmciir;qw2a#u*srS+o!l$2F9tr>`3;MQj695& zy^`+!vYgLftcjY&ddgMO+qZv@9c;E-^DKw>b>0A++m>pE8#Vv67hkGAvw9Oh}mAZ5)Q4nu3PSaZ&2U6da0|=k9wg9 zz;hQAhDc#JC+JpEf&cBW&q-o*UtvHfAb^SYN$^!n2c{0T^zKJm*2%J$VvYs-@XC)# z{_Y0}v0O4d5C?k3MS}jL?d%P#-isdoFX-Xg)KAXdIM86KK}V%-NI8*vYPe_m$ecCz4UK1tf_s&p4L2@|?-!gx;aGX(|=0 zJ7y3|PJ*aLJoq1mK@Q(>SYMrtcrkL(l!pvX5x^+=zEzAO$CWu-dQd%zkLP)qAC0{0 z5D^$uSY84$u3D_m7Sn*#pfye;ECDu@D9M}`)MC)d@~(9Bcn4Zj*xU8jZrILnWPEEvv9>p=QlF|De@mo5*DVia1> zHinZr0!t72sLk)Q)6;CiS9CBX?UeezeU1GUf)45ecTl!hJ3w+7xg2Z;TdYTlBz=1Y z>dRzUYd${Otx!<9uU9N*XYzvJpzU$2dQ_t8?EwQ5Dm)207}5WPl?BS_NEZK*8@VZ@8^-*@|aY|I7gh}12}ECJzj3k zOCJ~|P=hH9`08|~hd@G=oDaGc0sSwrME|vnh8xB{dy!i{wKn={S%q_EB{42-g%2g| zi$EWVUP77I?s7VK0q+di(MoP`+@xnAl%cUASy2i=P;pU9c15FD6tZd^lQ)hO*9 zResKnl@(Z;vh!BeC}u8?I?YuK$zrqV;|T5hO)nq0gd99v{}kV^1`XC7$M1f!EKP2q zL8hXFmB#Mote5I~skVt5v5h>b3v5xAHIf;1IaX=T!s)Qr>6f0oGRqpMeY}gj!hUsg z1ocO#ARK@OIl|6+#S5KOY?NQ&sLP!PE*Kl&D$wc`T8uzoTjBxdrrZVhYM~a^o4;@& zCqQ4?S)&7OBAuz5>^uz`bKGZ+kmQ`~*GeLnA9yF*`Vb@xqOU%1xd zdpo&5ABNpOqXZ0o89YRDwSIZK*0yFo{$Ov+p!0fBN6f*`xA^wU;O}GmbKFyR)~CVx zh0}|1#d;0V#rM`AoT@XUXFi{xBj6H}X1wym)b0=Ms6 zo?vzbxX(jJ?gvRInRpMr^amcSx5;n3J>FcNA28uD*DGJI)H<9l$&#A{X}4_|El0gi z_Q0Jt;+W4vU;Vzm#!$Xb1YI#Ky|_&{--+2ZuyfF#_ z{$H%ZTSm&Uf`b5LvyJTRknqg9o{Q+@XDN7Vq@y!*;|kB5vu(77Aalvc9@l8D ze9g}9wd)Jk-AcYU$)aFMql6dB3a8o35go~Ii(6kuEhY}IHyt<@54ztXVVZZl@P3sEl>H?-uuHWh~cxHr8LtinxB$J z=hYdFB=W(Z5hYiWQn{SPu%6U%2x4?p*$tndY;N<%4~ujdnG_hQjHJp3O-oFvL^y!; z;_lpXuAoe&TKoWCS<+Q!X(HX~n)v$@#RBQjv$7Et^To8o`YNn>!kW*{qwtmlsJbEPs_-GQ3x^jqL&CKEC-T^2Q93S&d`@3wH=bd1yzUh~Pv<#Q zKIXtGGH_w+99N3*_FkqxT|ZB+9c(Y*_y&JAZ%DFEu6jfRCyYHkPRbXpH6@3-#E$ez_W^!=+rR-z9<(s1YfX zx0QFM_No6=NwYK1^Xsx-8fQIzt);P2V&Tv$usv!YBqiBqV(Q>pwRSF1DyYG=%9l((R0C#U4K@4bKs4TN8yi=U%*-&q zI4d4tMpxML<&`WkYjie({Vbx(F(HIUH>Qj1tY@;vR)OOi3tKB4G_VLZ(hvif)TPIw zNFp*g;3R&th&*>@`~+UzjK+DDe4l-L&oY?KEMu@4I)*lR9WgJBS|Hr+Z)@Hhel+DB z-={-#*v#rw*7fu^rM7&@bG0r$io-Q=3v3JU`XH3!_70zF{-ojiy!6Ql?XbZc%=;R^ z$aV`>;@S?FCb59+q*iaOOG?Y6AVo0sy~gxB!*RR5;v3OyL#+vtp~il{@m-(XuihFQ zb~Waqma15-OZw@JUA4}Cl(zgl&qL=(wKXZ|*Rcd=3{L^863QZ{) zPHC{w%2p*tTkL5BACdc!(==iISazC!r9SSg*@_&Lwc3yEYhQK6(=M<*`H6X*D{)zO zdg)BE$N3&3`gTpXW9+l{O}*LTl!SMlb@FDD+g-l)p`T!1%*7@5+q`|?9?7r?d&vL5 zgcJWJ*H~vJ5M^cR7i&xmY$ap>$?Io!*z61; z48Xq3?d~-+kL}2w3@!4==%-;0x7Jc6-H1A-slXa`Ix&?-!dDQ(Guvy_{j#JFX2v?1!LE4l zM$_eHA1T`+HVwAiaE+)|&lr`c)Yy|N;vurHTrgU1=CD7XV0XRJ=&xlmv{K*yo+w53 zM^2&XFumdNWN4y{)l<5-NDF7`RioKW0Lx5-X=6k&ukHn zU)VR7OGaj!C}9ii*r}4^fjFV3oPCV^4wz|vC#F~~wu>bbu?bL- zH9>_PGW05C0vwK_mK&eOo^M*i2t=ELJbm4%;E2^Jg1!U!?Pz~wy+TvZ zrjaLv>#4~j;fB)2aLEmU>|r|AJzM~J-?3wC=31fNB}P{4h9;M3ZSrHlJ!~}-Q<%vk zW!}SSj1SFpjVsk0Dr)GM<8wAFcI#oa11t7_6)=PP!7ZSwf@8ZQ%I5tbAENs&)t5b0 zm~%1c97~P5#frsasBY~}EDa$2UOwsE3F@SqrFr1e ze}3;`*8K&RIIdqWY>rY{0M+W-1%NXTb0}*XPsjWll!lc6QJVNp*8r(92253bEba%k7gA zzHjl*^}FG|`)i>5_!IXea?Cv-6yS|Mdt$ebovOd7l%g8M3$(3GzS$kuX#it}zF*+K z*f*Ffw&X=CI$y;I87YCW>@|dn9MV37jT#m8K6i5{!i7~IF_WX?1yDx95t#EtQ`U`3 zovA}cFLtXcpYNjtaKNn`yBr~*7;xp6XlrhEkz!s4Xs&#|A%@<5%PI93;=(fPl1trz zsoRcmr2Nw6SKe>d;Y5%>uXEELLQK)fN}uHh7YxQ5o`kl|eM~~l-H%WiX&qM_c zZdY0jgpo>?j`FIswv+1A7%85`yZicdY7ME)v0vJH!+2U65)G5bI_ZyTHEvZapq=v<#6Da{CVhDhj->t}57NFO=%~%bLpV7_<7rGji1?xIcJ| zl?q9Eu3t%r1$jnixXhm+bv?ad`y|i9hA%x+Clw<|3NB$uwVUe!S!z!oY~G?=oBo{s z?55YOSi;IXyii~%(urDnBx*GhB!w^=Jo#rLZ*t8EN>eq27?ivS7})_WR=<{r!b#9} z!AZ0wG4D54<3$0=@IJa>C2jKKUX|4m2C>`lVtf9QT_h51w2Gr?1SmDXH^nI>yL7E? zKYqV~*&}~vWXMo*!{6(RPPp5TOYeBF(W!I333|~N)%9g_I-cV4-C_VebnJ9|T>D*# z;XXtN_Rs^p^(S{>090iiA|A7{mwA3az%lontlPhBKpv`ko(V^~h5mz#4L7#|XC%Jo zkC!yehLsHNxL^Kg8+*T4oo;jmEs&h|&TSrq?y&0M@|atH9Y4GsACm*Uv2LuirkC}c z!+CFbB4m$p&>8!OQhezPic|W>{X=MCHBixVi;4h)0z)6)&SB}bxEh>SYn*;>XO%2Z z<_gSJ4$&e8)kkEiD8Qhmj6pwFSjl*l2*0D-P_RSr1_7E$5??qDp0f+M7?$`V;|Zs@30lS@cfn=0jh72YqA2?m*$`4NC{R9x##>8o4&)1` zbD82VUV-!AQzVQb3 zfBoB_`_Or|3I^oPu+pt6w#+^g*z2V8#KwF7J`|(+86a4H*dcflcnG9wNb@bP7!7N<%b8MdeeZ8?|VK*1NDU6Y=6BMym4kL-$UTIl}ET%K}Uh$X>2L z@^gUS!I7DK6ca4#pl7Jv4z-|Id{!&(Y;oI^T3yxA*ozWy_825WM;6f8e3^SbS>wAu zIAw#fYZB8tc*lodbZu{7M-y6});J%rdvtiRS5}jUXlrtVL1nVG+?;IMQ{z<6%c8t` z;oD}q?|WQRbNRzj4tJLgWd3w7>GRN=I6F^79Rp5GL3cN2jVI|FQ!iwGA8+bJCi?Qc z2RFkBaQ+x{rtV1UybafEZ}i+?pEnPEf33h|JxH>vI^m^RzjEvT` z82g^LGRqKCt=!urJ*i~#c?L^H#K{{Hrfg+msd&@ar$GR_Egv;O1Q{k`bSx>6mbLoY z=nHSjgxhRX7&KmG{nD}Os}YGBoV&F)!qha=SnRRZJ0Llp%?8NNQj^dCmOd0L&WOk* zQ1Rn#qXYei=jP9r%s6|xxxf=M&e7({Xyw+5Pt;)3oVU?a&Cxi7!?qd^<2D!tCi@)| z6MXtg4>l(37vfs}GNkLlJEo1jhU`x=kxQx^5{@Q^^fctMu# ztjG~^9^Ykig{smFttjYHxM(liEDrEOXfP|vvC|!}oI><@_K5mA@tVHra6Dx2blyR~|r%E$tGlHGRSoYD?*GDn1&y)mR z97EjYQpUP4xgvB=cqjMdaJYhIZ(KuA_lQRK$kN6=!GMQcNrrw;QZQOzbbxsGCPagR z8t*%Uel%Kv4o=~_5*)uAuWGk>B6#zzALIShKRn=zo?Wde)NBcKO6mof#wxS+^#lIa z#@dwJ_tl+RdN?I!^W8MqN!77IzD}}1kg+wB^$QzlSPbhH?#k_1>P2@Wn`#o) z-oRuc-XZIqdB%SzSdbd++-PZ6u`7C0Ep;d!rX0_<<4r_+S9AS>O*$&Fb41#iI9Rt` z&4=*$&rvu1+!C_jdO53FH~Lh&&q%5<$o^wP=nX3Tb7-idx_KE&8Ufwwpa%Ssy@3>m z96PoVT&rFu@K+!T0e@qp#l$czNP%a_Rq`QZxsl&5!cm+PppA345zM{Ky}#`((e@ZuExpr6+}rr(2g&(~Ym zEH-6EN35a2vw=CzGMg^zIhP7$UIbBSntDWWS{@jP)dET!`$1e)S z)!*jr4HwsOf-`~hf*x`Db@Cb0LBWAnBGlZ#_4?1OL<& zo~Kc+ssz-O9kPvi51K(rfPC-5+RKH>&S@xV_1x1bfZr4kT>OP^Y+H;!4qZ zIhm4#8gvC$%;EJ8ICfgO@MEXH-7+P5_qExb*Ugjoh~aYYKotF^rDY#Dm;pHALy3vI z#gNtx%9p?x2M%|sU5;J3S2*(W@oE~IfcCJHO)C%-M1WQCTsn4YBm~8P;RZ*2gc4tI zPX?$0P!?{7QTJ=gA+8kvc^{+q;ddNU=UqLk`qMT+N^8VIF zwocIxKGj&)QjOh(_x!HjF;9*cJnpNS$p+0{!%8YDjE9wTqO&J@$F0doL!;kgG%%s( zf_;8zUd|x>HsPnS*`r2n9Q%CC0(Odqz2W9WG{RJpo?_p=Gl;fOTEDa$^N!;+&6T85{ zyF_6!ZI;UwaWTV1Qq%gf;pA)|f%#QZKYKCS_7;;D9C>2AKuUEjny5jxzXM89(Fa#v z&@es%xtd4uV4OmCyB7*)wO@6k9(_tG8t~hxnKv-aB0>^@Xh`vYrMZ$F}Ixt`zx24d`IHQ zCi?TBSpmYqA?vZwgqtxT_CwxdG{pj8X)$$)47h*i`yC}E9=7W`s4+yy)86C{$N%pV z3SvjQN=4Hw+kMb*Rb$x& zbOEhDu1{6?-0pa*PIFpyZVuWNe*6fIy#V5x8I-)kPIvnLfmEVVla9e;^fgO_Z&rP%$o22qoPz=lU%spDr#6?``T zPYwPPI7?dE=CH5^d;OD-YtW>SgEl6(w6WQ@0!m7cpEO0YDN*Z4jk|twn=np}yl9*$ z@gqxh`&?l4U6{5ZjryXHLs5nB_|A#w^-i3jauAC?v?1>GlO_CQT(D+8TAyCc@2F3+ zQW|Z}T|e;uNpI|D}|FpKV5nc_<3MpfGuuicLE9^03Nrwi4d{)Z9|?9@|?j zZtut8Sks^jrnzL)z;e77Eyepkg)|ha5i*pcu(~>OFrP`f#o2;yOe-(&jY=Y*k*xy5 z>{6diTA2fnd#ugMj1$jBQbb(|-hgMIcn&$Wlp$3kUw(iRx>qI5_oQ*71vt;w-G=`( zKTRoZc=#dxMO`!Hy!0Pn0l5L{ACnJflLA=Bv$=_)6E;Bevu%o&>eIa^gel(ruU9U| z_28sKWplrdS-~s@1<2O)2&S2NIUj0H_w0~C+jnT`0@_fJ2;HH!!I9YnC_J2ao09&_ zYW!Kr*!c7RIf}iMTV}-;GMXx-J)8cKik-$$7M5vAKm!?C)XM!XQu_}b{AIeWBBhy8 zTNO3$l8g7W_DcQwGl*RPf?^EINDpnwk~=ii;xE^qF_AFPK41HlIHkD&i~*qR0*~+Qo{ofdCwX8Mj}VVpGad@I(KRvVg-6Qbiu_i~{MNpWbQ6S32`5%QEK0ePF}fy@nhwe!J)&8`57kvVh9(c}d(g zNwu^uU-6?|Y)5pDH7E|-T+G>2LB7B?(+avYmu$7R%SpX98QZ)y;~r3+f~6jGNCW^D zT7A5ky$Qy}CB;lmNBCcR*+2YVFV}{n(U@~PTbl{|+aV|51$}Mg^RnX=V@f$)>Hlm^ z*ue3W2<-B{>pskKJG;OWZcc^nPKIZ9So>2+WN;3Z3&0+E#0u*YxGAG4*-}P9M-ovb zA!B7>J2Ka7Dr0Bvo=!O$I(=u^kNH(OMRBYRe)KNg9W-Cj?4S8f)WwDcU!*x>9{K0j zV=-I0DEa+o!MlyUfkqwVa8=f@!1OHjKhjN#V|8F6(eNtVkNmzR< z{!_|nMId5W00qJoq#CSyP$C-zHmjkT&XQ!cxlQMuOw^MupYu;r!2EyjzRWFprxO?rkYH5(^fF8*-gYl_`cQRf#WK{$mn;}u`Dw~c`)UX_BR%-r3b zpvX#}OJY$4qfK71^z4Zr`?qGcTDTi^!0|pe1wrZRC`UmnwXr1D<7aB|ns0FmG>$YB z)~<;ancW7K7BcfQC?wr6I;xsZ{VdzzR2(4RDm$j#tef65mrLDS6haue7vR|QTjL{sevQnbL&U7-g-hG(1-cr&Kqt^fPXXFzlkpGy6 zuYW`W&i_PHC;^a`E734L+el}tN&%v1Mvhfng*Rajl|y*B8Bfe&IR_#VC zDl)pVf$qM-Id5QM1=g3tG8tOHkLEy!>vwpethu5wamb3}2B&+HRMAjFo!#9rlV*3< z2=Y+xMp7D1NLBVNsE(2{bVLa^*_9-Zi1PpB4%5V0R+_p{`2_wGA_0*FFGuyMdzdh+Ib8K7E(Q4aT&QZqEG@ zlOFprCiwk%AKPAUMk1ewspE2DzIfzOs-rrf*lu*VUvDSuViQ}jCJ|bpA&T+Pszfs* znG;zxRKsB9(BBbBCX0R%WJQe5WJX(FSeTe8l3nV4It(REa!#Gw(A02b&m{SO;$4Jo zdQ$y&MVSdP5&>;8#W8yXpbBC9C!mv6q&yam1I4G6cmqb4%M#IJkB}CYZq5Nq}uqe(q zK#h*IKR`XLQ@QYdCA!bHzgR;J`S~{I=7p@vB?ychN~yx^xls=T`NcnEohvwuaJH5a zH&JG>+s_@KQccB+i!+biQ;kcM}?5D{z2?f(r)kYPuE%T6I%qou?OP7|;cI zX2_c0=R*UaD8$l2mZBIxbWc*5b^3FG%3OjdX6DD2h8Rg@{Ku4$=5wy_p1c!wAjNK#hEHC4ohdLBno#V-OV%EeFf zJRqo{`LU_O!aKhc^3}c^{Bj31{Dln>ejejx{iTi~^ezNN$ZadQvKUQC%dwKP`MwD_E`gFiG3jmti3dmMOZYu5`RhoQ zh9+7;9tGi4|4ARt>i?psX+wQR$sRx>$-mpO|AKQ8FNtDn&PxKL6l*qt+G7)Ymio|p z2q}nA-93)He4??n@A^hNqR>uCaI)X0u4ygc*g){oHk_;^i_e$|&g5akl=R{EJE)KT z0U*wS{D(f|pW}5Nr!WD)NY?bzLQ@I8W7)*DG2`dt%-ptR@tHscJ6NNN2+J;8gz|TZ zt-SN(U3g4vddk*Vlg{xgG0`!84WC*9EfpG+Npi=iYNJDyAkydx1WV)`KizYBl-}p_ zm%kn5T8z=BS*(bDHIqmeCvkuc@?lZN6rCO$qf)gCQq`}$!B zLQeS}U6VDBCQ6WXYk^rb4lOQ`8aMq8Ng8-X%eh}5#Ou6%qyoCFR#l`eAx1aJ;^8EO zkR$O3K=Go0M}U0Qrxk{3?Q$U7MWx2WAx%+%h64tDsK%EZycmTC3VldD*er&utPeUF zQulwo0O=Mre9tg&T9u$p>zYKhDx#Ph5hmrnyJF%B9)1oj%ZXAn-DiH3UfW^sfr(zMa`uWvC;j($CI^8d263N%|o zJ1C%*5#1}5zXOu(zG?jE5xcnri&q+#%sDx9P$wR39G_a9ER;O>CbYMloP6zeQfz#Ve|DocMf$14_+|b<2^gqu1`GyX zh|u^E=*m(czY}vx+ShEpe4*7=(Tss{g?9VIePAEt)N#sJqn<6Lmg@PGmSerabf9ASQvMXJ*~kY_+p9OK>3d`hYz7HMlF14R==^*qKAYY z;=is1>p)p*#N1muLFO9JZ)lx~PBr(-OIYmn_03GgE{##H7K5~^2^rV`T8cRlOJv*L z?`12yU5qyGV(HkqYP&#A(c zBtO40d}3nd5{rkfp{oaBD2>{?@`BFaOdVyvd2AA=rC@WP>N~^}k9GC3rKU2ZS27^; zfVMrG*5|BK77i?cb^8yyUq!{p(<3MJ|wZVs} zAs~#7#Fk>6&i7@|wI&JF!9UhaLfd7h{gitD+48o%nvm#g!(#42Sc!qE8nR3!6#JmY z4nXnSq}JEHPF?Iz3h5VmP;ft_n7WN*`36l~5!c~3xG2j!z6_Bg0!Sm?H8Q%oA7lBA zFKxvk#IgLPokN=yhvyUXp%o)>F9hxO~v{QE?y8955*0`)4AQ z6zaVSaz@hc(r^X-mWVL_amkI<=#O-35r}C(iS;GJRRDnHN3}{&#RC&)_Uxg4|0GX z*J=1;uJT%AZc2=??T}^}m6id)ZgKqvl^#;SjDtUo78Yjz^x3aKwHoL(JV;P~3y$|^ z8RTd@lQf_BVj4J8EGV2C7`5Q=Vli2TtVwAgWKr7!CL5RutD8eiYn#P=_o|1VAV)OW zTIp^-`4g{N+o0>a9wVgkhCo(y4H|a06fD_O6S7e1avbUW zVoOX|(hMrVs72jBnkNh9a^oi3t!|2t_`?;`->%{}dd=BLA(83v$tXO5I{lI9u_axq zir>;GT?-_cY^X~K%t@+-VzjYc3vNZI=ltLUWy(aThz8AoG#p6a#|u4(4K=R0gMo_9 zj2!a@pjD#IlT8!`jmre=lDNi+rSdC4w46jLMn z|2g2%8Z^)KJ3;Lx!7TarBCC(4+s9%DYZwdiVkxG$#b^wN75idd!Wo~s8q0& z3}A5gpL7Kgz=2J;$Hb8h_{ZggNZ^MP-WbcS+=SxIs+*9426!GKjlV{2((z^F0HoUe zNIQuP=!jcZsIaI1U+(_7*m=UG<`zA-DZzb~$pvQFVsC&Hmk*RT^#)<%e0??9cXApt zDV#A9X)BFSxtm&u0~QU$0(<_m`ZV};7eUUSpzh-pK#fMD$of0|UQ^YpI}qr{@84Kz z11KXgO6-DGxjPy1GA(6P;SA~6g*LXxBOPOybhyqF^A!5aU@cJg|JdB~ZnMx{Do`T8#;@^UsOjwY5 z)?=E(Cpjw-w%z`83y1V1=o98j6_jWQH@llv>_S!yzl}W35%N^8(5kJp>mmm*u86T| zF@E}X=I{M4^rffUDJxAH$x+^owQ_Ci0s8xMsz|pxRYjx@bCBAY=CdjNgH>P8-hyNo z-xzzIjy-4H3bY*$e$RTnQ3D?*%O0Zrh#o3b*ZM}k6bx~BrcDZDA@`l<_8i#NUAw-8 zpIQu13OW%ijz1`>TwbS>ZvU+%+lO5hVr@fQG}~R_4@gVYCEeb}L+N_z^K9}y<-y1O zv^dzFm1*~Kg9Odpy74JM1xXl9TH?atVi6ieak#t``ys`j59uzc8a2rkJYDZlwYcz? z`%B(3D}(HA@54`5`u{E69-j;+|2KT8A`X&)1W8tMR2*o;fJlCI$$=UmPHskB_xz1~r`UkI1>Uyu;ZnZa=wvp} zc3{@s7U(de270~P2|xcOS-S`n(i2_q>MVxraRSCzFQ3~jBKYn=5?$N4EjP^hiLvpQ z_vs*sX2|=Eo*M4f-XVTZOw}MuyfHkb545PDdk<27d!bhUbEE1k$m*Y7 z<37tr0Q(gy-rid?)8pG8jh8ZUsKGI^K~2L|6w8$pyJU}_U7wTSfNNsg_6)7tU9q`^ zi(6+%PZrv7$>Ho!F+8tkbvQCY)`P<>rs%sBhX&<)&~EKkf9DvD--;tLT8G4@GyC;Xj$wRk^9*XlI2W{Qrsn4}0f zQzX|KB?ANqQoq|zaq$MzB-LxKRGtBU%zd{8 z9a;S!t4EIm9)zA>597~QJHcvgT30TyMZ(YFJyUm^7j^d^vxN@$-{+=zXEHFF9-)2| z81;M=e!KsD^WjE)lGH!9F5tH3k}X$cu;<|<&)o3It#1Cqm-;Y-*Ab_SU#Uw@ZmUgQ z0pkpy+kjV1Q6oRLo{NBhm}9yrMmoCt$DS9l`}y{#`<~Z6)uw%o5P6UO#)V_OrpDkk z8X-i-*+pih|32ba{mH~=fs1jxhbuP!?DY6pvmd>U%4^rB@mHRt9l3E;oTF9*elHB5 z>wVe#)7(B(9`Zkcu{fw(H{Soxy8ZDaD`oA;6Zg{4ressXEvWomVk-%|1cE!|MZ$4zTxxWKmZTEXY>^aYlKVQ%dNFoB4b<<}f z$@d)&1>^<1Z1xOARtD#%*W5jS=(+biHJ6f*@DR&;JbCL)u0wPFwzm1Q?DMkz+%I5 zI_;0`wP=$ARnZ4s-?J@t_~Ew%eM9KILQfB| z2s+J3_Cx1adD~YABzu#bX+NIcmW9sl87r?pigC4;;3Z8bO-msa1e2E|@@mybcXy^W zSJ&2BdairuHavq{YHOpr&0$drm(;|g%FDG9H|%~~?`;UV`3H>Wth#E_Qy_VfiDgt+ zPXgTz#@iu6XHOkpRVp|5S2!O_eFz*(_Q{*e)Yitmi}f{E?ee=8WQ;tt+K(Jazk9>K z8q>M{UOKht=>EIWmZTgR+4fPL^bRe!*K6uBJ`$yTm}E5LYG|6`QxqSw)q!;#*LLRs z;Gva>V3~Fr4Xo{i(5{01&E(s!8@$}s!RoPk2ziH?4vpB=8Z6s|)bl%)&TD(YvO}%E zfFY+&4Y}!TGoos-=Ec0ci8<2k#5>O!%lT!%Tql}~45pz@jjUpvY&>g|40CwaQ%_iD zI6MK~yrj8^UHBydPxeIRnI;*cN`eBjmAC5ZqAyoddFgr786+NvcX~Weqf1$PMIzTE%AA)i z=@Qe+F@bTb+>xtwSr#mw?MVS|H}tbwn2KfHlf_fGjSWheiyeGJIROIJXv4n_*ZXC$ zUng-U_g;JAXolc6ZqD4!#6Z-?o_B6nfww_y+ti(W`w3KIZ(GWEt8b7*H#Zk=&qn(h8KAlU)Ozr& zI*ncC!(g_OFL@mL8z|_MG&rd_`o5+d8}G%1*=VQB{%v2%IQKc=x${Qz*+C25d!9!- z$Chz4y=TuEjm(Ad?Q~&;QPRXz_ma*wav7k}1#uI8~Pe_g>#-dek`Si`7c?w2oTHrsr>=}!Hh3BA30PdV~S@A(2o z&Ex(0XV(gcp}<1I38BrPAd6Z!n6qRA*HepD1i+RJ^iGXN|gC>N-dgo#UxbqD`zH~ z%jpFcdUJrfrQN;jERj-9YZ~R8bX8XoKjiWXe`6z7C+{&xa5kcUjVm6{SWjH~x8>Sf zc;ZqARd5)`WqGc!*OX${<+Hu_rU!H8t(5vTO5JA9k7lAMcHrS2npmtN8~@-VK6yv? zJd+r_PnvJ<@tN;PZ!ArQH>H5=OI#=GR8y^P4I5FQ-3_Gc`G^~|sTb8NWtGR(esX|q zYohDq_u~eli(J0f8D6(5^iG5SmhElm)B2i6i^yAx)Ax&k!{^3t2qpgHC1Q(jyE0)V z1b?3|q>R0s;GKOD`|8w~zjq~-A2wi-I-`(ejL{x`~_Wn3+3!(eFv9OQvH0yLhIM@W5MUy=E4Erb;bd}M~U72do{n%UK@5k zkJ0o-)i8fuAK)tY{<5;V3!yQ%iL zxg3lB$mOzOHHtc&m9mE%e?(3 zK={`^`lO#zI_1Ag@lXCF>4z68_o%pDyFzLlnMk_doV)+JkAm z8cHS89Oe-|6Cs%XiIKny!y=>tEws{%mq)vn@(YdO*p_I{e;b$Sa!9b7WrDw+agJ z4$`jY3dor%9KVkB{u{Ziv| z{Zm!q*M9w?7Na>%Z|I;;cXQ&8R3T*(O7k5pu28*HIu38!E!X*@?#1QTsRy#)nw-&f zR(#{8Umw1k+kgx_@v8a1k;K>@BW}b;G4`D|#xNUBYxuA~cG(eR=ORk?Ts%}IdmSv_ zx_A3f69g7jwRZPLbX9l@i;e0I*D3Pt@}DX=p5%ow8chK`>8@sX9bHK~>Pa0P?W(A9 z_JF)?`jdQ3x-*azM{3!rWunge$B`#5OAqv-$@bj$CxDc*+I%A>lKss=YgOTXLmvdr z*E`7ULQ{^HQ}aiRZ^lFXcAvV3Esl9o(_~K@+%SmGWKK{F6 z+vm#}yzxyJoU#_QzkGhwHv~gG)|UAWFS#E#{6diiNJ+bF=?~RUq!%*Dqz9tp*Y5tH z&i|NeyBx1A#vl0W(M(rc3L0+>I!_&(o_z&s)s|-me*&RWL@bymB3E9v65!q-^NegMWZA{`4#PEny>Q>H!ACHEu~?Zhb-e zyg;|dQrYgV6E*`u=M5uWjfDI_s=#Xg9~j4UX0snwYpt3C@77rl5H=e_aNWL~_BAIAooJwLgL zuHcWy3)0`^t5vpuK&38$`aVxWjBzD(A_E9^-fpify3iAR(eP?+mbC2&h$>ko=lH(kb)D^a`GBc2U!=UNUM?zJwEtDKFoMA%X zG{gB`eVyfaR%SAhoI9W}bfgr#d6rDBKlhyCIb`rHah$_w)_LB2eC4pKXKiFF+i550 zRo+Hi;tn3pLV7MK6H(ioZCDK}Y-iTI$@$&78*%mXz6!bWrw-U3&Y__NWT}r1FH4u7XlS_ll`0gl}J6x--o)1&!s{*v1ux{D} zoeF4bD$^Q=0v68Xp~8Z6j$by!K<3NnWUwcjNk}InXPV;HeAlPTakJ6^`!DTw3}hQo z#Wu(Dzq}2M10Fw77pRN(Y&cwO+~8=I)gCNWbq>|r#C8qny@JtVYuD1&VWe)%L$QPk#vLeV=e60B2xB>@?GL!DXnQlIU-#*JyBK3NPrH+n zh1!0Qx_7v8iEXz2SSFe(wOjlZ*gOxF{fulwm}*6l7f)eyNkzDfI8Mjpt?tnK8zOi* zeWz*FFb{+I{nqFOI|D&*?k*1dw~J*Q0YhI@CS$6TD?2Xd+GEIQ1C&jln!ay6YErQz zYDtfSVxl;|rAnBEi(VgsBl*z;!?jhVztG{$^}jz1v55 zBKu9X8%V*^eKxdPm8$g4Rc zusuQpUxWQF4~w0)kVi9-5^&%bt#eWgbLhW0;%Td^e zTtSY8%9?PY@?7K7+Kc2HrfLj*coOA&+u)>!hwcHEQs?2s`-GtB^Ck0#(pw6J7jwC+ z&mVZ=m&zoYOdH}kmOHIJ4OnosDF)r}kcj)r!hdr=KUvYndGwwg6$LCG^4l(OkbpL% zDohI25*HiT_E(#eCPe9h?szRjyxILH1NYj7#QZj%YuZ4bv%0RQ?&RmHvoBwwtc+ws z?U7R|<7rS#VYfO0wy5LpMOyQ+6oEpyDKX3jG=jm_(n2v8PPv19Z&{K9GwCjUsSg(I z(fap)VU;RbG|}kYo=X15njn+z*zQzxtTp5FvMs((3RMLs(t8u-i&t~3GOz0d?ZsD2q5JR(3$Ja9 z+Ojqb*8)!}uYR+%xKnw3)tOG`zlJpb1pHR z5B(heyllKD#b8ZxW2t6rO}HIoC&!L>#?*_H2JQY+mGr1eT&cm+_xvl8l%eBi8m$w|A#_^0MncmC(PW}3+ zj4;a6sweEm$#aMDc3sBsgsyVTxZ(kN)C>H^4H12j$ctcq=&Vknt@K7>@HiFgHW}<- z!!6{&SICYQ`|aU|u-0N&g|o8TOx`d6$D*HMl_w%XOer zrog^>sp{nJb+*&q^(hiXXWRMh-umF|MAS8Aci&3BiuaW0Pb)#iR&5u_swF?QoHOmZ zktl}*eFmLd?rjY%-?tzp{#pU`coE{<4O7f;r>)58R{Hg;>q-%9JU&S6jHEsN7^2xR{ zwQrL5a-*DLsB`*PWrmjIRfmKWEd^T!y|g5?)>I3Eok|{F;~q>3IY|$7&U(yHTuV#> zDhZ#M!q8ixn_*ZFqrP*)#rh$h+eVrDE=_VI7Vd))xr_QTn>;5?4E{lTr~mZ=7zh`< zg7W2+G*H6DC_f`h?qa-BD43%p-{4WFMpl?jsR_)w$?fWbYuS1?d8oo-a|v+9QaCxZ ze-f9$99ET^<7D+kU46*_`*ZEwt8L(*djnh%@F1b9{XeKUV6DX?xNfVzjG#|nG##zx zE%WT*bCQH8hak+>SbNRmc~+sjAA>XXZfzyv=>oGa91TZlUEBYxCF8Gw-)q(LtrPSm zU1ID^78~M@8#V$Zi%=ZysJNvZP7r>X=qu4UVy)#Tq44{;3%=dkU?nG|<)eWjw=)SV zDM{n4)N~F8*wJu%9_i~?Mz{B(oxZ54U=5bC-JhXEGjD4ja;!v2x*g@C#)$i9Vh)A0 zigBs&1=b0qM%rf4E5o98g<;whSuI(B2yCKFkG1W*?q(nlf-cl}CO!;C8a&@70&>eh zr_dWZ6Z$&eb(BqNqE9&>NbaVlXD?eL1J9P zs0aMa@j9Z)n(vo^H)T}>7~4H+RIGi$70(`EJdaY$Tu+3<9L_HrwEpXdC7zD%x)AHd z(O9jSD3uywQ%f@a4H-Q@9Xqz0;rLIoG&((4oouilb5uKnhY;ATZgumxULFuK1W))K zJMDzKvFzT6w|*U+)d4YRCBtTyH$g;*YXLh6RqYM{$IowN?McJ4%C{e8>mTpxuV4b~ zy2z}NbKvvj%6<>Ij6PdGtX{EQX`C597v!k-wTG9Z74m$4@mjgw%V_d;y&frg*!mrQ zf4*FCo#V_})4LJ=`xa-$U&?{KO?$v*(e$C>rMqL$J1|pb$?Kb8gySND>&JcO%I5zZ zmZq1$rY)FLKl=|T)_x<_d7^!rqJMicJvT>};xJ$Hs%F%tR&v|lb{p<^=bpx++nV#y zTvC}bFZ1N>Tc55oAg0PNW?Nh_Ten+o(UI+BFQcFs7pUJeq%YZKEc1xcV$Q81EoBc* zfq}&X25H`rMjw}77SKG{JH2*~)|9nX4l_UygH5o6xkg%49?XNWRE8-yiJVL#U{ehu z#iBadm68&*6A;*dgX##q{xd1G6`_52cEqNB?9`IQ&ZEXbMrGPa^HyYjQZXmz>=Jfd<8IO7HRzq06fcKzu2|HyjFsJen>T{HwI zxD(vn-Q6v?yF+j%xVt;S-EH9-+%>qnOK^g{nZ56M_q_4OSc5hB0kgWgyuPm9aD~gD z-Q0byZP{U!Po9f#7BQ2Cl%Rjad7tJW3}@fo#9~u~qgELFo3Yhk+=66rb_p^f%~gC) z453wTG@ty+fsW$}%-rSDcK)3K9-;Hd^WkO3v&E=2q`2ihaL3-H84dxHs)&DZLd!T9 z6~z@Ml7ZL|$DcxA7M0#Z9yLpOgiQUZW*(lAQh`%ah&sdoFb|99I@ya&cf*{fm@?_7-v0?mVzIP-Pu(%3PrYS*A^i569%=ozZC{wcb4R?I3shWM7q7~ z33xk|E^LoJZ@zd#d!;GCkN&o9?M}n*p)^Ws$Xi%$N3Ruz6~T1Z z+lHtox&);kvuB59^(q=uNz zz|FBS%R}RM+Rk=oO+p(SI(~Pme(h$s8m_l> zIny>rm znc*AO73+qMkee}NWp_s3dE#O;cRee?yqFnMvEpmogvYZy%MfCbPvEbI7_)pf`Rk$~ z(H-MM27$nO{*Cu2?fIP2d{4u+b-X_(_xk(tS9?^F2PF zB}NG*rX-p2=KuWs?I@t4?~$nCmObMClZbg`l)`s*eEmg`bl7X5C3 zhW_3rw1&ejv(JcpNMej^#{Ho8qD@!ZaRaGU2#z%)hK^>tCG~-$FPtB+Ku7SL@Hka20XHPM7vh__XPI*&^TF;5^?6SBz4ZHrf$w!N^87=?$VV?w!5G zJ14?7zl!u+%C131>5Y?F;v~~F=|4lc=>i?t)q4>j_YAJa8_UUI@4q{A%5fvT8k#bT zdBpI1Z&W1$?_eLkT1Qo+&ND3x+d=M2N!|ml@RKtYO=pIPyJudtB`n8xg&7Y6Yy>{5 z!x^MM;!kRf`wyM^Ui!hi!vdaN@9w9Tr|0TASJw<8(-52w>zJEJFI4bUZ4G`c!bwTp z3wdy#+vr_9Ho8IR@iYl%HTuW(oDRWm=(CYqk_H79~HIxSt8Mj0bTH0W&C(GcoGd>lt*K*|1euR()(#1{;#sWv$Bbf!}0U zO!28}v`zNZ7$V0yf4LSH2n?yGE+Oy_nF`4*)iBS1TNk=kUHl^D!yi8rh39887N(Yp zc2?HbmQhw#&R)WR$T$B^i&LM9ju?DurNrpTaU^Ri0ceRi_$smPG) z!x!|N+c`8r^9iebWdl3?qJM@J$MenB@~W$!(*8Yd`VCQ;2hsTT;)tm|yHWd`bd80D zr6f&aHU3N6bqkiK-&o^ow1UhgwiDFR?DK@6lkDz~&W}k`im7XGN7Mb7qWSOu(`3fo zjYo|H?nB1tk>5%(lt;l|u(^*MOXTz?dbjRLKRTmup4U6PZ~9cI=o8jWYR!d&Y4O;j z3q>y{tZ^)du^i4S?U(fHwDEw5FsW4Gi|whydXpU2AIuY0$j@5^gHyOFF4$Y)$9t}>Vo9~ax(qNcC912pF5A}WEw zMQ`LLwim+#I@*x2rt7~nP3@c{bEk`i>hLXre(^UgqjS}yvRai@=x5gBMdV4rn8_OzHz zoHu>!2;dwjOlR^nBokdjlo3}6T%m^Pe3NjJ%$c@xu^bj%EtHr~ob`X);MydvWe9l< ze@0~wAt82KJ4?aLXaUMwVuFGL9wpW#H9URUfvZe$)Qw-tzxk6!xNS%}jW+FJCl};f z_QP^+g#NfO6?nOZH1`x%w44sF{5!|kNKd)x&6_i-^7Oig_1p~SP&{Y#G>FGtg|ARe zaDPxn8*|LZYsr32b7{*T7AKf7$Z5}*H-O}2jN6VR5GEMQA4*Uj+ski5cKA>rdt+Rp z@QUQyoA6V#r+$g`y~rCid{+N7A$MRC%VEj2yz_oXPXFbKGQCx5S}-OSoM)ax!XR~{ z@{3JR?pW2M4AUom>MY+u;>!Mzm>L%asHZY|j zf&bTJ)yeOQX#aY}D#wPpitb=Njj~^AxjkaKfA7BgY{`=AeM90*`)R!hbRpbBhZW2} zoOhihrF?+o{dT!@Ucbf6-NkiM!HToC;Y7Ro9IeyF{)fM0!Xxd&X9wDXMK87^CxD-q z!vohv6_Z2mErTLf6^j>B&g7la4BHOwyK(dyowLp`(ZY0L~QIGnkIPBb?-zac&jAAzhtJQ6+5xKE|>LdZA0B`JBAB}*h) zn7(cl0paYVB-(*kKW2zS)X{W?Je^Q(jPllkP zmxm8eWT1Pj_Fcbp1zD?o9JAA-gx#>zxZ{y}HANsJveWNZuwz5B5b;Sw#qIPJ6Ws5u zs(w3QEvvdwQ8(MZ$F=mIhJi^c>0_$aybZO2jo0tLf!pV-LMrw4J*!=^-({8S);=g* zzv5aJCx)TVaZmz)YQ&g8nWAqejQ_GF_vrfFZ*jsNk?JC7ub~OjIBhUsDE>PEcCxMh z)Vcdny_+INUcyW>mZaW`JHuDqMr_#p8!N;MN_1#NNWGk9vP@BYcDRbfY+Q~;lQ+7< z3%A5IQZx&E27Fxb@}9$(-v+n&@)i=7Zm$!Tu7=wJZmgg#D3g0ia+jd;aegZmZf{K7MJTfLutmg5>J31? zB^KyLaG(^uXL2h0O@^g@(+GQc17NNoWW-RQFLaAR?yjPpDXgrV&}v-`-A)@zW1z;A zENt2-Ws?=EegSS9LjIynI~zbL8f@QO#Xzs5Jpb8|R4M(JW-`ro1^YRP+gD*DR6Gw9 zpltj#_S=U6(PAP!-kP{yI6>pZr-S zODMV6nI;Qjzk*v(GIU-sGr-$1Sw#+#{f`_5xAUxDDFUbA9Vphd!MLbMOxY$?lGnd@ zfG;M9_>|q?K>UP(G>MTMh*O2ba9H|khmTV&DshE0d@?iZz1Z3>v$blMnXniSCtC#? znEj?*0;gB2mYB6W2i|EVjBHJ=9?gtGp#!x;$Zfj{N_f6K;&4la5atSA_IV?2if5A# zQDzY~LlY3fJe62goR>T;^CbPhq$M%~M+-!5Nm5~8*~$h*o`UayTq!feRZLmg)5IcQ zOX<}4+sQrJQNu3F5Gsb6VIgL*$f7YGY=8?W?@XOmqBtMDB>yvmVSf$EW%~k%U(Ip> zkB4liO$b6-n2T)r3g_hwxG!%<*0lwgC?QG)gg^{0mc-O(svPvRq5JP>IHJm{iYgNS zU!qXI{5w8VqG%ExKfAm%G0kYn0T>!T8XpNmY4L$5vN1Iq`CSvef}$F$<8~7p3K-uD|~oj!mgIc0z3tJ^V||JV09}Q<9U>fe1@lS{m%DWN@^E z*l2&+4b6C$?URKj&9xuBHNk-DHVhWRFLfsHBTEz&%7=m)9a%R%sbV@5-RaEs1Q3usFy=(RpeR!YL$UAGrw&Hn%$@E9xhilW;{_4O?>s5 zi|QMReh2XFP2&E)D!j0;zOu#Uh8ajkPCj35!^y(ZaQ)mFlZ_9sOEi>TU^OXdaBxuG zF7O*bC7P(aiuwU1`!57dx5V}m0A;`I*vwuCm(FdSss7@=)||D}TR|?cnm_?N5D6a< zlZsr@?dwjfP!-h5ajEE&D{8Gm(vr3 zj5u@oyQKh;@QN`RD_($I0B(=7n0yfZa(%Q`Hf^TS9gLc~JSLkQAP|aNJ2<$+zL*G* z2g#J_-XkO}G8#x4855x&I#=C2;2RGG#7i3I+y33epaf#A?icj*-7a z&7TygNl@se7;j`Q++7IP)&D~pI?Pzb0y;?|L!j@G5=E06P);|COadvDrMVW4%xLrO zzm|U{G~#M=z0O1{$2vhWr`^(QWr>oS+zvIS0qZsAyIxHM1DzC4r3IJ``FJ4sPUCQj zg#hmB3QzL+?;wnP={=_oX=!=01N(7YURr=p&?(o_!j`_kd)42g#FF{ssAz7DGvVZs z23(3LlvwuL*+B`~dM&FRx`-Q2khqvZEZ#j$^kd?cpV?%~*uE>FhC5@cs&f_r@{9u0 zl8Vk*BM%;dL=7XN@=x>rGkqg}w9x9Sd8;5NzrHQ$r)ikg-}2lg40>35`k(IIt$6NLWmGXrlk6(x?l9nqn>pg(xYPiga$! zz+;0IN$SHS?hj$wnIQqHh%oZ7DF5r=c0O1Hy$C@!!(5-ZbbX{%&eJPJi84xRR3^O) z%}fM3d5Usb-F+Q@{_RDz+v`YgU=1W46F5ns6%Agyxt<{1+ckQ>s0@wSg<_<808C=w z%s#Ts=#VhrOOP{3{)2`BWbEIkPq?HwB7QbFDv?N?c@kj*L(-}ksdkHO>HwzE9EAD( zv6_c?d?a29o3w^Z{ajLSop%HAs&XTGoW-`GfbnD|w4y3T9 z=J%FOKwkIv_s4$ZgGotC!=_&C9ESi5Jd6Wq`Oka*{d7RKJo%5vaM;Vxdi#@;@vzZv zw>Pz$nO3DIQlvg%%ez=g>n%APYcnCdpsR#LC2BjYz|h1KgZ0RkBfGoCK%r`Fgc}t?I6NkL5l9yQ&&Nm*1*O4FEcttP@)Mr$0;A=IR8by` zm?odZccj;ta1I;_Lu(#Gf6=8Gc7%C&J=#~idA7>oDzPtUzzuElCo{jwK8IO+*l7Y5 zLZTIS2~Zm2(U#nB9d&@Axc?u93J{U(cM}(w-VU5`GAF~q`f2tRDvw1%mxs_1T$uE^ z_FwPy!D5l)w-azNzxvO#TS3(jHY4WAVbmq62At_28tmyJ(o6<9J^~RIoJc{@0Ny5+ zSU4b|Z6tyJYmme5fbe+e(D1}bq};8%f-$10gI>3+3dkxl+o(~giZw+V9eMcU#7z7k zsIYhy(91lKEXzgb_Y3OnYz}cY(>&QmpF0G;v zD+7BK`@i=J861TeiGf6hgRvs{3rrGDKcsND5w}&X7WA2tdfbEK4i%(=-Y`nDXrqaQ zwRSIlRcN`K9BUopwZZjnKu?J>*)aFY1@E>F7NBW$Td+HUS%wKoNwNwGcFUUoLLB5w z%Kyzp5r2m>a3QggAUQzQ0C*g^U=JrZcN@#jdT{wo#uNPbht&)Z%TrpOkd?SgbaOO> z+J=seDZDg-EC}?08ctZQ#Y}9gQRU{<{DkeY7wP&_nOJYG`q{rVNcA(oSL*?77O^Ku zr7R9x)A!f5|5{dXiT}}gKx*>E(PV=m20rJ5X*GW@4^Nc;3D+;|nL-GoB_`1!zrcop z4#7@8N2W|&ohfe0mT9h~eCl;?65~&U!ypuBMs7vJx-GB>Uj&ZGHiydp{r2*a{!8GD zSe*bA4GB4U(_j~6MTc2RTmiszZ)6}|`X52HbPR`Cc?zdm0Wr&kOxr^(r6^O$i+(|a zwiO*coh2oGl>GjsF%d#G8dNs{*Z`DEz3Mx9S~);s*HvkbWft>oo)AK|bii64kK6!F zH2$x8tQ%L6lLPq$3_TbM87_QK;=Qa^r!{MFhA4N90WIEtmj7Q0f-$4RZ1$g?;~ z;It^M!ED5Jgg|0_@yC`l7${#*VH$JDtUJhMR{M%XmmGp2v)NKakj@)78bV9+Ap_M< z+e3|LT-bCKV$P!>d{>lYjE(1n>3|gKYmx(JjT_!z|4A49KVwG^7&}}zwXx~xjp?Kw z;lHCB-0J_hA50P4XP`ckYKu|iK1Ht8(t>-Q`9^r@=|ylDf7W`Hssu`%()q`&Om?7T z{0Sw=hls>fMMP}0BqG0UMMXKpsKVMfzgi5+L~@mxXmSKQ;o;A(on}m@$*&A7JTCl} z(&eAbXdEm9JW>Lx7g|eID#bi1UX61Tm%_)j}0!%%sT2l&PiTuu9hG2uS4K);lvhyvs@txIA zA7erm-FwAKrxO+3$GDYbZs_YHtV?sD&aUC{R7k;wan*1Buvh`>M&a3UM{ozOs;$v!}b`WC1aJ zq9q`#DRhS^KdvlRhej0VXud4{nU6M&<$g>KZLK2u9Z{h^f+p)12Jud-?h_E+$02mSGQRK^GyL8lZ!E ztN(EpmUxKWMCCG1C&k0yNkirEcz)~_amNZBVnO`48g<_#b)ZVLxBAs=v!{R5c8;eB z+7_s`jUF@qbi~r@8|6=$RYL!`f%@=t7O3RB$Qyt@E_cD<$TYG#%V2jV&Km2@UWuB; z(w{DtY#MCpIimBP<=*o^ecbJ{a{Rvo<{c*&j9CIUQ%y@=fy)x;H6h zx_+q8L|~fYj&oW z;7OWtoR9m!@@O>w92SBxvjhY>+igdp-B_7@H{1Dr^mJ8fL;dm$N$}F@A2vNP)Uyv; z^L(UB^ZiRjE9xO;z6ROjS!;V%_L&L)gCa9soNyqd%1ywd1iUxsQ)W<8(ZBpj zID9|vMP5x@@OI~?@+TIBt|Y-6*5u2JIcbH_BGD_9Xq<%<2!v=oR?qV(;KbJsg-l8) zIAaKH_s&7jMt>G9tx~$`_t%KrCGEa~{)^9z;q1;^N0W+}4doL7XxEq8Q+Jev2TAn4 z%eBsIy0OO1mk&wtnF`i;su(qr7dj#uo{@6H8?2aGWr6chCzcp0-ba$2se~mz`f`Pa3{_=T!0A6!FykYUQYQ z{(4WF^R~7(LB}SfR&TU;M5k*TkgV_=oWIkecMBBcz$14i z1BnFO=T8;Klmb6>po9*WdE4$M0`^PD6O(5{cCa>oO^Q~BnH@zdYU#mBj9U9@cdww5 z>JhAG<`&!wUjIUKznMu}V=kCel#-?7=g)q~a9q$;1lwL4`2BKqm@!Sqth|(GyoOrx zabZJ>XQYO>Ih7Nl9~Z%qiWT!GFwz?1|sltv1kj4@+xLR zW=Xycas8|tPGuqFbbX7~iROngf4vVdK*YhrI0h_L^+l+hxqg^>#{?ZG7kDw7KhX&Q zaI1zRO4t;2`ZK^+5y?)dU~EJl@5|@sYloVo|ALj~G9N(i{d)L_9ulX%mJ7_%=^1L| zydS(NT1m0V^$g;q*c3Cs>h(ITrHa7YzU=4z<72-w*rIQ?Yl3v$`@!Ff!H+LQEsJSK zJ~zS~hx`6QQ`_StXu=8xr^V!srcd*9QK+~KG4tSZ`QsU3GyM*J$t{Ko$IX5j+}~ox zmG63FkMFJT?DZ-$hdnvLq3+L>?vxjy5T8%%iLi0&O&;O-{;ps+jjS^e7s2 zzbA?>w7nUA^ol)qbkki=WEyUe6~0EN>)9xMlBcj}9`}7(;ANV9 zKV4J4H3dBPsDlx}Nw|sLNeS0oUuX|0q~GxK2CHXB{LYHXl6M?U&w_xvc9lg*VtS#yQI*QSz8@0;bW@l}Y!V&j9wzUvB;t{gN_kNu)E|jSo-;aJ{hctV3?55JK68!(M;d(PS>mr2!FDT^b=y+{{`Qzsdb~tdnF7O*Kv|fA_ z!73lrkiI%y%RpOnp+X*!n&hpE;XbzPFV;myV_Ehu?AggRkRYhMz+D+MV3024A{5O- zsz#$%`4+c8^R#Dzpn)eCmVqlj=lZLK{@(?e@`?#u=ZUJpqdCr{h7#HhtxzNMdh;%J z(rk7Y2t72mnV%jcRr4Z>f3TsJ^BJnzN$gsQ77VIZQQZ1 zOT%ia9hQ!j4P&JMBrQu;nZcyac1nyz|zp^aJOGZYt{dqf@i18b6gGOXv~OD zbx^u;M!f>^7bo{GWgNbjtC~(CgP{+qn=Wm#MheAjRx%no1!Zf&ZWevvzrE zAC>v1(Pxx~28;)9`nU$xQk!H$f>4%2=-E2;@=lGna`ngOY|y4;LMgbof#a6i(x0VL z!R=DLg=t`h0x|?~aCkgDE($qruFQOd;PnfaF;z`~xqW?zXc+~Gh!hiOaN-&S@^32a z&)@s)>XMze4NF`(d=2t9>>Q2PEJvAPiE!qXispSni3Ca(f0y?l>NfG+yiY=h+;D0C z1-v2`cy}n7tX<$Zy#!;L-43j3&wAD^pWxkhl+u(k=kZ4z4Cl0*w-LjA&E@|p;UA^{ zIK0^^?KW&W$(wh?a&Vf4X!d?5A*_ieCl7IG#rG$ikJUk5zI^=3EG{=Q%5@sYmw88> zB>0qEn^tSKxlft?xZy?Md;PgRJi|0-PtH!3=9?G_ZgXnk&AO3b+3UG}M?Y#)wv{Ob zm(Uk)XRD{{!S4o<{zqT&+DN&|{b)tLf+Un5<*e@q%1W6Hv#S0@pw_ob$f>fxdfrtaqseSvy>LZg3g7}nrSI9;N_nsORtwCw zaa1Zo1v(H<9hL8X^sV^PJ-*T=m8%FC|NXg8mHteDH2IuBOYp{93QB@S`Y|6Yfk5?*m7$k$jKQ=aNli}b} zWmfvFAR*Qpj3ZKnf6WOX%jT(iJ^fTN?64D0ak2aw+HhoE)k)3;-|6cpP69)Z@v@PS zYcOM2Pu@RFyCYHt*!6F$&>JGkif7};1nLdEf?nyAf=>yq9Wqq3wB}2p#ooNY6-*Uu z?T>Qcc@9%pbkr-r&%MbyHaB+4){@semy!U)EUP37>f4+3{25q!l` zl-HS9+=Ux^sM_mjAq2wF)DW+sk)#vMSs&adc*u&t``d04XdzvO?0Nj-Khy(Y*gDU) zVOZa=yEO@uxvW%>k%c(DN23=GsMLlkYI}@Wg}kx6%ogkm`m2`Z%?`f}ee4M5WO?99 zQha{Ln9x@5^egzsY9c&|aPp#WxDR4ZzP?uyQ_|ywV=l_tC!`bDEK(qBEBdud-9i&y zYbrV=rMRcA%D!0Q{RRw&<9upbewjOv`$;BdT?Kq&m1;aDCFyrz%ah=!VCeZ68<5T- zP{JRBW(Dd@_xsyfrd$nN5AEaWevh(r73=+hoP_4TvUZ?N9kq`rTd(hKk2#%5dd{CC zp?x{Qp=q|Anh+>+1%ufTY;IZu9L*o{k}POCDi*KLbL~td+&-t^nxrrNjtuK$og)5N zOVdLyd|=F<18FJ+uMEp{6~ z_T-zMg{?cE+2irk#j$;GVET3VyJCD=`RO*A0GboF;fzMHrDQeJK{0s0g)<-iL2{PN z)t>9h!GF}*S3BtJ`FFAHb?M-9F4Do*fsN^n(cX5@#S;Kts;6hAwC{%)0rtLK8t{hU zLb3mz(ezKM1A==y`ty4J;|PxakDi3EYwnc!`}YW z5wQ#H)P%{ZZf_jA^`uFjtjH|!oXYG6ju6R;?e3s>Be2&qBq*{{Mg}li9e0RBuxMWi za*cL5RhK;$8jMxJ%$O(QnXwg>QNl06focsHG4uDCDx2UwdWZq3u$mp1fpXQdygDly z40Jj*v9C%IP|eiEQi@KEHFtSf!1(gXc+1DF7g~bVJxpGNXG0la8pxKFKX&5wm_SBH zZfn?h?3HBL@A!jo+G2kGFhcPqt`JfiHb1Xx3WbyTSPyh4g}IK{?Sh_-X>xv~js2=r zzVx%pPJbhO6~|;d^M%Ni?=m|5v?IpiE{C{2yIV=TnI#~C7PjnGi6UR9IQ)KaI z{H+BYuLNtM`Aaol3i`$B)RZ!oRd$%uCVr2FW`&$M;a3-BXoF#&r8u%Ruyd`cb45Yp zQ!F4s6Nqzk-|TGW2-Bx-$cnoKZH+MVxNj0d0O%uv`;;rK+-;f?fT7s-M^T0iwI z)*=2kLvF&+M+D_nPyDNO$snS(nO9lWXFl8GAFCIyZ-I6mpXl@bX)jyD^V=dUpQvhQ zq@*8IFvj^CY13FQGkO;a0nAVRfRv)TVr+-?XFp!jG_O_3U3XBaXhqUZ7w?L`W&kB? zTteE`Bu3W(=|88Me?XKP-xpx0_$jJ*KfnKole+vGYF8w*3jLmmb@&=P4vcg(PoYe$ zF&X5xQc$c65h6*8Xi9}y|#G`mp@hC0CU{oO%yUQsB=Xi;QeX#0WMIs92w>{c?1@b8G*{gqo|}U?Rv5NAl=N zloFA)A0kZRWEbmc-X~Y(7^k=Tqb+L3isajuzJP7LTR6@w_81W$Ok8>4R?ndp=AtX3 zVl?^n$_+30cUfY*pP3Z*YBg}?XM`;*HJWGNyGc_idVMmVSoEk0-cpOtw>#(GL=qB_ z$=b)F&h%poHrr&y=$Yn&@prh9ku-wh2Lcbtszx-l4;{*ItjLcWY}ID-pulgL0k&{9 z9Y&hNIH3h|XGc*f0&*KcK*+%L&|1{m+Ny3N&Ffnqtp~7FWFqr<5-i*ATCW*SgjFaC zxSV09G)b!|uz31a?7+U(dDt~~w%mj=RM&}g@Am7ead|Z1hbwp--5+C*H-CwvMh5e6 z7|Q=v;QNe(T6I{3*6ZP*F&wwNELU_+2UX!Lh!9P=GB3I@gaS&|&S*>#abhm|7Mfeh zkfD{A9C(1z9zrfy`WT#T0iCQ&F?xv=eO5^yOsi{C=dg8p^SGw`jw82}1Fg*9H=u+} z6&V!;luxOF$}K!D$4~hz4ycgP>$tMMY8s#>7#tJ?lPL0KNO-Yq&9iF3r9>`kQjr#z zJ|zB`R0T0$TD34(PXM}P2`N^6Klg^$j=Q?Mh7kC>|A^KJ%D?h#fsiFK(DrdbSJ$l3 z(IeoefE8{QL==g1ZPE-h;JEiz@@( z{uK&^ECe$)s>*k-wRx*m^oN$D&d~bpXA{H=HOPGN?Mo+|j~_=3LsOyuJRAr{EExSO z8VWfQLNSKGYpB_2Sr`t#_TOA`Zfh?hBC-Vaq-NPA!&m7S10n)bsdP|t8y=4%1GJie ztixRvg-pP-#qK@slYGdQy)ncbryT1s!Q#bVzlTdcftvQskEgR2{-cRh(>c&d_iT2k zGKba}1r>=G285EB<6Axq3Dwxat`nytm>6=Koj*}{U_qe26Gn=7o`0{IRlwZm;+IUH zX6LELmzTYt8FF(jidoVxo;OtT<`hZx=5wQ?k3IsqaY2=?2hJQzze492d@O^JMRhr{ zVm{as{Qd zAX(9o3zESgkZ;A)LSoyDVg$CDRTgW)Q9}}!2aD=xNC78bybys$(bd8!gm_y&-;%YMOPJwT@c92Mm3#cKO6S2ld^-0wrAObkPsc#=D-r~xwPmtnFnN}^-*T9k6> zQj(G{>TzZj7P15Y4JnTy$Rg)_uw=m3f41ursz_vhb*It#9E+sLC>Z)FL?y=|w?kxg z`)~EnW2dt6$uh>#)ey-Ck&^y1+Eo!cjn*N6C*w z=_b~=8O0KfZbIK{OC;Y?rD5ra0$Yh54i80-!HIL3D^@v_f~(i4s0lqkJ~n)R9d16` zE8NR{kQporP85Nuza@)D|36~Xl^z^#tVoi-Uo)!*4jaOR5PFXLw|QP0NUi zg7;5`jKbl>C-I_32oIMlrW#G*n>I?_;Z!vh8-RGkucI|JaRzNXa(wO z_R9eP`P0Pt`@L&3T10G^NHoqQwry|Lmi3G3ed2<3Pr_1`1#R)Z$ZAZO;G+511$%Oh z9ovOXeXCU8*oKs;#4!@{>r)~DiHLItMT#g%?u9{V(0+!@sVr>b&b%}h($Pq$35;V# z_+2{peF9+@oHBUX7K$bOys|x}QdJg?cp$d&L}*TXJoNs|(#Dn?=Cwi#(%636i=aQ0 zow>l1O#AGYzCs8Dt;lym*qcE^8caA)Q**=j73KDhjyegEQCs#JnxbX(nlt6C86i<0 z@DsVvvDjkd0`p|fvxb!++ss0;fp6r%`e4-pFGa8hiO5nMK4G-jK$^fC>7ty3QDlPQ z1Z#c~7mQ}O6}>5r2gD<}n23kqOEehnWD2}I2wD}9LLSfDx|&Hst;r+CT*E82#1tW? zvc4OT=^;(sY6twu(?vemC8IehNK>fW9W;@1;|^ejvgZGmH&wp!Z+U06j1(DCEM|XQ zSTfqjG9HYlEvm`W&de9aFgoA=0%*%@CD!SWG{$bzx-=rOLHF8D?|%=IaNfDii!q(; z#~2SA_K5vqaY73-unbL?3mW>n@uG3dhif`EqesqFy4}&PXNxSZt8#rz%OSMrBHt_D zMMofmy6A}AxM9hm6_7P}Qd2BMQ-ekd2zQ}Fb*TXpq6e9qNZ6zmv)YttSz@Xy4r>_E zH*$kKHlvBIgsWaDWF!8=9afU|rnrtJssUY0xa5pRw;C2S=*s46DAk(TtEmhdwxsU< zE!WMl6E#0E7D43PYHT60;{~Gf)Os{E_3ilhINnehhPYX25-Ra*spgj}UWr1kR(Ukr zkZNd2TFO=}Vr5Zc!^0f{+W54RAF;`EyIe`^39EVi@j=LU&6ddg(XvdT6Z2@kpv2{F_}nP^&Y1@R5XxHrB+_2_`@r5;#_4^a*; zKH}8~E&Y7xOBs0^FJijIGg<(&H| zpYSLuVWxs@;Ehrj zz7w_Cy=j|Yu6?ePUe?4HQMXULN1zbDSW!-2-@3J^Hd_yl==^&l%Z?_6dGq{BKM?s- z(VSKHkAnm6=UrK{Wb^`H>TY{SyQTG4fo0j1v*;|Qguy6%qx~}Gpe}Uc2O3lnxY?fA zSVT+)ZzYk@?7U8N{5I4fZS+~X;Xp#jIB`KT4Y-quXOJt#pS|sG!nKzdGWQS(2iahd zE*ReG-$!OM9${nhJ8icuW11-7=u!&4!R@{H{~6^~<*FW13bHZQGKk*B7H>;cLskY?gE)A(JrjXUc90}PPOo*7xWuXSn`l%sJRl?VJ#c?vRg5p?1hxc~FxvwxP+kxt z-fThG)UR55d?&38{|z!>Vd)+r?L}QV^yVkrEF-WYiMUm-TTh=hHQ2lbyhEsiyg9Y88(fuY16UA#OLxLOtNEk(sGBu6{F?#mkYJA25pRBj0O{QeyB`_bpi_dPE} zfoDyM;+K~b+tHqP_e(EFKFS^oOjpr+=+|$HN7$9$M-$0kr;+?dj;46ytmgk_Oj8>b zA%iQ3ND%`=Sg2rGZf$g|7zh7Ok!%cp%2V!I&WKwDeYKg@77s&I+c9yqS)A&e9*g1T zaTcKZrfn2c%zxBqZGu>wSI!Voh*480O<8CV3xF9gG^B>;b(CgbM~T>!kgdSzge{pC zjRuG8HZ|8GnwUC?#gE-@{uacvjJ>Ia@{GM zFsRGy`{bRmXm2*!sQ5?OrWMUy*>r%FCp4i)4u07?KPzgHkf2=n%QDzH53;PN7V=-#=A#iR=IXkm*u6vM1rO~aNPkRCG zY}C9F1Uer^k;KzlnaLFVV8&$7Wg_D|4lctu_}0*=Mb|uqgdl85^-L^mcuWJ~vhSna z5&2})8CD-MB}vPMItFMY8?kl)G#HUp?;*3e>I$AnFvxfEcC#Sk>4zM34ns{8s` ztlFQnf8Aqzp*MKGv88($Pb0I+jddA+L=c8PxksoMuD>En%Z!fw^O zt@y+{LvZuv%Vl;?ID&ZZIE(M$&d-MxMW^#R&bF6|@#iDvwA&yn1YV!RKL08!&~(9C z_aKIi;DoC)W&Gx%%-meBFMX374S7pZmPL;{@mA*6?Uxu8&dBnY>Aj-I+%e`P)*uzV zTD((<7>m9qbC!+*9-c?^kLL*574NqR-`6vQ+z>K5%auXtj|ADn;84>h*v z{Zeb;@0+uWOK{#AZ%fM==A}qBIHnIN_B-@r_+WnQsw3#JBi!^_Oly7Who0j~Ht_z7 zmn4_PlVb04o2Ank17t#KEmYt|sLJ7bV!)!}9+Z<`5#fBpnNu%C{>6+_opFWii<(vH zZ_VHJ+K7iMnspCu5V9dTTrj0uGY^N*GlFG#PPQ#Wjx1|^nrr_D3os{*B}pp>MBlCj zD+zYG+&Gz*p=Qs2qZ#4?l!{Oh$+rwKOdY6GMMO-19I4G%N^dOcP#KeV^gz}H)NY!U z>AnR{9b5+Zt+O2WP8B`+mAQtk`0BraB@9q*3@BnRac9@tI;-U94(-ECr*@q=?Erbd zZarN`llj)9U`z}4*4n=_=4d*>F6nx3X>okqUpnrNX?)ssXjl(z;mNDwBVfmqTAlK{ z>Hm0wy=4t^T3xYV=^7<&d0^Mq=ZtRGe#647*|XnzWAuF=!;XF429oT0f_C*r4<)RA zH)$FcWSg57FDwy}(aDBTZyjj1o}e#NyqPHIC)zE+&ySmvR}7imK@?dIk9qlV6+g9p-O(o6z`0u-A?H>h58|J)~W{TS-aJ}`>BKzAXf^giJAw#;pnuB|Z<@$qXx z)O>%DZ96U<=XydjI9fl+=5u;$dVPcpO=!^{2ylg4yN6%W_rQ5H+v*QV!C-Ex{# zWY`5xXE{#33@u&?FR7pZ8733??f)(!CB#t`ygW#jye~%EYe}h$a8w|bEf9wxSwIeSc10>TtKi85Rsu|v*m<-tK`Vit~Vz;j`t z*z~OC7b83&3f-6^>%N&hP^3n^@o1pX<#}XLMfHvEeMLMchYpLI;b^4g({L|q_S3-Y>U4QlGr{|O;x0|;UEAAfw zo7^Y%rH`BG*R2Y^3Okap?#CqWLmy`vH4cuR+}5Syb3?IG{Gz2TZsMEw38$Ibj?4zW-{HH zdDZ0yUyY>S@3(bFR(Xs0lcI$1MR@9Me=R*(*^t-s{o$16+77uO!w zpFO4P*?CIv_Gor(e)^zvi!bNzf%|Q?{l@4mZlq9R0=@lSmGkuO$Sl9gC(bwRp}kOB zKEKD9b*HN&x{HRQA-rCo`-Moz1H$3doet8_orp^%{Iyi`1pk7RzvGH3V<&8m$7(Sh zYtck7qJxP2tKHdeo1IdJhte(F9)&bb@A;#)y9iJ3qg6$w$&J50pPy`U zwwE|JrVMiQgj*o934RTvqCKr+ZA~YiGWJ^;=Dt0|Z8lebF9V-|Fu&>%Bpt)}u|sp& zeJtY3`hKSSQT!(Tc6&;9+$V*PP~=Em}L2>(}U_q%5-{?4{MSEMKDM{hq;&*-rEMDLk* z>KQ}#Pm5B&oJt0s$P)Z20r(EiM3w$@jvof*@~dOSN=7I>ZTI4+_HT$E@8yM-st(Kh z3i?mSldtEqicXUe{ma)Q)2;UXrc>|l;y*c7f=rI$wQj%|x(*aDd(LjpJF;jGp$86Q ze>ETM1taT_5f>ph2?IGg8pN!0A5gHt7A%OJ(q$7>rv5fNk>WuX6`6&vrLQ%sP=lP8 zTtKR^d(W;HJyK#h;Zc;tkW&cM(s)q15b~Nx@S&3^HVGZ@0+dtU zz$IyuBQHs-*^8N~RKKNu4kE-pO^j!RO!gM-Tj0`8P{8KXnBQC&RH;ulT@l{ z{HMZ%DhEgWAk%~^rLHz$Ylo@i%AQ|Q(^@Iqyw$=!8V%hnMhr^%b(OMK29I`>J=G?O zWbywH_Eu4GHDUK>5`t^6K=9!1uEE_x;}YE6-5nZtXo7p=?(PuW-QC@$-|w6M%vy6Z zcYV`qol|GkQ@i%Fe^u(aL)Q|vJ4KD!zur0ocQZAlUK`XNI~Qc|J?_cz1t-Wmy#yLJ zoDfr({fTs(;6$hsP65aocdkz|#e2yW=a=3%74IW5sC*-Mweo+mJ1SrU0jm#ZADhYZ zy<8`erp&C(5fV1dpc4j19M5sewB})$`ht^YyvJog4C`}y_O@kc^v{p&wb$EwTSvK> zdatv`Z(LiNgiY79uTE2B@~@ZQ-A5n4uAMkW;cW@y`3~3om`%|`or121LJDC@@sP& z`?JhRMTU~`iViDOjB?5%ikH>5$YhPzZ#FmO>X+zc>GC^0j;6nf8m^~5)_ZI#n*#=h zQyiDwQvRM{fjry$aYW?3_BYl3)yHV( zU>whrE0ld>D4WnTa6A0vdLS>7iBw^~s@i^}wC2?H)yZwwTz3(t=3?JlHlR-I2Xi94(pzun(BUiikX_O(KFU9JCt*$ruwDj8oqB8G2s!Dy8ye$cd#CSSQ4@6)(?a4~GkoEaZy@ z^IU0bMTSF3OK2&F7X!+HTK(AGfq7UEBivp);NNCwh{{h3*b~^0mc-CJDB(Q&`a(>c zI#n_qh$R3UsjE5I6#k;9w_!VqMAC<{xDq(jF(PO<5)=74_D%Q{!g$8k&EN8(@p};c z)mBnlIm(D`+?PGX_!^;nz_adl&iML@YN_;FA(tGnAx|6EtZM#K`hFm={3}f@1ux&n zrf;>~mPbIE;Z9|%e+evM-)BSoRNjm&TF^f2hg%RQ0&KOO1yjfLSyAbPMG)vIe##iT zATu3>EGy1-rw4I;w^gtO6e)Pes|n=y$58Nqm5?3Z{?y;^detf>m+A0yz~vqRTgPX;5r-4HW_!d-91Jm+PjAW?hssfQQ8W+{Y)!; zb8}tfqEG-usomw2T-k0FUtRICQYw3Fe*^oxBRdo+;DLyr0rzQio+w^$!-)JjhKdKJ9+QKUU|aoY_=tT@w$c0^xihZ|Fbd7 zP|0^A_b%XzeBu4!25h~h0G{~Piv$G4zg59tggpKNHY7#5y}EK zY2j#m!xW?J!-Y4-S#Ia|22ZwYw9BX7M|M|-UQ(zRMe=l0$F5cG{Ip&b&{iDneD%i) z@Zho<1-}ZlfRb=;*;KFaENa-O@tJ3VHx6kzd1{1-0*RI=B++a|-LEhWpU)M0hcA^Q z@0nCPodPK^*mn9y*I`KD)2qVy;!)+)V~CXt7AMFPMiBakX=g?)V~1aY>8v%2Eytm{ z7_ajfj;?b}P=>J;cFRJ8G6JHNww|BDbUA`Ebw;KvE+q{@R5pVI3wNnx6)EZDnWz*&4qw$*e z1cl3{GQ|ObF5LG=d9SSEr4YZ#GXvsGRWRGP#()I>`BIrzl$^O#pfw^4-#l^(bnhg| zBt-G=Ix)PBNkvT+PRsXcVnEyJ^CF(N2kY@);@g&=k;xY3d>Vg3_h(L(_AzG?|IE-^ z6y^7^CKPJR#H&SKt3{Q#TnFsYjGUjkn_o@`k-!t?#XPgA;k)q2ygxDdew;wPZf^LD zR|2_XD!uQ`W-TkHhI!6gc1c&~PIfTMR-ibZOw~2Vd$6ztKfEUdc3#>m@#@oo#C>|N zEY`ByVYpgp?F||cyZNq{!52iM61qMCq3`c%=gr$PuNK#>O^38ZJM?Me-kFmvMLvV$ zt^$1ldw7QwV#|$8^^#RHdrVpyzi0LZCX}_WP<`BQboe^V$Sa^jTa4ge2k61n>W~EG zbJhF)UH{n;v}BcA;M=^ zmH5IfYV~Px0VHNE@2nF7$z^2}q8QnqKG46T+xBiAaYuY(am`;Qn3+d3CqiQdPf zdU*pfo{A$%Ke~uu(*hay8ax@HfB!k_>AZ<7V>l?i;l;f#8|AWb%NDZ6(~Lb22l&q~ z6Yx+MUtFc5dE!BwQk)jZhN?}Gk}6Rcp&(Y9S%@~cX7eviQgqphL%GfPjzxb6_-9Tv z+dR417#R&9&c@xus9AV=z~8W^-&w`jw$GVuSmlC7s=%q>$8XF2!vQvFF`|o57zxXO ze@xD@P=@I&kPUVORom&vi>wux^r+$bd09THnr!te7^B|ax$Y+$_T-e6dM$nhMSMva zUcQ7@SWb!dUCKB|jn@;){-m!}Rf@_C#5x<9Bp~Tq|7-HF5>2lrx%~L24qsMygT50q z%GnN96*awcKYbXM;d1l{5pD4=j3YI>95H=M8%gt3BF3${f}lnF)}yH}IUj=sj)Utm%{Rc!Xq`U=G6NF1 zHo83RMYG(}&3A{#GXyF?d|YkJ-tE$$-Sx2VHr|H^>%#l%A0IE>k8q=^ZQTyXJ=wu2oT?BK$h z@M)sKxm)kEm73C*I&2J0yVW&K-@?qV3?YAO%!2QoRHjhv)f)>Ingt#Xj4#z?)-RfM z?*D!a0v0->DG8fmJl`+*89E$ch+0lJ+&>b@{#@P&kg<@t^nVBcj!M5_nD`h`Cj6Ks zSiMmO?f{Ry#oJc6fZ{D0wbeo71nqTS3ChodTBzm7GBHWhe_u}1EQ{dmd{RfiTZkha zU9=h}bg+*58ZT_c`=6ojC`r|fi`{}CkOfU77)FDd6!As00xO5PTs^zjk-b7kETTi- z$S}fbnmoyFxDODAj9baVI_>=5h96IjKbaQa6CkB}_T&a0nE6rg>l<`f4!fW5ptT0X zSA;YtAa25PtM@B$KIRWTiJ5twpGVxY1==l$zwo8SE$#792bG%anVLQNmBpho$dVw> z6ezb+!+^7kg-p+)r3_cySiH2x{2Lt024lI+RKv#_v5u#+_o*yd?8JEpn9);5y?5Db zP{vs?YHbmSW*JfLek2#LC2{kffMRU%g$f%dEeHUJk#X99!Gn#TzYR*P&)m9el+yq~ z{gAN!WL8k^F~}hT8`B&1hHA{GP_sWSe~gkY%zxuvz&~0-ADuBdID+@GZe|h_JgBP$ zB=p9_Xp^&D<}yaOl32JyFPwcsQEbHyuFZb?UohU+)Kd}^)(KJN+z8 z4lq6u!WC<&)THnHrM_DK%R!W0oUv?mBYX>!Zy<_yul zQ%W+07*f~x2tJZ%nvjkI#1)5$a2kPz2Ii@gpJ@dz{!ByVwks_o*V4RC82k-aEm688 zRD)lhCOtaO^*0El$l}x5DO58*F^1afHcmmdx;g;TQV6uvxc(>TW9ml{vnFZ!Icv^a_bA+NQrFT3!#X1S`rOI_?LkN!a?{G&l!zg|CKvtb}H zd42rjwIhl8WzxXF5hh8iHvsm06HtTC>56&BD!-Lzj_SCA+A$onklnNOVw7XaK8pC_ zFnxs~^>-MlGSGSQ^0L;}cs6H;`xoN#ioKo6KyBaj$PU-L`LBwL< zh0FPwbk*aou~_iN2qjBJVzGmebq5Cpk0bafn#A{I-1f266)t+sa!A3QRwDYMifT8QU3fh)95Em5jPLgNwUDD{IX(>Go~to zkul+MI*Sr{ac44gC6-wxZzKh*)j_Sc>(w3~@7s;G^OO?XndF>NRY<6hJ>$vXq{*~h zO3(54$BhhIL+7_E?SJu~5!&4PVYwdsp_-q;wwK9cvI38B*ym5(jZLS?j1JGYD1^_$ z(R9{6sNV_L@uPE}78M+(6*~K(^*WOW9D#*4Q(4+?is5gamp&$BjV-z9WLWXSx0Njf zX=7sO29xADMz-RV%s$_<+=(K^xRuCrlo1T|_XhB-b;uQ&YM8|+wx(n6FSX@bN`+aH z>?A2YAX_(y-ziSjfFg!gAjQ_5ihKyb^NXgEd26D4(__wOEa-&!03y*Ba!qPQr3b14 zOrSDO*VGhY5Ij9lc~eM-riExw$qDO=w2vgk5lb$K)XTts?e(e9uW_yN3RK(@|D$v> zAxbw7L5fB{KYabjHQA`Lk@lmEevZJHvsEI+X6tqab$5B=Jwi{w74E}l%=Bq>B-?)Q zqI1%?5z0(?$^LXBQy8$JFt?J_xzuxj`*lx`u9P%OI@XH5Vb!N@^5%!&8M`)#IebYG z^9R+##;XJe=4ZZi3ZgD*y+3(bvjWRW{{bi)WQmTdGL{;ZC{^m^#{c-wQ~xWWkDrp6B$7l%7iezx39#|8;BYBv;eZn%pDY$Vq0sPx+IX`43QnKxQ@8G6Qy zfbX{eZHME3SyC&Uv0jEp$~GNO2N|p97}<|pFS4N3lbxW&nOh=ZXS;6h>0c3t_r&bw^I4P<;Mtc2+8J)^*Gu) zLCzbZLBUS|1*$4}s#6i6SOheUhAZ1Y9*X7|;;8dUZ1F@8M+wB_VdUGUPeLJk`RLS;*J~PUgXoe8^PmD>HWPe3B`_2)stCs&jpQojsQDsg#O@=ZdOp z6rtztuk|O!BE_Kt*TM8pO&>A7n|ksBNy;UdYsM!w(qWu=Ls-lHhLex zZh{yMq)R7ewtL}&M0*vsf^k+2_Al6Gsy8pSf9`U*g^}U;udRKs;CMS zs=Q-~3M?P9+;e5@h+nhrv~0=oL}#o?UqsNveIeP!98Bd)kru?yPX>NOKOp^2jSZ#0 zIqq#L(h=>-cUQ4r4O{hY7fts}W6^Rc_+I}7^4?vsd-nKTAV?w3`_@k_F?W=PNrL*637TE`+L0Gx;FN;mW*N z#$2Cx&a=Fp4KkUB1;u?@&wsca&LiC9UHAaGYQKLoDSUZ(vL0^tqFfL>c34&07=B!H zn>FD5eslXgmFYC~?rDspVN%hZx?T}MDWBPeYwG2vW!oMd<8wHkHK(fUWqQ=0vvG0L^ekaE5-N? z|GkmMO|}I^iV;MPtG9nh)m2Ud%5N!&KM*CL$FHF6HdK(Hr_$dB%T9U87+Mz?mjUei z$xB%RYhEe0%;~TvqeD=F+_Rq(C8(Lo)$d^-9ik=YmLV!Ol=1Fb1PR zNed6>P@j4Q9=t!}fpq8nQMr&YQeVJ`&4+=tlF0Yl4WSmBn+m#Juot?Kt-;xA^LT^b zh@QU`jaO`VVeK*e{SmWY(+cc5T;^9KFNG7JxDztvpUIqTrSr2iz=`e79!JAw0pY5AC{b$((QfB> z&|y?nrrEeG5JS(T)I=bCeb2ngx9VIa2ZSnnATNP2w98%iqp;3++z_v1kTn zWlK673o`%a=f!!lz!rB{$Y&8Sg?Nd|>V`^JzGwtN^UJHbF}~XgTJ7VLwjJi2LBS`}hPhq98!w8&btM5g}p(G_IzA{H4sVI(4uksTy>k?(Mt;;rjP~vH$`C-jo8N^SnOFkuJ23HV>*9h-9Df2bWHrpM+5s?(!2l zip?SUI1-m2D;TI@ap_+u3(RifwFV-7^&R81q<4u}3s(zx9tqUr-=ACIrXu3SaJTE`xRp zzK~{>E~chtmm_-)wE;cm7m+`7&eW)4D#6VTZ#->AnwpIf{PADoL;}Ykp(`X~c05V> z%r;a;Mlb(f6hU-BIFFyDTy4|RfzW4%O~?`#E#aC7$f<{9@9t~taF9r@g|3m# zbg!f5Jrp`vq-X8ct=o|uUhUyHevY+=vNZJ+9y{-w;9iCW#&|@OfOt-QL<_5!sZ?Gb zsKN|!MD$D>;Do|=6eNT8B!IyTC*`43e{g9Pe;<7UNmY9$UPlSX5<3(ZgOjby zmlQypBz>_;kgL$gaBCk2Nl~vK^H5n^jZx%!GazNtO2IiR(?yW%&ih>@o5E|#O;P4# z0J*Jh6S{}V=s8NG)jdi0ROdM)9_nZewZm5ELLtn0uZ01F@~oE@QI9NPn8{%l2Y#?2 zp*}ezc63&_W?DbTUdn0CI@p1T0XoS4;>TCDsC1r8{e z7^v*w#!2gAaZ$W$@y;Qq!9{n6qyCNx?ot;vCV|VFke80TWlQ_l-`8z9;&?Rn{qXoW zUQrpze90==s-gP&2ZjIqzgZ0Fi8u=>$has%A}Kp9_@1jPZg)v3X&;YyyN=D>y77zA ziOTt`xR6lsM3lyhjj}#<5RTiP;&sajnBd=w_w80E^5GkNb%pk%;qKuS*>5TAj>j)L zSurU0@ga%!7!=pXxhaiL_tMnb3k41Y~^aLqcC`1Zzp{0hk? zerv7KI3sr(qvjve>RKi7bZW8|1zq7hM(T(#h&&*fBPuV!V~FcFuvqME$)+&ko27-4wj2-JkBh=*huK|CJ~QY+JLA&o)MrN= z!0925Wl}zMc?*t(P;`tziv{=diqkTMN5>90>zkX+=}!0z6l=vlShAut8#EC?WyO(| zjaSyC2NFGnN%07ZJn2Lw^%1?j*Ut8~fUY@yN78C+L2zG?nG2a;O$57+7|)Hs8gw=! zM_bjkxP?0n9|~-ntZ~+90_N^&Wj@~gSb)4J98!*de%TcUN=bZg2r}NE-C}{wr`6-RnQ3 zm;VaUC`s^a#oFb$RFnioPM5L>PESS1=9->RZyoFE8#x1v*e9o&0M=Ud$~Y^(nM;y@ z2g1p;6!Zf$+DJLoW>{+!OfV9(6w&2*Bxn8+Jp*3Ue-#my7B%t@GpPJ#u?6eQFe=0b zyNzhRdDE};`{f^OHly5`$7s89rnR)_++89t6O)Clw*JQ~Z+Tcp)WN$u(MSNGZ3 zruasQ*=CD$SV~|5y8g)WaZalXW86Qtf%B$Dwwr$lvf&u!C0BqUJVv}!GiJd{NbxTW z3Rxpa>&MBdZnSz@7YyM;AhGZNGQ6{x|L;>5*}}m)gmj|i-k(9__Z?zBUalfD#$@O$ zO=-Ya$tL-NYvwJnq_{pn0_bFu8!}PrRmn2tdLebL33%+vP(S|{;6#K#!%9J)d^kgs zcnr@QuGI*mkLmEEKoQu_j*!Ge-)}Y zgW`eoaks~i*j^6vY!RO_JOMz8LOWhp#Xa>JKOT@23qMJ|bzJrLZwY<0XU(mAN~=vsKa4B+G5(V9_BxFQ)>?Vk6nR@{(qkH7oa@{ z=D*1n(!tN2NQdQMwA~!1DY2yu=r%0Cq7|*sEZYEsqnxrj^z-|j@wJOwht$AsPEMH0 znpz+tjkN~$mWn(TOg&9SX^cNre&YJkVwP1TN&~pQQO9@9xi7cc&`pX6RL>-TAo0hb z6U9prKD8a5^g9Uip?{r7;Eof#sLw*Ad|G^LA!WWp6NH2GA9#?};dMnG*#4D-=09fO zzaRHF9eQ7gFTDeZyEJQ)V$i&)3+(W}FWk^rFY_@n50#UZIr)W0@;7-6vHrtpqS?{!;5TW^hCxFP@aB zM4q4h@Hw|~)S2qASeXTZA$XCWEDRH>0AE(5YIUDJ~8WID(ykqSxP(-#Vp*9oLVTvgd`Rpn?HLQ1t01; zonqG4rw}V6Vg5IFkwX4!+typDcVZ9Psm4|(oS|KKv=1h?~dI6^9-B?Hv)gQKbaRT+7QGPXybSg7C*c1k|4Q(HMVGwWY5$D$atXGaQBkX^6wmdpN&hH>;efiEHicGtEgD#E0>=)2=FwVL0=RWgo z$#kR+!k>*(O`0rYA324XL(n9*snK%1#*lC0c`jQcqVx$6-Av-Kk{~>6KoaVl$22vC zVAy33D9BYRX`-1@M-K=6n-De*F>>}--EO6gueG390Z62cdXH4bhfAsKks&=j1od7yqzxhmYuu)2 zAtK47D&?mODWpe-W-xl6s5tGsT{_5BN86}D9~!r}6shT7=D9EG0R*ea3E%oSN6r^J zpA>#BP%TCOSt^i|uZ&>qd5;}+X1=goyY?3@Df$6u<>foIu_ve+$im;A(bME@$xZA` zAeVy~jV|qu0)cYTG_|SYF`-2FMItm6*9M&b@8#_w7tKHjf==PWd>$4>-(b|=fwUlg zM_=RnN&8@ZRC@ilxudf!?Gg_Xx z03?$OX3~mvEPeTx`U%A#VmN3~z*Cs2B&kOjUr5}Mtn5KQQ=nZ{#88SHKl4#^7foh4 z(r1Sqn@!2@$ZXD%OM!d$RHlEeCik+|RZ>Hjh$o#~BaL2T9T2g?Mh$J&<(G#kzVv8F za%3iKec9{wHZQtWnm3%Xov=?25!L^MTwT9AJKym>6-TrueT~b-P7;BjcHGMJ5RtFQ zp5d;JN5CcC{_1?|!fmCZ(yoU>?b#zqil@h3R3uchT2_DTu*NDiZBV5Ourb&%0rS@2 z^5ie_Rle>^tX{LQb?m^&6BG+gW18#L#z&uX-Q%W}$CZ?YG!)tHP&U#B+7oS`QC^r4 zWFAj8xLu4&HhE>I4KkWClPnkW1prVqNZ|1(D1BK(O? z_<*zvvs@EZF3oT04#q}orb@H~jkW+T`!F>Ss>Yj4>aD;r>s!8}W0{85O^PfvjUof(x_P7uh%!Dxt;`PmF>CF-KApkT4 zJmLBQPuhZ{Qm``8Z~wV8~gA7r#iu?-tsI9kcmKG zrXX=ZK? zgqVo7Qrv=7_%+UeiYhdA?sE%-=Pph^lfowGQw@RnW9_w^bzG8JXa8(=*o4-Blmuk279lln+L7WHfIL;AylONp&`2 zi0E--6ciW>YHHpaLW&oiUPsi^4Ur)vF;?SF0pSP#{tv~imtSCvvSj5_l~Ow%Ro$ei z*26`4?x6yXj{?Wg;L8CJ%N!)ibZ-dDe5Tmuw-1YZYCI7CIO-cc3FM*u?!S2G5c~uH zSeX0k-${w1%CP2=h)vIxTaVuC=GN1>)g}?wC4seaj&S*ZgEv)Z1SXWG-oC%B-Dd>i z^o$IuNZ{L(`Qa;slN7yre(L&<>Vl_)6nM`$EB)9CS>lVMM+I#;HJdX1vI#d@qKai* zqS`W?7TqV-2fme#BS5$}*Vb+?E_ZQE`DBYJ(eBoHbKAp5t?T-g65t8X$0lveC|*{Q z^eegtvgCI(YRf@}h?bW78cPw(m&tPjy;B_}wFR)HUTZF#GG-!r$uvBg@bj$a#nYSp zg)9ZhXO(!_O+F%K*2eKP-m}#k(8Jq)VGiYXPEXvuowe|@s zE!FdeOxx=eYB?RN;tUVVZP*O=MPIiO_XYE}<)JZ7z3nVpAAON}=<^1oOgs7o!W|$% z;bSx{_ZhOEw*2b?ROWm&_+fPMQIOEfMPSaaY(?pHCzBL+l^a8?IMKP2!NjrkbG|s9 zIiUJBA?0aOqE=n?hq%GhK~P zm0g~E*r?MU*lD5v-tVry*lrJf;S}$ONq#f=*IWT}Kl z|KwrPev3Aen$K5phiouOk39c9#&)2)vpXWiDW5v`> z0ISF<#u1A?*O|t(Qtp1PIJ%G~HCGianuil0cDeM5oTv&TB|j}S4Y;mpa~hKFW}>G% zV5XP*#cZuc#$}E1C$(YGA~hL9q1AMv$mqgoP+c{Rfy8`GbnQ*4$ky^{JYJG4T*Lq| z{!gURVox3x#!t(}+T5((;)I&!WmqAeH*wU$in0dSQZvHo`eav_u%=G>wPZJo!49$0 z`r3;K^>$b#Q!Vla0VO4V0aGTPt`Ksu@)OA2MYpmoNJCN9hBhzBur_W1PoKMHLO2qC z*WA*AnvLzpBR|iZ4S`T;% z>ETyi%ot`B;pf&d*`k&Wo2Q2)=ep;Toz&iF3z;khK7!akJ^N@oooPf3+llwAogH$w z5%BM<+>XetzUo)sxFO_vK;3Nk#_Dlu4+N^kkjVVDGf%>_<-9C3{Tn(`Or_K9%D9gF z6l1UPp@W=DOIF}@+fq$||<3)%XDg1|5|!T zAZnBmR}7n^R7REgC9l!_b~!k4RWLZ=S~FMQrUm4L@xVDK5>A8N_C~FxO_=%sqi6eL z44we}jvzBtoxLpWYM%}C{?J&?ne(pZYWEZ=+pz@*fYs|cQ`=R39X8~1UYjFpzeGB4 z*|=&|P?V6kC*TE*2qTIJ2+H+NM!I&Z!0I$Q`#Mpz<~+Fxo&@y zDbW^il-@xXG}fgvdAUeRcFtI3q(CAj2@y`_8rx!0lCNBRVh_Y{ygZFg^;FJ*5QMmo9CTMUxr4!GRPQMr z2Z$flv9}yhMzs<94e$r&(jt|B9f=aVD(P)_G-KTVEGkeYZc46QA+3* zMZ&Ioszd4k1c11;^+WtXv1NHa&dtj)oKv07LYxb%a z_hjXBFsf-1({pMf0njpOe19({!k-S<=1p^OFE!A4NlhX56hg1(-B*)Png4RV;BGC2 zGqnBh1`|Dd9dSRLkiH72E1A z=5h4^b#g!2Cp%&?fEEgW-ehbbjTh$0}k{;C7rpk*o|Kw`^MX< z>eBmfBExSl+9Fv?9om9WcImBgr;h&$JxWy z%d9dg!ct3f_%-tLzuc0jLE#Mj`>c_u{rWZAyB=sbS^Qyaqs;O~a_*+jkOB3j4yh%E zJ2-NZKNUy_UGCuHwe?FERGYqi{r3!J;%Jn_w>6Aae1H4P&l7>&oI$|?GjI4gn7@Tn z=8sp}c3aiA@&G?3TD{aEWJi=uki$Sk4mMvP!jo6MUgU`%Q$3J_!u9%7cFw${d(P~J2_h3yvD;x zq=N&00M!cHhtxjaU0}x+M`?C*^u@MX zB%})6Vq4ZK><;6}=Vim{yZEjy<7U3lrkp*t_YH&fU`2}sz3X(@j$L1fsR7_`KSnBHFs=1!-P6g zWdvHXH!d0$2y1XBZ3rqu`p4U3N2sazd)Ie8uW?S+l&ZUhqqgT0rw&|KR@bb#WUS6) zs6Uwe;{`?hI;Ib2CnHRYS3fD*j=s+$_$doC-{A{{>)|ernfQjy29I7sVdD4OL$RKr z)+Jj>2)=`O!mDTS^tz#hJKKj7c}>3kMf@&c&uJLp+pv;%6X_f7QA91KrM+ff*4PWD8~ z<19XJr{wGIhXT{QYop-=PEps!uQfbl?*}@)i}yU;(U2v*|HF>+iO;8hbD3(Fs?}b$ z%m5|kZ9~PR0DowMK`;_*4%5#H+G)n^tXcc}?H5bm@uYx>&ia%#a~09^)!O5&;6?T3 zt+06EAZISC>ADX!RGVM2Fc?-iT5aj8@&CyJ&~Y|S?rvoenz~;kG~Xt_KaTk9j+^TK ziAWpGOMprEL7ii^<#C%w;jr@$sXRX0?PlpAts$RR;`K}zXmgS41i1tGWgn_-_JEn; zEF&r`v1&Kv?odK|%vppb#{#)_!!xVs-4@xuq|sj1vtrAo#qx14yriw&;ghMJE4;TC zOhNw~W`6&h+51uUw`i9^!JZe&VyZ1HzALNtxh>=|T7=Cr!4U9N`1SA_&dg-5r;Lx$ zZ#NwN<2H_8^JCh;BuD(-Yy1+8tEq>-=S(}4*w&m!Ht#(F%E5%NchUVPE!QEebe=!5 z$23JC)7>NB8gvjtK~HvBc`qPWC!p~vge&GCrZo4j zrG+)Jq-hqpXjXc%Xv$!;Lh-~-T5=a=162tPLUL7#hM)QJSp-giACvh2vA0wJI^&Qe ziE>TlH!N~3t=lGu#=%8bjjb~+n9b>a-bGq~G zdra$&IH3dY^B*yCtJKly2oc<*tCw~7t4bh%Ba)KvB><2tejp;hGqkSD;r-5cQOWdU zt@GC`CJ(Y82r-msve`J&?FLNU*4mJnX6rLL{6ZvhYQc8i`sFJ-qTXiCLkb#kic%4% zt1F77!}m$f2Z$koC&0Lk+Ad%5nOb^)gpO{8%;1rFqb`%^{pCw%s(BQyh7kJ7iFNtr z5mO=*FPEz)HX4XE5cWH`TQJAZ$<$tXr2~nuj%Zlu!zMuN!!4c&JIUTF6yFq)NCyVR zdjZag%;DvKf1WwwFJ~W_Sr=dLPR2^l2i#qC2I87s3ullgQ+aNWNI>bm>6c5d`s=B@O%Hzt^C{R@{bspcp6Rr7ugchcdOKvd7X z%4h!NtsUWC*6H<=_WSeqayTT4^s3A$Lt?1sf}DR)7uUF}e4V_$+`FGzC_nS-&}G_F z%J?iv=i3&k7m?LID25=(is*k5fvKZ(dU|WSId?#5uT$tf!!%B`MHLoTfO)tO^Zf2~ z^3B?k!?$@0`hEKGXvrJ1{z@}X9IGE|bM&U)8SLbB=EY=+fdw-=)mA~g(g>X+=Q(Z2 z=odiA3d7AI=H_meV%1i)1>wQyF7pW8rrn zDYE!v${gt7RAz={36-zv*x1gN$U>`DO(ynI;yQ6I$iu5vAG&x#aVYJeFc(-(=2^h7L~{(6wXP8f)$5 z5mxzMrt7U~82E$B%Zp9Pg_5@NJRz65=|yXh96DT5N1bhofBQz$pc*^Nfu~bqZ(#M6 zJ);pFjbGAoAqHh^i_@a`7J2It-dk#+>o%ooZj5Xaa@8#_(ZXPX=)7-tkyswLVW$(@ zx10e~-AdSOQkO*Vs!%i)F#(2%*`f15_?nY|6CJmnx1^L2iGsX0@>`D~?(|e+ue*YU zV66NIDjnA7({3v=5_c)Tf4?465k7Ik`kGhRwX;?ICm#F9AVV%L+Ma1Wy`48KK=lq) z`i$R5bYjR?g4>o0zA_8wTI`Uc=}OqMnS_7wtV7as{%~Ju;WcU2oeqYrwI&L?ei?Z{ zhnOqrXv=;gcE#L@;SB5^^{2T`E~G;vZrsfb2? zXaRL~%|J1EC@r$MHFiV+#sUvcoiXQ_w6W*ljK6P3NMq}357}Z*3PjflI77QR@_j{> zf`bI90~Og`z0bW0d9{{mgT)H5a$*@d9;_dH2dy3_P7P1=E=-xZ3F{NE?Dms74{-@! zWJccDDBP(#1fq^}nLPIpM#co#xOVgMS)F>mIW-*^gPR~{r|@{;OAs~)X8q2%VWshL zy)aB!q3p8FE>^pFdN`#t=l)+sTahtOEZwp?4AGj@gx~dhBj(hiQj>pkP6PF$2S5Qk zikM&cDTJ5fD*jAj)mPaaXVyX(c>-Sy(`65QQAxCVE3cXxLU5ZocS2lu^sfBWCst^Gz7MKRAz z%bC-sr{$C&nT1uk*eOELR2bJZ&U>k+4DdjY?@DHRYg`lgy1v`2`pV{>y~uhC8Z}zoRT;T zQ68Y$*&gd?5HK<0E$o3v_#4Jt!qi=JCheM7A(CwS#darc?x@}PXYi9Pt^^njchD#c zmriYCcg7#vc}~%(N9+YM`p#qD4NPLiJ!f@ditiME`^-RZ3Gm$>`>)Rbnh1=CpOPlI z8cICozul&*&Klm=nAw9({YHusYz@Q7=S>jQ4gB*k7FvKBEso?o{LVnw?FA?$`pclw zvo>RYwJ`&AWzoi#7!G@(;2T#?=}PR{M~%Pufwi@*Iwkqp6&HI4U?YnbIG4!j?jYI> zbq_GDgk|)I`!?&BQB*hb3y(;`L z$!Q{c!;{|Hz35LWG24(!@KOiaBY=xPBTuKGiOw-+PWvHE&*ctzTMx}86>d*Vwv-_~ z`y%Vp(ISJL|1~L`u^bC^_B|hxg3jW1yoS)2Jkf;EmTh9E6(Qj8@`m3BlE)%b`Tpfz z&UF-oG}^t#X6FlUqJZIF-dHcsPhe zxRcT9eZ5TG>Y$uz1~)hF`=@viG&{v=I__(7CM`)ff6t2nW<^*PXmX^sYzXWH8{t}pJx}BO8%4Ib*A3f!VT|dcB&2SaLOGfalhe7P?Y0r^w?Cflq2InHayQ$UR7oC zaIS@p`2ZSiNWA;i^-GETP&hI(y^9A1wFtP-=6mwOK`1yba(%1p6!kB@NNP$<`xNz(`|1cX-_uj*jkN1e zHr)V~XlGX15NaxQ0}zAA^{^W#3#3Mx!3a^m@6Hdo784+am%@^y++sai|AeEEnMPNN zk(P@N99oJ)H=NIuC?ECc8U4!fHCM1#L7uT`x9BoY{EV_TAN-^f(ZOW)F3wv&7R6o? z7>MMKT0w0<2qtAyZdA#$LL(Qp4l}N_L*nt1uKd|{;JRLT zN`oy-s=ncuBRj7eHF5DZ=V??{hJ3i1_BhUcy()*P{6mluEMsPtq6SHGO*mQq^QCc<$oS5!pz|M#7og?{$zdO z-B%$gm*W9i;eKIg%hZ{{7Ollc`W_X387=Q}$CHbofF%EUB*=ZWBk+gfiv+gHUX~IC!kZFOcQ3+jL~P+w6R0E; zuUzI3oliGnz4hZ3?33xjPbIYWqtl?VIUyk4g!ppyNVq(xQjMq8ZUcq)#xU>H$(ol* z@a_D>nIchvPz0KBXJ*wfPk)wRWTof1DdGeMZk(GR+w-sM+ijJ;2bjhYtgiR2W=Dzj z_pjG@`aWU~_dJPbvv@MGQ-DAn^+ua#ZJIlDCmy6CCY3L^f{ben`ji^4yYRCMB<;Lp z50f^@C^h%u+Oi`5;a zdV~{r+qRegW5XJkG+Vbt_!ebW$CFV_`1BJeD#+to*e}1ZaT!7$qF)5AXZVM9F)LAG zkCUs;J&>YTjdS(RW(Gmn^C#fnG4vonhIoj)*3Y3a%nYYNQQ05K8|?%gsnm6+2oY z3y$RcNk#Hclz5Xisd^`~*E^#ao6}+i1~Eas(su*D5y@ul6n;nP@D59bIb)oX_ov8$ z@0#jWzV&v!;e?bG_G|I=CFJK92my6{bvEW8$Y`V)7;cEdOkV*JPa!awAV$2l9C>9v zE|4PDV0}J2xflF~kSGP9mmJ9OHiAe3%Lfyw-z1sy0v~8NlhSr`S9J{@CwQ>WSmNg2 zK|UbYaaiPz!p(`6^+{@Y8eI%$C_K(g49($)Lok>Eu{J)4cuC@60tD0<%}F?dn&@uh zB;u^Fs?Rrn+{u><zV#`}3kUVr)+q=tE9u%~(}vi@ZD8bV;Sunx#8y2&60} zJhe432TsmuBlp_AU$~|m=)(L?ah9?Wo=^4$E7kJ^E4^@`g;MCZnCUQNGiPrRmv)cI zaUqgah}0=Ft=o1?t>1S1Nu(urHXM(&ITMe?zQQGu?ao^`v%c8ahshI~Gd7e@dFjJT zpY1oQu(!33XNz+3KH|LizVAMN?DCT9Lx7aMvGXO4*?}(=VPpIQOqwMdsWZtNsnaD) zZF5v_LT}$qm(ZB<$n>ao;Y0WBx>C36HievTo_ZCgB6SMAjQS;}y^@!u%9hc|GirAG zIpIyTb72I-#CO>4u!;i*=)&`c?dA8^J}2u1HUCC*yqSa9{Y4_r{mXio=KFz5O9{gM zJWBVm_9(ty-!Amw^}*-I=qyGMPW*DJTi3X^y^rwWaZ9ia%{c#e_-tIQt>?N#?5AEz zPrnJW-&t&-gz2~+)p%ueT;YRWAMM8#+X7z6NYVW+vekwsrHr?(;)Im=%TXqM7G?Zo z50dHc?AGfpg?T^a9aK;tBt#o1f`KLly&yjY6}`A=bUxxAf$Plt8ii*zoA)V)vs ziO%9|uGgu6B8a7y*mjFI{$K{*pO*g9l-^VA(cXtNC7W+BC!g5j6MtJhuh*Gg^&C<8 zH^BJ059~vHo{sfoJx`=5Tx$QUmzLIZykb)jrLEF6P85l<|3sVx?sk zzyWUZ`y!KyK=vp6d(5&td+O&;KjP7y;UdwREuN zwBY2kBwQvkwL8_aycTt7^qK^}Q%79%W!XLll<-WhM z(smNc=57Swv+<=H_~}gtu1=y*wJAHFEVO)Uw}Tp9h~G6f93LpA%WTbo$t!jHuDFg^ zM0vzwVIuhC$^Gr9syRqgPk*gxy6+;qb^JL9Ta_htIO?bl7D~a5ah~^2+@d>tVIpR| z$0Dg&Z37sAy&ESBk9%JEh(-SHg!bGIy>;>Y-@XXo;#|W7$WGr?NLL4?lp4T)%ZBDM z=itQp^f}uj8jm%PI^7FOd%jesw!P|jKQtar=iNF`(lmzjq}+9pwLcs(&34~WGj6;x zlep`2w5SOwoh8kK6W0aKnx(|Id>Wia#Dg*XmHY`f3VM9@xa2_OI+tR2UAO0Zd0@MI z$JFH(<*K|E>NwzR;JaefbxobHX8cG+%6S6s1)E-u_%cg4L&C4}k)(NO^u>Vx6Jr?s z+6{Xk7>~h3gmWFKjPrNSR=1QMPj+gLcZ3yq8_TiZj;E|ZJ{g1bMG(i}*};h~A<~AW z!(*X>^FEg0#zm;p%O^3DfmEHQLCKjm!zim_P_C;qP?%wgl(7nvi6h`dR)n8 zBa-`hdg!ukVdWpM9y^OV?Gv@}i&tEB8BRDs%9$j!$pld2?AFVF_o6 z#K?rd&I`ZAG+)<}b>Hv7E`82Lyf{?Ghf0K0{br}J_ySohaozFLmd*vwk}*uDo((CC zk+|`58GU1k1fOoRvs|9xX1wmX<);-^xDA>8GB`5_dfsnd&oCkNI^F1NyFWlhi*i*B z4d*ojg<_Q~Y}lRq62&GH65mI>y3wJXAHAXNjpi&)cLonr;GbZ|BOF)8oOB`+!k{f6 zT}`F_AEhz?e^?S6IX*2#!MgZ!w0KtrD6)*M%+COpu*owHbh8}?=#c|kORLG-+1Q3_ zkjg9I?DWWZW(q5Thz&JgD*GQP1mxG}$T_N-WMREm!x-x(Of$&FCbSTu99j z7ThFpOT>w7P?>E?O_+>OBhyr&$v~L-`%2LOQOP`!nT!?~f1%yBR~%xomQY+Wi1(|#W^uCC*jTtqfyJ!vciIt(?8CWbq!!PNyHG_BSc z<8YiyMqpF)*IBks*IkFflZJ{@8RdJkX57HbWF&ZyD9gVkNM(e1w3-yiHGpbnatb1` z;jkz_Z-XtEf#kT&!m_g!`+0E`<3kiLr_?a$!##m@1}d+~QCl(esLl68094=tvH18> zh*oo`L628d?ve&7cvF+l3lVq3c!DBEI=>z_0qXf#*ZJN= zmq8=w>{DuU+!u#hL>iQt{yfG>6%?iaDdf=%Sgs!3i0m-?mN*phgjXWI!ZfG4h>1n7 zlh#BhoFLRvTXO7)?Wuy1cJM3tK_N!(a1uiB8m)d^?Z>2QA8wv#V`Nu6w$F2tXBXZm!HRSjUiZkZ`R6Ruz8l+g=Y}Lf*qGu_1cvnTKj+dGaE-$P?VD-o7@|j z6bJes2_~aGVdqncHKkI!RLu3QCc0q7U+k|H2Ft3YC4jO>A?H2qi|6s?Al^s_b=zbt zbQ@+RMeS&4W~>gq%D=0g+VHUAEM8SwcF(m6CWzxzb^#oBtgX03j(atK$TfQl)p+mxnXx@+s%9mrULs@D+j0KWH zrqpHsoExp9g@7du4cX;lZB>UXr>iMwVhK0YD)LTLL=Ook6VOfx=O&|lLupBQ?WY|c zTax}ALT>FO1Bi%tZ?7s-F#wgP&a(kk#OVthsoUxd<`<{QP1eic_R=)cXDk>}xa2)n zqJwAjlcYP3jdu4m!Q^8z-D8CeE62~`Tchso-s%M>y#N5C48n;}x%f(s< zMlLFTcJqGy-Z#5MpNTT?als*HoGTc1!+P_7TaHPrvT756U}I`f<= zl)qFSP0fm1sO74>1Ufo~OS5_w8$B8uhFUejn-V&d%1XUfi?g!W5##8oly*uP>m0Wc z9CFJwHilTCD-FWJmj8mA;1=L#m#jTq%A%VzuKa5s6V!TTP!(-CMOAS^)}HMP8uhC2 zkOJg-ogsgt`K;9m1&DmlR@BnMd6i4F14fu!ksCq*)znUw3bEE(?947Qw!?!5={EsD z)&eRx1`SlL)m4*L+g8yk5Dx;1Pu{qj2c(poSgoUSb9>^d8q+o>6wOo@x8F4DeNOD` zYpLKmY7*n)fo|)%bPF5i)B5eO%ao_~Lw5&XoXOV936>A9|mxe>ZVSe{f-Y`Hq~5|Dq#?K_RU|=uMLC4wQVA_}%&)RCxJZYDRIqQ9I5Jc)VzOfB z5L0bD3IC`YZOs%iUAO>KVY^I|0Ti7d(Jl0TxqnZYhE4ri)IzVJ$pFQZRu|PF1jbqi zVoVW5>?lN_9VI?b!IV^+^dRE&R)?(28owRp!o)UtRFU*~(x|vvM9QSAopH_NIbn3SAawuO2? z1r2ryy$~ERKC^M45tC6=dz>^0Hw=j}&(* zXW8OR_3R6(&XTTrN4U#iVm>72I)b z6w{1b7zsKV>XJr9>J%v2WK1Lt5o>T)d0k&p=m71rdJOvi*=9h(W?o&L{do)i6G(B1 z3M`nPFDRPaHz|A<{y$m(awAAB4P{ZeF=Ez5Ij&XlOjXVlCMk6nm;4_P9VOp!Fe_9< z=gYAzCEG~rg7>4*Tk^^(EA*h1g``#>OjMbgg6WNLl?+jtWTV2V%MiuNU~yGn27f~$ zK%k&4G@W0_=`>AEOO-Uyhz-?^P2MPCge&eMp|Mm+IzPVxIa2dDLr;bUndKKdhMo!&{Wm$aIdtB6c9ZlN7|YaGj*yFB zRdlGyhBCxed7%GT9}x>osO+0zP@Lgtdqzo}UUG`VsicUX8yuNe>p|l)p2G=^-dgc4 z67GXD%Fc~P=KUMc4Ww#aN^$&@2Gs~hKM>0-Zo-`+#Qu)P1^&B-{I~1|JSpaXLIXf` zL<%aoS}OE{eSB6?{3)=Xh$S2wbG>pv^_i>BKj9Wex zoL#EK3X`LyRh7Dlst+V6?w}823Vkt5tTQyy3ar~5s)7rEsvlGLj@Q#rhOx-pHB?Nc zgJd#aRcBKpp`8|GviYu=(ij^qJ|vw%^v@CF*~Cmu$w4eyk&ZH{Wr&pUe~bG+h7B_F zqGBmpn}ARyIR-c}?jns1`1}Xb zaD+wm?XstACCP!}%4aY#jJdCRMy3FiZ5>^IL9=OEN#Dgh7>q>Kir)aYnWjVw2d{EC z2-kLs`b1Z{$bioK%fIvfw+c%&vBJimQjmj8$RdcSUnbm{|Iyyuokp(3pwDGS(1 z0yK%pUN~vAC0fI|NwfV^oMpk&AmDgg4DSzVg!1L2QBej-v6+oW-Q8LX^+Nj27ss>lPt!|> zB1F}JryLA887Z)<<@=hkK&?_IsYAgA%e|3&p;J_Y5=YS-K*7Y2Omhhh2;L7;P1;^= z4Zl2cJ$nlJ8Ul5q3JvcxpI8oi!8L>4ZO?6Fbyr(t8PSnRLreP)fr0d^AP6=v(DS!b zj1;H{(wd-))Y-jzB_t#qZ?v-##vZx5^O09$kwEr;kK5ZbTB0#(i!Q zDXF&R!+%ZhF4tqHrs&YQ9Zh2!jaF;nSN(_V>=}cJk*^yq2fd`FTjrZ}6l51H1~C73 zp3nSjrBvl@d@v{@byaD3=BuUzb8I1R=c=I8Bb;AE8ct3J3c?w2hMgn??nQlWG9q29 zsNt~5bJ!Z*?Z`Xx7zr*2sU-;)&V^KP`mJ`~qEm>`%X#DJ55rwPa7!?}!6s2PrGy)( z|9@rtf9&u>YfbJy^MaKcfJ-PYF1E;!NM+E|?x0S<6jDM@NK8CgYo@JKuP$h6N;*G3 zPq9n+@BF|9!|UueabND%V`ua_nq8b9y!;eI#GM*a?fAXwot~9jl5I$6J8jWgWdHN> z@vGu?G2rGwe_f!^JB(^+P9ZW(ex5ZPfwMB$HNa9#HVSl0g8@axx?oU_rLi9H*l-Q2 z8$dxbkVXxN1KQysrI7z}#fX&=RYTW-cU2vEaU~9^(@QTQF|wQDIm>>?ND+6Ep2!gt zer4hvY2YxhRf!O5ut&a1Nn)-+1scWI1^#K7SN3{1Uj^|205DHa&nnG&k^ki@@N?m+ zj8iKX78V`QcS~608uU#ZOpaNu*A!>|FGdv+-UKy?i$`Sw=4a|44Gj&`T3{?SG40sE zN?QQDPl$4oKC<&5s6fIx{j-Fs%qoj@oh1XrG4ZkE$5?bwT1o+7m~kn9h?A3(45w%U7=V%zEkaDY1LvRT z1DQ-@tryT-ncnmh61&}Wt#yks{)?gsL!g(nbTn6LfnvZ^UYx=X^xdH%h_n=_7%Cj` zAy@Pyo=Nnhhs#1L|Dt`RPT!{Nl<;19se9?LKe@ti@u(3+O@il2hJBxpYdFvG z5@Zt;fti?SQPI*5u*7w8P@k`ub0x@S%;refs$VJc{WvPf!5vbm#9T$O~_W=vrE zisUr(?s@$%jsRtXJOZ=M4oU)pk(%v6~+x-K&ROXW1&QjjF&g_s6=l6?RIus zILStCli`0U#x+AW>GvkK8JBu0^A>1Q1sRS)Qy4+-)9kckLoBAhZmi5;Vo_s78|r`1 zVULh@i@;KHio(0wzXbdh9^|GBmb;ih5a8M$>Lma6?Nu79Rct=G$w+wKWIeUj@nleN zKbFDi`S_PJlRx(^+X#|z1*1A&qrmY`;K2B#OI$ze|20mElrUhu%{P{ua* zd2~PgXMnx#B9_8N$IjZU=i654lgAtfDRnlE_lTOV^NY|tnjE(s-03@o>)Bdz=Vh03 z>H2@jiquk%ErhE4l#tX6S>}hKwx&p<_?XxGR~Z<#;u=<`<(q1Mo51fOf+Ch`FklNC z^`Azt<{~Nkalc(S31h>)Gd*<@SsMubtDgg^)3ucv{>v28Vgo-(BvzY=$--4rcT0ct zZn?mIQGtIu$kgI_r`Keygw)$k3@TGYLW-E>PBTGEm8fl z-PV_$-3kHY4gYyVYVzh>$tWugkei+vx(VHi6@lSFcQF~R6|a_#JNJJ2p5UJWs?#pQ zuLy2=+SR|8GCnO@jRpt7Whg{L_Q@?L+U^EGT=4r?m6USlxQ(z6o47&dJ~1 ze!-KGS&M5Z^}o62i)nk&k69TEdj|y?cy_&iSEcn z;8humQy$K^3ZJh!OPO(6_T6PUvt)@S*Y#J}6mmG?rxWfT$0y?(X)ka{*>S0D+2#CV zXRlBx|84nS$vUXwNPY_G43Y0jwG4#^bZ8I(4p|8h^lPH-RnZ6rQE$(htm_{g*$Vf`t^0*J&lEZ1Y|cP+dh#`%gA~!H+*_Z9 z^lAg5`d9L*;O)HLYg-R@r^~T!_v?T+I!`y!W&f1XAxitB>lWMsFA@b^H!|n1+cW!U z(1|6%t1M^iIX)-Gerwo5z>Bf#o6nsF*mq-jBoxi*R7QbGJOf4}8RQM_FWm1fI$ZMJ zu@P9^;;Fm_dutyp?trE{$C+L;fiQtLz>$mVTKyq61K%$9tJ^H_?bRSG!7};gmREuK zqMJxlh5Y&t!rt*MrPIiDmR=8vPN66ZKHTp8(tRZdKxpJML>sGEY(7a$P~~RN_>D)c zy4iQpf7`2@I+$$N%<3eEr1)|7JeMa^E0?~&>3D3-L#Z*i-Js@2AS?F57L*zHx9!{! z8KP-Z+vdInJ21(tXQYn~e>bvil|Y9Zq^^%QJfCM7X7EMH)LlvE!}ig7gUn;Brtt@Z zt^JTspMj0$WKAnL0;G&FTmP%k6f8=o!K^QBNAb?#U-eV`JP1ntEw*QP@9k#v+@aku ze(ueTG8C1r`%Oirx86R61!a8>>EY+-y=sSWPM1=}4gI2JIA{`{^C;hq{%PN!HaxFj zuy=rDR`25tNzNUMa`+@A<#gsY?4zB%mb2MvU_=r$eQsK)s(2V)YfHk6( zXJe{S<3iHuJ5H^OB2~tndh1J5;l~KsY#(AA{~=#&-5DK5MheSw{tc(CfFfF^Diw^V8K3wg z^PEZ1F!Lqn+zgv{BN`RKN2fc4R;BkTTOSSc?%@`k*;_|TN(K)P7D_j#{xm-ijqE7| z#_s6}GRk>xdxr8=DTMLOHHH7c7qGIhp&_B5VAxwMY055BGS7pzguIT9iS8R`i;VKn z$083wVa?DB4CI9TMqFe=RE;DVZ6T`3xjjKxNnaD)TGRN@{ta8xt3ZSv(R|JuBZ!j* z6*EvUND9#yim0eTO`qNY#9Bz~5lH}IWa4?n+=b-9Iy6QjA+N9Fhw6Sl`;J>}M~+^6 z{$zs-bQOND>87T7T(2)^$Ct2;p8dRhuOsLmi-NiY;kK zIX)4uM8{HBoul!E7b79Z}QCE(}G}D=M;M zh~FrCj4a{{UTNp}-C2L#@&X(F^!uTJGb%WXCze54rhSdRyu}~1af9VJ3f>Nh*)wR& zxIN;FGwU#FxN0$iDUj*J9?N(!6mM=Ydi@5lYI#11D;9mW>p}F~Z$Ib=8lMxo`4d+I zsdbZ!*Y?bhC8_K24a>bj^QO1d({6&Gi0_7&66^e}dIf|!D#4*mk~~Nn^2?T%o_xrw zqe6qno?P#T|GvmeH#T=mCVDg3xqRLBeZcuPA$g4@c>-#+Ih0d*pfZ;SKb>wi|I@x$&N$$(`Ki6d1~i4&t)@%iR=J1*%tejS#e| zJ0J0z|6;{?6aPFc;T0Uleny`?ZnBQvi(;QRK zA-y|4h6t?sbVN|UjK8Z%#i}rOqtJMcGyqAdGW5gdS9Lyx2daD@5$!w+lX^yT2HD@8 z^BT|_b$D=TEcr+NE>9v-E|e3nS9 z2(G`t!c3yM<0@kmI6CS4|8`zjLf~G+_y<2KC@KxmdhD&fNWwRm9XeHI?PI4LHDFN- zyb%5-Fnb#+xS;61eNYtegqT+ZxyR=J?H*?U{iHWm`FPMBdn(d}ZufUQ`pq(iBbw{} zL%D${ON;Qkoh?~JbtfuiVa?Ra`^);sVoTGVs^W$94`1$<#iQHZ+v3r$$Ho7yzz^yL zz3%_C18f8!r=YH*`$A@<*l0@-)$5s_>Ibs9!7=ClikD35^gBG)Jm=E5OUqv7_elHa zcCG=3QB$C_k3PLY;GE_T1wki-_fj8ezTcJ{5kC!g2TSC?|mt?=rJ9)_+N`?z9(P5`}Y)C?JV;7TwsyZ=Xr z+DL4z?Ed<1SXq2fDQl0NY}n`Zfime1)II!C!m-{i=W(j%$>}mp$gklF=`zYqt-WbJ zFPA!qSo%(Bf4$nJEJ2hL6xv^M)oa#m4VL&gD^mUv-zu;rcEn0_5IE>O_hT#DC1tud z(ca8{cO{YMH*ZOQNv*!IdL9zi<|h*3xN?B--6WFZfh*C?%iDh2Gg#1St*->|!;;VC zz=Nl4@6mq6w4&8gp;SIQ+>XZUXL0NYva{dX#5e(Oo@C4Qq?6#u7wGu2iq6^;XQo63 zgVEj`pA_qvSH=)y#DiPgmB^BPcHQO-@N@y^lxzP&NYX#mIaCdk7Ad(K5*H3V15Hc z)IPe#SCvHwW_t_v=Z9qM(MnpfP}Fy0F3Wyf|4&auT_JW=*-w8?P2m>%Zz7owMWa0w z7<}p69$I3KA>iKi#IO(~A7e+p22?fwS`|^pw`97gVBdDZwI-7vKF_n=eb7xmMz>kl zW!x6LrKIwMw}ginzF&3c9<9za=6boOG`i2~3;!mr`Y%Q2Q1l`V*6km+U9dJ0+|BYD<4<%OiycDsd=Px;`AobRmS@W(DO2OLFqP3|0iqW8m zueC_x@k**-2x>}-=t52j*#)B2bfXt4brSd7R&1EF=7&wIbh?e%6JH5960v;hJ>I3i zXLjo4V%ffbwHh~3ZGlyTeIZaIzjmnlx`g5i$wtnk&3FK5*Ums1R%AlKjhM8<6YXj4 zxEUenFexc$Ecgd;hMo<2Xd2zFYW(yQgtba`AeFSD ziAvIS3xqs10=wq6VuBa@K>rE7);K2Pxz{s`yxpNd{Z-jOVNn=wP$s7U0C<`bXksEY z)#CJBqJ5T!M7FZ^4*JliEyEa5FuQ(JI8N0H)V@X1i@^02>Q@(@=YEOKGtceU5BJ-r zAAMW++Z>0}FKI7NXxyf8dM@!-p_tDA(FL-SkmDHt#Fr*fuAc9hjJ_XViY2o|LOVig zd#U?5K9obb0dJWajf>C+tTnsmICGP0(t1>ESO7d~eSL@%NfxS>lL?lRSQEvmFgm?< zv@0s2$yTc&B7vkLvpqxaczn?`zM-+gZ+`jbL-5KK>Nno)KS}l*EpVuR3r!47dU%|l z*gTmEZ2P@qC~WgYwSrz5_pjkf3T`0f@RGg5TPO{|h81vo=L^HR@|~U7gF9t1zOjru z>+e-?XHYyhn;M(8X{oHxbx-1yf6|BhP#-y6O>zcIJxTPJ@mtb0a=p} z@^mf=x#%p!qN~0T`BI5^grD55w&=kZIU}_%de?d-)Y_t9%_(Gf+_nNW7TCtDb)zLJ zDk@)uKr~n_3Wd0zRDcjM46&0893+84Dh9wEi-GA{4im^f%}` ziTjLxt~Gh)F3^wlP_FB?uXY!eNd5W&OZdk1%M%M(&Aqor2=!P(L*F!Fe1Q-$M`7L~*cagK)8vEpk2h-E(j*m=fXTl}(Z- z9DEYOc|ux=N%D7YX?Ht9D$BmM5OHbr8xylZoh;VTePvXh z)CthAd^Bjm#=-s~I!aIaq2!JZm?LNTNMi2#xaRBG@}e_m^XY$!1jO+95v#-c{0NO1 zn7rI=FngrVrbi+B^{WOUq`UY>s5I{=UXz+c1FOd=6&;e#U_pX0m(!q8OZrzwdAs#? z8-OVOGnzc9|G-UF!w#&S&kK}3zChAC%NHCE?ZMu6G(sN}4unJo5T`&5m^(H)k$Jpb z%6)E}JBgnQ{Gz6cj$!g{w-HY8V=Fx-k;hHlYpxEm<-lD@`&&zoBle@)enWb&7;6F;SS-V_;KZnJ}h2?|v$DdL9(2&7G>@Nqkt3#)7R9yFTG59G8xdOi9S7VvBNqI-6Qmfci6VP^7Vryn*7KdvM&qb4&y?wYWT^xp67}avqt}=F9zt*I9LyL9YOl6StB6Z|3t*|3qRG z);|EmSjMAFB&^1r_mH+%!pT1Z;GH=F^+Z};As2XjL*^z4={R1dahO;zpE&0#&rR18 zTzg1=)OyAF9p8!TUFT1j4FUnI=tx|^z)UrJ+)6B+D4DhYs>Cfk*VcS8dgAbJ))JNu ze*>qN2QtnYpv||*s(fKu?Lo@1A1luyxgEP59b(HQ!>v+XaU7 zK-kslHtEOU^eoutF;hX1l!cKA=&W@2{#6QheCKk9;SSotLNdF=VXryKW3&T z@IPyW&WfYLfzw+iG-)sI1ab`TFjGLkT4Xq}h5lkww9KFqm;G!Mlkkj?Rl97$!HG}p zOZxz>Ob0ytsIWY*x+#Dfmin=>q6sETfo9kGI;&CkZO?&=iVhzgQASg>N!g|48zh7< ztB455PIufsG2tKsQ(RUJ-$}q}08-5^(iL(@r5Q^jK^4K|Gha<*p8Zbp0Y77k?Mq;1 zuNt3spd*wl)tlzC!AE6O$Krl*!1^KDQh4zu@F(h0`n-P7^)wU6=3=L3T-fz(RBhST z&ZfDun4`~_uYG<@Hky{QFfW6v(UwEf^}mQ+XdipOyQ$N^2F*(WwXj4Ve)MRz_>L_! z(&G5OXGc%Eu-siq-b`*Xvb>Lq)DY#41+yaUZbEPG_k>~0@L3^KQ!pm4Pddt{OU7Vl>qqsq5n@E|;P%%(j| z;QN6ZH;tqKwY3riFX1ni9x|9Whm-cqSFxzLCeXg4xqafPIYYlR3G(W0Gmkus9+Wv! zJFX!T$Z`i2c%s`7wr@6T&&sc*xcAkZ&5}^=bqvR+3 zos-kJ^>^gFnYb{%@oCL`hYvXP>iR!gfL7w5do^i~&(PjmyTfeRPERUKp>q9^tW<-8 zeWKRArKzHa*#QIq^y8ccUuvpAx%r=@@#qUY(b*K#hnRaj4kq6h7gRK~bPikG)Qz_4!t) zj)qgCNzDb>{2Y}=iNdTh8FCaI<2tn&@qK_~3I_@|@JIS@(3nI6_{kT@4W?QV2!5zylO!$@Q z#C^D8BjA6lrLTqn-)jq2eGP%Y3 zNUxw@T-e z6C1Itv$TKqORtW$mTQmhe6Y5Axqi||j5-evd6?j8-CX17J})}tJnaxl=sY%wqz{QQ z7@o>-vRpirI_Uc`wKQMd3_a?H>bm1XdDl9)(FTjMWPs|nk&)_dZtVX+6*1S&d9_MI zM@L3xO3R0sZOHxkS;ml{{k2E&4wS^T+ zqRHz)CBi`@@oJ`-IGdDS>5-9eET8_rP|+Rb zv`FGBa)C(ISEATLkvNF&R684)40(QQVO<_q8$vx|K|g~x;#UC8zGn;M&Y)K>M(<|M zx&&YSeKe51lX4g{6)GbH9FV#LzcCmH#F}4bF5CFOI!;IVmwqI3C*Ad?=}7PjE!Lsu zj2nzF{HZ^=YVaB}z-HRB-|#kuU^Vj{rj6~24GgBY1(YHP1m?@bx>({r)_OB_-S7}~ z-$UN=5wACJy{*kfOGobSSbqRlqgbEr4kVx8`;Lj5x1}2B0)gyzS=x{VM~O|JGknZo7^m*5Cdzc}Z6i0by&!*1rp&X4`W07-_tT=D*9 zOB`!!r}pcV#BtX4)$0S=t`E@EoM_%m=NiE<&8x46J#Q2WpNwGvNcead>YDP{Gb_KB zJ^XjIt&oUfMIok}GsxEge-%z+^%9uR?F2IkMloynw=vDO-wn1Hh=zcv+XKHKiH3W9 zCNixvCa5#O+n<~XyHDiKpTitBA}E+K>H>~({RzQhN5Awr&kD{aYv=>@ z^$C%={PKIJE}uAYge3oxm@~O84Zd0MMlMKlWpr=>zP-FPoda44GJvZz#^!mjZ@H8>{n2pF}01{=YWAfh zYj6p$pz9Bgul{-PQ`R%j5;p^KULZ0t{%l)SJ4!)m4!ZU1hT1~N(HLH>X9+Kaot-^x z+RVz@x?cN{E2CNl4Q}$(fs2}9Yb1d@ilMFGQUdRj=wtE#?BR z(wsJcY#dFD-s+Oq!>I;NZ#Sd+x6f|-3FvnKQgJC@!T@7Xe;;M8&p^_6B5%DLxZk@c zvuIeXmB#AuH!O$WNybWQLpGmO-FIl;frVXQVyKVa=OpZbj6j~5nRkc}jW6XG2F~jTjNhL|X*Zn1(O^RoknHy|((5<)-sMNci&wAf zpFe93O4@zfFN!m@pNZTr%|(MOpeN(OReXR1~}|y10Q8pR|CD-+??A(?)yZ4 z>KhxF&pjh*Vzp;dTlI#cpVmDNQc#*&y+_qlhGV0!oQ~rY9l0LnE6P*;ei+RD85W3T zeyw3%dFYt$4m0rA7un6r|Gm5{F1Oua`pv%k9nr0mpmlwk+dNm?Ef^d2d-EEVc6+q2 zXy`jIpgkh=g~w03Aufo5V2PP_13E@5rqQb($Z9Vt* z_vdTPQKzR?8+M%O>FKZg4wvp?|M3Nb>sop1*h^vTkK6C^%Ft*bYO3o;8L%sNRlv`e#Mv(dUK$NtfkW4ZRZmJ2zr%dx$;5Yf zX+nXIeEv~9DPxfpx6t5s2#G3N*WHELiyMRpYh6I_+p7Zat!$~RZJ>yNFCq!)@bBgt zj|BX^%H6b8lb^q#2VGf(`Qt0jM6I;S%v{O)!|qxgEN6Q36!ZVZ+FwRx!98!lsFKp4 zbazN2Al+Tk9TL*rAc%CgbhmVaba!_*lG5F0>+}1cb3VKuU%%nH*S+?=_rx_b*USVu zxCXraVcOALOd>-goMe9{PnppWJ(lkNea@J7K-GZ|g(zcksOy)MgyPWGl6*5LQgrA} zYNs*$m=)9!>7E^f<*Gngy_U|e$G*SL=J{4s!!9kK;@vDpjB+VH&&^aG?V{Ya$*PK_ zr3e0gH6vK8GgkhaMy5qS2$gI}aD5dg?eYUHZEQ8z>icmuaR3@CRa{loIFu!M_QMGp zLxRPLs4YNBsBhefMMY_I&XL7HD>75c$gA6@ZsLuYB%fH{tWO}?K9X;+ zEmNqKj8dj>WceBLcD|L|J3m{*j}UoA+JfX*)Q<`|Es26@?jP`bo) zGGg#CxU1(gbdTo_JEIT5rRJ15pEy$9A%_T?6H+$xz2lIRJ$f>3uqU%`6niEp;#IcX zv%i(TJ=kAzEyp&vm)qQ2dKv6bd|KkHev$vV)S3)^!eC$+2OCw9k7 z)*NrZjp^na6QRoSWTb#jKphP9moLWl7qTBeGhu6{tr5V!txb?0MV%P`tK)(;1@sE` zF8Z>uv9X1vrC?CB(x%h*KkbSqVzZLy-B8)~g}5R;qrtPZH#ll(Se`QQcz7~DW}#t} zBxC%>c7+vw*ggq?r#I3-ZbO8DL_$KEKe|?(TZ=;#`kPY1=g?-rVn~qQjz@p9D41{* zK6Q8nn&k|uYmf9y8I0i8n=eL}jvkXfm=I_dr8Y+!ZKw2BP(sQ@k_fo*GSaw1SB!xm${*R91R9WUnf)%Z#il$=>G@IV`^elCT%lxOF+@--W z;o;%nIVxhA>}_)fj5-gmApFu|wQwOI)+p=4A;fvB=$M(xDuoZ~iv>T$Ep^GY#Y#oK z)_(Q+Y~nK$lu+aS)2r^D9{&V{8BTJsgDdwiF~bBbGRQj;g72uL(4eLv@a`??hY!`j zO^_dVK?u@1Y6!B>(uS%Ot1>(PnLm|aGu3HdjQu&2;(AzG&(#D^Ahee{SS6#NfW|pu zujB+{0VR3#r0K3tCc{``EKjQic4=I}KCZmS zGkLDBR1_4@d1(UEdJ`MQ&Iie+&QiJJ(NADn-5$sWV+w40n7E5J5%#`DUn%{_e1r2d z6pA{_Qw;+>?~9G&pPz=>-D!jHLjVSliB}?1jX3ZTEOJnuDD{goo%^m`zPzw2-rK(6LShC_L$q0$7>y?>Ssu?5TE0*EY7u6nr?mla^ z5^Pih$?Pp(l8*ao*!TmC?`z2zuoM+#Zs0SMAAg{BYz%qU)P84Y?$|Cu!r^7$#Y;OEQyQC#ti3A;VB_gK2f$=Wm>R}!Ok+IM;cg&%fLdxjz}(&4cUBZfZ#W#-dZ>Q4sptDgFb}InrV+{Gmnd|tzBP%Pbysj?t_Yq?<1_tE%C0P{}3>aVU?QNsM!9n$tbLjfd z*}ZWv0j&4AmvA>IqSx#JBl~>&w)Wr34aDnfs5$>DYzm-~0{%efFL)?fiL&Ga-s?U_%~JxU z2=3tCfB;B^0;c5rO6wIacFSdnuGgz|nGVt9aG^ay)@!Zk=;)3(%=dE{SRWig9X{^K zeRUV#Ws}CWWi_8Cl0@NcwjUuI9hBfnTt8Hr@Vbq~)Q;?M;+bY>gIB}0!Tt3So@La~ zo+ArPPV^^!_YAVg^u;oIx`!tH zTO?xqbjzt27}C49e|$zZ#PH2B>@52@J$E)0y=hZ(XEPzT( zTa04uV#P6+Jz{J)o+)_i!B}h z-@c=mXL(_vS#=3`6=r6%;u$H>X+jwb2cE~!kPNs>)8qhf3o%K2qw z5dcJiQgZ*;7z#HRH)J%?Uu|#1N=nk$poYkPqTW0HKjiBqA(UxY4AlJ+u!XCONm2i> zO(09sN2x1J$*a)gLw}&-Gxw(R+(1o&=cf<(seOpWBr5`$0?$|B>8od$HXY*l(zaH0|Nebki5kasjHX#<1=B#mewzFpyL9> zrjj~30(%JloJdFv$ic>P1+m@TBH-9RfBpnF0N=C{(PIEtPR7ZZ1ehvdr`ylsV&VAT z71-F>^sguP9bw3Lx<6MhRA`o(CSziX5Fo>Xh!!A0fG5HK0mqXycX;-oCFUIDC1G zZ3MRe3xbGoQ%1xL4MC~Shg|ZnQ3X2C@wwfgk{FCdl$6ZP%Tv(Rl`}J=v93?~^^4<= zGdGB~T6819gR`?^YQ%SCDN#t8s|1Ax2fv1Z^uZ8)-M)L_536Xs{0l8%$bV`-H!I6K zF%f%mYO3pw^y!1QX41^mEtBNrEvv2#G;q>G8XpVdvO#Pa+(>AB~Lj$85 z{4aM{Fuqv}PIwG*Dqk?fa`wN^s{Iu((oPB*;8h?5S&geX>2CeIc{9cW>4xSl*)t32 zNhRNcNe4zoMt-6LR!b0V;E%#rMqj@8FZE@m-jHA!8JQ>SplJiV7m{6J*oXWI7Y|QT zMuwb>3^I10JDy&vleCara-%~zi!1OATr`P-V)+GV&TBEZlI zqICSW->`20kNbbWvA=)Vz-hw1H-h_3#@ZVBJ`^5T6}c}=m7MiNB7`3`&3t){bBlle zz9SbaYp#3_&!*<|;bn>T8LS2;Cu~XC-&9B(T$rs>ketvzbo3oaD!bL9pDxj!H9VXf zWh_k*(&VFqm>7(E3$U#Vbyj#WtyZwaQoLNae=5}r5{&K~W zj;A{~wfY-dXI=y&ZsFp3kS_l4S^bVm4(;~n7(GvzjSBNc#j$9?Gw49ur)6>&_J^kR@oR1c-c9F43`Tte7vsYqB z!-_Kt$o?IYRFdTm`e}f1Z|#3K1~dtkLGqt;5R;GqXFPs*MGw`# z`Wj^X^=xf!4gAvE&YCH+&DOwo6ifwod=AnCsTlA3dw10`ZJ=g|*7aY@573fmW&YLQ ziA^JHrQzMVbTp%;S;M(p=#j3RHqb15R(<6Px(sUdtNVgZqyC<3kHr-iuduAhV5yFN zdZuAwI-N!;A6ER!F%nROuQG_t*VuHbwBujDwf%pe3D_#2I_MZ>7^WohVy%IuvYE>n zH7Y2;=1rK2fRLn7+u50&f`S5MCuJy=nxH_1V{*e*QM)-Du%Vvr2>zLvNJ=KtJW8G@ z(V!TgbRiJmv&rUUHzTIWeey71p{#CjgL7A+Qu`#Dy)R=vvg5`}XkJ8HIIkimq*#SY z@z?!~@85dz6&1hNsCPz!gh8U@<9ogTDv*F%zC^1!M8UAPzNtCijFok|I>!~S$vG}M zWe!F3?4%FZncnX|@Z6F%J*^zipcArUje-zb+CZ`N7LrZ2>+s5eCF$DYp^ctdxg5nY z`p$z70925U0?#cT7akfaTrgD$>>CUUVe`^a4dT#y7<#zB3jOp;n*r34;DI8DPke!v zbSXv9h>?_#5mI@i=F?-E;4n26#E^!Uiox|Gq^|w>Q4mYe{F>y4k}mxFXifR}ALQdT z0+VhUF%l9o`5tOGPRE*mm?I$d?Ec9Q>FI+fNRK6X`vyi$eFUD~L14V*nqNhOPW2rA zzWUpd`Jl@YISI2H|7^)2iS|FK+bhKm@2!ZMPq!^C^|y)pk_4oMRkq8#w(jdv47q=X z+T#7PDuJ>}57r+Wh`-prjfW5gfV`#^!rB(#$_@?>gTuq~fM|<){+D?}btI40*A~`q z>#+&VY?}NMc-eGlZr9tF^YdF2IjeFMD}>+#dChm0;39u0x^d<4Hy&cmjnmS?@Xgcf zZt$`wp7FW1cC2idaH1-<-MyR6b9K!Ue}OGAM>|yKKvQsN=$gZZu8g}eCUEr|$OEfF zx&I4vRz-w_AXHRT`m4g|hV(l=0WqURv@i-%j_jKPAKW>-xbv8vnVFcM$HpY{kV1!hW*nL5(4E446hDm^wlV!zP$iz%6W)ZRUcl7?2ynxZ4 z_~3iw8QdbST_*6}cy+lBTdWko9gdU@ljsNMmJDGD+8kivD z58WR{-9Qy+@CG2qHKI3H$IeY!eo{wo0I&zqyK9TCM{B$%;Rmo|9bX-0d_vayb=FrH?A3@|F*LV0y#wMRT(ectMbXsHxukj4e-Oi#ea)OLE0K9 zGDyj+c=UxpZ^gR;87E=}ms!Vpki0M}+f4CSPyi<1+}{g9eS^#c=z?EcR#mz8iBauK z>z|!N|0h%7>U8m#Ze(O>F(Vh^V%bys*)Ys=Gc#F5=07aX$3RKrXJKK-*4dxB<;|W@ z$xolC0O}3x@#!%D7+9_#=hv^qoJhXpF}{benPRn^BB{XXzt989hZhZ}lkHN6cn{gp zOpxXDHLI*>jFW`-e8$F|Vs{fcG!%VJkcYodnQ#p<&L|0vv##HTw-hRRatf5mU;KtC zJN2`us9QzHH~Y5dYDNyAMdZNP`CBL>jb*bN_}Y*>o3YQ{N6KYfG> zA}kCE5KxJB1Iv4n!}N1;u>dC|H1m{(73);FUB0=(%F5`XqW9pDmRe-iT0INP%li(m zK>6hU8?7H?XvaeT#_4xr?JQC4YCR_6Ib71NASCQ%m{U zQw3`eYb>SXwv6g3|K=usxLucs&tMt3ObUBAz*)LU?BUXkEj9ks3rk+L|K~>jMnFKN zxm}LCt?L6tC5ivUSboyom#9=!xL&nV*VFq10woY1eQlwGf*QC6xsOlw_AH{JqUv;* z$||A?Kad@q{sqP3x)2+k7M&52BvSB=EIL|Ih!kj0LZb~)IdU*Ps+2?uVP)cP+^&&;2(J{+JCvc>v8`Di!E6D^DRQ5i8XDe1!RM8Oh z5ZZh$-}@$Dp+0~x_n2U_ozw6zFoZqtOrC!n=t4`xca7R6;~rE%1}(WDW8hASL&N-V z5~z2fSY?FyVf87VKg2@em9zn2S43kFtsKtx(k>BsIA1dVFd0a=9fqGJMKh1=7y5)L z4d3aVU}6H&+v7luXd)?pg)Vo4KD|$PZgq$emUoQ%BXTD`bGhA60?)%Zf=6RBeg=J@ z@K{@P${0b!g#eO)x%0`;(8%xShvOwH9Pj^944E$LAo^0L+h96wLKs_I#=*&MP^TGU zsjaQ0KbK&)$?*5<1Iz~C8Mv(r7K6zpu@Pb9`0kAO9#4fm_%gt40YM(8`Tu#sp{ZJE z$u~g?P=4ZUbRz6=N;BW*MB>S#av7*ZA#R7fb`M1l6_oBj7|4^drwMsFak7b#alr{~ zJ!?9UT(Wk`!pb6KG_|XRM-di^#$k|XK@)%x^w3W*=nyricx0me{T5|zUG-Fz+-P;R zPqVDyVI_PeS}N1B(31Y={x%=lW_<)7A~g8Z2af_N#tldINiNM~8um%@|MbbSmU;Se zXWT-(tZ)4~{ozfX!lP`d(WUwou^0L2BDw;lA?PdNLBEqT`nQm^ws1V%#oqo#j{oh9VKSXw~IHHO2n-T9wRM33d&j%J`wae9JRn2>hY(% zw75dP)QxR>Q56tG!E&A@j%TBSO^+=dMmWD+EO+0DmwAsp@_k;M`+mf zR)@oa#2CrEZ6yu=_$b%Z;UvCQJ`aaX;MJ ze{lTxVAJIPuKLnd+RJFR_=M@C{%AUg()QsAuRNY;nuI&fjS*8uD61sBWUBp{zsx{A^zl5Bh3iFSLSoxDSd1(9OscZaZ%v+$ z-bM_we?*8lF7UWxDWYCKdHEG3Z0PIICl~U5W~Xw*(r6#SK8_oK^2bIxn9HL z?QE&m#xa&jB+3xR0++VEOo&q8~s$)=>;br1CVBm{F% zV5jH|og)}Cd2UcV+Sq%1IMd3qTs7pm&Wqc!ZbG<`c46i>Zx-yVz60V`AQdOqqL2t1Mbo zZF3A)8nUQDXO@oeg$JMB?w@JhsHmv#GxGG)t*AJ*XDv3|ENJ9`RG$0^tL*$@+_Am^LNFDD5{qWIt$p; z4lqo$N8t*gTGu`9YaDudt1~$>{lTXG6Ss@qSf6`D z_s8E}6-KhWWDL#GIS-m=jTHA>MvnI#GObLpN}GS=b-;Fp=AM`If$TLMakTRiE-Ia& zQYvEufllm%u!!@$5nAq~o6|NoH&2E>U5^iBo8}s4`*<=xGxb8oo9qp|XxL@WPrSUAsCV&C8=dYysuM=={U7 zoht;3ZryN0%1g6SZ~GSG#?A%}wnZqI_OL*=q!`J?N8%CwnAG>i>yGGAl|;ezmKeW7 zD~QAj-|$@lv&)1rPwZB31ZoM{Rc^uUsS(aVQ^c;Wb!)E}J0s{=cO*5O^6{zO{ye^M zSFx@Zul62MVyR8PUP2q;}GCt zMiyd)tFp7R4V|+wK)PkV#_XM)o!!@A+N9* z)H&WH3{m(=3KmR(7*aMAwIq2AGAg&CV!Sq*DL-;E^R@J?T~ED71w%StL&Ns&muOQs zM^c#UPmv@0-2fREZZ6jNShb3X5%Gp-7enR7{E@$XEOw$}-0vgN2xzZ@p`zI@>^}TH z&H5Hm@2n>z&Jrh4=vyDf(V~qXH0i7CuHyi5)4w$ebj=Do>_Ne$REZ8niycHRc`sS1 z9!8JTGcJ}-9IYCZSpiT8-4I`A*Gxu%F-K{lLk6myc2Oz!R&9yV({ zW^?;3mGh=$slxSBRl3u>xPB!(#1&(iMAgA4b;ddQ*65pxkiJtMt2F)^vg)ct!<}f2 zVPOES;0u&A7Y#OVa#kVQoS+Qh7NuY7cC2z`klyNZ_|a;P1fr@mQHmVMPHc>@A8wsl z-UeyVUWI{a`3dzHz(p85se&AXMJr-i```3iL z2qV>na17S(e+3n+HvaTnGKXv{_QSPA?@md+{&Ye-^4jT(iYr?MYg@F0{n8C)ZS>%| zSdeTVPAe4o@R_8#hMZ(L(RDR34mN%rqt}Vfdl}&Z(*KEVu%Afp^V35tki+xeLtP50 z&Qu}+-Giv#+S+>7#lQFV_Ci)*$@&SES})cF%ciiumeA%W95}fhh`+8j3itaH*1RB$v%a=KJvu z-kf@+&R-VKYb!;CR)UB<+75cOf&q0f2fJN-WK=U~fMj60CZmcsj(iJOYH*o4sZuaQ ze_a4NL}Ff1sj_UoeN>a*ZMMul#MeXy{={-xoBj_Nl_VE%;cqSkBo;q9OKPdHEL+<< zZ9EgKZ);CQQF9pHDEQzU>60t=bRAbRzi-qZR1$B{r#nuXVdRP+J0;}Ur_d&~kK$3a z*xTWP(#i0;QKUD6!)K1@C9$We*FlD5xIHRDi;hZPYLFy7?#CRBz=t>$ ztf8QbW8=kOt=pr(liT~^{pIIpt+3WxJ?*bDm}Tsd^qJEYo3V4QhsFH!+Nz-jRRUz> z?Dq==w6a9va#hH+MKNL+!%ZRc+sqR$nLLXjnM)*p0@sR0pRcBpe|xN^{Dyk{f1Vnx zg>QV0$)NiUNcK|G(sqI147#uXT2%{q0Q8*UInSvgOpK48Qr!?IgCZ5E@}psry{@XR zzEO|N%_R>LBXigtrQqWONpvkIkN*O+BoNk$Vyg+7OsY&5VT$ZTO)Fza#&osVE7CTs zk`mcAnPVkd+Q+2x|F~>^{_%XxhVXprk5nigwbHQ^?PHBlia-!tQG+&1rc%@I7t?_& zEvw+>8!L(|DcF}?WbPy>i{_<3|B{;Wa{cz+qTpgZ ztMcLar)7xAGO~A0I$Qb52;>*6)6Q`jx(Czv5O^iLKd+*ajROh-`CRw~Cx_z8+ddbo zj?6AGt`8YI@IP3?il-$J4)4>}W(!+1wV;^2lI*iRrRW`{DNVVKQ|h%j(kn!RAL_tN$`3Jp#2u#lwJZ|`+}%=(ntEUyMCB#e-g zC+n-gvB5)yRpExmHT~=pOg4Ps?}ZbrGW=iO`;rwPGe}tEZc|pV1{^0YonlTh?}`sq zcQvz6JZ)=hJ9TWbONJ72Efmbm(WP4TVzNUcBP%&u6T27D(b0ldR`gYLhL!4|%(qlp z{2xDIG#eB>7}R9vnQ25MJG}FU5MnxxhipyU#-W1pB~3Mz2qjD7JZyBZ$j;0v_t@i= zRx+b<=14YjCJM4~gdVcUCrRHZ6Bk4`QDw<0#{Eo9FSBLDhL~zLydn!wt)+d{L z?~-OX@rOj0Y?^68UIj@K`S+{vVGF8S+fH~WFHBjL9Q*9rH*f@;>sh5vBFz>f`SrD{ zEk%ZS8B5j6d^R6D(Iij5;UR|-kjMwzlsCb}ARQ(?=x_!rS#Jk)#hOkcdRnGGy&Cg+ zqDYZz{K4n>V6bZvU*hzErxWGqDn#Pu%KfwJ%_y^#L-r&&F8OkXEJDFdjU;sguZ@a+Rh>YQUl+6UWJ$Su5chYb?^M% z`pnE<=HMW2%U^pR{`gCwgC+O7UxX`H6u~84Z0K1rDZ5IMjP=F*J+)Q~hSwwcD2KHN z@pe(4bX9N#F%GVE?(vq5Hwj53`km73Qzhg{l=ijSO%$IUb=M}_j;ET)5!yRjiFMki zqzvZ)beVqRPYX<129zd3eCIcyIcxv-<%!{|5z&boJmP&0n3M9TQ8d${fJT&@$J&*` z*%s9QsG)+Nx{O%Ru(0|TYb`(yOP_>A(b1CL(jr$Kj^4lT+K*=?12oJVe=x320@WDy zp`S$|fG2{oSoAju%A(JNo!vR{pK-vjYtTjqsaR`(c*cK2OR|oW3&j2{U8kr_O$!i8 zvW-JC!g7yj(XH+OoVI!n|EKbMjfK3}h7V%zux*_Tv@UVnJ9tl zh>oS}`TCs_sgSXC(rBEC42lQCuVgb6e8E)Bkt!73otemQMxk0=sa#=(42>ZUy_Cmi zJ49je1bi?XPw<4Ho)`gK2rd)a@zeRXe$dH}?v!x`TEM0c)7&Ar zZUsM7W;l=<-p|XM4=Nx~GMk^{grz>vfkg>)WlB^rc(fa12njYv+*cWBYXmwt^5ex=Qmn!_iC)~9DG^cbApQ<3q}2fIPJy>NOYNo2;M{RW1U)Xb5aw||Ioq1P7 zoQ5-=5Ok;Bh!x;5FD-G^!A^YSN7`Q^O8ihCwFN_EaBZE}iC4Kc>so?dY1$zrhW`<& zPj?21yzh-1_w)cS5%1s!W-MFfR*bpia`j_tchvKe|Nqc<^7`oY!2i1)N5^cPBnx)` zm-uyCxK5Nt-|pwH#f_Yf{7JsdswkfsGiziv=YXfiDU2 zp@3i#+B0eJ&4J`CghB3%c@Lm`-K?G5AO!v^05tOamj<%U+}rdqQu@}tt}O5CE#XUW zWv^8D25sZJn#;|ejC_kfM#|xt*>_8FO6G*-&Tp+&PSC9+&E%! zVwUgBH8eOIs5siB6;#^@rS(y!Yc=3Ir|bH+7e$a~wT?Oz=- z4b-tpTmK?f#*ytas_H$Lu?g}6pCwt|V6PQd>R_&@;oxow!r2xlZWm9Z^*!mAo-dxt z>>khX<$R9(vs6So)59xk&pFFc(S12_Tk{0!*LOCo`Kj&QHSuX^@sC|NU$oj|#^Sed zSt8y&douRU>1}n@XM9eFHPXTq*ZvsK+=aYx|3JQ)d`9Syt}6ifSuUmhOIPm{)ftm@ zv))cAj>GLS8M_A>Vdax{jDXxL$xE zv6f{h6;a9@agRBRnB>e?yN@Z1na^=3ZfC6x)UROh;M~Z9Ctk;-Z6s(9Y*Xoe8I8SR z)~cFvIS3CU!X#=T)OoLD-GFyjtCsY1?(4BVhnC`k3ff`d`iyJKyC`unhAT}|TJ?9- zntsl`(m}-W&A06Vt%dTbh(TyE@T=i6>imOe4Hba$t%Vqkr)hpjJTi#&Ch61`vW`dv90m~?mY z^;hMz;6urWUk<%9h#_Y$#YJ?*5H2iNM|I|*1ie?Eo^f`4$Cxf=>rX|UHhv8eZp{u9 z%2uJ(dl5I7T^JjWxEIbIeS27v-doI*4u6o8Es8-$i=NFbDGAg2cISV3YI}XWqDAId zLi@d>SP@jP)YaA3MaJ)yCD@I?fD>Ip8XB6bR!<=zq0*rPcv5;$wiFQ)GpJv( z<4RLjR-S%n5D^g(@-~*1QmS$bfk$!cHa+WPOtL*7AH;Jrz3q7XH{7ml&lSeTBI+kLbWSTTp+it`a8_=vAI)xtctR%ATjKZclT&AC`~UD)OvpvNr(GGETL{HwGq&8~S=Z-y z_9OdhUB6WNUJ?z>3sH2)KfC8IDK13XSSI4RFP-hG#tWWx$y;U+HDN;?8FH zPkFO}^7{Ef0m26Gu0IkI4C|L5^;CmQJ-vv*Eto*xD>zc`b}LdcGG;``z%K6-CS#jW@Br||nk-~w(lask%6^u_aLPoN$mW=) zixnz?Fy78Sb;EK_ z$bxOJW4jqP_RN~_=^NeZ-T|cN1IG`iFHdXY(U=`2^m?e=rFG~aC2)DG8Usj+EBz|3s_^<0g~7~!|ujllLBK$G+fob%dq28?9g z@_NB~7p^q#8;D15QyCeV79wF05%OuMv3O-XH8V?+uOh?NEToUJSP6uE;4HVESDc>{3K9<*)&7%`lJ&5P#aj2 zG~->}_E=j|!&+g-XrGH%EHnv^C|iAhqIo1W(CFfG)a`QUH+1uEfgj_8set2^o(+-P znpYPiIZI6r?dk0^EX%bDdc4(U6Fp4yQ{&|5apzJ!S#!f#>D$x);{yD^aC;|Fge(RD zy$K$xI=J?3+~GhY?2srFDfZW8;z8gQf8d8s=r5u_+>nM3esjHbk2$!kH1otc+iFBp zDITBVSbvlbW#g~DpML#PT))74yN9{459GWsIk%ao5GJ=Haaty{{R} z;TM#wqMje#J~oWVr)gI2t*;WE#G%8$ZnGpH^nfheW@HvOd~hI4Chw_5Am7<4Uv)p` z1Raxa_IBY0QBJNg@`fnwwMqJL<_{(wJj{#Hul(=guef{j=AXC$!(=W0MsGJQ#5=d| zN7Zrc++Anv@P5HH`LGF<+wub9!=9OL;+YGNNko>KPizbuci}6<3~!tyA+?WmuDeLB zVfd2nVQJY%U%lTTf{kQjf`NQ#74-M14MfiR9wvCu&r=;!uG(Eq#L7ii0 zZAfsKCZLPri3$pma6Va-1khleIBz=l-#hI~Nl5`r&$d|FsLjxopOd+o2v^36z#2yvDnH z=Bp9~$BMbs!viC3z0v2GwBM+4EI|VwpW-ETx*j?8mMgX0GJLnycF!gpKHQbR{$(I} z%=Y7w8qZjrakR-A{!8`h)?FZf2kd%SzGuC6_k0KIQFr7$Z=22q%1-k$`PwFN*%*!^ zUmX$eSWD=Vfv)~&!OPA-WQzb4!F{?+eP!R|L__(L)9GQY3TzZ5`^(Al(<;iLQ=yRJ z;`@RrN%!5dk`WD2Wo2}52UteGmB;RDt<5DrR+^j*lAp({Gsc#R)-t+&pSjvL zZ2^EpN=hnjG?vcmc)ZM6v4)O`8PKIrP4L|Do)GY24-ld8G;z%KXG`(EUT;qnDp3Ki zaOjarhD7=rFtdOcv*my&cbO~G0et`lhK48)cA^02so3JFsYnk-zgZBNw634ZQ6oBc zJ~6t;&nPU~D&aJ1v1A9yFzh34$4ueA?XFc4w1o_>>y6fhTyLDb2NlVC8!a6xojwoA zrY4jEnWk8!{G%7d+OCmlxHY)TgDQrI*GxxU`K>Z8Hs4?RQArTk(Emb z?an3dB9Ytgv zL=uD&hEUO%1ydR=HrrnGfXTQ0`!~VaRtWH~TqE&6$S|tB$!|Z(zl&9rkdZPw&I^*D zRj3x4gqnIa)y*Vx{o8K#kGNOyp1|QG47|0tWxxS3+tmDA_6$>zUIEg(ciime?CtDK zvMKM$^G2!xFY_cy!k}+^97od=9h;Bn`l$#Xx$yqRx4h9_0NHY>uGBk?0}U-YzMl#4 z{QO)(S{m~4A}>vj3UK!mhE#U7`=2!Eb?H@2ao)X~GPSTQA%6R~BlLc2vH2?NUUBb7q`RWS z4xzmvcPlaZJ@aH3cFX6X)4)Pwr`!V+ME4I1dc<_7QUJca~XCP)?@p49&Se#p_7%R7(fcS+ZW zMW1VhSM)SAG+O4=R8#LKDXK(9u2Hfcy`DC8VKI5ilms7cY$ zc~6>!^-*O}=xAn^a%$M%g`YpDK|VA;H{`RYAaz~VuQDTfbtx8`6*|HEp=fKV(H*;( zEWxVmiC7=iAd470D_FHEC#Q>6&R$=lx{gtHuPqoa&;KMOJ9mf=JeOO%%$dK% zmpSIvTG+R(a-TuTbpe}H4#9mKu}Uk7h-h+^&q-TbGXUNno3ZHPkuwkghz@f3tufv5 z1khx~wbV@XbL8;)XlZ{kPr9_va=5PK0rY*q4TslUrvTfJE(YXXx%%2<#k&65=65E# zM2JMnWVH!nJl%5$MJVPtBU$LJAM*2o_^_vn&-5yZ^~kl?O5Iby=G{_bGWzz(M&AtW znR|=l$!c}58VxB}y)O0lTvdaEQs68g*r4L)&p;;Nf`X7NCY?0jfPX$62QaNP{iA0l zF*7@RUGELx9=>usoTay2m_R1gVZZ`F8O+=gn8};kCud|V?=2|-U*^HLrl8>qT$r@} z(NvM9BC?owZcq)4bD*z6U$}nPeMZ0Tql)iNRLvJa=?LnVXmmc5i%f4pcI~YS3Sv$D z_rQ5YB_)V{PJjsh3hcBnOg0W@FHz;d;W3;{KEc)|F;h_`wB$cBg^gZE5y{EUeg&~n zsE-Zoup%(yzjT0w$P6z5@z1p7R?(C8LdM_6bg*RS=KXq=Wm3*J(t?hLhQ^Z)6k;;v zpEVBzOl5NU33Ua9j?px3z$FH4a17&{)8>`tBR>#Zm*F2i>k%iVrfP4I_k->%N;uN3 z%}v{;mG;&_af{h6*4EbHcRxS?&(;Y-JhTPzYKlCD+5$hIAK)HhD;HZeUG+R_Mhe=2V-j0%ztQ4ZJwub;6ca`}Bes5QsKN0{|`@XcYkk5^{2K z{5U3-_wPdh@7(=zPw)Yx7$NA@Wv6m~{tNufI9X2_oR%Bk>E40SU!BP114MJ$19M&cm>S()|o3rJv{?M8S_{rUv^GT za6|+p3rkFYzl6*bV$GMUI}h)1(7CNBFE0SzCHw)5`U4dBh_0{IAf#F(w($D+FFvwL zg^(;9pflXswK;D)pU}27#{zr5dBHhJtpsf2jD%52fc&$<(gs1jc#8qnTOcyQU)VYv zRSluvTDN3K*F+Fg8X7wx@tDsK0HBK!b>b5RoZ-Gd_Z|&bpV3 zeJb9ZG>5YY4hw^N{6LM=^~Nt!l~(<*5ioB7mz^(zQJ%(Zp~)P`$yP0?dF# z!eft$i}N4)4L@vMv)C-OvXOh$VwUX9ZmasnOoa!gKD5?H9JK2G45`^s1(KC;er^xI zVJY$3&meVBt2qTOO`=PkB)66~OTNl#Vqyl=eg=lkrZg0FABv6>o?>A?nXi>}adyXD7M9-DcksQ2N^!Zs z2Uqz*M|XhxRd4m+1m?{V7%;Io1JKn5avI}l89KeJZ8D{H54&gN=JVG9FUxclOwBBT8n(cIiTFlZm^vkU$s zIdaCB`1Phgo#rV;fh|BU=6_2J|iUXfF{(C?c9Wh~=PVhRgCz+2{X+BYo-2@N_@RjI_?vq|%RTz%Qh2ei zO}QeQ0=_by^aY4Nw?j5Dt``Q%Q4G9Jg!ZuOV|_+*eph`=Ajk3Q3tj7r0VX7B-!0B3 z=291qoz;v){zK6n&)RgMjL>V(KISirqWBrO5lETlAEkHs`ihBO+9Ozk zebt1tx8r1SQ~CF|zRq&q%llF|R#9Rz`HDRS>ql{hchBcoqlc5pn>uE+%71eSS?xsu zj!j#Dr%t=D5Q=hWS7*I4I9saq7ksZYI&rTbmDpAR3xFyG;0tsSjukuJXeJuGQ>}fi zk2CtEN~&2BM~a3!4l(+Eu+^ zH*bYHn>6+R;q5GgstWr6FCro>-65@XN~0iMN=i3~bT=2In@cw$B}mt$FWqqI?(Xi~ zgTC+1?w6hYu(Nl@864%FbDrlp|M>l@B&l^IMRzs-GJt&*hs9cmH}WoX((ZEWg6Fs1 zeX4uXLvhS;!a^tc2pYG?-SY}E*PyK~awEF}-;vaWnp2-^0#VpT@9E9!O@{2|IC-PS z$i+GW)_pSO1449ex`Q>=ql2Y;#6x|j78~Y<2NAcxL%rp_O_%QhzOhhk>BCZXJCnt> zEOJ6r!OeHf2c)9goiq)Crt`zDkAN%T-L~BPC6aZ-z?p}LBqz!EkLyPA?bVzY)}^p5 z;XNhs)iVjD4!(5gHuJ_|Ir_>}(B4pZKPEQMW?`8_q*YH`7`N5|wc@B&W#qS>644nx z6{(OQN&Ecy_$phWP~ebu1Mq8mI191S{ZeSlvQdWxP~j!>a$ZRgqTkLAo=?Z`gV_VoQ~UDg6!O0Mkd;gu{2A3lmqhCU3E%geXIo$ap} z?K@)}iCr+uXOMinFeeA6aQj2Gg_FjJ5J-*BwbEh|vj-ivu`p}04F)v(HSuBDfyFCf zR?HN>2cWs$@>%@jjt!Kpxbh$Gg=a-q2r+E$dFNljckjqo zc?^bZIpX0+tgu6k4n{a?%`Hv`iHci(|NPxey)o5S^AO9qw@&6{t~ad)c16yq6Sd-?cg^6^RnBG7Cfg{DH8X&U)$l!f=yu$t}Y+glUK znN}=G6u&@bMLL~M-ID23d{?r(dp4VK^~;nh^Q*BBIIZDfP9EU5zeP>lclPw4(-*g71BC7eTXgGOxi@ zHe>KTy#xIqds3vl#hwmV4%f5r$Z0+|WV9W!?>H43zUmEUg9a1$H?Iy8JAyZLdJzmq zf{(0L#&@lj?<+N34lXi3!FQ&^YF2wUgPXS14+zM7(R>>sfdqMc%Ind``8Hd}erwdj zsTi;Og4rkxqQRf&wOYrQ3Rgy_)XclM=*ng8+gjZ+*Bg_Kf z$?Kb30_ZlGOh=h9;+}!i2t0aev}`XZR!F_SbeH3kZ?5M6T}Hn~Ao=YoE;XkGRC2k1?4Y2D~wO5rUM$Fl5$m5JFc zjOj4p`nX$IL&K-T)fwcQ(0^(inB;|7Iua) z=`E?N*=S}++9eDaUMM-2_NDeP(PgtzGsHuhGzTw2GSAQh%jNo}ekAfYL%~+e!eO7a>?A%=0{QFGx5> zSbI8>Ko?Hfi!Hh6s_&llvc271(UV1|IDNo(RC)k3Er)tCFByv)V<=vaxRw7Toi_3DSn zgVSW_-TkWU!L=juAbO*eb5ma@8ZLF%q14zzSAeI~nT$^dgKE_7{>On;pqC)Fq24-d zufRL@H(&ZJZg41IqBU|I;7=A2vl&%XcMq(E)o8TjBuym@7CWoQ+;imIbC{RNe9Fe< zE5MBw7t)2Eqirk^6Q=8Rjavzrp1Gx{TYGkm^$%D6={i)mKa}2RYD{v%&b424^$dDO zFqM+1t#;g=iZRW;LpdUGMs*jeQrLYy!avY8VNCqnPGq?Ncg@uuyffQ7x%*BkDN(HT zV}jKR!)2-asNDkSTJznUAy1|`4}tqvflV3dms)*P_z|>&FVEz2zIUt(TyG-_70JA3=6a`nWZs! z6%vlWoG*b=p$AkJ>#e~Kj;>Lfo<1&96Z_ph*wOS-ebSKavrSpqm558S<&tG39^=n+ zzdXe5%12v%B2h`%Iq99b(j6@#>KfkkOAqpwk&~tx@4byM?cQ3*4hN=!Uka}!nbz4Y zPaV`)A{4SbFNmz?o15Pv++00 zyJw{7s!JOoL&s?HY%^^*NpsO92r(7k<|c~sS0n#jB&lW>F$LuaG6@iff}AoeXkri8 zUYcSV!jq5^*mhEwz&sLYYN@UqyF`T}oG&9S3SZ^Hjs{=fTF78g*T79y<;YIg4Y=y70F@9bUqB#tg8w1jw{?rgpO!}-#aeBP9D za*j35XPBFQQEJdBrl6ees?AkltiiA=&86XW%Y(%yC&Boz-`9nM({d`LKH0U-@Z``F zWOByY=D}amT3_Vfy~=AfS2S&ea-nuTWg>mw^#-Vwr#kBzbyj-G;O5)CB}Tia(e8D*`4B zjZDU7OU|u!B6l3!f%L|>8Zb=LnrvN73cdLwgoAl=4R*LZuGVDSj@Gs<=*Z{XF}SjL zYI-^qsPch2m=JrL*Na+8p191Hd9x)2+hzGY(sg6`J?pEi`{uW2M8hd#=}K3pmB{Hy z{0VgBp5AHQ7u%?NlM9dVsD~ZbYL5YFu!b&Svb)!mO1uvtnPIYWh9hYlL5cV5hy9*E zU<}@+E^ZO^(YA)8NEQC0fW7KUabT?Db*Vy@q6;?_WU-j^kr3&o?+s;9bek7}k5S`+ zQqKlV$RJd`JHDQIW%a)JeyLBF0A%GRI0GW1NJTLS-R_%el25<;j(eRX#>lYKy0#i& z+Lo75!D@=(X%Bt#-iw|}2}7V}#B+{o&d!Y};zx<=1Ni;aa508@&$5pxw@)G%v{Gze zH_ur5NHvOBmD#sg#Wf8`ch?t zlG~pzM0so9wpWYT*fYQ~-mri-9ohF_&77c)&ojfOcjAbe=~h3( zco@gMTUS#fS!!}xd8yMZ_}K9CLS?uy(PjN!w&CQ1b9m-uJ42FJGcf>1nI(!u%J=Am z?${&Iv4Nye+u)BahPWvUE>R$_3pQf9`*fZ0gk)83lXblSA0!^^+Yy>Q?Fpt?X|cxz zKlsFs!g~|qO5tGUVeZn}9-@rk)gFcGvkJQtz6=o)1Jmfh&ZFtPjAN-``I$&QALbUR z6)&x0Gl#f-WxJ5cxc-eQ_> zgE8fRh)Og7kv=0+Mxy%zKT&4BsR=h17tgJB)_Co1&+-AVQ{VDxGo!9HpF7=hpu*q2 zftQMjPQ~b9mS4Gwc{%#W-MJ32>4w#A<@GO=rj@p;oT=7T8-6(;Lc=hzrlvoWB^eaZ z$cu92X2>Sq|7?(7FzpV5Tz>tJpmryiR;qj@G(Zzayykel~~BT+XW-mJo-qu9%tcN%o5)<)!M8vZ;U5loGf9VUh!Tl7egN} zQG+fLJOeJT(C{PV4Mst_M*3DbqRdcm1qVvfW4q_wb@gn`YH-PK=^gRBEt5HtvVX~4 z1Af%pjrT7`4{uRem}k(u=e>S!)INEXPcYjEq&mxv0G1Q|E0wqYg9YH8&x&HzS)~7J z6-C$9XgHFgP2~$dDc>kIe2t08>jCihf)M11;(^RkipMAjWOl!1-qr|>z>k3u^;9p@ zh*+t5#8W$523D=CCiW2nSTmZ9eAMDCXO9uzW zzX>{^fqz@Q?XY53OTcdNVrqwwBZ)RZpFYf@s$i;W4ZG=mwB-I$r%F={E}%2u?-S$K zRSDj{5v>4%BQJK6G4iohl0&;z-_V)Hi2&pm{2Eh0)2=<}3o@-}6hJ6}$ObS#aPyS( zCSZg+F>b9`vsE*rgsPA#BN82Lb#bqP*>xc)!;|-4DOlwz>93nf!4o_7 zQ33eIp`}IDcdh#oD9WD9mDZ;2%Vi{Dkyp z$PmDpEIhF<9EOyD@be270Wtxy$O0PCu%{aWz7g-QJhAlUg6~;6Z)=E zW>^@br#yLXr^}-9o2R>^9QpAZI4aqW0M1Z>E^5A0c~S{}#Y^9%s%(pd%{8mo|g&Z>R8U$_|Owe66vb8)a)75L2VAg%sO*zBsX`*{WfN)q9O_00r0D= zWRZvv{^Ht%H^5@*rRk#=04cpdjDGS}1`s=-uDpWuggoc?dG0E=)Y#plKE5^krvNx9 zz{kwz$u_~dDMYTtq!NewO<9iC+oN72{dey-tRV*(%kvX|XgJu}ftv=x0z&U*{~!aK z0(>+m5P&n9$xy?@@WJWAKt-%tADQMU9<6IbJh?Q8XX!J$Zvq@0uGZ9IykfK-`shaO7BkrCe0Ih0a<62!(v z*d~?xu?Qa1RrDxrno7yD)6e}QRSnm3{IT*4@hG=`zF(T1OJD~2XGwRqiuhU*cc_#j zcu)4p+ExYn5+HWCW?X_v{;5-@&`0XZU(La0O#7#^8qV^A1t$DS@*KdfWe>2T`U<+b zh7oXWMCnrlv`=sCHyaQW_ZgU=H)rjG9H0L(wxBOYfA2|C2-9gjvCJU|KS3QKXy6xG zDh_2?UWjyFhWF@5xnADIT~kF^?ER@X+;(TRGtfV+<>)QJce06>k1+sbwDP)LWMCwv zHP&>HdPff#snBk5XaQ>7_*F3@37dg(=oKoY+IZgTJ-}X5swx8CJ>8rf5XRuy!4%in zLq|jlryM9B7qB1j^so{RzRjo!^8%xJcxUw#8cAGencQ!pculE>VF3Km*9R!W4e9X= zwo08l|NUp?hSquhf{6GzuO83wb|ayH6V~RP+q3;h8<@w9vC^& z?d(=j;3ZLG%Vun9VYqu1`|hG^WngQ?W&5JJG#{_)5^6V( zBackddOKonhuIo_2&PPy)6mA3;>CI;sgNQ6Nj0`LcUB|!E-z(Q>!P-hq)lv;5N_Hkl};^n-_}x);x7e=&W@uF#KMy zkF?FOR6pwScwg&Ugc@`Mb?0FolqPCiE7JN1*I08i6Uv*ZeZJn-HRj@Gf2h!(aZY*K zq@j=r6T59%O=RDvj8H)9GQla|>*#WI-3IC!;|-rL2PfTq-@(=5dDkNhUjYOM9_|%% z9*nqZUDp1b17Mmh+H)8unuIW9qdY9w?1iQ zAU_$H!0O*pewpC`2jX#0g!T>>>h*219QDs3+$k^l0-jHX9fz~-Pz) z^?;mI>E1*5Wa@D6{Bf8!NLU!e?+J2??K9b1ngeqF!;BEeR?qiuRtVEmA*pip0?N-n zIT;+iO|a>h_6>6OR$r|(JhpO8fC>s`q!!i2y1==CL~%yoL!E` zF#uy0Q;0%9C~m>f3x=jt5oGpJzk)=_98vVinhxv|1_B08M<5T7u)vFkTpZuu-TgF@ zc-OSn(N5dGB#Ze;aA(Vv@e}EAt3BQ(?rGnnXW(9}ujrbx*Q^;uTRp^HPCNwsDc_le zR7K039&8@eBA^QFB3tcZOC6NE_IvH1J0)?{uKVEx7>W7Uyx(!S^y_IB@E|LiG@R0dz!Wf{(h{y33O7 ze1(oT=oOJ?!XtS-n{(#$f8=S({MWjI1K5X$%U68|dLEbbz4p#4yEr~*tjN%fZT^SO zkt^dgwx z;_3i_mFcL zMNgKInNq}e|7AQx8czp{{pSCofcqBi9H$wrLW2`N^!6uRmV!WVFdex>#s#y*hmIRaB7`|z3PZbta9-=f z%lXHeIAaLQO6UOoP<`MdxRuX@-Q_cg(g#jZR#pC%PDT}v^(|L=`JISIF*W;3)8YC? zc(=0xFE19T#|2Sn-;Het)##OqJ3nhsVI^`g^juKq@c{LJNSky{+*yC9)aUFKfBUWR z)zNjqZ^l96QH%`M+s65+It2=j>Zm=a=O?S3YXPqTqmk6+pKxcT)~h=SI;Jht@PyF! z2?mtgi=fv@f`()V6Mpto&)+9SsoBVompP|WPD_-?V{_+h~p z3L3fjYfXB6hZw6|QDuJo>v;6xYB5d6T59?Jp%it!eWNU{5e!g=;4DJP8z-+0ck^Go z2bHoQvE(dxszCxj&-Hr7p7ZftwuX9;k)armXMpoXAov;g`}+vFzy|VZWdp2nd(?X^ z{wotlxTb{tDWt*!r+>gqJE%Mnhv;%C-R7Nuks3;fRk)hd|aB`SJwE?dP&R zgoC=1cVrKG*z38Gtr?))M6HE7$9J2l?#xerl&l}UJp}d`PbO+jG?nQ_=1YOOwy3Gp z88+NqwQ474T$88GDC9@;EXLz-y1cWh2j23cJr)paZ0f>MJ$6G+3-!jn#n#G&-mmPd zih~*KaiJ{Gi296&*hcul4HzU{B%~}RPE!`^C3?2ulAKM{5QtUE!mxuS&d-_Op93-3 z<9zZ@?e=iOwSyyb93m0EI!oa7P3p13{#G;nVBXws*s0%ern~!8eIOS3=UQJ}P1zMh zv>YW-4S1P9#`||q8WKoh2@C80pxK|AI?m@KDiWbGt~cO?1^5&39niGef=($L2^pKd z%g^S+b;3`Ewq(jmDY-a>ddaPF`E^C)lmTA(dU5EL3)Ecr>IA&psRJ#~OEF25YN0iSaDbpsn*_9#S(`R3N z+{GhHJjWr&xIyVkri~hmbuTfj;ddc*qU8Ss=SW&wv0CY?e>Ex~5y+->Yeb8U2Wr2y zhW7fNKDPbrBU_7)*(tuCy>uf^ep%OjGoO((!+@;zi4lo(7JIkAtI)3EKmx@}s+d#$ z`ji7zy~_LguyLT`vat`;HP`G5BKcVVvUqI?{rtvaD$R)%Z6{9RIeH!`E1O(1=MkZv z^9;uEdx;t3Cx>oKwVqNDN={RmulQHdR21Mws#X~tU0BLYtA6(XXKMU!?1}|+&wzo@ zlxx3UO7HXpuNZ#(%HKI+w(-%|dp_9SY8h`TukAW&1!Tr5@W&#<+WH_?yWdh9&1v?JGw0=5U8HJ%8UG@o})f?JLF3->p1@S z7^OXmMqR9P^%EiaIWxhn=`3PxMgBb$D!5j~Zs`gz)BvH@r&XT(`cIQ7ep!`gIYI$BW)<(X`LRr})9>3{PC_uoJw&tnOA){+~kDeXL>2xD`+BitV zrSDDQbglRLx2)iK4$mB{TvstoI<@*sZpFb63o;gF2-{JJQh}&A@j+n>;g*}9>*4IS zeFt0nY+JCP{)=Jq0Yf3FR9^11nRBp^ljj3!iG^e?NF0Flsmce}!3frjS?nIKMfv?P zF*$;z1o4h`7Ri^A8l*^O$I)o`r&Sjcf>%d&F@F={6g~KfT}wGA1a~w!W1Bmm%6IZ8RyI1RPt#uL zjlQhJJ9|OuaDjArE~|Cws(c+?RtbrS6al|lg{J;C*fj5B%^=UID(j;ub~MrBcDC`; zCRG|+L5KmL+ATsJ3~ZO!zwO!``dUF#gdGR?)>So@TL1hxAP&2-iv2aEB1DK#JrMPO zfomY^et6dZz_n4rG)4ZGVSBt0Z-!BN9`tC^HDB62T>H9O3|itzM3CwN^z&s(VM6T02>IkmG8`OFOH+e#=_v|`X46l&{6Tua*L0q@?^2z zq_1|_LojK#<#pB*VPKm_Ao8t=hZ@J(k81s{`YgTgKhJ`@j&1CEBUt(71 zZlu*J9JwOA##4aVs3^P$%Tj!a#yO8AS6RLmWNo` z#mi({^YCPa?9r%eE7nK0&WosRu396_EX@qMv6C-+dif{>6MckNTY1eqNYJQDfpkIZ z?#Y!NcQrg|W8D+laCERMaNd@(Nxf#c@qKDV{3C`=P!ZzYd%u~V*Y_7QwnSnI~da%HrDpw9wklPIcu zX_=|w#lc%6Ke+(%up#^rg>b$${{H%8jIlUKG~#6Biw_beG;@PcE7DKO_9c0kv2h3M zyL)0`L>$%A6E=591DA7we=_-b0A2wGTt!VFmJ>0%ulkn*AOoG+5j&Ad-sx3V!zH_G zI=4JF3H-YKBLc|I8C0OBa^Jc^cPZvry+uDK8>*;&wqIoklkQPZ-77_<8SK@mz6tAv zmALRPsJ!)v;DJUSGgTmd2%eDrm|e*gsQCrpJ(QVrH5w-$0M@my4YZ|wcd?86S6LY( z(+Xvj07z&g$YXE5DZu|xy&r~v>mB}k4K*Ug2?HP9F)O0TGQ~tHMAAH)LUE?tGt)>l zG?f%cW~Su2hM0zaSC$poQWoQ;{YPV%l70QaY9~Y83@Pq!uzo#pqauIDsLs{5-&GJz zJ`wfuea`>udpMT9qZkF?#s5Dj8j#2@A4m_Q^}jG_CiEbZ8{cSgN1j${2zEVkV8qurOKgX49J% z6Paq||1v3FKt)N)WL`&6qGkPagzN!sWn?stOjdMwUy;S3*->5gWE}(B&;0@LoUnRv zK8277`9A?CrWg>X&+X5Q#aoe{nM`AutPj900nXH4sqII(OMyxsgu{#V01_7acMH#U zjRYdQluBX(eE-mB+7YY$PtcegPz6s#Ig(lk&-w%z@1QAw$YA(@$t&{UoBRjJn@4_e z4_pyT1Hge&GBU!2pNH!dY@EUIpK$-7FH66th2ljZ=K(rh0N~AP12?!*s3p@#rYIeN z&t~C78n~_$6Q=Gmh4IS&fsO%B(2UR~b z0KWh1gk{jv1@eI>G86jyG$Nt^5pDjmi#`%6=Q{E!3>{rtP0*ERD8 zwC0&)T1-IhK4JOn($mUk%N@Hm0$59*pvi51!3wf!=f7;TL#6ad0|VHP?qR*E&=Z#s zkqe3-NzH%TvJF%ctUA>bSBaT>?xE0#+Q(N$6hGNm%l~WcC~?N&P^OWz9dcI&PkK#_ zi|nYy50#K0PPC_8HZ1+`A#xkj_!8W`{ExhG7FrrUnp&UknIo#n1ZWVeJw5OLE9hpa z@W1DFCcE=eG6et&xh6Sg)mIQh{7EDUS6U;kKhsX{$vUbkEua+5Q3^U&x3+`?wwgj4 zYw-b8Iiif(rl+7oMkOB3YeawqOjaP^{f-Kx_Uf6Z{_JC1?3@gZFUzdqn>&ju6>>-F zgTxtd_HvLIn;_JhL)T}~I^I+)6wDody*k%8`J>$OZshZXdJHAy`Fe(b3?a{Na7+as zHF`}@cc%I{Q41XoKwi7SWz~^BQM26DKuZkqFoN;DjK?Pg4Cd}wwpscuL10$KPr}I= z!r7;K+Rt@)TzgM_09s+c5#v6Bi^wgH>rdrQqgk&i+3}hdc0J0zKor)@w?nZOzIff& zDbT*?-d>%^k<#C*?Qy%HweVm-Xzm=RztWyI6PQV--P{2^5UWIH$O%7j5dcu-KqD%q z8QgcZPXU8Ic$j91fC0`W&j=i9aGRkc*|!~aFdFV$XBDsT<3C9@62*FQbg)WgJ-o|E zAJJ~?)ial4YBxD0>gO>);nco_ixNTVO zQQ+aCi~inTrvCa<6P-;DGEY7BH0{r2w&;-g3pgn?*xJVOL(0DEr7E+_w|;1@9K9LKDlhwy zv3VcPyF1JFa1mHz@%i%>-;LTw$Z5IhbxOlgZ=gp-)e8>3xE4CyX7WKY@n0yc$m$m> zs|^hfRQ$+&Y(Brwz^Pt#rcTT^(Iu06zryH8B+|Ra)nv2Xv-gh7Xr zIUZ%@Ofp+-Y54_F{ZsbCzleJ(RLoqp;)PrI6ZQu(Kcy7-{)#)x5Fp)*sQLd#x*5Em zRsc}D31Lb7zsTV&IDrcyKBcqbr$WQ$Ze8JD`T67A{=oRAUcOV;jTj{Pe(LBLrQdDs zak~ESDtG5Ta4*A9e!fWS)E|kfVE;dnxro7ne4}pJ!a%3fHmVERgrY zr0wH5TjD6*aR)FYxBa2Zb@kxj1u??j*ZK%;eKq!#g%{65olC!syvUXRDz@1dU<-?S zHN}<@oZ-=*XoA#WtIb&*<2q>jz=WCq{V7B0_iv_M4VFmBPX#F=fVKv&ZZ>^`>FjD& zl12Jv8V&gW4MJsq%%4D(n)LF@I7Sq|4CECY3o9D_z--A86U-6M!FY^HL@EK73Wg-J zm|A5GU&Bt1xb~0Ww?^WBp1Bk7JEG6tqSC#~hi`%w>#GF#R)gggALm4ZamyzP4N9`m zn9}m^n#vM4eEG06dWJ3syrP#Y`sOj+-mrw%gL%kH#kAY++u~1-)&Tr6-m+3S>bkYP zz{#Ajp-x;y!FQwQZL;>zqE_LI#c{`EC0Ozn@26*F99p05(&U;0b1h|SqrM*JyqnXT z!LC#3jsxB2l-pyCe{LTln0mC2Un}8T(OQ2XXrX1moFDvohWm}&(D?kv=2wW9@SxK` z%a9I50NvEf8aOLxPK5SF@)dhtny_Pub8v41$d<7T8 zI){TGVN$aJJG`dok9ifaUuyO!4#Al&+XkIQUlk^}hK*MRGdIV&(rXpz05X-WKY9Zg zQU6vy>y(6Lb8RguS`oxN!=THgj%}&+S2p`9ruk&sUs5({l6NP;XzJQnWO$?!oK{EK zDz2EQf>Dx5ZXHS7Cyo`sEc^1->*!Q)vQB(wAwPhQW#`@>Wj%EHrgSzVtCCQyl40ds z9n?CuLWH`HJv03+oV-VLf9$EYJT)q$A2Tt8R>{a}kCup{X)``k_Vu1xRFXKbOftCy z!MrL)wih-dk_->ssuP+Qf-i0N+eOJy=F)sC7}NRDCpz0X%a(fQ6f#kM^q34d zXB=I_7^ds!@bgN9aYS4);V0R2dg&YW`J{=o8dAn*(L`W7A~EGi6|ZOV`Q?&&3eq}^ zeT-bII|p7FFx;Z^z@a31n33!tYL?U6h={Ex(w&lklYul&aDc?e>>!uF;phCsym#e7 zZRkX)Fk6anMHZoliE#DZ9SVxl#jwyS20KxP_+fn9`)3>CdE5WZ1vp2t z6iea8EqtTin^c<~SKkumi*_1cb{XcwSw`ad?Ij+Tm3#Uu-&I`wq#q3{NrrTW$})eu z1()w~tAD5`%EFp|&+N9{yrsZ+4(|iP?$upf^I-UD7REE$k!fTPOK@HiO1+0LzS%1kkD3lz)v%^nwxyvU9DTH#}uF=QzBOs7qSNZ3_`C4oPxT)dKQ z#uShEQ8wqdlBNe-m=^aP_@(|P7U{R=F=-L-*kxh2Ng11X<&bfIYVpcn(`Fz0?_HT6 z6B<)Wh3R+Rf=!vfMT{G@Exz>U^$%30(`cK; zGEVCd)(m+?N}$k;sjqV^g`VptxC>c*wcn?LPgAQ|hle}bo>2Z_q>%wW;mgPDP5iFf z#vj*~3(6*zd#yuGa1t9~LIc|bW(LOvDn>jj-^>tf%0UCV*`q&BQ_}W6g^URTaxEYF zG^xv7a$73S0s%~^m>C;e^+{ZO+XcvZBI1NW{m=D_y0O+9LCr=c6;M!d789#*IQUj9fI9){AhW$M{a*7Bu$3z}RImJe~$M&P{MmM}%Z$c}{fFF|7UMmMfMz4H?m(-LLN zMlhB`HZvLXfJLsTt?C5(#VFdsj59L((KJ1@Jnc#r*B;%Yft9$z8+$nW1mh*toqiSH zs(%n z*k?|qM#~x_q39?ya&Xk>^`RB5&g_P3Sq*1B78@+uohQNxN2g(tuI|0x(Gx|sJ?B5P zjtH5a@mEOdR3$G$K^eo?5WPcpru*bHmmtsb5N^-b`^ouc4tpnC=}}s{=s97BsYXFm z%zP22`o_W)Bnnvm;U``=sJsDA$1;m4o~R1q%xS-MS= zI(*knFeLoI==Oq{`ucW!?XDhWQfD@8GS90w8|ucsv-jCeTR72wwP_usn=T*O<@VZr zeNDbo;6Um~zC!#S@6-;i@jOWZdnNb!TJSt?O4^yjU-x*3v5|v*aR1+Wdy5Zbl zOlrNFrg?Vd-!YP}YbvcpOXS=T{N)jS0~7<@I2UtWT1+j*7jDO%Oc(EF(}1W7!k=5? zS|8t)8bfA@_b_xQ&;4i0L%Q>i9OOPf0aZ?%1J-iGs>*joe|jOda*~n)>ysSoMy8~v zv6IpMr#z3p`sf+ydDck*$JiwlXls%EI|z+fvAcH;)V~k3j&yQ=Mau8owz2AKTM78P z1u7rC$e?sAxeAMLI>e&ThlN>Zxf4GEH2rW?@QoxO?Ui#*7J&kJ%JVoni_v5IbxNwN zM0VGaw~Ybso*d&AV5lH4$JxDsz>*8jH#sSV^ z(=mx?g7ueoxPL;fzdX70Xd(Ipz>G(04AD>7s;8+QW5E*O8Q7>I8y%hI)1^_K+!jT0 zF>!Hy$gCr(wl7fG{gh_Y{RZ&b73*D#*Q#elfi43yysp8JKx)eK)F)dipQTq#Y-h1D zIp-hV_!RLIP+c>d-PMIR9wCwgU!F*eV_IIlq;xNlw#P2QIaQQ{N%{8!7~`X zdk&k>0vY(}N|I^0Proa=D;fgi96SMnonI$iqdUwif+p4&Kc#W+1wAFE0$L> zC;t5vm7`P#9L&Sg*?pik8HNnZF@OXB&ie>hT^&bG5079>l14or$Y3UvKpCOW%`RB@SfmN!Co429^UVe3z z?1+5xK;S-6L-u7+Cdz&1v-jwYZ2Bmq6%$G=6p-ZE)!D4EuLbKczc!@AI$YZSsux=c z{5V`}lb)%bwO&Yz-GT?d5~CB9r$Y5)Adll~xf4fzl9;!DRJ*yW$){aW z#N}RU*E^`I=NIl5XG_HiH&tqT5ec zm^wRQzFfivFt3lA?S0PtD+nA;x$Vif&W8rDpsRedw4?*ZOEhR)OPXluvn6VNWAqrI z$MiKc_qVMgHhb!G=W9i0sBB*NjZ z=vCe`=?g18cgznmb&#^v8U75m$i|kibu$uH*{}2_ivg~(9&Xf1+_(nM2Jb<~r@yL< z^|KM^Y9qv7X!fEt)9am{CAb=X;Z8KHJ;}f2{gWSe$3=cTlHGBawohGZM$JSd>Qsyd zLRp%E)fuNu#cBKbV0dxUFF=wylq60MAqmmzb_#K|Jid)$LQ}-=DVx_Q+-DVYo9^(S z#&yqp;r|G41e*#QKLj-5=(n0UM%OQjm1YobE?kGX8ZcHo%t$rqQW0I_g3G*^$$`qA z(@V2?nJ0Xf-}v+*p1;!|XJ=v>pKZg!3va3-&+ ze@YPRdfj9N^*g3?GoP%&atQUpbIn&R@8Zg$#b%L%W+3qn-kqe|1P4opQW+HuufGXI z(dpL46?^6P-y9ms&NRdv=H9h*C|uI~w4k%B+4TK_`pxLGqLH3sPVB-V#eG7V0OZVC zi_>&yRl!z;7$jTMt*|IX_Mw|MjKGsW%~!T2m01?AdD&H)STOxAs;fRx>UP_hETuGH zjwa^%IOISJdtG!6ZS{KPC8F!}^xAr%;Yh`*dNRZ($4o5M47QS>0HTC1rMU~mC#w(o z3Zfrm>0HrlkB8-!Hasgta%Oc}{k^X(8~b96(+Jg8qBXgr zlXvIa-0@5^vL(NiQJbOnW35zJEDt*h}QaFg37ad95|y+7ZIid3?}cWkgzT%Z3<&8g^I2IE%g{5&35G_3oin z>kjr3%Af;xk-cyq_(bSZ>+<1ENh!#k7X6>ZmfT;@k5=aA2FGMk08h^USFVrtk{j-t zj?a`3J(t36A$7xU<9m3k4s+JEqQhlKL!`zcjrlm1H!G=qN3*Db@l18*!YrKp8ZpC) z0Qdf)=Gt$Hevnyz^X%ATeqX&f;J`I_ur>v_3o?i5~xhpeiXwNk>YyVkl3g1PpCE(XcR*aC`O>DP4)F# z%JcNUYUpkHs>Qv#lCyGOl9J4-JayGGdcDp15KX%D;8_(W=YVf_= zRZ25jk94%|o4*NpJnFk4K94|SlLiU0Fn8~AOwyIA%X!=a{ zrLG(L{q4msxuX`rlsk>+q4xW9H4lg%sn~&#k<}-UU5d*`Q{wh?Pp-B=j7{B1 z-9F<@w3Q#DR0ULoc$naKYyI<`y^Ct#FLqn^H1&9=`>96N_^ItGoH_=vM&-sGMiFa~*+`!m3-u{`<8wgIOjE9ByBfMg-EE8j@y(4luD zj%f4A_Vx`bdf}D#m(?PHyDz4c7I}PR__Zls#xs@VU&~F1)oHvzJhr5;rE=nu9OqBx z69fDl8^;q{gr9`(g-D4qcA8|T_Tq<)v_E@QSFq$gR!oeyBQ!6Cnafc4;&Y3AL>9 zr%Ja~wW)Cw##I31&Grg$4cjm|GKSD3odD|Tg6D^4Ozt|ZOc32qC=5eZJdulgOId`_ zvbXo-+}ZFGA1EnjqhEiAWAr=s3{vyR<1o7^DV$0clR~6SG_CAuDl{*+7K8R>6!`}Hpaoj8Ea4cuG8|pbM9CG ziaWD)y!PnQ@COQ!pr)i$>FGA^;eZ^8KcIzyt|^n_@z!fEV5>>>3vWD!zI856`EeZz z_GzLh@-Dqlvf-3USnMUmhfoa}MKvx7&+&PUwoFz!t@a=(40(;3&vEZKd$j%NSxg9R zBaw9|>8m-fmib2RU{tr&tJS@AyI^2B7v69;hVbcHIrRe(g68#lMPw4UH7O8a^Q+$P zTOxk!xIXqt^p4)Ia5>#4my{Y<@`mT}fQng zU_@b4iRlFq0-o;z&XqGQVG!Z9yzxh1I@xoX7x3vG;qEiJ$J-SpM{V&I($5|@=4*~} z>uUU3S;opq+eJMnCks~@K< zyi!h>V;-!T2s`x9B21d^VK(mgx=#Hb@m)~Hwbn0LghcLIo^v_rZ@Xcumunqo+x*Su zhFg=1?zhF3SXfxNbwNJ?wX^2pQlRee9H=*t57D{q(o~f6zJawiS2!5_s5`X6#qPXJhw22?cKq82fZAkc~<&jWGr&T zF$g|bc=P3;CH4>VOT}ETJHo8swSB~T`){<{MvI4w$7;3&_>bm#Ir`Lt4iATMc~>^} z=FAPpU+TC9jR*FmWC@a_kIf&~N8-6j)QP`dE3gS#d&NF572+Ix?Hvn7yxfJHx-YnZ z;3h&3R+2b03SgawTj=L{GGrjl&HOHf&Kws0*H@4e zICGvnZ8wy(z3?MvlhV6gjKqhV!1l(|SAxD6APjjsNU?#T6Kb2dmaI9{U_@=uXyr_e zVAe=LAp=*nZhrX|QVtrP4NGf`^nwU>G*7_cnCx{L+vi&UovJxz^o-tM-$;u?qszFDG@fw;YjrI z{B_hK{RzSmL_c5Y85TE=l5RMEbI&u&#n3pJLHlL+6C$aHl=GqYXDpb2?`X36PZ@Wj z9H!NLvCwJNv9wRn6L_1bGBb>?Geh~V>ndJinoa9{RjSbs_tsVdOa$0NIifG7FNI-~ zqm%Xr_i56QO6=Jxe(eIMBOE0v>T5?tIR0l!rI`m^G$S8z~wS!_wjn1AUx&~NS8qkP!anT&O$2%sD_cc>4b zUL!4!7-ns}&U1rkY4B{k73!z+bRG|<($xn&9oxG)hCB^JKD&NSE21tI^mjFp@PlpS z3`Hj#q?VbIbkmyL(Q*uKRcP-}5afJ{w#R2$8<>cv54S#j6T7t}eJNb~KNx%KsH(!R zTNn$Zr9nzsI;Bgx5s>cg?go+W?odj)yQI6j4jj7c0Egz=_`C0WV~l&p=NSIw@tkMx zr`DQtt~H^D_?{%f%@2M3jH|=f({K)*?*jIcKYTS7O%*rXvIZB9^Kae@G*t4m;A;9M zl6p})5SR}Bu487FrE6q?D((FU!_U<;pL>=Xd$^?swV#q;qZ0JI3mE&Y8IGYTt|4L( zRZO1$ixaqwp2osMG9oU1k^f`gX}0#oeiz@}iTl4m9R#urTe0_J7?Q4 zYo8k%O5z7s?CHBGy~|a}1^KN}T=&wOb@!(R-n3y`&jg)GmyG51&ARhp`E^m5lrj_7 zzyk$PGDRyRG+KJ=fkJ@AbH_;R`3!5-27k0;+`8ZmcC3bka-8CtxwiybZ1F#xdK6$M zjwZM&aX?+u00$7`iiQuQA)(P!4D4YA#`@%f@SbzF{b)K_raU_r2XvBoF zT3(2+qaEL4aSuJo)_+@qc0>j1687_DhN&?_R9zA>hQ>tlxZMH%{{RDMrwW8+v9St( z;wLVyTID4o#=7_!mxX`}WP-rIDk>7!nU(E+w*#K@N8;JdPKu9q&%7gg;~zs#pWfv~ zRWCclwVhD5O=%OEM**RG_FtT2!3AyilN)cLM&=$UuG-J|@?O{MsL!Uz$yw8pDNMQ< zO`bQN+8_0C)&|_qlkJs*-^t)YKcaEyh|P;Nm8L$zLr{8HF2hi6Hq~3I9mXOAZtR-^ z1{NsaHdZfGa^aUUzr9bXyy-&+sKMll_6=1{RH2J0>X2jk?DepM0_LjNcFeMewAAp? zOS6;cD$j9dsP7C{rE2YSyVFS8_{O8G`6@52BS>ZL)L!r#*~2JB-st~w>1f{GrIRYw z04eZQC=Dr)3q;WX^kX_%t)eGP2>_8vm z1Ph{jg|=^*gRE`|l}w+yN>O*?ZDHp0G-{7K-=W~BrP)6jj_W^d85a27ps#V$v4tIW zM~<)d1S()PxbtO@?b;@drS$ga7g^{*7__b@2rp~X{n)iUPq&YJPI*9hd~gq%Q>SCo z5>ln>fij(!jgR`}59=5^5s!@1uVmWaa#)Lp|Bbh*UDup^YMG+3q!92rr)oi1fnZL8 zy-wO5tw`xAyP0TCm1#+J^6SX+8Gx>!3&$h?#vdCWUk2Q=d9W`sHS+~toVUd^)nKN7 z*yJVXzp+WqM346`&_QgxQcQOL>0tQ&w5#8I-$X?R^jdqFg5qlOBe7y(LPgNX;NYaB z9U2;=w1mVAv?@g6%|=n!%sYSQ;UBjUx&7PXm;{8xghpBm0tyPCtsTD1`maez<7&=Mn}zvF+os5lHUQqi$tUK?kxE$?)|Ue4mXg%alJMLq7SI_@ zQaKhm9q6<7T|u8jJvBA2z=n+R6edXem27ytn$@aB9Xcj~jNLb;r&q81_Wvwff@lkx zpiyZlt^f#fKQ*N&DRq#G5u#{G9fFBg^!P&U&)5QcC4bo4tZX1i%0KV>fP$is%8n1a z^@`wqPt?z#AbpnXEe-vvfdb^iy-C*n#s&_gfFD1+Tnh5zVylAZ?5*v&2@`E9+$%D6 zKs?2sAOLNB0+gFGVgUOja)A93cU~z0ThcG-&TJi1LMo6T3ccQTO)5AlGbA(;a&2cp z*v+{!Rbu?`_$I`1I%*VuzJ`M=v+|5G(Wb;*fPJ*h_4|Blc!9 zm+>3^8$U6?0stu)u-y-Mw7=a(e}C!fib9ZMzxmA;H6LV#hS&S_c+X6ne;^0Z;bVh> z?0F$TW%gBgA%ITeuTb%z125%GV@121qy5P!x6wjFB$Qu} zaEa9|F8*<#iwuQm|BeGtyBiz-O`TIoODiHeT0Ss3b#x~l!FlT&@Ff$#GY*_jX~Zci z)$#-X@$uzk&a(l!8+O?=9NuWrU>m%vq{P#c|J<_Drf+P?HGZw z2YmBvVL{Nb*B;}omv@%LrV5ePQv z4OU=Z49X8gL}SmCGJ23CdMq2NLNUiv!Zcu^aVc0fd3YE znjranLps@R-52$IjiIpb%q62@xnhyMb0=5CnOPE?`n)i5xhu}%>wO*|1zY~ z0D?b|NZ6tOz|rC8om}(2q#E)AWvbo%2O4qS)MH=l3Rd=T7i$0c$|D0QWn|7Fn&f#^ z0|yG7>B^hj6zh%@pofFx#Lj-}?bG`#gtU?LWR41FUxhXdeKXKk$5Z+%l6qF80NB>&s`K-=Z{HfQ%!c~Ph#b7_W*Pt*T+WXXK0wAzG#YGj$JO5Kz(*Xpv~-biQEzKP zwzszS-Uf$+z`U^F7KpClD$1fmp+(latp6cG` z{%1pnl!>qgcCVmbFut4pmT2u~fm~6|+pBS{G}HrGM0`K^=lc1JiI_y*r(U)8{cFDnKS9!E z3WAknvdnDVtgm76wnSbV7E5>Oejm6|txyep^_{O_VA)(^*dgjf&($%A;*ln1rQ?H- z=a@EIC6bnB`_Jg|YhJJ>n4PrsYC?2+uTLcVEvhEC+pX1& z^M<#(j93U2srPT6FTRG+Q0J3y^U7R$q0k`U@kPIT`xbqzfDnJau#(Xb6)c;k@+_D? zrb=%i4#)5@=jw#%1Rbe2)mWZvW&LkqwVe&{Yz7y5DamSc#K8| zV%kxe?q*fpk7wuIHdZLr2wGD3?(q2K%)2O1`Gz6#t1{88*?0mL2^uF5pKBk>3^`m3EH&9*7 z(6Xac6^#s6IAM*GjdbWvWv&@9B9)B3BZOqd-6xz($tc$^Qn$V5z9h||H{NxUY9zis z?R~_)jN`nEN*-3Cjb(;IQM}=)l*$q7fd%-w4nbC5KHS4-lWi}y!`%mF;;$svGvT4y}i9^!Ai47q;}9k%CJs*u(YAI;Onwex$SW( zY{+5#uUczO?mvIcny-#Fe2x(~s}%?H&|)J|(oM2ev>Z-n>D=6Ce9?L+Y!M^D2 z%vwHB3ObJi#J-68fQZD%#;riaE;O}7(fhXkk4 zXMXHFo%IKOzla>DP6`fYt&Fi-hOEqWR;;v9()okGYt@={y@;?J8m!9bPT7z?g&k&j z$4sLxHsL*=!Q^Z;f`SLJ{UZ4=396gEcJ?v48)|$ktMK+;mU-SB9A7hXfX+8kFrQ73 zq&|f59k?TDXv7>6zwN=Kca%*vc>XRWp?^;j zefijnd()yjAKQJR^*+LS1blnQ+|Y1?hJ6Be=n{MU(*zsJi*+*oiI`{fxbCo`sG%!0 zi2pRF37T|=F7Yw{w38_Up#yaoE~!ud(4?4O@nc>$e>ax*Z@%nEVWG#VAxUBXyaeFH zbaRt40n(tC)HtKH2WYB;7V9AhfM60=XIdhE>PYj8XUcs%h{-(d9d^Q{})=(mqok9Q@dI@LC! zlwa>Q_Cabr)pdd|cheeM$vkqso6Iv8OnlVW?BF4R=7S_5H*fh`fPgBNp&yMf{<2NY{ zg{72?m8`1B7E;%Y6&F)I#F9^zUrLhbH-Z;CVox})8DJ`drWux!Telc*OwOfL9-I7{ zhFL1>n<9oc!qlJU5L-DcnTJ&?Om1zT`x`n66un;Cc7FtAvHQDQ>2%=00Ub^Y%xxi$ zAhq%q(+~94er4|hPVl6>=Ehqedp!^$3|q~p$y_PoDIdP-I|*<%J(*k`%$Cx${h$X@?07M*k`&5lD;j@Ndsz8`IHuRPMu#d06SQ>CdN`3(Wj?aouD*6jn! z#qe51x()KBNy0r677mW#_u9HBj{maj3&*^X z)&Q+azQ#;>yWe)o|EX{~p&{+4RO7xnlnDjPFYAx9vbd2d`P?MT?7$J82k9a zS;VtX>bA~A5GsgGrzLflFKm|fL`FO&Rd~Ce+*G;U>HN8jerz|2)(1RkPJDL!H@S(j zc_XN18|zeJr$UR#!StdJkrmZ*MrgjD>g+oQoTaaNFzXX6HwVj`v8ni<{ajB{iy_Sy z6guO+fy_!-V6YdWJ|W|_K<-JtW(+*jXNJ$OJXu#VsiN!nQhs<|Of*teMfpTCN00u+ z`!Jf?&^BfmpgGsvkr&)hvC?dfk3r?*(Br}Za(lx(IT_4*I>UDKpCrW=RFVXCAk^4b zzAkx%Cjgmp)NkcIumB(?EGwmVx|yWT+UCyZJEQ6D&{fx_`;!)+6^~=Z7xkr+g~!Of zf}}FVthqQVH6Yb|RfY;EbG}F&Yl@1<6{5b@0wYFQW zoByEAkn#$+RmqKyY0!C`i`W(?T2^o#l6?^`6!-PY=2253bS-P1U7Ydx+1 z%?2im+&zcw%6#~T_*Y}vBW^eR1#IqN@>vIRFnX8|-7E4`XWJEr3z`NLwb}yZjr*{$ zb?^?p{n(|Z7bT~xw$mur+dQvmBbP5X(^_o6D`p*2r(pD{_`-|MEn>B*8W^Q-n1#9? zh^{CnA`-_(nx7O+fNa|9a#H*)99(>Q`Y=7mnv9p1*W$bPCN+36lmKo7*?AJlX1!Nk zY)8C+-0-XwVxVSVlzD8MeKDZZNdS%{~n$)Y{I8NiiLiitoWc!z+>u5NkOtxK4EDoB9Ap!mvxy@aK<2dD9Nf_37I9mw7F~fN2YD8CUE|C2R)z zZ#B)#xeF+4m+(e*+x)!RY=74B_Xgv)a`TL?W-UAIaTJ1OcKegF)-!JzJXRlYpWPo^ z?iQ0Sz)HC(sVYBSHA=Cl)j06XJ-ME5P0K@i0_LYkAUGiWFR78DCl(_siNt4HC61pt zf$|<7Fc(o1`Huu@MkbT2mNqEZa9Xxu$zi$G81c0Io&m7H0sJLi`tyU_q1x=$Ag=)z zB8x?u;AxGxtN22CO8^pv0hoTAhAIg5E?_E91Omh;bF2}=o|_}91Rh1_{A%FK25BWkFQ2+1TEm?@xLh3&Ugc8)@*hWfP^t*VH^S~pjn@9B3n$` zC8Sr7)R}}k_v_Xq=EWAf*|U$JBKjZG6}R)$Z9fOC_;0VWN9H^tc2*@s8~@aeBp5+Ja*aMEM<-rpVWgPS6f}6d-{A$17|FD*2MI77hoOq+6v74vpT7*iJ}p~ zwb~{4t7K>TipOkvFiPd1O?9|7TQ!N|a4J)3)Po*=kpJS5&k7S#83{EO0Jb@2&jkaV zYO@Fs1r5aVD#zaGB%~DxD$*FSq{1V;H+p{Z{>f%>bPS%M5l1wo+1%Y7&$?U%U{Rp6 z--@M`vLM9A=>-8CbFYwO=vE>zUw0^sfeR2_Ih`~{0bgpK?q-PV>aB>=@16c;D|G^= zzII*MD?(u%$~tvwvTIs-wq-$qjbn&^r_|K1E%@#9-t8hxZiHO6&>S=Xp8Qx1UpJ^kn|sinWe5+?#Jqf493Te@?gKJzTre?1YeO5a}SBH=@9M zh*-pjJz1p56jaxd$>w+FFBz1EQxxry(8;RC0wF80$_ROO4$2Qz=;i<9c@~c&%)GgIK1{Iid2*sXS19 zfk!KDj(Rt97m+BZ8}8U(zCDy};<>sI~g{SkWHT)Yifg6{x$4610-qnp+CXVe4IB|nC zme5p;ex_x867qVS?kCm^h~|=%Zp+cTAs^p8Z^|DnpphEXpUi-GMaIh!TTbRJ+u_i9 zPtD%UpPe1yEj8Zks0-cTthv&Y@uP`7w$DxSj>)9_@vjQ(33Z880?Uu7dC0m=2ym$`kisdMF^1aWjY(4f2|f?CI+>U;(!Dh#H-V6+i#T z(=|SyTp3KFGJjNNrBNpo}#VQE(xT_so>Y~p$jQy zHm&-cO8nVUcHcfU!J#l&?KuOsKM5s4F8a8X$`V8NfJ_UyEWDDxh~8iYD#(MO7K(LE-M-aVYlxSgq%J7 zxN=-^amVOnyr+M@Lad+G88Bw43;L6x2!Fph_m zA#eUtY+B3tQgX_A=TT&p12$zz6~^&F`2SsLKw1|DoC8I}yf}o#kF_)J5MJc`U(J>a z2*a_*HXC{o_hKeu`+k8>4#Xp>hbjWBsYLTyy9LN7Q4ncgAq9*)-m5jZd;u))ZrG14 z+!7R3i=?VO2o{r+{VM?7yfHu_NoXg;#Jta8QUrV-;}n0z#Tmf-hpU4&TRJHI7gMLA zqHa)g`@OO3zlpjwO7&8eJ7msuS^T;9hh+nV3rHIud8|jT5eu0318}{b*P72ig&;`B z<|-r_%}xS!e59mQFZ*H!3y=*Su1RI)0T+$(cQpxs5hyBJ9vJ5F11kzn|GwNB3k3=m z-oAswAi#|96@;<7#HdY!h5-a&F6B-eWvYzHQoVeR_5E90R6w#jq>#8(r1WxTIvl(_ zkM*ujRA+gyL{a=dx{ZRu$oRx`x(~Tl{z$^iOl^@{0eSKNKbt4Q`<=qwftQM0@a3+N zP=2vQ019&8;X?s76(CV#H98$%{l4#le;grqj(<6+yA`^wk2UuvsxX+V>_J3wWrn!@ z9b*|CzA*OIOI7Zh9@i#$7}%h^DeEt%{V)9F!3%AK77d&oN7o;0QFG;hq67e@Fs72k z5&q*@m|96# zSc;t_5zDZsUz9NaPm)hU_WZl#!O!fM*gSSg=w&^S%PSi>T%a>Xi{x1pVg3<<4SaqW zNFe5L$blWtZ*iDEf7}(QY5TKtO-*ZL*{sz!|MZi6i2?VtNkvz>UccT?KI9CfzRULXh-7kEnoA2lv0KnIx z147Kl(OPDV8;U0d25ubJmd>>G0i$Di;){5$EYuRLgC{efLoaTYR^g z&@dKeq40exm_75T0==$tv~E{*mLXM%p2XC|4$n1+X3pbkMCSaN_fHVkMXY1?KLX=z zo>Uwtmi>>!@&#-}7VxE>ULAPs8JJusT#t6S>_mds^$r`3AQp+|98>`ir zZ?SZ;TVt%<*-5!|yR_w3Mw?>W^YL*pZSF%GbbX>1OQ4GAxWdne#5IKBs(t%1a``8>J2@%Mg~uK32<{!7@+ZV!Pdl_iN$&NC68=dGT@(N_OChWh-T zSJthn>W@dg0eVxG(|mOqIsRm-yP#%IFt-cDH+zJF$K^9eHd>JGpfT^$8Y?vekE!jq z#$(Su-v_w%w!4WblTOesjfcEX79PaKekUfW?gDjSKy$dz)@w9tx*U9t|NC6>IdUA7 z+NJaizX^esQ(lC8<&n7Gffw_Nk%HojJE(dwAPNxlJ3?DRQrLc!`%^}|8ODSD{cN(# zxZ|&2ieBr^ZQCyJqwohMWM_NSa+TMO%w@8*1vo>66>?{=qvFu3S)euMo3Kk%F63-% zNbybV1vk!X|F45_mQ$$c^uRN5EA~+MVC%&pQ~RaZs`v+hv#%=T{rUs&imyI{nlchz z6U%v?5tScefAg?qyi0x>8&K~V(CpXu(0e_e2^K$?LF47+d#qG7ANyjcvpXb#7+8du z!V8w(DKPqG0RPK5n`Fgh_|wLls8)*(BUP60>EjF+TgwNS%E^NbxJLWiZZBRN7Czk; z|31gHjp<0R*2Tj&^f#*&Xe-XR9L~1^4g8N`b*r|8N;zM5rsQuAvMIH=JwDKv`PEUM z6Q(5{4<|MyoZCKpwJ+3k1Mk=@j0y_!n!2JIfxFfg9ryJ#)TAZSZ|S0a)dvD|#aP+q z?ybjV{IzkJ-MLND>uOJaQPI0R_8m|A8fq%M1IvKL+S6)bv!58xFGo#382p&dbyVl` zA|}d}qX7X z$!2LO>0ue_T;qgl0y-?M&nNUJSB{6y(T8oEvEWN46WEM_> zBWiJ0x=w#8*H=oEyyIP!WKH1q;KtKCGWf@RXUg^m{M#=s)(Vw2@S4`EcvG{CH;zu= zn~q1~0EG6PeJU!*&1Ibv-=Rt$G;(1&WMt02ySz2V=iS=HMzH1Qq33`G-TT0hs(A;K z3XQnHYFV%IOU#|Xi0v`Ws9b5|`F=a};6|pWJHv6qhSE>8@+_6Au&jQ{UFFs-BqV`9 zHJVkEb{{%zd5{#NKKj*ch>>UqvC@IMivtg-f2CX@s_0W9nWos1B2m4Gk_tNRsAQX; zCO|C)mA<)g)myp-C>#E7uQ^Uhzo;kmp8R-iV7U334|6fRyj%ou)~PD;P)6}%{Nu=- z`HDz#(~dh+nTDE$HJKVPd_*$`dwn&s|LGuJ3btt(iTY9@&w!l&M?ea0zG+$|itnT=h z>Ajgv`>hp<^x^Pdva!uOM~ua`8%q@&luM|N;l&+RjUN}lQKz!bv!;@+rt|qDN0xaw z1SRcW5I)Eqr%1|LBR$wy{r-v(Xk)Zot}HDv^y;F{+Imo{Tm}U63*@+GM%X zJKU3Su*K~kyysbH#!xUXRClmHFLVhfkCxT91!M?ekUpZCr6ph`eU*6oYc|?3$#!?Q z$&&e%%gAm?1gj+fg8DJ1=KEE-)>z2S+E_&XG3UccINnsb*vG*WV_V2&KNnK#0kxcL zu{xjmPq7a|)Peb(K1>}EWovw$kN*;6IL7$wIg#J&NKlK zU4Spw7EkC49G%AwvY{>x!-Ss_oEm-~O5wg`P9Dvh%DThXeLUViAT11Qf4ZKwoMe3$ zS2WGC?{-I~#rCO~<#R0zy4?DyYh-@>s_(9TPoGLOx)mp>P)=z|HBJA-(86Uerj9Pr za3~b?b%P3eG3EFSo-no17rD2%f4|>#hNhhooA?*}SOS-%heZb-Ez(tTJ6mf6@5wC` z4bspSWPBhM4rnUPi5Pv(9H>Km7CJa)y!dM_`GrQzn}At6RG&lBQM&4nnWN^3i8Q~+ zRRqNhkzDYNomzV;FRD}h8#T;bo5BxN+8Lc!Y{*ue(=NM&Y*7E-g4Oz^U5Q?oLsXbi z09t>?{$!9N+vXcfbD$>da=cW>``m@NV)uA;d7?e6k!5{(#VFhD>Q_NgTBhjeJ+Mw( z1BAG2C~KZcGc&K~H@w3`*icVH!{h)LVT~EdBvRR5fk0vZwEp@pr|#~#NcR~t6Selx ziY(9Iy4A7mq?&m6ijniK{iO=Btx^+D5gaXs`7Ga6^pN6MSnd$#UZE%5t)!EtTDn74 zN_k!BekI$Thgu>6=i^xdg91nwZTP}sjN#>+4NF1CUssA?htb*K$zSzs=;0fLnxC0A z_OOk9^$=l;rdBxgckf@eU){FBvcg2&d_{_?QG*DZY&T~bYhpvDUGS;N;@^{H5Rt`K zS!?#lYYx$zbjj{y)Wz6+Ft3yU{#UIaaQ~d`Cfk^3uXC}U51>|26aJ2R4jW;aLZ*LY ztEaEAS^BeIJ&8Wd*0Ud_)mh$-msFq=6Oa4qcqe|boVP$$H&>+BrDlWMDBi1y-8ZoaUsK%@ZEjL_1(qbo{k*))<&Aw=y)leWqm~$j)%T^7c|D8ND@l<&=b!toLyJ{9 z%|5JJ!oX*ps#iDt1XJsOs}E(&_D#Nmr-GcRW9gd)D`?&NLYi)ylHDJ7zuORED^*a$ zNzILHtgW4pgHwb>$Zv(-mJF^G|) zoXJNC+C9PA?3CJnBOk-mI~O2t6%5% zDTmjc|J&rX1qhNQPP83fPyEEbP*&OoYya~+O|u45pb zU2OP5fzAZPxcBBm`n4`Q3_j`np@jwGhTSJ0&|x#XoS|CdYN~{crQSBS3awyY-shWe zuXeWJ>OVvgO?xgUt$Od7>9=aj->4kdt+c2z61m@i+ib|D7Jk%8Rhx+Bp|Aiuq1L$= zkPYi9=kb$;e8)=g&kHs)2*W;`tz}$Lc>O`hYD!P8W!qp(GQ4L1y4D<{?3kp0lGnlH ze>pTvhU>g0y^`ia-XLIL*jI~syA?=GUHkV$W}{L%*ik0?&qsPC;pQkKqX(;<(bc~c z)PtCGUYV}*TwHgc8i~-e-Y6Gt0+)9<-rk56?cDL3)8$5jTkWKSEcH*#DaSBt?SH%1 zR65!@=G$+W`bA2&gLM)2SZr_!y*mhp5y}+<;^?p|&i|e?G#(x_6l;As^B;G6O8F)g zKJ<4H&obYOR}~LHD6p@@ydx`?SH25trZAARk=1u9d&gW z=$pOgJ5r?Ot)oghyt?&Jm$c0@T=m60*$OT*x+(A;4%_hcwt6uF*UuUouimu3SqdGQ^i``WVRyAGDColvh6 z-qds|BDu>nbs3+1_9r~P%)vrebBvCPbq@+@O)M)*NtzerEVd2kurS4b%TEOurng;- zTA(HLPm;z(fx|q3zs>A+L}NXv!KzQ32c4-AOT$lH82xs)ZHA4l{H{b(nVWKwI}lfs zs+*A(F0YZdAUvgEmutBQPczs}3oUTYvbvJnbLmH(g8Lg&ci#ul$%;`wYE6pnO!hNR zv88CB%zr|K=|k2V6Lk9zs9s;9;SQQXD_udzfg<_Hx#<--)7ZnE$$UeX!w1c=U)`qQ zS8g}pR~84lt*vIeQ%^!ewLAuUN8F%%fMYxL=g;d8kiKa?=BgnVID)bN<=rd8AM$66 zcv>28-knA}j(%lU_DIS;=qO4Z4`VDi@1sIguNf9r>t~DNhdMBDl1^$;*lt4(!%@$U zR_G9T^=hRJdiGTvMhCr%VqKN>KY*wMYvr)W5bk5Y3GGktM1Fm|{(h=1dU$(%Jb$|$ zyuFpc6>{kK)>O*{zMo39;$W?NsV%LdplvJ|GG952Ar$N*ZvXTJhqYM~7naE7;RbfR zBYSMJa0lr zMR65E2|R|jwY)t(GP8eHe}r`fK?$ppdHdR^*c2;OTf?AhL!X>R3|Cxx^?Puc=%};v zapX7a-X*i%HZ~Tw9YCABJzYjTBgb9I?Y9$_Ia#USiWQQ%N&XIZowmW&&_D$%t{1s+p60yy1jX&Y3xo{J? zQ*ZLOB%7Z;jIAG{{??9H9d%1}7l7=f?s9e8E~5%Pfe@%d`TjZ$)g)F>J;tlfU2gOYpBR~0@#AH8^p-$P6z;YMZJ?Hec% z1qEMa2zRXIoO2_?#iimtYF^hjv~c78Y>O+DbErPPi5#)N@vv!kU9I#%AeHrzP{)HU z<3x#}P(DRee+OU2?S^(lcxi)52bymhTL0KdNn>3J<0&dNtA{O%rwa zD|rUN6kRs+hi9Lv|Apc+XbRIL|igXOjKHM#g(}BFuyqjAjv(I(wTgFT;%1pme(0*}Ly8)qMA~lCn zFuBOP8A*a7fz`yJJ>SC$l*B<-;*)eo1TrTyrp~?e#r0@PEVaiH|f!SJO%FxQ>Cte1BiKJ?-3IYxHPw;a-Sy~e+*b50ea~aBU7Q-~a>_mS3 zl_zny?Zho9D@(@16Ml45Du^mhzE?Mvy9+n+iF6y2LE0STS=W!WbvX>newuj_s9&Rh zY@4I!Si2+~>m0}m1GK0bI}8g$wC5DMR%ie1UXi}T&y?l?N)G`1>T$(J8||PK@^%o& zyNvriZ-P2cF5je|ldfv6wz%+mocjX2XgWG$cdx^<<`?1tBKE;?5kLaskqqtzmiKM=3llok7;{@cxtA9qbH9=lQ-lQ$oos zibnE+u`Z-e7JDoCarZuvQY*g{z}jP2W@Kpf>g(Bn5|p$(3fl{F+H zg8Y}^t{)siXIC}dQk~<8XE#ExXrR?{^Sj~d5c$2I{YpB!S{5LvoFg%=EZ6BMWh+O2 zq}|0P^z>r?;A_Sj)$h59@z{yzya{>rD@SaKH}`JKxf-{nS{cZMeQ}t|WA#}0)X_XY z9OXWOt$j5VrTUp@-bk+b(AWI(kI1}ki?_cZ3s=EmI?%HnRw=O~44-ok!A%i@6Tf;~ z4&7gjH=9yV-Q9|qN%3fipkJ+yutIM-%djVyT13IDqMduybhYwTg6huIKTtZUvaOY4 zm7q?mfdOB$u-op7=oce;g7W}e0@L#=)YL6vKJ>y@|1%A$;!H1nk9 zl6Dak{cbFRYVbfEy7J`^J}avIC&ry<90U^F`jpg7XnT$%1FSL zcLQv+67E)&)sWakvI-kU-Pgy=yO*?nA(yB7yDRFaKHY(cf-RGScvn_oFp zI2p6q@BI$$<+&oKJ;sgiYEU5tW9CSw;GaL}jc}lbD+-~?#Tbu)PDj#Ipk3dZ_D*Lj zmSf?t!68e*Go$F;%5%e;^+`P5ODF2h=o9(Y+z`UsDgrMxgxoQ_QMl;wjaVw#F+TQ0 zxnKEPHbW#PTlPqybxPHD$S19*JO+&J5uWmUQ=GOEUH5h;fkj?-M`SWy5S1}G7uMZ; z)x{6KEDeO%!I1_ml$qVmYe?YKMO1rtYqv50l;l z7HKnsCN+~J{Qf_qGNGW(N@Qj}BCBgyl^!*{G|H@t3lGAa+1B}&O)<)RI@*iv$fPRo zk-$&v^*0%^W-?R=QYj!g=AJyv36Kn{eWicNd(Thw-Z#=bn}59fH2r5Eba&v8(qS?d ze!>4I*>fMgZ(GkLYSz}%J`8)8VKZ}{VWTVI8Sj_c{=>_ zd%k$UpLNB(<=VZ+4BSf0`II;PnSsV4ia8Y6n=?b14e_D^40OhzM%cM$}+17!wDF)nQAbOGx-L?Q)Hd zIlu&_4kt0`t*?tAW`-lULX_@aixJuzZ64VPL=a&#-Y2DbfepOJzRmq(*3ly{okHF+ zK97F@u)Xs%p55R#&EA@vO}ao+w70ar$Q@fw1}oNYu*Bx#jZ;%hW>wl1V$qNYQu=H= z9>>tc3BF2%WX8p9d~Colx3~h@K)vwkq@6N}aipDt`%ZCbX<{xeD%*$OMNl$cWEN=V z7!fJmshpl~3uJA54@%0vkdu*h;d%GyFC4Q=g@s#cbqE~Fg&jpV&&6Gqqwbfkh`~XD zp^&QqoH;y0QPCTiOAk!**W9EpZ42qc{>2sR#*S|DcL^Rvv@vgO_x9EbW_1Ik>r%e2t@L%pSeK2EVP7tX4HH7M}t=ABCM0}? zScR>flTQWSRyo1b2gmd=rP6M0Z{HIN!22ybMR|6Bz_@7M3_*mtwhnAsMbVn;%m@ zJq`~?SgdzOOfp8xo`8=o)aoEMXL&jlwpnY}hC>A&KCc&oN>S&MDKQPH2rF$z`WeE; z|F8Uf%79ZyNMdg$wDOFJg*8brrsLCuuti;SGrD@4g=ia4bNmMQT`Mbsb3z_{4_>t- zk9w(12r8*JN!}?7p~wdyolhDQNO-X_WkY+paDh++cs*0Ufu@h#JybChRjj!FM6!p* z9=r=j*FmcT7v=Q()QqaQ)RE=&pQieK=_Lte{(TF;n@UWdKWqKM_$Zo|{GXX-77`LF z(a9Mp8C`eCGiJ z3=SAxeWO~{Y9K`C1R)fFJ%kgzO}l8jAz2=D-=<}KCB=%-B7l6s>uj*pg_;x8^(=!Ow4V?;Et z#3kMzFZWN8@V`ozjpXbpG0~hW5a0yfTQA_a>*OZ~3|oUfFpkvorkJ!&B~QZcpEZf~ z4`cO*+ue^{Lfw0*88X1Z5v2dtLyfd7$XFc{`@{sIn|=cs|8jYd_fLX=s|1klzF#ohgvIC3tSi!4u65GXd?{whm z0wdZFfo4h)a{I%@SgA(~?U6T8nQ1;b=uWQ*MgCdJ0md?~1LLMtxH9Dwm8NTf!6N?F z(Ey&D(rSEBOjXs@@1uV1>FxnI4-kT!9{v@GSedu%sip(g{9?PeW=*8Rj{vCO->d+A z;BCM5391I>?cb~o1JvVo9#ibxSchmymqct5P}~C`lkoWz6o4W}xv8`qf$15hk8N}Z zFGnJVS+-^f;%i$Geo&DQ=-s5GrPF+3(Cyma=mLf)2*sbK{xvZPsl@^$TJ&d`Z{gq0 z?gH(ji0iC?ego{CrGI{wmi!)u*43+&y?o-L4co$V^rj=J*!N2Nbn>u386vQ-{V(Ii zT}PW8Js#`ccRJ7ue8EaRfPw#iT>S-99Zl0U3_}PI+}#p9NO0HS?(XjHt|1WI-QC?a zxVt+93+`^;kmS1G=l^G&6<9gLOm}s4b@i_5+7y)i3QVMBVZ(7j)BCW$s}Tw9bpE7_ zrRXSrBZ@bawFhr4_RTX?-5@AuoW=y}rO@bvq#4UU>rrp4)pp4YBW!f^}(z9 z>C2_8q3z)mvcCwyx{4yezCuW78NerMO8qn+Jmc;f6%Y6-Z&J@Ub)#(e@D;Pn+FmO+ zpOm;z_8cBb(x&y^hj5AmMLH)&VE1DvZ}dTOT6azV~#2&CcdXmNuQVyh#2+#Pcd3R*WF59iLNwT(}74MPh3lSdSJ`4ja2(@1v%s{^3he1-6>o#xEpqw&JphsYSR_!cx^#q zw9VX#*(z^d)q8i@kBiy&@LEl)uGH*7S8t@2Kz9V&W$2k4eWXUB(SdRNyXeJtIYcQ5 z{p+pwPKKf|Nv3vyL7Z77`p8JyIL!EH&c|!$^!8f;WPZ;GQ)1!tjpe=y`kDFqdL1V1 z_7@6*5%tk8(jU&BBX<4FX$(UtVZq3kSyKWD(g&kckw1oh1MRmPgu}2`DL|}Tp-MWZ znrU*^m@u|g}2|vz8AxR`TFDbv3yTb_k!&UZhp#6M#tD8aHCN|U}ZQM z+ivUYR`%qNZ#cY@(D^*)ggm8dJ>h&PA960glGzRUtWrH#eZoqCrtN08oxozTnN|g} zSppdzrJ-5rk7ZLub@^$M&@;eLzPTj-h-}Hb*eC5`bbvv%+L*SA|B*4BJ4b3aT3niN zc9H#%g_>e6@~W&w?YymGwQ5nW*fKA=0()vd^b-e1$!yw>(!V1f@;2hXo;iOhgeXNR z0rfcERP5!L=&-o(vZC=)o1kLnqSAA+;7JL4{BuHBFPhq;es!_8_6tS^H@z>56a3z< zWuQv(g7%2}mSH~kcINO5w>qm~rj&v1xha{mTiJUyGwlf*sXa ztEMXst1|4E0jT{>eX=|4RL}{MTLcK+Kj;qlteP4j>q}inTn=qt(jO7ho6dDog+AjL zasA|3AN_P(OW9~H^X@xG;KlaM!T{JuqpPx~tt$aOpNR#j1@fJ7RRr42)ihY{N4JxG zjLw_h8W;V8^+i3&%~Rn%eadC2J_6NmsSXUc_}C%FkK?VkQ^Kv$QC*E8<(sI=AJ{_H zHg}veHo|7S?YDeB&=QK`x-!q>;R`=bAXj5Z?Cr3SA+4T*m+#{UEsmi*-57rk)UYKJ zc7(~YnXd5Q*2hLs+B?5P!HB}0bLiONEs}%Vzn#T!Eip77maU;SKjf6j`z!YnK<<^7 zo}_>aMTWLE6+o5o0^v+acs5cwNhn4w5Gdn%@_d!+XRv(XhiZ zpRo?|(?Spdd+=TG<9>>P@gTx0Ap=9;1-B-}?u6*6KfIBMF$XMu*MyeMVDvKX_F%nJL-g;u${!cs)y7|c!r}Yt&M1A3jn&yH3AUij5EWRW|7)RD z@>zneULorOqvH+hgPqDYIvGK`AMXx_0wtzdFi7)Ng-(*0wrkQBowa@*QK(~Etf>Z@ zvXd_|W?)To@AfNtSJf;?DFB{zugn1*9p&6%Pc^M%jM{1a9ZkGNZ4ieU;yLsB0<7Nj z_o*Kds?|k_RmTZc^~9mE{2Jes)+3tsZuh(i4AtrHYu^43<=F~Fa$WbwnLFv>3V!o4 zWtZtRhY}dhF*`@r$G|6glxfg~r6@%3VJCDstKl1lxzVbxfuV`2BhGrW|Jt)lC2b}lYiU>U)=m5S`XNhz3W;l&kq++k4t5wrBzb}F zNGE>l9K0dlcNIQ=zmsuzFo_{4u)vt^H(*KAziI}NYmK!?OX$~}zC7P*BZ=s#E>Lvo zNNZkB5V{bIr-!b+URM>URlCjRstOC1zYjkh(Km7bn#=RoO;pd9k@NE%j#D{L*Dx0h zt|j?94QV~mqS1U)3{_QB+PTZ>>N70vVSgcTM{8Mk^uDfX4pcSZeQj-vAN4r480J#R z|Jss^FHjQ{=Q^3XM3b*pA`3GfeqA~eunjZ*tI+qDwlYXG^hV1(d2NW*x@-TrsXmL1 zJ$h+WXEU0_g685CzeAedZntwnD&3jVuZS3wtv znhUtFHhzNUK(D?7ZDu*nj{&qGlo4nlEMHgX93m2 z9$tjS_T!DIZo%g%lgr3S@L^iqxJ!K%6yFHut+u$7Q#z@iBw-7WY~IZRFS2}UuTyH1 z3~!5AHd?5(h*ztJfC88br?LEj94TNgngahnl}Q6sCKpK10Ld+&fjtuu$Zh)khg7Vy zxGQq0N*mfR?~?%U6b7Q?ej& zoalvz{m4)2);mtUr#fZ(9;-!WCpUiIdi$i`+Imb3FKWlvsl<$St9yirk%grBd-GdU z#>lV0peSOY<$>i|oDcn1E@v*BYi7K|S1%feKlNCzQi<+?dL)Tg*`0Sp(Uq2J10(x~ zHsQ?yvgbPVJ`DX%yS_B(8&Jz^p^}FJE~@w~e2)xfyi|ifvOF%PC5#X__Xdd zxf$aaq#fvVZx|%PIWx8%1R^gF$y%Q|U|a2|R*&#G+ifPU!e@W__j7CnpKp~_mb$>= zM|`!Y7{jzjKigFHWXtk~>>6UO%t5WBz^^dXJ;zudPnGP+YBSMp_ibX}(K8=0`9gpi zwqN9l?zgDL@7j&SqZYqIomP5hmIM($+M`ar#HY{zF>0^xisvI8HZr|2b=}j=01_``(*ck^BeQQ(AwES8wU~?%B@a z<>y)uVUA{E1YL_HNJ_%EnsNND8C3?Cy$Js%YY7&q=0ol- zHq)?Ny24hx=DyZ~QFZNmj`7^PUoE%Zt~VrI2(q!J5=XRz>4Le@on@D>?lv{t-35>( zS_(ut97*D`v!>@xzl`zlt?NA`&@k-t$4tO`vONtwvb_emSj4a2l@{xl;Z{^^S6t9@ z;DtBF)^*DoCeB)@Lr{%`YvEzY1TE;j(sUeWl35cfMaItDU!i5mPW;!V# zIb&M8jv%4zb{p>MI;*!WyIDFP&7vR%%b~77SVVjyqC!FfqV=Qw4IbarYJ+PnzjIj6 zb0b7ir{03~XEP-*(e1B`eW>3wTa`Bi_wBN1(I}e~zdfCqv zHXhuGW5rK^MYrUeg%*XOLZO10uBG6RQu-bd?=09l1RN-O+|ZUcud7E~roXKvD#*%Jv zgsdKqO+!9`W!AlIg#}sx7fAmcDCYTSfplKL3uyJydtnjs7N=vNgIzSV^qGVPbkryS z1+oN8&M)r`OMcV39&nI6)c}OXDCK-`H`K6U3~oKeTK(%ft2!#!l+l(j#l_0H3n%nS z24}J?-OW+frTWg>&b2>IZ^+eE0D#Yj{Ow*87pmBhuiT?H(jv}Zmw=e`T1RkK?mHZT zpsMSA`du!eN);A!e4&XrVK2yRzLvz%a- zU}nIO2In}Qvo6HA`ItKa1Mh!0k|`-bq|o@U%}y_G zC&H@3&Z%j#C{&Ty5dV8tqTfI~P!SJM9zsWX^2M7w@9@bBWJ0^lH}L}(BNBKNb;REe z9OTPX9|JkTX48~}VSjc#0BA&H$DYQTMu&D-vi&*8LiewCRP78+qTcbg)R|3E{GWTz zD!x0kteTn5_%*FsrH*0ZdA#+#$e(X;4yc((I2HTvt+*g{hL)ZHj)qC67YpC@){vus zQ{qO(jtI;mrlo@z}^&iBnl8hQyyH)O+gmkL` zg4j1Ud*GV!KkaT_PAdo=VE*eP6J8gH4aRr#a4IPR>E#&z-6juY@G1(_fBHLurkILi z5IhN#Q8P2OLsmc*@TlP1ZV`|h*KexRdSHCte(xrah1CMI36L$IAis&lkxZTb4UoV= z=G!C}$lXL0gTN@59t03Jo0ipsJ3>Oxe~h~V1~dSm3Ot7~m2Yrk!;a*a9Z7UjQdT() z06qTqn47LYm=A;-=37sx!}$h(+OntR=EH-?0L_07{#OEogzlRhE`La8fj0sI1SOBP z5Na=1AfZ~;|0Ie1MuZGpVZvG4?L`y-)iT(B zkB9r$kiHz5YO62Ggn*mz)+O{mAk^DHgZ-P1wx455KmY%9`+xTgc>Q6$4-(muBi{#d z92P^}tLMn(*M@xy~ea{f001b@Xdb?1?_4JgIZ_9GP#Xa!VlWCRun^G7ND)3l%g zFQBH-+uOG;%Muckljn^1L<^=>Rk4zjnk1;z1aoJLls}r*ERqk5n;%pGH@*dks(}=X zueoph*>BSY+wyblm7?GSehq*UD^&2((^CYe9xs2Uq_jLZ|MM3LvPx=V?aPu54lO`B z?f?73GOk#KH}R2#KUmD(j#Kh(f+`XA{0#8xm<)9$jLh+|OXAodC|g z#Q^~nc)~vcjxXB1I>0%Dq$I4e-RJmMwWzWkAb@ow+4gP_9uN6{_m9Q^vWu~Kx;dor#pjPu zSak&{RjDHUne#SQ|6NgYrFq8kFI(0Y@auTBq#@pF3R~54H<*8x!hcUEX05%lD6T(e zAq_mm(f(zsiq7G@`)e&OuwOph5ZqZ;|1L|(I9|K{ za9n_^uuc9!Nl7R@HD2-Xwmkzu;hJ``VZ8YZB#=u&m}kwS^hH|;$hu*72;$kW z{U?XNKi-oew8VS#3|l`XtT&*7R)sbn(ph2R3RcD+D?$<5+dEG*Xq%Jmug*_9$(NQS85DRQ{q-k z31kqy)5{Hf1m`ms-l}Hxc4RB{nkEKUXXPhQe)G_TrGQ3Cp>`1yYq>I(bB?#Mf6WEfI)Faq$q}X z5$k(?8Jje=FQv@&<)|bd6Wcokr(Cqiv%P5wL^QUJj)De3H{_VFvAAbvPH@@_qWdf` z4;a10KF*1C`|f_#>U#wAr`F`rbna9jI8Lj~6?KW2$kLXx zMdhNydU4@iL<)1sH`Aj@RnS7KDs&dd8IgV4M99JHU{8Gq@6Z(^mJ`)NL#nS?a=RaY zXwrYn@6wg>KQT=aE}D2xxN5jYp6>bPexv&2d6Dsyw5QAa4X@NAUcx)z6&=5;mxW-& zyTGG*o=eS8F!mhdbk!B>6m;X4Ujc!^Lh-!oNzVFWB~u3dPo!yo{|K&>5Gg!9NJyXF zlUePH1M`e(6Mb&uh68WQpYX|=~nDc(Qr{~8i*c9UB!8f`U(~&NaAD0 z*>dTG>auxCBk?yO~5d7`r7-S$kPd5YODI%?w$_jvj>Alh-n!>;<)dEV6_ z%{Nyue*9*XgLW(CH|`>4?~B_SI7vxRsA&Y9BS{bLbjoDi`)H+eHEx$}S?-~euFuZv z-L9TQbftRCPiOGc+{xoZczHSzS>vCbZN}NSDlvAeUB!{AxHJS<7#*y74<6i=+IKoQ zpT-d?GhS(LahGL%EB{SdkrG3>R_d6F4z<9x{z2Fi8C&838@qurdA1qaGmBT6*OK{m zWY+VW6ihs8NOeqGJe|V~tkOYoy{GlQNrvpuqLl+#mq6b9jgvEc=a2-*&xWU3|v#b?CKDjZs$hPM~jMkVI%N& zM0T?inIjF5sXZXcj}sN7r@FS%?xRG%oNM^|^**<)wjaIR)0MlDg2iBlr=JnO&aYQ= zRL2QFOgGS7&m;f#AdSK2K}pC79S!j&>aX2s$XHNdPyV*F%mNv3k(T7~c#iROW>1Ra z6#wp1t#cl&%{EjhkD<&cPVCNEKaIDn(brvR^Ff|LK@Aj^)GuR+L)Q^6LKm~rcDIQK zv)|Cc#kDuX`*)}IoA+Ed&JvAIyfw$-U%7`gt*wl<@gnSxC!El3t*a@YRZm=;K6_TU zr!K?a5;(J|qmLTO_aJ^bsS4O{SQ*E-nRdOZsu3-gSfmP$?-)Bq*^6;Gj#=>(z^&V1 zQqqN}kn;F(b(DsxgzAm6S=9)T*&p0ZW3tqu=nA2{+-B;~A!u_1NDL27B1r5G#XcN2 zkZ_(BQy8wQJJW8KnDj|MU=%o(;0MN6TlH~C+>MO6N^=htX(qE#R6Eh_z2LZ-S{^Ik zw(3D6;7w=E?M-<&M`!nMy zwJFB_K}hDQ-<^D8wU&lo3BDf=M3>d zNMfV>cHMXr3yk$kvrp<%}u!A+5RwrINWTx#0N;%X>^Vck zP}SqUPh*AmDty(&B)53W&3@v3!My2LLyPNuFAJN)lp`*(e)me!SV;v{AEpH?!cVp|5+ zDr5LMLP~$785oZ2w1qmvYo4<@6U0n>d*!O6#Zh=#lS_A=bsbobN7%lYyy^{Y*GH}L z>OdW@XOnpJphV;!vqE5WK2Am&ouEA3SzGn;)*jJ(y10SysA{4Kyf$yRrA0<)f=S<6 z^L~~3v7TqxQmuONsMM#e(E-)qjNLt}+?dqDCdZ-A9Yxk`hgxm}9}3 zXf&6V+TLMY_ZFj&7cc!O>k&KJWDYkemLaX$Yn%R#=--ZXoGA|{tRnEn>JZ=%u4w;r zF>|4?#(jN{QwM9Ir1dzRF!VW?cA`!Z+2QcSo*rL`^u@9I^R72T<=rkP z0ugL3hoW%ZSI$H6q9h)TSnsrqhv&EygV2O?iCQLE>Ui)hfBwvZ#lh@6W@#PVyJWFH zT3oe}>6^Q{b-6#TxI3;O4E-(eJ?+6;!+n=m&w_W68NL0Q)B1_V|2S{MhSc_o8o_Qf z(gc<&xMufkZus?LvsgGFh#jhYOB*W^+-STkG%P;t;XNKP=4>#`l?E3ML7@GRjdy6R)Ah@7w?fybT@?5 z(bzu=(tx>_XwfZJZIEacejq{-(i$)%Bao=LqZPBSR;i+h(DEcIIxe(Y++`a{e1>a! z5{>Ad!dzVgDT(CqP8__F#)xdH`w*0S7zERJ_&`*=x|D73+|TK73R%jKwJiF&?Q73b zdAIoEX)nQU>Pzy^Ou3u9P*=cb_6BJ_R7VMZpt_Aq9LO zRI2nL1ojPuDjVPml|JBkqNitIxx(zoDhxJ5#4Ovq5V3K(JK) zET)$X8I~T_0|%vtZ>)0#+Me7I8w1xbnm+SiEkbDG0@cYV(*X7hO3%JX0rE1b2-_rvuzv+~(pDu&Nio6S~T%4X}3<*o=gRzuae#2KhMa9;RxowDU8TZbagT@s%)tq4op2 zW@I-U3Z~y`W2N5bel-N5`bsG47m~SGj}I*w7zPGWaRojpIedIC*KJhkZQn-w%k>pq zk&xEoK*0!d+Zn(noh%vF$25ee0vIL0eVqBVl3D%Gxt**|wEJ#hSZueG(FWV-!GoML*+gg4n4E=!N7MGrJdaQyZtoV-@- z<3zuQEav~<_woOIypxASbX-tW(y$y<8}Sw%kN2q@Ii9^{nG|h`L#?_`r&DS7%4s8<6UA zuL|0X8)MEh2CZyS-nqR5N}Q*SaU4GnESMNu4=0Kmgr+qWZD(p zsl=KnY3B9C*Oau*08JQ|*W|9;bFL56?TwQ-+HKHRpqPri)};sM7fjhg(mt}s%}>eC z5H0iRKOqp;GA8+3WEG$$x>J6B*wZ%`>`N2As7AnSIjX+YFr5@bpDIhoRMh#>zIeqo z^3;PAdS!M)m_tzF-*Wx21%6FPY2){aePYYEwU(9A+|E^b1aPGbLm!VXWp_bP;tZx| z0}P?X`+cMQlu5Up46n2Ss6_k7fF=~(=Hx%k9z>78nk3JRtVwByXTz3K_l2PXj@ zJl@#&O!;RHlAcW!HO5CqeRtNZ>NPAhp+ynKeV^hzZczx(-qRiDsc@b&BRGvTq<#IG zgHEMtq1_y@roXh2R_~t|SekhwU^(6yz8HnI^DC1)KIVyW6NcHCRk)}wGX426!FDiy zC|P>Vu#=5Lr)PdxsIN}HH01?<(JBTL`*73FN>!@DaF`mzS*`Vg;#nU$;j*X3mYgXK z2*KI)X|;Pi!rUa|c9hhaSzs;xUST3>C{tnU(1X@K8iG)K-?ok`w5-$EYDqBY%6s4i zp9lAGuEga`pfA^i1p^N>=xMlA+u^-uN`eckB}9#NQd-r$`)G|! zvVeQl$xrSt>RW~SAV>SSJWe>CX+{E=7pxs;V5H@<6J8S{>BWThn+X|-3_CoSJZV3> zbq$HWQLN{4GBEy-l{ca{bo-DHj=6s}hmz-40>Uikbn+`-8Rfg8v(S@*S1smB#%PY0 z!Q6$D$I5ii69K~J+@>Xd`HegOJF^v!cW0krNhx*=vjz>0?t>dT76PGU9u){BQbZ3% z9m-Q#hjv!_L%@kaQ$m$sn9Z^K=cSFStLRbO6OOjXo7}f@6k6BNfL#VM8TzV)LgG~!zu@}`d&k(LZw0q33ccw^*&QjkfFmON#9H!zWPj4=iJ#X@neRpYCp96r z`TJDE(^>>~e(P}ZnIYl)GDBe|PQxZy7Z}RgkzjtLzdpWsAB#7HN8#+z>L}8Ta?f#^l%r^axaX-DL+?o^;W0 z(-CYHgV@_6U>?sW4mXdrB)WIJewQ?rJ9~l>9_u6D5D3VIKI(QjhI$&?{Sd{J3jDxg zL5hZyj-0Pw_lETtBgXr$i2wU&*eg)fEg$WyH3Rz{gLLjr zNxywm1s89v0J_potARa}?bq+tFgtwXIlej|RBpt`%tqyMOl}&Z0*X16T-QwAwGNa<1cgGwGCyzjhw0I_3JXBQW(mT5TyEZ6{gB*y&MT)*EL72hU=WP%ok4 zOWQ-G3v5GOYDpn{eq+0=V0K>h;xtW_^2CA4Y#}-9ak86y2r|`j=x#AmgzHBkyI9_b zJz|0}pNscO3!^NFx06{7rlYyGGg=Zrj&w|%N4MU!u%faR8Y0_^j@tSVu3$EuP>Bb_ z>S9KcBLB;z(v&N~w-|vdbnl=Ew;@H8WjL04wlLD(fn=E;m0wnh3c`p@*G30GNkUb-q?1C2Ax=p>N71dW^9v+1_z0%uM9pm`9*VbAc?Q2aT z^n0qIxba@5OWd!YB1mC9t;x$M_9HK;2H2^c4|vdR^dB8aPsD}tk<+xj{nuo}y;9>i z;I^Z1)Ejr#gYokirYrRaD}B@vnb>)Rw%{_VOoX+u9T56FT(DBClnd60uA!b(g*(e& z?F?zR*WA+;Es??=>vN8z1`DKO;oTo(oeB;LmuaB=_gTf~wQ~-9K6B2}ePEI)3I5Iy z$Zgo)j>EY?Inz7phNcCVv9WkZoPX33RJC}>sWjeeJK8Z4zPf+c)9@)@gPKNe~Rz$#t*U zlyIwpN@tnBm@|hF`CMZ~ZENi@SZKl_${<8<+K@EUCM@FJNbg)M=zjjQu;}*pr+e#k zXCXhc)_#B=X))Dew;p`usbr#Al0&JZA@#LoCKPnmV60S2(oP41+YGtuCJ#EnI_dLI zjQcLN`1Bb>o(m=jW``^EuyQ=!Idg&Izg4|w3cj| z{-_ILyJr*=Ywy>a5pGcB&KkDlw3~%rR-{@CRp}h)oWz{VzKfSZ`;-#1ApJUIdI|P^ zW#`JBhc>uqY(xp<5NZ!hl8Q$1%OZdED9-vse(LoCbpzd&kbn%XW=p`??owRp(cQbr zB?V?`uHg1-%q2xhqXNC%_(8hM3ZMj0`idGS$GIu~nKCHdOiAi^VZ(eCc@c*-)2&na zYDl3FoT-dTg1XdRLN5m`3ua#lSnB)~cGg08drv5J|De@$y}FL9qnTSRx z8S-S0U|=+l9R;OQdfqdPW9ocDb%wv#4Lrtuh1QUl`Nvgt z#ocz6U0`!mz4NKI6f20rl3;i@tsjvk6#c7Nr6!T>^ye$2&AUU(6?P{i6bO&i*9Rp< zl9M*d>Z1==gTV|Ndm=HhXd6=&->*|VjdeRr+``<_+h=PBnv~h1igkw5`WPalD4PN@ z4-IyT3RjwGs98KauM&hJ7ph=1TR3?&3UQFp<)kzqryWUQmt>7xSnQ!?Qo}}2RfXy~ z`p89*p`j_zK8lh@`&}4=MbA;LGDF5V0;&7GScjfY0RjgB7W+ZjfMCytS;wSjLN|@P_=>DawrmSg z$f@m1*@07JP$4=>_B;k5XFmJrI;a`E0!-wXvVx#;O@+1 z^P`5F@%kFgN&cFr5K12gD0#ozh0EhBOXvB14*M}q8t*2>?^X`m*W#d5vlqQj)pm$J z7bLu7{c9tck$FV(XBC5LX2?!d{U+|x`$YS}5yx4MvA(hzWjx_VG4bT@zauMiCX2lH z=*v-UwhXF?ZuHiMXLQ}gIZJX2BBPq)5!R{j4K6Z|8$VxBnR%4NCIl(a-AG7fG{X64 zmjs%q4a$nim9fOLBSRNGN`WCxxxa^^(-I^Jp7jFq=I!|B50x(ln3rXnTy@T@cr(hs zhHABaXDDm2H!?+DLbskG6%j5j0R3VdP3dn8zN;!=z=7>fYG<;aXz(o0 zb-6fi=#v@L;l;aWFTByNE&N(sHlM_ng46i86h%hu&WxLi_U@Q4FdI}WDo2&FqErbm zTEx|MQ0`*rZ4$8)0mj1jE#v<8C(rjsrg{D;uZEZ9jEiDMUocx?F^LFrm1mUDskkH> zI+fyy)qc2^!)!t@Xh95&4hZ&1-2~Ja$Ohc$`6>^pq>&<-IT2@&v}NLFQL7`@x`A$9&+AuxRcr)^J(+Y0sadcgv=J2YQje;J2;ev(dyw( ztJc*dAsVnQP5U1X7ZMul7*sCI35p>~b-G8*u3U4_@gE#uRR%`lKyg0zSMZB)9H4W2 zY>repa4RL<--f#gc^93xU0{1q>__bk!Jm;Rd(rfh(GL1&gGx_B=DYdy;xA)AwiQFr zk&xYKQi-~?0)Il|L79W!lsFvg@ow?Yeq^%yz6FDekEH2B>_)>Q=@mB|#xap6PNp^s zMb0v;`hTs)UC4k^_0)K^apSUb~RXmV}GpR;nYPA%;|;2zaurc;6D^lUJEw`>0PvsD=dWelcSl(_dGy zuEi*ywi)X^@3C;8-xaA(V@Ph=`3PSoxVA{U{CJFyo(uU{7dq4dP9t3+nV^}X(&ENV z@Vbtym+|5wo5qRDQUTU{v0DiL9<61oRD}J>gICmkZB^-LpgNS;mx@wEPIrezIDR{0 zG*ZC;%lIh9r!UPETjA;KccI2U{yn+i?beQ+*k|;dbCMN=8|}apVJVvyYt~M0Mi9-! zUJB8|6Q>;6i=M)5ohtn84If(;GKVnKTHzGe4n+}ani&Bj;NwfAbu&^95R~l@JP3w_r&+w4c|(%(iJy! zc|}TfDT)~{W(aMx6#-C+%Y}sm6OHVv3TXch978l4%W35iHay;OsOLh>^Q$EeeFg=> z`eBqFCoG@+C%1gBN+0>Ttx^^5sCs|OI8Ch_M+hIZ?R8DYF4wi#^;7;P6rC& z3Y7<^3;mG`wvlVr;NpFerT0ZqcB^zEP9sJ*!67Fj9U<@VN42)g)<|C0{rX})COAr- z+8*1A@=-jEi#28V--disD{Ho2snd1v+_L{AMOEuG8xl!03_O2@4Fg-K|S_0@n^&JHFgoWb)G8B64D1g8{V z7YH)`spy{6swtjZgHJ9r3yrbQ)*M^;9)%B3w^;glbDhq6ZH0q}ptfHcLt;993_>em z_j12f5?6firvY7z;})SXinATXa1=qIEa-wrNBl8;W*%HQ{|6ttct`b^Dsm8dS5ouA z9ZE-|?K9@PcWmw*215tK;jo+}3ESH3dpxg6z0C~yb|)pp=deX$bwOq`r|h9f$6WWv z1>>rYbOa!hypqMgOH;WVcBBLaNdl)KjEJp;4)s~D3 z@vdust&ZI-4nF4+&U|#t9?4)1S*0L2$3pMG=;hSlhCl;iDBYOaw=T7!#we?7ImrtQ zA~JF!qkWZQqE4Udi64J^`q!n|+B~PWcm!QB9vuV-%8v|b{mJypm4SL4IzyZ55wH5= zHX-gzIga%gHf7OqXWO~iHHC^@a%@lcd5CDMMGzTiszi(4gn7Bsq6SPEd9Vw?c*&P^ zu%z&|+I8-pU;8ol7BBc#3t|~yB}5-hX9z)~rTHIfFF(5O*;-V!nW&4x6;5E)>5P&p zmv3k8fr^u6|=3p$iA|-W(;}u>)nh{!Nk49nA@Y+BKk47Rw-cXwm#51o3KCeaIQyX7GxM|Z@-t%u2P-sNt*Ubrd;Ta0!F zWBY5tiC~)Z^!V$V9<613o#{^dF##Hlo&$ejPlcM<*WyO3m0Nk*dM_1WC{F(vecY%R z)+G&Q;%B-@68+STBeK#ICBMYl-0wz|)mLKn9)D#+qMj_Pw?b3*(;be+NLMYjN6iLq zX9R4{{3(1hg-XQ{$@ybB|g%T06Tt1BA5&wkJm1nF@vIUcS8o z-0Y$3XqiN<_B02{(RWNtkz)w+otp0y*jH+GM#lvEcvV()8soR0S{(*DyuQhkGR2ug z#$_xN!R0D_NQ{=31rLaOf`z)FKsl;dsOt|{8OCiU)Gw67BY@lSNRPWAyD4E5m9}|? z=REH#kT}o|SpHGMns^UvDq;RF=FHct5Yjv?P-yTymyn~{eESzxbO1{^Dr(Et_iWNe zu5Jjj^M_LF`z%vQt?*l-c4hH+Db-Fg`Qt0kO!%+tI1LqRU6k!$bnVECWDS*+3_)g{ z_=Qvnte!O>oUL@3ou|ISL0UW^izvoAZn-3~US?D*@`F(3{DIgfg`6eG`-aBLz-cZ; zmY!uh|CJ!6+7Htl)rtzN;=@W$r0Qe!P@HvTciI$_hg%5yGZjcFMz}q8dieSoDS=8Q zT}CmV4%m!Bo!?(-$Zu)$#DmB+#kGYYXf6258g{o$yGuv~*-Rdai>usX{ElJt&BHj- zSiUDBIDV??K~{vE+xC|N6@cihz}UPQR~n(+*wJJ#P!@ksjpSKBQu8T&K9QMkTp>=1bypt@>T94 z^J~|z1J60Wc!wJQ)~&nzYz}Ayw@MP-aKU*~KM4E09ntfKm>2uI$#JJo8jxnEANpN2 znnRD1zq_bf>4cel4I4nrJ#JC0s)AoVe8_3g+;yr_2|1jvBW&lV$(|qI?vH5iSt%i5 zdSX9DGF`8Idct@XThV zDRl9QlAx`~aHSw3K`q9&FrDki5>tk~>JJH=^dPm8POBPu``-?0mKqkao@mW&7q|u5 zuhIUCEq4!(tl2`0r0&zW@u%%j#9NEGpZJ3#RL6+O5NfT|_E?Cn6b|Wv0!zuDJZK?= zeK#<@*423d+^b|`80TU;;mn`jTUxv7Zd#GWXyr#&rC=`#R4yB|=! zjSKr~?|x3yqk2QVo&{Gjke4aGMxD(B zI^TRa_~zPvfP*E14yFEA+8L;Rvx1%Km-?V>p)xIhlVsMows^+5m)bP=Q-`jCE4w8M zhXciouyy=?I~~#r9)?Tj&_Ya4!(Zm6i{2xB_lp%5SA<|Ueh09RN7Tvgac#U$?t7Yy zngG#wmH6dvcCU94aR-JJdr1SrA?mt#;*uYZGmeI@HPx0HY+`!_r#AAH`NQ?`-2`qj zosR{Kq%->bzH$ZK(AnxeYd9qmx|AwY<7!*VY^`s9)!X$PlZN?Ou+=i-2vG(r%jHt& zMhMgX6sx+=6@+m6ULO0iYu)dadWBJ!M@z3~T~Om0N0-**q?-N+40pMJF&;ncuT4j( z^xu;{pNGDf1l1Ij$3%BeW^`;0cbC6g5Ykf%u526nUGU{;W@cAG zuTJ{HJ?Z+mR?m-S{z8mL)UCirI^%t2uf%$64&*+us;lDpa0u>5&mHoVcv4#~3|?v8 zGI&m&TsSWZticKrnur7yPHPf#+4R*N zkD|Ay;}}=9fjJ|e9z-0BS@?9y!0y5=&P=ea;l|C}0>2Id7#VR0LU*{G&F9m~izP*+ zktKl==PlEWwK;MPNV%C4?dhvrwi%>?XOHe>FXw( zS=>6$WIh^nt4Nhx<5#!Nx)-L*I*O8f#tY|c{7DR?paL|&r-ap&K00s7~8#<$_1}^k5y&bF-GYYy~T1yl!ulA zO&sY$9iumyB0~gNltrQw$|ZnM@A3@?MoNWmYz9G6vYO4S!T_{EoMkry(wiZ9zid zdM04X!xg8J_S^B*TxK@PjEGCt$}&yvp9-`Ns#~5WG_^hn1A{g@6$kYS9>n{GEKh{a zw&@+iGmKLn+2y_1$w;_*dB9lIZC#PilWAZGlwM>dc4CeNo4~`8Qs9RZj5TY}HDp_`D8&58M z`A4~Dx^!;CcSC^V`Vb1c9&v>QIcD*uaWoez*`-8|&V-(nPE<+n)kPnJbdyTHiu=?0 zW++^gu(){VXxG&Y{vs*)W9V8_&;$2*68e^*7BwS?K!pe8R3q7oq%F#U3NY$3|vAMQHsk%<}#!#|vI(TiXO-Gx4ImOIHG+v`wC_=`734wVeM@ zI@^(g8Z&+NRAnAY_Bp#g!Y#gEf@c328i*g$t!&h zgHz0(iU}gF53x1jvfs@0S(@Pc1{P1nh^vn0M#K3P2M1;MQZm~<)=dkTMT1yvr8+Pv z>VAI>$k8Sj=z#nL)eWeb9z&c^l|>>$t={c%(k1ON^DD?L=>_c)(c(~vYA=7Y0^c-; znUqc7--zZsMwo=AVr7L%+}rfMd7AptLdIkrCzgEemzr2qANQ+y_qarIA27QY%2^SE zj~-$_XM5*Yv_^Qg>_P0d^2xbQmL$sCuRn)Xjn+{rD>%S2!IaDaFL`m4JR~}tHhLL~ zNIvj2J73i;3TjQTGpFwSYTloFAjCIAmW{D%r5gAFWA6NMpHIlf?QS=?bW4A0C`6bI z?IIYrgN<50&)9$0$)%tm^LkF3d=aN^Jf8tuJHLy*S2V_% za+XNO3K2p2O0Xs-%yQ2!o(Nu}vgm$GA=g0unl~bv6MQwgiIdixu2n8i1{+91C7tAG z36C;wd&DbcYJ#aa^Aay@(g{5;rO}D3BR=^ zCoNM`K1OLyP1{jc%+yqaj?#wE2NIXVbbbnCzFYXs#&NW%S|QA8ii4{fF}S-oXQB^- zDFUJzk8dpRTQ5VRea#ZT3 HK_~wQFp}&O literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_ptp_img1.png b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_ptp_img1.png new file mode 100644 index 0000000000000000000000000000000000000000..5ecad9d06d1ce29c5c6fd4624a44c08f50137216 GIT binary patch literal 163741 zcmYg%1yoy0*ESR@ZpGc*-AfCFqNPxzxCDwza4k)XOM&7Nq(E_ZcPDsohv4q|!+U%0 z_s?2cImwxsGqZi3y-)B56*){aQZzU?IL!C*G9TgKkWk>@5FB12Km8NkKkxGNg$R7} zUgPCc^L}X-^z@s=SytOw-QL{U&DaS5XJKb=3*Z2nIspK7z%TaBhX^f_aB$Rc?`5Pl z+*9@z+!6>jt{;yt2UE(uhA;GuwwvVD^Nrv;IY|k^%Z&MH;$*VavfhVr#8H)X|MA{` zr@R*;fy-ue`0eFbltJ8nL++4xi`4K;_dz@7M?|R-J+J)~7Kk`sg8_7Nvl%?+dEh!n zjJ;Ys6z#I0tv2Bw@`Vu=p&;DH*cGzM#)Gn)dERGEaK2E2N?nYS(Pp6Fo zEgjO~RHE*!qJ{?jZM_%xXh@doKYK>teOOKbtVGg2CSv+2f`s zfdsgJThxAUwe6iRRhp@(VBuzzWyzXuc`;k;D7&|0(7Uw;&j{LoTRB2)ap-RS)jifs z-OSluTXG-fS%kHn zR#TD`=0|+c{M<()fiCh#mqr4}+lO((9LcDy@v%)z5qfkHU1bKa_?bpIzoVU<9ecJl z1EO4Lh>D3(k1N!K}%A9tt_Nkdftc_I^BuS^9IYk+CFz4s9KsU z8UJ^$^6KJ{j%{cSR3Hu@;PuhPZM3`uawsCHVJ@L4ISj3cNkv$i-HZOg+wEpWHKbIs9PGBB;lv-Ha-uQ+5O z!*stGk5S&zL$v4qT(mfi|NDuml$mASPJT-6`7k_GIT3`px~O$f#Lln{0s$&X*8f~NPv!~`-6c&U-+#X zlZJi>%&sZRar)S?Ya~MNs_o-9cqsvYIa3V4Ml8Aak??96adE@O$Oqb}R!8!8k(6z7 z_K;~~)BRx+M|7e2z$l5auLoiCY%vy#+wHNDDRO=V-ya{&A%o1J%gEJ+$a?CR-nR?N zhzJ3jA>;Zn?;J-z&tKrz?eXgSSF?)SY{ zyk}ov6=XN;h4C4}y4;A=0f6$$F7-P1QtW;U$H~J-Wo|A|&<4)w7)u`W@~T{k@gJ(G z7I%X9;V%oo;J-ZH8?GiLFU-$3w=qr_R(Vw;`r>t!^;I*g^pcX_zQE=CFs1QVEPEK0F&=>~^IC600;D?{`^a}iOtwvF42f02fqK;zMcH-pyth6$${Xq@L zHi3ITBOCX)QNP*D*82B<`SzOob>N+n-azK>S-I<+DkG$=aY>o%*moYX0+V?>>^*X> z(=<$UpmZ*UPhBu2AMU|~>fHnZQ+&b&2dlL-%FrpfQu26%{Wmh~xud|?U$HomGC@q@ z7)D}}cX5jR#z~3sUln8g*pwc1*Q}}XW!pJTTc25YV*Q+a=NX8S3{^#?l(VT z%@yENIE|Nn?VYV_JUzhU4WMfMV^DY1tQqOBC>C4&Egu$nXVxAy_V3MUZQ!J)KBrdwOP&Z-t&!d-)Vmk{zL^_xW+DTpFR^` zH&c4JfzEdfUltykki6CP^Gy;MntkZ#zAquQ%6a>W z5?n9zFl4A_A}EyNgJNAzF=TvupxWJxVgk220<42G>f%lFI^^)CiPvUR0LQeHxFmuZ z?DZgrlzQLdXQJ+h@92p3z_J&iFV}ZZ5l*+~`xiD|9}5QhvPmT!o9O zqbj8*OrOh(2M(#(!pEPiqP5qh&RlkeZsXO)SsQBqpsBg`(E9ZePsp9D|HuX#n=8=; zrM9~*+=6$f)c6b&ju@|4mRhg>R+YmH%$E#?W?Bh1K7^YjYggob$3QOytpjqIF5`={ z+OLjEQilyoZc>cpz{9_zJa7z$cd;dg=1w(NAt(5Jgheci0^LT_h3xa+WB@#53afb# ztO%s)TZzZ}^j`>ALyY`;)Sg#FrJ{BY9rq%QEeP&|w@Vf{m1V2R3tvJZfT|rg`TzKk zgJg~MW;%RXwqVz_oAwN|D}le1&dsmtTOUZ2H{Daf?vn?|nvb0fQb5*_Q|2gcK;(bD zm8F`LI{!zF&bkKF)~ea_!io=3NXzeW)3i!vA(2P2dF73r&UBRF;)zxS=htW<`*k;L zUsdS1{c<&*$Ky~4L zX@z8c(X#K*T)p8AMLc22WEBsZ%Ixx}1nk!O1x*Ik!P_1@ctBSTXN5)o!+R3&}jW10PT1;|zR> zLLHAt@7T!&3F2{sr()B@e5F!P`2Jj5+CNkbh+JXMOSIE*h{YsrpeUN($NzAQJPTuw zYvGBh3Z#5%?7%IDp|nYM8je)@#VUcgz) z_pp3Cq*7d1S$R?4-`}g*^Q|7EZDtAOGy@&m+(E^J%N*-fm(bA+YOnxsaWkfTlsq~X(t2Q=mLd?ieX4hJ0Ws?JP3^6` zGzH^)I2Ra)X{{XcaEl;clLPlPBJ!ld9mHmH+U~aEHm$}PdgLak7|X9u1r6jQi$#2C z{gwI^HtmcJqK1GMy$-Y*KhTfdq(TN%cS2-Q?Uyc87nfdj5cSgzomAYUS~gAqUXi7_ z`va`eMe_FuHnr+-6iO~D9GE$S#}6;vj-kfqAs$2$y>(DuC1x%XYW`%^C9Iej^-H6H zw3c8p13e{86-Gjyd}ve8Kxo9KP?I-?q&K&vA^XbLj`|mU)T@~Za@0k~H~zX_oL$JW z|0sr;n>mK=Fu5~%audc6t|P;XP%<`?Xwk(fs{9@|=pk+Hkym zBP8U;zqQj-aG4)%LQOZ_+E^DE!@b)GK}|^h6M#o@j;i2Dr9H2-`h9COK7_(#!B~_c;S~pB=)`>8 z!(~o_h6qo%!cWD;+BEdEp_O@Eh{YpDXz^RG=<_j?Fql6pc*9A|{@AZOYoz@|1!iBmu&9bIG z*Be^O5mEB=Nl!Mv+SPn*jWBp}=}4ku%l2}!_kJ$BKZJ>w7d<`5KK@e_1xBgk@Z;4V z6C6{gcfg%yI5Ll0KSk8+Ko$cr@FYeC84a&u%W9Y9lG@HUe9rU{Lo`?OtmX7zEj3(} zJ00iU=(Z5qPiHRcm3!AFt*(CL z<$J43ZhhvPl}wRa`TO@wnJWevYx8Oul^WEyE3On1;0_83!%n#=9zEIAUps%n1Pn;!rrwJOJ;Bd*!@ zU})bCPZI=Q<)w&FlQ5%SB=w%VQGms9bXwo~HGn6pGwQqiD-m@PEq~5%SMYp3gaMMi z7~(nWv9@>z^blbfJ5eP!w_J%VNHxa07+^qOm@>IvcV(0Nn!B;1r>7)Be5F5=YUGx) z>E7yvrgmrd{(DSrK)1I}1i|?m{hXU(sBPv}VWgzjo0sXB;HfERuQS&9MHZ+Fy?&X4 zBp>%}DwPNWS-a7lrOETMvwDwX5n9eJzE;L_3fz6Dc&p1o5Ma;Y#LGohF$Exobtv1#1Q3Qe|Vh) zFOF?$-(pmlk+-A+O0_#a$g+3XI}aRZ^byXbyn6wCbR*F0m17=~JN6oPe}oYMUlU(F z_%+rt2lOcw3?BSzpGu19IHF!jcGvugMayHcLYzTYqyoukh#kjxpVO^o&XnXE%i!AgkyI-Oyz3YW$d3?BLS{IC`xUuwN&N)9f~*| zAb=9i!=CWS5n&s%ltW=@X{oIVgy%xgKCiwsb2&`k%)>;+ctQ#Viwy&J8Mi8+UpX4N z{#!_D?qplrWP#R41I>3(&V^D6l({l>V;*!eL_(`CvV?%tBW``kt zu*)4u1jRT`8a}qri+!=ZL6-jjK>oZ*K-Vs`C`&QkZaN|ya@cb$LjTsVPp{5wHsw>Y zh=sAhU2}uQQ1&;UZsO8EmwJK;W^nu@uaM9|u*6xd@j7)PLe7NT<^MpvJLXDJVdBtt zPxhvaooue$(Z-Nh+gL;!Fp^+KNc}S;&?B!GtW}Yl(%Y*UwaahG(Ys3A|L+XW+?U%;NS?+qwVOJOu4|&ixY6#$P-MYipx_)QV9{ zV68dddBULbyv3}sgF+{f#bbe4SiV|iaeQv9slvM@X2k_aG|4Mba#q%ck3~Vh7 zM?*p(4kiym7kSB?g`c!sni6nm8k3W)5Jw^gxF4)fNZB_8$z3 zlW!Cx;&~<{b9wp%NSZWWkMQLIW(&}cF=gLf<|ObIjGxe)fcDxN4ugV#svXbj6xrm#EHTJc_@)1UsJ!HtG2bA ztLE^34HqV7%k7ALm(P-|V&|3jI!~*?R=j4jtE#@xy)T+U>AuCQ_V%E`ksPJR@%bY0 z#_y6DbR>5$rE8DG_EM8m7!+-&yRb->vUqn5@mu5sas$AQm*?2WE?w2dz-_$Hx;X%6 zQI!#Z1)3_9wmVE_NJjtHU`6fQlTDGISv4GcrUlxHR54Tu;C3V_i9%hDKPR$4cdqF( zYgKT-Rp-E(js^KnylHqT&HPb$-s$+yKyHGo0^2v=;A3niOmtOP8_KU=4?!X%A{^Jc z;h$jWD~R7iM_JE97~gP0r@h~;7+j7@Q}X=09wWIAMhv4eEe8kY*C(RK#~XMXSK!hO zUiVzje{>YBdGH`dQfZKc~#Oy3;On zRj8r{BAQMk{oy|!4{R5i_5bp6-`vg>B+L1B%_Y?e;_~axXD#Iug6=)m7o4@6$@ZqE}xY%R-L(qeoL_#1iHY36I-i?A2_XSc)%< zPy)7;Cf&q+_c0p@B*P3Jj-h7*e6KiCA@z5aQ@KS4?;g%_rTq033SNM8arCeFAHB_y zI)>}w6mQIN^cxaBk<1iD9@HeXDb{+K1v>t93VGwq)!ar$Kas%&1*~M=#cXUB+}#Su zU5#cVBa7@HA^<73-Y!s#kyK?bFYL{+7*j^YJYrh z-0xQGJpP#7-kraz8D?t*)hmqB3+1jeh8y4?)UXAW@qZ=z4s4Sk5dkt`$VH|3uNMF zt?1sBg<-Gb5k%H5TTELk&&poU-w zxIVsTF=^n&T@oQuA`m^|P?a&;`+8c+{Y&?okW3s(Puv>)>C}>gFRQ*{)Pkab{#V_S zLhvc%p9!cd#T>6U;_a))aTZFyh0Gg=E!8mn@J&}1tIvt>+&FbVZVvR=ADGktTx=d0 z8n#+?oRT6LCW}GZaP&n$atA&kB#Qks*3=eyK9Z@y6)ROOCj-#0g`nEa2*dH*YwdgN zkAC58Ibkhlv9tSB9i-oNlkI(^0qgrkP zFvwsa&S3&pXDpoL<15X8ky~#L9`5bowjKtOuXf>$r+ltnVXNH!?CKh^>pf06EdXuD zZI&H_`(@E#~fxz-WpYan&^t6e-j-Yv-keqXl}?h|tEPl^c-(^-vYGgkTg zdI72r=oZ2OTm8AUdqS%4wju|!*1|bWZ*2|I^H^w;!-I2on;kISiu^{sZR&1P|Q>${n-R6ijqO~%$=$6cTro}xW&&id=uCsWOR3vP?; zrm1$TgG~WHA3KTIS*1eS$s<>+(Fa<6JgW>|`0R13o-2Q*v8NZOR;Exb3q@>fFNvSK5rd{qQ~k;C@wLJx%lnncS83 z&B@UGos{rKq|nJ+2w#fN+@Xz_1rzNTR*n80K()=oO=> zqr$z^^ZBk))NJgPqDqDsv^0MMirFonHyo}tLoZj8z*QMG*KRgj?xcRVyRXMgC|8V- zZ6h0glKCZuSKUw?JoKz7k@0-3@a($Y85Q%$Fe8UODImo@T0SqY5e=ePTJI-6k%Sj> zMq(;X2j^3yO5Mn0<~T2=C(MWh!K+s%16-%=m4aO3Z~v_Yfb|UzoA>`};d2B^pYW#a^+i? zIa<7rncL2<`8}~Xkmxg#GnrrjN7%Nf=j7CmK`xZ!ddQifUi0SUEmGJ_3CDQL%dtoxvRnxHyETu=K!Feeuo% zVMy1Tm|_@-DLm6lbcB&AsFbi(L9l&>#QLqKjDDQ`pZ0`xC!{q0{Dd|aOjO9S$4C~p z57XdpfNRZvHW85A9}>>fH!p13deYn1mti|stsN)J{OU#P;pKRfm)qTgS)!%-+_Ag3 zz@|BT1#MsQ6fE2edC2L+3>nV~#+p*|Dj!G+tN zDAw#!c;H2QOL(P2d4XRNw5n6Bt)+}Z8eGTxp)d%(;0{B-<~ z?@G=sOy;JBOF)2L`^2boyXlznKPW32A&IH)H0xzIx)tUZ#rsXjJkCQZ+^a71q`b6**n2MXg#>{H-`t() zqQcxv$`+Las4p}#1> z@2by+ntD=-SgqF68MWqOQc~deF^o~Mu}CB97qeB?F>h;=kx=xx5=FK=-3?9>U>@$A z+7C)&jm)D7@q6EGKEmqY9Q%bd@N|!;iNLo;3+}ck9|$g^)deWic30pTtCMt8CA--S z3)A@zIxJS572N4rLzvHbNpP#l201*WKrqkRxI7HKv#7h3;&{tL+RVV z3rtCDa_+DG2*})5O_$6pEbvAr>xRIm$dbRl+LZNeg(TN!>CUV7mvdLAJr3?|4B}@B z$S8&qUzG;h!3Q`6L2sDBHVwaG8BlxA5gZjXKP^LsXucqA@tPZs*(LR98ETX_Iqa#!?@!CtI^l>DaJ-RN6!(GnrQ+H=;6Va~Nw zYtF1bhfqLvNY(Bl51dNSH;1ojOpEREJq;>gvYVSYf3)AeCl{ThEH5QT=V}HfMcXtp zE?p@a8p~fQHS2xGYMrYgZ^Abpb(#ZWw&AM?`$G-=QV$jyjAsP9PqpivNdl>SuUs1f z|A~0yfz@TYJbmdY82A8yl09)^=hd6Pqh2{d-$dcg!%!|yrWZrB$o_aepVh(Ew#3pM7wSS@W+%aOc6 z13@U?J|u06-;{OrDtbjDYg6=aGkP;%OYV%^7gqEETX%eM-Zsv8*vuL+QjI0Et9R$P z!!L+^-w|kcU?z9tMR_GUx#U}amAcD{m?8`M8I;~iV^r9$ zMCD2@H==q)5wI~NjYp+K7%NzKx8GHXROS`a&u63Sh-EcjF_~|T(6H;({)}bQ8q2ZO z6)ZGVjii(y7Ip5O;v)R#d~@yptiIzvJ9&)?&7h0FhANPB`{x=f$Fr*20%6%xRAIBuaia6Dl&(e@4a@M}wg(DiXdC71~DuBb6CGov{lYBn0ah{F|LDVC0klfHT zG8Z8o>8aJB<(Q}AYp}w@{kq?_#|gZ%a_9&1lP~dV@}8qm?>Zaij&ZhN3%5-D`X2zD z^YWMKhp1KqK^)P?(roAC9G^5QgP{+%hK#CdgRTAu@K#V4@tt0}ng;{ZPo1gM(kQL8 zriX?nuqNw>&xm8>Cf1q6Pdty|+Pu0wgk30t=}315A95d}-rv9N8;NU?K%pFqf`R+j zf@nngmJPqawcM?}7k7RH-EC~3g~dvPgZ)-Qg7rzA`>J>^SD;V>r3X6I(`n3-kH*vq z(3mLv+w!I6n1riqjf1NWvQ9WAjb7hGN$S~lq4f{$)3PasPF#`q_a7-E{96_tQvA;A z!g{Cg^pfYz4{Pp~6M27>K`38niW-3y;2Sm8Qq8MV2bTo+lH^?aJ~#VrTc&C2`-i?& zi5yMwI7U5_M<2bPmnwaD2&`K>$^Fw!W8ceFcY?iUNwhPdh7wS>yT~+sI0YnG*z-WR z+StfF9`T7hfeNuwk3@Am-U%^=i~KmIQU&xgl&n^Yd{r!#3iRQ1HIuI@qgmjy%rx?i z@z$EP1#%cfm<)v=A1mzxs# zEU;3ok|Zie`eWHh_`l#(wA^H_`1g7rG@UG&JzTJ=Sn?(7mUzY(Yw2}MUke(HHr%z4 zHE;genATsCY9K2%H`8L_7Qtbl>4Aj%HgAG@&-f? zvOtgvT+nGkeUalyGEU|39oo!7i#5G%OXAo~C2abESiLN0J5s9eY-TqYC$oo#$Z3=Y zUea&x(fzLCyc279f$^EJ`_xL&h=Z+eI6d`q`bZfC`*Lc{l2JS4^Ml`= zq0hMhD=S-~xCbvBw)hbo$qF|$8;2P52$!zzbXcrzzk2Sg8RN)F@(lE}21PL#}`? z)zmYso$b{--#~IyVr8E$3sG@ipke$ylLqy=j_)M(C)ssXVbWq0YFB^PwQB(xKHfmr zH#e)@3o*$AKYaXH-o;2EC3;R;u3e4UZ8Ts12@gIDW<2MrZ&`*it!pnomV$~Ou^Ovo zP_*#ZDB$AGP*$@DYgc5Zwf!~$P!j1snL@8KS|+C4F4Ndwze>$Uzb&u|YtONE{hq?+ zH{g{|5H-(E>k>dFVOam9}4xu*b9m4Hq_+>bUBAq5;81KaO>=Ywe|)%rDDpB!vS zKo(Jhl!#%+9}kUG-BJI;j+v&b%S}v6_cZvh){NN^DDw*IJk{Q`YEOLcN#CAtTTt4W zv7*do1aeQN3QraznoimoC$0x;HrXitw-EiJ$u?>EqAClWb1Rz|f9`>a$OP#jRKn!L zC4`Az1Y`(;6Nrz|x`OUi%AwW>L8z*2|7AX|kDs7xZeom&YC0;l5~ys*{@Cy+4Kg!#MKz5x1j(2qP9R0&gNE-nTM4j(OLy?-(2 z-~O5?C=D*VZRG&PC;bGQIk1z^D8`Y-unScyu_lg8m_f3qnC20pHw7bYladPi0{Wu! zd^Gpr(Y4FJ&`5Y*E*{7Hm&mbvPSpJVL?Z@u@qi6&Pgl# zjN}bon??&2#Owpb`<*F+(e!oA>9J2}X!bA%2Z!nQn5=u6@f}Uz{>9;c37!j#CyjXF zp4M}By;Q!{aQ&30+7I&B=V5YinP^Cc{!zCMItAYRSDAi_E+nELray_y1O zSi@-&$$ns8&wYy?M{Qo}-peKqZ%HaiFm{jQOHLBF0k28li`#oW$F9;;(QCrs;`Jt| zB)#Yp+<36QG+FU1d{G>wVRFn8N=v>7+>D8YTOI}-K=#nHGaR2w($>a0ATw7=x*;>pD3Dj+-uL)z8Bi=vF zEOmANFl+j-9x;3B-CV?BNNjuzRQE73__W~j#-s9ZEU=AEuFNeSBW3o!?$s+{GbD}w zvEf8LTAu#p4?Z|h=e;lJ z#NbLinoGBsxl&iPwI}%?zQBADtZN>msUdyA9U)dPMZ);*Q{U$}?^BWzi5nKf12@)O z%86L#0otb9j60_jEcegPM7&l5>Jx>U{d8Dt^k{r+^O7;;k>B_7x!FYoN!Pm_sKoB%8K1l68m{k|%A&vC!r zm&h&W+>yRV?Bfj&5vE=_xVk48HB!yNWi9uZStsnYY*L$5P)Vc38&(W>C(nqD^_Yw7 z)?%}jGnmKe+~liI-SDAj-_rK@Ho3dcAl93aGLd5?V3^c+i>dm=*Hr01X;YD#Bbrwl z-ktaAOY{zBKT_&c83|vTVA?DB{G3@BUcQvFR_%)~IqeUdDa=1=jhD`iVsf0l!ny%z zJWGVwkscAkbVSFj%kFfgC4RN(X?y5+Z*MPsT99#=8~}h6XA1(X)EyM3RX>ZYUBRbB z(0K(S!b3nJhyx4DZdf5}06uW+FqG-~B4&eps)7siokHQ}g2j@VS{pHA+Gw&3>$FDH z`vA-YX0mur3-!$hayKQSu>~~Ep~S917?n|$7pD}}FWI$rv*fb~5Q*0wH}AMDt#ymZ zlZ!pkb7dM{Y+<^0T3QEr6a?*!M>@B%8Mck_24daM8rk1{hZ&@X^PLc@P$fjF>%U2q z3bplidS>WM*zP3a4aQneBoiB3C==N8#auND>ueBGig$P4Uel!{J(RN0FZ@}wD>XxT zQDQLV3z7X_`=xUNO96l`T^yUYm*NSAjd5r-Tr=m9b+`)oG)<*U5)GA1aL#{3%s*mB zjfYD%)#E>9{|?@5wj6Fwl7K5R9;GX7cdpTHxYMc6Gk|1xr!6bj47)*L+j1?wGlMra zSOv8ATd~MdRp>MFUhk?^JJ>j~lc=AZ>Tl~n9L)u#XNzMd=Acg$*5;0EiV?NLYBNWJ zSA1>kk=Z@ zQ6`^ryZK0cHKu#VTj(dpo1_S>;M;E2B6If<*`?#hC3j}g=MLTGgOz%}Wx@T1sE@F3 zTH7txmM*>SdxdIP5;9VdI~0IqeKZ@MVLjarmW)R5^c~n$UF^DPeoG>0f(U8=2;Jj7 zCy{?o>Onh38hN}Bt_NcjE~=F|V8>@H6)Yz%{?`B4tIPP;8dX?J(KH0ev&KCvfi zA#Pz}*o=Dw+)r#%tN#d8#yLhoLCzx6u18IoZ)g1GD=iu!5hImyi+;hy4jUk%+N8Qv zwa}@uH0MSisFu$m5R~j$xWZMHxToAjyV$UZVLVN)P%&z`aOp|?CXHc^nhY&2 z`P|C7;oUdGlrwbih{!Me3jCx+L%b77|MM1e#RQ3QAfkI#9I7>u@Lnk&+(VffAMc6l&+V)lSv9#>zYWs7Lge_PvM<>8+Bnl) zgzQU4n`yu}69hR^!|Wzq+@&eCCw){u@py-6PBH zw+G1>7^JFzu4l=SdY!erJd*L2#I=iHj(<7`m#*6PqZu7r9A|cd@MM>@&dHF4cN4F| z?Y+uIDWzpN#4R-8bjIn${%jP1{D*pHj!DE``2t1HFfK2!n^fL!uV!$07uCt;oA`)8 ztlT1OcE5z52n8(<0R$8m8E;jXoD%zBhd$D6Ke4|1@Ca&5J%Yw)DQqr%y6=YMk{xH=i}w>cIdv7?yC+dxXG(v{(Ckd)DW_eXjjICZY-`{hCO>I8PN>OM zooEuah2a!leTz@KdI85I5qY7H0-PYpn$A^x7b7stsM)_$$9HuXt{~ouR+W8Ni!V~C zm|ko(O=PjrQ*!j)@&%iQ{Nr0j$IjkHvueZ7LRh5GPz#I3zU0tbr%lN$VaGQlCBxnz zCJQFeH&^Z$S1-U+Q&^=PIX5r3jxThJ+Z?VgFSst&I^nS`key4BUUgu%Z!1`Clxd=0 zi{_a#$_nT|t~ZlQNSYq~JJ<63TC0@ymwdXP-h}!@69IwDix}ef_s;+8b9JbE&eVXR zm+;&c!{ljR2O>{w7K`K5M|%xPlOW+Xg2QK0`EA(ZD+b6^J`p@#@lj9IaUk^*=9wvS z_sTQ4Vs~p|H%{0`0B;|N|d^sO6#B?Ev;QJfn8*FkJU*F^j&#`j|j}0s9>YROZ zPI-PvdxSnI`C$t*;`-KkaN1GKOz&BpsZ2J*-26t)hXiMf{`^_)A~O8#cDShLIs-5R z00e{$^z;VzW-2@`W-Jo9jbChUZ%cill|s((M6mA1_{CweIS|@Aa|k`tGi$=}foGm8*p_M6 zTB4*_!X7r9nkekJij}fDtbTt#cLz)SA>$)*M(jb$6lvGt`lar9JZ+IlRE*YZ6@#|o zv}Z5SOKD&5k9N>D;Kvd?mGAVb_F;3d+2x}=-~OlFAztB)_UlWaJj>*8h@wK@P~YO@ zIL-_s=#pgW3z#S1>8w?1H>5y2i3{iLOGUDxG+Sw{=&C{)mtB?lY$-nvpp-r)95;AU zJ}se~f<~48SAT+Ev^Hsp1xo?P(jC$ z^cS2`8J8E>IQ4)*uS;7=gU`~T9nJ=*hLV?5IX=}?i zbYG}Y}eRnxv;oMTcRs_f_xL)24bibRtyfXj9Kq zRs2}S_q@Q+@n$=B?eez9x6PbWj5YF!e!YFBqGitd3>EQqv>Z6pc%eLvZtxVR;~o~n z@1DApbeSgQdx$%Y+Iu({AbU{(S>sg>-UzF;*B`=oyi8xr1;pi|AL!+rkWtoey?79i z3N*%7g_ZZEKGpe~U+6+kN(vZxPJfBjp3c*l-KWp1Bp0p_Ib4F~tf!v!ObiSVxRV>s z%%qikSjT@CNh)h+X9qg{r%=!`Ra1aIQPM}Wr11pJ?{1CiDTAjGT2jD5a4;)K@pVQ~ zs7w_l65+;z*TNE)iepZu=Psg(p1KzITiX3S#8JJ3HN5Q#fdX-AaJHEDwfmeS7$ubt`|U zk0KX}Ta9dn)12AsUlfmevh-N+(Yw><+WpQ1=D!Zr3gG@7DitBBPUXee+;J676wLQ^ zcr-Al1Y-?U>^lu3*~)gf$yH5st4C~Gy6a6Rz0j$uanGO_vN$sxSyfG3aKJr8nog&u zu$bCD-@&A8W1h;c^o*R+o;hdIJYL*z9ks0yp#lm4NS%yNr{AD9qMO8R>4~a z{TL(im~-us>V9UE58TntDjHv=e)xmDK8(v^6TdVRiYk*j+_0mI`%F0R@nGlwtq2ZG)3~2CrU_6M}((@k>Qnqf@*3lmM6CzPW ziVP<=?Iu9Ut;};UHJudPk~YxLu|8g??L(p8%Dh;(tM|1FUtQ$!)ChSfIMcOcMXAty zAY5PSbm8jZdja~@ovhGVVqlJrT2+sO0h{gn^+nmcqx%Lu=zV*QzJhUaMTX5Olz6dR zm@|$=Z;>~q#FzNT+p27g(+0PlfrTotNmo5t0`%d#7Q+5vvA#v%Bb&Y2V~yHf-5s$# z(nJ^IzG&cuVdvgPl=o<<@VsPC^$#iDvLz2Zm}*vmmU$O}`)$LcT8|2R6Ght8&(jK^ zfF}z0{PFvK*DSZgypck7s`=!Z4h9J^BuoV1kCo~A#f+aYuD<`nTK&g9(b%K=&@0!6 zK<}NpUnPdD?>9OaOr(eb)7am;-b7CG1sLwe?=z2jqnqEwY=JLm|C;68h)ye5M{7R|~u;X`3 zO2EL{nnP~M?*>m5SpSZW&3g=^&iR!}jtqnywJasHlIF5sGhX#P+1W+g9~yWk@(ttr z<6dLh+a2z&`QF?9TMN*af|(oSR|fw+YLs|L0=3XECZFN#L!Ff**yovS|D)Z@V|tdO z5LY!dwG>g8kf)3!OpJJ5I76hv&}qfVjCZlCC~s$@bB4_Fu9hxsgqdRKUfD-4LdpC} zk@Gs?BgW@$Z(~`vL-(rh5|3GL=cMl(7u+E}+d^1C&aROBa37^M>aWA7k2wIERc_eo zVTF9ga8& zQ*G=aM{G>p8RXNEm3;a@YKy00$&H2^zr%`=aNU8WjX@Y1rTHJn8hOnjmU9#PG@xj! z#eygA-tNX;UG&*+&i#KB@qZhBkoA9uMJeT*Qd!CW^g6bFx&R71V^L_1N0o_QbE1-t zkZynd%py?f)T%3?@_-N`FxVi~1QN=&F@ly2&|3l=2IFw8EFEQ*BIsrk6qRz}nJ<<6 zKONLd34QLq`GJN;DkKzUJROEUaUc}d=b~U5cAdpom6}VVvxgQ_Vi%z*?onMCoKeiRLwq>XJfP13lrZQC}U*ftwCwr$&P zV;hZa`#ed%zw@t?@;@T0~Bw`1jw0#xv<1?2m=;5sf=KL~Jb(mHpHj4PUm zT<&TQedxSqQan+CzY4!4aY`t#$G%cZg=g0wr- zzTtotz&t{YNhdt%7PYih((o}+0^@TL$_~{?vsyl5i(~5j!#r<`vU4v5bzEw(^U(A% z`e0O#KBzoa&VVVHgpx09d6J7qnVlH+AuiL?4oFP2SURj^q-m7vPV78jSQ{(qHKni6;gDQTx9!mgx40O81a0(iT=#J z{RjseoBo2X*qL`DM|f#Wi@9()54pLub$*4=H{1%#JjYB-+-HBmPxf?oLUQdvCY9Pq zb}0YxbO!qaJQTrq%#k>*`_wus^SO$?aVFT`^NO2k4r6fle+w7yM^lGGBc`!h7f>yd zIo#xny#zdt9kM8eryI46rWg7p)29Q1P;g|k@8%_IA5*LoC3P2MC=#qd+gR-&!L{vJ zP87>(yIYOFk~C6n#ol3nuphS{O`ODrTm$65xcQf6SXXtby^=p5HgR8*9IOeW~pO{Il#W z$3z%lM^fm63Ai=ILN|Gv2drsbO=U_JcTL=CqMKj{pKrGmdyqlPY!Ftb z_Kzi)k>US0$W5JHJHLH$W@L1xE}Vyc&A`MpUEbk7>%_VX^(w`x&O8 zIffAa&{>Ah*;!dhF?@^=4xwcuRPWT2ll{MElv+hE~-Axq>Di?P8;z~#g&hg*}9QFtD@i^efs+b zZScuFsI3cz=q_4urvCl=o5itA0M%*9?kvbGJ-5buBc6Y{e|s7Q1-^p4Ed{^UUA`j# zA=a<6t9$5m(!t{Acf)@AS5Blbx=zb*mc`o>!Gbq5=|}Jk=9^1)uLn<-d3l1rC>GSw z65f@1qdamCl@EGLXy{Clv8l{eM;_0yYMJlyo}rqI6ZPwaG9|PCK&IB66^;w!Cc*%+ zB9CByVC)x_4wA*4WGUXgM5?2EV^TTU)P~|F#3y&n8ec9^h&0?3 zcdIJy^+|CP{_XcN_0U$Hf-EWoL*gZ)$BjlL3Fx1}hZ=j1HEp|Q-HmPk26t66+(8U) z!9=&U1xn~p{UC{c~e4TG6q7`Qw)wYdKU_g*=`T!pGRlx z%c<+IIBTAO>Z`*|Bdp$#8E0I`l^3sU9nLSt@}TD^fE zR8dO#A0b^3AySmvY`2749FGRp+T98M{PA95O1w+-rZ?8g-ivGTcq`x?e zGF2NGV63SWgmaTz6H+u@C|c9EB;e7cmLz(+?g!hyoL%qRHVEmCt2JLlGLd>pzQS7X zq{Lft-mUM>q_a0mY&Gj(Z~i_PTKp!yB^@2)JACC=^tXt`)6Hr_Y?Zr~Eg|>ZeTn46 zAS>8!zRKTuPBvUa21g>W%bG?H1eS%FzQB?MmqGZIP33feMg>l{si%$kM^gO3;O;`2 zbu&{A3w-|GHOveyLC*$HhrVg0F1z#5aM9v2TXhXaWUI*3%kWm-?xSGZZtLFh!~?8h zFSS~#-248&jn(yK3D4tK8f(=slgMrRi@reVRhD6r5bFX2fq^Vf!9n0qBu7z^t zTZ9c3HXqkyLqw&^bJba1ck+@yA6j#j5K z6Jr*1}n5g|D6sCe5~fS$x8<7jev%t`5(WCt5DN zSzKP#zlz%tY;Ehwb1t#J`^(E^WjSyuX35HBjxG{oU&o}mHSb03x@SYV`^pr*7sf?5 ztn;ocQ3?1y!D#M&dv)b&zUn9pFM*w5v~HzOTKDq8Fw>a+8B3oRwU)imF{h=xf(%2)%U8$*{nX+m9INs~ zrz`8tK2PPP^2_8vIoj;?Rn@388JUwhGn5^OyYvwqqKD z6^ros-h{8iTRXDC9o2Sy4Z17phhec5xdrr(G8CXmaDM!Kp$pBaNl8Hws!%8`2Sm_v z`t$iP8eoUrQ4@Zr3*yRlfAClpG`7m@|8pM8SUMBOf_>+j)x#tu1n$t{Uiho8!V~LG zUn<#a(q`PVIMK<5>`%fY-pik%T8DnXOfiM#V)w?=D9l{5{8RprjU_?hrXsCijpHUI z<2iX`gm(AJm638EpP4?ktIV(ZJk3Td+|Z7J4k7;{JIf!Hb9TLVvzcs}N6*V;hs|wv zj5PDt1O4K``X>VGClq<#V{U0{?HsM@a2OERb9>LJ*TY$Obh@MW-7{N7NG_tZ9Ugwf zjD;m!RAfd_;r)1*HIM1P6@Y%V)Yx}<1d5(~md*M7#nwubt^Z`M$knWh&JGqVm}-jJ zrPN(xy^4ODXBZP={_5U-f04bmE5Ox=YH0 z4!hk^BFlc`Gy9?ER85_Ng6X_1XRv25z4b6C{9Ah(Qhg1cM`xVWWcIu-;jB$&aADzn z*t#w8CWyq^<>r2#E*KXM!>ca7LLN2e7b+7-6Bc`lMuCV%)_Z467sbN1m-NjMF!zaU zNvh#TIKzt_{#6yV_qC7vLfBVc#}JgIndI7>MpHsWC1w(0>g zhd!5wH6_ZFH`NKy5oeF5lI3x(E*;h2>la(4O(A#RI?X&VGL%o5WSp9)ktBL9M_H(S zWL0xba0^@YP79ESyKwFpSI}=7+ua$=mPC2RRcLOy!UT}$=#5N#O_{kabW2}~s~>e$ zhs#aL9IqKApRECpr?c5m{R9XlxmEsU!>~~?nh-0{Xctog`)v621V@oHOPI^dP2HFS zaC$;(Kd=n(Ei!FGCxNxE%>}L{{4u$GPkFl6iAJG=t*djuKa`R>*kOR&jgY8=hksKG zVODhekEXoZXNz`Q;@qnumU+{?-KBTDVM#y7tiT32gyBf)vo z3@!lB!|=TmZj*4^ef0lCX(bvJ@LnV@pPMoHXJeEAI1`XNdQR~QxPZrgnYQM9yiAtQ z+Y42M4%V7TDz%0`-W<`Y=rHXuLFgMFiZOSj%t>wi`DLBi$kd+3 zLLm}4!Ezd|-9RjDVXP)l!xbPZF|@e$1ur&Aa{Fey66Ib~;8~m_r!TO^a&GWAm$h9Z zv43hx6xRIZA*=-ZAt6s zyPsdOyL-p#ITuC=0&$VIKqL+P0MT3P(8n(Dh=R_9uZA+LhKG~2_`N%}gc+d!g>Ey2 zde&)Wc*&U8Y^RZ~GGb$49fBIhxX{_JO)|+1HlHdz;=z3K&DqQv0KuKP_}3%{oEP3y zZaWf?5w6KjrOWa!TcZ~Bp269%?iZV+!e#B))8=FN-`h_u-S{2g?;A|TAB+?ZUVBV@ z;7!)|T)i)_xlh!w8M<2UXsEbX6Qd0{CLKQc;I|@oaOw=eBGro_Z!J6(t67t0;cB_Z zcmQ5M`w%QXIz{xRY!I2#C|nr1Q8-6rvuF` z(HE6qSpC5PJRA<(22NU|`P)(T-tW}VJBA0s;8}VVEaO+IA3rroB>s>qkW9|`A@CDy zx!nNf)t9NZf<~j}=luF{6d@Phgsc);Zb?oFMbNbaEF?HEjGQgfKl}T;h6Ltds(jF( zxLj=7I4WP~{cS&ks|}tSg3U6|qWz=%nmc4=NdIvG61Go4cD5n4RIcGlf@^d=)JG5p zUBQdA_3A{o zWXiw6KnVy4B(pnGBqSuj;&Wos&{XnQ5>j8XgBxoR_bo+OKPKijueOJ&77A;c{2rfg z4&l;KPJE|k-}JC4t6JPE57jzY{l@d*emL5R|q`DPk2X%R+|YW~3~oRo@EBwFCSPH* zpM7Jnu^8Pau@R$z*$L>V+XEq@(&?;0t-(<7o~1Ti zoMlDBXz!fjQi5u#egT}MTCFi0fyX&0(AyYxZml&Lr#G7++uYo|Kkr3YtTBQCVj+y6bz__zkY zg(8<*oj~?}DPNuEc$@F9jv-XNL44i)axxBl8N@(I-~5tqh7CDw!#71aYp~1{3{WmbI-D#wEhYycW4@KGM*@Y5m+s~O9Mk*?5TSr^MEMYj!G%d+o z=#MQG^hG7Zy}r64;^a&N#(En^N5jCd&&Jt#yml3sz7ms`HU=J>PfAXnEK_56KPM52 zMhW5qJU8!p)_w<4q*93wB!)habZ^-3C12=9xb;E0a&3(|EcAnw20TwsrR&BiB#3s! zi90hS=(T3a_Z7hp8jM#MUge}rwrUJK9C}RIXK-_@aV{@L-nE$ww-l2Ur7~3+tIabi z$J?5~_Gs!Su63}J-{=#mzB&d8)^F9C>^|vz+CVxpn-=Sw6OX|+0ah`>KSKGxXnh`%@2z|ipk?aC921vbA1$r6shzCK&e z1WXsunNEBKR-_W6#Oum#qE8>oR3)~?cLpDxAz5e64K%7=+NIF&l-)%C*xHLb;3k5i za`<^r(?NwBvtK_H3BL=;6aWh+#`I!xrc60q!DJlCi8`%zPe3i&c4YA?n~coCVi|IN zJGE$DK+2Nwe+k3B+9KYKzd&^$CLv*Py*~*7>|>$OXtioOT>u9>_N{+^zVQhHLaxol zwm>FRI+@-mi4lMt85#L-IK0x0F7p>1@|7)JQhbqPI+P#?EUP&0_ZjcITA(W_(c|sL z#DExdn%LU8rFyhP)7vQUp;f3IfFnQCmss+ZN?K;q<>gZ{8vQV4z%fD)>h;ZzM{`ta zHQT;^-IBMrx00#M5kOY2*6)V~et+z5w|_F5y>W*>A4q#UL!}~Zytm~bmhef)mU;6D7i2!8t0ieTqaq0dGXtYdz0vh!v!iB>pk!&nDxU{Wg$YsXM3`&$M1bcaT zrr@oG6e3K8bBiO&{>!2EF_5nlL8+9f`lhBX6LHuAg=V2j4?LxSYvJ1zJB;?>c5lMj zcr_tmyklu(t<`MrY@RdKz(kAQ^(PS+!N$ zHZF`~YiP6#3^a$A9~vhBF{)dE!1=)P)ae>-_i&6&rfWNk(ZiekB>ZX8Vx*OT?Sr*}fvH&_})- zh!&VtAoxflfAcOYE59g{g;+-DB%!8e8s9<_>Rwp^6EVR6{s5EB|L#Nq-7LL-Zqz+O zbwhRc(m+Ac@IQIu=Ol!#)A(@3J{|;0;G!O+&FRRWoC<+=fsB$B+?JYBhz)Il{r9Rq zlrh+@{(RN!^m8YDdWA&G9(kypb_t}FGU4oW4d9UbN>OR);hP&RB_+jNBq^n<*{Gc9 z?CGP)1H&3$GTdk+me7_@@9+BghROiU4MU#OoCZso{TXMIyVl zkrnD)U8~#DQ&ZuXJ8SkE;<>g`tDP`&F~c%o>#&4@8e8o-cvy2FGnbn|`vOz|{D7tQ zjVwayh$h!eeQU@aj$7eB28>z8jCL=5pqna^Mk;=2wk*~YN2P30wHE(-Hy<~)6j z8msA^20M{@U~HIff8++Y(=99+2VtUeHkQ7BK~#Q*sYm~WY;P;3KEF1>A+(twT(>)4 z-XZgz#Ph{M4!X814yG*Wl506;v{+{gZ7GqCrcMFl!2knTcA#wz^oIohGXF!qT1#b0 zAS%n0LXBsn7Jim?wJ3XmeRHmW#oL29<)>kMR@8U2LwCskwrERbe|a=i#{^hh+e_dc z4(VkMG&Nb{eC@}7#W<$2MUqg;x7%v8IGc+>aP(h?Y0qu7-X(Sn9$aua0cRWKU$FZ0 zSWzN8#WaC6B?kvE^^qb^i?^oWLiA0=rgcFl+^bLo|{OC%)*)_9a4fCxp`&*rouO4cZ*e1`habE$hpQMN44dMYk`itVi0JT;nvyvapvyCSkyK%lEjyVgVbonLsZV2bkF1fN zQ!Ja;^!uMq=fm3g+essOdU(ihBWvPxx+Ik!idJLAV}FYqfy2}{J4-UoZD&_kZ>SpV z=F!alNvKc+;?QMbQ_o~AhLL)YA^jhjNkXb#gF!voh zObO%aW4DI(-@sEDIvU`zqTMVpV2r7BPhKhe%+XORr{c0$o-S)L!PyzD2<}5d?49g( zhv2LVL6f%Bi^*4^+J%C}oyZJfmN?xnw~jO7RUmuKoT=4%J1W89VX zL(zFo;1Nw(`ZXwCZ8k3j4s69>~(L$wUX7e zpQ4}+3r%r-g-#ej2v&i?uso>zKL`1T-O`(Si?HdEf?#5_i}9CV*(7V#BV?njgd~_e z1@jjc1ZIV)H0m=L_QvGK27^PzIfZ9c|IWMpCw#FoP|m{H!1k&J!MB*O2SAu+OiBU} zT1^Zoz>ddJH?H!u@}uH*u5G@6nLRyH9$p!?NZT(G4cC@#UQ~=>dvcBUxm<8Mx2hgQ zf8qX0+6tR#s+allD#MW0{&&)23a?-rsFXs;>Kzlf$f(KF119W#H)3xy-qpi;7T`Ec ziQ#d~!>!E5&ar>Cp4h6PrM2+JIQ%!QpvC5NBWac4I@%@^1Q%e1vpMA8tbH&|L98gn zHz17gn8z9#@+JX@x*$Cq(eYrh!+x^b<1`uAnqF4u5QC9)=l_o4%CN|ILOJ-rBn3qt zEm)xXcYiayT>tBC&yU@R*BDDVVMeC-BdA-OU3FOU6u@1-48C(r2MOJ^_iS z&EFX)njzvnKOyGOc{iE(6C!tVcT_~jJ;rJ?Ie1=5E^r_9XbDEvMnZl9Plt(b9c0Cz zdd3?(jU$@7RC%-5MTco_e@kqmF}>^PC@m3hfeZ~qgNn@5-&=QAZSX?5XX0aE5U8yK z>l~ocFo1^CN=rXjYGx?@6H-*7z8}T+<83tT!|IV~0t37oH!kbj1A&Elb7E3b6sHqx z;9vgQmEJ`xKBT?Y*=E>s#S{SHX`G&_PvGpArsk5{bCyIPBpRLK;2N~JT0MthjchkZ z)|wzg5YD+NSiE4-7m#(=rDf?a7?VDh{(4lZwW0O)!SaLHKi7VzMQr4mcq^y_Kp4E# zx;6uAADW750icDj1?=*ytq%B4v7qx9$yJ6-N3Zu89q|sln3P^i0(B*;SQx5@OvW#d z9Hk~Vbb($Wgch(;%s25TG(ywid41Y?XJ_6v=pY)L5~@3PUT{(jh5B!qs-0#?Jf0kZ zXPpGXL>ymVh+BW;8{!(6ZLJv7Q@-A(2cA6{Kw?H(ZmOoGoJ2ISIS$h3?+lTIo0~y( z+D;+a+1~bk_kFB1n?lQBSUwcuG^T zdxo}A2JE%%PpaQt!_1|gXmn=wl`v#q5Zr82h`$0vRiRXiOMD;n4$|TCbH2-(Dl3|4I!zp(T6nG7i8%eUShNR=1`AlH!Z} zDp54UrFNAaC`yby3(w92ugx3JD}qZ(Cqn)XDVq-9?#(mO3fFqS5*JYM2#;u$%ZJF& z+1)7=WdqpLC?2=Q6}VO!#8 z##-lSBamnjnQwn!9^V}}{S-$HAC@Oi?X&(?fwkQ6qju<-ABC%hl{Fg^5V8)2#}t0r zlcsXb#MBhcd#jCfAxXp_5I7=G9wp8NmzC3Oo-QbbpotAyMJE>B0iXGY~)? zHSKfZ0NYt`QsBLBCI9-VNWB>u4*W%QwG~^DD1|OJ!YnRx=_3^9=k$kWEN4{+LGCAT zF`En-M$&9Y9!sQA-VDRT2oa<6fmeTUpYCfke0^g29{a2Msii^1B`|z_N`^7zHwBVf;sj1!80_|>tm$991iBH0g+o>IQ}Z&%?vNi2j`7BUb*$I z;YqrYiAI{lc>x<5!vs!0Z_zpYqBsC0@n?HST(PP^KAfcZh|+{6ubq$gcRT80c~DkY=y5}0UFGQV!MzW`V0kg7SMUhI&wY6 zB#Rn255|>T^Yrb+Tq(;6VjOHXJD>Ph4rr1dpdbsNdpheIo$1_UAFdy%B+Q!R!2yw5 z&|7`V;qfB(#rTKnc*pRCDZ6{yQJY|8hgQ*1(xl9yl?UfJvkuM5*u#P$vXZ-aOeEp5 zPfJ;H9f|tIq$14jxOz`P?*_46kVGJ)!~VKj=!*<%moa%n~gaJ*^C+3B{QX6 zC2>9}r4_L_KpPBG8;rif2$njbGQb#}UARnWWC<7-eIe}Iidi-fkuDsZD`yPu&Ha4t zFd!-Sl?im`82^y1HDxJ%F-%`n*xJ&+1b_=2H89qT)4nF@W&9uUVCCO&YVlM%N~WaG zuDxC!TGX!1>g2-KtU{}1+@BI|$m*`bdh()iUHuhFoztjLX$Q{l;+&JJi10n2YIXHt zG~?Rbfdxf58x~*|+S$JKs9`2f(MpSZe0JRg$^FGF!;>r&9_lW}rF|tze{H9_uEs-i z2ReF3eF5zm+}v1ey#~OgR0@$r1|6C+eDBs77HO`hQl!~`Ry{M92dEvKs(jZheZ33S zp34ID-dTWtgsphxde!D9%(F@^;lZHSIPwju>9`Cfbt?Do2)dVIRW-E&nYc1JNL>lS zH1l0wl9hp=vZ!IMVPO#T1oS4i_}Z|Jd z>Rm$ooJlL^|C8p}5s+|U0l9g?46zDEJXYTVaQkKnCPOO-BH$o8+7e+`PhiYR81Nek z3Mx3fGBS=rDW$5aw6A~l`;Q+LKPgdiY?43~ixl&Vjy@$>4?RTs%qF5 zk@EvsuF{4AR%%Z+m4-43aM%KhWJBT}3<1KANl(=z^Y%<_P+4Rv#$_ zaWDR^kt&)Wr+uBbN+pT_ki%JXkgs~#F5kJXHgx&Tz&OTE^9r1$R&b#^24emxcHr;~ z&M5j%ZJB9$y0=xd_J`GyeCRT*ChWK?&U!ps^O9Zjy@|qT*l3PyG6naYSgW3>(Pyt? z;d9btLRwM(dr7pEUVzU-JQo!Z!ungikoF1H@C!`;^lma7lnS2q=tTb6G`GmtR)(5mLa|b;$6|q-~@+7C{GT|O)FzA*rI}MO-CIp8K zSR>)dXR-%}9@?G<_F&Ax7-ZWLo!7c2_$k*Qms;!)73%9^)dp7WvX0*<@R*-N%ZjvFt53$a|;^3 zN_hboq=PU^wR588*enC^*mG|^Ip+&JjkPh~GZ;&s@E??YX3EH?K9NI(5VFmE%OIAu zu5O4Ts6YqGa25?ZZ(`-^cj3{sr}mSnF^|Z~P2NW8FKQwrCkIif@ z@-t-mALFDMdS}r0DiT5~L|3rw@9l}WxUg;wMD)(jC+hVEZJeII@_JfwwMU2@b@pjm zkIh&qoZDQJ$-2Xg^A3^MF0$FviC|w7bPIX~-3A zrA6P;Qc6_xb9Z+)l}^XA4miKm^a9=^%ZoSuJ0kM=@z&02rK3BbJ`*JodAuny6Ipht zv&%6l-ql|yEOM%N90W{@C-+b33npz5Pr5n!-dLWAy{1RW#+h|uc|OD%jng25CSA7d2!&(u>J&=Ph!yE@)vvs^Kxy}YYut1=HuSh>}_~mhP#!MBu zgZp>0OT-e^Mz)(f!j|^uU5=w@3pKVlGiLgok5sh`B#Sdbg;u*bIgRzT+Y8rSX^Qam zXeui^I|>Sl)+aP*!oFG&{;nR9Bec*Q#qAAUMMc!aA6hw1Crc05Y)QtF1{WP9eZ&J* zqz*Khm8qgBgz>Q(qyXh&a%;xZE1!5(`HE#&c*p0YUrL>{cZ11o>PH6Lhip+i-X3t3 zPfQFzRM@4g@aWR-4T(n4Mq8gx=hBh_5csV6wbs8yhz_SbEIfx!j>mlX&Muwi-I{m` zZV>L;rZc!0w+1I3=r^uTUwq!8kbFM@C2KuwnCvNs$N$ zqQcDS#1n24-V4vrqWoQ?vA~@KGH;OoU=#rp`hk2mBsH5X*nF<5xZUBylFrsX}V2Xv@6cu{!8j`ZDpLyGP z*78e6Ut~Uvvs@CYP_`W!52wC_-N}+tMW7a+0@DaJwT+$KKq0@;@v(i!0nizVP7m;r zsXSKrRT~^g5240MA%qRzV72<_>qN;soqXYR1$xZhnYvwP2(o6*6%Zp$LZIik8)vrY zJM=sjXn>T*QZpE8m}6yYpFSu`xfn|);9q0}M@*tHm%y=+Wn^{IHS#A+U!2|~ zYUOKtJy$aixEpS1-Bvr6<-zh1cnQ;e(71DWwvfo)Q(}43l%1$a+7h*4-`!cYqabgC zBUFT71wk+AQ}*V**C*9jGnc>M!B|)tp{#ToHSwOsACc_n0!m0qrBVdyB8~yWWE@QT#fOw za?=0t{~LJaQK(ak%wz|+!(OFe%yVXdc?@@rKsXM*(s$K};U9~xceXvm@_O*%XqbH= zw@e+{kqZ9Vlkt6Qw@B`2A8#V##?foz_obD~8rtT8jpqdp`ZY}UYH)oK)+M(;`+V;( zeZsW&=vFD#a!4sl#%yn?p3p6q)HN#${%}h_#^?R1SbR9Jg>xrY{AnuVe|}m3A_5`9 zrx9Gf1Tp@uw~REstanB)`Q6hU`~{NX2C#rbYA*5}(lfGk%m2jAB1cbFlN_lt;4Iif zK=qps8a`+_R>!u)K%I@C+#&t#UUb?SM zQYL`6^g7&U3ZA;9v0u>02>(@}1m*tVgBibZGkgjU3)|Wq6$J^!pow-&Efcz1f7e)k zAAUTO#Z*qwk;~VAz$BW5y7wB*rrQ7SlP1)ub63w2c$cfM3(@|H6`Oag;<^9BANf27 z`Yd+7HG0)VeTRi3?cz6J>kCO&!yyaMAuw9YcucChZ(WxtQlq0yV6nY!A5g-I0xRu3$E6?)5VfK``a<8SUo|OG+-c zJY?j}r}ENOD^im;?Up-9%ieaBXx%w1fvzA~f!MLaj;~c6_FZV0=IuRy3;*4Z1Ezcz zhmSk*$GG>ADIAk&%$(P>ae1-&gr$cl=VxT3WpF!cG;aZ7mFvtzo`J1WkQ)a^iaWk3 z@CEt*y&$1m_p9fEjn`^jv=U`!w)LB@!^u)a6)nRKGtR?9Fl&`Ff?Rd=wd9e~YLlQK zJ3*jNxeT0?pNQw4fJPcj#u1uZP#`vgZ-15kL!#Lcu=&DN_x273@Ss>lyR$%^SWpzM z!kz(qu)jas;_-TC;#7=2EIfEY6=F`VzNn=5_X-J*QYe3g0{q#MWsK+`favG_S8IT0 zMHVd+lN6lPlJ6cH66~J;Qxa&o%7W%PmCjt*9u5`61N6W9eV%1m$)*}m!5~TRpy_}_ zN>E;Z^y%iboY&~WEz^;32mbsGxy#AoDI$C$myG7RP?La zk?g+hZi@QU0?!T#gQ+{5y7TZ z#Yu6P5hm;|sI5?0I1VDFOzZ)0;la8Xukh_ zsO!L#P*N5_bcX@N ziOk2j_@mD(8&mWtQ)GhOwTvPn+V9g3Yxfyl_Z$pbw+~5oVrFm;Y`DY!=*TGwm5o>h zU_4P(uaOfqXJ(8Z%g;VSgB(sW3$~e%{*(Q3TrD_RD3f@IXA@^()cv+Ps?n;zx)u4h z)sr2Kxn$R9(RzQLtfDH1J=C;eQU_pNIMrzdHt7KN6Z`q-2P^w|0XM~I<-!$aQlnH( z)E+zKr^{u%cDDJ+FmI>w11wZ|>Dg}ElXpQ3Yh?S=mkMljITTg4OEuf|*%2)TNOF_Q z8E=!xQa(BZHjZ7LLB}wA-M<#v=^POOB@s(kELSgc$huJLZ+1Vgf`l7n?bOD`oX`Fa zKrEA$zp9c|lfPhk^}!maI}yL+EU|=|w^wStw_6cZ_wPiM#goy9D!_Awy@Bwx^gNxJ zpXghxB4#ZN6@R*b1qb(R=O@65OZVdnF8Y+^Bct<@GJTdQ7W(HwZJ9tKsk30@A@iB) zlG0R{^>gHq_!-BPC;Pm%T$jpIEbHMtJ~d_}Th-3q+$VowaK!|J#l}FiGfTbxlvU2> zu4Y)X>Us0Fxu>O%d#Cc)fD+Fv#Q_iRi~8wts|3Uz!O_pLr07#5uTJHJ2)q(s_nKeI zo0WT%P^a|PVfbeo)bp*>H-pS0dlD!%)k7Z}aM|u|M}qSyKa#Q~Io?3>$k$+karqxg z3ID?aKqRJ(Bvs(k)ZYMpu%7@x9(69Lq9hlGpRTwKg#cu1y@fV|MW4I)7CV&M9;TK( zT7HD_i(sw>Np8o%&}`>r(_z}mo==0}d!D#AoQ@61!y}6KbcLkZKIpL8>MEWbG?Ul5 z99}RS7*QE6PYpKh^kIYFEn@zyF=dp^$S3;eJ?diwci>SaXXt4`5F8lr+#*u+ zm^7j8`4!WNs*;XnDR~4xSh71}5&6j!GMH^Ue`dV@+=~7l50OB2lMl_h<|wedn~?JTVC=xxjjVM6ca;!*aPhwv)q`je4d&zJ#|proxzN8y9bD zdSsW8sUUr4QfZ!-)9lSq9~J@5Q?7T!z4sazm|7K&emQstldP~1h_NeKsL@Zz{CZB=Dgny>1%bM- z&V|5y-ThbVH~g(?2DiDF@2uEWS_Viwoh~qM+HQzGNvB*PKbCqs+PEy#TO<6S1OGJy z%|Hcn^2nGCIAvLZwo{MXYu;WnEX>UiF*E&E-hGvbad3d8N|kU(H;2A8?9QsH)N)K=}$AoMxxf${b!kW~Um($U@^ehyhdb9>pHNIc;)xm8xUw zyY^yB;+>98?;mIBGFO*zdzZ`VY0t$5%4#L6y!#dMlTcfDt|t=3#EH$9t3p zP(-o2zRv`3ygisMe-zrh#USRM!O9XwM_Nz5D5G2I?NrbgI2F4ZCK{Y=^iw`6+|P1p zzV-=IFjEsvvqzBYRfq)8pGGD&85lyC<2MJjDx+3kEjwiiufusD7s52L@cnILfI?lfjMM{LD&g$?eWHelw9Kz>T!;sNCb; zzS!~`bk)BoC@~Yoz?q}Bt`GX&0z2|GIpWwBHAX@6iyl4#o#3>$A#~XS&`gZck4-Z5)_bT-PF03lT4fZ`ey`BhcII<;|iBQP?{@v~CPV#aB?aq*+yt7xghXEJJ`=}2>jx*Je% zU+feItJe~+RCn$N^~$Pdj{yIC)Hsj;BmlF%c-n6@5vsox@g%$i(rf^bje!@zx^$8j5iLxg6Qz~mEIVxy0RjSrn-hFIyDTNlzBc5NZPV!aWNN;UUT%KFU$E$nZ3YOh*jLG(- zG@EN^frcHAG-ICi?ROtnzvpeF$bZ>yc@UQ&P*(EI2DnYxu@BW5SV!fvC{Ob|EK zJ1Ob9Ut+_vTL{Imr)4^g`oJAIeq&kp)F1T^&kj%t^e0J>r=UY-c@@WRCW+p|@EAjA zg|@{s*ynPB{J-e>%CI=LElMQ8B>{qKu;A_*+}&M*dvJ%~9yB;1xVyVGZXwWUBOTmn z+QKJI;y^%K}VQaPZ7n>2AjWs{Re@zhj5i*uv zx4O$ObleQ6nfMJRT0yaR&ygj5$tjRb)z{K>vuQ2M#JefAd$GDdvhJkv^tqRFW;+-5 zHTh{$g@?blaO^Y2{ZF)Uvl~C`KU=t2uj=-YWMN^*EG2_gU>6#asr~!DK^-2$Ashec za}r_o^DPM5jvJX)0Qu8#iqa1?#AS%y!!qW2+pl+#Mk7^Ffyz?cW>`fhN8iA}mHJO{ z-dd(4QmNg#rp`_C#sliD=oY~jO1LgJx+A*u^R%j)c1sK))%!j;?KB!rXkZ2zWJSjgpiI(9)1gCZmiUFN^ zgI0R@@9fpDs|~~v9wxBHg;($jZ@W~DpRk&7()F59?yh=s#Z(U^{2lO^y)-X~YCj=K z6VdWN2C&NzM~8n=m;2;2ves<*Q(p7);wPzRSZk9p4@!_k$SeKNbi-)5s6xAu1fa=6 zsA0|%o|^J_>e8O*hd;Wu;5v0V_I#V)K}Mg={Z%}%wQjWEdi^Ot&emc6&pfw}L2nId zVcSC&r?j+GdlTbmPv0QSrYU!MSr10BkkQvCf;C5()7Cs_$@nFabUa!gmIL5FU8-rLCUw+ST+g%};p`r;{3R{BWP`PJSDl z#^-NCN~>S}=!I#>a%=5UwasQQ6-i&W^`}pC7LXDtI<5kG7V!<9)*RW^#~-!D zQa7FpSE3188gHJ`tJjRBtPFdsQvvnd3J=br{!h5QlZ@!)knZ_av+DcK+vWL-*bI-3g=J4$ggcc<`aKDJJT7GemxD% z1v%AOnBH(0DlXlU0phf_NSpnl0w)(=a#lyh(cbuPb7wq~33p5hU;~a_{Ns#(%l_K! z>6%_&kkidFL|9hZAe)KCu*Kf0#x(KG@v^(v{*ln8k=RGAn3M{+WTg_b3KQ!m=|p17D%-g0iNBBQsjzNWSim)Gy+D5`WnN;(Q;o_d)=p z;XWH|N`ShG)OmDBC0(Np8#uX(VBxNIe>}SGVGzUISWaGL173~l-~seAjRkVP zn~%7EIT)?=k(h%y+xyw@a$mfk3<5ZSJKqqb(ne@yo~`h)H~9!^_9%KcYd0DK$Zr;$ zHGT8ePZC%AZ7T(=?kP%15k{w(*@~>9PucoZVg^00hFUYIZnTE0s&*xG&CF5)%qXpg zrKbYh6Ye^tvxs1djl!U4n>4GMUEuHh+^;ikN(V%0Tum>Wo_S+8*eZ_a8{hF4?_mo_ z2Pjw`X0PDopJQg5Me!$O~8&1g0~nRNBrdu=(@6p1ieo@|7}#(1hDRfFCC zWw#~oq>dci`a`FwZW+GR2Z_v+4rt`XUExdN{&MmVZu&xsO`5k={i*(RYh;BFvUnln z3!4l@dcR|puh*W-;_HPHXEPSV=QjHmvAid{FrkLcWLivzY=Bx78WBqMP#tusZt@U> z><>|NgWD^{NbK2CV9m}|BHSwf1OSkmbUl)qAW|kM<_CS()FdS}dLo~lHXS@N)*)3F zA@W}DFmhYEuZ(>wgseLQZs5~x@bwnurDOzuB=5E4)Q8yjS!=;mEqB{ZPM*Aos(2}W zX3F^~3&sUciRQm1Be~)=?o~j+^HyC9&0$ofe5op3xm7CUN**^2%-d^1tXqa~JN##_ zWtn-6kvVLBq|QEB9jTV=Tm7x){wBe%${DZCNjR3wFNyrgimXpu)Sa1dLQgOy`zVm{ z?D-SRawTA78JBf0yGbT_s|KO?n$M*71zvPTCj>M4gd;nZwJXJ4#Gj}&>dJZ1Y=Jt^VCGn4LW8I$Bjd>l^iiMO z&!0u;mOuk5+~aU^d$eHMsw|;2C#k6-0EoQqc_;MjQIEl%n&!q$VCpU^OBW?k0==|e zd!WL8@n#_L-c{3shgBC`+j!QdzBrOan99UMaIJ_{`uBqp8MSL+i|~N4+G_=|$UZ2< zZbIQ=_vKCwvvFJ0M~P?>?$ouj?hvYN zECp{C8xX_=@=!|ZSjvy2B$~GpERq$nEi&N(8(N)_%1_u6Ol(OcBI zN&w4#e`TBRUW=-JSKAv&58|kT094%q-gtht5~*ns5Zvu^f$v?1?t|G4D;n8$F@{Rf zJGt9emdFVSUg){DiC-|S;nztHsZ#dC#w2_$Blb|b3=>j`<`3wgWL-L z(++!Ay%)=YFp*$H7tseCirNiX4w+wmDqAk@wN?gvsnFvv`aWQsRKe56BXBCxSoeqw zsErlm%2gJ;_3SHt z^G@;AcO_10;~BwQB^n@3E1>AIokB3!aake|zxnWe%|ER2^dxt5*JjH7)&HMkr}2mL zb*txn7gJaEO`Ux`0cIw9GU(Sn%k`X2G34Ly%hSJvSZ!VhBpACL%j8F&r}A4sdMjRm z!$V1I=0h{2u_xksx=5;m$#HD?PtK+j?X>HuUv3A%;q&C|jT zwD`Cy&et`y&7K_Ay96Zhh{{ZdhgHwPYe~Fr4`Y`td}eC^hr8MR(PsVifOEo>F&h^X zVN@z{D~_%p&@d=mv*Yr};}_G_z0*op_vOX(mlcZLr3&L7rAZzfAzQ|MQ*R7oOUZg? zXbOQzK;jcFGGy3}n*Wy&^P@A9^a3_L0PeQibmcqA#81>7_S1*(4plb#>*AGl|1a1Q zH7kF^Jt5GyLJDK6nd6}Fh4#yA4-3Yt<>qT`kl#K0&g0bXlok*_h5uoR1Rn!tFvB{O<9cOH_jeqTYDy2<3aF)be> zoJ( zJy;+1P30IiXUZU(TdjsQ!q*oM7>V_o_fNBqkj9tM@M(LJz<}VVUX9Z$gnWDKA4GP5hp9ZUxA9TkSS3GUWY$QHp0BvFhqb z?9%fhzusG(RCC+UN;GM<2O=6?J$)TWpl7EXl(}Uyy?7jaYNvYI6!u+|vk>OP&fMYR zf8Eitrj_~(vK9?E!z2%2PN2Gbj*)s!;`4vSUplRKyWiz=^O$|7(5Ta}RRU~Z_&$No z7f%O(c|bp5?Ud=XB{U$~rHI~rev|Escy(USr0G`HZq{ENpUANEzd;Zh?7+0iW+rEF z`|JC(FXyBAN8RJqQ_#>+=b+3F4Vvv2=LEmmD$|S<2|A;`iQEa1ckVI-k#WWypAoC9 z7gFe7OiivktL{Yv$J2E_Q)jWWQF&Jc&Sd$~17E+#!n=jv6PZ#4^#vvUxN-;a`Bro9t%CO*zV=FO zI3)Eos5xwsN>>M;QGwP4zbQfkKM*y1_063R1 zM*5&*6k6|E%-LE|!`L+kDIA)9-aTMcaLrYEK)Jl1-O*xX*rhzEXW%;E3M-UE&c8%@ zbER}`Bj&pc>39Z7FD!Fl`8)}18QHPNhK?FdU54$HueZg%Wm^KNEF^Y}zV5wDtdj8p zOueXO?@&s7W(7q9z}n6aH6gO(f=Qvos^dy2@#QSHM(vgH+P_Ye&&T#pno7zv0$&O0KzY_ z=&5K{>R|8k*lvKXvv>7yT#%CIhouUA(fTbB!ZCE{GO<&~#`gD^OV;)4U1gcfjrG)h zGDbD4=IZ{s#g3Xx5~0kSp@Uh!jEx`D(M4q(4N<5vwvKOk|`YF#q+6E zNzl1oDo4E_`W#Mdb&g@}()SWdcP7_|n(lYo<(@@`1NpNAjlaHp0aP5^lWiK@Jq{Z=9Gb#IbjszAv+t0GpJy6L zbS@TNud-e1kGFFJml|eH6UYXB)-$sP260SclMYmH^wn?me|aO{^E5}l3fw;?v9ufZ?V68Qu=)ctTOfr<#_D3f6A5sNBQyujl?27UH|HIE5< z>@IviZk)OD15KI#8Xm*FK5i}Mra2-o%5OkfXPNI>OB7<=Aq`PvcTJ^@9)ei-#|PZF6}c~~*5=2h7~BeCY-{0)ae!{I^}><{J&Vj-Gdu9O8} zlMO8=a}W~F%$0BIxz8q5ulmD>`4iOdB-AmGx{)QBa>uYDnYpSuiW$!g6*&Dgd-T8& z?v;g=HM@O*V^KmyS-krN&sI`+NM_e_YC3FzAr@@1qvji9Bymtm^F5LEpm{UlKC+vH zI=x`iVAEOY-^5=*eCE~MN2{7p0bK2tf|fdw*U`atgAYBtS6Jy1gJysPLZw+XLjEz& zZo~hCiM-k@?W8+jV|m_1vDzw?t~pkQ>of3DUPmRmyw9pnDcLsZIrZbJe(;Ls%q56x zc;BsdTIrlR66bZ7t$McB%r!f2L(H*A_{~+&L&Qn!=VI-Xy{Apb>r8?8S~?2cm}%7F zo3qp01h}ng^{{1lrEJdLBfT5S;Azip6VK{y(@qj0!0{`o>n&K`>^?b_u)yM5vSjpT zPXLJcm5&z%|LMFjEAzqd_+>(fBqD??PGWEijKuF!$sak6X~iy!U&A+KK%(akP{9} zRnpqqRo6h}AUDk@vL#V83CzO=I@*}_Cd`e!n_$;8*mjsz8palh{~}7~-j{%@$ho3! zZ9TT#%R$>+=tXV^4$r(<* zV9l}H1ie~L2SQHc!2;$miq&m=zc@oz&?>q}85c9CDY7}OTKQ?9vtVuZsdR?dJ(jU> zCa@v&Vz}4C3RghdtNzpmOpd%elH@jEm$VpUdj-$@;k(+gXj{jqQ0OyRpQaom_)&)U zK$vkBG~BAe6$TS!#>IFfGxfYHy#=1Dcg6O+LV9ZfNgw)vP)6tnW9iA65WF;^!@Mic?BoIg7Z_OtbWz!W9~y63^G1d>|bzbz^O zU4=hp!6yt~x9~V6C@dBM!q%Q_J5dErq+WY&f4iF^O|MjFJ6thIm}8X4fqCz;8zz1- zYft(co?DRA1Ml2o$OMIG6)0f5oJaL-V1Iq+dHnePIW@`W{4yA_^eb!K zeRBk(jw=thKV;zVlasad(&&2N=EvAFqp6!8WvO`9=F?nvR|bnDkhwn$QwJ^4ZK50T zJU!@4!3?GQm(&N=2uc;X9#8J^FTMf=0r&8#o0n8B$~~b`by-`1dEI_lC>#>7V@=Bf zy%$l?{BjPeCLxuUMP1M@c+2tzd&zWCquWo=yH!@1ygS`9(sYS08PPlM8wFWZL^C& z#2q*cBqf=0W|zUc;e?lKWwD8AgiyaC#-`feUHXq>l{9qz|(HmJoVx6l8$+Vow}yIG;{Zo4>uYNxs!zsDQu<455;onyPGXPrTOVg6N@)VV!`EOs2!z%axQ~!l5L@5N0cxx0?w^t!`&2 zup7%kDs;+I`wE2m*eqmWP#in?M?ANBv3ZDO|B*wC9IV9F?eS+>>q|B+cDq~|b@Pcu z{Y1q^RcxR9vHwq=U0HF{R+j4h*$+_l%<+JmEDbO~9f^y9+S+v`+K%+@wXgkZaim(% zMVJehggAy688a z^aXOnd>|MmIuGlh)9*3I+{5n_DGU&1@@sJ$&a4!<6y169#Bp~B7h3VOI!JX5< zCh+geDma+g2N!28Y23zWsF(=X_rCJU7YaUoen^1@RpA*)C@{{e+uBe2sdLBMh(sa= zBTPflE)0>!8pp59<7o=7KM&IdfM5HPbWtG116 z9-iL75u#vXhiRaf%*0nDaG&gl@YVQEP2NEVt*esr!*Cq{aZJk&zh~!`tUuOzGroVG z!d{EZ6s?M8oR2js=|&J3NdwYA8@ z-n1LvM1@J~e|df8dLpyZ!+M_Sbji5dt)s=@Kjme;yWOh~U^8VKr|EWK>x|I--n8w_ z9G<$(D_wE@W2F049iCca`fu%1$dBM^vkOyLav>y~avP51zdt>8SrNXleWb^vi}1eR z6e>s+w1m4XEIFC-#$Jx6i5E(bVcVQUSA9|NUDa2rWTdSe(;rHqV3IrjA*W>ppJj+n zS&ZEPTKmXXz!5sW4~=O%^9k$9XLp;w7A5o!o^)Vyp?98um3n6<7Au?*a-^BxwkJPy zzsMueQ!FL8uBm)=;z?p3xv^5q&6@@J<*-SN;^=yWq`YbhaF*VBY1rgW4=0iD9{QUW zKc7&`H8@!vR>iFp1Ry{ONumlHQimjV4hyVHJzo5o4a~n7DbOSaB3CYXf7MJIQFY)` z!_l7!O(=joc6G_~Kk6hZaGJAY>_2PzxoxarUx-gh=dyEu%cTH;BRr90jCoRf&EYl( zg-Qt6`d-nJdez5KiBQ80_}uG%mHwpemPHcriIYYbD_i?R6hI`B1>%so zy3-q&eDkLjS7sBeYH>jixj$cjNXz5MI7|1}yV1sHAD#p=G2HKRW$b&}h%0adeu<4u z)rK6fN)bvPdYv!>f}ZhIpEpzSas{&B-PAHQC075&6i@Sq%~Oa>yL-P999L5lt@DKM zAnM~?X9L{Aik{CaD@amj^9Kk02{`s&6=w31KYKSNtXGE(E9VXnCUKfsdO(uA@B1|c zgusUR{(o7?XGIgJpGGB2=sH5$5Nsu<72oYOTbP%?_*XnF)gfl&%ckop4X zv2>R)m+?_YEum8oz*CMd@|pv3cIR?=R<228-TK{$aN>GC$fPMBY{VzS^-kznPbNaN ze~p5e$Zt^&C*InkOqtSe+&@5q(?&%R@;)&#nX{Tc(nWOtM)lE6!UZu3OPcfXQyn^K z>nBq&WQ`Z$+4GiRAP2>dgWZHha#6$f%yQ~ST+>Tr)81Rwr%olC+f058t#?`P-mV<$ zwR$m*gGuhWniHh41hR1bs{d}tl2bpV;1ZIXP9(bMxWf@H%{38l|`2_wd z;*P4>>>W?hMJtDkZ|KQ#1tk~>a(982Eyb584fP%go?jaJQCc#7xi&6HcYkevAjlI$Q3tlrv&-D@Bi)48% z>d}$u_$vbLsqaUPgdhd}ak^(uxf+t(J=kJ#LTqB`g~`gxc9Iz2+V140g8Wfg?9FjQ zI3zBP4dex`2^7MZL%weVVzs*o$Y+HJC^5%F_f0O_R|~S*EPq@|H?zDiC?J#F_XDX@ zzD+6h!-b@QH$Bf*eiqvE@+fw}y)ilH`Mc>!`8)aEd3Cs(Tw&DynH&g_&ESWpw`;!l zc?2C#4k(Jmle9#sgoOHBev%8%Ne!J*VtA9C-abC+qPO3DB1vEP$nPgp{%uV!dxeE7Lzoe^bl7tr2TV}3~aD%>{90M7DN^nC>- zF`%njh^N3~fV?IWt$;|PzF^v0e8f!o3I2>eQ-M4hT*6M}-MtQ&>g+Yvx$q;)h`7&$ z6pU;xUNBtLz6(zcF*O<9q1Vy)8}fY_MCv=fP0Gbz3IA3KX$N33FvRB#euyZ5OufW4 zSW;Gw63!4^mI-cCZinRrcT0A<5B8nIrcqRNzsy($=-=>+mSKS}@Z2{{V+jX|0$z>R zaUp0BQNM^M(ejY*{7BFTQzg+pRPbaOQtq8o|G5AdyvpUP@nTuHG@CZ@v5_};dUDdF zWDY(Yz;!UD*zs@;3OBz94rKSskFtC=VOu@TKGfLs9NBE=mEziKT$u{EumT1BH34x- zEx7DqDiJTXyaKE5_<>f@M1b*B3%#Cdg3%=Tew6T=EP#59q8?r#Y4DZ)szdQ{8dGM^ zCC6URjeVA&Fc*$ zALpPl@t?z}FFYmFdzx)Azm-+p#+~}yu5hv4b?hf^+Z<|@ zvtXsDK&f`Ch}DsLO{4Llp3yOmSF0Le?J-2WW`b%Tsx9&-V||qRQ`(^;LQNP<^8tf6 zmE|~RcQX6tx!;cuh?2gv;W+CiekKU=`AW8-nls7&HtZwjPW|gd+2k$GT9c4*5oPcb z8puZsHd{ccFzFji&ECyMYU7-_G6wk>`2Acn7^|8EhANwES|#SDLzMpBTjXxGagWqC{gS*hCnb~bLG7KJA_MWK ziS@wpnp9Z|e&u!W(*K04)KC#GyzS>KXye*d8wtbs!;hKtFD({a3;Z ziHfR)ZJ>ad`#Nz>vMnf0*Z;Vo0D2|{)=n*J;7b0hoH@_ z#OJK{uH0^bET)tUIcmnkxwB5QJdzF*Xjk?RZYDbV4Z2Hucx_S}&+HAnF;qv|Y`@5E zG?Zbv3L!@l#&(DrgqZ?%CG+kO0oq@kYjTWXkSk#zvd3!|I1+QGpp>kwPTYYM=abRr z!Z?YOrK0~K*%LaPZ4EhTw2I!@q2_-s{Fa&V0RsluNr&Ag`M*0{8N?BC zh8HkwuR}jiF0@iSuS~fxl8#03e>i#}h<43x?^(tYa@y^VfanG=L=#0*Qaki?C@`S2 z_Q%=h!a+x9cT`v+r&rJ=rz*42Oop&f_z2hE;hgsUZ}6FJW@~NPd?o>@CC9gFQYda= z)?e512-s=XKOffaPWit&iEgHaP2-J*jF)je)s1qn|AtHd|GsX|gp3?-{-ZD&ErzvMT5mB+KNaXFP)I+$ z2cq_WXFp9xUrhpgYxjyR>;)OBj?Np52=4nHq+asZpmR=lvn4l2B2FW(btB(4w{tgLUS_N?FH2y08+lMjyQsSW7tb)1Hx2tg z8lcf~-yP(N-r1BSmvm|{F)8x3;OGg$`a|uvNEAHWcKmfPnCd5F^!M?k9I-=405dZ< zN%(Tg8!_gxo~+)k1}dj3so&PQ0|!a@dmTd1IM1bIj5A`1 z%TQ)CKV@+HfV$`r=0?Qh22#Ya&-J;xzR-n1{{23UmePM2M=T{&2@<(btlWu@K+%rD zr0eT=6#*8NZ;}qGu*dkbIc?_8+%ODlP#>0l9ykUF7ytK~1PTb(L!>MH3NeD^ zala^U@|O$?N@1&yO|mLKRd>|auR@X^*w`#CahVvtipx2=BzFIGO(Y`KR?V`&Of7KA z>=(FOp0^kk|BnL=v)QyQC%WNMs!TiK;n*617Xl4(muS<(mv9ymU-+=pVfPWAxIQ=h z|KK72iCoreX<6eJ$(9CIYy0`$pFPD1r}*IVv~BVX4zdR;+?WOi1tJ!Fd;N6!mw^1m zT-E6Gq2qyeQMdNCSJPKk(|*1G4S|R323(}MoHw6-?g6wHcoB7nU|7c5cb};W-{dSW zFCV!%H0Z&oJvEwziz_phr~zj%_>qVU%Tp zF)B@OB?1>0pE++%)o9(f&!}GpR_CPt8~Eg?8EozR2bMq4yt6w|_<7N%W47VG0{OvJ zxs@S~uY{?LhxN}M=T3Im4!C2;`5i!31IynUFsx)nZwms6zvz>FH~3W`SsZT+Q@MHX zA4H}Hp;N=&Pd0;v8aqJ;Sc45Fa&^E^bZ_=VHouE}mvHLE{2OHOt1yN*<)4kDWjE_O zC++b5u@rX8vphk@+=(Lv;>VOmrfrtV;=n(VboO%p04Z_AxwSQ64cp+jQwnO27Jn%qJkU#G?*j#Lj zRG=5G9os7`*8ZEZl6CN^2V%d8U*c6;>IXnJ`HR7M;99pTyHT8}?za)sh5yY{#hC4a zwtij_m~Sv0@P1!PAlMN{gb!5o0;pz=HTMNaH-KIX#5p)qTL0zV;Ycy#qi1F1tg>ib zI^yAwkm$CU+T(ppDErP{?+K{vFa6zYiE3S2aQc6LKcORvu|`|^0ER=gsm4oL+Ge5g zc+2#g7ltJL3Ogr8$a{J?du{3w`1e^hr)E@k{(cO>K@pa8(d91}FWIeNtiQ2aZFqxo zSGLiEh^Uy`4l>{+OSpI_vroudJT8bY1}X{YlSA%(WHTUac*-FJc|AJh^a->0|Y z)3f&(brU);;vww9fm>&)4LRzcHGEfo15Zon6B%CFX-^1C2)uWu2XJU;CDJ}QjO!VF zaQ|T{!mi&l>jb!MdT20gWWOWUs0D-Be^T6An(~LrPL1!d|1#SXsQnDz&aL90`oS6F z-_-6@4vQEt=;UIjoth=`;U_`=D9c&MrgR@1iZiS;#N2*$?_nuL{0xE z4#ZX2n~AupPk;^UDUCVM0ljZNe}0kVW2MZOA#w9Bnq*b9wSM( zY9sycb0OE)7pg3t|Ksu2H>l>B^k2MQ@c1Q)8pWALebf@E=t?W&t=RdA*2MR;ib z|2CgET3S#ky}|o3K)HixwjO}yO@10^e znqV4XPR&eTYVgL?>cIh$?sV(5LFi(kSvKY)99{is(}RDuT#>I1Zf9xB(IY4TW^V7> z8_5sY?ysxjGGSx}5fjmVpzqLC*jIVkOtaG|m?qhn_?Ah>{ zbVi&Vo?Z8a18#(0a?rIORQ&_j?wzubGokVaXVQPYk?cEhhTP9%^Ck{}eMwY}`NG#} zYc6X_4&K^ZdaS*7!R%4#`dup|WE&@D>%`6@_%>CjZ>RYs2iWbA!+3Mk!?d)qh_wYi zImyB(Bnw*Gd#LcsX;~|vC#wH!kdHO}P05lr-F9jHT+8;m<&_GbRXMbSbEfwy#kOJC z@rj8EcJUjn?Z(E&qy02^k%@@|^OnXDYX=W@6JuIJXLlX=PKg0_%Iwa8pP@%)-#KkY zEfjLT(n!IW94bOfTH6_x^>}}UR{iP!siP1!(kf(I?pV2V{AIz30YjgrEt?{6@O93h zp~`=T?5*k-Cn(#JvZk&3SJ>Z}m90z-G;&T%C@?TFRbe4$hLD@Ib`UJ@zd{deZ)Ic= z{~Z236}=6N{I-Shw#ZmW?$m5fo5$zfSZMv`kldK=s0Au%?c5a4tdK3ecx8Y_g94>mh4iOZJ?@J)I9I zJldi9R#rhkvTD_&%F5v$?Yx8yplzJuv4r#eXRK8$L5dS1v`$@g31xPa8MAjk8*h(S zCKem?5aM9eTbTLQ%(k2=UEQ7NBrJ?5FSFgwo%1QAlh(5{l7e}cFZ^6=y&S2^w zyquECFXnDM#Vo^-Wd9aQaT*@)=~}b7$fgImhBfe%+2nt$p;YV8T_qsoc(yYerss5 zpm>ITM(nMyYXUIlUTOkIJEHEor;@=noG8|u+7Vu zr)>yGE@LnlJK_Ok{{($sTm4oWTM1)eXdsuEGk8_~`YJ|q29qebb3{eXQ9s5~aJ8Sd z8-)d^E(AGkWF)MA9{|Fi^*5c2a(M5&$aSi!z>y7)mONzPsgUZ&ogH%XD47?;ygO!X=!a)3Li;8-) z6_<+qBU=XMevJd2>A(6s2dt!@oCdMt-b80)dsSBq4wR|zAGq?5O$vq6_Q7@aAVdw# zlRwes8NtRPH8i++$REVc>uv6k^Kg5W^w_N{)c;;iH~W+3#)MhKI5;!fF=O^co6zRf;Rp&dzRi9@V`EBXQiRS+3{s~>%k7}Oh}wsDXY_)^DuNy;u1Enl5dqVOawn6$ z+=iZ?Y`va*uyZgS!ea*cal=D5PFlsyP|T1tBT!wdXijhpo9OmGvr!e2z@A=WF~>q8 zk2e235o}<=;`tsK7IK&O`7o(9U5O22a0DXdghu+;sgt_g5b%;dZ&*9lG#*EAMcoN!Q+UvpQ!;UWf)PbXBJQDUg;FZtdLz_FW}@+OS=)X?7!wIw z0Eu6i$T|xlH@x#COXLUsg7?;_RrwmEBqWNCdZZcMl}8m8_MIPWwyO!(u~FZm9_cZY zWiUM-ZwABZ>vzd3dFXuTszh5{u6+3~N6D0lbV?*sQeU4TEb_~iD?d9SM$2n$P|Dh< zu@(nquTQVOcfto3mz@nU8<P z7SQJB6m{OpFlt(Uy$bf((!LxiXAAA3J4;?s>3Xo#? zmEKq!?vl$s{Q$;YT6t5dc*dX9WWAfn=!x6x-dtj#JM9Il5 zb%`L*dKVN^O<%u)?|Dbl%8FLJhVSmIfQ#4xU*wk51?Ga723?Ytx(3;Rnm0_a_DIHw z!@0z&9bxeL+Po4El@j(^8}kswzKN*b05{}I-&)%mxPHZNt9LGc+ykZlJ-&6#>+<_n zM9=(VG!#oE-aZFxKXk+%kx`(fgIg7T^$t%UbS1Y`heo4FGVQFVyBaN<`j))gkzhf;3$`xxhfWz}+6vL3&E z?h--Dsp0eTuU?al+YqHdA|14jqhp?$Pe2K2VF^fEmA@ADW1n zR1yq;`-S$)Lq)D1f{!Et=|lqpPxbbt%&K4{wn~s5%x&s`Qg=Di}cd|^LC z?qoqwTc!&7yzy;gWfOAs7^?*w{JOZU%%-vh>gH+hh8y4R`McKDG>7LuofXxOpHFT3 zNrDnKN}Em}!)*x8tOhq(Edzec1aYtbUoS)g>lox8%glNrmXq`?&8Jn02g6)Yr|R6S zzW58fmp{f#;_#VLNL8||vb>3bJg?*mBAv^fP%Y(qzby=Sk|D#N2O{69lN)*B1B)D6 zVv*d9G3BPr>g>%OYnwGs$gy;k>1&5-cIs(5!rA=^%||@Tund{;nY*}%ayV5&qu=9v-l2fQUiz;D|vL6%h&pMzFz>$ZBZgLd!jY}i2#{I>Mo8&S-! z%wY{prK!K)4}E4ks%H56*uFMuM66-;t6|M!SHR&R_!cDGn{Z{c*(XDtK;uudc7?O% zrekA@ib@mOC>e@S!t7Lt?y-5$o7dWRo14FICE|2gIG5g-TyZH5A1V z$1l{VUOHgRPkvoWRRldmZb&|fGo`>j* zW$!g4F!w37!1dwpz?)*}S;IQWDu{89EnEEJ)AHYs&TlP0yzlzfFs3$IW9IXYb+FLp zlTql%Tr;410h3ysnRk|l_H(KD0W}NOHv9V}7lz zrXsXm#3TyhR@+o`Ht*0fd2U<@Rs+d%TrCjrGxjp=X3DI8dOiAefdV^NNI_&e%fe(nF(5R zRTLku#Z|WK^EmX!W8ZU}pi)-)gqT$C60!Gs((|wwvm_gJfuR0m3f<>UzNY-GLJ{EH; z2~BmrbYJ@1$tRFMaZS1eJ29%%NyenPN%tOOP(5AXAafNsv-RQ&=;bv|4}|mXZ1lYu z|BeoO_wr1tB$08#+YurxT{8|x^Lo*9n+82ZFGG+7CQqZPl(s>FE}Y@=ni=5~6D5{l z#mGwsaujDv`yp)v+5W0=95xch1){m5(}L%R6F!WcMx*7`d|A>r(LBQ_XXp~^JRES> zbOo4H`RFqF0TqKozQPOy$zH;KiUl5FDD`t)9?=4TYg5~Nv_5JrHPcGkXZMH#d z*DP4c&rhNBZ%y`}5*KH^6B&)Xq4N39LJYQN=plc+7&t~wZDTe>YvRc1E_z*pQ+z|; zG}L;vRvQn50fWPkb&UqFdjqL75hT6svWbFm($PgH7R9EY9Y1AothV{9ORZ-l5$&t} zi0C}w3OIen7w*|OS$h@620bYDzD|g~o{#?EkmiPv1i`ErW%YYu5!2HKvdfk$mG_~@ zVb(go(HI1&3ufExCMwk3?+C& zt}|FMOv!3AnX$miE~-p3wduh-ZQj*pFQKBs+Nqf)E1y?8Jco78qmu#8^$nbh-m#eQKM)6b z?rI5bQ{R3RdmdZ#FnS=V_0>iE>WJHB0wKv??#@ZfeEY^O*PD|d_^)IxN6E*IjXdeK zHI~~6)%#^=v)fADYZ5xiX=-2MkMnKp;GpmOULKZyT6B4@(yvckaj0jVKdS0_Q$3M) zLJA%`Y`%_YQ|t}9)n2Fax$HEZ_lyULyfD6A^p$B^dNrJZHX)-=1z_drk8|zc-Rqu7 ze^8jpLccy8d>(c}rHLZ9wY8c#`4Z16m~VM}dpUkgv7LN+zP-TpEKcEH@WRyBbz|E7 z2VYWy-u1alY{rH)TQway+Uf6-gR|K#iHlqP=$mJ5@+Vf-O#jS%o$9@oHf|*-Y>3bS zW3SJ+(IC11w%?Pz^V7?z@S}ndhiWFnBLnWLN;&l z1`ql$etht->~cTeadUlYFj5d4jGWZdTOisIyM)WOyHFtf!|sFSfy;N~_g&Ji0OOUZ zsg))odPGQ))!{RxY0nWpc|d0#7aLI$K^*ASV)$g|A?G#8aLKW#riuXlf^TZK8apr% zyI|S&9Q$&5{kcGFuaL#~8veud=vUBa?M`4^WirQd*ADRcq?TZ%p^Sv=```lq>7ZuC zN@q&CLz^o*>w9OBJf^;9c@@PEEUb-U3F?usNKGAOh+=1#^}Tc1zsn7_9Y*FndConC z>KlhY^U<|Z9(-3}apq}F!AI{2NivT7ybPXpgPZu;S>@B}&tCbdxq?x>E-Y+GM%&)wb6>okS#y9e|$OHt%o(|eGIt6#qNtl0oDmyVnkirk@?aXKtO zuk6F+owEgGxanbxFR>^lu={b0%yA@VJs;l~`brDkWCOkveUJ>;Egt`rN234G)>o2o zb-R*p$9eYLm7g~`_VvhAppR^_A!k#bQPXoB`F`sp{rT%i-}2+@ESJzRpTG=R|H;~@ zL*2=s|BQBkzx9E<4yg}ZhJkNGBF89ytl-5A^=VwFUmC;|1+ULhn+mp44qTt^2^I%aMDDo*skw%Ez1ME2g6I zTaU}uU6i*L=2(3nQ|5f;0;fZfLEF>TWjT0Nl`qbBCuxEO8qpfeu08EJ&A%zUpUc~V zqmh?B&&)o2FXG@7+CycuI(MqfK`lB4zH01!gT8v5XV4$6lXmGZ>voRbzw^9axxW)- zjA4D=5Nf@D)h22KYJYf>HOVmT|Ew%AEGL_|Fhft#0jC%r5uI}wKz>S{@TO5^Tn8Jp!(Tb^R{HmR>9cAj>jHi)x@6*Bw z+w5iU1Xqxk3DR#q?Tq0BQc}Y(Tg48kqa6pvn!MCIHG5^02}{|%_-U>aEu;ZE6JKn5 zzDI1rAJzU_*%^$eCah~JAz|o1SMP)0bjVsM>6(%oA@56VRy?6Kv15aCdWYRun-gy* zqimz-b8tBIC)3NIg&`O8d_8FC{H}bSC^gy$Pjf8-Q&LiW_Uaa7ab5LrfI9S7joZgX zi^Q4(i67Z<*~x5~FAvGQkEIQpaR*wlysJa!1a1+@l|fWej+!{-#saQ#sjHMcgtX}_ z9faS{P2AY}=oe-6h|?e3E{M1UrdF_lt5IAJX{hK3oUVqfH`AZNJ)b^g=DvpVQ-9}T z5i!P33?^+pjr_(BbsiU^EFG2$n`ISC>YdwI^OIj76mH)xnCf|ap_}>+(vrRoD`XjT z*nw5NZol0V*~oV9rF!pW@5cvdVh9WkH~qLB%y4CFb$%;#`$w_S#t90cBerZGF5Dyh zHvWR&TuWuKjE0$>AE&S92x>o3R6k!wsLJ$4o86@fjNZp}$}X3cs3dMsF0x|R8|cAC zSgKlkCtP(ejFp))8fSpZrh1qzE_Qw>^SoqXKk$3fRTwuspS$r-;<*s+;2y!wQ_4+Ypj*P18#3@u4o>k6=yBk^i{8ihFIj(PI?7Qr^-0_$meLGHNG8GDeY;;sR z*6Ox@9kBkRIk*=|)}#az7QTsYAKNv$Us^VDveprDeA%8Y7rnnZ{`E%7!IfW!T>Q=T zjSk6)({g){<>?r($j?aXpflY|j?)e0MX2AKu5 z>S|7NVF+Phk7@!E7TWO|RYKF&UY((@qO4^wW5prX z8Mw9qwSrYbkIr0rdEG|(zQ+dNR|$H=j_m>lG~}=drT_jSK;YQD25LJ2(yMoQi9g|b zOw*02#4KjgocaBTm^4PNlZ`9-*y7c7);^W2o?yhc(4*|1kA5$+JA2SvCva{Kc@bv^K==p%x&!F)u1%>J?4`ltPF_<)>Mu6A4`#VEa z_lk@6Ue~heN4FtLm~&!z7Uu)%PsAWse~a9RQz%}NKXkkIQ-tVdC_KweBDbd#)zS!> zTg74%NSotPHO=!KoykMyjgIp~KkBP*P2`bnO&7<*@cg#m*v9xeuU`mf9=0H0U!_A4ip#dsm>pBbv!`JKQJ7~O=>QFjjSJg=|iK*@(`D-B*Xj7^$ChCF6c zj2&c%E6S(0R(B{aCdKo%h8Y4w3OsPnNSu}%UA_=xtHDi+Qxjz6o@}>u$WYcV67edIK^)v~NL+E0w-0GQ$Z%XlRG%0+H}53C^9S&7y&$kOj}w&Z z{Z?Oz^5c5u8_wS{P4#-KoQPd?VdeE%hNCp`7hw{KYrL{#ZuFIJ6h1L0vH6Xx#OowG zgQkA?e05f4kxrLt94K`z@4m2;`DKA7hOU4+$WY6*QNk${<-I%xZ0Qsk{6VaW(Dd^8 zBy5;X=CjE8g){OShK-jX{lBH2*hH^?OVZRC>ORZ#|IUMEE}X?)EbtqG=4l;cd`^y- zwMF(2@2Z_$n=u`SlkK_ol4q(1D|ANI4obsKQWlWtz=Q04&u_^NyKf$-# zOpJz2!w(SVck1G@(;ck$Zbl8(>g+Q7X-LKmDC27I3S?=oW*Bm07Vy0&8$R4YA;mjh z%!STfDD$`2_}A++W**nGsol(N&LHR(n%@1AVG7PzThVi5*zs7YuUkXQJx|D_M|$cX zFP?-_3#{hikgIIWb<*Kzbcb1T+r7YeK7+5&#b}2^+wbuLot!{7zdbAZY~22pXNBtb z#i&u^(j+fm1CLsf?OsX3d5W6o;0Vi=SLvH~OUUNkJlt-OOY;~`h^lqpkFWEovz3*M zbWw`$P{kA@#Xh58ZCa57?^w!{tEgi*b8<^eaF*-;E(rD2Bs*vr@;g*e6=8;!$E?b|3jKr)SmTu>U< z9T8$Qv6rk(IZSsi&rwo{D~TcxdXDLypJ3>R-f@$#eA#S!E-bWsx9V=4Ys0Q&d{>-5 z>U>yaR-@7JWrZoF!d;zmS>&@a_ZBh;|BsT{sXM-zcTagyw%WFRdv$+n2XRG%YMh?$ zJlH0G`g`RVZw55r{jk%g(NO=Z{F|k&6KMhur9$5q_%L1!n1!)D&gu)bGFeR) zy4n>aM*z48O0{2mLq*3{!&o@jJ2&-$;14GtO##$pbu<7iqaVMXub>wmHPRg1x!MP3 zD}{!ye~HKMscBCiO{Cn-TV`GT2@1K0{(f?a;?D*Y|rP_0Xh%L z`jn>$l)6g!eik3L$;vP}yIO(kFW8IbC5SOq#!cayEOiNW2Q{YsMN(|P6D^*HvCdzX z7CB z1fUbB#?u@KbJ8BMF#F}{+F9MO1aJK_RGOe^^t7q*Yvw2e4v-)DK|`gNTb7p@NLZ1c zT_5|kV&A`DrZ|-}g$W%94vGe}^ zO^7fMU+Za#CG)?WKhKiU$Nt zq_Zw~P$0024auXv5b54|DN(PMJfYrDnv^)U}@Fk9N;>kNp6G{}&uo9=es zxY;4tAkzO0*%USzx7CNS8KYyr#fCXROjueRh*atEhaEeMHMQ1K{~$9oKnQ6>|1a|8 z$EkkXOS>K;pDAV27A}Dn(g^)eBS+kv&QC^l=u}prd;62B0YPHCaTXWWOkj>?Up9)| z-b4g?b~YA0uOTB&_F}J}z#Xe^fzH?}LS`wT-E#ZPurFN(k3$+BIkJ@i9FdI67h(W# z4W>5Ymo%O8#PD%7-E(;3vAs7D`Zc!WMD()#LC@E@s*j>%vKX`~%$i5?>1ke5k*p&a z%Zp$6r+@&RQTSH4o77C1%V&x*razsc8AW>s2( z)VuV9#*RTSy4;jOT>oN)D|MWA7(b;M+DRT+fEFdj(o+MTnF#J}f`Zx~?rezQI~tX{ z#IPcqFIsBI>h(e!zpV-uHKxV@cHUw(YfO-s<$#Dn*tx}T5;x^5M$>j0uFF~*G2uHr z6fw@CBqjuJN1=5UVz2%U>zMn}0QJ64{VqNGgmvBpgQ7RL_T*HCFfMCA8k61x(PRZe zT(!>%dY12DBN<${*eL1vD`-y^+z4ZV?aptKxmXwl!einx44--5#FiAr;j@L$SYab= zcV{`A{2?ml(cJbUsl7svXXQVH-tl3%0>5v}=|RoR4D;@1o4jwYR?i)9Pr_{S`u)N1 zaN`N3X}5+SXPHeK4=oaH&*q=RUw$XM$fj>g|4!ulEQFw{8}nJqZP#d|rzhv}p{DCj z*!>Y2b$Al4+g1=sT;sHTFL1;Urk{4S2wjE-lbw_U2ZHJt7(>7OO^ zvaz!wlW;`GWZ(QI^{<||tbk=1&$g6|*^`ZrK5=$se!Deso5RyYQea>Hm~|!6lrT=p zPV)qYgAAkcPD6$9r!alzn=$_9-2KMK@v-LHp{Xj-EpzwhFRc&nWsD8)O$b~`RW`x_ zF((CM-zF;zZM@9&oCDbgmJ%tok=yi7meyMZH>cO>9AZYbImh!M@7qX(G(yQ`$J>^i z0lXLmM};?nERG*?F{>C{5lr6O1KYM;xqcrXT%PptA{ZJEeH2-9T3qEk-H|H<(O@%vg8ExG|OPSvKXje=x3b~(ETKS2k1RLJ*CO^j~lh6ruV zV9Wgb8wGlHgs+(`^Xz%=HgcDo4?eflu|HPc6+tt?^y3kUH;?O{m(!PSj?8#+0?kV@ ztrWn?45CHS8F4fvL2w>uE$xKhrCTxsJGJ)D8FS}^v>Z%0r1a<@n};K5eOwD)b-X_ zEcY-hx91E0D?)b%JzM#vd?60V>gyuIb^A0-p7va+IiHhM!hAqaw++i-h^}xRt)EbT zCb`#)kO;bTAapap!SkytJiaBSVUusM<@1TIiJUq1uJ0+ful{ZN) zRR|@}3c}}ATmGUuk=Rm;>a1`Dqf^p*7vh_XRoWC6n}OI}bKc%Nm_!RQ$wgg4hp{9R|=t_S|c86vs-d(?}l_NXx-wMbnLEVKj dxq zlJty{;PS4T(#H<_9F3~c!Q8+4W(8rpZW45?v?dM#x$9c!vydI`QsGHaIW^3<*?+So zI*=I}?h5sfwL${w=q>#jvC%q^Q_h4p&~XkSLLz0~hzTvfVUZ_M zMiKThx*Dt!4F9qv?#r2X-38KXd_Y@6A1O2$U{2LZExf1?J^LNQYVh z)cR=7atnJyzxFdNp=Q=$1VQpug(0*A zNS^9XLBYhY8^iXJ-4?#tsR5;jAT9o1;g>+dT!$~M5vH=bW<3U>HR zR8xv4V~w*KBdpo9Kpasreuzq};x5HkJ*)IQ$!btp&hrcgYH2}lE2L z7zhm4?mMC65nI{)mAX`{!!rf-q9K8DYEhCx18=rpO#w2Qv>Pvh(Aj7Scta0N-W+)6 zE*wt8N#FI>7OvN{Y*_;NZs>VzG?BdEydTOZ_tc#CFmf^@WKCaOP?hKQilg>~cMiW_T#Mh)BLxz>MF1 zd}02Vpry)R+$i%&S!pGflA4@fRq#mj4TEQ>IE+;fibo&rJ(%T~nzQ90$SG|X5{A3z zl>o0A0uZih9Z-#&2_`$YTxFjmuS!O_Xr?31KYaSBY1R#~wa%iNjREHw8c4q55;pqD z?^_VF=L5mWz66+$kQZPu`~2GY=aAe&vzdx_l#Xz#bh_Bh7bo6%M;0q?q3A8R z)XE`HA+?)@yUdBMyepyPczeVvF1pRO9zg`_&FE2w4Y3H215xBskby#!3hB~kji-{z z-JVo!;VW49Qvbc?fZ+=Rw7s2P>H~QElc84D+`5C484+%+hEPJ<9c=m;%~pD8N%p0C zGw02wJhM4cQ}p=+$vcla3RV=gBcXovAC4cCv19f%Z?MskHQifDS8Ux&VhA$}Nn z(?{Ur+4dPBhGYU{1X!a{+qa^DHR;erUcRmTOD;sQoSc2Nn?W;3Hk)~~D2Nf=d8bBD+SNhvpM(B$ zUfeIioM|%onE%8#v^I;c7o-xJ{RZEIoH4$3h@9}epg}b_MA-ZI=+Vm*j|JEMN(Az6 zV23s8DX1uC%cIO-72UnGOZo#+pQ=Wmx=wi z>r2_sW=GLs*w-yeTek|o;6v`6Z=McZOTM>4=f4Rke@;pUOLE%7FC*a-^vCR+82`V= z5JAvjb-yc$pXxqssx`;#fC|&?n892kGtI?7B|m= zP$F_HXd_xCKF}b4{ndy)PsV`Sf3gu43B1xE2Pw8hFnv z(d&)tC=?@&E_i+c<_e{ffzxH0Dc86%C&aV(--*f2&m?u@8d1an zh05aZCHu8OopiyS+G778Ml1Bgg^`Ls@;fr_QNDIzbFluJ4JdT=NEQQ?T)|kU(moRD zmHD!f#gIapQ)G~~#ZS#N@k3SysJA1;X<0MSk;QrV@(9H)b-2qdHbv=at zzvURN=X7Ul1&0YFvyBekS#=PC@pOt{hbE{p_czMV7RDcQM))4IfhByYY@=kKx;BlU z^)MK6cF68Baw2m_6)TilIY(j6D3O8!c0oOMStRKmE@^_WW>n>8!xG7W`XC|TnMF7= z3o&}(2PO%IPOSt*QNF{0_>;$ovu%hL)$td`NF}W&a%hChpWm->5P5P8%}6p>+A>P~ zn?(c>k#pkYe+BV+2t4amD8i&yP|NxQTfPmX|9_1Gmu_!o_7a{x6=;uUR)m2P%%U9c z!WUd^zIc2FpMP#4eECOVf>yLz5qHi-xIMP|Q9xjXoq8NbF)5~O*_ISog?A6#H1yT8 zAxB@5VaQ-v0R?ry3RT5;2BuXuOIlTkt8``qNGQRO=N#{dK9}x&0x#Dt+`*r|e6Tub z+SV&Lh6Xb(MKhIsWq}Gy0No)Fp=ub=YgRfR8^THbzdjH(8mv$ae*znVAlCJVK^tPK zB&UTTAQPPC_wz)TFY zL*_(XptT<8YzQNcV&X_&k<(P&+`%lyEQGqB~#hx!~< zB5#+O@#VltlDY-jh6Mk?3kNK!mLtOWnE&$5sIcPxv!uvJ9V^fz&?)_j2*DzZh)0D; zMqMtr9diYF^@VVz5&^e4oBzpcvd8tX(XO~P0hudcAfMARO! zFBP-Gl0aWMss!h8pU`=x!}-oY*?HkYLaYE~wXyUfA-!>y!jn=rh(uRtaelo`PK{ML zioI%;0zCx)YfxJ)#8LhNUanA^PD#{~P+i`(W+K2m6F+ z`uB{y>=BbCTaV{MbR9$j44)!~<_JQP+2yD!!W1xrouwWAbZv-bF9b0G@vP_;L%4d> zFpvlM)`cZye~3n>;Bt!BaYk{vMSf6%SxG0tBB=)gNRD-i^t`2{D*7z}k-7*x4yF z>Vn)M-J12(vqq-n+jiKfd} zZUiDR$C7ap|GGGXb7DtYHr5_8DaatKi7dWUq?#USwsJ!$GPgMj zod5}eRa>eSf99e+VvpcHf_WuZL~-16BDKw=mh_=L@Y6wC@kG$r%4#*G<#LNCgz#Wm zygdWesVcfO1s-Re;K?)R@5yaj@W)=*)T}-~Mm-ueQO}kGO5ekq1lf>-j6Ay^~iC?SBcKM=o0!7m-M5 zoDn+Coc38X#>PM?n>AQ*rZ>EQkMg<#(o;H}06Vh3ZLk~}-$JcFR%0Z<4yxA-F`%)2 zfmr@)-*-GJ`TA*w-t()QMv6JkP9_;)Ov1C~{FF2)ldPo+M1;)qn{~nj5tmYXwp=BQ zU(jGd;$m0pujG2!ws zVm0Q`67;f)fx;KucF{z==B;}x+wGaej6FEjc{WpInJov>+u`Rhd(Fcg5=Au$U0p2S zUDba@PC%#>Il8Lq8qQt{lmh=VIe%{4FPT*ETA$bN-)%PrJ&vJ z%R#)xcQ*l1P8~5H9|*0{rUUrIP4_{eRc6rW)Twmyh=QF2(y6{(XQV%Rul1@@-YWqt0m9QVg-jxmm*61$D>d5cg2*6oM(dIf3AYTyl~xcGi2YDD6YNKuIh@+F(g) zRD6%f$e^Y#C<6NGqd592QK!JZ{XXHN#nP}V8-JpS?~#+F5g~*Z;#7SPSQRVGXPdGv z=2TPZyc@?l_IAB_}84LT!oX7CxGRf!x70*`S>dkMu;K&>xGI)5^6|n7!~~OyY)``q5_7(l8_8JI`7-> zfv0!hV%hZm%5+wsDITai71O&Semm4;;O?MI;$?O>gD&qDh7bD=7v%Wk!XPXopu9Vt zBgCB;Q5AoI0*(Pyu%;SXGgK7(<>g#rD%C}GJkUL>&mA-B*Vm5hAeu>lw0ydFOYzBs zxdmi@J{t79?`yH7%e^u-{mCdg#K=|*A_+P?q(p2&JJvE3@7o)vK(5~`XrzzY^^|p=gR$+ zguC;k=NcP^{=G>o*P*q1roO^UE>-ru-!t6Vm(qikIPV-cvF{GIO^>9ycWTvHRvb!F z&XwkeJ?ek=nvtE$0*6;+xc0Nh(-UMRwvj{d;okY+wG8>9V>{!57Ayr&j@w`>G)4kqd^x*xc#SjOP|#W4-fPij|1QjCT4qCY=r*7Ne#r+tp?z8R;g>kdIQHwKX%{1MlcjK%5PT}v-~k5(!ROM8KL4<+w(y8ycS_0!**wu{Tl-KcNj>5 znvx-pPoa(LJlK0aRbXBdpTt21uDJ$%!kZM~sSDLhgW?H-#d=Gdz&0mki`( zfkww^RA*&CKdAWjaO=m~NJ`U0*|piF*pSv>Znslyu3q<^7L|?-iSG6Mh@pMM(3yt8 zeR;Gi(zRk*f^qKi!t`dDEPloVDsy4H;jg{1#ce%Ga91hZc0)}G&5keJ@(-r(V+m5? z-W=keGT4wke%{!AFCVc#qA$uCcfR1ohALPHeJrz^(ihUQ3}xn4h)3%rk4&M{G(-LH#Untx|0Bp?TzgZZQ8 z6$G;FIVTAGx(MP%2Ks_v;{C^~Tcbk|;`++O(mrAY0p2H3_er}I-?WJ#DXfQzRkS{XJ5xYODjLswGNI%hT~EU(gp z@AvtmMU?iipZ3=^n>>l~DSkMPmSI?z>@3_scKD9+Rpgj&XR+4#coKrMBeAiyB2VG% z<@NQY<{J2TlOFs0<;4!6$V(o*_t~)DZ>aP(<|0ZvTi~qBD|7i;g@z)VEqT(5ZR`8a z{%|^c0RaayIK=uab8yZ-{C(Ze_PxBZW*jd+tO_k6KBm`Xb8%47=9mVwF$UNljvU(_Rq zLgBk+&ITj%#ojoVn>+=&f$Amc)06i58xqvNz+WiN0HC+NRu*Br{v_sp=jQj_uuYiK z4ADVtv0*x;BzlU;Y7ZK0XnHgy4!KH9BB|l{(}zk7=!s4u#Q#N6vv3bqC`jm^bke&2 zhH+3CN2wHg2HBCCnvhKu1F|r;%SyIzYv?#Qjd1z$1t)Zmx*&pE*RG4BynjpdU8Z=fjN<8LEKeL8WrM=;NtAHD>rMm9jo90 zBv2sQyK&cJ24HlA=2zQ&RUJs5m3Do+_SPMQ0`J?QmcfT~&mRwh>RdENmC=CgLeJ!O z>0}qvy4B+5W5yUI8q{AM8y#7`?pU;1VfUly_+it>a|JO`bK{t&9N_b{9WJ6e`~2dT z+4rc`8NN<+G&udf)`iN;i@`C`{WibB?^UQ&caWUjCv{YtCq}Ao(b}USad5*MNwHIv zJ;cx^DeW|)^OM>4y>Mjiif*Lgr1O^3>L=Lms_a>}9km>HXZCNdMC4x|Q5DhS6`A3! zS!hu6yTO}XMwm(LN=)_D$Vj$|I$CBpUNTEYcbj!BqK&tT#>f^##r}oqh(R}2R|{ZB zgdy^dSF2EWXgvmia4k7pd*LQ%m(!*sVfrtbIEi{*+)z#LVeme{E^J_I2c0&t^H*_9 zVNG56n$nBT19a96TTDdl`>Lg!<#Xq_p zeJ>NZ+S(0az55%TmmCjO4V&>eh9+?nrdNIfU@dRY zDXQ5V1AutG`cf`xT*tt6`H91naaSa>eKvZB_91S@exv@p&I9(k{JSf=`?j%7>6nDDH^jlTs)nxp*to0i>7uC zjA@_tZ`ZFUTa!ZA33~^>#S00@Za~^Cihk{!uJ^j-e<^NUq3KXZ>EO5syEgh7h>i=d zhd#LXo{dSnAQ#y?pSfDbK!M&Y)+aiG?K;f6pint1!#}N`zpBPs-Nv;VUxeD6XEF3p z-|=lY*8zT{Sse`fPq*+Qf&@GY*8M%TwYifQQ$tCLB8uq$KG7fnh!FFQAo7@YiR198 zIl%l9pHjXtn>1D5mrI*Rg^6zT!#KkQH^`dVsLhIQ;wH#&FKAddByC{pYWfxEc%q{XuQ=I``C}@aIdUXlbo93 z{?@m_GUO(ozL;?(2cE$|0{EuaKx=-=6lNb{$10Wa+IT?=j@mM5zMlIf{-vr0v93m* z+Rv%Mwh7!Fi#SHnFyjCN3u-BR?Jf&LvG4ODL{b!Ft`Y7#ikR|qGe?4uz|JZF{@kZ?@AW0Ff(lXz_?c*Io zDVSpH)5+;+-ILe+o-aGU0PQREZ^L`Zl-;Z=VL97&yPfXg7u`!*brne%$;odxa^K?9 zCFoz!LJ3d=Yv0;gtU}`z8?Ei;!eSt8k!e20OR@PGoHbL_=$?%T*`#6;;Jp| zj~n5%iJJMe)wfe{O2S#-zl_0Mn@q~t#aX$mTq9+O>o_YWIB5^@ovU35M6Lj;m)^GwaN1+ot7EeO5K!$xTsD>Xo=Zxsb7O zxkCi)cxlewsgjadew)nkN;2(5qRZOx*)J&@sCwWgxFIJ#^!eSgeJkxDc~i$_H?qH} zS{wI|R5HZS+K@h-zb^g09mJx-prj5a7EJDD#cgp}{R?Y@$JGT>!?8V)hZm>@9sPuBC?A6%P^T2}PT@@v%8lXn0Po*(pN)^Akd~jM2-Ba` z=J+b6C4XOroyIy8^7i|V-FIS-h007np6An9xtwkixWf^bJ*m5UiV(0-$FCU16q6z; zrX*fIogaVbR7?B$$f$EaM1|b+R*M0>G|lOrt7Zf zQfp~tT2M19EOhAIdBgOBcGG3#2ukhldb&*s=fAlCSN&vCl`Ed{bRXTZ3X^wEt|}gz z`H+P~?~||3L%G{Vr{?@aJbwnafBcUV>HBS*PU*Gorlh1an&oj@R#heOS5kAk?u64l z{=TNGe>aEl>xlUg-$XiJ)b7rKiabHeGrV|;cR?N*>=9k_d>J*br|C_55OSRcK`3xF zuOAyWk+C15IZDx^s?7R$N0Eatz08u$N{gf`>Os)INu}hd9h^W3uqq27hL^^fMy;GP z#sEFMKeKKnkj1v|eZH!a5LC<6b_wD$0NfvZ>awnE;i2dEcC=ncuO6w00jbQ@CYi-7*8`tD{4#v+%{-{)w+Qk68cn+_N`{=LHF` zP*kx`Jk8q_DXRZbKBJc#SN;b!W}hnh8Wzep*;)yZD+3^6Z>4~Bs)2!htq334e|o`!-n7-gg3T`n@qh9!B=LXcp~Zqe#^kPjF(8cro*P&Ec`t8y zz2`Sfx(h7UkSjGIH)I&fdXLNgg!!BM$%R;ycpsaLE9&H?vj+NAsefe-VHY2+9){)XEDi4XJPQzHSYnEM6xj9hktbg< z>kc-4sAXl#VGWU+7?Sbp^u!ZZ7X`TLJd<81u-{b}RyL9Mi(<2~V5c?dyQ+(g02vWy z$Y9@OIXn`QsQL7i3tURjd^kDk@9m_)s}_~0h9N!)Sm&rfgA&|v$D=gFfok0oPPlB3 z#pQvI_lMjGHl>E_P)2-|lI<%h{OW>k9PE$9X|m&U(;qe*e0*VCGd{l=Q8aXLWb?Dj z1K|IxeLxBGxYol_NT}xk(){$mVI&YSLAl*{DXGoK|9z5xfS3WjVk~v3ftBynnLT~z zG~fh^*Ixy#W=||A(n5>=9AUNQRTWRyPJH={IJcn|3`FJvl|2UcZ;FVq7^zXkNl}t` zxAS+9)ItnUDdq9g04Mim{E=v|E0+E7XXy07^d-9C5u#=n!f_+mXd^KV$LsS!DbC1d z2*Do*FARu34i}K3NA@S$(Au}n(d;hj<}=1g$~b?@B=I9ErAIa zDsQt!nJKYyR?8Tq3o7$CO~wMloBwOLTor|Z9QIEW^*$-Sydfahs+JFx_|LttlamK} zVu61KW{)%)Oj>Gd{F#m?-$jWsNt=IoSPSMi`$oah&L%r`-3sH09 zBWz8<9{-01K#beKSzr(&gXw&tfat7}e}=ai^+guM49in**;WI?s=iYFMuM1kXpe$_ zNfOF{+b={+Lk_O1X_Su_v%`u!N3{$-1A=HELietHnms2dc~nyW_iA9x<-0b%H45Z8 zchKY{jf5#EhLo?N#Fez}GkwdQ#>RN}?U(6A^kJ{Red~Y4&)clBs_f8b4+CkVx87&)MA0Qp`yxxgm((L~3=MV1XHzF$#E`%&r&KW79>fa&31I4H z;M4&H34l5*)i*J&(;E@} zBdR1OR11x`9J%HWjb4d61&gq^Wod!=u-1$=IaBEd7qs8em$^r~ zwh{SvpfW0Hz5!c;0xEuFE9QThDa5$jN3JHT&Qm;8Y?)m>ZB^y zEYXT%>?e)qCp9h-_~EZo>^X98pXb?b-rw%uKXXeGJzmT@>fHm{Dd3Pybtwc9h`w(E1gBS)olM_wa9?&M;8k0}>sTTA8u)CoqE|GB`k z;llszzp)itWZ5<5;KE1mX6H1|+uru*bOO~TOUcQXrkqE<4nH8C*D!iHhzTe6z{Up` z*rnFKttsm;Gx(pMNzD+=cr+X|9_jc2kRJMh^2Ic&35SV=#1#?CBBfoFL2kT0(oPn& z5#k{@R@*U1gF_}yMh%n3g^k)E0xk%;LnKL(3eolU>-nfQAUUTK=Hg+ZATNN}fBe_y zP&v~2h3oluDL?jdcn)@OPbD18Uv$%XyUDoW9y5mrC;21d{32mNdBs3GRY`dfyL7@| z{+N%%vNG7n{yh49t!{QgiJKzrXR{i%Ml3Vu$U*=#6?``4|NK_1@Q4sulv4Il7{PVb zg5B6j>tHB{&~c_shJw_R0pwzZ!?rp`jfHe+!&crloV%aOd(!78YF1wG#fwC+>(`!6 zG)y%oEx3m6NK45n1z0$4x7rBd1$u|ehhygqX_4iDB#aN(!dbCtr4}t`dlfCE8aNP{ zU*R}f$bVqT)9aB&N{LDC8IC}6ae4M7;?~bcWEE?%njB|RL6L8)Z$OSK!zYI<8 zfajkww6*}L(fg}!FWFZGn9G;HolF0}@;tK%)B2x!u#T22&6Q4Q_XG6f(3wZYLvF7e*!u7-!4X02F z=#c#eD7({|OJu`caxH2Jf~|=mp+T&Hk}l{)F2ZbGP>-(4BA;@=nW7rpz`W(msO|0M zb;v%^|DRf!RjkXw;(kNi6Rjb&K7eD3^e!qh{Z_*9g<0D{F-gpXxx`9H2UXPwKA#af zk{J)c4B$}HQ#pZ;fQ}bUH_|k=!G4##)1qan>AkW$n-?*m;d?EF5<~0UIRW{6B1cWl&tvvMvyu0E4@`ySuvucemi~65QS0-JRevKyV4}4#C~w%{f)~ zzW3wp+C4v~W>&9SyLa~|1u(zZWr#Q6>Ia6uK?|H7jP?2;WI@Te3ssU9W~R@QVZW1 zZ5w02E(zNpP_8WQzHB%v~oFq4ER4M*czYvklKf#hLu7iP8Q zV#N4d_JfKDC>%}lv#SeMB1U6?46yl1kknyRHIMXWHHSY$@GC}Am+Qn^>=H^$aDsx!I0XrV%ZTJ&j~X~(7x zqAKcHxMwdvfyZjgF)UD`X#aC1Z%U6{SmdrNv~*_fWgMrI07TtIjY%U7f=?mE5sai2 zRJf>nh4Y8feTszg4Ez|hSas607lOI5uV3yHOipMIjGN6bW60euq=K+dZbpE@#5@kf zW{-*+s|{D;KYCyabv+wJ0uEZ<7hi0JOne^suyKvPcm8DfkxbCYv9Zr6U>XtR9oVtn z(~WGG@txfw)D$haM;jndPUqC%zCcN-m?x=n17KHJ_;Pofc@%{dDX8cOflDrHu%Dv+ ze@~LtZ?=;&mQu`PvqlNNuD@pSbeA;?8@S$!B7V1GPc5gg`0}XphulLH9i=9U>YFJ0 z89KiKr4el=B}1%ash+(+j;6Os>E`rfZ@|X~XT~7s# zHKm~(QG){-XMrj2V}=<@cQgW^+?Xre>PjRgR`-s)NIWRwvuLA3CXg?fIywxY!#ycM zM-yjAYwQ{MFbj)_kgWORE7}%-WV}OxzArD%LEu7&4}7qpyr%*ae?Fs!c1yT1(~RO4 zw<4{J2V$-J{u@J|CUpD!n}iPk*{_exB<ee!Jd>7gkcbbUgS zM?}7q6C9E+LE~TWLLRwA<^;Z`*r6nCCA7&%uah4%ecuIIKXfriF~#Vqhqth(-VRS8dII}vTe9nTe=6)VHKdE!h~1w?}$V0Nl8ku z;C&F%6xyffEu>B*@FzpUU{~g{h*2+u6%tTk{>jJ%5Zi$ou%+GIMcH5~loOjh&=>C4 z-f+SmPJq3w;LTg;pSZ>?mxY6;(&(_w7|?I1KXPaxMZK>ERkGnm`i|(y)hz*bi7bMIfk2`t;_5PP}IjT$o_+vb;|ccqu{^ZSq6+ zMtOH*qod&HsU2mhr361%yOE7lFTfeGENR+@82}a@d}Nj4`a{nUl6H?*9@5iFL8 z74yz2MvCqZ50%+6YHo9!>>}o%dJ7jS;V-SKq((YaQiX zrY23Q;uMiqGH`g=*>HbY#Y`vIB12|fP8vxT|3u|pn4Ifpi~nVs%i9yMBIEh4ojxmW zy8^D_F^T}ja?Gpp7ejQFZ2$XNli3hhR4{ZTgt(U%6iD8ubp3~@k^o`D`hH=ndl!L^ zB9eO-8IKzGUy&6Qyuuv(9@d*_QdoV6-8yGMnRE|8QV-0BKNy5NpExPV1Y~%%XCqdR zk}4dl9GzH(e9i@MU&Q6@>4ccc59MNpV_Czj)Y!t_+f9sJLX2%@=P$U%n4wAeO=^z= zo-dv2uh~;$Jv?Y#u6SVIOn&D28>29Y7h4N@%I{$#D;;TF^ki`2Ze%>-{U`X(nc9A+ zV^lAOy_C+FHK0z-l=EPdqa&~>e&^tn#(*O$!mih`g$-k055SqM>Iu|+?QF7m}d9>aOAE*2|HMtM&>ssy>qE*&N$E8Ch2 zYEUGY1RB)k>E6;~W7@T+%q52Pt_t(ZJs*miN(pvq>EN_Nl5NW{ZGAFbvvM?DbNEkO zHwdl#+{O}8c!UwP=OPI-O0YOtjAka60kN5%Suyq`2+)CwbPXiNL{ixyi%f-H82xc% zl&~Y_WhsC?wU~sx=Oe<-33Df`#OHp#vbs2hLI7qoly=feqZ{3`Zzi4-_kEwRFtUVV zt@{RE@U`x1;MDFmxuOdB9f9bFeeHbri%Ao z7Am|bDSG|EK_R4C{YB9cKeYvZ`U&hELA0PNO%1n!>6y$7f)E>3Wy0JQ%(r1#Ksk4T zw`SsuF~u;9t5c?ETms#`-LqFcN=2Gc9_31s7Ul<=-m)c~p_W8KZ+BXy3x%%$-DTdT z+r6QB0Phj}boJJsklx^kU)a?HA}O!PM>hvm#yK80yXa3WdnQT^Un|rE@OE^D`>76h z+^_eU=!m3UIOlAS(U&_zSX%|w@79wz`)ld&1CQgq#zLw6?G6nmaxhXkPS_NDPl%?o z4Gt?+AYXtxw$*sYE^GI$8+NV*e6)~TG>y?alqgaS8B(dVMDQWYDOg2)I!a9Ugp=Xm z->Xut?`fB#FI@>fOdNRH@cqOD4u{U;Rt6L z0R0y2)0%xWH?=v;KkFx7JWg%Pq9lSBSQZ2kH3z=V<+(Ho=31}Hg9%YT^2!K%AqiUaWrQ`0K zzf6%ZdLrteHiv{U&d^E-r01B-nBd9J1p-Ky7%0}+l|>6G0lsgtqTDj{8$W2>;=Q(H z<|BV42uf*W(%<2Dv&1=eph-lMwzZi&z0AX#gOybKX4eXLGy?@m6Ni;+4>+6bCdT+O zB+<{qmJt{jZrTZ$GRh64TYc97w@z9^O&ha~Ro4Ni-+Yel4Kzg~+fDY`E!IrrBqdaj7;_Y=Q5W0%ij z?I7`AQe!d&vsD$d6-(U%6CJ(HUY{=K zeSl;7oeOM#7s|8ZiTe7Oc*5oZIbbR_5c-|nU)z*}9Koph7G{WEmt;>l9 zPW1M{`J%J?1J}rhU&}8KF2kBTOnpzmK4#~H-d6FpjtxC}#SfXTELt2K5)S59Mak6O z2$h5yBTBcI77Bj>)hEaPj5^N&N0nJB`i?Cc3|m5(484Oax88#k>{NsYHCe*MrhjKN zmmQ$$-2ehU+s9hJ@Y`X41K*eD7EABBz_Bd`N1yBObL)ERw-Hs)!8r9w6v15e-FYBw{CCEQDK|W@91k8 z4vvqM3*7hY9YoYQcsDiPHTYbsU?fDqH9Q>f3kFkz+m2(}M&%(c zV7}2|J=A1C734S5FPyldDqcgh7WxbynZk#>L6k{n2vi0UV?W8L0_4r9{Ikm~XGBWl zfKUAW@DsoEp3?K3wM6B&YKj%(+6%%sZ;y(RZ_n4OtzxwP)bqr2VtobfOPv;)Ca#5U z_+6Rwu5Ji3!)x(>&-TJi*Y}4v-4NA8{z9PjtJ#KLHrJV0U;);xh0I5%N_T{d)U&QKRCQ zyWlKmXbdT?>&YyR5xWZ8x4^Hq3MBCF|Kg4B@ni4f@s|!NuasQ3RBAQ~h6X9+7Z;ej zUrkOj;)V|s@v^mLntVMU`4qOZ194E$nr)BS)HeLZ2TVkVkilwF7o3N0aD--lNo*8L;> z)|1=hJCKs<;Dix;2fj8RiSv2Oo&OsmLxVyl9mby!(W5ygmI zM{WV49=`y;2V)7MvN2eU0bq^9r|No|f&7PL*TOcFi#4_SSjF@g1UH)@j~etB;Lgeb z73nG=U0gar6*4jyvl2T21Ww6c5c?b~YTWBEo2=TDV6QLh$(yT#eC~i*_K-o*ZUZLJ zAfSQRjDBsdSU7_ur$ZTmZJwZ{pyylRWYpAl9Byop4hFh}1e|09tw>Q+6uvrjS~2@l z8yYpPten5h)5fpLdvy04=PhphS61yy%I}ZqK}G=0hBqcnf+$8c>S4k$w7rbvfX_q~ zx@F_a^M2j4!7x;;*O+EJs=D~{c6I}SCi#`s9;7zNlmdj1Tu|9^B147x`eFChb8QYE zdURzxvKu?+cT<{YBUe54>Ne;q$zB{M!9mirp!R_MiUS4Q9yXodVZ!T@IW`)?LpF+seJu`K>cEa;t;pj8j;rsrpbw}(>8cF$ebv}XOFDOVU5`)1S zpIeL1hS_lztnFcQ4BE5w_w?{jU%tAo&#{fd)EwuF7y7ps{!4xCU|q$i+l_6^S^j-q ze;<$+k@%N)g3pRl$LU{ZTUco?V8kjcUyq+yyuj*ObQQL~fIPBh9&;v5DY(D;Qy0Cq zQ2M{7zf?4-BTrZRGfKWAaS6~SjATfHxpI9T+=uSqyvw(fJ@MDO9&YU6YW-0T!vm8& zClD5zD=5lVo?jxDy3ZKdXWz!arco)0Tm;i|1X1!O+JQyepHW!j_6K>@qq}1(l!2mW zoDnakR}f8rrl{7Pf^pMCUP9#N|26e@_Q=?j7Rs8orcaK*#CnCi15P=cjJGH5BCb# z%?$!tRC8kap3l}-w@8-Ci@=VeHeTLPF z!WldpAu6EIc@-m>OurmE-$!T}wAGxiXyQp&zj{;D#vI(zO&Ul|%r0EO7aI7s~K)hLlF9q}>%FgWEBE*%mFD>1Fi+u=dY76tJ@6|H2 z$Q?#}(wiZ-OgUo0I(2ch;nPv1Nsi}FCdv_1c$!`as?ob9%6m8BXk~a6sCpmHe%^Me zO8CpE!RQ_1RqsD?G)>8HUZ(cnpkow$%SUk=U< zBj@n+x+3yhUoLzPjkIT4sMHRmTJp?YU$>$Ni@-y{_WU`+WU1i~UEi(mRvVk5eY3sZ z8E~qxh5^{Xz<0cT>O6T?d9}1fe)HS@!0r27B&7{-!V)5^7P=lkBRmSz?K^7N-W=gc zUCPZ*2eo{fkkOpGZsbt6jx4eYbsLk!HKcpwBus3;L3dO=O|QJmX}(g_gp8&+D%FlS zG&zm1R@^4tCd(W(<##B!q`ul}s#DR>SO}<9Pnbt>0%2a2ic+XZM6Cbis=HcWaWvGHH2Z=8}Z1rNe?Ul$_J^!gvIudYkpx-Bl*6tXa0mKpF4dQQ?ej9o6+=~o<@!6%e=c)b9GC$l- z*G@|42Pc%fCx&3Z7i@WSx1*1G81OqV@kdI3nS<80#=xp3ky5ID+2@&NM*B74CEX`l zA2&OZob1gV|$9)?Y^l`Wp@*w^VMj;V15Zfz&V=&7P8+Q z4oZ365XCFIjV{W&JelL;YCk+^)JW-1KB)QvHYTZ}^Y;4!DoXd$3>`%yGfd`&zpUZD zk{DWvih5Z0C&4iaBBo~~FWKYwyUHP-gP7!D{Y(GS5(_lnZ9Q#>fB;(DH4 zwclQQ3~^ru*<@tknJwBV-JR6xCVn}hQKf0mv92`=aW?JH5Csj>ZuvV9pef;oQa!wA zTOZW=*zfK>9O8Q|smn98&duQ$v2O=qZcbNwd__{}mOD+y&sdcB!N}$NrUY!$upYGC z;aJZM8q5*ktVw67urmh4rp}2f3ceXd)&8-h#3UGA9c3~BM%Uk{d=%#Bq3YI4Ix6vp zPRvjXSGnUdZ46IEqed6v8cByn7zXbtqC0NR>N^8?Jt~ZcW_Eoq~=H#4Eqy_3g>M8W}GdqAi{C z`Xm>J-yv{35D`4+JZik4Eyv)|6Y1EWspq5!m1Z}gb$#FSUiUz?;eATZ|1%*r-|vsb z(~X}0ocL`Bz}OH)gZIUL;Nsn@F?AKoVDu^cr>*auQ(nih1GmEmqRY7Wd%i$_Ei(P6 z%XOxnn=(|}lAG`K-jQcO3pfu9qQcpQ!S+xqc}34mpBO=n@4Z3Zmp%WnegI3c06fF` zI*khgj?l8lYAYq|cYz%<#>~uc&7{@G8(%`Ne+GKRpTo#ft3F%xoz;hRN(CVl&W1;& zl}K6V>not9xeOd+uz)v;y<6966kbqIJB3{hC3`82x%JFpl!VG9Dip~dQ+dYZ$jK(nfEZ&7R}gyLqVh&yN2{X?6pFER7@x4khBhm}8l>&>z?;d%)u-;sOJq zBUp}wfQ$flYiSg4g#`ehp}o3-2QbxYVv{&W51}y^q~2nL)-MBt5`b3KZgdPiup5Cz z23}Aa`cs>*fj{_^dX0KcP~wS}qZ|6B;20~$uDCRlYoy%N(wr|b3amz{ZC%>QZ*6}S z+#!e?*4xU-*+p~-9lgnaN>?LRH1ik-6)WAoEKXXU ziV=dRYZN`Q3?9*g+=puGI7R8Z=v?vL2neltdF9hKR>c-4j%>2o=UKO$FXda_Wp^QX zu|^Zq6F5zmtuCScn^Fx*gF8NpqX#a__@%ceOViw{B5IYEX|M;pzMaLao#x-oINCVt zOyx07XjCA%(;HJ+IR6)$yf?>h1kJZjHgKbCpWZgj{A1;J+a-;jNPS;7)@o~DqT&V^ zTJwn;l7^$Qb`FK#sD`4-9M5kk-o0-~djP)e_R-gVYD&iqasqWk%xY*Wrs5oiHL zlFkA%8PTdd<*3OZ7tkK#@Q+NM4Yl{)9KWiZ*((U|7o0qyVUX`a54~PT9vNe`jPKq* ze?gsBp=cZK1!QJUToL0tR>Ax)47c4?^x?9CMZS>R+gnCs9T|Jotps`qC!e%Jb>lp>#??Arl z&K5SX+lghSzR9`YZTT*k=8{~g_j)be%Q`nc*}gXfjn1y$s)})S94p{C&sbt3 zqEBP`MVGuCxqoHkY)c`E?P3f~ODPW%T_{H?_jA$}E*UeqMMU>$j{A7x_w+y+G;HJQ z{gF{j(2@6DkzT${hFditto`ovz1@nIFL=F3gPrZ+AAeBUDa$OO7w!gEx6j5 z6irHf7hvQW^V&-&shBzGSuEbZHmQpsNX-4|>>u#> z%M%L3Yfh{W82NAr5Zg%*bE*kQrO?%z{Jo1ijgt?0Y~!2nH@9C>q#We{gN8b8Z>Kiy zc8cGRvLs>zJvpTa1hR3|3B`qNzS52jw_>hN3~!QMBonbATXm0&R_j1Ivso^6hv!Rg zo=0_fUgf^D&GFn(#Yi}Ejr!77+akNO@U$^+`D=q+Grbb3r+W5pb26{^Ioxry6P*4m zS3GHXtazi`<=*fAJi$C`|HsHA8a zcNo_QxYibG(`0a??W45+7Cp#DzA}3Fr;(B<=Dop`b?QR%pA1I@Q@+7e8jno%E4vEy z)m}~x5|?-*KhndOm7<>yU95oMH106&UAc}b8CQAw@hu8#7swri1$E7;OnbX`zlKn* zf|s)q%ussqVFMT7c=R(dLzgU&9LLa6Bwb}gdN9xZF~GrDY6@4Nc#8|hP}r-X0)mOu z7b9qCjnxOu8%cm6BI=7K5QPzJ0CIfwQ%5hra4uRq>K&pY0yNDTiKR(EH?#}tV4i-h z$%apZex)=ZwChNo*D!k6oFM$KYDX_=9!8sv9uXGif&i^g%0cOmbEPsla@TJe5W9j> zRni^xhZO9P1HA)t~ry%Ml#r2;PdJ(PguICe9yM@kVYsiHRpM72i0KI;t~y!hWpz!FQ@i36teGc zoP0RXaOXoYq0PKg1n~JI82a6VU-jN`xA(LJ(i|Jkq)H(J`}`T~zMFfS5Iv+>uXiMZ z#nS(Nk;SCo8jov(&9ay>VhW9mu_KSl*GIn6LB3-X+zQ+L5g#vnS<#4ogKP6|C`UQL z7A>Icb8P*^J6dv!WS<(K`j%i@{mlC&t;k~y+1}?#$5qFZP#h;1R*Zx#Vvp(Z?{l>t zVh$Ej={M%znj?ev@Z4d)LjvC}eKOqN`DcthLY+*Z56{K$w`DqOG^Wm_#npBUowWyS zs5Jc7Z}0a1Xl$SN$hHEW2rj=q?k0tvP3$|YFuh`{SFzjJoX3^VeN2#$wnHB$aQ?_F ze`S)j2TU{}Zyo1Cc{F+YbCm0QAeJv4xV}VFOjCLB(Y60PAYc_VM$#F>V@ou_YJVMhRlaRU9sgMMXqI zD3pYrQH&U45gl4P#b5g6qKZ@ueE3^(gR6#W5RD$HZoS5m7pw5gaeEZ-?J0DR;uLD& z*G&cI91dVcq|%u)9MLQD7>j=*N?Vh8D*N?NXPB!Amr3r$(=I`K;i18#*7Vy%ud1VB zO&M`@9;{X_s_W~iTI8FNE`kkpd++ebgYj)VPO9KAht6n{50Tl!m|W*a5a#D&c6f)! zA5VWD6B>BA@}X+7v2Z_zH~8`7ebA z9br{v6FV!Jbu%kf^@u8?#*wzU8NsEnEXSPQc=eVAB`Q=NRI5C=9CtOsJ|8E-yA4CK z$&NJbPzar|NPT51hjbOS(0anoWT`&-W#`&(MKWb0e>~6s z@Skdg3o_E=^UL%J!^-ggtoz2eAErtxonjA_olP1oD_4Ukp|HyIxVHbI)uT4W9an`T zTAxX9u$o6Gu zBuE-fI^e-JtSZf1hWnTU;Cy@-UibVv5Wg>~2CII|{p8E6B0smB4E^i1neTG4GhuM` zAfP9Qb4;W~l`}IHAP_RM<(I2z*crdw%N*3%_e$du=7!uVXQrud!$9;I-6zvS1bD}p9)Hh=q?xdvDMRYRY0goaP$rNwcUQyxKQE%qrPf7WP3PNK=sC0Ui4<~{t zhp*rYuVuvRsi(Ws&HV%tBJ4kS7c0a2d*JOdoB~NYAX|OIsr_{hK{2LUGFsh*od}H1BIs#mOs{SZM(?Y^}EezCr$aN-Ehisp7aALJJg_G z{(#o*3Ob|*lXi4)k49lT;(iNJ{`hUS;eis@Uzd%XRe97ApzV%Xy74hJvq5 zTN#hKaZHS6_so1uzW3-eN|_T{K6r?OS{6!7vV6ifjmqjRTmA;|waLzMmAGlwjzzff z7J)Ys1}d{^7+$08w#6}Q$CDP$jj*osX*Qu6hz#!h`VJ?&ZDk`(Ta{H?dK=kHkA$r@ znN>yBBR(8moIrj_^uhA#rCI#Sh|qYPtsEWSFM~hK8i{eltep5oVP#R@1@_D&nY=RM z-^I8Or53q4Fc9e!@gUy`Do`>X@WfU|^N0Vi**Jo!IKe8O@PfwcFuXR}jS@f=)yU-{ z5>MY1?HP{;RyO^%5GUr5?du9PWef}o<%!>z*iK6{_?*+Rfngq2^hk>8bM%<_8YkaF zl+{{odMhLSzB!m<5T-Tz`X%5b-;)A#tjH#~Xt+l;cAL%ExLEnm&6^ca871W_#l1Zk zTBlUvDk_G7SoDU=i&Ec|Bq$iuny0V!hJQLlHP_k4Z(TrbfhvZX!R|j&tVfwrp8Kn% z`0fuR=oEUBb7k>f?lJ;tD90|sg|kC9#eeIclO$z%+QzaPVtWPecrcU&AO#INlPEib zP;G9#t2d!@!Fh{mD;0C&k|Obab|I1YJi))-%DFtf9{$8+&fj5)6y!f97tY3EWEyqF zl)1~$tWNc0beB|F{j?w-nbbU=j6mWJNxDqc3_tOmX2@`xf zLnh^x2QgcBZX~wg?f0ql=CFa}eM26bMVX23WbrMGuIUWQ7q^JtJ zrF;i^G`1>j$)rf?k-DMk3?PmgRrSbi*)!VfRcp!qWZ2_``FE*@8Ul_!1W`b15P=IF zP5Y^x$EoHB_TwFXkpj-ugLQpr%$?E|Xb>?SgT`d?YP|3=j2oI$_q*YZ!TDh0|je|FndR zlo%GGsw#-MK%!Xs!tS8WzW>3C;Q}Y>LvEr zI8W=HI>3(tf|~}{1A;7`Cu8qX$?M=o9XQ6`ogMG|fKljP7o0fAv!p%BK!3s~3dDLNc4qK&2J99@tCw z+~x^(ZifochuN8JZ-2&SQQJ$MG1xhDqEfa)o}Hvd!p+~{vR-MNto)4d`s$2-cZo9% zYR`4p_Dn-wqs#EwgFV-tiv6A!+_NTKxEnm0(N`JK$Z?2%5HZGpr`2{MXJ!qN93^2} zXUbFf6X))#FFa=<-dI)}NwxsW?fI8{EpOO-Wmp{azd2qS)Df3mWMoPr&_Ni|)_6OS z>dIZjUh5TqY`7|USyRo!tVEPAp3TkrI68W-ie$3eGvjjHXQn9NN)Ba0#c};&S@vV<+%_dL@bv7h{8Hp)}d?=!bHU3`|y+czqjV}?? zqKZ6C8A3ge$d@QehhvREZc@7z84i4{9=szXR~Vw69)$Dqmlu<*@Ub&w+(gM5ZSE~yRUf)Zbhga^~! zbbxpUpAUQJkBqfi0`e`;5DaNy#Jpo!cfn8&M*T_ftDCiz$hZG!!#KrDVvtWm6)Px5 zuj{gBUQ_qsMTai|MO_Ra_?(fxg$-=vc#)oCV3#tvQ&w;6@a~X|4tH^kO$|5nO(?$@ zWi1?w>jqIHT?Y(O&)mHYEVAlBwHMQ-ga|PsV z^oY=KuC6;WW5^k5b{#Ok07+u8(NR$k4JXo?ozb_ivBYqJ1p#T2MJ@LC`^Zu%7gfLVMCW1sR!(YCi5t0H2)VM|S0}YQ1&+E8w z=UQT(Cy-fk6OBB-Wpb^ZMyKlw!u5i`DFo|DbnOj2wJhXs`tl8E%j z{b)ceZIu(2_q#wCp`U;PN#Q81WQ&6aWE*_g2|2+d-j+c6>9;D}u>*ME%%(Yxoqm)mTm?*XGj<$( z&u9;^{B)qfHJYL+a6V_|$3#>>qv5%CMf&;&x0UlmsTvRfj_>{< zAYa#gPs?FN>U2d-Mv&va{EgQ*W7waR{yFhx+@B`5SaaocEmEo#N`Fpa@sq4|hV%tS ze|!o^bUiavAk*K31{Qw!W<6@tTv%1<2P)){_I185?A6$KGp;B zjK-6Nv)@rfe|C)I&nRdf-5wxZU7rXUN&+RfI#h8u-MA@W^!lM}J||odKBus>t~K)e zXB){@uK$Q``Edq9Xl;mP`u;P|;rOwJ_=q<`gGC_qVd`*u-Qy5!3Jc@sK)MiS#`5Hy zVR;F6?6QC%n5Xj$9{+Wwt_JBGCcksQ63#qL7pOIMUhNvgK0m_%%x6h|czzA=mku64|jZb@nWB@jd^It-$RFgWPnsu}@ntSCxO@yt>B^5M-SXz_;qs# zDnJPHV&EH77Zsz7Gd%Dt;_MLnC~fk{x8gf6afcer@(Ce8|$O%zU8>8 zjQRZs<~i*UeKe_py86vV@WjN#!qv&$12q75ci5mFtDXzX3HW2f84$i1HN}8pbv4Ws zp#=$Eba7Ds9M%-~uyU^0ou1kN zl9vn~0`z7QF-PnV6j57L7WIEu;rpc=VmAab!c(2?srT>8vvfSuu(G#MWO{)H$2>ZI z;<+*sVU`wQ9vk05k(tnC+7K@Jft5hu>V*s+h7B8-QX+$pCE3{a3qpcMtN+~ z@JL9n6x!0mfm_D&`vQ_J8`R)O-0dy=_yk>2ROWYdi_~5HiLR^e&H4`*%HtjXFcwUl zp{OU41G@=NhhT9TCqhO!7&pV7X_xF=dG11p%57ty4~)LoLW`%?<>#XuQ3?WM-HJ52?g0cYW*HjNH5=!$;bGY6o{kgG58o z-HfOuL^lMZ4q>HX{C>&4POMdi;Ps6LfSUs0u5MnrE3l#)Vdp>?(il#*PqMwKrBu=% zTVmC64m-O=Lsn(Hai+PeQJk^@!L7%`Z3lsrjSxK)HL+K!+F;UrlxW&oP(ce?`-T@s zT}X18{G>6Y_qIYUdWaw}#zKO=6N(v9P!?RHIuD?@=Kr}SG}~ds+U5Sm(nP7jhVt(- z2*q)vgJE#1 zipV{_psHT|(;2s+h1h|N3KIEu=aKtV^=C_}h>8Rda8WI7X&7OpFtUqOqrbkm4AixYeQdJ$`6JY287u+xD1L;K<4Jx(c zgY3fyyh)Cg+%36|J^n8XEx7hSBncLo;N>$YY3~R-d&Mh)3^;33jzXH&4>V|zCtNWs z*wVldbZjl;I@a0xr#Pe!rexR@MPm6IVmSNF7*%x=^g@vqQW!DrHV5ElODu`KW5>qI zIioEnO0)uGHB`NV_=2rxEoqU2unNa*Es2?peGPSF7^(Noj#P^~0EL~m7#>BFP*9x= zJ&sh{4i-X-p5nmmHSqwQ(g z0e#s=qQ#)&_Zv8(l88y`AgV?eeQQYS!IXsbcqc9DuKo%<iT?Wh21eS zo>-tmV;>=xXc?b}3_FQ?iA!e(4TAocm1$J%_wg<-|2+m~MI4pLgpaRytchx~u17g) zMJMd{mwhnlQfa0==S7n0+6%K_Kwu`GGzP5d zu*lS*_@e6+OM?Z&<*N>SD07}3kzzPTzT6;QZFr6|zARWfIVxsP1SSG!HI&4Hqc#KV z!ERg|iJYKw3MXk&e1n>Vno6TI1S9wwUoor(c(%Iug;ilcq4?=Wm_;!3Vx$DEFsAr8 zADQ%-P+?^WF7R-unEJ0)t$;U@z1Z?e+k+T30<;v`_cUV^zZLXQMkj!6D)@O1SzCoUYll7TXKyP?~(52y*%}=~X5b{PT-WO85}e&uvDTkPUlrww#raOHyMc zFF4h!@-kxXXJ+7le?gVEn_}|h|A!nUJ9VTfgqAE4k%%_q$-%D^2tMX>D3ZS6iX#15 z1#NE2CM%)3omDO8Pv>KnCB=mtA*?0qJa>SrZmu8&^D7Kmpxub_I|#f{UWj=jY*QD` z(fW7H?x3p%wvH5^6D55p%B+=m)o&%#-SLooyO&0|%EPhUvwC7x(YiVN6mj0_V6 zR}vG8C;4l0qFz1$ugyq?J;8>Zgl=1?JnxOzP%|mk)=Ew=nRE;PoB0HV%Oe`We(LH& za5I7QoWkDD6DCZ0TI>Yo;N4eT#^l%!N#W5A)dheHl*7gSw-1$t+RkzZi=;T26YNLK zb6oHO#fvxDxc0_k$KJtm!S$c;Tp_YG_Xa^>=t>YU9^XE+eOfv;*RAXcbAgFJ4mk;| ztvL$Oos?CYuEoy_3mVT8;nJWyWIhLQ)em!Dy~n1erh^-56=rsi?|@Je$8ohO0S}ad z0cbDoP}$4jik=kS0_Y~Qh?*_fcpUxvmy^AEY?qrjxbcM< zKv?!ij9&M8|1_tB1+jskf6aKW%4l6?1`>WOjM`6%VM%!0GO|xc3b{gNam!v4-i-y%5@(GTyLA5E-9U;MX#?#gFNO!;}GS!feTM-o*A{FOC zDm;4hzo+Zj;Jpw;{Dk9=xbBW=}tmYU@j#eo4N>P$UTQAwI>{*`tn1Lj=f znsN$gm_Oei6V`txmVU%9=ZxB1M?@t8M4JC(w(>6d=HZM>%M>o;Y-EWmEtQ>JEZ zCM3@iOa>&qA7ULtgT5<=aG{G(LojIP8|+?CWvpW)+SpNXmupgn(Yd_)b4poCe8(%9C|%G(*lv zs-B5ZCk>BRX5eyz_n^csHb6 zjecf(eIm?N(cvZ2JaMXx;9QiIK%9~elPKSaC#BuaaYHHTy&RHibJUkE0?Up{3&XmC zIt%z$J{gZ-st`{}r5rk=9z#(~nZBV6B_#5VsELo0WqF#734H1{seO!Hno0rI%rw*L zC!s8*)G1o6I<7X57B9$w3(}iq{I?-&AOj@XrU(#@p62?TpwMD82`s%m1_vSy0!6B} zl9%V_IRPvQ>YZwflV)NO0FR=9fuFR3yS81JV{u&1Q;=Kqsjg@>9;TWzvWy}g5^w}5 z_|5$0(TH!izD z{j9aCZHJb>1s~sveWk+4f0>KJB(Vg)Y4 zIGODIv;&y_gB8xRU0$BYj}L%3_hIU|DCe$yVP{x1JJVsq#ZHP7mrzHEG;1oFbnrsW zZ3;mNifhP@!6PHxdnW$Ync6RgG>S$+2oEVm(?}l(fB+5!V#o$zfR*l>-YX#CHP}W1 z(P?5C)we-10`4XIQxUL(|aS*~1&T*V8ESO?CntE3)*ExpHA8G^Cs{ zdj_F9CUTUprxS>yj1owA{|OLIoF&0BV0~`jNM4cXT$F_XwCpcFjHDl&Sq{OOQJZBy zs>EKv#rtg~`&=;W$gg6WurK$sPq6&AH~x-wWCfHsw|44lLNtSONj|O0YV}sU?)qgGz`1fFVgoO32bmnwV4| zLth(38io|;lvh9$<|n-h3_b2dzn71~sYlk&nSdD&yEvPKa)VZ~5EI8k!=JUPmfs*{ z)ZqEgo6MDAgqhTUcTzmOo83IqRi;PB{}SR(FglbTHh9;t z6NJ!&XbVM`F9W4wmCM1P7p8|aPYh$vSs_3o<;E=r?EX1sOS)Pa`X%zu{rS2^gDmkV z19B>YBCGzDSBa^C`;nsRbVv@TjjjVrzY)GdbIo)@8nw|FENzyEM-aPxnB4o)2f1sa zz=|D~Qf!#znwok7#!yF+ET8BGIe;k5U2Ag4OtJPA6|G{{P%&tReuX1(KAJK`aq#X7 z6?#$_bRJf(1OmwMCd_a6tVYrz5{Iy7NFf29sZ$5Fm?`HEYoC|sRDA(v@c1vo9DmGI%z05#O-GAoP#$rfDE#xJK=EJGEH6}6G_xG8TmK^cX(Ba-;j$L3MJC|havo%ygUavNE+D0LG+Uvu-X6d=;;O4?8xMH~Q@l3`?1 z==p8j(U%%Izzpx4ZZx9b5{5py4^J8yi7CorjmNR%ml;Xs1TKw*1x)`eaTh@?vhIj1 za6bKNW#E7AM{vJ`W@eSzlV);Ya^hGGX?}hz!Nw^@slFDXF9H{6F7N#rGfyPXI^d)4ExY47SD-86`nLXD|>Aw874`Ncg1D(jn;>GVu9BkK6Wn&ZTH9)IEJt zCE@xT7LkB67!PbVKjL3t+3^CGNJcY)fjAqt$YUD&4-D$&}UwV+W%|_hYi1vT)}CX~n%O*$?;E z*>JPuV6Ua84uDrHJj-zxA{Jrk1bjF=7XMYU{>dLCnpK-Lo3>I%Z~9cY>9ovp=;)#Q z{`rZ#h1v<4Zg;j+2HldPSj{DR?*;k2@;AnO0sAe{iw6Qbcl!3T3NhZvBv;8 z!s$HGfy9-$V;g0OLel08F}0CK08eXr&AyV2{?JcC@JVU4hz<2Dnxt6(@JDXcx+R^y9llPfu`tC9mv>>yP` zN&@2lKTuI)GNJIH1CGvtyoB>#VVbP_-0)HPktbuiYUNWMt!A}YGfT$wa1wGCp-Ssk<2dtk8@@Qq4EL1~auXJ~9$o2h#4yR_(4z_zdAQ{B@wNSKD%Qp#e_nOlFA&O-Cz+`^ zuV|;XT$7xk0l^ARwu$@#r8W5^DH9Qi--CG@)ao~NoauA<>(kv_C#y8TtyE)W}SBWLU z&h7cRLq5M@LNn)jeVVW$Cyx!G3i)TbB0_QV{10pj$0CPD{4ZGf4=ljJ<<3gJFLGg( zm9}hHSPoCM!1Oiof!aLV(GWB=avBHmhImOo#2#*T+UJ zE};Zp7og6#kdzjrEAUojJ9dG(u?r+)Ya(9-y!vS27TC0c8M2qRgMi<*LX-_L52$P~?WJ*(E4V5g}MZK4LsK+8Rp+-hs@-n3E)Z>CfQUyukZu1r(cz5`!jNKg?-+jt6*;uyySBgOL^g{oJDgOiVo&NYd zT_9UENpipa-*yOLlub1i4jZ%* zxRyp__#}tq>aP(>MkY=O3%K&$@uCH!7O=v)8~$D@9tcvpf1eUHkwjjqY$b-fR9ZkR z?#R=)L;<-1RD%7MqY#6USVe`ne^>Cu0<{J&f^)|#*i9FOWxuDOE^{v!p~MjIFR=ix zei?ni3xaSxB#G{g`^v}L#Zk%Jw;D=bMcrX>4c)U&H9k(P1A_sj%g|32)yLuGpE-+DPp2PDJfDAR;h z;RjA$5#pGi1CljS>_+N!soR?!gPT|Wdln!=cy|SDT|QAk%p;KOq+~Qg0F>YC(OA0w zS@|`?HhaMD0ieMd7D1n#Z#t1jR*glxf+^e0-mceI!%-6K7o{H8GQQtQ_^K!FF%6B^ks#*V3@GwDYfbqHi}R;hOJv(9av$ z)4X?ze8ws{j)U9RS#6`r<{yyL+yq8fAo2SsZ5{uOZgUFc)M<5?s##CIoZ1$G95{^} zKG`95Y5U?$7t~H#X4~6_>>$6lRjSjrl;~1ibA(riUz_3uNY)5igfXU?`LV=;&?-PX zZS>5IoiGtfmgv2IeAGL%%wbrHhQO^S6kR!VTtg*AMM%U3myE~Xh%0pBd*EXevNJm* zHL&WaIT3Xl$y45x?fv;a=yMVedGmRTzNF`VRFcb8w6*PgYu+<0x(KX2LIPVKoG>j^+P8On9~qU}7$m-9A*LZ|Dt{lYIl>=^KW zoEH;-jy>yKU-<7~3J@RL_jVIzgUa}P+Mot%g%zG&C zEgTRIp@+Gi|Ey+&KJ7*flz??CF+#1d($}&si73YzohGe#B4rTGdQq|zih&t~k%bGh zPGXM1P=N_zF0z=)eOMVy9&_P3xN^ba@z`V0g8}=x?#2JmKEzeH65M10(56TmbKzqB9G?Yyz`__H51IP(W@UqCnTy z{oBh#zOdWo5PsUanw#K2vGyaV0RYWn+IXjO`*te+b@F|#liNRO33ayRRUYM{BiY~Y zlS>mDB+?CQGT68C^O1VmVe5qSleebt zF}O_)7>teVetH4NKFPb@3xB$ec94TZLg6{`Rk5N*hWqX3;f;Dcp?`a_WL{nOQk&Sf z>JnBO-rwCRkT&M-&x-u*>&KI(NTG2z z@9^o&9}%1oIHm|p!zT75!MmUamU-?7GP2SLD*@PGrNlchnK4iFID6W^;Fe}GAAfz$ zq9w!OW%n=e7Cg4)vY3yUCqxi=3tH^SdpmszK?72a{mgF4f{PBF z1mC(RW*5OkLk%B3cG-NMdfnzb-3Xt1b!6%qkDRtXQsbS)jo7w5FNLmdd;EyH?6H@b zgj`L`b(gQ7r@Lc#y!}U@x88l-5QN4z*Q;_qW8QrSSQq`UZ|)TBV}}i1Klh6By#=as z7<^7IKl1dCkozat0|8wpboqOb=S10d&mhEIwc)IoUu+ z!8OOk`p!(^9>x-WcCn5titYDbQ~XL>zrZ{B8HpE(N)Dg%cP8w$;_#nC28PwKomStp z+gr^8!impzUrNty^tw6p9tS2gO$1X||3gJ_*L-E=e!fT0b!zj5)yu*TjBfkm@b+`M zO`QEBIV^yLVJxqZ739#09vVEe%)~t=_8rq&zm;T|JlZPBL4)u3!bTEg$`YC>V(&)( zFR)YGx4*-P@Dkar1i z{(ChaGrQh9nKUj~Jly-ipjc~o6q+<0Hx_5RSDM!|xdTeN6H^{_4ApJK`Kx+AK$5lrVRapKd1PS2e zDz73#Pq8w&F5PaPtEr?qD>Ex`2v}qqB`va`%^KHK%v;rLd3%Tu%UJ*3TvfpFKuAMBV7*=p#LFilaF$e* z!mX0niywK{T#D4(0Tsd7MsBsD_;h&}A&L2G*&q2@e>o3NHYZA(CqWMp81+>cLxjcQ zgkWazf|v)8ffp+rVw{~ZKtqc;jSt}xk|Co8mtBO3NOM+_4n~L5(ffwP;PB)zN<@58 z4qP zW>>Y0iNkt7n&UP^hf8;RQ(I5UX(N`$q~qqZ*X_NNJn!fH*G|tauom3`X9X6kf2o9B zCFU2S`4qVP_5-IMs++kk``*Q_w|2PCXzu!6hSCM4 zgGW|sF-Cz~_o5btErK4)Iyjr;!08Jq$wtS*t(s-|YjAQ9-Jci%TJfDg;N{Kc_Y{@j zeeWRE>s1mtYk~=DH-(j>3tZ>PBRhNzXxsg8c*_lw-tW(!)S{(_L_PPXdD7^Ck{H#g zrP1>Z&KW&ITVre3-FeL?rZd@o^TA|i=2J``D33rJ!Pr9{>yr5k9E{RDB3ckrpLC-aghdIro$}fo>6R~)7=OAVW8XZR1L?dXW{XG^|pYF8Z{3ws4SPANVzYr z!vIf&dwzjL4wJ_DGTL@4f+U7X$w}T&9&X^`!|;o(#p6*M7gw>9+itQZ1$tMeGgzbOv)s@9qVna@xiyci}Xgx-coik~Z zgt-n#^zar(S%OX;G96XJFf92h_*&NVG!^P(LCjVu*sq3S&Z+KaDpKte#t!W}n>M-GjQnG7%D>?io0CzFbRIPqXdjJ_#au`u#HMfUNk5uZ@- zr6axBDsd-18@S|mvAN-~#*bFYAgPZ4_NA1xE;k(X5I1pr@(JEPY)@Hzt@J0x)6f&h z*YLTR*Wo$D%6`7#)09~;;Ot9ZH-VH*cs+w;tq zQHBOo1s8tJO2-HYszD4yPL$H{SoqIhYIq?_Ar%t!A~a&6AE4BwO0AR_(X7HT!ejdKTB`bU;55yI2A$T(R1ge=lb0B-t&DdmFp^yE5zrv?RB-woeuvI%ndykUZa85ydQzx z+Uk%)r~8Vrrx(7axo-EnbGiMb^?x9&1FP{16keI}%~yy^ppK_4YTGBwv~0S zq@m?=C~96v^-y-#&}L%Mwkq5li^vgoSKc`(k*%jVTQlPh&_Y7an_|!98R!xA=n6ga ztV$3h?crl^u~@x4QE169i6sXTVZI2(9VwPFjY6VaL5d3=9EmI?%Oa^onQ+pIfq*Yl zYf(UFba^8`?%OCDKLKpG(R zf+9F_DQleZShw3J{+MR4DNe=q+mr2be0JQ(R+O|bEPT1#FT=Mta^;zD{meT4fwh z=?@K0Yqj*_shT=rvGI}!-lTDWYtiAL2X@#|U@_Glv>GlV-5!L#F}JktB&eHvlxp$w zwZlkHE4Zz5Dd*XAJ$5U;aysRcgP3C6sa+;+0LCx7WBN)W`|v}uQewv`lLHt zuLl}Om@%d(lj# zN;pMI)ENtrhD!?*G5@79u8NJDVGB85W>3?xW|i0LME@wjwJIjz2$*>)w7R=f)#8HU zv#OmXHBJhS^$WPiFcNx_SJq*o7lAO6>+QEiYb!PAfp(UZ9vHcjVrg68RzMsHmPEtI zM7II&Qw>(DQ4gzKlK4`8a5lIyM8Wah7|bsX@RXhc$?h=Lrjz9#=98$4*jsqDK`rcL z_EwTE&LQCjzc~#^xP2s%x_=hg`d55GJ4@dbVkbWmfmGYP><1b!-MwyZW5%HKebN^TxxAS z#2wFA(nkHu6OS}_-TO+uq99O#+{3oBC5J#`am@FX#xnwxvIvV}0FZ_y5oGdz5Yg8% zHCMj)Z?*J*V1vWzkX5l_dY%hKoalijYL;nxQJ6p}FjWMh4Msz6xFhv_Vne&_gHxJX z{OSEjcFfiUb$_bNan6@4rcsqBuV?3;m64a+i4!`Pq0-?*pmiuE#SP{fbZzz@x^F8Q zQbn$zs%xw@8O1AHN=EiQ5!hiSttNiF9MyI4_wb)y@PUYbrDb%XSG2D{ObbsqxH%+X z@Z#8u0i-cG_;Kda76&^B(zPBO#BXoa@ehUsP&#Q{FZs{EBcF2|${HB!q;f zG@K2^Wg1e0&WQ8XP0J=FUDa z;QzjCe;=7d+Lpo|`tAY&TVurk$JiO&Lu*IktLz+hsjCf-id#eHL!3dmX3677kXH_9 zIW^P*-3SqPPyv*dpPL%HRcxi0kQi09S$Ga5xG;^#IWpZ$&l^A&J(E8 zKj+09ksa7~^oUFh{%@7zOWaP$)Is~LgY;4>=qIc5*Gl6B^_fV`4K*!grOm*TXJWty zUAqbL;ddno5P3#ToJ`?zmn@`A2QjkBky+7_5FUsLKOkryuwa14BLgyrn5h=j|M!3e zIDyu_qIvE#d9w#L){uapm2~7oM9RRLjRuiO7GW@v$Jlm0!2wk?G>W>277SL}-w2?U zNsyD!GzxFRVdLV_s^|2=Q_a3yPFc!WTT22WH7N>)&>BB6ZY zq1I(zHmcp;&~W8T*=Q^Yff;{(>M*v%5z->|0_!9Bm6V|bU=ZspgHJ3`(eOx zcfjwKzD8=9h&0i@Mv5VwR3}@iBeE*r{)B+R!5cyS0%)ltt`;vL8({wOy2zoHZp2_= z!c@e_lS5bvk4jNbGZ0jj>A%K+gM@!WPV9F|2$QuV z$IG9&CC;~ar9Gxh6o*nx z!;jto06+zfV7;iBLv(u{5E3Dq9vWdv4rD(d4)~Th14;%8jG+~%J2zMKZ*)A{+@zsR zRfm8pC1$M~xh$ryO ze5tRcp=Rt#I7pg&8;TVem_k7NC2n&mHH;ncnJnqK*}Vcjk_>bdFu(eu4w}I2J>?Zn zIVEKX@z@kFSsD6jH#$HQCAjG~5?*UGWjJ1~tOkZL2ad1lr#6OJyr{xhHM~+x;6fRi zHMKP&T1yHyTuSv7YnBon8DTA|? z48V|FTr*iG^h}Aq?PrAeE7eOq}_3Ih`AE}mEbg5o{ zbRaZ3hJ>Afs{a~@@;6vK28BI`fS@{Q=6}^cL#%~QWqXLiT?-Z7(vU@+0V|?s z!kDPqh;Gzw{HnXNwXlY4r7<@GSylqL1O@V8Xani~yg)c_XgZT@oehB`9iBj5#Bp;d zsr6wba2q=D;rJE4gJk*8gE_Pu_SWyD6*Xa4fp!Q7DDAdU0T78Q5n??H|OWs%z&pOIUefDfSKjy{WarLHf(Vy@Z=nEgd zN3Jh_MukJ^*7e-w^0SeaZU1sGDunVl2;V-+D@(s_OV0^%ZiyfLr(LEZlt96Y6R$ZE zsj&*7BX2|0axnp$JK^Tx;^&uhuEs=uUBnwr^AH(I6A;fL5D6J_rT?0ow2GHMu1|u1 z$9#5%+W!br*;~f~DDFa%-eyH>R?>q7LTN(S%uy3w3MpanB9K%hC7|k^=bt48Sy27L zOwhqoE&{<8^;8;22*sn_J;hL?08T1>EOqy34S#9nfTKBmVhYC=Gpav+?*J zGT#${3ccfvQonxR*P9NTv(a~+^Vz6|-&W@B$h!B$#|u1R`swDZF3=9DAzWqi)gQg^ z*&XEPUyyS5b$fts?|1h;!{qz_DYrqoClvq=Is(=9DiWzxV zUlbXAXnf7rQIX#Jx5xK9ap1)9j#Hs=0{fJ9P`*Wr>A`X(4k??Kpirox)*369&vMtv zt}T@i;4im1*hWw4g?q-Aav#6KYc@5;mZ>EzrBE#A3^Woxd?OeyMFT=gfwICE;g*1u zvh%{hRGFf&lBr$aw19W8|3X{w?n7&3f>p>S>!v61&6AwZBzmY6jSv^CHHTjqFig7T zsLjQy#g#H;pM$kBYKTKGzp|3%hj$1xBT=Sfc;Jdfx7QsivTwUX`4MoIXD5EeUr##p z)qX4_nC-`t7b_yLWK5aicYi-6&a2Vyjk`Zx#$~Z!J`2A-9KQbykHZ+Xabl+5o|JC&Of?Tv^JYB({EIz^LQz@fzh9+92Jp;BIKW9TJ}lHUVcw{ zeSN(Okh@*gX*=_>>w2rz$!m*yASE*8U)ucSuWL6)JRE6)IJZ4^JRIk`=`q(E<%!&1 zFvm9&$&~y&%=K})|0P6JQ^rH@I|AtXbGUp{qdG^h{9V}|eY^cybffWy-8@9)gX;Ht z^XV_k8vWt%vuhWkil&p-`~$YvkEHBrJ*mGQye?m2I=5KH*At>C3fp;Y51@g-OyX8a zYM>DJMj{mc##x*y0iptj)+RPi(FSlNjI@>|kTqBg3!(QQn+l?uyUY1rfG2jwExR8Y zl~gVa(NYT5G7kX{=|!G0m_Us%iwA^k3(0*6j{ur#Y58ZWm1 zAw@DBU=NrD6K+l!3mAPFJCV5~!l?FrBlMIRJqi>#0`Ft4PQN2zMx z&PQ@UKhbhx&SSNa6;Z!eb>vIz8=%s@9*9Wu%ir4w$Tde$MuaQ{VNHwQq^F^Pw5o`z z7N8|lKu`y}0$NCq=+_~>${5@W8)2#qK0Sd!`aw`kK+t}Wa@@jVwy>wdhUIc+;y_9j zI_P5C%K?;a(Hs-oHx0bITBsRFpwB3)(y`)?g}qRhGD|5Y%?g{Hk$w{s(z+uM_n{N{ zQep?4#yuF~B2Xa|d!rCV?gBYmPdr6^0RiKF?p=Q*<7Yz^-(k zkoz1Y-Z$71F=;>esW%1IfBZf7u^mOXVZ>AxRz3ZuYjK9Mi^bGcNqI{zCe3ge-Kp`(b+^D(vEPG2VM* zDj)AW@srP^39j<&uKe zh*EnhzW@DqzYce{&Z=wT<+3z$$9Ge%>&WKc72R&2-feziz1^o~ep^Bi7hpdJa6iym zd=ohApGd03@+6mg;*LKb+*4jZWBpHh67A)d9$0jy<$t^b^4A+9KTuG36!dR+1#jN` z4*(3X5u8`{y(>y9sc<16k%wJ78}Zk}6ZN|o(V4Yu=ic==e|s_?w_m~imfxw| zp7F%!%=I1c-F5HHU*B_CW5_p$qu72M)ImBhV&}~(Tx$0Y)cgMkuKvoONMW(dO|EOj zZLb6YOhk43a)H-xUHA3%Mvf{0cn7=ozJZs54XJtkw0Kf3d1KC>e;IWAPjm=KRvh`b ze4mk)F3(Caz?w2u^PbVRp316u1XR;qL0^g5Q$L^jb8F5kNwTmUXBYjaKK)LU&x%b|SG51vN z1+N5KcVLl8b23k8YbqH3L4|axNLFu(Z|jCEZ3)tp{nx(GE7bP$bFdAQWzy~M-B7Ns z%u~j{IM3is`$C?h)*oyiA1yy_F7rK*Z1;rqs_}X1AJgAn{WnJ5*P|_xT=HJ61fP1a zoBP<)zRKz-_dfuIy)J{FbXN+VhP~;bTqi;k3l5Yw5l{$+us-ET(*UxZMxh%Gfk%%T z4Wv^*)=CqOq_m+)Pzi{G4sSUSpnW#uC6;e`#T`7Dra`lSo@lCYQ=9eP^Zl{R7&FI+ zptg6j>jQa3awyyiQ3QREqiHn)P2g{TJRb7QZJV(`QVg<+RE1oEtR)%Z2Pvr~wn7cD z1ESnTjQnR>*gLO?LuG5nemvnPh0t^Knz&B*#$bpDDVtG?=0-e4ja`LNedBl9^v0__ z=lD#^(s{#>f_?`VZZ!0`cxfJs4#HL!&p;~ojlmbyPHv&iVV?4fV+8zrhfCGR=2>W| zB&=`a!S2_sLK~0)4HROYc`J9MdW_R%gB;UTEN?LGXB#L#q8hTK#7!U0Um^2f{oSt} z?hQY-J7g(|T~Gwau2GkJ7^5{9^ERAu>1Zr0^)eJiJH%qzt9h}5-`=nPxSiVA1&6VrJzIO%-Fgg${?oCmCnzW+Qh2$%(cz8{ z(w0kt;QhUGm#qe}uOCpL(w<)b@y0=)P6?1%SL9+S3YmceO_E%<*zR-$4!V=iM+Ycjf(W$XGn0< zprH*|1>{{>$z-ML_q$$tfOgb~drs*3oNDXu-xAV2ZO~!w{(dz_4#HD|1U zOf2XxfOa%Raa4Fihn!Ue!f2b3^vv8NQ}-JPMJ1dHNOs8W;|GHLqMoI z#@6o*&9>7Uii+otN3yCHd8PB1=2M?NxIEgr^b02T-FN)$H>Q96knTEgv3+~~`G)n$ zYLBV!F-61e!GB*If7Q5doAOv6E{WBZ^yqWv!;51wZ4kF)e8ZIb&*=2fSErshMCjxguJe}GNW)zz z8j|res%niAU~WpPLS4I`WHd^qVj%m26pE0KNP!H(f-acBxPv;vUfNW8IV_9a)~VqW zqv6sBdtH7nmuQ!tLu}iD9%fyop%Z077=qp+?7}FA0s|~aH{ti!NB1F^4xRqeFW6a`|!KL-N^QT*tUTIW{OF{UY$msb21@?;(BA)P_EAWLvE4fr9aTI<1S z*vyh_M|20wh<+!0N7p~UAQMzvC|z&#*kT6G7;w9B=OkTa*z6n~ZB5E!=S4#DzdvPn zoE`DhbRAClxZP69%-iMeOzU^Vez zfUmWmkDre`?$zDZkE@TFlP;8`V<9!dAtWTUQ9(9=B*+m#1%f+ao$a?*uhL&sjH$B- zB++*<-dRk99b|wL8Wnk22ook!n4i9wPpkV_oM5r`Yk} z?J~>3J z6`;}UK>o19#j|#gw?e!!tXj7*g}E%4uF-vC#qZUz_kH@{r8;lme!4d|{ZcLG=_q&- zr@?u9msrR-M!x>dQ|~pWdbPtEdHeHc{?5o<@!PR3)gKHnXy2O6hQu!i+9pHpXy>{| zOw$p%gl#8G#np~f+s=op7JRPc)Nf0`2wACD@t0o_>_Aj({fpUTXiavgV|^OWw%KPr z*LQEcA!ifn;#9Kp&XTT@WrH1_5^(J8#o8($bS-9_x<#sw{3@KKcK>2jPF?b}$CE(y z%AFE%=L4<8hd7J3{tZ|KR=-8rquAFz_u`uo<03E2uVH+Tg6E{b#5^&c#HH|50I z4po`n0W9F6@L-^eR&X{vrn@<~hjrw|AT{gvT6SaH)Dj-U9Nr%5<{#k}XF?tnFSFvr z{5%LXKSQV9ppjL~ig5Qa9vx&+KaA0iXaQtUSf5Gej+x)sN4KZHH13JF!Gj_qoZZQw zhSFR7m(F(AzT?eO%K7GbF|!!^Siq-)&C0d04xvRKm2;cp-QA_rIiJ7#Zesn*_m9Uc z0y%oe$Hm@(FBq!j=lv;yRi{(OjGmeNSyruAx7C73^aQnl%+kE@Yd!h*yurm+Ix_Kl ze(!+$CDnetLioXK-*Y~AEA(h{ytMoY@k2jTTs%5GC?v-Jecxv%zh~%OlCm;^j8JQ3 zZbuEg_L~{vDg0IL7+hFC;P)-G)}W4hPp4N1X)ULRwj;%51-7};P0WyRcK`q<*nscpZV z&zr^YcotpvUr_Qe-U^mW`}|a;0*V`?e|KFH9c0( zEFv>{O6#9=E~^(5{O-crZnHn{=(?^0vpp|v^QN8c*BDcSzY;^?RrNhFae~kOCA6K| zOiSBw_kx=K(h@B?|bcdl+(%`H{gmvvpqbU@S}O*guME6NH4CXHi7e7 z)dj86&ZudM!WyY(9k@bZO|^0@?C{#TA)WZRa@xigOPq44mJ}tfhPJ4ewK5uvn;sEe z0YXfWu6((qkz!BKwrU|u?gd^$l8=%Wa)6#va@a2l(&8l)+~8x2y$WHAK3*ipF4B%b zR;4<_qRooC_mVo^ka;1um)A$1Y8cQ@!~6|w=O^cKWQtC@ptRyBC0dBk_B$6L`%SZm>!WV;SB=|@_UC_bU|To3Q>+#rSYE=~{_;riY-Fl!*=*e22+h2&h{7pb77 zqV4t%F4M|#TuwdYRo+YZ=W5rv%uZzUuUW4L-=53;!u&Hr-d?z_@jLT_y!ViwSG|dN zHWPO`h-QXXC?L*VEa3;)K8VT>?i%Xsk*7DFc|RWWup8z>lg(om+mCzg_m>~zfe)`w zq1k%B+Wx@5jrf1OTYezgf4+m*OwPG4?%irGe1BWjQ0O6ZWDLuj*51Bjhk6UpxdZ_p zW-q55h>r8i67u+eL|tQeWZlw@ZCexDb|&V;wr$(V#I`Z9ZCjHGCQdpwCf3*U-uvF~ zJpH5pbU&wRSFODYyY|@F4}o`g{}+xg2u~E3URPH)9_8=f%bo5kh^av0vb&!y*l(en zxjqp39`<74E3s2G49CNo>?&Xr`krIE?_XCEAJ{Nnpiib+DdE@P9=jhey07wL->%R6 z^Uk=iB8lJGKaUhIdEPNOA8xq%?(7BAeUR$jJMv9+>dfC}J#HlPdG5ti00862wLTKK z3#GS3K_G@lT(!F zB3g%yl$W$yK^2F-CF}+i?|3mt@tvm=;ik5FjEz!xq=-Ts&9G4x^ z)6XsT5U12r0`i_nLub8PH($bBF5WbC5>EtGBvm3-R8!+0G{Q@Pyq)(8Me3y5KEt?D zds&qM`v~G(=VLPMT3d zM@*|k-^Xm8K%_{hhtWsEJo7X&cMEt%v5B5|Gh60%PK}v@7N@s;>W=NO{XQH7uucK9a~?7vf?T?FMjx|y>?N(Weeo-zHKs&OW&UoEp$cjBD*2u+B=tIGrR0D)AFJmlCK%lowPvnwnP? zeQuQ`D)He!%v0B=HRhM-aimfeQ_b_n7Go~sD|oliNp(yJots%?2)YD^PokdiYtw+8 z?O0lNdfk@Ei5)mKkFJ^4sQ!T8@HESNk5EQfig&hzV#E)PIw?^!{6zh6NZJDz=P9jL z2+1mGWF=x$M#bM?gq>im4!&48j zmS=&o49<09en;X(M^{8bQ$zx5>ZkvR`y#u&V57ZmRSEOXlfTixrU9EL?ZGiWK7LR$ ze>q5hlq#$Q(~yFRi^NI!nUw!AP3&w~fe!7Ta>bx~d}+76FMoA%WT&6pp)Om;pz%(? z^%D5IU*-Gt0=nZBcJ0AyPS^AK<@S!ni_jdgJkd^1{_2E}8r=4>73;8#!}?U$3VkK~ zFZ}ikuYh4H!8feldH&h*OJCId&Zbuc{HKmxhZ~)hcc{7ED@5Pkx0Sc+d|&2@V^Bjj zq5D6?hMEn3{G1Crvb|mc4~gSBo*xx;XH+yCC{+56SRjcL4l201W6Ll6;6L?Vsq~+Z zhyhgZ?5S*jJ_YdoKdjxk9jOI}KR*j~VD#U;^qc+p2>e4Ud;$Wifd9hlSIyygAgcVw zL2xm@#}&(0_ZY!X>&JOd@+VJy&{;U6Nk+iF3-K%eSzb?Uk;c*WIphb*0Pap1wopjG z%TWA{&*#{+$sCc{+~+X13s)0qU8aCGsa3b1z3Qcm{#&<^|H0zb4svk%&0EzOik89IJR zk4nT`^*#|M)nV;Iy@=-Ap(g=N5+yh>WRWw8BxB*F3KBUB9~`6p^o-N0=A@Yu(-{`&cATYGOF4%xwTMNjAr^!w@(L~- z!?d`3Mf09G1632vx9pFx-$_gsESxp+wG6PJf4bZ2jMYc7|lMOO&-0}P-RG{ zL%EZIwyAvur{8@iA^35;d0s_fm6X0Vin#uP-RC@okLz>}Sv%{jr6T3h{R_f@txf;^ z_3ihf4tm%lW2P+nW6LBL45rpHm8%NY3QNUuaLB`Em2MxzW< z&sUbEPZi(2mh>BJgAWtI_N%^*K4rxR3wOtvaNl{4ug{=}_RlArf^l;X$EC6v6SKwpo~LNGx;Y`b=)t^|J0FLOqdK7%udT;8MU>+4+TL(% zz2|dyY@|wlAOAgu?&BWf{idrI20^F-u-l>Z21=i-vfKf)jfcCsl9%3qLni;nrim#7 zCfUzroALa!!U}di{Tp|GhELFs2LqvDJ|TQCD=5jlu~v)i@%pM0pS3fu%`J}`E@Ce# zgR#Q)Gs#uYy8~Onh}X&GvEPpe@wXkwGiczveTn70e;77;?;2cPFhnC$CBa0U8lET1 zy{hkNeCfJ*J2{PQEHK4?>M*m0%LvGop>}cNGVk$e+R%KZ%D7YCZ7mBUpGrtB9gm&X z*3AtKU!&P;ri^MfFfZLh<525g3uEeRML7CqEdVHvz@1HH?yoJ}Z;p~dgVWcS?_E2naYIv!Mbl6T^XxQeGtFVRy z(N!l2CPF$~@#E+DH10NMz(HT{xJ%z>#E+F()h=hNir5b_4d3HUf`f{;1N_hAb%n=K z9T?J36jAzY?UupMje9eD@0UmVbrya=+FjR0iE}v#Xkq(KR_jEHP-=>Qb9x_GRlMag zk=q0mV_uaLq5P!Fc&UI=0=%y89O(+TgxHZKx3nx;jp|Q}1acBr!p%&^92OV$jXVYB zc5W1iB%=KZIgDy~Z+^u%!h_=>$@ZTiG0@uraN>yTX_iJCy`vF<#ZGCjr`egY4yC5G zq%tA|B_Q$`!UXN)X32PMu-|Y{X{GR3mloCr7P87%^!!F2!BIa>)tLtU`-Pf!aC|>e zox6^B7sJ*Icbu(-B2r{ZDZmFy!9;{95#Vpg9gos`J@wNK5`lrtKVtJQNI@31Yx*u> z>Jom*>`ROQoFOEa!evGn!{Nq@j#3e}?=KRKRL3vUqg(h$9V`QA!a~Fs@dSSswUC*} z952sy8Vo}!-_S3Ms$nx<1lFrL9aE=bJhb4LRHTBp{sJGhNP_FJM!z0-DHwXD8DZZ3 z;>4ijL>nVHS7g*y99RJA)B{^0)k%v-U>2p!A}<7nZ^XB0M&Ik;)G`uPXunp4?Hz0_g_~grjAfLxA zDuit*%nV9SnVQ($QkwgTBN?FP3aW+%=tpHIQb z7N}>E3X~0<5W3C%*0tvXyLT#N{~(K>RRve7I5$=~5#kN=$!mq#rCoT8IS-O%+z09KC&CtpKcaspd4PaJHMBh=C6N zcbCilUV^BiIoVWh+!^}!5$CUALb{hj|B+ge>|!!6F>w4qD;>nD09Zrm2jjL1ac!3s=X7wD=+Kd zE*D!8j9E!p~^2?%VAb>`LI*?;~u%G)*%hRCcs|RR4udi{Ed-HEqGb9 zwMF#h?7+rr_*^702Z~ghcmT{$=@(z0eoSEn=>a*obWAB*AbE_*E$CNa0!d4YUFBWw z2d5l~mAq%c*C{=Yzdo-32fF?az0LqnPL3g(0}l%w_RoT8LX|2<_4r&R2P)}uBU-l1 z>ij^$NeZlQnkwxcRnBMLABwF1R;=sJEvR?UAH~WSp$GK>jYC@|UCt4r{mo_+IJ2;! z0Vj!>8FPmD!r^OiWRAY8jetWgk#}}P#sgPEw1tSs=rr1W368t+bOrA!c}h0CA%`@D z5kv4R-imWTd$~(BB%W%HF^V76(#8*+p z>vZw6yBMHY`mYky$1Tt<;!bV$-nUR zeUI>97?+VCyP(sgq=0M)|9>*X{FPxcxO4=Hm(F2G9q0J@iQJ%T8}?-clp0AXE7!D@ zT~-i1>6QSmuuzxQ=`FOZu5G+Ci2m$l3d2RP!&RsPBeVn5+gP>-cOeFgaY)`9ns#tG z)C6QPc`yb454JMGQ8WRMd$2b|Jo_%|j$uuo+pJU84T%xK4v$jFU?}0}F^Y*z!B1)C z>joWWI)ENTVo4}vi&7>v=;Us>{4NQ73 zv6()`HCnDWvUY`L<#ZYp;6xlCO*VU71I~K=&w~eE>z9xu@nGX4=PYag(IB} z4+SRQFb^*40^B-~MIy&W{!G*NjN}~%B9Jn63MlqCbs+C1kpg#Xl($t2%&J2V&vL;l znz}FR@W1tGT~vOV%j~lX(##EbgP=x?P7&l*qj(|>jj#{(SiQJ_3gG60wteMTSHu_@ z9#uv*Df_+JVXr2|pn1?u3q9kD9HG!}Sv>pYCQhAsmDiUF7AD(Ccz{JSJBA`E7q7CV z+p{;-P+0(H6mJDt>XmV(Ub<#s*qwvGHHY%#k1~4HI&k!2;D$*bK*8ul`1%9h{k?p# z=a*(BOc+XeW6_X^4MQ?IU|LQQU3@o~%9V7ubm*QxdPM&P8fxKXIyTqAf3%4xq8QEC zi;&+lK|Y(4;n4`j>6_p4EED}BsjhP;xM;;6)G3DXM;10r7284AVu>BT68?yO7Z}~x zhO{rqY7*r(Pqk%KXN&pBn(GE;<4h(h;QJ~H!y|oFkU~2w= z0f&BQFq?AsqK5}!NcjRG3fL{BK7Ez(HVJ0I)iEehO&ErY$PqjIa07)qdjreYQD!~+ z>rHf*KL=nxKe(&B7l8z&qgj%pFSoiPS)D?{4b_BURFf=MCJhIhRHH75ab^MG4ksDH zKkumMf}3ZoluKa5brrh(DQ{#E$>_Y05U8bwzy_k3aU}z7N39;}>B!KgAm89nJJ8@QMG8oE`=Topd&LW(-^e?>0B47x;)2+0?`&Kqv19G+ z0{Xjo3bPc-k27=j%o;Kd41P42C9P0!&HutpTJ2IHZDnicD~pgoo4cILG|08LJS0hDI;=T^@n5P;M}dA5%< z8ON5)Jf#r~7$SV>O2=0tAErd%A=riKL|NO_L%?IlnZH#6;%Cta=$xi!SZl=yfMQw` zNCK?GiZR1flZ|Ohekt`k(fet7XW~)+gwZXK>kUY-b%>2p4qu&S*7yz!05Ksm#_&f0 zjzG{}f&C)O7Gq^C-lAPNpM3FBW8T%9&wD09o^DLiuIYhUNY8;KTD|I6|JyQQ!W8>l zhZf=d1MY4qOf>TQP)13Zs|U)epSFkMz)1XWIZD~4-vH%Xp;)V?1#~AjL*-HIz8SQbvx23k4!yZAFdQ0+-ov5IX)~jP5rqj~GUo$YSsf zW6@(_Xu?zSsy(IC(>%P>?l5`9QHheYr#!gGygnu$;gII#sM~T|ru{&$Jwt*_*H+j- zVhTmU-cl|!e6;#5_Bw(Y-oP&iME%GyIWW-%W#8gDWF1BqyN+qBF*8|+1Ml&bO0}0e zP)-2q4;+4CE>>>s2Huzsqw1J%6Bv6u0#eJOxY^_(SY#&OC>8W=XouL?BA(RU5(4fc zD#DUc@CdR;Sk$t3BS^uZlCP1a!lU-B%z~)Rl+*##;=nWIQO7gTC4N5`?fGDy{2{ju za1aE)fe(zV&Cw`}Qx7MvL?c4m(FxZtH>wgEdU#sagk6H{Iq^!6=A~eT1Y){}e3M1w@E@938!iFW^Rf%}9iT2cwR zHF!CMdbp7m+F5Xx3cyLaovQwh z*iZ}_Ya(K!#0v|fV{!rVT}%o^gL$o-b#=-En*RkKxzT<{W*K>zn;$e{3^WeBhnrKj z1Y|TwBt=~ly)Y64IwDf@IajeJdwIl7nBz;9{30CqSwFjoVaH|X6mQ>pG7XAu^dB;e zy$K7mh>%Uppk*-3ORq8fr;EDjcpVStA={ok%Dm4#;se|>y0(Vv$j=1z&hQU+YOjwFbSXJ`U;Krm# zuBMM%We&I*D%h2sBY|SsV4gu+gAfoY0#?yAy@QMjf|LQCPT}+3U#Ej)D&fDez@@Kh zsy||Jb8+z-IYWAE40APuBj$?!@3Gah&y;}I_~N&D8^$9U5VAP_ z3@AOry8{hJGxLwrEUf-wk2^*Q_fing$vogi(S3;qA1jKtxf(*L78qukjVKm2+aK?5 zO9tHtw(G5S{vA#$KLcE%IYIyKcQQ_U!#G1jVpD!N7R?@4??3V7 zs&SAHoY~LRh?Ed(rL-pdz3F?N1NF4>LyBE~WrqWBp%v+LHJsYHrfg@&9tj9cDo)t* zCQPjT9PLJ1R{0p_zrd0 z88wmD9|-@^omaMf=L56u6-Gp0?x69PW~A)g@9tWyhI^+?S>%f%yBq&*grM*rSnl2$ z^tHVFi*B0iBBS8zB}D5KOkc8X8*{ra*8B1wyN?9e@enB~DJ|LY-hTUR4SjiiVBj{$ zrl-RK20}@j@3CDSfH=OiSLRvm$CPiRX<{4I-?F(QRq_iWaJ}CmO|0@OM5BkYU!N~e z!3^*w8$L7FpQ?iBiRYet*&Uzd6tjl#PL1S>anVP@=QYjMgg97-N733M6k9d3yA^GY z35LXwxGu8H-}^D50R?5>QUcLrXks!cE_e_yd}(7TwP{k^=W7C)0bEkvNXzLbd0l^+ z#ReYcv4G@qH;(Ue=h9)rZ|$ynV{%}&*?5t|vjmfNHIf?6DvOqFj0hNJ`v`5v-EBDE z@$%bfi-Z=U-)J=%_FMaG^hAjjNiF2yAMLb#<|6w$BeWcN#b9*W%fdKm>$KV)5?(!h zG2Gojl_HY76APo4;rYi8fP+h!_W{J85ZmvU9_g=2L~NOiw2aN(EgL7Kn|aLDEg`aU z)w-uGaK9`jD)DTb6HbmCg+LqvUcyhAQBMJvAG+^ zyeFgjbp*;8lHf|j*nnu5RD*KyfR!e1EdgspDg#`Yv|G#|&({$6P!aXb5viJ4o$1_p zqzCsulR4yO!k$`XN}9NVc|Q?(MOxEF{}^5SkzNY@#7?1v1X@v2M#B8p#{i4Jrad0t zjWTzq!0R_6h_0O+8oY_WY)tQXEY^KuMw}(&GRJA}_kQNJbm>~IK{^c`Jc`qK2V+97 z1gLTd;4rv2N7U>&5`Vi@3u@Eav5wNOjX}vFEg@r9W0w%5^QxAuuSOjkq z<3Z}Z?kF#N9(&CM@?3!N47bj=;&A*K)Maxgx5kv|P8xnHX;2PSnttneb~Q4v`O9GB zpcPYm&)*pw6$#4TbWSF5A6rC+gRDlT-0bR<`8U@DkkrlU+}fxkY4{#UDhAls*P*-f zs|1piw4sTl=&4U-&0 ze1`-!Ju>Tjv&_YawJcSVp&Hw>M2aOUc31`Zsc@?6%EXa&L`bUMJCn`jv_Fn|*$&v; za9_8hVqnv6*ez(dm=sxuyF2&2=KAB;eIU=pwo^Wi^?Gt8y<4EYYs8nsES<30xv}Rs zzvuKE|4GKQo;IL(bV7Jeb=BfsB<)E$tYzt!#V3^IDH3MiW$f7N%xqLbs{sTSIAzB4 zzwRbgzG%DJj#{c=yOPV6#C5u7n=y%_15NMk`QzRncnP_~v3h1$F$SocF2JZ}fSVCl zZ79ly8?QP{Yf~}-*DyIt?EVlpyT)jWYP^KMi2LMf1qo8GhHrOoRW5a@N4&L@Gzi#K zRXaX_?otAt4p31mT19z68s#O|gI=#AOb$eWl+QhT6O^H0Pg~WjUBzfJbIWuHc;rs? zhWv$M$gDuLha<@Y97D+Am_u3m8~o3My;siVGZp0t5THv%tY&uRQ6h-^Qur?0L$=>H zx3wH!CJ~|6Q$t1#~Sw(({AB!awEJsLtW2l*q2$G86%tx~tS=4P( zYaULUg3aolab<@uC9cU*ufSG-1?kc}<>(4$@RbGKHj{+ObMh)S8p@%-STUZ- z`$+3AgB~195}{|u3gbW2SjWjT-E$zqV&~_x;Wd}C%a&s@Yi4hn6SatV1P6-8ELg1R zpwgQwSjwZRYvwCO1la~+)9Vh5a8-S`I;&tLB^dv9bOBS`2ZHpZU$Jcv0463Si&pxN zoh!MnFMo+TjC4G4M$!`hPbqHh%u$?CrNQ+18^SV_+7gRJBY3mM$5gnz)GT ztQeeFC~wG(Jt)@lm_3Mw`3MQH#%h%-c_zrOQ?k3?fEC@k@v?=sEE=svT#CUn8LE$5 z%sVINmu0Np0-PPDJ`ph@J)-X+1p{bu)1?}Faidw*p3-F)*38JMLYFAJ3v#gt%LrT5 zgz8}}+#XY6k)QO)q+X&~5-p;BM$x7jIoyS6$lTe1#ZDPHSu&@u^2-o!R3oHC;}uv_^byF&VY`fT*?riJxA~sPPt4@4_g}4 zN`>;)+esR%S8j4-DT^dey!7=trHJYbzzb|VQLkYun#XIFq5V{~(Th@sOZsHfvvO6K zgFpfgBl6BP&(a4DnfOkn1V?)bqG|cHNA;96)qB{sRN7X%PFyE7>cYeK47w5l`_bk6 ziTib|u3ycHjO0K_Zp{NXyF$5QT87&b)J+3*QrU!5J?_Z&>ap=?O`x3L?;t#+Rn6>| zKuRe6%I9fBJCZhL$i>|n`Hy1-sD*0(0pjaIY~e>V6A%Z0>@xtlH)Y?N{0GV&%Sq^! zG{5K04yfRD5OSVt>&n|x)$eXEwgiY^8LK2~L$7IApu##Hbh{$l&Q_QnjdW2qr`{ee zdjdL;CFzSNe^Cd)KZ7&|iE%nc-ew1uWX*vIN(Vyk=*ZGCg5q5?Z?j@ej7=yngzNM# zm+=T<*ZWsoYKNF;_aNCt1qEej1Cv`+qRm^PoYa60xVgJ_dkZ0G^D5G?P=3@z=F#xbv0Cb7y#Eal=rs z^EnDIvbpT`m)iQ0`rfYnoTRxI;g4mbAVQ1u z8BC^%OLVDCRZ0Y=l$0nClG=c&_t9y}BR3x@Nw!cC;+|O6rV4ooVFv&S$Ye;?qU;Q8 zM)dddQ{+Q%aSb+2sYFGe_BiW%JXnS$L(sMSJ6>Fb)j^qQQ< z8d}oOsirV=WYLZkwLO<>?FNt=Cn(p&*XbuTOc=5`9S#qe0%(#|RJz{|m`ng=vE)>3 zpDRdj$y=t~&|0 zdOv$Z&Me9L;;U&2Tg*9o!hZk3k~v{uf7d6F{DzEp(ob@!vk!Kyqpf(Q1A=P6Ahf{LIWu1;FpGXUP<$N$nyOsZyJt7&-2m!681#J>sh|mbU)kPb3WD+Xa%-&+uz}Hc zWx#`jMll(dU>Vs|#}8VoK1H(yDyg{&if<(JyFOyIQwh>pr;Q)BIcH za2-bjP~}4-Q%*%GPKzkQMO%r_+9{2DT9!s``jeb+!RT?|>bY9bT))`}Ljp6GXb&vJ zvt|IR@b6YgIZ|aLXX@PThB(q1c)Zpd7KF7?IszzGVWC+q@1Q|Pj_wU5+{ByUvJw)ESH6xwM9~MX-A2PZ>&Gjw&aKhs zOVuUezPQdcUx8z*LGhd7^BGN)Umr^dtLt@y{1wzDGcRjD==F*s&ZO0@@7zgH=C4G} z(!x*ssA&?g#ryXeSHdiaG#U%}P#SauR9>59UHW|tx)(k#m|86oq1fEq*8Xl9$B6ZQ zvAY9b*x0{>6dy$-9@h>lGQeRT`JfCcw^asa>6Fy@C%^_Q2ESjN>6|(emPV$KxjM6l zuvgT_Q54m_n`$+fTPI??BkEBI|)#bVqGq8lH85T&*pi$Q*|eh@yJnNxcCVQ>U|NfMhD{ zeAo5w{tA+8a*)|T&{56JQy_jB2`XELvtx)WW*#lVWY3|e&HM6;#+Yi9P?Quor$CWTbac{LlJBUEB$s^M}A|lEc=3( zhF3qUQVo0Pm`iqt_=@q{GPshjeusDVkC#0|%;K zA_swhf!gYOiwpku-n@s04YCu<@Msm&3PYD0H`hmwU%bzShLoAAcPdeYHIBKa+_3i1 zOSm%)lUf8Z?FDN=hJ{Loo1OP)C~yr3>gU2}f0zER#{v(ux&eLU8#PAStK%>vD<4m^ z3NA%Va^QKXhHzIu(*+;*vzlei_&N=Nf*P)>|Oj zG)M;yhf2GY!0+$R7$E``&%0l|jf6d&6WUz)+quEyB=7V<7jMqr+}vx+*#;Y%Vzg|+ z9B4huL`U$y(uJ*K!ga=#CEVNy%zP|#xOC2qJo7p4J069=;cAi%0~7~E{`0GomPaOwxU@X-C5nqAhBuD$W39%&pi5hqbpTAQ93!I`JFwaew3YXT7K zIy1zUh=kwZgMkxLZ1vQHLE{uU@!Zoe7BNu-N8Q;}2FesrxP@{`s%RV37*~;C)_0B0 z51lv=!XuJQJdzH^WP;A9z275a>dZNkFx?d&- zUc(mf3dkrCm!gWzhGgQ&t@MA};ZC?^7w3>)k_{J7DDhBsf#%iey1le*Gsh`b^I#fy z260?;jLVZ_h3*jGd6MfXf6anc^eYPM9ToA`Ig%dJYYOAT47DC$0pX_gnySM5Y7aF> z-gBO-zEhEE>{w4sxWwy+Fmh|d#PFBs5fT4e&%7bmpiLzw@dX=)W?qr%<-%HXk>va% zycMw-W+fVski|9r)!V{`dHjUSFAI2_omn*?vhYPL*TnL(o6|QmvDdrY8U+g~@*+Rk zA&F^GFFA6q5`e?C0tcB)F$OL`Q_oCTN^C<~+&=yvf}Vav zNyr!6xQZ=K5t1>e>?4J7w>nABTsF-c}(d&ww zSs(>_4{T|j2!Zz@9SSW4i?9&bl)=^&s#h3g!ScCRz_@5LM6xrT0UQYZ6IO}Y6u-2* zv*X|7wL1?SC&42oajjYC>`mR5jp|WZjXuoV52b zpk_NeorKt(9PYciXnTmTUc2*+b-~mEzgp;5$4A32vy5rxGpNQI|G@8*m>%z*t&l!` zu_3ui^@}c!-Mx4-AUj_M>Pdk#vU-3|d zg&YCCI^y1or$+V%vkd8I997K3Ehq`V$9qM6z22u8g_noSrshNf0}h&INvpin!hoY~ zZp^(3nw?K6OQs->V>!^3P@E*qzO|HKNp?Z3jEz(+dz|iT)>OaH4-FFyaywi0@W&F( zA^xA5;H88;txlpODw0C2dXvwyD=C*khWvOc!w5|yB0%dcc-)F#ZQY}!7PKSrP(v0% za{VT5_oN#a$W1B;5tfLd3@MY10-DN~#+Z4o-uCvI9R94Va7!)GB*j_$=n&sqW#pbO z9UO7j{DG<3ZKNE_63)uv&nwHuDVK&vjSCoG=@dfreWE5`VEVz z0hh1uw>s3y7ON2&NGp@i#`--O<*O0P(G$wKG6E%Qk<=^2E}fqv zxBt`)z5(_457Yg-()1O4BoHOM7GX3PaLjiMj-+Q)ucFCcjV`2n6zn7Q{17Yci5sLV z&lS}8R=4u)?oq0_-~X%-m7P8$elL$i+~12*XxNH6bdN*63|w{GSVw9wQh0Q$t;6_J zSZ&7Jm0W0^>da7rid*2rJn_4NM|@emZ=|FTwr|PfntZ%$DfZu90GYxR@fJ$d?A1MP zxa~y@0pc3WZ?@DcqLhbtDQBoH=5lP&G!G07{Mx!`aRfZ{Kpvb!^*>`MSEWsDKdY@{ zW_9lP^P~;B*0T&3MieLb$BmVTn>kk%A)H0G1@BS_^ch6e9&_Y_^k)zFv~`$WSV*QN znpAj(>ZU)`DYA;wi&R9iDS(Djd3|Dq>RMNfKu6g;2{7OeVmri2WLv)X;Bxf=TYGME z|9D1Fs8@uigo-}}(>TS&NP`XMdl_yEnXu?)N|aa+wC{K|1u~!ZlJ9b{!hGr zGSX-$jLDb6^}wq!A&DQGI$s##xcV2Q+n3YBhngmwb!?2rM%8o;_!UN}%K*y#Y~le7nanw{lnufoCqvpvi^b9s?{?CTIPyj{2O^&p6F>Ht53@MZv~77;y0~{_ zB~>g-e{@dR%`J%WW=TBz{Y_FcYfY}lAXxmYC8*{V2ezD@c!qcCXmT&)w^@+g$Uovs9 z!LhjtumMgrTng-dQ%HWUiqvcy;i3LY`aeju#%>OHZ3*V155_TOVh1*IWvwRFW_|}Y znl?NMy>YJWkbw=Hu+S#om7|^DxYdmYaaM3%&R%ae1Svym3=4JPqZXYk=EuV)xiaMJ zDDdS~Tzo3p{Y;B!f)B|@_Z_p@o}lpbiDg7M4=DPo)N2Cg(eNVyC0s>X%^zO0tHXw# z37wavCz#);U;LnL=o`Xva)JCbrh(mL%BU%ATzfJW<$ED&c*ZK)Seki?DwFyt>WL`v zUQ}^cv{D)#yp#KXs?R_t%i(~9Vaj8J47OuKFFu!vSFRMBgdCGj!vM#Dw+VlpK(0Bc z++e}m!uT``}FT z4Gd{6%Ht{p=1aS>waAGIX=?>0?SXGSnR0Hy(rxjsc1f`_b!@n{r1**e@w2EH(#5xc z^UagwFOqqOk9A#RL3D{4m{p!`$gdg2<66K&Ua_O5U96e<$2A}NJ?i)ri>uvM%d_VR zGxkaH%cp(0uV4SCQ z>;7G4jX6go^t)%eNaP=qut51h94p^_=9nLTB*Wv451}ZUwft&?mTUbD zNGCj8g7rW6lb@p(auv6x<}|kIF7GQ|KV|AP$e79oY|4_QP7LdGPQZ1+ueH}gNL-TIvzgYzJT`v*c2={baD*U6&=62S2^%d3l?t^ ze0wqxZJnx)qHmN+hNQG@8uvI^w=;K#CMKhriSEt=WQkRS9>qUbJkeFN{23%gZko$z z8XCTEY{sj{kIP)ObL3NXkKiBXd}_i$zah%u`1(_l8+fQEnpr@4yOlM<{m!{fucK=^ zLaIfEbhi@eY+bdb+#YOclFfiKhtc|WSHh-Imv8edxe`X)KW|V3_3z_l2M4pgkqr@? zd*CYrJpZr@LXbhsVwzf?CkJ2(Kb^!f#Q+xPG33g zBhJ$o5QWvUN&!q0tPv&}^$7Y#MHMyur8NA;y-?CbyufMg!MB9uEhf-oAH}{GaJAkU zu~yAaLW%oO&Z8kUD*7fn^JgrtF7;tNaJ_b`pAo7I8NHOFzxFoNknP>^UQu8#DjtGR z$nI!oSP)qBF4t)Xe@PHzwBbivs14oNj$DVt8{CgkYOI=9HRv~e&HHz?wj#j>HC7;l zb5~#?xb6Wd-(Wk!SR=gr+&9P8=eK*Nw$$SveXE;G*xkf$3P82o0=h$tHMB%PG4QC_DtL}_L!`@*IIhOIT+(VmmRx^GIAALdR z3x~P09(jD;W{E_nSK>moQHC@m1Y5?b<~WNMS(#;#L-xgjTfVqW!dZOI6QX?S|G{(k zf5D+b_GMKrYrE01uQ!IQ(4nm2s^rP zkX%>WAVK3br#eWJbAe4qxW<*nwnFueeCKnYnIY#YSZZQw(7uspi$&JUsUiL^kq5L# zKN?kQ$1F?4$+;HHF?V}>^4mn-vp~lhlcfvO|B%#DoYdeTmIvY z;#mD+L=75|+@f1sc!l+KaSy1Me<#u!h`%Pn^UD~lIR`1&-KzSuxTUD(HlxJ&TgoKD z;;7*)SXb>P*Kj)$PI^_i`0jW$g$EXb>9S$W0FT)6J$jqnz*K5^Iv*H~ur4k*qNFW=L19> z-BZ3$SilRasP^)d!RIaGy8CRvM%H!7Ti5;1xS_>p2mh>kYyM?PTE0+D`lfq~NP3+4 z?=FW36YtTfGcYWWAuOZKyWFF0BZe^*YFwk^X*wc2MRURWx=NU0-S~-SP!|lZ)VU! znVWyN{=Jhv2BPW~e=ym$9xYHll?4RA2ub*GA2=wA-_>NQRSg@aaM0p^!EZmeT^e-P zli!?w%bZ_9&GQHo#{EzdnH!OLT&4fv^F1@A8dN^i);IlqUFSUGm7s0Oe{7Kv2Z3Gi z=P+n5(o=$4839OMqOd93%B5QoDbv+`uSR{MlIu1eR%+a}h{BgQV4#%JG z3@_gyF?g1R<0*PJ8XH60bgnBS5vKldvb!LT;;6@tX~~)+dgWEZ(Mg!jibl%)-uq=2 z^qPWGSRv4Y{KPDU8A^&KI@JCVCANLU{zpr-BGpr!LoOXL`FFG=75wduUi8+EuD^t{ zo7?B7yU3?oz&n~(+P_|AQR^bS3K^uAx&w4Ghr7v#jTC|(;Pm)Uh7jeLwvRir<$Xod z3;|tT`_d0OzA@8j909)Ur4K=ru0e(c4?#LjuWy!uyRK@?ddpTmSB+&>ZSS4k(19~P z?1E@f7zZ$h0%P^(h|B_>%{c&dy2*`!lEJ6&ZvmtA_@^j{J_0TC}IJKcwPE5HR+e{D~ol%|S<;!GMpXkj^it zrEE`!lahX>U#BWg&6W7CSmm`U!cVp&Fx`)kP*n#6Lb;87;K&vUd{0aEl|*mXiv|sC zY=^}?q-N?a){j5b5rIOPx!!Jb&jh^x3XU69U9`ozDN@53>Wu3$%p~lw1PSQA_D4&LmbbPrIov7^nXdAlx#o`QX5EZuVq2kz=-G z&&b%=bU>+RX4)1Aq0F5*IM9xSEEZYq3&&37CIPT1o64H#Hdp5$U_yLeCWH*RmEqKh ztsAn*&~=!LYw=Y1pGkI2I@B_Wk+fU&9c8xCInTG>XLN`i^dEx!;Sk1j zRQr+3B{BSER=0~JN1*w4fBY^5UK?pRFI!QJ3<_RR>d8i6I~R_lBD^~LRk!-hJKX33 zy!t`a{UAH@l}WZ8b#PrR9^A!T#m? zq(hb%@FJwNRDs!PKiC;vUn$_P=Aw*?^?{XQaw}R`(9LH7x{F;&mICjjUuY<~TM+s8 z$*G!|%zl0B>pULP>YKoeZ_AWZU^F9Hg}qNg`O??BS&V~WI-9`reNK{lXHiFNa>HrQ zx%FYXRC!; zbEG|%z5>>~K5Lc#UHQu*@Ih?oU~FR54+=_E+XEsK7gn2b+pLkJ7%dG%u<5FBfL7|7 zy$Si(`gCr;Ai=4Y{WIVx(nhcRPmG?(+XEaQzG`X3+(`!@LXNLG8ws7X>h?Cu_ykcE8b z(hWW?H;7gKtI2NcD?XC$O5oK?^IaZ#AB?w9Q#~-o3pho z0e?s(7n8a(QO;4e8BZ;33#`CG$H^Wd{loQ$7KTxQxVN0Pm|EJsbM)jAt!y{w>}A)A zR~4X(}J1JyH!{+h&Q=kDu z`mm|gs)_>xY6asDqUgTG4$8_dRmv5&3=!2_vg8OC|G41hru6nsw# z$bBOmd6Ohc?W06iIYqf9{mG6>4Az2NeJZ>Oap>Rg`VHu)t@fL}t5lTEv3BaHl}k_N zUE>sA>cswQT)IDIxE3C-K8Xz9%`3Y;8hZ zlnXI)fj7RSXc3?oSK|CBw4*>ur2Kk{qWLTwnP^+@v!E!uFL4Yuz(}Gj{C`w^1yoht z^EQHnbbINP?(Pne6u5MEr*ukph|-OMbYHq#TAE9zNS8Ez$JhA%KbC8$Yw0=r%$|AX znVIM8&aJ02bHPkOF=&qq7zQw)!I)pwbtRDG%l)=Wop zN9Z!m3Ej0VOi6EPN9-k$u*fJ&-Ji)F!j;n0lnBRSs52^aXRP^ne7g#N{!l075W|A0 zj<-JQG$YX!_XiZ4Ff$zUVIFUCeny^p`uHt?z0}+)^oB~1%hEePHjIx|k=JRuxx!~a zD638rF<}*Ptb9M7?wNvpv=sSux@V8ZLPbatGi+*Sw7?;A<6ty+!GCV*#e;dULhj-C zktXR`($KT3BfqYPyN(VQ$Eq<&Q)G>LM9LO-iH?fYX5o@WPDAc#5ljg^O^n+F2E>&5 zA{(LT508T!{TV2Evn{X;$Tm+s9}vrH(;79;J^peWe$CBO&{}Z`vZt3hz;f0f zA66O3?M6$(z~j|=USF2jU43A<4u!;zsVHP1$*ZS>{!*K2eDig~FD3MyJ)1Xz4XtK# z2hFRrr9F>Vb~OdmnYre>3IFlhvoT>Jr0)HC9eQ%1O$0$E4OzYp z^4`PC?T1^KmULAfP8y5&$c2K>=Ezt_a+_ z;-W|Wg8M0m+M@koDbH$~#i~&9aWM^AGcb|e|1QebXVN6`+zbn|+DNUiZ#8V;b!t!-=G0JwxUtn7;ci=3C}~SXYx+^WA~@uEDU{A{CMBj7 z{jbShHH$e3t@s~1rt`o_to64UyFZYW+(sbEqG-6S73>gM9C1dCtoJa428nD4Yjr$z zp`@paQFNUQXQiw({?Db32X>Ad$o#J#b(jdeNdCIkCj(+6e>aMOulAv>9>?cIS78e~ z!r!&m%-T(Aw7OuTMrwqau1~Gvt`b31A_%BPzS2nOww+r4UD#2q7HNF-yu9c6`Q2eY z{hvCnNUu=rMEYp;=XV)|kjW1K3yqJ%zc`ar_+bmV2sW=`AAx`I0lc%PD zz4X99Y6P6x-*i!L9IBX3d(H6Iik~o^=D6X&b+36(kG@YTfNQ_$3$UOitrljY`>ds^ zGCfc>0ve3OGKtW#MH>B{kD)KM@>Lsv0L#jD6l8cqh2J1Bh~_o}+hw_27sxhGoR0VN z)~qw&voM_NVEL&bJ1MS$5-g(!(PoM@R^$^~w8RmDkvE=ZLXyyS39z#I#6_ ze?2A2vaSA=xBsEx-O7ZbA;+1(pNG_QonyxIomE6e;q z;+nHos}rRkWoJy4Ifd(-zU?inMB;v~Pq`Z2`gxt%*5x0=H-%OBkYAT&0FBuiVV8hj zGP10*O~Z$%uY?Y zmZA2WmAEJ_)rp;G^HP$(d9kqZzQ8(%&u`GEbbyIW%`QO7Q`I=OD?Oq*tY5Z_J*149 zlg$BNp-4@J3O_!m`sL%_DuW?{`OczduaR> z;{7m2OVt^l?a8t#wkeyG(~Adxr4vrE_1r7%5L{3+CVm7sHr&i#bN&n2x35&=q7-i_*pRj4Y5p?26vpPQ! zEd1Rf+3c2N) zS#TY2ZG0i>d^gK{8=&{~`sK+JQ`EFjnn3O~rUdQVNi|J1yW)jlO5QFr4()N%HU@AA zU{lsV5cB+CK@9W4;+uYR+>^ZV2DVaJplWmi>cLKpj1yThW7xH#!U3tMh``o$EtD^JggGZ( zIo0=PLhyacQ)o}vORm7sHLBUd9~;XenInZ4;mAnR4(Y>u(N#Wv*$PS*Ehn&lF6gC< zi)2t{i-+0#&OD1F{&}hm1F% z8y?PW>;vBWUtZ#$FN;vgnBI2Lhn2{2tUg0&_YvB@PU7;Y8gUKNg%S%tCT5p^Hj%5N z0i(lq1xM23E|DbPdtRqzwLza>Gf8db11T!ovb1{VnSsHd?O#a*Y6VSR-{!FjX|Zpw z?tO;n>(+}8U)=+g=hAEY_whJ>Vi$kL|Wr!4C#@_kfm#|Ibcabg~9O_pQF^+sj%716IP>vO z^x)B9wzgNeh&8%US(H*eURd=9+{Rj#I?{6vKQ<2JMCrTlBNAJZnV@=-Is3~gNgFc| zPEpD9G$H~1w5Z2KIqo&19iqeGQvU~#yx{e>Lsm}olr_1@V=Ac2D9;z@N4K6_i1z99 z9Q}e0{tYT7>sn_8#^@nbT<6dRVVEC~SETnhD!{I`PW-0hqBE3bUO{ugzo}oMrqc&l zDlbmpika#9-fn%oNg_Y@f+N_&PRT8~7I!?kgOMnL*uxSowW`qqBaO-_5awCxlcl8m zR~Bryo(uWkN7OLQr7+^QE*?me1^)7#h(YtowxKqUW7q!8Q)#j>M(Zk^&7?JL4;qa0V-{6Nn;J)swqpClQol^hzPx3RI$M^pz{z)#+Igl zi)sM($A_eNGT}oE_0j^JjKhdCHAYGiU@VzE+;6v7;Dgd?L2wmlPfP6pl`zcYuPGb|6h{`&0dINt;PqD9HY}gXY*8D%- zAJN(ieukvkClW1iwp>6k47R~w;TSG2$ByZ0a&I@%t6)T+V5Y7{c*T&(eeeP8{N{i% zCn`cOVGa#GX9kS+*duyhAkobz^E=`r2FDw*pX4!~ia0fwc{LI~p}&R_eAu!N&8;V1|kYWcy?g(*1gAWL9*f-}LyqA_ruex|d zq;+sLr%@-UCo4OI;7;??i>OP0EbZ4oyuy#-Qw2@cPA8OW~YQY-HwX3e+sn>*bgC zP3>`k`C!>4tQe`D$P`TWk)#gdo!;-PUtKNE*a!UFN>ZK!N4Yfp!GA0-w2UX~0CER- zt$b^KmsH#{s^ROyu~qvJQ6F|_+!YAVdCOIyjD&T3i`s+5c!kv;NWsi_dO^oA@R++k zX(O)D`ZLf;AWM~bXM%}8OKIWzZV1SE){adzB!=kR|QC6B`xL0ZdiE{@~tQ z+rL_X;4A+T%9I)Du%L{u`#YNZ8awiri}ZhE$zTqsm3~=g8uRCH=Qcb{K>F29#tfEQ z)4=BxHPi;}5nI^Aujv%8=Uql2>agw@z%z!KDudVgS&}}$wP1$_j zWXB>;cU{JU3)G|A`G2O8TWHH2KW9E6Bl(F~RHvUUZxRf5xHNLq;|Bh5E1Yt*hh`}jjVNPO=WPT$Y)JYGrK_cicv(yhEnarN zugIFOiv~VVI-uRtW!<;(*-S;ihKvBirEoZ`Iqht;L&Uh%CmKpd2b`gihq6qQlq8Rb zEK-Vs1eW&X|F@ASDfHxe(y~~TKyMVW6pIWlt@gm~l1(`o1Y>Kp6xeQP6x)SayjQ!8 zq5ZX>+K(+vP*7M2U1Qh1O!VPI7I~^_YWx{HFu;`Fxi+G;Wdu!Cw?JCiU6V1X65r@Y z5U>NU`53H!!;9-jBqSsF;bJlT%RjpILL7e{24*em9+YfrRpeE?`c+TaJR3A`_)RlY z1V&nq{-MIBVQkFi{Baw8(`p;fizy7>x~n&f!q~;Zk0ZXR;DOTzbfqkRL0Qz-|jp7-|)4!w8bSCR)dLK+e-kd*Y?1GTB2A zOnVj$m2A&obvm|UA2M-?FeG;MuAFETU((t1jf*o1a)eHf)HZdxA%96T%0;Uv{z zHa+|ViK5Wm6lk#B=?eP`q)!o{?xm$4=Zx^_Panh~TYM-OBrYma6WJ4dJe*W0?{_iR zpY;lIG_8$NDephd`O_Oo^w{Ef8gkCocP39jEKw zsv;iKCW#7DAZjjJmFlXtg#T1MlxOJrZ^W$5NvGsShwS=#3gGW_U!>Ju$oQmg;+QDB zP2;9FlIMJ&^wPpSqY%bdhbWJa7whFbw672JWf}yWEDmsFDe;SVD)dKTcp%!LA;`dbRk_i1h=mzs%ky%4mOx)?yYyk*BYT)6KbVtzd+-d+y_~nh6n`(<~CR(qXvOT<*8JGBXtN`cDD)m0 z?Al`wSI*ao*;Z|20apHElu{_*FVK$-NrT&Pr$01ik^|PjBD*)kNo^8G~Ugb45#5ZzO-++a->L zl3cF|i$xck>i%?{KIZ^qD9gMw#Z^f6$po)_$-FW+7{S~5?YIn=hzYY)PIphYR+zou z$%@9%qBTE8}uKwbK2zD5%pj2)7%8@Dr4$IzcA!5s|7aSjv=%0x~dJ8v=@CulX!2^0BIF(9TBpXqQP7tsbNIYqb_Pz*~36 z5oHzbBmpYG|DX;?HdIb7f6>&>Q70sShXO8RzylEX5`dRC?*=%N8OL)|s%ya~MFVQT zgfW?4E{60IX8diPpbr7y2_XmgPOSU>Z#r(b1q-E9)8|ePA-I*kO6J`7E|&dCV`WMEY^j0~(NqB(IHq$p z@-cY>vT>rloY{qoo(;yb5JAJX)6-ksd6QT|O+@zWR;#e?8v_dAb26>Y?z&zT6EM8*|eDe&^zZr5Z` z{11eSVf9a%X%yz1J=#`FNS0___HQvJAOtv59PgYvfPrfQtP>`Y8mA{NkdyQ4x}QiNke9B_j2G6N2R}H+~-0B zu6y->s6G2BP&a>G7K;sGNE?cY%jUpcG z+^2mH@#4`557Z|5296zAz`gijUwYyl`BKlgtP2t?#$maKx5qOE!3%Mkm08k2UyOY} zpv&zo_X|Gs3|C^L>*bv90@)=a&G^u33~`P0>$ z|6s9Q9R2+sViSOU#5m{BXl?TkaT#8%C}m21;H6kv>>i(S!g4& z3BIagiwNTkH@!YD2jgwwQX%ST@h6Hft~*Cdbxhv4uKKrY9JPFwd)xFU`x2s-YDOvi z!Sxm-T(GSAug+XE*`=Tek$e&nsEZ51*8FnhQiIUsowD+XvhQ3e&W`G5*Vn*?x3Y%@ zJ(sOTT8;Ue#8I3eZs)GQ3MoC(0tZ5oMIDYbOQAHTrj{v7+k_-ENVR?G@$7fn{I}&h z6v|%=lT4w=Vm&{qdUYhz_`O4I8814Urt$4wy8bMa!}ELSE-yui9w{${F*Vbd1DTZ2 z930_{!8WH<6TMxxc#(&eSdmaA7aC}t_ljh;0gdRjc>@-K(e$wPY_@^JX;+m~oIj~H zezsk+5l;pOu?>G@t3YT_O3&{Cec$pF`7ceJlx>RGZm5ByU}pP^js(GiG7@$X6z{ZK z47A?0)LDHh@w4Imk!)}TdLJi?7>|ekg>kPWkTwYi{&mXW!u~!m#=b}Sx8_`6&AWMK zTN=b!8VaT959Oo{9^NA*2xOFQJlz#a)d&`SB4cje zl)&c%w3;98)uSlzFg_wkL(sSNbRRUg=pTl&^fd{9mq#w4kw39*QTZg(imIwklfgKP zpXd&mx@qcpF}}o-$=l+Qvw}P@P#jaQ5WDf?y)h+sh zd*FQv-nG4s-`tC$O#&J^HoP0uEyJD4Y0VUVI~T+H8ViA01~Q3F(=v9d=A@mu{M}kw z7CvGnm@!!R^_D6YmUwOtaJo{Qm_MZ_I&uxOf^CdygOPmR@pCWCB85|Nxlm@|h@ zIWv*?$*sN7QFZ@GmZY|X_ZP8_m+$VgRx0;nwRiwI&gRg&e%b!h2%tT+H74GYxyjYh z^CgxI!Gpl&D?fE+^N}+v||+2-klxUj{dD^qxQ3HH=c8EQ`~O z1(0UFlHFmFKKiQHx3s>9zvqzTg2YM}`<}>Jh}0Drr0pB+Jw8-Ui}>~_&?H@w89uI) zJvx(|Mg0v*`OQ|?Zdo-@bCjr1YE{|PPMm+DvrmgGHF%TXmT&ZU?VR~+F%?}$j?UmBU4xe*~8p)ooD^*a?Yz#PA5NmWjMCz0S)@foKYSp_>joO zsNc3`Nj$e=Cp}u(KApU>%Mb}K!YhEbXqjGHRgO>AnHX@tS%xqQnj{Fyw4s_Bjc2U| z8576D%v;B$TKbICJ`|A_lnR}$mzmGi&G_|$zKmc?`Z7F4We@eL(pL0@^nQdgEwK}o z_wDU(%D5Sl_rHNVmhcgjjQLS~_a&<@{Uy?xeM@KWn}KvCfPm9H?&riexq008uX;2! z19OwxDW-1Dzub-`&4UE9c_l(rv}}G=|7<|E$hTG0h&aA+8-s7@E(EY_8RM};i(T(? z!Flna&Yy`1$F6$!ms3{@{(23_aQ)qO1fZ?G#a;Jj8Kbj?gNxt0E8sy^&@kwo&y|my z)9gmDV|*5bn_t)=QBZHj2#~9Ccm%AkYza)hOU$rzH68*Kk*o9MT7NUwi;IfYDt{N- z5M4z;rG!3g=+n=S`asbEI&f)T2J$ZPKKV6nvUDf^u-q~4!2%AUC0kvw{=W1=mc6dm zlvhi8nI@lag#F2nv&x@M!&7bu3dkgiLbgPZF6`!EsNi6xJ}cc&R^LE`{Vp(VF9 zQopu&<#+qp9&Kf_XNO1x_&UuFx@1Y}J|2N74eA3{Q!Uf^gS*9=%%^3}VNu|C!&>uL z+m!cj&hMMk0u!g*@23^S#vs^&wm=`VT#?q)X|@nRqT*7nIvU^$>LMOvLLNf<$WqNs zA>#pdoBcmJi00C2f0#FAb1AFv*lR?u)P=tIw};iZI`@y|aNSJ=r?cmX`N$OnVc$TbH^iq1#Fi++*7P>nFttEVaqbd`86R*G`wK1mSRH>`Tieb*>0G zBK($PJQ&bewh8x{P@;w+l)aU*%K68UIkyk7?CyABSduo*aaAHTwj;HQlP-#MxbJhO z+IAc?-Wzz>FyjA`vtl)E(IgECkhcc>ymBNqOLgg;L*(w!<^Q>P7O0(_9EF-8>Ce8j z7Pn9fCRj{VDXvh68$jyd4v-+}pfYh?OZ^%53ll zyl#-p*7S??yO~&Rt$#ftWY6bM){S;q9v0@Nqv^wo{M<#aQ}aZPvJb^ipO781E^bp> z&iE+b2J|QiZ7Fa5gby@6HzPr2fyO({CePMCcZ|aFZZd_G&84LMcrpgT&A1%6G|$xU zD7Jmw53*CRRW|W~a&bQT?MI=D!vNn# zX<_B=$COKP^Y{uz?bdE_UI|O7ULGNy8^C+Ujt%#e(!BC#)T(xqUA8^RBcNZP2fgQ= z4Go%e!_Q;*t$mfMNN6Y~XF{Y3gHlSwa1e@c9pw8PJ~qhd{gfOOiC%F!yBDX!J1>|8R2*cUs|evx37TYV5=tg z`EUO($}1!AWD?cI@bnv3XIDpOcuo;yWA+F6rsm~4>u;5kHE#o8XUld`9m3v@BWG;C zEuTKKZFZ&au$64ASd35gj%i_NcPnKR@(8fg1N8FyQBt-M6xdYsFm}5bx0ulHeQ)$_ z_`?b#_g~OaeEiadNhR{x<%Hoi9o_M;MTYa$4ftQ%8QFU((5cQNr69QS#>{>2VWDGl zwV}K|D-CMei0G`1vnY0GeC(K$L3LeL!OVxr?rdtcrq)0K?{&CMA;Zx!iNCZGzwBFP zNj7`(hR8W)|GX>wO$SBctIqkbAm`0fxx>C#jF5!5u7Q_0lYCTD|en+KLl8$I^>s;wHzU(kRtX(0rf*4dg@AhnU9MK~B zLs3q{)$?%?K2sisE6OKqPpUG`k<@e@Q_v8y#}Hc1jj0aKDauMwO?B}=k^AEAbhuMPnXlPB3+ z{2Z91AI8nc3+F%m5joL8>ruSK*uu^`tU%U|dlF#i^2tbrMg5j)RI_tN|9_SvYz+8W zs)>$tC+OkD5x#KxM49yAV!CimaalnOP<<(Xa5p~3?Y_l@Ms*JPN-wX7z`Gceu*@o! zoY_ifjbGW$mUr6E%OYcsC%+f+Q&=vlF299C5(#}L@-f+nY)nbfv+$8E9b(QW^uT?1 z$}Li*cXR1H)aT=V{@+A)0hzg}y}5k8^8*Ufj0V$h?VI8f(Bf93_%e+9fVFvuB|Dsc z7mwC)FYRe;%U-qe%~nCT#xffNyQM@VaQSuQm8C13sSlhfxhp;y>{y||2TV1jZ<3uL z$xW(?a^q(_nal+2J7sxIewm}OgZt+h88Og-0|wN68u;X8w`Esb-9lm3li*D0e^E#1H4}JJM1K*6S9(KVTjFochg1jl%s%uCsSitoLXnU0zarRH=IQL_x={Mi0KwIFRwy@o_YTcb3_0;vH5KOLL$T=RteS zzcNQMfS#P`j_SK_@7Fos^R@Se%Oq+$%JH8t#_RUna~zJC{CSjOI@jxT9Epz>qhsa7 z^$+2M_pKzTZK0v#i_sM65s`^jBLZayu-%YAr)7rBl4s*O>AaS-?&nYUY3kALCJ@3Y zROj-tJV3=Z<27eYns1Z7s8|-v-HHrD_e!1)Y#tU_KqB>?19PpE`k8+OjQjFATz6KB z5dTfO?!Z7Mi-W2Cv&_-`{l!qp2xZ`{F<)^* zIT)omf(nc(V5_=q)DU}JoKVxkyJ${FUx!M;jQe)FeJM*6nn0{wpJ()3RU@{j1a7!_ zeR3#B+I@GlW8q8(C|w@E2gSeK^8FOBOS}K7dl(m!;kUR7tEu0gg?z+^3Z1O0KyFl7 z7R6gSIj9+QT8;9s=u}m@5?q8I`sVA8^+0yE@8%=Pmulw-S%*PATxt_Kk}Dc`VW|-0 z0Z(_Ro8W1`>6={>vt_fTdR&j)xq+$d%I~$Kz5DJat7D)tVeny^X145bkjZubs)d&{ z;OM*y|49NXS@iMp{+1rL6+WD|`GHQ3KD{R1-8rOLE*?cc)#)r1O78dZ1Gy-`o1d!K zLj8*MuWx>syreLk(?BhjsB$2TTB;W}Mn2`RtnN2*X)!Q66KMV}Mb_LDM2~wASKs2R z?L(L|rksKn8=pC%d4DCEGhs#*HRo^U1xALK94(SYbLZLz z52=e^*L>;vcxtTwV!p1+$OHIl4-ck7gWOEe+ieE@AD%U9j~)X+zaZGVz(ae5fSCZx zt7(`c`d@^ho#qF!ZBhC1Y?yIu4+QgjxZmx+pr|90DS)jjGg-=$_kb?45Qn_i6DfNixbGFG533)0kL8ra4+|U@1Nhyo!ED z1*lK$*|fN`o1fk9oxTTN4`L(5Whx0IDM7_3D3tk9L+HSoO2Z zk+Xd=JE6ibdx!gbANVYbi#4D z9r~0F;TWzSW1s(RDGhx1c}bpwD&IF$knI$L3FR1wGQqOCjR}0UJ%&wbUvG?zLWODa zT*<+KCGK#peoksuUKVg3Ao*fi`|O}DR9{eG{6M6j2+dR0KaN>f|bKJr6P4KKqZkmz7pa8(K$38DrKIHaXtn?c~s8}dGd@+b<_D)*;d z`Tcz4ZnH?WC*{cSNh7vekg$;!I%$eH-S;(~ z(P>=MhSIC04Jq^7W#NY=%VlvW%}gvGaeY-YTz8>SY8ZrVl;pLqL(<-h8xX>qFw@A1 zsafw}L5aNF!Lh{a<6`?1Zl>3Ndet=<--$5$m-+2bArtd>Psni<-ydrirZjFFdEbk9 zgpwP1a$wWCB+;F_z7WY5D@v0y)kSiwi_8~8fWUcauSlg@LOu0psv8wYY&XvvBXlEQ zbvFu5hV;6>h028lLF}p1^I<^3hJp#RcGT4nBPIt^e)K})i6t!aezA zZ{NKZFHnz`SSoW+^|r2s--MXdg_}ri;-`u55|BPOXpmb{;(B)KgKbOFyoq2CAMG^V zZJu6?P8VrC5DAZhhHXnG0OGU$5a2BMntdKO=1nEd{<-{LEdXmO5{xTn8}9Zk(skRL zkj>t#Y&Tr{fv!pmYA7DkQ>3RZYClB*8eC)hQJH&18&KH&sTSN__p&%_ZDb&kQgqm~ z4hsqlBb0(YifTR~RwMcR*tAo-{f8W^p8vVl5ck}@;otBk!!X^VgiCQ6P8?vhe7A}-H8pP zzH6NLx`uO(^6zU#l!DCUZ@eC~AW5l5wt~PKsHgU6aAT)t30`0~ME2X4sl>d~6SdEJ zEp#NshP38R)Tog3S=@w_T{tTdD#=ca*v*attU;lgBs_?iOijLKXv_>2QK7itby2 zmAQ!F_i0FCYN$D_03I|h zDHY*>y6_f%_cpT6o_HYd6X&auS8w>-iB-jO@HpA3)bwNRTUw&4?QxC`I5EjZp~E9* zFv*7rd#*UOc4bF4l9qwpot_>qtuX3(99{xWJ)a^n>Ph`qaFF;u5go~XN_OOx`Zd2& zjW;X!X~gPo|2_}>*k~>%HZ7_08(D|rs7bt7ILroY#Fk|m0#eQIP1;GYy&F7zO$1W; z_{m8~cN-eYU?YxeR|F&&657`@jV%D@;%NVCm~|(nN!0_G>j_?k#^Pilw1=J z$oYLlG4poLKqxe3Yl2bWob>DE!q$(DtyAh7a$Web*3)LTi4EOL)5v_hJfhR~!$pPA zRqu4ZQ2SPga?%Os(|SHuc;1L4Zi_^{vYLZ*PNg-B}Oe9UqmttQ7rQqdQV zkBWNreSiORv3A8?59Pc3FTdY4$66@7K<>_~kWWcTk!_@pIDDmRi>bOCjpjW|(Ysd^ zW)=dpLeKIjQ`4%e7?k}y8goQ;A6?w`Eh~|F% z9K4ClEw+CNs9<*sIEiV#;uVq8PNq`xXU$PeGB=8%>oW5;_I#~0Dk{W>n&D}f5NgAi zSy*7l%E+$S6i6z{NSVq06>%ksqaXzTTG1OZP^1*h$&Mlop`(%_g?nc9CBhxg3prsz z2>~l=!4DU6(2s&7byKPmoJGWr8U!mx14KM_xt~8B93>3)sEG^NoDNMu5A)#~1f@lMf{0@g)WS2mja_ku4@)$^PZ^b(9qzYayE-tF#c{yv+ zFt2sHrh`AspxJ9pOD3+RVV*Z&ZyXlf$J8JDHQCA_GYjTXR&9(?&% zvlnrYa0tfi-*jdg{(h+^KbSYkU{MGy{5r+HmL43&?qZ9+A(oQVWl<2qtnaKa^)WY> zyq=#Jebc3>{|#TNmNFYwcZsTsl+3%o5w>6qMMhGb>fLt1ij_I_ZflEMcn^*nu;-Xr zPW(*R`^_MrO8IaN6Q{df+m!97fG1Nie47eCdU6KuV@(Z5y)bs9Q96x5WlBs;48`)@ z*lfd4goHTO0>R%n8LUFZ#ned-g&y`p4yW6-wY335m{fy^3YzR9o~9J!I&H(`95~VZm^M@)Y*^!C#N%dEDm5{vG$0(;MHX z!CkLQ$<=>3H`>1tk$M^|p3I`J;C3m5j1p~SYx{zV3a*Fk&xdIY(Mfy)QHP}-JN5ul(B6 zS}BCG8XEXnSy@{N!=}htKWpR?!;*rMGd=)y>=?b+oAMai|EQuO)erl(u;AEuXM1gKZocl) zG&nY9alSJ;4^b2sf8x-PQCLVrJkSNPZ#7^I4Gr}aBBfDGm(tXH4m|psH*a7fG1+bs z-=m@ckUTUt)_;7p$%kI86?3^i_iGjQ8BTl(uU}&|H8n|5CQjpSn!PZo@n+MW{Kltb zqu-r~{~wJg{`{F5WcQzW{?4-_Vj3$dSoByl89YnN%guYj&_*(NF#>-*C2)I5NjTv4 zW%@12`T3MJz@3MOsW6ceQc}VJUuKKRrth=1gDP?3RoiE1G0DivB6b-i0Cwcy@T1{GsWaj~OParbXE2ak@9j-+!lg46g^zm`5tnyQrk|EzQGFRHJv-`3rz zU2E});q0BUp_dmwU>snLi%_Vwy}kM2V*S?E7ADeDz~I~;K;9mNO8#{a$k&w>B{mQ> zD{DNkVXt1j0yfoTrUd*H_6e{zs&k4;Zc3FP1xE#JaTvSvNkPm{o$eEd$L1E&2#D#!)SNFGechxjBWB_*q{yGnd zwTJd@Is;K=P{|ASOJ(uO+5#y|DU|zY0sqL}{;=xEFb{yk=l^ad<<({YJdE0m=McGr z4+Rk1#RE4tH^o(K52+uw+J}n!hE%GKn}9R72Hw-F4Gs_YKn@Mw8oB5=1A%~tho_a? z7luakxxD-ZY5b-;#)GCsXb>gG%0Fg;1=i4*E#+W+a1H1(`U-&@BE=8%JV7K5G}i_^ z-=tcrGVm)Ks*9`Zv!}3P`FoMTR_>lB=Hw{L%Ma&Pf?vNb{*?h){-qnN4_R4?rlzJg zubjO(6CX=prY_5AmR>`V$pFmUJYBoGJM|PQJ_L+`i;G+3eyD?tg3{m<`+qsZVB;_4 zeSfB=p|^isIC;%feI2CQe_6Ib7!^3BBdhz+z1Hj8A}T6M^@*{IH~U4|>6}NpGZVf% zHwr;n`&8l`6#b$(8;JV-OKkLTpU_|!PKyzv&Hh-xm!iR?8kas}C?4uyMGZDhB|+$ozTYljr(=;P;OO680`?v3z%f{$%O?u zU0tGer__a7%c38wbab6;s2XKj)nJw4%cs(2l>dO1>PtW9Tb2*I$YeuXQNAx zEWcR~WpGkT%D~QOhT2p_l3G+leX`N*CIkZUA8#oy?(vhR#QFtRR#pIeM31%Ug|)6-M%^8OzpWTYZRLk#{?hYm*2GconU!TZ)Ss-UCDscBB-7&={BEir2OtcXw<3AH0_wcqgsY0eDu>`d>_h^Z*q&igt(pgN9nv9S^0P|Dk(JwS9}G zCkV*NcMlH+RJh2KGczAB@0fwlBH$ta$Wm=(2G$1WU~6NOUtDY*9^BQ%z{F(s3-EG8bKp_%QJXnl zOPRI)EmUn{_^=?^I1c#nz3~)TXKi2;+6@ z8V~`jLn|*YSI8Dh<9FS;XgLNR&TBK4_uIh%H7#v;ax#9a!Ex09IVDH(XP4hIQ>m~Q zoSZlV&hw`6!cN4rGsU2nr9#l3s^li!pGXalVYTQ6sIfo+AgtDSu5?92`zbG_}{ z*ZKL+h18TB6vh88)CUBg)4Buzb7*L2sf7!URPP>Vi4@#vlngVRa=&z`&~Y79L+a~4 zwP$mfBTp46AA5*GA(4G106#U+#s`S=e;C+98XPgG^^biMB*(;{0vrdpZGXQ^uE%_e zZ{~ewrpUpmXM{{aaUffhA8_qNz@qrk?DmUXfXB6hwXC5$i7!K5QU}>X&CJaK^W~L@ zW2IH8^Z!23g0ja1%5&U$j{f1p2T!4F|03KgO>V1nnp02T((}vb=;(zuYyiTZNh>&n_aIlk0QHT_yTB}Sm2@fkV@~;Ff0yTPIGwf8014|P61mqb3$N9ekqtP%Ta{}LJ zdgG6kSp6@lqDEV%7jxe*4R<>eFCFz(W(*~;@qgf>U+Ci_jd#RD|1o2zm0D*pN~oiw zV`XpuskJq8V`D=+mLEU*Qnx;}qP7+%XFS|GeR^&V<9jP5{Z=aB zdHld#)~lr}XD-6!5|eS~O!YqF=AE6L^6coojbGu1ityxfNy*|4Ujz8Xyr!k4WeGBe zbNRe`Jiy-KrxSk1iB#Ap;BF1j*tvbcrE2GkH?I zk$2WvPpQsC`CZy`aDYxdi91%~=l1&`_P{eBxMfBRdKUaGy$F(DQcC~~iI?|z58L3# zNUw9I!9Kdgaa>=gMpjhPl7mMjTXjc?c%Tv6V1F!88jn5UTQQ^3L&;)X1xsywgUaUv z&H$wW97m5OWtIql%YSSRHG1{9sJJ*XCVz%$?6+>8_|0Z@ty8!-SO5$yN? zAXG(D_}cKIr|%=+{A+Gg!CmViK|}lLg=xr)J?m^hoIJ$I0Vp_;{`<%TN)DzWHm8T% zOEJoq=bl0UcLPYANDfBM_*|)+90%%wF84Q=CJ6GCMBW6CApU>}JMf2uS=@u+Z5M!m zdV_(({JOT>0f)4@LPE^KB(gEs4ej_D+m|tI{OZ4LGW)2V3@-*G;S!UHLwD138ixj= z_yI{78JQA+aW+(dgn`j%n)HCvZfe!ylfc1y;v^Sb1kXYxfCOzEyS#atkd%~^K4zCr z1Hd5#F*?TnFR$u4VlU@wO%JbViF{#0Z-8Wmk&8AUcl7@ z95OvU-H8}+xJETr4q#J?tD}R1FYSYSFHEMm_P%}l)<7Xwqv{0Qbn3=e1RxFon?9Nm zZUQ{xB{i%VAF&3|dBj<9ai0MVopDd+RKm7uB4ytK== z@JmgivBW9W9U;;+(W*O{MJfHZ=;2tCpYeGilh5M&3Z7Z-chB3x3@?`jghkW9w4N(16w5f>?2rzy>SP_OnfSzPkCmy;0i6{Vq$dHVDZ;()fsU z&U%?^T!BDBc0j-9X8$=Kx5e9z_weBX*_BydFrSvYKH}&M2aq;~(;DdX7h+xy@3;1!c{GPzJUra-Hl*tH1x8&7M@fWjBT|hUFA7lrEW3*x5+rE zn|uJk6KH^K^wjIJ?&iFm$^Tsdy>$iw}D@T;` zi{b;#MNK=myH+~qh9U>|sN2QX1EfFN&M@@aQ^*AI``CF-ru`y~oWATjZdlSPb}gSV z%mtjMwBAhC3TgVF?kcaXbrob5ijfZiX{(h zDDnAb2ERu|PKAf#X1?uX&rX17&lfdTi@bo39TdM!FR1KIr)6k3$5`dsviv?JrHItS z&Fzzf1kmuXu%H171Sv`^21Z7p<^s}#9kie8*M#L82`XWjI5{TJ$z0=8VgM<)iz@kN zp~6kIQ0gX20|jSksck9C0D#%A+2|-+va;9`xswQ@EsXB3{4y^ctG)F@`iQz}uZA+) z*PK_{i;{T_v`6z_lAy1xZknolcI|0(C3QTYVRTf(m(JZ#zL6&RW~nGKwHf-ut+kfo z)N?s_bq58b@OV-}-ldw6A3=b!Ma!+Y#UO}W1qmtEvO*f2M+}2528VF%T;9iAB z^nrZ8Ma0ZY^8eBG)?rb-QP(hvfC>`Qtso$Rbf-v(q%_jqEggd(T_PwABBj#Z-Q68S z4;@3y0K?38@TbrByzh0r|G}9vbMABJUTd$tw;lfKce&+8WmWTHV~vHDx~l<4F35_h zz)phcZ2qL|hYsOMAv8tAjV$CCb|xvAIy-4zF)F?`@aj)`6@YfI4-S+|<+J~hogDQkE7 z{Aq%Fs+p2m_+wT)8q!I{0OJJ^A}}93Iy%B7rTbWqT(UeI8 zvM1!GE)Hm3=CVbEU28M(icDFMg} zVV&W3E~(iJN1JK7!h|d%=y6?HABS$1&gA1}ado^AF8xIApN)$D+aGtrZJP96ym;{d za{&_vW5Vsj2LN%o05j&-@87#sFXui#11wwjhQ9v(w;HgzB?kb<+xWGg8F4TJqW6uN zK44`MM$3c)w)KnlW`Jb{_<`K2DmOSgaQF&Tsl7LFu{WWZZ(KFR+8yC~OHijfTaToW}DZYxXgx`%l^>It(S;9u&^rZBqfN zTl?a0S16Ru&y8EDs_%Xry^z_@`+&sp9pNF+fw5oC32X>=@(esoDe$A8 z;7NklMt$q9jZvh=E3dr2_Aj{)z#0X$J&{jjqk#(U?(Y69C@{S`+W`P3&CTJ&+*~+d zln49eSp&P&RS^vjPLMY1H*czF88zadK73BUmu(*aG6z_SDXF?dI1#-Lj*zn+x5VQz z^nTCdkyNx}RyYy%T}(2DeNZRQ;q}CPr$0BIg+#^!coI(u)iU_aHK=$?*VaPCjPG>J zsCe_&%7o>9PescWyZ+JA&?aO#y2tc!sg8)TvDnQGfo z3SebSymBU`ZcUaIY2!sXY-RWO528f3+Skj&bU1d-bZakN<2}at|1;^$wlTTgJbz9X z>U7+0@w~Pq3=9lJq@<3nC^NLAWyRW(GdC=(@yW@1t^HqF0J9kLd%ZUeG4WRbZag|+ zlagYLNv~{84~0r+e!qvtjvbRG8*^vnhgA!G@>J7Il;i~+SYhALYt7tu{ijy~e4VH0 zCg3YAuL&IcryfTv7r6Sxt{#y;RCf`ct~Lwu7NdDWQo)IkR@d+@!%RXpoPTDy+{Y{4 zMB!#i#<3INTt&cE!q^Q5@WgeKwV|WjUq|%42zp*V@4%CKP2hCN{4;5wS>ul0Y^Q#F z(wDuwf~t>0icfu|HK<&MiPMBf@MLg(kl+#cDk+|Yk~8M#`sAZtLrsY0Z&;4V2VSRedcd_hT{#aEr`9KHAM!J^&DVxjA+!zhk#E6$J$arr*$+ z4+#Oeqk;EHurYOsPT}8kye=Z5|0yr|TynMDT>H0|x@g+KA8d6Net!D9yehANvw-p+ zrQDG9k7--N;th-*5lUDXJ1j$MN zoE=fFHeBVW&z}&*g)6PD+z;#e0g#SPX=60NKDYm*gTtvo)U$DUjHbV|)8rl6TG=g< z>;cYUscZnp=&|uT#A0=DpdEX5!(cj6&*b(V98v)JTg>Rx1OyaTRY(=1qYE#sc&Vum zW~==l5CTBKgZ1~^5t^^o&Ior)B4?%!wi;rsW(&+nlJylxx&9ssa? zbiqL#__1B^?GBAW!%|Xi3ytUtCn+g2GAI=L*wW(N>9TObx394mPVU+wG^RkM+`;sg z0P6|M7~o1umOAwECV8+hrDGRQb9A;ex&6l%`$KT8Ik!V%Bzc8JezEBSUx@Kpy#v(Q z8;vjS1X<`k8f_B%5`U%;y;{8PMISu5MB zzSSgh!-!eGHQptRSFP}XXbzcI);fX*2NGmGQ%b-mb7K{*$y z0{{N-H9_+8KNG*VmRm*OmZ`)Yb)M%Lk_e7)_Xu8jrQzH+a-+zU@qCr*jP_xk;~wSb zi*y%z#qTq(&9vS<7eMA@8_{S|v12nG)&E z!jmqT0-@>EOU`MX$fX6GBo11?qlFy^eO1vl@0+72(YSiE$bGw=iO{|O9O0j>sdkc0 zM8v090VkozTvLYx)9a3Ye$Vv|_p|kau&^kx(Bkuf%Ol6P3zq_72?879R@Vp5NUS7{+L3O>iU4SitVRU{?!{O@E z%;gp8>gwevB-V4HG_=Ry7W3N`t4EPK5t05MZX9>xYpFvDyww*?HQA$CTXRGON3ft; zH+?QcJ<_*@GU_kAkshPx2yctw`%JmGV^@$`sixOpxHX+vq3gAWq^!L}CjL4& zQ)xW*9Bx_L^ZngEgE|hW2SJPLHBaFhq$dN9@?DSgEjg#SCugJq`gd=mDATZ0Iq?Dk z-Rgf~+#RUwbOupc`~E9=AO73bSgaP6zgW7d;9pUCx5f5a7AJhETR3*9n>N$)W6hpW zss82Q)kdH1%@_7E!;jcu6Qvt>92y|(jg5AMPaBCwM59JPdvs!2OQ&Mw3uj!Q#!J2o z?fMucuIFF~x9{e7iStGRPLxFB$VbVU(2?Uk%EOJh6vnk>RT3Ik+(ZK>DsDuO@A;Mb zo#8ajpiBdw13nn}*+7Ek1A!GU%=xX#ipDKWf9RE;NQ1Meet@{oozTI$&y=|0p>?K6 zzll8S^GmAh%Fs|YvA|!};x^|gZ`1cD7b`f=eWA9%adIQCM2&mTCSjpw&*kR zr)@?Y#Q4*btZJhybnD%-I9NS`4X%&a#a)K_y(_ z?~#6x1H@{>(-$;Nar6%r5I%mXVDNIxSCQ_WoUKFJ;{n*kW~uc}u!r+*W^JJS;ZM^{ zSO3<_ig~X~Sj!p{xRSXU_V}WI_d0S~1%_3~r-|6}_Xk}EH7=tmkbfk73(RO2yVpe$ z&##EUh$|-MXRl>DLRtN`Mi5u!9@e!d6k`15QH-xbu!SD|7V~+KuRQSdJ&X%wGvUDxzKrw&Sv^^Nr_4**!AlOYx_R){*=^%P$6 zVc}M?;c}~T^QH>2wI=Ck33;8igm^68w1R1n>58khbR`GnZ|S9;(I3HRL&MW^<9R`LQ?!}t6Ab=gDuN* zZ+dY!{KlMpcNPh?KSSYX+$=fVA_kQ;*nYdi`h%t46k*6S``ojqMwLR#B zc;EJ(iN?P#86Lju|FM?B|Fw*xIl>U{SLviBQxS^ylHJQiA0NEq`5ZLEE z_^KDy7u(jJ{OZ%?L}Vwep=hF^Z~b=ps~n3MeQnbKWJWQY3O z(M%{a)-QBX?Wwd5KSXgi4hN+9eS_=*+&oUBN5JW|!-c1H>rB|5knSTV+Z&`WDx{?R zdyDuQPDtEP21Ktrqpyb+9KX1U+DdhNY3jOv|Di7<23E$1IaZ##Q3iw;d3r-!Xcu0g z{4I61o1w9sGg6Wr^-%Wc0BcLd8K?!)boOZ3K8(A?#plS?;JpDTj8wP)K1~d9dXtR3 zHL`ZyHY0cr!8_g!&fMVcTJTz@Zom4dOU-}Z{mLI*pQXr!#I zsRdqH9JcvhmM+w}A4YYue%dziKZf(yI(EFBmR$=-gvwb$gG?Q^1{mg+rhJX3iyKq0 z4QtauJT<0f>aK_%y6`0?+}xaNl$g?){vmQC7jf28NJ8!^vwW z9Bx-nks7YjU)b*7Onu^=fw;`(VXOUA;zw5k{xx#a?crI__kFk2>s~!*1(v|Rwl3`f zg>~76`_`kOI4-b`2L)mqGLT$(1ZzVD?!N7STSLm+69HEU3pEv(w#aB$^+EWbAfdn1&zx?J=cX<_2;^lE%p2J^DG}9a3s-i{6c>&n!&;*nDm-3 zDvIu|%yZ|sxWV`P9FKTG>ti#L3PMnoJiDotnLOJLN|BT8>)PMVtv|kW`XsP;yoeV` zZ@fYHR34l=yF}yfEo&>*u9yz8yxp%jfC+{=>5`Jw5naU}*|0$&>T0d0jSWxxEhD(A z1%bL$w6n0bPq&9!GHzlxVm?tP-Bl1q<$;fJfob4%+JM;aQ+v*))9-=ZByi%^ zSZN`+><1c&*NAtjUtVh_Z62AS?K4BpI2gzEE}Ho9*fM-UK6R^i%M`oKuJ_tG;E^a zEyvCIHpGnxfGX6K;pe?qk2eiwBdT#Yr@&o zOJV{x}5D|9fWM?-(M{@MTtpkhI3&FNr<{|(Oc$r*`JpV$(Rg58BC8agB^R*afmH#zkX z@$!OATTgH^Dl<@x@48V9n|rMLmdq>2Wk>|cp=8;q089+Zcse{6`Z}<%#P+FD)KA`| z`y*{YT_;b9MfHFThfa6p^=L5-r|{6e2to8h?`6C2*KNjw=R&#%nF3a#u`N!*dk;(2 ziq>0(^hi0@RWA5!ej;IbUMsB~bsV3Q#`i}=!?G=@lyIH2lZQ@QE-7M*bVe93)STV; zC725YLbCISO`OQCuMO6ZVv%3-Q;Rp(86F`nyBe&M-TCpTy={J|I*r@Cw#{=rTo@@c zZ=2h2KnQBhcne-8FkN;PwpVI(TSTs-%_%Uaj3A;iAQO&#+Zi&8r``hABZ6eN`o{fg zfK~gae5y@V1Hj07*DNkpWbD=&Hg3+7mqd|{-Z6#ep+S%4^fFzwO;7ez9K)t>Lk&nD&{UgD6P3 zI)%ZargQ@dn$SE)!57o))!*T`&Bx?jhK1o6E;ad{M#3&chKtt1O8mZ7kXqpTUZBq> zwf~r#`z78^ro*kx)6s=Z>;KBtSK{d0%A+hQ7`#?3tYp3(T5`IP4%$FqQzMTXPJizD z7jT~9Zd`*Jqoeo4SH8+RK)#julCBr03jAtEcTecP@}7rYtirP)9SxKmLd&bPAfL$W z0x>z`T~VJEm>@U}wLXu{chaHT($xq1U^s!!XP&TS*Mc3nE+9-Ka!xsWUXy7LJCtdR z4j6Ub?@aOe7Ibh%a4b!(T)Ce|#uu*Z^iFKk%Hr4+2^QQRok1QA=82t)}(h|zuWJ%F|ct7lNvGUKes%mkvt6L>CFB# z-O_Kd{I zI(x3=d&56TjWBg++2?qBb*VHvRC3ap>bzy(eSdQSu2kpB6erJ=nwl*DC-k7jEB{4Z zo(Eay4XJ4f3p>l+&~ElyWnzY2^$xTDcs>Fa{W?vTWVa~Pnw5RSM8Tb%wBs6?Eat&} zkvu)T0}&s*;mw5n_N$ji4YKxI4rlvriCio^x>;B_08^Pn(;3I=wuk25|RD=|UTS2{~%m+t&R4fGTg6_)4vL^Oo zp=M-|4~A_@BABK6o;amNo@=$0{oQ5)R_kq@j1I!HN^7+ULD0qyI?37l7oASToI|B^ zqw#1au(w?4hgEmeE1fvXmN>jGc1j{j9eJ~wB&X8p)+2%8F#c6xhb(Tiu9zuHCS4 zh`>ehaYN?5iZ{>Omz+k*`wz)pqr$+C@NohqkDEZMrXwr8vBdVLl$&htdowU_3k-i| zHCSDNe&M&&fVJHRKZ9zx5nz<*_JP> zi(@?x=&?~}FRpY=_}W_6VXp<=cxp9qk~3nGjKN>EC4V#yAdX%)ny7Opf0V#ssR~_c ze^%lx&N$IsulUps^I8n6w&Hn)x{~ISDFRyZQbBC>lyz_(`I&m5t-A0jNZ{g0N1NH^ z_a*94opAi&vZBKtzY*J2flI1J2m!n!_Bh+)+r*TgT?w8mCKf4$-A}&^x3Z1XcYKq` zcwTj9r(%8$`$YenNuu2i*Ck^*rTMz_44c4rj7w_BQsN0$8{V0A1wo0tg`SlJoPU z2AEL|0>%X*988`156&g8bfnA*$$Ee3`N_t3sHaIu{I83uF{8oFmAUhd zZOuke&fN?oFX$25#$Jzd-e$+wN;IRDz9O^t$t}Y7qf{C;(AR=9O(cvnnKh>^kp5kD zdNo@r{{Ax_v;01M`F>d`Er_dpJ=c-0G%l9j$QrPm{g`F#M(bS#lfotxrSIl2NB*)q zmSMW+Nx6<8*kM5RBmvF{Mb9=}PCbSyi!L;&J-A=ff~!Q4jIzCz!q|K|{IQ z^U_cR0VAj4o%)_D7};q)}y17{Y;vCX4pg;2NQal z`?A-Hgw4Y(u8L=b`AsqW8CU4+!To|aKgr=7oCSDU5xRBf-B`U>?R5QVk64>fxSC`Y zmsgEc3MP{dM}EWMqqP_GyL{8C6&pt)R~xBPV*XECL>-5pSYYi(Sa*f@3kvfV35b5) z%P?8IaVs~${qDIHy~wk!!sdBmguCo?$1tM~PpuK(&Tx2q$f4P3yDRNDyrS;#gx0mc zg01+sK|sOfyJG*Hmk}4WnG-GwtOIWq_T20MQPSPg{X^zK%{9S->*a$^Q*_l4!Rp}6 z1lP466Lp6>l!BK#0$PKpXA>e%`S@nk)ZJ)gE$aH7Uow&=iVoss3MMzAvu(__ixjGe zo|LOyepq>B;Y3~2cqQ8pOL{9X+gtg2Vs0Vv0wvt>hUb}_JJTU@e-<;wI{v2!)d7W0 zTwFdQJ(nIZ>3lE-=;%z_1n{uh2Em!wC3T;%4t+oQIQqtf#SJCgD6RV#iiL|JFqM4J z3k>~|K+6*$7)gR^b4O9~fT|Yzr_E^QM-)(Wx4P6|xBC9tK znqoaSY|hrr z)NV1gmQKhTZXNN=N$dy_^q`C1^@7lTCB2Wp2iz5uDG);Eje^Hr3Ejv(&L25d%JjR_ z!^xa>ft?}jz?I0}lq#QD7c9V7=asot-d`a|H}Po8j!oc%77noIEqcB*9$jslsRN|3 zVlk%qzR|jPzM&;eNc-WRAER2w^h9@_T|m8k+Esemc8Xu*j_i%yxBzNJ#)MAqz&o$9 zzT;#9z6f&LKOPy7ssRFuP35*2{z>v?MsUBlUP#z;^%5cg{b~AOW9vT({GJH17o7?; z{!9A=4v@>C_ZHrJ+V>Suxpd|~xaT74BS3!`jbh97UVdF)FXSU?sH-~=geR&12vPvS zs4p^VA~}Tw=Psajd{=?c-+DRgnkunZ0rE*%R}u)f;z>#3OU1#wR>ym7($p%YjhWq& zvN8Mx-jOA_X5ceblqbrE1H?i&*C~h?u!H=r9>j=Ud?KKf{#g(|li1vU2a8(B1VZ&Y z2#>tUcA$8srtb~MY@UjWj5-iZ5!m+q_EoDB4SbEYdt$Hu$m`~so>4e!H)RwQ7OtF4 zC~q$N96{s_ukL>RO00XZj#DlnbJQ<^Mb*&YQyQNlk^1m1;nExLycs2_m?iG$+3JR) ziPpBZ&ek1b(s_ZHtbPgd86XhIomc~a?Alp>GJSM^{WwikSom{A7~Fg>wrAq#LLyXS zX(F8PYv8b^nxD@>%EesL_u!yDpNbtF!!c(&oQ_U+Dg}B1B1W>M)!*qh#dKT-R-zI> zga|+bv#J1&n>_1X_H}@K@07ZPEpiM!pdPd%q-0(7AM~r+`1KH0a38^z?NdT+f}!7t0@_y zPO=UUKYv_mqWFBr9J70luCq)?=F3$6cPc6Y_6tc%tD2w}`AVkJLq5K~V|O~&PBLZ- zuE{1i+U`u4U+ik8i8Y6RZ@GR88h$GJY`@O3W?%?ve#Bva6{XLu5=RXLesn*+ZA!{* zQx?{ZuLfUyWA`BiS&-zKZ!RFrC8eag85gSO%B|^9K7J2;4z5p}sSj(H9A4xrn3@l9 z-9}NcF{e5mDaNdSjwd8LLjwLUT|P%ZQyLK=Used`GmM|paP{2WB-MuQ$`2N0DEmvi z`|*NfER&xzhDgb_4Sc z@I99XLHKBgPL&IWd}8CBW*mF#AE z@|e$UxBwpP)?rHg*D2at8(;Vs=MS^477yr|Z}H3U-%1`*-O+zS7d8pP_kC{{jOD*j z1JjIY!h|J#t|x`X8M2Deubm0V1^+!}Q1$uqamm{!;PgUTytXmXJFa6g^9QCk>={c9k?Zf}Yx5nwe1wvkX-vPIEG*Hd0Sc?E=rN=q{UX8&zC^_Q~%6;yh7ugYJCaU4OSn416@&hOLG zUbay}{NemOi{#s_jp$!bngAh#y+Gv&kC{B6hcOlZwhSlrwM1MQJsA!#I~9Pp#rc<^ zZq68ZPB-K2oBazSFzfwaIR819%xzD!{fkow`trX9AkO&}@LP7Ve>Ej@dH)6gXfW_4 z`+vUgn{n?zI>KAOIeD0%jh1_0<4tI!;OW^Z{A9wdQ^x*DkeRI2LTw1|BZ9Qb{PfWm zH1$wBrJM21#g-ohGkmw|gSeCD|96n;6A81sZ}9HNx3z@z_J#&c%VgAV@F}z$OgJ0&1R_GHbLCH%;pxD!`48%+X;5^JwH+U*QMDa(AKM{J11zg%v?v4#c@5`i5 z#}_Wg4jjiZ6vU8k*~FcN$V0m1NYjE(-V0BNSBYPC57YL=T%-1vywugtQ9UP+baHpH zds6DJ^zezD_!)+(R0qcGI0qQ6HNh8Cc0h)P{&Ks^>5?u@`-$;Mo~p{h!AiZi7%*xr zJ?(p+jJJ@;fOqOD3ULyKv91DbgZj%JY|b$yr^v;c@PdTNk@rp59^kmX#--H{+wax( z{-jmRQ62kC5QnYy_AQi?x}DBt^#z1=KyC}1$z?L=i+gu9opy6BmGC|f8h2=dV>hSi zqkE}EZ|dp4HG-Nq4%ksflDh`wRA>gDI;(SGAj~mQzJPW_X~4}u^Ll)V#R+7ZRiOlb zx2i7UV~{t#V@-f5R>iVUtWH67*{Km5;|k$el9K*fuf9;M`1C=K@x6BiZpsd*qbZ(; zm6d6<`M5C*{nBrU{YEC}tQC^Sj zFxYoo{K%AYc}Iy$)q)FIK&xHpPV1%p;P*swLuP$?WMH`t_qwTAay1;A|HnTqK$s%A zQt6c%?*$hP#e61a^QO;#0-r@5Gbt%x54oShgjT~s11 ziQShrYrCV0d_NG=fo1?F!-ZZHnq6qI+~H`%aWX5jKOv1O?>+|M@zJ-I8uclg8fNfc z;L|=FpXi&dPkO)(QN20HukAD_R494!`mX(oMzm4Mmw9SM+#6_LyQgACUEQv7r#`1C`UakUNVl zPd04YH#g@7>__30b8B%U>uPk;n!a`Y^3r<@s;s1#!L=N8i<1?vU0k>EINX9b2e*CK zLcB8YY{YUyJP~K2Er^W_@^Sz22;Y}Ew(-m`AaE63B7EuQmbg8yf83@eNamYO}SWh)E0Ds6~TFd;{H(94RR>-Xx`;3MP1X$Eyye zy<}a&T#+BJWzUTExjC!xA8I%q*A}Qj%E-{1bm3W4!#m#6{0X zL$7hmjSy1EVK#Rnk0G`h<_Nw>8=lexr}XL+El#@+8qJv^(K;=TW(OsPT)VK`#jE)W zJsrt)h1+dzV~|?hOe*VpSVAyR!VEh;S1T>5a;qoM*JBWOoS!QC~p?DZEkD z9*@>v&71mGGe!nGHp$$)A90yF82}BmvqTj8v71cmg5oNm+!?j!+>MQ_0#s~2LHx8) z`wG1~q;H&-v;*hfc4x@p_?eM&wz0V%u~ml?ylLWdGdi$BJ7nzCxtHSxnS_x4*Z*Ei^agXK(=iPMsN-^*a8puzwG+Wur#yC$#mh} zYX@(Q70I&WLTJQOv~KUtL2PcBokD(qtrwcrf~Q8aOuN#rJ$HdvP1jsFn1bL$;S`ka zA#-heH{>=v<_&{U%|h8iPeSv4`J1M&Gl=-;@!;T~88GO? zdU0WTR=~PX1OGL|JfFOzCLvJ%5dbvmuZ7ha1cZhXDCgc@`dg{06a6+?v>&indN2Q0 zB7TG6f}+=zl#$9Qcf$H~a`FI}OIjZU>0Cno{&Je>A)7>lXwu~50R#;vMgXuXPgat6DzW%MR5qFzXP>SsyaN7(1zjm5s za!6EIZ?MdrtT&sRC#&Cqlmt2|0Xr^Vt$jL7+H5L}%e4FJxc(SE*|&!j+lSk4GsWU! zr7JPjHPpBm?W^GC7qhFq+9sU{C>1tnXDJlI@;G`zm$xkVQ~h=v*wk+u?UDdAy1ys8 zqb(5Od(n(~b|)mheg>yqK$`#i&WCaqFJ|k^mdOUJ-MgTH%}9R@kwb{gIJ@TJZky-W)nX1Xp;mWLhngI_Vi z&lHVO<`oBlDCP+=vSZd~)@&v6Nd3*y=8#BQe`X7NTSlXa@_{myV%I0w@az2YVJ7ZhGCs{TGNm+#Hbe3PNgdU|DhOPres@nAD!}5%;mD3d7oUrB( z&g9i+S%+@Q+*hn=6?SR+*EhCXmY1vM!=`I%w&c^ygB66kYMicf9GeUo>7D`Ac+nfh zd|A03Hh}WVxD1VP*cAmQ_R_Cy^kE6frK%mnikqNISXz-)=fr{55Y`I?C)RU6Zk7{5Q(gG+ZY|%LxwXhdD>m8p;T#^oFFHmRNYtcnX$*SJK6*tZ7bldWn#C z_u@+g;*!Z%*pc^|itqLM{DjZwtNJd?v_`_wCeYU1kR>EZcMvUSqdtD|83=yS6q{UT zM_Q+S|7UV1C}c@Fl!J**jxl!YZZu1@(39gAS-TUwf71~Z~yv4?hrW}ivXslVScy?bH+{G5pbO5(BE~ppY`%Z9UZ~OySIk4Y6lJO+j|Zy~g1OQWktD-!lrZgH`{_Sg<6$ z&z)-zYC56bhfJ&iLzV*@t0LSdPZUcUBAF=Qan)vA>&rC#G_E+YZiNw3gXXeO8~0$n zfglflx>2vsQx!d&JJDMAW4w}W>E*A#a`h=?bT6KT;55>GBnhp5%xQ)@F=Not6z;#G z5%euwLP4iGjoPay?DXT}OZi!uJ4hDyIB>m~*habACGO9u*xFa<1q^Y;=-J!UkMHj= z55kXGmOX`2Y9Vadzo+`zcq4U^N&2GkmgPH)m_i@g;9Crsb|jj&SmwHze?gpMHF9m@ zQ`7i;6nh2`3(5W$| z7ZY1v^gQ78%+LFr`ZajM>stn5OGN5D-@Pe&nTe3|W9}!_+b_9oF~EbfatC)%>lZ)T zXdju{73yEyZDF17n3|EMIlTip(pRQ%ukQ>_pm?@vpVr-JMkyC7+WlQqg6!3V%&M6P zM_^z4w9_Sev|y80=VcD90M1kAN~`gJ6a$qK|32N6IhJGrHx`ZhXrtrkN5>de6tXKB zK3=Zv3e{EAuhM_;wk}USwi;{X^nVra&B$`(n<%bHlcP0+4|Rn)$yS66k%DMhYH^Pt z2`wDDIC*YAF-l$}j#VJl#nQ{8{2v_rG=bx%Pu|Zo)GVVb3aEX~w2)}mX&iK>!7{6* zlKBSWj?_z3jy??k^uFz}dMux;rgG2YX3$glW`p4v#K6y8n&QX!lBba=B<|8cF(ku$ zw~@CMpkXV6-ilG^M}8nc!99{XlXA<@3N zl|%K9C^)n+RR&>uPSq*J!N&nz2e)bH#qk?T*z8%sHa}4H& z_s*o_y@}l`2dNl}5lKAOT-TXHJ?Bg5Nn8=m;TN+FDMMajJXZPz9Ax)6_IYdwAJGJ}!}~q}JRY>5jV~?Kr$nD3U(9tR(NAZHqoxZVK43y<2PMdG{lxQPEq~ zk*z4oQ&CV3gR?X*3$pCMdgGh|#6X+lA%!ciwHfcE&p*z{{`Atw@i+DR_WnXAHU|xA zN=7eXlH<*MHMr@L5-v^k4n3eY2?OcHaAI*m?Nn4%!dV}N2%aG7rJQs~ZQ1Un5 z|H@11^#VqjCk{Oi;TxFLBwxP^&&5#8M?I6qzSycc znV)52&EO6?%8xegtvTNjhLRT-xFU37E}tmvQSr&I-jsjR8*CbxVXXP7qU(eswozez zXjh%QV@JssKpVSo9qJ`qSE$Tm@bS^k1pwx;uwr$G$loM+8B=#&t|pd<6ObcrGY%K-#uGm&J?83M&?S)|p^Pt;lFo;Z%CX2p6~_WvX$=ie9P z3c+39>ipg>s`p<-h7BqpB4tfSL}=i7nkG)(MEGedjrJuAKkQIWeJDk?TFz=VzQ;#$uMgthb2c$6Q9y+3fX@;UHpgio*QtyN?kfo7$a zofl@?DIc%P_&>xNZJ&)rLF>X{N$Ya>HdEy|y>1hTL@ zO5D8f!Bzh?*2J(b!)mlOpBiSn7AsoDqSz$E?G+M?>A_3ixhAR7^X#sLvIPl&-U4&I z6P=3@kz3Ge;x8~4dnt8QAgkGaFDByci?t+Mks=@What*5OIYGPs|1plE*q)pl%k*5 zrq1rY{==EXsrCJb_Bwl*@Bx(k>zIzXGrL2gU`ocxj2gLI7n;W_SxSz%LEWwm!;*Vy zjFx0eYD~0Ya(>!lp274q*=Hu@hcGz z`8}T99_*57^M3w`Wbqw+e-WG&#WzJVmEp{-m74hrKovaSiwE)DF(&UnetmI1&t(NK z(j(XZpok1VJ6H&uVT>|GfJr&aPs_w(tu*!%itXHslY43}ff)X~22VSyukm8_)Ues9 zdZr+f>3f;0g5cQ8vuh8}RZ7xj4a%{d!??2}btu=i8W*$TpeQ} zOhoq&TFozWwT~{oOBL4(gfX1jj#YU*CutSVBRuG^uP(U&5ID@r#$t1;Wao}GIsR7H%iOW zjJG7nS4jsYJ`megdi8;@hV~Wx`-k|#Dp`fFqkp`%Ny6nF$8YwA}TeR=`xAOcgXv;}R zFhc`i{pP>Aj=(imn9;3ga`S)P#&i4sUxwlKYa;*W=jsmuvv2r+%8JpcS_7UE!G^z# z!!5qLB{KH^|91Uh9V$9B$1u_&D!!wIb^s1L)`<&}NDQD83i)DNc>cbZAUc32Z&?&E zv5yG}8xd@>IjYd@dqX-Ztsl{^wHux8_)J`^eaH}cQuwY3le%6{#>we^o^?jhi~kNE zshl6TP2mQ5QHbi26}j$VBPH!>pP5>wUBtAs43Gs@tBwdU5RKM(eX)Y^+OH%*+e?Ri z9+!IYS13%e6`|hp67kKN0Nc8JEgS<&U(C35;!;bbfS7WzBnsuuxVF)9aXa{QbfmX^ zg36E;cOBhyDFcDUzRXvPn{(VZs(N2nYwvS$uz(fftZBo6mk)&;$)#{~@}#EqR~>f8`e*N1PrarQg(*ZEzaP4l?U9UrJIPC^`(b$nMkPl@*} zFKbLa_gijivy^Eawz&gPxL)BSi8LbF^GXJu%rlNBxi&+9e=$&`LGf=zXI8*MaJdo@ zV;4v_ItCxCn}LrExy1+Y*j;G|Neu@|m5ehIQ&MzTQSKx@jlQcT4-kzN7&B7dhyxZA z{8>1OOrzeITN;K18)aT&X9XeN=tWlC;uK?oDQJPpF7xUxyOIL^1+;l_CI{0WV<)G0 z;S<4rkD%dGy8?+CS;M@AI8BLTR#(~AG(}-i{Ohw(^kPl2^Q;&DXA({>jw9$@jB+Z`NoEB1|-Lp)(1ZcGv zBj(#nv;@c7SUC37VIL#?#KroSXs=IJu18$tj>DJShDBSTLysw`Y5f8MVhvWVt!;PQ zJ?{D+K606XP9@$y1cIz7-Soc|_MO?Zq;f4D|- zM(Mg^~>GjBNDI^i*X$y6!-tT)x%dvmH?dc}_n(w9` zVNN-hX^F31!i`Q_8?D6Kh{j}nx@-hz0+kET+_Sx3Mt$W3E7Jd|QAp{Gg2lR?!h*pw zwI^=fdZgjJKB5(3H=p{j`92?>(G?n7I@G(OQPcS;l?+S{8ZT{-dil5ef1#o16SXhM zAiGEo)oyu4Oy{`PtXtBZ4z70a7?jyKx(X^0&uC2pxyRX-spTu;VJ^7F00wc?4s;O853agU)ez35+O+yqVZLTnoqa&v}_VSX&cX-b2-4N^LvJ#Tmvt z@U4QrgxFc^bA$PVh3BoHE;gv{Oi!p@H5>R&)fEA$wyNIOo~=3VSnPx zvCMX8>bkWq(X*5oo-e4mn4*Oxk!3ET^6FBn{+UMDB#DcmPfUT${BwcYZ!P?g3Fvg& zhvFUS;h4?AB#szMelzI!&(v@r(8OuTL^Ds?WjzQs&N17U6-e=EPQgXBn{z)`$LT~- zWVD6v+a!kX&81a_$nm`r@I+vUAMMI-^A0$?$k_ zUmoa|>_*0s!{p{yK6f_J6NOqcZGR8TItudp^`#{)nbvPe*s*KWYJ*rx`{li;bz<0i8MJ=f$vS2;R@_Iv3 zU=an$HeTohoqKu9cpD3iztQ>s8vDwCsG6vMLIec~5s(HcsinIVR3t@8kQV6_M7mSy zTw-Y?q$L&>Wa)-Qx;q3UmTumQ`aJmj|L=!)KJ16R_s*PqVrK3+zd5IrQtQyw>8lZz z@vyPS`Mlh;X9xJT&9p3})Vo-h67jX2+-dQcSd}4pYBgOIc?AEpwRm7h3k2De`Kc}u z$95YO?y%$^37_vP1>uyxfynI`9XZo)>S=Xb|ne#WsfGCq)HRGgBHwWho8W{B>F zn%GKEwW2!>;f%Jj!u-E^B;u4{&JPpv;BWh9%2!{W$49V~ z8?e6S$8JnxW=|TX-}3%0e_~Auo+8&P_fK}|(fVEhBTD4Yan|t-OiAeVTY805jk1|z zMCIaz=Q8itPNfcbcwCtCn?<@ipI=Nr8S`3J$Tg8!0uI3&U+gQa5)eqIVip;9^o2X0 z`$~+7Akx;$&P^$7qwv0SMEPZh=B?a8Mvk#*pNZ>QfpQG<^>OZg9EExr>0Pu8PqGGK z@otA&KCNlba9_yi&^se9E*_Qa-T!`r zn~8{A%2~2r<+Qvu=|ID|{j*)k6#qE9+LtbzSY8~VH^ra9_qg1U{ZrkA4g@-&t!Iad zp&z%oywE=Mo`3SM4z04-OV%bcMoo;qAKTUx{$gd_NQ`v27b?#eyLhxNaX}mCOrFEV zHTJCEFq|t_4gY3e`Ab1kyb%4JEI#dvM~0IocxHW3%L=}nrjk;Z`)uSDe((?bOrUvu zvoU;CD5(PZa1+97)KjPQ_gYn?xF?8$LM12jRcgy=*Dsqo<`S zne)bc@uIIFWsN3`CDm^oMi5!)x9AI^5CNBPvqFMjt8vg4Iz-m(xyB{-{1{`ohpj)F z)K{j1uVsAEco2_Cq^c}}CyCfOYfoxR~ z<+RwoR`wv8SR#>K>~JDi^uxoFGLLEQ+o0;`qy_ZOTFa4nYQ=cGw;KphVxexd}e?w?2t~T(D`;`L>^43yY`eubGA&JxC^nFwZGnEn9gH{KM zhe$fE?OVkP566CXeeki7Dq2@d-P;PZ+v*cY;Pez*HM)HGdq>)vmnkKC_fJHm4SCl4 zxC@Sj*;W2RTx+G*I4PmG(g6JkS>@;%4~_0Rc>*@@^1GE2R3B0w@ZQ{Xp%5cqFe0KM zD&)S4UcYy!y~i>%=A2jlG`KuMU0b^!eFEaD$tgxE4XSmU2yOdz%u~j{w_Nn>vqK{3 zePVQ5gSa?jQsRP}@!8H%_4zS}VQ|YfU3CU(6jlHW#M+UdU`naodNx)vZ-oKg{ZU<@ ztBxZtPk8%-yvg7XnZ-)D|LFzc3$-ozIQQ)bdes7;4dQr4{2m2_*b@s^WGLH_+CGPt zm6xaQG>&_%h|05X?NtVdNUrzy1(6>iq8y2I#a@c%AmVI9A|AmN1_)Vp}bjcczApGtyV56iQ=ssjzt~c=7+vB zuL@`i({#T_++o60KkuP3?=OT2M6_jZP3t$-4U3-1lBm~hKiCTS7IYF`&alX~m$SU3 z$QWySGS!!?K2-P!v`Wh_s8sGf=buVK(JQJw=!~V$P^CzTrrA7>Bl2y*_k|5g2NzDk z<6DzQ-4A{A+!Mp4aoYIp1OfERPaSFTEhLH80_pqfV#oGhVHdx3AIMGRu) zx92Z)iYA1&?%!hF)w}V>Ye%d-jC`WsnC_rgt$nJ&u-Q=FyBHYKR3x(>w|dMGnm0+jLYBV8>SYE*(%U-qiG^eUeB=jY#HBl@I{@Ysrk0P7b``3S>R;_1@aw`%>NI7n`oHYzT%N&6Tjgg|3LrXtuaAEMRLZA``W0^mMJ{p-yRoM3*@0X zgqz`qWLwnOE}w0qdzzG|>3nZ1Gdlsxvzk6GthXHQCPN@d`Kq!_(9dk~Fvc1XgmrW@ zc~rC*T1QJ=GC%YB6D}*US|3^&lrqEw+rUC>0zF&gE=@8$bH+84Jr}wV{BLY7?^Ax! z>X;EufHG014M`y)?orAQxN;b+2H_nYA)KfbzbFTYINIqBk>NK8oC%uYz67@@x2$tc z@O&zatyKH`I0R3H*uBTGr-)&i$(J{ifHG+>hBmaKrC+g6U?m#(&Vh&r|<-*wa0Wz-#+DnDR(6d2RF?cKhhpcxHPZ z&NUsWkm68t@c!o>yyD%4F6y}|+K+2tcg}b(L=K+p&DNo%T!5Z{a`x@YuvAXng>l>P7U4uNIa$w2CRCTfO^G8>t>ev52qNAw z7gs=UB-b_(nJ0duO>r5a*Z@_;F%ZY?SGQ!u)+57!ctmjg`tG&^0Z>k%NFd}y9PA$v zp){Obknz}Uo*7*d+|{zS7^_-HrntIpipmpBR2FA1IBz~aB*+!D8@wUk-ILK!1u+szEYf6&ue2N!!eL^B+<@RkNJ;=(#F*AcHhpZ->5m*SqPjd&2K0a9fRE_ zrlBe2fL7?X=cl@Ljb}q&p3+NmU>E1+JMZYh|^HkY3SD%B`pP2beN(f@O} zX7%KRN#0`@gOSvGa8*1$i}==_iwk|-LMxBee9{>N(?)4_YK!*l(TzJI(~Z05MQJc1ESdR>$iv+wX+lzo6Q-SzN9H<%f$7lw z?DSMrCjw_p2^h#Vm0CRU@p`snKWDCxqo@#yIHb{}_)>Yaf(&1Aim~*D)(g9H1*vfM z>DS5D;dpI3V@&d7BA?7K)V~Z`bfwKf?Tp^vg8MQ6?v?-F`#A@v&ibV*5A2Gryg_l1 z36UP1&(i(jJ(*hJx)i-VVkCT&SGayUj_?W#mFE&pvoRA|Y{Wkg^zSC|wslRA@4iKy zb8hX{t}mIGX>{7A3xHy?yvwuL{Pe2Ya1nG;yMJ3OKfg;3aKy8OPeLp&wD&};CLF(% zkO{vpsIm_ZY9BL!P7vXDIvZRR@AjH{hHp}ok6Ai7JAdEFpy%uqf7h^Nr=!&Hrzk}b zgvtT`(>-DA%m$0q)#xGNCppudMN`;@uZx?%phFNmb9EVmb$q>xEQM#jhR z7i{4FT#oS;NdqWf>X>3Tg-&Ren;KOAr_ZbqpigHOoSTGzk6yID@b&axUEjp;Q>Fq? zEJ1)9lEFx=8bl|a@-L4f2LKpCFYzJw8%LhX6QhQ-G!}_aE&Y|71ub zAUFo!!|#O8Z4%#L$=N@Ym1WME{~E0j&``$@Rx`*-n=5`@(kI*TU`e1}=rW{=l03 zKIPN$PwF@ToXd0@i-Tg*x&c6HP|f^@T)QlT~Yvv1L}g9_>T&za$@+UH2kA~bIucgGll>Uu+iVqJCLA% zgZ`WE?$3cpUhiMSQ8iNHk8gay@LN%Gb3^-MPg`ID>KA`bLjcx}`OdKjC~oSxY1F)l zIFr=)k0hy{qZ%9oxv&{oSWbNYD@oB9w8k>T6g!r7$$$8d)W3~|pmM-YxsDD51*$x9 zGM=3B`B0-X<aCyvoxmxpyr*76J^v z9OHjPmK{fs>T|8%vaR1l)lug@PD)7wNIA0~&q=TKI?r#i0gTotxtUJq1;cpZ``1&l zs0OrvJp$GM5YKomv)EC83j8lx+uhgNnzsmQ_Tpq=Njy4w0^Ce{ee;SpHZx5K0OzxY zmYIH+{C_Tf+!X`rQf|;QQ~-(bciWqdg?Nl^W3m%t2Y%)5toc=uJ@Rfsw&r&RKrpIg zD;AG_PzgsxeU;X$^S)1AdZhYAEji%mkTFTJNWL^&e^*$tO2lEerpZ6%%j#TlpPRl ziAschGc}c#{H)_cPDB(sWzEFrP$Su3r<`re$;Ji=Kz_6wEBCI@)LhbuzFH)(l#ZN| zyu2QfwKCl$Q&(fv1jtcTF}ee+Fd~4axm3;yDP`OC&9%2#<7LZ~#VzTCUp@BxfsbN;=pHlF^JdQTofofpD z8J}y^O!RN5g`jqfMnd#PAc{fP6*Q$1w={XJVs@y=7_Yu>A){05SI=B8KsL^HjKAz2RTfwQfjR8TFf||j04Cl+_8+v@R<50P8@!j2>JA<)4#I0Mi!H-fGoZnmrv+ z0)fn@0t&iuP=GwEDh)$d?7C{tpXU7(dWk$i@F$(#Sc!&TXuCC9tW5CzhVlTllk?OL zAh}s52*#vSa>o;(dIcsP6%CIVdQKV2z}`=_HvXhL0C<-r-9Uq_RR+pl?5B36J6DZ` zWa){&Ws4`LLiEN&<@~~e)YQp7@({0{Q<17ljedZ)qoc#v#GEB>q7t7{Wg>H(=%3d> z(zp5fo*r&BnO7(md-&2(`hvjg&u5JjjX%!Rc#5mEVY+S_#vKTI!97v-$$8&yti8ph z7=59kF~m~@98>(=7Waf?^pKR)Zv07b#}zc%a(Bynhx~WyRVK#5Zeij9_xlDQKU-^y z-hVG5PA^LSiF$@Jgix|^zPK719#Yf-8)Ckm6S8n16i2SYdJ4&H{SIant-*aRR#oP+ zD6keP>SZCJsxvuVie2m>D)&HIg>zZajD+ftQX6>#i(L;Veup5XF@-Ad~K&)+EeQxN@-0bI`Yb z(Gq5IlEB04?STRX4GX=e1DqCfT>;7Jc<`2|0p2`T{1S1i8gtr0N2tQ86+SJ2eVn6Z z&P3$gZ>o?Nn}bOEh`1e|8F`mjgHx)7Sdm2hAEV990(Fa9i6cApJUw}`!ZmmS_-}j* z>}t&)a#8SdDUf8Z;!b_lJm?g|(TjI}D`KI4qf|kpugt~<6t=?-br=A>n!7viBPOwX zvMn=6dclRHt)rNzHl;Y$y#!kc2A3@>%1XC%%w$;DMa7CbEs z%J7PGJ3u*l+F$jQXUzp&A?bVlmKoLQGCA>qVw%M)?SYY${stp&XufYT>V9u)ORV?j zBRmi|KbH<6I9{=07irH)*jR(y-X^(!d^R9D2}h?Tha2tDad29I%W{KDMC-a2;_y?e z6p$m=b4~jy=g*$%75|m{C)+fzHp|(896h7n6dybBd=dhn(o-yD!T1KsjG&92PIw4) z%0h$5%v=%%`E!n~yPslU1xKcix-ZdIsAYVQo~f3hgeDEZN~wLfDBD6XYN~#oJhv1a zEi*-ic3e}#! zYW3XavyU3E)$q-8?AV`lZX;?wsafwZYTywlMe)*yg}H>+%-065@yO<0f!Kew#d~ZQ zej}e4YSr10p^c-CmlpC#(Xu%n6-eRz>B|&@hO}Jf-fg-An2EE%s?BoGh4<=~cw3Cw zl+#+mg~ki_EFQal6I|FzRD4^l`cd>8VFcGRz?6ho-cqLnX)MANaiDCYqP3{OjhND?M zxASRfQ9b+y5j<#?tzbEc!WLC;nxJ0&Gkph+`iGTgnt$oaE%YXooF+|0LjFleRE%AS!XX1u7)Xc z4c=9YVS>kqthG^y+1c3=2Zi|wO(te}<|owVA^OWL5vScYxXjj|H{^U(@Y{nR8>$!Q zU(|ESwcZzrDw}xr=*8iR?)L5TA_cr$CS7fr4?X#kDujB>%oDvr>w`?qhj!S<5jsg2 zoLo5h|L%7Pn8|sP_y$Z$Xsf6y8r`_c93w>??|M?=u5`*}Y>V#`C?Xo*@n<@B##{Ui z$99>b;iEw|yRN2MJ-5Sj*pAHj#49WamKXGAI@u{2!4Y0No8+P(3SOO}#=ai4pPHW8 zAG>Z;dUT4pi;KbDnG(9-RY$L{JI!@Y$?V_~N^g=+dNh%@rKFErbMLS{seq?^kT?&A zZn*|wfE_Yjp}oAttAUSk^Eztg`m6)+AfA%WW||oBpxx*0AQOYo00aFuZ%R=Dyv^d{ z|5^apv;#Xv;VF9A2>lUSrdhe66ZUE(vTiSEU+8qD%q4Ok;dnMvjsvO0UvsPtH96Ij z&&BA5xLckX-nFl5ufL_?WBz?70CRPu9-O@1>%rU-%Uw7sRyx}JUI=@p-~)pcRh6I8 z2-WiWOj0)pi%A@GBgs{SW_$jW)J3PzFiNedOOY=hTTVl3(=(HHiuj^AWDEXwu)xOb z!^HCdZ7Pk?&l(jX;ds?C<}GM12eui@Z^ye%MoU8guO&{CSjVVXOje1jQt}mK{Z~_F z9OfLyA(KsdY&0oaOP*Qx<3qod8@WU?lU~eD_BT?fs6d+SYvj9-`qoMAc}i<$18A}l z>sI;C>1y9KK00sZY@(!WY@kjvcTXd8J#YZ^nd-*)V3?HxoEF_Io(n7>uHEd7Mgp=> zBU25ON~)9^A{)$ox0mlSgu(`5Qp_O)GsKU|6|FRo=q(iaiC&}}U{Byp1exOFo{p4_ z2>1Z0;Hi#36}%*pX!7q`^9^Ma9T;o)02zSJGx8VPUb}@VA+(V>FRd?)bBld%Y@^O69*Sx&0FA$dUd|%>bqC(!@HN zH*%QUI6X97fm_Xt)i0wkq=Gn0MBuItsVsYtbyn5O+aTDmzYi-B^+T8v44>eMKt=@v z48MYJuub;$*HXg=J#3KPcki~6yIC~{%?$f5@j#FcoI|%U4>vrSD0-PFb!DB}NkPYQ zB592;?fE$`DzoWNMV{QYWTrMaSzmEpPsS61?mnzxOr32meXx~$76sKi_G7&fV4b`e zse@VVip_S0q|<|(rBt)jqck~+H>*hFDX|USYB_aYelnYRD6u>+VxkceVM(@B*S0nH z$Sf$!29lRUZM@loaN78cFj!DOkPzyaEqLIH}d&=JH=91%`PyZD)5eD>s}NaEf)T+ zn|>lq2!7d9AUsK&{;PwiEcKqk=AM-}e|mR!_wH9LtB{5QGCZ2*&oei?TexA>1Oa zaot{yyj!eqKQCB0Lp8N4{dz;-(stdCo2k_GRl2zvw9dN!$ zOe@eni5|0;K)xt!9d%kI!~ER?ec_$1in|;&g;f#sF^3ZMTk!MF?o}1l{GoOH@5FB12Km8NkKkxGNg$R7} zUgPCc^L}X-^z@s=SytOw-QL{U&DaS5XJKb=3*Z2nIspK7z%TaBhX^f_aB$Rc?`5Pl z+*9@z+!6>jt{;yt2UE(uhA;GuwwvVD^Nrv;IY|k^%Z&MH;$*VavfhVr#8H)X|MA{` zr@R*;fy-ue`0eFbltJ8nL++4xi`4K;_dz@7M?|R-J+J)~7Kk`sg8_7Nvl%?+dEh!n zjJ;Ys6z#I0tv2Bw@`Vu=p&;DH*cGzM#)Gn)dERGEaK2E2N?nYS(Pp6Fo zEgjO~RHE*!qJ{?jZM_%xXh@doKYK>teOOKbtVGg2CSv+2f`s zfdsgJThxAUwe6iRRhp@(VBuzzWyzXuc`;k;D7&|0(7Uw;&j{LoTRB2)ap-RS)jifs z-OSluTXG-fS%kHn zR#TD`=0|+c{M<()fiCh#mqr4}+lO((9LcDy@v%)z5qfkHU1bKa_?bpIzoVU<9ecJl z1EO4Lh>D3(k1N!K}%A9tt_Nkdftc_I^BuS^9IYk+CFz4s9KsU z8UJ^$^6KJ{j%{cSR3Hu@;PuhPZM3`uawsCHVJ@L4ISj3cNkv$i-HZOg+wEpWHKbIs9PGBB;lv-Ha-uQ+5O z!*stGk5S&zL$v4qT(mfi|NDuml$mASPJT-6`7k_GIT3`px~O$f#Lln{0s$&X*8f~NPv!~`-6c&U-+#X zlZJi>%&sZRar)S?Ya~MNs_o-9cqsvYIa3V4Ml8Aak??96adE@O$Oqb}R!8!8k(6z7 z_K;~~)BRx+M|7e2z$l5auLoiCY%vy#+wHNDDRO=V-ya{&A%o1J%gEJ+$a?CR-nR?N zhzJ3jA>;Zn?;J-z&tKrz?eXgSSF?)SY{ zyk}ov6=XN;h4C4}y4;A=0f6$$F7-P1QtW;U$H~J-Wo|A|&<4)w7)u`W@~T{k@gJ(G z7I%X9;V%oo;J-ZH8?GiLFU-$3w=qr_R(Vw;`r>t!^;I*g^pcX_zQE=CFs1QVEPEK0F&=>~^IC600;D?{`^a}iOtwvF42f02fqK;zMcH-pyth6$${Xq@L zHi3ITBOCX)QNP*D*82B<`SzOob>N+n-azK>S-I<+DkG$=aY>o%*moYX0+V?>>^*X> z(=<$UpmZ*UPhBu2AMU|~>fHnZQ+&b&2dlL-%FrpfQu26%{Wmh~xud|?U$HomGC@q@ z7)D}}cX5jR#z~3sUln8g*pwc1*Q}}XW!pJTTc25YV*Q+a=NX8S3{^#?l(VT z%@yENIE|Nn?VYV_JUzhU4WMfMV^DY1tQqOBC>C4&Egu$nXVxAy_V3MUZQ!J)KBrdwOP&Z-t&!d-)Vmk{zL^_xW+DTpFR^` zH&c4JfzEdfUltykki6CP^Gy;MntkZ#zAquQ%6a>W z5?n9zFl4A_A}EyNgJNAzF=TvupxWJxVgk220<42G>f%lFI^^)CiPvUR0LQeHxFmuZ z?DZgrlzQLdXQJ+h@92p3z_J&iFV}ZZ5l*+~`xiD|9}5QhvPmT!o9O zqbj8*OrOh(2M(#(!pEPiqP5qh&RlkeZsXO)SsQBqpsBg`(E9ZePsp9D|HuX#n=8=; zrM9~*+=6$f)c6b&ju@|4mRhg>R+YmH%$E#?W?Bh1K7^YjYggob$3QOytpjqIF5`={ z+OLjEQilyoZc>cpz{9_zJa7z$cd;dg=1w(NAt(5Jgheci0^LT_h3xa+WB@#53afb# ztO%s)TZzZ}^j`>ALyY`;)Sg#FrJ{BY9rq%QEeP&|w@Vf{m1V2R3tvJZfT|rg`TzKk zgJg~MW;%RXwqVz_oAwN|D}le1&dsmtTOUZ2H{Daf?vn?|nvb0fQb5*_Q|2gcK;(bD zm8F`LI{!zF&bkKF)~ea_!io=3NXzeW)3i!vA(2P2dF73r&UBRF;)zxS=htW<`*k;L zUsdS1{c<&*$Ky~4L zX@z8c(X#K*T)p8AMLc22WEBsZ%Ixx}1nk!O1x*Ik!P_1@ctBSTXN5)o!+R3&}jW10PT1;|zR> zLLHAt@7T!&3F2{sr()B@e5F!P`2Jj5+CNkbh+JXMOSIE*h{YsrpeUN($NzAQJPTuw zYvGBh3Z#5%?7%IDp|nYM8je)@#VUcgz) z_pp3Cq*7d1S$R?4-`}g*^Q|7EZDtAOGy@&m+(E^J%N*-fm(bA+YOnxsaWkfTlsq~X(t2Q=mLd?ieX4hJ0Ws?JP3^6` zGzH^)I2Ra)X{{XcaEl;clLPlPBJ!ld9mHmH+U~aEHm$}PdgLak7|X9u1r6jQi$#2C z{gwI^HtmcJqK1GMy$-Y*KhTfdq(TN%cS2-Q?Uyc87nfdj5cSgzomAYUS~gAqUXi7_ z`va`eMe_FuHnr+-6iO~D9GE$S#}6;vj-kfqAs$2$y>(DuC1x%XYW`%^C9Iej^-H6H zw3c8p13e{86-Gjyd}ve8Kxo9KP?I-?q&K&vA^XbLj`|mU)T@~Za@0k~H~zX_oL$JW z|0sr;n>mK=Fu5~%audc6t|P;XP%<`?Xwk(fs{9@|=pk+Hkym zBP8U;zqQj-aG4)%LQOZ_+E^DE!@b)GK}|^h6M#o@j;i2Dr9H2-`h9COK7_(#!B~_c;S~pB=)`>8 z!(~o_h6qo%!cWD;+BEdEp_O@Eh{YpDXz^RG=<_j?Fql6pc*9A|{@AZOYoz@|1!iBmu&9bIG z*Be^O5mEB=Nl!Mv+SPn*jWBp}=}4ku%l2}!_kJ$BKZJ>w7d<`5KK@e_1xBgk@Z;4V z6C6{gcfg%yI5Ll0KSk8+Ko$cr@FYeC84a&u%W9Y9lG@HUe9rU{Lo`?OtmX7zEj3(} zJ00iU=(Z5qPiHRcm3!AFt*(CL z<$J43ZhhvPl}wRa`TO@wnJWevYx8Oul^WEyE3On1;0_83!%n#=9zEIAUps%n1Pn;!rrwJOJ;Bd*!@ zU})bCPZI=Q<)w&FlQ5%SB=w%VQGms9bXwo~HGn6pGwQqiD-m@PEq~5%SMYp3gaMMi z7~(nWv9@>z^blbfJ5eP!w_J%VNHxa07+^qOm@>IvcV(0Nn!B;1r>7)Be5F5=YUGx) z>E7yvrgmrd{(DSrK)1I}1i|?m{hXU(sBPv}VWgzjo0sXB;HfERuQS&9MHZ+Fy?&X4 zBp>%}DwPNWS-a7lrOETMvwDwX5n9eJzE;L_3fz6Dc&p1o5Ma;Y#LGohF$Exobtv1#1Q3Qe|Vh) zFOF?$-(pmlk+-A+O0_#a$g+3XI}aRZ^byXbyn6wCbR*F0m17=~JN6oPe}oYMUlU(F z_%+rt2lOcw3?BSzpGu19IHF!jcGvugMayHcLYzTYqyoukh#kjxpVO^o&XnXE%i!AgkyI-Oyz3YW$d3?BLS{IC`xUuwN&N)9f~*| zAb=9i!=CWS5n&s%ltW=@X{oIVgy%xgKCiwsb2&`k%)>;+ctQ#Viwy&J8Mi8+UpX4N z{#!_D?qplrWP#R41I>3(&V^D6l({l>V;*!eL_(`CvV?%tBW``kt zu*)4u1jRT`8a}qri+!=ZL6-jjK>oZ*K-Vs`C`&QkZaN|ya@cb$LjTsVPp{5wHsw>Y zh=sAhU2}uQQ1&;UZsO8EmwJK;W^nu@uaM9|u*6xd@j7)PLe7NT<^MpvJLXDJVdBtt zPxhvaooue$(Z-Nh+gL;!Fp^+KNc}S;&?B!GtW}Yl(%Y*UwaahG(Ys3A|L+XW+?U%;NS?+qwVOJOu4|&ixY6#$P-MYipx_)QV9{ zV68dddBULbyv3}sgF+{f#bbe4SiV|iaeQv9slvM@X2k_aG|4Mba#q%ck3~Vh7 zM?*p(4kiym7kSB?g`c!sni6nm8k3W)5Jw^gxF4)fNZB_8$z3 zlW!Cx;&~<{b9wp%NSZWWkMQLIW(&}cF=gLf<|ObIjGxe)fcDxN4ugV#svXbj6xrm#EHTJc_@)1UsJ!HtG2bA ztLE^34HqV7%k7ALm(P-|V&|3jI!~*?R=j4jtE#@xy)T+U>AuCQ_V%E`ksPJR@%bY0 z#_y6DbR>5$rE8DG_EM8m7!+-&yRb->vUqn5@mu5sas$AQm*?2WE?w2dz-_$Hx;X%6 zQI!#Z1)3_9wmVE_NJjtHU`6fQlTDGISv4GcrUlxHR54Tu;C3V_i9%hDKPR$4cdqF( zYgKT-Rp-E(js^KnylHqT&HPb$-s$+yKyHGo0^2v=;A3niOmtOP8_KU=4?!X%A{^Jc z;h$jWD~R7iM_JE97~gP0r@h~;7+j7@Q}X=09wWIAMhv4eEe8kY*C(RK#~XMXSK!hO zUiVzje{>YBdGH`dQfZKc~#Oy3;On zRj8r{BAQMk{oy|!4{R5i_5bp6-`vg>B+L1B%_Y?e;_~axXD#Iug6=)m7o4@6$@ZqE}xY%R-L(qeoL_#1iHY36I-i?A2_XSc)%< zPy)7;Cf&q+_c0p@B*P3Jj-h7*e6KiCA@z5aQ@KS4?;g%_rTq033SNM8arCeFAHB_y zI)>}w6mQIN^cxaBk<1iD9@HeXDb{+K1v>t93VGwq)!ar$Kas%&1*~M=#cXUB+}#Su zU5#cVBa7@HA^<73-Y!s#kyK?bFYL{+7*j^YJYrh z-0xQGJpP#7-kraz8D?t*)hmqB3+1jeh8y4?)UXAW@qZ=z4s4Sk5dkt`$VH|3uNMF zt?1sBg<-Gb5k%H5TTELk&&poU-w zxIVsTF=^n&T@oQuA`m^|P?a&;`+8c+{Y&?okW3s(Puv>)>C}>gFRQ*{)Pkab{#V_S zLhvc%p9!cd#T>6U;_a))aTZFyh0Gg=E!8mn@J&}1tIvt>+&FbVZVvR=ADGktTx=d0 z8n#+?oRT6LCW}GZaP&n$atA&kB#Qks*3=eyK9Z@y6)ROOCj-#0g`nEa2*dH*YwdgN zkAC58Ibkhlv9tSB9i-oNlkI(^0qgrkP zFvwsa&S3&pXDpoL<15X8ky~#L9`5bowjKtOuXf>$r+ltnVXNH!?CKh^>pf06EdXuD zZI&H_`(@E#~fxz-WpYan&^t6e-j-Yv-keqXl}?h|tEPl^c-(^-vYGgkTg zdI72r=oZ2OTm8AUdqS%4wju|!*1|bWZ*2|I^H^w;!-I2on;kISiu^{sZR&1P|Q>${n-R6ijqO~%$=$6cTro}xW&&id=uCsWOR3vP?; zrm1$TgG~WHA3KTIS*1eS$s<>+(Fa<6JgW>|`0R13o-2Q*v8NZOR;Exb3q@>fFNvSK5rd{qQ~k;C@wLJx%lnncS83 z&B@UGos{rKq|nJ+2w#fN+@Xz_1rzNTR*n80K()=oO=> zqr$z^^ZBk))NJgPqDqDsv^0MMirFonHyo}tLoZj8z*QMG*KRgj?xcRVyRXMgC|8V- zZ6h0glKCZuSKUw?JoKz7k@0-3@a($Y85Q%$Fe8UODImo@T0SqY5e=ePTJI-6k%Sj> zMq(;X2j^3yO5Mn0<~T2=C(MWh!K+s%16-%=m4aO3Z~v_Yfb|UzoA>`};d2B^pYW#a^+i? zIa<7rncL2<`8}~Xkmxg#GnrrjN7%Nf=j7CmK`xZ!ddQifUi0SUEmGJ_3CDQL%dtoxvRnxHyETu=K!Feeuo% zVMy1Tm|_@-DLm6lbcB&AsFbi(L9l&>#QLqKjDDQ`pZ0`xC!{q0{Dd|aOjO9S$4C~p z57XdpfNRZvHW85A9}>>fH!p13deYn1mti|stsN)J{OU#P;pKRfm)qTgS)!%-+_Ag3 zz@|BT1#MsQ6fE2edC2L+3>nV~#+p*|Dj!G+tN zDAw#!c;H2QOL(P2d4XRNw5n6Bt)+}Z8eGTxp)d%(;0{B-<~ z?@G=sOy;JBOF)2L`^2boyXlznKPW32A&IH)H0xzIx)tUZ#rsXjJkCQZ+^a71q`b6**n2MXg#>{H-`t() zqQcxv$`+Las4p}#1> z@2by+ntD=-SgqF68MWqOQc~deF^o~Mu}CB97qeB?F>h;=kx=xx5=FK=-3?9>U>@$A z+7C)&jm)D7@q6EGKEmqY9Q%bd@N|!;iNLo;3+}ck9|$g^)deWic30pTtCMt8CA--S z3)A@zIxJS572N4rLzvHbNpP#l201*WKrqkRxI7HKv#7h3;&{tL+RVV z3rtCDa_+DG2*})5O_$6pEbvAr>xRIm$dbRl+LZNeg(TN!>CUV7mvdLAJr3?|4B}@B z$S8&qUzG;h!3Q`6L2sDBHVwaG8BlxA5gZjXKP^LsXucqA@tPZs*(LR98ETX_Iqa#!?@!CtI^l>DaJ-RN6!(GnrQ+H=;6Va~Nw zYtF1bhfqLvNY(Bl51dNSH;1ojOpEREJq;>gvYVSYf3)AeCl{ThEH5QT=V}HfMcXtp zE?p@a8p~fQHS2xGYMrYgZ^Abpb(#ZWw&AM?`$G-=QV$jyjAsP9PqpivNdl>SuUs1f z|A~0yfz@TYJbmdY82A8yl09)^=hd6Pqh2{d-$dcg!%!|yrWZrB$o_aepVh(Ew#3pM7wSS@W+%aOc6 z13@U?J|u06-;{OrDtbjDYg6=aGkP;%OYV%^7gqEETX%eM-Zsv8*vuL+QjI0Et9R$P z!!L+^-w|kcU?z9tMR_GUx#U}amAcD{m?8`M8I;~iV^r9$ zMCD2@H==q)5wI~NjYp+K7%NzKx8GHXROS`a&u63Sh-EcjF_~|T(6H;({)}bQ8q2ZO z6)ZGVjii(y7Ip5O;v)R#d~@yptiIzvJ9&)?&7h0FhANPB`{x=f$Fr*20%6%xRAIBuaia6Dl&(e@4a@M}wg(DiXdC71~DuBb6CGov{lYBn0ah{F|LDVC0klfHT zG8Z8o>8aJB<(Q}AYp}w@{kq?_#|gZ%a_9&1lP~dV@}8qm?>Zaij&ZhN3%5-D`X2zD z^YWMKhp1KqK^)P?(roAC9G^5QgP{+%hK#CdgRTAu@K#V4@tt0}ng;{ZPo1gM(kQL8 zriX?nuqNw>&xm8>Cf1q6Pdty|+Pu0wgk30t=}315A95d}-rv9N8;NU?K%pFqf`R+j zf@nngmJPqawcM?}7k7RH-EC~3g~dvPgZ)-Qg7rzA`>J>^SD;V>r3X6I(`n3-kH*vq z(3mLv+w!I6n1riqjf1NWvQ9WAjb7hGN$S~lq4f{$)3PasPF#`q_a7-E{96_tQvA;A z!g{Cg^pfYz4{Pp~6M27>K`38niW-3y;2Sm8Qq8MV2bTo+lH^?aJ~#VrTc&C2`-i?& zi5yMwI7U5_M<2bPmnwaD2&`K>$^Fw!W8ceFcY?iUNwhPdh7wS>yT~+sI0YnG*z-WR z+StfF9`T7hfeNuwk3@Am-U%^=i~KmIQU&xgl&n^Yd{r!#3iRQ1HIuI@qgmjy%rx?i z@z$EP1#%cfm<)v=A1mzxs# zEU;3ok|Zie`eWHh_`l#(wA^H_`1g7rG@UG&JzTJ=Sn?(7mUzY(Yw2}MUke(HHr%z4 zHE;genATsCY9K2%H`8L_7Qtbl>4Aj%HgAG@&-f? zvOtgvT+nGkeUalyGEU|39oo!7i#5G%OXAo~C2abESiLN0J5s9eY-TqYC$oo#$Z3=Y zUea&x(fzLCyc279f$^EJ`_xL&h=Z+eI6d`q`bZfC`*Lc{l2JS4^Ml`= zq0hMhD=S-~xCbvBw)hbo$qF|$8;2P52$!zzbXcrzzk2Sg8RN)F@(lE}21PL#}`? z)zmYso$b{--#~IyVr8E$3sG@ipke$ylLqy=j_)M(C)ssXVbWq0YFB^PwQB(xKHfmr zH#e)@3o*$AKYaXH-o;2EC3;R;u3e4UZ8Ts12@gIDW<2MrZ&`*it!pnomV$~Ou^Ovo zP_*#ZDB$AGP*$@DYgc5Zwf!~$P!j1snL@8KS|+C4F4Ndwze>$Uzb&u|YtONE{hq?+ zH{g{|5H-(E>k>dFVOam9}4xu*b9m4Hq_+>bUBAq5;81KaO>=Ywe|)%rDDpB!vS zKo(Jhl!#%+9}kUG-BJI;j+v&b%S}v6_cZvh){NN^DDw*IJk{Q`YEOLcN#CAtTTt4W zv7*do1aeQN3QraznoimoC$0x;HrXitw-EiJ$u?>EqAClWb1Rz|f9`>a$OP#jRKn!L zC4`Az1Y`(;6Nrz|x`OUi%AwW>L8z*2|7AX|kDs7xZeom&YC0;l5~ys*{@Cy+4Kg!#MKz5x1j(2qP9R0&gNE-nTM4j(OLy?-(2 z-~O5?C=D*VZRG&PC;bGQIk1z^D8`Y-unScyu_lg8m_f3qnC20pHw7bYladPi0{Wu! zd^Gpr(Y4FJ&`5Y*E*{7Hm&mbvPSpJVL?Z@u@qi6&Pgl# zjN}bon??&2#Owpb`<*F+(e!oA>9J2}X!bA%2Z!nQn5=u6@f}Uz{>9;c37!j#CyjXF zp4M}By;Q!{aQ&30+7I&B=V5YinP^Cc{!zCMItAYRSDAi_E+nELray_y1O zSi@-&$$ns8&wYy?M{Qo}-peKqZ%HaiFm{jQOHLBF0k28li`#oW$F9;;(QCrs;`Jt| zB)#Yp+<36QG+FU1d{G>wVRFn8N=v>7+>D8YTOI}-K=#nHGaR2w($>a0ATw7=x*;>pD3Dj+-uL)z8Bi=vF zEOmANFl+j-9x;3B-CV?BNNjuzRQE73__W~j#-s9ZEU=AEuFNeSBW3o!?$s+{GbD}w zvEf8LTAu#p4?Z|h=e;lJ z#NbLinoGBsxl&iPwI}%?zQBADtZN>msUdyA9U)dPMZ);*Q{U$}?^BWzi5nKf12@)O z%86L#0otb9j60_jEcegPM7&l5>Jx>U{d8Dt^k{r+^O7;;k>B_7x!FYoN!Pm_sKoB%8K1l68m{k|%A&vC!r zm&h&W+>yRV?Bfj&5vE=_xVk48HB!yNWi9uZStsnYY*L$5P)Vc38&(W>C(nqD^_Yw7 z)?%}jGnmKe+~liI-SDAj-_rK@Ho3dcAl93aGLd5?V3^c+i>dm=*Hr01X;YD#Bbrwl z-ktaAOY{zBKT_&c83|vTVA?DB{G3@BUcQvFR_%)~IqeUdDa=1=jhD`iVsf0l!ny%z zJWGVwkscAkbVSFj%kFfgC4RN(X?y5+Z*MPsT99#=8~}h6XA1(X)EyM3RX>ZYUBRbB z(0K(S!b3nJhyx4DZdf5}06uW+FqG-~B4&eps)7siokHQ}g2j@VS{pHA+Gw&3>$FDH z`vA-YX0mur3-!$hayKQSu>~~Ep~S917?n|$7pD}}FWI$rv*fb~5Q*0wH}AMDt#ymZ zlZ!pkb7dM{Y+<^0T3QEr6a?*!M>@B%8Mck_24daM8rk1{hZ&@X^PLc@P$fjF>%U2q z3bplidS>WM*zP3a4aQneBoiB3C==N8#auND>ueBGig$P4Uel!{J(RN0FZ@}wD>XxT zQDQLV3z7X_`=xUNO96l`T^yUYm*NSAjd5r-Tr=m9b+`)oG)<*U5)GA1aL#{3%s*mB zjfYD%)#E>9{|?@5wj6Fwl7K5R9;GX7cdpTHxYMc6Gk|1xr!6bj47)*L+j1?wGlMra zSOv8ATd~MdRp>MFUhk?^JJ>j~lc=AZ>Tl~n9L)u#XNzMd=Acg$*5;0EiV?NLYBNWJ zSA1>kk=Z@ zQ6`^ryZK0cHKu#VTj(dpo1_S>;M;E2B6If<*`?#hC3j}g=MLTGgOz%}Wx@T1sE@F3 zTH7txmM*>SdxdIP5;9VdI~0IqeKZ@MVLjarmW)R5^c~n$UF^DPeoG>0f(U8=2;Jj7 zCy{?o>Onh38hN}Bt_NcjE~=F|V8>@H6)Yz%{?`B4tIPP;8dX?J(KH0ev&KCvfi zA#Pz}*o=Dw+)r#%tN#d8#yLhoLCzx6u18IoZ)g1GD=iu!5hImyi+;hy4jUk%+N8Qv zwa}@uH0MSisFu$m5R~j$xWZMHxToAjyV$UZVLVN)P%&z`aOp|?CXHc^nhY&2 z`P|C7;oUdGlrwbih{!Me3jCx+L%b77|MM1e#RQ3QAfkI#9I7>u@Lnk&+(VffAMc6l&+V)lSv9#>zYWs7Lge_PvM<>8+Bnl) zgzQU4n`yu}69hR^!|Wzq+@&eCCw){u@py-6PBH zw+G1>7^JFzu4l=SdY!erJd*L2#I=iHj(<7`m#*6PqZu7r9A|cd@MM>@&dHF4cN4F| z?Y+uIDWzpN#4R-8bjIn${%jP1{D*pHj!DE``2t1HFfK2!n^fL!uV!$07uCt;oA`)8 ztlT1OcE5z52n8(<0R$8m8E;jXoD%zBhd$D6Ke4|1@Ca&5J%Yw)DQqr%y6=YMk{xH=i}w>cIdv7?yC+dxXG(v{(Ckd)DW_eXjjICZY-`{hCO>I8PN>OM zooEuah2a!leTz@KdI85I5qY7H0-PYpn$A^x7b7stsM)_$$9HuXt{~ouR+W8Ni!V~C zm|ko(O=PjrQ*!j)@&%iQ{Nr0j$IjkHvueZ7LRh5GPz#I3zU0tbr%lN$VaGQlCBxnz zCJQFeH&^Z$S1-U+Q&^=PIX5r3jxThJ+Z?VgFSst&I^nS`key4BUUgu%Z!1`Clxd=0 zi{_a#$_nT|t~ZlQNSYq~JJ<63TC0@ymwdXP-h}!@69IwDix}ef_s;+8b9JbE&eVXR zm+;&c!{ljR2O>{w7K`K5M|%xPlOW+Xg2QK0`EA(ZD+b6^J`p@#@lj9IaUk^*=9wvS z_sTQ4Vs~p|H%{0`0B;|N|d^sO6#B?Ev;QJfn8*FkJU*F^j&#`j|j}0s9>YROZ zPI-PvdxSnI`C$t*;`-KkaN1GKOz&BpsZ2J*-26t)hXiMf{`^_)A~O8#cDShLIs-5R z00e{$^z;VzW-2@`W-Jo9jbChUZ%cill|s((M6mA1_{CweIS|@Aa|k`tGi$=}foGm8*p_M6 zTB4*_!X7r9nkekJij}fDtbTt#cLz)SA>$)*M(jb$6lvGt`lar9JZ+IlRE*YZ6@#|o zv}Z5SOKD&5k9N>D;Kvd?mGAVb_F;3d+2x}=-~OlFAztB)_UlWaJj>*8h@wK@P~YO@ zIL-_s=#pgW3z#S1>8w?1H>5y2i3{iLOGUDxG+Sw{=&C{)mtB?lY$-nvpp-r)95;AU zJ}se~f<~48SAT+Ev^Hsp1xo?P(jC$ z^cS2`8J8E>IQ4)*uS;7=gU`~T9nJ=*hLV?5IX=}?i zbYG}Y}eRnxv;oMTcRs_f_xL)24bibRtyfXj9Kq zRs2}S_q@Q+@n$=B?eez9x6PbWj5YF!e!YFBqGitd3>EQqv>Z6pc%eLvZtxVR;~o~n z@1DApbeSgQdx$%Y+Iu({AbU{(S>sg>-UzF;*B`=oyi8xr1;pi|AL!+rkWtoey?79i z3N*%7g_ZZEKGpe~U+6+kN(vZxPJfBjp3c*l-KWp1Bp0p_Ib4F~tf!v!ObiSVxRV>s z%%qikSjT@CNh)h+X9qg{r%=!`Ra1aIQPM}Wr11pJ?{1CiDTAjGT2jD5a4;)K@pVQ~ zs7w_l65+;z*TNE)iepZu=Psg(p1KzITiX3S#8JJ3HN5Q#fdX-AaJHEDwfmeS7$ubt`|U zk0KX}Ta9dn)12AsUlfmevh-N+(Yw><+WpQ1=D!Zr3gG@7DitBBPUXee+;J676wLQ^ zcr-Al1Y-?U>^lu3*~)gf$yH5st4C~Gy6a6Rz0j$uanGO_vN$sxSyfG3aKJr8nog&u zu$bCD-@&A8W1h;c^o*R+o;hdIJYL*z9ks0yp#lm4NS%yNr{AD9qMO8R>4~a z{TL(im~-us>V9UE58TntDjHv=e)xmDK8(v^6TdVRiYk*j+_0mI`%F0R@nGlwtq2ZG)3~2CrU_6M}((@k>Qnqf@*3lmM6CzPW ziVP<=?Iu9Ut;};UHJudPk~YxLu|8g??L(p8%Dh;(tM|1FUtQ$!)ChSfIMcOcMXAty zAY5PSbm8jZdja~@ovhGVVqlJrT2+sO0h{gn^+nmcqx%Lu=zV*QzJhUaMTX5Olz6dR zm@|$=Z;>~q#FzNT+p27g(+0PlfrTotNmo5t0`%d#7Q+5vvA#v%Bb&Y2V~yHf-5s$# z(nJ^IzG&cuVdvgPl=o<<@VsPC^$#iDvLz2Zm}*vmmU$O}`)$LcT8|2R6Ght8&(jK^ zfF}z0{PFvK*DSZgypck7s`=!Z4h9J^BuoV1kCo~A#f+aYuD<`nTK&g9(b%K=&@0!6 zK<}NpUnPdD?>9OaOr(eb)7am;-b7CG1sLwe?=z2jqnqEwY=JLm|C;68h)ye5M{7R|~u;X`3 zO2EL{nnP~M?*>m5SpSZW&3g=^&iR!}jtqnywJasHlIF5sGhX#P+1W+g9~yWk@(ttr z<6dLh+a2z&`QF?9TMN*af|(oSR|fw+YLs|L0=3XECZFN#L!Ff**yovS|D)Z@V|tdO z5LY!dwG>g8kf)3!OpJJ5I76hv&}qfVjCZlCC~s$@bB4_Fu9hxsgqdRKUfD-4LdpC} zk@Gs?BgW@$Z(~`vL-(rh5|3GL=cMl(7u+E}+d^1C&aROBa37^M>aWA7k2wIERc_eo zVTF9ga8& zQ*G=aM{G>p8RXNEm3;a@YKy00$&H2^zr%`=aNU8WjX@Y1rTHJn8hOnjmU9#PG@xj! z#eygA-tNX;UG&*+&i#KB@qZhBkoA9uMJeT*Qd!CW^g6bFx&R71V^L_1N0o_QbE1-t zkZynd%py?f)T%3?@_-N`FxVi~1QN=&F@ly2&|3l=2IFw8EFEQ*BIsrk6qRz}nJ<<6 zKONLd34QLq`GJN;DkKzUJROEUaUc}d=b~U5cAdpom6}VVvxgQ_Vi%z*?onMCoK>O~b}%Y}>YN+iu+0YHZuK zZQFKocGBhrBWg#2>qSIWu%W(1}8>xUsi)*Fmzx zoaHiC)(c>0ov9|{CWbo0!HAu(_iw6%EMyFb>>ploCD*#^C;EsL$#9(&ftImI(G)%Wl;yCPUy6VSCT3nsKL4)6J=~|vrLCcnMa zk*;8jI6EGSqvp@%r}BaHdp@T#(B>X4W@FZIyB;3J>cc_rm*hB;!^>LrY%OlNw4Aw4 zja*u;C*%$HUCjRenR9C*IXDwj@}vX2nXd6rkVV(DLV{|Pjc9iP|JI}sEt-on(VMY< z5N>a6-J91DGxK5O@UIMM&=;=e!MC=zFRtNxhMQp+=NJhId+aWGNuTdeiEi9UB~t21 zk7Pf8ozD6Q2Z2Y7J`&6EkWyo5Hdod&&HytoFTa&)KL-1-k-zvbnlc;`K8?}5fNY+` z<|)AzpGht_U9V{{z0fP3HtipXge9GIKQCVWlx&qCuDu{d7HrX|XwmE=_~s>8;}1I+2}njU^RQJjq`jflE;bm)jVT8*>@d z6zpn`(Z+$JawVkrFLi6Mhp)TOH$~=9Jfp3C=(lqied8%z1Gn0nYkz!vDDC|HGVLtJ zgy>*Kl79u^ajJ`iY;iaCS<$!{OBF5d8M#(PH9+IP+-=KuAp)0J!#gL*;MH_$WGJb9 za<}htl?iBwq|kn587V7=U^cKHK8_t79hrQ=Z>h&C^Yx}UoaH*{l0-9^1oBT_mzQz0 zSTA8P|B+F}IasqG(XwhuF+>9B(vYjisrVHcAPrhhfJE^Spgae#mNMGSPUf9l6^3)S zu*XU<(b0m^^SGo?#zld7g$05HMMb*wn6#MTnWQCZ)gb`zqFlA(YQL;nOx`H2So^2W zf!FK5vw)w~B=&{+oG^)^s3;lmtXbxKN*<}Pcbxvi5kBcn4lVO(@=rew)QS(stYE-pxT8Pfc5pTA>IyK3FbBxMbe zBhNcG|DUx-W>uK3u(PI-z?u8y$1{DvBcUL%ZlqpcS>LZsk8IHRIZY`X43>X-KixRw zuRfR`RHpt*R%T{mG!H$PeMrd&#Rv6dr2p?3rQ{`n@N(1j`jL_vW4!x-7saz|D}P!@ zIN}`Qin9gRA6q{{n~*;i4zQcjv0)OzRe&|%`~Qi8HjVzbIOGK+6u7hIii|Tl zEuH1VQ)M+}u4KvZd6j1!C0B7)QBlw3g36N&+Gu#g)Df=~Y}p8@?OVz73Nqf&=Z$wL zy)R~gEgjGV_fhgQwIA=_DvV(;QJxm>%mmNWajnla;QFWgcczh$;L2EAl5wluWZV4V zV|?2?I)~mS?ahA^>-YY8V@C+3ZMO(xTD&{u&wEFad;(2pyuD)eeDq+Nm&MyaGN+6Z z^D5UF<&t?Uf7Dq*L1l=DNnxxwc7KUc$@q}>bmb(h$Ump#$su_rq$-`6Vb}m}!uKaF za1ZhW#D0;vKoZl+RbD@>D4kR8d`=UWEJQn(NVK$Xjmjq*T98}?cw}yv<4VQy5r!LL z@03M7zsPUFy$>u?4sGYiNh5>NC0xO9r0T1>+R@t$wY1%ewZ|?Zjxhb1s_oI37 zC%CRHkVA#&28#6>-lR9A55u$G$bOLI2bO?LRUa5x$WJ&zGaTM3#UGzD`v#^LzYgl> z>w7Sr4-cq6^{lMCM4?pB>tmc#;s~m}Gm4+GHga3kKD>sxl_BWeu}wCdLyVv+LH6II zsB_|Ul^q`*I9w3I$;eri=1EPry&Nt0$V`(+FL#G6qrq1^M`KB%b|7Jy?scILJ~?4t zP2GgXT5tgHB%fV|;C!J4iG9+x_$IKlf(FUY~w?0V0RUT|k@wb>$qk;HS-HO6{7 zInI*PUTtRvt(|E?vuPV^Bk^2F;k)RzWK^Kf@U?HjMgfzDtL3K1DrYrIeD;Og649w% zW{~fEg`d-$beNhHmRLZC6}1i!3=`w8JPRUhI>9#j88<9GrCeNoV(;b{?pq`!CiLHig&5k8T zhDi}wmZhcR?qtrXPY4xpV91u9I_-U!*XN^mh*2H zmsfSKV|V$RTe@`mRJQm^sW)g`(|h7}U4tJtuJu z{4zPOlI_Oo>7Cts=q&Ex_H=ip!NU*IjEjVdDBT8!izu_k%LkfCFm1ybCe}g|X&z)p zRCUC#PnbqjWqjp4V_?nU*XEfk}H#amRwejFs+Ptup^re;=4f!>AC@M~_Tn6x$CQrc_rDs}g zX)o4!3Qwg!Mu&<~rf+Y`2F;^m^&G42bY=7fqC-~DhfS9>^;~75!OykBW@g%yNm%oU z9o4`VstlRYL^DMxF8OG!^8(Xs^^PW+*iaHdA>%T=R0+?wy@j(C$nGA0>@G1KQt2$2 z1TXd{eC*#_5#{bFc516oU64Nwi;d8oN$)6K9*P+I=jTgpD0X#nGO`f4d`TGqf|lKn z$D3XcGxVMkpO`j~Bg^g4eU;zPGNX6!B8I+rCYA~F-X*h(K|%o3zR4~BPfwW##=WjY zlINuLxJO}vqc!PZ{1eXW;83-FugOdynfhYq=JP1@T%+uB?vS+wUjCLm4S$ux7CHUJ z&xml%&XsEe#U370T}&6LKeajP^%&S8ZGCM5eg(D`RONHF-S@K@EE&fy%OyvREw=R3 z^EZ9HqCvW+eCwxVImFR-G}X2amNi&(-`2CcFDTc;n7Fh$qYm6MnuUliBQ@4l(qQ8^nJsiZtE9Dy0Ry6(ta2rB zUszLgg$+bkKd!rWmSI!rpd)*M2CvAVy{DQ4b!Yu}F4wc`EL4!okA#3C@Rzo}up;mE z#1>puW&Zs6OqNfMZn?;x@?Z#>fNATg&;D)2%m1~QWMZgIJCdjJDn~M5Q+(Yyc|wcT z_Bes*p#FvR$YZLiMo!Ln-iAHMBj}gaFfiPEYbru*6^?s*ti)v2ybu1Ybw*Hr{zK@x z4dE7$*xJ?BL5?;E2NvC%Hm+O_B|9XA5x5bP9a%kJcs=uj6S}i}e#>jx)(D8(M3y+k z@Dr^59&2AacH0~n>+*LZqcLGVwim<|f}YZ7H^P)I}9G zJ2_*#YV_x96=)o-^``O{lMtdi#Xr_`n`NW%F?{v5(M2#X`rl8n#2I%d09erg$7;UGUWs|`yp<LSEAfu_NpU>+_aPw&GIy~*)mOPY0lkv~#3hX#qDD+rf=h@qmE7KZWw^cY=mNbru zeSMSM+}c(zIM9mV2@AaVBBkv{!;E92jZP;#$sA$64aiJdqZ@{e(6Qw zx+?}A?vmtKye#{=J!)R-5tJ3Fh5^Nq*S7!#$W@!5e0w~R+JG{Dm4Hsvc} zWhmy87<$aDX1Lv+E*Zus6*oDO_f{&5RhWwYt6ufGK&@C7`FK0l+v5-VN7P}?20uhd znPW?(nlFnn?me-FRCjSf)D%y~eKQzk8rATgoXiK#IoP2qj%z-M{boiFB!K^u7C5^| zS6Ga0m5L2;I2^DI>@)`RccZG^#FS9G`iFv`nL1@mDI4B^DoGs8j-x#POflB?RQY6-g8}$X~ub!U!-qio3s?kKQ2JP^v=u5(x;Tj)?bNtiK>P84C0{5 zxUv4t@T&|mvVc#RwDdInbc(VB-D)0bo`ST#W#z}R_t%|kyUf@ za3UosYkF`$`0s&na{Mb4Ye%BRX=llVbk+ZB*oR0O%y2@D_MY%_uJ4aSNq}51qk+dI zIx|M7E-rtNR&T3$K)+VOw?)#B2>%}blG0L%l!yald6b$kA73aaR&FpvL?oIR*o#k6 zK=H3}?DB|-ZAu%n=Vxa+t^#Z?^-;(N@gVes#R$xo+!nMm&HL9#UBbX;!UW4Qm^*4& zGAc+G;}fyw(OI1g@D<4Yg98FEl&CavqOIc0>(`PLQ(0W4jq>)VdP5f$mr(?gKJAB8 z=?`cKK0dxARtK{9_;?sxc64g$a^7-$$}3h-Lk+^7rEsgKgzU!E)==eqL3N{n@%hGJ z4lTum4{G*I4;#~1KVPx9FF6o#++>VtX@yKcrOfTeEz4>Ccz5FMc7CL4OaJo%ZO7#Nw!iZ2wO>2LP|JwzG3P zK+4O_y#$!}N{?qpUtizf;o-Sf>uoibtI1AlE)dYrp&+;Ux%m|iPUzS0f_4{qz@XZv z+J0UvO0G3zB4!EC0B8w(q^{Ht}k z?b9^kfmdQaNj9f*NDbYP{-;mO3ckPD^!7)w#*0%@Qqrcb+4!CWmhoZ{L&#q`3yG2> zUGvr?+_bz3y=TXj6^@G9+Mh?!|JFbT&};wC@bIM*kmtMvph7Y@al9|L`{kM)Er7@r zO9tS%UOE6Fgv<3P6C@nmn39+2mId(`XXiMA(|S5~LQh*a8AEb>&TV^*Q=x1`Fr}sS z%1{84=pgU$hx)WgF4CPqo|)077c;xrHH-Fi#S6hFGTX>3^3k;jQp^P&RxI5eS+t9F zs3Oi3NcmB!@lqdGF70!@KYsljVjy3L>YY%JR>$ZFaMbMp5K+lA=D_A4h&Yd8YYz63 zf?<>oPH`?mHdeg^Oj4!V5Ejq<0vOC-u&R!WQd!BScfTV9h=9*%HPh0a$kMpl;CC@p@rFDY%bT|Gc)uQ6qGg&Hu#x>u$CcG2x0JX-7D9fjY8n4BE_fgB1Ea&dwxj#>vD8_CyKW z)5U&i%>F!(2R*DaT%miDkuch>(sQ@(GG?8@{%eJGbvg2(&7?Rb7$nFQDN2~FUy#|} z*Z6iuQ@(JlgPabaPNeu~>BX0-j3?Rc+t@hfneC6AXgqkdIWnSd+1T0Jb`36K2*{i@ zek#S^5ON!^BI+6%m34K*goOSZ8`|;ITIojI=*Y-DT<T}}oS#{+OiZs9T5M4NYh6}ZI zti)<{7dd&T&U%+Rb4l~%L}VPSx6e4kE9(<&y1pKmAR-SqF;I%Mql*@+ApCOuP%v`i z#`MW~C|Fa~jP}fSXY9ia;hA%k+*W;6wOGtj{ zU;`UcYi4v5l-^vigaVCb$2K9jr>CbX+g;I*<{D^P?!X;6A~AsG*Fc$q;g~n)>lr4~ zd9=n8-vJe=ghN6dXlY;XxZaQG4tJU}QF5 zcS>4_Fhkax#{$6*A(=A4z=}4$oSZ39Op`MjM{uM}t={KTiLx14yv`yewKrb|pWjI- znCFwQp#NXOu&26!dvgPz4n)Mn^llC&!2o?MWq0mr^|A1*e(fPu-hINRh& zWk@FdGDxI1L5zrqcsv?j=|q*0Wj6tMu+1$3YDrI zAKy;#ySqE_6vl7>t5@pwLIK`Cw$bUA#A0XI=Enon+Dcb0PZQ^5(T^c`8oaGq1fFb? z-wUCmFoJ*wwDoAB#eR9^_6K0JjDDNc>rR9ThD{<^o3n9fT1x$t8ZXf+QY_=|=H{A$ zvl5UCHx|q;3@`aFhuTB;bDaP}sYKZ)C1sg_%?2Pe3l%z`$$1$HaMebIx4IoZlGf}9^z<+xaC)w+K&Iw| zO?u0Rla)9SQt9CIrjJSlryayKGswtkaD1+;hnNMwO8;eHe;dgHc!qOGo2EHvQDbbR zkU1tuUQeA5t%%uB4MRG=zq>oQ4ut}W(Mpvz+mp@9(=`kIErOK>U4-5@HylNQjBQh4 z0|gVHHGY!>X<7x!nc%?%31us*>R1~1aWVi`SGyJLmt?0`5MJsx4<$SL|3j z(~qs8(9qFQA6g z*ZjCm;hg&Q@wy*I=Li1lSnt~xCy*2@sL7}4#f`Y-q^#<#L$a^o?>grP^2`FC`ECGO zU{((AGmZSitE8m#vP2qe8NQu}l9FM38%3aVWd%gY2o3NBbQbT269sUybpE+f5AcQOox$Ry*~l_7Z~49{%F^Qp zS>8clPt)g}vL}U)Mroo^!$>IE>9v5f3=W|d?q1O85)fA+zUC_*Rz();|F>2I(i>}O zf$r6{nr$5wCAPWqM&BW>8%vey2~%fN3_X@M3uuV3)vm+GHG5JsnHdyFfCAt(S!&(P z#HS2zaLLfM0^eo36CBi|&ordBeeD6>QW7^%@vYI$9W{Ipe|L;Zka8Ss3D%na6r76D1 zOk*+?u90fES(?>?tOeGsxjZH>cgE!3`f-_)B@;QwvWw$j07W_CCVL37)#s*6=2Omut!sVU;eMn0 z{mv9QZ#G(2neAGt$>aK0I!3U9>H*$M0||j?Xrs7&a$r3t*Q|-L;O*G`A_z-sKQs&$ z_HduIQ@WWFl*e7gSr)oTc%uUe`pnPFJiF&5$T~`v&N_~S$G7GA!qn6ApY?~jk<;65 zCjha-4O*GQmJb(%N%l=n4l^_fZ*!NJk~-(3&C!1qS7MMZIh`n?*Iq2DO=WS5sFIwM zFPT^O{hvGtI1y6V5^3I-DnulP=us1eaEvD@Lm*MR>aYTrJR`4Jouw-^hE+P?5{0Vl$bZuc|UYS?MkD9)g4I9Buc8 zU@h~36Sq|hf38Bb3IvHdlIlY*vAbPupQOjiY%w?QL9ab;e*c-ERCS||#(6h-Czh0F z$i6?AnoO4rX(nYy&+-AjM1Xf^e> zFtGhXLu^mJBU&K5Wk3)N7c%efe%@i%w1(~iOxnai=xEJC+~qeG@oLp@=}1cfF$NF* z+=T_cSwRZ5+H|`8F`2Rcpb$}Z!CB>vdDs7hFGdQ&NiYl0UR5vX4jtwY0Mm?4jt4-i z3BmbTaoB3cm7Z6q%I@Y`=JOa?(;{Ty6k!T9eIrn?ZD{9(Md)@W*SKFw`KPlhYJqeY z9^=a$|5}%T}`CEV`22TGll+=ieHJ4bbqLZ%x;eROqjxY%zdwn5?ij1|Occ_NU5;6eRoj zhvJ`bSwVr{#sg3nBu67!?o76r&zAe_Mt$4U%W`cZ(Bf{q#7Hi5i}a`D{f`V15I>{% z^OQFZw!%tvzwdQX?S;QZTgV8~GsGQ3+*$9b!u(A3&5gq-uu)%N$$%$$5y;`;6N}s$ zNJml+7VY{CHiyc+#lRaMv75cCBs}guR-M7d^;&d^{iH*KH?lSo{2OpOOmzD&GaA_= zPVagAo0)Tk7n5yNsQS*2gcfS!`?j{?BGD%BkN^~jhz#BRbvNZYPoxJ19y&U{>Kc&t zJ_tnLo^7HZ81Nl1_!PcZ?X z{4~qG3Yfe}x~;NIF=YzLOa!N~x+=bavR)aRiSNu=5C9RWwTptP(O_$I9fj7j+#Xx0 z0}X*WWhY~BgG60|*W47Bq`jg|dRzGEP$*Z2)Y=8f_GA8D8%Pb`%rWwkQ#Api^Hk~B z3aEZ;D6BRC&WFikm1SQeziW-nI3Fm7vSlea}#CH$~v`U<){J z#}g!A`~FJUOqHvTt!KKuV)%>v?IA7T{84Za`_R>Z4hueAqs!+yXEp=~ zR#e{h4P#A5Wr`S#7MEaCS)6eHKfL^_rwK_aTNiq?PeGAsfU3b4;r8a*^>Al)Jwbn) zEkgLV@h)w(<(S$u6i5I#X+^-W%H1xZb8yS5s(9BSUjBUF1{=&zl+;%HQzPMwx@_+p zWwQj(Yuk@Rx3h|oLp8zR-1Iws@Bu%l>6QR*8IZDEu?C0e0q{Mz{nwZDSGTDK`;A|N zX&UD^X$cMt&o!E`qu+Xiw^pw=gFijJ#IS0= z^mI*_U1FZ5!Add#ki5)!Q+lg6c7KuS+N*)ayk6lrdqDED9z%7m^C6VH)eT}N&hfTE znGgQ{qXHt_*hckkzi^-5UOBkTFi~ejN8-JXvUn$)fD-pQlW#lqdNX zt=p>(Jww&pYE)Kb_|9VQs?pbD$Q^(>D?0gCbnxZ2m>4MqM%$+}7fvI%&)$kTgSI)^ zyUPAZ8CPu8;eO0a5uJL-=3c}tHk@aQs-n_^S-8osiZb!<>G7C_i$il& zZ^N&w>8nXBtq^N_PFyP^1UIbS_CLz|qBW;|8_ec~!7q$?=QHPGH=*VejQssrp2wPq zjK*1T=QVz&J6eT5p3JYTqDeX0@vx2JrHmmgjHM0y_J6(u2;0^q?JME6aMLvdbqKPdWAX1QR*UOZ>h z$>3oG^;X2O1Zu^tP#m;i5n69J)knAKo_hVaX9nV!Kb6gQ=&O1Jt~Ft}t0wQJIH}%P?~HMZ&G&WB zlJyMKl0;4mm{4d&u(~;mPGOgY{zwVGTiar@tX}z4ga|O&*ONSHXKejlS>~=AjXW|& zT&pse#`SE^m6ZNMa->k`okTE8j<`f_u(04piUW#tzxzoNZkOdHx-$=G^72#4`UU^8 z)urJATGwCfm~dyudofJGne&QC@Jt@kOYxzv5YXrT?Pts*H)9Oa z$N}>p97$ErKTgdQGA+TzL1wdZ3IF^BPSi2U%QVqGoArsxaB8p%(~VFNWKMKv1Iy0q zt~_INf0cnWq*5Ml8@@DVb!$0p;V*B~C^$}>lv=cOXTM<7qFxz$T+l~Ua1)J=z+d)m zDoLs#QoWp%hu#}k?aJ%kB-9BM3jnu2cqL9NTJSb(aU!;$qKiClOkcLkvfF;tIk!Db z(OPh4g-qFkTX2ot3a|pB^)3Z%tNWD&E ze^y8>V6unO8>ZA7eS_vNc0{IwHaNd@o>0r=Gc16_@7a!CHVc-_@1HBB59-eTdSTxu zF7urMc=rVNh^0AsDQz)SS6I->!mr2#8!EDItQ)I!P2AJ)KjOj2e&E#ND0UT$NnTvK zJ>4}ZT^dz?3R*D>teSFuiN7VSxeo2hiNbdAlP7UXr9h_XySR^aN~|Ejb%&_d)`eD& zZE*t>6lJWL09j}!yXND%nOJ#C4bJh|bt43~SJQM4(hxX^`)KFZl}O#S-O8FOclBMU zs9n_slowDlLyh$+6Ap!9uuM{@kiWwZuI-@_W;#j*>b>WcGjlm6)%{cDADX47d!f=} znXlF>)1()EJ5I4yx$znOyqrU@KkzN~=O)E;Y`TIfg&Q%v_LWFQRW)A*w$xvQj(9=p z`3}e=MF6NQa+qUS5C}CMwZS#6d+QcnL`rJ78XgRc&d%-x2fMPO=Fhy@KZi1%Cjb|Y zjJrQ@wyJEH8graR52{EAY^7XtN6?*JJ21PRYn1JNFOC(?>D&-7@Ydv%tyg$$MNLnw zbFi-yN%{PL(i}4a6eh?gGmoDxQbv!%?2`v-*C@uIZwW>Kc!-v!NYKRt5Od=D4?sXb z1cjAH#F8l_S5y@D^sW+9QI-8BNBV1>2rOS9pIdPJNdW?VOPAnZfYy=@7`({J`aR)) zsQ?Ar9y&6&uZgeE4GqgcJ-xj)*M@{ZxuS7>ot;q?G}El3Nm>EUs7WXe!2f3TnPL$2 z#-Klsg3O&a^MbghlGFcv#UgmHzX)|! zL(%;CIT3iiX?kR$^G8@f5|)<%@GsoCX;hFsrLk``^*;w1aY*=O$g8YrNJ)QldOS^3 z8%d3f;%OMiW5CqiTFH59f16P&KAf(hZfAAWp1FaXxue;>>jgb&+X;;6B_J=iMki2t zm*mq-_KdnZ4We<>)cB|;x4Ll33AOc1&1zK`bz6g27$AP8mD1e-aStkLybG=oJtlM( zCV;8IqA=bJzzxQEzb0xmO7MWIluNU_*bBH^g<~;Im8vn_ovl6#KRuhwtFLL@0mrS9 zUz+sOf|;h+I#RN2m6&kZac)05*>^(dqnvbcReMfiV)H1VPuMIAVQEFP+THM%ps;U zcI)3v(z{p>8?t;^o^aKEv)mEVH516q`xc0~-)qdV>C@EGcJ5z;-XB7;T73^!N~yCA zt@e9{G?~wEMeeC1W0HrjwZE$I&LcuuYM|LE7-D{CwJpeRcz7A*+tD#GxOPN<%{c2v z_GFRH6$S8mp=hfsETFys43LpL`NApx01YCF6Ee5^x6#2E5@wgHFFHC(=Sj)WB||X0 zbQyjpSgHD6>A$+=@uB3R%2*Ee_l2CDnYa7GyXWT|61Su? zs!=mfuSzveP*Q%AbU4GsF6b{ERpu<@$N;W3w9)UWpBZy3$7~HiD0QvsyC=UJJB_DA z{wz?DvQF0jwRUvmDQ|z@UrH{D{^r&Etr!uI_{&=n1OZXW2F}=x@{WxD5F|nY-1mm2 zCS3~)31Q){ot>Q&T5T^{pu7^(3ph_quU@zxh{%_xJ6p?@wod=r45S3a@rH;DMCqmW z4u`}z7e9f}h^fMHAP^0%?7`w!beaOLG&9uwu^b~ib@!spbE|~XT(C82$9}eL)&DoN ztnGB_ij0&ad6;%XbsH?*N*>xRfQ={J3=37gdAug6#u_dlkFR&XKsRCd^=WIyR2iy` zbD+^VdJ=H?I9bH7nC?S94cQ&=<<&hJpH2ylCEY~g@_u(7#~jE>S`!sHN=AmHIe zhMLie#^1$%6rQ1kc{@mA06Pg_-ax=xtwQTuI) zApn)?9dS*?8Buu*2F_wne#@+3d7t7D72+KZG!ibhUg^8uV7G1OYNbkDVwTklcPrOg zZqewg)TeQlNkkUNvPI!y*A=rpT~a6u(BP3{7@?%Jwzcic=QTJvv1>a7I3rPM{@zmM zCvtzPg93hmsj!m?V8Yc|u0HuVk~2;xT{>O^pRl&4?9}K3t(kH7M@tgn={W4gnlAbb zy^ICuA>=Ss^@r&HwKU53mfF>u)-O!H7(>hFS6~23NF+BG&$gLqV0qdxG8n2W`pYPC z<$G%_MUDTp^$D(STY>qHIA3fRXW?z3= zqzvs!1pV$xCm!1?kU2iUnMl8N@Z21@vUFZU**diLxWq!efzDbDsx82{;`C#k?;idY zKkYTTU5v3DT+Ea{+g+?9aK|BW!_0&`+|-Nq^Z z2sTf=2ye%Gda6$52cs7abae(ng45laEMSqC2@!*PL^N;vo!XlJ)sa^JiO}wU9^}ra z{KFds7q}FoZAYxH#+qN|=-1tTRO&ff0H;M8*z-`!lOCI|+k4m6v6)&_N~n z=J2(_$prAiO1{SKR7;aFO2QiU$UG?PC9JoD&CtA+7=saqX_vtr^E8tfM`u9~H>KTwG9|5c#)Wd?CU4Bt8FzJ!H_Ztsl>1BIYbM>(XF2;8rKXe{4P zKc2yS>TkiZ^Y?$iB-(H_+t=`;EPEUg) zBlDfgH3c*P-saAcP0)vnxHE|RMdT~tcduiVp@C_m41d4R$F_p=OjPIk>^M0&Z_Z{z zld26T+qJmd-2vCfNdh$9attEKp3vQQVPD>;l$AO15o{MSoy}^ou77F%%buyj+ngLi zJqwE%hm^=j`0f7i?yjw~@ad`Psfk1T0FA%jwz#%Tpn$KpJ$ZBtF9>+D1}vbe{QN*U zId^TWt#<$|slEM&#z7$e;jVg+hR4RzQ9=%ORz{a@y?p;g4OI5O02@lPG!F*?*d16{EALih1gAVg?a_j)V5dsOKvqm>1{?_0@(H`2 zET0z$S`KT|{8=c4@UmF#N}lDKuJaH|X0!-w_W}waM#8`#VRk+Ej=#BCBW~LTLj)Q^z5*sBv=m{PA?igi>1R90zthd6n7vA(3(0F0XV2bWnusP!7L^HlDC5P)R z+-TkuLnIUg^=yf@2@Gm#WR82G{6GPPQ{dZ-~3`;5G0&`)O$nK0m_BScLcFimcg{l?&R>)iBx@N~Cq0h}>Lsw^ zn&u~9C1yx_sugMg3ue&K4j~(RauNB>gekPk!9^17AN?_c`+fD zl`GRj0lP)^kHLOyZLR?JM|7~)DZcwlWVFAm3WJI9UqO73{BZ$kMiu2>ajlmnN?-q5 z$8TpCDNzu`z-*KLBI4L0TX|SU>y7U~Q&X~oLqk7USuw5I1J>mFLT4zK_3>jh=w?&q z$-n%^4E!q*^+dqrfJrTUJ01#lV%9)FmsC_F-rPjs7wS(9DXgiHS11t8WyrgQiu>=P zDUgoft|Ttkd$;nO@uWk8JRzEq%dsjr?dl z!rvoC3aQMI>_}0&NSZ^1l`On1yLb!5L6nF2oyC~=yaxf!Fkm82B1Vo)dvJO1<(+e| zA7ZP)e8{V@%#0@RX@zpy6Se&fxA^MyV12vFRRWIj!R)ieWt6In%n#7mFC~x?Yy$_3+*H)<#!Dr!uz**D@G=3#P)8>FO?z$Jn*u+uvM#i<`mcW%sK7 zcw|UyzCAV%aNV+Tb!{(AZjZ=_qjSYN;=`td6gSUPeX@!}rvjohj?{5P9*$d32%^s| zha;J(vFubu4_$tUXm_O)c+<0MfTXRo;V<~~OmuJYHkWr2Di22(o4#l@2`_4cvnJnY zFa4Z9GW2_7l3tt%1FHq=3wn1F!Q}cbr-OO$K#|d+9h}*UIoHRE z93T=eR}}1vYWItqn%qu5CnJj_3ev;`AQH@3>{t6HQBdTQ!6(=v_PD#P?k`YxN{-ZNFDJ*oG;3x%q@YD zYp?!Rj5*d*TT<0jehm*z$LRA6FnARDZ?qX``>#V|poRJEsC+fj2T|RQ*2+KZa>y{pJ;O~@4w zA7J=IfjD|z{VJzk@AM%^+ED|_pjk?_0cLu?{^80<%e%=F-*N{oR(G;J5zIaU)M<0W z@s3HXLF(an-8bgyp+%m>gc z=C`n3K4Ip*9pkC8g4S&@YdG&plEn{cxusmjhHbzmlS5QT?#I8NpFlJw0s1|vmbt%B z{D`11>a=>)hUuPnkN#05GR(+_M38j6zM~~YNrD4JkVv=K`b3atDQ0jaRzJzKa>oMl z`!`gd@b^l5PPq96y8P0OMX%AFdv_pEL2>M7GnRAqD!&_(>4vr#yvApyDo-iXK|1g+ z+CND*dd5&%7;69QS*`HB(?DP`22OfjSgW>IC!Oxtoj7x%2K9GB@bO@pJihVsgGam~5|}93`%<>Q_WHO3 zae=%-4UTh+JdNjH^G}Gi*sBa*d3_NhsymM9-UGBC<{FLw{x;V_l*IzUlIr=V^w=p% zC1>ZK(cmBgZq4vu$T|h9M`Wq(_oP^4T)dpK$q6KuMLTJ45f&H!K~8=wA2uuG&LRni zx^MGi7=}Bvy8QcQkW0yG)t}tw=P9>Cr5D&59Iv078RI%JEct{i%J#8Vd$E8*`dCAZ z(ixuU&LzsrItSe&$E(oZi!&_;wWZ?|3+q}BIind|bb@X@64;6AjgE*e%@KB9n>Dlt z2wa8nkY7vv^*1f|iil8LJo~3J%JQAsggzv<9kDlueTbK|W*@x!z3FzLMVO=jU2&D5 z$L(XH6OiwTdy_npnBEW5mwid@HhKt)7)km@7J6gZpGL>`usm+d*Q|Et zX(d*lpl=98E^F;v%FynVC3nrzy%lMjeF}THU$i&KZgE@eKw8cHmw_2{Ydrv0SNE3EXKW zLZb9oFV=?HXwFxAI(>QQYuysH?9rO|JJypPvByb!d?jozF7j7w9`g;(&ZpmasZOzl z2i>)vJ%sn~AE~d8{+8y>{!t{Zg2tZ3#59?r4OXtRp0JIsS<$_yZw|$ey=&dlo~0U) zBv_ujR@80Io8e*s(mohC@S#=B|8(>)iHLx6tLRS~Y}rjbxSg^9jD)aUcsGk76{k{O zk(a6nXSf55{uO`)(X$KEyigzvKSi{3nJbKp7^rsI8hgDF@m+ z>qavk>Rcgy6o;}5SO227{YC)=jjgfT>g(v%GN3~cDHRTr_ZP44O6{GQ=_W~ZJMtjSu(Vsrh zTdCnq;1pzuZ0`Xs5X;*hTm<+oyM!!>QiizRT%Ew2=-Cjomm`ntm8|f_-M!na-wy2j z|9D3??;GstL7GaO$5-rX<6^znN6hqO@#AWB&!wm)3@a%W&|KI`XPwgNJqeNd zrzVyn#P!+kYGN}wRp%OCyx{?R$=aJBqgfbk4(WY?s;4rrH6F+Pwj0+qBADp30PEzV zlh$uxSY|O0RjXluOTyv;otYCbqpp|as4iox5q#T}8%VrXny28yV30yl0+~P9X)ZT7 z%HzqY{WW{PXBZ>;7R$w=-_FsF0Qng~Jz1Z8U?hKmDgC+(YiP?zF}m?i!7`a9RL;(! zJIMBQVDCXUQzeqK=MS&xUgXPulNRJ9EduM_dm_W6CkcqL*C5Kx+aWAt`a^ml(~CEP z*deMOhLeNW4_ND*zM#xlE&8;S?huZP`x7^pnR@|Ti}k3lcTq0-`0y*WQFRE0a5#>w zg-_aH7+nf{s8=^dF#xEvRhesh*8S)0?>SdRa*)QY52E5HimEEe1=K*@E~* z6%O#G##@U=O=K1LEaGsg;w(FAqLy`I-nUi`!0;R{ori3X4+zT1*dxP3`RI5*z*okg z;f1c}Ud|sZ-bE-lb!yI-Ld^az%HAq0uC0j{B@iIETd+W|;O;b*K=9x$!5tcRcXx+| z1PSi$4#6R~yG!HRoX)@ZzTbJc_u;;;wYs|IoHa|vsIqHG;-Hk?>v(KzkCL<-*-D0I zSWHfrlm`+rp?<*?SZYp-7i3?JbwBqRxXVOOjAT(HMPNT`A#TdI!bAu~fmcgp3sJn2 zH(ZCdbplE+pj&$<)VFv-O7R{&D+w#=iA$=Qv73)yx@xo1)`IHTS~+Ez3k&IRUkwm=n;2 zJ)7|H(sVZpSp6IBq~!PYg{)x|v!l3MNPaTMmcbE*OPkSrnf4Z}J)&|-AANdtoja7D z{QJr1zPTM^kLx3C+GR5C=Q94qipF1mW1amaL%Fpkqb-THNSiXz7j<{l3dOK}72XJ= zIZK7(^+lXgb)n?rPd#T(JooPXm+yu2Ns1m=`9!N^3$a4Ia0#3$0e z(WlgMHtQ>x2j2L-<6`~Wk2(H5u#qa40UADd136C#f6b|U<3h$#hen|;oKD-Au0SBU!K$kCQ!Q=U}Qqm&>h*3W8$|QBVF2ib&6wS}+sSC#HhEJ4*KTv@&C;L*Mp2-a+!}z$+ zP09Q1N$-jDWP?v1ZYpoemYr)(j+w>{N2V+9_a>Z|)*e0R-P2J#_L zRtEv9d^UkjUylZds<(M*Kh<3OBRuzHB*tw>){0=17qAf`geHtBlH>Co;gzPZe{g%bud^`yJCE7s z&SPWRWlg;A#Ib0UzWBKUX^8}?%tuv>>aCNN?K8p0EQ_Uy9*c>~UPaGGWTm+)bsrh#j!Q3u2B*^WMP{N*^sSu3!1o?J z`D&^{AqCY+Wh%+z>9f!k&wZS=;DIs4SU8*k0C>;!HQ2*WDN;r@G z>;^9^iI2Mt&COt{+x2Qa$#}gVTY|iq?szs2NY3kVeucw=+26Ho*wIWfSp_%k_F_Cg z^uBs_*}ded zx6`hFXHWN$90e0IvCn;eZTv>u5RAz9r8$jvs7;{4m<$GKh4-)gn!F7V$F;21r241Y z>Y%#(*9Udra^IT*JQUNI@|<27^78M?{;0AdU#D+I)7XQO_)I_?`LpRdUOo)yl{(`Y zHj=~CX=;JAmz7dKEy%55Mx^G(o>^F!g}bCU2z#_R+@6_aJAQHG6~&m4F%;>HNW)O_ zzAmQAMraR;&v&BUjhQaRv7&0Ql0uR()*i!5d`V~~XtP|*^tz&X^uJEfNOXQ|l?8>#_+OA5Rli@{<@ps{urZPTZ6?1|kq zMCO8QZz9?6^rd_4$O0Q1^6)O z!ZPig>vv~00ybOAEz+W}oCqC0B|_W*q=9I0cLa__?PCf87!Hi{sQ1{j7KG|+5R7pv z91X2#?V%|y(8Xh$orW(RhPXMbnU2#(*Qj%epg^S`8Dw@ajsvCu&nxc^omNm{ipUl} ztvz;_^D}M8R-*OO`cvCezhwhCOCKi5;6^6-b_bSX)-Spa`kP0aCc?XVEC%KYq2%`* zyAwi*0#E8BC@!N!nfC|vu9|HS*5-T6X@?Ju^6imWV^I|o7`Hk(EI+c(TvtIH46b^` ztQ9h(PnQ(TFEu;+De4`4R;ekdanCokO)n+eBohqe=Xhjhr9GYQuQ7=C?3)yOfTh@P zx%c-Ng7#MeL4w!Zu^6cRB5W2TWP1aRaLuo)#i$E+ZDVGE)2Anyy-cZNpB|e3mvL&^ z+20;{=+y4q*9a+Isje|Dr+0it(H&^|xVw&s{f4)z=Tjr=-GWpEiFJ|I;PYjQ2|y$L z*0(<@HMPb<|2>~+SLdf}MEx!_o~S(SrbMkFm+tMB_yni}Y%6?a{k$*i{sCQVVV>rYE!eO|=6Vr~JS(jVbE2 zq7LE=70z1~yoHOBTZ?f_vFU<&j)P1yjfYt^+e5D069n1Tw7U}>ABD6oafLhT5LEO6 zN$gz|-2EEgD>d{lx;-zhRM=Wc<1xD+@Mcu#SGLZfkG+}vX(`9zZbh3qj(5$RXm`|} zcNlFi zDZq|(_L#T=Q=q6i@YUJog0Ct9V^4m+0|$fSY8((mJep5iX`-wpQ_*3^9*)&-Owd0y znl_lhG)!XYLWHrJ_}N?(nPIPOV@_1RQ!2EkVxvm7yc zKZZi_^J6A&l_j8d4J2=#nl1~N9qo_RODuFy9)w}N$0pmycreko1rBKGd0l+*iGDvE zjqx1V)B|lhb0<#bO_jNCrX#-pfrIwW*3?sixWd7(LGL|*K0X|&lwYc#|?~GQ1aTK&S$rCndZ9^b1!tr?@-6}o$e@+ zwfj5FjK{Qvr^W4OPf`AM+LeK%itc?Y@jqiSi*2Lgb8YAx}!Ly;g~ySf-usU4-d<2gF%qKko)9-HvKHRUEe?|Eza} zH43ICNbe`=1W$`|Xh3IDAD^|yxKKdz4whn$!mQ-4!TaPn!}_{3_IEf!VZ300D}B(I z(VoT{1Uj`7CAQjR#>Z<=)>QiS>(J6j?A0E}RWZ!ccw7AV8+>^P7#%}iTkhlw54f+3 z@DlQzFu(UoKJlgFH;x$(69EdB&>HU9{!Fm~`>O_a>I&pT`wwKkizjTr!WrX$?DNYa zj$qve;VKIRG4?uaAkI1J2Rhb$OBjoOYUTP_u};*J<)F0cxDd0R^00D}@Lf|xF#D6~ z@b~S^iL0scoRw?PFjX)C@cA1z(xyZ%H`GeK-^$Vqzl!Kq)kzH_^uc&-=Oq(Hy_f7a z*wGk9V|!_OF7c_m5FTzQI0xr(t!d+{*k{>b$dJ6WhJqI#qe&PKB==#4R-XiXl|K&^ zil-sCQ2Gf%^gyuXm3cDFA`G+7VugaNQRd{VZ@E79zzn{5-Xik3`EqT(*9(@@zbqW9 zroY3JQ#B&7zXp7(nKpPwI??Vwl}$+pXbX8bPg<3~iD_Pi6u$O93eg~}cnO;|UVOaU zK*0CC2Vk5J?F8>v78>OSu9ja2{c-jpD)G?E!)CLR-_0~5H-(PU8t;Sv#-lK(d?%Fj z(p$qx(@m)nb?u$l-u~k6k^3ZdpsPduvCDfPr;t=eyd*M&=5~7m(!64FA{9>Wo5At6 z@rM~n8K5Q==L=j|b$c~kW*x~pyZK0f?lO+CUyMsEM%d|qUx&~;*vH#r`9DblZZ_RK zC*)3e(v)tJ(9N*QbIY8@?W7%;cS3fK`Yqc}c(0nBL zH1HE3MFL;+`e-rpjI@_^(!WC0E&ENi)b+V?4XB-8W5H>L(PuGhyZ}5W69AJj&{!B< zUvB^gc06qmD{WUA&$XwEzMYD`wKjg>D83f-d>g1mn5V8S+X?B3Li3FWsuCM|d4(Tx?EC?rFU_UOF_4*A zT_Q!aXpC@rAz7_OaNEf1du#mGIlu>Aw8EGDKK`8@yZZZ=DL}-|g9exNu%j}q5h9jD zqLd+r5?*B1IqiM6mQaRv$LiZJ?;yd=v$O^C-DD}wcS%|{?{1}AwB{6- zWQYs`)=UeUjac@oo?;X==H`JI2UX76%Tv;cC#ZueL~v;50Y!r?Pk`Waa%Zc81|kAl z1ElmL+o13F)92{?v~h|SP6VAnZC+B3yF7&~EDxG#P7^qcGM)Z)c<+O=gRPn#8wrQ| zC-}0_vo7JH_3kE(kKbgU<0ss_gMTvNC3*_N)#@{@KDzvLNIniQ0%Zo?w_gq9> zahIyI;&n~O11apT!gVPMVvYMa-KK?b4UI1}3o5QU>-_4D3P{O84&XiJYa6jw%5 zaK|?ZS#LkzYt(L->%*hLC6kHx&x>-DM1 z*|$61S1T1bD)bxCH$j$_O7U412U$T|FCA`j4j< z+_`$_g>$Kq%op^IS4xqTFE2QmF`tTIr?L8BnWgl3PBfTuuL;&tt!Y2<3yqt=Pd<)T zyoFrQ5-Ud@WEt#r*|pML2VV=nQq}*$LHdGt?;IMAZ`3mN=fIc}D1wVruAMq60Yg`; z7O2(agtx^BMXm!==n@1T5sHOD%Hq?2MeguD>GCD9o=p)KgC!oYA+lPP@?4VA} z$}q$QI%%i_C~>fw>2r#<0vVe3s`^f%jF>#~NA*moF5teVWAryjUB_<5_s$ToVx9Z? z63c2~{}v&)YM8rc@2u=1nTH!%o+hA15{l)@+ccN%D2f)A6G#a$&ivI7!*E=;9#NuY zGnl&#E;6M!D|6{QLObf|li|M{qJ7=m+o6EY(8*vE=f^rc(Ts|}CQE`jx86M`i45E^ zpeXKS+BuC?uiEVb)*w+eO~Rg{(AC|?vw9H%BO!x#%Z>-jgF_wbw+6P}e5;xFn3#Z+ z@OIzb@*&`2VTHl%P=I-399~EewpS7>^JQoPcp04N$DQtcVkMblmut`MB%nUPx zu(qa}3@`Ri-w7LlTUJTNSvLtzWZt~lA>do)_!zI-iZe1#Ma~8k)noTWhCOhQIGzc| z7v~L!!`ZkozBSf1CpaPuhWMR8XtvRe37Il~u^Z6~kHwH;iUB+;r zocQ|H4+;4rox?QMjC0!q&T0n{qktZJc?I@wTaK`oOmxwa(u0{YcD6&ANGu$}+f#-% zrzfE@60VGDBZh|cE}9Bf^|AZi;nTXLI*J$jbXh|7H$VWWddqz))_B(tPA>iyjwVN;N7=FY z<#wIV?Am-Fdmhw~CxNWY0Q9kcG!c=T9pkGNY`v1UHb~xHA`<`Vh}|Q-y*l~(5ZFMl z0wk33nW#0D;m66?zgRZKS!EFh)hWz_%^s=ya&!g@%gjTr?P6HCG)yK5rHC_)b{`cq zZwOr*vQ1AjdX$b`N_?1$N#I0n5T}{khX2h%NvHK3cpJ6i8wi-YHFri zl5D}3WRcYEoSDr~T%0a77`Ie0iDPD0dPOvgb6>0`RNg=DzFvTP*aUAz(XQ!uvT?fn zx(}X>Ocj663qFjBIfSlu2{lq-Z-e?<=0T=*HcO7bRQ_anxSZWk#li zEKH%}AEP`^o&{?rt#We_$Zh{A)jKi zWYSlf6krq;0ddyDzI=m({P_j8V7iBhY-g?M zK4qL>e0C%2@C2)2A5R^qTVNe+(f<=bSHbWA@c+OdD4HMnm<*OC$XB&ky@Q8d@d`K7 zo}sj%WU;8#4LTjt!omU#()Kovye?x0@E-6V7Jz|Ix$&q~F-%~l&dB2Ht*XC;MKKgq z5|@%9?d1jA>Us|AB!6wJDub?7vv_$(qP8E(W~hUm7veC&_686{b-lf{{OGV2=Dq?x ze+3`TjT}x*Ox?=Y7`;%KdvJrh5m0Gme03iW>o{UVfmy>ANB69}|+ahY-B(-QE#Y?;X8~3a_x1&PRUU zJ|MH(*q6(cVzN{io(?jNd2NpGtut^!@s%F~msE?z*e)w5&gAL1Z+0bo5`m{NgyT)7 zV!emoj15oZ!%J3rn=skV=hc$KL_~l&U$@hghHhstBK5I6(NI@5*&+9UciD9xd*O+l zS-4a{R3u(sM(o!?Phs58~K!ZqYBl-mHuGqVB|{tE1+{yzVg@jZ$+QE^|93@eXj_31_2nr}o){ zhrO)@(6eou0{~cS229KZc8a~_gZuiIy(+5o!_}%+2+S5 z0i)Rs#*LgAU;GjsO?zh4_iE2Xu`BC)=(Z(kf$r-KzL~t5eY{kl&BezB^2^#whU{;( zy9d1bPU?n>(i-BtLXy;*Pg|~DSKa`N^1*4L7Ca_|pv$qw^;*WOcVi)_MfJUK23;(Z z(07MehS_Elf45`sDy0s#T?~P&M{e^>_77&jhH@vW$3k@#MB(P73YeJ>Hm>M_p23_) zN8QkjIuzmc6|PqAUMrDf?)5mJX>XGd}R67TDx@SK(n{PU&2_wPPTUJeTOZr7;Ln`CTLYb$_NztE9bw2ubojXXtHd4nCporSM zM*_&Xq)=$H+6F40R!z2Pn5Ie06yKT0m5 zhsBe4FhZWd>21T;AMoV0_T8?W#AJb=xFQ$2dw)Ag+_(}Mp_Ay3NP_Bi$||M^(aSe~ z^0mG@apmbXiWXs2q=-v==IA`L=%2&~*Twps3%@3ji-k_|kA_ACE^ZjWTE2jbP7i=i zdhDUWj|*0Gkw`T(0UDJJ0Izm7%8Tq82zR>pR32#w}XD-FHO?wwDf= zqBa{6qU7ie{V|Jdwp76$gOepu;FIXfRlKfefV)L1e*mMOxFM-imG6uZDVD(l9nb5`V1X@X>DTVu!!zJaPY& z$%Fws-53Fmy^&U7Yrb+hnHZB3OlWoknL!d9m}}dD9#2Apw2|Hi{LKW4fHy(0gkKM) zCp5(vQ)9o{Cy?CGR%pwh3i}Qs$}F)p)cfPm1{^x3^unWCCD-2hyAi}Xx-$b^MxP`F z?hTyd>A31}}7Sw2RaHHIO8qkz*8tk;aRQhx=8tUlox(!nZ9fb3m;qp_7_2pALwEL1TT?;6-WC`)nDp~!in4oFvz?D`9QM! zo6RtQw-R;v3bNmZ`)x%eD>|MG(Z?P$QbAhCqD(|=&E$ctsjCSJJL%6>8R^Y-KBJx! zUXmRYNpohhJ78_*JS%%HmnBSz*X8s52u;U(%Ykh#RD*XP%G}MgLhfMzLG{N{`|I;p zn~n;KZ|^1QzIWkaIF1yw%Mcu4 zA^;4Dr`uhusdKm=)h6tRTc`I;;=j3v2??57@jSIP^?F2CfV_o3lT~aMtfYYol9l=a z$Ak>N_b)?B7oRxrO$~0N^G6mNFIR*Sl$Ac?_HP^y)Nb{&xEpoDAUqdg@CbU5LjO^> zRV)jW6vk8VWSKa5&rz;leT~<)#MQzdSUpf}RAUY#BcO|rzmKqM!@OwJ_;kH50b_ml z8?y)uo76t={PbfuTR`5TJz2(H?~1GN?hJu?0?OL~G()9I|GIEMWGLwB``Jk_qlYsW zAN~cJ-Pk1Y#Ky`vMKaw7cFd`BZd1WOQH}UsgU1uhJyHH~_}BLY z;|s5XGLz4D4?8od_j%V`k^`>LjHTbDegq-xq1PCWAl@yRV~0nqSqCHR`a6F8d%F-F z<>-5d671~`vL+G^`Eem}1Zc&=WKB83@$2F(PHk%EgcQmKfmBf*4|uYm96Y%3_`G2x zjQ-Rj_|NhEaTQV8#2qw!wS+N^ZLrQU z+w~uGDBiSy;mCj_VyqS&-J4% zp}NAvqS8;fr0NRZm}@#+)9DIVii+=;mP3vD{7m1=814TGkdf-CX@j2Hzn)`pREZkA znxr!nFx<;trUee9{=`;&1{(HC3%*?!ljI^#aYLw@jt2(3JLD-?Eu6O6jzZ6;%*CXq!1~{}gCCm?e^ps$7M*aMg41rIWQk&J8z&q6wGEH!!UAw=jP< zlKIW6MUF|Q%%^f`r;0I4yX^Ls+T=a4>+uKcR;ob58kd(gL-%O1b3M>rNiOdbOzRAz zy%!vLM}z;~cF~l6xM;j|wef2|&j<$AS`D9^_y?mqdcZSfa^XJu&uDBY8s2ckE8u1w zBB3^e-^Y!Vf3TqMC1A%#t1V}2P&Vk3gj>uJ!%yP5++R6|cump(erLW^O;8$J(v?wD z*?`cW-+D2r;Dle(=r#0n#)o#EB|~rf%M&H>+7A&kgxjNAm< z2iAH6)m>~SXEDEg?n6uGSZsDd(zoRkYaqIa%-8Kil5IXT)H?1>4O3oAp!)OZx0d%K zKZsyFVYwL5OPD90<5|+zKJJ>+ zWqmh5@z-bB)61X%iZf|6xucl>*am2Q04a?{OC~6O=5fP%sd~<@RB&F^|9#Oc6gOBe z^Dk{79$~LcYs6)hsT_k2tselenfFu1-s6N~3Q(8t>vC9ZaDt^xTlaYR%s&~d-_^Q1 zcOy3xWp)cEFcSz4DbKrnfI%tj{r_1j{g(CrSyg9ozl>=7-xOQQt>Ch7c)zkT*`>zw zt}g$;uTX-^-I=cR+X(F{IRx%oC^rDvNf-(}7eY@A{DPRuYBPuYJRrAqn8$Q6B2PWm zNz@?%9I$J<2YojmLd`DKQ;}9)K4VbIKaW4pn_Z>x zr8K_IhO)|#ebP?g!{y3(I+vv?qsuNIS=-+xn!uVeeV;n`)Up62Kp>pkIan`*1p+2$ZbWRxJkM0DY8H5RFgW2w$ z!c0@}$et5(w4GQ)sm=RWk8(hOoKSe}MpyV1a6tL~TCW$7O<(Ni-YR>cj zmc6qZGJ7Qt&E5Ul^5tq`* zfW}tF63sDexI&bFu)tTemKaF&z~wS$>r%F~wfO%i8~J35l^EtPiyb}r9XQCh;RMI8 z$yB(mGjTI94C-UYtKsCuZ39b_eL(SqI%me0_OKwAsUTdlgfDnX@TecJ1hm=Wz=8BQ%?b z<`o25o{O#a8I5%?v!1sBD?QEMvD8@cOG@by6x7143MTQ+pt9lEzP@}3J|=lVaa{!g zHPzfdDFc6?0y(fcVw4K&kER&Wz=&yGZ%sehnHV&^`Pl1jo`K=G{*t7iuOr2bNyrl> z)s8St^Tb$=i&oYgHLj}~@l1>^54s{$4s<(Pb3={#Ekqe5LqqZN(Enjz zqiy8m3neQ%oNG=?D>`mf0wxn#VBVJw@YhfB2P*>^a();in+?0wSNfIz0)92pAdp{4 zJe@{Io1NU*#~ZP_cKgSs^;!Kfaj;*wsP&s6sK{tI?4X~g%Lh_pB~jrjKoNmH^tQD% zHSaJmSk&`o7Y3@EgqNL>k2!$4Gdl(*rjLKe?uLE+vr6EFy5G5^NDNU=I@80AsbLyYD?ps_( zMq{F8b%okDoO2QsX9Ixla>A61U+3sO4QLc4OQ#z7d9xDYI9ybJCBXVL8#gSwR#Ou+_8Hr#UPX(Sdte`|35NZpH-9iZZ4A*d?b&0Ws?>;lt>4b&0t?|3JwkRe6I6-vKWjo z0)M^*q81J2{qxG_l_YO7Ifc#-DXrP97zXcd}7Y(#pv0SQlW0wr?$$%2_{_d)LG7SOqCD&kQ z#Q#z2q+pB5pv4U3FVW)C;|kGMQOgylg;e_RMJvHq6*)_v&`ZILycUZ#+yN}1JZ|~`-d7g-DORH zGx7U36&qW)pPxug4f|rdXKDiM)>M|N?x*-4B_ax@m~_Y5_90^1d^zzAkSE5{}a9hLKgDw`$$twhY9e8sP7YWTy$+Qc^4 zU)r>dq_Fv=5nyZ+G2@nsECvmC%}c6UeXw%>-SkYqN+l?6Ne!HV(4pa(2|?o>sP^;5 zet(#tPKg(N398E-xboje@fK)ak7^ae8mYd0CIRY06L~cGRPE>l9i>e#h_g4QcRe9diG(fcbG8bsS9SO{-;?X(F&BO=={xts3G;bEL^qp6 zsrpY(LciJ0n@N8%wh#NS@&q-8;bjWnr-Z53Dqzo1QB;(O0L{h=Nwsn2l%7bkaoWC@ z*sK1#K3khLH8o%5*GlNcz#ilDWm)~LC9wUIuF_C~_A|2<7@%^<(E zjiX{gsW&$D$HNLQ_vcNwh^qfqx^<#|hKr6+H1PlWwu!te=UE{=4p?jM2y}5_4_$qz zRwzGwh=ck)+_pNEkv?E)=v#cyEyMNi9(JSPV#I=4fiDZc-_JvrQ(?8DugMKs5Mh>4 z4z?N~FtBoP_}4mz{dw~wfgTP1p(vPmJU|gs>YGHEI#5_GFEFRFxnwl-d$fx4TK=!` z3~Orr-aBD=p!US^4z4{nsb=4DvTQw*RF~&;^ZJy^cWOA&e+kR^ zWNtDJlhUd|O8Q4N^i}-w=aNm5)Y6|Ve`iFw&P!`a4vSRDGi0%*gr`CJ9&vhM)_AG95P7~+O95Sr5r29{Auc&Sd{HN;|Hn$+A7OoceW>j+ z$DYL{Cl@au`?-)DQrLv2P_TffppxBGUObRMQm5pQ4~B9jwcKS?4V=wM!kThi%8Gl? zD%BDL@gsxb5O1+I$VGHU+yVl{SqYE!dsD)a`wc00ctXVym09t25+_TdM|f3Eq{Wq2 zB8rqwB4|tGSLQ+nQb>YkX338Zj=Zx};l;M)5%{65)X*W7!P712zjZQ^pT%@8X7w2) zfc3~f&!|FR5N`>SHS3>`qI zp}J+}mYQI)p}L@=cVGD*bSot)w(3m!FDqaFURfD1);2?{SJHC}jbvB;aEKPNVUmu2 zRPUYGtF|1&xNgh!B{T*DZ@7WJCJBN4=pBcRH7{}J<{oi3;@M$BlH}JmcD0>;b(QZ~ z)hQcrTa}h^8@GyaTerfGKH3)CE~v?WqL^mYQHQ4|($KfY`S2d6C>>Vre*cN*+V(3v z2{oMmd+)q&HD?BD=|@`8NE6>^pfTBtfl0N7{dIfCX}@2;;hSX)Q|FeH4hHB49?r1-^HOmoOCrq&M&epHA|59ZW8nJQAi1_kfaUOxOSSX0Xvo{ zy*?JHDLWQ|ac~H1Rmb(~Ss$)BWve)MAr(%Za1ge~=*w+2Tdh71rez9hS=i6M=0eie z%9`XrZe{)~CPKYEVr@AzhhZmUJdp1L!B0t@0hrKXQI0_#M$@TAsq0ME1YSE?bpCI) zaumnsHa=#m>GU}Lwl{c%c!#Zn?#Lqbi-Op+fD z)&%@Nu<-6gXoFQEbitqNNgI`TRK93Dso)(vl<+farO958jWbK3!V35$;#qgD+X^_V4_${NBH( z{i^x6BiI<#EsTepKhR!%UZ4mttd&K=k-4tP7}H6A$8J)Xp-Dh2?7dyQuI*cK9bBT` zSCMWc3>aRmw4s@QWO${aaysemavT6|PTmGRs$Ab&R%y^738HAA-Us(%gEnMpVJzLs z92OxE9Kl2nCcEn}+95^Tl|VY%(mVqJyVa+6GO<}LNwiIx0xf9o$98cfCIO$3em>?d zl$Vfj%YsG|J98ji)~L}1;uGI>c){3E5Sq%3kob6(d1M*drZ~ZY1yL8NIH-UCMOg|N|tTb*16ZAgj8~a;n;cbPA|a} zchxPk+{ggPKaT8m<8XguO^tYB^K5XHy5D9Nk037nJB8DR&5En2AmBkW|4erQw@36tKA$uMx}d&VU}Tyjkk#YAT8gJ;q!17C)+)v4d!2jS)@7 z-=<}xeSm$JOZABm`K@vSA!FH1Q3~>(edw*5Z!vgQpd+53oTb{br~+?G?Kne+8cjfNCw(5llNx>+Gy35Z7l0XOz19ch9f( zB!v^a$}mA;b55vy-G{%+5j@?|2E-2e#pddZrF|RZ#ThC-&xYUiRXBHwTdY!t+vdRt z9SZO0NPr&r@8mK^9LZ_H)6S&tpm2TdM9A2AZz0Fkqs#r99h;3};+(y{n@SFES=52H z(@_wFU-x#Siaj8-7oib&^Z`qhlZ>bMpA zrA@#N$?A3d#{zBA+{g1R@bV+J(*{b8&m#^uIRgxAm^^>zF^&GnhDPS$(37d-Q_v!{ zuxr+6>qHb)4R7F>e%qL5~vwmQ1GME{}7U0zdp(6lZ_l!b!f{50XI zkRM=7sd;zT?!o1}e`N_eDQD*(Bi7vt!p=?hyB9N~%MZnFt3jp(XA^GcrT_WRuF7j) zG-e+k?hWTTM+i`guj~cLrHyW?OoAx`lA&>&FOUj)N)l{7ys!j#dA?b<;g`)(;I%=F z*?!EtFQ)AN4-3HjLlTFaT@8|&sV{A|HNwJ9(Lr^@NW_rtFe0K>CB5bs9clT9C()+NG)?Phvpy&V|VS!+d- z7$tcD;(zDvMVkm*KK_CB}`1#>f@?@a#+D&Wd{*()HwNx2|(Hi0z+R!ch!U6%cbHv(hS6o3nP4nYTWG+wp zvhBejet%C=-08U}nz4)t~4q_sVQ zbLtM`8~9fCCt=d>cNNnewW43 zlh0dkLJ-~z0yhugbP&9$e0DawTwpq-n*AkociLvO3+Hv!AzXD_PHti{#s4u&z)#34 zXdfgba!?X`5xdVRVu=yE=7F;g32jI9x$$+h(KZiit+#(g4`g|c^L?HdsGjiV@O^2( z`=jN39kP$5DpIcBn|z)#;(Wt)uslHedc$FRuO%=7xk=`m7SZJf9^X$9pVS!le*$a$VB?GtaC_-#r8}yQU~t-y`o)vbBC8 z;RhnnYgTus?*ofQ&7~MVn3T-7Bb8^=Mgk7OMR-8wmSnB20jp@AqT_PWuFA1r`~F~m zUTOL1EsyVh=L3Ni{`%vN#}>!uefr?2)!#?}o`RAX%UrX`K7<;hXe`>X?)`?T#R~}l zIDWRToUGwxQxfX5ynbMN9ccsFz7reEx@y6&m#`t7I#JAdym7tXP3;`{aqg3GwS_+{ zq6*9k1?tSso=aw_2 zP2{7XN+g24?2-41vMHcyTze&<-dsilC48WFG&5x{S2)BUcZCKuKx z!HrN>&ahv(+bI@Vff`!?;GTqwNIJ;ONJZAweh8}1?ymg_+#;L+E_+L*bly>Fal!9C z&jaTaY&TMbovSx97R^x|H*&PyyZI7hU$>u{Cvi~K`&=&u&0Jx*)lIs%oDiGbC$zGj>(ZbNAl9 zkjR>7V%4@IC7c50bcgkuH|otbnsaLB`xSW;Yv&u{9}ZIQXDeOo`v=vc7xdTSzi(>@ z33_ak^YY($K)K)ZY&;8~zG{kwFtp5P_(%)DEvCZCH7+7Y>I~scj8vuBc$>?-HyrP< z@YU72bIpeC`VL%WMleEI+MTG2^#kmk@KS%hT?WVLY9PBULmyI%V&uuUgpZyeXd`i- z1lcVy<{a{Y>X^%m=sTmb7hZixB+4&kKNFj@iDzp4)+eGz=H@7mbFO0Th7^wZK1B_& zo0u`Lv%`KJhkl8agD$0LGxBFx7QxlvW=rPK^9Q2XeD%xC2?$;)PjMIj+$hl&4hX`; zW80kKyVevEiQj|4V^?sVoI0IbxLE(d$6l*H_E^Y>=xdCw7j_UFJ~w0_xWbT_u*smz>-Dx9maoI$V~9ML3M9@|q8?wb=Q8jo(KH4kg3 z_Bge~Kdy3!>F$m74!sCrXu8kR!d2rqJ}_-~J^|jM>V1K>&|ni*K;t+lzeq0uGGvj+ z&xTO%>iBm>xn5++6H0vJMERb9B+C)`U4)U_nezKL!fUBFREZ+&JUP$D#_UpAQy=?w z>GUSn?Wke%$ce|10$7!BQCr}-atEFS?$1X)#EHI7kPS$GkC!7Y-5=90=k`pf={Ol_ zbx%O|61WaLNyov;;N)&sSQ$th=LfjEkS=h`pP*K>e;D8Q^<{a|cNe|8^D$G^tLckwVF;762-IO(9M>gp5eU~wB{ z_7j`S>5B0n5gVDcwQr5h@ai7Pq+(hV$hKAT&f~B43IDKo&YWf&rX~wcVbIk@pDYw4|g#oj24vgCGwy+$CU; zFLpz|^WOHxUe1x@1D-M}03oMpg|3%!12+9`frksHuU$8d`vmihoe{C#m#0}+&Q+oO z?wcl|o7d_OU74L)O+F(Ok8+tySlb%Z0D(6HY)ZY{F7Li=bIy06v-EhCQWhj!H4CaUo=GVZ1GZo8f80XI!SRg{y3V#u*|m#1@*TOzo&}ePK%?k*dq}?A znE*e3=IvW+p%h9ho}Mn|GsvMr^VRSw$q^5ddgu`5_o2Ef8*0}}n!BgmlK6plZC;FD z=3;*4eOtwa^Nj0o!q+B!x>Dgur@5vLE+%g58SV!SJtVHCb64Z2Zxn}r$9?rYQ;02P zL#1KD9+2Z805cUvwmnS{Bm{1ce!Z=AjCi{@WjH_$FG|ORJkVxE_z9KXuRn~px;XtH z1vi~6GMCV;E%U8s94pQ1k{T@ zv7jRiS{r2TL2ifI`X(SZnJYM+)aOrqh?+`8_SXvQJHo!?8a%uWjvypr#AsFW|3lSV zM#b4IUBkEx4uea8;O-3W?mjpJ1Pku&kl+D=I|TRO4uc1G2*HCB2ofOZH}{eEJl|T^ z{GHX^wX3RY*Y56n;>|W%{MABfre)a*UkOG^(l&yT_eRO>+iwF;vW4t|G=4|CcBjd- zs2Hx{dm}Tf51?4)h>zHAfs(d06zFpc<-GbtFh&~_$d>!PM|+(RTf@u3eg*XredOu* z0nrpSsE~LB=s*2_e7ekXo_6R=VodPshFD=#<#~@fFX#n6TyU1tXAbJVT|`A1Jx++< zK;LP%N=aY&UD@Y{jCBj4y1!k4k?NCG@38Dz6B2X=NZj0~*PhTRkkydazc$nYHJ(@c zF1IwM_FCTDiwI1{S^aty6AV0Mz7WwR{rlLj<@TXBYCou)l?Dr$eBE7w6_!5L``?3@ z6Svw4)x~+ouglD%jRfd=V zORdW6ytJX%xR@MT`wRU~+kS7>V`R2#-OOB;vo}d@ibYx^aSHSK3h3JB$_D@2brY zcSOv#A4=cbna&}!iBB(E8SqrE3|OfZx7oK(<5AH;fP88MIWh6Hj z0a@_xX&_O#_hD9tW*7)<@QN4vmCpimX|P1)^nw|bYw0Y%uPU-ma%tvX3qkjgCJHcD)(JV5 zmcQ}-9hGMv9lwsgO&QW&mw3vJAx0Iyw(5O&UV1tl70@i#=aR44&b;YeL4dYcQSA+HM941phOL-4#4>%tYUy0i z9UU1LLPdO~0tOLLT)1w?giK3lB>DgGQM7l-aA@j~+&vGv%s znDo30nyU;OKbH_6OTPCPZx80a2EZN$@9@+ixZCt%)VRwZ7*W|8>hKAQd-Jp|sA%Rx zqe3oCB7&U^?>I5y?jUrh2CEv0U3M^D?XS{V_2-IhSDQlMTpQRiCRl=iyjfa|QL&HL zn{I)EIRU>`0f`QK>ainK2IbSMEe(#LzyPyVWj(qH%`4ol{J#WuqOEVGTy!DB=^77dxGH zcqfG*u=Vnu>Z5b9SlRsR6`baN!n=gBeJ3WItx?YN+!?}apnpKMVZe*S=I?JrFZF}b*+xgc0&PsM zO-4h3o<9I6w9j2Ztf{4;f zc^>Ci+z#@#4i#PxjlY=rh^vtC%`1o~P4|47NMBSv1^DxLGsal?k>x&$Ilwzilm;G& zXz+KuA%nkg1`V5vS3lsSj(GJO zr5{pwBJBNmR2ACv=|%v*1VFmB)~tf^%UBsLVX%&~<{58w;>-mS@2u8IW>plN06S48 z@cwW_F}?{*p5^k78OBb)MPDIDIrN2YgLf^sj7(xde^Iccl4r9*J@~PooLj;WnyZ9O zkKUFHO2ux3YZ{T<^z|YpD28XA9Fw!AcZM-y!*HFCKP}}hapQC+KW-$A9CG;L14s#u z384D*w)aK)K*8OEDv8S}M_|@-l=8SD5*lBxw4q^l;X! zb{hm(s(uN7{c|ku{IoCjbi~^C7Q>ETjZM^w+pWd-u^c71f#tqQLeS8Yf&wTguFKck zy(5TCJW_u^1Yq<3n@zjWg^$N>aW%ljZkbB`Ncg@lcpnCIQPSkf_VBCJpdY7u@kQs3675HGf^0k@8 ze1iS!Jqt%cPnI!n{r1UH;}7m?EaI!ind+x2+qUb+3$f`!M5FG!gbUDzIN}x6)5Q9J zP@5_-^y6N-WasJbYFZ*F`_0n~=UVH-a}Cw|cwoYK_~Ch|Om54?Gx_H4hyu5jwwJsu zI40r7^monjOFsg%T`A~W4e$SIeoVUb%dX4)=*Ft55l+zQj`wYxx=ykPSZW&4yCDo9 zY#Ggof=oiWumil%arPsnT!-5mvt#cOEa~xfER(sor)R!x9t3U-3Jed~%`56<+{@{F zV@E5&h|oMD6IEmO+Lcnq%LUVUi@JTb4lzGBSnj!d3ad7ML{VMvJYBz+xQY7#x4j~w zV>8AX1o7WC==K;@4^yCaHP!+G<>VH0p`Fu|ZCl@90xO3__K8*YgjE0dkjEpR?ZK^W z5AwZ*@4xthlHFJ*b*;|;yvcil&gJ289KAo=FZMhcNO<#)=+qsM0ag%_Dn{&wX(`IK z(9NGeoPMPbbon8uO~kn}7h1*tN!K!?pm2ItL9`MD)rl~%D~k7hFXRqC1=fUrE}O$k zAQG?mY>X=ph>(;*l}hbq0v(yA^yAHnc#%cR3rGtQG7%Db%l)Vn8>dmt-{m1HFV<}T zJAlg2^1?JNh1zzWXzchy!|iAAst9)Q2atSRI9hm;)nSQL>#qcIWK>i_dU3#^nJWi&6Zv~mEZRaGLiF>3N0CNwqCKR7URxIdK!!Nu z_gt~3_%&~?FVpqxN*0>uYb7^RHq#zO@;HEK##z42zI=1D=8R1E;g3REOrGOpPV{7- zwgHi~o#W?(MEXl(yLbAv$aq}bbB|s(!9mu!pbEzP@MOyvK!Fi(G%;wUd?+z)Xh0KO zUSFSN$47>X5BwcmxR&_KJGO}ucp>q-bChrf1!>@|f%T6|OUYpCF*i-!m881+T1Uj! zd61-w24|;Pm9#SrkGD$6klp0$})r{`R+W&dg6_hMO- zpAN5Z4GOv3Ojfoj_I3s1%1jyhR^C{a=co}FRfGu&-}Z*4pF zfP4G_6~%<#Q7SLy*<=p$l(5oJg6FeYd>AG7o2xRLlBGM{0>PJ3p3(A>#y}1hhTp0_%uE@^j z#HW-D{*6(}#IGJcK@l$TbZy1-CCYc_udTg#gT-Xf*eYQe8EG3-Rc@q?i$Dl(Pd{@L zs1O;HAMCF(-Lj`KiwvP{uN$!Wh8~elILdcuVL0&MJ6!jZr0h7()PB?6Yt}i<%Vb?7Rm?`ZD0}M84u_p?ph>sWy+v`jC>Y;(CX7i4HaEXrP0?mA zHsY;Pd4`o?uMejx#909GzD|U|H7_ErcEbdUec(VZ{30#YOj4LShYWR3Q2NGCZ$C^( z83&Vn4LR2rzXlSFj(@5gulAjTt%_KA z!v5)2D_y3_TCUJe7c6WP-=@5{6Wc_1n`uOJ&F2e`N zFF(gcgj1va>x-8i8u&)i+{ac`Fv$TG~eOEOZNa#y<0)KG&qiw`zp!7;t5x&e)T8f zwN9=>u0lK?uvE?qA!Q(DUWU26dQXGSrPjrT%q4qGxD5iJ<>%TuOP}U z!>dZdt(s10VW({!wrB)_>nvpomkSNw7yq%dizt1&T;@RKc+=~!?4`2Uto zcWAY)pFjlf?(Q@Wfj7vVK~-#jcc@GVZPC9Ch1pRzrF2$|1cmM77Zs5)XEHH{3@9T2IFFQ_@2^0cy~lML8o{I4gEy z)a&XrI9_0o+ButzSLLFFBH;rc%A;ciD*pK}LJcI2ji~68d~F@?HoxIGW`@0xi8MvC zE=Kh?Y4Uk_PZk0Gra*%|05c(BzVntG^mw9j_7`|a>OUjHEz4td?Y<{y+=X63k5z=+ zxMpxQ_a(BW?<^qAzKt!T9$4Y06gM%up!1(w^~~+^oEPV(i(cL%mY(|5Xz3f#jKC4h z2oDxl{0FW5M3mIqT8OZIW*z7wak@S{*4I|#hzPqzGB!1Od^f9=ELF*0tn^oYEhx1g z8L`#zU@K)qR%$sasXmrJ{Sn0UZ|tL$lkyG|g3MCJ>DF6&s5zwJ;TG6TU1s%~szS|v z6xR3E1pYI!E<5tScHE&(A{ABA08kk@y$HI`iEy8j9m!!6(@Z)zp2O6(2Qn)V9oIk} zm*{`T?NNZ~_It^&K9<%`PjY||un=Jl-N^-ksqqtc;hJgn|BA3m7DJ(xkrLj|m!cb1 zqqNw7fBPepd0N#6IWfax=>7SRTT;Sb_zX#F2eB4L&k#1mn%l6ayu9^#unT2AcpXEg z1-ldj6yEKVg*I!|mx2}-)x1|FD<^Q3WY%py$guKYwqoz+rx$|@!=oc?)`dn3Ik6dv z(RuXCW|h!7bX(Jzw?2IV{sTMIlyPfk6OF^w|M`R~z^$qifiYZy)VReVp&d0 zGXgy^QKuvVIeBo_l_b781yc~|ER?J;^t^&35li69f}!pbEpIMOvAiN3zBLbga>J?` z$s%?&6VnBg7G%fMMtC$a4Ysd!Ytj2JUSr#jc2(4GX+D1eW8Nf#%xDITA{hhl8RC-= zxVc5s$cs)4U^R>Y;72!iM4?a+H1HmL_Wq(Gw#&! zt>?!121%N*?+$=Rf&ISD>J!^`Xu6xZkK7b~EdVb@c8r9S=L7H2v#@_3V<;6pBd+{B z9=Hqp;rW8`1VCh>Z&kje_>%)D0 z9$Mr=2KR8kWoP#U&y{e&hqA1&k4{uxiP(drK4M?nb*v`3+Amk*5~)1iiivwYu+WsOc*gVItk38x$IfeeVE)`Yd-IE5a%51X9tvSR0mKQ&yyAN>=y2;m7Y zaw_{1#J)+0B*_8fpxMR{iWy^EbsYIP%7U?w@*$!B zIy#{%Pj19MEtD2X9or30U1L&!z^W%K#;}q|AO)WT>e*TJMG_<`8hY&x?A!$^#KFTX zDCe#(?CRCoF2g8b8d|){q&!l)^a0BnEy^iHo_FTUa&akN&7xntCL5#dyGzRguVZ5F zeY|1;(NoR)F~d>yU*F-l#vedVIZ@x0~yv) zKHVZoyJ8&Kn|L;_cR9gI4Kb%WcI<3n)r{S3qvL)h*h^TaLd8qh){h9k@jHeJq&21# zc-JQc&MM;dN8VzeFBzlLcZ1xa;j$K6ddLs?aR7cwEeGLZ(8N3@J0*3>pDJRM_kire zDwgcBjus@R3T*ylWzw5JdU!E>c*kZDduQ;s@sEap@a^4#ZegvBM0$O;|Ab;jczX(W zcO`~Y$MstTkHgl2&uAH%@<7b_ks(>=XCL%-G_Q>~CR8M8y*Vu)YB@W`C7Wjy^}c)@ zr|>ebWmD{KD14QGs_~B^{@QDI7ydkZAEOO|gv-O_j>yIh6Xzdd*u1?s29Wo18pn2J z0`NtyftR~)8-YtxZh~gHV=1Rl3WO>^Ky=~m`&>9aAKAHrm1EN%qse+igKG`z!>jX} zZ&_NMtVoHzNRn1TX21DFOk49(eGjf#$^MYSdPL2u>feq9NqA|KF&HfNQXQ zGgM$fsL&p{H=0>&!A(Gj9%mBS>0Ee<0yEB6PfiC>G})dpu332EU*ak2k;SW_duBLY z3$_6vh>2tf8yn3LYQ)${ZS3$O5xe zvQS$%S=$BV`rW72!dmbj<9>r6BWm#~BQyXaUc6*iATdq<>?d`G68L9H3VrCXNSBtb zM#D9dlx-olE$FOv9}s;$@?&aqy%n=>qyC*x1=GpdksfMHC$|V~ z0qN0U`g6*oH0!>7n3$MNNw5yZRKkxzif+tT{D}bF`5;FJj8xL55Xon=ijC+Y&6yE( zkC$bGove7;k3CVk(EX3QeJiTOO!Y|E{G+)6oIUMRV*@lY!`FbHL-OU zQ^qWQB832XzzfNtp`#-$NXwiFW3TfDfe(K4i98CSD2Y^%J4f{QjS zn)zolC_lO0JmFIVN)d}h#v6S->Fn5I=87s}LKq!v;oE;pwQsgkruxtW{(1|>^r!K^ z&~BD(m{Z35wfev_n(LAejIsVGyPT?M6794V?IfY)O(}&tGLl58kVo9vE5+M~d!bJXq zYAW1AJ^>I+hi@96Ohb)yr-l6WZCu;#!9{o9-G*l$c^ZTBJ(tweYjh+BqE37zp_=xg zKqt--Rk>2eqD+*+Jd3qL>*J3Q@f%|ptQg*pBc8-@;#1Cs=&i^9GT1G!U- zaMwT*9g1dPlw86&@&0!qufW3?^;}A#kTy%6y!0|$R=Jf;j%ZtUh#A{!$?VBrUWwDDmP@kIm!=Y!BX71ab{=^<(;=G1Tm1wcPM-IUYL#W8h3IAmEPy)xl0v{JvV=d%h z$~@G`7=F~^_>ErcuUT;y@foKe6n-G{-^@zurK%+uz!GqYcNS-xy347>jU^4X{Ua?5 zX!Z!vB9g9`m$W@dzk3T2pHo4P*K$H~og)tVEx5#unZ}&zE@i(mAQYR8#Sx)?4+bv2 zxoKM%KnvqS=1Yk)%W-1xFE^8>qafuJAnz>P>`I1X2h!rlND@+LlV$RX#=kSf#Khq> zS&SUofIAv?NrOjUH(ovMVY7KBy$x<>)O%2(BWkc5rgaSX5IBOgL&sePx{ta(G4vRG z?)EYjkqCL=q7fS+2m-veQi`{^2a53CJ!RLwoXH)STtn*24Me0HR4!iFUoS>%?R_^G z{boq{37r;B*1x$p`)3+nJRx0M{^eAqrzk0^MCT`>y37-Fi(dV6>f*6*JMaU)r7Ceb zsv2}iNZeqfZwtCearNkRFn9L(b1{zX*_sk!6P^pEzG`Y02#B?AGlaWobd3j1-BuXez3cyTBH7b$DY} zWNFUgoB={SM5Dqrn{-@sm7zE*`>N{qg+$Wm2{rGM$E~RWobLvkG4BwT&S^epwkP^l zV_twmze}aQm<0hLuCU9CU)od0gI}I(!HvZU3FPZ?ABiJV`!jbftkS-%2rmB?)Sw#C z=hxJu;LBI3d|L3nUrAdA=lpi%(os_Vde(6vbzA4l9v6Q6^9(}uI@sh~U-1){h+lOi zr((pw?boBenk7c5>ljIdW>@}-c~>^l;Frg))t6Np*a0r*2Z?(svpJsOu9}ZrwZy%7 zeWIjY|E|e1rD65s03mvt%hvQ@lBn#6r)@cGt-H>9;}q$j&?pSU#z3L$GKeA3&YYkK zy&~%R0&!niYC9V1V0_LE+eOyuw^T0zeQJCoc@dixhMLdh-VnS*IDTGpd?C-nzp%KB+7V_p9{LTJB!sE>0%-SU@Dw%Sk&?o>Adn42hk3>Rm%J3duVsxGwm zl*|2bj()yb5Nc!LF{N#STyMv?@fmpQ9$z7GL|~_@4HZLkRwbcsriPOT4|&MSmRtDKKkI91yCN|{eYT2%jCz1OzeCJ{`M+r*Kkj` z@_YFf?U@qCZN^FB%gSHYP4}{`I-#NB>aB4?4^PRr-*Ktj4hFfsT_15=APSC@2Hls+ zWMA_{y|$j8&PMGwAH!HPOCp>oEbfXR>85{)rX3f#y4tR{H}2e<<|h^Ejis9>d3B`m zpe*1){qb0kgQNIqeS0FtuAar&;f&AbOl=!K@1d!!PMp%+=JU;v*#LQ6p2DJXW5~oC zzoT?>&4nv?4!e2k^_kucg}LdT^fvZ{JxnUg)1bS{#~kS7!zpW)PGHrKN{bxtHyY1Z zs87Km1h(dv@^udLq~bS7eO*4pd5Eq5VajrWZ0p1i6dYY=Jh1F4`1-`4t5@3fNCNoX z>eG|Z@_uD!g?6(3EgB<=Ikm|@tU(R}$thO+el%-f+Ia>yH^UA?Gwm<1SbgXr5nN%; zU%$lk@z)+GsQ-6E;$9?EKEs>D_ATxYVc@i3m74AC?LgrAP$dtuCM~b&ESAv(liLa2 zAJiZOgrY;LMQIvZf5=*7{Ba!!%pcnN0OzmpX3olxO+-EHx4va-2=$i|!U9cZLPidJ z)YApL&NAy*@OOT2JfykNN`Vm-M;sq_&(uv*+$Egrk1iWFLbLo1tL^td2jk3lSWAJA zde`v2yJNT!%W(YP?I=Nx(b_V&l~&Uo8I8L!=2!j8R76e{jL-8CqFIi6^9F2R&NsRA zLt^6*lP9UD0)L`idTdUSbK3i8WxZIw}3=%_p)~u zbQCyc`I$Ut7Zl*8zxvp+_0>ruR!z-IXvN3zTsxhu3WaFS%LrMV@Pa#cSnwcFE`<>w`F1jMA|I6#7MCPB7WS zd&mI}qe3zln74*NY4j+b?8biJfNg+LhdB)gY0z$vZw0Xr`D$aZG!E7P5Zl8?h0j0J zaSP2rWuG_vV5bPNtyiiDj$iIh9Jlx`A0r#H$aYYCee$lpJ)wtrEH1oyU73 zLs31q^D)d`rqxB_!-v61LvhxN3VjDV^G(dITMyqci1o)dAcMl4r_7$=WYYd~LeU8Q zWRf^k<^!$mwa@X*1W@#@;dhnmA+cYqn;?tUlTU*p?^j zg_P8E|INh9TN(;rW4Bs)j8gji-j2ZG;Q~o|HKWLdS+k7SyL2GSEv+`}hyoRczn(P; zvZ0Nln-_vMkTI-!v>p;$`@Lc-JqU?9(7ik@jYW4HTuK<(h7oZ9(UH$1w8Qbwre9Tz z#VoUgzYKm7v08eVmS4eYzW1&fhyW)L>30vIV^TIF2g$~x`JbBM#~4+SFE}IO-F{NN z<^43>o*>iRZ=$qkR;Um=XM=lsV-~?Oi*Qmx^zCO^LVB8fo3_PHY$O?zI+9~hB$p!R zV%c82nPQE$TLaIEMZUgj6V@PDgRS-5kgb}mpH!ys2tD@Z^0Dil6sm6AK>Yap>!v#B z%jJP#R%6#s;X%wy+&Zk1!ny7jOCkwxk-MkHf)}f)+}DG@x;upi#}?R@!rSt4pRab) zbiR(ygGd6L^mO1*8JT`A<&EZ;^+kWz%&Y>lw{Z7#iwZvR+->-h8&@rREP1VPF_z7~ z{&2Y;-YYpLR6`ymm2=hO2U47I^R2nBdpe4=ss5|zf^=FK6D95mqY-l&O`7^_M3p&o z|EhIC-X4|s%MEJ;sWQMcYj#x({HRf>)5qtue9Pp1E8BL!_wG8|EQ>nAjd|QRpA+pz z;$#Q3ff!K@{egIJdU~2bj8W!-f{oHz(q~^!L{mZPg~W9G4_0|c{-`!N=op=i%M>*D zF`9j|Ib(kNfYkAz8wxfE{h9R{i5II-TZu;4R0=AJI`sfn zBdhqhP{&qrtMU|7j<~VCDImT(CW2p2G2LAcwG?a0>8nCZ320S>dA}z~=av1%`mKrj zn1IDcXQIWmevSXQz%q4xf~NKD+Q8YvYtrHUHCf8%K|)~O4FsD3GQCgial7l#PviZ? zc&d#$<`c?{~9r`SPXFgg;E!uo~OnPx6UqFe^Ojb(Og%eFd0lVT=(-E zm6{YtRRtl+e2ymx_yFEKI@0kh5U7AGfk~WD{FN01Z<0?lL+_Tdx>n%i#tZF9kC4?q z7`;{2=G_3hD=81ht8i1TSmavDHG`>dgf^mJ?!dx=k9y(1_EUk=$c_eUm|%!vd&+}- zbI}MAKgg|wBcq7p7u>^Dn^X5Q(1WujCtb4^jwzBBLP9mv9CjqlvAH;T_j;oeOc_Lf z=#QmzzFFFElzp8^h|p4&Cd4nGA<>>?+furS`6t<4EOiNa>yS*%)*Kouu?4*d31(c2 zGw8%pdb#K7`Ez*ry|j@zez{6_xVrE{E`QyNftbV5#v4oYajd{spffwY;#+?s4K>d} zdLm=;ty=CvLxO^HPgK}C%%dJkmcwUoxh z)Bzk_JDr9O)G8c-us22%{TS-8VKwT7%uT|wh!e|%2n z+s<&LvG*DK%qO8WZsqKtbs=#Wj|t1 zgA5cwWqsFOH;_y+B6Q?$pze!BOIt*(nh2w9MzM3wnyf^xiqg9Omf3>%xG(|TK^)!A zqKr1V5Ce)Dz+ohazcGBG&J@r0lQf77g>i7Z0Z|WPV}7sxLK{fzBjHUh=;V#IG;Q^Z z|6>2jtEG-tSoA}jd4RZ>kLa|EOEAKX{B*kwb2Sn0+$^Sc*XyFC<8>Of5_z+eNM7Jt4Yt#x`<{*~mX*Be z%1Yd~$L@OgEc&M_=RQ?WPUby7i!w4H=D!=6i8y*o{GzW<|EL3`qd$ll$L-!H#JB-} zct-CPMnSuO|7g-3WnciHK`-f;^xO0FA3YN2)d^V=2{a;U+j4v9-@J()j|+$M(m%FZ zX?8XluOXjsK(feaZ9L+x%OsBT(lmZ#ijfVMi{mTY43G^7BLOFxWW)xj z$Lt?3qv0dCo*$APHXvm)!#pWgW7HA8Hy}rB?rze`n)`5%uv5vPOYu-G!)PtxU#>pr zLhjziNJK+54r?=QQ?MmDz5ikE!BJ;ND6>-3^NgIGjRNb8(c!nyr9OAud4HvKRZ8Z< zW+S&6X5YWyF;<-nsqHVzRl8z)^L0=FW?lc+cN^wXObgoanFiKSkWsZMJFtukfml9b3f{FfIDv-|2U+fs;bX zVM-_r#MUuh=|2+e#r#S)>;B^@0pcuoovq)P$f?wu3B zeR|ZG0#pmDpS|CErd{d5&Pp(1k67jnS#Aw;Egw1{B!1C`;IeqoXwMG#vMC4MKJAB7 z|0LC+c$$0=?!A73pAK>t3<;8FuReh??!vkN31X7AqXeS8Z69dJ(S|d7e$sB%9qG79Uki#&NH&{0&na80^dv24RE~r{Pw3niW+Uz3tkfCu2AIiClYFDb1c^n&qb7D=xQJm*KGE|w#)V6^ldM>)o=jZOU6&?_M zXYn2r0M+HOsR93o8n?m6XC<;h;1xUfJ<3IwkBy5OkA zUe>6iaKUyaZ!&$6G4JIq!1uX*RAdB?1KPL|lr|C*m!!U;!^d|m|XIPq2q{{&J~gPi%#0WVd~ z&V1S$Tfp)fejNA4M<+MaH2N+%16+1&A_CFbin&cENNX0@4Wmcr(AD8aSZAO!ftmj@G?PK^Io9LfLK5oyQsV0!>j^mg@a=&7|b z9X{jVkMf7@A0wz5r%A-bR@A=+o8~LhNqcx;Oii)>6C&_E>{YQ+RE04yt1#b1SH_+* zl8Hr^5dtDK)MX%CijFGrYg>70XXE{DiTF!Y7_838X8AVlPFEN8>sr1wV;8==L!}YZ zI&&@G8SjODz?w{Wb5>HIG(5L|Ilw=~283+8D^0)Fh}S79MoBJqRC22;K0p8c#3FG` zJZvbm$%P#q!vsd}l^7mjpOF6znnDs)kzWIWnhIXRIUN~0r-horiUqT!v1Js(4%ntU zbne_@P%&eBQ-GJ4pJVtdck!@z$qG_DuOVdCwyM+M@uMQsBoIbYz_Nl~wZ4YGv9`zm zMw5+uRwYdNHgR-{f7OlKW|V%u%f4&_ipPUcGdyF1A zm#5V%IoY1}jTJqd6&Ds_=qZ9>a91Ek8SuhUZ{{AiQ@xuhcJ<#CnTROG^2Jb{d#u23M~u(5xA&oqtoI87<1giw{-oB3{c{KwE*^(zUtB&ZX@)-5#bGmjG!6OLyOAc zxA{4YXfqmWLkmZg8tT+08t>e8zeP$F?Cn!wYM)#maS}&X9>}7FOUY9Fo1%dEL*__3 z0W=s^U>;06N%&dHvNi-e2)y%Zv+8X}XWdk9R`kqP)NQ9|TumLa^eb2wB5X$)C1Ad# z3>T7kr$@*qC?kqpmPP#Z4G+7E*qp1T4rSuo)&(dtC@3@3?I#-dIzt%~A*FE*Z*=}V z_xJeZ#2I|bIEB>R$#x}zemZ371(TUodJLU-1(HZmSqCPlcehYOlP>c3@SM;_AVzjK z68--_+5?zOGiCf|GfdNfQr(q!V-*+1XXHSPJH?NWo}%gKinnrt8x;wFuXAX*!WF&v3bUB<;cjz)c|zMzk%)O?}Qfc)Qzo;?Jn z7i-?OPMp><)Z2+3=@5FU>&QpNWvnLH46>cem+|e;prehXuiSs#d3h zv%xI=pF^xj>X?%haNT z6ur0neVp(5C>CTd=osqQQ#JkYyfB zGqkwKzpQ6rVwg z>+>Ev{m=TltGWT|-u~4Fk0)$s^7+6eAYy`PNo|Y>0k}|`^~v=rb2{7t7Y~(j z;3eo0Wc6!}uWb_QMser<6~`gFkO?rw-el-;AZ&KB%CTGcA3IPm##$w$;?WdG@5F|O zy~B!@%7OgOMgO2+xKc|FHg1@7p-l;Pq|}!LBF&00UMNOhaqm()l_{ZG#f1+~!y@)k zWxdbap%T2Lt_%xDlB+t&g55{GOVNopicn{I-5 z5BYi-@N~|KGnnGzzxrDZl9|M)tb`~E^_)k}E-Gt}8Yr~*`T3JTDK5p~-NIyL_nR@u z{4ot^ioT($iiy3+hTGzcoueZ(8RVu!F6LeKChAyV;kO&>h12 zVPU`Ls=$^lpX6~MoLZHYfmK>oEj2nnA3>|(Ti2pr+XoRDO1RO7UEbN9#H;?)dJS1r{jqNK0_CZUyE%jyY)^?S9^` z9gmChykX67K|!GrG@uM~ArGhpE%!YC5k|O~r@!{+SU?6oh765Q2qNM+Jre-s)!AuS zQxD|+#WGCuV*dEOy3UH}k#dc0>@_kpdOF&zr@XT_iknO^VA2u7_3>k5IT5cZE~a)e z$oKv?&B1hzhN%qVU|EdL)4ms35fi`*3v0Di?XI!pZ=II367b>;3#r5cL5QeNZ(!)M zkjK7QHe~EkuqpY@<-oPG+hAY|q=4-c1Jj`L{k<#ZT zJjS{{TBQ4vI(kd5win%ldll2YK^kjo#im~4DQq2giB6pZH=Brbl7LU$pCjq$K1BxF z)4|VE({=w;qo!N5Kv)G@arN9eb6L2$xNB|nHtFIGi=9D-%R)~0^!|zKYnKo2Gqd^z+Rg~q61fTv_w8fG z+#GlBn0CiC#sRGl^3RP_6WXHZj=}S(d^$86?G#Yv$5Cd^db-N0fS!z8`)oSBv{>cS z6;1+I9;|~=ah~b>HL=FgrmCvM1QeRYXB0u$4<{h5mgmB4xz4bsx@&KHyMb~HNS&Qy zjE2&M-rDj9AGt~914a};p;+swx_FV#cMY71KjS#sgR6hMwfO3Ail#!smZb_mR`93y zFhp)m(d*Ckvt65~z{7J-)WFqZ65aZOj%IoQH0f7hWU?Z}O$*oG%pxYDY^^CjNbQ?r zNn`TwZ2pQ)buV-Y8zma_SCL@LJ*y-5o{9o96-C>;KA*9c?_cKK+)9t$EGIHO>5 zm(fF9U&`@RNcBsE-j0)>G(4~AAP=Z2PYW?A(`KjtNDd<9gI!q7W=ogi)jUuP+OxeI z4(|7I979A%PPejBT&eTb zgymJ2!mF9beTjz_G}@yhq!L{cFth$+(tn#Zs`mEg*~4}9@wd09h4Xh=Mon|G>7kvs zuE+lMv)iiNu%;|giRwxW;^pVUqLxns*=Tmd$A0M4ds=80=2N?XoDr$0+0Gkpi`sRELvDN3|84-KS2ujL??2^TR19$jovnobl&kXv^cABSLF--B*S zGHO7Z1T~V!p8Y$eC%Z?~^#dO3tS+(JOyd`tRm$5?KODaUC~kC@^4B`p#fQvbyV z*USc{;!Fl*bdNxSejZMT+Ssa}MEVk!#C5xl29g|d)Vg}ck{n`vg;D$LnQP1W;dz3K z%w}Kr)^h$qH8HANu)_40mB!WR96R3DL_u>u@~r`~%=P1MhL~RTcTe}WqAP(yA-$F% zEI!hu!#%c7c8-2tRChtbx%DQlOn;*4v$_Zak`mmD7J6>%^Y3e7-)gKEbV^wZ=7^01 zf44CU_-Xi^vEZ8$!MEEl(hbOKyl1w!4`+r%?3b>qq#tIvLJx}v`tu>x(6i&m!|et^ z%`8=IN^IWl?r@qkJ+#X1FsY+olwh-IR}o^!iPUO&l|5v31Ws0RPR>)QoT5T0$o7@~ zOG&E$o)z+3yq8sFpO&Ovm127URP9|56P!|Iw6tS2SLTcd7jN}OoOkfR$;k^wM66{k zYR6zK22E@_?5_7;=N!F>J(OyvJz{F7cmOBz?}V8;2oSIQAJIjhQ7y{|3g^oTS5A)p zKeFC}tIlBQ62<}tcXubaTX5F^!QI_CxVyW%CJ+ej?(T%(?(T4K_|AQ2?poi>^m_Ua z)KlHnRkimnxhprFF^Yr|!*Zx1^C&4fTv(tF;-&1a5zY6oy^91&0O%K~@leMA%NWhv zqPcH#Q?9IP&b3`lnG}cU+O3i{OM>yLjPTGIUy}2Y9$6*t4PT4?4G_n%z6bYuG2ZB} zzqIpkmNa{}-nf73ieU+A>yEx$mC+F&>Pq5_G`G`O5g|t0$hrMRbd0?9z z;f1A*e&vT5^=a)%Y|VPu>F@S@ToSKjt%JH;)=kY--%i@vIMDpS*^{ewV)#i)`G$FM zuGC2K5Tmz06vy|hfkc0llAwzLly(-n&M|$L>WhCsSH~_aUS1@Van=6C+Gx3o0pp*| z-1HL+${`z#zr-RMx}YI<`^a=LqtqmKU4r|_HU-480j->nib3QyTTx=Dx~Qw0{}^a( zXxW-DYX`-so4MZuI@sfX|80SgbbAoPGB>RgS5DO1HL|#_u<z-tC@wwC9z@(|JQDiy#peBQc;U{m-^*O8DpWu47M`txG;fmFQe#(M|vQV@~TxqIx z?sG5pj^l|K_!U3)UkoeKrVbml}=(KKozc^rs4>($k(;v&WZ|N5e#vW;e`tw>~NSolH1> zP$;~vK5D)73O}!WBYe7eIN}fjZsfUfFmt(_rv>aU=hbbvu1O9jbcfOUKVu#a#UJ7> z*G^_T^`7|lC=#bk(j4lxMa9yfHhxiaa?CPFDHtUm(2 z3tq4eYc8imX5H}2uq{_}ub=~0npbAJ902TYM(#|-fd+rG)TwAmSB=)ST)ZgoU1 z39pI$gQ=^`pPNrOd_#;cHmBt?yuyoKRys2qGkv`61(Md5>Oy1D>(V$~XW=>>#v=5S zZuXHnrafJLDz1isd5m{f5$FHoZKKtEOZR!+@t0m{nZ!gO33frl&Wh-OJ*h@&(&C?k zy)fy3%&)MA{q!Qqg`J(bJJ{G%DPsn00DG!lxS#tprRA{1(OjgG156U)f(E=%l|5O@ z{l@rWFUE};GOXD%k#af9oURC^)};tkOhX3T zURld0vb!$>+C zG}`DMlIM%Wym4x+eY|-+0!trv)6vzul0%R_0 zRHGPL)9N@!Yyu2$-fXE0v6pR|j6@J+yWBb7IoQ4}M`a#c9YE z*Sqbo^g54W;i%?I>tMa$``z#m)^nW`&@)t;3I7Az6h$yco<$(om;@5soPB)J^|F2j z!`1l7RJlNC)7?){@kE!RB9?b|pWCI+R}J1kH0<8B!AZ>SK5EMg@ids@Dgl>+N3X(X z+_$^va?hA%_`>g*cSfKjG|`iH^4eHQ)ORj#e#6vrP8jmnTr8BH-Qa&;*8drCo65d*3pq&sucXwS>HA zM4E6%vwX5FywBqZPtE%`@=-MZ@ly5nlHL;8bUx#3$QN&n zji&dC&LZT6y43xQj{9-TeuyHg7&3I;oM&I(byRQPY-q9N{)>rYI^_I$8Og6#xTYh| zqx02ie5q$}R4!?Galv;}BVK8p7!3BsYAzTKms|qAi`&QYzsc)ydI3nfa^fbI$Y44n zS4Qk`Nh03vw&K?!h+Pbo1NGgiFD8Ni@CJ{PLk99=^$UAH2$9_9yIsC0?yzn)bC3U+ zc>c-=Yhk(tlnxe6)k5lF+;i@j<3d(kn&0YxC}2*Ws8L2~QZ_Im>>3ngBh1ZTRwDxz^AGvVHjFXykBc8(Pkt5S3nw+*NLx4IXBm^Rc zR3Mm}cL`5`qrM`!GwYfzD`{Gq21>t`RH8fRiwK%`v-xMiS8(}q(Xe2~Mmk*u8nVC7 zXnL3Gc|1@C9^SDdBaN%B+b~`08CMD8WG+* z0>iWqPle;*iZ0l3jcGF8KtE6_Pm0{sLl^H1x1$u)jK{7 zmG0kg45wOfbf>s;{h5~X|7Lzdr0w2h0P!KmE<~j$i!6kYS0*B1h9?ggGKj_9+9b>l zfb0IROzX2mDE0!6b?qd)Ta@zr@{O0&)p zKX7s$r$)`*+e5A=4{~ zBwCJFSjidEUBm0%DUX}gaSeVGSTa?$H!{BM<3 z@-bo^IHg;uvlhp!$p$J^eR|=G(CPODd44Sy=FcM)o)&x*9`HF6!!yC30)9mNnI^Th zubI(vXRXFoavsr-@$}Kh!{ljFRXU2kA{JMzTk$5mD9fTjJDL*K>(7Bm1uoBgjD9gq zEjCqJyNCMx0FN!#YYMA{O`X~PYiKwOnZ~cXmWJd~t$=dIGbM80k zv&*F6Y6I9LKljUj1$xZzJ?U z@9zczV3qW`VfQN3?kv2v-R)>ADCphk}VOI59oZfzG+g(lI+h1C82C=q%?)^oFBPMc3WG8j9XPaeKPPFqW zD0RvltB|v|c~&T=pHI@6ywR?HG{Pofb0yALi>s+*yim`KDn!iuF(Y9dfr;CnhXJ#J z@g#v=K(U*yAR#Wo=_;W@9RwR3F8o5ya27!x4X-dCS4@W&E-FAWa^QlF5`hi_sjiOx zqN2#l(PKnYF{7yy-}gg8G)HeUWT^8CP({O{t6s8SZ+SBW^qKYKyXf)ts^7_rPtL=m z=6cs;<%64+YE=vYhJA360z9Mjx1!BzNbS;*4Y^yLM_-c3x&3aTVyVpX`g$@>ABe_L zOd4(7{RQ1ORhTxv`Nlk78+po5D8|oraDC!L*Xtvo=3qK)fbVd9B2x$baK7y7=+c&_ z@B>62@0dK-o^dxvkd|fG6&pfmwoABmP~`Op44EB$$WSOzkZIBjU=)4=iK@krc~;yY zI%`aSLI>zEay)ik-82=^{3&=vecZg)>|FW?PRR~;S(miDo@T*yI#cC%T2UcL*m%Js z=3w5He$}L~A%;jL^<)#~?S{u!rnenypgq}BIdA>R#>pudMS@yye(+5IRFeT@T<^K*H+JfZq*T+ zyF`VkA&08Pcgsd7)iO?W-_MBk&s*G3)IT(do5@jGbsL@bD2O-1okMoSF0cV_JKC{M zkP<4Sm|9qDF+@Rs<5SSDC@MSf0#5lxAK!95yPeoEmFy&j3(wx41r~&o=UGU`8a}n; zeAEUiQ~T5h?)RomO}xI4bmdVaUhU5HYz#%7^$aNVNLfwMRvtU-R9+EH09wY=6AFwr z`*WUr-mji-a%~1HyYa`kh2A}(Z_#m`o@3@)H`Zy*H^98^C0u|SXdGAOH1?(U%#^=e$@Vo|zm~WT{?5;-eXF9F7`Fgk@i~WZ7 zGlOupj1@JezViruhe3Uq^7nj3Hh9`6o6glpbGvB{&?gwqsA*69Dg;SI>P8ZBT>sj7 zIDCW#ddI*m*a&XDQN&pf0FM=Z5nA#G5uXqh#?i;9|8` z%fW2D;x2uh=|jQ!N6DjF^({2zYLe<_Tqd;XgZc(J5vLxWFA-z=_Z;IRetB$74lvN( zrA@FH`HGYQRYPfdl;5(uOQyL*jgGr%U3NayQyJ(9x3XvSKfAK(*?Nu$P1m27&W_h= z3>wn-6P-_fhoGQzM4wYV8wqbSEA|vD%=PiA52bh)e7&`ocMqG?3u&gaI-+RYT5Cz6 zNvAC$au7vrB7^jUQm3vgQyla5id4PiPNGJWCCibO80-BATT03H!3d6`D);k`aPHUu zmp>;Cz#KF%>TZq`xh)dcKt%mD6n$;Ef1AIc=DKeTS4=Nb!MRq=Q8dSHZXRU78M_Id zRGm6#Ae~$(Mg{7dKq3V_dq?Lj^T8vAFDsDH&wuj+$RipmVZLYy|Sfpfb5pdO!%Q27u zp@6R`eqBMR)q0JH(qm;(;U_#$reJE*1(PzYy3ihKyXl+a7GUm#lMz9 zc`t-2*c4@K`I%ossu2MbYn%SGJ3#1lkXJ?I;X+wmYTEiV`S>&x5^%I->$-WKKh_e! zxmNKn=d?jjDD?2q;xGE2FIRE$J_Z;2bzmNP@u0x(yS(x{?>CyS?Grre9Tzpx- z*_QXM@6H2|bjo5%4b8bY{OHx^NwyIx5-b+4j*W7`raYyNo<n^_8ozepQXU#OpDnGW8>Xnj-X#~IJFnS63q^J7myMx@Oage8XFk-f=ICS5g{Out zZq+M>k!hnBEMX|sOrbW&hl(8SR;Mo!eztljN;8`snP=4Lab}C%AhD9)7oI7PM*$MH zCd^!4fe-jT-dEi6d;xZ4L*s1oS==iLy;Q^AUgC%%-?pd*4aNjd>>De&I68(iM*aZVyj%pAx=++1m4OgySF-WYsy}%Ql565Npuay z;57|LZwB;wJ*D(ogHZinaad|zF-oM%a2SnPmc~u&%_<6HS2df0Y#A|a^!Y&YrS9vj z0*uDyx)|XtY$J@_eYB2JB(R=u8g~z`bxl)cL1dKAV_=+8G^Lc6rjH#YY@E%7`@cPuFLW0rj zLq0QGcIw=dtEA@=AcLj3?nI_y`7OI6|a4z=Z_WB zzo?3}DZ@td@%05c&Xxfg4ecM(&^!iXF;lOskF}J%yW3I!I>KT>g8xwLi-z()W4{}C zUwf?*Djl_d^7RpfsM4~Dt&b)g2@amoDjjC3Im`U=N8l*?u0YyQytII+-}%1$O(NVc z%TV7_p@RL@^OU=1x`QRzG@fW%NaUtXXsPX|$F)Y$jmIoj`!`HwzHt1M;aRc)8E}o_ zSZ^S;JgYsR{q$%VCrZhnBj0?YeO4NEti#}ClWfaPmT7Y&`q8UPX#JENcS)6 z7vvRAbXIf-RSYyFNW}&r4h49_C5zy`dRKo@NfqTEo8G}>WB@3k0#p^Om3ni;aAPu@ z>tVvO3NK2-g@4{>_2LosFn!~aTG1^=+@*Z1j(xtHiRR`SnGI|9@WI&@T{B3iIFj<$ z4U&BdqO%95@GP)u8Hm3moLM<$3YpLj)%6hP5OzvgKr_0v5kDA+ub$0f zLVa`$#B|Xnb2*4boz}hz9u8B4WiiqmC}a zXTnE8E#@?SXCsr0xRn=~f;~++7GFt7vVcxE@{Zfvj=+N`CQmGu11>kUjIl1}?JO%o zoSO(?CNlR?o~P-sl;5hsogQaylMMg9%1R9~CQ{0XBulc!{Z);%Y_=vihK;P}4J1Ui zjT7606G?DKA5iSQD0aTIRtVVD!0B{GU|K-*vN&`4-sQ_^eu=U^cpQJ4o>%mvxx z^Ce`#4xT2di=7_f=4X&Kf?yB#s;Oi};V+H;<4Y=w%vAF=!6jZ$g8AnTlw*yKXc;%R z+np(>QFey6aqW1e@*Oc{Zpck^Wf$y~#N zv?~3?peq~Ap0WJ}1LwgZb!tOaL6zBrCC_S(n|){|KjLw+jUwg#hTCU`Rg+eV7LXvs0AthDb@e(iw!e5-8&ZqA5Yd zl9KQo56vqzU&c_7uiTiCTrXk&P5CyDNyKdQKX!Y`Uv>X;Apd>x+jbG7DC%TDK+ry? zYwfg48l&meFV64^$o@yjGepzCJ4x67j`a?;Gmv3gII^PIyz+Z1zMv`Hr9K<1^rgt_ zvlOuZAYlJmvEb(L4>;opCe#uLU@Y^$V*gHjbasO~L8Np_lkUJ;&hP0mjE7a8&6?(o z)8MKaT=EAvBPv<*r=qW3QQ7U7mwqtznbi*f%=Lg?fumzzI^xTSjuRH_y;nskJE?iR zn=kMkC6^}ZQvp5}{i=K#Sc~Udf}k8MQ`#H}#?7d2W{NHKB!_H@`sr4XW+})qa&&T~ zhzW$a8;7`k|HjLkDpRDqqj1$LW_SI%0x^jcQ4V{mT5yCIi(UV*`kCQvSa2bUH*X>l zd5=Qdd z5Dr4h^gUFg;HLn|D30XrO};s}r)C}#*c@TT8GTAWbw-(?Ru+o&GkaZ95rG?=)BD09 zUvZi-vCallLKdR;1gt&bXeSDU9%!1ap)3os9xk~PhT=33&T@@j^!ci&mp-^#);eL*|U-i;x-gHP>4LyT{*^ za`J;2ZUG^ajm1Hf7DHtYIWOU|J*Xpt6ZodtGf-(E{PI%v+!Mf5C!wr5$)=$I zqYa}ftrW-8u*9CVHzTt$tU&$9#fk*2iUvWfzNBjUZ`8Av`-dspo}URe>beu*Ovco~ zAKZXE6>KPb$MxYUrIbyJ>AIydN3;8?)Vq<>9&(xVQClpbk6+89QgpT^t4Wq@7uS1F zPx)Ied!1{Z?&F`P=}DK)QlwS>1mW;V2eG7nF@D}d%kZ8y>v*La{|vC~DI2aZ)G2MY zA0B+0)y-YxL26}lpdo$?HI8DPab8eYLQB5!jL_b%(2qLb;QZdKifA@Y~OvEMZ9y~TtWZRLfYb&$ytuYh7mDP z>t?!NWc|-=y@De>dj1cQd>0hW&u61epA-Ci-`%UX+a#6Tk9kt3zt;wQ2rsoI(@oP$ zAAJR+>%IH)Gkb8V;oDIGeZil3BNjg2XWB?I0f1vFH^Fx=r8!P$Q;0jcfJVicGl!@Dmt**9d+(5Dj=bErY%)LAX!q`;H;iIPA+a`ckHzQXJ)JK zhB>q4PRbBoFu+p7AGB)z=)889BSQdN`gJ#noFuvkcX?(ql1%dw>dbCL9@QOqo8Np&he-BVXhG7%&E`QyfE$^Xr$o!12LiA z)BFZe?d1uJFMrGm4;z<;GoWz}Cj}knL(3Du3NEoNa+K6J$O_Ty}NT#{L#0}|j zACuSl(>WGzb?|ye{MrN_tJGM_L!>mzf9tHl;ELl#Z1MG2M@Y|c8=pVcWljP`ZMQTe zP4{0co6SXW7D35k;sTp1v|wxcK_2VrQwpEiU#IMdW>y<3dX(zX<+Bsr@*}1A55n@; z$f02%Joo8cajXhQc+qU%DoUKl1nc;6Z^wksi&eqrqKB?F31dkS`5jSJ*|>SSQO)1g z(RGW3`++R7mb>$ing+=!0!Q7g-M+GBvUvVnKie>YHO-aV9gI~gGqJq&5gN%?$^}+; zfGe}UTt~Lw8~F6PZwVr+^1XDDx?HGBt8TmqB(TJs8@-m3wI!**OaC+Q!@pk7Tm2k8 zjRSCX9UX*cvr}P04TNJF5j*nJqWf`0(B#Qtq4Pu- zALzeeo|Or|{wg!j8J34r)2GdrDJ*B!Ycl?M%bd7cE77+ZCO#lBuz=YKT_&CUYvq+{t8S_HBX4Co3oV0O%Azm%-d8Z8t#&TYv zGx>grnv_hr{%>^a;iBs(^6bb+1Ghv9K5zuE;p8eD&5%1&8D39WI4HK^)y$--Akvw! zh=rS1%a1B*R%f{E?;nd58%C3AQAiF&U-4 z1p_wp_*`l`QI8}$H?kvai^RntY(%V8G*^)0K8!ChjtGX&;|dN>R~zdOecgJ%r!hTe zXSfytC!U^wx%N@nZu;;m!?^mc7N9iJE?o>#MNGo3bO5RvW2VMu=dozfINK zPkm%08jxQS%ydYEusL|oV$%dj^QMi(C2aNQG2r(4@UoT1YNf&k|I&KyG6h*bkW4b^ zMnU5>m*rA3=7cA;r-MA9SMpu0YXk>CjKN&sDM%)62;vhai~+`wED@)Cmzhbe$k4#! z4~2*B382Ti{FE$*_3eqB0q%+QJcLIW?yB%^Qvzu3Cp|_o8|iB-Z?tJdPhW7r#$cZPNUc%>X|b-g zV(j5{NVb#8x|1=-Fjw&YysmSW}cdIr%T4i%bW<%C?N+ShMcBoyivP z1ox<$lYYskV?0vpN$=!BF4W0rBo@0 zPGZ$q-9VA+z?E|ZA+Hm}btdt0ciTo5eR(`oQ^9_e7Sc@%DOe-}FIRFFdDX88&FG}= z5U=U1pqJ?lh_p!o>j0AXwY|54EB>-ceCfR8pFmZ(g9a3Hm#|3NWwu<-OU1(tcgmI) z;*stFsM&YEyg=HKOvUPe=x;b4^X$f)iVFsz%Nprjw|ERUz}Z0?Dn}+-QiGYRf)7F- z*)pf6L2rREhxRluGR>VfR@9!uv9lKX+!7Z2O7QbNCLMV)Zi3MRQ&O%Po({!*@l2V7o?nwbR6hueg%$`khm*JqP$STL1YiL`*xd;5~62NcR ztsQRDWN87xBF`HT^HCp3_>sPU-?mq-j#0URMhz5NKlIDwD<9QkITFf1!PE78OZ!vl z_n^M0j#O5zM^Wd6U&VH<-b#MQd%vUsEwltO{TE3NsOcUuor&K$c3z-c_2y45ch1CM z2j-VNQ1UtDPUaHwpn z+QN%hff3t6?|T9*`Tir_MA+5M_zrx7(sXB;Ig_=MmSx@{{;sad0f9grvdZ#u zay%r^4T&A&F5fbOZ?b+Y6)YD{2y1c>9rktRa$ok?6L3j#&rsQ_ z98Y|8tol60qT_j6cjUQu)8WMixm|2>jAwzOSRB(d*>#T)3U_sMV4bgO)3F*#h!C-UZyv2J1dUB;D4H%nA4~QYw9tXJiPDinB}}r5|zj zicc-K#qxU6aLrK*T+h1LB)sCoEk$RR%G}L?YxxU=+N__H79TUNG=9&2Cf|>s1wLwW z*uvO#P+oCf70RW?Ig}nl*Re83yHub0^go$=jlQ5#Dm}yV6#TuLtEFT)T%}~0?(%&m z^7r}7H6R^REOXv_FZ;EX(7jE%9w1s1&-y;{=oIkcM_Kmq+>^VSz>V%3D|ET0=d<2Y z^(aNC^nU+sU0XY>Y`cju4OqMV#HKs|p9n>FvaXK6B<&kk1AXF5HtLBT8G&(}%J#7S zx%2Z=U=-N$J~aMGO_#b5XA+rV(AMC6t89L7KLIFf=wgJ>vtVHPd{l29>|0|^OJROR zphapB!@pMCU7wf^^isO%6*e4!F*8ISph2M}%g!v77|5OHy)W_ca@Yi(-{k;#_mVjx zr+lJ29KHLzw^RMpkPDJ4x-TXKK{OhmOWm(Ws4GXPd_a|hlFT^eaki!I>EMjDsI0*K zGUs)@Zugt9NqSsX{QkMEE-3R4-+bSF!_J?<$F{rMj^9na?V(3}ujJP6aIgQFejcX5 zY*!c!iW(mDLK7r~VVqNA+fsJSU@XA-ru;cG1e25vcf&rrC1s%N>;QWf1!Jg6{HZtC zKWqxVjO);_(_f<_IzAh6iXd=uy#JKvdtH`&8f7z(J62lR2ezIjBV79IOJ08TjIcSW zaGhXqP zl&4cQ3l#j(b^DzHa8jbLHx_`~Js$9JLj?=fO|Hu8+~21_k1JAPe)&td@BP@9B;akU z69-zE&Jl0so?lbSv&(IrYy5g-Yv&m*zihsHm{Vzg*)bms>!}>cPbzy4Buy0@4L%~- zCRMT}gTr&%|23J*>Kld8u1H|8IQ|fY9*vI5J)T=tPU(!A1@jX9Uc&YWf<3IiD zb{&Qwk2meNeIIp!>(Q9?*T=HTyJ5|NYgv$Qs~>1*HW=vl@fw^iwl_msYPcAQDGZ!l6+(7c)sya;r>u3Gf*vYM-UvxW?e{5IRWZ!<&?Q0;qUMGdFt z34h@s+{^K}Io<%lEc{SX%Knem2+2r~-*^ZGC2Wiuw%;9qy$NelE)W{Zltd$wAOq+M z6XScJr^3Q9b(1RuV}#7`Gvr0*xm*>++1#l5B+x~9m^K)a6-@AGNMuDusH$_Q+=yWy zGgzW3Pa}f?<-WlVErgOPXp(G#n&ux)D!RJz=H}E!2NJOo6n2}Pe3n+u%xWq>v6rnh zUz*0Kkya&2lvR>?R0?J)aP%Vepp3uf@F86rqV)wmq z-VRYaxj$U0-%qXNIv&>fctM2g;4N^@)cj-JL8D4*`sdeqcQ;M_%hi{`1I!cw zo4p$hlT(B!dx~CYt6v7#CHw8z!}_zGAg@>o5tJd+k!y7ju7}W2OaD?X%hty-j7?FDL|^R0G`hmFqh)rB05A@1@2?wn(Ay>g(pY6bk{AvTQoM!+U4W?rH4eneNkH* zHSsi}oo3N&IeVw6*U1oWv-SH0rbZ{O32Dqll90)xG;RHks)7Hun}!<=1f=1B#G&3B z!*oubAzor9M8AYr)rEFvosu>xivXA})Gr>F!*7oZgl8B*m(Ql~MD9w0lAOeeTF zmArAqcqogasF?1!J)SQ5L~a}z8QE^7KCHT$xpm4u;DJR=0 z6A#S|rXMopL>BP#6Ge^6RR9JKl{zLQb7%nLk^b+9iDP0SRRwBisxXn~3B+O(L*nJf zURhZ{M}VORAf#8S1I7(H#<4Amr{6z(hA#nF`r6R1?}175itGU1Bp_Jbi~j)S6GQZh zz<50;C!x`ocNpw2EonIMprY~w4(cgA_Dm*>Lj*yMS{PCFmd*~L#J5BU-O@V0lXV*? zefi*t5Qz%0dIz+5Glj7bM!Cq+NTZ(;A{pUS(U~Ned^90svnLNa*hycCl*Tc1c_*<@ z-So??uhP4uIo!2kR5n5?wogmn)L)y?x>p`!kp zH^DStq73UBSl;p4Dj8W3S%kNYkwP^AC=Ozlb)~|1P6}n$s&d)Q>kccM^#ivP@p#| zM`D+bRhT97_2LvVtl8EyHwS>n?==Y^!_gz@eEatr0OmI^lRN2FU|ITnU)>D3xLEW_ zC|hEO#)h?jyLvgXI+fws|NGEbSY{eWtC}ohM-o+dmRSrT_GLD*8b;X`JGHs{;t&(| zEN$u4*CJxHWe;q`5W6H+LxcYFXIG^x`A3=;aul63I%3AZX~x8I{r?ZEC!{LZhbTfY zt&f1dlgVmfVX)trpS?e}NSbOAiEHsTbl65qJvt;j6w)r)TcBewF*FH<$-U%^nbnmd z35I!2BAuG5FD@2QRUTfVSlw0PHgr}(9uM;i+B&SzKv+Hk6OJU?g&^GoJ97r2;=D}h zAfpNvx@6MnsZD!F2lzHr!Adg6Z&{0*IVurLhYu5z2s*-mx8Xuo3STnNB!j4`Vs$Y) zfl~)-9sbkA0tV!t$XYcpt=)LPMu!+hW&hDE+ginr(G4oa$Z>$#RH+BT=yQqi16pvi z_y8BX)VeJWC+aX(N(D-ft}iEi$Lox%=lXnbaNnlNiu%_? z7^&l3LkC<%YD}oRZqem_uw01}5?d{89G6o6Yb-S&2@EAvOmkxJmw;aPhPmf8+I9Dt zbt1PVAYiYjA_oWt7wn^uM}M6Wfk{;46g5&6hsH>6Db#OU1_OR$hGyLaFBE^%AtY1H zZEVD?T}Y{`gA1#r*2UHY%=#l&k9!Z^C$SRC>kDDua0XJ^0-9^*R^Zjue-SJ9ka8l6 zkxzuw3S;?rPJ$@b4W41`Igxj7HVfJ|-dDdH5HjWXXt_Ostr}3Uxqf z%?*Y}Im#fJ=Y{G^ul_na`LGU#2t^-K{5Q@}ZUnC}>tkpWH0_jGSZs8N#!d_y{;(&< z*Js4z=Jo)t`Zhak^wSoc3j$B`<;HdktqC&O$(SSmiv^gTGRY>Xfs`g@jLj2S27BeV z|AT7=juNgjG7*9~f_!S8y0wUDYGp8D$LIjCTUM%IWphXnL%;l7eO9}LMsS7-gxcYq z6G4+j#NOSz{(Zxo1u;31|y83*vk)-68B4vQ8I=V#CPwU;=SLK<2YVQ>8KYH> z518x!xXOG$>eoFaGl~j@6@iG26~}X!!>|7?7ZV|*PpeNcWcNjk$}N+sOI+cL8n&bg zzydxHV;k=Hm`qFm0(_`NsuPXL1Z!6FL)-e}h+$EwqeArah+W+d=@c)wl_?7RZg0RZ z8gcYops&;3vg3dDbuG`zX;X&b@`Aq1-Xk0yn>P{{mm63O1Ca%|+%V>AwV^8(1{b?E zVf5~ym3_~rw)J2Pj7V0Us?@;G4V^P`G+GHEo{;+MHm^1Q-1JS z&iI2=BQG-2J4h9-t|gJUA%O%vAXB?A>omq+KN2H9?|i~vOhy*PZl4maT&1xq2d_J3 zmOMTEJK7ESs*AwHAC2lrB-U^AHXmhgnWC?MDlNarV!Q6!y^dJ(cur-|Y z7r|GnjkPLISyl!kDt6ZMRalyg2rnytNtxG-x1qm|5(C9R4n!mX8dUW#6Ncpq-V@Bs zkPx_xC5HQ-6#ATm5!wbY<8FBuzdvVX^@HYXo9>7KtnSHDMD0T z`}-BEY0h`aB20$$g)y;$jtI2-!KARM-^`c^9pQBRVp)+GsFVOG>`a+ri12vY zG@e{~4`7lm!(c((oq|v8B=nU|Y=o?C9voC?gBpx*9pUD>~zEuP&iO z*m1%eV3nasQ3Jm_H#fh7wGK!UR5dkVgCNA%qcSmiup~wQlotsS4d>;VvXQhHtU!_N z-XbiD3AXv;#r4vHhJd-+~f@V)L)udpnF!mt%pe z5vlc;sMr|{5qrkr@XEK~A2wdnWQ=iOdW_x7*V(z2*~2)tLJk{H(__q-Hu=mLJ6CzfzV8gn)Dq+h>pGrld#AR-7e$p9GOhMs|B!E#U$%>4rii{ph5Xmg8J3tIL9u(833y%*~ z57Lmakx7=gsFgBFGe6c{LldN6o=U@k#g-&T1CW)4^02ATQWC@^E6{+!adzviN$u_Y zHnz4=QYC<35f2ZZ1p}6ag#`()2uKoO%nC~d%UM&6EMfpAuzHgC@_h0_FjEPyaqOOK z5(jV>iV0_y=0dJu3Q>WzAjbi~qsg?v5QmzB(GQdp{iUF$;`SSBz)tZ6KHFdZZ33Ns zo%EkTn3bIzJoWTX8uhnQhbA#IsTrV=>jY79lCegsa~!tvCT2ZJ|F{{xTmS|>{kHBx z%e(X6XrZ*``Kn|7zSwtk@(brBe&|L?#KG7U?XOis8G-mN(ak31{in!-{sED z489(UgIR<=T{S(1jA;{D9LadW(GIgCxsE3-q86HAayh3h(K;DI&pc1U#^yJ@()Pp4 z4Vc7AmwAii+6E(FfT`_FKCbTC*i?kxXQ&=Np({Eyt0U4oNLKAZ(S|uFt^xzO9>iW; z97+cm#b)Q{munLX>!8p#8dL~d$}Wv6rgL?5O_ZU!_7rHi2^YkMrGaiisDmE+-L-|7claFEGE_Wq+Mx z%#uK3oZ(TeH7EuoTX)g_VrAx1Xkji+DtmE`dcutAGzfXehV{e0F!Mo`Y9=zlCWhB4 z8ZglT*bPmheX(!XaSgA;7`u-kVyZG~Y^1mXMO4s>I?e!^{a`L2?gJ}I1p$v1?xAFv zF$zhnK%aOpF?BZ*4A1=Et3xoDj5jUyQ5hiu;hV{JJXM1-d@n2`YCrvaV{~wG2f-KBUS^4A`Yj zQF0-Ox4}-!gmid|G3mx@R_K!{I;A83#u-#3Ce4x{itK$r!8oJhwt^1u8CBOZvz)90 z`CdkbcJRdW9)snh|M#k$AEky3#-#83!|E%<4UyJICL~&@iq*$i&WR+U^0TBmUuzGr zUK&Y8*Qy6)P@}G>HBYtc{KqM>LW=ExDi})|O;Q2+-_M}Z6Hh&wdO$cGnWwASSjc&D z6$NE->^3@@VERZw1&tGfgWdwd4+W|$h2k@QdwcuAJ!dS*qV{$I{iS(fywUnta3&Nk zs;!;XWLlB!K7tu2!zrh=qH!Ov(4DWDzYhDC2-Q615|4h~FP z3=;{~hT5setVe52)T>7m!*IYM3nfiVgc<#Uk?zC-505s9A+BZLX?$K+7dNtpzs8Z@ z^{63Fzu+?c{O(*q<)$=a;UlnjYr|N_1jw}n$+UGHw)tt^(;rtSYFJhoWoJb)`xPc4MS_xo z*$KZ>1iB;Kh{dqNRGBtfkp-L51x)8Vnr9hAN>jona3=oD*uSJ5Sa~FQ?}l_xlJn)4 zM|I#=;B&U@pj;<0JIg%{cJd6PhZvH#S(l>G^Qq_>Z^9B3sC8_M zS&JFR>52gJiO){?5>!;q!I57ErdbJj93;LfVl{o6VNgN{&p61q?wy<#xdHxB)p>}PHyv*w-Zx@dM zQoySsr(jJkF`Tc3!H⩔2&jXC-2fJWHKYq6Bx*TvrdeLm=PY9?F0hzTqy{v8N&_5l!~D1Re1ukQ zi}XY@LjlxQ)E4#EdZs8rtX*(1k^(z*CPN5vLl~Ql5oLmuEP@&C9dW4`BhIrr9;LWP z*8O@o{9|56<_Whci{xLRa#1)^QgG6F){VV@@UZ5~h|oV$fu@JT0IwvVfui$YZ%sEHPDdzM zZ>^5GdQF`;h?ki7Vw_Jkxim51d?(wZWBc;@8nw4DIi*;vhB%_qS}hfcM*ja%^_Ed> zgH`cEpNeHD0Z;MKei_AnN;V`)K`1BvRD{v1{s}$04d#B)^cTEx z61&ejTI{CtS2-dk^B=hX;tT(&a?1#& zPUXKu)nCOKP4F^f#}9?_?=(1?RubwZH+a{FXLKt|8$v$fp>d=eWU4V=_vyc4QKnP# z`nkBPo|fjwMu!1w^2k9AP@$o0BhkXj@kJWVP{fqA~}9i4nP44k!~u#dnA3d;6S}5usiLf)%Px%(3!)sxL_odg-D) z{_VkFbgMCcxXV*99lOI_1%6tp8LYydTsokIJfo3xcOh4$ch};IOa& z0jzWy-*1lsBIA`2r`uziiz7l2r=9AU?}Ad}5JcHsv5*He3<0c>sbLAC|9iED>eOhq zF~O=_U@vF{Qvei!edjl&QQm?GA(@&Qy z{4plpXwQo4yAUb~Ffd>|B${HEP-QWAB2ir)ib|SE2-x>eb&s(M}L@pw1p_P%v>B1f@=;HJz326|RK4v~t zAZ9#Riw}B!ajNiukUkqMvxMJrKK_iVdp>XP$oN!VIEE4qH${ zKvpDlt;G;NdH*E;qWwGsXH)eU zPJnze+_p1@^|+cdy+lUTZ=;gGY0V1e2gc1%LdW7;Zd|qj9$fKI+mWuaN-(pYeGjp! zRgmh@z2K0~RBcFVrt37|U#pSU$td^EZjen3YSfFpZztL*BqZ^y) z!ow0{?2(Edk#5f&=bKnA#8A0{9qu-%#F&+p;W9BqfaqtuQH=H$Xui?!)@LldHKW(| zy|xeB{-)Dx6pcjB8(gO@kq;Lo!2}0Xs-+IQd`W>UR+9gilw~crl`oq8vtWZQMo2eE{uwjN}_9{#;I5bwKhe0^9SPTg{$lKpjk|@@N8AlVCR(KBFB;viEMG!VDfx zOQ94?60oPEK@SHfF`F1&*H*G(kf!SsO$Z=Y!{YDhbG`u$#3u%?ehAVGiIfiKKF`~g zMJy2+sKSR^v9X%R7ky_(g4@ZyqtX$=@mo~|wF=D>TkZ_?NJdG+ z2qTF$YK{zgs$0Q?VkN?>p$CUDpO>je^eXaT_fT3=ox51TodFeWID7gLzxxZ?QPSKO zRV<6n+6}bh8l5iNH~2Ou5+Y3IAq?6&2PmV=%N;Qb3f5ZgP%Tc@u?T09sO~;dotC?n zxwZ`1eJm(FXfO4>iD8eKTzr0CxG4U%KoeG^7wuO-;IMmB3Pokg<*RKL@%$tjBLcdb zp0GEdKiyJtFz*a=vuUS)P%U0;0!gB-wtrMIU=nh>pMq;wd%_Z+w9v`;s8NIhM{?I6 zW(mAGXT0}jk-T>yfcC#bE*zk%j1Zz!E^LG$`J;@XS}SI5PVMhMK~NILC##FP5q+7F3Ox=20SPlR6>PtbFg`t72MgU5GJPsYT=%cC+>-30byNG~pwf>$ z!^9yIOL7I8ac4z__$UreW!MVN>udF$?#>~MVVwAB`QiddVERwSZwPG^8sirbB*wf{h0LRC1&BO#J%nH<2Gt_T-NQ&<} z7Qe*0GhIh_RxPuFDNPQH`nkm7Ptq_@V!3e!nDGhJ=o=3T7R#!1p!|c|d-dvt#t1q} zNAr-ZZVsBS8e!}F5ta~N+HJuB(XdRqr)Z@2r-#=4Ii)U67DozuUQfEC)f5G()wK;c zrhh}-8pVE2tCuM3--*auLzaJoz5gglP>4|ur(&?j{lY5Za;YIBOMsmsTffoYj3$#S z`ZD-x3_rJe?09mrQ6==hR${G2Z~1<=S^BFT<;pNPk;~U%bcX$(-$zmS%RjD?Bbd|n zWjV(#wx2n*{XgjJRvUw;O|I^4+s==T2PX1dd3l&%nA4K6NH6x&90S4~4P2S@A9#K| zE-vK_&WOCobe(%2bDfhviMp(bz0fZl2Bi0CroO+?YLRgSaSSJc7x-3t3{%h@V5u730KYHRG$WMm42v=0RdSPmXJ%iKVf}014KpA6P*^%@!u(mTy|$O>(V#r=Ed}TbSt&% z(K67qjmwl2B@0gmLd|o_FocWmx3gL&Wndkg*YDAPayJ}5Y&ccPNbe`BrkIpD-N<<& zBnS$IZX*8iivW$}@qG%%p*!=~fzzyRi?|%uc|`l#(;y5SiZ>r%cMK^`Vh>BSC?QKYVzQ z-)A8ta?i=^sz550PSX&7Q($@gi%IAJVK#^{HD228a}*pF1E|r(5z?fXQps9X?KdPb zlZF}kIf}g-{>w1xAAzmA9m1$(GCnJ_&A&q;J0*`#I}7zN}@e=4Q7)=kvN=u=Gy9Y+a*Ni6V@d)ayWw%-&k+ zN$@kn_S1&S)2>qOfk@!1`cgW&ze~d-`eDN%qv{?~?F*?9j~C+K4Sbg#T*yUrNd2u{ zeYdM>ybP8_An$k=66{=>OG!)fRd(acB37D(xyqFPeDmJN0P}&+Mh6y z)|T7qW0r`TODs*O;UpEl(dIzb4MW_beMyS6`W_S)7uSz6xOgec*pWHFK&d28a+pzb z?B7uLDG|HwQ;UEzb*?X7;1JOLp;_ID=S0*kr`a;csRSE#*wcbX*ps1+D|wz}ypuH) zFDmM_A$ZK7;bpWc*0Qa$B{=aORcHa(@{Hk^Y0POFp?ZVEtm0?UY75pGl!6@I@9P{K z&_?wxs(N+l!UA4yW=^JK{}3E-s6tPt)msJS*n41GIW6-n^DNvAQ_j=@QplgNYB~sZ zQVgPNIwAOPA)6=%jtV}(RnO_PKkw=L+Lc=GO|w|C4{ zHix|Ozj-WcUFPYGCEJuu*foJ^d+yplb~;-x$_im4W;$}1jBf>voIG$5mEOuf5&|Cw zlr~y@L&D*lY;wF;8{6$n=$4HdGdGn0vUz=t#&38=A5E!`K=MNL>!3n2EJ+dz5YeC+IpXPo#LC`hh;NV@>!PiS}@)c&!CHmM%lQs?491; zf^E(ZBqx8Fio= zX;ev$qgy{H@%?XskH2%V9zwSFH3D4-zZ7P6-p_b1$h8){ z^)oksa>nP>*6T?_%d&Qr@YMeG53vz+5dTN6-~A?jgVSyAb?x>VyeH~SN=#tpAF>lC zM6t(#-mCoOt>JeJ1*Vz(==82UZ82NY(qjL7Rg{g}DV5vI+RdT2TdWOED&o9}^^b$* z9Cw^SOcn@4B-UrN6VS40CbG`@R7xHHRj(Q^G&B?y2yArBpA$3E(nCQkM@&^uQD8h% z`f1hrHSDLUi!~xJ^`wFNJ#y$lBN-+IeOeNlhad|yXc|JF?P4Cs8+YV`kZ9HH_5FJ+ z%|DOarEf8*m0CrrKN|+@3H~M1A(p1lS39RjGRY)SL-N-@an$(%y9FY^#DAX_^)#-9 zElR=+WJ#4VSQm=x8xy!(e@78o)O;Y!@Ndd@le)?RmXKx9JJ4~skTQUuoO2yI*i_Iv zR;57ZaE0J3X_KUI64IS^AEC+z2g}01?}@y2Dr=K6=x4(?o}2@vfBUB?S$wXV7IuHJ zlu>)Pp-VtTMQ190z5Hw%0X3>GYRWeyXmM}6+W4*TxS@XXX1LASQ$9*Rx47y0UW#nA zL>pNv*x9wYN0Rdn9H|V&3qe7dmw6{B|3?cz(I*yf-Crt+Q73stZ7u-H2&}L;z4<;N zArdI0ZRlu35D|Eq`?@v3r~2EWGEZp0`Z(*%`W09G^HPW2#FIM$L@7P%oYp^n1?J4q`J4l;Co~mf9^XYzcE3)w~)$7^_%ym55 zZ94b`tFv*CNP1zC*=Hu!duRJ2OU2KTh@P{-hoD@vKbBFW$?Qr^g70!qtXB+H{9p9; zAu!-DDx-yt*lUFN9i#cI^=ZYB0T^($e*Rc${B=rgqdcc0qSt$Qbd>~V7#bnt>rEc` zdhA0a{3B6pFH;H=X!Npe*E3TE_Co))S*A(4n^W#6zR`}`et~d_!nQx)67v%1Cq_h{ z`{6d-WHxICEk3h7@`W!093*G-s2uQ6`t&lMY4HA6Y-s(1SM+iJ#~9dn%D?x9mzQ~C z)L#Fd9d;*s`TQxj_3BT;M*sa$Zc1 zyzPUv=U?E~@Y@vd`dGV0mVEd{`m9Ffb~(Q{k9L-?B_b=w)TO`tn?9iqbDDWw=|Ea! z%vnd19t#9qni3?HF+dRJq7BdZA!nZFqej1a#n9_3iDZh}udp+%LRX9Q-mf-ylp=)@ zb|m2`40@p{SmfkKewr+@P4EtTNhw&b>ME!41ze?V_`68T`kqA9vX#l~nv#Xl$a5H^ zbIbZhIlxb9nbevJy8sE}(va=t(Mu8EEC1USTs#Qr25-6q1gm-lh`StjS6xQ(@GN-N zX@xa7elyu21f>JXD&RJ^LV6YqS=pC!nbm$a6jF!Vy>!(UQ0=^OSe0{8i+0Ih(vJIJ z{B37|Sw`iqfSXk`*@o6p5{cW=}4N+@_tsZzC%81<4@^^nNlpwYtq7;Lc+79nSVPOGuL zQV#vh|1jU2+nO*ey%G4QmxzP^(ct~@;{}bC`Eu_p@LE~4*Pm_saX`gu_oJt)^Z6IV zj|12pb`uvktpBl#jm#P!`{Ma{=P!16 z#;165hkx(BG`$cJ91D^O67qs4UKww>Vy)SpI~w>0bXgF=RNd)f;x*ZXkp1#yEse&o zngW5RFVR1*!X{?KUmqRZT=6@JqtSAS`EHgHy3;MS%G%zG1Bc`sbarak-s#r!t}T`h zE4}cq?|aWK1n6~Nuay_NZew0Yd6BA)Aec_ns9mk`-^UvV_B z;NE`^RS)q#i!*)A^|AY*HwuLjh6Z<_2MH4E+EvsYCdvWovkihFu&#f@qqKs%kZ)w5uzk01uO&W zdrCF5HMP(cjwJT@%5w{BIG_qL4BWmDV8qi6|L%96OfM`8Oqc53gu`B4dke%?9-;1x zzXTFI3IF?TZt~7I#YPTJy5heh5soIv$@zMQM>QUie}9{!?TV2zf;(e8b1c?@)_#kA zKa$SXWknVF{_nUxL}*1yzPs9KWXW|4x$P@3%$0@u?K9blRZ9iC|C{C0df+?y0}G)w z4$nRfDOZD~j@|^ktw;|?>U@UXr$gZfFgMy4x%^knrF3|!gn=UL9xO}YTghlohcf>& z2olP!*<+aTfEy7+4f^RZ^{4Fj`K7zbk&H(Kflj(XQRA+&O6Bdc6(o>w0r2ep1 zwo!+-?x9SqCCwp3Wsh3 zs_NB@C5q0Qga3#oTX~}TUjuFy^Ui$u<7wjyPc#V$uQS-D_I(`6!3ENv);^s%(yLZ* z&a0yM`8CZ?&P*l$N?BAnJv)=KRa#5W+NE|at7_9QtHOy|`dt;quY`rga2;myvE`3i ztJ9ywV++wl)P*=5jt`b0Kr63zXZ{rzH_+kP(u8auq@!Wg`{Mf zq&iGq`P70P`(oQGG9sQ2s5m7dMd`-zPncfx0~CG0!NHnF@(iD_g^&p)q87=kMY9TO zzpe)wzoEkdJ};udK4zsByDcfV>C8xZpv4K5|Nf%yu|&(@DpG+HQp2*HJAJxqnEm28 zn%`^j_O9q*!=*s&9=v7x>dSWi(J%W`Q_o>E0}2`Q+L0%HX`%@!zp+F1*QV|L!PLs9 z8|~U7M18WI5w^fE;=rzCVAsc&9P1gh1R*1L8Xl88Z?cVLNk>#4ID_#?SZsMH$DqUp zlmU*`iLh_0fyP9cZ}pqaDu?*2B!5A{$c)*t==sR*2yG`%_&bj6xlj5%ga7(P6k$ta z@+MZzD-l@INCLl!Fu_g!g}>}_2hOdEfSG)PtNBDtj65eQbv2yRs@*V+e!M+opbVY& za#RD?c}{Djf{R&C3PFW;VgMP+q$332X#*7LQNLF1`PohTBW5^QEqLwe6q~5T=77!Vp zu5+L53z0&WTo$nlsdE8ac4J%aRzbe#&ig=e5 zo;@;qr=&61UjU`>x63D6%a$)|et(mDfnr;c>B4)KRMUO%!Y|B`7y7u##xGET#@^Vy zH%&^hY23Sate2}>M^%wIAr7-H&|jb!{=Wy^uU_?!vUCaL^+l;h#Lk zG1)gs=$z?Wqj$+XksPO)w7|n(_M;e?9*C=Tg~*+F8Qqs>8#e>3&QHJRQ@Q6hVEpmB zGFlDpJ{f&9&fL5NI`b@aIv%7&8ShRu-w$1R?V}s_(R{tyfu2fZ3rwy~@9Lo1T|BS$ zJGEKJSY7oJuuSW#j#q=%^TBTwnLU_yP2bvuBA%}5t3hxvavfxsJZh^J2E1>Wx^S;ibXLu4!vCpvYrGZjhN+Gh%K(#QO+ z=TqX7bi}r^_58!Hd|3zgSGa5@gGJOW)QLoH*|~i08Aw5Udea1M&I19_;`9&BVCl(Z z<%h(m0Rb!Z_R$q@OZCSQ?zzzI!l;u|M7BvON=!R5!0!E~J_DMVG1fmy(~vhQ%v7JFHN$6;VT#7kD{cU_8Ff!bcw`Gr98L#?yua(w$$0&sz!?Ax zL?KZD(i6if$u?p=%RmpfA}6;~?1>GI0VOcYd;>~0^zE`xl3MDDvR z-3CV}3KF#VJoKjT!s?;rar7x`uL9Al{y1jxIb=xmu@9OZ*}}}`)}!)S{v<3?!;$sK zfck`0CvuCP-xd~nsw$(PVg7~F{)RgB)z76352m@>tv(0F-I`_|Kj(Gdodlbh+2AhD zZue71Z^QQGw&dH3g>R=Ecgj5dBZSr(H(tYmPPh0z+60F(mVRN{y^n!cL$;#*Dwe&Y z#&1Ut&U35lzX{m94ogu18D2x4mnL0_jXK(of*NygemA7OGY{J7)?mRUBV>5qo`b_2 z*G)@^UJBv;zMAjmT3viGJ7CE@;3n~7Y}=kemAC@4379eR@13CX#eT3t4nB~j_wl*> zCKqTlGtg7%^#Iz83mbRs4^9imX;m)IF;BVLb|vy){Lc@!W363j zGx+KQRTWPwGriv%Kkp;kh_XLB(9pS|pLImE8X$3|fQ$|# zhXXm|g`cgeg7{3mKGKecoAkQU#(RPVV9W@pdm@aKWi~(=vbrXe%sVH=w#AK zFuO$;>09^&tB}6Vk|~Mm90IE)I(Di`zZ72BpbLkL%W6c?4>1eC}tcsSQ#+CcR z5`|@ogOvh96o-(i4qu8}p{S-#If2!imP95GnonVmNAk;OkCtze4X1Q{d2qF^7|LDk z`E%y}_uq4ax`|o{Mp|kZlc$|Ulij!%UAF8H%{(fV^NqBLmR?ReuB}hanFD$Ms~n(F z*=X!s;0VrWK|?*~4FLbmeAue7M!kS^VZY}DHp-hs42GoxBvYYy>U!x*y3C9nv@g*K z2fKPpK%aFODs%g4mWd>F&w+nQrIzho_mA46HI906m_l6m{c2CFog$}36Yv)(!eA9% za40WKMziZ2RryqD+}A1E5aH;5YWuoF=Xty1XO{L*YX))fvG7;hj~C7M>=N5S$$!EU zYjY*1Bnv&upJeMf(r&*w#|k+tzDVFM@SKJ8g7i)8_7ye`dzOR%`OP!_ z499-|$lLV?PT9Q3&dA!|+wmqCLb)DYyAGwu4?=1K!aq7LJz7*EtPB|UEuF7%dxs_h zPqDP{(t6T5P~&hiauo{MN8o#}1h-zt?dF}Igts0eYbl=>wcEY5leJDbG?k)o5L*YX zmT!_gdlPPs#YVMjx5qW%!si;j*J6xj&hlFE@{2(oOzNJv<$vyPcQ&Y53fLPGCyX8u z0{$7iga-}Z=#1Q>HRZ$}_`h!`+GIB6NhM|68CU)>v$BFfX2qli)(7DP{(zv< zQw{_~+`LwP!V_;(IZ?8Xk1kmNT{f+Oq-}VLdqkwlS5s5I5~9`vtoU&1>aazXLO>xp z;Y|cUI;a-nq_vNdFE6(kVe?b9-qHs+Tv+5i4KdP{Rsj9NHWQ<@v0YnDF%t&zh^YvR zOko4-%e&Qga$Kj)A1)2g?YXU(wOa(|;u?Q;@@kNTW@2b*#ql%w$o3(b9%N(4p5XdU z4|Goo9ZAUxE<}feP>({KPC*sfO8nD-^NM%v10{%?Rl)*^x`eJvo$O8$va{20X76p@ zKB^XPs4`1MhSuL-VsTDm|hm~z|w6!1X){(SdbjfU$Lx@imnhMK6j%}vI7NQF5bJWB2umK?-vfqLYt#?`3@w50xJZp z`&;zou}39|TAl&47xu2ImVuUuggsE!$A)J;HObx`D0aC)Qad$tN~cvLeBk^a(x-g# z@x9ykJ85Kur&G^}^&f$9Z$4XF_9et(f7yaB$f`&kG#&mDyh`Neca+TD%tYs9 zkW36(NhrqB5bW}3RW;S_MS^Qpk{5~0m=n>#uE@aWvcjBF>FWGhVpP;#EOq{~tj=rAY7NXSFU zWLZcN0k74Te)q%FoF$cEQQ|0AXH5yJBU@Oa#KtR(gE9+9e>@`fi}%U@;Bi?lNFBKq zNzvjy&i<)0r)3{85|NRgS1zb3V;!zYqrimcCYeMpttvgtaUmOe8ijN)0E16&ZsyYW zi{AoF8KHYu7Lj-DnqKew0Kwb6Uco3H*-6Wd|NnpdrTflrJ7# zxSOY=HgM5vcQPBTP?o43voc} zF2cI3pbWOas;`Fyht9DRCynnxy@bzr#!p8HZT4r%9(F9QG9SgN%S`wg3I>`vAwoj} zy^q;lZ!%@VbN^8q<4uf>De-MO6?yiX3D`icZB=@R4#qjBt7KVzfId1SE2~JWvM@!D zyN_Ds=@3G90?NwEuir3>e$yze)BhRy$r8LTo|tA3~*emc&2A z2Nm-=V_}!uH^u7G(0qNb_y0Ss@_LI0)9DE=+Vv&{(SOlK^+8u`EjY+N9ZC$-GkHd) zMdk&E+VrOorVD$8T(^?$raw@Jk#S@o@^IZ$dPgjbK@_*kG-cG8c^BPw@*p|Ed*O(E zx5fhHmf8LkI&JPr%TUE86kD4kZ_QV`zs^8X&`GxYv>~+E`nt&{UWVStR3ZcZA@?eb zCgJBh1xcOPwJV+Bafyk2lKeS3qcKO3>v6dU)6G=jkIj$C`8h;L+IAN^=Lz9`rnh%j z8XWOoB{Ch(_5%Z63^OzHP6w+3L(=44Ttb5Dw7@Y29JGdp#^lTlWD^K=!}?CdG%rm~ zLHYsGe=qVDu1zc5OkuE8!ljUJyiqT8QwdRp5>>FKkYor?DWfY$Ou~%Uphnf7oK)O@ z$(oykqQR{W$K!H|;8qfP$=!baLHw-Iv~9tQdq(M!rp{K&d+|A8?*%^|c5RSsnx zlNyT2L#f+5OUlj}ka1uFv4|2`x)Ro`jQ>f%p>b2XN!_?Pj0A_(EatTCAFFG>eZdjtp z%zSP!9E};4VQE482^Ck8d50g1FoS&5*(oe^KkJxtmM=(=dhHKIzpXcb=7%I!JEr|+ z*_@SXmgBqw-H>zWA1d_WF+r*?0FN&?D^)|APIiqwm*qN_T#D;kkszPOVAK;s_M2y@_rXW*%-<)**1 zWp74U@kB-RA%sX+-`;q%QnfhF(e){<1gv!!k;r2PKmTvdW}3!#Zl!BsAf>eBqnv1c zTWu|H)Y*!g2qFw38I@3gG{Q|$pn)Lp9C^8{6R+{jt!ez#FPj2zdKSuEy~mGKQ>oe`*-5nQ z&VZ``nS#Y=)u;g`ub&-~sW|nqQG*$>jS^UpL5)S1+H*>K|D~cfwp zUsd?K;|iX?T-emzKc|->$YpOu|A9HzasQdIvpW`U@~|>fe6N5mT=zk_nLq3ze0m}A zsFUqv_kZgSTyPE;C}R%4m9N^VS7@p#K)Q|mNBoL`j*40bsSW5uC133g6murw5)s)1 z;UD=!FF;f@G7!y_0tf@nD)*Clu!0Olmo|34gkd8^MFb}$x~h93 zrj%qhxo$etikIS-Bl?CHSU9gbvz~F*S}o1Rgcr{f11WH%^i^ebEg> zCLz$B&t^|fi~pMRrv0Vr74tSzRHc0rHqqfHQs3djoSY2HHMVlZ(lP^b)ajv^XC2ts z`S>6u3vD&xwsWg|GclM&aje$rVluQ?#xFQIF!lJ@ogAPn3Jp-s5Tw}YbF)>dUezp1 z1oGz3&0ps68iljsaVmS~g#N!r^%yl6Hxx`t?B+uU%`FMD1rA(275 z7KWa(4`jRQ*{)r9l&3??J{SOQNnw%zm?kW|#;YS=bADzsJW z`uTb*di7`NIiq>aq%!vuWQ?Nk`_%mC{PD6Cvb;E@e=rjj7;Jb(Q_w33W;Ap^28PXN zL3;&@fFgke`bd77$e*N`m~)IR@*oKErH}ufn^A{>1stwJpSLD&F(2!r5e4~{fs9B2 zv&-z(|3?cTCy6)mps}i7*Z`5=ItUc0N}M9Zvzq}sZkW|DEE{Idl+gYvHVDXGmbz(# z&*Iu8U3%oo21O=$yOHbLa8Xk$cD8oH?D>gDU%sbw<)xU-yeSD*SH;XaU;WYkRysUIX|@ZzVmsux}oSfwe1sDLLpoX%7ngy%X<>V;|BALq++P7LdW z(Z_sEN)LWF12;|_G%;j(U{AO(aK0W=LMQm0&~~Y+Ib_yl{!{6YyWt-Hh%-rw zdRxsp0}xNrtqk)x-5bQwcXj1dZfq*r?qy>Wtye8us9IpaTHIWw$*4v@biTr&SXdWK zF`Hqx#jj_(=T+3$y~H(C&k*s)Czv~&dR3;nEHisO;Vf$uCB$Uz3I~cZT6_YP*BZ4r zhU;+{WFUk-jt%q5WnY3Wir)i8fDT{T1Ap6a)T=9t^)@nr}#Z4B$e^8Vh+UaRY zP@-%(r1T@=PcsmQkr{c->N5MH`$`Zi;azo|rp)V)&xI0Y>z+hTyQPEaVo}dYABBk} zOOH!6EnrwxP{-}oh21lL)s!su^##yha!n{*x3;I~cFceilb-gKV_C`4=6!tybaF_} z(SS!2w=VmAhB>STjBx$({a-Odcm=GynOz$c29ovAZK^Z#vc5TZq;V_FMrN%$2+&;n zgcQL^0w8r3)S!XBK5@$+ zP>)i8|F>^p+)7A;7!VwWgAAZNIkRmv8z#ZVh6U7Tbrt7>sN$i&g*e4>D**<1L1xc* z2>lB87oDkvg||PKTPdX#I!R$ba#&exgz6-ta<*|xKcz-j>ue|V!!LAjOg)nOJYx(jqOxt{j!-CLPUkigHbM< zf-6FS?cJVpD@?VhE?#DlRJY%!9e`l_tmJ)TovE^riJ|zVwb!(KvaQ^JpDuNiEDRE$ z|NGr3bK091wnI77b6ZHWw`2aq+s}U_1hljC*pnlt9eXU%)pn1ipt7PN6$*~ znPlFBqxAndi?m!SXiLxUkS7Blglm(ZCz6BJa7Z<6Hhe`?h>l!1(!++$Fqhp4iL=Z9 z=&3ZWUYw*y3Z|&j(||OhmZHt-e{8k@89evBzcMoennWWhRHf&bYl!RXR1uSyuQwwx z&=o)>Dg4vc{Aj`JL}l*YNS428R@?>~M$H`Bm{2rW65&?~6=4iH__@mob2KK=`7L%~ zAnns5yuK(NULvRJ1C4DA{#Ig);$}IA_9rC<_pGOe(*R4tPBBIdTzz-kUXaVM)SN#ttIj20Aqw~qNW@hNKjGW`eYT@!f$=J zvp57TynHx`mW|ile&A>(@C5GO`%R0gBAOQ6{p#41b2Z_}l>V|(cg=j+ee6(1M@OK` z;qmHuNB?*t_wgc5h5B`g(BpO=0!%8cX(DDI+K{UVZ4 z4u}NxMbt8n@kY9>a zp5aDtQJ!Bo4u12!Ce=zeU>$XVnS-$0V`6eS^dPVK@&2(2B;H(q^gfBbT5Z66fQca^ z62zu861#Ka?X794O0O}^{=rVb`oKiUcEGMa$11uMi~M;9@iEl1YXMm@#+iZJAz8dlw*5Lh{B?|FwFW5G99>+yFh zNo7``4YW=gp+RG8B@vW4`Bz&qw>FKe$XR!uJt4ZS$WenJOCdO*$Qi#-^P9;lle$Ex z>ziQMpEp@r2LfIZnA1$C93V5OF>2U^+SD;b#pdUEI6ctWl9Mg%s9t3By!@$k)}IyS z@VuwtWehQgf8g9x6jv7(-69ZgY$y+FAAAy-c&9H*f#7TJ0&y#C)Wj0i^G8xyTg&2) zhE(9s_7ib#32}BUul?!JANDGi{T{&|^+JtS=n` z=5BqPtA9;VFxOCj<$s#?I7Rf1Ih0;tI_MY^_Md&y05^1vnt=Te(z`|eQ_k??I621U2Q`M`Cr_fYLTK`nat#Ba=7zLCnVlbLNBhi9F)T94X%F86kF#P(& zosITd#pVsU-!&PbWeHMS(ga_~9QEb8>`CJsMU%fR6P!-ccNhs-nO#%DT7P!I_kSQWbEZ~w&^DCSw+?E|LHEgkEDf*v5^mB5@;!+^sa zhDVgakx+~1pL0H(nip&_+lnX2!NhdLla#pHjWk~i z8$=iKE8nt=)juu}3*llfc`xEs=Xa*Yh!>O1s;Dy7{>Vk&R#wC5IuVNdPLQ|@V|qXl z`~5qCtt<)?B;a2l@rF1Y{?%ul`V9G=z;RLNR??ZJ%E+KssV#t-!>QzF)z!9V8`w#L z18;pON1%0np)n|q_!w?t@L}zHmCm{LLn^qq=uD5|8&N+$Tkx;(I&(;AHaD7KdaeC> zS$n$x1~w^)+!3;(Sl-BGUA>S^6ufU0`qonVAUGS1%p@3#yZ{57p6ZJYOMe|)=#40W z9OfR+!*;nlEYcJ$yGy}orCqF7D^R8-yONMr zrI>-C*>QZH?x6H3;KtBQe*Uy>zF-#`9nIU0jWNO~L|&r~T$ILS}|!(AgI=8#>D zfN&3t-wwwhhK5&K2#p?+INYRw6R^vl)L4l({r;B&V%w7_j}bFm#~fq88d+xjt&vc8 zrd)@4926s7)hJ{qysI`nRG2`?;aW7`CtfNcc=%Tx$H$qN4w4KPOJH_3h&=w40ZaR% zaFY6WNQ>Tm@E=LSE!ZY%^E`5<`*`?I_-3cL zitx_USKs6Q>*K?y{QBbN_pbKM0yIdGx3dSli0sW#!c^Xk^OhQ5lz8&)|kr$Hn{R7rd3^hA!B{torE(WlKCF zu3QAMFXU5FoWyW)ks(1mNx%UNL!WL9DmJU&4L>d)eURD2vbR22G06e!hI&}Khz^5E zco2Tr>kAE#7vMDy~cKAUkX zK2}wJ zyIUpMd_?v%au-M-cNf^s&-5N_1bqGfsQRj)I=XOK+-2kLE&&1rch>;H-8Hxbhu{uD zg1bwCySqEVT{rHocm8wldARdHQAHIGYi51cUw6+IBcfH* z2&P;q?2)gLN@hQwhxpuV_Z(w>nxBmJ#dFztQQ9t@+&g#5Vj?WNA5lnKlXJFg4wa9) zG9>Ft086hMIlP}eE6uEY4jI8>Z@Vq-UcRJDUb}GG>&lsI-?Pf;6tl_Ucw7`uxz3{T z-xu8EON*_s-P?N0{9rXSUj7yGa&^~MwVQg1u=?^Q;5LS%=QbAfhl#NMlMgFc|CwNe zm*(^k_P3k=wm-`UhP!YZ4qXGD1OG{s-fP~Q^->~#j`0IEhvoQI#^&STk0#FwtjS|m zt!nPEl(=_RHcAsqi~~p%)jEU0IlV|?b zNB`B$7y;7(B-HY9jrA~gJuP&xXmRKO+YGza{@p8^)xIC!(=~9=^tq$#RuL$|2TTCf zT61meKTroqiKwca3+4%~xS!0_F-K&c;P{zXvT#z|oJef%&2natV!&qh+p;56t+@mi~#vhT;;bOc=~ zU>ca8j0GyvaFjqa0CS1hfRIFmM3iLmX0HpoODncC8r|M`6}JdQ9NRBbwp(YdQ@M)R zO3RH(*{>_!L0_lg)B}X^t+^~`Yn$b@wrKv{hw`!;qN>O^N%S}V<44@2-_;<-?iBuK z&gJ9#CTXkr1j4G1-geUW_pFZt zOC~z!fAEvLTV>Ytu5}jb0fSlj%F&V;nIxo?l$Z$8l8;F*@OgJxdY5Tb);;;tb;)5L zj{KV+O_ql(*X$d@*6y6lgV7KEAx!Qo+u*CKD&OCtWFXJ%&TYc;>&=M}FtLmV>_3z_ z)x=?+Bj>M6+IINy>=hrEmaVke!;&wBO|0XL+Gsp?JUXmTRQ^6k5|e3yKR8%AzlccM zZ8fv{XP}?sz$8>K6i4&CJ1$?g$+sNRooL@)RXK0`5kP;x)p_596|&qrXmL^mV?9Y;pk13lM1Uh-_!#+0813DcwP1;~nr7K}W<@woaNLDZi>dG(r5H)?7w zr>?Zev6`Xv<$oGb@Hv$0j--9^JVvTlFFY)zpS6AzVO7pyU{X?y_a!kg-Xn|-$Iq%k z_(aZ`4hID$i%GY(hJYST4HJn z=}^u90zzPB&>gRQFOzVeANnqe_iiFHYXwgXWJyvn!aRdzX2uxE4=ItH7Qxe{jG=WiIR;brFmb<(Uax}Jo(27 z->JHRK&=xoA;%+{c3o)to2icnmBN|kfL)fhSQqd=)Y3Clf!5K2tL7=k^Rl`Mm(r*> zR0Ej0oDO&W-mvq8D!MGEFPpv}63!3$eIIRCDy?Hs^|TfwZ;+GxFJ}r-Oyt)BE+=kl zJ|9R1=M@mNb6=kwI^XX}t&to$sw~wr5#-?MFMV1~$1Y zcv?C=2J0*|+fA9Xw=Pa`xNmeT`gvSF{ zS#n30`+XH`z99MKfMku=O-jj)!M+9j~mP6std&DBb2PL>~Y>4ABSnB)jus)E_%{)GS(*reMjP)x89dD1AjuD(2n~J%}!iiFV&yI zAx|&o^71^mH(X9&NxbWm5ySlw!?l!t1n}?7*3<`5;h6KHhpR*642DFr;~fs3k2yMa zs$rbR^k?1^^K)N;sX{EgIgHGt#jfa8|J$y$w>{d?(n3D8T`a>lV(BI;eAQSZr_~G3 ztpLr(!_#H_r;_ILoiR_F1#t6mK9!YMR;57IlC-R~*}<#lcvfSOZvH5vpYM%Js>2J{ z$5RBAmGaiFNr5RpGsKU!TiZ(B&iD=ZkCZlVeqOLQo|8=pr1XWj^VyM}*WB43&qZ)* zQ&iTTX|L6TQI$nkxD0vpl3lR9(2!HPzjt<6RGt4(K9lZ}?dsV|~v%=l5Drv%~ zFI-&T7=Hth>}sR6t!@9!H7D0W_|Ps;tu@+OOCXS`&zBnyv z6#3B!t9;lgSaz>apRWhynJ$RQ_fr5WaY?tk)qEr~z4uG10M#U#VyvN*?i-JCG7 zk~f2Q6$KYI#C-AQcLk|QueWfICYp80>sr?9-hzXZ;#`Bz{t_;y1M-93leu554dIag zAu=)Na$$3Wgme_z8w4Ek)EgmL77nx|9lulRVIDU}2Wqz04r%sdiZydKXRXY#`w!rP5 zOO2Mxsy|TRxyJIpZ1P%`Ppx#lvdUMsXa1Yt+)0XVHF$$awPNKwT%$suuK* zOK&{$)@;L`;$qd7%l4yWgUPZ)x3%}*%-$mhTS|K4t9-7E`f-k{ z2{|{PiZ6w84D0QL-xy<$&;@$k3_BzJi(|L?@O+X82vQV*HQ+n+%G@dQQl3XIgw<(h zLcFZ5pGoTj!uFj3tM(qDA$0z!Q`swU`VVz`mfY9t5y;KY<6ol}m!?{DHLDqOOcnyU zJgF!nncKD*?G5Y5$lCTe#;00u?HSp=U_DYrM=)n%>^?AQ#NSxVkrr=y1bql~yjom$ zhT^Y@8Jbg0T+OIUxu39SAFHSOo^F^0T}-69(udV=bmzQ3u`gWRcz7N$zBVTEn0;ew zRjobsJ!ST|M9e(+7X0z(k<>ftPow$@sgR_oesMueB1L0g5Nn zE8da1WXln6TSih|ti*&pjXH7U*NB-`odqh;b`=*3gsSx12%z*C-5kIX4kvd(GUh-wg+BM z2KjXMf7}l8@W!)0|4MqU)ia346PYMG79uqLd4Wi_(+Orp5gEYxHhdGpJa~$S(BHdK zFqA!R_$jr>{WGsh@^-&lii#?9lQtiYE_9W+76va-;u9=9Grenf>l?WORi$mh+3o9! z<*aJpr^yr0O(Pwyl!U~o>tF{DeX#Ixi2OSN-^5@OCT(El;sddsX3ylq<#lb>b={_e zXqpoxaxFQW_)umXNGNyKt9x7B$<)4NkmKPEXd)a2vN#xcE>Jk`g@3-$+EhCVP3S9K z?)wW2kNt*T@rCvk zFq?t+K0)&OLmi9G@Ll_Bx;+GP>Q&mEfZx%#bNvU3gK>f%B3&u|8$ZZGLhu~-wP&+> zGe>x)d?LSJc!u_v#?n0P=)WHGX7#Z=v4e?I39e7vUm#s)v5IGwc}L^AQwPfopQ$st zT%a`#o0D`O_(R>Z9ic9?IDeb1FKIcP=GM7b%`ZIe1@V|TUk+uKEyW28&rh{09MzX` z!bD9(ytrIKzWhR1t#>+Ik@%A~GRGID;WjzGH$AXTR%dncMi;mFjIDp~R9{Y&6E!S6 zSaq>6Yye1{fQf}6QBxPo>aIr|954DZJTG48T*iyIoyUj&u2Y4>^kRU7I>Fn`G|$|X zJ9(QM!|bemGhs>tg$_%DqZKb5O&1gS(DyWu%$W=x_iJR=({;RGv)6EKMVteEB*MrXPyO6eV0|nz+o`>YCYQG$(-)Przcx{U%IG^!HEjt#4wgB zW2{p>KBEgj}j>>gNU8;n9QiX6ss`ff8b*-Xh$hKpY$(t~Q$9XSy{@-1Id!NG`^oslVp|m@c ze$*xRka5MsC(%O|_J2!PMOMxECBLP)GWGBTz}txGe6QG-!J+i!kG<$fL8;B15v;ZX z@!eOLw>A^Rspt7SGIS=TN4U&OHc%|gC&^>PhDbpxu7`Y3aCCA`-_&ik`xJgbQNSuA zeJea=EE$qA2jau+Am>p`yc|A8O&vkgPA-040&X-j2x&X+;+KipI|}{H^rG;j_C*iez)(h>yG2LsA8W^?Q2HV|dAwPBb+bdt#Y3u9_ zsE@_5+}+(Sm4{&#QBjq~tja`gQs)kki5{33CJ0g5CxHo#H>M#@xVyu@E$rgsrRs5C zC1;x{M+kF&0B>Zjk0iw>l|S^fWLj{mc0yRZEn{CFs^nF0l6Tize6hiuGr$#$DqEZPrTue&Zwx)R~0n5*v1_3Dbp_Vk+X-J z*HDAVepwzU3$12{CED5`t}ii-Io-$4@LgKY@aTV6WGeIHBbT>0ZJQL&7w|j=0{+Si z4Z)|N=%*Z$_M2Wmo{<|u$BXbp-*LP2Xmc;V+ow+OPrhN3>86S~g+&Eby**S?^QWeV zSb9Z0*2h$b$eu*wKFYI(Xll26AAZLA<8MLb>J$E^>z*}j#;Hg>F?Ne9AOcMzrRqYw zaIbnV@Je#;B0eB$oZ`qPlWuHW4u{5f21D!rVj<+;gGKE98qt1|1z)GhCm@YOW&BgW zm$Yw8)On_8?K(`pr>ffgspIvve5~d8RUoFTJ=!!q4Asw2GW9fL<`W(+od=+;ghKOQ z{`(S{uLj%gArNb~x_9HLFh`9WrG5jlNpDotx^k=mh2miU(Z%gtv>MyJ@|YJqY-Iry zby-p>dTB(wy@ez5e|^=xeE?R(xl|Y%um&;bHKp$L@YGdRlvUJW1G*WtOUuQ&HsdN< z@d==bh#PRBz{*PyN*?toi{ud~-PRcAT68`jW`-ax z{lmlN5j{}2E$JCF%@rWGCJ$;1M+NtUhwJw>le|37 zr%_^rTja?g^f`2WA8XR*Hn{W+=r+2jW8EItmK~P}EpEhRm-=KGx<-o)um61B73#27 zn;k;-s>!0y6Cq&^+houZtQg>N2tV1H&O@)v=^1h(m~&ND5|RhMy=gM+@x9Q8PWeSJ z+7i1ULOV*yVqxdOaWqKtX5iu@oa~|t9gGC2us{CsED&aP;*hC^`B%14)*|o~rv%S% zSX1eO81t|cUwg&v@TQNb2xH|cosgu!?tLbc>x2F)-;t8f+h(%k*%awu+Ny+EwKc{Scsv z=)oQRYS(rO`7x?4?8_gw=ECj%_THxF^I|LHvFv~Se)PF}h=JdT_Ott7M1|AJgC`!? z-;)I0cZcg-M`b6KDijSs9?%HsYg?wIe*Myq;+JtJ{Wh1u*1+@Wuaj0y1SC4G4Md=v zAv#U1!f5QOZsi$fM3AZce;LKbXA=NVH2U1_bT`~}k|inQyPb%EJ3<<_3x5!;jD73t zFJs@UJdErDy?`9J&5loK!yHVWIHFuO3^LJ{=r2-ZjGwS*8qu`E58~t!Qq;`|0DIov zGIu|~3JM}myO9{)iK+u%4yBKpIS4zP#uQs#lF{l=A#iDdH)CsT5W$+h3}Ke#qS)m1Z1n8p2lAKz&1l zsfDTm1Epd)`kI?aiJoQ4ECc<|AxqgAyM3YQcDUM#Jj)Zk1@uXpGTwgTuy#9T{|uFC zsG3Pg#d-RrZ(3?<-P{bY_7aLA2EskXa)I?a$;QK?GS|Wxc{Gp%%x3(#obb3jvwWLC_s0D-uQB7UI?2eH<6l-8F71Wbfn`bSk;X614gRr zV9K~NDkvxAV6pNGj_kYo_)rw%jbV{{C=qqF;OA#J+BWo0FV81<4qm-g(lz!};or9OV)r5SWBZxUwphs`Hk8H7Gcr$CP61nlH zjK$1_R2miAV&U_Gan%Iv_M66^TDAUzXGX_-R-1u=K|f2w~&eyKl= zpco+c3ApHx%Q&fiqm-h>MnLC3bp)u+u{}LK>+6N#+`OU2Y9XRiH|3u2WBcr7YLQyZ zp}RcPm=QL^4)k!LSTN<~Xc!RUQW;C|yzc-wMIRYxuHA3)*`x-4eSN*Yu~8$A^@o|`w6@WaGTiJ`EB$q{s>Da67?V0DEr1IznI|FJ>7)?p8JVA- z#wZrgB8r0(j5<2aV3dbQsLJ};;W}!Mnc-(cNvL0_TtIn)vWm@)mO@nsgm|@LmN%P( zx{)Y9C(Tw7@ty$;H>_09+9sZ|Q_8j(+4LX$5sIx)Jk0`|b*9p)ot4_`;xat+H4R_;h}fT!cD-lc#ebkFo%ZaeJt z#INrVWAUc-sc6NFOmy<9Q372CV`d`%5IsrUrieWCDUGqe5@XbS2dXV2dgrJ4>Kzg0 z;B*oE_KoZT-}Ws}(F|X7hx!a#K}9LNKOG-re^ULh6**}0*SeB6-#u9_(l^D%wWX_2 z5j;_jIFq=~u75$XX7-e^5RutxwyPIn)At_t_tahD1T)s{$AU#eJI=qpgWY+HFZff+ z{>moJT4Z;Dxik{@Q$x-QEpx_4ILNk=FZ_eZ>;%)qA4bLUGEjeeW_p~eDX1k(FdTX& zz-N0Aoxv*|T+YQE+>iP9ad++B1nlCn_0%7otjP84>@Ii7QVG5IX0o4%+IK!@6ix|5 z1eFwB!Zq$p2}Oz%mT*+`>|S)ee|aE;pH*H{@O)kcI;S%!nIsLzG4zXomKXMlf{14N z>APF9r!%Qk$7=yXlfUC<(N05sP1kL2O6H=rKS|An!kVVcp}G!OyIR5+h9>f7N~`^h zhJ-mk@x$58D*S^@R3U8-TYcvY!s5em_GP#ic17YK+}hxG0C(PED2$b&xZ+TZdz8lc zusG<}|C&?E?Dl`%mg^+pPjn}n7M&$BGHn`!q+_-StOaW@k;cf7uR+=*9AIOo2zM8- zD(?PTeDdCGNp&UtAU$|JvV6TP&8;RvY!bPvChjlm)(h=hPEd9rTKmvbsE9{J#Dz?T8&tXq{A{)uDu&rl^3 zgnRE~o87Go4~{b!6Q%U^q`q8q?|V^em-FAPc^+iGRoH`%U+nGyT54JcodzSVGuk;b z7f@Yw2g1HcKpFS501&dm+@Dr>tO%H7a}{P>*&<-6S6+aWjxVzby?*?n zwMGt$;lPF7Ceu$dDA*Oc+$qgniG125eS8S)^+%~VV2z7!zA%)I*V!vJCg`$~r(HSt zuhSuF^aYX#H$7^@Vaan_?{O_#k_XI#QL~?%hLDDRNQxnpO8_>!n|dhYi;S_ds)(i$ z`C{zv@jPtWJWw#?i}EyiRbZTw)K!|gGQqf583tpoh*2JsbGxmLwTTy*n24T()RK8I z{8Lg0#Lh2G2m%nf3Wd1{JGxzvvNd#@e&fWQxwoAXX_t|Bl@;k**rKQ+b+9H4w$%S* zG0U~cpjloSb^P0)Z}L?8q{P>)&s|rNzW;7YAI2hJGk>hS>%C+cUPqb3U*|~srcc?w z8rr|CKQy0eK>YS-4D!|8rN0|0^VavZ;J>INLO`zlXDCyVUkM6lllZapK&Y`q*fl8{ zAe|n1{q4PdFYZY>+e2Dl)x`?NnZ~6> zmfn_gex=uk&*72|y(<#pCwgA^ z>xFvGO*P*v0JLma8yiN4k5>v}0b?#;lIiREaftr9rRyKqZx;K0E6SX0tI-DvDVa?= z20OUFC-K9Z=^3`W;lgJ-F^~y*@)bk9f8TwLny=`zm#w1ugM;Pu^1L9MxS*v4l z{=Qh7t%m0%G#N`L)3oC%i6xo|rn8cZ8yo zi@l-EM=a8x@%iQll@gmJ6|18i1Pq0bb?;rF&D#Up%Hx0lSzH|jjUU(n(>KP{v&m7zae%u;=vIx$Q z=#Rt8;C>(uCXw8_^fks!ZKX;wka}EVaOJo%CjO=*GHi~^AZ?Bagop+Zp({iduT4&r z?&U|2d=sbRv_9OeR597BXy^U1|Nby7!LwZo$?#VR zDHtLJ!LwC8_13_P_nq%PHH9;^Q?fq$wKu%v^!$I#&<>Cn{RrN4g?HLg)&f#a_lA`W z253r7TX+?Kj*_)XcPN2Ic-5jU&i~c)bbguFtSkJi&bZGD4j%r$6`~Z=bmvjp@ejPX zp&Dl7)kfgQ)D3`uTBU~?&^UX3b zf>ZLaiVUrK6u^@Y)Q>Vdvh_=#;&W+F3Tm-){^M)}N#E^)3YdeCKbD~o3` zZwTVsSNuDJX6r&dfM&%uQd9_~(PhD}MA=4VSDI;SClO|dVu}&~lpnVs;Ie-WWwp!? zVKI&bGTVOpYGr0(qL#Tm&iM=Hk=xXZ+Ti?5AW$G4&OQvw}7L-adz zg+4CRRWLRZx5jLHxsO2KdT_{!C1<7#esuWK9eoh=8-Kg^3pnV(skbXrp-687#|~<1 z%KugH!+Dm$xtL+NicTy56dEq$r*0O=y)N^vD9tt}!{*Inrhew&d6S!O%@}I!{BYqv z6mcPBt;zj^DBI(d0!_fZK!EEos{{@naTVQyWoP=?Phem=XcO6->C zPNlpjej|7+??g9U&w@gA4mSJ@c-I1+XRtor>)wy&I2V?q-<6l);GG4or@w97$L@|; z5dCNu7xd%q;s-;&daS6w9(1A6HtY&+xC&e?^)WAbQxtu38oqWrS-2Y0b)d@ns!ET| za}#7atxbmlMV-iyEc}7uebKg=x}N@9UXs+;gCUH}ZG4IN(#`R9b@08TZrS#~riY7o zs7!-NL%o4;J4u07wqyl4pZ*f*lvl)PCq!LR9*4kE+IjF^jUh#8t( zrU#n}%UuAzg^IrBze%*>uX(`XuZZG?hIHhFR`D20G1w~&0~EfVHR10>Lm!Cf0davM zj|N}#*6{8&D@tGd26)`jq$Ez$P~3q-*qy1CosJyjQ^ zHijVtK@8SEINoCfzW6=!Yoti4#=#Wf*@H{u-D01X5EV8;?)l>l`>=HB9Cxf=I%z_B zK-$z3MAxZ&Zb+7yPJDpRS*FwAkwO-qd4^rT$5$VLNqk~rv!`pYR<#jX?egil{nvh7 zls-0~+NJ=qygC!{!`i28ugl>@tbSz;U2xYXiPvQA3ouDopCu5 z7KhL$*{O8=HK~Lr0XTI23n5FQ`b?pgKrr zqFLiRqV&lUU{jRRg!{8!fgY3-0!E} zT#=j$#wxQj^SwX`If8L-&-B_tW_patUIj{66uKHm!49CJZ5fufT{a)L*A2!qB&ji8 z9=B0!miJjnU^2b)Z1wG;L4_40h1MIsE~>>$jg0_|tsCv;HaC?h<8!~20c)Nz)R_O% zZ#m(!5Wi;tBooHlqt;14*E~(3SO>O6jj_gQcko!KCN*=umO09mPto81gUb$?BqqIL zd!TXRErT;LY1drYENB0i1qsPu85(xSA~RpLzxETG(V27DoF&rAoEwz>CGlJy1IxNI zNnial^kTQ@_w6Iz%iA>(w$;fy`6CDoS?;eLrCJ|4KT)dmQODzvabdTckfwjQ2Be%P z1{NUEV~L)HB!5CvY=rcp7x6;5decfLc8BHDB~0X`^|U1yB|XeeBbU-f3UU!$MvzMp zh>Rny3+XjFBtK;pp9vKjGBfH!fR)*Rm{8(0wU7R93b#Jc+_5{4TIcn-+^w}1UIQD} z5C+l4Hvsb$UL0#D7EkE@s?NQ6Vk&v98Cj?HrF0TMl3altg%uZy21OjCPUYt&47vC0 zoLVDjGfae0GXz2m_9lHU#CqIOr#n5jh(?tAA^i1Y_(v1@mlGHvt`pSuu=37#L|S#S zE(+YX@G~6h8BV)@Zu@eG`rP@ptW^QP+5kR!u!;U5NP!v?P{PPl?sm$wflRD+jId4@ zC-$Q-=5Yk>TABh}9~+xu;%_F+omQ43*H*$}U|^I6Y;>v(o3Ud@4cM*F1b0_1)m}T+ za%ot}H_c?6h1jwsD~nK@brmu36^TVCsKU)_I+3JAbp2ryq)6SQ%?b=k#)ifTp`hr_ zLsU}6y1DI^b-yHh@^D5I;MJ`}ZkjocOq>P@NR3p$yBQBZ=}7K*P1G3Yi{xS3YC>Yr zVT2XyVOXu57p9q%u`?Le{us~QVke;KNjKr}p2Nwco`#f@iws7A>ouzGU@c5!cNi-l zefx()zKktEd(|o-=29U09N|ih0>nCt!M{8_4KAz(TJOwZ#DH+($pT;JR(=6vtzT<- z*>0!K)daU)M)S+xL77dZH+Inu8LgLdS4Hp>IJsu=mY}UT57?9!1xlHniOBrY!UTrr zXD@3j+m-c+Whzg>`N-WX7+o&+xwSRD2~*swV*nEizY6@%qcFg3bv~b_^2B77t40fD z(XC94QGJFLHwy5G1+1e_oWlwD-v4v!I6O2uTWRpfmIff-$iVHZUenMM!0}L-UTu{s zSi}RWYp1}5@k%Xo!IcTYijhmjhF%s+Oa~HG?O$$bs922}wMD;G=UA~zx9O(!ub46A z9X(i`6KCn*uPxJx^!U8!Df!(y4cAwD#4V>+%T%^J5jw^Rt-dFQn$kjf-fa4-#it_e z4e(V}SV_b-PMC=XF!7Gcp-Iq}Wg3`ezENA+*KdhGlcf$wgV%yB;9AE}?7#l9XC0eD z1EuAn>XWzbVdw*h&YQV5r+MO{z;)O)pG5ENuJx>v#DFv<52Q(0%l=M`u6_`v;#8PT{WP0x^(Dcc`+>L&Gz_c@EbxkSHiR90Hh z zFcef&AuG4Dr>o^liL4Ce5la0S*dO6g)rkLiuj7=XGrW}paQtB#v}ikVuaV5YkZ+TZ zO0CP-Xjy$GoG0JL1aSbqn}v4HSXktkYREOp9H8f)!iYZT?JV?5)eSI}@Ve`qV(S5Y z7wVD0@oe}QpF8mZ9Ig0-*mrzYyW<*`)X$1x|C=>%EGBsQ^hgnA2?QJ#pS{o9?J~nK zFfdG&=s^bV>kXz}Yend%`&~GVhjY*jzotxf*><{o46K==%S!Ev9y-8# zj5r_$%RmoBe3#%Oz=uqVsA>Y!0w1P|y85@uN)MtnK=yhG717ET6JJ*jKO-?$Z5W;) zltTxDW(Hy3@q?tf{nJm*eKj(e9Wc{adfLY%OG}fX=HDfF2TbhrJj*X0LDLr{HUAdL z4`L`WrHuPAFm@B2n9u@I;L|xAAYBeVEv+j?golTF$SX6XxVNiF^pggcm)F-f&MXKy z-PXoqQ0;h=mKUFS^mzp{OYOASQ=@c>0zH)HKdlzP_4+~i)TtANt<}yGss3I9j8CAQyi9n#VGO7P< zfuC2YB*>G|LPaO-G4UAIdGc^A*1NTW8(utX>p=IS!Zw|*>b<3v5rRG&Aa!>kWhhc<@#(ILXaT#sL|(yxchlJRmbhrIRD+@WV>xj^v~>!x z9_^fWA_kUYaiah}cpYa?@%^<1$E<8NRG4k#vTmzfQ=7|~uLloiylVb#?u z@^gt4*nu!zUaplKVvJvlT~tEO8Gylk8M4;pBu>@hjHc9Jt!q8Y1rcP=IP~x@>?X6e zP-0!kCgk)3kv_woC6qZj>|q>{Jg$iE&_qsFUr@)50@&rOYA~CTToh^d6hT~+St8*u zCpq>O_fGmAd2ELO$sw;F8VBD|fe~wt4Pz-B``LiXVuQMaJti$~EfDz5e#;O8Y^?wH z;1B&bK=bHli!T3(hvc*GOaN>O`{n@ri;EW&G&GHnez_ts0aEEX&R|e#am0Um3&=7( z7-q3O%G9pJ@S6`#0Mkl^!4KQ;=R$pieMiQGIhMR4)iK>l@8y;z1t{H9H8PNh zsssRdvPGzYhUb5uXqW#+5(1bSv}lrobDV%=jgOF0nOZ&oiH(?*Pl`<40K9czKf{eU zbWDufSX>-BO(7IW+k!&FY$lQ_!~4#+==ZxzXhHYnI7NX6Po;uEg>Itb!O1YUx3_a5 z!VKb}Sahyw->VOYGrgR|(+gh4rsE;8r=iUiRi)A2P}f5WKF_u~#+MII?-wxRR~v~5 zzrLAUUBLWBjr3e{r{R1M{~D(FS0s1hOOf|5j!oVGCOm9fxiTV*nDN;3PX6ln!L==t zJxAsFw`pI;({4~U!pgphH{XvQqrO_`VfEZCvd}6T3~#|eUdxF_x;YF1DJP83K%+fO zV7yQl-VfZX|9u31{#PDW_3zG(;2ux`1tL#uklku&xg1c1*jLaa4Vc6O@P`c~rv9S2 zaXf=6;1Tw?RE^yy|Z>fyFR2lv%OJKqq}6 zIz@(mci4F?sKn^B7P+jBmv*dpxq;aCMpjEszNDh&T9v)e`80R&qhj$PD(Q4Rfj`3c zdpr=}T8$fij%aJ4V_*Mrh<_PRRo;p){_h;vhM{rN3798Jj2h6bgG}jzw+CxI38By_ zT-yOgZ3E&O11>IYWI4v%mn1iDRjqkBlPl-?WI*xzmoZDP4EBEq5b^&VK=6MDFctg+ za84x`-GV`Y(X_%`|9`Be!A3hEn2^TJ{re}uj9bx)`11UGDS)oOXl-Rx%7=y{nWx7B zSeX4u|MOidL=@9LL}Ck<^9!s#j#Z>sC_7D6oEb9+7F7m+mKT4{?DtM2Q;L$DxI+u= z@jPhW(Xlfsksu>|Vz#E(im0J5SGCTKZ%-qtVh|rwjflwpdMtRig%slt~h#`s(JEI?o9lDx8qdwe?rI5 zjlln8Wp(i|k|EJWH-$NgInLQ5&Sl$VAE7u~M*MOSRI#8E8(&+t!csis-iV2<<>{WQyegO@xRy>q*p}KqRw!SL9s0=Y zX^JlK4j9%j?!JoILF`P8bmbWB{o1oI<8gG@GO+#n4`zr#$f`x>_S|Sn!{CRi2-;Yx zM2}s;YPwR-*`Lmmm^e!W%vdO>QKyzccpr4)&4L|21}XyhvkTN@$Yt7ectQLi6tNMk{MB#r5{ODrq)M8nH>c)lw3@MO)9=WEQu}74b19?U} zKue{Xxq^}Sz?oOVXaHF})^`)WWVm~QC4nzB0BiamHm77RQ!XRY-&-~cjr^%!EIrLE zA)?}mV!3&vm%9_NE%rLlOQiLMM)Mk}t2$VhxJtk0y>(ToG}%J=YLw#b`||-~&MA1p z?_#FV?A%+9t?}DazBH+GwP(+=B0@{pCHVgi(qZb#-(9vhXsNvk+P4Wp$t_n zO<2-Twmev9h6ZqvNJ(pLBQ(GX3KY+uIGL+O>p1HJp{S>?2_yaty@o;)D7O{7(*kGp zUm^|xVIzBv7Exw{&(04dP63`9RlGvHoMkz5^i{XpqJCRj1_(oC|MiCTzvBu2++VJC zIJK|U4K#(N;KU@zMO|;FCh?bSHofaEzr0 zPbD#^QUM`)4TOv5L>y+z1qcFJrAqJ*UFSBol-p1?F%+&U#OLk2O?-u zSDt;%!$b!u_>4ho9{EOh|A})T>;Nh0ENO~ywH3m_pY))q=h%S9d1bnKSGN1O)ecrI zDx&>cX!71+$kzkN3J4mCRBYC3k&6iIwPiQmB@yCv_z7tGJVL1L)+Nc9S_hLTo~;7o z0=pYD0(wNua|l~lf=0bn8%IYDCBX)u-lla~G7bYK<(P;j8dQ;kA3Rh=kWhh2Oive8;3czi>!|5=6 zz@uYNvjZu>9hT_F-{M1iYZ20@(iv&*c*tvMfu$K2`oJ~28^s~^xHPWv@}_{YU+9+0qAz-ZEXxyk*Guq(m+9B576?N$IAI$fj9qK z4gx>01X&hE_q<`qsO8t9ufkYXB;4>!zUi10BU*$yyaFW5{)96Q8laZpfW_0 z^uRLNcve?bpvkR7pnVkV=VW*sa3g>tjtnZMg%uTtRg8Z@h9rV39IK*YLJp`e%58%A zE9>bupHJzw-Ak2*fCGEb(3?C>N?+i0MI=H}Ds`gKgeuLYD{sWaCfM>eV+a3@+{b21 z1j(MMnjn?f4;f)w-o4l%-rd((10|1EuObMqH;qbQUx zc45qyqNbCpU^F!`WP&51#OSYSMOCbVI-`4VlBq>GzSp!Qwyy$^s^0Vm~1mer>S0x3uXkcXXU_+`Q~^ z)a}b5QMVwEbHt6DPx@Hg*YP)T+;~o$y!q926l8UJu=Xf>Y=!FU3H28y$ZC?iIxPq7 zIcsw{Yz)_@!L$8X3*J7T4DTUiOtuI42nl1+DjJ_1yDx9dNSV(MyKnNE)oH(9r*zf& zy*+QAkJ|4d1+#dk+j8k%>m=_d5ZwXIS09@)Hf-Eb(B?%+adD^utvDwk4G1BZUT)+W z2z)ZsV&tIQs;W4^P(Xy(6kBox?=vu=x7F!DVOR_cmRO?NRrw@PPAwLQgRGu>4O~gw z1WlYX51eg!HFJt!Lmd$UP zULX~9Rw_xDsi)S%qhM|2GzhxTS4Z^~afE-NMrdCH)N~p~-!~FOpvJCEQ-6&X$q>*c zQ)A7){p70N&6n=ps`TiawsWdZJx|b_{9Ig9GvbEP+I%W4@UrIeN53hSV@d#8aSo1| ztfr}N*SAB0k&$hjSOyc9RwmgOKe0MkF5H^e1XGJvD;(TB7oN^c{WX803x6*gigODwFe)+>UBYI_B0fOs+8kno8#l-ORo-@1U5JzKjm|eHa5s# z5q!p0XfKaY`h3NhJC^Lh?1Z)=mJ4A2|6-+aVnsoDy*er~HB^Lrfs_a;Qy7Q^3`-? zcrltGcO}tSiWwHCoF|ED67Im^C&=mD@pyYpz0E-M7?okQw|F8);eLWy+l`GHd+%;` zGPSn1Kltw?bah{T?o-`IP7Lo~9%f@|rkT4h){4|3or^?K28`667S!>_6pkPg4Ckz0 zzsR0)wj^+ck?7Eb7fTg8$(Kf6nHK66u|IPU$xHg z!XLOz%Q(Ob5z2{k$=C5qo^|13q-6T4t&-_5hKeBIG|eFX%uBAQU*n z5HXJq0|8QWb8~axmsdYw(l~h81YoM6ICs4YIAO>p0K=={S_N%wg8BLRe+vuwCwv;T zU&oGVVCI=Ic?D`CG;5~KuoStj=yNM@(@pYZ#MK zf!p~>`iP{=XiVo~i(#Ec?y>wXVA3N}Z#_LJwghHXeN}Pxw7q0qxCIsb8smXv^X0GX z$cW3h5Px3-+Aei4|DGzV$zs}zrNoE0ICI`y^-JEE*Pw?$*qUg6>x?j$?`y-Ftv58G z(Y^z(wp{3k6APzs9L%4mk(H#;t}KY9=u9oyOd${yq}{f^L%0i6!yXGyIF*n}EPE4% z5m2`^%@889c_}QP`x;VRdwcpfQw#zZ&^sz`vIm%L>yx!()-aE~OtkRhqc{)XXkL~G zzeU8}{~#nhcJG`;7)1U2V;SQsBnSv_L%SnWQ~S=IDTd@AdPEA+amdr~(61O6L&gYu z>px?4tCs{1|D*tYkVPFp+%-3+!I6tbgNjB{q{)Z`hA~&A?6tqe7tJ^&#WbnSw0jHW z=xd`Hb=A^VOY=WnpsDQz>MBFo1Wsq*R%HnU(ZmXuZ6!Mi)r2^r-&5z&!HSZM`m*&@ zQro7AEv*6xW23m#kttMAOh)Y#)tcS4q}x0t|u-5Vh ze4HJTxBv;2d2(VM_9#t+(6X2qY^_K_G$yq^_UI=z<6qZQ;&~@ur(Mc7UrE|PS<^d% zgS5p;IifB%xF$7AEL~MmQDYlJ?23FVt5;7xdd?#>(8+ODPRat1W9;!}un~d!)3wYL zq|0#$)crJn-gap3!s5OuviZ~fSyD+&fw7xuB7cwdahg8Vo}2^tS;B_;lG2#Jp#Oc% z9GJ1x8(t1mI~`>3RQge#W%&+fKi_~Tn0&$M5~why6ZQyNN>6z_+05Sm-LyED2A4$M zuv_dnU1RBZewB!L#=#LHxw+zhvn8%n<0#4GgmtOG?stHZ>C^K>tPf8_`pCEGl@Cqp z_nY70k$%7Jxo`9A!u+EL2Vs-Wg4;pBhxG>9^15Mekc|y1ENUzaG{%@&AEnh~Ptw(P z#>MisvX}}S&ucW*Yx{c6$_im+^Un1f7ud*OFJRi7jXh1lU>q4P$SOU#YvBh{uo*i+ zV2_cX--lk%q(I5vDe#!XoD*DnaRl?2XOD8SInXR66k@8VBv1nv-&c1-><+H~WWETsQBi;K4!<&oEm9s}sXTs}t5hBunT*9gHlLnV0_8$jn*hnre`R$yb6 z0p|+BcQ91Xils&u8Od%^HxeM#rb~XSqkk-g^s2>ll1FGKpb^(0IBovUrc6xy8n^*I zA^bl)eFaQhUAS#=cXu1yt;O976p9yjXo2ExMT@&T6nA$hE`z(f7Iy}H=f5}a?qtX$ zGa+-%+56MA)@N2V{Ao$_k2?c0^tR$Bg1iVYqG?}9R3wF{?>m^^ieLWN=1Y@#+WmCf z7DIy#?bII*j1e?unY!Worr9-OTESBv##{N*_8C8k$wr2a2s!*B+KQ9p*Z1G}2(u^d zm(QXHZz(p$n14?TZ11o&>GzFa;&f8mGrxmAgPpp!Mlas7i<6gnDgCjEZO=|RY8t$9 z%FbJpi(B5>{kE3Pn>Y|-|Czo10RI*^h8uezj_DBG1iyunn7oK^>{RIAB7e@nk^FPK zfufC+y#A&!(S3_iL3$u@By&Nx;dy5(P}ly^uZ7V16_U%X&9vPJKoN%8&+`GlD!So9 z@ME!(q+$Inq{uqcZKKtD=W#c*o!JZaQ~y$-XtKF6A$(ia?wu!aBp@or`is*%tfHGI zci4!q*OOWKG|Il>^h3EryFDj&2YsOt%=m)T`80_55dq2>XS$lxiv;wGtIImj#-gH>&xntE>nriBBEM+kGc#e_NY0Wpr{pr+YC=@&jh_+>{u*CS0%VsU6W})N1lbHmrU%uWn_bYcJAX!w=3#41YG08g^L8j&J=+ zV8lef;tIHHs;1`e?Mnx}<2CjuLi5nftHw^%$}#FaO#4Q->!BcKv9au2)sg%UMAEfn zfnj;~FO)Y5vJ4`ITB=m>Ul)xyTD<7jcJzYmN@^p(!`nDR6q6o@S%M`Th&1c`N1!Yi zQrWa!9vthFijMoewY(;Ug=O8;el1~e!sh}Qf@4$2V`-?GaK)xga&#g4wc{p>aw%Hg zxumLPii6%S4_OJ-F1gH5lZT^WG7Gb&j{X_nQ63`g)YKIPk1R|3|Ly*vg^+*QhcL$0G(L3ZH?M}ma(_{SG+~E;|@4NykjEIy51*y5+!mcG+GXCG#?r>A>G}3FVlqxL2C83FgP|?|bp2>Ok9c3s)B6w)MH{5OkF)gr%11w) zXG=FW)kFFQl$3%a3D zo#$^9wureROUSxX_u;Ym>InRx5#ud;@Lhkf{7IgO(6==gM87sxuUs8D6e<79tc3fbK=z9x zhwVe`DIe;C#`}lg95+XSrPIig3i4Ojsv+;u9`d!x+oy7($h#`c-533znGFm2nXGkN zjEG8yH)@1A6lXe2XfrG|IHHWlu@*#<3vBpg3wC#toh@!| z(*)uTHpEx>UOvBUjcAHdsgqMrccy7kw8trxlC(eq@O8*jW@KUsq%ChTABeF7cL*vt+s?4l^r5j`^C;|b7o9?Z&L zeX%t=(mrKB;$83<4pvw3hbTx(sF^gl?qAA|s?M%U2`*kbBY}Ou#o_drq-u zr`vYad&7h-Diw0g^p6F?|G1#~dM-QoxzSKYLT19bMz|7(?IHH$n5xa1GiELE&-uc; zu|bQ~yzeG>;(iv(HbQ4D9d6-rw2o~eo5xWyUjJ7OMY4PyJ-o({)4<|^s(iZpkO02$ z9bxzD!m9Jjj^_k1FDcm#$6Lk4RfNx#rY8Gmif4M;bb@4|UKCaYC(k~)R}}KJR3DPm z_aptaD?j$y0sptq^SOUstxqcm@;;|OUXD&08$_I#yWj75j~>=W2qT4{qK4OMF%LC+vb8~ZvLC*kI(p*PfOAE+Ova+&h^NrtQ zcT7k*r>re?G&h2mHd#w>_j?mSVa^pW@mY`FT;$S``yFMNs~yP0(i*{8y;OCeHUGRl z83_xF@UKkhO!4;HQPut{xrj)~AgbYcxB;#Kd^oaBHH*uucpVUCdNSsNqh?;EarjUL zRdR7%{zYfgha*Jaf+k58HB2n05KLH3D&yZx$E158GBq@64`-fSwBNrw5gJs!9THN? z#Ap>S);!_SDnk<&_IcCGwYeLX9eHZ00=y1PDztXuuR??a=m)J%G0u`v`g$Q@`mI$& zwFLnIgzCdCyGv~m^F1ADrH}ZNBKGLtir|idn3+n?b0Sv9;?|y+CRuC>&P#AobuYy0 zzS?-~8?rW1B! z={PTz(qNn3Lg@{X_^%scrv5~{^=pa@ahHQ3MxIAw)vvX+;ry9TwY86O z)A#3JH}pD_vQbNVzR{7pBO=T_KR7;?vtHw6C1gG@;|JU{pJo!z$7`%uz~+C<{F@@u z_?skqW2}k5ZdZ_fP5!RIgVgW(IFqNCygJch9S7rD27B(<@Zi%TnQT;Jlx0K3n zKmv*&-_zb-|FiVqI*-#D4C2o)Q+K8%u ziPEgAA3Wb9Nh;;LHW7KdM?7oRw(yU490$7=ZhlUmcRMo^&9cWQ@M9Ve$ezx8oNkNr z`q1bA*DHRx$nCMh^BvEBd5pg%KDv5iuseHmj4|iLkK@yk`_E-L}2_&VexRGn>v$8C0~huyK}6HBa55|7e5Uy6YX z{w{nq#;@snY(HP6{iPpOl=h2gLXZcI%*%PA*B7`8`gA^XJwq3*iC3Tz-wwsagJ_+plC7_i4g-T_fIFsM(O_=DtLB77vJoSkhn5>Z{_ z{eyH?4J=;l6AYfrq;gEB)4G~szt|g?A9to@{VRJGS-`&=FnL*vopSg0ZA_tiz_hzN zqj=MbHcTQyPJz&2DL$V2+M3Y{(GksQyASQZc%fn^ zU~lr&@!ZT=R(GLmHuMtV?))o0Z=-w9OWyj{^Hg1t8;TT^&Vb;(G*RXw|^ZW1z{2EBV#b>FXhH`HhGmEhjeB+K%(#OUCFH8mpc!3Z&oK-TbL)Zou zswX0g=>kS6u_4yfMng-bSb@AIlo>ig8Do#Z4+H+GXV{5%l<{RLa&F4=I|{T)h1i9& za=;W6V|lA-mv48tA|K;N<`PC=BNmiD47m06IA92%i|70lj5W3=Jv92>EEyh}-(nPY zLhbB8loq*GTbbqbU*g>_uX4)l6u)x7h@EtA`>Ar~#86&BqE}Zmqfb^8{6hPa^QoU- zy`j4&9-m@1CPN|kdz`iI=nYB+l4ACFtrPT~fM%^3#7}2WIL>qkLe-e0m1EeCKWT-| z=Z(LgF3xpERU>F@*)u{a?1t(~1e%Z08l7%Dr8UrFCvikp60N;Bd^e}DbO5M>J zIFzX`e9_k1o&5dwuz!1r_}JeMBnQk{z_=pQFKNqNqMzash`#>TA=ntslI`ksn(X}R zhhLHVnnmDonMDAZ3|qXKMft9Mqm1#PHObBX7~+;#vC-I?K?*a_qX^cusrw+{aezQ@ zrFhl5_pym~26oecwl0FlCtl4`>o-Ni4$gL1dx94G`<((U0p0OkjN@wzLA)7h%Gy15 zw6%}gt$g~J{1DGh97u>e#?o#%;-$PeA?Zg!>Mo5lOG}F3`W@-Xlfow5ddhO4VaZ;` zUmejWVW;Gm49gt9ILJ>s^RwO|$awFe$5|Ok>&x${DQ1#&7N19aJ3kXRVOkWf{#ikH zw*t$_bnawwTiKL+B^;oAFdv&{glnd0BUV+{E5zhE(sT@=hY|v zCIi|=7a*E>ruUVh>3)9d3AXnM)n6(i(jTjQcj88Yk-b`GW;60;kUe=3y&i&GOeL8j zI&h4K_z906sOIdP)LZ6ffBfb5cKnm&b<8O?rrhjHjV>QKxLZ53=}q#3hxhupy>ZdK zJx}vXU(YA^lco$0o3mefk7w1KteFM%yvk}NM}Ht)UuckBir0H_iotSeyleNNz7gEx zEIMiZq9=rdf)0aSNTl4e;7!dAna>9tbV;WfxFiG>gB2o4eKN~14vb@wG0H)Z>iP<> zQT`SFXF*yzA7>d6SUnOKzEF+z0dAB}@qnfA->MZJ_lgoD?6Ew;fT(;t13UW=2P#kq>iSv<3_C6h zSv-+kbO9mltY%DS>HWIDl#J_l!qG)d3|>khcmyF9f5evmDW!&z*V9J5Qu5V!C4V(t4Ado^2fm~-P!jw|Wxs0Isj&%r*4{37|pRegGpS`C+!-Rdlh!KSj~sg)y% zW(~jYK;96=C||ew^+YMuDMX z|HR($wlQP&_NnX;#kc&%oueQcpVU0;G-n>mF9PZHU2_xV4{x#WuMe)@-XVz9S)Pm` z>pq3Y49cncdY#(Te68Q7z7HmwFQwf{jETHyr*}yEA$&*M^)cF~sV@|N(0@CuE!ZK8 zPg$8Z*2W1u(DL~7eF3tDJDHMS=q^au1T_7nH&-7nPv|vc|0DyIk%`l5J3!i_ zphE|wj{erEePJznhS36TjFlW_O=#wX=(y0#h7#6Ns(Dg-X zwRdP;?M6@;GYH&$;9S(samS|4OAeG>oR-Jp6o;>s!#&GW_k@0#-GMP=hCMj%VVwKD zrbLBIav>d|_g)K4Y6(mN??$#C4D-f!dO=gg zxBNdQ|47l($Ru{e(D`e!Z}GafuDYuZc2kr|I&w5W3F|X5t-yMXLn%A&$z*>wlX&3?HIiO9qoduD)d<%MrP)| zx(aVO2M)six{3}=D?<1jYiXcU>D=L04ZfHEo?>`@RRFT0v(t2p$r8R!z zwW)vT-Zy*?uLxXG) z8Ngwc9?L^zdMG%SA4o2x>EcRjZpSQfolzm{L8~KKonNz(BE3h-!v6gjF=*S@xYwA$ zgFbC$bb#x$23GglCm0t)x;Dc8>6VD+)6bEJ5ugpyHv|-6Z$dcfCv!p3b9BM>CbB%E zUdRt&UQ-Sz;hBMr5Ywt4{5fQaWP`p6ex(MMCAm;P>>;HvN?m+SHf1mN)a8Mf*V=)? z4D^2a3?0u^6HEbrrZ0ZR6$ou?*Gp(tYl z_hmu8B$?|hcAbCf#Z4^N62>Cke|UQb#0w&t395RyJ>YkL1Xz9dA5|Cv$Nl+4V6pQ^ zX%l8D#X9J7suyj0NvWfgf2Mky&ZJ(7A&u`h;Iq0S|1AM=e*Dj`fr{c7?MF;yK`(eL zxJW64Ga0yo7p_h85hxC5n#4(*Wp zBNr(nn%>zIt-})b(NBn^#b{V~JY?GF}^# zpObaQHRMkc#~YSlk{M0Bl0C&}_Tgn6iTS$SJWND)wV>&cQ}0S{ndwPduq?5<3FxPL z{JxN8T7~$IHe6pzV$^X6q~nPA(IF&Vh&tvmW3Je=<}i!6`E9$HVGsF0-W|3pK76g} z=1VO!9WiWqs7=wHUX!OV@YX>YEFM8~kc)r0;|bnAn-EbfL$Jqc^ASKmO4S_4MkxVFu*)|b={9bGHG_b<5xauI_sA4MHN~s}(GgVBa znOq3*L$Sfqy;~uQXk{jx)9qybv9ju@uD!t;tBqFpf^g4a0gx$U$vq^+{UE|r-r&cS z(SS66F$B=5!T^^5UNU-mbXih1p{yz*1$j7ru7SLo>huIx*_qsyT~K*-ro`op7X@o@^n*>1$*56@#gaK|ApnJ6UqS2u4+>=It5OMzf ztR1WxrQ!jHC%NYqK-y`WWfn_Wn>4;YTT)UdYlMFk73wf{JM*t}m^^^%7TIf$a@AR+ zIGBGAflOhi{zKZoz?l6lrp7;=70(amQy>idD+4+PvD5cnB%e>-`OM30qYD52QJvcbj z`g}ZaTbsHtNW;H17N;9)#-aBMvjhTK)DwweT9>Tp2ruK>j7=G7t^DG!%T7-3aT~1X zdgWYpOZamQqDH>5y)B38K9+|u#ZV^a*L4!wm~Z<9PuR|}S>(@z18?;5$5eIt#+Ly{*N#HtPNOd}{O`;HAzXq{^imU{ z)IX#LQSD&^rt*$UWoA{Ff4Cna)U^0MHX-M)c|oKIs7vLrh83#8F(Hw?{88WYX6A zj{JftdEr$jKwY0JjVHu-FNQ4okwG4NM4R3i!(C%a14$m+M38ELAqzJAr^A0LW(l?5WVkD?x;HN6_@?rXSxRoH^W=j;Y@J*5Li4VPevd-d)Yh{Km`Lvh` zHtn1+UQ~)fod?bt^qEQ<&wm47;Ol|EEts602}xkLH6K#=8mB?z$XI7F?YV(0Q72!) z9NPRSEGZ66?osV$Sg5ug32c{S^;E;9P?&de8BjJSsY^euOfWKyC@o+3F$F|O^zrSO z$2vFq3AEV>Xr=R)gw;T6evBxDEzMF7HPQATOr5RX$uAhhP8Mg6$OTiOzsyWNz8yvz zD>M>d_U0CQqZYj0|LhYCn|#@$*L$h8o_q4wgO@~8qZo~=#g)mj=he#Qi&8+VP!DJ* zM*ZR%+u;|VGjjNry6Jr~OUiAFJ77Wb#SB+1Iu%Tq0~;Sxn$w<5;eF3An6gZ}|5A{8 zHv(?GN{{(k>5rSn;xr_svZJfh{vB-`DN5x`3O$ojVzF~pWODwHR05b$qK-kx zQ{EWra2hEydTr{7WOxUP7CKyg*$ZHFhq66g_CAR>C8ALMD;GZuK}wL$xc2CBx~f$y zo04y|HJbmYBsKqG-qe!2QvxdF%^)%Bl)m!K9-X2v;3u^cM#&O;U*t1N5QRX;d}J11 z9MW!j$r(Ek-eu^DS*#+bTkrHcDVt3sIz*CvO1s+cV?p>#{+(W3;)J9k>}bM=Md zt(EcbTJ>vChFwQ3$GSZpMRjD|76tQfUFZ4&GYue~mGQSss6X2zQ<(;?M1uDu^f zx>u4Mw$tq`C*w+pChY5q6=t37<%wL+t{`LXVx##$pZF7q8~fJrQaZ(i$cwwW$fZ?( z%M<{_hP_~4on!#f2VfrnfIJ5(NPhH?+PS~O#JJjhh9}C1BWUrY@8oV~m877WU70Cq z3?*QHf1h^szbYJ5H(efhDkEee)&62%kqUMcyCjYLk2*%H6vo@xXU5OAQV9sif$3@h z^50mcDY&QdfWZuD@O!X?k8-Y#Av|&K6fXW5j?Pj2Kv(1$y5a#cv=KGTRirk1a=?Vh z5`Ji9XY3NZ0=LbV6VN;|$Pg~R>|&0HFHvl%{G_6+WUH}FzyJe3JPJ0DF`x&3xLD%2 zi9VDNE)J9O#~1YW^u&%I)j_xz!8FJrea6Hj8TZT2Z7nUhgtbK}JIGDRu-ftVN=p-5 zU@qwmaWy%GIw!A^_b0Dnw|gC0r7yB1-$t5Cwpgnv73sl-tNy%pOKopPx?j8&B=@k$I9;^ z@_xh)i!4%J9c^3O1aVaUsHlsP*hE>c3g+)0;a zQ7PiCWec(>JcfUnii74F;i#(L2rgd4dzVu4$B?Msu@FHwHva zHU|XDMDI!=yC{}~{QF71S1x$I{Cmg+6aCj| zG4%%@X2XMdv;zJL3*^R^G)s#2Pf-*(a%ssXpPk*9UQf}S2fcU9=tOV))_iY#{!FGy zRxbCJv)jQHq0fi_C9iHduOkk&EWtt*1{i-j5eIeN_qQQYcWzG(C)x4bXj#ZA&7`V> zGg?|SVWTW*UnjPM(a#rv-7dDk2M(}X~XJRWk`y@iPq#s1?6Oa#jr`bmywZ1*9$ zg>y}e+q;J-yOgZMQ>oe|RBO)@_k+qGY4(~@8)zkSrqbl31`w6Xx$MWLi@fgxg5t;y zQ*f5E&LcUG+mk1)Fa6)XyB)V8rk`!S7$oV9Dklc-=8z2CE1RrdwcL^k1Wq2RfR+Wd%~&zwk! zdgKY|ORn1Kl1{5*YsSL~j06vHVMRrxI#X~ygR_f^s)2zrTZ&{nsKbE0ttvse_$IF( z0F3?*rg2n@gH1+5LnynKHOyHcDt{QD?|WN0)Pko@?tmKDumwtQV%m||-Kf+owhl^% z4DLu`Z2PBzGVE+U3`kKO69Ck?EMm>Mv5 zC*0mKqKe5DCQw{$vHxQ#Bv1)@G(jDeJL7)pObWwcGj#swwWE(1! zk{)>UGHc46^8H*I%ifG~53egaES3rgV>#oHDN1SAU?5+KDH1q^y$BBJ=R7Vo+PwM% zUr527Ig4R)r1XiR*nlDLuj^X0)5ATGSUW@DJ4Gq8}o< z@$lum_1JnQn|N?hE?-;beb?O)?fu)7($Q4REe<>s5Ah+H{M}~G*0ENzQ~wpc!?NY# z&FawkPEm?jxkwO75ye2zAaE$25H}x7$0Q%#g1a~Gk&#dmFA1aDF72Z;NZk#4lVrFj zH(!M+tey%08i*aC~~&~qTmje4L&kqVo5RIa#mOk~Pn^!+m3H-tOO?!%PLH>8_#|) zEBemU!I5O#MP|Y&uP~OE!ah%mT9hyTMuhMMvN3}vclito1|LuXsICyX5cwC8=3Ex= za9yM^p|hLYAxiS!tpLhccI_QL=m@e9^mq^!axfq#CXsBuvuHl0F^^qH6FK5-l4?7)HHz^6F{@m{LyZ(EY|voa@akL zlp_$O?l_skO%d*<*)e_#gAU44UasPjKl5i^FJaqos-`nRQ5n%GZlbbjNx`#5cqc^+zk2)WP zo$(es>?uF3c_Jk|sD^iT0s~mt7k!TL|1e+d49N!HL#&1I&n&K(QTXm#zF?xux_0&2 z#e3esPqdk!5ePlXPW&LPeXzXg=rY5awH%|4V^E@OwVtIT_k6?(=%EDE?QcYV$W6n8 z#6NV4{vE~p$-V0j^N`Asp?G`sFCUK7<5u>541N=4h*Z$~qO%z(v=6BGK~WM%VTVas zSy|;)RYjwbe8fbARlyBbVF;Q$EXd3I1<)I?C2R=c>sS)N`wYy?;Gfmql-h~_q3hz| zaG?K<%@mM)w6z`JQR15ZXrgc1FQ|)Hl!5dQ^9tEvy!zJ{xN5o}F?}|~AUxk~LLSx< zG(@c&8q{w~#)FB-foG01*!YbL3*LrLD1k*9si?Y?JgBOKw5GowV2>1#4~>LivXH1S z=}tQWp_GE~x@V#E9mzcZ*!^`=r-;FU4u1`*e1-j+yp9PQN=^Q-!Y08mE=gEKiTp%q zxBxDdN*4beDEu2>iix4PxV%O*pkWL^=P_dgPd_!q(DTZtQq-hcG3)z4@M5rRDZ^oo zQ|zQxlMB(69JX|CRZwt}8emst-Q7bwuK?;zgQLeq$?3BB|?c+&C|cRx{Cau1!_pH zS}7mJG;IMRUJA(1F+U2Yudct2`3E=dC(Jwl2db4d;Ky?JSdML zP%9r978;N{6q3QHghqwV)Wmo6J2(nUdlrfNyOf-x6Ru1O z5`uBpL6wicb!IkCMT=mD)L~?(T6Sh8 zGMtJ6F1{(=mTDw;AF*injtqC9Fie;eB0Wk{8;aszcFsO~WZftL*7(1sR9(t{O(_=U zKVRiRI!tQ^J)=Vyx~aHKnz-@-_Z6Lt*dlL>-DeHsI+mGT1YR{1JExfRsB7pr1a>_$ zo#yh%tZ&G=R$Otzq0B8UEvIDn7I?cilP)o#Uf3>kNZz{#I3evMUT};;F((vRpE`VE z%_}I(Iy-@j%^1|TAb9_UXog(c%k3; z2|Y+W=n>&nFHpMIXu=i!0QI@n%2BEp@DxbTT)n(YwqlSjPQ-KUo?LruSa z-IQhe|HlGcQ}tmX{_MO<7H(UIWACn!Z9d$oYmNn_`HPBaY0<~kU1wEaQf+mrGv9mq zG&I{5B^GwdYWw$a={ym-*p+07>Rx zlrLIaN#h(|xI9T=-~ZNuG6P7w`fuE25QFBSs7Sk}q0JL8wraZG@t@`|!h$iMS%SD# zZX0tKS&;knGX>v6l zeF7}A`DJachAtBW`;y~bEJFJ`Tr(;fNk!A9I*pF}zTG+h(%UdP0Vrx_h}*^#U6SI| zMiUws`_Qg$*ypIb?-dW&pxuDWqDcXBT8t5TdU+wiLIZRnx`dv?n>>ba{Z_uuY$+r> zp}A_YQpx-JZHxeMwp9=PKOvh08%>+l5f{YEL1y+kyW`J6LMjBp(3?Fn1VBx$|5l0DK(0a*$&gCoM)LUboYwN*WDd;z)tp<0%mBDEa= zrUad0dhh+ct<6eHwhdK%894L<)_Ja)j)t^`(t0id2hh3#mKV0hY)Mt+!0ug*BZOA|nvXtzTzm^>j&d<@sI7PufPVkTb)Nm&K(q>CtU@6oEObM& zjmc{UyXrB42fPn4Ebo;1B`zNI)%RY3Tt*WSL@X6GX)Ns7fTvhYbXvj94wfFjPvSgW z5wj;BjY0YOqL663><0o^rO)bg0R6H^1CeF|zZHQMmVY4hc@#$M=eu&5bGQ0g(?)(99g8yA9KEXxDh1?>0E6FU(7srD3 zu+S+O=(XCB(h>MvK2NBNS=`X}j+McESPND9K;bzbAg5pL4yggwoGEFv9RmI91HeU$ zbywB1uH`%0qqu#3J!<1a7hwCFHNCp}Sy~$I1E2-&Y_qCxlK!d2M7IK*A6ZS>lF{6N5fnUf>U{XQL~S9t$u&&D1zG;iEFOuHl4zP zt)-~+-R@t`UsNvK$<(n&6PV~RSv2B-d;9nLnba8(UhU|KY3Is-iwKPnB^5TXvu~~b zLH7TexDK;afFXyk;yc>;zg4HES)~VHe*2_qDG4K_ZMRL~fFH~QOn(C|5g?cvq`Iy7 z0_;Nwa{ucMqJW*KEQ4a<*Q2)VYAKOK-6@YmP#ECJezrT@aW%0RiX~A%i^P4I!h#q6 zE)F|qM)hG7+nq-+hB80OEEK&wUfYg}JWJx@q}^B@E^%ms3Bqy}X+<&@?0y)8A-O$$ zbb;N_7sguNfGEF!sWyR`pG$+8u9?RXhPxsfERLYRSWT%MWu9UW*XuK+5|A4Rl21%YF(rftF5$oG<6)pZ^!Icb4#Gdp%>??3 zHvvWtg2gK}YLUPQtUbvXRxmNwA>b4C9Qkpjx9<+$6>H0kcVc9_V_7WENF6EM^snZyczl?%asYSh#C7G z@4R^GMWY2wFc)A}j`$Fs>!uQL+2k6zS|Yp_%t2eEIePzD)M$tQvn7BgC9z(fB))0k$bbvLZNJo?D*94woRG3*@loiN=|GRsI z`m_;ZJQZs&!TGi?)j@5zRdyy}FW4ZC_#>=>K?<50W2;-kMrNb$9m$VW5g1KEUsu4 zXsUZ=UeJlJ9hhM#cK@}(sR*Y5c$$v0-9>J3dxoA}F4@L@U{skku&aQ`AnQx!>Hio( z=vd{#qvLZ6aI(_v-9Rlol8=*z-yA_0VLL#Thn1v~=)n}}&Vjn8O2GhCiSh9SA_a(v zpgwlyS`!0B&!QqaAZX%5rMWo2@$B>9c5H9NeU-U}eAcZwOF{8Zkgq6pIa!>EAG;~{-i&4iM&HOJJ>RNL9HhA_*! zEwDDFp}9Q1Laz=rem{H6gG1H9|H8hk^&p!rHvPuGsYvDRDXPIyz_pF z>$hCOOC}o?=^H<`x^Uz{(`kj-0?8(^g(EeAF{ftt;Q)~0_>xZ|hUOcx0)riJ3FIxP zPR|IGZ3sXPq=CnJJ-pf-26PBWx}Z6;%QwIK2|9wNo0_igwhe2HMJ|; z-CX@iR%iBx$NlDUX|da3mdSZgdvOrDg#aZxB?<>WR+xVuHO)7D4mf=v7q z56@8C>IJz4+cU<5CM`t({=Sv~!YU5qFZQWr{X4i+x3*0;X5`(8LeAVf)VZ_V$7{6I)ba+uBMI9&iO*l*{bH3cq zVkG8^yg`R8mHr)J%qBO-y65(+>k{qTmiPll2fbp`Axlx?;o2aNpLcg$l@;M)3AR~HZXa&INU4zubl zg*Nk^e6%Tggm4yFqh2c(R3@ccv<69qqC{vqp=Ls5VmjC^77SE&+|?Cz_T(=F@U~eq zlp2~g)KZ;#6m!zz^mr2S&d9`KutMntnM=r2VTG;meMndAc42|rE%pL3E>keUR;<^o z3g2*)3G!HYlby}JRr0lzdegkZqe5GOUijg}5B+)dL5U3_={T+#Pj7%$n64@(bFvTn zfH<&&;y&!GsQ|*G%F5^G8+ar5N1Ezmqx+&$tr5=4&O8F~VuXNx0(?CZeBj>+99Y{m zyH6p;n8?AyU@*+r<9=YWS4Kuek+r)}{$w#AziGk=Ace?d=$9?0Z0}%Q+SdWx!M~G} zgjL|)0rD9Cy%nb;j|uI{z9XHg3?Joq)SqJgjOc_Ui5SGDf%sEd9S*+==k-IVPL=pN zYzj0(jqE4)pR|f>SgWJAz5WxuaD=r%gU-#l;$;ns=U7(IVCEo_TBaGP4`E1+RB=WX z44TWFrH&@LbEvw7eVm^O=hQE?dyRBhA0zgzDuhknUsojjyhMHFLB|c>960c!uo2C8 z+$$J4r#w&?-SYmp7#mJIBmbH>%oK*3?LHKv4Af?6i?~+@HV?=aPDi2Rrq(OTmfhD+ zED@KPr{P^r0QO9WvLRS(l@^fNtOq|O4VZA|PcsO6-U8lg`^r7uDF=|D)ng;*F~Zc) zNT~V?ME-i4Z@SS^)b5=ddxUGdSD0Z^C8ec#ZEb*TOFU`Dk90ut!qU43UXN*R(8!*E zD)X3b%n(JmPK~xV))8&x^}{-Gr2Wx%CW*|G1Qbrmpzy?luscBnshD0oJf&NL{%vv| zwJOnd|M~e|q9Z3A+5yewnsu)!jywUTpwXZ-1seEm;-JJIQ-$FhVMRzJk}5g~**1)* zuCS-ON&lTPYT%Udq}#)<@JaU+jfTx;90DrGN?`#Lhcm$&&Y^j1-D>~s8#z}LU2KFi z$NnM}4UHy`J_BR(i1yIMVZibt2)&e579b8U*iaLkr=d50RBZ5CcTj3EyB zXS&YHf#{j<1LA@%Q#|8zPUo3BdB+`pg1Fb2dO~7~xp&(|GF{)GaGR3ou&(aBuXk8J zq6uc3!do`QWOg@9i>b}RJ^Osb^s6jSw&e|;;Zb5@{-ma+5miOMCnY~qgcTiRBd~vv zS9Z2D+_2uiIJUEWxWIj$$-P9=Zt4SXafD#tvF9S8_BJ~5+No3B`ab1&|=mvRl0$cM<5!SKtS;asMr9dikBB}bq%F^LfClufwn3&Ev*z( zQee85@-IM5x8OtoK-V^}DgleD21HX}e5Yw#gn7`hpr;`I+s6*vvCdym%W(|qCU>dkwZLQQ zyr`n^eI;&5)_ zeHyQhLZ;D8{N&?H`-dSCqb4E;d$jr-zd)69EK**+geDChbL=MC0*yh^rDe;-flo}=2-0;6HYU&EwP}u_j>gYo%RFEaIf2cvE7qbN+@ad zxgj&NXQksm|Dx7D_JAKR+zMLm7n}(9BI>LmZWXR~re>QJnr^3`XnyE$JTSlDutvz7 z(pwmKpi#KIGdJrCM57laxf0qeMkicfJL0owi&R8NjRNpgW@ey13Fu#9b(wrX2P9&E7%@Qk*A!%uXZ#O`6`@Rm-PTps zXi1dBVns+kIpS6I=Mc^ z7_b)5O2U4}Z?$A&!`M1ZohuQ}4m{o8xM)&4=<6W{O$W79M)ee7_P+$1nJFw9$qpV< zlk!;Em-Exf39udK8-0EiX|i*e@@Q;}6Dv`B>*;Pe+K>)94P6s&ONuti>HD%ZT&sV@ zgx)&x|HyjB@VvSwUbv0zG-l&8wr#tO+1NH4qp@wrjxil z5s5<9(9)FUE)Va~VhY;rysqc5si8z^fP=+$zbV!6v2auFz#zo&T@A}lF_xpXa+u-5 zerq1u-O`@xDpL2<@!CZDDg@QInH|&B^tX%r=+xNmvPXGJB49`0xU>`9w*Ds4y&up;ku*j(T%8k{Qv$0C}5u5^{NQ=&{T8Unr*V zfi5+n8tBa~7yNk5Z0}36(J?OiQaZA?H27K&tv*-Fxo>~gn6XzdYoxty-fG$g(QeXE zpb*EdOtE*^t-?I*!O_~jQy7GWGwE>Ri`1%udV^v;=#x;QfiP^T4DU{}qribw#!i4y z2HQqCB275`;*vyVHpe}56UwZ7uyA}!^6vycDT7fOx7WxTyF|#ebH*9t_f9n}((R4A zx!7L~3_x78jObe|s#;F}lv!*yl1&+Rg0j_6k>!^D-N$uizKhbH6S6jlsa+M2{-gkX zG*KbhRQ(QHi&LM9CJS|CIPVF+NN%FGx4Z1gzx3_=?V7yEIbuAK!Tvxb*}-{V_Wad*X^|(zf<$wyMR>N!p4&EwUZ<|z zm6B@&4ywMiwA3asASdc(r*7>GfN7A%!W5bBIJCwc^|e(|ssv}+^xVc79$0_-B6E); ziv{&Le4aTEcDQjJdQp*}`uH(*-n%)8hNDRe6nuThy^J4R*OtI}sGjReX}{%y{L>@o z(zFF{bYXsMR`{P1=E1 zvdnPOJ|`NK=BAuFL3Q)r>&j~ws?JUw)yfH`a3^V}wGLgs4i_Bw`6G-&h~bPLG6i1g zc#yDcr3?+e6%}C6tZ`xMOh)?3nk`B?PiENpHh0?F>a?iPi;6#DMh;yq9fl^D*H z$1uL#J-sTM?$F{-k`(K7OBvnR8z6qmC=HEvus4d>@;wowu#wSZ0dWcO60V7Pt}+`V zaHcS&>M>qy49uU_nwCMRZ}kT}Xb4`7oNi1YdSFKAdPY)iYX9bym*d~#mB)_<{X#F2 zvU)q<itR`K|QFx5q*#k%^okA1sHrR*_}K zUE!)U0sr)K{>-k$Sxnp4D1i==b637?@yYLR4Danu4DXH?T(>vjOBGo!3X`3*y+0m4 zk^^s4w$!XPgp-2#PVKL+-9sfz?DJ_avHnGCfs5)nf-7v@GiFz5ONdEA zYyVA<-+6SL`s0_yZ+Ts!(t7AzH06~JDWH}QzL{};o9^@_`Q07{$9c={0#oIw=&!I% zyAvrF&)?Cx70wrS8&8IA0}UO8jlQKDbH@mYq?_LOt?iWqig){c(K<}*wfdxi;&J`e zb(WT!Kk};Mse@XWd|WS8GF)xNc=v?D`V02jcl{9+ynltE8V-owv>Z^lb%NWfI5;oXAjY|&oSwtM#kPLPeG!%LxApJHZ#QD@c0ylX6|>MDo#wV7WD7C^ z?R8*+_U=&Z;3@TaV;$HAE0}lrxkytvaWP}W(1gh%$-WKg%LbKOMViK4j`0V^(HX%9 z+%t*61;r%|x_4=|yCCX$G1(S_ex6`L6Uf)naZTg4z*$qK9h3Bd2le$w29KqDs+v5t z|GR=PaULX48}^HVZy3IrS2R21J*M^tH=6*Rp}U%`eAlbp^P{2F|PtI(*S}#pnBcfRXZCaC~XX z{QHH$yU!ReTr_98Z{&&aW4ng?<UOTf3Xr2ba-iA;@91sm2f5wS z1is3tjf3a?_k6k}%zrh6Wlm;=+9d=#&c#nC8TCT zqJrKOXv@z24~~X|OKnftLlM(%QmOH}{Z|QRTdz^pvM+X93$DxprULSvw@z@oY(tS&JRO@x28dw)R-Zc_Jrnc4{=5YeNX~vClB^bdj_7~fOZ2^pIiSJz zdOXr*s$iK{&3Tv*7a=cqB9ea{>-h`CcI^B5`)H16r|H#a-9wv6%W}!QPmCz6O~{8KqWg{sbVV8oT{sk z_l1*6yIU{)S)5koEMY)kKr!i>G?4u8+I~lWp&w0>Q=S9`9ihO{EoB=3_fW?-MXbj4 zU+S%P>kb6_&z06ErI!h9Dc`GUNP~Jt87m@w?o2Yo_U0IuF9+lR_gKxi!NR`wbFYQt z0ib=8mXQ%)kSQx0DVibrO~`vdGs){2fyFm~ULvxn+vGBztpaoH^Sl+59JZ--zJNE2 z@87wPmq;-ar9tC>Fj)U{1Om2A|&7~Sn^yY1{o zQ-*WhZEA7mDT3=!59v8U=)J!w*?3;UvwIgXK0OOCllAq_kEOocO3WjILbc>mB`F~f zm!g)N5A2pax0D+%-i<3f5)Zz#r`YXnae1b<(0u+sSO8?xr%b85S*O#GI|A~DY00x{ z%8+xu@c2pb`k$YtI$NW4i#Nwsps{j3gL`C*)m4A=Nn}?agy!Y_WiaY<&_WlRSuOa! z$Q-uDgVni1tLtq9DHcLB)zZPv*vdxpWU+7L73cm9i*UhmO&D(sODW|`cn8uhMvwN4J5NBK&3RTZ!VzP%z7 zdDr^)#T;cdO+cLRy9K*%`@bF~-Ea~sK#DULP>quNdd8b#5%x5qqj_D8w_DM0hw(6Q zg`Xq-#FJ}h(8+9@sW+Ya+uwf*LI&4yW&Svc!e7egsd|KZ2a@G|Xe>s-gl@y+i{p%* zhT{o%jNL#g1B=-OIG5X~P~-)=DERpEh1>gr-f-%*?YoQghdAG(*T#c>-W&T1CVIL+ z2+x-{*~4YSx~8l6$s>WZ*uHv2>_NY7xG$Txn0i$LsXlxbsUPD!Ry3SxTJ(Hbx;B0& zwH%n6WmqRPKk{2Ha}S!X64Qi~nxAa>lHa1Ob%A8gRjbTGlYrJDhK zJcVEqd2KL?r7^Guwe%GD5sg*C>rZtHj9xgMCG#u#C*v72bAL*jLZ;_@yI|dKNmXvU2hA#(+v*cu(6?!ME8KP^C zDUYcPRT=c^rt;3(Ipp$dseVS->LBga!BfUrW(TPe={P9ol{H^5X}c}+R{2qzQj6zds-kP?250CX|LaIYEMuih(OnR=Tp1Yw~``mRJbqR?C)A>H^ZHLu=3tt zl%?&m$0^@cu;`&m&YoVFKb@u@`5|`q+qB)Q&-5Hsl0IU~)UgmTiX#YX%GBKOgbeo$k?l1MW+_B!=l z`rZ+Hc+ZdOTDuV_!~ETEy4G>+Ta4TnIfz2ANPB;B32icOF!5pz<%RYhytJ}&*-^~IpPZYgeJ66xvCO!t@|SKU#tsv~ ze?-&P{?H{E=Mw8~p$EX^l?Y$u>cdPbpmy)+(-v54nYmAW4kv=GQ>AgJVG(fXU5LR~EA`*)mR#5+NGSU2W$UvxAnmVezSf?T%H z0QP_V&hdm3X&B{uDa3xp4Yakxo-kj5kla+8j?`AC$*DRI{qNM;i`PX4`iFO3H%%VKIv;0T4u0X(tUe~_Ifj_jLuFW%aDRj7Db@e*jwaBcb@mZNZc|F~p&V|sM8y3=SqONu4k*5mGq&>`dkw_r@m3J`zYwv<8lyyZ;UmN7yYRjW6mP|NVF;XVbPv-WxB! z%V3H`x~0qU!OLlD+E{3&wKH-3n(o1F6>YGC^^#PQTk(b2 zQw7~N(9_5K~ziRDlit4ivliOwM~f7_u^yA zeegp~;Q^)4PPD(4<%ybfe4C7>&vCHBMS}5?61B#@$4H>}GNo*PA1o$Fjcw5FZ>5Df^--EGT8K7_*JwompQ{NHQjYhWo3H#G{t1%L zbB^-Ih2`(r*l$cT7?i>+>XYF_B5ol>0~eG?y~t$RIkV@ad6T5dVxX;<0dMU;#fr=Iz!+7) z7}q60o%eb>s?(v^1&y`Q!mzdB#DdJroz{gFMG5QyK@zX`UEKM}-8t#?Pv~_X_oVUF zsi2M|`>%LO1~gsTbqUxfzExyQ{j{U=aa&=JRoeUxxHpID{k|ps;u;@%=gyDr9=?A6>BOxV z!zH}e1{&#efAXkk6+`$>Mb7V6Gv0P29j}-&vCiNLY zw(kQUrU#LX@!DIx z1GxAKo5F-uogB8w>*x@KiRP_*>hRfQHiUAjud7^7qk>!nWX%?&sW+9mYSH@~5V`f_ zmAbiVYO?XpOODy35>bcHNM-w3f5|Xb!TVS9LnWeOYr$<*U{$N(VZN+=)q+S+mB57y zOhhe_G~St@?>~sQ5t15<=dMDGr5BcpyZs%zND8+K%-g0L)&s<6p=>h|deC6xLV_2A z`X)37lhOU;wl0r)`JUcNi22&xL+L#dmkxS|!2||_5raPWP0u(3g2&=y~~3c7`x@bjzF$-HD=rWmj4LeYgl_eh|sZUWf1m(yJ~YEkikN7bw>8LV77& zKsq+5c6`bv8hSa9c)WOT&Fj}-*u5M37+2v+zBz0(3iDYXf02d4{Qgp2eUK>g(F-rO zbhV7xcUgaTd-1sXgZK4ks8T%gPpgZH%HVHKWa?F#0}bYL!&6fSwL^rv6)aN)1qEc} z#yq22YlZ=wpEmjNUEFLp-CunOdxi*;<)pc562}ejIST#Cw>o~McTn0N4%BX;a_+(pph*MuJXbqw;$C z(eJkh@UpkJnujWcY(%yRufn!GdQzL-5c;|=9yp#n+{JOoTmLvl%ys44ecFJ~!7}fqXG=8hAa@EBr<0r$6E2oN z(j(*WkId(wk!-cZ%0CfBIB*hC)_|2cwN~?#cV&%M#_9 z?Up%#kO%-6s>gndX{?k=j3$L0Bjo0t7Pb^lV#(4Kve)tO#BGkop~!8eFpZ~?pIV6v#f%i&4k+L`{z>n}2A z%H~T{VMJM|f54oasW(y43cCz;y4*VZuGPB%l5OWbcl~}fg%W3ghzD-Up{kESwpel}Og=-(;0pqt-^KTbSsdLrXA{y*Y&Nwj*(!7o=w0ch*#LPse4{5EgeYq5uMIa_vS7Ol026PoHK=La*9C{GmBN zUT==pni;0;rZ>NQP(b49-1A4*PHm09MuKy>7&Ee=D!tym?1Ea^Euj=ZHxSJe;ov^p z$Z5hO{=)dT_iucZ3feu9Sj8-+<8=61yjk$JEx(J2JhLcmQ{_}g3Wid4Tc0cPvGoT~ zO@SrGs~$F4t-rYXGr?f+hs5?#Dk1t}!EPx*Ry@O=A9{-^Gx_2fnSna>Hp>BLSHgL) z&Ms8<&&5IC^0#1$U}($iekwWOHdKSj)%}SH@Qc{$IPn0QK(hA;kAO zrUHle?%v79`|IZ_tLL|KX^IRd&g5*39uWrKo~Z25V0lgA5ZzseODaKq7|JZp@U<+@ z07?)+AL5JEg=uvh%aw3db^2rjN7h~va$n1hhe`!8m?*{-p)%HImoF5xcK89mLc{er z28K5Kt#i{`-;PdNz7@oB{DrA%-DZd!$H=aWV+)o!f9I*fBl>#}>I^459ML^kAKZ#Q z-t>T0rW0-B{H~@pz_ImSJ>#%D7)_3H$?;u*bcHY@{;*I61THCGbb6q55@cZtpyGiV z9zd7?hGw+1@PK!Sc^Z&^J^{jEY;0^%t2dl|lBXzW019VsNRFwdo)cl{j!th&9{Z1@ z^#;`agVSAE$>xz($Pv1&M`zx1Fr9uhA5)Dcy zRT2c-S(w&S%~JDl%L0)f$l5<^p)^2}Qlq%wYpMwJSPPYwR;?Z%T5o2=q7N+2-a;Cnk0I7kCXHCjZihEE2S;^xg`SO=na4b{~?M4aP7lBfe_gdktNxwj>D# zEF^Bn8wqXi^SA!Vw(QanC&RNjEXS3TU)r~S*ktSCct~X~wmW=ZyA#$f{uB_xS?YB} zM(7RizrIu*_<5C!UJs>ezxvuL+Kq_38s%lHnZKB-dUdNE@P)oq$6Q}@)+H1CwYgcN>3#_CXWg;e zPQ3@;W%0}Mp4vro8iRS)|E*!_93Rdyqe}A2FT=e6y|ANG$gdWi(LLW=ipQkZ+(D90 z#6Kn`SGQ!u3~lvWQx?oduCa=9UFbM}W^t(AnE^$S)kKvqsA@2O+_auLcK1L8u9+LK zj=Jrg?z)!@S$8fpUt)r4zEx+arnUmOHi2(X(F--nL|9r{mR45Eg-2=atmoNM zj|R*0Z};D{{^CrQ50)qZ=Lji+aW!b;EGacq*WW|VBi0VPcL@HhR~cB06{fr$^cnA1 z*?=_txF=ID0L%@%j`jO2yB!6tOl-|tUAt(Hg22-wN9C3UgBynvQkr@-A8Jc%T@#O$ zyL#E#xujR36?9Q7#%H-cNp^fDCMBg=+&#RMqS>FBfpX+FL+YSOf(MY4ShCdXQ21Msl!8rQ)n;?$f()cwkLQM^ezX!Px^|?uoH9vvLpH z$gO!_H)CFfZUbT4*;M!Gyy^KdcO20Asz?O(F7YxqdMtW9(I~uO=-<|Qo42)8?lD<50 z;uVqUFHf&NzJbiU6X_f(ii&DjlXw8fX4-@|)?17#rQAM%m@&4>CZ?CUw8^leQx+}W zyk_3+l-~;#Stmj)MH6|tm&A*xxzBDU((p`O;AQbEL4 ziI71@W|vN5gBTek`A7ZJKw+-F{-M0TN)D#HG3cD(X7y#PJh zyAnmGqhj5|PO!$1QAwpM%zWCi@>GxC*3f1=MV>j$jAX&gjrCPtmhn<7JIAz|UCN-rL*TA$9+7 zVZ@`dVW8LklBY^`MMNfY_Se058WC311WLhx)~0t=J+M1pp}3p&R)W zSEqUPy)sowJy#YhslnQqq4!>d5nEPwTKcZvI{lzRorl?_l}}YoElj+1$(%cG^B3oRNnOnhSQg?jjCNC{^o9n>@}Zs z-_kBzoc40E=La9hzQO^<_aw2Upo%g!IS%8#WlVi;DyCLrUmgJ*IQmZdn+ zdiNa0Y#8Fm00eN3e>qswDMCeQ>A#O$AZP}_iUAiooh7^k#E^gPfq-2)`CRz!6=zxz z`A}Y%sIY2HzoD^-3EK;I;Hyaxx*{RkQNO(iLSbD;>8xS33@5Zu3w(2MrX50FvHsGx zlCa)nP%o@V|1L!@^)iD(qyk5X7!H%5FjlXAzlNl`i-8IQNo)gG27RwkAXKxtQt8*7 zj$vTy^VG*x zNw~GqK24@|Y>@PdQ!JVCe6F1qLB91Cqucp%nnqwPl%}xmGUtBjpTYe2Nq)H{L1F9d zH&LHQT?XM?svV+*4XlpauN4N6!jkvP@3ZJ@nMQeLv9o1Z=s*u$r_c^$ zgmZK5E!)0LXc3(yYF-u5W?~}xS%hQS84L1-urKAu!_^_(Tqq%5Qou3|zt_DpyG!bL zcCe}8;ng7Qx_Vxdd15=2pAE=WL}Cf=gU$ul8>a8)W*@qONHKU-A!tf)sd?Lbhnxpo zc#PlVy{t->%I2|)Z|(QHHCb8dB>vFK62WjwoV>bwQS56458RP&ytzWS43kSa)Jan% zWvI+5a(G@G>ahOJK3pBo-s$;<`O>MRX6@90>25HygQFu8LjKAhZ~#!OgA#N`+ZYFC z00#h_fis&Jji+$=;z316M-DJB1d3H4)P{DkgJW6c00_NdEjGX24c6LO+)fS_b7%BP zIYkh9=_P71sVKiSUXW1os-mE(|L3QgGO%|?L7LNX#vC$*Z}E%<-jy(fQrI;eV7HON zUI5asLF9i>KP&KtyV>o1GvM2TX5YQo4~+P;U#P*M5lJXHwj63xsFzYZ@HZ%=KHg^y zrQ&RwM<+17K*me}d{}wa7wclr*7PK7ZD%o4;P=}HqaQd1V-XJ?)uP*!WI zc{T4ur6Zc3BV%dy-jZWReRtq%kB7S@^b@iQjeS~|q zsUE^!>ZNDTL1{f77&X5|3la++(En2QTa^ObIMvnd(vxF?M6A{og8{mtE4|#C1gy;4(%6Jxconr@|&Dm*{{B}X9}P5K2{ zP)T#B-_Ub5Tp;BwT6R6D;d6hI#8tSu-E8#2NU7Ls$uq}BLVT-Ef3IIRi3fxk1^ltTpv+@LZtw6EJSzVq24uAlFF zW?0iQSGL^E&IbHR(#_9@4+6~B99n^993>?sbj*+lX{>%TM`AwGE z&9b&#Sn9DtOA%Do^OwcN?iW0`qXdRuLQ+GyB_%2PjFNJ4Q5Y~r$y=$DI&do{<`e`7B1=k*XXCcb(a3EJyRx%g<}}}pP31B&kb0w!%WC_?08NkU zt}~fOP=h_%))8t~-TQ)Jh!HnqJh{V3w7Yk0T9~Q(;h}mRJ zB&9&@(gr>EJ*0dp;_jsn<}j#Cb`s`|kyiqcY5{TUu2V)vQDKwbay1sO@vQ(v!I| zxcqjZR7@D=JQSZQv=Hz<*)Xs01<#rhZ#6u<3-t=jIb_-?`e2pdh!-USb=6Razt;M;by9G5t=%PJLaSqAj8I1ua~yGllGLUbD`U(!>oazY)<#`-JgT z>*1QMWD+E~vE{M0bA$qUWK&Nji|Fpq*D)pS6-ig55)u-wpE{N{a~!bAi3 zy`^2Qbfp0mIAhbfRmdJE?mR&%uwD#c73)-&S4sgZJN`8M|Z+w*HB;QDK?O* zEfRExfnyePXZU_J@KeMd`Gj4ERTZ$Eis*|GDVh^q>@6XN4(Ly`2w~TbN zW)|QEPZrlCq=gPKcN7YiYn}#7pvcD}_z01K#{vk`|M^q@`R^KBQYK9rD>%oB&}UXh zWTvNp^NC&zCNP{dF5H`b`xC1i$)hza=5(fj))+3ut0p{O{lL`G-VmH3&X>JW3SoS0 zHIe`TQzkX{AI{nBJyy+JpoUF`!gy6K4W9g-#a<~C{tu;BkaI`{3O}G3uMe#X$Gl0D!8=$-aQveg2AZS zLh|-s_Y^QibFmP@fHnw1;OgqC@s?1wLWy~iIq-W|ZoDnpRRvnpUobzTa{pv1iD+pY z3uGH;Q_H16c)KxB@!oy+j}4-NJx`$6S*a3I^G~A^MJM%cAuhjS=YK{;I-@*40hSbe z%Qm1y0chjLS@-ZSN?{oo84nNN&z1cH%9U1^ljD~uWQ~&%Km(^9H1qKAcpP~ofa4=I zXA}L3T1G?rqkbO?VDd4hAMGt99sNWsYlco96Z@(D}ruscbjI(%`h<# z8lLt^xUNCFdXB?B0X*{Mn85QFG64=NZWKEeow{1$ z_iw>NU-3J|dgeLT-%Z;;Fml+TgtB2^YsEy=P=&~Sn(ZK?kFr)d*4#^|7#ng7s)gr* zo?BoDO2c~jJC&yvhPIPw9@#R&b~b)wXNb;r9SwKF6jD9@SF`I_BbMa>XBv)(0$_fc zQe=MD%Sb0iomiV1+0Bf%E~yDCy-LuIQ)?I{E-OQd5fwh*C!vOMeci6uW&ZWfxzYG1 zr0ZL>43J?&cZg#{>pkwK5b&83fO`J?d}DV`m3?ihEXSh06U>&!8IdzR|b?`+o zH<%1Zz|O>aDp9)V>^U&;fkwn#2c)v>nPd+k&fYrDtu(7Szw5~)2UH!33u_bfeS$>5 zz`&4WFO#KiB__drN0P}5)i3NGTsA=RAordBF_>{I6(A7FC`!Q-6gwF~v{=mPdp z^HJ}G_n_ZRx9-i(Bq+RK=O0R_ooNU! zcxu0Xo8gXJEF9d$O7^F3*8G={+Wqe}r8A=GXsrH6YUNnq5UuNz6ta6Un*z&CxuF_y zGSpcD6653ByK!l4S=mUozcFXn(uvrTbB8!V8#Vo+WfG{@?mZH_YDui8xF^kmYEu3z z(1I?%l-gk01#xjmRM}n?Ye!#-(Ab(J9caELrknhGt63m@pXJUkyCVVD;nKS@~BJ{DzY-f|&Bs$v_IIqEa#}rpka_g47fs zs{+ycbY^vgZ$%p&j-Q8ga1LRb|Ir`Oe zo|Udu9-wXKht(N0lb=)jY01C_h10!LqsVtp(<@?OZIj&Zp+dq84NTb+u+PI5=NiBj z>q}L&G#-G-xpN*H)-;J1VaSm2z;9QS+td#UuPE3pO#?a99$I!zP>BtreJSUzW2Z$8 zqg-(zfr-2_E;voUE{O9;Qu00qBbiQxUXi>N2xDd79%8ch)1s_w<}T>Ud zJ5Tul=asY~p^oSvlFa#K(~ zgA-B>OC}<7xX!c70vsk=TH|UVpgd7lQHehtT2Uj9ai|EPW~(jr`S3%bT}z!V4CarJ8q|phEE$#RRGZ#Xoy)`&#={}GK=YYUwb33{_Kn;JHMW+X z#}HG#mF7D^xy~E*3J2}cDe*lzOLa$ZOxs7|8a;u8*4)xg`WI$XA;iMsh+3h&b*;si}s3Kx&k|Ci_aw-7eyx}x)~06u`2QiM**6F8P`>^Si$un`Qgp@3^* z_LKzBu$wW%0BXe|b3on)Um?9H%mCYsZTlXcQq;T#Oq5`o6d<~X*dQ|Ivcu^jI&`2g z1!1?8>YmCJLGkgx3ZdWz#|ieWq~3Iy_g1r^D6y=xQc42jbVQjbt&=Oo@PPJ^MLMKa zxr0w@Nk5+a_9Q(^;}|csh{z)D{*pfTx=vkO32j-82_`d zNp*fZ%Cay+v`fetoGdpQ#+VRQmVA+-7Kbu^LIcMCPtm0Gtv)S3n%t^)dZ1~j-s*EQ zs8-UOrpSVaV5U0b@FVKd6W-g0bGW&7eUCne7{5d#?6}PKDk1C#opXoyzi)I(&`Dac zRd28-R#V9B5f*cnJP%)}i#`NYWV-Hf7*%}ixc}_9ME%gxia>h2d0~F-o_m~A0v|SG z5>oCV_?F`to6f6esm3Mue-SmcZ1PQ1uO`ORTdnu;eM(*D+?TgAd>0_`RsYJ_&|Z&u zu5;W8ZE9-E^X6l{WO(6dyc$5Pv>yi;uGTULFvN%;a}}(ZkgWw{K)BLXdJA`a1$GfD zuG~G&ts5^u(pA*cQvlKdBLWZ`K%wOx-MkUFfC}UB>-T|kVZY!8#017(U&$XK-YS}2 zp+jXcoZ2Gc2diPH;(oDq{2!VCGv6!i&C;kol92Z+`>776hI^ij&A4;R9Lo3z0S^Mp z)}pG(W{$XB@RM+p#9KubDDfuxscOuz7Nw zwsWjUy6b_MTMX9uJ2~=C>!wc)i!p1jDF+8c6OAwXGd6N->{V5=U0yWtt~)ZKNdhCz zt{pOl!v+~1^MY+|`Kqti;N>j!mko#6F~a-PbB_=6N$Wqy4CX)jGY*J$bB6>BqL3y` zaVfvQjH+orU*d{q`p_LD&odvywm&WDA#w}2Q-5W~7;pQv?mdIpf%!^+wNZnWN~^as z8A88zJf6D#^itJsOiVe|O&0-t42Cl)VpR;tK1N(J6mTFn0aJ{+=A5xTi)rY2e$Z09 z5Sdbp45l~undQ9;vg_I#)>6-;e%;UsVWk0s(A*cW#Q6+y^Bs1bZwLcoc46B|CIe_A zRsZ8UU)peCtx~n#SZY^c~|7 zqdVvPp!#X&g_-6g58LnfMD(Fc1e+=6w_thNnWkZ&k)ZxXmI{o#3x5+_j2MFyP-AFD ze`l2@{yp=jwy0n5fUaQu*BPW%hHY7Oapq66`1dCWFr5CKTAhYsI@wdKO?MNn))2kE z(!FNK6Pb(JpF2{Vv?>#q3|_al2)fU1uz2}@CWr>FG~S}VhU@f??%%xL0$8<)>L9yV z9S^Dx*E4|1lq$ODH99_&br4vX9%{LAyK-YT$z=&g?oao+d8qcyyj5(-`;Q7QdXorL4ega%{PIik(;MZ~wVP%vF?DS=cz zcxBtXohZ;%2uJ^073F%*8A!wZBs{_`_ob27VMX!P59d7B>x9kx5g7YOe0JiG4-JBr1S7pMGwy`-{I9DjGZ zEQOe~x?}qCahmCxq3yiURJ~=c;@Z@FTX)14#A@MY7U0L~jftX&Iu?kC3QwhmfeNV< zi^MOfgrXRZf&uHVD6$%S3nspo% z3wp?bAX@h(0C~O{<$Vba$Q^$l*Jyd2Gir0fA57jHG~uw`t#{oVO;xLT6L57rn2T6x z;gf~y=I}cz))`-#Oc$?B({W2!VBqWRo1!zmi)PJ1FRL=m&#_)_Wh9iS^!KM>#^^ml zp}**k+A7c+4h2BF`3xy`ISf5;nwB69$piV<%jb4@>-$S7E%wRE1{l((_Cv1a$|Ev@ zLz?fRZ~=TG?-O>`XW?18nc%`es=IhFT4VpG#Q_Dh7~FW}m6TA^iuPz~>UzkUzDoW` zRYY6pWM9HqVlsnWqBAp%i*Wu-UB01HLQ~bJnA?f)<1YbyUGgnKkkGYg|M*KA)qInr zfgzd8!Rj zPBn&ZU~bRb5LG2wAdUtcgL>)z*5N$f9)>prmw__A%^otSg?AKb39pj$`E{<>~u$@j>g#2KHVFWK)3 zu*!GbhIpCOOtb+KV!3*6aox5y+o%4xKX=i-RE9I1MQAw8+6k1F=(|Y@El@aIV?o=y z*rRA#xW9J|J)Y?V;&^X_8Fyy<8uCgf%Ho0{@K@6Imo^$2&{}ifTge@zI-YbH)5OS_WrBC_xz( zv1(8s=jNk6?Z`AbSFmu7*|pOal^<$~KcsqsE_Yh8Zf^o)cad%TmGhati~Fa-ak7~4 zuCjVF(r&H?@XbWq;`W_0pEW zz^ok8>B@?a`=pPqc!_GJGv$0@vTeh5D2qE&>c) zt+9Vcj8$k=<$x#qm_yRO^yIb*tdHoT8?IM{=&e%B|JO*6XYs=El8?p14`ZpMFOA=Te-LCDNl>{g!& z56(FHAttnG>xTF>7SEuN=BlJC(SguZOypGKOR@`9CcCw5$~#au+ZODNx-m$_o$?Lr zNz((|P2_XkmppKqp%eaiQpNS-*h4stG3;L+eePwjQ6PL_4$MplqPg$D{&V$U0Rab3 zGyNC88P-l?cI@n%X|^*2*&A4SA*spj!W$diCVQqa^!Zh@CrJu;CACsOrKL2WShsqH zV~?pSKJI7p;5I)T#s%R{4K+J5IKxlIr_PQpPr20G$nd_MKRZn4QuUKtiG6fGCbGhD z9yQBQ^NhEB<9U~J`HQ{sku0FVI>QN<(@PBmmEHH#U7U+`CZ-XAdd{R~v7V`6xzGOa zl=xx^{)aDqH;0Zf>*}BBb@)p0k2zV^&#MRBU+~6O?cOP=l8?2U7Oj1cq_$P!jyCM_ z#!UJ^EUB%Y{q6WZ;H>r+Gqx+@645m$_49Tn>!WQ$O`EoFbDC^-)ar8~QxN!zO66mvTq~h>n@*O_UIH>;GWxQ5Gr+ig@XH4Ko2SNy zIIJ*B@95)PXATcxS1MzV;*b(m_HYFi(M-DXFB`|cDIfTJCH_n5Nfv<^g3^YIzjzwY zO6V);zkhjGcsxNfTfC@>M}b>`V{ypnH2}e{7Ee(^N82p;{@jBh)Tp z+$jimP`(`EL3?Gz9~M~dJ7hCgrgX+}G#I`cee^hTftTWsR4avC!uDqLdNEjNwMpa~ z;2h;XT&f1DCF51CeJ2{hRZYI#>EM|p5JM@i9PINZ*d(JQd+#}-oz+7GMoJIOTXVO( zk}EZ<6akn3N?t+R|ULofCK9KRVUGth=Dd9VZ+h`8lU53bJ zvg7RdR$s}#Lp#Zsf6~e5J1c3q*xk&fUnFNHMVNUm~{w{;Kb6MBZWeYY#&DnPp_Ce{sKzZ@zSJMUa>{uZoRwArQtAt#9dMn|g zai5f5VZ$Z!jo%1)#e0v=s#fkFZ zK?`hx5c{bNJmdK9KQw_-P8L9VFRj#a5;F*Y`L^J8NQC+A@W}%Xrk*9rfeRdv)2Qaeq z-m$ZCu-NszP#N8DUk0tTxo(@lzwsG}VQ1DiZ63sfD}K=UJ_RL}Pk$l>hwU4cOgYj( zr`)Ff8vk){=VT3OnU4T2>D-cks72Uw=GKkRn^EWeXtf0R!*%;YxAEs?b!#8?F8s1l zP%p=YLwd0FrC$aD%BQ7~Y0e{S{_bn&t+#pIZr;n2>5Ze_mi5~gyN>6dGqTTbKi*8F z*?OO(3A_G+wz}FmbqeO2!*O1Li;b~2D9tBt7b66I=kyoeSldn>960eMGEMJaKUhtHElAQVtVU0cRFg4MY0)k&OZE7J~tSO!U$znC`QiAxqg{gSXiVX!wDu zJUQ9`t6VS&&=~%jA`BByS-A@fstvMwn}+@@Yp?p~)7s*%;FRNwWQ!A#g9_u6<1gw} z*#NvgWbBoJv3z6q+(K;4LOyuI!T%aD+mgN&x7+P7KgK0@idUn8GPENeAe6>AVIgA| zx83H?3I#N-NAin1VO)I3=pCXAGb4%RNNo;I*Dn=msF4RJZCJ@IVN5OrzZb`bCKIdv zb`x93(FsFp2)%4ZH3-%}<9jW1+NaTEUL9y95WSKn=|j$3n;`j`aKRXB<91rmqOuwI z7KYrFDY0F(cy_Nl<5sugfPLM?@{6C5L*SCnse7iyb{rkvX>)aTc;E?F&$}j}QKk0K z>weN=^?JeD`#09d)9)#3&hsgRFZ|iW*Nx^b$y@}|`aK9Z+JJUm!Di}A)rFdN7QxFD z5wVxiQB({Y&c=1@hwo=|<4=T@kAYAZDQWrYv(~JcSX-lIBpjE$lc&52NErOcuwxfi z3~qh`w7p?le7CP+WL7Y@Sax+0-#D6be_DPaK+97+PM{lqxl^nM&Gp9S=Y0> zr+l(XKZ<73DaPzS&YFHT?wUlxA#*t}y`Ii(k2wYCaVJV2d92W_OIt3$MDr;)O3qJG zvXl((rL0;;WE)?RYyBS zG-4YUU4mlHNiha}W=QRI11+X7L3(i4XxfH3>2(HBzU}87q~g$mtqO(HVwr{ri?th> z7*ESdrAm_bWL-UwCX%e`h~ta1xh&2z;F5OEOs}vLdq~2AKHhpH*a|_WaaH0XXTk#7*poB4n}z2 zW??(1>T%O&5$>L!2<-L}{gu1wJA#P3#7gCNVnpVDbY(z%RKfNOnwz8BJQxtren|QS z-Eld25#@tv;TdUu8ATVV>bN1b_=T<5n9KTag~ZKxjNQ$C<=p+HL*Cv+Cgj^QhXB>n zGYglaBuG^Ys@D=TzQf(Ktl#b9me1lfY@QPaStP{IP@JIS*&p*#C*S^t2;B9{xh=R} zHAH_^idgm&cE913$0ZP?_LT;k|NhE-ZH+UiG(_q2QIYa?nAf+Dojd2W(G2=j^SyPY zLYC(TQcBlTVDc`pIL4q~?mJ!`WSo&?fbe-LJF&6h@+1z+(hy4&;mNhioMCra3KVnf zL8)tYT-h<*EYTzP?LXX5GOUxH=gzf>|%gqjix(e~P6Cgxz;4M?28_L?4j_nq-9?=iX)7sXl z540##AZ=DZhazQ-af+io96hl;s11zbY#3E=U-nKF+hhz*geI?XS0LB$J%9H0`B3$} zpN^Tni92q>jy@skd*$NbyVISmg1z4n_%&DBsBqs?B&~>&OyEURw(UgmsP9D;l13zF z@OS5F#!p4|Wws-ldrfLY^2ETbaTeBN$jN;{9e915m~!M#f`!H$0KH8LpL^0Tyx4S1)$A@9&HUE>OVueq;gNotN zsl=zc8}CJ=x3f#)uizAAs$Fz~E67NGzBR<6S&Uh{1aWi%K374An}F6}R+ivjb><3Z zl#zh|e2*=JR7TB-yEb+MBNSYw*rKv%S6zbjOANS*&Nz27H86==Wo~EOF$LUuBBXc_xu?{W;t@yqdH~yZ)HE?pgJC%H8HU^Gs!Y#<^Uw zyDR!afwJDO?dKOQpwpAEW@on>4T}5yo%Zc+16qUc5QCxPw`667+4@vm6B2!!uk@{` ze1|*AiOHhHwFe+dz?nY9Qz(?3p=D&vO^K&ZY#88%fyD z^*ZCA{hpoP+_omUf?WNcu6NO2B1-T8shE? zbhA$rayW6k(@k-oq3VhG5KvBoZl;ch$;QuDGl64-FL(-{wxACE+I(SCrPHR8>$f`3 zP9bB2o|z{fH(1QjOY@4co$gqlEZqK;$xP>iO`7&6PFs(Mva#(iFyi|5WUE-b!}@xj z!TNpaP~ORL0x6m3ZuCg|Uxq@|bww|F&7K!$7(2PhJpC6E7JoS#pql(H(LXud;T>Oe z+nQD(9luejGkHi(jJ;Q?;yn6|r%31NFWiyD5^G-dxx2=i_O!j5LMaOV!Y$vxT2(JjKU|?t-RBVE=&K2p-W03??L8Kh6 zja0+z#-nd{wL!b%ol)RmGF|#?n5E;m^z(-n)4>%DXX5qtvvITb)$v1B-oPF5(dm@$ z%(59WFFCeD&d5M&Di)SsyGhxt=}dOS&hnhsK>og$1pB8y5B7evGfrxs>?i zhW?I>i3DN0*IWo*Ay3bwqe+!~LAx9qk6>k8I(f;KR;s@y8?2%C>h|<<8nDWCe9!v|K3DX1ppJpt zfYX)zFYD*3!XZY4^+W>-9ARDB#t#`B4nxPeYHLD3ih`9?M)-5Fiew#mTdUn4U4aX6 zvy~ev?l6J==gvACOuXqknfgson#6sRSKnAM5+Ahw@$b4{QNANSTi!Y(!8^N|jK2b& zuE4k4RZE>&-Wb|GuWRUd?tr|_2xIf}j-b}NHqvg#2XPFeV@i7IBX-KKGg8Iw6ETly zL9@JJ?PIR$`Peu`$7@GT_IBj?CQbji^b-5#RX->9-0ehkdoQ)_BeE3!dOxkK%c6al z|8O;JXS%e>ck1%wQcL5sduIKeI5-dm;Q#MG|AINia+hkqdY^?|I}Il|J@6KaI>I=R z+Bw}HwW0T2*xF{QZ(YiS+gq{N^|izwe4LmF7Yy<##K-B5?eyJgey|>{^1A#=fOzn% zL8ia%>k7X`r$moxae3lJi)nN|`vwe;0qT`pPRF~<&I@M`FM@pnf|rpcvgxcx{;;*T z8hF=qTH{;Bu>wpiThOt5wI`|cT+QuA2L=32L+6}G0Jn7Oj>BXC0#HC=O6Hc9q-YVl zoJbTD6u{;IEazKo4mgoiRO$|vb<^^4-^_hZXU`8hCF3Cd}% z?D@zrzJ3|YN67#Zj!UiS;1l%$FGQggrNyFGptF2Iuzp}ntqSZ)!(W{(&~(chjn~~D zJF5>?FWZ;+7pgsJKA7^k$TJh2UY)f_yjy2FBWu_w9x>s%WBEx=sBnAy6V9($5nINS zL(2igTz@z-qW0eW=34~QcUGGTc&G?F<@)YJO?>U~+rGHF<|kPoF@OXbr}OCCtZb-V zNFpC5rTkU&fnx)uTBHovs^ZBJbDYBCeVA|JDOoI~<8gAxYva#YmRmJjuqS%c=YKR9 zqldA!BC%0l=hJp3wesj|NL1_hm>GHi37^vt`tH{lF2 z^}gl0(G|;ylwQaeB%{7D6}cQONIjnXRl9bGDHrNtq@E!`5{V{9M7)uny)o}v&LE=L zZNX|ph}a1{Q+@t8Tu0fV_E65572$#7K7ZWn5`HuH{Q*K=+<)A+EaR?X`gKbp2LeCB z#IGTz+W+LxGxZqy<#6%CFksQ?`ln|%KKJhxOIo{Ny zS02^H(aKb7!-19ipV2ON3U@U9ms#}+8Z=hA_NYxgd&k0}92aia^svj)J>MhGURL%# z<_tmj)Pi-_<9)u(^ll%X`_HXLpXkQAoW~;31uA=G)@t7B-`;?puHa(TL@?Pn z65pTO{CwK$vM$N~g6V1daTR^x3OEmt5m~q1#^8x36dK3Y`1keAER3p@Plsc09W)yo zhj27j!dfzLrgQ!-dF^l$o`; z!tkKhx??0h>ei-5%{kc%i6KeO7Yf64yN~yLFamAv8rgV#r75-*d>Uz*~*iQ zc7m=jJkR=0{AiyniNt>ubA1P_fhmBCcZ8h-qh$Vi0+hc-Dxd%$0Fzq4(#Qx>en1nHFhMBv^ETXMHn1;jJI- zjPxSwkJd;@dvS87jW!OiGp8wc4s0n(?a8z&N_(#}NwTbFr8zc4$fDKo!2K*&SDAW2 z+WgO)Lm0;$&9Hhjifgz*hk555xxDJ4;^vF_y2iu$;feOUi)>mpGA!XP!E}$E6wXsl zq}~QIN5`YTyndT<_OlbRRqQZzUP&h(9fZdFY3b^h{4s9dz_vzDh)y~VF`%H0t9_3g zKG19TqZh51=t3$zP8<&VeCNP9-eYgL#+WKa$zGHQr21Q}@%7n;gl}G)6mB!^kb;8N zowXQSe;8_h{b3A6jP;R4rH?-uAp$Wo<6AnN)4xuoX8#`5xA*;kS3W~IW(TgxrfcvJ z-At+E`k0#Ln_t$mi?HzS8kj^wfVX;^=t3WgFv%|R{8Dq!VBtQ{>A#0Rr8paS-jbp zFeOp-*FUXB2JRxISL$?UN)fFpDsjUFP)w-6&%Hyti#vX>2^8)&^xaLeJT?$Rk}A#&t1z4dI-Mw& zI$wZSHiWDadc<@>JXMuBJUlY)RU3v@bzsi!pRZP_^1ZNrc0Rg(n1XwPsuh_stufk1 zD|uh>m4&_H0?~b=pQG>ee|Qp?M%FuW5jYLku;4e-LG3c z8{T}n8-3`v-~T|rQD%QI2_+JUf~)Bf6Zv?EjK(=7{zf50@F?NhRSet|`hY%lKqR!G z()FJq{as|`kk*?cepq0jV%wO)uz-H^kUQ`?V>u)iM(e~4ZJ1=DRnjWQ`GOwa&s z1_caIZWmI+kVsM{l~-o+#Fe?URj*CLcOlV9o!^3iB z)|J%;iv!Pv3RQ)=#6y(*2T5jXdfKAu!pf)x`3RF8d#@uG1agN)FcN2!A#17w;j2QT z0yNwqi!^=U5;ZG-aW;GSmb}3_rbmD*BN!zNFmrEAAUJVe3|F0Vb%sQA@?PX51Qf~Q*M=} zu6p?3uVR-s;C45dt5*^<>Ms!dw$eQFpbwVF#3S9O7 zuZ6qIzcikT24F|4=Oc}Ds%jJ#u#d4!+N2lp=x~^r3m(Wkw7+U85$FTu=dzuziGSB-TCGFXnY`7`kS-D4Fol_0JO601SJwOE^E4d`(J;+&3Qn6LF`{wV4t`h zJL4#2Ia&StLvRECxtBq$l>wY!Ton_2$b3ISJ6vFT7iC&v^ zMw=@Rr?quf+#Y9-CHuEku9YxC78)HVX)8Syk`99WZ8WKW>p``e#3Fw$Emk7s+B&@i zGkkcV#Em%)l)w-hIR6V6=x_BYRPhDqASMesg3oVT%}?`;Tvu+JnJaZkwi}Q|QwK0- zFbH18eYszWk|#+7gX2~F9rp76%REA~QT?zBVrY*k3aiIsJ~ic+(b?q%myMghs-AnrI6w;(^W)>dW5r37$oChfq#bdB^JiO|mH;h@5 z+y`HmgWGpr=Cc(UBC_ym@!`h^OvIsm2?u=ex-Y<;XF7A|$k5I{5I{J-L(xx>+HiBV z8C)p|>ZOn8jz^##@6_((*gZ3>HPg>brxO=f4;QvXnO182{32jE^RfXY#jcHVe z`kgV#qig?FQpF&(07Op7fd!(81yo`K-dhL9aKJ@Z@=q3#@P`%(33pne zWh$M^ios&hy3c+F#wb#;A5=`OQ#R)0ALn|K^P)7#NJD< zbyA!{2bIG^XyGd^=1G1bNem=#D2JIZ;0EXJKxMfmfHtyG5@nLCrk57+A;26F3TI>C zg%RV2QN!A6FkXucYYi4su}Ut!A;Vy7%@{N@AKte}bd6-EjjUg2Ug2o^VVqc5`4w)e z5^2~rMe+pz!!a&oeJdR7xLqII{wXgcUohF&irmj+nj4aCrqm_zCDV=#h{ie1L z8>3p8(n5PqQ&*l}uNoPg_#yP&nFoi23>s}cs@{#{e|uL^HKjv>XsENrO5@Kl-ta*V zye7FWNpS!9J+t)_jQC1)BJBRZMN2sRnFS%d+3M5-oissC7`F*G%!B~U*b#_OJ0u%n=m88mfobgj?i8`bW*un< z_V(d}+xS)zJQmaSUFvvAaQ?@p*}?5xdwj`t+l&SguLy_MF@s*bpJgKRAS2fWYqj7- z7!tIC_fJ2wF(UU1=52|EK;o8%iIwJDKOx|%$;FM>Vn&%3#9@$krB;fbz}HS87L|BG zkr5{xtKE~qmh<~9C{Ayc4%ICx^<{X!%Xx?$eOv+lPZFoVboR$E?|%cZ`2YO(`2Pom z11TX&j}Y-RTmWzqn|#Y~LI7rQTVzs4{==Vura9|Zm`p^1Enk{0A3aTu-R=35{8P@| z@Yn+g#K*iJr{w|6b!WqxEd6kpqOMh50M zv{k7=rQ#d?_p1MIgGiD?$LC_fGz|is{NhCmpi>)Jc=&>7nHhS($r&p$NLqHd?g2X# zIS!9?MyaW8u;8d0P=79)cwv?aCd4*V|3_Jt&7Ua{B1hbLgro#JUsI|68^yS&#UE9c z`E~IsdUI8{a;(h=4Ap`R*zO6aH%pL3qG2hjhG-C>vC^4^8&3M*K%~&xMGS11y&@Vk zk}Ze=G#V6XPYxdnlS4sZ=p2x?ljsJAq5MxNQLJy6VR{@o@q6VdvM~P~s*+M6VHHm{t zP3otv8i*D9=_3SodrAKH>%4F>`FZ_cFgmK>Ii!Q1YPid{IN#HaO!OO zCGTVa=c}7t(rZAY3f!S~^|DE&rQ+MZ_lUpoem8VFw5jN-%`{MDOBxz7&u(i`|MypQ z6Wscq`=u*P2xt*NUNQsd2i7FmHM0xCzq+GuOz5yqYGH4!MX)SY!N9^@*A-J=fX z_@4Yp$k_&~?NU@Rkg&pI8B;ZW#&Fdj>6V5eVzxJWa*?zO3`QG7L79gPLRM;4ZX8%|s){up^O<2g{SD%jy5~bQ3Qa4icL=iuyGs0n3$@ zJ2_Pfx}f-kOGj87>7UqfK;nOI@EJ$(BF#47db}J2JA5zyh zS~kbsrfsyJYj0Hb^x89r%JWxShjSF%m4%4+0|g%J=ek6PZ2RTce=p`V+!%Fryxat{=oh3C@ZhG$s&k}na=f^CJYe*QB%ucdBnlR z!{ZsHy@HnCN8i?bB0ML};Gv?O+z_*CcuK-~xT3mrD#qhC7~rWgmnQl;Rw8#lsQtVE zseU_L`iDfT6u6--GODT5du|0V{*#v!E7Jg@V_R*^w0@idM-4HG{myHoq_(ZH0~U$S zOpFqKAvFwf4I-sN@t>ou6aOksS&(}NGe`!HNu04wBD-tk~5)KY--i2U@$=t?;fzWZv%t?mE^xuqHYb((0-@1!CdEwvrfYMl)movaFfR~jci_iVdFPSw#%1Q zJwHBy*PoB=hs6V|ALns8?@fJ;%*%_k?E)}?NesuM`o3xCv`6;}z<+z?TtXTYB=#=^ z4j14q!H7KoAly=m#vfBy!6<93F<8_Sns0Pt%h&s*ulL)jm{HTb+-(F~n2>}wmM9O&9j&jM+pc%forY|gs^L9?cRHZ6g z!Mn@Ac;qkD7MU6fMgAWaV3{RcnJ=;wZov|zuK=YH7iHiptK5JcHO^OeA0fD&!2CpF zRJ+Ztm{iG5sa9wA2W$25RQZATpWOQ)3^{_PM?Y(sJrITX@;u40a=LQ8?;E~cwn~oO~oolZt?JmR^>VS>o z?$ph<|31jKzegZ*kCN%iJtfck-4|@9FD!2wfwbPj`&q*K9h7M2V{wkNGs@R5HxhR= z2Jh;DGn+lh;s>JlrCZ(2WNA~obuzT1%I8DU)@xeYv%NMxD$aC*na-rT084@}V{dcpIl?3>_rp$Y4+~vdi+**!m_>$n?h^%i;Sr-( zzpLZfGVCS?uXGH5JCH?%uJpPbju<1%dTsaXft{O^Hjerliako*RE1qXh9_L?*&0#k zeHpuh#;n%cbsoE;>8}sG3LsRIztaf!_D--GJtc7GVbxZd&hK~jr@Jk+GKafyR#**GAs;61=Vb8`*b12og zL71^OYxg5-+}2<8R-U%|po2;-BO}l`@A`E5i7(L)g5C#S)}zCH&Uh6WMK>sBi~ICU zhhXB+UB=HS?(krz#StZ2=@X}kukFXHc1j+jY3-(_khe#3j&lcXH=FxB9j1CTruSd@ zx8nE1W$H|}L2}koM|eMn$=BMP`M>F(ImCIrBmiwUCKfK;(M6nq;x3{TJBr$Jk zdeld_SFy7#y*ID#INLv)#`Y_niiVxf2Yi)RX|7#Ce^O0_mYKBW?eC*PgR=^P_P>k! zkl3CYQkVqdKQJH23IO|$QVWe#v2Pi}0JU#w3x7Z5d(YA?`u+ACO4EVaCgA7UpA7hK zt{W$KOPeB6-lp*MABO4TutH^!1(>LgT9SAatS{pGELjzjb!8e62I+>Z1!#=r16J%R zDI2m`-z8;!I`fvsFk+f|8GPfngC8)Vg@r4xZ%cHolya}6mX)yh2Ie0A^LMi1u4sdE zNITM3W+Mm8I>!2I9z9YaAgYoEqAD*^lHNM9smT8wn4IOaLR}GVxgOYNFPhVIO7qY; z;~7O88c);E&CZXdl{Q}#_`4zsRciMq=Lp77Rm>cJr}>R@lxle23`beZd*SeU;peU# z(b+HskQ4Q_#Wn-wle7hMu!%SiGndHUWK_zHn`&@omCa)hp<#(VaoTAQ|LHxgJ(am$GH1+j@4WH-=8y#scIK5|UEi6Mg-XHbck~3p zjqCO6Kasf4kzxuCHUEw7i zkpPYDH3Sm^E&nmi?}Qt-^?|%?nLbkn9C<`dz7%+ggrn2=!t!}&B%?F92#?h!#!H48 zGtAM8eluOPnOt4pMqj5?WH}!(aC3d*FlWqEP<`TlBsd;Vmv0KwG9>af{e{wM$ ztJXekL(0O!NG>ULjgS!Z1=V?9#m1t4UqWR!G`p1D#bb}U{YcYL(pKIM!6V`x*J=dw~%LM?!r;e$*w(2Pgv4zyg8ITJp7#l^dUe zok&er-%0E+Ba19h6C|NLcl;vY3)a#CG+@5rj|nMZ0F?(dH8sFXT}TKN&}A;NHqgP> z*b|F#GYHspl?+twf5^Bs1=ME3Qb=XmqTSl&#c=H|>YNxHtRT8WiwFy%&UEO7$!#Zi z7tRWz21@2b^{tC;X%DUhW(+SGId77bBsVTok07mKlxD)5Q&c0*3|Y1A%Csb|z~5S+ zv=8{9#@~Cs6HY4((J3s!+ zf^6Zz*}P#u9AQakv0sfr5|Pj!K%T`3-grZL{QOPRiWy^QltR^J0u3|!%)Xi{fI;TB z?imh;IL)9g>tF{*V0MxvR2DPxxHaxWgmA=?raMwyKvFIDdDz;;zxFG&5CB{sY=w0( zSRt*uvR0<$@13pD@gQrQivNY#_@qd!f`Cvtvd|!ta?c(oZ!S%S-W5L#D0~;}5uOp~ zq~%?S`3r!5f~t_k0cMWz}|L1!@or^o=Fed-hcrg`TLFW2+HB zQbzdy+JL-ZLeFsF=nqBJu_?Pwrg+=wdE~;Rn;xjN-HfU0n+|E}o2nkGP|=$Hu#30G zgPxxp`?mB1UA6O?epvj|9V8p3YWp2RTEWO%&>x2Rb{MwYZPx6g3Klmkdo=itM0Ez{ z?)RIzM@dWULdO_0tN!wNupVzX>QGn_GSQ&g=PUD0*8%btvmg0tu5w7J`nOu#ZJke8 z=_4hC-@?WhzX_Rj+mhopFrcCt#z4@nQjK_!(s?wR_(RQpdt^IZkbC=AAQSgr4G#px z{eP%a*QO~TRX)sjsD~7m1N^WKfL8J3)D&Q^@}Eut56GG^QeS^58{yq4c+9}!DD6KW zTT30Mz+VHE_r$Ffl(jEv4g{e!NW(=l2#ecABkCdnzlM>nsnI-P)h1f3*`;Y@Y7N8M zLJ78&5F&eHa)M_9GMb+(6+_OyS>MGDJK%?k>lF;($QRjt#Y`Bnb_?DA386Nju$CWF zq_sXOUKM#cU{MZ>o)?b32!2{B@pK>sFVGP zx@2De`9T!x7o2go{+ijK?r*h8=8FtM`niFviBWO5mQ>1OISeCy_z`0`TOomB?i+-y zK7x`fESx!1`q?`mA!mU9pXFRjEoB=zT2BYCw6?b^0&t-9f#<+ zjj+%0s!e)8*WmThfMM$wnYZ(RbHAfI*hq1Gw}Y*BICT6r>#P0ri0*f?+=hKWFxcmG zQlrq&bILiJqZbZ>;Roy6dUG6zY26ly!#^(KII+mvWe;h!QfKx)>bd{xgAxhpf0$(P zUdLwp`Ffj1rxC0j?*f+hmH?VT!QG1QM6Ag8?Q4U<7<@ zA3i=4%YI(c8P8gxza$)74^w0D>r|5iq}ZB|W&=2oV5qL4g^seX>8u9VeGp70S)qbu zdKax@Wy(!zhn~@pozaLWVah7d5X$9$g@f(ESk#Uz{R(k`g&|f@U=LNQ6j=ykt$-?2 z7J_9=%>zey&_>2;1&*2v7L1a-;BPplh#4?fw8zHk9V$$kF?ewH)lg<3?`%uK{%0Xa z$=VXMy_W#Pwc-oC#s9670GYaj18kLLnwt!9vrQIIPgnZ>z4zSqH;8+@qL!vms>I@{yWEW3jNKl-Dj*0=Z4BKGcyqPCqudNp`bjnUQdb>IrF^|p4yW!fj1I?jswJnl%X#u zrvp={s0IsZwFi+ZU)P^52)5kc=RE8U`F7ct(?&*t7h2J=9sqSL8f-S3o7?v@3aQKilmva8#6F7Q3Y@a zL-{Qw9s-ZOhxR~N`o{+tk~W(|4=%$^I#}a1tKj{AF+RR`Z~cFcOo={Yta1wz*e#|Q3C35G{X>h>jBQ!8w5Ns5_KQ**?}u4!t|rTCnHDb=`9mAq-Z$6%v|lrF z_mL%nYckTV zOhyJi_JB!VGlXz6RKX3BTLfIrzqYevgary@ke+WF%Rwd<&sdkSs$j+iJa)Vt5z_YA z9*ga|zY<1c2LYaAAR)*=v_U)aVj^41n;U^4*xsI%xg~jW$?`W}5wlIWMG3@=c(*CK zp^|WxmaK;(qKhXY&D4M$J1VMJvFz&TX7hKsY~?_IBM;cmLw2zgc(oV-Dy&0v$O#RymPXmsn26 zhukdU9}f7|o2i4lhBBF#c0Ge(t`y)?!lx13sl9(-WN^lg&_w`4N&ul595@vMjBG4y zZ7B)k&;dV^XYc=lU!bv*Bp;N?wXBJ>+kP|vwzhN;>7MjeC!y{uOo~w3VS58&D-dJJ zU|#V8`sDTeB%RZgd=Ze;g3c?^zN_5aztLD!!CC3u4UnpGOp@9&NS_*LS~Nc*)Kte- z>X|vi{C|ncd81nOG@1X2r8K$9C+-amR~DY8Hgz{*cG-Z@@)=^LS6rYBt@NjV;$%W+ zvq2@64$_LO$#73Zb(S0Xj?ilN9NhSu1p1IC{trFim$esnA+{C^s9pXm582~GPc;Il zW3k_EvAM=O*4&#HTZ{uVz)DzB>UTdF$x{n-<0k;erkz7Rr0@_ElN^V9Wo>ONEIK6$ zQkcUx@N)ugah}ri{e{JWwRWCLGGS8gd>+Y=3)yk~b@d zievvQO-*E%zM;zO#|bDE?zg3^#!&oWQ#=3%nqQ%qxg{L*C4Z(|SO&hET#6>28mumH z&}zKA@*^1G%bDLHbZRt_KmlNt3)qGHS4?eOeWLWs`}AroTipjC29xc668~^_P^bec zdkGiQ0lkK72tg|=-+^W^&~nsW1=uIx%Fl5@Hswd4GQ9ncqkPc`9n{Iw zTRYf~63kmGXl@=ad%aP-!!@#2))gztrZS39%T!LCu^PQt&wR|Fa0}b&C82UnAmZVT z`FHy#tLK~f;H6&`9C<35C85t53B*{a*j1r3$?9Sqg zO10N)SP=q~F!;SEy+U_MUIvL4Xn^tGRO}^g%e~&Z8udA&P9DPo>-wK%8&I#4u|THD zkfg@gsPn+Y!XgdMuc?VCEu{o%nvQ$qABECwWd>!6iPhn0@9R+fwgJc=0$y%}MC!WN zes3-n;0LOg zkI=8?A)Y#POC47)`}sw`^2JoFlX;9AuHB*X9$lFj-DPzGgs&x+Mw+KHgX2rq4>B{Q zV;r$7gV3_!d{U$9g5#0%8Mz`NhF{?3)AgYQ%}f~(Looe&6!N}M8H`L=3VEeVIq-3f zC~W@T(R^q3J!1Mf!Cs2|AH+9GUTWm8>m22(oMm7Rq~>|J<-OKD7t z$z^tj>-pn88o%=U7O$td>}25*S1g;j(r}Lhy?7bLLOp(TTG|s)peOg;;gs^3l#>lI ztg_IY0pHT0tF~pWXCfZ8f#PU&>#j5DRT#4%I0*PL|9(+JmJB%>omTU%F?28h4ePE> z965>>DFKFd6{>Lh?TA%fpRJ`o{L~b18RkyDBI7Dyc4abY2g?>L5ms-wAz2e`qxGS` zF?ji9!r?7Ea0efO)a&qRAW@QO@+AMk_R>J%C+ORb+ByAEj*~P+q_W{WbwV-eD)^*Cc5J+fnk@QXa zPS{s0;4;NE8vpz5Bo83VpnDIkIM86A01E^d9!7}Tcl`p6+;17h=Ki*ST0qpH14`F9 ze9{9RwS5M3OAy;IaFI;wyJyau!*b0n_4C^)K@+0i3tD)yi(d-x7Eq)a2kIX87rk&Y zGIiHKeb-lSf1@sF5s0Y+qYNM*$IA`r@^@jB^mW9(BPKYv?6IjlIH{_7OJk|H92ABd zO`tUK>2|odk46uH7W-&eC;k3@(9G3;-BxXs=&) zRS)*Dx^j>FNUQUo66p~rrA1jU25yu^bPOz~3MKXV3EyP7Y{##g;YM9{$8WP?F9)~(+Lr#0L&$?(L ziwx3f=+9Fh!h+q^6mSkZo57-82|40QtB!b8V}&ZsN&Vj*4@U8dk3&c4)L{VJf&kB4 zYjyyr0AdXTO{_ELQZz7JwF&*_>-VpaDZPV=l+m+`knCx4GB-SGNz%DkV8nw~1P zjS5AF0N*GLQm z7&#gVRun2+$Um(Sp?4-hcgD)D&G3;B+0pNTJAbrAFo}xGwjE-$3s4G>(ggw5YQ+mf za$oFVzW1Pk#$|`{zYj#t%KURC)nTZAcIPh+o@4ZQJsC@3tq6)}$Lul5$Xl4iriy#( zvyCWcaUVJ1GNqVy#FQQr`lCdo0&~Sc@sVt@YOB^HP)!Q5e?z{8xmfD$m7p{W+>wd%&lyT78*8!+|9|QS@!lcM#i8A(; z1_sbUrI31jgWo_;P-?k=`c&w z_Zn8Ms(9Z47wv#S_`?Gd0Mo0&Me2@zv|-n&qFq&}1uOqQbiGwTU0V}0h`YN>Ah^40 z0tqg`9fG^Ny9WsFA-KD{ySqCCw}ZpKbMJTmnTMG-9uDX1y?RNl?y73u|2uC1-@on* z-rDDS6_j9O4PYMwSTw+v44D`JP995KmbeHi)jgc7$uSd?C!pdKEz!5N)CBoqB#t9} zz(*rMu~JjT<|or8A%3~qciA^+rJ^A3=c`Js}0TNz_hr5+F{=M*hU;;b5%M zgo};qs=@Lgwl&Jp7<`9$}v5RZe{>`}gqcZ>D`!_Xsh%sQ10YlAX`W|mWUUjI7 zF7sQ3S)vvO929^=Zf|dYe4(Vl zLf{D~PeiLA8c7W7^w;T4{qT?pIwMfn4Y;E%x>>nbv+Zo|v~`eUdREtHx82+y{Dvy7 zfgi|vhc>B}nGIb{6x0sV$LxiWXAP)DBR34b{G-?wWCw6J5OE0S4a`si^=%kwdx36f zRbS#81k5Co425EQi3w($arv{$sA)h@`+v>^t0NSIj)7^iWQx}!M>*4bQfpdW?MV?* zedkwIpP!f)E9|dOgk$N5NDU9nrPlCVn4tZI9m8sX>`D&cVI&w@f-O6>VEgj#hFbq%BOZLagC)Ig5<4fQzd-i*I@C{=B?w)7d01bviwLK zjIAFvRZbfWrTxqDV#-vdBT4d$oC3__r+eJ)cvM-t`SPKqzizR_V?}6Mjo{V|b$ zxv*$+<`9`FBPLo+O$QsqZbBrqw@>222@e|#%y;&Peu&CW5MB8hafb86W6??yHQ-X$ zow3aBewUd1c#EG+td!EkscHi6`+BxHl=0%sM05hfKC+6sJ}C39;dK@zm!Y)@d=nht zbczf(!j{(I03^b+v^0et>%Ce_J44 z8-fcbV;0vT^uCy!O3#oJrZ)&-FCR!4eM+_K4Z@ovCRJNw4DY)_ud9GkH#cK@cF(7W zK^(ELvua=#msg15!9iU#((wbdZl`xH6fnklbcVIy}WN=bE4>rVLPiM6VzSMj`m_`bO2`n%s^)y0=kkDf(N?s5f!#VrY{mXot;nUL58tQD_ zkePu;CI!vWk3~kAQL)(sN*PjU$N&>&&5BT`4a)TKxQSu<%L;{xhWO!LLt90Z#?kVD zR{CNhEFf0$fT+$Iu3JB~#60~~rj}RLg}k%ly3_6JD;D?0mGOXQqdhEtf{OlnhGf&l z-j>@a7A?AZw-G_3iGOvR# z@RIh6GcyY@6sKa{A08)cn~v2`Ivgjz6VUHxB24JBTc64O5#&Gbxf5H9X`}e->Rf?a}+gqCogK=D~fLs!FPEd=4e`5 zF7m!?p5a&tF+f=eR{e(M`kk9UV8cy-T*$emtb<`T8_o zN0W;`${2>BB^4jmbhM&B{yU1xN&0rrBt}y=f5hA3Z+Gf+58@UlcUp{2T+@|a*I-fY zde3|%vz-WVvw5?qT(dS3rPZ;Ey6a&-Dyp)ImL2K8d{(pHFyro*`N@f>yTgpS)<;OH z(|@K$B6DP@U)1hf$0ApOCLzeQwak!WgZ zeWk&<%TE@BRwss)KUOws9&h?FFzxam7vQA?&uMkuV`GL{7N&x6DWO`t=kq#9GbnRg zmKt5HkvXuw$N9(N+4D_EJE?3i9J$3TsS-=~ zwR*m{dS5vKgP^Dcd=3G;423A2tq8e1c_LNz*QxSnI1iz>dR(mr^OMUS9WM#7Q*BV^ z&4UeZ)u(@e;PcN+5)>TO>oq;$oOAZ~*ho*CUDH^h$v+emd5&5*m>dZVJ_kZR?~Th0 zk@!vNRQ4~t$#!pw#BR6P0uSS*?k%qtKAz)FFMi9<9~O08xZf_`LM&0?bM$9!UO2T~ zVv}uOHj#<&>3nZzj#GtWX`UVX7nu)I?2bX=F@Id$=-YoDp0_3snlMtB@D!7}5*>Qq zeTcDf%u8WC*zn{>ZR^Nzt|r3SvdI|g)Ul(*CE2j5OjTbSvh;n?BY7AUYeC#9I^J8V z-0V^-fKsTUDf5gDJ$FyR-O!R$MP|F6nr%9YpXDVh1BYp^e9dZo6A1mN!;0YN`-1y$ zaUy55L?v9$Q%ua8b+vmYZNABtY7Jw0=0OY`&>>Rj(!&1@GdIGB5P;m!8a$ zDu+?+G+NB5^oZA6yCaHJbxoK7uMe3JpWKqxdqdiVNSyBqn|LO?7kWUlWr#HbJ73`% zAiB%;dJgys1liR`SO;~*I=Go8K?u%5g)XQGsvF&J@7rH^r5m7o-cKwRFGGKu*9;%8nMn{T3jUR=3*oNl}Ic0{iI zP>zVdt2;RM>^oX%Gh_DJ|1rw`yGHG$_riQuKQ|Mhd{@}@+vdgLwBE~4g+s=S`2vho zXIxi-_0eg)jwDRbP27oYEbEw&Z&PG@x6a~Qz@laB?vCv{EMq_F;~?oPo6pV;`Ipzz05>hVC-TjHR; zZmU*=du6Y7av@)=m^Qt>>vr3(7_ZrVB8-`X@mNFFde}CD!hK!bm$?E@%Ql%T;0zXb z_Zh~%c_0%CKZk*3@&VQgsWZf!B^_Otd9?JcxT z^xnVvr<)&E*ve7lsDIz+%(k@kzzH>hoLC=OK*p;&?H_{*=tJN((Ck+AwBco2k4hcThq z#`D{_)2v1U=e|t(x{VVlE`jro!oQFFmIB;wkDg~u$_}DY5$du8!drX_E6_50Xg39S zxy~PEoxQG7r-|%e3V9y;DDbr|K7n74k{ni}TJY{Bjq-Z`#!Tt*(bF1Vv((+srk^w& zqw$olC>`MJ`M-RT>qHj&i>e{2))#=GTLEJLId*i+Q; zisHe+EK8i}1oLDw3~Kcy%*w9Jge)L76Yl(KO}Y7qgF2}#@wDUtfkZVl1X*aW9|@B{ ztXpck|gxCDL5X_NeY=k_^{_WYD#PY0Md;s0pbn3BDuZM4(ff zKf+@zQz{U$YnUdu~Esd@~I zk^2b(X`gdJZP(@fm)lhAJ%+#~Nz%CA>4TlF;Mx3#L_)3;yxMl-?4Q0=300taOhnk? z5sQAgFJQHwau{1C!q1r0QS*r;-Mm4vtsGAhLOVE0>qH5WH8pIykVIRoGM6c1Rj;=T zTsPtWuA$p{j4>g20m1Y(!rlBXD4o>n;+y99-I$)SBDXPuY+_`NqH7mN+Ogrr z3dDI_p~C`Jq=s;Fz{T4sxUTw=tbyYwBFeCfkgy~pMH~!T+0!+RCTXafj4#n z5~|e&t1}CIl?e}t&7kL# z$rulj9{?Oj_YBCn@;uPw-QmOJ)BZ`NE)|;naR~|(_mJ^VWPZz3QH@er)ako{Joi4> zug|cPgVoxcO;RX;-Bcwti?e~#WS^OOJI)`NJG_g3r~&oz+Zf&TCbnwEwjG) z8mBrte5>5ib(=7;x4x+}9((bOJt_Zci&D>!6L{nEAd3R8njIR^*bR#W5^s+XhpxR1 zSFq%XsNCd@OE7uQEX;QMk)Hj`uH5OxuI6juNZLX-I&FL9ZaXskoB7QT5p=(OJEySt zO+LD|t}=u3VWT8FzBW_Y_tFwY^AcfF@D8f;v6A|6RB`j*pmLn7E!wokSj*D~p`@1t zobR_4+7ywCaF#K6Qn*3D9|hx1y8Ptw<_dFAwg{Z&9 zFj4oO_bHfPoUzpK#tz7j*+%GvD0k9pu47TD3hZiJ>=rHeUiHWTv0}R8kpu)2=JJ9e zcPL3$o5Lo4?nQw{40-jgSX#8P`o7E4#u^bK=trr>5k}x6@L2z8F7AhXjF(Ow=G{&^Qoa|}FbIkv8FYnXCm{>__ zzYH{1>3k3vy-{IRY_9v|6=-e<9y4I?)Z*a9;>!7W-BC^{y>S}wRPFSHtq>r?XYgUA z2G?!7KlK(#I4wN7m)Fq4Z=bnsNLW`x{&jnBiudOR4d+lw;C=6B)KLnm+&BvOs<@9= zWY%YAb!r_UiWm4Ex4(H#@Lb2-y$%=nn_v6{E(EUR7qA5DWUc(`9W}5{o}pCL?@=P- z35a*o8>=%%MXS8A@+6~w*Ouop<3ZWgvlZ)*v)PiYCk99c4h6U_cd<~_|BU(FWG=L& z?6sY?+WgwHeLL|&Zz}=o!}rb3O`B%#i&b@C$s1H_jXd~5K3SofEIrdJKBy}~eG`;s*oX}| z_e`c0q{8|I_c@?Dz(5Ti2iY@w`B%G(L4ldAsqh4u1r*E8SE6F351&cV*I`rd@Q$Ou zv*EYQf(Sx&Vv5af@o0+F=sgx z=W}0u#yDi>%0mKiia$@XQ7?}bXQvIk^**be)@OO4-M^dROfvDtvgV0F5#va|b4K>( z#hO|zNoJDHMCxuw?*jqa`!4jJic;Zot7?O&Jx06hbvx!A<+-Vt;&5D?-z87ZbNs1* z$9BKEP-Q^ZgRH(JJ;2hCK$6{*c(uF0IS__W_leW@p(+@X3aOBFD`%?pry5D7 zbhlvQaj&);gsI&?k(%3Ovhcy8psC$a7}@WI*+jy@;X!94@-?<_<`>vXiL~X)dKk`j zpw*`{T+mes3V|F?vUqlL=H{9McmL#gPJUCAufU}m7f1F0_N+J!(kbhzoCR@crYnSR z^OgMCi!ch4fD4qxqRiXFGb%iZGpA&u8Q!|>^?jrBiX_)IyAgv14(uSDg?4&K&iO)k z-6dSC@C)jn7>i$1nK_SDdH4&q^sv_1b2p_0oc4REuvbHwsF$;wB^jQ3c0__VZs!z; zX|6BZK)yWIA3F#jZfW2EMU&oX(JH*WBd~Ig=p_bZNbuJBC6-)0d+*{0?=5|d*#PmC zt-LShXWb>YcAxM}D(c?rs$zFtMJX*|gP#|BpEx+4aug91BT(I_T6vPeD7 zw>zjWb_RX9>^Y+jmqu-NdW;C^t%s$?DTTb%ksbB$C&HCa6u{LEtBK|x7~~)151K&T znh~;>LDfl}eSyqQ3DscbAr0Zw_Q$T1yO)~SW`|Mlu9#WRq+7PJYu(cBT`HqTyG*-H zMxhue1iw|=2OUwzjdgIkWOpADns(?1XN&xm_yg%}TjGT`Ug1#Jo=~r){I+4Ud+Xob zsdn4=YiL!wg^w|8T3SLj1t0K)vlC%}*!nzaX*-@fi6QJ^l3qRS$~)LrwqxXUIwP2R zos<$0ed9XLa;eMi!N&FKV}5twZhS$^h=#Sl3Zj4s3e52_oUGhbEEe36@=0k)_RR7M z)6yH+DI@(F3PJOSDnUFL?bnJvxB*xC$I6zYdB}%xED3XhYDKyT61+F#NHn{|l~99( zZc2nIHkdRMTlUL!?$dKU=c5t?qVyM#b-TuMicL8)-q@e2y2T2tv15?+ndq|@61%^& zmVAM}2w$+cd>q5dY_u{ozZ=me+(`{dLPAnWTVb>foZmKu}uG4)of+dS)@IZB#*ybKMaR{O+@+y= zq3K$k7R8CV_o5Cqh)0FL$&vpFLZHzk)MY|x=a4BeyD0xsfu2$1hz2@p!r@X40S~+1 zhLA+ja^5bHO&4_A^@` zF;ij@x#QeHUEB?4G%?!fT`5aG|BmjudqnJ`b5GT+|3jbU8+<%wcvDb-m{&kU9nLoW zyT_x^nHJr@Nl+pbSh>Q4K$NWRAEU(#K$oMBcv!9VFD1U$v8V!d^$VC&Gi5$E_HR{d zgQBLfJ*}nka&Xm>G-_yaDJ*fHBTvaOct3W9i21p22AB99oTh7+sg@(BEOu+qF*2im zEUqB>E)rSIx5cQgcskNEF~dRAIlIxQTR&#`sLo4_Kt<_br(a6^p#WjaUc@q5aOk)G z5~P$vNPDJu{UQ!W26$bXiBrG0Xd|xqd=ybqMI3i#lk3v_TI;t)6l#p{5f3x5FnQHS zXAjeF|JLmcvjHi42)Qz0-$ox{@UK$trZv&a)L8wNeRzvgNjdJ%6SqgJgAmNBnb*?>Z|fph|c1ZOb}FK6!T`75xJL zjHu+^D)>-8uW(X5Y5Fa*Jw{)&5jJ7Zi66_V9DE+L@3wJAL5A(&i%BF;GzYK1QAw}~ z_T__x>yq=D){sYl*s0G*uKr^^U)-0|A3qfSNrZ`o38qP`8zkG08YG|n!9*83g@%C% z;^pkJXor?J5VVohLj=4ntvd?LQK@O^QE97Tf`a#*5z1$#;{QPSKzkb6H#V6hr{@<$RAXMhleInhqG{?6lM7(ZuSgK4jd_ePRuG12Ae*= z`(yryh;>!79uCkbmSfR_&NZ(DIHSYcyweRgDOl zq*33iBZKbkQw%dmbqREoLtZkcQc&=wks?s2VhH}QBGic#2u##djgnG0VWxjd6aUoX zZrNGLwB-@$jYbu);1Y2Tl~oj^=RHE8|M>ms0zd>^kfBvFu@;n*oc2CgMaHjv_N=jgjh`tccF^@)69mF|&j%w^l`ZO`Rt>DXdfq9-6S%(Xv!n%zO9t~E`)PXUP*nRh z+^+tv*JLXQ{w1R018qLJH{!0I|4yq$cQ6Nr2a{y1sNj{NN-$eTqsb)Tzcta3(VFOx zsLz7;tJJdoFGeM$29hEF7AwOpQ;-6)`*Ljm+Uuf z_8DTDICYh%7q=+*8*o+$EO0ON6|Y$L;jin;UmnTv7z^B5cn6Bt#Avp8{A8N^rtR@F ztQ4~-End67q$TegOIYuSfjSe?>~AW(UkWtmN286a%XQ9f+e|_f|bsKEuD=_o`~g1Ri@*W3nsAH0lz%6u_3CqXo171NpsdK7KZTuOd=~qXOSZ~WFsGY z4ol1*W65+c5iSaFE_XS`%emP$ptIhRk~Q}7w?^iZdU4U-fM4*0vo0)0mGF);l&yl>{&_RetQc(kR6@y>O zskdNsV2MC!{w-gP%eyn+(_q|*EwsH)ApR7za~aseShR!g1@UQkdbNch7kGOf#_=>5 zWyB5K_XXM{>Nefy_to^(f2fLoCnk4Vrxb~5DkXNRRVqH>m~Dz&cS-zp+_O1_lJ7Tt zc%w*z<#Gj>KUsf(o+4_%YlwGEYktq8uc+HFs(uDqCF& zrvj}_b!&1aB^H}!8(oU4x|9$JX~Qq@LWX@1Z@$H9s4pskxXb)1cmfv)O+q21im05f zhcbcwX^p}T`>boqvX3?mhlB#$BHbIU^n$+f>5C;vl)hk08hVh_VYSPdCIJC~&~jum zH7!b=aVq<*~pfR4h zqLl$OBg2WVd@J>5b!o*D@XFZ=5}CcTlJ018JN_KO@0I*btjN52u(K;zN=d|TkANwE zSzeOW{JoH12sAK6qW)o?P`PUe4w_+UJyH=Oa)_!6Jav9;1R~rLX^z-?8-Th7|MrT^oFlJVeP}*Sy2QIk!W5yEX4=XX!$QHyLnHk;_EXH_MliQ}9DM z9$4+muR}|@y3Kx5Kpj+RymatOYY%A8aNGthrX>_}sI1Itmn(jcQ8*;GR@dA}b|Sh& zq8I>1gk$>-fQ1F{knXrAMm39=b(pgMJZtJBKx zfW;IfqwBqIEaW|48j3|_6P>8hqv!j~%|`8;i8$EO9d!*7zh|BG#nI!>!#o@m>ggzK zdeZQ@x#UB6z5XbJN~`7T_ctIGj^N}=*bnfo{3N!R%~R@zB7B*Et8wR%A0p9LnVeNtDX}6$T zmfg#|1_C4qv$O$&P}0xx@~G(yUUive+PJoDe95Q@`8m^Hs;wq0+h!BH*T^N8hRX!j zr8joUQ4?CnA8}eYO%4d9!?5yel;Ox-694#BX|Kb8m{$O_Sg?Fwv3>u1_52F(HWqNj z+i-{nMr8cz?TIVVE3J}ed)!J2FifSDgWEl#qA0YBKaBBN3|knqNn`*=3UrvM>;gV= z*>|W&r90rgN=j5G#A(|rwUUDWwp|Y?FyFgz*JfIjYn2nnMuJIYG57$0_{8B}3?u|{ z7o7hUt!dvcg};>)RF_xhBVqEaUEw? z^!}?%3hcY|Ub}k+D%N9k9R}U&=K`)!^sP~nzD~rb%r~s(n{tQrhYiZ&5zeGcd%Zy_ zx(iZ3odN}*(WJ1Oa$!=*{O;)RcM|}5MgWR8pyVDb<}a4(S77es;sUrB0?m?vhQCb7 z6xowEx+TF44P04TMzBr==4>wN$i&3Nl#oWJ2t7*sdbwt3^etiihGi>NB@QHj8JrWJ zEYOC0-@yP6CRhx-a5lW8B+-r~c{I0#4q*7$@hGSy9vqnLTs`DW+M32GD8N~vUuw4t&E-zysvNCosolM$#`UShK!zHT}QvS`cuGa zgig-$M8IQ8*t8577+}Y~c9yz@IX*7OPJVB~ydEVzc;564IiR=MEfGH*_sD)BITSX3 zZ@;@~bX+xEwZmgQ5VYsM7ruj`SH{UoqRnucR^z?jf+51Y?Erb6Kiv=|IrYT zbyo7@TOa)Y6j{~3ZoJ*0KJL?(e#Ot3b7I@w?Cet$7IcBb_H`HXIK>rcV;F;9T27o^bNV|aL4a`a)perkyfPXTIH@}^OQHS`BH2*jIh&R8hRq}&>+ z9knnadR-}9CJ@v>sVb@?M@Dh_8?<)s<5=S1)su?vj+(seVS%#qo5!Kirvl&1lh`6j zdI>N%;ydIHUk`pcEbV$(vpo&aCFhG)Pt$t$fZ$7{q%Y^kJ5o6{Ii zq%>+J#okD*35fLZ3{seaoaF%eITLbOb#ci4`SS{(qAh|6*kwx&Li@nVP(2 z5^lR!+|>o%q2hnOWo%>ec)wFOU0gLb>+hfkg59Y z_wKJ=n-|}t+wpHQx4CU$%)Sfp<6Lg z_eZfD%KH|!o@v{3UdyZycg+n)DYZ83+@oHNEGzSrcW=zivt-dGP;VP!JYR9K`>u?G z50Vb%mJ`U}i*DbkHQ!|R_X%xk@NQarjBP9}G6I}MV>Wya*-|K5dbNu0XDjtG89eBI zPfs2p$b=HEuG~OivFTN(rlv&5(8!2T0J0){XfF>D3fAXhDEE1p*)k+-R@nrgq6T=& z#eMl9l@Dlp>gwup%n1M|i_eO5dVekVr1ptMPf#R2pi9J(=oHJWDVCH%jh-z6Snae^BSVArtGE~*ke$usO9S>cK>zL$`_YTDdF;UXz^^sSrzou`Gkgp|OY8qT zV1E^_qNAW3vqGzOJh-ZAu&U~~qEDhW-KQomI`r@oTY1alSQ!h|VBif_?-H%?T7t)g z+aC<@^WF^QJzfSUr!*HO+ppWs=CY@%D^bbD$rNt~2I&BKp~Mc+_^< zu@9cZTuX=(F&bX)4PB2!PjFjN^oDzD2#c4Tj1X=!e5 zp4;4e>TAvo0PP;II$qQ#^?Hr~*zt$b*>IEmHGoie7m8>w3v0cX{}9eC#WRq zvu32ME72Xb1P0!~;Ld1dWHm`Eax{0V%ezvuK4QB!6cZ(sRkl`-Ox=ba^!!@a*l2_Y zW4Tyiyg!l%_#gIGpOL11#KICgHK+WSK55UB`6`5G6>XkYq=gL2s1%uHELW4NEX*1G z8hT$jXeszecK$xikfq~5Vk8j^$H$QLW{DFiNjkYy70W058@IiOa5U7hZ)n)-jh$Hg z^{9Lerzu5w;PEK+8SlO$k7a*GS=v8a%x)%J#(rbONWL_x{Z_<6g`0D5tn2-L&@IMf zg0I0SvX}9iP+0%%tZw|ZJO^?1r+HR+Ca}}vCSNEhC0)H%G zY^Iki-mg*O^`1N{THZcdFu%bsX3o$F7r~Q*)EOBWU+Hd2-ia9S7*=P1&b-G?4iDR* zK#=+k;|!~pm~*~i1uM{eV!?wt&80>Og#@ns026oG%xP%Q!uH|^IR&un;$O!8()6

    zXZOu3^W3kQzr1WeyKk)rHXlFmp|oGhm~1?9``iza6Wz84;< z+E{h^@mE-Gcf)QL9&h>%baGny4Ghh8_JlL9cLn;oMdEJ?1wk@5M`7A_CJ&xJu*TAX zOJuzeo5)yxAA=U&X-Rv2*mbBepuvt*h7)zu&H-$e9+zK;3D}RSe$#T{WP17e}AOs<^lk``Pt840(TU= zTSvoKvcW4~lkH~bIbY7Z2Ol2GINY>&fy(Mmnqwj63U4o6W0#mmR4|%n3*dh$FE5{; z-}EHtT1D4o0a*&)xR}%>OPr~yX;c}t7x zKPfigo~5n?2X#2WbqxSDfPDy4ve8%yYvS3-pFav% z!TpiAwl(Kfe_>~P+`q{o+I_{VaB;e5`(|P|6D8hjDVeHdOOHIQafYgC*KJ?K`_wSjZYi(9q{a?KRO&pN_E>{_d z8%x={f4EFqz$kkYB8f$9*|aX+nLNA!WC1O$t$=L&HK2QAV`JnBXm7J^=aw)SOTZaX zQNaX!GhA>_U8Cz1z(5D|mQuibFP;2DPWh)e5=LCm(Ls_t8eCwWs8H1+W>-ZgRyeC+ z4$C#38y!YkrUYER(IfkZ4sX)EGfGOv(z#=S>pXzWVXW;d08|ecyiOi!G}(a*3a-Dx zBm?(>jqew5u+cO(x?cW`$&y9<#~5;?+bi>?l)&ix{mJJHz>XUJwIonPKS6jD#wS}U zzoA%)C4GEmbyb=v8Bjph*Vji8a@&-aj{<)I7Xp~jUeR2Izqh^W#TEXQ2t=K>%TVE) z+sMi)8mf~oe}yu@H}_Ti7~Kah8m7Ag(dl(qs6?NX-dcONkJK28Xb^(M3~C2yLj84g zbQpA6esWqaAo1cc5RL%Gse`f9XmEBr;ncrQo_^311fv7Pa?xAkg0z zqDokvme0)wK=ML`&gk#22|QbVkPWrLjCW_D8s)j1ld-Z(YO?BN0BY*Dy@;Tl1Iskg18EYfc z>Gy5yR^AIwoCc4o6OTg@Bb!X^O5l#z~e;~rm5=w-j7KuS6X-nnT zeejDmhqOccBM_Fr{DfgDhhosVkKczYI11_O5yC6&F$7etu6dZAyOVi%e*e(pPY=C# zW@qkbO+vCE7$hD}P1f#%r|`CVyf^tbArDvmOotQcCO|?Zm!Y2e4ao`vLlqWaQH|En z>Fu9!P%juG6BeAfLfNlF7pxy*l6anDz#FSiS z&m6SWdU`n`TJD2^*$W0!ier=WC9dAKL-z5WwHdlrJuBF75<_(#98e_sqSYSit%2HC z-Cxd_l#&7@{5L)_J8s@BUJd5+GIWKtjUv|Ks0N542mU50kSOH?rys46wA}`2*eggn zfa>)Qn@nI+q3J7Xi-5D()aEc%A9mY{F4)y*H;+7L0B?T2H33eXv zgf8R60*~jKY*{P@@>%Eo%-88BwugG{HFCfHo$!W}`!|nO3@7ikuA5h~^XrgzoFm8~QnoL*IwN9`-Z|6N@Y^Oea7Ob<~BvDZz&Wz|zB7-`_4s_;SP76Gg z%2QlwuqJ$bd<3Jm=D;8zkTNhJ4@DvVz|XIh77s^0A$_-5zQi7nh#~m6FHhm_iK3u? zyHh!?kpmda>eIb{w>H(Wz@O%A9R`Ctl{rLXe`k6KDBl;^G8W)})ot<2zgqXrG7!C1 z?9jWVR_kN|vmKPfaDT|7MY72?WGgu;`H3>{szM6nL0a13N&3al>6Hno8nJPV10=%uQEdvgsuCA_nxvII$ zhajGdIbR1f+m&1M!0SVjIf~UULRQ~ZQI7?LVpIM+l|bvBRXj}xxy@ZMQO2xtX*Y>OE?-+%U=Ie>jU8F z=fHJQxg_gT-;Iq$s4_Pr{^E_-6^ST;L_VSaj5^u7iphXav}EL&#Ct4G((&4Bp*19! z5sLryKYl_Q6*Dul3>9W@aPW6i)7_I3EyiWo`9iGKLe!Hxk0=QWzV&eF*#u|$ipo0} zSt`u%@bFKcKFvv~t|SD2D05(VS zHDR(=Ykcxel3IEC=v7J&<5YLH2*X_R5>V;2STs;rI8GrV|Jgr0Z}i~OIX1cJW49l~ z(T7Tt>nOC2=O-;6&Fr3z`A9eG*Uh!pgGlY=p5YJEUt^)1y6+FAzX+f0T)24^u zI^>OdqdxrmPo5Z@QVY-5G|l;Ix}*_69odm$upU@K34!gk8uG;ukQt=WI4($Zya{DC zP}uzT6lt$q_K6fO?ZF||s%XQx zSf6?MV@csnJ2Oj3CQ2Es#?y-STOA`_SUp4X^$(%7>j#blA0GcmFR&Y-1xjp(53uI_ zhBaVxpFJ0=@}S7Vg1*R0#SqtG(}dg~hFFHOn2C^Lyvm(wTv>b`WLdS`0KLM*;8|Tys9O)^>fgt2iQFLDSF#hUacb>E{TDk$;FyrE}BC zN^tY7DZJO%^eQGPvZz2nso`}vtL;hsW>3Gd?aF#)iGt~uaM{Aan@slYE@wt94GJgF+Nm^D zkMajoh5-Mq^rJfJaKQE>RoBSXu8K=p?9R0o)7BubuX4kgcorTfZo6*(cZ+`FS`)jw zFhcXrH)Rlx&DJ_K_hptYDivnQ8wAz3cSzO-Fj8hLAFVu3>~+__9W_awU`nJj;j+FA z5aOXZmQ-BP^{)yUjt~)J3Y|+G@lMj!C^>DRCNaH+BeyCIbAyMV?eOc3IUCvR{+bFY z|5Ek6d?(XO zg6I%HRzAB@0jwZSsBc%>!ACWeA4ycgwfAj3YHF3wGGw~%N0U7`)Soot|HRifpXKuvQFA(N0Myoiy=5` z%5umUM;_rwr7m{LxDK^g6B%mApV@8|l3!TTPKmnGRog2JPMb=@=D^P+Sg%OfNtri? z{G$R-N5Pzgf+Xtmjeg)vVF+w}E%0PA>aG!9a-ktjfJZXurZJ&!IihzzclJ&4>d2Vq zpP}JM6*-Z1GnB5w!Jat623UNj;Eon-b!6deen-LbYK=NbT-5O<0uY#XaZZk( zpC4FP(hz(4xLhH|;!De~U%yoAvr0;0B6>?4933s!P1#b1hlg{kra&7X8p|9j^GYV7mJ@g0WZoEfh$!m*u51kG1nePNc%}6F4qgMl~vW+lhBiI z$>HnB6+$2WF5$0?hP{zS?yXK7j=G%@mm`LpmgjW3UYCMwA@W8ODJ(ADmlzjiU-OnA zBJ_~FD?5{)D{Vq!e<@WRxSwY_ZuN0F3%a>%P_B@=1wI#8b+a$U&SZQTBM>%x(jIKT z*Ae&JTuBwO+|Vo#d_rm69R#|TsiB=IN(O&gCii)N!Az=H5pVb^sjv#Z#&Wq@UQTnVZi!6#&S!7FG8*_-h(FK% zJ}%L2((-I`!^iJ0SKzgs8w0sfVsv0gakp&_9H?t^fg+44Co8_%;DosClL7B%RHYD3 zKCDPSr=JI$Z}x^Fl2e*~XV4eQ7=2$eFX#L)!ET)Y(Q3Ia%Lfvs^%M^V#bLc8*v{3Y zHkC=Ig{K88IeC?i<<>(mD=Q1wy>b^%rjMOsV)-&rfb_%6>}+^cRG&VJJ_iA>(-9i* z8A>WD)lx1QIk}4Z&5R5pBpA~8co%dmtWW7&))^$5sg%%Xe<8{<6Em~vXc8k}3f9(c zypoL8(_W_0{wE`gS$Z9~`k6IgwS&ter;N%LMfe|eP`Ksjp@EY~#8|2KtwGyvt-7G> zt}G`gR!2Qe_B!3b)CmQ7DmL4cqr!_zuye42om`DGyzMy_p!c`R-~RUSnG%~wx_B)7 zkWzZlQZ~^?mW1@i{mOel)wn&vDnCn66{bUv%VO(5fNlE{YN>V1dU#><+>PL|e!~&? z!TBfY^4QKJHR#0o7Ix#!4!Nb2WUoI`E0=zPY?U2~7-Y0Cd-sOhOy7-(Mf}Dns4yCP zy?B7tbn27UT=enjS_!FKPz1LpcrhkF(iagaiJ`&g6@7FgL;A&2etNX>z_Log$qe=x z(XiR*WxnpJgv{s};r|f!)=^PL?Yl54D1rjgA|cW((k2(jC%`bTg#Lz|cd7 zv~&*L-Q6*q2jAcOt?#UL&N_e2Fw8vr-usSyUDv+%220vtaT!nJcj@ozwW+t0F%1X0 zCeZ$`OI|~En(u6TE29iU&qRb_PIQ*MeY|MoVwHyMM05-c6z|_7i7OplTu6Z67P}ZC z=TV&oMZ}Jr#u^1fudn= z&#FobO8GnxYm@3$u%7r=7+p(_4<=D-8-^pn zW96o%GtVu_5gp@YzjyW5Pj`1;r2xe^0UP0~wY4=YT-*S)A}6^{m=+X5RLF9Iw)H>U%5043{Vl__nH^=CNn z7|djMuJo8P&CJnV_XbgDnlYq|{@$+=&ck1HD*q`t6j~40uW_KQL=7*nmY?})&t~eZ zxhyQDgLRiKY6mrGOn}?c?B|$ezc&^_srsd;tuN`PaBMCL=G0Ql-=mqF3w&>SYmM`& zW$I%0tqT=Jz4cn$Bt!$g#xlNieId-isele6GJmu2h*~q!0PJC#U=4Nu=}0d+LLL_ zT7yX9^KkdtAl1qkthulmk;}g#deDoV$zSCIc8~WAf7HW zz~yc4mbC5ZMFzA-^Y*pRhNiIXXGN6G`~tf-#)Hd$TJl|;TSDJFYxs$UBkCzaDOli! zs*qvy5GELM;csKw7BS5~*ZH>^&*!^XswfbM;tYU$r49QA2c1=4?KtwX6Gi5}2~A2$ z%H>s5!Yx)S%C#R>D1xu-dVG70=>a!@i;+x0koPS~GA+M-DV<#1+}xBhvS26r#KuMv z2@1ek@uarxctlONP^CjYUmKF>KTO3G@dht}?2^;Na47gGct>KX81?Uj{GJCW#ek%) z#Gi%ow+ooJ2OlkEyh}N|Zjf{JCYZ$77Pcy*Nz9Q$%*QDl!3}Hqmw9at%7WY~7vDSf zJ2~1ab(AE4DgoI49Us5vn;;dejYL@G@qr7=OzM4`n_aJS!bZGVPY%9Gb^}KGm zD)dNH#Xh7d#pKBPhp^!X-TFrX`D~^7TtZS(92l%9B^5#;{2Ojz33@YA^lV=xeE-rC zrse6W(D>R|5hExtkmtgIStZ!=?O$^ts?RL&p_G5rOrSkqQ<9MBLe;Cud^cS{Ui^rl3Wr8SI^;Dm`%P7yn3EpJ;Fzn z8>s}z(>oscrB@k;8zsL~Q5?O+s+)Wn`qhT3ji%UGa<9*D5(m5@Z3ncpIxY?S`&=B1 zt4<65a-^U?7@wXTUgdpi-^Lk<+|6tR>mks7{Uh=94ySfISMl0z!bjtImg@?^LX;G^GTnD++v;K7JfkH8+^Q&bEL31Fe_??-~cWWdhZjGgGry=gT!hnASvG33$fVV3HRO-;bX zvU`gyU{V2hZtPHiW5&hDp8)oP1|Nq3b(JG82?V07r1S%BkzZE!6R48_pk{r29r#de zMuslSEdV43RhgVa9Ne1$uA@MXdbYuEqc_kC1oK8v*hE>WP&jVUKOj9F-px=9jjNqx z32m}Wm!5lkcsqm=2#<_RpPt@oEKpWP=IG?4ZE|u_N!-kqIcLO-mX3~`iz^98^8l;E z%KAo8akdSJy+K!3u2Cd>&p#_Ey%84|H}#YQc!z+100WrT@!1)0M?r){t|2`j1gtjH zVx6LYPX(keGiFL2+8rZC78)Uz=&CAJ8JRGp-R+F}UyPsVZz90CW7|jPr|mhc-5Mm(2{?Al*R7_0F4-^#TT2Nd90u?PS89@0B?6Z^Q zr~yj>M&5GdecZbhsX+mFB1gBuISL{SAQTXYn7H^8)m=3m02TZCHG6dzA}{ahF>81)&Po3zL>SF@4Z%^V!%m#KkjeM z77N*ggh9>MvSjRVd;2p0dGqu0`$!ZiCMmgkco+>}Di$zc7Rt1s zQ2_?4fYz!3Pyv}2o zF9KD7rLC+ZXJGKN2y?yh&09SA6NFbl5UkiCqNC}n)Sm9D;|eo?QHA{KRD%+HR#jyW z3`B9FMk%6aW;VGzm_j)c3rI|s}tAW|sB664ZPjnP{c@mep-5x!ce-cCUK z9#$Ybmk~GBAYs@6L`wiSfLbIN0Dv>FJ>g!p24uW3iHQZ2Y=ED0cFAo{2wkvf3 z+PBiFbpN7+D@Pr-y8Aft&a8XeB#7lpH6V5ekC;(%aVcW^i-edc0!2%DU_CY!9v+^n zEB9fqg}$6-`a57Eq#oxXWCQ3Fi2*M3e}QR#v?Pp;jg5+qPQlK8mbbe9@(*M_z>+tE zi_O8_9%-t^maMK0NIQE|p}&8d0b8xXBM?A{U5m;X5&h7rN?Q+Ccc?9X!xB5dV|BkF zJ7FwC;p<)iQLv@!=>arcG4X3_yxk!Vl;tk3LmwCF$R4oN)MeADsP;=P&#z+R;s6Hn z89)vjEH-;lQc=NoFP>CXR7_QxV(v~r4FSTGbO>sCBg+Ks3<6&fk1%sI(lX%Bsgi^Dkv&a(9>(tqXuFPBx=*VfQP+8AiQ5PAg;o_ z-WvbjIKeHC^l)h)sgy!SpXaaZ&(0yicI8v2#+w-q+=okPJ>ALJ@=ARZ*M#Cst+JH2(ak*z zdU44d_#1P4kx&KnXYFr~yR_%n51xB_@5naKjext|ufCToL%LFjssqQ|9G10|WzPbf zn}S}@2y_h74pkBZ4P0-~51SEB7+g#YPeP4{GjLqpja;72l{dpA#8o}hI2(DLcBH$< z!#7A>eoi%n6|LwE0b!elFZ`~NzuzQI&I3OubNkF)s|uBpn-Vv&c$r=-?wev%hD}^d zP<%>?w228dQiJopMZA}c9}qy&>#Xg+Fv*%sGsJ18@5$eiPkuV6roE%x(CD>MMgvuy zr*2!&8WUe3Xrii!2d@U3&CW93p09!Sk{d%b7talLLMB{)OT@Nh4vd~n%$?lGQxfr) zM-Yoo${&^+Z8JaTABTQ=X+gO@i*jea-`h^nu(Amn{7s<0Qcu6Ts}+8E1m_E%##x9) z7jfEZVqX0K|By~&XV#O9-;*ij@vPjdgBD)Ttn&15W_K|QLiL$c5A?lYPk zZ+XB{kH*ren>{zBqImEYi0kZOs;4H&9f-b{`b%dEID_>X*)lK zX5I6|JN4nEeyJ?=Vh&C1J=L7|1MTzb6R}p2!7fF&%Od$b*`b1`N82A$)oJv_jo=|f zn8)4v;$p8_LZk$I{j&G1`FXza;k2Z8atY|IYmH%>csWD_@GUIeXlRSaaMlVL+TMoJ z;CuGhs1Gh(aAPud`)sa~RfLjVGv52dd@+^wc{8Vyc>MMC^%Y{f#ravc@n?u|-}{y% z!GbkgDk}{>`ac`UD|HPGmF=yw?`4>t04EiJ#$JL>YCt%ZBCIl^mN#mf4WWR4QjzsQ znar69fMGazG>WX~4F0UjrLp>RnKZWMWq&mr=+@xb7Z;R9TTP|WUrE?JSoVw6Phm5D z^CAKX|K!5P#8i#J#`TUz&-3dC%(>yXC89z#hYQjRg6p5dtY>2QYR8n`9XNBONrR(Y zz!4j+7K(;;Gtvfnw5HV*-VZD-;5X2yJ9)u((BH zx&%0^R$`8_Ol4lH9)<491e|}I9gR#dggY=l`DAKqsvRY=LX~k;+9)_Id6w-G5_ta( z<17E_@j9_q3z?-EnY*N_gz#~}7}d+4=UjXaOtm=Y#5S8bb=@SC7bPo@+R2*YJ-H9L zTF=I1?xtHxQfTAzvanQlk=%JVLx8GFLOd=ZVcM)4fG`9iM~v1y4HVI2;;?}Pv7DIT zKas;eD)YYBkg?gRUqUI|`3T;d+Y#j+?7@O5iCz^wQf|AeikOOT<=_gMdG0Pbl5gs= zBH%j+rJcE;44yF`Eb*|a!jE|cIF_P#xTH~mdl9nZIYxom#8vR0I4&C9Rp}(&)jw04? zf=j;6JLsQG>#tp_z^^WMLI8CzvO zDowWZZcg$EEsPm6^GiGtqO+flwKs{HwI*n%6=d188C;0Ei)jUq_S zmWf=H)|V8gqds%>822QR80EBCLPavayU<2!Kd@3~szXB4d&zbD7cVXOZq-|))IBf_ zyU~IJuYAM$+rggD$IG-o6iER}3>jI+f3kubl~wLR$LZ!&1LhI7GB6$pgqJya@XT$q zcGaD1F@6OD+e6(>b8a^URv}VWB13a4X*w1_@~LU#%riWlOQbfP zkK?U*80F(QnSpWsK98hqL-IM0%~g%Gm+LE%7MJb5)mv`H&;uQShHnCv8j}@;bH{uw zVDbzl441zlpV@0#&XAjm_I3QpJKlZFnArVtwy@QcS3L7Qq2gZF)x(_E+9s{ntJlZv zB0Pn0@A|nRAAF0pm@uTF_T3X9n_mv#DWV?HKdF5;(SinX2<*&vvuXH)VUESry#rZX zTf0#0a_0B3NHTx_LO*vJ%$C&sToqr!A&?r-*`n*VY(m92_7mu)E&D7BWl~tL(~MtH z3~2GVPp+p|8l78xv;>cStiQ#&C2$@1_F+4uM-L$}!_)1-aE{@w9y;v9bS)6XVe>3MXjqsvN zDn}s0!amUVdRvN%Wo>Tj{-tw&~kS5wE z@*5`A#HIatkU&bP!jIuhM-1nJ?Sw2XP%(br+eOt~9_rtzZvRaKUPZj(6(#3y_Ctk1 zpdoXpcwNuFXX6w2;G6WwLS;7wyc!YqlGNRoHZxVikold^rkx7IZiNy-Twvdhb&x`= z+PLl5?XQDvAHN<4-he5pSGb=pTHRYdYhiu}a3R>w>nLF0!o)0Cx_sNn`C!U&|Rl4UF%eBe9v-2G+Xb~|elH^J<*{NQ3o>Ay|H1A%0FxN8? z`LH@GG-{|&OFMUQw?q?>mqNGcdc&O*+R<$$ys#dnzeC&%CP zM8@LCqL0bNG0ziTBzL{%QeCV+*xz$1^|~&#fdxZEl&9gBe!FiU&8hnKcElvsrz4x7 z7bM>n)}}ohzOuD=@dla~F>Kj)M^e_SOsaiXRm#}jPcQnf#hJrB1S)}z)NW%j#DK}@ zKJQ&EqM664r}U&2;&)BE$nX4&tLcO*A9R?MHkZ0B54Fbtsj6o2zB3i78Luqj#urKF zG5<8m{phyb;JV2<7(M$Ff|EaI=2iOD@nl?#-v(v0og&r06YD;A?#v@H^XO)fyGR?# z0Sm4UkjeZ07>;U}uHF04rns3v*X-DGE6nV3i_@IRkzgFgV%JVHTw_X80<;X%12Zkn zQdM5fqq-u;e(&+u`NUm6f!oIG2K)M5k!+%PUakv9fjV@RQnzS~W!Ju|GYyfkUHSWU z&S&+Izv+ykxoOYI20HJQml#Hoc2$5no`csY;3OU?33CKmJo-3Z$fr;fLHdEm)3r05nv+KiT^tYq@^A#ut?GSh;e*1O5o z{-Sd0m)9TdA;#_%648g-g|(D-T6Oc7<^yzz!OAKrLJwnFt6dRq<*nr8V4E3f8zlc~ z`rQ@17S$q{J4*a&E}CY`$xL5squ@F|_N%Zs*7Ktxm5^U06+4_fJSmrpKHy+d_YeT7 znVktpP&u(~d1Gy)RgU#q7RU~QDc=|axb^7*QLYOb7pDKqp;lCMasN6EqdP~>^t|EX zmCNiD)%7HDnx{XFS?6c-=7~{kX}?0hA2eGGL~*lPsLd1>W$s=STIzSU5*XyVTxbi1 zE*eeCHXZStv!zC~Jj{u8E9lZOF%>%dSXj{Od)5&GwIhHi)ZJHWhZe?CKGELn%A4og z8qe3x(0m^1F%Q%eBg1yKa_=UfCPzQ}NZ<<9(c;BE-t4x-yHX{tCEBx;+ruh@^%7d-qqOzQy)g2#I!Ye?ckwr@z)&^Ph&enerKCeEk7{#u((o$@B;=gO1W@@qY zesW(Mm2cMF58+FRqT7iO_=%j2)u#JDoMjUEYq`Z6i-TqjWB8mc_xE~-pk05eIM8XL z9VNmyQ!?WX{W+~pqf0@#5vKCBt zd;EJR^c+i(@+0U~DA#Gf*KNwjOO9qK*GQ$%YV*Ip)l}g!ZYTDMd!r+lDW-&DeHXuE zYcN{Zgig@<5~*3wj+tFSHe6+1itdi*qSR&evhebIfA$692>Y|y*uedIwSlPr`$>1h^x z!i!Z^O)pHLpj1z7IiRD-2HRuOY^#FQMj+&#e<}DIPw5uiPgUw0 zy`9#xi!`9rXc&7`;E6NoGv{Lc5b~%6>#Aaz1zBftnh*Og|Mvc3M9abqr3#rhB1kDq zBs0EzgQqerchKZ!a{lZUpMr2;0=|2|cL%gE<=YN55ME^0MXiOc0iXwDsK_)Jd;e18 zKcrh5o%qPo>b|Ir4aO6ur@CI;HtR;AvGuNQZ~yjlD}4EarB(#5rjEnGzKnYW(nPzO zST#W&>sevepgYq=E^mDIiqmx5hC3kp8nu1Y=EY+Z1IlRnj68Pem(=gqV&*8+-N zeTE(2+DOnyj;h-n+LhiJ?a;zhDrCO^w@K)0D$ebjHZN69WqVw7i0VFn$N&Mg3Ne%Va8|B=e&gjaMfCG`KAqL|q+hlJzcA6P^OIRsquo#=%HIaC z91BsiMQlz)S`c*=3)DjGC1k=vnt*&Rw319dzrw#9jW$7AEHXzJo_?K%Cc?0CFEg9?IMwg+dXy(eDsh3H)KO=27;;v2>s~r*%w($qiR3MN> z7Y+yrG=ynDK%j9-yVA6d1r8$KgGgy;F@6E! zXX}jz6<3vhU%_Fc_)&)X2uz%{!AI-w&WeUj9p@X&aFlQ_4>ffBa1@j>jXjk}CJofQ z$=3vxd(id9i^RZpEZd)xB~jcY9gYsY=yadKn8Tv5U~nDvvM&E>t$FL=6RG z*r}GXOS;*Nb-+kZp9(;iXz1v`jCD*m`=G?W#}#3h28vs!Q8RYr-~eo*iC771m%XlH zd~~Ayw|b35)nQcO)1F$=*r5#$kDd_$c!u`+djK4V*$)WAc2*O>MXoX5+qr{m+!BP! zYSBFs zvRZtQwzthkjn?=d6}ZZy%>OV3;{3Ar5Ao_Y|0R(0)FueKACE+gH-Npb5{pWjegIhU zZ2b>VYV1@!wz@9EKqk_Yn(f~o0{W�SuVL2yv%qDd0XdZx|9DE~ofJwoR!$+nmOSFu+4j8UP~|fFoz>7_ z2qyGIDW*+$qrew^D~%Q)CHeoEgCy zFitsZIse05<<|KP(Ppe6w_BZ?tE)T5&N13Db6!tKSsaj&a|(a{drA{>Uh2dqX}<0G z0y65#YcYD!+;Xok>{)&iMLZ*p*=W*OVH;i3JO(X_MQ}i^rkW`vlW;0p05A8LbpEzF z!@kgpw)}0>NNijjI{+XmKL1CF5Lg|$uTkTS_inN@TeN!+`FY?JKu89hDZp`=YNz@RO`y{TR4rJc)J0Wi>zq`fTV`%I32^a2O2Ea7##p}}PQBz(>v0H_=o z3u?`W9$_UKLpPplH#dOoP*zrEL=S%!li-7#`7%QxxD-t0-Rw=+*z_Y#pHce1Oi2?f zU06`{KHq4ym!vvb7_bberDFY7bO0(L@v#ot{|_;h@^`3j{SU&c>To*f^h9UXg-`{xK#0%Qp%2w;kI#OYsjO7~Rt&ZvR}a_Q;${99)^o0Na3 zaEK-R|9Q=2i;B#w?ppsRfbQ1N#7J2LSANwu`R+{jQQk;_wr9R6G3zm&%&gEShN z`s2!NgXf8L(IuSt3sr7y0(Y&fI6+}%m_nY%48f6+HHvols_<39GckJbe&B? zyC`6|s<|YQRxY9?8eEItfkrg5o2bUMpy38gUpZ0`5TOt^-D5?ev*CH zy*Lyr!W&|L|2bIMm{?brc;n@waq$c|7<8QPbZA-kQ0Vb?O8M}PZo;#gDSa>veKJ-n zho)+mgP(hwJV?S#4dV-);(eft{)2A#a{H^@02goX~ZmyR%cY|AJ)4Du@ zK_PKPS}`C%uJ=p+osOrN2zOFuCN)r_iRC)Pc=jY$g37W2>%g}F`4<7P;7^ac(HyuXJB&>@pz#mxYLpS~S%bzRknpS8i zWhfKB-SxQS;L5Evr2Jmi!0WH8KbMukp$;q42;%& zUMb_YgV}$*rpSMLmYbj#?$eT1XSU41sP7bkpwOyY=aOQY9x%1_k>3cc`DEw#iuCQf z>$@g#{!7n~`kvp>m0%b%ayMLYgbzwY0^M>OtpOh<1~UfwqBURYe#|@&M6)s96j*f>+%JUC7x=qrW$B*M30xU{@0})ew|-AJFhIO>S*--QM6qaK>dv z?#q`lc5I9HFD~|%{q(#PJsKg9_=E&|c7e#^V!4@)8W;x^c3U729Y=7bA8DScx&0d) z->_V5Y5LQpHX7263(|#z%ZsmH)=e#zd8rrq-mM?IHF}m4n1^?>ebtnRgo=IN)wZ=) ziBCMK;$Td#30RD+_-OvjY|Iw1GQoftBT(BLCPG;^;faI>Ff6r}Lt*{g~ zjlO-}W%ks^BY5Z{`1=3@#rB%vY>@sXUv0xIxbbvaeeZxLidm4}Elp^($p5PjpEWns z(U&m$dP@vx<$*Ag9U-5)>$f^wZxGCv39s*<-%YurUry z&rT=i{&bb5xnlt5MQ@27Vxix15w}V41Wz1x6U_uKTzsZQIM!1>sX&;RmgKYQjhm|s zEj!z!y8H+63~#`K6+&dB?O1>O5C&jOJM0*xk+qGDoEV70$X}qe;Ra8|`Hkj@p^gmC zRoH)d=S~+b{Uo z#FeSRLU-XgOAK!3H=N^g*N=+2Kh{1+Yi*kS{6yxUUvR&H%He)`;=z1bUS-_>`;OL(|pTzoug20U95h)6Svq6Zf*a#b~Rl@MrGMb`mbC=HpvLZYR5K{c`kPO8ZQel+x1+1y@j>1;&Y|Dv#+>I zBQ#YE8>9A=8C=zyYRkUSbNpEFl^n3daKQJ0O1Gl@tc(IXN4{Tv`aslYwQKIQPBB+) z^HhKDEcbyeck4i%flaFCV93o(3ANjPuS64;prF&#qLA;|8N2zX4};qO{ED4R9%F{~<})^PeN;er$wm;&mTfr$^Qz24o!X^H zV8U@t(qu^S!`_X0U1eM}g?Yi#0KL|Zhj%ira1r9pxIceVlg^18p9>`SvVv|sCOTrs zOpbdpKbNH6;f^dSY_#({mAWz z>JuI0VPmVR@R$HpQ>{a=5KgyuX4^BBhhM7!n6BIP$yy2V&h@@hPie_1YMv(a7EW!5 zL=pT@)Ok;;#o{O0XfVWz#yG5uo~4|bCQgSCyK0z>F0{m<8sIv3U+#h@jgk2l=18{K zf1fzI9BpUXaHG#(kzKS&cJc+WTgx!G?@{L?BY;LCZO~r|2 zmdhS@HhZ1-tIX6>+r`2OC+VxW48m3RmB_6x>5ZjNT`hK~SHPZ^dD7TO{~woPUwmrD zi=ff2V5ii>^{3J6(2Mt_c3{(vN~3pCk{aE?MctP+-TYzy77qCa(-^$^r8RUeQt0-d zfLve8C2!%zt3B}`E~C_5Ofn5zKAS^tY8gshkax7`c8dD)<#NmEMofC_2g|5133A{@ ztc}4;+`XytxbSE(RJ&%ozC9K39khje>gZbL{Sb^}3AzD;%hGTjUm^d7plph!UNiLR zqKCd5+-F{-J80d?(kP#UZTqBhf)=DM$rd(O=_={!?B0h8Yss*U(M2=|WLtY#s9w-r zRd0*^UdMDMW*em$!I6`^I-Y0Y!FA_Qu`QP4e}1{TP`>NxIVkCQqAr=aNAjgeWcaP& z$)|+}rs!evrX%UafhY)z=&GpWEGP0#B=I%PYuGxdQ&Ot7kbmhJUm< zUG(+>B$(A&0OD6_#@kEvX2V3xWYi?Jv;{6$*R4lId0q}aQYq3lGPFkL!T+d~V}cji z!sJFv^RFJvo}N+1q-1^-t`Q0iap;?3f&I^xA&uS*p&+(%hO#%Z?s89QEHptS+Q z&6s#fP^C!7&2nOoFnTj`Jan_*TIi=C@-PnJ7@)@`>>S8wQVN4aF&jVZ%qRblKNJ|E z?!JG1({x0x=uFxlpw8jR`e51YWlNDyZ+353>*`1rRCQj+0aYh+dqWV&Xu6-OdLSd0 z<=Rf;d0i@$q|h8A<}ZNqo9b%CqxRHC5s-Fa2RSt-DZuKZ#*IE0?t+b;++C2W_!M2c z@?&~_Odt=!RkLQI{^n}iwzo*)va4n#w>o5m9?!V+(8cuo;!9Fe^X5D`a3C?|PYYG` zilo2@S5sG7{+OHPNVps=9DD9sD-(QzD&T2tr*h|hytZdLB7f^!lc^ci$rpsUZ0cd- zs^KlSOQ^sg9^@xH1{ zw5t(cv1X>cHAC|_={FFAMKDMFBmgh2FjHg0j&E3sUeX+2WdU z|J`@~p=)NNjq^7HE4S-?@{&EkV~1#I5lkn0!`Hlb zd4heDVdSAV;$Q1=^8P?DBV$+F;hN)Ru})QlkR-q8gYZzL+0Qt;neRa@Cn)Wf0$I86 z$jYmoF0FUJ&Y_1#a45}eT|zpjPeXm;M|qN%np#xt*P3slih4KZOf2y8+H>y0!u%`8 z19=9I>|E-f=v{J#%gObbc}sW>D2hJcQ4}XnmnUifn@)_3L^NCC{_lj3ISGlb^&Ah4 z%>Qt=0F&7C>Ev*dO*5VNPd<+RCL;3*d;(ruz4OKLjfYS2P{E?jd!9uw*rga+$ zjF4(jbdk8#{SwuJW}HMh3T_MGjh+w7rT%=BR$YMwZettHw$3zg!$sX2On%g;L%GQd z8pzCeh3MVIH4URK>>n{y{jBzDzwRs9-dv;0>R)$=%qVnyr^MJ}+w)g~EP7 z`IX9kRvxP?+*{z>j-D1GdGbbVzUX*VQ zL~F!(#-{_xhvK6hQcXld8ab<8Liaavms@+m>pRKnJp6C)tKpkiNjGyH#XDS_l(eLe zE+Z^=4#lrJX;Sqj=LTwb`EatHto~4Ry9r!;*|^I=t*@jgU43%z2{Rxd_19Ys^kAm` z{9}Pte2(BEgt{)YGerJkBE9<5n`mv1bY3n+$EzL%E(WH^u6Y@DV>4x>IX zv8*-sE#ls2gXJdJ2J<{mt$q3VH>RubA;DSkzwogwvK2kcsEn5|+LV^c_P2t~=z=AJ z$VW z>*_A&9E2|a;GJxoUVAGF-Lnf^ujyns3~KiFowDzs(6%G=D%ac%CEYHtRxweB$Yb^H)33eyjl~oZfo97+(ljd=C+LNT2Oa%!! zjXechD}|c9QpF=TREpcnG>6LW%l9|iKWwm87W23c8x=5p1jV{Dd?LF{4^)pC6nA14 z6Cf8QLR%dOrdPz1K1J#_w@sN-f=Z_PUe#!E&F}_A{^DC(JIG-=S>eZ?(*2@Db8$DH z4Da=PqRi#VlAGq8b^E`##3gLZw^0>y3j2tY7WcD}mTkr9!1~=P{<@vsbzMFB$?~Kv zE}T!s&ffXr4&c#68`%k851+g?%3PVl)q2Flq%j%4tPr19@~1n#Dgkmuy2O!^AwKPPJ8MM207lJ^42|6PFd(RBV-k(i2&RE;U5$%qB zemxZ=<=3-Cu=B|iep21jJ}?70d3I;_rRhNRMacOsv)z7!Y@XibMVU(?#Ia`FWBg|m z!asVtNn{9d`|g~7!{mPD?5j*-UU{I*qGKU&*BJ*i%{}TlO>I0I;>$WRn9cL2EW<*b zG=sWzoR7xhvm1)sjc|fSjo%zAg~%)f^|Ll7 zjdt?*nBk%RIFxwif2s0s7Ht>CW*6_i1O~Q-jXt-yBRAw=_uQUAyx1K^w9W>)L3A%l z4m1%kdeikDl14sVjpmLbdzPNymPn1+Pf&cTJPpKgbtmZZ)BX%Jj3a7QI ztvA&>2`p8!pQNfLFJP%@nv1fVhp3>pXyjU-=fZ1x9GpF`^PM3&=-y6=`0Vceqg`R{ zh46j(9aY5gsrz1?I6d7gWL`#1CL<sMHg+s zw(Ph~yi1dkF1)G!=*;7t(1b^>=ee?p>0bTlg3<&hG1-$AuLcmKwN$`g5n5D7;-?}= zFT%cKF;y@Ik1CH=&j=FL2JD(WU5XUUX2^#cQJ8mYPrkj;J7>IMC7rd=r}*sEK{ge z=;neDeaD+M_$706XT}dINT+DJ;F>^P-wxjXBI69ntH&P#wIGb=1wEP}zcLE?m}!-r za}>#h&LjtBwSTQT_K;J|B16ilU_Wa($5RE@W$q8EexTW^RbkJ0aL(-*ltg&<4%=AI zs4?Wm|8Xk5G+$Mqv;O`I8|qAYD5qAsXJo!!138lQO8?=a)&$i_urDg*yLccH=MIU& z*HHX8oF?~Qn|!!7E`6_ne zu2hvH7yTJUqe+39j|TO5YT*y_^O;7AjGf9~+f}PHFC-i-E#Q38ua^zB8oO+4xmQ$& zvajuTy_mEcWL0R8yTatxle8}K_33ia$NS)CcM8U@NQ)hr;5%qF3e3G`ry36XUDyeL zfis+oF%DMaRr5(wn9a4{DgOLf^u{ZnsxPUi(fjvJgO=-F-zOe>%!C5AdSvaTUeerh zO$l4(GmFg_m9^@;`be@JfBRsxU$3kSd_YnZohU|Tz0D=aRK|AMtex%`A2%!aft%pHPXhm9Q{ zmsXUwM(Gx=ZZKQsGCUMl0JBtsUAy*GtO1r!Ny*<^sH?a~G&!q$pV#h&9g=2v z%{N3OKyt_sa*}w9*#spm61xiAdcl!fMUJM>YQ{x+6hjIIuLuPpa>dC(FtlN82<{SG zVA1!GIhhOAAo4i2BTC*Ht-fonSkNop)Mpqy&aFt$X;9F%Y}ccw6SYYinGp|%(LYGN z?vpWdFi48`iB))a97S4p$h@BJ6Wet@SGb~1I@zY{f8MSliOe}>=8o`-a~vIZVZR)z z*=c2>F&}+jQ)QXk3klwxqBtRP+7Fc75|FFANkXtaFu6qyst*NNB&bzUAlsYl4v%3@ zI@An|4Y*A1oKZ;9&Vurxd~6)c!%yV}tm`uy44VqAE?V%{ac!w^z(M#w5Inw;AyJ90@q*W|EaYWiPm||W9Ik#7cV81`BCvZN5d1Lvng53 zEkN!k(@&cHR;WSTbm(u4it=DYpA$T&#r(j*W&6^BswaCb0zov%$5VD-v_qR=*Cl*2 z#-_N{qlh`eBxwR`k}@SDIW0W5xP-CQ`wVl|%QCZKZzb#zcB}h;>CvwUuX<7disaQk#7On=D25@~j%*NJ%Ct%6bhfD#(cTSqV1on|7Uc$U4By<-KTz!ywbrJj!!y#y1Go(^aKgX$l1Fjx_WJA3LH&MXhmUYHSN4=cUm=$8%Zi4 z1NJ;A{3W0ED*(B`!UB>8jL-iIR*0M+SW(ZG70B*hABd5WknCD}x#emYlNo1Q^?ha_ zFpd{0DzO}1GXM!s0x2x9xwt|UtAuQ=9 z``2J%=5kE8*?ntK?Ahvfk=VgSkdpYmgyB|8nGe_j^~m!FuM!dxDtb_{P*72y?4d8e zM?wlAN?iYd@c2Ei|MNY}NwRLwo}0sJGztp~0j~-^D#UqfIG!8aFwm{84zf7?75S-) zmXAi=+ee^9P$>$S<|$#o7dZm_fnT+#{XfLeWcdFjhI+qk*RiYmfQ$T1#r3m>ni`~{ zV)TId!I zaGGzAQqj{B(M8il`(h#e8wlykm!Xk|?|pXIF@hahN=n<84dS_b%K)o^8yBzvqsOAY z0A}5{wu71i2^hcB|CytecGg7w)F74^rJ1UD-1x})rmyPu78 zfFF_;P;+;%v^xoO5Q*?~HRFjG?oPMNv-raClPqgGuNO3TrED2d$fL1!R`abbBNnm@gh#&s6&XW+S74?iuN-BE|wcEwhUN;ZvzVy zN<42Qn4e;WU=biblQ9({B_hQ2|Goegqn;SHYNF2YO%{twa9S^;oqNnq@k#8JZKKe# zi4$Qx7@>BH2v_zTj0N875g`ERR0;?vtE$?$nc7J!9Kh4oWx!J-C~h0|_fxyB*S}>y z>+jYA1O@KC?2&8J#0x3crE=LAm6vt@Ajp%fuPxvJ}3)ZZX zO4_pnbJfP(?72f<2_gLOUNca15P~8Udf7feI{1M+HwFv146KEkLu_#|owhnG*Enl| z(KxkzuBh)-V@!25z1-V-Y0p$-_tLuMGZ5k6m9Ro zT`$j|@seNJUO&!uw3>;1p0BzY#D4dKt%C`)z`e)}!d{0T#_k#Y;^ThMI(ap3P~`X? z^GzHIQb3=vB<)c2mNl_ zxRL`FR*S-v;v2QU>A1hl>A{6hc}^{@Av4dYtoc2d#sXm!(-NVbOAL8 zB}zk-8j8E7$e8!J^uW3{>-y7e-V4~w0bT!ff}Z+ z%|BP2X5Ij~Cz~-?J+xe@z|`xyQ8Y;A7f_}^pUY(8lT5bH(uUNja`bns#Tal;YcX>u zi7-bEeN9NtkaeYGnd{$v7di8Om=O)sW?RB8FC|@yCKfi5Vc!^`e zlSBo|ND&pu!+JT48bPgpk|Acy9A;ndZCGBYre|xadbmZ3hAID?;pRQl`dml-WpX3j zTVR1d${3~2W%^1dipNVmWn+^oci)=9y(P+hZ&!@f2$`p6v%ZR7>AVJWD6C**&Fz+M zT?&e>u3W8C(W&uGSj{wMaNEv8&kjVph_cY6NE~&LqCGWpJq}s(TcuP*4(~#JX8E#= zv?kG1?%nWW+h(-O+MbH?KYB9*8%q;Ak^0f1g2%KG0d{1shn7gD$Rd7?_x1&kH77=q z?T`u}7B4gr@2@7lv@h(xUqi|5BVM^UuYXvUu&d;NQWN&e7JI{8$JW2=af(;F+q!}| z*i(WmP%Y%}*#u6(Pwq^5maD)ws-=RJkj$>Jks{q0+7J7)7))xndyFUHPRT@0$+`Ax zLsT6SLqg(eeYYg(fFrP3d5?~N3@%oNbE+`*J2}BshX%^s21NpYvBULul51q^ruK{` zT}N@g78=4Ioy$Y_cW*~Pmaw6s_Y~rt!j^Zu+kw+uS}nWznDLDT4r#6DFN)~%2K9HG z1~BSQ!y^U&+R~^EgqJ?zzSG5(y%YJS-9O?pij136;y7mxxG>eS59fK$Q<4wIiD6&9 zNVO)zqt!N9tUJm+@AmYUW4wAz-A8}iY*W^qiZwVIH8WWL@HC@r|Mzkl+q%xtX5C`C znNr^PH()u}%@5@LBCoKkt}%ZUg=w#8bvqp+ntX6R4Zq` zh;r?XYs?dG;H``AwwUAkHf7A{l1GdxE!I~!_dn%|==+#5CS-192#jcEcQAS@hrNjC z-4c&q`9Sqe@3*H@Z*dn*_9i@5RrRMaq4=@&cZ8HMyV8qc1=vxrQ|znN&ZBvn#%tX) zMyb213D#XT`n#P2uKer=zTUSZ2j6RoTX2}>=1=uvQb-2FGuXH!HGvaTDcopX))b%o zv2PCxY_hi0XMrvX-fX0C~aC%wYd;QeUo$KZ}_e_ zRH&JOvMxg=(Q%b>xq@`U%ECacp&l$F4cbBgUfJ=|E3ZniQ=iG&ih%iBBHu(mZ{r=0 zx4@n9vonZr2q_BY9xv)gbxRJb#o5&hNDc(ZD;OSmco*7_XC-371U5?jR!#-zDr(1a zD>4mpd1NbnjXa#;3N*~)sKMC|J*S>+cX-{p=2pY&%yC(s^x> zI?g2uZ&bU@Hx4nxei7&h6S!_nW`OK=Lvl=!8Rbref?6DN8GzqVwv?(Ro>`d4_}I9yy;eoAr(FGZTGPX{#qnP#Z4?c+s|wHx@uf9q*pK7L7pn4<*NWCB1B% zhm0^VA|bKJr2Eyjg}ZYEgJ8mJQuSdSSlnyc$?$Ck4#u(hYNjB!fBFtKCa;6b#Yvcl99pCFf ztyEg}(oaIFbj-UBN_`(?7i;C|FE5QoEsgiAgrrDPleJTe1k@MQRjyFn!rrh_H(@+G z9O-{NQt`3JF-uz@zVt#^U=Jn7b}6iEQegdN2Q_m>38}Clb#%CId>Pw6J@5P^a?izT z2K41E!yuGiHDKUXS#9sFTZt55*Ev76E2AMTNyU2Rc)7C(P4Oi4!KWb z`6~;bv2snS^(olFeuNcGqwjPJQ&nMc=n^+frX`uca#&e-pzWY#+WC)r+lTn#Gs0r) zHH8%&fwI)zr={WAZG>9xkK;~IlbckuR>d-;iarK*l7^p`qj8myEZS+hOhpN0k$s4p z%-Rfs@(haD`)D1_}u2f%&c_#dH ze~qYcYE5JMCf=+)jLX8 zn-vIC-&XdvJCJc40hmWUp!{Ss!7^eRn$_0vPV;?RphtcQDTEE6*rocQXC!W#~_zz^=) zomn4lAgHQw3*j8vh>yKLzL7A!LGIQ@Bf9Ax7HqlruK&g2;dpXy61gD+J|-nH#M@i$ zS{LiRtecp~JxZIa{bH{-qPT0wzSSmCgj2uG{nl8tu#C3C3rwYqJDv4`Pb3Eydk1`#Ql#G)%71lmY@;#K5PGZOzM4FyS)z=^0 zS`;JshF>?W_4}Pf+3niT3#Lrj8BS@~ z)1K_`rK8^WZlh=M#WF18c2bVG=~9$qNx}!^rQ9?jg;G?OvA!?H{4qFKsg?j7bPmV4 z%k*a>g9E`Iq#zmd=QT9;F6mu+eIBOwru}%m*j=G+Sm1pAF>I~3IgcM{EIuIQ_vyvZ zp{3Sae!C&zNg==FXa6=`E27(259TS9Vx&xHpNQXYv?fC!~U!yo#~=K0JpA6hB}2gWT#?aV(d3#>hd zqGy<3zk|KqJNl8oG|H+|KC5G6l^skCapz z)YXM*Qi4DX>ll60fi1S=g0Zo&?`Y?=XC+GesW+eQ^H3lZHIqrw!*r<54H7!1aO!p7i zJc6u#TEjG!cSVn{#mli1fE?yMstF1F)%Lp5gYkELAV5s`GVSF;YIHcmgz7N=lgENM z{`X1(3>$y2l#tKVX5Smawlv?MVMeX}v}!;`N6YFx>wk|2toF~*uJ1G`Kx3^7J8di6 z0K9OQ`a*{!a9ZEonh0g_nW!AKwi)Tx3!fKTv{02tnoBRbKC|mK{M0f6z})ucze);9 zqy~uaw$eIHo~sxb*mx!YA@Cbum9+n}O8!(<8~{!Z#3GTv4GK@S_%=%-7)D+V3YG<& z#{^WD$^(~lEo!wf^Y%AJxqIGX$TyR{K}kz{&EDg9^9 zx^h6U$oh64^;sApU#r4VPUCrTtX#kLkK;`F3c~VnKyudrGTRqX_iRA_eBUmO zg6-O}uHn&{cV1q>>N*nf{<%TCmX@}Fh2l4&*Q8EEuLwX=1Xy<{3@+mMmyKogob)D{ z4Kt4?p+pyPJTW;z2N0R;5-!R|uJeHYlqRN>?$22&2|w06I_BC|)?E-^9{&mJK_GO* zu#t=ufg3a6ZZAqtA+X_FzX;MU3Yr1yPLKE>zyP2v2L|Jy8E3#3HK0&xAk-fI1OEWO zt_LG179qfdT@>&vfCoSW|KI^2kQ_^Kl^hbFswVfB>RmkpgIpGDnbh)2umOY!l?Pa0 z{L2Ex7SOm$`fuZOY=DKWD;5R-79h80z!$Fpg>nKcwBEVEOn{;p0 zSJS6aR?xxHv(f zEWl`(g^#RoFTE?0z66Otpj2I;1}DuC_#QwX+0t#2tFbJh!UBYS0+-DLO@U|DKi;>L}wZC(a(S{sDQp>0mI@gCsXxI`0pSiuE|M-!U-=J zU3c90PvRad87wnvSJ=Hes4gms5@b?Nr~$oG!*&VRKH z#1M+0(0UdQ41ex6@RbIE{}`6SX9kBmrKu{^OwERlxLG*;OAZUm?1az(a+f2$`oEEM zyMf(saDDmqeJ-wuga3I!QYeBC_1OG>Uap_VMB`$zff zu`XaqKwki_BM1GT-aNoftDMDEPai+dARx$y9bX&z)us(xA^!^#@8&6jAAY4d#JDo7 z*bUY<$K0A$xFg3N%w}KlkEX^lb9=L=s<8L^sxch}g?xxc)iZu4NSV;p&`bs)_*|M@ z{^#9}a#GZ<{&GJ0Kb%j7BlsQ$p>Zc2tKxZ}H_9giX0Bd#SISFjaqE}6r^rh-3X({o zGe|7JCiLpfV3{Cn8Q?2pQX|1{-^#ZG;P$W){f>E)>Xm}Zz0+8r#WGtUj<_CD^hzxJ zy3m#5rSK;p5KU-}VT)qp2UrMuD-g2z%LTNnB?i~3KWq;QzQtHN%?f>(w@0bsS{ zPjZZU);#*TM{s7qfYJfldlL+vkYrR(2DkwdeL5yiN}dUnFR!FY$H6i?zR;m;qmo-! z3qmF4N>b^x9YGPDR^ zTE77}TiQ}uir0b+Td*)1=5;9tB7E)szKX0GV<2ae1SJPr_9#Lf3O$Z|l+`zSC$m5x z3IUlJ1MBig>y~Whq>jP(Ks?g6VNRz~mbSI7ca{c8hBtvuU|a|uTq5P$flsCU&4~4N zmy6{Gu+@51QF-|_f+Aod0c|fU(~_xI>&td7(J!Va<)zIkt~)W#_gr6Hy-50il>Fx% z`uh3=YOG_+V3v>CumN}T*iLHEqk4YpS1Ag>G_{4F>g#h=iOQO{s4lL;fIf)Z*GaHJ zC4TALU=mZKcaHU4t&YtX#6X}2JpA!=ZFUr|hiJ z!>@b(J8#0*-(Z;mj{Wt6*&d#be-~`fOBOBch92sdIY#du%&uQ8y%bh5STh&q=}}#M zf|30g3H%S0KJJmus|lOc031`z)N{ZSESDkp2DsVZIMx&V+xs&%b1jq-n0e|C|Mqxl zg&&mWC=HlQ)ia#3p@REBLf~@w1|f>91_uYP3;Ww&wtPbKL1Hc+X8@nq3olxK(Yw^$$V zIc%@xyqh-VHNWeo(PH&z#d@ ze}pdbp#WxV+Gu*|M_REwaEjCS2W~`1bR47q6v=Wl2t*qG0FSSkOm; zSUzA&vKBxhaktX0YoWqDSO2$h}1TOh$| zZyt!V!rk|suGo*5-I7PO}4%;))5_@p&6HUeO5|%SU8n9$c5B2 zSln@7%R9cprvbQ3R~=!k1Oz+rC%%3PRDX6gAjJD$8r^|iJ;C_F-xv!6g60h&3tVvB znc*KdKU;X%9!`W{^z#r$fg&LmXm?i*!`)aFfKp;*J=)WOdSg6Aw@&f-iSg;C z{*l0zG;a_+fmkWbNojC=Ksy{gZ#EnxlX%JugV3%|yBhyo*gU#$Qh+(5N_A^T^My>$ zwYsFI`$tT=zE8#*;hVx!$V%@Yc1-v9jky)kGwg?P@r#dhh#+GJRmOI8OC^e_bNYED z^#Zk*b=FUd&jikSP8~Zc(K~GBu_pUj$w@o05^J7si@uoElvT>oRatTNv#zZ+d2&}$ zlhM+ccf8kFL>Vznzk1_nIBO@|^&vU`q_p3CmWIvlE!)@r-`NhLd@{y$OLgdl>_b|g zeo5efb3sc|m|mm9UzZQ62pQc5Q)Dj@xJu9eGZMYpC@O_^t)jg3UN zIbUeGeOfZrkNeD66jB|Os@M8<-)#Yx^2@*(tscKjEdx?MYPhMF6A=#LE=FU0vUKRdwAs>j=pUezv z$?8CAx-Z=oKkVs;mPw*{@fh@`%>w}1OVjoLZOZLZod+mm!E!GQGUAxGxerb`ECntoLFNaY!zz-6B2p9^`mh|rT1iclN+@f}$IJVSFjOdouW@A5OWi_NEG+mG%rEjpLLpgbaKwaU9ibi}!p zf=_jhm(<-h{MfplLO~!r7bQ7q9f`q6ntt7{?ja|HUpZ+_GXv~D^Rq}E7z%KD&)L8i z-xtvpC1yRyjn7O&yB#Z_eWvIPp&};c5z)W2V2;UnB#LAbygDCck(!1aGX^ z*$vgTwbbFfokZ&N*N;kCfBENk?Bmfht+>^A^|cRL)!45V8IjOFcs81;@PA%3)Cw z@~w=nK`r|#(OJrIarUtu4g;zS-}(z`wv#XqQ8qc>Cy-i0m&g50H{}TQC zZDXvZQ(r^f1Yn4+Iy&z;^cRJdmb7_>-9M9A#}*h8I$}J=Ty_)lIu=NVAE-%mSS}(iryvp)RPkX?`s)j#Ev+{AyskslXGrU5eqmTT#gOOOocm+zETU7 zn-!!ET*Xj9$EU>-r$;KbmQ~MzQ!y<+1n-p)mqf=7s3c1V^0WJ;ccFh9lNKLHb{-yU zIpkMaik1r$L{+SGXmL~*AMH)r7G1vWFp^eQ|y#F+7Y08S17!Mp=VA z^JE>-sG`B&zl<>}!^JZS=if$^OYKLySib7yCsnUtSB`qb(2`c)`s)-N_=M|9W<2J) zq|H@G&70G@>NS48usHE=riOl1SVrzh_VO~FRWl~(VK3%o;s~#%+BjK(2>S)j zc~K~AZV2p7NUr3%R#&}^pc}Gt_D##<^c14ya{TuD*#RuD%(6g76@5m?@Z8>@9z(#Y z6nUTF>|=;PV;KuG&79Vr^ylT}KsATG*%n2(l}WCs6-vXvxwawmOxu00QrjKHV&Kh) z3YbK!YnJbR^j3?x--gWk9)<_#*@$rP-?$ZIea|OW_~s2|RZlLm$lrj{0F_qBNsq;(LE4n|_q@=BDYRbz&Js^_gzuD!2 z10hJVrRGK5SBbZ58;y?pD4rCk#|I`Mxq)v>Q0tLX+Oy==W_0pl8;@-H2j?YeM9!dF ztni6Pn0d484)N|8vKe+>>&br^4ONLfw7;pm0?|LPbftdi8>8w;>#A9D$)`iBa>@R(Xw>#KjSV5gT3Dw^l+BX(#l~5UQMUe${ADh)0Ze*ZXf3h OiIpC!$rZ^w3-~{P2{ +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "test_utils.h" + +#include "pilz_industrial_motion_planner/command_list_manager.h" +#include "pilz_industrial_motion_planner/tip_frame_getter.h" + +const std::string ROBOT_DESCRIPTION_STR{ "robot_description" }; +const std::string EMPTY_VALUE{ "" }; + +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); + +using testutils::hasStrictlyIncreasingTime; +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner_testutils; + +static std::string createManipulatorJointName(const size_t& joint_number) +{ + return std::string("prbt_joint_") + std::to_string(joint_number + 1); +} + +static std::string createGripperJointName(const size_t& joint_number) +{ + switch (joint_number) + { + case 0: + return "prbt_gripper_finger_left_joint"; + default: + break; + } + throw std::runtime_error("Could not create gripper joint name"); +} + +class IntegrationTestCommandListManager : public testing::Test +{ +protected: + void SetUp() override; + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(ROBOT_DESCRIPTION_STR).getModel() }; + std::shared_ptr manager_; + planning_scene::PlanningScenePtr scene_; + planning_pipeline::PlanningPipelinePtr pipeline_; + + std::unique_ptr data_loader_; +}; + +void IntegrationTestCommandListManager::SetUp() +{ + // get necessary parameters + if (!robot_model_) + { + FAIL() << "Robot model could not be loaded."; + } + + std::string test_data_file_name; + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name)); + + // load the test data provider + data_loader_.reset( + new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name, robot_model_ }); + ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; + + // Define and set the current scene and manager test object + manager_ = std::make_shared(ph_, robot_model_); + scene_ = std::make_shared(robot_model_); + pipeline_ = std::make_shared(robot_model_, ph_); +} + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + * + */ +TEST_F(IntegrationTestCommandListManager, TestExceptionErrorCodeMapping) +{ + std::shared_ptr nbr_ex{ new NegativeBlendRadiusException("") }; + EXPECT_EQ(nbr_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + + std::shared_ptr lbrnz_ex{ new LastBlendRadiusNotZeroException("") }; + EXPECT_EQ(lbrnz_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + + std::shared_ptr sss_ex{ new StartStateSetException("") }; + EXPECT_EQ(sss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + + std::shared_ptr obr_ex{ new OverlappingBlendRadiiException("") }; + EXPECT_EQ(obr_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + + std::shared_ptr pp_ex{ new PlanningPipelineException("") }; + EXPECT_EQ(pp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); +} + +/** + * @brief Tests the concatenation of three motion commands. + * + * Test Sequence: + * 1. Generate request with three trajectories and zero blend radius. + * 2. Generate request with first trajectory and zero blend radius. + * 3. Generate request with second trajectory and zero blend radius. + * 4. Generate request with third trajectory and zero blend radius. + * + * Expected Results: + * 1. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive + * 2. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive + * 3. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive + * 4. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive + * resulting duration in step1 is approximately step2 + step3 + step4 + */ +TEST_F(IntegrationTestCommandListManager, concatThreeSegments) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + ASSERT_GE(seq.size(), 3u); + seq.erase(3, seq.size()); + seq.setAllBlendRadiiToZero(); + + RobotTrajCont res123_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res123_vec.size(), 1u); + EXPECT_GT(res123_vec.front()->getWayPointCount(), 0u); + EXPECT_TRUE(hasStrictlyIncreasingTime(res123_vec.front())) << "Time steps not strictly positively increasing"; + + ROS_INFO("step 2: only first segment"); + moveit_msgs::MotionSequenceRequest req_1; + req_1.items.resize(1); + req_1.items.at(0).req = seq.getCmd(0).toRequest(); + req_1.items.at(0).blend_radius = 0.; + RobotTrajCont res1_vec{ manager_->solve(scene_, pipeline_, req_1) }; + EXPECT_EQ(res1_vec.size(), 1u); + EXPECT_GT(res1_vec.front()->getWayPointCount(), 0u); + EXPECT_EQ(res1_vec.front()->getFirstWayPoint().getVariableCount(), + req_1.items.at(0).req.start_state.joint_state.name.size()); + + ROS_INFO("step 3: only second segment"); + moveit_msgs::MotionSequenceRequest req_2; + req_2.items.resize(1); + req_2.items.at(0).req = seq.getCmd(1).toRequest(); + req_2.items.at(0).blend_radius = 0.; + RobotTrajCont res2_vec{ manager_->solve(scene_, pipeline_, req_2) }; + EXPECT_EQ(res2_vec.size(), 1u); + EXPECT_GT(res2_vec.front()->getWayPointCount(), 0u); + EXPECT_EQ(res2_vec.front()->getFirstWayPoint().getVariableCount(), + req_2.items.at(0).req.start_state.joint_state.name.size()); + + ROS_INFO("step 4: only third segment"); + moveit_msgs::MotionSequenceRequest req_3; + req_3.items.resize(1); + req_3.items.at(0).req = seq.getCmd(2).toRequest(); + req_3.items.at(0).blend_radius = 0.; + RobotTrajCont res3_vec{ manager_->solve(scene_, pipeline_, req_3) }; + EXPECT_EQ(res3_vec.size(), 1u); + EXPECT_GT(res3_vec.front()->getWayPointCount(), 0u); + EXPECT_EQ(res3_vec.front()->getFirstWayPoint().getVariableCount(), + req_3.items.at(0).req.start_state.joint_state.name.size()); + + // durations for the different segments + auto t1_2_3 = res123_vec.front()->getWayPointDurationFromStart(res123_vec.front()->getWayPointCount() - 1); + auto t1 = res1_vec.front()->getWayPointDurationFromStart(res123_vec.front()->getWayPointCount() - 1); + auto t2 = res2_vec.front()->getWayPointDurationFromStart(res123_vec.front()->getWayPointCount() - 1); + auto t3 = res3_vec.front()->getWayPointDurationFromStart(res123_vec.front()->getWayPointCount() - 1); + ROS_DEBUG_STREAM("total time: " << t1_2_3 << " t1:" << t1 << " t2:" << t2 << " t3:" << t3); + EXPECT_LT(fabs((t1_2_3 - t1 - t2 - t3)), 0.4); +} + +/** + * @brief Tests if times are strictly increasing with selective blending + * + * Test Sequence: + * 1. Generate request with three trajectories where only the first has a + * blend radius. + * 1. Generate request with three trajectories where only the second has a + * blend radius. + * + * Expected Results: + * 1. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly increasing in time + * 2. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly increasing in time + */ +TEST_F(IntegrationTestCommandListManager, concatSegmentsSelectiveBlending) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + ASSERT_GE(seq.size(), 3u); + seq.erase(3, seq.size()); + seq.setAllBlendRadiiToZero(); + seq.setBlendRadius(0, 0.1); + RobotTrajCont res1{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res1.size(), 1u); + EXPECT_GT(res1.front()->getWayPointCount(), 0u); + EXPECT_TRUE(hasStrictlyIncreasingTime(res1.front())) << "Time steps not strictly positively increasing"; + + seq.setAllBlendRadiiToZero(); + seq.setBlendRadius(1, 0.1); + RobotTrajCont res2{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res2.size(), 1u); + EXPECT_GT(res2.front()->getWayPointCount(), 0u); + EXPECT_TRUE(hasStrictlyIncreasingTime(res2.front())) << "Time steps not strictly positively increasing"; +} + +/** + * @brief Tests the concatenation of two ptp commands + * + * Test Sequence: + * 1. Generate request with two PTP trajectories and zero blend radius. + * + * Expected Results: + * 1. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive. + */ +TEST_F(IntegrationTestCommandListManager, concatTwoPtpSegments) +{ + Sequence seq{ data_loader_->getSequence("PtpPtpSequence") }; + ASSERT_GE(seq.size(), 2u); + seq.setAllBlendRadiiToZero(); + + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_vec.size(), 1u); + EXPECT_GT(res_vec.front()->getWayPointCount(), 0u); + EXPECT_TRUE(hasStrictlyIncreasingTime(res_vec.front())); +} + +/** + * @brief Tests the concatenation of ptp and a lin command + * + * Test Sequence: + * 1. Generate request with PTP and LIN trajectory and zero blend radius. + * + * Expected Results: + * 1. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive. + */ +TEST_F(IntegrationTestCommandListManager, concatPtpAndLinSegments) +{ + Sequence seq{ data_loader_->getSequence("PtpLinSequence") }; + ASSERT_GE(seq.size(), 2u); + seq.setAllBlendRadiiToZero(); + + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_vec.size(), 1u); + EXPECT_GT(res_vec.front()->getWayPointCount(), 0u); + EXPECT_TRUE(hasStrictlyIncreasingTime(res_vec.front())); +} + +/** + * @brief Tests the concatenation of a lin and a ptp command + * + * Test Sequence: + * 1. Generate request with LIN and PTP trajectory and zero blend radius. + * + * Expected Results: + * 1. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive. + */ +TEST_F(IntegrationTestCommandListManager, concatLinAndPtpSegments) +{ + Sequence seq{ data_loader_->getSequence("LinPtpSequence") }; + ASSERT_GE(seq.size(), 2u); + seq.setAllBlendRadiiToZero(); + + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_vec.size(), 1u); + EXPECT_GT(res_vec.front()->getWayPointCount(), 0u); + EXPECT_TRUE(hasStrictlyIncreasingTime(res_vec.front())); +} + +/** + * @brief Tests the blending of motion commands + * + * - Test Sequence: + * 1. Generate request with two trajectories and request blending. + * + * - Expected Results: + * 1. blending is successful, result trajectory is not empty + */ +TEST_F(IntegrationTestCommandListManager, blendTwoSegments) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + ASSERT_EQ(seq.size(), 2u); + moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, req) }; + EXPECT_EQ(res_vec.size(), 1u); + EXPECT_GT(res_vec.front()->getWayPointCount(), 0u); + EXPECT_EQ(res_vec.front()->getFirstWayPoint().getVariableCount(), + req.items.at(0).req.start_state.joint_state.name.size()); + + ros::NodeHandle nh; + ros::Publisher pub = nh.advertise("my_planned_path", 1); + ros::Duration duration(1.0); // wait to notify possible subscribers + duration.sleep(); + + moveit_msgs::DisplayTrajectory display_trajectory; + moveit_msgs::RobotTrajectory rob_traj_msg; + res_vec.front()->getRobotTrajectoryMsg(rob_traj_msg); + display_trajectory.trajectory.push_back(rob_traj_msg); + pub.publish(display_trajectory); +} + +// ------------------ +// FAILURE cases +// ------------------ + +/** + * @brief Tests sending an empty blending request. + * + * Test Sequence: + * 1. Generate empty request and request blending. + * + * Expected Results: + * 1. blending is successful, result trajectory container is empty + */ +TEST_F(IntegrationTestCommandListManager, emptyList) +{ + moveit_msgs::MotionSequenceRequest empty_list; + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, empty_list) }; + EXPECT_TRUE(res_vec.empty()); +} + +/** + * @brief Checks that exception is thrown if first goal is not reachable. + * + * Test Sequence: + * 1. Generate request with first goal out of workspace. + * + * Expected Results: + * 1. Exception is thrown. + */ +TEST_F(IntegrationTestCommandListManager, firstGoalNotReachable) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + ASSERT_GE(seq.size(), 2u); + LinCart& lin{ seq.getCmd(0) }; + lin.getGoalConfiguration().getPose().position.y = 2700; + EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), PlanningPipelineException); +} + +/** + * @brief Checks that exception is thrown if second goal has a start state. + * + * Test Sequence: + * 1. Generate request, second goal has an invalid start state set. + * + * Expected Results: + * 1. Exception is thrown. + */ +TEST_F(IntegrationTestCommandListManager, startStateNotFirstGoal) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + ASSERT_GE(seq.size(), 2u); + const LinCart& lin{ seq.getCmd(0) }; + moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + req.items.at(1).req.start_state = lin.getGoalConfiguration().toMoveitMsgsRobotState(); + EXPECT_THROW(manager_->solve(scene_, pipeline_, req), StartStateSetException); +} + +/** + * @brief Checks that exception is thrown in case of blending request with + * negative blend_radius. + * + * Test Sequence: + * 1. Generate request, first goal has negative blend_radius. + * + * Expected Results: + * 1. Exception is thrown. + */ +TEST_F(IntegrationTestCommandListManager, blendingRadiusNegative) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + ASSERT_GE(seq.size(), 2u); + seq.setBlendRadius(0, -0.3); + EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), NegativeBlendRadiusException); +} + +/** + * @brief Checks that exception is thrown if last blend radius is not zero. + * + * + * Test Sequence: + * 1. Generate request, second goal has non-zero blend_radius. + * + * Expected Results: + * 1. Exception is thrown. + */ +TEST_F(IntegrationTestCommandListManager, lastBlendingRadiusNonZero) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + ASSERT_EQ(seq.size(), 2u); + seq.setBlendRadius(1, 0.03); + EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), LastBlendRadiusNotZeroException); +} + +/** + * @brief Checks that exception is thrown if blend radius is greater than the + * segment. + * + * Test Sequence: + * 1. Generate request with huge blending radius, so that trajectories are + * completely inside + * + * Expected Results: + * 2. Exception is thrown. + */ +TEST_F(IntegrationTestCommandListManager, blendRadiusGreaterThanSegment) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + ASSERT_GE(seq.size(), 2u); + seq.setBlendRadius(0, 42.0); + EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), BlendingFailedException); +} + +/** + * @brief Checks that exception is thrown if two consecutive blend radii + * overlap. + * + * Test Sequence: + * 1. Generate request with three trajectories + * 2. Increase second blend radius, so that the radii overlap + * + * Expected Results: + * 1. blending succeeds, result trajectory is not empty + * 2. Exception is thrown. + */ +TEST_F(IntegrationTestCommandListManager, blendingRadiusOverlapping) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + ASSERT_GE(seq.size(), 3u); + seq.erase(3, seq.size()); + + RobotTrajCont res_valid_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_valid_vec.size(), 1u); + EXPECT_GT(res_valid_vec.front()->getWayPointCount(), 0u); + + // calculate distance from first to second goal + const PtpJointCart& ptp{ seq.getCmd(0) }; + const CircInterimCart& circ{ seq.getCmd(1) }; + Eigen::Isometry3d p1, p2; + tf2::fromMsg(ptp.getGoalConfiguration().getPose(), p1); + tf2::fromMsg(circ.getGoalConfiguration().getPose(), p2); + auto distance = (p2.translation() - p1.translation()).norm(); + + seq.setBlendRadius(1, (distance - seq.getBlendRadius(0) + 0.01)); // overlapping radii + EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), OverlappingBlendRadiiException); +} + +/** + * @brief Tests if the planned execution time scales correctly with the number + * of repetitions. + * + * Test Sequence: + * 1. Generate trajectory and save calculated execution time. + * 2. Generate request with repeated path along the points from Test Step 1 + * (repeated two times). + * + * Expected Results: + * 1. Blending succeeds, result trajectory is not empty. + * 2. Blending succeeds, planned execution time should be approx N times + * the single planned execution time. + */ +TEST_F(IntegrationTestCommandListManager, TestExecutionTime) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + ASSERT_GE(seq.size(), 2u); + RobotTrajCont res_single_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_single_vec.size(), 1u); + EXPECT_GT(res_single_vec.front()->getWayPointCount(), 0u); + + moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + // Create large request by making copies of the original sequence commands + // and adding them to the end of the original sequence. + const size_t n{ req.items.size() }; + for (size_t i = 0; i < n; ++i) + { + moveit_msgs::MotionSequenceItem item{ req.items.at(i) }; + if (i == 0) + { + // Remove start state because only the first request + // is allowed to have a start state in a sequence. + item.req.start_state = moveit_msgs::RobotState(); + } + req.items.push_back(item); + } + + RobotTrajCont res_n_vec{ manager_->solve(scene_, pipeline_, req) }; + EXPECT_EQ(res_n_vec.size(), 1u); + EXPECT_GT(res_n_vec.front()->getWayPointCount(), 0u); + + const double trajectory_time_1 = + res_single_vec.front()->getWayPointDurationFromStart(res_single_vec.front()->getWayPointCount() - 1); + const double trajectory_time_n = + res_n_vec.front()->getWayPointDurationFromStart(res_n_vec.front()->getWayPointCount() - 1); + double multiplicator = req.items.size() / n; + EXPECT_LE(trajectory_time_n, trajectory_time_1 * multiplicator); + EXPECT_GE(trajectory_time_n, trajectory_time_1 * multiplicator * 0.5); +} + +/** + * @brief Tests if it possible to send requests which contain more than + * one group. + * + * Please note: This test is still quite trivial. It does not check the + * "correctness" of a calculated trajectory. It only checks that for each + * group and group change there exists a calculated trajectory. + * + */ +TEST_F(IntegrationTestCommandListManager, TestDifferentGroups) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequenceWithGripper") }; + ASSERT_GE(seq.size(), 1u); + // Count the number of group changes in the given sequence + unsigned int num_groups{ 1 }; + std::string last_group_name{ seq.getCmd(0).getPlanningGroup() }; + for (size_t i = 1; i < seq.size(); ++i) + { + if (seq.getCmd(i).getPlanningGroup() != last_group_name) + { + ++num_groups; + last_group_name = seq.getCmd(i).getPlanningGroup(); + } + } + + RobotTrajCont res_single_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_single_vec.size(), num_groups); + + for (const auto& res : res_single_vec) + { + EXPECT_GT(res->getWayPointCount(), 0u); + } +} + +/** + * @brief Checks that no exception is thrown if two gripper commands are + * blended. + * + */ +TEST_F(IntegrationTestCommandListManager, TestGripperCmdBlending) +{ + Sequence seq{ data_loader_->getSequence("PureGripperSequence") }; + ASSERT_GE(seq.size(), 2u); + ASSERT_TRUE(seq.cmdIsOfType(0)); + ASSERT_TRUE(seq.cmdIsOfType(1)); + + // Ensure that blending is requested for gripper commands. + seq.setBlendRadius(0, 1.0); + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_vec.size(), 1u); +} + +/** + * @brief Tests the execution of a sequence in which each group states a start + * state only consisting of joints of the corresonding group. + * + * Test Sequence: + * 1. Create sequence request for which each start state only consists of + * joints of the corresonding group + * + * Expected Results: + * 1. Trajectory generation is successful, result trajectory is not empty. + * + * + */ +TEST_F(IntegrationTestCommandListManager, TestGroupSpecificStartState) +{ + using std::placeholders::_1; + + Sequence seq{ data_loader_->getSequence("ComplexSequenceWithGripper") }; + ASSERT_GE(seq.size(), 4u); + seq.erase(4, seq.size()); + + Gripper& gripper{ seq.getCmd(0) }; + gripper.getStartConfiguration().setCreateJointNameFunc(std::bind(&createGripperJointName, _1)); + // By deleting the model we guarantee that the start state only consists + // of joints of the gripper group without the manipulator + gripper.getStartConfiguration().clearModel(); + + PtpJointCart& ptp{ seq.getCmd(1) }; + ptp.getStartConfiguration().setCreateJointNameFunc(std::bind(&createManipulatorJointName, _1)); + // By deleting the model we guarantee that the start state only consists + // of joints of the manipulator group without the gripper + ptp.getStartConfiguration().clearModel(); + + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_GE(res_vec.size(), 1u); + EXPECT_GT(res_vec.front()->getWayPointCount(), 0u); +} + +/** + * @brief Checks that exception is thrown if Tip-Frame is requested for + * a group without a solver. + */ +TEST_F(IntegrationTestCommandListManager, TestGetSolverTipFrameForSolverlessGroup) +{ + Gripper gripper_cmd{ data_loader_->getGripper("open_gripper") }; + EXPECT_THROW(getSolverTipFrame(robot_model_->getJointModelGroup(gripper_cmd.getPlanningGroup())), NoSolverException); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_command_list_manager"); + testing::InitGoogleTest(&argc, argv); + + ros::NodeHandle nh; + + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.test new file mode 100644 index 0000000000..1f4eb9489d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.test @@ -0,0 +1,68 @@ + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp new file mode 100644 index 0000000000..c724595bd8 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp @@ -0,0 +1,501 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "test_utils.h" + +const double EPSILON = 1.0e-6; +const std::string PLAN_SERVICE_NAME = "/plan_kinematic_path"; + +// Parameters from parameter server +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance"); +const std::string ORIENTATION_NORM_TOLERANCE("orientation_norm_tolerance"); +const std::string PARAM_TARGET_LINK_NAME("target_link"); +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); + +using namespace pilz_industrial_motion_planner_testutils; + +/** + * PLEASE NOTE: + * More detailed lin tests are done via unit tests. With the help of the + * integration tests, it is only checked that a linear command actually + * performs a linear command. + */ +class IntegrationTestCommandPlanning : public ::testing::Test +{ +protected: + void SetUp() override; + +protected: + ros::NodeHandle ph_{ "~" }; + + robot_model::RobotModelPtr robot_model_; + + double pose_norm_tolerance_, orientation_norm_tolerance_; + std::string planning_group_, target_link_, test_data_file_name_; + + std::unique_ptr test_data_; + + unsigned int num_joints_{ 0 }; +}; + +void IntegrationTestCommandPlanning::SetUp() +{ + // create robot model + robot_model_loader::RobotModelLoader model_loader; + robot_model_ = model_loader.getModel(); + + // get the parameters + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(POSE_TRANSFORM_MATRIX_NORM_TOLERANCE, pose_norm_tolerance_)); + ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + ASSERT_TRUE(ph_.getParam(ORIENTATION_NORM_TOLERANCE, orientation_norm_tolerance_)); + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + + ASSERT_TRUE(ros::service::waitForService(PLAN_SERVICE_NAME, ros::Duration(testutils::DEFAULT_SERVICE_TIMEOUT))); + + // load the test data provider + test_data_.reset( + new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_, robot_model_ }); + ASSERT_NE(nullptr, test_data_) << "Failed to load test data by provider."; + + num_joints_ = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().size(); +} + +/** + * @brief Tests if ptp motions with start & goal state given as + * joint configuration are executed correctly. + * + * Test Sequence: + * 1. Generate request with joint goal and start state call planning service. + * + * Expected Results: + * 1. Last point of the resulting trajectory is at the goal + */ +TEST_F(IntegrationTestCommandPlanning, PtpJoint) +{ + ros::NodeHandle node_handle("~"); + auto ptp{ test_data_->getPtpJoint("Ptp1") }; + + moveit_msgs::GetMotionPlan srv; + moveit_msgs::MotionPlanRequest req = ptp.toRequest(); + srv.request.motion_plan_request = req; + + ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); + + ASSERT_TRUE(client.call(srv)); + const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + + // Check the result + ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory; + + EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames"; + EXPECT_GT(trajectory.points.size(), 0u) << "There are no points in the trajectory"; + + // Check that every point has position, velocity, acceleration + for (const auto& point : trajectory.points) + { + EXPECT_EQ(point.positions.size(), num_joints_); + EXPECT_EQ(point.velocities.size(), num_joints_); + EXPECT_EQ(point.accelerations.size(), num_joints_); + } + + for (size_t i = 0; i < num_joints_; ++i) + { + EXPECT_NEAR(trajectory.points.back().positions.at(i), req.goal_constraints.back().joint_constraints.at(i).position, + 10e-10); + EXPECT_NEAR(trajectory.points.back().velocities.at(i), 0, 10e-10); + // EXPECT_NEAR(trajectory.points.back().accelerations.at(i), 0, 10e-10); // + // TODO what is expected + } +} + +/** + * @brief Tests if ptp motions with start state given as joint configuration + * and goal state given as cartesian configuration are executed correctly. + * + * Test Sequence: + * 1. Generate request with pose goal and start state call planning service. + * + * Expected Results: + * 1. Last point of the resulting trajectory is at the goal + */ +TEST_F(IntegrationTestCommandPlanning, PtpJointCart) +{ + ros::NodeHandle node_handle("~"); + + PtpJointCart ptp{ test_data_->getPtpJointCart("Ptp1") }; + ptp.getGoalConfiguration().setPoseTolerance(0.01); + ptp.getGoalConfiguration().setAngleTolerance(0.01); + + moveit_msgs::GetMotionPlan srv; + srv.request.motion_plan_request = ptp.toRequest(); + + ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); + + ASSERT_TRUE(client.call(srv)); + const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + + // Make sure the planning succeeded + ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + + // Check the result + trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory; + + EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames"; + EXPECT_GT(trajectory.points.size(), 0u) << "There are no points in the trajectory"; + + // Check that every point has position, velocity, acceleration + for (const auto& point : trajectory.points) + { + EXPECT_EQ(point.positions.size(), num_joints_); + EXPECT_EQ(point.velocities.size(), num_joints_); + EXPECT_EQ(point.accelerations.size(), num_joints_); + } + + robot_state::RobotState rstate(robot_model_); + rstate.setToDefaultValues(); + rstate.setJointGroupPositions(planning_group_, response.trajectory.joint_trajectory.points.back().positions); + Eigen::Isometry3d tf = rstate.getFrameTransform(target_link_); + + const geometry_msgs::Pose& expected_pose{ ptp.getGoalConfiguration().getPose() }; + + EXPECT_NEAR(tf(0, 3), expected_pose.position.x, EPSILON); + EXPECT_NEAR(tf(1, 3), expected_pose.position.y, EPSILON); + EXPECT_NEAR(tf(2, 3), expected_pose.position.z, EPSILON); + + Eigen::Isometry3d exp_iso3d_pose; + tf2::convert(expected_pose, exp_iso3d_pose); + + EXPECT_TRUE(Eigen::Quaterniond(tf.rotation()).isApprox(Eigen::Quaterniond(exp_iso3d_pose.rotation()), EPSILON)); +} + +/** + * @brief Tests if linear motions with start and goal state given + * as joint configuration are executed correctly. + * + * Test Sequence: + * 1. Generate request and make service request. + * 2. Check if target position correct. + * 3. Check if trajectory is linear. + * + * Expected Results: + * 1. Planning request is successful. + * 2. Goal position correponds with the given goal position. + * 3. Trajectory is a straight line. + */ +TEST_F(IntegrationTestCommandPlanning, LinJoint) +{ + planning_interface::MotionPlanRequest req{ test_data_->getLinJoint("lin2").toRequest() }; + + std::cout << "++++++++++" << std::endl; + std::cout << "+ Step 1 +" << std::endl; + std::cout << "++++++++++" << std::endl; + + moveit_msgs::GetMotionPlan srv; + srv.request.motion_plan_request = req; + + ros::NodeHandle node_handle("~"); + ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); + + ASSERT_TRUE(client.call(srv)); + const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + + ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + + std::cout << "++++++++++" << std::endl; + std::cout << "+ Step 2 +" << std::endl; + std::cout << "++++++++++" << std::endl; + + ASSERT_TRUE(testutils::isGoalReached(robot_model_, response.trajectory.joint_trajectory, req, pose_norm_tolerance_, + orientation_norm_tolerance_)) + << "Goal not reached."; + + std::cout << "++++++++++" << std::endl; + std::cout << "+ Step 3 +" << std::endl; + std::cout << "++++++++++" << std::endl; + + ASSERT_TRUE(testutils::checkCartesianLinearity(robot_model_, response.trajectory.joint_trajectory, req, + pose_norm_tolerance_, orientation_norm_tolerance_)) + << "Trajectory violates cartesian linearity."; +} + +/** + * @brief Tests if linear motions with start state given as joint + * configuration and goal state given as cartesian configuration + * are executed correctly. + * + * Test Sequence: + * 1. Generate request and make service request. + * 2. Check if target position correct. + * 3. Check if trajectory is linear. + * + * Expected Results: + * 1. Planning request is successful. + * 2. Goal position correponds with the given goal position. + * 3. Trajectory is a straight line. + */ +TEST_F(IntegrationTestCommandPlanning, LinJointCart) +{ + ros::NodeHandle node_handle("~"); + planning_interface::MotionPlanRequest req{ test_data_->getLinJointCart("lin2").toRequest() }; + + std::cout << "++++++++++" << std::endl; + std::cout << "+ Step 1 +" << std::endl; + std::cout << "++++++++++" << std::endl; + + moveit_msgs::GetMotionPlan srv; + srv.request.motion_plan_request = req; + + ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); + + ASSERT_TRUE(client.call(srv)); + const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + + ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + + std::cout << "++++++++++" << std::endl; + std::cout << "+ Step 2 +" << std::endl; + std::cout << "++++++++++" << std::endl; + + ASSERT_TRUE(testutils::isGoalReached(robot_model_, response.trajectory.joint_trajectory, req, pose_norm_tolerance_, + orientation_norm_tolerance_)) + << "Goal not reached."; + + std::cout << "++++++++++" << std::endl; + std::cout << "+ Step 3 +" << std::endl; + std::cout << "++++++++++" << std::endl; + + ASSERT_TRUE(testutils::checkCartesianLinearity(robot_model_, response.trajectory.joint_trajectory, req, + pose_norm_tolerance_, orientation_norm_tolerance_)) + << "Trajectory violates cartesian linearity."; +} + +/** + * @brief Tests if circular motions with start & goal state given as joint + * configuration and center point given as cartesian configuration + * are executed correctly. + * + * Test Sequence: + * 1. Generate request with JOINT goal and start state call planning service. + * + * Expected Results: + * 1. Last point of the resulting trajectory is at the goal + * 2. Waypoints are on the desired circle + */ +TEST_F(IntegrationTestCommandPlanning, CircJointCenterCart) +{ + ros::NodeHandle node_handle("~"); + + CircJointCenterCart circ{ test_data_->getCircJointCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req{ circ.toRequest() }; + + moveit_msgs::GetMotionPlan srv; + srv.request.motion_plan_request = req; + + ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); + + ASSERT_TRUE(client.call(srv)); + const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + + // Check the result + ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory; + + EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames"; + EXPECT_GT(trajectory.points.size(), 0u) << "There are no points in the trajectory"; + + // Check that every point has position, velocity, acceleration + for (const auto& point : trajectory.points) + { + EXPECT_EQ(point.positions.size(), num_joints_); + EXPECT_EQ(point.velocities.size(), num_joints_); + EXPECT_EQ(point.accelerations.size(), num_joints_); + } + + // check goal is reached + ASSERT_TRUE(testutils::isGoalReached(robot_model_, response.trajectory.joint_trajectory, req, pose_norm_tolerance_, + orientation_norm_tolerance_)) + << "Goal not reached."; + + // check all waypoints are on the circle and SLERP + robot_state::RobotState waypoint_state(robot_model_); + Eigen::Isometry3d waypoint_pose; + double x_dist, y_dist, z_dist; + + const geometry_msgs::Pose& aux_pose{ circ.getAuxiliaryConfiguration().getConfiguration().getPose() }; + + CircCenterCart circ_cart{ test_data_->getCircCartCenterCart("circ1_center_2") }; + const geometry_msgs::Pose& start_pose{ circ_cart.getStartConfiguration().getPose() }; + const geometry_msgs::Pose& goal_pose{ circ_cart.getGoalConfiguration().getPose() }; + + x_dist = aux_pose.position.x - start_pose.position.x; + y_dist = aux_pose.position.y - start_pose.position.y; + z_dist = aux_pose.position.z - start_pose.position.z; + double expected_radius = sqrt(x_dist * x_dist + y_dist * y_dist + z_dist * z_dist); + for (const auto& waypoint : trajectory.points) + { + waypoint_state.setToDefaultValues(); + waypoint_state.setJointGroupPositions(planning_group_, waypoint.positions); + waypoint_pose = waypoint_state.getFrameTransform(target_link_); + + // Calculate (and check) distance of current trajectory waypoint from circ + // center + x_dist = aux_pose.position.x - waypoint_pose(0, 3); + y_dist = aux_pose.position.y - waypoint_pose(1, 3); + z_dist = aux_pose.position.z - waypoint_pose(2, 3); + double actual_radius = sqrt(x_dist * x_dist + y_dist * y_dist + z_dist * z_dist); + EXPECT_NEAR(actual_radius, expected_radius, pose_norm_tolerance_) << "Trajectory way point is not on the circle."; + + // Check orientation + Eigen::Isometry3d start_pose_iso3d, goal_pose_iso3d; + tf2::convert(start_pose, start_pose_iso3d); + tf2::convert(goal_pose, goal_pose_iso3d); + EXPECT_TRUE(testutils::checkSLERP(start_pose_iso3d, goal_pose_iso3d, waypoint_pose, orientation_norm_tolerance_)); + } +} + +/** + * @brief Tests if linear motions with start state given as cartesian + * configuration and goal state given as cartesian configuration + * are executed correctly. + * + * - Test Sequence: + * 1. Generate request with POSE goal and start state call planning service. + * + * - Expected Results: + * 1. Last point of the resulting trajectory is at the goal + * 2. Waypoints are on the desired circle + */ +TEST_F(IntegrationTestCommandPlanning, CircCartCenterCart) +{ + ros::NodeHandle node_handle("~"); + + CircCenterCart circ{ test_data_->getCircCartCenterCart("circ1_center_2") }; + moveit_msgs::MotionPlanRequest req{ circ.toRequest() }; + moveit_msgs::GetMotionPlan srv; + srv.request.motion_plan_request = req; + + ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); + + ASSERT_TRUE(client.call(srv)); + const moveit_msgs::MotionPlanResponse& response = srv.response.motion_plan_response; + + // Check the result + ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory; + + EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames"; + EXPECT_GT(trajectory.points.size(), 0u) << "There are no points in the trajectory"; + + // Check that every point has position, velocity, acceleration + for (const auto& point : trajectory.points) + { + EXPECT_EQ(point.positions.size(), num_joints_); + EXPECT_EQ(point.velocities.size(), num_joints_); + EXPECT_EQ(point.accelerations.size(), num_joints_); + } + + // check goal is reached + ASSERT_TRUE(testutils::isGoalReached(robot_model_, response.trajectory.joint_trajectory, req, pose_norm_tolerance_, + orientation_norm_tolerance_)) + << "Goal not reached."; + + // check all waypoints are on the cricle and SLERP + robot_state::RobotState waypoint_state(robot_model_); + Eigen::Isometry3d waypoint_pose; + double x_dist, y_dist, z_dist; + + const geometry_msgs::Pose& start_pose{ circ.getStartConfiguration().getPose() }; + const geometry_msgs::Pose& aux_pose{ circ.getAuxiliaryConfiguration().getConfiguration().getPose() }; + const geometry_msgs::Pose& goal_pose{ circ.getGoalConfiguration().getPose() }; + + x_dist = aux_pose.position.x - start_pose.position.x; + y_dist = aux_pose.position.y - start_pose.position.y; + z_dist = aux_pose.position.z - start_pose.position.z; + double expected_radius = sqrt(x_dist * x_dist + y_dist * y_dist + z_dist * z_dist); + for (const auto& waypoint : trajectory.points) + { + waypoint_state.setToDefaultValues(); + waypoint_state.setJointGroupPositions(planning_group_, waypoint.positions); + waypoint_pose = waypoint_state.getFrameTransform(target_link_); + + // Calculate (and check) distance of current trajectory waypoint from circ + // center + x_dist = aux_pose.position.x - waypoint_pose(0, 3); + y_dist = aux_pose.position.y - waypoint_pose(1, 3); + z_dist = aux_pose.position.z - waypoint_pose(2, 3); + double actual_radius = sqrt(x_dist * x_dist + y_dist * y_dist + z_dist * z_dist); + EXPECT_NEAR(actual_radius, expected_radius, pose_norm_tolerance_) << "Trajectory way point is not on the circle."; + + // Check orientation + Eigen::Isometry3d start_pose_iso3d, goal_pose_iso3d; + tf2::convert(start_pose, start_pose_iso3d); + tf2::convert(goal_pose, goal_pose_iso3d); + EXPECT_TRUE(testutils::checkSLERP(start_pose_iso3d, goal_pose_iso3d, waypoint_pose, orientation_norm_tolerance_)); + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_command_planning"); + ros::NodeHandle nh; // For output via ROS_ERROR etc during test + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test new file mode 100644 index 0000000000..ba73e8b56b --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test @@ -0,0 +1,62 @@ + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test new file mode 100644 index 0000000000..d360fbcafa --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test @@ -0,0 +1,63 @@ + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test new file mode 100644 index 0000000000..64fb904bde --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test @@ -0,0 +1,61 @@ + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.cpp new file mode 100644 index 0000000000..0cd4ac3bcc --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.cpp @@ -0,0 +1,116 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 + +#include + +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/tip_frame_getter.h" + +static const std::string ROBOT_DESCRIPTION_PARAM{ "robot_description" }; + +namespace pilz_industrial_motion_planner +{ +class GetSolverTipFrameIntegrationTest : public testing::Test +{ +protected: + void SetUp() override; + +protected: + robot_model::RobotModelConstPtr robot_model_{ + robot_model_loader::RobotModelLoader(ROBOT_DESCRIPTION_PARAM).getModel() + }; +}; + +void GetSolverTipFrameIntegrationTest::SetUp() +{ + if (!robot_model_) + { + FAIL() << "Robot model could not be loaded."; + } +} + +/** + * @brief Check if hasSolver() can be called successfully for the manipulator + * group. + */ +TEST_F(GetSolverTipFrameIntegrationTest, TestHasSolverManipulator) +{ + EXPECT_TRUE(hasSolver(robot_model_->getJointModelGroup("manipulator"))) << "hasSolver returns false for manipulator"; +} + +/** + * @brief Check if hasSolver() can be called successfully for the gripper group. + */ +TEST_F(GetSolverTipFrameIntegrationTest, TestHasSolverGripperGroup) +{ + EXPECT_FALSE(hasSolver(robot_model_->getJointModelGroup("gripper"))) << "hasSolver returns true for gripper"; +} + +/** + * @brief Check if getSolverTipFrame() can be called successfully for the + * manipulator group. + */ +TEST_F(GetSolverTipFrameIntegrationTest, TestGetTipSolverFrameManipulator) +{ + getSolverTipFrame(robot_model_->getJointModelGroup("manipulator")); +} + +/** + * @brief Check if getSolverTipFrame() fails for gripper group. + */ +TEST_F(GetSolverTipFrameIntegrationTest, TestGetTipSolverFrameGripper) +{ + EXPECT_THROW(getSolverTipFrame(robot_model_->getJointModelGroup("gripper")), NoSolverException); +} + +} // namespace pilz_industrial_motion_planner + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_get_solver_tip_frame"); + testing::InitGoogleTest(&argc, argv); + + ros::NodeHandle nh; + + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.test new file mode 100644 index 0000000000..6f5425d85a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.test @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp new file mode 100644 index 0000000000..43f0415771 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp @@ -0,0 +1,132 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 + +#include +#include + +#include "pilz_industrial_motion_planner/plan_components_builder.h" +#include "pilz_industrial_motion_planner/trajectory_blender_transition_window.h" + +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string ROBOT_DESCRIPTION_STR{ "robot_description" }; +const std::string EMPTY_VALUE{ "" }; + +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner; + +class IntegrationTestPlanComponentBuilder : public testing::Test +{ +protected: + void SetUp() override; + +protected: + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(ROBOT_DESCRIPTION_STR).getModel() }; + + std::string planning_group_; +}; + +void IntegrationTestPlanComponentBuilder::SetUp() +{ + if (!robot_model_) + { + FAIL() << "Robot model could not be loaded."; + } + + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); +} + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + * + */ +TEST_F(IntegrationTestPlanComponentBuilder, TestExceptionErrorCodeMapping) +{ + std::shared_ptr nbs_ex{ new NoBlenderSetException("") }; + EXPECT_EQ(nbs_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + + std::shared_ptr ntffse_ex{ new NoTipFrameFunctionSetException("") }; + EXPECT_EQ(ntffse_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + + std::shared_ptr nrms_ex{ new NoRobotModelSetException("") }; + EXPECT_EQ(nrms_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + + std::shared_ptr bf_ex{ new BlendingFailedException("") }; + EXPECT_EQ(bf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); +} + +/** + * @brief Checks that exception is thrown if no robot model is set. + * + */ +TEST_F(IntegrationTestPlanComponentBuilder, TestModelSet) +{ + robot_trajectory::RobotTrajectoryPtr traj{ new robot_trajectory::RobotTrajectory(robot_model_, planning_group_) }; + PlanComponentsBuilder builder; + + EXPECT_THROW(builder.append(traj, 1.0), NoRobotModelSetException); +} + +/** + * @brief Checks that exception is thrown if no blender is set. + * + */ +TEST_F(IntegrationTestPlanComponentBuilder, TestNoBlenderSet) +{ + robot_trajectory::RobotTrajectoryPtr traj{ new robot_trajectory::RobotTrajectory(robot_model_, planning_group_) }; + PlanComponentsBuilder builder; + builder.setModel(robot_model_); + + builder.append(traj, 0.0); + + EXPECT_THROW(builder.append(traj, 1.0), NoBlenderSetException); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_plan_components_builder"); + testing::InitGoogleTest(&argc, argv); + + ros::NodeHandle nh; + + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.test new file mode 100644 index 0000000000..447c618525 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.test @@ -0,0 +1,65 @@ + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp new file mode 100644 index 0000000000..a5883e71cc --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp @@ -0,0 +1,628 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "moveit_msgs/MoveGroupSequenceAction.h" + +static constexpr int WAIT_FOR_ACTION_SERVER_TIME_OUT{ 10 }; // seconds + +const std::string SEQUENCE_ACTION_NAME("/sequence_move_group"); + +// Parameters from parameter server +const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance"); + +// events for callback tests +const std::string GOAL_SUCCEEDED_EVENT = "GOAL_SUCCEEDED"; +const std::string SERVER_IDLE_EVENT = "SERVER_IDLE"; + +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); +const std::string GROUP_NAME("group_name"); + +using namespace pilz_industrial_motion_planner_testutils; + +class IntegrationTestSequenceAction : public testing::Test, public testing::AsyncTest +{ +protected: + void SetUp() override; + +public: + MOCK_METHOD0(active_callback, void()); + MOCK_METHOD1(feedback_callback, void(const moveit_msgs::MoveGroupSequenceFeedbackConstPtr& feedback)); + MOCK_METHOD2(done_callback, void(const actionlib::SimpleClientGoalState& state, + const moveit_msgs::MoveGroupSequenceResultConstPtr& result)); + +protected: + ros::NodeHandle ph_{ "~" }; + actionlib::SimpleActionClient ac_{ ph_, SEQUENCE_ACTION_NAME, true }; + std::shared_ptr move_group_; + + robot_model_loader::RobotModelLoader model_loader_; + robot_model::RobotModelPtr robot_model_; + double joint_position_tolerance_; + + std::string test_data_file_name_; + std::string group_name_; + TestdataLoaderUPtr data_loader_; + + //! The configuration at which the robot stays at the beginning of each test. + JointConfiguration start_config; +}; + +void IntegrationTestSequenceAction::SetUp() +{ + // get necessary parameters + ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_)); + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + ph_.param(GROUP_NAME, group_name_, "manipulator"); + + robot_model_ = model_loader_.getModel(); + + data_loader_.reset(new XmlTestdataLoader(test_data_file_name_, robot_model_)); + ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; + + // wait for action server + ASSERT_TRUE(ac_.waitForServer(ros::Duration(WAIT_FOR_ACTION_SERVER_TIME_OUT))) << "Action server is not active."; + + // move to default position + start_config = data_loader_->getJoints("ZeroPose", group_name_); + robot_state::RobotState robot_state{ start_config.toRobotState() }; + + move_group_ = std::make_shared(start_config.getGroupName()); + move_group_->setPlannerId("PTP"); + move_group_->setGoalTolerance(joint_position_tolerance_); + move_group_->setJointValueTarget(robot_state); + move_group_->move(); + + ASSERT_TRUE( + isAtExpectedPosition(robot_state, *(move_group_->getCurrentState()), joint_position_tolerance_, group_name_)); +} + +/** + * @brief Test behavior of sequence action server when empty sequence is sent. + * + * Test Sequence: + * 1. Send empty sequence. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Empty sequence is sent to the action server. + * 2. Error code of the blend result is SUCCESS. + */ +TEST_F(IntegrationTestSequenceAction, TestSendingOfEmptySequence) +{ + moveit_msgs::MoveGroupSequenceGoal seq_goal; + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Execution of sequence failed."; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests that invalid (differing) group names are detected. + * + * Test Sequence: + * 1. Invalidate first request (change group_name) and send goal for planning + * and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is failure. + */ +TEST_F(IntegrationTestSequenceAction, TestDifferingGroupNames) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + MotionCmd& cmd{ seq.getCmd(1) }; + cmd.setPlanningGroup("WrongGroupName"); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME) << "Incorrect error code."; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests that negative blend radii are detected. + * + * Test Sequence: + * 1. Send goal for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is not success and the planned trajectory is + * empty. + */ +TEST_F(IntegrationTestSequenceAction, TestNegativeBlendRadius) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setBlendRadius(0, -1.0); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN) + << "Incorrect error code."; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests that overlapping blend radii are detected. + * + * Test Sequence: + * 1. Generate request with overlapping blend radii. + * 2. Send goal for planning and execution. + * 3. Evaluate the result. + * + * Expected Results: + * 1. - + * 2. Goal is sent to the action server. + * 3. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceAction, TestOverlappingBlendRadii) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setBlendRadius(0, 10 * seq.getBlendRadius(0)); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN) << "Incorrect error code"; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests that too large blend radii are detected. + * + * Test Sequence: + * 1. Generate request with too large blend radii. + * 2. Send goal for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. - + * 2. Goal is sent to the action server. + * 3. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceAction, TestTooLargeBlendRadii) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.erase(2, seq.size()); + seq.setBlendRadius(0, 10 * seq.getBlendRadius(seq.size() - 2)); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::FAILURE) << "Incorrect error code"; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests what happens if sequence contains not executable (invalid) + * command. + * + * Test Sequence: + * 1. Create sequence containing at least one invalid command. + * 2. Send goal for planning and execution. + * 3. Evaluate the result. + * + * Expected Results: + * 1. - + * 2. Goal is sent to the action server. + * 3. Error code indicates an error + planned trajectory is empty. + */ +TEST_F(IntegrationTestSequenceAction, TestInvalidCmd) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + // Erase certain command to invalid command following the command in sequence. + seq.erase(3, 4); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_NE(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Incorrect error code."; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests that incorrect link_names are detected. + * + * Test Sequence: + * 1. Create sequence and send it via ActionClient. + * 2. Wait for successful completion of command. + * + * Expected Results: + * 1. - + * 2. ActionClient reports successful completion of command. + */ +TEST_F(IntegrationTestSequenceAction, TestInvalidLinkName) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setAllBlendRadiiToZero(); + + // Invalidate link name + CircInterimCart& circ{ seq.getCmd(1) }; + circ.getGoalConfiguration().setLinkName("InvalidLinkName"); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_NE(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Incorrect error code."; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +//******************************************************* +//*** matcher for callback functions of action server *** +//******************************************************* +MATCHER_P(FeedbackStateEq, state, "") +{ + return arg->state == state; +} +MATCHER(IsResultSuccess, "") +{ + return arg->response.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS; +} +MATCHER(IsResultNotEmpty, "") +{ + return !arg->response.planned_trajectories.empty(); +} + +/** + * @brief Tests that action server callbacks are called correctly. + * + * Test Sequence: + * 1. Send goal for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is success. Active-, feedback- and + * done-callbacks are called. + */ +TEST_F(IntegrationTestSequenceAction, TestActionServerCallbacks) +{ + using ::testing::_; + using ::testing::AllOf; + using ::testing::AtLeast; + using ::testing::InSequence; + + namespace ph = std::placeholders; + + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + // We do not need the complete sequence, just two commands. + seq.erase(2, seq.size()); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + // set expectations (no guarantee, that done callback is called before idle + // feedback) + EXPECT_CALL(*this, active_callback()).Times(1).RetiresOnSaturation(); + + EXPECT_CALL(*this, done_callback(_, AllOf(IsResultSuccess(), IsResultNotEmpty()))) + .Times(1) + .WillOnce(ACTION_OPEN_BARRIER_VOID(GOAL_SUCCEEDED_EVENT)) + .RetiresOnSaturation(); + + // the feedbacks are expected in order + { + InSequence dummy; + + EXPECT_CALL(*this, feedback_callback(FeedbackStateEq("PLANNING"))).Times(AtLeast(1)); + EXPECT_CALL(*this, feedback_callback(FeedbackStateEq("MONITOR"))).Times(AtLeast(1)); + EXPECT_CALL(*this, feedback_callback(FeedbackStateEq("IDLE"))) + .Times(AtLeast(1)) + .WillOnce(ACTION_OPEN_BARRIER_VOID(SERVER_IDLE_EVENT)) + .RetiresOnSaturation(); + } + + // send goal using mocked callback methods + ac_.sendGoal(seq_goal, std::bind(&IntegrationTestSequenceAction::done_callback, this, ph::_1, ph::_2), + std::bind(&IntegrationTestSequenceAction::active_callback, this), + std::bind(&IntegrationTestSequenceAction::feedback_callback, this, ph::_1)); + + // wait for the ecpected events + BARRIER({ GOAL_SUCCEEDED_EVENT, SERVER_IDLE_EVENT }); +} + +/** + * @brief Tests the "only planning" flag. + * + * Test Sequence: + * 1. Send goal for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is success. + */ +TEST_F(IntegrationTestSequenceAction, TestPlanOnlyFlag) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + // We do not need the complete sequence, just two commands. + seq.erase(2, seq.size()); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.planning_options.plan_only = true; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Sequence execution failed."; + EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; + + ASSERT_TRUE(isAtExpectedPosition(*(move_group_->getCurrentState()), start_config.toRobotState(), + joint_position_tolerance_, group_name_)) + << "Robot did move although \"PlanOnly\" flag set."; +} + +/** + * @brief Tests that robot state in planning_scene_diff is + * ignored (Mainly for full coverage) in case "plan only" flag is set. + * + * Test Sequence: + * 1. Send goal with "empty" planning scene for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is success. + */ +TEST_F(IntegrationTestSequenceAction, TestIgnoreRobotStateForPlanOnly) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + // We do not need the complete sequence, just two commands. + seq.erase(2, seq.size()); + + // create request + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.planning_options.plan_only = true; + seq_goal.request = seq.toRequest(); + + seq_goal.planning_options.planning_scene_diff.robot_state.is_diff = true; + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Execution of sequence failed."; + EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; + + ASSERT_TRUE(isAtExpectedPosition(*(move_group_->getCurrentState()), start_config.toRobotState(), + joint_position_tolerance_, group_name_)) + << "Robot did move although \"PlanOnly\" flag set."; +} + +/** + * @brief Tests that negative blend radii are detected + * (Mainly for full coverage) in case "plan only" flag is set. + * + * Test Sequence: + * 1. Send goal for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is not success and the planned trajectory is + * empty. + */ +TEST_F(IntegrationTestSequenceAction, TestNegativeBlendRadiusForPlanOnly) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setBlendRadius(0, -1.0); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + seq_goal.planning_options.plan_only = true; + + ac_.sendGoalAndWait(seq_goal); + + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN) + << "Incorrect error code."; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests that robot state in planning_scene_diff is + * ignored (Mainly for full coverage). + * + * Test Sequence: + * 1. Send goal with "empty" planning scene for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is success. + */ +TEST_F(IntegrationTestSequenceAction, TestIgnoreRobotState) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + // We do not need the complete sequence, just two commands. + seq.erase(2, seq.size()); + + // create request + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + seq_goal.planning_options.planning_scene_diff.robot_state.is_diff = true; + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Execution of sequence failed."; + EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; +} + +/** + * @brief Tests the execution of a sequence with more than two commands. + * + * Test Sequence: + * 1. Create large sequence requests and sent it to action server. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Command succeeds, result trajectory is not empty. + */ +TEST_F(IntegrationTestSequenceAction, TestLargeRequest) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + // Create large request by making copies of the original sequence commands + // and adding them to the end of the original sequence. + size_t n{ req.items.size() }; + for (size_t i = 0; i < n; ++i) + { + moveit_msgs::MotionSequenceItem item{ req.items.at(i) }; + if (i == 0) + { + // Remove start state because only the first request + // is allowed to have a start state in a sequence. + item.req.start_state = moveit_msgs::RobotState(); + } + req.items.push_back(item); + } + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = req; + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Incorrect error code."; + EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; +} + +/** + * @brief Tests the execution of a sequence command (without blending) + * consisting of most of the possible command type combination. + * + * Test Sequence: + * 1. Create sequence goal and send it via ActionClient. + * 2. Wait for successful completion of command. + * + * Expected Results: + * 1. - + * 2. ActionClient reports successful completion of command. + */ +TEST_F(IntegrationTestSequenceAction, TestComplexSequenceWithoutBlending) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + + seq.setAllBlendRadiiToZero(); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; +} + +/** + * @brief Tests the execution of a sequence command (with blending) + * consisting of most of the possible command type combination. + * + * Test Sequence: + * 1. Create sequence goal and send it via ActionClient. + * 2. Wait for successful completion of command. + * + * Expected Results: + * 1. - + * 2. ActionClient reports successful completion of command. + */ +TEST_F(IntegrationTestSequenceAction, TestComplexSequenceWithBlending) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_sequence_action_capability"); + ros::NodeHandle nh; + + ros::AsyncSpinner spinner{ 1 }; + spinner.start(); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test new file mode 100644 index 0000000000..df51f007b1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test @@ -0,0 +1,59 @@ + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test new file mode 100644 index 0000000000..bc6afb0a8e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test @@ -0,0 +1,65 @@ + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test new file mode 100644 index 0000000000..c45f964dbc --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test @@ -0,0 +1,60 @@ + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp new file mode 100644 index 0000000000..bf9198d52a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp @@ -0,0 +1,180 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "moveit_msgs/MoveGroupSequenceAction.h" + +static constexpr double WAIT_FOR_RESULT_TIME_OUT{ 5. }; // seconds +static constexpr double TIME_BEFORE_CANCEL_GOAL{ 1.0 }; // seconds +static constexpr double WAIT_FOR_ACTION_SERVER_TIME_OUT{ 10. }; // seconds + +const std::string SEQUENCE_ACTION_NAME("/sequence_move_group"); + +// Parameters from parameter server +const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance"); + +// events for callback tests +const std::string GOAL_SUCCEEDED_EVENT = "GOAL_SUCCEEDED"; +const std::string SERVER_IDLE_EVENT = "SERVER_IDLE"; + +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); +const std::string GROUP_NAME("group_name"); + +using namespace pilz_industrial_motion_planner_testutils; + +class IntegrationTestSequenceAction : public testing::Test, public testing::AsyncTest +{ +protected: + void SetUp() override; + +public: + MOCK_METHOD0(active_callback, void()); + MOCK_METHOD1(feedback_callback, void(const moveit_msgs::MoveGroupSequenceFeedbackConstPtr& feedback)); + MOCK_METHOD2(done_callback, void(const actionlib::SimpleClientGoalState& state, + const moveit_msgs::MoveGroupSequenceResultConstPtr& result)); + +protected: + ros::NodeHandle ph_{ "~" }; + actionlib::SimpleActionClient ac_{ ph_, SEQUENCE_ACTION_NAME, true }; + std::shared_ptr move_group_; + + robot_model_loader::RobotModelLoader model_loader_; + robot_model::RobotModelPtr robot_model_; + double joint_position_tolerance_; + + std::string test_data_file_name_; + std::string group_name_; + TestdataLoaderUPtr data_loader_; + + //! The configuration at which the robot stays at the beginning of each test. + JointConfiguration start_config; +}; + +void IntegrationTestSequenceAction::SetUp() +{ + // get necessary parameters + ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_)); + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + ph_.param(GROUP_NAME, group_name_, "manipulator"); + + robot_model_ = model_loader_.getModel(); + + data_loader_.reset(new XmlTestdataLoader(test_data_file_name_, robot_model_)); + ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; + + // wait for action server + ASSERT_TRUE(ac_.waitForServer(ros::Duration(WAIT_FOR_ACTION_SERVER_TIME_OUT))) << "Action server is not active."; + + // move to default position + start_config = data_loader_->getJoints("ZeroPose", group_name_); + robot_state::RobotState robot_state{ start_config.toRobotState() }; + + move_group_ = std::make_shared(start_config.getGroupName()); + move_group_->setPlannerId("PTP"); + move_group_->setGoalTolerance(joint_position_tolerance_); + move_group_->setJointValueTarget(robot_state); + move_group_->setMaxVelocityScalingFactor(1.0); + move_group_->setMaxAccelerationScalingFactor(1.0); + move_group_->move(); + + ASSERT_TRUE(isAtExpectedPosition(robot_state, *(move_group_->getCurrentState()), joint_position_tolerance_)); +} + +/** + * @brief Tests that goal can be cancelled. + * + * Test Sequence: + * 1. Send goal for planning and execution. + * 2. Cancel goal before it finishes. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Goal is cancelled. Execution stops. + */ +TEST_F(IntegrationTestSequenceAction, TestCancellingOfGoal) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoal(seq_goal); + // wait for 1 second + ros::Duration(TIME_BEFORE_CANCEL_GOAL).sleep(); + + ac_.cancelGoal(); + ac_.waitForResult(ros::Duration(WAIT_FOR_RESULT_TIME_OUT)); + + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::PREEMPTED) + << "Error code should be preempted."; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_sequence_action_preemption"); + ros::NodeHandle nh; + + ros::AsyncSpinner spinner{ 1 }; + spinner.start(); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test new file mode 100644 index 0000000000..614b5ffe1d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test @@ -0,0 +1,59 @@ + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp new file mode 100644 index 0000000000..02f4bf5aa9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp @@ -0,0 +1,426 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "moveit_msgs/GetMotionSequence.h" +#include "moveit_msgs/MotionSequenceRequest.h" +#include "pilz_industrial_motion_planner/capability_names.h" + +// Parameters from parameter server +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); + +using namespace pilz_industrial_motion_planner_testutils; + +static std::string createJointName(const size_t& joint_number) +{ + return std::string("prbt_joint_") + std::to_string(joint_number + 1); +} + +class IntegrationTestSequenceService : public ::testing::Test +{ +protected: + void SetUp() override; + +protected: + ros::NodeHandle ph_{ "~" }; + ros::ServiceClient client_; + robot_model::RobotModelPtr robot_model_; + + std::string test_data_file_name_; + TestdataLoaderUPtr data_loader_; +}; + +void IntegrationTestSequenceService::SetUp() +{ + // get necessary parameters + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + + robot_model_loader::RobotModelLoader model_loader; + robot_model_ = model_loader.getModel(); + + data_loader_.reset(new XmlTestdataLoader(test_data_file_name_, robot_model_)); + ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; + + ASSERT_TRUE(ros::service::waitForService(pilz_industrial_motion_planner::SEQUENCE_SERVICE_NAME, ros::Duration(10))) + << "Service not available."; + ros::NodeHandle nh; // connect to service in global namespace, not in ph_ + client_ = nh.serviceClient(pilz_industrial_motion_planner::SEQUENCE_SERVICE_NAME); +} + +/** + * @brief Test behavior of service when empty sequence is sent. + * + * Test Sequence: + * 1. Generate empty request and call sequence service. + * 2. Evaluate the result. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command is successful, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestSendingOfEmptySequence) +{ + moveit_msgs::MotionSequenceRequest empty_list; + + moveit_msgs::GetMotionSequence srv; + srv.request.request = empty_list; + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Planning failed."; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests that invalid (differing) group names are detected. + * + * Test Sequence: + * 1. Generate request, first request has invalid group_name + Call sequence + * service. + * 2. Invalidate first request (change group_name) and send goal for planning + * and execution. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestDifferingGroupNames) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + MotionCmd& cmd{ seq.getCmd(1) }; + cmd.setPlanningGroup("WrongGroupName"); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME, srv.response.response.error_code.val) + << "Planning should have failed but did not."; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests that negative blend radii are detected. + * + * Test Sequence: + * 1. Generate request with negative blend_radius + Call sequence service. + * 2. Evaluate the result. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestNegativeBlendRadius) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setBlendRadius(0, -1.0); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN, srv.response.response.error_code.val) + << "Planning should have failed but did not."; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests that overlapping blend radii are detected. + * + * Test Sequence: + * 1. Generate request with overlapping blend radii + Call sequence service. + * 2. Evaluate the result. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestOverlappingBlendRadii) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setBlendRadius(0, 10 * seq.getBlendRadius(0)); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN, srv.response.response.error_code.val) + << "Incorrect error code"; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests that too large blend radii are detected. + * + * Test Sequence: + * 1. Generate request with too large blend radii + Call sequence service. + * 2. Evaluate the result. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestTooLargeBlendRadii) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.erase(2, seq.size()); + seq.setBlendRadius(0, 10 * seq.getBlendRadius(seq.size() - 2)); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::FAILURE, srv.response.response.error_code.val) << "Incorrect error code"; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests behavior of service when sequence with invalid second + * start state is sent. + * + * Test Sequence: + * 1. Generate request (second goal has invalid start state) + Call sequence + * service. + * 2. Evaluate the result + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestSecondTrajInvalidStartState) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + moveit_msgs::MotionSequenceRequest req_list{ seq.toRequest() }; + + // Set start state + using std::placeholders::_1; + JointConfiguration config{ "MyGroupName", { -1., 2., -3., 4., -5., 0. }, std::bind(&createJointName, _1) }; + req_list.items[1].req.start_state.joint_state = config.toSensorMsg(); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = req_list; + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE, srv.response.response.error_code.val) + << "Incorrect error code."; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests behavior of service when sequence with invalid first goal + * is sent. + * + * Test Sequence: + * 1. Generate request with first goal out of workspace + Call sequence + * service. + * 2. Evaluate the result + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestFirstGoalNotReachable) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + PtpJointCart& cmd{ seq.getCmd(0) }; + cmd.getGoalConfiguration().getPose().position.y = 27; + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION, srv.response.response.error_code.val) + << "Incorrect error code."; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests that incorrect link_names are detected. + * + * Test Sequence: + * 1. Create sequence and send it. + * 2. Wait for successful completion of command. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestInvalidLinkName) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setAllBlendRadiiToZero(); + + // Invalidate link name + CircInterimCart& circ{ seq.getCmd(1) }; + circ.getGoalConfiguration().setLinkName("InvalidLinkName"); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_NE(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Incorrect error code."; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests the execution of a sequence with more than two commands. + * + * Test Sequence: + * 1. Call service with serveral requests. + * 2. Evaluate the result. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command succeeds, result trajectory is not empty. + */ +TEST_F(IntegrationTestSequenceService, TestLargeRequest) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + // Make copy of sequence commands and add them to the end of sequence. + // Create large request by making copies of the original sequence commands + // and adding them to the end of the original sequence. + size_t n{ req.items.size() }; + for (size_t i = 0; i < n; ++i) + { + moveit_msgs::MotionSequenceItem item{ req.items.at(i) }; + if (i == 0) + { + // Remove start state because only the first request + // is allowed to have a start state in a sequence. + item.req.start_state = moveit_msgs::RobotState(); + } + req.items.push_back(item); + } + + moveit_msgs::GetMotionSequence srv; + srv.request.request = req; + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Incorrect error code."; + EXPECT_EQ(srv.response.response.planned_trajectories.size(), 1u); + EXPECT_GT(srv.response.response.planned_trajectories.front().joint_trajectory.points.size(), 0u) + << "Trajectory should contain points."; +} + +/** + * @brief Tests the execution of a sequence command (without blending) + * consisting of most of the possible command type combination. + * + * Test Sequence: + * 1. Create sequence goal and send it via ActionClient. + * 2. Wait for successful completion of command. + * + * Expected Results: + * 1. - + * 2. ActionClient reports successful completion of command. + */ +TEST_F(IntegrationTestSequenceService, TestComplexSequenceWithoutBlending) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + + seq.setAllBlendRadiiToZero(); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Incorrect error code."; + EXPECT_EQ(srv.response.response.planned_trajectories.size(), 1u); + EXPECT_GT(srv.response.response.planned_trajectories.front().joint_trajectory.points.size(), 0u) + << "Trajectory should contain points."; +} + +/** + * @brief Tests the execution of a sequence command (with blending) + * consisting of most of the possible command type combination. + * + * Test Sequence: + * 1. Create sequence goal and send it via ActionClient. + * 2. Wait for successful completion of command. + * + * Expected Results: + * 1. - + * 2. ActionClient reports successful completion of command. + */ +TEST_F(IntegrationTestSequenceService, TestComplexSequenceWithBlending) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Incorrect error code."; + EXPECT_EQ(srv.response.response.planned_trajectories.size(), 1u); + EXPECT_GT(srv.response.response.planned_trajectories.front().joint_trajectory.points.size(), 0u) + << "Trajectory should contain points."; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_sequence_service_capability"); + ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test new file mode 100644 index 0000000000..9eec6f308b --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test @@ -0,0 +1,57 @@ + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test new file mode 100644 index 0000000000..dbec926599 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test @@ -0,0 +1,59 @@ + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test new file mode 100644 index 0000000000..f214d4baf7 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test @@ -0,0 +1,57 @@ + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.odp b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.odp new file mode 100644 index 0000000000000000000000000000000000000000..1e437d42041f326cc09967f88bd19e2a6a76ac4d GIT binary patch literal 14825 zcmb8W1$Z1uvNhU53oT|@lEsq6%*@QpEQ=W|W@ct)W@fOMEM~TtnVvj*=kx5|nf>35 z?y9V;&WP%a?267fby7wg1QZznfB*o}xrij`iOY(p0RX_S?fn$M!qmdh-o@Hb*V@|L zRA1NL)XI|D(b9m*O4rWRj>^i~(9*z4-@(Gr(w@rN*3j;~{G*M`zZsRXuYKM42mt)r z(BF+Jm^fJIS?Zda+fm#9c}Qh#X&fvgB@7FN3H9CtR#Zel4gdgpKLm6^fV}Tl=p_{Z z0N@uH2?ar5AP^uB7+?rkC{SQXFbFsZa9Aj45C~KdcpMmbR44>&WatlAh%l%qD3B<` za5(f>IQY1DpOFdaF=)Ao(O`(Nk!i8Oi9Tafe@3AuKxZSvBp@KbCnhB!qaY_C_(DNJ zNWn-!%fLcO#7s*;Pft(F$jZ#Z&dNy7!NSPS#)d(`gTo?9!YoR~DND;K!N$(d$tS`j ztj;cJz%6GYLV+pBK*Z0%EW=JJ!pov2#Goh6YAnwwB`PQ(_P!~I$*Idp2r0`*s3@xn zDH%&>*($0VN$5GL>Y6E-sTuf?0Qff+kN=j;ALVj?1bwXNhcurkxc6oYUerjGxT2XmY zVSQe#Usgg$UQ%dDtanASe@R+IPFj3bT0mh=dVN-CYe_<0UQSMad2UgDUP)*wz|po+TO0#k*>P2zSf1F@|m9MnZf4S?`?}iHA^Ec>yxcJ<4s%B z9fRM$_m9jD&n(T2_bp8i&CShC&9BTYt*$KoSXfvXTs#h zL>2j=4-wYN=M?xA31u5J8qt}%F2_3;ui|NY@d$OYhRmpqQ*9^Elb|2Dc`)R zw|>Il2i19K2Yqa1^0U6WkC;ldJa+#w{X%}{zmsvYxD2?rA$B_Ce0a(RvfgE0{v?Yk zKl$utU{D4*KB&Nrc+mu=Io7;gcm~SCOl><%{;@>wG8ccVIKechnEhMGD=FZovyJTz zpM5}lC)U$4%57aV!iBZ2<<2&)U(|UoeG%6Y^_2jeG5-a@Hpxr6Z!PUZOpr<`-sD2@ zGGNUOOsZc&6dzTs6c>4d0h$O^%?%pwZ7zRnzfV0Sk!$#eU!R9@+AX#QekptoiyfHluu=hiOz0#7y@7g+;cJ1rwb^oQdEG3VyW_KPKdz*SR8kZA?@_y2 zXy>ht)6ue_T?_?W!mA`x!K^q!))W}Z+yNGAt#FruMqZ7B){J*6=D@&SdkAV?2~&q} zAwK{xU9m$&hFO=m_!W{2y=ASLt#m!0Zk!V|S4df6c^?*#TwjVo6<(#KyDmvK@_yof zWv$h4E&+yClE7PTp1KDN1N+Y`s_88z^JK^ePPQ^uq-jRlfmf46Om8dfYTRQZQJuf(73>WDES<5I7r{Y zLz8Z=VVQ~#Jav;WrAWPK52yfc5;c?QfvWU%C|3{moE&0M&3xvZ=EhV0==|d7KERmL z574MZtF=2%$tsZzmt^j|SoxIGcKOR4~WhyIWV5 zOcJ~r=*Tu>H0~DegS#ao@L&>Rpzxy0~k$L%m zDxx=)W_kzC(ukgZo}oDTJPOlupNK*Kdbq7S`Eo%Qchwd{Ud^e4#vi{1WX-7ql%~D~ z2G~cl1A+#u!*u~@05AZM|7U@R2RN_Sbm$1$tn#^*2bnIt?g+}(M>#44ND-|_>%0WL9-z@o#K6S8=gHIcx{i2I3vB=YjV;1cKkXs;Z7q|{c70?2}9KZy?^7{MgD-@4- zQkTF&yM>oO-V)CsIKazxQVunBZ5jMW10ANC{+%blEm?Go)-bOTwrKX~ngN7{Df2$eM+o-ImP8F3$k9y#lJd!XfJiv7nx#Q2fU=~Eb-X1RT?jo( zwtLhgOODOEfUF6}k5$hMPY-TVft@HfbZ6RS+i*Z2ga8RhXjZ6iYn*@;8hS5=Rfry7P!;lkSRmr7u`MdgAeGT9T^65d`lXP~#Scw1S)%!j-h%T2Un}F`qyZJM z0CiSL>w*?KOBEm!<>(5SjY*hb(+^n=#GArbMY={jCGA!qEOsFZyMw)W7I*MoYl^sD z36OH>oxmo{sT?3(F|N9dB697RI{eUr$wufCpo(eGAPPYMPB)+haZ%-5uZlFiz-pk= zF^~XmFE3XHA!xCh3Lut zB~9Q-o==eOoP4=f)B)g@0RE{i*$&VyT!PVt8oF9>fT7<<$&IHJ!DuZS9t&a#we+<{ z@GL4X6Ng^F4-NZ0UL3Hxv~r)RlotnN9aYCDVIanAebx|kU;stZBxbnSEc;kZ3mPC^ z?Bfa`fwB5K0F<{B7Z`A25@QKW>M#csKfI#Z6JG=h$w$TX*4n8zRg+`9M}5&(Qb4W@LK7u_hHPgktYk2F}_W!uFF zjt6K^NbyN9&Ci=5N~`lMx8;!z_=#*v#%>SrlNPJe;kJ1XEtdWs{&Qb(Ct37?jYxr_ zoC1Vh>x6O^fq>4fiS`r$sN=Ddr>;z+QSHixx9WhMRou9rl|W9YuMcuv06fz|+9a|_ z7@aYNGXSY4ur*%+iR4b@%zp8s2iD}ss-id15yyxnjw{Hd#JVbs!rK$zWuJUnRnEO) zzEmvT=s=!vFB`1uR#%sMaK8&1tPP2O0-djQ;}~Vs0|rm0le)_llE-Teb>Bo>T?VVG zp#i)Rs0r>I^VhwG=LIMVzGUaBK=T7P$x6r@@tp^}qGxa3UuH1@ve<)Q0h`&8QpWwc zUfV7(C(jkuG@~G-@z+IQO+UO>;&j{$0rWuth-8Tb*KKv_7m0@RrW{LcbQxS6Sy!bM9_qN7dR~M9Ef#{<%s4yn zs)lq2fKJ*0112E*V;Pu7wEmyv-zfxJ-DCiO;u>dra4X9zNns}!@zV{!IZgeJ`Q_A| ze`2%;WS8*)9}_4uKw1dsUs29$1Kc{EZ>HEXrP>|pZ7*KN0;~iJ9rJ!k3OtTslJu$V zqd5nve7Q;)|!2?zOz_AKz2Q-iNfrZoro_$0>*8JikuZ=DmgL)x^10#Xrt z36>$9J-z=e`wym$3f~iKF+d@e5AB7Fma8sRo_h*RKbT9fprESg)4k*+_QG>z8Dl}^ z9qvN&WfVS%@%-SITKe6r1avI^%^=yBKJIrM~T&##KdHfWDH!=7>MmX$|Jbr zG0A5-&FEIABXIxCa>8#m7h)s3VQp>k7*Wlt9&9$fgMuz^qGexJ(q4xO(s?*^I~=;6O~4`j9~rj+Bm*yHyP z`SjsUwUlCbn3PLpwTer6_49hALXR-}xLZM93zIno*?rh|In+X9WJ4p;I9%NQapavk zCga199dH>O7L9Y>%jTY*w9y{vX{WYWvh_lkHm;3JBBzMMpJrqZ@qOudf6i?GXM+2G zD{up zNv$MBO63rJwq1tapK6K$LXx>qsYDy)ep5sGGY3xOkWIXaomUen6nY7LTrG43t^}4$ z8t+tyY&Tz;4vW5UU}OqrwYYdY!kjV|TEeADB$4r=xS2aG=X3!W=n-f(W7;_Vl4Fg# zf=%Cmgj4(cN~1EnMcgrt1-+h?1}@D0NxSexhVrCUyN%>CasLHpQ%Lt)AtlvJhJF6OIQeczIvQ|%oH+7s(CybnP@HInr>X$6xLT>dA=FR zt*d%o1AY#N0Lc;`@qKQd(GN(jv)T=WFMb#EwF8nbDFYt0N(-2|o*Vm5kZ$l|&J9dA zp(|JytYIH#KuJ{jGPLqArCQHi8b;=(fUAzeU&)eu(H(vs^x~=G#MX|kN5uB*)-KV; z&xxY?e{08Ozu$owR>ac;QAs@idUL+!RQU#w5&r;j*x`lL`Ho#s{Q4XJHz?%K0smG0 zf%hYb$F9 z>tC1rFW(aPZ?0!*p=)etN6lwyZ=q{#_dm*icQ=syjk2!3zM;9{yPvFVsr4OfZGRz7 z?>7s`AO3!C4*>qPdzXLq+~4`Y-pb1SJp(i}|1W*Pez*Fc<791XW&DnJ{Xe|-cO(4k ziobpScisPmf7#i)m>b&t>fwpzSj=_V52B(H*JS+(SR>sD2s6?DufujwKo^Z`G=An}_T zRbksy9}mt7OtblIF`(L3@TTev+O3?D1uZ+pYR={pa6+^G`*9(-yqK4>P5ugH=b3BO zxCTIc=u1&`Qkt4&B>b>SK5SDpy(49>wa{~;Kzk|Cx-Pf33ci!skS2T+#+HdZ7-lxT zupggqq{1$JGqcB}%2&Qp8ASNU@a)1mBE^1Ww|u{CgaCvdQSk3I%`-&O)pUu~QcT`U zhLdW%Ej<~^qGAix!hP=W+p{?CO;2A{l3))}bUO+mM(c6D_5>NMt1ovk2*nZ8Yv*7- z2zVK!xz z=>ZnpA)&Gfj|4nPFbj>T5@=FkT?j!=bgx97!<|o9a2g>>f+TG1vv{YE=4fwu*KR#pH}MiVsG z!H!&aU`2`l5qPVQ1ln=QGfn3jBjz2iH3+|rKam*d!muSy^nf>DlsD_J500mpT8*&J z&km{_;LDp@XO&0j^5QHzCsh)B2?cL}3?bmTgXs8xKmx{F2M&BF<0*0|>^lnm0A5J=2;v5w z3{S(NI|SJ!3Z=;)K$HgK68BB+P?;bU2>z5kE-m1e(&QdVE@&n&sD;{0HkM0L;rU$r zA=#c4xvLOFdaQ^%Gj+D&<%Pspk^7Bqbg)ndT2IHIcY|gS5^sj~HsEB;e-X41Z5j^B z&5AU2a{TV+^olieuWv%XCeUX5F-%QBi(qQPk)}wYLv{`raLzE}n5l)L{@3|r(68dg zj0*KYw2T(vRc~NZv*5H!o`M0dkrrw9BgO^32_hC(GOS%lf4=Orxw zDkS1A53pNhv*G}_E=*_h+>LL2VbK1V7cI0Bb%pqt{lojiVxONzv)&LWb-KN~B16X= zqyr6VypBHuOO;VJ@B(6iD)V*?U>SF~UE+oA{O)?+UhdYDdqmSk*anx(pJkxCe0~_{ zbAAR6Q#ci7`Ix^OXl6=J5T)BQ?OW`;vRlwGce>)L0&#)Cr1 zT>7Aa$`M)d*@Fm+yd~#ZaMbU$Lzc#ZV0*1W_R!I&A)f@QU$PD2+C~8Fx-3 zd+$@?>2};uc{cJs&>C*yi+#dx1L9p1_ZezP!kzFV^$U+m!0~T@G^AZ#6jcnnF$kY> zte=~{Qg}cx%-LaG%9Rx$UXb$8*3_a3Xf0IRqX_ptr~9mmpj^#MA=*aN8{qOoctF6u zQG8JCI&leC;`^&A!SX>=e=vD2Gd{W;QWiya6ca103|Wh;{}(~`h$8&^-t71<$;}1z zf)a#HUm-%Wvs&P72=G5!3zZ?W=E^{)N0auAAI|!G5lJfOmaGHUFcfF<*WKtf^mok} zBtI#`7t!Q9#mS>t@IFUVdj?uH*e5Z_*{t3lCQX9q15Vp@GRa>UEcT{b#r~pa2>v9G z06oID$0%E8>W&rhN#*(K-JWuF;COqnJBJz(` z%~Q~7s7~<0+dS^WM;o@@ZTsT3YMou;iNr{B(RB$S0h5Vb3K>Fa(rmL^DY%O$Ag|^9 zJJ5SaC^h{bbDDtQmrDYy??tU;2PXxhKNZH~%SVrjf6wFSpu1Oa+%v6hM!E18nc1vb zf(yLava@MWqr0?g?m?$WtYFX7o=0b(wSA3y;qkU4=L1s}?7zoBqulVdY)p}ugLh8U zvSsk$5}c`4uvRm&3tPen==zCYacGN&0z!=6Q_bT&kJEx?`$ZW#T^_;g5=n38v^dBT zFR1Yz`&Mnu&8eopVwj=Wyzskp^ng;6{nlh-c4pqwRe`=i{RI$r0R|uRhKH7JN;x{ww%9nXNb1V%)n|OYE7fgOX;PMZTcT79s~11M&$nY4G7$4=kBQSh?fhq>&?a0h zLKLQn>9?qMhQWmb#V>OlW zZ15>sW|+l_nQrvo%GngsgEx#60*iBGtl!d9WiiL)a=#n7(F$sN)oz zca_>#*vcR`(}XJQ-tLlD@X(^+a2K3sitbVtgn!4Icm=L$UJBQuLGiPv+tZF0&40m) z=%*ap;TKTNAN%qsw2@3W^Q_(51wc!3Y?atNWo4rv7Pa~xW94K|Ea7X_@QpGflH>zb z3=J>(U-S{|T6jk-gOd);o51w)z0575TChhsQLAm3gIFmM~m1|3>1O?RuacQN&{Vp znp=ATjX19Ufj)(MSbk)+1{vQUr*TD3kN(EN-d&wJ?k8eFaC^C*tU{g|Y@Kc{~ zKRB8t|GA49fj~CMoq!gR^=G!=wYmh5GjT;ee8~N)@5NA#s+RvmnGjGi@*F9i5F@F= z`H|>ppczlwLW0j-Q%kNYZ3}Hiu((dh7{fHE`8E5#1 zSP88HJ6+;iZ@eR=nHC24L>)a`h@P3zHU{`XTH2Dv!h~BR4?4BfREV!k%uFM5`wvOb z#pdqj5|O-0!`kw&XQLU6Titb#HL%{FPTW0cUqr23v$Pi-7-c`tInHLiJ?g-GF*&S( znPm;2gxguT0>vxPAO%$KJ(rjP1xRDF_jtUfn?Ks!elC-9u2s7B zQn5ewftVZ7`dH2FEPF5zeQkCk(N8C`(=#pWRR_O=^a!jQiBpW@4okOLdMzjQr2}e@ z%(A9H`DxcFM>&Q9LF{8xm2hxnTcLWS7#|#=f#+szsF=Fu{7qF)Vrx_NrK|ko{^Sd- zLVK{K@y9R;{B0i)c8Z(bQw~Sip^}11UJPJd?6XlWKXaBS$03T&ICqHJtiK8ibBi+A^qt? z$AzmP4W*yc_YA>OJ(I0M2g7IH9g|)idSSy>9ys|xCzcu9gBp2260TJSb6?}9TPH`M55_+<=?wYCN4~N#2NS57gF!jG)KBoEBnPF5B zTzjy8?3YS}kS|foH00W<>emGa&Kjtw@_o^T-kmG+jAUQ(x;PeP+836re`4DB>KcpI z*n_Tp*yB)RXG-bOVuzmd!N*w}@tmzMG3lq9KWvpk4aG%H6i`nMbjlUWyyVQD*NoMg$#<1PqKf!^_zs9J=FhbGf;>TEQU5Jwnc-nZD;*E}|XqiQP z==|Qvx+fCh`lY8wtyl-jf+#5LCbnWZQs3ccP4X$rQYy$|b*!XWGY~r!^lo4=x`VZN zva!K2%p;@4(wRhP;>)gs%TlxeuG+#9UU62L_Sh&Yt-AobJf$UH{C(Vf4#lrC(_;r3 zWDU6!4_Ks%dW9Nk{MH%9%fU{mKlMw8zosW6g2k$|SU9p_#c8aF)^8RTw$yxdeRWESPNE(LrEPI^E1;#W;;K8p{j(r@hkbXG zqT*2xdg^Fp5gT$AYkl)aw$UAXdd2yn6Ne&u#-(3^g>N3LG>+&RG_BM~vJDMn?$MGpD9g z$0!87l~qq>y)^uzdjuQvaqxj(M{#nz>RzzL2D+jCn2wH97(7`ub_u`M?;D0rpy;Gc?D8#uYmZv- zjJqJCX4;|Of2z2HuhJpeVC?Wpa)Cq@pp#=_ymM;r^Z||TjQ?q%J2Ba^cfAKS00Xgi zNE5PmBu&)Ih#VA8ka`HThy{S z2Id%|6k*=9#HDu_%T{IP62?sF2T)BnG@s*C7mPMMIv?#Lc&%zzZY;&n$q1_Fp;4Go zi1Jr;Rf*p^QZqEF&1b$OSU9H6R4dI95!703m|oEZ*VkI%CNF-Q+WZm0^ zaN^5wX6>0ZdPl}Zt7+9CzL}TAZhQ-*jXj)J)Wl-;dW6>SkrY|WZ2`N@gbx2taLm^lX0OjLI?oO>w+Kav?Edi@>tOkruUEG(ozXMCMQYmEaL=EJ5x{ zG5huP=)vvg)k9;&`hEoPsO>T^diar3`KQPVwWRs)zlJ!wl)?zH_$aRm+NH9&>Cn9g z3s{6z5+clQvoTqDVyH-I7<}ezF;%8CDr{AdOx#xGg$0`9strMXh9k?=!_P<5uuoBR zU`gwE5&h|(lT{PZnSpe9m@_PpVVg)*L5zpN)6$+0fDP`#6dJ-5$yZbe*G2Ez1p>=197q#5zE`G`m$~K+ho7smg%Oi?IsmsHP zgu|4=C4}vRLas>KS1O`ztyM0!tR$zWsI4%VrEpTxL=c-uFf%;u3w{*Bv<5`f#xHj} zxn*<@hGVU3Z?8|88VvbJf;>|JmFoOeI*bp+=S`9o$-MXez|V%blrkA`<`4$%{S+kh zq0Z?)TW5NMh~5iEff=aMjq)^c`GFhre?Mqd4xT&@gOIF5%${0= zw|(j@6)jDI{>#6W0~;vQ2jHTE&FSB+3Bwv07vZPa*5 zFk81hQLlJ;g>Tti`b)h7;k_}5E z^vCH{sQT7P>-8#e58TNvyT)OD#q|5$H}7LNjz!Hf1UDy3q$BmNUa)HNn#Ly$jo=Zh zTT`@QE@Q?ryH}=UZ2mQX)XmR`$MG=jdKCZu{Kg@j7?evp5L2jUQ)>H(_1j6+OFkZm znan1wLSo^`!Xl)eNSwU95~3y7eg`c- z5*2w8BYGpxN=wukv-an58||FYF?X}7gpCzA&NOW9DvF%09wj8cY)4<+xKezCHnbxa z=fE+ZT83vzzE@1#!7H_-EcMN8FhLt7Me9bP$NsVcWx9gcss})?eE5LtW(Ot`DR?8C7_j5xCGE`>Q zj|V}{`Q=^qK8J|M0P}5vDp=P$I7yMbG@L~DX{FoCmhqlF8av0|xTa zrB6R?O9?CZF2BaivxTabugsvbsv9~DoJy0(_p6>aK3_XrWW`n{Ve@)F#H=oU`U$PS z^_A9*6f+Ga`lUE1%{_L|bS1!c-3_UvN81!`xH*rKG(l4O2jjFw^|$I!P2-%>>FV7p z$;8A$@p-C)%LWIVXmzxRl0H*RC`&9f$|<{gN{2%3juyj4eQprmyP_M(8*81;n~umE z#1YFvu2}XO7`u^?lj;s|`E$GY$5L&tW`6gP>dX_(^ zaUw=x`)}bU!Y}vwzD$G@XCKMe>hm3mvlnV@EMhQabEOL}Jt7yR*5$7b#Q3DoBj5Rn zW(ZFQI3Hk(wUSh%IKw@jnwLFx*bL@bID!Tt)Y^jdbzBUo_>!2&7*J$wmLuTPFu8qy z1B1EE^j6tsWN3i!M4DfKsY3=s2=X;R0uDKceC2B$Bfy1qz;M5!?%|IK$>Erq=qBdJ21Jc(fL!~o}^lfMs zBh*-s`O%t*Sd>;rHSiI>AkN}eQ{v7Tr4}VLy0J=G6}NzPgCHf*P9?=|3^3tB0|2NT zW?$*_{yIV58QorHM`9+qT*qmpHbq@sYoO9t=bOWu=dZkh9M56*?L95vG5>FQ1I(`n zZie>urk2KkJaSuAQ@7hMBaY{NRa8RCpg;IWyDU;}TSk78$nCQO!Y$$5|vfi7_qErt)&= zVdeP%s!)E9H$4aK5zk)Gd(J3`jdsSrq*9AHO(uj5v0p(G9#=v0n*$6_S>bG>@H|1y zY^?EkpMGz$*{2_xPSq-DRO8aJ<10(MyZKn{nsBg$q#%O!g!e2xC#=hLI3Z-IpF(Re z+c|L&m~&42W_$2Ajb{)2TOU0+)gHodM+^s*IIcl*U3xqhzThW$QiOeKZ9I%G>0P2` zUfS+mqKl?4Y;Fjw$6%=38dH5Krh)CKkB+^so?WnpF?eb(D($G;3+M!5N+lWQ zE_Hvl_Mq_SC*k#ngqq-SYSxK%Q<=inx3YB!(^I)Vh>*z7uLdnNJ;C3meZ|K(DRt&{ z+#m_U4zf;B3yAvAK_Yu>c>(Sw)-)WnrcA)=L#vjsRbM$3XaKGqoJN~HOicSzK?Nd&txHB;q z>tMn?f9!PT73Xzr#rt1GkBv`^MNBV+|Z#IhLvMDqVZ}yai{xiZL-=;QfhEj<@7-bm|KC z*#4lo>l^Wdc)aFFXc`q8THV+K{#56B%CIV8c#S|r1zBA#D6xM&AmZ$;hpi*AHliaW zMo8R_(tJEt4RJS!9JSqVq zF}Z~p75+hSk0=u(F@IFwj%?9#XLC>ilQ-dha3iUcXT;F1(bBQwN&5XR@Nz zqcBM1=&)mlX}uo=B?C;ONmZH!qY(2xIBA*l7@gQ6kcQx8=ieAd9Pt~F{IivqNf)*E zpy0LIa63OQeOZehtu?~X<_Srlb7X<>FYNUN)9#SUv{QBQZfbs}4JKPTkhQ z;$RLI#UDN(D4QfwRT2Lp?jCu-f1v#e*2i2g;6uu=C>S^08_8lRTbR4A-J@Cb(BVWK z)pa_4;yrh79bQuZt*bf-%NscG2ATbS4UOy_}K zDI(^2?cCtp8y|8mk6bZjrF&MNPJ>c!PjdFwf5lei#|LbDFaRKv@V~`Yl=s*wDX73p zDJmsIZJ}#vYGi2l2XmubzT2`F52pR@3;wBhc_AdxiSBeq))PP|4H*&!Siatq8?o4; zfK0yj#4kK^{Tq7oVrF`|6nyN(LXGijA34tnyD$I98l2S)^zs=~xV(w02w^3!;v>!K zbi!`Xbg4m%z%HKX^z?_W@uOpas7L>Z=t065jQOIa{OXOXem2$1g{|=2;u0GvTZ05e z^h)I?d!bo-$VoaBA4WAY(NmE4YSBr?9iTuk>VaaQ zJ{9f!EiYv1UJV^0mRR(WQW0&=Tchtbt`193`HU*0#gTmb=!4x9X0MV*-IZb(!q6O8 z75gwpeMp)Vd7hctHUiueKb_hjgNBK@zBVDO-`d|n{_-mqV;B$0J9%j9ebtkUI1n&0 z0Qn!6XT3Z3FY@PNt^d^e&-GWoRTSS3|7kVWpH}`v+Wtyv`o&lKEoSe?-1{=IKiF%3 zYW=yO?02hw5uSe6brAkHt>!oV>EHYPTkAiqB>at)-*l*dw(^S%^jk*%#>!vmQU8DK zfFl0yKK+$0_0M*G5vG0%$KTlbU-YSe8T)g1ekuRSf%-+D`YkShW9%R3RR3-C{@r7f ze`EBoWU7De=`T9fZ-GGi8;|`Px$2)S{i0X>7QVl+^qXw;@6P%wp8Sh$^;?|Zt^6im z{ppWCo&4AN5RW>{~zPqGJ603 literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.png b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.png new file mode 100644 index 0000000000000000000000000000000000000000..0dbf6f8d88d5d761e0c4d79cb2b2852a4448a942 GIT binary patch literal 74304 zcmce-byQr>w-7BJ5G1i2>1 zx*kvJ>+9R$6&T)pru{1bH{|<=f#QQ3s@&Tg`02kz8o{EvT5}zHeS?wCdbOiF*E|HF z`6I^`gDLw~?pb9E;ENzEL&Z>lHJQy>l`3|&8|qc&3^`cj*Yri0rQa2ishwPMB*Ctr z+LSg&3~6^)#p8jD21ey+$KBB?bpZE+qLyzOuE=|1C6~;q^rl(?SF=0GWy+g2^|*2( z?V-{fGk^R7xyw}A=zt~h74)t(fts*4y>eT~n`?Kp9;L>?=Y#CK(%g0lA8`3-A<;~- z3JklK1TsGz{FnRW!Qn;DJ+m{5=#-*x)4z7o)kUY129ip~S(~)erh~>(KzmFmC>meM zCp4G~4=`=|gt;}(&`zAFiv9FHLj-rr!h&Q|Lh|sMr!3$#-7||Wx6C}!{msGIxeZ;Z zy8~Nzspx#gFVYowjMmuS!$xm(QpO6J@&=h?afGL&)ajO;$;{Cg+6U47#)fjOp<{iLU%kH&wjtjc`P z=3npW9ezwAY=jri8?q2M?#iYYkY<$tE#e-oc_uxO2YxCdf8aNxF*dqZOG8$mQ2O6FkKaK*kC79iCRGevx zJImISQgNMXx-&})Q|7T9WByz&Cg82WPMK}LVuzM za@f1N!#h0R$ih+0k359j3=2p>+*mx3@r)8Bh!9IOob=MmI47lVsTMe_fBteq9pXR0 ziGL@6SoB@xjm-}YF{1zca_I;M4Mv~(qpwovOtqn}go!yP&G z=vd-?nw_Y>llL6QPOP10WZ)_xDx|9zDTMaFI0;OIP;J5fKxw0VE}>PH#ZAhnj#6-# zZ19^E|^M^Srh^>7b|L6 zgNRpovjWj{{qB9$Y~L&HE}}}NnmD&5*@c-nf*reDZ8jaJ|6l3 z4yU?gZ~TXSf7WR0!{ovY)1d-aEYp6zpK;kcZ~8o#TS5h%E;pZD;y=DE6x106VD zV`#_CN=}|axdrsKVV}=aQ12EP>%i|>D@i6Q8l;kM4ru~&(;1O5&2Jx0DZH#ztOd9_ z#&d_Iv8gQPYZUdY=SP|<3xu29XCIjuh=Jv3+C2g7exNfBrT>Q>cxxlzt!Jp-@XD1CN1%!Gi;e=gZgD)8GWc zb2aU>ys3#=nOMZ6w~IRTlJnh9)=X5{en0b)JmvKwnK{hol#lrHL8*B`NTaDMjQUm8 zEXFVI#iK%bN1<8T2KeFrc*gm@C7%);H(bqW2AviC_*y%@&1434VvmhS5AEDEJFt-J zRU28RW!3C*G@7K1ZVEcfEI^7~zZ&YzdG%1LeOj1ifZB|cpB=tLes`)u(n zyRuNh;EF-{0do9K3oa#lA_&W-DcoK`65=U5!? zst&&6Wtn5BKgE_(2STAhC#AQtw!Oq<`{l=&X~qb?-0rnTMb1C?!>?3KZL4mn*sOvo z+>leN;_>^}e;#2T4l|Jk_Ircr!yztolW70u#Kgegxc~8K6C#SJ-uRq|MZcHYi%^lO zyeAK)O0NTT@~8#0nH_u#vM#@U&F)qlSto1t@%hu$y_`C=*R;fzS*=~UTo{HyuD!75 z;i|9V_gw^>vvsNAn2=>_livih?fV~7aw0$uKyWB&-K+;%D=|lusQsM{z+UGB%im%6= z#W3^hKr9q5U1cQiaifQBbF-dlYiDpyr2vQ1P+f+P4XX$HY8FkBlQEPHRPF0+$Ima! zrYeWupd*2EsYE@6jU|=;E1u&&_bc!USFgLkc-MFu3Pd+#xi_Tn>SAHv za9Zx|L7BI|QKh7yB(P522g3 z`-xuN(YcIRHJx$%98kBL+O)dH6VT4fr}*xh%I1j%+s!Dh$M!QsJ1ma4dmR(>W(n;&g&~<_8OLW)cP73Z55=X|2pQqpA(I^ICBQ z7jPBbhAVWUH*wL&d0syWFm&28I_!7mf6!FEH58O9DkyB;`8iQ{N^d~`%RbYMB#^+WtazKoe!!9*=^P+*v3TL8~q zBJ4A@sPRrB>3=N9|3b_!=a`aB_aav$JD;UjR_12orS}23WGv=`5jyl;5S?REDpB`P zV^moy6X%Cy4(}C>hi6&9l1Qs*u5Wop%xR%9JO6`Dxe&g-!&XNqy_pSFxBnseRGB); z3~*H@DG?|i(>=<(vDF|c1cFc=oo^gLEyJ3PUfkU7DHepTte*zj@y5_DT47@YtQH< z++t83JD1gIMLWKThs6^nDCT@mnI&AjB21GC?i=Y=o15tso}5)E%a`0?zwMs*O#N24 z&i6|aj;GJ`aAgOZDgwWMyC?SoN6kKI^57@`evP(d`w4`;1feNPJalIJDSx7pMow9?AFCFe5Mcgr>Q$Nx~LFutk}14eT3Y7j$knn=Ndh~;y$^sK^C}A)4QURnVPb@ zJSmiciAP4=(%57*%~s6*5b-{gmnn@&{Oj->{%@cTiC@RSXm7vFk8-xx5K>>__m%kI zY>#lp6omxOjsAe4cVHG$U7DHNWcQ|p;r<#v=Pk9rL{_r;Jp_w%ud}_Sv^2EVg6I=b z3WoAe(ol~hsMXe3*L75cGDYY+#N&3M+|D}!{r>)`Z^wKvEgpKp)t(%R{n)x~M%EK; z)6NyXDKCo}Z6|OceH(R!mXJFqvL!koh?43=D*IJ+QfTdbd7zPQ!FlK1gkUg)P(jOz ziEe`;N{eN1x1ak+KoUlz+XQN26z|1n!m%=rgf(znka!^S>X8^1iPed#m09fo&%<$n6=8R$}bA= z4V{KVBr3U_I7^h`fKT(dkb;)*H=5fGq3N}c-T>dx-1ws}gP>%d=5cm3mhcU$@w0^l zYS}iov(@`j+EJ@^$(>p8O8usxhiqL{ONK2Kir{L+@DbiA2z5W`kRuxK8zceu+)?MH zojTcj?(sDYol?CDIP~E%8S+W5>QDDc@{L?JriS`~AVN_vOxzuXFcT6h5LF%?UrjIf zUMdPlY1US8@kYQLQYy)_lW!&>(2*GIkmb{Pz5S$qX#eO9Ut3zCDDf#usPZ321YuC8`(61fKOW`Giw+NhG-++9hEJ2&(l7m zqfJ_?3DM8NlLh-gL#pbyY7IY${zi_jSElhQRRwwt+nI$74A~FqdJ--n57Ha8w4sfK z0OKYcQ7i7`jrr)ix>3cqo=Y4}&+r!2a$a+Uy?^Mjn63*sWB?CcmtBi8O`XK7d@36g z%-A%=bwr<1!Qr1oM{3bOD*Er}bZXN9jur}ASk#n~432bFMapihhw}eHj{dhV-xS~J!o!t@S$%+$!%swjD|#;v4>zrKmlzEAQsXu} zQWiR|vLS^1B|QQBok2-O~BhTNI&i0QPE)YwRiJ`o{{q6tyX;N3FBs3bS7ZYIk7Swi+GOH5he2=iVi z13>dpSg)OLZ{e9KNA@ramMu+}vdAEbTaN0xc7iA3Md5|$r^?zg$6gI+|FiQL`CNDf`Kwu|CdIXzD=Lw9K#=*PlM4c_j&H&7KaJj`bZZmlrOIj+5C!Ty>VcY9t z%CQdZOX(t zccxj((aAp3u<=-A5eEl0Lx4U%t9KV$@vvz3AZ>WjX_3LkE90?oW^a|olzad!a;j!O z^wVbNszr-XF3Xw^yS^O8uJ`+OTAp+@mHmM_Jj*cVdnF;kO1Ec+Tan;DoJaG6w8#ZB zJfYz0IgoMl?1`b>!E>HyR)r#41&M&d5IaK`!S#0FJoV5MT#CE|(Z1EM88))fYnt*o zd(S^N2gcpr{ai`ORDi1BU+j_7PM}DH-&m<~Cj0LRu=QA@c8U!&rc0^BYYH6Es(`a5 z$D}F5czh$+;H@r#W|I};`{IYODv1PuNAvWGmP#0kjypI~#DTten;Swc=$K9F-A4O2 z+cxr@*#`4ddIb#T%_AhNsq`Doj$U7GySvR_=p3iaCrPJv*WJ0gCy>^Zf4PP%f; z6^cI`x}Oa1_qM5O=VHDt#!pq_RemL0LJwY35y~m>I~Tfs5$LaZ`fV=ENq@st-DcmN zq2u5eBD(QhxC%04+3P!aEb4S=x5xIJSbcEXdDDMKrb(RR_ zEm%JbyRVE#&JLO?$nX7j2>`h(^dk!B;Y0$a_U#N_MS`!5S2ygF2CL}08738zQ+ktQ z<2Q!OyQhD?N_BIqOl5@ZV%I^Ue>crp5ZDwvJ-P-cYpfa>&ikYFUdtPG%g212q&};z zF6f9QUwEm~D7rjZDN9T1JV{`qi&O2^0K^D?Rp3-?Hp~9`Q|^AZXB<~ph9}+6DF@=$ z-&D^0)R4|>hF3e97ga>#p!SucICWEc_a?$mn;#{I_KrRkxX&@>bN@uc+udBuoiM0v z8+pa(AcSQUoTk}2)Ot&kb4=}cQpHH9IU#<<)58g6k8n~AMr#%JJ9_lIhj;s13rgBH-t)_UTNjtYq256 zT-V2*pin{WkOoofN?kR{0=IGUB?e4lpa_m{JnT1FI+zqpektP&9iy$-#IOn5)HCRM zDSCT+2q=K^zE@{!KQU6enq?bccotT6(QJ;DO-!c4;&QcAEI;sJ0JhW}BwEqeLSwA$Cde~IbRhGv~gTFM8;>0ea{hO#Sc zh#`i&r<3mq-;5|o5xLD zNZ3f`)RS0ZJz=BEl%y`~$`wmoT%5-aEEzZ?8c%|XKaqq0w`2bwN%NlyUH*$U0x_yX zqL3w8B2c1m)B)nO?`zz7RJ%=mN;+0glZBMxx+%~VXg);6_7!M)9VFO^b3fxz z>~~ECl(<`4$dl=fkL@S+KW!3O0G^m~{n8{cnOU0Q6v>}kEx4Q^JTP7EQOJwc6{f9| z@z9$FR{1>B1=w4cp1_f%iS%R_>CJ&N`0Fyug52i$)7D`Oh;!xI8Gr1_msp!4WCN9a6m z%)U9tUJdy$LywMLyZCl=gqk$C(x&U?s9o8Yzq)L6#UNX(1=|yWQlYOTB)%7u+M%tD zohOVHY=T?TSVf}f~;zLMokuK_|Hu^Bz8$k)YxrMgtyKu=L932%MSfkU} zySdov^XIeEBF{(YlZQEzN*ZZcCrM=@{iawjSX(c94TjgX8Ho34BaFUltuxk_p+gr|pajDb=O8IXxN*_Xu zFHQ{jDCFtAcSx72zt|)h8VtUPPlGjx2g1qyi*f!76k*F*;*#M{UwAwgul2{Q5#wAu zW6z_jy``De@z&T66rv0@@_{$|d!ua1%*=iz9%rcM_-XR zKA5W5c<|jlYtY#uJy%OTNy?!e$#lw5;NiN-r~h)^lYMXoG2}FXs{z>#E7vQRmi83_ zMgq!aj>JsMTFH%EM}9AMl@2MBYX=LU{N6l$UVDfm)`t1YErTK>XBu7J8eh1vU;1n9 z=cC!-z)SBexv`OBUUaw2R?Db@oSkEBUr_CDV?b@Q^4&Xk>ZP018JbN2pHnKVZPkpr zW@w6Nuj;FHJqKVU1e5ySlSaH-nwt9iV<7E#hDsYOf>X!QM ztbTV1is3~d8@YTGPw-e5&{ew2(Nq7j(Jv@`r$AB_L0ut#qA2MRklUrFw$}Jm6gFwG zQVrXw8m9W|HVC(P?>wBo_$M{fJp51$o)F?6mW8bQ>8DIG*i&etK+88K}Lb zOi?#V*{&>S8XVU{V(Eh@C4L4Vr@9mBkl%kpkFatya7$A8Q-c!NsVubCgyPGjidm7D zBRjA039zEa(vRgZ5oddM({&ji;*wBQrcwn1O3BsP5Z3UOf_d?_(xZjqdhe z{+YM($@P(~gwGdqOO}U~=6&MI2o{2`#uquNvPmJ?-DgZ`@_TQT2Rj!}&#~+lJoR|a_;|);1g51}a+$nb<%L z(aJdS`~U9*>OZ9{{{wmcM>mAzy$45MI5CkBMtHbci9k5G#&`-iF%|$a+~@Zw|5H=> zADVC`y4%L~>%WGTUDULWjE+{560&G)6@=lUc$(KvAN&YYlXdq@>kInYEf z4089>>iS~tnDg#TXs5H%*wdk4q@ZRyRcY{!>slr*v&@Rre(dDXCFWYi@$y87Z~1`p zP5YJ&!$0LEZLzZr&)>$6n&LmB$_IW1gw1;L#Q2!^aFvlcfAuCG^hoW9Ta`-let+CX zPAd&9&tAA^Jf&V47KwEB$n%$;Z7KRDwcpn?NsF}ti#XNwdHV3Tb!De6Y|kW0#8Xgt zoxH*}ux0z~19Md5ph%ahl4KGz0vRk>IhPgUjXw07sEg;t%@U!#72K=uZou+m*|76x zZi6Wq{>wiH3o=66k;9_CuM4f;99+LFlc}s*{#20h*7I1um-|vsjIv#wuI=%sDaf%I z{X{Low7}@2nUtKWUVAQ)aJ`}NyuVvvS=c<|fFN6!!0ixu%!@kyO8T0k%+m^ZRi^)% zN{H5o-*Oi^_=L?=vTk&A6QWR>%O3}$AZaP{&bbfbQ?v={UGbPU!teJ+PubO#rn-(J z1?w@CE1r1;nW1`z0A}G~!L6wg#*#T&5sd~l5%hmA&gpKVj~~Y`=Ia`%HI6Is_z;u+ zZD$X<27kC8*}v@=TJqc4l!CmLmgu zMI>CCI4ABJI(m~68Dx7`b#xZpm()r3?hZqbU&zkz{AhCx(`v49&cS51!Up*Ftm<~> zOK5|li#r##k7i4Z!PCJJZr`g;#2_)1W{YJdm5zp*4V%MFm&bPzuzzw#`#C950HE;k zZSTRT~hl+H#at0@Gc6Hh&@RmUPd!-4~IS)=ieu#%e6( zBen%fL3s+TEP#ea*^$Lu-Op|$=*7kD>Dc7%SGcv6cfenPdluulOsa}=oW1t7!@paW zdR7wfg^w6i6|4P^PAxwDt^uRe&5K+S0Gs8c?O2%fMN>OTQanF&^bPF?TL`aXbLOz8 zSaue&?WZUU8?U1%w}(w%yiGl6sSv>>VAA>s3O=|ZdiG@FTFF!o6f)QgXl;Wu^uIoC zgzneG?O;iW&RyyIGs|Q3rauXyPSPjG?kQSQ1yACI*(BkMI&}8!$!C3^v!a)aa1ydx z2bU7Sw(zViCFwa4_FuQ2u1t8LJjW=^uM8I`fw-@r&U&Ma1abn)sNm^ed6O4*pT-q^ z@a{55%-Prgv}4uSs@xA{9KKAg%U}=7rutb`16M-o`1QE>NUQ)Z;h z={uf&ZUnwdwGULHxwwQfFUz8bACr>BL?Xi@3ge|lnrWD6MDwU0<0f<6X1S&mub{Tu zO~;*E)v*XwWxeuIpA#3T!1!^%Hd7bb-)qdJ9cs z?J|r0L&y0r(bp<}L7wv4jsFRFsSx1^c)ApyR>+W+(;Ey62U>60##C9+X9q%jYj7Q)nf&@4QE zQfW~rdR{8{E?TX)bd1bb>vVdBTp|+Z?&`x&oC1V5aC#xIudkHiJCq@{!Q;#&)_Eoh zkALs139%61O8@O1+_x`sm~cPv{!cYICsLq?%8kwot`ya+eX~VsT@*1_?zCXr-^f(A zpz=3hMJZ_Fy52%yA(x@ePji`J^c$1*CcKUI4>mK_NnRBsWPykat(>%qwv?l1AS4# zPwX!Pd!8=eu!ABxoM3(F6W^BG%#@^g3*~B3C0w+IBf`y&VxODI6|0jiaER^MZFKol z_}ZG8VKD%E+=b%X`IIqFM&GkGW9ac(|M_8mVy~&0%rTh%4bLf)V$l`DizTc3?+c>= zq)!nIZYb+NW4-?iBK-eD6RKSxK`YcOKIEI!yAXL&Vc{>Rq0Eqi6E{^1&nHez^nzOB z3ac|8PZmnn`8tEBz4|b%KgNS(wj}$3u$WBvJHt%0bK6nTGvC7}W)GpV-binb-~$4z z#36S-#Noir@sZkQJ2L|9-~KuKeamEhNMn(YzC6@di@)0!$j2+z zYv$n{sF}QQ=9X#_NLO285nZ&uD`SqoYx1;WLcR~@>k$L6E0@Ab$~M4~#WyQoE*xA) zjJ^k^VI%6m`V9|%r(uu5eIb!@8PbdX?(dWt(Kc^N5)9w^12$u{+vB4Ui{7e&h(TAl z$(iFR({sPG++P*iOnkZ`fB67I(jlDU4Nu#;1&WAaJD0p0{q+{S za+rjr!e7}cb;|u9cd6xh=WOKW5o!2{y7@|&SSYxbtElR(^rrpp$UiJ#qe&i&^~=4( z)`RH>Li}hAID}sR9IJA*Yti@1n|Svv{NY=0{D@*p-t}Fx*|$3>-3uKfs1=3QHV`yc>*G9{o!bV%?jL^j@&-a zt!b`;rDp))=;`*o-fhM@koG&$QUOAa%U^eod zF9*gNU{AD6EhgviXX>IR9!dCEoMjjg5K8{{4cNEvZL>C%qdKGv1~c*~#l^a4{>;T)kmwC3kmmYrjjBzS-F!fa!H#vG zz18$X$ibBj|7@Jf(Gtb|DiQdN`>gAgBfGQGaKMFnq1Jq0IL2t9M?$`rvYKoPZ(v9M zoa4-4giSo2ggXr3jZJm{-nErm_M!kAAbd%Xd@U~F!nUSYyKSp-#z%)d@qv2pG_0NX zYqh0!_Kz_#0J0p=<2KTJ?^H$F#UuoP{BG1C)QLbPKM3IsgVH)Q*>YA!KdmsYZJ(EQ zyy;f-%coYWMwI165ejiJ#*-jps^XpluiBKd?-xvIwGHK3F5|nrrsXMMN`7m6EQF%V z!B{JZEyhd2?SNZz>a?w`r7xz*b_&fo&M>K%O9^>g*y4M!yF^J z6{x4_T^KSc`Z#TCjZR=QXgada2bIDtJpfvzYV`QHu6-8{|D8%^Mi4+`lPc?v}qd{6BY} z1wXpV%yZ=Qx*L=!!^(P+J4x$rd+jIHQWU?st`s7*;!W*AA>_hkU|A-S$L&eA;DeVM z<{Mb)Q3PS#OJ&^tkc*0eY+4qKH=E!(+cwx25HCT!;ugr7h0;lI7OfbG-xqpv@jP^E zJ(=BsdEx_sxxBu{_Sa5q7jb#;?LiEntSh=^ku(36aB0&1dn81^8hS@y z(pz8O%diHA>DB^`x{(6XxZ@TV4H$|MX0-1QkE{Nkx(Sr%8O{hHO(ai0vHEzF+_?Iu zQspv%zg3u__-P^_2bY-WC}F&NftTYCw#@F%gKOf}6+y@ie6bG@I>T8U^)Vl?c{#u^ z{w2A7a3wQPS!P_w9GcsTA-E!!s-m#o@+Q3nEDT^^G#(T=N|a?zn1 zO_eHgmw}!x)`uC-?r>n3vRnH8C>@V|pS#jO4-Is`4zNelRGzGKeS1v%$@b26>=Q4~ z8-6j_p*X8YINt(jwTRYFCsK?BRQtf8*kRegjx^6&hH-ZixYiFS@C~&87VOtw@%7m| z1TFAX2{$%&1|p+KU?+&fuo7{q<4K!FOhqj4?y#@-TQTX+73FcIl?vL>x6LAsXKK}V zj+xThPRwif*GRtQ<9#E!in=^QS}%_|%gdJH>Ic|;TZN7r!BTx`t_?hGxfzab^Rt~8 zqM=RYmitq){a>9O>?K$J$#=9!MP#(*tCWcNXU_H2oI=3VL7AyX1wy9*NJjq8<+9Q# zj%wh6t_~*}zziEls&}WqNPnlnZbhH&AKV87j!M^72&_DgypsQUGu2T}$&g87t(o#8 zN-Oj?Cy&kKncCF)tFN6hk12i0?jDp#i@A-ec|LCg=K6yO4Ewb@MYVP5D&X58HW*|1 zeAmhC_}=&Ua4OYvC#!*f;Sz3#4hAc$LLT;O`jmR?HcnvzFozXyjJt@S)~!*f_7eoc zfa9$GMT@L8{AQmVuC4@zX=97-htEAC$~jgL*rBcT0g!39ok5+ONs5HO9h`0mRQ$)C zb^+Z=p0|+y9=4~Ah0guC`2os(!qsD2x(l)4*~R6hbK>Qu0Q!(9%;ZZ&&)_PxDwQBD zwzzcu{Z8V_=oQn&ioQ&tOXiyg2X_ghUKBvI-l4pq=$M#7Dr#!whR$b+42tA{ie`2q zA+HN!JgUR4lw6PS6Kj+E{xNk&FTs%9kH9dY|il{$q6=1RDvf{z1Am1o3vcD~AbN%2aK%#)dH^D`eEY`rc2r zcMqMzUQNdD!abodU5Os=Wa!>5bRK5bCO2QdczjymaEdXFjIvqP!yZB38nl2Q)@~>b zVm_K~G}Kl~6x$SGO6o>dM88d%HvQ5%3ezowuq@J5z7_F*j@SN+rj8J! z1it*m9#BsN6%vq9hN?H6)=MeYGqx4{k<6C>JK*)T_azVNKc%&P7`Nt12u4;${>U)! zn8~r;-fLsye#+Ip?zTN%w2#tPP{)d*Ed2Z+7G?3FIZQ>Z*+A{q)kck9O9b3>87H+-MK&AIHHitj@%O74^tdV}%+v zmvBf-3=yMH;Vu4zI#|J}ur%@RKcUv&J-*juQmeH9KQ~b>^zO zj%vA`z&U38`lMJuW9TIQ?4u)i1Pyz5faybk{m*n`&9FXxWMV(h%$topMHI%BD4s&j z*L2SBk@nJb8(|4THb2tFtDoaRbb!>1$1h;hAS?m$BjN-x(oT<$MQ+<{{^pgHzNdne ztD^abX~Bbd{*{rP%cQvnN~I^_!w2$(=EvOe_S4(upU=Hd?V7t&H_Iiuq~N3+fQHXa+9c8?+!Ex1=-pJ!NDygeklu!tr#DtOf;+!hye z9|K#SNA^^0wy5m#}*IX)x2bZtjl^#+av~ASMH?S{+@vSC`p~?IrQ(9EfL_+=yYl;L@<3Ajz?K#KZsjw8~ew z_X`u5gROmy@2~k7jivj!Yk5fVtUAhW6jq-ALv#9dQ(b|fl2=Cr{Z66uB_7xA@h+Eh zUA8o#h~!Q)Rz13@ogm7id}!V%O^xZG?C=7Ur4D`E?ShYJWH`qrN?CD^yj&29r_Q1wE9a3prpts;+@sF0pP;o+9f)zWiGGW!&Y6dQw7?$eTH*q|KqeVqZsRbNvfz{r6~ zu8~}*tJ_W>wG$W;eB~NZZF!3=`UY_l3 zpj?G|ySNqbk(Lc|g=Gfa?Ju{nFqhvh_iHN{+gpOZ|Lj2L$aRXMckTMv463?KP5x5X zEK{82Z{`@%^xVam3RJX%>ZOtfD3{gxb;{gO;a)8veZ)0ZdC_g%qWNA^G3X1`zJwIG zsjX4;zpMi3&iMyWvOrF^5b8U^TGdAG{6u5Ks<4%{|_HLVe4v5QJ!LD0akRsb&=Ja?Eh3? zNoWfcQn}P?s$w%E!bgiIBNuHg#Xyh6tH3Ow>|ktj}t%%Hk)qx_hQySQV@x3l=d7o%r`NJmW{KU z-Xe2Q7iFhU^GZ~0e-C^wE%gS=+f&@0eDLHZ}Jmtk0SrL`P}H7z^W5i^&8 zO-+^nJ*lYcwUQ$Y-Beu4@6aFG2_vZAXD!}qZH5L0JW*VImcqe|Drd(R z%1zokaVqAId-cB5nL|0!=W-hw8J!8M0L%c-RLdsQs|_qRq1yM%iE+{d%GQaPNd)m)7f z$~!rtbt}*7S8-JWcr-nN(4&AK9gRjM69wTtA{nQGP8EZXY^MF=_Bnz2o~PawfsN~F z6$3BNr^@?=gRGwS3BMMq)xo=+4lK6WY70$!e|+`p+g?`VJN@y8&tRLMGa>I|JQg#z zD8Y8GcF$9{*@18PWk20lgCex*u~yb*9={m2-2SYp^1i(A=zK>~VV%_YC`CkGuEX8TQG1j{q-TjtuqodG+U6J(<8;@6PhW>EX7{;>t_tDJnuYR?|Rl5)w_v`{U>h$4WEl zAZyy{;8_#n{ub{LOY#!*H<#E?9&s6ag z5f;%Z-bxp@OVIyr#kMV8bok37*tKTj{k`sTE@hyXiKlf0J*{GWORqe0!c$~T zaFfSzqM{q;H0pz3`|Id{cIV<|RZ8}mXiTBB`TOHxt925swPBk!3_)G@iMasu8@8@5 zbC(nHOG}k)6}U)@#-IgO^ORt^ZWzAVpF>svve zY+Ls)Hx_Mp(Hk_qX+v*Jl?#7c3>e>PMyFM+J*c(&q*PyScO9u5us@5bM%V?p_+iQR z!!PAf1aOY|MMp%F6*8rEcI?qKa^|lg-K=RX&)*&AP?rlTk#Pa;@8%4izD{+DGAilB zvI+XjQntqlH#%BL8FB-az693>4R#@=o@(|%bDjw1Z60g1KEckSt5&y+y*>uihu%=O z@A$6Y94v)P<}91<-i=rF89YAX+N94)h_#^;wbp+nY;WG|QjNUL6WKHi-`U>|OjrN- z0e{Dw17SIgssQfd0CG;-f6@DT^qh))j3#`xJ~I7$r{~eKa(UiOdScQl?fo)zIkTf0 zY%D~4!zgn7uufU*Mkd-uJaf45<(*L^k4#%#AcYJMMP~)?+~&tT^=6#@kw>18u?G0w2*%a{`(=oP-;SGp^c$6y2^m^-zJkIoe}WEPt;eTDM3UEj1slZ&zzcwe@dg zw?yNRT2*)()s?E0S8``(G3t6e!D`UjY#Z2Cn*1ExEjS_YG!3SuyhQ6gpraXlI3{~A z-#m>z89D3Hff)`3Ir|ZrN+EiY5X;L#N6|{lekipiS?0#-?23+51lhL!_69O1sgK`#RcU zT(oKw0~A7Yp4OFl^b`hp+O-uPtOKujqZun$GzN)uJ9DQPq?~6_Bypg7FnBAfi2>9{ z@JqZB^_gEqt*wF_n%+8)b%<6L48HuHwik}#JjY?FoAF#cDfN7I$|DPH`s`32l+!?v~GT9gx#A21`C6T6E>-)uD`onPfz$&CX4f z1Y+|C6BUQt)W&G6VlM{pQwQQTa9H((AQ zoiafd5X)b0GNyzpDQPis*}$c1k2f)G;Eh@K&d2TO$WnDLot6u;J$|Jt5|z^Tjf7@} zp|1uZIo?AaWT?M6msdxvx-#U`nbZ9XjU`T5H(ZaPPpc)2no_NSK{}?UM@P%T<}Pnt zKK%pM)BLRUVa{F<;GwZsE7vzU4~}ZT4pPm5wK33%R4BAI+Tks!(RO#|l?Dg=G)7LdoRL%m$z`K1cY~i^rdGsIt!$gz=M^ z5`4=Hb{a3bV*$S6+ZPdfT+l8*Ir+<6`TCXKBedg~E&?=2*+jV+$!siRZJYNL(g5@v ze9mkf$|qnUo}&x9-q@c#j#DBz*J6`!n~ujYj3PMaz{h#<>+*@-vbxjTER-z-B~5&; zjX5bjs^Q9y(zwJ-5oUqj>*>WO?#(yO*MxTJco4Kbv6=EqFxn1aru&*i`2L)zQm(?O zD17`j$BUpxI_3siO9~nhxc$PVPMZZ4nt8{xbktEx#dL$e){2IO3 zw9r0BDCX(LTF&z8?OhgX%1zOIN%-vIUx65>nG8Z4fc{*Epr(@7C~Pni?YZ^XN2UP{j57feR=au$)>7FMO zJ1kj1&mrHv(M*?|Lt?TqH9CD@L`O2q9O29;@?)gMltwAp3Se|=3w$wZ@`p1hvLe$q zoHrTPD-b6t+I0&Y@jTx>CUP)_ggm$DloMl77StJ;i%U|Eop$u|xqpnWEHmcmyuof& z$ChR(oecEqt9aJadxw);!RT+(;3GG~TibC)A+mBd0Mazei>cP1`4 zj_V-A8($6J=N9TS&^=%=)?JT$d?(WoXP}b`C$(D=u;T z0gnD1l6&ORr8g_Q#^Fg^Lu9rd)M24^jC1%{`eal!=eSKxfvWB{zAlCH9aF2QYH?`B zU2MKGn~1@A0%J}fVaL1qx+_?ZrM0tYfW>C|U3i3^HVbd!ac!lu36X)P?C`Yr_j%V< zVNaO=5N+ks3h}2VnHeAlVPzW{+uhE_VZ-!J z1sZe!Uw+xBpZe5Px4grT0hh9&o0yh~np0=jJjThVF-|d?hoG`Hy<-9zXp@p>kzy?- zU;kVEh`*@huUobpM&0EyP)WlLS9XMs0GsY-&tjP zg{Uxv645TF%UFBk zC{$Wz7_G0&FzRhai0<7)u;Be#bhc;5+_3C^zFp~X9`B{siX|PQ|H!=CcR>}n&=?Kp zw6srBy4#XhJHZXmEWL4zu>5AZ+HFSixjU9vAsr{^{^nhcWBwbzpstZFALuaa62JMq z%Usx`%?@jSw?-w>hdr;U2^M2S%pLonYeatiyIf?B)#nicDW{*WhKg*=&tB^YH+L;_ zdAYH5Hnv2(LsjYG?&6Ra-Etgob?Azqj$L2;o9_h9mF$5HTT)o8rRt`8ldq$-M?v>G zcF_;yYn9R4#m1b~!}4KdL$ms3s6;F;IZr%+LtqC{v0U<%qGuo`L2K4n)XUZAqNw3x z>dkuJqtd;aiK@7A!SX5ae&~fv*ze-_MA_*r!c~-0xM?3XHdlPO`{R%3;;bQ8)i%qGWkAokK4HXey$I9BM)nXq}}5(Fh0oPt7#WEXW&U zV61c5)+TM<>q5EwW^EI2pW^1Z#Ny>UH)IbO55H%vQ6)BHw^T!qJL*=%-nOd`0_j6b z7$4m&nF-E(V>J+D?G^d!>jl59ybUu2-Y&LwLaev%rBc8tgcJe$im)5MYLj)YpsU)! z-^eQ7_*+V12zYkJ>r?kC0eQ8A902L$w!cstKMrO{*Qh>p4wT{u2EO?GEIRV397xAU zqBTPbve4J0b)mGi>nD`i`K7)4;W{ZP@BdeA5E3P-JK|AO? zO{!ku5+&r}c15P%@UG+ZI_rEr6s-g`m)t+#wfi!X9bi^GlOl%k+eECrt0zq!=6H~~EMG17vm~dgYrT9c z+=~)dINWL2KlUv~nNF~UL*RsR6}w2n6_SLym1+6U<9@g)Ze}?$L4LM!<`AdeC3j<@ocLH+bm87$G{hFoh(=H->&ZF_id9Mc>3Q+ESkS` zERPex6r5~JVF-#Yj1$2UUTfnK%D=KnvBodXS1nEC(q^?mfX6Wr>o-Sxp^IW?v zW{*-f0+ymNnz8;zjU?o?%B1-3RQTl5izJON1+b46-)Nqq>LV(@L`E7eGCBN7soPJ#uv;xzx;z(>k20Cz# zL3~B?4Q7x~O`vN2YyuSq-rk7rVBguQhxTZf7_<0OoR;RDB*9v%rvvjP(C2TVO8C^J!bP=P`19*%A#j3hnr{RY zJw(}<0iA%(;=jQMeu3iVG(EdC93g>G`Z_AiHr&f5`uYNrasmRWkxFCLujR#f z_pa480)TwH2588jZFNe1f9Y)HT3l2Vn?ND9WVpiwp?>o59n+<(%ygM5Cu_o{GiFF` zX?Zzeo#69n^zw6`dVc!pYT(|GaUrSL%8RGT3gOlMM2&u_DHpD+ z=AkGZIr+7x1GJ}~*wDcAi8TMv0HJ#*lHCfiVw-CPhOxP5yb5+^EZJP`%h#_4Tt7=y zb5XxtA7rsG>@Lg4J1|NP{p3-*`7|UIl?kG0m$xr7C0vY>iPvQBQK`2b>iGr*PIh%c zCN>_Pt;nD!dWojsBJwHdQS<<`kV};J`Q$<_*5NnY5n9-)$H4hanuZle>`+4;A3HH2 zN?JmAT>NtXc_ED-b^&-S;QzT_x@(| zUHt1@MH@jod481SDFu)ZZ@Fgh!pZaI!I`y*lo?HvPqEL3B#K5C(g@X2l97s~srVpn z_##i0mwjJg|1a@{>7M*L7r(Dzn-MR1$cOD2jnD>Oa$pBq2>Mm@UjxQ#p;L z{L*h@S2-c4vzLPi#Yku#NCm=5+_Y)l2-X1*dh>;|*Unk!1> zJ(pHMb+#iNlNbw>5lEqK)TrQ<^iDZpm4J;ykG0eq@h`D^_L3v9Fuv8X7UmB0F%e^jy0gM3+xR6hX29-en}?d7MCUr zdjBHNWag*J!8StZ7C{kkKP6G5U5Z-$z+Y7Fal*9m1P|KwCD8hzTH`F zE920Mc?9>(!SCN|lgi4h;{%zvYD}gsBP}1~b-tyPHq@%i4d)rp$v)l_qb0%$BSSoA zIn6yWQQ{)~6szr?R`ESg;`jDXX_s`HF8&I?LmM#uFRKn&0W9YSUsLyTUtRtVG9hQ( zPgFPQeU9cw8vP0_kyLD;CDQUnUgL9@*Y4xE65FVFDt_W84%G7Pe1nQI16{!<5*EXo z6KS(msRk=em3^0n&)8yr##c~jo;j9|L+xz9-I?zTJL>bJpDX42`)U5)EyFL~I}0 z%~w<6pf&=LAMMZEI);-$*UE~gE)9V9qgF$+T~5CfiM%_ZpM3;sLSH9hpk7aQ`L{&_p*qoKmv)X&mL$pCrZ1q>gPG5`KR?$Ie zsFn%V6B8-alw@(Y)_6Pu)B$aaO=eMrCXi(Pe?pek(1aU3sr7LkmKnx`2P;ysCHN)a z=tm9M`ZzUzw%?tY_M;2xb{19W33=C-PxiDZNU=6!c;10?jod3-v8XAe@zcr@Cteb^JL_urus<3kB$;b6@8XBNs{Q&C> zbm7CK)IcbnlAojn*I4MDGQVH*%=&OXI{xwJ2cL_=DwnWy*pm09j@`O2ht*gWw{SYt zT>Jf}R}2Yu5E zCKR8H93SUa<2#3H_;t%RrGD(anXrW80zZ^7|2tV_{if?U5D0?QxBgzhwT983{23vf z5=ob3(UsUTthgILL}TKyo^-R1Y_5(XNRV+SaCM1!pjh{RbiQE<@|Xtk<*E1-g`k2- zLF7OLhqJ_fT9M!f))Sb}RAGrzWLEnCtL>5Xb+Sj;*0PmEV?;6bMVx26O=CwnNA23p zS^@GsmXDbvK#Pyr5^n7e&_~6xkO%r=pisi?P1}CkGl?;zKuy?J3R;3klzm^fwX6ED z|8RW}DgP?nT8MgMHl`D-_Tj(~!}MU?rQj;$yumY`-p}*svfWvW5W;7g4XRv?kDd2+ zLc2AH1_}vydUhP`CtU{8s>bpl80h7khlwe6gcSzFQBySX7Rx7Qq}A(ia}8bMy4*|- z(r8rPT3l(*N)!^vxd{}qJM(;7j5x8Lciw=bGa9Ma+EhLYQZxQ@k$C`;%lV(>k{$~d zaDB2f(<1Y-(+Uw@e!m?eXY%ZaMrS>u*f1 zOMez>5<*jyXnzagatmvuzX=rQ9PP9l@9N>K-AmtiJWa3VM{48aD5L8QUOdXO6}GD~ zQbgCS@8sH=8Tj%NPgU#O-*NP*xnUcD=WDa`AQkdYVjP5xon)VA`i`u?PbI%tTY71y zG{aKGgYT#jLzO3Y#E&CCb>rlWMI95_6|LS9i%xYd{WXapYGxjnVE#B(!Q((#V_cQH zbgRpf!S5(={P}N1U!HSXd6OOGyER1TSICEC6D;nWmuy`{jUk`1a)^_P6To?ahm^wi zAa4$Uqfm2TT}8~SWozV~VO=kFt<{rbqMrv#D7yD_V;MswkY&Xr@2VdWaIxBLb(k^h z{y9+|6|rP4ue`XOw8shu9CfEv--dn_qR`-|Z~ib?Kf38d9j7SrjYjI3uSnIGvo~*> zsy?M7Y~JX}#=7ZMV*d;4CT5di`=YySUY_-uLQoZXe|w|uc2t-KqCkBHO3dA9Rin8p z@mw7TA~^`RaRL9RcA#DG)q&ONVyS7iy%1YS;Qq*l8UrST5_|p&Yw>x_YccY*1cq() zaz_!40TEP=#Vx-MltnM4-MRvEf(Yl?*B(14@rMTx8%?I3>DL!$v#gc48}2qubeI?F{P6!tXTL&FHum19|`uW#lWLzGlN!)>K}BnX_H)+G4zk50?~X5NX^-JZlm5)f#Qj3)pWK3ylQs`vSJIJ8dZv>R}4 za5D@9XvlozHumTuoEFYt9rq&39!iL6ub;#_r$t8z-A0keN8&8*?iRJAwycjL;>i57 zNM50`Y4Hwu*#PfMv0>148PwZ_W&|up5L3ISQM`b^_$JOiH!JdYbDdV*H) zR~jd&PO~5O-DjD;P6p3btr28ZUjO3xyyg?MkLw{cH0D;H743Gl^C`w|I08^fj4-Ol zF)|}-)*$g}(k=q%D&)3aa@0Ed_gzhg@x_=M83`I1{)hkCM3|#Icw)-(1!h!54paKj z{H-`ur5~|~mB|kb)GRX5ye2l3{IpU0ZeLqa@i|NYbV zYyi4411s%DG7P7d*4DoIk(aLxuh@0z~KVp(eLl* zuB-JHJ+BHim{2+q?@RH4bl;wS4~lKDyL(3_I6>=GXlC%w)9zr^&~m=ZRud%BTIwx^dARuLvr_aZ2w~#7tJnhTH_bSzKM4OM@!?h=5^t4ReMfZ)F!|>zo&qc2BG5y@LFPay-<`VdcPrrrHJS|9U zI`o=3^x)i(Mf4c!p#8KNU9O7rB}nKh0J0M73a96W{wqUKWpFXX{n_7@dx7dV7NW9f zG_<&X$@Kr@pdkl1&Ej@ko5b=^D$l3Cb8j@@g1n;yljgk=iGIx0SUdh2YQuI?q+CJ>(`a;V&X+C0j2r*_Oebl#-mf> zO^H{mM@Fw!*&;!msz=UN5r*rInaCwC#UK19&cAAA`LtaOIbb#(fCFpNlm%MRwHY+#%GOVmUB$d$aE{tCs6p zbhCJ@fXQ=sl>MzSE>`u;$EWE5!sFedutb1UX>DIvoz5C1mw;;S`)s!JEPcOb)*cy? zRIMtS1_7_(I*z;fL+JhyKE%Xb)#wTe^T;1F5wA>`%&W^^Z+4ow<-%<5CZ6{7$$Feu zYRwB6Y}SJKeC6z9f%Dl0YEgDbh_oN&_-xpW$9s&i!|$p}(c^+#4fJ8i}*cP9RdNw;g--|gC~-vS8*>y}*^RBcxueNSKvd6-1VXALuw z3PrE9U9ODp>di1)k8{?+Z_+3uA{;(M5V&!2!hBR$&&oEI?Slyj3koE0k`(a{JOzbz zw>H=Eh9MFyjzJrpTWvNu6I8TskO~dAtD5{56#b9J&E98GU`xYqbadH}f&8u1jEvW; z3@cTl>qqe+F=^;<{xwSQN1u%_o#n)HCpyI044r*YRg8;j*Qj05OVP(xoDKEIA^&$* z-OwwohyxVdG2PUAyx0Fii8m^szCL=*kA^9$t?lVg?(^AQJsd{kmO!DKUcPoESKG_9 zHB7k`7pRT2>Gv(l=>0Bu8Kj`ax{}x`?=PUtcA2AcjuFqVEU3xunYu|it7i&$hYw!t zcHnzR)QW+vS&^9M%wlZL#`TvuL4M-XY8j@3i3H+YrRCQzO8z7}d4A#dcyHY+|_!MYY%z< z)f0mwn~zeuxP8=-P0MIChdNBDp{wDF;9)y@xLT!a|wUWcM>5`b#5- z_8;}2qdzoQ#-jnpdb@crLr7Q65lH?)HcT(PrpH<~B;L||nvC&IvEZC&#T9z8gm$rQ zODauR|01;io}-6i_;*Ks0&&ZnqugkZu3UIjg$sTEhi~sh?$r-`r^&8CdCB1Hv?SdP zjInR9C>@DACE&{*SBUrEr4I&!8Hx!d~znF8=KKPH!) zKDh*${dt#=J?U7g%nGt9x`*a0n^zvfKb+`I&cyWJuzh*Y9osxfi9U~8Avo$&nL%>> z`S6ukaEPFp7Tev;Pd(zWWbd5j$bk;y7vO0Gh_@_(nVNB6-Tv6t$28F2%u zIq*l9y6NrJ;OsBCic+os35e!xCf(gs8iDJoSI3gPGTtqpI{n#BRA5XgdQc~J?rDC! zy-2z`8sky-j0Y0AwEs!PH49vkFMjP^8U*cK&N8{a=8zxg!XGAT5dttb{#iN4O#HBM4)VKG`}@~Tazsq5KfOi8oG zgXl6aR^%SPbf&qf<{b6&Z@<3l^>-o(7g?l4;HxOFf_NnlakDx zFbQ9|TOgTjZOS^+R$qdiRpj8^*S@pTVOQ;yD2jZ_7P0$Cb(Mm=VoSIzPZ->I1JD}a%Mgk z*0Fqa>~zU~tjV{{GS{N?xVon=ShLK&BqNdXDRn7^Be2ucW}m5V+dU`o4MrCK9v_UA z{F#}0{kyJ>02037jTI{_YyId(RcIv&V#zKO^M1MIh0k7;sVR-B{ zMZtIlDOiV3Ff$u}ZiNj(re?%05!j6(VKaBM?>jn{J$dHIdyK- z|BDRp|BFkQ{ytq3W%7WZkJ=(6p~uhVWGzi9=Yv+jhBa>jrI=71lpj4GRJ4$6Tz$Yh zcJ@Ix!P{uw&mH7Y?+JZx-`~rj;nTwhD}RLhxfek*k=C6ODcke z*4$=R6ca*s-M@sXk(x66MICbW$WiK_{#Nm?Fh$KP5tl)OHo*9Fm`ZRY7pX5ls(na4 zu(kB^_08G$YDpZ;Pa0*f6DSe6xOjc3eh{;(tbH>J*MoG7Cm?5~_f@-uDv|F*%#45- z_Y8%1vc7a?IXHw)TH)+ejo{p@RISQ8jF0EC1FxbPhayaoL5jdww?VbgpVWW%zM&}c ziAfLGR%YRw=tHRQjvcV73Zf&#eqf2RJku8Qnitz!n#Fuwq~oJ#)nheX+t=PS9sPd) zA}subxu`GkO=~Y!yGg{auw!Ld+?H^7EDF+o!A&Ni5l&M?r4U#?zw0~msqb^_O@)Om zNAAO>gG#yTTf9xx$R`-fj{A9SkmlxhQ5v)S2NzA2=<%j|Tcl(bj<(t?C4LYW#;Vj0 zx0%}-fKJdt>xF^@a^j}w-L@l`e4^@DoIKq6fFy(x1`|3KLy-aml`M_G{wa{I<#~}Y z>w~A7L!w~M>d9M?@dL&WcO{lBFV)*yYXgKmhFR(ul6)-3TPd*CZgja@IUZ~7e?93S zQ@N4Q;fo7^n}@O0h|F8NLVF(b-%Ge3DBY`@cP}4i8OqLmo;a`_UfOo4aO{S&SEMZt z^scX!8H314_9&?vn9OU|Q`U}2t`5G{W|^_ZcQ1=fij6*e z;Z*MLoc8fP_2+G=Qa6{mh-EETr0BUzH0=c&7hGQawmiuH=p9$p)rRc8Mkt7Naoz(} zt6ZbL+Bt_d1_Es*&jbpa@^ycJLWwGjIM;=uzvBg}R6nQVxT;3c$8Nu2TUk!fC-;+a zdvNv#1c-*DYPwuNZm6pk9QC2Skuj+}pSILr(ZFmiQgHC*&VA#~lUsS3>IHq}Dx zr1xV+D}xrGe@Cmd912tr6O#vzPbDfIIHnOX91v(O&5h>s5tWrcAgY^J0w!Y=)usuW zKF@eqz<@s zuI9gTTlo+OD_fScVLrHT92@535Aezn3f;+=*^Oy+=_Q? zxF77~grC1Of47+JCE_i-p;69XVMTPLo|UyRwd8K} z!H2R984_xZVD~aE^;kbSmqB$M|M)BL2-&J*ix3`%c(dF71WH1J6?b3$YfEBO5V3NM z&I}_n?$2s$k=Ff4tku{hb4^%GJWex)|bBF zMWI{j6j>7|-|tRXoE;yodLA!aA~?jH2i3$xE5=rgiu7MzBYq^B5Lxs0V>yywa_}J9 z%Pl)SkLMDWW~&0@D6dt0VBs~=Pw5*)k^?u4PhXt5>99@EGLM(oFZra-C)GgAgxTII z#2%UB;35`p6ktV;5wLslT(NL5^y@WyrJm9V&@qhl@U2wqab$EjNJRNZ@FZ|v$>!ju zPn=(igX+j6r|V!DT$oWo#dS70YlWF$7Brih2X1w}#Wc_2Y?lupG)`GupmvqQNrK6q zTPLgmVcqj;yd!@MBQFCVKqpkPxJ~~AN5So z0>Wk}>??dYIp;KVP<`()dI8Mvo;+9qK*pK9;q)wSolBI~?@t^G^WKuFk^iv*`<3`i zOIK0IkM7go8>AGoVyRYd#f3VptpqNeku>V3Rra4TiN{)dUAD`~?k)sl>>_7mD-WB`sSK!?6(3slbUv?_2jxH5#d zitumvM(%nAgvCOv$ZhT5H#N2T$BC9V5!Hf?8XWP6Y{u?=m%dJk%Q!*G!_9i5CF4ui zbSTL;(HF8Z!GzaiRDKK2f0uI&nQct^R~K`6os^!zFV*h7!&G2FUyENJ5x%uE=Q1PH z@!%pRDo7m&4w98^K6+tyl(0ExzgGL{ZK+)cNsVKWpg+FIop@5F-UPKp^ddnArBE{O z7LA(e$NK7J*Ukx@Hs-CyNQ1uIy)_j&R;h(1oQCV*y7!k&^+xCXh2Zm)&)nn@H6!@~L)m>3L}JO!D~r<` zc68FQfRs5me!iT+uYU0!S9#v;S2;nq=>ubfQi}TM;a>h|peM#iuRPtp6JDLmH}@xn zRP~2sUy3^R+uxzn%2gQkRN}ei;rF25E!3$lOMNsy{sDz}|5CkePY5b54R~*}X2UN< z;=lMuC80MAg7^4ZnSxE*3-JE%Crohqu%MF%V8R>xAq&7Ki<8s^PQgnMyYd~WgA^1) ziZO>?gi6gW3LvNfS%Wc=j8#6D)K+%mVaB92D)j=WE@|GL@p+X3E#oeyP~+Hwh+M;w@eA(s?+KK8fCf^aO9h0C*f++T zo-T2hkwK3Bh0|!FQjMzmWz<0oqa#-Jq3kZGsyA0U zpFRqK!<1+e;6U0Xg0pvnMO9-qJdC zUkgQq_gq~8G$#VyM~Y<3TGGHDft&`$D1RM@*RgdE19V7WI`B8ni0TfjuQXdprGI}T zk`uVp_2*lSgSr(6>h3Env|wU`({LsGX|;yfsqAyHIM%Cwq>n>FPF9*r9#feQ|D7jd zeD;HtI!XN%7qu3pg+F`KijVtULq@#g#R@}Jz7>mVu*m*}bX9e=wwa(p6jzkEEraE_ zd?ice6)VKaK+{i3Z|;pBA*$yBn`aHKP)Me|w!7?V(Z%nPhRm+ynk{?dkvKDgYQ1eD z5&$?El)F^EiS4NJ9=T7IvDqivj0Z$pCSoZoGXlwAZX!^iTw!&PKHt=gsVwFUAw0jh z$E%E)0;E~dwWOw5I5ptH-7xLE-*ABos7^V}rm~W48?Hb(&@o(=;?4 z_hb0`=c@X1X$zpYF0t+p%Yn+>6zX*@t~yhR89Rb@x?7JmgO8PU9X)O>m~oEfVgbo7 zV&lImaIvK`h*9tI=4~;`E4F9+v~GcIU0QL9_1)0vt%~9;7FvxbV`~PcyAP?2?-c*z z=zN@OiG0_iuyfKbbU6;b66G<&J!k#5oN7(anR^yM31qMAvyOFNt^l$h=;biUuBL(r z?6yoG8HrWr>p9|QHV`4AQbp@I*{fLwt)iQ_P@QiS;(lLybK-IvZa=D_|4)7k@x-OLTGjrGU+`y~WG1c44-=_x)>jwK79S5>vsM$F zEFzTWVcxEf>7zZ_Xj%_WsHh0tPe)v_r?`Ryak$0u`m^vZX!}*ToCX++JhO50 zzZ#yyrLE{ICDeW>zJ(U2SWRr&Y+Guoaw-}j^U+(~Q3@on9R%hnJ?TlA5-F{WW zY*O!oLhuHgL+^M4XA)N{zZzn}q9oILeVHQ-*pAt6UBXIv8pugxcB$wfFlE zmXB|i>~B*vWLr@TYjaS!JYr}3aQv-)(Wf;8O-aM>R5X_s5g2&D@$*x0ylQ##?h;UU}63e9F$~gl_Q}HxAuZ<7i=pGVy0&08BM2R)M)kcUcMwL zkP&QOa`f9N(Ko=MMWjL7>42y{0`H%Hc=2&o)+Zy`#`~Ox#?!~~8vU@8pD#93MeIXP zmhe!@%#WAeyU{@@f<8=tF8Hg3;KbA-+ZST`D-;cZKGV|&=@RuvbFR_r?^++O7ng8Q zT9OgN$s7K+r}5hY5p|1DsqHY|GuX8-``SI2Qbg3V-+cHujLzTJ{`?fMCIf$cF2ZS` zbXnt9*`3DK1XhTY!~h>m!+k_uJ@wv#nhCcS5RNS6*QRMh(%wf|58Ziq8TTm%bf1f> zT?SXKbFyX*7^_}$4n+6_+E4yV-N{5y@wh;788xsgsx`-IdvtelIwjV(ZZV-v{;^> z85b$Xf3#=1N_L2gAmD0uPIt}S$>34jrFbRU1%WqLmtQvrn?+>qh4oUXuy2)`1ci1^dyvD6BgQm>5@~O%WJWR5Q(m32AU+dj-jqh`mav({Rk#^>d zEC1o4nzYEfWxB2UIxe%o_cO;J%nfFOy-E?mKSfc|lBVQlMMM0FuBcu)70JUP((W*C zjr@=C18>;qJ4X-*=ZU zPUASb0AC6a^)-j_LJRAg3-`JDPE!Lw!sYouQ%-MCdfykl_tH2>^^BQTZI!zr7)E0mROeqU39%k*)n`gKS#J)fL^m}U$ zi=iTAHAtYnt19rzQ?9oN^HMiZa0Xk2-I((IDsA$o^+-g^ICrHU8peAEnRTo` z`aP8$UheJ>IJ(OfTC&5v@K*o8ObGCh^9s+BG(7-y9)4zfC+4=}zHDB1iurjatxySO z&cplRJXkYdQS~+}6FNk?@~8@>MqEcpBO2yZW4nUg?Q)v19+H6}?I-7yb%-<*SvUDj zeREc#(_n+P=0E{wgN&>2qEBl}LI70&UTNDmp!`$smZQqfRIB;lyMwXz=j$lFU*P|h zMCS>x;SOj7!K`3mZ3&ykgAxQ9=E0+1agK%Sp(oMr<|vE10j+JGPWli#b3A3HrS2e5 z@6~*3So2fr$-2duK&ynD53($wpi0Zzy}37s=bF?JTl1<9_xrlXE&c@CcKEH{jaMr} zO=~lN&-2dh4dz}T82o^B`9tx%@3Co2l~v`-`gw8%nZa`O8+!RW9%4mJAEzcJN#m5^cQVV z(#4UlRohpc_ARe&7F(>Y>O|ip5-tH&qW5l=Q_9jWAmYi_LSj7g^=FTju*^<}Ciy6Q zLO#d2e`c@Vb(sLIp!ObK_Is(K)+qod+VtwAr@0pKJ8b0rw z!ml3Rl`kqOMwxfk$zrUg*3D3&bf)J~I@2YrmViz!F`q16`n$Z~mdP6IjGNoGF153G zMLWTg-w$~?2-3<-X9LIc={?rsLmi8LnW31-1&vbm%n!VR>nRl*J`%4mtXgZ#AFC8v zVMI)bBYGs?f_grAKpKgAAf?D{?_A5H!aj}oa(c=jao^{;m5U{CAxICtbRJA2g18$h zysN8A+_nee(Lh!w%|cHLLG~f+g&IFrZDM%)fUQsT%{rMjq6z{#&k_xOBY)V_@O)A3`)v$^C7oM826n1nyL33^R* zSVTV*d@|FtD~YcCbqM%D?RC-DhmM!ru`S?TQzfx!X90Iel_srEqNX{}yvld)XtWht zM9;8YDGnayYBL+zMIp&7TM1xi1|ES zj9jl=6ngU;L};^r8y_M~S$nXzD;mgd61^cR)t7H9yY3 zDWrcr1akhcF8bwhbucL|Av0H6H4`w|ZG3^>okZOXJ55_nX!}JtXY4}>BgLlYV;x@u zl9Tu{ulWbarL+RWR2$-}u*dnAV{7W4Ish~H-S`+tGZ!@NGC{}k^QzCD;c zCa)FsH`j)&h!>fTe~mCX#!M4urY1AN_@3kuEEyGm>1?cU-Jb_43yM(qDl+y3Ic z?a)cdazA|HucEB?C>kiG+tO`Oh}p*?;D6O|ggU~nN&prZ_Bd?zJaDOcxWH<(J<%%5 z^iQE$c?2@$(Z;o7F(ogE)41tY8qTTw<}8*@nqg$%lrjIx+4P&dx|?B=rikvP6H8cZ zmXZBWhT$O_B3fxu9}3S-H@hOl;m;J+uQy=98Da6FvnW{t*lQ_vz*S|Kis&a$K^UW1$$AA?XCsG~c+KDRyw7 z-@te{+T|gbw%^k4UG@elF;INJ0VG{)OTF(*3Q((twK#Y?|+59$zh_-{h8>5Hu$&XGyaoFS)jUBy&HusPTLr}xeeZ(AAPE*ExC9R# zJa~e;y9Rd}YZ_=8k^sTo-66QUCAhmwp#-fQjkee0*x8Q3W9=DlGhQIyy0e3cm@9XS=Zn)ZZ1wyC+lKS)OoeoA&Ar=W1; z^}5fkp3V5M18xkdFEK85|WztCPJICY@g zl^h#oD@;sPA}s;O)M)Okmc43f8Kx-Rm2@cRAjeUTE=V^S}ZY_ZzmN z5ZjIC51h5vKWv7a{{t+ME3D%Ds zXz9FWyxfU&6m5(Qes)XDE$^Z8ML)AHcb0sF%!&STXTVgH3HLb>Ua%>7t=s3UgA(-? zDW4JvJioCt?t>*pu%?23)6;S&;re;}QF{8=!`w_WPvZGA>y5R|#wv@Yd6r!yG){&R zc@aYfpLsN)W(KFdIT{}Y)xZ>eg#9+|`KEMo2j7Z!{f6woeryQ|+qh2}N;1giUbaGh z5bs~X1g%N^K}{i)D1PKd1o?m1%YCHE6j)hT*;6&@o}OGc}VA>hVYcv zDF30y`4BkkxdPUyI?qMc0hO-V49=#C@ZXbbj6g?0c}tIsFv&EmkC3Jtd2u-Qi$DIT z_sRkMh~9{K@eA}D$)Jw1a6XnG5A&nzmbkIHKUv!*$9|AvM#{;y%S4P zLZp*u9u4d@U*gN`2Ov7#UmvyRp#5&2y1woh4SiS*s?GsG)k3=CsWMb zlqit8>wN9d^Xk>qBW%^H((1z`UUdKTl-g&MD0$?~aCa#n6b?2fX^x~KnbKH>U{PC@ z!;sKgd;)wG!LhdFax?oFM^YNRT$Jdb0Y<*yZs-{tozfrz3BLxgjn31y=y7}PL_M^Rg zk@vX>d%JEOZlIcS#~qWo3n^c!?$Lt*ChkQoSLBP`014VS()sReeJO|ghL3{xf7-HQ z_v@-Sw~RsceBcaMoaf(>K#z~KM8C%801vn&E%B=NZ;<|ncYETAF+AXc`v#Bv==e!L zlMm*gQ*G?n_r`OsXZJ4`*HXQfXR7)X?{b`$D{I+}K$?CeL@wrpjUSH;#p$`E1eJ>e z5PKy-J+il1gC%~472QzvXyAqGB1~5Jamx<{9{EA|iD;Hk@?|CGeL`=QCwZx_z9(yz zxihoLfu|3pXoqNfW$Z!Z+hf-aA!z5@!JTCY3b#=u~ zvx#!ymU;J&P3ERX zqbas(kmtMA$+%zW9l4>*M#?e%R)`=mA&!V zJ%N5pP_R;hh@!Kl4Wa%`f(gkKa~$tzJj%20cps@h7#HI_OroMCSX^e=L?LUKJE;j1ErC?CZ9*#ES zYsmB_U0ltRnE=(G&Xj3Ne4+N?wfe<=5%X}k&NfXYk(0#h8LeHaI9DHAAb>t+0|KDL z{72+G>J>yptIzrv(ZS;cBNtFw3&wr`@5G*Niv;B*CwEC*)qe^aP(gwCadWKCr#{ z&P%0=f}$_6ccvKjAUA-@zVcXm5M$c>=YyAoy8gqcB-=Pg-31~y$7v=e_8Efr&`P^R zjkPfRm~^S%mvN@nyeakhMZ{gGjH)!%8!YcLbCrPtpw`EP{PWe3L?Ij{a((KPKXlir zDE&Yr;O#prL5@_do9$m9QGlY@&u*P8<}hl1=@nA#lnfzp+%g7we?~^y)%l5Iz8nMz z_d?e4Skv&dX2bI*zMhqnrDqj02Hluh|IpQ64nJf!JSHYf!@Er*c)aH>@s-s89yYQuJ4ybk~ABB>9b%oIJYEOhyUb$rm)IzF%kXW%64Vxb*Q^;w3Mr>iVmk0i&nY z`(z$HS8;lFCb|MnGCQV7_0r)RxF$!DdG*i9gC(7pIHD?Nj90}W6Mz;8WfsdSH-Kh= z4z_%cV>P^RWscKxiVERG+uF7tksi;r|ASKhSC+-Vv(cS+DBPe|eN83@X!aV1%^3I) zd$FbDM^Y>QfdCgGCSeeO8byJ`21aGCo!wr{8T+RG!Rj~*-3z0prbC=^Xf~unxFV%x zIxF4Z)9a;@-sU@Ub#<+X9lyPmI!BgA4=dIs7be*&)*mj51XJ>bQ=|41BVXtTadJES z)Y6aCcRWlBftyYjVaQqwx}maxUCYIvakD=$rn8CMlwUqdpVDb-0r}p&^#~>ld>vKt zAIXV+J^nVqg^>FYC01?GjWCeEEir7f53vb5`ZiN+l&}gMO%B?;T-nTbPenlVu>_>nELW(&|otX6%(ZR68{ zaV)kTAaB840si;uMkZMmH(q__1eT~1daqx7=R!^&O#nU)rW<)ol* zN+Y%wZW<%lcVuVYuuO;Zq2DT5|LhaSe_RwmE1b9eD?I1+jcqcio!8z?2KX+%D8H#D zJG=csx+;X|O{QVfW##GfGl{FKZM$hEc(vCUaa}|=J^~4iuFG}kN-R0;~NK( z`M~opZDikCEGkMCF0%OtT8>HY1Bz8j9;PKWRy%r+7@>3qd`;jdfo>CXl&C-?oL}1d zi10AGg!0vb4pEy~AKr#3Yu84eXRpI+M5PY;xvk)p7Zl{tveYGxZF!ZT9*g*Of%6~FT{w>Hq z-`jN~7vvUwphwW1N#EaN{eyQ!{)h7ysndNIgdDyX`;`z1JW_&PdQMiEu*#&?zEiLT zS}YA4okHX|EEjLD>AMX$%FWDiIa@(~yp9P!`()Mg zTFKGMDVy#%I#+6~C-}HfXUp_8zI5`xC3JcBU(0;$5beM&DR-yM0V9rHxM&4fSEqM; zD#iB)WZdPw$y4@|4!!cbzCCPRcaHkqEccPYy%=^qY*l?Xg#|n6ajP$8|B9gftFtzW z#USM$N7Qr@XS^CsZp*{9DNNL{C*i&Au2|@S1R3G4-xsWRTNs&;B1x7)8op5>{-ql3 z$s+5z72STMu;@L4yOp%&Uc=+jPBJXnG0Y3AyUe_%PqO^bO5PqsSm4^YMI8eWQ1m4J zY%^yD;es`ScPRV{eba-8$dFD=c_q|`rP55pW1JORV3E$^Ev?|#boK*K3>y3G4F_v4 z1ClG{?Sdb5&Yy2jt5C!5d_ebSWWIoy=7zXQwMDTBwPnL!EiFr^q%VFAp^}cCtz&>d z$S(xrHlS*ATXt*AwfQ8Ac^3%bv4EX6_wucsErVdDyL6&#oYiVjAk0p+IpV}ru1n*u z*TGBE$v?jclD9ggkz8RmicU_A$(>)uv|lH4`i`;)(kxYEid?2f>7vDa$-}0_`N9%W zI$xvOk11ZjCg5=*pWDe|ZIBof03>`LMev(;>>m4I*dyym;DJvtuz$7^8+Li6<8&Ai zb=9R`U%IT3h(isJlE|ZqsoFg35hufga}9gRW#Hg6_|IBwV_}kN&6)~*(F2(tPBek8 zx<4c9aLmqP7h>33JJJ!hM?b!wZ8~A2h>?eMMP3d0A5+ysNSfdC;KG1}95F9sU!ttF z!~-;nthNH;C;UB-HzPcy@nKqK*S=oN*jTw0j8NTj|0K$tuN^??My{_nlU*$z z>YVB*s)AUl&v6wr?0az>{Q0AdqV(e~AxSyssJR21n3!0=ZcW4Bu^Fj(9WS3kx7nIS zv83PO$DIm+=8hpHL~_CGY^2nc&03sp@2jExXR;Eg8}Qx^^NRhzu>_Wcd->F+Tr_RLvj6hb{(ZyylRIXPc03w)7}Kt zrDGNhKnVh%JbZW#4nJyJRfEF3_kBHKgZh45N+x4 z+l9Vzv8-#Qs56Af(Xgfs?(rX=HQ6iaZjkR>_eJICy=7QY)y{`{V?+p}OE#N-%c>wW z_C*7ew#k-^jZiGto3lIg_)iBQwc++OZ8y@(>kZ(9CNYyYB3Z(%^k`D3=efL8274Z@=gafn0dbL^$BmVT=yoY3TSbN36a9Br!8C!##=pmgJor6dM0AI zw8J%KRdhNm6a6P9x;;ZkfwodPmAF6Cnvv|o2s*m*tqr`O_YTTP`9_U7CI}5SIceYn()Wm)SpO8d*xaouC{bORgXzK;mU}n$cnPmtnJUvBowCyaoVvl(B6O5vb zn;@q$f%^F*Z)9JEiyh_lcOCMX1p#2q{e~jx39<4jk>JVhFE?&7OECr?uxKYK$>n($ zJWjSP1B7k@TSQt<_c`*_y8`sE(2HIppPciwp*a5l3wpt1#l|2QuyOeHD^=`sc4Np| z3qWq_IY*x^aM-o(4lxJ)>ZpgIY1Y!qS803ESFR0TG00gNw8>^4B?(zWhu18nh1waQ zOlW>wtF#!?jl=|a=kFXW{mu0{I6pdRR=p0{`^cS#i|p^=^5F2AW#1i5>$BEP(&}Be zLEzy|ZlbRn)0C%apcvSU^a+Z-%04Id8}E&ve3$-WDDQq4=CwCW!PdOmsqZ=(-Q9px zv&!CQ{rccKS@d3`%)&MgraE_SVcx;oRC@R>>qphfVBPH=rTr9WC)#ifdbe|RgI>Qs z^LzY^G5sKX3TQtrjC^Y!6LV)~M$AaIv_vQGIyzE6ck>;^DFsYzMS1`$jMtQbg+{$S z;mpK*MH&m7&N3|L`HRGUw(I^ zSAhCnwzR0Hm=6>k%BU$x=}6_hqD69me4+WT^LR>!wgLP?=6E3e?GWu>cEqzUdQ!O0 z%03#jI1P1*)lMTTwSC_b$Nj-B))g;hr!1s9*&ZX$Y}gm0VHY8)tHm8Sb0DNUVhXV) z;3Ue_E=OwGoyNXP`I^YBwssG#VPM=5XD2813{7zV+H`PNGoCV`@y*#?%;(w)+DQZ4 zbxJbJns10M;e!rE4E)n2EjLGbv1nDs&Zz_ONnCas7MjWD$UZt*tK$>TrGIC8P$+c3 zl(Chi^wHhei=Q{P=`X~v?`NZ-*Vc^t$^b(seLcJR??as!+x`%{XRJdOhkCSuGbxR~ zREzxW51V$UuHSPantP4{ZMoXFJ~{NQgpxD0)K+qEc_1%}@`{!)cd839hl{TiA)v1C z8RPqk_B&x#pDsA;Pi}hq4-;Jx%Bw5NeF2K@6W5KitJO*;ixhdX95wATEs9HnvXiT= z>OcMdV(Fjl1!3Nc=Su+JaI$*aXDx4uZhRZ`GMFAAWY(cRY~eC-B?)jx{{xPu+3e zp#@l7puZha!2OXVLycmsD9N|9f1vaTt4amjJBT8@L+;#-$phofUnCg50qAhrb-ab! zTS+-+Ytt=ix;U1dP*?C(bI$lKmuQVLHHT=boAEAR*c&f2<=2!A5F@*EIutqZQHIhxE&JjJ>s=+V2$*cTlD5f9&!6!+m{)h7)zRnj9)we_`clrbV6-ZtulsqF^oLi|R*i!2lSF)Gh zek}6OHjr9~q=ps4js9D<>T@{nrxML$o65x9U2B4;s;ABI3}q4(sTt*xFB(Fckzmaz z-v3>M|1QY?&Y=Jk%9mbo+~^%5CTUQ~LHXfFB*n@9_8w%U#f(>(ZZ~7@wP}n&$(4$9 z{?4}>R5J^sDBsQd0_91li3Qt@gt}EJw0(sI8|*>@8)X;6-k?M|M@xJ#f*IXvg+45G z>YN6o=dZIG_P#)o5tgCm{5{oR+|a&%o~^+TYN>;#m5z1uEI&Z7VbV{JR?Hy5V+rI_ zq9Yq7F{T0@Kx=8ztPQOTB5m!EJ=sh!_E?Ze`0|n#^G>b6&Y7gEPFeF+e@WRLKwZ3t zlr<39?LEj#qk>+Vj^*+_6wq7Gf2-{?cdl4>IR;Kbl_`eY)K&42)7t`ef3Nr1N1o5TO4c+yuFIZ_kko zHBv*YGy@NLLJ3$8QgHCzBDHd3Yh=!0Q|<$`Mb(%&+xq##g zi4bB=^Zu8>!lzXK=Q2Kav!ym=C)2)Gwf;Y8HxRV4YWo7e;xw{$lykgFDDlV~R~EGI zX_^6xG!>yL=vY^HHDFB{C`$JG19Q`j?qEOd3Zs<*tuj4uEkA&Pp^#z!5rT>wqQrD2 z48gae@pAFynH5f5n-DmibS8zebz?yf*Bs;F$(gZO*T}(u)mPh{qoX7E)&O2%|D2YV zmQjgTnXP_{`|(R8F8BQE%58S0^~Y9$Quk85LkpAb((Umy%^m8I&R0gSNmjvFuVoH*!L^-G@*f_|{FC@yntF zXCaiB^k`cf3;3N$qMz-9N9)&FEie{v3MerU>?eh)N#D^$@z0KC*{#RaK@J%z9G?uw zEC2MT79aZV%m5|6cuhI)RyoGVSD=xu8uGT~S98w+g$9CYpZEXS(c)Mb--*)J7vvi{ z+`P8P5T!hs^LBqfgxMT{E~Bk5c$@I3qUGFv5W)IhL%tU9R`Dad}#IDj0 zMbg`I9b=KuBH93?egB^{tN&N+C92wg*0LdkOxjE(l7w3tx{Jp{&!X&A}7JcwRS7iQ~RvTL>FZO4P zy{o|WD~8xm_Zf46H+Q4QkB>iTWmP%OZfeZXPDr^IdX-c! zEspUETDMt!`LUzq#fPfpyvPqv%Q-msWtwaec78wD*Kv-wiC-Y*KSQ#{rCY#>u%rM> zO_NP}9A$HmX1;9X8z}&BEeZv06ET)}Y{gx;HJDOAG-YUbTx-q*>`1PV8Bngq63&7z z4|4qK6ke7Ta@B z6Xu=e6AlIGWL3q0g({8)5Y2*PMbf6Xp*Bb%r|j~~Y)~hWd02gQdpA_RXKaC>=wVKG zAg*F-0h2Cs&9v>b^)r8sxxrFyNtv77sJ`a2ralFRP0AWJ^o6-v(?o^IW6x6!INw#7 zh`@mW06^E0SQ9lvUKl{i;1i+5xE(d@4JdA9)qn5wx#sbgb`_4Guzq;Chn4a$knqml zYPOXBVAa=Zhv4GkS`ke!;I$4+KYXIB;Fyxb_H@XS#?E{J-&MG$4$D1`TaWTw)qdP5 zyYG8T$L?Xtel;94F^TkJ0edOkP08c)*8PfuVrcc50DSRG8mdsrTew&9g$GSSzB+B{h(8s_|7lo zZ!0)2yEBUye8lt9d{?kn>@+t$medP11IZ6Dof1ZOBRPWC1$%+}m6zg$BbujWvEj>A z3OvQg@2xgPTuw!cIYc(;=0qufQ-t*sn`NvAjBUAUhB!_h&Nz^aE;?6=)8VxhCU$A) zV)#eOxf~r9dGQLTdI^YTw#-?7sgV{eB;Z;!)EnFds^?cv;Qe44s4S0UvQE_9dZ0LI z)HHfu%Gt&%)c0qmLV7zjljpu_zMxR+FUwf0;CitFXC@wjpL@IQdoE}t&1gKs=h5gA zjix;L7wLfx?I(te2Zv>pDJ@TGR!^fU@pANHMnppA+9?eU{XU|L4<~g_E%nqH?4Op^H75NJJ>nz(|)?H8cW#hn*-7uDbS=h+ZNNlOAQn zF0E6`CLDa@<*2Y*ty#)i7Qet<0=mAmo$G28r%$dcCDR+f_O>E*1rvIF?&UrBtlD5k z9P2%XRAB5ba#KUL0M~vAKIhxQRAsx6Ai!n@+G(81tD<2TH+P1wut&i)JN?3GZ@qA@ z0vq4One@8#*?l84o?Vas^@NOGxqYI|`cyy!$6Ql%22<_G6YSXa6(!Dr^6h9uh@G@} zyl!_u`z!JtaD-+&57g!9wx?PwCN$}=TCA-i(lpX9LD0@3d<)t z6}K>7URNd*=xJ)d?(UNy*Z>MxxXPaz2mDg#%C+=!Z8*N_=LOj zm+&h0W(b-GrMgD&7srcm9h#`L*PJVywsN*48yG(4Wj}oc30E~sOBmtv7pBJFn%B%` z%x0P2@}s0jR}nWEfZ5ZAO+MTOy9B{}9}DB0@$k*apBJ>26b%yYxynP9aSSReCm@0!EJHu#~hfT zDQMo^iwS+kkg>dR!|Sx9XL&cb4Di7ZX{=s$OsR)P%vaaTJ6gSn>;Xxhw^w19rIj3P z`t?EU-vyAtDui#V^eTcj?XRftO;w9x5i|PQS<>T7I7_s+X@x_PsoI2&HIDjD+wq_o z%5VAaC+}b2tTNH?=3M+;TC-lw(-;f_JmAR5MMA`AMBrBv-_JN036EclnT=JsnA(3m zzwL+P&iHUF0@*Y6E>_VT@>Bma%~ROZtn zuSZ|3RCERDlSQrDkjbH%_Ugx)kjR!&`GwRm~~oA+_Jz$1$!};X?PZm&PWP6gq=tn4-PIx26$z zn%;*(j2p}CzXD!RL^!N2{}1hH*M(IHHXF8w6UpcbX3`SOWFGgi|j)j6G=?{AQ9b z2uN93sPC*fZ4_&QVHfF`-~R?af_n^<2kNjkmKrvzX{B151{7SFw8P}Os_(gB8r`$Q*kRkJNVGjSFD}B4587~OZiU=;$(;+!|Lm2neGe^ zj)L3ViBL}xeEhN2asi*@A502Nv!|Cz3zhm1X_sXjf^6I z(2kA<=H0PeiTmH@gXAqD`>Ls-e9=iOJ!{Zp+~tjiH0HcDgv8lk8#{(~8ev^VE>9UHtfG)q6T zuqtCHyxMLSCz)FIN7R*ny4>hP8d?lm-*L>P->1c_(8;PPio(iMHnSBZ*>sDf89ikZRjblo`6;z^+pne9d}K;!Bl5?4&GgqCx-4Cqj2qe3a`ygCIyRj z3y?0sY$jl{CL2=EzmBN*{D6SmG9BEXEzI9QM0(1+Xq2rXFZRi0CGc4zO7!aAdbHoS{PAO>|LGAh!67n0$C^6GnT!|h$0#X4_bb0AUflX{VW?`p#>1vEE8dX;H6$`Aqv}dRCw0y zco^%q!}&)Offi|VqKg7^q2Q)!UgLK4{5u0ngnJUv5MfW0elb%fLhW@t)0%t?u4}q) z@KWtXADVBlGRV^YO0B6FA-cbxczDyBb}oNmLTw8Dn=?^Ons^;fc2jw9c`u&e`p0jCzvqQ2Bs_>1FuY%Wp#>R`UK|tT zsaz5|*WWsv;S)(PAh*zp?&`e}2@4Y(y~Z?uM~fpPn&YiP9n3l39Gaql%pLx(AoG6@ zHUF>k|1WwVNGu;2SNd1E9O>GBb>c;Gf+G)W%z%H%7)WAp9(*L>IP&;DtAb2-`*-Mx zQz5C5k;ng+jXpdxN@bd~D$Q^cZINBjNoAWcA_9u`84Y^&y znf2~jp3Az?ZW+PNsYvr`Oo_0cf0T_7l`(cKm%inkI!HV~=b~sN>)W>mPi8dbW1Vjq z$+Tjz%$6S{13G2}Kig^bJ+h0t78{f7I3lc75+6dt171%7tz|6j54Zk$`Lk6ao&~)M zc>O{1Z6fr9QsX>OPrbod`|agtv>G>9(Mlfl<{*BMjBO*~-qO8KL77z{>;b>{+xqEs z??rMSd7|B}GJ$BLo$IlpG^OTaK@^Vn$lmJcE32m^-Qn6&!PB#aa8ce7hmWmlUi;rq zaojA_o<<>sPnX=(nUB*d?R#)&`XxJ8f*N%ZkIJ?qmj^hr^D7{RJ7rSa$D(ILO1dq; ztk(VVtw_26fuRKz9+hlznhCTOJB$`5BadkE*|Fo{Rp@n6{Xn&wx3kpDgxs97b;u+3 z7klLEC{nl$y~Zj-uN7)u_v?o{tg;Mx;mw8XU+MQaC%>oubW)V`THV!-QgF9bIYiUZ zt@QFwM`8KHba~9fY3V3je+rA4U|B68MBo>!JhOOt-mDU#sY8qux^F?l+O%n6E} zI@YmJe`Ro*tJU$oq!rXlqq~IE2J4(UQVk>hkXqA1SUNL|8xD)ePr*K+OgUB#!W#`f^Oxm(SbPYIeE;1^A{psU!2 zt;RoQ#~%)(l8W5ax>dMV0GH}Z7He%A%S%HCgmrV!DdYxDpP)yfPswwx3(d!6>>W6| zcw{|`k7ma^$9%B86*HyBhXle{H3QW)-T3@8!xsI*fLpKRVzUk_!V^VSy)JEF39b8I11gAZ;-v_gBha(=!z#9Qzq+11pU0E{tYvz8= zfEQOTaFaJl7dd^~!0A5wqOb;~cmgY~IB)KByZOKpttEodlg4lGUZs0S&-ZfH9^$)i z$5KJajjf8dlrdJv5*Rn*<=ggi>8FQRQkme7{^yBO_nF3No}Wu(7-`P;XJ6%?I@FX+ zJst3$WY%>Uc@wU-XR%i!#;>~pG*V%wstYEOKHa~oFyG&Oa~Y*>LN#h<^tVZi6_}`& zpw}JBN*Xy)r%-6TN$~6Cz{k>yS%Ey!BHaSpanMw% zMw~O*~nQ`=HJXq{;p4nD z9IQ3&#=lPdi`zk&QU%ax`kEl?t@Vpuqn&W1Z-%A8vU|UqlCTwEKg3mZS>F^Z5ZcsP z-jk}itEE*@5*1j~kxvJY;ob)KtgrK+&ZIfnC|G+NUv0d*xT<|jsVYP(C0=s=eh0j9 z)St~rTl!tnCiapRs7Erj&Y4%R8Bs0*mb+e;Txj}YW7kg20-;sq;J0bw?D)lhg`hB};e(dHWT?cr{ zRbg+zVsT#F=(>ZuW5MxW{@K(fMD7W4-COlc^Ma)N@{dAt^knCmy34iZntT`O5J|n` zEcpJjF%`9Cx^V0H-o7uga_Gr)>&^uLS3&cScx?J+%e4*PPJgC|YNQ_}??SCRizj@> zujc(-^FewzNZ4O^##oE}bVCXJVzKvVd(vYVO1$ZXU6wlcAUAyd?!>jvd$;yU7#y+q zxI4rizyPpW1xQMzV6yO-FP52U-tEkak{=4uv|}9YpNM4NY-m54-EgujKi-JCBJT3V z_gSP(MTd~XvZb;bL9_dwRAVs*mB3yxE{0TCTu0#no=C9KBB(=q-4h zSiJ(d40CLLc-qZu;ASE@+q@#Ck#hUk(9e;;Ed0}$tR_>m^3WKfExJbl?0HE>u2NjN zf0fvg);QyB$zM|z9o%Fz`V8CgH);}uf=Q-XfO|Eo6OTeUhyZTIh@oxCU_Px%? z{AMM#*k%V; zx~g3p89t>nAg;X<>qk?|n`c~vlh1^5(ZS90MN*^XyI6s*TlOGiao2q;N!9 zfW13cWjukqQib+MWpEz_O7mBwO4e0cc?l94JDe-Fh!+yRU`jfj_$uLiDQr1GW0fC^ zz8dr}vFM8B<+gL-?~<>8C(y#Ap1k9DFo$jXaU-$IgCh$`WYgjNr_XkT|(~BQAeWCU`6ER@=WjqU-dyy19 zWF6oxR;9=W?&3_w@+}p5bqJX0D#LWb1?Sx250CesqM>kxc#b9F)dk@ffg$X^hQN;d z3i*?p3^m@>glB}Bb?rA0`b^FCBdGV zhgZ2%>Zx5 z5d7iUjE-+Ezu!Ukj^5{^NIO)Y)ui&?BsJu8!zvZ=SPI*Hkuj>P!GFK#$k3VUy-(~l z{9F3k`EY3EHrGGZ=k3#^s;v&HAtgwyYd3>lt;=c~Ceww~s9>b8FkY7xDAh;d4Iaog z`jLWtL6l#RIe8UxY_AQz;2Y3M$Y57COxG!otCE?fgGSR1F&Vj*I~JcLm+fyOe)@MM zQbhJz06w+ucelibK3ONl1zHRazCs*&)@o==!)GM=zNs#Nrz;=SwJ=-x*=@o73nOsA zEnj8PH)`ZoaYDtBQ)IE3DFKzl09@PA-g<(zom#?;esE(mLP^(!#cp7y9d}AwQV==3d0%(g=o&lUaPudoPtlhay#USjuoyx-g*AKqZEd2I7VC) z=ANqa;dgmCq|SR9*VsA}NN{4T0AS-V?9z#{8BNR5JAg&h^8HAOLWNOVgF63vpV)E2TkEX_v8AUe#Mp!+{me~Rvk**M0O0*8 zZ=BNcP8wIZEKhD`C@XwZi3zbhr3!=*#Zg%J6UYW)~bh zkN%ePxfX0)^=P$WP$oi6t0$i9+A7Pj$|%kmh+vg(gbgAbas`^cVYuw$6XtN~%A6UN z$e}FW<#d{@u8+7Nl@0x(Y@z*mm0aDKYx#7_q8PDzIk{z~6SfO{FuskAUqCHAJHTz5 zF_>}jP90d->J*i&N#sXTtmWHIGXD76nOW|3bbq>(nbTm2eahDkZOro_e(Y?_TMatT z?;G9swp^L({p7q?(Ib4yM_x@EX(VQ~n5$4c(dm~fM|7;{fF!OYPX)1>YB?N zNd8dRoefh=*Ef$jq4P}Hm$98c@lv|Z0QIjT9@r;DixJCX|ny$z_k9yepa?hOsiX+C@(^(nx*)aW>G zRomtFM+i75=@UPC6_;)g8V<5J{*n-!Ds9dMof&0;Llt_78{b9C)wdX(Wc^4KKA7ib zx&_Rwv>&(Mc+eWpPB`pTN0{#H&PjC^nb!^R6l<>TrBh>Dk zYtK^f}BIdex z0-=eA<5>lS4@n;ey(fK)0kJRORa8nmaO-;|gw(fW$U7w(iX6B2x1G4Sd7YlD1Ue=; ziL&e@)_ghNT8^Mb6t`5Fstn5}ZI$m#G^>wUblli~?)aWBB%4^nVEEeuwq{rlNKvdo zowq@9)KJ+Qlp51QgyOfr4(<9dDdD-&%XhE1Qya?jdo9L$of+zUPi(~WGvfznkrdJ> zw8${TmpOCy^*V5(G1X{>HGY}!w@`SL1;vW*UOFUCY;#pWS*a@X_VxrUtx=_@gUiXt z!{VwC6J9-%YP#C$o5s)8JGRA?GnQf8zKc&qJN2}N zvcM%n6GA_IU>qA{@re__X5BD8iSYdad zB%*vD2fg*NOK?M^wc4!O4GW!^CX?oXkEMtZw~#DuYgc+BtK%6Kwenbzg(r^CK5 zO{lwEErh#aa%JhOY^I#)WM@J@cRQ@Hw*;PD6+vN>Z%j@1Ik;PxRKC*Gy$}?N>weq| z{*9p7yr=Bs(ndsFzdIFLAwM1JS=l3{zb*Rv2s25xOJ#O52=ZVDKTxs67MKOL!79BG zBR6At!!{*}f5!~+Z;s39wUY%BeHOP8CofYTI310K+p{Sh^~s@)r4y~hT)qn~%$3&D zK0bZY2_LO^4s5CzL4D~@taGPc1A6wFuk9F062-c2EkrwZdn=x6T6QYLrkGTEI3G)% zNMJuXXKJRB)s2;3;_&CxwP2ln_OwxhSMR9Bl$Wan1oU@p-f5+&sa3A4hcR)%r7+U=g1u3Pogbz=0ua8@69HG(Y)tn7%ZHBOZw z3!Nmdh{Hu|Q+i`^U3Xm!-AErJxjHy@QEkv3zLjIBUoE20sk_z7rCkM)F|~aL7N0F8 zy=nRy{hg#KJm&UO-4z260Q2A1-MuWHFzdtx7K-@6k)r%^bf|YcgU5h>clSKapkkM} zz;0J1F=EdPL$-?*B!^a9-X;E|X=jx7jDljek@G!-)|c6O2@b?!_xj;lj<0PTN z%09ihKORc_IryGQ;AHF3x+e%6;@qQf!{XTDvjnFbS>itkHuog>ak28G02!K{zaE3A za(Yn)^e;T@3S*{SIZ8Uj3pRiSie@S;X6Ag3$Ss&Q_A11Gk(D%fKmVj~)AgDsV5m2Ij^S&bDAK3l79TTtFfJBZGJ*0RJgwad#aqvNc+Udqq? z+9g%7PV39F9(h%`;^k2V*e63EJcV=ideP|2_RYfrCeudk z%RmHV==ZnX|Hj^1Mzs}vZ-c3~&_WBv3zXvCLV@BGcXuuBP~3vF6e&_%f)-jNXmJe` zDDLi1+zB2+NHUlH{%dB|yXM3H%RB3R*GxXK!sT%8J@@Rh_p_h>7prtl^fn%!b*!dM3 z;6a(-JA9GG!a6uF>iRd%j0pSfioV-EN$K3M@>I}?gI#A>`{L!w&KCvF>AE5l(+rs? zYO#+`s9SQavwL3sv6e1UAOH3Zg|avtyHg%cZo=rY@TY&=4pcGc_#u1|}`*Y8MnxeWX=$JENO9hR_+zpmO3$nARMm)lzC zlqcn8{Ppwb+1+}P>W+|>Xz{y@%!isPb@M<1g8I7wgAQ1cy-WTSATrHGgAU^_8Mt5GO?VJcMWI*i>x z*yepS4+{2UU7#o-GcsjA(|n~=k4P|G;VZH;M77u4w3OFLSeZ@b`c|*lhNi$WkHoPm z8C~vm#Gk4fJcY6-cAe&^<0(lchCdx2ebVEr1e|=Qc`j~=^aw45SbW2-gr=2sPgFm4 z8R@%OfB27COu@W4KFIL~=H-f);)lz5C2q(Vr>{&Ef?I_jfJ4kDFc|9sBBx~gxsBoS zvk*6A=ob0JwW$hn^>FK?UO;5P!fDl!PABxB z1+)F^O}VIJ<(eLDUTyhkcWHJex`slOr8`H42>Ip`oowbF zqV{%Y(^^U1zB?NfI(NN?Uw_%?lr?mZkxv)XI6O@?eI`xBv&bjU8Thf=)(uNKU3zW! z;7%_h{W#?6*+Gf}uOj(G!SeEh2Z3&ZmBtOf1Dz<#P85`3RdNPo$}8ur$gBkycgId? zPkT5SF$&rBIZy^Wk^gIw*@q1k#L_cvgGj|g1EBVh`z_Paxd@GL z95)1p|7|wr*tr{padcGkM~?QNW_b*u$B#F1)UuS0z{+{G#BTcr(-d{??V|q0*aI^< zf5O0?`A7)wweZojNFX)U31*!BEWfA-##-KdGCY(_oolRKuHJdV&T6*AgXI^fSsz-2 z4Sn@BF)-=fOyrrZEbWaFrr0{as;DkqO)oBJ7cs1xd*f`FvW(RiytX>Rww72O9D}iw z6-4NHOEhdWD^}Sg43D@;0K?_fJ!mPj*Nv1iJUvhKO{U@17dkv@Kv2G&$GZFN-}=t} z{8f;qoI2$BzMosG%CbYz@31Zd>^|>z)E`<}pvXvb`{|00nM7~kgi-t!4T;a3;(X(N zwlPdWub(iGtl9YDFE#%C+U;u#|61coeJ4G4?A{$-7=8@vA`_yXRW6!NBNG~iC_R{O z42L9oawtspKU>(d`8ZyNuVV$*uR+&pspMXlEG9VOwel_0ZXzUA2E?@v;KAm62+oTX zJ+QT{C8T4Q({GXw+yxP3lgN_O2u7vRr7_Q!vfe!Z5&UJs9p3hhb~F! zM<7Cp6XIqKcPsK$LoGnmOAR)ea}BE7qREl_{;*D1C`9&CSh!u>P;JxC%A%I_p7h^2 z$=KS`cTK3x>CDzMU%x+AzI74Y1zv>QJbH9LxB?C5n(Qrp;AmIAt5tG!un+0m=r(7) zvs)%SmLs``wm1ITY=VL%9=y%5%Kal!Rz$9^yY)iKh4=zsdI$+v>k6IAva>7dm5eT2FhqFrJ!^ z%0AQd0wUBoahlt*8+5eOL0yD+*h%ltmkMZsGqlH^aB*_2ZiB7YX>d0@F|a%WxqdJ+ zTd&n*Qf^y3cpzA9s77C~QwsMl%x%$#EWj?=t6iJqBwkZfeKZzw^DL*gudQpi)&~2T zxIETAtby0=UXDjB3GZ_BSaSrTgk84NKG_S#BxN1ols#XGpH7N)s>;WVd#yAklxBrK z9KY=1_A2&kRe*nTyYF6B%~bp;{9Mt~&>}ex7W(4Ij*D&;{;_;7d7gM+xBA0uqq3iK zAY00DQ{M-n(9WhB>SphL+C0CE zj=5Q%^qv+kq&j}4qgThmr$t`aF2QkhjzT@+Nk1ia$g$OCGXDZ=F)?hGj*`sz1g%H?PuVqR=rY*O1-C$Jy9)Hz8=5tAhpI8wVu9ofn2K&OBR z_40d6D&ns?Ptfo1YIuhHdGGowLQG6z`(=O7(y*dvXs65LHi8C(34JbWK#ek_g` zVMOfa3MWaHI#C&q5*B!b1TLuFT7Rr7!p(S0pIwMv$8*evKXO>Pyt-_UH)r5@(Y(=< zM4g4gVre3-l5=KHo_niS8uuFUMvxA-US^%A>L;&I=!+a)Q)l~;UuL0IbbuE|9{`+X z&CeWgz=hboM4w&d=?XA|2BdzVa!gBEmhf-N@k_mV-)6OL~ zg8w+7xCE!J8HqYcUJNwhXbi-j5%B92-(FI>Kb&25Sj{uMttk$@^2KkkNSiOme3#;$ zH0m4F$gkuO{Cm9_b_a(}sN`D@rG%fM>Lff)Jz;v(GsU3MkB|H{irS+flb>O&R$DiW zc<;3fdMaG`$8l+Tvmj|%U&i_|9j)cJ#tys^buaS4cPzq5Mj^)5Sm;Qx+!*g~ud;5< zipOd@tM!CQo}vf~J%N^YKXz@Bj$+k*77h2o1F7w?Gk0F*0ny~Y&wrmw5z7?sm_E&@ z(r3iAKae<#bAIJQ?)Lm^PNRM%TLxwJ@CZ2z_@Enz94Ri@NTV@ZGuL>qjufJeR4L_YR8H!?6NC6j&KT2kAbH6R zqgR?6;wRM~^Bwxy+OPTr1lo?UNEYWu(vuB>`@i9r~@?nE@r2TfYdrI274(XAm}?!553l5I(yaYv+RO z45AcwN_s$itMF$~G2w|0v&jB7fAN$lMOL<#{pQQo(!D}}f71tyxYR=_E_f(3bpS4_ z^kJObuDK2?y`c2HXt-7x5-l4x)*p9G`q?55(3OmEnr*rAd`EfiD5p)?*IwDe6G|z! zr0MA7jl%AMK-IzGlD-z|U0z*~d*__iaF+%vp9-1s-3W~=+QkQC51*CojetO8di7^u zt>7-7xdUUk^Kmddc1J1H;&iS||Fz8Za}Orru%hz0w*yU!k_4uW5zm1tsuwuVb`l4; z>SCHoy2z2DnB>9t;CabBEec3nfIQmW`>+k}yu%db54=dld$1I7< zs$f~0g`*=NCgF9|ZVf=sIc*L=TBTE=(W$pUvK;-%?0fb0(`Aj_-zbZ93zRb#LqkK~ zH-38r0)c_`7s!PhB5dnKJgg%`I;@pdR_j}@Px{$Bo{6KqfWh*49Ov9Mrb_g(83PkR zAUaWJXXkz@VP7^??7`lieSQmz+4=6IBT)VZ0$mb^JPhv$!`FfEIWMK4+>+$;N-X=6 z_v)qYfMT_)frfcfva;a;>wAD^Yqir<$*i=~C{T-fp7S)W4eH!$v)Fi^mU_UblEv1| zP7VsH7V}r=8p;&h>vVumK*!bpuZzzQ7g5ioBxvHRxa9r( z7JBo;g$CiZ-SQTxA6muQ_A%6=#_!WzZh>NT-#GV%%lSu212vjce0CEy#en7$2z31Y zY25ox4+Uw(-7U$Fk@rC_c_Q|JK?U5me_aoz^FbQ^js6K3oj#?%!vyA=L~qqZCs*_^ zp2O@m=q0m&=YGB7VyDAa)ni~5K(zYL(2tPg09AFsN6a4J75)2^0@2{VAF4zD|BL>2 zegv`Nnzx4cyM~*+QJd6{#Jr2O74488Xmn``TULk8BJm(B1V{HUjR|P zJXt_p>>YyRMzh}DK6T|u<260$5ji_psr3;4-1s?2wl0247ah3r>Hf5e@s%S6>J@nJ zAtBUwx~L{?rVsC@?h#;G)PV5oxpm=nj(JU&l~*o7OOtGhUT z$h`*;(ZG|E>$2cF2E!xb!}hUB^N$iup3>r9x?TwEcc6u6NfsK9)E|zkJc$pehgF?I z8!b;1ku(YWKc_k#1U;s)?Y09rweN1$iG)tD;(U8S(AUgxLK~>b=dOB<7|;Uh@|ls2 z+6KKL&W0MQx?7Ecp0Z_3DafLqFRG`Vemje8%6e>xqff5P?Soewh`mAdx>v&N8lJLpK0TqB0!T z?8bj8P~xse0;F?5BdPaq=J)*>fEnBUbY3xFvETk$`e50!vGwThn_=I)tD0VCk>&02 za;w6h-@S(2%4fY@&XG^!jQZjc`rM42yhdSEjW==C61C--r7E~L)N!w&*s+4A2|O0#eaX%BZiwv+L?T* z11sD{zo7+Nn|bld7m;C&g9jFVH7{O)Ku|mYI}MTT`c@wH#HAY3@^w4J+O5BCqdS}f z(oR4P4$h=*K4RV^AvH6#&Bd}+0P8ja=2F<;Wyl@ zu=$IxwIsHG)xU5ZK~;Rdzfgu2{xhgMetZkm6Aswtom7@r-AgVPJK1h^#$?2#q-?{R zHmnXM178e*hQm?2kfY-#&xehh35$Nc&UO**<6}-1Jbh0Y`ULa{=OJEQR_(@Bt#H9% z3LiHQf!=}RmxkG`NP;v(`m0T9Z!j;LkYRHmyjY}#+0DADQD~?8q=rrD$M-^X+Vmx zEIZ*da5gmp3=P?^uCj(^#BZM%^1R3`q^6B5j<9e8lQCUN+bYa!E4u+%*%gkL*I1yM z3W}xMo#!OSR?N#Ri!J|>6uK8)Kck!eh*d!&aw_nkg}X>&XDV-DA5kEd3T(_n831G@!#egYW4=wu}8ThwW3(R7SJ@u;A7k!Zj z7vYCZ1(t=JSO7V>)ep~l8l<^4ay~8LX$0msPCp$W87{T@n%834F*-GHis3F7cjDmP zN}yo_w&~rQndVy@4jAM=2#r$uW*dU?7c$KIwN@0mVMNl&mvNPoy2iWYyBvzRp113O5hGddnw77 z(BOKn5q;RQvA=i?5eJC&a{q25V_(r(hkdj9ysv{K+Ox9(uCDeT}j}C?KP8WvrzAo-0$l zYr6EGU#99o@J}Bbmd4Kmr;0O2j#QTJB?`HoDq(x3i(~vau*FG!Rw0JOKTR(|W52K* zuotI8jF;mHbQ0}}T{r4KQxy>kRqLs>it&kI1$^8)Jm@1ie3fOc+027@cl@-xiq+9s z1^hD^cP4-&l8$a!4+p$YI!a)FXBL_8qOKb)4T;O<0emJLLoiftT1LTg>eMYj9Yh&$K8(FNpY6EdIZ;yg+MQNezT9l~ zhI?4XkWepP-y{l@ZFU|By|^gCRLEz(b)?WTMr{eJk$1YzPU#gnJ8 z0>?Jsfo#WO8IUKE{-venpA{KGNP3fy^^F>>R15j*!~QWG+u0-DfQQQW-}X?0WaF<$ znBw1lR>VoUD;fr@BtguLFMG-Mr`v8NO{NYGwScgfD3t>(E#}wkp!LLYY+t$sniV-G zCVS6B-5|2H*&D$1JJdJjCP$DKWEiy|aV_dvoc^9SYO@85mFOnKhY5<{65vT|Zz3WZ z^*K4SI>1W2=8y+E;=tjf^sg$VB3832dDE*u={2=>*%#7$C$-SKdP@lJ<+Ikz7{n6p zQ~r>Jg*w0D7l%IQe+Z3a0G;+Ryc>u7<^80At;KF}lEYRw_~Mf@%4?;4tfy~P`8Z>$ zKVs{n?jo(qRHxySPZP34M*~jR1Ce#q4^8u&txo3jRZcK-8uYk$IMpXOnpRf}AD$cM zzYke5T8P^04^a(p(vXKc8QKXrZJj1Pqa}ZsQ&3&|#=1BuR@L0vk_1gvZ3Pc3byD)sHhXgQ7aHs5*NRa+5tBq%dWTd5)>L|; zDT!92oo?S>H7hI``6(Y$dvS75ajtc*wMw5CyMAmwj~Mj1EQ>&;Mr^jSd{IZ9ZMS`* zRe7NJyw<6%+oMm5H`35MkWztJSkQC3PE>gjBIMh!-*d^)AV1str@N;mr~Ht}(37#G zY5+PjOFa90-V9Ru{I5^x0^DBc!q2)F{T+gYEmCAh3iQI(L9xXD)-HR;0K7d3{m5Vx zcI)Tc-rsXgt^q%4x`!To3pm}D@X5y644sM}5H-l#O(51GNLVFRNY&pJCET^Hl8@(_ z(|X?rLa+HzMfr0oo~KQ?lR9vG{k^>w+6Uhkyx#h$@Tmjndj#Qa$r$}hQ4D8SXp_1d zr1_tqjO%&caFqiE$309g2gf>dmStWZ-R)IsTE#<`RmtTyj3Ua!#IxCtZBweGtsqCp zK$TwXN=?z8@mKx)d$`VJPJnymp?E$?QcRpFWJ#DsSI4qvH#bB6HTyclGlg1{U(E)T>|FBd{<| z%H`eMz2A04u{KzkHM4BeOlIa)W-y!(HV=%`0#Y@^x~W|wq%;T=@gDS z{(VlIcbXh$UT@4@;BHOwzFs@J-S1J&DG&YKTEWEB!o1G>RcyQ7OPo@V&P`&zWy1vz zDoa|_YQbBLi4~-!oRaknZ|;cc1GqnMqpBMB_KX%hKW^>|=7>VpIU1eS>l^@CQ4^=O zOV6XA6o}4?bF>0!Q}eFofW_{#$#SeFBtl!ivA$539F+>_To!wkXPiq`52@w+ISaV9 zyC3xUKVji_oC(oNv*pkYPRTnoe2?Gwcnw?b=K5`!QEHnKYgWmv99}S%*VN}FT6&g4 z;MO|A$NK%x?ueyH-AoJ2mn7dK?cBVB92K2-W9J7*ermcxo?qR_Ptp1}q?D~nbiD+$ zt*ChajOtg#MT@g|+06WMRquitpsrUN%|47-=NGWe<(sB=idV~B)q`R${%yQvrT8zE z{`4u@Ri4YD&#FwSxMl-#AAXuCeC3&`QMc@#RfF86i?N0~C`LW`0{t4N%C!E<5c(Vt zF>LSV6WfUG-JhkVl&@jkPTNT>Grtc4i~ci6dKSYDu9dmz2WRGWOBc}&{ppinm0rWC zNDoqC^_=e`A5TrJvjNE}j6yfq=S7Hw_PmPGi=W{f^X(EbF(TIp_xg~y>%+<%p2(fO z-r#CdF3?LdV7{?TE4Kpl%9>TR`{O#m=}C&0y8=`os75e>9?HajfAQK}%trg1o$P>x z-))CKw*j$GV26-H;WfUqB+gSZ1%KCrOrmv(-zty6>PxuRHI%!53#n6MxCw? zunY`Dy-f?v$})V;p3!gdcfovq^SoSpls*!fX@LhAxb~ZKD99oYfNo?wruBU)(+(y* z*kq0Hi~^Q;Rz)HCQsIRQZdN|kdlNstiSjWEouO#&fNTf>UxF6*yh<0loIr^$E;aS# zzE((RYG|P%zZFO}_NoL?w3fw+;_K0v0rbim%GTcRn%Wj&74zh?Y$6O-SQr zAQP6LD8=D3^T_*7L>`p)gU&41b_!0fgWI`E-<^0ba(cKX0YXf&!t(3A&EpG_@aU8} z6DzAGLA|R1G{J5J;BSD+@It;Dzld;0&X3zV4@NJ+D={ z$2U&~$TdN`r{0_&q{l2AkUb^cPn}$1=v_Fs* zro#mmtT;wkkF3UV9=WA~+oVpFms_g0dNECX^lZ*CqHh`sQAtT1Fp{Odw^ZuEi%B3JYNKphslROzISPIuK|Glk99AdI7 z-~{@O_%~>afo$qny{u<3&n`!$<8)^V;T~}nbqQvklWAaTLiJeN>)ETC`2L^2h_y}z z)V&(N-2;86ys>7#y~cHVZo3MmQijj*1}=BxzL^Ki%zxiI85S4OAJ#i?f#*EseS}|P zMZVCWGz31pYNxfl1GwKik8YyM!rNK1(b+!77k6k%R>`H8C$5!-Ug#mT*ptllpHXyL zGMiQ3WnjJMZOn>5k;kBofvRqM{etP=hQ4kI{O&)^d#g-}e*WegUc?A)ytyn1)5)xB zD+=cbGe9j)`o`N4soPpLJ0^A*BV*K z0;M(BQLAsc^@0P2-9?3L311udGKWALN;fvl__^7kiT&)AiImoF`mKA#O{AchY?7cB?SwEn6f) z`l8InAJy>yvtJh(49|Z&9(X)ZJU5GiH)REqPrTNZb!8PS=B9Vm=VaY9tNirpW+Soy zMD`)5f~H57u3J@*ko<%7vE+;1H8ZJD~Cu zlbGxFT0$E-TXba}A-IKn&(44U3PQ22KBfKIx%fe z4(7-nJ-hNrRYflPhsh{4KY!PrW4NJr%A$KVPV#D1Fe`TFE%5ezo#iyAVAsSPoz0qH zL@om4!xtFprBb-G?5vWhmHu$6#Lh8EoC`095T~T4`)=}*WbHC!*BN)>6*bL->X4!) z%WRg%ftGJ- zrR!hLel3;RS{fx>%dM4nl}d$#G+38aZdfbzI#B^kTS8toke!2UOtrb+#%Nz7@6oB_--{6=p&uJOW z?+82wktO9+iYKrLoB0HcGVWg6%w8vS$2czYZLKu19GbJFVT+#9tgc?cBCY(G1a+U6 z{<0H)vZs7MTV>%oTiw_5Y$Qljc{%f~XWkO#Yo?LHeBUa@;m(asxqRtM z^Ae3sTBfdIW0+oj$)0I&-wG65BR#puUvz7H_(mq{$5|YNec$en-xPT`0~EEPQ}i97 zO`(%DgIN|-TVe^UC{4I8pl=m_%1QE2HeHuc8i)mY`57h-*ZG&R0ynNZopOD4+sd6`%n^yW%3Yy~ zbT0H)nT0#)UwC%4YkY$hD{1v3@d0aauNRMFzJYzpdQ7r%;NquWal2Ii9~ai|3uB!> zmQ^_JX|9-Fi7ZhYIW2=nT-Ttej?G2HJFjM|jc5K5sfwCM+}lAzfD5A@Zn1yqJM&a# zZ#}y*tI{St)R7o>lmS@~eRkM%q0nHf3Zr_$I;pS;08f_#|1p|84UVrF0KB#0S4D$P zWA5PgSl;qbhTz`B#;IRSv|ci_E4XZSeJ_DlACv~1Rbu7_Ln7J8O>@GxN8JV?c+?W?jXtl%v z!`hRDQ6hHVz0Ueon)QzK7ct_J#59lF50vNvc<#vF1ME84ABA#I-5{e7P^gT^zrHy{ z+h|OEL+?j`{3sF_El=m2omF{Jq5zO-EC;cgZRT}jbZl^AwVQ{Lv`pS8vN!1rtFii| zfO1ykf80sQLJ0Yu`y?=y`&T}=8$#Y^@k*#F?^pq`&&x=jGW>?eo?$$!@f#soLR`xN zD6F?_B}gduUj687cQ-Po28RxRId=jL7azpLZCS>+;|XHOpTCd;YUhd|p< zpUY|)$&+0mkdC%h-^yWK(`O?kGBxrS^D)Ev?z^(X)GA-I#Fv?VFR8=h9^$AQznuj6ct8Ai|A6J?ONg9cm zV~rA-;~3jkHe4Y+8@t~zGsvCK0Qb2P$xM$jtRiG}R z|L0SquDdUh2Q5e7KQr`<(oI`#1bGo#${)QUiUa^|H<8(P5m}z1Wau9-G#BTw05k^> z8WpbxsgvDc)f09DODS9-S$T~Qa~^FuK+eJF+eJK7E$Fu7iYKlNZ^2>m9PEsaFgFc< z1?hybZ=&{ZH6*#s_o62aVwM~;Q*TQ;unj0lVy8*cA(7Zf+2e$t&y zB@-$!7zo%di{$JscJA%jvS~4xesfFKnBtpX6r=%jeH7p68p!gRE~%p86O;bC0=YW5 z2Sd@jhK(M_LY`q7FBL&h(i?D*|05C^;|#lIk0eq474VdkTf? zk~Igc%#I>#jAPu|q5h~z9U9-Vr5G|-T){qOE^;v}H3e{mJplFmwnFPSXlAwhO)^s@wfPW7c0yq%ZCn_79)FpQr!KIj1({LG$?*0X;s^Iz z@LMrpKRIt4l$b36U5a= zW=Sa4b?Zx)rN%o=5cK)Yi(2`X$J@)s*RAS%?CUj1I)}=V+l5Qh!tBA)E(O^f>)Bb< zgo)jxDAd}C3D8pu@iweS4hT2FI!iC2<ew?Eti1W-=CMkE8x45ZidRM5UpO5KuX3)zx+E(Lr z!j+pM&_h%fpwfWIR4v4hY98i5sO{^uW3C#pW30O-U6(D476B=*kx5vm660s7c{9I5 z_2jMMBJmp|=`DNdi&}`M{LOy0W!fu~#=P(aq;SoW7S@{qJ=A z@|+!HK;e@A;IaQvFzKf#&3h~SY}CZ{%SX$FgABY@e3{Yo_(mZB9|;r$z$xxh&y4Fm zSCT^j?BV9CcmHrFS={5d6{*Y84GsO3LUDBAL{YDioyPM$0sQdmw8+aWCxV_KIVi z0R(00F>roLUr>xL49@mRV!v{%^*>xfep%0GJ{YR$nf7(n`><4EtvUuEGpk2OYxLE#q~Pr^PTTxdGho+ zSW%148qm3(w^4WS476_WoRONMxT)~q3I`Hc`syNEB#gnS!7aVrd-r1B|AU%G-QH^d zvD!TDJ3R+meFg&+qt1IT&w=gn()dDKdYXU=r?-84f<)xC5HoypX2|2t*15y zlk-hS7?utl&i89M=KMh*K4$bqHGwY2z1P82-k$;Xy=sF2*BrK`hH?g)KlJ_W@;Q}i z(I(+I6gn+M?~q9^sHfT$mNT$yn^ma6C^H?DhNPP-Gyyi4*c=d<_I#g6LG20L2aDFy z#=gcSgjo|UIuzX8$Py-JR`Kx$-2FMw_tUWmdl^HFq%qlfKY_DY zrZ!F?mY6x}mMX`+jC%{`_f(hwv9EtDiJOc52l|$Uh3PFTETtbmLN4`}!oXjSS;dR= ze#sB=H=IJyOEfSg3yc3-TncQ{b_;evbu< z)buL_7xQu)Yw+05+8T8M*~I^1!Tf7#W-9Tbl!wy9G;=K=yNq9+!CLX=_uuXRuKUCdp+=m(L> zBWRzQ5$oCD%F$$z`tTwADT|hD-YaG769=v8s)mXd9kYV@+!BqtbQLi9jcJx~9JX}! zjJ*N8KtVG%9vL+Db)HMBPl1zR!>N$p(Ei1%gy;q)y#cnew(~n*Y%b?pw?u`&XI$0B zkbT^%I<^Z1@l(~FyuZ9enRA>X+ZF7UN(^!ZBw7VAEVVn>|KQBMLV`z?XflcB}s#JXz680ZsJq!80QS7gG+ zk$Fy{AtBYg1{#T~s;b&bU{uM7AZGW4`kZc3Ok@@p?@1VS-vf!6l^DYsm4J>DI*_!q ztI1*=XX_jK=5&#keN0@O+V?W^U{Bzb93wk-BoJmeKn@FMpB5OrersxFN4Icl&F3rx zMs)}W^7Wcod_RhDzD(Z8Xk7B$)etpcSUbIOc-=NJ zF{!$t#AhHPt03>$xyDEl@>LzlbO`zKtdKA4+ z@p$n}d8M#`$(p@98NSayxVty%n%BcB=J+V#~449#(WQMp#$tRkHQ-X+Uq)T~JUI z8_@n_xErABzSYCQGmUwSVLoWwn2x;Q@yl#ncK0MpC>^~jUh3nUa?xJ^$GD&_9th2` zW81*zUW53(5x{-o>7{PBiV9RUtkfrM5mPpw{qj(rE|Jb>2z~U;I+sxxe04VYAPL%5 zES!-3R-J^W`YA_{q&lN>FE>4;uV{B+O$FU(iGf)hXWmot63O=6)13_9hYdU0r+Z2N zsENp)gL{vA*b2{ER)%YG_-sA`^=z|f6}f9J()a)QSM5{ZdN8{`IXncNbC9weHtI~b zPF_SWXfVTcM@!_2h2K-2d~0}nuuMHL&Co9tcO#3qGxN)nzNx6mi~7z%fph&^pTc)O$<0x>yGY?&7q^0;u}W&s6w!dRx-G zO8MBNa7^1*jJRyk3x8^A?7K;5bHkb8G?0gSO5K~AHXYBMuxs4Uy=@j*6zB|d^ZB?V zdxQy&)T?o`MqT z^&1+A^8XQtSq@B9Ta9|3W55@=nRWb88p*}5npE~f z;zq#{6uWX)!K*-PXA?8OgS>#@m7k<(H5BL3iGO+h)G3%_;Jln0V>A2NQUy}JtsA%% z-MO)A6@Pr!4J5{rU+zi~;C|8g;~TvJm1?zZqg!Cz694lp4}eB-lYk3)+%1+=unY~! zLH}u=xgu}Ghk3~HPpJ0;;Ocj>Nf{*6H0eQ&(02y|&!I`tsi_)cmg8R=|HVE4y&ODV zoG&c$FLgv+Th`{<%rEn0pI`CY%PGv_8g++!`RD8dJYRk*o;s=%jpIyScC#P|-X$my~gjgb_tC zx23h=yoH3WQH{oBgVIsX(P&g>42jORY?r0ZTsaR-jO?}}|_LNhpPCrj)( zBn!)Ef!>Ep8apwq`}^DPg{MQ#tC|Vka%j-pImXPJa`kxfm0r6f)v*+mb@aU6qX2l3 z&tJuY+o|@3i&NeFhr8M32aR~@4J;q*j24?~$b0Qq1b7~9;LGgW!e1*N&Wj0*3h?FXU@*)**;h|nc`isY<@0L2tvJZgA$ z2HKz?Cmq)K$Y6!Y3&cWGdG6a-yYts)!_M76AAVevJIVHkkmmDGg$}z5YAT@~q%M4% zmL>C{QT#}5^U_SMa`g94xR`x?vzCf(@-UIoZq8;sudPnT^j+RxIDu@dCPM?`)GC}y z^wxn@dJ5T8?4zUaJ&lBwHX<$cSS|P5C!PC(kz1&lD50fsfA`6I`F8|{GezCSc;*kB zFQSWQpwnGs%2iIvLYGy0soT~^iRvTs^Sq4Cm&6YT4@<#_p?+y6+w(Tb3+)D&(bCS7 zy7e6)@7c2@BL#gb^hZ0-3TnZj3~cS*3f5_`bfH(AfTn_dZSCIqoF@8R-$#dnT-?Xh zB`*8Z0qxhtTH4Ft`YI1fKZw`SO4gw{0kw%mDx3)G(Z|;?RQov-bBP#kiLQLGAjDlI zGi}Vu-Wb*7Qah(g;= zQwOQ>pUJ3oIz1Df=09Va_MPq$XI&CrSS(AHFss!tOQwATZ}{^kW2AX%)3K#U{ZLCT zz?pTKg-)|a=8H3n=@Y$;qTh9U#nWCp z-saI3T3a1is7QrmzGs}l&fi?!yiDBlmymv?1w`lb=|^1IZ?_NCF$4O0*9?|vm=+J+ zJ9QpN*RS1bI!1p7ZIO0Kq~6<(<-vl=%0(0N*OL*7$1Qvg%0h}CTQ!976pnPu!JIJ& z-B`{ei};u~j}9LY36g=q*84<-Z=adoaxJfeJ2X3#d@hMv$286!uJ$JpN1n;gU1U{x z{;t2mvp42|E;ozS1+%i9On8qt?-ZE?vO>L%`nI`RlP||DuCKcb}*<0t+}SJkpq33Fa`gXA{tZ_Hq=10x~kMq;reOd)B7xlm8LD_rHo=}baJ$|_o0*V z_~oBrDSPENRJ@y0)HS$E<&GJtQWem;v)@a4dA{I?^6+8@{Q@cazQeiY6;oBC$W>_$ z=QWd|D{MWc(gP{VTE%xb>avzG=Vzcf%w`-?>NM}z2PHVo_CE8ZBmp;T^je(;qdbIN z$|qYlqWGX&J5z2OFa^(YV=JdQ@M-J$FnoW9uJw%P6{K%uY_40AM%?m#@(P4Y4;CYv zA%Vt2Ku1iYbNym89_TAEnKy5Yf9K$)qN=HfCp8#v8Lb62!W_g+J75*;KRkjbQ5VBZ zD2+cjj|io3vgYkw7a83aX_^WQ9SwI>T3e_Sh?lJ~-KZqHCh_C(VQbIo%c{gtPY;8* z4XQ0a4iZs+@|R+%^wj~USvv`QEtXeRy1jdmgu0u@9D2hIbK!hpyzC_nboMf5f{YQ!ps6yzCJaL9T7=`gO?5E`~r2E?Um8?xdvHkTlt2z20nHeL7Du=FrlAa zR~=~kYrU4kztQG`FW@$Bk$;bR{j;WxeOx5p{J0Y!)5u^3$ zi@Bskx~FO(jkqXuiY^c3PFfv?T5#D_M|6Dw8Si)YVqo}_6-G9KIXDA}2a+?#9JcD+ z8h=cQdmV#LJ@R|q^@c$W^?pSWRT^c15jQ1pa!lXJOnN`SSvt3HXkuy0yVrXBq^ew9 zWxB?;9G2I3fH>J{`rv^bn?kKWOV3kHR~9^pA?J66F9v-U&M@R)?6$UbdFJ;_V97CE zA>*54#ye(1al$L1>XEZ5)><@hmN}`0i&Y+rE;U5Cby9}yQTr?$@4AjtWs?VUq(AO6 zI6F!t?dUrvGF4BUR>NvT+v)RE<%(F=ttattZ_mMY zd@I2)%dACt{@#$*Ryrsh4OBd9xfi^boHL(7wvoBxa*zx>7Ko(^VNFTA0EgBefyi2% z5P4`KG<}_mdC1R;=;}n3c^6VTO!(YYjwvMnSgtbsxLwMoo@L2g0ldf8W*Oh#PN;9y zTUj*yd8_C%uE_bLxTVE+sn%-KyWA*_^TVaJ_2A6}`W>y${RP)T=*x@A89FDNA0P7)MycTx1Fkm!;q z|BW|G{W189Y-+xT|3`aQ7L^17MVp$kGi8>VTB9l1}nep8ni6VZZJsKzcObCrJx*V6u^_46qjS5!!-#FjVaG;N6yQuXSeqm7VOcuOs#r_V%5)*s`R4e zrQDIoeGM5T3t(E_no&uWuKuQQ|Tik{X(9Y$n4b>f^E3QLRnygET0D&HYPX?`P z$?O*1bBb~hJQD^eW^0O(HggJTFvxdXlX}j*g<}__>{Hri1;^iAbN2J?nWmSprrHU^ zR{T(i!atge6eJRu1Qy*R02S|nm1kO|IY@WKz;EY$z?hYy-0-kMGe8D#jW$S>&~Hoj;j zy7HBE6SCY>4y9Dpk;4LRrSBe+t5V76gpU6!(jya)mWnV>8C0$?rIwx$qyWq4&6MIGG2>C4N(0ST_sd z(Ka=H2CSM*gmgFz2ri-@D)Mf)CJuHg0R2TMO|#Tba{&SPT$KQ;4igw!tely7Rz+qq zt2lvGqF^9blL@7>f_%0uO+mbVJ)xWf(3BT}9q*Olo=n>fB+B>-56g;W#Muo@{xGJb zP>6()pOq^%JTp$j_BO=D$C*FQMl{+aH$R6grMgRw&pgK)#tg_Q1Jz%$msjThwpyiZ z~ literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch new file mode 100644 index 0000000000..af5a15e896 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml new file mode 100644 index 0000000000..9699c5caad --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml @@ -0,0 +1,173 @@ + + + + + + + + 0.0 -0.785 0.0 -2.356 0.0 1.571 0.785 + + + + -0.022096 -0.753529 0.007612 -1.716165 0.0063464 0.962660 0.771123 + 0.2 0 0.8 0.924138 -0.382057 0.0 0.0 + -0.022096 -0.753529 0.007612 -1.716165 0.0063464 0.962660 0.771123 + + + + + -0.061035 -0.301221 0.410282 -1.665777 0.120602 1.389535 1.138547 + 0.4 0.2 0.7 0.924138 -0.382057 0.0 0.0 + -0.061035 -0.301221 0.410282 -1.665777 0.120602 1.389535 1.138547 + + + + + 0.0875228 -0.540765 0.770139 -1.990062 0.366596 1.582282 1.560837 + 0.2 0.4 0.6 0.924138 -0.382057 0.0 0.0 + 0.0875228 -0.540765 0.770139 -1.990062 0.366596 1.582282 1.560837 + + + + + 0.164988 -0.805223 0.304894 -2.020563 0.230321 1.247100 1.238191 + 0.2 0.2 0.7 0.924138 -0.382057 0.0 0.0 + 0.164988 -0.805223 0.304894 -2.020563 0.230321 1.247100 1.238191 + + + + + + + + panda_arm + panda_link8 + ZeroPose + P1 + 1.0 + 0.2 + + + + panda_arm + panda_link8 + P3 + P2 + 1.0 + 0.2 + + + + panda_arm + panda_link8 + P2 + P3 + 1.0 + 0.2 + + + + + + panda_arm + panda_link8 + P2 + P3 + 0.1 + 0.05 + + + + panda_arm + panda_link8 + P1 + P2 + 0.1 + 0.05 + + + + panda_arm + panda_link8 + P2 + P4 + 0.1 + 0.05 + + + + + + panda_arm + panda_link8 + P1 + P2 + P3 + 0.1 + 0.1 + + + + panda_arm + panda_link8 + P3 + P2 + P1 + 0.1 + 0.1 + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch new file mode 100644 index 0000000000..4c43af1420 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_deprecated.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_deprecated.xml new file mode 100644 index 0000000000..65d6b388ce --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_deprecated.xml @@ -0,0 +1,434 @@ + + + + + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0 + + + + + + 0.5 0.0 0.0 0.0 0.0 0.0 + + + 0 + + + + + + 0.0 0.785398 0.785398 0.0 1.570796 0.0 + + + + + + 0.45 -0.1 0.62 0.0 0.0 0.0 1.0 + + + + + + 0.0 -0.52 1.13 0.0 -1.57 0.0 + + + 0 + + + + + + 0.0 -0.63 1.4 0.0 1.19 0.0 + + + 0 + + + + + + 0.463648 -0.418738 1.03505 0.0 -1.45379 -0.149488 + + + 0 + + + + + + 0.0 0.278259 2.11585 0.0 -1.83759 0.785398 + + + 0 + + + + + + 0.0 1.52001 1.13 0.0 -1.57 0.0 + + + 0 + + + + + + + 0.244979 0.333691 -1.48422 0.0 1.81791 -0.244979 + 0.4 0.1 0.6 0. 0. 0. 1.0 + + + 0 + + + + + + + 1.32582 0.333691 -1.48422 0.0 1.81791 -1.32582 + 0.1 0.4 0.7 0 0 0.923916 -0.382596 + + + 0 + + + + + + + 0.1 0.1 0.6 0 0 0 1.0 + + + 0 + + + + + + + -0.463648 -1.64103 -2.06227 0 0.421241 0.463648 + -0.2 0.1 0.6 0 0 0 1.0 + + + 0 + + + + + + + 0 0.4 0.5 0 0 0 1 + + + 0 + + + + + + 0.0 -0.0107869 -1.72665 0.0 1.71586 0.0 + 0.3 0.0 0.65 0.0 0.0 0.0 1.0 + + + 0 + + + + + + 0.300000001 0.0 0.65 0.0 0.0 0.0 1.0 + + + 0 + + + + + + 0.0 -0.010787 -1.72666 0.0 1.71586 0.0 + 0.300000001 0.0 0.650000001 0.0 0.0 0.0 1.0 + + + 0 + + + + + + 0.0 -1.56309 -1.72665 0.0 0.163561 0.0 + -0.3 0.0 0.65 0.0 0.0 0.0 1.0 + + + 0 + + + + + + 2.61799 -0.0107869 -1.72665 0.0 1.71586 -1.0472 + -0.25980762113533159 0.15 0.65 0 0 0.7071067811865475 0.7071067811865475 + + + 0 + + + + + + 0.0 0.0 0.65 0.0 0.0 0.0 1.0 + + + 0 + + + + + + 0.0 0.3 0.65 0.0 0.0 0.0 1.0 + + + 0 + + + + + + + 0.244979 0.333691 -1.48422 0.0 1.81791 -0.244979 + 0.4 0.1 0.7 0. 0. 0.923916 -0.382596 + + + 0 + + + + + + + 1.32582 0.333691 -1.48422 0.0 1.81791 -1.32582 + 0.3 0.2 0.7 0 0 0.923916 -0.382596 + + + 0 + + + + + + + -0.463648 -1.64103 -2.06227 0 0.421241 0.463648 + 0.2 0.1 0.7 0 0 0.923916 -0.382596 + + + 0 + + + + + + + + + manipulator + prbt_tcp + CIRCPose1 + CIRCAuxPose1 + CIRCPose6 + 0.05 + 0.05 + + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCAuxPose3 + CIRCPose6 + 0.1 + 0.1 + + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCAuxPose4 + CIRCPose6 + 0.1 + 0.1 + + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCPose4_delta1 + CIRCPose4_delta2 + 0.1 + 0.1 + + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCAuxPose4 + CIRCPose6 + 1.0 + 1.0 + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCAuxPose3 + CIRCPose5 + 0.1 + 0.1 + + + + + manipulator + prbt_tcp + CIRCPose1 + CIRCAuxPose2 + CIRCAuxPose2 + CIRCPose2 + 0.05 + 0.05 + + + + + manipulator + prbt_tcp + CIRCPose7 + CIRCPose8 + CIRCPose9 + 0.1 + 0.1 + + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCAuxPose3 + CIRCPose5 + 0.1 + 0.1 + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCAuxPose4 + CIRCPose5 + 0.01 + 0.01 + + + + + + + + + manipulator + prbt_tcp + LINPose1 + LINPose2 + 0.4 + 0.3 + + + + manipulator + prbt_tcp + LINPose3 + LINPose4 + 0.2 + 0.2 + + + + manipulator + prbt_tcp + LINPose1 + LINPose1Opposite + 1.0 + 1.0 + + + + manipulator + prbt_tcp + LINPose1 + LINPose1 + 0.4 + 0.4 + + + + + + + + manipulator + prbt_tcp + ZeroPose + PTPJointValid + 1.0 + 1.0 + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_sequence.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_sequence.xml new file mode 100644 index 0000000000..3b87ef0328 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_sequence.xml @@ -0,0 +1,227 @@ + + + + + + + + 0.0, 0.0, -0.4353981633974483, 0.0, -1.5700000000000003, 0.0 + + + + 0, -0.016763700542892668, -1.673499069949556, 0, -1.4848572841831293, 2.3561944901923377 + 0.3 0.0 0.5 0.923879532509 0.38268343237 0.0 0.0 + + + + 0.380506 0.731299 -0.960008 0.0 -1.450284 1.951302 + 0.5 0.2 0.4 0.707106 0.707106 0.0 0.0 + + + + 0.927295 0.708913 -1.343015 0.0 -1.089664 2.498091 + 0.3 0.4 0.3 0.707106 0.707106 0.0 0.0 + + 0.927295 0.708913 -1.343015 0.0 -1.089664 2.498091 + + + + + 0.588002 0.221990 -1.742660 0.0 -1.176941 2.158798 + 0.3 0.2 0.4 0.707106 0.707106 0.0 0.0 + + + + 0.0 + + + + 0.03 + + + + + + + manipulator + prbt_tcp + ZeroPose + P1 + 0.1 + 0.1 + + + + manipulator + prbt_tcp + P3 + P2 + 1.0 + 0.4 + + + + manipulator + prbt_tcp + P2 + P3 + 1.0 + 0.4 + + + + manipulator + prbt_tcp + P2 + P1 + 1.0 + 0.4 + + + + + + manipulator + prbt_tcp + P2 + P3 + 0.1 + 0.2 + + + + manipulator + prbt_tcp + P1 + P2 + 0.1 + 0.2 + + + + manipulator + prbt_tcp + P2 + P4 + 0.1 + 0.2 + + + + + + manipulator + prbt_tcp + P1 + P2 + P3 + 0.1 + 0.2 + + + + manipulator + prbt_tcp + P3 + P2 + P1 + 0.1 + 0.2 + + + + + + gripper + gripper_closed + gripper_open + + + gripper + gripper_open + gripper_closed + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_with_gripper.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_with_gripper.xml new file mode 100644 index 0000000000..cece30dc4b --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_with_gripper.xml @@ -0,0 +1,95 @@ + + + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + 0.0 0.005583 -1.323096 0.0 -1.812912 -2.356194 + 0.3 0.0 0.5 0.707106 0.707106 0.0 0.0 + + + + 0.380506 0.752267 -0.595009 0.0 -1.794315 -1.975688 + 0.5 0.2 0.4 0.707106 0.707106 0.0 0.0 + + + + 0.927295 0.607232 -1.173008 0.0 -1.361351 -1.428899 + 0.3 0.4 0.3 0.707106 0.707106 0.0 0.0 + + + + + + + + manipulator + prbt_tcp + ZeroPose + P1 + 1.0 + 0.4 + + + + + + + manipulator + prbt_tcp + P1 + P2 + 0.1 + 0.2 + + + + + + manipulator + prbt_tcp + P3 + P2 + P1 + 0.1 + 0.2 + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_all.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_all.yaml new file mode 100644 index 0000000000..bd165e6ebc --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_all.yaml @@ -0,0 +1,37 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# 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 Pilz GmbH & Co. KG 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. + +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2 + max_trans_dec: -3 + max_rot_vel: 4 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_only_vel.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_only_vel.yaml new file mode 100644 index 0000000000..4ac9ff1ae8 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_only_vel.yaml @@ -0,0 +1,34 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# 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 Pilz GmbH & Co. KG 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. + +cartesian_limits: + max_trans_vel: 10 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_valid_1.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_valid_1.yaml new file mode 100644 index 0000000000..dd953add3d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_valid_1.yaml @@ -0,0 +1,52 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# 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 Pilz GmbH & Co. KG 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. + +joint_limits: + prbt_joint_1: + has_position_limits: true + min_position: -2 + max_position: 2 + prbt_joint_2: + has_position_limits: false + min_position: -2 + max_position: 2 + prbt_joint_3: + has_velocity_limits: true + max_velocity: 1.1 + has_acceleration_limits: false + max_acceleration: 5.5 + prbt_joint_4: + has_acceleration_limits: true + max_acceleration: 5.5 + prbt_joint_5: + has_deceleration_limits: true + max_deceleration: -6.6 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_max.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_max.yaml new file mode 100644 index 0000000000..7aecb01bc7 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_max.yaml @@ -0,0 +1,37 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# 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 Pilz GmbH & Co. KG 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. + +joint_limits: + prbt_joint_3: + has_position_limits: true + min_position: -1 + max_position: 4 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_min.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_min.yaml new file mode 100644 index 0000000000..50612ed284 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_min.yaml @@ -0,0 +1,37 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# 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 Pilz GmbH & Co. KG 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. + +joint_limits: + prbt_joint_2: + has_position_limits: true + min_position: -4 + max_position: 2 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_velocity.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_velocity.yaml new file mode 100644 index 0000000000..8373e05b28 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_velocity.yaml @@ -0,0 +1,36 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# 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 Pilz GmbH & Co. KG 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. + +joint_limits: + prbt_joint_3: + has_velocity_limits: true + max_velocity: -90 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/testpoints.py b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/testpoints.py new file mode 100755 index 0000000000..e0056728f3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/testpoints.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# 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 Pilz GmbH & Co. KG 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. + +from geometry_msgs.msg import Point +from pilz_robot_programming.robot import * +from pilz_robot_programming.commands import * +import math +import rospy + +__REQUIRED_API_VERSION__ = "1" + +robot_configs = {} +robot_configs['prbt'] = { + 'initJointPose': [0.0, 0.0, math.radians(-25), 0.0, -1.57, 0], + 'L': 0.2, + 'M': 0.1, + 'planning_group': 'manipulator', + 'target_link': 'prbt_tcp', + 'reference_frame': 'prbt_base', + 'default_or': from_euler(0, math.radians(180), math.radians(90)), + 'P1_position': Point(0.3, 0.0, 0.5), + 'P1_orientation': from_euler(0, math.radians(180), math.radians(135)) +} + +robot_configs['panda'] = { + 'initJointPose': [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785], + 'L': 0.2, + 'M': 0.1, + 'planning_group': 'panda_arm', + 'target_link': 'panda_link8', + 'reference_frame': 'panda_link0', + 'default_or': Quaternion(0.924, -0.382, 0.000, 0.000), + 'P1_position': Point(0.2, 0.0, 0.8) +} + + +def start_program(robot_name): + print("Executing " + __file__) + + test_sequence(**robot_configs[robot_name]) + +def test_sequence(initJointPose, L, M, planning_group, target_link, reference_frame, default_or, P1_position, P1_orientation): + r = Robot(__REQUIRED_API_VERSION__) + + r.move(Ptp(goal=initJointPose, planning_group=planning_group)) + + P1 = Pose(position=P1_position, orientation=P1_orientation) + + + P2 = Pose(position=Point(P1.position.x+L, P1.position.y+L, P1.position.z-M), orientation=default_or) + P3 = Pose(position=Point(P1.position.x, P1.position.y+2*L, P1.position.z-2*M), orientation=default_or) + P4 = Pose(position=Point(P1.position.x, P1.position.y+L, P1.position.z-M), orientation=default_or) + + ptp1 = Ptp(goal=P1, acc_scale=0.3, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + ptp2 = Ptp(goal=P2, acc_scale=0.3, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + ptp3 = Ptp(goal=P3, acc_scale=0.3, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + ptp4 = Ptp(goal=P4, acc_scale=0.3, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + lin1 = Lin(goal=P1, acc_scale=0.1, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + lin2 = Lin(goal=P2, acc_scale=0.1, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + lin3 = Lin(goal=P3, acc_scale=0.1, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + lin4 = Lin(goal=P4, acc_scale=0.1, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + + r.move(ptp1) #PTP_12 test + r.move(ptp2) #PTP_23 + r.move(ptp3) #PTP_34 + r.move(ptp4) #PTP_41 + r.move(ptp1) + + r.move(lin1) #LIN_12 + r.move(lin2) #LIN_23 + r.move(lin3) #LIN_34 + r.move(lin4) #LIN_41 + r.move(lin1) + + circ3_interim_2 = Circ(goal=P3, interim=P2.position, acc_scale=0.2, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + circ1_center_2 = Circ(goal=P1, center=P2.position, acc_scale=0.2, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + + for radius in [0, 0.1]: + r.move(Ptp(goal=initJointPose, planning_group=planning_group)) + + seq = Sequence() + seq.append(ptp1, blend_radius=radius) + seq.append(circ3_interim_2, blend_radius=radius) + seq.append(ptp2, blend_radius=radius) + seq.append(lin3, blend_radius=radius) + seq.append(circ1_center_2, blend_radius=radius) + seq.append(lin2, blend_radius=radius) + seq.append(ptp3) + + r.move(seq) + +if __name__ == "__main__": + # Init a ros node + rospy.init_node('robot_program_node') + + import sys + + robots = list(robot_configs.keys()) + + if(len(sys.argv) < 2): + print("Please specify the robot you want to use." + ', '.join('"{0}"'.format(r) for r in robots)) + sys.exit() + + + if(sys.argv[1] not in robots): + print("Robot " + sys.argv[1] + " not available. Use one of " + ', '.join('"{0}"'.format(r) for r in robots)) + sys.exit() + + + start_program(sys.argv[1]) + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp new file mode 100644 index 0000000000..2c9b859e56 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -0,0 +1,1430 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 +#include +#include +#include + +#include "test_utils.h" + +pilz_industrial_motion_planner::JointLimitsContainer +testutils::createFakeLimits(const std::vector& joint_names) +{ + pilz_industrial_motion_planner::JointLimitsContainer container; + + for (const std::string& name : joint_names) + { + JointLimit limit; + limit.has_position_limits = true; + limit.max_position = 2.967; + limit.min_position = -2.967; + limit.has_velocity_limits = true; + limit.max_velocity = 1; + limit.has_acceleration_limits = true; + limit.max_acceleration = 0.5; + limit.has_deceleration_limits = true; + limit.max_deceleration = -1; + + container.addLimit(name, limit); + } + + return container; +} + +bool testutils::getExpectedGoalPose(const moveit::core::RobotModelConstPtr& robot_model, + const planning_interface::MotionPlanRequest& req, std::string& link_name, + Eigen::Isometry3d& goal_pose_expect) +{ + // ++++++++++++++++++++++++++++++++++ + // + Get goal from joint constraint + + // ++++++++++++++++++++++++++++++++++ + if (!req.goal_constraints.front().joint_constraints.empty()) + { + std::map goal_joint_position; + + // initializing all joints of the model + for (const auto& joint_name : robot_model->getVariableNames()) + { + goal_joint_position[joint_name] = 0; + } + + for (const auto& joint_item : req.goal_constraints.front().joint_constraints) + { + goal_joint_position[joint_item.joint_name] = joint_item.position; + } + + link_name = robot_model->getJointModelGroup(req.group_name)->getSolverInstance()->getTipFrame(); + + if (!computeLinkFK(robot_model, link_name, goal_joint_position, goal_pose_expect)) + { + std::cerr << "Failed to compute forward kinematics for link in goal " + "constraints \n"; + return false; + } + return true; + } + + // ++++++++++++++++++++++++++++++++++++++ + // + Get goal from cartesian constraint + + // ++++++++++++++++++++++++++++++++++++++ + // TODO frame id + link_name = req.goal_constraints.front().position_constraints.front().link_name; + geometry_msgs::Pose goal_pose_msg; + goal_pose_msg.position = + req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position; + goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation; + normalizeQuaternion(goal_pose_msg.orientation); + tf2::convert(goal_pose_msg, goal_pose_expect); + return true; +} + +bool testutils::isGoalReached(const trajectory_msgs::JointTrajectory& trajectory, + const std::vector& goal, + const double joint_position_tolerance, const double joint_velocity_tolerance) +{ + trajectory_msgs::JointTrajectoryPoint last_point = trajectory.points.back(); + + for (unsigned int i = 0; i < trajectory.joint_names.size(); ++i) + { + if (fabs(last_point.velocities.at(i)) > joint_velocity_tolerance) + { + std::cerr << "[ Fail ] goal has non zero velocity." + << " Joint Name: " << trajectory.joint_names.at(i) << "; Velocity: " << last_point.velocities.at(i) + << std::endl; + return false; + } + + for (const auto& joint_goal : goal) + { + if (trajectory.joint_names.at(i) == joint_goal.joint_name) + { + if (fabs(last_point.positions.at(i) - joint_goal.position) > joint_position_tolerance) + { + std::cerr << "[ Fail ] goal position not reached." + << " Joint Name: " << trajectory.joint_names.at(i) + << "; Actual Position: " << last_point.positions.at(i) + << "; Expected Position: " << joint_goal.position << std::endl; + return false; + } + } + } + } + + return true; +} + +bool testutils::isGoalReached(const moveit::core::RobotModelConstPtr& robot_model, + const trajectory_msgs::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, const double pos_tolerance, + const double orientation_tolerance) +{ + std::string link_name; + Eigen::Isometry3d goal_pose_expect; + if (!testutils::getExpectedGoalPose(robot_model, req, link_name, goal_pose_expect)) + { + return false; + } + + // compute the actual goal pose in model frame + trajectory_msgs::JointTrajectoryPoint last_point = trajectory.points.back(); + Eigen::Isometry3d goal_pose_actual; + std::map joint_state; + + // initializing all joints of the model + for (const auto& joint_name : robot_model->getVariableNames()) + { + joint_state[joint_name] = 0; + } + + for (std::size_t i = 0; i < trajectory.joint_names.size(); ++i) + { + joint_state[trajectory.joint_names.at(i)] = last_point.positions.at(i); + } + + if (!computeLinkFK(robot_model, link_name, joint_state, goal_pose_actual)) + { + std::cerr << "[ Fail ] forward kinematics computation failed for link: " << link_name << std::endl; + } + + auto actual_rotation{ goal_pose_actual.rotation() }; + auto expected_rotation{ goal_pose_expect.rotation() }; + auto rot_diff{ actual_rotation - expected_rotation }; + if (rot_diff.norm() > orientation_tolerance) + { + std::cout << "\nOrientation difference = " << rot_diff.norm() << " (eps=" << orientation_tolerance << ")" + << "\n### Expected goal orientation: ### \n" + << expected_rotation << std::endl + << "### Actual goal orientation ### \n" + << actual_rotation << std::endl; + return false; + } + + auto actual_position{ goal_pose_actual.translation() }; + auto expected_position{ goal_pose_expect.translation() }; + auto pos_diff{ actual_position - expected_position }; + if (pos_diff.norm() > pos_tolerance) + { + std::cout << "\nPosition difference = " << pos_diff.norm() << " (eps=" << pos_tolerance << ")" + << "\n### Expected goal position: ### \n" + << expected_position << std::endl + << "### Actual goal position ### \n" + << actual_position << std::endl; + return false; + } + + return true; +} + +bool testutils::isGoalReached(const moveit::core::RobotModelConstPtr& robot_model, + const trajectory_msgs::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, const double tolerance) +{ + return isGoalReached(robot_model, trajectory, req, tolerance, tolerance); +} + +bool testutils::checkCartesianLinearity(const moveit::core::RobotModelConstPtr& robot_model, + const trajectory_msgs::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, + const double translation_norm_tolerance, const double rot_axis_norm_tolerance, + const double rot_angle_tolerance) +{ + std::string link_name; + Eigen::Isometry3d goal_pose_expect; + if (!testutils::getExpectedGoalPose(robot_model, req, link_name, goal_pose_expect)) + { + return false; + } + + // compute start pose + robot_state::RobotState rstate(robot_model); + rstate.setToDefaultValues(); + moveit::core::jointStateToRobotState(req.start_state.joint_state, rstate); + Eigen::Isometry3d start_pose = rstate.getFrameTransform(link_name); + + // start goal angle axis + Eigen::AngleAxisd start_goal_aa(start_pose.rotation().transpose() * goal_pose_expect.rotation()); + + // check the linearity + for (trajectory_msgs::JointTrajectoryPoint way_point : trajectory.points) + { + Eigen::Isometry3d way_point_pose; + std::map way_point_joint_state; + + // initializing all joints of the model + for (const auto& joint_name : robot_model->getVariableNames()) + { + way_point_joint_state[joint_name] = 0; + } + + for (std::size_t i = 0; i < trajectory.joint_names.size(); ++i) + { + way_point_joint_state[trajectory.joint_names.at(i)] = way_point.positions.at(i); + } + + if (!computeLinkFK(robot_model, link_name, way_point_joint_state, way_point_pose)) + { + std::cerr << "Failed to compute forward kinematics for link in goal " + "constraints \n"; + return false; + } + + // translational linearity + Eigen::Vector3d goal_start_translation = goal_pose_expect.translation() - start_pose.translation(); + // https://de.wikipedia.org/wiki/Geradengleichung + // Determined form of a straight line in 3D space + if (fabs((goal_start_translation.cross(way_point_pose.translation()) - + goal_start_translation.cross(start_pose.translation())) + .norm()) > fabs(translation_norm_tolerance)) + { + std::cout << "Translational linearity is violated. \n" + << "goal tanslation: " << goal_pose_expect.translation() << std::endl + << "start translation: " << start_pose.translation() << std::endl + << "acutual translation " << way_point_pose.translation() << std::endl; + return false; + } + + if (!checkSLERP(start_pose, goal_pose_expect, way_point_pose, rot_axis_norm_tolerance)) + { + return false; + } + } + + return true; +} + +bool testutils::checkSLERP(const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& goal_pose, + const Eigen::Isometry3d& wp_pose, const double rot_axis_norm_tolerance, + const double rot_angle_tolerance) +{ + // rotational linearity + // start way point angle axis + Eigen::AngleAxisd start_goal_aa(start_pose.rotation().transpose() * goal_pose.rotation()); + Eigen::AngleAxisd start_wp_aa(start_pose.rotation().transpose() * wp_pose.rotation()); + + // parallel rotation axis + // it is possible the axis opposite is + // if the angle is zero, axis is arbitrary + if (!(((start_goal_aa.axis() - start_wp_aa.axis()).norm() < fabs(rot_axis_norm_tolerance)) || + ((start_goal_aa.axis() + start_wp_aa.axis()).norm() < fabs(rot_axis_norm_tolerance)) || + (fabs(start_wp_aa.angle()) < fabs(rot_angle_tolerance)))) + { + std::cout << "Rotational linearity is violated. \n" + << std::endl + << "start goal angle: " << start_goal_aa.angle() << "; axis: " << std::endl + << start_goal_aa.axis() << std::endl + << "start waypoint angle: " << start_wp_aa.angle() << "; axis: " << std::endl + << start_wp_aa.axis() << std::endl; + + return false; + } + + return true; +} + +bool testutils::computeLinkFK(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name, + const std::map& joint_state, Eigen::Isometry3d& pose) +{ + // create robot state + robot_state::RobotState rstate(robot_model); + rstate.setToDefaultValues(); + + // check the reference frame of the target pose + if (!rstate.knowsFrameTransform(link_name)) + { + ROS_ERROR_STREAM("The target link " << link_name << " is not known by robot."); + return false; + } + + // set the joint positions + try + { + rstate.setVariablePositions(joint_state); + } + catch (const std::exception& e) + { + std::cerr << e.what() << '\n'; + return false; + } + + // update the frame + rstate.update(); + pose = rstate.getFrameTransform(link_name); + + return true; +} + +bool testutils::isVelocityBounded(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + for (const auto& point : trajectory.points) + { + for (std::size_t i = 0; i < point.velocities.size(); ++i) + { + if (fabs(point.velocities.at(i)) > fabs(joint_limits.getLimit(trajectory.joint_names.at(i)).max_velocity)) + { + std::cerr << "[ Fail ] Joint velocity limit violated in " << i << "th waypoint of joint: " + << " Joint Name: " << trajectory.joint_names.at(i) << "; Position: " << point.positions.at(i) + << "; Velocity: " << point.velocities.at(i) << "; Acceleration: " << point.accelerations.at(i) + << "; Time from start: " << point.time_from_start.toSec() + << "; Velocity Limit: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_velocity + << std::endl; + + return false; + } + } + } + + return true; +} + +bool testutils::isTrajectoryConsistent(const trajectory_msgs::JointTrajectory& trajectory) +{ + for (const auto& point : trajectory.points) + { + if (trajectory.joint_names.size() != point.positions.size() || + trajectory.joint_names.size() != point.velocities.size() || + trajectory.joint_names.size() != point.accelerations.size()) + { + return false; + } + } + + // TODO check value is consistant (time, position, velocity, acceleration) + + return true; +} + +bool testutils::isAccelerationBounded(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + for (const auto& point : trajectory.points) + { + for (std::size_t i = 0; i < point.velocities.size(); ++i) + { + // deceleration + if (point.velocities.at(i) * point.accelerations.at(i) <= 0) + { + if (fabs(point.accelerations.at(i)) > fabs(joint_limits.getLimit(trajectory.joint_names.at(i)).max_deceleration)) + { + std::cerr << "[ Fail ] Deceleration limit violated of joint: " << trajectory.joint_names.at(i) + << ": Position: " << point.positions.at(i) << "; Velocity: " << point.velocities.at(i) + << "; Acceleration: " << point.accelerations.at(i) + << "; Time from start: " << point.time_from_start.toSec() + << ". Deceleration Limit: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_deceleration + << std::endl; + return false; + } + } + // acceleration + else + { + if (fabs(point.accelerations.at(i)) > fabs(joint_limits.getLimit(trajectory.joint_names.at(i)).max_acceleration)) + { + std::cerr << "[ Fail ] Acceleration limit violated of joint: " << trajectory.joint_names.at(i) + << ": Position: " << point.positions.at(i) << "; Velocity: " << point.velocities.at(i) + << "; Acceleration: " << point.accelerations.at(i) + << "; Time from start: " << point.time_from_start.toSec() + << ". Acceleration Limit: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_acceleration + << std::endl; + + return false; + } + } + } + } + + return true; +} + +bool testutils::isPositionBounded(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + for (const auto& point : trajectory.points) + { + for (std::size_t i = 0; i < point.positions.size(); ++i) + { + if (point.positions.at(i) > joint_limits.getLimit(trajectory.joint_names.at(i)).max_position || + point.positions.at(i) < joint_limits.getLimit(trajectory.joint_names.at(i)).min_position) + { + std::cerr << "[ Fail ] Joint position limit violated in " << i << "th waypoint of joint: " + << " Joint Name: " << trajectory.joint_names.at(i) << "; Position: " << point.positions.at(i) + << "; Velocity: " << point.velocities.at(i) << "; Acceleration: " << point.accelerations.at(i) + << "; Time from start: " << point.time_from_start.toSec() + << "; Max Position: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_position + << "; Min Position: " << joint_limits.getLimit(trajectory.joint_names.at(i)).min_position + << std::endl; + + return false; + } + } + } + + return true; +} + +bool testutils::checkJointTrajectory(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + if (!testutils::isTrajectoryConsistent(trajectory)) + { + std::cout << "Joint trajectory is not consistent." << std::endl; + return false; + } + if (!testutils::isPositionBounded(trajectory, joint_limits)) + { + std::cout << "Joint poisiton violated the limits." << std::endl; + return false; + } + if (!testutils::isVelocityBounded(trajectory, joint_limits)) + { + std::cout << "Joint velocity violated the limits." << std::endl; + return false; + } + if (!testutils::isAccelerationBounded(trajectory, joint_limits)) + { + std::cout << "Joint acceleration violated the limits." << std::endl; + return false; + } + + return true; +} + +::testing::AssertionResult testutils::hasStrictlyIncreasingTime(const robot_trajectory::RobotTrajectoryPtr& trajectory) +{ + // Check for strictly positively increasing time steps + for (unsigned int i = 1; i < trajectory->getWayPointCount(); ++i) + { + if (trajectory->getWayPointDurationFromPrevious(i) <= 0.0) + { + return ::testing::AssertionFailure() + << "Waypoint " << (i) << " has " << trajectory->getWayPointDurationFromPrevious(i) + << " time between itself and its predecessor." + << " Total points in trajectory: " << trajectory->getWayPointCount() << "."; + } + } + + return ::testing::AssertionSuccess(); +} + +void testutils::createDummyRequest(const moveit::core::RobotModelConstPtr& robot_model, + const std::string& planning_group, planning_interface::MotionPlanRequest& req) +{ + // valid motion plan request with goal in joint space + req.group_name = planning_group; + req.max_velocity_scaling_factor = 1.0; + req.max_acceleration_scaling_factor = 1.0; + robot_state::RobotState rstate(robot_model); + rstate.setToDefaultValues(); + moveit::core::robotStateToRobotStateMsg(rstate, req.start_state, false); +} + +void testutils::createPTPRequest(const std::string& planning_group, const robot_state::RobotState& start_state, + const robot_state::RobotState& goal_state, planning_interface::MotionPlanRequest& req) +{ + // valid motion plan request with goal in joint space + req.planner_id = "PTP"; + req.group_name = planning_group; + req.max_velocity_scaling_factor = 0.5; + req.max_acceleration_scaling_factor = 0.5; + // start state + moveit::core::robotStateToRobotStateMsg(start_state, req.start_state, false); + // goal state + req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + goal_state, goal_state.getRobotModel()->getJointModelGroup(planning_group))); +} + +bool testutils::toTCPPose(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name, + const std::vector& joint_values, geometry_msgs::Pose& pose, + const std::string& joint_prefix) +{ + { + std::map joint_state; + auto joint_values_it = joint_values.begin(); + + // initializing all joints of the model + for (const auto& joint_name : robot_model->getVariableNames()) + { + joint_state[joint_name] = 0; + } + + for (std::size_t i = 0; i < joint_values.size(); ++i) + { + joint_state[testutils::getJointName(i + 1, joint_prefix)] = *joint_values_it; + ++joint_values_it; + } + + Eigen::Isometry3d eig_pose; + if (!testutils::computeLinkFK(robot_model, link_name, joint_state, eig_pose)) + { + return false; + } + tf2::convert(eig_pose, pose); + return true; + } +} + +bool testutils::checkOriginalTrajectoryAfterBlending(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, + const double time_tolerance) +{ + for (std::size_t i = 0; i < res.first_trajectory->getWayPointCount(); ++i) + { + for (const std::string& joint_name : + res.first_trajectory->getWayPoint(i).getJointModelGroup(req.group_name)->getActiveJointModelNames()) + { + // check joint position + if (res.first_trajectory->getWayPoint(i).getVariablePosition(joint_name) != + req.first_trajectory->getWayPoint(i).getVariablePosition(joint_name)) + { + std::cout << i << "th position of the first trajectory is not same." << std::endl; + return false; + } + + // check joint velocity + if (res.first_trajectory->getWayPoint(i).getVariableVelocity(joint_name) != + req.first_trajectory->getWayPoint(i).getVariableVelocity(joint_name)) + { + std::cout << i << "th velocity of the first trajectory is not same." << std::endl; + return false; + } + + // check joint acceleration + if (res.first_trajectory->getWayPoint(i).getVariableAcceleration(joint_name) != + req.first_trajectory->getWayPoint(i).getVariableAcceleration(joint_name)) + { + std::cout << i << "th acceleration of the first trajectory is not same." << std::endl; + return false; + } + } + + // check time from start + if (res.first_trajectory->getWayPointDurationFromStart(i) != req.first_trajectory->getWayPointDurationFromStart(i)) + { + std::cout << i << "th time_from_start of the first trajectory is not same." << std::endl; + return false; + } + } + + size_t size_second = res.second_trajectory->getWayPointCount(); + size_t size_second_original = req.second_trajectory->getWayPointCount(); + for (std::size_t i = 0; i < size_second; ++i) + { + for (const std::string& joint_name : res.second_trajectory->getWayPoint(size_second - i - 1) + .getJointModelGroup(req.group_name) + ->getActiveJointModelNames()) + { + // check joint position + if (res.second_trajectory->getWayPoint(size_second - i - 1).getVariablePosition(joint_name) != + req.second_trajectory->getWayPoint(size_second_original - i - 1).getVariablePosition(joint_name)) + { + std::cout << i - 1 << "th position of the second trajectory is not same." << std::endl; + return false; + } + + // check joint velocity + if (res.second_trajectory->getWayPoint(size_second - i - 1).getVariableVelocity(joint_name) != + req.second_trajectory->getWayPoint(size_second_original - i - 1).getVariableVelocity(joint_name)) + { + std::cout << i - 1 << "th position of the second trajectory is not same." << std::endl; + return false; + } + + // check joint acceleration + if (res.second_trajectory->getWayPoint(size_second - i - 1).getVariableAcceleration(joint_name) != + req.second_trajectory->getWayPoint(size_second_original - i - 1).getVariableAcceleration(joint_name)) + { + std::cout << i - 1 << "th position of the second trajectory is not same." << std::endl; + return false; + } + } + + // check time from start + if (i < size_second - 1) + { + if (fabs((res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1) - + res.second_trajectory->getWayPointDurationFromStart(size_second - i - 2)) - + (req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) - + req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2))) > time_tolerance) + { + std::cout << size_second - i - 1 << "th time from start of the second trajectory is not same." + << res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1) << ", " + << res.second_trajectory->getWayPointDurationFromStart(size_second - i - 2) << ", " + << req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) << ", " + << req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2) << std::endl; + return false; + } + } + else + { + if (fabs((res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1)) - + (req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) - + req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2))) > time_tolerance) + { + std::cout << size_second - i - 1 << "th time from start of the second trajectory is not same." + << res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1) << ", " + << req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) - + req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2) + << std::endl; + return false; + } + } + } + + return true; +} + +bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, + double joint_velocity_tolerance, double joint_accleration_tolerance) +{ + // convert to msgs + moveit_msgs::RobotTrajectory first_traj, blend_traj, second_traj; + res.first_trajectory->getRobotTrajectoryMsg(first_traj); + res.blend_trajectory->getRobotTrajectoryMsg(blend_traj); + res.second_trajectory->getRobotTrajectoryMsg(second_traj); + + // check the continuity between first trajectory and blend trajectory + trajectory_msgs::JointTrajectoryPoint first_end, blend_start; + first_end = first_traj.joint_trajectory.points.back(); + blend_start = blend_traj.joint_trajectory.points.front(); + + // check the dimensions + if (first_end.positions.size() != blend_start.positions.size() || + first_end.velocities.size() != blend_start.velocities.size() || + first_end.accelerations.size() != blend_start.accelerations.size()) + { + std::cout << "Different sizes of the position/velocity/acceleration " + "between first trajectory and blend trajectory." + << std::endl; + return false; + } + + // check the velocity and acceleration + for (std::size_t i = 0; i < first_end.positions.size(); ++i) + { + double blend_start_velo = + (blend_start.positions.at(i) - first_end.positions.at(i)) / blend_start.time_from_start.toSec(); + if (fabs(blend_start_velo - blend_start.velocities.at(i)) > joint_velocity_tolerance) + { + std::cout << "Velocity computed from positions are different from the " + "value in trajectory." + << std::endl + << "position: " << blend_start.positions.at(i) << "; " << first_end.positions.at(i) + << "computed: " << blend_start_velo << "; " + << "in trajectory: " << blend_start.velocities.at(i) << std::endl; + return false; + } + + double blend_start_acc = (blend_start_velo - first_end.velocities.at(i)) / blend_start.time_from_start.toSec(); + if (fabs(blend_start_acc - blend_start.accelerations.at(i)) > joint_velocity_tolerance) + { + std::cout << "Acceleration computed from positions/velocities are " + "different from the value in trajectory." + << std::endl + << "computed: " << blend_start_acc << "; " + << "in trajectory: " << blend_start.accelerations.at(i) << std::endl; + return false; + } + } + + // check the continuity between blend trajectory and second trajectory + trajectory_msgs::JointTrajectoryPoint blend_end, second_start; + blend_end = blend_traj.joint_trajectory.points.back(); + second_start = second_traj.joint_trajectory.points.front(); + + // check the dimensions + if (blend_end.positions.size() != second_start.positions.size() || + blend_end.velocities.size() != second_start.velocities.size() || + blend_end.accelerations.size() != second_start.accelerations.size()) + { + std::cout << "Different sizes of the position/velocity/acceleration " + "between first trajectory and blend trajectory." + << std::endl + << blend_end.positions.size() << ", " << second_start.positions.size() << std::endl + << blend_end.velocities.size() << ", " << second_start.positions.size() << std::endl + << blend_end.accelerations.size() << ", " << second_start.accelerations.size() << std::endl; + return false; + } + + // check the velocity and acceleration + for (std::size_t i = 0; i < blend_end.positions.size(); ++i) + { + double second_start_velo = + (second_start.positions.at(i) - blend_end.positions.at(i)) / second_start.time_from_start.toSec(); + if (fabs(second_start_velo - second_start.velocities.at(i)) > joint_accleration_tolerance) + { + std::cout << "Velocity computed from positions are different from the " + "value in trajectory." + << std::endl + << "computed: " << second_start_velo << "; " + << "in trajectory: " << second_start.velocities.at(i) << std::endl; + return false; + } + + double second_start_acc = (second_start_velo - blend_end.velocities.at(i)) / second_start.time_from_start.toSec(); + if (fabs(second_start_acc - second_start.accelerations.at(i)) > joint_accleration_tolerance) + { + std::cout << "Acceleration computed from positions/velocities are " + "different from the value in trajectory." + << std::endl + << "computed: " << second_start_acc << "; " + << "in trajectory: " << second_start.accelerations.at(i) << std::endl; + return false; + } + } + + return true; +} + +bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits) +{ + // sampling time + double duration = res.blend_trajectory->getWayPointDurationFromPrevious(res.blend_trajectory->getWayPointCount() - 1); + if (duration == 0.0) + { + std::cout << "Cannot perform check of cartesian space continuity with " + "sampling time 0.0" + << std::endl; + return false; + } + + // limits + double max_trans_velo = planner_limits.getCartesianLimits().getMaxTranslationalVelocity(); + double max_trans_acc = planner_limits.getCartesianLimits().getMaxTranslationalAcceleration(); + double max_rot_velo = planner_limits.getCartesianLimits().getMaxRotationalVelocity(); + double max_rot_acc = max_trans_acc / max_trans_velo * max_rot_velo; + + // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + // +++ check the connection points between three trajectories +++ + // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + + // connection points + Eigen::Isometry3d pose_first_end, pose_first_end_1, pose_blend_start, pose_blend_start_1, pose_blend_end, + pose_blend_end_1, pose_second_start, pose_second_start_1; + // one sample before last point of first trajectory + pose_first_end_1 = res.first_trajectory->getWayPointPtr(res.first_trajectory->getWayPointCount() - 2) + ->getFrameTransform(req.link_name); + // last point of first trajectory + pose_first_end = res.first_trajectory->getLastWayPointPtr()->getFrameTransform(req.link_name); + // first point of blend trajectory + pose_blend_start = res.blend_trajectory->getFirstWayPointPtr()->getFrameTransform(req.link_name); + // second point of blend trajectory + pose_blend_start_1 = res.blend_trajectory->getWayPointPtr(1)->getFrameTransform(req.link_name); + // one sample before last point of blend trajectory + pose_blend_end_1 = res.blend_trajectory->getWayPointPtr(res.blend_trajectory->getWayPointCount() - 2) + ->getFrameTransform(req.link_name); + // last point of blend trajectory + pose_blend_end = res.blend_trajectory->getLastWayPointPtr()->getFrameTransform(req.link_name); + // first point of second trajectory + pose_second_start = res.second_trajectory->getFirstWayPointPtr()->getFrameTransform(req.link_name); + // second point of second trajectory + pose_second_start_1 = res.second_trajectory->getWayPointPtr(1)->getFrameTransform(req.link_name); + + // std::cout << "### sample duration: " << duration << " ###" << std::endl; + // std::cout << "### end pose of first trajectory ###" << std::endl; + // std::cout << pose_first_end.matrix() << std::endl; + // std::cout << "### start pose of blend trajectory ###" << std::endl; + // std::cout << pose_blend_start.matrix() << std::endl; + // std::cout << "### end pose of blend trajectory ###" << std::endl; + // std::cout << pose_blend_end.matrix() << std::endl; + // std::cout << "### start pose of second trajectory ###" << std::endl; + // std::cout << pose_second_start.matrix() << std::endl; + + // std::cout << "### v_1 ###" << std::endl; + // std::cout << v_1 << std::endl; + // std::cout << "### w_1 ###" << std::endl; + // std::cout << w_1 << std::endl; + // std::cout << "### v_2 ###" << std::endl; + // std::cout << v_2 << std::endl; + // std::cout << "### w_2 ###" << std::endl; + // std::cout << w_2 << std::endl; + // std::cout << "### v_3 ###" << std::endl; + // std::cout << v_3 << std::endl; + // std::cout << "### w_3 ###" << std::endl; + // std::cout << w_3 << std::endl; + + // check the connection points between first trajectory and blend trajectory + Eigen::Vector3d v_1, w_1, v_2, w_2, v_3, w_3; + computeCartVelocity(pose_first_end_1, pose_first_end, duration, v_1, w_1); + computeCartVelocity(pose_first_end, pose_blend_start, duration, v_2, w_2); + computeCartVelocity(pose_blend_start, pose_blend_start_1, duration, v_3, w_3); + + // translational velocity + if (v_2.norm() > max_trans_velo) + { + std::cout << "Translational velocity between first trajectory and blend " + "trajectory exceeds the limit." + << "Actual velocity (norm): " << v_2.norm() << "; " + << "Limits: " << max_trans_velo << std::endl; + } + // rotational velocity + if (w_2.norm() > max_rot_velo) + { + std::cout << "Rotational velocity between first trajectory and blend " + "trajectory exceeds the limit." + << "Actual velocity (norm): " << w_2.norm() << "; " + << "Limits: " << max_rot_velo << std::endl; + } + // translational acceleration + Eigen::Vector3d a_1 = (v_2 - v_1) / duration; + Eigen::Vector3d a_2 = (v_3 - v_2) / duration; + if (a_1.norm() > max_trans_acc || a_2.norm() > max_trans_acc) + { + std::cout << "Translational acceleration between first trajectory and " + "blend trajectory exceeds the limit." + << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; " + << "Limits: " << max_trans_acc << std::endl; + } + + // rotational acceleration + a_1 = (w_2 - w_1) / duration; + a_2 = (w_3 - w_2) / duration; + if (a_1.norm() > max_rot_acc || a_2.norm() > max_rot_acc) + { + std::cout << "Rotational acceleration between first trajectory and blend " + "trajectory exceeds the limit." + << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; " + << "Limits: " << max_rot_acc << std::endl; + } + + computeCartVelocity(pose_blend_end_1, pose_blend_end, duration, v_1, w_1); + computeCartVelocity(pose_blend_end, pose_second_start, duration, v_2, w_2); + computeCartVelocity(pose_second_start, pose_second_start_1, duration, v_3, w_3); + + if (v_2.norm() > max_trans_velo) + { + std::cout << "Translational velocity between blend trajectory and second " + "trajectory exceeds the limit." + << "Actual velocity (norm): " << v_2.norm() << "; " + << "Limits: " << max_trans_velo << std::endl; + } + if (w_2.norm() > max_rot_velo) + { + std::cout << "Rotational velocity between blend trajectory and second " + "trajectory exceeds the limit." + << "Actual velocity (norm): " << w_2.norm() << "; " + << "Limits: " << max_rot_velo << std::endl; + } + a_1 = (v_2 - v_1) / duration; + a_2 = (v_3 - v_2) / duration; + if (a_1.norm() > max_trans_acc || a_2.norm() > max_trans_acc) + { + std::cout << "Translational acceleration between blend trajectory and " + "second trajectory exceeds the limit." + << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; " + << "Limits: " << max_trans_acc << std::endl; + } + // check rotational acceleration + a_1 = (w_2 - w_1) / duration; + a_2 = (w_3 - w_2) / duration; + if (a_1.norm() > max_rot_acc || a_2.norm() > max_rot_acc) + { + std::cout << "Rotational acceleration between blend trajectory and second " + "trajectory exceeds the limit." + << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; " + << "Limits: " << max_rot_acc << std::endl; + } + + return true; +} + +bool testutils::checkThatPointsInRadius(const std::string& link_name, const double& r, Eigen::Isometry3d& circ_pose, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& res) +{ + bool result = true; + for (size_t i = 1; i < res.blend_trajectory->getWayPointCount() - 1; ++i) + { + Eigen::Isometry3d curr_pos(res.blend_trajectory->getWayPointPtr(i)->getFrameTransform(link_name)); + if ((curr_pos.translation() - circ_pose.translation()).norm() > r) + { + std::cout << "Point " << i << " does not lie within blending radius (dist: " + << ((curr_pos.translation() - circ_pose.translation()).norm() - r) << ")." << std::endl; + result = false; + } + } + return result; +} + +void testutils::computeCartVelocity(const Eigen::Isometry3d& pose_1, const Eigen::Isometry3d& pose_2, double duration, + Eigen::Vector3d& v, Eigen::Vector3d& w) +{ + // translational velocity + v = (pose_2.translation() - pose_1.translation()) / duration; + + // angular velocity + // reference: A Mathematical Introduction to Robotics Manipulation 2.4.1 + // Rotational velocity + Eigen::Matrix3d rm_1 = pose_1.linear(); + Eigen::Matrix3d rm_2 = pose_2.linear(); + Eigen::Matrix3d rm_dot = (rm_2 - rm_1) / duration; + Eigen::Matrix3d w_hat = rm_dot * rm_1.transpose(); + w(0) = w_hat(2, 1); + w(1) = w_hat(0, 2); + w(2) = w_hat(1, 0); +} + +void testutils::getLinLinPosesWithoutOriChange(const std::string& frame_id, sensor_msgs::JointState& initialJointState, + geometry_msgs::PoseStamped& p1, geometry_msgs::PoseStamped& p2) +{ + initialJointState = + testutils::generateJointState({ 0., 0.007881892504574495, -1.8157263253868452, 0., 1.8236082178909834, 0. }); + + p1.header.frame_id = frame_id; + p1.pose.position.x = 0.25; + p1.pose.position.y = 0.3; + p1.pose.position.z = 0.65; + p1.pose.orientation.x = 0.; + p1.pose.orientation.y = 0.; + p1.pose.orientation.z = 0.; + p1.pose.orientation.w = 1.; + + p2 = p1; + p2.pose.position.x -= 0.15; +} + +void testutils::getOriChange(Eigen::Matrix3d& ori1, Eigen::Matrix3d& ori2) +{ + ori1 = Eigen::AngleAxisd(0.2 * M_PI, Eigen::Vector3d::UnitZ()); + ori2 = Eigen::AngleAxisd(0.4 * M_PI, Eigen::Vector3d::UnitZ()); +} + +void testutils::createFakeCartTraj(const robot_trajectory::RobotTrajectoryPtr& traj, const std::string& link_name, + moveit_msgs::RobotTrajectory& fake_traj) +{ + fake_traj.joint_trajectory.joint_names.push_back("x"); + fake_traj.joint_trajectory.joint_names.push_back("y"); + fake_traj.joint_trajectory.joint_names.push_back("z"); + // fake_traj.joint_trajectory.joint_names.push_back("a"); + // fake_traj.joint_trajectory.joint_names.push_back("b"); + // fake_traj.joint_trajectory.joint_names.push_back("c"); + + for (size_t i = 0; i < traj->getWayPointCount(); ++i) + { + trajectory_msgs::JointTrajectoryPoint waypoint; + waypoint.time_from_start = ros::Duration(traj->getWayPointDurationFromStart(i)); + Eigen::Isometry3d waypoint_pose = traj->getWayPointPtr(i)->getFrameTransform(link_name); + Eigen::Vector3d waypoint_position = waypoint_pose.translation(); + waypoint.positions.push_back(waypoint_position(0)); + waypoint.positions.push_back(waypoint_position(1)); + waypoint.positions.push_back(waypoint_position(2)); + waypoint.velocities.push_back(0); + waypoint.velocities.push_back(0); + waypoint.velocities.push_back(0); + waypoint.accelerations.push_back(0); + waypoint.accelerations.push_back(0); + waypoint.accelerations.push_back(0); + fake_traj.joint_trajectory.points.push_back(waypoint); + } +} + +bool testutils::getBlendTestData(const ros::NodeHandle& nh, const size_t& dataset_num, const std::string& name_prefix, + std::vector& datasets) +{ + datasets.clear(); + testutils::BlendTestData dataset; + for (size_t i = 1; i < dataset_num + 1; ++i) + { + std::string data_set_name = "blend_set_" + std::to_string(i); + if (nh.getParam(name_prefix + data_set_name + "/start_position", dataset.start_position) && + nh.getParam(name_prefix + data_set_name + "/mid_position", dataset.mid_position) && + nh.getParam(name_prefix + data_set_name + "/end_position", dataset.end_position)) + { + datasets.push_back(dataset); + } + else + { + return false; + } + } + return true; +} + +bool testutils::generateTrajFromBlendTestData( + const robot_model::RobotModelConstPtr& robot_model, + const std::shared_ptr& tg, const std::string& group_name, + const std::string& link_name, const testutils::BlendTestData& data, const double& sampling_time_1, + const double& sampling_time_2, planning_interface::MotionPlanResponse& res_1, + planning_interface::MotionPlanResponse& res_2, double& dis_1, double& dis_2) +{ + // generate first trajectory + planning_interface::MotionPlanRequest req_1; + req_1.group_name = group_name; + req_1.max_velocity_scaling_factor = 0.1; + req_1.max_acceleration_scaling_factor = 0.1; + // start state + robot_state::RobotState start_state(robot_model); + start_state.setToDefaultValues(); + start_state.setJointGroupPositions(group_name, data.start_position); + moveit::core::robotStateToRobotStateMsg(start_state, req_1.start_state, false); + // goal constraint + robot_state::RobotState goal_state_1(robot_model); + goal_state_1.setToDefaultValues(); + goal_state_1.setJointGroupPositions(group_name, data.mid_position); + req_1.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + goal_state_1, goal_state_1.getRobotModel()->getJointModelGroup(group_name))); + + // trajectory generation + if (!tg->generate(req_1, res_1, sampling_time_1)) + { + std::cout << "Failed to generate first trajectory." << std::endl; + return false; + } + + // generate second LIN trajectory + planning_interface::MotionPlanRequest req_2; + req_2.group_name = group_name; + req_2.max_velocity_scaling_factor = 0.1; + req_2.max_acceleration_scaling_factor = 0.1; + // start state + moveit::core::robotStateToRobotStateMsg(goal_state_1, req_2.start_state, false); + // goal state + robot_state::RobotState goal_state_2(robot_model); + goal_state_2.setToDefaultValues(); + goal_state_2.setJointGroupPositions(group_name, data.end_position); + req_2.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + goal_state_2, goal_state_2.getRobotModel()->getJointModelGroup(group_name))); + // trajectory generation + if (!tg->generate(req_2, res_2, sampling_time_2)) + { + std::cout << "Failed to generate second trajectory." << std::endl; + return false; + } + + // estimate a proper blend radius + dis_1 = + (start_state.getFrameTransform(link_name).translation() - goal_state_1.getFrameTransform(link_name).translation()) + .norm(); + + dis_2 = (goal_state_1.getFrameTransform(link_name).translation() - + goal_state_2.getFrameTransform(link_name).translation()) + .norm(); + + return true; +} + +bool testutils::checkBlendResult(const pilz_industrial_motion_planner::TrajectoryBlendRequest& blend_req, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& blend_res, + const pilz_industrial_motion_planner::LimitsContainer& limits, + double joint_velocity_tolerance, double joint_acceleration_tolerance, + double cartesian_velocity_tolerance, double cartesian_angular_velocity_tolerance) +{ + // ++++++++++++++++++++++ + // + Check trajectories + + // ++++++++++++++++++++++ + moveit_msgs::RobotTrajectory traj_msg; + blend_res.first_trajectory->getRobotTrajectoryMsg(traj_msg); + if (!testutils::checkJointTrajectory(traj_msg.joint_trajectory, limits.getJointLimitContainer())) + { + return false; + } + + blend_res.blend_trajectory->getRobotTrajectoryMsg(traj_msg); + if (!testutils::checkJointTrajectory(traj_msg.joint_trajectory, limits.getJointLimitContainer())) + { + return false; + }; + + blend_res.second_trajectory->getRobotTrajectoryMsg(traj_msg); + if (!testutils::checkJointTrajectory(traj_msg.joint_trajectory, limits.getJointLimitContainer())) + { + return false; + }; + + Eigen::Isometry3d circ_pose = + blend_req.first_trajectory->getLastWayPointPtr()->getFrameTransform(blend_req.link_name); + if (!testutils::checkThatPointsInRadius(blend_req.link_name, blend_req.blend_radius, circ_pose, blend_res)) + { + return false; + } + + // check the first and second trajectories, if they are still the same before + // and after the bendling phase + if (!testutils::checkOriginalTrajectoryAfterBlending(blend_req, blend_res, 10e-5)) + { + return false; + } + + // check the continuity between the trajectories in joint space + if (!testutils::checkBlendingJointSpaceContinuity(blend_res, joint_velocity_tolerance, joint_acceleration_tolerance)) + { + return false; + } + + // check the continuity between the trajectories in cart space + if (!testutils::checkBlendingCartSpaceContinuity(blend_req, blend_res, limits)) + { + return false; + } + + // ++++++++++++++++++++++++ + // + Visualize the result + + // ++++++++++++++++++++++++ + // ros::NodeHandle nh; + // ros::Publisher pub = + // nh.advertise("my_planned_path", 1); + // ros::Duration duration(1.0); + // duration.sleep(); + + // // visualize the joint trajectory + // moveit_msgs::DisplayTrajectory displayTrajectory; + // moveit_msgs::RobotTrajectory res_first_traj_msg, res_blend_traj_msg, + // res_second_traj_msg; + // blend_res.first_trajectory->getRobotTrajectoryMsg(res_first_traj_msg); + // blend_res.blend_trajectory->getRobotTrajectoryMsg(res_blend_traj_msg); + // blend_res.second_trajectory->getRobotTrajectoryMsg(res_second_traj_msg); + // displayTrajectory.trajectory.push_back(res_first_traj_msg); + // displayTrajectory.trajectory.push_back(res_blend_traj_msg); + // displayTrajectory.trajectory.push_back(res_second_traj_msg); + + // pub.publish(displayTrajectory); + + return true; +} + +void testutils::generateRequestMsgFromBlendTestData(const moveit::core::RobotModelConstPtr& robot_model, + const testutils::BlendTestData& data, const std::string& planner_id, + const std::string& group_name, const std::string& link_name, + moveit_msgs::MotionSequenceRequest& req_list) +{ + // motion plan request of first trajectory + planning_interface::MotionPlanRequest req_1; + req_1.planner_id = planner_id; + req_1.group_name = group_name; + req_1.max_velocity_scaling_factor = 0.1; + req_1.max_acceleration_scaling_factor = 0.1; + // start state + robot_state::RobotState start_state(robot_model); + start_state.setToDefaultValues(); + start_state.setJointGroupPositions(group_name, data.start_position); + moveit::core::robotStateToRobotStateMsg(start_state, req_1.start_state, false); + // goal constraint + robot_state::RobotState goal_state_1(robot_model); + goal_state_1.setToDefaultValues(); + goal_state_1.setJointGroupPositions(group_name, data.mid_position); + req_1.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + goal_state_1, goal_state_1.getRobotModel()->getJointModelGroup(group_name))); + + // motion plan request of second trajectory + planning_interface::MotionPlanRequest req_2; + req_2.planner_id = planner_id; + req_2.group_name = group_name; + req_2.max_velocity_scaling_factor = 0.1; + req_2.max_acceleration_scaling_factor = 0.1; + // start state + // moveit::core::robotStateToRobotStateMsg(goal_state_1, req_2.start_state, + // false); + // goal state + robot_state::RobotState goal_state_2(robot_model); + goal_state_2.setToDefaultValues(); + goal_state_2.setJointGroupPositions(group_name, data.end_position); + req_2.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + goal_state_2, goal_state_2.getRobotModel()->getJointModelGroup(group_name))); + + // select a proper blending radius + // estimate a proper blend radius + double dis_1 = + (start_state.getFrameTransform(link_name).translation() - goal_state_1.getFrameTransform(link_name).translation()) + .norm(); + + double dis_2 = (goal_state_1.getFrameTransform(link_name).translation() - + goal_state_2.getFrameTransform(link_name).translation()) + .norm(); + + double blend_radius = 0.5 * std::min(dis_1, dis_2); + + moveit_msgs::MotionSequenceItem blend_req_1, blend_req_2; + blend_req_1.req = req_1; + blend_req_1.blend_radius = blend_radius; + blend_req_2.req = req_2; + blend_req_2.blend_radius = 0.0; + + req_list.items.push_back(blend_req_1); + req_list.items.push_back(blend_req_2); +} + +void testutils::checkRobotModel(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, + const std::string& link_name) +{ + ASSERT_TRUE(robot_model != nullptr) << "failed to load robot model"; + ASSERT_FALSE(robot_model->isEmpty()) << "robot model is empty"; + ASSERT_TRUE(robot_model->hasJointModelGroup(group_name)) << group_name << " is not known to robot"; + ASSERT_TRUE(robot_model->hasLinkModel(link_name)) << link_name << " is not known to robot"; + ASSERT_TRUE(robot_state::RobotState(robot_model).knowsFrameTransform(link_name)) + << "Transform of " << link_name << " is unknown"; +} + +::testing::AssertionResult testutils::hasTrapezoidVelocity(std::vector accelerations, const double acc_tol) +{ + // Check that acceleration is monotonously decreasing + if (!isMonotonouslyDecreasing(accelerations, acc_tol)) + { + return ::testing::AssertionFailure() << "Cannot be a trapezoid since " + "acceleration is not monotonously " + "decreasing!"; + } + + // Check accelerations + double first_acc = accelerations.front(); + auto it_last_acc = std::find_if(accelerations.begin(), accelerations.end(), + [first_acc, acc_tol](double el) { return (std::abs(el - first_acc) > acc_tol); }) - + 1; + + auto it_last_intermediate = + std::find_if(it_last_acc + 1, accelerations.end(), [acc_tol](double el) { return (el < acc_tol); }) - 1; + + // Check that there are 1 or 2 intermediate points + auto n_intermediate_1 = std::distance(it_last_acc, it_last_intermediate); + + if (n_intermediate_1 != 1 && n_intermediate_1 != 2) + { + return ::testing::AssertionFailure() << "Expected 1 or 2 intermediate points between acceleration and " + "constant " + "velocity phase but found: " + << n_intermediate_1; + } + + // Const phase (vel == 0) + auto it_first_const = it_last_intermediate + 1; + auto it_last_const = + std::find_if(it_first_const, accelerations.end(), [acc_tol](double el) { return (std::abs(el) > acc_tol); }) - 1; + // This test makes sense only if the generated traj has enough points such + // that the trapezoid is not degenerated. + if (std::distance(it_first_const, it_last_const) < 3) + { + return ::testing::AssertionFailure() << "Exptected the have at least 3 points during the phase of " + "constant " + "velocity."; + } + + // Deceleration + double deceleration = accelerations.back(); + auto it_first_dec = + std::find_if(it_last_const + 1, accelerations.end(), + [deceleration, acc_tol](double el) { return (std::abs(el - deceleration) > acc_tol); }) + + 1; + + // Points between const and deceleration (again 1 or 2) + auto n_intermediate_2 = std::distance(it_last_const, it_first_dec); + if (n_intermediate_2 != 1 && n_intermediate_2 != 2) + { + return ::testing::AssertionFailure() << "Expected 1 or 2 intermediate points between constant velocity " + "and " + "deceleration phase but found: " + << n_intermediate_2; + } + + std::stringstream debug_stream; + for (auto it = accelerations.begin(); it != it_last_acc + 1; it++) + { + debug_stream << *it << "(Acc)" << std::endl; + } + + for (auto it = it_last_acc + 1; it != it_last_intermediate + 1; it++) + { + debug_stream << *it << "(Inter1)" << std::endl; + } + + for (auto it = it_first_const; it != it_last_const + 1; it++) + { + debug_stream << *it << "(Const)" << std::endl; + } + + for (auto it = it_last_const + 1; it != it_first_dec; it++) + { + debug_stream << *it << "(Inter2)" << std::endl; + } + + for (auto it = it_first_dec; it != accelerations.end(); it++) + { + debug_stream << *it << "(Dec)" << std::endl; + } + + ROS_DEBUG_STREAM(debug_stream.str()); + + return ::testing::AssertionSuccess(); +} + +::testing::AssertionResult +testutils::checkCartesianTranslationalPath(const robot_trajectory::RobotTrajectoryConstPtr& trajectory, + const std::string& link_name, const double acc_tol) +{ + // We require the following such that the test makes sense, else the traj + // would have a degenerated velocity profile + EXPECT_GT(trajectory->getWayPointCount(), 9u); + + std::vector accelerations; + + // Iterate over waypoints collect accelerations + for (size_t i = 2; i < trajectory->getWayPointCount(); ++i) + { + auto waypoint_pose_0 = trajectory->getWayPoint(i - 2).getFrameTransform(link_name); + auto waypoint_pose_1 = trajectory->getWayPoint(i - 1).getFrameTransform(link_name); + auto waypoint_pose_2 = trajectory->getWayPoint(i).getFrameTransform(link_name); + + auto t1 = trajectory->getWayPointDurationFromPrevious(i - 1); + auto t2 = trajectory->getWayPointDurationFromPrevious(i); + + auto vel1 = (waypoint_pose_1.translation() - waypoint_pose_0.translation()).norm() / t1; + auto vel2 = (waypoint_pose_2.translation() - waypoint_pose_1.translation()).norm() / t2; + auto acc_transl = (vel2 - vel1) / (t1 + t2); + accelerations.push_back(acc_transl); + } + + return hasTrapezoidVelocity(accelerations, acc_tol); +} + +::testing::AssertionResult +testutils::checkCartesianRotationalPath(const robot_trajectory::RobotTrajectoryConstPtr& trajectory, + const std::string& link_name, const double rot_axis_tol, const double acc_tol) +{ + // skip computations if rotation angle is zero + if (trajectory->getFirstWayPoint().getFrameTransform(link_name).rotation().isApprox( + trajectory->getLastWayPoint().getFrameTransform(link_name).rotation(), rot_axis_tol)) + { + return ::testing::AssertionSuccess(); + } + + // We require the following such that the test makes sense, else the traj + // would have a degenerated velocity profile + EXPECT_GT(trajectory->getWayPointCount(), 9u); + + std::vector accelerations; + std::vector rotation_axes; + + // Iterate over waypoints collect accelerations and rotation axes + for (size_t i = 2; i < trajectory->getWayPointCount(); ++i) + { + auto waypoint_pose_0 = trajectory->getWayPoint(i - 2).getFrameTransform(link_name); + auto waypoint_pose_1 = trajectory->getWayPoint(i - 1).getFrameTransform(link_name); + auto waypoint_pose_2 = trajectory->getWayPoint(i).getFrameTransform(link_name); + + auto t1 = trajectory->getWayPointDurationFromPrevious(i - 1); + auto t2 = trajectory->getWayPointDurationFromPrevious(i); + + Eigen::Quaterniond orientation0(waypoint_pose_0.rotation()); + Eigen::Quaterniond orientation1(waypoint_pose_1.rotation()); + Eigen::Quaterniond orientation2(waypoint_pose_2.rotation()); + + Eigen::AngleAxisd axis1(orientation0 * orientation1.inverse()); + Eigen::AngleAxisd axis2(orientation1 * orientation2.inverse()); + if (i == 2) + { + rotation_axes.push_back(axis1); + } + rotation_axes.push_back(axis2); + + double angular_vel1 = axis1.angle() / t1; + double angular_vel2 = axis2.angle() / t2; + double angular_acc = (angular_vel2 - angular_vel1) / (t1 + t2); + accelerations.push_back(angular_acc); + } + + // Check that rotation axis is fixed + if (std::adjacent_find(rotation_axes.begin(), rotation_axes.end(), + [rot_axis_tol](const Eigen::AngleAxisd& axis1, const Eigen::AngleAxisd& axis2) { + return ((axis1.axis() - axis2.axis()).norm() > rot_axis_tol); + }) != rotation_axes.end()) + { + return ::testing::AssertionFailure() << "Rotational path of trajectory " + "does not have a fixed rotation " + "axis"; + } + + return hasTrapezoidVelocity(accelerations, acc_tol); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h new file mode 100644 index 0000000000..2937ff1dae --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h @@ -0,0 +1,484 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 +#ifndef INSTANTIATE_TEST_SUITE_P // prior to gtest 1.10 +#define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__) +#endif +#ifndef TYPED_TEST_SUITE // prior to gtest 1.10 +#define TYPED_TEST_SUITE(...) TYPED_TEST_CASE(__VA_ARGS__) +#endif + +#include "moveit_msgs/MotionSequenceRequest.h" +#include "pilz_industrial_motion_planner/limits_container.h" +#include "pilz_industrial_motion_planner/trajectory_blend_request.h" +#include "pilz_industrial_motion_planner/trajectory_blend_response.h" +#include "pilz_industrial_motion_planner/trajectory_generator.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace testutils +{ +const std::string JOINT_NAME_PREFIX("prbt_joint_"); + +static constexpr int32_t DEFAULT_SERVICE_TIMEOUT(10); +static constexpr double DEFAULT_ACCELERATION_EQUALITY_TOLERANCE{ 1e-6 }; +static constexpr double DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE{ 1e-8 }; + +/** + * @brief Convert degree to rad. + */ +inline static constexpr double deg2Rad(double angle) +{ + return (angle / 180.0) * M_PI; +} + +inline std::string getJointName(size_t joint_number, const std::string& joint_prefix) +{ + return joint_prefix + std::to_string(joint_number); +} + +/** + * @brief Create limits for tests to avoid the need to get the limits from the + * parameter server + */ +pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits(const std::vector& joint_names); + +inline std::string demangel(char const* name) +{ + return boost::core::demangle(name); +} + +//******************************************** +// Motion plan requests +//******************************************** + +inline sensor_msgs::JointState generateJointState(const std::vector& pos, const std::vector& vel, + const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX) +{ + sensor_msgs::JointState state; + auto posit = pos.cbegin(); + size_t i = 0; + + while (posit != pos.cend()) + { + state.name.push_back(testutils::getJointName(i + 1, joint_prefix)); + state.position.push_back(*posit); + + i++; + posit++; + } + for (const auto& one_vel : vel) + { + state.velocity.push_back(one_vel); + } + return state; +} + +inline sensor_msgs::JointState generateJointState(const std::vector& pos, + const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX) +{ + return generateJointState(pos, std::vector(), joint_prefix); +} + +inline moveit_msgs::Constraints generateJointConstraint(const std::vector& pos_list, + const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX) +{ + moveit_msgs::Constraints gc; + + auto pos_it = pos_list.begin(); + + for (size_t i = 0; i < pos_list.size(); ++i) + { + moveit_msgs::JointConstraint jc; + jc.joint_name = testutils::getJointName(i + 1, joint_prefix); + jc.position = *pos_it; + gc.joint_constraints.push_back(jc); + + ++pos_it; + } + + return gc; +} + +/** + * @brief Determines the goal position from the given request. + * TRUE if successful, FALSE otherwise. + */ +bool getExpectedGoalPose(const moveit::core::RobotModelConstPtr& robot_model, + const planning_interface::MotionPlanRequest& req, std::string& link_name, + Eigen::Isometry3d& goal_pose_expect); + +/** + * @brief create a dummy motion plan request with zero start state + * No goal constraint is given. + * @param robot_model + * @param planning_group + * @param req + */ +void createDummyRequest(const moveit::core::RobotModelConstPtr& robot_model, const std::string& planning_group, + planning_interface::MotionPlanRequest& req); + +void createPTPRequest(const std::string& planning_group, const moveit::core::RobotState& start_state, + const moveit::core::RobotState& goal_state, planning_interface::MotionPlanRequest& req); + +/** + * @brief check if the goal given in joint space is reached + * Only the last point in the trajectory is veryfied. + * @param trajectory: generated trajectory + * @param goal: goal in joint space + * @param joint_position_tolerance + * @param joint_velocity_tolerance + * @return true if satisfied + */ +bool isGoalReached(const trajectory_msgs::JointTrajectory& trajectory, + const std::vector& goal, const double joint_position_tolerance, + const double joint_velocity_tolerance = 1.0e-6); + +/** + * @brief check if the goal given in cartesian space is reached + * Only the last point in the trajectory is veryfied. + * @param robot_model + * @param trajectory + * @param req + * @param matrix_norm_tolerance: used to compare the transformation matrix + * @param joint_velocity_tolerance + * @return + */ +bool isGoalReached(const robot_model::RobotModelConstPtr& robot_model, + const trajectory_msgs::JointTrajectory& trajectory, const planning_interface::MotionPlanRequest& req, + const double pos_tolerance, const double orientation_tolerance); + +bool isGoalReached(const moveit::core::RobotModelConstPtr& robot_model, + const trajectory_msgs::JointTrajectory& trajectory, const planning_interface::MotionPlanRequest& req, + const double tolerance); + +/** + * @brief Check that given trajectory is straight line. + */ +bool checkCartesianLinearity(const robot_model::RobotModelConstPtr& robot_model, + const trajectory_msgs::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, const double translation_norm_tolerance, + const double rot_axis_norm_tolerance, const double rot_angle_tolerance = 10e-5); + +/** + * @brief check SLERP. The orientation should rotate around the same axis + * linearly. + * @param start_pose + * @param goal_pose + * @param wp_pose + * @param rot_axis_norm_tolerance + * @return + */ +bool checkSLERP(const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& goal_pose, + const Eigen::Isometry3d& wp_pose, const double rot_axis_norm_tolerance, + const double rot_angle_tolerance = 10e-5); + +/** + * @brief get the waypoint index from time from start + * @param trajectory + * @param time_from_start + * @return + */ +inline int getWayPointIndex(const robot_trajectory::RobotTrajectoryPtr& trajectory, const double time_from_start) +{ + int index_before, index_after; + double blend; + trajectory->findWayPointIndicesForDurationAfterStart(time_from_start, index_before, index_after, blend); + return blend > 0.5 ? index_after : index_before; +} + +/** + * @brief check joint trajectory of consistency, position, velocity and + * acceleration limits + * @param trajectory + * @param joint_limits + * @return + */ +bool checkJointTrajectory(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + +/** + * @brief Checks that every waypoint in the trajectory has a non-zero duration + * between itself and its predecessor + * + * Usage in tests: + * @code + * EXPECT_TRUE(HasStrictlyIncreasingTime(trajectory)); + * @endcode + */ +::testing::AssertionResult hasStrictlyIncreasingTime(const robot_trajectory::RobotTrajectoryPtr& trajectory); + +/** + * @brief check if the sizes of the joint position/veloicty/acceleration are + * correct + * @param trajectory + * @return + */ +bool isTrajectoryConsistent(const trajectory_msgs::JointTrajectory& trajectory); + +/** + * @brief is Position Bounded + * @param trajectory + * @param joint_limits + * @return + */ +bool isPositionBounded(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + +/** + * @brief is Velocity Bounded + * @param trajectory + * @param joint_limits + * @return + */ +bool isVelocityBounded(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + +/** + * @brief is Acceleration Bounded + * @param trajectory + * @param joint_limits + * @return + */ +bool isAccelerationBounded(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + +/** + * @brief compute the tcp pose from joint values + * @param robot_model + * @param link_name + * @param joint_values + * @param pose + * @param joint_prefix Prefix of the joint names + * @return false if forward kinematics failed + */ +bool toTCPPose(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name, + const std::vector& joint_values, geometry_msgs::Pose& pose, + const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX); + +/** + * @brief computeLinkFK + * @param robot_model + * @param link_name + * @param joint_state + * @param pose + * @return + */ +bool computeLinkFK(const robot_model::RobotModelConstPtr& robot_model, const std::string& link_name, + const std::map& joint_state, Eigen::Isometry3d& pose); + +/** + * @brief checkOriginalTrajectoryAfterBlending + * @param blend_res + * @param traj_1_res + * @param traj_2_res + * @param time_tolerance + * @return + */ +bool checkOriginalTrajectoryAfterBlending(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, + const double time_tolerance); + +/** + * @brief check the blending result, if the joint space continuity is fulfilled + * @param res: trajectory blending response, contains three trajectories. + * Between these three trajectories should be continuous. + * @return true if joint position/velocity is continuous. joint acceleration can + * have jumps. + */ +bool checkBlendingJointSpaceContinuity(const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, + double joint_velocity_tolerance, double joint_accleration_tolerance); + +bool checkBlendingCartSpaceContinuity(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + +/** + * @brief Checks if all points of the blending trajectory lie within the + * blending radius. + */ +bool checkThatPointsInRadius(const std::string& link_name, const double& r, Eigen::Isometry3d& circ_pose, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& res); + +/** + * @brief compute Cartesian velocity + * @param pose_1 + * @param pose_2 + * @param duration + * @param v + * @param w + */ +void computeCartVelocity(const Eigen::Isometry3d& pose_1, const Eigen::Isometry3d& pose_2, double duration, + Eigen::Vector3d& v, Eigen::Vector3d& w); + +/** + * @brief Returns an initial joint state and two poses which + * can be used to perform a Lin-Lin movement. + * + * @param frame_id + * @param initialJointState As cartesian position: (0.3, 0, 0.65, 0, 0, 0) + * @param p1 (0.05, 0, 0.65, 0, 0, 0) + * @param p2 (0.05, 0.4, 0.65, 0, 0, 0) + */ +void getLinLinPosesWithoutOriChange(const std::string& frame_id, sensor_msgs::JointState& initialJointState, + geometry_msgs::PoseStamped& p1, geometry_msgs::PoseStamped& p2); + +void getOriChange(Eigen::Matrix3d& ori1, Eigen::Matrix3d& ori2); + +void createFakeCartTraj(const robot_trajectory::RobotTrajectoryPtr& traj, const std::string& link_name, + moveit_msgs::RobotTrajectory& fake_traj); + +inline geometry_msgs::Quaternion fromEuler(double a, double b, double c) +{ + tf2::Vector3 qvz(0., 0., 1.); + tf2::Vector3 qvy(0., 1., 0.); + tf2::Quaternion q1(qvz, a); + + q1 = q1 * tf2::Quaternion(qvy, b); + q1 = q1 * tf2::Quaternion(qvz, c); + + geometry_msgs::Quaternion msg; + tf2::convert(q1, msg); + return msg; +} + +/** + * @brief Test data for blending, which contains three joint position vectors of + * three robot state. + */ +struct BlendTestData +{ + std::vector start_position; + std::vector mid_position; + std::vector end_position; +}; + +/** + * @brief fetch test datasets from parameter server + * @param nh + * @return + */ +bool getBlendTestData(const ros::NodeHandle& nh, const size_t& dataset_num, const std::string& name_prefix, + std::vector& datasets); + +/** + * @brief check the blending result of lin-lin + * @param lin_res_1 + * @param lin_res_2 + * @param blend_req + * @param blend_res + * @param checkAcceleration + */ +bool checkBlendResult(const pilz_industrial_motion_planner::TrajectoryBlendRequest& blend_req, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& blend_res, + const pilz_industrial_motion_planner::LimitsContainer& limits, double joint_velocity_tolerance, + double joint_acceleration_tolerance, double cartesian_velocity_tolerance, + double cartesian_angular_velocity_tolerance); + +/** + * @brief generate two LIN trajectories from test data set + * @param data: contains joint poisitons of start/mid/end states + * @param sampling_time_1: sampling time for first LIN + * @param sampling_time_2: sampling time for second LIN + * @param[out] res_lin_1: result of the first LIN motion planning + * @param[out] res_lin_2: result of the second LIN motion planning + * @param[out] dis_lin_1: translational distance of the first LIN + * @param[out] dis_lin_2: translational distance of the second LIN + * @return true if succeed + */ +bool generateTrajFromBlendTestData(const moveit::core::RobotModelConstPtr& robot_model, + const std::shared_ptr& tg, + const std::string& group_name, const std::string& link_name, + const BlendTestData& data, const double& sampling_time_1, + const double& sampling_time_2, planning_interface::MotionPlanResponse& res_lin_1, + planning_interface::MotionPlanResponse& res_lin_2, double& dis_lin_1, + double& dis_lin_2); + +void generateRequestMsgFromBlendTestData(const moveit::core::RobotModelConstPtr& robot_model, const BlendTestData& data, + const std::string& planner_id, const std::string& group_name, + const std::string& link_name, moveit_msgs::MotionSequenceRequest& req_list); + +void checkRobotModel(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, + const std::string& link_name); + +/** @brief Check that a given vector of accelerations represents a trapezoid + * velocity profile + * @param acc_tol: tolerance for comparing acceleration values + */ +::testing::AssertionResult hasTrapezoidVelocity(std::vector accelerations, const double acc_tol); + +/** + * @brief Check that the translational path of a given trajectory has a + * trapezoid velocity profile + * @param acc_tol: tolerance for comparing acceleration values + */ +::testing::AssertionResult +checkCartesianTranslationalPath(const robot_trajectory::RobotTrajectoryConstPtr& trajectory, + const std::string& link_name, + const double acc_tol = DEFAULT_ACCELERATION_EQUALITY_TOLERANCE); + +/** + * @brief Check that the rotational path of a given trajectory is a quaternion + * slerp. + * @param rot_axis_tol: tolerance for comparing rotation axes (in the L2 norm) + * @param acc_tol: tolerance for comparing angular acceleration values + */ +::testing::AssertionResult +checkCartesianRotationalPath(const robot_trajectory::RobotTrajectoryConstPtr& trajectory, const std::string& link_name, + const double rot_axis_tol = DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE, + const double acc_tol = DEFAULT_ACCELERATION_EQUALITY_TOLERANCE); + +inline bool isMonotonouslyDecreasing(const std::vector& vec, const double& tol) +{ + return std::is_sorted(vec.begin(), vec.end(), [tol](double a, double b) { + return !(std::abs(a - b) < tol || a < b); // true -> a is ordered before b -> list is not sorted + }); +} + +} // namespace testutils diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.cpp new file mode 100644 index 0000000000..1809ada9c7 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.cpp @@ -0,0 +1,92 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/cartesian_limit.h" +#include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" + +/** + * @brief Unittest of the CartesianLimitsAggregator class + */ +class CartesianLimitsAggregator : public ::testing::Test +{ +}; + +/** + * @brief Check if only velocity is set + */ +TEST_F(CartesianLimitsAggregator, OnlyVelocity) +{ + ros::NodeHandle nh("~/vel_only"); + + pilz_industrial_motion_planner::CartesianLimit limit = + pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(nh); + EXPECT_TRUE(limit.hasMaxTranslationalVelocity()); + EXPECT_EQ(limit.getMaxTranslationalVelocity(), 10); + EXPECT_FALSE(limit.hasMaxTranslationalAcceleration()); + EXPECT_FALSE(limit.hasMaxTranslationalDeceleration()); + EXPECT_FALSE(limit.hasMaxRotationalVelocity()); +} + +/** + * @brief Check if all values are set correctly + */ +TEST_F(CartesianLimitsAggregator, AllValues) +{ + ros::NodeHandle nh("~/all"); + + pilz_industrial_motion_planner::CartesianLimit limit = + pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(nh); + EXPECT_TRUE(limit.hasMaxTranslationalVelocity()); + EXPECT_EQ(limit.getMaxTranslationalVelocity(), 1); + + EXPECT_TRUE(limit.hasMaxTranslationalAcceleration()); + EXPECT_EQ(limit.getMaxTranslationalAcceleration(), 2); + + EXPECT_TRUE(limit.hasMaxTranslationalDeceleration()); + EXPECT_EQ(limit.getMaxTranslationalDeceleration(), -3); + + EXPECT_TRUE(limit.hasMaxRotationalVelocity()); + EXPECT_EQ(limit.getMaxRotationalVelocity(), 4); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_cartesian_limits_aggregator"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.test new file mode 100644 index 0000000000..382a5a5924 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.test @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp new file mode 100644 index 0000000000..b749d48ae3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp @@ -0,0 +1,145 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 + +#include +#include + +#include + +#include "pilz_industrial_motion_planner/tip_frame_getter.h" + +namespace pilz_industrial_motion_planner +{ +using ::testing::AtLeast; +using ::testing::Return; +using ::testing::ReturnRef; + +class SolverMock +{ +public: + MOCK_CONST_METHOD0(getTipFrames, const std::vector&()); +}; + +class JointModelGroupMock +{ +public: + MOCK_CONST_METHOD0(getSolverInstance, SolverMock const*()); + MOCK_CONST_METHOD0(getName, const std::string&()); +}; + +/** + * @brief Test fixture for getSolverTipFrame tests. + */ +class GetSolverTipFrameTest : public testing::Test +{ +protected: + void SetUp() override; + +protected: + SolverMock solver_mock_; + JointModelGroupMock jmg_mock_; + const std::string group_name_{ "fake_group" }; +}; + +void GetSolverTipFrameTest::SetUp() +{ + EXPECT_CALL(jmg_mock_, getName()).WillRepeatedly(ReturnRef(group_name_)); +} + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + */ +TEST_F(GetSolverTipFrameTest, TestExceptionErrorCodeMapping) +{ + { + std::shared_ptr nse_ex{ new NoSolverException("") }; + EXPECT_EQ(nse_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + } + + { + std::shared_ptr ex{ new MoreThanOneTipFrameException("") }; + EXPECT_EQ(ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + } +} + +/** + * @brief Checks that an exceptions is thrown in case a group has more than + * one tip frame. + */ +TEST_F(GetSolverTipFrameTest, TestExceptionMoreThanOneTipFrame) +{ + std::vector tip_frames{ "fake_tip_frame1", "fake_tip_frame2" }; + + EXPECT_CALL(jmg_mock_, getSolverInstance()).Times(AtLeast(1)).WillRepeatedly(Return(&solver_mock_)); + + EXPECT_CALL(solver_mock_, getTipFrames()).Times(AtLeast(1)).WillRepeatedly(ReturnRef(tip_frames)); + + EXPECT_THROW(getSolverTipFrame(&jmg_mock_), MoreThanOneTipFrameException); +} + +/** + * @brief Checks that an exceptions is thrown in case a group does not + * possess a solver. + */ +TEST_F(GetSolverTipFrameTest, TestExceptionNoSolver) +{ + EXPECT_CALL(jmg_mock_, getSolverInstance()).WillOnce(Return(nullptr)); + + EXPECT_THROW(getSolverTipFrame(&jmg_mock_), NoSolverException); +} + +/** + * @brief Checks that an exceptions is thrown in case a nullptr is + * specified as JointModelGroup. + */ +TEST_F(GetSolverTipFrameTest, NullptrJointGroup) +{ + moveit::core::JointModelGroup* group = nullptr; + EXPECT_THROW(hasSolver(group), std::invalid_argument); +} + +} // namespace pilz_industrial_motion_planner + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.cpp new file mode 100644 index 0000000000..488a27bb00 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.cpp @@ -0,0 +1,111 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "ros/ros.h" + +#include + +#include + +#include "pilz_industrial_motion_planner/joint_limits_extension.h" +#include "pilz_industrial_motion_planner/joint_limits_interface_extension.h" + +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner::joint_limits_interface; + +namespace pilz_extensions_tests +{ +class JointLimitTest : public ::testing::Test +{ +}; + +TEST_F(JointLimitTest, SimpleRead) +{ + ros::NodeHandle node_handle("~"); + + // Joints limits interface + JointLimits joint_limits_extended; + JointLimits joint_limits; + + getJointLimits("joint_1", node_handle, joint_limits_extended); + + EXPECT_EQ(1, joint_limits_extended.max_acceleration); + EXPECT_EQ(-1, joint_limits_extended.max_deceleration); +} + +TEST_F(JointLimitTest, readNonExistingJointLimit) +{ + ros::NodeHandle node_handle("~"); + + // Joints limits interface + JointLimits joint_limits_extended; + JointLimits joint_limits; + + EXPECT_FALSE(getJointLimits("anything", node_handle, joint_limits_extended)); +} + +/** + * @brief Test reading a joint limit representing an invalid parameter key + * + * For full coverage. + */ +TEST_F(JointLimitTest, readInvalidParameterName) +{ + ros::NodeHandle node_handle("~"); + + // Joints limits interface + JointLimits joint_limits_extended; + JointLimits joint_limits; + + EXPECT_FALSE(getJointLimits("~anything", node_handle, joint_limits_extended)); +} + +TEST_F(JointLimitTest, OldRead) +{ + ros::NodeHandle node_handle("~"); + + // Joints limits interface + JointLimits joint_limits; + getJointLimits("joint_1", node_handle, joint_limits); + + EXPECT_EQ(1, joint_limits.max_acceleration); +} +} // namespace pilz_extensions_tests + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_joint_limits_extended"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.test new file mode 100644 index 0000000000..361e6f5102 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.test @@ -0,0 +1,40 @@ + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit_config.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit_config.yaml new file mode 100644 index 0000000000..4f4bfffd6e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit_config.yaml @@ -0,0 +1,31 @@ +joint_limits: + joint_1: + has_acceleration_limits: true + max_acceleration: 1 + has_deceleration_limits: true + max_deceleration: -1 + joint_2: + has_acceleration_limits: true + max_acceleration: 2 + has_deceleration_limits: true + max_deceleration: -2 + joint_3: + has_acceleration_limits: true + max_acceleration: 3 + has_deceleration_limits: true + max_deceleration: -3 + joint_4: + has_acceleration_limits: true + max_acceleration: 4 + has_deceleration_limits: true + max_deceleration: -4 + joint_5: + has_acceleration_limits: true + max_acceleration: 5 + has_deceleration_limits: true + max_deceleration: -5 + joint_6: + has_acceleration_limits: true + max_acceleration: 6 + has_deceleration_limits: true + max_deceleration: -6 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.cpp new file mode 100644 index 0000000000..06827e3284 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.cpp @@ -0,0 +1,208 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/joint_limits_extension.h" +#include "pilz_industrial_motion_planner/joint_limits_interface_extension.h" + +/** + * @brief Unittest of the JointLimitsAggregator class + */ +class JointLimitsAggregator : public ::testing::Test +{ +protected: + void SetUp() override + { + ros::NodeHandle node_handle("~"); + + // Load robot module + robot_model_loader::RobotModelLoader::Options opt("robot_description"); + model_loader_.reset(new robot_model_loader::RobotModelLoader(opt)); + robot_model_ = model_loader_->getModel(); + + return; + } + + /// The robot model loader + robot_model_loader::RobotModelLoaderPtr model_loader_; + + /// The robot model + robot_model::RobotModelConstPtr robot_model_; +}; + +/** + * @brief Check for that the size of the map and the size of the given joint + * models is equal + */ +TEST_F(JointLimitsAggregator, ExpectedMapSize) +{ + ros::NodeHandle nh("~"); + + pilz_industrial_motion_planner::JointLimitsContainer container = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh, + robot_model_->getActiveJointModels()); + + EXPECT_EQ(robot_model_->getActiveJointModels().size(), container.getCount()); +} + +/** + * @brief Check that the value in the parameter server correctly overrides the + * position(if within limits) + */ +TEST_F(JointLimitsAggregator, CorrectOverwriteByParamterPosition) +{ + ros::NodeHandle nh("~/valid_1"); + + pilz_industrial_motion_planner::JointLimitsContainer container = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh, + robot_model_->getActiveJointModels()); + + for (std::pair lim : container) + { + // Check for the overwrite + if (lim.first == "prbt_joint_1") + { + EXPECT_EQ(2, container.getLimit(lim.first).max_position); + EXPECT_EQ(-2, container.getLimit(lim.first).min_position); + } + // Check that nothing else changed + else + { + EXPECT_EQ(robot_model_->getJointModel(lim.first)->getVariableBounds()[0].min_position_, + container.getLimit(lim.first).min_position); + EXPECT_EQ(robot_model_->getJointModel(lim.first)->getVariableBounds()[0].max_position_, + container.getLimit(lim.first).max_position); + } + } +} + +/** + * @brief Check that the value in the parameter server correctly overrides the + * velocity(if within limits) + */ +TEST_F(JointLimitsAggregator, CorrectOverwriteByParamterVelocity) +{ + ros::NodeHandle nh("~/valid_1"); + + pilz_industrial_motion_planner::JointLimitsContainer container = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh, + robot_model_->getActiveJointModels()); + + for (std::pair lim : container) + { + // Check that velocity was only changed in joint "prbt_joint_3" + if (lim.first == "prbt_joint_3") + { + EXPECT_EQ(1.1, container.getLimit(lim.first).max_velocity); + } + else + { + EXPECT_EQ(robot_model_->getJointModel(lim.first)->getVariableBounds()[0].max_velocity_, + container.getLimit(lim.first).max_velocity); + } + } +} + +/** + * @brief Check that the acceleration and deceleration are set properly + */ +TEST_F(JointLimitsAggregator, CorrectSettingAccelerationAndDeceleration) +{ + ros::NodeHandle nh("~/valid_1"); + + pilz_industrial_motion_planner::JointLimitsContainer container = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh, + robot_model_->getActiveJointModels()); + + for (std::pair lim : container) + { + if (lim.first == "prbt_joint_4") + { + EXPECT_EQ(5.5, container.getLimit(lim.first).max_acceleration) << lim.first; + EXPECT_EQ(-5.5, container.getLimit(lim.first).max_deceleration) << lim.first; + } + else if (lim.first == "prbt_joint_5") + { + EXPECT_EQ(0, container.getLimit(lim.first).max_acceleration) << lim.first; + EXPECT_EQ(-6.6, container.getLimit(lim.first).max_deceleration) << lim.first; + } + else + { + EXPECT_EQ(0, container.getLimit(lim.first).max_acceleration) << lim.first; + EXPECT_EQ(0, container.getLimit(lim.first).max_deceleration) << lim.first; + } + } +} + +/** + * @brief Check that position limit violations are detected properly + */ +TEST_F(JointLimitsAggregator, LimitsViolationPosition) +{ + ros::NodeHandle nh_min("~/violate_position_min"); + + EXPECT_THROW(pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + nh_min, robot_model_->getActiveJointModels()), + pilz_industrial_motion_planner::AggregationBoundsViolationException); + + ros::NodeHandle nh_max("~/violate_position_max"); + + EXPECT_THROW(pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + nh_max, robot_model_->getActiveJointModels()), + pilz_industrial_motion_planner::AggregationBoundsViolationException); +} + +/** + * @brief Check that velocity limit violations are detected properly + */ +TEST_F(JointLimitsAggregator, LimitsViolationVelocity) +{ + ros::NodeHandle nh("~/violate_velocity"); + + EXPECT_THROW(pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + nh, robot_model_->getActiveJointModels()), + pilz_industrial_motion_planner::AggregationBoundsViolationException); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_joint_limits_aggregator"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.test new file mode 100644 index 0000000000..28177da403 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.test @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp new file mode 100644 index 0000000000..2f7f7b80a4 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp @@ -0,0 +1,229 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_container.h" +#include "pilz_industrial_motion_planner/joint_limits_extension.h" + +using namespace pilz_industrial_motion_planner; + +class JointLimitsContainerTest : public ::testing::Test +{ +protected: + void SetUp() override + { + JointLimit lim1; + lim1.has_position_limits = true; + lim1.min_position = -2; + lim1.max_position = 2; + lim1.has_acceleration_limits = true; + lim1.max_acceleration = 3; //<- Expected for common_limit_.max_acceleration + + JointLimit lim2; + lim2.has_position_limits = true; + lim2.min_position = -1; //<- Expected for common_limit_.min_position + lim2.max_position = 1; //<- Expected for common_limit_.max_position + lim2.has_deceleration_limits = true; + lim2.max_deceleration = -5; //<- Expected for common_limit_.max_deceleration + + JointLimit lim3; + lim3.has_velocity_limits = true; + lim3.max_velocity = 10; + + JointLimit lim4; + lim4.has_position_limits = true; + lim4.min_position = -1; + lim4.max_position = 1; + lim4.has_acceleration_limits = true; + lim4.max_acceleration = 400; + lim4.has_deceleration_limits = false; + lim4.max_deceleration = -1; + + JointLimit lim5; + lim5.has_position_limits = true; + lim5.min_position = -1; + lim5.max_position = 1; + lim5.has_acceleration_limits = false; + lim5.max_acceleration = 1; + + JointLimit lim6; + lim6.has_velocity_limits = true; + lim6.max_velocity = 2; //<- Expected for common_limit_.max_velocity + lim6.has_deceleration_limits = true; + lim6.max_deceleration = -100; + + container_.addLimit("joint1", lim1); + container_.addLimit("joint2", lim2); + container_.addLimit("joint3", lim3); + container_.addLimit("joint4", lim4); + container_.addLimit("joint5", lim5); + container_.addLimit("joint6", lim6); + + common_limit_ = container_.getCommonLimit(); + } + + pilz_industrial_motion_planner::JointLimitsContainer container_; + JointLimit common_limit_; +}; + +/** + * @brief Check postion + */ +TEST_F(JointLimitsContainerTest, CheckPositionUnification) +{ + EXPECT_EQ(-1, common_limit_.min_position); + EXPECT_EQ(1, common_limit_.max_position); +} + +/** + * @brief Check velocity + */ +TEST_F(JointLimitsContainerTest, CheckVelocityUnification) +{ + EXPECT_EQ(2, common_limit_.max_velocity); +} + +/** + * @brief Check acceleration + */ +TEST_F(JointLimitsContainerTest, CheckAccelerationUnification) +{ + EXPECT_EQ(3, common_limit_.max_acceleration); +} + +/** + * @brief Check deceleration + */ +TEST_F(JointLimitsContainerTest, CheckDecelerationUnification) +{ + EXPECT_EQ(-5, common_limit_.max_deceleration); +} + +/** + * @brief Check AddLimit for positive and null deceleration + */ +TEST_F(JointLimitsContainerTest, CheckAddLimitDeceleration) +{ + JointLimit lim_invalid1; + lim_invalid1.has_deceleration_limits = true; + lim_invalid1.max_deceleration = 0; + + JointLimit lim_invalid2; + lim_invalid2.has_deceleration_limits = true; + lim_invalid2.max_deceleration = 1; + + JointLimit lim_valid; + lim_valid.has_deceleration_limits = true; + lim_valid.max_deceleration = -1; + + pilz_industrial_motion_planner::JointLimitsContainer container; + + EXPECT_EQ(false, container.addLimit("joint_invalid1", lim_invalid1)); + EXPECT_EQ(false, container.addLimit("joint_invalid2", lim_invalid2)); + EXPECT_EQ(true, container.addLimit("joint_valid", lim_valid)); +} + +/** + * @brief Check AddLimit for already contained limit + */ +TEST_F(JointLimitsContainerTest, CheckAddLimitAlreadyContained) +{ + JointLimit lim_valid; + lim_valid.has_deceleration_limits = true; + lim_valid.max_deceleration = -1; + + pilz_industrial_motion_planner::JointLimitsContainer container; + ASSERT_TRUE(container.addLimit("joint_valid", lim_valid)); + EXPECT_FALSE(container.addLimit("joint_valid", lim_valid)); +} + +/** + * @brief An uninitialized container should not have any limits set. + */ +TEST_F(JointLimitsContainerTest, CheckEmptyContainer) +{ + pilz_industrial_motion_planner::JointLimitsContainer container; + JointLimit limits = container.getCommonLimit(); + EXPECT_FALSE(limits.has_position_limits); + EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_FALSE(limits.has_acceleration_limits); +} + +/** + * @brief empty position limits for first joint, second one should be returned + */ +TEST_F(JointLimitsContainerTest, FirstPositionEmpty) +{ + JointLimit lim1; + + JointLimit lim2; + lim2.has_position_limits = true; + lim2.min_position = -1; //<- Expected for common_limit_.min_position + lim2.max_position = 1; //<- Expected for common_limit_.max_position + + pilz_industrial_motion_planner::JointLimitsContainer container; + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + JointLimit limits = container.getCommonLimit(); + EXPECT_TRUE(limits.has_position_limits); + EXPECT_EQ(1, limits.max_position); + EXPECT_EQ(-1, limits.min_position); +} + +/** + * @brief Check position limits + */ +TEST_F(JointLimitsContainerTest, CheckVerifyPositionLimits) +{ + // positive check: inside limits + std::vector joint_names{ "joint1", "joint2" }; + std::vector joint_positions{ 0.5, 0.5 }; + EXPECT_TRUE(container_.verifyPositionLimits(joint_names, joint_positions)); + + // outside limit2 + joint_positions[1] = 7; + EXPECT_FALSE(container_.verifyPositionLimits(joint_names, joint_positions)); + + // invalid size + std::vector joint_positions1{ 0. }; + EXPECT_THROW(container_.verifyPositionLimits(joint_names, joint_positions1), std::out_of_range); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_validator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_validator.cpp new file mode 100644 index 0000000000..f622cdf8f1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_validator.cpp @@ -0,0 +1,490 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_extension.h" + +#include "pilz_industrial_motion_planner/joint_limits_validator.h" + +using namespace pilz_industrial_motion_planner; + +class JointLimitsValidatorTest : public ::testing::Test +{ +}; + +/** + * @brief Check postion equality + */ +TEST_F(JointLimitsValidatorTest, CheckPositionEquality) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_position_limits = true; + lim1.min_position = -1; + lim1.max_position = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim1); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check postion inequality in min_position detection + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMinPosition) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_position_limits = true; + lim1.min_position = -1; + lim1.max_position = 1; + + JointLimit lim2; + lim2.has_position_limits = true; + lim2.min_position = -2; + lim2.max_position = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(false, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check postion inequality in max_position detection + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxPosition1) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_position_limits = true; + lim1.min_position = -1; + lim1.max_position = 2; + + JointLimit lim2; + lim2.has_position_limits = true; + lim2.min_position = -1; + lim2.max_position = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(false, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check postion inequality in max_position detection (using 3 limits) + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxPosition2) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_position_limits = true; + lim1.min_position = -1; + lim1.max_position = 2; + + JointLimit lim2; + lim2.has_position_limits = true; + lim2.min_position = -1; + lim2.max_position = 1; + + JointLimit lim3; + lim3.has_position_limits = true; + lim3.min_position = -1; + lim3.max_position = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + container.addLimit("joint3", lim3); + + EXPECT_EQ(false, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check postion inequality in has_position_limits false detection + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityHasPositionLimits) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_position_limits = true; + lim1.min_position = -1; + lim1.max_position = 1; + + JointLimit lim2; + lim2.has_position_limits = false; + lim2.min_position = -1; + lim2.max_position = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(false, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- +// VELOCITY +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- + +/** + * @brief Check velocity equality + */ +TEST_F(JointLimitsValidatorTest, CheckVelocityEquality) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_velocity_limits = true; + lim1.max_velocity = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim1); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check max_velocity inequality + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxVelocity1) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_velocity_limits = true; + lim1.max_velocity = 1; + + JointLimit lim2; + lim2.has_velocity_limits = true; + lim2.max_velocity = 2; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check max_velocity inequality (using 3Limits) + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxVelocity2) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_velocity_limits = true; + lim1.max_velocity = 1; + + JointLimit lim2; + lim2.has_velocity_limits = true; + lim2.max_velocity = 2; + + JointLimit lim3; + lim3.has_velocity_limits = true; + lim3.max_velocity = 2; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + container.addLimit("joint3", lim3); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check postion inequality in has_position_limits false detection + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityHasVelocityLimits) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_velocity_limits = true; + + JointLimit lim2; + lim2.has_velocity_limits = false; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- +// ACCELERATION +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- + +/** + * @brief Check acceleration equality + */ +TEST_F(JointLimitsValidatorTest, CheckAccelerationEquality) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_acceleration_limits = true; + lim1.max_acceleration = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim1); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check max_acceleration inequality + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxAcceleration1) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_acceleration_limits = true; + lim1.max_acceleration = 1; + + JointLimit lim2; + lim2.has_acceleration_limits = true; + lim2.max_acceleration = 2; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check max_acceleration inequality (using 3 Limits) + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxAcceleration2) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_acceleration_limits = true; + lim1.max_acceleration = 1; + + JointLimit lim2; + lim2.has_acceleration_limits = true; + lim2.max_acceleration = 2; + + JointLimit lim3; + lim3.has_acceleration_limits = true; + lim3.max_acceleration = 2; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + container.addLimit("joint3", lim3); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check acceleration inequality in has_acceleration_limits false + * detection + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityHasAccelerationLimits) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_acceleration_limits = true; + + JointLimit lim2; + lim2.has_acceleration_limits = false; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- +// DECELERATION +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- + +/** + * @brief Check deceleration equality + */ +TEST_F(JointLimitsValidatorTest, CheckDecelerationEquality) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_deceleration_limits = true; + lim1.max_deceleration = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim1); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check max_deceleration inequality + */ +TEST_F(JointLimitsValidatorTest, CheckInEqualityMaxDeceleration1) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_deceleration_limits = true; + lim1.max_deceleration = -1; + + JointLimit lim2; + lim2.has_deceleration_limits = true; + lim2.max_deceleration = -2; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check max_deceleration inequality + */ +TEST_F(JointLimitsValidatorTest, CheckInEqualityMaxDeceleration2) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_deceleration_limits = true; + lim1.max_deceleration = -1; + + JointLimit lim2; + lim2.has_deceleration_limits = true; + lim2.max_deceleration = -2; + + JointLimit lim3; + lim3.has_deceleration_limits = true; + lim3.max_deceleration = -2; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + container.addLimit("joint3", lim3); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check deceleration inequality in has_deceleration_limits false + * detection + */ +TEST_F(JointLimitsValidatorTest, CheckInEqualityHasDecelerationLimits) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_deceleration_limits = true; + lim1.max_deceleration = -1; + + JointLimit lim2; + lim2.has_deceleration_limits = false; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + ASSERT_EQ(2u, container.getCount()); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp new file mode 100644 index 0000000000..f413cb835a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp @@ -0,0 +1,232 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 + +#include "pilz_industrial_motion_planner/pilz_industrial_motion_planner.h" +#include "test_utils.h" + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +class CommandPlannerTest : public testing::TestWithParam +{ +protected: + void SetUp() override + { + createPlannerInstance(); + } + + /** + * @brief initialize the planner plugin + * The planner is loaded using the pluginlib. Checks that the planner was + * loaded properly. + * Exceptions will cause test failure. + * + * This function should be called only once during initialization of the + * class. + */ + void createPlannerInstance() + { + // Load planner name from parameter server + if (!ph_.getParam("planning_plugin", planner_plugin_name_)) + { + ROS_FATAL_STREAM("Could not find planner plugin name"); + FAIL(); + } + + // Load the plugin + try + { + planner_plugin_loader_.reset(new pluginlib::ClassLoader( + "moveit_core", "planning_interface::PlannerManager")); + } + catch (pluginlib::PluginlibException& ex) + { + ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); + FAIL(); + } + + // Create planner + try + { + planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_name_)); + ASSERT_TRUE(planner_instance_->initialize(robot_model_, ph_.getNamespace())) + << "Initialzing the planner instance failed."; + } + catch (pluginlib::PluginlibException& ex) + { + FAIL() << "Could not create planner " << ex.what() << "\n"; + } + } + + void TearDown() override + { + planner_plugin_loader_->unloadLibraryForClass(planner_plugin_name_); + } + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + + std::string planner_plugin_name_; + + std::unique_ptr> planner_plugin_loader_; + + planning_interface::PlannerManagerPtr planner_instance_; +}; + +// Instantiate the test cases for robot model with and without gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, CommandPlannerTest, + ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); + +/** + * @brief Test that PTP can be loaded + * This needs to be extended with every new planning Algorithm + */ +TEST_P(CommandPlannerTest, ObtainLoadedPlanningAlgorithms) +{ + // Check for the algorithms + std::vector algs; + + planner_instance_->getPlanningAlgorithms(algs); + ASSERT_EQ(3u, algs.size()) << "Found more or less planning algorithms as expected! Found:" + << ::testing::PrintToString(algs); + + // Collect the algorithms, check for uniqueness + std::set algs_set; + for (const auto& alg : algs) + { + algs_set.insert(alg); + } + ASSERT_EQ(algs.size(), algs_set.size()) << "There are two or more algorithms with the same name!"; + ASSERT_TRUE(algs_set.find("LIN") != algs_set.end()); + ASSERT_TRUE(algs_set.find("PTP") != algs_set.end()); + ASSERT_TRUE(algs_set.find("CIRC") != algs_set.end()); +} + +/** + * @brief Check that all announced planning algorithms can perform the service + * request if the planner_id is set. + */ +TEST_P(CommandPlannerTest, CheckValidAlgorithmsForServiceRequest) +{ + // Check for the algorithms + std::vector algs; + planner_instance_->getPlanningAlgorithms(algs); + + for (const auto& alg : algs) + { + planning_interface::MotionPlanRequest req; + req.planner_id = alg; + + EXPECT_TRUE(planner_instance_->canServiceRequest(req)); + } +} + +/** + * @brief Check that canServiceRequest(req) returns false if planner_id is not + * supported + */ +TEST_P(CommandPlannerTest, CheckInvalidAlgorithmsForServiceRequest) +{ + planning_interface::MotionPlanRequest req; + req.planner_id = "NON_EXISTEND_ALGORITHM_HASH_da39a3ee5e6b4b0d3255bfef95601890afd80709"; + + EXPECT_FALSE(planner_instance_->canServiceRequest(req)); +} + +/** + * @brief Check that canServiceRequest(req) returns false if planner_id is empty + */ +TEST_P(CommandPlannerTest, CheckEmptyPlannerIdForServiceRequest) +{ + planning_interface::MotionPlanRequest req; + req.planner_id = ""; + + EXPECT_FALSE(planner_instance_->canServiceRequest(req)); +} + +/** + * @brief Check integrety against empty input + */ +TEST_P(CommandPlannerTest, CheckPlanningContextRequestNull) +{ + moveit_msgs::MotionPlanRequest req; + moveit_msgs::MoveItErrorCodes error_code; + EXPECT_EQ(nullptr, planner_instance_->getPlanningContext(nullptr, req, error_code)); +} + +/** + * @brief Check that for the announced algorithmns getPlanningContext does not + * return nullptr + */ +TEST_P(CommandPlannerTest, CheckPlanningContextRequest) +{ + moveit_msgs::MotionPlanRequest req; + moveit_msgs::MoveItErrorCodes error_code; + + // Check for the algorithms + std::vector algs; + planner_instance_->getPlanningAlgorithms(algs); + + for (const auto& alg : algs) + { + req.planner_id = alg; + + EXPECT_NE(nullptr, planner_instance_->getPlanningContext(nullptr, req, error_code)); + } +} + +/** + * @brief Check the description can be obtained and is not empty + */ +TEST_P(CommandPlannerTest, CheckPlanningContextDescriptionNotEmptyAndStable) +{ + std::string desc = planner_instance_->getDescription(); + EXPECT_GT(desc.length(), 0u); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_pilz_industrial_motion_planner"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.test new file mode 100644 index 0000000000..574f005626 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.test @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner_direct.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner_direct.cpp new file mode 100644 index 0000000000..35bb528d27 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner_direct.cpp @@ -0,0 +1,121 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/pilz_industrial_motion_planner.h" +#include "pilz_industrial_motion_planner/planning_context_loader_ptp.h" +#include "pilz_industrial_motion_planner/planning_exceptions.h" + +using namespace pilz_industrial_motion_planner; + +TEST(CommandPlannerTestDirect, ExceptionCoverage) +{ + std::shared_ptr p_ex{ new PlanningException("") }; + std::shared_ptr clr_ex{ new ContextLoaderRegistrationException("") }; +} + +/** + * This test uses pilz_industrial_motion_planner::CommandPlanner directly and + * is thus seperated from + * unittest_pilz_industrial_motion_planner.cpp since plugin loading via + * pluginlib does not allow loading of classes + * already + * defined. + */ + +/** + * @brief Check that a exception is thrown if a already loaded + * PlanningContextLoader is loaded + * + * It this point the planning_instance_ has loaded ptp, lin, circ. + * A additional ptp is loaded which should throw the respective exception. + */ +TEST(CommandPlannerTestDirect, CheckDoubleLoadingException) +{ + /// Registed a found loader + pilz_industrial_motion_planner::CommandPlanner planner; + pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader( + new pilz_industrial_motion_planner::PlanningContextLoaderPTP()); + + EXPECT_NO_THROW(planner.registerContextLoader(planning_context_loader)); + + EXPECT_THROW(planner.registerContextLoader(planning_context_loader), + pilz_industrial_motion_planner::ContextLoaderRegistrationException); +} + +/** + * @brief Check that getPlanningContext() fails if the underlying ContextLoader + * fails to load the context. + */ +TEST(CommandPlannerTestDirect, FailOnLoadContext) +{ + pilz_industrial_motion_planner::CommandPlanner planner; + + // Mock of failing PlanningContextLoader + class TestPlanningContextLoader : public pilz_industrial_motion_planner::PlanningContextLoader + { + public: + std::string getAlgorithm() const override + { + return "Test_Algorithm"; + } + + bool loadContext(planning_interface::PlanningContextPtr& /* planning_context */, const std::string& /* name */, + const std::string& /* group */) const override + { + // Mock behaviour: Cannot load planning context. + return false; + } + }; + + /// Registed a found loader + pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader(new TestPlanningContextLoader()); + planner.registerContextLoader(planning_context_loader); + + moveit_msgs::MotionPlanRequest req; + req.planner_id = "Test_Algorithm"; + + moveit_msgs::MoveItErrorCodes error_code; + EXPECT_FALSE(planner.getPlanningContext(nullptr, req, error_code)); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::PLANNING_FAILED, error_code.val); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp new file mode 100644 index 0000000000..016c612e0a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp @@ -0,0 +1,282 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 + +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/joint_limits_container.h" +#include "pilz_industrial_motion_planner/planning_context_circ.h" +#include "pilz_industrial_motion_planner/planning_context_lin.h" +#include "pilz_industrial_motion_planner/planning_context_ptp.h" + +#include "test_utils.h" + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string PARAM_TARGET_LINK_NAME("target_link"); + +/** + * A value type container to combine type and value + * In the tests types are trajectory generators. + * value = 0 refers to robot model without gripper + * value = 1 refers to robot model with gripper + */ +template +class ValueTypeContainer +{ +public: + typedef T Type_; + static const int VALUE = N; +}; +template +const int ValueTypeContainer::VALUE; + +typedef ValueTypeContainer PTP_NO_GRIPPER; +typedef ValueTypeContainer PTP_WITH_GRIPPER; +typedef ValueTypeContainer LIN_NO_GRIPPER; +typedef ValueTypeContainer LIN_WITH_GRIPPER; +typedef ValueTypeContainer CIRC_NO_GRIPPER; +typedef ValueTypeContainer CIRC_WITH_GRIPPER; + +typedef ::testing::Types + PlanningContextTestTypes; + +/** + * type parameterized test fixture + */ +template +class PlanningContextTest : public ::testing::Test +{ +protected: + void SetUp() override + { + ASSERT_FALSE(robot_model_ == nullptr) << "There is no robot model!"; + + // get parameters + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + testutils::createFakeLimits(robot_model_->getVariableNames()); + pilz_industrial_motion_planner::CartesianLimit cartesian_limit; + cartesian_limit.setMaxRotationalVelocity(1.0 * M_PI); + cartesian_limit.setMaxTranslationalAcceleration(1.0 * M_PI); + cartesian_limit.setMaxTranslationalDeceleration(1.0 * M_PI); + cartesian_limit.setMaxTranslationalVelocity(1.0 * M_PI); + + pilz_industrial_motion_planner::LimitsContainer limits; + limits.setJointLimits(joint_limits); + limits.setCartesianLimits(cartesian_limit); + + planning_context_ = std::unique_ptr( + new typename T::Type_("TestPlanningContext", "TestGroup", robot_model_, limits)); + + // Define and set the current scene + planning_scene::PlanningScenePtr scene(new planning_scene::PlanningScene(robot_model_)); + robot_state::RobotState current_state(robot_model_); + current_state.setToDefaultValues(); + current_state.setJointGroupPositions(planning_group_, { 0, 1.57, 1.57, 0, 0.2, 0 }); + scene->setCurrentState(current_state); + planning_context_->setPlanningScene(scene); // TODO Check what happens if this is missing + } + + /** + * @brief Generate a valid fully defined request + */ + planning_interface::MotionPlanRequest getValidRequest(const std::string& context_name) const + { + planning_interface::MotionPlanRequest req; + + req.planner_id = + std::string(context_name).erase(0, std::string("pilz_industrial_motion_planner::PlanningContext").length()); + req.group_name = this->planning_group_; + req.max_velocity_scaling_factor = 0.01; + req.max_acceleration_scaling_factor = 0.01; + + // start state + robot_state::RobotState rstate(this->robot_model_); + rstate.setToDefaultValues(); + // state state in joint space, used as initial positions, since IK does not + // work at zero positions + rstate.setJointGroupPositions(this->planning_group_, + { 4.430233957464225e-12, 0.007881892504574495, -1.8157263253868452, + 1.1801525390026025e-11, 1.8236082178909834, 8.591793942969161e-12 }); + Eigen::Isometry3d start_pose(Eigen::Isometry3d::Identity()); + start_pose.translation() = Eigen::Vector3d(0.3, 0, 0.65); + rstate.setFromIK(this->robot_model_->getJointModelGroup(this->planning_group_), start_pose); + moveit::core::robotStateToRobotStateMsg(rstate, req.start_state, false); + + // goal constraint + Eigen::Isometry3d goal_pose(Eigen::Isometry3d::Identity()); + goal_pose.translation() = Eigen::Vector3d(0, 0.3, 0.65); + Eigen::Matrix3d goal_rotation; + goal_rotation = Eigen::AngleAxisd(0 * M_PI, Eigen::Vector3d::UnitZ()); + goal_pose.linear() = goal_rotation; + rstate.setFromIK(this->robot_model_->getJointModelGroup(this->planning_group_), goal_pose); + req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + rstate, this->robot_model_->getJointModelGroup(this->planning_group_))); + + // path constraint + req.path_constraints.name = "center"; + moveit_msgs::PositionConstraint center_point; + center_point.link_name = this->target_link_; + geometry_msgs::Pose center_position; + center_position.position.x = 0.0; + center_position.position.y = 0.0; + center_position.position.z = 0.65; + center_point.constraint_region.primitive_poses.push_back(center_position); + req.path_constraints.position_constraints.push_back(center_point); + + return req; + } + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ + robot_model_loader::RobotModelLoader(!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME) + .getModel() + }; + + std::unique_ptr planning_context_; + + std::string planning_group_, target_link_; +}; + +// Define the types we need to test +TYPED_TEST_SUITE(PlanningContextTest, PlanningContextTestTypes); + +/** + * @brief No request is set. Check the output of solve. Using robot model + * without gripper. + */ +TYPED_TEST(PlanningContextTest, NoRequest) +{ + planning_interface::MotionPlanResponse res; + bool result = this->planning_context_->solve(res); + + EXPECT_FALSE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN, res.error_code_.val) + << testutils::demangel(typeid(TypeParam).name()); +} + +/** + * @brief Solve a valid request. + */ +TYPED_TEST(PlanningContextTest, SolveValidRequest) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name())); + + this->planning_context_->setMotionPlanRequest(req); + + // TODO Formulate valid request + bool result = this->planning_context_->solve(res); + + EXPECT_TRUE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val) + << testutils::demangel(typeid(TypeParam).name()); + + planning_interface::MotionPlanDetailedResponse res_detailed; + bool result_detailed = this->planning_context_->solve(res_detailed); + + EXPECT_TRUE(result_detailed) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val) + << testutils::demangel(typeid(TypeParam).name()); +} + +/** + * @brief Solve a valid request. Expect a detailed response. + */ +TYPED_TEST(PlanningContextTest, SolveValidRequestDetailedResponse) +{ + planning_interface::MotionPlanDetailedResponse res; //<-- Detailed! + planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name())); + + this->planning_context_->setMotionPlanRequest(req); + bool result = this->planning_context_->solve(res); + + EXPECT_TRUE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val) + << testutils::demangel(typeid(TypeParam).name()); +} + +/** + * @brief Call solve on a terminated context. + */ +TYPED_TEST(PlanningContextTest, SolveOnTerminated) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name())); + + this->planning_context_->setMotionPlanRequest(req); + + bool result_termination = this->planning_context_->terminate(); + EXPECT_TRUE(result_termination) << testutils::demangel(typeid(TypeParam).name()); + + bool result = this->planning_context_->solve(res); + EXPECT_FALSE(result) << testutils::demangel(typeid(TypeParam).name()); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::PLANNING_FAILED, res.error_code_.val) + << testutils::demangel(typeid(TypeParam).name()); +} + +/** + * @brief Check if clear can be called. So far only stability is expected. + */ +TYPED_TEST(PlanningContextTest, Clear) +{ + EXPECT_NO_THROW(this->planning_context_->clear()) << testutils::demangel(typeid(TypeParam).name()); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_planning_context"); + // ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.test new file mode 100644 index 0000000000..048e67c804 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.test @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp new file mode 100644 index 0000000000..a8926ed17e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp @@ -0,0 +1,179 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 + +// Boost includes +#include + +#include + +#include +#include + +#include "pilz_industrial_motion_planner/planning_context_loader.h" + +#include "test_utils.h" + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +class PlanningContextLoadersTest : public ::testing::TestWithParam> +{ +protected: + /** + * @brief To initialize the planning context loader + * The planning context loader is loaded using the pluginlib. + * Checks if planning context loader was loaded properly are performed. + * Exceptions will cause test failure. + */ + void SetUp() override + { + ASSERT_FALSE(robot_model_ == nullptr) << "There is no robot model!"; + + // Load the plugin + try + { + planning_context_loader_class_loader_.reset( + new pluginlib::ClassLoader( + "pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader")); + } + catch (pluginlib::PluginlibException& ex) + { + ROS_FATAL_STREAM("Exception while creating planning context loader " << ex.what()); + FAIL(); + } + + // Create planning context loader ptp + try + { + planning_context_loader_.reset(planning_context_loader_class_loader_->createUnmanagedInstance(GetParam().front())); + } + catch (pluginlib::PluginlibException& ex) + { + FAIL() << ex.what(); + } + return; + } + + void TearDown() override + { + if (planning_context_loader_class_loader_) + { + planning_context_loader_class_loader_->unloadLibraryForClass(GetParam().front()); + } + } + +protected: + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam().back()).getModel() }; + + // Load the plugin + boost::scoped_ptr> + planning_context_loader_class_loader_; + + pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader_; +}; + +// Instantiate the test cases for all loaders, extend here if you added a new +// ContextLoader you want to test +INSTANTIATE_TEST_SUITE_P( + InstantiationName, PlanningContextLoadersTest, + ::testing::Values(std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderPTP", "PTP", + PARAM_MODEL_NO_GRIPPER_NAME }, // Test for PTP + std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderPTP", "PTP", + PARAM_MODEL_WITH_GRIPPER_NAME }, // Test for PTP + std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderLIN", "LIN", + PARAM_MODEL_NO_GRIPPER_NAME }, // Test for LIN + std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderLIN", "LIN", + PARAM_MODEL_WITH_GRIPPER_NAME }, // Test for LIN + std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderCIRC", "CIRC", + PARAM_MODEL_NO_GRIPPER_NAME }, // Test for CIRC + std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderCIRC", "CIRC", + PARAM_MODEL_WITH_GRIPPER_NAME } // Test for CIRC + )); + +/** + * @brief Test getAlgorithm returns PTP + */ +TEST_P(PlanningContextLoadersTest, GetAlgorithm) +{ + std::string alg = planning_context_loader_->getAlgorithm(); + EXPECT_EQ(alg, GetParam().at(1)); +} + +/** + * @brief Check that load Context returns initialized shared pointer + */ +TEST_P(PlanningContextLoadersTest, LoadContext) +{ + planning_interface::PlanningContextPtr planning_context; + + // Without limits should return false + bool res = planning_context_loader_->loadContext(planning_context, "test", "test"); + EXPECT_EQ(false, res) << "Context returned even when no limits where set"; + + // After setting the limits this should work + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + testutils::createFakeLimits(robot_model_->getVariableNames()); + pilz_industrial_motion_planner::LimitsContainer limits; + limits.setJointLimits(joint_limits); + pilz_industrial_motion_planner::CartesianLimit cart_limits; + cart_limits.setMaxRotationalVelocity(1 * M_PI); + cart_limits.setMaxTranslationalAcceleration(2); + cart_limits.setMaxTranslationalDeceleration(2); + cart_limits.setMaxTranslationalVelocity(1); + limits.setCartesianLimits(cart_limits); + + planning_context_loader_->setLimits(limits); + planning_context_loader_->setModel(robot_model_); + + try + { + res = planning_context_loader_->loadContext(planning_context, "test", "test"); + } + catch (std::exception& ex) + { + FAIL() << "Exception!" << ex.what() << " " << typeid(ex).name(); + } + + EXPECT_EQ(true, res) << "Context could not be loaded!"; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_planning_context_loaders"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.test new file mode 100644 index 0000000000..766bc1af98 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.test @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp new file mode 100644 index 0000000000..0b34d00991 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp @@ -0,0 +1,727 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/trajectory_blend_request.h" +#include "pilz_industrial_motion_planner/trajectory_blend_response.h" +#include "pilz_industrial_motion_planner/trajectory_blender_transition_window.h" +#include "pilz_industrial_motion_planner/trajectory_generator_lin.h" +#include "test_utils.h" + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string PARAM_TARGET_LINK_NAME("target_link"); +const std::string CARTESIAN_VELOCITY_TOLERANCE("cartesian_velocity_tolerance"); +const std::string CARTESIAN_ANGULAR_VELOCITY_TOLERANCE("cartesian_angular_velocity_tolerance"); +const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance"); +const std::string JOINT_ACCELERATION_TOLERANCE("joint_acceleration_tolerance"); +const std::string OTHER_TOLERANCE("other_tolerance"); +const std::string SAMPLING_TIME("sampling_time"); +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); + +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner_testutils; + +class TrajectoryBlenderTransitionWindowTest : public testing::TestWithParam +{ +protected: + /** + * @brief Create test scenario for trajectory blender + * + */ + void SetUp() override; + + /** + * @brief Generate lin trajectories for blend sequences + */ + std::vector generateLinTrajs(const Sequence& seq, size_t num_cmds); + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + + std::unique_ptr lin_generator_; + std::unique_ptr blender_; + + // test parameters from parameter server + std::string planning_group_, target_link_; + double cartesian_velocity_tolerance_, cartesian_angular_velocity_tolerance_, joint_velocity_tolerance_, + joint_acceleration_tolerance_, sampling_time_; + LimitsContainer planner_limits_; + + std::string test_data_file_name_; + XmlTestDataLoaderUPtr data_loader_; +}; + +void TrajectoryBlenderTransitionWindowTest::SetUp() +{ + // get parameters + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + ASSERT_TRUE(ph_.getParam(CARTESIAN_VELOCITY_TOLERANCE, cartesian_velocity_tolerance_)); + ASSERT_TRUE(ph_.getParam(CARTESIAN_ANGULAR_VELOCITY_TOLERANCE, cartesian_angular_velocity_tolerance_)); + ASSERT_TRUE(ph_.getParam(JOINT_VELOCITY_TOLERANCE, joint_velocity_tolerance_)); + ASSERT_TRUE(ph_.getParam(JOINT_ACCELERATION_TOLERANCE, joint_acceleration_tolerance_)); + ASSERT_TRUE(ph_.getParam(SAMPLING_TIME, sampling_time_)); + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + + // load the test data provider + data_loader_.reset(new XmlTestdataLoader(test_data_file_name_, robot_model_)); + ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; + + // check robot model + testutils::checkRobotModel(robot_model_, planning_group_, target_link_); + + // create the limits container + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(ph_, + robot_model_->getActiveJointModels()); + CartesianLimit cart_limits; + cart_limits.setMaxRotationalVelocity(1 * M_PI); + cart_limits.setMaxTranslationalAcceleration(2); + cart_limits.setMaxTranslationalDeceleration(2); + cart_limits.setMaxTranslationalVelocity(1); + planner_limits_.setJointLimits(joint_limits); + planner_limits_.setCartesianLimits(cart_limits); + + // initialize trajectory generators and blender + lin_generator_.reset(new TrajectoryGeneratorLIN(robot_model_, planner_limits_)); + ASSERT_NE(nullptr, lin_generator_) << "failed to create LIN trajectory generator"; + blender_.reset(new TrajectoryBlenderTransitionWindow(planner_limits_)); + ASSERT_NE(nullptr, blender_) << "failed to create trajectory blender"; +} + +std::vector +TrajectoryBlenderTransitionWindowTest::generateLinTrajs(const Sequence& seq, size_t num_cmds) +{ + std::vector responses(num_cmds); + + for (size_t index = 0; index < num_cmds; ++index) + { + planning_interface::MotionPlanRequest req{ seq.getCmd(index).toRequest() }; + // Set start state of request to end state of previous trajectory (except + // for first) + if (index > 0) + { + moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory_->getLastWayPoint(), req.start_state); + } + // generate trajectory + planning_interface::MotionPlanResponse resp; + if (!lin_generator_->generate(req, resp, sampling_time_)) + { + std::runtime_error("Failed to generate trajectory."); + } + responses.at(index) = resp; + } + return responses; +} + +// Instantiate the test cases for robot model with and without gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryBlenderTransitionWindowTest, + ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); + +/** + * @brief Tests the blending of two trajectories with an invalid group name. + * + * Test Sequence: + * 1. Generate two linear trajectories. + * 2. Try to generate blending trajectory with invalid group name. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidGroupName) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = "invalid_group_name"; + blend_req.link_name = target_link_; + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + blend_req.blend_radius = seq.getBlendRadius(0); + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two trajectories with an invalid target link. + * + * Test Sequence: + * 1. Generate two linear trajectories. + * 2. Try to generate blending trajectory with invalid target link. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidTargetLink) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = "invalid_target_link"; + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + blend_req.blend_radius = seq.getBlendRadius(0); + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two trajectories with a negative blending + * radius. + * + * Test Sequence: + * 1. Generate two linear trajectories. + * 2. Try to generate blending trajectory with negative blending radius. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testNegativeRadius) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + blend_req.blend_radius = -0.1; + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two trajectories with zero blending radius. + * + * Test Sequence: + * 1. Generate two linear trajectories. + * 2. Try to generate blending trajectory with zero blending radius. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testZeroRadius) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + blend_req.blend_radius = 0.; + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two trajectories with differenent sampling + * times. + * + * Test Sequence: + * 1. Generate two linear trajectories with different sampling times. + * 2. Try to generate blending trajectory. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + // perform lin trajectory generation and modify sampling time + std::size_t num_cmds{ 2 }; + std::vector responses(num_cmds); + + for (size_t index = 0; index < num_cmds; ++index) + { + planning_interface::MotionPlanRequest req{ seq.getCmd(index).toRequest() }; + // Set start state of request to end state of previous trajectory (except + // for first) + if (index > 0) + { + moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory_->getLastWayPoint(), req.start_state); + sampling_time_ *= 2; + } + // generate trajectory + planning_interface::MotionPlanResponse resp; + if (!lin_generator_->generate(req, resp, sampling_time_)) + { + std::runtime_error("Failed to generate trajectory."); + } + responses.at(index) = resp; + } + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.first_trajectory = responses[0].trajectory_; + blend_req.second_trajectory = responses[1].trajectory_; + blend_req.blend_radius = seq.getBlendRadius(0); + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two trajectories with one trajectory + * having non-uniform sampling time (apart from the last sample, + * which is ignored). + * + * Test Sequence: + * 1. Generate two linear trajectories and corrupt uniformity of sampling + * time. + * 2. Try to generate blending trajectory. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + // Modify first time interval + EXPECT_GT(res[0].trajectory_->getWayPointCount(), 2u); + res[0].trajectory_->setWayPointDurationFromPrevious(1, 2 * sampling_time_); + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.blend_radius = seq.getBlendRadius(0); + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two trajectories which do not intersect. + * + * Test Sequence: + * 1. Generate two trajectories from valid test data set. + * 2. Replace the second trajectory by the first one. + * 2. Try to generate blending trajectory. + * + * Expected Results: + * 1. Two trajectories generated. + * 2. Two trajectories that do not intersect. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testNotIntersectingTrajectories) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 1) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.first_trajectory = res.at(0).trajectory_; + // replace the second trajectory to make the two trajectories timely not + // intersect + blend_req.second_trajectory = res.at(0).trajectory_; + blend_req.blend_radius = seq.getBlendRadius(0); + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two cartesian trajectories with the + * shared point (last point of first, first point of second trajectory) + * having a non-zero velocity + * + * Test Sequence: + * 1. Generate two trajectories from the test data set. + * 2. Generate blending trajectory modify the shared point to have velocity. + * Expected Results: + * 1. Two trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testNonStationaryPoint) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.blend_radius = seq.getBlendRadius(0); + + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + // Modify last waypoint of first trajectory and first point of second + // trajectory + blend_req.first_trajectory->getLastWayPointPtr()->setVariableVelocity(0, 1.0); + blend_req.second_trajectory->getFirstWayPointPtr()->setVariableVelocity(0, 1.0); + + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two cartesian trajectories where the first + * trajectory is completely within the sphere defined by the blend radius + * + * Test Sequence: + * 1. Generate two trajectories from the test data set. + * 2. Generate blending trajectory with a blend_radius larger + * than the smaller trajectory. + * + * Expected Results: + * 1. Two trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + double lin1_distance; + lin1_distance = (res[0].trajectory_->getFirstWayPoint().getFrameTransform(target_link_).translation() - + res[0].trajectory_->getLastWayPoint().getFrameTransform(target_link_).translation()) + .norm(); + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.blend_radius = 1.1 * lin1_distance; + + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two cartesian trajectories where the second + * trajectory is completely within the sphere defined by the blend radius + * + * Test Sequence: + * 1. Generate two trajectories from the test data set. + * 2. Generate blending trajectory with a blend_radius larger + * than the smaller trajectory. + * + * Expected Results: + * 1. Two trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj2InsideBlendRadius) +{ + Sequence seq{ data_loader_->getSequence("NoIntersectionTraj2") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.blend_radius = seq.getBlendRadius(0); + + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two cartesian linear trajectories using robot + * model + * + * Test Sequence: + * 1. Generate two linear trajectories from the test data set. + * 2. Generate blending trajectory. + * 3. Check blending trajectory: + * - for position, velocity, and acceleration bounds, + * - for continuity in joint space, + * - for continuity in cartesian space. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory generated. + * 3. No bound is violated, the trajectories are continuous + * in joint and cartesian space. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testLinLinBlending) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.blend_radius = seq.getBlendRadius(0); + + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + EXPECT_TRUE(blender_->blend(blend_req, blend_res)); + + EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_, + joint_acceleration_tolerance_, cartesian_velocity_tolerance_, + cartesian_angular_velocity_tolerance_)); +} + +/** + * @brief Tests the blending of two cartesian linear trajectories which have + * an overlap in the blending sphere using robot model. To be precise, + * the trajectories exactly lie on top of each other. + * + * Test Sequence: + * 1. Generate two linear trajectories from the test data set. Set goal of + * second traj to start of first traj. + * 2. Generate blending trajectory. + * 3. Check blending trajectory: + * - for position, velocity, and acceleration bounds, + * - for continuity in joint space, + * - for continuity in cartesian space. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory generated. + * 3. No bound is violated, the trajectories are continuous + * in joint and cartesian space. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testOverlappingBlendTrajectories) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + // Set goal of second traj to start of first traj. + seq.getCmd(1).setGoalConfiguration(seq.getCmd(0).getStartConfiguration()); + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.blend_radius = seq.getBlendRadius(0); + EXPECT_TRUE(blender_->blend(blend_req, blend_res)); + + EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_, + joint_acceleration_tolerance_, cartesian_velocity_tolerance_, + cartesian_angular_velocity_tolerance_)); +} + +/** + * @brief Tests the blending of two cartesian trajectories which differ + * from a straight line. + * + * Test Sequence: + * 1. Generate two trajectories from the test data set. + * 2. Add scaled sine function to cartesian trajectories, such that + * start and end state remain unchanged; generate resulting + * joint trajectories using a time scaling in order to preserve + * joint velocity limits. + * 3. Generate blending trajectory. + * 4. Check blending trajectory: + * - for position, velocity, and acceleration bounds, + * - for continuity in joint space, + * - for continuity in cartesian space. + * + * Expected Results: + * 1. Two trajectories generated. + * 2. Modified joint trajectories generated. + * 3. Blending trajectory generated. + * 4. No bound is violated, the trajectories are continuous + * in joint and cartesian space. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) +{ + const double sine_scaling_factor{ 0.01 }; + const double time_scaling_factor{ 10 }; + + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + // prepare looping over trajectories + std::vector sine_trajs(2); + + for (size_t traj_index = 0; traj_index < 2; ++traj_index) + { + auto lin_traj{ res.at(traj_index).trajectory_ }; + + CartesianTrajectory cart_traj; + trajectory_msgs::JointTrajectory joint_traj; + const double duration{ lin_traj->getWayPointDurationFromStart(lin_traj->getWayPointCount()) }; + // time from start zero does not work + const double time_from_start_offset{ time_scaling_factor * lin_traj->getWayPointDurations().back() }; + + // generate modified cartesian trajectory + for (size_t i = 0; i < lin_traj->getWayPointCount(); ++i) + { + // transform time to interval [0, 4*pi] + const double sine_arg{ 4 * M_PI * lin_traj->getWayPointDurationFromStart(i) / duration }; + + // get pose + CartesianTrajectoryPoint waypoint; + geometry_msgs::Pose waypoint_pose; + Eigen::Isometry3d eigen_pose{ lin_traj->getWayPointPtr(i)->getFrameTransform(target_link_) }; + tf2::convert(eigen_pose, waypoint_pose); + + // add scaled sine function + waypoint_pose.position.x += sine_scaling_factor * sin(sine_arg); + waypoint_pose.position.y += sine_scaling_factor * sin(sine_arg); + waypoint_pose.position.z += sine_scaling_factor * sin(sine_arg); + + // add to trajectory + waypoint.pose = waypoint_pose; + waypoint.time_from_start = + ros::Duration(time_from_start_offset + time_scaling_factor * lin_traj->getWayPointDurationFromStart(i)); + cart_traj.points.push_back(waypoint); + } + + // prepare ik + std::map initial_joint_position, initial_joint_velocity; + for (const std::string& joint_name : + lin_traj->getFirstWayPointPtr()->getJointModelGroup(planning_group_)->getActiveJointModelNames()) + { + if (traj_index == 0) + { + initial_joint_position[joint_name] = lin_traj->getFirstWayPoint().getVariablePosition(joint_name); + initial_joint_velocity[joint_name] = lin_traj->getFirstWayPoint().getVariableVelocity(joint_name); + } + else + { + initial_joint_position[joint_name] = + sine_trajs[traj_index - 1]->getLastWayPoint().getVariablePosition(joint_name); + initial_joint_velocity[joint_name] = + sine_trajs[traj_index - 1]->getLastWayPoint().getVariableVelocity(joint_name); + } + } + + moveit_msgs::MoveItErrorCodes error_code; + if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_traj, planning_group_, + target_link_, initial_joint_position, initial_joint_velocity, joint_traj, error_code, + true)) + { + std::runtime_error("Failed to generate trajectory."); + } + + joint_traj.points.back().velocities.assign(joint_traj.points.back().velocities.size(), 0.0); + joint_traj.points.back().accelerations.assign(joint_traj.points.back().accelerations.size(), 0.0); + + // convert trajectory_msgs::JointTrajectory to + // robot_trajectory::RobotTrajectory + sine_trajs[traj_index] = std::make_shared(robot_model_, planning_group_); + sine_trajs.at(traj_index)->setRobotTrajectoryMsg(lin_traj->getFirstWayPoint(), joint_traj); + } + + TrajectoryBlendRequest blend_req; + TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.blend_radius = seq.getBlendRadius(0); + + blend_req.first_trajectory = sine_trajs.at(0); + blend_req.second_trajectory = sine_trajs.at(1); + + EXPECT_TRUE(blender_->blend(blend_req, blend_res)); + + EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_, + joint_acceleration_tolerance_, cartesian_velocity_tolerance_, + cartesian_angular_velocity_tolerance_)); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_trajectory_blender_transition_window"); + ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.test new file mode 100644 index 0000000000..2d0bf6c3d5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.test @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp new file mode 100644 index 0000000000..a70575361f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp @@ -0,0 +1,971 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/cartesian_trajectory.h" +#include "pilz_industrial_motion_planner/cartesian_trajectory_point.h" +#include "pilz_industrial_motion_planner/limits_container.h" +#include "pilz_industrial_motion_planner/trajectory_functions.h" +#include "test_utils.h" + +#define _USE_MATH_DEFINES + +static constexpr double EPSILON{ 1.0e-6 }; +static constexpr double IK_SEED_OFFSET{ 0.1 }; +static constexpr double L0{ 0.2604 }; // Height of foot +static constexpr double L1{ 0.3500 }; // Height of first connector +static constexpr double L2{ 0.3070 }; // Height of second connector +static constexpr double L3{ 0.0840 }; // Distance last joint to flange + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string GROUP_TIP_LINK_NAME("group_tip_link"); +const std::string ROBOT_TCP_LINK_NAME("tcp_link"); +const std::string IK_FAST_LINK_NAME("ik_fast_link"); +const std::string RANDOM_TEST_NUMBER("random_test_number"); + +/** + * @brief test fixtures base class + */ +class TrajectoryFunctionsTestBase : public testing::TestWithParam +{ +protected: + /** + * @brief Create test scenario for trajectory functions + * + */ + void SetUp() override; + + /** + * @brief check if two transformations are close + * @param pose1 + * @param pose2 + * @param epsilon + * @return + */ + bool tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, const double& epsilon); + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + + // test parameters from parameter server + std::string planning_group_, group_tip_link_, tcp_link_, ik_fast_link_; + int random_test_number_; + std::vector joint_names_; + std::map zero_state_; + + // random seed + boost::uint32_t random_seed_{ 100 }; + random_numbers::RandomNumberGenerator rng_{ random_seed_ }; +}; + +void TrajectoryFunctionsTestBase::SetUp() +{ + // parameters + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(GROUP_TIP_LINK_NAME, group_tip_link_)); + ASSERT_TRUE(ph_.getParam(ROBOT_TCP_LINK_NAME, tcp_link_)); + ASSERT_TRUE(ph_.getParam(IK_FAST_LINK_NAME, ik_fast_link_)); + ASSERT_TRUE(ph_.getParam(RANDOM_TEST_NUMBER, random_test_number_)); + + // check robot model + testutils::checkRobotModel(robot_model_, planning_group_, tcp_link_); + + // initialize the zero state configurationg and test joint state + joint_names_ = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames(); + for (const auto& joint_name : joint_names_) + { + zero_state_[joint_name] = 0.0; + } +} + +bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, + const double& epsilon) +{ + for (std::size_t i = 0; i < 3; ++i) + for (std::size_t j = 0; j < 4; ++j) + { + if (fabs(pose1(i, j) - pose2(i, j)) > fabs(epsilon)) + return false; + } + return true; +} + +/** + * @brief Parametrized class for tests with and without gripper. + */ +class TrajectoryFunctionsTestFlangeAndGripper : public TrajectoryFunctionsTestBase +{ +}; + +/** + * @brief Parametrized class for tests, that only run with a gripper + */ +class TrajectoryFunctionsTestOnlyGripper : public TrajectoryFunctionsTestBase +{ +}; + +// Instantiate the test cases for robot model with and without gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryFunctionsTestFlangeAndGripper, + ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); + +// Instantiate the test cases for robot model with a gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryFunctionsTestOnlyGripper, + ::testing::Values(PARAM_MODEL_WITH_GRIPPER_NAME)); + +/** + * @brief Test the forward kinematics function with simple robot poses for robot + * tip link + * using robot model without gripper. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK) +{ + Eigen::Isometry3d tip_pose; + std::map test_state = zero_state_; + EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, group_tip_link_, test_state, tip_pose)); + EXPECT_NEAR(tip_pose(0, 3), 0, EPSILON); + EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON); + EXPECT_NEAR(tip_pose(2, 3), L0 + L1 + L2 + L3, EPSILON); + + test_state[joint_names_.at(1)] = M_PI_2; + EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, group_tip_link_, test_state, tip_pose)); + EXPECT_NEAR(tip_pose(0, 3), L1 + L2 + L3, EPSILON); + EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON); + EXPECT_NEAR(tip_pose(2, 3), L0, EPSILON); + + test_state[joint_names_.at(1)] = -M_PI_2; + test_state[joint_names_.at(2)] = M_PI_2; + EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, group_tip_link_, test_state, tip_pose)); + EXPECT_NEAR(tip_pose(0, 3), -L1, EPSILON); + EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON); + EXPECT_NEAR(tip_pose(2, 3), L0 - L2 - L3, EPSILON); + + // wrong link name + std::string link_name = "wrong_link_name"; + EXPECT_FALSE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, link_name, test_state, tip_pose)); +} + +/** + * @brief Test the inverse kinematics directly through ikfast solver + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver) +{ + // Load solver + const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); + + // robot state + robot_state::RobotState rstate(robot_model_); + + while (random_test_number_ > 0) + { + // sample random robot state + rstate.setToRandomPositions(jmg, rng_); + rstate.update(); + geometry_msgs::Pose pose_expect; + tf2::convert(rstate.getFrameTransform(ik_fast_link_), pose_expect); + + // prepare inverse kinematics + std::vector ik_poses; + ik_poses.push_back(pose_expect); + std::vector ik_seed, ik_expect, ik_actual; + for (const auto& joint_name : jmg->getActiveJointModelNames()) + { + ik_expect.push_back(rstate.getVariablePosition(joint_name)); + if (rstate.getVariablePosition(joint_name) > 0) + ik_seed.push_back(rstate.getVariablePosition(joint_name) - IK_SEED_OFFSET); + else + ik_seed.push_back(rstate.getVariablePosition(joint_name) + IK_SEED_OFFSET); + } + + std::vector> ik_solutions; + kinematics::KinematicsResult ik_result; + moveit_msgs::MoveItErrorCodes err_code; + kinematics::KinematicsQueryOptions options = kinematics::KinematicsQueryOptions(); + + // compute all ik solutions + EXPECT_TRUE(solver->getPositionIK(ik_poses, ik_seed, ik_solutions, ik_result, options)); + + // compute one ik solution + EXPECT_TRUE(solver->getPositionIK(pose_expect, ik_seed, ik_actual, err_code)); + + ASSERT_EQ(ik_expect.size(), ik_actual.size()); + + for (std::size_t i = 0; i < ik_expect.size(); ++i) + { + EXPECT_NEAR(ik_actual.at(i), ik_expect.at(i), 4 * IK_SEED_OFFSET); + } + + --random_test_number_; + } +} + +/** + * @brief Test the inverse kinematics using RobotState class (setFromIK) using + * robot model + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState) +{ + // robot state + robot_state::RobotState rstate(robot_model_); + const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + while (random_test_number_ > 0) + { + // sample random robot state + rstate.setToRandomPositions(jmg, rng_); + + Eigen::Isometry3d pose_expect = rstate.getFrameTransform(tcp_link_); + + // copy the random state and set ik seed + std::map ik_seed, ik_expect; + for (const auto& joint_name : joint_names_) + { + ik_expect[joint_name] = rstate.getVariablePosition(joint_name); + if (rstate.getVariablePosition(joint_name) > 0) + ik_seed[joint_name] = rstate.getVariablePosition(joint_name) - IK_SEED_OFFSET; + else + ik_seed[joint_name] = rstate.getVariablePosition(joint_name) + IK_SEED_OFFSET; + } + + rstate.setVariablePositions(ik_seed); + rstate.update(); + + // compute the ik + std::map ik_actual; + + EXPECT_TRUE(rstate.setFromIK(robot_model_->getJointModelGroup(planning_group_), pose_expect, tcp_link_)); + + for (const auto& joint_name : joint_names_) + { + ik_actual[joint_name] = rstate.getVariablePosition(joint_name); + } + + // compare ik solution and expected value + for (const auto& joint_pair : ik_actual) + { + EXPECT_NEAR(joint_pair.second, ik_expect.at(joint_pair.first), 4 * IK_SEED_OFFSET); + } + + // compute the pose from ik_solution + rstate.setVariablePositions(ik_actual); + rstate.update(); + Eigen::Isometry3d pose_actual = rstate.getFrameTransform(tcp_link_); + + EXPECT_TRUE(tfNear(pose_expect, pose_actual, EPSILON)); + + --random_test_number_; + } +} + +/** + * @brief Test the wrapper function to compute inverse kinematics using robot + * model + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK) +{ + // robot state + robot_state::RobotState rstate(robot_model_); + + const std::string frame_id = robot_model_->getModelFrame(); + const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + while (random_test_number_ > 0) + { + // sample random robot state + rstate.setToRandomPositions(jmg, rng_); + + Eigen::Isometry3d pose_expect = rstate.getFrameTransform(tcp_link_); + + // copy the random state and set ik seed + std::map ik_seed, ik_expect; + for (const auto& joint_name : robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames()) + { + ik_expect[joint_name] = rstate.getVariablePosition(joint_name); + if (rstate.getVariablePosition(joint_name) > 0) + { + ik_seed[joint_name] = rstate.getVariablePosition(joint_name) - IK_SEED_OFFSET; + } + else + { + ik_seed[joint_name] = rstate.getVariablePosition(joint_name) + IK_SEED_OFFSET; + } + } + + // compute the ik + std::map ik_actual; + EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + frame_id, ik_seed, ik_actual, false)); + + // compare ik solution and expected value + for (const auto& joint_pair : ik_actual) + { + EXPECT_NEAR(joint_pair.second, ik_expect.at(joint_pair.first), 4 * IK_SEED_OFFSET); + } + + --random_test_number_; + } +} + +/** + * @brief Test computePoseIK for invalid group_name + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupName) +{ + const std::string frame_id = robot_model_->getModelFrame(); + Eigen::Isometry3d pose_expect; + + std::map ik_seed; + + // compute the ik + std::map ik_actual; + EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, "InvalidGroupName", tcp_link_, pose_expect, + frame_id, ik_seed, ik_actual, false)); +} + +/** + * @brief Test computePoseIK for invalid link_name + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName) +{ + const std::string frame_id = robot_model_->getModelFrame(); + Eigen::Isometry3d pose_expect; + + std::map ik_seed; + + // compute the ik + std::map ik_actual; + EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, "WrongLink", pose_expect, + frame_id, ik_seed, ik_actual, false)); +} + +/** + * @brief Test computePoseIK for invalid frame_id + * + * Currently only robot_model_->getModelFrame() == frame_id + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId) +{ + Eigen::Isometry3d pose_expect; + + std::map ik_seed; + + // compute the ik + std::map ik_actual; + EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + "InvalidFrameId", ik_seed, ik_actual, false)); +} + +/** + * @brief Test if activated self collision for a pose that would be in self + * collision without the check results in a + * valid ik solution. + */ +TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForValidPosition) +{ + const std::string frame_id = robot_model_->getModelFrame(); + const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + // create seed + std::vector ik_seed_states = { -0.553, 0.956, 1.758, 0.146, -1.059, 1.247 }; + auto joint_names = jmg->getActiveJointModelNames(); + + std::map ik_seed; + for (unsigned int i = 0; i < ik_seed_states.size(); ++i) + { + ik_seed[joint_names[i]] = ik_seed_states[i]; + } + + // create expected pose + geometry_msgs::Pose pose; + pose.position.x = -0.454; + pose.position.y = -0.15; + pose.position.z = 0.431; + pose.orientation.y = 0.991562; + pose.orientation.w = -0.1296328; + Eigen::Isometry3d pose_expect; + normalizeQuaternion(pose.orientation); + tf2::convert(pose, pose_expect); + + // compute the ik without self collision check and expect the resulting pose + // to be in self collission. + std::map ik_actual1; + EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + frame_id, ik_seed, ik_actual1, false)); + + robot_state::RobotState rstate(robot_model_); + planning_scene::PlanningScene rscene(robot_model_); + + std::vector ik_state; + std::transform(ik_actual1.begin(), ik_actual1.end(), std::back_inserter(ik_state), + boost::bind(&std::map::value_type::second, _1)); + + rstate.setJointGroupPositions(jmg, ik_state); + rstate.update(); + + collision_detection::CollisionRequest collision_req; + collision_req.group_name = jmg->getName(); + collision_detection::CollisionResult collision_res; + + rscene.checkSelfCollision(collision_req, collision_res, rstate); + + EXPECT_TRUE(collision_res.collision); + + // compute the ik with collision detection activated and expect the resulting + // pose to be without self collision. + std::map ik_actual2; + EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + frame_id, ik_seed, ik_actual2, true)); + + std::vector ik_state2; + std::transform(ik_actual2.begin(), ik_actual2.end(), std::back_inserter(ik_state2), + boost::bind(&std::map::value_type::second, _1)); + rstate.setJointGroupPositions(jmg, ik_state2); + rstate.update(); + + collision_detection::CollisionResult collision_res2; + rscene.checkSelfCollision(collision_req, collision_res2, rstate); + + EXPECT_FALSE(collision_res2.collision); +} + +/** + * @brief Test if self collision is considered by using a pose that always has + * self collision. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionForInvalidPose) +{ + // robot state + robot_state::RobotState rstate(robot_model_); + + const std::string frame_id = robot_model_->getModelFrame(); + const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + // create seed + std::map ik_seed; + for (const auto& joint_name : jmg->getActiveJointModelNames()) + { + ik_seed[joint_name] = 0; + } + + // create goal + std::vector ik_goal = { 0, 2.3, -2.3, 0, 0, 0 }; + + rstate.setJointGroupPositions(jmg, ik_goal); + + Eigen::Isometry3d pose_expect = rstate.getFrameTransform(tcp_link_); + + // compute the ik with disabled collision check + std::map ik_actual; + EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + frame_id, ik_seed, ik_actual, false)); + + // compute the ik with enabled collision check + EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + frame_id, ik_seed, ik_actual, true)); +} + +/** + * @brief Check that function VerifySampleJointLimits() returns 'false' in case + * of very small sample duration. + * + * Test Sequence: + * 1. Call function with very small sample duration. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithSmallDuration) +{ + const std::map position_last, velocity_last, position_current; + double duration_last{ 0.0 }; + const pilz_industrial_motion_planner::JointLimitsContainer joint_limits; + + double duration_current = 10e-7; + + EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current, + duration_last, duration_current, joint_limits)); +} + +/** + * @brief Check that function VerifySampleJointLimits() returns 'false' in case + * of a velocity violation. + * + * Test Sequence: + * 1. Call function with a velocity violation. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVelocityViolation) +{ + const std::string test_joint_name{ "joint" }; + + std::map position_last{ { test_joint_name, 2.0 } }; + std::map position_current{ { test_joint_name, 10.0 } }; + std::map velocity_last; + double duration_current{ 1.0 }; + double duration_last{ 0.0 }; + pilz_industrial_motion_planner::JointLimitsContainer joint_limits; + + JointLimit test_joint_limits; + // Calculate the max allowed velocity in such a way that it is always smaller + // than the current velocity. + test_joint_limits.max_velocity = + ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current) - 1.0; + test_joint_limits.has_velocity_limits = true; + joint_limits.addLimit(test_joint_name, test_joint_limits); + + EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current, + duration_last, duration_current, joint_limits)); +} + +/** + * @brief Check that function VerifySampleJointLimits() returns 'false' in case + * of a acceleration violation. + * + * Test Sequence: + * 1. Call function with a acceleration violation. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccelerationViolation) +{ + const std::string test_joint_name{ "joint" }; + + double duration_current = 1.0; + double duration_last = 1.0; + + std::map position_last{ { test_joint_name, 2.0 } }; + std::map position_current{ { test_joint_name, 20.0 } }; + double velocity_current = + ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current); + std::map velocity_last{ { test_joint_name, 9.0 } }; + pilz_industrial_motion_planner::JointLimitsContainer joint_limits; + + JointLimit test_joint_limits; + // Calculate the max allowed velocity in such a way that it is always bigger + // than the current velocity. + test_joint_limits.max_velocity = velocity_current + 1.0; + test_joint_limits.has_velocity_limits = true; + + double acceleration_current = + (velocity_current - velocity_last.at(test_joint_name)) / (duration_last + duration_current) * 2; + // Calculate the max allowed acceleration in such a way that it is always + // smaller than the current acceleration. + test_joint_limits.max_acceleration = acceleration_current - 1.0; + test_joint_limits.has_acceleration_limits = true; + + joint_limits.addLimit(test_joint_name, test_joint_limits); + + EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current, + duration_last, duration_current, joint_limits)); +} + +/** + * @brief Check that function VerifySampleJointLimits() returns 'false' in case + * of a deceleration violation. + * + * Test Sequence: + * 1. Call function with a deceleration violation. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecelerationViolation) +{ + const std::string test_joint_name{ "joint" }; + + double duration_current = 1.0; + double duration_last = 1.0; + + std::map position_last{ { test_joint_name, 20.0 } }; + std::map position_current{ { test_joint_name, 2.0 } }; + double velocity_current = + ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current); + std::map velocity_last{ { test_joint_name, 19.0 } }; + pilz_industrial_motion_planner::JointLimitsContainer joint_limits; + + JointLimit test_joint_limits; + // Calculate the max allowed velocity in such a way that it is always bigger + // than the current velocity. + test_joint_limits.max_velocity = fabs(velocity_current) + 1.0; + test_joint_limits.has_velocity_limits = true; + + double acceleration_current = + (velocity_current - velocity_last.at(test_joint_name)) / (duration_last + duration_current) * 2; + // Calculate the max allowed deceleration in such a way that it is always + // bigger than the current acceleration. + test_joint_limits.max_deceleration = acceleration_current + 1.0; + test_joint_limits.has_deceleration_limits = true; + + joint_limits.addLimit(test_joint_name, test_joint_limits); + + EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current, + duration_last, duration_current, joint_limits)); +} + +/** + * @brief Check that function generateJointTrajectory() returns 'false' if + * a joint trajectory cannot be computed from a cartesian trajectory. + * + * Please note: Both function variants are tested in this test. + * + * Test Sequence: + * 1. Call function with a cartesian trajectory which cannot be transformed + * into a joint trajectory by using + * an invalid group_name. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithInvalidCartesianTrajectory) +{ + // Create random test trajectory + // Note: 'path' is deleted by KDL::Trajectory_Segment + KDL::Path_RoundedComposite* path = + new KDL::Path_RoundedComposite(0.2, 0.01, new KDL::RotationalInterpolation_SingleAxis()); + path->Add(KDL::Frame(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(-1, 0, 0))); + path->Finish(); + // Note: 'velprof' is deleted by KDL::Trajectory_Segment + KDL::VelocityProfile* vel_prof = new KDL::VelocityProfile_Trap(0.5, 0.1); + vel_prof->SetProfile(0, path->PathLength()); + KDL::Trajectory_Segment kdl_trajectory(path, vel_prof); + + pilz_industrial_motion_planner::JointLimitsContainer joint_limits; + std::string group_name{ "invalid_group_name" }; + std::map initial_joint_position; + double sampling_time{ 0.1 }; + trajectory_msgs::JointTrajectory joint_trajectory; + moveit_msgs::MoveItErrorCodes error_code; + bool check_self_collision{ false }; + + EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory( + robot_model_, joint_limits, kdl_trajectory, group_name, tcp_link_, initial_joint_position, sampling_time, + joint_trajectory, error_code, check_self_collision)); + + std::map initial_joint_velocity; + + pilz_industrial_motion_planner::CartesianTrajectory cart_traj; + cart_traj.group_name = group_name; + cart_traj.link_name = tcp_link_; + pilz_industrial_motion_planner::CartesianTrajectoryPoint cart_traj_point; + cart_traj.points.push_back(cart_traj_point); + + EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory( + robot_model_, joint_limits, cart_traj, group_name, tcp_link_, initial_joint_position, initial_joint_velocity, + joint_trajectory, error_code, check_self_collision)); +} + +/** + * @brief Check that function determineAndCheckSamplingTime() returns 'false' if + * both of the needed vectors have an incorrect vector size. + * + * + * Test Sequence: + * 1. Call function with vectors of incorrect size. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeInvalidVectorSize) +{ + robot_trajectory::RobotTrajectoryPtr first_trajectory = + std::make_shared(robot_model_, planning_group_); + robot_trajectory::RobotTrajectoryPtr second_trajectory = + std::make_shared(robot_model_, planning_group_); + double epsilon{ 0.0 }; + double sampling_time{ 0.0 }; + + robot_state::RobotState rstate(robot_model_); + first_trajectory->insertWayPoint(0, rstate, 0.1); + second_trajectory->insertWayPoint(0, rstate, 0.1); + + EXPECT_FALSE(pilz_industrial_motion_planner::determineAndCheckSamplingTime(first_trajectory, second_trajectory, + epsilon, sampling_time)); +} + +/** + * @brief Check that function determineAndCheckSamplingTime() returns 'true' if + * sampling time is correct. + * + * + * Test Sequence: + * 1. Call function with trajectories which do NOT violate sampling time. + * + * Expected Results: + * 1. Function returns 'true'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeCorrectSamplingTime) +{ + robot_trajectory::RobotTrajectoryPtr first_trajectory = + std::make_shared(robot_model_, planning_group_); + robot_trajectory::RobotTrajectoryPtr second_trajectory = + std::make_shared(robot_model_, planning_group_); + double epsilon{ 0.0001 }; + double sampling_time{ 0.0 }; + double expected_sampling_time{ 0.1 }; + + robot_state::RobotState rstate(robot_model_); + first_trajectory->insertWayPoint(0, rstate, expected_sampling_time); + first_trajectory->insertWayPoint(1, rstate, expected_sampling_time); + + second_trajectory->insertWayPoint(0, rstate, expected_sampling_time); + second_trajectory->insertWayPoint(1, rstate, expected_sampling_time); + second_trajectory->insertWayPoint(2, rstate, expected_sampling_time); + + EXPECT_TRUE(pilz_industrial_motion_planner::determineAndCheckSamplingTime(first_trajectory, second_trajectory, + epsilon, sampling_time)); + EXPECT_EQ(expected_sampling_time, sampling_time); +} + +/** + * @brief Check that function determineAndCheckSamplingTime() returns 'false' if + * sampling time is violated. + * + * + * Test Sequence: + * 1. Call function with trajectories which violate sampling time. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeViolateSamplingTime) +{ + robot_trajectory::RobotTrajectoryPtr first_trajectory = + std::make_shared(robot_model_, planning_group_); + robot_trajectory::RobotTrajectoryPtr second_trajectory = + std::make_shared(robot_model_, planning_group_); + double epsilon{ 0.0001 }; + double sampling_time{ 0.0 }; + double expected_sampling_time{ 0.1 }; + + robot_state::RobotState rstate(robot_model_); + first_trajectory->insertWayPoint(0, rstate, expected_sampling_time); + first_trajectory->insertWayPoint(1, rstate, expected_sampling_time); + first_trajectory->insertWayPoint(2, rstate, expected_sampling_time); + // Violate sampling time + first_trajectory->insertWayPoint(2, rstate, expected_sampling_time + 1.0); + first_trajectory->insertWayPoint(3, rstate, expected_sampling_time); + + second_trajectory->insertWayPoint(0, rstate, expected_sampling_time); + second_trajectory->insertWayPoint(1, rstate, expected_sampling_time); + second_trajectory->insertWayPoint(2, rstate, expected_sampling_time); + second_trajectory->insertWayPoint(3, rstate, expected_sampling_time); + + EXPECT_FALSE(pilz_industrial_motion_planner::determineAndCheckSamplingTime(first_trajectory, second_trajectory, + epsilon, sampling_time)); + EXPECT_EQ(expected_sampling_time, sampling_time); +} + +/** + * @brief Check that function isRobotStateEqual() returns 'false' if + * the positions of the robot states are not equal. + * + * + * Test Sequence: + * 1. Call function with robot states with different positions. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUnequal) +{ + robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); + robot_state::RobotState rstate_2 = robot_state::RobotState(robot_model_); + + double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupPositions(planning_group_, default_joint_position); + // Ensure that the joint positions of both robot states are different + default_joint_position[0] = default_joint_position[0] + 70.0; + rstate_2.setJointGroupPositions(planning_group_, default_joint_position); + + double epsilon{ 0.0001 }; + EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateEqual(rstate_1, rstate_2, planning_group_, epsilon)); +} + +/** + * @brief Check that function isRobotStateEqual() returns 'false' if + * the velocity of the robot states are not equal. + * + * + * Test Sequence: + * 1. Call function with robot states with different velocities. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUnequal) +{ + robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); + robot_state::RobotState rstate_2 = robot_state::RobotState(robot_model_); + + // Ensure that the joint positions of both robot state are equal + double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupPositions(planning_group_, default_joint_position); + rstate_2.setJointGroupPositions(planning_group_, default_joint_position); + + double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity); + // Ensure that the joint velocites of both robot states are different + default_joint_velocity[1] = default_joint_velocity[1] + 10.0; + rstate_2.setJointGroupVelocities(planning_group_, default_joint_velocity); + + double epsilon{ 0.0001 }; + EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateEqual(rstate_1, rstate_2, planning_group_, epsilon)); +} + +/** + * @brief Check that function isRobotStateEqual() returns 'false' if + * the acceleration of the robot states are not equal. + * + * + * Test Sequence: + * 1. Call function with robot states with different acceleration. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAccelerationUnequal) +{ + robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); + robot_state::RobotState rstate_2 = robot_state::RobotState(robot_model_); + + // Ensure that the joint positions of both robot state are equal + double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupPositions(planning_group_, default_joint_position); + rstate_2.setJointGroupPositions(planning_group_, default_joint_position); + + // Ensure that the joint velocities of both robot state are equal + double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity); + rstate_2.setJointGroupVelocities(planning_group_, default_joint_velocity); + + double default_joint_acceleration[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupAccelerations(planning_group_, default_joint_acceleration); + // Ensure that the joint accelerations of both robot states are different + default_joint_acceleration[1] = default_joint_acceleration[1] + 10.0; + rstate_2.setJointGroupAccelerations(planning_group_, default_joint_acceleration); + + double epsilon{ 0.0001 }; + EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateEqual(rstate_1, rstate_2, planning_group_, epsilon)); +} + +/** + * @brief Check that function isRobotStateStationary() returns 'false' if + * the joint velocities are not equal to zero. + * + * + * Test Sequence: + * 1. Call function with robot state with joint velocities != 0. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVelocityUnequal) +{ + robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); + + // Ensure that the joint velocities are NOT zero + double default_joint_velocity[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity); + + double epsilon{ 0.0001 }; + EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateStationary(rstate_1, planning_group_, epsilon)); +} + +/** + * @brief Check that function isRobotStateStationary() returns 'false' if + * the joint acceleration are not equal to zero. + * + * + * Test Sequence: + * 1. Call function with robot state with joint acceleration != 0. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal) +{ + robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); + + // Ensure that the joint velocities are zero + double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity); + + // Ensure that the joint acceleration are NOT zero + double default_joint_acceleration[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupAccelerations(planning_group_, default_joint_acceleration); + + double epsilon{ 0.0001 }; + EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateStationary(rstate_1, planning_group_, epsilon)); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_trajectory_functions"); + ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.test new file mode 100644 index 0000000000..ff1c41bb0f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.test @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp new file mode 100644 index 0000000000..c1a24e7285 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp @@ -0,0 +1,146 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/trajectory_generator.h" + +using namespace pilz_industrial_motion_planner; + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + */ +TEST(TrajectoryGeneratorTest, TestExceptionErrorCodeMapping) +{ + { + std::shared_ptr tgil_ex{ new TrajectoryGeneratorInvalidLimitsException( + "") }; + EXPECT_EQ(tgil_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + } + + { + std::shared_ptr vsi_ex{ new VelocityScalingIncorrect("") }; + EXPECT_EQ(vsi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr asi_ex{ new AccelerationScalingIncorrect("") }; + EXPECT_EQ(asi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr upg_ex{ new UnknownPlanningGroup("") }; + EXPECT_EQ(upg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); + } + + { + std::shared_ptr njniss_ex{ new NoJointNamesInStartState("") }; + EXPECT_EQ(njniss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + } + + { + std::shared_ptr smiss_ex{ new SizeMismatchInStartState("") }; + EXPECT_EQ(smiss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + } + + { + std::shared_ptr jofssoor_ex{ new JointsOfStartStateOutOfRange("") }; + EXPECT_EQ(jofssoor_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + } + + { + std::shared_ptr nzviss_ex{ new NonZeroVelocityInStartState("") }; + EXPECT_EQ(nzviss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + } + + { + std::shared_ptr neogcg_ex{ new NotExactlyOneGoalConstraintGiven("") }; + EXPECT_EQ(neogcg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr oogta_ex{ new OnlyOneGoalTypeAllowed("") }; + EXPECT_EQ(oogta_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr ssgsm_ex{ new StartStateGoalStateMismatch("") }; + EXPECT_EQ(ssgsm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr jcdnbtg_ex{ new JointConstraintDoesNotBelongToGroup("") }; + EXPECT_EQ(jcdnbtg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr jogoor_ex{ new JointsOfGoalOutOfRange("") }; + EXPECT_EQ(jogoor_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr pcnm_ex{ new PositionConstraintNameMissing("") }; + EXPECT_EQ(pcnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr ocnm_ex{ new OrientationConstraintNameMissing("") }; + EXPECT_EQ(ocnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr pocnm_ex{ new PositionOrientationConstraintNameMismatch( + "") }; + EXPECT_EQ(pocnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr nisa_ex{ new NoIKSolverAvailable("") }; + EXPECT_EQ(nisa_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + } + + { + std::shared_ptr nppg_ex{ new NoPrimitivePoseGiven("") }; + EXPECT_EQ(nppg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp new file mode 100644 index 0000000000..1c1024cb4e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp @@ -0,0 +1,785 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 +#include +#include +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/trajectory_generator_circ.h" +#include "pilz_industrial_motion_planner_testutils/command_types_typedef.h" +#include "pilz_industrial_motion_planner_testutils/xml_testdata_loader.h" +#include "test_utils.h" + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string PARAM_TARGET_LINK_NAME("target_link"); +const std::string CARTESIAN_POSITION_TOLERANCE("cartesian_position_tolerance"); +const std::string ANGULAR_ACC_TOLERANCE("angular_acc_tolerance"); +const std::string ROTATION_AXIS_NORM_TOLERANCE("rot_axis_norm_tolerance"); +const std::string ACCELERATION_TOLERANCE("acceleration_tolerance"); +const std::string OTHER_TOLERANCE("other_tolerance"); + +#define SKIP_IF_GRIPPER \ + if (GetParam() == PARAM_MODEL_WITH_GRIPPER_NAME) \ + { \ + SUCCEED(); \ + return; \ + }; + +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner_testutils; + +class TrajectoryGeneratorCIRCTest : public testing::TestWithParam +{ +protected: + /** + * @brief Create test scenario for circ trajectory generator + * + */ + void SetUp() override; + + void checkCircResult(const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanResponse& res); + + void getCircCenter(const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanResponse& res, Eigen::Vector3d& circ_center); + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + std::unique_ptr circ_; + // test data provider + std::unique_ptr tdp_; + + // test parameters from parameter server + std::string planning_group_, target_link_, test_data_file_name_; + int random_trial_num_; + double cartesian_position_tolerance_, angular_acc_tolerance_, rot_axis_norm_tolerance_, acceleration_tolerance_, + other_tolerance_; + LimitsContainer planner_limits_; +}; + +void TrajectoryGeneratorCIRCTest::SetUp() +{ + // get parameters + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + ASSERT_TRUE(ph_.getParam(CARTESIAN_POSITION_TOLERANCE, cartesian_position_tolerance_)); + ASSERT_TRUE(ph_.getParam(ANGULAR_ACC_TOLERANCE, angular_acc_tolerance_)); + ASSERT_TRUE(ph_.getParam(ROTATION_AXIS_NORM_TOLERANCE, rot_axis_norm_tolerance_)); + ASSERT_TRUE(ph_.getParam(ACCELERATION_TOLERANCE, acceleration_tolerance_)); + ASSERT_TRUE(ph_.getParam(OTHER_TOLERANCE, other_tolerance_)); + + // check robot model + testutils::checkRobotModel(robot_model_, planning_group_, target_link_); + + // load the test data provider + tdp_.reset(new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_ }); + ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider."; + + tdp_->setRobotModel(robot_model_); + + // create the limits container + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(ph_, + robot_model_->getActiveJointModels()); + CartesianLimit cart_limits; + // Cartesian limits are chose as such values to ease the manually compute the + // trajectory + + cart_limits.setMaxRotationalVelocity(1 * M_PI); + cart_limits.setMaxTranslationalAcceleration(1 * M_PI); + cart_limits.setMaxTranslationalDeceleration(1 * M_PI); + cart_limits.setMaxTranslationalVelocity(1 * M_PI); + planner_limits_.setJointLimits(joint_limits); + planner_limits_.setCartesianLimits(cart_limits); + + // initialize the LIN trajectory generator + circ_.reset(new TrajectoryGeneratorCIRC(robot_model_, planner_limits_)); + ASSERT_NE(nullptr, circ_) << "failed to create CIRC trajectory generator"; +} + +void TrajectoryGeneratorCIRCTest::checkCircResult(const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanResponse& res) +{ + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + EXPECT_TRUE(testutils::isGoalReached(res.trajectory_->getFirstWayPointPtr()->getRobotModel(), + res_msg.trajectory.joint_trajectory, req, other_tolerance_)); + + EXPECT_TRUE( + testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer())); + + EXPECT_EQ(req.path_constraints.position_constraints.size(), 1u); + EXPECT_EQ(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.size(), 1u); + + // Check that all point have the equal distance to the center + Eigen::Vector3d circ_center; + getCircCenter(req, res, circ_center); + + for (std::size_t i = 0; i < res.trajectory_->getWayPointCount(); ++i) + { + Eigen::Affine3d waypoint_pose = res.trajectory_->getWayPointPtr(i)->getFrameTransform(target_link_); + EXPECT_NEAR( + (res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation() - circ_center).norm(), + (circ_center - waypoint_pose.translation()).norm(), cartesian_position_tolerance_); + } + + // check translational and rotational paths + ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_, acceleration_tolerance_)); + ASSERT_TRUE(testutils::checkCartesianRotationalPath(res.trajectory_, target_link_, angular_acc_tolerance_, + rot_axis_norm_tolerance_)); + + for (size_t idx = 0; idx < res.trajectory_->getLastWayPointPtr()->getVariableCount(); ++idx) + { + EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_); + EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_); + } +} + +void TrajectoryGeneratorCIRCTest::getCircCenter(const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanResponse& res, + Eigen::Vector3d& circ_center) +{ + if (req.path_constraints.name == "center") + { + tf2::convert( + req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position, circ_center); + } + else if (req.path_constraints.name == "interim") + { + Eigen::Vector3d interim; + tf2::convert( + req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position, interim); + Eigen::Vector3d start = res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation(); + Eigen::Vector3d goal = res.trajectory_->getLastWayPointPtr()->getFrameTransform(target_link_).translation(); + + const Eigen::Vector3d t = interim - start; + const Eigen::Vector3d u = goal - start; + const Eigen::Vector3d v = goal - interim; + + const Eigen::Vector3d w = t.cross(u); + + ASSERT_GT(w.norm(), 1e-8) << "Circle center not well defined for given start, interim and goal."; + + circ_center = start + (u * t.dot(t) * u.dot(v) - t * u.dot(u) * t.dot(v)) * 0.5 / pow(w.norm(), 2); + } +} + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + */ +TEST(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping) +{ + { + std::shared_ptr cnp_ex{ new CircleNoPlane("") }; + EXPECT_EQ(cnp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr cts_ex{ new CircleToSmall("") }; + EXPECT_EQ(cts_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr cpdr_ex{ new CenterPointDifferentRadius("") }; + EXPECT_EQ(cpdr_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr ctcf_ex{ new CircTrajectoryConversionFailure("") }; + EXPECT_EQ(ctcf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr upcn_ex{ new UnknownPathConstraintName("") }; + EXPECT_EQ(upcn_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr npc_ex{ new NoPositionConstraints("") }; + EXPECT_EQ(npc_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr npp_ex{ new NoPrimitivePose("") }; + EXPECT_EQ(npp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr ulnoap_ex{ new UnknownLinkNameOfAuxiliaryPoint("") }; + EXPECT_EQ(ulnoap_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME); + } + + { + std::shared_ptr nocm_ex{ new NumberOfConstraintsMismatch("") }; + EXPECT_EQ(nocm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr cjmiss_ex{ new CircJointMissingInStartState("") }; + EXPECT_EQ(cjmiss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + } + + { + std::shared_ptr cifgi_ex{ new CircInverseForGoalIncalculable("") }; + EXPECT_EQ(cifgi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + } +} + +// Instantiate the test cases for robot model with and without gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorCIRCTest, + ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); + +/** + * @brief Construct a TrajectoryGeneratorCirc with no limits given + */ +TEST_P(TrajectoryGeneratorCIRCTest, noLimits) +{ + LimitsContainer planner_limits; + EXPECT_THROW(TrajectoryGeneratorCIRC(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); +} + +/** + * @brief test invalid motion plan request with incomplete start state and + * cartesian goal + */ +TEST_P(TrajectoryGeneratorCIRCTest, incompleteStartState) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + planning_interface::MotionPlanRequest req{ circ.toRequest() }; + EXPECT_GT(req.start_state.joint_state.name.size(), 1u); + req.start_state.joint_state.name.resize(1); + req.start_state.joint_state.position.resize(1); // prevent failing check for equal sizes + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief test invalid motion plan request with non zero start velocity + */ +TEST_P(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity) +{ + moveit_msgs::MotionPlanRequest req{ tdp_->getCircJointCenterCart("circ1_center_2").toRequest() }; + + // start state has non-zero velocity + req.start_state.joint_state.velocity.push_back(1.0); + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + req.start_state.joint_state.velocity.clear(); +} + +TEST_P(TrajectoryGeneratorCIRCTest, ValidCommand) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + planning_interface::MotionPlanResponse res; + EXPECT_TRUE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); +} + +/** + * @brief Generate invalid circ with to high vel scaling + */ +TEST_P(TrajectoryGeneratorCIRCTest, velScaleToHigh) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + circ.setVelocityScale(1.0); + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::PLANNING_FAILED); +} + +/** + * @brief Generate invalid circ with to high acc scaling + */ +TEST_P(TrajectoryGeneratorCIRCTest, accScaleToHigh) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + circ.setAccelerationScale(1.0); + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::PLANNING_FAILED); +} + +/** + * @brief Use three points (with center) with a really small distance between to + * trigger a internal throw from KDL + */ +TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithCenter) +{ + // Define auxiliary point and goal to be the same as the start + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 1e-8; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1e-8; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.z += 1e-8; + circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getGoalConfiguration().getPose().position.x -= 1e-8; + circ.getGoalConfiguration().getPose().position.y -= 1e-8; + circ.getGoalConfiguration().getPose().position.z -= 1e-8; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief Use three points (with interim) with a really small distance between + * + * Expected: Planning should fail. + */ +TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithInterim) +{ + // Define auxiliary point and goal to be the same as the start + auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 1e-8; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1e-8; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.z += 1e-8; + circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getGoalConfiguration().getPose().position.x -= 1e-8; + circ.getGoalConfiguration().getPose().position.y -= 1e-8; + circ.getGoalConfiguration().getPose().position.z -= 1e-8; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief test invalid motion plan request with no aux point defined + */ +TEST_P(TrajectoryGeneratorCIRCTest, emptyAux) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + planning_interface::MotionPlanRequest req = circ.toRequest(); + + req.path_constraints.position_constraints.clear(); + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief test invalid motion plan request with no aux name defined + */ +TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxName) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + planning_interface::MotionPlanRequest req = circ.toRequest(); + + req.path_constraints.name = ""; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief test invalid motion plan request with invalid link name in the + * auxiliary point + */ +TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxLinkName) +{ + auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; + + planning_interface::MotionPlanRequest req = circ.toRequest(); + + req.path_constraints.position_constraints.front().link_name = "INVALID"; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME); +} + +/** + * @brief test the circ planner with invalid center point + */ +TEST_P(TrajectoryGeneratorCIRCTest, invalidCenter) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief test the circ planner with colinear start/goal/center position + * + * Expected: Planning should fail since the path is not uniquely defined. + */ +TEST_P(TrajectoryGeneratorCIRCTest, colinearCenter) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose()); + + // Stretch start and goal pose along line + circ.getStartConfiguration().getPose().position.x -= 0.1; + circ.getGoalConfiguration().getPose().position.x += 0.1; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief test the circ planner with colinear start/goal/interim position + * + * Expected: Planning should fail. These positions do not even represent a + * circle. + */ +TEST_P(TrajectoryGeneratorCIRCTest, colinearInterim) +{ + auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose()); + + // Stretch start and goal pose along line + circ.getStartConfiguration().getPose().position.x -= 0.1; + circ.getGoalConfiguration().getPose().position.x += 0.1; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief test the circ planner with half circle with interim point + * + * The request contains start/interim/goal such that + * start, center (not explicitly given) and goal are colinear + * + * Expected: Planning should successfully return. + */ +TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim) +{ + // get the test data from xml + auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); +} + +/** + * @brief test the circ planner with colinear start/center/interim positions + * + * The request contains start/interim/goal such that + * start, center (not explicitly given) and interim are colinear. + * In case the interim is used as auxiliary point for KDL::Path_Circle this + * should fail. + * + * Expected: Planning should successfully return. + */ +TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim) +{ + auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; + + // alter start, interim and goal such that start/center and interim are + // colinear + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose()); + + circ.getStartConfiguration().getPose().position.x -= 0.2; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 0.2; + circ.getGoalConfiguration().getPose().position.y -= 0.2; + + circ.setAccelerationScale(0.05); + circ.setVelocityScale(0.05); + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + planning_interface::MotionPlanResponse res; + EXPECT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief test the circ planner with a circ path where the angle between goal + * and interim is larger than 180 degree + * + * The request contains start/interim/goal such that 180 degree < interim angle + * < goal angle. + * + * Expected: Planning should successfully return. + */ +TEST_P(TrajectoryGeneratorCIRCTest, interimLarger180Degree) +{ + auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; + + // alter start, interim and goal such that start/center and interim are + // colinear + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose()); + + circ.getStartConfiguration().getPose().position.x -= 0.2; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 0.14142136; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y -= 0.14142136; + circ.getGoalConfiguration().getPose().position.y -= 0.2; + + circ.setAccelerationScale(0.05); + circ.setVelocityScale(0.05); + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + planning_interface::MotionPlanResponse res; + EXPECT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief test the circ planner with center point and joint goal + */ +TEST_P(TrajectoryGeneratorCIRCTest, centerPointJointGoal) +{ + SKIP_IF_GRIPPER + + auto circ{ tdp_->getCircJointCenterCart("circ1_center_2") }; + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief A valid circ request contains a helping point (interim or center), in + * this test a additional + * point is defined as an invalid test case + */ +TEST_P(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + // Contains one pose (interim / center) + ASSERT_EQ(req.path_constraints.position_constraints.back().constraint_region.primitive_poses.size(), 1u); + + // Define a additional pose here + geometry_msgs::Pose center_position; + center_position.position.x = 0.0; + center_position.position.y = 0.0; + center_position.position.z = 0.65; + req.path_constraints.position_constraints.back().constraint_region.primitive_poses.push_back(center_position); + + planning_interface::MotionPlanResponse res; + ASSERT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief Joint Goals are expected to match the start state in number and + * joint_names + * Here an additional joint constraints is "falsely" defined to check for the + * error. + */ +TEST_P(TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint) +{ + auto circ{ tdp_->getCircJointCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + // Define the additional joint constraint + moveit_msgs::JointConstraint joint_constraint; + joint_constraint.joint_name = req.goal_constraints.front().joint_constraints.front().joint_name; + req.goal_constraints.front().joint_constraints.push_back(joint_constraint); //<-- Additional constraint + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief test the circ planner with center point and pose goal + */ +TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoal) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief Set a frame id only on the position constrainst + */ +TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraints) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief Set a frame id only on the orientation constrainst + */ +TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstraints) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief Set a frame_id on both position and orientation constraints + */ +TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + // Both set + req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame(); + req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief test the circ planner with interim point with joint goal + */ +TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoal) +{ + SKIP_IF_GRIPPER + + auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief test the circ planner with interim point with joint goal and a close + * to zero velocity of the start state + * + * The generator is expected to be robust against a velocity beeing almost zero. + */ +TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero) +{ + SKIP_IF_GRIPPER + + auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + // Set velocity near zero + req.start_state.joint_state.velocity = std::vector(req.start_state.joint_state.position.size(), 1e-16); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief test the circ planner with interim point with pose goal + */ +TEST_P(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal) +{ + auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_trajectory_generator_circ"); + ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.test new file mode 100644 index 0000000000..6d84d42105 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.test @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp new file mode 100644 index 0000000000..aaba86c0f1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp @@ -0,0 +1,434 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 +#include +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/joint_limits_container.h" +#include "pilz_industrial_motion_planner/trajectory_generator_circ.h" +#include "pilz_industrial_motion_planner/trajectory_generator_lin.h" +#include "pilz_industrial_motion_planner/trajectory_generator_ptp.h" + +#include "test_utils.h" + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string PARAM_TARGET_LINK_NAME("target_link"); + +/** + * A value type container to combine type and value + * In the tests types are trajectory generators. + * value = 0 refers to robot model without gripper + * value = 1 refers to robot model with gripper + */ +template +class ValueTypeContainer +{ +public: + typedef T Type_; + static const int VALUE = N; +}; +template +const int ValueTypeContainer::VALUE; + +typedef ValueTypeContainer PTP_NO_GRIPPER; +typedef ValueTypeContainer PTP_WITH_GRIPPER; +typedef ValueTypeContainer LIN_NO_GRIPPER; +typedef ValueTypeContainer LIN_WITH_GRIPPER; +typedef ValueTypeContainer CIRC_NO_GRIPPER; +typedef ValueTypeContainer CIRC_WITH_GRIPPER; + +typedef ::testing::Types + TrajectoryGeneratorCommonTestTypes; + +typedef ::testing::Types TrajectoryGeneratorCommonTestTypesNoGripper; + +typedef ::testing::Types + TrajectoryGeneratorCommonTestTypesWithGripper; + +/** + * type parameterized test fixture + */ +template +class TrajectoryGeneratorCommonTest : public ::testing::Test +{ +protected: + void SetUp() override + { + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + + testutils::checkRobotModel(robot_model_, planning_group_, target_link_); + + // create the limits container + std::string robot_description_param = (!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME); + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + ros::NodeHandle(robot_description_param + "_planning"), robot_model_->getActiveJointModels()); + pilz_industrial_motion_planner::CartesianLimit cart_limits; + cart_limits.setMaxRotationalVelocity(0.5 * M_PI); + cart_limits.setMaxTranslationalAcceleration(2); + cart_limits.setMaxTranslationalDeceleration(2); + cart_limits.setMaxTranslationalVelocity(1); + pilz_industrial_motion_planner::LimitsContainer planner_limits; + planner_limits.setJointLimits(joint_limits); + planner_limits.setCartesianLimits(cart_limits); + + // create planner instance + trajectory_generator_ = std::unique_ptr(new typename T::Type_(robot_model_, planner_limits)); + ASSERT_NE(nullptr, trajectory_generator_) << "failed to create trajectory generator"; + + // create a valid motion plan request with goal in joint space as basis for + // tests + req_.group_name = planning_group_; + req_.max_velocity_scaling_factor = 1.0; + req_.max_acceleration_scaling_factor = 1.0; + robot_state::RobotState rstate(robot_model_); + rstate.setToDefaultValues(); + rstate.setJointGroupPositions(planning_group_, { 0, M_PI / 2, 0, M_PI / 2, 0, 0 }); + rstate.setVariableVelocities(std::vector(rstate.getVariableCount(), 0.0)); + moveit::core::robotStateToRobotStateMsg(rstate, req_.start_state, false); + moveit_msgs::Constraints goal_constraint; + moveit_msgs::JointConstraint joint_constraint; + joint_constraint.joint_name = this->robot_model_->getActiveJointModels().front()->getName(); + joint_constraint.position = 0.5; + goal_constraint.joint_constraints.push_back(joint_constraint); + req_.goal_constraints.push_back(goal_constraint); + } + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ + robot_model_loader::RobotModelLoader(!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME) + .getModel() + }; + + // trajectory generator + std::unique_ptr trajectory_generator_; + planning_interface::MotionPlanResponse res_; + planning_interface::MotionPlanRequest req_; + // test parameters from parameter server + std::string planning_group_, target_link_; +}; +// Define the types we need to test +TYPED_TEST_SUITE(TrajectoryGeneratorCommonTest, TrajectoryGeneratorCommonTestTypes); + +template +class TrajectoryGeneratorCommonTestNoGripper : public TrajectoryGeneratorCommonTest +{ +}; +TYPED_TEST_SUITE(TrajectoryGeneratorCommonTestNoGripper, TrajectoryGeneratorCommonTestTypesNoGripper); + +template +class TrajectoryGeneratorCommonTestWithGripper : public TrajectoryGeneratorCommonTest +{ +}; +TYPED_TEST_SUITE(TrajectoryGeneratorCommonTestWithGripper, TrajectoryGeneratorCommonTestTypesWithGripper); + +/** + * @brief test invalid scaling factor. The scaling factor must be in the range + * of [0.0001, 1] + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideScalingFactor) +{ + this->req_.max_velocity_scaling_factor = 2.0; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + + this->req_.max_velocity_scaling_factor = 1.0; + this->req_.max_acceleration_scaling_factor = 0; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + + this->req_.max_velocity_scaling_factor = 0.00001; + this->req_.max_acceleration_scaling_factor = 1; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + + this->req_.max_velocity_scaling_factor = 1; + this->req_.max_acceleration_scaling_factor = -1; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief Test invalid motion plan request for all trajectory generators + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidGroupName) +{ + this->req_.group_name = "foot"; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val); +} + +/** + * @brief Test invalid motion plan request for all trajectory generators + */ +TYPED_TEST(TrajectoryGeneratorCommonTestNoGripper, GripperGroup) +{ + this->req_.group_name = "gripper"; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val); +} + +/** + * @brief Test invalid motion plan request for all trajectory generators + */ +TYPED_TEST(TrajectoryGeneratorCommonTestWithGripper, GripperGroup) +{ + this->req_.group_name = "gripper"; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS, this->res_.error_code_.val); +} + +/** + * @brief Test if there is a valid inverse kinematics solver for this planning + * group + * You can only test this case by commenting the planning_context.launch in the + * .test file + * //TODO create a separate robot model without ik solver and use it to create a + * trajectory generator + */ +// TYPED_TEST(TrajectoryGeneratorCommonTest, NoIKSolver) +//{ +// EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); +// EXPECT_EQ(this->res_.error_code_.val, +// moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); +//} + +/** + * @brief test the case of empty joint names in start state + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyJointNamesInStartState) +{ + this->req_.start_state.joint_state.name.clear(); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief size of joint name and joint position does not match in start state + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, InconsistentStartState) +{ + this->req_.start_state.joint_state.name.push_back("joint_7"); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief joint position out of limit in start state + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, StartPostionOutOfLimit) +{ + this->req_.start_state.joint_state.position[0] = 100; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief Check that no trajectory is generated if a start velocity is given + * + * @note This test is here for regression, however in general generators that + * can work with a given + * start velocity are highly desired. + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, StartPositionVelocityNoneZero) +{ + this->req_.start_state.joint_state.velocity[0] = 100; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief goal constraints is empty + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyGoalConstraints) +{ + this->req_.goal_constraints.clear(); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief multiple goals + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) +{ + moveit_msgs::JointConstraint joint_constraint; + moveit_msgs::PositionConstraint position_constraint; + moveit_msgs::OrientationConstraint orientation_constraint; + moveit_msgs::Constraints goal_constraint; + + // two goal constraints + this->req_.goal_constraints.push_back(goal_constraint); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + + // one joint constraint and one orientation constraint + goal_constraint.joint_constraints.push_back(joint_constraint); + goal_constraint.orientation_constraints.push_back(orientation_constraint); + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + + // one joint constraint and one Cartesian constraint + goal_constraint.position_constraints.push_back(position_constraint); + goal_constraint.orientation_constraints.push_back(orientation_constraint); + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + + // two Cartesian constraints + goal_constraint.joint_constraints.clear(); + goal_constraint.position_constraints.push_back(position_constraint); + goal_constraint.orientation_constraints.push_back(orientation_constraint); + goal_constraint.position_constraints.push_back(position_constraint); + goal_constraint.orientation_constraints.push_back(orientation_constraint); + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief invalid joint name in joint constraint + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointNameInGoal) +{ + moveit_msgs::JointConstraint joint_constraint; + joint_constraint.joint_name = "test_joint_2"; + this->req_.goal_constraints.front().joint_constraints[0] = joint_constraint; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief MissingJointConstraint + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, MissingJointConstraint) +{ + moveit_msgs::JointConstraint joint_constraint; + joint_constraint.joint_name = "test_joint_2"; + this->req_.goal_constraints.front().joint_constraints.pop_back(); //<-- Missing joint constraint + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief invalid joint position in joint constraint + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointPositionInGoal) +{ + this->req_.goal_constraints.front().joint_constraints[0].position = 100; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief invalid link name in Cartesian goal constraint + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal) +{ + moveit_msgs::PositionConstraint position_constraint; + moveit_msgs::OrientationConstraint orientation_constraint; + moveit_msgs::Constraints goal_constraint; + // link name not set + goal_constraint.position_constraints.push_back(position_constraint); + goal_constraint.orientation_constraints.push_back(orientation_constraint); + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + + // different link names in position and orientation goals + goal_constraint.position_constraints.front().link_name = "test_link_1"; + goal_constraint.orientation_constraints.front().link_name = "test_link_2"; + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + + // no solver for the link + goal_constraint.orientation_constraints.front().link_name = "test_link_1"; + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); +} + +/** + * @brief no pose set in position constraint + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyPrimitivePoses) +{ + moveit_msgs::PositionConstraint position_constraint; + moveit_msgs::OrientationConstraint orientation_constraint; + moveit_msgs::Constraints goal_constraint; + position_constraint.link_name = + this->robot_model_->getJointModelGroup(this->planning_group_)->getLinkModelNames().back(); + orientation_constraint.link_name = position_constraint.link_name; + + goal_constraint.position_constraints.push_back(position_constraint); + goal_constraint.orientation_constraints.push_back(orientation_constraint); + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_trajectory_generator_common"); + // ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.test new file mode 100644 index 0000000000..702c2dc5fc --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.test @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp new file mode 100644 index 0000000000..f5724b7af5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp @@ -0,0 +1,470 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/trajectory_generator_lin.h" +#include "pilz_industrial_motion_planner_testutils/command_types_typedef.h" +#include "pilz_industrial_motion_planner_testutils/xml_testdata_loader.h" +#include "test_utils.h" + +#include +#include +#include +#include +#include +#include + +#include + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string TARGET_LINK_HCD("target_link_hand_computed_data"); +const std::string RANDOM_TEST_TRIAL_NUM("random_trial_number"); +const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance"); +const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance"); +const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance"); +const std::string ROTATION_AXIS_NORM_TOLERANCE("rot_axis_norm_tolerance"); +const std::string VELOCITY_SCALING_FACTOR("velocity_scaling_factor"); +const std::string OTHER_TOLERANCE("other_tolerance"); + +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner_testutils; + +/** + * @brief Parameterized unittest of trajectory generator LIN to enable tests + * against + * different robot models.The parameter is the name of robot model parameter on + * the + * ros parameter server. + */ +class TrajectoryGeneratorLINTest : public testing::TestWithParam +{ +protected: + /** + * @brief Create test scenario for lin trajectory generator + * + */ + void SetUp() override; + + bool checkLinResponse(const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanResponse& res); + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + + // lin trajectory generator using model without gripper + std::unique_ptr lin_; + // test data provider + std::unique_ptr tdp_; + + // test parameters from parameter server + std::string planning_group_, target_link_hcd_, test_data_file_name_; + int random_trial_num_; + double joint_position_tolerance_, joint_velocity_tolerance_, pose_norm_tolerance_, rot_axis_norm_tolerance_, + velocity_scaling_factor_, other_tolerance_; + LimitsContainer planner_limits_; +}; + +void TrajectoryGeneratorLINTest::SetUp() +{ + // get the parameters + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(TARGET_LINK_HCD, target_link_hcd_)); + ASSERT_TRUE(ph_.getParam(RANDOM_TEST_TRIAL_NUM, random_trial_num_)); + ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_)); + ASSERT_TRUE(ph_.getParam(JOINT_VELOCITY_TOLERANCE, joint_velocity_tolerance_)); + ASSERT_TRUE(ph_.getParam(POSE_TRANSFORM_MATRIX_NORM_TOLERANCE, pose_norm_tolerance_)); + ASSERT_TRUE(ph_.getParam(ROTATION_AXIS_NORM_TOLERANCE, rot_axis_norm_tolerance_)); + ASSERT_TRUE(ph_.getParam(VELOCITY_SCALING_FACTOR, velocity_scaling_factor_)); + ASSERT_TRUE(ph_.getParam(OTHER_TOLERANCE, other_tolerance_)); + + testutils::checkRobotModel(robot_model_, planning_group_, target_link_hcd_); + + // load the test data provider + tdp_.reset(new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_ }); + ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider."; + + tdp_->setRobotModel(robot_model_); + + // create the limits container + // TODO, move this also into test data set + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(ph_, + robot_model_->getActiveJointModels()); + CartesianLimit cart_limits; + cart_limits.setMaxRotationalVelocity(0.5 * M_PI); + cart_limits.setMaxTranslationalAcceleration(2); + cart_limits.setMaxTranslationalDeceleration(2); + cart_limits.setMaxTranslationalVelocity(1); + planner_limits_.setJointLimits(joint_limits); + planner_limits_.setCartesianLimits(cart_limits); + + // initialize the LIN trajectory generator + lin_.reset(new TrajectoryGeneratorLIN(robot_model_, planner_limits_)); + ASSERT_NE(nullptr, lin_) << "Failed to create LIN trajectory generator."; +} + +bool TrajectoryGeneratorLINTest::checkLinResponse(const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanResponse& res) +{ + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + if (!testutils::isGoalReached(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_)) + { + return false; + } + + if (!testutils::checkCartesianLinearity(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_, + rot_axis_norm_tolerance_)) + { + return false; + } + + if (!testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer())) + { + return false; + } + + return true; +} + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + */ +TEST(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping) +{ + { + std::shared_ptr ltcf_ex{ new LinTrajectoryConversionFailure("") }; + EXPECT_EQ(ltcf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + } + + { + std::shared_ptr jnm_ex{ new JointNumberMismatch("") }; + EXPECT_EQ(jnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr ljmiss_ex{ new LinJointMissingInStartState("") }; + EXPECT_EQ(ljmiss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + } + + { + std::shared_ptr lifgi_ex{ new LinInverseForGoalIncalculable("") }; + EXPECT_EQ(lifgi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + } +} + +// Instantiate the test cases for robot model with and without gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorLINTest, + ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); + +/** + * @brief test the lin planner with invalid motion plan request which has non + * zero start velocity + */ +TEST_P(TrajectoryGeneratorLINTest, nonZeroStartVelocity) +{ + planning_interface::MotionPlanRequest req{ tdp_->getLinJoint("lin2").toRequest() }; + + // add non-zero velocity in the start state + req.start_state.joint_state.velocity.push_back(1.0); + + // try to generate the result + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(lin_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief test the lin planner with joint space goal + */ +TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoal) +{ + planning_interface::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + + // generate the LIN trajectory + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(lin_->generate(lin_joint_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + + // check the resulted trajectory + EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); +} + +/** + * @brief test the lin planner with joint space goal with start velocity almost + * zero + */ +TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity) +{ + planning_interface::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + + // Set velocity near zero + lin_joint_req.start_state.joint_state.velocity = + std::vector(lin_joint_req.start_state.joint_state.position.size(), 1e-16); + + // generate the LIN trajectory + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(lin_->generate(lin_joint_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + + // check the resulted trajectory + EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); +} + +/** + * @brief test the lin planner with Cartesian goal + */ +TEST_P(TrajectoryGeneratorLINTest, cartesianSpaceGoal) +{ + // construct motion plan request + moveit_msgs::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; + + // generate lin trajectory + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(lin_->generate(lin_cart_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + + // check the resulted trajectory + EXPECT_TRUE(checkLinResponse(lin_cart_req, res)); +} + +/** + * @brief test the trapezoid shape of the planning trajectory in Cartesian space + * + * The test checks translational path for a trapezoid velocity profile. + * Due to the way the acceleration is calculated 1 or 2 intermediate points + * occur that are neither + * acceleration, constant or deceleration. + */ +TEST_P(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile) +{ + // construct motion plan request + moveit_msgs::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + + /// +++++++++++++++++++++++ + /// + plan LIN trajectory + + /// +++++++++++++++++++++++ + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(lin_->generate(lin_joint_req, res, 0.01)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + + ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_hcd_)); + + // check last point for vel=acc=0 + for (size_t idx = 0; idx < res.trajectory_->getLastWayPointPtr()->getVariableCount(); ++idx) + { + EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_); + EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_); + } +} + +/** + * @brief Check that lin planner returns 'false' if + * calculated lin trajectory violates velocity/acceleration or deceleration + * limits. + * + * + * Test Sequence: + * 1. Call function with lin request violating velocity/acceleration or + * deceleration limits. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryGeneratorLINTest, LinPlannerLimitViolation) +{ + LinJoint lin{ tdp_->getLinJoint("lin2") }; + lin.setAccelerationScale(1.01); + + planning_interface::MotionPlanResponse res; + ASSERT_FALSE(lin_->generate(lin.toRequest(), res)); +} + +/** + * @brief test joint linear movement with discontinuities in joint space + * + * This will violate joint velocity/acceleration limits. + * + * Test Sequence: + * 1. Generate lin trajectory which is discontinuous in joint space. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryGeneratorLINTest, LinPlannerDiscontinuousJointTraj) +{ + LinJoint lin{ tdp_->getLinJoint("lin2") }; + // Alter goal joint configuration (represents the same cartesian pose, but + // does not fit together with start config) + lin.getGoalConfiguration().setJoint(1, 1.63); + lin.getGoalConfiguration().setJoint(2, 0.96); + lin.getGoalConfiguration().setJoint(4, -2.48); + lin.setVelocityScale(1.0); + lin.setAccelerationScale(1.0); + + planning_interface::MotionPlanResponse res; + ASSERT_FALSE(lin_->generate(lin.toRequest(), res)); +} + +/** + * @brief test joint linear movement with equal goal and start + * + * Test Sequence: + * 1. Call function with lin request start = goal + * + * Expected Results: + * 1. trajectory generation is successful. + */ +TEST_P(TrajectoryGeneratorLINTest, LinStartEqualsGoal) +{ + // construct motion plan request + moveit_msgs::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + + moveit::core::RobotState start_state(robot_model_); + jointStateToRobotState(lin_joint_req.start_state.joint_state, start_state); + + for (auto& joint_constraint : lin_joint_req.goal_constraints.at(0).joint_constraints) + { + joint_constraint.position = start_state.getVariablePosition(joint_constraint.joint_name); + } + + // generate the LIN trajectory + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(lin_->generate(lin_joint_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + + // check the resulted trajectory + EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); +} + +/** + * @brief Checks that constructor throws an exception if no limits are given. + * + * Test Sequence: + * 1. Call Ctor without set limits. + * + * Expected Results: + * 1. Ctor throws exception. + */ +TEST_P(TrajectoryGeneratorLINTest, CtorNoLimits) +{ + pilz_industrial_motion_planner::LimitsContainer planner_limits; + + EXPECT_THROW(pilz_industrial_motion_planner::TrajectoryGeneratorLIN(robot_model_, planner_limits), + pilz_industrial_motion_planner::TrajectoryGeneratorInvalidLimitsException); +} + +/** + * @brief Checks that generate() function returns 'false' if called with an + * incorrect number of joints. + * + * Test Sequence: + * 1. Call functions with incorrect number of joints. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryGeneratorLINTest, IncorrectJointNumber) +{ + // construct motion plan request + moveit_msgs::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + + // Ensure that request consists of an incorrect number of joints. + lin_joint_req.goal_constraints.front().joint_constraints.pop_back(); + + // generate the LIN trajectory + planning_interface::MotionPlanResponse res; + ASSERT_FALSE(lin_->generate(lin_joint_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief test invalid motion plan request with incomplete start state and + * cartesian goal + */ +TEST_P(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState) +{ + // construct motion plan request + moveit_msgs::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; + EXPECT_GT(lin_cart_req.start_state.joint_state.name.size(), 1u); + lin_cart_req.start_state.joint_state.name.resize(1); + lin_cart_req.start_state.joint_state.position.resize(1); // prevent failing check for equal sizes + + // generate lin trajectory + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(lin_->generate(lin_cart_req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief Set a frame id in goal constraint with cartesian goal on both position + * and orientation constraints + */ +TEST_P(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints) +{ + // construct motion plan request + moveit_msgs::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; + + lin_cart_req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame(); + lin_cart_req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame(); + + // generate lin trajectory + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(lin_->generate(lin_cart_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + + // check the resulted trajectory + EXPECT_TRUE(checkLinResponse(lin_cart_req, res)); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_trajectory_generator_lin"); + ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.test new file mode 100644 index 0000000000..9cec117a1d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.test @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp new file mode 100644 index 0000000000..94889460d3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp @@ -0,0 +1,971 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/trajectory_generator_ptp.h" +#include "test_utils.h" + +#include +#include +#include +#include + +// parameters for parameterized tests +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string PARAM_TARGET_LINK_NAME("target_link"); +const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance"); +const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance"); +const std::string JOINT_ACCELERATION_TOLERANCE("joint_acceleration_tolerance"); +const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance"); + +using namespace pilz_industrial_motion_planner; + +class TrajectoryGeneratorPTPTest : public testing::TestWithParam +{ +protected: + /** + * @brief Create test fixture for ptp trajectory generator + * + */ + void SetUp() override; + + /** + * @brief check the resulted joint trajectory + * @param trajectory + * @param req + * @param joint_limits + * @return + */ + bool checkTrajectory(const trajectory_msgs::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, const JointLimitsContainer& joint_limits); + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + + // trajectory generator + std::unique_ptr ptp_; + + // test parameters from parameter server + LimitsContainer planner_limits_; + std::string planning_group_, target_link_; + double joint_position_tolerance_, joint_velocity_tolerance_, joint_acceleration_tolerance_, pose_norm_tolerance_; +}; + +void TrajectoryGeneratorPTPTest::SetUp() +{ + // get parameters from parameter server + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_)); + ASSERT_TRUE(ph_.getParam(JOINT_VELOCITY_TOLERANCE, joint_velocity_tolerance_)); + ASSERT_TRUE(ph_.getParam(JOINT_ACCELERATION_TOLERANCE, joint_acceleration_tolerance_)); + ASSERT_TRUE(ph_.getParam(POSE_TRANSFORM_MATRIX_NORM_TOLERANCE, pose_norm_tolerance_)); + + testutils::checkRobotModel(robot_model_, planning_group_, target_link_); + + // create the limits container + JointLimitsContainer joint_limits; + for (const auto& jmg : robot_model_->getJointModelGroups()) + { + std::vector joint_names = jmg->getActiveJointModelNames(); + JointLimit joint_limit; + joint_limit.max_position = 3.124; + joint_limit.min_position = -3.124; + joint_limit.has_velocity_limits = true; + joint_limit.max_velocity = 1; + joint_limit.has_acceleration_limits = true; + joint_limit.max_acceleration = 0.5; + joint_limit.has_deceleration_limits = true; + joint_limit.max_deceleration = -1; + for (const auto& joint_name : joint_names) + { + joint_limits.addLimit(joint_name, joint_limit); + } + } + + // create the trajectory generator + planner_limits_.setJointLimits(joint_limits); + ptp_.reset(new TrajectoryGeneratorPTP(robot_model_, planner_limits_)); + ASSERT_NE(nullptr, ptp_); +} + +bool TrajectoryGeneratorPTPTest::checkTrajectory(const trajectory_msgs::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, + const JointLimitsContainer& joint_limits) +{ + return (testutils::isTrajectoryConsistent(trajectory) && + testutils::isGoalReached(trajectory, req.goal_constraints.front().joint_constraints, + joint_position_tolerance_, joint_velocity_tolerance_) && + testutils::isPositionBounded(trajectory, joint_limits) && + testutils::isVelocityBounded(trajectory, joint_limits) && + testutils::isAccelerationBounded(trajectory, joint_limits)); +} + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + */ +TEST(TrajectoryGeneratorPTPTest, TestExceptionErrorCodeMapping) +{ + { + std::shared_ptr pvpsf_ex{ new PtpVelocityProfileSyncFailed("") }; + EXPECT_EQ(pvpsf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + } + + { + std::shared_ptr pnisfgp_ex{ new PtpNoIkSolutionForGoalPose("") }; + EXPECT_EQ(pnisfgp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + } +} + +// Instantiate the test cases for robot model with and without gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorPTPTest, + ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); +/** + * @brief Construct a TrajectoryGeneratorPTP with no limits given + */ +TEST_P(TrajectoryGeneratorPTPTest, noLimits) +{ + LimitsContainer planner_limits; + EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); +} + +/** + * @brief Send an empty request, define res.trajectory_ + * + * - Test Sequence: + * 1. Create request, define a trajectory in the result + * 2. assign at least one joint limit will all required limits + * + * - Expected Results: + * 1. the res.trajectory_ should be cleared (contain no waypoints) + */ +TEST_P(TrajectoryGeneratorPTPTest, emptyRequest) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + + robot_trajectory::RobotTrajectoryPtr trajectory( + new robot_trajectory::RobotTrajectory(this->robot_model_, planning_group_)); + robot_state::RobotState state(this->robot_model_); + trajectory->addPrefixWayPoint(state, 0); + res.trajectory_ = trajectory; + + EXPECT_FALSE(res.trajectory_->empty()); + + EXPECT_FALSE(ptp_->generate(req, res)); + + EXPECT_TRUE(res.trajectory_->empty()); +} + +/** + * @brief Construct a TrajectoryGeneratorPTP with missing velocity limits + */ +TEST_P(TrajectoryGeneratorPTPTest, missingVelocityLimits) +{ + LimitsContainer planner_limits; + + JointLimitsContainer joint_limits; + auto joint_models = robot_model_->getActiveJointModels(); + JointLimit joint_limit; + joint_limit.has_velocity_limits = false; + joint_limit.has_acceleration_limits = true; + joint_limit.max_deceleration = -1; + joint_limit.has_deceleration_limits = true; + for (const auto& joint_model : joint_models) + { + ASSERT_TRUE(joint_limits.addLimit(joint_model->getName(), joint_limit)) + << "Failed to add the limits for joint " << joint_model->getName(); + } + + planner_limits.setJointLimits(joint_limits); + EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); +} + +/** + * @brief Construct a TrajectoryGeneratorPTP missing deceleration limits + */ +TEST_P(TrajectoryGeneratorPTPTest, missingDecelerationimits) +{ + LimitsContainer planner_limits; + + JointLimitsContainer joint_limits; + const auto& joint_models = robot_model_->getActiveJointModels(); + JointLimit joint_limit; + joint_limit.has_velocity_limits = true; + joint_limit.has_acceleration_limits = true; + joint_limit.has_deceleration_limits = false; + for (const auto& joint_model : joint_models) + { + ASSERT_TRUE(joint_limits.addLimit(joint_model->getName(), joint_limit)) + << "Failed to add the limits for joint " << joint_model->getName(); + } + + planner_limits.setJointLimits(joint_limits); + EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); +} + +/** + * @brief test the constructor when insufficient limits are given + * - Test Sequence: + * 1. assign joint limits without acc and dec + * 2. assign at least one joint limit per group with all required limits + * + * - Expected Results: + * 1. the constructor throws an exception of type + * TrajectoryGeneratorInvalidLimitsException + * 2. the constructor throws no exception + */ +TEST_P(TrajectoryGeneratorPTPTest, testInsufficientLimit) +{ + /**********/ + /* Step 1 */ + /**********/ + const auto& joint_models = robot_model_->getActiveJointModels(); + ASSERT_TRUE(joint_models.size()); + + // joint limit with insufficient limits (no acc/dec limits) + JointLimit insufficient_limit; + insufficient_limit.has_position_limits = true; + insufficient_limit.max_position = 2.5; + insufficient_limit.min_position = -2.5; + insufficient_limit.has_velocity_limits = true; + insufficient_limit.max_velocity = 1.256; + insufficient_limit.has_acceleration_limits = false; + insufficient_limit.has_deceleration_limits = false; + JointLimitsContainer insufficient_joint_limits; + for (const auto& joint_model : joint_models) + { + ASSERT_TRUE(insufficient_joint_limits.addLimit(joint_model->getName(), insufficient_limit)) + << "Failed to add the limits for joint " << joint_model->getName(); + } + LimitsContainer insufficient_planner_limits; + insufficient_planner_limits.setJointLimits(insufficient_joint_limits); + + EXPECT_THROW( + { + std::unique_ptr ptp_error( + new TrajectoryGeneratorPTP(robot_model_, insufficient_planner_limits)); + }, + TrajectoryGeneratorInvalidLimitsException); + + /**********/ + /* Step 2 */ + /**********/ + // joint limit with sufficient limits + JointLimit sufficient_limit; + sufficient_limit.has_position_limits = true; + sufficient_limit.max_position = 2.356; + sufficient_limit.min_position = -2.356; + sufficient_limit.has_velocity_limits = true; + sufficient_limit.max_velocity = 1; + sufficient_limit.has_acceleration_limits = true; + sufficient_limit.max_acceleration = 0.5; + sufficient_limit.has_deceleration_limits = true; + sufficient_limit.max_deceleration = -1; + JointLimitsContainer sufficient_joint_limits; + // fill joint limits container, such that it contains one sufficient limit and + // all others are insufficient + for (const auto& jmg : robot_model_->getJointModelGroups()) + { + const auto& joint_names{ jmg->getActiveJointModelNames() }; + ASSERT_FALSE(joint_names.empty()); + ASSERT_TRUE(sufficient_joint_limits.addLimit(joint_names.front(), sufficient_limit)) + << "Failed to add the limits for joint " << joint_names.front(); + + for (auto it = std::next(joint_names.begin()); it != joint_names.end(); ++it) + { + ASSERT_TRUE(sufficient_joint_limits.addLimit((*it), insufficient_limit)) + << "Failed to add the limits for joint " << (*it); + } + } + LimitsContainer sufficient_planner_limits; + sufficient_planner_limits.setJointLimits(sufficient_joint_limits); + + EXPECT_NO_THROW({ + std::unique_ptr ptp_no_error( + new TrajectoryGeneratorPTP(robot_model_, sufficient_planner_limits)); + }); +} + +/** + * @brief test the ptp trajectory generator of Cartesian space goal + */ +TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoal) +{ + //*************************************** + //*** prepare the motion plan request *** + //*************************************** + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + + // cartesian goal pose + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0.1; + pose.pose.position.y = 0.2; + pose.pose.position.z = 0.65; + pose.pose.orientation.w = 1.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + std::vector tolerance_pose(3, 0.01); + std::vector tolerance_angle(3, 0.01); + moveit_msgs::Constraints pose_goal = + kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle); + req.goal_constraints.push_back(pose_goal); + + //**************************************** + //*** test robot model without gripper *** + //**************************************** + ASSERT_TRUE(ptp_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + if (!res_msg.trajectory.joint_trajectory.points.empty()) + { + EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); + } + else + { + FAIL() << "Received empty trajectory."; + } + + // check goal pose + EXPECT_TRUE(testutils::isGoalReached(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_)); +} + +/** + * @brief Check that missing a link_name in position or orientation constraints + * is detected + */ +TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints) +{ + //*************************************** + //*** prepare the motion plan request *** + //*************************************** + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + + // cartesian goal pose + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0.1; + pose.pose.position.y = 0.2; + pose.pose.position.z = 0.65; + pose.pose.orientation.w = 1.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + std::vector tolerance_pose(3, 0.01); + std::vector tolerance_angle(3, 0.01); + moveit_msgs::Constraints pose_goal = + kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle); + req.goal_constraints.push_back(pose_goal); + + planning_interface::MotionPlanRequest req_no_position_constaint_link_name = req; + req_no_position_constaint_link_name.goal_constraints.front().position_constraints.front().link_name = ""; + ASSERT_FALSE(ptp_->generate(req_no_position_constaint_link_name, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + + planning_interface::MotionPlanRequest req_no_orientation_constaint_link_name = req; + req_no_orientation_constaint_link_name.goal_constraints.front().orientation_constraints.front().link_name = ""; + ASSERT_FALSE(ptp_->generate(req_no_orientation_constaint_link_name, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief test the ptp trajectory generator of invalid Cartesian space goal + */ +TEST_P(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0.1; + pose.pose.position.y = 0.2; + pose.pose.position.z = 2.5; + pose.pose.orientation.w = 1.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + std::vector tolerance_pose(3, 0.01); + std::vector tolerance_angle(3, 0.01); + moveit_msgs::Constraints pose_goal = + kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle); + req.goal_constraints.push_back(pose_goal); + + ASSERT_FALSE(ptp_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + EXPECT_EQ(res.trajectory_, nullptr); +} + +/** + * @brief test the ptp trajectory generator of joint space goal which is close + * enough to the start which does not need + * to plan the trajectory + */ +TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + ASSERT_TRUE(robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().size()) + << "No link exists in the planning group."; + + moveit_msgs::Constraints gc; + moveit_msgs::JointConstraint jc; + jc.joint_name = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().front(); + jc.position = 0.0; + gc.joint_constraints.push_back(jc); + req.goal_constraints.push_back(gc); + + // TODO lin and circ has different settings + ASSERT_TRUE(ptp_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + EXPECT_EQ(1u, res_msg.trajectory.joint_trajectory.points.size()); +} + +/** + * @brief test scaling factor + * with zero start velocity + */ +TEST_P(TrajectoryGeneratorPTPTest, testScalingFactor) +{ + // create ptp generator with different limits + JointLimit joint_limit; + JointLimitsContainer joint_limits; + + // set the joint limits + joint_limit.has_position_limits = true; + joint_limit.max_position = 2.967; + joint_limit.min_position = -2.967; + joint_limit.has_velocity_limits = true; + joint_limit.max_velocity = 2; + joint_limit.has_acceleration_limits = true; + joint_limit.max_acceleration = 1.5; + joint_limit.has_deceleration_limits = true; + joint_limit.max_deceleration = -3; + joint_limits.addLimit("prbt_joint_1", joint_limit); + joint_limit.max_position = 2.530; + joint_limit.min_position = -2.530; + joint_limits.addLimit("prbt_joint_2", joint_limit); + joint_limit.max_position = 2.356; + joint_limit.min_position = -2.356; + joint_limits.addLimit("prbt_joint_3", joint_limit); + joint_limit.max_position = 2.967; + joint_limit.min_position = -2.967; + joint_limits.addLimit("prbt_joint_4", joint_limit); + joint_limit.max_position = 2.967; + joint_limit.min_position = -2.967; + joint_limits.addLimit("prbt_joint_5", joint_limit); + joint_limit.max_position = 3.132; + joint_limit.min_position = -3.132; + joint_limits.addLimit("prbt_joint_6", joint_limit); + // add gripper limit such that generator does not complain about missing limit + joint_limits.addLimit("prbt_gripper_finger_left_joint", joint_limit); + + pilz_industrial_motion_planner::LimitsContainer planner_limits; + planner_limits.setJointLimits(joint_limits); + + // create the generator with new limits + ptp_.reset(new TrajectoryGeneratorPTP(robot_model_, planner_limits)); + + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + req.start_state.joint_state.position[2] = 0.1; + moveit_msgs::Constraints gc; + moveit_msgs::JointConstraint jc; + jc.joint_name = "prbt_joint_1"; + jc.position = 1.5; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_3"; + jc.position = 2.1; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_6"; + jc.position = 3.0; + gc.joint_constraints.push_back(jc); + req.goal_constraints.push_back(gc); + req.max_velocity_scaling_factor = 0.5; + req.max_acceleration_scaling_factor = 1.0 / 3.0; + + ASSERT_TRUE(ptp_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); + + // trajectory duration + EXPECT_NEAR(4.5, res.trajectory_->getWayPointDurationFromStart(res.trajectory_->getWayPointCount()), + joint_acceleration_tolerance_); + + // way point at 1s + int index; + index = testutils::getWayPointIndex(res.trajectory_, 1.0); + // joint_1 + EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(1.0 / 6.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], + joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + // other joints + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[4], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[4], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[4], joint_acceleration_tolerance_); + + // way point at 2s + index = testutils::getWayPointIndex(res.trajectory_, 2.0); + // joint_1 + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(2.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + // other joints + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_); + + // way point at 3s + index = testutils::getWayPointIndex(res.trajectory_, 3.0); + // joint_1 + EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(4.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(2.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + // other joints + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_); + + // way point at 4s + index = testutils::getWayPointIndex(res.trajectory_, 4.0); + // joint_1 + EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(5.75 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], + joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(2.875, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + + // way point at 4.5s + index = testutils::getWayPointIndex(res.trajectory_, 4.5); + // joint_1 + EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(2.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); +} + +/** + * @brief test the ptp trajectory generator of joint space goal + * with (almost) zero start velocity + */ +TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + req.start_state.joint_state.position[2] = 0.1; + + // Set velocity to all 1e-16 + req.start_state.joint_state.velocity = std::vector(req.start_state.joint_state.position.size(), 1e-16); + + moveit_msgs::Constraints gc; + moveit_msgs::JointConstraint jc; + jc.joint_name = "prbt_joint_1"; + jc.position = 1.5; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_3"; + jc.position = 2.1; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_6"; + jc.position = 3.0; + gc.joint_constraints.push_back(jc); + req.goal_constraints.push_back(gc); + + ASSERT_TRUE(ptp_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); + + // trajectory duration + EXPECT_NEAR(4.5, res.trajectory_->getWayPointDurationFromStart(res.trajectory_->getWayPointCount()), + joint_acceleration_tolerance_); + + // way point at 1s + int index; + index = testutils::getWayPointIndex(res.trajectory_, 1.0); + // joint_1 + EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(1.0 / 6.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], + joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + // other joints + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[4], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[4], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[4], joint_acceleration_tolerance_); + + // way point at 2s + index = testutils::getWayPointIndex(res.trajectory_, 2.0); + // joint_1 + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(2.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + // other joints + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_); + + // way point at 3s + index = testutils::getWayPointIndex(res.trajectory_, 3.0); + // joint_1 + EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(4.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(2.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + // other joints + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_); + + // way point at 4s + index = testutils::getWayPointIndex(res.trajectory_, 4.0); + // joint_1 + EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(5.75 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], + joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(2.875, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + + // way point at 4.5s + index = testutils::getWayPointIndex(res.trajectory_, 4.5); + // joint_1 + EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(2.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + + // Check that velocity at the end is all zero + EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().velocities.cbegin(), + res_msg.trajectory.joint_trajectory.points.back().velocities.cend(), + [this](double v) { return std::fabs(v) < this->joint_velocity_tolerance_; })); + + // Check that acceleration at the end is all zero + EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().accelerations.cbegin(), + res_msg.trajectory.joint_trajectory.points.back().accelerations.cend(), + [this](double v) { return std::fabs(v) < this->joint_acceleration_tolerance_; })); +} + +/** + * @brief test the ptp_ trajectory generator of joint space goal + * with zero start velocity + */ +TEST_P(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + req.start_state.joint_state.position[4] = 0.3; + req.start_state.joint_state.position[2] = 0.11; + + moveit_msgs::Constraints gc; + moveit_msgs::JointConstraint jc; + + jc.joint_name = "prbt_joint_1"; + jc.position = 1.5; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_2"; + jc.position = -1.5; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_3"; + jc.position = 2.11; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_4"; + jc.position = -2.0; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_6"; + jc.position = 3.0; + gc.joint_constraints.push_back(jc); + req.goal_constraints.push_back(gc); + + ASSERT_TRUE(ptp_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); + + // trajectory duration + EXPECT_NEAR(4.5, res.trajectory_->getWayPointDurationFromStart(res.trajectory_->getWayPointCount()), + joint_position_tolerance_); + + // way point at 0s + // joint_1 + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[0], joint_velocity_tolerance_); + // joint_2 + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[1], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[1], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(0.11, res_msg.trajectory.joint_trajectory.points[0].positions[2], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[2], joint_velocity_tolerance_); + // joint_4 + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[3], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[3], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[5], joint_velocity_tolerance_); + + // way point at 1s + int index; + index = testutils::getWayPointIndex(res.trajectory_, 1.0); + // joint_1 + EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_2 + EXPECT_NEAR(-0.125, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(-0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + EXPECT_NEAR(-0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(1.0 / 6.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], + joint_acceleration_tolerance_); + // joint_4 + EXPECT_NEAR(-1.0 / 6.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(-1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + EXPECT_NEAR(-1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], + joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + + // way point at 2s + index = testutils::getWayPointIndex(res.trajectory_, 2.0); + // joint_1 + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + // joint_2 + EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(2.0 / 3.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + // joint_4 + EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + + // way point at 3s + index = testutils::getWayPointIndex(res.trajectory_, 3.0); + // joint_1 + EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_2 + EXPECT_NEAR(-1, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(4.0 / 3.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], joint_acceleration_tolerance_); + // joint_4 + EXPECT_NEAR(-4.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(2.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + + // way point at 4s + index = testutils::getWayPointIndex(res.trajectory_, 4.0); + // joint_1 + EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_2 + EXPECT_NEAR(-2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(-0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(5.75 / 3.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], + joint_acceleration_tolerance_); + // joint_4 + EXPECT_NEAR(-5.75 / 3.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(-1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], + joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(2.875, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + + // way point at 4.5s + index = testutils::getWayPointIndex(res.trajectory_, 4.5); + // joint_1 + EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + // joint_2 + EXPECT_NEAR(-1.5, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(2.11, res_msg.trajectory.joint_trajectory.points[index].positions[2], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + // joint_4 + EXPECT_NEAR(-2.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + + // Check last point + EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + + // Check that velocity at the end is all zero + EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().velocities.cbegin(), + res_msg.trajectory.joint_trajectory.points.back().velocities.cend(), + [this](double v) { return std::fabs(v) < this->joint_velocity_tolerance_; })); + + // Check that acceleration at the end is all zero + EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().accelerations.cbegin(), + res_msg.trajectory.joint_trajectory.points.back().accelerations.cend(), + [this](double v) { return std::fabs(v) < this->joint_acceleration_tolerance_; })); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_trajectory_generator_ptp"); + ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.test new file mode 100644 index 0000000000..86dbff3599 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.test @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_velocity_profile_atrap.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_velocity_profile_atrap.cpp new file mode 100644 index 0000000000..4962ab5301 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_velocity_profile_atrap.cpp @@ -0,0 +1,631 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner/velocity_profile_atrap.h" + +// Modultest Level1 of Class VelocityProfileATrap +#define EPSILON 1.0e-10 + +TEST(ATrapTest, Test_SetProfile1) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // can reach the maximal velocity + vp.SetProfile(3, 35); + + EXPECT_NEAR(vp.Duration(), 11.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 4.0, EPSILON); + EXPECT_NEAR(vp.Vel(1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 7.0, EPSILON); + EXPECT_NEAR(vp.Vel(2), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(4.5), 17.0, EPSILON); + EXPECT_NEAR(vp.Vel(4.5), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(4.5), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(7), 27.0, EPSILON); + EXPECT_NEAR(vp.Vel(7), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(7), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(9), 33.0, EPSILON); + EXPECT_NEAR(vp.Vel(9), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(9), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(11), 35.0, EPSILON); + EXPECT_NEAR(vp.Vel(11), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(11), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(12), 35.0, EPSILON); + EXPECT_NEAR(vp.Vel(12), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(12), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_SetProfile2) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(6, 2, 1.5); + + // just arrive the maximal velocity + vp.SetProfile(5, 26); + + EXPECT_NEAR(vp.Duration(), 7.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1.5), 7.25, EPSILON); + EXPECT_NEAR(vp.Vel(1.5), 3.0, EPSILON); + EXPECT_NEAR(vp.Acc(1.5), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 14.0, EPSILON); + EXPECT_NEAR(vp.Vel(3), 6.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(5), 23.0, EPSILON); + EXPECT_NEAR(vp.Vel(5), 3.0, EPSILON); + EXPECT_NEAR(vp.Acc(5), -1.5, EPSILON); + + EXPECT_NEAR(vp.Pos(7), 26.0, EPSILON); + EXPECT_NEAR(vp.Vel(7), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(7), -1.5, EPSILON); + + EXPECT_NEAR(vp.Pos(8), 26.0, EPSILON); + EXPECT_NEAR(vp.Vel(8), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(8), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_SetProfile3) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(6, 2, 1); + + // cannot reach the maximal velocity + vp.SetProfile(5, 17); + + EXPECT_NEAR(vp.Duration(), 6.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 6.0, EPSILON); + EXPECT_NEAR(vp.Vel(1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 9.0, EPSILON); + EXPECT_NEAR(vp.Vel(2), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(4), 15.0, EPSILON); + EXPECT_NEAR(vp.Vel(4), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(4), -1, EPSILON); + + EXPECT_NEAR(vp.Pos(6), 17.0, EPSILON); + EXPECT_NEAR(vp.Vel(6), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(6), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(7), 17.0, EPSILON); + EXPECT_NEAR(vp.Vel(7), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(7), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_SetProfile4) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(6, 2, 1); + + // empty profile + vp.SetProfile(5, 5); + + EXPECT_NEAR(vp.Duration(), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); +} + +/** + * @brief Test Description + * + * Test Sequence: + * 1. Generate two profiles with same specifica + * 2. Set double EPSILON as duration of the first + * 3. Set the resulting duration as the duration of the second trajectory + * + * Expected Results: + * 1. - + * 2. - + * 3. Both profiles should be the same (checked with testpoints in the middle + * of each phase + */ +TEST(ATrapTest, Test_SetProfileToLowDuration) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp1 = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + pilz_industrial_motion_planner::VelocityProfileATrap vp2 = vp1; + + vp1.SetProfileDuration(3, 35, std::numeric_limits::epsilon()); + double fastest_duration = vp1.Duration(); + + vp2.SetProfileDuration(3, 35, fastest_duration); + + EXPECT_TRUE(vp1 == vp2) << "Not equal! Profile 1: \n" << vp1 << "\n Profile 2: " << vp2; +} + +/** + * @brief Define Profile with setProfileAllDurations with to low duration + * + * Test Sequence: + * 1. Define a profile with SetProfile(double, double), this will yield the + * fastest duration + * 2. Try to define a profile with setProfileAllDurations with a faster + * combination of durations + * + * Expected Results: + * 1. + * 2. Both trajectories should be equal + */ +TEST(ATrapTest, Test_setProfileAllDurationsToLowDuration) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp1 = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + pilz_industrial_motion_planner::VelocityProfileATrap vp2 = vp1; + + vp1.SetProfile(3, 35); + double fastest_duration = vp1.Duration(); + + // Trigger Duration()>(3*fastest_duration/4) + vp2.setProfileAllDurations(3, 35, fastest_duration / 4, fastest_duration / 4, fastest_duration / 4); + + EXPECT_TRUE(vp1 == vp2) << "Not equal! Profile 1: \n" << vp1 << "\n Profile 2: " << vp2; +} + +/** + * @brief Define Profile with setProfileStartVelocity with zero velocity + * + * Test Sequence: + * 1. Define a profile with SetProfile(double, double) + * 2. Try to define a profile with setProfileStartVelocity with zero velocity + * + * Expected Results: + * 1. + * 2. Both trajectories should be equal + */ +TEST(ATrapTest, Test_SetProfileZeroStartVelocity) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp1 = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + pilz_industrial_motion_planner::VelocityProfileATrap vp2 = vp1; + + vp1.SetProfile(1, 2); + + vp2.setProfileStartVelocity(1, 2, 0); // <-- Set zero velocity + EXPECT_TRUE(vp1 == vp2) << "Not equal! Profile 1: \n" << vp1 << "\n Profile 2: " << vp2; +} + +TEST(ATrapTest, Test_SetProfileDuration) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // set the duration as twice as the fastest profile + vp.SetProfileDuration(3, 35, 22.0); + + EXPECT_NEAR(vp.Duration(), 22.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 4.0, EPSILON); + EXPECT_NEAR(vp.Vel(2), 1.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), 0.5, EPSILON); + + EXPECT_NEAR(vp.Pos(4), 7.0, EPSILON); + EXPECT_NEAR(vp.Vel(4), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(4), 0.5, EPSILON); + + EXPECT_NEAR(vp.Pos(9), 17.0, EPSILON); + EXPECT_NEAR(vp.Vel(9), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(9), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(14), 27.0, EPSILON); + EXPECT_NEAR(vp.Vel(14), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(14), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(18), 33.0, EPSILON); + EXPECT_NEAR(vp.Vel(18), 1.0, EPSILON); + EXPECT_NEAR(vp.Acc(18), -0.25, EPSILON); + + EXPECT_NEAR(vp.Pos(22), 35.0, EPSILON); + EXPECT_NEAR(vp.Vel(22), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(22), -0.25, EPSILON); + + EXPECT_NEAR(vp.Pos(23), 35.0, EPSILON); + EXPECT_NEAR(vp.Vel(23), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(23), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileAllDurations1) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // set durations + EXPECT_TRUE(vp.setProfileAllDurations(3, 35, 3.0, 4.0, 5.0)); + + EXPECT_NEAR(vp.Duration(), 12.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 3.0 + 8.0 / 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(2), 8.0 / 3.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), 4.0 / 3.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 9.0, EPSILON); + EXPECT_NEAR(vp.Vel(3), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), 4.0 / 3.0, EPSILON); + + EXPECT_NEAR(vp.Pos(5), 17.0, EPSILON); + EXPECT_NEAR(vp.Vel(5), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(5), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(7), 25.0, EPSILON); + EXPECT_NEAR(vp.Vel(7), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(7), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(9), 31.4, EPSILON); + EXPECT_NEAR(vp.Vel(9), 2.4, EPSILON); + EXPECT_NEAR(vp.Acc(9), -0.8, EPSILON); + + EXPECT_NEAR(vp.Pos(12), 35.0, EPSILON); + EXPECT_NEAR(vp.Vel(12), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(12), -0.8, EPSILON); + + EXPECT_NEAR(vp.Pos(13), 35.0, EPSILON); + EXPECT_NEAR(vp.Vel(13), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(13), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileAllDurations2) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // invalid maximal velocity + EXPECT_FALSE(vp.setProfileAllDurations(3, 35, 3.0, 3.0, 5.0)); + // invalid acceleration + EXPECT_FALSE(vp.setProfileAllDurations(3, 35, 1.0, 4.0, 7.0)); + // invalid deceleration + EXPECT_FALSE(vp.setProfileAllDurations(3, 35, 7.0, 4.0, 1.0)); +} + +TEST(ATrapTest, Test_setProfileStartVelocity1) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // invalide cases + EXPECT_FALSE(vp.setProfileStartVelocity(3.0, 5.0, -1.0)); + + // only deceleration + vp.setProfileStartVelocity(3.0, 5.0, 2.0); + + EXPECT_NEAR(vp.Duration(), 2.0, EPSILON); + EXPECT_NEAR(vp.firstPhaseDuration(), 2.0, EPSILON); + EXPECT_NEAR(vp.secondPhaseDuration(), 0.0, EPSILON); + EXPECT_NEAR(vp.thirdPhaseDuration(), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 4.5, EPSILON); + EXPECT_NEAR(vp.Vel(1), 1.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(2), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(3), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileStartVelocity2) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // deceleration, acceleration and deceleration + vp.setProfileStartVelocity(3.0, 4.0, 2.0); + EXPECT_NEAR(vp.Duration(), 2.0 + 3 * sqrt(1.0 / 3.0), EPSILON); + EXPECT_NEAR(vp.firstPhaseDuration(), 2.0, EPSILON); + EXPECT_NEAR(vp.secondPhaseDuration(), sqrt(1.0 / 3.0), EPSILON); + EXPECT_NEAR(vp.thirdPhaseDuration(), 2 * sqrt(1.0 / 3.0), EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 4.5, EPSILON); + EXPECT_NEAR(vp.Vel(1), 1.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(2), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2.1), 4.99, EPSILON); + EXPECT_NEAR(vp.Vel(2.1), -0.2, EPSILON); + EXPECT_NEAR(vp.Acc(2.1), -2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2 + sqrt(1.0 / 3.0)), 5.0 - 1.0 / 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(2 + sqrt(1.0 / 3.0)), -2 * sqrt(1.0 / 3.0), EPSILON); + EXPECT_NEAR(vp.Acc(2 + sqrt(1.0 / 3.0)), -2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2 + 3 * sqrt(1.0 / 3.0) - 0.2), 4.02, EPSILON); + EXPECT_NEAR(vp.Vel(2 + 3 * sqrt(1.0 / 3.0) - 0.2), -0.2, EPSILON); + EXPECT_NEAR(vp.Acc(2 + 3 * sqrt(1.0 / 3.0) - 0.2), 1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2 + 3 * sqrt(1.0 / 3.0)), 4.0, EPSILON); + EXPECT_NEAR(vp.Vel(2 + 3 * sqrt(1.0 / 3.0)), 0, EPSILON); + EXPECT_NEAR(vp.Acc(2 + 3 * sqrt(1.0 / 3.0)), 1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(5), 4.0, EPSILON); + EXPECT_NEAR(vp.Vel(5), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(5), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileStartVelocity3) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // acceleration, deceleration + vp.setProfileStartVelocity(3, 14, 2); + EXPECT_NEAR(vp.Duration(), 5.0, EPSILON); + EXPECT_NEAR(vp.firstPhaseDuration(), 1.0, EPSILON); + EXPECT_NEAR(vp.secondPhaseDuration(), 0.0, EPSILON); + EXPECT_NEAR(vp.thirdPhaseDuration(), 4.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 6.0, EPSILON); + EXPECT_NEAR(vp.Vel(1), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 9.5, EPSILON); + EXPECT_NEAR(vp.Vel(2), 3.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 12.0, EPSILON); + EXPECT_NEAR(vp.Vel(3), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(5), 14.0, EPSILON); + EXPECT_NEAR(vp.Vel(5), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(5), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(6), 14, EPSILON); + EXPECT_NEAR(vp.Vel(6), 0, EPSILON); + EXPECT_NEAR(vp.Acc(6), 0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileStartVelocity4) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // acceleration, constant, deceleration + vp.setProfileStartVelocity(3, 14, 2); + EXPECT_NEAR(vp.Duration(), 5.0, EPSILON); + EXPECT_NEAR(vp.firstPhaseDuration(), 1.0, EPSILON); + EXPECT_NEAR(vp.secondPhaseDuration(), 0.0, EPSILON); + EXPECT_NEAR(vp.thirdPhaseDuration(), 4.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 6.0, EPSILON); + EXPECT_NEAR(vp.Vel(1), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 9.5, EPSILON); + EXPECT_NEAR(vp.Vel(2), 3.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 12.0, EPSILON); + EXPECT_NEAR(vp.Vel(3), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(5), 14.0, EPSILON); + EXPECT_NEAR(vp.Vel(5), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(5), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(6), 14, EPSILON); + EXPECT_NEAR(vp.Vel(6), 0, EPSILON); + EXPECT_NEAR(vp.Acc(6), 0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileStartVelocity5) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // acceleration, constant, deceleration + vp.setProfileStartVelocity(3, 18, 2); + EXPECT_NEAR(vp.Duration(), 6.0, EPSILON); + EXPECT_NEAR(vp.firstPhaseDuration(), 1.0, EPSILON); + EXPECT_NEAR(vp.secondPhaseDuration(), 1.0, EPSILON); + EXPECT_NEAR(vp.thirdPhaseDuration(), 4.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 6.0, EPSILON); + EXPECT_NEAR(vp.Vel(1), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 10, EPSILON); + EXPECT_NEAR(vp.Vel(2), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 13.5, EPSILON); + EXPECT_NEAR(vp.Vel(3), 3.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(4), 16.0, EPSILON); + EXPECT_NEAR(vp.Vel(4), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(4), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(6), 18.0, EPSILON); + EXPECT_NEAR(vp.Vel(6), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(6), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(7), 18, EPSILON); + EXPECT_NEAR(vp.Vel(7), 0, EPSILON); + EXPECT_NEAR(vp.Acc(7), 0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileStartVelocity6) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // acceleration, constant, deceleration + vp.setProfileStartVelocity(3, 15, 4); + EXPECT_NEAR(vp.Duration(), 5.0, EPSILON); + EXPECT_NEAR(vp.firstPhaseDuration(), 0.0, EPSILON); + EXPECT_NEAR(vp.secondPhaseDuration(), 1.0, EPSILON); + EXPECT_NEAR(vp.thirdPhaseDuration(), 4.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 7.0, EPSILON); + EXPECT_NEAR(vp.Vel(1), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 13, EPSILON); + EXPECT_NEAR(vp.Vel(3), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), -1.0, EPSILON); +} + +/** + * @brief Check that the clone function returns a equal profile + * + * Note: Definitions other than setProfileAllDurations could fail due to numeric + * noise + */ +TEST(ATrapTest, Test_Clone) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 1, 1); + vp.setProfileAllDurations(0, 10, 10, 10, 10); + pilz_industrial_motion_planner::VelocityProfileATrap* vp_clone = + static_cast(vp.Clone()); + EXPECT_EQ(vp, *vp_clone); + delete vp_clone; +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst new file mode 100644 index 0000000000..241af55e15 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst @@ -0,0 +1,84 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pilz_industrial_motion_planner_testutils +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.4.10 (2019-12-04) +------------------- + +0.4.9 (2019-11-28) +------------------ + +0.4.8 (2019-11-22) +------------------ + +0.4.7 (2019-09-10) +------------------ +* Fix clang-tidy issues +* integrate clang-tidy via CMake flag +* Contributors: Pilz GmbH and Co. KG + +0.4.6 (2019-09-04) +------------------ + +0.4.5 (2019-09-03) +------------------ + +0.4.4 (2019-06-19) +------------------ + +0.4.3 (2019-04-08) +------------------ +* update package dependencies +* Add missing intialization +* Add getter for CircJointInterimCart in XMLTestdataLoader + +0.4.2 (2019-03-13) +------------------ + +0.4.1 (2019-02-27) +------------------ + +0.3.6 (2019-02-26) +------------------ + +0.3.5 (2019-02-06) +------------------ + +0.3.4 (2019-02-05) +------------------ +* Add high level abstraction data classes to represent configuration of robot +* Add high level abstraction data classes to represent different command types +* Add functions to TestdataLoader returning the high level abstraction classes + +0.4.0 (2018-12-18) +------------------ +* Use Eigen::Isometry3d to keep up with the recent changes in moveit +* Contributors: Chris Lalancette + +0.3.1 (2018-12-17) +------------------ +* Add RobotMotionObserver in testutils +* Contributors: Pilz GmbH and Co. KG + +0.3.0 (2018-11-28) +------------------ +* rename get_current_joint_values -> get_current_joint_states + +0.2.2 (2018-09-26) +------------------ +* fix missing dependency for melodic +* Contributors: Pilz GmbH and Co. KG + +0.2.1 (2018-09-25) +------------------ + +0.1.1 (2018-09-25) +------------------ + +0.2.0 (2018-09-14) +------------------ + +0.1.0 (2018-09-14) +------------------ +* xml test data loader. +* Contributors: Pilz GmbH and Co. KG diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt new file mode 100644 index 0000000000..fc692408c9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 2.8.3) +project(pilz_industrial_motion_planner_testutils) + +## Add support for C++11, supported in ROS Kinetic and newer +add_definitions(-std=c++11) +add_definitions(-Wall) +add_definitions(-Wextra) +add_definitions(-Wno-unused-parameter) +add_definitions(-Werror) + +find_package(catkin REQUIRED COMPONENTS + eigen_conversions + moveit_core + moveit_msgs +) + +find_package(Boost REQUIRED COMPONENTS) + +################ +## Clang tidy ## +################ +if(CATKIN_ENABLE_CLANG_TIDY) + find_program( + CLANG_TIDY_EXE + NAMES "clang-tidy" + DOC "Path to clang-tidy executable" + ) + if(NOT CLANG_TIDY_EXE) + message(FATAL_ERROR "clang-tidy not found.") + else() + message(STATUS "clang-tidy found: ${CLANG_TIDY_EXE}") + set(CMAKE_CXX_CLANG_TIDY "${CLANG_TIDY_EXE}") + endif() +endif() + +################################### +## catkin specific configuration ## +################################### +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS moveit_core moveit_msgs + DEPENDS Boost +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/cartesianconfiguration.cpp + src/jointconfiguration.cpp + src/robotconfiguration.cpp + src/sequence.cpp + src/xml_testdata_loader.cpp +) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS}) + +############# +## Install ## +############# + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/README.md b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/README.md new file mode 100644 index 0000000000..d24acad427 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/README.md @@ -0,0 +1,34 @@ +# Test data provider/loader + +## General information +- Use as little as possible test points (Reason: Reduces maintenance overhead). +- Test points should be defined following the concept shown below. +![TestDataConcept](../../pilz_trajectory_generation/test/test_robots/concept_testdata.png) +- Test points can be defined in joint space or Cartesian space. However, one +test point should not be defined in both spaces (data redundancy) +- If a test point is defined in Cartesian space, then also state the +corresponding seed. +- Store preferably only valid test points and test commands. You can use the +valid test points and test commands to create invalid test points or commands if +needed (Reason: Reduces maintenance overhead). + +## Diagrams/ Data types +- The following diagrams show the main classes which can be loaded from the +test data provider/loader, and the relationship between them. + +### Robot configurations +![RobotConfigurations](diagrams/diag_class_robot_configurations.png) + +### Command types +![Commands](diagrams/diag_class_commands.png) + +### Circ auxiliary types +![AuxiliaryTypes](diagrams/diag_class_circ_auxiliary.png) + +## Usage +The usage of the TestDataLoader is as shown below. +![RobotConfigurations](diagrams/diag_seq_testdataloader_usage.png) + +The idea is that the TestdataLoader returns high level data abstraction classes +which can then directly be used to generate/build the ROS messages needed +for testing. diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.png b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.png new file mode 100644 index 0000000000000000000000000000000000000000..a8e633c8a2cbb3e6e2b75e34f945c97602804526 GIT binary patch literal 24644 zcmb5W1z43^^FE9sqEceep@Ik^Aqa>FsDuhC0@9_#rn_4OK~X?yrAt6cxF)l|v%xs$J@5PbKCW|d5cYodidi%F+;h)*_3*)Mf@2iNu&}TQ?%olW!ooUafrWKo z>BvF&P0NqzFf6Rccz1UDhhXtj5D_e`bdAz`D;yGK`b-$@>vP=4@`m}4wH zt;jFv{TGodgjVXDRw?Z<(&n7x#bgE*x<{(oh7+!WP#Z>a{yo#jP6p%um9Auo{h5 z*$*K<+{i@+h*f$T`F=bF`HtnvjC?<%d;t01ATlMaBXY?1zrF0K2@dkBy%+!OWq*6- z-(L3TGjozxINk*Y#`WKuNhi<1!8|9v^t8*~DBu~G$G|%X@PCi}A$P)Mrl=kb zFB2ThB;DMYVUtxF)`NQkI=*KAwUZ#0}ac zr5vNCC=m|=ZlfMDN=iPvC7nFS$>f~O0ZCcefRK=>;bBgvt+kMlkoNZW;N1-##z5kc zI-dSF9v(hL<>lp7VWHS>vVVN9@?GtBF3}X6j$Bz;`TY6wYf?5nrD_^Y!E|JlEK1p9 ztI`Sz@BRGf>wMkYQ&hTJTIiUy>Lyc5`kI?>tK}6H`m`J& z{PrvJ!xhU)*L=cUGNrjqE@^9OQt_Gz+mCInFPfT~cBvfo@IlAWon*LYvx3%^2)TL_ zM&LnCG~mS$LTsahMK{=ck2(dmFwXf=fR?9*U`MK&b)L3JuFq(f0rz%a>!8@Z&}&)g z=~5)cTMIR>mRl6)8`gy*h*IW>`BV}>A&HTvMybtIFX`i zGl-L6Qa-Ev42}%g`B<5f*v*^IhMto!F){I3O*PKQK6<1>i)TTnOfEpeSOd!v78X`n zSqWBKWlEs-&TL0MVkM3RotvB6nIvs!Xn6koc^@>xy`ax_%X%5!I0xaOa7u2__DM}y zZ7z+sn46nZQc$F*7U^Z6SEkU>Djr3Jh1buWi^|THEzohbx{1eA>K~IZHa6xhxRV8Lr1JCg!6Ju-hl`4eun!)DMaj_bV4ZFn8d9&~R#jDX z6HhB3PBH4s|6D+St|GScwTH*|mqpp1lar_W>}R)Ex{X6~TDur{lVf6pcjH}CmX<7> zot*^*1r^>poM)!B8#~JzUoM`8z4wV<8g0aR9o9b6=d1E0e)?p$P<6r9{?HM*LTYJs z!jzTZ?VNLLtfi-)5)Y-Q#hkrxVf3$8CqqL+!<{yC)zybKmM7b~dAPXlf4DLZre(b| z{SHs@sR`G0^z_Qs)|RX5AzIq-FJHdAd4suYRm?CmwoKz~Y1%u0Pv z7&+KXafToMAucX%|Kq*(;fu-yt&F|PPcmszmJ$}guLy3U8|WnnTc53sx|-lT?yRY) zF<(Df<|f#IKY;QouwIMU?s;Q-u(Zu2!>IRKIgXS>=(Xie4R5e|+{1@iRSV@epMS)E z@Z`x8@T~6c6?M+rLhepI7nzxTeS91o9Msj-!5>ZLq8?~yL{nY9bg8zx+rHY5sXN<{ z6F(Pyle*i_-~arD3pD~2-n8MqzAar{6AKQZg3eB*u160uIsS?JJ5kPj$W`3yrvc|WdqAMZ0kF`2ci$Cf(aH-VT z0@5ZdeR8Mx20h>y6$T%0O}u%3yT$3%t5=(=v$GQu8#|k`a>gC4_O`2O;8bbNv z$WmNziK<%=2PS=SEzf+E4O}l^VR%x;0 zW>rReoiu*RHPWFsHj`vSyX~g<2&QZZ<^3(Gxw%(h*1Qx znC|pEry8T(U30e|yvW7XHb?iSD;nh+(@`va@}U>gx0~_D zi9}mmW8>M*PI+E2ZEfvTd(Nq9gV6~|3Lc}79Z7`%t_jDLYe!JS2C;k-k{nn1w8WQ! z?_yI>P>`@H=UR+Qt(MA|n0&57wRn54#G~m=(bjqgtBJ7&dhyRF6X;D46@nYOJHNf$ zp|-lT)H!V>;yF_&dhnG1_gGs8c=!^hDxdQwZh;dcuxIBRt1I4$d7P?}t2_9dq>Eyi zF`>*KEzsujc*TObcYKnOH|HR_|ot<}AR;(%(Zqqu(#5aL6O;IWMsK%-B?G5;~<)(%ZUQVki`7(hz!U@$}sj0d~ zRC#?{w4aYpx<&=zw2_F&iye(;MfJ?vXO_C?nO&l<#xLrcp-+tR-#OGQpu~s=@vuuM z+<7Ojn-=fUy%VLUM~-04867^J;zm}`D*z#=r>Dow%`H(bwaXwI;-d~Jxx|A9td}l9 zc)xD9GzM<%^oaevd-v|&_uOgGv0t4bAS7gEVS!jNJvR2X0FUI-+hsUNn{#DETAr@1 z1eDzOg1PHHOJum@lct=q=gd?{Ghb%(4-C95EZo}Ou19LnsMc6IB2afYvV%c^oJ!<& zh=gxVK<<`&gw_0;q@*PH;Ex{}Y}%lP>C&d!dLlT-Sc*o>r}BKy@TbcUm&BSdI0Qdn_~&N%eVUOYLEBJ!@J9cEv3 zKVbj|1_nuifq{#$ohmae2FL9Tef+;lTKu(sCC}j1juc7sZSR0PA06HA-#L!N3TioH z2{AG7ag)nZQ-Fkg97YzTrdTf=9rQ3eguSz?gA#O^Svh1gozEbI!Pft$?f%TZ|5$Byz=bcsO-JDB>^WP}bd-QXCl+ek-5fJ<317i6vU%q_*{{4v? zv1-|C(JKI2THSBFGRe!!t3A+n=K#Rvhu|e1W5qisG#p1Q3HEJ+4VCQfY}QNU*)31R z-1Vg@5w-@OjZ0&5qkV}Y#g#shc4y8Vmx#ts({gF7Nfp~4{rwS41fVD20^^?C!CXH3 zm8szhZ%$krIvfP*-@n28oR2yMo<0o_egWI^^y$-Vg>%oXc$k^R+vShS@ffS@RFsts zH#qNPL=mtCChx{?wVH5azg|& z#DVuETO(|4(KcpN^~{pqV0S$7oD^V^YgE9aHOZC%s?DUO-LA*&Q0Hm zUhygZ&gf1^{o%5P+a=BekVhRB&s9Hk0Dtqt)7AVZ65E_(9wstMN}FZ0J0`0$y&HIn zg)3e9yYsZWPp-NqH95^yPg#E}0h9_ppKr&FA!oJMI(%`K>S6#aLD5^5J;27h3AP6a zECK?|*Beq>`1u|M@~j2w#4K*X4~@b4&wrl{Tl5pFd-i%=)7a> zys7ly{Ne7o6Dd!hHMh22(QS@d9Btq{RTzs+COcR?&99?ovN}I+JNFe=&0$`o!njjO zFkqw6s(>E=-r2J^S7^z|g5JH8l#$`G=^MTz&@_airly7n6ekgG)x&SQpeZjepE}HY zWH*5tg1cy9eHP!XzszoW%eISN=jBDZ-d3+dzxsC;PmWTn2uEY!{N%z%+wCE zMMaeQPjQ|oSdJpxQSXkZs(nmyPMPRv>7M3o#5nYvC?bZKhpn( zZ=n8O_TW1Q*tahQpt*I+esDi`#;ySiL9BY*ezp&sWgywuJA*%nk&wD5a;T_XH79 zavQw#@(Kv$bGjm`-t9spFg7~Mqu+jM`hy%wV-2S9!qs(bSG^sLX421mO)4iV%W(eu zT^>NFfaelb1mr3CUw2Gp_dN(KJKyK9SQj79sOzAt9AY8NU#K@xg<4S@Ytc04<#4M) zakun@85rT?fZwsW2=oN-Gm>Tb||ePS6|-?5@O;` zi^2VeaByNje$=xjWmWE+B-{CZLGWFdO7~Q#b>BNKuMq?e>QpAvvoiIJ)`u!ReE3+$ zdzt97fd4zYvUm zcszyulz%`#bz)yljs6`8f5~XkUgzCy2q8nRCsMH9M40&d_((}gMy9f9*9Jw}MEMy3 zEO!3>?zU`-vTo;(g^?O5?~A1}Dk`B?IeB@!^z?tFO-)WRi$p2}*D1z#KDlw@Y0WGF zA5TJYao<$##thknO2+UtqSTszwe@vg*5$-;2xW+__ z8-0q6YSOpPfbobNnFkU?dJC-83aqBB?)e`GS3&Xlt%$SHtC`+9Du?BEFE_s;q@4c^ zCC>vj{p$22({^hq4+ws(viQ zehS~;GfJVGxup^3XL)6P+RhNkMa9M4wjVt-cq;QT>zl0dA?^~N6y(1!S*=b^Q*TTXA z(juqemN7hs&ZSiey1&)ku$WWCtlHt4*w7(OZVQo(aZG&kL;!#4NSVD%Jc zq}P;}b<-1aa&}6`TQnyp5z}&=kSr%v=K-pTl#~akCR@_pbgkDbeg}3Jrt6gYg+s4d ze)9H~5EgbFUUVwj$zkw)UsZbstBh9}P!{`)_ilb)V7e+ z@$2({oy=iVW~yJf5bm%rOuH*j;R9RN!xsvAXSNKPVQO%?sQasul9T;w7r)}tUZtU! zW9y&k%98= zn$k=<{%+~DFQ89${Ia%{>F!wW<)x)oV`XJ!DoV=9krAzJsi_YT<(Q;D+SuERpWpaK zX*|ZmNhQARy!#7zr!%jUn%0N#ZuBczpl_8ETF>^e>=tiqY(QFpLZS3XH)pc$qI%Qh zXE9#p&RrI3Ut3Dz?Wx#ax-oE&YS2587Fg0 zhrd-;-q~k%4khHs(Mn!M*NP^j`qtTM;@dwgH0cR8Kq05`ZK5zlxDd`aLwIR!-UL8I z@CD#*)#NNZKq!FX2OWaD>kVG1$!2DV&ymk9J)REK#Wc~)^ttZ-oq)?}V{?OLKCj8# z2USkasq(1btzS>w-AD!6rpykEtgf`J?%B>l@)X+Bt%qEhd#If^TEfrK(S?@S za%I}s*z^F_4dyZa;kq{sK+F5NE{4H_l;Vu?{0YG1>RSi?z|(v29*g}dP@nijTlxn9 zWIZi`3IW0$97DlfNCv9gU* zDO7lJDar}IDKvKFDkJItVB1Ea2JrnsfWJ^7aFK%B;}QhSu^F?)o2=$LR%DVHuYbUugn~9Y00m!@uyE*q zs$od5?(FPjsjD}`raPUbx=_oNfU&`Xykl?e-;ArNN zF}evDq0`SZGuI5+_J4jNWMPpxWe%jEI}vR_=E{Hz(Z>3E-3P8oaP%Igki5*^x^-*G z`P}5xlpj}mpJ`5dItLTe-3{=t8CKI>VPP~vt1PMl-F9>;gzsz?>v*E0qGIMDw{9k$ zmPd6nzlleeQ!*hecxju;5joZzCN{P}MH9e)KsT5%*YH`-Sdgd0OqpN0z`!umU!pd# zj3lsapJbS2li0iSlJ4HQ^R_oTCkLokEp2Um(^5(20)rR@-&Q_9#l`;$0uwfely^Q* zcbFUrtb1DtHljwRXa*3FX(l;D!1m5U6(q_dV8r#F>V@Xa#pF>AaJa^O|kl1KHF979J6&Aa#y{y*h`N zmYSMc0cfW)-8ndwRg(S@u-%C1vZE=hbDfv|Ro5C6V7t3^my@Od+bu&0 zA?8#?I{t^jl)uQ9bBiDtAiPOC6hj~jb#4mqrs9qAZiHsfJ=Z4Cr}C9XpZ~qRU-=6Mt#X;^N|>Y>|2i z8cSqdzny-WF!lu`U=o7cOYO-@Y|6P2IzE4iOs6QQh195J3{wyerjs|LRm=Q03j1?S zg-qr6jo_HQV{inX&TEanrt3&$`Y21Zs3%mZ-l|kR?;flcqpNefvxnB?!WFWJxe9)= z0PJ(_RXUE6Wx~EYuBJ4beT$Cmj29>!~6uBhft^_9SBuhvk~~)y-V|W(Y~(WP5Uf?WD&X5>l4W{*@$ZO{c6- zGI?Ft!(_zF{*N)_=JO3!*VIgQW$ABl{ZXGG6CfF@?e8xsD(X$!YPwCUt8{HvS&~c4 z-`J|^Kn6o^ro+urPVM?P)O_t?zO@_bMO&5vuI8NkK&mWSDcity=+%Y8?8;2vrhS2! znfG?SgeSPsgo)2baL&MKh>eSGqk;j)zPT?QnP=8 zApLz1ge;~!n=^sC3HlAB!%EH~xbBGGb`CwdY@L$SBl9MhPWGBYe#xk_7ZOSXA3MVav3&*GANdrk7C#D=7eY$&u8-_{)iU z)3D^Yh0%rVuf{vuK9Xr#@Q&4e>_;+l%xzK8Ypms}j~>xq5Gg9! zcuA-I%N3N{_f}#MiTABr0nVG<{T!E2gtP*e@?M($!Ok^m^YZducn~r+HMO(QAJapC&()Yw=RJr8 zugNWfC_Iq8yWPJF#BVm*03XM#=-an%5fKpq0VHk*ks!@JN8_|Mh`9gQKvi`{_NkHY z-zyEAClcbwjv;aQ3s!fgZcE8_cS$I}JvSrcSg>*NlN;<{(N9`!kZ zAOCR&X7| z%}F4t@$uYZKYaKA#Y~7a6}=b2Y=-hi!!+bEg@5eF9VseO`V+g0#qsevP@jH-??zko z2v}C6P}&Yej*fFS4ibC5I0A_7&=3n{5-N~H^RY&?|ry(mpEKcUw^rO6W8rZ zg})-bFjG`Vq3t3We;7twADZnE&ZvWX-^Ku(o}y@qYcol^NMgmJC^6vfE?Vt_^*nT%O4)-NvvhnQMBu)7(c^!7;_m^k#X&*=cJS)wtDib0 zBq!e&7uTTLA7+GFeHg!zqGI*c$KMC6Q}zdh31`LkZz#oya?ewO6dKj5i*D+l9&pZ7+!KemBz zJ!4okv$>L^|HrBs$|@>$b#_)FlXyYBfn?q5FP*B)aXgIt@>?E8^l zHn@G_^l8hLDTV(WsYwo$0ni5A-wx2>mYCxprtmLo1;0Y3(Svfxqph{wJ0L@Kfi+St zyhO;l-4H|@WZTac1^(U|Ui%e;M~_bZ8p8iR2H?DduIQfl_3CXi&{P0B0>e7?e-G;c z9GUJk4O}=fFF+}Q?DSz(I2h6r{N-04|IbCkmxhFqRyJ9&{P!{V-uvrJz z@9g-keWh0)3l9pO`o9cJ7sdtuX;868{WP#IN|`!MYC8+0Ftfyg0jVj>{vHSn2>2G; z2^t&Zto%0le_6DhRNa=3)VyYzZ#%xSlC!QcSfdRp4MlU+vpKsDz?<=DA&&(;8uj-< zkCrRfciS!~U+#WCjZDGK7d&Hdov7qO#iybSB1(ZO0P z&x1mghzuQ<**rGLf|_jG6x_VA6oDg*6oL>%%(1(nkcQWl__LA`Jg8=I4Z1K@p6 zpVGa1wUI*Z8;`JiHD(^jdQt#3A1`MExdw=_2qVpWy}$e#lmFROiJgBHWk8xZyFz%| zlfn`*z#TtF>(TIgy{54{0=5g^8=O`+6AE!Z&6jNocDYw)YX6*lf_2|bH5syr0n5pD z7RAgvY1^Ptf`D=xK_bRQ%K`0m!0CVgt_iXeDEEwh|DKth-M7`TOKi=125Qx)N00ob zSXfyftV~^3?xJ8?n}M_{!7a;uuP&Ims}{(jMU+m9M&<|wf~C!bq@;UL0{T%Uq~DP; z4JFmdSH9SugS0#fRS3aI0Oz~JD-eyNp#P9;bhg@yluw12>6Y~ZU4&@pD+P#Ng# zyb861=g$vLmZ|4-0~eKr$SQUhHWiK9TU&MbEi>=i-)Z7TGw}0oLmt;fIFbj74uWIH z6bo(A-Z-R+aJGb@>kV+h*Rk7}SALF}jZRISjTH~96+N>jmQZu754v1-nZwf3vTNhV zj~{Q}zE!QM!zj}F&n^4r_xPEy2neWdcD6y~*_&1{rN3En^V%IgE$oVPzgM@tsG4Z5 zIqyKNL|LsNtF;{|-}W%`07*$;*g_+fc|SWLHPzZExOGFys-{vRh?L{HfSH*Yz+|AwU{%R)Ei#iDE*rOxF26 z-Vz7;0^0-^E%^}IqGUA(il|@Xz#5#Ww-npH$t04e8@n~(oVN}j5iqtiiK^532*}wQ z)bp6Qmf*n~wkaj+KY~H}73eLNkj%<+nqMX}xnrGSCzg~S3h z(|?go#IdrmdB1!apPbxQ{3*q;yx-GIdZOm>ARnFbDefL;UW zxJ*p-c&h6_c4x`L6*cTr2lZUDzf^-e+S{#Wi&mXBr*rC+^bCEgvoMl5Kf6Da6EeMZ zN*%1`TP&p`pLM9@3RE*t_qDa-h9#K@0LE2){>-jx0*W&Q1%*`7xV}tXU$j=PN_L%A zKO(dvKmVFEj^URhJ|bt`I{&1{`jt<-^~n?il7c?>c>)th8le-*@I~ ztM;AMRaLyEL&DnHqjBL*pmfM}+O}I>fvTS`!+kh5*R|bE$_;R_4?Qqu1tuBsYVqo&N|@J27*_(v|^ z2|GLA{=A4qk62Y zthl+k>qGf+A|k#tMZ3DXBC6t_G@Z7V)_Y6YG~d07tm$D}mY!Y{EmMx*I?X^|w8@d9 zk{2Yx{DiiC@RS@qc1-O4{k9qK^>57&A;G#VQht7U8H&3;=xB0sa>RGefMP&^m9@Sp z8iW&ujv!gRbLXXk_Gv!wnxps&in)`6)kB)x!BwmmG6s*Ih^4AoFyrj;Pb$ak?9)No z6sRKp(&}0WlTh*JJ8kFJ2wbyaO^{gfF8B_UKM2+CFVi~-+W z9ntT;QiU?{=Vm&Q-1Op;m^Yl*dqYy+tHtPvf;|Hv`&7O!*yTjkB20}m)A-1U<#ZR< zs#PiZ_bm4KQvWB&LVzv9e-D$ke0z<;1sv^+jEsbY%Gwg;47t0*`0X>r(`I`Mc&=W} zg*+DWRuJCKu*!eFp(6I}ufJMbTgL>owHz;m+*HMLxu{Iekb%C(uqS^nuikBIYXfx_ zDJ|{n^fdMHxtB?A|%@dG{T2c(BTk zX&-+>KY9dtKcnpb=X2;|eh3+sOT7HohZR?TWPLF}^1`Q264TNyGcv{l5Q)UHd4b6` zdjfrZ&HEi!`IlRz@o~N|V?!W{f9QD0tw+K> z_wVkk;uHUTXu-cS2~Xu1k(Iqsj>@=e%5eTe)MqHs&|u@t9YfSnSn*p6fhFKiV%4!L zc*gr}N2$LZ+aHEWa%}7fjXoR&Ee%op{a>;t_mLgif9B(Xk&%(hN3c>KVsfF9{)pK@ z7K;5BMp$6)uPb9O*8Ycy@%&sKKcoG3VZ~DJkN?K_;r-|0^KE$$?*Dc#XGL-|m;`(w z%f6RulRb1$q1pd1vX?wMtV{U&AM-H(!2Slo$LtRxmh$H!@`&+du=KL-#8Vtl#n4DM z5E(W2X9U?lbz}Y{3VVQT!&q7B*qwJs(GmEhrs&&HeFtfJdET1u$bUjAL~qxk1J#wD2R!9XbcXz)3)Qr0YKKXX1;h4 z)etsK^|!*n2-17dl69e4GR$s*i8m_y%d5SG6Y7T2bH2%7X-W#Kf;<$MA!-X;gCLUf z)2G~%)Ed=+YHcr*v9!!W3oyGOodS&X#T zFW4QayX-W243j(Z7W9UNhljs?I{+HXpAxc>l|B%y6H;GmjVoQHF)u?oY@h=Vkb4w3 zMHx$fIQIn`lZwCi-;F?@1WTx|cODygMiE}nm-^KzYSymx5Mt&LYKn@A9?;(7OIm2bjLj>~Hy*DNQ}DroQs2?_D>l_CGhX4X~x zCFQ>#Cs>z22?-VBZND%(dwYrZ99qC@x;?qV7e7?E)I}Vqy>1nvlfh|3c6qsz!po>r zTWwR_lmTNya}rXy;!JF(^`PIUIaXX>Uq8#8K_bjyZSG0Ng#btdiwgdh2u`#@{au0X zSzJN_6CWQR)Fic*lS-6iWX|KP{;%Gn?t5V~o&FXV_D|e(i@AZXpiRu1h9YGI$6h8F zSmb*F)LTK2N|q?2*q7HTpRLUH_qVkzZ?E>JDrV`Gdz@BW+>`!~rmIbeibdEhx5=?V zHq_g@4cV6#3PllSWc6^_7BsCj&j zZHbUyutV7m$u*b>DAzVN#Hfw4%l!QOKw9HCa!unK{_RjjQ0FHbW7PTMS#zOW`T;xQ z0#fCl%KTeLK=a(C-2eyQacj-gkTY4Q*miR0*h0JpFCE=*?TNrxYWtm_@?R7oWpatf>V6zpkse?QTE!{f!soqxpFk;;0*;2d2H5Cd9RS&WILy)38%yJ( z;m)caHqeVRzdn(e%E8GA!elC1NnaPey2+Ut*(LF&O3;E;M*l7x4s2OuxO{4O9Yu%2 z*c_7Ey)LWXVy;Q;aILYI__mYT&uANm3YOlvRg%A6rYWRPnI!dC!2On7Xd zh6OrL8&rlKjU2BQ>rZ!__4CCG6xlWtdyOMKS}gLL(L^o>OfBD)j~!?cTM?K$@a8y$wpf$ zG+ePmv7;`ShtK)aXQVM8)CDX7>LWUcchB_3$(J;z6kuVfDQYi}l84F>%{7ZVUWvO5 zZx$;KW|iny1wbocJ1#(Yq`T={$eU)cFap}c(%wj>!ySX@v(6zJE_Kku*~lvX95uhPF0shP2MNg)o|HKT zXpU_hmjBWj7FmGl47-)xkwO`%(pc*GLCFK^&%YbP(ABgje>H{?1e3GsoUOCEo(cLP zP`N_!1T;{*ZxIdDvzjOG;CO=mva0fttZaVan`d?oAbX9qg^nwrxI(X*Mx9eG#~@uD zT5;(sW_u#6$RxiML&#Zx@^_>D0K1}kiuy8NP0p||q#U3R0F^+M5VZJ6+_}RJYJVe& z!!1Bq@@%MUF+jY9|DKvGvw?7|phMMuI?|n{Idy7c0!p~Ay}eggRtS$A34*#T2;(zv zLv%XdZ37ZH*76zAI>*I!t@QJ!cvBixpt^T^Dhm&oC?Gp#q3)^Qc~3v6bq4U?#_jVK zv2k&m@U^~P^<0=VF#7E98N;#VgFPPepe70C(DFzMJh$V?)A!Idak@K4oR6QMKg+$A zA`iMSpiZ>;Q(_gyYyR!CP=-t2)U-FJhga@(WQyWJkcMBNCjGG2hrqc%!8v7^pXsmp zaowR7aF9Q?jcIc8F$+0oIhJk{WxI!77<)I){EyDIs*TvH%fb>@_5k3Q0gg3ik+{sv zN2R|tOFFq%vdzy=%+99nzBwl{@vtfGWZfRJ1{jBgZ1{qZH^90p9#MEJ6dgcks40rV zcDuRB^1xUi7=wq|NnQez+7Df@6!R zTOA29ZcUdg{SHX5u7;`p6y^-3*Dd%V5qTRsbsr9(qoM1=ZYr$;yeKv{Hmpg~bhXKt z{f>V=8k#fAge$xUAMD5CCU(Afjo>}ViXL5?A2v5LQ&dt?A^ZUaGzdZwGtVRIJfP8Q z{716d4!qmnd~|EEfOU1;?6@*_m5E22OSC|IO^asxiHyAIzNMMT9xL98q*`qZVjdun zlbX0Ye?i9JR}@zfC^yOxZz}*&wPJUsK++LNRe-b980f3wb%K~UlHcx+7%XM8*Zu1S z0+G0IKI{4<>2zA%j7;UO67$gq$p&q~usuKx)vcVV=q+(}mXqs1TAlUUKAmQk)nVbG z>dLD@)dd{e`44?hrwh_ONKa2c)tw_Dn(zxZTvEQ9GG=M)X0DmN*P0}L`0a&kOp|ks zPU0$h@=TCthX0H_?fHxV#vj>~E#01Z(>>OC`dR{e&`u-w+CPJM0$ymi`WcyJ!JT;` zXg7l>C;JXMP>)SZGU;LTGz369dc}2gbgXu9C9Ls4BMyZ4@tTYV0p{&yL5Rf6FS7j4 z=0jw*c&MK@*jTT^t#1W^c~OEgctmL%Ceqs0rfVG>94vI}7QRH_TN9vdZqwKbFWi8B zxGP_femH1|ezzFgpvciR;iyP;Xqkz7(#v16IoIHox>3R9qTZnHddH5isIUgaLZ99w zP5JmBbmI#)QQ_LqYuI_&uyZ)j1yxr10uLIjq2KHnrsChF!Zea~`_KO6q(GdCmXX}{ zO`H9A{Yi5?wA=&`ks1IY(97{&G|+WvH!F(_Eih=GLc+DsAK|(XH^b#x_T{bk6sO13 zAjkkum|h(3O*eng`S)=7Qt3_n!l^|rg*Lt&rSW&_A_XqxWZIiva>M> za)E;0l=!C5jvy|j-|@V8vb6M88BHyrUQtp`^NU~-kXI&4diz{cem^bEe*`6w(;Qmt z%DmfieEM5YHpeN}QBK68J#Ku#Jf8#cT@K>A>(P^_x(`{li>N?ayemoXIGX0y zne(9Qvc27TXMJ>gU4GyVIj6_#*U6daSPcK{^PW>%Oieab)o_hW2I=UQB7l}P#R|cV zF*;~Y+@pxLDKb!#p}giOsOY*mexPnGGKuXB-aE|DwTV>JH_YR9FAF>aK4EbxOs)r6$Cn3Ca0)P1@5ydMFn*m-81I-kqtcv`v-TX zSk+l%6gm*>q3;fQmd-k6#UZL=zvQk}hZreLzzocJfN9*zFk@P|AIyE1U}C|nO)BW) z??~MmAzSL7nH?V=-ytq%ZE1OTb$EC%KN4XVMQZu-yY{YSgxwj4t_<16UXg?3k9??<7ny(bR6-`+X)64TyU-eYBKw`ys` zw|eVv|B%Ds{ZRUw9)Y-uq~v?`|HGcDY&r+_>6I^~luP`DBG|t=P{TNk|R#&!7(fK*odZky`Wf!?5QM*S!cku+L|S@$3%?K4zZ} z0@~;|AN1e(oG)`g&g_#(UwU)s2fT(YaghQzS=gY*O)P0b47Rwa!ajmohKOby_!V+2 zFRDCca_4)sS1pHM2bn!yK;8%B{LdeOYIG@!e_n-+0qHsNpb=bOUk8>(KtSNxvuD6D z%gD%B7wd2Lmp!%643Y_Gt@!zjJfRfjJV@Kq9o841Tn0>WaGk7*3O@*w&vs-zn>aFf z?B`2OE1<`jghf$vW$a(%ygJlOmoDKG5v|V6ne?=^gsEa~?)VNA#4jPO0w(MJo0u2h zMFZ)V-e>h2e${yu$6qRF)LM1M(U6O}FyZ#4V%Q{l5@ygg3&0fz=;*zMs=s%-C(}W- z2RfOgEvBJ%x?q9Hmv3sP!LyIo5W-5I$|!|Q!mny^p}xp?Kv zOJ-TP&fxs0JbD3omXLD$Z3u#GDm9grSIxe^v|8P}k|X{2@#AM|NN{gE!_<&^YA>dq zdYqa1vi=)&_jdd5*D3Dt;$v^sV;J(%wi|ALw42Y5UzL!MNT!31yor<-Zf7^W-xN}7XT6r`hQ%iR8 zfh;k~VSetuNO6~MV=6{Y4I0I_H_nR(04bL8`E%oE2y>sj6K7!cPw{2;MSrDw3}F{B z!_qctDX3lM61tX@INRG7XsiP#Cv}L3jV%?b>Ej)6n?rPNuDs*YzU~w0rv8-iHlGgV zDLK_>=e_@!hvPeoIg$U?i*oD-6m}pP{m=YpOi(@Nj|{8b%Zm`>v=s!QWKSfx^|1ZU z^RCZN%0MNBzX>$XHM|5q^>(sy!sOMLkhLPrmj8?(Yy^Fy*=)sLm-UP26dg6 zJ4lm1-g|#;lJ4BOKE(0mb`?9?gJ>L(-A*JnE@8}XFB(Iv@UM9cOcnB&;g=p>L0qC~ zLX-UGl%L4&gKC(!G~qoO#I>Dbh989t7lI7OSC%yTkc&zp)ZtgI%zN@JBxUdpd@Y)iX7F=LJqTv<){@Rp}ZUw$bz!+Gbpr3}fYc zr*hdC(U&|Nf6nASc-aql%D zLge&v*VEI3(!r8MN7&k=+HTipj8+U8(GwjBC{c$&tc{I{xk>^e$|u_apmA?Su5GCW z?J2J^6Q>9g#&Qetv}Chsp&S@F=RdG@t`js zbfK*hoWQ{JBE65$R%D*ryi1?6BZ=;uvzR{1TMZ|Sxn0|1%mh*+#`cG;f(`&VMQW2_yH==wex8=Dh^)2lu`VV zr!4#zM)&tZ)&bi28=o5&eopxn5^sx#Kph0JB;MkyUm3e*ZUheEu8{S|Ww*urn1pqY zckf3i68-Z>*lqYNtnHXY7?7yoK$Fkcjt*{Qv)eqlV*UpH>fDU=k?L^G_kVTlMmc90 z7=AQH3OVXE^Zz(p7Ef5Ywv@q!&2h?lTe$zDZM{7MjLo0*i(gqp&~?zGgML(@*ufUi zv9)zh9V#&;1*(t5-G*dbdXJo)_-sDc?fznTZGGwYLk(H0-F9eVKtKQ(MobL7VclLd zh_R{uhi->k{69NTBY?0M z>&0Vlx8Lkx77EQC(GQXn6IOXLa@1CbkP;VO)n^;&|vzgA&hWpk8 zoVIea-^*U}$ad@5pMF>353TA`Twf+BGWKHbNekLbwW$=CkxF<_Fc z>F>EAsyn_6)8*lfbmg`tAdMUf0~e2}*`~m~Dzo>gQ>auxWZ?G#NK_6kE}4?|Pvqc! zC$Evl#)l0`qMj5ZgM(+4s-OE0Ufa9(icO@Dsng^_zB?n3hUM0LWH?w}g%{j~) zu{z(8;N0{_aCZ9F z>mz~v_Otwuph&p}+zqi#S({7tYelYuh>Vf-miT*QU!^#^^u8|XT~u)~g!N_dfJ^U) znc;$^>%Fjc!xyhb5z=GZM-epmWDYObOFK?{TsDanV4B!pK7|lAoqtIHN4;_2q9Dc; z^S6K8;Ifi-gGbM(EUsKLuftC6*yPU(x|aK0O_2+_tXa_Ma6#8b1XT9nzJ)ukzLJVX zc3FdEk0<8mS64gSmJs;h3R)ioKImGra4+n2-f~Sf{<+WB=;>pA>YaFg@`3Q#`-CSJ zmMBTa?yErgbEo54+d+K@oh9#go|9M=z>bZ>wH5Y!b9Mq zn;Y?h(lL$WB!Zx@Toaw=;TnS?xE2GhS$XTE^{W>oC&&86jV~d*=FoBw%de=cdeT(1 z_xRUNo_zQW$CFc4XZi4RR#DQxa+O+@+By|70@b&jysVE&)lH7R;>o>?^GdSyueanX zoT!(kmpmVsYDCGTCCl@K8@GSI?)ac#efc5zkWC5eJ^T9c+5UCIb*F5*jfq5P=u zFnlakkdu{F9_HUDBQ33w(+wBdjJyygEo*yg`Svos16Gc@qk}+r_jJIp)L-Cam`i1# z?E8ij4l17(cO}UCKywvb_rv7sbsF#f*$yt9_VEniU31aP#AO9r5PUVMk%k7R?g7DOMN<+r1Xf~ zNw|2-1p1{Ov#&u-4d_#DF0O=Bxc$xvnsJf@KTxradmct|NVq2LGE^&11VC*|MM`RD z+0N4Yn^(A3uaWn**SWz7t;>;=1b`$kn0)UI`vPvE`4V$39Hs}MV<^Qw`@uy=G^xY2 zH~m8^POkz!U>vfDWz08J25)FRAl9upKR2_on$Gu}U$PwD?VX zS)2KcA!cUxdf`SfNV@==dK{dGfr9UMLapO!S_T4N-#Km}>de0ad5#BvH-J4A=-jKs z0Tsb(;d4#i_C%-uy^hWuyEi*zDc#coc1!U_T(#FcRa=^xB;gVc!}?&J)X2yu78b20ml0(z z)jcPr3+|?L0%{nf zI^&4`pbHK1Q$M&GBmC>*hhl>=iLzYW+@gZ2($eN2zsmS3&c}fw&wH~A-oB-}T^=;o zGqbZi`ZqVGCnxb<+**ZtE;P3QZpnN%neP>zj{Y3`T+n3T3-B@DDk|7i3&-YdELegp zDT=<0m;tRODys_@*1=_yDJk^`V|=Ic!wk)Pt`yZ+&;;E*{FVrE=Z8Cf9qC;Y+$UL| zqpvBw_ga?WpnRond?BsKx3k;LJSROI8qBe<*i^T$S%4czhZ{%0%K_jAGrOxoruJ~+ zj?6a9lYulvHc38G=KB!T?wjhxB<;tWV>>%P!=;F06>y8*i3j&D?!cvuGEgQ^7T8(S zcrP~v_jxb?WqcmE^k?@TQs$*ht}z7@KwX@9C(bkcy@Sl9?3OVufp7_XJ`N73b1U~& zoTuMf8}>FX+0Op{%r$xZ@ZtY-W8uUyd_p_6JPA|!+=m=)XtR`ntgOzXy( zh$4R}RlZ4+t5Un%P}l@x74YQ%N37ahhXB!ZOzB&M zr*enomS3G)5!~#^n+2p9-EFU)Vn@DWoQwz29b-<9c&wg8Lm3|GZLxum*@HfFsYs(d z6Xlq|r>!Iv2GIT93Oj!b7apo#n<1N>Gq^X2do3o*>!abjaG@rN;5kI(3QhZK(o*9L z_&?d=iuQhw17thpah>W*K+w0Gj-FsE1j18%-VvyRJMoYY-+(AFuCt z-|zk2_jh@IzvoFqFVM5=o9@eGg*@k&&Vy|R&<+Z-OUy>03QTs{a({7B@9;lQn=^9u zw-&ed+)UxG?o6MD%`4*bpL7J(Cf0IGbk_`&#H^f4fxV9^53L0uLhXQhWIRxSWi z(!X*PeT!6tC^a<^AtBBS&sN}Ilxd4{7kzb@!AMG|6$+bPf^!ng!_kK*Zpxr-*pU9* z8Axunhu=nRwX*H`Z1otBpv{boww)bd6kI_A>D!I~f3auHkvB+SdVX%cnwRGXt-h)? zQYzid!)RA)BC7>Q9PQx-!&`T{SBI9(xoXa7)Ed7Y!xGf8W2HK-($-U1Ftieb6=5t%K0Se3cIS5uCOsjHfIr4e|4d} zTaagdsi+8fB?8)^jr*7;ds18UYFw?FO>jw-rOPo&m-E6Spj&}YU7IjFNl0m>MGkthf8n*amw zLzrq6p3Bp;QL^mR*xx!B>eXqI&%^DjJik&&8IGRx^`9D7M2uu9XPq#IVcn}dL(3Y)|}e;w_nd=Z>I>#?Gz|)D|$0 zB@)TBW9tFdTMt}&b&tLe5;N|axD(U$zj zZ;qg(0r3ZZ(4N4>KvWUW0?`bnW+~k>H<<9V%^(CjIfYht;*#8_#Ip)o_EV`;lr!?A z$YNKoT9uHLR9_-!_lxUZrR)1%x4SwZFwn@@_!pL`=e6HzYscU+pl{^u?@#DGjYirk zmlvJk(zhw*SUV_W_u#jfh9P5$Q8bZoI?Cyp8|ic@p?Z{z-oN8VJro-1G1`S-fWcrG zO1@^IJo^|-1XMCmO@7U@shUp&0Nvb5ur(~uylrFS0Xmbb>)!OE#UA+!d~aGc?w`30 z=|GAZ?eCssm{6h`8w3=vN%_s#{w`H(GU?o;6qCg^Jf@plA>EtJUcasgUjcNQz#s$? zm8celoq@V*3B_zH2fKj#@b*VvUthTWxCTk1-^mRI=X0*58qSNL%qk2wVZ~EEBfqWu zS(M1D_QfD?=+M>$*36P|BQqlvQ5isf5IJBnFXz&+@h8n#pt3eIprdC;TCp4(^8 zzAUL=KL~t%&UWm5YpK`np)ziU&*_;@4WaJxd}W=x>Ks`J@3}-#N+IBO`7V^CmYvyR zskQ5VFT%cv8 zrL}c+(WumSf3_?StA?(s)paN;?kHe1XxTF<6bh2rmR|!O^yS8ybXrQ)?Ma-Eh`hZo zG6PVY10&f1=l}nf3OED$(RRiP$4!-N2Z{v3k7kb_uiHPtb_F<5+Gxlpa$|)fhZbs% zzuf2SCK4qn48fHq4auZeTFo7pE_usKdkO4Z4P~40+xT%v##LR=Xw?x(F_UpsxBK|` z(2dk-WXA9xdosC+`Sg0&O=Z2RzhY_Rq^%A*={_n0rfjl>;UpG4!Ol%m8ykOkg2^Rw q(+}M-yUZ6Qhevl2v)kJ-z87s|AO9ESlZw;; literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.uxf b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.uxf new file mode 100644 index 0000000000..a1f7b6b415 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.uxf @@ -0,0 +1,133 @@ + + 10 + + UMLClass + + 40 + 100 + 380 + 70 + + CartesianPathConstraintsBuilder +-- ++ to PathConstraints() : moveit_msgs::Constraints + + + + UMLClass + + 410 + 430 + 210 + 40 + + CartesianCenter + + + + UMLClass + + 460 + 90 + 470 + 80 + + template=ConfigType> +CircAuxiliary +{abstract} +-- ++ toPathConstraints(): moveit_msgs::Constraints + + + + UMLClass + + 470 + 280 + 230 + 50 + + template=ConfigType, Builder> +Center + + + + UMLClass + + 730 + 280 + 220 + 50 + + template=ConfigType, Builder> +Interim + + + + Relation + + 730 + 160 + 30 + 150 + + lt=<<. + 10.0;10.0;10.0;130.0 + + + Relation + + 470 + 160 + 40 + 150 + + lt=<<. + 20.0;10.0;17.0;130.0 + + + Relation + + 490 + 320 + 340 + 130 + + lt=<<. +<<bind>> +<ConfigType->CartesianConfiguration> +<Builder->CartesianPathConstraintsBuilder> + 10.0;10.0;10.0;110.0 + + + UMLClass + + 710 + 510 + 210 + 40 + + CartesianInterim + + + + Relation + + 800 + 320 + 340 + 210 + + lt=<<. + + + + + + +<<bind>> +<ConfigType->CartesianConfiguration> +<Builder->CartesianPathConstraintsBuilder> + 10.0;10.0;10.0;190.0 + + diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.png b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.png new file mode 100644 index 0000000000000000000000000000000000000000..74900cf3381f2af13a6d3064c368752c7be8c92f GIT binary patch literal 9020 zcmdUVbyU>f*X{_SvW4+dpc{wPLUAI!?bgMs;jpJ5+@$~qv2piUQrRmvX)=mr~`%WoE;#$^1RV+ZcJ7wKS~jH zDthuBKYgm$KN4eD$5)%3`J!k2=F8WSBRUajudF~{A%_%+FAVk7*!&* zNaJxhckwV#6AB$1Gw>|bbW!@3C6hJsHBC%R0&IiWGg1V!-~3dp83_V zN6!H&aeEv$>M9QYKXFiDyWF`hS)=^v5e~`y>WXtFYb~2v?x9xg&Vx-Pr&d%;Ld|Ms zrsu`^X8mq&VKm3@C;si9LsCRti=LIv59g{yM@KU$CH*Dlaaz{*h%K)YHYs(p)}$+D zVES~vH7HTQhMMbz7d#!YNn%Km%d~Zi z``#cXCGE;<4I))5D9|c?{vMxdq0N`sRxwGi)Ma;xg@vW2rpA6%T`Q^w>`c9*ZnKf? z5f<10lkGh}{(s)x!To1Kv_^6AA(y4Mp`e4W*^xrlb1Olg9 z*=D2uAGGO@);XoZ&b~exe(N}>cCz3x0iIJmI{)33??lQX+sk4X+OcQ^XiLWn00!EE zT9OM`+1NmBih>p`{5A3};}*XRgMffm>^DM%>pDCXaS_hBaCjV_m8PkwsgaRUR}4Ey zMXhlgeuL_1d&tqgN+B_?c|WbF=vk=e>5q|ot&tn}kB$~1mwFO|_90&-<1m)Ia@O|N ziwe>3eG#jm!6_dYeJzY03HbdedV1sK-sHS6BO)yHLJa;7v7McrGYzg{2UB*^q0|ka zipR^$f&v4N_t*2Qf@sDykdG28h9l8$;_q!DJwadQP2o|oB0WxE`q+;N=t$%6QV>H` zb(*hF54Q(0qy?--?=vtgb})T9UCl2Z)#-dAtW9(`y|&D8IG;OrDj_#?#2p>?_tz&;+`rbVkV>B(omDlR6&4`e z>7(oN^y?e}gy!Bq`ixALr3N3DTz z>7KBaKQUcMYP<1xAIn^mh@uKnu-1@heEZ9itngbfMU`m}xwd4#L)^!Uf^FmLYePAq z6OYrc0!WYK!&A|;@Jk};6XG%bq>*;NzxC4$7q#nJDT`V*+!jlAK{qI~py1ATd7vfJgO3Z9D=*Jil~x5kYqLtxrPO1l-eqW;85>`E z?l!Bv$J#{2Pt;%P1RI$Z0MYWcnO7Wr=5t)v34rr>b6iiR~(X0a$e z|DYq1$t3zR1SzC-D0q4}tJPRhGCynrAjBe_mP4k$$Z8EpfTC)>j}^4Z8S=rc0@TIn;~9q|65GK!!m+mV-**RT2jV zEhcjD4)s-o^*6X4fUFO=ueRJ0TN}=e%riQXVs#qXS?n5V5<2vlj56En*U=*y{=aa}B+G(?b90X{xx6Q{LzhTT?7TAR3h^1fX0B?#oh4bzhO z|DJ+1PQ#~hkRw~{Pi#+cHDO*(*zC&85%NCFhOY>~xNssaaY+)ec@8Mj`SDm&o`f!? ztcr|+f;F=7Y}4aF_~@1hKdFWkd6ZIazJ-Mak7+kE8(Uokj9J~cQGxHbZW9f)_(5g(8$lK&!5f zkfHy}N4aKNlbE#j=wIl^Cw@Cu3K9`Vun zIf*y$EoN%~CmFtddbE4DCGK;00?$WrJC#NW}|`=D3+yw!D&3*=C-W>a@p`1>V*t;J~J8L zMxG2T*by6_c{_m3uvnu55px!L7)QBC8rr?s>)~^C;>|5;j>nE;*yIAQ+%~3!6lES* zAsgLy!pZGd`o*IZ6UT}T`Z_xakE)qx&sVjY?gqdSt>M?vC;H6#Qbu%Y>kQ=&SK3MF zaCTNjEr*%Mp^}O{WZzOXD=#0b0T`-O&oDZ|mAX7CoS#%DMdcdUeEZfkEkF#4Mo(-J z4|8@59Zx55py~Hk z|I!(44kG1@kh8GJjq1;ezBkWFS1D-Z_z}&!Ubss~T0j8@O1P7)ko3?YFsZY55^DF3 z*W9t=&cu!uA#c7iAUI+dN6Ol40an!3e?Ml!qLJ#^D}=WHcqtiL`2uh6F86H?WZcB6 z0!+4zU_uK_{dARTfSlLRZ+zI-8I^eGtrW*4u0yo2xEOFtePaHDPK=h#Q5%u=&y{yV zyex0-S@Kq_P&T}tlTfY9TEeyH=Y53@4m|*jm}0#eDD3`!6+=1JO?*mcr29cuc6JCk zpPUCBYb(Y?uzh=a{#2^CZ(vZ6zh0Nl_X;a51Z^SK%CmcqR4jt%v9JN~Wz4cQ)ks=d z`~(qb7|9QtTf(Mm`Iwlbqo{I)TK?hL1T^r-MN}>ab{J z>r(T45%rI`dgcFCW@e;K2)mr%asR+X=@fhju03Q0;8ZcxU4w+xLnmjd9_hVy#Cn?;#df zMF0(V=Lw4(PuR{ulaP1%`ugV(2`_D?e`~Z20CKR9(HV&D1G9jlz^^Y8)v8zC}E&Y{! z%^&XcT&|HolKJ&^rwN_UX~Z3E^?>-xwN^^9<*d(2d!lERJ&)C#K?;xr;KsaFZ-vs-vaYy)}MC9H>!Ud>-ujNTNZ6Lhka=*~vj-@?O;MukvWG(3(}C{V!z*vC7NLjQ%41 zIv_Mwn4+^{=<(8HqM;58zcY}a9~}{fqQw}AGq42;0v!6CZ41*%8)Qm+Jd2fkq;mom zjfRz}7W#>CBHLRr^MQ0pS=j*66JsSHj7)4rMhc|bd(DdrAQo)bXzWJ*@MX5Nc8NBk zaO*c65e&b6j>T$J@_x|K?7WWQ1WwRsXf2NwD-s{75`4tKBJxLI+P~=T>Cw38Uc5RC z9iBS!Y_TQ1@K|71ycs7|@!A+IOoUZ3&x9DZwY{xmx|DXDiH7<2uE zkIuOtgY#m)YC>Hu!)8CAi2K7|Zq78=>%;@!VFs9D&VSx6u^vZUDj_DOA?=6_`-#mQ z;8flEKe#{tU^B?G_G^|xZ#E;W5l-u;$OA}ORh`J`Sy8VifE5`yg95HUR_hT%Xqj}*7pvVY_R&x z4-ij4Pg{Gu&S`Tw#dTw4AY-gZU)XiuT2)mQ%+?WpjVFRT zJpsBEm&^Z?&TLW8isPTFncihwK{kmcx1oBLw&q$UF7U4rkaag)&1t#A09PDrY^e}R zmI_{Y518z;Go*qS#%v+4bNcz^>;G^ROW7&5 z%2HO=oZ&HF8-Q09 z%W0s=z&adPS;ZVgX|$Tr{cUM{cO?#NFVz}RS&DN6xZ9$I{Z#cEtI>jRjqCdFJAU<% zi%ETcfhf-Ka*j5xC63ekX22Gdy zXD^-6uALMy5iL8H?lD08jNAt+-Svv|A+KYl>cufXZI?elEb-KFz{LE*z zm+jKJ!~AyhZ#8-t*`^Bty#=FLEk`NZ6EjA`v^_&6v*H0wi1b64KGYv}FFoJxG<@>p}H|dgvvw zemWp?oSn}LD0T?f+n=rVa~ojNfpU`)+3SCjqm&$>0p2poXnkmK%rcI;3)N=!^Vut8iALRBO>aaD1@k7RNo1*-AvnM_eG zI)Z{5SBRTLxLhGe!nSU?C+LhnW(IeEW7>6ng70l+zCgpz;UfJLf70h+tB|K&H8_9p zFnWvVfDjm5?njHUJlx#myyjc8zPIvdk{s7w?+ys~7se9_a3vPN)-DwjF;&*k;NZ$B z+`Yf8``P_`k-8WZZ-NVv%FCQGn5`{$4;__jE`xs;3A>I$0{p0pki(Fl451Al1`<;K%E01H@|*?|+0h|1ksm9}xDRT5{8X|LEulR6fxn zQ;Ku?4R-IC5!}uF`}#fSVN2)j7mv|dqc`V?X`h2H0w>$uCZnCxBlNAMrxIM zcM*q_BU2#%?XT5=KFJ`Gj&F!-Z_ZDTZc;lxj=n=o46LuvH6*N0m}UglrdN9#ZpL4#Pod0{6e(fj~_AFEBI$iPICz^vuuyr%7S8yT2G~Fynb~w)!BZvnPSC&UrgM>JVQoq}j>5 z(QhqB$aS#!XK~Z{R?97`;&9zoQs_O0x4KaOC+~36oLC$%EBUhT-h;MgFSaH$fP04F z2H+T9R(BlN++db`dZ6hn$Rscg+?J7L1<(3cLK#c7?d+A!|MmI-O;J{oGX?gnfu38d zwU2W`Sq6vDz|i^1Me-ic2Ij~s=GN9$piK87JNL*F*-^ylLJ^_%$sN7D*`F2{7kLAG zZ{F(d>m$|Bt+Y;~l9B$%rd8D6k*=7?-yDU~k)s?1l>p>zbXP2=+5r&glAG|n^vpk| zA#0kI;5vs!x{(e+SP~lE^i=MyeUHUprfieP*}@Mn{Y>PME*Up$QAGAhs(m$;hIz@Q z4hr-y#oWz8Q~fy%=#R%MVY;ZX>~sf(N6@+VG#;t>8);A8`z_3V@zvJWz83{^Tb3&` zepFl0PLGv)QO*r+M-~$A7W#?0?#s}#RvuLqg5;#ImTD8htfw>LZN?Fc@9-!ZPd4gW z-rowP7Jb+s#f&NOs>PBfd^3+rF}KWLxG;* zrvQ#mqx;Fi51=R?+ON=dmK!hnFt%57nbgJ*zOMi-!%#*6VO;hKrFd}>D+?PNeHyFk z19nUqr8%i63vKhzJE1SZzI6cu2YM(o?Axzi;Z1mGV~e`5@y|G^c8}v;YH{(H{XDnK zG5O<)Tyv_((Daf^M-%6dqgfHWsTY9gDIm>snEiZBo1u3nU4b<>H`l(!`PYaRD)sF0 zp2zgxcmLxrc0z52*NKGw)s==7q3uo@aoOrq72bazkZLnvLQUxGgm>QC#_h77%^B0> z&!~9DBn3LzY9vn*4}L#y1(7@B{*{0KR8KyjWuiNtC)W|&V1d(bdOhcp&72nWk%Srb zKuAbC+Rytw^0y7z)wrvvtu5O?EFb6rjUTPDHm1n79HmU)G;MP`K?%D)FW7A?4mZ;( zrMrVV1-fu-=mDJeAwBH4H=5b%2VRn)s?rB4V<1o3xu@-NcZYJ6bLgoV0s3a_M)TDa?dD&y&D7qHiUE*9c*x1ZR22)l2D!FZm&Cf|nd2@}Prt zQZE8bd@Fs0cF7CxiN{95`;oQBrq!Mo9;jwR zMSmqW;6bDynOE{EtjD=E5cVi&EqY!AmwASx$iz}`kt?{E$V5AfG9&Yb#pP++nKueP zG<}4u0vNh?Pw zx#NS);&Xl}f*JL8*t2zd=o{-N#CC*#6F~iRRzR$*Hl&u6lmPeupc>Y6@axPNU)+Wf z!1tX+AfpCc>023Vm-K)UxVzK?Of3Qp_5Cx+ox|-PptD!l^9u^BPxja2r_fy0PPni`&Z!84mahS$7K+i+qPxEW4o!NJLRt5kidZk7f7*U;_ zaY|D5j2o5`1F>a)MfC22=SA-KT}T!5Lgf)o(VQK-JWwgUJ~l9U6tC*ViYJ zQv%yrgj(3K`^?+U&K^y1!(%U5z^FG#h}ZErxBLtX`KL@b;@tLsq3=CvsWVL(L zj0waP3XgWbN~PV literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.uxf b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.uxf new file mode 100644 index 0000000000..1c57939fb2 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.uxf @@ -0,0 +1,175 @@ + + 10 + + UMLClass + + 530 + 370 + 560 + 150 + + template=StartType, GoalType +BaseCommand +{abstract} +-- ++ setStartConfiguration(...) ++ getStartConfiguration(...) : StartType + ++ setGoalConfiguration(...) ++ getGoalConfiguration(...) : GoalType + + + + + UMLClass + + 910 + 560 + 230 + 70 + + template=StartType, GoalType +Ptp + + + + Relation + + 970 + 510 + 30 + 80 + + lt=<<- + 10.0;10.0;10.0;60.0 + + + Relation + + 710 + 510 + 30 + 80 + + lt=<<- + 10.0;10.0;10.0;60.0 + + + UMLClass + + 650 + 560 + 230 + 70 + + template=StartType, GoalType +Lin + + + + UMLClass + + 530 + 670 + 300 + 70 + + template=StartType, AuxiliaryType, GoalType +Circ + + + + Relation + + 540 + 510 + 30 + 190 + + lt=<<- + 10.0;10.0;10.0;170.0 + + + UMLClass + + 50 + 200 + 400 + 150 + + Sequence +-- ++ add(cmd : MotionCmdUPtr) ++ setAllBlendRadiiToZero() ++ getBlendRadius(index: int) ++ getCmd() : MotionCmd& + ++ toRequest() : moveit_msgs::MotionSequenceRequest + + + + UMLClass + + 590 + 200 + 390 + 120 + + MotionCmd +{abstract} +-- ++ setPlanningGroup(...) ++ setTargetLink(...) ++ setVelocityScale(...) ++ setAccelerationScale(...) + + + + Relation + + 750 + 310 + 30 + 90 + + lt=<<- + 10.0;10.0;10.0;70.0 + + + UMLClass + + 640 + 80 + 260 + 60 + + MPReqConvertible +{abstract} +-- +/+ toRequest(): MotionPlanRequest/ + + + + Relation + + 750 + 130 + 30 + 90 + + lt=<<- + 10.0;10.0;10.0;70.0 + + + Relation + + 440 + 280 + 170 + 40 + + lt=<<<<- +m2=* + 10.0;10.0;150.0;10.0 + + diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.png b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.png new file mode 100644 index 0000000000000000000000000000000000000000..4ad0c01e3523c400f9530a60ca11a348ae9e1118 GIT binary patch literal 18548 zcmZ{M1z1&S*EWh`FoKfODj*FKg0w}ql(f=Fcb5(#B}jLuba#V6w{#;QT}sM<1OGZ4 zX6Bvud;ja2>x}2@y`NZnJuB{YFZ|?X#jac?y^Mi@aYaJ>@e>RT%=Z`=XI@`C3%{)2 z%O1hN5a*Y8{6NWRbZx>_Q+X%x_}7CUw;BDu(~``oWHQ{esrr6?DLEyE;YrH3_*j9Z zrRSVoAJSH(joyBEK1ZXrFV85$%+iJ(%d_oW&Ltxi^jsh{vE_?Yn&0I&Z=$9aDm;Sk zuB_G$MV0czwNKV0dj1@CT`L`$tFD{caAA9zvmvZ`0ajAeB`kjq{(oa0wXOzD`0$Fx zhR>_EXW&zp6+Yfn!UqOBF?{@=+fHt{-*Nu`d)t4E^PQG`j|u~0=RWQqUUhlf4)G)%*bfturJ%`yf@)ECKu(mlM^y9FwmDS7e(=o!)&U~{l$yr*t@2&%Juif zq=&k_%NCahvNvk?CO>&&K6?1@@bIw8er5OPS6s`4^RBqHAB)TkTHu+Ts3+@h62McUQ)gOv%uCvu2Znn}T(a_QH z7YB(P`%U=T}c&&&|!P4CPY^xj7ZD5ih=3 zLh$w&zA!bVa{BJA@IiEKxbVaK_cNPw?P9@{T1EPPlXnM-OUTlzjsKiArP@&!-&nRG zmi$0H|6Z*>WwtrkaeaK`9xV?K&+Xey-Q98?bqDL!h;n@wGqW7&;J0tJON`$|$gOhkT_DWWok&R?3rsw43a2j=q4{i(>&Nc=R?~G(+W$F7%tFA_s zmBh!#zjN_h%9WEHP0rHjwGCI>%wY%8`z156-PlsGo)BW37YO_b``l|74m-Gc0=HYFqcvjoY zNt#T?@mSVbO}{)0A4+oHjoHnm6m)4Rbrliu(JXS@m~@|Q3R=z|&Szp|Tn=EXD|q^7 zW@e_$Y~)r#f2HlhZMj5&=U>hh^Mc43INRlx8b7L1aGO1Ka46$m7|2$2-CNhv)XeGJ z@8p>p_c&PNP|$g^T~ln-E#$guCL`0CDCoM4KMDHyk$7-d)uXL$MQ3TOd?Qb*nta`- zie6tz|2?yOo7>^z&L1nJT}hfs#=_D zpX+F!QZe3Jt)0J;cHTu*mo>qRGxrff2D?eekcO6)kbvNNb;KD=EaRzO{J0S3ZQ^#l$X$9S>N>F6&8u*#W44wdc5n`>1)frt|<{W9w85Qf@Kr<1`74W zoD0oHic%i=lI|JoZ_X_vdn415v~n6WK_B8}3i+SBp}a2S3t5?=Pc zT7It_ry9GMwf#Bnv(;P7yBYpFe1R^@lLKa^S}R&w^sMFEdG<}3=$#g4KN3ygcZ`&M za9r}p7pF;ERJ)yEO{3}HioT1naR!13C-@+S(};*oTflkqhX)cNA|m3xKNn?a3yPw& z>FDHCQI=3wS7&G|X}4ROqMWTVO7&~bM+o)$H!5b|_LBW*EHxbp#E#*!&vJ90Y49sj zrjw1o6Zxw1NPX$efXGb^-6?ujttuKatJPul+>NjyoBfnYwzX=6nTl@SNFire+Ea=* zm6MXO#lJR2n<$@5WOVY;r|aguueTr8indq7amCOheqJc1oHO*iUvV$bP-8`XWBH+V zWvXK?=X~#p%Ey^dWaZ^eRy$gP2~3wunlxq~TKo_!RE69f>zpU&(0^n1uJ_Tv;9y%? z>`s|_x3P4_t}9|GJxK_;-RJrHE7gygNspU0(?X_$Ie~$HjI6@>Zj47UG~iuJj*YUZ88 zL06?ln+^oI{l>D@$-807L+s^Q_*HJ_HE43FQ6P>EHuxP@ouUo(I5q&G`Bds`0s z^5qT-_3Hvo1qi11_A7%;{P`Ch z?Apz1IyqFi70XlAjJpYw5z$g~D$9?!SMuw* zg+4ZUbiIFntJ15hy`A57UfRGQwT6OSx53^t*hNB0YOKU0%uR~hW3tBC&(F`Ln~acf zXMZ87U2IcPo6Hm2V@S(UaQ{cx1zgH-ig!}Z442o0-x7+X2w6%HGxIYE2XRKPMIn1v z+aDw4@DP71Vpdw3VN1x(Cj+S_@xq>UWo5hlnTqXEY(r8VISO(1^4zJpi>&>~Tt%(O zstfXa8AN({os7&v;_cIeRfE-4Dw;bRS_3DJG&O9;i?76bw&~A5&mnU)?V1`IT6VHK z@in%WdYM=>D|#x<`RZI_QppL4ad&qwjj(`?bVRdL@Y!9we7R~aQbXJAm&lSWMHj>F z#uS^z+{OkQMYD!wZK_;(lA<_USZF97IXSabB=Kklz6#$A=!(Q|oIn2f0~=dPPL7R* zWnkgmA{#Plw0^!x7)`O1J zu=A*34;8IJ{IzZ0py^awQ&ZE@(gND&YIoOkAZq}8Q_Zu_XL0D?R3?vo96QHO{5uXj zP@YD)EPKae_pF#P)3lMZ1_uUgAKT}>UPfbA$yCW)we_z5_3Kw*uNu$Rk$V5&AjezY zzn{O8E8vSmLCWu7W^8PHu)EeANMeiFHgWG7A0Hncekv?n54tWAK(wU8YxARJ@!N;5 zU%w)eNPIlJ&8@A{{h$&CUS8g7*RI)w%I8bvXxCPG9v_`Kb7rF2ap)+=u>L;j*}tc< zO0KFJJ=!e-v9M^AY1TOX8n3hsqmxU#^ZccDr&*KMc!l+c-FMsH{^fKbexMkXc8&`T z-9&{km3d&H!AjV7#@;Pr?w^glIE+iVXNAhY680hE+ z>(}3_xH13IBFqB6L9=9Wd3|U}*?V`F2nVP{)Q%Ob>mva)a0Jk6~k zup~y9ju4Mf$0?o#^_jRmar;fBu{9$T)0!m*HV#f+Mn;b|_>YNUow36m^Pk^8@3u3u zuw1!x>6>=@m!RWdGaZG_L0aT*5Yrm5Me+RuIfLMPIe+i?(}TICp?n=#Sy^S}0a*G( zwkn&Rr}i=4MAx zUktsu&i%dH@6jN%PM;$P8$f{$k;qgr53u2Z$eqLeEj?Y`AIrjwj4!6Y_#jk2>dDB; z{>uQQtJyMW`KtL^;kY#(UV-jrt&=4D;hMeZeTxD#~} zdxow--jkO^c^#4=NPkWeBRq5})SxLgb_K(VMDPvYhb_Ol&0$3*}e7(>q^y1UNtG=)3BM*BeRZQ zv&}pFrI=^WqEs2|DU%Kys9DIDG>&z3z!^dM!f&^D6%X&0;KuY<-vmL?!G->s-!lQ= zw(4DWvT8hb22_)r=ShN13%k~B!b}qfa;BHBvkwK$InT94>`l7Lx$yEjczAfgd5?*S z(I~gDcV6R;SpMRJ?TP41vZzPyLUvbTKIQ;E2Nj`@wbuwKw@7;Pm%4SYVG34}nZ>uM zbn9BX8c@$yc~Z$Uw`BR zt{gHf7Okohp?ZTmDp_FF!ByFB{%8gNTVuD>3;w=Fzcus%rPHU`(VU$*S23zZzuY2g zMh$9$fC~|B*g=&LXCl}lvHrN8EEB#my1%tBW!+5i;K2isUANFdPW&~4D(S;ns$=?T zn}ICl(Rj!gVHfHBO8er-iGqRxFpSG9S`}8$B_;ij9AHI`lg>uZo;^#KP4L*Oo?1%i z!WBO3NtGh@9aCFq3Xh7Ci{;9MS5ov;f)H|<0ady<8bMbGc{LxuCA}-ToUv`kf$a7? z+LB_c{n&l`+T-1ao~C7{x0slux`>MSM${`ABMc-zD7&>kFWg)FHVe5*iA|u;(Qmyq zb1SQ#$q!!Pkl#zb7i(ysp`pPP^{t5BT|`+~nIpLPuKZhtg|38!&N#ZUsXQ(I%Df17 z&+TtwW961?sa3j-m+jAFF})tyWnvU{FYEK+`|#VJq+FG=IR9zBAamlz>A?i_OmQir-;JcEk6hOq%%Xdz~sj{+CmPK>9z9emCMpvy1{NU=w#uUH_ z5K@Ws6-*hJ#P#Be>xC0#O{&*UGYy*7yA`uot5O-)l9`ok>A7?F=2VOiU&vyLQ)A%O z7$GMLkTKxQHpx@wCiZ_BJ@!3e6|5Wu8JTIk&D!YQ%rg+z!Qgp$c}EafQ@Sjsq$hc( z3%MNrYEGpU7v^_I*qP5ms%|+_WB{2+M9jbxg@Dt~`OdgfORYLLUJec$kjSV;%=3mf z*B~58wPOdfet_KY6gKR*zDB*|N#*?G+R2;K*VjK>lJg9ue4yI^Dh$!2MC~PsdmhV~ z=`=)1;JJi_b+W1*uA$uT%a>=}37^c)*%Wu_UV}pe_B=+=wJc{nO2@+)4BDJxWBID1 zn0VMxyUvmB+;syZqkN6>yrQBfWja-uU^L_Xi?nK8N;L_G5A#~0%b5phc}06yYIg=T zV7Uj7WzcwBA-^Z@ui%eF?oCC>N@{ue`=<x@O1BFb!tM zeC4H3;qJ<_r3KALYc1~QK{yVrJB_axpj_)8reK((Djp2X3;a|L37P?ZQnfdQ8@5N{JreF&fANSipiY%zmu>Cni03wstqCzzG*MC)qc&%TOo`Mpq45?z^6*gG zd5#cCy%Y!$X!7q!-!+Z2Ryf$( zsE439K3B>n@W%^{`thid%h=eZ<-IXj=gwtd_vGkilFysgRwGmPKzIU!GdLmousqE zSSEpR^I$F=v` zpRZrP7|YaB8STzw)u_aZVByKrsxxKhAZ=wwOdTV2)=J57oJ|h}Q#FH5-@;UmZoe&m zLN2F&ezdjgT}9B(yTwU;Ik6+%nR$jRcjmd}ahZ(}I-W@7q~DXS8%qH}fq_b@l*Jzy zA|x$SoWO+gezc5vU`KruYDIHL1;;dQ3C_i2?XBbxM=buC^Uvp!(WGRq3r9FK~M zB1;G4MdTo?4cu4WR7JMv{5z=*FbQgttQuv~2aR>&9Apw-q+UDBG!A+2w(w1)oP#n)K^3g?DO*=lE({A)lN%IT~e(A1KdlnqvOcn zu=Wa9==$9k;lDmyA>%goe8+we>0UU7I_sLmeB*mYiO!-b<*s`cYDHWh<%IV2A#TN4 zPzuit2~en1zoXsKcK#J&NMu=$M(FV8BRz$Mxw#-R9$76fL&8UeNPmBY?lrrzF8Vs% zrnLd-UxX_I9Uij9FARm#t73nRX*%wFg6yIRD;uD|Lz3Q%wlJL4>)cDnXiUmVqTOx4XW)0<~}XdKDc#v46l6Kd$2l6lwWwT7^U_WtC$0) zYJZ?xm}%x8mT4N%>GS@^Jhu}a6EQnL!~tMn)Msmd+6-gikkf;c$*qey;M2+4t&gj9tvqQBbaeLh@!_qxxl=XWb^S!Uo2HT^jGouL z6>vwC9H}fX!b@>OHVI>m9I`yh(;;Tl)?COFJXkGmdMlycF1R;N?XB^PZ= z-1DG(NZVEBA2A=V*=7B2F@q-_z?^bkNC5b|Oy_01OzGF$j7x@Mii{DH&v#dbx^Lr) zCq&`byJQ2x@)@wi(L)bJ{qaFPSaX!94Gj&|GhLfha-QbDA00FE(?Hx`nUMOBKsC{O zVU*xMwYu)98DrdxzIu0?VJ%qlE0e8l?+RLpY@+9~@^W&dS#QlTim|dc2UD>v^aFIw zrc*cWacQ*AynkvyXssvZm408LUNdREkV!AiQXRMx4J|GAqeJIPi!g1C>Pw>aoJ}75 z17y?#thBTS5H%N(JQ@2a`hr*+kwoZmd@$8M5ZAu}VU<{!b0|Ej{hu~1X>k>Jpy0}8v>J->u>3N`0%VF$k-M4{!LXiCR z)VV4yI7s5Im?bEvayA|(RhGvxfO(D`A^xBSwnkB_^G)UYvNwOJ4xf!6zZv-#9+n zXsRz0e;g84o3l=J{EKQ1Zx^aG9j&dE9E64Af^9xl$Om5Tp`UQ6nW5LP%w3y+Fms_>;iJWHAv|A(AK2& z?%lh+>46mdWh9eqatYq)&6+(IdM~^L%yY~MUg%E0qMZ9Nat|yiINNHd4IQu49|h>1 zv44%qfs=7RLXFU+Y;<&VA<452vJB9(m8tq;h+8XP?$t$_%{`FqCxINCN}NhuM+|c4 zDi0_vHfW-8Ew5-P`03=|OZq3}L{zmCDvs_H@Ue(k@;*(tqotre`Tx5aRSShN5Pa~_ zv(tx(d1Lv#9zds$Pk4e@G5h^n81g#kXDkk9@wj^|b7HGOm*btg#{x zGI@6`&Y((+$JW+`Nl7}In&SXJ9YN%ir(T)`1;Mhi$j~#fx&HqCU%q@X=}Vgcg9)x_ zGm_&>%}?T1f-~}-j^oz8=f=-t6AA2%+kOxWUI(CPWiWScq2(R5fux1SI-mivNkTFY z&d4nKQhBs=cP9xr+qk*8J$v>&IOs8#86sViUN%Q zV%PRW?f^b8?!Gr+K2{d==1mAbOXGR2x68}Rm=|!ww>mmH!Uu;SORT&cQ4Sf+;PIpl z;9`>esW~TO0t5!jC*WZe{}E9PMWUMq@GzvFSXfw+)p1-VPqeis-%*RLF7Ub|wq3shs0gxq1p_Wl=*-2jDFrJ3mOS6Y9HdPmFOf7YFM=C}I(i9s<Ac9nW(09Gw%o3HbM2gI9d*UH9j%fvS5*?HfF zs`G93azNvnKDJX}gMxYCr$0f~`9PKHu8>gO%|rz3$L8k$jurCyP|he3{PR?R3J z?K}BeoeyBH!^6WMgBSR;zhxn*HtJ3k;378Ghw4U1U|dpCe(gXQt0OZC4C;b%ldPLt z&DUec7=Fh*>8LQ))N@>Kp*GN3VLFs|SGGSiuS5~O!+0wwzyg#D<-W1ik7PtdO_+Ax zqBpEnV&lGIOYOhmCqjuUJweD_{g;V^goM8Sj}ntUdivz)ju_5#@aq2FH_37Qv7ii} ztSy@;fGV3C85(Z3(m*XmqrAJ8FNi zy)fb}YHBG-N&3s5SV&aKLVSGpP=b`PYz?J_8X;hK1n1?c@XQqz6=~-=2(V#IJ({b- zh19oh>8_7g5}f}Y3!(uiAY>Zr3?R#f3!u4+BgJ%dCZ9?JnL3S@`!fkphrEZJ{q~*j zXWeh;_@j=OaUHA$LY0nJAs7KAWFFhzZYT@DWeS01;H5wMGeMa=Zs?$R2c4vPXE9&+ z|8)asIt%=O(;34_2PiIk2pgz{x1o%yC?5PQYO~giTmcR8Y7X;?i->59{`AQnYx6wp zwgwlj|M3GGrg2J#qGr=M(SXMLb*OuJ#%f)8^v-@buM1N)Xns0C0*wGPeA+FTD8ibg zg&4KsYuE9wvs}R3KMQMO!G^nOpPk&D?4v9F%!wU6i8@!)x!0=bVK87LmS0b1z&H|y z--pl-V0ikndrvQ2KN;4G0WK^UYN8)c{u2G^=RNcyKjFcZ3(rsHz>s|gb4WHBpucrJ zmwv`&dwQEl;H5yr01ywHj(%cTSLNw5>z|`P84I0G!w=$*#YE5Y-2)y;L+>4f^fEjm zaR1V@6rxN^r1qS0g-KR4AdkFpdyvlW@zefh14#nAY&LZaY@jX4b z@EU!ggdp(JxXtN~BYoJtYbj6HSa}_PIDnJ&^xHT1c7VPOc5gu(^k81JftOYvpcjJ?4=;ey8@-rMCa{=}(t~_dL!~WXPmH_ z&JDtzP;?~H9mGn$l&^}mwk*v29+VPu9|~X_*$Tb6cYB6|`c*L~(;DAi>bPHbWB!YV zDL9Ei@WT^FId;oT7~Oqmo6i~E(79?m-FR2hr3|tSu&oeRBpU>sKC)sOuGt|a#$(V% z0Bb4uW$32J^V2Do#Cqr{>MP~~m zx#;OL0P4jR`u!acRyPQs_=pK9D3Fj+aM@fmb(5)x<6e0YuROMzk9}8?byZJKkC^!> z2ouAS5h^f&?MfRf z2Tux%kD8NGL*;J!Hn3*py)(&)ixSLDnzIoLP)&l|BvVeU*2Nw|j{EoT+u3agl5px{ z9pG`ZifOX}`VCcs%?OAK4Gattcx^=sxy%s|42T9(QczN^Z)_xmggoHACG&Cxbf|< zJo9HQ8EK9@8m-o9I1ceCXGj%n{VGEryn6YvWGFK;Q^$QL;z&VFP0h^AEP>q-WQiDq zh{k?(SfQ5d8=&mYrq*IzNy2Qc9n)eEUUnB5w6CVqp(#MNlU`>qm&czKPmpsNhtO!b zSLr_vU@wO{=Zq|a z0=X_*&Xr1Mo`|H*^w2hlxl^|zH#4)a;x9QoWxi6*LjtPV)q@d~LDD$RR`+`bQq>;q z3s_iue5zk~W!U9~0rT$0GH zYLkBJ43k4!X|!qKB`{r9ud$3Dv#`!AzNQT)q;n=0*{>HDA@Cq#!`O%{Q0IhqWGXnZXT8vQ{)GT+Y|ZhC6A4Pq2nK& zJ)8+)U3;O`(=5XSUBP*EowJb67#Vws{up83g9GQl!5BYk)PPD1}r4udGFgD@J_D;^Y6Ygiqu8I#tFEyk7 z3b|-S?d*0K+TS7wG^MQeZniW9aL1!Y9Aqf?4LtKb z0eUBG=it1AHhR4*Xj)|U^|*8xXEgfJsIauzc!dIz@Zbd77|PQ!B%A+oL&E=Hm((SM zW=95lhl<8e^szU}`^JZ8j^G3R?*Xa~oS@~KYrYob^OD84A*0H~AOOMPT?^I4YasHw^uG>5Px zYp$OIrgHNXJfRbl52?v%j>0A0m%u4>}W z!;z5@f6oWDFwzPSONDy~O`KNs`uc-VBT)9Il826gf%HXT+rwkV%PTmsaPJN3zT7jH;#S368hpd_LG z(0f?8=n<0nEttk;6bs=A(y$|1K|^E3ijZ3lyE4!1wV$G#y!G$EYsC~Lzcs;YzHJ%E z=Z*#5@8sC4o0`ZJ4s{kHK3IeqAv4R2Ackk|G=hyWL z1NkIug~vC$z*+qbg5-9W(1XUzXw$jMTQ-$U2QyX*hGT~MZbz+vW*3`{a2z-@6P;|> z>h97%ce8WZb}XyDFaXUn4nHY}|7@rYej zRb8DF5AO;3;hBvSB8_R`qVcN+O|Qu_;7`rZ>u$B+h;RYT{qmJ7o&RF}{TZR0k@S3K z6<=vJiTz1;bX;U)nMvR65o)zlLe0uJckx0}i`hh#Js_3|o%P9T%zUFa*onXYaRvNJ z+1Wy8eP2FO$$79+mvC@!E?*8M#v5Xeteinh{)mKnC$KV9GAt|%6BG06moGr0f?^zc zSmQWiDk?TA5m8andfn8MB_|ssgkbdJ3tp;BD!Mg)HaYCP@neFE^i1HI`P96?Y{;v? z5jy=j!v>CIqWi-auQeb-9S zYVTv8?;}(X{UR z1uC~YFZ$_cQP|lru=_G2VZA{^e=?l)iZ-2xpdYX}99-PJy*)W`aX{UM(f~;2gShj? zjT?M?E=_?X0I==W&s$eSg6jmnxSD5NTpSrb{_Mhn(J{01K0PJM&LC8o*FBV!`gM*Gs)sQU3$_vm;Q2(g&lr#>Lp*UyUWXYb5GwT=5 zpO?VlwvYqHtMo^8Fdz_=@nY; zN%;yA+Tn$e@>{*KwzkC~Jr|b}OdbuP(E$H?#fNPyXKP?@%yt+`g$<0Dc^wf3R8w8Q zF5Ur*EYL>dmz*VyTaMP&zjD-yIXF0Ij^}>-Fg_z2T%+=`boYH#l?y;~z^VWQ&PA_D z^Vfe+sv}u647c8m^!`X!>nN=_P*6}1drQvl;qv?EG?e4AXd&KXC%#|&rm_mcxR8*m z&g|$2lRrKkLN(PqO@*8}PzY4_gby|`)M%eS1JVyd3<694I3g@u%A8v6gGk*0aNE^e zSZk`4L&ryH=Y6FV$4ioj-QzqTw70i&dQ=&Id*9I^{nvDT&Eq8{B@nYKtEi|bDRrEL zjS9Dk2nl^&zm|LrB;!Cg8cVUj>nWX76ck!dpE~Utgc&TVXM^QS7Vm4PFMQ!{Yf zDfNrZ_=MTVaS$1yJm#riRsxi{I3#wg?w&a%J-5vd@ma}@`r{+$<9YIr-U9EG73iOL z?%ZK!W)>8zEh$loW8G(iNb26bd-U|;+T#6vDB9jFM#i?Gp`lf8U0q-dKxjC+0^xUa zLqiFKvq#>#2(hA1AQ#f8?>{wERMxDfI?JAeOANK65R&gO+Y8!KQdX8o0aqvPdH^9B zy8sYhLEsQ$TiyY58A)8b`SP=e$z#-Uy7vf^>()t-13A?-Kr2qNn~Ns_z?GgiC@ce9 z&I1RBU0}H6*HQ|(-8=RXr9Odw-x(SEn$|$5<+@SJ15h-)Ng%*lOjHRT{`?9RXNwXU zM5m>Po?D{DhX`PB8$5kF3{|Ysaui0{P4M9w3Q9Hp$!Yf}zJ4uAO+(}06DDpT>D?EQ z0YFU!q6#6G?Zr@9snD=6vfQhb?%N9H_x(k?SgG88daqVkxBZ;{Dh!OP7q`(0h2K05 zfkPmrAfJH1_|%jz@Zo_C6jyiB>+{>o%L}-Hz;C!Iyz9vv5pwL*wDb0HMnk2LFjFA@;A6))h-0nIWHi0$eKKv&jqp&k%f zAp=6Ol;WI$&P1mZ?hfQ=RGSM;*`!T73wT-J9rlW~-C+t^;b}SnPJ_sp$slC8_s_C5 z|0}I7`BZfphNvkk)ABkE7d(f6&9FWEu-tJ&yOA5}DUhZFBf`P1NiI(SsT9=VM|utS zTa27HmE5l|EypXq-Rp+#72v?jje06L=(k6*fSV8iGx?)6j7~cGS-jZHD!d4$cDHFC zVpM$*0{0cm9d9?n)SK7#^F5K-)~|4>gqTMXVkJ6I_;61HdpTqQ@EW=C0q(9xEHfc< zJFWr=tgItBIeB^cMI$!>F)>;CX=9Dz24vOH&?0yRTkWNX;-B2zuv)~<3X1H|SO-)s z?I)$y(|9jRhw!$|CncA2i@EftXb(tLA&qCi&x`>l1piqGJvx5H>njz_Xa54!2Lc$_ zhTBZTCG+>vzfPJ`z!7a~IX^FJ`t<33oM#nBo`T0%Zzqg3eIcSx(sASGeO^*ABc7Ao;A=%@aCX= zsR9H*EiLo-5kq@JOTyG^goO4#TCUB8gSQnE6I0~;+pR4FPZ$E;AB`Zl`R=3*t3=3? zP3u_)YIjFX0RcY+4BfuV#gAso*Hg z$Ur%av*S>+`9bZ>uiJ3Z{0QC1G5hERc;454klf;ghzmtVh5#Ux1J0`jku6y7ky=;B z7`0;~4#E1jWIPto;{}|As0fRZqEyH*A@3L~c>e0^M7TIY?s!`c(EzPS;99_Fxg&Rm zfaq^?bDmLh7I-r~$BHtxkj4PG7XFVGIzp5OghNQYAP!nuT0$t6(8xoSbMC?g@SUyL ztq!BxgiI=rMMQuuMM~_kziHqXiagk}>`MzuN=nl2K_Q6%MHr6VkbvW!1}=vimmrLM ze^@WrM2;sQI5?`tIIt4>3NeXTHI2*S@IHHP#Z4{Z?`=#0hIa!yJl6%P@_+~V_l9gU z&p9mi++14&WU=|@RkYT`r_?@|L&1f|HmcfphS`)+RW&fby97!sD6N8+H|sov4*<2? zr$*HW|Bn?uL3l2rpYjqt_h0uWUpoV%pinTB#s0tddU0~WxNv6GJ4i_H3w{ejO=?w) zu5vQ#)Ez3 z80vT2`Yj<6VYi_!Wdtc0Dj?`m)X*3M7YQJVzm%$i!cAgK8}nz}U%fFeeC*NZZo7=y ze{54bTYY?V^iw!P++QJYpgpH3&;7&YerTRrrG($y{QMJ1lq2g22lf)B=b?469BrXQ zZ)*6efuW%gKz+M?*x1;B3!9pnGMsoiV@cQ$ba;S?Ej`WvTMETO8g5@GEmIH?Ieh*O;Y2V`R+nhbJxV?NZXxT-sO=)@DG?_axeO zzGHgxq(C40)ZTsv6IY<8&i$aMh{Ii1M5Ga_!41@f{cvhF0iIfEyngK(z`$>z?EEn` zwXvybeqllJ#3T1`y(QNFf8*BebbY!RPNB@vB4qxW!0Cyh=t&iV5=b&=P0zxTi(>5@u$r z5Y{`>lbvktHRuJr7>To-NB9v(8^(B6v>;Oz=zNh(k+7CMoal@qT>f^|@_;L-0{0sk|oH$2`t9dsGTn%d;mjKUwNO?>C}CnM~kaU#G(t*>7- zfrl|U^Cvyli5C0pz_mY{--xlJmg0q9q44?D>9vAC$p{6m1?!%)5h#vld|x^pVt7XM zeE#PT*HO{T_v2a7^BZ5$UrLPOu89j#=)sKrDil&^zoQqSPGq5EtZ+KOMsv_*79#Y> z-%BeL&Y+j>RX#0xe)04k%TGalmH$N{Nn0yV@&Pu&v^Q(sA`sX0uI(moYdEEYWW zAVI)6^mG$S;~8F}icg+!Kop9eGx@XihqI(p;7#x?PnSe`4xTc}raIk{Av`nhg&x3) z&O+&a@fY+8E6%`u&ZkppHUwQZK3yTk0_;QJBKrAfmtcdBR#;9)yAIzzobCz-ZmYpW zU-;<>%h!I5o`!`2?g~KPmFxpAsN@`4sK#n=7fbG!trM|)YKOI}-$rl5i4&HF-Hcuu z8?43)H5k}7?U!VP7_RU66NZHnh9*i6 zp!iM@fyZ)vCyD5?I{p0)AD*Pg4CeyC9(1N$@bz|y$2I)dLjY23l9FhZBXj%kKcP7~{f^Gg&LBU(Ut3#YNznYC_jTmQ4!WtG z`0CZ3zq{J+k)JUJZVi9}DmfF=%jB%bh^b(PIjh-!SEzpR$j^$Qo83?m+V0R3z3;s2|K$3fz=+1tvw&pP;Pt=CSvckC z(DMc9JBLQ;$Z|)dr`A4k(_PcSmP#vBHHrmYjY|I-9v)8Rd8pt@Moe7(--Ryu@G>D0 zEepeghaKi7EJ_pP9m|O_g!WF`sJ3~Xs3d6lz%xg~h%9}yqh0?A8cN8?m8-q1`Vy!7 zfA#=zo|3%$v(>o^e_a25XMTOEUdPiFs3K5F;TuncK$o{MHh0v_-mqOYDGcihseTQ= zysS8poG&1CI6NB=k=f`L_x_C1%82@*m=w50IOi6l8L`({cENXddCZN{)arAtUBx~t{PdY%g5so8s#IB zaOMv6cfX-(S|sFu*RdIKwZFoIsE7wc=isW|BSGIDIU~vPL@1N9e-2FQsa{q_eyP|Sa@((Sz&n$-p2r$ z;t2w(TP!W&qpE~@FaAego=LBlXF=n&NhLcF&)u{IYb0B7ozB z{ZN>kkTc*!Km`L9yXA1f1J9o!Bg>ln6m9!#ke6 zj?Gw!>b(9_S`OXDDE%&$7yg7uL&M`Z14hQ0_scH-h?A=?+7%$BHE;f^zYz;{F`sjsxWO4-CT;N1@Nm;IQ9gBQQywfk&5>Md=&r1R@`jb3|CV7?_1HFlSUpg}WvskjSzj`{7Ee?<5cL(ekr1>42X&);j= zE&`UXJ}$8hMiHt^RJ=AHhX}+0OoGZ6^nF4H0U(e(ri08lg7>6LOi{L9B!J%!vA+-R zP78{ENg*~aGcfP@48CGW Nh{!(9d-&|-{{wHqcDw)p literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.uxf b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.uxf new file mode 100644 index 0000000000..a6d1c2b94b --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.uxf @@ -0,0 +1,133 @@ + + 10 + + UMLClass + + 330 + 210 + 280 + 70 + + RobotConfiguration +-- ++ setRobotModel() ++ setGroupName() + + + + UMLClass + + 220 + 340 + 170 + 30 + + JointConfiguration + + + + UMLClass + + 530 + 340 + 170 + 30 + + CartesianConfiguration + + + + Relation + + 310 + 270 + 110 + 90 + + lt=<<- + 90.0;10.0;10.0;70.0 + + + Relation + + 480 + 270 + 140 + 90 + + lt=<<- + 10.0;10.0;120.0;70.0 + + + UMLNote + + 700 + 200 + 210 + 40 + + Defines a configuration of the robot in space + + + + Relation + + 600 + 210 + 120 + 30 + + lt=.. + 10.0;10.0;100.0;10.0 + + + UMLClass + + 20 + 20 + 450 + 80 + + RobotStateMsgConvertible +{abstract} +-- +/+ toMoveitMsgRobotState() : moveit_msgs::RobotState / + + + + UMLClass + + 490 + 20 + 400 + 80 + + GoalConstraintMsgConvertible +{abstract} +-- +/+ toGoalConstraints() : moveit_msgs::Constraints / + + + + Relation + + 470 + 90 + 150 + 140 + + lt=<<- + 130.0;10.0;10.0;120.0 + + + Relation + + 270 + 90 + 160 + 140 + + lt=<<- + 10.0;10.0;140.0;120.0 + + diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.png b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.png new file mode 100644 index 0000000000000000000000000000000000000000..4659b8be23fc5302521d82bb079472defb31fd10 GIT binary patch literal 16098 zcmeHuWmuJ4yY8ez1tcT|6_FH>mJUJbZfTH^ZjcfYX#^%MozjzrDJ7wlq%dg#>5@{K zGf;6Y*SF5MzjOAt_qq0U9sV)r`_6Z~V?5(|?&rSm5vU+{{}MJCHUt8>B>CW;5(I+m z1A!pD#6$+4z#=5>s$mfK&cQH}W zvEK^Q;iRh|MeXaC3>s;&Z(mncpjaV%dox6#o07a6L%)M*ynIp4$p?<8YYR%lG>-O8*ps@#Tts7q0bAkWDUbt8mLiR zk(Ea;PtMJVOP&oT$WAJtWjvRt!qs{EF()f`s4r(83})Bli*|hYO0wGulwj<{G6nOJjw)H21~_<@Z55lb!v@`jgJl+P7kwX6Dj#!^(`Km2tbn- zwb{Pld~ zP6oqSlAx&wB>uGI{^ijtn$0GG)mB0Wiy70+FXZD`pRO;yAWzW5HK9F-x%lSUcKXe; z>1*W?FUTt{rs8#6Jltqtksd10WVb|=`1_-e3O@O|dV%H5%gD(7GSi-g6z`LcXvXwQ zS1nqDh)~ckrs|1e;83`Jejl;5wN>wV*v~vyvb=2J#$i!o(Cjbr3|&!GO>Jnj;&G^| zeWJn%Ih*kVL&F=K+SRtx6L6Q%ipzR$bl%rEEM&_iCB(!G)jj!Un&RpFF6=56ZU$FY zR@UHrS~Dq$ys69Ow-Iv2>U83(n8IUO$^AoG+*MXQ{YXogv+u%l_;@Y7t0efL=`ciz zR)vMCv$J!KT+-sglZ`rB?6olJ-XJM~EIh62+}7h}{M8d#sa)l^a&mI29uLdKyr)Ay zn}PuzQkOOVFJm?hSrLMci+EP2Yc}2)uWdJH#1+K@w;P|LKtpWCt2f)oZQdMhb*8_< z5fA*@70+H#QQz%IkXSA*+y3LuTf^A%E&RSMdkd*6aOQ(#1M(=ar$`%f!dd6qOVrN0a(Yci6Zp z;g|-A1maB4f~!bo1D~N`JXNyKnQg`bX#CbqYBQQU;2+7%h7qGmh*Faax1$fgZRDsw z5Hf#q_;tEeWe0x<8IXBxD z%ICh5J6>lyEh;A$HhxL_fJn`MXR+^@vq`i*uc(p|ew~U@V8pJY@8%6ZD>*`#LMb1c zFgZ<*V@r({gQMG+1QQnbDNV6-EOWN#zy#SH1-3#PT=f%u5ZdLw?K&OrsPBhy44zUA zNG`Rvl>dB%yKY;V1^6^=IyvY2;s&F7k>zmF#CsV+V!&T-f!GV2^jDRk z8}23RWL3p_4a_d|3t5?&?-?|c6B0(#G{Gb-nUM6*Xz0DUVZ1(GqjI>MUtFZuZ~~Si zz1Puik)UKk9CPdyone~s8m4e!c5;8VR&143OQfv>@;ZoW4Yw*Slr=JKZS3~;_BQcn zm9KAFdQ*sYCYd8aG1MPokJdXZgoXS*TAR9EH4xz5-{4cE7q+*y7K?IUzpi=m&DzK) zgWq>aA!x#H5uY_n>~RrWX}zlx=J83j>*frxo=3#`%hJ-)k~z%2Q>@YT8<;}`BShFk zVUTk3WqPYTr?dII?tQ$Xhk;8;p*C4KBn)pGit*({3jWx&&U2%M8Hthk`7WAqyU54HLZSEeZ!y7x^>4^$GM8V#Fv}QJ9#vV{ zs=TC)^B}j$4~xL-^l<7$v`hZ3f^%qL4xU5^$>!!es!koP?v|9)wJ-v;6D%z|`T~|l zt(kzc3~v4^1nw-hw>zvS^c7(=WB0|z`*4gvp8xt(8^7bC!ht|}efdi635cp;?h5<~vCb1)gj7K!_l_*`^r?4Qfk z8ny<3G^J<&a#XUwll7?vmeF38yyh3UDPZG()S8%(!2Gbf1fW0f)@mtjyT|*4-Gpub&dt^d3jwIkY`|3z+!-u|`sHEmA7yT;!)N^x zQ><1e{j#bFhlurW+qImPBo4W@3BLzs{TInN-tU?A@2(8vlCt@Am?tI&IygA+xva}< zKic1#!{Q=PfhFqiktjWSG><{$fd?Z^yDPN~-cm+Rj#9|$=;Zh?v?9CBR|m6u)o!*; zVCy~g>({RbvSkSx&nP?YUE&96uo4_RVb)&g*LDkn z2Zp=0HP+QXs%zZ#*vcjaGbZZAY)3|0w+J0j@g9aLckQq3y5bO{BBLwU5>>x>|Np4? z(*|%~#QS|{1TEy~n13D?)~6@)cTmhaySB+^nms0xY>6)NCJK4i*I+^#(I(W0ZtCmX z@8?rNd;+cWgLLbjlk~5tA^ErtSgH2qSK*1(PB1|5Y-$S`+;^`SKB&@0gUrZQsfrg* z`l=`?DJdvitE82CRc+Rn{$fQtd#n%{vY9=Whu<|B6CF)+>(h51W##<5yq5KNcGLd;ez5SA)*q2q43Hq>y;iDNk8D7ec;mjid|k{xj&bx> zY*as)uCA`2Ne7`3mH?{FXnDIv|Bds@H`Q&HmWwrDhK)W*I<=0ZJhq=AsPEIM?yQZ) zI^Pc^Q`OPAYVGy#5M&Yi!>?APs@mGpJr1V;(J(rno_uQvr?|^D{>oqe70-3-o)jS} z?B~YsS-kh9VVj?tP~7+6w$r`^qm@?VCf4pgA38d;exL*K{U5+UDirRrak5(0vyOm? zL5sC282I?$$~kg7+jZY237+>k-YezHFiwIxUmqggpEcUXy;laduJ_*bi_=rv=`SEn zn|^p@&SV~8oq|C5KIeCeS0~diitGm}G0hF$b(r5-{`eRaE_;`)&+X}?R=p@iAzEdR zx>rZa=GKcrN)09F?r+je2{s$NGX9xZxy0&JiGl(UYU?9#DE&#wY|m zTzgYRcFOAOlOr{~yu2hO19LmNyTPn2RXiTHo~U(N>`lWWxTP>1b6dNAVBp)vl>GCZ zsLCpvDax=~>ScB*HaqN~mjNvOgmP@`>qI z>H`foiFT+6(e=xEnVd)i&vjrmQ&8v_musPxih{gO!h2Q&v{yc++_@j5|`l&tZa3RAi(KR-7~3dClT{{w%?q=X^|2bCyEK z!d)3DqZNpcj)_qscR)1DycUJQ%?GkmPLCGKH}y_;^}LT}sZ92v^vW`(DZEa3x#<*Q z?Mv{DdVqt-k7cB#Gg6>VAsZ)I!z|k|AKRm7iCJ`yW`bFiVu)}DhEQc19Q>k-$IJ5t zGxV|5UufuM(({iW(JneHd?1z1%gY1xJcI@U!Bd~liqEO6jG>e)d_5cxspsV-Siylo zeiK2yiq2 zc~tM?)|AT%nHY)Q*RHY8g5A8~zM9?apLmLTJEf$3s8D+@CDkO&ojZX&;cW&M7P+o5 zKqDIyaMOYWLC=F)rxh}Ca+CJ6UDFk3VAjtzG0BYdBi}qjmFcN~dbUo9eiTjXZ3!eG z>S@CI8rV@Q9quwRGIHSx6Kf3Nl`CWkM=6?RkJ@oj8;-w@fnrSD^BgQwmX*5(2iIhk z4VA!KCHq?UU52b}0C__}WmcJ+n_C;NiL$?Yi~}=l#a_0r-e;Kv%|KCZZmQ^k2wk$x?~ollQHTCMmL2;Euhx=Mj;}ZS$e4i_qw1LSRE1Xt8p0a|65>4|-0I)^Jc`u43WSfdkN@9yVBw3D0#2le7$(->%5>7in=A69tyIa0Kh`w zf(LXb)HskYF`$}^O`e>k*pp55)P+8mNw;?%*IqE|zTwetgsj}R_Q^?4pNW4|ZctPf zkvj6Ta?Dg#Jww05MvI8~{Q0xSY5c%t-Uk3JeLfXOgNfocfwe?KL)+1?EGm1@yZW@-pq<3(z+;XH;fldYdDJvTlas74!h{^;4OX1;H$yx{)LWS`@L(o!~VZf+qVp{GySOHh!6P4_5D*o6*9 zdb+SGX@pMqdvs37Sz9DMJw5I0?C1y?SXgl3?=x0~ArQ}M9l9yEg(Lvq5*!NfP4vEP z`1;3RTwK1lc40N~ zDR_6@0js3|I~#z7kIr&(o99+LEkT0Nmh_m4uZvEF_#apG=2v~fJba{&f$a0aQJZ&X z`8LnndQ!xXC*#z)UkEF)L3h3eS(K^D5(D^z`tNW3a#!Dns^sTfYS1-Z;R`QCL4xc& zeU5Jl`)40J`g0o~9r+}kzwE#M$bT|gR1HZ!n|>D!L;> ze0TajSY}WgJ-55L$;rrWRH{6-w0u~iFLHWv42npy>r9|z)GbvNl{oT>7WX#L{tONd z=H)TKp3HZ}mzQ(wV`nUETl-4a>L~r1rF%V&5p(Nj?Ff}na%}9kgO%dMJgs{$P+(n` zvSQ^@1U(l%ByBJB*o;+L4dyD&JKu&S!`{5{0I0hS-yL(Z+&l_)lT2{qR<&Ob63UTJ zU402k#qK(M$+6sL;yFN>37BY)XorlG2Sl8RC(0M)s(RAm=5*8Al=_B9QMV z;V#9wWIy0#NNDI{GzE`sYFgTKTPUDiHrqN`8kq7ItV%oP>j{|O=i)~6EI z^4=Y_so6T=fKKW&7X;-N9>TDg1q=!>W6M$Uf)VhsDw|kh+wp3<`{9()s!$t{23jgP zrvgGjul0(@&Tth0E3W&R@LFoUpZ@bYNct&H*6+fSpisxPQBKV)+wFN-_bX7iV^R$Sa&!tGlbtoo#08kVGR!)x&eo+~>!0zPY8AZs0yJ`RaMp0)NG41(+FqM z)DyFWn+sG_pgw$?SOW2C)YU^gb!R)qN~oMeqquAT84!Uc(TYF71FZ|u@c3x|PSr?X z#{J+mknmgKQ2C6~6NddJ5Vz~$On-ZKnaI3`I2+Ssp#Ht}gumHi;7zCjU(@&lO+gk8@Q^O-)Tt&l{|FPO`gHgpaK0 zO#miR_}xm=B&&a*h&Fs`_&Or`#ckS9H}1lmN@ryU$3B4B7rGN&X4@_~=r|S--$ndN zk)vnSqlAP6pdLX~0Seu=Q?oNl7k^u?{^8j`FLJYVgYF*C zFGO%Xul=k8(8RBV>9`^6`{P^&+$|n+r)6hn=jPH8V&B%T4DqVDqh58dit)9&P1EJC z-HkY?_MhKhmby6KkJz+ulmb<*C(vp%;G4Jjk}CJpR{-Uvre5RY8(TW9NFU2x`lxN| z=LxEEg{p?eUMsO)uh9(PXeDs1f{9tUssgUcO5BgKKF|~wZ)NXJKeZ8bBIi6$fT@*W z199s&`V{HF`^#)VXgxgDRE+BD%S=4EkK43ECxhGa9cJ`e4gQ)&2p+$ztZZk@?F-LE z#l^)rIXUMoPyl53@uU8z5FZV877i;t2pkpmHhB|(0L;vej*jv1@uMS;uQQD}J7o}O zEW_slv!Gt$vF`5fwzjsWDdfI+!@p{1r;l-<`KLQ0LDB(0;R-;l4IekBZ}l1e{yq8+ zXwYHvln@b&!q?k_jGzf1`Wj-2fh-Jx#>T!7O`Dl998lMPuET`0DlBwCKlYj$f;}13 zTq`XnCnwMWT3cJ2m}J!ykv{p$!_DA}Zrs$=M(vR_K`eoWEPuT}VderC>OZ`Y&jKl}2Wf zf)Z1H0D62w>v&2~j+uOPd^HSuTtSgXCI9P~F}eQ#fc|IAkPm8i+#!|YDs$PNm)1uu z>VEzQ?0@xB+Jp~WTwDpkqoroG6qkS724@!d{VF}0o8zwL(9 zkDp;HmdeTlr6!%CX$p#pin6kyHMO_PR_ud+i$dZ2b^=&gB9~RihY!qjbUaSW(3+yP zV8oo+Z=ngs|88(3C@9D@q0b-2{{S)D`P;q^X^;An0XR+|N!9PqCtkXADSdog<@Mhv zb1e`2!sHmp2b!4$w)&s32%HFx-!!App`!k61L@09k^*XL4X8~Eb8~ek2PjH(N){7xI0r` zhlQ|v)>Q#y>gwtX;BS5&6w!qIXYc!V@BEfHeduWATTnd!xFw*K3jy3j#jq}W#pPUM zFopImKQ$zToRn0p^IAedslyqnnv8}!HlCrXW$ITdB3#TYEIE2)LBYY#b|R(%{$@dv z{ivMQ$M5p-Jpt|gjZjToahl;`y;wNZPFYE*rSb-nH$d{BIR|YVf$36j+OvXmosI+l z%FA+j`j#GZ9no*!zU}Ig*;k-^5LOiadr-693fBp!0L7Nb+ZS|l8JW7u$`0SwC)(O1 zW(}y5->RhRTgi;X+OwJ%&Ngsh1Xgq^{`UTmw$HE&O9 zyX!WnsepLn<^ZZA_z~l0sLk(Q<}N?O6=bcnF*A!3^7d-*ItFb$mG_a|K9wDXkk>1F z1F+!q2B6D)=xG67YtU*@(j8PcpfQQKoBl&i=$-kThB0u1nYX%zE_pF4LGQR(Yt^!P ze;C<8QTvK)EcI_w>QgP3Tuu3^$)-4ms07~iTX@?g@!JXTJe$&$XViCz|E)TfC-7@I zsr}FS6raOr-McsbaKmq3;}^!6Kg@QD*SjQ42*ev3W?!|OF*^MUWbIYsHXO&EW@WKN z|M*#q+E8EJI<)%;ZU9pRaOE#qkv$5dTI9k8k^+Un)WucdZ~Xk6<>qmBy;8^M7SB=L z#P*lL-fV;Uy{Fb0spkOk-}9qOU$IoY`>hB|w4UB}W;4R5U6_821fgiMK&wQwhg@Au zevyL?u|>@%LKxDrJEuOOOeTXgUM^764^HYrJYBDFUA{tYCTe$;P?b3v5FCVr0c(K- zw9H!Ni_`9qx|S8WXApgs7VutL{7%7_{6NA4318@F8h1XZlDa85mlNaYf#RHmj*gCn zg@vkWG(x~(UPenxOH8b}xsX3&Y|Q&evPv~pC~n<+V#4A3xy3w+fuFqj6qsEH~^_oU~y z36MoJSPFEwbL3M_re9D6Ss8#z0w{To7}xjifvLMoe~uuToXIj%T$(^IM?*tHYAPy# zxYFhV)&psG_XD6o+Dz1n3z;Xfh2|dY7JJK*KIe*I(K{KNI#Htx-B}slpgP^du<|(G z9d^tG-9!IZ;3oles(0U~FVoF{ok&jsXdN1~wY@DZC3R2rmoGxX6bQb(Ktg`o5i9a8 z=N_P3NZmL8;=2%M-<3pL2~fbQd&En9HPrpi;~}Q_3$j=|hU`-LTW|Y&LP9Q*62TI0 zva@dk@gVa4Ezs@m@V$Vwhz&`j7RpyC{vw2GO?f#@nBsfhgZ9Pb*=1^p-lBr^dvwX} zKo3s#I#>c+K`)m{1Z|2(q?B>l)X_Y*iHXVdy!TD(sruvPkNM_@g;kT0Ztg#6hP5Ra z4=wF|%WsQ)VuCc%&Meg9`%|#&-y$frVB z#xB(ZLLg(HuQE4*&xQG8{lRiRXpI{Hn?P6ogmiko9>l7!nkHaBW-@)9?&W?4E1!)& z;ExDHgy?2YJP$?4Mna*+QjO(f6_3@80$&uwVrr%QmR}q##FI-N33d#X-!bWc`n`}% z;#K9!cqKA2_q(K+d>wGfmb@{aNL*@=r>gr|La-Fs0+?L6AW|ETNs* z8vYPZlI%b>06gD?u{Rg{bR+C!;XzT@KSxGC+fT5TCiViv&LU7XVlKlahl=%VJZ4Gg z>F5w82G77YsP#ntnBqu|wmpG%O_O+v4u&#OmJBrFk23>ofSS$@zuu>81iV#%@|j!k zM#rU;XyXe7*cAmzPSQLVeEB@K4!9;aoN;JCjW!6B2l> zMmhHH-dO3xBP1QJee9ytTLjSlF_;0 zXiHk}7^o(NJsvJd4~AN~rB;Ojs_%P9c=cNjxQLdRc`QP49noe6ekae{}dWx%jTdWwS!>1+H^q=*rJ z6*$6hHU=9@fR2lVf(|Ib=wwg8xyq!dA^Ti0_)(HE$IH!?!7Pc0G`&{N>2i`|mAiyD zw6P|1l6hXX=jlq$RT+m5eXo&jc2NlR{tArri;AtUj_y?G%lWdDWlq1NrH$n(vYG6! z$Wp?*rpcZaQLA>{Q2$SA4)-t;1Xa;53r{#8j-CeGexu}E64LW}jsp=^7-4=636deT zeyt{Q)fGGy$RfZr`6)>gf@hhRg~yTe0lKX9+zfe>69&O^z0LAG^|n*C$}j^^lVHL)fS7g)lJO5)gte698Q0(0*{3xQkfM6*2T}vt}D}UH6XB38%JMmll>>V6HAMFl! zdv`a`!lYx^8KpwwwOpB7r$Yh*%N_7$A=ut^7XK$Q{mZU4yg+owgiB*)$zlD zc#&MILr4UaNm(x7gc=9@_T2jO3&dx_!B^n<6vrlB{B--4eruom&Y}psRH}qB@66Fy zT%VhtpCS79Pe=x(xhK{>7Wf6n$NK?K>v?+otyB}>)EAF{Tx2s`bd@vJN-;Vz(*F3s zX(SSdPR>H0f=Q9W>Mt(EY+-tO`gW*~Iqv)`O)WvSo0akZwPfXVW%O7*^c zFUY{`F)FI60O62IBg_BXO8WIgNHN-5;FWn3eTjg8G^i2%mj(yt#J09FOO3n261cC-F9 z%)ElD(BE?4Sup(&|5pL~0K2fW6Q7hM8y+1Lemw$=cYc1pLq`e*y#;q#i2+Kk*U=uZ zjCbY%Y_5#64$wX@)gF=H(+uk!FRPoIo9__vdFn6|$IB!B*a z5UwsKF59HPrnFtZ&Q^&(GyIKCU(ZX}a-xV%K2RZS^IpF;2g3r~JowAbZhL6u^OyX4 z9VxZ&j=W(2t_KduCyZsM`)4<*1P>UDrSs?n@m%GaWME_j!a9)GM~YLfVy5~BqE!yA zgq>yX+*`QhIkWF@>plVFb)t0hwD)0^cW^e#5Z4g?eS9xyTpR+JuDGW>ls#^?&lxo1 zQq>UE->RvvudlA=A;gXb_C|p1ya2deKQYo$r+}?GOdu&EV`=Bq-bDc&b zBgO_PW5FbnpgWzvbm?*Ango~S_SJTSP9$-xIuc%bld1jf1#(<7D+-$5CW_STn@KI; z^0-cf0#1*oP6dFk5LURiuHBkWahWsYtbvOM1mvHrcf)&n>=H+6I-pfM4arOal+o6k z%VWXp9Y$m>=Ch!|0DA-M!A!clb~7!2II}uxdtP!pSNgT8m62(*m-O{ZT_Kvcz;@LA z>h1s`^F5hH4s38TA$2Mgc-+oi`=tyA)Z`7fH17Z)9RW=C3eevC+sw?sla#XLGt>q+ zJYQv3f-8-<-$O_Qebh64J>_I4XLI&l=N6#2kN11LW5&S3tTPyeaug@~i z8I7>bR4prQodD0y=c3J1{}6<6+8)Oj8{TUg}1Hvk3HyzYt>;gl!2ouXRm zBV0;;h1HI=fT_I4-|C(^uMyq5Ca{4+$zNSdY860i0MKB96FE9m|mdj3j^CM9N*y&C0ljK zb|s=vFRFkgZclj|2ZM{M! zm>O2w7U%^&4h;pZ$qOA1YM5NA;w#;J97#l-zeiA>Q7lgpm+ZbwZxqe>(9U?Dvpp!H;xO!UNi!HjvH)n4 zeqRk%bcrK2ooSbbL!GmuGC43-%hO+jP{~Jx?iJQf&&%%tkR8CX{<9VPxV9#j=9-k9 z7jPCQW<@2tnU|YGL*~E!+wSl{Trh!+i;ETjdVj}0uzf}p$ zPv}o(U?@m~tgyxy_X)-C9mBZ3JpXU`4E#gB8u$(eRiU{BS^H-lLbZJ+unAs@aMBO$ z3(9V7#u*kuu8#QRyRU&qRjXt1qZI?)5HcU$HLrif{YWzy1O0#WZ2xr^Iv%3t2>~Kc z1G_1PS2^!{uLBYB{Zl&x!Mph{f4O;1oi-|ARu0B1eJ|z#R5H@%J?uG1@Y!oB-}!6R z|L?l6?`Nl@Pmij-Ec5#N+CWgTVnj=_&RRt@Aa4u5{FlFM3AeU6-K3q6K$5vGC_eRaOMbv9Q;Qge}*OyR#U%l|i*1X-sD@18}? z|G{$nY4Lp29iJSAf8V+17RrDBLf}7SrMl2K#cKW&!+$@Gf@fLl3294l)K&!&M+@E+ zCP={T@5fmF_Xm4#{W6OCqF$7#_0)%)A7T-`xa6D<5q`5&nFqM)fa??1M34Y(@N*zR z-pM>a&XTTf12`nOD&*Sa&&R&BuEK`2B>w%08Basp@4t`sPmUx%m8`0g{_$XN7&*$C ziUi{0Du#?6fDH~U`;cphJ;efdH(n}8YxDyrmp`EoDd>xWQ_Z$0kLg&?PCQpXmmGBl^ZMU#p!(nal&7Z{ub{z)Os=P>;HM#y;&S(jMD>0D7nVFM A{Qv*} literal 0 HcmV?d00001 diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.uxf b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.uxf new file mode 100644 index 0000000000..9707b26ea5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.uxf @@ -0,0 +1,215 @@ + + // Uncomment the following line to change the fontsize and font: +fontsize=10 +// fontfamily=SansSerif //possible: SansSerif,Serif,Monospaced + + +////////////////////////////////////////////////////////////////////////////////////////////// +// Welcome to UMLet! +// +// Double-click on elements to add them to the diagram, or to copy them +// Edit elements by modifying the text in this panel +// Hold Ctrl to select multiple elements +// Use Ctrl+mouse to select via lasso +// +// Use +/- or Ctrl+mouse wheel to zoom +// Drag a whole relation at its central square icon +// +// Press Ctrl+C to copy the whole diagram to the system clipboard (then just paste it to, eg, Word) +// Edit the files in the "palettes" directory to create your own element palettes +// +// Select "Custom Elements > New..." to create new element types +////////////////////////////////////////////////////////////////////////////////////////////// + + +// This text will be stored with each diagram; use it for notes. + 12 + + UMLGeneric + + 252 + 96 + 168 + 36 + + _:ExampleTestClass_ + + + + Relation + + 324 + 120 + 36 + 408 + + lt=. +layer=-10 + 10.0;10.0;10.0;320.0 + + + UMLGeneric + + 708 + 96 + 144 + 36 + + _:TestDataLoader_ + + + + Relation + + 768 + 120 + 36 + 396 + + lt=. +layer=-10 + 10.0;10.0;10.0;310.0 + + + UMLGeneric + + 324 + 156 + 24 + 312 + + layer=10 +transparency=0 + + + + UMLSpecialState + + 132 + 144 + 24 + 24 + + type=initial + + + + Relation + + 144 + 132 + 204 + 48 + + lt=<- +myAwesomeTest() + 150.0;20.0;10.0;20.0 + + + UMLGeneric + + 768 + 204 + 24 + 84 + + layer=10 +transparency=0 + + + + Relation + + 336 + 180 + 456 + 48 + + lt=<- +getPtpCart() : PtpCart + 360.0;20.0;10.0;20.0 + + + UMLGeneric + + 612 + 360 + 24 + 24 + + layer=10 +transparency=0 + + + + Relation + + 348 + 336 + 288 + 48 + + lt=<- +toRequest() : +moveit_msgs::MotionPlanRequest + 220.0;20.0;10.0;20.0 + + + UMLGeneric + + 576 + 252 + 96 + 36 + + _:PtpCart_ + + + + Relation + + 612 + 276 + 36 + 216 + + lt=. + 10.0;10.0;10.0;160.0 + + + Relation + + 660 + 252 + 132 + 36 + + lt=<- + 10.0;10.0;90.0;10.0 + + + UMLGeneric + + 336 + 324 + 24 + 96 + + layer=100 +transparency=0 + + + + + Relation + + 336 + 276 + 204 + 72 + + lt=<- +doAwesomeThings() + 20.0;40.0;50.0;40.0;50.0;10.0;10.0;10.0 + + diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h new file mode 100644 index 0000000000..4b408e2bd8 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h @@ -0,0 +1,176 @@ +/* + * Copyright (c) 2018 Pilz GmbH & Co. KG + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ASYNC_TEST_H +#define ASYNC_TEST_H + +#include +#include +#include +#include +#include + +#include + +#include + +namespace testing +{ +/** + * @brief Test class that allows the handling of asynchronous test objects + * + * The class provides the two basic functions AsyncTest::barricade and AsyncTest::triggerClearEvent. + * During the test setup gates between the steps with one or more clear events. Allow passing on by calling + * triggerClearEvent after a test. + * + * \e Usage:
    + * Suppose you want to test a function that calls another function asynchronously, like the following example: + * + * \code + * void asyncCall(std::function fun) + * { + * std::thread t(fun); + * t.detach(); + * } + * \endcode + * + * You expect that fun gets called, so your test thread has to wait for the completion, else it would fail. This can be + * achieved via: + * + * \code + * class MyTest : public testing::Test, public testing::AsyncTest + * { + * public: + * MOCK_METHOD0(myMethod, void()); + * }; + * + * TEST_F(MyTest, testCase) + * { + * EXPECT_CALL(*this, myMethod()).Times(1).WillOnce(ACTION_OPEN_BARRIER_VOID("myMethod")); + * const int timeout_ms {100}; + * asyncCall(std::bind(&MyTest::myMethod, this)); + * BARRIER("myMethod", timeout_ms) << "Timed-out waiting for myMethod call."; + * } + * \endcode + */ +class AsyncTest +{ +public: + /** + * @brief Triggeres a clear event. If a call to barricade is currently pending it will unblock as soon as all clear + * events are triggered. Else the event is put on the waitlist. This waitlist is emptied upon a call to barricade. + * + * @param event The event that is triggered + */ + void triggerClearEvent(const std::string& event); + + /** + * @brief Will block until the event given by clear_event is triggered or a timeout is reached. + * Unblocks immediately, if the event was on the waitlist. + * + * @param clear_event Event that allows the test to pass on + * @param timeout_ms Timeout [ms] (optional). + * @return True if the event was triggered, false otherwise. + */ + bool barricade(const std::string& clear_event, const int timeout_ms = -1); + + /** + * @brief Will block until all events given by clear_events are triggered or a timeout is reached. + * Events on the waitlist are taken into account, too. + * + * @param clear_events List of events that allow the test to pass on + * @param timeout_ms Timeout [ms] (optional). + * @return True if all events were triggered, false otherwise. + */ + bool barricade(std::initializer_list clear_events, const int timeout_ms = -1); + +protected: + std::mutex m_; + std::condition_variable cv_; + std::set clear_events_{}; + std::set waitlist_{}; +}; + +// for better readability in tests +#define BARRIER(...) EXPECT_TRUE(barricade(__VA_ARGS__)) +#define BARRIER_FATAL(...) ASSERT_TRUE(barricade(__VA_ARGS__)) + +#define ACTION_OPEN_BARRIER(str) \ + ::testing::InvokeWithoutArgs([this](void) { \ + this->triggerClearEvent(str); \ + return true; \ + }) +#define ACTION_OPEN_BARRIER_VOID(str) ::testing::InvokeWithoutArgs([this](void) { this->triggerClearEvent(str); }) + +inline void AsyncTest::triggerClearEvent(const std::string& event) +{ + std::lock_guard lk(m_); + + if (clear_events_.empty()) + { + ROS_DEBUG_NAMED("Test", "Clearing Barricade[%s]", event.c_str()); + waitlist_.insert(event); + } + else if (clear_events_.erase(event) < 1) + { + ROS_WARN_STREAM("Triggered event " << event << " despite not waiting for it."); + } + cv_.notify_one(); +} + +inline bool AsyncTest::barricade(const std::string& clear_event, const int timeout_ms) +{ + return barricade({ clear_event }, timeout_ms); +} + +inline bool AsyncTest::barricade(std::initializer_list clear_events, const int timeout_ms) +{ + std::unique_lock lk(m_); + + std::stringstream events_stringstream; + for (const auto& event : clear_events) + { + events_stringstream << event << ", "; + } + ROS_DEBUG_NAMED("Test", "Adding Barricade[%s]", events_stringstream.str().c_str()); + + std::copy_if(clear_events.begin(), clear_events.end(), std::inserter(clear_events_, clear_events_.end()), + [this](std::string event) { return this->waitlist_.count(event) == 0; }); + waitlist_.clear(); + + auto end_time_point = std::chrono::system_clock::now() + std::chrono::milliseconds(timeout_ms); + + while (!clear_events_.empty()) + { + if (timeout_ms < 0) + { + cv_.wait(lk); + } + else + { + std::cv_status status = cv_.wait_for(lk, end_time_point - std::chrono::system_clock::now()); + if (status == std::cv_status::timeout) + { + return clear_events_.empty(); + } + } + } + return true; +} + +} // namespace testing + +#endif // ASYNC_TEST_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h new file mode 100644 index 0000000000..8b1e0382ea --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h @@ -0,0 +1,128 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef BASECMD_H +#define BASECMD_H + +#include + +#include "motioncmd.h" + +namespace pilz_industrial_motion_planner_testutils +{ +template +class BaseCmd : public MotionCmd +{ +public: + BaseCmd() : MotionCmd() + { + } + + virtual ~BaseCmd() = default; + +public: + planning_interface::MotionPlanRequest toRequest() const override; + + void setStartConfiguration(StartType start); + void setGoalConfiguration(GoalType goal); + + StartType& getStartConfiguration(); + const StartType& getStartConfiguration() const; + + GoalType& getGoalConfiguration(); + const GoalType& getGoalConfiguration() const; + +private: + virtual std::string getPlannerId() const = 0; + +protected: + GoalType goal_; + StartType start_; +}; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +template +inline void BaseCmd::setStartConfiguration(StartType start) +{ + start_ = start; +} + +template +inline void BaseCmd::setGoalConfiguration(GoalType goal) +{ + goal_ = goal; +} + +template +inline StartType& BaseCmd::getStartConfiguration() +{ + return start_; +} + +template +inline const StartType& BaseCmd::getStartConfiguration() const +{ + return start_; +} + +template +inline GoalType& BaseCmd::getGoalConfiguration() +{ + return goal_; +} + +template +inline const GoalType& BaseCmd::getGoalConfiguration() const +{ + return goal_; +} + +template +planning_interface::MotionPlanRequest BaseCmd::toRequest() const +{ + planning_interface::MotionPlanRequest req; + req.planner_id = getPlannerId(); + req.group_name = this->planning_group_; + + req.max_velocity_scaling_factor = this->vel_scale_; + req.max_acceleration_scaling_factor = this->acc_scale_; + + req.start_state = this->start_.toMoveitMsgsRobotState(); + req.goal_constraints.push_back(this->goal_.toGoalConstraints()); + + return req; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // BASECMD_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h new file mode 100644 index 0000000000..b1c9dbeb53 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h @@ -0,0 +1,188 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef CARTESIANCONFIGURATION_H +#define CARTESIANCONFIGURATION_H + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "robotconfiguration.h" +#include "jointconfiguration.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Class to define a robot configuration in space + * with the help of cartesian coordinates. + */ +class CartesianConfiguration : public RobotConfiguration +{ +public: + CartesianConfiguration(); + + CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector& config); + + CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector& config, + const moveit::core::RobotModelConstPtr& robot_model); + +public: + moveit_msgs::Constraints toGoalConstraints() const override; + moveit_msgs::RobotState toMoveitMsgsRobotState() const override; + + void setLinkName(const std::string& link_name); + const std::string& getLinkName() const; + + void setPose(const geometry_msgs::Pose& pose); + const geometry_msgs::Pose& getPose() const; + geometry_msgs::Pose& getPose(); + + void setSeed(const JointConfiguration& config); + const JointConfiguration& getSeed() const; + //! @brief States if a seed for the cartesian configuration is set. + bool hasSeed() const; + + void setPoseTolerance(const double tol); + const boost::optional getPoseTolerance() const; + + void setAngleTolerance(const double tol); + const boost::optional getAngleTolerance() const; + +private: + static geometry_msgs::Pose toPose(const std::vector& pose); + static geometry_msgs::PoseStamped toStampedPose(const geometry_msgs::Pose& pose); + +private: + std::string link_name_; + geometry_msgs::Pose pose_; + + //! @brief The dimensions of the sphere associated with the target region + //! of the position constraint. + boost::optional tolerance_pose_{ boost::none }; + + //! @brief The value to assign to the absolute tolerances of the + //! orientation constraint. + boost::optional tolerance_angle_{ boost::none }; + + //! @brief The seed for computing the IK solution of the cartesian configuration. + boost::optional seed_{ boost::none }; +}; + +std::ostream& operator<<(std::ostream& /*os*/, const CartesianConfiguration& /*obj*/); + +inline void CartesianConfiguration::setLinkName(const std::string& link_name) +{ + link_name_ = link_name; +} + +inline const std::string& CartesianConfiguration::getLinkName() const +{ + return link_name_; +} + +inline void CartesianConfiguration::setPose(const geometry_msgs::Pose& pose) +{ + pose_ = pose; +} + +inline const geometry_msgs::Pose& CartesianConfiguration::getPose() const +{ + return pose_; +} + +inline geometry_msgs::Pose& CartesianConfiguration::getPose() +{ + return pose_; +} + +inline moveit_msgs::Constraints CartesianConfiguration::toGoalConstraints() const +{ + if (!tolerance_pose_ || !tolerance_angle_) + { + return kinematic_constraints::constructGoalConstraints(link_name_, toStampedPose(pose_)); + } + else + { + return kinematic_constraints::constructGoalConstraints(link_name_, toStampedPose(pose_), tolerance_pose_.value(), + tolerance_angle_.value()); + } +} + +inline void CartesianConfiguration::setSeed(const JointConfiguration& config) +{ + seed_ = config; +} + +inline const JointConfiguration& CartesianConfiguration::getSeed() const +{ + return seed_.value(); +} + +inline bool CartesianConfiguration::hasSeed() const +{ + return seed_.is_initialized(); +} + +inline void CartesianConfiguration::setPoseTolerance(const double tol) +{ + tolerance_pose_ = tol; +} + +inline const boost::optional CartesianConfiguration::getPoseTolerance() const +{ + return tolerance_pose_; +} + +inline void CartesianConfiguration::setAngleTolerance(const double tol) +{ + tolerance_angle_ = tol; +} + +inline const boost::optional CartesianConfiguration::getAngleTolerance() const +{ + return tolerance_angle_; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CARTESIANCONFIGURATION_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h new file mode 100644 index 0000000000..c30ce98360 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef CARTESIANPATHCONSTRAINTSBUILDER_H +#define CARTESIANPATHCONSTRAINTSBUILDER_H + +#include + +#include + +#include "cartesianconfiguration.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Helper class to build moveit_msgs::Constraints from a + * given configuration. + */ +class CartesianPathConstraintsBuilder +{ +public: + CartesianPathConstraintsBuilder& setConstraintName(const std::string& constraint_name); + CartesianPathConstraintsBuilder& setConfiguration(const CartesianConfiguration& configuration); + + moveit_msgs::Constraints toPathConstraints() const; + +private: + std::string constraint_name_; + CartesianConfiguration configuration_; +}; + +inline CartesianPathConstraintsBuilder& +CartesianPathConstraintsBuilder::setConstraintName(const std::string& constraint_name) +{ + constraint_name_ = constraint_name; + return *this; +} + +inline CartesianPathConstraintsBuilder& +CartesianPathConstraintsBuilder::setConfiguration(const CartesianConfiguration& configuration) +{ + configuration_ = configuration; + return *this; +} + +inline moveit_msgs::Constraints CartesianPathConstraintsBuilder::toPathConstraints() const +{ + moveit_msgs::PositionConstraint pos_constraint; + pos_constraint.link_name = configuration_.getLinkName(); + pos_constraint.constraint_region.primitive_poses.push_back(configuration_.getPose()); + + moveit_msgs::Constraints path_constraints; + path_constraints.name = constraint_name_; + path_constraints.position_constraints.push_back(pos_constraint); + return path_constraints; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CARTESIANPATHCONSTRAINTSBUILDER_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.h new file mode 100644 index 0000000000..3e7d7f8a55 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.h @@ -0,0 +1,60 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef CENTERAUXILIARY_H +#define CENTERAUXILIARY_H + +#include "circauxiliary.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Class to define the center point of the circle + * on which the robot is supposed to move via circ command. + */ +template +class Center : public CircAuxiliary +{ +private: + std::string getConstraintName() const override; +}; + +template +std::string Center::getConstraintName() const +{ + return "center"; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CENTERAUXILIARY_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h new file mode 100644 index 0000000000..e9519f2071 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h @@ -0,0 +1,62 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef CHECKS_H +#define CHECKS_H + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +::testing::AssertionResult isAtExpectedPosition(const robot_state::RobotState& expected, + const robot_state::RobotState& actual, const double epsilon, + const std::string& group_name = "") +{ + if (group_name.empty() && expected.distance(actual) <= epsilon) + { + return ::testing::AssertionSuccess(); + } + else if (!group_name.empty() && expected.distance(actual, actual.getJointModelGroup(group_name)) <= epsilon) + { + return ::testing::AssertionSuccess(); + } + std::stringstream msg; + msg << " expected: " << expected << " actual: " << actual; + + return ::testing::AssertionFailure() << msg.str(); +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CENTERAUXILIARY_H \ No newline at end of file diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.h new file mode 100644 index 0000000000..1888cfbffb --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.h @@ -0,0 +1,106 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef CIRC_H +#define CIRC_H + +#include + +#include "basecmd.h" +#include "circauxiliary.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Data class storing all information regarding a Circ command. + */ +template +class Circ : public BaseCmd +{ +public: + Circ() : BaseCmd() + { + } + +public: + void setAuxiliaryConfiguration(AuxiliaryType auxiliary); + AuxiliaryType& getAuxiliaryConfiguration(); + const AuxiliaryType& getAuxiliaryConfiguration() const; + +public: + planning_interface::MotionPlanRequest toRequest() const override; + +private: + std::string getPlannerId() const override; + +private: + AuxiliaryType auxiliary_; +}; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +template +inline void Circ::setAuxiliaryConfiguration(AuxiliaryType auxiliary) +{ + auxiliary_ = auxiliary; +} + +template +inline std::string Circ::getPlannerId() const +{ + return "CIRC"; +} + +template +inline planning_interface::MotionPlanRequest Circ::toRequest() const +{ + planning_interface::MotionPlanRequest req{ BaseCmd::toRequest() }; + req.path_constraints = auxiliary_.toPathConstraints(); + + return req; +} + +template +inline AuxiliaryType& Circ::getAuxiliaryConfiguration() +{ + return auxiliary_; +} + +template +inline const AuxiliaryType& Circ::getAuxiliaryConfiguration() const +{ + return auxiliary_; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CIRC_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.h new file mode 100644 index 0000000000..3fbe7b7b7d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.h @@ -0,0 +1,49 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef CIRC_AUXILIARY_TYPES_H +#define CIRC_AUXILIARY_TYPES_H + +#include "center.h" +#include "interim.h" +#include "cartesianconfiguration.h" +#include "cartesianpathconstraintsbuilder.h" + +namespace pilz_industrial_motion_planner_testutils +{ +using CartesianCenter = Center; +using CartesianInterim = Interim; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CIRC_AUXILIARY_TYPES_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h new file mode 100644 index 0000000000..81958f9b67 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h @@ -0,0 +1,91 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef CIRCAUXILIARY_H +#define CIRCAUXILIARY_H + +#include + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Base class to define an auxiliary point needed to specify + * circ commands. + */ +template +class CircAuxiliary +{ +public: + void setConfiguration(const ConfigType& auxiliary_config); + ConfigType& getConfiguration(); + const ConfigType& getConfiguration() const; + +public: + moveit_msgs::Constraints toPathConstraints() const; + +private: + virtual std::string getConstraintName() const = 0; + +protected: + ConfigType auxiliary_config_; +}; + +template +void CircAuxiliary::setConfiguration(const ConfigType& auxiliary_config) +{ + auxiliary_config_ = auxiliary_config; +} + +template +inline ConfigType& CircAuxiliary::getConfiguration() +{ + return auxiliary_config_; +} + +template +inline const ConfigType& CircAuxiliary::getConfiguration() const +{ + return auxiliary_config_; +} + +template +inline moveit_msgs::Constraints CircAuxiliary::toPathConstraints() const +{ + return BuilderType().setConstraintName(getConstraintName()).setConfiguration(getConfiguration()).toPathConstraints(); +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CIRCAUXILIARY_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h new file mode 100644 index 0000000000..6bd4356637 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h @@ -0,0 +1,69 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef COMMAND_TYPES_TYPEDEF_H +#define COMMAND_TYPES_TYPEDEF_H + +#include + +#include "ptp.h" +#include "lin.h" +#include "circ.h" +#include "gripper.h" +#include "jointconfiguration.h" +#include "cartesianconfiguration.h" +#include "circ_auxiliary_types.h" + +namespace pilz_industrial_motion_planner_testutils +{ +typedef Ptp PtpJoint; +typedef Ptp PtpJointCart; +typedef Ptp PtpCart; + +typedef Lin LinJoint; +typedef Lin LinJointCart; +typedef Lin LinCart; + +typedef Circ CircCenterCart; +typedef Circ CircInterimCart; + +typedef Circ CircJointCenterCart; +typedef Circ CircJointInterimCart; + +typedef boost::variant + CmdVariant; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // COMMAND_TYPES_TYPEDEF_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.h new file mode 100644 index 0000000000..ddded20bf3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.h @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef DEFAULT_VALUES_H +#define DEFAULT_VALUES_H + +/* + * @brief This file contains all default values needed for testing. + */ +namespace pilz_industrial_motion_planner_testutils +{ +static constexpr double DEFAULT_VEL{ 0.01 }; +static constexpr double DEFAULT_ACC{ 0.01 }; +static constexpr double DEFAULT_BLEND_RADIUS{ 0.01 }; + +static constexpr double DEFAULT_VEL_GRIPPER{ 0.5 }; +static constexpr double DEFAULT_ACC_GRIPPER{ 0.8 }; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // DEFAULT_VALUES_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.h new file mode 100644 index 0000000000..dec173bbc1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.h @@ -0,0 +1,52 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef EXCEPTION_TYPES_H +#define EXCEPTION_TYPES_H + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +class TestDataLoaderReadingException : public std::runtime_error +{ +public: + TestDataLoaderReadingException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // EXCEPTION_TYPES_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h new file mode 100644 index 0000000000..1595570449 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h @@ -0,0 +1,56 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef GOALCONSTRAINTSMSGCONVERTIBLE_H +#define GOALCONSTRAINTSMSGCONVERTIBLE_H + +#include + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Interface class to express that a derived class can be converted + * into a moveit_msgs::Constaints. + */ +class GoalConstraintMsgConvertible +{ +public: + virtual moveit_msgs::Constraints toGoalConstraints() const = 0; +}; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // GOALCONSTRAINTSMSGCONVERTIBLE_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.h new file mode 100644 index 0000000000..b3d0f2fd5a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.h @@ -0,0 +1,48 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef GRIPPER_H +#define GRIPPER_H + +#include "ptp.h" +#include "jointconfiguration.h" + +namespace pilz_industrial_motion_planner_testutils +{ +class Gripper : public Ptp +{ +}; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // GRIPPER_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.h new file mode 100644 index 0000000000..d6822ab43a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.h @@ -0,0 +1,60 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef INTERIMAXILIARY_H +#define INTERIMAXILIARY_H + +#include "circauxiliary.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Class to define a point on the circle on which the robot is supposed + * to move via circ command. + */ +template +class Interim : public CircAuxiliary +{ +private: + std::string getConstraintName() const override; +}; + +template +std::string Interim::getConstraintName() const +{ + return "interim"; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // INTERIMAXILIARY_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h new file mode 100644 index 0000000000..b133d568c2 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h @@ -0,0 +1,145 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef JOINTCONFIGURATION_H +#define JOINTCONFIGURATION_H + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "robotconfiguration.h" + +namespace pilz_industrial_motion_planner_testutils +{ +class JointConfigurationException : public std::runtime_error +{ +public: + JointConfigurationException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; + +using CreateJointNameFunc = std::function; + +/** + * @brief Class to define a robot configuration in space with the help + * of joint values. + */ +class JointConfiguration : public RobotConfiguration +{ +public: + JointConfiguration(); + + JointConfiguration(const std::string& group_name, const std::vector& config, + CreateJointNameFunc&& create_joint_name_func); + + JointConfiguration(const std::string& group_name, const std::vector& config, + const moveit::core::RobotModelConstPtr& robot_model); + +public: + void setJoint(const size_t index, const double value); + double getJoint(const size_t index) const; + const std::vector getJoints() const; + + size_t size() const; + + moveit_msgs::Constraints toGoalConstraints() const override; + moveit_msgs::RobotState toMoveitMsgsRobotState() const override; + + sensor_msgs::JointState toSensorMsg() const; + + robot_state::RobotState toRobotState() const; + + void setCreateJointNameFunc(CreateJointNameFunc create_joint_name_func); + +private: + moveit_msgs::RobotState toMoveitMsgsRobotStateWithoutModel() const; + moveit_msgs::RobotState toMoveitMsgsRobotStateWithModel() const; + + moveit_msgs::Constraints toGoalConstraintsWithoutModel() const; + moveit_msgs::Constraints toGoalConstraintsWithModel() const; + +private: + //! Joint positions + std::vector joints_; + + CreateJointNameFunc create_joint_name_func_; +}; + +std::ostream& operator<<(std::ostream& /*os*/, const JointConfiguration& /*obj*/); + +inline moveit_msgs::Constraints JointConfiguration::toGoalConstraints() const +{ + return robot_model_ ? toGoalConstraintsWithModel() : toGoalConstraintsWithoutModel(); +} + +inline moveit_msgs::RobotState JointConfiguration::toMoveitMsgsRobotState() const +{ + return robot_model_ ? toMoveitMsgsRobotStateWithModel() : toMoveitMsgsRobotStateWithoutModel(); +} + +inline void JointConfiguration::setJoint(const size_t index, const double value) +{ + joints_.at(index) = value; +} + +inline double JointConfiguration::getJoint(const size_t index) const +{ + return joints_.at(index); +} + +inline const std::vector JointConfiguration::getJoints() const +{ + return joints_; +} + +inline size_t JointConfiguration::size() const +{ + return joints_.size(); +} + +inline void JointConfiguration::setCreateJointNameFunc(CreateJointNameFunc create_joint_name_func) +{ + create_joint_name_func_ = std::move(create_joint_name_func); +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // JOINTCONFIGURATION_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.h new file mode 100644 index 0000000000..56e74a51d3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.h @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef LIN_H +#define LIN_H + +#include + +#include "basecmd.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Data class storing all information regarding a linear command. + */ +template +class Lin : public BaseCmd +{ +public: + Lin() : BaseCmd() + { + } + +private: + std::string getPlannerId() const override; +}; + +// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +template +inline std::string Lin::getPlannerId() const +{ + return "LIN"; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // LIN_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.h new file mode 100644 index 0000000000..f54beb0c56 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.h @@ -0,0 +1,93 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef MOTIONCMD_H +#define MOTIONCMD_H + +#include +#include + +#include "motionplanrequestconvertible.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Base class for commands storing all general information of a command. + */ +class MotionCmd : public MotionPlanRequestConvertible +{ +public: + MotionCmd() : MotionPlanRequestConvertible() + { + } + +public: + void setPlanningGroup(const std::string& planning_group); + const std::string& getPlanningGroup() const; + + void setVelocityScale(double velocity_scale); + void setAccelerationScale(double acceleration_scale); + +protected: + std::string planning_group_; + //! Link to which all cartesian poses refer to. + std::string target_link_; + double vel_scale_{ 1.0 }; + double acc_scale_{ 1.0 }; +}; + +inline void MotionCmd::setPlanningGroup(const std::string& planning_group) +{ + planning_group_ = planning_group; +} + +inline const std::string& MotionCmd::getPlanningGroup() const +{ + return planning_group_; +} + +inline void MotionCmd::setVelocityScale(double velocity_scale) +{ + vel_scale_ = velocity_scale; +} + +inline void MotionCmd::setAccelerationScale(double acceleration_scale) +{ + acc_scale_ = acceleration_scale; +} + +using MotionCmdUPtr = std::unique_ptr; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // MOTIONCMD_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.h new file mode 100644 index 0000000000..803fc499fa --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.h @@ -0,0 +1,54 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef MOTIONPLANREQUESTCONVERTIBLE_H +#define MOTIONPLANREQUESTCONVERTIBLE_H + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Interface class to express that a derived class can be converted + * into a planning_interface::MotionPlanRequest. + */ +class MotionPlanRequestConvertible +{ +public: + virtual planning_interface::MotionPlanRequest toRequest() const = 0; +}; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // MOTIONPLANREQUESTCONVERTIBLE_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.h new file mode 100644 index 0000000000..cc7c2ce228 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.h @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef PTP_H +#define PTP_H + +#include + +#include "basecmd.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Data class storing all information regarding a Ptp command. + */ +template +class Ptp : public BaseCmd +{ +public: + Ptp() : BaseCmd() + { + } + +private: + std::string getPlannerId() const override; +}; + +// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +template +inline std::string Ptp::getPlannerId() const +{ + return "PTP"; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // PTP_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.h new file mode 100644 index 0000000000..433146dcfb --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.h @@ -0,0 +1,92 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef ROBOTCONFIGURATION_H +#define ROBOTCONFIGURATION_H + +#include +#include + +#include + +#include "goalconstraintsmsgconvertible.h" +#include "robotstatemsgconvertible.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Class to define robot configuration in space. + */ +class RobotConfiguration : public RobotStateMsgConvertible, public GoalConstraintMsgConvertible +{ +public: + RobotConfiguration(); + + RobotConfiguration(const std::string& group_name); + + RobotConfiguration(const std::string& group_name, const moveit::core::RobotModelConstPtr& robot_model); + +public: + void setRobotModel(moveit::core::RobotModelConstPtr robot_model); + void setGroupName(const std::string& group_name); + std::string getGroupName() const; + void clearModel(); + +protected: + std::string group_name_; + moveit::core::RobotModelConstPtr robot_model_; +}; + +inline void RobotConfiguration::setRobotModel(moveit::core::RobotModelConstPtr robot_model) +{ + robot_model_ = std::move(robot_model); +} + +inline void RobotConfiguration::setGroupName(const std::string& group_name) +{ + group_name_ = group_name; +} + +inline std::string RobotConfiguration::getGroupName() const +{ + return group_name_; +} + +inline void RobotConfiguration::clearModel() +{ + robot_model_ = nullptr; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // ROBOTCONFIGURATION_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h new file mode 100644 index 0000000000..4b9ba87df4 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h @@ -0,0 +1,54 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef ROBOTSTATEMSGCONVERTIBLE_H +#define ROBOTSTATEMSGCONVERTIBLE_H + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Interface class to express that a derived class can be converted + * into a moveit_msgs::RobotState. + */ +class RobotStateMsgConvertible +{ +public: + virtual moveit_msgs::RobotState toMoveitMsgsRobotState() const = 0; +}; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // ROBOTSTATEMSGCONVERTIBLE_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h new file mode 100644 index 0000000000..e555b8405d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h @@ -0,0 +1,147 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef SEQUENCE_H +#define SEQUENCE_H + +#include +#include +#include +#include + +#include + +#include "command_types_typedef.h" +#include "motioncmd.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Data class storing all information regarding a Sequence command. + */ +class Sequence +{ +public: + /** + * @brief Adds a command to the end of the sequence. + * @param cmd The command which has to be added. + */ + void add(const CmdVariant& cmd, const double blend_radius = 0.); + + /** + * @brief Returns the number of commands. + */ + size_t size() const; + + template + T& getCmd(const size_t index_cmd); + + template + const T& getCmd(const size_t index_cmd) const; + + /** + * @return TRUE if the specified command is of the specified type, + * otherwise FALSE. + */ + template + bool cmdIsOfType(const size_t index_cmd) const; + + /** + * @brief Returns the specific command as base class reference. + * This function allows the user to operate on the sequence without + * having knowledge of the underlying specific command type. + */ + MotionCmd& getCmd(const size_t index_cmd); + + void setAllBlendRadiiToZero(); + void setBlendRadius(const size_t index_cmd, const double blend_radius); + double getBlendRadius(const size_t index_cmd) const; + + /** + * @brief Deletes all commands from index 'start' to index 'end'. + */ + void erase(const size_t start, const size_t end); + + moveit_msgs::MotionSequenceRequest toRequest() const; + +private: + using TCmdRadiiPair = std::pair; + std::vector cmds_; +}; + +inline void Sequence::add(const CmdVariant& cmd, const double blend_radius) +{ + cmds_.emplace_back(cmd, blend_radius); +} + +inline size_t Sequence::size() const +{ + return cmds_.size(); +} + +template +inline T& Sequence::getCmd(const size_t index_cmd) +{ + return boost::get(cmds_.at(index_cmd).first); +} + +template +inline const T& Sequence::getCmd(const size_t index_cmd) const +{ + return boost::get(cmds_.at(index_cmd).first); +} + +inline double Sequence::getBlendRadius(const size_t index_cmd) const +{ + return cmds_.at(index_cmd).second; +} + +inline void Sequence::setBlendRadius(const size_t index_cmd, const double blend_radius) +{ + cmds_.at(index_cmd).second = blend_radius; +} + +inline void Sequence::setAllBlendRadiiToZero() +{ + std::for_each(cmds_.begin(), cmds_.end(), [](TCmdRadiiPair& cmd) { cmd.second = 0.; }); +} + +template +inline bool Sequence::cmdIsOfType(const size_t index_cmd) const +{ + return cmds_.at(index_cmd).first.type() == typeid(T); +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // SEQUENCE_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h new file mode 100644 index 0000000000..5b2f550bfb --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h @@ -0,0 +1,117 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef TESTDATA_LOADER_H +#define TESTDATA_LOADER_H + +#include +#include + +#include + +#include "jointconfiguration.h" +#include "cartesianconfiguration.h" +#include "command_types_typedef.h" +#include "sequence.h" +#include "gripper.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Abstract base class describing the interface to access + * test data like robot poses and robot commands. + */ +class TestdataLoader +{ +public: + TestdataLoader() = default; + + TestdataLoader(moveit::core::RobotModelConstPtr robot_model) : robot_model_(std::move(robot_model)) + { + } + + virtual ~TestdataLoader() = default; + +public: + void setRobotModel(moveit::core::RobotModelConstPtr robot_model); + + virtual JointConfiguration getJoints(const std::string& pos_name, const std::string& group_name) const = 0; + + virtual CartesianConfiguration getPose(const std::string& pos_name, const std::string& group_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual PtpJoint getPtpJoint(const std::string& cmd_name) const = 0; + virtual PtpCart getPtpCart(const std::string& cmd_name) const = 0; + virtual PtpJointCart getPtpJointCart(const std::string& cmd_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual LinJoint getLinJoint(const std::string& cmd_name) const = 0; + virtual LinCart getLinCart(const std::string& cmd_name) const = 0; + virtual LinJointCart getLinJointCart(const std::string& cmd_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual CircCenterCart getCircCartCenterCart(const std::string& cmd_name) const = 0; + virtual CircJointCenterCart getCircJointCenterCart(const std::string& cmd_name) const = 0; + virtual CircInterimCart getCircCartInterimCart(const std::string& cmd_name) const = 0; + virtual CircJointInterimCart getCircJointInterimCart(const std::string& cmd_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual Sequence getSequence(const std::string& cmd_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual Gripper getGripper(const std::string& cmd_name) const = 0; + +protected: + moveit::core::RobotModelConstPtr robot_model_; +}; + +inline void TestdataLoader::setRobotModel(moveit::core::RobotModelConstPtr robot_model) +{ + robot_model_ = std::move(robot_model); +} + +using TestdataLoaderUPtr = std::unique_ptr; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // TESTDATA_LOADER_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.h new file mode 100644 index 0000000000..8c6c4e6914 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.h @@ -0,0 +1,80 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef XML_CONSTANTS_H +#define XML_CONSTANTS_H + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +const std::string EMPTY_STR{}; + +const std::string XML_ATTR_STR{ "" }; +const std::string JOINT_STR{ "joints" }; +const std::string POSE_STR{ "pos" }; +const std::string XYZ_QUAT_STR{ "xyzQuat" }; +const std::string XYZ_EULER_STR{ "xyzEuler" }; +const std::string SEED_STR{ "seed" }; + +const std::string PTP_STR{ "ptp" }; +const std::string LIN_STR{ "lin" }; +const std::string CIRC_STR{ "circ" }; +const std::string BLEND_STR{ "blend" }; +const std::string GRIPPER_STR{ "gripper" }; + +const std::string PLANNING_GROUP_STR{ "planningGroup" }; +const std::string TARGET_LINK_STR{ "targetLink" }; +const std::string START_POS_STR{ "startPos" }; +const std::string END_POS_STR{ "endPos" }; +const std::string INTERMEDIATE_POS_STR{ "intermediatePos" }; +const std::string CENTER_POS_STR{ "centerPos" }; +const std::string VEL_STR{ "vel" }; +const std::string ACC_STR{ "acc" }; + +const std::string POSES_PATH_STR{ "testdata.poses" }; +const std::string PTPS_PATH_STR{ "testdata." + PTP_STR + "s" }; +const std::string LINS_PATH_STR{ "testdata." + LIN_STR + "s" }; +const std::string CIRCS_PATH_STR{ "testdata." + CIRC_STR + "s" }; +const std::string SEQUENCE_PATH_STR{ "testdata.sequences" }; +const std::string GRIPPERS_PATH_STR{ "testdata." + GRIPPER_STR + "s" }; + +const std::string NAME_PATH_STR{ XML_ATTR_STR + ".name" }; +const std::string CMD_TYPE_PATH_STR{ XML_ATTR_STR + ".type" }; +const std::string BLEND_RADIUS_PATH_STR{ XML_ATTR_STR + ".blend_radius" }; +const std::string LINK_NAME_PATH_STR{ XML_ATTR_STR + ".link_name" }; +const std::string GROUP_NAME_PATH_STR{ XML_ATTR_STR + ".group_name" }; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // XML_CONSTANTS_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h new file mode 100644 index 0000000000..c07da6ffb7 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h @@ -0,0 +1,223 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#ifndef XML_TESTDATA_LOADER_H +#define XML_TESTDATA_LOADER_H + +#include +#include +#include +#include +#include + +#include + +#include "pilz_industrial_motion_planner_testutils/testdata_loader.h" + +namespace pt = boost::property_tree; +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Implements a test data loader which uses a xml file + * to store the test data. + * + * The Xml-file has the following structure: + * + * + * + * + * + * j1 j2 j3 j4 j5 j6 + * + * x y z wx wy wz w + * s1 s2 s3 s4 s5 s6 + * + * j_gripper + * + * + * + * j1 j2 j3 j4 j5 j6 + * x y z wx wy wz w + * j_gripper + * + * + * + * + * + * MyTestPos1 + * MyTestPos2 + * 0.1 + * 0.2 + * + * + * + * + * + * manipulator + * prbt_tcp + * MyTestPos1 + * MyTestPos2 + * 0.3 + * 0.4 + * + * + * + * + * + * manipulator + * prbt_tcp + * MyTestPos1 + * MyTestPos1 + * MyTestPos2 + * MyTestPos1 + * 0.2 + * 0.5 + * + * + * + * + * + * + * + * + * + * + * + * + */ + +class XmlTestdataLoader : public TestdataLoader +{ +public: + XmlTestdataLoader(const std::string& path_filename); + XmlTestdataLoader(const std::string& path_filename, const moveit::core::RobotModelConstPtr& robot_model); + ~XmlTestdataLoader() override; + +public: + JointConfiguration getJoints(const std::string& pos_name, const std::string& group_name) const override; + + CartesianConfiguration getPose(const std::string& pos_name, const std::string& group_name) const override; + + PtpJoint getPtpJoint(const std::string& cmd_name) const override; + PtpCart getPtpCart(const std::string& cmd_name) const override; + PtpJointCart getPtpJointCart(const std::string& cmd_name) const override; + + LinJoint getLinJoint(const std::string& cmd_name) const override; + LinCart getLinCart(const std::string& cmd_name) const override; + LinJointCart getLinJointCart(const std::string& cmd_name) const override; + + CircCenterCart getCircCartCenterCart(const std::string& cmd_name) const override; + CircInterimCart getCircCartInterimCart(const std::string& cmd_name) const override; + CircJointCenterCart getCircJointCenterCart(const std::string& cmd_name) const override; + CircJointInterimCart getCircJointInterimCart(const std::string& cmd_name) const override; + + Sequence getSequence(const std::string& cmd_name) const override; + + Gripper getGripper(const std::string& cmd_name) const override; + +private: + /** + * @brief Use this function to search for a node (like an pos or cmd) + * with a given name. + * + * @param tree Tree containing the node. + * @param name Name of node to look for. + */ + const pt::ptree::value_type& findNodeWithName(const boost::property_tree::ptree& tree, const std::string& name, + const std::string& key, const std::string& path = "") const; + + /** + * @brief Use this function to search for a cmd-node with a given name. + */ + const pt::ptree::value_type& findCmd(const std::string& cmd_name, const std::string& cmd_path, + const std::string& cmd_key) const; + + CartesianCenter getCartesianCenter(const std::string& cmd_name, const std::string& planning_group) const; + + CartesianInterim getCartesianInterim(const std::string& cmd_name, const std::string& planning_group) const; + +private: + JointConfiguration getJoints(const boost::property_tree::ptree& joints_tree, const std::string& group_name) const; + +private: + /** + * @brief Converts string vector to double vector. + */ + inline static std::vector strVec2doubleVec(std::vector& strVec); + +public: + /** + * @brief Abstract base class providing a GENERIC getter-function signature + * which can be used to load DIFFERENT command types (like Ptp, Lin, etc.) + * from the test data file. + */ + class AbstractCmdGetterAdapter + { + public: + virtual CmdVariant getCmd(const std::string& /*cmd_name*/) const = 0; + virtual ~AbstractCmdGetterAdapter() = default; + }; + +private: + std::string path_filename_; + pt::ptree tree_{}; + + using AbstractCmdGetterUPtr = std::unique_ptr; + + //! Stores the mapping between command type and the getter function + //! which have to be called. + //! + //! Please note: + //! This mapping is only relevant for sequence commands. + std::map cmd_getter_funcs_; + +private: + const pt::ptree::value_type empty_value_type_{}; + const pt::ptree empty_tree_{}; +}; + +std::vector XmlTestdataLoader::strVec2doubleVec(std::vector& strVec) +{ + std::vector vec; + + vec.resize(strVec.size()); + std::transform(strVec.begin(), strVec.end(), vec.begin(), [](const std::string& val) { return std::stod(val); }); + + return vec; +} + +using XmlTestDataLoaderUPtr = std::unique_ptr; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // XML_TESTDATA_LOADER_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml new file mode 100644 index 0000000000..624abcec8c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml @@ -0,0 +1,29 @@ + + + pilz_industrial_motion_planner_testutils + 0.4.10 + Helper scripts and functionality to test industrial motion generation + + Alexander Gutenkunst + Christian Henkel + Immanuel Martini + Joachim Schleicher + Hagen Slusarek + + BSD + + http://moveit.ros.org + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + eigen_conversions + moveit_core + moveit_msgs + + moveit_core + moveit_msgs + moveit_commander + + catkin + + diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp new file mode 100644 index 0000000000..104d762567 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp @@ -0,0 +1,132 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner_testutils/cartesianconfiguration.h" + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +CartesianConfiguration::CartesianConfiguration() : RobotConfiguration() +{ +} + +CartesianConfiguration::CartesianConfiguration(const std::string& group_name, const std::string& link_name, + const std::vector& config) + : RobotConfiguration(group_name), link_name_(link_name), pose_(toPose(config)) +{ +} + +CartesianConfiguration::CartesianConfiguration(const std::string& group_name, const std::string& link_name, + const std::vector& config, + const moveit::core::RobotModelConstPtr& robot_model) + : RobotConfiguration(group_name, robot_model), link_name_(link_name), pose_(toPose(config)) +{ + if (robot_model && (!robot_model_->hasLinkModel(link_name_))) + { + std::string msg{ "Link \"" }; + msg.append(link_name).append("\" not known to robot model"); + throw std::invalid_argument(msg); + } + + if (robot_model && (!robot_state::RobotState(robot_model_).knowsFrameTransform(link_name_))) + { + std::string msg{ "Tranform of \"" }; + msg.append(link_name).append("\" is unknown"); + throw std::invalid_argument(msg); + } +} + +geometry_msgs::Pose CartesianConfiguration::toPose(const std::vector& pose) +{ + geometry_msgs::Pose pose_msg; + pose_msg.position.x = pose.at(0); + pose_msg.position.y = pose.at(1); + pose_msg.position.z = pose.at(2); + pose_msg.orientation.x = pose.at(3); + pose_msg.orientation.y = pose.at(4); + pose_msg.orientation.z = pose.at(5); + pose_msg.orientation.w = pose.at(6); + + return pose_msg; +} + +geometry_msgs::PoseStamped CartesianConfiguration::toStampedPose(const geometry_msgs::Pose& pose) +{ + geometry_msgs::PoseStamped pose_stamped_msg; + pose_stamped_msg.pose = pose; + return pose_stamped_msg; +} + +moveit_msgs::RobotState CartesianConfiguration::toMoveitMsgsRobotState() const +{ + if (!robot_model_) + { + throw std::runtime_error("No robot model set"); + } + + robot_state::RobotState rstate(robot_model_); + rstate.setToDefaultValues(); + if (hasSeed()) + { + rstate.setJointGroupPositions(group_name_, getSeed().getJoints()); + } + + rstate.update(); + + // set to Cartesian pose + Eigen::Isometry3d start_pose; + tf::poseMsgToEigen(pose_, start_pose); + if (!rstate.setFromIK(rstate.getRobotModel()->getJointModelGroup(group_name_), start_pose, link_name_)) + { + std::ostringstream os; + os << "No solution for ik \n" << start_pose.translation() << "\n" << start_pose.linear(); + throw std::runtime_error(os.str()); + } + + // Conversion to RobotState msg type + moveit_msgs::RobotState robot_state_msg; + moveit::core::robotStateToRobotStateMsg(rstate, robot_state_msg, true); + return robot_state_msg; +} + +std::ostream& operator<<(std::ostream& os, const CartesianConfiguration& obj) +{ + os << "Group name: \"" << obj.getGroupName() << "\""; + os << " | link name: \"" << obj.getLinkName() << "\""; + os << "\n" << obj.getPose(); + return os; +} + +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp new file mode 100644 index 0000000000..4ab2cbb5d9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp @@ -0,0 +1,161 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner_testutils/jointconfiguration.h" + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +JointConfiguration::JointConfiguration() : RobotConfiguration() +{ +} + +JointConfiguration::JointConfiguration(const std::string& group_name, const std::vector& config, + CreateJointNameFunc&& create_joint_name_func) + : RobotConfiguration(group_name), joints_(config), create_joint_name_func_(create_joint_name_func) +{ +} + +JointConfiguration::JointConfiguration(const std::string& group_name, const std::vector& config, + const moveit::core::RobotModelConstPtr& robot_model) + : RobotConfiguration(group_name, robot_model), joints_(config) +{ +} + +moveit_msgs::Constraints JointConfiguration::toGoalConstraintsWithoutModel() const +{ + if (!create_joint_name_func_) + { + throw JointConfigurationException("Create-Joint-Name function not set"); + } + + moveit_msgs::Constraints gc; + + for (size_t i = 0; i < joints_.size(); ++i) + { + moveit_msgs::JointConstraint jc; + jc.joint_name = create_joint_name_func_(i); + jc.position = joints_.at(i); + gc.joint_constraints.push_back(jc); + } + + return gc; +} + +moveit_msgs::Constraints JointConfiguration::toGoalConstraintsWithModel() const +{ + if (!robot_model_) + { + throw JointConfigurationException("No robot model set"); + } + + robot_state::RobotState state(robot_model_); + state.setToDefaultValues(); + state.setJointGroupPositions(group_name_, joints_); + + return kinematic_constraints::constructGoalConstraints(state, state.getRobotModel()->getJointModelGroup(group_name_)); +} + +moveit_msgs::RobotState JointConfiguration::toMoveitMsgsRobotStateWithoutModel() const +{ + if (!create_joint_name_func_) + { + throw JointConfigurationException("Create-Joint-Name function not set"); + } + + moveit_msgs::RobotState robot_state; + for (size_t i = 0; i < joints_.size(); ++i) + { + robot_state.joint_state.name.emplace_back(create_joint_name_func_(i)); + robot_state.joint_state.position.push_back(joints_.at(i)); + } + return robot_state; +} + +robot_state::RobotState JointConfiguration::toRobotState() const +{ + if (!robot_model_) + { + throw JointConfigurationException("No robot model set"); + } + + robot_state::RobotState robot_state(robot_model_); + robot_state.setToDefaultValues(); + robot_state.setJointGroupPositions(group_name_, joints_); + return robot_state; +} + +moveit_msgs::RobotState JointConfiguration::toMoveitMsgsRobotStateWithModel() const +{ + robot_state::RobotState start_state(toRobotState()); + moveit_msgs::RobotState rob_state_msg; + moveit::core::robotStateToRobotStateMsg(start_state, rob_state_msg, false); + return rob_state_msg; +} + +sensor_msgs::JointState JointConfiguration::toSensorMsg() const +{ + if (!create_joint_name_func_) + { + throw JointConfigurationException("Create-Joint-Name function not set"); + } + + sensor_msgs::JointState state; + for (size_t i = 0; i < joints_.size(); ++i) + { + state.name.emplace_back(create_joint_name_func_(i)); + state.position.push_back(joints_.at(i)); + } + return state; +} + +std::ostream& operator<<(std::ostream& os, const JointConfiguration& obj) +{ + const size_t n{ obj.size() }; + os << "JointConfiguration: ["; + for (size_t i = 0; i < n; ++i) + { + os << obj.getJoint(i); + if (i != n - 1) + { + os << ", "; + } + } + os << "]"; + + return os; +} + +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/robotconfiguration.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/robotconfiguration.cpp new file mode 100644 index 0000000000..7de626c3f1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/robotconfiguration.cpp @@ -0,0 +1,62 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner_testutils/robotconfiguration.h" + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +RobotConfiguration::RobotConfiguration() : RobotStateMsgConvertible(), GoalConstraintMsgConvertible() +{ +} + +RobotConfiguration::RobotConfiguration(const std::string& group_name) + : RobotStateMsgConvertible(), GoalConstraintMsgConvertible(), group_name_(group_name) +{ +} + +RobotConfiguration::RobotConfiguration(const std::string& group_name, + const moveit::core::RobotModelConstPtr& robot_model) + : RobotStateMsgConvertible(), GoalConstraintMsgConvertible(), group_name_(group_name), robot_model_(robot_model) +{ + if (robot_model && (!robot_model_->hasJointModelGroup(group_name_))) + { + std::string msg{ "Specified robot model does not contain specified group \"" }; + msg.append(group_name).append("\""); + throw std::invalid_argument(msg); + } +} + +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp new file mode 100644 index 0000000000..b606668385 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp @@ -0,0 +1,120 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner_testutils/sequence.h" + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Visitor transforming the stored command into a MotionPlanRequest. + */ +class ToReqVisitor : public boost::static_visitor +{ +public: + template + planning_interface::MotionPlanRequest operator()(T& cmd) const + { + return cmd.toRequest(); + } +}; + +/** + * @brief Visitor returning not the specific command type but the base type. + */ +class ToBaseVisitor : public boost::static_visitor +{ +public: + template + MotionCmd& operator()(T& cmd) const + { + return cmd; + } +}; + +moveit_msgs::MotionSequenceRequest Sequence::toRequest() const +{ + moveit_msgs::MotionSequenceRequest req; + + std::vector group_names; + for (const auto& cmd : cmds_) + { + moveit_msgs::MotionSequenceItem item; + item.req = boost::apply_visitor(ToReqVisitor(), cmd.first); + + if (std::find(group_names.begin(), group_names.end(), item.req.group_name) != group_names.end()) + { + // Remove start state because only the first request of a group + // is allowed to have a start state in a sequence. + item.req.start_state = moveit_msgs::RobotState(); + } + else + { + group_names.emplace_back(item.req.group_name); + } + + item.blend_radius = cmd.second; + req.items.push_back(item); + } + return req; +} + +void Sequence::erase(const size_t start, const size_t end) +{ + const size_t orig_n{ size() }; + if (start > orig_n || end > orig_n) + { + std::string msg; + msg.append("Parameter start=").append(std::to_string(start)); + msg.append(" and end=").append(std::to_string(end)); + msg.append(" must not be greater then the number of #commands="); + msg.append(std::to_string(size())); + throw std::invalid_argument(msg); + } + cmds_.erase(cmds_.begin() + start, cmds_.begin() + end); + if (end == orig_n) + { + // Make sure last radius is set zero + cmds_.at(size() - 1).second = 0.; + } +} + +MotionCmd& Sequence::getCmd(const size_t index_cmd) +{ + return boost::apply_visitor(ToBaseVisitor(), cmds_.at(index_cmd).first); +} + +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp new file mode 100644 index 0000000000..69054bc45d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp @@ -0,0 +1,555 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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 "pilz_industrial_motion_planner_testutils/xml_testdata_loader.h" + +#include + +#include +#include +#include + +#include "pilz_industrial_motion_planner_testutils/default_values.h" +#include "pilz_industrial_motion_planner_testutils/exception_types.h" +#include "pilz_industrial_motion_planner_testutils/xml_constants.h" + +namespace pt = boost::property_tree; +namespace pilz_industrial_motion_planner_testutils +{ +class CmdReader +{ +public: + CmdReader(const pt::ptree::value_type& node) : cmd_node_(node) + { + } + +public: + std::string getPlanningGroup() const; + std::string getTargetLink() const; + std::string getStartPoseName() const; + std::string getEndPoseName() const; + + double getVelocityScale() const; + double getAccelerationScale() const; + + CmdReader& setDefaultVelocityScale(double scale); + CmdReader& setDefaultAccelerationScale(double scale); + +private: + const pt::ptree::value_type& cmd_node_; + + double default_velocity_scale_{ DEFAULT_VEL }; + double default_acceleration_scale_{ DEFAULT_ACC }; +}; + +inline std::string CmdReader::getPlanningGroup() const +{ + return cmd_node_.second.get(PLANNING_GROUP_STR); +} + +inline std::string CmdReader::getTargetLink() const +{ + return cmd_node_.second.get(TARGET_LINK_STR); +} + +inline std::string CmdReader::getStartPoseName() const +{ + return cmd_node_.second.get(START_POS_STR); +} + +inline std::string CmdReader::getEndPoseName() const +{ + return cmd_node_.second.get(END_POS_STR); +} + +inline double CmdReader::getVelocityScale() const +{ + return cmd_node_.second.get(VEL_STR, default_velocity_scale_); +} + +inline double CmdReader::getAccelerationScale() const +{ + return cmd_node_.second.get(ACC_STR, default_acceleration_scale_); +} + +inline CmdReader& CmdReader::setDefaultVelocityScale(double scale) +{ + default_velocity_scale_ = scale; + return *this; +} + +inline CmdReader& CmdReader::setDefaultAccelerationScale(double scale) +{ + default_acceleration_scale_ = scale; + return *this; +} + +template +class CmdGetterAdapter : public XmlTestdataLoader::AbstractCmdGetterAdapter +{ +public: + using FuncType = std::function; + + CmdGetterAdapter(FuncType func) : AbstractCmdGetterAdapter(), func_(func) + { + } + +public: + CmdVariant getCmd(const std::string& cmd_name) const override + { + return CmdVariant(func_(cmd_name)); + } + +private: + FuncType func_; +}; + +XmlTestdataLoader::XmlTestdataLoader(const std::string& path_filename) : TestdataLoader(), path_filename_(path_filename) +{ + // Parse the XML into the property tree. + pt::read_xml(path_filename_, tree_, pt::xml_parser::no_comments); + + using std::placeholders::_1; + cmd_getter_funcs_["ptp"] = + AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpJoint, this, _1))); + cmd_getter_funcs_["ptp_joint_cart"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpJointCart, this, _1))); + cmd_getter_funcs_["ptp_cart_cart"] = + AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpCart, this, _1))); + + cmd_getter_funcs_["lin"] = + AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getLinJoint, this, _1))); + cmd_getter_funcs_["lin_cart"] = + AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getLinCart, this, _1))); + + cmd_getter_funcs_["circ_center_cart"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getCircCartCenterCart, this, _1))); + cmd_getter_funcs_["circ_interim_cart"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getCircCartInterimCart, this, _1))); + cmd_getter_funcs_["circ_joint_interim_cart"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getCircJointInterimCart, this, _1))); + + cmd_getter_funcs_["gripper"] = + AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getGripper, this, _1))); +} + +XmlTestdataLoader::XmlTestdataLoader(const std::string& path_filename, + const moveit::core::RobotModelConstPtr& robot_model) + : XmlTestdataLoader(path_filename) +{ + setRobotModel(robot_model); +} + +XmlTestdataLoader::~XmlTestdataLoader() +{ +} + +const pt::ptree::value_type& XmlTestdataLoader::findNodeWithName(const boost::property_tree::ptree& tree, + const std::string& name, const std::string& key, + const std::string& path) const +{ + std::string path_str{ (path.empty() ? NAME_PATH_STR : path) }; + + // Search for node with given name. + for (const pt::ptree::value_type& val : tree) + { + // Ignore attributes which are always the first element of a tree. + if (val.first == XML_ATTR_STR) + { + continue; + } + + if (val.first != key) + { + continue; + } + + const auto& node{ val.second.get_child(path_str, empty_tree_) }; + if (node == empty_tree_) + { + break; + } + if (node.data() == name) + { + return val; + } + } + + std::string msg; + msg.append("Node of type \"") + .append(key) + .append("\" with ") + .append(path_str) + .append("=\"") + .append(name) + .append("\" " + "not " + "foun" + "d."); + throw TestDataLoaderReadingException(msg); +} + +JointConfiguration XmlTestdataLoader::getJoints(const std::string& pos_name, const std::string& group_name) const +{ + // Search for node with given name. + const auto& poses_tree{ tree_.get_child(POSES_PATH_STR, empty_tree_) }; + if (poses_tree == empty_tree_) + { + throw TestDataLoaderReadingException("No poses found."); + } + return getJoints(findNodeWithName(poses_tree, pos_name, POSE_STR).second, group_name); +} + +JointConfiguration XmlTestdataLoader::getJoints(const boost::property_tree::ptree& joints_tree, + const std::string& group_name) const +{ + // Search joints node with given group_name. + if (joints_tree == empty_tree_) + { + throw TestDataLoaderReadingException("No joints found."); + } + const auto& joint_node{ findNodeWithName(joints_tree, group_name, JOINT_STR, GROUP_NAME_PATH_STR) }; + + std::vector strs; + boost::split(strs, joint_node.second.data(), boost::is_any_of(" ")); + return JointConfiguration(group_name, strVec2doubleVec(strs), robot_model_); +} + +CartesianConfiguration XmlTestdataLoader::getPose(const std::string& pos_name, const std::string& group_name) const +{ + const auto& all_poses_tree{ tree_.get_child(POSES_PATH_STR, empty_tree_) }; + if (all_poses_tree == empty_tree_) + { + throw TestDataLoaderReadingException("No poses found."); + } + const auto& pose_tree{ findNodeWithName(all_poses_tree, pos_name, POSE_STR).second }; + const auto& xyz_quat_tree{ findNodeWithName(pose_tree, group_name, XYZ_QUAT_STR, GROUP_NAME_PATH_STR).second }; + const boost::property_tree::ptree& link_name_attr{ xyz_quat_tree.get_child(LINK_NAME_PATH_STR, empty_tree_) }; + if (link_name_attr == empty_tree_) + { + throw TestDataLoaderReadingException("No link name found."); + } + + // Get rid of things like "\n", etc. + std::string data{ xyz_quat_tree.data() }; + boost::trim(data); + + std::vector pos_ori_str; + boost::split(pos_ori_str, data, boost::is_any_of(" ")); + CartesianConfiguration cart_config{ CartesianConfiguration(group_name, link_name_attr.data(), + strVec2doubleVec(pos_ori_str), robot_model_) }; + + const auto& seeds_tree{ xyz_quat_tree.get_child(SEED_STR, empty_tree_) }; + if (seeds_tree != empty_tree_) + { + cart_config.setSeed(getJoints(seeds_tree, group_name)); + } + return cart_config; +} + +PtpJoint XmlTestdataLoader::getPtpJoint(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, PTPS_PATH_STR, PTP_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + PtpJoint cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +PtpJointCart XmlTestdataLoader::getPtpJointCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, PTPS_PATH_STR, PTP_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + PtpJointCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +PtpCart XmlTestdataLoader::getPtpCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, PTPS_PATH_STR, PTP_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + PtpCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getPose(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +LinJoint XmlTestdataLoader::getLinJoint(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, LINS_PATH_STR, LIN_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + LinJoint cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +LinCart XmlTestdataLoader::getLinCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, LINS_PATH_STR, LIN_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + LinCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getPose(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +LinJointCart XmlTestdataLoader::getLinJointCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, LINS_PATH_STR, LIN_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + LinJointCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +const pt::ptree::value_type& XmlTestdataLoader::findCmd(const std::string& cmd_name, const std::string& cmd_path, + const std::string& cmd_key) const +{ + // Search for node with given name. + const boost::property_tree::ptree& cmds_tree{ tree_.get_child(cmd_path, empty_tree_) }; + if (cmds_tree == empty_tree_) + { + throw TestDataLoaderReadingException("No list of commands of type \"" + cmd_key + "\" found"); + } + + return findNodeWithName(cmds_tree, cmd_name, cmd_key); +} + +CartesianCenter XmlTestdataLoader::getCartesianCenter(const std::string& cmd_name, + const std::string& planning_group) const +{ + const pt::ptree::value_type& cmd_node{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) }; + std::string aux_pos_name; + try + { + aux_pos_name = cmd_node.second.get(CENTER_POS_STR); + } + catch (...) + { + throw TestDataLoaderReadingException("Did not find center of circ"); + } + + CartesianCenter aux; + aux.setConfiguration(getPose(aux_pos_name, planning_group)); + return aux; +} + +CartesianInterim XmlTestdataLoader::getCartesianInterim(const std::string& cmd_name, + const std::string& planning_group) const +{ + const pt::ptree::value_type& cmd_node{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) }; + std::string aux_pos_name; + try + { + aux_pos_name = cmd_node.second.get(INTERMEDIATE_POS_STR); + } + catch (...) + { + throw TestDataLoaderReadingException("Did not find interim of circ"); + } + + CartesianInterim aux; + aux.setConfiguration(getPose(aux_pos_name, planning_group)); + return aux; +} + +CircCenterCart XmlTestdataLoader::getCircCartCenterCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + CircCenterCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getPose(cmd_reader.getStartPoseName(), planning_group)); + cmd.setAuxiliaryConfiguration(getCartesianCenter(cmd_name, planning_group)); + cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +CircInterimCart XmlTestdataLoader::getCircCartInterimCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + CircInterimCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getPose(cmd_reader.getStartPoseName(), planning_group)); + cmd.setAuxiliaryConfiguration(getCartesianInterim(cmd_name, planning_group)); + cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +CircJointInterimCart XmlTestdataLoader::getCircJointInterimCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + CircJointInterimCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setAuxiliaryConfiguration(getCartesianInterim(cmd_name, planning_group)); + cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +CircJointCenterCart XmlTestdataLoader::getCircJointCenterCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + CircJointCenterCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setAuxiliaryConfiguration(getCartesianCenter(cmd_name, planning_group)); + cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +Sequence XmlTestdataLoader::getSequence(const std::string& cmd_name) const +{ + Sequence seq; + + // Find sequence with given name and loop over all its cmds + const auto& sequence_cmd_tree{ findCmd(cmd_name, SEQUENCE_PATH_STR, BLEND_STR).second }; + for (const pt::ptree::value_type& seq_cmd : sequence_cmd_tree) + { + // Ignore attributes which are always the first element of a tree. + if (seq_cmd.first == XML_ATTR_STR) + { + continue; + } + + // Get name of blend cmd. + const boost::property_tree::ptree& cmd_name_attr = seq_cmd.second.get_child(NAME_PATH_STR, empty_tree_); + if (cmd_name_attr == empty_tree_) + { + throw TestDataLoaderReadingException("Did not find name of sequence cmd"); + } + + const std::string& cmd_name{ cmd_name_attr.data() }; + + // Get type of blend cmd + const boost::property_tree::ptree& type_name_attr{ seq_cmd.second.get_child(CMD_TYPE_PATH_STR, empty_tree_) }; + if (type_name_attr == empty_tree_) + { + throw TestDataLoaderReadingException("Did not find type of sequence cmd \"" + cmd_name + "\""); + } + const std::string& cmd_type{ type_name_attr.data() }; + + // Get blend radius of blend cmd. + double blend_radius{ seq_cmd.second.get(BLEND_RADIUS_PATH_STR, DEFAULT_BLEND_RADIUS) }; + + // Read current command from test data + Add command to sequence + seq.add(cmd_getter_funcs_.at(cmd_type)->getCmd(cmd_name), blend_radius); + } + + return seq; +} + +Gripper XmlTestdataLoader::getGripper(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, GRIPPERS_PATH_STR, GRIPPER_STR) }; + cmd_reader.setDefaultVelocityScale(DEFAULT_VEL_GRIPPER); + cmd_reader.setDefaultAccelerationScale(DEFAULT_ACC_GRIPPER); + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + Gripper cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index f32c11dbda..c6351baef6 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -516,6 +516,8 @@ bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path) const std::vector& joint_models = joint_model_group->getActiveJointModels(); emitter << YAML::Key << "name"; emitter << YAML::Value << "fake_" + group.name_ + "_controller"; + emitter << YAML::Key << "type"; + emitter << YAML::Value << "$(arg execution_type)"; emitter << YAML::Key << "joints"; emitter << YAML::Value << YAML::BeginSeq; diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index b39ea2c013..4535e99498 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -297,6 +297,17 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = 0; // Can they be changed? gen_files_.push_back(file); + // cartesian_limits.yaml -------------------------------------------------------------------------------------- + file.file_name_ = "cartesian_limits.yaml"; + file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); + template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_); + file.description_ = "Cartesian velocity for planning in the workspace." + "The velocity is used by pilz industrial motion planner as maximum velocity for cartesian " + "planning requests scaled by the velocity scaling factor of an individual planning request."; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.write_on_changes = 0; // Can they be changed? + gen_files_.push_back(file); + // fake_controllers.yaml -------------------------------------------------------------------------------------- file.file_name_ = "fake_controllers.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); @@ -380,6 +391,18 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = 0; gen_files_.push_back(file); + // pilz_industrial_motion_planner_planning_pipeline.launch + // -------------------------------------------------------------------------------------- + file.file_name_ = "pilz_industrial_motion_planner_planning_pipeline.launch.xml"; + file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); + template_path = config_data_->appendPaths(template_launch_path, file.file_name_); + file.description_ = "Intended to be included in other launch files that require the Pilz industrial motion planner " + "planning plugin. Defines the proper plugin name on the parameter server and a default selection " + "of planning request adapters."; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.write_on_changes = 0; + gen_files_.push_back(file); + // chomp_planning_pipeline.launch // -------------------------------------------------------------------------------------- file.file_name_ = "chomp_planning_pipeline.launch.xml"; diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/config/cartesian_limits.yaml b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/cartesian_limits.yaml new file mode 100644 index 0000000000..7df72f693b --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml index c9eb4e24c8..f334bb4d57 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml @@ -1,21 +1,30 @@ - - - - - + + - - - + + - + + + + + + + + + + + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/fake_moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/fake_moveit_controller_manager.launch.xml index 06b3a93e74..f668745bb0 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/fake_moveit_controller_manager.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,12 @@ + + + - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch index b41620c229..6637d21031 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch @@ -15,6 +15,7 @@ + @@ -46,6 +47,8 @@ + + @@ -53,6 +56,7 @@ + @@ -68,8 +72,6 @@ - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml index 9ce2a6e1f0..f85ed516e2 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml @@ -3,6 +3,12 @@ + + + + + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 0000000000..ac6272e235 --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch index 54efbec4e7..635179b3b8 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch @@ -14,6 +14,7 @@ + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml index b58d3f4660..f1b5c7fcec 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml @@ -5,6 +5,15 @@ - + + + + + + + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml index aec604748b..2de6f893f6 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml @@ -2,6 +2,8 @@ + + @@ -15,6 +17,8 @@ - + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml index 07ebf68e85..59470ce087 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml @@ -3,6 +3,12 @@ + + + + + + + + From 9f9e69c54529e1aa156851abb6aae7047b20780d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 3 Nov 2020 17:10:16 +0100 Subject: [PATCH 508/612] FixStartStateBounds: Copy attached bodies when adapting the start state (#2398) --- .../src/fix_start_state_bounds.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp index 6fe04103fa..83ecdda130 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp @@ -181,7 +181,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap if (change_req) { planning_interface::MotionPlanRequest req2 = req; - moveit::core::robotStateToRobotStateMsg(start_state, req2.start_state, false); + moveit::core::robotStateToRobotStateMsg(start_state, req2.start_state); solved = planner(planning_scene, req2, res); } else From f85db808303cb7787320dc48162eabdc07593fdc Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 4 Nov 2020 08:34:01 -0700 Subject: [PATCH 509/612] [meta] first-timers-only issue template (#2385) Co-authored-by: Felix von Drigalski Co-authored-by: Sebastian Jahr --- .github/ISSUE_TEMPLATE/first_timers_only.md | 65 +++++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 .github/ISSUE_TEMPLATE/first_timers_only.md diff --git a/.github/ISSUE_TEMPLATE/first_timers_only.md b/.github/ISSUE_TEMPLATE/first_timers_only.md new file mode 100644 index 0000000000..b907471b29 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/first_timers_only.md @@ -0,0 +1,65 @@ +--- +name: First Timers Only Issue +about: Create an issue to welcome a new contributor into the community. +title: '' +labels: first-timers-only +assignees: '' + +--- + +## Background + +Overview of your issue here. + +## Instructions +Hi, this is a `first-timer-only` issue. This means we've worked to make it more legible to people who either **haven't contributed to our codebase before, or even folks who haven't contributed to open source before**. + +We're interested in helping you take the first step, and can answer questions and help you out along the way. Note that we're especially interested in contributions from underrepresented groups! + +We know that creating a pull request is the biggest barrier for new contributors. This issue is for you 💝 + +If you have contributed before, **consider leaving this PR for someone new**, and looking through our general [bug](https://github.com/ros-planning/moveit/labels/bug) issues. Thanks! + +### 🤔 What you will need to know. + +Nothing. This issue is meant to welcome you to Open Source :) We are happy to walk you through the process. + +### 📋 Step by Step + +- [ ] 🙋 **Claim this issue**: Comment below. If someone else has claimed it, ask if they've opened a pull request already and if they're stuck -- maybe you can help them solve a problem or move it along! + +- [ ] 🗄️**Create a local workspace** for making your changes and testing [follwing these instructions](https://moveit.ros.org/install/source/) + +- [ ] 📝 **Update** the file(s) `$FILENAME`by applying the change shown in the diff below. + +```diff +$DIFF +``` + +- [ ] 🤖 **Apply `clang-format-10`** auto formatting, [following these instructions](https://moveit.ros.org//documentation/contributing/code/?#clang-format-auto-code-formatting) + +- [ ] 💾 **Commit** your changes + +- [ ] 🔀 **Start a Pull Request**. There are two ways how you can start a pull request: +1. If you are not familiar with GitHub or how to create a pull request, [here is a guide you can follow](https://guides.github.com/activities/hello-world/) on how GitHub works. +2. If you are familiar with the terminal or would like to learn to use it, [here is a great tutorial](https://egghead.io/series/how-to-contribute-to-an-open-source-project-on-github) on how to send a pull request using the terminal. + +- [ ] 🏁 **Done** Ask in comments for a review :) + +### Is someone else already working on this? + +🔗- We encourage contributors to link to the original issue in their pull request so all users can easily see if someone's already started on it. + +👥- **If someone seems stuck, offer them some help!** + +### 🤔❓ Questions? + +Don’t hesitate to ask questions or to get help if you feel like you are getting stuck. For example leave a comment below! +Furthermore, you find helpful resources here: +* [MoveIt FAQ](https://moveit.ros.org/documentation/faqs/) +* [MoveIt Tutorials](https://ros-planning.github.io/moveit_tutorials/) +* [MoveIt contribution guide](https://moveit.ros.org/documentation/contributing/) +* [ROS Tutorials](https://wiki.ros.org/ROS/Tutorials) +* [ROS Answers](https://answers.ros.org/questions/) + +**Good luck with your first issue!** From 382aa5a8cdd39eace07536d39c497a4b21f0f653 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 4 Nov 2020 19:10:32 +0100 Subject: [PATCH 510/612] Fix QObject::connect: Cannot queue arguments of type 'QVector' (#2392) --- .../planning_scene_display.h | 1 + .../src/planning_scene_display.cpp | 11 +++++++---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index 99f9ab8634..ff32a3e5d4 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -130,6 +130,7 @@ private Q_SLOTS: void changedSceneDisplayTime(); void changedOctreeRenderMode(); void changedOctreeColorMode(); + void setSceneName(const QString& name); protected Q_SLOTS: virtual void changedAttachedBodyColor(); diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 862e41ba63..067ead8875 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -583,14 +583,17 @@ void PlanningSceneDisplay::sceneMonitorReceivedUpdate( void PlanningSceneDisplay::onSceneMonitorReceivedUpdate( planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType /*update_type*/) { - bool old_state = scene_name_property_->blockSignals(true); getPlanningSceneRW()->getCurrentStateNonConst().update(); - scene_name_property_->setStdString(getPlanningSceneRO()->getName()); - scene_name_property_->blockSignals(old_state); - + QMetaObject::invokeMethod(this, "setSceneName", Qt::QueuedConnection, + Q_ARG(QString, QString::fromStdString(getPlanningSceneRO()->getName()))); planning_scene_needs_render_ = true; } +void PlanningSceneDisplay::setSceneName(const QString& name) +{ + scene_name_property_->setString(name); +} + void PlanningSceneDisplay::onEnable() { Display::onEnable(); From 0247ed0027ca9d7f1a7f066e62c80c9ce5dbbb5e Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 6 Nov 2020 14:49:50 -0700 Subject: [PATCH 511/612] [maint] Changelogs in pilz planner (#2408) --- .../CHANGELOG.rst | 105 +----------------- .../package.xml | 2 +- .../CHANGELOG.rst | 83 +------------- .../package.xml | 2 +- 4 files changed, 10 insertions(+), 182 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst index 1acf7f0272..b711926f83 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst @@ -2,105 +2,8 @@ Changelog for package pilz_industrial_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.4.10 (2019-12-04) -------------------- +Forthcoming +----------- +* [feature] Add Pilz industrial motion planner (`#1893 `_) +* Contributors: Pilz GmbH and Co. KG, Christian Henkel, Immanuel Martini, Joachim Schleicher, rfeistenauer -0.4.9 (2019-11-28) ------------------- - -0.4.8 (2019-11-22) ------------------- - -0.4.7 (2019-09-10) ------------------- -* Fix clang-tidy issues -* integrate clang-tidy via CMake flag -* Contributors: Pilz GmbH and Co. KG - -0.4.6 (2019-09-04) ------------------- - -0.4.5 (2019-09-03) ------------------- -* Adapt to changes in pilz_robots -* add static code analyzing (clang-tidy) -* drop deprecated isRobotStateEqual() -* Contributors: Pilz GmbH and Co. KG - -0.4.4 (2019-06-19) ------------------- -* fixed an error that led to trajectories not strictly increasing in time -* Contributors: Pilz GmbH and Co. KG - -0.4.3 (2019-04-08) ------------------- -* update dependencies of trajectory_generation -* fix CIRC path generator and increase test coverage -* adopt strictest limits in ptp planner (refactor JointLimitsContainer and TrajectoryGeneratorPTP) -* Enable gripper commands inside a sequence -* Contributors: Pilz GmbH and Co. KG - -0.4.2 (2019-03-13) ------------------- -* re-adapt to new RobotState API: remove #attempts -* Contributors: Pilz GmbH and Co. KG - -0.4.1 (2019-02-27) ------------------- - -0.3.6 (2019-02-26) ------------------- -* refactor the testdataloader -* adapt to new RobotState API: remove #attempts -* Contributors: Pilz GmbH and Co. KG - -0.3.5 (2019-02-06) ------------------- -* Increase line coverage for blending to 100% -* refactor determining the trajectory alignment in the blend implementation -* extend and refactor unittest of blender_transition_window -* add planning group check to blender_transition_window -* add more details to blend algorithm description -* change handling of empty sequences in capabilities to be non-erroneous -* rename command_planner -> pilz_command_planner -* use pilz_testutils package for blend test -* use collision-aware ik calculation -* Contributors: Pilz GmbH and Co. KG - -0.4.0 (2018-12-18) ------------------- -* Use Eigen::Isometry3d to keep up with the recent changes in moveit -* Contributors: Chris Lalancette - -0.3.1 (2018-12-17) ------------------- - -0.3.0 (2018-11-28) ------------------- -* add append method for avoiding duplicate points in robot_trajectory trajectories -* Relax the precondition on trajectory generators from v_start==0 to |v_start| < 1e-10 to gain robustness -* Set last point of generated trajectories to have vel=acc=0 to match the first point. -* add sequence action and service capabilities to concatenate multiple requests -* Contributors: Pilz GmbH and Co. KG - -0.2.2 (2018-09-26) ------------------- - -0.2.1 (2018-09-25) ------------------- - -0.1.1 (2018-09-25) ------------------- -* port to melodic -* drop unused dependencies -* Contributors: Pilz GmbH and Co. KG - -0.2.0 (2018-09-14) ------------------- -* Changes for melodic -* Contributors: Pilz GmbH and Co. KG - -0.1.0 (2018-09-14) ------------------- -* Created trajectory generation package with ptp, lin, circ and blend planner -* Contributors: Pilz GmbH and Co. KG diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index d69a541779..41f8eb032d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -1,7 +1,7 @@ pilz_industrial_motion_planner - 0.4.10 + 1.1.1 MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof. Alexander Gutenkunst diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst index 241af55e15..69e8a98417 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst @@ -2,83 +2,8 @@ Changelog for package pilz_industrial_motion_planner_testutils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.4.10 (2019-12-04) -------------------- +Forthcoming +----------- +* [feature] Add Pilz industrial motion planner (`#1893 `_) +* Contributors: Pilz GmbH and Co. KG, Christian Henkel, Immanuel Martini, Joachim Schleicher, rfeistenauer -0.4.9 (2019-11-28) ------------------- - -0.4.8 (2019-11-22) ------------------- - -0.4.7 (2019-09-10) ------------------- -* Fix clang-tidy issues -* integrate clang-tidy via CMake flag -* Contributors: Pilz GmbH and Co. KG - -0.4.6 (2019-09-04) ------------------- - -0.4.5 (2019-09-03) ------------------- - -0.4.4 (2019-06-19) ------------------- - -0.4.3 (2019-04-08) ------------------- -* update package dependencies -* Add missing intialization -* Add getter for CircJointInterimCart in XMLTestdataLoader - -0.4.2 (2019-03-13) ------------------- - -0.4.1 (2019-02-27) ------------------- - -0.3.6 (2019-02-26) ------------------- - -0.3.5 (2019-02-06) ------------------- - -0.3.4 (2019-02-05) ------------------- -* Add high level abstraction data classes to represent configuration of robot -* Add high level abstraction data classes to represent different command types -* Add functions to TestdataLoader returning the high level abstraction classes - -0.4.0 (2018-12-18) ------------------- -* Use Eigen::Isometry3d to keep up with the recent changes in moveit -* Contributors: Chris Lalancette - -0.3.1 (2018-12-17) ------------------- -* Add RobotMotionObserver in testutils -* Contributors: Pilz GmbH and Co. KG - -0.3.0 (2018-11-28) ------------------- -* rename get_current_joint_values -> get_current_joint_states - -0.2.2 (2018-09-26) ------------------- -* fix missing dependency for melodic -* Contributors: Pilz GmbH and Co. KG - -0.2.1 (2018-09-25) ------------------- - -0.1.1 (2018-09-25) ------------------- - -0.2.0 (2018-09-14) ------------------- - -0.1.0 (2018-09-14) ------------------- -* xml test data loader. -* Contributors: Pilz GmbH and Co. KG diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml index 624abcec8c..e2b1718a89 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml @@ -1,7 +1,7 @@ pilz_industrial_motion_planner_testutils - 0.4.10 + 1.1.1 Helper scripts and functionality to test industrial motion generation Alexander Gutenkunst From 09afa50b40ac8cb8644ba1a627bb7643137959d3 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 6 Nov 2020 15:53:06 -0600 Subject: [PATCH 512/612] Update collision-related comments (#2382) (#2388) Co-authored-by: Ghenohenomohe <71963599+Ghenohenomohe@users.noreply.github.com> --- .../include/moveit/collision_detection_fcl/collision_common.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index 62db457a55..2359081e9a 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -274,7 +274,7 @@ struct FCLManager * \param o1 First FCL collision object * \param o2 Second FCL collision object * \data General pointer to arbitrary data which is used during the callback - * \return True terminates the distance check, false continues it to the next pair of objects */ + * \return True terminates the collision check, false continues it to the next pair of objects */ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data); /** \brief Callback function used by the FCLManager used for each pair of collision objects to @@ -283,7 +283,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi * \param o1 First FCL collision object * \param o2 Second FCL collision object * \data General pointer to arbitrary data which is used during the callback - * \return True terminates the collision check, false continues it to the next pair of objects */ + * \return True terminates the distance check, false continues it to the next pair of objects */ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& min_dist); /** \brief Create new FCLGeometry object out of robot link model. */ From c616ef9aa86b34c4b237ca7b1cdd66c19dfc6f20 Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Tue, 10 Nov 2020 16:04:04 -0500 Subject: [PATCH 513/612] Fix empty sequence in moveit_setup_assistant (#2406) --- moveit_setup_assistant/src/tools/moveit_config_data.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index c6351baef6..2ce3543a18 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -572,6 +572,10 @@ bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path) emitter << YAML::Newline; emitter << YAML::Comment(" - group: " + default_group_name) << YAML::Newline; emitter << YAML::Comment(" pose: home") << YAML::Newline; + + // Add empty list for valid yaml + emitter << YAML::BeginSeq; + emitter << YAML::EndSeq; } emitter << YAML::EndMap; From e8f1a4c8156fd1655f9fa6d8e0c57806b4c2ec8b Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 4 Nov 2020 13:08:20 -0600 Subject: [PATCH 514/612] Move timer initialization down to fix potential race condition --- moveit_ros/moveit_servo/src/servo_calcs.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 8e34341380..bbec0c173c 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -182,13 +182,13 @@ void ServoCalcs::start() initial_joint_trajectory->points.push_back(point); last_sent_command_ = initial_joint_trajectory; - timer_ = nh_.createTimer(period_, &ServoCalcs::run, this); - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); tf_moveit_to_ee_frame_ = current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * current_state_->getGlobalLinkTransform(parameters_.ee_frame_name); tf_moveit_to_robot_cmd_frame_ = current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * current_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); + + timer_ = nh_.createTimer(period_, &ServoCalcs::run, this); } void ServoCalcs::run(const ros::TimerEvent& timer_event) From 7f647b0594ef50674d711d97ac8f73b06bf06298 Mon Sep 17 00:00:00 2001 From: Thomas G Date: Sat, 14 Nov 2020 00:12:11 +0000 Subject: [PATCH 515/612] Changed processing_thread_ spin to use std::make_unique instead of new (#2412) --- moveit_core/background_processing/src/background_processing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/background_processing/src/background_processing.cpp b/moveit_core/background_processing/src/background_processing.cpp index 37991357b7..e2b05c11c1 100644 --- a/moveit_core/background_processing/src/background_processing.cpp +++ b/moveit_core/background_processing/src/background_processing.cpp @@ -46,7 +46,7 @@ BackgroundProcessing::BackgroundProcessing() // spin a thread that will process user events run_processing_thread_ = true; processing_ = false; - processing_thread_.reset(new boost::thread(boost::bind(&BackgroundProcessing::processingThread, this))); + processing_thread_ = std::make_unique(boost::bind(&BackgroundProcessing::processingThread, this)); } BackgroundProcessing::~BackgroundProcessing() From a15af07ee83b909ca5ef0a14e764dc87d62a93f1 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 16 Nov 2020 11:43:01 -0700 Subject: [PATCH 516/612] [feature] Low latency mode (#2401) Co-authored-by: AndyZe --- moveit_ros/moveit_servo/CMakeLists.txt | 10 + .../config/ur_simulated_config.yaml | 1 + .../include/moveit_servo/servo_calcs.h | 31 ++- .../include/moveit_servo/servo_parameters.h | 1 + moveit_ros/moveit_servo/src/servo.cpp | 13 +- moveit_ros/moveit_servo/src/servo_calcs.cpp | 133 ++++++++++--- .../test/config/servo_settings.yaml | 1 + .../config/servo_settings_low_latency.yaml | 68 +++++++ .../test/servo_low_latency_test.cpp | 185 ++++++++++++++++++ .../test/servo_low_latency_test.test | 18 ++ 10 files changed, 420 insertions(+), 41 deletions(-) create mode 100644 moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml create mode 100644 moveit_ros/moveit_servo/test/servo_low_latency_test.cpp create mode 100644 moveit_ros/moveit_servo/test/servo_low_latency_test.test diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index fcded51b2f..b167019ec2 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -175,6 +175,16 @@ if(CATKIN_ENABLE_TESTING) ${catkin_LIBRARIES} ) + # low_latency + add_rostest_gtest(servo_low_latency_test + test/servo_low_latency_test.test + test/servo_low_latency_test.cpp + ) + target_link_libraries(servo_low_latency_test + ${SERVO_LIB_NAME} + ${catkin_LIBRARIES} + ) + # pose_tracking add_rostest_gtest(pose_tracking_test test/pose_tracking_test.test diff --git a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml index 59b6599e5d..21a2c6aecd 100644 --- a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml @@ -15,6 +15,7 @@ low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the ## Properties of outgoing commands publish_period: 0.008 # 1/Nominal publish rate [seconds] +low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index c92171b179..055335e3f6 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -39,7 +39,9 @@ #pragma once // C++ +#include #include +#include // ROS #include @@ -69,10 +71,7 @@ class ServoCalcs ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - ~ServoCalcs() - { - timer_.stop(); - } + ~ServoCalcs(); /** \brief Start the timer where we do work and publish outputs */ void start(); @@ -106,8 +105,14 @@ class ServoCalcs void changeRobotLinkCommandFrame(const std::string& new_command_frame); private: - /** \brief Timer method */ - void run(const ros::TimerEvent& timer_event); + /** \brief Run the main calculation loop */ + void mainCalcLoop(); + + /** \brief Do calculations for a single iteration. Publish one outgoing command */ + void calculateSingleIteration(); + + /** \brief Stop the currently running thread */ + void stop(); /** \brief Do servoing calculations for Cartesian twist commands. */ bool cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); @@ -257,8 +262,6 @@ class ServoCalcs trajectory_msgs::JointTrajectoryConstPtr last_sent_command_; // ROS - ros::Timer timer_; - ros::Duration period_; ros::Subscriber joint_state_sub_; ros::Subscriber twist_stamped_sub_; ros::Subscriber joint_cmd_sub_; @@ -270,6 +273,10 @@ class ServoCalcs ros::ServiceServer control_dimensions_server_; ros::ServiceServer reset_servo_status_; + // Main tracking / result publisher loop + std::thread thread_; + bool stop_requested_; + // Status StatusCode status_ = StatusCode::NO_WARNING; bool paused_ = false; @@ -292,8 +299,8 @@ class ServoCalcs // The dimesions to control. In the command frame. [x, y, z, roll, pitch, yaw] std::array control_dimensions_ = { { true, true, true, true, true, true } }; - // latest_state_mutex_ is used to protect the state below it - mutable std::mutex latest_state_mutex_; + // input_mutex_ is used to protect the state below it + mutable std::mutex input_mutex_; Eigen::Isometry3d tf_moveit_to_robot_cmd_frame_; Eigen::Isometry3d tf_moveit_to_ee_frame_; geometry_msgs::TwistStampedConstPtr latest_twist_stamped_; @@ -302,5 +309,9 @@ class ServoCalcs ros::Time latest_joint_command_stamp_ = ros::Time(0.); bool latest_nonzero_twist_stamped_ = false; bool latest_nonzero_joint_cmd_ = false; + + // input condition variable used for low latency mode + std::condition_variable input_cv_; + bool new_input_cmd_ = false; }; } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h index 45b208db8e..14d993cc71 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h @@ -71,6 +71,7 @@ struct ServoParameters bool publish_joint_positions; bool publish_joint_velocities; bool publish_joint_accelerations; + bool low_latency_mode; // Collision checking bool check_collisions; std::string collision_check_type; diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index b35267cc5b..aba61fc1fe 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -136,7 +136,7 @@ bool Servo::readParameters() // 'collision_proximity_threshold' parameter was removed, replaced with separate self- and scene-collision proximity // thresholds // TODO(JStech): remove this deprecation warning in ROS Noetic; simplify error case handling - if (nh_.hasParam("collision_proximity_threshold") && + if (nh.hasParam("collision_proximity_threshold") && rosparam_shortcuts::get(LOGNAME, nh, "collision_proximity_threshold", collision_proximity_threshold)) { ROS_WARN_NAMED(LOGNAME, "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate" @@ -170,6 +170,17 @@ bool Servo::readParameters() error += !rosparam_shortcuts::get(LOGNAME, nh, "warning_topic", parameters_.status_topic); } + if (nh.hasParam("low_latency_mode")) + { + error += !rosparam_shortcuts::get(LOGNAME, nh, "low_latency_mode", parameters_.low_latency_mode); + } + else + { + ROS_WARN_NAMED(LOGNAME, "'low_latency_mode' is a new parameter that runs servo calc immediately after receiving " + "input. Setting to the default value of false."); + parameters_.low_latency_mode = false; + } + rosparam_shortcuts::shutdownIfError(LOGNAME, error); // Input checking diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index bbec0c173c..e94800d9d7 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -86,7 +86,7 @@ geometry_msgs::TransformStamped convertIsometryToTransform(const Eigen::Isometry // Constructor for the class that handles servoing calculations ServoCalcs::ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) - : nh_(nh), parameters_(parameters), planning_scene_monitor_(planning_scene_monitor), period_(parameters.publish_period) + : nh_(nh), parameters_(parameters), planning_scene_monitor_(planning_scene_monitor), stop_requested_(true) { // MoveIt Setup current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); @@ -150,8 +150,16 @@ ServoCalcs::ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, tf_moveit_to_robot_cmd_frame_ = empty_matrix; } +ServoCalcs::~ServoCalcs() +{ + stop(); +} + void ServoCalcs::start() { + // Stop the thread if we are currently running + stop(); + // We will update last_sent_command_ every time we start servo auto initial_joint_trajectory = moveit::util::make_shared_from_pool(); @@ -188,19 +196,78 @@ void ServoCalcs::start() tf_moveit_to_robot_cmd_frame_ = current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * current_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); - timer_ = nh_.createTimer(period_, &ServoCalcs::run, this); + stop_requested_ = false; + thread_ = std::thread([this] { mainCalcLoop(); }); + new_input_cmd_ = false; } -void ServoCalcs::run(const ros::TimerEvent& timer_event) +void ServoCalcs::stop() { - // Log warning when the last loop duration was longer than the period - if (timer_event.profile.last_duration.toSec() > period_.toSec()) + // Request stop + stop_requested_ = true; + + // Notify condition variable in case the thread is blocked on it { - ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, - "last_duration: " << timer_event.profile.last_duration.toSec() << " (" - << period_.toSec() << ")"); + // scope so the mutex is unlocked after so the thread can continue + // and therefore be joinable + const std::lock_guard lock(input_mutex_); + new_input_cmd_ = false; + input_cv_.notify_all(); + } + + // Join the thread + if (thread_.joinable()) + { + thread_.join(); + } +} + +void ServoCalcs::mainCalcLoop() +{ + ros::Rate rate(1.0 / parameters_.publish_period); + + while (ros::ok() && !stop_requested_) + { + // lock the input state mutex + std::unique_lock input_lock(input_mutex_); + + // low latency mode + if (parameters_.low_latency_mode) + { + input_cv_.wait(input_lock, [this] { return (new_input_cmd_ || stop_requested_); }); + + // break out of the loop if stop was requested + if (stop_requested_) + break; + } + + // reset new_input_cmd_ flag + new_input_cmd_ = false; + + // run servo calcs + const auto start_time = ros::Time::now(); + calculateSingleIteration(); + const auto run_duration = ros::Time::now() - start_time; + + // Log warning when the run duration was longer than the period + if (run_duration.toSec() > parameters_.publish_period) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "run_duration: " << run_duration.toSec() << " (" << parameters_.publish_period + << ")"); + } + + // normal mode, unlock input mutex and wait for the period of the loop + if (!parameters_.low_latency_mode) + { + input_lock.unlock(); + rate.sleep(); + } } +} +void ServoCalcs::calculateSingleIteration() +{ // Publish status each loop iteration auto status_msg = moveit::util::make_shared_from_pool(); status_msg->data = static_cast(status_); @@ -213,22 +280,20 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event) // Update from latest state current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - { - const std::lock_guard lock(latest_state_mutex_); - if (latest_twist_stamped_) - twist_stamped_cmd_ = *latest_twist_stamped_; - if (latest_joint_cmd_) - joint_servo_cmd_ = *latest_joint_cmd_; - - // Check for stale cmds - twist_command_is_stale_ = - ((ros::Time::now() - latest_twist_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); - joint_command_is_stale_ = - ((ros::Time::now() - latest_joint_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); - - have_nonzero_twist_stamped_ = latest_nonzero_twist_stamped_; - have_nonzero_joint_command_ = latest_nonzero_joint_cmd_; - } + + if (latest_twist_stamped_) + twist_stamped_cmd_ = *latest_twist_stamped_; + if (latest_joint_cmd_) + joint_servo_cmd_ = *latest_joint_cmd_; + + // Check for stale cmds + twist_command_is_stale_ = + ((ros::Time::now() - latest_twist_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); + joint_command_is_stale_ = + ((ros::Time::now() - latest_joint_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); + + have_nonzero_twist_stamped_ = latest_nonzero_twist_stamped_; + have_nonzero_joint_command_ = latest_nonzero_joint_cmd_; // Get the transform from MoveIt planning frame to servoing command frame // Calculate this transform to ensure it is available via C++ API @@ -980,7 +1045,7 @@ void ServoCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& del bool ServoCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) { - const std::lock_guard lock(latest_state_mutex_); + const std::lock_guard lock(input_mutex_); transform = tf_moveit_to_robot_cmd_frame_; // All zeros means the transform wasn't initialized, so return false @@ -989,7 +1054,7 @@ bool ServoCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) bool ServoCalcs::getCommandFrameTransform(geometry_msgs::TransformStamped& transform) { - const std::lock_guard lock(latest_state_mutex_); + const std::lock_guard lock(input_mutex_); // All zeros means the transform wasn't initialized, so return false if (tf_moveit_to_robot_cmd_frame_.matrix().isZero(0)) { @@ -1003,7 +1068,7 @@ bool ServoCalcs::getCommandFrameTransform(geometry_msgs::TransformStamped& trans bool ServoCalcs::getEEFrameTransform(Eigen::Isometry3d& transform) { - const std::lock_guard lock(latest_state_mutex_); + const std::lock_guard lock(input_mutex_); transform = tf_moveit_to_ee_frame_; // All zeros means the transform wasn't initialized, so return false @@ -1012,7 +1077,7 @@ bool ServoCalcs::getEEFrameTransform(Eigen::Isometry3d& transform) bool ServoCalcs::getEEFrameTransform(geometry_msgs::TransformStamped& transform) { - const std::lock_guard lock(latest_state_mutex_); + const std::lock_guard lock(input_mutex_); // All zeros means the transform wasn't initialized, so return false if (tf_moveit_to_ee_frame_.matrix().isZero(0)) { @@ -1025,22 +1090,30 @@ bool ServoCalcs::getEEFrameTransform(geometry_msgs::TransformStamped& transform) void ServoCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg) { - const std::lock_guard lock(latest_state_mutex_); + const std::lock_guard lock(input_mutex_); latest_twist_stamped_ = msg; latest_nonzero_twist_stamped_ = isNonZero(*latest_twist_stamped_); if (msg->header.stamp != ros::Time(0.)) latest_twist_command_stamp_ = msg->header.stamp; + + // notify that we have a new input + new_input_cmd_ = true; + input_cv_.notify_all(); } void ServoCalcs::jointCmdCB(const control_msgs::JointJogConstPtr& msg) { - const std::lock_guard lock(latest_state_mutex_); + const std::lock_guard lock(input_mutex_); latest_joint_cmd_ = msg; latest_nonzero_joint_cmd_ = isNonZero(*latest_joint_cmd_); if (msg->header.stamp != ros::Time(0.)) latest_joint_command_stamp_ = msg->header.stamp; + + // notify that we have a new input + new_input_cmd_ = true; + input_cv_.notify_all(); } void ServoCalcs::collisionVelocityScaleCB(const std_msgs::Float64ConstPtr& msg) diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml index 802aeadca2..b081f28c73 100644 --- a/moveit_ros/moveit_servo/test/config/servo_settings.yaml +++ b/moveit_ros/moveit_servo/test/config/servo_settings.yaml @@ -14,6 +14,7 @@ scale: low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less. ## Properties of outgoing commands +low_latency_mode: false # Set this to true to tie the output rate to the input rate publish_period: 0.01 # 1/Nominal publish rate [seconds] # What type of topic does your robot driver expect? diff --git a/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml b/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml new file mode 100644 index 0000000000..b470e02ac2 --- /dev/null +++ b/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml @@ -0,0 +1,68 @@ +############################################### +# Modify all parameters related to servoing here +############################################### +use_gazebo: false # Whether the robot is started in a Gazebo simulation environment + +## Properties of incoming commands +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.003 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.006 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 0.01 +low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less. + +## Properties of outgoing commands +low_latency_mode: true # Set this to true to tie the output rate to the input rate +publish_period: 0.01 # 1/Nominal publish rate [seconds] + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) +# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) +command_out_type: trajectory_msgs/JointTrajectory + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: false +publish_joint_accelerations: false + +## MoveIt properties +move_group_name: panda_arm # Often 'manipulator' or 'arm' +planning_frame: panda_link0 # The MoveIt planning frame. Often 'panda_link0' or 'world' + +## Other frames +ee_frame_name: panda_link7 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector + +## Stopping behaviour +incoming_command_timeout: 1 # Stop servoing if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 1 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 30 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 45 # Stop when the condition number hits this +joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + +## Topic names +cartesian_command_in_topic: servo_server/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: servo_server/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: joint_states +status_topic: servo_server/status # Publish status to this topic +command_out_topic: servo_server/command # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: stop_distance +# Parameters for "threshold_distance"-type collision checking +self_collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m] +scene_collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] diff --git a/moveit_ros/moveit_servo/test/servo_low_latency_test.cpp b/moveit_ros/moveit_servo/test/servo_low_latency_test.cpp new file mode 100644 index 0000000000..011e7bab1b --- /dev/null +++ b/moveit_ros/moveit_servo/test/servo_low_latency_test.cpp @@ -0,0 +1,185 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik LLC + * 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 PickNik LLC 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. + *********************************************************************/ + +/* Author: Tyler Weaver + Desc: Test for the low latency mode of servo +*/ + +// C++ +#include + +// ROS +#include + +// Testing +#include + +// Servo +#include +#include + +static const std::string LOGNAME = "servo_low_latency_test"; + +namespace moveit_servo +{ +class ServoFixture : public ::testing::Test +{ +public: + void SetUp() override + { + // Wait for several key topics / parameters + ros::topic::waitForMessage("/joint_states"); + while (!nh_.hasParam("/robot_description") && ros::ok()) + { + ros::Duration(0.1).sleep(); + } + + // Load the planning scene monitor + planning_scene_monitor_ = std::make_shared("robot_description"); + planning_scene_monitor_->startSceneMonitor(); + planning_scene_monitor_->startStateMonitor(); + planning_scene_monitor_->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + + // Create moveit_servo + servo_ = std::make_shared(nh_, planning_scene_monitor_); + } + void TearDown() override + { + } + +protected: + ros::NodeHandle nh_{ "~" }; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + moveit_servo::ServoPtr servo_; +}; // class ServoFixture + +TEST_F(ServoFixture, SendTwistStampedTest) +{ + servo_->start(); + + auto parameters = servo_->getParameters(); + + // count trajectory messages sent by servo + size_t received_count = 0; + boost::function traj_callback = + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& /*msg*/) { ++received_count; }; + auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); + + // Create publisher to send servo commands + auto twist_stamped_pub = nh_.advertise(parameters.cartesian_command_in_topic, 1); + + constexpr double test_duration = 1.0; + const double publish_period = parameters.publish_period; + const size_t num_commands = static_cast(test_duration / publish_period); + + // Set the rate differently from the publish period from the parameters to show that + // the number of outputs is set by the number of commands sent and not the rate they are sent. + ros::Rate publish_rate(2. / publish_period); + + // Send a few Cartesian velocity commands + for (size_t i = 0; i < num_commands && ros::ok(); ++i) + { + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "panda_link0"; + msg->twist.angular.y = 1.0; + + // Send the message + twist_stamped_pub.publish(msg); + publish_rate.sleep(); + } + + EXPECT_GT(received_count, num_commands - 20); + EXPECT_GT(received_count, (unsigned)0); + EXPECT_LT(received_count, num_commands + 20); + servo_->setPaused(true); +} + +TEST_F(ServoFixture, SendJointServoTest) +{ + servo_->start(); + + auto parameters = servo_->getParameters(); + + // count trajectory messages sent by servo + size_t received_count = 0; + boost::function traj_callback = + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& /*msg*/) { ++received_count; }; + auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); + + // Create publisher to send servo commands + auto joint_servo_pub = nh_.advertise(parameters.joint_command_in_topic, 1); + + constexpr double test_duration = 1.0; + const double publish_period = parameters.publish_period; + const size_t num_commands = static_cast(test_duration / publish_period); + + // Set the rate differently from the publish period from the parameters to show that + // the number of outputs is set by the number of commands sent and not the rate they are sent. + ros::Rate publish_rate(2. / publish_period); + + // Send a few joint velocity commands + for (size_t i = 0; i < num_commands && ros::ok(); ++i) + { + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "panda_link3"; + msg->velocities.push_back(0.1); + + // Send the message + joint_servo_pub.publish(msg); + publish_rate.sleep(); + } + + EXPECT_GT(received_count, num_commands - 20); + EXPECT_GT(received_count, (unsigned)0); + EXPECT_LT(received_count, num_commands + 20); + servo_->setPaused(true); +} +} // namespace moveit_servo + +int main(int argc, char** argv) +{ + ros::init(argc, argv, LOGNAME); + testing::InitGoogleTest(&argc, argv); + + ros::AsyncSpinner spinner(8); + spinner.start(); + + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/moveit_ros/moveit_servo/test/servo_low_latency_test.test b/moveit_ros/moveit_servo/test/servo_low_latency_test.test new file mode 100644 index 0000000000..6df90be617 --- /dev/null +++ b/moveit_ros/moveit_servo/test/servo_low_latency_test.test @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + From f56e17508c5f24f96c840a07243101f38f26096f Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 18 Nov 2020 01:24:43 -0800 Subject: [PATCH 517/612] Add Stargazer chart to README (#2417) --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index f84724d9e7..785b82079e 100644 --- a/README.md +++ b/README.md @@ -79,3 +79,8 @@ geometric_shapes | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ks rviz_marker_tools | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__rviz_marker_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__rviz_marker_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__rviz_marker_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__rviz_marker_tools__ubuntu_focal_amd64__binary) rviz_visual_tools | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__rviz_visual_tools__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__rviz_visual_tools__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__rviz_visual_tools__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__rviz_visual_tools__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__rviz_visual_tools__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__rviz_visual_tools__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__rviz_visual_tools__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__rviz_visual_tools__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__rviz_visual_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__rviz_visual_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__rviz_visual_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__rviz_visual_tools__ubuntu_focal_amd64__binary) srdfdom | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__srdfdom__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__srdfdom__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__srdfdom__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__srdfdom__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__srdfdom__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__srdfdom__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary) + + +## Stargazers over time + +[![Stargazers over time](https://starchart.cc/ros-planning/moveit.svg)](https://starchart.cc/ros-planning/moveit) From bd53490301b4c0076483e713af082e44dfcd4074 Mon Sep 17 00:00:00 2001 From: Stuart Anderson <57372215+stuart-fb@users.noreply.github.com> Date: Wed, 18 Nov 2020 09:05:36 -0500 Subject: [PATCH 518/612] Fix OrientationConstraint::decide (#2414) * Fix OrientationConstraint::decide * Add new test case for OrientationConstraintsSimple * Add comments for clarity --- .../kinematic_constraints/src/kinematic_constraint.cpp | 7 ++++--- .../kinematic_constraints/test/test_constraints.cpp | 7 +++++++ 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index f6f74656b3..7bc9c6434e 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -620,9 +620,10 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob xyz = diff.linear().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints } - xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi() - fabs(xyz(0))); - xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi() - fabs(xyz(1))); - xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi() - fabs(xyz(2))); + // Account for angle wrapping + xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::two_pi() - fabs(xyz(0))); + xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::two_pi() - fabs(xyz(1))); + xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::two_pi() - fabs(xyz(2))); bool result = xyz(2) < absolute_z_axis_tolerance_ + std::numeric_limits::epsilon() && xyz(1) < absolute_y_axis_tolerance_ + std::numeric_limits::epsilon() && xyz(0) < absolute_x_axis_tolerance_ + std::numeric_limits::epsilon(); diff --git a/moveit_core/kinematic_constraints/test/test_constraints.cpp b/moveit_core/kinematic_constraints/test/test_constraints.cpp index 2ffe6df1ee..2dcb7acbf9 100644 --- a/moveit_core/kinematic_constraints/test/test_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_constraints.cpp @@ -40,6 +40,7 @@ #include #include #include +#include class LoadPlanningModelsPr2 : public testing::Test { @@ -652,6 +653,12 @@ TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple) robot_state.setVariablePositions(jvals); robot_state.update(); EXPECT_FALSE(oc.decide(robot_state).satisfied); + + // rotation by pi does not wrap to zero + jvals["r_wrist_roll_joint"] = boost::math::constants::pi(); + robot_state.setVariablePositions(jvals); + robot_state.update(); + EXPECT_FALSE(oc.decide(robot_state).satisfied); } TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple) From eca2deb1b80cbeb3eceaf00090ad853cc12851f2 Mon Sep 17 00:00:00 2001 From: harshgupta246 <53207868+harshgupta246@users.noreply.github.com> Date: Fri, 20 Nov 2020 23:01:05 +0530 Subject: [PATCH 519/612] Updated First Timers Only Issue Template (#2419) --- .github/ISSUE_TEMPLATE/first_timers_only.md | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/first_timers_only.md b/.github/ISSUE_TEMPLATE/first_timers_only.md index b907471b29..c85e0638a7 100644 --- a/.github/ISSUE_TEMPLATE/first_timers_only.md +++ b/.github/ISSUE_TEMPLATE/first_timers_only.md @@ -28,7 +28,9 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa - [ ] 🙋 **Claim this issue**: Comment below. If someone else has claimed it, ask if they've opened a pull request already and if they're stuck -- maybe you can help them solve a problem or move it along! -- [ ] 🗄️**Create a local workspace** for making your changes and testing [follwing these instructions](https://moveit.ros.org/install/source/) +- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://moveit.ros.org/install/source/) + +- [ ] 🍴 **Fork the repository** using the handy button at the top of the repository page and **clone** it into `~/ws_moveit/src/moveit`, [here is a guide that you can follow](https://guides.github.com/activities/forking/) (You will have to remove or empty the existing `moveit` folder before cloning your own fork) - [ ] 📝 **Update** the file(s) `$FILENAME`by applying the change shown in the diff below. @@ -38,9 +40,9 @@ $DIFF - [ ] 🤖 **Apply `clang-format-10`** auto formatting, [following these instructions](https://moveit.ros.org//documentation/contributing/code/?#clang-format-auto-code-formatting) -- [ ] 💾 **Commit** your changes +- [ ] 💾 **Commit and Push** your changes -- [ ] 🔀 **Start a Pull Request**. There are two ways how you can start a pull request: +- [ ] 🔀 **Start a Pull Request** to request to merge your code into `master`. There are two ways that you can start a pull request: 1. If you are not familiar with GitHub or how to create a pull request, [here is a guide you can follow](https://guides.github.com/activities/hello-world/) on how GitHub works. 2. If you are familiar with the terminal or would like to learn to use it, [here is a great tutorial](https://egghead.io/series/how-to-contribute-to-an-open-source-project-on-github) on how to send a pull request using the terminal. From 712a73a485226e7f7b9344136058347a4e2019ff Mon Sep 17 00:00:00 2001 From: Nathan Brooks Date: Fri, 20 Nov 2020 12:38:53 -0700 Subject: [PATCH 520/612] Prevent moveit_servo transforms between fixed frames from causing timeout (#2418) --- moveit_ros/moveit_servo/src/pose_tracking.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp index e8dcff87b1..c51a54f75a 100644 --- a/moveit_ros/moveit_servo/src/pose_tracking.cpp +++ b/moveit_ros/moveit_servo/src/pose_tracking.cpp @@ -238,6 +238,9 @@ void PoseTracking::targetPoseCallback(const geometry_msgs::PoseStampedConstPtr& geometry_msgs::TransformStamped target_to_planning_frame = transform_buffer_.lookupTransform( planning_frame_, target_pose_.header.frame_id, ros::Time(0), ros::Duration(0.1)); tf2::doTransform(target_pose_, target_pose_, target_to_planning_frame); + + // Prevent doTransform from copying a stamp of 0, which will cause the haveRecentTargetPose check to fail servo motions + target_pose_.header.stamp = ros::Time::now(); } catch (const tf2::TransformException& ex) { From ad8ce6621bd676ef93f02d96753400d32ee6b58b Mon Sep 17 00:00:00 2001 From: archonixX <72344256+archonixX@users.noreply.github.com> Date: Thu, 26 Nov 2020 00:59:33 -0700 Subject: [PATCH 521/612] docker-source: support running clang-tidy (#2430) --- .docker/source/Dockerfile | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile index 1cfeb73e87..4328ec4ded 100644 --- a/.docker/source/Dockerfile +++ b/.docker/source/Dockerfile @@ -24,7 +24,11 @@ RUN \ ENV PYTHONIOENCODING UTF-8 RUN cd .. && \ - catkin config --extend /opt/ros/$ROS_DISTRO --install --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + catkin config --extend /opt/ros/$ROS_DISTRO --install --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON && \ # Status rate is limited so that just enough info is shown to keep Docker from timing out, # but not too much such that the Docker log gets too long (another form of timeout) catkin build --limit-status-rate 0.001 --no-notify + +# Environment variable used in instructions on moveit.ros.org website for running clang-tidy +ENV CATKIN_WS /root/ws_moveit + From bdd4bde4938c7bdac3a57a791cf738146c8834e0 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 23 Nov 2020 15:02:26 -0600 Subject: [PATCH 522/612] Delete CollisionRequest min_cost_density --- .../include/moveit/collision_detection/collision_common.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index fcda6e9efc..fde530f553 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -190,7 +190,6 @@ struct CollisionRequest , max_contacts(1) , max_contacts_per_pair(1) , max_cost_sources(1) - , min_cost_density(0.2) , verbose(false) { } @@ -220,9 +219,6 @@ struct CollisionRequest /** \brief When costs are computed, this value defines how many of the top cost sources should be returned */ std::size_t max_cost_sources; - /** \brief When costs are computed, this is the minimum cost density for a CostSource to be included in the results */ - double min_cost_density; - /** \brief Function call that decides whether collision detection should stop. */ boost::function is_done; From 10d6380438b2a5a5798ef9e02daf173e7a67b82c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 8 Dec 2020 22:17:32 +0100 Subject: [PATCH 523/612] Upgrade cmake_minimum_required to 3.1 (#2453) to suppress CMP0048 warning --- .../pilz_industrial_motion_planner/CMakeLists.txt | 6 +++--- .../pilz_industrial_motion_planner_testutils/CMakeLists.txt | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt index 7c3f86c8a1..05ed96c791 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(pilz_industrial_motion_planner) ## Add support for C++11, supported in ROS Kinetic and newer @@ -339,7 +339,7 @@ if(CATKIN_ENABLE_TESTING) test/unittest_velocity_profile_atrap.cpp src/velocity_profile_atrap.cpp ) - target_link_libraries(unittest_velocity_profile_atrap + target_link_libraries(unittest_velocity_profile_atrap ${catkin_LIBRARIES} ) @@ -502,7 +502,7 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gmock(unittest_get_solver_tip_frame test/unittest_get_solver_tip_frame.cpp ) - target_link_libraries(unittest_get_solver_tip_frame + target_link_libraries(unittest_get_solver_tip_frame ${catkin_LIBRARIES} ) diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt index fc692408c9..3e8b72f629 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(pilz_industrial_motion_planner_testutils) ## Add support for C++11, supported in ROS Kinetic and newer From 7e1ccecde9b956c358d9d40630b13635171ec70d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 9 Dec 2020 08:24:50 +0100 Subject: [PATCH 524/612] RobotModelBuilder: Allow adding end effectors (#2454) --- .../include/moveit/utils/robot_model_test_utils.h | 3 +++ moveit_core/utils/src/robot_model_test_utils.cpp | 11 +++++++++++ 2 files changed, 14 insertions(+) diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h index ad6246dc37..e4946d85d5 100644 --- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h @@ -164,6 +164,9 @@ class RobotModelBuilder */ void addGroup(const std::vector& links, const std::vector& joints, const std::string& name); + void addEndEffector(const std::string& name, const std::string& parent_link, const std::string& parent_group = "", + const std::string& component_group = ""); + /** \} */ /** \brief Returns true if the building process so far has been valid. */ diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index eabeb14af1..ad73be4756 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -342,6 +342,17 @@ void RobotModelBuilder::addGroup(const std::vector& links, const st srdf_writer_->groups_.push_back(new_group); } +void RobotModelBuilder::addEndEffector(const std::string& name, const std::string& parent_link, + const std::string& parent_group, const std::string& component_group) +{ + srdf::Model::EndEffector eef; + eef.name_ = name; + eef.parent_link_ = parent_link; + eef.parent_group_ = parent_group; + eef.component_group_ = component_group; + srdf_writer_->end_effectors_.push_back(eef); +} + bool RobotModelBuilder::isValid() { return is_valid_; From 5d2549fe5018bca5fa5cf22f90ecdc7b8b3f7762 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 27 Nov 2020 23:18:33 +0300 Subject: [PATCH 525/612] RobotModelBuilder: Add parameter to specify the joint rotation axis --- .../utils/include/moveit/utils/robot_model_test_utils.h | 4 +++- moveit_core/utils/src/robot_model_test_utils.cpp | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h index e4946d85d5..0667b0408e 100644 --- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h @@ -101,9 +101,11 @@ class RobotModelBuilder * the joints will be given this type. To add multiple types of joints, call this method multiple times * \param[in] joint_origins The "parent to joint" origins for the joints connecting the links. If not used, all * origins will default to the identity transform + * \param[in] joint_axis The joint axis specified in the joint frame defaults to (1,0,0) */ void addChain(const std::string& section, const std::string& type, - const std::vector& joint_origins = {}); + const std::vector& joint_origins = {}, + urdf::Vector3 joint_axis = urdf::Vector3(1.0, 0.0, 0.0)); /** \brief Adds a collision mesh to a specific link. * \param[in] link_name The name of the link to which the mesh will be added. Must already be in the builder diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index ad73be4756..3492e12dbd 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -110,7 +110,7 @@ RobotModelBuilder::RobotModelBuilder(const std::string& name, const std::string& } void RobotModelBuilder::addChain(const std::string& section, const std::string& type, - const std::vector& joint_origins) + const std::vector& joint_origins, urdf::Vector3 joint_axis) { std::vector link_names; boost::split_regex(link_names, section, boost::regex("->")); @@ -182,7 +182,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& return; } - joint->axis = urdf::Vector3(1.0, 0.0, 0.0); + joint->axis = joint_axis; if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::PRISMATIC) { urdf::JointLimitsSharedPtr limits(new urdf::JointLimits); From 253a2177f23c2d5e2e3fa415c75f720c9d6a98a5 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 27 Nov 2020 23:19:50 +0300 Subject: [PATCH 526/612] Fix validation of orientation constraints (#2434) * Add tests that trigger the bug * Use the angle differece between the desired and the current orientation to check if the constraint is satisfied * Handle singularities --- .../kinematic_constraints/CMakeLists.txt | 3 + .../src/kinematic_constraint.cpp | 80 +++++- .../test/test_orientation_constraints.cpp | 244 ++++++++++++++++++ 3 files changed, 320 insertions(+), 7 deletions(-) create mode 100644 moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index 619f7a4229..31198de4aa 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -20,4 +20,7 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_constraints test/test_constraints.cpp) target_link_libraries(test_constraints moveit_test_utils ${MOVEIT_LIB_NAME}) + + catkin_add_gtest(test_orientation_constraints test/test_orientation_constraints.cpp) + target_link_libraries(test_orientation_constraints moveit_test_utils ${MOVEIT_LIB_NAME}) endif() diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 7bc9c6434e..9a63c5c3b4 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -58,6 +58,58 @@ static double normalizeAngle(double angle) return v; } +// Normalizes an angle to the interval [-pi, +pi] and then take the absolute value +// The returned values will be in the following range [0, +pi] +static double normalizeAbsoluteAngle(const double angle) +{ + const double normalized_angle = std::fmod(std::abs(angle), 2 * M_PI); + return std::min(2 * M_PI - normalized_angle, normalized_angle); +} + +/** + * This's copied from + * https://gitlab.com/libeigen/eigen/-/blob/master/unsupported/Eigen/src/EulerAngles/EulerSystem.h#L187 + * Return the intrinsic Roll-Pitch-Yaw euler angles given the input rotation matrix and boolean indicating whether the + * there's a singularity in the input rotation matrix (true: the input rotation matrix don't have a singularity, false: + * the input rotation matrix have a singularity) The returned angles are in the ranges [-pi:pi]x[-pi/2:pi/2]x[-pi:pi] + */ +template +std::tuple::Scalar, 3, 1>, bool> +CalcEulerAngles(const Eigen::MatrixBase& R) +{ + using std::atan2; + using std::sqrt; + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) + using Index = EIGEN_DEFAULT_DENSE_INDEX_TYPE; + using Scalar = typename Eigen::MatrixBase::Scalar; + const Index i = 0; + const Index j = 1; + const Index k = 2; + Eigen::Matrix res; + const Scalar rsum = sqrt((R(i, i) * R(i, i) + R(i, j) * R(i, j) + R(j, k) * R(j, k) + R(k, k) * R(k, k)) / 2); + res[1] = atan2(R(i, k), rsum); + // There is a singularity when cos(beta) == 0 + if (rsum > 4 * Eigen::NumTraits::epsilon()) + { // cos(beta) != 0 + res[0] = atan2(-R(j, k), R(k, k)); + res[2] = atan2(-R(i, j), R(i, i)); + return { res, true }; + } + else if (R(i, k) > 0) + { // cos(beta) == 0 and sin(beta) == 1 + const Scalar spos = R(j, i) + R(k, j); // 2*sin(alpha + gamma) + const Scalar cpos = R(j, j) - R(k, i); // 2*cos(alpha + gamma) + res[0] = atan2(spos, cpos); + res[2] = 0; + return { res, false }; + } // cos(beta) == 0 and sin(beta) == -1 + const Scalar sneg = R(k, j) - R(j, i); // 2*sin(alpha + gamma) + const Scalar cneg = R(j, j) + R(k, i); // 2*cos(alpha + gamma) + res[0] = atan2(sneg, cneg); + res[2] = 0; + return { res, false }; +} + KinematicConstraint::KinematicConstraint(const moveit::core::RobotModelConstPtr& model) : type_(UNKNOWN_CONSTRAINT), robot_model_(model), constraint_weight_(std::numeric_limits::epsilon()) { @@ -603,27 +655,41 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob if (!link_model_) return ConstraintEvaluationResult(true, 0.0); - Eigen::Vector3d xyz; + std::tuple euler_angles_error; if (mobile_frame_) { // getFrameTransform() returns a valid isometry by contract Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).linear() * desired_rotation_matrix_; // getGlobalLinkTransform() returns a valid isometry by contract Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).linear()); // valid isometry - xyz = diff.linear().eulerAngles(0, 1, 2); - // 0,1,2 corresponds to XYZ, the convention used in sampling constraints + euler_angles_error = CalcEulerAngles(diff.linear()); } else { // diff is valid isometry by construction Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).linear()); - xyz = diff.linear().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints + euler_angles_error = CalcEulerAngles(diff.linear()); } + // Converting from a rotation matrix to an intrinsic XYZ euler angles have 2 singularities: + // pitch ~= pi/2 ==> roll + yaw = theta + // pitch ~= -pi/2 ==> roll - yaw = theta + // in those cases CalcEulerAngles will set roll (xyz(0)) to theta and yaw (xyz(2)) to zero, so for us to be able to + // capture yaw tolerance violation we do the following, if theta violate the absolute yaw tolerance we think of it as + // pure yaw rotation and set roll to zero + auto& xyz = std::get(euler_angles_error); + if (!std::get(euler_angles_error)) + { + if (normalizeAbsoluteAngle(xyz(0)) > absolute_z_axis_tolerance_ + std::numeric_limits::epsilon()) + { + xyz(2) = xyz(0); + xyz(0) = 0; + } + } // Account for angle wrapping - xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::two_pi() - fabs(xyz(0))); - xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::two_pi() - fabs(xyz(1))); - xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::two_pi() - fabs(xyz(2))); + xyz = xyz.unaryExpr(&normalizeAbsoluteAngle); + + // 0,1,2 corresponds to XYZ, the convention used in sampling constraints bool result = xyz(2) < absolute_z_axis_tolerance_ + std::numeric_limits::epsilon() && xyz(1) < absolute_y_axis_tolerance_ + std::numeric_limits::epsilon() && xyz(0) < absolute_x_axis_tolerance_ + std::numeric_limits::epsilon(); diff --git a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp new file mode 100644 index 0000000000..dc4eb8a37e --- /dev/null +++ b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp @@ -0,0 +1,244 @@ +#include +#include +#include +#include +#include +#include + +class SphericalRobot : public testing::Test +{ +protected: + void SetUp() override + { + moveit::core::RobotModelBuilder builder("robot", "base_link"); + geometry_msgs::Pose origin; + origin.orientation.w = 1; + builder.addChain("base_link->roll", "revolute", { origin }, urdf::Vector3(1, 0, 0)); + builder.addChain("roll->pitch", "revolute", { origin }, urdf::Vector3(0, 1, 0)); + builder.addChain("pitch->yaw", "revolute", { origin }, urdf::Vector3(0, 0, 1)); + ASSERT_TRUE(builder.isValid()); + robot_model_ = builder.build(); + } + + std::map getJointValues(const double roll, const double pitch, const double yaw) + { + std::map jvals; + jvals["base_link-roll-joint"] = roll; + jvals["roll-pitch-joint"] = pitch; + jvals["pitch-yaw-joint"] = yaw; + return jvals; + } + + void TearDown() override + { + } + +protected: + moveit::core::RobotModelPtr robot_model_; +}; + +TEST_F(SphericalRobot, Test1) +{ + kinematic_constraints::OrientationConstraint oc(robot_model_); + + moveit::core::Transforms tf(robot_model_->getModelFrame()); + moveit_msgs::OrientationConstraint ocm; + + ocm.link_name = "yaw"; + ocm.header.frame_id = robot_model_->getModelFrame(); + ocm.orientation.x = 0.0; + ocm.orientation.y = 0.0; + ocm.orientation.z = 0.0; + ocm.orientation.w = 1.0; + ocm.absolute_x_axis_tolerance = 0.8; + ocm.absolute_y_axis_tolerance = 0.8; + ocm.absolute_z_axis_tolerance = 3.15; + ocm.weight = 1.0; + + moveit::core::RobotState robot_state(robot_model_); + // This's identical to a -1.57rad rotation around Z-axis + robot_state.setVariablePositions(getJointValues(3.140208, 3.141588, 1.570821)); + robot_state.update(); + + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_TRUE(oc.decide(robot_state).satisfied); +} + +TEST_F(SphericalRobot, Test2) +{ + kinematic_constraints::OrientationConstraint oc(robot_model_); + + moveit::core::Transforms tf(robot_model_->getModelFrame()); + moveit_msgs::OrientationConstraint ocm; + + ocm.link_name = "yaw"; + ocm.header.frame_id = robot_model_->getModelFrame(); + ocm.orientation.x = 0.0; + ocm.orientation.y = 0.0; + ocm.orientation.z = 0.0; + ocm.orientation.w = 1.0; + ocm.absolute_x_axis_tolerance = 0.2; + ocm.absolute_y_axis_tolerance = 2.0; + ocm.absolute_z_axis_tolerance = 0.2; + ocm.weight = 1.0; + + moveit::core::RobotState robot_state(robot_model_); + // Singularity: roll + yaw = theta + // These violate either absolute_x_axis_tolerance or absolute_z_axis_tolerance + robot_state.setVariablePositions(getJointValues(0.15, boost::math::constants::half_pi(), 0.15)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.21, boost::math::constants::half_pi(), 0.0)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi(), 0.21)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + // Singularity: roll - yaw = theta + // This's identical to -pi/2 pitch rotation + robot_state.setVariablePositions(getJointValues(0.15, -boost::math::constants::half_pi(), 0.15)); + robot_state.update(); + + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_TRUE(oc.decide(robot_state).satisfied); +} + +TEST_F(SphericalRobot, Test3) +{ + kinematic_constraints::OrientationConstraint oc(robot_model_); + + moveit::core::Transforms tf(robot_model_->getModelFrame()); + moveit_msgs::OrientationConstraint ocm; + + ocm.link_name = "yaw"; + ocm.header.frame_id = robot_model_->getModelFrame(); + ocm.orientation.x = 0.0; + ocm.orientation.y = 0.0; + ocm.orientation.z = 0.0; + ocm.orientation.w = 1.0; + ocm.absolute_x_axis_tolerance = 0.2; + ocm.absolute_y_axis_tolerance = 2.0; + ocm.absolute_z_axis_tolerance = 0.3; + ocm.weight = 1.0; + + moveit::core::RobotState robot_state(robot_model_); + // Singularity: roll + yaw = theta + + // These tests violate absolute_x_axis_tolerance + robot_state.setVariablePositions(getJointValues(0.21, boost::math::constants::half_pi(), 0.0)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi(), 0.21)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + ocm.absolute_x_axis_tolerance = 0.3; + ocm.absolute_y_axis_tolerance = 2.0; + ocm.absolute_z_axis_tolerance = 0.2; + + // These tests violate absolute_z_axis_tolerance + robot_state.setVariablePositions(getJointValues(0.21, boost::math::constants::half_pi(), 0.0)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi(), 0.21)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); +} + +TEST_F(SphericalRobot, Test4) +{ + kinematic_constraints::OrientationConstraint oc(robot_model_); + + moveit::core::Transforms tf(robot_model_->getModelFrame()); + moveit_msgs::OrientationConstraint ocm; + + ocm.link_name = "yaw"; + ocm.header.frame_id = robot_model_->getModelFrame(); + ocm.orientation.x = 0.0; + ocm.orientation.y = 0.0; + ocm.orientation.z = 0.0; + ocm.orientation.w = 1.0; + ocm.absolute_x_axis_tolerance = 0.2; + ocm.absolute_y_axis_tolerance = 2.0; + ocm.absolute_z_axis_tolerance = 0.3; + ocm.weight = 1.0; + + moveit::core::RobotState robot_state(robot_model_); + // Singularity: roll + yaw = theta + + // These tests violate absolute_x_axis_tolerance + robot_state.setVariablePositions(getJointValues(0.21, -boost::math::constants::half_pi(), 0.0)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.0, -boost::math::constants::half_pi(), 0.21)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + ocm.absolute_x_axis_tolerance = 0.3; + ocm.absolute_y_axis_tolerance = 2.0; + ocm.absolute_z_axis_tolerance = 0.2; + + // These tests violate absolute_z_axis_tolerance + robot_state.setVariablePositions(getJointValues(0.21, -boost::math::constants::half_pi(), 0.0)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.0, -boost::math::constants::half_pi(), 0.21)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.5, -boost::math::constants::half_pi(), 0.21)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); +} + +// Both the current and the desired orientations are in singularities +TEST_F(SphericalRobot, Test5) +{ + kinematic_constraints::OrientationConstraint oc(robot_model_); + + moveit::core::Transforms tf(robot_model_->getModelFrame()); + moveit_msgs::OrientationConstraint ocm; + + moveit::core::RobotState robot_state(robot_model_); + robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi(), 0.0)); + robot_state.update(); + + ocm.link_name = "yaw"; + ocm.header.frame_id = robot_model_->getModelFrame(); + tf2::convert(Eigen::Quaterniond(robot_state.getGlobalLinkTransform(ocm.link_name).linear()), ocm.orientation); + ocm.absolute_x_axis_tolerance = 0.0; + ocm.absolute_y_axis_tolerance = 0.0; + ocm.absolute_z_axis_tolerance = 1.0; + ocm.weight = 1.0; + + robot_state.setVariablePositions(getJointValues(0.2, boost::math::constants::half_pi(), 0.3)); + robot_state.update(); + + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_TRUE(oc.decide(robot_state, true).satisfied); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 97c0b28f5580eadaade3f752f66f8eb12b0f7063 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 17 Dec 2020 11:01:38 -0600 Subject: [PATCH 527/612] Fix doxygen documentation for setToIKSolverFrame (#2461) * Fix doxygen documentation for setToIKSolverFrame * "Convert" -> "Transform" * Make function private. Update comments. * Make inline and private * Longer function should not be inline Co-authored-by: Robert Haschke Co-authored-by: Robert Haschke --- .../include/moveit/robot_state/robot_state.h | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index a762528935..db7e2470e9 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -900,22 +900,6 @@ class RobotState * @{ */ - /** - * \brief Convert the frame of reference of the pose to that same frame as the IK solver expects - * @param pose - the input to change - * @param solver - a kin solver whose base frame is important to us - * @return true if no error - */ - bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver); - - /** - * \brief Convert the frame of reference of the pose to that same frame as the IK solver expects - * @param pose - the input to change - * @param ik_frame - the name of frame of reference of base of ik solver - * @return true if no error - */ - bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame); - /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. @@ -1757,6 +1741,22 @@ class RobotState void initTransforms(); void copyFrom(const RobotState& other); + /** + * \brief Transform pose from the robot model's base frame to the reference frame of the IK solver + * @param pose - the input to change + * @param solver - a kin solver whose base frame is important to us + * @return true if no error + */ + inline bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver); + + /** + * \brief Transform pose from the robot model's base frame to the reference frame of the IK solver + * @param pose - the input to change + * @param ik_frame - the name of frame of reference of base of ik solver + * @return true if no error + */ + bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame); + void markDirtyJointTransforms(const JointModel* joint) { dirty_joint_transforms_[joint->getJointIndex()] = 1; From 798b32f6769d3bd5635cf50a4aa3ef2a269558d3 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 18 Dec 2020 15:12:24 -0600 Subject: [PATCH 528/612] Add an important sleep in Servo pose tracking (#2463) --- moveit_ros/moveit_servo/src/pose_tracking.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp index c51a54f75a..0c134db35c 100644 --- a/moveit_ros/moveit_servo/src/pose_tracking.cpp +++ b/moveit_ros/moveit_servo/src/pose_tracking.cpp @@ -128,6 +128,11 @@ PoseTrackingStatusCode PoseTracking::moveToPose(const Eigen::Vector3d& positiona // Compute servo command from PID controller output and send it to the Servo object, for execution twist_stamped_pub_.publish(calculateTwistCommand()); + + if (!loop_rate_.sleep()) + { + ROS_WARN_STREAM_THROTTLE_NAMED(1, LOGNAME, "Target control rate was missed"); + } } doPostMotionReset(); From da926ac95154ea2699247fedf6e3690b15d8eef3 Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Mon, 21 Dec 2020 10:55:49 -0500 Subject: [PATCH 529/612] Missing RViz and moveit_simple_controller_manager dependencies in MSA template (#2455) --- .../templates/moveit_config_pkg_template/package.xml.template | 2 ++ 1 file changed, 2 insertions(+) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template index a4d4c40b2c..c0555ce40c 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template @@ -22,9 +22,11 @@ moveit_planners_ompl moveit_ros_visualization moveit_setup_assistant + moveit_simple_controller_manager joint_state_publisher joint_state_publisher_gui robot_state_publisher + rviz tf2_ros xacro From abb83f178075b4d5bb655df2dc9dd3e400e258ca Mon Sep 17 00:00:00 2001 From: Udbhavbisarya23 <43880582+Udbhavbisarya23@users.noreply.github.com> Date: Wed, 23 Dec 2020 01:54:33 +0530 Subject: [PATCH 530/612] Fix some typos in comments (#2466) --- moveit_ros/planning/plan_execution/src/plan_execution.cpp | 2 +- moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index e7657122d9..f4ef67a931 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -220,7 +220,7 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p if (preempt_requested_) break; - // execute the trajectory, and monitor its executionm + // execute the trajectory, and monitor its execution plan.error_code_ = executeAndMonitor(plan); } diff --git a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp index 0078ad5578..1ef3147279 100644 --- a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp @@ -150,7 +150,7 @@ bool plan_execution::PlanWithSensing::computePlan(ExecutableMotionPlan& plan, unsigned int look_attempts = 0; // this flag is set to true when all conditions for looking around are met, and the command is sent. - // the intention is for the planning looop not to terminate when having just looked around + // the intention is for the planning loop not to terminate when having just looked around bool just_looked_around = false; // this flag indicates whether the last lookAt() operation failed. If this operation fails once, we assume that @@ -212,7 +212,7 @@ bool plan_execution::PlanWithSensing::computePlan(ExecutableMotionPlan& plan, bool looked_at_result = lookAt(cost_sources, plan.planning_scene_->getPlanningFrame()); if (looked_at_result) - ROS_INFO("Sensor was succesfully actuated. Attempting to recompute a motion plan."); + ROS_INFO("Sensor was successfully actuated. Attempting to recompute a motion plan."); else { if (look_around_failed) From be0cbaff7546348b9c19bd46a98ee253341f2c97 Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Mon, 28 Dec 2020 11:40:06 +0100 Subject: [PATCH 531/612] Catch exceptions during RobotModel loading in rviz (#2468) --- .../src/robot_state_display.cpp | 35 +++++++++++-------- .../src/trajectory_display.cpp | 35 +++++++++++-------- 2 files changed, 42 insertions(+), 28 deletions(-) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 983622814f..9568dd8978 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -375,20 +375,27 @@ void RobotStateDisplay::loadRobotModel() if (rdf_loader_->getURDF()) { - const srdf::ModelSharedPtr& srdf = - rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); - robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); - robot_->load(*robot_model_->getURDF()); - robot_state_.reset(new moveit::core::RobotState(robot_model_)); - robot_state_->setToDefaultValues(); - bool old_state = root_link_name_property_->blockSignals(true); - root_link_name_property_->setStdString(getRobotModel()->getRootLinkName()); - root_link_name_property_->blockSignals(old_state); - update_state_ = true; - setStatus(rviz::StatusProperty::Ok, "RobotModel", "Loaded successfully"); - - changedEnableVisualVisible(); - changedEnableCollisionVisible(); + try + { + const srdf::ModelSharedPtr& srdf = + rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); + robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); + robot_->load(*robot_model_->getURDF()); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); + robot_state_->setToDefaultValues(); + bool old_state = root_link_name_property_->blockSignals(true); + root_link_name_property_->setStdString(getRobotModel()->getRootLinkName()); + root_link_name_property_->blockSignals(old_state); + update_state_ = true; + setStatus(rviz::StatusProperty::Ok, "RobotModel", "Loaded successfully"); + + changedEnableVisualVisible(); + changedEnableCollisionVisible(); + } + catch (std::exception& e) + { + setStatus(rviz::StatusProperty::Error, "RobotModel", QString("Loading failed: %1").arg(e.what())); + } } else setStatus(rviz::StatusProperty::Error, "RobotModel", "Loading failed"); diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp index e5fa830afa..a3b5edb7d1 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -64,22 +64,29 @@ void TrajectoryDisplay::onInitialize() void TrajectoryDisplay::loadRobotModel() { - rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); - - if (!rdf_loader_->getURDF()) + try { - this->setStatus(rviz::StatusProperty::Error, "Robot Model", - "Failed to load from parameter " + robot_description_property_->getString()); - return; + rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); + + if (!rdf_loader_->getURDF()) + { + this->setStatus(rviz::StatusProperty::Error, "Robot Model", + "Failed to load from parameter " + robot_description_property_->getString()); + return; + } + this->setStatus(rviz::StatusProperty::Ok, "Robot Model", "Successfully loaded"); + + const srdf::ModelSharedPtr& srdf = + rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); + robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); + + // Send to child class + trajectory_visual_->onRobotModelLoaded(robot_model_); + } + catch (std::exception& e) + { + setStatus(rviz::StatusProperty::Error, "RobotModel", QString("Loading failed: %1").arg(e.what())); } - this->setStatus(rviz::StatusProperty::Ok, "Robot Model", "Successfully loaded"); - - const srdf::ModelSharedPtr& srdf = - rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); - robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); - - // Send to child class - trajectory_visual_->onRobotModelLoaded(robot_model_); } void TrajectoryDisplay::reset() From 59a372484f8f928152eec71bad636c58087d0f15 Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Thu, 31 Dec 2020 16:25:32 +0100 Subject: [PATCH 532/612] Provide a function to set the position of active joints in a JointModelGroup (#2456) * RobotState::setJointGroupPositions: assert correct size of vector fixes #2442 * setJointGroupActivePositions sets only the positions of active joints * Add overloads * fix asserts * implement JointModelGroup::getActiveVariableCount --- .../moveit/robot_model/joint_model_group.h | 10 +++++ .../robot_model/src/joint_model_group.cpp | 2 + .../include/moveit/robot_state/robot_state.h | 43 +++++++++++++++++++ moveit_core/robot_state/src/robot_state.cpp | 24 +++++++++++ 4 files changed, 79 insertions(+) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index 7626306b6d..256370877b 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -411,6 +411,13 @@ class JointModelGroup return variable_count_; } + /** \brief Get the number of variables that describe the active joints in this joint group. This excludes variables + necessary for mimic joints. */ + unsigned int getActiveVariableCount() const + { + return active_variable_count_; + } + /** \brief Set the names of the subgroups for this group */ void setSubgroupNames(const std::vector& subgroups); @@ -694,6 +701,9 @@ class JointModelGroup /** \brief The number of variables necessary to describe this group of joints */ unsigned int variable_count_; + /** \brief The number of variables necessary to describe the active joints in this group of joints */ + unsigned int active_variable_count_; + /** \brief True if the state of this group is contiguous within the full robot state; this also means that the index values in variable_index_list_ are consecutive integers */ bool is_contiguous_index_list_; diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index b9c6685a9c..6ee0d03e80 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -105,6 +105,7 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode , name_(group_name) , common_root_(nullptr) , variable_count_(0) + , active_variable_count_(0) , is_contiguous_index_list_(true) , is_chain_(false) , is_single_dof_(true) @@ -133,6 +134,7 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode active_joint_model_name_vector_.push_back(joint_model->getName()); active_joint_model_start_index_.push_back(variable_count_); active_joint_models_bounds_.push_back(&joint_model->getVariableBounds()); + active_variable_count_ += vc; } else mimic_joints_.push_back(joint_model); diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index db7e2470e9..9f05a95602 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -615,13 +615,17 @@ class RobotState { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); if (jmg) + { + assert(gstate.size() == jmg->getVariableCount()); setJointGroupPositions(jmg, &gstate[0]); + } } /** \brief Given positions for the variables that make up a group, in the order found in the group (including values * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const JointModelGroup* group, const std::vector& gstate) { + assert(gstate.size() == group->getVariableCount()); setJointGroupPositions(group, &gstate[0]); } @@ -635,13 +639,52 @@ class RobotState { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); if (jmg) + { + assert(values.size() == jmg->getVariableCount()); setJointGroupPositions(jmg, values); + } } /** \brief Given positions for the variables that make up a group, in the order found in the group (including values * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values); + /** \brief Given positions for the variables of active joints that make up a group, + * in the order found in the group (excluding values of mimic joints), set those + * as the new values that correspond to the group */ + void setJointGroupActivePositions(const JointModelGroup* group, const std::vector& gstate); + + /** \brief Given positions for the variables of active joints that make up a group, + * in the order found in the group (excluding values of mimic joints), set those + * as the new values that correspond to the group */ + void setJointGroupActivePositions(const std::string& joint_group_name, const std::vector& gstate) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + { + assert(gstate.size() == jmg->getActiveVariableCount()); + setJointGroupActivePositions(jmg, gstate); + } + } + + /** \brief Given positions for the variables of active joints that make up a group, + * in the order found in the group (excluding values of mimic joints), set those + * as the new values that correspond to the group */ + void setJointGroupActivePositions(const JointModelGroup* group, const Eigen::VectorXd& values); + + /** \brief Given positions for the variables of active joints that make up a group, + * in the order found in the group (excluding values of mimic joints), set those + * as the new values that correspond to the group */ + void setJointGroupActivePositions(const std::string& joint_group_name, const Eigen::VectorXd& values) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + { + assert(values.size() == jmg->getActiveVariableCount()); + setJointGroupActivePositions(jmg, values); + } + } + /** \brief For a given group, copy the position values of the variables that make up the group into another location, * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the * RobotState itself, so we copy instead of returning a pointer.*/ diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 010820cdbc..f147b2eb6f 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -511,6 +511,30 @@ void RobotState::setJointGroupPositions(const JointModelGroup* group, const Eige updateMimicJoints(group); } +void RobotState::setJointGroupActivePositions(const JointModelGroup* group, const std::vector& gstate) +{ + assert(gstate.size() == group->getActiveVariableCount()); + std::size_t i = 0; + for (const JointModel* jm : group->getActiveJointModels()) + { + setJointPositions(jm, &gstate[i]); + i += jm->getVariableCount(); + } + updateMimicJoints(group); +} + +void RobotState::setJointGroupActivePositions(const JointModelGroup* group, const Eigen::VectorXd& values) +{ + assert(values.size() == group->getActiveVariableCount()); + std::size_t i = 0; + for (const JointModel* jm : group->getActiveJointModels()) + { + setJointPositions(jm, &values(i)); + i += jm->getVariableCount(); + } + updateMimicJoints(group); +} + void RobotState::copyJointGroupPositions(const JointModelGroup* group, double* gstate) const { const std::vector& il = group->getVariableIndexList(); From ac55f9e252fb24321c4066d71bca4f447b55d961 Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Wed, 6 Jan 2021 19:14:07 +1000 Subject: [PATCH 533/612] Python3 compatibility for ikfast's round_collada_numbers.py (#2473) --- .../scripts/round_collada_numbers.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py index 2e587ae49a..d299c51e5c 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py @@ -39,6 +39,8 @@ # Author: Dave Coleman # Desc: Rounds all the numbers to places +from __future__ import print_function + from lxml import etree import shlex import sys @@ -69,12 +71,12 @@ def doRound(values,decimal_places): decimal_places = int(sys.argv[3]) assert( len(sys.argv) < 5 ) # invalid num-arguments except: - print '\nUsage: round_collada_numbers.py ' - print 'Rounds all the numbers to places\n' + print('\nUsage: round_collada_numbers.py ') + print('Rounds all the numbers to places\n') sys.exit(-1) - print '\nCollada Number Rounder' - print 'Rounding numbers to', decimal_places, ' decimal places\n' + print('\nCollada Number Rounder') + print('Rounding numbers to', decimal_places, 'decimal places\n') # Read string from file f = open(input_file,'r') From 475e5810911961ae51cdb5cab0961471f5d4aa1a Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Thu, 7 Jan 2021 13:00:50 -0600 Subject: [PATCH 534/612] [Servo] Fix initial angle error is always 0 (#2464) Co-authored-by: AndyZe Co-authored-by: Bjar Ne --- .../include/moveit_servo/pose_tracking.h | 3 ++- moveit_ros/moveit_servo/src/pose_tracking.cpp | 12 ++++++++---- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h index ad391b158b..5163cc08c8 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h @@ -39,6 +39,7 @@ #pragma once #include +#include #include #include #include @@ -187,7 +188,7 @@ class PoseTracking // Flag that a different thread has requested a stop. std::atomic stop_requested_; - double angular_error_; + boost::optional angular_error_; }; // using alias diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp index 0c134db35c..723fe2d8f7 100644 --- a/moveit_ros/moveit_servo/src/pose_tracking.cpp +++ b/moveit_ros/moveit_servo/src/pose_tracking.cpp @@ -50,7 +50,7 @@ PoseTracking::PoseTracking(const ros::NodeHandle& nh, , loop_rate_(DEFAULT_LOOP_RATE) , transform_listener_(transform_buffer_) , stop_requested_(false) - , angular_error_(0) + , angular_error_(boost::none) { readROSParams(); @@ -226,8 +226,12 @@ bool PoseTracking::satisfiesPoseTolerance(const Eigen::Vector3d& positional_tole double y_error = target_pose_.pose.position.y - command_frame_transform_.translation()(1); double z_error = target_pose_.pose.position.z - command_frame_transform_.translation()(2); + // If uninitialized, likely haven't received the target pose yet. + if (!angular_error_) + return false; + return ((std::abs(x_error) < positional_tolerance(0)) && (std::abs(y_error) < positional_tolerance(1)) && - (std::abs(z_error) < positional_tolerance(2)) && (std::abs(angular_error_) < angular_tolerance)); + (std::abs(z_error) < positional_tolerance(2)) && (std::abs(*angular_error_) < angular_tolerance)); } void PoseTracking::targetPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg) @@ -293,7 +297,7 @@ geometry_msgs::TwistStampedConstPtr PoseTracking::calculateTwistCommand() // Cache the angular error, for rotation tolerance checking angular_error_ = axis_angle.angle(); double ang_vel_magnitude = - cartesian_orientation_pids_[0].computeCommand(angular_error_, loop_rate_.expectedCycleTime()); + cartesian_orientation_pids_[0].computeCommand(*angular_error_, loop_rate_.expectedCycleTime()); twist.angular.x = ang_vel_magnitude * axis_angle.axis()[0]; twist.angular.y = ang_vel_magnitude * axis_angle.axis()[1]; twist.angular.z = ang_vel_magnitude * axis_angle.axis()[2]; @@ -306,7 +310,7 @@ geometry_msgs::TwistStampedConstPtr PoseTracking::calculateTwistCommand() void PoseTracking::doPostMotionReset() { stop_requested_ = false; - angular_error_ = 0; + angular_error_ = boost::none; // Reset error integrals and previous errors of PID controllers cartesian_position_pids_[0].reset(); From cc5d5a4e21c513e1527e306c2adaa8386a4bcef8 Mon Sep 17 00:00:00 2001 From: sevangelatos Date: Mon, 11 Jan 2021 18:00:19 +0200 Subject: [PATCH 535/612] Fix RobotState::dropAccelerations/dropEffort to not drop velocities (#2478) --- moveit_core/robot_state/src/robot_state.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index f147b2eb6f..909017f573 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -257,12 +257,12 @@ void RobotState::dropVelocities() void RobotState::dropAccelerations() { - has_velocity_ = false; + has_acceleration_ = false; } void RobotState::dropEffort() { - has_velocity_ = false; + has_effort_ = false; } void RobotState::dropDynamics() From 50e19608d8d27a86f1e69fd2cd8c8b7abc330fb0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Tue, 12 Jan 2021 13:21:56 +0100 Subject: [PATCH 536/612] pilz planner: add string includes (#2483) --- .../pilz_industrial_motion_planner/joint_limits_container.h | 1 + .../pilz_industrial_motion_planner/joint_limits_extension.h | 1 + 2 files changed, 2 insertions(+) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h index fac49e9106..7a91ffe545 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h @@ -38,6 +38,7 @@ #include #include +#include namespace pilz_industrial_motion_planner { diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h index 84bb1fc79f..789cafa88e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h @@ -37,6 +37,7 @@ #include #include +#include namespace pilz_industrial_motion_planner { From 680db65c14b3f0ac84beccd55bfc548719b8af84 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 12 Jan 2021 14:38:41 -0600 Subject: [PATCH 537/612] Clean up collision-related log statements (#2480) --- .../src/collision_env_bullet.cpp | 5 +++-- .../src/collision_env_fcl.cpp | 13 ++++++------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp index a9f2b57757..0be13bcfbc 100644 --- a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp @@ -45,6 +45,7 @@ namespace collision_detection { const std::string CollisionDetectorAllocatorBullet::NAME("Bullet"); const double MAX_DISTANCE_MARGIN = 99; +constexpr char LOGNAME[] = "collision_detection.bullet"; CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, double padding, double scale) : CollisionEnv(model, padding, scale) @@ -227,13 +228,13 @@ void CollisionEnvBullet::checkRobotCollisionHelperCCD(const CollisionRequest& re void CollisionEnvBullet::distanceSelf(const DistanceRequest& req, DistanceResult& res, const moveit::core::RobotState& state) const { - ROS_INFO_NAMED("collision_detection.bullet", "Distance to self not implemented yet."); + ROS_INFO_NAMED(LOGNAME, "distanceSelf is not implemented for Bullet."); } void CollisionEnvBullet::distanceRobot(const DistanceRequest& req, DistanceResult& res, const moveit::core::RobotState& state) const { - ROS_INFO_NAMED("collision_detection.bullet", "Distance to self not implemented yet."); + ROS_INFO_NAMED(LOGNAME, "distanceRobot is not implemented for Bullet."); } void CollisionEnvBullet::addToManager(const World::Object* obj) diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index f44f4b7d56..e99c954100 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -47,6 +47,7 @@ namespace collision_detection { const std::string CollisionDetectorAllocatorFCL::NAME("FCL"); +constexpr char LOGNAME[] = "collision_detection.fcl"; CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, double padding, double scale) : CollisionEnv(model, padding, scale) @@ -74,8 +75,7 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(link_geometry->collision_geometry_)); } else - ROS_ERROR_NAMED("collision_detection.fcl", "Unable to construct collision geometry for link '%s'", - link->getName().c_str()); + ROS_ERROR_NAMED(LOGNAME, "Unable to construct collision geometry for link '%s'", link->getName().c_str()); } auto m = new fcl::DynamicAABBTreeCollisionManagerd(); @@ -112,8 +112,7 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, robot_fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_)); } else - ROS_ERROR_NAMED("collision_detection.fcl", "Unable to construct collision geometry for link '%s'", - link->getName().c_str()); + ROS_ERROR_NAMED(LOGNAME, "Unable to construct collision geometry for link '%s'", link->getName().c_str()); } auto m = new fcl::DynamicAABBTreeCollisionManagerd(); @@ -273,7 +272,7 @@ void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, Collision const moveit::core::RobotState& state1, const moveit::core::RobotState& state2) const { - ROS_ERROR_NAMED("collision_detection.bullet", "Continuous collision not implemented"); + ROS_ERROR_NAMED(LOGNAME, "Continuous collision not implemented"); } void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, @@ -281,7 +280,7 @@ void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, Collision const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const { - ROS_ERROR_NAMED("collision_detection.fcl", "Not implemented"); + ROS_ERROR_NAMED(LOGNAME, "Not implemented"); } void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, @@ -429,7 +428,7 @@ void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector& li } } else - ROS_ERROR_NAMED("collision_detection.fcl", "Updating padding or scaling for unknown link: '%s'", link.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Updating padding or scaling for unknown link: '%s'", link.c_str()); } } From 0983cf9ab0a289af975971cc7374bd9182938b88 Mon Sep 17 00:00:00 2001 From: Shota Aoki Date: Wed, 13 Jan 2021 20:24:17 +0900 Subject: [PATCH 538/612] Fix scaling factor parameter names (#2452) --- .../move_group_interface/src/move_group_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index e524e17e2e..17c4523c25 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -126,9 +126,9 @@ class MoveGroupInterface::MoveGroupInterfaceImpl goal_orientation_tolerance_ = 1e-3; // ~0.1 deg allowed_planning_time_ = 5.0; num_planning_attempts_ = 1; - node_handle_.param("robot_description_planning/joint_limits/default_velocity_scaling_factor", + node_handle_.param("robot_description_planning/default_velocity_scaling_factor", max_velocity_scaling_factor_, 0.1); - node_handle_.param("robot_description_planning/joint_limits/default_acceleration_scaling_factor", + node_handle_.param("robot_description_planning/default_acceleration_scaling_factor", max_acceleration_scaling_factor_, 0.1); initializing_constraints_ = false; From e573a36c6009c200c70d805d546865d0d1974bc9 Mon Sep 17 00:00:00 2001 From: Boston Cleek <36201047+bostoncleek@users.noreply.github.com> Date: Wed, 13 Jan 2021 08:10:19 -0700 Subject: [PATCH 539/612] Improve robustness of move group interface test (#2484) --- .../test/move_group_interface_cpp_test.cpp | 216 +++++++----------- 1 file changed, 88 insertions(+), 128 deletions(-) diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp index ceae107b73..3789ed631e 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -33,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Tyler Weaver */ +/* Author: Tyler Weaver, Boston Cleek */ /* These integration tests are based on the tutorials for using move_group: * https://ros-planning.github.io/moveit_tutorials/doc/move_group_interface/move_group_interface_tutorial.html @@ -150,16 +150,27 @@ class MoveGroupTestFixture : public ::testing::Test moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; }; -TEST_F(MoveGroupTestFixture, MoveToPoseTest) +TEST_F(MoveGroupTestFixture, PathConstraintCollisionTest) { - SCOPED_TRACE("MoveToPoseTest"); + SCOPED_TRACE("PathConstraintCollisionTest"); + //////////////////////////////////////////////////////////////////// + // set a custom start state + // this simplifies planning for the orientation constraint bellow + geometry_msgs::Pose start_pose; + start_pose.orientation.w = 1.0; + start_pose.position.x = 0.3; + start_pose.position.y = 0.0; + start_pose.position.z = 0.6; + planAndMoveToPose(start_pose); + + //////////////////////////////////////////////////////////////////// // Test setting target pose with eigen and with geometry_msgs geometry_msgs::Pose target_pose; target_pose.orientation.w = 1.0; - target_pose.position.x = 0.28; - target_pose.position.y = -0.2; - target_pose.position.z = 0.5; + target_pose.position.x = 0.3; + target_pose.position.y = -0.3; + target_pose.position.z = 0.6; // convert to eigen Eigen::Isometry3d eigen_target_pose; @@ -174,46 +185,7 @@ TEST_F(MoveGroupTestFixture, MoveToPoseTest) // expect that they are identical testEigenPose(eigen_target_pose, eigen_set_target_pose); - // plan and move - planAndMove(); - - // get the pose after the movement - testPose(eigen_target_pose); -} - -TEST_F(MoveGroupTestFixture, JointSpaceGoalTest) -{ - SCOPED_TRACE("JointSpaceGoalTest"); - - // Next get the current set of joint values for the group. - std::vector plan_joint_positions; - move_group_->getCurrentState()->copyJointGroupPositions( - move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP), plan_joint_positions); - - // Now, let's modify the joint positions. (radians) - ASSERT_EQ(plan_joint_positions.size(), std::size_t(7)); - plan_joint_positions = { 1.2, -1.0, -0.1, -2.4, 0.0, 1.5, 0.6 }; - move_group_->setJointValueTarget(plan_joint_positions); - - // plan and move - planAndMove(); - - // test that we moved to the expected joint positions - testJointPositions(plan_joint_positions); -} - -TEST_F(MoveGroupTestFixture, PathConstraintTest) -{ - SCOPED_TRACE("PathConstraintTest"); - - // set a custom start state - geometry_msgs::Pose start_pose; - start_pose.orientation.w = 1.0; - start_pose.position.x = 0.55; - start_pose.position.y = -0.05; - start_pose.position.z = 0.8; - planAndMoveToPose(start_pose); - + //////////////////////////////////////////////////////////////////// // create an orientation constraint moveit_msgs::OrientationConstraint ocm; ocm.link_name = move_group_->getEndEffectorLink(); @@ -227,72 +199,7 @@ TEST_F(MoveGroupTestFixture, PathConstraintTest) test_constraints.orientation_constraints.push_back(ocm); move_group_->setPathConstraints(test_constraints); - // move to a custom target pose - geometry_msgs::Pose target_pose; - target_pose.orientation.w = 1.0; - target_pose.position.x = 0.28; - target_pose.position.y = -0.2; - target_pose.position.z = 0.5; - planAndMoveToPose(target_pose); - - // clear path constraints - move_group_->clearPathConstraints(); - - // get the pose after the movement - testPose(target_pose); -} - -TEST_F(MoveGroupTestFixture, CartPathTest) -{ - SCOPED_TRACE("CartPathTest"); - - // set a custom start state - geometry_msgs::Pose start_pose; - start_pose.orientation.w = 1.0; - start_pose.position.x = 0.55; - start_pose.position.y = -0.05; - start_pose.position.z = 0.8; - planAndMoveToPose(start_pose); - - std::vector waypoints; - waypoints.push_back(start_pose); - - geometry_msgs::Pose target_waypoint = start_pose; - target_waypoint.position.z -= 0.2; - waypoints.push_back(target_waypoint); // down - - target_waypoint.position.y -= 0.2; - waypoints.push_back(target_waypoint); // right - - target_waypoint.position.z += 0.2; - target_waypoint.position.y += 0.2; - target_waypoint.position.x -= 0.2; - waypoints.push_back(target_waypoint); // up and left - - moveit_msgs::RobotTrajectory trajectory; - const double jump_threshold = 0.0; - const double eef_step = 0.01; - move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); - - // Execute trajectory - EXPECT_EQ(move_group_->execute(trajectory), moveit::planning_interface::MoveItErrorCode::SUCCESS); - - // get the pose after the movement - testPose(target_waypoint); -} - -TEST_F(MoveGroupTestFixture, CollisionObjectsTest) -{ - SCOPED_TRACE("CollisionObjectsTest"); - - // set a custom start state - geometry_msgs::Pose start_pose; - start_pose.orientation.w = 1.0; - start_pose.position.x = 0.28; - start_pose.position.y = -0.2; - start_pose.position.z = 0.5; - planAndMoveToPose(start_pose); - + //////////////////////////////////////////////////////////////////// // Define a collision object ROS message. moveit_msgs::CollisionObject collision_object; collision_object.header.frame_id = move_group_->getPlanningFrame(); @@ -304,16 +211,16 @@ TEST_F(MoveGroupTestFixture, CollisionObjectsTest) shape_msgs::SolidPrimitive primitive; primitive.type = primitive.BOX; primitive.dimensions.resize(3); - primitive.dimensions[0] = 0.4; - primitive.dimensions[1] = 0.1; - primitive.dimensions[2] = 0.1; + primitive.dimensions[0] = 0.1; + primitive.dimensions[1] = 1.0; + primitive.dimensions[2] = 1.0; // Define a pose for the box (specified relative to frame_id) geometry_msgs::Pose box_pose; box_pose.orientation.w = 1.0; - box_pose.position.x = 0.4; - box_pose.position.y = -0.2; - box_pose.position.z = 0.8; + box_pose.position.x = 0.5; + box_pose.position.y = 0.0; + box_pose.position.z = 0.5; collision_object.primitives.push_back(primitive); collision_object.primitive_poses.push_back(box_pose); @@ -325,16 +232,15 @@ TEST_F(MoveGroupTestFixture, CollisionObjectsTest) // Now, let's add the collision object into the world planning_scene_interface_.addCollisionObjects(collision_objects); - // plan trajectory avoiding object - geometry_msgs::Pose target_pose; - target_pose.orientation.w = 0.0; - target_pose.position.x = 0.4; - target_pose.position.y = -0.4; - target_pose.position.z = 0.7; - planAndMoveToPose(target_pose); + //////////////////////////////////////////////////////////////////// + // plan and move + planAndMove(); // get the pose after the movement - testPose(target_pose); + testPose(eigen_target_pose); + + // clear path constraints + move_group_->clearPathConstraints(); // attach and detach collision object EXPECT_TRUE(move_group_->attachObject(collision_object.id)); @@ -343,13 +249,67 @@ TEST_F(MoveGroupTestFixture, CollisionObjectsTest) EXPECT_EQ(planning_scene_interface_.getAttachedObjects().size(), std::size_t(0)); // remove object from world - std::vector object_ids; - object_ids.push_back(collision_object.id); + const std::vector object_ids = { collision_object.id }; EXPECT_EQ(planning_scene_interface_.getObjects().size(), std::size_t(1)); planning_scene_interface_.removeCollisionObjects(object_ids); EXPECT_EQ(planning_scene_interface_.getObjects().size(), std::size_t(0)); } +TEST_F(MoveGroupTestFixture, CartPathTest) +{ + SCOPED_TRACE("CartPathTest"); + + // Plan from current pose + const geometry_msgs::PoseStamped start_pose = move_group_->getCurrentPose(); + + std::vector waypoints; + waypoints.push_back(start_pose.pose); + + geometry_msgs::Pose target_waypoint = start_pose.pose; + target_waypoint.position.z -= 0.2; + waypoints.push_back(target_waypoint); // down + + target_waypoint.position.y -= 0.2; + waypoints.push_back(target_waypoint); // right + + target_waypoint.position.z += 0.2; + target_waypoint.position.y += 0.2; + target_waypoint.position.x -= 0.2; + waypoints.push_back(target_waypoint); // up and left + + moveit_msgs::RobotTrajectory trajectory; + const auto jump_threshold = 0.0; + const auto eef_step = 0.01; + move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); + + // Execute trajectory + EXPECT_EQ(move_group_->execute(trajectory), moveit::planning_interface::MoveItErrorCode::SUCCESS); + + // get the pose after the movement + testPose(target_waypoint); +} + +TEST_F(MoveGroupTestFixture, JointSpaceGoalTest) +{ + SCOPED_TRACE("JointSpaceGoalTest"); + + // Next get the current set of joint values for the group. + std::vector plan_joint_positions; + move_group_->getCurrentState()->copyJointGroupPositions( + move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP), plan_joint_positions); + + // Now, let's modify the joint positions. (radians) + ASSERT_EQ(plan_joint_positions.size(), std::size_t(7)); + plan_joint_positions = { 1.2, -1.0, -0.1, -2.4, 0.0, 1.5, 0.6 }; + move_group_->setJointValueTarget(plan_joint_positions); + + // plan and move + planAndMove(); + + // test that we moved to the expected joint positions + testJointPositions(plan_joint_positions); +} + int main(int argc, char** argv) { ros::init(argc, argv, "move_group_interface_cpp_test"); From a9c8f939382193bc250788c62dd57c6627b25a2b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 14 Jan 2021 11:45:14 +0100 Subject: [PATCH 540/612] Suppress warnings "mesh_use_embedded_materials is ignored" rviz more stricly checks marker parameters and thus issues this warning if mesh_use_embedded_materials was set although no mesh_resource was specified --- moveit_ros/robot_interaction/src/robot_interaction.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index f2ebf2a4a1..5f7d8f9c6a 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -413,7 +413,7 @@ void RobotInteraction::addEndEffectorMarkers(const InteractionHandlerPtr& handle for (visualization_msgs::Marker& marker : marker_array.markers) { marker.header = im.header; - marker.mesh_use_embedded_materials = true; + marker.mesh_use_embedded_materials = !marker.mesh_resource.empty(); // - - - - - - Do some math for the offset - - - - - - tf2::Transform tf_root_to_im, tf_root_to_mesh, tf_im_to_eef; tf2::fromMsg(im.pose, tf_root_to_im); From 664ae01803abf5e0b4649063102357262de9e05c Mon Sep 17 00:00:00 2001 From: Boston Cleek Date: Thu, 14 Jan 2021 23:20:58 -0700 Subject: [PATCH 541/612] Improve robustness of subframes test (#2488) --- moveit_ros/planning_interface/test/subframes_test.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/moveit_ros/planning_interface/test/subframes_test.cpp b/moveit_ros/planning_interface/test/subframes_test.cpp index f38be9ea73..d18d8b4939 100644 --- a/moveit_ros/planning_interface/test/subframes_test.cpp +++ b/moveit_ros/planning_interface/test/subframes_test.cpp @@ -33,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Felix von Drigalski, Jacob Aas, Tyler Weaver */ +/* Author: Felix von Drigalski, Jacob Aas, Tyler Weaver, Boston Cleek */ /* This integration test is based on the tutorial for using subframes * https://ros-planning.github.io/moveit_tutorials/doc/subframes/subframes_tutorial.html @@ -59,7 +59,8 @@ #include constexpr double EPSILON = 1e-2; -constexpr double Z_OFFSET = 0.01; +constexpr double Z_OFFSET = 0.05; +constexpr double PLANNING_TIME_S = 30.0; // Function copied from tutorial // a small helper function to create our planning requests and move the robot. @@ -73,7 +74,9 @@ bool moveToCartPose(const geometry_msgs::PoseStamped& pose, moveit::planning_int moveit::planning_interface::MoveGroupInterface::Plan myplan; if (group.plan(myplan) && group.execute(myplan)) + { return true; + } ROS_WARN("Failed to perform motion."); return false; @@ -150,7 +153,7 @@ TEST(TestPlanUsingSubframes, SubframesTests) auto planning_scene_monitor = std::make_shared("robot_description"); moveit::planning_interface::PlanningSceneInterface planning_scene_interface; moveit::planning_interface::MoveGroupInterface group("panda_arm"); - group.setPlanningTime(10.0); + group.setPlanningTime(PLANNING_TIME_S); spawnCollisionObjects(planning_scene_interface); moveit_msgs::AttachedCollisionObject att_coll_object; From 960745c8d7aaf8cec9703b810576d22cd321c54d Mon Sep 17 00:00:00 2001 From: Immanuel Martini <35950815+martiniil@users.noreply.github.com> Date: Wed, 20 Jan 2021 19:57:56 +0100 Subject: [PATCH 542/612] Use kinematics solver timeout if not specified otherwise (#2489) --- .../pilz_industrial_motion_planner/trajectory_functions.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h index f31bbada01..6a23e3f41e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h @@ -61,18 +61,18 @@ namespace pilz_industrial_motion_planner * @param solution: solution of IK * @param check_self_collision: true to enable self collision checking after IK * computation - * @param max_attempt: maximal attempts of IK + * @param timeout: timeout for IK, if not set the default solver timeout is used * @return true if succeed */ bool computePoseIK(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, const std::string& link_name, const Eigen::Isometry3d& pose, const std::string& frame_id, const std::map& seed, std::map& solution, - bool check_self_collision = true, const double timeout = 0.1); + bool check_self_collision = true, const double timeout = 0.0); bool computePoseIK(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, const std::string& link_name, const geometry_msgs::Pose& pose, const std::string& frame_id, const std::map& seed, std::map& solution, - bool check_self_collision = true, const double timeout = 0.1); + bool check_self_collision = true, const double timeout = 0.0); /** * @brief compute the pose of a link at give robot state From e32288400e80a1999474be4f076febb6c289046e Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 22 Jan 2021 08:20:58 -0600 Subject: [PATCH 543/612] Do not break out of loop -- need to update low pass filters (#2496) --- moveit_ros/moveit_servo/src/servo_calcs.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index e94800d9d7..5190b3889d 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -231,14 +231,10 @@ void ServoCalcs::mainCalcLoop() // lock the input state mutex std::unique_lock input_lock(input_mutex_); - // low latency mode + // low latency mode -- begin calculations as soon as a new command is received. if (parameters_.low_latency_mode) { input_cv_.wait(input_lock, [this] { return (new_input_cmd_ || stop_requested_); }); - - // break out of the loop if stop was requested - if (stop_requested_) - break; } // reset new_input_cmd_ flag From 0e5911316ed51a765fc6b08a35f48c35f275e026 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 22 Jan 2021 08:21:24 -0600 Subject: [PATCH 544/612] Protect paused_ flag, for thread safety (#2494) --- moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h | 2 +- moveit_ros/moveit_servo/src/servo_calcs.cpp | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 055335e3f6..5303b8a296 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -279,7 +279,7 @@ class ServoCalcs // Status StatusCode status_ = StatusCode::NO_WARNING; - bool paused_ = false; + std::atomic paused_; bool twist_command_is_stale_ = false; bool joint_command_is_stale_ = false; bool ok_to_publish_ = false; diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 5190b3889d..2857ef13bf 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -86,7 +86,11 @@ geometry_msgs::TransformStamped convertIsometryToTransform(const Eigen::Isometry // Constructor for the class that handles servoing calculations ServoCalcs::ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) - : nh_(nh), parameters_(parameters), planning_scene_monitor_(planning_scene_monitor), stop_requested_(true) + : nh_(nh) + , parameters_(parameters) + , planning_scene_monitor_(planning_scene_monitor) + , stop_requested_(true) + , paused_(false) { // MoveIt Setup current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); From 74b3e30db2e8683ac17b339cc124675ae52a5114 Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Wed, 27 Jan 2021 19:50:34 +0100 Subject: [PATCH 545/612] Add missing dependency on joint_limits_interface (#2487) --- moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt | 1 + moveit_planners/pilz_industrial_motion_planner/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt index 05ed96c791..5f9be95485 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -9,6 +9,7 @@ add_definitions(-Wno-unused-parameter) find_package(catkin REQUIRED COMPONENTS eigen_conversions + joint_limits_interface kdl_conversions moveit_core moveit_msgs diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index 41f8eb032d..5c4635686f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -19,6 +19,7 @@ catkin eigen_conversions + joint_limits_interface kdl_conversions moveit_ros_planning_interface moveit_msgs From 8878682c2a71faf822db638a8ce21cedc512e4d5 Mon Sep 17 00:00:00 2001 From: Jere Liukkonen Date: Thu, 4 Feb 2021 09:35:40 -0700 Subject: [PATCH 546/612] Refactor Servo velocity bounds enforcement (#2471) * Refactor/fix Servo enforceVelLimits function to utilize joint velocity limits correctly. * Use uniform scaling factor instead of joint specific velocity bounds. * Move joint increment outside of if() * Add friend test for velocity limits * Clean up debug code, clang format * Test for negative velocities, too * Apply velocity scaling correctly and skip clamping zero velocities. * Improve enforceVelLimits unit tests. * Use EXPECT_NEAR in the new unit test Co-authored-by: AndyZe --- moveit_ros/moveit_servo/CMakeLists.txt | 20 ++--- .../moveit_servo/include/moveit_servo/servo.h | 8 ++ .../include/moveit_servo/servo_calcs.h | 3 + moveit_ros/moveit_servo/src/servo_calcs.cpp | 56 +++---------- ...latency_test.cpp => basic_servo_tests.cpp} | 78 ++++++++++++++++++- ...tency_test.test => basic_servo_tests.test} | 2 +- 6 files changed, 108 insertions(+), 59 deletions(-) rename moveit_ros/moveit_servo/test/{servo_low_latency_test.cpp => basic_servo_tests.cpp} (72%) rename moveit_ros/moveit_servo/test/{servo_low_latency_test.test => basic_servo_tests.test} (86%) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index b167019ec2..96791050e7 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -165,22 +165,22 @@ install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - # servo_cpp_interface - add_rostest_gtest(servo_cpp_interface_test - test/servo_cpp_interface_test.test - test/servo_cpp_interface_test.cpp + # basic functionality + add_rostest_gtest(basic_servo_tests + test/basic_servo_tests.test + test/basic_servo_tests.cpp ) - target_link_libraries(servo_cpp_interface_test + target_link_libraries(basic_servo_tests ${SERVO_LIB_NAME} ${catkin_LIBRARIES} ) - # low_latency - add_rostest_gtest(servo_low_latency_test - test/servo_low_latency_test.test - test/servo_low_latency_test.cpp + # servo_cpp_interface + add_rostest_gtest(servo_cpp_interface_test + test/servo_cpp_interface_test.test + test/servo_cpp_interface_test.cpp ) - target_link_libraries(servo_low_latency_test + target_link_libraries(servo_cpp_interface_test ${SERVO_LIB_NAME} ${catkin_LIBRARIES} ) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h index aeb61dab2b..8258c72923 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -38,12 +38,17 @@ #pragma once +// System #include +// MoveIt #include #include #include +// testing +#include + namespace moveit_servo { /** @@ -93,6 +98,9 @@ class Servo servo_calcs_->changeRobotLinkCommandFrame(new_command_frame); } + // Give test access to private/protected methods + friend class ServoFixture; + private: bool readParameters(); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 5303b8a296..437700c4d5 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -104,6 +104,9 @@ class ServoCalcs */ void changeRobotLinkCommandFrame(const std::string& new_command_frame); + // Give test access to private/protected methods + friend class ServoFixture; + private: /** \brief Run the main calculation loop */ void mainCalcLoop(); diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 2857ef13bf..6270c507eb 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -758,58 +758,26 @@ double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& co void ServoCalcs::enforceVelLimits(Eigen::ArrayXd& delta_theta) { + // Convert to joint angle velocities for checking and applying joint specific velocity limits. Eigen::ArrayXd velocity = delta_theta / parameters_.publish_period; - std::size_t joint_delta_index = 0; - // Track the smallest velocity scaling factor required, across all joints - double velocity_limit_scaling_factor = 1; - - for (auto joint : joint_model_group_->getActiveJointModels()) + std::size_t joint_delta_index{ 0 }; + double velocity_scaling_factor{ 1.0 }; + for (const moveit::core::JointModel* joint : joint_model_group_->getActiveJointModels()) { - // Some joints do not have bounds defined - const auto bounds = joint->getVariableBounds(joint->getName()); - - if (bounds.velocity_bounded_) + const auto& bounds = joint->getVariableBounds(joint->getName()); + if (bounds.velocity_bounded_ && velocity(joint_delta_index) != 0.0) { - velocity(joint_delta_index) = delta_theta(joint_delta_index) / parameters_.publish_period; - - bool clip_velocity = false; - double velocity_limit = 0.0; - if (velocity(joint_delta_index) < bounds.min_velocity_) - { - clip_velocity = true; - velocity_limit = bounds.min_velocity_; - } - else if (velocity(joint_delta_index) > bounds.max_velocity_) - { - clip_velocity = true; - velocity_limit = bounds.max_velocity_; - } - - // Apply velocity bounds - if (clip_velocity) - { - const double scaling_factor = - fabs(velocity_limit * parameters_.publish_period) / fabs(delta_theta(joint_delta_index)); - - // Store the scaling factor if it's the smallest yet - if (scaling_factor < velocity_limit_scaling_factor) - velocity_limit_scaling_factor = scaling_factor; - } + const double unbounded_velocity = velocity(joint_delta_index); + // Clamp each joint velocity to a joint specific [min_velocity, max_velocity] range. + const auto bounded_velocity = std::min(std::max(unbounded_velocity, bounds.min_velocity_), bounds.max_velocity_); + velocity_scaling_factor = std::min(velocity_scaling_factor, bounded_velocity / unbounded_velocity); } ++joint_delta_index; } - // Apply the velocity scaling to all joints - if (velocity_limit_scaling_factor < 1) - { - for (joint_delta_index = 0; joint_delta_index < joint_model_group_->getActiveJointModels().size(); - ++joint_delta_index) - { - delta_theta(joint_delta_index) = velocity_limit_scaling_factor * delta_theta(joint_delta_index); - velocity(joint_delta_index) = velocity_limit_scaling_factor * velocity(joint_delta_index); - } - } + // Convert back to joint angle increments. + delta_theta = velocity_scaling_factor * velocity * parameters_.publish_period; } bool ServoCalcs::enforcePositionLimits() diff --git a/moveit_ros/moveit_servo/test/servo_low_latency_test.cpp b/moveit_ros/moveit_servo/test/basic_servo_tests.cpp similarity index 72% rename from moveit_ros/moveit_servo/test/servo_low_latency_test.cpp rename to moveit_ros/moveit_servo/test/basic_servo_tests.cpp index 011e7bab1b..b73e59c5c6 100644 --- a/moveit_ros/moveit_servo/test/servo_low_latency_test.cpp +++ b/moveit_ros/moveit_servo/test/basic_servo_tests.cpp @@ -32,11 +32,11 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Tyler Weaver - Desc: Test for the low latency mode of servo +/* Author: Tyler Weaver, Andy Zelenak + Desc: Basic functionality tests */ -// C++ +// System #include // ROS @@ -49,7 +49,7 @@ #include #include -static const std::string LOGNAME = "servo_low_latency_test"; +static const std::string LOGNAME = "basic_servo_tests"; namespace moveit_servo { @@ -82,6 +82,10 @@ class ServoFixture : public ::testing::Test } protected: + void enforceVelLimits(Eigen::ArrayXd& delta_theta) + { + servo_->servo_calcs_->enforceVelLimits(delta_theta); + } ros::NodeHandle nh_{ "~" }; planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; moveit_servo::ServoPtr servo_; @@ -170,6 +174,72 @@ TEST_F(ServoFixture, SendJointServoTest) EXPECT_LT(received_count, num_commands + 20); servo_->setPaused(true); } + +// This a friend test of a private member function +TEST_F(ServoFixture, EnforceVelLimitsTest) +{ + auto parameters = servo_->getParameters(); + const double publish_period = parameters.publish_period; + + // Request joint angle changes that are too fast, given the control period in servo settings YAML file. + Eigen::ArrayXd delta_theta(7); + delta_theta[0] = 0; // rad + delta_theta[1] = 0.01; + delta_theta[2] = 0.02; + delta_theta[3] = 0.03; + delta_theta[4] = 0.04; + delta_theta[5] = 0.05; + delta_theta[6] = 0.06; + + // Store the original joint commands for comparison before applying velocity scaling. + Eigen::ArrayXd orig_delta_theta = delta_theta; + enforceVelLimits(delta_theta); + + // From Panda arm MoveIt joint_limits.yaml. The largest velocity limits for a joint. + const double panda_max_joint_vel = 2.610; // rad/s + const double velocity_scaling_factor = panda_max_joint_vel / (orig_delta_theta.maxCoeff() / publish_period); + const double tolerance = 5e-3; + for (int i = 0; i < 7; ++i) + { + EXPECT_NEAR(orig_delta_theta(i) * velocity_scaling_factor, delta_theta(i), tolerance); + } + + // Now, negative joint angle deltas. Some will result to velocities + // greater than the arm joint velocity limits. + delta_theta[0] = 0; // rad + delta_theta[1] = -0.01; + delta_theta[2] = -0.02; + delta_theta[3] = -0.03; + delta_theta[4] = -0.04; + delta_theta[5] = -0.05; + delta_theta[6] = -0.06; + + // Store the original joint commands for comparison before applying velocity scaling. + orig_delta_theta = delta_theta; + enforceVelLimits(delta_theta); + for (int i = 0; i < 7; ++i) + { + EXPECT_NEAR(orig_delta_theta(i) * velocity_scaling_factor, delta_theta(i), tolerance); + } + + // Final test with joint angle deltas that will result in velocities + // below the lowest Panda arm joint velocity limit. + delta_theta[0] = 0; // rad + delta_theta[1] = -0.013; + delta_theta[2] = 0.023; + delta_theta[3] = -0.004; + delta_theta[4] = 0.021; + delta_theta[5] = 0.012; + delta_theta[6] = 0.0075; + + // Store the original joint commands for comparison before applying velocity scaling. + orig_delta_theta = delta_theta; + enforceVelLimits(delta_theta); + for (int i = 0; i < 7; ++i) + { + EXPECT_NEAR(orig_delta_theta(i), delta_theta(i), tolerance); + } +} } // namespace moveit_servo int main(int argc, char** argv) diff --git a/moveit_ros/moveit_servo/test/servo_low_latency_test.test b/moveit_ros/moveit_servo/test/basic_servo_tests.test similarity index 86% rename from moveit_ros/moveit_servo/test/servo_low_latency_test.test rename to moveit_ros/moveit_servo/test/basic_servo_tests.test index 6df90be617..930d0d0e41 100644 --- a/moveit_ros/moveit_servo/test/servo_low_latency_test.test +++ b/moveit_ros/moveit_servo/test/basic_servo_tests.test @@ -11,7 +11,7 @@ - + From 6512d987b8379519146193267906ee957586c4e8 Mon Sep 17 00:00:00 2001 From: Pawan Wadhwani <40732709+pawanw17@users.noreply.github.com> Date: Fri, 5 Feb 2021 03:26:23 +0530 Subject: [PATCH 547/612] resolved typo in first-timers-only.md (#2443) --- .../{first_timers_only.md => first_timer_only.md} | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename .github/ISSUE_TEMPLATE/{first_timers_only.md => first_timer_only.md} (98%) diff --git a/.github/ISSUE_TEMPLATE/first_timers_only.md b/.github/ISSUE_TEMPLATE/first_timer_only.md similarity index 98% rename from .github/ISSUE_TEMPLATE/first_timers_only.md rename to .github/ISSUE_TEMPLATE/first_timer_only.md index c85e0638a7..57cdf54dfa 100644 --- a/.github/ISSUE_TEMPLATE/first_timers_only.md +++ b/.github/ISSUE_TEMPLATE/first_timer_only.md @@ -1,8 +1,8 @@ --- -name: First Timers Only Issue +name: First Timer Only Issue about: Create an issue to welcome a new contributor into the community. title: '' -labels: first-timers-only +labels: first-timer-only assignees: '' --- From ea5080167be92f1ed4441b560f9d94d542b06f4c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 11 Feb 2021 09:03:48 -0600 Subject: [PATCH 548/612] Fix logic, improve function comment for clearDiffs() (#2497) --- .../include/moveit/planning_scene/planning_scene.h | 6 ++++-- moveit_core/planning_scene/src/planning_scene.cpp | 10 ++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index c4f648959e..7e85755e51 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -774,8 +774,10 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from void removeObjectType(const std::string& id); void getKnownObjectTypes(ObjectTypeMap& kc) const; - /** \brief Clear the diffs accumulated for this planning scene, with respect to the parent. This function is a no-op - * if there is no parent specified. */ + /** \brief Clear the diffs accumulated for this planning scene, with respect to: + * the parent PlanningScene (if it exists) + * the parent CollisionDetector (if it exists) + * This function is a no-op if there is no parent planning scene. */ void clearDiffs(); /** \brief If there is a parent specified for this scene, then the diffs with respect to that parent are applied to a diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index a8a05ccb6f..969a596820 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -389,17 +389,15 @@ void PlanningScene::clearDiffs() if (it.second->parent_) { - it.second->cenv_ = it.second->alloc_->allocateEnv(it.second->parent_->cenv_, world_); - it.second->cenv_const_ = it.second->cenv_; - it.second->cenv_unpadded_ = it.second->alloc_->allocateEnv(it.second->parent_->cenv_unpadded_, world_); it.second->cenv_unpadded_const_ = it.second->cenv_unpadded_; + + it.second->cenv_ = it.second->alloc_->allocateEnv(it.second->parent_->cenv_, world_); + it.second->cenv_const_ = it.second->cenv_; } - else + else // This is the parent CollisionDetector { it.second->copyPadding(*parent_->active_collision_); - - it.second->cenv_ = it.second->alloc_->allocateEnv(it.second->parent_->cenv_, world_); it.second->cenv_const_ = it.second->cenv_; } } From 14cce9e124dc0d86765bce5fb1922ad42d4ceaa6 Mon Sep 17 00:00:00 2001 From: Omid Heidari Date: Tue, 11 Jun 2019 18:51:26 -0600 Subject: [PATCH 549/612] TrajOpt Planner (Empty Template) (#1478) * Created EmptyPlan, a MoveIt planning plugin template * added trajopt from example_planner * fixed trajopt_planner_mannager * modified emaptyplan_planner_manager.cpp * applied Dave's modifications * The rest of the modifications from Dave's comments * removed moveit_ros_planning dependency from package.xml * fixed clangformat error and dependency issu * added include directory * edited package.xml * added include * remove include * remove version_gte from package.xml * updated the setup assitant to work with trajopt planner * removed unsuded part in initialize() from trajopt_planner_manager.cpp, kept the initialize() itself for later use * Update moveit_planners/trajopt/src/trajopt_planner_manager.cpp Co-Authored-By: Dave Coleman * added README added description to trajopt_interface_plugin_description.xml --- moveit_planners/trajopt/CMakeLists.txt | 29 +++++++++++++++++++ .../widgets/configuration_files_widget.cpp | 21 ++++++++++++++ 2 files changed, 50 insertions(+) diff --git a/moveit_planners/trajopt/CMakeLists.txt b/moveit_planners/trajopt/CMakeLists.txt index 4d7d8f54e1..b1e7e4c3fb 100644 --- a/moveit_planners/trajopt/CMakeLists.txt +++ b/moveit_planners/trajopt/CMakeLists.txt @@ -11,6 +11,7 @@ if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() +<<<<<<< HEAD find_package(catkin REQUIRED COMPONENTS moveit_core @@ -66,10 +67,28 @@ set_target_properties( VERSION "${${PROJECT_NAME}_VERSION}" ) +======= +find_package(catkin REQUIRED COMPONENTS + moveit_core + roscpp + pluginlib +) + +catkin_package( + CATKIN_DEPENDS + moveit_core +) + +# The following include_directory should have include folder of the new planner. +include_directories(${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${TrajOpt_INCLUDE_DIRS}) +>>>>>>> TrajOpt Planner (Empty Template) (#1478) # TrajOpt planning plugin add_library(moveit_trajopt_planner_plugin src/trajopt_planner_manager.cpp) set_target_properties(moveit_trajopt_planner_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +<<<<<<< HEAD target_link_libraries(moveit_trajopt_planner_plugin ${PROJECT_NAME} ${catkin_LIBRARIES}) ############# @@ -115,3 +134,13 @@ if(CATKIN_ENABLE_TESTING) ) endif() +======= + +install(TARGETS + moveit_trajopt_planner_plugin + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(FILES trajopt_interface_plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +>>>>>>> TrajOpt Planner (Empty Template) (#1478) diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 4535e99498..3005044159 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -403,6 +403,18 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = 0; gen_files_.push_back(file); + // trajopt_planning_pipeline.launch + // -------------------------------------------------------------------------------------- + file.file_name_ = "trajopt_planning_pipeline.launch.xml"; + file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); + template_path = config_data_->appendPaths(template_launch_path, file.file_name_); + file.description_ = "Intended to be included in other launch files that require the TrajOpt planning plugin. Defines " + "the proper plugin name on the parameter server and a default selection of planning request " + "adapters."; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.write_on_changes = 0; + gen_files_.push_back(file); + // chomp_planning_pipeline.launch // -------------------------------------------------------------------------------------- file.file_name_ = "chomp_planning_pipeline.launch.xml"; @@ -460,6 +472,15 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = 0; gen_files_.push_back(file); + // run_benchmark_trajopt.launch -------------------------------------------------------------------------------------- + file.file_name_ = "run_benchmark_trajopt.launch"; + file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); + template_path = config_data_->appendPaths(template_launch_path, file.file_name_); + file.description_ = "Launch file for benchmarking TrajOpt planners"; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.write_on_changes = 0; + gen_files_.push_back(file); + // sensor_manager.launch -------------------------------------------------------------------------------------- file.file_name_ = "sensor_manager.launch.xml"; file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); From 260d4c5ccd9932b206e5483668c4ff78f8d29ad3 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Thu, 6 Jun 2019 19:31:17 -0600 Subject: [PATCH 550/612] added solve from tesseract. added launch file improved test_trajopt node edited package.xml --- moveit_planners/trajopt/config/setup.yaml | 0 .../include/trajopt_planning_context.h | 84 +++++++ .../trajopt/launch/gdb_settings.gdb | 8 + .../trajopt/launch/test_trajopt_launch.launch | 27 ++ moveit_planners/trajopt/src/test_trajopt.cpp | 230 ++++++++++++++++++ 5 files changed, 349 insertions(+) create mode 100644 moveit_planners/trajopt/config/setup.yaml create mode 100644 moveit_planners/trajopt/include/trajopt_planning_context.h create mode 100644 moveit_planners/trajopt/launch/gdb_settings.gdb create mode 100644 moveit_planners/trajopt/launch/test_trajopt_launch.launch create mode 100644 moveit_planners/trajopt/src/test_trajopt.cpp diff --git a/moveit_planners/trajopt/config/setup.yaml b/moveit_planners/trajopt/config/setup.yaml new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_planners/trajopt/include/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_planning_context.h new file mode 100644 index 0000000000..3866c51c10 --- /dev/null +++ b/moveit_planners/trajopt/include/trajopt_planning_context.h @@ -0,0 +1,84 @@ +#ifndef TrajOpt_PLANNING_CONTEXT_H +#define TrajOpt_PLANNING_CONTEXT_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include + +//using namespace trajopt; + +namespace trajopt_interface +{ + +MOVEIT_CLASS_FORWARD(TrajOptPlanningContext); + +struct TrajOptPlannerConfiguration{ + + TrajOptPlannerConfiguration() {} + virtual ~TrajOptPlannerConfiguration() {} + + /** @brief Trajopt problem to be solved (Required) */ + // sco::OptProbPtr prob = sco::OptProbPtr(new sco::OptProb(sco::ModelType::AUTO_SOLVER)); + + std::string model_type; // sco::ModelType::AUTO_SOLVER + sco::OptProbPtr prob = sco::OptProbPtr(new sco::OptProb(model_type)); + + + /** @brief Optimization parameters to be used (Optional) */ + sco::BasicTrustRegionSQPParameters params; + + /** @brief Callback functions called on each iteration of the optimization (Optional) */ + //std::vector callbacks; + sco::Optimizer::Callback callbacks; + +}; + +TrajOptPlannerConfiguration spec_; + +class TrajOptPlanningContext : public planning_interface::PlanningContext +{ +public: + TrajOptPlanningContext(const std::string& name, const std::string& group, + const robot_model::RobotModelConstPtr& model); + ~TrajOptPlanningContext() override + { + } + + bool solve(planning_interface::MotionPlanResponse& res) override; + bool solve(planning_interface::MotionPlanDetailedResponse& res) override; + + bool terminate() override; + void clear() override; + + void setTrajOptPlannerConfiguration(); + + + +protected: + +private: + // trajectory_msgs::JointTrajectory interpolateMultDOF(const std::vector& v1, const std::vector& v2, + // const int& num); + // std::vector interpolateSingleDOF(const double& d1, const double& d2, const int& num); + int dof; + + moveit::core::RobotModelConstPtr robot_model_; + robot_state::RobotStatePtr robot_state_; + + trajectory_msgs::JointTrajectory convert_TrajArray_to_JointTrajectory(const trajopt::TrajArray& traj_array); + + tesseract::tesseract_planning::PlannerRequest tess_request_; +}; +} // namespace trajopt_interface + +#endif // TrajOpt_PLANNING_CONTEXT_H diff --git a/moveit_planners/trajopt/launch/gdb_settings.gdb b/moveit_planners/trajopt/launch/gdb_settings.gdb new file mode 100644 index 0000000000..d8ac14fee4 --- /dev/null +++ b/moveit_planners/trajopt/launch/gdb_settings.gdb @@ -0,0 +1,8 @@ +## This file allows developers to set breakpoints in the CPP_EXECUTABLE_NAME.launch file +## To set a breakpoint, compile with debug flag set to true. Add a line below with the source file name, +## a colon, then the line number to break on. Then launch CPP_EXECUTABLE_NAME with argument debug:=true + +set breakpoint pending on + +## Example break point: +# break CPP_EXECUTABLE_NAME.cpp:52 \ No newline at end of file diff --git a/moveit_planners/trajopt/launch/test_trajopt_launch.launch b/moveit_planners/trajopt/launch/test_trajopt_launch.launch new file mode 100644 index 0000000000..e5931e4b4c --- /dev/null +++ b/moveit_planners/trajopt/launch/test_trajopt_launch.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/trajopt/src/test_trajopt.cpp b/moveit_planners/trajopt/src/test_trajopt.cpp new file mode 100644 index 0000000000..af56adc99a --- /dev/null +++ b/moveit_planners/trajopt/src/test_trajopt.cpp @@ -0,0 +1,230 @@ +#include +#include "trajopt_planning_context.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include "moveit/planning_interface/planning_request.h" + +#include +#include + +// test + +int main(int argc, char** argv) +{ + const std::string node_name = "trajopot_planning_tutorial"; + ros::init(argc, argv, node_name); + ros::AsyncSpinner spinner(1); + spinner.start(); + ros::NodeHandle node_handle("~"); + + const std::string PLANNING_GROUP = "panda_arm"; + robot_model_loader::RobotModelLoaderPtr robot_model_loader(new robot_model_loader::RobotModelLoader("robot_description")); + robot_model::RobotModelPtr robot_model = robot_model_loader->getModel(); + /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/ + robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model)); + const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); + + const std::vector& joint_names = joint_model_group->getVariableNames(); + + planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); + + // With the planning scene we create a planing scene monitor that + // monitors planning scene diffs and applys them to the planning scene + planning_scene_monitor::PlanningSceneMonitorPtr psm( + new planning_scene_monitor::PlanningSceneMonitor(planning_scene, robot_model_loader)); + psm->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); + psm->startStateMonitor(); + psm->startSceneMonitor(); + + while (!psm->getStateMonitor()->haveCompleteState() && ros::ok()) + { + ROS_INFO_STREAM_THROTTLE_NAMED(1, node_name, "Waiting for complete state from topic "); + } + // We will now construct a loader to load a planner, by name. + // Note that we are using the ROS pluginlib library here. + boost::scoped_ptr> planner_plugin_loader; + planning_interface::PlannerManagerPtr planner_instance; + + std::string planner_plugin_name; + + planner_plugin_name = "trajopt_interface/TrajOptPlanner"; + node_handle.setParam("planning_plugin", planner_plugin_name); + + // We will get the name of planning plugin we want to load + // from the ROS parameter server, and then load the planner + // making sure to catch all exceptions. + if (!node_handle.getParam("planning_plugin", planner_plugin_name)) + ROS_FATAL_STREAM("Could not find planner plugin name"); + try + { + planner_plugin_loader.reset(new pluginlib::ClassLoader( + "moveit_core", "planning_interface::PlannerManager")); + printf("===>>> planner_plugin_name: %s \n", planner_plugin_name.c_str()); + } + catch (pluginlib::PluginlibException& ex) + { + ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); + } + try + { + planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); + if (!planner_instance->initialize(robot_model, node_handle.getNamespace())) // namespace ????. I dont use it + ROS_FATAL_STREAM("Could not initialize planner instance"); + ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'"); + } + catch (pluginlib::PluginlibException& ex) + { + const std::vector& classes = planner_plugin_loader->getDeclaredClasses(); + std::stringstream ss; + for (std::size_t i = 0; i < classes.size(); ++i) + ss << classes[i] << " "; + ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl + << "Available plugins: " << ss.str()); + } + + + + + + + + + + + + + // Visualization + // ^^^^^^^^^^^^^ + // The package MoveItVisualTools provides many capabilties for visualizing objects, robots, + // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script + namespace rvt = rviz_visual_tools; + moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, psm); + visual_tools.loadRobotStatePub("/display_robot_state"); + visual_tools.enableBatchPublishing(); + visual_tools.deleteAllMarkers(); // clear all old markers + visual_tools.trigger(); + + /* Remote control is an introspection tool that allows users to step through a high level script + via buttons and keyboard shortcuts in RViz */ + visual_tools.loadRemoteControl(); + + /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/ + Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); + text_pose.translation().z() = 1.75; + visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE); + + /* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */ + visual_tools.trigger(); + + /* We can also use visual_tools to wait for user input */ + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + + // Pose Goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // We will now create a motion plan request for the arm of the Panda + // specifying the desired pose of the end-effector as input. + visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); + visual_tools.trigger(); + planning_interface::MotionPlanRequest req; + planning_interface::MotionPlanResponse res; + geometry_msgs::PoseStamped pose; + pose.header.frame_id = "panda_link0"; + pose.pose.position.x = 0.3; + pose.pose.position.y = 0.4; + pose.pose.position.z = 0.75; + pose.pose.orientation.w = 1.0; + + // A tolerance of 0.01 m is specified in position + // and 0.01 radians in orientation + std::vector tolerance_pose(3, 0.01); + std::vector tolerance_angle(3, 0.01); + + // We will create the request as a constraint using a helper function available + // from the + // `kinematic_constraints`_ + // package. + // + // .. _kinematic_constraints: + // http://docs.ros.org/indigo/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132 + moveit_msgs::Constraints pose_goal = + kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); + + req.group_name = PLANNING_GROUP; + req.goal_constraints.push_back(pose_goal); + + // set the request start joint state + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + // get the joint values of the start state + std::vector start_joint_values; + robot_state->copyJointGroupPositions(joint_model_group, start_joint_values); + int r = 0; + for (auto x : start_joint_values) + { + printf("===>>> joint: %s with value: %f \n", joint_names[r].c_str(), x); + ++r; + } + req.start_state.joint_state.position = start_joint_values; + + + // set the goal joint state + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + robot_state::RobotState goal_state(robot_model); + std::vector joint_values = { 0.8, 0.7, 1, 1.3, 1.9, 2.2, 3 }; + goal_state.setJointGroupPositions(joint_model_group, joint_values); + moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); + req.goal_constraints.clear(); + req.goal_constraints.push_back(joint_goal); + + // retrive joint values at goal + std::vector goal_constraints = req.goal_constraints; + std::cout << "===>>> number of constraints in goal: " << goal_constraints.size() << std::endl; + std::vector goal_joint_constraint = goal_constraints[0].joint_constraints; + for (auto x : goal_joint_constraint) + { + std::cout << "==>> joint position at goal " << x.position << std::endl; + } + + + //------ + // We now construct a planning context that encapsulate the scene, + // the request and the response. We call the planner using this + // planning context + planning_interface::PlanningContextPtr context = + planner_instance->getPlanningContext(planning_scene, req, res.error_code_); + + context->solve(res); + if (res.error_code_.val != res.error_code_.SUCCESS) + { + ROS_ERROR("Could not compute plan successfully"); + return 0; + } + + + + + + + + + + + + + + + + + + + + + +} From 70c6dbd901966974b6dc4b46ed121dce3db90e98 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Tue, 2 Jul 2019 17:07:38 -0600 Subject: [PATCH 551/612] updated resp req updated planning context working trajopt without any velocity constraint --- moveit_planners/trajopt/config/setup.yaml | 13 ++ .../trajopt/include/problem_description.h | 120 ++++++++++++++++++ .../include/trajopt_planning_context.h | 34 +++-- .../trajopt/launch/test_trajopt_launch.launch | 6 + moveit_planners/trajopt/src/test_trajopt.cpp | 69 +++++----- 5 files changed, 202 insertions(+), 40 deletions(-) create mode 100644 moveit_planners/trajopt/include/problem_description.h diff --git a/moveit_planners/trajopt/config/setup.yaml b/moveit_planners/trajopt/config/setup.yaml index e69de29bb2..f44b12ef66 100644 --- a/moveit_planners/trajopt/config/setup.yaml +++ b/moveit_planners/trajopt/config/setup.yaml @@ -0,0 +1,13 @@ +improve_ratio_threshold: 0.25; +min_trust_box_size: 1e-4; +min_approx_improve: 1e-4; +min_approx_improve_frac: -1000; # should br -infinity +max_iter: 50; +trust_shrink_ratio: 0.1; +trust_expand_ratio: 1.5; +cnt_tolerance: 1e-4; +max_merit_coeff_increases: 5; +merit_coeff_increase_ratio : 10; +max_time: 1000; # should be +infinity +merit_error_coeff: 10; +trust_box_size: 1e-1; diff --git a/moveit_planners/trajopt/include/problem_description.h b/moveit_planners/trajopt/include/problem_description.h new file mode 100644 index 0000000000..73d7d1abe6 --- /dev/null +++ b/moveit_planners/trajopt/include/problem_description.h @@ -0,0 +1,120 @@ +#pragma once +#include +#include +#include +#include +#include + + +namespace trajopt_interface +{ + +class TrajOptProblem; +typedef std::shared_ptr TrajOptProblemPtr; + +/** + * Holds all the data for a trajectory optimization problem + * so you can modify it programmatically, e.g. add your own costs + */ +class TrajOptProblem : public sco::OptProb +{ +public: + TrajOptProblem(); + TrajOptProblem(const int& n_steps, bool& use_time, const robot_model::RobotModelConstPtr& robot_model); + virtual ~TrajOptProblem() = default; + sco::VarVector GetVarRow(int i, int start_col, int num_col) { return m_traj_vars.rblock(i, start_col, num_col); } + sco::VarVector GetVarRow(int i) { return m_traj_vars.row(i); } + sco::Var& GetVar(int i, int j) { return m_traj_vars.at(i, j); } + trajopt::VarArray& GetVars() { return m_traj_vars; } + /** @brief Returns the number of steps in the problem. This is the number of rows in the optimization matrix.*/ + int GetNumSteps() { return m_traj_vars.rows(); } + /** @brief Returns the problem DOF. This is the number of columns in the optization matrix. + * Note that this is not necessarily the same as the kinematic DOF.*/ + int GetNumDOF() { return m_traj_vars.cols(); } + // tesseract::BasicKinConstPtr GetKin() { return m_kin; } + //tesseract::BasicEnvConstPtr GetEnv() { return m_env; } + void SetInitTraj(const trajopt::TrajArray& x){ + std::cout << "====>>>>> setInitTraj is called " << std::endl; + std::cout << "x.size: " << x.size() << std::endl; + m_init_traj = x; + // std::cout << m_init_traj << std::endl; + } + trajopt::TrajArray GetInitTraj() { + std::cout << "====>>>>> GetInitTraj is called " << std::endl; + return m_init_traj; + } + // friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&); + /** @brief Returns TrajOptProb.has_time */ + bool GetHasTime() { return has_time; } + /** @brief Sets TrajOptProb.has_time */ + void SetHasTime(bool tmp) { has_time = tmp; } + +private: + /** @brief If true, the last column in the optimization matrix will be 1/dt */ + bool has_time; + trajopt::VarArray m_traj_vars; + // tesseract::BasicKinConstPtr m_kin; + // tesseract::BasicEnvConstPtr m_env; + trajopt::TrajArray m_init_traj; +}; + +/** +When cost or constraint element of JSON doc is read, one of these guys gets +constructed to hold the parameters. +Then it later gets converted to a Cost object by the hatch method +*/ +struct TermInfo +{ + std::string name; + int term_type; + int getSupportedTypes() { return supported_term_types_; } + // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; + virtual void hatch(TrajOptProblem& prob) = 0; + + // static TermInfoPtr fromName(const std::string& type); + + /** + * Registers a user-defined TermInfo so you can use your own cost + * see function RegisterMakers.cpp + */ + // typedef TermInfoPtr (*MakerFunc)(void); + // static void RegisterMaker(const std::string& type, MakerFunc); + + virtual ~TermInfo() = default; + +protected: + TermInfo(int supported_term_types) : supported_term_types_(supported_term_types) {} +private: + // static std::map name2maker; + int supported_term_types_; +}; + +struct CartPoseTermInfo : public TermInfo +{ + // EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** @brief Timestep at which to apply term */ + int timestep; + /** @brief Cartesian position */ + Eigen::Vector3d xyz; + /** @brief Rotation quaternion */ + Eigen::Vector4d wxyz; + /** @brief coefficients for position and rotation */ + Eigen::Vector3d pos_coeffs, rot_coeffs; + /** @brief Link which should reach desired pose */ + std::string link; + /** @brief Static transform applied to the link */ + Eigen::Isometry3d tcp; + + CartPoseTermInfo(); + + /** @brief Used to add term to pci from json */ + // void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; + /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ + void hatch(TrajOptProblem& prob) override; + // DEFINE_CREATE(CartPoseTermInfo) +}; + + + +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_planning_context.h index 3866c51c10..01e277eb37 100644 --- a/moveit_planners/trajopt/include/trajopt_planning_context.h +++ b/moveit_planners/trajopt/include/trajopt_planning_context.h @@ -15,6 +15,11 @@ #include +#include +#include + +#include "problem_description.h" + //using namespace trajopt; namespace trajopt_interface @@ -24,15 +29,20 @@ MOVEIT_CLASS_FORWARD(TrajOptPlanningContext); struct TrajOptPlannerConfiguration{ - TrajOptPlannerConfiguration() {} + // TrajOptPlannerConfiguration(trajopt::TrajOptProbPtr prob) : prob(prob) {} + + TrajOptPlannerConfiguration() {prob.reset(new TrajOptProblem());} + virtual ~TrajOptPlannerConfiguration() {} - /** @brief Trajopt problem to be solved (Required) */ + // std::string model_type = sco::ModelType::AUTO_SOLVER // sco::OptProbPtr prob = sco::OptProbPtr(new sco::OptProb(sco::ModelType::AUTO_SOLVER)); - std::string model_type; // sco::ModelType::AUTO_SOLVER - sco::OptProbPtr prob = sco::OptProbPtr(new sco::OptProb(model_type)); + /** @brief Trajopt problem to be solved (Required) */ + TrajOptProblemPtr prob; + // TrajOptProblemPtr prob(new TrajOptProblem()); + // std::shared_ptr prob = std::shared_ptr(new TrajOptProblem); /** @brief Optimization parameters to be used (Optional) */ sco::BasicTrustRegionSQPParameters params; @@ -43,8 +53,12 @@ struct TrajOptPlannerConfiguration{ }; +trajopt::TrajArray generateInitialTrajectory(const int& num_steps); TrajOptPlannerConfiguration spec_; +int dof_; +void callBackFunc(sco::OptProb* oprob, sco::OptResults& ores); + class TrajOptPlanningContext : public planning_interface::PlanningContext { public: @@ -62,20 +76,18 @@ class TrajOptPlanningContext : public planning_interface::PlanningContext void setTrajOptPlannerConfiguration(); - - protected: private: - // trajectory_msgs::JointTrajectory interpolateMultDOF(const std::vector& v1, const std::vector& v2, - // const int& num); - // std::vector interpolateSingleDOF(const double& d1, const double& d2, const int& num); - int dof; moveit::core::RobotModelConstPtr robot_model_; robot_state::RobotStatePtr robot_state_; - trajectory_msgs::JointTrajectory convert_TrajArray_to_JointTrajectory(const trajopt::TrajArray& traj_array); + + + trajectory_msgs::JointTrajectory convertTrajArrayToJointTrajectory(const trajopt::TrajArray& traj_array); + + // sco::VarArray convertVarVectorToVarArray(const VarVector& vv); tesseract::tesseract_planning::PlannerRequest tess_request_; }; diff --git a/moveit_planners/trajopt/launch/test_trajopt_launch.launch b/moveit_planners/trajopt/launch/test_trajopt_launch.launch index e5931e4b4c..f0d35bdeaa 100644 --- a/moveit_planners/trajopt/launch/test_trajopt_launch.launch +++ b/moveit_planners/trajopt/launch/test_trajopt_launch.launch @@ -17,6 +17,12 @@ + + + + + + diff --git a/moveit_planners/trajopt/src/test_trajopt.cpp b/moveit_planners/trajopt/src/test_trajopt.cpp index af56adc99a..55a26c08be 100644 --- a/moveit_planners/trajopt/src/test_trajopt.cpp +++ b/moveit_planners/trajopt/src/test_trajopt.cpp @@ -31,7 +31,9 @@ int main(int argc, char** argv) robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model)); const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); - const std::vector& joint_names = joint_model_group->getVariableNames(); + + const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); + std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> dof: " << joint_names.size() << std::endl; planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); @@ -43,9 +45,9 @@ int main(int argc, char** argv) psm->startStateMonitor(); psm->startSceneMonitor(); - while (!psm->getStateMonitor()->haveCompleteState() && ros::ok()) - { - ROS_INFO_STREAM_THROTTLE_NAMED(1, node_name, "Waiting for complete state from topic "); + while (!psm->getStateMonitor()->haveCompleteState() && ros::ok()) + { + ROS_INFO_STREAM_THROTTLE_NAMED(1, node_name, "Waiting for complete state from topic "); } // We will now construct a loader to load a planner, by name. // Note that we are using the ROS pluginlib library here. @@ -56,7 +58,7 @@ int main(int argc, char** argv) planner_plugin_name = "trajopt_interface/TrajOptPlanner"; node_handle.setParam("planning_plugin", planner_plugin_name); - + // We will get the name of planning plugin we want to load // from the ROS parameter server, and then load the planner // making sure to catch all exceptions. @@ -192,39 +194,48 @@ int main(int argc, char** argv) std::cout << "==>> joint position at goal " << x.position << std::endl; } - + //------ // We now construct a planning context that encapsulate the scene, // the request and the response. We call the planner using this // planning context - planning_interface::PlanningContextPtr context = + planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); - - context->solve(res); - if (res.error_code_.val != res.error_code_.SUCCESS) + + context->solve(res); + if (res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); return 0; } - - - - - - - - - - - - - - - - - - - + visual_tools.prompt("Press 'next' to visualize the reslt "); + +// Visualize the result + // ^^^^^^^^^^^^^^^^^^^^ + ros::Publisher display_publisher = + node_handle.advertise("/move_group/display_planned_path", 1, true); + moveit_msgs::DisplayTrajectory display_trajectory; + + /* Visualize the trajectory */ + moveit_msgs::MotionPlanResponse response; + res.getMessage(response); + + display_trajectory.trajectory_start = response.trajectory_start; + display_trajectory.trajectory.push_back(response.trajectory); + visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); + visual_tools.trigger(); + display_publisher.publish(display_trajectory); + /* + // Set the state in the planning scene to the final state of the last plan + robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); + planning_scene->setCurrentState(*robot_state.get()); + + // Display the goal state + visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); + visual_tools.publishAxisLabeled(pose.pose, "goal_1"); + visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); +*/ } From 4b84e1754228dac5f73d9f0ded7e5877237660a6 Mon Sep 17 00:00:00 2001 From: Omid Heidari Date: Sun, 14 Jul 2019 21:25:53 -0600 Subject: [PATCH 552/612] added trajopt_interface class --- .../trajopt/include/trajopt_interface.h | 93 +++++++++++++++++++ .../include/trajopt_planning_context.h | 18 ++-- 2 files changed, 104 insertions(+), 7 deletions(-) create mode 100644 moveit_planners/trajopt/include/trajopt_interface.h diff --git a/moveit_planners/trajopt/include/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface.h new file mode 100644 index 0000000000..12ce32b414 --- /dev/null +++ b/moveit_planners/trajopt/include/trajopt_interface.h @@ -0,0 +1,93 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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. + *********************************************************************/ + +/* Author: Omid Heidari */ + +#ifndef TRAJOPT_INTERFACE_TRAJOPT_INTERFACE_H +#define TRAJOPT_INTERFACE_TRAJOPT_INTERFACE_H + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include "problem_description.h" + + +namespace trajopt_interface +{ + // MOVEIT_CLASS_FORWARD(TrajOptInterface); + +class TrajOptInterface //: public trajopt::TrajOptPlanner +{ +public: + TrajOptInterface(); + virtual ~TrajOptInterface() = 0; + + TrajOptInterface(const ros::NodeHandle& nh = ros::NodeHandle("~")); + + const sco::BasicTrustRegionSQPParameters& getParams() const { return params_; } + +protected: + /** @brief Configure everything using the param server */ + void setTrajOptPlannerConfiguration(); + + ros::NodeHandle nh_; /// The ROS node handle + + sco::BasicTrustRegionSQPParameters params_; + + sco::Optimizer::Callback callbacks_; + + TrajOptProblemPtr prob_; + + void callBackFunc(sco::OptProb* oprob, sco::OptResults& ores); + +}; +} + +#endif diff --git a/moveit_planners/trajopt/include/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_planning_context.h index 01e277eb37..debb20a29d 100644 --- a/moveit_planners/trajopt/include/trajopt_planning_context.h +++ b/moveit_planners/trajopt/include/trajopt_planning_context.h @@ -20,13 +20,15 @@ #include "problem_description.h" +#include "trajopt_interface.h" + //using namespace trajopt; namespace trajopt_interface { MOVEIT_CLASS_FORWARD(TrajOptPlanningContext); - +/* struct TrajOptPlannerConfiguration{ // TrajOptPlannerConfiguration(trajopt::TrajOptProbPtr prob) : prob(prob) {} @@ -38,23 +40,24 @@ struct TrajOptPlannerConfiguration{ // std::string model_type = sco::ModelType::AUTO_SOLVER // sco::OptProbPtr prob = sco::OptProbPtr(new sco::OptProb(sco::ModelType::AUTO_SOLVER)); - /** @brief Trajopt problem to be solved (Required) */ + // @brief Trajopt problem to be solved (Required) TrajOptProblemPtr prob; // TrajOptProblemPtr prob(new TrajOptProblem()); // std::shared_ptr prob = std::shared_ptr(new TrajOptProblem); - /** @brief Optimization parameters to be used (Optional) */ + // @brief Optimization parameters to be used (Optional) sco::BasicTrustRegionSQPParameters params; - /** @brief Callback functions called on each iteration of the optimization (Optional) */ + // @brief Callback functions called on each iteration of the optimization (Optional) //std::vector callbacks; sco::Optimizer::Callback callbacks; }; +*/ -trajopt::TrajArray generateInitialTrajectory(const int& num_steps); -TrajOptPlannerConfiguration spec_; +// trajopt::TrajArray generateInitialTrajectory(const int& num_steps); +//TrajOptPlannerConfiguration spec_; int dof_; void callBackFunc(sco::OptProb* oprob, sco::OptResults& ores); @@ -74,7 +77,7 @@ class TrajOptPlanningContext : public planning_interface::PlanningContext bool terminate() override; void clear() override; - void setTrajOptPlannerConfiguration(); + // void setTrajOptPlannerConfiguration(); protected: @@ -83,6 +86,7 @@ class TrajOptPlanningContext : public planning_interface::PlanningContext moveit::core::RobotModelConstPtr robot_model_; robot_state::RobotStatePtr robot_state_; + TrajOptInterfacePtr trajopot_interface_; trajectory_msgs::JointTrajectory convertTrajArrayToJointTrajectory(const trajopt::TrajArray& traj_array); From dc272dff77c8e96b2f4f4ae8eb114bd14e55d028 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Mon, 15 Jul 2019 18:13:53 -0600 Subject: [PATCH 553/612] changed the architectrue and used TrajoptInterface class to contain the solve function details. Created a TrajOptInterface object in TrajoptPlanningContext class instead --- .../trajopt/include/problem_description.h | 2 +- .../trajopt/include/trajopt_interface.h | 25 +++++++--- .../include/trajopt_planning_context.h | 47 +------------------ 3 files changed, 21 insertions(+), 53 deletions(-) diff --git a/moveit_planners/trajopt/include/problem_description.h b/moveit_planners/trajopt/include/problem_description.h index 73d7d1abe6..58286cc273 100644 --- a/moveit_planners/trajopt/include/problem_description.h +++ b/moveit_planners/trajopt/include/problem_description.h @@ -4,7 +4,7 @@ #include #include #include - +#include namespace trajopt_interface { diff --git a/moveit_planners/trajopt/include/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface.h index 12ce32b414..683fa6d187 100644 --- a/moveit_planners/trajopt/include/trajopt_interface.h +++ b/moveit_planners/trajopt/include/trajopt_interface.h @@ -58,24 +58,32 @@ #include "problem_description.h" +#include +#include +#include namespace trajopt_interface { - // MOVEIT_CLASS_FORWARD(TrajOptInterface); +MOVEIT_CLASS_FORWARD(TrajOptInterface); class TrajOptInterface //: public trajopt::TrajOptPlanner { public: - TrajOptInterface(); - virtual ~TrajOptInterface() = 0; - + TrajOptInterface(const ros::NodeHandle& nh = ros::NodeHandle("~")); + virtual ~TrajOptInterface(); const sco::BasicTrustRegionSQPParameters& getParams() const { return params_; } + bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::MotionPlanRequest& req, + const sco::BasicTrustRegionSQPParameters& params, planning_interface::MotionPlanResponse& res) const; // const + + trajopt::TrajArray generateInitialTrajectory(const int& num_steps, const std::vector& joint_vals); + protected: /** @brief Configure everything using the param server */ - void setTrajOptPlannerConfiguration(); + void setParamsFromRosParamServer(); + void setDefaultParams(); ros::NodeHandle nh_; /// The ROS node handle @@ -85,9 +93,12 @@ class TrajOptInterface //: public trajopt::TrajOptPlanner TrajOptProblemPtr prob_; - void callBackFunc(sco::OptProb* oprob, sco::OptResults& ores); - + // a function to convert TrajArray (TrajArray has no dependency on tesseract) from trajopt_ros to trajectory_msgs::JointTrajectory.Points + trajectory_msgs::JointTrajectory convertTrajArrayToJointTrajectory(const trajopt::TrajArray& traj_array, const std::vector& j_names); }; + +void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res); + } #endif diff --git a/moveit_planners/trajopt/include/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_planning_context.h index debb20a29d..20edb5cdbb 100644 --- a/moveit_planners/trajopt/include/trajopt_planning_context.h +++ b/moveit_planners/trajopt/include/trajopt_planning_context.h @@ -19,7 +19,6 @@ #include #include "problem_description.h" - #include "trajopt_interface.h" //using namespace trajopt; @@ -28,40 +27,7 @@ namespace trajopt_interface { MOVEIT_CLASS_FORWARD(TrajOptPlanningContext); -/* -struct TrajOptPlannerConfiguration{ - - // TrajOptPlannerConfiguration(trajopt::TrajOptProbPtr prob) : prob(prob) {} - - TrajOptPlannerConfiguration() {prob.reset(new TrajOptProblem());} - - virtual ~TrajOptPlannerConfiguration() {} - - // std::string model_type = sco::ModelType::AUTO_SOLVER - // sco::OptProbPtr prob = sco::OptProbPtr(new sco::OptProb(sco::ModelType::AUTO_SOLVER)); - - // @brief Trajopt problem to be solved (Required) - TrajOptProblemPtr prob; - - // TrajOptProblemPtr prob(new TrajOptProblem()); - // std::shared_ptr prob = std::shared_ptr(new TrajOptProblem); - - // @brief Optimization parameters to be used (Optional) - sco::BasicTrustRegionSQPParameters params; - // @brief Callback functions called on each iteration of the optimization (Optional) - //std::vector callbacks; - sco::Optimizer::Callback callbacks; - -}; -*/ - -// trajopt::TrajArray generateInitialTrajectory(const int& num_steps); -//TrajOptPlannerConfiguration spec_; -int dof_; - -void callBackFunc(sco::OptProb* oprob, sco::OptResults& ores); - class TrajOptPlanningContext : public planning_interface::PlanningContext { public: @@ -77,23 +43,14 @@ class TrajOptPlanningContext : public planning_interface::PlanningContext bool terminate() override; void clear() override; - // void setTrajOptPlannerConfiguration(); - protected: private: moveit::core::RobotModelConstPtr robot_model_; - robot_state::RobotStatePtr robot_state_; - - TrajOptInterfacePtr trajopot_interface_; - - - trajectory_msgs::JointTrajectory convertTrajArrayToJointTrajectory(const trajopt::TrajArray& traj_array); - - // sco::VarArray convertVarVectorToVarArray(const VarVector& vv); + // robot_state::RobotStatePtr robot_state_; - tesseract::tesseract_planning::PlannerRequest tess_request_; + TrajOptInterfacePtr trajopt_interface_; }; } // namespace trajopt_interface From f5c954a063e1cf688ac66c4eb4c6c09bc3c73c69 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Fri, 19 Jul 2019 17:15:39 -0600 Subject: [PATCH 554/612] added kinematic_terms in problem_description, added comments anout term_type added ProblemInfo, TermInfo, InitInfo, BasicInfo in kinematic_terms, added operator() added ConstructProblem added some param for problem_info --- .../trajopt/include/kinematic_terms.hpp | 41 ++++ .../trajopt/include/problem_description.h | 195 ++++++++++++++---- .../trajopt/include/trajopt_interface.h | 3 +- 3 files changed, 194 insertions(+), 45 deletions(-) create mode 100644 moveit_planners/trajopt/include/kinematic_terms.hpp diff --git a/moveit_planners/trajopt/include/kinematic_terms.hpp b/moveit_planners/trajopt/include/kinematic_terms.hpp new file mode 100644 index 0000000000..4167344ee6 --- /dev/null +++ b/moveit_planners/trajopt/include/kinematic_terms.hpp @@ -0,0 +1,41 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +namespace trajopt_interface +{ + +/** + * @brief Used to calculate the error for StaticCartPoseTermInfo + * This is converted to a cost or constraint using TrajOptCostFromErrFunc or TrajOptConstraintFromErrFunc + */ +struct CartPoseErrCalculator : public VectorOfVector +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Isometry3d target_pose_inv_; + // tesseract::BasicKinConstPtr manip_; + // tesseract::BasicEnvConstPtr env_; + planning_scene::PlanningSceneConstPtr planning_scene_; + std::string link_; + Eigen::Isometry3d tcp_; + + CartPoseErrCalculator(const Eigen::Isometry3d& pose, + const planning_scene::PlanningSceneConstPtr& planning_scene, + std::string link, + Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) + : target_pose_inv_(pose.inverse()), planning_scene_(planning_scene), link_(link), tcp_(tcp) + { + } + + Eigen::VectorXd operator()(const Eigen::VectorXd& dof_vals) const override; +}; + + +} // namespace trajopt diff --git a/moveit_planners/trajopt/include/problem_description.h b/moveit_planners/trajopt/include/problem_description.h index 58286cc273..a9e8c31868 100644 --- a/moveit_planners/trajopt/include/problem_description.h +++ b/moveit_planners/trajopt/include/problem_description.h @@ -6,12 +6,153 @@ #include #include +#include + namespace trajopt_interface { +/** +@brief Used to apply cost/constraint to joint-space velocity + +Term is applied to every step between first_step and last_step. It applies two limits, upper_limits/lower_limits, +to the joint velocity subject to the following cases. + +* term_type = TT_COST +** upper_limit = lower_limit = 0 - Cost is applied with a SQUARED error scaled for each joint by coeffs +** upper_limit != lower_limit - 2 hinge costs are applied scaled for each joint by coeffs. If velocity < upper_limit and +velocity > lower_limit, no penalty. + +* term_type = TT_CNT +** upper_limit = lower_limit = 0 - Equality constraint is applied +** upper_limit != lower_limit - 2 Inequality constraints are applied. These are both satisfied when velocity < +upper_limit and velocity > lower_limit + +Note: coeffs, upper_limits, and lower_limits are optional. If a term is not given it will default to 0 for all joints. +If one value is given, this will be broadcast to all joints. + +Note: Velocity is calculated numerically using forward finite difference + +\f{align*}{ + cost = \sum_{t=0}^{T-2} \sum_j c_j (x_{t+1,j} - x_{t,j})^2 +\f} +where j indexes over DOF, and \f$c_j\f$ are the coeffs. +*/ + + +struct TermInfo; +typedef std::shared_ptr TermInfoPtr; + class TrajOptProblem; typedef std::shared_ptr TrajOptProblemPtr; +TrajOptProblemPtr ConstructProblem(const ProblemInfo&); + +struct BasicInfo +{ + /** @brief If true first time step is fixed with a joint level constraint*/ + bool start_fixed; + /** @brief Number of time steps (rows) in the optimization matrix */ + int n_steps; + std::string manip; + std::string robot; // optional + IntVec dofs_fixed; // optional + sco::ModelType convex_solver; // which convex solver to use + + /** @brief If true, the last column in the optimization matrix will be 1/dt */ + bool use_time = false; + /** @brief The upper limit of 1/dt values allowed in the optimization*/ + double dt_upper_lim = 1.0; + /** @brief The lower limit of 1/dt values allowed in the optimization*/ + double dt_lower_lim = 1.0; +}; + +/** +Initialization info read from json +*/ +struct InitInfo +{ + /** @brief Methods of initializing the optimization matrix + + STATIONARY: Initializes all joint values to the initial value (the current value in the env + pci.env->getCurrentJointValues) + JOINT_INTERPOLATED: Linearly interpolates between initial value and the joint position specified in InitInfo.data + GIVEN_TRAJ: Initializes the matrix to a given trajectory + + In all cases the dt column (if present) is appended the selected method is defined. + */ + enum Type + { + STATIONARY, + JOINT_INTERPOLATED, + GIVEN_TRAJ, + }; + /** @brief Specifies the type of initialization to use */ + Type type; + /** @brief Data used during initialization. Use depends on the initialization selected. */ + TrajArray data; + /** @brief Default value the final column of the optimization is initialized too if time is being used */ + double dt = 1.0; +}; + +/** +When cost or constraint element of JSON doc is read, one of these guys gets +constructed to hold the parameters. +Then it later gets converted to a Cost object by the hatch method +*/ +struct TermInfo +{ + std::string name; + int term_type; + int getSupportedTypes() { return supported_term_types_; } + // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; + virtual void hatch(TrajOptProb& prob) = 0; + + static TermInfoPtr fromName(const std::string& type); + + /** + * Registers a user-defined TermInfo so you can use your own cost + * see function RegisterMakers.cpp + */ + typedef TermInfoPtr (*MakerFunc)(void); + static void RegisterMaker(const std::string& type, MakerFunc); + + virtual ~TermInfo() = default; + +protected: + TermInfo(int supported_term_types) : supported_term_types_(supported_term_types) {} +private: + static std::map name2maker; + int supported_term_types_; +}; + + +struct ProblemInfo +{ +public: + BasicInfo basic_info; + sco::BasicTrustRegionSQPParameters opt_info; + std::vector cost_infos; + std::vector cnt_infos; + InitInfo init_info; + + // tesseract::BasicEnvConstPtr env; + // tesseract::BasicKinConstPtr kin; + planning_scene::PlanningSceneConstPtr planning_scene; + std::string planning_group_name; + + ProblemConstructionInfo(planning_scene::PlanningSceneConstPtr& ps, std::string pg) + : planning_scene_(ps), planning_group_name(pg) {} + // void fromJson(const Json::Value& v); + +private: + // void readBasicInfo(const Json::Value& v); + // void readOptInfo(const Json::Value& v); + // void readCosts(const Json::Value& v); + // void readConstraints(const Json::Value& v); + // void readInitInfo(const Json::Value& v); +}; + + /** * Holds all the data for a trajectory optimization problem * so you can modify it programmatically, e.g. add your own costs @@ -20,7 +161,7 @@ class TrajOptProblem : public sco::OptProb { public: TrajOptProblem(); - TrajOptProblem(const int& n_steps, bool& use_time, const robot_model::RobotModelConstPtr& robot_model); + TrajOptProblem(const int& n_steps, ProblemInfo problem_info); virtual ~TrajOptProblem() = default; sco::VarVector GetVarRow(int i, int start_col, int num_col) { return m_traj_vars.rblock(i, start_col, num_col); } sco::VarVector GetVarRow(int i) { return m_traj_vars.row(i); } @@ -33,16 +174,9 @@ class TrajOptProblem : public sco::OptProb int GetNumDOF() { return m_traj_vars.cols(); } // tesseract::BasicKinConstPtr GetKin() { return m_kin; } //tesseract::BasicEnvConstPtr GetEnv() { return m_env; } - void SetInitTraj(const trajopt::TrajArray& x){ - std::cout << "====>>>>> setInitTraj is called " << std::endl; - std::cout << "x.size: " << x.size() << std::endl; - m_init_traj = x; - // std::cout << m_init_traj << std::endl; - } - trajopt::TrajArray GetInitTraj() { - std::cout << "====>>>>> GetInitTraj is called " << std::endl; - return m_init_traj; - } + planning_scene::PlanningSceneConstPtr GetPlanningScene() { return planning_scene_;} + void SetInitTraj(const trajopt::TrajArray& x){ m_init_traj = x; } + trajopt::TrajArray GetInitTraj() { return m_init_traj; } // friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&); /** @brief Returns TrajOptProb.has_time */ bool GetHasTime() { return has_time; } @@ -55,40 +189,16 @@ class TrajOptProblem : public sco::OptProb trajopt::VarArray m_traj_vars; // tesseract::BasicKinConstPtr m_kin; // tesseract::BasicEnvConstPtr m_env; + planning_scene::PlanningSceneConstPtr planning_scene_; + std::string planning_group_; trajopt::TrajArray m_init_traj; }; -/** -When cost or constraint element of JSON doc is read, one of these guys gets -constructed to hold the parameters. -Then it later gets converted to a Cost object by the hatch method -*/ -struct TermInfo -{ - std::string name; - int term_type; - int getSupportedTypes() { return supported_term_types_; } - // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; - virtual void hatch(TrajOptProblem& prob) = 0; - - // static TermInfoPtr fromName(const std::string& type); - - /** - * Registers a user-defined TermInfo so you can use your own cost - * see function RegisterMakers.cpp - */ - // typedef TermInfoPtr (*MakerFunc)(void); - // static void RegisterMaker(const std::string& type, MakerFunc); - - virtual ~TermInfo() = default; -protected: - TermInfo(int supported_term_types) : supported_term_types_(supported_term_types) {} -private: - // static std::map name2maker; - int supported_term_types_; -}; +/** @brief This term is used when the goal frame is fixed in cartesian space + Set term_type == TT_COST or TT_CNT for cost or constraint. +*/ struct CartPoseTermInfo : public TermInfo { // EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -111,10 +221,9 @@ struct CartPoseTermInfo : public TermInfo /** @brief Used to add term to pci from json */ // void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void hatch(TrajOptProblem& prob) override; - // DEFINE_CREATE(CartPoseTermInfo) + void hatch(TrajOptProb& prob) override; + DEFINE_CREATE(CartPoseTermInfo) }; - } // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface.h index 683fa6d187..bf1978cf5e 100644 --- a/moveit_planners/trajopt/include/trajopt_interface.h +++ b/moveit_planners/trajopt/include/trajopt_interface.h @@ -71,12 +71,11 @@ class TrajOptInterface //: public trajopt::TrajOptPlanner public: TrajOptInterface(const ros::NodeHandle& nh = ros::NodeHandle("~")); - virtual ~TrajOptInterface(); const sco::BasicTrustRegionSQPParameters& getParams() const { return params_; } bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::MotionPlanRequest& req, - const sco::BasicTrustRegionSQPParameters& params, planning_interface::MotionPlanResponse& res) const; // const + const sco::BasicTrustRegionSQPParameters& params, planning_interface::MotionPlanResponse& res); trajopt::TrajArray generateInitialTrajectory(const int& num_steps, const std::vector& joint_vals); From 96a1e766d51ef1647582bc26fd69a6558fd8f61d Mon Sep 17 00:00:00 2001 From: omid1366 Date: Sun, 21 Jul 2019 20:56:01 -0600 Subject: [PATCH 555/612] moved rotVec and concat from kinematics_term header to implementation in problem_Description, added term_type used MotionPlaneRequest from planning_interface instead that of moveit_msgs for solve function input added generateInitiaTrajectory added basic_info to problem_info --- ...{kinematic_terms.hpp => kinematic_terms.h} | 38 +++++++++++++++- .../trajopt/include/problem_description.h | 33 +++++++++++--- .../trajopt/include/trajopt_interface.h | 6 +-- moveit_planners/trajopt/src/test_trajopt.cpp | 44 +++++++++++-------- 4 files changed, 91 insertions(+), 30 deletions(-) rename moveit_planners/trajopt/include/{kinematic_terms.hpp => kinematic_terms.h} (61%) diff --git a/moveit_planners/trajopt/include/kinematic_terms.hpp b/moveit_planners/trajopt/include/kinematic_terms.h similarity index 61% rename from moveit_planners/trajopt/include/kinematic_terms.hpp rename to moveit_planners/trajopt/include/kinematic_terms.h index 4167344ee6..b5223a73b0 100644 --- a/moveit_planners/trajopt/include/kinematic_terms.hpp +++ b/moveit_planners/trajopt/include/kinematic_terms.h @@ -8,15 +8,49 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include +#include + +#include + +#include + +using namespace Eigen; +using namespace std; namespace trajopt_interface { +Vector3d rotVec(const Matrix3d& m) +{ + Quaterniond q; q = m; + return Vector3d(q.x(), q.y(), q.z()); +} + + +VectorXd concat(const VectorXd& a, const VectorXd& b) +{ + VectorXd out(a.size()+b.size()); + out.topRows(a.size()) = a; + out.middleRows(a.size(), b.size()) = b; + return out; +} + +template +vector concat(const vector& a, const vector& b) +{ + vector out; + vector x; + out.insert(out.end(), a.begin(), a.end()); + out.insert(out.end(), b.begin(), b.end()); + return out; +} + + /** * @brief Used to calculate the error for StaticCartPoseTermInfo * This is converted to a cost or constraint using TrajOptCostFromErrFunc or TrajOptConstraintFromErrFunc */ -struct CartPoseErrCalculator : public VectorOfVector +struct CartPoseErrCalculator : public sco::VectorOfVector { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Isometry3d target_pose_inv_; @@ -27,7 +61,7 @@ struct CartPoseErrCalculator : public VectorOfVector Eigen::Isometry3d tcp_; CartPoseErrCalculator(const Eigen::Isometry3d& pose, - const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_scene::PlanningSceneConstPtr planning_scene, std::string link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) : target_pose_inv_(pose.inverse()), planning_scene_(planning_scene), link_(link), tcp_(tcp) diff --git a/moveit_planners/trajopt/include/problem_description.h b/moveit_planners/trajopt/include/problem_description.h index a9e8c31868..cab7481232 100644 --- a/moveit_planners/trajopt/include/problem_description.h +++ b/moveit_planners/trajopt/include/problem_description.h @@ -45,8 +45,26 @@ typedef std::shared_ptr TermInfoPtr; class TrajOptProblem; typedef std::shared_ptr TrajOptProblemPtr; +struct ProblemInfo; + TrajOptProblemPtr ConstructProblem(const ProblemInfo&); + +enum TermType +{ + TT_COST = 0x1, // 0000 0001 + TT_CNT = 0x2, // 0000 0010 + TT_USE_TIME = 0x4, // 0000 0100 +}; + +#define DEFINE_CREATE(classname) \ + static TermInfoPtr create() \ + { \ + TermInfoPtr out(new classname()); \ + return out; \ + } + + struct BasicInfo { /** @brief If true first time step is fixed with a joint level constraint*/ @@ -55,7 +73,7 @@ struct BasicInfo int n_steps; std::string manip; std::string robot; // optional - IntVec dofs_fixed; // optional + sco::IntVec dofs_fixed; // optional sco::ModelType convex_solver; // which convex solver to use /** @brief If true, the last column in the optimization matrix will be 1/dt */ @@ -89,7 +107,7 @@ struct InitInfo /** @brief Specifies the type of initialization to use */ Type type; /** @brief Data used during initialization. Use depends on the initialization selected. */ - TrajArray data; + trajopt::TrajArray data; /** @brief Default value the final column of the optimization is initialized too if time is being used */ double dt = 1.0; }; @@ -105,7 +123,7 @@ struct TermInfo int term_type; int getSupportedTypes() { return supported_term_types_; } // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; - virtual void hatch(TrajOptProb& prob) = 0; + virtual void hatch(TrajOptProblem& prob) = 0; static TermInfoPtr fromName(const std::string& type); @@ -140,8 +158,8 @@ struct ProblemInfo planning_scene::PlanningSceneConstPtr planning_scene; std::string planning_group_name; - ProblemConstructionInfo(planning_scene::PlanningSceneConstPtr& ps, std::string pg) - : planning_scene_(ps), planning_group_name(pg) {} + ProblemInfo(planning_scene::PlanningSceneConstPtr ps, std::string pg) + : planning_scene(ps), planning_group_name(pg) {} // void fromJson(const Json::Value& v); private: @@ -161,7 +179,7 @@ class TrajOptProblem : public sco::OptProb { public: TrajOptProblem(); - TrajOptProblem(const int& n_steps, ProblemInfo problem_info); + TrajOptProblem(const ProblemInfo& problem_info); virtual ~TrajOptProblem() = default; sco::VarVector GetVarRow(int i, int start_col, int num_col) { return m_traj_vars.rblock(i, start_col, num_col); } sco::VarVector GetVarRow(int i) { return m_traj_vars.row(i); } @@ -221,9 +239,10 @@ struct CartPoseTermInfo : public TermInfo /** @brief Used to add term to pci from json */ // void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void hatch(TrajOptProb& prob) override; + void hatch(TrajOptProblem& prob) override; DEFINE_CREATE(CartPoseTermInfo) }; +trajopt::TrajArray generateInitialTrajectory(const int& num_steps, const std::vector& joint_vals); } // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface.h index bf1978cf5e..b4dd24c9b2 100644 --- a/moveit_planners/trajopt/include/trajopt_interface.h +++ b/moveit_planners/trajopt/include/trajopt_interface.h @@ -74,10 +74,10 @@ class TrajOptInterface //: public trajopt::TrajOptPlanner const sco::BasicTrustRegionSQPParameters& getParams() const { return params_; } - bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::MotionPlanRequest& req, - const sco::BasicTrustRegionSQPParameters& params, planning_interface::MotionPlanResponse& res); + bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, + const sco::BasicTrustRegionSQPParameters& params, moveit_msgs::MotionPlanDetailedResponse& res); + - trajopt::TrajArray generateInitialTrajectory(const int& num_steps, const std::vector& joint_vals); protected: /** @brief Configure everything using the param server */ diff --git a/moveit_planners/trajopt/src/test_trajopt.cpp b/moveit_planners/trajopt/src/test_trajopt.cpp index 55a26c08be..232c6f9f54 100644 --- a/moveit_planners/trajopt/src/test_trajopt.cpp +++ b/moveit_planners/trajopt/src/test_trajopt.cpp @@ -58,7 +58,7 @@ int main(int argc, char** argv) planner_plugin_name = "trajopt_interface/TrajOptPlanner"; node_handle.setParam("planning_plugin", planner_plugin_name); - + // We will get the name of planning plugin we want to load // from the ROS parameter server, and then load the planner // making sure to catch all exceptions. @@ -194,23 +194,20 @@ int main(int argc, char** argv) std::cout << "==>> joint position at goal " << x.position << std::endl; } - - //------ - // We now construct a planning context that encapsulate the scene, - // the request and the response. We call the planner using this + // planning context planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); - - context->solve(res); - if (res.error_code_.val != res.error_code_.SUCCESS) - { - ROS_ERROR("Could not compute plan successfully"); - return 0; - } - visual_tools.prompt("Press 'next' to visualize the reslt "); - + context->solve(res); + if (res.error_code_.val != res.error_code_.SUCCESS) + { + ROS_ERROR("Could not compute plan successfully"); + return 0; + } + + visual_tools.prompt("Press 'next' to visualize the reslt "); + // Visualize the result // ^^^^^^^^^^^^^^^^^^^^ ros::Publisher display_publisher = @@ -221,13 +218,24 @@ int main(int argc, char** argv) moveit_msgs::MotionPlanResponse response; res.getMessage(response); + for (int timestep_index = 0; timestep_index < 10; ++timestep_index) + { + for (int joint_index = 0; joint_index < 7; ++joint_index) + { + response.trajectory.joint_trajectory.points[timestep_index].positions[joint_index]; + } + std::cout << "-------------------- next timestep ---------------" << std::endl; + } + + + display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); display_publisher.publish(display_trajectory); - /* - // Set the state in the planning scene to the final state of the last plan + + // Set the state in the planning scene to the final state of the last plan robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); planning_scene->setCurrentState(*robot_state.get()); @@ -235,7 +243,7 @@ int main(int argc, char** argv) visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); visual_tools.publishAxisLabeled(pose.pose, "goal_1"); visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); -*/ + visual_tools.trigger(); + } From 2f0aca234e772e1cc1969bb8929347c6720aef28 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Mon, 22 Jul 2019 20:00:21 -0600 Subject: [PATCH 556/612] added panda_omple_planning added checkParameterSize function in problem_description added JointPoseTermInfo::hatch in problem_description added res.error.code.val to check the request in trajopt_interface.cpp convert req.goal_constraint to JointPoseTermInfo in trajopt_interface.cpp feed the response with the trajopt solution in trajopt_interface.cpp remove comments from trajopt_planning_context.cpp --- .../trajopt/config/panda_ompl_planning.yaml | 203 ++++++++++++++++++ moveit_planners/trajopt/config/setup.yaml | 27 +-- .../trajopt/include/kinematic_terms.h | 5 + .../trajopt/include/problem_description.h | 36 ++++ .../include/trajopt_planning_context.h | 2 - .../trajopt/launch/test_trajopt_launch.launch | 1 + moveit_planners/trajopt/src/test_trajopt.cpp | 114 +++++----- 7 files changed, 310 insertions(+), 78 deletions(-) create mode 100644 moveit_planners/trajopt/config/panda_ompl_planning.yaml diff --git a/moveit_planners/trajopt/config/panda_ompl_planning.yaml b/moveit_planners/trajopt/config/panda_ompl_planning.yaml new file mode 100644 index 0000000000..491e82914e --- /dev/null +++ b/moveit_planners/trajopt/config/panda_ompl_planning.yaml @@ -0,0 +1,203 @@ +planner_configs: + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar + FMTkConfigDefault: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMTkConfigDefault: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDSTkConfigDefault: + type: geometric::PDST + STRIDEkConfigDefault: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRTkConfigDefault: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRTkConfigDefault: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiESTkConfigDefault: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjESTkConfigDefault: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRMkConfigDefault: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstarkConfigDefault: + type: geometric::LazyPRMstar + SPARSkConfigDefault: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwokConfigDefault: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + TrajOptDefault: + type: geometric::TrajOpt + +panda_arm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault +panda_arm_hand: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault +hand: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault diff --git a/moveit_planners/trajopt/config/setup.yaml b/moveit_planners/trajopt/config/setup.yaml index f44b12ef66..d5e7728589 100644 --- a/moveit_planners/trajopt/config/setup.yaml +++ b/moveit_planners/trajopt/config/setup.yaml @@ -1,13 +1,14 @@ -improve_ratio_threshold: 0.25; -min_trust_box_size: 1e-4; -min_approx_improve: 1e-4; -min_approx_improve_frac: -1000; # should br -infinity -max_iter: 50; -trust_shrink_ratio: 0.1; -trust_expand_ratio: 1.5; -cnt_tolerance: 1e-4; -max_merit_coeff_increases: 5; -merit_coeff_increase_ratio : 10; -max_time: 1000; # should be +infinity -merit_error_coeff: 10; -trust_box_size: 1e-1; +trajopt_param: + improve_ratio_threshold: 0.25; + min_trust_box_size: 1e-4; + min_approx_improve: 1e-4; + min_approx_improve_frac: -1000; # should br -infinity + max_iter: 50; + trust_shrink_ratio: 0.1; + trust_expand_ratio: 1.5; + cnt_tolerance: 1e-4; + max_merit_coeff_increases: 5; + merit_coeff_increase_ratio : 10; + max_time: 1000; # should be +infinity + merit_error_coeff: 10; + trust_box_size: 1e-1; diff --git a/moveit_planners/trajopt/include/kinematic_terms.h b/moveit_planners/trajopt/include/kinematic_terms.h index b5223a73b0..25162f8abe 100644 --- a/moveit_planners/trajopt/include/kinematic_terms.h +++ b/moveit_planners/trajopt/include/kinematic_terms.h @@ -71,5 +71,10 @@ struct CartPoseErrCalculator : public sco::VectorOfVector Eigen::VectorXd operator()(const Eigen::VectorXd& dof_vals) const override; }; + //TODO(omid): The following should be added and adjusted from trajopt + // JointPosEqCost + // JointPosIneqCost + // JointPosEqConstraint + // JointPosIneqConstraint } // namespace trajopt diff --git a/moveit_planners/trajopt/include/problem_description.h b/moveit_planners/trajopt/include/problem_description.h index cab7481232..77f4adbcc2 100644 --- a/moveit_planners/trajopt/include/problem_description.h +++ b/moveit_planners/trajopt/include/problem_description.h @@ -243,6 +243,42 @@ struct CartPoseTermInfo : public TermInfo DEFINE_CREATE(CartPoseTermInfo) }; + +/** + \brief Joint space position cost + Position operates on a single point (unlike velocity, etc). This is b/c the primary usecase is joint-space + position waypoints + + \f{align*}{ + \sum_i c_i (x_i - xtarg_i)^2 + \f} + where \f$i\f$ indexes over dof and \f$c_i\f$ are coeffs + */ +struct JointPosTermInfo : public TermInfo +{ + /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec coeffs; + /** @brief Vector of position targets. This is a required value. Size should be the DOF of the system */ + trajopt::DblVec targets; + /** @brief Vector of position upper limits. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec upper_tols; + /** @brief Vector of position lower limits. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec lower_tols; + /** @brief First time step to which the term is applied. Default: 0 */ + int first_step = 0; + /** @brief Last time step to which the term is applied. Default: prob.GetNumSteps() - 1*/ + int last_step = -1; + + /** @brief Initialize term with it's supported types */ + JointPosTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) {} + + /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ + void hatch(TrajOptProblem& prob) override; + DEFINE_CREATE(JointPosTermInfo) +}; + + + trajopt::TrajArray generateInitialTrajectory(const int& num_steps, const std::vector& joint_vals); } // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_planning_context.h index 20edb5cdbb..92d6642aa6 100644 --- a/moveit_planners/trajopt/include/trajopt_planning_context.h +++ b/moveit_planners/trajopt/include/trajopt_planning_context.h @@ -21,7 +21,6 @@ #include "problem_description.h" #include "trajopt_interface.h" -//using namespace trajopt; namespace trajopt_interface { @@ -48,7 +47,6 @@ class TrajOptPlanningContext : public planning_interface::PlanningContext private: moveit::core::RobotModelConstPtr robot_model_; - // robot_state::RobotStatePtr robot_state_; TrajOptInterfacePtr trajopt_interface_; }; diff --git a/moveit_planners/trajopt/launch/test_trajopt_launch.launch b/moveit_planners/trajopt/launch/test_trajopt_launch.launch index f0d35bdeaa..c80e5c93e1 100644 --- a/moveit_planners/trajopt/launch/test_trajopt_launch.launch +++ b/moveit_planners/trajopt/launch/test_trajopt_launch.launch @@ -28,6 +28,7 @@ + diff --git a/moveit_planners/trajopt/src/test_trajopt.cpp b/moveit_planners/trajopt/src/test_trajopt.cpp index 232c6f9f54..28c2c27033 100644 --- a/moveit_planners/trajopt/src/test_trajopt.cpp +++ b/moveit_planners/trajopt/src/test_trajopt.cpp @@ -30,11 +30,9 @@ int main(int argc, char** argv) /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/ robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model)); const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); - + robot_state->setToDefaultValues(); const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); - std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> dof: " << joint_names.size() << std::endl; - planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); // With the planning scene we create a planing scene monitor that @@ -56,11 +54,13 @@ int main(int argc, char** argv) std::string planner_plugin_name; - planner_plugin_name = "trajopt_interface/TrajOptPlanner"; + planner_plugin_name = "trajopt_interface/TrajOptPlanner"; + + // planner_plugin_name = "ompl_interface/OMPLPlanner"; + // node_handle.getParam("...", ...); + node_handle.setParam("planning_plugin", planner_plugin_name); - // We will get the name of planning plugin we want to load - // from the ROS parameter server, and then load the planner // making sure to catch all exceptions. if (!node_handle.getParam("planning_plugin", planner_plugin_name)) ROS_FATAL_STREAM("Could not find planner plugin name"); @@ -91,19 +91,8 @@ int main(int argc, char** argv) << "Available plugins: " << ss.str()); } - - - - - - - - - - - // Visualization - // ^^^^^^^^^^^^^ + // ======================================================================================== // The package MoveItVisualTools provides many capabilties for visualizing objects, robots, // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script namespace rvt = rviz_visual_tools; @@ -129,7 +118,7 @@ int main(int argc, char** argv) visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); // Pose Goal - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // ======================================================================================== // We will now create a motion plan request for the arm of the Panda // specifying the desired pose of the end-effector as input. visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); @@ -158,12 +147,11 @@ int main(int argc, char** argv) moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); - req.group_name = PLANNING_GROUP; - req.goal_constraints.push_back(pose_goal); + // req.group_name = PLANNING_GROUP; + // req.goal_constraints.push_back(pose_goal); // set the request start joint state - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - + // ======================================================================================== // get the joint values of the start state std::vector start_joint_values; robot_state->copyJointGroupPositions(joint_model_group, start_joint_values); @@ -173,17 +161,19 @@ int main(int argc, char** argv) printf("===>>> joint: %s with value: %f \n", joint_names[r].c_str(), x); ++r; } - req.start_state.joint_state.position = start_joint_values; + req.start_state.joint_state.name = joint_names; + req.start_state.joint_state.position = start_joint_values; // set the goal joint state - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // ======================================================================================== robot_state::RobotState goal_state(robot_model); - std::vector joint_values = { 0.8, 0.7, 1, 1.3, 1.9, 2.2, 3 }; + std::vector joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1}; goal_state.setJointGroupPositions(joint_model_group, joint_values); moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); req.goal_constraints.clear(); req.goal_constraints.push_back(joint_goal); + req.group_name = PLANNING_GROUP; // retrive joint values at goal std::vector goal_constraints = req.goal_constraints; @@ -194,56 +184,54 @@ int main(int argc, char** argv) std::cout << "==>> joint position at goal " << x.position << std::endl; } - // planning context - planning_interface::PlanningContextPtr context = - planner_instance->getPlanningContext(planning_scene, req, res.error_code_); + planning_interface::PlanningContextPtr context = + planner_instance->getPlanningContext(planning_scene, req, res.error_code_); - context->solve(res); - if (res.error_code_.val != res.error_code_.SUCCESS) - { - ROS_ERROR("Could not compute plan successfully"); - return 0; - } + context->solve(res); + if (res.error_code_.val != res.error_code_.SUCCESS) + { + ROS_ERROR("Could not compute plan successfully"); + return 0; + } - visual_tools.prompt("Press 'next' to visualize the reslt "); + visual_tools.prompt("Press 'next' to visualize the result"); -// Visualize the result - // ^^^^^^^^^^^^^^^^^^^^ - ros::Publisher display_publisher = + // Visualize the result + // ======================================================================================== + ros::Publisher display_publisher = node_handle.advertise("/move_group/display_planned_path", 1, true); - moveit_msgs::DisplayTrajectory display_trajectory; + moveit_msgs::DisplayTrajectory display_trajectory; - /* Visualize the trajectory */ - moveit_msgs::MotionPlanResponse response; - res.getMessage(response); - - for (int timestep_index = 0; timestep_index < 10; ++timestep_index) - { - for (int joint_index = 0; joint_index < 7; ++joint_index) - { - response.trajectory.joint_trajectory.points[timestep_index].positions[joint_index]; - } - std::cout << "-------------------- next timestep ---------------" << std::endl; - } + /* Visualize the trajectory */ + moveit_msgs::MotionPlanResponse response; + res.getMessage(response); + for (int timestep_index = 0; timestep_index < response.trajectory.joint_trajectory.points.size(); ++timestep_index) + { + for (int joint_index = 0; joint_index < response.trajectory.joint_trajectory.points[timestep_index].positions.size(); ++joint_index) + { + std::cout << response.trajectory.joint_trajectory.points[timestep_index].positions[joint_index] << " "; + } + std::cout << std::endl; + } + display_trajectory.trajectory_start = response.trajectory_start; + display_trajectory.trajectory.push_back(response.trajectory); - display_trajectory.trajectory_start = response.trajectory_start; - display_trajectory.trajectory.push_back(response.trajectory); - visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); - visual_tools.trigger(); - display_publisher.publish(display_trajectory); + visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); + visual_tools.trigger(); + display_publisher.publish(display_trajectory); // Set the state in the planning scene to the final state of the last plan - robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); - planning_scene->setCurrentState(*robot_state.get()); + // robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); + // planning_scene->setCurrentState(*robot_state.get()); // Display the goal state - visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); - visual_tools.publishAxisLabeled(pose.pose, "goal_1"); - visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); + // visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); + // visual_tools.publishAxisLabeled(pose.pose, "goal_1"); + // visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE); + // visual_tools.trigger(); } From 0375247aca6d28a6db3054976dce823074da86e7 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Mon, 22 Jul 2019 20:25:57 -0600 Subject: [PATCH 557/612] addressed the comments fixed start state corrected the yaml file multiple joint constraints --- .gitignore | 4 + .../trajopt/config/panda_ompl_planning.yaml | 203 -------- moveit_planners/trajopt/config/setup.yaml | 60 ++- .../trajopt/include/kinematic_terms.h | 62 ++- .../trajopt/include/problem_description.h | 199 ++++--- .../trajopt/include/trajopt_interface.h | 53 +- .../include/trajopt_planning_context.h | 20 +- .../trajopt/launch/gdb_settings.gdb | 8 - .../trajopt/launch/test_trajopt_launch.launch | 1 - moveit_planners/trajopt/src/test_trajopt.cpp | 489 ++++++++++-------- 10 files changed, 526 insertions(+), 573 deletions(-) delete mode 100644 moveit_planners/trajopt/config/panda_ompl_planning.yaml delete mode 100644 moveit_planners/trajopt/launch/gdb_settings.gdb diff --git a/.gitignore b/.gitignore index 808fe85341..cfc7f50ba4 100644 --- a/.gitignore +++ b/.gitignore @@ -58,3 +58,7 @@ qtcreator-* #gdb *.gdb +<<<<<<< HEAD +======= +/moveit_planners/trajopt/launch/gdb_settings.gdb +>>>>>>> addressed the comments diff --git a/moveit_planners/trajopt/config/panda_ompl_planning.yaml b/moveit_planners/trajopt/config/panda_ompl_planning.yaml deleted file mode 100644 index 491e82914e..0000000000 --- a/moveit_planners/trajopt/config/panda_ompl_planning.yaml +++ /dev/null @@ -1,203 +0,0 @@ -planner_configs: - SBLkConfigDefault: - type: geometric::SBL - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ESTkConfigDefault: - type: geometric::EST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - LBKPIECEkConfigDefault: - type: geometric::LBKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - BKPIECEkConfigDefault: - type: geometric::BKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - KPIECEkConfigDefault: - type: geometric::KPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - RRTkConfigDefault: - type: geometric::RRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - RRTConnectkConfigDefault: - type: geometric::RRTConnect - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstarkConfigDefault: - type: geometric::RRTstar - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 - TRRTkConfigDefault: - type: geometric::TRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - max_states_failed: 10 # when to start increasing temp. default: 10 - temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 - min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 - init_temperature: 10e-6 # initial temperature. default: 10e-6 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 - k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() - PRMkConfigDefault: - type: geometric::PRM - max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstarkConfigDefault: - type: geometric::PRMstar - FMTkConfigDefault: - type: geometric::FMT - num_samples: 1000 # number of states that the planner should sample. default: 1000 - radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 - nearest_k: 1 # use Knearest strategy. default: 1 - cache_cc: 1 # use collision checking cache. default: 1 - heuristics: 0 # activate cost to go heuristics. default: 0 - extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 - BFMTkConfigDefault: - type: geometric::BFMT - num_samples: 1000 # number of states that the planner should sample. default: 1000 - radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 - nearest_k: 1 # use the Knearest strategy. default: 1 - balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 - optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 - heuristics: 1 # activates cost to go heuristics. default: 1 - cache_cc: 1 # use the collision checking cache. default: 1 - extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 - PDSTkConfigDefault: - type: geometric::PDST - STRIDEkConfigDefault: - type: geometric::STRIDE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 - degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 - max_degree: 18 # max degree of a node in the GNAT. default: 12 - min_degree: 12 # min degree of a node in the GNAT. default: 12 - max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 - estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 - min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 - BiTRRTkConfigDefault: - type: geometric::BiTRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 - init_temperature: 100 # initial temperature. default: 100 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 - cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf - LBTRRTkConfigDefault: - type: geometric::LBTRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - epsilon: 0.4 # optimality approximation factor. default: 0.4 - BiESTkConfigDefault: - type: geometric::BiEST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ProjESTkConfigDefault: - type: geometric::ProjEST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - LazyPRMkConfigDefault: - type: geometric::LazyPRM - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - LazyPRMstarkConfigDefault: - type: geometric::LazyPRMstar - SPARSkConfigDefault: - type: geometric::SPARS - stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 - sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 - dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 - max_failures: 1000 # maximum consecutive failure limit. default: 1000 - SPARStwokConfigDefault: - type: geometric::SPARStwo - stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 - sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 - dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 - max_failures: 5000 # maximum consecutive failure limit. default: 5000 - TrajOptDefault: - type: geometric::TrajOpt - -panda_arm: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - - TrajOptDefault -panda_arm_hand: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - - TrajOptDefault -hand: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - - TrajOptDefault diff --git a/moveit_planners/trajopt/config/setup.yaml b/moveit_planners/trajopt/config/setup.yaml index d5e7728589..d844f801d5 100644 --- a/moveit_planners/trajopt/config/setup.yaml +++ b/moveit_planners/trajopt/config/setup.yaml @@ -1,14 +1,48 @@ trajopt_param: - improve_ratio_threshold: 0.25; - min_trust_box_size: 1e-4; - min_approx_improve: 1e-4; - min_approx_improve_frac: -1000; # should br -infinity - max_iter: 50; - trust_shrink_ratio: 0.1; - trust_expand_ratio: 1.5; - cnt_tolerance: 1e-4; - max_merit_coeff_increases: 5; - merit_coeff_increase_ratio : 10; - max_time: 1000; # should be +infinity - merit_error_coeff: 10; - trust_box_size: 1e-1; + improve_ratio_threshold: 0.25 # minimum ratio true_improve/approx_improve to accept step + min_trust_box_size: 1e-4 # if trust region gets any smaller, exit and report convergence + min_approx_improve: 1e-4 # if model improves less than this, exit and report convergence + min_approx_improve_frac: -.Inf # if model improves less than this, exit and report convergence + max_iter: 100 # The max number of iterations + trust_shrink_ratio: 0.1 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio + trust_expand_ratio: 1.5 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio + cnt_tolerance: 1e-4 # after convergence of penalty subproblem, if constraint violation is less than this, we're done + max_merit_coeff_increases: 5 # number of times that we jack up penalty coefficient + merit_coeff_increase_ratio: 10 # ratio that we increate coeff each time + max_time: .inf # not yet implemented + merit_error_coeff: 10 # initial penalty coefficient + trust_box_size: 1e-1 # current size of trust region (component-wise) + +problem_info: + basic_info: + n_steps: 20 # 2 * steps_per_phase + dt_upper_lim: 2.0 # The upper limit of 1/dt values allowed in the optimization + dt_lower_lim: 100.0 # The lower limit of 1/dt values allowed in the optimization + start_fixed: true # if true, start pose is the current pose and the req.start_state is ignored + # if false, the start pose is the one given by req.start_state + use_time: flase # if false, it means the timestep is unit, meaning x1-x0 is the velocity for example + # if true, then cost_infos ro cnt_infos should have TT_USE_TIME for their term_type + convex_solver: 1 # which convex solver to use + # 1 = AUTO_SOLVER, 2 = BPMPD, 3 = OSQP, 4 = QPOASES, 5 = GUROBI + + init_info: + type: 2 # 1 = STATIONARY, 2 = JOINT_INTERPOLATED, 3 = GIVEN_TRAJ + # request.start_state should be provided for JOINT_INTERPOLATED and GIVEN_TRAJ + dt: 0.5 + +joint_pos_term_info: + start_pos: + name: start_pos + first_timestep: 0 # First time step to which the term is applied. + last_timestep: 0 # Last time step to which the term is applied. n_steps - 1 + term_type: 2 # 1 = TT_COST, 2 = TT_CNT, 3 = TT_USE_TIME + middle_pos: + name: middle_pos + first_timestep: 10 # First time step to which the term is applied. + last_timestep: 10 # Last time step to which the term is applied. n_steps - 1 + term_type: 2 # 1 = TT_COST, 2 = TT_CNT, 3 = TT_USE_TIME + goal_pos: + name: goal_pos + first_timestep: 19 # First time step to which the term is applied. + last_timestep: 19 # Last time step to which the term is applied. n_steps - 1 + term_type: 2 # 1 = TT_COST, 2 = TT_CNT, 3 = TT_USE_TIME diff --git a/moveit_planners/trajopt/include/kinematic_terms.h b/moveit_planners/trajopt/include/kinematic_terms.h index 25162f8abe..c61052194a 100644 --- a/moveit_planners/trajopt/include/kinematic_terms.h +++ b/moveit_planners/trajopt/include/kinematic_terms.h @@ -1,14 +1,4 @@ #pragma once -#include -TRAJOPT_IGNORE_WARNINGS_PUSH -#include -TRAJOPT_IGNORE_WARNINGS_POP - -#include -#include -#include -#include -#include #include @@ -19,17 +9,16 @@ using namespace std; namespace trajopt_interface { - Vector3d rotVec(const Matrix3d& m) { - Quaterniond q; q = m; + Quaterniond q; + q = m; return Vector3d(q.x(), q.y(), q.z()); } - VectorXd concat(const VectorXd& a, const VectorXd& b) { - VectorXd out(a.size()+b.size()); + VectorXd out(a.size() + b.size()); out.topRows(a.size()) = a; out.middleRows(a.size(), b.size()) = b; return out; @@ -39,13 +28,12 @@ template vector concat(const vector& a, const vector& b) { vector out; - vector x; + vector x(a.size() + b.size()); out.insert(out.end(), a.begin(), a.end()); out.insert(out.end(), b.begin(), b.end()); return out; } - /** * @brief Used to calculate the error for StaticCartPoseTermInfo * This is converted to a cost or constraint using TrajOptCostFromErrFunc or TrajOptConstraintFromErrFunc @@ -54,16 +42,12 @@ struct CartPoseErrCalculator : public sco::VectorOfVector { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Isometry3d target_pose_inv_; - // tesseract::BasicKinConstPtr manip_; - // tesseract::BasicEnvConstPtr env_; planning_scene::PlanningSceneConstPtr planning_scene_; std::string link_; Eigen::Isometry3d tcp_; - CartPoseErrCalculator(const Eigen::Isometry3d& pose, - const planning_scene::PlanningSceneConstPtr planning_scene, - std::string link, - Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) + CartPoseErrCalculator(const Eigen::Isometry3d& pose, const planning_scene::PlanningSceneConstPtr planning_scene, + std::string link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) : target_pose_inv_(pose.inverse()), planning_scene_(planning_scene), link_(link), tcp_(tcp) { } @@ -71,10 +55,34 @@ struct CartPoseErrCalculator : public sco::VectorOfVector Eigen::VectorXd operator()(const Eigen::VectorXd& dof_vals) const override; }; - //TODO(omid): The following should be added and adjusted from trajopt - // JointPosEqCost - // JointPosIneqCost - // JointPosEqConstraint - // JointPosIneqConstraint +// TODO(omid): The following should be added and adjusted from trajopt +// JointPosEqCost +// JointPosIneqCost +// JointPosEqConstraint +// JointPosIneqConstraint + +struct JointVelErrCalculator : sco::VectorOfVector +{ + /** @brief Velocity target */ + double target_; + /** @brief Upper tolerance */ + double upper_tol_; + /** @brief Lower tolerance */ + double lower_tol_; + JointVelErrCalculator() : target_(0.0), upper_tol_(0.0), lower_tol_(0.0) + { + } + JointVelErrCalculator(double target, double upper_tol, double lower_tol) + : target_(target), upper_tol_(upper_tol), lower_tol_(lower_tol) + { + } + + Eigen::VectorXd operator()(const Eigen::VectorXd& var_vals) const; +}; + +struct JointVelJacobianCalculator : sco::MatrixOfVector +{ + Eigen::MatrixXd operator()(const Eigen::VectorXd& var_vals) const; +}; } // namespace trajopt diff --git a/moveit_planners/trajopt/include/problem_description.h b/moveit_planners/trajopt/include/problem_description.h index 77f4adbcc2..8d1bd605e1 100644 --- a/moveit_planners/trajopt/include/problem_description.h +++ b/moveit_planners/trajopt/include/problem_description.h @@ -1,16 +1,15 @@ #pragma once -#include -#include #include #include #include -#include +#include #include +#include + namespace trajopt_interface { - /** @brief Used to apply cost/constraint to joint-space velocity @@ -38,17 +37,20 @@ Note: Velocity is calculated numerically using forward finite difference where j indexes over DOF, and \f$c_j\f$ are the coeffs. */ - struct TermInfo; -typedef std::shared_ptr TermInfoPtr; +MOVEIT_CLASS_FORWARD(TermInfo); class TrajOptProblem; -typedef std::shared_ptr TrajOptProblemPtr; +MOVEIT_CLASS_FORWARD(TrajOptProblem); -struct ProblemInfo; +struct JointPoseTermInfo; +MOVEIT_CLASS_FORWARD(JointPoseTermInfo); -TrajOptProblemPtr ConstructProblem(const ProblemInfo&); +struct CartPoseTermInfo; +MOVEIT_CLASS_FORWARD(CartPoseTermInfo); +struct ProblemInfo; +TrajOptProblemPtr ConstructProblem(const ProblemInfo&); enum TermType { @@ -57,23 +59,21 @@ enum TermType TT_USE_TIME = 0x4, // 0000 0100 }; -#define DEFINE_CREATE(classname) \ - static TermInfoPtr create() \ - { \ - TermInfoPtr out(new classname()); \ - return out; \ - } - - struct BasicInfo { - /** @brief If true first time step is fixed with a joint level constraint*/ + /** @brief If true, first time step is fixed with a joint level constraint + If this is false, the starting point of the trajectory will + not be the current position of the robot. The use case example is: if we are trying to execute a process like + sanding the critical part which is the actual process path not how we get to the start of the process path. So we + plan the + process path first leaving the start free to hopefully get the most optimal and then we plan from the current + location with + start fixed to the start of the process path. It depends on what we want the default behavior to be + */ bool start_fixed; /** @brief Number of time steps (rows) in the optimization matrix */ int n_steps; - std::string manip; - std::string robot; // optional - sco::IntVec dofs_fixed; // optional + sco::IntVec dofs_fixed; // optional sco::ModelType convex_solver; // which convex solver to use /** @brief If true, the last column in the optimization matrix will be 1/dt */ @@ -89,10 +89,10 @@ Initialization info read from json */ struct InitInfo { - /** @brief Methods of initializing the optimization matrix + /** @brief Methods of initializing the optimization matrix. This defines how robot moves from the current + state to the start state - STATIONARY: Initializes all joint values to the initial value (the current value in the env - pci.env->getCurrentJointValues) + STATIONARY: Initializes all joint values to the initial value (the current value in the env) JOINT_INTERPOLATED: Linearly interpolates between initial value and the joint position specified in InitInfo.data GIVEN_TRAJ: Initializes the matrix to a given trajectory @@ -106,8 +106,12 @@ struct InitInfo }; /** @brief Specifies the type of initialization to use */ Type type; - /** @brief Data used during initialization. Use depends on the initialization selected. */ - trajopt::TrajArray data; + /** @brief Data used during initialization. Use depends on the initialization selected. This data will be used + to create initialization matrix. We need to give the goal information to this init info + */ + //trajopt::TrajArray data; This data type does not seem correct, it should be of type VectorXd + Eigen::VectorXd end_pos; + trajopt::TrajArray start_to_end_trajectory; /** @brief Default value the final column of the optimization is initialized too if time is being used */ double dt = 1.0; }; @@ -121,7 +125,10 @@ struct TermInfo { std::string name; int term_type; - int getSupportedTypes() { return supported_term_types_; } + int getSupportedTypes() + { + return supported_term_types_; + } // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; virtual void hatch(TrajOptProblem& prob) = 0; @@ -137,13 +144,15 @@ struct TermInfo virtual ~TermInfo() = default; protected: - TermInfo(int supported_term_types) : supported_term_types_(supported_term_types) {} + TermInfo(int supported_term_types) : supported_term_types_(supported_term_types) + { + } + private: static std::map name2maker; int supported_term_types_; }; - struct ProblemInfo { public: @@ -153,66 +162,91 @@ struct ProblemInfo std::vector cnt_infos; InitInfo init_info; - // tesseract::BasicEnvConstPtr env; - // tesseract::BasicKinConstPtr kin; planning_scene::PlanningSceneConstPtr planning_scene; std::string planning_group_name; - ProblemInfo(planning_scene::PlanningSceneConstPtr ps, std::string pg) - : planning_scene(ps), planning_group_name(pg) {} - // void fromJson(const Json::Value& v); + ProblemInfo(planning_scene::PlanningSceneConstPtr ps, std::string pg) : planning_scene(ps), planning_group_name(pg) + { + } private: - // void readBasicInfo(const Json::Value& v); - // void readOptInfo(const Json::Value& v); - // void readCosts(const Json::Value& v); - // void readConstraints(const Json::Value& v); - // void readInitInfo(const Json::Value& v); -}; +}; /** * Holds all the data for a trajectory optimization problem * so you can modify it programmatically, e.g. add your own costs */ -class TrajOptProblem : public sco::OptProb +class TrajOptProblem : public sco::OptProb { public: TrajOptProblem(); TrajOptProblem(const ProblemInfo& problem_info); virtual ~TrajOptProblem() = default; - sco::VarVector GetVarRow(int i, int start_col, int num_col) { return m_traj_vars.rblock(i, start_col, num_col); } - sco::VarVector GetVarRow(int i) { return m_traj_vars.row(i); } - sco::Var& GetVar(int i, int j) { return m_traj_vars.at(i, j); } - trajopt::VarArray& GetVars() { return m_traj_vars; } + /** @brief Returns the values of the specified joints (start_col to num_col) for the specified timestep i.*/ + sco::VarVector GetVarRow(int i, int start_col, int num_col) + { + return m_traj_vars.rblock(i, start_col, num_col); + } + /** @brief Returns the values of all joints for the specified timestep i.*/ + sco::VarVector GetVarRow(int i) + { + return m_traj_vars.row(i); + } + /** @brief Returns the value of the specified joint j for the specified timestep i.*/ + sco::Var& GetVar(int i, int j) + { + return m_traj_vars.at(i, j); + } + trajopt::VarArray& GetVars() + { + return m_traj_vars; + } /** @brief Returns the number of steps in the problem. This is the number of rows in the optimization matrix.*/ - int GetNumSteps() { return m_traj_vars.rows(); } + int GetNumSteps() + { + return m_traj_vars.rows(); + } /** @brief Returns the problem DOF. This is the number of columns in the optization matrix. * Note that this is not necessarily the same as the kinematic DOF.*/ - int GetNumDOF() { return m_traj_vars.cols(); } - // tesseract::BasicKinConstPtr GetKin() { return m_kin; } - //tesseract::BasicEnvConstPtr GetEnv() { return m_env; } - planning_scene::PlanningSceneConstPtr GetPlanningScene() { return planning_scene_;} - void SetInitTraj(const trajopt::TrajArray& x){ m_init_traj = x; } - trajopt::TrajArray GetInitTraj() { return m_init_traj; } + int GetNumDOF() + { + return m_traj_vars.cols(); + } + planning_scene::PlanningSceneConstPtr GetPlanningScene() + { + return planning_scene_; + } + void SetInitTraj(const trajopt::TrajArray& x) + { + m_init_traj = x; + } + trajopt::TrajArray GetInitTraj() + { + return m_init_traj; + } // friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&); /** @brief Returns TrajOptProb.has_time */ - bool GetHasTime() { return has_time; } + bool GetHasTime() + { + return has_time; + } /** @brief Sets TrajOptProb.has_time */ - void SetHasTime(bool tmp) { has_time = tmp; } + void SetHasTime(bool tmp) + { + has_time = tmp; + } private: /** @brief If true, the last column in the optimization matrix will be 1/dt */ bool has_time; + /** @brief is the matrix holding the joint values of the trajectory for all timesteps */ trajopt::VarArray m_traj_vars; - // tesseract::BasicKinConstPtr m_kin; - // tesseract::BasicEnvConstPtr m_env; planning_scene::PlanningSceneConstPtr planning_scene_; std::string planning_group_; trajopt::TrajArray m_init_traj; }; - /** @brief This term is used when the goal frame is fixed in cartesian space Set term_type == TT_COST or TT_CNT for cost or constraint. @@ -240,9 +274,13 @@ struct CartPoseTermInfo : public TermInfo // void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ void hatch(TrajOptProblem& prob) override; - DEFINE_CREATE(CartPoseTermInfo) -}; + static TermInfoPtr create() + { + TermInfoPtr out(new CartPoseTermInfo()); + return out; + } +}; /** \brief Joint space position cost @@ -254,7 +292,7 @@ struct CartPoseTermInfo : public TermInfo \f} where \f$i\f$ indexes over dof and \f$c_i\f$ are coeffs */ -struct JointPosTermInfo : public TermInfo +struct JointPoseTermInfo : public TermInfo { /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ trajopt::DblVec coeffs; @@ -270,15 +308,50 @@ struct JointPosTermInfo : public TermInfo int last_step = -1; /** @brief Initialize term with it's supported types */ - JointPosTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) {} + JointPoseTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) + { + } /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ void hatch(TrajOptProblem& prob) override; - DEFINE_CREATE(JointPosTermInfo) + + static TermInfoPtr create() + { + TermInfoPtr out(new JointPoseTermInfo()); + return out; + } }; +struct JointVelTermInfo : public TermInfo +{ + /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec coeffs; + /** @brief Vector of velocity targets. This is a required value. Size should be the DOF of the system */ + trajopt::DblVec targets; + /** @brief Vector of velocity upper limits. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec upper_tols; + /** @brief Vector of velocity lower limits. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec lower_tols; + /** @brief First time step to which the term is applied. Default: 0*/ + int first_step = 0; + /** @brief Last time step to which the term is applied. Default: prob.GetNumSteps() - 1*/ + int last_step = -1; + + /** @brief Initialize term with it's supported types */ + JointVelTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) + { + } + /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ + void hatch(TrajOptProblem& prob) override; + + static TermInfoPtr create() + { + TermInfoPtr out(new JointVelTermInfo()); + return out; + } +}; -trajopt::TrajArray generateInitialTrajectory(const int& num_steps, const std::vector& joint_vals); + void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, trajopt::TrajArray& init_traj); } // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface.h index b4dd24c9b2..deabc02168 100644 --- a/moveit_planners/trajopt/include/trajopt_interface.h +++ b/moveit_planners/trajopt/include/trajopt_interface.h @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2019, PickNik, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +14,7 @@ * 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 + * * Neither the name of PickNik nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -39,65 +39,44 @@ #include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include - -#include #include #include "problem_description.h" -#include -#include #include namespace trajopt_interface { MOVEIT_CLASS_FORWARD(TrajOptInterface); -class TrajOptInterface //: public trajopt::TrajOptPlanner +class TrajOptInterface { public: + TrajOptInterface(const ros::NodeHandle& nh = ros::NodeHandle("~")); - TrajOptInterface(const ros::NodeHandle& nh = ros::NodeHandle("~")); - - const sco::BasicTrustRegionSQPParameters& getParams() const { return params_; } - - bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, - const sco::BasicTrustRegionSQPParameters& params, moveit_msgs::MotionPlanDetailedResponse& res); - + const sco::BasicTrustRegionSQPParameters& getParams() const + { + return params_; + } + bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::MotionPlanDetailedResponse& res); protected: /** @brief Configure everything using the param server */ - void setParamsFromRosParamServer(); - void setDefaultParams(); + void setTrajOptParams(sco::BasicTrustRegionSQPParameters& param); + void setDefaultTrajOPtParams(); + void setProblemInfoParam(ProblemInfo& problem_info); + void setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name); ros::NodeHandle nh_; /// The ROS node handle - sco::BasicTrustRegionSQPParameters params_; - - sco::Optimizer::Callback callbacks_; - + std::vector callbacks_; TrajOptProblemPtr prob_; - - // a function to convert TrajArray (TrajArray has no dependency on tesseract) from trajopt_ros to trajectory_msgs::JointTrajectory.Points - trajectory_msgs::JointTrajectory convertTrajArrayToJointTrajectory(const trajopt::TrajArray& traj_array, const std::vector& j_names); }; void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res); - } #endif diff --git a/moveit_planners/trajopt/include/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_planning_context.h index 92d6642aa6..3a6497bcf8 100644 --- a/moveit_planners/trajopt/include/trajopt_planning_context.h +++ b/moveit_planners/trajopt/include/trajopt_planning_context.h @@ -1,30 +1,14 @@ #ifndef TrajOpt_PLANNING_CONTEXT_H #define TrajOpt_PLANNING_CONTEXT_H -#include -#include #include -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include #include "problem_description.h" #include "trajopt_interface.h" - namespace trajopt_interface { - MOVEIT_CLASS_FORWARD(TrajOptPlanningContext); class TrajOptPlanningContext : public planning_interface::PlanningContext @@ -43,13 +27,11 @@ class TrajOptPlanningContext : public planning_interface::PlanningContext void clear() override; protected: - private: - moveit::core::RobotModelConstPtr robot_model_; TrajOptInterfacePtr trajopt_interface_; }; -} // namespace trajopt_interface +} // namespace trajopt_interface #endif // TrajOpt_PLANNING_CONTEXT_H diff --git a/moveit_planners/trajopt/launch/gdb_settings.gdb b/moveit_planners/trajopt/launch/gdb_settings.gdb deleted file mode 100644 index d8ac14fee4..0000000000 --- a/moveit_planners/trajopt/launch/gdb_settings.gdb +++ /dev/null @@ -1,8 +0,0 @@ -## This file allows developers to set breakpoints in the CPP_EXECUTABLE_NAME.launch file -## To set a breakpoint, compile with debug flag set to true. Add a line below with the source file name, -## a colon, then the line number to break on. Then launch CPP_EXECUTABLE_NAME with argument debug:=true - -set breakpoint pending on - -## Example break point: -# break CPP_EXECUTABLE_NAME.cpp:52 \ No newline at end of file diff --git a/moveit_planners/trajopt/launch/test_trajopt_launch.launch b/moveit_planners/trajopt/launch/test_trajopt_launch.launch index c80e5c93e1..f0d35bdeaa 100644 --- a/moveit_planners/trajopt/launch/test_trajopt_launch.launch +++ b/moveit_planners/trajopt/launch/test_trajopt_launch.launch @@ -28,7 +28,6 @@ - diff --git a/moveit_planners/trajopt/src/test_trajopt.cpp b/moveit_planners/trajopt/src/test_trajopt.cpp index 28c2c27033..5b3d48a1c2 100644 --- a/moveit_planners/trajopt/src/test_trajopt.cpp +++ b/moveit_planners/trajopt/src/test_trajopt.cpp @@ -2,8 +2,13 @@ #include "trajopt_planning_context.h" #include +#include +#include + #include #include +#include + #include #include #include @@ -14,224 +19,304 @@ #include #include -// test +#include +#include +#include +#include + + +// This file is a test for using trajopt in MoveIt. The goal is to make different types of constraints in +// MotionPlanRequest +// and visualize the result using trajopt planner. +// Three cases: +// 1- joint constraint with start_fixed true, meaining current state of the robot is the initial state of trajectory. +// 2- joint constraint with start_fixed false, meainig more than one goal should be given. The robot jumps to the first +// goal +// and motion planning is done between first goal and second goal. +// 3- Cartesian constraint. int main(int argc, char** argv) { - const std::string node_name = "trajopot_planning_tutorial"; - ros::init(argc, argv, node_name); - ros::AsyncSpinner spinner(1); - spinner.start(); - ros::NodeHandle node_handle("~"); - - const std::string PLANNING_GROUP = "panda_arm"; - robot_model_loader::RobotModelLoaderPtr robot_model_loader(new robot_model_loader::RobotModelLoader("robot_description")); - robot_model::RobotModelPtr robot_model = robot_model_loader->getModel(); - /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/ - robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model)); - const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); - robot_state->setToDefaultValues(); - - const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); - planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); - - // With the planning scene we create a planing scene monitor that - // monitors planning scene diffs and applys them to the planning scene - planning_scene_monitor::PlanningSceneMonitorPtr psm( - new planning_scene_monitor::PlanningSceneMonitor(planning_scene, robot_model_loader)); - psm->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); - psm->startStateMonitor(); - psm->startSceneMonitor(); - - while (!psm->getStateMonitor()->haveCompleteState() && ros::ok()) - { - ROS_INFO_STREAM_THROTTLE_NAMED(1, node_name, "Waiting for complete state from topic "); - } - // We will now construct a loader to load a planner, by name. - // Note that we are using the ROS pluginlib library here. - boost::scoped_ptr> planner_plugin_loader; - planning_interface::PlannerManagerPtr planner_instance; - - std::string planner_plugin_name; - - planner_plugin_name = "trajopt_interface/TrajOptPlanner"; - - // planner_plugin_name = "ompl_interface/OMPLPlanner"; - // node_handle.getParam("...", ...); - - node_handle.setParam("planning_plugin", planner_plugin_name); - - // making sure to catch all exceptions. - if (!node_handle.getParam("planning_plugin", planner_plugin_name)) - ROS_FATAL_STREAM("Could not find planner plugin name"); - try - { - planner_plugin_loader.reset(new pluginlib::ClassLoader( - "moveit_core", "planning_interface::PlannerManager")); - printf("===>>> planner_plugin_name: %s \n", planner_plugin_name.c_str()); - } - catch (pluginlib::PluginlibException& ex) - { - ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); - } - try - { - planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); - if (!planner_instance->initialize(robot_model, node_handle.getNamespace())) // namespace ????. I dont use it - ROS_FATAL_STREAM("Could not initialize planner instance"); - ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'"); - } - catch (pluginlib::PluginlibException& ex) - { - const std::vector& classes = planner_plugin_loader->getDeclaredClasses(); - std::stringstream ss; - for (std::size_t i = 0; i < classes.size(); ++i) - ss << classes[i] << " "; - ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl - << "Available plugins: " << ss.str()); - } - - // Visualization - // ======================================================================================== - // The package MoveItVisualTools provides many capabilties for visualizing objects, robots, - // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script - namespace rvt = rviz_visual_tools; - moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, psm); - visual_tools.loadRobotStatePub("/display_robot_state"); - visual_tools.enableBatchPublishing(); - visual_tools.deleteAllMarkers(); // clear all old markers - visual_tools.trigger(); - - /* Remote control is an introspection tool that allows users to step through a high level script - via buttons and keyboard shortcuts in RViz */ - visual_tools.loadRemoteControl(); - - /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/ - Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); - text_pose.translation().z() = 1.75; - visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE); - - /* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */ - visual_tools.trigger(); - - /* We can also use visual_tools to wait for user input */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); - - // Pose Goal - // ======================================================================================== - // We will now create a motion plan request for the arm of the Panda - // specifying the desired pose of the end-effector as input. - visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); - visual_tools.trigger(); - planning_interface::MotionPlanRequest req; - planning_interface::MotionPlanResponse res; - geometry_msgs::PoseStamped pose; - pose.header.frame_id = "panda_link0"; - pose.pose.position.x = 0.3; - pose.pose.position.y = 0.4; - pose.pose.position.z = 0.75; - pose.pose.orientation.w = 1.0; - - // A tolerance of 0.01 m is specified in position - // and 0.01 radians in orientation - std::vector tolerance_pose(3, 0.01); - std::vector tolerance_angle(3, 0.01); - - // We will create the request as a constraint using a helper function available - // from the - // `kinematic_constraints`_ - // package. - // - // .. _kinematic_constraints: - // http://docs.ros.org/indigo/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132 - moveit_msgs::Constraints pose_goal = - kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); - - // req.group_name = PLANNING_GROUP; - // req.goal_constraints.push_back(pose_goal); - - // set the request start joint state - // ======================================================================================== - // get the joint values of the start state - std::vector start_joint_values; - robot_state->copyJointGroupPositions(joint_model_group, start_joint_values); - int r = 0; - for (auto x : start_joint_values) - { - printf("===>>> joint: %s with value: %f \n", joint_names[r].c_str(), x); - ++r; - } + const std::string node_name = "trajopot_planning_tutorial"; + ros::init(argc, argv, node_name); + ros::AsyncSpinner spinner(1); + spinner.start(); + ros::NodeHandle node_handle("~"); + + const std::string PLANNING_GROUP = "panda_arm"; + robot_model_loader::RobotModelLoaderPtr robot_model_loader( + new robot_model_loader::RobotModelLoader("robot_description")); + robot_model::RobotModelPtr robot_model = robot_model_loader->getModel(); + + /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/ + robot_state::RobotStatePtr current_state(new robot_state::RobotState(robot_model)); + current_state->setToDefaultValues(); + + const robot_state::JointModelGroup* joint_model_group = current_state->getJointModelGroup(PLANNING_GROUP); + const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); + planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); + + // With the planning scene we create a planing scene monitor that + // monitors planning scene diffs and applys them to the planning scene + planning_scene_monitor::PlanningSceneMonitorPtr psm( + new planning_scene_monitor::PlanningSceneMonitor(planning_scene, robot_model_loader)); + psm->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); + psm->startStateMonitor(); + psm->startSceneMonitor(); + + // set the start joint state + // ======================================================================================== + // panda_arm joint limits: + // -2.8973 2.8973 + // -1.7628 1.7628 + // -2.8973 2.8973 + // -3.0718 -0.0698 + // -2.8973 2.8973 + // -0.0175 3.7525 + // -2.8973 2.8973 + + std::vector start_joint_values = {0.4, 0.3, 0.5, -0.55, 0.88, 1.0, -0.075 }; + robot_state::RobotStatePtr start_state(new robot_state::RobotState(robot_model)); + start_state->setJointGroupPositions(joint_model_group, start_joint_values); + start_state->update(); + + // planning_scene->setCurrentState(*robot_state); + + // get the joint values of the start state + // std::vector tmp_joint_values; + // robot_state->copyJointGroupPositions(joint_model_group, tmp_joint_values); + // int r = 0; + // for (auto x : tmp_joint_values) + // { + // printf("===>>> joint: %s with start value: %f \n", joint_names[r].c_str(), x); + // ++r; + // } + + // printf("--- get the joint values from planning scene current state"); + // robot_state::RobotState current_state = planning_scene->getCurrentState(); + // std::vector tmp_j_values; + // current_state.copyJointGroupPositions(joint_model_group, tmp_j_values); + // r = 0; + // for (auto x : tmp_j_values) + // { + // printf("===>>> joint: %s with start value: %f \n", joint_names[r].c_str(), x); + // ++r; + // } + + + + // ======================================================================================== + + while (!psm->getStateMonitor()->haveCompleteState() && ros::ok()) + { + ROS_INFO_STREAM_THROTTLE_NAMED(1, node_name, "Waiting for complete state from topic "); + } + // We will now construct a loader to load a planner, by name. + // Note that we are using the ROS pluginlib library here. + boost::scoped_ptr> planner_plugin_loader; + planning_interface::PlannerManagerPtr planner_instance; + + std::string planner_plugin_name; + + planner_plugin_name = "trajopt_interface/TrajOptPlanner"; + node_handle.setParam("planning_plugin", planner_plugin_name); + + // making sure to catch all exceptions. + if (!node_handle.getParam("planning_plugin", planner_plugin_name)) + ROS_FATAL_STREAM("Could not find planner plugin name"); + try + { + planner_plugin_loader.reset(new pluginlib::ClassLoader( + "moveit_core", "planning_interface::PlannerManager")); + printf("===>>> planner_plugin_name: %s \n", planner_plugin_name.c_str()); + } + catch (pluginlib::PluginlibException& ex) + { + ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); + } + try + { + planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); + if (!planner_instance->initialize(robot_model, node_handle.getNamespace())) // namespace ????. I dont use it + ROS_FATAL_STREAM("Could not initialize planner instance"); + ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'"); + } + catch (pluginlib::PluginlibException& ex) + { + const std::vector& classes = planner_plugin_loader->getDeclaredClasses(); + std::stringstream ss; + for (std::size_t i = 0; i < classes.size(); ++i) + ss << classes[i] << " "; + ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl + << "Available plugins: " << ss.str()); + } + + // Visualization + // ======================================================================================== + // The package MoveItVisualTools provides many capabilties for visualizing objects, robots, + // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script + namespace rvt = rviz_visual_tools; + moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, psm); + visual_tools.loadRobotStatePub("/display_robot_state"); + visual_tools.enableBatchPublishing(); + visual_tools.deleteAllMarkers(); // clear all old markers + visual_tools.trigger(); + + /* Remote control is an introspection tool that allows users to step through a high level script + via buttons and keyboard shortcuts in RViz */ + visual_tools.loadRemoteControl(); + + /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/ + Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); + text_pose.translation().z() = 1.75; + visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE); + + /* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */ + visual_tools.trigger(); + + /* We can also use visual_tools to wait for user input */ + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + + // res req + planning_interface::MotionPlanRequest req; + planning_interface::MotionPlanResponse res; + + req.start_state.joint_state.name = joint_names; + req.start_state.joint_state.position = start_joint_values; + + // set the goal joint state and joints tolerance + // ======================================================================================== + robot_state::RobotState goal_state(robot_model); + std::vector goal_joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1 }; + goal_state.setJointGroupPositions(joint_model_group, goal_joint_values); + moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); + req.goal_constraints.clear(); + req.goal_constraints.push_back(joint_goal); + req.group_name = PLANNING_GROUP; + req.goal_constraints[0].name = "goal_pos"; + + // set joint tolerance + std::vector goal_joint_constraint = req.goal_constraints[0].joint_constraints; + for (int x = 0; x < goal_joint_constraint.size(); ++x) + { + std::cout << "==>> joint position at goal " << goal_joint_constraint[x].position << std::endl; + req.goal_constraints[0].joint_constraints[x].tolerance_above = 0.1; + req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.1; + } + + robot_state::RobotState middle_state(robot_model); + std::vector middle_joint_values = {0.5, 0.4, 0.65, -0.75, 1.05, 1.25, -0.15 }; + middle_state.setJointGroupPositions(joint_model_group, middle_joint_values); + middle_state.update(); + moveit_msgs::Constraints joint_middle = kinematic_constraints::constructGoalConstraints(middle_state, joint_model_group); + req.goal_constraints.push_back(joint_middle); + req.goal_constraints[1].name = "middle_pos"; + std::cout << "hereeeeeeeeeeeeeeeeeeeeeeee 1 " << req.goal_constraints[1].joint_constraints[0].position << std::endl; + std::vector middle_joint_constraint = req.goal_constraints[1].joint_constraints; + std::cout << "hereeeeeeeeeeeeeeeeeeeeeeee 2" << std::endl; + for (int x = 0; x < middle_joint_constraint.size(); ++x) + { + std::cout << "==>> joint position at middle " << middle_joint_constraint[x].position << std::endl; + req.goal_constraints[1].joint_constraints[x].tolerance_above = 0.1; + req.goal_constraints[1].joint_constraints[x].tolerance_below = 0.1; + } + + std::cout << "===>>> number of constraints in goal: " << req.goal_constraints.size() << std::endl; + // planning context + // ======================================================================================== + planning_interface::PlanningContextPtr context = + planner_instance->getPlanningContext(planning_scene, req, res.error_code_); - req.start_state.joint_state.name = joint_names; - req.start_state.joint_state.position = start_joint_values; - - // set the goal joint state - // ======================================================================================== - robot_state::RobotState goal_state(robot_model); - std::vector joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1}; - goal_state.setJointGroupPositions(joint_model_group, joint_values); - moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); - req.goal_constraints.clear(); - req.goal_constraints.push_back(joint_goal); - req.group_name = PLANNING_GROUP; - - // retrive joint values at goal - std::vector goal_constraints = req.goal_constraints; - std::cout << "===>>> number of constraints in goal: " << goal_constraints.size() << std::endl; - std::vector goal_joint_constraint = goal_constraints[0].joint_constraints; - for (auto x : goal_joint_constraint) - { - std::cout << "==>> joint position at goal " << x.position << std::endl; - } + context->solve(res); + if (res.error_code_.val != res.error_code_.SUCCESS) + { + ROS_ERROR("Could not compute plan successfully"); + return 0; + } - // planning context - planning_interface::PlanningContextPtr context = - planner_instance->getPlanningContext(planning_scene, req, res.error_code_); + visual_tools.prompt("Press 'next' to visualize the result"); + + // Visualize the result + // ======================================================================================== + ros::Publisher display_publisher = + node_handle.advertise("/move_group/display_planned_path", 1, true); + moveit_msgs::DisplayTrajectory display_trajectory; + + /* Visualize the trajectory */ + moveit_msgs::MotionPlanResponse response; + res.getMessage(response); - context->solve(res); - if (res.error_code_.val != res.error_code_.SUCCESS) + for (int timestep_index = 0; timestep_index < response.trajectory.joint_trajectory.points.size(); ++timestep_index) + { + for (int joint_index = 0; + joint_index < response.trajectory.joint_trajectory.points[timestep_index].positions.size(); ++joint_index) { - ROS_ERROR("Could not compute plan successfully"); - return 0; + std::cout << response.trajectory.joint_trajectory.points[timestep_index].positions[joint_index] << " "; } + std::cout << std::endl; + } - visual_tools.prompt("Press 'next' to visualize the result"); + display_trajectory.trajectory_start = response.trajectory_start; + display_trajectory.trajectory.push_back(response.trajectory); - // Visualize the result - // ======================================================================================== - ros::Publisher display_publisher = - node_handle.advertise("/move_group/display_planned_path", 1, true); - moveit_msgs::DisplayTrajectory display_trajectory; + visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); + visual_tools.trigger(); + display_publisher.publish(display_trajectory); + + + std::cout << "===>>> group name:" << response.group_name << std::endl; + std::cout << "===>>> traj start joint name size: " << response.trajectory_start.joint_state.name.size() << std::endl; + std::cout << "===>>> traj start joint position size: " << response.trajectory_start.joint_state.position.size() + << std::endl; + + std::cout << "===>>> traj joint names size: " << response.trajectory.joint_trajectory.joint_names.size() << std::endl; + + for (int jn = 0; jn < response.trajectory.joint_trajectory.joint_names.size(); ++jn) + { + std::cout << "===>>> joint_" << jn << ": " << response.trajectory.joint_trajectory.joint_names[jn] << std::endl; + } + + + const std::vector& str = joint_model_group->getLinkModelNames(); + printf("end effector name %s\n", str.back().c_str()); + + const Eigen::Affine3d& end_effector_transform_current = current_state->getGlobalLinkTransform(str.back()); + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.stamp = ros::Time::now(); + pose_msg.header.frame_id = robot_model->getModelFrame(); + pose_msg.pose = tf2::toMsg(end_effector_transform_current); + + visual_tools.publishAxisLabeled(pose_msg.pose, "current"); + visual_tools.publishText(text_pose, "current pose", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); - /* Visualize the trajectory */ - moveit_msgs::MotionPlanResponse response; - res.getMessage(response); + const Eigen::Affine3d& end_effector_transform_start = start_state->getGlobalLinkTransform(str.back()); + pose_msg.header.stamp = ros::Time::now(); + pose_msg.header.frame_id = robot_model->getModelFrame(); + pose_msg.pose = tf2::toMsg(end_effector_transform_start); - for (int timestep_index = 0; timestep_index < response.trajectory.joint_trajectory.points.size(); ++timestep_index) - { - for (int joint_index = 0; joint_index < response.trajectory.joint_trajectory.points[timestep_index].positions.size(); ++joint_index) - { - std::cout << response.trajectory.joint_trajectory.points[timestep_index].positions[joint_index] << " "; - } - std::cout << std::endl; - } + visual_tools.publishAxisLabeled(pose_msg.pose, "start"); + visual_tools.publishText(text_pose, "start pose", rvt::BLUE, rvt::XLARGE); + visual_tools.trigger(); - display_trajectory.trajectory_start = response.trajectory_start; - display_trajectory.trajectory.push_back(response.trajectory); - visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); - visual_tools.trigger(); - display_publisher.publish(display_trajectory); + const Eigen::Affine3d& end_effector_transform_middle = middle_state.getGlobalLinkTransform(str.back()); + pose_msg.header.stamp = ros::Time::now(); + pose_msg.header.frame_id = robot_model->getModelFrame(); + pose_msg.pose = tf2::toMsg(end_effector_transform_middle); - // Set the state in the planning scene to the final state of the last plan - // robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); - // planning_scene->setCurrentState(*robot_state.get()); + visual_tools.publishAxisLabeled(pose_msg.pose, "middle"); + visual_tools.publishText(text_pose, "middle pose", rvt::BLUE, rvt::XLARGE); + visual_tools.trigger(); - // Display the goal state - // visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); - // visual_tools.publishAxisLabeled(pose.pose, "goal_1"); - // visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE); - // visual_tools.trigger(); + const Eigen::Affine3d& end_effector_transform_goal = goal_state.getGlobalLinkTransform(str.back()); + pose_msg.header.stamp = ros::Time::now(); + pose_msg.header.frame_id = robot_model->getModelFrame(); + pose_msg.pose = tf2::toMsg(end_effector_transform_goal); + visual_tools.publishAxisLabeled(pose_msg.pose, "goal"); + visual_tools.publishText(text_pose, "goal pose", rvt::BLUE, rvt::XLARGE); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' to finish demo \n"); } From 1f4f13d854a359c9c8aba30fc4bb8737dadbf9ac Mon Sep 17 00:00:00 2001 From: omid1366 Date: Tue, 30 Jul 2019 14:55:02 -0600 Subject: [PATCH 558/612] addressed some comments leave setup assistant changes for later on another PR cleaned up test_trajopt added unittest --- moveit_planners/trajopt/config/setup.yaml | 42 ++--- .../trajopt_interface/kinematic_terms.h | 39 ++-- .../kinematic_terms.h~HEAD} | 41 +++-- .../trajopt_interface/problem_description.h | 49 +++-- .../problem_description.h~HEAD} | 60 ++++--- .../trajopt_interface/trajopt_interface.h | 18 +- .../trajopt_interface.h~HEAD} | 25 ++- .../trajopt_planning_context.h | 10 +- .../trajopt_planning_context.h~HEAD} | 11 +- moveit_planners/trajopt/src/test_trajopt.cpp | 170 ++++++------------ .../widgets/configuration_files_widget.cpp | 9 - 11 files changed, 207 insertions(+), 267 deletions(-) rename moveit_planners/trajopt/include/{kinematic_terms.h => trajopt_interface/kinematic_terms.h~HEAD} (63%) rename moveit_planners/trajopt/include/{problem_description.h => trajopt_interface/problem_description.h~HEAD} (88%) rename moveit_planners/trajopt/include/{trajopt_interface.h => trajopt_interface/trajopt_interface.h~HEAD} (83%) rename moveit_planners/trajopt/include/{trajopt_planning_context.h => trajopt_interface/trajopt_planning_context.h~HEAD} (80%) diff --git a/moveit_planners/trajopt/config/setup.yaml b/moveit_planners/trajopt/config/setup.yaml index d844f801d5..d3c8518cdd 100644 --- a/moveit_planners/trajopt/config/setup.yaml +++ b/moveit_planners/trajopt/config/setup.yaml @@ -1,24 +1,24 @@ trajopt_param: - improve_ratio_threshold: 0.25 # minimum ratio true_improve/approx_improve to accept step - min_trust_box_size: 1e-4 # if trust region gets any smaller, exit and report convergence + improve_ratio_threshold: 0.25 # minimum ratio true_improve/approx_improve to accept step. [Research Paper] TrueImprove/ModelImprove > c + min_trust_box_size: 1e-4 # if trust region gets any smaller, exit and report convergence. [Research Paper] xtol min_approx_improve: 1e-4 # if model improves less than this, exit and report convergence min_approx_improve_frac: -.Inf # if model improves less than this, exit and report convergence max_iter: 100 # The max number of iterations - trust_shrink_ratio: 0.1 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio - trust_expand_ratio: 1.5 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio - cnt_tolerance: 1e-4 # after convergence of penalty subproblem, if constraint violation is less than this, we're done - max_merit_coeff_increases: 5 # number of times that we jack up penalty coefficient - merit_coeff_increase_ratio: 10 # ratio that we increate coeff each time + trust_shrink_ratio: 0.1 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio. [Research Paper] tao- + trust_expand_ratio: 1.5 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio. [Research Paper] tao+ + cnt_tolerance: 1e-4 # after convergence of penalty subproblem, if constraint violation is less than this, we're done. [Research Paper] ctol + max_merit_coeff_increases: 5 # number of times that we jack up penalty coefficient. [Reasearch Paper] Max iteration in PenaltyIteration loop + merit_coeff_increase_ratio: 10 # ratio that we increate coeff each time. [Researcy Paper] Penalty scaling factory (italic kappa) max_time: .inf # not yet implemented - merit_error_coeff: 10 # initial penalty coefficient - trust_box_size: 1e-1 # current size of trust region (component-wise) + merit_error_coeff: 10 # initial penalty coefficient. [Research Paper] mu_0 + trust_box_size: 1e-1 # current size of trust region (component-wise). [Research Paper] s problem_info: basic_info: n_steps: 20 # 2 * steps_per_phase dt_upper_lim: 2.0 # The upper limit of 1/dt values allowed in the optimization dt_lower_lim: 100.0 # The lower limit of 1/dt values allowed in the optimization - start_fixed: true # if true, start pose is the current pose and the req.start_state is ignored + start_fixed: true # if true, start pose is the current pose at timestep = 0 # if false, the start pose is the one given by req.start_state use_time: flase # if false, it means the timestep is unit, meaning x1-x0 is the velocity for example # if true, then cost_infos ro cnt_infos should have TT_USE_TIME for their term_type @@ -26,23 +26,25 @@ problem_info: # 1 = AUTO_SOLVER, 2 = BPMPD, 3 = OSQP, 4 = QPOASES, 5 = GUROBI init_info: - type: 2 # 1 = STATIONARY, 2 = JOINT_INTERPOLATED, 3 = GIVEN_TRAJ + type: 3 # 1 = STATIONARY, 2 = JOINT_INTERPOLATED, 3 = GIVEN_TRAJ # request.start_state should be provided for JOINT_INTERPOLATED and GIVEN_TRAJ dt: 0.5 joint_pos_term_info: - start_pos: + start_pos: # from req.start_state name: start_pos - first_timestep: 0 # First time step to which the term is applied. - last_timestep: 0 # Last time step to which the term is applied. n_steps - 1 + first_timestep: 10 # First time step to which the term is applied. + last_timestep: 10 # Last time step to which the term is applied. + # if start_fixed is trure then we can not set the first_timestep to 0 for start_pos as it is going + # to be ignored and only the current pose would be the constraint at timestep = 0. term_type: 2 # 1 = TT_COST, 2 = TT_CNT, 3 = TT_USE_TIME middle_pos: name: middle_pos - first_timestep: 10 # First time step to which the term is applied. - last_timestep: 10 # Last time step to which the term is applied. n_steps - 1 - term_type: 2 # 1 = TT_COST, 2 = TT_CNT, 3 = TT_USE_TIME + first_timestep: 15 + last_timestep: 15 + term_type: 2 goal_pos: name: goal_pos - first_timestep: 19 # First time step to which the term is applied. - last_timestep: 19 # Last time step to which the term is applied. n_steps - 1 - term_type: 2 # 1 = TT_COST, 2 = TT_CNT, 3 = TT_USE_TIME + first_timestep: 19 + last_timestep: 19 + term_type: 2 diff --git a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h index 96274cf9c7..51dac66b6a 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h +++ b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h @@ -4,38 +4,33 @@ #include +using namespace Eigen; +using namespace std; + namespace trajopt_interface { -/** - * @brief Extracts the vector part of quaternion - */ -inline Eigen::Vector3d quaternionRotationVector(const Eigen::Matrix3d& matrix) +Vector3d rotVec(const Matrix3d& m) { - Eigen::Quaterniond quaternion; - quaternion = matrix; - return Eigen::Vector3d(quaternion.x(), quaternion.y(), quaternion.z()); + Quaterniond q; + q = m; + return Vector3d(q.x(), q.y(), q.z()); } -/** - * @brief Appends b to a of type VectorXd - */ -inline Eigen::VectorXd concatVector(const Eigen::VectorXd& vector_a, const Eigen::VectorXd& vector_b) +VectorXd concat(const VectorXd& a, const VectorXd& b) { - Eigen::VectorXd out(vector_a.size() + vector_b.size()); - out.topRows(vector_a.size()) = vector_a; - out.middleRows(vector_a.size(), vector_b.size()) = vector_b; + VectorXd out(a.size() + b.size()); + out.topRows(a.size()) = a; + out.middleRows(a.size(), b.size()) = b; return out; } -/** - * @brief Appends b to a of type T - */ template -inline std::vector concatVector(const std::vector& vector_a, const std::vector& vector_b) +vector concat(const vector& a, const vector& b) { - std::vector out; - out.insert(out.end(), vector_a.begin(), vector_a.end()); - out.insert(out.end(), vector_b.begin(), vector_b.end()); + vector out; + vector x(a.size() + b.size()); + out.insert(out.end(), a.begin(), a.end()); + out.insert(out.end(), b.begin(), b.end()); return out; } @@ -52,7 +47,7 @@ struct CartPoseErrCalculator : public sco::VectorOfVector Eigen::Isometry3d tcp_; CartPoseErrCalculator(const Eigen::Isometry3d& pose, const planning_scene::PlanningSceneConstPtr planning_scene, - const std::string& link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) + std::string link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) : target_pose_inv_(pose.inverse()), planning_scene_(planning_scene), link_(link), tcp_(tcp) { } diff --git a/moveit_planners/trajopt/include/kinematic_terms.h b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h~HEAD similarity index 63% rename from moveit_planners/trajopt/include/kinematic_terms.h rename to moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h~HEAD index c61052194a..96274cf9c7 100644 --- a/moveit_planners/trajopt/include/kinematic_terms.h +++ b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h~HEAD @@ -4,33 +4,38 @@ #include -using namespace Eigen; -using namespace std; - namespace trajopt_interface { -Vector3d rotVec(const Matrix3d& m) +/** + * @brief Extracts the vector part of quaternion + */ +inline Eigen::Vector3d quaternionRotationVector(const Eigen::Matrix3d& matrix) { - Quaterniond q; - q = m; - return Vector3d(q.x(), q.y(), q.z()); + Eigen::Quaterniond quaternion; + quaternion = matrix; + return Eigen::Vector3d(quaternion.x(), quaternion.y(), quaternion.z()); } -VectorXd concat(const VectorXd& a, const VectorXd& b) +/** + * @brief Appends b to a of type VectorXd + */ +inline Eigen::VectorXd concatVector(const Eigen::VectorXd& vector_a, const Eigen::VectorXd& vector_b) { - VectorXd out(a.size() + b.size()); - out.topRows(a.size()) = a; - out.middleRows(a.size(), b.size()) = b; + Eigen::VectorXd out(vector_a.size() + vector_b.size()); + out.topRows(vector_a.size()) = vector_a; + out.middleRows(vector_a.size(), vector_b.size()) = vector_b; return out; } +/** + * @brief Appends b to a of type T + */ template -vector concat(const vector& a, const vector& b) +inline std::vector concatVector(const std::vector& vector_a, const std::vector& vector_b) { - vector out; - vector x(a.size() + b.size()); - out.insert(out.end(), a.begin(), a.end()); - out.insert(out.end(), b.begin(), b.end()); + std::vector out; + out.insert(out.end(), vector_a.begin(), vector_a.end()); + out.insert(out.end(), vector_b.begin(), vector_b.end()); return out; } @@ -47,7 +52,7 @@ struct CartPoseErrCalculator : public sco::VectorOfVector Eigen::Isometry3d tcp_; CartPoseErrCalculator(const Eigen::Isometry3d& pose, const planning_scene::PlanningSceneConstPtr planning_scene, - std::string link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) + const std::string& link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) : target_pose_inv_(pose.inverse()), planning_scene_(planning_scene), link_(link), tcp_(tcp) { } @@ -85,4 +90,4 @@ struct JointVelJacobianCalculator : sco::MatrixOfVector Eigen::MatrixXd operator()(const Eigen::VectorXd& var_vals) const; }; -} // namespace trajopt +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h index 6ad995e4ff..1aa2397f6e 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h +++ b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h @@ -112,7 +112,7 @@ struct InitInfo /** @brief Data used during initialization. Use depends on the initialization selected. This data will be used to create initialization matrix. We need to give the goal information to this init info */ - trajopt::TrajArray data; + trajopt::TrajArray data; // This data type does not seem correct, it should be of type VectorXd // Eigen::VectorXd data_vec; // trajopt::TrajArray data_trajectory; /** @brief Default value the final column of the optimization is initialized too if time is being used */ @@ -122,7 +122,7 @@ struct InitInfo /** When cost or constraint element of JSON doc is read, one of these guys gets constructed to hold the parameters. -Then it later gets converted to a Cost object by the addObjectiveTerms method +Then it later gets converted to a Cost object by the hatch method */ struct TermInfo { @@ -133,7 +133,7 @@ struct TermInfo return supported_term_types_; } // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; - virtual void addObjectiveTerms(TrajOptProblem& prob) = 0; + virtual void hatch(TrajOptProblem& prob) = 0; static TermInfoPtr fromName(const std::string& type); @@ -141,7 +141,7 @@ struct TermInfo * Registers a user-defined TermInfo so you can use your own cost * see function RegisterMakers.cpp */ - using MakerFunc = TermInfoPtr (*)(void); + typedef TermInfoPtr (*MakerFunc)(void); static void RegisterMaker(const std::string& type, MakerFunc); virtual ~TermInfo() = default; @@ -152,7 +152,7 @@ struct TermInfo } private: - static std::map name_to_maker_; + static std::map name2maker; int supported_term_types_; }; @@ -168,10 +168,11 @@ struct ProblemInfo planning_scene::PlanningSceneConstPtr planning_scene; std::string planning_group_name; - ProblemInfo(planning_scene::PlanningSceneConstPtr ps, const std::string& pg) - : planning_scene(ps), planning_group_name(pg) + ProblemInfo(planning_scene::PlanningSceneConstPtr ps, std::string pg) : planning_scene(ps), planning_group_name(pg) { } + +private: }; /** @@ -187,38 +188,32 @@ class TrajOptProblem : public sco::OptProb /** @brief Returns the values of the specified joints (start_col to num_col) for the specified timestep i.*/ sco::VarVector GetVarRow(int i, int start_col, int num_col) { - return matrix_traj_vars.rblock(i, start_col, num_col); + return m_traj_vars.rblock(i, start_col, num_col); } /** @brief Returns the values of all joints for the specified timestep i.*/ sco::VarVector GetVarRow(int i) { - return matrix_traj_vars.row(i); + return m_traj_vars.row(i); } /** @brief Returns the value of the specified joint j for the specified timestep i.*/ sco::Var& GetVar(int i, int j) { - return matrix_traj_vars.at(i, j); + return m_traj_vars.at(i, j); } trajopt::VarArray& GetVars() { - return matrix_traj_vars; + return m_traj_vars; } /** @brief Returns the number of steps in the problem. This is the number of rows in the optimization matrix.*/ int GetNumSteps() { - return matrix_traj_vars.rows(); + return m_traj_vars.rows(); } /** @brief Returns the problem DOF. This is the number of columns in the optization matrix. * Note that this is not necessarily the same as the kinematic DOF.*/ int GetNumDOF() { - return matrix_traj_vars.cols(); - } - /** @brief Returns the kinematic DOF of the active joint model group - */ - int GetActiveGroupNumDOF() - { - return dof_; + return m_traj_vars.cols(); } planning_scene::PlanningSceneConstPtr GetPlanningScene() { @@ -226,11 +221,11 @@ class TrajOptProblem : public sco::OptProb } void SetInitTraj(const trajopt::TrajArray& x) { - matrix_init_traj = x; + m_init_traj = x; } trajopt::TrajArray GetInitTraj() { - return matrix_init_traj; + return m_init_traj; } // friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&); /** @brief Returns TrajOptProb.has_time */ @@ -248,12 +243,10 @@ class TrajOptProblem : public sco::OptProb /** @brief If true, the last column in the optimization matrix will be 1/dt */ bool has_time; /** @brief is the matrix holding the joint values of the trajectory for all timesteps */ - trajopt::VarArray matrix_traj_vars; + trajopt::VarArray m_traj_vars; planning_scene::PlanningSceneConstPtr planning_scene_; std::string planning_group_; - /** @brief Kinematic DOF of the active joint model group */ - int dof_; - trajopt::TrajArray matrix_init_traj; + trajopt::TrajArray m_init_traj; }; /** @brief This term is used when the goal frame is fixed in cartesian space @@ -282,7 +275,7 @@ struct CartPoseTermInfo : public TermInfo /** @brief Used to add term to pci from json */ // void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void addObjectiveTerms(TrajOptProblem& prob) override; + void hatch(TrajOptProblem& prob) override; static TermInfoPtr create() { @@ -322,7 +315,7 @@ struct JointPoseTermInfo : public TermInfo } /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void addObjectiveTerms(TrajOptProblem& prob) override; + void hatch(TrajOptProblem& prob) override; static TermInfoPtr create() { @@ -352,7 +345,7 @@ struct JointVelTermInfo : public TermInfo } /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void addObjectiveTerms(TrajOptProblem& prob) override; + void hatch(TrajOptProblem& prob) override; static TermInfoPtr create() { diff --git a/moveit_planners/trajopt/include/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h~HEAD similarity index 88% rename from moveit_planners/trajopt/include/problem_description.h rename to moveit_planners/trajopt/include/trajopt_interface/problem_description.h~HEAD index 8d1bd605e1..825c82262d 100644 --- a/moveit_planners/trajopt/include/problem_description.h +++ b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h~HEAD @@ -49,6 +49,9 @@ MOVEIT_CLASS_FORWARD(JointPoseTermInfo); struct CartPoseTermInfo; MOVEIT_CLASS_FORWARD(CartPoseTermInfo); +struct JointVelTermInfo; +MOVEIT_CLASS_FORWARD(JointVelTermInfo); + struct ProblemInfo; TrajOptProblemPtr ConstructProblem(const ProblemInfo&); @@ -109,9 +112,9 @@ struct InitInfo /** @brief Data used during initialization. Use depends on the initialization selected. This data will be used to create initialization matrix. We need to give the goal information to this init info */ - //trajopt::TrajArray data; This data type does not seem correct, it should be of type VectorXd - Eigen::VectorXd end_pos; - trajopt::TrajArray start_to_end_trajectory; + trajopt::TrajArray data; + // Eigen::VectorXd data_vec; + // trajopt::TrajArray data_trajectory; /** @brief Default value the final column of the optimization is initialized too if time is being used */ double dt = 1.0; }; @@ -119,7 +122,7 @@ struct InitInfo /** When cost or constraint element of JSON doc is read, one of these guys gets constructed to hold the parameters. -Then it later gets converted to a Cost object by the hatch method +Then it later gets converted to a Cost object by the addObjectiveTerms method */ struct TermInfo { @@ -130,7 +133,7 @@ struct TermInfo return supported_term_types_; } // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; - virtual void hatch(TrajOptProblem& prob) = 0; + virtual void addObjectiveTerms(TrajOptProblem& prob) = 0; static TermInfoPtr fromName(const std::string& type); @@ -138,7 +141,7 @@ struct TermInfo * Registers a user-defined TermInfo so you can use your own cost * see function RegisterMakers.cpp */ - typedef TermInfoPtr (*MakerFunc)(void); + using MakerFunc = TermInfoPtr (*)(void); static void RegisterMaker(const std::string& type, MakerFunc); virtual ~TermInfo() = default; @@ -149,7 +152,7 @@ struct TermInfo } private: - static std::map name2maker; + static std::map name_to_maker_; int supported_term_types_; }; @@ -165,12 +168,10 @@ struct ProblemInfo planning_scene::PlanningSceneConstPtr planning_scene; std::string planning_group_name; - ProblemInfo(planning_scene::PlanningSceneConstPtr ps, std::string pg) : planning_scene(ps), planning_group_name(pg) + ProblemInfo(planning_scene::PlanningSceneConstPtr ps, const std::string& pg) + : planning_scene(ps), planning_group_name(pg) { } - -private: - }; /** @@ -186,32 +187,38 @@ class TrajOptProblem : public sco::OptProb /** @brief Returns the values of the specified joints (start_col to num_col) for the specified timestep i.*/ sco::VarVector GetVarRow(int i, int start_col, int num_col) { - return m_traj_vars.rblock(i, start_col, num_col); + return matrix_traj_vars.rblock(i, start_col, num_col); } /** @brief Returns the values of all joints for the specified timestep i.*/ sco::VarVector GetVarRow(int i) { - return m_traj_vars.row(i); + return matrix_traj_vars.row(i); } /** @brief Returns the value of the specified joint j for the specified timestep i.*/ sco::Var& GetVar(int i, int j) { - return m_traj_vars.at(i, j); + return matrix_traj_vars.at(i, j); } trajopt::VarArray& GetVars() { - return m_traj_vars; + return matrix_traj_vars; } /** @brief Returns the number of steps in the problem. This is the number of rows in the optimization matrix.*/ int GetNumSteps() { - return m_traj_vars.rows(); + return matrix_traj_vars.rows(); } /** @brief Returns the problem DOF. This is the number of columns in the optization matrix. * Note that this is not necessarily the same as the kinematic DOF.*/ int GetNumDOF() { - return m_traj_vars.cols(); + return matrix_traj_vars.cols(); + } + /** @brief Returns the kinematic DOF of the active joint model group + */ + int GetActiveGroupNumDOF() + { + return dof_; } planning_scene::PlanningSceneConstPtr GetPlanningScene() { @@ -219,11 +226,11 @@ class TrajOptProblem : public sco::OptProb } void SetInitTraj(const trajopt::TrajArray& x) { - m_init_traj = x; + matrix_init_traj = x; } trajopt::TrajArray GetInitTraj() { - return m_init_traj; + return matrix_init_traj; } // friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&); /** @brief Returns TrajOptProb.has_time */ @@ -241,10 +248,12 @@ class TrajOptProblem : public sco::OptProb /** @brief If true, the last column in the optimization matrix will be 1/dt */ bool has_time; /** @brief is the matrix holding the joint values of the trajectory for all timesteps */ - trajopt::VarArray m_traj_vars; + trajopt::VarArray matrix_traj_vars; planning_scene::PlanningSceneConstPtr planning_scene_; std::string planning_group_; - trajopt::TrajArray m_init_traj; + /** @brief Kinematic DOF of the active joint model group */ + int dof_; + trajopt::TrajArray matrix_init_traj; }; /** @brief This term is used when the goal frame is fixed in cartesian space @@ -273,7 +282,7 @@ struct CartPoseTermInfo : public TermInfo /** @brief Used to add term to pci from json */ // void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void hatch(TrajOptProblem& prob) override; + void addObjectiveTerms(TrajOptProblem& prob) override; static TermInfoPtr create() { @@ -313,7 +322,7 @@ struct JointPoseTermInfo : public TermInfo } /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void hatch(TrajOptProblem& prob) override; + void addObjectiveTerms(TrajOptProblem& prob) override; static TermInfoPtr create() { @@ -343,7 +352,7 @@ struct JointVelTermInfo : public TermInfo } /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void hatch(TrajOptProblem& prob) override; + void addObjectiveTerms(TrajOptProblem& prob) override; static TermInfoPtr create() { @@ -352,6 +361,7 @@ struct JointVelTermInfo : public TermInfo } }; - void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, trajopt::TrajArray& init_traj); +void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, + trajopt::TrajArray& init_traj); } // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h index bb48692b88..c2df8a74b4 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h +++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2019, PickNik, LLC. + * Copyright (c) 2019, PickNik, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,13 +33,18 @@ *********************************************************************/ /* Author: Omid Heidari */ -#pragma once + +#ifndef TRAJOPT_INTERFACE_TRAJOPT_INTERFACE_H +#define TRAJOPT_INTERFACE_TRAJOPT_INTERFACE_H #include + #include -#include + #include "problem_description.h" +#include + namespace trajopt_interface { MOVEIT_CLASS_FORWARD(TrajOptInterface); // Defines TrajOptInterfacePtr, ConstPtr, WeakPtr... etc @@ -63,14 +68,11 @@ class TrajOptInterface void setDefaultTrajOPtParams(); void setProblemInfoParam(ProblemInfo& problem_info); void setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name); - trajopt::DblVec extractStartJointValues(const planning_interface::MotionPlanRequest& req, - const std::vector& group_joint_names); ros::NodeHandle nh_; /// The ROS node handle sco::BasicTrustRegionSQPParameters params_; - std::vector optimizer_callbacks_; - TrajOptProblemPtr trajopt_problem_; - std::string name_; + std::vector callbacks_; + TrajOptProblemPtr prob_; }; void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res); diff --git a/moveit_planners/trajopt/include/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h~HEAD similarity index 83% rename from moveit_planners/trajopt/include/trajopt_interface.h rename to moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h~HEAD index deabc02168..74432fa83c 100644 --- a/moveit_planners/trajopt/include/trajopt_interface.h +++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h~HEAD @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2019, PickNik, Inc. + * Copyright (c) 2019, PickNik, LLC. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,17 +33,12 @@ *********************************************************************/ /* Author: Omid Heidari */ - -#ifndef TRAJOPT_INTERFACE_TRAJOPT_INTERFACE_H -#define TRAJOPT_INTERFACE_TRAJOPT_INTERFACE_H +#pragma once #include - #include - -#include "problem_description.h" - #include +#include "problem_description.h" namespace trajopt_interface { @@ -52,7 +47,7 @@ MOVEIT_CLASS_FORWARD(TrajOptInterface); class TrajOptInterface { public: - TrajOptInterface(const ros::NodeHandle& nh = ros::NodeHandle("~")); + TrajOptInterface(const ros::NodeHandle& nh = ros::NodeHandle("~")); const sco::BasicTrustRegionSQPParameters& getParams() const { @@ -60,8 +55,7 @@ class TrajOptInterface } bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - moveit_msgs::MotionPlanDetailedResponse& res); + const planning_interface::MotionPlanRequest& req, moveit_msgs::MotionPlanDetailedResponse& res); protected: /** @brief Configure everything using the param server */ @@ -69,14 +63,15 @@ class TrajOptInterface void setDefaultTrajOPtParams(); void setProblemInfoParam(ProblemInfo& problem_info); void setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name); + trajopt::DblVec extractStartJointValues(const planning_interface::MotionPlanRequest& req, + const std::vector& group_joint_names); ros::NodeHandle nh_; /// The ROS node handle sco::BasicTrustRegionSQPParameters params_; - std::vector callbacks_; - TrajOptProblemPtr prob_; + std::vector optimizer_callbacks_; + TrajOptProblemPtr trajopt_problem_; + std::string name_; }; void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res); } - -#endif diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h index 9c06eb0f5e..34aff9355a 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h +++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h @@ -1,9 +1,10 @@ -#pragma once +#ifndef TrajOpt_PLANNING_CONTEXT_H +#define TrajOpt_PLANNING_CONTEXT_H #include -#include -#include +#include "trajopt_interface/problem_description.h" +#include "trajopt_interface/trajopt_interface.h" namespace trajopt_interface { @@ -24,9 +25,12 @@ class TrajOptPlanningContext : public planning_interface::PlanningContext bool terminate() override; void clear() override; +protected: private: moveit::core::RobotModelConstPtr robot_model_; TrajOptInterfacePtr trajopt_interface_; }; } // namespace trajopt_interface + +#endif // TrajOpt_PLANNING_CONTEXT_H diff --git a/moveit_planners/trajopt/include/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h~HEAD similarity index 80% rename from moveit_planners/trajopt/include/trajopt_planning_context.h rename to moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h~HEAD index 3a6497bcf8..12676f57e7 100644 --- a/moveit_planners/trajopt/include/trajopt_planning_context.h +++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h~HEAD @@ -1,11 +1,9 @@ -#ifndef TrajOpt_PLANNING_CONTEXT_H -#define TrajOpt_PLANNING_CONTEXT_H +#pragma once #include - -#include "problem_description.h" -#include "trajopt_interface.h" +#include +#include namespace trajopt_interface { @@ -26,12 +24,9 @@ class TrajOptPlanningContext : public planning_interface::PlanningContext bool terminate() override; void clear() override; -protected: private: moveit::core::RobotModelConstPtr robot_model_; TrajOptInterfacePtr trajopt_interface_; }; } // namespace trajopt_interface - -#endif // TrajOpt_PLANNING_CONTEXT_H diff --git a/moveit_planners/trajopt/src/test_trajopt.cpp b/moveit_planners/trajopt/src/test_trajopt.cpp index 5b3d48a1c2..5dd17985b2 100644 --- a/moveit_planners/trajopt/src/test_trajopt.cpp +++ b/moveit_planners/trajopt/src/test_trajopt.cpp @@ -1,5 +1,4 @@ #include -#include "trajopt_planning_context.h" #include #include @@ -24,16 +23,11 @@ #include #include +#include "trajopt_interface/trajopt_planning_context.h" // This file is a test for using trajopt in MoveIt. The goal is to make different types of constraints in -// MotionPlanRequest -// and visualize the result using trajopt planner. +// MotionPlanRequest and visualize the result calculated by using trajopt planner. // Three cases: -// 1- joint constraint with start_fixed true, meaining current state of the robot is the initial state of trajectory. -// 2- joint constraint with start_fixed false, meainig more than one goal should be given. The robot jumps to the first -// goal -// and motion planning is done between first goal and second goal. -// 3- Cartesian constraint. int main(int argc, char** argv) { @@ -48,7 +42,7 @@ int main(int argc, char** argv) new robot_model_loader::RobotModelLoader("robot_description")); robot_model::RobotModelPtr robot_model = robot_model_loader->getModel(); - /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/ + // Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group robot_state::RobotStatePtr current_state(new robot_state::RobotState(robot_model)); current_state->setToDefaultValues(); @@ -56,16 +50,19 @@ int main(int argc, char** argv) const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); - // With the planning scene we create a planing scene monitor that - // monitors planning scene diffs and applys them to the planning scene + // With the planning scene we create a planing scene monitor planning_scene_monitor::PlanningSceneMonitorPtr psm( new planning_scene_monitor::PlanningSceneMonitor(planning_scene, robot_model_loader)); psm->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); psm->startStateMonitor(); psm->startSceneMonitor(); - // set the start joint state - // ======================================================================================== + // res req + planning_interface::MotionPlanRequest req; + planning_interface::MotionPlanResponse res; + + // set start state + // ====================================================================================== // panda_arm joint limits: // -2.8973 2.8973 // -1.7628 1.7628 @@ -75,38 +72,54 @@ int main(int argc, char** argv) // -0.0175 3.7525 // -2.8973 2.8973 - std::vector start_joint_values = {0.4, 0.3, 0.5, -0.55, 0.88, 1.0, -0.075 }; + std::vector start_joint_values = { 0.4, 0.3, 0.5, -0.55, 0.88, 1.0, -0.075 }; robot_state::RobotStatePtr start_state(new robot_state::RobotState(robot_model)); start_state->setJointGroupPositions(joint_model_group, start_joint_values); start_state->update(); - // planning_scene->setCurrentState(*robot_state); - - // get the joint values of the start state - // std::vector tmp_joint_values; - // robot_state->copyJointGroupPositions(joint_model_group, tmp_joint_values); - // int r = 0; - // for (auto x : tmp_joint_values) - // { - // printf("===>>> joint: %s with start value: %f \n", joint_names[r].c_str(), x); - // ++r; - // } - - // printf("--- get the joint values from planning scene current state"); - // robot_state::RobotState current_state = planning_scene->getCurrentState(); - // std::vector tmp_j_values; - // current_state.copyJointGroupPositions(joint_model_group, tmp_j_values); - // r = 0; - // for (auto x : tmp_j_values) - // { - // printf("===>>> joint: %s with start value: %f \n", joint_names[r].c_str(), x); - // ++r; - // } - - - + req.start_state.joint_state.name = joint_names; + req.start_state.joint_state.position = start_joint_values; + req.goal_constraints.clear(); + req.group_name = PLANNING_GROUP; + ROS_INFO(" ==================================== 0"); + // set the middle and goal state and joints tolerance // ======================================================================================== + robot_state::RobotStatePtr middle_state(new robot_state::RobotState(robot_model)); + std::vector middle_joint_values = { 0.5, 0.4, 0.65, -0.75, 1.05, 1.25, -0.15 }; + middle_state->setJointGroupPositions(joint_model_group, middle_joint_values); + middle_state->update(); + moveit_msgs::Constraints joint_middle = + kinematic_constraints::constructGoalConstraints(*middle_state, joint_model_group); + req.goal_constraints.push_back(joint_middle); + req.goal_constraints[0].name = "middle_pos"; + // set joint tolerance + std::vector middle_joint_constraint = req.goal_constraints[0].joint_constraints; + for (int x = 0; x < middle_joint_constraint.size(); ++x) + { + ROS_INFO(" ======================================= joint position at goal: %f", + middle_joint_constraint[x].position); + req.goal_constraints[0].joint_constraints[x].tolerance_above = 0.001; + req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.001; + } + robot_state::RobotStatePtr goal_state(new robot_state::RobotState(robot_model)); + std::vector goal_joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1 }; + goal_state->setJointGroupPositions(joint_model_group, goal_joint_values); + goal_state->update(); + moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(*goal_state, joint_model_group); + req.goal_constraints.push_back(joint_goal); + req.goal_constraints[1].name = "goal_pos"; + // set joint tolerance + std::vector goal_joint_constraint = req.goal_constraints[1].joint_constraints; + for (int x = 0; x < goal_joint_constraint.size(); ++x) + { + ROS_INFO(" ======================================= joint position at goal: %f", goal_joint_constraint[x].position); + req.goal_constraints[1].joint_constraints[x].tolerance_above = 0.001; + req.goal_constraints[1].joint_constraints[x].tolerance_below = 0.001; + } + + // Load planner + // ====================================================================================== while (!psm->getStateMonitor()->haveCompleteState() && ros::ok()) { ROS_INFO_STREAM_THROTTLE_NAMED(1, node_name, "Waiting for complete state from topic "); @@ -153,8 +166,6 @@ int main(int argc, char** argv) // Visualization // ======================================================================================== - // The package MoveItVisualTools provides many capabilties for visualizing objects, robots, - // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script namespace rvt = rviz_visual_tools; moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, psm); visual_tools.loadRobotStatePub("/display_robot_state"); @@ -177,51 +188,6 @@ int main(int argc, char** argv) /* We can also use visual_tools to wait for user input */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); - // res req - planning_interface::MotionPlanRequest req; - planning_interface::MotionPlanResponse res; - - req.start_state.joint_state.name = joint_names; - req.start_state.joint_state.position = start_joint_values; - - // set the goal joint state and joints tolerance - // ======================================================================================== - robot_state::RobotState goal_state(robot_model); - std::vector goal_joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1 }; - goal_state.setJointGroupPositions(joint_model_group, goal_joint_values); - moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); - req.goal_constraints.clear(); - req.goal_constraints.push_back(joint_goal); - req.group_name = PLANNING_GROUP; - req.goal_constraints[0].name = "goal_pos"; - - // set joint tolerance - std::vector goal_joint_constraint = req.goal_constraints[0].joint_constraints; - for (int x = 0; x < goal_joint_constraint.size(); ++x) - { - std::cout << "==>> joint position at goal " << goal_joint_constraint[x].position << std::endl; - req.goal_constraints[0].joint_constraints[x].tolerance_above = 0.1; - req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.1; - } - - robot_state::RobotState middle_state(robot_model); - std::vector middle_joint_values = {0.5, 0.4, 0.65, -0.75, 1.05, 1.25, -0.15 }; - middle_state.setJointGroupPositions(joint_model_group, middle_joint_values); - middle_state.update(); - moveit_msgs::Constraints joint_middle = kinematic_constraints::constructGoalConstraints(middle_state, joint_model_group); - req.goal_constraints.push_back(joint_middle); - req.goal_constraints[1].name = "middle_pos"; - std::cout << "hereeeeeeeeeeeeeeeeeeeeeeee 1 " << req.goal_constraints[1].joint_constraints[0].position << std::endl; - std::vector middle_joint_constraint = req.goal_constraints[1].joint_constraints; - std::cout << "hereeeeeeeeeeeeeeeeeeeeeeee 2" << std::endl; - for (int x = 0; x < middle_joint_constraint.size(); ++x) - { - std::cout << "==>> joint position at middle " << middle_joint_constraint[x].position << std::endl; - req.goal_constraints[1].joint_constraints[x].tolerance_above = 0.1; - req.goal_constraints[1].joint_constraints[x].tolerance_below = 0.1; - } - - std::cout << "===>>> number of constraints in goal: " << req.goal_constraints.size() << std::endl; // planning context // ======================================================================================== planning_interface::PlanningContextPtr context = @@ -263,24 +229,10 @@ int main(int argc, char** argv) visual_tools.trigger(); display_publisher.publish(display_trajectory); - - std::cout << "===>>> group name:" << response.group_name << std::endl; - std::cout << "===>>> traj start joint name size: " << response.trajectory_start.joint_state.name.size() << std::endl; - std::cout << "===>>> traj start joint position size: " << response.trajectory_start.joint_state.position.size() - << std::endl; - - std::cout << "===>>> traj joint names size: " << response.trajectory.joint_trajectory.joint_names.size() << std::endl; - - for (int jn = 0; jn < response.trajectory.joint_trajectory.joint_names.size(); ++jn) - { - std::cout << "===>>> joint_" << jn << ": " << response.trajectory.joint_trajectory.joint_names[jn] << std::endl; - } - - const std::vector& str = joint_model_group->getLinkModelNames(); - printf("end effector name %s\n", str.back().c_str()); + ROS_INFO("end effector name %s\n", str.back().c_str()); - const Eigen::Affine3d& end_effector_transform_current = current_state->getGlobalLinkTransform(str.back()); + const Eigen::Affine3d& end_effector_transform_current = current_state->getGlobalLinkTransform(str.back()); geometry_msgs::PoseStamped pose_msg; pose_msg.header.stamp = ros::Time::now(); pose_msg.header.frame_id = robot_model->getModelFrame(); @@ -288,34 +240,30 @@ int main(int argc, char** argv) visual_tools.publishAxisLabeled(pose_msg.pose, "current"); visual_tools.publishText(text_pose, "current pose", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); - const Eigen::Affine3d& end_effector_transform_start = start_state->getGlobalLinkTransform(str.back()); + const Eigen::Affine3d& end_effector_transform_start = start_state->getGlobalLinkTransform(str.back()); pose_msg.header.stamp = ros::Time::now(); pose_msg.header.frame_id = robot_model->getModelFrame(); pose_msg.pose = tf2::toMsg(end_effector_transform_start); visual_tools.publishAxisLabeled(pose_msg.pose, "start"); visual_tools.publishText(text_pose, "start pose", rvt::BLUE, rvt::XLARGE); - visual_tools.trigger(); - - const Eigen::Affine3d& end_effector_transform_middle = middle_state.getGlobalLinkTransform(str.back()); + const Eigen::Affine3d& end_effector_transform_middle = middle_state->getGlobalLinkTransform(str.back()); pose_msg.header.stamp = ros::Time::now(); pose_msg.header.frame_id = robot_model->getModelFrame(); pose_msg.pose = tf2::toMsg(end_effector_transform_middle); visual_tools.publishAxisLabeled(pose_msg.pose, "middle"); - visual_tools.publishText(text_pose, "middle pose", rvt::BLUE, rvt::XLARGE); - visual_tools.trigger(); + visual_tools.publishText(text_pose, "middle pose", rvt::RED, rvt::XLARGE); - const Eigen::Affine3d& end_effector_transform_goal = goal_state.getGlobalLinkTransform(str.back()); + const Eigen::Affine3d& end_effector_transform_goal = goal_state->getGlobalLinkTransform(str.back()); pose_msg.header.stamp = ros::Time::now(); pose_msg.header.frame_id = robot_model->getModelFrame(); pose_msg.pose = tf2::toMsg(end_effector_transform_goal); visual_tools.publishAxisLabeled(pose_msg.pose, "goal"); - visual_tools.publishText(text_pose, "goal pose", rvt::BLUE, rvt::XLARGE); + visual_tools.publishText(text_pose, "goal pose", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); visual_tools.prompt("Press 'next' to finish demo \n"); diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 3005044159..3840253681 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -472,15 +472,6 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = 0; gen_files_.push_back(file); - // run_benchmark_trajopt.launch -------------------------------------------------------------------------------------- - file.file_name_ = "run_benchmark_trajopt.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Launch file for benchmarking TrajOpt planners"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); - file.write_on_changes = 0; - gen_files_.push_back(file); - // sensor_manager.launch -------------------------------------------------------------------------------------- file.file_name_ = "sensor_manager.launch.xml"; file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); From cab97a7b7d55eb99aeacae498ca8a9c7e2e639ec Mon Sep 17 00:00:00 2001 From: omid1366 Date: Thu, 1 Aug 2019 15:32:41 -0600 Subject: [PATCH 559/612] added kinematic DOF of the active joint model group to TrajOptProblem class added name_ for ROS_DEBUG_STREAM_NAMED addressed the comments moved launch file to moveit_tutorial moved yaml file to panda_moveit_config used bullet for collision checking --- .gitignore | 3 + moveit_planners/trajopt/config/setup.yaml | 50 ---- .../trajopt_interface/kinematic_terms.h | 31 +- .../trajopt_interface/problem_description.h | 38 ++- .../trajopt_interface/trajopt_interface.h | 8 +- .../trajopt_planning_context.h | 1 - .../trajopt/launch/test_trajopt_launch.launch | 33 --- moveit_planners/trajopt/src/test_trajopt.cpp | 270 ------------------ 8 files changed, 48 insertions(+), 386 deletions(-) delete mode 100644 moveit_planners/trajopt/config/setup.yaml delete mode 100644 moveit_planners/trajopt/launch/test_trajopt_launch.launch delete mode 100644 moveit_planners/trajopt/src/test_trajopt.cpp diff --git a/.gitignore b/.gitignore index cfc7f50ba4..917f3b56be 100644 --- a/.gitignore +++ b/.gitignore @@ -59,6 +59,9 @@ qtcreator-* #gdb *.gdb <<<<<<< HEAD +<<<<<<< HEAD ======= /moveit_planners/trajopt/launch/gdb_settings.gdb >>>>>>> addressed the comments +======= +>>>>>>> added kinematic DOF of the active joint model group to TrajOptProblem class diff --git a/moveit_planners/trajopt/config/setup.yaml b/moveit_planners/trajopt/config/setup.yaml deleted file mode 100644 index d3c8518cdd..0000000000 --- a/moveit_planners/trajopt/config/setup.yaml +++ /dev/null @@ -1,50 +0,0 @@ -trajopt_param: - improve_ratio_threshold: 0.25 # minimum ratio true_improve/approx_improve to accept step. [Research Paper] TrueImprove/ModelImprove > c - min_trust_box_size: 1e-4 # if trust region gets any smaller, exit and report convergence. [Research Paper] xtol - min_approx_improve: 1e-4 # if model improves less than this, exit and report convergence - min_approx_improve_frac: -.Inf # if model improves less than this, exit and report convergence - max_iter: 100 # The max number of iterations - trust_shrink_ratio: 0.1 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio. [Research Paper] tao- - trust_expand_ratio: 1.5 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio. [Research Paper] tao+ - cnt_tolerance: 1e-4 # after convergence of penalty subproblem, if constraint violation is less than this, we're done. [Research Paper] ctol - max_merit_coeff_increases: 5 # number of times that we jack up penalty coefficient. [Reasearch Paper] Max iteration in PenaltyIteration loop - merit_coeff_increase_ratio: 10 # ratio that we increate coeff each time. [Researcy Paper] Penalty scaling factory (italic kappa) - max_time: .inf # not yet implemented - merit_error_coeff: 10 # initial penalty coefficient. [Research Paper] mu_0 - trust_box_size: 1e-1 # current size of trust region (component-wise). [Research Paper] s - -problem_info: - basic_info: - n_steps: 20 # 2 * steps_per_phase - dt_upper_lim: 2.0 # The upper limit of 1/dt values allowed in the optimization - dt_lower_lim: 100.0 # The lower limit of 1/dt values allowed in the optimization - start_fixed: true # if true, start pose is the current pose at timestep = 0 - # if false, the start pose is the one given by req.start_state - use_time: flase # if false, it means the timestep is unit, meaning x1-x0 is the velocity for example - # if true, then cost_infos ro cnt_infos should have TT_USE_TIME for their term_type - convex_solver: 1 # which convex solver to use - # 1 = AUTO_SOLVER, 2 = BPMPD, 3 = OSQP, 4 = QPOASES, 5 = GUROBI - - init_info: - type: 3 # 1 = STATIONARY, 2 = JOINT_INTERPOLATED, 3 = GIVEN_TRAJ - # request.start_state should be provided for JOINT_INTERPOLATED and GIVEN_TRAJ - dt: 0.5 - -joint_pos_term_info: - start_pos: # from req.start_state - name: start_pos - first_timestep: 10 # First time step to which the term is applied. - last_timestep: 10 # Last time step to which the term is applied. - # if start_fixed is trure then we can not set the first_timestep to 0 for start_pos as it is going - # to be ignored and only the current pose would be the constraint at timestep = 0. - term_type: 2 # 1 = TT_COST, 2 = TT_CNT, 3 = TT_USE_TIME - middle_pos: - name: middle_pos - first_timestep: 15 - last_timestep: 15 - term_type: 2 - goal_pos: - name: goal_pos - first_timestep: 19 - last_timestep: 19 - term_type: 2 diff --git a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h index 51dac66b6a..9a9a8a5d38 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h +++ b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h @@ -4,31 +4,38 @@ #include -using namespace Eigen; -using namespace std; - namespace trajopt_interface { -Vector3d rotVec(const Matrix3d& m) + +/** + * @brief Extracts the vector part of quaternion + */ +inline Eigen::Vector3d rotVec(const Eigen::Matrix3d& m) { - Quaterniond q; + Eigen::Quaterniond q; q = m; - return Vector3d(q.x(), q.y(), q.z()); + return Eigen::Vector3d(q.x(), q.y(), q.z()); } -VectorXd concat(const VectorXd& a, const VectorXd& b) +/** + * @brief Appends b to a of type VectorXd + */ +inline Eigen::VectorXd concat(const Eigen::VectorXd& a, const Eigen::VectorXd& b) { - VectorXd out(a.size() + b.size()); + Eigen::VectorXd out(a.size() + b.size()); out.topRows(a.size()) = a; out.middleRows(a.size(), b.size()) = b; return out; } +/** + * @brief Appends b to a of type T + */ template -vector concat(const vector& a, const vector& b) +std::vector concat(const std::vector& a, const std::vector& b) { - vector out; - vector x(a.size() + b.size()); + std::vector out; + std::vector x(a.size() + b.size()); out.insert(out.end(), a.begin(), a.end()); out.insert(out.end(), b.begin(), b.end()); return out; @@ -47,7 +54,7 @@ struct CartPoseErrCalculator : public sco::VectorOfVector Eigen::Isometry3d tcp_; CartPoseErrCalculator(const Eigen::Isometry3d& pose, const planning_scene::PlanningSceneConstPtr planning_scene, - std::string link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) + const std::string& link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) : target_pose_inv_(pose.inverse()), planning_scene_(planning_scene), link_(link), tcp_(tcp) { } diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h index 1aa2397f6e..b87446715c 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h +++ b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h @@ -141,7 +141,7 @@ struct TermInfo * Registers a user-defined TermInfo so you can use your own cost * see function RegisterMakers.cpp */ - typedef TermInfoPtr (*MakerFunc)(void); + using MakerFunc = TermInfoPtr (*)(void); static void RegisterMaker(const std::string& type, MakerFunc); virtual ~TermInfo() = default; @@ -152,7 +152,7 @@ struct TermInfo } private: - static std::map name2maker; + static std::map name_to_maker_; int supported_term_types_; }; @@ -168,11 +168,9 @@ struct ProblemInfo planning_scene::PlanningSceneConstPtr planning_scene; std::string planning_group_name; - ProblemInfo(planning_scene::PlanningSceneConstPtr ps, std::string pg) : planning_scene(ps), planning_group_name(pg) + ProblemInfo(planning_scene::PlanningSceneConstPtr ps, const std::string& pg) : planning_scene(ps), planning_group_name(pg) { } - -private: }; /** @@ -188,32 +186,38 @@ class TrajOptProblem : public sco::OptProb /** @brief Returns the values of the specified joints (start_col to num_col) for the specified timestep i.*/ sco::VarVector GetVarRow(int i, int start_col, int num_col) { - return m_traj_vars.rblock(i, start_col, num_col); + return matrix_traj_vars.rblock(i, start_col, num_col); } /** @brief Returns the values of all joints for the specified timestep i.*/ sco::VarVector GetVarRow(int i) { - return m_traj_vars.row(i); + return matrix_traj_vars.row(i); } /** @brief Returns the value of the specified joint j for the specified timestep i.*/ sco::Var& GetVar(int i, int j) { - return m_traj_vars.at(i, j); + return matrix_traj_vars.at(i, j); } trajopt::VarArray& GetVars() { - return m_traj_vars; + return matrix_traj_vars; } /** @brief Returns the number of steps in the problem. This is the number of rows in the optimization matrix.*/ int GetNumSteps() { - return m_traj_vars.rows(); + return matrix_traj_vars.rows(); } /** @brief Returns the problem DOF. This is the number of columns in the optization matrix. * Note that this is not necessarily the same as the kinematic DOF.*/ int GetNumDOF() { - return m_traj_vars.cols(); + return matrix_traj_vars.cols(); + } + /** @brief Returns the kinematic DOF of the active joint model group + */ + int GetActiveGroupNumDOF() + { + return dof_; } planning_scene::PlanningSceneConstPtr GetPlanningScene() { @@ -221,11 +225,11 @@ class TrajOptProblem : public sco::OptProb } void SetInitTraj(const trajopt::TrajArray& x) { - m_init_traj = x; + matrix_init_traj = x; } trajopt::TrajArray GetInitTraj() { - return m_init_traj; + return matrix_init_traj; } // friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&); /** @brief Returns TrajOptProb.has_time */ @@ -243,10 +247,12 @@ class TrajOptProblem : public sco::OptProb /** @brief If true, the last column in the optimization matrix will be 1/dt */ bool has_time; /** @brief is the matrix holding the joint values of the trajectory for all timesteps */ - trajopt::VarArray m_traj_vars; + trajopt::VarArray matrix_traj_vars; planning_scene::PlanningSceneConstPtr planning_scene_; std::string planning_group_; - trajopt::TrajArray m_init_traj; + /** @brief Kinematic DOF of the active joint model group */ + int dof_; + trajopt::TrajArray matrix_init_traj; }; /** @brief This term is used when the goal frame is fixed in cartesian space @@ -354,6 +360,8 @@ struct JointVelTermInfo : public TermInfo } }; + + void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, trajopt::TrajArray& init_traj); diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h index c2df8a74b4..183b4b3406 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h +++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2019, PickNik, Inc. + * Copyright (c) 2019, PickNik, LLC. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -38,12 +38,9 @@ #define TRAJOPT_INTERFACE_TRAJOPT_INTERFACE_H #include - #include - -#include "problem_description.h" - #include +#include "problem_description.h" namespace trajopt_interface { @@ -73,6 +70,7 @@ class TrajOptInterface sco::BasicTrustRegionSQPParameters params_; std::vector callbacks_; TrajOptProblemPtr prob_; + std::string name_; }; void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res); diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h index 34aff9355a..fc8641c59e 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h +++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h @@ -25,7 +25,6 @@ class TrajOptPlanningContext : public planning_interface::PlanningContext bool terminate() override; void clear() override; -protected: private: moveit::core::RobotModelConstPtr robot_model_; diff --git a/moveit_planners/trajopt/launch/test_trajopt_launch.launch b/moveit_planners/trajopt/launch/test_trajopt_launch.launch deleted file mode 100644 index f0d35bdeaa..0000000000 --- a/moveit_planners/trajopt/launch/test_trajopt_launch.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_planners/trajopt/src/test_trajopt.cpp b/moveit_planners/trajopt/src/test_trajopt.cpp deleted file mode 100644 index 5dd17985b2..0000000000 --- a/moveit_planners/trajopt/src/test_trajopt.cpp +++ /dev/null @@ -1,270 +0,0 @@ -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include "moveit/planning_interface/planning_request.h" - -#include -#include - -#include -#include -#include -#include - -#include "trajopt_interface/trajopt_planning_context.h" - -// This file is a test for using trajopt in MoveIt. The goal is to make different types of constraints in -// MotionPlanRequest and visualize the result calculated by using trajopt planner. -// Three cases: - -int main(int argc, char** argv) -{ - const std::string node_name = "trajopot_planning_tutorial"; - ros::init(argc, argv, node_name); - ros::AsyncSpinner spinner(1); - spinner.start(); - ros::NodeHandle node_handle("~"); - - const std::string PLANNING_GROUP = "panda_arm"; - robot_model_loader::RobotModelLoaderPtr robot_model_loader( - new robot_model_loader::RobotModelLoader("robot_description")); - robot_model::RobotModelPtr robot_model = robot_model_loader->getModel(); - - // Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group - robot_state::RobotStatePtr current_state(new robot_state::RobotState(robot_model)); - current_state->setToDefaultValues(); - - const robot_state::JointModelGroup* joint_model_group = current_state->getJointModelGroup(PLANNING_GROUP); - const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); - planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); - - // With the planning scene we create a planing scene monitor - planning_scene_monitor::PlanningSceneMonitorPtr psm( - new planning_scene_monitor::PlanningSceneMonitor(planning_scene, robot_model_loader)); - psm->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); - psm->startStateMonitor(); - psm->startSceneMonitor(); - - // res req - planning_interface::MotionPlanRequest req; - planning_interface::MotionPlanResponse res; - - // set start state - // ====================================================================================== - // panda_arm joint limits: - // -2.8973 2.8973 - // -1.7628 1.7628 - // -2.8973 2.8973 - // -3.0718 -0.0698 - // -2.8973 2.8973 - // -0.0175 3.7525 - // -2.8973 2.8973 - - std::vector start_joint_values = { 0.4, 0.3, 0.5, -0.55, 0.88, 1.0, -0.075 }; - robot_state::RobotStatePtr start_state(new robot_state::RobotState(robot_model)); - start_state->setJointGroupPositions(joint_model_group, start_joint_values); - start_state->update(); - - req.start_state.joint_state.name = joint_names; - req.start_state.joint_state.position = start_joint_values; - req.goal_constraints.clear(); - req.group_name = PLANNING_GROUP; - ROS_INFO(" ==================================== 0"); - // set the middle and goal state and joints tolerance - // ======================================================================================== - robot_state::RobotStatePtr middle_state(new robot_state::RobotState(robot_model)); - std::vector middle_joint_values = { 0.5, 0.4, 0.65, -0.75, 1.05, 1.25, -0.15 }; - middle_state->setJointGroupPositions(joint_model_group, middle_joint_values); - middle_state->update(); - moveit_msgs::Constraints joint_middle = - kinematic_constraints::constructGoalConstraints(*middle_state, joint_model_group); - req.goal_constraints.push_back(joint_middle); - req.goal_constraints[0].name = "middle_pos"; - // set joint tolerance - std::vector middle_joint_constraint = req.goal_constraints[0].joint_constraints; - for (int x = 0; x < middle_joint_constraint.size(); ++x) - { - ROS_INFO(" ======================================= joint position at goal: %f", - middle_joint_constraint[x].position); - req.goal_constraints[0].joint_constraints[x].tolerance_above = 0.001; - req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.001; - } - - robot_state::RobotStatePtr goal_state(new robot_state::RobotState(robot_model)); - std::vector goal_joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1 }; - goal_state->setJointGroupPositions(joint_model_group, goal_joint_values); - goal_state->update(); - moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(*goal_state, joint_model_group); - req.goal_constraints.push_back(joint_goal); - req.goal_constraints[1].name = "goal_pos"; - // set joint tolerance - std::vector goal_joint_constraint = req.goal_constraints[1].joint_constraints; - for (int x = 0; x < goal_joint_constraint.size(); ++x) - { - ROS_INFO(" ======================================= joint position at goal: %f", goal_joint_constraint[x].position); - req.goal_constraints[1].joint_constraints[x].tolerance_above = 0.001; - req.goal_constraints[1].joint_constraints[x].tolerance_below = 0.001; - } - - // Load planner - // ====================================================================================== - while (!psm->getStateMonitor()->haveCompleteState() && ros::ok()) - { - ROS_INFO_STREAM_THROTTLE_NAMED(1, node_name, "Waiting for complete state from topic "); - } - // We will now construct a loader to load a planner, by name. - // Note that we are using the ROS pluginlib library here. - boost::scoped_ptr> planner_plugin_loader; - planning_interface::PlannerManagerPtr planner_instance; - - std::string planner_plugin_name; - - planner_plugin_name = "trajopt_interface/TrajOptPlanner"; - node_handle.setParam("planning_plugin", planner_plugin_name); - - // making sure to catch all exceptions. - if (!node_handle.getParam("planning_plugin", planner_plugin_name)) - ROS_FATAL_STREAM("Could not find planner plugin name"); - try - { - planner_plugin_loader.reset(new pluginlib::ClassLoader( - "moveit_core", "planning_interface::PlannerManager")); - printf("===>>> planner_plugin_name: %s \n", planner_plugin_name.c_str()); - } - catch (pluginlib::PluginlibException& ex) - { - ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); - } - try - { - planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); - if (!planner_instance->initialize(robot_model, node_handle.getNamespace())) // namespace ????. I dont use it - ROS_FATAL_STREAM("Could not initialize planner instance"); - ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'"); - } - catch (pluginlib::PluginlibException& ex) - { - const std::vector& classes = planner_plugin_loader->getDeclaredClasses(); - std::stringstream ss; - for (std::size_t i = 0; i < classes.size(); ++i) - ss << classes[i] << " "; - ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl - << "Available plugins: " << ss.str()); - } - - // Visualization - // ======================================================================================== - namespace rvt = rviz_visual_tools; - moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, psm); - visual_tools.loadRobotStatePub("/display_robot_state"); - visual_tools.enableBatchPublishing(); - visual_tools.deleteAllMarkers(); // clear all old markers - visual_tools.trigger(); - - /* Remote control is an introspection tool that allows users to step through a high level script - via buttons and keyboard shortcuts in RViz */ - visual_tools.loadRemoteControl(); - - /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/ - Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); - text_pose.translation().z() = 1.75; - visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE); - - /* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */ - visual_tools.trigger(); - - /* We can also use visual_tools to wait for user input */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); - - // planning context - // ======================================================================================== - planning_interface::PlanningContextPtr context = - planner_instance->getPlanningContext(planning_scene, req, res.error_code_); - - context->solve(res); - if (res.error_code_.val != res.error_code_.SUCCESS) - { - ROS_ERROR("Could not compute plan successfully"); - return 0; - } - - visual_tools.prompt("Press 'next' to visualize the result"); - - // Visualize the result - // ======================================================================================== - ros::Publisher display_publisher = - node_handle.advertise("/move_group/display_planned_path", 1, true); - moveit_msgs::DisplayTrajectory display_trajectory; - - /* Visualize the trajectory */ - moveit_msgs::MotionPlanResponse response; - res.getMessage(response); - - for (int timestep_index = 0; timestep_index < response.trajectory.joint_trajectory.points.size(); ++timestep_index) - { - for (int joint_index = 0; - joint_index < response.trajectory.joint_trajectory.points[timestep_index].positions.size(); ++joint_index) - { - std::cout << response.trajectory.joint_trajectory.points[timestep_index].positions[joint_index] << " "; - } - std::cout << std::endl; - } - - display_trajectory.trajectory_start = response.trajectory_start; - display_trajectory.trajectory.push_back(response.trajectory); - - visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); - visual_tools.trigger(); - display_publisher.publish(display_trajectory); - - const std::vector& str = joint_model_group->getLinkModelNames(); - ROS_INFO("end effector name %s\n", str.back().c_str()); - - const Eigen::Affine3d& end_effector_transform_current = current_state->getGlobalLinkTransform(str.back()); - geometry_msgs::PoseStamped pose_msg; - pose_msg.header.stamp = ros::Time::now(); - pose_msg.header.frame_id = robot_model->getModelFrame(); - pose_msg.pose = tf2::toMsg(end_effector_transform_current); - - visual_tools.publishAxisLabeled(pose_msg.pose, "current"); - visual_tools.publishText(text_pose, "current pose", rvt::WHITE, rvt::XLARGE); - - const Eigen::Affine3d& end_effector_transform_start = start_state->getGlobalLinkTransform(str.back()); - pose_msg.header.stamp = ros::Time::now(); - pose_msg.header.frame_id = robot_model->getModelFrame(); - pose_msg.pose = tf2::toMsg(end_effector_transform_start); - - visual_tools.publishAxisLabeled(pose_msg.pose, "start"); - visual_tools.publishText(text_pose, "start pose", rvt::BLUE, rvt::XLARGE); - - const Eigen::Affine3d& end_effector_transform_middle = middle_state->getGlobalLinkTransform(str.back()); - pose_msg.header.stamp = ros::Time::now(); - pose_msg.header.frame_id = robot_model->getModelFrame(); - pose_msg.pose = tf2::toMsg(end_effector_transform_middle); - - visual_tools.publishAxisLabeled(pose_msg.pose, "middle"); - visual_tools.publishText(text_pose, "middle pose", rvt::RED, rvt::XLARGE); - - const Eigen::Affine3d& end_effector_transform_goal = goal_state->getGlobalLinkTransform(str.back()); - pose_msg.header.stamp = ros::Time::now(); - pose_msg.header.frame_id = robot_model->getModelFrame(); - pose_msg.pose = tf2::toMsg(end_effector_transform_goal); - - visual_tools.publishAxisLabeled(pose_msg.pose, "goal"); - visual_tools.publishText(text_pose, "goal pose", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); - - visual_tools.prompt("Press 'next' to finish demo \n"); -} From 345d2908b66c24688660a320a3dd49bc4b1a9694 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Thu, 8 Aug 2019 18:04:29 -0600 Subject: [PATCH 560/612] clang format --- .../trajopt/include/trajopt_interface/kinematic_terms.h | 1 - .../trajopt/include/trajopt_interface/problem_description.h | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h index 9a9a8a5d38..5c60137b8e 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h +++ b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h @@ -6,7 +6,6 @@ namespace trajopt_interface { - /** * @brief Extracts the vector part of quaternion */ diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h index b87446715c..d682e38f42 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h +++ b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h @@ -168,7 +168,8 @@ struct ProblemInfo planning_scene::PlanningSceneConstPtr planning_scene; std::string planning_group_name; - ProblemInfo(planning_scene::PlanningSceneConstPtr ps, const std::string& pg) : planning_scene(ps), planning_group_name(pg) + ProblemInfo(planning_scene::PlanningSceneConstPtr ps, const std::string& pg) + : planning_scene(ps), planning_group_name(pg) { } }; @@ -360,8 +361,6 @@ struct JointVelTermInfo : public TermInfo } }; - - void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, trajopt::TrajArray& init_traj); From de90464fc6b256877244b79007607f828de3d392 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Wed, 7 Aug 2019 09:01:56 -0600 Subject: [PATCH 561/612] Added comments about initial trajectory changed my email address renamed rotVec and concat added tesseract license --- .../trajopt/include/trajopt_interface/kinematic_terms.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h index 5c60137b8e..2796d0dffa 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h +++ b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h @@ -9,7 +9,7 @@ namespace trajopt_interface /** * @brief Extracts the vector part of quaternion */ -inline Eigen::Vector3d rotVec(const Eigen::Matrix3d& m) +inline Eigen::Vector3d quaternionRotationVector(const Eigen::Matrix3d& m) { Eigen::Quaterniond q; q = m; @@ -19,7 +19,7 @@ inline Eigen::Vector3d rotVec(const Eigen::Matrix3d& m) /** * @brief Appends b to a of type VectorXd */ -inline Eigen::VectorXd concat(const Eigen::VectorXd& a, const Eigen::VectorXd& b) +inline Eigen::VectorXd concatVector(const Eigen::VectorXd& a, const Eigen::VectorXd& b) { Eigen::VectorXd out(a.size() + b.size()); out.topRows(a.size()) = a; @@ -31,7 +31,7 @@ inline Eigen::VectorXd concat(const Eigen::VectorXd& a, const Eigen::VectorXd& b * @brief Appends b to a of type T */ template -std::vector concat(const std::vector& a, const std::vector& b) +std::vector concatVector(const std::vector& a, const std::vector& b) { std::vector out; std::vector x(a.size() + b.size()); From 23b9acad21f63d2ac7fff3eb429fe708135453f3 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Mon, 12 Aug 2019 09:45:00 -0600 Subject: [PATCH 562/612] applied Henning's comments --- .../trajopt/include/trajopt_interface/problem_description.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h index d682e38f42..97d4b046e8 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h +++ b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h @@ -112,7 +112,7 @@ struct InitInfo /** @brief Data used during initialization. Use depends on the initialization selected. This data will be used to create initialization matrix. We need to give the goal information to this init info */ - trajopt::TrajArray data; // This data type does not seem correct, it should be of type VectorXd + trajopt::TrajArray data; // Eigen::VectorXd data_vec; // trajopt::TrajArray data_trajectory; /** @brief Default value the final column of the optimization is initialized too if time is being used */ From c8f1011b5417858b82d61dae0c7de4bd2261fb23 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Mon, 12 Aug 2019 13:33:07 -0600 Subject: [PATCH 563/612] added catkin ignore to ignore this package for now as it has dependencies. The next PR wont have any dependencies --- .gitignore | 7 ------- 1 file changed, 7 deletions(-) diff --git a/.gitignore b/.gitignore index 917f3b56be..808fe85341 100644 --- a/.gitignore +++ b/.gitignore @@ -58,10 +58,3 @@ qtcreator-* #gdb *.gdb -<<<<<<< HEAD -<<<<<<< HEAD -======= -/moveit_planners/trajopt/launch/gdb_settings.gdb ->>>>>>> addressed the comments -======= ->>>>>>> added kinematic DOF of the active joint model group to TrajOptProblem class From d119525ee51a7e70995e5d7abc38332eb8419d07 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Tue, 13 Aug 2019 12:23:27 -0600 Subject: [PATCH 564/612] addressed the comments --- .../trajopt_interface/kinematic_terms.h | 23 +++++++++---------- .../trajopt_interface/trajopt_interface.h | 10 ++++---- .../trajopt_planning_context.h | 9 +++----- 3 files changed, 19 insertions(+), 23 deletions(-) diff --git a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h index 2796d0dffa..96274cf9c7 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h +++ b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h @@ -9,21 +9,21 @@ namespace trajopt_interface /** * @brief Extracts the vector part of quaternion */ -inline Eigen::Vector3d quaternionRotationVector(const Eigen::Matrix3d& m) +inline Eigen::Vector3d quaternionRotationVector(const Eigen::Matrix3d& matrix) { - Eigen::Quaterniond q; - q = m; - return Eigen::Vector3d(q.x(), q.y(), q.z()); + Eigen::Quaterniond quaternion; + quaternion = matrix; + return Eigen::Vector3d(quaternion.x(), quaternion.y(), quaternion.z()); } /** * @brief Appends b to a of type VectorXd */ -inline Eigen::VectorXd concatVector(const Eigen::VectorXd& a, const Eigen::VectorXd& b) +inline Eigen::VectorXd concatVector(const Eigen::VectorXd& vector_a, const Eigen::VectorXd& vector_b) { - Eigen::VectorXd out(a.size() + b.size()); - out.topRows(a.size()) = a; - out.middleRows(a.size(), b.size()) = b; + Eigen::VectorXd out(vector_a.size() + vector_b.size()); + out.topRows(vector_a.size()) = vector_a; + out.middleRows(vector_a.size(), vector_b.size()) = vector_b; return out; } @@ -31,12 +31,11 @@ inline Eigen::VectorXd concatVector(const Eigen::VectorXd& a, const Eigen::Vecto * @brief Appends b to a of type T */ template -std::vector concatVector(const std::vector& a, const std::vector& b) +inline std::vector concatVector(const std::vector& vector_a, const std::vector& vector_b) { std::vector out; - std::vector x(a.size() + b.size()); - out.insert(out.end(), a.begin(), a.end()); - out.insert(out.end(), b.begin(), b.end()); + out.insert(out.end(), vector_a.begin(), vector_a.end()); + out.insert(out.end(), vector_b.begin(), vector_b.end()); return out; } diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h index 183b4b3406..bb48692b88 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h +++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h @@ -33,9 +33,7 @@ *********************************************************************/ /* Author: Omid Heidari */ - -#ifndef TRAJOPT_INTERFACE_TRAJOPT_INTERFACE_H -#define TRAJOPT_INTERFACE_TRAJOPT_INTERFACE_H +#pragma once #include #include @@ -65,11 +63,13 @@ class TrajOptInterface void setDefaultTrajOPtParams(); void setProblemInfoParam(ProblemInfo& problem_info); void setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name); + trajopt::DblVec extractStartJointValues(const planning_interface::MotionPlanRequest& req, + const std::vector& group_joint_names); ros::NodeHandle nh_; /// The ROS node handle sco::BasicTrustRegionSQPParameters params_; - std::vector callbacks_; - TrajOptProblemPtr prob_; + std::vector optimizer_callbacks_; + TrajOptProblemPtr trajopt_problem_; std::string name_; }; diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h index fc8641c59e..9c06eb0f5e 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h +++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h @@ -1,10 +1,9 @@ -#ifndef TrajOpt_PLANNING_CONTEXT_H -#define TrajOpt_PLANNING_CONTEXT_H +#pragma once #include -#include "trajopt_interface/problem_description.h" -#include "trajopt_interface/trajopt_interface.h" +#include +#include namespace trajopt_interface { @@ -31,5 +30,3 @@ class TrajOptPlanningContext : public planning_interface::PlanningContext TrajOptInterfacePtr trajopt_interface_; }; } // namespace trajopt_interface - -#endif // TrajOpt_PLANNING_CONTEXT_H From a1d26bec2de80f14db9ab2da1cc2cbf53151a005 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Tue, 13 Aug 2019 14:36:49 -0600 Subject: [PATCH 565/612] renamed hatch to addObjectiveTerms --- .../include/trajopt_interface/problem_description.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h index 97d4b046e8..6ad995e4ff 100644 --- a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h +++ b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h @@ -122,7 +122,7 @@ struct InitInfo /** When cost or constraint element of JSON doc is read, one of these guys gets constructed to hold the parameters. -Then it later gets converted to a Cost object by the hatch method +Then it later gets converted to a Cost object by the addObjectiveTerms method */ struct TermInfo { @@ -133,7 +133,7 @@ struct TermInfo return supported_term_types_; } // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; - virtual void hatch(TrajOptProblem& prob) = 0; + virtual void addObjectiveTerms(TrajOptProblem& prob) = 0; static TermInfoPtr fromName(const std::string& type); @@ -282,7 +282,7 @@ struct CartPoseTermInfo : public TermInfo /** @brief Used to add term to pci from json */ // void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void hatch(TrajOptProblem& prob) override; + void addObjectiveTerms(TrajOptProblem& prob) override; static TermInfoPtr create() { @@ -322,7 +322,7 @@ struct JointPoseTermInfo : public TermInfo } /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void hatch(TrajOptProblem& prob) override; + void addObjectiveTerms(TrajOptProblem& prob) override; static TermInfoPtr create() { @@ -352,7 +352,7 @@ struct JointVelTermInfo : public TermInfo } /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void hatch(TrajOptProblem& prob) override; + void addObjectiveTerms(TrajOptProblem& prob) override; static TermInfoPtr create() { From 9eb372b5212af8aeb663d1223ea925d77f0d8f60 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Thu, 15 Aug 2019 10:00:37 -0600 Subject: [PATCH 566/612] addressed Dave's comments --- moveit_planners/trajopt/CATKIN_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 moveit_planners/trajopt/CATKIN_IGNORE diff --git a/moveit_planners/trajopt/CATKIN_IGNORE b/moveit_planners/trajopt/CATKIN_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 From 529cb65993452382e8879640a48492e0037c87f4 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Thu, 15 Aug 2019 10:34:31 -0600 Subject: [PATCH 567/612] added catkin_ignore --- moveit_planners/trajopt/CATKIN_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 moveit_planners/trajopt/CATKIN_IGNORE diff --git a/moveit_planners/trajopt/CATKIN_IGNORE b/moveit_planners/trajopt/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 From 7d3c518f5ef0ed51f5ecf37aa7e328fea9dc63e6 Mon Sep 17 00:00:00 2001 From: omid1366 Date: Thu, 15 Aug 2019 16:44:31 -0600 Subject: [PATCH 568/612] prepare for merge --- moveit_planners/trajopt/CATKIN_IGNORE | 0 .../include/trajopt}/kinematic_terms.h | 0 .../include/trajopt}/problem_description.h | 0 moveit_planners/trajopt/{ => trajopt}/src/kinematic_terms.cpp | 0 moveit_planners/trajopt/{ => trajopt}/src/problem_description.cpp | 0 moveit_planners/trajopt/{ => trajopt_interface}/CMakeLists.txt | 0 moveit_planners/trajopt/{ => trajopt_interface}/README.md | 0 .../include/trajopt_interface/trajopt_interface.h | 0 .../include/trajopt_interface/trajopt_planning_context.h | 0 moveit_planners/trajopt/{ => trajopt_interface}/package.xml | 0 .../trajopt/{ => trajopt_interface}/src/trajopt_interface.cpp | 0 .../{ => trajopt_interface}/src/trajopt_planner_manager.cpp | 0 .../{ => trajopt_interface}/src/trajopt_planning_context.cpp | 0 .../trajopt/{ => trajopt_interface}/test/trajectory_test.cpp | 0 .../trajopt/{ => trajopt_interface}/test/trajectory_test.test | 0 .../trajopt_interface_plugin_description.xml | 0 16 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 moveit_planners/trajopt/CATKIN_IGNORE rename moveit_planners/trajopt/{include/trajopt_interface => trajopt/include/trajopt}/kinematic_terms.h (100%) rename moveit_planners/trajopt/{include/trajopt_interface => trajopt/include/trajopt}/problem_description.h (100%) rename moveit_planners/trajopt/{ => trajopt}/src/kinematic_terms.cpp (100%) rename moveit_planners/trajopt/{ => trajopt}/src/problem_description.cpp (100%) rename moveit_planners/trajopt/{ => trajopt_interface}/CMakeLists.txt (100%) rename moveit_planners/trajopt/{ => trajopt_interface}/README.md (100%) rename moveit_planners/trajopt/{ => trajopt_interface}/include/trajopt_interface/trajopt_interface.h (100%) rename moveit_planners/trajopt/{ => trajopt_interface}/include/trajopt_interface/trajopt_planning_context.h (100%) rename moveit_planners/trajopt/{ => trajopt_interface}/package.xml (100%) rename moveit_planners/trajopt/{ => trajopt_interface}/src/trajopt_interface.cpp (100%) rename moveit_planners/trajopt/{ => trajopt_interface}/src/trajopt_planner_manager.cpp (100%) rename moveit_planners/trajopt/{ => trajopt_interface}/src/trajopt_planning_context.cpp (100%) rename moveit_planners/trajopt/{ => trajopt_interface}/test/trajectory_test.cpp (100%) rename moveit_planners/trajopt/{ => trajopt_interface}/test/trajectory_test.test (100%) rename moveit_planners/trajopt/{ => trajopt_interface}/trajopt_interface_plugin_description.xml (100%) diff --git a/moveit_planners/trajopt/CATKIN_IGNORE b/moveit_planners/trajopt/CATKIN_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h b/moveit_planners/trajopt/trajopt/include/trajopt/kinematic_terms.h similarity index 100% rename from moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h rename to moveit_planners/trajopt/trajopt/include/trajopt/kinematic_terms.h diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/trajopt/include/trajopt/problem_description.h similarity index 100% rename from moveit_planners/trajopt/include/trajopt_interface/problem_description.h rename to moveit_planners/trajopt/trajopt/include/trajopt/problem_description.h diff --git a/moveit_planners/trajopt/src/kinematic_terms.cpp b/moveit_planners/trajopt/trajopt/src/kinematic_terms.cpp similarity index 100% rename from moveit_planners/trajopt/src/kinematic_terms.cpp rename to moveit_planners/trajopt/trajopt/src/kinematic_terms.cpp diff --git a/moveit_planners/trajopt/src/problem_description.cpp b/moveit_planners/trajopt/trajopt/src/problem_description.cpp similarity index 100% rename from moveit_planners/trajopt/src/problem_description.cpp rename to moveit_planners/trajopt/trajopt/src/problem_description.cpp diff --git a/moveit_planners/trajopt/CMakeLists.txt b/moveit_planners/trajopt/trajopt_interface/CMakeLists.txt similarity index 100% rename from moveit_planners/trajopt/CMakeLists.txt rename to moveit_planners/trajopt/trajopt_interface/CMakeLists.txt diff --git a/moveit_planners/trajopt/README.md b/moveit_planners/trajopt/trajopt_interface/README.md similarity index 100% rename from moveit_planners/trajopt/README.md rename to moveit_planners/trajopt/trajopt_interface/README.md diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h b/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_interface.h similarity index 100% rename from moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h rename to moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_interface.h diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h b/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_planning_context.h similarity index 100% rename from moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h rename to moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_planning_context.h diff --git a/moveit_planners/trajopt/package.xml b/moveit_planners/trajopt/trajopt_interface/package.xml similarity index 100% rename from moveit_planners/trajopt/package.xml rename to moveit_planners/trajopt/trajopt_interface/package.xml diff --git a/moveit_planners/trajopt/src/trajopt_interface.cpp b/moveit_planners/trajopt/trajopt_interface/src/trajopt_interface.cpp similarity index 100% rename from moveit_planners/trajopt/src/trajopt_interface.cpp rename to moveit_planners/trajopt/trajopt_interface/src/trajopt_interface.cpp diff --git a/moveit_planners/trajopt/src/trajopt_planner_manager.cpp b/moveit_planners/trajopt/trajopt_interface/src/trajopt_planner_manager.cpp similarity index 100% rename from moveit_planners/trajopt/src/trajopt_planner_manager.cpp rename to moveit_planners/trajopt/trajopt_interface/src/trajopt_planner_manager.cpp diff --git a/moveit_planners/trajopt/src/trajopt_planning_context.cpp b/moveit_planners/trajopt/trajopt_interface/src/trajopt_planning_context.cpp similarity index 100% rename from moveit_planners/trajopt/src/trajopt_planning_context.cpp rename to moveit_planners/trajopt/trajopt_interface/src/trajopt_planning_context.cpp diff --git a/moveit_planners/trajopt/test/trajectory_test.cpp b/moveit_planners/trajopt/trajopt_interface/test/trajectory_test.cpp similarity index 100% rename from moveit_planners/trajopt/test/trajectory_test.cpp rename to moveit_planners/trajopt/trajopt_interface/test/trajectory_test.cpp diff --git a/moveit_planners/trajopt/test/trajectory_test.test b/moveit_planners/trajopt/trajopt_interface/test/trajectory_test.test similarity index 100% rename from moveit_planners/trajopt/test/trajectory_test.test rename to moveit_planners/trajopt/trajopt_interface/test/trajectory_test.test diff --git a/moveit_planners/trajopt/trajopt_interface_plugin_description.xml b/moveit_planners/trajopt/trajopt_interface/trajopt_interface_plugin_description.xml similarity index 100% rename from moveit_planners/trajopt/trajopt_interface_plugin_description.xml rename to moveit_planners/trajopt/trajopt_interface/trajopt_interface_plugin_description.xml From b7d5ec8bf71242d390279054786a5c00b954b4dd Mon Sep 17 00:00:00 2001 From: omid1366 Date: Thu, 15 Aug 2019 17:52:42 -0600 Subject: [PATCH 569/612] resolved conflicts --- .../trajopt_interface/kinematic_terms.h~HEAD | 93 -- .../problem_description.h~HEAD | 367 ------- .../trajopt_interface.h~HEAD | 77 -- .../trajopt_planning_context.h~HEAD | 32 - .../trajopt/trajopt/CMakeLists.txt | 70 ++ moveit_planners/trajopt/trajopt/LICENSE | 11 + .../trajopt/trajopt/include/trajopt/cache.hxx | 26 + .../trajopt/include/trajopt/common.hpp | 3 + .../trajopt/include/trajopt/kinematic_terms.h | 4 +- .../include/trajopt/problem_description.h | 8 +- .../include/trajopt/trajectory_costs.hpp | 533 ++++++++++ .../trajopt/include/trajopt/typedefs.hpp | 77 ++ .../trajopt/trajopt/include/trajopt/utils.hpp | 153 +++ moveit_planners/trajopt/trajopt/package.xml | 28 + .../trajopt/trajopt/src/kinematic_terms.cpp | 6 +- .../trajopt/src/problem_description.cpp | 9 +- .../trajopt/trajopt/src/trajectory_costs.cpp | 955 ++++++++++++++++++ moveit_planners/trajopt/trajopt/src/utils.cpp | 90 ++ .../trajopt/trajopt_interface/CMakeLists.txt | 37 +- .../trajopt/trajopt_interface/README.md | 2 +- .../trajopt_interface/trajopt_interface.h | 12 +- .../trajopt_planning_context.h | 2 +- .../src/trajopt_interface.cpp | 61 +- .../trajopt_sco/3rdpartylib/bpmpd_linux32.a | Bin 0 -> 532260 bytes .../trajopt_sco/3rdpartylib/bpmpd_linux64.a | Bin 0 -> 740972 bytes .../trajopt/trajopt_sco/CMakeLists.txt | 138 +++ moveit_planners/trajopt/trajopt_sco/LICENSE | 11 + .../cmake/Modules/FindGUROBI.cmake | 43 + .../cmake/Modules/FindqpOASES.cmake | 51 + .../include/trajopt_sco/bpmpd_interface.hpp | 40 + .../include/trajopt_sco/bpmpd_io.hpp | 168 +++ .../include/trajopt_sco/expr_op_overloads.hpp | 40 + .../include/trajopt_sco/expr_ops.hpp | 163 +++ .../include/trajopt_sco/expr_vec_ops.hpp | 30 + .../include/trajopt_sco/gurobi_interface.hpp | 53 + .../include/trajopt_sco/modeling.hpp | 239 +++++ .../include/trajopt_sco/modeling_utils.hpp | 108 ++ .../include/trajopt_sco/num_diff.hpp | 72 ++ .../include/trajopt_sco/optimizers.hpp | 128 +++ .../include/trajopt_sco/osqp_interface.hpp | 84 ++ .../include/trajopt_sco/qpoases_interface.hpp | 95 ++ .../include/trajopt_sco/sco_common.hpp | 52 + .../include/trajopt_sco/sco_fwd.hpp | 48 + .../include/trajopt_sco/solver_interface.hpp | 206 ++++ .../include/trajopt_sco/solver_utils.hpp | 154 +++ .../trajopt/trajopt_sco/package.xml | 17 + .../trajopt/trajopt_sco/src/bpmpd.par | 189 ++++ .../trajopt/trajopt_sco/src/bpmpd_caller.cpp | 102 ++ .../trajopt_sco/src/bpmpd_interface.cpp | 521 ++++++++++ .../trajopt/trajopt_sco/src/expr_ops.cpp | 103 ++ .../trajopt/trajopt_sco/src/expr_vec_ops.cpp | 12 + .../trajopt_sco/src/gurobi_interface.cpp | 306 ++++++ .../trajopt/trajopt_sco/src/modeling.cpp | 266 +++++ .../trajopt_sco/src/modeling_utils.cpp | 266 +++++ .../trajopt/trajopt_sco/src/num_diff.cpp | 118 +++ .../trajopt/trajopt_sco/src/optimizers.cpp | 491 +++++++++ .../trajopt_sco/src/osqp_interface.cpp | 270 +++++ .../trajopt_sco/src/qpoases_interface.cpp | 251 +++++ .../trajopt_sco/src/solver_interface.cpp | 305 ++++++ .../trajopt/trajopt_sco/src/solver_utils.cpp | 134 +++ .../trajopt_sco/test/small-problems-unit.cpp | 175 ++++ .../test/solver-interface-unit.cpp | 244 +++++ .../trajopt_sco/test/solver-utils-unit.cpp | 261 +++++ .../trajopt/trajopt_sco/test/unit.cpp | 10 + .../trajopt/trajopt_utils/CMakeLists.txt | 49 + moveit_planners/trajopt/trajopt_utils/LICENSE | 11 + .../include/trajopt_utils/basic_array.hpp | 102 ++ .../include/trajopt_utils/clock.hpp | 7 + .../include/trajopt_utils/config.hpp | 63 ++ .../trajopt_utils/eigen_conversions.hpp | 18 + .../include/trajopt_utils/eigen_slicing.hpp | 27 + .../include/trajopt_utils/interpolation.hpp | 48 + .../include/trajopt_utils/logging.hpp | 71 ++ .../include/trajopt_utils/macros.h | 71 ++ .../include/trajopt_utils/math.hpp | 10 + .../include/trajopt_utils/stl_to_string.hpp | 73 ++ .../include/trajopt_utils/vector_ops.hpp | 17 + .../trajopt/trajopt_utils/package.xml | 16 + .../trajopt/trajopt_utils/src/clock.cpp | 40 + .../trajopt/trajopt_utils/src/config.cpp | 27 + .../trajopt/trajopt_utils/src/logging.cpp | 54 + .../trajopt_utils/src/stl_to_string.cpp | 24 + .../trajopt_utils/src/test_logging.cpp | 12 + 83 files changed, 8708 insertions(+), 652 deletions(-) delete mode 100644 moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h~HEAD delete mode 100644 moveit_planners/trajopt/include/trajopt_interface/problem_description.h~HEAD delete mode 100644 moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h~HEAD delete mode 100644 moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h~HEAD create mode 100644 moveit_planners/trajopt/trajopt/CMakeLists.txt create mode 100644 moveit_planners/trajopt/trajopt/LICENSE create mode 100644 moveit_planners/trajopt/trajopt/include/trajopt/cache.hxx create mode 100644 moveit_planners/trajopt/trajopt/include/trajopt/common.hpp create mode 100644 moveit_planners/trajopt/trajopt/include/trajopt/trajectory_costs.hpp create mode 100644 moveit_planners/trajopt/trajopt/include/trajopt/typedefs.hpp create mode 100644 moveit_planners/trajopt/trajopt/include/trajopt/utils.hpp create mode 100644 moveit_planners/trajopt/trajopt/package.xml create mode 100644 moveit_planners/trajopt/trajopt/src/trajectory_costs.cpp create mode 100644 moveit_planners/trajopt/trajopt/src/utils.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/3rdpartylib/bpmpd_linux32.a create mode 100644 moveit_planners/trajopt/trajopt_sco/3rdpartylib/bpmpd_linux64.a create mode 100644 moveit_planners/trajopt/trajopt_sco/CMakeLists.txt create mode 100644 moveit_planners/trajopt/trajopt_sco/LICENSE create mode 100644 moveit_planners/trajopt/trajopt_sco/cmake/Modules/FindGUROBI.cmake create mode 100644 moveit_planners/trajopt/trajopt_sco/cmake/Modules/FindqpOASES.cmake create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_op_overloads.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_ops.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_vec_ops.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/modeling.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/modeling_utils.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/num_diff.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/optimizers.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/osqp_interface.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/sco_common.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/sco_fwd.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/solver_interface.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/solver_utils.hpp create mode 100644 moveit_planners/trajopt/trajopt_sco/package.xml create mode 100644 moveit_planners/trajopt/trajopt_sco/src/bpmpd.par create mode 100644 moveit_planners/trajopt/trajopt_sco/src/bpmpd_caller.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/src/bpmpd_interface.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/src/expr_ops.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/src/expr_vec_ops.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/src/gurobi_interface.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/src/modeling.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/src/modeling_utils.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/src/num_diff.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/src/optimizers.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/src/osqp_interface.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/src/qpoases_interface.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/src/solver_interface.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/src/solver_utils.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/test/small-problems-unit.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/test/solver-interface-unit.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/test/solver-utils-unit.cpp create mode 100644 moveit_planners/trajopt/trajopt_sco/test/unit.cpp create mode 100644 moveit_planners/trajopt/trajopt_utils/CMakeLists.txt create mode 100644 moveit_planners/trajopt/trajopt_utils/LICENSE create mode 100644 moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/basic_array.hpp create mode 100644 moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/clock.hpp create mode 100644 moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/config.hpp create mode 100644 moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/eigen_conversions.hpp create mode 100644 moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/eigen_slicing.hpp create mode 100644 moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/interpolation.hpp create mode 100644 moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/logging.hpp create mode 100644 moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/macros.h create mode 100644 moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/math.hpp create mode 100644 moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/stl_to_string.hpp create mode 100644 moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/vector_ops.hpp create mode 100644 moveit_planners/trajopt/trajopt_utils/package.xml create mode 100644 moveit_planners/trajopt/trajopt_utils/src/clock.cpp create mode 100644 moveit_planners/trajopt/trajopt_utils/src/config.cpp create mode 100644 moveit_planners/trajopt/trajopt_utils/src/logging.cpp create mode 100644 moveit_planners/trajopt/trajopt_utils/src/stl_to_string.cpp create mode 100644 moveit_planners/trajopt/trajopt_utils/src/test_logging.cpp diff --git a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h~HEAD b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h~HEAD deleted file mode 100644 index 96274cf9c7..0000000000 --- a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h~HEAD +++ /dev/null @@ -1,93 +0,0 @@ -#pragma once - -#include - -#include - -namespace trajopt_interface -{ -/** - * @brief Extracts the vector part of quaternion - */ -inline Eigen::Vector3d quaternionRotationVector(const Eigen::Matrix3d& matrix) -{ - Eigen::Quaterniond quaternion; - quaternion = matrix; - return Eigen::Vector3d(quaternion.x(), quaternion.y(), quaternion.z()); -} - -/** - * @brief Appends b to a of type VectorXd - */ -inline Eigen::VectorXd concatVector(const Eigen::VectorXd& vector_a, const Eigen::VectorXd& vector_b) -{ - Eigen::VectorXd out(vector_a.size() + vector_b.size()); - out.topRows(vector_a.size()) = vector_a; - out.middleRows(vector_a.size(), vector_b.size()) = vector_b; - return out; -} - -/** - * @brief Appends b to a of type T - */ -template -inline std::vector concatVector(const std::vector& vector_a, const std::vector& vector_b) -{ - std::vector out; - out.insert(out.end(), vector_a.begin(), vector_a.end()); - out.insert(out.end(), vector_b.begin(), vector_b.end()); - return out; -} - -/** - * @brief Used to calculate the error for StaticCartPoseTermInfo - * This is converted to a cost or constraint using TrajOptCostFromErrFunc or TrajOptConstraintFromErrFunc - */ -struct CartPoseErrCalculator : public sco::VectorOfVector -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Eigen::Isometry3d target_pose_inv_; - planning_scene::PlanningSceneConstPtr planning_scene_; - std::string link_; - Eigen::Isometry3d tcp_; - - CartPoseErrCalculator(const Eigen::Isometry3d& pose, const planning_scene::PlanningSceneConstPtr planning_scene, - const std::string& link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) - : target_pose_inv_(pose.inverse()), planning_scene_(planning_scene), link_(link), tcp_(tcp) - { - } - - Eigen::VectorXd operator()(const Eigen::VectorXd& dof_vals) const override; -}; - -// TODO(omid): The following should be added and adjusted from trajopt -// JointPosEqCost -// JointPosIneqCost -// JointPosEqConstraint -// JointPosIneqConstraint - -struct JointVelErrCalculator : sco::VectorOfVector -{ - /** @brief Velocity target */ - double target_; - /** @brief Upper tolerance */ - double upper_tol_; - /** @brief Lower tolerance */ - double lower_tol_; - JointVelErrCalculator() : target_(0.0), upper_tol_(0.0), lower_tol_(0.0) - { - } - JointVelErrCalculator(double target, double upper_tol, double lower_tol) - : target_(target), upper_tol_(upper_tol), lower_tol_(lower_tol) - { - } - - Eigen::VectorXd operator()(const Eigen::VectorXd& var_vals) const; -}; - -struct JointVelJacobianCalculator : sco::MatrixOfVector -{ - Eigen::MatrixXd operator()(const Eigen::VectorXd& var_vals) const; -}; - -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h~HEAD b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h~HEAD deleted file mode 100644 index 825c82262d..0000000000 --- a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h~HEAD +++ /dev/null @@ -1,367 +0,0 @@ -#pragma once -#include -#include -#include - -#include -#include - -#include - -namespace trajopt_interface -{ -/** -@brief Used to apply cost/constraint to joint-space velocity - -Term is applied to every step between first_step and last_step. It applies two limits, upper_limits/lower_limits, -to the joint velocity subject to the following cases. - -* term_type = TT_COST -** upper_limit = lower_limit = 0 - Cost is applied with a SQUARED error scaled for each joint by coeffs -** upper_limit != lower_limit - 2 hinge costs are applied scaled for each joint by coeffs. If velocity < upper_limit and -velocity > lower_limit, no penalty. - -* term_type = TT_CNT -** upper_limit = lower_limit = 0 - Equality constraint is applied -** upper_limit != lower_limit - 2 Inequality constraints are applied. These are both satisfied when velocity < -upper_limit and velocity > lower_limit - -Note: coeffs, upper_limits, and lower_limits are optional. If a term is not given it will default to 0 for all joints. -If one value is given, this will be broadcast to all joints. - -Note: Velocity is calculated numerically using forward finite difference - -\f{align*}{ - cost = \sum_{t=0}^{T-2} \sum_j c_j (x_{t+1,j} - x_{t,j})^2 -\f} -where j indexes over DOF, and \f$c_j\f$ are the coeffs. -*/ - -struct TermInfo; -MOVEIT_CLASS_FORWARD(TermInfo); - -class TrajOptProblem; -MOVEIT_CLASS_FORWARD(TrajOptProblem); - -struct JointPoseTermInfo; -MOVEIT_CLASS_FORWARD(JointPoseTermInfo); - -struct CartPoseTermInfo; -MOVEIT_CLASS_FORWARD(CartPoseTermInfo); - -struct JointVelTermInfo; -MOVEIT_CLASS_FORWARD(JointVelTermInfo); - -struct ProblemInfo; -TrajOptProblemPtr ConstructProblem(const ProblemInfo&); - -enum TermType -{ - TT_COST = 0x1, // 0000 0001 - TT_CNT = 0x2, // 0000 0010 - TT_USE_TIME = 0x4, // 0000 0100 -}; - -struct BasicInfo -{ - /** @brief If true, first time step is fixed with a joint level constraint - If this is false, the starting point of the trajectory will - not be the current position of the robot. The use case example is: if we are trying to execute a process like - sanding the critical part which is the actual process path not how we get to the start of the process path. So we - plan the - process path first leaving the start free to hopefully get the most optimal and then we plan from the current - location with - start fixed to the start of the process path. It depends on what we want the default behavior to be - */ - bool start_fixed; - /** @brief Number of time steps (rows) in the optimization matrix */ - int n_steps; - sco::IntVec dofs_fixed; // optional - sco::ModelType convex_solver; // which convex solver to use - - /** @brief If true, the last column in the optimization matrix will be 1/dt */ - bool use_time = false; - /** @brief The upper limit of 1/dt values allowed in the optimization*/ - double dt_upper_lim = 1.0; - /** @brief The lower limit of 1/dt values allowed in the optimization*/ - double dt_lower_lim = 1.0; -}; - -/** -Initialization info read from json -*/ -struct InitInfo -{ - /** @brief Methods of initializing the optimization matrix. This defines how robot moves from the current - state to the start state - - STATIONARY: Initializes all joint values to the initial value (the current value in the env) - JOINT_INTERPOLATED: Linearly interpolates between initial value and the joint position specified in InitInfo.data - GIVEN_TRAJ: Initializes the matrix to a given trajectory - - In all cases the dt column (if present) is appended the selected method is defined. - */ - enum Type - { - STATIONARY, - JOINT_INTERPOLATED, - GIVEN_TRAJ, - }; - /** @brief Specifies the type of initialization to use */ - Type type; - /** @brief Data used during initialization. Use depends on the initialization selected. This data will be used - to create initialization matrix. We need to give the goal information to this init info - */ - trajopt::TrajArray data; - // Eigen::VectorXd data_vec; - // trajopt::TrajArray data_trajectory; - /** @brief Default value the final column of the optimization is initialized too if time is being used */ - double dt = 1.0; -}; - -/** -When cost or constraint element of JSON doc is read, one of these guys gets -constructed to hold the parameters. -Then it later gets converted to a Cost object by the addObjectiveTerms method -*/ -struct TermInfo -{ - std::string name; - int term_type; - int getSupportedTypes() - { - return supported_term_types_; - } - // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; - virtual void addObjectiveTerms(TrajOptProblem& prob) = 0; - - static TermInfoPtr fromName(const std::string& type); - - /** - * Registers a user-defined TermInfo so you can use your own cost - * see function RegisterMakers.cpp - */ - using MakerFunc = TermInfoPtr (*)(void); - static void RegisterMaker(const std::string& type, MakerFunc); - - virtual ~TermInfo() = default; - -protected: - TermInfo(int supported_term_types) : supported_term_types_(supported_term_types) - { - } - -private: - static std::map name_to_maker_; - int supported_term_types_; -}; - -struct ProblemInfo -{ -public: - BasicInfo basic_info; - sco::BasicTrustRegionSQPParameters opt_info; - std::vector cost_infos; - std::vector cnt_infos; - InitInfo init_info; - - planning_scene::PlanningSceneConstPtr planning_scene; - std::string planning_group_name; - - ProblemInfo(planning_scene::PlanningSceneConstPtr ps, const std::string& pg) - : planning_scene(ps), planning_group_name(pg) - { - } -}; - -/** - * Holds all the data for a trajectory optimization problem - * so you can modify it programmatically, e.g. add your own costs - */ -class TrajOptProblem : public sco::OptProb -{ -public: - TrajOptProblem(); - TrajOptProblem(const ProblemInfo& problem_info); - virtual ~TrajOptProblem() = default; - /** @brief Returns the values of the specified joints (start_col to num_col) for the specified timestep i.*/ - sco::VarVector GetVarRow(int i, int start_col, int num_col) - { - return matrix_traj_vars.rblock(i, start_col, num_col); - } - /** @brief Returns the values of all joints for the specified timestep i.*/ - sco::VarVector GetVarRow(int i) - { - return matrix_traj_vars.row(i); - } - /** @brief Returns the value of the specified joint j for the specified timestep i.*/ - sco::Var& GetVar(int i, int j) - { - return matrix_traj_vars.at(i, j); - } - trajopt::VarArray& GetVars() - { - return matrix_traj_vars; - } - /** @brief Returns the number of steps in the problem. This is the number of rows in the optimization matrix.*/ - int GetNumSteps() - { - return matrix_traj_vars.rows(); - } - /** @brief Returns the problem DOF. This is the number of columns in the optization matrix. - * Note that this is not necessarily the same as the kinematic DOF.*/ - int GetNumDOF() - { - return matrix_traj_vars.cols(); - } - /** @brief Returns the kinematic DOF of the active joint model group - */ - int GetActiveGroupNumDOF() - { - return dof_; - } - planning_scene::PlanningSceneConstPtr GetPlanningScene() - { - return planning_scene_; - } - void SetInitTraj(const trajopt::TrajArray& x) - { - matrix_init_traj = x; - } - trajopt::TrajArray GetInitTraj() - { - return matrix_init_traj; - } - // friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&); - /** @brief Returns TrajOptProb.has_time */ - bool GetHasTime() - { - return has_time; - } - /** @brief Sets TrajOptProb.has_time */ - void SetHasTime(bool tmp) - { - has_time = tmp; - } - -private: - /** @brief If true, the last column in the optimization matrix will be 1/dt */ - bool has_time; - /** @brief is the matrix holding the joint values of the trajectory for all timesteps */ - trajopt::VarArray matrix_traj_vars; - planning_scene::PlanningSceneConstPtr planning_scene_; - std::string planning_group_; - /** @brief Kinematic DOF of the active joint model group */ - int dof_; - trajopt::TrajArray matrix_init_traj; -}; - -/** @brief This term is used when the goal frame is fixed in cartesian space - - Set term_type == TT_COST or TT_CNT for cost or constraint. -*/ -struct CartPoseTermInfo : public TermInfo -{ - // EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** @brief Timestep at which to apply term */ - int timestep; - /** @brief Cartesian position */ - Eigen::Vector3d xyz; - /** @brief Rotation quaternion */ - Eigen::Vector4d wxyz; - /** @brief coefficients for position and rotation */ - Eigen::Vector3d pos_coeffs, rot_coeffs; - /** @brief Link which should reach desired pose */ - std::string link; - /** @brief Static transform applied to the link */ - Eigen::Isometry3d tcp; - - CartPoseTermInfo(); - - /** @brief Used to add term to pci from json */ - // void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; - /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void addObjectiveTerms(TrajOptProblem& prob) override; - - static TermInfoPtr create() - { - TermInfoPtr out(new CartPoseTermInfo()); - return out; - } -}; - -/** - \brief Joint space position cost - Position operates on a single point (unlike velocity, etc). This is b/c the primary usecase is joint-space - position waypoints - - \f{align*}{ - \sum_i c_i (x_i - xtarg_i)^2 - \f} - where \f$i\f$ indexes over dof and \f$c_i\f$ are coeffs - */ -struct JointPoseTermInfo : public TermInfo -{ - /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec coeffs; - /** @brief Vector of position targets. This is a required value. Size should be the DOF of the system */ - trajopt::DblVec targets; - /** @brief Vector of position upper limits. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec upper_tols; - /** @brief Vector of position lower limits. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec lower_tols; - /** @brief First time step to which the term is applied. Default: 0 */ - int first_step = 0; - /** @brief Last time step to which the term is applied. Default: prob.GetNumSteps() - 1*/ - int last_step = -1; - - /** @brief Initialize term with it's supported types */ - JointPoseTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) - { - } - - /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void addObjectiveTerms(TrajOptProblem& prob) override; - - static TermInfoPtr create() - { - TermInfoPtr out(new JointPoseTermInfo()); - return out; - } -}; - -struct JointVelTermInfo : public TermInfo -{ - /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec coeffs; - /** @brief Vector of velocity targets. This is a required value. Size should be the DOF of the system */ - trajopt::DblVec targets; - /** @brief Vector of velocity upper limits. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec upper_tols; - /** @brief Vector of velocity lower limits. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec lower_tols; - /** @brief First time step to which the term is applied. Default: 0*/ - int first_step = 0; - /** @brief Last time step to which the term is applied. Default: prob.GetNumSteps() - 1*/ - int last_step = -1; - - /** @brief Initialize term with it's supported types */ - JointVelTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) - { - } - - /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void addObjectiveTerms(TrajOptProblem& prob) override; - - static TermInfoPtr create() - { - TermInfoPtr out(new JointVelTermInfo()); - return out; - } -}; - -void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, - trajopt::TrajArray& init_traj); - -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h~HEAD b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h~HEAD deleted file mode 100644 index 74432fa83c..0000000000 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h~HEAD +++ /dev/null @@ -1,77 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, PickNik, LLC. - * 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 PickNik 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. - *********************************************************************/ - -/* Author: Omid Heidari */ -#pragma once - -#include -#include -#include -#include "problem_description.h" - -namespace trajopt_interface -{ -MOVEIT_CLASS_FORWARD(TrajOptInterface); - -class TrajOptInterface -{ -public: - TrajOptInterface(const ros::NodeHandle& nh = ros::NodeHandle("~")); - - const sco::BasicTrustRegionSQPParameters& getParams() const - { - return params_; - } - - bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, moveit_msgs::MotionPlanDetailedResponse& res); - -protected: - /** @brief Configure everything using the param server */ - void setTrajOptParams(sco::BasicTrustRegionSQPParameters& param); - void setDefaultTrajOPtParams(); - void setProblemInfoParam(ProblemInfo& problem_info); - void setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name); - trajopt::DblVec extractStartJointValues(const planning_interface::MotionPlanRequest& req, - const std::vector& group_joint_names); - - ros::NodeHandle nh_; /// The ROS node handle - sco::BasicTrustRegionSQPParameters params_; - std::vector optimizer_callbacks_; - TrajOptProblemPtr trajopt_problem_; - std::string name_; -}; - -void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res); -} diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h~HEAD b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h~HEAD deleted file mode 100644 index 12676f57e7..0000000000 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h~HEAD +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once - -#include - -#include -#include - -namespace trajopt_interface -{ -MOVEIT_CLASS_FORWARD(TrajOptPlanningContext); - -class TrajOptPlanningContext : public planning_interface::PlanningContext -{ -public: - TrajOptPlanningContext(const std::string& name, const std::string& group, - const robot_model::RobotModelConstPtr& model); - ~TrajOptPlanningContext() override - { - } - - bool solve(planning_interface::MotionPlanResponse& res) override; - bool solve(planning_interface::MotionPlanDetailedResponse& res) override; - - bool terminate() override; - void clear() override; - -private: - moveit::core::RobotModelConstPtr robot_model_; - - TrajOptInterfacePtr trajopt_interface_; -}; -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/trajopt/CMakeLists.txt b/moveit_planners/trajopt/trajopt/CMakeLists.txt new file mode 100644 index 0000000000..3e6b065bc2 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 2.8.3) +project(trajopt) + +add_compile_options(-std=c++11 -Wall -Wextra) + +find_package(catkin REQUIRED COMPONENTS + trajopt_sco + trajopt_utils + roscpp +) + +find_package(Eigen3 REQUIRED) +find_package(Boost COMPONENTS system python thread program_options REQUIRED) + +find_package(PkgConfig REQUIRED) +pkg_check_modules(JSONCPP jsoncpp) + +link_directories( + /usr/local/lib + /usr/local/lib64 +) + +set(TRAJOPT_SOURCE_FILES + src/trajectory_costs.cpp + src/kinematic_terms.cpp + src/problem_description.cpp + src/utils.cpp +) + +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIRS} + LIBRARIES + ${PROJECT_NAME} + ${JSONCPP_LIBRARIES} + CATKIN_DEPENDS + trajopt_sco + trajopt_utils + roscpp + DEPENDS + EIGEN3 + Boost +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + SYSTEM ${EIGEN3_INCLUDE_DIRS} + SYSTEM ${Boost_INCLUDE_DIRS} + SYSTEM ${JSONCPP_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} ${TRAJOPT_SOURCE_FILES}) +target_link_libraries(${PROJECT_NAME} ${Boost_SYSTEM_LIBRARY} ${JSONCPP_LIBRARIES} ${catkin_LIBRARIES}) +target_compile_options(${PROJECT_NAME} PRIVATE -Wsuggest-override -Wconversion -Wsign-conversion) + +# Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" PATTERN "*.hxx" + PATTERN ".svn" EXCLUDE + ) diff --git a/moveit_planners/trajopt/trajopt/LICENSE b/moveit_planners/trajopt/trajopt/LICENSE new file mode 100644 index 0000000000..9103917b0f --- /dev/null +++ b/moveit_planners/trajopt/trajopt/LICENSE @@ -0,0 +1,11 @@ +Copyright (c) 2013, John Schulman +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. + +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 HOLDER 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. + +http://opensource.org/licenses/BSD-2-Clause \ No newline at end of file diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/cache.hxx b/moveit_planners/trajopt/trajopt/include/trajopt/cache.hxx new file mode 100644 index 0000000000..7fc8edaa1c --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/cache.hxx @@ -0,0 +1,26 @@ +#pragma once + +template +class Cache +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + KeyT keybuf[bufsize]; // circular buffer + ValueT valbuf[bufsize]; // circular buffer + int m_i; + Cache() : m_i(0) { memset(keybuf, 666, sizeof(keybuf)); } + void put(const KeyT& key, const ValueT& value) + { + keybuf[m_i] = key; + valbuf[m_i] = value; + ++m_i; + if (m_i == bufsize) + m_i = 0; + } + ValueT* get(const KeyT& key) + { + KeyT* it = std::find(&keybuf[0], &keybuf[0] + bufsize, key); + return (it == &keybuf[0] + bufsize) ? nullptr : &valbuf[it - &keybuf[0]]; + } +}; diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/common.hpp b/moveit_planners/trajopt/trajopt/include/trajopt/common.hpp new file mode 100644 index 0000000000..41b82a6046 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/common.hpp @@ -0,0 +1,3 @@ +#pragma once +#include +#include diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/kinematic_terms.h b/moveit_planners/trajopt/trajopt/include/trajopt/kinematic_terms.h index 96274cf9c7..8149180af3 100644 --- a/moveit_planners/trajopt/trajopt/include/trajopt/kinematic_terms.h +++ b/moveit_planners/trajopt/trajopt/include/trajopt/kinematic_terms.h @@ -4,7 +4,7 @@ #include -namespace trajopt_interface +namespace trajopt { /** * @brief Extracts the vector part of quaternion @@ -90,4 +90,4 @@ struct JointVelJacobianCalculator : sco::MatrixOfVector Eigen::MatrixXd operator()(const Eigen::VectorXd& var_vals) const; }; -} // namespace trajopt_interface +} // namespace trajopt diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/problem_description.h b/moveit_planners/trajopt/trajopt/include/trajopt/problem_description.h index 6ad995e4ff..9e56462cac 100644 --- a/moveit_planners/trajopt/trajopt/include/trajopt/problem_description.h +++ b/moveit_planners/trajopt/trajopt/include/trajopt/problem_description.h @@ -1,6 +1,6 @@ #pragma once + #include -#include #include #include @@ -8,7 +8,7 @@ #include -namespace trajopt_interface +namespace trajopt { /** @brief Used to apply cost/constraint to joint-space velocity @@ -109,12 +109,12 @@ struct InitInfo }; /** @brief Specifies the type of initialization to use */ Type type; + /** @brief Data used during initialization. Use depends on the initialization selected. This data will be used to create initialization matrix. We need to give the goal information to this init info */ trajopt::TrajArray data; - // Eigen::VectorXd data_vec; - // trajopt::TrajArray data_trajectory; + /** @brief Default value the final column of the optimization is initialized too if time is being used */ double dt = 1.0; }; diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/trajectory_costs.hpp b/moveit_planners/trajopt/trajopt/include/trajopt/trajectory_costs.hpp new file mode 100644 index 0000000000..5741151f74 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/trajectory_costs.hpp @@ -0,0 +1,533 @@ +#pragma once + +/** +Simple quadratic costs on trajectory +*/ + +#include +#include + +#include "trajopt/common.hpp" + +namespace trajopt +{ +class TRAJOPT_API JointPosEqCost : public sco::Cost +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointPosEqCost(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the cost as an expression */ + sco::QuadExpr expr_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +/** + * @brief The JointVelIneqCost class + * Assumes that the target is ... + */ +class TRAJOPT_API JointPosIneqCost : public sco::Cost +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointPosIneqCost(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, + const Eigen::VectorXd& lower_limits, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*num_timesteps*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointPosEqConstraint : public sco::EqConstraint +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointPosEqConstraint(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targetss, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the costs as an expression. Will be length num_jnts*num_timesteps */ + std::vector expr_vec_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointPosIneqConstraint : public sco::IneqConstraint +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointPosIneqConstraint(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, + const Eigen::VectorXd& lower_limits, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*num_timesteps*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointVelEqCost : public sco::Cost +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointVelEqCost(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values using Eigen*/ + double value(const DblVec&) override; + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the cost as an expression */ + sco::QuadExpr expr_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointVelIneqCost : public sco::Cost +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointVelIneqCost(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, + const Eigen::VectorXd& lower_limits, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*num_timesteps*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointVelEqConstraint : public sco::EqConstraint +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointVelEqConstraint(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-1) */ + std::vector expr_vec_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointVelIneqConstraint : public sco::IneqConstraint +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointVelIneqConstraint(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, + const Eigen::VectorXd& lower_limits, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-1)*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointAccEqCost : public sco::Cost +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointAccEqCost(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the cost as an expression */ + sco::QuadExpr expr_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointAccIneqCost : public sco::Cost +{ +public: + /** @brief Forms error in a vector of AffExpr - independent of penalty type */ + JointAccIneqCost(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, + const Eigen::VectorXd& lower_limits, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of acceleration targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-2)*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointAccEqConstraint : public sco::EqConstraint +{ +public: + /** @brief Forms error in a vector of AffExpr - independent of penalty type */ + JointAccEqConstraint(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-2) */ + std::vector expr_vec_; + /** @brief Vector of acceleration targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointAccIneqConstraint : public sco::IneqConstraint +{ +public: + /** @brief Forms error in a vector of AffExpr - independent of penalty type */ + JointAccIneqConstraint(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, + const Eigen::VectorXd& lower_limits, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of acceleration targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-2)*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointJerkEqCost : public sco::Cost +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointJerkEqCost(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the cost as an expression */ + sco::QuadExpr expr_; + /** @brief Vector of jerk targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointJerkIneqCost : public sco::Cost +{ +public: + /** @brief Forms error in a vector of AffExpr - independent of penalty type */ + JointJerkIneqCost(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, + const Eigen::VectorXd& lower_limits, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of jerk targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-4)*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointJerkEqConstraint : public sco::EqConstraint +{ +public: + /** @brief Forms error in a vector of AffExpr - independent of penalty type */ + JointJerkEqConstraint(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-4) */ + std::vector expr_vec_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointJerkIneqConstraint : public sco::IneqConstraint +{ +public: + /** @brief Forms error in a vector of AffExpr - independent of penalty type */ + JointJerkIneqConstraint(const VarArray& traj, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, + const Eigen::VectorXd& lower_limits, + int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + sco::VarVector getVars() override { return vars_.flatten(); } +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-4)*2 */ + std::vector expr_vec_; +}; +} // namespace trajopt diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/typedefs.hpp b/moveit_planners/trajopt/trajopt/include/trajopt/typedefs.hpp new file mode 100644 index 0000000000..28b06f3054 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/typedefs.hpp @@ -0,0 +1,77 @@ +#pragma once +#include +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include + +namespace trajopt +{ +typedef util::BasicArray VarArray; +typedef util::BasicArray AffArray; +typedef util::BasicArray CntArray; +typedef Eigen::Matrix DblMatrix; +typedef Eigen::Matrix TrajArray; +typedef sco::DblVec DblVec; +typedef sco::IntVec IntVec; + + +/** @brief Adds plotting to the CostFromErrFunc class in trajopt_sco */ +class TrajOptCostFromErrFunc : public sco::CostFromErrFunc +{ +public: + /// supply error function, obtain derivative numerically + TrajOptCostFromErrFunc(sco::VectorOfVectorPtr f, + const sco::VarVector& vars, + const Eigen::VectorXd& coeffs, + sco::PenaltyType pen_type, + const std::string& name) + : CostFromErrFunc(f, vars, coeffs, pen_type, name) + { + } + + /// supply error function and gradient + TrajOptCostFromErrFunc(sco::VectorOfVectorPtr f, + sco::MatrixOfVectorPtr dfdx, + const sco::VarVector& vars, + const Eigen::VectorXd& coeffs, + sco::PenaltyType pen_type, + const std::string& name) + : CostFromErrFunc(f, dfdx, vars, coeffs, pen_type, name) + { + } + +}; + +/** @brief Adds plotting to the ConstraintFromFunc class in trajopt_sco */ +class TrajOptConstraintFromErrFunc : public sco::ConstraintFromErrFunc +{ +public: + /// supply error function, obtain derivative numerically + TrajOptConstraintFromErrFunc(sco::VectorOfVectorPtr f, + const sco::VarVector& vars, + const Eigen::VectorXd& coeffs, + sco::ConstraintType type, + const std::string& name) + : ConstraintFromErrFunc(f, vars, coeffs, type, name) + { + } + + /// supply error function and gradient + TrajOptConstraintFromErrFunc(sco::VectorOfVectorPtr f, + sco::MatrixOfVectorPtr dfdx, + const sco::VarVector& vars, + const Eigen::VectorXd& coeffs, + sco::ConstraintType type, + const std::string& name) + : ConstraintFromErrFunc(f, dfdx, vars, coeffs, type, name) + { + } + +}; +} diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/utils.hpp b/moveit_planners/trajopt/trajopt/include/trajopt/utils.hpp new file mode 100644 index 0000000000..9d24300402 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/utils.hpp @@ -0,0 +1,153 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include + +namespace trajopt +{ +template +using AlignedUnorderedMap = std::unordered_map, + std::equal_to, + Eigen::aligned_allocator>>; + +/** +Extract trajectory array from solution vector x using indices in array vars +*/ +TrajArray TRAJOPT_API getTraj(const DblVec& x, const VarArray& vars); +TrajArray TRAJOPT_API getTraj(const DblVec& x, const AffArray& arr); + +inline DblVec trajToDblVec(const TrajArray& x) { return DblVec(x.data(), x.data() + x.rows() * x.cols()); } + +/** + * @brief Appends b to a of type VectorXd + */ +inline Eigen::VectorXd concat(const Eigen::VectorXd& a, const Eigen::VectorXd& b) +{ + Eigen::VectorXd out(a.size() + b.size()); + out.topRows(a.size()) = a; + out.middleRows(a.size(), b.size()) = b; + return out; +} + +/** + * @brief Appends b to a of type T + */ +template +std::vector concat(const std::vector& a, const std::vector& b) +{ + std::vector out; + std::vector x(a.size() + b.size()); + out.insert(out.end(), a.begin(), a.end()); + out.insert(out.end(), b.begin(), b.end()); + return out; +} + +template +std::vector singleton(const T& x) +{ + return std::vector(1, x); +} + +void TRAJOPT_API AddVarArrays(sco::OptProb& prob, + int rows, + const std::vector& cols, + const std::vector& name_prefix, + const std::vector& newvars); + +void TRAJOPT_API AddVarArray(sco::OptProb& prob, int rows, int cols, const std::string& name_prefix, VarArray& newvars); + +/** @brief Store Safety Margin Data for a given timestep */ +struct SafetyMarginData +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + SafetyMarginData(const double& default_safety_margin, const double& default_safety_margin_coeff) + : default_safety_margin_data_(default_safety_margin, default_safety_margin_coeff) + , max_safety_margin_(default_safety_margin) + { + } + + /** + * @brief Set the safety margin for a given contact pair + * + * The order of the object names does not matter, that is handled internal to + * the class. + * + * @param obj1 The first object name + * @param obj2 The Second object name + * @param safety_margins contacts with distance < safety_margin are penalized + * @param safety_margin_coeffs A safety margin coefficient vector where each + * element corresponds to a given timestep. + */ + void SetPairSafetyMarginData(const std::string& obj1, + const std::string& obj2, + const double& safety_margin, + const double& safety_margin_coeff) + { + Eigen::Vector2d data(safety_margin, safety_margin_coeff); + + pair_lookup_table_[obj1 + obj2] = data; + pair_lookup_table_[obj2 + obj1] = data; + + if (safety_margin > max_safety_margin_) + { + max_safety_margin_ = safety_margin; + } + } + + const Eigen::Vector2d& getPairSafetyMarginData(const std::string& obj1, const std::string& obj2) const + { + const std::string& key = obj1 + obj2; + auto it = pair_lookup_table_.find(key); + + if (it != pair_lookup_table_.end()) + { + return it->second; + } + else + { + return default_safety_margin_data_; + } + } + + const double& getMaxSafetyMargin() const { return max_safety_margin_; } +private: + /// The coeff used during optimization + /// safety margin: contacts with distance < dist_pen are penalized + /// Stores [dist_pen, coeff] + Eigen::Vector2d default_safety_margin_data_; + + /// This use when requesting collision data because you can only provide a + /// single contact distance threshold. + double max_safety_margin_; + + /// A map of link pair to contact distance setting [dist_pen, coeff] + AlignedUnorderedMap pair_lookup_table_; +}; +typedef std::shared_ptr SafetyMarginDataPtr; +typedef std::shared_ptr SafetyMarginDataConstPtr; + +/** + * @brief This is a utility function for creating the Safety Margin data vector + * @param num_elements The number of objects to create + * @param default_safety_margin Default safety margin + * @param default_safety_margin_coeff Default safety margin coeff + * @return A vector of Safety Margin Data + */ +inline std::vector createSafetyMarginDataVector(int num_elements, + const double& default_safety_margin, + const double& default_safety_margin_coeff) +{ + std::vector info; + info.reserve(static_cast(num_elements)); + for (auto i = 0; i < num_elements; ++i) + { + info.push_back(SafetyMarginDataPtr(new SafetyMarginData(default_safety_margin, default_safety_margin_coeff))); + } + return info; +} +} diff --git a/moveit_planners/trajopt/trajopt/package.xml b/moveit_planners/trajopt/trajopt/package.xml new file mode 100644 index 0000000000..d17422a2a7 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/package.xml @@ -0,0 +1,28 @@ + + + trajopt + 0.1.0 + The trajopt package + Levi Armstrong + BSD + Apache 2.0 + Levi Armstrong + John Schulman + + catkin + trajopt_sco + trajopt_utils + roscpp + + gtest + rostest + roslib + trajopt_test_support + libpcl-all-dev + pcl_conversions + pr2_description + + + + + diff --git a/moveit_planners/trajopt/trajopt/src/kinematic_terms.cpp b/moveit_planners/trajopt/trajopt/src/kinematic_terms.cpp index 92d75fb78b..4559e5b8a1 100644 --- a/moveit_planners/trajopt/trajopt/src/kinematic_terms.cpp +++ b/moveit_planners/trajopt/trajopt/src/kinematic_terms.cpp @@ -4,13 +4,13 @@ #include #include -#include "trajopt_interface/kinematic_terms.h" +#include "trajopt/kinematic_terms.h" using namespace std; using namespace sco; using namespace Eigen; -namespace trajopt_interface +namespace trajopt { VectorXd CartPoseErrCalculator::operator()(const VectorXd& dof_vals) const { @@ -61,4 +61,4 @@ MatrixXd JointVelJacobianCalculator::operator()(const VectorXd& var_vals) const return jac; } -} // namespace trajopt_interface +} // namespace trajopt diff --git a/moveit_planners/trajopt/trajopt/src/problem_description.cpp b/moveit_planners/trajopt/trajopt/src/problem_description.cpp index c5d263bd99..4f1625b965 100644 --- a/moveit_planners/trajopt/trajopt/src/problem_description.cpp +++ b/moveit_planners/trajopt/trajopt/src/problem_description.cpp @@ -32,6 +32,7 @@ *********************************************************************/ #include +#include #include @@ -39,7 +40,6 @@ #include #include -#include #include #include #include @@ -47,8 +47,9 @@ #include #include -#include "trajopt_interface/problem_description.h" -#include "trajopt_interface/kinematic_terms.h" +#include +#include +#include "trajopt/problem_description.h" /** * @brief Checks the size of the parameter given and throws if incorrect @@ -71,7 +72,7 @@ void checkParameterSize(trajopt::DblVec& parameter, const unsigned int& expected } } -namespace trajopt_interface +namespace trajopt { TrajOptProblem::TrajOptProblem() { diff --git a/moveit_planners/trajopt/trajopt/src/trajectory_costs.cpp b/moveit_planners/trajopt/trajopt/src/trajectory_costs.cpp new file mode 100644 index 0000000000..eaaa1b0fd4 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/src/trajectory_costs.cpp @@ -0,0 +1,955 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include + +#include "trajopt/trajectory_costs.hpp" + +namespace +{ +/** @brief Returns the difference between each row of a matrixXd and the row before */ +static Eigen::MatrixXd diffAxis0(const Eigen::MatrixXd& in) +{ + return in.middleRows(1, in.rows() - 1) - in.middleRows(0, in.rows() - 1); +} +} // namespace + +namespace trajopt +{ +//////////// Joint cost functions ///////////////// + +//////////////////// Position ///////////////////// +JointPosEqCost::JointPosEqCost(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step) + : Cost("JointPosEq"), vars_(vars), coeffs_(coeffs), targets_(targets), first_step_(first_step), last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // pos = x1 - targ + sco::AffExpr pos; + sco::exprInc(pos, sco::exprMult(vars(i, j), 1)); + sco::exprDec(pos, targets_[j]); + // expr_ = coeff * vel^2 + sco::exprInc(expr_, sco::exprMult(sco::exprSquare(pos), coeffs_[j])); + } + } +} +double JointPosEqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = (getTraj(xvec, vars_)); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = + (traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())).rowwise() - targets_.transpose(); + // Element-wise square it, multiply it by a diagonal matrix of coefficients, and sums output + return (diff.array().square().matrix() * coeffs_.asDiagonal()).sum(); +} +sco::ConvexObjectivePtr JointPosEqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + out->addQuadExpr(expr_); + return out; +} + +JointPosIneqCost::JointPosIneqCost(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, + int& first_step, + int& last_step) + : Cost("JointPosIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + sco::AffExpr expr; + sco::AffExpr expr_neg; + // pos = x1 - targ + sco::AffExpr pos; + sco::exprInc(pos, sco::exprMult(vars(i, j), 1)); + sco::exprDec(pos, targets_[j]); + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprDec(expr, pos); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprDec(expr_neg, pos); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +double JointPosIneqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd pos = traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()); + // Subtract targets to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (pos.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum(); +} + +sco::ConvexObjectivePtr JointPosIneqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addHinge(expr, 1); + } + return out; +} + +JointPosEqConstraint::JointPosEqConstraint(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step) + : EqConstraint("JointPosEq") + , vars_(vars) + , coeffs_(coeffs) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // pos = x1 - targ + sco::AffExpr pos; + sco::exprInc(pos, sco::exprMult(vars(i, j), 1)); + sco::exprDec(pos, targets_[j]); + // expr_ = coeff * vel - Not squared b/c QuadExpr cnt not yet supported (TODO) + expr_vec_.push_back(sco::exprMult(pos, coeffs_[j])); + } + } +} + +DblVec JointPosEqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = + (traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())).rowwise() - targets_.transpose(); + // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector + return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal()); +} +sco::ConvexConstraintsPtr JointPosEqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + for (sco::AffExpr& expr : expr_vec_) + { + out->addEqCnt(expr); + } + return out; +} + +JointPosIneqConstraint::JointPosIneqConstraint(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, + int& first_step, + int& last_step) + : IneqConstraint("JointPosIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + sco::AffExpr expr; + sco::AffExpr expr_neg; + // pos = x1 - targ + sco::AffExpr pos; + sco::exprInc(pos, sco::exprMult(vars(i, j), 1)); + sco::exprDec(pos, targets_[j]); + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprDec(expr, pos); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form lower limit expr = (upper_tol-(vel-targ)) + sco::exprDec(expr_neg, pos); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +DblVec JointPosIneqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd pos = diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())); + // Subtract targets to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (pos.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols()); + out << diff1, diff2; + return util::toDblVec(out); +} + +sco::ConvexConstraintsPtr JointPosIneqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addIneqCnt(expr); + } + return out; +} + +//////////////////// Velocity ///////////////////// +JointVelEqCost::JointVelEqCost(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step) + : Cost("JointVelEq"), vars_(vars), coeffs_(coeffs), targets_(targets), first_step_(first_step), last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 1; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // vel = (x2 - x1) - targ + sco::AffExpr vel; + sco::exprInc(vel, sco::exprMult(vars(i, j), -1)); + sco::exprInc(vel, sco::exprMult(vars(i + 1, j), 1)); + exprDec(vel, targets_[j]); + // expr_ = coeff * vel^2 + exprInc(expr_, exprMult(exprSquare(vel), coeffs_[j])); + } + } +} +double JointVelEqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = (getTraj(xvec, vars_)); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = (diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))).rowwise() - + targets_.transpose(); + // Element-wise square it, multiply it by a diagonal matrix of coefficients, and sums output + return (diff.array().square().matrix() * coeffs_.asDiagonal()).sum(); +} +sco::ConvexObjectivePtr JointVelEqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + out->addQuadExpr(expr_); + return out; +} + +JointVelIneqCost::JointVelIneqCost(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, + int& first_step, + int& last_step) + : Cost("JointVelIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 1; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // vel = (x2 - x1) - targ + sco::AffExpr vel; + sco::AffExpr expr; + sco::AffExpr expr_neg; + sco::exprInc(vel, sco::exprMult(vars(i, j), -1)); + sco::exprInc(vel, sco::exprMult(vars(i + 1, j), 1)); + sco::exprDec(vel, targets_[j]); // offset to center about 0 + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + sco::exprDec(expr, vel); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form lower limit expr = (upper_tol-(vel-targ)) + sco::exprInc(expr_neg, lower_tols_[j]); // expr_ = lower_tol_ + sco::exprDec(expr_neg, vel); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +double JointVelIneqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd vel = diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())); + // Subtract targets_ to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (vel.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum(); +} + +sco::ConvexObjectivePtr JointVelIneqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addHinge(expr, 1); + } + return out; +} + +JointVelEqConstraint::JointVelEqConstraint(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step) + : EqConstraint("JointVelEq") + , vars_(vars) + , coeffs_(coeffs) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 1; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // vel = (x2 - x1) - targ + sco::AffExpr vel; + sco::exprInc(vel, sco::exprMult(vars(i, j), -1)); + sco::exprInc(vel, sco::exprMult(vars(i + 1, j), 1)); + sco::exprDec(vel, targets_[j]); + // expr_ = coeff * vel - Not squared b/c QuadExpr cnt not yet supported (TODO) + expr_vec_.push_back(sco::exprMult(vel, coeffs_[j])); + } + } +} + +DblVec JointVelEqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = (diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))).rowwise() - + targets_.transpose(); + // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector + return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal()); +} +sco::ConvexConstraintsPtr JointVelEqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + for (sco::AffExpr& expr : expr_vec_) + { + out->addEqCnt(expr); + } + return out; +} + +JointVelIneqConstraint::JointVelIneqConstraint(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, + int& first_step, + int& last_step) + : IneqConstraint("JointVelIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 1; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // vel = (x2 - x1) - targ + sco::AffExpr vel; + sco::AffExpr expr; + sco::AffExpr expr_neg; + sco::exprInc(vel, sco::exprMult(vars(i, j), -1)); + sco::exprInc(vel, sco::exprMult(vars(i + 1, j), 1)); + sco::exprDec(vel, targets_[j]); // offset to center about 0 + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + sco::exprDec(expr, vel); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form lower limit expr = (lower_tol-(vel-targ)) + sco::exprInc(expr_neg, lower_tols_[j]); // expr_ = lower_tol_ + sco::exprDec(expr_neg, vel); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +DblVec JointVelIneqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd vel = diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())); + // Subtract targets_ to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (vel.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols()); + out << diff1, diff2; + return util::toDblVec(out.cwiseMax(0)); +} + +sco::ConvexConstraintsPtr JointVelIneqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addIneqCnt(expr); + } + return out; +} + +//////////////////// Acceleration ///////////////////// +JointAccEqCost::JointAccEqCost(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step) + : Cost("JointAccEq"), vars_(vars), coeffs_(coeffs), targets_(targets), first_step_(first_step), last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 2; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // acc = (x3 - 2*x2 + x1) - targ + sco::AffExpr acc; + sco::exprInc(acc, sco::exprMult(vars(i, j), 1.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 1, j), -2.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 2, j), 1.0)); + + sco::exprDec(acc, targets_[j]); + // expr_ = coeff * acc^2 + sco::exprInc(expr_, sco::exprMult(sco::exprSquare(acc), coeffs_[j])); + } + } +} +double JointAccEqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = (getTraj(xvec, vars_)); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = + (diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())))).rowwise() - + targets_.transpose(); + // Element-wise square it, multiply it by a diagonal matrix of coefficients, and sums output + return (diff.array().square().matrix() * coeffs_.asDiagonal()).sum(); +} +sco::ConvexObjectivePtr JointAccEqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + out->addQuadExpr(expr_); + return out; +} + +JointAccIneqCost::JointAccIneqCost(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, + int& first_step, + int& last_step) + : Cost("JointAccIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 2; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // acc = (x3 - 2*x2 + x1) - targ + sco::AffExpr acc; + sco::AffExpr expr; + sco::AffExpr expr_neg; + sco::exprInc(acc, sco::exprMult(vars(i, j), 1.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 1, j), -2.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 2, j), 1.0)); + sco::exprDec(acc, targets_[j]); // offset to center about 0 + + // Form upper limit expr = - (upper_tol-(acc-targ)) + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + sco::exprDec(expr, acc); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form lower limit expr = (lower_tol-(acc-targ)) + sco::exprInc(expr_neg, lower_tols_[j]); // expr_ = lower_tol_ + sco::exprDec(expr_neg, acc); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +double JointAccIneqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd acc = diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))); + // Subtract targets_ to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (acc.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum(); +} + +sco::ConvexObjectivePtr JointAccIneqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addHinge(expr, 1); + } + return out; +} + +JointAccEqConstraint::JointAccEqConstraint(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step) + : EqConstraint("JointAccEq") + , vars_(vars) + , coeffs_(coeffs) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 2; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // acc = (x3 - 2*x2 + x1) - targ + sco::AffExpr acc; + sco::exprInc(acc, sco::exprMult(vars(i, j), 1.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 1, j), -2.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 2, j), 1.0)); + + sco::exprDec(acc, targets_[j]); // offset to center about 0 + // expr_ = coeff * vel - Not squared b/c QuadExpr cnt not yet supported (TODO) + expr_vec_.push_back(sco::exprMult(acc, coeffs_[j])); + } + } +} + +DblVec JointAccEqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = + (diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())))).rowwise() - + targets_.transpose(); + // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector + return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal()); +} +sco::ConvexConstraintsPtr JointAccEqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + for (sco::AffExpr& expr : expr_vec_) + { + out->addEqCnt(expr); + } + return out; +} + +JointAccIneqConstraint::JointAccIneqConstraint(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, + int& first_step, + int& last_step) + : IneqConstraint("JointAccIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + // Form upper limit expr = - (upper_tol-(vel-targ)) + for (int i = first_step_; i <= last_step_ - 2; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // acc = (x3 - 2*x2 + x1) - targ + sco::AffExpr acc; + sco::AffExpr expr; + sco::AffExpr expr_neg; + sco::exprInc(acc, sco::exprMult(vars(i, j), 1.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 1, j), -2.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 2, j), 1.0)); + sco::exprDec(acc, targets_[j]); // offset to center about 0 + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + sco::exprDec(expr, acc); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprInc(expr_neg, lower_tols_[j]); // expr_ = lower_tol_ + sco::exprDec(expr_neg, acc); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +DblVec JointAccIneqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd acc = diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))); + // Subtract targets_ to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (acc.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols()); + out << diff1, diff2; + return util::toDblVec(out.cwiseMax(0)); +} + +sco::ConvexConstraintsPtr JointAccIneqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addIneqCnt(expr); + } + return out; +} + +//////////////////// Jerk ///////////////////// +JointJerkEqCost::JointJerkEqCost(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step) + : Cost("JointJerkEq"), vars_(vars), coeffs_(coeffs), targets_(targets), first_step_(first_step), last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 4; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + sco::AffExpr jerk; + sco::exprInc(jerk, sco::exprMult(vars(i, j), -1.0 / 2.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 1, j), 1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 2, j), 0.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 3, j), -1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 4, j), 1.0 / 2.0)); + + sco::exprDec(jerk, targets_[j]); + // expr_ = coeff * jerk^2 + sco::exprInc(expr_, sco::exprMult(sco::exprSquare(jerk), coeffs_[j])); + } + } +} +double JointJerkEqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = (getTraj(xvec, vars_)); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = + (diffAxis0(diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))))) + .rowwise() - + targets_.transpose(); + // Element-wise square it, multiply it by a diagonal matrix of coefficients, and sums output + return (diff.array().square().matrix() * coeffs_.asDiagonal()).sum(); +} +sco::ConvexObjectivePtr JointJerkEqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + out->addQuadExpr(expr_); + return out; +} + +JointJerkIneqCost::JointJerkIneqCost(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, + int& first_step, + int& last_step) + : Cost("JointJerkIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 4; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + sco::AffExpr jerk; + sco::AffExpr expr; + sco::AffExpr expr_neg; + sco::exprInc(jerk, sco::exprMult(vars(i, j), -1.0 / 2.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 1, j), 1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 2, j), 0.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 3, j), -1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 4, j), 1.0 / 2.0)); + sco::exprDec(jerk, targets_[j]); + + // Form upper limit expr = - (upper_tol-(jerk-targ)) + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + sco::exprDec(expr, jerk); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form lower limit expr = (lower_tol-(acc-targ)) + sco::exprInc(expr_neg, lower_tols_[j]); // expr_ = lower_tol_ + sco::exprDec(expr_neg, jerk); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +double JointJerkIneqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd jerk = + diffAxis0(diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())))); + // Subtract targets_ to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (jerk.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum(); +} + +sco::ConvexObjectivePtr JointJerkIneqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addHinge(expr, 1); + } + return out; +} + +JointJerkEqConstraint::JointJerkEqConstraint(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + int& first_step, + int& last_step) + : EqConstraint("JointJerkEq") + , vars_(vars) + , coeffs_(coeffs) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 4; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + sco::AffExpr jerk; + sco::exprInc(jerk, sco::exprMult(vars(i, j), -1.0 / 2.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 1, j), 1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 2, j), 0.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 3, j), -1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 4, j), 1.0 / 2.0)); + + sco::exprDec(jerk, targets_[j]); // offset to center about 0 + // expr_ = coeff * jerk - Not squared b/c QuadExpr cnt not yet supported (TODO) + expr_vec_.push_back(sco::exprMult(jerk, coeffs_[j])); + } + } +} + +DblVec JointJerkEqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = + (diffAxis0(diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))))) + .rowwise() - + targets_.transpose(); + // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector + return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal()); +} +sco::ConvexConstraintsPtr JointJerkEqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + for (sco::AffExpr& expr : expr_vec_) + { + out->addEqCnt(expr); + } + return out; +} + +JointJerkIneqConstraint::JointJerkIneqConstraint(const VarArray& vars, + const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, + int& first_step, + int& last_step) + : IneqConstraint("JointJerkIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 4; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + sco::AffExpr jerk; + sco::AffExpr expr; + sco::AffExpr expr_neg; + sco::exprInc(jerk, sco::exprMult(vars(i, j), -1.0 / 2.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 1, j), 1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 2, j), 0.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 3, j), -1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 4, j), 1.0 / 2.0)); + sco::exprDec(jerk, targets_[j]); // offset to center about 0 + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + sco::exprDec(expr, jerk); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprInc(expr_neg, lower_tols_[j]); // expr_ = lower_tol_ + sco::exprDec(expr_neg, jerk); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +DblVec JointJerkIneqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd acc = diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))); + // Subtract targets_ to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (acc.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols()); + out << diff1, diff2; + return util::toDblVec(out.cwiseMax(0)); +} + +sco::ConvexConstraintsPtr JointJerkIneqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addIneqCnt(expr); + } + return out; +} + +} // namespace trajopt diff --git a/moveit_planners/trajopt/trajopt/src/utils.cpp b/moveit_planners/trajopt/trajopt/src/utils.cpp new file mode 100644 index 0000000000..95fb423fbb --- /dev/null +++ b/moveit_planners/trajopt/trajopt/src/utils.cpp @@ -0,0 +1,90 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include + +namespace trajopt +{ +TrajArray getTraj(const DblVec& x, const VarArray& vars) +{ + TrajArray out(vars.rows(), vars.cols()); + for (int i = 0; i < vars.rows(); ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + out(i, j) = vars(i, j).value(x); + } + } + return out; +} + +TrajArray getTraj(const DblVec& x, const AffArray& arr) +{ + Eigen::MatrixXd out(arr.rows(), arr.cols()); + for (int i = 0; i < arr.rows(); ++i) + { + for (int j = 0; j < arr.cols(); ++j) + { + out(i, j) = arr(i, j).value(x); + } + } + return out; +} + +void AddVarArrays(sco::OptProb& prob, + int rows, + const IntVec& cols, + const std::vector& name_prefix, + const std::vector& newvars) +{ + size_t n_arr = name_prefix.size(); + assert(static_cast(n_arr) == newvars.size()); + + std::vector index(n_arr); + for (size_t i = 0; i < n_arr; ++i) + { + newvars[i]->resize(rows, cols[i]); + index[i].resize(rows, cols[i]); + } + + std::vector names; + int var_idx = prob.getNumVars(); + for (int i = 0; i < rows; ++i) + { + for (size_t k = 0; k < n_arr; ++k) + { + for (int j = 0; j < cols[k]; ++j) + { + index[k](i, j) = var_idx; + names.push_back((boost::format("%s_%i_%i") % name_prefix[k] % i % j).str()); + ++var_idx; + } + } + } + prob.createVariables(names); // note that w,r, are both unbounded + + const std::vector& vars = prob.getVars(); + for (size_t k = 0; k < n_arr; ++k) + { + for (int i = 0; i < rows; ++i) + { + for (int j = 0; j < cols[k]; ++j) + { + (*newvars[k])(i, j) = vars[static_cast(index[k](i, j))]; + } + } + } +} + +void AddVarArray(sco::OptProb& prob, int rows, int cols, const std::string& name_prefix, VarArray& newvars) +{ + std::vector arrs(1, &newvars); + std::vector prefixes(1, name_prefix); + std::vector colss(1, cols); + AddVarArrays(prob, rows, colss, prefixes, arrs); +} +} diff --git a/moveit_planners/trajopt/trajopt_interface/CMakeLists.txt b/moveit_planners/trajopt/trajopt_interface/CMakeLists.txt index b1e7e4c3fb..b415435f4e 100644 --- a/moveit_planners/trajopt/trajopt_interface/CMakeLists.txt +++ b/moveit_planners/trajopt/trajopt_interface/CMakeLists.txt @@ -11,7 +11,6 @@ if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() -<<<<<<< HEAD find_package(catkin REQUIRED COMPONENTS moveit_core @@ -20,8 +19,9 @@ find_package(catkin REQUIRED moveit_ros_planning_interface pluginlib roscpp - rosparam_shortcuts trajopt + trajopt_sco + trajopt_utils ) catkin_package( @@ -50,8 +50,6 @@ include_directories( add_library( ${PROJECT_NAME} src/trajopt_interface.cpp - src/problem_description.cpp - src/kinematic_terms.cpp src/trajopt_planning_context.cpp ) @@ -67,28 +65,11 @@ set_target_properties( VERSION "${${PROJECT_NAME}_VERSION}" ) -======= -find_package(catkin REQUIRED COMPONENTS - moveit_core - roscpp - pluginlib -) - -catkin_package( - CATKIN_DEPENDS - moveit_core -) - -# The following include_directory should have include folder of the new planner. -include_directories(${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${TrajOpt_INCLUDE_DIRS}) ->>>>>>> TrajOpt Planner (Empty Template) (#1478) # TrajOpt planning plugin add_library(moveit_trajopt_planner_plugin src/trajopt_planner_manager.cpp) set_target_properties(moveit_trajopt_planner_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -<<<<<<< HEAD + target_link_libraries(moveit_trajopt_planner_plugin ${PROJECT_NAME} ${catkin_LIBRARIES}) ############# @@ -133,14 +114,4 @@ if(CATKIN_ENABLE_TESTING) ${PROJECT_NAME} ) -endif() -======= - -install(TARGETS - moveit_trajopt_planner_plugin - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install(FILES trajopt_interface_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) ->>>>>>> TrajOpt Planner (Empty Template) (#1478) +endif() \ No newline at end of file diff --git a/moveit_planners/trajopt/trajopt_interface/README.md b/moveit_planners/trajopt/trajopt_interface/README.md index 98824eadb2..84e0ce1d44 100644 --- a/moveit_planners/trajopt/trajopt_interface/README.md +++ b/moveit_planners/trajopt/trajopt_interface/README.md @@ -1 +1 @@ -As of August 2019, this is a work in progress towards adding trajopt motion planning algorithm to MoveIt as a planner plugin. \ No newline at end of file +As of August 2019, this is a work in progress towards adding trajopt motion planning algorithm to MoveIt as a planner plugin. diff --git a/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_interface.h b/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_interface.h index bb48692b88..a534d01cff 100644 --- a/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_interface.h +++ b/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_interface.h @@ -36,9 +36,11 @@ #pragma once #include -#include + #include -#include "problem_description.h" + +#include +#include namespace trajopt_interface { @@ -61,15 +63,15 @@ class TrajOptInterface /** @brief Configure everything using the param server */ void setTrajOptParams(sco::BasicTrustRegionSQPParameters& param); void setDefaultTrajOPtParams(); - void setProblemInfoParam(ProblemInfo& problem_info); - void setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name); + void setProblemInfoParam(trajopt::ProblemInfo& problem_info); + void setJointPoseTermInfoParams(trajopt::JointPoseTermInfoPtr& jp, std::string name); trajopt::DblVec extractStartJointValues(const planning_interface::MotionPlanRequest& req, const std::vector& group_joint_names); ros::NodeHandle nh_; /// The ROS node handle sco::BasicTrustRegionSQPParameters params_; std::vector optimizer_callbacks_; - TrajOptProblemPtr trajopt_problem_; + trajopt::TrajOptProblemPtr trajopt_problem_; std::string name_; }; diff --git a/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_planning_context.h b/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_planning_context.h index 9c06eb0f5e..6241d09396 100644 --- a/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_planning_context.h +++ b/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_planning_context.h @@ -2,7 +2,7 @@ #include -#include +#include #include namespace trajopt_interface diff --git a/moveit_planners/trajopt/trajopt_interface/src/trajopt_interface.cpp b/moveit_planners/trajopt/trajopt_interface/src/trajopt_interface.cpp index 7a5e95b71f..f721c16c09 100644 --- a/moveit_planners/trajopt/trajopt_interface/src/trajopt_interface.cpp +++ b/moveit_planners/trajopt/trajopt_interface/src/trajopt_interface.cpp @@ -42,11 +42,10 @@ #include #include #include - +#include #include #include -#include #include #include @@ -54,13 +53,12 @@ #include #include "trajopt_interface/trajopt_interface.h" -#include "trajopt_interface/problem_description.h" namespace trajopt_interface { TrajOptInterface::TrajOptInterface(const ros::NodeHandle& nh) : nh_(nh), name_("TrajOptInterface") { - trajopt_problem_ = TrajOptProblemPtr(new TrajOptProblem); + trajopt_problem_ = trajopt::TrajOptProblemPtr(new trajopt::TrajOptProblem); setDefaultTrajOPtParams(); // TODO: callbacks should be defined by the user @@ -87,11 +85,13 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni bool robot_model_ok = static_cast(robot_model); if (!robot_model_ok) ROS_ERROR_STREAM_NAMED(name_, "robot model is not loaded properly"); - moveit::core::RobotStatePtr current_state(new moveit::core::RobotState(robot_model)); + + robot_state::RobotStatePtr current_state(new robot_state::RobotState(robot_model)); *current_state = planning_scene->getCurrentState(); const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(req.group_name); if (joint_model_group == nullptr) ROS_ERROR_STREAM_NAMED(name_, "joint model group is empty"); + std::vector group_joint_names = joint_model_group->getActiveJointModelNames(); int dof = group_joint_names.size(); std::vector current_joint_values; @@ -117,7 +117,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni } ROS_INFO(" ======================================= Create ProblemInfo"); - ProblemInfo problem_info(planning_scene, req.group_name); + trajopt::ProblemInfo problem_info(planning_scene, req.group_name); setProblemInfoParam(problem_info); @@ -131,11 +131,11 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni current_joint_values_eigen(joint_index) = current_joint_values[joint_index]; } - if (problem_info.init_info.type == InitInfo::JOINT_INTERPOLATED) + if (problem_info.init_info.type == trajopt::InitInfo::JOINT_INTERPOLATED) { problem_info.init_info.data = current_joint_values_eigen; } - else if (problem_info.init_info.type == InitInfo::GIVEN_TRAJ) + else if (problem_info.init_info.type == trajopt::InitInfo::GIVEN_TRAJ) { problem_info.init_info.data = current_joint_values_eigen.transpose().replicate(problem_info.basic_info.n_steps, 1); } @@ -151,7 +151,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni ROS_INFO(" ======================================= Cartesian Constraints"); if (!req.goal_constraints[0].position_constraints.empty() && !req.goal_constraints[0].orientation_constraints.empty()) { - CartPoseTermInfoPtr cart_goal_pos(new CartPoseTermInfo); + trajopt::CartPoseTermInfoPtr cart_goal_pos(new trajopt::CartPoseTermInfo); // TODO: Feed cart_goal_pos with request information and the needed param to the setup.yaml file // TODO: Multiple Cartesian constraints @@ -177,7 +177,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni ROS_INFO(" ======================================= Constraints from request goal_constraints"); for (auto goal_cnt : req.goal_constraints) { - JointPoseTermInfoPtr joint_pos_term(new JointPoseTermInfo); + trajopt::JointPoseTermInfoPtr joint_pos_term(new trajopt::JointPoseTermInfo); // When using MotionPlanning Display in RViz, the created request has no name for the constriant setJointPoseTermInfoParams(joint_pos_term, (goal_cnt.name != "") ? goal_cnt.name : "goal_tmp"); @@ -192,7 +192,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni ROS_INFO(" ======================================= Constraints from request start_state"); // add the start pos from request as a constraint - JointPoseTermInfoPtr joint_start_pos(new JointPoseTermInfo); + trajopt::JointPoseTermInfoPtr joint_start_pos(new trajopt::JointPoseTermInfo); joint_start_pos->targets = start_joint_values; setJointPoseTermInfoParams(joint_start_pos, "start_pos"); @@ -200,14 +200,14 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni ROS_INFO(" ======================================= Velocity Constraints, hard-coded"); // TODO: should be defined by user, its parametes should be added to setup.yaml - JointVelTermInfoPtr joint_vel(new JointVelTermInfo); + trajopt::JointVelTermInfoPtr joint_vel(new trajopt::JointVelTermInfo); joint_vel->coeffs = std::vector(dof, 5.0); joint_vel->targets = std::vector(dof, 0.0); joint_vel->first_step = 0; joint_vel->last_step = problem_info.basic_info.n_steps - 1; joint_vel->name = "joint_vel"; - joint_vel->term_type = trajopt_interface::TT_COST; + joint_vel->term_type = trajopt::TT_COST; problem_info.cost_infos.push_back(joint_vel); ROS_INFO(" ======================================= Visibility Constraints"); @@ -239,15 +239,15 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni std::string problem_info_type; switch (problem_info.init_info.type) { - case InitInfo::STATIONARY: - problem_info_type = "STATIONARY"; - break; - case InitInfo::JOINT_INTERPOLATED: - problem_info_type = "JOINT_INTERPOLATED"; - break; - case InitInfo::GIVEN_TRAJ: - problem_info_type = "GIVEN_TRAJ"; - break; + case trajopt::InitInfo::STATIONARY: + problem_info_type = "STATIONARY"; + break; + case trajopt::InitInfo::JOINT_INTERPOLATED: + problem_info_type = "JOINT_INTERPOLATED"; + break; + case trajopt::InitInfo::GIVEN_TRAJ: + problem_info_type = "GIVEN_TRAJ"; + break; } ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.type: " << problem_info_type); ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt: " << problem_info.init_info.dt); @@ -266,6 +266,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni // Add all callbacks for (const sco::Optimizer::Callback& callback : optimizer_callbacks_) + { opt.addCallback(callback); } @@ -366,7 +367,7 @@ void TrajOptInterface::setTrajOptParams(sco::BasicTrustRegionSQPParameters& para nh_.param("trajopt_param/trust_box_size", params.trust_box_size, 1e-1); } -void TrajOptInterface::setProblemInfoParam(ProblemInfo& problem_info) +void TrajOptInterface::setProblemInfoParam(trajopt::ProblemInfo& problem_info) { nh_.param("problem_info/basic_info/n_steps", problem_info.basic_info.n_steps, 20); nh_.param("problem_info/basic_info/dt_upper_lim", problem_info.basic_info.dt_upper_lim, 2.0); @@ -400,18 +401,18 @@ void TrajOptInterface::setProblemInfoParam(ProblemInfo& problem_info) switch (type_index) { case 1: - problem_info.init_info.type = InitInfo::STATIONARY; + problem_info.init_info.type = trajopt::InitInfo::STATIONARY; break; case 2: - problem_info.init_info.type = InitInfo::JOINT_INTERPOLATED; + problem_info.init_info.type = trajopt::InitInfo::JOINT_INTERPOLATED; break; case 3: - problem_info.init_info.type = InitInfo::GIVEN_TRAJ; + problem_info.init_info.type = trajopt::InitInfo::GIVEN_TRAJ; break; } } -void TrajOptInterface::setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name) +void TrajOptInterface::setJointPoseTermInfoParams(trajopt::JointPoseTermInfoPtr& jp, std::string name) { int term_type_index; std::string term_type_address = "joint_pos_term_info/" + name + "/term_type"; @@ -420,13 +421,13 @@ void TrajOptInterface::setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std: switch (term_type_index) { case 1: - jp->term_type = TT_COST; + jp->term_type = trajopt::TT_COST; break; case 2: - jp->term_type = TT_CNT; + jp->term_type = trajopt::TT_CNT; break; case 3: - jp->term_type = TT_USE_TIME; + jp->term_type = trajopt::TT_USE_TIME; break; } diff --git a/moveit_planners/trajopt/trajopt_sco/3rdpartylib/bpmpd_linux32.a b/moveit_planners/trajopt/trajopt_sco/3rdpartylib/bpmpd_linux32.a new file mode 100644 index 0000000000000000000000000000000000000000..52720f64e7eeb41975d20ba6b10060e380e6fc8b GIT binary patch literal 532260 zcmeFadtg-6xi-Eh86e`|#5yQ#W1Y4`8zjaA22D(?0RjXKkb4kgAtVzL&CO&Y*v14* z(z3f5pcQW|J=N+t$J5eZsp8QpS^{VYUh1tjqZvexU&|=`uc`vfpr2oYcQAxZQPR^tY?la6HjyI1a)6lYAq=d&Ee{9UA-lPMMK#$3`RJ z--1TMn=bu(8#Mp#_y1(@S9HLc;WA>s>K%r2YrX#6R$w?E-eowSzFYr38ZsuFebksR zvr7NI?lUHg|AihgCj4xt{=HIQOo;nU^c*$R?~C^v6KCfc6X&isCYDY6+rKRvjfr>o zjfuOr8xvn#XH0zaurcvSM(j5U)iL%&oi7+J@>i zhBCp}rc7(rv;~yg>gL*-`s?Y|T3_9sIg6(DK(MK$R=IVwtqD}eOgGR}W3#Jit*xn1 zWH7aA(~Z?_bq(tjS$&|jwnLd(+ZvkdbkGjl)X{32T5DTYU#EPV18Z9~zm~?@)s4!n zrLnzPCA6lc`Nlw7kd6(_4Z-#nb+;#=LULV$5w z%UZhCcGT1dRMrEH4NVOyL{nXLO|Yd+rB=)&t?pP;7gUm~X>XOhz@Vktv1YaQt81=p zZMc#2H4VYGfMU|v&|Di(NdS+g4wZx(fa{DU3(6Lku3Xwu8(5leELoCYTDiDveu1%a zep8y21%V1Vz^-V0%qUyF_14X_h z(A3g)lS)KWYez6}3IQmqn;7r>mg=_lKr;zoD+$y!G{>mnK(WQAn|9L1Vj3Gq%sGy~ zNFkmJv}irdnbCDrZMgN0JfvBV{TxwY4Ivn8<}b*vC0aUMQDCM)y~QTn)vz@Q{`BJNWMuM%eP&(5!g1a zoQoD0wL0D@AFUTEdpjeE(Ur8U&0km=?=ruoQM)v-9JJ&c&^`N|!4s9*hp&(9e0Y0s% z%Rr!0Q;X2*9**K|5JveJZ00`2v)#`Gz1?xX$NSo%?SGOARq z;@lgn8yaD&8`DUNOgjsbR}DQvjtX_6fO$sQp@tLsYidK1`rLlDvq`k?aeK1 zO_W1>ZHtPgy|ra6?TsyK)DUq^pt-58MGbZ8uW#0a#ip7X^!s$vzAKy6U=D7mgX>yW zY6HBR;|5t`W~i37+M1vmATh1AYH-zBQ?sUto~<=EG%4N~_@N7@Q+u$v7VR26G1}1M z!J7ItP3u$u$fvPg4Hq$%6}PrP6Y3=cwA%y7w%V4~pbF4d+uGK$n!a^9eKfYQxxlyw z#+p%MU|kKyrOLl~jSQaw2&UXx)?yGTrrHiQ_(Pi~D6WQ!7*2gt(3By1I7EN77qlQ9`dpWM$8u<@IJ}WMClzf3f(0znA#^UhnMeOYoP^ zs>$683Ku#Y4lGIN$6+KIqc+1R<7x#SzxtbMoM~W5BJL@#tSGh0e4gXCY(y}J)##gM z1%0XRzN7@h2)}D}`n*=9(^~1vezn_c<@&tK^}bJ zcSEi_DBpS33@MLyr93>=Tvy+ou8SXI!JGZ>OxYU$Se`FfonLT*W13lZzF~Hr@9f!^ zw7BE<*L;2Dwfn;FMqmBQ$OsbbiLBQW3m=aD{*#fBt)X3CVKGpDdXBoR9lnjQS>;Z1 zD&xs1_d$(vp#taAYHjmvBDlUkzyFOyQ@c6OvdYeKBK7{9Ykj8EZI-#6UB9-LB$-nb zizTVuSA~y^Pp@KcZP)yqxAq@S4DX=?hW(S~L58b_&(J9tPMZj+`*X@uBPoep2cTQ! zE<0^$-R0 z- zEPRM2HR4Q0#{K7n4~7pPIvCzFG&pop3B-jc7eW>X550lkpv0}E(UHNp?_%63cHDX0 z>Cj)TPu5?PM_x=G$+kSQx~mYU*UB`5z9gpqa;0ygAO6Qeu0JU;mHGUkl*y4qBs>^N zNs1&cLaSF~&SAcWU6W@bG`!MG6?{1DSkBE^k^z3g{61pj=Z)B8bSEl7a?5#+n z{|QEdS|2Lhn*`Tl=Rz~lEOqwuC&4lMFJ13wFt@SyssqwqFnC$S5i2iSq z_TU$~Jt*_}RC|y>Enhfh@{I0PqxGv- z^Y`qOl(4};t#zJ<#tdyr#EB||;_8MqO=x|@ znHWhJ8XTTXeuoB!Cq*s*l|SN~IP}8nheuS5$M;Wg_vWDtA_?Kc?12vrMy4i{KcXA{ z%|}p z)0@bmM#3b$RVuxSk6I+?+>rzE@+2|rIIJh(z9S~a^jIAR5=(;FGO zhXU{0)8T%EN~nu~rXuu;>w&NOeZxzP@W+vBjh_9rj`;jVhN4mQxZxww ze)O{Ff6oGdhheP$m9c34{L8$lieawto5p(E)D~z=4+ho+ zjdUy;RU7H6+uM!wwia5_u`=v8(wkd?fpl=Y2qV;*>y31b>(w%Witg|Da{*|W&xt~t z2mI$V!~$UX^Z7h}an45B6vIeGZKE5*vz=qjoi27X1?o8x9|$g_<1-UK(#O+zVYeUp z@yWoC{69kr9`dt!&`vXdm*Yn|`#Cx;UfXX82y8QHNoVt>-S#^hW;<L@I5RA(Io&&jrtgaI+tqpKZ4>-rZ@ur`1S2$IcSSKRyQ9<0VD= z1k4@uLmvnHnRi=;w4Vz*WpDd`4rZpGZuI{mkX@*4hcWgf#5{G$0O z@BHe`OFnnS@%;&Aqc6+s^yPxe>F!NIn}H$0G0+X<`R zfTW{)IqbWEU`saESs~BHQJ%fN58zh>UPSUDmLM##96X7Y@aOQjIJ8aiDhcgSSZ;n; z(KC=#S)qr`N7qYUth2Fl&{u}W*-AXV-{J0^g$A0^fx2)pA!s!?A;mll5?V1FDTJ3F zX)~+{`7oAw7A)YI1Qq2JGYRS22xCv~Wh3rQ+u>>cB~g$4k-cpsI@e0e z0?Ul-x6LnrPpbJ!cOg=>o__hh&QRJ5q1>(FV7?kk3CC7vtn;h|hI#fv3p4Ukr)aj+it=-Bex6GZ1rw*o1@leT?(U}Ko=wDli= zjqQUI$T~@6$YdbHBmw*1OmMu!AZ#>9%tplV8FgU1G{D70HOfR$)u z4#UoHjE~`_0ipS&2*Rk#ReQ5x|4xvB8=FNdyKltZ_Xhek=zXiLDb{(aXjdy;8NV)u zx?$x{an8)2!UlXlm|#gQ*1}94>70g`SvZN#Gm)09FwJzHl}949TS~2xZLN9$y4$K& z1+1&R*@wDs2FI5X08NLz?q^LxQdznt)TaM%g5`0*fdy=(InZ2{VJ(FX)(hRUSmQz- zJD@!;zYxh`D(8+kf7-LxnNx`N=xwwYt=0D@2`YEzI>MsfXb3X8oM|S$a`Jrz?kxu}FzFj~ zZ+?_q;G$ZS;H}Q3s!1^i+?##~3J9kYrMw?>q<0VH}6+TKiB+= zo%_%?95bHF#yqzBg3*?RJx(%n7rIV0ERARqj`Y0FoIfoBXt!4@6>h1Mjq z@jEB6cFs)<-2tJCSA9<;&x7YQ$YUA=##B9nD9zk2jG!e4bAJy+ zgf`Z_CPO77>D^T8?S$27KS8({vfr?IkGU^cXWctrTmdM{qJ9~-L zNi*#nDFMr$%wkFOpT}Y~gXgig^HWz@NzUy0IkF{Ha|NoVd&^JAX@{&b={HTTcx7O@UD6Y^E5bp8Yx z^;4Q=+AWbuBKEnFl)Vr_hILLJVkaSlOwpnqoB6jH=oQh1p!rycj@*gb?A~09#T{EZ z>;k(xo`5t*W(Iv6O6+0+E=Dx|1yEQ><9Rrk7l&NQ)@R|pleUzfV@R=Gsf_9WDQHxn z)CFcL1%#HLjg)m};$+kHbL&D?>-ENi>Hi6&>d*P6w`&`hbV0_pp{Sysb6BwNC9L$8 znt!gJ0po^!oO71S`YGFp&LU;8m2NUJt-=ID&ZTBsa$=hYL+mR2JdWd7Q#w^Qx!Rj zAeNJ!Jv@7IuG^E4NWwip5)yl!Otxn=7%Ub;sEm~(7N$rh8T(iD?5|QQg2-1PB<+lT zo|){`Q!s-Sq@w;31gKw%KUyQNVPz~4@2Jh5C^=%v!4hu&Azd1wT@(XW3n zGE%H+mHC2>qn~jUE8fdTlZ$+Da5bRv_0D&YY;RpdW8*~)&E6VysH`^K@M3HKVje5- z>eHw=#8#W;HKt$MSUbI4|21%!%G=Tr^tRM_OBR&OE%Y|&%YIeYMgF;^+`={PDW3X|( zlPe&UA7xFmEjt*EXAo&V4f5FhY`cx|?*9|(ur^=f>DEoh`X?zppc{3E>!_YM0_N)r z?~*dA$_)O0n%>BLYO-Aqs?4n9X;_a$I9Dz{ynIVl&iB)AhJ*8Tniy3Kw+Q7 z8e;+k|26!$23sJwLa+{);4+MM;uOQ!i(dlr^#>s7|0sBZ?x0JAAL%lH^uJVaKHcGe zmH2O?I|lbZ5dVJ_{|CfhzaH@Suk~fm{`=SZo;gno)UO~FYyYgdqYuwmY&g0D4$`%t z=h;u2jMQZ~sfSGHh14iWcS`0Cwb6`l?D|Z-2w>{nYH&HMxx7U`JybM%^|)h`?+E>R z)g;#3re=<&9>F4pWAxLjX^3GHZ99A?$VEnHyL89D$+w=47(QXBxei0lAciEZNH^;* z;^*KQ{TCak~W;w%@Vfs*p^*_~?AXEzYdJs)LJO?JEAuJ4x;XcA{Up~X`@)Z!a z`w9UF1Cc8J^rRSN2Rz~0PE^c4vS8{;J)h1@9SkFd~;(-ZX*!c`hu-ompBHX2Q zZ6XeUt&8nB>!L@Zd;`uW7}PpnMUOS!0q0$#4k1@WSvczR;50!Jvz%pm&N4f3Zi^#D z?UV~&I)%RJpyxP^`fglu^l=%>(YMFFc?)>UT7k|zs!s&te{94L4u!e#zQEo0&*+Mg z@w5@ArA39aBslOeobt2~)R6#FyAd2+7^9YjR7(dYHh#?_f54dt?Zg2m=5?W|0D5va zY&o;so7ZCWJ1`*+Ha@5%h1QJBJ$-+0_x=-t$rimnbCq{*9!XGwmaE5{QUpuLqbq|w zE1YVWbIM4hRmsXt&i;#e`Et1Eh}~N*C0F~*lM+7^KH~g!_+6a*fUZ#lmS;1hU_NV3 zEkHQd`)${&^3 z%n#jtzr|o^U_ugn_zX;NX#-5TFd=bJ^fvoF_i&PJl<=1fsy zQ-!Pd0`AFUET|6IiSE7@)Ree#!>rd@VxZs#-Mzm>E~Kn+_mc zUZfJNs=0g1TreKjzDd)-#tlt~<@`>>`5cS=UY3){)_m68=fN1~zhBV*9|jZum4ZGC zI&n%thq)B}j}~+z0{``b{@97iaJ3`nd?h*68g-wk1s;741*}UPbu!!G?wi535pC=M z*dkT3l`(-urHXY{-*NZm9rRLFCFId1md^$uc?7GM-wfsIhON<8A1b`uh&XT%7zei4a6^-W`NR3BlWjHA^m7b9V$SW9eUL5WpdSmD{UH^K|Kyq-N z?M*sFUc~{e;CUePT4`=%z~}yX+GMtV4a%Q_X-%hQXno7qhUi=OD&qmj~sHh%hqof`Uol&e0fO=ts*6jyu*vn-Oy%dA4 z`2d_Sjy_DE{m&|E+Xe2KSc#4|k_0F4c zAu$%pCW#EWQLlrbhnf~~L|;a-+;aqbGTgxI#^&+Onf>`o?g>WMv zsG6Cj^~>yh$f)-rkt&lc7H*+!fti};kqKmAE4m)e4ZBjM<_{-C zK4(oKg>`-k|LS!+6Z;pY>tdvRCmmHywx*C^WP%red-msv;o-Dun7uz)q)rX6ift61Uci1EUe=tC0!G;Cd*EM63%R8TKE*SxePHqn^Bh*C-C^ z<_O0?`%!Qn94XYbTb&e58veZ0#CK7LaOM;1r^CJvV?Ff{>t~hEH{yIy+LK(}p2&tv zA;x%RXbPS0;efYZ|23!jk)EM9a0X%MRWuP{jz!)#FUo@Jz=T5lWof#5IWggpN)n)o zUJ0u%@*-Pd>PZao`AU%Rta7MsUNO49f(oRvi0h{J!=LM>EUI#?fW1nmLaw3WdPb;5 z@8I`5%-k%F(~--SI_Uz|EI4{Y`HDVpCRO%{fyHsp6K2&X&YKs>sC)_;V_!=R`nxkn z$H!AM-~lOYBKWr=(fK{79wWhN*k{Xr*1U-R*n3xFlIVP{wn9y;^X8e{+qoN_IcIlG z9GC=2U{Il$$$?_d06f36GEfv4{y%6bPFC)}!HxmyV=91-T_x_FEzqThR27d5LEn%s zGZT_?4oswyMz8ww$cTBInj3xm1awc2CzT#!$bQUfJmxena)DC6k)X#}uo}@$kjLh8 z98AgxPL11Qt~~{h)12Ye#(AYhe5J!p+~Ar!TR`l)pm-I-bVzSDAN;|PuEdEXs+kTiPcI7Ua^TR1!0YK_-VOb>f< zA07$%=4Br=m1oy$GF^IC<_jtjQQz_ZvZ``x4XXN6YmU<6++VOZ4CFs9Qv~Ixb@i69 zQAQuet^pL29V;~y>dQ)kM%27?AVI0`Ch9EPOOTnMw&z2bT1K45;)jgLwh=8ON$@Lr zAS#m|HRFg)f&9_mV<+NM^z|QXPmdvLb>6=fr<{hr9DYL&C(zPWa|=W-jfK*5YwEuc zt#=9<0jj1shYhHeI|Ij%>YY56Mr|wB+J;uC!n~YH3$bKXnpzjie0e1sIG|f^UZgs+ zMqdHj%OdVNQ46_?wG&vWw6Ij^-e_a2TS>PPrrv^ov)hAi>85*nT7*` zhn!SL-JDr})a4bu1%|DZ_utc=%vZ#^{ecKRwLOWx_o?Oc54I;CA`^%)7)m=4SzsJ` z6NB#2BDxaMMTSBtkAo<-)Z@7zzK~QeXZb&im6_$^3yEEpIY$)|Rz6T(l$dHBOg+{> z-6++?W53ZOujZ)YDUT~2wnVykPTNT60h9Dty2VhDnYr(oRqu@!4_?Vsvzln@F)5}s zFpS%a{gnDMla#DK_7Xydy@W8n7CrKxs+~ZWG>PNH=)k)O>3>!Uk+Z-uUt=g2Z>h{0 zzBqD0)PygD?6Lh*L;mClj&2O6NAPkXNBYqPAThjHL~r-v-DBK6Qfqjz$Jf!++@5C4 zTbe(=;Ht|EZ12rcn|$goBU4*P%77i2EB@EE?DXppqwhv!{r5NTPPcbAzytE~Bnyt; zyW5Xi{|F|wnEI<0mhEH`e$&Oy!6cs~*)!0#zp{4+x+?56;9@sI7-07u(Qf702!X}A5b zxHo1`ZUyMtNFa|*M~vU&d-=226M_Q=N9W_mJg`j0@9{ZN9>0RS{n-3$yN&Vg|HvMn zz2id}+3!}+Y}?uJFnzY2dvp_8OQA;_TPKOddAB8(=Pqv#*1>EY7|j=H+pA z7-J9LO^JX0o;}gA^k*0w#esE{d!*lsBcR-WFwTDOIQx!qb~PLjH-nXvu>bmqf1}@EMO1}iJuv}_fm!4&SI@z9q;PmC5f5K*nRgD>F;%C4x1ALBXAZ{eo`_CPB5xXNKT1!3x3a z1-~cwBf*CS2L*pE_@-bY_L|ApE$9`zP%v9?uHZF-0l^l*n*{B5Zhk8EhXnTuMg;#u z@DGCT3*vPvm7a-$XA6E=@N0t01g{hPrr<`w9|`_K@L|Ch1*3xYyE^Bf0iztgBzU1< zzTi^9<$|jP`BpZ=Z4>+_!Ji5~CHRcsD}uvrLf?$1}y5s?37g4Yr8LfbdR z{(|7|1RZF=Nk4}Oy356WOmG=Gboy5aUN3lqU>6Y$davMj1h)wO7?^-T#bTG{Q!RL% zV2j{mMD%U_VjmEELGU;*;fscGQtUh%!F2fqFBDuLc$HwW;EjT}3O*?q6+9_884IT5 z`+31)!J7oP2;L^ROK`8?F+l^{ROB;3@N>X~FQKm!`&R@n6D$?HTCiHMP4Ihy-xvIq z;IQB^!Q+Bgpz&t>3k0tcY!Yl2GzGT{{zULm!4CvKg8>WqeMRt6!7{UL-5VCIVvoHFX$6IqZ4Ln}`T;oLOGu*OsQ zzGWBT08}1AV{eCNcDho3iNJ={;YsXBUdTNpSk0hy(ma?`Zt!)2w-A6aV##L*{J3~? zCy+;miM+t$d7yfG5?C2pki_FlMdkWkpq~9+gdBnoL%s}T$X!5&+yrDu`tx}ZrFO#jP8S(jm*MTHjbHtuN?QoUgS642RnLh8Y=6LD-Ny4^*5g(93#Hvez`LG+B8cWrg;*j4GB5d(monxhR<7Gr z9!qrItOY19FZNaE#;G#4_E3TsvzWQGl?9CC51fD$$*V_Q6Qiz7wH5M^{8t zatwncFi6cjo;jkJn0pa6h_F~i4(>Z)cJ4dLkVj4v@>*BlYhAN-HRx6GS!D-KATtFB zZgn0wX_h^A!aSJ$s@eJ6N!Ez4T_bj@GL*ghT|O4D^#u{CoKFDyyvpHiZ`Beht*)N#JUoUB6Hsow4@gPs1lK>Bk^rCu)tGFOa&FMBh0rO36De6${I z*smI3sU|!q(Z|!jtiss6R`Kof?N_afS46&5&i0b-!^$1++Em0f zmS<-FI#>;&At8EM5!vk;Hu7NSZY2FHk(}K?W{F6qL`L=qkSpxWBI!N=j%E!RItd4B zS~dy^su0YvZua05!a_Ep(nul@BO`M=F@>;``7U6EE(bQED(IQeXErBPX{3W2FM{q_CTdqC(otYzeCq1ICkaG9k`ek@xiiwMs z20`-T>xUO%Q#QP(V`3y69(?C*Y_){@Mk3qQUVFy`{VGz3-EQ$~PT%nP7^PyL4s3b) zt~UeR%;!ElWWHxUL>lY1Wc+*utH@Z`5H``5XW4szc|$E{ua_ulf;QG1c!m`H*P$-2&}$U$P4-a zo<;nP*RBNjqmX#up#WGK!aGYK;yqi`C~6QuTSZtMD}d-x7P}$Ap8c-+xx}MMkrZG3LKy40P>T{Nv)?na#fJB_ zsib)C8W1~zHH>5oU@5^c9^*q$t8)y;ef z+r*Go70kn~4|A$~xrh2gC62C_5gGdrsR0o1GYt6+7b2p&b!A5OufZCJlDp@j;te74 zP-QHJe8jNbw;Vwt&V4Y|EB^b;m$Kj2{9hTzzgP2LK5Zs8_!F0V0C~8t0F49~yEiXE z
    (d?rvrnnT@dAYGLq&$mL(-+++aJF~?rtRzz@lhlzZ^A&gBpP=Dz-T{IE=N-s| zL7C`5Oir=ie93&i>u2bGnLmhCwYfLBti{afpC#YsiSCu)p=14aC|AeYE3vXESE5YG zQk(eZE5T`_F{%VgMhh|!H--<4^XKE@lwsA?PRVJ`)xK^&lPd96-Ek5b*V*8iTMo5Y zI_66`%Y7_7Tex4gVIcK&?a8v`kbLw62V;$6SQ%+UDoBi=500=qe^%v1{wo6CS4l(Mq260RH^mdMRz#i);^Fq6P z%Ml?XIsgwwL=lWeWKV-Osi7H%P_^)nb&e?rz<>;(Wv9obY-oGo&H$?z01+$)GV4S( zyk)>j5{KRh527`u z_i(RzB?R{E$_r2c3h?Gg4_8?3Rzt$@9-N>Y9Nwgf55hn`JROEhH*r)|f(er%h=TK= zVw4TJRiJMpa;`=!g2;%aq1|e#TM7DH*h$ZNL3&md(q{lUz&wVma>8OYwugkz<+6G-`RG)DT9 zLeKD|?*&rM^p6d=cTv^!dpOJ!$261`)5tUtS@DRBgGgDi^P{YYq$iS|b4k(@Nxxmu z?^Z~<9k7#*NV@$9N7=nhe(oo>`+fuiTLKoA^|i`+SUK|C1}Kr?Lz!-ptR;;c9$}h) zCN@>A+cF?_v~nBUu*3w;M`2ZOa`a86gH{7;66C@-QHR*kR6yLcvUpTdAl^omG@Kb# z#fU5bS+j{$oBcqJmx&zW5b4i0iT+eO4xLT{S-g!v(y^uBkcsNTtaIK(_39P>PVrv_ zcls0Q&-qlvsJ_?}9=y#hjD~?Ev=g*j7=0lAd5RHkqyjQQ^k;&&X2k>%Wg;&VFntmz z+a0`&Wqd#AS$5QWhGWym3=^4QBAYg%EZ|@Qk@;fgYkcJlVIZ>*^%w z7$GI)u7}*X-L$|TS+FkLpZzr3GE5NuXdW_O?Y=JDug--?N9?=eE9nt072-Gcp|B7n<+LK5%HSxzF6&?Zl;)4~7S!0_#xJXn;71 z0^HBE$%yMp7sh+^wn{RxUof9G4|KQ6cx$wMwbS!$q=)T0)1!OWVb{Q4PtW}9myntv zm6nH*mNCOVJ1yIF4={h)jAM!OJu?pU?LnIL$n1+)cP<8KZy#=IJvvyEfw*sk58-b)JvOm2aNIKRE zyI|e1}79FY$T>&Ta80Cqg^Ui$J+8biEcNq>r_m4zt*4 zO~lo-NCv}V<_eRJ!!dke z%7HsibX#L}S|@QyJ#1tQaWJNcGbiN9GM^s4F#CN(gB7h8L_8Q8vZX^FxUh+3u}jpl z7|u0H(7$tK3?eO{AoO@Z&G6sE)!J{cKZj%v4t>Nqwy4TeT$RRnfdd(L-{+DMZU_Sc ztvFc42BA7wJ0Unps351eKn$TD2K0P{%SgCK78yeNIaX5X*Yj8FGRz8?C#VtUt=8A{ z>u*8{53U%Y!{dM=_9kMF?u2kZ-c4AeusZx4Xbrj)Efk>)LZsfU2U0(&kJL}Q)qY<5 z86o}I=+a*|w)U!4xeH!gaR|9?FQzy9^im{cCzFC`dWFq?Ala}wknSWf))pffIvVLj z%BAC5#cWCcFHXszrFgi^I2AmQ%WbeCmp{P{3naoe4F8c~7v2}SOqcyGEJ(shFC$p_ zPoJ>Lp61Mvnq+u-U-Q_;}X}c8$#C=%N@} zRuup@jREOTVaEH@&QS%WKq(PPH$*x+_ZhlSIkF&)>b21DJc68%7pMJ9+#`zDHf1Ld zy2tz5l2)TF`0EnDg(fO)yMfxS#t$r!qmmXP`LM%cJa+F%{)|WQ5n-qe2y`G$q_t;% z8K@5+)PQQKDS76V8o+#zAH$P^{E6g$1jzVpf1=0&R2(m1vS6y1Kz$Ux);S#h@Zi*33Lh5)7!ksKC0-qgk_1l~fxFS+}CxU%X+oUhou3vY4E zz(tK7yj?9D7`VF$6F0lvBthDI#@6-I$dH{3+%90hV@ zUQxe_X~L_dC8|BK7bsDcS=sP@Jcu}3W#3c^WUCun8GtBRP8#6}7FW&_@D>KFF|;9DU>bLs&biqXokNmVw+vt7kAIu_TJ*d9yUyBNKy31;{m zLO%>-cv}xyS!rh}QqOEVD;>w~tkevz8?4dzy{fWrQadyZuPgs1RefkbDd9f=%7!F5 z%sk{0TYJbONU~r-jQ4{RJ4?se`esG-ZqmQP5d$?n9gFmBL`eSsIzGg z)^IHsvxiKa0J0Zjc1cep{Wc(H%KL$=aav96=D@C*k`U0bEn%B@6iB`YfaJ@@iF}y> z@?|lSFOhs-CY`Rqk-llj1;)S|0g#}AKIRN=TLEC_B4DTqu86Fca)hYixM6=UvhKJP zQ7|bZVnk8Nx1J0l}( zTdRP)>8=*(I$it2aTFU3rkThz5Y;>wSX5t!ko7k)IKO2g!=Y)P$}l+^eS6GF3$;D% z-h3D@5O7WZQDnd{pGVfpV9GvV24BJs^GheW9D77o3!)G5U7>JvJ>Q-TN6P@QJYbbv zCnBy59Xn*^5*lV1>Sa+B=`loe=%dIqWUy9c5Q~h!ih9<16F z)+u`)o75dFtT0uI!yxI#t`9vSLskm8yDlPSj1I9E6!}>?8tY?4t zsT}l>6}O3&SeX^|=OP^lcY@)N5@J#P3^;yjc&v~zjLr&j!m4kcYc54aNk$2;LYG!+ zKJEdHskYmobI(A6)(R(PsP)qSMSgeyG3BiA!L2oNZQiG^y7Sl#A|nf~x{qDI*A1Zc zb$k8Zs>sTI$qaWd!{P*z#>#5tZpNXZe31eFt3`2b{lDQOJC*GK&ik&tuxx z-}N}UYdtRCus;P1)LYkMPJXCKXdd1aqjoK%SUPdR%9tJ+LOqGF1M0H`(XDu~SIrL# z;KuesS8IDoXmqKFO2>AE`DXQHyTR(qvjQdfvI(5XHk7g%_YrPW?K=-9+>JC)z1S*L zAowm7e6*WB3OkjD-7)DH4e96}TXrvsUd|WNmiReW*^6=lfllvebEDc!FgPF#C~6LZ zKL9d<=Yfo1r}9V7g$Ro|Ia0J0Hyt`AUAGO?ab>ybI}uBr@qtB9qQEFzJ*5lTLpoo&HQZhekT-9WHpO zq^CSkN^eF{983Bxm2|RsMA$GG>6mmzL-zD%(izQLu*dko|CsV0Gm(rwfVa-V>P8h@ zxG*@ZXXRYtadcd-51>WV9=$zFU(ivcYZk_wxZS~5H6eHwSClx*pBcRduWf}tJ~U{b zNJG6Vt>1(4t-qG4d%A}49emvDod@t%<@tO|EF66s?{mR+h%ah|quc+WY~LWf7J04b zs1FNM$@r<=-&J5*Ur)|KU#-1qF6|iF3a%gG^g;A2gnk569JX`GjK=7}qj4>EzAs-d z4m{BfPrKPhRVz5DZm@p;pTBcH*1L>4YYx^gj5_nujVlvYT#e4{e$ZEr_FB<1KGv#m z7-SrW5jpW-Qdy0NEE6JYA5qm(AQxD5Q;#(V*hx=R8xx8S_gDkj0j8pitxGHF4cINb zQfRMj_{;6pfM{ZLWMus=-!f3F2UZfJhmXW%`;@`{HD9mK(+ou(wkVdbsK1*Yr!+ng zb9C2#Xr2c2Yg80=(5S`tvh}zHmvTcMMs(tvs{f4@#=UQlLu?HZ)CJ(hD#a>7U8c^n znjQwS8)9?94HqBez&f%YsUy9u`$W;4={amBJ(2W8(w_hw`I4S=x*5V|3hbm~eo03pU*hO& zWb`n`Fx+L7rza87dk;az>L`m6d256<>BAvYIYmoh&HlA}%ge*)E_7e|QFH(n)V=Ql z5G7R+Cs}%zgkinSiwIE})ght#Cw6qBRYR3!(jE+{%Q zd0Mo%0nYJ0hw)r6W0S}w6~{4_s4~IN?wRxq%<&$n*jE+dLli`|d~6ZPMN4;=k}k*T zEG0@U56IN%1)EJOHEcO`>FCxs-k;h*e;<&!%VKz`Vfg5VthFR!G}~A6p;BQG9Z*@* z3(O4s4D3uC-I)#BpCV!==ugooI?derVFnzrtO5CNT!lzOp)5FHAwCNSn_#j+9@?>h z0lA`B3IJBQ$9gD(A(1Y;GZ5Lrl^STp}rCC%Qh zs@qk!ML8)x@IMARCXe>mL}nETRHiWzq6i*hCQT)qX!;OtB1XVM70~XE?R?o&QMU#y}!U!&}GQ)K7MULdf$W67s3_vn`~ zQXE~KUV=G>6xq*EWN0A~U76%+y^7wRfbUZCL*|;L#N@C@D8^w$XRoX2^Pa^VC&}ux z=buW{%nIb3Htt037_3)T;FW6#GCyzMls9Tb^^6qCAdUj5{$2ziCzg1n>BJwE+CdY{=n)VV37;8IV*Y{$_|B0RryFQza`TW1VpgMPY z;=d8S3wG^^Gv__yJlkQHDNwW!W=?ayhhRwj4uGBbcNMb`@8mOzEwV7&Y9hHh{zB3arfwja`9~< zM)4@3;O68LSen;S+yevnA&Q5stYu~v^5nIW9S4DFrsom$31%?NN$TcTExp#HcOEd0 zBXPl53wl0DaBuz+r>{t&5<<^j?^d-x-sW7o*lhFSyB&Rl-S746Ptq59@skG!N9H5m zI}A|{-~r)#5iBcbQ67cgodI6Cp~61Hn$iD%5I6A^h*WE#Idb0i6EDK5rRme zdtaNm`hyhvP>4w=Im0seWgJ*c-2S`eM{tSPFC;pt|Mv;VlP{E(Z8?2d614C=m<3cF z|CK$tUmIccofY_Kh|_$!dnufmxipgE145zP=R$=cBb4@32>k)}(;aa2Q~sUjniJKv zcp`hJ54-+7PbSSrmiZA9+`EQ*c+omRDoIu%Q|R8@#2i9JJ9!`2x2exwWEd~iT+T1N zbzKUFREWl(`C8`j9(?jOJ)oasu z+XFSm!o?+}WeexdFI!gPEnB=~fmemi#5DnR_`HGFSlG~9-RLb123i{f&1-`7?Y95y z!1T;Z(q{(@FP{7k5ZTXV#@ucO7I+()>jKs7>F_CPYiJ^k4vx=b25|pMW>$K( zX0UAab<6+~isv$;sATaHd}rY@ueT)7s54-*y3EKghZm?$>&fs;4aI`ba+2BnmL?>g zS+8!3Ig^f_OFNnZZ4EU@T5UsJT|-SrW3VC6j>}U*xtZk@X(+<&lS2woQT<^`i%sbr=re5#Tz#1*faTK#4 zOOl+za*V^$Ky{n9CDb*|F?c6iuNQ988BwgB_BeO`52YjxYH z3BBll^f{Hk*Sk2#2}&-d@*^UEtV4ql;M?Z3DONngv8H~F_; z&NU40-2b(Ekz$`R8FMw#sO5j@8`AGyV&_%9F99n~){prDs%Ji?4jBL7e32Rub11|C z=5(=hAj2o)QjPXs((##zAL;)VT?S~_)6K$<5BdF=4)V~*9}6X(?r@f2+{)3FJYFFE zmkDy=lh3*w!|*DT{=+o!SA`$xZMr)_$5m4MVLte4z>jqHb3f?fAKQ-`dA6DUq_cU` zZu{Y6UQ8x*e-w1L&>)XZM~us!(fBpP@Ish4`lZT*X=8eEJ;oS*o8}tEMw_Dod4V7>>rG?kBqaQ0lC@_`JFS) ze*QT7SI5~i$Jukn*|}?IKjdFB&d#kF`=NjBID6AL``U5#4dd)`kwcAs!?ZMWXwjfP6 zR=3qPsIZ8uqgAE2h9e((HtUq=uLHHzwzf6V^@i3!TT_P$-F{P3U5zr;wAQxP=+LdT zHQIz&s+&~;+XA()FmO$CV6BctnA5Q-J|4+E`nj6wps}uHWt06~qP6W9o7(X<)wLL; zX}!sqgK|kgBX>LTEZkN>MCHE@^*jLuy_twcbRpWp1T?Ul(NrYhEcv@cOng^kGSBdX zM1(&VOBM`Y0p$14Zz7@tcu?$r6lAAB_pcMdt46R1m@pNW)D!U@#s+c!mf%+Lzf1hv z(U2vaXBgiiBAq_~lJC!m;QNT+;;IE_oe(B5shj&;!f}x#zRENw-jlhd>>)02{Ujn6Oi)ROgqBgPDJ=8#C@OOv*Q07vA-_% z-xEP!fV7iO36OkN5aGU-?kMX!#D9n2F9aV0l72rC^kH#-SMYtoPv{T2iI5HHJ`ZGk ze3hFxNAL>zgRX!Gx@*L}Rj@;_oBp8NNCe%#K-T256G-<*h;VP14!jTtdmzIE46Z&P z;zZ^Mka}=FWXk+i5hvo4VMOq`9!NgzMDY1{aeqwkN%4P9{67|VC(4H5&H$3n*|a14 zmua7D7#9lW)BPgD_$BR#cOUHt_Y5)JFy5m(!v9&^6H(UWGZ{#F5AC2&5%({P`$ghD zN8A^P`!aE_B!d3|$Ry!n^z)EKLIze$h|trYB2AQ24`?W-#h{^{ze7ZrpMy3b6ZH~SK(enNkwKM}O_Ka&Xe5RmTQ7kAapp5`8)*|Sf750HP(C4M#6@&4_5 zfY@g6@vFb5|9%;_S;~Cg$Svy~$jrQT_Ib+%So6ukl1whvee$pZlke_JNP?K~9j!1bKJ945k$_TJT@C1j7iwtm&1@@`Ig9p0#HZP4FYmjI=si-n*VWmH~R6kWzuJoQPSK17!V?+)Ik z@3n62g=SejEaMG02{F>Gb`Z2M;AY%QT2p<^C_GrKuF@cL-ahg6&|u_+@JZ<88{uQ& zBY1oElSmj#fnIB4Mgrgc(CXE7#_@i<@s;IdrYHq!GfRVt?D!P%OUkH5#)bN1vg zrxw9tPAS9Rj->Fr5q+ieLZ>JeR%18AD}02@ml5@C912OQ1?m&GqpkB*!Fzth721gN zg`opOdxvn)NEc45hU(0_`}z<>o5$&o<@ymm?nYksD5*>>gfZvqKKpCF?#x^`;B*-xp3_a1r=QLD!7l-e@|K~Z~j3w3%?W!a+Y>OmSSJa(=1ch3$V!F~Tq zKB_%Y%~J;JKgw*_J2(}*d z;<#)J?EWfJiYgI(S8XuJx_bDq2uJBqbcowe|4u^j$C0(PczT34QnN{(1rq z?W?aL=*wbs1uC{wOJB+hM2&kedcn50s-7k4Tj2FxxIngr8~V93TrB&%_^y#JZFJwX z4kwfC&S^sM4Ak+%(JSC~XfT>3Ug&&+QunP zO43SHWY!cHoQo~h$CT9Pz;X{-93~Tg@hPJD_*+k-4%)NdhxDe|DRo&>Q%I&iiBqh? zTdGFFiPf{Xz8HV4sUA|@lZ?Mb_1EC9%K2$oa!1aKi{~vihEWH$u*dFWqCBp~3B6c- zMtOaNZ=#IahtwCTBdYI-UI9{2@BrJeN>Fq?j6Cj$ny0$-sGkm~F?mQ<_(-%4R+tR0 zx1hSM_A;P%VOt=8y@HYji^>Zx^Om;M1!3dmYgW>O(HQ0@yTSX6f>|pE=$bXi&ldeI`1p4uJg`iEx#QNZ7KVBF} zw@eVzbM>Tx&W?)?H1o%{jP&-C0lN6d_G^TrZKglzY~HlnerLmM%Y^RpK-WQoJT@H> zH7)k+fq?IZBijW`inC`j;rBeTiuS! z;b8^NHrjbPe0wvlhHr1d#qf1){H!)^hi?n4!Ts=9p2G$42$z8?<8eLwb-Wp#i(qPj z?UY5mtC0o=1P~D1LPVGFFJga4aG&7Qf-ecaCisTn9|b*7F7o}7;4Hy9!5aj-1Ve)N z2=bs9!w(AnTJSf5M+I3yNaq&hyV=%4i@OOgm2)-xiLfnl1OM({*<_TUY zSR~jY_zl6k1n&`iK=7d8M}m{kc{84~1}U-;4jO*zNo9kBZ%g zc_ibp@4>GX`*ngHf;S8H5YgD0V&5uwySV>A>^~N~U)+Bt_&AX3nFonjQ~V3v5&vhg zDnPy$0O@}r5&ju;hySI5mlHuZPwa~Y)jj!eFQXl6n=63ivxW#hTy-FywSqSjLEj_x z?+9)c_uIw3UGT@^{;z^R1CsxvMDVxo*Z(})AjXp>m@SBAp{=H7bU`RHGb1ZII}4w! zKmB^poJ%js(1V}9y&jZt78(OQe)TsM;{!w*_t@u(dVRfc#mBK)omlr~gX$tkLjmlk_)EbJqIZTXrIb@DXz1IVy=-4Yn=k z?f2&E_zSRJ6>&}+R-bc2dVIsqiGU*I#ts>lG`Mk+ovN*!k%^JMV=xa5;)asga$Rp2 z<#6Z?J^8tZ?8$n9R|irHViWPQ=3!)xwv-gE(7=lVK_!Iu8*i&mfi|2~I?W`Ae$K6)!1ePT+>ak;6_X!mu+T zmZ2(NXC(2o89IPLkDVb)eOSBycZpCaa`w;0>@X|j$rzsE1#fy!B>34d^`_z?gPH8WVnhBrp&x?R~8m-f7YMT9)M;8k)! zF<-taD?uX4ia@jpPNfiMBagJNr$6`+OOUiqzP#`jEc7IEdJ_4{$1^~l-Y2TJF@U7o z4iRPnmww1`P6-;=@GZCu93g@55(vB#-oZ>|c!x#Qv>fQxDs_I-DpClWIFfc)b z1`U|DqXvmMfk6`!>Hq;zg9Hc=ZIl4P1{)#5ps|Gsn544Z3}8h|{qR;it)9baZHv{m zv<(*t;0Sz_xqjy&TsPUb$i!kuj{+s^)9v( zR8V)73i{SfIlfAvk-Q*z>bX8gLA&#J2cno_+IWdHDY-ho}IqQ=*k_JYP?nv8BR zXe+1|D|80{ZrzC*e=f$*lVmg52k=_S`p%umFz9n0_$ z=;}bf&i1caY3BQH+Hr*6ykMeJImps|reh6~i;%1CF?Em#6~I zs^U_;qMP4zT>lt?Kg94a>>nSsTt!6ky58b;Q}(z8F>k6upnNXK(+Q}+jjPH-=(sLZ z*9T{=*9;n{4KE{&GDtoKoL zs@53k9@2Dg`aCzMa0Cf)Lk}QCyxFg*dmXE%BQfPXsk0T16>zJjTP1tLIvx$FVlT!n z(0;0`fUjbm@K8BCl(c4Xc7vG;evn4~J3BcXNre+gv;Z)aqOpmBFKiI(R7$%s z57CUXUR?R;-ITzFW^e9)?TGd7e5NCY-LhBIrq`WxdRj>NTE7D$_aU^(yh%EUxx>DX zuazAxMkh!+>wSe@<*7F~WHJUlSD3L?S*;)1Ly%~XJq%UZH4xcBe`GFKaUOku1p5yt ztRl2XHcz}xk;aHu2(BC3ij-}c*iM9xO0kBtbxE#2^0+-L1j^1-TwIVE`)sd25=ce4 z0pUtrG|GF@I3hIg6-v~`$jSksE&M`>KjMB_%rjRp28jRL7aHz0HiPtB71|_tK0r-6 zoq^0aloaq2PP4D|*i$Wg90}nJfpzSc=%`Zs>s8=ilf}uugp%ur20nPj4es~Di6{2f zlG@eQOsIE2Y#V5*fU<|2nRR&e)REVw4=fr^uW01vv7&py@^;4;Z~YkFar9j6pDE>n z;i2(X&J;M9BwSj%? zrl&X15ovE&4w5OJy0Q3Y4&l2#_>ZCK0Rr#(3_u;Em=*JTG^F7vNF0O+|A|i{C?+`s z+HnEjMDV-w!8RU|H)hlraF#ryAYm1rHy;>YsoBys$Cs8C5$kJkK*~{<3AB zQ6dKnKfK6r)c=?bt@woOI52%sPYC}nW4)IrORzT|It%OO1~f+UB^e&Co=p7C5i^D$ z^{jExq6Pi&8HpeLyZu$tK_0sC;mAk&qiMiHdZqeR{ihp$T9eh}xgQWxlzWInv8a zzA>3N3p`7fg=@h$Zd|w&cYk_rC5)zJ1KGxC+qDhg8rLpg2DWj-(q-ToF9OH-2Jnl& zMSk%D1OmHxUgLrVjfQKS6s%mZD2$Uzijxd_^^1>}57q4!g{0aWa2(Q^24r~>rwYy! zTrc>af{zRSMsUC29|b=UOhW}Ey}^R#3yv3@E_j2WEy&pr!xG_>e=WF2@Q`4d@M8N4<_nexP8YmYuvPHKf^ z_-6(GD)@<@;iU%A!O-6n!TEyR=cN71K+4#fyG2K%!=-(Z;C+HW7u+ZKyx@z1<_^&* z=r|dEwV)$-m*7spy@D~pzyO59fHGEaJdix%D~R9`*V7(!HwxY>xLxpd!8ZlHUhO|9 zSS}bATr0Ru@EO6s3Vtfs5B(q0bsCWAIg5z&e1-N%&w1irAUK-t@K-{FKX!W!{};UL z!orJ+iWKwu^Lf_~a?rt}`MD>)(~!$WsLF&d2pbBET1jifl|)!W@~0#fU4U3iJ>}2t&Gk402x7C=Vxa+;Y3qcJ7aP`*rR|G>Vzzqwvq}8D}Zt{0FlHav?A%0gZ^w9>;GT>p$XESOjAQ{|(Na zJyxZBc@b#;TGh3?HZq`d82_7JOKaVy)9hPQ3@LrEsPV;EY3J0D7Bcw^fVy#rDYh{# zy@HuaYKYYHbO1PK7U~^%(~1iN{h7*3*dM0*8oEoZC%qP!N$+DIm6>c)o^jCvkp4S> z)bdHBKO+4x9aJI|1Cnh`r2n-*ttQY>#Q>gkb9b6=-P33&ssjJ)FnmDo`5MZeez=;6 zgL)3uj0&As=xc$2aS_QnCzAG)K&FLAcNWmhStw+%pUV8@RAZ*1NABy%$KRmB>HRF~ zL}OeG&oZDp^G*dx9|P&mJhNYvP$ULNM*1X>DqV?G;KoeOo`s?-r;3$wI_XTxW&<-x4jVH= z?K6yvC80mGW}&cxd`vp<$IOukk&3>UO1cy2PNcier1`+mu}yzQ_N~FWHp?`|qNcB_ zZf>-65_*fnv3|f##opmmwp%0Kb8sz%(!B7zUHQBf{Z5+cDT=V?Fez(G6{wd?3N~Gz zY}e9lWUn*r8r1Nq$^Mn*4KdAVc0VOK4!$V(f{3BOY(T`Ueq@*@@Btci;{yx5WO)$n z@BSWbJJ7W7AE|4 zfKh@)ud+GS-&87PW%|bZb+=sLtjM&`HJfgE^+za{Rfu?s>YNAJE5M-N?OdB_%0Afi zjbuM-y7=!$E!EE==LOuVV8TOWB7;qe6#I2zk?3H*3A*3Kwu0(%vAt1_sMzDCMIJ?x zCwb+=1e$7-@yPehy)$aXkvtE{}?} zLBVO}P^qr&&?Dr=Y^2y-fJoSGa=bC)92cDy80o?-Ta6bB=-?K~}yA`b(q8 zBQ1`*MvipU2!-BkD?OL~0_BKIsQJx93>lKD;E%lMnjI81nSiGJekK7C#M2{S5dxZm z$B~h1gYh9D_|f7CzV(md9d)cnDzbm!YW&jbsVOD9Uo%y|e2C?~Pi2gFOd5e6ut*53~vt4vxx9 zfqm4Q9>=txf?;=b?o!nY)0Ms*Pj~J^M#?FykMyBiGwDO(aQ-5dIGk%47Gezg$}DCW zx!2=;?37qKdJ)NiE&+B$nDSBvq(biyX(jK@GRrv)6)I0llxXP^8A+UFUR+MwXkja3 zU>1Vf<50gbCuXu)ts>W?HWgSq9n0rb`YgO+x58)Z*C{F6s+4WH*iMA!YO%&1M)3!c zU+TUC_Ib`NnXMnXq8rsCI&6ce=tu(FASymh;x!>bo*p(sE&j)S0{|#=zRrrPvpM)Q zd9mqgC4%L^<;Wkz{s%HdOcksgkJ<_zp%iO8%@lS&H1fO-<(}9m&Vm zv3Ju|C!Utvw&J->Y+JR(hpGVgAAl#7if}gKBFT1eLhyegi@>9fF+pqOx~*8R@{{ZS z5#3BSFKViUR1)6RrA?n8Z_cn<1e(~b7DKi&IFl^X5j2g{TNWoC%ZiYo5SJMk(l~C} z*@L3XlJj7HWS?^_q<7e}Uppb#=1v%qriwdZEqU`0bY`mZr8_OZP zqe3qaHr=WZ@tE*K+~IRKm~jf-Hqumq#;7X?1}!x-@oZg{mUHp;Gao{aTK?Eq@P*tX z)(&{qi;{kkdoV<<^qQFrI)W8QH#&loEKJY?=$^ah`a~U9L8{nq`?*VBjXoTmo1VgT zyawy@&=;Xw=;Fb)d`bi;VSxR*0J@HpEa#GCO^<0!5tM_m{ag&$4c##%=Vj6Cr;tvy z=8JmIfG;ds&KU7->=e9WxzcWCA0?BDAV;%Xyl zMmeMJHB(p1FtZ#%ukzt{8L=!!dab!k=FHkFl{sdTnP4U>I5{hysdEZlkjkmtUr+6y z8(`pCikW2Q7#Ee5@~hIF_7r#e7|6tLQ|>0Ssr6-t^51(oB5N&^Nwe$M;Eb(-f8N#p zZYDmetB<-gS^rNm*G|r{tI$KSqvAbhW}+V1m7KC*!qRjd`WeX9=>FxO&9^JQ_8K~ns!MvNj2EM|Vn|D{I=H0zyp_1#dqNXcV0~ym}Z(8~j$-ngg)_CX> zvBtwnuh%*+Rg_%kL4Rqo%UD=kS|>)l z3Va$2lSr8U=&@0lO6^w((Z%deNNREE=*1=Oi$i$yUn~4IbMVoo%tj1qDGBb1;l1FN z54u&-t%1u-CBejwkK&%y`iLVs^&024UW>~S7CWCzH9^7>2enPjKbyYY^LzG~pLj7p z(d%{2msW7W*;o2eRo#(K_Mx!>ICefz<$e${k5v%AQcNwjt<;KIX8KsdtC@b}F82hb z>DCQO=V_1lgNWa~P|-z<2+C0gShA!NgXNSfF)=0sbsNcmoEzKR36dcTj>uX)lWhKb zny>XatQ+)!cT_5sHGLfoh9=%{iO;|>B({!UimlzOWHCj*3qhUl9)<(vuK<4J z6ULiOfLHp(zk2|PP~$KR0HF-Qtb=_m6fSeVt6ArIj!Rikj797`Ahx6q*dvFG-WiyS z|Muzl%z3x~#<}A#EtM_FZbz(J=jsfiK=PbvJ^B4(-TZ!4{%BI*^P_mH z;lBbr|GqHx;`!?>5v16YSOH-eXROMn7Bt4CdAN{)3DOsDy^lYF}_72moN*zs6a;mfGvyx5tm4lvkbbLcjVrHj<_^J{6jDHf&WvgkN48~uO4lr zY#{cK29jKbjdxqWPjT5-x$Mi0{X_()HV%4~_*k;f%X6;FY<+H|djoaS`gsDYAVv7wx1@_r3f{a&aW0jr+Jtg}%ik z5d^t<)V!pu6)N5Fn@w+;&8EaZ)P(|>bwx}KM%vYTQu0?-2wNcRebw@&*Ifc{>bSI- zjAgT9GK}J^#FOtS<=PI2(Y6ZogW$ z2(Y@H@#{!&kUL3PFI^MmppUcC|KKeyxI%)ET5fCkXIXB0u-x9?^_~o6@uAdWz~r#_ zVt2IR4V?wKE{Z>sTx2i55E6-l*|Oue?}ex2KH>{XXmI&$7~}N(Ha<9UGvn_(_5$qm zN~RrYM{S;fk%3Ja&W;+xKo9J-CzS2WxQLAWOVy-rK>?h>-3e72u0q}DTcK;itGYJy zD30u8aU{+K6uy5;)yVZHoSXW{tYKSm%L+>N6(|gWkf)@w zANOAqD`CgTdOkC6SrlGSmls|l`g1R+rJ~(=P*l5kVII}0QOzuL)u#M4%nR4$UB9Fr zs#)pw&xQ6XpuKDnHL_hWQEL!hvXnY;am<`Td)&ju>F$R5Q6A__)k=kvC*GARTv3VZ z8Q&v8(4;Hch&|V<=lx5Ot4sgIGJs=7A14!-QWaX1bX%}2{QpXCsG1h&@5RoRPj6Yl zHowPn6b*w7mIDy0F5AgDV&umf49GSmp{7kVc$slMpiHcdW^$7 zd3d3FNYMCO2!HvR|+z_B zArxu}%B0NXlKD_b4Y^4QR8n-35|s@Qo9=0!}EF})> zH$wIMs6vQca-bMh81z6=sc@{4vAjr0QKpnvCM7Ey7Tk2B(!trVV8J()-b-|nCziE@ z_>+>6;;f6ZQi}z@E%*b$J%R@W4+$O??1zTJ@VrQpI92dlf;S0LsTSSW3;tAaub?@X z|7S7(L$E*EG{c#5_+!O9L9kkIkzkA9PXwP7d`2)Xn2t_CrCX4fN6~zhU(8wl^7xMzaCc(P|)wy!edtA(i1^+78 z&&Uu8o+~(6aG~H*!G{ESuAAw6T+qlS9ujjVIu5$$2%alQS;X`_=yMSB&jddb9E1** z;f4r)O>mSTb(7G2li&k_N>3>4m7Y-GL9y=;{IlSDf&)Yraj@Xog6A6I%S^%B z1*3v@3I3bl?}<4c&r@Q4i-<;?h5?Us1`1LZlIDQmm4ep_ZWeq-@DGBWf=2|YkCk+W zW4DV~AUHvArr-^Nw+TKh*dh3yAot)%$1nIT!6w1)3GNnrL-0+({}LRE&YE=Y0kVB= z7V|#A+cAM)``iGezn=)!W3s{axg1FQR>AKH?iM^K_?qBg=v<)xVS?ufjuV_AxJqz? z;QfNT1z!{NVco9i362&VFL;mOBZ4~xUl2SOOaz9%L~w@SQo%a~e<=7X!A}KGJzaQn&3?fW!SRBZ3I376P#iHLNT3swP{-i1V@cLnX? zpL&Xk?L_#0neOoS8WH}Eiu*g_{(;~pbcesbm^9Ph89>q@^NHve94S~x1pP9>D+T8O zNvDPgI=9gt;kOFz6g(pM6|`Nt&m@B0HG{6{o?+pxc^M-e<`?K z+}p){0W^g}UwBeMNm4(_$@JA;e32IM{sQ`HE%d>Bg6e-h)`A$UTo3h=tbS`VY{2{2 zM$oKvaIUx0w9(&2*s3;5!OYkT6K;*%L=d*0mLWyB1GLmThqzI4EyLl~d4k^lv$y)! z(2L$V1&x;esoQ!nmud0hj01+qkL@o@)`q zTl9qsPzhW8ektkKZG83^g5rd*^emaHSpq;-R&9iF1We* z(Xyxb@mpXSFMH85AvVBPe9D-oWNSs|9&susH;bF{Q zb$%gT$8vz*P|0*C;{6aAP$e|kyG0eyWE9Zvk$LDV6_2G(!JS~3u83R5T;*b4z)bsE zc%23a)OGg-lQO3}T}lUg?cM6jwp%|)m684IjyEXWUm4BKk8@LW6==i?Mj_cOG?o(Y z3Uku7u;`qdtgDLO)!Rh4R_x+ld%&s~dY0E##metpWV*ndMOJLU)L4G;n<%!ONUWDv z-05qtodRAT7t~)Ynmf2;WYgu2?}YK`Jw52#LF&~+!{a@Kgz2zHWl}<4~q3iY1G8Hw{wH#oGB%uzp0%&$gUq`6&@^n1%X!Jn^s~g zp?aWv)6M9Ydwx@hgO*g_n@%v_bg|dedTb#-i4v7fKE;A@ec#+Wk&1@6)e(DuEmhTf zTh;RLO-OwfYQC=G17+!jfk;rY zxm+ELI4ljP5M**Pc*k9K#&U>sL@t0eL`yU)zXDbALFAcI|7!ELVrFzs=8QRJdmv9M zZRJIL8(ElhE$Y~|@ycCoD7#gvE6kY)BQ1~4oCIrT%htx(&Aaos8^lJ4EPEnXc4Ey7 zbqzGA{xB*Z3*@5K`5CyLF+Rz|ng;0Di!4;faKOf2)3I-StZ#8=e0bzpY_O>BjCABU zr{OleBIg!tdPNE;TvhH5QwV41vqpd03C2~9&+4Q^P6D&S{|_|ZZH+}Vb3HE(P+qysE=Co4ngyk+sdfe0#KS-~P_ zuQ?0j63diQJVYw^sG1ETH9`cQSs|*Cni@n<5BEk?(TCv|W4;A!Rxe?iA0N< zR;=F5DGDznOhAo8_v+o7#kflvP31d~V|i!hjEXuqcxKgM4(Cy-2BJ@>tuT8CdI!%I zI?8rxwkgJ}#MxA>j0V$#)@;za0N!V5%~M#!d7=-H*36UADtrZ-FNGf_#vqL&cHkSZ z%s;A*dJ;a^KvKT$N?4%yb7$1e$D7(Ki>@b8a}>pussOeL8D=SbPCuFdyu?^&kI>`2 z?ml$GkKfLQ4>^lY8MN_Ex+shYW8xUjY$Pi;Qo)<5X|g)(H^{g;==y`qb8qoL?Rc6najfXj4@ z=UEMT^B328@-WR_Seu7yDz)qUCCeArj`HXW?~dG*T| za(fD!Rb``=)i3xKv+}RsV}hQE)SlA6U>oU#Iw$T2FUdUY!}(yj)nk>g7krHlQ~`Qr z$H~vg9`zhCW0X?QVHYi0&>x?X_|d=HUoIWwp&Oq7exyH|20WzqBmJuW(~ZAN1;PGM z&l>QpOkA|!JAdW)(Z6}t!k_ywZq=|f?{ud>Lz`yf1~yGH5467@{uc5@9^;RQ;z~ZR z4}{JRSTcGPrAiyinkz9=CV2H!!f_d2Eb6GF*RlUqrz0C7_ znQ!Q2_Lv3v0w|PFDu%J?F&cXwV?5V4&c{~C^^FU#Q38d-u3Ns6_mU+q4Qt>9W$Ico z^`@>Wlk3BdQ&H@B6d#p51fvW14`Q}Zk!U|ykUF+#=I}r?I(_TK+${KWL8Fhs=wBF$ z3deAx1&t2hd1Ag*uvu_}AaBcNxc!2!2^yWe?Az%+L+}MeuvTbW~!aBe#=gH0VEz`};&xdZX)> zI%|2S*=iu^JxwzjhS`hALW5!W4j|qCLPW!QTkw6sZgIEJ*y-;SAnz_Zl?Wc6U+l*a z5x!XP>w;H`dxhXEAj4lrMEDlkBiuT{y9Bw1L4S`B;ZN;hz~Ar0eU~8hFwoy?G$Vce z(dLNNMEIXC_)WnZ#r@lYO+bcULqt4xihVososLdoLEXZJ+T}^sR$h{~HTJ~1cZ0z% zu6wr}aq$VN|0mYHyZSB85ixr`K-jL?QCd13lhDJo)G9qOF*{0Ai{d9?qPYzw%swBZ zhLgGLaHg;HpP7cU#cD@ARMSV1g!-Xa-k49L}FMAfZaV0N78{^YjxbZ;qYUA=3@E1)u5$FzH zHy}7zN(TV>ys*mn8&bvyTCYK`74K;8lMfS01)RC(JKstA@@?FS+o50`EjzVauhm-* zZt<&-R6OSUsS3!YM?Agakk_wxTRd(GMIF3}tmO^#@~QOVFmy>DW>jZyT`5Ws;@IO< zu6^tN8{G!Vss$b#tJA6Mv^0lD@d~*%mO~JJEYeh&M;1WP`|QJzp~@tt#%$k>hfb}a?LbJGg=wda!VIQ}_g()NRH>f4uspJB!4{p z*d9#IEc(@gqCS4P=>el{Kp zBLyn3co;}~mf;L)ePH2MzymFknlRLQ9lknXnPQfc&SCL(6i7NO-5IJLJ;XG0tNvui z;dFTYBg{^AhCRg37p=k?n!`u(Z`3OikD6wYR$B3@KVm@7_)JzyiL2edT03 ztp^*IILxTB1K$Ug(=i@OquGvgX6q}B1&*RIu|z4iw3Q+9W8HB$>T4xz)MzQ+sUEw( zGkl^w+!-QNL!f%>H|G2ij@LVxZdp}ou%8M9&Ry+vP@U+j^V$H zTeDq^yHwqC^(;;0lAmFD@PhJVeN*Mf4}v;X=H%Xi!db4$PubxzMV6lmfWj(*KU*0h zzka9`2A)6y3l$fBq4?`#8S%694O7lQizM~FhK)~Ko&g3ctiTWvmlhbrCGqcItDp^) zODU?Tu{Xq0Y(Odc)_xzJquILSLVR&+aH>DAR`C4Cq2R-7j7fjeF5_-myw{*!Z%fPU? zammdKYdw(OtzUlQ;$;P%iBl$3l+99-y<-)(M*aU^sh0DEJd#D28=EIfUV8IL>e(DH zTGA^=Vhca^TIYxvPU_j}qD2e(<1-RJ`ZxX_gTE;gQhag>;aIZu}V@ ziLrmRSG@GYXBvLyoo4#`BD@dEJXJA(Jcb4l9YFHo{__D?a;nb0g85|`GC3x%c=hmp zEjDxW7<$HR&~5)^cptK4P-RVC2lL9Z<&S2jk?CVPX+9g6?J~1rn0(Ov0+@4M?i@z* zT;?LY`(0*sM)@vt2=DBxO?+i|=QlRy%kfT{#?0?Un#Mc}@1$wW)p!rN%nR^d<}%|o z`Fs)H%njn3ZGgO(a5A(mr${gJ?Y+$FdzsCGaDk^`(alT3V6X5pCurovb?o4=EI{u8 zHcNQvyz9YdSxVDbh@D;p2F%4^z!Z5Xh6)+hfTsb9cnduZqbZ1ro`zzmko>0*DtaMG zq4GW`wfNL;-@LI=Nt=S?%_q3{{cTjNG!)`Tf@h!-(>zLWwxHp&EEV&21Ub{9|Cr#L zf<9Do+Mh1?H9^CFF?S|iBlbM1MgNBXvPR6C1Um%dg657SUcX9xfqfB{;m;8?dNMB- z^K8NE1?vTG61-XPSAtzY-g*AIn9UtY{e=&6f#4XyO9W>KUN6`v_=wB1OMS?4Uyd(Zmnvss53T_kJDR@xu&w?Kaek#b0iQ)SRo-KHeptqzkFodb??I%l$J~)bnV9jJt!>zKD~){ZH%Ph1-obPF*~6U`!+B0H->w`6mGi@_ z=3SYWFaHaqIVhO}DGg_Qfer09H(7;m7w@r0VxgYtTYnb_aJCpN{W;atr}UCk`z@!0 zYayrLW2gLM3!VyW_T*fx2ciYP<*ThHfVt;8-u&XHZLh!S68I~Ek35{K3P!z$ocw6^ z156(j0F5{b%lxR-u4Nl0#`2qYS~#2tP5zK>h)-_DWe8SD*q<3$t2=2?BmN7v`El#-Sn286EYBnNlYPJN>F_@#o--LqEPgI>Aa&sM*jqf;s=r5$ zD&3kw-X2tCkFl?`oTXXLHG$%;rg2DA0ccb+#gA=p(o&j8f+LPX2?^W-P(U&nSQLX1 z8_GKMxml4HNYA+;=q$}Heh2)ZtfpclR+AeEa*$aVY9_dl3WD0M5ywbxa;)H0A%t?w zii6ZkD2Xa(d5$wRgpxQ1zN(|yPi&a%oDYhnDtGPHF@DVUvSXvzCS1-G6+-LZPfwBUjSXQjvfWXF+o1P>th0)NUvM10ePT?7PSqAAQGz?Mz9CtM^&+8|I7 zD>xh3t(+Vy8^*P8si{$3sMYkXtHN9yQB^q&nNC$czR^fTUM=-0jmw`(e<}8K0VvY+ z$*~)yK4DAuRgkN4Zt|j7y+}0HC z%YfV)ehDo%lOLz42qf_>ex~aqK8ABX1fy6RRd&D#h-aAEm>oZhHId;We{9~J!zJRb zc*n6fjou8_M^rbh3=b7TH~@yb3PB%-g0dPscuL{^ihBTuVX&ARAVLcc|S&yT6%KZq2BAz1-J&^&dO zWltkJvB*Bdxw^oaZWSMDIv2%L8ue~KdtkES6iKVc(f;zMAdl9U_5TpWM_vP!B4@VO zxh{Y(h44`tEx)@bpM|0`XuXtd3&zU-hTs?K;Af-$LXn0uGs{_4RQ!I^SR^5vB+z6? zWZSt+uaky5qUzCY=oAEW3i^@6`|-0QhgDYT@qd0V!vFXp1kSH9-xym<_t^ zzsx$yln;^XATEFTZp62f%_1w=OBeGM{Ab3V*@$RyVrG;_WCAj~g{ zSx;E7L5r!sSwnEf->?)D88zdtU2fi%H<-7E+9mUEQ1?G@F0=`>h|deoMf^1Hl)1Y( zkHrF}y_w4o7xUYK3l=ZDrJ*)8drHooP9ncucv101Uv%yil#lz*=T4c02hpumXi;8U zkNC53nH)>Ga9_#nBK9Qcx+-mT;kvD+Hcj6|?~&^a1)tkBH&U!V$67xU!{8RyFm^cT zPoaPrLb|1K4@5F~>mNACN8+k+NoVS7VTILlsOm~UMvkZ>Tb=8m$Xc1M!$oBG!aB95 zpjnHE57FrThh$vX@PK7rY(OeH)~;l9tMR!s^Xws}lYKrmnshqL{#iP?%XM5janBrC zc+OSO>Z#It6{JD8GOfB(HDjU5h+{pV%Ld2q5pNol<6>a)`k@OFg@lC1M!+Tya;w>X zo%(oM*AW(2j~qMz^1X6^%@AA-3L@nv&q4SaN&QS*yr@eV5}@<-3?L7>CMRL1Nb4LV zA`*|NGz@2f`-nZe`0(c4nR=YQq^4_#8L3pjy8Rd)=3vd~Bgil>0#!p`PAj3;mI2WFFGx6TlBuG5PeSXWT;Y zVczMP{=UfALLS5y8C!NE;ccMHv19;#%m>S+_t?^cav`tEJnl3ZJEGhE%Zx3b$D;!~ zpa+kRV+Wf7)6KES{hVm*`KoweUEtW_kq#V}lFw_b<>fL&o|IlBVm};NR41=wr8pL_ zk0mm0B4w0_7Yoi1yiTx5@Oy&)NAOX>or2E^z9X21+w*?;) z+$FeIFd_J{U>eGT@th_2H9@e}YUkIVU~Iz(Ru~8b)PD?Sc)utJj=-2PqrWg;pj19| z5P2nE;6!&PuYM~AH|%U!NGP^2{z}t!B~O7JCkj$v^CpY1O&{0M3M>&Ihn6CByjJZ^ zVIyiUgUhX}+x5{MeY6RZ7j1Ogst@pNRw5IS=GYEPNM1Ziw{7Y;4@d_<8iE^tq???Y z(h?#fF9N|0jvS!*2ti~*WLLUHi%P83au;$VM2nJzK(-=g&kszn9wrMdE*^J-nuh3K}oB?uGgw0wALyKFKq=U{X5?BG5 z5r}(ZP7ub*xW2v2Uk7X6I!&<*Pt)1R{#v{=w^`k=Yo{ZRI@H3VWOU@%Z|MufSvDo# z@+QNbhj6Q4*E1Y8dy!fCW$Y`p7NOuCW?VZmVb5E_J->EjR)iD%U*>!YG4EEX4COo1e0P z1(}Q}@^ehL>RR9St-Bdsn#yOWBD}$)woFl0jk8_brdou-tr`l6|0`3qUDfN=yFY`0 zW3>`;#}sEp^Df>kS%>lo;9J#Vu^4bN?Y-Qi*2+trV0uQ2%0dZ!)cDq`;?h+-TJQ^0 zqEc{Pg1ygHm1B|!UO_rCz6z1@CX7p3yBizZsbQ=e#8orMCGl-=cQU~jtM*qnQHknf zumIg6_~I+RBbz|9pd(tkH=28YGDC~_HC*AMbc0sOwa|Gz3u%EVVDY2-TK|FRM}nuPHo$u|%BK0L z%wxY#?KbbSKOM2N_+xCNR^h8xBT-CeZPQ#Nx(x=TX*(dA``u`8XS}bHq*$x!WbA6A zQhFAu2~=x~hPm7Wm^fF$#D-vdH#n!UDWrkl(!3`V2`Gl*!N}wIEY(uEfACQgFI`>5 zXv*)goS|@A;goJ_?Y4{UPkb#$G08{n3oj zceL(Xz6Uj1HJHJP^LgySG@nGSHmT}kozdFX)DAZmSKNA4m6YxUW7nvXf>J>4b0GdU zc!_3ja*X!lIIfM@t}+L`T@Vk0ppp_n+24_X0;>HGfTEsesM4bP`))#*e%Txp| zmv0SA_B$$(xrd_Jx8p|Y^w=0!tpPzW&mRwsngCrf=TLB_BT*=FL~Gc1P9UV<<upr|*{CF`R@BlhEy*P$)0XnxZR+97`brR9z-3D~ld@fp#@Wx?;peSm_hF^;YWz_A5OUQB#rW{ES(SseNKuMoy|Rh2U`dgO65v5z{KXM(a0N&T=lLS0Gijc5r~b(vx@^r)y3s zuQ9E>Y(n|}f8{l5xNwmtOR)DEkN33&;6|^g;A+*72|P#4!gldI=%NLu1r{=a#rnf|`WTJQ+`RWX1(h6WM+Tk`oU z7J!ez#Hl&UgM9+akjXJ+f>+POC_k2!c?>;cHt4qhGHXFYhj^m3Tu=9YD&4fJQRR}R z)`i9o^O0)|K$7B5bS8odPNvg=qcr*z_68XC4iL0RDf;8XG;LJ^ zrRZIb(zI5~y^FxRNVJFyITkZyX(f*81X z!+chC>1*|>O|RjM_`B}ZL6ajoXs#jX@e1Kq(S&Xu`EV`ukR7$g+p1;X^>Pl8`4Jhi zlg~ISv6R7}gMeJKA(aKR#Tp9Jrg{+I2q>#AEPXho=EOi&UsLFpa_{J5K~OECM2wwV zm>Z3-dnJffAh^;K0ncR!6atx$CSFD|*nLBETzMgOBk+#IDoOZldiAH*VxU{6z4;NM zfv?p#bkChHa2r7=9~2<)UV%d4)81!_e!+oe+5drZ2;aK5Q9B(d?ybc9a9}+>T6bXV z>UJ)6svq~**IL~^XDscbxsQ_oa`+%k*+VgF(_gN{8oK=FcvoLIcWCP?O%KDQzGCib zdsqWQor&~r#7o>d6eCkaohzRdS0b3c)?cDKF>0wf0A)iAGXo0J3*gb#+IH9aSHXbG zJmOkhmIsb2A%$sWYze`^)*^`DuVTj0hRKwYS=ZAB+#Ffy&%*ozbAT)iaM`|=B19PP z6F|V^%)-}lCM;2SDT(gGn0Oq-oahkeE-lZYy*fHK{5IZAm)iqtKmk;PNlA8g99wc| zi#i3D05VK9*P0!T2~QoUboWu=hVVZQh7Uq=hp901PtP*jf%j7xX&LH?J31)Y5W#sr zC~V2YKNVFPbIw^;EccL1xOkzej_#ovc*AWkUVVoqyh%}EKBgvq$Ygt;N_gVqYrP2} zF&*iX2NS3_#0olNV~b#j5201Zo1Bj$X$5*`I+lsTLady>BHiaAG#mq*zH6$eq3*)| z?@WFH$u)^n*SwAZ(_BpN<4Qj_=AFqXFx`I*(TIlcNz41E2se*o!qa>$^DL4L!C(^0 zBqRO#J-$LU`d>`HbOH~niEM6{F0z(A=%dZ40?d!2xjfn50R0m23^M7lJe|q=NgHiz zfL%Gj0+DJh=nI_`x~bSf=Qo(nQLeqr?ik4n3~C+KxNK`(T{#k@wJl>xN&PXKU_p`c z2Z7WC{20)E(TK5UN9wk3gLy_B+a`K|4nTV!(&keEGOq*ao!C=i-5IOeq&{d6S7$Y1 z-uI9K#t=*IR$T=sY=#jTK79fdqqivOY$r2gpbD$+Yqh=iz@Y`+M&`i(gLrX<&>gbw zcJ4i{tSW^EdfrZ@1v-l+%le3vPCfe`o?f62UL5iykh#&B*Ia`cbt(<1+d~e+6ZI(^ z1@cnEcYz|DPoLeY7u3S}2ViFZqIop>dNe1PM&bPT?XlHxwTG}S`PRph63}{o-x)@G zBx#KzvB_5L7gej=$-NVD<~O4zlwp0KI|WSi?d)~Vuo`$a=Y^odBI-Sz*Ss~J>5L_{ z$t5~UWr*CDsG|h7<}o?eX;P%c*6GW6w5QC;v4=Xo@wU%VQQMVK&#@;^$ts;QC)#Hw zBd)N$_a~OH{taX0_=p-$^6azQjjVnJES-T29!w{_ZotsXB2IBn zM6<$nSgQS^&ciX)wW}tzd^{45ONTnxNyUarGnv60Im5HOzEaaoz_ zrZn`ET-}}rMb%tu=Jq!dL8xwMq7BjDX%Ksl^^Fft%mS5oM~X%movzhC+0!nk#+}Av z+f!b^fGcb(JZM{k?8-spq=n`6*u6-Ne^rskXa5SzBlB_f+$8GFa?-${S&Ndu0Gt~F z&lIYBe68!~Rc=sZS}n@hpit5~cp@hdJ5`G1HwdUYz@u2`g4q-6pX~5_Eti7`EUEfQ zSHcabv=IFFwN#=*L$bVE>JT1@&2jpo_GnMvwTINwHVu`YZPm%%;e>bK*i|MCNS!^_ z%Qy5>J8&K44!tvMD?JQxOb?KtHuj*{m`sgi%ZKP)O0sv7{eJiqwSfI0RLXZA?Rvjs zpA6ROj6)~V7misSUHcrROFpi&!5G$v=6gA~1-NoJlICZE!u}O%aO0^k*uv2s!T}a7 zd>dd!oobNS&82P9R;#N-}Dg8eK&TRt^uv%Na!q7-vG2aVK&5y`iEpi^|gKiVnQA_ zk!CTPeP&M8^@%Hrm*Iq(vI-FOzN-&<^zpU)3g-CGsP}I`=6w|bx|A@MNx37^WVvex zQ-&<|3RUd#CBROUqj@QHJD3Zz2WR2)R9hQxhO4wSXCvxW)5)~G6*#cR5>E~0?8(`^ zm>UK{de)WjqxORnG67!?V>c2R^?sn|c{*>CmLExcO*^WAJ@GruWYKqN=m}&r_dN-s zio1`m^-k?cO}!FpT_F-pFi6uuBSG`fD*m0|ZMG(PT!TWQr9oMIOEoCAG~LP)|BjAG z$-5ydlh9C5YdI0ymw5F<(;#zQh4sRAQf|J6GEd+1w%4*&lrvTf8oYaX_6KD~KU)!E>|pq^ocpm;s3a;#_Ph#4b- zdR}+Yq6Pi&8HpeL8-GXPFH8q{=*EX%5?xY~H${K{j5lTYOgZ2`n6#4)>G4;NAL*GV zU-%TpY}_~(Gw*bxKSP^l;|BE#$vn{h68Kxl7kP|7BBu7qXB7hG!2+v34|`~q3)730 zOAmSvLZ1WsO!F9e#%$1S|7Cbs{EmEBMwCxrUQLsLU6=zu|Dfmqz^e23)2#u zM!ucoXp=e!rH(%#A4YfmyQtJ@DDczJPKYA~>jb||M8#|s^A^EJ1$nCy!|fA%Uhq{x zqr3hhbVBr3CfNJD(+aV_O>mvyF9dfB?h`cUo!%Amr-DNe2je+UFeqqr;$J~C^7AdR zZxOs(@FBtfB*GtMqDb#K!B+&22>wNIHm1?^*C5y^xKglP@Q;EY0;&7^GcljiSBF1K zut=~<(CFZ=6SFzjv|Y?RuS9x(7V}$zeIYN*bY%;kDL7hig5cGH*9)!^yj?IV_*+49 zUg>o)eCHub~;~dkLK~@hw5}&S~}vo+bDd!Se(s z2wpF^Lhv@h`viX^_)9^fTR$P@4+S|`Fy20b=K-nXnfyfB&k>v}_$?ypNu!vlgN*)u zC%8xOIU@YMB4(;%rN7~V)P76zC4wQr#e&NO?-ZoUSEg&LnA-*46#P^$cc7+wp5Qpa znSw^we!ZBN3jRoNtKcrd-a7c-7W)qb`=W1UKC%S!1kV*L6}(QcR&b?Yv!E?FmGvZ# z`zVWVUbr-z+(Un`g#o&Jv3-Ct>$!tEB*?JIcflRh{lmvc}aOXQcWrrW46XyR|imz_|G{bK9wb-y} z{xm&YdqsG9#w5xx+m;`cgU%G}NUwem&Z`{DZ!{CoA&XA5>T8$xwW8C~N~UJo_zH;e zerRKn%L93QM-z-^XW(MOV=vhs+B-6)VlfOO)`b4dxV1B$JLuSNld`w09d`izVP`!?uPnWxBkq)Y}dPd?u{ez5p z=+C&(!n_CY?k3Tjo^cDEz~2{{|6T_Imm{os3_T)--Q>f2Vm82pE>)$Q>0{Z@Z1R9t z&t4eNH67Y|3%lQ9;V}q$mg161HY}7&y`MK0cJ-5J?Bxq0_a^tnjkz~ zaH}9L6`JSjHa_<}YjN=xKa;VMl?jjOHwLMdhPLB>eSR{ROi^cb4Kjfvt&kN0v$oN< zj+bkY1?1cng3A`pqa8S_c%J1_`lzTm~VOugcKc2le@+YL~7O%eB zw&a$Arb?cd)dd#5n|!y5Xzt&bf0jPQuvYiTzpwQ|v^4CcX+A4-JClQ%J5=egRym(r z5c#PyC)Xq|)D-A^r+0q!#gFBWdnU^zyG-9LAxR&Zn&!rMt)OCUrWCk^ImGsxG2m-fIkNwc`X


    kg4heK- z>*8zPomVoqpy}e|{&1>%sd1ejM%!3}9B{Oaqi?(;>hS#@=}r6j!O}cx2#Ro-?;c){@Byz=r8nFLoJx9$9Ff_69$ZZQtG?TV z1G^KDYMij8h$qftF~zZh98y$C9v5P3`03x#`VqnZ=!vzu0Q82{{coRqEE= zWOjGXxZ8dw1c&9;NIyf zAT_EJsnv@}UOwB8+kFkpbZ7gaTDfZ77}$Q$N-K~Fv>()2GO+2FOm5YzP+|~zX91e_ zV=61#4?4gr;2v-WP>`VKyiV#{8R{F!+q}{`FZP&qcUn50J*m*O_GnftNQO$Lp`q$tb+0owOE=__xq+r@;2O{^ z%o*M*y{cOB@{FkWDHH)KeCddesI`Y8qZkxG7c;{NaqxLnlcPwOQ9^E|}b z-TZMn76>_=@Az8kGGSbS+Yx*%6R}=!29mo5nVlADU7`GAq+pq#}|k^Ofem`3wIck zRV`M6Jvb#)!<{qbMBT)C&yr3kTUC}f!WOdS+~5qvZfbN_6P@|wx(CH<+6^iIff77)DR1e~`=Xh^NPtYCz@N z?((%f;quQU!#{?~xb*=hVGI-VzAKeqtteU1b<)<|h@CL^XyjW@2BxcjP4!10SrpCQ zb)xj1=ZZf^$8XYGcB1rB8KlXdq!+?cR8jz0n3(td=uJ{hVmiII1bU=*PR__VnHh7k zN6ztLp{!@2>rP`GSCdq-BWF0%GUa9%Rp5t${^s47JqCG2wXTazOI7<$&Nt1zyZS7B z@pVGB%0x7`FJyZ9>kUCu`}^q6-r44B>5u5}Ma;(R)69Qo$EUb5{45?#z8V*rsNQ*Q zF!DWjQKQDQe@WE`l&`7^h`J1O792kQXHkb|B_myazC;IluJ{6xA94+!t7I)q{X>sC zmQ@M8B4$|-7l{P}m|5Yt4Cd^|c5Hxr`u3h@b|E&=BSgrAz(j0xA{P~?m;a7LKU!`( z7tx?CCi1vL`k|b8hWRk0>D}gvUgj*Aoq?=J&RDgPuSF2FF4ch{*xO^v_+K>wdl;8a zx+6kR+c3b;|K)HUAj(4SXE z+K#6>cTDcs*SSv*C3@_Fbc2e!bAPN)9w^YvxH|W%kr;2XL9PW#BH!;Z_$*D<$JBa} z%Nf-xWp=X8?3ZpIpLwT#L z$gmZ@RDZWT=1S#NcW?;%kIrYi-j6-gwXgH(t{v*23_cYb5I-3^*u)!M05Y50)rvbQD({SP00fcn*e^_53K)L?Ty5FTJ7^K#s)_w5m z!7)U8Y&l*spqL0J5uvL_y76n+>pNpGAM&d8J3Yaw78->Cby4phM!XwWT8nUcDdx?H z=@zf%EE9CY2hE1qDfSQ*6gr&brng_1&VfZKsv^R-ihxAepDinPunv6df*eRW_omXZ zRN?s6)j$%%-jS?d53$&gFD!y~8XXSyE<-#=e=Zas6iVrGdoD3t$$A`n6J4|YQHN_f zh$y=gZ$au}dL4+WZBX_wrK|T8p_OaFm}`)zQhoPNvN_@oAxr^$>o*{1s=iOu>-V7U z^Bl8&52}$+%eO^dNK`8!kC}BLL}+8Gnw852LJV@$UFs z`~p39P8@`o2fuTCVq;1CwK2a(o`>MqiIBt7%z|#`;Tix9sbtK+A<_fR*hhEfzJZP3`CC9)I24X%Khy}jZ zai}L)6;xufcqZCk-iNE!ph@HCr4J6cVD6wjtcD+2iv~hbm6FLTxeAKrWPBGXy*~5? z%`2Tp#w+3Sp!ON-55IBT5E*$H90_|E>r2UX`AuU|?SWRvK+u6sxYL~6+e2GuqjxJx zCPPUdR*vOy3;MvQO+^v7s-S6Pr_z3c@mJA=4i#o%Vmw{P(Xp@Ox&jsNt0-`&)@m*W zLx5An<1+;=lBODHD#q+kXi8*y#dv^9@wVeu_yoNdk3e^ulQHXm5mvlyvS==cqv3Vl16M&;9jrPC13vnp)+i) zs}a60kWUD|(lhv`pDe-Ndt{rjxNN`%kr(c$E^1FJ6L^l8v6xoR3KuO}SlaqHnX^^! zS1)b^(&7`qkMu{=fQSC(=~wkX5C7>N5;Xpfz@JG2oRlB>nL?%ZPh;%$F%MnpsRt zpK)hBq@6LJi}yU2`2xJ>yUhH~tQ*FE5#IST=C9*D07e?eh91 z5MpbfX^f{~DNI4nGK#P*!!=G9DcLnhur(}&1luC$;)bMJfRbv3o_V#k^Xeh4HrjLJ z!f@S^T8gkK-QGdED}CIH7B33JiaWUTmVfj5@KSl3uix~|X~*4-2H)2&TQIMFA+uC~ zANPA%H;Cf}`MqgY?Ho8?%r_8+cvSh)oeLAFmqSBrgE z(8$k4#r*GrF9;qId|z+?Xp{a3!5am6Uoz#%lpGx1sWzAXJ{25=RV>Xx!3{v>Zx<2y zdq(Wv7tFvwME~arP7wTt;7Y-@MC9*>Vtz*O4MBB38|Y;96MBM1o^7g_7Yg$JHpY9i z;BA7N1n(F8iJ;NT{aZ0VBlxOd1{yZ$og%2@+h86o=1RdDL9+Pi|2DxN3GNsCgWwUt z_XST!2hVVq2u>9IhTsChn*}3+-xs`3aEsvMg6iHk&^;*T*9G4c{JUTpI(n5)!5qPn zf`x)t2v!JQEyz=_D&K+|1@9I755b=a{z`DK;46Yh1V0rt@_DB~n1uAdCOA%TqTp=7 z`GOHaNAO<3rv(2X_?qB<3HHTAi18SCJszZ@xl-`Ef}aR>3-%eP{S6QtEO>_CFu|`0 zju4zLSSPqjaIN4*!5<3l5qwk7$l;}l{M@O67YSY?xIl0*5$&y!c)G`PyV$Q6{J!9Q zf{zj5@0ViUA-G@gfZ$7luL-^{mv(Ab42tJ;BcePd!!gEqERg zLPG^a%p*#Hl;gdOn1{I*?UCO3f?>f;M1*^Qh;aV_WVl}t5pFx}5$>Si>w>umL;Ldt z#|usqyixEwf~`b+pYMtJr-B)XlXO-K-Y&R7@P~qr2>x2|Il&`>9|-yo2kD(j#P6$O z9xXUquv&1T;8G%fw~2Y9;8ww%f(HeU3i3`(#+NO4rr;>S@q$+fen)V%;5~v53;s;- z3BiMc9|(RXcoEh|j2BDPh6OG`IwOBO<|Mj}FAfxbVco`SQP=nc)&G3xkVfb0dW;+$ zxNc6s9@_{fFf=M1NXUix4oQu|wsuUI@LumPf)3=(N;et!WkDNEj=uW9w_+gLvn(o> zQ?~(Xl@fA>Z6=r@4WLjmvj99c>Q;OVrY)RbZVB;U0RNh%X+9!7LKdZsVBLcaKd7w` z*%U~|Xyx{lG+=1G+Xxl(6Ch=vqKHjmEL4>LxN{{KV=CG!15GmxUrzbzL0=e+h=ydD z6ghYs_SeU)zA4h`AbiP8Y+F>oQ)C-d#G8B-H7x-PCg7wgcKa{k?2&WtJ>v7;WA7aa zhCFhGB(yLpt8Xu{31}TumMmc&zJ?mNR1%oGBIfSLddEVixASORr43t4ki_Wx6q`NR z;OW@W@d?ySeA)#KTJapsv+w*=DK75V+3_hP$ha9hi>8q-k=6DYQwxyLZMXSE1zfRiP}T z36ui4n}QP+=X4`IA$u^&yV6c~PKgFHoB!Lgclug*Ik#+SJp)A}F>07&xR#Z0z^+y@ zqb&l86qx`gxD7MZUUDv!kn79R ziJh(FD#AI@(*7Ca*imGRV;9lfo#|`gmC$<6h$Rr49im@UrGH?Ff;Y>;jB01FbF~FMOV7}9 z7L^Yna4jlTg;T`i3}+(10TSM-PK?`M=LH*}MAaD3S`U>)YyC00SCPse`IcoDYDz|0 z0-H8BfC)b0Q1hp!_|`?~2NI9%87O^^lB;a*(|}hL6MK(PJRJ!ujipVAtBu=RzX3Hh z?2e%IpbOoT*;nt}ulEk)L$&acK9`U3I|@2o`^ZM{eD*6$a&gR^mmYQ}sy%AQ2pRg&7%XD?iOYx@K%Z`qCW+9zbz*XJ#3pWqEI zZ15y%831{aSlct0hxUG#hZinwzs#B!UbLkBvdqPK^^4js^WL~9>`9Da0G-6&h2ZzI zU!kw@eDH7AJ8GBzdVF`*vah3jD>90jyDeV}^_iJME{6DJ2LdRkaH-@Gt%3E1N&ev9 zP=e^1KqV9#uB-XRU911Z&Z2o2uU;v)&rbX&_Zm>8bO!N;gXI3kRSo@C!9&%mrTtb> zZ|rV_nN5Mst$3M^2#9nbQi#>q(_=CKwd-t@08A@Is@`Jo3JPLt zy4(Fv!eS1*KTPlV77qd`NJ^ZIbP(s%u|MWoo-Mmt_GGm{Z5-lI%0ZV&3_2Tevncfl z={VwjV*?o)8E=5}DcsTeA^0BSzRQQL2je-m3a_;X*vhXxHX9*)>ozl-Yi81PwKI;j zOLFOC|Bg5EXw}nETp`L&w-)X(?_jO8N%tjj&t(X)K{=dmD+*e|qU1XTBGd3=q^dFA zPqLzQJH!S^@P2xX>Dbq`FE*e%HrQFd=py6V? zwa>8Uvb)&Jys%F|Pk>eoZ_m&sKs}r&XZu#mE=0d~fR?z{3zGn;lFn)NEtW%h>I%>r z4pORHXwiK_vF?r_DvIV(X_wf8(I+L&p?r|)3fOKEr^BeX>(C^Nsg?(de%k58PlXO0 z9pJjZpgB+)Uuvh^E{y0{YgkKJYgp+X1kOg{h!j^Q&Zw)x|GFCdbJZHHel*?>fsVpO zZwAH&iD-gWu@{Gar9f;TgR{28206D_g}8aB^+PR)A{Tt!ikHr=jhd8k5$e#-v|{Oz0YuezF>)bvL2Xq3Y<06Z4r0r|W#o z7xTI63%(adYTbkCdV-NwZo|M>xC`$?YM2Ac<)npgrZZ*)L`EVa;dLd6PZGb|Zj z>wn;Db?#CVq<~UAjFgmPOmFXp&jxp%oY9-joL_dmhRGS0Q>c(>h@dn!qGNyO4*d3Y zsDkWDpyTJXNj1T)Pf!c0;=Jgo3W|KSET6 z|8e&%;8B&=+6mb}3Ih}BAi*=pv>iP`1`Rlo3>h>)fS^$WgiAeXMF92w$xHfRf_GQN-L$_n?y~v37T17?q-*!1N@bT^RZ#M#pkQ>_`xnjS1=**Sf|x!n3Jm{uNG1sBJ7 zD%UmGr~X7vB(fI?i_tvVVJ)2O&D!YPllb=zN;)b{^uj{;r9*EgM#-=!&y1w*mO0OO zJ+dAtV%vXUH-3)YgQdJoO2Q3zma? zlX?88@D=@6*#^ zFdU}Eq`^x(hv;MXU{Psi)O;u~qoy7ZmY>vNaJrc6if@cN4{(A%b`O@XUIu=``FVT+ z44k3}e{p>SlE5)m%@yCaEQ0OvK4Vu30Z-Gd)N^1-0q0kl02C9b=t#Z&PWl06IM4iM zUIl_o2yY)PylnZR%NNxys{2;xvc(HR3+gWuH3&bU0+847_T!bIB}?lT!9CoJ@WPk| znFLqX*Iu?Pbos)X%dTFuq<$GfTfS()!cb1wy;d0NT;!HdMjm630&~mI% zmS3@G;qp+I+faeVn2xT8Z(QT}OzFRDd0oid625XF5|I(yXYij@xopYhwF|GxD5ZrO z@BAfKErrd(#kfewl0#`p3PTeB=izTR{-9n}dh-wY-9s~c|8luB|Ihgc&cIoy_i>Kr z*OPx>3(k#pTmL=(Ky5}?3 z+ET?aKz_F);b$79{L&5oE>^^`qkMmWJ9cpIWBT}4jX%2g+#QBH{7-*OAO9BNkM6wh zXr?0=dUiee2k6cVn|?iaJRaKRq$*k)m zKf;ex%f3&3fuD9+OFiAyL!UkeoaMcp<5nR4_ z!Qy3Cft3JH@li20Ygx^5DvGm~EL;Ybg8F3(YcXk&u>kV87hswodjXV7ML8yG0XY^t z0XbweD9Ty_gLAUZte6CmIs667RmCJRIGa2M<`7$Mj+Ma_isvd`sMwsr~^X8pUMt0KmM87=(W!=p&ObI8YW#Gm!Dvir=I87s1P* z`n`%TD;`#SL-Ae391PA3XQ1L(#qo-jinA3jRlG{EL9tnJ9TEAxN%b2QwF#eo>q={}%HG27IK6i-(yQY=+mq_{-!Iz>;0&UaM*GsRyj zQkXcy-%UimJQ+Jrsdg+sO}zh-Z@`H#%(pTi2ws^t6(=Z-TxjlSS3jd3JUIJ_2=4{8|BK>B zirE;r7!PT5wF?)2N=D8=n*M4Y&;8!)54Pp@o2a zlFO5eTF{POOqTV|0ujsdNIPIBj$;f$m?Ug~XF0c%BoG`H9C`=jBXb9hPcY9Csq}FE zF_Qojl8{Vfh=>t7B5!riUvd~B(q2yCtY@Snf`oq@lHO%9P=*i?S-}Y+!-hXCj&(I% z^FU)L57+f>gPVS{6Sp~9KjB7@w-`7TxIPZw8R6{4y5ucGma zs_u-8V(Ka%ylKkv;eE(}+=pZ_Vq>4kEFWxu`S59)R~`DfJ@hsL$+XYuq5lBq)M+{GpE&bWRd9a58D~ktv zU+H${;TWkgloAkMx961{*E%ns4n$U%hU`Pqx~Q!npz-kx&|44VFzuzpfC5B_j`z1iI26-}abQPN6p}BQ1K&6S z1Hnp&Z*MbsxO*4^(vatSz0D=Wx_-${UfHCTAwL;k=)OB>B)8WYE<==e93(`pa7q0& zaij8le4;?S1Z7XwTouY|la!kftybcb+bSn;tY#(;gZqcH##dI6;VZeSMSJ;7>rD zxM3MWvO5n<)C-@uY)Av%q+_T(+s3xk5%i9X@yqm&3N)yBDB=aETD}um27LqiMkyLj zEuvhCR?ashdFQVH*LM!sa&^xhz-<`%PYwE0G0H27Bgu6%GB2;8M@8VX6zA!yV2VLE z4o5+d{F4wsq>`EBX#LFLDD=J^o4BY<1Qk~58MPGnezz?WGw<|G&sNY;R!QjvKX!k6Bf7= z5?0Q{LkG-VM8o`1G7H!BixE=eFXb3;vQJY}xbljm5DLxdDP-wLe9W3KlptIP0pMA1 zI1$MfUWTAV*uj_F3Sb>VY>cw+r=kS&heUa&i86oWlk8>K^<=SKNLj74{s}1!e{3rsLNcvENeu;u&9-5q4{wvz!8YA za~~c7het%73(`WLE;2o@td^05p>&1eU%+bPS;ODc`Jx*z$s@B$&^%{nAFLk+FOj5` z=TWqq7h$__vA_9pfc+i>y<_lEpK=->Nd$-Jpmi3Uz&9!=Pgw%$20HRS4TTjZHuV|_M*3&VL zI9+Yj-}WNHlDlZgZo0nLWz=`lqTr}7Vh({LD0|fUI3v(0M>i5(|%7M+6g9t zk7%WH7j15*u~6j|9Mrmmi`Qpc99V0ugVzFOkg!m@^2%k%R5!&}ptgevFn=6ib>m99 zb3Nypo>l$|tNI^R`985sSGiEsC@zVkdAhoP(9Kb1b-Ulrw#Unecr6xIr?JA#cwGis zm!!rS%yH5gz;O~|bi8FZ87wZe8ZFF+7%eJf5jF=CA;~k}vgSBLXV#2>WBVO57w+X; zXlU+u$!isCnjWm}nhrZtwPp@16@ZwhWH4-^tC#?6~QAa5f7Dvp>!-73Wm@ztfND0gaxP{wyd&5_-qic-u_f? zw6^5dS9#T9N_ukrc`z{7B2kK1adoeVx}aI5NasY*N#V3hPF<1Oi0L%hiHqT#M;=Oc zY8e#OA*)gbvyI&TnKhTP3_dW|v}>+}#-rB-=IEkQ%fP(%lKLt#E$d@x$7*;MBI_Z` z3(1y$auXEGl2<_u2iz;*C5J6QlOuEIbW>>w5GQR>$0))q!iMS{+?OAr=yR zV@3N$w<@7f2va3fR-^`i2SBT2Pr6EEB_U-nXA_qfm8M4iXo?`?vOZcR`a`8xJ>6?% z4r^t{>CA7sJXrpzm5fu?0jXvv(QPJ|VHZCxgyYH3a&Y}%KYwLi&2o7JVKmN87luv~ z$eLMIdD-IH?hb3};BeHabF<1o*$ah?r$Tr-ZTV@Vi6C>4xYyI7XdzSfufFfv^M339 z$~y{EcxbC%o+8h0ag>$*@98~GKr%|_f$mO*JE?hU@OfbnF@mSRY6O-C100|Imv>NWaeA>e?W-%i3?or`@lf-x zpdm3|tONd$=7OU$p-r$1sb_mY(xgnO9}abXhW<-Xhw$h5|97YhGW4wf!VEo>>5nv< z(a;s+&+|`pDgM0pS>I(DdaBDa^wXebU3>01;Z@?#3vVve{PXmeLOm}-e+ASv8TzZB zUYenYGX3RbVJ)5Uy@P1hQV@u~k-la+pM;*`tji$} z0CXX|j8Mq?rla8!1RmJcF1m_zpe4AO3c3%&1Sv=Y1$7BmRPkH_2|YX>T};)og$tmj z>K|V%DkN%%h><2aV`HCUMQpJlE|3mRI#TAO{ z74KBssrZ26yF@I4K2q$9dgndTbAgPvLUER2z2bKjw=3SK$or)9cTn+3#pe`9pgGh2 zEX5YZHpPvKKUKU_@hQatX#Vtnl44Nt48>(#tXaf{+^#RG~Rir#w( z@2Nh3PR;noDPE{JNAV^_k0w-zK1BB(4d`;!*C?)5yhG8W?fgdd_bWc8_*cb%(sDwI z<%)9@zoNKF@vx#t0~)Th74QD#EY<&qVx8hDVm`(Q)!(Z4Q$>%)^Qh{*dzlnS!FJVG zku)5(tBKU3Jf^DoMXHzknQ;FfYJMe+7T=6wdR|ri+ln6p8Baf~T^P?vK)M^MI9_oG`XB92M|lzP zcJ{I*-%OL9y5y|m7TD;*!cV==Fs7)mn6EQ@mUpge5Es9w{1#vxf=-d~d1sN;IE#cF zDJ;Wluy3uyUTy{U*bO+0D94FnDHw@Y!56q7*AUFJraZLd`J%GH;zY0*CxcNrv&0k2 zI9gms$GgJo3Fc8$1hmlvbQ=ICR}q9FCy_j*M_{!RKE;7 z&&}c6p^?WTe2xEsz2_V0y^%A3Zry;3U!F4sn&NrK?MEAiW3N=~#!qsFL&zAS;pK^_ z8_fe)C4K5@+^;}hCA}i|WhJ@R-2ho>ahw89Vi24LhZrcy7K3fGaR-V0k)S)miW*)}ACK927?^DMMOrdS-%?d8#+ks<2zffgafoxlb_ z=|}(xGnNA=9cekSy@vm|@q8qLf}V37K2RNDs!qlut{MrC+=e*uC!OjiAsQVmILjW- zCWR+88!{ir9FK;N7oiLA+x6~f7=`2m6SFnQoFL&%W`7GAj!AJufrj*)5Sf1p_3a38 zq3Z}ToU~3>k9eLN4tjd}CCqr`2wqWUyoZ8AO>AGin{U^MJ(Ej2@AwO37_xtK?v+P5 zg(V!`3zF7hBq={(y(u^BO#Se@j)W-d5O0;G&y2`slI#Q*^xlqlaIS`%T>73RhCwh> zKf@H&g+p%OG4CzYmK`t?>0_X=(2mriT}$+ckO!;a^>1zbN12@Jb| z$4#KU>n0#TbRB1+<@9r-=8OQPS_IFlY~NVBY^){r{pbQ-)$F)*wbW4&AA00|FzlMN zCWUibQ(XV&Bbz{uIRsmw#HFV42&zL9BOSf4ot~z8EJ5Y#$OwxDo^~YEbf4+2 zm=5%#9X}3eOrN*UW_V#*=tn#Lou{bAS;xGH)PQdIU5r0ZO+DRxmi_ZU^wPQXpr028 z5#vGn3t_ES3PUWJL|zh;!7^kyd3l9W-ooPk!u!1NJiUjR=Ksw8nd#ui^5lM)d8OLZ zp8_@0=jlUGa}4nGr$bFMPtW1C0DsIE!{dIKW}f~Ws99&8z65GcQ=T5m^!HhKASbA6 zwn^?MFG%|~>7Lm`?=4O~{Z3y!Uep_d7y8DKH4(4zVdFU(J5T7(fhk!Mv8KVsbLpjK zeM5mqix$^iL6fVNE?Zoe#f1RZM?X`1N^t-xiRQxOF$qO5Mir7Q&A3?F4hD40$MIU~9wVwm^Kvgk9k{VMn->urXXlXb$tiDa63G zi5NBj;Juu7Yr|EvTW^-L>%*1Q?UFSv;#*6XyTa9Uxg$K6cII&z1lCNujp2E;+ZtX# zyXNpJ7-F5QK zEpM6GER_fPa^fq(vNqWWxU~aef6MnVHe+pvM`7~7`Jz6sibNS9)-Edbg~nYm++np- z)o$I4<;!s=hFhbMis1~spyhdc=*k=AYSTQ9I|$ZXplt8W;hQE-Vat2|)}c_!dpa~` z83U5mBZiYl)}>hNK8Bqkmf?l%7RNGO_aa&5Ze$|(nC=K}qC1p-yT$tU*2nQjp{98x z87M|_3tY==f6NW?WMG^V?}MA~r(jtZJ#O`p^}Q_Yk6S-9YiuNJ4Z>)O-+0`=HpEyq zYDy2HY;J>9BK){lp8l3kF#2NQH}7~~^!p3HGgz|9JE(zQ84c*_C1Lz{(M z#`%O(`3w|Ec_p+}yvFx?*zp?QAAq!}QHpc+Ta`K3y3xs>VU_&?9lb`?b9`eefYf<} zsZx_$yoeNmJ&K4!TuFa_>sQe@+IyEm#}6^ib50(;8w?H2UmJk^587wx=2F_2!LcN} z@!yPdW*uqCck<51b~ex79EE#lu(+*5H;>3 z{0kxlVK9ludA4*jZzLypv0*wv21yX!tk}3EneqK z-Y@_zpp6>RK@>7~?;=734PYAD9HLo;pq=8Wb}<@5gQ2I}z6a2tD(^%0KR)sll56mG%=j51SiKW6;<_0Ct*&UTR8>QeV@=R4!2V0f4oza{l5+R6K!u$+fd z3D~*qlO&U>J&&C`-FH;m$z-aCAEZ8w3sy zWN;PK&Z(0i5TOB$Rm9!ffm>N6_pq@d6U}fjGS|F}W&3`c(OA7F2(dze8%`VS=F<`C zgFwnQpgCI`&F3IyRtTR@6ZePqz$I-A3ycwG(A8RJ*Hpp|fwB>^#j`2SL7+ZhB?4pJ zm}lwCGXrk?$8bp>tWLzjun`3#>)vRe+Z-`2wgD1-*l15(Ia}V75Q~kns&;(e)A5UJ zsa9Ld?iJ^;7d!pXzLq*NJK4Vb%tdUx4k2F6gko0bCDEH3e{oz^cd1P^%#0U_x^?gmL6ukZ_XwU zrO+OCBBDeTjPtorko}zV7v*W8^9B>bJ&MKdCS(=KjJweeT|rPNL^yFeIx2XLJV8+{ z@>B#6iQ)&ngT0EcBT8@TIntR`3a2qM+Q}&92*tn-d}3F8Vmad(7)G?%M`mq+ zw<56cQ=TPfKp|r>LQtIGjVO`1TxQjsJR$ahEtZYXo54O+htfuRZl?#cgT={kB5W!Z zHCq3BWNDt$Z-U(q*I0>1?_H?CON0mm_x!Qa9^*6TNm z2T8U`UTP@@IADGi%wjCAvu6g>kft4uhSa zwoqx8>1JA(`ZC?`;8Z!?hMqV<8>&JZ`n+z0?q?eUubXRq*sTo}m^Os% zV{W<~?UeInM7RT~bc6RZu}+k04{1q7QvKF&ZAt5;CC$*5geNrkTx=QNwUp*m$yt;w ziL+=|OTv~aI{m+EO)PFRZ^~Q=KPz;kFl|awVmEYVG^J1MK&M$MbiT2&7*Bns-;*%y z$%sjEf~}}(w5JWn>%tlB3Gbnrk;n{oGFzv94pu_sprv= zQfyl<*yu*7O>cJPzX2pk*!I;L!_kX*pwx4FA>1Og5KdK>hn6k8YRT0gi9l#aZT(e? zmq)V7W=$%ed_fm+YV_n&{9k{EX)b3la_KjZWSHH zWD7P3nN}?z$Ewp*&jkg)!x>>wPj~!!(o*QI+E~di!{W!jz(ziX8hmtjmQl(t?fB=> zQb;S{mmlbgi+aY(zY6@(z2`0jcV1jD7B%%>#vk2zUjf`@exBVT7)L7PD` zBz-JH(ptc((Tz3}hCYNp=7aWZ2Q>5aW1ud`(8r*TWa!63%{Jk=p8$0XjXm_Y_t5{MhyFJ`^d5m^ewK(p0`ntjP@{zg1tA%r zL7kliV)0cVMb$04>xWp zgV4XP`eTZp2mOWa{tZakAzx7aNX5~LV-?F3FIK!RM{1sfg$2VMq*$UjS@C?ud5VjG%(JEQ~X#lfO(JZ9L0ki ze?qt?0v>xj1-0ZXL#*%O?;Lw#y8hVWv7cG~e{RrBl3ZtSb#fy9PNnOw53kBbZj4QoNEF?Gbj-BEw&cRB z#^k;9qZuj5%LvEc_AqVMhJO#Z7ZpNR6>yCJDgo_Q9`|?cWrJZwGU7?c8!pDvzUGdQ zzlCkMD@tYqd;L;)v?D_hk|u?RuOQ>AHK_;=0vu@#DYb!`e$Tm~)imw3O8WX+7b9|% z7JY(@Y?9e{GGdglwhCWDdzs6$W&IA}^4N$OFB#V@lT86hCTqaUB-su$8%^<(LCXCi zhWC~O{+45yCcP|~gKQ+AoH+nXtU2;>pfU^{Bjb4}DQ0u3GB3cDm!VxI|Atb)3>7zC zk2+@!x4ehUFgcBbkz$%4$}&mPcc3BJM4UbEZ*E5yK^#&9ult*KQoBns>GZ|n4T-ju zF~d}WJnx`yd{AyPs;p~k5H{P=o|k$dGLpdDXg~X4(>@EiD25gZro9u-Z+e&#rS zjtnt~`a$r{gTAKt&|d!LpI|401Axky@pEl|1j$HSo~rvLPMRxY#yf;(pvmA!pl%v+ zx*iUuX^pTw`PW`gTd-Wjz-s|Z*lhstYuDE9Z!19mLwV6231w?{0^30QHRoae*0*Q^ z4(yiK>-(cf=nlLNbx3XCa@%;fF^-XTtms?UBu5EmYK9S$r*rOc> z{B86G-wYhFF{^GL_g4Ou~clvSvUC*JOS;{k? zx5ALdfLipqRoTZ8E16J|Su>vVg-x)qMO1YcA{@5umZw$>C^6jVes?|_;9AKNLcntw zVBVo4RPzpkX?V%%@QNP%tCP{Vn%bqb{H=bp0*Kl%%p?Q-ZEs*=KzXsC5V(v$(RVBqp?Jl>bc;PN_Cj@YR6|4(L$Hq% z>5Fk?tAy!qnL?kYML2;zNC(@T9r+I2Gt!phb%PL(#J|??GzGtdp2}?R`BN{nFNWG) zHcSYo2#Ckz&YAEc9o$f#M@D{t^sshDM%E$boy@V}K0$;stgSb#Q(I4@4GpDgWc0=n zZ$OC1iUhaLStW+|i=|9HA{5huF!ir}4eKHV!x7kt423FARd8s_>sm|xWSjZ@fM+&G2P!hm42+uP==dOB&!<@|5E=6 zTJ#9E20_^UZL7^W8wJHkP%gC$cdf9T*^%v9lx`pedEvVZ16yap_X5YQ9i~hSAvvqLj2HV`g4Ni%3hCL1$km1+&2;@KD>6JnD z$#~%d!okd|No*I&S2@y+a*W#%{FNDGrbfmK(cGo+nfkEf zXLGUvT~}tTN72YxBmTD67?7zcH-0DP!*Vz%M=%m5Hd>r#r1E)mBcp=sWXlJ9uTp$M&hrU6MEBaZ0j+*>M;vr-~@yyzD!582fBfL_-EX1 zbg0uShodS(bIMj9fxi>9PNwwIz#P+u&suVCUB@(F|)kI zV+@f~{jFa_L-WdRt10{jmyL_XXhscb5#S4R0s}{Y%X7p>Bf#}xCow-%jd%v5*?zc0 z*>UtlJko`%fo^rL;~<8cyHglAZ=)kKi~HMn1chQ?TlLc2ELloge?-(`9$$khh#=8m z1p^Z&oF*+i;FRzk;b_&l2lD@S<2@=cgCQjC>yKo<2hTtw2mw{Thpb<}lgeV=taHS{VJJhiUElV<>`C*oYM z#B$Tje7u2+v~U8&(p36F^WUIBMy2IPxcXv|@^9APY%~b4ZAXVk73i8fOiIq-g}|f?}CNk z?X5*xN||`GZ^0sSsZ(8qypCmF`IIf%+V7d>^}2_;SxW^C7X8ZPhQ(vnunfx%{uZ8S zWk(Czt+SwD1CVjfFnF(=BUxQTHTLkp*?psqhhAQINK8~6o-ajlbu zFSjKJDKoF?OkzGwu#Ha$(@E_}ES*8{lbvwQ9soA zTkj@o(N>dKGfDf~Msm?_1}Mk6!`{2+WxgH4WVKs3oRHkHQ|n3uNj9DKBqV~%lpLhm zH>A9U;wWpeCY6|4dLa?^zW1Mob1uqqPK8422vsx>MkT>Qzw3tr;T&Z&Lt=)Z#J~0; z_IMK#*5%Ew(sr#x>jp+U<1of9wU5FZla*tQEl1eRaKi*R)~|A`-@=ZAqTLsBw#ciC zy>ItI59eZhB{{Hyuyw%|WZdD}=2$hv1dGv-?pTS(`=b0&dek>Qg)}e=@KnxZlh?@K z!UvxbKXZ-AX&3Ex7<>>Wi zNGJ1cafWgsZRR`&Q-`VDoWQA*&qqEPJzJoL78u_6ID)zz(ZlFAfWPfW_=QUo++FCc zPjD*NF<_&NkJvv-xp@oyzD_(R``Mg8pYvnSN?lf=;e51ml9#H$PVFCKGzkuB)HEL$ zu50QP?=qAuaFKPeRJ7sI#tzc?Eq~L!1&+E7u{YYN`6b1F)ol zY-!S4Y_4$Zz_!VVS4zmNc}$7l2G`(&!7n;4`HfOngUpU#*q@xfBe>^ePaJ9WLu5PQ zEJaDDFqNKfE{)iMrc^CV$&0yO?pn-1DHp&SdGna=nXdARz&-*!xsiy|-HpO+!slt| zevaE5K8#4nW5O!%-rg#N`*qMeL%}I(Pr(3_!QX4961sp-1jO{_1lv+?lF!4O-i^dC zt?Pl*6!KhT@Fkg<4-;1~x2fHtz`ZxnD}(k~D04Q=3ky+we(`4Q3f%s}z81~a$GwIEC&EeiG6d5ai zCW_*y7KQl3u5hVJ?J7l_QXIGhjNN15iZcR?LxtH3rywKkC#P|r zl=>z3|G?R_3mw+*y~{+5#M4(!a7$_l93ZXIuy&ZX7Puw#IlgCujVc^2V4!9&N}Ufj z6F8Q3rRQmp@1>q1KZqG&^gt~2a~h=`mNE!9oKI6-1{t2*)HoQiRM8dTh?f#rsw^#O zFzHe?4`9)r=6J7Yy%gsR*K7->r;Brk6z6QTKo%$OHb`-%ixSfo0&sFM39R`U+qPLg z!rg;#kKw)wB{(UvKU zH^|RDg0p=yHL&PXUwak7Y?LC&$h?y?*_+(;aJTJCaCnaeFQ@^IK~B_<$!8u~UjwVaw567t}zPXvZZ5F>I}Do16!VF$7LcnN~Z0xb=+N6!ikMTjAl`_Te)z- zC5JTZK3acf6-3Ye)@wMc>UsECoK90Wq+qhuEDIZkaZ|va;#BRl-uXuCp(G>@ubhTHA4cTTADYeXA#m9B z-9=Fp86poD7*@;X#5|VmZ@wM|TpO7EPZ-OX^>Sv7WG_Wp@j#m=y_$7yPJO52)Hm~; z+kU7!?2kjZX}8tnuSx-AGHBC!?sTlOh=Z+wyJVQ)zZG!gj zWfrDc9wJ-KaEP$EIJ6OV>h-@$!Ls#x1ff*kD_uU)M~HFyuJXrywm0#wA?Zx}3_lVVaf@tZbhHy>QGQQ-tY`HKzsWK~T3;B1jjs6{o zC%sT0k0J10h;S&VX~f?UVSdGB^Xrxu%}*1+M~^Nnj>U>&V<5%oMDmSdh37;+>%I2< znxUv<;>7pbn_esPI{pqhHE+iW3`T+4@Cx$#Z-JWk$!-Vo5nB$y9Oj#Wys|^&1u-Js zuL91*Al1O93L0j#%ZukQ1224tfKOLfl%iAVl!riD7D+0se2{YROU~?kAok8RPW%?Q zTj2(eV&i#%We>1mVGC_K$(`sLXw6Ngc55fpBI{qo_1{SP(g22IKep$uT!gHG8d+eL zlEE=p@dn$ivCv>_qug>?&IiGQ9K^1nvgDeOc@n{pq~++N(izk8US?`}=wi}pck=WR zY|>5f+~b&fgAJD>q)KNFZ>&|t0s|X)t#+Op%yrJTiz?u+WVz*c_oM51>%v;~)K8H2> zWoaiYHPcT1gg&B0MZ&Z1J~jvY6SUJ+a3fEHjLX?&x>w&sRx%%Svf6MZ?!>wj@|v7X zVN-^dZVzDdGqSh*&G!!Q)L45kEVttCd(5M;|2#Ma@~uw9eM5>6w=t+ppn|c!c4d6J9Xi_mAoieXCJa3am$iN$?t#?FllJo zotvn5CTqf!MApO-JYtc|Z+uxj7BLXysQCP;M!>%<4J>sWrv~m6Q$zI`lHv&AHPk|{`d4Y}11 z;l+|fjx&l;Y4SIfL!JCLH~90BIXR#yZKmGom%Sf}D3q4hwO^CQCDr8wT5Y-c=tv#L0=Lwi z?oP_?!Edoy>UST#xuw~$s_T!{zlLb*Ouh0-p>KbpPx4eZI%32pBZn0-Rb~lqSKoci zk<^$AtJ#l_952?U{-hX;panO0rm2__ADOJ68?kb8d^b`%x=Fn6HAl>pPn8{V_2jITQzXL9wXA*f%@m&5^^fXWvu|mpEk| zVWbVzKu3yH_3C{TXJ~E`UxFdHIxUzUu~$0vq0FU0<5`r+Tkd z%Aq{=mhF4fu71Hko&Q_Bs8t*^(s3NPm8L%X?C*U7ZSpOT^#$0&{LE3@_xk zfrDs-cy%a*z}(1D2r` z!+R)o3$$JL>P%0c!;_6#jQyiZ;YaS|*O(G?BR4tMa`396EEse77kZLKB0KF-0XoNm zZmU_@Gz`F{UIsC-M4E<~_WW&Up>I3GSkLbAm z9A>6wVeoJlXSclG)d?D-&VUdcA*eNgcB_^1fg_FB``nJXgeDTfjiN1>YPz@j&spujFgQM`09Ga1zF~USFT)Xb1T)p>cx^aRxlvhkh2A{^P zJP~Wg!)7Kn(p;rsKTgpeEQlEp)rzj!Fw00E)mBat3QL?PZDgpFLTOHw z4H?JGdTwxwGq@a9BYAYnird7@+hXQMS33jI*%+D|=;s91p`v0U#>0No-~1wbu<0yw zyyRkgR_mBctnTw4TUacx0qjkavNPy4C!GH% zJYl~ZI5grY#-$ZW>*0TH_Df(##L~!^|B|Qt4`#qZ&VWXogW0u?#2U~{Y+~w8&30#H zm!J%A5^oy`8?wUO%z@B;!@qU~7v{rCp+kMJ#a>{V9$J7c#&+a~sFE#oU<9hf*EvPG z(7KUNW_b<7^hDjjvmvkwNrX62Pb4Wpw$NSjn=tVPQ}=@ol+)iNAB?7K(P0GB)t)g) z`5I^H1XYCz%Hm5nE6fCyw4Rpv$ZU08YurT4MpzH|bSfGgLRgJ^_0>*kMqEXtI%Upd zBhsZI)(5EvWxiq=rRHN^no|Q6$L1a4EP`J*a&(s5KA(>6uoDI|YiNuDVNuOM5o`m_ zt_ji5JJ~QL3<`Pn2h*{sVe_AZ0gXOblYAJ|hvrU=}@19dshH%`-J6@VipRBB*G5j(GObOFG%cR*% zWt_LzDsUi>{r2=6??}eWr8(o}kxfRLi$d_gG{1*5_E`88DPw1em6SE83-|Ad9>rvMs=PQ>s&D{)>(JbCN4_x+8iseetYV7jH&`-a9QV3 zr>gT9)GKW31H0-23!-Xdt*4YD{vV-t*@R+nqGjUoU*;OA| zFu-`@=&D`MVpu@9|9Nhh zJ%7s4UYZ2E!(ee+WYLE<40JF^|FP?aTdA*uEJO$Y;Hz3vhv#^{iPtL@ExvMjXvr14 z03TYgaOuLu3l=V(zi6R(-sha!1t$vNUwJ++GJ?;>qv$tZXV;S+Faz%D z=|Dfv9TAJ8^!EhLv`b;&g9(oOvslhdFN(e!Js=PD!P5@!^TPA=9%h>VGxPw~H$RpU z@2vY(&2&-E_k5UEPtPD4E++Q%QN&>L(Mq6_^ySz zGD8n#`ui;Ol(b_gPIL#pSsi$H;MZk%V7Uvgk(yEelOFbW_R!zmL!Tz5%+FeWW?@#{ zf<>2Ii7W8SaW@^(n3u{`^|J-X;2QiQ+=2fZF2H{s7vO7g0e%VZzw`S0H+cJf$r9dt z|0eFXpPjX60q?!T|B?j@dG%c`!k?3M*>c>69}8H_i|>~$$6fiD+-N6FXUVcFgdkM& zb?8ob1AaczwLq@GFI#-&{F<-ht~K(LjmlW8_)Q@1Xxpm)k>U=;dle5T{z>r*=r{~# ztfKd3(6?0IrucJ3K08kLWFjFxr}&EEF-7iYX@9C>k>U)+xr)man-p!u^@=+b?^E2P z_?Y4|ib=&c6hDW7n(^l=p00SV;$+2HidQIJrMO)2dd0Ph-&MR>ag*W!#U~Xz6<<;O zSkaF(Fs}(8PlkR?^_&XwKj}NK-dDZ_-^Ukw<+GC z=*c|Tse142qz_d;2$Ley^CiWz6wgy!q-we z@l?gI;u(sCiZR6riZc{1QJkl^RPk!XX2tc2H!J>J@h-)?75ifXm2@eFh?ql9Bc6h{ z!GOI&82gD|z|Mr`$j4U{TNHn!=*d+0h3fYxKBf4)Vqegy8O{L3QxwN2PEfp1ah~E* z#b!lM_QH2m|6|3UDgH+BF~ye^Ur~Hl(R&LjjESA;8Uf^;_%W*Q_7)WEJ(&%^qCNWA zz3Oha;_nroQcNhmtoWv4FDw!m-zkcPiWe)|imi&@12SDdRQ;`r+Z7*Bd`huH@z08{ zDS9#_!C7E;%L8HF{#)K zI~tmMG9LIw6!YiFc=)2)7b>2kI9>5lMGEd^I7=0;R$Qfcz2Z+4?^3*5@kvF#CB<+C zVnIg?CySf={x6#3pJa zghdU*$yXc(WI3L$`iSBL#R|n4ik^&#<*N5&Ol(m7ZHhlryj$@;#YYsMQG8kPh~fu| z1F->MI?I8*zLb@9k?I#JHY(bRH!0q#_zT6oipPKqhwo7^+@Xpi6pIxvRIE{~Q~a*t zEsCDZiF;N5qT(xxuPgQk?+xP{t{71~PjRy1R~45iHY?t!C_FlgFF)I3()sUrbjJP< z-kvB$+u+A_K6eKvdV8YjwSdr6KsRc~)QF=3JXA1;haEgvubYX}>Dn1|K~hE)Evjfy zNfDnF6!BRBMx1hb#Fr*!fI8+Z6{=Mj##rKIfoer1ycRowCP2B<@U-Rlo^~s<86G$j zeEaN%eUNcUci=dvLKw?@qG4Nc}baOEk2@WC9 z@{$!ka4Oj1t#KO;PpV;lQp;0a@%USR%z%X)>iVeWVNrzaHw|-93?ajM6DI3TIcy5a zdIR1L?w3ktg7xMJ45~1XI?;71Z8B{Gq z$nH4Tw6S*Udt%^u385a#OD+v1105^B=xWTE#Nvm!flZ)htsM}jNOB1dOhsj7C=%Y+i zY^Cgb(d?2rd4>VN-^!;-VLI0d_QePy3_Iq@isUFCjp6*f8pTIucsEuE~Uzo`P)^dzN95(SDdK*PrnGALs zj=15b-r!4aJ8MwO?)rJceOJ;ugv509la=G(J1UPq${2v_Ka{{QKzai@u^nwQ zBa<^&JIGuLCdJ%vom;p!H{~U&zf0Nh;T)Ee|ky6AyUTHy1lb?vtPih(ry25o#&qOp4Rq6usB};{$>?D0D`1Yw zz>iFcmGJAhz4i{*Ail>&Mv#a%W0%6u@0x|9>5aUo@?R`+5TJWi_AT=N$o&;bK zV|H+Zk8yn$b>eH>x{txd!eyR`%qwC9UYA;r?wjnGQ-iR`xv+{bo^8X8el%NhP?JkD zLzS+qk!CAP%Wk>A9M9-#ND@6-nZrzJ4)B(dc>~B9&-^k6$Atu1p(Iclg|*X5Mfn(? zG)oNeNHjMDKReLMA=`2%XY&^k@9+mHOGE;eKe-H$1BKc>(FhD+9_#7eMx;0MbgmVas| zuYa=R_&UI>gh3Vs7fM{RLS8ni@85a&(A&wOsqI4RHuatQDKx@3*^{FVwVwH4&n4K> zL}qCY^P~j7(S2`nrZXyxL@<7o7*@`AIf27Y#bIkQjLKtww*wt^MTcdJtH!6A9`wlz zKW4p}nuR;6@P;{g2m}R~0jv2C4aY|jsz;GV0|%=w>blAvP)zgaNkWSbnwVu>4O;a1 zphXXM2Ga#@m`P>T|0ZQKIB?QBRY_8!(H=cYw5%d0n5fWqv6ki7XUAKQ+)G0Aqww-5 z3DNLkNYL!p!z9wc27MlkLpxr)lBC0FjU9LNfhCFG4m@z=2Q+WDupvNY@%kjL%Too80fipvnKuV;-i^g13ytF~8#;%T(^QY8g?v1t%KYe#-$?w?eWts4 z(0d>4__4wn{upZTF+8?s?+Zae`%*>E-R*FPpZ8{Bzu(SU0HaZO0~ z-nX+yK0LcxIPlbTOLtz_)O&U~%1P&y=8wQ#Jr(+S?ueMe(;v^0=D~o23P@T~&a4w0 zHFpbd3(j1ggPHev;dy!wGtK`Q+A!0>k7Yz!FrrPD8TB|;7h14qPug%k{#fs{9|2?s z_4K2m4rSLt`K2 z-MpWqLpSxZwTJ#UJ@mVK=nwSJKiNZXXvaL{o1X=`@l~K713(W3DH*imWedM4)L%nB z9(|^egCX&}2xR1QKt>)5GICE6vJi&H7&7tMSzl)uq$8824Dsogi}ZA*DUUTYWk_Kc zaq1vMQ=Iyk!VGe91~r+I)q4_;J+n`fnjwf2dF46i3}CNN7oB;DnzO8FzeSN{NqxIw zhhj?c6U97qLb?kn@+vp=S15j4ah0O2c)KF6gU~%MlM(-@=+T`&R{bE18?+yyc&1{{ zH;SjL`PURzC^jlOir-bdS@GwJ_bdKh@j1m06#HY4XF3Kco}%~##eY{Er+A*CN0+Wp z{Y=FbimMdYDBh&_3&r0l{z36&#lI=$B27%!V8yA5vlK5?T%=g9c!S~(6@RXHzv5Gh zd7|E%JRik^)9 z_f+5CqaQ1V6i-)-C`J`M`t~KNzfy6DV!h(G6+QZPtLi=a_9oTuQ#`2njNXg*HyLd9yuYZO;2-k^B1;(sdcP`p=>S9}@nqlyW|R}_ybexf)6 zlc1zSagyRR#VW=5ieFQ_R?$}cuHsJ>pI3ZQ@h?EquisJq;Ln+Ozo0l$v0PE!5Wpn- zb=5CdT&dWq_+!O874KL43y}1;BdX`UZ02*C;%voB6&EOeL$OKmKY8qHpI85iWe!)Q@l#?8pZD@-ln)k@jk^z6#t<3zG7c2hL|27 zkaW5)sD7m4MT+wj>lMAX3RbKB2E}_7A5?sdhy~TtL@dDn1nd>g%6fx%8YUB^Gy4?B zC{9$It$4YjN58&a^&1s$SA1CUQN?E!KT;%zF7rD`@l?gI;uV+UiIl~~9=R_qlYUI> z#CP&X$HuBq`g#3UPOg}eEo{`t7rtz)ZZVCezfvv~@o{a(6``-&ZUj8tcx#>~JRa=` z`{Dv@8RO=0M26&VeGI={cO=kRuzz*&%gu4bfq-%xV%y`ny2ep9&$}GH7u~XnE^t?o zVk@zOPWYbH+bK9|#Sz(m=sx}{@Se+4eUcF3xb}cr%E|#zsib)21bbhPJBN-QzkDC( z6rUj2XwrIiA|H+5*#Uhf3QA7G_0ea&ClbGv&E0*;w{X3lDfhRIerf4@MKp9wD-OZ!6VJbodSE* z21W(RLbQkIxe*QOErxFN4qU1zaF_HV zjcf&-nP5+~pKt?rxJz=>^{@s;HpUHXfta5L1nMrqaR*ai7pzMFgA;vW_L-SwEJ2*$bv-;6r0z=7u>l^`octH zewh@n7md?v^Ew7=#4MuNBv?sxH6xglYY&!kYc|qLi|z628DPY;Vcb|?WY!r-5Lb#2 zn>d%=;(K90D+F9=%tiE&bu&2U#o<;XZz$3Zu}bA|;XooXgzV9C11L*dr95!YO9OTz zd*QH*4mD0AjE64NTbX!co)KxO1liqpyqoWN>Y~dxycN^1S@PgO5~<0QYrVSXr5xB& zmeS@rWFJm;K%KNGDDO6?c)z=!c_x&128@jK#OJa*aCs(=mZu~Khm1#NHYwx;5KM9* zPZ}apv97z5&gx{19I&bxcfqt|?RWP^goLwk}!IZa%QC(!91DPw^rF2vKPhxT?0 zt(bXPFBR7<(DU;dXs9cwJSrJ?;zERqEJ(#{l%|;&*m=O*uwVfk+UxrAO(cj|3zexb z@~h!6Ik*%7!Ig;;m0-#v#Nf{lA?EUeu9zvDjLA=K)6?;ys#?{$*AP+{Cp&bd84JoQ7+MyrlVVL8kdol+VpT9nv}>7-wQEQ zkOmilUyK4F07PZ8KkeRZd!&A!}CI5+R_%481(?fV5BII6N z(6qO(X%Nly7fQ_XujJ6u1DuP7=cH$97 zn-2GB*`1W9I4N9f^X&+)z4sRU!Wz@V2mr3pK^l)y|L%En0n%Z;m zHfQU2%qw=0RMIG^o$)MdCmO~qXkjgoYY*dKVGpR`Z_zydj+ZJJH5cmiROFa>;pq`a znm+2|Z@U^46>oOB;Tmj{j)z;|oOh14u|Njg1`P(oTxgp1^mT)Gb(^A)C$ZVxi!+rs zO>N!)Pi~aAuxaa@+VoJ4{bA;0<;HK2Im~l0w&|hnGuAlFPHNfoP;azVi$!PBKE9a= zG)A1$)-Z|dX* zdHGbz%Q=@^fVl*gEC|iO1svm9^~{olU!I4eV(2ts5=|zJv{3Z#w_wCt#g%T>p-eSy z1Va?WykIoJ*#00~*zL(#Y!_ac^6d;AfZ1{#TpeiIn>TW;G~lLGZ=DOw^UV+mGq{GX zHkyWOnnzPR=YpnvoC`ohlYvP`pkdXFQV6C=k==n|B5V)19j2K>IUnK1P@c!z!S=@( z$pgs6enw;xjriDR&8P_xD+S1ajN!g@rWIh|4nS05?+{&8h4T{qv@N;5{U})oy3k@p ztMrx#UF9glcqzOl24bA1i*joCf=-@^bK;>rxMQEl)`7WQ#%63ma&bF3jM>ST9BM{y zf9o6>sA~~`J)UJ{k6M6V?9_Z1sq-*W2kc?&ZJ!>aS3lT{axlcn!MDN5M7JKIvo_GN z=ygy=vI-#M!XCT=Hg5F$_+^bq*LgW7X`O^Blj>fo3W2PxB< zBa$26kH;&72A%Y7rh@ITXAR9`u0C=`ZDCwwr-PhX@qMNfy+`W+_mp9j=o3!|keK_~K^M1PC`12v6U|6}l5!s5dUKwgw$2};2?ZZkY(sb7=xs^~Vv z-SlvdEedoUK4B$gdBpYo(ksDiH~vfa>%BI!=yWFZyVvBH612y&T;1p|opaJVMVZ*h zS%fR)CNZcuq(O@bJ>opk^ zWHHzH({2NybhA`|>?^waAAcJH#0~nnO%U7Gwf`4mG~T&R|EAr&WIbh00C*WedF4s>7AatXO z0jT19mLTHce4Ok0PSG`m?1e(GGj)NA$7GEyC)$A<9D_2(HkCnOxnUT=WLK-RvNV)( zE+(pxFz`UJ_4JpmsSke9CKwCaY}_6;4{<=!*FFZ-)SlP0FJITm+M^3$k02;bUYb>8 z%t(|$)9bw;b-EUfXjIQeG#Q*Mc$EW3afrRNTT@~dF4SquYdwj9-6t?iPg+zm%|jqY zqQ4^p<^<%LgDBgT%sAaf)0^yJEa%h?tl+)YB!ene>&>ue+S5CuL5U4|ir75dZMAM@ z7%qX0M<<`lb+gpgHnrVbtkb)5!o@n4ysKzwMENxB?QP34T@Q4&8q3L9o7ccm_X8a? zriP3|omvR>#Q6)PfqR3ywnXqhIc0dMM4BJuOWv)@8V3@IjVVPjHgs>4Y>-vq|HIt3 z2S!<(>u<;cDJ*W#MWScHwOuvH5(8``OM(Un5G7)OaM7Zof(;rq!lLL20Zd}~{8+$B zz4V|$(H^U`-e^lLdH~U=)fR14(5#o{m2^|_zBWy?ta_x{p!PnQ(k@m3>Z5g?&B`o!KsTlpav3oi zP5Nl`+&NYxL!X?{P~c@-%&uv_dbXnq3ctei1(QDNDW z`ShSty5NAfQ*t*<*>c?IPtX;xXzaX%?l1>Y&HcixJHUl54#ug#vnm@_v{f!H{v@BsuVn=Tyk!DA%4w>SnLR($U za!s0Mmb(o;51HlCDQlLy6;``)yM-UtnaG=W_TqTNz;*$Zv)l*i)6Q2l*_E)=ZMwRd zfiRP#=fh1`S;bw`)eXkxO;^_&n>Sr$&M>*W%{XRFS4m;IX^qw@w&#Lp=fJ@}Y99x2 z22gNT3rRx(XV>Cf=h%T@@PnSp!NV|ijthez4j!hbLdJmS|7~ZrsvkL#eU_bei)pYB z%fhGG{*J600nw@rqJUUxCXQ^YslZ_5-;fR68V=n?1T3Gj=XUS4R_&dcH)pL5V(HNw zB+L$v4og~MDV5PgKsa%fR5>~Z%&Zb`2APj+vNo6)bH)W+PPf<54EANx>YX0gygk1g zol%La;APW1{P8%1p6)BpL5~}1l?^q2;76^IpIheYypFgxFwA;W{N|?$ns;(q2zkt| zM`bMurUz9p;9;e*KSOH}xJcOf7=5__9gDd^B`_((y%z9y*$OyY0hnbtn{ZtG&f+X~ zkL$gBJXbT~FWXe%AS zI{n3T7?B>doepoqp_C4z(eYTg^q{BVj?TzfjsH=`KRP61fU(n&^spk$n>3lt z0VWM`f1O!Q!`Om}q&t8)z-kls1|r-Wh<1~4uqA*pVY7hVz=kS71hSq%Br8tn^Y0i# zk2zpKjmR`bwPB=}(~qsy8AM_@V@&fbF{IdRw~J=@oEKcvZ!M!)a~l-rb?y%QB-iW? zgb^4Su4a6j&5C6=-qX&{U&ed*@vywdg!((Y#{~Q`-b2D#7G=GCf%p7c9_&hUGnV)} zS&QX5Ut}$;H+13W09_mg#ja$rTg=+z-(xMi5bp1>mhJq`YT#AO&Mu6Ix6!RTYhm#Y zWd&Q|*2!8}JcT*b-xlM~v<8p&WU&mS$F_{#xyf)9yM6v|U>SDdT`XgZWf|lsr?F%6 z6B#_&t0ir(qp{_~a95|6n&>I&_PvkcsqtZ0iBbHZT*Za7qs4 z3w;pCle|~irQ{a7{#}pNlj>+O`cMoW488G67L$UQN3d@5Y~N=QZQp;BroI(ep4j=tmE{;aesLbuepWyT zDoh`R_~MlOnSTfD$KMsZA_!qtma$Hy z)^B%oUH|=?^6@bhw)=itOcUp7-YQdKH#Ru!&p~Ze&{O;aETC?)@3O8kHN!33@?(a4 zFo{wg)3QShB)bY{%%QcCWbXgujxjve9?Ed%JO*ZJNNSs3`&XckoC~16!0GTPSVelU zGmHV6@SXt^$P^(KR2DfMeuGrmn)dB$mgDKHQNsl1KP4gk)=C2X^C~ zkBRwLLZfC@55&Tj>9;BWUrhiz-lhF`yj$f)f!SuwK&n=Oi3&d3mf9W&y5$8!J|R7Z zS$eRI9G)KYxJbMYv;IoaWQ!StvWFNfM#dgDE5u(x3Z@Ol-?1yiBoJo5-DreM$fnu{ z8A|!NPHZXV>;tVZr8Si5lbYVcwgOK!{2}^84H6(x-C$}F7KlkVOT`rQ#U6*2R^tq! zD8O}h5fX1vTkhqOke_uC5+=hcp=UyTFiQkO->b6RuQFO=7mislZ+V`Tjpg|+DtGdw zy>CL!fDn;QSx=Hdza^(|10#ogIu=hH8aW1~s=$RUxMvFu?{V4peJoE@gY;F|)Yejb zb_4Ak*}uYdCe=7rs4!Za2jL=REOyqnZ;cAwKR4#;GwY+#c;(+vJxC=5% z@z0aqu$|ui6}Yh|m^7fn&ES@d>2RoMyU1U`GaH6A-PKLBvYdB}>4Gb;I*fQEF0~-V zZsHOhzEoV|$HR(CCX^AEY#T7WIt5X}Ox3cMVA&?RS_lUL?*xNdh?z$%XC}JlQFxyT zIac>w^5tR<7G%HJPSCd-6#9z-PFJ+`wyl_R7iMJf??eMa;JQsdHg?GsyFrk{%M}mvrYn}HF}0vViw2mJXlV6dCUyT< zWGL6Pp=+IUJ5sWZ(MStsQoz&?FBnK1D3>_UlyU}@W&X2GmH!;AehWTNBAN`Vcc{g*} z)h@NVq^4>0R)4W|La4os;jFhWHc+n(S{?OYY@-OjHY+?Si##y*hYHuL;3gqlz3I^A zY_HC5^8X7hnlo`TlzH=UN!6=Nme6M)me{Uh=htGJR=9?MYiOwBh5k`)`Ou*_gNKYy zvYfQT3f+enPoCtxWj%vqsfuP_icT7b>pD_oo28Y#kB$*|B^>{8Hz>`Loa1&?ZaV^M zo@_dTS(P&#!E}i|{QPCIa;YaK$2-O3B@_{%M#&jO+C%jV|Bf-eWSHqC157Wup9M6` zGcy2B2-}4?yws&@e+T?>(ttP7UWX=_3OJf0^z5LyI37?|3gs#=FI(X3u-R}2@SB;y z`)|eT1blW0x(DdkK8)eZ`eP9l*z{5=kT%hmBUS8&V|5_y?>ot@iM02lhr}!C@DX^V z0LI|z8KBHL_Kbu_?2kLFA?TAI2%WQeXe?3a)Wdy@(i0&_ zo#gZVYzXcv$9%q35d@qxe*EZ2;iM@uhD1gdjl=~=U(up#FHc^05iq&XVx4DNIKRFq zIe$sgS2XX!dE z#|KK8`>QDW7w&tn&x#A5jX&a0{G)%Yl9=BH_@hMO{rHVP(8t8z!`SflCj6DkOz_Lh zWN`hc^S8Si`VnDc{%}sSGk@hUXC>p?!+=>o?;C=D!QVe~kNaK3H{&qzF{2UqS z^X*3FadpVtD`h$I@sDNjXab{e|0tjDZ(gQg&oc6J=P%p+|8kF;`QVRq=e=w+X=7%- zXdeI!;GbuQ+EkNY&(6E(1^DNMkHQ?uvZD*&mQL6ABA7{f&p*gv>eaKejw`ahkAb-= z%RV0Fnk@SnFwe$6FF(AaJ15Kjb(rU7+2_H`f1dx#VXn)vUk!78mVFt_4O#Y;FekI@ zH^ICl%YHk|jal}MFfY%te;?+SEc-(+Gfpo*KZSW!mVF1zYqRXnz`QQY{sPRmW!d+@ zygtiLmbW3x{u`L@&a%G?^SxPi7(0KwhyDR<58_`Q{;@rL4BVP!{|n69v+TUO^>mh< z+K_f<+55rHJbLjR1^b&>_G4gwC(C{u?E7K&=u-&$pR?>#M|LpFJ{tDVv+UzxN5(tD zb70_L=fyVz=DaNX`7j5v?B9SnKg)h4%$)Lj;je+26HL#(3}#LzJo`6cj%3-XJ3N|Y zzZGVd$@6~~%&{!{f5S`x(ECnhhVd->BQSH~?tT9m%$#d^_U$lN;GakDHkdiYdEcLd z+0ew0$?tmsc8{T(s|l`ArNW&De~i z{m-yBoC47sVQBYVqRWC8`>wvQ9$EtyKxM$CP#Hia@gseUs48F)^#m*$2}J>m;?NYZ zs06A47M%)p^NU7N-~6IdDxBxde`p4Pt^pWEQ0+VoBaP;H7~)hKK$jAuHvooHseGO; zh{P%mKs*ViJDi#WfY2Z?oH(**gvn35Xv9d`ONvIsiKiBgFv{#l6^%HR_R^veqkNYv zgu;VTs52Nrb@(HTnZ$^qkt`G*_&|9^qEv7|u^wTv-l;|LQ&pxRANe3QdD9}cWM#zIKls@q7qZO(xQ@6O{q$#q@l!+3UMT;{2#YMj~@E?oSLOOgwX-UVn|uZP2)7EK622`wAp97@0MqS{f5akTnebBKHNvIB&BBL;TZNu3g3n~{4dICK z_7i$K2nuDtN_eQQfm`MNec=}29^vnW`-FcH_R?Mb0^!$$bA^`+?-Jf4ykGc`@F&89 z!kpf=9KD5Og;xlZ!dBrL;qAgr!XFB^3V$j5ov5Q{}7H4o-UjqtP#!>UMPH2_=fQJLQm&<5hg68 zS6nz+SSdVHc)oC+@CxA#!iR)E75hj*I97PS z@M+=igzpMH-Ru1!H!=Q!!Ybh$;YGqap(9)){J!u3;Wpte;ctcS3;Tv_dPfOQ5GI7D z3ug$wE^H89EnF+SMfk9AtFTR2fe9wdcb>3O*ebkRc%Se=;eO$#!rn*Qc>Y5;Mz}=S zBy17hC|oD}j_@bKmxON!-xnSf_Q0AC^PMj&5}qn77mgEF38x6>3X{U?glmPj2=5m@ zBz#KvoN%A;PeM+{nXf|OX~MUKeoWx`{Y2p@!l}Yp!t;c83x6iuA$&pjvG6ZK>M3FT zG2v;#g~B_8j|+DRpB3&FzAAiEcm(DUj5jVEEvynw7oI1)RCu-UJHqb?HwzyXZWlf! zd_nlRFdDY$pDHXDP8H4))(aO3ZxpT--YR@ZxK;R~@J-=I!oLVd4YKLa5?&*`Rk&67 zrZ8`?^*>ZNNjO`$K)6DA3kyA@e?V9#yivGT_+8;<;g5wogg+O)Bz#xc zKVsuQUU-slnDBJr1mR@iRN*Y)0^v#`ZE@`-EGBj|!g?riFhM_B_GnV~}vDaJVob zyi~YWxJCG=aF_5|;hVzugdYiyM_Xb09wIzNI7v87I9qs?@EYL_!bgR>g$IO3oMht< z35N^IgkyzY7hWj5T-YeQPx!EKtMGB*%ffer?+ZT{_9(RZI7)bo@Oa@w;T1wxc(-u7 z@CD%?g~1^<-eZKv2~QVJ5Kb30_k^2;4-0=Od`tMY@N;30lWjf*3I_>G zgk{3)WD6AJ=Eo>BS568mL(>RJB0FQkcQ9!s$XEemOm~o(jy9Js`{%LL4^!Af_3_ux~ITj0(}+ zj615yAd)c{7hba9ig^tSu3qFRU+a8pY1H9$uZZ9oxY|t z#$x_h-D}(cu1Mc-Fvo1m*0=R6#Yb$XcBBW)g~=V*zB&(tX#X*n*05#F=b~{uWbZqC zqGC3D;3WhQw5?|?%!mXhFowVA^h?4te0y6i09W>*zk5IkrcI=uGu7`7JQJG4ab~jt z_O_ljhm5KRqQXaK;F-bI`w(~|<7ve^CVZTdx3-}|-VGsOIoz?uT$7uBrZb=d7Tlok zR|7O};FR^GQ*e!gP}rY_9`x(&YwK;5DSUuqKh_XF)w=#00X4b)8_SGVo5sA3o{W6o zkFn)EH|>AgUhaj)ICH&@%HMO+<_2D{bp!nEc&i6IB2&29!Pf@P%xbfD5Q6Q1-SqG0+L&M%@_PoGUdhSKBsUtSMXYbC}KYaa} zc2glX1Ll!|#m8@CWq~{^|BKa!xfS-scLrBc^K7tXmUB&ViLgH89Gi;sJr zX}^Un?u|EbuFYI|$C=TAhW7kyo1Seyi}RT87x7{Rp7`mFLgP_}m^loP-m~w|8<`X` z0X2KqQx`sq7F7O0 zz)Lsworiwp432rG#?l{0OpOa`CbJBYAQL~apHky;HW}yHbbd8+0+XO}CTIvuJlpkV zSzFI)1m>@;XQegpycW& zKRx~5ac1O79v~^Ycu}(SUq3X0shXi-FCc#v_%}gg0_|+<{9(%6?My~ZAM7!s>wN|8 ztOGy(<>4PhZ(}z+9p;r;alvZ*5g{5G|JnYQ@PmGgi$6AK#*Za5^J9G8H#*SFe`D~E z{=DDXL(=uWodpk`nSSZdi<@@u8&VrEh@W=Y$UO$!uJ84EcmSRjDlZLc^6-YMb*)i4eMexrH z57F5d&9XyeGiO1(?;~K3Wqm&tX4awieKgD*$vyixn91%u`(&6ovU>KZFjr*RXTe;V zWj_z*D*W^Edoj#4S>G>*xi-sw4a~Fg&-1?m=DAsRvc$S9`?p|jz&|fPx5J#wvcuT< z`)73g-!0$lV|K6H6YehdU9zB_yXA{;aEvPyiVgG2uwPz}?eZ(J zTYd?}LTs2{Zh~RYya8L~*i^T)0?xlkmTU_Xr;l@=7Gr-z9uk z_<_*d>!z$ue_s&}6efh@g_DJ~!i$8B!u7(x2#-YnVZ0IH$-+|Mg5<*am#{@&d5W0{ zbD=l&eB*hl87~Gp*2RB>TYH4R?1ECbRXhG89N6%}EYys+?pj0&QM*4*XfJ2{ z#jOW|t#`r1$F*vsOQs=(B{PuhY`(Ns%10CNt`;ta@O`yvzK@HC=jx;L=%spAr=n=w z3Zkz<`YNX{JfMaH|9pe1l8;Q8r*zBh<7UHO!wsw3n7BPDJ^V$Sir&C1?20%?JLmh` zdZUJX4ldW8=l9fEyBGLv>4rZu{6N_SG5e^LQ$nWi^sC@s^Qt4B7qdi2-a#3)D((Mp z{GO+6&~`I(Ltv{*q3I>}?0lym*?igAr3h#q#B}HUNC97+h=9he-Ar7|uE9kc4mt=3 zsXA>O(oZH$t*n~Yl54M~Q zXS~T>4beby$#X>dNG1!W`Y8&<;9KGUie54QRlA=1c>@Y!I?AJCBP zBfsfp2H498pJ7B5uvvw5pbK8p4e%{i79On1*K`u{Q2Qv2yhz;!gt%J4dZ0t%*`|HA z4dRkx3qR#xPGg_ty}lSN6>pN|-Rlpw_CoS_9uf((FI*x8h6i*$98Hcv)zCV8d}nzH z8_aB;8ZV_TaIuJYPxM|wb63D`(_$lGFqvJ5%EZz_3vJT zOJ6J_TFq-{UZ_H+AFQ}##-dr3=Jr+7WoPSo$2lo{t-)6$OPyP(P(ixKo~PSUG7^w~ zk$f#K*tYi=n?BMuAbzK8fQDor*KB&o&N%merXZ%p&i0b27^q`QPq zO&3p_LCx&*_yF}>7TJ6E5p%{YH~XM2v(?sjHj5+ZG-e)a8)8p1%Jd%d zUr+jKH)|ngfg{+$(OYkenx~LK+a=CmXNuo7G3ZTMTyQe%$m{6L+#WLro{$_U1u>>q zX;$Bi?_><_Lu+ZWtvyk^^a}JxkA>Se^z4g%VCPOr6}&ows_{3t<~2-Dv+JXizmvU# zm!;x;(!Iu`W@^)Tfg1};maXA3EqHx;aMLd`xAVT_y6AF5*E~8m*{h9m2VY)XlFn^6 z3khZlpNt@%jp>|rqrcKR^#fC8IQv$n=KAmm{a|TOuL7SukL#n$Vbl8CtbjK-6$jT$ z;oUu3?U?HFqUszjCU9_?J+3|03(3y4-rJ)&>_ug>iqqjD6{Mo@$s(V zt@bd(8=m>|hPPEFSGKa+a?qW8nLlCbzxNq1oOC!^2M6*V8o@d};}J00o{DenC3&>M z$I(C#WD8;N^g8T(LeqnZV#SV9(A17(&AZ~LhS)Umru@uWvmB90up0WYQfsEVrr}Mm z`x>J%L)6TT75q|%Z3?UeZ~%iRjw{paD)85Nxo$XM#ZydYrlO9b9%3F3%+K5oYGILQ z0AVi{`UWuI-p?{&I=q(I%vV2O<*JQUh=ebXzzKf%i$tnNjK<$-6Q*A)Ttb|loZm3U zcgD1dv{a86} z^LKYvT(r|4e}}G%+zWr4uX;Z(7#TL>=X{$#`kQ2p<}bhT-|0fn9~W#cFh1;`h`yWo zTbBC;l5ssJ#>F4QdS+vXzaweT&--@h`pH%BSLb;Gdj5!HZQgHNU))WABf#8KAzeso zX2+u)jJ|r%c@+)%dGUF653}9>mGu+S{hwJsVY?~7KhJ*@=17)(2+W09c5(>TrRP5Z zGg*jdKMm%JEc$j89>yGWY`SlBApD#=bHwu3xJPsX+*fAk;eEnK zgpUiobMha`?wyzS-t6m(83EJpFB~Z>6OI?2BfLP^AiP@GDs+Y45pEDZApDW=O(Cax zO#hFbj`y(+3@w@jn--WU#g?9)a6+R=(2z&Kb`Gj8+P7+=~1jD{mxLD}DrPm^V ztK|M)!tWBn{CUp42hIxqjEDv0-E>EO_XuAnB7X1O{kwACFZTno|5ca^Uc-3%5E1TZ zAmjfU5%HJP9r4c;)?*=beuKBtgY})^#ifZvX=22P!>{!WkH@vx^N;t(pZ*#;J$`ZX z_wZ{yEB?3{U$F?Xovj-PJn6K7j$4c( zOT_XHzgbOfczNbFezO<%5$Aq>(_}v=m%-r99>B-_Gu8L@Hri~MOz4V zdH@X~jo8gw0_Ac^2P%iL0bP$&{opM~HJ@Ph+u?$I9W+AB#mA`q$h&Pwy#RpFbKs#G zeq*#^?F#<5_=-rLV-~DhMC2sG@6e%wAe9JROh1DWHjeV-@moD!80TCFil5-Ul;1ZS zb&#AX#gX56_*@F#0qYwYttpwe;U6c;0fRJ!I!$4i;F*NjK)@Dr(|_)LAF_U(B?D3 zDM1+A&;Jqra(QyD8|clw?s*VlFxzYFz--S%H>ty8@R1OMuKIRHY41*W9U8!+A-*Rm1*H+iQwuyEIqaLjKYz>!xE=yl2Wp1G?(9( z6Kwewrg5pTC4qH!W5=S3s-4*2!>85sQ@4cav>xb6rRh}UaE_u6-|Lw9omc1EqRZjE zxx6=$_v4dVFvTT3-vUzLh59KNYh1MD9|7qum6m zcR4;L%8yE305Vs(ay+=jnRUoFWID_^U4r>ohA6m!PV5%nw8^iHpe*PfFm2Wwbn=H zbZ%~&PBGpwfbPuFl?`rsIV*xywp-<(Ta9-6nZ&TJ+~&U#UR8eNx~9#4zi}v`kBN=} zH0}+sm5pk83f-J>yNmga4Js=I>&xv8dau5lo-9!VmZ(@yb3=VwUtD}Os zM(`(LxI5+qsGyxYN^!M6Mm`1u_mtH7`fFRH&CbB4K(=(;%lGlXUyoV=;%k zbvZtn6M;k7=*A(gp$RnY zUky%7M=-V}iTLfomiG#LzR5O$vVx_*v7H!I+mQBuviB`89=3`0?>wF3Eo$YX!mHZ+ zKSVWjWPZuBGnR>_LJzoo5O4_^3Iadv#@5(XPjx0L-9Of?{@Cxl!;vP+C(1ul*LEID%^=ShS4a1zvJ9t#vQvr_6<@ec9V^zYXqoqL%nfIs^u30Os;F)SPpz` zWFdH}4&yW5`$7-uV|q|L*NNY3+IofU65=yGf~$|j>WtDgc$ISzqTX_QV<&AueSVax=Zcg+Fw&XIq)~g%Kb%xlk zJ=U@O$a3Yfyrw%zSUzw%cPf0d-AqT@xEE-5ztEUA!_o%fTS5aEtsiFSuius5dH@`b z!$qARtq9%a0d$wbjr3cwiGLC2dO%t3vLg-a34N34xq(|+pGjtq`&Jxq3tQi7!bU}y zAq|IgO(cd5SuKMr`6*Nk=C}%`p|vHb!$!z#p;6rnDBjbz=^@t~pL9!1O!my#Zg&ie zFy!gP-z?kh+GK!06*4&wu4zP>$zYSf#IwQG^*}2UnWDt*a!ljVP}XLSK$=ACpau2k zcvIIda<7Um#}~J}e=2s|S>O%SLu@*SZMw@!IvYEBA{u)w2Qi$vTO0%#3O2E=S)bYM z{pao&eqdMl4D$lJzsK8^pmzy-o|R>SYp#dc9TVYSzVQ&-iufB~1RuZGbdJDhNM^R1 z{LJqJ_Sw^0uIWWia-Sb9poNh=KQ#6z3ckb*uR$HwI&*%A{R9(f;8p~0>f9Rlu+{PVZXsv`3BCT=#F6-OqUhqcuK#wFwk8$2p?Nffo<*2=rF&MuTU!ri7ivC{Mte2* zpv>P-WW9G7ZGO9MQgVJKQD8 zrI&Nm#DUo&#ykOMiJ^{;SutA}!IoFCqhif&KV~-ZtYix8j0M-Mpii@Lu59AS0p6BI z3usJ*&=(j};$+lgaWIuYOrT-|l!CeS&;eGDr|uf@)M#Zi55=+@af>5PzXF7D>^}Li zP!vYnuu9{`*wEgJ$Ko9zc#Oc>^i8yfF$}^A`#zX~TN0o*^Q`PdkgeJ=4o>JNu8A5I zI@ddPsy!)oDkJZ)Q#r}U#9BymH}r{H{fXZ-`Ef#TIMr{!VzS7C1;f~u+yNE*Yc_+N zBp!tbePCul6_)EJ4sIfUUly?}8AHg?u3^gwd;Av+IJjmVc6v;p#KC35>|W;hR1FTV zGpJ)WQjr~qvfp%KcNvdmJ^lF9^eeZd5NHS3{Z0uTgDp3K23+FFzoJ+>M|4os>mz6uefU5Zc5vI-NhwW8h1Jf}j z^FIOAb@mgH)u`k;6`>H9&Y8D+%0C6%`LLR zOa~^E$#E@i(@mzsy79{qVp{Nn_nFs=5 z)=NOLD*zMr_sI}FPIz5 zSo}_9+J_gf$dXNdoY=#bhePJ$%Pg3mn3$jx2m#2r!XG%6qZ$91sjNzCJO{?~p3L=> z=)thii|V75P(#d_Pd`@L-uHv6e+k45l=mex@~TVVHg9&RRjhudhlP5BSTXn5@o0q0 z5y}pDDM+C1DY++8Krw|3uHmAx^U_-N6fAwX{+mI&%C!H9&TfHj*BK?@W7` zC9%8nEQ?JRGMvtz>@>1qH5Aszv7>$ldpTs9*ln*rgT~1cfd-~P!8IkO_{bfENj9j& zU+qdt*LWRVL*+Ce$Q(EH{gnUP#;@cKGXSMRxtLFGi7v+%l`+uE9iBU43OIo&;ZhSXL21#fuSt=UAY$y3w zw7>{$X3ptU|J4(rLD417$3tV^!7Lgqq1LJH6rsbnf@%<K*?Jk@~+7Y^|grtX~`=Pjgf1{~vk!RkD`9BTe&K-QlHa4y+NBl&zp?{Fh45XEK6+ zPJ%X-&l_?kKN^sS;x3$8ffEXL$=4X6?Lj>^JSr9HduHN=t|d~4#8!;%#|#G7`~+jx zMjF+%_P*up*z=;qVjjT8(V88$OQo*OG3zpBFlc%fQn7m0Em@NEYmCZn+O(&PB&}D+ zS|v&Mr6fI;lC-(wba0jeE4`wcwc+GkF&INj#whPH+!as)SUmpd#UP7b`;;f8`X#vdzXyP*%P zr0ygkYx5`{#O}ZkC99jMfR(uEUJoyZIpY-qt#g|JEAiLh>Urikm0kF<;w>6U$Xw4$ z>`(T^J;UBp>^dy6RhbTdk_k{9vYEs|UT9&i!w=f%?IzQ{?*?0c&OSq?SiZ-t-_vax zV>>p_Y1x1F)EzDe-ik2U6PxU6$Xthy7+T-P(As&{67~k3J=-f5|BSIx6iWMdLkzV7 zQ2ROF>EYXWLTyYna|~YXV0&Otgcj?>6&A{|)lXtw6G8+U4apVyn^XNazcY-%GBXha zk5{5S#M^oxOC6cFK@IOd9Trg1{#O}Gs_{vmWYbg9$7L4bV%vnw3YKASW&^-ABYdWr z`qvG!CPU6V_YtgiB8K^V<}q3=ySIapTfW-0r*ttrb4<3|kZok6Bl7@Wiwm~q^3Aya zCEb4soz`DU_lNNjbUp#F7Wn9XLjZFa)>yur+V6b8vi&R6{vKn1%-!ZQlwSJhLv(Lb zNI5*cA9)`khZUmAuxdBCKV`uYa{`M}6G)WfCJPQX7xoGOOZW!~X z-e?9`WW6)YKaH-dvDuI8@K?UsePwGVu5wtYOR)i|-6 z^~qjSMOdf$fzR!P8Z`hZ=V7D)=_bm*hBy$?jU`J+X^Ae!=iJcBGZQZdSAWPZU}goJ z4%lv(nfQ(&z9BqzfSmAsnWHGo*i}osleq6?bu`TvoIxfPt@LMnZ^JW|iwyh362Qq4 z`AsLs1_TJ&V7DDo;ZM`K`pPX1BjKk4``!NT0>5*a-#JQWqK8rFpMog_oRI;?WF@of zx6FM2pOE`d=4kYZtaV;>)fnCvtA!Wj26x$e<9+Zm63=r^;g}U7Ebf{;NKg>1;Zfub zOA0GmUt64S)^%A9Gw*TQCfUK3aW!Faw~BGrW8VhP%wVj909wPp2;b(v$+S@0t=KjH?uxaQdNz}>qO^`CWGd-|HGwui&(8ZYHE-DC z#F2$pUbBdYbR#%QGau(^a9s9;Q9K3gt68|<@(b%D3$D0i{)LMcT(qEmLGoJoO3uGH zIe#9`6thJ7?f*C4Z$9L3@4x4|^I>)5@A|5j|1ZDqTo0zR9}BPl=v=S=Yh7{Cz;FCjk_Vqo9nxndre@qVQ1 z2G0Vd{oX_DJ{|hI*oRBX7va<}0C${y_Rg{o>T<5Y$>D~ED;E{Tec)|Z;OKAz5GRKh z;NbA3MO5dVz-4Nj8orXJhH+JTgim*&5pcoPS0;IRnpdE4a~k@%d2N~zNW-`kjN`U- zf@g|tc4(?=ShO(t#lywOe-j#Q4hVRoaGmf@;m?GeP10Xa^cSLczITx9yr)O^$-+9} z6+%xh^>=08CVWHq2jL%uJfFh&J`=Ko(;gI_BrFn+7oH`YC%jqs9pU}LM}$uZpA)_( z{IjqJ(qy{4(nqWn&KF)STqV3sc$aXC@aMv`@OQ$Gg@K+*M_42*5l$7(5-t?pEcEnb zKPvkZ!e@l92;UaIC;YSUGhvR-=ME5#6xImm3Kt4ng|2X&@H@gE3)8~i3*Qs=#F8cH z+D|w}=$!+eCi^AA2H|7ECxpKj?i2n=_=zwNOo{0nAuJFM7M2Q66HXM)7B&c%2%CgA z3wd{j@!v1}k??WhZ-qG+TtA$I2tA*=?-w}RSxLNqH@PCA_3EvTZEbNOxmFb)y94Z_sJX<(JI9FIF zyjs{Oyk59cxK8-EaF;M6JSglC`Y}*4KLO#7c%1Mg;c($7;Y6XQSNvSr=L)YBE*35o zUN5{+c#Cj@(9=i0OZIl*d%}Z6+&}o6@JI-Y%>O`O4+w@Q5F*Rl5aT~xc!tpE8^`^M zTV(%%@Co5=p{HN`L)kwQb_n}J5H|T2mI^C`lZ00YuN5v6ri46izDEzVTN#QSr?ZV#+_X$50 zb_o06SO)WXlrSnBE<99!`c%2k7hWd3O4ua)rZ6SEL%2!k=~e%k?9U6+!ruz>Fu`U1 zP7n?i4i}CReqDHj@Mhud!pDSv686J_0pl$Yo+La~I8j(5TrRvrxJmeZ;RC{-3;!(4 zfsDs=dJ8>0>le!2AiPF+hwy3PZ-nHrjE9$Ch#}$0!ZU>ngh^qG@LR&Wgg+2ID%>vo zh42mG?}dL9=3`7^`o{`Oh0}!R3Kt5O2wR0~h2IhWK=_F83E?kj>=3Zjr@b?l&3{T+3B7gt*UgA2`D}QnGw-D=+n0#gZ ztlr)L7Na9>6;98$QRu=t3sxacbc?a-T)OxDM`=w}ev*fK7vs|TRz1*x^d4y{g;pN- zOuyYlD?2k5T=NHb-}}CFR*eC(<`R8{8~-U%iKnfq)6jl*&VK0XVQRWrfNemYnU9)X zAPmhu^XhA%)d%ZvvX8xb>R)r(vfh-nXW99vtN&)|AHnmxscM`O!fUjtIWPI31_e?# zj=toid6X=kuI%X>*WTA^5S9v`fzsgf2sR{eMFX=!?2BUVz-*U}b7FsT8vletRi+>~ zw^lYY7F-?0?hN)qp@+y_#CPW5yKGVEvV?1laIbJt8)#}6TwW@Ow+DJ#%DMiiTjf8ay(sZ4^w2zt+~(mPqoEMAfR)Vvg{FZ*oyxpS zJ!jTq>+6tvLAL!-xlPy|(e2jVFsg7eP0ySp^7ffghvS^;C$atXq`xBZVblA%K8fQI zZjB#1hO3W46LDvUcm!b^OK(~=h6vNDgRAd_DXaaMqg*IkB!yG_xtk&kvB*Cqu^;%Abb|cf^a4N09I|CkzsfUhCy&fC<5~YPS7>N|t_wo%BP~BmK}u+XJ~eT`2nX z>ZG549GYji3_8%vLCE0hi@5)nM5&Houk`ZYRQhNSkG>IBP9=2wBgA7YKq z`;j;B2eHAd{~HVb%moYMwP1EsvL{dzkJ1uk49f`uk0uX!h)6L(Ct^#XSeI1-f~5wq?3` z_6W>$^XyTW>E_vC?ED>W51~+BuL9AyM=%2RsX5~mM z?#E#+;ro?YDnGD1e-(!0CAJ-Q}{dKr@{^)^;I)| zGA|> zaFKAG@Lu5`gqP1xF6exK01}F)V8#6<@vGEVb zk`;fXR+KhyL%LvBe;cg0sDz^c+hM}7JZfwWwv0xfa?9Mw5u6V-u>@QHgynVwtU^hM z)$m8G0Y!+F0Aiio=Yl_Xz&?+5F!y;JaHzrqthn;t9DJDr`Y}YsJYcZ(GJLx?C)hd} z*!;)9r5&CYF~|XPdEFu({-Knt4!(J&kvVrqS%qM@fKe{3WxE2eLdC&eN*J}#ifJ`C zbHgIlAOvw1N=kG`&P=V37Q!?=g%c&7%CZO&z?nrVVVH@KQJ~c_cKaud(VCjQjnUIC zL4Ho#i%j}_KMK4Wh+cw`uzW9u_1B=_0C*ZMW?b51dJ2a-I&VZW9o~L?v8m($NQB## z13|``6r_%sI7IPle4aY3?)m3dosMj_!#3*+xtgBJ()!3#`g2LpzrZi@$(rXK13+1i zV2YY`Qh`=xaYv@d=QzU-H5zdH=pIB*zo=E*9BPUpn%Hm-qm*4;+$pXc!}Ig>+uY`# z;`nb`oZ(lvnYf29ElGub#CUU|=j{+##T~7n4n$epSwSQdn|J!lMm9ocd^8V7Q#&*B zoXyNSo0%6Oj`^W+6Pa;d&XPSseceiPe3jmtcQ8M6@5owSPS@}8ETxz$Gn35GY?z63V@p<>= zES`iA>Q1}|vL1nrM1jT(`m>H_&OjEJqZ*_`oQnD&x~I%g9wPgmgnu-QYERW&{=WY< z-`edBjI<8zpcn1Ck&OwL3@cGO#K@YR`kFu5yvv_9+BcCb3;gO4-u;fA!Il&%Z}@@4 zFP08a0da;MH?~-w#4j2@9Nqyf2K7cPhn5iZs|YG5Z0?`p#tgbt0op^vJvJ4%(;Z%R zl^=~iuyi>5hf{$EyPv^w`#T3w10Jawq7oXT z6|+zi6`89b0{@fUBJ(?(xD)|iL!rnTIIJQ`!x~0T?BorV=bmF!7J~ZbQ@-u)C|wmq zaO(6eXH8`Dhv}f5;S)zsOL=?(EK6!sw-~)DGcBz_`Q5C;{?jbg!4oXKn|DR=8bb1G zsq#}3uLM&YaniFH`isHiZh;Y;ZytGP#(@Q-sH*`XFmah`sNprq%w-G(cIe}OC>xWS zj;bS0yEI2F%k5QjsUI0F=U0U-&YXgWQSGxZh!nZniQGwBGGd+xN^D_MrYASL?LeEr> znC1~a&(jl`pScqX7`wL@J3hq`4#lh{j1M8A*)Y|()ALkkJ9cD_OO@xm&@qI!;R`{r zLeyi7x8b%$D;R20xcOkN17*Tr19_Vd`hzWhrt_+3CE?l%_na#5KeR@7L1EKZkTs^z zk%!A(bI`s6&ZpkoV%0R#5D#WRzI}OtiA92QX6oLk?X+&JC)gg-Y=Hhxll zJ?+_GcpQZl3nE}_GdNB%@u|}f6B90~OiN)&V}@72{LHcBiJ~q$H_1E;kr6#+o1ggu z?_;ShsK?;2zEH3VggP+ZpR;%zldplT2CnqY;u1Xx!fS>HCi%gcZ$SLE2Tp2CU(lGj zy3>AHWBTij`yR?#Yp`?uIHsu8XouDA1$pkm5ZY-Nli1laZONR@<_|VJ$8>>WH2-)C zv*(#q8$E|Aq9xA2G|xS^!W~y-dq|}9HTSy0CTg9X-SL&y*P14n_Wrup-ut-zHD}>t z725g~C$?hod+>8S90Jbkln;)9;up01*={acB@DF~kZ)f64jC_769$2Z<&Q|NaQ3-% z5jtYD=;XsOvf7x2lUns*hX;#=z~V8Nxqna&f2a)>N)0nXk>qYN0G zHXZ4J2jQ^sEYiShjA1k=eg-9?)19YGP;@91wRuP3RCY?%SBx1uo`cpDPtvY`kOK}hP$Eks+YK!|FDWQ(ZO(@o~aDe4!( zcgs6Sm-V?E2$~g0g5^5PWl`1ySbZTS+4qc)P}sa3wUFBkngG&%nQX1zu<1cA^vxNH~o2!+Nm2aRZU zH-cU5)0uiUs$XIaCG$NnF|-%kKH1)*?u0zV6>Hs&#u`B`3K5e(xbkH1#T^H8n-9)& za)YZsN1JNPkv|Sxf=uLNDwY4**F%6K z#nzoRcYFXXqxoP?a$57j9?4-yl!Pxt0ivks2$F>)5MZyfwQV|*@{dap(Y%==7oyp> zb1(iF41W=QCQ#-Bs^bu^i1|yB-@#D$UGvDa6TOR+Il4(A8Qg995 z324jZl1%0RWZi6?RL$FaXMO=UZ|&5|L+eQ|Om_eW#v!@ZXE6=NK)l7e8<>*sUKhgH zW-Kih&f`4RIMjvF;fq107)H8mFZKqyJGB&~i-UxP?)imoU8Q?X6&#|dg9seMyxqX2 zhPC|Zh+rGiwOx#Tq|NHsf}0P<79xHyE+^s#BiDTRgK?cT(YRJd_H?WqV`@AW4`E>W zx@+y0-7Qp*+Om#*-KPDxwhyl%XUcxG<6@`$ShumjKeW6cr|C@yTW2l)1@hRyQJneU zSic=eL5e80AbpZE^HIDd7{Z^%Co9M{Ab_!*6$QQOFw7fU0|_wK^EsWVLJIJo zPdC<~9S^FbtlEeh?$i7iEP1@dkK{Pz+|MW`FLIN8{LSTgT#YCPnM;E$-$6{Nz#S%{ zB&KGKfd-Vb9O@cMmySdbmN4ai9Bq%Aa-~C_c6NCE#qEKDL=xc}f-Pea3kCMY`<%x0 z4Lg3@BV~0trUJ*hWB0=wN0%6{Nrgt`R)*&~`R>&HJATu{9s8!+7_M_(9_k;Mn+(r( zdcixq!A}K3alItp4Go6(bM}vSuX@utXTKA_3zOM=|5?G-zUbS-J9zvUL?~}S+rT3p z)#cSwFU_$6ftxI!i}sGeGwOT@I+dzAx1D6l%NIAHzKehNJ>S)Sk`VlQp^Z#J)dVt|$*!dBqc)COiPty>L7PVSKu^*LQK7`@`{N|uqGV>6l$pAVGv)q2K$;WN{n>hw^*Y5n8 z{E!ctgx!UCU=`yEhoD9hAAycB#~)pK5>nlAB7%D374j0z*)twDw;C}J@9-x+!aS@o z1DP+k<1A=0YKpjlsPj=*d560*DD)J)U zg~I2?ca^j|wxR>^kplo4A0>xRM(52$9#_G;l@XA)V&o>t+>lRO<+uqta9fBN$7AV`Yn(25~Y?^zvf5=|(;NuYIJjUsj6mgeb;9q*0^S2Z< zMS1jgq5SOI^oH1Gqf`;6yb#q7o^Ud_{>kH!|JgaXjmeUo?{C}=EiUPdL(-}`4b_lvt=)S8@a8OYBWSy?Z%G+K{x`0oH#Y4)xugjGX0|L)t_onP}*MJ=d>N=gN8=4gTn8nS~`wDhYWOx}>!ot@4{o#kU?#qyn4 zfAGV}i*dpOzFD0RDsxc844-TD_~CM-*Nq=?$*?EtO@v-u8jBiFU)RgZC*@G0zLVcmi&<3bVg}Iy$Ez8Aj?$DeenASPeAytml z>r$bu?&zUQea>jO)S!6t;L1n%>YY%r6B`A+%uvIOOVMSE$Fh05v5{r+f8VjuR5um;`$<_orgDG>XClkqNeQ;F}ya5fI?F8j)KuKa?Whp!Ta;8)`R}_<{u|L^*V%Kvp8@_kwdNxM0#0>1rPG>2))=pX36F#_Bo9Gw{RKSy9WNTJG#xp zKdW2(`|f1>KsBu1%E0J?X!`-g2t9BZ)o}z5dO9Gy(sC4>NaNwg}^B;{!LTcCV}UBC`R&p)1|9YTWUq?sb*!`Q?bb z0)(Te6Q`)-Wdg0xt*c_b$*1ghjb4Uy5qH|`jrP~%oc5YjL?!>5@&$hQ@R?4>i8Vw4T z3pVcxBUo%IcsbOxc&o$W+YMcO8{z?Wa$<&W&$ImIh*T(rY;ck~8&z46Kvf1?_*{$O z({G@B8f^(+*K>?-Fb?-jvgCEr**l^A1k7!& zef)AQ$QHFiP^lZp#Wpf4J`Nq|&7)C7zVwScP&SXIphluUgk%t^KA8H1BnSUP_+re# zu>}Bk?kR>2;U5s6;in4v=@Bs(t?D?l4-Cdc2UANl%>bgIV!p~${8+aZM;TCwDag^_ z&*1z+z$3~|5p(b5<_y{G{t$Yq zuo$B7<^|QiP9b0Js=O-%^KzpiL%*st=yR9ZLq|9;d)>q*M%>} z(mWiPH>6@<=3G2QinOxbW^5@bE;o*l!Sn4Jtgu>n1@ z%Dsl$9`*1rmy_$|h=b=@n1dQCFynKLTf@UiH3(2=y8C4z_d4)L9wC{B+NgEMMUdV~ z1adDeaL?tD5^hM%b)gh|n_c1(Rc2L!9U@9@NhHnDp;ypaaE57>nb?ED#F0Gb`0PTU z7w2eM<0;M~p{crotxg|IxH_yh(#08csXCs;L^K?0J&7eyb7_ZhH#0C66tUKIU%q}2 z{`rF|3C16Cgce<%*kX!gAE{pY4HT9vxE77G5+p2!y%2>rVrtiVlg$xGY1#txH9EV= zeAe4Wfw@*9+zYkTf@G3S9eTA0>jR0+rl^gNgU}VIauU;76qtc_v>Zn4klOV$y8z7` z+I@brp~01FRvF5$3qVSsjcteNkj7=`&^~@%`s?%d{T!*7=3CyY*-o@`(EgVrP_SjM z-_UXuEJ&A`4L%@cu@Jy1q99LM33eO?QzmWW`U8z^?3#f*{t z1A}xbezV&P+SP(B)Xta+wK5d^)g%J>!IuAl+wd0??=FqesoQ)m6*j9;3HEIGwLcDr>o+;2Fv+QBVI+|8JXp_2+*nbAc`2vDU%x|2vRaU z3Z9E|j>*MXg0&_0w8SfDSeB0WBdk_~^eP1U8)fIxL+1nkh@L!Vb0|HZf!jYv&ldR1 zrsqW14^Phx$T7Gz>$4H#XJ}39i+DF%FG64$BH_O!d4fswIA=c?zvOFmtVBUFPwBje zfP*}>vye?vVM>{dJ~1-b!rI1CJ=dvfoDZ;`I%35iyha3ua(0{l5!7$T;J_JLIOIgK!C$JXl zoHh7Yo%tAJ8S1hc9i|w^pg2uA%Q@P#RlSfD$aipV5aMIA^J?|1jH3~24_xW7%VHh@s+5+XwIn2aE|ck0bk7J z|6}icpyRr#`(KT9;|$4=QSgw+VMyjjhgOQFAcrZ@plOgT*;ZoNk!{&l9QjZAsjXr} zF!6{u$w+n>34FM|jwvpLHYHwtKvQa(0H#TcDK<{51WJNQ0XvREffhVb{x}%o{3Cjw z?>Tq$#|e*iy|vz3>#gUN=+3?O+;jHXXYYOX-e;eEvaf+MTmBSN`)`4AlU}_R%3dDt zj)3ljYZUOnuLAT4wPzL^UjO^nZdI{D!zPguA=F6K&+-UV7 zXU|!==m?~=1%1OEkAn2HQd`S^j#%QmzU91Hu(>J?Wp*ga<-9~pOFFWQxK$UWS4ZQD zkIX-d(%~WlF+*-O0#G(mg+A~XuMY*Y|Nh#F*kxUUC1WSA86~;xY}pld@slaP%U_)G zH)_i6^anFM&?05KKOm=g8&6pbMwn`ZDQ3m3ddELL zVUa7H&AC~X`K4EnM*T zd-&~)knL1_Sld0^^orzJZr33;#_v|Yk*h&|+bTD)I#r~CWi1VL6J>KD!1ve==C=Wd zte{M!h<67GuZdMX%wNV++rtQUCev$|f4%3Qmn#om4I=s{x(7!7oBjJac=;{;OMpq4 z)}HtJ>Uh-0d>yjzO&(;|!0a|@cjCwgu zMcnq=GwW26-)G|gGRihF(_K00-K+8gPt#gIbCbNjeN0UW&pbxYv~Z4gPcsRs$ujM2 zr4vk7G5)VHnMi~N_&;5aA?GLcUn#}faDbYqG3F|3pkL2mjQ$cqRTq?B)nvisE{f(oRuSk z-s@IVAP@87kEQ~A{Pufp>T_P5gZT9D< z{Oj7ROPo40lvq#6HY*{YMM7Ggyc1{G+ZaJP^|KV~lD@;&jeh3K=2F>DtWXU+HDNpv zYfybCW`w|eibVD6416={^48~K^aPbJ=7$B6I^Cp$)nyCngP(Rc%iXsp->b!Y7M@QAbX*|1pOLI zV2enhgDRBKXf_xz?x4+hl(#TaM5WT8*q2bI=9AFmJpF3>X-{>|L~F8CE- zKNbErb3X-srj$^*#<+p9QQJF!Xgv_8a^5E=l>H;q>@@~$eXemr*+0ow7fdSqDJlE- z9dhRu3TXKaAR=X+ou(g5>ie1L`aTs$5L!sUs23{p6Q)6iN~NGg0kAGK5unudCRT$q zO!0rRJWnzR51ABrmWlaU>nG+&lTT>vEfwl9ej;j@FOq&XYA!FYl*pDw4y5|pWM_h)oT4d8Gb&W@41yjuLg%+ z$rX-Sj6fRHG3wdF%ffbI<=yH4>nUnyyMCh{j(ao9+xR^x-?b+5yBdtq;4;2)YIPyq z_xSa_$Jh3jfArhruZuUuK5}0O7gwz}?dwZ7uaB5fL-=mzMl-KJK71U+utjcoL5u z`m1B+>e`@iyBD;32Elt{iV*iUDy5GZ|3fHDzYYW47ZSafymqa5VcnX8Q~0$4gaGhq zzVHDe*4Kt0sOc#Ul3m~)$~F3(w3-eIpY(!7-q^Nj^gVK{&|j&u!kayRROMeq&lXjx z?U)I$3ENf&o%^q_NA#rQ9xo`I#e(eZYRKG}@i*GGy1wQGi){~ArONMu<|_K)L|eZV zi6G4O17y+?MA2yR7dhcyPt^(V2=0O&(7Gxk6s}8^zYnp6^3ct%ZhRNVZhdk0MY3`n2xo1`+{S`6ST+sGO&kSb9{^Eavl|=c!7^R(r z-!Y^$&fEN|2!BD8T@)`!ElmuaUpkTari77M5OrkgYd=MN_yJdvcSOQ+s_^^5WBGXf}wXV^A~MAHaB|AV<+9?_SCt`jyjKHK?-pkr6(>4 zVl{_f%q@y-`IJB!m}!nMiMu@`k1yyyzP5ktX_{H?m~!m1JJ}UhIG_Ds10R|4_+umE zTTYJsNNkD*^vL9!@&-tM8oy0>r-c%yEncsN%1Td6EBMYNY{~a?b_ET*c?h}O%8OY#Q|D@n+XuPw`w0FN{3{h3)I0iF!V;0jRNNT zT|7jwYmqYnHMW+H%6?GL19soTUS+Pv<6YulD;&1#U|vBwAE>fWT^4w3)u;HrzxXXy zSV8-4>UC&8epZU##{lK%LytMPcZH5>*qIq0{snpx(5#>Hyxr9&mBI}hu=KO67`l0q zUj!@G|J{G5*O0k0@(dm`Sz80gz)YW#7ywOwPd5W-A^esKwTfDYI+vz~lnPpb8+xF6&i5LUik_AC zu)~r_@&c2&kYj0^i{}&K_!WKN29mdFnj}2?J_bioVU3J!CL}oLZ{fZ|p_c{TSN+=N zZf|fcUo`^IZt4OcEdt7tv=OdsG7hqLRpuYJ%$%hJiUvJvZm#;PL!4gCc_5*t= zzWrHgLe3*EbP6}4Co%NE1ptzdogS(4ko6 zNku#m5qhb=BuxzhbI$i6%6?$BTvx0i;J3!)C$RQ&v7N=*9TrzErUJB|i8yz#t@yyq zXSO*?u#`GnfLL-Fy?o?(-Em zAVJk*G9R^@+HX;4HU0^6{uHqVrLBj$`WKq6v$T^}WXj@@8iql%dpa8rG$Bw>F8P)q za~K3zOyc6KcHIw>pJI%=A)~G!^CBQ6{WYSkZhyebDNdqqK-rVJ{N-N5EPf~|0=Xm1 z1>S6#a*gJ}`jecLg=2i+LfXog@)wO4-oCK(Kr{rWxCiR5^(3Tw1X}|Rh^yIuR51i8qArtT#EB=BPHg>|b+}soK20m*MAeRON17O@H%|#-8 z@vHS8@uXZ3Q{u0tYc1tZXkYl0{f})&5#QiOo;VqNszfZS6d#TBs0F-e`e=e>vwiRBq=5SY7DPgUE$x{p}}QP-T|7)VBhv%h_JtK z1zJeye(wl^QfHo#LZKSJ1=XgZ0lYM@^Si>LEzFu2hLY52n;@c)&KB>}tc3rf;$D=L zh>^$4Zp)zxtw}VQXUqo6g`Qoh3wk~vo5iqfJ}(yXpSigv$kbXcbOJ|QDAyT1V+s+& zC2r@W8nZZKR-~-Dmw@voBA{>saf|Nhcv#|!8I&Eqf8Z}pBcFivjQEfq`k%beB*k`1 z22M`!9P_iQm3$e*_fivQOlJ+kDLGZ%n?`Y$UKM{O{fe?lNWQZ)Wjm(o{4bCXD2pcHLbF}_&m_uV?#0#r)kD0@R^Hp<@ z*hx*AKg7PsxVt9pUAy1PXTKF&UDJbaFx8NG3X+)17cue4Vz2lZWKT2|W-(o6rEWoK z)S>8B3N`UevuMg-cC3kx&JSmqfy7GUj;+Ee1z<3>ih*P{oggVj@$GgL?MCg&dZ+f- z_9HqgG3QI1|IW%y>|6IWW7ty}!?bBdNOmD4HC_|rmoEOQ{9YcO;h)plp9=EH;M$<4 zF;fVt%4cEAPqF82eJU-zW3Nq2fl$ipZrEM+xC_zh*VJg?$W0+EZ)wQ`8EnhmVK0?f zm4PHOUeAdF*rrlfrz&-a5NOVjGFAaw?=&W%O!7J@3EfTKXiZt+4XRSI5t);~9$QJzS33j4VeV zqN+ZfZwMt?VKSTS^PKs5mOO(;ADTFw{AIG2@pxbQA9?;~RI96Y#=7-G#;n(WzgOKL zkA^wzT5D{3c;T2#zPepDH%9fmwBMwN-o3nB z9=Pq0D?p0!8&u>-N?&W5T^bDA@c8L{=4(0gd}W#}G+^Q~YsI&{joyfI5&KeI_u1y1 z27otqH?`>>C<#d<=R$mnTYx)oQ9%lH6Kb98>s}q~vaK(hHckXRyCGCKfJ(c$fKa*0 z&^a=3O*5idsTzDcc(wq}Hjnx7`fta# z8QIBLC(JTg+~hm1)P49xd^o8)r!Y2jq??*uM0@Goo+m&IqSdjaD{C?EQUeP_tbOk!lMv+vtAt8V_uXH{-#DjEwZaU2X_b5)pU! zyg$VyoTULd1vFbJ@8@S#wxP*rWhp#hZ$!Y}FNpfG`a=v5;vEA9n*>y(2&v+-Ir0SU zYjm4u^0txC+M_&<;eQ&a@@LdP$W6bo5xR45D#wQ}_1xx(pQ1p~=cFd(q?#bU)UZlU z^+|sY0pl`pILy$+&KY?PQ?`su6nkHTz< z)-FKDA|a{3mojp=g(pZUK4L?vnZzoA+wtKyh;_nPip=HB{g-2;{~CC}8yL+NQxgoWrLM~(LNO1}2< zdycKV`&!u$++omH0>?%;SAOdNN>rViVvswO(Ry@amog?sJr;~VP2K1CM=;qDw_b@ zXanoxJCs~8#G)A#q_!cnPOtQUXD%wY@LuuwU7Fk{Kf6=_!drpNQG)VEX6pf;F{WLR zr)n`xDD7d5j858IRz3x1XsYR2wu-`epLNH73X)5LeRu5Rg9$^vdicsZ$IVi%aGT5NkVmNXKm^iQI=5Cp%ut z&K|Y&0ufrB{$&jVRVy8=uh&lbYN0QjOz64Yq6eB3QzU5?$$T})lpvah^UaE?@-L{< z+AqUyI;j;0_;GWj{=v=fM%8bR2M;h*x(BHI$bJCSY~Q}QhA|Rpz`0H3f2PdoBYHk| z;`EWRL#H1rZ>&7(E}u9ZzU?YM`n)|0xXN9AszZeBe6RFQBx?rH*ygvG$AY<$GvuGb zoZAOp@I9X$iL)87nrZkP58q#V&^-?Q`Ewr1Jx7n^#ZPKV7@S?H*0bvj<6iY#)(~^g z6`%3Fd#O22san!5jLUy~<-iLg&u>ne7lI{`Gwg$LtxNJ=UU}g!jGrL*br_t?VDjG1 z6%+~QZgP;R>|%#Q4a*5&ILUHJ=p|elMaJ0&-lwEC{3c=+#>@xTn;TiGOU8#+BL$y# zYNhgw)>3`dfNVrXzuC!`Od7I0weuuMhN-o}M);skA5bwC8FS?9>2rXt4L_3EGVE zt}@Yt_8Y6x>_SOLkM0kcz_iNsorJ5Ye!W z;y)cfQb9+2_-sv2PBH<~Rz5b1QdFZ~$c9?~!T=BJUVCpA$R8(+MSeZ8EX0^^dBP+fs-eJ_bIp{R{ulu%QNVsItsv74ZL6$E7@r20kF3Z zLaeof%dS}mU4>lCqD)~ycr&w*7nsy1}@qZKbISI-V=J zaF2sZ+#*u+&z56kRm@ca+?KXjvRsg&zjZ9W$XcS#^1n-7I=!Lvvl&xp3y!Fuc)*K@ z(n153{!CC>;>NHA2jo;rJBIfnKCGrt~58v{#8MC zp7x-hdw<0!DYo7Y+M}RDFGs#_<}2#5cIfKKp?lO{=*COy4gWXCD^XqqX0#E{;!|qe z0E){;716#X7J6qL&zgm)5TX9U7?@HTt@7mvH@QsU{zZkoWFY)TNH6Ok4HwhR`=jQT zn$gw--=nX~F_$;XE7=FkqGXtUfOR`7NV@(obO~t7o0T5brZsJrXNKNx4$gVVJb4EF z`-QmIkd2${xt42BVg)J+>EO;#{yKT7{cKWy6dT>*=1D;clQf!}6?c^PJH8>`TBG@3 zjk#J87cH!Ezn1icM&?3nqzW>lLqPJRB`wYD3oIP(3F zP+6LxCp=0+Labkt?L#yY{B9*M@CpxG!5gd&mts~0Lx^J21tLNB$y?`;G z)x-x_Yy3B$OIbBC9pt|PW0;#VmW@MyqTIl0k+>!KqFB?mBjxlT)3i`r%&X$BD!7{Z zUiiB8DG`2!N%q+NL{340PwvvIUFqGrke`@~vgtUP&avxEA`}vs==4+@;-`NCw1Ls0qqEJyW^;%*rLZy|NCA-RXi@J7G41RCoB0&;XGsCb3`CaMk zP9S4iCpdR@hM;K=w{7$>QMDB5%CwVgp)PQ{WZ+RXs%eLgshjb;r!97;!-<&&TzF83VfIJql zu=?WNcG?l-to$n-K?Qm7;cbG{P3q=4&B{9|#eOXo5b3>cIBo(PDCw_MJ|l}{6mXli zwbran4Q&dgFgSJW>AEy+e)MNGFDI!C1eu7Okc@{-C%#06z*b$lKdPh`diDpw#Bz$+VUWd=GuMLN?LW*-N;@QXP+GP)xn0(|R z##j?+%B)Tet(&C58mg=exFfsEsh}qEEHpepcE63@QfJUxjc4giY1yXGTUzwy7{$`t zyx&4^b`@iS%6<>>N(ETJA(c&)m#;%_-wNq1UG^CyrP8yMaiQk%siPTEhDhQ8$VVs! zKTR6luR;N%nt^aiMy=6Zr4jlKdDW)vTmZ)vnqj= zRA_gaV;$D`SNPRtCVLEHrkSkvF&t3uF}e9h(a+K5>%8K<$N*2P^|b@rOA71@!}=V& zGHUzS-%3(4f3@;rE1aUm-=N5fOrKIqZwP(obfc3=_5-3jX6K3!Pj{g}Y}G>|nBBZf z$!^HGh2B=pc?eXI{w_cG$L0heQR2%`APfoJACWD6!Jh?9rd*K_7- z#^fVxH|pbQ+o&huD{jfCdUGCiHqu}!r!Yoo4Vc#zUg9cmhP?Q-qbiP)dMAG+l1k4g zSc1|CYZxG3&(v>FQ;4Mi!t<%a3R2oifM0}K;N9zM)u;*g5G!jvu+=C@lmH;hO)|)q zs5uNItE^|u4!pk+47PKHvw+2LXGQfYulOGT)ku(0)4)fmR}q|2>tsRImR^dO%qLjr zEVAKYyl@Gz25&LnF^6%qwtBg54O)d|=T!s0f$^>!C=*M8`(}B78NV_hDBn4$@lNL> z@m>2%_xC^oZYT{S3@Rvnf9@ zlSIbf9{Ya9kazxiiZ_-YmHNBa`i~v`Hu6Qgza;8kU*oUU91`CxanTD8yRon2wQ+AAJmKR~+pw{)wH zZB=PIVwou;HZdQ&{9 za+>mW@!|hN)hHO9cB80KaBx6gr5Rweu6dp}IL{=Mx>AlSjTJLeDaif{cX&*Lv1Hq$h->^LD@};&E z%X1zAxVqfoQ9jmq4ZhQy1od@0OHDHh;=!a9zRy|VTQ_*VImwHGisY>;KBv*+<63>J zp$XX){I&C{10t<8=Tac80$fv~=rC+=SYgj@=#|0B4n5}p+W8Iqq!^O4+o|lvpDBwR zT#QQKeSMOf=Fx18@k9e1`YK55K)?b(Y?7beWTWU(r77b-SL4rUz*LXY<0QnEw&O2< zyCW#3EUAHbg^c%Cp>--9&4qXzv?DG$r7h3It+cCUbnD2ECxzSzqKbS+c23M#b&Nlh zif>5t22`Wo$E}b%bNnSGQXENl`cM>90ZAQgWz5OK%wWL_rq#PERhU^ika6j}{hYSQrCuk9LBKKxZH=tCw< zr#*IX=k)up?sVnfZ_9s7<%ypr=3^38)RNxm&{P(AiW1#>!g3h*mR-BEuum zYFBt|53e2JwI#f^>8fKWR7Xf3VRY%-CfKoYO?Tg4Fs=}2zcZG3h+We6_j<^5?es_E zLO77M`}CZZno!m{(}cCLVe5H1>Rfdz9wVdp{mLkQvrcQe`D?^YXd_%0s=Co5srcq& z$PKPHhu^FWzj1Fm?3)uuoIl{`{z{GRZKvMq1Fl8t>E=2F!9ab=q$o?J`JM4Q!)5TG z>#wW9WE0I>T(_r}I!v_C*EZx4gx;u=8Sh1r3kH=8IoU~7n^&;=Zvrn^BG!_*$*ay$ zq6g%63%P2xha(TrxVzH*6As3$ys`ukD!?0A$p8R7#^|v`5T*G_>~9~>HJ-iVG1}G` z_uFUQ4m|=&d3L&xH{GPflFNb&Q$SaXY2=@~a58h2@|<*QEv}gEXvpEMX``wh)j_@z zruk8im!vLG<>8vG)7}ewOPx8YLLGNdgdnK zU&gUReq?0-K#jS4902hoyV-QH!ho$#Kp619m)|Rm3x0QwmZexT8Vg578Uhzd0I^#$ zbAH~xh^Wb<_0Pq(SzpCwk7d}vTlSf-;OW6=?m&F|-%>~JiJ{A&AzK|J;plF|pD`#K z?uG7HfrL8xppss$8^CRE?wMjggJ4@)KnCuK?>nb{-<96rGT~4EzJu={DQ4)1HO+2n zQR1cpz|mRhla7NCG)_mOa~r%016GPaq$&{_XKr=Yok+k)WbK(B>$Tv(jds<*hp&Ks zWp5#x4=G)PHPpy(HIgCgwocjeE<8F*w*;2Hy>Kv$g|qpM>UyA|p5QF}s^oBv0ijt|#My zRfk`#KS^+%1O!Aai4)4+N!q+Yw0(o4?Tyg%MrgZ*>kZ)tlaf?#+Uy%iQdM?zf2AJx zoh0xLp(HI(x|914Bhvc*gTge^%UJX(WlHhlYS+=Azgljm&k3>*wwYfC?QC~DNg(Ej z5B!(MaMYQr6$+dVI&Rll5kb#cF|*J!3-e}GySci@T-{;T$+NnX3YssE-&x7~rF|*j zF!H^i%WzIEsMAhqHlDp1w=ZwH+j&$Yxx7(w8PLKh$RVpbTropPF;aGVlw?Nlm+u6o zbviLDv3Zh4G4QBwGb#HDgd-{zW=$27tdc~p2RzY&m7Ekx@w->gB&^W9N5)4zWo)gA z?|6W})?S2iVBgO8unpeg7tP0P<^&f?#lF%D0lckYc1v>)mL?^r)lzg(o*&44QZ}~% zbQam%g4WkIq;bFlbwNco+j)O5pbD8GQYz)Qu@T7m9idvn!|0jvxnrt)M*kPzk-K|R zK8JESWEe-B>T)Qb5zJ!HP&AK&03Xquxjbn5K-C$tS$oYw*(^!B{IQB`exFVmd7qQ5 z>!G*x&|MGL_tBFc&cOTMqJvN>>&pto7Snv$lYXD0{OMn^6CiiGSdZkjQhBqL$}NDm zEFhR_saD1&MH&6!ur@ufJf0?xc}+OBTNZC#-ohp>gpB4pDfW!f*O`5*=7o}0_Swpo z;?Fd4fU~^yP(Na zpGW3Pw}$lMxBbe=zi#d`VW;)YW_c5o)u2|N0TVOE>q1o(xCP9mfm`QX7Pys4C+j&p z5ung;*BtB&7>v$|N}OPIWMOwWGb6ktmqvm8d&~#_thtDg4qzPl62B*T?KkF!bH3k9 z!W=sv^doYk=8XN~O%wBh_M)q-J>Rj9K1Gd9Z9nRLN<0VGR1{!^#xfu5aQIMiyy(>V zp!8H1`0V$uN%+?(Z`f2XfED9Z)kqDhG8OQ1krSK zf29)ls!6X@nrkODNcu06xWnt%{EqmY-*TCZf{wqa29|ywUp0P>Fh`KIUJmK0fy-(Y z;`2CgTYFY2EK1`l*9ljh0^h?`)DEx!Oo!<(%={vxA)g&^GBG zJ&*1tLd`UYX=_Yl^=u?xYVZ;o$8A=lI{ev4xkliut7nxJ2V&!vs683w+2S|ERnn?8 zhE9MBa7W^O{(`)Jxp~fSs(&%QZ3Ddkyde3Wz`GU@JzDGi%;{Su)x^_7?*z&HmD#a! zlB}~$DhJ34vschS6Vl{^V55&kdf3t%nxk(gB@w64cBQXyGHm*{_@US;<`uPu*Xw_% z1%Kp@uhPvPBm4iG>(t28TCyG*?JeCe@7&ZBQLR14uk9K8 zobxZiDPo)1&DVI~Aw8&VptahQ>D)FPi*ZsuZN(U(F_-c)#|w*EC%d3CSDJfjyy6de zEq76!xGspB`;w&vZkrz8&~tosPdNoSoZ-)N^pF7uEgHU%9_ln`@sW>-9_SP(Nn7xEsuLG+ z1VaRyrTb|yNB&lAT8csh%PE_!P^gH&q4}D%3eC^1G!Iyqs6J64fmnE`+$bH>xN^0^ zUL*jA3@X8rHz}_$Fu$f^;cn+5KS4s!oKi8U8ZYFu2QYWA_$=?`$QAcT!>&jvNStdD)f#5g!!B;5z9A`p`E3j`a3sba zstjdaIFx@X&SpM$p?+pIDS&Y+u|dCAQuFo{QjUQsEyvW`K>knO0l)QW6)ZJ z5C1xn&!zaN(O;lZ)$&Fiqbi?5Qu4h+``Q)UYT;n$v_oY=7U@Is)8VUC=HjjiHpj*3 z_H3jSmKiP)F}@vk0GTBE(VSt*!De2Q8VG8dWcSeiL~Qz3g!}*msZ#!&@xo2`Ol!2- z7#ny(&DwLEEVFm$1k8TUQ=x39GN5w+=7brd0cRm+C79|8RXg^K(sn29ZlwK!Teq6R z>{QeK_&jmX*zVANAmln~j}ZcU+9+vNlEL{w!mhiW!C$~q$ByQzaHzRWS;{S}Y^`*P zl@w|CJ?DAF!@Sr2Hgc6a0YGFq3~rL|0OTf<)MTA(5O9W(U~r@YgEbs>3JB)JbKTJssMDMuLq#O&fKVz92`y(=w8_U5P1s zDoi;kHK-!ny?V;QpFGH{TeF-h!&63iz>1rMPS3<_lX6stUnTSi``%|ibNXeSOMktT zyX%jiB?049y#B|mg4U0MCKuB6yzeXBZyAI@qeg5D*h7e@_}{@Rrx$q2rky)7G{YPL z0X4?c_Rp&ae$v@@7@5greannWcjUQj!&V+0x|m-z6rWb#W#itxe0$}sk~42h&n z+jM`vXPA{z!07{ebda6kpHQ9(iThOR<$p>CBX6RMz?Uf}Z1Rd&(aU=T8L2`RLVfs}Nq_(rCinu>sIA2?pVRiD$(AbYNVF`fkVjN!Bj*We$8a*K*G z9AGsmr>KdUsL(z|PGu`{N*kf`wAM1&OZ6FKNrlOqOe5ppVlq(@E1LXe@OiT)!~5U+3e=PW7Eaf!24#9 zKQP*A4bY%%=&h!;ajn0paow$ll#b49YV13k={20|S~H`qb{;6*U!z5*33{R7(N>G; zzPD*MUKt~MiJL`DYo5ltSDP2(cR$kiUfzc8uO-nS5x1~hXOir?*gJ;gCSdF9en0HVV`HH{uVsvMGbe`R{Haais?i$@C z<%%TTCE+gCq8*)=c6UzcoM#;gqw}12K@%l$;Qi%58XXIx}-yH!h1Uyum%SjgQ@+@Vo~v_6|LF zG3+5d;Sj}<{aHN^6An9+B0Q<*A77_@-<)F*EW&GZJh1hl9l?0%$`O`mMS-?(##!DT zlq@U+-pB3ML!`rBD%;RZ9KUlb#fuyuq)@8M*}~tdT8saLoUtfHEEExDk~=NJ16Pa3 z!QV#;Nc~D7Kss!O1y;W3d6#8V2=xwXT}1-FlPDl(XjSItqGqA9 zfvk)E+8qso;{;_}n8w=T!(Wvc&~zbfn6?oZ9YNbyU!g`F2c%-uOkFEFSgj4d>0K7| zr+Zi6NaFeMx?YDk;y7EXChiVn}Iq0p*uI;hjETpDC>JUCo=k!OIMhN76Hr zU)kw5S9}9ZmjUBjYKjytTHt$%4ygr}=EB)t@#viW;jydK*$70$M7_Cc;K;~BqgAT< zJ`vnNa%6u&8xhEJn>P7b3`NSmFxjtk&k z-8V)5b@EEvZa4B-p>H3}O=#YV8^RS`^K~*ARJmz7jB(q~A-%tvb1e7rpf+8^%*&Pe z$H9x#STyh}^Kx!%P}%WgOdzIQf7ED+HW- zx%ezqBn4fNy;ES@_rIYE`+ltWEE~p@-6t0ASMx6$Z`)gAWz4T=Ibj~eqSx-Kx;f+Z z--~biI-fS6+14L6?2Z2dce(Ekz7I&A9c&P!Kr=mG`uV{;()~p~{O;0AGYfNcqHJ!A z^Q2k9k}pD3H8`=N?mT$Y^ci!p+^S=vwgm-M#b~84*A$g63i}8TDEwwWX zH;>MkN!4J(^Sx>k`WDy$KEjhKNAcB#?$IhHmMX2q&1wFSN7fN^h9LQ6lSrQ-kJU4g zabl@IOD^ct(hAe3#5EO;`BC^8wxMJ6Dg3E{nF7BMC7J%);ExD?G1zZ~-xK`ae;fXD z^)cY z4WF+J2*`{E*L&q&$n^|)Bf@Q9Mm{h;!@P9ZW)R~%?b7&J40mm?^p&B7%!*(z_a5_d ze8=xGG(;`)4b5;^(<`N?y_H}5doZIUpD zr}1O%)TX3AlP+K29@B9iy&+dU^lRI{+;bo1eRCv!+g}LHWiR*Kjn7SiKXvc zC(TFMBLm1RVkXASg=2rmV3^Z_P(75!YB3;>r7KI9dhR zw5a$J4=e>E$W+df{PGjpbg6RK{9{L^wI96SBm3XC&5z1DX~#Tb9=5r}%yZWMtnlyu zPe1>kqaTJ8+5Z0s{b*8eME=BgNH9W@=)woA{7J-jXy2#g4`o_9PrS?>q}^6J6gAI5 zvG81$L+8J3B~dey=uz{7(zl|yU*GU7`e3%8kclk(&504!B=6ImnQ6#o)@!#7$fFra4lswJB$d&qi}|Q0KKC7aO_=EHs)ER<3D@mKbp|9!j(w&XqZVWU$I#**-)|E81jvTMAci~Y~>jk;?WfORz|+fuabaMu4tgkl@V|g`d4nO z@J=*gmMwthIIo2Ye}=!z^WVk3BcvT;Cb9Z1!!9HWjRnD)sCh;`5d!Le32x?(eX-pEs6FjRcph@ZI9XqNhkcAkOU`rVvGMSEQ%gG_Jir;7qGZICR7#9 zGYIOWlw_;YvNwFiofL*nf|{V^it74T@rp%DsRsR%;H$KO%451ch(#a_{%z zJ1!9(A1&8%VU!)wFN&6*0k6~M(+Ym+ZiW7TX{U7P|1dCTOHW$*?}D&bK>y&SF?VtN zHU&V$hY#`Bh+9+)?M879V!o_TH)qFh+hj}q34eb#y-MWxEKkz9|@ zm#09#6j!FJ64>dg6RUMyN(2HaruoHy5`Y-&AuCtKn)S~u-LJsikyAXvSyd}@gsYfe z*Hn0QE=3$R;eCxBNG-b~{R+R9)7Exg`4MMkX`5wAD^YJ#Y|7h?o+V$Ti%?0qm8jG) zoHDFLy$!{d8{a4@?ACXD>}cyE#&d7fB8yz zI+D&o4Xz2guDOeR5N`w=T~E<6VI_T4oSG;w_wjtGf8nwf;TN3w$`k89%9id==@aGf z)>WCGB&b8@ce3KJRRp3Pqrs|3;oMvA*RWY}%}8wJw@+rCp*J%OdsOp-Vk?^;NBMYI zxKRn4mgh|rQA9cNtMY#2q0M*mRArRYXJY=TydV1ny?5}?&+=MG);d2Q$60!u&_xiC zuo<7+Q`QUhwEQv@H}ykX>)+nrvI9)GtM9T?MD<8WKi0Qi~eHufPl&G*Tl^`#0D5H#7}hI_H=vSNuDSya25B zF7`S0Y!CV0Assf58V>v(_$k}<`*<)Te%nqiL=5k>_oYL1I@8{k9rffe+{kO^<-T?; zq@pTCv$oIWOL(GRk?$OSxf0bAJ9>hq^^|DpBfkV)a`v{ZSkADg_`k;DMi|^{O9lDQ zt~w5z^MCY6`H2_p2VEc`d|&^>1aXBxS8w_A!W+a_<#~{U$_Ymc1h_-mE_=nQ7HR0w z(hV~L>l3}h-a1g(5)Sr~I4Jj~Pr_G$J8OAm#fwoh_SZ@ts_!T20gt*D>DlMYgYD@f ze5+7d`@(iEtT`+cAu&eSt`ak;Sf{+8-LLmis{DH}P=ITC6;9t=Y~(LuQZOjxL_LB*}$6k^^c!> z-7o#TC;W*7fbBXEd5b_r2Pv{7hvWm!&j}Mr1}*_t5Ucfb!K#QkpGhIbf+=We@r#Wl zbZ?C8OWsa}I0XrJ=^6MYXwbdr?Q>jLGN|9KJ+2MLLYim#7eb$P&bL*mNzGdAUaK7}}o7d@1{g_-rbW>DlPxVe}dp zY=vonbNjT0x1yU)d+NO(Bb4Gj9+kB5tQ(r?iVwd}GczF#ob2bf<@*@cZVE)vgh)4< zxA^YQZ{$&ojr+t}n}hX_gi*FExR@6w#hCWNb->U`C>i}|kKes3wwqE^DOOBs@qwkT>Rqv| zJdNMY5{tHkVXuO(&tRP3Pf-D{jrThe}LanBRe%708<|2%+K6c zIghW0eBSmPlFpmH40YR)aY>sEy0F_!1yB7Ot(mK0@XvmJ+2FyHq<{iN5qWy;MBc6U zi)yW;vSJI?&=-$?Y!`oz->JW2hro!XjP3c|+zTZlFoaO`_WU-U_=!eye))^^*62(} zS%Uc*%0vwb8lf3x$nsgdt<|LIydeAuUlIloa_vL_0_zzxw0#xKMM?4K~z2 zOs1UY99pQf$Ni%<}96RlWpR<1`HmIl{H3fF_xz7yge7Mz0Q zmY}0GOk^K)+{f8EWFD7E+k#I$P zK`#s9RZxtZHO&Gd51m;=YeD9{wm~yijo_a=(9E{CBw=0*dS37%ztM44U7SE;Ay}J^ zJjmGRgFad&kN(PxxjsqzW1oH+UkKWLPZDxN$J;UYtmiP z?oy}Ame{SY%E*Y{*23S>YWh}HYK|1<3-rXhE5};4rmtBODg`9{LI^cN`!@WLxwB%9 z+d$Q6{9u7G-x@mWUj&uATV6AC)_uA)t%;z2MkI)R*k9NfbiHv#!@_Ltd+{Ax1p)_j zh1hIOUempz@|#}8c+KG>jA-tcgS8|Sm?A$+4ckIuxnCBax5pV~{Y5FK=!f(9yom)t zj)eauI&I*6`ScT}HDxx{qK!FgVNq)AzeFY%V048e+^TVZC->oEW2@kx3svP!(o`nw z0hj8c{B@Ye`g6kgAP^dkM&YfU%x=qwwYViZp<~f(n59JX<%{N}(?_e9*9132%py#H zN-S6Xx0MFeEN5$k`Cd4m%ym&Ha1*&de;H)&Gn-Ol_xP=;u~WAFmCLoCJwf}BMopp7 zUlcv?lWIE0zJR5UQ;2?B&gzV1x6#(9kc)D3?OK};l)d<@;@11E3D(c*TT{zW6c?qI z2FsI9DH;3jbbl8!fAu2m=Md?snI~t2RbNBXLE_48ww;WRzjthhBNOp2LLgDfMG6~n z5e91`#2MlryeaKpS7%NQwM?%+XdO2Pl!G}NIs9snIL)`o+&P061>TSSt33Zrrrk3& zBhRzT!}R=Rl{vNL;UN2C6(WZYOEPAN=iiX{oU?z;!ST6D#!>5kh^6mkLE^JuDv3<1 zMq#K%&d-aEx=5cwwV}RQluhQA)(##pOKZpWO~UW^FU3kZ4JiPPY#(jV1_CYRuc-?X zzJJ;146%tn-)kS)?++?<3kODdLw_D5M)CdTzH2h+!fOAVT0m0zoc8B=Y#f^Jk#@mfp`CpmskE}6Fi&jxK5il2Cl%-yrMomyWym+4 zz0;Al`h14`2zuTf$sHV=IB!}_or4w;9hx7!KazW3@Vwkr(V;h(v5NlE|6?!kPW!PR z8$5^nM?92?n(P<(kupyJThQ|sYApU!`9pMgXRs*39_-*x)p>x*&0|5(x<9`oNZWqw z`LRbP`@*`MQi_j#Wx~G|^+9AZL!|<17<9-fF7&x8*gUdwP4V5%z3H4Bqs5+eJw=Cj0TR zFHHM>dFL`Gfq5;mu0v642hVLZ*br5R*zCa^4{MjT~d-R{~+n|1wJ#$d&dqqdXpiS_Ls8d`982m#EJC5;3^S%8i%C0 z%8L*Glt&N++2hmx{5mqsje57MFccT_*NBk3H0!{R#drAnnBL6f6y$%^97eFmGa5iq z#o#s5@GpXtRS_Nf)8P6@?gz+&cFBWhD)`HQmD6zXX)_^_Vri571h`5|3X^;oQv9L$ zL2++DS+*1NChF(Yl1;1!fek5~Hf&XCRaZuGPY(XTio3q((7%}J)?4D09@gqb*k5@vFzaJo312cu=uu!vck`+VYQ&Y^RRnf1|%+ ze=}`7-1{$0dH>JU8?C4=?)Ch_@ihh4t+2jfTc7^=^F0@P?)U{PW&DMCe|BZN_A!-d zA(KX<@z_kh>@%mg*zJ*@#%bZjH;rCu0v+2-WT9VN>G8Vv6U(@eA=eiJ$TxQH6W@0im&6tr=lryq!Kr+A3`} z3-Wd=SkQIFjJ^~U;8dcZJ!%#;1s!K|>EOiKpyMQ`hIwXDXV5h>iq;<`&yj+!7qyw? z?T+#x{7Ly$da4up>!TZeaw}wx?qJhAOe3RT9;9|Syve_T9aK&J(!Aei zH?<%p8+k5$ZNm)P9T=R1j=-b)Cz3g()8@zKoe(v1!;} z*(S9W)7V*}RH%d)9T0|ABs#-ux32UGXzj)ge*rrPe!?>-B}F-_U;>&Aa8-UtX(~y) zvq*J8m-X>R75GU2qWN*8<7-vbMfzt$fUl(J#Aabj2BHo5FNk2(N6m#1R$~56tr3){ zjZcxwX`uL4I~=uqumxdT@%4%qz##l3g2lr-g``a3kE!$ zZ2|AfhS<(`I;}FH!ziO3QN``??F#z_fc$}HE~(~1UA>22RjuYjjb<)0mEFX7kRnZf z@f}_+|BivZV}^XbGTbsXJWoe57neJ!W*WacdDKOd{nW|vyZ!!L?CNX(nSJn~?VV<|qnSR>n*AA%8G*r=|k3~Uz*`(d5m5u9!`S~{8lcf@o+J+7# z(ZG2W+Fl`*Kz07yPL^(HD%_AqOO{GRwS#S|pb~8QiFv!g>x@57v9#D&B8+x$o+oEp zk#iQ_xO3`1pv(B|6w`iHdvqR-K=zHXH<1_m$yi)HaCDB4-JUxQJ++D6;z3jr7>k+C zYU0;EeoJ922B6?f@H?*?`Lxc@D9AGH$1XORi+GUtSt01U*voy7;w|yvOH>_AS`S=& zSc~AiI>7GrHgqrCT6D%HiwvD=75?R$CiZ7Z?-Qo$M34pTdBU3}osBb)NrZ?CM3lJ1 zD?S3OX+JxgSE+`=TdNE|3IA25DZ3kpwGCd0|B?n+2dn0m9`FEByVqh1@zX$q6>dsxyK0flwF4isQF1`;V8bjjv5U&Tzw4%*~JT4IJwE>gS^ldvHVKW-t>ipiFl4%pOV`OU*8^G zI2gk@@$ua00eslw_tlWet>XL+3%b{p!b=sD-UR?Xpsv&JYxCE(_%|wxj^w{~4zw{B zbWCIrsmT2Hwd>?SR7(?Ln@v9oXVyl&KW z=xVy%La#fg$t%VqJgG<2(Z$xwJpP>-=&lXt!}JJmyHjkuEwPb6fQDRzAuDO~-y!h4 z(HtN~zW{k}hP?_jZJS~Q`_{vcb0?;x{&n4byTzdc9@4h3Aawaro?Zwh^oDB~r`LgR zF{*E%4gg%6&r*kAY`e^B@GgVIF6Iojt$Nvc63%Hd94N3+zp^HF?|92ecu(hFur#58 zprQ4FONqiTOn?jc%FJfx!BK!jzm^iNR0WDt>!g`?gF7t~r7es&wkPG&3byM0r9PsC zwj9Z9hp(d!8eTH+vxV?^(kqQehVGU2!F-^IT?o+aI$RC`Ns(DetyZ*$xwo~+Mw=UY z*|vgeG=01-qcCw@Uz0zSyumx8T4 z!fny0cQ*oo^S4NrkH0Tpa*0xMZQ8Pt8cM3u?o0+^*6MN>uvEfPm4!}Bs zWW=v-ZY5-71l^C5W|{LDKpZ*Ss5r%Kci@$vP^J}shGr`5l+0HbwB41}6z!vrnQDYH zZFKWxY%WjgB=Hw{#qZKY+P~&xtLg!T8FS4^@^Y4ymEFbXh55~oFOBeb8w3n7_&94;(J$PoVFj(i|VDD{pQkZYni&j}fW=hcB z@J0dT$AhF}e+v(bAHd_cy_F5s%)0xB-Ws&++9>Wv$Q$j$>DJFW)A2V|;{Q>A+s6Q| zXW&$4XTWX{K%4*%K~TvJ4Alq~oK=j{3idVqx6tDpovQ&nDVU9dZ`ypuS#lEzw6nPc zDe|v${D1Yp&*r(w{-wy!UhzN6xyowMxDtR0nwAP~PNUAeQ`vNuZVFdO7w0UZ?Z`iL zDTV9&$JL=sjb`MWM?kfItDeTS(mMdZR?y2nFrcGKdZI(IeZ)|#nuQMDnVBN^4 zK3{qO6~ax4Fk_guFkIVOdSJ}iAAJ8g8Z$?c2b-IKJO9eau~VNf#Qw~`;Lps3r9(4H zqci3l!&%~e{Jd>(;w)T6{?$^XI)a|Xgj`G|OERu9iA|C&S+w?8v9Lk`Ba@P2iWGof z;fP88{yaPfBm|4WAvl3Q%1t}NlL)f+Fk<5GYVkL^0Yd?QOONqpav`%d)0V6Om6Jx7 zgmR_PeHUJ@^*5bdKs{1-SXqF-zg*5u;qM-X6#OVWgf7?%%6s@=k9*+aLd%~~WnDoQzmEu^p*FE533hB%*C+g&bxf{qD0Gj$U)ki}l=nCGunfLmAGrty7PR;2 z&Yj*`VKJ_jnORAf;<5t3?&m5I`#r7-_W4h)j>Hs)^pr}OL%}fnYLUuJN87npw4s!i z@gG+O!7i*ESApU`Tm=aBJ?Z~Y4S=l2;biUt8K9>->|2e!4H|KZjq5Z-vfaL&D;^Yp zDiTUEx2SIkQq}8hTD=WTUKcV!1$%7;D``e=L*e0KD`?Upzul{jR<}owvh$B3OF8iO zB`SO@me!fS`l$%IUFjG2b>xi+{vO?9Z;suO-s!dod@>1-)J(d(g-Vef{LkCQ-H(fW;7~Qq;b)5FhGympBe{M^N5NQce z;O*4NMHB}~i@@+Z*bvFx{9aLk)z&S+zBm2vJjadR54`qciPSyzDX7umUY7DN@$;Y6 zNyBy|D<_5A$Gnt%v0%WUvZ}+4gQ2Gtb-T?o^W-@Ej6;A zfzds|mz4Nti||DrL!yVJm+&W_cmqFPxftDQ`3HtF~Mm-~L{G1}PtS@)EWJ7dajk$ya34BC0&nd$W!xO*Aluw&MIJLrhfX z-+|TIKXd`5H_tT><2?!T=jo`@7b{=RH)SpNCF@H+d$s;J?H07$=FTvqR`V@IGvP+sB<{pPyF$>Zm`Qww4}?kRN`ndJ zH~Yy(uwnrvoQjeX?C4gJ9-I*_%*$|S>mCaV5QV+HgHp8epsLL@vagsto$R+5cT^1> zJ8@=M^s6%+C32_=_@8V**c6fAOyfrs>lsGgMiEsZWg#d=-_ocV;EVs|S+VSt` zNYr7g+Gpzki;_K5a}Spw_UXX;2fCGpkyA8dONNm>nJ=4XG{EkFa;Dv#`T`s;ReoPzy10;0@F-Xsfdzc(TeO3{y14ned9gS6=X|qYk6sR9 z+oV6%2c8=s*CRi1cIhRCN9WtnNz2}uvq6IDiDW+Y+JOp3&c#4sfHk-MkypG63Q7~e z#b0csu>E+DlL*DMran=&j;SPKo?ko=ytHUXpetY$?xmO7CNG~z1{~IDy~k>7?+6bI zv{i+0I|jnQx2HApCL1{iyjcdiDcaCXzeAR+^R(0~`;Rd>pRF<>Q+l7~XqwPTGutFT z9Ug?e-QrmBY0V)Q!PY6S`4(?*F|`z=N^f^KCH^l+{_}Ku{9Hy&lwZn!9JC763oFp? zvNcQ1EMDdne^5bfh)`^PCaH`kX5D_4#KJUk1_+i_LYls9CGl0zi&YoHqo^H52`=7B zo8#t|MzaY&=$~=qE#OMR<{pU$tGFF!(>6d#HE`O?Q#>sHF@HoK&PoUp5Av0z25$~h zqcP&0W0Ql2G#TWd)M@bZD>T#KUCw%7`C)+-4lS;Z5WA4}cFV#hdkJqf{wA{nIv<^3 z834DJe+7dt1z1fs`50+>L)nJXhvaZqW81;WqHB-#6oIp=T^XA zJTw<)d&O@~veRxJ2g$Qb&1`u|l4ocY-B(d)v)hk}r$n3$u?v>U%nMJUf*-c=wDiD? z)5lE%(_Hx{M8{2z0<&=!{GdR6HyRi5Z49}-T{YWa&1(0m-aVsmTZ5j*jJm#C#k`BO zNB$hdZ%cgS-B188?Dx2gs%w^UCkaHnKK-j`{Waw`s-V*$g2b}`(jAVE7I`Neyg~hP={6eXtgN)T0_`G9y;goz)|@-- zFKm}mw%6H@2bo7SS2f^{uSnHR(egReIek3oV_!VD>IDn>_XM7s&!Lj7y5n`ZjkhE= zcoP>hs-Sq08cp5K1#QHR8Dizz4cP{n6?!b|Wh>Vpwlzq!Pt#B|W)5c~uQJ>aZ2S`yYV{ZwVvRVVIz*<=!AnkEsa~zf+5Y%#zt7u= zy1&S0a|aHU&*!1_zrpWGT8$LW2e&Q!VzJg7C?DrQYpTrhfN6(L<@GA=N5-^g;v^tM z7@S=k+Q{R1UfEaqJ=XkFQ0^X9(s+RiQ(fN3D|@7NU-_*nPh)QZYKPpTK~HBsUFqltX`n9QtNlv%d4!t?8;)A9RXs9eh(J{^uejE14DUL z#ZV6GrL1NYe@Wh7)#PvPsaVL<&1N@GfYtSsl@qL2*U);A9jn39*o%U+QRF|lItxRa zDW2kPnR#HsUM5)#9jw?IDODh~Yh|#<8h)~UByAmQO|j;j5S}o11^5(m_x0^%XCNwj zOz&$9ZV}oPgL}JeHFUZ=dwESa;Qt53w7E?%g=-4VUdE;oE9)OrO-Ysbf7im@XQ5p+ z4TBi{zr(^6yMB&dD9h-7+rq^Tp}P#Wy{tOZ!gcZida@JK|Ios9b}LFv5_tX{$WRDn z`B_?(g{uQXt>Ae*3wPLBxZyI}xw3H2Te>RPWi8wZH=kWwIMc!nez%3|PK|WRBoK*) zNZ)}Jd9?oER3lfyXxnAf$3wA&{=szO%!*nPs1}CG)wZcDyRbPj2Nb-pm_oIWKvw<=lhi{3wFanp4_3k-31;Rb77mE*cPes#!P7JyJ28 ztr5Wy)jxl={yCWyGPy&;Ia{x%zkaRZJXY;iji`E<3WG#6aLLqJF|;ba2r_em*he%8<-#W>|5w=2p~{W{7P~XVE2fmj7_DcsA8QoKL(7ZI z+MtqBQprPbs9-mDn9DS)zMJIbjLID<5n9r$7W|*=y$gJl)z$y|OfqT;qZ1?|V$`&a zEtrS|V`U`OAmJhg3^&0V6(mv604bR$+7Lhz<>|vD_?9aAqVm4I*_Kw?Vyl%}Ky9eC ziq%%IYO%G}5w+M_uhg9HZ}0s~CVq*?Imcc@LH;}_XOH_;A$q&cK__7&YvI1%%Atr z$ilfp&kDr9HGf{B(SvEVYWeBJ2T!J-(me71tCQ&~5Tq?st({EU^GpB#e40k1=IZ~y zc|JY$qt2(_wu5_VT+OG`pa;ySH5*7jhR=U=K5h3?oc8leH=?{A5!7)L80mNqW@qHec!?7&;$SVIrOud7uxojS-&!lA|7G0y&;BPS)NgCr_@NW(*Rv+n>umpS$04cGUj*eV=`NX2n=y}c zf227teQ1Vp?tCrt>4tdf)DM|YH>6Lt^Xc~xd(EeNPo(W0WN<<%_Lq{vAyc)JvrG(N zB3(E>wPxKukjxl1y&!uc{S&n{H*hLl4d=|j>6FAT!|_+V;|T>BYp2l@EI*9M6I61h zvkzweY#cUNhfM}BS~1gzaQ>_ijH!2$Cte!>cK&Q)hV$pFbO|eE{x8j+sg{|<)qJ{E z@L!luzfCkf9H)urLFdz2u+V(E0eF!4w46V9J}uwOgjc*^CfPHeHV2jzB(w7*dFwTw zmT#~5Gzs*~r+X!TCvgv$PtPU({_|3 zT4U&)Fs$88ewt4!wB=y0`LvwvcJT5CYxy=eDnD$;_!ZK#>C9x-HIOKle`IAAY232U ze|A2d*)h(i0@a`xfNM+H$nHPfF>W&}I0w$BMSMmVnX@`}K0OO}yJLJW(@DBD<@iB6 z#`d*bd#%$=_Sm2arbqophx{gcLY>rv`XUm{o>05R^9{Bnc6yzvy*r#@qen39zL7zA z;_>)x?(N%zEYB^Dw;iIXXtH?mSpRQ&XtMe=Z^iTPbD2B&5%yfyt?8-9o~a|+aWzF{ zn*3Rf`|SY;ZN%Dz~6`K*@)GQaeU84tUQ=U>J)i-@BOEbI-lJ3e|SD= zdux`QXGGI_ytemlR=8r+@sa0~g)F@2OvnyOP75@%_5X95vjhLR%~|dbW_GFmiOt#n zd*_pdW`DNu-`bxoMbivJW{VNNBDX(V9DJnx+0qZ%pDl9xvqk^L{_MaH*qSAKkwl4%Mae4t%RA|DbqjLpN;jN zTh<)7KU--B0`%ajy{!G&$F+}{J*$+hs1Mnntvq0Vw&H{LXGiS6KU=Zi{%oZkYH8E) zfc;tRA(+|EzqCL5&Ie8>r`hq~zpy`h7`(DG$Wb3UgB)pRkRJnR25IN2>{_ZO-Fz}Q zJr#SpqOQ9HuVf##FZA?pk5%R%r8C+yoWZ0p&XX++swWu52Cg&!~I^l zV+d)M-pH$qw{2lwXb#c+NVCc$;oX54yXO;d)w#5@r1me-lG2AUduea&)Mh55oy&;U zavTfJEeyYM0W1D!kJ0J6{^Im(ycL+iq?ewiyfzzyk=nam#R&_F%6AaEc$zwo}la}F4OeQeUOt1Z* zt=nompvrFDo(0mo{n`0vOetM0BZ*9TA4I8+XAKRl(TWU`7S8)+^R3d-!XeRakbDhH>gL{HDqRou+C! z===!K&d?AB%_P+zW~A(aFyaK%=<(>6D%v8 znn}smHcK~hCayi}b2h4>R+Bpg$xJKF*u^d9jR`nHEOVOJPN2F7LFUfD zUZiA{4ij%OjuePRl=#XH(!xW1_h@_p5q!y zZM^f#3g{GY>2{Vq>zEFj`#>8^@RDP&r%1hGRB=%52?0;Ql_Z9|=kJWK+;k{=gv?Ik ztNx3xc-wXSM+#NM63$#>F?v?K^GrFif2u=vcIA+BZv&$`#j7LX{n(Op1=U{dDM7-o z_^zYZ9p5vT&#NOxpoVqXUMJnYN)`lCPB2}+aYXAGrZjIX>PUV0#87aS1fjMQFisc6pbN-bVG2{Q>AZn zk-BS4lo;XR`&9X$U;z7`rkb$dZ+C1_17hTo`SjOwA?!{Ndcjm|wOjum(zCgsdI`_F zmWwrRC%I5UwGPh+Ca1_DyJp-0G^?$3+LbHRE#IQ3ML`4sDH_Rgv5gT=ja1t}#krQ9 zVtUH4)KJ&BjRI>zkg^U-Ra}=Uy&aCpCQh;Ic9Dp|K}mRsnqdh<8oDjd{TPRh2L{6- zU|6IqR3V&qJ%e_FX%=@nh4ZdSl8g<)MvM-jucdI>pBuopI=7$Yk_Gx?`k;f4qH@X zvjPhg(UJ3d&bu1fY~dELPTaCaAUxjLtj#$FG1fLi1Lq;2t6|7ruqETh9Xc|x!AQor z{`g`3+{)yjZW&FmJ$HhOw((V8V0@c?mk}l^uMw5^XFlO}GO?@WiS+M5?0&q{Z3TNN zwA7&N3MA>$qQi|%TS~hOS9AVV2ko5Gf0rK}oEbI?tfQ?n zlDuRl?YM&SyLzi+9&F2ZYWPB@y#o{!M0DQug0QqP9SAXPA$HR^bcg0s%GR7(j&R;p zr{UxfRD2WeRTFwWSV_h4Gp6_MBN>YiV3tMjWdz_*2^(;-{G_RpVJx@Gh3S9Akgb_H z5g3nX<zbNPLj$cdd1lIVQ!4rd zeL-DG>>k8qJ>*0t=A|Dd^lDV{%y=rPxsTPFj7S#4yGHfH>CfZNNzpL|7?;Z>HCEIj zuP*<)hB~y~S@Dh{Ji3@;$R*t$#5NNFwvtlON?E9HVPJt#IxM)W;CA>Y8z(cPiGlIk!cpq4$*6Y4-y%|y z(ub4D!MwITR}VBVwu3=Zsv(m`34?m>DKP38U}iMRU5m-ox7_R)oPjIna%Nz4B70ED z08xhlsa&@^;m(;|Q3;R4mc?cWDO-lg%>03b=5qOXG^PRhiM)s>E`ENdt;@YOkt;==3wF%l*ewOqf!I!-Kv<-hqZH zI)eP@kcqlKlW&&?4-=zg{XICEuPR1>JS6JIe*GzJIa%qgLz$zU73uHCPEYA!#T?88 z&sAs5A#zL|vx*7dVQ!K53@i-$U+N!T%?wJv=)bJtH7V&8ISt~FlE!%37hx+}V_32Z zMkRYVo>Yb-Yfp=|+=rt^(xpMg5u`OZQ86Ii@pZ;%>AU#Xdwxx%xlq$cVP$2d${*;L z4xp28-2&H=gQ3t+3*GsIsVFHuI#scg7_E zCR}QQ^0g)9{m!47>Lu({=}Ri1HSlHaN$957v}Zk(e^HoA&X9*-r3^$3%;ofd$rz7& z1Jf`ESkb2&kAisnwNmtS4!Jz6L3a6*r>5N8F1$XPow{reznRFN(74)x=i#UC4!dMo>ex^IW#SHETbf?utBe zORrxctJ<&2mdAwiQcY2KA2sXYtyC0aT@?y&X%%?)1+epPF_{%;#kF zSj<}2TC<-R{j?SB=(G7%BGxmhVskMwn3o@1U4V^W<7ihMhcOBmj4aHAu$i>;5m-?U zCi5ymZE&$HgB7DMN|DI|jKN)zYl=tgI=Rx%1(+hZV7&UnX?lW6r##e4r$2h+VK7x7m0cQK+z?F43$E0dD;BhtYCKes8MK06 z_A|J6L~Ahy(?ELHcMy#SRqag9>>E6Lg9Qz#iCE*9oSI%Y#SU_OXV44M26l~YFlA0M zWP(#phO*3d!>?%B7HpP`s`e+>_@k!zlV|#)YW>Nx{84rOQ!?nQ>fa9M_xgubOW&xmdozd}~hNFW+RoHEr{k zZ#Lgs%=d%ldprO4llT4Y88f~y@>uiiAtMOQDkxGY!?><}EPo}&d?~f6xB$S7wKc^9 z^^sa*meMWPO#+V1NCu|Ik^`xA1SOiqsg&uT%Cfp>s`)^Z8XdAfNcntFY;=BgY zIXnkSU~)|kS*SF`EFJ4=dwtPlUqU5pfW*52lskj?%4 z=lmx|ot=26NEG4$kqwX?}!~%zZrSl^;)Y&hNQPMhjR0NRhS;GsEb_~rfk)C zFSg6u`SV=^QGUG7_3X|ER#SS$MJ7Z(6(B=>g{?y*c&T#JtMgd|1z~^AdmMdtMs^(& z`K#!7{aJV$3cQhjtIuMriD(J?dU(cXQMC2yvRApVShQpz*%hT0?F-X0;_Lpl&)_lKYooyhQU7!7?nh<*=7KyQo5-w8T^kE7i20w3t#~Nk zzb2+!TE3YXkF9vn+VHp8$5VPMP-%zx`-8{83=`>%^yCBeGhuZaGa*YTj9*wX129W}6A!1jzE8gLQ^_bNq za&_?$cK+QvGV+pms%28JLa<)SOJm zi2dfS)?;CEo}cb%FTAH7{gD|P|1H%NS@C|Pbzo{n)ZZ1>fAG0|kwMmEld^s zD4RZ>6+FiuzT#yJ%`g3H_`*-?r%*dlY~V9rkS))=pmb#_dRLGilyZ=THD7DO@DFsU zC}<;qRz+WhzN6V=v>qdHhFBhWciCBe7rq~7AD+YZC-c;7eHi-l{#>E|G*8d%H=YRn z(UvBf$xpf21T}vP6Ey8L4F6HnBR=}X=#o6?v+g{jOEUUKOD$O6rj$mV*thK!(DjF5 zem&(I_-~f)qvgk|S2EH+5O()!zac#sh?IY!zw_Y!AwMZtG#E9kWK_A;zeaufPsnIT zwEv3T@3V$X1rK>3@^o-PdE^QIg7V;;(p2;orv5{AptBDQKLp$NT$Fx#$mDYQ+sR`- zPt3>)>(sg_ZYS9E^#d=oa@505%{FBXwtVj8T;mE3wVf@CRfp1g*`fTj9_M@Jvz6$C z6%R&t(a5pJ{&Y|K@cY>Qf@&L~HHHzgAUR?R=jt)YCPQ)+&ZOjiC`$)nzg`?p2=zvY z=W92L;^St3QFRXrN~v zlE?6#2Jvf(3D-k^P&u$yEGUi|!}z>cgI&B&rp^wR4{ZAy-~4yE_r=}xpWql!hEeW4 zb?{#Dc#k^o?EIkTpBewT@e|`OGoni-RKIkAJTHUrvK^jmC~za+Qk?&e%D`lgXgar zUirJ0xMt5PXCm4OUE3qOc*|y6%WM9QTi$w4BQ`^izhlpfjv<-xg;K~K01jO8NeuuE z+jiybEWF(e&ir2{SqgE(z&$N5nqlAp@11<4@-F!&$CDq0zKH+J=8r%h?4!#&NBo12 z1&fc}-@bHoKm^6}a`c>Q-q4uausc@vb2zD%KU@VxUTO&l}roT+CNHO_0SUo^i5 zpVOFJl!eV(Q0GmpPfl+fy>RxTruuP%P(UnshXV0NZ>g(z+UsPXzP`~-J7tdKp z#s`UZ)rZAvoIgA1jb2nwPQ7JC{PHHwTQGXzRrQO`C?Z&W7J6C3?8~#zhWe|h$)3=* z#65UJQ!?XwNmczE(J%4Rm#y|Dy-Rs}_*~T+%DtpiJye;mpag^t6KilFS zZkazD)cv1mm^%s4s;{e>*CcV9w{XGe(MXCKz4syhQUf32-((tvSyJqkV{gQz4@0d4 zlR53FN7O^TF%SOFo83^Fjdf9vzRIq2OZLfU z+E$*C_~A2VUsDt2J+XfN!bR7dQDl?oo#B2MEx#$bsCiB@6xXTs6k&F9AyR+o;@Ndq z>=%B2^h4tleZib-_K(sFKk1~CEVV_mZIc)4E}K2Cp}wx@vW1I^PHY-lG^HN7mH-qr z&AZkp=b}rmNh0C=w!H6o-{x;Dy~lL^KF{9`{CVRfBZJ3Hnm(lH)S;&iE%&^kP1np% z&b}0wTx8+g@M}?h!_Z{?;-oj!Fn#EyO-jH%fTao`VBrR;*GbT=D+gNUp0RUABJlrToHKq6Mg!-fIo!`^IcE4V_oy9KiWx{nRfcu5v1j91O>+L}iFQ2=FalR!3qEAq?W~Y~TV#FJeRj@EUg7x}k zyGOm()q*m=+zhqWDYsV+$}i+%=zmBiu0C=z(i{47+BNH!3ui^*>Q2v_1tB@)r*iGM zu?Ig2d=}dj|Ly#b6MnPjeH^AJeD*K5S4}E^tQckea=n;0*x@>xdL;2r16L{Z*U$S) zx8!x%VG-{np!_A5t8*jXCmlW$xUq^m1u$Fw_24@WiFn62{{`}2;dwgZE&uDl*Hg$o z&?{V%%e`%$_X!tXr$?7XBVMh;HCf7^=6MYce+)Q`{8duLe->B?P7^oypMdKr%imo3 z3OMVto;LwjE4=QfFC)D#Qb6Iy3D1dm|LfwP1iob|;T?V^_(m!rhpJTg8u=G^-eTu} zq5NU%Z7#oSz$@?YytAEuC%DIs&~uJI8^Cv5=6UY|mEKPjp7M@#;kOH?lCN|0Jp&&8 zB-hV_*U%Ve0u}!);0~npQv8G;109vX8=e0d!Ye#a?*hqxJa`TL#8nQz0DRdz4wAa~ z^T2n^^*q&s!Y={$zK`qxh2IL?LZ$R|@$UjJqLH8C!fyp1_BGFY-QiDw@1}gE4u1x` zhVUmi{1xzxheo`u4&Mj91L1kd@%wQ2mXACkVkZBi!H0d_%D?C-mj8D>uinvfDtOJE zjMH5BO7M-}WV^`up9jAF+n#re!!HKkOZXqV@bl#laTmG#uLDwm%d-weK&`YLhw*TD1Xq}Dq8hu|B* zi(UE;g75wucaiCJCxbS-F)1`kr zcq5(FrSMt)!@yaP_dW*{UJ0DF5PgKs5Y9^<-umy+zFc~Xz<2-D^FECZkpK1Ijn`A( zF1;JUYrcrQy6|5GFA93}+r8j>QwMmoA*JO7syesvH2S8(qp&iJ|V zM4)r^E!2lA|FPh+)_LA<9R3;bBIJ!GWa#}YcqMq)-o`5a9n=Tusys8mm)+%g16=$% zg{SfFaPgbL^EY{(+PT870^ji?D3j`{5J!!)3307V;SO9~bTC zKL>mx{oCn|zn=%+n`iqK(bo>X<6{wtjG^}y@X9`sO#kp*@YUcUzkde45j+h49C#7R zGQ{6fcyQQd(mMpbvL5^thaUyL13av+kAv?8KhgQ03|0bdRNNrzvr_~4Tqej|AP5k2~QEqEn(*uL)oZv?M$;lH8y zS@;hXKOXT$Isf~?XMu;|w}WrY^4|fjPCDerF7RRC!(9A7C_Z?|@0Y=sfe&{6Z-Q?G z|CGbK!FOlHKa{aYK4bGr=N|{J0UzXWZ+?ApUenMyUemnKvs^LH{AVpIgjdaOFrQ{^ z;1W$#Cslb9Css|Jan7{SAYeC@S&bJx^ zo>jCAGOj*3G0RV~nenTtZ>Tp(+IZ6&SwraYJ!e5v{UQ-Gu6{xNqS+0-Gd*A-l7I)e zO(y=2P7%;(hcV|tM6?d+XMu((o(Ur(bv_i7_;Jr2Y1K7V0Dj{~@^XBLfYev8mgxMnSF zPi?*wjWv7Tg5J)(F{Nx%%BuA(ZVZO(mhZee9)@oFd`km zw@c1jP(O8E9lI7%Mn;p+K*iT_s@em>t3nTO9P{}Z>;v`k=+OsRboNq`-W(drmEKiN zjp}YL^JZTvCTFC+NlIk6*W?7xrKi%8(E+<3qJIDG>|kyp-v*pm-!yl4uYM@Yy{Av? z9lpn13C%D{bktQWxq@BROQ$-_DNX4)+m5WP97I%X{h}YI(&k9c;#2GEr7ts{Z1xz6 z>gQZJzJ9jEU|xn}^RiTiRRFB{t6Jtcee5)Is1OW?G6nlUa0%B4;cF=sBjfM(D! zFBH}uh0?TO;iCECLsQ*ClTB0O!mEWhEWBK6iwXWBEYdMG@^V){06~KdfxvE68>?Azv%E7&Jq`Uydv)-cNz5-vQ9B_$LVx|LYE3hm0!ztq!II&mcT9o5LdB$%3RaNs#oP5v)M| zke!^*BCmp^dn7U{c$wgt@K^9Gp0Gtmb1Hd=)WIo5;N*9OgZBuM-|q#P+$O5+vR>LE>#b1xUPof`t17b*XrtpbmxqM38t>h@*Iy3MxNA;(eOB zP`KMEi{K8y@mRnBs@)GrfK$iQfWqs9pF@2*{1)fG)8Ts^UIot-{z9PQ|6KT?%mv|L z&IH<*gIgW^RE2e415~_QgrCd0lW^kwNqC{>y(3snI`BGYBJv_Q2|Xlu9&<}U!p#LL zovVe{pm!YpWru&o;Xe=#&#RG%oXNCf!6}~i22lB4K;0>w2B6CElpy@OaFm6&0_Fcz z;rRbq@EGRoXIuBNK)JUH5^uGGJDmTs&i@7KNa?%`RQTm%fY9+Xpxhr3j{Dn!q|=8w z7OVowf0A(g*E;;W4&OS?hI`V%JnB~Aj|3|G>C~mjevmN{|LFm~o2v2|K;G2T9hr_8$!D0u`5}d|9xr5&kMCJ~mZq-h15~Qv#fCp-? zw+X`gL#A5)yNE0P968;V=UAZ1GhL9sE1dha!lC`~^KH22fC~4?89?Z50g4~(!g2q? zOzXbE!S@A;bK(U+^1BeI@b$t8|B!Qk*SR0^Io!$b6F`LEoa74GDVfzV$ocoO9WD!*%lBk#X(_>;m3KW>%{f3<_J z3qnJ3wvE>URJWt#kO-9X@gn?#FmuS)GlS04iRU!zT$RURk~K2O58ePZEy* z1rBG{Yw(84fTa5dQ03SsoOI{R1ya6a=HZ^lcn2uGFiMbiet{rmoF{k*dd0clEl3&f z7et3X@7!N^aK#n4Q>L{*<@dkBq3b&i|B=HVa`Vg6Pcm1y5&R>slak@Ti01z5q`Ase;76)ZteN68|MZbm6;# z#6PwLNc>wI{P+@Z;+F{$f2_kV79{>ILGs@tNc>nUkofO7xMV3f@qIz!-|FyhJGj}w z9fG9yB2apE)H2K0YM|skXE}c-AJbYLtmtTm!qd0Z11q{(OZEhzz;#>wGpU(u0xRYZj$>s$fe+sv{S(W^gsU%Y+(OY za3KAsAo=}N@L1Z7AocNk2j3PfVm`1A_;Jr$FGzlm3eJbV+khw1zX?*VZGyPJEQpRM zyxqe2g5(Q@Oz6wOX zP8Njj*w-u^BS?K-1{6Q877l;E=J4+chd=iTM-Odv{!cmoKRJBO*9k|y>m7`L!{Wy} zm;x&OF9|38+Z_HChyPiScC_$LAas7g!3~1&;WrLH>O0_PGOxW0IGlO_D&5D0pNl>g zT!h<3Am#kHgOv_WcChe!_$Sz36P(JpP;eCOP>^^xICw8m<^HK~=sy4ZK2PFMd96Z~>xtnn({T2sTJ9y$x zZ2F}@rGL7^$2fe#{kW6z5Y3CU_=h3?2hguD=UXuH$!Dc$y&Pn(E+H z4z6+V`ws3Eq&$Z`ZsD;G_9L(8m{VN42 z*9-@*6r^0&Ie3eM8y)O&@MS^D^|m18I`nx9rwLN7?+8|Mz~8|wzXzv0kEE^poR@)T zF}`;2nmyo@=lWNHl;<0Ql;?gy%JGbY?>czgYu0@hQ1bRY;gsVcLFj+R!M{4#=g-#t zEJ5f^2}0-dg3$TDdx6k*$XgblF8F!ID{ljjLLa>Ygsv+Dp{rF8y4E=ORYA$EAnEpf z7f8Adf~5O*LFnuE9uWFYbnxwcHr(vL1Ih2p?*kW8eqiKq$|d|#*1H6+VLmQch+nsb zMGlrZSS^^Oe-$KNy~D3|aJh3&30}>*m>}u>$iaO;=~tbxlU}(=kaBHuun%X*q(9CR zJO;TDL~mAcwkGFV*0F$Q{wn+njDH1N81F`bgPHFNF7dqiK;^SYIQ_&!f~50=gU@r8 zL*w7q9305m1obPQ1ZsQ}W$$14p9GZqaN+ceSC0jf-XO{<_W_hm@UT`3f4$topQ_)5 zZ>N0+h#XucK9S#g@q@H?IQS`*mv9}N&B$5G`VMC@)DM0|a1`q|Dlhrg z+^A-$UcuJA^m6Z(}kw}b43@e$VjX8($q~jjX zu&+L1mdC#7h^bcwLkjaGpvy$XM(UmB0|#p%QDm6gLP4K|tZp^R8#vd&Sq@exV7zk~ z|49)pdt6%Lt`x@E-jLe{W5yOUR>SbZ-dZJ~OTm;H0woyL!%p2@QDl&lf%cWo6(hP^ z4t2!Af8y3+p~#wM8y&>SB=HL_ZCCg^TW`cz-v&bHjFy*p7$OZ(Sp|uA-i%i&`epvh zXzf10ii+IsmU$qr$gNz@nWLLLZh6<_mLKN-Wda%2B%a{*bv5ZP!)vJ?L#KUWgm0~` zYGFQdgGs8p&G>VXtGc`DZZQDUp^DM^jN@|9hx=g_m5J?K;;!(w#XA})ux~kSxNLZQ zAlZFCL9rxz;;5xNsK{;aZZ3hpB6mea1bYL+2!^+H=pTu9q(6>>Gj4YTRI9*PC~NOt zoX55D3Lgtx7w`!6McJuO*P+hE=^?y5SDL~yka^-y^G z{7XssK*ze0@)LL^dcDG|D&7egSr%V)1PNu;1XN1ug1%f#KJ^||EXfa!NEq5hWGI&6 zQ_!apb2k;O&x*_gn57e|63G3TwzNjP+GL8HDmXMagF>Ig)ybK-(a9*kLZbQ=hx%C6 z4f;eLb2M>leP+-%WD7j`k;{{UzMzQtB^Uw2Q0iJC))Mx}{_$0>pYY+LcNf7^F)Mnm zCf1m$N)`4=732jc<(9xe6~ix$2gBRnTznC@k;+)ye>C^hNn8-+ZFk!3GLl+)7I_;f zj0M3ELaH>ng|9pA6%F$BHyP={PTN$LmO;1(O%>b_5?Y1TwLy|j zh;w=*VMF_+x3|h$1bat!GM?UH`(TkZN1!z0P58w-pQUl& zt>lSQ6GZQ&q#N<#ntIAU>LYvFV&D0YF%38!;;Vr*#`+E)Q>a*umSHPllTn)Ilc1W0 zEQc2Xr>Cp}uOY#gNP22&je##gqFinH|@QFuh=2E!+-grj@D;6`Tl!nQsXCU4) z_$==lLEEN9Ranb~7g992+bi?vgUujNL$)$2ZnDBWMC%l*64qIx{F_X%AzY>Es#+oD zORN;UtLl1#c2!+xpivkTJ*zS9KZwT2>fyrPtj#b$iK0gu96V6M-@>Qt%`(|5RgK;! zeGE~`HYm7G;?5w%YkxD|{@}3``e$&X5r0M!9wN#!%kU2VHOAXh@_$O)!oR@!8{u1H z+rPE7TZu;y9aIuMX2q5q>=jv2Z9UnEL8_`9UF7Q8FF1x<@w5s5*M45I54@Db__5XQ zt1CJX+Cr@w^a<4)c4!mZ;~l5y4Nb4 zeHCHkfcUCY3QRO*%1e%rtL6}tpAkw^!AR`RScB#9wl|pp`2|0qDYZYNPYl%(&crol z($!P~#u=-O_yx#(Pux39-0vxFn~mE*>7*ZnfukWvsSxoE00!{NZ{gj9=S7cczE*Jt zg?&Q7EI1-4jXSz-2iuZ<+b z(gP*X6$66Wqr6nZNZE59Na%C@!hVTAgQ!R8iuNPCEH;nuU-Q#T$6(k$Y6@-4Z>#B& z`^;3KvmS44#8DFYqzdlgOJz-9>HKC=8kcy>Pq*$4b(0}{N4#U2ZMOpvE<^q%)7MgF zF}0U`m_lU6p>_y=g8#RPG@2R_l6Z}B)j@7`s5Cl%tTb$gje+@8`Hi&tRP-mfDrpsl zno(I{p2^R)iLNLUN*vGZisq=b1%(kRQVjAjfA7WzYix~Ga4g-?jAPQqii1-u$>cU;Ox;zYBi2F$%+`6!@qdPRAuYlU$E~AQhyQywKG?t zZT2-?JI+j`{n(jq^yo*=hXr^xxY{`rRW8PWrJl9G1<#KBC^pD${t!d|T^_rE4ExwSYR5 z8Rfc?O-8a4ZC1eT2qeC$SwZY;Bqa)I6CBIntj>*6;;V+(Flrc7mROZ|A5T*eSbAm{ z(|JC_6_PQM8@nE6N#dL!Fovw~AB9C-(Z0AU4I^-cYU`%znJMPh!mwT4s|@4K;1g0- zDXU+=$2ELtD9RMljX|q!QzyRJNLng(b1r-Jxtg;<( z>G!fKV#MJPM5wo(;DsY;l7uyuAMbb?gzDHS&=2;m=Oe!A{1_;M5g1evZ~LJr(kV`~vuYlCmdX#b>fbi6Y3DMDXU46CvXgUa*Zpcw|K z9hUb!54GJ=Tpe$}4?dEuG$Impr?OmS1D1c2x^AS-D~PJ`s$afBgK(^I`_VFDH)4>R z@S<_V=#f_cEuHS9nf$9$<+r9PzG&v>1IR-y*-iY-^D6KeU63DdKOP#o%xcSMTBl2i z*%8(lor*ypNvBpQRVZpKk-=uyq~7vlKaRJ3w~y(oN8mdGdW~c~7fKd`<8SF51FMlC z#dyC=WiEsMc<0?vMzMuB-jerX-jl{866N<_59PN_4buOM)UsOwi1w$Z-d03AAj^#o z2uhD0PthPb(?;L>7||Yh>j@}$fv~xnElHz|EL<5+?NDqd9c7G4)`G`ZjUfudl{R*D zys$Dio19Vw%uQYXR>AfqK!d+GDDQ+x4MFU_@#=sgk0!bd6-nmG!G_0MMTyV(qdgYp|Rgx-{AWI|(5$WzDfBNNKN%7n(Be))ZxCrTzN z%FyZ!p-dpt;*#wr$yudMl@17E{mG~yccPcjO2S0OjHP5lr)Bh4A0n()ejBV7tG?Uw z`}9^&8SQi~(;cW;+|J7vgL6$BqgGtyKMV7hkyO01imWqD=FT8tm9rUi6r2fZs=eHT zkrT*NRQl6Co6(6@QnIG(TS-W{BwnH`T*gPd^N;)|s_9Rwv3pyML14^n>(Pt5@lw{N z2{LN1>q7oZxzWBzg3(ud#^)CHNj%`cMNJl|wwBJvOUy`>_i@ScNNS}B!#Yu_`ZiBC z|N1#%^_mmmRj+=mY~=Y8 zo_q|uH%&=s2t{E(zsZit59PJFjvtZ`(5^jvY! z$A4HdVb@6@M(dEFez(14SJhGGMJ#wG?sP?uCOt2R4PeF0aWC>%Zmj<~iN{btHPYk2 znek56l0}Zq@xIkTv!+c_YCUg*sBVLb2;$Q5pb~Xmht(nhLxxyRs^FN^wM>9qftQx0 z&W%|8YR5}K?aFskuyCl{`$LHWPT zEtP7Nz|;QJtq0NE)U-%ij+WnHLMaYL~> z*useabk-vCQoZY7>cjFP#VQpvX4wS$7wCg4%V38d!7;th1B^k-D zsZq&Ys$1@*&RsL1Da0=V)}&UMIoo8?eM#x2!nLh7Gal8;>FNfxR;F~d=4H}%cAMp| zmd9I_9Y&XLGKBhjwdml+7D;Tal}8ZYxg?^oMf;mHzj&KWVEeJ}>zT`Ua78ET83`GD zBW3|E#3F{scQLjpn2qrb8>4~Furw(1n?0gdGL93wd3B>SdhUu>quqI<8Ns_TM!e$( zY7kTe6>e%}PuLbZy0IbO-kr5x-db)AFw?U+xecmU@1yV%#j_KPPP(EUWTmZ!o|zuh z8Xf7;%IMt3SR@r{Aq90jQl5Pol&(2~~RX@oK@-!S#hfB7q1C}^q z1N_0PHrWZXU)o7<)uuG5exWwle{|oPqSzHqh$!?1k>ljB&2ALMHkuU_`S=erf_haC zN^Sa-QYEBBBRF17@eKty9ttE)K5Zo^xS9P6__S2xWyMd=CvtZCacgenUrbM;23A9d zh8gM8K+Jg2t=ZwfkAa)~@09-^G>ThiS3pJ%c?>40HNzyPOluY32YM^j3W#6&{d5(m z%w29`G^7?_mj-4KLXtiuWhby|?h>x)gcCeJWhY4rC*i~>yVd7*&2+_s!>G9NcxbAy z_-LpwJkrv&9Z9E0^A>9MQ$c7c27h+=mRopN`tQ6-`WE_-s9%Nbh=#WfX1uswr8rfO zmTE%aIh(aBx}yHkB(8R*v2Zsl`RT{rpsY4SN2rSYS?enDR2~VZG=rK<4J}zJSua{j z?)1{9pvtdGoKB110#u6^R5-<-abWp3*zz}Pt###*bUFkzO!)iOKWd%i6`}_P>OssX zhxPf~f}#Es=fXHqK&M@ybrL%85$U)8LUhX^8!@-=&h!=>RdZV9YBRNvs=agHRCJSc zOXfYJ^j)vpaCVIfDpX;qhCB96KOpEVf>L7B9J7Wjq9@+D`>);I(#_&WP`!4azgm+uvBw_t**&ZwW`KbCI)b4CDuN;cADRirn*OXPX}NWvRJqn#Q8FlRTh z&U%dk_M{xA%~7+GXrEQCbmJ?GSL{whdM4ilH8aBU8`H;FFVn^%@>*x=SAwCEY5T$o zkSz-WVMlk;{gRyXQ}zi*F?|cT1V$03IzrWw)Z2!pvWF4RG@zkOar{g6@Q~0i(YIvt zB%y-558gjsI+`Sv)QXCmyO$J^PN<07!s7AJtVDbkPbZ3TJ$t>a54r)2_E#FF&aG5l zRG^inONjUkF783p2V*e2ZpPeY3PdV)>qgJuDo|LE{Pa=K9dG}2+VHcAG6#`_T~mv9 z{1jIZB@yrZzJl060Qy!vWBLv`+4l>_(Igh9>*U~CPr244_7cqhYi{Au8alq;RaM-A zgO>Z(2=e1|ll{_&S0jR@$<(9>8L8oA^tByx1<Nl`2u`##D26{I1vhhY`1yA@;3eJMjIQ8~74y8JXMhz3PX!(#=6>egO5c zlGHWbiN7-@G$dZje4vH86OE~fkyPPLy=}zeRj!Pubr^b?GjZcu9{SwY43iLXMIwA}5b#zo?Hz0EVR;w}$^?l~dyyL<4Hl(amq zeN*d2J&cgEW4=D)o!WWI@Y^m#28B0-Z&SHxhO66(KK`?)k%FL*2Y60k&QbOzvyNR$ zOW>LswvIJq>wm_Zwn2@`EnlJim3LZSq4|n#YGg!E@Xh#Lzd=g1{LwPRE^;MU^+-xH zH(>49N{M4X(%;hlTZmU(i1^ks%p(&$Hm!MK&klWqBr_7*3-FvRPoA31CCzxA3HfOk zxu9klDIo^KNYO9+VtUn!=3K>UT;bPZ<&*aQWaL{Rp?el&$vd4UsM;88)^?m!jUWAD z`r;RGwmW#z>T)WPlWpL%?(`eKH`;&h7E-}kEpz!p+V6Ca=|Pp;Yy+npGfAfFFa{aj z=}Ouy9n4%Y0C6)>f2OGRH9ftCEjPPqN;)R0wphEonkNV?2i2){R2GB z{L{T@0W->jerNkfMU^y9;_2`PS(v`TKO9Fd-hqi4FE~!$1Rs$qzfGU1Xjzb#c-%ic z52S@R-+m4B^8jpryzM7BL)97f{^OP6#cDVLI1Ga{!m})PuEU;@yMfF^4wbm52$7& z-ROx@%VVo?FM`HWw~NkOi(RHd*ty>GGfT~$FH`7>+=4Po$)4+-bJ#H5cuM><+Y#8XA}aaQ?{Q%F`5f`U8z!%eJO=GVETUXh4?srj2DW5umc^OA$=v>@)B%!$WBj8AgY#lxd2as+{!zss-F*WC z|3^OX^NFQJt5Ynj1&1a$Uv(rxVSFkRJHDb2{u8aw2j&s^Q6p`h25DUt9ItEwc24)> zW?{c`6HoicWzo0L(Z}MMe|Qmzs$^O{FX-zCq-eSQLIOXwKY@ev4mIQ@XY5bfi@|Z? z!03@SJA<@}bq&QMx~fukv8#m_Mp(Y-j(3@3cj>jva`x*F5u1KCS4+^In|k*T5`$g= z(odnP$ob&EQ43}_T+>uK&#SAeUy#W$D*=vQ>h}-gPdaZ}(U?i+O&>SDXyTZO z=S{hw=$uJK)5eb}nsVL@MIU=k^_ZfAlWtr;F#hQCs;5t!^e@D(8Z&Lo=xJlBM99?X zXHPkA`m}Qf3Uhx>(T2*QTU zZ!v{fk7FNPIFw+@D;|nZ+35|0e_z5a{DtX-cnGuI{}VQA&88`di@y>6vvq8~L!6T^ zrd~o^!-$v*qwvLit308<>QP-)h-*JkdNssH@-6)r;%D()l!cGxyEqFU&$mP-49{og zcQ6~f=Q-bVNklgNR7dL(!aq?d!sldSTe`2@4^QrgF9ok5t+Qdg;`{sgd*Pw-Io`aX z!@R}?$zd4I29ymQE--xPsepxb7~^g8sa)MBNI5v@1nd5FpwihWoOJFNB%NP4 z_g_2rKREZsPulP+fC_)3AmP95-0yepk2v>P@HavQU*+I+4&LhEw;bH);5G+;i~t9el&VcOBGSN98ln!7+lAYdTPL&Ju*qX1Not&B0X;zU1KF9X#?=Hr(+Jj&g9i zgO@tE(7{dz|JT909Ng~UpB;>T+NRsb!6O|!*1-}7M>tsN;1~xlaqw~nKkwjT2U{KN zbnvSVe#^n{JE+%*#D_;5+~wf&4!-E%yAI~iP!&GrV1EaXad3u%7dg1t!Bz)dQ>qIu zXk_kZoc00tGs+Uf%GJ4lg!>s!I{ja6+V2P5&v*r2=4nY9I}bK7JJXUC)8c1v=gSKQ zn-dC)TgRJ~eM6 zN)YDf8vz`Z*=(lu91-^)H|z5`aGY}ZA)#wd)q zu+x0>Vn|S8c)h;IXKl~tYWLaNYMH;vtjv;0Bbh*;xJ&V839PdlBX+;Nt7^Ha^6>bx ztE;Z68(1FT4h^>`h=XL@IA>0z5g zFxb>o;^nY@w50SAMDW7mtm`s1ub}*}_BXjMv(9Y3aOsEX+;W0JijmZcqq#1Vkb@Nq zo{3*&8D7zkn=b`XiUT5=l60hLwLNh_;)Wf;@`YItwPEi6&< zL=DMTgAIn-ihedr2ZR-E8xEOhc}!B4oypSkZGEebbajTU?8Vx3Qd@(zYMd#?V1OSx z%-6zEPGl>z8-J@bf*~Q7V3D@&#TklRpt#4E*o_eQxn0h7qif_KP78k`yhHQyLFkHi z-bL`9J2aw?$YNfvJ2VZVEb9)9k=yJ$G%XNoB=oFMLfsu26)n6&QwzCuO3B=zQE;iJ z@D7b|bB9L3q(>@)p{OpHscT*C4o!B`va5~65~njN;6UcbJM|*7D!|EKK}1&zo+As# zbe`8v)pC($CyTf(p_bI87EznxWJB1lP=>6SA-IrOj$e;v^b0$<)WRB;))^GQE@S%~ zUPQudJJ-ggSW9bO2GzM0N2r}!h00={+PUMRF7eoQ-Wg^soqm!U2@%4q-Bx_`o8}Ua z@>KFc3wyTqFaUDel3Q`RJ7h(^FOV<%C?eD*lLHwo!!3y3Pq}yVr4tBi%wnfb7O3KN z>!l4iovLjC@|2^!1*szjl{nN8P!g|zGhM(M1!MzKx?LNPf>Tm<63!>EMwdC$crFO&UWb;3DfWb++3^)nO0?w!OD;P%&#gO^X>zMgO}qyleF2$bJ|Ajm1+ez!twrA*2Oc2p6< z*Et_s9>N)qq2jYqjQ+OPo3jdnG@A;uH~L6G$r5=4=^z3o@;W0yUD5C2rD7xvbk|@w zuu4!Bbc>w%byqV+TVt#GB#4$+B~FRAZ7L*5V(Tp170isN(=7xy_hs~0FR)B7ZMcNg#0>^70`ZRLSo}nMwt(mi?Rzv~sK{3$x@Y4C zj_dIzZB?edYNML5S8aC4%bkWjjFgP%Z89CMy=|kmVQ<@Pw?WNt;pq6Pvzb~Fa|^Ea zwvC#Jy=`N}*xk0-jeorDV+3}$ZT?)qr)yj5Y;<$sMs3JmxT(~ayKrNp-=#PO=EBVy z8{N?9w(iWGn=Rx+8H}7{$fk+YPjl^tsR{f}5-_qSK~J;J+zgDj{g{O;q;0bj zvO8}>o~4eI@Q`&zvmz$!EE=Y|3kn=~v}3+hvZR}eUq;~E*wK`46JP$LcE+*JxU0+I za<@Im0$8aKcBuD^K6l1Dr4%ScofuHD+}x3X0@DcXV3HH}H*Wsp#yKv|uNeKpInK?7g?{N_ByR*|m1zJRq76zp)cIL2^*MK) z#vaM?pI-WDJIkOjs;pG?PeR2ihJD6dF#4VUbnEZJL5e1eRmJ7ubTQsB!8YrAM44^C z@IXSNVZ1vzyvhorB|Pz0zwi{wVpL4*q@`mCFAk&%Zx2fRG#nTIHOSIzeT@vs=lovwd{M59nMtkPQVd;T&&&u&R$;3mY<&cd=uX>i5R_31`n*=-U5HtaQ;Ai= zuu=@m@m*_Ghw@AnoPv!3 zJ0NqFO+nWYF1K(fBB1&V(1N?R}G` zg_|^1Ol-$}vu@J7muE^clO#-6rb!vC1nL#Yf50#QvHF5dc9^l*@C3SyXH>Q?3|-~N zbVXTnCn+n(gfLp!_Hd~}-3d{^AeNuoSL;iD-+0@_WX@gv2EfWS(b8#dwyNfoAIYdIe=1VTh!5d9XR z)tC9$pJYiT_ARd0tg(xOrPoD+ciqH=XOm?>pfwat&XRstB5A-%ACFM&$fRh z^1fTNud$8Vl}2-wdro%*!qmCakbf=yNYv0nMsdffw^^K>d+2#_q%nYVu!bGOnb0u zJp0y73%72xa-!t4Hs$i-MoUkBBmHc~hWp>Q86UOxZRE+~l^W3g_iao+rJaTFz727z zi^qreZKQVtbKl0$YP(o><;Eh$v{TE{iLRxE^l_#I8tr`apQjIoi@iG2ZQ76S+tkE6 zSBe^Y%$*h|QMC7M40FtVn_2X-)d#(ABZ1Irmpk)J$_>QVeVaC*R=-++x>qBa=~?u$ z2Q}qh>)dCW@NH)9uKPA>S-Ni{m|6TH{b}Gmi(ghH>CClBKkL4YTAuFP+ycZ~gogKR zDo6tLH&FL&oN0(PX0YZi(3CNTbQfs!QXtxBy%MRy_<&geRVQR`-Bj}BB0Zx-?X8=w z`gA>4HP;y2#hY0SJjHh|-c8yzpN38CctfWe!zti^`EcB z+d3d1Y};;@K#%2S2@g5NMfpvZu6tc)H;u{z1~k^k|4Xa(7>oiPHWsB)EUpcs@RA6> z;77X0$>PSgrMtqhzDsM3>EpYiU(}>5JzPinH{0GkstX~FL@(j057J)4XR2yN9@eMq zDk%##7>@9VncG2Ekuyaw8G$+ni3t=)R2xD(PcE+_JX5H$mDp7?Y;TExay$lf9kOLe5VTFhUqYXq|rtX!>Dp z*zC7HfW9w?B0vS)(y_B^4Z*vkk@N)wCTYWrzxL;JjSKFE#+!QLB$chCcMp6WYr)K*3>rH*2(J9DkItcv~P+iMQ3qSEO%0& zl>G9WS*lK^ui)lSz?74cDMx==%G>k!>Y(MzQ9t27mHr7=e02Yto0DB*JOW6n!~*}& zAbJOiY9&q>QSc-F@6wmp3qXc)YeYnOlkY)EA44Gx>6i{#XGyQxH{9bfwA(WgenH2c#|fj>Yc3$o!5{t^%`#qLSD8t;&jqB9SudkhUV9lY zMg~OF1tis+8i*Q34az|i0&@XL#g^o&Md$*OT0*8}kbokpLD-gK6=`^HM;DNkL0~Q* zRg+`-Mb7Vs^&M}owYPOz6egD$$`%rjcZ|gqWTTMIgo4*y^A>753>ZZegFUd$()p%-_bv-c`NUTPHdZttPjE?ga)URyQLVB6qz6hO=< zDZK$o;&<)I-C#KEzqa+Y6C!_mT=$X0@t+XJLnVlE$9cKjqkr*E5xbwhCkGWTD7^0f zV((qxqpHsJ;hAIyQXH62gMtk@wxcCx&;){JNU#ACj2a;%;ZhqlO3+XvL>Ov3CrpA# zls@Ay8$Nm_i9%AfjN3i9Uxr`kWcPbmaVNds!M(}P_?vK%LbjK+M4qk>ke7o-hHSA1e_XQxXEO^VcjI;uyAX6T+o|9`0ly%na2iqy`i61VX>k3L5NB3lC^ zBy1~f8fjChM2fY*sa;~pe(~uTFI%u@(F`hZ4@@|KU(HYPoD-mlP{x;*9YIUbjmH_`6@mL@Y7?_vA$D{i@lGrOaS$ zuwHj**C4MxM6P+!xJH8#S_>C*8{6rwHCV6bNmn9^bm!i3%u2k8=@{(mP$ui?RNt{P zOYTlg#O*3LzgL?U!Izo+X%A7Vb;ZUph+Rc;XVyV)V`Bm5(VNJR}Mxeh)j8!DzU%GN&Ti z5LMWXM20v*n3*_n0SBz1`1t0@mk0emRT^%{NMW@pKyQV#QPurf4s7x-j64ls8f-^QvCir4u9m! z5*&PI-g@AOu{N>s88t$DyG@jLw zpN8ew)@bG?@AC6Dr7a)Qu_N;d1amX>>x|D~ze}4N8yp zd$>21mvBP*EF6$QUflhI!{6|D&RK>E`sen{1^;i|rNh%D$&YP|-{5|^79CC%%-P?t z>e>H*>^JR%4w`86WrTcJa3|J8nZ95QiU($_%xQjC<+jfa~5FZyNHC%*;w zv0qLt8`1*0wB$$q+4wc%#~o+7_vlVWb+St7xn7=NUlwUtvOMBxSt2*Yu!x5r_Ft2L z>?bc3{Y)YXuvW}}CFVOtpAbC{W6<6EV!rtLYZpY8Okw$@-R)jAvNqtv|>x3X6u{3geEsqdLr4t}ts9CP`Axz|zMlfUa_oNS`RpmaEjNUEJ2 zFAh~Ps@}4ps-aECStWH3Jo+C_A409_Xesn!l^bA;19AJiKL$OhwMOxLUvEYZZ0VwH ze(X(soxB%pUw-=DJ2T#E^eBTVbc{l1aN7SPIv?DOW~lZc!qBy zHPHv0z75PaOQCpb0PE5K)}%f6(Fn^+%ec+DISX;rh70fT_t-Fw9a0v)ZzB&8yRgq3 zUIxrDty+LWL;S*GW2AFt+ZWYB%-3P6yZNjitI#re7DRcqRI*Lqqn0Y3=_-9! z2e5+@&;{0Zx*KCO3z61_OIG1r7HSXW7{Q_1RwVtFfH?_MIV_)kkJ3U>AtT5Ws&1t@ zN+q!5o3Q4Zu+}OpHYc&sA=n1XH(5j|@YqaF5%?x!^BGVlr>)j-s%?WA_t+GZUu;eZ zK#gS=3l+j>%uy9oKwJCsjhb;sO}dS&?rU=A?17=2@JQpF#@AaB4R&R2GD3 zSUL)8EJK+Na#nSvbdaNB4VSTuQ@(1?Nqp}z>9aZIt0>1keg z+U0z1BSVQkWsM2|ao%{8Dy6RdT$Qu1hiAdunv~7Is5*!d(NYhF(|y}Tpo+0np(N@c z`5#5!Wc#+`I0f$B@$jbcidp-LVgIW)e8d^4%Q(6Z6|4e5!m5fjN%*_ZMvSm%LbII` zVu)7-Or3AlV8b!hI&q9{7up?s)>|dF!*v7=lr@Mg()IA=pe&rvIi!KA*sJk7voQKN z(^_J?bra7pr;uv+oCs;*I+TT0lj@m2+t=rK;ZZqYr;y?Ay3L0OSr(`hJM_3y?uk#y zgnv!v4)y_1^za;UjE_=1c21^8Ie;`?`9KUj6r_)S+Qu1H5l-TCjX^J;($XML*VM%h zx!gFzX7113V=Cbwx~Z9?Dj7SssvtSm9L~}jyqBrSRh_*m@~^%XqprVm6Uz-dak^nz zp+}G>NDtX$jb#AdcZ%!oJcia8^mqM+!Pf;FZQ~7$t2cxF6JQ;u^6f~&RXc`aWa+S2eKY)wUR;9_UtDq<5zn$OT<(?PJ&12 zAuX$Gw;mMO%SOvIeWM1^tKLl`AzM{W)9t7i2UZjzM_dmNv9Ro2ZRq*deAk7^73jOxEC~9jGbB~;qA7RrX(z*g@Ta#C_X9 zAS^yturM|pYd2DFjr2Z^=rggO zIU1pJyRohqyOB-7d@X*{p6=ZkphGyaDDos9n)K6f-?LE5XX(vgIQ;xQ`|;PB6yRT; zY0G}l992S%s}yzxw$j^0#eI`8I(<`ml4+2`vD$vv9xyK9D) znG-%BB&AXf8qV=|PX-l8FQXcW8h2PWx?cw&JCbL^(6gb8gF^X^@s@>S*=50TeY=n=Rl#=l+i_nkex7rN>P2f+ADiV~)!Z&qDlF*75-O00Z-ykS1B| z2P0JxD|Cm2F1T*HQ3{_;;kQlSTV~x`hLSZDeZFHyiRO}z@K%CcR0$yMqbs^TP_;PF zYCypufA=4lX;t?KywT3tqni@S(o{=>iU=7FnBz-PI#xbXP;XA+V8{bLD>w{5q|t9oE%^v~ zc9_`@i*TS&H}+Fk!?77U%NolNP7vAS?>UQ80o7^PvySUNjdQCylq2U7>;a&uwQvGo z&kM}Z#nvbWgn8;(f^Q98Rk}=1**0Xtghz=&yXMFcmi;JdpvA6ysed*Ciqs%4t#js5 zaN;_+t!^BLzm28%lbp5c)D(UUGv)^4O@6vQh~o^2mwPbSI73hhCm-8Ba>unt(0$6d z#xV=?h{0ppF$3YFBeDx)2v`kg&Y71Ks{vONy}$ca*3$TFrbHF6c_uyUYR-7U$| z%K3?@hL2AefBdTxYV5g<(G83}7*06$gjRXqx9WayEUbc9z66B_^F}M@VU!R@bB>jv zGqgZ6yCgAB)i179s6su0LIt^3t3t&BViP>n%tKAFZ09zTN(RdIxSA&&SMzSH4iaDC zAaS)967wkkaOl~47% z(?L?2?RzPHO5BSvepDf}&X|;(oc}APUNIN!>6tiNpj&F3drVDJot)<|s!j{P6Fnr7 zv4%4Ql5XKsbCI&vYH(IjtdTk9Sf&NOu=did?|JC-R>6!%c1~8%Xc-UCi|R}5oK3pF z%{vv|I$^$n`PCh+UAPi5v-FiQJA0Uwy#^H%9u=a4w{UZApmEOg>dAsNq++8Qxzbfn zwSi?D-aasPVYtqbi$#+HumidLPsT4yo6SBu`^nsXkOXGWkEf1aXvt>fFOEsiIHdy7 zd7sKtW0VY@ss!R`rn0kf6kU-_?nllUrXB!M(CNN!>#mXK0QB@?ugq>RrI5Gc^kM<) zm^@6@^oXU5AlcHM`&+si^L&@D9XfB?95aRB)*AJMK1m73yzkW?AgX^RgFdPH5-LR8lK%qT10MHK9s9` zAF!q|K*PXBojMuGHPxk!s5WQiJ+D-c47ZqNwGfXFsHGMmILuK#j|ya z*9Y67;SP&R@qedz`>P-ol__SY$rSVHM5cI-P>V2XM+IQU6h8}AIiHA8x5{LdUk6de zCD!p)zold(VhS2ayv{>qiWCw@L4nB6cWHvn5o>-Rj2Y5E?fZMi!YcOW2F3w`!>aB! zbILeq?Cf9TFPAyBxPFY4qJ<-o#WS*^6Oe`m^KnJwwsCrKep?`hQ?ev=xuZG{Ke~6_@s+q*TF6H`e);$@e0N_w z0Cz!J$QJ<8zEse4R|a=#dk^OP=!U;pf^HV8;Lgn>tW=s9cb&MS8NYhCOZ^xYzb5=# zHT87ohE2U|H&|xfQn*`A2lBb@h?rF*zqhf6SqcLmOu$xC<;V1*=#ym!r6(Wtv6Tw> z-0)nzi>c=S$UZyM!H;D`nR84F)vSN&kqnhqS5Nw)8&^-JRd$O00;uW7wVwzzTfM8l z2%Ri({1p8aP#31?YoIPl(UT`#lA^x`>e3YbBB;w! z^vzH+4mZAApsq~ON1?7t(LMc5d zlIN=_^7NKaoF0Yg-F$vQfoI8*^9v?;Zb3IT(X*)G`~oG14v1IA>!p8;&>3+ZU-;v2y4R=g}PD(5=ni+Ce2 z4Me#@^xXKO`Hux(5&TeaBpM9uxxyrl7ra>TYQY-?dFqJnHVaZL4fSsczAyM^L2hf( zzC>`2;3C1V33ds7UGP4^&4P~!J}3A)LCRWW_rI=O_%YE5}Y7dB{)~`TEQCxZxQSe zTr2o(!T%8ak>Jk-e=SJv8{>USFednxU^WIp+Fu|zRq!&wD+N0Q*9fi`{J!AN1b-{I zQ}9*6qk?{PQH*b_;1>k*1PcT&66A&j-Ioi_6ueUKI>AQ47Qv|CYQcL1|4r~e1vd*m zBIv`w$@s}7lKa+9u)j9!FL6Dxt!tW2%Zg0`#gB{ zqF*GqT<~_mdx>ag_ly1~f_$lx;cXM#EBLgaBlxc1Fia%qJ|Ord!DWIif-8VbSBL0L z!S#Zh1b-#?Tfx18&k4o^UlB|Q9u+(;m@!kPs31;c`u3eFO|TJU(Y(5y~)1jB;U1TPbuEjU;3YQd#~ZGv5b8wB?WzAX5v zU_$UM!S@9}798T2^a+j?JXi31!HWei7o08FAox|mC4wsiEy1;d_X++;@I}Fw1rGvQ zjtSAfCCE$EEJw28h^GpEL9kG;N^q{=)q)Lz%LH!~TqSsy;Jt$P3qBN&2PnlHmlu0GGmMg1!GdQW|m)xKy1Fu=JUxF~k zbPP8PHc$x<`M5h`RXTNiRv}JmtuhZmEtP{4q{mGd3gr}QRQ3N6X7`sCD`y^H;swf|8%4Z z?y=j)(4zM^eFZNgqXBT;do#)yEI!Ri(keRR=eM80-|5;Pd?14F$+oRepSZv^2i(Dc z6XZa$BZY>hUQs!zQ(z{aq@M*7$;V7eFt3Qt&Cm^Ra6Q#8|uVF?W?`%=u5b+ zq4;cJ#i#RkEs;bN~4p_AZjM$qH z_&1!Z*^QX(U~?X0TJnU%1XSW6-VQIFwj11_v*D10Ue6bRBd8I@N3e5F!^?-7d&UiT z-zrlvUq~vJaawrW#wvJQaSrq$ya8pXEq!Q^tlP~~8Nuek3WD?dDmAvlV0oP5=jI;u*}L!&xRuoyovM zsp?!%+k^qR3HmS&Y0vnXV;r$-cY5OLJjRT8TKGw9-zx{vm-N92b1WmWQ~~mhONSfH z`bgBI0AqMZpU`NV7<*Hn%~9MgBsPS1Ol+KOP^V>hOa`ZAaGVV1WR#2Oi#QH5i39>t z86*9HVtyi%^NS1D`8$7_hTLE&!M`d`pxC}`vE6Zt3&hx~vE4ea&h&v3FfNa9G5wAm z@EhAY=-A7BsCt|VQ*G+NJ8a2L4iYpPUcv|bT@!H7Zi7nQ*648daxu+8jrs?gaAG(d zS^6weWia1fiaX*yU3{qUD%*GwPt~ydEi`ov8{f9;di!g1?+eOmshKyoF&{CijqTJ< z*lA}Lb=Yy=i?LUo(t$jh&$+qmW7()&G6U8mQVKdG+jt)ekAo){<6dWE>{YOiGE>g( zK%y~wuEIVtN0rMnr|FWwD>EuT{_Y@}Wg-iIH18g%&BB{Ek(x$!G|WM3b)%4NlV<-m zmcj-WRn_ik4;*9iJ~@{MIcCp801X^_>fCL2x^}nS%Yvo7YP8WgPAo>e*vS5(0!aXv zq3U>uUATR&N(5Iwnt9h)K@~ya>PEm2_LD-_pOe1F2#*7Ur zt(w)PdLXlQpJA5m^CWuUN3~u0({-8n66oqkOb+lN&FFgA3T@vIYm9C^FvKZA9OF|} zz+o}_EScl+YHn&qHg+Wtk6|OT2EYR7cZ>N%*wvgGukrJDTe#wty8U>~(37TfrFm7)rlmJ`1OH_8o9G574c z2anY>!Y%kP5CJ3;6No%jB+^(T;jIm}!-;MD?C_6)Dc?WFi}>HcgLs!zBwv;wmYr(f z+coC{DG!4Bu%uKiiiAjS=ZId-7ojf18a(x*JAQfi(Y@=AYyVnW$mb@3M1?Li$%80` zyP)d`7{HJArGgWH{2Jg6+ak%2Zupxeh$*)En&Hlkiw0ElSBD?nyI%|3rGBnm6AWE7 z?di@9n|jv{OM_%yX?`c%EvG_0*BudEa`MacVGkV!faKmh>x9X1%Z`qVur}`1kS@(@siJtc*!TpdAN}t$bBH+VS;Drb>u$4 zr{X&lYQ6&=0^%z{;6Ern1{e?nEQi0BX|TG$ME{wKRwOYXnycy1b0_qW_WL&jk+%9v18u z{IlR^(D|rz2wo^yEI3v062Vr%I|SDXeqZpXf?EY27kpCi1;GP?|1J2Qpv%h`fxI$Z ze2SC!1;Jdw0>Np5a|IU&-Y9sp;4Ok(f-W!QF42EqaEIWNf_;Jq1m6)nBIp<1#o2=6 z1t$tl5v&rtMzBfnUj;h^zajX2!G{E0-pPxie;Jq-@Oa)7{ZQdmoGLh0uvl<55p;W{ z=&uu8Cb&{?wcvLI9|Lj^f4Arl3jRs(W5FT9yEsK~lweS>0?2S?ihiNs62awyw+U_# z{7=D$1b;2~m|#rs6~QBdY3OH3H-mX86UBU*;AMhW2{s9SP4F9n-xnlLgnOi0ME|(p z?*x}DZCDU#a(DVt?#)g*`Mt7&f{QMi@G1BDHgV9!$8qtK_WD-7z8-csvh*NfWf0B0 zqn~o6|=%#p6Ou8H4f~POH08)=!Q&I)Z^h!(%-k|6Sp7$bB z!X1%Ey@=HagGjk&L>`qR@<0{wZ@Y8Db;EQ0eNwpMZXUA92q;8MNAyV^J}Cm%{~&*Q zdGdNYGQx3J9n{2a8*6ZQ+_5Clu=838E6Y14D|@;So9dEO@#2{D-Oc_ne1` zHwx%!TFNUP2OugI5-5{C?=Hpni*I8rA zA?t;gGH-DMFEx{*XWyjxzBC&TO4P`8 z3DD0$dTw2FO!9f$c+?7~Uu-@%EBujN?TzvYaSE~5rCuK1$E+bQU2w1Bz1%?1eF((7 z%s|HIj%f$gd+>U8$A{i%`6Dbkz1Or_Jw0wUeZ3!H(pP%Z-ThLW=}mX_n@x99pb104 z(Y*@uCA=v86w|R2n}XPlR+7uRpNBO>BEcXVQgxXOD6>#3EDv1@C>93KXWR>idS65l zW6I}AFZAMkCq?S0wENO=&=)VGLliA)#=rVwAAD$;ji{=uj@Qy`pLY^L-nP8pgGVs{`~|%#Ld+hD6tGS#-_Z17=CDhn z-WfuqsH{K08|Xlbx;mrRz_{e?lZFu30r?AT$%T7xsL$4<$NhkVx~%VjaJWnRXc z*}xeC*;Lt5_hsAaxnYB}kP7XRpCR0QkPi;4-?qkOC+OFJESv^Kr2g&&U{_0~`1k=& zW<=S76x{gV40HQih`|jJxbUeTEd-Gcsd?S8*MjxkvJB$SvXRXWxj2lg2UXn#`yvs zzuu+VBH|k;kH4&Lf6qeJCd4+PkpzIXXhW=xO6YTtAXRH8-mB65tE%k6bS1@{8PZMK z%3-odE0)h|8)L$cnSVw(7n%Fb*ovENE#D0#!_4EpS!*x|F>lKG7S%qHW*@rbke&Td zvOfpO9b|w;4^)b{j61rvMuyEW(~!s@`f9XaA2z^SgQ4l^LlI@Hb+5S}+91Slj;3u9 zNM*D{Yx+i;Z|ClkWSYhpdjRTBuKH2iiZW-Bb0WY?CFFNjQX%Ifn~gP}d?W^V!cJqZFg{AhU5B_cN4q4#VGd2j*Q!UlnEa@|+S9 zLPs6H;tT{YV{o4NSLPTL?zIK(<>7mg>CFpFw_@KD*~T^J+9;jCtyoQK zj^#S$)8*3tfK%4=tz)Ecjx+mllTExv&wg0Nz7q~vrQ!Ga?jjT(+$qFe6;u#zfR00* zR+xP&3Qf8@2wOZ|BjRdv?l9|qtOQk0EykWY`l1SR*NP#agv!HDv#!wft1d8NvyGE@ z(+gLjbV<_~Izb?{sw?fCW=O%POhqry4TsL$@eU(?NqYL}<~$vrxr?zej_|J5XTaW- zOsPva=HBQvw>W#-1D2k}0`%h?a8XAsO*`xvONqEv?f0;n*z!`p7yt5~%FN~>-l z;W?`;%6QL4Ld~4JkqMR=S`=mI9-^|>m{S(S$!3=_Xs?aFOGdUBo~$>gkj3gY3;_i809VK&kR{)c`Rq#D_N9YM~K49=h`h!?q@V~QFM-v=Ei2dd{dhL4M=R^~3TteA0iTjZNl*+f3-*+tJ|9SV#H{EV zm-k!cRv*fRqlV72KfF&>XIqggQxyJmuXY!Dg;NpwAK=)`z8%S;H*1((?hWrUcS5et z^Q==;abYROsC-?){+@rQC}I81l3m&5vBpRkxSwy8e9sJXDof|vqkmBT-Eb34^ayh8 z_%o$VC@NKHi-xr%J1uF>D$htS4Oy$#1za8iN{%(V;e z=aP!*IlQGq1=m{2(zW8`Ouce@vSBp?VwoeMf(m?TSL zG7fvQYV?$3%UshZ?YUl5?zw)WLet&K9c$ny(GG?eyR5{k;GA&;3BXj}1^x=$Z-Gv` zN6w4j8Dm865R72Q6ZN=D0}tlj#vC|GUD3{mTYA*5Dxx|`_UaWwup_&km97^>$cD^k zZUG*1XZVF=*UOf=UbWO3bPElD{&caqajJEb)J$qMoH!mC}JkFtq`HBo~&dI?>Vx-*ex~c!bdqHRm$gx zL=5u8EBrmb1YoA(qqGYje5>|<7DTZljqdyz6Ouw}Si*u+6nq_R5~o5#na`L%C&CmO zu4VD|dQ?5GqnK=372~2(svIrK(OLO=T!+o3VNJjkNIh^!`eGBsn7t0bSY-Hy%46yl z>+jykH~i#%<<20N^dOuzs~FiG2S4!gFwE$CR_hIxrK0j78eNt;JsNy$z!2=XD^q%v7Z4=D{_d}# z{IMvhNBY{~krVuz`AR9FAxd+Qgv+Ge$4%`O56r3R>=Oso&&r4o}(-l@A5pYDD`)~m8Ei? zPtQZc+x$HTV1P8JjCVf+jq|dUgUNTVeRSwsh3onwQJC41E&u8Ul)lsF^q~=ljOkp5 zgx|CLD=4Qr+s)GI(aI5sfuZYHopgA7HHtLUwc9G8AmO!2XO`T_2bQKViJMi-rtddW z_*$~GqAgwFcFC|KFQQHr@ttK}q+T!@Vs1aOA5>B2)iZGQPUl+IgMOCPJPkGWAoTqF zUBBdV`vEnXv+yDkX2_At@uwN7I|c_BOd>YK+{e;~T^ioo_Af|OKaFvh6ZKllcTw=R zTDix3oTtxpJ3vHiTDDQZIUVM^kgcHn89_oClHs#n1eKNK*fYOzF7Ah{KqN?zYEb+x)rmF7>$dzemOg{MY*#sn#Nw@-RSR; zoE`AUZc`;Eg4Dl_&aBnnb2`>Gh?V8U_jOQCIdGC=6`B2d(@Q0My&`I}kV1x4otAh} z^WEJHP^vVN;W=^u*mMBzP;0&aj)#IvzfZrAJHKmyvG9Xue2c;*oP9 z+}BKS)}eAm}OWejPQOq{Ze){d!rAbV)))g zGsry02s4M><3ZFY`3Tn2m`Aj%cvTM&60dWLxm-hf zC~UrV8LGdb;0-)cuLlVP7C^c7AshV6kC3a?I@Ii6!7?j#Jj0V;n{}RyZpfh2d5D|| z%E|KXG)HtiZsh5i(zrGUUy(Rs2@YaW;kw}=nV#tBNJu~>!@H9y2uKQG+E3G|e0*sX)CmRd)7D zYXX)s4QAo5FxOSP?xeFk6tOvscbkVG9@V6E37era?m412zosD!cFS12vyklUHP$fp z%|FIA1d_yF%Fgb_>)~1mw;AGo2~&jztcObkO`kBPV>A0Mr zo$!EeygEl=JvT6o`&SG_-@$nT+!4@)DcaYw2@G@8q0#Ebs2lA?f2Gmt#%&dxtkFV7 z1v`)M3n0hVwu=#CKLW>s7L`TmOXI9F%s#z|t7oa2y@# zj^z@e9zV^{w?H=8<{S??+UY2aY?VJYc*v?(&3!(b`&wOPTY~o@Eb{=;2@wDsM?e)! zcpkfK>1#`HMA@#yt3L(HbC6Wk{B>iMmxg3kgh1J+LpneJiRQkEc^1N;S+16dV2$z0 z%hqmP%47%6t6~C<-hyQZkDlZtjf=&iFLiPJi~O%^Z00uPNt1y-nvWrfFm!XTp64eQ?IBiAM+n(7uz## zZo5xM-TY-Kv`v=O$}rYaj-nPSQ!NyasYxp`N3(SlpDttwS{7mcTD*#xzH&z#aVoCe zk(UoYgkYU9(#9GUyyjIWU3e&KE-KqK+=@LuWyQ*5y1cLC^Go%u{NtkXGAP8z9X~k~J$B5fj zfQ0tbgyqO?TXj=Ohd{V{jHOOpEX#`4fC%Qo8&ZP=$(KYFKyDR>{}Vf9N{#ckw8HqKqQ3^e7iD zNnieR#Hc8K99~ZHz8QW`^{0rRd)R>$mm@A_5eHk5Sv7gk@r28M;fW0Apjhy1`g&bX zO_<(b61a0Z1eq}7n3P~YymiZE{;o^edFXeWq)8m%rmKos=8K`zY;TcQMhJ#xRxJW;RDtSU6a?Jp?}@TK$_SymEsPJG zePWA9ZEm+>1G6bug&vS;1}9j-7L{Dk3{wram2g{z?xYkHSc32r{TV+)3fVO9*82?5 zv~JFD)#C4dhsMIF&PDk!*WnBN-FMI&aU(tz^cm=MDy?yOO~EGg+vBj$j0D4pCf_{l z)4+9wxW=rs46CLI=2=O1dXGSf#e~CiioTyN*ZCltkNwkC?lxi$Q+H zAOv>an0_tyOeF{O9o*!C__Rw^uNU6;TO^fT!SjH&#)0z!h(TrF_vn$>C0KiwahW5# z?zlWP!7Titc^;xuG*CFqe7n5kP+H4rw(o~__M`UI$E`HeiwP!nfiGYFp51i*al7^6 z}eq(3T+;$v@S zmYmmXs53$cuL<;#kI4>Nz#AMgWC%>Rn~&#_)77cAtl+bgk&5_aEcrl;oUHimMc8da zC9Z}haTV1cAcO3IEMFFzIg-OBj>SG6LyQ>U+2C>P2)EZ&e_r`(PvRVQl)^_3@u@D| z*ng1lX$wUSxMd&@+mWu3AXY!7n9S>RtMCHL$W7ER2X0^Slf-ZEgdFaBOyo;)x`}c& z6oh0V?4UPJf=73U6S3=ghFU-Jo}K3C*x44!s2+!B-jUYVb~=Z#)Vdk!@B9~Lw+X)V z2_qYCC6C4)(Rx+)=r-yQ71O#7&~~qW1{}Lrh-4gmhJB|lxA6A$2lkdP(jkh512U(P zeCxcWO(@0+Wp%v~xf%7Xc7H;raq9aMc)&YY3>j+(*|O4$8qDae5$03d-a2d6*&kw> zqK6^`rn=nFDwLF}0=#-GL-Yt_faSp1jWxk--_Xrx^99qq?F-f^<{pqSsHPqPs^nwR zE+dh@vy?Rk);=FQ-prx51*AR{={V}+$M1~v}WJ|b?X>AfHS z>KhSB;v)`5nlN9*!$yZX zUNfL;Y}qSuS`!nOAV6f~X`}`2J&7yu03YyV->jt#3ztIV*5tqyixw}sY57forX>p? ziR=1BO-z6DIL>dY*Iw7OumM877B5>E;5qPRlLO}zL84bmBny{C=w#8dWeXRmAP4N+ z1U4+ZVd=t!p7QGIS=EyRGiL>=E}c88HZXPW)JrR;POB^rFd5&zhu=H@E98PDR8OD_ zH%r4`yq8fn9xwC34*Qs@&PlSF_?;tqOlZ^>8Ycry+{tmnA^tP&oG?%P{(wyJ&Ple`s=v%<&LBPM%l&A7fvW*hUxMG;P<)7f zo8iqEWKTI@?nn1(bQA;XJxlcAX!YQSR-aS=m40x^b47XWr8U>gMBg&Aj2B3*y=l?n zh8w6l;YFs#=4DGGp62BdUJ22(ngNz6)O`1d^_GSTz7my_Hr(U64+QANc=8aC_B)BN zUkEkdS*nIQ&F}H-fjSL|{v*(f@Ls1LZjKV+W(?dgoO6IQpGbsx7W~kC4v^;O5n;ZF z2tq9eL(Gd!(4i=7`u73TK-A6GEx6&Pfn(zX?=6K-es282g8V{tZ2VKmh$un6`4y_a z<3R7E@PSXiM?z`C0qemvFn?s^YvWh`>Yi`iJO$&>Dj1lfn)!El1b+*jE!b^NqG>Pk zsXt7SUC|PHEXSbQ2`h6HgENP(<=@lHnTDBVRv21BwMem@2Q3M!cwP}^TFv^M3N0U^ zZIa9b#?k@B`%mBYYC1;J3R9av$U^N1qxhO4m{pm^2ot*KGdv*C4iA)HkzX@s9;WNf zyn$TLcTynV0z#~7;GvPXe_=-X7HYa^U1R8QJ9gwF%(ffcd6K1L!^VhhynuieqZ+qs z9dA0WrNal=5>pA!&y`K7=+bOoys79l9KitOT46Wna_s%0bpl>=2%AaYp6C zrK#^B4+r&uEB0N~xg&AN3om_pu=zyezJY*ZNBchP+Y{U4jAe3ALILMC)^)rZ5ybW| zaP?Y+zl%3Qb<*KSKN-USPtyTb!sAMMj2%7jVc%}vy$?X=xY8>E*2>i5gQsL7vem(M zI`K*}-B&6iXCzA_t^!fqPk-0rAV0`0{hDV3e0hp97@ObzIU1MKSGtjA)-8CNM9R6X zXkZ>bn4aoRLQnIP^rX*vv?Dy8!yuA!8c8`tyfO|MDL9=#GssQhy>VDH%!XN|K3@WI zf~)y8a4?RZO0*r!N+@xqj930^wZ&jM43G{hQ|Qm<(qGUqM#Nr)&p`Z2)C`{Hh@8e` zQ1A#^0hDeA?9098a8(GiilUp7L>_w+VLM~1ke#-RPdpcEsWP+be!=>m?-=`MAenU# zX|D@2u3mg7zD0aTpYE7~KzPXW_oxw+xuYCF46O<$KB*M8%6U5!Z52-6;C1*YCeMm| z6I`owmZ0s+cf8Z$!|9$yr+bLFF%g}FGbZ|!j-U#i6J79*dH!s)b9Isr^X#PHnp!q{ zR20@URjNLmv8C{!OWEys3SKVQ%@#ke$kZ>=sC(>flT-FBPF5bZDdkatKvf2*h(4{8 z=crdCV%zXuV>Qix_a5M!N{`@4AmEJ>yZK13+7Lu@Q92&p%N&6rl$aw`HZX$ErhnpE`&XD+Uh7XgvxYxVg8H2S; zCUmIlw4_xg-)nHZ?75f>M@a^8hD`6V}iSpqWv${M{(}fJog? zp`h1FkxZe(^mp>r;@6wepcPOxCRI(MYMVTmo^*XxD*CN+j6E}u zW=)IAd+{wHXDq{kima*&+nIp|F zqiHw8B(6lZ$Ccz5N#&Q2O5JbkBIy$X@4SXGyjA%Obl&A)1XHGWXRsZiVJyS=5Fa%f z>Q@fRi~}87Z#pOIkuRk5{^OJrc6P&&=2g)34nf2o-JCMuR_|vy-hs&IR?lMeX16uZ z=gv6t+rnyJNcTTw=zq{maPZbKILoM?77qujTe1=s4O+ zWZPW!c?Q=<)k($|NNke%N{7fs8Z`bT4q*!eyIdw9wi|m^_Lyu0-%O zVrEK%`0z|9;6lGF*~m?n>PFt*H{VnKQ%^#6>?m{xsjNgBHXRXR3pU$Gi!N0RNpN3= z1?AG)EF(Fh_*J8H5Hw0sRkwwh^`La3oq8q z#$0P;wJKKRd=rgk7**DY*{Wc5=f>hyg?+N(RfX#wjw~C9vtvhAA^^^=vp?})V4FC7 z5YICzfbe5S6R*=?f1(w@Q9?^-Vz-f4sW~j|aEC6~>x|KpNM=>f%@RCq37y9i2vkm( zjK9_8HMa`35Njd}o2PiDO}~Vi$_Z0Ef!gYdnp>w(_{Ah7(<% zs5_~*q~Y`>i@&=3hHE1W12-(awqeo2#gTyen3@*M9scQ0ou*2Ar}X#5OCk%;zhvf| z@yjCDF1XQi{(>bp-Go#m6Zk0s< z_i7L19MQY`498N!f|YVd%)^iE*>%UwigmP*k9Pd>@niTCsNn~9muQvxOFRC)Bcqu4c?=92JTdY5tptzBD;O}%fY&u z!zqhrEPgCMmNixx17!krB_enh=zeZ^uHMB|^M7QIhVk-aIw%K)X<;4kM?H_dx@M_z zQ(Sx2djLOIAA)*Zihd&0tV`E^GSo%*x$#{LHD}ANej3!J__^*cgSsL`&wY@p6g~S@ zPBC2f*FjyMqQ4R9#uPmpNpp&R1=KAm`VOeuQ}ia(tMGHvcNf%aQtY8je*efG*SEzt z+aC9zHYP*U<^H2V`iBPTw++(o9;BCzm<67t3$I^Hp(%@(-UOj3ElU?RL2e2l0+}hd zQddZbKw?S@om>dPDOzaCbvI&D93tDVRS2=|*WI{yIl?ILG)Q@SO3$7LXzTjrTbTlr?EFoiq z{yr~QA$Wt}O@dz&Tq#HvH~sYp-Y@9NREdfH6~V)Tr=@9s>?i2ol>>i+=x-AIhTsE& z+XTrur@t2jU70EGirzryOM9M_qbwGS81Q%o%Rc$NxO+$Nn4l|{Jr7|p99MS9WYJF( ztQNdduwHPH;BA8U2yPVox!~)9lu^QZI4b&K7=IXVmf+_Fiv@W{k@lAfRtwG->=Iln z_-(=a1b-~ZagO0UD@Yk7)c;A)mG6Ew22q-SQP7q1UL^Xdf|m-;61-OM2El(7yhCt} z;5~vH1zoxC4~hO+!9NPVF8ID61&%R&pA|e;FeErhaEjn`!G(f12`(431@97cWtwak z{qutF2)Z&%J{J8jbku$bj>EFW>DE#zq zl&|_!JOMsM9kkYQ05`|pi@%S=R_uQt%jwln|LW;?f5StWowQZ^1?KQ3sJRV2z#5Q6 zx64O(kdB9Q#1Jlkdgo2^0^}t##UeOW7KB(vHG{iQ2i`!$yd2kg)t)|Hjl?kv?(SjB zRqf}i>ym0epL_gzhi|U17Pz}N7yJ3LNdGM&heT=4qO5^EecLxw@8`$IIU~7OS3FYh z==*yPqW)!LA$2JhKmf9zpWI@dSrq;E2dUABj83zW!%v%r{5_PJ26?fBJoW zw%U{I>)%h~J{HX1v#6klBgrV2uey$KP# z$<3PBR&3YAC$WsC#eG9jgmrqS#>q)n+pUV#gPM1;y-s#|n-SZ67<;kKs8aY4X3E%R zsCzTk;V-}5jP(lN{_O^0amKy)>$e|8N9<&*fx1-hxo?EhBOC8XF*|q*+h%&3eP&VH z=htjdY4*IOK8*dU(5 zFa(4hRBSl;2lxme@~xIzIp8B)4Nkx+WT5TBlkGtxp$K97^ti9r-8iP>q_61nj$++V z|~G@;-5LNC$=>{iPbd?%H6Vk z7aB@Z7}GXNqT7A=(Ud(cr;pd<+u?^)1Gb^+Vjvg)({(W}6Jaix2#i>>JliG><~d_xKqg3ur7A(XHA;3|I=i+M7~N@OC_klsi;?$NJEg zT439^%TX-|W;@lE_d%#W#+L5z2kG~TzP+(0P>4U}M8#>MLBJxVNt)#2A5{)H$H4N%67taitijQHAtz(iE zFT)($0Cn4igLyEjFQkMH$9QEb_%BKy&!~rEgyW2*0^!fi(owQ&nvl{StxxbjU-?Iw zQW1@5UWR5lfd2w^DEKc*Fb(-GiWr6eQU&)UGWsL`rJnv3|E1asYEax4OW9f~chWh7 z=V6ZAK*!c7W)yw3n(w5WHDHD`!sH~(kk?_X2RofUVTMFb)d?NoO_VPmiVgQQICqbU zKq?btOOS1!Opu{zP<$d2L_a^=>qQn)m>@chPWnJZWP<20dofla8*^PI$lUz4B6Qo< z6pl|;?XPiNOL!osg3jwNiQ^svjwG7OJ|4^>@fSzI0~s0Jo8(;R0gO#63!x+?g3|vw z%iMTi8>|>lL+juzsDVU#i+(ej7b>zB|*4Syaw#lE z7Q)~^C$c3(_kRM10ozL0%{-C6@LAtLd9gD14dyRAkInXa%#$$PSN;uPCVuCL9$QfA zt4#@u26V?S4?nth-OY!)t*bw^~6>VA#jFSNq|OK#8S@ngBL ztg+fjmI;)eLx>bC8&KbiAM2IrT@T!ZpBq0{ zau25He*pEvDS9ZA-#@~y_^J42TPDBakz{C^&UOsaKRHO>H%NbQkRJRBZW8?u_!SEZ zz^qsR7DK*bP!zZf3eBkigJPn~pa7==^4%wS@JcP&6nJv>A`c&)E%K0k5%z!^G9lk( zTui`IwP0LKfVB7}1?Nwg2!O7ToQmcJNb=v$#_*hqymP;dBQWtI!Ks4t1+Nxt5d5kj z>5TrqCiq(*-+|jD`ririND1A2LC}>I&*z_LK3niA!R3Ot3$7Mavf|?)$ZeME|CsD+At(PM_|F3Z5!>mY^#O z-jxAgE#~(C(}FlVO~gRs%69*q*q@Ft8SXiP`GPK=W1i@*61-k;v7jyZeZgM}ZWr7q z=m?UZ!FtRCGTy;_4VRyBgV^6B_%%V7zhQ~~>w+$y<6hDKKybU@UO`tjdqVW?yS7I~ zpN8?7=^7@OEjUgvUvP$?%jakkeMGQTuv73(L6`60zIS_{m{ZsVdLlBQw_!L8l&@283>?!Y< z?#>YNvji2t1mRDBp5cFqi15p45BHUV)kK8HI~l|l!Km2ZA=o3hR_wnm_-{nS^Y1{$ zLw*Y5ao^MZJ?#-+I>uz;X9SxY8kTPf5&-7 zs2Qiys6SekT@ywEtX7p^(6?}<7J_eA2LtG~aUv~;0C5`xND(C91}*{DatV+S2Ixo_ zkm!tygdK;CjyH^sLq-Qe!l`qJW+}xv-!dgJwh3E3kf@9pY7v7$?~oTt!1Ze;0#~&O zxVlNeEn1zj*8!gt10^rlA%=Ry(16?5jfkNMF*GBFL-Y=TpoF!-gKj999j|2~Z4vlv zfzMV%8>bUUUL@14*`Oi36PvafDm9n$KlM5H!?d8Wp=!k*^$WXcvFR_wm&pum4%)=V7L%kS+u*;xtCp+tdhoD2)g|KumdMcMy z%oe5=(BP0LV#RgXOaQ<52jL71 z6(^=n;NKZQLbmEtvtS!_1{oic!XTZ|h|7bx7@d;8j$oOD;%j_uS8#ufNrD}226t@F zOxeHGA~o#kUX-dzGXh1*IR$$AOWJjDxzBto`XUI0!Aro_9V{3~+xiS7{Q0{g=*Ci# zfC^(^Bu|fZakw9Mk3&>L>;;2>EC{%G0tP5q$fUuzL=i|Gk7q(Qd>1s7au-n>zNe#< z6$xlT{5bLK7(@GbYX4Of0VC<#P7QYPE1Hw{6?Crro$U+?w<`3;BP1Z<%?*I?k7r+2_ALtjC$-o{FOt&Yl)cm+KgE0r^hi(fanp)#HJs6{ z&!A{sG3VeuA+03BxbGAccLmCan^cHDRO%KUT09G&7+Gb8nfGu%`k#=$I}J&5`cew< zH*iT}r;p<0nQj?ALSui=9r){LIWgc!9Zd3@AU&$rVAnyCqSy41cFqe*?-t=%ef!m| zj^&?6=qy~PY^+D`{boc)aBsiYxQivl{Z^(P`BI6Po(b{RFyNEKPM@16NZ&|FwW{-I zRLL`@e@OJOv+oe0ADSp}OF%u23b|a4@faR3y+(1;$|>_BCC?Cp4Eh=a&U&ep-5sv& zKDNTg;Cjn?jwxq4Rgm^kjJ`QEHQXQlx|=3V8dgO%#CP!CQmHk~aP68NF|G(7!jbw4 zFJwi4m1#xpJO-LXz`yXQfVFRQN7Pzbh_uv-e9JKR&YTLVaZf;agRO=6=-o^d54$~_ zpO%Ji-Ps8J%E$PZ(%h!z!1nb_{b+F`>w;a3-WsNiU-p*n@pfM$TVNyCkVsSE9K|G= z4|1dLAsvX%KU+<@CTWq`%MwWuP+`OfAV0(h0nKC}Wt7%DayNQbp^7K-$o`wxJ1a zc8QJaN6S&Kg4~-wz-BhWIHX+8%ub(~1zi@Cll>F}W1P62X$CP2;wBWc#;O;wp1t#14?+w1UAjNcr9P9;v7<`z zZ{5NCZwmN3N%kM?$n`m}!T0!XXBCM8+J)4m!pKsUtr+1Ei}RR&n1rI3l4sITuI>_f)@n?EwAc)Bf(I=-s71q%ilj!8%kzwT-DcvcQ(f>0HOo78d3@ zDj#oV`8U87kAd8a5Xt1NW0PGA4ZX0?>7JGYRJdMIEuRQrp|gC6ELS4^5$TWRF1KIs zq6st>c$(KR^z0z~;7q{L_-DG_zy(fT&@8o$yR0l}sMdVr^6>t)i_Q1!%e+d~ zpc+5Q?mULdTN`YL_qcC6nmH*PRxJpsa!LQ^T^@k8kn z%sm{!{M}cnNdS_a4>@_=mm`$ayvp!-LDw6a(q4;J;Xua+daTt%gz{DMLUuhQlfkrf zHuwW@^8l=zoN-Bth;9{X;7CGCwidQ+zJ^@J7dTcy!&+Jc;}NVg2SS2ki%&gnYGh&(MpvRbo_`6$xs2CugPlesbptYb`6XMCu z)=`F7+EXEAEHm6^Zj0`g+Dj#t%_-4bor!2d$jLf{#@M(B_$AaH1Ojnu|H4&)3h6Bg z3DrPeHu6!z1~-&$rB?MCCnYLrd=Mv`Y<_KJ?lBI7LmS&deMd>!aldWh`XBPcz>l+Y zWLa1jgUUjsmj-q>OZf5vtx1qOPE*aVKMMmW92T>rZJC3j5(M%@(IHf+S zXlnQXIO2Jlli0RXBtKU8uv>nv$<02=e;!|eVIX;EkXV#*zOkJ5G`V!s7eA2)YOK@P zJC$Lb2(K!()rx$rB~6g8HXARRA#ko@@f6x@!@r7~tW}Mr2v5%^k`uzK{w~@%7n$6O zK-^0Gr0R4FI;Gkqv%27d(o7&>JTCMxfw)V_eltzN11?dx-2$Z^iFYcpWc_oBL=8{6jZe9cgO z@RK<1bOti_G{Z55X%X_ch|Y&k`<}#+uAE^AdaH_MxHiF3URnzJb?qh-$5+)Euf8YX z<#Py+t&8F9?t6kk=-rRFFB9EIsJw5llaq-|b!?+R{@1WM6l&wimvh-I$J~F|Yw)mu z9}p0*7tmpbT*)Q#U97Gy(jQ{ zk7p`z9&QJUAP2)>nmb1V5#6jEf(8)cy9eM59YersHmq?o`L;sa3|(%)7Tfm&JNr<| zc~LKV7)e%|Q?`@S1s*hf6~h$6(OV{vNBPK`c}5VlHwHpo6@8{7lMxs_k*;Mcf^utU^`fDCC5<-=kIH;Ag5usK!?1Vo|sl2u#HT1ajA`u#=A(u=`ylE5%2?x?yIY#tj zw0YSbBQ~qK1}EB_GE`$F!f|d6GKZtUSQNvMqDVM|amvs@2^TG#OprF_p#R})moC|# z&?OVf;D}tf5YfTFma9Az7Y*pa2c!ogAG3KFF1Tpj1hhks%;BVe7u0mS8pwt64&??* zO!(0g0F**KH29B!xgJi~Z|iNB#?0`WZPTnw7_t=Xx?Ma5&3+JlLrQzKM&ji+dxqCM z)%;gT{AA}5K4d-~{ckCwl{=acrV`M^Q8>pDHOTDNBA zO~&pm-OX#f5J_mfg6V)XVLcW4GRq&9Cv22hryzTr+T~?p`pcF7?8YxA{bK9CS{BsV z+S{DQ=A$&`lukK> zwdCcS32x)9+(sj88q0*m-?MauDI7^X21k>`gj$VzWt0okbyXnR<+-d|glX z8W@@w3~LgyMfnEwXeehR+08)uT@MfR$i9uf9tJYSTY*e5`xp9SXU1szfb@3|NPkQQ z{k;ujRD<1nBpxog>7G=~&9WenvcW2DK|@jM7*JP!lueg}~58LrAdknUSi6LVC3 zuiWt`GtW|Dlt@f9`$wBsRO@Ue~5F8 z6NS&2tVR3XlI#D&{_g821Vy*b*de_UARH96*E9-y zO*2p!5i1+(lgM=)N7Ev+I*$H1av947$>ipd&@?vU^?nfvn$n=+4NvuBvhp70HFdkV zd!#qg^k#cs^ll7cuTikQp5U}cwhwJTyh%^sRL}+|a9a_Wjpua0qG6CDB)~Hd|AY&8 z&1IQ48>QJ?g|35*lpAxe1LwHIKp=4gM@rm$;T!^e1p5$fNp}MI2qs7Pd8k+1C(<0c zYE6e&^P^)-q``N}*qIm!IWF#c1->Hc832D*3xr3PCzj$brog({o7^>vJasJb9Aq{l zyN^JaFn}_JGq@R=IPb8lIu7}_$WbFq$@D1HqZB1Fb2M<8^2UxlpDksdOU zQc^blpFrJR$Jzco(G{p|=Kth%eYst-6P8>Du`M}9kt^V+Yi>Gf6`UeO#tM$-T&$Qt zc+nFUNO45oLPLrbl}&axhx()IlAA8}JU%z98kFqwF3R+N9DWURjga{aUdBYou|qC% zK#K!*j4bSq3BS+f+BwE)poGWp``rnwJ8WRCMn7Ucnfqsqjm>1q@I(4Aed)pb6Ia7F znZ(==k9s>u@+}eQ)zpKsr!G3dmH$=sw(gQCb_f`F5Hw}N?`&7 zy*e$8wJlMSCQ3?^HifiJoAgDE&_atr1BCVlixZNRBo2qulUC^nItb`6GgVPh>VQya zOAA2+7MM5-xtAX&FVWDeAcK}VrVDE2LB{EXSgu~IS0|*lLfEmeoY#D-W(ui^=uWFE9 z4b&+5HlSL#w*p7T(%i^tK*O2L6yA1+H;s4g>=Kl7R^=nU*8|0OKQQOlqENnSzJ+B5 zJc+jyu!?SmvUNGB(Yr?H6t`&q3>4*^Kut1;m#R#Kn^AGJFx(*Ch5-DF*lR6AO@Aui zSYO$J&>?#L9ZQr!K3fMl5qhR~9}&qe@}}6lx^AfL1ScC&OSGV{FJ1Mk$iNJGGjfAm zu?lGaiF12(O)_g+G=|%cs}WO0BW%uThHaWGAXk?@T>ksvf-RAWM-tePwa;i|?Y}!Z zLjFPMmuJ$ZoTDT2UYhxyX{qBti?K(Ri1bD8FfrLO;Fo2#fFS?cSJnG=mJA%xo!NzJ z&hp+?taJZo#2Sv}D_jxkggQd4KNL4DbJOq4Ir5}>fW*rn1l8kt0jP{$1FE`H%4pU6 zGAeiKqh0)=xsP?K%k@Qd`F5ax3U_KPU*=z$Siwx~N`F-W^NdUL!z`%FOg?D5p!e+1 zP+e0e6y94V+p>3kmgy(aSyq-CT(ZO?QqG1f)Dv8H^UCc3A=RM%Y; zOQrFxZ^%P&O#(U4z^0g&DVJS+tQBC`1pi%3B5LwZrgCWNs`xrC;vLX~Xj(k@R14d> zfQz3?a?%qR1z-@N|2cUc!rRQkf%grs8;JLOPf^*u;^|0GVwtcLGUt!|2M10x9usep zq_z+uXR_wJ=1-zVjQWeBSaovqYsA`Vj*o?nyn$a6WeQonDo&Iw=={ROPRLT7so4`d zIYWIKYC%84@^VA7#x-&WCQ9`5DdzZAGDub3z+^Mh5%2yB6a(*n<1>-;39u+uX(IJC z=mgD_pOtU#MmKwt+mh4Nef%*L{%N5`%NW=4ijx|2iEsUkK5;HCZDY;Xe<8ZWKZOzd zkHS&A*;|6xQ(6=gjt^+14A-d9W$8MUdwI0;vWcC#+D#*7e`FDO-)$H!VHsgfHw#a3?J--m$g$!EUJ%(abt>gnbaH9aecPIkF= zinml3B$`}nW(%2YR+-~BMPa?#n}VjB5q>I{D|e>q$;rY3zo8&{cG(jh??-^fAlg4> zf8qSjmWiERd-BVkX#X?m&{4SL{pTUIGwkWy^hC3xn6gex11Gpj`M11~zQ;-5Q*5t` zcmI@tXEN!NiADGVV;3AEJ%wJTJhHVx$AqQgD!=?USfg&L(Vcte;q+%Do3 zCQWWflMF;9G$b`urBW+fK-2wY)VUw0sm$4ZX8eO#YWyR;SaPsWrkR;z>f^zi#MNH) zn0eLikuw3LH!^rF;?(%3dnL)tkL;kyQ2?@qyo+S|)c8F%Jz-!XA^N3H$qdTt+PBQJ zJQG?JsoQYr1?x9*^nc5S^&2m3zMPZg2s4jG>n^=y{mhv&BSoD4KT}7}mCl@VA>Ws@ zUVQ%g4Mmq;ShV5NFNC|e=0-j^N1Fw};tqiklnX9xZoT*tgEcp@fU5$EE?>LhBHcYO zLb>G9nc_B&YX~m+;`$AjI&2rbY)o+N+6|xOVuB*e8PLjcbaNy1NRL#hi!NSYBw=T> zXF<{qr*Q3Oxv79d>@M99nK%46oXALn2z7~q(?oI6#tYUqa~VU?+Dq0IjS$Z*l0L`( zh=)X7Ty)XKA}Nx46VAWFT*v^w51Wt;IH3fNa>TD+E^Z_d`?d@xZUZf^qxk9{vdX-tb8=zKdQCGoQZ?x_4?N z^ZRoaF9irsKP~SS{@=r!1&;X7;ZHyDJ4ylWr$_xMBeL;yc-QwFjzAHA`X271JCL31X`!M^q2t1mQqxh3-$~%_6kK#9rd{ZMU{Du6&_Yh`>|F7)J z3;78Cuj~UGO|Mjp{PNhn8HOxK+I?bLrxyN{?!xc|sCUqYe|kqY zAX|%R!6@+td>yC!294z2D7soIksJk5&>m9@Vv>?;&7r5{y2el7I z-#gv+KT`m8r^FCY^ieWfQ2RxMrz7*lCnHE^WRA3OEph73hy>qsN5ouU4m^IWcMRk~ zL2!PdaQOA>9e%$TIKhh2QMpcii{e-S^+P z@ZUT5n)^QFzQ<0m@=bE^AApi?h9L6IW(A@2b}EG`_c-=s2=ZPIRJ!T}(bu`|{Q?Is z28z#>g7CS7y(ziqE*l6+C2be7~h%>)_{s z;&X){eBS;5kTgu994nuu3KCxuD7<1p@Kjcsvo^BKo;|CytgN)`^peBvW0^f?mf1=F z_xG`E`=BkzB?e5xKVz&}Kf90RRzhfq?-4k=pzQ3z_?BY3Y|{k&;CbBfT~k*6{>GpD zAn$Qqtha`~g!YWo(g~^HBYl=Oh|PVE4z@VB$wA|>z|)8ir)jzE0$+$}BDm-EH2sS9 zkKLKew^8d@;9q6V!kiUH-p*ntt-g^ z(_gXU1N|@4O)zs!rr#W{#IjoyU>+m3bV92OHToDtzd>XK%%lUaITA2bSN27*Hzk+s z+eQ_g*^`w2Ab z1UvPhb%IY|FI6mH>=yd7+RhZ;@;!ZIJB`5hW(+qw7`Z z6{NXnnr&7u<#R1!4eE-azNt&K|LW!Za;7k^{5qLd$c&ze-_F~U>?$}UOy|vBkca$- zBp?!ai9DB;{_*orQzBaSnDSS-`bFE9!wh;}XBey#67(5quYrDm2Q$yk)EhBy<%xe( zc=fKk$eJkw?~mwJM0vB$263rZ(0YI=%}$7o5ivijYyO_)9c9WWusaf8gdR%9^uy^k z7jci~8+(7R)nI=XiMwPsQI4uB`78EIO3VHT{WM;Z{8e~Qc6@qtHrXuFILTx>$dYX+ z`emN^ua+5QI|l2;uJW~tMV}Y?v8&R$|NWN|Z=d$Kpz4Zr;Y01aP*_oPalB^@_D?@I zi6%55TpZfWu4E@fWReDJocO>xR+tO$0Q0x~jH8w$R~P9dXsC9EDiB~(kCr-}}~@}ye0e;eV- zR6FW_gGspXm7Y}^s+X2ER4+maBqiGfLM6AsS{Mn8-&OQS%4!D3=$xHI0T^1U4)#QW36KbhJ+G0kDh1cTn z1hr-4R#<69Tv7^OQY)Vop3PTk5&kZNzdhM0Jm{`AbX&}fw(x6smmJp%AAZ{$o}fml z1%)RlJV9lLptMy76kkE%cLRklDE!qx@f8%_7NGD1mr4C=c<8!6CR~+U;WioEZgYpG zozjLdk&BwSrd|2Tgv3^zFe}R<*r?okB}EZ)Q3z@W!#1M=XHi5W%UVSdxt?(0TUEN% zF-W9$i6bZ#%Cd$H1AM7m%P0P($~Dq26I33l^lFuq7%nqKhgcpakUOD6%X%r^9nfq7 zOA2{CCpX@G5fAvOO&V1G49}WJ>ox{aw=Ce|VXfLa0GG}8@8Ii-fO$n(QZX;muu8T{ z^emuID-Du$VzQu325*;vt4PjHSSja7O6+A&reF>Vq3qbMLyYOp)l2=f8oj~CU3aK61$IXn~BN z^dKlaLE#B1BLqj>Q@C)42BO0k6#i<1AG)W|NSzU@UNlCvHzr&em1JmwMpJw;D(9Za zNx7$qBQ_PI)h?TgLL{5oVv4HDW6Y56GEjNH4GgWrkS%FuQBbxbn^IDh)v`gs5zp`3 z6sna?iLbs3->N*!ijl08%Io1Pm19#|jPa?!$vr90#lSA+K(WR>+5Nz(_@0h;zf6h{ zLkf94!ltatc%TWjNh|%<^Q=@lr39+?0#-eWt;uSllL<*qnU>0C39ud$P&!p+q)y=r zs=*PaOX(55%8Kw6E`0GF78`t(74dI#_%^G@`l)WS-D<=yjrgH0;tyXz@fB2l2#Rkl zP;v+guM;RdL7ANVllTe>|7xJ}Lr|tFy!uf!-WFq0!nG#lbYP1$2_949X&H)4ihOX& zR>4)2Q4^(eS7fnFN~QGx&_4kg8$sCn+uWS=3YIMrCz8MA}#1 zg`U)$KPXW3<+pjP;)}xHQrrHQy!E6rIcAhL2Pl>3UMi0Z3{U|DOFHi8`~GyZj?FP2BGe~ z6MW&d2@ihCHStr*geR!@w!80J+;>6w4MEWhj?}eb9US%n#@4Z+6Av?WY+oR(V;$Z~ zd0I8r>8k~_iCW< zwgJ@_RmKQU;mQC(m74v83s0;SYxuoJ-{CK)>i&B3J!~hBZ9ltsh*$Bc)Cek`I-uH$ zf|6GmuhJtZyfBr5!czthH=?{JR(9=+iH*sXuyip-<0=X@CRD(l)OMV(P<^|T(0KQC z{7=(T%9W)lA*$gbCgNR2VM?%7T(^O066sy|wGRJn*hoQw!kXesSd| zpEqI0fc3_Dy7r6v8s7b!9hEW^dB}M8ml0iJHYPce-S+N7LohxvRH18>U6D_mQdUrS zm+-|<;w6EKuo+md-uKOLm!d<2CWj!z;dT+Nyfmxg$}D32XF2m8-`rvTJt~_RPElGHvb&qgb6PKB&fut=+JA|# z70aN5l4Or$WO$EcYG(RCPVBl7eHp3F$`&r(`vvOb^Z~oLi;Fy#*~6*W8y_Sd9mwt4 zA5EV)_<$XDP4^>ZiH$6XKIR`af;_C-dnM{zjgPzb((;w?^odKUz9Fjr#{!P0pRu<~CQ}s**KYz^B`R z-CBEcE9r8sgj}obJ7TJGi(kA)m77_Z$?`bz#4+Klcb7NS@$L6*{>>0rLG7_amJ<6i zdFUO*1k-Vayp>OhZ+(&A5&8V174A0#Tz62!Q#$&Kr;c*xJjHvo3KBs}hhp0-;ylmb zmmkyq0Gc2PEEe4PA?SwnMIza-`kcD}ytM=Cu)^jVVOe)_W+dprD4HTXN*{x-P; zW+f9ynTaEY8QSZ}_t=*+=I(%g46lZZhu1Q(w$1np+1bg`n;*O0Gk0BMb`}1Va+U11 zH1vm{v~GfdHI|E)yN#J%@muAj*9%#9}UFYJU_RQ8Z(Zk7*km3jwU=I4$p z1qOxJ`4h_MG<{VspUNrwnRk-H+YkE38J_U)V%Pdc7HrJKX~bbZkTeo!onO{7lxi?u zEqdL*8H|(5x&Nf2(oN9ph9UPnm@iDmL4$XD(~sVTgf$C;#6Hh{qht*n^Xvr2U zdqXs+#pP4uTmMt>G*IEhzAlGfr`J@nX76uuyDFv(wXN~B`n#koIwiGYzIW1UK6jm& z+wl(sHu!~FgQZ5sGLD9rcs zdiHE8!!eW}y++}NMf-y;&Uy-svZZ_l2U%(TLf8I$V}8T?%FgI|H`gmkb||+R{1VN9 z)|Hjyw|>v65ijKF^7hsK$@6uMy<$b{jb7qwz7cJ(n~=l*>R1A{wz=q{OD3o zU(|e2>WZRu>r?A5NU1YcEJ$bh#w`yBx;T&o@h~i#D=Wxp{r+(o6m}kMZ=1 z|G!C03aXUO|7Gn)BIDLMXD_;5pSAuxD+kVEjrTg2tnu<2iH7wSu8aOZd?~qZA(}+7{!ndqP-bQ}e;r}nzGl#4EB)9I- zV40ywL-;OTg*w-FmPMNMDNOH5^Hlyq{8M>W%Q}2lwiIQ(Pv?0Wes!h3|8DuH-MIpT~0zf8qB4e#Mk zcC$Tv0qXt6G4DIZy#Hv-`>rwX`^UULGUolsG4Icfc^@3}{ukb7k-=M?{OZN*VV$h* z{8oCxMXDD>J~wk#gd5Kz8@XS7-9;B($o=XUa=H5FxLN)4T&up3Yt=8BS;~8v&}Kzg z)ve3aTQ1RMBy$A0j^uQ3xJmtUx}L;bqTa&CGa@Z~D%HyFmf_{&7c_nL;!Wlf^^NOO z%^Oqts*OmOd{$5F3UW_`QLCD;ru8lBTJ^N9m3rdybe)TS!}^OaUmy7=^rE%KzdQnefqs6B1J@nnXJWXyMf7ZO5PH|y z{JJ~%Q#fk9KZ7ID9m3x~+P=?@1M#Wv3iACATC<(RHYk`{BYd17yr%(mXUE3`KNN{< zbnu_`9lR}q;C)5m;C;ir|GR_V69n(a4&LM7{SH17eosJ;&+~Bvl6x|RKX)2y$OS3f z4{I$pzU~}sUi_8|67RV{#oH{1d^aeZLbcby0SABM;89d)`aZ|OG6z>VxXHn82k&rj zzk~VMsOS?8&Uf$<2d{Q;hl6)H__%`u4!-4JE{;z8KI&k#gXcO5(i+Y#TB9ZWhn$HB85 zT<+j`4qoWs)ee5k!FwHi!NHdueA~gJuyMs#;NTPoKkVQt2RAqvIC!grPdoUsgOji| z@tfh`Sq`pru+_nB4&LJ60}dW=Fyr7W4yvq)?}-i;J2=O|xem^EaGQhQaIl5#DDSl< zzhw4%ugNc+J%`o$`u+Vi`LAMo`l&uttr;8MO=JQh{YT&|ID2?y{pJ^M;VDgdqh#7; zlXgA0o$b*G)WDJDH8CA2GO`kO8`wGgb$sD2G@!MqcM41>v&-$`TYD)?jAK8j(CB23 zsUJNt^CMu7ChB_Y9k1~Ccz2ye)fl@!qngq6x54p>CkhqhP`c`9Gg55E?v7TZ-Db8oD9#_aSf_BLCmeWM zLwiT0&V}T8NIFwTn?293n&%^VwygYyqgkPTbi^KjM&JMl9s!;{xV;A}qRfB|V;s!F z=sI)g$|-17Z2O|rJ(BxKBX^in^tdZ48*apLCC4eQNMVyP7!3a4gnptC}+RE z*cpc)#_=fMwPPSv$;vz}M>qKt-Dihuk4TwK^@uNWOhH@gcbb>>N03o5$zuHIQ6^*Z znWXBuRd(9QH@mLakDaJx!5T5?*^`=%UH+2CbhK>1F5xnH6YoA}DpYo|e;~?r-(|2? zQq$3L7%A-o(Ya-RNf*uy6LU5KOasd#$R35oOh_U~>;!X&uZ^?fcG z1EUkdw)4k{!KQnzOLx5cE5wMbF914IW!Q*Yh|pLX^%JF=|D@1)*+z%Xe*u?*eFETp@fGXJqkTe_TISfKu=Zlrw`WWxMMRWVj$-17G z*fl|MH6`^dhED68*YKlX9WI=w5cTttg2IDZRFVgZwF^KBNEbeF7Iw6>^?@w^0i((? z>t#61B|I!mjv=}`D(UPQid_|D@3Ih>37}AF?3R**tkHZ|oZ6ot_B%y$rHV*Zb}YE3 zGV7^2P9v6w!ID+ah3UJA5vHkRmHeJfI|IsgQKElh&(rLOu=>a#74h7lc-+P%yB2@5 zn(>_sLK1fq#Z%LBv$mn#S^iA2sIn~W3^=~^b2L1?j7~(n)m8+k>x~0J)4N^oux|c# z`ioufuqNy~=3zp-`TMHSUCJ+RoY!aPLd-5DCjUGIQgYaS-Wdn)pXg>#(92}kxYW)Ye*h2Y6 zcpOCaKqlUO8yuLsu?2wHr-LNOuyZaT`kk&r2>ChCOpI$4Ba`5Y(Jn?#NB^4b4^oBc z=pVUZ<_5F+@!5k1p5tm4YS&IES|`C>C5CZ5c|z< z@Lwl&1ly3PC#)j({#FMMOIJa|NgOIXz$CYsVCsJ%-ZMvW>0B>)mvrNMlx1p(~5?8i&&VmVD@h!XzNxGn5DNQ%=jz{K4Y0MgP%&xX48#-{ose9Ro){F`T-s&!JPpK7Nf#cUvoEb}TZWeHP<9{Gf#-rmDaU@; zx1jLH@Zjv#seTRx!^7-1AVJSdx}!y3s^i@{Pz-d{dO^xN&6^2U|RPyIi;Leu!lA97HwJKo$=m1s*Y{t z0zv|3ot}8$r}OMlChR-QFXxOGYL(q&@kn;sqad9z)c%D@BK)RR@yWwzxhT)1A#sBL z#i)Pe(y}+1V{~=4+dNe9V2}zlVNvPN4A>-TCG!iXmKAP`_nH+WVa;a*8wkrb>s!;h zAV?AT4O64@&eoC0efedNwf}J2kZo*WvvO};%=Jg{^b`9Z%ACd8AI%bvZlB1JIaj## zX)(0hw%F^X&1#{`Hu{d1sa5b*A-SVF6Pp}5(HUtPH&H>h`wbtDQmKEOeU@HhyLQh- zI~IbXoPuV3YTf4>AR;77lPI!5g-9qSd zTz=2%t#dK9=fO*J=Iu^bEgpkt#tsViL4Js@+zo|y8*V+XQY}~&O4&@+L?g`RQ^&Co zo2qVx3^&t;xSRLi0C6(tJae)@e#gmm7k++_o;OxGz&8#D_AUCauGnV|pYG zdGA4auRn`yr}5@N;Y7cA0!_pTV3?v@k?${z`f0H`=DXXJpM7_2F!| z>`g5AH>Nr>d9Ce|m6<+NlBEe-)e)(L=a|EEbTVI6n9uSao8d~gJ)StI_&Hgm5p$X- zC9_|m>Uk)rxR&kTo7g;gH6$2Z4b7(QJnBc<6bf^73d@7hy!%^1xxtjPVNoXk4N*=VjwQ9hEQBdAv9O@$5rzkp7fzQL!avpspA6sckw1X*yDv0_KdzmeQbbW>B)3bI$$t zsDBLgq&K;vgw#r2@_!G2a#i>cMeA=DD>PM8&N@*DuKXelRKmQU`zOw@jemreGe$~N zUa0nh!Z%a_!}SxOOX481fZO2jm<->lynA}^4x4NJKO6jyaE$;R1lKaKtx|?hh{EI-D>Ks3H)v3)Nm3nE z63g~pSspquTS%urXshL3PKU@XI~4EElO3oH99GHCWNeMRsP!3RG9S-J&>-=Sb-Xwe6wd}`9ZY?dH8(ya8 zy~P*l%i3$+$@qXimiy?}dWO%jjHSe3jIfawTJFGfd{Sll1ec={Te

    9ay3Qng2Ta zJm8q>BD%1XFAtqgJTZ7bhip(PGrC3-6_0d4Y(L7SgXRDgAF0UKd61!u=NhkcMCv36 zZBK*r%Au=Q-Tx3hQDXtwJWV;VjjW(Bu9Y6m`BALY_`kM~Oo{ATVKa^>4fJ5J2r z!-(mM(chLfvl#t5^C5l}i6HRp|=#~<2DfG8{jd<$YP@xNNf%0_P2A;q%qAaRqaV^qFb0=tk) z2;3T2o5ArOUC9Cy6_TLv)pXu)75^nZnBfa!7!|kq6B%HR*?jDqh!BxyUL!uar!>XN zRub;&srgYtBk8j?1qmyc-O)llFGJCAqPsKqXvJ<%EeX{B%Nf2#C`}9`!bBdu-RE|v z)&$WhQGL<7#lBBw89IApu^Hq~)lDd5dZlfd_sui1U{4~Qf#OK5)ae)A_QumQRCk^8 zl1^dJZd7|L|x~y!%)aC_f&a zT6;ulYN)8_FYLH`b?XtPbwUacf|;gQKwHWx#hfJCe?&HiYWffEZl~yj;VASGrX@s& zR15s*S}D(^F2Ca1pm>&7@m)_9p3;-I0-arpT+*mAjDCHZjbC=;!5&LIP?AG!mM-Kv zCgZ1%m08jbs0=D*6ZeaviX94yEzKluQ*yFloS{T#^ezn@!&%soH{9tm@$y0s|%^qBTWBaY#m zXy(uKBD54SePHlKL{#;Xr#8$;BhBd2bqk$O%l9{$4kAgZ$~?$n^Jwh_CB)4;*yFr? zy7|qUIDDx*y2MLd;Wb|o^<(9%gXDCbO5 z2EZH&)QZ?B(zc$SV_yWBa5p<50B>_N_2}>0|A&$IZ3yXVi$3$i=Lxc|nrdLDEj1P2 zXWmOiDYK5x!W%gQ)V{rXCXm0PN>*wB_jLT9TLbrh;h;~1J%vBzCq~aSrTD>tKk>Q zqyWXKzmM~mjn^~=Px0#SY=;LhzfEK06A%49#b5ZWcj1LH=so<#5)sxSjr^_w?}Cxn zVLajE@P~hovR2?`-dKua^eWkuE|#i{;@5(n6NH7oFy8PzgxTT$FV?b!@d|2fmYLq< zS+-->CfUh)b2z0Y?m0(N>DT%c=~3%dF5vt{!FQ(G@F}ck(RZCOD0s1h9KyS!#+b9cW)D4Jd!ICnjAtA3ogJy+eIV@FK9w!Y-EX3nu0u!0yI(aA zLmZ)`hoPk%M|ZvTaZcOj?dp>kF6ese$kY*CZwqeuob;M zZ38*k4j%9Agpi@v-anB6G!J7sq+%b>yP5cjm{H5OgH*ERPyZ3`zMkM~*eX-;$<`A( z+mfeNw;l_4e|_swfDf>J@ADv3EQ$C0mI5GzrSYBv_VN9Zc+Vp|&fYWoahjV`smls? zGqE)~h932@=s-;!IF5&KCSfy=RDxY+ zHzo^!Q>Y{gx_*hOen|~vkv#8`0op+rf zTx{yx%mTpf;}IrXE3HKg@9U8CLMjA9b8m|%Z%pxjX0k}+O~;UEUWom2A<0)aiKMqp zBnPS2cmJbE{>^kxa?RYMhZ6Q}|jDOk@n1 ztuWeMB?fY%p+X&l5d$&|x=j`u2G=J;21~?X@mK~k#+s60U~RxKICq+fzChF~4?{gh zeyc)5ecmiX{WvYJ-RB*K`tZW9E;iKLMJ*RKD(Qb;@)AS6Qq-4;I@=OYG?SXLcbDO- zD!ucnGSh|ph4IMB;VesUukn`DX5MDtefO0@mXisy#B!GP-C#>neVwL6l|r#C3@o~@0_;GQ%ouB{&yi~Yp7_f?ys9F zFK|<#yug2ojeHaE~%0qT>+#WL6&9Lb12gWie!WKrkuB}B<%^j+wGh0ORh)A+sK$R`Q zqf3#=;jkVFE1jK?(s818h;+~zd(_*kkjzKL_Sa+CmcEy38pT$#7MAtw$M+u0zvj#0 zHn?N=vtpHPisKV?Jt*ECH}HSv4u*es|9(74XzI?MuDw|#ci$qCZjrn)o+Kqr>Znc9)v305&sh;IiN3k8JI&njF z?-Q%hs#Bt}t1=m_cgHTp-lo_miDZ1~%5o{!?Gee={UTW;5|y?wvB|Z zGDRe^!o$SkwEfjbMbi6Ak^Gi}n|A->*jQZY8m$)RFN}8n^&rGNP04&q3_fxg2BWi! z?zq}3hQamEhYZ$=!5L#26p_Z!+B^V(cgFz#XJ(4zOp*L;ZN4CqtuKn?Ev_)!y)i{xKK(lDMRMOx&ZT?x5ecIe%$wSAfMMe=Ep zj5ek)matHn0Zv~Notck_JJyy8Ts=_?)~LB)3{ndDO+Q8BJpK)nd3yVQzWC4 zuUN7*wpt`NHHzdnT;{l22ThMs@_QRoc9UccB*qrLDF!-4>Ae{I4F|9;WUxjIel7+Y zg$VQPy)p=(nTtj8$FULG##Xk?$t=CwZxG2`k^EI8**@>@=hMxQ z7%|^`1qL~EF-9>Mi8Cxsor?30ZpHb4NYo)4W#=cFPE?i!{!^)UPDSQxLOw>w*`?J9 zI7W%t*{Hh;jex7gV2T*Lw;r7*yj!Kn^KX@a9~8+cB6$y`{EavdLTuuEp34k(f9Nnm zk1?HWPxySu;NQexrWlMaabY2KTD{{o3B6TCGt(jx-S{_3Q8pvw3S2s`y;CH2?-I$! zMN&DQM0I4AiQTwIB)$7Za->LV#*@euWHnN*dr%~IJS>tYUxK7=JV|KCVFnx!i9aBc zTST&YJW1$q!aDL%k=*!8kz57|BSb~Z0v*=Wv<6~|NClrZKOn8FH0L#)M@d!K@R@*_ zGU&SZ1gT_Esz_r(c0k^(%c)qGYOJVA)hxJXLie85*XjZkxvFUs>>wz%EE#`d*lFt< zqvOZr;GLNt8O85`3F`-M&pHc%_uJMFeq;8YyCRWS&&+%m9>YUsoA;~gF)Q14>w@QO zONjSe#YMOplmjJ_Y2}6SvIn?-j=`(X@>WlFm-4N?Nwa;t7)9)pBJ9CcpQ7Qeu<%6A z0(xYzp2u609Iesxufu4o$LXt)GzsSi;0!)BX1osxiXxfI5x^$Juo$qp@Mo?vXsP50OxObN(HUL~L}MBE>&rFaXW4MhO-71Z;YUn(r46slA098A#m6z@rQf7m z+!swEZe;fBjE|~i9|?9DI7VN?nT-*-@zGC0ZN^9I%md@23-$1BI6gX^x2sQCuyEA) zXu$&$86Ry+u3Ob~5OoIbz8-IK=!e}-VnWPux} zon*&pkG12plQDHUd5zPmoxo7sB*tlVX9itr|BrRA}Dd>(Wk$zaZ{cqWM*pw^hAjmRwH6?0$oBKMWZW?YasG+yk*hM5yJhz zSlmfjxU#9i$9ESCC-m^*cbXNbF*!k!+@TZR>p<1WaMTE-UgU&~wx{g=n25ucqKHqj z#0|*_b}Z4RrrVsAIkdV49v;FovLAB(S+3)(_BJ63J^?;O$Axy?qp z2}sAFeSxz`Rhsqz7gcIcRC3V}-8pzpo@z{K@~d-)G1`(L4xeHrP(@z~j~V=4t0 zQaw@Q>f{;%yPh)VjUX{=!8KnmkiUC(;?<|bx6TBqv#q2c()!EH`*|LwK2Fp(7>5&5 z|H)8a6H*@)-})*8iy`&%JP%Vp$&2&Qc!>xiu5r%-&Htac}c6v%qEnmFN{^_R6|oe3_d^Xqc-m^1lLwbXUjIi zb~Uya@0kQ!ydD^l%n^#h)JJR3?Hi02E+>K0_%>W0Dd$I@^t;Pg!*IpSIv!SrnCfT+ zky+2Lll??F>~S;ay6Wh2;0!)R{Zqhr1<_~MKaU`QeZSY#KR@7Ihk{C=Ww!pYk#Fvk zd0G+99c7-YKh?B`l-Wbk2EYd|1Vgyq$;v>c~$lDMN1Y&>gre2Em>Ms z9hpCGeob}#qEAKY!;rca^)(CT&2NY_5DlF8A5IIZ=hgEfFekET$%2NZwUMgoCH1w{ zONgv?1@U~UdU>R7dDXn-)x=p{zuNr&$;je)tCv?Vi14?#x*<}v!XVW1T(h)lcEml* zi7Y+q>?KQ=FODp)sW-5G(L!K#q`q!meSI}?yN5ZEdPJJPba~{gdCTkJ2w1p$=_K*d9}5XhFZNw=2tI4jS&#V)X)wS+lNI<>Km%- zz*t;YEA|Zw7Oh@64|c%%+IjQOiBuU-yJ#_q7siS@q9&bsuB~6Ts(R7Fnuai)k@|TH zs-*|AY6UVZ`cyS!)%8njS4v`Ks-QGmZS=cvUR{J#*3MfTsh_`Kc{RGNuUfP`LT(Ak z5GzxNuy!d)IBQ9jfpyClkx<_FU?Fy~T)76E=iq9=1&eBH1s7d%IoHLmV=Dz7_(c>Y_h(lnz(HCqUQC&8u`pkw<(T8;;^I8PHEPRgw?Ix zu=Zk3vf5xsM?M%PSoi5=*1qeOFFh-=WZq(OpKNPDJH|ZFfvf{Sx&)RlS!muCEj(-K z3X*E_cICXc308aQ^(cMbj28Suwrn^qHq~m`P;j#w#vPtvw%@+F)T*__Set z^UT!xO{vJtb!$^=BQww6xG{2=v!WoGJu~YlssD>-M=jE#8TTu2aO^sohd;oXHMAyG zQ{XToJeu_^A9U|HZS&ib#Y+Lg({BcU>h}LVyjkFg{~Z4G6Tgm-hX z07gg<>Z%j#T!-YfZgl(?>-d9C$6 zXS;6#`W^`v?U)5X8`j}JE{b9MlO=Q}AK)WPpLIOyP1zUh0lgBu*Y$-##mJR02z zZ?1!%b@1yBKH^{m*@ZXH!K)nnv4b%Rn!c}c@EQm2aqv|K7h>$f`-+42J9x;!859hC zKi9#V96adYWOT0Y3mm+{!CM`C*1==R6XDf6xZS}%2mk8ehsi79t#R;L2On_oO$UqF z<}SSR9K6}V=Nx=LtBdsg0tdh8;4dAVh(YRmjf4N>;0^~f4i@5Eg!dT-Z*=f62ctM7 zeLus&&pBw$cOv{L_x=)4cgVgahzuWN-IeGnfwIp}>7Bg2Uhl-c!+rm)gRd(b`Y7c~ z^v455f0Et_pW(vGU3j;H*SYWi?!MouaN_xq3*Y0y%W)3kR|8Zz>JcPQo^as@^^Tm; zDHiV^9GvZ7jf3Yoc)5dtgReL^ixpIgr_sSq2k&w4B?nKUKnriZgPVX#=a&RY|4tWv zuilaCfZj>h`%dH?9ZwfTk7Ypd+34Vx6b_&5g7End7k;Y?zsrR`Rd5m-sMW z>1rJm*ku7*1(p`%{G5G%w(NFbXx_6VEb$|n8a#Hvlj`y#;hUm2PyvAxqz)16kh z?M_4bOJ4A{8bo|mpeg^2UvV794Lmb;+X9F%@JHG-bmX&ZG~QdJC%ZWTloW3f|Hv{{ zKE}5mp-=XLECjesUv?&U0zjOKbB>q^6uU{Ut70zK=zf<#?-1-v?9I8+#eT)yvUju@ z$9sYQTjE=;777C=EK^UwHD7Vq#pd423_%qH#e+fNW_GTIe6Jb0@*}M8cZU@uZu4_F zO}E=_lsPhpy~^%tKgVAd^;Sf^YL@S(PgugGw=G@Vb^B6&8EfdS{1JP^3S5qOuQHre zz|28{Jqkl4m>oJ(qb9w*AEs;1wl5`={8mEQ75*!IY}$*N%`FO4lwwCJI><1#d{1vGn@l|5I6G)7TQUls0ypIb^# zGL+*!psg4@T+SBgj^kaf`i0w^C3jWaICSMRM6`}LyDFx07{zzr50h>a zGuhVcuTBKDpBUn(m|Avt^`5S;_Bu>4!9AqtF^4$(KPV8?;)&_;o1Qovy zO16c`m=q+o`MKnF`>B;)F40e6qc&HO{t@kVdK0}>Q4_g2Y(g@T&>C)f;dF*F-rDbk zX)`5Nhf}=($@C{lxh%`$bc>xvuU^Fre@P75RM-8kEWE{_WwwnloT$va!Pq#;kbU22 zEX5k6xum-7<0!fc6IDEATol()%&33QrXnaCG4%!x@~-7?yE!?)i|I&-gP-%(ncUuMNt0FNb-iitBCfYyHml=*$QSY?Ta&~}o?uvHl zsA#`4Iw}yAP{b@SMe!b9c(pr_{rnlV$y(3em5XK$@~QlsSbWQwL|lhN&8xI+LW{Uc z?3nYp*n=!{cn-{5jO~ zKda|(Uv#|ZCwk?gS9b1*g6vKy^Ed<^O|Jt{Bev*ABb|7*bx3C_7@V9}ILRx~QFYRQ z$EH-XYgLAr^FUs7QYCBug(Z`n37JxDE)*lZO1g5gXUMCVLJ?~ArlyW0CDMaesLKLU zoW2q&CdytABZTtIk4?oD5F2hlq=X-!>Ym=6h91L8|3Ay$jI`O zz(S)+rZ!Cei}>=+y?d@OksPJKubAe?X0Si`?6N0OX^rtP^#~*n7!=;SZRE~#yWh2| zV&YKSiQIWEMfs(s0NiTkC%uMyB16h(TaNcdO(EWmnRP36k_ua!vKvjx&QlVG%aJs$ z#C6>t>x*6og%VIq2H&b@eszJpgh%^>Qk&xZ-ubzFk00(YBJU{5DlNfITbE-Mn$ zZBEK$T?eRnPhGpE{LyVoig*R$$$5)TP;|l%ZN3tE$wGElacxDPI3TECq>Fs31}-3U$I&Vs?Lp2s;hlBP1*F^lxY6sw(g zMKpPgbYVaGxLLB=!xm-~{}AD2D>I63cTF;v!tzTnwJ*^r!n9_|v~Dnp_1Ot|gOh7U zkM@2v$Gew;=0&fSbKzR%o)7EQI^z*yQv?mM^23h~6Teb0iue(|a;69wa!`Uc6;)&C zv%IDvWM&K^;=SsXd|UU1I3u^F%SV+vzc|k?ndld)W>o&?yzCd}l7l;woiM}bn#s(@ zcuyU%z(B>>Yrb8(<(2zJktPwTW66O=s)c5-Oo5L2Qn6f$JEx*zlJ}02A+LBcTDt`f zi>b@itmN{Hj>+iyMZ)YcK=Ixu=~aN%si5u2n*r1RNGvr$?UbSEJ56(#)=ERNjRK@I zS%krBt%_}YYYw79oev12?-H>JG3qZ2D(((SuHB~P6n>nnFND0AsMYA=l(|064SbOjHz(b2U!j$$)U{B(eI2I<%W_7}cd5lNtwSlR{g=1n-FMK@^L|x!KdCjO z`*fk7IJThd8hdg{VSX8nl6dba5)Mn)`#+TRFDOaURpN9cHo`e=$GEWSqqw*DIc5E1 zc8%Ooe5=;#Y=f>;YE%I$>GEE)hwvFzyG*JB(n{Vn2PJWThCd~!J#DDGnL}!}m5fmVps(s9IDWN-`R zro}7wG891LP3_IuWtomok>SQye=-Lq%6craC8+2PN;nUU{N7>%tNIW54gGI>I(|s_ ze(m*ddvULycA#|0`CO>7EE9Y>r~Tp3Wm9kwriFrZHeShyT09X2$$OF=#@8iu&XM)% z6DjM~*~hyVGo0YXuA>>-bGjaFQ70u8GAz@UvXy~X*o#EssEUvVpBJ0jXDdQSBm#Z2 zM{)Ym6KhCvQVHEM&lj@xCxo)v9_%llK`-xqTF4x+X>V2a?h7h&+JBAOQY55Cjy&GGpJpclnK;^r z11m(FPDy3$#w)x7quRuWwz3DvUCQg1^woAY)PnynUL*c&?~n2z-Og93&nqwG7arg9 z5{YTXci@~p%}^cbFvXA~#6XuD$)j;%CtSV8AG5(4U;8?43JPf!PR30YUuQ}W!qC_@ zmGm{}c+-r=WE<((G@k^uQBak=@hyG%M8s927~rO|L$1(veAe6Ns*U+$*6BFq`VHTB z8=)$YJScutXT)5oo$<0yulAWkUfVNo)BRQED9QsrRiveyfnyHGQ zHu!2@%G7dc#My8R1S=2-32U$7uMwdvEU)-R3?!w5d&OUKiS9TLbs2x-Ecm>fpyWG2 z#S3H36UcHm$BcDG{iYLGZI2sNg5`c5aVcI*#;4@T?U|gaGf|#AgN*7AR_ z{iih3tgECb*2s%Ye^bfD-YbYtN-ulYi`^dYJ_=f-F0Cx5yt?8C_tJQ8t~?Ij#FWf; ztw%Y)#)L8Yr_WBwPzwCSO)45>s(b~)2C*sTl5>;CoESoePE&Rl%ITPHOci&J={Qs4 zf!gRx(9ks}i!^(sbY!fP@+z)JX1SJ7lR3oU>RYws@}g7x+T$WMJwHWqRkm1ef&xq| z2PN04!Z#kxQQc^&HY`o<-a2=`_!N#bRCqjiO}Ms7^_R((w<&nbYN~BC)JUhV`{zQj zR~iSYlOmkNWovxz&AeJ(T@!^4-y%QybxdQO%C&Cd%X!dPH|-Q}g7*$*0N-Gm0YT#K zAl79|cCZ`f5r1yASEd82_C~4pYuaBi*=Bnde&ROSG&Z?|59^Fg z?zCOGL@)VgY%*4Wb6e-ez_QyzDYZdtrnwvnDvt3v>x6XV9dF7nhrypntj4cn!8lV| zLpiLr0jG@%@Gye-*1Or8FkIby-ITW+ zx!NC+U&be!R_8jgwO%+V{0VI$+f-0PT781VN8lq1^PYrv((s;{S1~Q@a9S721&ph5 zTzkdXY8w|n+xY$Hy~t-8Wo=Y{Z8+9uu7ZASDE58hGH4DyG*Xz%jY^%q%ZsZwdK=;1BdfaZFI<6jG^`AJkm_+Yf#c_3t6mM|ck}*-!L?^{?)(ss z8klIr_$1&Pv~T^`?Kv`0%1!2J*3OwQ+wRZNXO$=0rp)}BwOrhRs79M@!sAvsDVal0 z8Bn9$cdL#Ev8E@JS9*{5}%dW&AL@~3ZQF$C0va@NBtB1GP zW?n^sUpy&DeLS)R#i&(+uhUp$X{L;grHCjeuBK(C4C$qkHy(BAdVw!_RY4^4Jo`44 znXdV47cK@3JKkO%*k0N28p+HtY=5NKXymaG=5l=u)nvZU);=<)8UD4fQO)kGe)tr* zlFgMkI1N|kLlndwHN*Rc$mWPw^%n&Fi&{d)$cyiFxPHkYZ-(D|DC%7l^=4cvuQnhAAVIValjFGGIUGBNrFD?_tJZ zt;(R`jDw%}tS}iB+HGZMx07&hSIchwDgF%gd`QSGkRz1Kml7gY6C+c=J}|%b8&Qu= zTxN+}1pTbCXK@*AWZfp~I(|(eU?FXx*5T3Yig$yOtNfF_ejFdjVNZ3qo5~ornoV}8 zzCY1x7!13qu7m1vP}~1(AN4Su(+|-(#h9}Dq^XaL>yM_5hg5&mo}6r)&s;g5KV|7* zu;17M6#)u0c2)B2%n#T>V}flp;U#X%Bo#yt-Blo&=m?56iQ6)LY%Xyq#vE?pJ-gh7 z**Ml5b{TV6RX;L#c7rhh-CZw3(ADW91`rmC;gVV)16U}(vD0>S=;mbJ)m{wCI49mC zC&{Rttd5E~oJJy$%gA-8ul$P4Q?b+$o6J@ILFT$L2#4-$(bt9O)~|$8hUV(t;U_-J zv4?e9r&`F%h}GJ8O+OcJd-YeWV_S}gl6@|(mjjpWa%Z{SS5!x2qF)vA!N8h%*CpUJ zCX!nXl(6#ja(YUv@QZ=Vz%K!gTrO_in-bYbxajJDE5R3iDCZjZH1Y1Af*bVyjf=c9 zzms2cgIhH*&M>oEF=l88Nutb{QdUs~hHLG4|N)tvC#f8=c0>3ELNAJR?4iB+b zN6Pv-)?uh?y4sR!j%+^xG|A7kuxb^$5xe4R2XD28)Cmh`4SwT0QEvWzC$h{g7{^|# zLfc3nsA3yloId!UBXN9CqLDtOA3Vw47ySj;4A=i6%1)l$Ly^HwaxZ0TTIqpvwhF(m z!PidEyca?d8Xk!*_uC$ctg36O)d^m75(!oH9nO2YMy{K#v9pWculoG&HPhH%jC=JV629E_R~hZ6%nsB8sGX!$y1dqLgliMB9~`f z6{XCQPUj<3swltD*4TQ91+}S#89_&ig$^c>|GC-iS_m; ziQym&nLIS=2-7&1Q=ESALeBjwo{lEF_Ui&9>GHtQ`QQ#r_3<%Y&Pg2hLm84VNR5p7I`zZ`I0<85*jyQv-h6 z=4cQz)1k0c%iT{MjYmk$Knh9eSEyfWnrE)NSbS$*$#=XG-GX&c2rG*ufaTwQOPK9? z@0bUUDZ39u{ZH&Q_f{y1ptw{+U^arh*xW@PZ?-2ZR{pXg%7z%U|1^wbNkSgIOYzyp zYMx!J;a6;PWwjiC5Dy*&Lo-^cCe=4l?5dFjRhicURrAyURn#hh!WUc)p1!M^wKbXT zBM4voLVr37I@MfdQ`>XBsSt~(?8GZ`y%X!|n-)t(#;om5lk?yuziIb>AV3qi{^IB| zzm;`C>xDbYbK6?E0Z$#-klF)zu~#_vNXE8+?Fx4vO{SNak5J zPlxNy%t_jC6n>3mDrp9|?)kc8#l(N-pSpAi*h9a}EcWT$j1x zS%OtEe_BB=KHmsh_+10ipXv^=~A2Xu`GP6$f0-NIOy;!SBQ?YFq5zCrBV`fyFgAX|S z4E4hy(sm8UX`{yrdtOSN8$`c{c4We{f>@qEsqELOc}AfXc|r8}VAD|9U%lDh6CEEQ zvPz{saofmvRnB}bcLBDiv7)!yUj>D9?HK$|xrn@y+`*UOG|ite{V&K@?9C*3$@1x= z6ShYQBiY$|AXh7;&Z#sc8~SopezFth=5K8Jn(u;zTx=(xe3=?ZvLY_JvujiAQZl}8 zBQRWnVo%QRlR?we(CVCtZbb^YxQ0AgLlNoJHmXX#PY}13*##NXA1TYH=r+ZBuU3$_bG^P<}7k(^YrNs7U) z*U;Z6jna+VI$N2&%U)tMakj?2*T8j6&*Q06Wk_}%j^S{FX>0o>GxB27q?y4dozl2H z<-vHb_8_o8WT(+1qjSJkQsq_L zs1zX24)JFIPYTR@NJ?eii&U?Q}s#!D@sjiB=q0|Hhp4VJ?DRk0&z;Zx! zrWJW!VQzXN&(cu3P^c*Fl#bLE9uP8nf060EU2N%%kz$8XcsRcBW0jWRbAEKzmob+NJqR$Ohvt;7-38b z_C&i%huRAIz##(Lv+`HPjEi6qBe?C)kc3_OdLnF{HMM_^Y+7R*wMNf0YOMmWxYc>Z zbG^ZHRQ#BJ=L~V8+iB7sB)&hYd>!c&Sv?f>5nV;9{SQ(D%uI?4zcYAE*Z$_=YA%95 zhL;R9yA^OA8d8|;phJ9+myv4Wp5cbrcv5)x<+bO(djSFtONAh0;-`)U}4>mN=DV#ec=2w6)|a%9&h zhT2N8&Z4vmj1D~Vyb?*WmWp%C%Pm&2lNSs(F7}*mY?O=n1n1l%=Oriq>h&Dn9QLv2) zl4#ICVi+)LoPbFxZ(k-rbyslXfyTDhy6d6ZdO!FU>Tdt*``+uCx!3Vo=kv3krP$)k_H4XMn?1MmZ;tifp_TM6I@ZXeT4X=p z)dRN#VqcBq=Z4B2L@*xn#L=FLXy*=(HeR6|8u1rWRKz`r5%(Zo)#eKO%sDy%xoM^U z6WlDVMZa5cgS+&99P4XXJ@uf8qR8gJNro&;!!&Tw@WAzqqED$K zcpXgI5;_BPBF2G`>m@j4H_SYa*-wvbzX&ncuLay z-)h`}Knjc`6fYbVK$9LO&lw}hy23K=EIWn$Bf9HboG|7PH()cYHMb-j$IX{xit;E_^RXnb-J zimb8pRc1^FFPJdjkPE8Dr@xD;QycQ~4Vo^PhwM)y36;nUZz%n)#QL9P<%ebz?kqcP zAaL#sJq7SF)-l9cbyRsjRx%Za&Yw|0s)(JZL1gvrkW2*}sH1mS)ft7Q2945z< zF4K{b!_kxDMLH72=|wavjojFos7FwyF@pM~5u{sXCh~Z|p4opb1_VYs#wh`*MbpDb z+mlMQ6E(3idTwlN{|t^Br~(tJR4>8&Vqara727apQPU;%z#Ww!&y2FN$9p8?dQh#G zMhkt@{%f=L*Y=q>AIH@Jol1RgfCR$!_3&*A?uX5GO=lNjBp%DuheH$c)}TxnfeCrv zLKg0Z3TIyHJgcGbEd`6*MOd4wj!;it!xhD_0gY4o&&PR~WV0Gwnch(PPCTxa6g$2D zM_BpsSqCosHB-QVeN_JhF8l|zOL%C3E*PbLwWiDO5blJ*f?uv$M zS{oYNl?^Lfm#zpbT>?=Rw)rbt8d~ccmak}VFKKRCk?)>5uCS=7-nL+I>vBjCSm9oJ z&GP2fhWe>+JgyiPbLP&02#LA#dBsb8;$d#}%$jMn7tWny+0Z<{;A+O>3%(h1t1h{C zj=Q2}Zgus{3J{w*ZgNw-+g)+twAm^i`I8&Iz^gKbJPF32X6Cf2X$wi<1peLb+RLhE z!e$cUKJ1$VF$fLIn^#`b=w8;athx0@cc9tb+JFpMwldJ*o_F!I+L{YhMh^2R+BtJ; z-81LRy<~Q!`{J1w&#k%4ec>E;ZRJe2L~PcDRWscu_Nq+3(7$BNKk{#tYgO}-#ZB&6 zkWw)5!sQ9YI~S4&nyz=QXHc*Po?^j1j`9OvSxb z7^cGnoUJPVSSH{n9YXJz5BJ((W_=btOK)MK`TtGw4>GX3OL+w9MxjKO=~P#267*^l zI6+?sbwRGp_6m^xQ7vtmaCABnO8sncOCY^@#PPV>jSQvzH)$as?dvTKkb1~N(3gdk zJcO&MZf^a&^n8661CX95|Iiv(+F z4u4mP{hMy+(J-z)3+|9U%JrcBFp&ByJkW!$_f{b3;6z0Ezl#X^KN0gM#eBDz|CZ*6 z=RvXmgV=u{_Lq&*_Dg__&-FymyF<+H5%Y({{MmD~`<=v8o9*|a|1*&OKO(~ar(e_V zMx*Z0d<+rhuK^jKx5WII!N;>9!WW)Y^_yRBS%L zIMeK;G(ju^>M;p!#4@}Dvvn_A;Z6$HQ#hH#)NVCAs+D&xX4f7#Qp;I*;I+4QICNrN z+yzI$YaXx%mS8S=YUzGQ&rd)Z?+`-(DwxKNEIS;S>c6_C5o&!{bqNl~X4$LFljk+2 z(d@Jdr`9yG{i=(RA#+eTU*|4Q{PYS$Jz>=&hSl43C~sW2@j;~BjZl5qxfXV5fpd6Z zi+u#VBt6o4G%q!95#CrF6P>SbeFetH`f!-fWl%1%$`i&G7-sSVIAsmm!zm1+b61(# zB8e~%yh9i}FsXcsU0r0Ho<}~X5SroBl=Ah%_K)x5~9L$ko4(BmQcy_of7gf6zoF*pM(Z%B!CK*?RPb+-Kcp1m_6Go#Lv+>4oJUqN2Jc8>PWJa@;kL zgvdF~f>}m^ub8FfSpPdNAt#tq#=i?Il=CCl4XSD6kSo!I&83Quq;Vk(L(dFKv%}_9 zOXsdS=50Tg5Ij?gmcxy$|1J>4D||C)sF^io&qOCSGcfK1i47DdCN%Ikmb?`xPmC1c z(ZF3l$9_T+?g>!UId=E98%1X+wzXpq(4LsvO!ky+cclUPGBo}hT z6HY~Mp~wck#ttdoZRk}{=2^5d!PMGWcJII~qr$09aXP(puc0i#Mv_HC@OSmRh`6a7 zt#)_6GDtR@s>Uxk(*B`<3d4X7v}$mTZx+)uyjdlGbRln-GZAK0T78U>YfSJXZpvDo%t7~S^(k!MP z&BbeYO)Jf)`QF>j4COLzntnT;NZA$f&nGGp$WmKnUEveykWGr3o#r`D6}vln&LN;NSvlqqUr$h+1Y(^+=sK>Imn z7JaA!pdyfklMi-buJ4L-6(Fz1Wl0|8`kb4CKQ%Mxob|;>zenYhAMMZSo9^yqrh!F= zb$_DD3hb5{&2H6AxZQypTNSK=Alh|3V|)K>T;DT_CFsYn9e!3VF-_ z*9dOIv_jQm8Om&a=(|+cs#C-KT(!;YWAYS7K$57+7{op|{kuqC1N=^_Jc6b<0I?*GdQSS?-7!sN>%*M5YCXtmC|{ z#Wb)?FfARjZtZD?gk ziLaKmlN1R0Vu!}GTK?ToGrgAH12sEROP>oh`w2^*2Q_I}de$Fzf*#8F$9EeOKgyBw ziosk|V6JPtKEiqX3zez|~ui%43B;h&1-GVO(Mu5r4tT&06 zru+}h;r~iRGYJ)l(=zI}3i8Ph>i;O%gfvmVmWXhAM88S&cMJXq$Z&R2j|ws!`9Xiz z2;M6A9l?7Ae<=8z;BLWJ1fzoQ3!Vg;r0WoL3FZk-5}YMiBY2r0<(kv~Qo&V%or3EG zHwiu@_>|xaf`1TvTQC{bnsiSSbPE;=P7(AA&K3NoV51=9uP8cCF+WjWt<5)Jnf&>+ z%cSBHuU)+O-0J(s?|weO4g{aB@40AmVxWx`Khc`Ew?Ky#$0LMw`m9P`7M3GCVTsU! ztgO4ycX;;GgXVz{W`4PG1S(P5ZSnL0aI1SAL7&rL)pD42k9*#t;T@jU1m3|F4n@sy zZCyU)Z3=QCH{pi3ULZ9h$QQLA3JW?0CBpj)L2jpN)?3hHOT`*YF>&Ff7le5Gh^Kq- zK^{PLRLPS!eMtzBa?%txXW=`WO5T|R#~G-&=uO{U0-*|c^@Bc9H3AD_j&L~E(i}iHdkr%5M8mXkr*N4;)1F|s`P zw~qO6>KwC=h4>FdHmaf*O^c<{U1U!ztD3k2QLB%7#>(sUj;;l82ltC&_Uu?HLyh)h zkrjVgzKiW`kUpG^?Rztps%Y)eL{h`4e)!`TPOZ=iTY!tb`pAx`CtF+vuwf(G-{ zN)NuaK{L3b;#HJgFsX)fwi!Bqjm|ZTVU8c_0kxRNg*8-_?z>>Hl~LnCKWu5zrui+qU*=j$2iutZ7%rfaQU?qCstmG|cESK7woUX`^XDnZiC z96FxiQ%fd8X@bx7s|#4*yff3L8EK|o9n_&=InATLuwoWYt%6VHzfP(u3Eb;9omfv~ zG6uLNYTA=ZclIVm_8qKvhpuf==K^Y#VW!^-bxX{?bN+#JPc#SPAYSY3++~MYKqI3P zI!E`n(P1MSAT?s2-NXy^`rzKkDuZpv8N$DQMwA{vIEH<+Q9IgxP_-2Vxt5XHqE207uF+=p3@}A!F$Yk0 zy6E1ySyg0IVtv2VRKcPGp^Or$GfwPTjp-$^8gGK0=JzRc&2hnMe}^*HhtKmDG^zk~ zbtWVCZm}gYgI=R=Rf)IK<$&r_SZB3URS_hASpOjp2k`@=YYwy8$a3MoV*&1y>ZP@7 zz0_51zop*=?trzo($`>p&0~Gl3tenjg`6huUA6i3x*1hikyUHj;E(k(d%90;Iam$l zdLI9(_SSzJ+l<~~F;6x|p_86rfJr945Jpf(f1tNGP#?VJ=xN62cta@QmB}Tj@H*Gw zpt1ito(-e-h#A2wtPjN z4aco`3Bfv{*5}kX+LjM%b?f^#uGvfeP3MabvOMI=6db-zuR_seV*bo^8%vo5NpIss z4=eS(oj{8QbjNQ3ezdaOQI>KYE#$Mju}P7Bk(kpRw&3F*?f7Gc6RkC}8`DlJ?l4wi zktRDsy0gB5q0`g29pf_Gokg2|ExY0C@9Ch|U=dGnNB8K>l(tQ17C)szJ}X>HZ(*YOzheE(bTLl+aXrp9$o#YPXF|=v)Y5+yYR2EvbEDLm zpl5lLhGow(w9-xblc0Cw$1te(0do`dGoWT&u>4;LHM>Ad&-HQ%ewP1*Q2X(-?5~2l zB0;|t>dFLt3)HL|mirr^uEx)z7lfKLEqf^A-@XN4_-K!D%UDkX1zN#ixk+LnEO55o2mDlONyuM2)8$aX<01dj@yga$_Y^8`x;`Ro+Ut^I|YMc*m7R&bwSpWr9JWSq;g za?t+>!7+m82o?+a1g(99D@ETb*dh2WL2Liud!m0t(AqzEUi7aCzA1P_FcotZ#_Mz< z^6P7&pD8#`kPpDo-3r0m1fLXqR&Xyc8RyIUMgM!j4+K9JRC4%1kE=n_oh?`+xIj?J z<%j(*pl3er67#)+F~Ro*j|(P`)O1G+o-KHu;B>(Y1?LJb5WGULUa&&X9W)n z{z;JY7N+Yz1apbmsJDV%!BW90U@|(+wbaAkR>7q-cdI5`zp~}?`!YokZeCcx^JDyD z9|r%Exg#^FxCE!o{QmL2jGY4wKK7aHSx>w#6NMcnS;q)ndWS>gG3kqG*hWx$8C%p| z2ByB5XtLO5c}@FMTNzzyCWdX67c~`bYGD4%%?!-acEb?cGrMTFP2KCZ_A-PEe4ScQ zVxq71EA(~;LU>Y#z-Y*bcg%hU+}{5j2A&lH%3=*sDUv-3&yj zgrKi%aytkF@6di%)7c&BZaeH7>5SV&TIdL(?Wc*BA%NUc`zMf+gT0aZFnmIKH|$Ay zKLCl{K+qhXh~HZ3gn#@Ybt@MX)rI~o>QWUb*2BN=6ztywa1!5weU~Qe;WUE7auNLU zA_AS;I*QfhD{u+dfxc5<LwY(Hq=7`UaOwB*bliRH(#2Y!t{v4G@9Ud?TCStc?as232imWLKUHMg^zI%c zSDH>o>3#?f6uIhP7e&yujBCi=xxp+zxC?Rjrxros&cwptJBSar$r$|Rk8Vk4^g>M= z?6~-fh$(S0hIh1a3mI=IkFTz?-;QmO^((nlk;_}BDbDiJ-#FHjjQ}~j1;h}82MGE` z2>GXR>0?J4V#5YZX*eEaxnh2RSvDFfi$5DQ>6-$39qTUDMDZvTZ6nsEs>Nd&L4hnK;2~~a9 zjte&wbX2wpl9fx1PaRz!q7R2rE*!Y9x8X(xdk}ewS*<{{{D@XrBF}@|D?=1iX6Q}b z(xbSBZ+cG&o@~4~{$oU-+R=3cIfH$lu2-0Y5Xxe@Hg_LfxgBMr`in8-ThF%R&y|Kh zH~({-D*FR-xlwnH~4bW{nG2`*~s<>Ixrg!SS&)s-dQdqmd%g?Y41bzN(A z{S}({dJ)UgCR|TIS{I?=uFH;hRS{w@_yS|*K-F@*Ut?Dw=6bC7%AkQ7zqXtT%A=fB3uaZ zi}0o|iWw9fQ5)RNR);k5;#3K8xd6=^XWnxVCFt^aLlMH|K0>bd-;TdQo&@--CA<1B z(~kaZ&8{dSrjErC2~~`l{a!d%au(`3#>13Z6LLO@BYu6&s5Eblqx&z|Mqow7y(L`S ztP8#YOVuKrAIFj-+fn89W>fSs;g9KFh^dxEE&WL|301KQj@Srd_8o`bgQp{rJ&`y1 zc(tOj^rPrWD9Ad<5#X-+%g8l8nVN>J-{dH-mC|1njPJjPkI3&1^F@xX_b^LTk%D8~ zx=m9tgMdLe?_w|%r^PWQlpe!B-sJ?it?*!fCND~0D?0>d5chQB?=-Ixx|(+L z%o)a+Chx!n4ClQ^e~G@fKFq~~tv?&ShVUG50}Yol&On0!&r}4B3$TfB5as$(-*1pj zF~nY#9v;g$16Itxe%@G*^$l@T!4ig>C(-;Ohx;D|2>B)4Gt?XR=BfSMLHs-WUlo^B zpK$-n|8M3~`M2=YXLBP#vDuzM#*nx9mx3<}zAX3_Fd1Q9jmkvcWV$79GPkOiU-S9( z;}@RmPxR3=vo=m~WT6I{Z_`e%P_d-iKs!ALMkM}1J1q+fYcP%pAib6# zQl__fR#T^Wo7*sb>!UV7w2`cN<@{dD@Ih!?^MbHi>|!#9A)*P1Rm)s0@eb2|@>r8s zz)iQ0>%)4?D&d>US_D#unae^@;er;vd8iLUwQ9w%O)Xd9+fUN~W-b_}L8Xjt@6f07 zra7xG?SC?e!Bmc(10SZU=;r?a(KF*)TA&a%qfX z)S63-oII-a+KvZytSMY=vNloAy2;b&$AQ)JE^TBaVEm#o-lA4SJgkmFKX{x?(P?kN zM2l|H2iKs%+N6(*tp~wURHESe4p>nLKLrP6eso|$mLCS4e@X{w#z-e#o$aXz=o2kD zaK(vX%zg&a3A36?eG5GX3B^)L5_4_!B%PW+*25|m>3dIy2CKudR8mO@p#)CAAg_qW z?>%Su;mG2Spcri?H(dY(-yyGv0M6dl&>N7}IIZBXZ;pzJXIvG)Q=*fe2 z%#>#0RT4YG%}_<5E0eB9$k&+S959@}GU|S1*BKSfVpvi#9KOtw)vZ(@GCCw{>C9gL;O zn1$xp8OB&-Nv)~!9Bj*|Yz3Ot13X6)GR53F8)q!wFHj@3lR3$%IG+OS9)W^~<1}FF zxF1*zhssFNu>KNWyk)ci+_LGg;lkHpGq@wtTkgx4xIvCMSw6+(i=rLv; zb2dmsgzLt;!O+a|6pG_|wEJLN2g?NFe!xhg`RFMvU-7C;Gc?OejvrKgRfykDL-xC& zW~*J)S(b$DrJryf&Q9N(-2GwOR}iTJ^JE$r>38*Sq$S@vQ2Z)&SO6E3lwzu<;^=;) z7mh2;QPXhPZ*lSa;h_I&Gz%?9LD(+5X(*y~@Kx$fb9BFe#e#}B#dV|^C+ii8W^mys zE=|da`UD=v1&xSFzo)QjxcUm1>@*q&90X7Dy|e za49%W*ox_#m8oWsr9d%Xpl%3L!?|iuGO!zgW8QEWFep`A-C$X>>meAb?GA)p=Lt|usNvja1oqkcM}p%Arn*PL z)t5xOp9>>I|F2PkSj=rDs2&(pT+vfi#}cSw-&=_gkbkaN(&zf$mE#V*C+soBRdA*| zY9p%vYNObT|NdqYjzkV0eI38Y5FHLi&fR8Uw_Ur?uKV8vn}f0q#BIw+C98x~zC$L6 zzq9_EKWFv{*MIPf@Bcx*hu66fk&oRAzu{}Zzk=^!5&CIPhtbWbs+I;ECwgmb$2CFX zXBW-zH6Y#9CO9N5ek@JWxA-3D?#rwJE8)(fK{{0P$C^TV)>l0&ewJMw99U|)rMoY) z2D}RH8Z6=oG)OCP4fsGh)_^eN8W2}sRsJ!rhpz$2`g1$XtuHZ7mKib8{GV9^TG_zz zA>YBjg71Mt4fCAzSZ3}7{S>Hk6ZGU;VOd-5E1+iUv-B52?Z?lGUp3TpW9b(_U5TIN z{tBq+#Ns3UVbBkvLfuc*k3>+BtAa7X zzX&FSwxTciRYB|if?xEtf-3}X7PR;b?twlTd;<52`GbPb3GNcyD`@c@ydnBO3LY2y zOfW4~$1h9pEWuTRJ%ZmAr1&qU=aAr0L9TF^ucrcO?-o2qaF*aw!RrNY7u+iNL&4_+ zExv<4h~DBiu=oseaI(tq&Jmm_I7u)lc$?sV3C091zJX(+CmS^BDLw(9;u8Q)6Z3Bh z)&p4{%S0a(yj^g!;17tW+H4OAJ|_5c!M(s_$Q8&$TV}XZ1?LK0F4!WtO0Zq8G` z4+=gaxLfd7f`4Rq7(iDnS=`hxxGgn!SANoo`5Fp~3yZ((d+v=M-NA6)^F@3Oop0A- zF2(B}U2ucBRug8WZJ4#Lh88;_71%NJY}&}S3_S1}h6g11R@4SM(X90FK#A|z z1u@imoFttTx>L#Y#ldJJ4dB^T6U(7eO^l5)EyWiSZ5z?w*cwq0xH58S>9v+MUjW~g z*}X^KKIZ5d%!wRPJONKqH$CQj!C$&J_z_;z()0e5&f_>M=mpKr<90{aT}qMS=(9Pb1fD#)q+r-yTP7^Njt`s#lMf?vB;HdYmtM{!yj3rjhbmbX5p= zTq2epJSClpH}WYyz>-&pWl1|ijjy|*FBzu)Ar@YV_SefQ5J#-lDA2N+OVQfu=3S!2 zL3ZwPA%5QZm%^%mYr=)|pkFvYZtd>hg9E5eJZ7TqDBz@@RFu<_c~JRlw8#jY^itbI z8g=tXuDvEs#BZkbw}A`_&slBGb*9YC4As7b#U@x6MC1gvC8!_+yM4ASP6oANC zAd<+}pyeOu6}|!we!VclV`+s4BS*SlL7CMKzq`+@jAgm;Z{%<-6;5^GpDimmLCJs9 z{Qo@Zv-iN%W|q^zY((7eYfCqj4)SoJ<#FMlcSq0nL1lwkOy?Fq0(vM4P<5B1XakAQkp z)KMDI-sQT>`bXi~M0`SC)vi^f>{F~w-YFCW$;)TL4%1rz=Lr!qvN#5yJz+KkuZVC;GU zS&I2S`rkk5{ZYMbg{s%RJ+ztqzoYBxDC+71UXQ+Lg9}5EKS7q{%aOmLw|paVEOG=} zhkuJjVs#KO3I>{(TFfKui=tzt;6}3|S$revVor69sjltJYdiqcHc4&hAlK~(zj(gcX_m147qgBMhA@xK&g@K zyp1ifX&xVTYtr457MwBtpeh4HU1{muRS+#_fd43|5~)Se(^OJdo{g~Te5c)lL8AoT zg~wy%XkE1Z18NINZ~QR}=>5xQ0mf1EWPbDOLrZ!lGf76g&h2T#__|`l(tzz?6xQ?@ z1f8a#*bj5qWu;M+eciZGV^twX53kBajtsj`rs^FsvNB|UY*_0@kFR?GLsj;U3OK7` zKNc!`-zh#nzG^W#nf;ey^KE#3_w`?a zZMTH+3a_Zyuq`?mn_5H#&wJ~3j_4WoLq_e87-_c&Q3+i+U^qWBkO&DmiCi8k;{7AY zx`B>$+t(OFJ$|HN{Alu&K|BpEzhdHMTx>TlO5=cD4L4|xwqWt+I*4D;KN6e!1^olP zqQqP#-uoLQ~~QE&Dm+)+XoXJyuh4-N!;_shmzhA$Zjvk5=f0bttPS^{Uv%z_{^nTS6sS_x%T=? z`+7+=xG&b%xhqXoUL^|%TiU9;_?EJZC3Z^TLA(%rxjmfP2=^>52$IKtz>r@!HJ}w-XCTvm3>}i?2A>~dQDBs}(ZH1)a{5Bw7{GgeIqfho%A4@t zQ3sbEraxr=l{wY0d$_NkMJwdH7nXT!v-a=Cl9G&OsJ!0v^4gF{Ni?h zw%LkzK$yYd)YWjP>h!q6J%tAo>UH28oXez#jk>ViU;2LV10#!0k;~b9!gJ80mSnBY zDYnuDl8zt>z&+VnmORjY3+pRi(kL{FH<7?)nCu)fa<?goksR&<5JOZ3FG+Lg|WCyo70uk+l{MVp$vTA9B8qwS-~@#M)*e(t#QIyd>P@ z%p#TGPmrP0kUiPHlZ}kkFmd!OWTmV_cr#R`q@hawd}D`7eBr_8xR6un3FW;`zD)4A zMi+`~vJtq8X{_WMjM?T)The|g@W7(nUNu|57hWTYL39GC=my4KqFhPt7 z@&3k~s9Z6F>_6sspfX6$2H%w|QrXerjckXvttxA@@HS=$4DrHg)cd+09Y4k*l`*IH zs-(Xf>kD0)q|!b{7hP~etZ$)iIMVH7y+DCU@%|t#TMKVpqIN*6)_pds37vvmGK;B~ z`K;!x{sRnIcz(g0)^x=Rn&S(8QDV?xU5I$U&~61fR8hWv_ngiN`GTaB$eP}sG82` zJSnG9n7|&<5n!zfxsGjR)S{zH_s9ODmmWGMG9Y6I3Iye+${Xv(Vv>!fP19ri%fYuX z#;aKB%yjyYnfJkoqvy}aIX#BP%MnSC~D!p>_1^V5yB{bEUN+{dJBgIzGebyFn^1X5ZhpTQ37};(il@P}xm- z$l)};;%zrFNG|w`=mMSHj&AbMBNb)QNs&*;)f+n_HbR)+(K1>T7Dnc{WLq;-Qqr4`^%PJB)#rbXNPg=LsEIQL9WI=gW6-L52wBc3jD&UefVc)N8Sy3$<}LT&;;euE+XU2 z92$h?Q-~uOXWD$#=`qZ)vuF%?@{}|KNdmE~G>{3W9)l^rLCTJvov;xy!k7>sW5NjR zGjJOY`wR=s5~EzPuC>_bfKR&E3-eeOV`h{`LC&$h2TYud0_tI6`!cg=ZDNrDj*WJb zMgQ!Esy1eSJ)uri!n^JzNHla)5<*dwgU_-rkWB?g*XeZEhh5awAR9f&%wViaae}Vm zvfHu#O_(R%!$3$T#Os@ofoR&p+)m)Og1%wXdzeI2GE{$PE)?4^@bk9pi#F2d| zV=aCS6@Oj_qO@Y!Y=8)p0egyB%_4=sZ+A46MKhp^W+0X6iRI8lr_EG(UHYDZ$BfQHmC*$?gIVp%HdNs|dd^3y z-pZmyA9D)F4*IW-e4?kpnw&QL!WDG^%7mh2z4NLU$gTiuq3jA=2PN8DlWuR=$gG@i zlrxatbo>dYh7-9}XYB%HA}5-3n9W!3%qsDnW{ROIjow9Rse!~ogW)@AKQaxRqKir*@@CdOWA#B z4+J0~X;=X1W&!tgp&OE7&X196)!e&c?l^jQSrB1Yn>VL(i?D+;ov(Kj`gm5GpJSw~<(!Ar-1;8}GU#~tf8fFPn$ zZC;;-od~zTS9wDx%}zERc!`RU56KRw$#kgVv&8-cljIB%*E=Y9anK!nTlaN}G-jQH zv>ZJ@hRdP(ugja6Dvdhi9AOrLlk9gvo)V#SM@Yf$PDu{64CoR?#VbMr1JU#Y>}w7) zRl1O;=t7S7??d`K)W(qgd0jly@wm+Uv1~*2hrzzk+$2>P4nuY5jvh)}iR_^n*1Be~ zuA7uAhLSGM1%lyKW$n;f9BeSBsEMBS{*WFHoM(U%0)HL*PdlTZ3lW0`6Z z=;-+f2cOknTyJKS?$aGi@JHY&Md0d+7J9_!H!x=dqZy1Kj+XzbOw97P5e`@zjbd^~ zL*y*~Gw94Hr!z-)D?*H=s_1NwW-uubgZ0-#dr?<%SgcZ)uvrM!O8P_H!C^^ebR2C< zu}X;LNS(@ctgq+RFKg7WV&4R}QtVRRyw2;~gw26vD6hJZS8X>0*Ru3uDrMsm>!-hX28LnC#tsPs@fK-YOCv9hE;9LRkbZv)iy^}+YDW8$(iwYt1Tx!JI%^8 zT~8x>Fx;s6t7~gwSmy)ddd^SwKTOgY zsP~}tf*&%)vgqmG^$}zWZ$7ZkzLVZHZrJp96}R| zjn$DGD+Rkz4H7djJ=b%a!I(sj1TW;e-Pn;3Bk#TF;14)@_+-4w%d$R*jvsQ2qW9%v zXL9EGYolCMpJ#QeNld7t=N)F6RVf#S?Dq_t8e=C$W&vvY+Ho<6)UlHal^rlo=MnJG z^B|cY<8iHo{dmr5?9X#Q{u6A=ywCl3H5+&t`{PK1@ha+w2ShRPc#0DbY}0yau3X{r zVjXUl><>9##d&cu*wNLonC{*D)_jxe70fg^hYKFi^{&?AnT}Ga<_3jsxTm5D$h>jy zfnrWDBYPvCsP0;=Cx+%BZ)nO<6dyM$Ks5Lsm+R8``jBPr0)51!CX+ZAtTr+jJOuOc zU(F)oGsz}T-6f*QjEm~ugV@z!af5nTiy1HIH{#mj6xbkt%KL-Fx`T7_+5OenRzw)6B!U!b{yZiBQ7H6eUJ^>~6S*p#MYEfv_d`Hg} z$Rk-4stF6@I4$P$Kj3x(ovK51HhUhZ;I-Cvo{zA|;2P@_Igw!5(m)+YsIoL_ zpS9t9LfooOSGU;Ef&s&&Pb9KPr~g+UAjy&bH}U>A)|k%_)GSc1ANqd|1H{ATwpF(_ zFKJk@Vyc^LI(U9dz0jqV?)v8C4f*-`wtV-rRSm6+uW4}CH(Z@>t6scf1tpc^(JuTK zPR=iFu+6>d+I&T#q25iqlKi3u-0Q&WUW)xk8;Xab!AG`UdXeAT7ffAO@Mh1AGtd8j zk#E@bRpzArG6je84L_6zxi8>p#j-{9kai&Ezn|k9u1larQgp{}0)BLl(L#NT;I0Ja zY<;xjSAZXli>SdzcQMc9+*QU@wzGTmr;~TET!Sn`X#%TEC(I4}L@x`ia2)({@ZMK)Kz+ji4gl_Ol zH2-J#h8ZqD<`MaP85b+hsLzI;W{fBGiQkF%a9wcV#LsfiY$gp$k7U{C#?t3Q?Z(dv ze=^j$_*wQ-p)N?UXP%WL=x0OiPtebSx)MLjKQ~fY_LiQzY}E<+B~aHU=o_J4h@TZ7 z@(C}(&+>l*)b$Da4yftJvhRYrDM4>Q-GZM*Z#~p)3HEnF-JYPo7wXjs`um|KO*?+f z{~rO@Cg^_x^#=Sb`jqdnDZ&0(sJA5OIq$kJLBAJjhGEhFHPjE_XVD{<+an43DAZ5l zXSsh1>TL=7x1rvipnnhQ-S}DbK7@LIg8ipZbG>ZQ;~O@833?8s4AZjDfc`M_7QL@P zeJit}1C-`UX$R_mJw7(mAKWIx_mDJUY_rrWV;?}#2x>5(V+PnPCC30 zHhA5#!hG)(Vo|=g1g@w@CJ>7y5yg^-;vzEtFH@6`&)>3#)ruv!LZBS2Sc2OEqFUT! znL$-Z63U&Z;7(MFo0_k>R$DJ_YHeJ>y7dTt|G=#%q{s&+(24F57 zlKt|20_;D5p6<99rah;@v>yv3-EzSj1bKW#^VbANp{`M%C1}Y>yjk?S1g-lCpNO6V zD8o$^bO0ItEyQW)2Z_@mkAU`|{|5Cl;2vt~&%rOb0&h2p{wl#HLF+w)cG2G|_#MGs z!AAvm2)-itf#85(YN`&`A$XqPbir!D#e&v52+Kv^CKwdFPw)qVy95skz9IOwU@H0m zrhBa5R|WG0rwEn{&Jes%aIWCxg4YUm2tFgYT`($`f^LuDT5=B06@38_p>=#VJ+7kowVUBQn9PsR91x|0OIE*KEJSMa|D zcL*L9d|U8+!4Vi+N$+cd69p+-n&wvst`K}t@POd^f=L+1>E0ojC3u#gUvReIHw3>U z_&ver1z#3?T`(s2zF-cj6!9NkvM*b}LDUj{Uj)fT8*C{~KuMoUU@D9NT1$PSW7yJ**Tj+0z;H`p> z32rB%?Yv1u_cTz96_?@J+!F1V?3Q|6>K+g69fO75t{) zHG<0oR|tMbaHrr)g6|1_ESNt=hvyZ%Kyaquje^~Rrr=$Iy@LNO_=Mmyf;$Amf-eca zD)@WBV}hRwjyz4rYmA^va265yUnTk)(Jv5e67%Z?R{>c*okWz+1~Gp~aF<|OwhqrF zc(!1X;5P)X7Hkq+E%+@#GPAS%pA&pRaG&4-!NYX@)0>Mhbd4iV-HVL*0wh0CWZxaj&-Y&RV@cV-QE%=P!4#9nb5y9UJek%ACmyYlG zf)@*3F4!P=onVJxx8R+EKNj36cueqbf~h$=oY8`(2%aT)zThmuuM5r>Tr9X$aFt-Y z;CjKWf1f0KEXc0LxOJ! z{zWk3t2&%h1<&NV4Dq1Meyo_A&!cE?e1(MtC8eb$rIQN3;J$5X0i=oZ`^R#`x1b*I z^Q!;P%Yjob>83yZ(F>29|77T$C+;&gb|qm&{KWCM&d0$KJmI)c{nLL;J%RhoHly#( z=N@}yOPNSXdeK<38lH@eu4HVQz1R5rwNJnQ`c3D@Hnz}oa~uC3ZO4CXV;WpU_8M!l zfyQN(PgXv>?XG8@dG_^4Y~uzvi0lck*#zVlUb981(*5Bz_r=X};qeOS!fWowKPKuR zdK{|AzSu?^iN6@z$oB{$FNW8+ffy>_)&mv4@U&d!M|fJEh63*Fgs1V*Mq6y2-?+ZY zm{)7et9$zU|M_6abH8gpSgvKDM!0kaP2;i>V_LN_ZJ}{li!ra=*t{CXc(r-+THuq8 zn~wjy@St&hnQ?uL| z#%Ase8k^nte`f2JurH$7JogzJ)%o9}-1vCbk&q)J20wJ^bIuy+mUpcMy$@CHQ~tX6H|4$2K2Bz>&S-HOGPc!fXDj(2KOI$SX5D zJYf4BW|>MwcTT7^3CQXRTk+|90N_F@G5xMuU6*O&kGNAJCI@m1CF^SCjW{_&oNQ(h^v`CZrH6y{9S|}-C3W__6NjJ$pT{D*3H?khNjTXG=oU z{i@)2!Se($Jy7;%BQccKnkzU38AmkjtHZa_#4S(s!A-k4-I+2yGgM^*lTl{r zMcIyZrKs$%sW)#bnQ&9)Y&>F z&Gy}#9!_mTaQwolwORo-m29Xwz7~HjuOw{0op$=|J_??1@>QlA>Lowi>u2uFw)eW> z!Z$n5v3{d+N-bix$h3dEm-Iqq-S}tFji%k#>w?->Q4X&9iy0w(K^PC)w|hzk?Dy&? z1*Z+D*cn>BfRb|QtNKq{@MU@6p4WPD@p^*&PM?Z=`v`w86M?(?ZU%+q`zrIa%n2qr z*0~8)HF8AXYUOS60~t*IeDH07Yb(<3S#>p-dR9G=el_lU1p3eh;2bhe4x*v}S0S_( zeTOuxE_>2m<^J?co&4z~9k`;2MC%CPDn2F0+HWmA-kv^QC1yZ*v|@;o^IcRDe9=_5 zRBQAc3U6m3Hl3d)8GVj*8 zN53A?$X1L*S8M5iyg0&}=@!2clG$WVQ!MH%9At|vI8NSXM0LfxwlL8WCjCft6Qp;j zQVbr14#iz!&doHh%p1RZ!j<;YzV-}rfp`4g>9DIc7U2F&9)06eHE9;vBs^5Sy0 z7KH%LNzf@NEn9QThyg`o26%Jn9}kA$iI|94PQuI(qwIDqYY^OrXCs2O19n{Lp>54U zyNA4Ej3K%BxG8EYRWJ75GD5%HYpRrhwi{L~dfzut!6soU#8zRhy$?CbmoLx3elYl; zlwW3op#qSb$6598oOq!&7kYkXTgikrb*o@@Y|RFcTA=HKu9UtBxG}Nk`7iimlFjP83Dx%L zAg10X#zQBtn2);b0oN8jBrUS%fRgv9sxCNgou2uP6Ja%2d9_r7ILjPmEm&1}D=V3* zo44JM)CSW_QyC}6x;hNb^BONAoona>!ZEafhlOz6CU@u5eQZOO1o~2zBm_K5BhKrn zlm=YKs%h$~X=af8jyEY;q`EZyG{^dfk>8+>?vWM6w$+V4zoMaP#`L`4lTeg}lMy|1 zo{Vp)BGUd9#@oDC)fU|Rm`g&#qHv{|MS5_!$aLL_oURVrH^TsZr80NjNpaJ75yW(! z4yZB!i8j~?f^nr$cDin6vyF1)4{tO3Zq5sCRYlb1)~3ETf5#GOj_kf%PfLI*=Gvp{ zX$FYl&`ckTSXCM6XG(y$Qeu{E*zrd^c3iFlb(Lx>h^&nVI^Q3>8{V5Rh;?trwRg-E z%(1+=)`Udy3#TqpNL9R3I!9hR#;ugF{Vv*@YIvHB2sZgj^;i`=K%KsD9ryVS8NbL_ z?AlnurfbQK~I)s5G}P**h!+q!-&1X<~Akyq^^zUQ(e)4@BNdyJd_{3IlE##S0g zT0zCI->c-L=+|`?hFnSJ=%ftgCViwaTG=r({pKiKYE`j%)+|@{kVIsFlU!xBV#1H- z$kL%XBEl~4p!tQRzwRj4QhUtw=MZ7PiPwA~8CQ?I&Yl7Loqsojo*$Y)tgfFMUdDrN zlj%s&4*)}!uM9Fe(KCEkf-74kdym)}BITcO(B)|Bl%O z>R0y*$8X4* z{!5RmitJeb0W?E-v2}{T1#hO`6Tbm031+E+VbEkqjmW zGZ%tX7Uy$_o*%C%>zoQ9r`SEBi@43m@X*~;1g3};^BLvdH&@3y$konXXn z(-Ml%D{ZAqwo#uNPaQob^au5r`tbJHZJbi{?WsPr2eTwC!X0xY+=L2T4Kiin5v+sy z9(laOsnQ9w9v1?`yIGY50pVHp_EX?#q3Igqs~dy1sb+(EBvoWoE=*IA!R;~CD?E<& zP}Vs$DUgS0ZsQ&X2I*cRUkTR)Lj8$u9adJ}<*x{Rex5$zx_{*c6!9Rf{oTYH}Zx%CZJN^%c#VSZsuT)LN zUU>8u@K+M9(2Lc6CE-O49sO@ns19gCjz?mo96e{KYD;pVbUpFKtrFAa@YOl&m|iop z@=o?Ooov756ali4JWf}>Hon;L+`>PjqrQgU?zv0 zA-F@wnZ>D0;{;dc43+Koau?aLE*JihVE8#z(aWa3d=Y#s_y;q&1&#aQ>I-Lb@!_#> z&y!%^@ngZ|#&XzWDmgJ+@o?@7^rC8BJMMrImV=U4v&03BNB$n|_$;yiEL?UN6sgl6 z#07_NY8Cu41NtxJeFrr;HPt#03w4UMEMVvdA)Wx!QwXH#+Y_zFMfogxL+G&MZ918P z@9o=z3tD^oFx|&YTHWDRts+%VTw_^&m^->s5uP?Nl`I;IijnR?Dz%U{_+vnQyJgak z;;0QdH(?R@aB|WPL-{izj)zB$=e-j|ueS}f=bNL{szp;Vrv|F@^xP6GR&GSJU^(>si{QvmGk-14 zM=vMz5CkjMpxTtOO@p7`MV+AQ}CY+Esw(~RpIN2Zjr7Of9a8txl$MoPIXwK%z(92)bb97$&06D42dyrgf|45O58*nT zZO+PNS2eV{o3F-gndK`QvopB40`3(pi(BEA zVy)dx4a=_yG`gqqZq8Lr4a?k(i&wapHw!s9S-EUEFW}gwEnnPp;|g5babLf3aeeFJ zz|tjd@TE2^2`pXJkPwfh%j+9j8t}I~;J&J<`MMQWJmSGGU5oWKJnYC5ZE~Z<2*ecyUT`T#2*}v}{xfOv{Fen$<@`s!}3~EaL_s0+?I)hjMekkix zi(qJmpF!l&0Biqr0P7wm+3GA{oai}{^4pX^i+Z}_HvvC(Pl@ijXdxf%_*t{Q z*zsB)mHeswYoO)sJ|`ZwwHyK6_|e^5LF}Ze??t$?!lD7=$6pQvL4O(4mH1iy zuY{U@EPVsi)d~9Rpsr2OuY`JGg1!T4)-lV!0rjE;{RXJ(6ZCgN-I$=i2kNE-{r916 zNzgw8bs$0iQ>fb#^iM5Ey_Vo1 z6YAJ7y>$SEizthiEyYEYW%amqQfONi0Pr3NuYvFu2rgpa0?JLefU<0H+mhwDgwnKh zMPNBzVj#Bg7E04%-a(P47~lkVP?jy@C6wh*^CHJ`jvu&%g26<+)G&n)HZ(1T8&F$P z&+8~n@Q3>-bcpLH%j=;lvN1}ykAiz8ypf_}CRbYYr4(F7nWFAu@GR{rs+P61E)T@7 zeIU)h#V;8ZnfIi~D@EP`Vv!)(3aFnYI7jeG!DWIs3w~ShyMjLwwC-f=5j~INN%yyc zQ9&Nb)BIz>B=j-Vlc9z9HNir`IYczxdeL7ic$1(Zc()+swUf?+f=>xHjXl`viX=_>kbw1dj-QCU{D! z_J5k-S%Omq=LudRc&*?qg4UZBKcwDcvppu}JBi5eu;43V{yLEP{}vJXZ{0olOzcNt zGnM`vf~N!N-a~}@b7_va7YJS;c#+@&!OI125$qJ)BKSSQoq{h3{y}g+a16Er7+#K` z^(MyoqQ6wIMbH%dj-Ykt zcavZjkomWsm}|5Bh~}WjV+-Qbg4+dO6g(t&MDRnw&jg((Y5#7)^90KUs|CL)xLEKS z!8L;45xigU3Bg|oT6b9bME^&@zX-Z9abP;e2^I-f2=bjL+SdzSE7&U7CKwd#5##|D z{e55X3BhLs!-9Q+zZZN*@B_iW3i2IW(mP4;Y(eXe%;lo57ra*RCc$-r-xGXJFf6!V z@VA0d!M6lI5d5oPTDp$UX@WU|69fwd%LJzj&JnB?yi)KQ!J7nE3z~wr3*IfrHwT#? z4+=gaxJ_`U;C{h~;M;=l6H(7U5=_EGmi0WHn2R|Ako2>OaF-+aHQK|Smk4)dG>5wy z!EV8gg1ZHOBY1=edc5mHI%DOo&N+e?3SK7IBKTi|4-5W6@IArL1SjGYpYgd`@Oq*L zb1%`aBO?5bf?LFVE0FQ|2@&bC?))4O`y)j7dq?oNnEwq(f1@yoqCcL@GklNWB*9X_ zZwg)~cq8$v*bEWi!P79&XLx4{juR{uoF%wG@Cw0t!D|Iu1Xl^( zCiq>!gG9tr-6cYJyhFrrldz~FjuvzYP7s_USSPqd@H)Yp1iJ;-3EnRF--53TzA5;Q z;0VlT8U87PZo%`2XQQ4HQEpR#ET@@7lv6tD3sG&aw0@q?YSNSw^F$XHPx{h)R%_8$ z@Z-9NtEJIuwWBoZPs{Xd0$0>(=)nfnMd)!s(+G zM1RUln!*Z(DD9^^%>ouJ!r?AN6(bIAMB0T&XUEh#%E9_XnY3F;pNG4UAEci4nb#;Z*=g1gop>g(vaW% zPSC60Xi={4q?5(kL%m<0rGcZzH!>Jaj34K2`U|IW?WwY&359jJt@~hb!iKFhv}Ev8 zq`ULv&QFpYJrq~x@1-TSa2J8fl^|rAIhvIB(q8AdoQJftTuy@=)Gf5)lUZeB^d2*61PUXw^v}k` zde|A|Xo|LuwqY_Di)jj{elzb8V`7yik~?7ki9Sd$dCI9`g&kzWwcgZD%6Sz0L1}TAhO!QB(Gj*P0JGZxdBxl$=0V%V5KYHjgdxeTH%}$o4_AO@ zF>N&squ391$fYYAj+Z*i#vW4J|h&z6&yElHnZ7SqsItTI9+xua6c zc-Dulh`L#>!U;gcs54>&KDHw>tIfQ7jJ)Uc0~s?kOPo)iD~evHnuy2!(c<5Ga*)}rlX~rg2Teh8)dvZ{xivw;z|UpkH4T$ zJ)4<19rajosYyLvrh5T1Lsdcare+=Fc%k1gRCUE;&AQ%NFG;IIDhk$J7)qaFo;zW@ zZ(<&Z-;$|c5izGu0FMq$OjSK*CDxVT=B;cP;MV9mihhILA$xxBNJG8WgNk2k<|UQB zYfQ|BGefoVj-`=zDKn8A47%ql`?gV@34805na?qKGtmYr^aDmb+BEvcA2TO1{iVOu zXXzc8s9ET?GE-F6T@+k3&8H5Ub$=SX2i$wArb)e2^`~R`WvDi0yx>?*ZkojLK({^w49=j3RVScCjW)uhj8d+*N8Uh@IOFdz zInhPOPn@Ncf4-V zNDKLB$B!c$bH0cgesIUG$NFf;-vxq}xl7g=mY#0-&Bf1BQ%`qTHvNChy$zh6MRoVT zN$$b|H*Ay@1Fm{k7aN*|23RypV%@w)+&~}+q&$hn0vc$95RyufykHXH^2(iH&7)}3 z8vgLu^l#cmo7QO4=Sg@=coBktB)W@HY5$3$M3EOOn&+AA80PDNXJ zp2iw^CougIz7$TdKK|A0fvFy84@}P~Jd9`2T7P=~R-o#e-oJzA*1GU%JS$In|4yFO zA1VCDJXh+%lXz~g3!lUD@VfB1JWE)l{L^_>pQo?};3)o5{VwCVo4@q_&v>3t7hcTs zl)A9?xO(cspWt~Ke+i#7Q}t`Yw~l9(nZkd?^W3`dZ9HFE7v{0}y^=kuFGXsNG3`11 zOOe~@{=fT$(_;LRCdm`_oEF%=P}7xv{y}zqnignph}|A@Bi3o~47Q&ZTKJs?&tMzs z^3&wOtc$HFww@MUwTQg(fOh$xU$w|wpxKweEuP({PTOg^%=V#T6Kaw6qRf|>wx4W2 zs>@xMxj!^EmE6~t9n4r*Y$g@EOHKFFF4-g=3;4Z&r@`Qg9~N#-zd>0nZ3{Uce6oyewey1GYB88w37hz<&$)Sit84 z9>E}2Ipu(F3;0I?#{`@h@PdFJ2zYhCr2#(?up01-0XGG_E8u+rcLz*+L`DPZbChVo zUrWF@1bkb-j)0Q`_TMX-6Y-Y?Tomy7fGY!jI^bUgyfxrg0&WZVc)%wDJ{j=G0iO-{ z%Yf;7=*f@S+nIaRo{@q~PVbEHm_;&&27ftj?dsJVK z@Erjk2>8u_djh7ts&^rnmH*s;7YCdh@S_1&1pHJ$`<^@bHbr=A!0iE_4EV!;z(L4;;W;wku>nsE_>O>S z&uMIgCkLDwF!@otF2XkiyfNU~fSUte0^bge%Y-L&)%@^|YQGY+SMZ65SRiB2I` z_%c)W2EDfLJ@aj|`-iw?_T6g5d8Y>nmDSBO*ur$=M-x0qK)&lu$KU*)}h2&5xbJ^N3?rc`T2^0ert(Db;PGuaVKa)Ux(Ca^$BP z;&%&K*RNXE{-+Z4P{YIGCoGuvi9~UIv>(?NA_^b8cqCNbdc->H}U_Jr~NCqr+n8<(ACn}Qx-3T z0?l29cFBief{xMUe_TALI#r*6Hvd$g(6RK4&}UTW{!Z5O|mjqCS);JC)i`1Iwt zE;5+~%{yvh_5Fvs`g{PecJ_K*ct~Fq(TZVNhu><*k5x%#4(R!3zX|O5yOn>B-#L4f zXCDFwF4rgatOX2((dQ3uS$z>Pw`w$W4>B9mZF)X1T4pK8uc6XGgbZFwIcu%HZs04U z#78SFYacg*fiaVDCS!2zor=RopO?ORTQ@m*ZkPR#%W=*}GLe;zB)WOkp@I1bLg$l* zcHQB8v``4`j&SzEDUY49tLyHTRqt0^!6|%So^aPM{2;mCCR}!wY>|B2w1Gb+RGYfE zo8GeO&14v-uW!cHq3qNObzS-(<`A*RffH$a_FQ7u^P$&)NBNhZs(zoD?8Q85bam8! z2yhPar^9b^=f_H>oS&bi_1A^-D*UhA`Q|&0ei4X|^-<6Ed`bI7^GFd%vW|AUPMgoH z=w2dwBdV8vA;rf#e?Z4k)z&ZQD|?GZby=cC$7XLf4jFu#^6;r73xUfaiC1p zvVTk839KRi(RY1Ug4az4-pG}h?4tGqe}^^Hb07E{9XWr=-}J?twbtx-jn84THabt?fQ_hp~URb|C4b>8DQ6ufH9r zLtViTLd~#AN295kzmp@(>|wuMb+{Cvbo!0pPwDH^_2@(1gdPl_dCEJ;X2JOW_3T<9{1_k?NtJ>LGiO+N$Ri=IFNdR|>z4 zaIk=-e+OaJKfV7$o;8e9xWD~K{gm3R^4>+bQkPyLsJ$*ck>}xc;r{j`*>c1ysXXcT5{o>CX)vG~0b`%jUW2l>s(BSK?M_Mp$y zB@i9|reC;F;4g7Y5C$Q$^P|IDUqt|5d|`D+NBWe1GiA3m|+(H=99{Jr~XVgP&33DjHb9Px+X znE}TJyddEF1O8b+yQ4t*PexeuP=4{gpfws@yAShk3FTE$i?+rLT z;GBRT4tRCIPXtVMr~et@F9g)L3xq$}pY}%hhXFOF^m zXFB4aj`&|Gp86b#+!3_1(ZIJwIGu@>BMQAwe){{LjlMYEe<)x&8;xXGynv0XWN3If zJHzQ`j(o*-h8GxH`bn;9HaNP8$+qt2c7|q_o^AQ0ihX~(f}%Ox&mJjrZI`-b%@+yx zJ7e<(A*pXtk=nE^pUgXTw(u%&Gsp?qX;l2(v zyt&%TYA54fs<&tK`>_Hj?b5y#cQxzm$epi+F8ivz7Gl=5>cevO>*oLe2%b=c=1u+j z>#DP9%U0smrAn@*>G|XcBXaDwz7Cfnluo}9{3(5XI_W;?m(of9s7KKsUwYR)K)P3= zTmAzG-kS=o!y&wg_u_XG72Qlox}@}T)lanPuUn4d#oLHazf`{zPOv`yx6v&VtyB-` zda8?_Q&@VU=$pbk7QcaXE5x1s)c5XlFbSPY(1J#?<8`_|$rFkS;)vAJUnS4l1C9$g zH6Xk6_WrtnS-`&x_*Vfp29&)~_`VwOu7D2){C2>95BRfy^0J_Oa-Ayp`vKD(tML(@ z8jy1WANt@Wi>~f3PtV}w?dc;WJNeI|lmGAR?5tEqoz=w~{a)T~+Y?Ne`Z5%aeZ*Ey5@`iLYQ|#yU+NB_392YrjEW+gH&Iy05fR5r+@^NKP z)lD)yo8Vc;Uhy5tou$9AN0+WWa`W^li#W^v^O?_`rGKiWs9Bj6*RtkH3EV}Lpc$lA z-KD48YI!)g=00@Hor;w)PAwjNKEhThmOc(iD)}{)Mt2-vrOmgBaY;NpZ@WQ_7|o|y zm8E5^v`(nmj(dij$coY}ES;PE^pw3E$I0wK&pCcxt!2&0%z8Sz1ot#8d;6^d6giTQ zpH^oL!ch*Wr*%Ek*LI&LS=tBYDSpmoTD9j3FP=|=p%={4h;v)bSrr~{ZRa00o34AZ zXR@<|)MmR=9e?YM&}3TP^DR#Ge2d4CS?fJn&poB;f(MN*{ONkS1EbSiZP!*w4{Q?vFqXNTiF{_?6TKYFTgPN*%IyG{fa}^{~#tuT<~R` zpSra|qS1EDcG;hpD!*XcF7v!y; z)HpGSbx$`{sEi(8F>_?gn)kZJu97e4*Pow5#)e%}7UtG}^4{NxnNHkY%=nejC$_9r zS5;e$d!Zfv8&43Sx=OEGHkRUM=jG%1s;hEQv%GZts(W>#rD^F9dF-*x51q077c0?{ z=@4k=r@V}WS0|gwLqU(~pxpVnt$b4WjYQ1N%O{NPIia-V=-Tc@fp%plXshK^x6W$o zarxleDng)|wjJlkG~H#ATV}J9M5BEAuo2VCUFO`XrTV`S0nTEh->e?ZoIC$GH%`jS zU)_lf(+J}_2g=b|5+oTsVG0ngITaplM=&#sT6%{DNq%uFhf>#EpaSIB(MC*_P9B*# z?kRX1dvMug*In|~FBDynijKOMzhJD~?FHRt9AwBSWp2Lc^QCjU4lex~WptyY$;qIm zrIs~c;(t0Ld#s|Wr7HhL9(Eq^;#lzjv^oN7zqj5Rx$OP@?dab2&d+9NsT-@D@XUv; z`p)9tZ*Y}mgB@ke>zQ;p8z-3|SPIEh5r*}IyzQn{xbv{8uAME><^HzZ2!HjVLQZtZ zuI%NPKda8r&&Rgq=&q9ZpiyP%&FCB5p zZp5PZ@qzD2fOtc-nbvs=VVwm*`GK=^w9p0vmF&#yd}!ABc{MFRXV{2y%3an$ zE!Ag4Ek~t;%%#t*+HRKemNk>H@vDDr6F}D%5Dee|@_)Pvf z-{N~J({%-7Vy**(AI4k72&qkL{|G`Km<{!Odm($oJLXPxjBiK0U;8B9M`#%I4T%pO z_yGUYczv_#ec;1_4>?M*GndVV@l1h9(P;pU{>}2kg=&cWl+v16QLR+J}c8M z3ozS{L%Csl(85diSXYfRbwLS4?WG!Tw_0*ejpl71hgickH5tB1Ahf+r3!yPcWjb97 zL(1>pOz$s0+*6)T+qT`5<2-V>>UIr!!ZW8lG9Ec#$8WPYk0QEd&FA>fN6z2S30(Ge zyR&1@dHJ(CF)Y1j44+AF+vQ1?evgRZeoTK_b;jBkFHoze&l9px6Rfy5cMu^xf%D;p z`n2LsgzYZorYejHRUw;kH&?0dE_JiPr_{LnwMLAE?<%8@^NTqxYu-lB84*s@?tUV| z(i#2M=SD87)rkh@lZjCnW-W9)i(qdnbUf>VZgtS;!M0t9Cq)sPYd&@5d@S8ju-qu3 zadV|wI*BE{k$LpA(vp#oF0;)sNX7WL3A`Oy=SkkA+)likAVBBvuJTJF939WvKIyw} zYWMj3h%Usa3R{|kqdQ;p@e(L2`IHK~T;{>nH}TcTmNj<*)PJ{5AxL_5x8=pLG8z}8 zLejE!4JqOt5aT&(T@&TwXbs1mjHzxzbPxaQ?*P16(KQ94gKG}iSrdq(mLt(pVuKtZ zK6=9IN_e6^wuhH8Xw10##eBluFZhLFtx0GpV-8joF-u--e5+6pFD@sdD~!3 zdSbSgwF?mz48*FR0eAjHK{g6!R#t{Mo_#ki&hBO=Ql_!ljAuEU@GR%?#p&LfuAgT1 za_P1x{{4gK2lc95IGGY+LfUHS%hpZZVzbd>E4EeKvSzHN0rl`~dV6F(y|n7N>Z`J7L+J#xw6$XK!sjn;QnY zS)i8Ti?!{mY9}AMC-SHxl6EXyU5@njX(Z!4r*SuFfq;@v1 zy0^Y*-=mnDsU^1`PO9C%!_vQH<@ ze`)?P?YWFAjsM{r{vAA4|4CDG^8VTCbF^26=|V>Viq@R!WXX?J%#QEjEAQB*tkja9 zrSK%r)Ag;)f=l;i(`nOO-i)5knJy(+Hp1ZN@y|^%VLFb{*OVjsw^OfV z&$X<22ff6tRxG5#hsRriOa@T9TodL7^C*be7=293+6@fcf#`GKMqZG)&VQS`Zj;No z%7UwK4|n-hR}R1Cl%`7MkBB?DQ&&O!KiTwq&wH=DhPxm0FRR#r?8-HlUA_3SD=)dM zgHr|z7s@}~l^=dB?5^RnEu9&txp3o4GWG9tzTkIaqy2qds)*mvfc+un%JMhuk4 z@Ln9R`xX(4 zi%>0{(lacJ-;7oSF%e~A-p-%epg!qu8-DTx_Xou*`=xSIIKle({|?*mFbY=PRd3mn zwWv}3Q@Fx2O)SDvJCrBAKb2?YNn!0zSL(uN^W0t+=CSwTndc}!gx3DDGC7hb^ggu3vb^E{<4yoBeTy6{S#r`3gJ*OmxP z^^tv5bxPq+^DJIX;ahl~TNmcB_zmf||E6;d^*{Mu`|F6fpI5Tg|7~3Y_1ort;jR6` zI|%P4?=@`tDE|Y5b%*L<7|(70$w1y{`=8J4n5*QJZNb9JKFZCQ%P+?>+lM&)VCNW4 z=Tw9po8UH!AEWT|6dZdvoh<|oM6ic&rmoF&f`y|Hj)ub!A0h5telo&P$RFD!^2T<# zuEJa)Pi$;GoQWeiys+t{1l7A_k)Gx)xMpF^-CA)&hKH*;e)f^YmWX2+3l@HaHZuHd ze_&?7%LAtUi9Es^0{%n5{}XU`!0!coI-s0E45D*e0=_-q=>f+DJU`&20skUk^6B=e z2&Z!qUyN`%7x7?(p9uH^;NW&v;}QNzz+VPD0-2k+cT+y6e#yCXiGhxk~8)4iQ_5xzCx z*8)BgurFXb?{Fk#s2o{71&)X;aw4acfg4OFAVsBfb#-g6>xFDj|EKU9&U~B zKL*?$@ZNwA2iy~|FW?UX9t!w+Hg;6+cLY2u;OKxC1pFTXuMYUpfXf4ZB49P(rvv^~ z!2dVkzXaSCP&+j$|C<5R{hj?0ekR~xz#+(Dm3MT&p#k3%aCpGe1D+kQ zJK%c*P7gRY;DUhZp3hARLx;5i{|YEN=)RBWaEIclN4nSZdnhmpzdqpG0-hOgY`~cT zF9~=}z~npb)(C$k;MRcm1pH>e0|B26_i)^JU!rf0WS)ee99#s za(@}|w*}l1@UDQn1MUm>vw+Pg(5lA?0sk=I$bjz-I6mOCfFB5WZNQ}gKNaxyfSUvU zTfqARem~%I0f(TZsUF7%Y!7%wz)=Cm2AmpjZoowWlMlER5l%kfJ{#e`514$tJsjb0 z2izC%rvaPTH&DHf2zXq;69S$bup{88fD;4G4fvse9}oD&fOiB;zTTdU@Q(uiG~n|A zUw@p}rxNhgfMWw*81RDu7Y4jG;3oq9Wx&4+_|F0F4tO6>e7QTq-wOD>fXR1U3u8|7 zd1JtL1Ux(72LoOk@Z$l^haCAnAK||bxHaJRfcFG^IAHQAcOb$~2TVTYBr{c?qXc>P z`UsyG;kQOupS2rQ!KY`$zY{2V@g70s#YDw}XIj8N4Y)9%`JUtb4G~@)@P7vUV!(e1 zxIJKRz#jxmzUYo(J*M(n0uBp^KKPLjU6oFAaBd^MJADRcHwHS-@ve8h^6yS>meR;i z>kF+)20G6X-?CiYzMdrcU$hSSi=ov`?aK+^VQk*cu@#o>IQ^UWDMKx9I{gsG1*U0V;6QeK_OTLI&!7lS&TdwokL$SW zs_iW|t@21|p?rF!W#tDH-r|W)!_Qey_LJ(lgYb{0?_Me^--TTE2h~Y~!K51Robd4% z0ZrN6cuTYQSCy;XgO=_i@2nM1m8H_tL-RXv-{$muXIJ+*EcxUiXvOuvMJ=A2%(XA(d2jeI|foGmqK)n_tWxj0{80 zq^xaV{4Y&4t(Psk@XXOH24=;7)GgO>iYNMPvJ1v)krR$J+*GERZh zg92|A5a&G@$0(TCVf zrr25IBbx}*_7B;CrMs(R{+`70lebTxb<{-avyR8CwJVxwb*4+Q{Vi)2Vw~fI5r*^G zu$CP5iE?8{&nd$X6S8mPzX(3q)_#|eZE4>wL^D3|Vs`h=2{*n-K)(HyWAs(!><%%- zBZmE5*6OZj6A`TUTh?~b3F3hD-u2}npn@4lLrLXOdrT$nInVr|V97Fe$LOT)SUFS~2GMLwwh8eftA?RpwMo%c{X>7bMaW$g&muk65(gD7!6e`Szp?1*Q_96>oxserSG zI3tc8wQPQN53!pZ<%DYa<2S=xN<>R77H0V@&#GUwbfbMJZFrnKacq2TCX$wJ95De| zJm?UWhad9SSNU?4W6MU3{^O>i15}O2-9@k?I)sr9{*BcHO^hBneb$N#J7t4&wEUA0i>FHXvuZ?%!F` z0nDbQPdXoXw^55qX*7Me<^u8R`u1jZA(-bCY>JYGbi7ZtT*7~ekA;5fg!$wuGI>D6`ZyNS3Uwco#) z`qc7&1!*B3XH}M+Se+z~Up<3{YdX*5Hz7(FueaneeE8LVyuQ5ex^k94l43UDh_;s*@#Vu(f@~5=5K`8>C zQ0yF6;lBiNKNZQ`GZ?*>jx#NVs;QlM+l^3b15Yc)+;ZspH&Ke(oG+#BjYw6B96)vBmlZY2jC#A%ZI951k(mUc#&31y*A*5!~^C?qMBSui^_W_k& z)?uaJ4xFCuA&;bq!Mf*@M=X3n6=n7!GFCu5ld87?r-9&LAP`i6g32_U_hN1h3FQ-1 zfe!#xpu$SO52#67Lw6eao+PY%orIN7g)5&Ls(cD7pTg-w!*j_mIUxz$a??bf)JL3P z3eTOh+pit5zJ0lJ_U9Hwf^`$1{_?gy#_dzwOdpsi1}m-3`N&nzUHi6+IIPff!MyEc zb4m9nR7s4Lsx)+(G`N}b)GSXii$*Ag@8`~{^09d5!moX-^&ci9{;%Eoz3W!KhiA>4dMo zf{ci8b9!eQwDii&!hhtd?Oz88=V*S)NxQ8K38@g`K-V5AQRzmX;!5KWG<*--LsAf3 z$_sj@dERm`c=vpOUKe3vOfnB>>X8nV#*&G!0&7UG|A#CTtLCh@r$lOftV4bt%dUh9 zCB4Kk$stVX*6M3Z`7qQ3{3pb$EKPz8=x$XGS#@P+qe+ZE&*tbmq_%baa_IpQ45#l6 z9n;o`wviL5@HUmzc1Bdn1QhxxN=1oglS6vlBBt!=Fqe%*`g{Hb;JKZQ8aLusO(KHl z)BCf;?5gEVatLa43tkY7RAPlSngtI}UmHXz-yRC9&-VghI$UtZ6sd#Qe%WCM!-|csi)%eKU($UmYX(nbi+mXq7dPH z$8N;x_T^wWzyF}@CK8g=N$q}P@xZmXA3%-CUPmV#xS8XK)8?H{rg`I))BTv&59e9b zRUH^C<_+C>1iH+j1INpq!xo?5!q0i>gb}6JbwdYZ;~Q*(tA1Ii|AeE{OSQZdg{zzn zKfjcpcuvmyrZyNH= zN5;Xtd|{cRSPLt>oCT4ZLR8%ILJ}1|0cJC9WTBa8e0eM|zklE%d`(QBH;IhdGuEB; z{g`#e$>DXzsyhc;51vmJ*Yb592zs~XZ^`!T*#Fi+cRu_}&fhkJ405;TuY;-Ds3#Xo z$|~TwTlH$i3xitL?pGQ+YF$NV<`B|QMSQfBo$0af^*aFXZCSIL7nVTWjjV-?-C_u7eFdl#R{2P}0dunRXWOWx!sj&)*KNq+RNb>|1G)47^rvHvHI zig&I(aFJH=rM3CcyqhzPtITo9lE25bk0rc4HyNT zOON3oI*ihqo%y3)yLvd~Hls-Ejo!)wr>TH{p)O5OZ#K)2A=w=}e)!fwyB~fbKDT~6 zMCI6Kg(R_@it~SM#2EcdmwdA&;H>8B{1!_7{!(^g_@nhZIPY8f(OZ=#9Yby7UW4YK z+gkY-*IVqszkeaK>%MEx>bh_7+w<1XbUngnb?vk>hcWG9$&Ys2Y*%@d|3?qSyS6n4 zK1_4+(q~3b_)KZZJ)8-%_u;ENJGb;d-hbzm+f;|V{0a9Yec*C(5OMhL+^bI02Q|5Z zd)dO`Ta@|?rrihQ-`OX3&A;^Ud?{w!;>+h>st;veMfQby;HNK_2`;+)BbSl?v@OcLZ^7nWZUM~L(ImoPFq4icQ;ESS{Mb1}L|ML>rX!QBTw?@xP-x>u~ z)4AuJd-22z&YNDK4OcPeO`CYZ*ctCV@7(l8@u!Xa(7@-*>JFXNeKfR5nwgZc*Vy0i z0Dfm`XUJZI&w!fmS;=13WcKUk|NY1Nv;IrC>DT|v*f2#5dHFMAWewN5pCplfBlweS z0fqgPZg@Nkq;w;BR{heMu?p#KQ$+kKKn1%4rgV35!)y+5>8CXMJ1<~5*HyuirFx{F zp7l4Izq(?YhKGGWrAzPT50Eb5R@#(KGiqw9;?D-(D>*Z^6`7=O^l82jDMc6c30t2H z_-;C>sYxc~^h@QYaDw&mzs;Gkghz0oGg*!Ak1TGrs@^&?#+H}0A%#UN(L04jX0H_#cXQStVZbfC^WjVn@n zz1~OoX5O6*VfB7SzxU#!^iw-7O97z5SM>|)j8XdO{Yw6JL!@5<)0r!6V%1Ju$tCgS z3psMNaN!3zbagoguCCy`)qHuNS;$!{MR&z@D&kJDBsOCkWC}ZJcFLx*j@xFg`Nm(^KJ3 z00;3-zL}KoONuAozY3D?*Mg*b7-R2Xc>M;f(Su);oA}MJ0t$_h#!5d zmp>M$^3M|_{R!-=iVp7*q`db6)sBkxu`Pv3k!;EsU11KRhsc>f;}{&B!x1pGaCQTdM!I5gl10n>N4-WB2Z z1e_UgPQaxBKM`o?Vq2^AZ>KNblqwl}-q}Nba zx#XHfuYof1R_dnt3HG&LH~+ou9+o`RPjdp|p!m%~=IEW`l%H<5IpyRCr+HWMs{W^R z`i9;&R1WfOy5>C&0wVc0%TjBJMp;y#R z@qfwRg?flzN+&22Tl#Gvuz>&~)9R)6COW0I64lgA0MStV622lL9_r)&J9LNC$KnCa zef6|4Uhu(pNo`8IWd2o`{F(LtU^2IoPcn2dUvr*6zcxtIxsNkXA9##*o_W?gJ8hJ{ z{1{#RMJJB)>A?P%Z+PnGkWR)gIwaRVWtj)|dFCtn=+EsGe`-f=81Guve31H+ zP$_nNe{kJC+{*Tfu;B4`b$gqhI=%*G`$SLq_-g4v3@cnzJHCWXO_9}(=TG4aGWqq# zH;>I$nw=DbjoxG=FTap&YQK~+CS&Te$kpw0RjHoi*+cmT_krdgKQvR`K$n3z)634M zDc?%H{`l63V4h93Y>OFks-<6~D)k&cECa>lYR7}1X(inZrQ6g#O{FY4UWP5J=9q&~ zgGt#owy=14{qdFRgkLp{r3&p7+cr6HQv5J*RjTEmM5VwpTveDN)T`~BN44~Pt$rO; zp}pGi3zBqu>0?xYmkj!m)$;zylfc%eoP-X-eqH{mVa>;?6DkU(omYUUxl-i3+P*EA+qViCB0?Y zdDRbn>n*A0LP_>23*sK0GHwoSFew zG*n3`snXv2tU>qnM|2afsm5x6a&NJbM*WqI+}~dgki^6rntxTk%mLRbhR*BKiigCru+t`Iz?9DGP|F^hHHZMKt78 z1g#kf=w*u(u=gPz_Ve(>PF@W67kg5;;^EyXJj}zpQh2C`x2JHkhqtD1lZQ8_u=tPi zHWJ?ZAP*b#@X!tz`X=+_P|{hxJiNgpSX+o<^FgFkJsw)-Y4#OVn<)8 zo$vy}@VItWp4hQsrOk#zdyi!EthpIVm7(T}9nCB5Zsy%6(h7>-1iJ%H2skC6MyK9S z3pgX-tbnrv&JB2JzyPJ6w8Y}mcn9GDlxljF{%$SyJ|72DKXn?F=_)bTWc{UdpOcqi&48tv#}N< ziV(A*7Sru9x71=pMbg|{iy7rHH$@CDm+Nt=w%?lRu$9((Y9Nmb^y;W5cJy-4tXZ2} z|GnMRH;jHcw`J8;I)b#xw5@zvRxybsaCvjT+59+(A^KU zuY#fLG%p65>su#;zBTCEXk4abxq_8nQOb{&I<(#tnTgfX{ZhEHD@tSYE6Qj@Vj!@X zu5?{o~YYh{8Grw}dn$ggOF;Y`Kqe0pqtT_rnb*u?yt_Q+2C#4=Gom48K>DQnkN@`c0m z>pI4!gqv(aNowLb?cf({ReOF#(a+>pjGUBGE*xH{YCa6Br61)NkIJs=sE&DJ{KSmM z)izAYFi(Y_)0tf{vTq1F;@FI5AIstz6Uf@6<2#5qJV(DQC+B{$Q0=Fb(|eEFON)Z< z^scvjPmDGMt0MZ2ah(V911!PaV0nuVK{aAo- zlJ%A!cipESF3$crl63-(nKq4Tz;!>kgalklEnN4T9(Dh?Xpfn-D6eHM4%Fv2CnQ?mun;w*f5M?+1zd;dN^_{+xTGo7)>OlTF z1ql9{r^t1_igigWf)d$#G;KHvVLz9EsRtr|?=gt5*(pZ-(Az@xSt&++(0g=9B+Ip3 zF~Tz~rP-<&;gPhkH0lS#GbP1rRLs#RSuv_(912f&N^^_S97CG*HZM_cb&O#imD1d# z7}su=>k-8VX0}4FRhS~scJ64#^4g5jLl=2>s1WI!5-EbJn4qfsTkD~&;4mYodT@Z` zM9Zp=qVC*q*Ux#j!y6%cFdso7Q(`*ai1xDH%|_)faiwIXYX%($IZRY$XrF>EJ1INE z=H1zhK-PyhJOe*3r>Om2)KdTmq)?1N%5fvc9h4*X?7f$z5C|ba?~!}&U8VJv<=*7F zF)H*-oxB)`g93H$Q4n)SbF|Z$YjN*^wCr}0 zw`P{Ao%f?il%t`2M^xK(OFJ7kHbWnrSt?e1^V33jlh?G-O5-KcI&bIG(Vo5`%8yi< zCt@J6Tt|T~6#1m+`3pXzt)-C>Gp%z#REB$4=D1$i%o(ZmKCB>I zn^f9H!Z}rG^QE<}i4wuCFJX(fp{KEAkknISh*RAc|LcLfbhfQV*|0LU4u_<{Fk?O~ z%DN&enpWE$G|S>*6y0@3Z)jRA{Vbo_9O1qp%w($6q-yCtDBhecc8bsUx$lN&jkUHU z=BQ^a=P3rQ@`GkBfOgLZ^p@}F8?^V{?!;B0U_o^bY}uq*Al|3Uo2)BpeIOh()w|+u z$t)oUn^N};HlC<;CaNc$*(*doZ-8izFhpf~?rz?DfArzr`*;wRC+^sDAEfeyEBs&` zdc@v)=~9)Yaj$kYulgp%3R_6=zPGfjIa@v{2khaO6t47(+ih#o)fZnZj_;5hD#npY zhbfJy(f79=*u|+hs35xZ7Zwr`W@Yv@L|)!lZF` zKj;g+>n(pfm;s`zFgHw%r(Tv=qD<Sryr$!nx(mqQU#(HdV3y`giwsB z4{D<^neo=0CNoqCEG2bpCW8nP_XwCv^J>9lg&f&VSBL?`Wsh;Y981`xaL$x)54C72a_WWP_z+8zQYzO-s%`h>GlwFY zs-3%eV~uYbJ8p7Dh68Y>vtBgBEU)S}3iVmzht5wE=>%gC$tp3W z3&CJYmsLUN;D(S@$UH3ogfsO@&tpvkt^j;2!->YJZro>NsMx+C==mY)f@gc4g}Kzo zz4wdNCpn*s<4I^3JS$WbN78*_+~C+=a)VkVA+@oxEn)XQ(4X5M@VvVK{lz7?6ixYF z^U!)#HX^m>v1aMMCR|kjZ|}n+afsJD%OAsAj zKNMWyU?`X{X&eeI427fU6XCDPTT?n3$HFnRMwPKd*6gGsYGjzvQt0~+63-O%ixJzh zN(WC5yj3+lY@qa0e|(0{XEs&K-!k2Yk+Y=v6h;m`H;)3xg^-5J%q~=VCYza5rfTai z@~NnRK0jeo8q2_#oG%{AI7r#1w!#T&J?#?$jS?3*)iFOK%OvuUUe{t&91<4oQG_7X z3`Iy3ULP{mEgYCn^nRO*_!vkruO9(-=xxa7-ZJv9K!G6bF03TtZ`!!eQAxU!$0{1e z66Y?4K>vZ1R?>U#V_dh&J#Vr%`4~wWfFlSl?Kh0dkH>I-%wV35rtVTxxn!ZH2DxGoCY#PqkQ&j8jWSU`@C7H4@*a zu_}5UHfC38zM97y4bpbqCTJX_{&K)U$`CcZSLXIm??Tue4Ocb}QyaOB% ztG0(N+vhYJxpSk&1C3zM3_XqMy}XuHJsQDtenvB#v~{Xh`Hl3FO?BVS+tzUVvCP`+ zVr{~io)zP?<6w2nUTId7@^hNAGsbg$B2qALR1ZjDq1X$=Nxr*>zIJrm3VfPma$n9%gLL#Gxs%(B8x- zFz8cC9OK!QvBM|EZ~J(DCE8k=7+qBhLJlx7mLKtXk@3v3AmI@xaxN+2f(I*12!AcKFq1J@`6I| z&`XKXlPqx>%Z*cZsGqb@d`l11jcM{yHT54T^x79Ydmoh0OLMYD^giD0{YI>2kvlQS zu~gl?_g+fa=BT#P~8?7-$|#BNqeO=3_ke+{r9XYL!bA(>7+W`^K)Cxmngeq2KY4RUX=%r@RbIQ&D6<*q?5?zgMS=A{~ z@_IF|rVI?YVz>F9$=!7V6auLMGY8Xk0`q>9$l%0_M*G+JG(Hd|?oc6L(Q+7DMn_I?97SZa_{ zd9e#o6H(N4vlq4nKyozzle=V>UNlH3>RelRs5Tr*AT3%A#+^-J-06+;Bg)O#6?58& z0IB${?!K%XOGDo5eA86U91Z}U`NNlzUN5;SIfh!zC4WtLnPI3=ddcvu;b@7}e9YV~ zK`lnamu-7nKj|ggyc%bT)Y!t{y#iymsYJr6#x}?`D6VX1C~8?vRu7;zwX8Zxa_#H` z4RUP&Ye3uAOhs~SC!Zt3K(+I`Az#L`*umT=(_}UW)8riaN-_BrYKY51nFw5osg|Fp zmj2V$I7^zzEyoX*KJ)>XiW#1eVh6tM7iQ%g7U?EU=7ri$sap6T&q8R&4*cS_(66dv zH2(^BKBrPGeS7Sr@!8@wq+d3tLLbZnN8O|z0xzA0MBs%DzCo+%FX^bu6!4@exXhd? z3CgIuzP+_uE0Msv zz-!e{u33AO3iO&|-I{NBSpJQR^9Io%7iQp-aDxD1=7d&zyEbfCMJ8gKpoG{qCnO0O z`ih?%q}2H;wNc)czYDcAV3N2yLWjb%!WUv~;tr&k60rPy<;OqTwlnsLZs6?&oX7Br4gfL{e|$EyZmD@Iq}zo0AOH z3oJi{{psym=QlU9?C0OlB|xoKSc;Pkb>|$ zGR00n%c>@c+J}AsNu0y$R-+wnqyu9$THeSv5fP=K^&w_YVRbq_JBgtneEG%AaGHs< z>q6NEWfp0h{6O(8rb+=hdyqZ{cqypV2UF$U6;0-iN6B=#%c z!%$Ev@Tnt_PP1`e*3-^sTxPUa0+DBh6ZSYh6SpA;lDK_>nu=`96v*qBTxIVThV^FJ zrH7301Ff=E5Oq}QeyiDGs8>u*H>0Kfn!@=PPfVS&B25wp!n;csn;k|20VRjl6*kHUZ#2 zF_NY@#p;ku-5wMq6?V-|wPiVxw-X714a(u7v&Kh?)Uulw_I^V9@H7?F9TCQB5V7jk zn&5vZW&w2!vilx0y^Au;6>jJgee9@;PN-?N*^xv&CuQ@0bX)Gd?{ij1HylBir~P*eGSq- zDcW@hT^ki`43Zpl{ccF-@mLm3$-WpRn@!vX?K-!Eu5LGARzRo^)kd#R#(Y&&bAzkb zlLK8bDJwnTusG=2_Ph?dMn2BFQW_^-gLL$X zrk^@nr|4=^!tiWi+hN<&!!wi-dit+UByC^mgQi- zdbx3n+Cx-a6Hdq(+44dUkKMdMOc31&mqsS1*nJbFUX*|@Qj7B8v4 zOWeRzcX@h3)IX!Tguy<{%z+t9Ym5~N3+e%@Y?M^c? zO1yg5b@=~^u#Zu0wu;{%@|hEoxQ9Bvb|879aYQl!Qr<2Ctg|C)IzXuntc_lK6B=ZX z+S6W9>9Cn%z2mt>nS4ShRtAtj(R&{X>LAYdTiA@Y4CnYFWz zcJMK-chJaC`=^jY(on+j)lDT5wz3}o)1Jeol2|UhwDowP^}^xS<5yyTVJ`7SW@1mQ z$ILEj9x);wtBg(k<`OeQtNyjgM0?QPDMpkm%r+>EdLcX2tDl79e8+m9s=Qt;tM1n< z^JkAk$C{Zs%`y##J2I@&q2}i^-JJ7y$mwzEKE+I?nVJ2k8J*9NhAfGWbz9czj6}ZH z$!GqC3l+9!Jr@FC>(>1ROwchgj3fIjz2HKH&=vDcm~*gHZ}S2>A0LYsPV6`1@~Hz~ zsK9>Bor-F!Jd4{RY4LDbi^Q$aUtOiL*ZI3S9XRsDf%W)oP6z4d@wRWs*h$&c_H6NR zn|~zd<`-Mjaa7T-LckUK$xvIBsB_bJF5lD5Vrq;K788sKiPe6SQ~Tm_VBr976@if1jDSr;Ca&GWSiT9diW{EIV$?}kH zKd7pRKnWU}q4@}sS^Wz`)gHAiElac!ufz~T9q9`n8LGyKdJQI-O(v`8k_VK&XyqjCOB6L@0cQp`sN>CW#hdzXQC=T-X(eLBHBpgH5~RG7(v%jI-Kcsr zTpSGoV?NSMBP7&M$%;z{X>0Xxif(XhHe*G-`%P9NtoT#SQ|i9K=ys}$j9rUZB9+?< z8r>pnxElqK%XrwQD_k9Y-h|&E4F-QC2B@pd`p>`tc^)pUUCiNHsjZrPqUeoSfiya<&Lq%GR-Ez)KIrQ zl2}|HiVPLXXO8SUf>`w-+shBLeLON-(qU8Qe5TI&)0X>{b5lB>BH6<@vt19LNVQ>! z*ibS`y%c~p&oyyQtP@@>u+BoonrELdx=Bx3y@GZ)vj0rU?IlOD(vc`sAlc(cdI809 z&G$e>cPgL+w&=Q+ndEwA+iQ!wyFKrtkypW}fI5@Bim|*4$h*k%fnfm@+{G&@{{Vn? zX)N{)E>><-z@_YOvskkUMChT34eg%05o?7y!%UWzZ4K)+rX%Z={>wJ)P74j#wgP&E zEq15t*KIbzF_maVt#okoW`RaqU1-uy;3>iybDb2Tc@Swny=VgATa-O+1-j}(X~Ic8^o}#K zDqoMF7yna@q%1&%V*meUUV+Hw#?Ya>H0;Z1T1>q?z)t;5$`1RhfJRRW?3ELv_u$be zVFuersD3`>vY)@$!wYWDO=!QCRN_htEFiE;S+oJQ-SRT8YL?upfb5W3Eh<8NN%na> z8qH&iDb1+bjcS-qEugk*LN$A(YFanDBJx>Sa#D4@-;>nB@^|e^;5nOR-*?|(v+QfL zErnE_MI&vN6Iod>9zM&u*tRqxvK?&hNx4L2pLf@poDQbfbO|cs`CwrkArMp7CRvtc z&WN>H_9$vJDkT#Ab)AmO0(c1U7rHZGYf+(o)yho2`PXZM$X7jER()3U??bzwy$=KJ zf5rUUF|_fPT>bu6>3KhZd0olQVA5=xd3}Pc&i<7K*E|ZCj3=hCY=ZgS7EWNMls;9% zW|xwR#B%}F+0QoNXK2!7-vZ;9r8dC=wXZeFpb3Bs>eK4%Z;;`6)`skXH5@j`$jCMr z+gXRzj2cbO7q-U>HoeBCdDm^qszrXqaBx;Kl6gh*g&mYRMP-(ro|s)RJo0egpMup- z)wZ9KLPU)$B;=K3(8T;=-R!S+?B$qfCoCh%u#2B9^f~!@%&E4KyI#)8%_=F!)Zv2gT0K(Ap`SAa+n zhH{8bSk2;mWs zgMNyDvrR(fR#PDZ=jOnfj^na=l<_QDjmE!1!G@n&J+qdows zdJ1wlMO?Xh+$0R)1t98SMhC^$c};+>jW+G&%I6*dC{3v;Uy;2% z&r6NaBQYAo%<%gNHL4LBioDpv3%i7`@kDrc3UBc6t`rs%^L~2@ixUZNC2Y#ZW<6-r zQ#wYmMlUSU->?h%Mn@El`@(`PB9Wm%ozS{nl{d{IE-3A=8+jt4`z%TZS9Omjx$b}x zrmuR^VemU;sdK0zS?hDl}pW- zxBbweo8vnM`tHOHPrXRh7`e8eCx4K(7+!6DzQP~mB2u_N$O=pEmNl9CgH$xCN}azc z=d!%x1U}P;DA6r8Fx#EoF&lBIL!1TVxKX#Z#~eflS^%0!Xaah}RI6!89wOy$QG2@h zL@bZRks&v~92LGI@q5UpO5}1e&<&O6p?TzFBx^Eumy7mNo*qFl$$c))6?U99Moqwr zKgiBqAZ#5=U1ke~8yw6bq}QrC;n^m6dZa3qNeCg~#rF}J+rZTx^->jPD8FyKqvF)+ zH+(F0sDK04kMe92HS5sjwjSkiYpI-7dXeBA3I5@etBbxNu_pGQ_)eKYl-V3ak_JkR z!eb`G_2Y(H$Q~8Sx|$#cPIWOyyy~<Xm*NdqlP4$%GHN)))Z0UVbi8gNY`0$hqWTrV)6kzio2kef*b zx-CMo#di>ofYB6PLt0qTrD-JQqzKErEvYw~9LE$ua)h{X05rDEkdQ!}4Wyw4w|njh zmOHt06hl#t&`>F*EL=MFM{^4=L9-q}L%NmYh`skiqG$$alpi!oDKvH07K%nupPP9N zqICe}<{M9531uUa6D5wat|jea4obtKmVJhvihxk0Q5+AydPvd4^m>cqodNd4^<~lPLk`kY-Kj=A9HPG2W|I zu+nr7oFDiW2(CKBG&qJQT~-xUm~lF(9bX-T^~%|E)op(ONt*cX@0RusUQ|o>&_4&2 z(r1U>;|TBfuu%6NhwkH+ZeHxG#azlGF?(t;b3JBvEoQdIsJjBsERRvuBE}?AtggJQ zX`W_tElrQdsP`h*6pz_Zi?IP6gU<1EdzzbTX^b-N9ZYzr{&Kyqy;o^3N^KAztj_ zN9s)TlCjC1M{-6^?jxfhuUQzr#86@Qic@nqkW49yDv8M`N1T-mU->yBBU$FoBTe1% zWA3V4enltVBgrIIW481v{e)_Y(=aoQ%C121`YvXsQ7j=A5IGj-L+6Yni*cSLI4k^y z1^z)&p~L-LDBZ;_eP%O(4@>v@C{iO%mH1`-NQxuaOVJoc7(4|C#4)fP<2<1XCG6$= zNbYk~PYQS)Lli&+l@@*^Et$vA26Z-Sf!ZuviFUoxNeamf=Ad4sa0vn;wV7;=Pnz;v zy-rpu9`=c|+1YXiSE$$$*)4?b5JWZv1$Dv4&vuek5oV~`<2ZU9he8F8U5?`cfN&`2 zI80>KnWlC-j)DA0Zg(7a1r7xr#|)mRha#*VyBxn?$_Z*y)CRn ziLbj0yPpu#jt!3EcE>S6AmOk*Ibi{rk_p*MY{}iiYihDyAVx`Hwo*Zy!qAqSuz*ad zg{*Ez&YD?ozan)Yz1fkj^SlZ*u%V6I*ku~Xesz1VlE2hDFJMHk0VbU(RzkJG&@JNPk=AhG)tdsN6hQzg_by~l+h zvzXl}&1@blM%`j*W~G?zDb0)&qi(S@(^Aalltu&F@N7(Jrlgn+DUF7lrBOeH_9WU; z=bJ5t8j8m)%}t7-UA$bb$Ek9E$oAL}^dXpB>3#=$WBs*QbmsFI?N-Y-wXFIude9Ab zy-3-~p`=E$Zu!)PVC|b54_Hi{DwAmeknhjUNiNCEn%{R>(T5U71OIO?*Me9&EG==xKo$nU{*Dh$1LR3gq?(Ml>u6h&` zZY6`JzDP0x+Ewk|A*;TL5-?wgf`VL~X_d~~I3!P^RW>9$_RuFvAIeqGGRkK_>awKk z?g*DeTF(;VMHK#r1OF&5lQkXLU0&vP;ZQt;j?9I_GjeuUL%($2lVYYS$0Vv9znsjD zYCf(xl)%10)wZ45k?o$8jl(C7Cw`Ygn zk5v)28n(WaE6H`S(v_ry!WszfC}jajS$k5Vp>^9oy{gXADfT~>Gs(tYjV=!*cWP06 z&!i=ppFc2JB&-Nmxq%Gf%nHr6>Fpwy6(8eh9!XQlZ(4lIfji|@a+9&9e6_B&Q;d1* zDWI~x?#nxG&X=e(Q|H_G&JN>fYAIjbtPvG_Gnv}!J)CEwZb=i@?ENCWMccJ}wU+zz z9GsSuGF8eBPA}$kxQ{}3A=J&-0Kt^Q5D=kZ7h{Qa#tZcIrOF-xDRt=`F(zXe)M?lA z>vtkG3?YHK4ea8Lpk?2n;QCOvgJpD>Hl2Fb@&pd6-8(mAKD{B5#V$e3#9lG=*)e6S zbXa5EgQ-+5bV`@DzE?)$dO<_snjvY6im)v=-_SN?Lo_iXjWM6!_7fMFugOh`4<(I# zq=zTd{_AN(O!S1DS$tI<9vj)vZ%9EO>#oK*-tS&JGuPPgm>L^)95mZVbd_4uD1Q1G zXJT4Ya%v?;)zO<(*Xj;*n>(KrDa58BlFEMsd4f%slyJ(G*l=;du%(}w7Yb+3Y8!s@ znk9)KvRdRWZx?fDvN>N2TMtc)Oa;WQ1wWU_pc=u`aJGEsm9wcjE@E^+G)bgwGp^Kf zE)Go0wgVG$$p=1l8A*OFv>~URapBWoWan-?=Q_XXroXtOJ3PR7a)yGj?s{ikzYq;Y zL>uaBYh&L;SBV7=|I*hE90-SpC!4JEYts$PO9c&2fW`wHiIsmT$G-Y9f=M$^YO`RB zj$)0YE@xEKv$IbtT-v3^A`DoR%BN9xM_E50b_;{=B%nX5L50YrWqM1V7$UI;?T5B| zo^eN3LN4p1H0y9=W!iOvBP&%zy0lnNP(3Xu7LbA0YT-D^kDsX(N$E{&c>IjA?;J5Jq_!Is#4_zR&^To!HIlyhY=gjG83Ny3NSlh-`RS1M@=iJC;W~kjJb^(}#5h`El0D2koJOA6 zG?o*=DSip#qm)L`)S>QRN|lWvsw2ZTRe1f(y=!Z>xY^yis)(`U`q0#-+T9+o#r624 zY%RiGl6(*!Bz2k~Vr2P@cKFDub1-FmK0l`YXG4Y9FH~{hxVT7Wik~xXIG9q2X=D{{ zoYGwjs^;>1iw_XrphKCk5(7^tAJOrCFjT_{(p!Jk_A@|NII|N5C}HC&rimfJ{!~ZN zI_g@?R{fQ$-O8TqKr!&b97HyHW;O|e-~D6^9-Hc?s5Az(Js?u;eE@X3gf+Yf^dE|M zQ7yfPHpMF&l6azHFqrQqaH(RP#^7?NPHNz9B&INiZF1ctEbzB#*lb!d3f4=E*fb1m zb~7QP*=?^obu@E-|J5v4m}7nI{6nqlyI8APjZNnWUaF^1xSV%wY#Qx-{91`Y)?;Y4 zs%&Z0g#I(&fFdU~AZA1%U?`=QteT*4e6*TVQ``oa5nI>TjXmZDv(b5o0m!(x#aX|I-lC=<6Dh6e0;0kHok{3YvQGH zH>0{WTcSJJvy{PwR$mhr0S5L`sjm3{;rM2t>{rcvMAI5^1q0zpHOhxE#L$q%9$6dc zF}H`Y?KccwQbp zDw8=4cMmAvSlMNU`VbU~37PSrWJVGdI;bTwneR6jjKruJ7{%nMGVbQBv~77_scF2D zqoVD4tdksSP3$`+$)RE!=69DI%#Y@*NPgcKO{$~Bh3NT`VuMDgK$uzsE{#FS8a@CZ z3G!L_a%`OOUsgZvFE#QrkQ-^iK7f9VOi?9k^S#Ry)hLl&I)NHwX;kChqqN#ILzG=U z`;D!H-pZU>$@Xg~4dC*!yaoqkx z>tiL*pa#$xN+je#5GTHH%sD3yC$rngxn#nAWtQ zfo%FdAZuEXOEvW>1QH22F}kI8yHq6WJx)^X9>4KJBN`gZyd-)s(1im%i3F;Js4*{~ zt=>}WUAhgGblbhZLAoW)N#4Cfx^=9vPe;0((x)TO;80^orHNdVGz-ENrAhj9WP`fo z*G*iJ;%k!=xl*D}M}}_6rz7pwrYf3_D^jQ_Q@H$gWs1|Ax+H1-)%0faL|)7#hpvhW zT`No`T14A*kt8a5aRobfRFk*`B*w5_SpQF!-fFtFxtJd&?bL-#x-;Q6SMrV`jrT?0o82dI=%mUN7y-NahP zC}~Dqp{IHStfe5D5Q0LZ(%s#Ns31`U>efKMD);aT+Vujcib(3WClz0KcHwqk=saqf zOc7zt@FC5+N0q;5k$Qq+g@fv7)Yr90!Jt85AF0QMAu~|t% zz0=)$^{Yuv^#VHFJ>34oVzfWK-p=-K?h=MlcEFK>^Jojo|Z_wSC5lvQJR{Rw!qzL7Q>IVrt_|)W(_GB{138s5UOqkXk^US;FF% zg%vEH1!^XDYf8V(XiBk`yYMul6>RDLe2IL>=nZ;f-0Um;Qg)`rVrjLVXi+zx(?)MA zWwoqbixkS|v`>a?VljKM1kb?jxR{}nlo;$*yV9@whH&@ZMJ+aA6SaQCCq%6qgM(A3 zt`tnlR)2&P;FJawHGT@IiJCIIeK^JUC{;g?l`cAHg?NrMPbp0yD~%D$&%^zC0*JdFk}56 zl!HP^7(ZX9>?6nv4=qNlhJ`G|5`$2*-i3!2l>sivwE7mB@(SU+M$y1cu26;>;-Z1d z3yQ6VLJiatrHE-*ejo0cWiu}z(-u!Au9;YO)~V*0MY9S`@;b!DI(IgMY_K-C)U5H- zMsUSdFg+sc<+*Y{kM)O`-E*b-wl)bQ&z0&NO%o>5MI5_2UVMK-A6dOWu5PKBeyCUt zvS_7K!FFgS?RjZiI~*AuDaIrXPw~nRZ-$6StHd%OJnTeI<}fm8d1%3Y;l045$VDjDWcuk443Go z&jC)YmzY2yog~3aE8FO%R<3p(dNnu9mKRIsqnFsfi!bVgDm#Y_W(54+5WwT{@~h2CpiWoRpDoY9(LXK0dNjK@l)bL37%)9nL2iqx2=iZSE0? z z`eGT9KS{#LAkygftm*`^Dgr4af0m2zXW7#}s?f6c@QSRx0A*Fsv#K}9stC(ElB}I% z9a&`E%`37#3Q$%B$x0qQQSZYifyplJym9>|0e8$$Pug9EFu8yXf%&TDfQAnZjYH3D zoCCHs%mJ~{8cKYc1JphZcf`Ro0BfI^18_y}8@#TkLzpd|OoO~{kSMg7Cnyw?TI}9V zj%+LaK2cn*_QGN4Daw7M*{+lwvMy4Lr|xIMGV0PrI_`8A4lJ0%1F+sl6_&z6mgjhJW#T44O{ z3^g&$*pwj(+ObYoZs<3Wku7Z(_m)P4jIVSXqXlC$_*522Vy^@$zLwK9w(#K8`M| zgt+ly|CQ~P&)7D2q;l7n;l@iHZf?BfVtFnBxmdO=UUo*#*cxY<({^wi zlT2aP4$(9<+;~|w+<1A-s0%g2=Vg0f%%&&7d4AC<4pI6q8`#&3KyS@SRXXQI1Kq0itQ`!9lUbW=T8aus3lo}{)Y8`6-Gxh$lsuW7 zY-;ak!`ZFc`7sx27Xnq0;kn9YuHv4C{9E?m-*QI#v}k6-_FQp2L;G`~Bo_BG!puf6zC+zsnRCliHiFgh6?GOwGJD*T4!k^(c#vy z4Z>8ocNRz68cNYZQfOKI7E!k-o8N_{2&GQ-h)Wpmk4wpz%;qw$?dP#^^(5z(`xKiT zI@?tSO>W{3yVEeCz2@9OCMFSa zpfek!nVZsxq@+1)wZ#NCE2R-x2imQ$6`5!NcZtAOmS|I3ZTX7KS{GM(wCf>qS{jkN z|B6g6eOi1?$$TEOL89`ijhY_6{!qj8n07_#PFTiyl31Z(z0(R!o-8$K$dje5&}Oni z!{j#53N7u6$eX2kfR23;JV3wXzDRM%#JyQ+h304`kE03dIPHrFU9m#*zS6e}i)ozs zuY#9nYF12&jVrXpa;WLkrrchtMXblj)Unl{HF@OHw=Dr3#s_M-9CYfMLLnqqcMon~Vhi(u^Ct<)uA_fS8 z8esB`E);?ZPBf61Oav$dN{7jpmSIUmn?&U3y$?ipSywH7?Ul6PK`CCT9|2go^g zJ(ubY?6WQC-1V^+aH2Q053>>}Ud1kXWo$aqt66Z0hh&q*kZ^RPa9}~BR8GaI37kUY z)HGZsR$IvQx(>k3(q_oMhpt1NJ60&=W-8sa%LqY!3}?H2x#oN_iO&pusFvKobid8i zK7e!9K3ErgK&Wm)d@AF&XLMre8yc?wpld<@T4V7=T0Uip*Y{;1!vN7=*iC`&C$+8;ISPXQTF3Q2j9Lm9nA2)$LRP+lG3VNv$gXGB)I-mReJx zSjJ>htI{jQjgwj`TKQo{44>4}QTN_|lMG-lZtK5uBYW}R_nfO2PyagFL-|f3b+fe) zcQ)Z89+@uMKNbgDeLTuq$X||;)HW%Wj;udl_Cy10G^~x8%g;?#f zd%?oK88{DW!joRruoh-;4RyEbtFIMb5;wo1&`Zf!w#5?ZjA=X>Dz_WR>Cn}g=0bZ_ z7C*=-l0g<6IK%c@CRTg79med8$>Zfub=tl1$Ftar!;{_uuvW8=maaP{kKPgFLaf|* z;Qu4!u@?G;4XmkX{rM6v$!r3J&zEqN6})4;d#qj1mIAL{K92?qiYlHM@OmrFBud*d$6 zwC`M?Ko~IXC+yaPfDN{kwCpdz1sA$VP6I1iVd+A=pueEByS>g^Gc{-Vh4LH4T@o1>^9AoqXY1pP1vG!$h~5^ zIzOdDV8<_gY&{_7c7Zl2Pk30uQ*y^g*@ zlirwLC_U)pkAR9FX0UF;fz`};g=Vi!Z`JOHZ2CS;@3(0dON3{!P~(qXG&rx&WZtS_ z#5Uzc-BLg;b0A`0p-GKlbs%oI!7DT={5eJ*)Rbf7QA83W2Id%Xwjj$9>40E`;C#Ua zf{O)f1%rZYDD?=W2_`iH`3N^SBCDKVc>I5|WRA>d`i4dz=U6g_AL>N;VQz3G041IJ zvWq_Vc&SI_huI=y66zR!HY*$OVHPMq>uDp`HQ9T-bkt*#ze$$ZgF}u@lw4eZ4{>ZV zf6w3`%Hv%Z7)IHb4P$${VQjn5FtRT)jM7ZQ__D_^zTq{D$zu)U;T*$w$Zr_QQw-zs z>7cJOjGyCf%@=D8V@I7~TyTqFd=fN_M{hHX50)FoFPag4g<<^mF2lHPonh2{$1skB z4dc9f4C5uV_s9PM5zm@2<0?<~thtqAJri>#=74=p%bm+YHA{e@W{pAH)Lhq)6RKMo zGIDBbLN!Ltl9m=DhnCHa+G2TQs4fT0CbWcVZobXPxw&!KvbyCV#Pd0y%is@LVT=&} zMZo$;g#e_9@e&}C9(%?=rrk!uhT&cKOTph5p_71owmW!{rawO8@JIij=y&JIa?V{uo~&{wS~gc>mcpX|&x+K-y-y&>xCS?6KX>hmD;E zI!=ba%9yVq9`tXgfzA!v3}Y+si}<5lOb2z%G&uRCA{|+ zrbRcmG%c^Ki?AOt;z%FBB6@~XA7!w0QZ2<6@v2x7YHsEtQ8CjHV8Heh6GpCG%sT<&uRR+ zhfi*%s!t_y=m|BP4@deo&sM!?D#Amxn^d`wFwDBIi7$V8KTfOQAv21 zR_N-i)2xW<|L}@2=1O3+JQ(lYk?wRJo+i34j>q+*} zgr_HYCEn$sn&zJ5RWOC>m*284c`Zylm*}%@Oc_*uI}i6H&xR%S)aJ8tx%Jd{{3(** z;@_o&e-48o_byJ0q_uaY6!f*7jP#nL7%C9Vz`L>pQzbGhv(^PCo=#5OMAA8;D#G zNiKqyz5~5|Zx5X8PIh zQ7M#TV>bA-A4{>an1$umXzHvi_$9o9EBP9O7gXxgCA__x3&4fCY|OvO%3@}hKTI(K zL6&YuApBfznJPnZfg(!Lhm#rmTOXrVv|r6|36aqrfV=Y$-jU>dypi*-ygh)O=@jHX zL`XqRPjz=}qX$^pHtok!%S(f>G=j76LizQTnWKteG_#8Fj)^c-O;@CtsxTtuRGD z(JJ8j@<{K|S0laAg;0E->MGtk5J_GPfpof%rbyO$knWBLp$lzBC1m2eFQwZ{Gj0B8>Y?!jtuej9Usb& zqoX!EA8=oQ{MlDj@GD3DfW37?Y8;bsP9~0f@lh_$3eOB2y#XSXS*mcA2R$Hk#fYoK z#ooezsu`%nJC%N!f7EjsirfP#$C)F$VN)91j5oZo7_j3Y2H?E|$YMccu^^TQ19%5n zvST4|iVh@w66uo+%i&IB#4IUg;H)zo>r%=ec7T+-UePLtzZ1$Ie#TAQi7Y@}K)Msj zXFE`*&zhHFs+PL%c>A%nQ|9?~LlYSdAI6%HL!Y`m4~1EC0^#%zBZ+K(P*2TCEVk~B zH!*Fk}L?wrz>@T8sSS7I_n1)4awl z%I@vL#f3^KW)|y0>&PC}MzbktKbDRitzZ$1x+tOT(b{FxUg0g&RZ}uMy^5Vja{{KvPU)#&NS?}B1d!S!cwzmiLs0~FX z1nv1C8#-5XEHu4!03VU+27lu#xHt@bE-G4ux3&yzQf(iP&GWOdyEWOZe0;Or09D|frvqUW$~WV^o~*5ULA%h*|sYBI^r~hWAiw}-`VBf$e}OvRUCkr)JrJ@U(Dd!Ku~rz zQl*Hl3rPE2V$asG6zqF}0jnhyUfGI-)BmlE){I@`4S7RVF(u$8Jh*W9td)S>Nzm+R zsGOL*a6bf^;WAOs=}x*#7v|_x+{G0@DrT2M#fvE>#if`O*V&S3aj^^vd!~`%mZGRs zP{muc$o~7Q*u_y~=~|V@TU5T$g(5rijUv-t=clMRCNg$?ATr%4a(St$e^Kj{Dq1xk z4Gs>q2+;$$3P}o@%s)`RnqlcIZ@^*zM_C&I_ z!enMq73QcH@m|v+<39$rGVcW^U&PhhUlKvnLI%>(Mn-pmaCdyw#RiNHk!0FLlDFcm zJ863`r_WJ+dyQ|9_3D&n^9SygCz=rX0sx29E(KUMs$dr>_65!Z8o zLVgc>E)g~-{pBL~M1)NbQ-7UZt!_13)as;$18@4hRu=VzHmckj#gvx9*guMCutq2W@K#`1KzH|CBShi!mulwHa(j`6@pdzwrc=WF?JrxYI#vv*dwY_}hfhy3Ib#9r znsemaHGN04p5OW{IC%RnRt$8pV4%Ju?hY<*BgX8AQg$Q>cOlJ9(fN%$9Unn{Q^mcw zY!x}tcN!ZIIvZ73K)VH@kG#WhuIY%DP+(YCeFtT&R>KPn9pMJ};MF>##-us)6HPNcWJJ5}{ZL;CAI zjS0_1N1q<*k8}ahmBy|v1=c9WZ|+B8m<;T$W8XYcN#KfCW9VO@LZl%&51pNfRgUOC zD1)>=I=<)=std$?F4B9B`1-n9XT`_pV~i13BC|D4H?6Kn-_ggxrH@*T&II4M^%l7F zMyJts=hu(|#20OZDxB?KKl*v28f#`AEOa-Aj4>^Sr>VJqSxtkde)-b6nwI({^$qo* zJFkKZ{^jQ7`0ET$Yg1EQv*#Q>wEK+4Wlarr%a(^!C}ro-K?NwxIi7%yxT&=z=((e& zp|wt}%gO61!>DMkhg?f)nw#qpeN#1>pmCu2zLSwXN_JFPpd*D;LkF^1uHF ze4e^Si+17~2Yt<7V}C*IEpRs?Hzya{4GwZ*UW~rEC6wXIi6t=$A*_=Lv$1e$w^2woNi#{w=^C`Pu$5;E!pvADa1Bk3age zpG^33KDOJPu(Zu|r$3uFY1?i1-bN1mv1PX(+aD35jMy`P{WKqx$wlD5#~)>5-eJi< zrehe5w2KX6p3MZb!${}YX}zY*s74tfd9iyd?W z%s~fz2h2?ldM(T=@kjkLJ>Lbc$DhsrZkRVa==)&ipDiDZvFGf)AuM(|o^$OLJp^}- zkZgWGhIuRg?D(I6d7Fd&1F#5zW`t&f`v*VK}eXEf1rBKJm?V#B$WFv)1DX=%xs zY&6l!l?FSToP47`r@*)^C)a1Rty=N$-R=X=j7%ajkiOF0xe4(WLj3gJSQ&~ShF%G4^mNpyorc2&yPs+CLz+i z$trN(mE0^`q5Jw4bo9$ZOKG#LsTrHw+)<>RvZ`5HTVHbv?UpaCxjEF>tZZ*z*4nU= zmNiS^-b@pOYObqwc%*TeBaE00?3baQATQ9|DP;Xl!1;hFf~7?CISa+TR_NP>#t{G& zZna>$peeXXa0@Uo8)poNV-4dcV$ZV9_`3yJo=Nu#zAMObNc&TQIVcyT{esQF#4B*t zL1;_xZozv6|4Hyif{zJ4D|kroL&1?~>>1BQ!D|I?5?m};FSt_hdxB339uRy(FcEzQ z`HmJGBgnpl_W6RPf{O)j5nM(@J~a!yLg;ls>Ukp(dj7t+KOp!+BEtPx=}r;?rkIfVX};6Dofi{LwgCj>7) zPlvxkFjw$OBGO+-MEb7>;1(jv*Mma;v*5o8J|p^L0w!Xxl=MX{SBBs(1y2DJeTK0N12oFr z1x(DvSPOk2=@wvO9_GeE4+>s@2Ab|;1SbhzBY2(QJi$uAC4zSdwhL|&yhreV2|h0P zGr^|>_XzG6d`<9O!H)$;xV4-Y2xbUgDmX!KvS6X$bio@0%LHo#8wKwaY!@^I?-sm| zh;s5zg4@LYUx6$)PZLpYo)h~%!Cwo$FZhvQ5(XR8`-Otz1q%dc3RVd&5NsA~6a1dw z!-78(>=t}m@O{C=^L4x<1;-0c5u77g`^+fd3n}zNc{FPvz;AufWCP7T!Rf0DN z)(ft{ydQ%lIk7WzW+zXd);a6U&Lm&%WHsmiJZE-Lw)yzgI~(VoP`MqC{w?s|!zM`E zAi7MwA8%qkE^zAww+o`*fa5bBEI?(J-6Er3ka{cZ%WxQpM@A#A%docQ?azXp@%BK{1{M3s?gV$|RJ1lYt5if^#aTvwmPdI} ztA=a|p^6=_@yzvjz?5Swqbj^x8LD-HL*8tAdpm8ADbb0=r9>*d8!yEo8MKmFc54b$ zHwg@s>YyHuxOgrJ3;xtb0T%e&n|=Zlwi|qe?P^1yoxg_T!ZI0t@?71lLuIAIy}fYb zAlP`o#c*$^f(;I0_&`9w7Xj6>44kI)hP>KCx1Zx>pIHjKMk>wuP;8Xujw3J9kFJDxX(%8iT7=V)z8FSkAx!QYD3XZxr)-anw zUJs69bhMHmqi}c3fl=k^Zk0#)`QJ1bUg;?XtB_k8mt*}g5G)1PU?pBCW!ZY$2Ib}u zEXrn=dtiJWM%HPF%;C7o^zADDEnJ$o(8UX>k-=vv83Js@unCe=rM#Zc0P2FWi59!P zoaUjtumDLphA*Y!o|JItCEOb3YM(N=>iS=9xp>)h{f*Dl2>C|t5{0}KY0IyNXt$b4T4HHhlfb}#goS#|4qDCRdJ3SG=a57Xw( z5n+(Z;DPk_(L~3tjGT9?czRH`DkfBNY39b)m?pzlPHdTC7=)>pvX* zZ>UVkdQ6(-8I+Yfk`UNmhR%m2zcgM{+?`fBLu&xFyW4|?gz{|&LB41hxw_AoaP%>} zqJIE^<0?1>Q-xHMRaI~!-obxllCe>(bqyZTH*ts}fPoJ>LHer>J{t2E8=KHy2mFC$ z6?zN$s|-F)e^20rJ}}=tUXzI?mc`sExePiVe_3VpE(oW>Ns@w3+2NusaH6yiV;_pw z++@ePV~$~TuGo51d*06eEi@%ge`Pwa{bi8GAE-uZn8(#RkK+>jP3&?}QC*ern*C>b zR5Sz5{5nT`j7sI#P#>zw6f;Unswa8|XDC#B*nvAQ`Wh&RTF$H;fP?|$4<)VAlE$gr z&(jxSY_FH=-qJaAihdvG9CVs9t?90Sbwwo#4LO#^r+tAp-`>*pM5`o_-Sby?i_3!l zjzG}9h>T{n2+fQUb;0kjGl0yj$8oR%@m7_iATYYh@D`PTGCh1);4RV7kq-(2hY!&a z2*a@Z`8!&6`?Y_l<(wH>CgmpLIylrkCWq3{?z6OcN<5zEzwivl_?JN1U+GE18F7t` zxUN7q6zhQA`nX@63e(&6F)lN~r4C$duCqW_f~IW0!{Lq9O0neBg`O}-97|*~fLZUyP2`}u* zih-q-(~2>37-`I$TRg2|+PT8Qt}@_J5jk1Hx@4Zm@XWbx$PISamXWjtP?2Laj^~H$ z*U$g23G04v@9kG_efH=lH_YBypj>cpLENAHo9$=n{mcLEmZiZ76w4BZTIwm{J5?Fg zD@}};hQ`h4cEMo)g_){gc#J>P{>2!f(XFee$ia&i^v7o${y38OEPp=uA^$x5@gcv1 z+Nj=i;~y1H#8g*3X_${@i*WnYY@s?Y00GwwZ49XY(d)yJ6}bO9Sn% zhrc=+6Ln+t~qPAbpoTY7MA%m$Yb-fX&asPf5{%B&4>7x z?2+jzfxyTg`+F*zC)0JYO#n$>K8zkajLscK8?tzzzQ@vLV`*bk-Ew0os8F4;RCZbH z-Ik>{H#D}?DgE;MEXGOXAUzSnepP6ekwj!dwb0CW(j2hS|8l{Vv`5<5qSO8b(#UHL zY7$U+CJHVh!o40y|KAgudllrX_8{QS!ocw5K!*Pj5%xR8eo*KypfJ#%+9QB_18DlY zgZALRi!{P>EuMHM5$*mELF`?bU`VGl#>e?%JbeIWG5q>-+xP+1vn7Le}u3(Ylh zx<`aoYvJ&BOze|Uff#P2U^d5WQ+G zK2sdFrdN>X|CBX7|HMgo>`gzWa;V}e^zYSrir zQx^}{lW-+K=d13Hn;JMZCLs3hWTJ2CVC|-4VXrKnF>8&siXnq0&*kS;9UiBw@u>L0bq;h(&=hP`xK+K zwrtSz%MA~}_uhAt!iA3~;GPWgAd()wE}`u$mDUS8Uv0Y(6SuUaMQOLakeY<0zJgcW zo0g!|Bciq7#9L)Jzcps)zCcy=4SOpQG+*APmeIN-3(h4 zk#(mjtNR|wj)cS1Yv*C}KHilW8jJBT4|5uNB^O17#j3N*Z0G$1_%flL-uS>HM(K=- z^Sn&$e-DE~4@JaXd?9rBfsW18Zu?eDS30}ezK%H*&K&A_mf7CL)8(tYMeu1Y*d6FR z+{zycQH5=$fyv+CEEX0N4D(dmPCEjfzl#pv*4NNswNjl$mcT7dB zFsIPG2elj3PYH<#Kxm1r^V?4+w2nZEd`C?c?QLo12T6COCk$FQWE6b7`T`Vb6)KTQ zdzvpa2OlC_31X8J^^)He<5xIScH+7$dO%l=ibqse>` zo_n~MqgKl)fik;052(t~Hh%9wA_{xA5+wOd*q@YCb;i6fj+?n3E5!x-+g?`<;7V8L zpnKz!sCGy`)R&(7zS2ns#cFd~ZD}Yer(m9C7Fb`(9n9@2IB2Kb9bV0FvAdM$4eQVR zYTEC`@nN_ef+T3={8nmvIDIUla25Oqf~)ooU(A_N^XTdou{C{6m*GD=E5Y*LU$D>J zaTKMj?P{!+WuP4OszgD?5^qM*^=X#tZL8vKm(nv!S26S!T7L#QKZ5}2EYa}kM`Lb_ zfz>1Pc#{8_g5R01nXjz6Tcv+?dgp<*zq8W4;moJ4i3z;^hGqILGWMayFab&9cgxFb9`|-02(M70uvHHtOh&B@UzJ$`yEbx7y^KcJ4(rlhu z^Pis{cAIK~-)|dft!1-|rf^nx7JzM<*|UZkN;|oZO?+bL0*d++n|<9{A)T~|we_Ia zbw2=gs|8Lt*$ZcB%!;Ew9&Qu8iE7yHJ%u2B$rvW{$DU9?Ph#b<2%_g>zp0SD3fiJ_ z=+1>EFc9W9vEc@9NoAOy;D%RuORB^8Ji7|MO3L9{qFvGI`QV@1e#C0172hZARC^>< zC{P@f@S{p%DTE+aA`p>*h}ECle~0oP_%q?wB6*C5V*tiOWIRO1L!?|p%0(RVuh*ve zohR-2<&*YrhqwLP;fKfbS^jPQpB?TP)1gZKy6yp(qkgQN#_B)5GiD`sy=7Ir=S5`ot$}D^gS%?g@b=Yo8>$D1_~a2 zjv&;@a_X#P(IynVJviB*Y7{Yg9l}Acs;eBq3L5<`WX9mB7aGTZBl@UbUE|P`4|u0p z?&uw`MZTzTs*8<&gm7@jAwGO>upHM(T!lmWH#aVCsc#A4Y8_9hhga{w4*#24L!Qgw zxroar|4+}}{}qngpL6Z%e#|uNhjV^D!`H6z;jka}^h#~A?qm9Bj1ihcC_eoTUZm-d z&p7S%m^xR=I=W1LW;F=lM-fM_sc@7uRyGWWS8eodx8Y1rPNQm(w0=7Y`Cq1`ZM7 zKL>@M_zS@o1s7tX!8MM0U?R%G?L;Ja4H5odBBBufZ^3V(&?lmxewPUQhXsEo_#!X? Ye(yvECE{a=Jw*6>6-fI$@Fe|z0Uv;4!vFvP literal 0 HcmV?d00001 diff --git a/moveit_planners/trajopt/trajopt_sco/3rdpartylib/bpmpd_linux64.a b/moveit_planners/trajopt/trajopt_sco/3rdpartylib/bpmpd_linux64.a new file mode 100644 index 0000000000000000000000000000000000000000..8c3924888764137d5623ec88e3f6423e22f23093 GIT binary patch literal 740972 zcmeFadwf;ZnK!;eju0{B1Wh&Vr1jJuZB)brqb8DS4xA%9dV+{hLW@W|Nl0pL=Av;1 zLoA79cjo}D*wPk9=Kaz4r5$|Bv^ax`8bC<^FNin1U}@D3sn#NO6tCy^{XT20lg)$$CG-Pis~S#?{Y{_^n`hK{|8iV91MFD@=Eo)QWb$}8`0 zC{$c@=_M+~qVbkxy=_@JtJVMc1C}){YFSIRSk~$}$Ns-pZe^U-Xl0akSsA@$R>rz@ zR>s!1kNw}1Wo5jWYh@hDvBm^9T4PQ>r2gl8VvV_d!^r>tid$p;`LH$S&?GDKlr2`K z9k()Xt3UStlSx+K)a6#7Fl+^qL+byhPgsHf`~QDs@E2@g?D>nVu?uQO{(rd28q5E` zUvG_lX|FZ*lLhiW?!p0U+`QG+xMj1{|Bkn{9YLb=pn+;ahr{VwO|H(DWt&^KJS||Tvoptilo2`@I+F+gh-gD{y@fRMp(*IQf zYy3i6{eSSBH6H(`6fU~4^@i%YnuhAdmfYdCP3{&iZmSj8jm`Xq$FAzYi?{5SW9Djvyhu;X}+blt%JaZ=7x^;7I|OS zo~UlDC9u7tHKAT7aNF2SSX*rk?)BU1W=R3WZ7sJFR@0fNucZKPsBLU$YLFC7b=8TE zmNubQ-!npW=i<5!kzb;{Rr7)yCEL!$H>!7a%{8qJw=jO9p`)!kfy`#1!+R)x!o9Jk0 zX`VhEudZyXJtpdmtLp&4i|O^%&5LWNTsgC|>Bd@UZ1FX1HMMPcF$)hJa~hgz+uHF2 zTrJHtygP9w#MIK%+E&}%Zb-eZwqtIqc_4RbWIE63={%A8y4J>q4vB3DfN;{ciZKXn zT7%}Qva_+Hp%q48D2mlA9(gO|G`DxObtXD08=9xLz=*D^ZLMyrhMMv6O6Z65wlvo^ zcaWu8|JD{Grt+HMmfnp!(MYLAP6nAJ_p zcX~^8TYGIYBj9OvZCyiiIyL|lo0o1rGj2MkWAjLZJ@~a0yjWmex3<$}MtG*KMi>WH z#DPOlH!89=7-gfgYTKH?7~W57t|3L!svE1D#R`$Kx~aMguV!7_S=(7ln$r(6YC9@D z?`W3kchTC$T0yDO&26oLeW%}6G`G_cKvlKPwQbdn$7VXctrn4$G>y%P0UisQT^p(S zc4vD>T0!mAU9F-hfBKm%mDTMxA-m%+-!StV^U`pWS6y0^wQX&+UBYo?bw{H9hS`;f ztZuKfVGi7M9y0Y>H@=kkK_5%D$VzX=0Dua`NZ)~f< zyODBu^{rxMBY-J*{f(B!5dfZ*ejENa?Q$yGr?)gW!*%_J#;NE=si*6ovOfVQ*(9>3FkS zZjD}fofk5_rBOi|s0Sta26#bp3w_*D-6&7Wjn5;MqL~;sQK$9^)4gzn#8;71@LFQM(=qs98 z@f}w+od`x$Dq>}Ad;OGS>J$%rq<%X#{s^3i=80E~+)~}ph^M+^oTPrzUXYGU6-Z?X zColx1JvhAUP?&&DJzrN#VO!~!U{@%rP26PHR#V9hBM!jh3slKQolc=QXa}oZttsgu zTfU|TmO=`MgoM;k--&wEH`0J;Fs3--DcYM`+L|bb_L>&SroFZ0Rz5ejES4tX;@ak> zx|SQ7rTM2Gq}@}0Q?qJ0HYE~h1Q3P*Ww5J7-vH9=X=LepMr>)TNpwg9iL};8AUJi-E1blwtArkJlCjZ zXM0Cub(1gyLq@wo39Pdf+EK0L&el%V)^5M8sZLRQbE{P4%n~ANZBt^rtqF=ItiH8f zwXbnQj|fl)8-=`Kt5_kHJA!U)@1P^Y-QsHKNAz@STSHT8yJc0^)wQEPGPy+$N|>>T zDVK&q7Z+ZPVF&eFR9JY)CB>o8B_*Zlv52Ly_?2TaGB6~et_z_x^X6BYuYME(qrY`%W8US#`mAyQ1#3fv7t_94mc!>E|mF+asy%6^TF(9*;zV z6SKEVgG-(5dWg2Q!&6m=hj*Bsl+#Bf*7l6 zi$CLO!C~f8oY8&1y=+N1&kFW_1xzEGdwWzU(y0s1-2*!EKt~Db$g8VcH5%`^h*Zo- z;gJ{~n)1hzkXP_CF+ZdGRbb6Qg2H3C19rhrw*J7Mk0@f^{LaH*FlQ2kM)rd9|A_xx zV0Pgj`ST&LSN(}>zR7&T8Qs4^KKbso<&n~TOTR?)#D7Gzki7}Oiow7xs;7I{y(arX znn~&IrFnMZBg*q%GfQ_vN;$cuM}iY`r*s-ud zd)x0ar*?)Zs)=AA`?T&Cqi$tZ)DhK2r&bm$jp5}5jH@t!_{UF-r2r9gC(??dpSKXq zhwvBdrC3hu-T|!HBUmQ|SN;GP4dzt}HRX62HFPr+2#=rO6v``fX6HbTn5*i@04`eW~s#5x4LGWNSr}*%RW4Z7fLJNN6j7 zr5{pFxjT-`fS1_T{i-rYcW#c+iz-lOBe|8ffz@IIWTy})DiR-7!lWX}+cK10SD@ZR zNj!38FeiV^Rmtp3WKs16TKlVj%|{&rqxeGiCYWiS*G$h+h15>GtIE3Z10Ewip6Q5d zXZP*uE|F>>tuA+~*g!n78w~)ItpkEQI3y?^lvKT_mS#pv^L4-5u6o12_{Q)KM2Fven z=nURLiT)Vy`R73_^Z%8F`Awvl7#`RCJ$G)lA+B)gt*Q)@hR6ByB#EG<|3xASk4%^L z%QQIYOeT8{*(`hv&;`ZgnB#LKY>0%h8l69$KXl6P9+?J@GOpWE21c1hAuJZJ0o+s6 zRn-01*yz-`#mwZJUHF}t7hL%(MhMQ^2R9(qF(j%hLsPsGPdjKu6!)j3 zsGFMc_n8&RB^T2X=1eLCuAHw?PISog^&N!uRlp{G6{rIj)dery-hL5ogCyru8VgAz zEk{}1leMSeW28<2+ZSfFp9I*%tZ4G8j1fyL4z9Rfyl4|DOsROIQ=8!O|Ha3KHdhn_ zrG;(I?rTu5D;~5gD&P*UPL`bxVdfOGrn~~4?=$?XFcPeqN(!b@OMx>GYeC>NvH>bZ z6eH|B>19V+FJx!eiP%a0tuBeEQi3byvQ`yjeFL4rEf9#uXB-Khw;u-h7IB?_BzW!f zq~m=&?OWGJ7DTR(ER5W+1C)y-h8xgg!Yh6CsPOO=I(+olXR9Yy0jvxwqbnzS zT-GXPk1JpQ%L9I{9;5Ce6LOaEwoLyksAXMnvbvi#A!p^7*o3?lnXw6>WFR)7ATpsa zGNEMIxI4%8pLErP9*xJEF@5@Fp@JDR=bjt7X!4ZF7lo!27G6|%ap9Cu!R*?akX_vo ziZ?WOcB!`)6i+$VvL>S!)lq#T!j3i-)|Y*kDqxN{OUoh}=A$32JoVZ-N5iG?YZ!w8U%uv*Kd(u6L7bzd z51E8IH2E8F5pGDACf9(|`4^-i>c*ru;dFqR-k6Jten`{bWdJ%f=?wUC-1vFoyMZsN z7=TgZ?Gua`!sS-TD{lxc<{sc-lQ+~Mm|B@iGcZK&m^mL{&jrt zT>iXHBs)eg*M^p(1xs|N;or|ci}Q){pQROg1KxWtfBxQg-h0LiSnqmV)X3Y+6H1J9 zoQM*_Po!bO#hZ>G#+@oBBk|20*~tJN^K8P?HQdO>z-c~=Z_2BA{}l~4_G91xgC4&Z zAe3-#ECnl&;H2Z;h;s*DCSMiXvZm`$hpowx@a!TqTo zy_kC7Y7L*I?a285xq$m=6m(`VDq&%us(Esu)iAlpx@qzh3qy)UlP_X0Wpc4KIX-;~ ze=g!r5q|`BF-N#gG|{f7fUT<}6Xh>Hhx<=Zsq&ibflu(jf9!#K44of%;NE;s(M5}|z3|de z;8%O#Ui`B?a4(+DQQ&{)gCl#nUh~0?eEu*B{NqvJr|N`$zRvW(z4U+01E=pba+~6T z^Hx7vGe?1UjRJQ(aBsff_rSgRuJOQms~@d_QQ&)ga1kbc!rB%3AxA*c+B&BaD(SDAKduAy*@Z~!0R&&r+hw-f0J*v zuKbPM4E&ouIC1bw_~3?~Ru9~(-&;L!#x?QRi~`>?3jD=U;J+UQo}mTk=PTxe3sd-+ z>4O_OZ}7mq_Pp2w_ww83fqVJAYZUlTd~ie0TRym<=lxOOdAdq7{F?Y@`rrnClMjAW zRU@f2KDfd2fCuj7ce@Af<@Y5Y+}PD`d~ie0>nKN>13@^&KWO81G^N{?vgKF5d}2PbU-zWsK))Aq6+r@ zQjlR_6;C$CDi7k>POc6|8Dh-lkc9o=D!w;EpO}JSpCXR6@YP`w!mOzt8}YXg4n8-?dkiXT680jDJDB|eoTIO67=jcI zV+bkoesy@2yzdV$N66Ws9(k5Q=LO+b+nK{b<~T=pqa59B@_ zisUUAVA~qb67n#b3-lZi#yB)+;{#|OkUTLRNLj7J!!^mk_c)$QeQ_wefCJTq%oM7{ zz;ryR6k#X(MNM(%NZk2AMwV?S5IzK2g(>PI=3Z~b`d$h4oeje`8m8ueJJ@>_+Jb9f+1b8%>sHKoaFlxN;w`MVuWRO08Q(T19|~XRk`;-p%YQ zo#!LV$|k0Q{Xdd?H>>$JU?tb_WbWxvH#;Zdj*mHGqNTf>rdp)}mq#(_ zm)aG>oMMrT^~Mt~q;|#=f8v;2?sL;zAWID@wD$?x+_$@5(X2rVsU6@pxZ)gmat_fI zBCpEC?&;2w<$;*HFc%u53?NkJHRmPRP#Kl_aliEg>^FN8Y(TALx?h*b z;BM);%;2a*dzJ<;fLjc0j!Es9?gnzEIf2uW3=z9uj6nOCbSO(FodSKY4G$rKo!Eh? z<}opMQf930kKL!rP%_|yfid_c>mthTr)lLSQUt&i!pdZGD@$P(qOtVR0idu_tQEjwd5?2ZJjht@6oEVd>1BAj2NsP7PHREtAm_=I~-Y+jVYFDXe)sV7^ z&TIo;I@vNGrrgq;@GSbK68feBdZKbh(Vl62t5MyR_O~%XG<}qf)lSX+f9;fU=5~z#!Pp! z$INiE$I(+gz4T1D63UrMNDaTY01FYslCd%tb7~2oxb;jD^>s%byWbI6^WN& z7AzmmwusyD(?G<%E|7Yv^o3ymKOuT*m!0^Gj4&>2nGxMN4?~p0bT{EoQgo~LSrK=3 zfbJ#ycIgYZ?*)qu2vcPkPwaqu$S7WhNHI>ZlcCrm z1j=A@%!L+~R$8yH3E!-hX6nW4LR0|wZwN#)v!_JeiSUWB(&uBzNGj^Qv*aAifu)|R zOdO)mp7aEKHmif7Xz2?=Gzp&c_U$i|KbrYf$$;QToiir2WD%vS)UPU377+>_S&ogk z#gIa$!%l$N`Y)xtpYSfjvs31^C#X~_vWO;CU0Kcl% zj7uPAXF(DbU=Ame%AT0+rpJxNu9q{H~iqNL4on_lmt~3b-#dmJ?ea+BJpPw>xyl!k0ITRn%Z$qa?Y0| z#X+Xbgx!I9Ok`T=|s*38I=AzC|VJb6{`HkF&)18ydKQ&Ui zFb}TV#_M4JJNRWSM1*eU&)F*v_EIFO2u)p>wKPP=sS2AFGS-ERam&W1o{l(W!3f@W zA2f~vB|o_G`-sR21OZh@f$3Tnrk#;m(%?63W74$ec-o%dVW#x*shw9P<7nhy;yUnl zJTY;4D)1vjjVHcIr-lKaE6=zqewC^)n3D?*T7@lkANoK3c3X%M56H0y;f z)(dD&F9@TWh=yk&$`iE7y^I1EPE}JE0yzX$Usd91K1wzSlR zX2)hn;`SqDd8;UyobcBka>sKHpVHJ#tXCO^r=Qn`ptS33Vw)ek_$0ivADP+G5vpx&>9ilXEmYUi7OGxsKgeBb+@fPY zw76c8VLSgqE^KEd^R%6NNrCPBs0zW(?~)0{F7?Q#K_Phfhafvt*U;E_K|^yWA)7mD zCR-saq`i=fctdLM7q*PlObS_NU)or6cDwoozR~8OIak4XXmbs5Kpq$7n5S`unG?;S zX%h+uT5Ws z6r@Ydfu)nD(_bMEP-; z*`zn{YXCE*ire(HW)%4u`b|X6!TMo*XVU*i=a`?+`PZ9_bm-6PM6zSlE>0-ps@DlP z7RZ4hKmUaJbq|nrP&of6uF9ZW-^tV~&|b{CnnlReVa}*o#A{-Z@uOjwzO$ zW24H`mq&pYi~=tl1ztJ|+|q-ivh70Mb2@fp&#m6r+s+-x9cojr>_$N(vo8Xut&lEg z49KGy+a2`G!LY0t_+@;+#klAJrLQY}aL(cLy4nXnSAnhB8qR!aiw6HUJ#g>b?Kk;= zi}Ag4z}r1=Z~OvXFEPFs{!gR8Lx{)AUw+h9@xOiyx~RBR4?_R-W6+D}^z~IPVL5m_ z#o`z=2cYM2=y^FPW2FuF!Pc;;IBLlSOPBNG^q?&32V(X9g4Jz+G5*hk|a z4;=xUn2Y3nzYI39sX0J6*5XUo3GvuY<2XsV#NR5TJMr{@w;CL^F(_0Z@iyyrWUsVO znSUPPpzko#gZ`ruZr0->>t*C-zPF#$rlFKZSqFNeC?FhAywpiJ=-;Z%~FFilEFtt76T;vJ2_{|C$gTC z=7QLw5}Kn>3udBjV62^~y1wb11J=^7+s;nx7xssMeNweHdb(4*N`c84Rg~JU z7f|N}SJX>?Orejt6EPIk9U`|X8w`X3Y21NO)Pi2cHXdjgBa5K@cX)|Gim|c2f$rPv zgt%~GP*$bN0C}Qr*vsC6a4@g}zv$I!kt*ApJ{b58!CG`mRNUW`7S$JIB<>21tb|K> zpMN>TmVa6IcWDKFi_GjB2(H)zgfN7&iP%VHt2nj;nRa5goeVt5SBR254!AIUdjK<3 zzj-%!=TWq>Fu0b~$z%u$ydn|e)r6}|(w7dB67KUFNqt~{aM`_*KG3goO>8IkQW#)5 z9Cr)$FGduWB`P2?7@&KzMiy0z`nKs!026{MzK)(Ly#&3 zji~|3oQA5(jJ*cNdIyc75HM`V(e{enDq}FhH&LxPkGi>fiw_14b-@X){5n!-tD+EC zUa*)PJBVJ6ID2WlOAc7m-LjwPO+CR2KMmelgEoWVMEl9aF75*e3{7_-Q!b1bmDJ(=w zB?$KJN7kwBQ^jMdQWsqD7|I-#LZzui=%pFKzPSQDkt)wXsSNgh1dfc8#i*U&k4&|? zFSShWo z#4|Jrj&K;q>E@qde1GZ7I#Fu;BJE}jsOadl;ng_ikCjd*=r~EVTR{!CPbw> z9^PgnZfR6GG?irW^L%AW8*QTp&lvE|u1My}W|LE~(dW5X-!6!HF1D^zJ&Ro%XFxWR zQRl@*blja1z6csza28%-+>j3RrGp0+gsWJr5nz|kg|7#{vkQIsm>d5brh<2Mp2c_P z(L~oFTi6b_^|HZLHr$J#gqAu69fT>+UOQ~@O*VL_sb;hGIEW*GF9>xoM(YK zeV_A1+DY!2pta!7B%3>&FMK!n-PO<&g!VgZ<$E1A_S^QavQ@eYXsPKn2xBfbHpYU# z44g~__I^f65M>Kqu|9ecm5S)4A{jr)dTHQ5#!I_iI{2G+S)be|ao1}!*{dPN?z0YH zdVh6zIg*1B7@UOXL7azJNlqGjAoHbx$brDvG@3hEaMcbSW(T!zLL!qb1jRj3ITP_? zREfH9*}I5CeUlaVhP&8wIOD*6T9f1r6UgIOc>cHpnPLyb!b(zArXc^$A0nyBm$IP# zQae?x6Pa2RTyZ7rUilMMK9czp`(EkBBqQvSjH9Yf?21rM)`R@fGZEOn8RnyX$6z^{ zz=z~sr@_GVawHuYm>yO!eb61!bHFPS-Xt~Cxx-bkGxE1S&0let3>J?YW;IN)Yu1A~ zUng3KzYt>QPTryenZxVA4kM`*F%+q?9o%G~ILzBdWP!17Jx8gMYBG8dVPq>(qQ+32 z4~mLb%+g0i&!Q-hGu=3BQc7aP%_Ijb)uwQ!1|kmH(em0}!z`-sHtOVlH+ud#o;Di$ zfnH z!{xl;^hJrDM_9nQ?MCn1w)XwnpHcQ%f?cgEFgE)cTZAy~`T1p#$``;g{`DyaF!@~U zE7P*ZQ}*S5_GmnLl- zY*JsKEM!`6Vf+9UE|1u(kG+V-@l?GArRU*Uu)xC<1(*H!L-3c_3Aghhq_Ut4cNcyf zyz@H*gN-%F+E@<#)wWdNPG(Po(4I2!6!xelNn+H^&$`OAh|vPd9jEJiOu>!l71q39 zJ3ZbG)#3|&_f8-~w$HTFNh+&D*6So|H90pk8IZLW#^UJL)%2pNMr3}GmQq4WI7d~$ z4aMX^S)OtPG|4RS0JNiWnVF(lg*aQ4Co@yx1k!zrI9F>Z=bJ8um_DCTntKQL{ATaG z{VH8jRC&R13#O_rQm%*;($0xyh_uOm--o6zE4w4rk(azVqx1#k$hx0aD27kM>Pcf? zv=MUao`J5|8R$Ka9UO;8V9|n7!vi!JRhaZJ1o&oV@kCz4&0aOl$-XzPI#<|Yxg3qQ z9}nKjn6YRz z-R-yNB{Wx>wW*(sX?si>^Q>acG;U?haVJ;Jv#Qccm(fZL6oPSe@dQpUIQh!b?0cbu ziP^siCdsotjlr0;>GB0v4&bG-9C`w;X~SL3OysYEY5zFY@wL>p(r1)Vy7ww9Y~yR2 z6Q4HxbIgMNyDeR>!i1^r1tk=yvUIF(%>Mp#C;Kj4c&|>B-7y%D1xLpidJPQSPtWj~ zpoI0HvVJ&cT7M_H)#CMs*TK$V(c)A+=KuVkNx$h4N)I)R9x8eQXuq5d>a5`gM5$Xv zCbj*KICO64mNLRZ-h;?-oW@hnZe{k<_+8lXJXECpdT{B zp&TgtH|L|#_b$uAPK=A~1g4YmR*z7;^jm3Qqw|gSb@0NEg3G>xsHUn%^S%${q)Q2@ zUyf(cD-M7~mIljGP3MGW|3yr@j&aaN2{J(sUg%BXZeR-4L|bRopC=Bu!WlxroHwMb zUaVd9*=Xfh#%mm!a^T0*A4(5gWl-*M`C|MFxX3{Kxhyi!%X782ByGZgG@oA3j&J$l z6S}tR0jAF;Q!s3}Hnn6Yj4D~{Hd~^kJsZ)B(&6Ih+FmuTjooflQSV0Qc3}-GXsuT` zuqV=rg@1b~tyvr^fG(3(Eu!V5bqjZv7hJaRPt@&TWilYXj(skiWdghF(0wSSV$jqG z9V5*jg3Hch8fb#mx9QawS4d@2$y6kjN)3>AEP-MY>YHA0ysEXSVd@hLVz5lejQw|T zRkc!>=y=3S8n!sRENiqq=3zV8VUHECD)#ABH~NfT4}lil{GiOg&c_koVkcgpe#>6e z`n~v_6PDfa04u*hj>#TQ&S0&P{b!uXvYC>U;y%3QkIE2+5Cy|*8Ejz>uYMbn#Ne%n z1&wzolr62o;n{eBraNA(LwuANoGP?{<+T9q`uE?`yZ=L2b+&;;gHZwZizWOo? z3ob5}Wf$^mg)rOH9>P&=II6V%FT@qM_a2={#6y>pk8yp`Az1N zrPJl<@Ei?ihrxSgf6@Cia?duI_nL$U;`FD_(did>BkQz9`q%+`3-1ud}ccIHv43j_Q&ovz; zy@69cCtB;VN$0PyjQkqD4Wtn_ei+}G^#9Sd9&hOUJ55G9^yhUV*)i%tE0f{T=@-5m#{17)>%p}L-0yEL!x!z4aPw?J>ib0J z15iwa8+$PE!x|n!$jjt(KJFQx?|B*c6xw|($$YdG`aW$@gLd*YcT&=Gi- zhR^ZAdo;Yt2j}C7&V^9$u{!p#LFNfdEb-8qkG+=5WqWvjf zlO9ukXZYZzyw4p4zGM{m77eF=BYxut9`?ll3gBj~)MFkv;~P3(_rVQ4e;x&%%fz@0 zUnc%FKDfa@*9SLpxX}kU@f$~hFBt{C*#r0TyWIolt?}DW_~0QjgX;w!JgmUhun%tN zJVUyPBXThC$)mup7zN(ogPVMNJn($bV)S^Y2TsyVz7LNA|6~;S`P$VPJsLbyM}c4K zgPZ#0?|g7mzue`6n|k3NeDGOX&j09x8+z{d!A*U!-3K@E-}J$w8vnof;D(-D-KaBi z$ky?{=z|+PXZzsgI{pP7xL04-c;M8Rx#s)eru^3U;N?2OO+L7h|Nrp84gLS}37bgDV zQR3h2gByGJe@21d?}Hn=9q_>mv_NiC^r4 z8+~zo0x#n6+UG(K+-slpKDe=mb|1W)NOAqG4{qeY$p<&|d(YiqzU4%MYp*Z9f?C$g zKDeRhO&`2mCwSWjH}rh!gB!gBSdro~ayEL&_Q8#voauubJB<0@wnA%N?}2{_Fyqe? z9ysM-@c+~U_v-5j51jFHHU29+YoWb(=KA2}I)0-M9@X%*9=JE(2R(2v{X0Ey=4<$> z)r$7p+e#0d@xwabaWa%PB7Xxv!v~M*_~-cGCjKNJoILQFq2Ux?ojoglEIQeD#F#dUE=shL|Ze)6StFM!D6d&Z*TQ0&LIQi9PZ(Zbr z^Bu1%d~n0pH9okJ=MQ~w!`Fj8c%edX?eM`({1<)jVjcf2AKcLMd907*W$ef3{VX56 zL`AdC*KlJeM$R*R@lC#s8cunhjeo;$y9Z908TuU`+|YTi4{qe}GauaK`%52ulBWM( zMu8t31%Ai}H}y!ab|B=7^c(zFjsmY51-{S+H}pF`IP>7O(g!#3|JesO_PNgk_uBIj z51hD7{0zNU%M z_WYg?ZtVF}4fosgS2RL{$K-pthWqU~>VZ>chW`0JxPG*5@xhH8miXYsT5fmx;Kts5 zFbe#^QQ+HsaAVI;_~3D!?>-;g@b#GwZs;kLndy;w-@qfIz-Rd2#X8^FKDeP1pUjZU z*r%a$xerbq@LJ)68~peC;Mpp=^|S}>EiW&7;NJS;6Azqu8a!Eg{>RX1=*jiLwHU3C z2ky;xk_S#4rhI+R1NYMNpa;(Q#;%^$aKAkt@WGA$|D6wR%F7`iek0G!Gn4=gzftvQ z<@n%+-!E&pzuZmr#P`-0(>!qUW$<6^gB$$wd~jnwH9k0T@%pw8Zut7X4{r4P3lH3@ zujf2);x_Sr>w}wo-|@i>{U3YaUi?{FK}OET9+Ez|(fhA8+^_cseDRGuANRoxJums- zMn13m@asqGkPluWQnsx3HQcZFpq{TLUtYa`-UFx144q&0!3~`SK6qT`d#Mj@>>=ud z8@*iRgByP5```xutvVRS$Z5@Ty0T;Lgs5X<<<^-_6;4#jac1Rm z-ANaaJjfyHL7@jb6`#Wsf*J-`U8{n61UN)*9q{7Ll0kFlLwn7#kAhBr?;^m0E8mCc zz%fSUqv$cn#u!IB=*B4jwG)dV{iL?=CV|t z^AWySQ#`=9xOmS^pS-%C=Q5glFiufbA&eEh&$T&=`;iGR+`g@EV?}S+ zpop76sh6Y3!$XmY(H z2p2B%UmOoG2<1M~~s}Do0r|wgcMBwmv6D z-6@ATQSs{F85}67N}G}pi`-BDLe|tbvW`Xdxt8%{na=tnf7WF*hl<30Wc?~{tueM4SQb6qZhmh+om7 z0)du$eQxIUInm%Rcf^vLI7Jh6I`bmf`dpFtOvuECnJS!}_-+!`dJ6ysU*QY}d+8i) z?%a6@W<**)hK-`C?8ePnUC*-Vih{AGYGkzj8#yen)fV!2;BYxnm>F5S&+}^eFu0k{3;Jb8pWo+NTBL#kkjCBr_#~@pr&bvF(japa3FZ zkzJD^V$VOvgRv51dok`Jjo4xVjN-EB6m7xyGR`m)8pC-J=WwzO)Zv6R;++^ia|C12 zzP?=^gxy~!x`QL=up0*GZ0jKp|7z~E;|LO_-G%# zV2IDksmG`@v;^C$x5_~TQeM?w0904D^O>d&$t-N`Dg#0khaNlXuf&*|PZtkyfUUKiv@LZ+ayRm5yo0sq!gz&K1$GBWGa5!VXy(Z$! zxf4gaDmR0ynGaWiMhB0kwqZRkL`V=$bG^q(7GLJ__HRw>393FTSi|OQ@ka(bC-BQ1KAIc7&&p{a`~gJ~JKq3f-~oBxgZRwsU_u#pU$b z65LZIz`GYAI7BJAF$f*1w9{@FYNxNq!4{Bj)WH`SuSNT0wX{zbtyG;@hD54l(*;d)t;Ok3$;rm)T|@Wi7kX36`FdUp#B zQ3aeV9Th3{>P6-13<#RXVTuhQ{UfdEki&1=j4r)TQK6+c2~Jh)r%nk_yg1!nLxAmL=dIa|c)ZC0s8@bg*2qmBu4OHUSU0 z8IbqD%@FW40@p`x7vq7Oz4Cse+IWO~ zTbT~|)k}Ky1w?9Ri=@X9F!CN7{^UJ)7I?oLu|jRF!?QUmm-#N2XZRU}Tz8Kn2BX+9 zg%#ZOJO+0WX)PC=*wiB7`t&g_h4Kv9NvO|i#q#i3Ok@#=1^zfjum%U(g5&}~xL>PW z?;ccVX)>1(C7dN#)=RsW5W%l}olm2>|_zAdNE{i{fSUGAz0t8#Qc|CQgCOP~Aq|KGLcMt(*M z<>p3*qpd9u>9Ix@3|?%-(jZ{6am2+OuI+#zxy-p(=Gla>t7EQ7cp}dKX!;_g zW&Ts}@4Za^e!Ps!%Y^0nUQbc*m0X2-?$Vf*fgAo!I2$jBON+@wv&wb)sx(C1m~;kQ zi5tI6_|Dd2az@USKb!IHt@z1!1&IjH)ByIzCS&uto$-& z+$VJYohG3U{pn96J4PMS3OTA1&<#)qe*U>6<3#xn=s~(~;=T9s=kI;zy=T1t%(dmr z>pyc%GRI)KdNQQ$L1fzKKRjy1wE_?h4WV)@|Q6oOi7tfuP5#+HOt*H&9=)g>BR+H1LmN0o_C>fUPrvryp-(V)Uh=?M zqz&A=7T7DFmD(ASUoZR;O@tplV-&dSS*yiqSCj8gJ#a7nVGo?QCjOu4bZ{|WmIVV3 zGchj0z49;fz$qgWzsCdj#t$J5FUpa)%%z6$S}x>hJFoONi3(3VHb14fxbzZ<`PYxl zuVSImSGk1cu(52M2U*Fnc{vi&c0T2)kQ@wmZ^*+piDX0()VkN?9Qf`KQbpZ+bMYK? zf3ylu913P1hP|5&2ss2%4ju8^MFV&~cTY4Kz#dI(REMU|-EjbC`~iGmMtN!vdu9g# z|8~*4WEhTXg_Xw?d%P^xV<2nu?L9O;r5x2Q+u2n@*^4h zGxlWcz^EeoDG=p z6dvcwO_q^(^wTqM&%uBXIjaH>7}~Nkob9PWr|j?1!S8+z@*q0CFTs2EBI?O&@_HPC zjvk`$jqWZ(2WZeeVl)T+2|d!nvre3M*h^90+h8(|?XtO_qvA5zA_1)q4-lktVQ+)- zRvjAjqH?Gg=Ed!?L`0g2XqKeH7eFM$&RoLLkHLW#awK|Vd}3}kyX30`fW5-(w+%2pa@Zj0vgNE8lF5|9J#dywf(7c7 zjvPB#b{9FI9LP7iZcE7gpAOO9h>KK5TnhuO%MYSd*h!51fpB?`9l3oy;2oi~pK(OW zUhIIM+jYyy@Aoaoi;T|m^!QijS4b@hi@4MA0f_10;L1x-Yrw=P2VTgGw}3pSAkfI!E#4i_Fr)$=k+UlMDo%Z5-Fk!zk&3e14;TVqW(?ZSTh* z%ui)Sz7-YsHRsIKP(|ebeSITO%kG@3LL&ho_poE z4$`$>(yL>k;)zNWC*URzx&*Q~R>>z%;0x-nNltdbV>i!Mz!(kAGh)fX zKuwJrZW$83SF6$s`g7!2dnzej$h=j67-?XJ8BfPEF-J-eFqN~khj?tP6l#(w`j!xb zbP{bz*rp8X`2f1B1lYkdoZ{+>5;h^2sA6p(MIg%aOi;q(W^jfBLDXH$t(EE8-gBw` zeyMrJG&2DU%vE8Oz^7n zXDQb;K!Pa|87|{31WCNkyO<|%c3+by{?7D4d?$q!QDka79PZw&@ljEX$-=|XotdZK zxVM%#L#6L7t$|PY66m0BsO^%mrj5$mt>M8Df3_1*oTDQr*xI7!_-}5TY7IHXAez^w>hJu7)LNBoQ5%BW`F^%+VcRx~JoI zBP>!atiY!t#Q?bh5t}63s4X2|8PQzgSg{U%^kkI!78w^XA7*Uy%u*C$p~6>9^dKT> zrw?#TOScNY9i>rB8{(kV?rDZFvMl9{gpG*XTQMuYKQ|Zk_$lyctZE<#R7Yd`20F7N zgJb!`1T-Y&#W70K=3ZP|hh*qw%P1Pz@uC zw)22ZSB?cksU3*98&#w_hzcjpY-|5lshJqp?PI~}dnF=nKN5_-%zSzL$%SIz!S7~F zvaApGI}*>mo5Ssx$rhe*gP9s6KYO-CZ>|$Dd zmW#*;4~jgukHx(Y+%4oltU6#VO>J;RHd%L+#Q^b@I>0%b!m6JZwOru6 zs#Kb)1{SA^g0d5SM+d$XM@os+!o!0GuEJSPTExf0qRPcawP^#h6c%6-(}yABWV5x} z{;}eOk>P8rSU6@04I5SWfZmye4S-NKwcwKB=GoZ97=MSVNoJ`-TUa{0r9n!P`nDad zUK(QvSu4`NgO&l_0{I51CQhv@lhr8V~eSUrK z&!gFjfs4ho8Z>Fev1KCyLN5dHY{6$)hp#Y>o~}`Z4!Lfka!>+k^1KE)&}R-?@0FAG z1<)IQ0#eeq&*@v7S{lycBrd&j#O*AOIlDPs=j@J@zKiMSrL%E(%`VhE%vl@@TT5_$ zlnG=*c}T4jE#2nqu1ri?D^v8t_?@5o4$cVL-Tk=6p0;$#mc&QhtFlej?%x*1oIR5B zP^9!j=d-1ArQDDyR3EH3C3nmmtJP1;oH~mqCM}bB`(fvq(svQn8Die~%zIDwpg(UT zK$*CYI?v#w-f&*@+`afLU^tiEM9Lw0?tZk-uQ?nm#X_vTX!=lEOJxFB5kX5(5PWx;MN!5!uorhoaf>o&ck(55;?45dDarghy+3FT! z)V(^}{kuX)LByd{6UBE?YesbNf}jx{U;<4j2&}g8#Z)v}pJnzn&s*((!y@ zj|T&?^uT!+DWoOR{glRG43=tR>(cwm0xq+TRCzzxNfU!V!cbbX5SP)>mC)N<%74aKK_t zzhNT-PwP;BM-vewB6?J7NFB;Ljmijy?bHyBM%o1k=_%QQMOQLcTo4@~i%_0~%01Ed z!R?{9k2b6FPar_y#SAG?B+&$w{7eg4@gh8|7Y9lmF;_NgOX#WLP1(K2hXe@Kw2)JA zmY_+-0cd_?SM?|4a7*b9*Kqc-5$zSUnwKMIA&Vk3WYG?^2-R3me>f!B!!;l8e?6@i zCL~!x2C#Q|2><9#y8P`sCu${r`LHgLzM1|nJL1pLM5sZbbaK_g3sVz%|n%v2_PmOUMVT{ zhf7eSkd;E#EMWIA*~R$DAqm&Y2o|k{rHI25ELb!Rs*U3;HlEoo%3vqpWcC!x#o!@l z(TmOjJV}*f~8H^|}M9;O5(nq=ihA;?Lv@H4-(xXpNu$R;LAxW&d_{eVlYGHtP zy~4Y!8YC&PVAV7H*~Y<(5)uoOD3ox$%*-PVT9{;?cRDr5%VQ33x@`B2EU&;fUq}{u z3`ZeqcBl^{WQUpteEhdnR0o=D4as}78zerysLl`t;}F8> z)TLBNh_tgCAn=W1#-wwT;8W&I1uEl(_XV8dV<}}mbPTlgR%R|G3cy2r2_n}qGLWg> zyJ?+m)UC7raF4u?hr6iy98w5Jl}(BeW=V)+N(GKDI?6K|1`=*mUw=X4YY|<@EPaga z9DX1jXF1ZN*t!^^$3|BLFC!--)}&8Hq@M#TN67**+#raJ!qH-uj5n*#=b*GYxnWAI z9^kZlVOl$}RR(3ZaH`5qY?g7`cmgIygo%D>3h{fOd!g%!#A(4t`ei5>B?~?fad5L< znHfmO@rLNZhyew2a1tSjaByli8}<|hZqVJsjauxg+vPGkAXLGxNa)5IIqr8JX3eAS zeKb)X?Y?|7X2WO)R8iEui0%Bo>44D&*ioZSMU4A{iq#uvKh; zXU7xoA_`O6FHNF~&j_U=K`}f?l_RfJ6ddeIim_+lNY!Vnr9O+IJg|>U=XqG$=NiGU z7WCLiHz4@biel1E_silw1UO5!lna=m*fUIV1LGrHCn2m!LUp=4%iVH$R;EcmjX@%I z5ZK74i7g^esyE*+Z6THoRsp6CA+2<~tZJ3>Lm354l#Mt+SYkN^qDIDa!$Z}>8&#=Q zy^1-sT3D#*n=qqwB1}DulrEIAQfOqzu8@*t?TkJQP2LvcXKw^mY$J_7UAy%P6R|EG zcC6c7+9ZXLiH;i|lj#)GS-{i~=&DQ<|Ah5y;9X6Lvzrxe;5{!ze}Nua5T)tPL!V)H z=DghKx!e2p!L->y2(GvdJI;ijRy?}0z8wNR?|10(M}iA+mMZ%Enm>#PfadO#mmLfQ z`+`VEBNi!|0*J!RC{!FqAaF<13sJ)LTH&;D8p04Ij_4e?S0hA@nyXOts_EORmLj|J zhd~}Fy5UVWcz_irgkHa1uX+?~#9R$@uwE@xqXQIMaye-eTrmg3FLW0a1lG!iN7_Ff z;V>IO!Ifb|fgog)#1Lr(bKtB|+n+ExM7dSoFryD|$whj&R}M23 z{1(OFAX18el{I6!0=^2^uTJE;_;gHWWis~+Qac`}re^bQ;&_}R{jRoNXDGO0BF0hP)Sw+20ttT4%WdsxXxWBT4UxZVD*E0dokt@SNsbm&{>Qq7?wI!jhQL~ zr%6lkLR&+Wh*qpTqp2XzoAuz=0uE7Oh?ey;>JJPzaY?Ca=ja(mv~D=fsA?BIe*teV zbB)Y+lOR<6Mp{iPGvd&bu2oQ5gjh#l=oS-_Oi{%Y$Rn#Vng0WJ&RE6K9k3=+)jSrC zD#z~~pF%7OilvvQ(m^cr6%flspG!|iX&a)!TbQ)FSdOV<7lx#XtkyXAg3`fE8D(MH zzgSkW7mss>x?j;2PTDCqLzl3lswAN*iWU3Z>jc>;CYivN1Xyq^jEoHXQ4!2s6rpNh zwi}T`Re7jmbXEflDHjS!i;?YvknIFaJ*X$v(1xPzNSg|}BmTV_3$ZCR0*lIATLBta zLl77nuT`YDUQ%pTJ$wq6DUoQuV9=&v*b!o32`~{%N;Bg{Ob+eBKsu;ma7EO|jO^z~ z9BD@wNv$@lk;`()3N!1pFBlo@iX7*qIaU+qMt)QX2_cnaEp1KUh601nQDRYq5mTDTU z5e~sIHSN4Z38QnbmMRp=JJrfnB?V5BS-U|_%}_98lZAW0U_@U`KqI!fPOzx9wG>ZM zQYDf6s~}dZdY$b&BHzGBYF1_e_0ArTY5q|vMWcGCz8eSPmSx4Gc zP`fO+#BT$?@TD3c=y0s~BZ@Q|8oh7e#;>1+9C zKA?O%=+IJ4#puv-hET|1fJvhh@K9;=x)a zJsMY(8hSJPMS9wGo9YF{YOla`*+o9}Opo|S0q2OtUg(XDvV#)Bhft3g!C;e;t-28D zMwln`pna{=ts@*YVGh%wSC%EA8oiL^Qj7=Lv9}dRsCrv!0Dye4m&L)GRr22Svet%s z1ipkFtR?Jf%|R~;r<0 zD{pAd@_^2l3^kxmO}j|6NFFA;s_~-)57H=DAm_{rV!#P$O;7{ml!DfV!ooyDBh?!a zqFr{w(MZP0Bx*UIIRZz-Ky=@7N)`n5NCGpm3(W)0{;#qiv?=7h>JHFSR61x5aE``2 zA(~z<7Kv^Y+?(lpZ?@KkSlFu5hX5ahj2_|N~v(GZj{uZ@`J=$HDk0PYO_Sd25wj>Dht4% zGqqdb87G1x&XR=d#gtgj!%Fm6u~EKiHc3xN>!11-`=kDKC5go#G_sHymv+LcZ2;XQ z^C(l0DAR3F>QTwjFssZPuPORS#@|qjv^nX~(JWHqj8RL|mLOogJ0w_5$4;3D84r9I04E`nkbX5rB`fg>qs%p5t`q=1x}t`n{95G8@OtgsCYNKuD72+>S1pf4>#cvBJJ zh*5}eL9nu_GHED_P}6kETuC{pglEA(4DB3^DYHCsr1sqVW-2AM=OAv-z^EJSD+lEv zRm(!n$W`fEIcb@Xr(>vxtgs{p8VGVACIm4v9?lP|z;byc6 za=bc0Qql(;Xn+2DlCn6H8A-|7|1V|A&h;tB5)ILy`Hyszh9JV=SaA5fm+{FKyt}Ul zQU`94BRjzd(4e^Gz)ZHx>};xSYe-aAJlGhjX{f7fNDTJP0$QU zbGMBEO5+quZTrqXS>A7?MkjL!^bd~ZAog&P-Y>gOEL}}2q*LIk zokMrSCN8ZV-H2UIEOvql7TCl^2>p9W z5DKa!%A%Z@$_1m60&`HTE0$?qAR&)5=A;+!4PSt-L!}EFmW@4wA~^q9_Ab;|Q}8=O z_!JV$r7WUE)X#%kixFPFB@310-9vaxskQ$Y|LBLEoz(F;J*;*9@b^^eYhSO}GPAb2 zE!5J{TJdm419tO-ZtR3}nEyn@Lmi>^#+F+{3H!m8W^C12T-yxCO+>tqMJ05s3-*01 z*m36`mU!E_y9&WzA01z!b}|6U_rG^JasxD7yLm97(%cYE}@j+WL|?EbeO2~BTn=zx<#4ANPB8TR>; zZrgd7?rCS=Z2U?oBPFci4+EfV@~8L+C#&b9P$dZ8DBUZe5l`!-M!}7$dt?$0Pbig0 zj~g{>R*}f%6q=ejGyEzdP38FeH&Usd&Ecg;)`Rf742QP?=;fdCY(!RZ=RWE*?j)&HD7s#P`&-o$YmLD^ayzZMtrha^$8B}n&b}20wE`cw zKwl~%A9TbEF2K19D%+&WQ=h_+WS)4(JP|NY1k4iw^F+{tK<8uZY&d!V5_`1e#&1ui zKx~Mmq5~3uSY}ObP@;Mmk5N8A1ai%tT=9TSCS}2XAQbIHnP|DOVvE8CEY8D< zgnJat`jCkGW`UBpbN}&j(!`>$kXXh3^p++gr^d;C`e(jMV^iZYSpCB@maO4lqaJ2| zUD~I3Nx#W{GDkM)oo8uhw1#l>f2E&{_!|V?uST2Du;c)-@np1U@uKKaNq-v7iB6F< zW0&C3{cGgcBXI47QR5tyvZ7`^N$h^sT!0@2ob>FKnoKq1*khxyvLAj>Li7?P#AqZ! z_Unfv#Qc?n>O%~YCw8_(^OZ^S&u&$cMksOj7O?Cvzke@FGy#P z$wLsGdy79n}LS}j?U3I>E76f|H2A?t^7 z)RFtxbPpRJ&i>&ecmR7G0K$AhZ~ap^{=mI2 zr0wbud4ZiNyHR#W``IM$wQAf5glrL>(KkRX%nr)+t+Z*J2sDNQ9Kd}= z#;111XB8PA&rAlsW@kdR08;c_Y_+JS0viU4q^TgS1J=(r^;FJky@Ynr!$iQooYNbo0_eU&jf9s=8Z+puM^{0U4C<`nOWKQP>Kubo zfV!7KX??v=8oBEd^dqfe(ncEV1#a4Krse7xCT`UU6%$KHm#Vd+?#~LLIv&Wk{3b?9 zN}W%Yv6@7Uc7G1uW1^$)eKZQtK*tlH_hm^q@jkxASK;v+b(@O00%XMnNJwF*Rxf;p z7_};cENM!jiW_pZSGYhmD@3X(D%!|$+6e4Gwe`^w#4}Z<%PwS&)_sr5%+*U2Zi4RkB!P*jauaxpVGNdNGt>;5@HCS&0;uM3Wo$j zIMN(O*GO!fR*2@X_WWRK7I}nl*c{%R9taiYF&ZaA0UC6l)+;A~2iPcT9LW$%{TMD4#j>0ZV8+GResXJ!}b%K#%4P38ga0iLPVX zW{fSV>XB%2Xb3bUvb;`G(ffmB@-O{In9;8c_tJ4fCh6WI9SiG$LtxYzegT}5QdJvl z*4;(picFVitF{5{hfv^XBcdv?iDNAQM3$3R)I79-v_a4oKnbRCxyMbZnT=x_0%_PJ z^v7A1b--&wC3ER+lm3Wa(g~o%=PyfJ9Y~yijIJXQd3buEDiRQtaV{ZPK$hptP zGG3jEGt-jSXT*Xt-{N|on=JzQG~STS<_V{ zo~TGX&wB4Lo>b?Blu9hq*f&@dQ2W<(f5kZ5;h*FC?x~@U91r`$J9fgRlw2ss9tX^W4gr&=qR2A8e~Z z_o#U>iXq0ukQLiiM-dJ$*`dy1;{14IuAPho9?e2dkvVhY&XYKW_Sv{Ag9NDLR;it# z_OgLM>gn5#L>?0LjTOC>4la#G9)`2Wtk$3?XIWWBDk!7Tkp~6ivX3!M|0!ZrB=*_K z!1vvp)NTh`#kWVCvL1JAYOr*$d%GZ;I+e&cR+J2sU=D6s*&RT|1^IG8{gJ}Xcod`o7_@g{QaTHUweIBbF+6@3WpFVue-vRd6&8;(8Ke*rF| z4?#LZ{V`mcc>OuJOvMhv9;)StLq_($h#t@+fqDt`|5R>k!|~_(H{(+3=TaIYs7t<& z+POHM!cZ+`P zC;Q*f`a)ejN2AWan?v&vsY_urViy*`J77ma1U+?r78=gw^@&$tZgcp|-u%MUGMGjj zlRR?Z$r(M>#|~Uq0sh63Pj{D+Wb9QrCzb#8I^ow11J@l*6{wuO=nk>+oXqbLCie|& zseHHVSsF5Ai~2ULHrB7oO4VjTxK2*ZF*#TMy!*IKXsXh-u0Nt~TkI#SamM+1*BiDO z%z&B2W%FAXcYYy~Xv5K!GT{)J)ppH-Wpb4Eq}G;D9p+u;cP_qq0glmLxOnM;B{a>b zVImz15}|YIF1mR3Inf!jFSsZ&yRM-Yrv>8}?lK&axo})Z=$yq%@fCh?=Ysau1xv5& zT-2dYCodRRHg&?}1tEUK%cr`0&Es!cC_4S(3+m_0u7#P$g$w3)a5BJt*4xC(5RsWw zOsF&oV0wh)gG@iE>sTyhY*{@2$`0DVyaE<{J|8KY78zHL$dSnT3$8S!^+HTSX&J=Q zBZ0?NPM8FC4e~TglXGNT#e_0s9XUM`Td=fa0i>k0V**l|YBIa5xPCnDD&+2**0%Nz z+@a!27n*$R|4QQ@IQwh2(LI>9ji;duzQf|78sIgresPt(FRHO7t}vV!wjwuYWcDWZ zr*JT@A?LKo4gU6@Wv*~OyV%f6pU}SMaC0zwY;?uBqwg5mBl`}``7*RCrjDjK=@5VB z=z{3z$eOJ6BYKC&Mi=+6*O&7{*Ym=y^d?8CYGH_ju-ptSo82if@B zbdd?rXKfrEg@d9o9)p+^Z;32;BUB`N6nj9v%?$3=MWaCfcC#<|_+Ip*g z4Ic1%k_NE;T`_aLzN2{gn%!~K89 zMxb(Ka6yK$`DaX_9V#)jTSiIpQ!>X|jSS@uPTKY#Bh&qeK9&V{q!UrV`kxVhWT@D~ zPqBEZhqGTHJry2q+WAO%VS0}s2cvvQ;=48u)9*`!B!0lYKlMFWZ}CkvCMkX2U1IT? z=?3pJUT7YCpErE;xd6N)0KYx}Ul)LXB>=xA0RLtHepdkg!vOq|0Q~U)e0Ko;LID1& z0Q|QB_`v{tI2!O!^)e~||5N~eVgOzefKLs;YXb1|0`S=Z_}l=TlRQI}w<7@W4#2+@ zfYUs}Q0ady0H@9Tp~C-b01j)LFwi+cOm>#iZs(-5@y>EyP2t~E{++?UvNHLvq>;|{ zMe{q#XyKEFJeRb!2(}y=0Q1vVSIN`P*7-}W;1yH{V9XO23p*0?Tg3+G!iDn_ooxbk zEN@@h*0Lbf+ID5Aqa9OBoeLQ@Z3}eq(#4$}ZLP$vTC%)#1uqf|C&3ae%k^qGR`Lzj z-h%aAhU@5TPb}-?)z$NtEnF;dk>7IICS@x8gyF?fLygI|t)+dLSR=iveZjIN%O!Tl z7nUqc$W@}fr9EL{x3?tB6>^!sR0v$QpanOQ_R80ZlLTZPX_n$3I8or#3Mo1mNgl16>*ytp$tI46=ql#=gWE zo?{!ur#kqt4t|bAvW`JEkL6X%#&@yG0|khq)9M*AWpUg(6+GcV9X z%6q$myXD>Q;BI-{bE5gyQhLrM5pc|xLwCj9^PoAFSNuCpc-QvmuN>U9E&G9k^Ifg! zjI^&#=F4`gc#ea+;jgkdle0rKTwz<8hnw+0D0ID(&It&o>92P1PdoT+UOGig9PS@I zT+{!Kla8DI_Z{3#|1~lfj^&Nur}X@%gS+yUMW(}Pd86hwl;`0(>^{cBn{4=Ei!)!> zmhMyscj-UN!Cm^tFfts|IT1gl|3(M@jDsI*KX4fSBnQ9D!O3GyXQzXYbMU!lKq&c= zG!4IkLJ!CEPsUI2-&>q~vrQ0J7|*jYz%jgA-eVlxEpG)Y1CHT2X4LeP4$iTq;$L%c zih$zJvGKz(o$>f7K8lSTjyP#m{9gm`tyb_D-c5fk3j;@-BxpJ}vM_MOPsLC1jSlXH zFNh!z9K)Z6pN9W(1UGQROYl>?he7~H{B-;jKj7eQKX&(L7}|t)`|UF*92R%`u~iQ4 z_G61|!_@kXo9LlyEKYjd`t5dbw|>7%p@C!g3HT}f-)E)55ub>k;%ixXaKuaTQ#_B% zg(F^upW<5_e3FB|>EJFs!^m_vmW%yc3vb{?d-yd54i#9O`MTvD@8E9w>jLnvIXLrH z`oC#$rDu)J_bw;=C@1{=UijN=_@8?Ck1T#Tg$d55f3$ZI@TtnYr=6k@wU3rMHk;AdR-0-Kf@xT$EjGxL+JsS@kan9)|K8uY9j`$S( z6u;cT`6^I+orC8%_&7E;IG>)04(`%l9e`hCah0o><>#dyUT^XFPC70>7dyDipW6cP zeGWbq<|>Y`JUt8XW*HpbFG8B^n8Yu1n2XuHvn(26LJji zrvGIU4M&`FCYo=Doj@U8>EJIqxcgo>ZyEx@F}y1$S5^ruxGN`Z)fxO$Txz}-o|(a2 zIXvj#%u2)OpJl=;pZm;h=md+C9+%IP9o((2DhGG@Qy+jwY(5kx=Amw$y--}|iM0L| z-&A7)Zui3LJdu3TWsc#^*!n+XXj}hhR8A_zq88krZ|naaGR?Xg@zvIUUGhU?>mP^k z&bhcgxi#MNb}^Q+*RBNp@GpRRl~wCB>R?=fpI48+MO=k$$EKOh;Wc&H)O<19HW2AJ zpwEZb90wG496Z3E3#*wI!fwpvFJN(7#zu^>Ql8}w#}1r{v7!~>p56G|;!5+KMV0~? z-eRBRtBA&NChYNNL#_T@>}LKA0?zG(2G};ffWSem zpP&F67K&j;r@1JXS3BzxJ8;lqLt=I>bVeoJIv66s!HXsDo*SN<^{IEy3D3!sk$U*#pElJ6^Y7R6LWwCvM-E5 z+1`R7|2N@N0u2UXZKbeYjHs@}AvX5&AU0y`rARk3)JK?ca6%z#V>jq#;K=w)mOc6b zwA~PK81fKc`6wp}PYs9}_Qk5^;H-e#5hYEt-5LmqP?j=lsEvl#9FHutI;c1a%4oDX zo`9{_=H3tlEjQ;CZUo!`7B=p})!favYTCjYmWq%_A_8(1Pkbz@F2Y^YUgQ!~v4P~n z*5WnP@~O>A%WHR=Pw8NaVCd$8L=41GuY+Q{UHKgDi8xgTMGy+XIF@2y%LR$q-w358 z3glk;OE#OLpLKnUI(Y1bQ+d!WbJ>_$UXT6klcJ1SC{`5S5`<>zH#t@ zFjIv6f<*nz=>k=N8F?j7hBtg3%)~+Bux%@J#;O*ScSp6FH^ovSwUEsh>Jq!4J5*=7 z8bN9EC{}fOIod?qbPct7O2uNlDfFh4aHk#`QUyr!U3Ln&xPXc7 z;@4nkb>Q)tiR(b5gyNC3RcN8;A$uA+I;-ousmlwK zLRA%v;hz#qiHtWSUWpEDyCBi>4Rnu!#Hy~ZnYk_Nb=Gm$^YNa?iX83fiW5ApBE{NoN^qaRtHd_(%2{!e4<0;&$<Gq3nimPcz^gmg4?Qi@43EH11pk$vebtTS;)$|mEjP364#{rZI zC>i~u5SlDRu_E#L%UY(vkDP-Ox}b)1QSG^N>Ze66Zd=%CwV+C^7L@BVLc{zO&{~Ss zMbDiUX`q&nBooC>Kqx9rk6v6iBaMry7ULtD@2FtKM}{EzKXCT6^`)081FCEg*j;Rx6#v{mRo^qap7Zprt$5kw+25XY#yHYbFUIB)sQuKZKJu$V*Tzm9~#>9?!78@jCe%=JmvUsD%`bPscCUdxqhalKdn zQx@lQI>kA!!gQ1$il1Wf2z)xl*%YBXD#lh)M9b1FO9a? z;#yyd(;rHgXDM)3mq*_R*8um^W*ThIL*d;4_}TzGWF~PEp(Tr#c1$S6DWWSnCY15+ zijE1Bcng1WsAI{5@=(Xp2~$EHZ4;*QkkQt~OD3EVTDIWI2^GLvCsc+IuQarH0@N2i zHvu{e&}0y02hnDbThVARdH~cvSg@k~P#uJYpp%aMEZc(4p?#4c9C6A>j5l!KF`nS$ zwTAzJgEM@!z2^i3ou(6!yHMyk>nm<=L$7+crr+=3oG+$($9U=VbsY7fhif~aE)SjN ztN1YicvS!%55SiO;NS9awof|hu+aJRYze@3dbslES01kQ^;-|u@E>|O>x}Mj5(206 zC|>B{%Ky^>@Ny5=dYKV`H+cALLq^Ee@!(gX(obUpbo5B0*5f=6FJd4#noywA@QSy4 zxTbT1gS+kGOAgL_m7Yx=uJnK1!^?$=Q0N{9Kh{a-VFzdWDko2PxTf>6hikcBcW@4Q zl+F(voax8#qdUsJ?D^QG=hF`EhM(-=`H~7gH$1$+;!8YS-;--RT>Fh%JiOkfbBBlP z`|#g9TEX(UB1VQIKV7~}2*9fX@N+y|^PL-j zbE7()UymIQ?(*Ra4$ezW{{{~)kyJyWuX}jR;xa>O^JNgl?+XaO#ltn_0z#n#Sm;S8| zPWm;S=L7IR1mH(<0TPaMy6KN`a5w#m0K6>#zcv8x4Z!aWz#s8&<^K*3FO<@RLauHe z>DTaw+aadbi}G!>himxbJzVAgBo9~lne5?8Po>5A*=DB>gwnG-T*tXHJY3_%Jv<_* zghCftoaN#-i}v%EIXJ^Aex-vS=ipa4xZ59I@8IO8@?k>&{&fd;)BlcxyXpVH!I{4D z?Ij0~ICTEX!5N-Ey0<-C%lnSSmH(9{c<2)kk6K(m=oo&iV(^c&73A}IjDtrIPV?o8 zCY{Qgz85~@;V}~|RBUmd&QdRYjtxJ}3(xr$y0bmJ$-tqQhes@azLUNyKbJT-!)m!c z@8IOU(%<3X+AdZ)xZBUHb8t7E+dN#;`Idvb>5w)$4rj@0b@=RAmPlk4%gf42E6b;p zS5B%Jdab;2YQVE3_ zl)rb}=p=F9Y;M2sy1Hfti=N@?uL$$zH!veQ8&O`%hzz{W+}6aCsD~@QpYN z)vUzhehluP!@|Q$n%^?!CMf`YHgl(zRqZ0?Fw8a8Vx0)S#P2N*8#B#sp{t7*c1F5L6%8Jrw~b;|0oV3}Q7X%a$#Hp;WcYS>fd`LAZJ80${C zv;Yi!g^5pw*RDss;rx;GVorU+q=KbG$tKo2eVFAWD(f+x`$3MDbAj_26_f9~ItkGS&ylgq#c$O2ZpJK?!^&=qjcjLLRx!)i! zl6x-TVsXqY;9=9gVZ;fb0cWg(sOFpk5+%Zt=F7R^#-ihdD<^R64aetLhL9f!{Q?)M zcZJuMLn+baNIW6@jPV3MtmFOOu6uL2u@+TKm4La`XVv!>RxuUBdnip*0ne)nZ#Y^Q z0wZJ@kt*-5S4l7noMO~P3i*pz?>WP)aEkb{nuX!8eaS`B+!L`}_e&msR5S*+c_(%~ z!sfjnu`|stRk?a#`H`6>D>l0jg3J_yW>}TG6^x{#A)34784iJmc)2a=0bHwyfy)k`MLz@^CY13TW849sxF&09@KE$e`l>)*PmH4n( zBM+^q^?AHO-i-pnbP=ri@De1hv}Dkv%{;L`Wvs5k<_65^G}Dk5toe{ASlY@mhGA(l zXFuZGo**`CG&_o1r=zIoo}F4Q&6E9wv{mL`EOa~~HvAf}uHAbv>IfPFRs$m0Ck=7y z_yahT5ht3k<#WL~hsqNv<{`8o{%=<$;;(5l{_-Uxu0xs))MiNQJiR&s z9YJo+knjwpu?lL;@nJ}1XQz)J&$LaKOIFs@Z zedtfki-QVXe0h}C_vTDSeUymiLe5mOj*1KY$e{TTxS z<^=|YCWK{sG5MR08*$|YR+qr}!_e}X{V1wID3v!Dt6?lFBcCrNeS^t!P}i8BC;T5u zVC@TxpZ+fI>k}q2#6VK(U51mf%prn&fw3Y;rWIfiOD2&N5o8JkM49ikPf-C5REgZ8 zvbW<|V@@vq^K$aS8~y-Ik3(N{yyj2{Bzm;56C4w2u6#MW36hhrQ0g*MLWK>3}H zH{KEd8}fTHrk7kE#uHN@K~j}$QoUas2G+ez)>V*o@{Z$sgR+s9W8|MJT8VQ@c+P!O zbs7E}&HE6yf8bp;-{%?PpTQA$+ukKqm1GAZOi ziYS9j8qL89xoNv>9@z#z5ryE;?DAb`2~bwL{55pRuXjM8QQ}X|D{Cmwz0N8@E(sXQ)`cqpMtUX6h^+G|MQ-#kgc!gLR~x%nr>7 zI&wK%nesu38!wk^>qxXzi-c5*^tcu_87(Z|v9V}LSfiiXADZ?QY|A@DC zkal4?*qYb>I0TNBU@8OlPmy4}S1eOM#QHQkQ^Rq^YRKl_2&w3_-MGJ_ZWo07`v(~a zBK;;uj87~(k>GtHq{kQCdwlXP{)}#2|2_Dobu=gY@4&s(2Z(-DQVKVHj4FQ0q_bMn z*~)abFdgZ$`-`At<))KQrK=&RDJB?5%@we-t#-FEO%U{^@svi}Vk;XDX|e!CT1S?S?VE%9vdUg3${Y zBMqtlJW7aDmTA22y{`sUiK?Rs+`CX;X)}PPPR&;i`&>TE2!MC+Q~$r?C;tVu8@6&lh;mzbM^xhNCe{vSBoUaZu)nqhy!Uee;QiJ`h!8%ETQj!YNp zMaE*1*h^YDTDp`QNySATHY8puL(*u7-bb%0F|L!Sy~!2kQVQfZ(=^n3=uJ%qLsWKW&fb zZd-3Qc-rGUlIbc4c zJ7DwAHMi-1&0qbC@Puix>m1~4#Ku8jhJOk-;KJt#pFJRNuneC$C&}ql&9{Jo;F|HX z)2wo@e~muj_JNjL!?ohV$M{U+muo=l`(Zgf=hq#BuDAIwxAD~TON^d%=V^r=*GA@0ACq^ zuM5Db`@~84M5wK!vjxgeD;6wU2IZ$O(3?cCovKgJd}?Wj=2Oe^4yZo0FYAEb(_&~n zeGWQLpQp}K0+FCHHGf4SvBK4Snk=?S(w0jSi#uU+Q!u35y7W-xC(udvIpaxNr>tmw zw|ltC&2=7byMqwRO~-D5>FAotO$6Z-*L3bRu;9nwTEp*kaHdmk@89!+lTM}QC{L%O z#?o2j;VN$>9 zm&=E-4nBwv0eEu&-sa#g|F3ayH{Tuyck_MJ!>cXdp7U_!^UEHt_42-lEC0vZA)nT- z^7A+k*L+X%a1CGM;mZFn1>oOsaF;*dad4MEKXh=HKOcCw(s{HUf@-;x4}a(3%D1T= zuKc;w!?nC~JzVp>*29$#-}7)yXNQMt_@4*h?*!o2vg3zic{zunfnFp;Q!>{tS^<5fA#PJJfnNl z!!`T|9kZ;=TQIJ2cPO- z`{83D;7G>3L$1?mGsCfLaW3phtQf~ANYVs7oiwIK!t}*%?6~J%{mocCxPcjB1z|Yw zz3w8hNp$X8cqSt@gnz1KIs6PJp(yhUv%ELBA-x5tuOu(WK6l zQt=!tbzLB-aB-;yAgL*(pZJ(MLR|KW^LAU7Y{$k~+Z9ZeSs@?Xav}-qn_IdbP3NPm z&ixG8-iPO@8%rTt2 z>UvrHFB#57yw<})T~EYP4Fy&|0`_2Px=*%iu~D86p4ifmC|qTf5Abm2w%E%gBh>Xm z>iTSxYSs1S-SbgLruJnWj~rqCsL`K5e{JA#oaVCLEWFnx_Snp*wNkaboa=4nx%lV8 zdwP3*^8p}@<;UPZuk+|o*Y{$nIWTr}H1mex5$KDo!BslfGl`S`55T9M$`f2plB&)u zL~e`qesyH*SlKf*3eXdcv!T~zr=8w7bWK;sSU?Cf- zETHZniL30o5u`ztNJ*>cZe`8)fskmbD!s0KQ_)H=q=9BSax6dV6FbI#RG)aBZQ>pT zr-~idMvIP|+1q&(PS83J3V71Dqh2cbr0{vtaWXX%XjLNbgsyL@XvR{TZbBNV2lg@J zp4W!09ojXqmK8htX=WneDF64;nvqN3?a5X4*$NMbmvO`gM85*4-DnP;OsF)OoF+`{p2 zvGkE?JRl9L^2J=0w}zj1up?EyaGoBNzPN}FrLRJq2xv71!B4Illc+`F8|E{s6>@Jn z!Tlf%Dq$ie(Vm0D!W;fkm|&i!Htxe?93{SP9ncTflDjzdYoa7@uketahwu$*X?Nf; z9oa@6l2gJy@UopeoMm%XUM?be2l1L3}%jGE03ara-JN|j$`PoUY;C)3Z9Cp9GMMOxn^q9;Xppi&e_z`GkUy# zHeZv~P^1`ww*%W@XGw@Mni$a!R+5`IVC{54Z#NPN(tU0+IdYT5Npq8o6D}%OApo!e zX7mkM$>e7Gq*DYg#yyYKBp3MxTqoRuQ1Bi(7%t)fM^+wLPA*0yk{sBFDp}U^t?n61EDLJFc(OoP+CQyZ|h0bC_BIhRC8`LJZL-QiH^@&h)`~mc& z6?KV&>`F>L>e`xGZgmyWEq7n1ovZD_MA(red)N0#^h(^IKa1jZHY1h4%Dhs;?^-!gjyxG%M)s~7X(UJI{n=|Ty-sYqDzIqS0P32)IMZJ!< z(W~%ZB2st+dwip9bckW8qVDM+PsS!`W*OZ$qsKHek+a^iKaQMfI18!e=8uK~9?bzk zf4b}OLGRP6w75o17mNGR^>rrT+o(*5D(BsKE29=E$ zD#9ua*6%O`|PNp0k-5 zCHr$z`L|mstxJ5IF8UiZI9JLzC1>%!a2o#S7Exikxm5(D=`CJAhrgz0dG!neUfvj`Q3;J1<{s)k)Hdwy7N=fNG0-C;vZ!UCo(ClIgECV}4G))}*ob0bl?v);p zTEyAX&=|iD`J~oxl8n6A1tsz7Ht5DF9-_DOA7A3K+JWR4>= z@5QU%G-8~n_XJX*cw=}0p=ddpCfnXcq&dH5%nzUmQ~aM~NI8vWHI*C#bmIHb@GX7k zCbLpHRvXx2xeVbd4{ulwcBQV*(E%FT(7%{Wj%H>?*G}ny>9BJ#CaI%VvD7rq9qfsv z@@jEZ-j(bg$uV}otY2z5lPnJIcal-85D;HDlEh~X$A5G4VHA=%+fj~PFp5{a8DT#{ zMK)zYOS!4~a28-&N6t1Rr?g~D@|A`+Tu$QMq3r~_H$f?$naE#{yF3~1%G4!ejI-4o&&r5{#61ENpgmUW9{FKuVwanpuijjiixSU7aFikwSr zAJF1{197#9rnR??iaiQzng?Sz3W1rHiUCV2NU{u-sp2^8LNB5DF}91OfgFxYy-8;2S4 zPAB`I3oJpkVX?r!mPUz(ym^jwW*Q?JFJjgu-i2|;dvR{35zZ)dOga8GcNC)PJRDCt zwysw`3YxDwIusvaWUuOau)!#zfj!wQ=Lz8UD_{r9A$sR?Uqu<_ijB3GkeHMgCI!Qd z(n!gM9W3ciJZpR&sVSUG$!)4azzPN$hQG$c389m;s;oBa@P=$~g?*4I7T!ZBR_4o) zEu{~oCq?6W>I0O zaFT=8ZFx4o8IdTy;G}h-KvMgB8*I14O3}l4kWuptAM3MV{_#dk7NV}8NoY3806r5; z3Wa;_!8S;8lU!t}$yG#NCkcUv!zh%Zbt!qW^Ppi!Gi8vm{D~-1mtM=cH{_t zYM2l1zynGpWxUD$*?6pL?1eJ>7VvoP)69fj?p(-;nRtNzkEVQ0|F|8*r{yP{I}!!j zD#j_fwQVao!jM33+zr1Y%%U;-x^1{cw>5?Tt6`+_P0W_D{Cz}kuy33KYY}u3<{i^7 zpe<04G4YIvQ|+||^_6GX4;q#|#QYN5ajI$^x;$HH=3}?E2*t}SIR?zlYsL5Vj#z3A zI!&ogyh3WaCvdo#-aE%G3onSeM74fAqjRdM+ZOu+i-Em!G4z@?1N6D*AvXd>ortKX zI|;MX4Gh8{2KE9$9?2wIbacr9h2Lhkth{6mu9+62M63CXP1jEW@ zEe_AcnY)%yj!*nx{Kad}8 zo{A3VLr5BWcn(4HPBbsXSn6^N{Dn-gzM^}fidbrz!`_vk5G2*&NaO}o{P=^YerZ&w z4VhlR&_93FkkbpGHD^0HN14Ee9Z3J^t~*sS$qqApF!+OvRP2`eGZO=;AMndgUkCi? zx>fFt1~^lTkcskDiX`VTDn9Hn4Pd@CdtS$;4gGY*On`YWlj#Q}DCPy^tBYT*Ixm3F zSEhltc4h+`zt0K?@LP|L*DHAeC-WO-K4b7KLL6VPWY;<=(SLOop%|0W>_z*KRW#K@ zhREzfF7Tdx={W=ptcDwT^1ph?frDfd{Jm0anL}7*IlbP@C77>Xd^<}>E1medCBs== z#e$nx3g;Hg9D&n#N)T^3T7)6gG3q`2%}|8wKVgqx1RGdvbPU& z2&_g7tT8WRw@hR{f@MmEdnZuiw@OC-*Df+-#vo*c_-3qLpUgoG7|zRJT<2#vhj69@ zG+pHfH`plz7L&y=%*4DWnL21fW|f^nSno3Pb~_CuGQB@X<^mkcAcG9{3nb&u?Y;h;62JLZ+Eh z2xcpZGDt<0&1#YGW@x6S_d%N3;nf4-)%jTCOD)gRVQ*3Q+4=%C18T}|4a>&EtuILE zl%;$)HxyxTky|(#Z_-O&@xy+*>@q+aEIrwI@ zef~#gz$v3-e9*8XE#fr6+>*2TMe#fKQ{`u&<81x~d(W47#r@)i)4F;O8L+Yry;l*t z8@3zt^cbvbPjARV+U9GU?2Qji-=K-(IT)7avMc8`P7Sf5|B=x{b!3)`p7g%Vic#v|3RMI2TQ1QOQED{D0ME!bj~w@Qn!D1T$rLWghVR5MQ%M2$Ij1_Ddw6 zS2PXv+mNjvyQtqg26JeLs>NiT;Rm}7RxJx25xL$9L){!}fhr9C%os?=TbxCbFHp2~ zj1y7KSz}nk5Oh>(Rvz2o9{@Dr3k?Flmuz{3%wUUbewor(c#JU{jxR{17~hZgI~1cn zC0F)fwmtrVWSZ*Jc4@^qtnvS7?N>SxCM>v68`=Dl;>Mt?DxS#6=7{17-UGS^GlXAH6bmongD}LAN)bbo{_o!vgjmh_eWRots!$6lV_Q!0C zb&2;tUo@3}Z@OBaHtjZDogBeRb%Hw7AzLlz$!6nY{c?{xw(H8SbbQdIr#H}eYMwRIwqDQ%u{5fb~UGd@}_;^dQ*0N)nq0F zFjX;VNt8#Y-1NaAaycd4COSIJq&-3Q68FOT4u-@VIU5Jp8vVUV7!@ zYg=%I;u^+EBkW&|ZJ){}vkH%T-LMsC&pxbO(c*vI!!8x=^E($s7RDcJYi(`22C3F3 zzh?6OlF9o^%=_^h#kOQT`E!%Y61%w+ZJT_!SJ;!`8aYD=B;zQe)X?ZdNLJ?IU{h{& z5xQ5jLaAklz+{XlGcx;B|i3nPM`OWoVD$RW)csZ~+UDc$2+R1ur-M+H43$t>?3!$;&U+4Z)NI z={i^gA@EIJ5%4Jw$Dy32NnyWl|6MpJL0;xeGWl~x5RTojjog0#KMwy!<2PLP`aT4} zoHH=EzQ2V70@zXY;jvqYIHn#^!VJ0OxF@nNmH$J;5vXkjU=%{G|{Yz{H(p=Nkz{xhNyP_4R5mB$Fo`ium zUKxCC;D`R2lhJcAVxRjaME6vsj!(YC%y@y#@PibNHp}$dkw|H?@{S}Yw==x8Ac6Td z90Jw#HrZ?jJq(V7vilbcXonVN?VKTHeJUn)7j{`J6skr>bt>fiJW&up`ut_fWG}OvGr=UwkYprcd=}NZ%-&{v*1)~Xt#EMr&c?K_ zxM6={uXEnXCR}E(b6od2+cPJ42E}=!SArel{)XpH{7>BLto*aw3CU|&Z{P>*b>2PE z1iIrm<5P7OLY3YNQea>?CzrV0!eY@-Xtu4lG+-}GUzkBlaG8ytAHUJYZ_NaiD~5Gl z8E%;WBp1ZB#&7X(hNV-##$LO6ynNng^N;YxahkvS7vUMxQt{KE`)vG11+1^}6mGzU z&l5gtKhK0vK9m0=@zeN( z{>yDV_5Ap}_j$*#i%v5!Hd(>oTm94c`Q@kTzb`wCd2j;%Lm@HYXD;R6&)<18=#t_7 z|G3wgY#C~=^HhtMS}rJ`si8X5UgvWyUgL$Yw|LycFSPh9i>u6EV)5A?{`VHw_M_qF zS)6iDr}$!Alg@br;S^tnYvL_}X7CjjU*zGu{cG-}?vnftx4CA#9*ZmA^~pMm`~7yW z#kC*M@brh;d;Jf<-M!cPUbr2&pElF@ZUFvo0r&#}_?7^CR{$=1wV^bh2ra#GemnMR zx6vyzJGaw&wcD;}pT7+IwYia-Tei8AyB(Xh=Pz7%1sbQc(Rq(V598^)tgDT3n zhZ_f7tVpzB8#lIl^DA*eS$PPVl}(Yo-BSmjWl_%CDRysnd!j|I(tEqx6IU%+MyNfJ zxN?cJ+Z)i5_GPeU($T(b@zTzPC@38JE;{ygluecEHiB@9tK4odu;6?J>mG#d4$iRL zpFsDl@eWDnfQM`NY}*lPzVmFjF&?h%>|_s*+wlJ0Z7tUoUU>EkbnPCl>G&5)Aki}a5tS^i~H%^X1j9cd!m!h9S-iM^P;Y!apJY3WF_rNMW zv)CXk{U^a!`R{aaH{aC`KB!(C+ztPThikqgawOYB_;yqP?(bdIa+NvZU3$)NaF?E0 z9$s(A2yOLnO@CJa{&E1`AApbGhZLMIC!+%J;{)*G0DO7?eqI1>47_IOsW9XX+A~YO z;eb_fO9RsBc5qj2*EqP#|9d>V-sbxs9xFSitNXW(y8U0flVL*#rwkJzV*JzJt5thuIG9*6-&7@C^a@*B#uIpKm+3oBqFec)6v2 zkB2M$FL=1}|2P_`g!AR6#KB#9COf!GPosw`Jy&_S((^?RS9*Tp;TrzM0Q`3WcmatX zqFm!0+$~p`gS+)x;oxq0J3L(F_Ery9`hOOHzv1A#RQY_*!QFgMCvk9EUz+ddJzVqs zUI2ctgY#1Jeb~X>e4h=#4+h{z@TCh!I^A-Ob#OQRh5-B$4_7()XAjr%KJ4IbzFQsK z&G#_TRmkvD(;4mIn(xyN?tTY-=-^CW$HT);AQuize{29=9e|(X;Y!c-9J*NlYl^#A%xCPtY9-d?It2{j4;&*v?i^YH7;jA0F z+=zJTcBJWy^Ki{~iigiL(L*ylT*tBB@NnhNw>`Ykrt`3eEB%-98= zJ$$CcFLcs(>747}ywrSebnq+(zs14betV~bGdzEE`#d~m;Ls=%0Z0BYtn&GD7T5AJ z3|*UttNe62>F`qNzrn#tg5v8PocXH!eAmI<@b3iRho6kdaHQYmLxdpQF#PyTU1H(l z6<1?Pt!*OpeTttj?HJijxlM{(Ku&o9x^Q^Q@7K)rM8{5}K}RNd9yVtXF*k z>s8baq$1)<@$Z!lS=_@!8oU;dbk4 zd`>Mld2V&2p@GNcGAbHwr;l`V`5gI@U-ikmiw=V9df*4dzg@JoX`f}E+{5AveA2mF z{6~tm(+9m=4$B%AndHdQ!0KEmt~vufPV_s)v=Qg{n*JSltJfJjGn?DVX!H zz+{M0n+VaWhLZ4x-+)@M$?kwPY&sTcfxjj=#fajM9bYg9u|`hqk_Jm9%kodarXLzM z?O`AU-iJ)%gW04THcZ2b4u}mj!#Qih!R%ay^@R=Op5+5 z|7_TycsMo8sLsYx7sBEjw)R>@=jg!BSmo|+9De#C*_wRPZX{tOY^65QjS3#m2iyTvxPD7M}*!TRU4Hpd?{m5EIQ!q0}@!fnugZ5%~~=TTjo zMUfNbMs1RMb{RMs?g_GjY<@s|bQ$m|os?z@ChLluGzwuYteR;xaf3n=Ek86CFlnBI z3F8qwo-^CX4OEn68aTb7B>S+Th2>3XRM*0ONX%|FgXM+r#-D)zDX+FH?8zl zsKXo`v@!ySY-N9{sTJ2c~T4GCezjG@Z%NWFPY~nIMf% zh&t^8#zrPW4ACy~jhLZbU@HwNP7{XBC_lpX2fKR=)bkxc#+N_MLjI6sbf9c88sBX30Tk$zPB%dtw=Of^O*J z-OMbixZj6M%KlyOncN-pll_c;k3e=mEYr;@YDn%gNlkXT?GL&-(BMSQ9|SQF81~;r zgS zD|^Ul*PV8sC=M@dOdSJ*HB)0KOD*0Wr!&`EkzkrX$yc;Yb;z>mjr-TlZTsX`M}bC46w2Z*le++zwsLH(!L-)UYwS zm7BX-c#53e+(l7ojHYr&NUyQ_>F@@&b&|>4lx%QU$wd`NRhpgAZ2zPMA!pNM8>&gN z#e17Rs*TC;)!oNH7__Ve&OwM!oV?SXxN6AdAm=WL7c(26*O|S8*@M>*n_Yg^&WDJh&e11n36rl#N&EL< zcx+Q3>)8V1CMAjVW5#$u5`yexZ8ymeoz$y1J#GBvc;X{g7~9K-g%9>W$RpNV(lW{F zCJQCy32YKE>1V>&s5WT-^_a9_bxKDO&P}n0pimd$FvW?_hkFuu9*4wYk2pF+^np`I zf9fmpYEL|gVT5%44ZT@1j9@aWX~rivf;S*6k~KzflNaeatmn0?<$nj>{R?wod4w^E zpP}R>d-^}%(QM&d!)AwMOReG8Q0zAJy!G7(kMmlwprs@tyQ(4`tvJO?_5+;J+QQuc zr4uGE2wl{64ZZ?GbJ`@}D^x?sBO zGvMxm4I?%VkBu(1`wh+;eE^aw?*HyJ5H{2pZiJM7_BN!r-g7FK->$yfZ`YoRv(1wn zco5t}biMiMN*pipL2j?Dw?QCo{JctlF^i!1`Yo^NSaS3>RE8_&KPiov3(fz#9?r0I z>etw7yY$CL;#RW2aDIMzul_{{!?aZV7>911jo)Z))BhSzfd*XoJmIrkTkrxKkjYUU zP2(%xY;hYb?d`MqrvZDR_&F^A5CP&CGKUJ@W%72lS-!I^~>-_xPXYMt_{WbR*uz64iUY(YUb5X?gUPG`r z#I;%#=MpIKT$_;MY}dqfZddUWEw1vTIHUhHb?i@J28W#fC?N9X4BcRNI&NLvc84u$tg(0894TDam0sL3x}k${ps zwArBr5A}4+=i#->tBwVoOWH)S{i-F)TLo%gzU;~cSBYXfujY$pd&~00OBbdU+S3~D zpoi{&6?8Tq*0IWsucNKJ%(3lE!!r%Kvpihoh4W8z8eY@a_UhwTdEqsk9|Yh(4!~dW zaBU~=2jEB9&Vi4ar-r}S!TMyTI`GbdRIuU!}m#fsn zb*?(@;mU_A1Mq(hz_$kAF9zUmdAQQEQobAxm7jG1_%{M@{~Ri<$DLkyOGfCA0r(uN z1Frnk@Cgss^g9CZwE_4o0r>YkT=R9$i6W-`_!|M?vpG0`Q~H(8u>p9ygLCYo_2Qog zrE=(=qqLKbXB?dGG!1{26+ER=%{BZ-4$kmO z|71r;+ok^v2WNPt=XVz8o5>x@e&pd+1u2wmUtt>lWW=NMb-k5N_Z%dqp}2bv65r8^ zkGJXjd^^+2m+wuw=^n0hUhLuJCV1%Yz4SHTc@FN~Y3cdA zhkw)J-4@sMmH(?8+@)u|gS&kA2M2fM`AI9cOrLA%ntsIQ&*1`}sY@(c(B85)a<=z-^tjC;P1ovEaD-hqTuW&jpoA)S3 zpZLajOdR1ta&OiGIb{|>ew?0`6*|ljnq?&9Cpb(Lif`8dQJz^spPf;{+~Fd54g=lyOOqx0#lN>`AAFXdV>M2o&J!PY92pPGgnQ*V%op+=blf30Y=<&F=Pc!Zm0A5j zdHaZ?e0PbDgnjE@R1MX^Ge{H*zJ>enAa8` z)}P?}>yQWv8f0D+gfSpy(_y=j=1Kw2OI30Hyc}<(`o!f~cGiBzwyfJRWj|1#7<*1C zb~6@%YO@AL>1x79@!tGLnQmqyD2s^H`n++e;>*152N^heGZ9vMq+Gt+sngYs59@i@W!$6=ZMP0U61hu7SM zNOg&U{iC@5$-_vDJ z(@0P?t|xvRzGX{qY|MaJ8Z!xEs(W=IVut@s$j8#oseR@N=k|wjHDyb9?RT+0vT)PD zG5MjuvAWu954eQ6Cf!m0jT zNM$1qfleG!A5Eq@PkwX{XsJvCu;iMSTe`U_I8PptyD)5euT&RHAF5*;r&|e zkWi1qa1uLd&8uiTmrq-a6xdouMy9flAlIG`uT7w1Gp)-e9jmmhGQ9CPkYTzHv~Z?a z@(Zl>e$%W9Q!ZZSipDkL1|E-9+9MRg8-Fckw6LP6}5AYf(B4NFxa}t%Gfh-9V-z9!lVCcL1}Fom}s`4l<1;Qjuq? zFhUt8K}L8ZeyMVaf#hWgRM;mVL7(-cL%Y_VQ4e$TPM|dXzzj`ditAIGej_6;72dqj$ zq+X)nQ4Xxerj3_njpVX@I}!+mZ@dgK`WqygeN`o7#XIXj{~18-yD|K4TS(~Fu!PtD z4wegXP9c=OkjeA=qAzC+f90K*_lA3}!_!P(-hU0kgQe5wY*3Q;(f&s2Q+xYyJ=r?%dI6r^q(V$C) z`)jV9v0LNXmO3pL$tJG%>a)IwS|j5&k)hVeICn=}B`aEo5(G(W! zXqATA%V-O$eG-jfVN5_%SU9GdhOjUIpd~CU6PC)NpJZsRu3WN0Ok5$|s}~Hp4tTQd z9O$u5*$>i{dU(EpLkm1y<>!U~++Vv>`Fz+5ukG-057&0I+rw3!-*Rwn`P4bfKR7tc zrRhwxof#h!ufdP*5)Y3E8VY^U!<9cPJzT?Y^zb5^&euF#)A^ByE1g?BT*Lp;!&wHp zbL<8CJzgq*W_q~R*F1|W9~56=alT%7S%V+l)lPWQsC@pqgY(kfh935CP5s`{;P+V zSp0quFS2-_himv3J-pb4e>VUh>8wqWehq)Dhim#LdbrYaiid0Xi5{-?KGnlj-Wohy z=~)(l-{j%S=dXFV*86=Hr<}-)kmb)q9?s`<|KZ{Ky|u&Qlpm&ZA_K!cXK~8g3HWLI zU2YRpIV7*?{>8(UpD%cLL_%XUEG7O0 zYl#)MO)}3K$+!kOgGYvHn*mz>+z!o|Gxl**bb4Fo~C@0+YhHUTP9Cu?Zby-&GvXZzlPamy( zvAY%fCvdX!d5O-OWzK2#`#3e>_2D>r@C@uUni{R#f<&akyDOuq@}lJCg`3hFk-jXm zRJ+F?h*s{DJx}ryfqpr5l;svcnS3uI7j!+vol=!AcHI&O!SPt??{iZ#%ds!2Ho3RE zl!-PZK0%Q46P=HSxWLo$K3263Fx6f0Td~r#4cRoFh`+oOyPmC0Ze`))(bTMpXyr5A z=Q9ChCp2>V5X49@*^QgQ*n4LD*IQgohTlQSk@)pBH@^!E#6t5DEb$!bY$)0`o?xg z8Y>`;AEl)c33P26B#o49k?&~g3pur^x>8ntE<1tjSmjf7M$)>^)lB1wZOjjguRS{X zTnIij7aq<(OheD%0T_kD$b;jM_46Hvkq1W(Y(r{Yn>RV~e@R{PkK8~#r#|_Ui>39& zx5+q(at$(Cv9@8duTQnGOS-OTR&8olR&A=Wq_*;f?i1@0JasjWbzt0rz#JTz(Uh|l zOv9-uI0p0;gwBbk;-%juRrYE(WU2%uHa0y`SzJ@g3AAD`=UwDaZhIqe}9P4pIh(5 zyV72hjyoNEuf0Zk8-~})j+5x<;x&h@&*~k4{U$wjNc-PASN=CR#3BMduu9V9wS1|u z{v!L0V{Z?cX9=1cUy);CaD9Z1V;5f9p<#N??PQDFPh#8|cbAv1;yRYmdyf5>W>9<` zi)nbxU+;BpT*q39YyLXMQ(WV#KLZa~E_WQnuzi+(l`~>=8qeH8?;00=nfPoaCv)NE z$-m(r>qz4(T@ApTA(0Io3f<~gsC{CqBOUH?T=^IuYJ9dE*ZEriG$#8$oxowt%bFS*NT)ZD@ z&Xr|ncnzn%2|sjYk4T4RDC6?QJG61}dpHz2?2s7zt|0EWBcdZh_5*=8)|~|3ouk$M zo^$HNH`xl%{`?G!M=UF}zm95ni>KR>d}Ls=1*T7R9#&>exx<33( zZ6dL}U9t9-wktk|4B-C3cp`($O22U)4#1~c`YE#%Q4R0g+*kZQE8O%lwT&Aqw2JF9 z-xj>${+y@cPdMqYkJWq+I5@*<`04h7`Lc;<_z14)SUgLz+&9X92G{!AcX{|G zn_pt-f@|7aGGB*(%9hRK(n)`@o~hD65^97F{Wgpz4?4r`=q1jLd@+g+s(h+&Xo#EUS%D zH3Nnb_8_JbVaB?a5Hpg+U^agp%y>M<9IU|v*rurKEy4IZ7bi?N!G1^M^++O*f4OJi z$Hoe3<~%9idTC1PdaJ`6lh2}N)TiRTCE3&w%)?#d9616|{&T~8EIkZ0o$8AYLf`G6 zXgaa@xTliS=8kl-g9-@i3THwyr8bp-p^I^(qqZsmLl?is6=KV$!JzVeT$d<$047|1 z!>~!Ayso$YOqlDau7WL*vq;Q%(Ahi@S4|V}*LW)F;$QBG{1e_Y#3}r`_^GI1WvF9; zVIYHMe0ni`&|A$@lXDA;Sc}Kvukq9P%cF%4ochS*rjQWkaNW=)eu{tD=es(KDIWACdgzs{9EdHu(=okaf z8ePDh*0a)M@mPA#H6s0Y>Wlmj90Mz#l*j(NFlY>{uSe=J)BUAm;3C_Ho7kCu?aLJj z8Uq*G_&Qe9@b1`wSkM?)<}-O6z*ii)hT<;f zLpa5iU-JztxI6Zlp_pjhc>s{+5<2 z!h*tSd9_@X0eG8-Yq@R?z`yC?O3xM#*L-(+xTYTwDi3`(K=q~Niq;{%1uAv7+2B2A z<&!G@V!M{QACM%}2p#U8GrQsJARf>-Qw1u3#+hcp=LV_+g5N0{#;8GDD!8#b$YEJK z?-4I9{wibH67g5!o*T&aB0hDn`LV=}V7D?H(%mQNT8o$nx10CaP_7X)VaO}y!m5sBnRoLUw|h`|e?M+$cyb*R!p^#t@TtSULbttHgOganVVqgr z$1*z!o!C6E8T+wrM2t|?kz8mqw&zS%=jC)CPjznAKs@n0wjFHClC6``s+l=mzm~l5 zD1{@;qsi@ErL-AkcW?DvcXVjrx}&fPmsd*zUaw)OFt7u=xn3j-_9Ip-H7xotC=b+~{vt!4?&`8=m>;sa*6#mEEm37I@n8KggTfQouIHu>in?nQP-XtDl zA!Fbven!mx2{y~*uSy;hO;%#jCXe@(A9w9U31U^pcdZ@RTt$L6T!8eMn_)alRrVJs z`%A1F9y^GfGoK~>xC5uQytM(Q#|A&zn*6x7?CIp@@mnenV#8Lv`%--N$XAYtniK$b z>CBc!e8=L0CpYIAe39#xb;$a}R#<3U8&8D6)GvY8vE-*>$)AT;-HuL8ng(K! zmV4;;=DZINIKKTMGxzbPZrINFuwC4w_JG{&jt_g2d#q~WSo5pk-WA!OmTUI6AZ96B zLWF+>6v%yu#R5^E=~idDXXttWY)Dz^(?zyYv<(E1D&7d(ouUn z@l>ihwR&LrgY1HgTp!;5Iar5e{h9lb{g>h1NNo2{nB8uqU6<&~u-Pz$e8pai;+MeN z=kc|F*@|Uf1tud4OdPCX)Rl-zO7DV?pfB(vHwUZKbDzg2Y2hAz8`qGjjd;KV;&UtU z30+Kcn$4T}9gMwrm;uO^8326r=VEUeY6`n?2BED)8*%b{)2X22{qWjRkjUD~-Ld56 z?npeb2h9T&ZaR@zW#NtC)o*gs-sV{4p00f$>!&g^fe0wcE<{3NJCRs+cOAup#gAiG z2?%?Q<>JLlpm1(cA0My8W85_r?FPZkzr@bJIWNgPKzQw{bT0Rk2_g?*3M;+7H~(dL zV9xBKdEE0@lFR?{JT5X;prm8i0*_=~Z{qLLw<)|&gnLdKF0v|CAiZ&B{sbb zF!U#&V#~nry0Yz*qio!fX`pKMhj={aBV1kbArdQol3AA`C+?8h5?=EddLi%@aYzE$ z+`^4~xM#97s=DPVB)f%z%BsLWl9|1Sscy&BC3}$0D;+uD=g0WruK1};YrqQudJ37Q zz=@AS?4HX#7Jb$|W-${*fGkbi74G4wr4%1>Y};^-6+li2njdrl5F$6c<^y1)vzQGl z673nzsj<9LnQTF-u*`SqV+grFyyk9%#MSojntzfwrXEnm{ftqv*VKSv!-0-P2)euD z3TDn~LNKh2=9Oce`lg~g08or>wdrhvKY&+aJkeZKA59JC{?6ghpa5Ce@mqLfG?o3~ z^W$Nd@VcV*p8lL@61#T`zgafW--=O=%nea~Og5Ow12si^-})+}W75cyDMApw^rs*q zXK(*3>1nZZ6}Myh!*XZ#PV@Qj{ud!|;5!y54dzUV#k4>-tbN4QRt#Aj<)^9R@UAP)g-MOez`x)h~ z??!d^(r~kT$*lHD!vDQm1c2 z&Di>y|3nXG8R*onvDf}O?;V!@Tn2WWmS6pg@Qi7p>mKCpvGE%Rp&9-u+<*(8Cwz97 z%~<(N{&U(#^Hsdr;{OkMUjrUhb*(*+QKCj>VpF9y>Qo1f5+z{J#Kf9`37yDv&} z{@?nItDYZmaI7?^{J)?6aI$0I8oLLge&ae47-it+pJMiP2T#Xx#E(}U?=WIHG%rW@@o;Nv*iO6Oth7Dhw>7_RkQUp6;c%{N z;mdu-tYwi|S$@zGbePpEXV^kzJ`N#adz|UvR?R}X_qB%C`aIc&=Qm=~`G}=6-Qujb z)GhrsvBkluTZ$K1CK%quPjPUjt>N1pI#gxFW4Pww>pcX~x$M$mzJ1gT|Lp^QcWSD{ zNcOMyw^O4+LHZ430{^2i`=BglBw~;O7m#$nO>WHt}PV!rMOU zDE+zc?Vn>PeiGo@|2)C*(k}Qd*58itFu^ew8%-|AF26MT!^@B=SO^4z@a%R4FW zL3fkj_Eisj+$1<$i{s3EK0-S1ud@qh`q$Ye_jY3ues0CttX0PY=Zf#Bt2xlzE)h4!*WfY1j@)NL zbg%Ov)Usv_m>vH*U&S~U{&ku|*wN#3e1KkezD|I;fhluvmokrft&5#>=1{&|mF&deo)wvI6LOBMyBKHPoq_s1 zhBUyw=nLe{^27VY=@JVx%IWRnIK2z=HA!l>(5G>wp^srZUvA`G7z6TM_`QU2_--)p zXyn=#&`aT|kO6|F|J#9>XP4tjeS#n-Hz6tV#qwy%ypaq4J0gbP98wDbJfbQ$WoV>n zIN>V;0+sQ7?9s@&LqO21DzaN@G{QAQDVcG(Jsg3bj;z~)(xCvin&epXZpI~(1=$1B zWz{(#QDK7V;(d{SbDrc&>tm7Lv7l;LP`5wbM;@3sV*H|@PK^5sx5t(Nh zQx8f14>D=~CaC1yq44$lTs-Ck`dqmXH#m3!ZkO*1rRE_%ExdyeEv88E;ywTr`jlA;MKUcG6e+e+Cp`R2?Xl;M+j`!=`l6`PrBD*n239;O;Gfo@d(Z?SixwZ>YkQICnHMJVwJoe%qhQ2Mw zNp#pIfZ2y4*Pf4%3{@rhe;Ss)U##69S^GisZPq>|gU`Fhhel5eqm)%E_aQ1_` zY!+oOo$_5wgcTDy*iw#7UQ8v?YRZp6&oRSBoG|Vv6E`EQQ-n>1K&a&CykDvm`NWb2 z*a%%G?HHW8RN>Psz5}N5d4sKbNIvy27Xa(wXX!-}ljepFY^oSUz7W!*Q}I6DswW}i z`Xrs%$<-d=o&3TuPj!4pq_-AB#;5l&Yo<l4jNR=%!>{{ux7UBwk2QAv=VU{L53J<}5{koxl=v~$kS5PHWdTGUInwD5si zC1~4)Plxz4XR;kxcP^5Z-Wl`?2^Jfper?9n@EK;))MX7#JAN@zdRGXEy{)#gEF#ep zBuzv6Eus>IhJhqmRNz3g4Z+GMqNpaIs7}r60VXqI{P>2=Lhzl=DLmQrgWs&&k_IhN z6c!uI`tv1ey$vA=DFoXmn6k_J(Yh|a1}sA;R$xXjh{LTZ@X6~KG%ih<{mf%A`9@NfaRuLs#%egnl@3%_SI+G8e2s6c1Y!>~2(wXxQBd-=~fsWiT8*hO*fM zBBabQh&X>@4Y*Z6JHTO;gTF0)7%M6`J~IxT650AS<4W`1p45L6hEhnCWGjRVQ=P@z z6Eus$7pZDJVUqI%gcm3la|dHX`1pAKqHK_ZSc`#K=TCTo5A0Y)QRr5gG0R5&X6P=$Oq?=Z`JV8ysG8{Vd5@*~QUW0JYU81J~GX}uDd6fn-_Y-cuj zC5jpwz#ZLK+xEfG%00QGv!%Jl5d3B}hSYHuK6$Qv^)8BRmo2jGIQx1}eIL9S)RHD1 zm+^EY-gzOJT^SnAhgPz`9+Qzn-HmpCm;cI%mZ&Ob%T`lB*%!UJH2a`{-h*m?9~+5I zd~CkW4k9_;*bUw}ZFRJ?A{aA_yc|=Tg*T#N&`2%DZw>kmUFNj_fvE=!RTR?Whb z(?JmJjg=}=U<*e!s^ztW{ZU>-K;s`!8R5w*xE3X2__8RFCxJ-YIk@gEZ_Z)n^pE|O za}MN)#~S&6A9D}-4ff!<2Y24#e0Whal>b}pVgDbPdr4X~e!k&ZdW1}?a!%-uZ z$9(sNmgT=$j*+M4TO)B5zlB5lqIz}U+5gU7iO0qG1;y_=FnT_Hg@fiEH2t7&&*Yg$ z1Ae@AKFi@Q6dF@1r;z5;+!gXqo_Pe#JyhBFEhbq0U*jpzj0;~Te3m-JgwXWyk>P#K zJ*>Cs8!Cl=&9@@$xbQJP)A)bu+`}C<{gs-KJ^N|#9$&H!I<`rZ6AEpzia|B}2!1L* zl}pgvL&_|g3hnYJ`{{d+^S2E5_ndp+ck2HW`&yNQ+SjT)6z3cO>Bn&9q4*iNCSDUt;kV55Lsn zoQvb3b;{^>I5bbt0{$u7lb;11e!0aLd-!K8PI^3){#smhu3rO z*ko}n4}E-t#iI&1=i%m@zJ%xQoRjt=-vmA!jeiTke;k0{9e{I=ijy$#=Z3utC%_Jy zXLx197ETjGvdba-xyh!8aOl>43Ea7@yafK-jQ6**W$j^&ar;(l+`hrLoA`Z$cQ@<+ zH4&cQ;M(o7GvL?_PTnSo-#0`@3TGR?Z#Bl@TdgxmmS4Iky!}tFG9FKY7^X7i}_x7#jo^mrQ`Z*bNRf>3$J`W?cpkiB78&Uq4}ybx1mWMUSjdIhikd_Se)Yz z@~Gp6wH_X`biU}((enDb#WC|FbY|J`-*>{F2u$VkV+Uut^KAHEd${K7cOI_!+V9|Q z`}QH*;i-J$mi|}|S2@pdaJSu4f6+=u({=a8BLB+g8i$U{&n5?Fc;)9mJUngrJckMkZV%cUOYwVbc$JUx^SDPRY3b~>xXMB0Kj`5)-reueISDjWpARJg z9LlHC|DcB}KgWBx$|2+78a^9;l^7d6+^TVCvxn<< zkFQvqYC$@D*XHpJi!)zw{8axdtZks?SNZ>*gS+jJzHhfXk zT&SEi-J?BR(>>Y4mCl(SUTNt!JGfh3oeoZ!Y51!>T>0tu@VKS_RS&PX_#KX3jRmIV z6|>Lz{e^kfBeQ(js)Z}M^2T}UqhvyDfNkE{weQ=1C2ipJXpjrzqa^pS;@M5T`@%C* z+25zKPo;VXi&MS7#qZVPzU}Gk?!x?a_X2!=mz*WwSdvycaB#QDe)iAo$?p7EuI#2n z?|VgE9rz#Fbzx%Y!LBolI74$WOsnv<{TWW$;EOxG+C>{NFHjmDk%TLX#@Yw1r;+Yk zKXLIrMWd|ml7X-c7_rj}B^G-1asP=-SUmgckB|t>tDcoqp3bN8>V1+?{ssD`U|DMY zV89pnha)*U3h!*aMe9^<|DXG!P`McCghCB^Vb4L|+Sl7L|2!LxZ7@r-*xrw^=aVgN zUsWv{3dN-8^SAj6ZR-?Z9VX47_ZdY*y@jIZjV;W&-Q zqe1=)e;QAbkK)4TiO=>~`Kg>JKSCN`@n&H5^#NCfV{w4|eE4ES@i9`-_}UgKWj`!G ziTko=*ey2TB8_Ozeth2hvSZMceHYepC;>(p`1$9Md${x$Tlw+*)II$4-DmDK!~H$K zm7DrdDEY$G-|~%BrBcX&>3XdY3$R=bXV=fZ-2FD1ulL$XW$>l3s<2^%>AFIp6D@6) zA@iU#XlQ>$1Lva3D!i-ce>8PKC)*8^qrGx&NQ9pUvJmG#~FM! z2p{U-t2sN6Fb-G6_cILKdta-`T{{n4a$7_!y-gd7tr+d6w<9js_U^*DZ#ylkAHfw? zj6>*V{y#TGOx~TpKRfN;(}jLi=8}y6En~7rdxtfk%lPY3 zE~=yq51I z#qytaKbY$b8ykSS&WfJXWh|jK#{Dx#VCCTXnPeY`NdA&_Gb@?~!uKLuq3%7&+%zmv zmZ3&&X&c=*G+;Mdup0pVL!pPc$AccZ9`h=fb@Y>(j-k5whGDWJV_ytyE1Gx zZmheqvgd5;tJvCbG-Y-rhPJ0O>3qIm@>qntRnmMTUc%-43yutRKjLUYb!gIu17|^~$$@H%nQTuTAt?pl&iU6%PN?kXts%NUCC&XE@YY<%4T*I9= zmefwq%wpllRG?#lpcta5%_r|6mANE3G`L`_4pes>{jt?rGVb;I4QZ+Jz7{ zmAtN(ue$4EL;{FLB)D?@O)0wA+?kDBir?PLr&G<2Vf^(GTZrhNMG0`q4?ybO~ z;4JL@xTOAaJkgvTOlRYw{xE9$s3Vl2?rQN{k;=TujIE{`H20TO(>9+wqPCPw?Eln= z+W)|P>0#M7yMJzkl*o149d?-mn7Ew`Z_*;&MVtD=r4VWAChQjcR=yhk#>7*T`L!$Y zvQE1qRs+_!y*NwF)XHkA(Sg;+)<)XiieavpS~JU9XVa*&Qn6_C#G0uUSV&v(*cxg{ ztKqlSP;0ZB>x^oSZ65%~+o}^-X9&4J?^y zomIKNdmz_Yrj)9CQd&KQqQs;+R>0MIWB;@f@V`%r(x#>}+fRK14Qn-8Y?ysj4{%Av z?~zDp^+5MM=%Tb*RP`)%YNOCBP^SeZjA~(l4u>a9!t@D+6jC>FCtRdcg;o66xqxh5a$DRH-T$2HEY=KF7BXa;V=5z?88pc-#7t+}SV1JUrX4Rc z8EL)B%8|(9sO+t*DWzp6bZ^g1DbWm7_B7Z!P7NV7swOh<7F(weiIelIeh9kK0>KUy zbZ1tQT6sO+YtQN4j^2WXgKy(@65{hESI$~mb)Ts2`Q(A^ZFA-kv{L71(kuZ})~3Cc zHW6(RWCEkOmUAsVa|T242%h{BPsZ9Oa_-(rW~Z;F{Dc~cZ~TtPx)<@8jF`xT*aMMu zRlpLtbqi3J`mn|5;kY#%n2JiWYTVlU?#Rg6@x=(=`)*NW?Jf-3jEN(>(5zj7JK!TD z8~MJ2{+Ui5B%wk-&D_idTXyiG=g7vzh|KQ^o9vQ<%uDF{zh!!BT5vQ<6*fd0r78%8%(>C7`o((AMJe0B z2BuThy%pvCEaE@ZS%zHOvS|ASSe0u$d0d`I1%bsQoa!6XOiGUHN)hXHiW(Wbk3yPC zAqkStuFW$s5B-U7~8w;+06e%MXps(y_3222M#tsfl|(x4bcA;?XuTtRp28D`^l z`)X85Soia@4aZ5r5n#(7t##xL&RefK1PL9esF4i|F-e~+_*G4cUsd)SPI+KIlw=Nl zT65+ZTf5Hh$I73-mTBp;IJ^Vr%<6y5c`3FyV0WA!*g_<pH91h8Zbi@r!t-k-MJN zCjW>bdYR6=G&Pt078KdazMrhq+IjS=4>Lcn6Wm9THoHd;jzhU14M|a%#ag^Mv{W_=#1PCrEtRYc-i>!m z4T`e}eQ=MMo@dxz8G#t*zfe+)MZq|#<04kav5+e(D3-dJ>&V6`8X4L6RTi}D-D2NI*|>;vAdX}(_{QjqZ?__v{NNkA z#FVjqf>W)UT)kLQ3Z)d+RhHNMLj-+@tNos4cu`Qs&R>|$!g*whBC_F9qxdF25XSV? zD3l#iC_9jGL%?b`o;)f~9?d^Nr5!7(3<<(ge(rCO>fATTjd8 z%lNZh5-xS2D!YAZuI&c2pZD0p0UzP} zQCmjl>PK>c$0nw6E>l8u5nx?SPY+&8QrY|21lscEn4aAe-`j6xV8~1o(~X-xhhF9} z4i7r`=p#0@Q}>%`3l?g8WO~HjWZm%;od}EK(+jxo20Vt)nC={y)Iv8c5L?qh*H`!I z5&2j3$cFupGhXUkk#3J{Sd8@_S&pHFPkFgRelU8bJ6nD zwiQbkE{J`6^7*OTV@o&VOc{1mv6*+s;r7M$y<2Z`2t|xWj;|YZ{ef(Zqa@SK9QjR zKn)#SizPG;p^Bafb~WC#{*vd+XA+ha*696wXaFA;Zm@bT&v$2QV_@7Ek$03GPw0yUEQDyi2EQV5aZ;z56EEF0)XLaY1DeE`*jq{qarEg!m z9<2jD2ZGl{YR0YVq2MYC%c0YlyJ9%04;Pb6vL@8i! z$`7gNvdB<@LM01U2BP6dyU~zg$#4dQh$;*(bKOzf_;gqGOi>mK2}vu; z-uM|P&s;WatxscJ8TxZ1`K5x~I8ZDtf3$mdZmNDgo?6{gqXdnLN&hx#CLQ*g?;tL6!KHkQ>TOqBz{8@r;?%5*}@z|tA?~HcXaPg=BDHOu9RT1 zZhAw{$@x0Y&K~X`Z!e_OfKPWjNUr;tP553My><;GhBT}K(3>GMD#Ql2n=Wx9!wmi_ zxiX3~-vlna3`3-x&|L#A3=gr5OFPY&z4{bj%+}^+1*WDzkMZ@iZmJpA3 z$|NFNf{^Zu{olF{?SH`OT?~0enFad1J9i8oI!A+Z&C8&<@NFM40pDN% z9Pf87e2tf`p>puNanM}2#&6e%_FU}U2hD|R{FE17P8kNe?)7S^hf?KnruZ z3J1-FZ{w0I`3?r|}0e2mXEezhBTn6N=};q5m4zjZErjEwF= zq3w9_+f%edRQJ%#0 zUeC-g@u(O61dFRYH2ldHk9pzQL6A;`hflP)>YLK3u{h5>6sO*jP6I(4iYIYRJSAwM zzVe=U+QXYIKEuP!ddzTWmWQ*ACY=@!=k0Low5Wd!KaT-$D4$DlO?f@JE0%-3HsQ=lf@C6p;xnyL5gB ztTh%aU9t?T%oeO{Uyg-l9lSd;)Ug70)uB!jEE8ABSTH3K1fMi^vmGXJ+)1_-lL=Rf8gP)?>t^Nr#zn;UZz>`jQu0iRQzz; z5c~K?0&sqT=HZ8*8GtVezVZgS-4cAAr9efWH@jAIS{i@bk6G!B4_- z<>$u^9(VBV4({^#cmV!N0Dg$=Z~54zbCiR-@;TPQk9GJt(ZO9h>O<70e_25ID;(U_ z&#N8WO?SP6yXopaQGU8F2ZaB90DdTm;_&I5;NcAt3U&k@p0xNZ57&B;3Ba!jz`x_+ zn(m)HJZaM%XS*u$KNdgLt7->l)CT-`ba=SZ`Lu^ONND(4_HYgVZyr9=hL0XLw&dVc-jf?mdjiRCk^F) zk%y~ZE%We7Nwu)Ix2CH$%GY?|HNV#f;NS4@q@{nGhbupKdARa_zlWi}T_HdUF;?%~SMyB@Cm9OA88RXV46xYC*I;Y#Ox4_7+>5P&ZVz&Cohru$tF*K~j2;hOGt z2j{1Dtrxo-yxhT`b8yO2)BUA~Yr4Pna19^w*12l;iiiGf2Wc)0RE*26Ww zXL-2tGsVL-{Oka{!^2fSdp%tF&jsLHJY4nJUuUfO{Z}u%=GR|;tl{qp2*2CIHNP(h z;6nlUhwVo@?H^SR<2_vEP!oVp^Kg~JEDzUwUF_j1heaN);g@^3hX0I*Yxo-i@GpC~ z=IeU__%;W3-`AdSaMmN`=S>eUvwHrXhcgb3QT*VKL*=jfTo!Oq=%<1z9Inc@o?3XEgr7)f8^mQOaCqpk6Zk457+RodANo@{3sJq<)-0J@o?p{ zDgbW^z%TT0Eywu*_+=ih^6d8T2Aki_4(`5>Z*}l6!t%#sR{;K$hbK+&(90gK{Jier zvuyaOSRlrO@;n(o)#nQwoV+NVO9Jo}0r)Q*+@=2;2Y2be9e@|xg3xkTe$EWQ)nGuw zt9+UQ!p{xBR|MeKc(~I4tb@Dl#`O;Fmispx+?B(<0r+e?52E>ML)q~7goiIMa0u(9 zx4!?dK>)_;5`Kb=TV&22kv|JI97(Q#-hYH!#ZntxL$BO zATBNJ;HtA-e2%5zHz8LkJ>z`WOId)CyR%Jc+GGBCXFN9kH50T5?8uD|Lb8 z%9@@}VZoyH)Sr1^Xh%LNE@Lw`f!Rk+eZv@b*EQFWZao%04Zj|*8kVZ9ZzD`gbsYbt z_{Wmmp96dyY>Wd7(+)i41=>!oNWJvIylpvMm838~ELUsh3kL zqSI$gfc8hn@%o|`)~4;C!{T7or_3Wa?wEib#&P+ZhRBTr+0jD-*>iG7C;Ll|!6M6W zWbGFqSIq>pYnMd2_A!xpD>fn@|C`9Q_u`HXMO4tdfocWSr;R(MkC%nQ!eU_=0K@?C z*d2k8&FKxGCZ!m;W(6s3o4FL0Ah9LOntrlq;rz}emn>;-x_e326|wot7Q~wFT(EL} zJMWtAg5^o9V$uB0rrWz>#&<|lwx~Hft++XRWwbea*_h^RXRJ9pvm!AtrRvg!%QA~_ zQN8fg#Qm@sVFS3$N`=y43r6W+M-X-Z10(sL7%1Y2nPgbA8%w0`4vQ5#8HccF_5G-* z{$Z4SWt=U_RuK}m%f?wN0xKd{P^3{ZtdML)x6Ans*^v^lOKDde{ct+BAl@NvJS4a& zz(j7gu)9Y{gKeCt3uB-@(P(xy*4tA3OON6G=tTBRoJ%9Tk?LJqgcWhCofUCp_7rZv zc4FrVWQ}4dI>zkyWBz;JD~_z)fw@)W(o{Hvp572y*MZJev}_)=i}FD6{sszj8f39M zvTg>SBO_8*s1Ywk*3|=R`biVKN?*FNeSYfh6|qYeE}p+?N$U3HD_5Y{JC~%kEm#J)xEN;rqj5cMbppd7;nzEf0EZnM2DJy1xvZqXQ zwTYtI-XW+o_qdP}5V74RdwgOK^rF#F_^wpbwizpyU(&vCX)Ja3l4VBmKry-mHbKqV zBBn?Qp3*qsZB*j%Cnh4Fzn^Ors27Re`->X069=Dw(KS;TBPrO`{&1_2-@=B9^oV7y zMFQO++o>hibc|XvFcOGxxEL2}R*hOinRYN3dhQgfu_0_W`C@mD?d7+aJxZ~?61;qv z4vchKRpt|=P^^7b~gqv`NOy|7L zW}R6~E3vt!L`&AKtn68?K18tJpfGfu*`*b^WR|hz6;}vg>7`jBz*RVPpQ*&| zduXwvCt`g_Yn*Mj_6GKho(@k+&?VXhch+RT`*W=9%sFc{a}Fw|S4AyciMv*lqev@n z%o0b|utq+@nn?Lw%o`KS4mucQyzUe$ZJQ)#fote5aquI z;n*&*4uN^Wlev+gUpk&;BB^pq*{ulJh<(#)DDq?Rs7)RvvLB`RkHudU+pwX<5`tG@ zB^%(fIPqp_u8tr%WDCVZZ+*2)f`8GYcy-L+y7yR{wwM|=Py{le!4<&B<*m5MMH6)w zExzguZJp7kbfJalTCzNK`?6U3lBG+!LIWchOxn8#SMKL*i$!CNHb$gy7I1 z18lnUqJ=B2h;=TVpSlxWP-oY|4)g(|42^M3*~^Lt-!R(;8O_qx5MAPZ3#S&0sm-AK3bgNLbn{l^f_E__VSE3s(`|3lZX5Vt z(3)!3eV2CK&?^jCjfk4=%wVBkYn~pc zrPZ1uzI-N1=RXDg0z3as^b1#_^S`VDrB;O=za}&|3A6F4ER-9SXn?s!n{a3&rA{+5 z7ScSRETj&crOl(xHQ+%bP9xcordaq&_R=o%|_eS0Y1JryzNom{Akl z8>0F5iMC&Zp^sTk4*ZqEa|PbQ3UW5`H8O1d59Hw~F#Do8Yad2OBrPwn z0z=RkW6B0;ttTTl_(dbQ-;IZ@94?n0#`=O$j;VDVLcq73>_!St5O##p9yfy%28}Yq zi;)Lrm=%Y4TP=ArgB1>Jppn#S`ZhB)G?ndQrR31WjDZ+?KXgpSJ5t|d96=pri6?U* zX&H*R_V@Og%iC9?e_7UacdQc~P3NK|O?NKrjCFBn+H@BOr=78i<*7S6y4Z}JijL<4 zXom1Ua+x$qXo_Nk-@_tTY60Jptg_zx?P*!RQ}BgJsQ*%%#A}k3FbJCzHAHzyD_w_p~2Cz z3K!pskZyDWQY*#Iy3Z%$El>YKw%UcioOlag7pA5ny?fq7Zg`DK@Kan8duFT&Nc5VcF>1`aD8a-x1QRC>?Ew% zn#E^$_(c|P@$mT;pXcH27GLb)T^8@KxJvd4i}N`T#d~l~{#OfHz2j<&ykyyeWh<6q+2Qg_{*f1FhgK}^Dg=VUdSD9rC zFYj0&R~-w$6LX2R7v?1W0Cg;7@0WYH%6YAaYq@;Y!jAiYHo-HcqxI(lBTNLvt8M%b2jFLWxbow#O;r4nfbgI8@JgHRH39f$ z4^P|h>Th28SNz*vc;)AZ0r>6!{520}d&%Pu0r*%lf>AdCOq{APNh_{wV%eT_QHT)zG*YHz4yxouw`D?8d|8zije@&I9`&BQz;@|gh z<>w~>_~QZiFFai7|26XdE!!^9Sw#IEY zHgLd;L-jLhZbRSpaHaF30Q}7W{5YFI(s%3c0tYAU#WvlmJY40o&cR)IKIq`2Q)cPx z^KhmAu7}&(&=E(Ph$;uA(_wLnmGoOI{XP$$XK{BeNF11!`yV{|Z8rP}Z!LnRd!&cY zvf+>QaPq|CL=V?|`D;PgR`U3$7rx!Vp{4+QNdUgy!pW#5arWf3!?br0d#&5 zfd9_JmCi`Mz~S)g;}H%{-ZlIf57%;udANqJ@Nktwm4~bRlOC@6FyF(q+?PAJTYufP z8pnfw)wkQ6@Z?4DCmo#cqKd!Z;4ue3#C9lty%_D`Du;z0u5wu8;VOsq95+a zj$=FT_Jg|}+#Qb{;@O5TK;C%#(!=8h4*kW$HQktf&S4^Dp$A`=+7~W6Xz#aRTg9`g zs$x9;`u%IRTUjv821&RfG#cK=5_9l1cPxR#MTyyoSy9Jp3bwuU$o>De((P&?&j_y z%?CN>0Rt^Ff5oro#hr&pjuQQoM;HbZbZETcO!!4jz+bbmnM}Z4b7bS|PDXXV5Mie& z^P6P$G352TOzv6y&3WF*EjXgdLL2go6zQ^CSnE7Tcoln#(^*iA{7drs5_y&R8G5|@ zYXpEKR05NPD(8$QDN#;%%-s)Zamyb^s80Tq@Yic?GaBnUt0Ei9ApplxX;Wg37``TE z%VtPT*(YXW13_+yG=|v(}lFJ#(yqDGf`T zGW8qOp7hu7DO;KDPo67@S3t37vneu13g!&TF-iwMQazKB?(d|Kmnb-vIS{bef+?3j z+pH`Unq{=7goV`vFn)mUJ;*7f2Pyx@@t2?7!#)_8$mO3YUQYNJ6EUd=l#wympec$m z`~r6IQ%D6?idKKPX+r)E!E6;8zf+v0V)oL6BeB^K=lBQ3H+dn3Bm#WrDtJgYVYN6) zVJKqL&0SLkIfF=PNgQ?$qk8v-SC%f?1WUFtj(#wa#!xEzb2C4G^*{q6$prp3T$njN z`Y(?j7;jKC!kJcsS9ZwiRs6NFDinNHi@Rz|S zB(gCnpj0+&^MM^cnlfcSvaX0}Wy}pen)}O-A@gUV*tN}iUfzR{R|nQf%1 zSWfW?NUs`yE!AR~8;5UQ_%g?oEjKoR#k600Z^T|tk}htJWQe zIsRU1ywvkZ?{Z-!dhu$Ke1(7X=iEx)djUd^f2P11o;as4EbPOe>fk}Qwn}v;C zK;)@N?@nqbkdiQ(nujhQZLDAVh9Z5`TPgrWB()fV-c3Hv#RJA^DM7b67uy!Vy*XNi z_wf8k`>3sCRIbl9Oobi|?bBhsdy6A|V@>Ir(1n)K-)JTh?b`<_O2q?lHj=xf@!W+R zikNX0BBpfney1_+*>HrkE(h$k5!rYYT01rXx?c;1Ow{k^S-~neoPr6y(GZ-a>S50? z=*5o^=s>lg%;MaUW2EfZf-W*1=H49Wh-?!WiGCDJW77(#N~8s&BW#fXdoW~zO_&JL zXo=_?9zzizM=|ATIgG`FdJFx5HXyuJ4WkiH@73~zwlL)0&Xc5y+VV!cFh_9rM6P`t z0;FC%WNL;b!*J5vi<$|sNEDmaAz~T#$#^2t`!g1@?PP#Fba^zHlX}+J_j;u7`*S39BSnl`_1Nu-}uS%qz*04g+9B${UP$O2V?nBtKM$kW|+@OS=|#o)elmYsUO#?FT+s+=U0H zpZ?KM5=&2Fv2#MCRt0K$4$W9WdoTaNq=or!eZFl`H5k?J3*`CZ9G>EUZPEFo$4t)K z6>TWmI5Ii9s%dl$ZHvs@h<=5hh1euvH*v)557RISR!NAr?$G4un&jviy=v!C zk2sqd(H>6~n+qqtwP@N5NKOKEDR`TIXT$3Or*irCM%v>NQqMfO{sK2u!p zgKV508Dm1su-{GiO^NB);>g=%oNRGh%q9~4Z}QUB2l_2Y?<;XZnnCebYe0L}^c9|f z3-V<@IVqe^zbcQI3yrVmj{;`=iC#GMljEn)usrm9p}n@%L!R-k!F~&TB#)R2O;_P& zT=+8Kv)HL7M9Q4xn=0#w#<#^I>xQ-N*r0NX+H!L-UMt^3;2wT_elqmS zj^UFw{grNHT>J5P@5_!+)aY@$C4MBZ6Y=x& x~(pLq22+!TaPv3p!UNhYPgpCyC zN!uOGr=DptMqG7IgXhOZE`fRD3y zm51|oI6vv8o`yp;UU=SgQ=R(CD^|U_Ek%0 zSp^^O6KFs+F@$GWmPLE2{?52h@t&#_bLpv(YXe{H^ ziiHbs%`4;39iHWvEbCfCAn({60WhRP2612rB?ram&h^eSu(Xw|BP!Ds4$kqK&PA1xv;P8z6yL3*l4KHz*&Lj`ld^H8&pKx%O ze#XJw{Q9<2%I8tO;78@~1qUaeO8;LxTCm4_AHL=+GI9I4aLu9o&^O_rT_%e8$af=-B}LHy*C? zdC$XDua2!?j$ddAO!KGXS6G;hL|-0r;0ZyvoYq_a5GE@ekRCUh_4} z;wN}`%HpR5;8O$e6&_w~>3li>@AGg?_j(Uk{=ekm%Kz6rT-}G?h=TJMO)pQjf?cpj1*Z(}} zYxpT%c*UoAxYD`6!&4^vq1FI=X#jpr0RCkU*K~vY&-*r!Nz2a*9-TIekF-NnP@~QMM_i)vRH39gyJY4BKZgGl({pP9o@z~?x1`UN?a_Ec&sQjxjoR+WB zKcvLORQi)Gp~F3Vs>LH7&i7g#F^iMFn{I`JpNwb9PnCmHZteK-XmW5@PZkH@eGX1K zS}vOd@UI8p+Z;RwI?AVSldANea>Bd(`?jj>pv6P|rTcPz%hFN&lO}lR9S^_M;>G*` zg~OM#nfNidE9Wszc$d%PJv?d1hQ@ig>PeNwl~2`^Ngl3xa*l_qo=kJ-yLvLy!Cn48 z;ovU+iyhq6lQjYOw;Y^w7TEm$C;-1V0Dmn2-|yjC?uXmKF3Zd1KVorTo@E}c`tTtS zpKkegZGv5SPPE~ft}D-T9Ngun!NZf5ezS*bzP`=KIDGo5fa=u6>N385b6j59IX z6~%J1LE$>Gko-{_FO_^P({<40AgO{%QvZ1TWn)}O+Y3i>(8cx!Z z{c}eo>rUvJ-`Jmm-J$3@98HG0cbdqU6g|7iOjoBeyAsTzGm$-E`#u7gDu-(cfAX7X zVLtpGnYK>%m+zJsm_UueNlLWj*y;TxV=$H4Mx#pTm!SOkU*l>%|=i{7~fD^AMQyXQVRs)7B@IE4jwxC6yVKE4#+nQZ!{orZP0V zh?d+>`4!_K?o6tXAW+0@No98AO4jjmp0YH~+_&VyHwr#3d}Hm7Zg}cIIWgVhT=nOy zjU&^$nh#Q%=)`n}E?%PLZ!$4sK`|B97mxDxgOrB4<3u4pU&JiHRwLD4BJVgh#&k=JR_Z>z;#VswW#7uNgz2rv5T9=cRv@(sqp>xrR&h z#K`RFy@@=DAv1?B%plLO1Z8sK?1KJiqFrJd8f~V9)A%wad!TWb(lta#yJUdWK| zM%H}`AyHyKWs(hc8Xm?jRtzj(+OCzsHwyf(SaF~zV1W=l0%g0S^Blx}5WgodZ4YB0 zSZ`wIdm6&*(ZY$W+YTAhA_sI}25^37;GQ53u z#cPHR0XMMBh{D`x&MVOI()fpPSHY{w;|Dim{2}tkUg=H!;m1gB)em3&9?pkHdK<+O zqV({=dR#oeXYf+kf#CE>L?YGLq7@4l##YT=aYih&V&VL*g$qK-$>|F}K1b%=C$%pa z+sR;htT7jFs4H@`(f_%Bk8j$xA9LZ8^E2riM`A8L!I^YUsADeu=KqFYUYE<7f6 zuCVUQ#4g0w`(|ADGU2lsb}oI44M_R(vxugvINycs=Mo#zoL1ZP4*+qa`BuDr7>v&} zezgX)XFopYZvC=j&|7T!D{VYI`SE$rah-o`wcidJ>_hg$Z2J8CGuUwH@3Zfwi#)=9 z`rhOGEyMji=fXJ_=iI6unlI`halO|w*KQHlZ`O*(EUx;XxT%*1`wL-xBb^GIc_;@u zSIy@<6h8~s46omI6|b|n>WAWtJKP++!JWY_0eifkIpc)HIhf8=i);RsHqXP&=}rUg z&gp79IUBgjm~r*E7#G9gA^Vb$33aTR-#!78u{=+NQPj$g37C*w*)fssFEE#iN!baQ zmBsy8K$|3*8VZf~aHh?p+QZo&@u+j?Gw;fO z*1?(Iat6lnl!wO+9D2#aRc=EbuJp^9Asm`7rom&Xhbx^q9GQD_i69GBW)?5JY?ycSn&IEkCrq4VU()~jb(A+*-&IVoK~i1%LKWjD)44}68^$Bb0=7UMS*>8>Gtq_bMm+Y;}P z`#X$tMueLu_-)1kCDNTC&o{?Ya<bBvRl~Wk;BX&~b2|G? z>s%6EbOpz!3kh$%I3xeXk`I&v&MBn39|e~N{)U`yv7an%i|>_tvll`tYdwJ@{4U_) zG?eL~+_uU2miR774=$YLerax5aOkhmgOVQ64z=Z6`NTQU!No5#4^{V2OzoL3I;g@gIz*>CDfj_lKp!J>R;EIln*Lk0+sN1IAVcIT`_v z!rkRccqlw7m6^mKiJ|b1^c%_+sRj6qg0E%0Ly_yIfEDI}8DiN=ULfkq9r$KYxfAjp z7g={a0wspvf!64>k{y84bn@PuImUjCo0<)O&XfmpvK0^WYnh^v4R_#upEPa;UXX9$ zDp!m3S{oh%#ZAht@%)Of390_@4U(7M`VnAzGpcSC8@Rugb!!fjf4@Qu@z~2vJ_wX z@G@J5?_l`Uw-3+qAF0+#_ORW?xkM9nHIWVDp>u_Lo5&qAVSC?Wk+qBPUZz?%WLFH< zjp#msA5c09Qc2{FqK?A7cVsPHIaulE3xmvo&txwA{pQTBq*-}5WgyufqOO~rX;PVY z@s;#Was)kzLQmj05!o{Lah5Ij9aa=87Bp9*AtX5d@rdXOz_#$|$q5 zxBl9ptL8~XwRM;>Wov>uojxSQ>RXPrd9xqI=dNGi6Q@)q=-A8rB!%r*u&(*q2T=9+ z$!Xr(i4oB*fEBMI#2_C2CUVVI@JgCwu>brK<_l<|?(oQl<-+HuD6gocQor9LZTqEDrq31zWz3jgD=9K5d4?X?B(|a2t*AK8kFsc&i z{V^J<0{^MZQ)Z!BqPFtJ$l4ShQW0+kyLgn$9fylhvbKCvWG!n(vOfV2(xxU@B}mqd zh-?^1B@kg786^9wOB?&%?!GXu=ozdCeBj`Z-znW`>S|eKElRF&Qdi%*4RR=a03ReX z2ch?{5{X>i~uvxJKuYR57WNP6}Y z(1;JLs;9aN^~_Vu`5HVW;d*}REWPRgpT_c;w}qc=Cq%COwe4@C$O1Vm#yN2(8YkI~ zU>C@+?-E5pQ$G(+$<$(o%Ha|zNkrp%dT4thtF8AEOXZ=q4a>Ron!SWAsn%BDHORWTE~38C2C+|1Z$YQ~V=)dFr7bvMPP#X>jO zHAaJwnISfGDda|4yau_*S_kz|@*@doy)c6*cGc@a-F)R`!jjX>qpUa;cz_ShttF2) z?RaVAlw4&QYHkZw@MB$Jb0~W{$xW`SJi4b7EksJYO5?Jd^=l8Fr!v)>%v3@mSMu*@ zQP!WqoZ%^@o;Lj?GtR)QZA>2%Ig_*1SDP-uFg!Gn&OBhM&6K+OT=ymlz5!ubP>hqx zyh%}QWtwczdG0UyBTlH}zZ$9Sh;BcT8u3=sh+U~7j4c_t$n=NoAEZDacHO`Sqf&$@ z$hc4-w%cG4S3aC9@0qLWAFeM!)kyB6~KMvo1!Pj&W9K@C~f5 zBZ(?z)K*JYt`FiNME*_b>DXFxNj6-JJcjGGu&mYI8R`8rVw24jVh^^08@rCv2!tdC zlQw}Uf>6%IINxq6*IGLiHhP8T@*x%gMUEy@^pTlCzUA6v$`p7CRm;3+B(f#fB*sP4 z83{vOSu6>uZ+O!rFku$d`EovbG4c{tfeYrAKj#aHV#qJ$ZZ$dVM*or|wdpuT9f`cm>Yhsk8#O_Z-YuaF{ON~v@@NHk8puZKsF-m0}D|;F$p(Qibb39Rb{hv`;YDUkl0jA4Pu`?JJpE=-#_RZT0Nb)VEsyE zq)Q?X3{2V2Lz>wAr7;`+i5UwRHix!jy|@`kBp{KgbmH zPm790^y+8Dj}HS=!UIKAKC7UKcb;n;w-B4loHBR`HYlMAk*D`kl;` zMeSAX&#hOD$i^w}pjVUJFmo(v%t!r_vDrGpuR|icpF{HM$EN1WH`&(+^csJNtlM!2 zdB^x#B=Tk=7yc2|z5Y#zStuG2uudFdM4&~O&ODfXid56PrE17b-2;){7Ep9NZWz4) z8_2H|eX>?gk+t-YNu49VX}C_nL}}&y?37-Mth+;=nc;t$Ev^}?GEDU&Q*zJi&{790!kW}I_nRSe4NZDyn!b7f~z z#2jU?FKu8hOkLLoI?4JAKOjxnU2GCC$%)_~kdTG!h1Kj<*$%AcHJ}T^v}-_I^bTk# z(%Ev`teg5}`aTt#ng2H8u6+kWD@{Kc>N(ZOocSSl=+35P!t2eLwCk9m9kp+o0dDut zjhYYA%M7br3ZW^D2Arw4P5hK=ur9{{Sq zDMkcfl1-WOnEE4l0-CDr3Q{ggm>e6wPC}*%so+)Tto3ILWZV7eD+~rLu z@DhPJ@@S;@d+32+QXs7dsbG_r=Kk=#90k(dQ|pJoK-;6NVUJ^zM+Wxvi_&5nND4;3 z%;yWTN)@vDMR^(h%GFO`PN{IxIUo#`sX^4nXi%Z~^K8l!a_`bEWi zg4HVfju~SYf}PgvfX$A6q#Y*p@VWjn%GG z4$v;4=#H(}kPcoXowywap$3VzL8@_X71I{Khax>`)sGT6>_B5`=FpBrHc}UHv`u(Y z1#ixrK$#kCE7|t3Y{{1j+O}zUZJTAZO$y)9wtu$!hDjA?ONl<3g%9?^kE1!io?0g= zIyeDA@xE=kw%VPMwa0Qh6X|cZNO|jfccQMS`*5e=3d2`&%wemr2!@98@60Q2AAAUV zlzKr2h*;lX9E%k2pe_A{tZXhH|8 z0fn&7W&6sd%Q`DV$=Op(&PChpJjUxIa;O-~%Qecz%@AMDw|A0+|%1;ZTK`-n%$R=VnjX!a8 z;L92h&V8Us2^I4O#~5=G8%W|DJ-J6pC-Gbm*Yc^;!8jK}nnCe7=kFexzG4$_!Sp98 zV9%QVcn@b-9(r!D*Z2^7z_Hr$uldx6n!cVd!~@b&_0w>hZ2T4l>{;U}+>8rfCVaN9 z%7oDLDgXCl6QTRX8!Cl=&9@@$xbQJP)A-dI(4PIUR1d%G7_{8ZBdxUY^yJ6qy)Qe4 z9b+d)H(BCrV`KQK{8TPMHW6F7hz-Zj%qjoxr=Ne|T`=7LO*RqC?{GE|%qDS_hsq~z z@fglLRL92Qn&J7JhvKY1#47|X;I$TKJ>{X{*>^I$Rnt74u(;}thNsN}!`I-0bgzLSsuQ|;w>J|+rOs` z$a+cdU>lHri)(qyE&i^zcvJ!BJe+OF7x3J*4blGMYru!2@!bIY2LbpU0r-6ZI9BA# zBzGnxd)mRqgtjHLFp&*bU|Vu#2*#I7VPev?V*Vl+nP3}d?y#~1Rwn-l8xw@CrQr$I z{c~;qgiw2DS7sTuRiS;#nOyph5sWPTUxLN{hz`pXEdB3T)`euw<{5DdrYd%4722x^ zW3u{x@uf>wFFn|D1%bfJs)e%nfB6E232|RW9z0R^w4VBVV8rl@#}T%@WB79X@Jb-Z zu?|i;s?X;J;Gg&KIO6cw>fu`7_Xglaw!$d?8oo6E&jjG^`eFuAI`;&G-yMKIAAtYf z!;_NRQ0Oln9=G^WGX5@Jg|9KuBx8WCgIP=D%*W$D})3${8dJpGw9{nDElz~HEusHRcbg0ujzF~37?L_=k zKI+m&%S-X&tnI4e`d)W(0KUV+RSr)F;JQwh`C)mb@Z-^JGpOmReEz}WT7OlZ84p+4 zwL5eeR`b>6;7nKZ^=S`Jk{KMEJY4y?$HOa3@X+G{`0F07`cupd;n4glKGwsPzQ1l) z`8+Ei{P#Uv!|OU(KBgSBp1$gYciTgMj}xWyt`nYgRG#kMCoY~UK_DD{etSJ!%WJ2H zs~p07aj*paQxK6RX?9{ z(q&l9?+Xsjbd~>KdbpO?N#1%xmCu<0_yr!GB$GH62jG1kuJm&ruKe8S;Trzy97M3=pJk$l&hl^#@9#mP>8|&}msmO*JY2(n(Zd^T z_^&xQbw|tnUI%Bn>+!IMSK9|qdw7Gz_j-7Z#VhQfU*)F!Pw;T1lk#v&CUl{PD?b-| zxYAkR;mXf457%<(@^CGeH6E^daI*V(&q4hoI;VOqs9%fXpf<^N|M zu5^Cx;VOscJzV*G*~2TXeEtxCf6xwKRGu3CI1g8SxYWZnd^P~z;^E5Y*F9Y6e9yyG zAAacJO6NHbFR}UkvxB>Oc$8f{p!wDCF%Q>tD?D7~|1A&Ka@=Wg)kD>{T^_Fb`LIVv z_3BxR`+D^YFTBb@J1k9C@#8GR%&)75CpoyQho^dY+RAgHhuhmwjfYp;@Cgss^1Uzs z?+n1N^l;_pY7bZayw<~2o=0IdH zC6@j?53jfQRRQ?59j?N!Xc__k;AY;Qx@W_Rh5!WLBj zrGJCks%opf}#Ta(S~mZ_~%N9y$H^g68q)6 z**Yh<&e~DI`U>H=!h-oq!*YKQ!qIeP4ekdTaf6xQV~dVYWwC>ZvGc!^5};xICP@lr zjSRXrewzg5BV$Sn=vE8ia+|$`p?w*`=SBm&MX~GR<)k6Ez@xa~fy-ihS$A5`M>tcx zANQPKWOmY-Cx`BXW&7tCw(hj<7deUk2Cjv}MBTW^#&6>eoRT07y!J76W;g97-XPRp z-_N+y`pfDEXe~N4c_3UvWO8r)c|%vt?0b8FY7mO_jmFZ!>~Fb*VKE{mL5tai+XEp$ zf{#cVSYCj0_7-{k@&1w%re@<~61l^sj4L^o^TdbI?4LXi-3R_QT#7VNIOGqcNwROK z`TWX_1Zm#EKx45oN<6DP(fKwHHrkA4?2`@eCFbz4f&B zo%A8le>>7whI96}WO_9oCm~K2RI0!J1_many}-$72hPY0A46C*u#~86%$<@P7q3eU z-3u!w8WFH4kegE@8!k7wAwNkFXRJmznvncR3Sf2*9@_jQk$?&FY-H^fV;ITEpb^IK zYv%)CnUQ>Bu0Ecu#qtqcLn)rXi3ctpF;}6_%pNj2#!30@!oYPByeYFIX&0ox#Cc?{ zd{p1FD?h-QbLJg_896et_S=|KQ~IAd0<1HIzU^J#RWP#lZZsFn6m|Aas!HN^#3u`? z#fcI7klV({#~*2&@N8r4dr4R^BqJM-M5)ki#{;xokTxfkX^ST*eJXP@7r&{o@0Ure zG`R!xA{!nC2&s~45;jLk#I3-NUog2PAsAb~)%QR$(r7|pUi} zQwHC|l&F;H{`j2;21~iJ*wkG3q$&N?(PZ}VWdHP`#@Yva&V|kSuTq)EVc$^k8vmZ9 zSwZzvL-!Y``62S5SlOfQ07WaG<7go+^?z{5wn zzw9S86(E0)vq)J!iCotWsEhQ8F|sii4nuU2zONuAEqlsf)eM6fW*CW4aBvqf`z2g_ z@=r)`{I6GCPC8P8xuan31Do1#vVZbWvVZP#`hF4T$-b0Hw{v*vIlJR7+7Idce+5Y@0lnoiLua5!m0>rW!lK*OnfAkq^MV*8&_cu z!;EPRnQb=XY_+IwsseEC@#|z3dkewcNh+BOO&u5B__1-}&r+(Ca|yyu9#{6ylXK-? zJN22m&C)+$P2Lu8gVPv1j$C&J-YUt$Be-MH@4`8eD?crftE|M5j5JLJLB`^k!7*CRmnsNP)oc`}$P>tmk1Y`W|b*NYb5$*l&F-(i(p5dFUl!IHZ9=dNB-U zi%~yYcO*`IE7_kuV+hY$MojL%>gb_l?e1ju1Gq1RBcRRkpMo{?_q%v{HL`IH_`+s` zD9Yo2(3bRd)KX@ZX_2`Q&WT+64I#Lmd8+Of$tzPn3Oy-0nDrn4d7a&qeK3)2|6wS1 zb~L>lOt zZH}nV{mZdt#2CFJuPn~&X%HlusNJ>o8@i5XgSClbVE%^gvqjFQ;f*VQ%?E6KNR`yE zL{0immbgT?2d(U=sYPWY(Remxc2aetNIlop_vdU^e+Z)%8E-^3Y($HYgdF}ib#DV6 zRduxuPe{a|kx4WnYSgGx3ld|1fW$;)V4@S40Ifk2iwX`}R8$Jl(ux6vsN*<6TdlUG zmA3XNZEb6joi_+@%E{yk{^;}DbAa*t}WBj8Jd`hJl33P`3Wkg z=(Y5)`105LMAO)UQ#vq_!Dr;So}c-o;Hn}}Ahw{X6QikNjp;IgWfc)BC)?Oq8Vgd( z21R?Zk*c7vupnAr5kc@MY)iWqcj4dR+vj(vM4XP?6?p)>@mbE>T+`aV7kU?$w~*m> z;MAS1_=U?Vf`1OdXQ4R@cYcJC8EIzlSyaJJ{5B=4ic%%NN1UBl=g0I`a9ezyeBiB? zIk($!4Hac6a-)KZrCwwWu!Q7DS*evUM`O|rKOEA~2YJV?l%iC9c(IqVUw9u+?q;=6 z;9|kpNcJG2YQeAEkVw8Twhk>+dpVK@YZ^GP)9R+*8oK-)CPrn_rj=r+WLTu$)%XZv zq%jq{6_r!;5|~9OeFUZY0PPgk%f>Sm#T0$X$5@gXPWLS=jNcwjw{(6sHq~+~s$oF7 zFjlZG+WQ0yH-0-C;VF;p#7`OaVxU8eWYG;P4tLeU3u~e*2n7>M?P8uN5PB)PP|Ave zIW--*gWFfeq^n9}ga6hzc+22z(ZO5cCF!*3hzB~VlCo1X^Y-gyuVR(sc%#$B#=1{p za2wr#@DVn=)6F#)zURVpmw^L~cs*-DEqWC*N6FA%GW+x~ED9yh)vpzQADf=}OX$&V z2n1hI9rOY=tUQc_xV5yce1r2G;nI<`do&^F4EAeEeFnPEkru2Tj$l1|CFp@np{s1$ zmpDJqw_^ET?5XVJ^=kH1uu-k-wU&`fyS{56kdE2(u^mucguUd zrX(y3ABV%9+&c70+b?X+G+@OoB*jvooZDElVV=F}YC_--d!Jc@vLXL)VO8sx`bOld zwTHRfMR)ZPvv~@0xPu+4mUl@*M|K8#S!KsdH)EEoVr*dmH#CnwNAuLV1n(0YQ){C; z*2M}J`MRrnEZp@zvyQCMl+y$2mPosKCEk+4XR{#;FM5VFXbxSo2z6xzusE!objAJA z!iu{ghQ`#oF{z4Q%|C{w(Ay2IXDO?mfsSHh<;upowN6n{b7fU8bztSPt|NZPwa()* z4ZlP>E;GzGBwZKFjK^Zsic?@rm7F#mtH&ewo!`)uT*t1BZ?vYci}|ER$%BhLC|$z=(BVjbO$bR~ zt$4RS+kpYi+Qz|nTz0(mc8lb&llkZ2MK53`1D=?UnI55=zja>s!(AU^ngBY_ywv+` z@8QKKKphgvT@;Y_Og>vs_J^HxXr*+38z=>i+e%y;(wusEO_7^K$A;DZ(9>vPc|Yu1 zjloo1;+b*yw&O9#4c*WNr0CV5v9QSP+F?60LHqjkMMs4*>|rsX?aXqzZ>-LDy23Zc zV8MdR{h$IDE33kb%h%O6B$ zI&VZGs7U4wey5m4$c}C>5rJzyLD{i3?k1TZsHVyvVWYr1P?t2s>U>Eo73sgGh_ixt zP`4671(D1+*&qz_E`tjyP4|x$hEexqilyD*MVpv9cur!T`uDCZ+^2WBRC5Zua1Uay zevitZe=<6wtR62i--Vu-oI(R)O)ypNb2=^EP;qJ@^ttz0;dg4P;cx_LT;iNBux{ib zyqHZe8W5J)8?A22M2Cx3Pq!LRzQ~R&22%&bQp=$Y`xlm!vIUQ(*2d}{pFi?~o5VPn zI-3R7M{@=(8myo>SrM6-EWcw+s_JHZ+Bp*KEqN5x*zr{FO3g*rcbz-(Q6OUJ`rOE6 zDi(v=EGK}uEc<~VdXV=)%U7d1I#C$RHLbv>H~XdgVuD~2CG`L@1J_;xM|(N8XJCql zLdWJeKLBdP{;tMU$-XgmBI3#Z*w`2AMA52l$PR9_yHIs^HqY!qZ3mwvmsGeW`%kR#h9Z^?lsnPVYgP)ELsVap=(RO;cxSi7^R7NTfbqjluPD_7J*t z*8vi89e}}4qqGC@XwN}z2L$%FJ2mkp675}%26tCe@{?$4%EKYHxed?u7ICXdS;OZ& z_hGjIHEJyjkbEJAUoGd6)jax zC7)GH)=5AyA>iqLPnef7H}m)npseM9p2pILXGMD$TW&JaSp#+lmZ11O1CV!!vKD4V zfzfQXf-au?G>Wa7Jws@(GD*^N0xqEPu$=2bLOpV&%UU6tM?MOi=B-Bc1xr*ZP#M(> z-5w;C413%fyw*V4?4Cdu{4|FjP6X4qUZ+CH!lR1CDQ2cW-y5GOLuf#c)EK05(2L4HYS{t4_X`OIP{ z<@3*0g!B8P>|Naq9dAq*#0rZBU~Jf!!oaY)1MwjIs8KjoMZ$}(ac*OY^cwI%W3$FXT@P{-3f&YFs{jrK9io7tUGGE_(j!9Zb4n zwytw7P7o>g_03b<6clDaum$j`@WQFUH*Y|)B*#_-ahxj?- zbXnbt;l+plgCv+_rYZ%e(GhW?VQ)4|7qZk_ahqQg$9NfYmUAboouw=KAO=m9E2`tL z37yv4(+4od4rhW>L)ydCyzRKjPnD;tQcR7PZe9+VT~l;F{-SaZ>RpW-bzSP(D*x^# zmOi;>UXL%tEV9UyV*+E5Q~Y>x7$*Pj<{T=`h~*A1sv>dfInK@zps!%b#$}3}3rphz z;_PNQm*Y$*sJa-UZE}v(&3$6hyb;P9%@iHlyS8p+c=3Z^sN=uXiUgt+LFx|xL>sXl zF+Lu{?~^iY!z|xvbr?rsHfG)X;T2EpwdwKA8+%t`f(BuGCqGg50;Xs-c05Q8sDUN$ zDLjqnCx}jrNX+-Td2d`DiuvVgJb@3;pSuke{#N+9dr)JErphZw8)RxEoYgd z;l_7ho_@YH-1#27bI*ZW-S)Gu;C5)+4RAD%K>hG@KeZQrg?&CM3LOXfLO<+z@$VG6 zpJb4LLp^)F4J>(!45ZNC2XU(E`#rC|mHUD(_$Z z6V1Q=o(%ZQp5H3ZTiNKf{!WtTVW)c>+vWMzJa$1wuOn6_u$6hA&GgMn^bL3T>2iL_S$xwA->~Q|UXTK9; zhjCwzUq`~T+4$1;tl4wih9Eg5Qyi*0{G~4F+LU`lDTq@Ricbvek>L6%#V3ejS^5Pu zJRBDP053Te$NR7>pIY}m+tX9l%Kyi*N0oen_(igJlX#HN0tRNrVbiS_zhd!9N9}Ey z_w=}6TC#o4h-5a9Y4%$qwRz&0B8(|QoZ|SO$1!b{GBC~2F5%HAV@W7_W^-yy^o*v| z$I;1%W|~Mn&CiX@p|en||U zEheM82_r_{R2RJ?k@}e9=UMF4ce@AiUKGE%fC0HPHz>=7RA4ZZK%Bkw*DBE!?D`>| zMaS=PiI&04GC>Y1*5TLnR(YK)xdF3(PhzdiVq9`Apmcw97M(6M`2{oRuQ{tjKW#Z8 zw7}kc&^Gp1YeWEB9Ius43_OR>AVnemE$Pdl_pIKPsl&)STK|p8aM#7IAG~Ll%a~(k z&ogF7jj5P!Lbv!n_u#^Y(%yN~_$h~3>A9uRx`qKAr#EFP4ocVih!OQy7KJl^fnLXx zSU2Pc2JH8hGIz=LpM-p{F3hI=5wjU(5&%4PW{1%98{C5`)uf-2ho19R=0u(Q}M$jSY9+e<%A+R?|m{_ zx51|rUOXT4QgB}u=8US)+dU>d@79nn7T!@Ygvoj@*Jui{1%1pEiHwQhIj&}(>0KVH zTQ0F(mWh%@Gm{EjjZ?juFTCx$KAQeUIbIWVan3Fqc{CQ8f$5i31(O1E|H~>Us%EGn5Aa6BX~jNRB=pt%GE^_ z&!&zaXb3`|l#fLct=ld#oL>|30-!!oT+PYnqM5H1j7#EuMBV$o%8==XjeLjFu)brR zC(kKG=i?niG{(~+dH48i=Gz4mlZb6u_ly?+!l8Dv*c}+SL-UYd$!VI`4QFJ=*S)qs<8_%f~R5Kb^K%dBMQRAGwAZ5&H8kC~!ok5uBDU267DaY!b^MwLMtNXUlvGW_FsqLv}nTM13 z2wxr~|AncK_0i0%0^H+5l!kZsKppwkaYhAfp6z%_MCG#Hm^r-{v&Z6uM2bBxaz;IK zZb3jfF?%vD8{7>OBS;cWj0+Jgvh%V9Gdvir+bFy`p75$HL>hU_+h&fx7f)mA>Bc%_ zCKj0z&MZc^Nu5W+GjB?+W;?e9i9@>0nTl0$z68!z2=w~^Z@`=-gP*|HjoQk0#FGt- zRp&$r8)!Q~JOo~GxH%;j1|gaTK*wU7lLqYoc=E;G@cGTIA^pA3(g$UV%Dg^c66X@w zPc$>Rz$Y0?A0O?-Vjfq6vBK*5mdF%eK(L1bIy}617cMT4I7k{NX=5V!5~?gc4-K2E z%`tUQ!uF#xXB0>r!_-SY5qRW2iR8N=2FdS2SnHB}X^5-IF`1%+ld@c zr^nW(+OPgheEuF4CbFPhDqd|YSRNbVEJ?g2xFRweZnhqRKV^qVt!iyD5zIYa?Q<>9 zXexDU!d>NPQ&j<+{8?9&cN49{iNsfJY)IAJA|t2}+-2{xP8FP6QT8rpP!g{pH~!PT z-0_Rhnv?%P>@K!-wNGKnYeKU8H>mrMCuS<}?$WCa3Q#%(6m7L@uomH6mS>3bsJZ?O zUGvp=GvaF=jvi}h;iO#{tVdXlL%ZWw2gAQDgZ2>W- zZ+*+N3JZ0|Ap#bRwX|VnwC=-L$~i$efU<_iIv&N>+~x*e78MT2Sxn};y2Y?_L^8- zPtZ+t{I=%;a27M{xsLw9bL`m$arrhljp=#c3ubqLp1L`?i}4y=#V0aVcQ+-kg1bYL zg&JX3&Og_joE$nWjddJ8MJxb$B!#6nuIBNs&})GE8(#b^Je74rem;=Z%kFwKGq)Gr z)jnP-r>3h4o04jdn5uSx)S2k1sqzAe(uuy{V!ZU|I9sk0-Yq?ftRoedq!wvnrU8-R zeBU1d`VxE_F+N%T(?sfZcHEmX?dx|^id*ogdjaOu#p#AfiX#c$^^Lf=?dD`p6GE!u z9rus~meH|oq?#gnEgNpO+h|GIhBaW*;awscZTDF`tkqo}s~7Y4lx_uFB6%DW@fGeXDVEwc_(@-Vr>0}SgL?x< zkht8SQN8|m(zwQfivZAcPDr9HMyvW$wyIoiSLb}!FS0BBuXc9Se z92^w-ZaKSx-C}uwBhfJx$5N;ml*1Lz|Bdz{l=-;@jdfU7?ck2b)hqhwn(Pcv-*T+g zjje6M8;0H$BI?F8x{cWe*oiTm`G*^D_*~#EyO#_^(#a;#(R5;t`!LE}#R2h-&(wa# zYCT-4V`8!bujQe)j!`Zn(1bHLjL#IIk#;QbOxu)KcCw$4y*?aQtpf=S?5KZ)#mBjK zDexi?wqI)+za0*S{gnu#8xV<_N4gdrYghMd&)N99CZFv&*0ACIARh7UdUK{~6|2xUo4#3x zPpDdhS~!@Jq~CUMG}XTFGoJy`M6apZ!Z(Inu`m~vbV(>$@Se2uzF!GmXy(79agP?( zE)aQ$_SUWOkA%DSaRYxpIB!nA;2pM4a?Fs5o4xF!2>MgS)e%O;k<#GPG3YjhSb?G$q$GB{!p2zG`eH zwx1gp)V_kZ#J#Iyg$>s>)~yzmr>za8tqoOY@~f;fakA;TH_+rhizZh~4D%R8X~7}} z(F!<&&eV6hvcI7%F`KbLyoJc0qjAff)ac1Te^M0TFS<;haOMh5)6IXOi!(vqV6$N> zZ7x_hK2tI*jty6^MSQ0s`=tqIBAt)7_h)5o0IM~9kfgf*o}VJ7GDXgOXui-xpv+tm zkVWQN*L$(O^rb-#Fkl?!!k)WKH5IIzn5p`d*%_2by@9M`%f;3p5a%4l^@T$&OF7L} zte+2%rrDW4`T%_W>*oS7t><&?Zn6J26$68X4U4fy>vbxt!+Q?JiVm@io~IE+Q|Bj8 z8rJ;Sbj3BDo4gH#7tO*_h(MHUuit)31-sN920b3AJ=j~;xJ>OptYeeOn6c@WBZ}&t zn>(@Yx%LCSp>)beIWE7er_?ciMdvb71K3k&f0Zj~Yd;aQI*IHxpeUCpU=C>8eX0J8 z=Ik3fcAIDV{)^LI->|S^nA0(7tceFh2pkH=!r{xSKC9TNbO&}yAY`hmd*AROwE4D0 zBz3J{k(-h2xfrkbz2wNY25d>O#W8!bY5D7g_+E^AQJ`ZMPVZENd(zw7OWT&q+NaH| z{PwijmtJ`3H|JDdb^+s!R!%>E*7=uCKmXF?h3C(45lKhRoDPm(JQjH9fBsF@hh5JI zW$$0gFR>2nDs1du>EHAE(4+2~tatw}ev_4-VfAl$|2M;tRzk3}7I~H7#f-nBII`DrV$;g^&R0K1*lHMP z`U(%l3G+2f0sWQqokvv+zxq6%;V~NjzwG&lJkR&6!9$6<*XdKmlSN|dLH_J%nj z-GB0(G2d4*j;0=3F1}ABuGjj@Mu2#!JfKgqn8f`%-<&RayUxWoRr+e77q^hj0qu`` zD)Gxh=||$6c*LPO{6xVA8T@3y=@)n?pD{QmpBjhe@Y4iWJyZIG;G+!v*98~z$nvM1 zk^g8ze~#cWgU=8=Zt%+lXZk!eKUd(K{3kgyhhHUli@|yM|J|AZ)dERGe!lEo{vvm8 z@2vjl6Dh$}e-!T$T(5(_g&}I`p2zob8H{_X?b98=EpI;g>pb`$^WdB*{4zeP^57wT zDV_|?LD19o=@(A>Cc>W1K|E4?S$CETPdeQF4nyeE3laD9A_P6X7(q{GBIxO5jCsnC zr{7}4)5|Vnw9{`Rp6T$=h0_`96whBa{d|Tyb-_mUctp->A@8`kGSx_8WUZp=g4}QGCN4uvnNow$z;7Nlk|G5U&@_x_Y z5#jR_gR39=v%%GtRv29M=V^m$y}V*@rQc$3mH$V1aQAs2VIbdXa1EwZE%&toq6!!8m6Ee9x?RFf3?B2 zT(25j>HlT$7=?`EV}q++eP(dgpT5i-4qGqbJou12_{j!W`NRya<@&n8wOp6v!LK!V z%x63FV}q;y++%P}cU=Gv*895woToA2f3%xT%E?Fd{B3+?&F8`ZJ!PPJ^)rK4GLtxdV{n!8e;ZuoxzgYf&nC3l;L4}J48Bx8l|o-` zaOHE9!BuX94X$zF#^!N=yo&oy|Z(9bk@ zjo{xgcuep`23P(0p23y>?FLu+-x|C@_^&d!(!XYK&F7~ESGo1$033(PQ~8t{JnqRt z0}Ng%xQ&Oc@;}bdYkq3;;NuOh>3-eds<+b&uKAf^a4pwm23LLVFu2-Fr@_@OZZo*% z^Ct#ZyI5v$rT=e(tK7N`u6))QT;*`2Uu2%GN6r6GgO8H>Iz@2)aQ(O*u>da1p53jl z2l!Bysy|Z#_*Vk>Hv@PPaFz3S46gb7Z65pygKK$THn`UBW`nET-Zi+=e{66qSD_4@ z)h;Sg1|B2w;PnPq{!Ip_EO<;dxRz_G!ACo4ETc5I%I$W8mkRx_46gFO*Wk+kA%iRb z6$aOQKAQ*sr@;qFes&pL?P&iJPoQ$v{2XO)O?R-tRi4#?vpR{zJloI+LtiEM97C`8 z+yMPCpjCaCAHd09^K(l854LNY0yycF&s%x$e(wEI&JQd7zyKcXXI2Gp^4IzrZg9=# zXoIUhoN92@A4F$zht`YoIn&??PZsj;zjFDn68yqE^a~8G{C{L{<@0-kv&=lY1=o64 zKJOVk=E+0FoQTH3as}(Re*hbNcsU+g@2V%iF!*Rkh5ot0O9a0^5B^L558BJi0i5zz{_h)H z{fXPS13Jy8>O)^nzz1-Z&mjh{^<*I%|6c8`$CFQm8@-?5w8+z5BI)kg+ z#u~gv_)jpn%B|Jl5uvwn^p*cbdFZb)c!Th{#o)^S&jzm%`fUbR`u7d4^4w`~&1bK{ zCkg)pvB`yp)=Q1Q3>}&WA7pS%_jrSAeVt-(tuGtDzf#hjVCc2H=Nnwpz1ZODe=aw; z(qC_Il|z@omCr2(*K#d2_-M)he;Hiu?qP!$3H^%(S3YkTT#sRy-#B z4;P&E8?@_40B2rQKdTL{?S8$%HQl(uwZ0}8T;(~%;7Xq~c%|h3$9eF(46c0a_W?AY z4;gyZ!?gxi`t=4^`R^xtLD}7;-1Hb=aFyo~23Py~s=+n?Qw3*zeHH(;UAQ2CGu=x3 z=P}>ln*X;9e@`6>>0VaNzvlBZL$CJIUj(J}n$H6SxA{CIfCuwA!r(E04V38x!m08S(+@6drM6eAJ50N(>#|PXu>V7)L z&FKb3ZUrn?AaCF^*x9fZzoGP1mHsZ}hamVTb6b+EP-Rw^0@k&!&B=|8SoAwCS$=!8 zZl|n7=d#3-_1sZe@=C{J>8nciV0SjS@mu9?WhD!=1^ifatXW_*UcQ~zy{7s}0aq_?NI14+)`lko@jV$$N6ooTKqaTT_EiCgEaAVCduh@P4r|dSCh8t21 z`$#sk>(qVOPEA!@?UP41X6|diPL#gbmAtnv2D7h=%ey9X3)SSH{85BqP-4H-T5H_p zn4f!3*K$d|U!SSLHL+cC|5$x9BFejQVYrL$n^|$xochbLM7N|gSN%%0_#jYY>&!v2 zI{{JQ2Vg6*GqK)hqS!1u0*@mA;+rMh(G(}N=@N-$&MZJ=-TTwc<=l|X-SAm`Nkm$)`|^j; zv~0MJrss0=@u%~zz+zHY;Gll_%xEsTS-RoJnh`F9ow8Y$>?4%v0@<&a-E@yVQ}N`> z*jcy{8#qw9vWoR0kaYX?MNTLkxA{WI>RXrly6rxN`NxVPU_~)j#bYaII$HOE?!thu zYS)u#`_rM0m%M%i?BwH)YRwKzgd%QEzS5W(2UA1g>)w-UYZ4pic)oW{{Z*BIwLQLE zfThc^^yLA=YecZsu3N!I_87_zZQ381>-tXeaSx34>O3q2l}aM|&Tuzq~~BscSY=KBtbkv!Ng7&iO#4 zD&Xsw7G}O(V##M*ya75fs&`YeC?)laY+z@8sD5%*Q5yDE`JC%m_$^v>pJG#Pq1Qm4 z#QZAyJ6D%Y$!DUyk0GvhntQVnPvav6pT|5EH&>LbLe)(U)g6eWVH&R#Q*v$zHl?u+ z;QOcI8w$PgTv532-;sNj!M7F`&4ZBed;OVs55yf2tdK-AUis|Uyr355t7&GP$Kyr;-i1Wy-Z_vBJ%T@ zt6+F-MXz9yxw{`;+=$i;`!~m6A$zNX^p>v!pU=AY=GSMp`d}OL8mQHStdyC%Q7Jp5 zmBL zV82Ki_KVQ^?H|#89d`s!J<{iy`p72T%M*4ItuMbWyl4o>AO|UAc1NDy)r}j?b0Gr6 zW^3z|?fTdPDC)uueV$Q{>Za zyh;6oEkq^Mja9pqx4v}u$RONtw_GO-J2mbjAp7pnQ)wz#8!yCGk_nlL?c7*W8BhJo zZ5?sjxc1nO;U$G2^VmXyc%@*orU2=~U%2X^T~G2fkc>dysPL_ro3IDt8P~8p8@_Qf zBF1@1*S#NJ)CeZz=(($Yiyi@YvDHPtcCik9lo41Iu>Wkdd56q&e9nl@S<*?^6_6?ls)VseqpECbcM(5QX7EV;LbnQZQ z?#;>9#Al67H{3Bc)o`=5_WOOJIgULyaAUMfh7yTFq_NCsZ1va38sWvqQOsVk@ZFj? zqA{S{OHpobuj%q@>nDf2WW!yLf|xm^OH3r6r#xI2D%FFndTSUP20c~mvW2j2;+8;o zjHh?1q=BIbpCk+`5Wx`$_vqEB?ThLdynX`!-W zFHPOlQ}jk)(fOOAB`>fVTr%by;pY%Sa&))nXHUqRv8YS(NJE0~eV5F(nU|~%K zpF1O?^->gBQ)PhN+=eLHT_x{1TlJ%aHGbch6E;lK2~s*5jh)Z?jZtgCQZ`0;r`|Kf z?f&C%BKZWfwiFZy(fw4mhTtG?CZ zqU~^THSY#Fc{$ZfZjg=wvge!GzoFl9i?eO)$~qz~VIbi52YNNer!#(_l{&V*aMYco+Zu>!Y-~1wPXb59avDWl#T($NbiQSZ1tEeW|V!;ma zRfy7J*x@Iz!>i_ouRRk!hSuW@iPqy{(}a3X3HY9lqvPlaWgZ?46AwmaaK%yuMg#_L zT(B5?_@rY*ZW}2}#Lb122jVpMu1+MY{+o%G@2dOAhv{G=Uj9x8#^bAI1mZ9Fj(yLC z2mmb}f<@W_J{Vx#gOD{ka$3~uNL(ptuTHxp+VB|kKv7+Ld&SseqhDWc^!qwC`VACC zaWP|^EH3|J&o996>AJUPoB~=8gsw`AqbV{V4CU(Opin;k+AV9Gfb?Mk?GViU7uGpBEXn$-cz+?Wu;V zd+MBn@MT|9R15~8o?>j84E-PO?s2ul1|yi@obO!HKJ7gGw$JvzGxTis`7@_gx1T?^Jybn?TKlw6 z^?7sV?73<0{~vq!RF0G>kJ0XL?oYlwe8s$hquY5vA2XApcB+71*9760^ZvvO@jrNs zl6MI~jOU!U4IZ2mB+q=$PYn_aZ4erY3#LCT2;yARj~HC@uWB7;JDxv(^=b4PHdy*E3Ko`k#`eUExwUr%dbr0kQ zVa`|aN8!IMKbNF^S^AZ-fb31&4<0uC;Jx5E>Hd>@_%LY)z0K9UmW!`ii0iffvN*(5 z9~I|12I8s@$`Rht;p&qWA1Zhyet9Tfhja3YI5dZ|6(v5%;8DT#oq+Ob6kOCd%fCr* z)hDH&AowW!@=*MAoHJdvD?AiG3+Kc~6U3qTG@KKUIW&h~BzWB5vjk5VJl|fu1w!9q z=s6w7bXyI+Sa8}m56vgf|2Ovb-NuB!#8WI0T=mIa%AYUW!}l}X3-005_USI*s+&O` zf6jw<=fR)IgTIgm-;@XUd;9ouDH+1PzDuyb4?ozihpm3t+c*3CZ@b-jetX|gZ13ZC zzhT(ocOka;U4$)u7k^=kpWC=M%x~ix9=e!3xX+J!{zimmpI1F}WOmQr2_d)VZ&b+b zA;dO6$9z}~xBIz2Ljybg0vr7p;@I!z!&z>JpF8{FE7+RvNrguD9mFdkwC94xr=2q4}wFS7_h!;GD_eVd*F4!OzQsUt)00 z&ou_Gbg6|xe=@lGpH~d7{>|aG;gHQ6wh0Zm2 zrQlZ>T+4g2!PU<%F?fUU`JKTB3jP;^M+9GGaIKdY46gL=7+mH2fx$JOMbh~5HuYKQ zOAW642N+!QQ)zIew|fUGrCehTz2cJ%u6({3z=LaP=LB%(U+KSNa4lCR55C0UDxY5& zT>1ar;F|v@46gK>46ge2y1`Yi-Zr@EkKJpi{C669#Y>93z_orAKhoeTpRXER%Qe#A z%IEYv_zZ(Hr*L3aPLB*Hnp?@Y1-p|b+e9@nj=fNut9`|Al9c%DX!ABZg z%hhOb&Hp6^SNU`pT>ZoK23P(+H@NEUGJ}tneE!McO8-nA{I3Sre7<9FEmwhC$&r(@ zBpd2)aBWY^4X*hN?oDKsYCfkM`Wi{#QiE$hbDqIlh5lB9tN#BY5B_U|Yd-&Ea81|l zanyW18K4jPu}uM-y^mr(p`nIe@e>TL@;u4l zDo?vtQt@OS`dJ3od@eM&>cb5N*Ye(!2fxGMYG1!JxZ2kr46f-ukOzM%5B|Er6C(d@ z2Co$S6N8rv-oMzNXg#VO4KTRY*AW3csBb3)aMCLMc!Mkd$$9WOdGG}WSHE$;;JW5f ziWTZMcwBBgZg9B_J#TQemwh;)fkXMLoWp`sZV~*~avd1JDFaQn+Tfc1I)hi@86HuC zt2~ni*LqCn!SBd}-(zqs*Ix{-{699h=D!~oG2>AEp)7a|G`P|qVQ{6dGI*uq5(=Gc zaLs3v!PV|28eHvevca`pW*S`cd5ys}pEu{hZ!@^^`I*7BT)#4Ssg(E62G{aFWpK^U z#yt2x4PGVuKQVZX;Ehyp9K2nG|B8<@xRz_8;H>wr;J@m>-OH%?yd*#$^c$G~&UBT} zZF%sY8eH?a-r&mTuK~O=!2e$Xoayo&j~xcr{O?ub@2j4x{+uT`A0r<s5pggtQgmo9g^1u3wUhRjS9gMCM-6 z%C*c`6W=#ph;(IjPprOl{weX)8d==koLq^;p%an`MC%`euUX|j{dC0mO!>3m&F%wN zeI9}jk-Ul|4(V7GLumL$d^1LQhPy^_0mZ^qg8~asm{Y7^^9vYj3$U)`ELpkzX^6uIL_Pdl!fJ}kLa1vGvv#AtNG=xz#V{YEIVP;~C<+(nZaw^-!55O-ZcQ89@m&C2POu zS41ZuKV;#0)-l({ZS@4>lMO$TRl<##bNhPQiOHhh$a0_N_YOZc1Vkfl~>&j|E9LM>7+P{TdvJ5zolx+xcPWL~{Cl z_%jpV8(;qSLVT~P6D!RSqMIKA8<$Mnip5P>2$p;WOSInP_xl)K8>>?Jk+2Q;>h+ch znaI`&nW~)=G7Z}kncB7E{FgRLB2CNx2BMPXTttS&ak0quICYqkSm$_Vim(Db0r>z~ z8`rgR+!KK$xPq#rn2#0n`w~y$x{cS^oq<#Nsu-|+@VPI>y2oNlE{t>!pl01#soaA# z_-upwaFy$fkk`Jjo9`CGJX`VElh%`AH78T)u^iKXg$p!(*%)?b>;i2AZqIWcb8@W7 zVl0T;W|x9TTSJ7Lwqv#O-r%+a|A&Gj^CjHW9q0~{@R=WgI1GbstL-}h-;V&fQy;KA zy_DQ%4$3GwQj}P&WGg%!fYZ*}jSNkme2fXVud@54vkIQT_ z7xQx*ZR0Aje!cAkrZPRkgj=5`aFt8&DK2n-mVfwB4u}{_-6^5tsOV&!J;|oXit=`a zjR|cf19<5?A)@{tfeWd*aN9+LaMgCPOTIrCBYBp%^`AUHn^J9kho@({O4`QjfACKa z{~U`yWyfksnTE4q0Isl9lXa-LZIo}-;ebW9;*`T>KI0-eDh-ywx1CqRERDst(x873 zh+ABqh;%olo@An1;+gR^aeT6JD{o#tkltzos`_nSo{4XQwf>!F6N=Nd199=jkoL%% z1P0^c^`-)RnjjYGrWQejt#9E@>(NaM*9>f0xVjP-Wk+Yu`LsFpB(nXtriCkuo8YVC z3s;O{5h8mxXU-_?+?4QsC*c?S$rtSsevx6q+kD(xdEApgt6a_a=WC(__>Rp*YgOj-u4b9lXh0FKGb}%+H&P~B@qFpJu*XOn|GrkDl*V_vp z=80y;_Vpy=lePEy95>_3Sgr(3$p$nGNS2N9x@_xmm$Vy=nTz+9+Y^%|4{5uSX~?)I zp!sZ+9}sFZ^Y==r)3p&H#ug}~;4H}BJ2gH1BqTOP+L1)MA;PwVEl6|n)fkrh-iw~f zYxu(6YvVH&@1l9>#Ga!qg%dN89cXYeSQ%W4=Hhh#RiC>rCbCJuXG8r5idn3Ds`(^2&vxnlLi)g6Z@V^>&s-bvUH{v7>2LHke<}y&uJP98fX(#}hV@;*U z@_%S5nul=P5S)3J{SnVjz@IYy$=aHlhPNv^?j7`=?R(Kk%3NYNY5Tfs5|X7*;O!*5N%~V;?dT-Olr7*#F8(r24*CJpJ%{uzL8_m4k5pL0p}t(<2}! z>}l4>Ef6OwI>86%)^{S4JK%k5=zUQQId|#`6-`(58!|I;Z_c4+&fb;%K1){}d@G9N z(^EOi*0Q)?IiB!-gSFrUjx?7f(pf!l(n$CnN2m+$ES>b+#z%_gQ27oQ5^@~y(WN@# zI>ELCrr361ESl~N0xcr0yMP1``C-%SbiWcxG{U^FvFXQ|Z z`c(XmqCag~xI+BvO4Pz?_*d9p*{H0abhaSwGTUakGOE124a?TLaMv1a-M|=SU|@_Q z``e(MlM=~YaF_6%ttgu3oi2*>uJBDAwnsdmwKxS`{+7rR@mH|XTd7@iA!xxyaDVdf zT-9;qd!*d?k;cLAHRE%b_*57C)mG@hTb-M<>Y)hJ50q~VqLi)};jRg;1$j=|NBE*8 zZc{um=oH0ysxg_^VIl6%y&W66uSInZWwV9GNZO(CnTBVmmQ=)!`=mYU`4FV z7?*5BvnN)R;P~V_*tS^|S-7H)ilFGvqU`L-dJ#CSnUIY903EPSiorJtv|MHtTk2g= zPE98c#HVKyzeIn3W~8-g`J3?F8@ai3=3ek^73k}?d?l3NcMClOda140+0a(imu|z= z2kmqS>=Cg7oIT-d*6aYvM`qm<0lD+pV`78ZWAu;bENBP6jO19Ny9C3Sf)O=L#mST> z&a}i{GDYYoI9fM4h~M73x;fdfkgAPV1*%~%ckF5ZUI)m!X2Dq&#pLCW=31>Pk$e)ZF`Dfw3m2}yw;}O0BlOaJJy7u2)FXAlD$@F?#ZFkq zQwEFJ;`fMXKSX;gtA`)z6`~2rnfIYirra7jH9Z9_tgo2AyWtn5fEvT(0KSyH74Elt zLT2U&R1n8naL>{mq|CWxtgWOB6$noYS?|YWh=o~Td>j%b#$PaWy5b*U=Y4pN8Y1jR zljmcn!PwWZ5o+ajn$|v~!p5fQ@R~?%Ia{ux17a!kR$WRvUXV@|>0An93o9COdXTI2 z0?aAOM^5*sb|*1B?VB(0pzwEJRZoXo-hbaN4$f(U(*opoGK`O(6Kp#_L@(t$7_Qi| zwXUacv<3hE8)U>!WgcIbIk z8OHNN$7QOH#89;#H&*j2w{2Aiz)?ASb6p!W7S(`#|H#>8j&gXG63-1nF{Q(@n*w!> zTA6R!96Nr-`g3}ehNF>sR@jawtM5d%H)L{gDSXU8X;H-0Q0aFvds9i$`VQ*VsVMda zJDnSou65n8!8yePUD;s&?QFWDePe?^C*<;yIp_17US5bEy?7%u)5x2vEc`mfWPf3* zwAG@1nIeq&)JeJgqY)Mzm6jjf^sAZOVf=${2g2v}uIBkEXbN0kh8O6Cw(fAF!Axv7 z`V_4n_^}M+09OTf24AJM=iPF)@pXCgzAfB!GPU*Rc;%oT&1vhCZl((vbFy|V%D4TD z*NVLo$&Z_{fzfk;KZC|DR*n5;Q8aTJ1|sjMzLwnP71bNO*Iy@f4Us4l!G1{3}9Ib#kKoZ(R9m>8l@q)YT;3#mwb zYCmkZD>)|k1Ulh(4x>cDYmNmT z!yHGr^4BnY!A(g0+vumfBu>17v1{2ueb)pYcvfGyAOxt3@SGG)67}Bb9OWI zWBPMP9m7c8%^bMOMtXbra_!2hA9tNx%pRN=_fM1F$&tIzeQAMcKHD1EgtZ+h{ek9W z#kF{#?FbHO-0XpK{^-}j&0{Kk4T(!4yjsET!T3zk23{;@!@&6iwsUMKC>2D0_^ULf zxGclS(j}jrCUAL>RwCPtEvB{Aex%mM?hD!mSli}o+YrrM1xss0+t4?W>}^VJ&siEe z=V`UHW(@hyhowD73AmI!-FQR>^7h`G{7|O?8XgquYRp_E)5^`sBJ3IVnUr2Ijf^wn zOLMaT*h}n;E<5Ot;2~FS9GL6PG_*I7oPyE1pDd;^a)AFbRQe@+T=zA6>N7XCN4r9c z1htIq@#WdaoQb+t=u1cOho0a7r2Rg|@ov^q^`@;a>xzHMU`JwI*+(#r z&D5UAn)39h={~T#Gu&u(9W1StpR_C+fG3?-z^@Yyo2(Z>h?hF(O5u?*r*1&5aP~RQ z*(Vx4Ouup-R(k8K73H&sos!>2GIm6Zx(^t7I+W`QXQbbykq)Qrf5R!-4)AK_bS)+d zP(a@eC4W~l$fv7){JLujiE{}u#^hYVejzx+qk!d7y1v$5FRPKg+I~~ zzX{)f*7UJKO!RG90NjG>8T}p3f>QvkM^e^DGR3#lT_C;bUx9ms2cVB!8W$hyjU(G2 zc^M){F3)IygRbK9%sHRsTt(ma=_-5~Q7l&}ET5B_n|A!-z}Zm;2F)%NcEqN_byVx< z8B7ji1Hdx7zHZxaaW`(>(Q6Qj*oOC7@}4hQbR7%sK16*KUj{la;^xH<=8dQD#a-3y zf*kP9Oy+uK3AF@J0l#)SEYN zOgRir0WbP+_Z8l);8dWiNn{{#7@vBA87MhCT_F+TxJ&>d%vV}eucUf=anxR z9K-?4-Jyh>rSNJWT(!PHdDh|OK5zK~0e#NTP~#TewY;++)6u*Pyt6=XH!yd;!@shi z+K~qDD;{(1A3m5J5C5LRI}pyPtweyWH}Gtkgcu8V9pomQoVdiFWcwnnrmbUfnH9h7 z#^{K_5RpN4)(tG?$UC(zHh7DBX>lR8uB2A?M*c~oa$ezYU-SxB!Yho0R~QekfOi(~ z3O(e%EHifRFS>@)bFKjf?=p&)+F#-tzR@?HsmL&0nsLreV6-bf3NOc9>ER~8$`vo? z9{%8Y3*MM+sAx*Q3`2Sm#VEhmy_E26lwuFj*2TUYHa=Ot$jyxf=z!fw`!;m9xYytT zI=K&W4icRP(ZriG(_e%Vw?szgdXlB(`_P2lORsWg!d`QsUM0hLhp%4SN}L6|;!^R7 zPUIcen=Lw-THypq!)xdR)B21i*N_`s0I%H#_=m*Ni_)F}@T&cRzeBu=27DX^PCn&_ z0B2y$w(`T-3CA>m?+G=gs}9e#-i?EIqhGp*p^o63ch)$>ND{hm##tX^;))XY?O33> zceka1ClhC#6u=QQ@i@&8*~!YmXBR`Ycn)*YZcg&`VCyFQ!Hl1bv@!a;jp#-)9O~+U zruSD84Mt1Oi`RJXXim%WYBOelFf_oR2^X&hhXVs?l1;E#zHDf$V@IkC6A@x$E`wr# z>a+bl9fV3#>2)UD^(0#Mz+5*>joDj94>y{PFrD6Zn4Y*p2+-BIHvoi_%47{C9XS!p zi0X4y1euja`ZW%Iqbd1LW9GbqrcBE}>3_GtyXurS#2V?Jgxa(r)VKAjFJ~D<1 zkeZ{{MaEu3#KJ_f;dYHvh`}#K*I4&=yujlm3_2ZC#J@~K$rZIowx zvT8A=#3boh=G;CIq_1`N9*TZ`nQKYBq|i@TOcy^IW&WKn^ILEZPV9WLVQ_Zn(r`U3 zwGr|_#L!VWd9eTbIcvG0x^eK|uttVe=hYH(+*SQBSqB+Sc@YEj_T3>TB&k&=2FWV9 zyz3^toy|H$40qL`8a2`yBt&uftVw&6ot}2|kRLl|qaj4B70L<`m2oy&Ld;gW2}-<5 z;}{e&6-| z(Sml;sxYVMvWW?l`@B&9Ua0b}c$Aj6Y!7#OYC5uiJX!IF3FzO&m%oLnMEJJks)?Dz zYRsf0HpAf1_9L*`Zk(YJ%v!>Jj=Oi@2wfFy`5E(aM5uA_>y4Rd1@vyu=30N&R?q{2 zHg=rr-nb!rQ8la3yFk3cX?NX=9nROW+u^D$-1QSTr!1=6g1Uja9LW6e#5s*@?Kq<5 zB-?FYe+fVPfXUYHIn76nF;AFDfa^F$=8?Gw;W6~*UET8b5#Bfa9k06A;=1((d0~Qw zS`UymTP+6#I3V0Lh4Td;VlLTry4luZe1BVD9am;N8ryFaX2upZCzm%RKVXk&8MEmU z4Yr3))Y2Z}{r+{?7Si272l{?rrFh$r@paBt)~NyBSbU!?8m8~kv>{Y?eRC$prt=-0 zRdn{BnfFihOHbnCjMbIU3%KyKNP0guH1WwpT+b z&1tur@g0EDh&kd;Z87hB#5=(?OSlsm$UATGPULVtBUOl|3ZaTm&e?lx@ZV#Z3-)ND zzK5t^+UTefhzRLu<6mg49B-L|YD!|VRogFDLJk6xwLfOLIH}Nfyz@}7%@?Tvu;j0@ zp>PXQ+{n`{SHTQZrigP0&$0EuTZL>p;p7SNV8x<4l_oY#61xQlIDe*v`1~>5b3pcn%tgVVOx-M0?gVO1Hq4zZ}vi;O_ z#VI)j0j(dwu*2kPn%pr+LG!@IAm@ibDWeup?nkvJS1en=4L{6bAIhn9xa&AfvP)Z_ z4wr`N?utJNOkq0d(VC%rj-)C1F3DXg+Pgu4P9eCLOBx>LYzW0s;Pv5e>2eNW`9>rE zt-IK&u;&5~)sMiZ(y3`H;1yoqFE2*#=4D>&^wT}m)z(pHXix~eyZ1@^AE^g%Zlajt zyTV;;(tA!qB)+EPXEAAjN*;A9ew;jGnK5u%;|enuK_Ry{B{wFLFJ{I-+}`i{B(NG* zCktYkv-a}351qVUT6B23+$i7J95DH*K7wi; zP3OqEE7=$ABDr~onC;-ZZiLqgDw5l^kASH7UZW@*@IwIaxXjj}2ad zAgGP$DL2AwR%$O7D&*GbN7D(ovx2SOdg@`j-X+byqA9gP<~O90z@-<#w#W>|0}IQbAzt0jNr`cq82AYXPC@2O!hYI^>nFzID{HQdYif zdzket$AUb+djCKETP`X5yO@rTarOzvnEs`?n2wpE#{PqD+$+|<=Gwlo{-YSvajK*8 zF(IeOIe+@d#wq{&m_6e`GDIYvkM*CVTjfLCL$oCDdsQS{&+Rv{>K+NOjv`>@Tx1>bQ@PxP*L>yP?pXCSAQ@)8gJK z^0dz{ljm*^ znm%LjNq7gL^gp8l`^kbgOm;#S;ikn%ug}wdgNLOp!%bWFq@OA2&j~(^bNf86ZQb#a zr6=%jD0H_Jj$2A6;J+} z;E9*W1B&yT&BSF(V}K*&;#y^S!v7p0^r}CaPDJoZPnJ!8u;7CXUM+Zy!CCFh&nScQ zn)qmgvvWc`Zg6kcMIqW356x#2&Pm_u&>a4C!KWJh8-mX;_;kT%8T=B#=NepNQ7$m} ze4)SI;Jju&Z!-AJg5NH<)+4{;Lwapv75|ywOAY<61;5kae-wO~!T%ySduu$D|8kr& z-Mbx{!ygy?UV}d;_yY#tAoxQD-z<2y!QT*kmBIfZ_*#SWvhWM7WGM76p|6swjJy84z!AlMO zAi?>e10G60RPaiJpC~w=3(}7jyvE=O!ABW(=oSCA z;H?IqFZdLLUnlrfgD)0*y1{P~e1^gA5PYV=eC@S6U+=pwr0u6ee>J^#gaVYA?xKYi*A z!Alhg{C-(X+P6U;j7h8Gl=lQ*=lJEuAD;@orNT4TaYZ58ZSt8_<8d7~gmg20GIYhX znTRBNA;QRBgfOxU@;W>;hv8%AFm~*m;Rqf(rxwv;=Zrx3*f}Gs>q2u*s2*Aqnlq|; z=uj687SUo6fA-S$p~I?&BKj;&hBJyRPim`&j&LUL;qIx(=HnMs+igrTvaFlISb*+1N zC`90kH>|qW7jJlVtuJ0}b*(Sni0WEjyph$l2$hTColsrtiw8cVm^-j>rGqeC>4sO2 zaHXrQ9^pziqI!fY-N@<@u5>3Xs2N9j^>BSiwK+m#46gUu z46gktw(mTkcA;_jk9RNb$PmOqoIDi&4M7}=tKG1j;MO#i2P|`G0P3&Hr&~ zKe$g!^FK}SeFWG1To9n2hbDNaAkL-Vic>iEv4OTVY|lW?Nx{w;u$ zxAr6YkTDMBulZn7(C+HhORQW^650V^2r!n z`TWY@Rl@&ugDd|YgDd~{3?30ay#`nQ{iSJF`P2&i*9@-do@H=N*MB1?CudFfTZUfK zU0`rcH)HSy$%G?CTJQA+pDBFC7<{_mO$OI`pJZ^Y_p=Rtukh&8vx`!g>GaO$Dz+b)Bb3!gG}xNxW(6dz*n3Qraq zXK&Fc1E1gJ+9DBS2^Ef@G42y##o&x_!>iBEBFS3w+p_-;0=PmXYhG~e`0Wze+gd<;ZS{8_4RXuuM++b8~h=`*9Y+Nz;rygC4d(J zSNeY#{9fVnsljg%+{Sk87W|YzOjGjL{3i^q^eqN27e3PsuI1I3uUd~v|D!zge=xY_ z=f4fEa(Fg?2j%v+03MXvKMj7b~03NLOivxJD-oIn;dxg(JgWn=}m%)|(R)cH3+nBFP|0hGgMEHMf@ackM zL$f-j=@#_*BHD-=+g#|2!5-;&F3aVujP8v;F|w`8eHXktSrn>{)&$xaKEra4qjS27gH8mdb}g%)brm2@FLLOBJ}qeT=_g`aMjz#46gJ~8+?iI-(Ya1-)8WoLVwf&{zT=h?d<6W zFBkfY^59n)yh7-IZgAyupTU*>5rapB&jy35KK#|-EkfUG@F{}#mkelmy9KW{c$MIF z23Pt$;1S{Tu)&qjYJ)4Erwy)rwisOXx#FNb%d7f4$l%i@KgSzHTXl4&tK-j z|77rcg?>!{5BjU819%Z=ZxQ;J4X%8)82omj-)3;7|H$A=gg$hzKT$a={Xl~+75bwM zuJp$m{7#`CVQ{4%ZE&5p{E5Nm37=I4FBg1M9{fXtR|x%q^1%)*ukty};7VU>@QCm^ z+u+LQe1j{WOAM}jIt{LN^svE||7wFP|ECSE{5Kg~`Txt{wNkF}vi@G>qxO|Fc#F_q zWbk&uzh&_0g3mSh6v3}Hc!S_K7`$8XC3)~)8+?_}-(zr<=Ys}Ud9E?ImiHNhYyMw0 zxXS-EgR30gHn`Ty4uh-w^&>D^FSSyx{UyVStNaf$c!SU%Yw&r3r}E&p8eI7=F}UXQ z7Y5hz{>I=_B;7w6T;=?L!L=SAH~786XCJqzCf7gI^>jxC@C0aeUal^H7XerLGYp=P zbsl zHweDM;4#79F!)oVhr12FUho6Cz#WH{_f>xxsy6tWf}dvacLbkm@DBy=F!*PJ-)8W3 z$^S15UL^Rv23PuR2G{a_U~nz(E`w`%izTQFo9=>bvh{*pVgR7o@X7F;MFFZ2j4wa|c(E$dZ zDfFWR-xp8u{rf2YcxYlC`fnLLA$+bhxbp8bxcbAJ46gJ)GEAH8`onh&uJj)me0qWB-~TB8YwP7GgKIsGG`QCLID@O6oMZ4R zN%tays~%nvz!S)twu9*a&UR4s`IiP)dw^%5PgKN3k4X)+7(%@QO*BHD+(!IgpDuJ$ z_@lv9o@)%Qda};oD(6iGSN(a@;41%546gb!EwX3*Dn7&D%KtKhYd)_qxR&=SgR49j z8C>ftZE&rx?-^X>ztZ3{rGB3b;E8}8Jr}@>fG320o59-!-~TIn%185agu#{ms|HuS zJ;C6b|Cqs*{w#wleTTs{KR+_K%HdZ5JSc~U0(ejk8w{>|-ZHqh7rP9uaya;yJ>{n5 zI>F$Y|C0@_`p{@_<=$;F_N+4X%8yF}UV4WpK^s9R}BQyA7`NYYeXZ z-!r(%zn?6^>qDKze{C1S0X$fbhZtP>R2f|BvEJZHKgQrKVi(^rxaNO>!L_{C8C?Cq z%?585{`cg;mm6H`@l}IsJ$__x_49oO?WvzyU!?|D{s$Xe`BxfTf4q89@MvO23J1s8(j5mx51UZXz-r( z(jxlM-{4Arq`_N-K4x&0!(@Y(2>oRSS9xA(aBbJR46b^z%;2ggD-5pn{;I*>^u-Tt zGkCk?Gj!~p@=<(0gKPaBXmHKvVFuTH4mP;Tr^eta&rt@~a-CvumFGBvYk9wBaFz2< z^WeWUxbnZ-;F{0-4X)*S#NaBoEe2P8D;ScKf6y-W4&Xt%*x%sFXQ08=E{-y|(hoLx zi|9kG!Il0*gR5P9&EQHu#o$wf{{n-T2%a{$%K0vXYrXu*;99?{3|=MtHyd2__G5#q z-j-D5>V@)e7d}TBT=nxrgDZW);HsbJ8~h!gTxhnzRe!!~@TY`+p}{r(HyB*ye6zt- z&Ob7^mg~O^uJXUf;99P~7+mXRjltiM{I55-*4J^zc>?xND1XIk4X*s_4X*hdV{k2R zlfhL!lMJr%pKb7&q7OF*@C5Ry{ixdlIP;$n`kxzI`P^l2wcq;;G`P~g zY;d*THw>=yTMe%E`>DZ|ey?g25{K58&Yui4c!}imWP@wHG#Xs%@dATuy?@K#Rl@&9 zgVzea)ZkC~Y={1A@OGhp#^7pSZy8+acNkplD15w6NadsUa*V;%E>1T1hmy}`gKPfJ zFu3OPY=f&lPdB*M%f$xQdTBSfmg`D`Yke&;xR&=OgKItBVQ`hhZw;>Xd%wX|J}V8b z^**R(Px&i;yup<}wuZYy>r3-_lEJmSF@vkz#v5Ga{B?t8>&4(RMGxBzzCiF_8+?)A zj~M($!Cy4E#(Vgy!RHD6KMbz)?;Bj{3x@6~H>E$+;7WhI!8QLU8C>blF}Rk?r|QF; zu+ARb&m+ZhgT*2KKz|RE8hn%hvRoph^iyPgWTm0sN7h%#up>)9R~U$RvUo}|qVw*` z|5p*uc(mbvoy>#84gQ*p=O-C_nPjBZ;7?1xbE?756+b@1;0;H5pwi%S8JcSFbu0*u zrS5M|{u)1HgTXa^#swE%I(^oKSInut?D$F^Lx8r!M`AMvXiqUmHp8@t^`IqbaZGx#2|^DiRg`scY=YBEy=(N z(K9rds(kvTDfQuGjR=X@jf;z+J*P7+@znRj=Paf!#qn{$^{|!-=;#)QG8R? z2a-&eF-qqw2Bo|PeSkn9%k%+pf5lwI*EG7!No>hAg}jQT%W@Ra-X|J6pO;9&;jUxA zEM4#j1<<>8;m(5a#OAm<^XLoqf`(O~iJ-&QPpJ*CiBuNx>`|zxVHNDUHvsfCR zU~~%XpQ?Ayh8N%Eo{XkzuZ<^P@}z5uzE8d}9}lxBxdL~ujuzG~s2@RmUU9E`cH5tE zi?QZ&5!(^pmp)1>xh}bGUZD;cM;#7kqW5liV89_^zhpES6dq zEi5X{%Dx%4t05?on7fA2{UcQ~(zP`fO-vSnOE3Ue_Cyf)wzP*IJW<=A&IzMi2_nGZoQGaHM zf23ok%isL7_!WS;3r7{SPeMs01hRyNO*5o%+Xl+RRi_V2jmo-%ijK;1Z}TZ2cYAg* z3bptK1k7LK%uqv^M$cX8lTRd{M-1^>IzRS>3wNCk6H5=Jln%gIs;r0Ki$}dUFO78E zZ}{3}DAWH#+`GU>RbBhz6EZ^7lnFX&v{7?)tU*#G5H(|>X2=XV0~17n3N0#nz(Pfh z5ESbqfJrFF!vtDUX~mXa?X9=9t+m?vK;@wXe4(~JY4KTgM6Hi2qE+Yj{qDWjWY{vl zd++D}|J^$u$T{bG_E~%Fwbx#I?Ke=B3r6J<-huk~1aN^9F3onhnyzS3wjJN;m@{Ka zH1pXMiMmoI%N_T9e0)81xBj_=7ye^PtAb$yXM@%A?6L}F znp?c^^^SWYxxyCh!)l-F4|1=kz+%PdqU@27e!@Qx3KLq9Mp6e7$E}tkMNT6V<1*pv_yv@4SnsX{ulV%`;_@Ol)MaT_3FV0rp;#?-sM>t77xV3~w<`U+L@C9O9DJZH1mQJ!G~f%i|> z*TCYR{FuLcP7fQv-h;Ltc$Im_%W9>ZY7f}SLo3G(tbKvlABE1d@@LRnVF+5@l$@x zBl%wNHeg1?xEuI2+Zk$Jk=MfCIe4%!zL)P=ZVG>P*kO2S%bwvoO#BxXevEtjdp_H; z<0sYo@h_@-L%baKN8!Jn{(XV%E&kc6mIwsy$NR#c9e?4w!h44Mf4PS$)1&rUG?9u9 zaLEyINvB*n6_>badgR|zCd5S_B>XoFUX3e%LZ2=^!$%Z4fU_%tc#VaRGC0c_e?sRZ z+%vqiT?9YH;G#Ezk1==yuKWo;9`~fvq|gEUOoKOD_}K=JTe#NaukOX_n($H%CEkk- zKG~vkg~6v9T;#dL;4>`TH~4G||B=Dl4KDHi*x(%&&d0s&d8_fg7v{a~op_6)`&Hxl z?*^Cjs7L(yt-(dF1n0Fhz|ytnkCk-c9*Oz-A>T-I0~gMj%iRsJ-S`FA;E)&WG8mYN zEe_{khr{n-gTsrkzu{bLZ+IzoH@uKvVuQnr)o0k_upL_)ViQ2@Yd9)!>80G=aQ=L5 zZFuS2nR91d!o3aW&vP%nbS5`9Jip`8xwGbRcf)z+4tok>n?7uAc*!N-=iY|+svSE5 za(hGU&Nq^O)ScR=??TnEWC~dx`J*jjUWs0tDewyCv`y)=Reo1N`02RkZ>GNF>4<(^ zY2hjJ`~wS@{Gp!kC;W&W>3kf(rCi>tgXhD~HP6pkxX}5?!lhh(Vc}9PhZw^t{0RTt zOO`*ug%0+Js+O1izsO{tF8iK7VK767QcZT=;*P6J<) zccC-10H-WP&PQ0d=t;!FCEj`q7dod}xX?M*!iCPo7EWIITW;Z!@2f1lTn7*QtA&RR z{_hrEZSXrRT;%hhg-g5-Te#%o;}$ObJa6Gb{}l@tI`3O}v*GhI3l}@Hk711Y%iCHL z{%b|>gDpI6!jCP2r!2hLgr8*LRR%xb!iAqX7A|x;EIeZ9EH8rp$igMwpIdm`(7DaR zs}263g^x4%a~7U5_&XLZe$WZV1)=;4?eMn>aDG~8=*+Tk;eTEcyt4?dJ5mPhr0`j7 z;-PspCP5){I_Ax~kRF5Gx6FRzz4R@geB!8))#~?O?^|wXuZpm?mqmK z+OLx2PYR~|J0t5!0Ri5}Rd+fyPHI?m{Dmv^>1B@lX>Nn#cHAG(hZ*toxW@4f?7_mB zkhlyhKe_8KF~29W5(Lmg6HfWdBh}y};p2EV^ib*uRz={rD-L!eT*`Zs-P;p+V%^9T zj?XSjfx=q6(I`}z6{p zHm-|o;nn>xw=w#N(;xf>5(PzaRXe^q-OFQ~OQ?zaRBI-Cn`&B=T5#+HUf3*y?M(Hc-V zZSIy>Zv8m7BLh8Bs;l*qrI;V!L|7E!P{qEGI8Gazs+#{4 zD2iu3hY$OE*MlFES7)LWnxZ4NK&$vT=sRhua2$$KH=*vwAavr#Fc&FkZ4~cRMv=LC z6yK^GJ(dQQu%&R%8I6;_&cnj>%pu8 zDh^5(>!0q1(vHFXs`@6xR3Zr#U|%Wg{9{x8EjrDxTvZX^D$VmltQAQh);}13F)DGw zQw+Q=ANY_wjS5z+I&1<#IZ8Lae88ZO46XPB4udsDow9xsVQ8A3$CW1FWp!Pn3M&TZ zVDud~eU})0o^%;+oHP=A*PepI0BfUEqFUCz!)hD5ewm%3sJl>fl{N83w_#C3{XU5x~Hx`V3||!e?a&QP9;jI|U<7AguC5 zVwVUcLLG&QZ6Kbz$={x|zJcMMLgk#2g>sy!4uMx{N}W@}%Kj^a$B-&oj>B>6_smBa zKy!i{Rc5l)2pOoZJ}WV4;VHOI>X_AjY9M*mSU#ji@h@Iosh>^_R8Kz3nRMl;fn(+$ z6Fq+PtUWIQ!lUQT+!NjNi2{OzJ7�zhlnjv#Q~9&bee3PvNV^kJJ|`1Hn)OV_MQi zv&XX7#u5LRUo0}+bcN?3F5gEFXvNiuhVh{*OL(dwL*w^9Ii`hyFZAc-hq%-u4=ns& z81bq`(pmmR47gRlW^)xen#X)!yF3>g>$=?x9<4TG7F03Dqsj@cm4?7K3{Dj<{H_0L zH>-Nh@>cj`n~^k&eLuA@unAiT$ygWr^b3Cye`_I#d!~Uuxi*`7`};DJZu@&lzkGj& z`QC^NPx?2}d|x4exytu)Js&t}!AR}#n_<5H&(c2~U)eGxAxBjc1Np@KW1JGNf8eDXKg25+SI3NG}V^4c4 z38W*bF_D6hT z5`$tqkSogb!A4@N45MKKLsvLm%2o9DJZB0gPVrPJefHGF3+4vqx6hk%NryUm>f-i! z>bT!o;FmuZbLxcX$2@}g+4$8O{t8|=7R2!}>AP(*1)g+RR|;;ArHGy5Ie7d@y!31M zyUxOe&$UJH+l%0Gew$7I2`jwByWPSi-nWY2pB2G}F+%(#UP*7^c~F$U;71n;Kf=O= z&&d`ZSFxd8Q-E{&h{*q%0=zKZ8!f!t(7Cq&FQoHo0Zux?kA419*wFdX3NL-J$C*lm zuSrMb{H+4KFy0FbaE6Z?`h{Z-g?d;xCQzt{_L#tKb|m3fL8ABxKl%weY~jLZO%Z%d z5!|tGk;5ztm;Ac82<}_Bc?_&7g8#G#{u>LIc<(8KZ?JIl7JuJ;Ya8nZs8LC7z?MY`HL38$5?n=2M?TS;S%qq7B2CoEnMtg zkA(}JYb{*ptg&#hKMz^Bq{}}4O7ih}E4-xZbqkm9pBKT$8`nkTUvA{%TDaK56&5c1 z-)7+w@BKyae-y#(^Pq%It#QeOPr=6)!7sLO$**fHT*~QcgR>dJHj4ayYT@QFaI=M1 zm=A6(zzf@m_ZXbRXr!L=Kl*xRjS_gyE0w6w@HTxtJI>t6fHB z3)cN$1a@y@v!g~w>qfCd?Z2_^r}-NaTOiP^Zkq!`1N-O4W-&0kGYOPZbn?ABDgKmK!RYbSv}tw znW~;|i_G8^1F#d>3A|XnTo+lXUiL&5s^?XaW$JlNWHp~LCe!Sfu%r7{Fyr7`3~*ua znPKqG;gB`fHX)7btDeZMxaQWghx~Shtc$ecs)wxll>r-=(Qn@&AQmlgFsOzjZPknc zBSIK+W5irg(&v#2S9Ruc?05#p>uO^C!9L&tJ!F=0*LxerWrDqg#$}ow<~7#cRGLfn z2i3v>^j)7qFtq&squA3Q9?T^Yy|?2*FQtj~$M<1?z`#HTlb`F?LrsmRg!-<414${a z1wOE_A?^n_RNnU)P!2w>G`vz~D}iyrL_0>R1|iZ=+zSsi!+ZmhNV@CQvaIf=l3Zx{ z*T9lrk5Nfv3qIGq{&Bx*P%Qe98GcD)>BtjU9p%UULHO8=4(g>{>3ssdxlr%)e}MpU z!NGcHTPHEMSrdz%2)cpJ`wSyVcelAqc)@jApd9-j3=(91WdGnn2(JgXCH=XLX1+m1 zsBeGBMd<@V4Ww#7LJ5DrMDNzFaa?4AeG)bdS2V6A?* zywwYr$wCo{vFm;_C{qvx7D$cRlRjCWsM1#TK+@f0L}vsYM6~@dlo()5M4vPQ?0Pt# z_@4bylQ0L@=F?PyJO5;?RqFdW?o1-6kPS(93o?UU1Q=N^_{0O#8%Xp-_V=uIsy8r@}I>BBiY(i^#|`D`+7h$w?Q)% zYuvBvY1$MPgaTRnhbDTrcg1A+5{7*p=3O(nQ9*-0Br&s$sii;2VqnTuzC2;XPq^o3 zGQW%aWdpHbNY;$S>2-8{k0FFAUwQq(5~cd%XPda& zm|CaBFZaFjQM!bu-2N6n*xkFn<4|TzvkdgP@1v0ZNr%z;z=7?FDR-MnkP%#iPkx|6p&k<}v1N#q5hM zx>zlms)JKtv$=9`q~ksr^VUP0*E()L%S{4C2syvth-4-ljQa=3a>k)r*#0T{n#fEo zNkkttI^1=u8D>Xv65b=Bz5ohR(*1xlGhi0qz-rX;dJwED-7lVr_ zj=eNCT7F}w=Q?x&0y#}-6A|DdG% znMxn3kb992x$I+Vh%_E)$IP8nrfOxEi&=Y4NA#4h zY0WY zSo|22MbuwqtLsLITp3d>Qty+uz&qG7mQ_ie;zeaG+HERCRoGSBj@uQ)BGCKblq?us zObUx&s9>|5)k=L+Zt&_>zx-Y?8T%=Yd#YxJaz=?;NQ1u^kl0+50R}Kx z&suk^AKbTfL~uWttX3~L)Ho2vb%-^N?)tT^kM)ZI>mvE0-+E6P$7T5Q@#wQUA44l1 z27C0C?#DGAeKFK`CvL>#14IDc8tuT&6suLS^d99QQP+8q6TFDi9c}H4X(54{?7ere4&%26Gb2 z#7dC#b7wwd9Dk&8TBIM0mB&j5({ifVW4SHyC>DwT8@|Sj!oSdElY%;(rr`{F6hx*!49~cSc#lJBTebH0`&|I!-HWJj15la%?fSC7mymVNi@xaiE z4QR5P5~cKFj2G_HJQ%lNAg_8+<83su?V5p1Qo3dsMjkn^U1P9191G7A{`6j`@Agn% zko1);rDM|(X>N1F%V4&z2OFk1<<{%n8+5L9{nqd-<^cJQIkA{Es3*y=Sd9ob#Jq5m z)J#fg21;NKz6hkC&{*_QU2sAxF0N1-rUasyMy`H{*i2%Kx`+H z!-DemC=D0uYJ^($Jk&L`^f^3Nzk) zQL@i_N*CEg^aZ3~QCi9YuHPgxnrm&$;NziKj`hQS(tQMV1WIQh7JWaIIT6F=T33~rRc${yU$r2gZpD# z1^V-iO1X`}j(d;LH&NMUL=lx%OsI!yNG(w92^9tzU(g6q+ggPBW+IsIr!^PeNX(zy zOIM`t7wD3TWiBg`HjHtv8tb~A)-ke$N9~Jwo7LpB#PA&tP$}xxC*5bwgga+Re>1#q zQ>p9l>OtZ)a_M;p9DbGMn12A;rYGjx1L($)%9;~=GYd;l>6RK_wt-3(9*TLV!@{Ai zM^V@ZEJdhqD0P2erkyH_w4X{m%Aiv@-==ra?tr$p0kumX-4>>iO<}EDsZ8*GrgD6y zevsoHoO0{0F68;F*Ii5LNnfZ=q@HH9%T3h1&sCel~ znAkXtWn5))EcZkr`r)E+N$-8fd)CZqOuApjGGj{|H+&z`5&yze75>mzX<1t)jOnp@ zpAp#dic@l1qg$Y*(|6OdpVGB1ff^54~Ye>RBI??-jSEI;jpuZu> zZ8lPO!Ggj^UV~Ig4_6}7USd)nhiW|OV6&dG?#@84;{a#LL+lVNSWLh!Hzm#cFS&5m z`Sa&oFemxo+&LXrR?nQ(k-T@}yg3)2Keu{L%Da|2lk$Ge#daz0zKOW>cTdINS#HOy z3;VmL<8~%G*IxQc6j?+FJ?hYlDAwZPUeq~A>P+;B!Aexq!qQyKz|Chde`sz)%tNQr z(0Hkme2UFPrDZzbTfOo%BPxvTfQ7%?sA3!3;0>>qGUuxf=4{d+rp8YB2osj4>;~mG zFjx5)vVu(cIeYkEp+CR;Kq^;RtbY(Yd-FQIF0vSpZSKqTBG4{!E~Odva5 zxfv>Kb|&uG4y#F0)$n`&bd-CO=9CwNS5mf@g0E{V3cP`cqXMxP9=7*YmP;XR%7w&)}&)*Q4K;g>NT z_5CP^TBb3-ELOT7z-cfCxvjX-)8bXR)m44hy~@W#cO}0)mC1{PWh}_4d5t<;f`>QM*4Aozj*1svD{X4XDn(}20o~vd``AZ4!$;61DhZfYe=&&8*b^J`4DJi$yZ^@OzM^c~W ztND#>F1#^?;j61#O6z-aPhbP2P~RoUzBczMQ*Ux7sDauIDz9tjP+v@IyDDymxM{7h zUW3xW-F;+?U*4B;O-Ch~E4@erHVmuZPc>J{(Ogjl4yAZm%G;(o=$}Q1xIdUx*`H&X z<`O7{=yABb^k9_Db0@_`Hj20-bbxdl<4{FDdhyPOh<1!Wr)Bb>+Kx#f!)v?w1 zz*-aSed(f<$V|e{W3oYD*LC>@FuKH~@4zo#obbxw6DUQ}<3y^Bxr4kZA?xx@dso;% z@xs41+0)V=xJ=-Vj*%<2uZVJsi8OiCz5Clc7U zjU^GC3P!Z1s%K5rcNE$o#_AaMHNWxbmX4w5)<+MPbfD|{)!m9wz-^2_`^HDC>s9iU zBEoid=bc$qw05py%dhutlqE`jx^6ouDba$k#;SQ5f2KUjt>FI5UEm%GE%gxG(9so( z)Hc~mNyylU>LH2Ycs_6 z9>sTDcJ;f`O-=6x9nVjTqlxNnB^$)`i?csKI{Il~1KEGg;+A*f#AYu8Li3M4`uf4j zc|m_CI*!YKpZ()o49pL8&KTF~SqGk7DpOt&^P{HM!7mTTO7}$zYFlWo!2L)y1v0NAJ`ax*b-;uW^)AK$02%GQG z&xg}RvKzaR_Fc`0bd8h4dNs8$7vljjFDN@6%D9?8`G=olqeF~>sq8|_HRi|f`5a~X zu)G(9Me+_?wB`4hP&zF6#HjE46n}Hfi{J6N5H*8!Mie4JxD;x!HpW_1v4>xAO-pI{ z^4!)~6oXZpSPZlcKNai;vNyj$9|r7mBm){U5kg`EU`6(A1l8h~Y{*XiJFU!C4AuB? z?=X{gZBD)L9}Elb6RAx6aWgnajjloSA-8p02A!Iw6=A9?=>D;EA*yks3=y>T$n3kX zP+{Qx4|tChLIdnCnzc%R31nJJCR~C6=X_O(?s28OSoMxt9oTE7#zB& z^}?0L_oPP59`82&5Pje4jN9RUg05iHGbNZQ&{NX!O+$^+UnuTTOJmcrE}6?GQT&{y(~VIMN_FOUz-)&8E(w(J;m{c!!k45Jt{;nnzWr=L?j zZQTJBs5bh0A0G;ScKije_PAxZ|Ceh8IY*ak?c^tSlqYd{mMim{xTIfjSu05W;7@Q} zE(iAU6r5rq9hqAzIP;mf=#}7&1{a$wIOjDod^N883EqNx;t_=o;H?I)vGA0^Yc2dt zgO4=0r2AV2Z?N$54Bl+vvkmSTT==}y;EbO?!7s->`JbrJ0ld@TlP$d0;8QJ}kN^Lz zb)=T>`OBzB^vqd5cD%7R5P228-rL&98xWvyP2&cWaX$xc+dk6zRT2D-B6z?|u66_1 z9PB+^Z8$*b|_ESM2f`-E(!LW{fKt zXSdIv*Ri*Sj7ZZoV=!o=s3W3Bvn*Wn=TZxoa(ShNOZoeyg-iK+un0cL7)#+t^2lBb zD0KeC3NQG{7B2iuvTzAMzX*P%g&Xw=thI0{PrtQrDNhesxY!$ezeS<*cPo4i@`k^6 zEnLFeYX>FX&#my1-okwwnYTi}!c;c=jJSj!UIdR8!OyU8N$)fZmwcIT;S%r7Metu2 z!T)IC)kp(>>nvR2{j-Hjyjcs6Ycc_Q?W2VM8ij?Q$W8S2XbYEk8!TMtTxQ{tzl$wg z^7ncR7dhMe5(=F^S>Yw#twr#cEnN8j$ijur*Y?>{4uT(41h27hp|8792IMJpzGH-R(Oe*gJp$s6MTt<3!R@@xcDO*EnM{TaSInc-)`X&{uK*v*6am# zTDZ{p*uq6VgG?hs@}=6)+0Viye3OMs__&3~4V{#QOZabFxX`!PJ_?yE3m1Ku zZQ+vdms_~Vr_;h~44)Yb7k+NCa0&kl3zzWgE!>#Rz>5|x;qA4O!spvoc+rP)<2sA{ z1utC7$T-dCfqy9yzShDe{K*zB{9ITB&scbkk>^cC@LyYag$aL$g$qAhEnMvO3l=VR z>762Ygbh^uL~atkz6gF+0sb%ePV`^)OcFY>7s4e4;U&)*-?bJlbZ#z!KVjjL-j^&~ z_#a~S$Q6DBuP%bu6v0PZxa3RJ!iE3SExekE#c!O2OFo`y;W{+hBNi_F&$4id_i785 z@HbkxV2+Y_xDGr+>C^u`}B(T*7~B;Zlyiuy6@Kq+ByB`6zhE!iCP^ z7B2iBZQ&xf;TA6ZpJ3q?jTX=>3Jn&5m7d}t2a4GNc z0=!Ue;|lOXxm|4G!v7T(F7f`+!s`v6TP(cV;7?n)gnz-ph5uJAT;l!E!iCOf1$be4 zrPEU6FLVYQ!3r*Xj3R)f?r_aLT9dpOS z6ySya)y4vx{8!^YfA3m2-|_b~Zc2)u$W8b^&BBGBGc8=o>4g?9;X8}qeHJcsesAH` zin~DIVG9>NUnsx}uN@hmSa zCBKCK;TB$N(lye;g`blwT=;LbaG^iJ!X^CK7B2L^YvIDr_bpuLtS*B8ssJz4=Q|4U zLcO}X058<5CoEj}dCkJb4t#Fmk}rp0BL)6M{(_%a1aGr&iFaxd{GuZGk1br}_InE# zdw#ctYpw%<2Q0k8ym-vQh5m~Lc%l41D!?f-NtYNFkx$&vKiCiyT;w*Y2;No%Ki|S7 z-Z@3^?-#+BTe!&KR~9b(KV;#;|BfQ~5K~Fp=^bj}LVvV{3;i)g@T7$coo`vV(3x-H zQjd39xP-sP!fQ-Et}eg}%iYfk@F2ogoA3` z(lyh7B1nNEL`GES-9lmBnub0U0~r7 zevXAp_)9HZ;$39nLT81AH<|QaZ{Y?G+-l(>=ie2FP=UZVEnMU<$-;%7X%;T>Ki|TI&LtLJYa;%sg$tj*v~Z)| zf!|uVgx^>M|I)&RpRaS`EPf)NpneP-YvGbFqb*$Ie7c28zKpYQ;j`Vsg`ev!T;#mQ z!bQ%vSh&#ny@iXMcUZXa^QMK1oZqu>2|uV>3rgfB_^B2y{G4Us5&e1Kf+DzQ;ZnZt zvT(_-CoJ4N2Hq%we`Mj3U!PjI@Ocmiv+-(}$v@0S)X^oN=PDETG$p%yNDHdwgmL!t;i z-ok~S$rdj9@Er>m`U{HSODtUU|3^jem0>Mdrk5fS|5MUk0*vqEck%fhIBjSzd$iZ@ zeUJ9hCyr`R%Mt$@d$i9mb5a>+vx>MGr)TerJ=&ArCj+~M$YI)lnc{elPx7z8$+4Tb zU-)jW3~k`bcWifp&GUM9bsg)6?qy0c!C~>tStVG4jRSG9zfFH|J?VG7X%=I~{3$`b z?lsmpr8cx;D_)uvx&sSq9qfVn2{xU?&a2q8PxYH};bytn0JVHbs^n$8Y_>6fR&Hp; z{fIy5Z8B?3vA7p2O@r4unaKkSd@Bya+UwF-`v14>o)gAwI`QsoS>OXAPHE|J}mM)w&=hl$e7<5G)u!9I~x`?V=L5CnR+fX z$L2WCB{JVF8SmC#Z;m463B!rTS?Zwh+Bz)3#l{lTYH{@K)ZyS~c-IE5eBK@R&kL)q zBAy1Ngg;9TGR2pW-~9k(1uSBjiUU#;9?lvs|D)rYO|P-x&KBLC&&D=f&ixl_swBHmg(=uqSzc{*vVZtCo@L**X^-P3L=!<%tL+CmC`LJWPVrI ziTAJ|Qh+NRH;b4z{Jk{#0jN||osi5_@!;lZRnV}hUq^&r@7k^zD#(XGe$AYHPHH^} zi#^3{K+eI!n&lOP&KO!bW@z=8p*4D)a(w8>rGw)`8@e%HnBR$A<#(5pL;MQ9g9p5@($~WhCobO%~INrVV zVxB9jIsX6lw;G=_>1OJAo>;TF8yw6->PCDALD?l`1yQdW%p6czL&`_SQ>NYyj17Ln z;H3tai=3w=&vLcnVqHg?#lAOHpnRT>Ux1TvL%s^@JHC_nm+_(CC;SUkhZ~zGzFTeP zj*T<{nSPdd`Cfd9W?*LeI_?I3PwtHaK~o=3 zEBqMu_V;|YWyeoun1u3R5&k%pl+%Xn^m7XI-s0bF=0-Q-ec{iJzwlk*J;VJ^t|69m zacUaZu*$Ps!v>e~BiTzmV0gagPjK2a;-V*lR~uYxp5PIK^F4op^S=06*u7~U!wc8^ zTK{TrDO{^6c6OA(R~jV{JBsrzmDA{A(dfv)e6A41qS8@VR5}`qO6zfdBJObRmgrjryYo&9<%gys_3m17Ba{2IrFS5d$$H4U#F7$t6;gXMk zvT*UGH(R)bf3XPufrSg5{f)wl93)>4EP_Xh;3F(tW zW?o775f(1x_pBngeIBNS|CJS9=-+GM!p|obF5#=e8-EnHNs4-&s{Rh(Qm!wa)iG!O zKohXKu5M(s{>1v|=(@eP0qaJN8Z}xS)HP$jMTykdK_w<*Rh*}(cuiFK%ln58glSf2 zGi**Me@<7M_ZZylFDB8+Uh+-@*zS)<;tYsA<9SZjNS+^6<9I0?C*|K3X-5d2n6(hs zP|tT!qdD%RYQMC1y`#P5Q1AW#L1Z?igzH=~fGfY`btW!>e!VTNdeswI41}x8fcD^) zD0Xt=EjMWYbr6sE4&SD6OByp~;`;9l7wWwnSSpi_%;K-t`_QJXROQ_x)6;CMSyUXCYtVIT5o%J=v% z^z99r@H%xC8220H@mvZ+_#b})KGZR=p!zbtItFyn1g-rX?dc9s(;qKaAEPN#yGzRf z2V~5Ecs?@Qn5NJGh_`v~r@Xf{6GC4o#IXhCVo+AwFQ>r(5;_By#zY{roaVL;Z5W(J zQny}xS+}*ZEzo%&cF#J)#rA?NUU^S$1EBP~l&g~Cg0T?af|I#VTXn4a^b)mQGu5G> z#lQwG1O%OE^WK6iQ{F3z{La4!UppVv2?SNNJbEHqzzUil`ex@%I0Pr3rztml1kOIGYW1dH7f8R+*ctFX?W|Nmv1#cz zH@r0FrLGH@6k(Ujy7h=f$Fb`xaU7{W#6DS|&uw3$pJLy-Be>lwPRHs?zp2QFGWVf% zYPeBtREOcQq3%y{(9HwXFN*K&>?#B^d59Ah_=JqV7;W`cDnHN&JMKfM!8aiX?)m)j z{F!xIj&rO281vFMVed`6OYg+C@c2IcNZr#;2AgnE1stOS+1=PB z@&B%0*&7jLATjrVWxK%S`n5cz9HDGD3gLcZ;Ftx-3nCk_qZta3Dn2_{f+p(krUbA@ zJQWpEdJ+HoAsf|G0NHGsKO&w9o*u()l$l#lRd=t`#vs&tJ)Vqdpbg0`#q*v?%q@>B z!y6Szb_RpTBdXSNa8rgi0c=(o;gyWmkkOH11i+`NP#=Kh;2z?d$k#zc-Jz+z%INX* zC!x#wA-ft|J1VOqst=MgvsVHwFk;yRp5y*^(3yjCpyK|dvN(+C+_*m;d#+-Wxwzj_ z5r=eU`d5W<{NXY%;Loqb0fvitH@?bSu0)Xi2M-D8G|5|z2lko7zLoTzH9aNTihn%G zdlW|eE!u-FY;S&f^+%y$@kp4*0G6r5pra>J>Gf~;$B~)(Ay0kjec&j2NPTvW<|gU; zK0%vbIRfVf;-e~bnN)S}E)DhkD`(g2F@(8@d4u279b{RwDHbg&|;5Mzm_JsGPU!^#!9q&CS zvr3-~>3y2={$co@;y9TFWp&sz`X5H49d|A=u~8MRP~W}a2I|e!A}7k)yjQ#zQr?Ft z?@OVPa(mRlk}18_K-b{9tvKvw`Uo6Wq&`7tCv&HX`5%gaiCeQHFTq3iM#t&iT;^of zso=uUl7}ez0)t7~nkkUmOC;V4Du$g8t9JzqBe;5#%$)5cGglzpP%|mXqmf&hDugAe z%YGNO=PPJzS0Y%lpkeQLsAn|3QSrb|P3>I`5#nZ;y2&0=@UG%I^)7@0fcNTQ=dbkv zfyy|lKD?N>P4^_jpZIu^)BTPU_b*gh$j)Tg1sq7J^bsZur89*Bfa36`ieE8Ur8buJ zHd9HPH@tUbrfM(}_|!kW7o7XRn(`2wqq(!@zK`-i&t_<8Gf2f13BL{7S>L}H;T(T~ zgSr)mK4N$M+NPD(zovp5m2z=_3Ur4( z-W3zv;ELR)=%YUF>X){7lkW~m2XYld&TiK;@hORS-sbmt^sibY0yU+H9%#t-pS}< zfkL5zHcpQOJ6%%~ncCBw?#_n5f^*1WwTb9cMvdemHxOTZcvcZXIdc$a zzMQqqd7ZrGHno<{{YC|&NdId}&gJTU$SKfwosEiW9EDdAW_%!Rgre*Goxg_t!Wcwe zU%sHeG;}YDz~zJlm73IsdiI04_gib?{v{RK+3TqS5?k`cn{ehreLHEC;Pmxu1Aj`o zaqk}q?;Vv4|C|V=Uri74S9ss%e%zM1v<#c}--Ra_dRRimZ>`ES7z&YM%x|3;LQv#h8P@|sZ`@s7r$!;y)2#1e2NP~8~JJx z==5uw96L95o{_@kN(vR06tb7GN^)_KElYY?_DZpNI=38<{EimuIkD-nbMo|{8{5#$ zk$nlSj5bglV4!_cg9;z3L4{`~Io>~^a1~0=%KfnwapYrp_wHQA84{YXVZ-jy?%gxJ zA)&rcA;j*{D^h|=E=%UKKXa<6!jKbYO z*@ODvGi8aOVJZKrg^vI0#UQF!Pt-jfZ=B4~!3g}TTY>Y3+gtP&^iMJ`gwK|1ks~0n*e?>FXKGE^d zhT%tvrO@D4o!tOhWSS>9qimevkSJu?jHsAeB@mY18h?Cy9d_29R~g;LQ?HqWDgWDu zVvTYoQ~tPS*0NJm{+N-hb0N}s5VAV=@+^n+ozBxtDZ$BTbjvCyxEvzdqvuBBtqrEScb^UT=>@U+g>*N3S`4I06!FnOYA}o14IC zJWW?8yv{)Sm1ORr+~Zf?$?gOuROv4FyRP8@Neyvt0tCD!!f4{&xnblnvoGd-uL3A_ zG3I?c9NPuYq+~%gbQldk6l1NROCH*S0AlB2=WQ~28)t9I)WwVrttoZg`4A4g$n~QY z@Z;P&Db)Z|OhzBc_1j(jeS<@T-H#Q;#Lm_JOFj-N031P|ctLgV#{E)keUE|od;F_{@!|c7MSpbT{9xzF4w@Ly zklBvAL&<@uuHYfPk)QDVtyZu0SR8kHH0*!)=veOIc*!%3mj%<0IglBgA)PN?HtxNR zDCcZ{9{f0^Ya)wz^>(O;kS)#Z6+z9$Z1yj5P{mcI{PESwNp<|I&;)Qt;+RC7uroJO z3FdKH0tmpERKy!62B{gyJv0zP8QS5x?b6XQ8wdE1DsnWD&-ofJO+oj^1Ei(MVv$PHbUPzsAFGrspU4HDE3h6)7w3qhKp$} zD0qq_KS0k(lX~#r)x-a#MRq8QO2`J=^W&(HjR~)+AvU7?^psx}@ybubrub!ExH0Me zE#}q!EYUbA7+T&8&w{jTwDHnzHu?#&okyZPqJNCJSdrU+lOpbnW|QtI3BT!{gm+5% zbw*g`%vrAl4dp6xaLUwuCWEKCFFEYxKIABi$6r8J7WE<)Ol_b|m?Iwe^i1?d!7^%=HO^KZ33XgaJQ1BW1Ia7oJ&4Ak~RU+W@I!E zK&0AtP52kKt7$)MsG3fLc=WZ-I@M@r(Li`-0(-YXrUPeXErIP!-4Ju~;xA9W1Ris@lm^{hq!V+Q=;KnF0%k z7DK4#K76KBmBwdA%6%(UiZ5}R*l!pO?FniYZRc?|#i^KvG%{9N*V0&jSZKu}Mu~_} zc2N)+whZiJ&Q>^zIYlzY`lN*AaCy=+1vD{;Anex+C zX4x3?>u{XPgR%Y+%AD-Xo{$OOipXP)-wuW{8;}-dU{dZT#W5XaGNQPiIL5CXg2O0E z8842ndK}5msN`qLqe#tZZ5h}D{{Y0|ZTJMIxxokye>Txrio$k!D6yerW6ZeQ=6q=R2`IU7C=Mj5jd;Sluw9fh;dM<_>d4Y0 z0rn1{x?=e)sIHK}j##ub)O!VDrBG(XX&@8Ps)o=C7Z^JmwI-QykZc=Jl_+u_>OGC@ zLNz)P(fvccG%i$+86Z^|iv}A*D-H(~8?oP439-q>Id7jMb<8T&yr_oQyxJcoyy{r) zq3)*B5W`>bKF{YiJg5kmH{g>oa0~<yhr~uq-UmZ%h2Jey)=$s*%eNo3@r$Dwp7JlBk@C-7==c***WDRe452!-dD_0Q z6fR_cISS{us#AU!qWUc@O3J?wwoU2cA89j@V;F2h=PNxS*gDLZ6tl@fJcHJ+hQ`pL z350>1X?kn;p=32}V5Q1$dc+G^#8Fwp3hJ_qGjn-F(EG#DpSM2t$@5Uzc<7Avuz{bz z2BM|aOin8xkPbvSR~y3FiXP8cS`0Q}2#wx}&e34OD3->=Aa({ zsxau7>*zc#1-}`LHskf&F+Q4d#tO!wP3Q`kuFN1qrk_nAYBB+P*p7>mS+s*^0_wUn z=3hEB?$2(AfWkPl8U1eTj)9AW0Sqd1HQO&7m(pg@E064v)euL_+ZgKkTv<4rph~vN zQtobA$7+~|`xz;l8E}bU%2Ju&X?e?dbZCV>|93jIDH37-C5CAb>uiK=N34zfsCmRN zWf$usu@NK5Q|0ia?*1&-ahkH( ze(KafW9n3xNm8we16Ao>NIj(T5?uJLVr3Yy2Jz>2pXn{TswI5r&c_zJcvYmQ*-} zNYz+BvOi9i+BfO`HScL{0MDmEMyJ4l!&akNb}-E-G9)(qX&gbw4=ar!t?-8q-{1{} zKi3E@4k8zspmNw^e$(Jyl>CR3=;6tMYQP_?tnKZ<$Ul?sxX&Z`%$$)p3hM?KX0QBb zZSF9f!rAIotvt zCpAmo6ZGfCqIIF(9WZ804AdPCXFOUC6a5HH-w2FP;K08(_E7BX?^JM9@onzII1_w{ zA$b5`N(@v`|f%qEz|E$X^FvL4qI`jqa*6|KozaS1K=&~os{hW!Z@KZXIU!lVR@ z;)OyhE(6Z;O(MIR(K*agEbqyT$Zq>AnQ^hplE1|BQ)_--=JpYfkxBC~MvBATi6- z_iG}HaeWK_aiS`ck@D9vVzRjwSK4RHMz~X@MipGgpIDjl7gp1rk8JfL%N&0;2+u~S zuBmnF+x$rltP9`)qoIWQ5Ixn2x}5G>;35Z@x`|2uBKXDV8DnC!W1NV-$(af89g^-g zmRB@+`vImhwab!jXTYzzH|CuaNdJ{dr)VDtl%Zd!4g>nyRzCo3%RNXCBReWR>~BVT zP=J_<+2}2!9QjK`KZX;-U*}}Uy;I2zCc2Ucu3=uKOTU_aB}LNazC^>;-s;!?B6X+IKo8Fk76U{NkGk%BfE3mP!Kgm_kV@iy1V`=O61fczp`N8dC~S{*Z_e+)Pe%mqV)~VZ`@XrszHH}B#2xU4!L)mBcm%3k)iP`YQ?+1)Y zL7)0fWsdjRl2>ymIBl6Ass`<>{tQ(s8#duy28^Z;LaHtUh6e2morkmkaRd4@3tpar zml*$kA^rF03!$DMpEiSm^wN0aok}W>`%gXGE+Px{tV1mvM>;9A6l!DiEVD;4^gAOu z)>l*n4*2dvK5&-r&mbd8oM$xNjk+KJoBu=QIp}+CC;rKVVVgoPV`aZ zw8fT^`s*RZ>qs>(Rfrn3)qpfxOM^AS6*pk;71x}Uwx8AJ_qbc`;vC+AA z1v(B^1kt;@0nfbw4C^r)-bWa8D5}Lrm_4&kARawF)WZxzMxeuQM<)h|;WeSS=5WYg z{JOv~q#oUYMNs|0#Vq3K9VPnwQKdBC05W8`CPOn&?Kqh#rACIar29BKUYR*9{lS|_ zAzqS-RPs;Q3-&oBe{)LEn$Mo5su8%iEDR1NS7ai{i!fXUG`3Ku!~HoW<~2pYD6&XJ zG1i~{HJIKbkodBgw_mmq#F=f&5E+KTIq_+Xmp&ZAX}X3QPZGoi0M)|$`cSL4E@ zeoi2P=wYsr?_&NvTDd2v&Sz6QOLrAgNcP&$vDxO&Z-(b`t9ra>BD9c9q&TXjvnme0 zXwn={ckinDo0as!HjWgcVb{*OdB$4^1DP#sR^3x9Rn;j~D*G?^3GFNU@s`dm~~P!UG9l4Dg_OFiwl%Qa_^)k;Q=Po|0og^^aj z)(1cEEzo^PhhcVpJUkYCPdg2>lkO8Nw=e*k0j4q$I1Tj?zv>P&P$KESa%ts6RJIhr zo}*^uhQC<9T1bQ>lBmmhOq8W~1GrW&7&~N`q)I9b+866ombcMnVHg07iW^J{RlCA8 z2voCh86$!5G!;*JlbQt@hrn*Z8N-qacdld6U>2g%V*x zur}}7l)GN}2}@LD7{*gMiv&>d=q%8?S&Vz?P_vI7Fck4Vl3>@{O?ZJfXnUe=J0Ml6 zdvkI6wqLhB=6?^K!YJfJjOJ}NhDRCwR8(%PDgu5ugky@ygFi%fVxh`rh=nQlNtmSl zTT4)LplkiT^n>xnTOw;26)XC4P=869Lp`tH3F%aCS*h!qoAo@C&NjGmkMMzvJ- zXcqrScL}&}Ihp$3A-xQ(WI)SHY7`iTFng3F&|gSV=mr{@Y=&2MeQQYsN{yx*8lR@R zM+?SBZ)BKS$>Pl#QJ&#!*1CXaAJD5#qSoh~o{ik021o zdR7gsCDVJgI35_63Fe{&VL_ z-D%{$#sMS@ryPq>1Jp8cuyiEquzgt{n>=A*SO#MBTphpuho}sfXQ#5d)fF|qK2pg{ zQ#f2xxLehhx2Z4r7QL?9{M0_w;zx@ShpC;^efG0YWs;1U&%ybm zs`c6Z&TN^?F@xi0=sx3dzpyS&sCPn;yds-vCZ=+<4Xs|5hN*EhsQgPYi1F>_B=mue zLJWG(jl@l79J_Nss3z%u+f0!_yDm=dVXd1h(Oj#hg)LJyUbT)OY^ZDG8k%W%yXba8 z0}tF_d=5s}$9M-VJVu1wztHr`WZXpE`b6~Q&cl=L7E(u_#`?BQ{R&`7_cXsA?X{EA zuaV^$rmd#ui7VT0HPj|Y;-JRm0AQ#Z#FTPajgPNAJ6Q;ib8xw#^L2Zv^+8WRl#*%ML z+6?*!$D^-ye%o;$fpfOP@!nUiF6_$^zeHKJ(XeW8)j7NZPdU`X6+`f4v+c_hC#o$Hr&Me`xp{kEjaytHbkN_~Q!k0sd&4G=I0WdH-RYbXb$0 zqh)ZS<31ISqCZu&9zN}?!1yGd(VAfE;ZxOm7-rI*%Ansn5gpaxI_@WRPe(t)fF|01 zPv>x6e7@Q6nQAs@+w)pUn))yu%I$qX-9Tjw<9iqQ2`qU^QZ`n^=BTAO8cl5A6u1q`)4l#0ZjH$ z;`KTXLenFDkXmt*>(71(qZ7-;_X%X*$=~rLc1+LE&T_}$41c%^??rb$=Ac6a?!MRY zHJI7>*M!c&`LBcdyFs{f+t>mlj`}r6BAb-Op(fcmi)VT4dgwRF@icb-TJMy_sn%Aw|((Z@QdGNe;^EZKo)=Y4w4w*?$ zEac&@(BCaw#?*GeAKEEpPBq8 zTp16&pP>t!7bU2Bk0Wqdl2Az^Ejypb_$}}?oghzFO@G?lQ zO5(R6LC0-DpTs0^+lY#z8G46pBRZ}L8r9@W7b#XsI6hO`49bh4f|J){T6oi~pb0gc zG||ai0Wkhc(B*8OmIhU*nT|9gaNMiV4If_YYBnk~euZv;_SCPNoZqbK#OsLIPWmeODoY@#}-x%aJ%N zc|jGV5(lLp$9g$a|M=d-C_<^u-idtIl`Gr7+zsT5N!lyGl(4-Mhn~KH8D) zx5BY?xCXpL8C_DR`>F<1OwRyO!7zL94%}l>ZfHdhZgN{X=KBlE9>i@RKWY0$<~hbk z(b8?466pG-5V!&PM_tmt(*Uey<&?z2xQaSp84NUHT3-m_de6NpWTd!Mv<4~zFlr_Xr+^e_n> z!-$upffr-Fdtp6qH&8oQ>&pZm*T z%ZS=vmh5EE^fU0q8seGod?l!sO!@#Z_V8;|Cl$q0Nxuhw;AAUrl2cneOsI}kqM}(D z9G{u|XZ)#E29zEVygPi9^cGete^~LKgz%eDT5k>3z1x<#0O6-!gA#iM?9F#!4$ebt zsAVBRm5(Ne$RoQUDosyAD8q&nk*Q>9M#`&){ZxtV=l2zK#LL*<+qQ|1Xsj;%$;rycA_cp5fsC;AKTn5<6X2X z<9@lHbTQmn-W`p0?CZD>=N`r+*d8yc{O7U3^fOSAc;i{Y^vziHXgG&uhdf%mxp!bF z@=R(i%su)Z@!I=`nqQXmr!-*BE7loY9FFz(DxUy}ERaCwF-gCzS>*vV6dNO9UTMOg zIU<-C5gEaV;$CEU`X$m-*{D8MLU+7Y$0)@xuHA88@|u34nM-4u2Lz5BfhK0sk%?Tk zqrZe}KhWWaE*FehQOHh9rs;B}1S%FbALCwoMXO(RY@0WGWNctoTbs8KeZ?;&ys6T7 zFmpYyWGoDOTnn>ZwZ-wi1%+t3)$uS7c`Q_=3+<(IVV*ZCxj+>ZJ4#>*!S+^RO3R1_ zWup_>i2kyqpM<(Um+PnR*y_(c1oI(l55fDfUhS~-->FoPs1_-khL8zfCLx?qW*3Z) z63r)G(;d3NeUyELASlo9no3e7yOZ8yX0jtz_j((!D;RRR%|!eye*xT9_A5jX`^DdM4Of`D1#DXfouA0Ft3}2jqxGD z1=wr~gEs6~W!h5I`M6hJ3U->%G2Oirlbz7%kSzsaU0m2+_h8qs;x6g!Qeuxy;hfRQ zs0XHa&rHH3lJ0j<7cwhdte48Kr1R%u$&I{rj^>X2Br6z@M02WyBWYrD^{(ZQ`>rOZM7C(kJkaRJS1ATn6)6#m~jh6)Bwnp^cJ=xy`kRQN$?ym;hZVi z`W=wK9j)F)(<~G4&bMft^v}j8zp%nwwTS+K47N#sN=u;6V{3ZYBKk}rdgW2Eo%k_E zW?nVW4%TptO62KHNT3gT>r&|t#(7P5nnAuN#!!l=Su;T6#!WgtJM)`%fZ5Ekb`?mb z_a)Qslk>j^sd4(f{1aKk`YG9NF}!;{W8xd5@#-Fy$foXyG=2a~S};mT2Vv4wW4In@;xk3%tSBBtJs*WfvuMt33PDlqV zHa}YiyaIUeOc3b%0nW|=fFq9wq5LrIx`;z zkCllQ7hrcaufc$5J%zV9aFR&HX*2SY0bTf9V9wQzmP6gJ*sbv<5Lm6@S}!yp5q_4B zBg$IGaJVkhs5*gfClW6?GL&}SjedO_@f|JiMrUoFm@PYknt=5$xv)U*=IX^fe(i4i z8ZC!Qk7Zqn)$nHv3~PLrx}V6dv2M_k@hyIhXR}NXd@g*Z^Wm1@3AqsjUh%$!f0#ew zA&H|whif-_>j*t2OmymI0LTN#Jq9ZHP$b{$57|Sr5wlk^CW!MEE`%L2&+9XV7fI|s zD0t1o2h7DGuMsC992%hg^@*(Y$JlRjvWt$G6R~}bQzDGY6^zy}Lzyzc7jNQb+%F-( z*Y&?K!&vy^wamqw>MH$HuYW3CuNTW*n}F4@`vnKcIMJzl8lS^bS`*R^@z2p^m*Y$V zM$2eae}Q8wcD=b0UG|Vf+^x;4%xT9Lq&8!Be-_fWm&DgvrljREAt6j%%{)i-1vMwW zAG3$i<&QD^MEU`9L!CnKL^nXXdq`pSfpH5NWejV{>F5GyxKw9kb&PxKoV9O!ZhW?E z2#1|7?ssY*alNxx<%hWbu_nU#7}q1P_E|oF;;DG*azgon6fiJ<0jADow|QnW!FG44 z7xZ`dOeFW+wsYb8bV7z4dLv-H;Cp)ef#mGOh zxZ=LGZ&^#co5gzNbne*i*7hV;V^vuKcP1Y6Xs7O2$2-~|-8_S@-wseHRVoIxQoWdK zEVc>6dK2%51>J9o;=d>7;&I2@69z~SOzJjQ3@7L@>O|vvoZ_(~3~*}6Ucs&IdhsIT zO=)ie9P_`Fkc}JViXkS`Z?bsYqDdnJ26uFpu#XzoKP>LKzao)U(-pc|lCn|~BiBB^ zf4$Ru9&+=!x^3srl{oFj?371Ws7`K> zf?gUj-XD9^_`0#9fc;OTn6=d^q&)pGF`vct_baNk?j$!hlX4nfL&wQs|3uC+tE&}9 z?Co+y&^^1ii*ViOlpePPp(-Au;_NRWNa&}Y*xbr!*LOIR^GWpP)@hQU%f*~TSA2n6 zqO?-jp1IAh#v;}@Lr`wfx-iC|clvd*YED5;J_*u0)r&NuV%_Tar@8*&-TiTIxYs5!oZkplk0N7bjVv1h2wlEN1<1{iEWd8A~V#`a}BLPtgb%Mtk*w{`Ji>FNaPXjn2aOK<>F1!6Nq{ z1zK9unAd`!i#O1ehk{X--S_=E&ktJo#9{sOZ)i;)P9T3OYlDY8o_}$uO3WGz%>650 zQ#k*{2Br)_Q_;c*kgZ+spP-m=os5}aS1GkV$M;9d zxtz(R(!D%EG}Yy3b{!*?pRDI_Q2me#nRo2#i$)~P<^ITzd28Xj$_7B?)3^oW6t2B4z1Za1DOrhnA z%FQ)Da#g~cG8(nW`?VTC$8U&s?PmRJ+_bx^x`#^~Tg@ZtZk&*<{uW{Z+G5ZwABYp) z5PLI0-GgPyFvM`e3vgD*7*EE>1J6$9Sv`ovArkj4yO-xWW`!~)nu&pbuHy`B@!MS4 zcvfa3q-t&A{9S^}-yIh{Va~FKpuks7mbUuOy&FCMuLz(!sj;>Joip;e%%etYe4TG) z?kE^Tv}*(3GchWy-lYf@^Qx;_)9dM9ytq#tBjle|?y`gzZ|OiITqhA<+^D=Y{RcXS zhr{=y{=`UZZO3O!=eVB|K1?i$JOo0#$9E@skt_A#-c|~!@NN}B{QusHft_`*a!C8B z`Wf2Z5P%4SW(dd-IO7r>M@T76{{cn#N?KHF@1PMbD3A(eW3#$b(jO&-bYPOH@qxW& zjO(9YkvkfyC2GfegZd^)yP!2QEx01A^`3b%bK$v_4Vfo%4$Gi_o0;^+e$&OySB5xI z_u~R6Qfq4rm-gTR`>xj;e3x;81>JAq1+cbr38hu7-v6vBcoVBB@E6C#wm?AvjUk!t zD}euTEZ?`9vIP@bI!#6*GqJ}Irv&1z*VHc){NrBa zPC;x#;134=k!xDh#5*T8rG-CSWdKZ_E^QV6S!n|;FZ>LC9^qA-!U&Rj^HsFYFN6r}?p{LE(~b%b-OX!zLUGOvXwR>*&> zz28<=$7OaFMVeIf5x<%&L@2wMm`qob-mC_%kBBy?{!iFTNvGJM$h?I64Fol?lWj41c!qFAgO!&r}%wk~f!X3^3>h7j*!9IA4e*KDBqosExW{w%=Mlyjg#dcJjg4IoGyt`cb&y4x$Uu6qRWoq5SAXi zrkRP}zCUo3XvuGIWA7WD&8?)j*zoLg3wX8)6TaB-<$z_wufZlFMNc71(|t(V9{ zY=&?kzaD28_9NKpi+Avy%$)_jjdsmtbOqy#c72fx0!~B9`*w{0*U*}NiR5`d=9YY= zf(Bp_zf5__F&$?H_8G%v@CI0eLd8&jc=M!4$BAfwpJIldg1@6V=ZiX0RBrTmf98Wl zPRZ#0_2a$DqnoEhP*gXlZ);@h=7Z}R-(s1#B3}G1R}B)5GuoP=XTv|R#0nuRy2zg& zCtfuwk57j5oRX$%#jj)@YwA?nNS1G~G}I48C%oNEQ_7n*sA6u>9I`6 zX8`|u%&WiBc942HzZd2epr{tPhJf4c)&gg+LTg|{+2-C93nUgk!A`2c^+lPn)CEFX6xoLOg|u#$HUXfWyZ=C0rwuzxcT zpt_pjO+-Rxory=?bp?}+{b(qc){f6c+Vbm+y4|={>kK%2JYO;Hk5-971+PZCwi#qE z#1q1*geQ#wo&e^bCcNDSRZ_z8ssKmem_-NR$R|-`qCe-sqIgM_S?{Y2jyxOShygft zF~AOx1W#qWMRfLlGXGg$OXXptsGb{Uh~3CR5PQ_Ab4x ze?8T)BpzRjg?&_O@f*(Weg23NYJHM2iKI@*%!t5g|e+HIWNbd)o%v3XV` zb8nD|RFPSJ5GFjR|IXQpcvd{3mQl*X*>ZwsGlYxatmn^e{D2X#4C0W4?-_(Oy$opW zHI2WVy4eLW$SHlpL^}Y%^glM zI}L|7@)3}hh**Pk6OZk7Of^wrwSFk-F#rY-)-m0lipMrayI$f3`8a01#O9Sum$89O zWVv5Xp%0?QyaupbQn)pk3hDR*Q^cx#dvJj{53|y2B69G+fHXcxkXs!^LD1@UzpUUUzR6uHl? zFXKXfp5DTcjeZ}iFKItM57$?d>AkkC{|sT6+YEkV9~jd8!ThKDUTECf_gv%FTqck7 zM*cd>#xW>Cg4i&X3?wUjv_eF283cksfCq2mR54fp59%#Ze-S?H1ld4q$Ou0nEr3hNcN+qk%5+(&642=YcH0Z(=7OYGoNiEoQQ-chM@I*# zUf3;rO_6YHG7PgQz``@pcVp|LT}w3dG-}p*26`m}{jnYBi30-#|Cs6}n(F!JoIP@2 zdS#at4)aNBG0ndn9x5C%$puI~LL)puGjBi!%miH*F6|3&sVc;!^}L8NgGYaa;hCCM z4Ps*8zhY0l?ho3B^{i3w>SKO~iNP}mmgM5vf`d)$G1v>{h)A1t%O@a%+ z0}2I88tiBlqY_Je5Jk zc}`}6zG$|LYldZ)!lfx+9+P7O94yT;v+H z=*FG{lGP!*IE+h0?K9)CN7^q*rU|N!sA?CfBCD8}JM6r~l3q`qek^|=s|`fF$MsK% zLmeREI}i_uxF(z-2cDAILcs0(oBP)%P}97M+YR18(4}E8qUtBE*X>8Sy+T9isHMYx zsK=%t;w`0hCRq)^1_bT*Q#bMAngWou&r4PyAAuWj62ayJ%N*S{E zdi&69e2+IV@*h=#z|hXutGw|M>Uz%qK~PHZYz0c4;$K`=7_DV$AeMU3pT%$<7M1EEs@pl_m^|f+_u0yz^&7+!Ep#Uam?Fdp>de$Kgt( zX|!NKoRZu+L(0PJYaLY+Dspyzj9t3AH4RJu3w}1kFW?*|Z2kngK{%B~1dPRmRVCJb zh>>P*a_wG3*>#9;Hg+>x)?{NM>qfe$&nAPRG+}t3de|Fj5U5@M_zBt67m5r)!!WbdO| zLVdTZ)XGOPJa3N^IEcdBfM)U4Yn|N-%VX;^=f?IgJc6hcN@1UysUG`Yw7#DzX5$5{ z76&ln`{`LY3e-5Ehkwo`z~;Spw0`y$KfA4aYjnBdL^YYr=%CJoSMH1OK-fo> zkBWgSi)Qqb*aGlsu+^`4QrgkHYKqlXR4Y#f(~26ySaQ*fe2U))hgh&Q;L`HR6&5YVD06X=!qMy?R5=Dx>J)xFt`J$>5?_`mAY8#X{E zh~7_n?+3A^_-8Lb6*(U)V^EZ4faOf6GLIT|lM-7c0L(hB#UMT@#9Pt{xQr3>XPa>e zJXI(IdTXxbGM^`V)bN&+33@P zVEA|fY*;bEDGdWsJs z_Ii66NdimDsB?H2kDaCJQ0+lUQMP`n>4=2p_X>lXX7 zl%L|wZnpip2))9eOc9m#fX&Snn|sZ$xtY}#ZKRmCyYg@f+sc!&A(`Yl*`0(98h2{g zld0o3*kK7Tf+NG{@qvSSh*97YB)g9fIvzfkQs;MiT_FYfjV}KS|HfnFMP6dK-FrsQ zf-QyIPHd`0a;uor0KYrQG2s!G-~CSOO8G(f-AMS|XA1Zo6EVQ=wjP4t>5FK1Tam^> zes><2UC8f7Lo1);cfI0wy$9iUmAq~UJKPGT)t@yNh_xM>*^L#mTLk1f#O%%-#OzW5 zv-`4;;6OH)0)y)}I+o4NZ__+~3P_vJiw}X2)}%dfC17**GNj*pR$E6^!jDj9;8~+? z^b_$$nT_6v*|$vhGdl*2#|ShjbLoDfPT-lgF?N*@&um)s4X2;@_79>fmRWw;WY$p2 zFwKIfaNPoiDJ~c`qF&FHmN8oj#*D(a+CmSDX;EEJkw4l;D=A=~#<@B$lblmYXs+I!E3Rj_XC>V8xkB#wQl5LxmU3p0d=U3MKH#2n2e_v-;aK_1 z=3|EYDX8k1n5SWr^~Nr3m_a~c9ibrhn&Qs}31WsJ+Tbw53+qEh823lS{Rs%GMu1`I zZ`>LRTkqs0u4k;+B@v4kjj+Yqc2Lguq2+upz@MtThg*j|ofx(sJ_LV=-ndTEBN#Nl z#m&z9p-m4U;HUd7K3P60!DBYDg z){nPIIQu#34FFhS8z!F~`)Z=bP-)F{~} z+IH!M)Y z4R)Kch-toNP*mEPIU9i1K*1y^YX%WA{EXzM@2*4%;ttds@k{*{c^pn4{779AR+b}4 zV-)EOYWkVZ#PG^^n_XfMo+n768vSr4Fu*>mkn)IfG$wjYC2_CmNFvIQB(W4PSV`_Q zAfqQola*dFz9iD=G!x3X=(iAPr0*WnZMsAR_yVK<(mM`wmw+i2JO%S*cWRc=ZM2=Q zFAbWnx0$O3CQ9>V=c-&hi)o5fTkznF)kGD{QrUBp*025 zW56*(rbEmyJ8Ve`re;UM)L3_~Cc_>vHKWbhp{6EoW+uL%?;cGItI>zyyr5C`SXkRR zp%^&PJlFNNpD0V_T4k3&IX5xG)d9l9C^L8dqMPO`VOFL$R2!L<`22I77 z|JqdC4K=VnFdqcAg7Ngn=&|9p$1&UQDuQzFS#Rgw6{V>mkOfSE~q2-wQQ!6|eU>F+oUSMMr z^Liq^4L1iP{V@up6O(JWa3d7#hMFG5M{N`YDVsh*Wz$E%Fe#5pCIR^`+u3QaEc1M} zV@Tr%g_hXZGcv@EGV^O(xl_}T>j|;Cg7RE%8?Q=selgKoNAU#K$^Nx?|E`JqU)dKg z9!c2Fo7vW4B1&~v@jPq$H!=BFbZ_!6+UK3PXlLUGy~f5$C9f03Pf@lYT8Z_PdLQrC zuQnyX%T^KdyMnkmQhBa&`b*d#;*Wt79o`dr-kPT^eqj%P2bKAb*7Oc!34>QD!#f~<-bMeQ+p*#!euGxJhdUZY%<2hg>HEome$d>c++Nxg4G0=*?0>kK zMT%V-GNw)hnMN;J9;-4d^`ED{{EYx&eO*~rXo}pwE zci6H5c6QUd}tq! zC)9(WV{vt9Hm-T{q{g<4Csi=(b{x}pWnm*$KtG_7mT(C9#uDjP(-?~B&DkbW4l9zy z4`e_R<6lB+=R1(%56-i6@qO&qJ59x&-_WJ#iSvx(cu%Oku=81>rR=XkNvytvu*)^c2X&RF6* z&orR;-FDz%V1OV9vk4BjpC>Su8t7Rj(L1q1Y_oS_x&4`Ge54vQLZqOxb6dH8Nn(I)fTrFK~(d-Z`KO)v?RGf!zb?Z|EdPV(H zn=g%Yd{K;>nnzE8z*qT{ozeakw~+>-eRRXeskqkM9*ty(T2RB8yJYNZ!HGv{*0=SC%y97i;-UYIA zQ020MeC;7J_?|!mk=`NpeVb16irP+p+`qxAxXn!TlNidYe{}P?k&b4-(S{RS(_h~II8UNS%bt+l z=GR}7O1}a=85sj!&3;&8irbGNw5@sO>5&XO9wR9fhG!V>j2etJjhw%|`-5JyA^H5{ zRb!Ryu-i9)x;PWiDOoU&A<6jP&7Hn`h0{iVCy|s;S0byn*dntY{%F7A2y|TwXP{r& zcb8?}!T4?dw{v!@_i#|IN1^WjIgC%D9PjQ8q@qwVNGp zz49KETQ_BW9mzXx?Ka1I#@YRhbxA!F+ui}|jJ?e^&FS~pS7(iz{dT_$QI~wN^TVO- zHRiK_39ApX1s5g>15OEg12P`f0LEfwCDk4jyQXiG>E|X5&5tbWWOKnII`@34{nHXT zNN}aMf9bn5;ZtoP-P_+wDTd4Ya>j(?+*iR^As6aOdC!KWopb#nOHixSxo`Nv?msLP zKZktJLX}fd6)A5+7?0VxXLviPiGHgDPwLKf#)h_6^cKs`3!X&oRm<4A=08<1Ea)Ro z2^NV<+IJ*njPET{jI||%$HlKSU8eU*?{(TEY5Ya2H}+ct4v7EHk<2rF{}%Auzu(`A z1N&R7{#uqte+8+(-`_~Szj6W|*k6tLzu4b=go>i8mapMO=DVBlyZRpo9!TlzFTAch zsqFYj!h06gnVp9tso469EBw=9F~VLG(W#HvYY%__%|z^(_Tm1K>;*%Dq&G5=y*QGH zk)FQm;u-$XWasNx5sMb>PC{bI*sk`;D%g|o-VNZF$WGel`bVT<=*649KXRpiw08_O zJQCUDKCWH7D;ayo^^Qn+k6ip@LSfr`65jsr-vZha(?oLX!bsoijNBHd5j{KS(6)1S zqV~fe4&-A6=KPVk_&0}~>3T12$q@_mDas>c=luwOkK$?I|4xiRLXYUd3Hy@Sg-EUY z6VWM;#fsUSjR+bqmyE_aOGHyoBx1St!|3F!B5!2M>r1eeb*%5=QU3g**va0mMW0S| zu4nXp7e`_xWd6i*msHSv^T_B5U!d;HaWi-6+dR>J9OhD!tMI%ovrU-mk8r)iUDeC@ zsP8{0*OSQ3inPKCYu`@Rq6qIxuIo3t2 zpW(Cs`{V2$cX$wi@;oG#^mg_=O%D>;e`O*bpTC%Y65eBGuhV4ql6|JGmYtppR{mdn z`M>z`{~TZLF5oXu1?*-Jf59Fb@R!nq@E5KH{G}{Grl4P(Qk5`qn9@NUrZnU*r6Gr* zT{w*Fb=gUKQhEMzG+vbOPBU!g zT``pkvQJ_qhRFbcJd=6jl4H1`;a;$^nL1;Z)x5zYZ+w!~yn(;}@RUC)O=uC06qk*-0U**>gbf z9{nH8ZypLt+m{SNw|G{B~Vy-gug|3S8>_{tm5Ghr*?_ zKZ^L<&eu!X7$CNJ(fhH@^M}dJ7fVDIz0UGwkSorbBjg&4SM0!ZXtes{09xHJuyBD^ z{`VuX??s}^<n*t%%xEO+=%?W>0 zLKu2F7@GT^20*zylI9GO{^5{F+B+CYpXBrq>){QbK+?yf-9Ip&Gi#y>JeT7F%B z76YVW!E)6r&5hy`KJ0yN%;C~QTpExS81?4ND82X#uE+f;Je4F3eKltZZ*OiTzqAHh z;MKHV^>VrM`DOjxV>UJj*WVz%I1px(5LV# z(J%-8kc4>Y7DC69=R+TtUQS znDV54TY}#vPK}kMhKs4-VuNH^m1@vaSbhV|*k4thpfzKvxt2YFC<;?xyC-}J7bZpx z$^clEghA+e@&df|lB?e7peXUG>SZZ3=l!eA8Ch%2kAf5*J$@C{1ECJ?fx_es*Ip7h6T-0*#*|QTnZaWdfRD-y#3%4 zYpP}M@rI(chE4vAhE0kY)i*6 zP>=itU8W->F%w_iA6bg)h#X16y>SBlS}EnmW5Jypl|hWFV9TOknxtYbJrvur@V&m( zVJC7I0H{19h*q{+P7twd4{~1)?-QnBi&+F=$o3?+&7iOKVQS=N+YZ-njZh1NI&~Dh zlxJ+L-%$oOw)nHknb)SA2}(S}d^dNYv09jwO2ZR>Q;}=xiN(FgL*?DXoh*5CG_Wx- zdo+gG4|kglKtaZxFWBk+AlO}^5Ipl8v)2VC4{W-G(QWkB6JB=$Z>OrOw^#@y?qim$ z>GJc~bO^6=t4+A$7};+)uis_Di0b7!+?g1*)&$L=d$$NzEfCKl`iG@DLCy}*C9|QV znvTz!iINM7BK{*Lr7NTH;$1vIUh$2Ymr==CpT7*wC9-D`XN>(MJD=smf;8^2LkGRK zg=i{I*zyti7U@3oF=pfo>alr%Sejkl3(Ol)(WFYg$x(GR@iX(PM=Ogxe4)Y&h^^GB zZ+E@7c<|ZU=ZWoRPBGniZ}Y-79=AI~(X*08)AeurFs8O9cb&KrM*zKCHX9-_aQ-n9 z_XGB#cyUcx@W6EYYQQL2{t zMwP6f7$0Rv!$OvKIDqf@)$G)y^L#f zWd)oq%Bs=l5^N1UTUNj^#)%tvA1Sjz1KHTG`n%6>PxQ>VKm_qf=kl^Z@2~ef4&8JSm}3 zsrhL3$21OInY;#8X)Vr0=s$4k6q5t6*lD-yE?w0OCC2-eDl$1tm6}Gdi(RQtLTC%S zcdPm=TT8gHhoRVP*~fZANH=#|!$yKdky1<~#7IM4O3e_*w4c-x~j zkHn+5Z(zmVcN16SrKsS-^W(A=$5BxN`Bjl7nETS3IUSl<9j$q$|Kazd=T~;>Rt@dJ z>MO4%j%{S+m6?saXG8ynsoAkta;Y_aD*I=#kIxx|`%q2h z(I4Cu5K79wxq30}XsmDmTU5${wkx0;(TI0(h22@t@P&XSSYl#9<*Nk?s=bW-v6L5) z7RcWbq@d>;nEyYwe>T zWe04Q(oMvu*$)u~h2geX7pL(d$}?3I>w}r?a(b0IwY%KnE`w;fA0mweqqN(IvC95X zvl)mz<~Wz3u96j2B(JbnCQ>-oIMTmOhOkZeGtD@#4887MNW9gB72ef4aoAo*zu+KTV7CMbRm7o}IUx9IZ23RNvfT;?$%H za!|p;y6Y*GW5?o|Wg#e|6I{XzZ%n4nOlQ`5C+h}XDA@HuH9Z{o80bWygwXb77lFJDRv#Ox0X>%dOV zwtumR6G95A448Vym+=Tt9bZF6=o=mEQN${n-b(hvSI2o3pHN3|8Ih;MoXFuXgd zH?m2v%Dmm0-VCa4q1JZ|W1Cvjw^M7Vzo^voDG+?DEhCV}kh&BricnJCc{;IX+DoDR zq(u4>;zwUCNvz#zYlit4`2xYXcYLWnW-<@O+En0n^m>0n86L&YncV}jmmqZhYn2JH zr{8BXfd;aDzxStu#c(Lz?0?l_NG zpG$m~>wP`4aLmf=SMb7abJq3|?=kksE!v`lLLJ+2i+8Y9AHWYcv7Sd{Y9EcwjK zzcx@N;kLMcq|>X~4*evu^>6I%A?YwQNO1yjP z!jqirr2Z{??UKGbc0$MT34h3xY~+dVO-b*aq(^4pj0icr*E`-L3qQrBzQ+;op73gu z>9<_(Te@|d5R(wTS?l@i_-yKkVi>izzT;^^>X?Y9U4N((dwt=T1#XAfh{N631Zv4l zeU$rP*b^zg>}gpn(-YKMm#X)Bk7QHBis`CpDRXz9pLL2|oS~hui%O%*Wj12JFAB{) z;jNwIS5O`3$Ygqar0;hkY=RQ{Cj;oFucrL*g8MrOe^P0J6tsUy_}@lefxy0nz&)v4 z*RV-V!i176O?Hmmg#qD927IGqz0q#74avVoy8Gv|)x`J;mCp*YQWg-?k9^vV~fn#7{1C-0WAFoOPO< ziJBY)tt}=PCbFf~k8DYK@Z|;i)_?l(i1)arM{~0-b5GyphQ7=MZH{K=1D)8{nIu)+ zWRt7(QzSTTp|v0kT)pz1Jr54RM%W;ZGt01&m9a-0Z*%+C07&~e5GRu6A^`Gdl0Y z5N3Ph%Jqx~=+;22V8%IBh=c5wbvBr>wcSnm6VM#Tffi?j7F&fDXM+}BC8TpLFOcdw zgq1Cz*&Yh18JcY?H6(jd$G=0daLKb(xM8wa@l4-6&RO>YfPTe3`R7DP1MOM^r(<}~ zn>Q8d`%zBOU>N9k$He^$Bgyphz3vS*`wb?=xZ)m_YBw#+w5DIga)lJ}vg41A`?Djq z7z0~|bxdNM41b&75c~7OS@G;NCh?Ju;jy;Tcyubs{+lT)2;-voav9ksggy#HsUH>T z1K!$+S+|H6HbQTiyLpo0r+kG7gwb(twjNX+)Yk(Ha%kL*ZS8nEHluXmR)x!Zn-giJ zuf5lI6ZYV9P|v0kYWXe|K%uykEksMm#RQ-|80ULi4EX$^eZL}iW4M1##qMXlb4K^= zY$aeQ*RNm~P9eihMeXl|sh{BjgM4N(J{2_(ltIG+P$~`ESOM7c5n3 zTX+Vk+5Glh6Te9aMPbs+A2odS?%c*u?B&lrXIx9H9($6CtYc5AAE+PAo>4&<*|sg$Z)3ydrk zJUiO7pj_4%b|>PP5`On$UPHxq`2n{XUNn0vhj851G(kN?Od7qPWwcw5kkRY<6(y>M}Owp~t*>%17 zZ3tG>OLQ@Pk_L8Qa1y2|tlmcuTzN!f{@T8uF#xu$nvR)2a(_;)8qH5&UmKX03S{U&zc?1VK`f) zcMZ&8ezwe2JBRWp@CXQErYerXnA7r(lUEw!n8ZNorbbWhUrUMLqSo{=GCJ@i#b1x{ z%1T)5Kj(NSWga(K%$wrWRL?$Q21ZwtX$;YF00k+@nq9x5!K*lx#rqWhFeH6kS3S`# zRY}sP8al_GL`Yf(4RGCC*Kv}qYnqLmno55No;=7MI+6M3q6y6v|JhzRpV+vm)vHbUKi5oBjT`dK+B&si99#IC z=CX`i2c*2rN_j5WP9bI0*uqkea&tWcX zZ5|C5#a`fOu8#O^4}RP9+rcipPf)OE8LAw{yVO5p79@4V|4O88B&rK({w5`8T)$h^ z=92#E))h}Ij*Orr=L?w)3bkxA>j%x6=3MXT5^5%>JYG8^ThQNY;D%)(!gaPqVQ!sAoF9AiqYr>WLw) zMOKNkxG`#0M!V$kw3#uPmq?3Bc0L?&i0xeXu%@Gw@3d2ryLyh`BI35ojoD^uPRBp3 zG$}i5`D-c&_xDaoS1z5JuK(${UJCsSSDQ1{bExXJRY|+kw5hLuMfXYl11c6u=zf z%=>!VC(BtfKMb%TmF{C}r`y<8r+B4TP0o{ceMH{-UnQ$Stzg7ubJVycQR-^zrGm_S}I=>HXBivG8WY0=*! zrgJU-&C}D~a8ddj^wc2itoc8qr~X6I6CRI)(bI?n>8bZ`(v#(Md3rL-E=D9d4Qu*) zr>3iKoSdHdbC}!>2hbCQ6cCj~fONa96x5&%nW_z|p{Nv#z)R*nab^Xpg{UMf9hCGJ z&KC2)h4fP3rIjD0#qlq)60TDE*3dBN`zJ1+QEX}#-rgG-S zY*Xe|G<#18y;)YRU30Y^%Ee?V7<*U3h*276;`3{a8yxJJ&QqX$BD=}}j-bt!)Ua2z zj@d_|-9vf*J_DUD^AZ%L2!1kSqFv7p<6tib2@ZrQ+v#!&s4nK0GXJUcj)Jcqj|1BD zBwP|*zy9`kUH#5+e&sL6*Ufx2k{C9VxQgQu^zh17rH0KTBM>&p8Fg65>7BOdxahJc zN*NzJnn+qA2egDk_Kw0iIlX#b3JyI_5VjpzWx(3%y(h_x{d@2zr~W^kU?e`?H{cF`wN+1o6y(3XKO4y9r? zI|Gl))}M;qx3=>Yl2~`Td~B}4nsC1_T;tvbRDB~~ks3jEl6IuMpIqMrdr8g>du+CK zPk0I<>M}4)p`VFW_5e?xoo?xXMnPiU4m<&$W1pi^r>>!Px8wcKQvc+Me$z43DZesa z*WlKE;Jruw(*FMFl=l!2Gtp%y@ilYQbsR@DD)g&E;%=T^Jtw;2Y0`Pv5=#A6cMLQ3 zUQ4B~4=!2dk77T>NYBwS*kypMRjsr&JLk)2u90dg`u|jeR)5*ZP}_FG&JV@QrVUC~4d62EU_yDat) z`zD5wZxZ17L(?o-fSSXE9|2&N!jvZEbgmuJxnT%tTWBfWMZ2FzVPVq90+;+=W0Le} z7L&&HrocJnX~W)uomaE-3W|0xZJ5i;(6db%_^F=)m?%fluJ1!`k_;(P_Ip7m-aLm? z$=mw_nq)L0X*YJW89y|#gHs7j34K!>!OL{A)libubKox^0i$*{+McOv zS~fJojr}OX@6~(WaUs#; z<)XjvzUtq!YNt1E9=o>nZ(uKD%un{s-gty3ih}r?xc~Whb2`#-Udnqs5HZ9KH}eGh zsR)-eP!(s!KW8Qai^$<-4z@tYdRDZaNbdp5h80hLkc?nEdgddT3!#(%J(=>(u3zOg z&jxM81q29)&jK&u5v-dn`(H*Ed7%0x^Vz>@jQRk^GR8ZAw>?}7$t4}RN8DW`6Mme^ z5;3n5ea4cHKV(s*5+ni1FpYW=c_CTykYG~QXAOtZJi$zHp5*<#L4)aTVM z9ua_9e=XZK8(w@|^ZBHaCj7E$1!@Q%1TiuZ)e%8#a@xJCsB`RPlq~Q#4f~}H;AVoK zj~VEE1H3a??-uWr5ZWHDCeam_6RN>7({4y|4~=j;L%@-le+RZWgFV0y9=g`=?k7^4 z@GO_`>+D^eky_ivgS&VMwvBu-VGrTk*KtSvTg+t!%T_U#hxlM&UB9f{V_+015O9c&h?C_Rl-DB)KOtz&O9wyu%;9uwFrLlfBt>Luc8L@;4Ep6k`+ zTwWBY7IQHLlMa{gun(>H2XSAor1D7=47x*mhcTGdJnqzPt~<){j)WSABMY<-hZ|N$ zyQgaI*~$fS8C?=3(qU5efy(@ca}C@UO6!B-fu#czWS4>zU&Nfb*qDT>|9I_o zx8!Jm&&N&)KQ=bn-A$Abn_f)@Gn(BF(hA?jh*429%;-$59qqYo+{v?^a6Ew)p(Jyd z3Qi0rsF;22xA;enudDb;iYihbA7+O*LQ$w@X67O{lBOgOF*!xKu~3MStQK=7ys^l8 zv*{{3t~Oo9`d&@%W-l70mw1Y>9^j~h+}XSh0p!KAU+uNfwv7Z_j!?PCl6c9uh>?7v zE0*$5_B!;g$OEP+1eG~F)Aw`Er*X5mJ(}{rMnKf~h_^QP>ia+<{Z;B7zf_Vc z`D#t8_UtR_dxeKJ?ZxfoLaG)Hv4`gt@N^=5TXhZRpe5lKw^fhfI7QNd;}6Zv;JypD z7HA%-=SIBvFV2#E+C#>PrvAzV%*p-suY&#cMsjDvVlnr71%^BU1SC!DUOI9$GErAD zy&rY`At?AGmrCUzdfiz#s0wridku>DK$kU2ODKWVpIMO`##e*&>0tUVO6%DTDS!E5 z^(0V(tK~!rn^%zo^(2TYRqU)-hIZjKdM3Jv_*tOzpbO7M0iK;# zrxX5?TlD1s1_XIa^EF_hr5gx&%7Lj_Su z$^$#-7Peqp{r_Aa29MhqlNsDnej$ck{QUnx#WfHegtLP%Rcpc?jT~mA--GJ6$RBO! zxS=4%Y^zlka-;5sO7vh=H`M<>m?%xR6-_s!&xzT}o1G;et%O(<20uvET^6Xib~nNT zRhKy3PpG&w0D1f508Wc|kJfJUp5OgEMHZq{``p+X%Y5fm&yHq4gWRa~qKC(e36HtW z=(w&wpA?u~YhVAIQ~a>AWxree4uwe$W%)oMo)#*^NmJ#96{$b<`Bg@@6|_b96M3Ryjquno|>>9HJ=%p)=d!FUqEsYqZtLw`~BfhPAF`Z3Ndn`>E(54$F zvduy=p9YgUo*>$L)q#+gvkE<5G@;IYGLjmmaBfuL8A*!Sr-osQ)Dp+p$Ed;_NELpx z6dcD@f#A`G8j7!^O{(jD_++RG+ohw`Ba{*6go2$XrB~If+m!gaW@1TYy%T$M;Xlwc z`maL)1_3f#S=0IQ{@UlYQeU7a)YQPxV3lA3MFJ&w9m;4bjdaawc~}ZWLM8}AM5()>bgrlsi%rJx zFM~5}y@8qn-bf2RWk3tAFJ+Zn8m~Lit=)N`797iK!C#OT{D8r)I;7lYyeQEXFAZqH zVy5D`#6wB{(qJSP8Kx>NI6RhCDzwzINVTU+48*~CI9*#A_TKSF5SF+!nLaM=*NFxs zQb=Z$5V27&(wd+I+-~4AcANRu^v)n^koD;dr?`Un^RgHYef;E@MkaXGw`%p<&U>iD zV+T~?pQ$CfF&CknIWzGTmTrt(;&eVdq;nlg@t-Nla08p}VpBjVmZOJuu~Fkp#F|oh zFn!EwK;SZ4=_0>|y77|*y74J_-MF(vFvAY65viMjj_fwiX^v!mc_=TgiK3lLB`r{Y{|UPHTEY zs2#Ott50u5(?ZUyrd+aHoGgVq&ZB;qH0FN7$gINR{TXC;vz5n%nsYe+4rGJVWSBq7 zt$og^8x@*QfaqMoK**!%7czOHaKu&9NmPrbF%!J{>!ijl=A5aJ){Mq%pPZbXw*h^* zP;GYoe{`EKh;&e=<#FNAW?nP;vR&N=>&paW8-@9gP`y=@qf(T^(S)OCa+sJgk<71! zjknO^O!GbMzqb3$Q>BliPF464bqYt4!dTQXAt-IdFA<2V&0+h zJV(m2HSbVOg7IRy zLp;tIKg?qRJuOpeH{GC#XEd#okjV>&vmZqfnMZ_9a9S1f@Lh;5QHeG~1dL zA@NQ821+u;gs+m4j96>otF$pt^M)d;9}Db@^?0vTw7N>dP$G?i%^XpC%R!Xo(N6P> zNXG@1E5RjULPlAp9tE|_U^(?-IU-i!GewZXnK9c#e?ncph)83KoJA)TW7-R4-~RRE znrHqxlDThEHu5W4VHR&QtkoD-jlTRXqc7umhpirl%wK1SVII*VpVX8o&Emcrc6c~bKsbu*gXVw7QP#SN6<*(Uul_XqB1&TNa4GFZtIMB~g& zWtSOA0VVipI$c+BCy~Y4LlJ=jL-r2sR$gj{e^{X!+yb_1SQm!tCysr;?235FaCYDx ztofGH^P=3>c>yfWi(zjV&DZYSnph3NTv2jqB=&f8#g+rKUBSO7SEcXewOy&reZR?T zyX7fT|4eYE_i%352O_7hq3*tny8E@7B>UG6>3cP=?T!>NN!xYE(J!y)+H$SpHK(AYK@^z0x?Y~lTbE3t)7*j7Wm%C;)C`KJ6AKY4F8_Xb>{ zg{|pLaoIz)dP{f@U0?ynMu}a?FDN1qqr@&4@Y4$R;RsBNnt{TLg_?O`HxAypgLUe^ zb?xAO+MCpUJDWyfvONv3Hh(gWv6vGra29ne3gwkpfdQ~&V3AbzxM+s!|4W;+*e&%F^Q3V9o$xXXQPFjlgTVuFfv~#1` zwMc!9Z^gE>L~qy-+XAV#N4tJgs+MOGScWo0JlLv$wAaVIKPR!YAVN-)jwP?-#BB9( zu+EWA=U?{6YhM-{ZZjk1b0}H1GHSS-<9tlyU5`(iI?d6p4^e*PdTbN9?x5=k6y$|X zx68p&F4&|m?r!7+SxbyfhgzGaDen}i=ndQ!pYwlUwJ+}nr96gKIU4P2alGz0xAxDJ z`V|oNtBT^g%@3Ffi!Pfj9V|HMN^;^tyMnG;Hyt4ff8@OAiv3m-Gr=NgU;wE_J}4VJ zVjU-Pw@G~L;Z=3d{@U@hW~YA{`S2K4O(aJ=&j^(Cyd4c{bt*lJex3w_(nlr7Xkcn# zvpK)M`X+uSO4Jb=tmcGGHHCa+al=PrI3>;~uJ{dJgE3xx^F*)uw{hH&aomy8vJ=tl zb-W@4Oit={Eyn1vp+wBmH=ux#^m(iFFL{#L)lMY>YE&YifrE;?{$;fe?YBvv)yFtB z%m@1>6AHo%oRib70)}{bWr@ajpmXxqNO%U^;IfvG*de`m%Jf(lfSG``B1nvK4j~x ztVH?Bdn=&_=;LpAEAOSPgLx~3DKZl27l^iLo_jbByp>CJ7${&w#{b5+D&?~*a98Tp z!FpHduACm|UE6sg@KbI!hX?Uf{@JjTU@`l9e#$#&WpbgPGRoTm0hDUtCLKLgyO7ST zynKuKxbar*;1E8-cr8AMbH{uc;t9N!0$T`s)Tz*0sYgEHt?c2VP*1;q&s(W>59+NP zJ4p2s-~xB009W9yl;#z7?33=wsK_v2rqFXn?n+@>bVZldxH3q$H=AXI@tXyLc{ig% zl@C3Z_ZeZwpMOYN2`P7`{V^&a52ymGML z66fEL_V!eId)QgrOQjQvl`f}>#RvK=Z(>-e2i9--4Woyx3Ah4$aj@f(RpwBR%bGwP z6VRnlp~QrN6_&|JB|C`cQlCGVdWPq+RS0ls&*h2S76-9kG=+KWm%xcFf|QyjYyG#3 zUuB<392^<1-Rzc(jC&*fNM1|BZFym+rTq=JyQR0|Gx#*l?bt02?@jrbcS=^+p1r z3{L{e!HfK={FcX|!M#p{MY(O#xmMXYGqz3BUO@K(q<(rW8~VVGfuSuug*AINruEKs zEs0_57RYXJ-H+0edCPe~@1~BQqtf9!suII^mf(Zgn9^TG=MwMW#@;FuZuUo`h4F+y zw1XxF_F~m)V}N!%`@or&5gglj1cF`e)XjUWh-kCRi8k9{qRp&{9Yo`c7}XVGkBNOh zCUy>@D=y?ina}d=1t8f18*R2Aj5ce}rMRu;8E#ST3{LV&R!s0lwSZo^+1#_XOTc;s@I&2O{WtF2!o5I06Pz88F{1Wv;yP%yfS;ml z*(isQ$I{rl(d*aey<8eAN z7$wElQ|anESnz%scAX*sQ?eB~YQ5hIgFIo-M>4mFgoQFz#hi*(!(J<%4FyJG8>Cv= zNMOCu!yD+4JNVIGsKqdEfPFB@(+VG(%y?8v9+c5_==3sQawSr;?#_8%%~s zjIxs)<~5?m^7=D~;nz6Lb0ZzorUdrem7(f{Hxj>wUl24k!nie*Y?Dau3Onsrl;SE| z6ZR9YX7jvLu?I_ly@hX0%3|gBYxqVqo8w&0M}%w39H}^mO14_CxQTmwGT$suIsH@> ziL599p+zbDHwsYoM>)ku_~(|^9VZ06L+`Ud3MSsUWPaMzrytgrc5b?p6FY zk#6uwbh2&>&U=fs7Rvr>tIo%$>v03E1oehP1xgs4j@*gE;&L1ovUMLQHa}?wUrm%> zomvbLLrOL(+Ya7{uJ{DF!$r`!D}h4MIoQ?NWd_(ML9l<9>8AqpcYC!g!U=c>@isHe;cA} z!q_-YAe`~KWO_IW6y^)qwAj+@5%2+1Ke#8Y9TeA942S@lr4Xxe_tv17r+fxad zCHjdVU^vb52uZykHD?E}fO_J6vD?gxzen}Zrvz? z(5qovn?K320!?aW0ugbViTad#qsHuB#ptU-;IPdQtI4%SeHv?kgPZwusZZw?a4*z{ zG4bNa8nwnAB1G#R70Emh(ua(1RN)Nrbl~vQ4Bq<{m!nItC|y550YNoUJ6iQYqc=}W zVPYGh%FQX#{;M!&Iev5PhfdvT9=O^J?uH*IXzV+NBCYXBoJt@hEAY#}6Y7DCw#ADp z@6Y{{C=%YO+aZ}LN5K;4#EP zaSGr#2M}g*f8`cN&tdmT;C0DAK-x7?fzcGS!ejGtzd)}E){ahmsf7M@nIQh94Hoi^@~}1*6Gi!_`Ax1?8YRMp~}j9bAe%oO#5dJd6{IVmR)VO2l|DGPXqTemZy$Xf<`dTg(qc z$)-rA5rDdww!(?ALhVYf*t=^6-96Z~a1Y7gBM*s1Q1;9|I;+iyXl$r0JyErKGvGuf zkiuZ4nT}L~yc`w9y^D{+CGfZ#oB)|y)9pWEW-yBj>(nXi27c3TsZj`f*@&=pCBd^* z=2L1bvo@53Wx-#}^KOjwDf6|sKM_HM4@SpOX8)t;vJZGsdg$DKmIAF^8MO)TW3@px8LE2; z7Ag#;#BgxG$*FxWUi7c?F8I3-#TKgR-H zOwJ}2;ol;e2i4WN;GXGrfal}OW>hD~Z^DU_oj9WAbE!@PM zyR|RG>qZk zepF%cD3g#MQ3VwM6c&O~BB+McsUq^<;lv+b-nl2*PFXb`GP%-l?Si^c-2K$0_~H7i zpVSN&JZ36y6g>MkZc#rgUy9136_uizMHj+SRHqa)lYg>n2H-bydSE#!S$LWUZkAtn z3vIFtvAga_bKG}3jsG7NrTUsEX#gY#jx|^Rx9|6@|HK;~f+fLd4pKsDe&Jnl-mt)w zj&q0eHH(O$J&1ZzB;KK@%c(~^0HHsoyi_WE0hOiRH)*QuS2_nGt#-2Kmc)zSX)S)$ z@t#u&NOny<*1=-Hc5LIanZxbQrK~8Tuu}A-JCa_UN>g!gJ#$mpq-^_+vQ)OEERl7u zDr@!Pvy)y2m8R}mOxKcLw}3AiWbs#5U@leEXsIbcPVkNTQzbkX8nGyysft(_qWJ%E zwP2-(OOO;6vL;+GTS6~f)7GDg80`pVb@gH%eF$hUIc$5rA@H`Ixxhe{8Y1kK8js~j zKkK02#hy@q#i2@Ru!>WX=|_u6>Tx0*)Q;yWK@z(=nsnxnaV?9rJ?Y*u;P%XvaF#H zC3U`NC7ZO))d9b4!S4e5d&Oe?@;7)BR^h4sxO$U0Fo&91{c-gUd!$1tO#0*SUVEg& zo`FLRUVmKOY>#v(QBQyWo3w9%kE*)fojgF)G!rx;Dr(f|A0%RcCOgprMTkN`f3G6VjF{T54l!uh`m(R;#wQ27K_S5AapA*5ad;h+4o0zUtiX z+mAUb2d4es-t)`kob#Qv*Is+=wby=}z4yW8E}n5DKg*iDu>Kmm=j|rN{;>bdKC~GJ z@dg<@1eAQ=hvqCgDM9~lw?{BK$ZuP4oRgo^2q>B0n185(0G$Xl9Pi419_NZtn0fVJ z{zyU?3@ucTFo3Sk9r=0cye6NUU<4Ao{G*ob^N+gbP5-FHJN%=nH~L3?`+onZ3-0uf zD*v8;RNw}DRE6C5ISp1lYT}0T9EPkIehA{nWhO2QDLmxlM=({g)IZ@8u#0B~Hf9Y+ z6Eq5yhei`j?!un`^qi#;pVOm9VV67y8?k<}bSO5$VHw~HGrf@<`QuYah(ZbAe8{cs z`B0HLn^=NW_N>-Q>iZg`*e!4ASm|8MuI?gko%x`oX4J3A3m!=wJBau1Yj zi-i+!|L1J^G93F>7)tvtvt<|Kh_f52_qN9y?;(IV@TN1Sm0E+pie=`n_^hz$TxZjY zz9C^WJ93;%r1bc3=Uf1PQexOoQ&LiAEsSgd!mD7r|Zo%{! z6|fa^;`&rkK~10SKYK>zwbQuq5=Rt#kHWTm2!ASdj2%re0064YTT1+AXWnuxK5veV zH1RQ);?G;(A{Vr7_~eeDzii{4-2p4Wo<}GX_{jIpE(Z3F2ok8e+rIz z0<*i)ombT^oLo^f^wB-~((K;Qi(zc`UMS?S_#O>G;PL{Ce#KuS*+vmo@Uxm_! z;G;wD$1AWucANQLQS{ZsR=nQ$33kkmUGx=S__yyF`>C$pi{H2&-%fZX@icnUw_xY= z@@N)@-!oU^BhPxe}*AF5Zzi~pD;7b}|*z*p_@ z?Ii5yUWk`s6A^uX5ni1AoGUsC^}E85r*FvaP|i9b{YZ4s({-u zlL6PG<{+A$S>gB`_;|dzd6>-u_v6^k-!45=ekZ?wW{tQ4JNwQ;t%!cMepSlhg^8@W z3RPo0cD)%%s=&vH@C7mKb36*4bvOo*p)YXeMtt>XJHL9Qg@!%vhdqRa7{N&EGe^iP_uO zL^(DzmM%i``JuwkDNK^#`EU^e^xSm1`Ro;6q_qu?mn3=;Tb6#snGKc4*+ z?B*K8Ao#T6T-50^w+7Q#)xV3+THh8p@Y%nK?<6KowHLOa>dj(%A*-9M0@YUdhiY;B zNBJV2_K8SO3jQSn_$Qvnmnd6~hCk&P89uVdZdO|2`DGtypJCb~L)CX6LjFHyj%E73 zX#J;cZ~TY$3fd2I`r7bS^i6C#EbKvhVX$toe4)@k(+ z)s!_>H!PZzcjbcW=ITS>b=O{ZWwY<1x~uEzudB-|oxQA~W>Vhi3y$!W)-P#jtZ8a8 z;g?i5UVSju;DxTPS=QvmbkrVxwbfVD)cPtLm)2PcRiY#yL_RnX2GQcHkx2JRMpquZvE~i`Fl#Yi{xdYHRDG)y*|?>Km_~ z)leNpmeee$xw5gQ=8(^pRJTOy5%D2{UHk9CH7u!a_LVl)Af|z~LVfuv7T1;5Ut7~S zDGy{d;&x$e^_Akbw&q&oq)V++?B3TlH7B3W57tCsSM^Oukycja9Ani>mNhrP6q~e- z&T6P#Y&9$FK*BmZD=%Q?l-Dh8o|LCzMn2asfSwNV;0mSirlhqu)+}6HSF@z1t~swc zY4_8OJVqMS(Kv3ujmM+k8n2E$-_q!3$urywj(>dX*=KmXpQQV4?yt+<{7#3V8#7gh z`?fjnHF)Z?+k447)og5csNn(yNJ zy3$fu9-WBq-#n*V`8Us-R3U59f>=6agtmM{f^z5<5$kUjBi|NR*XA`;H#Xy{zD{HW zS(P=`Uzg-C1MHH8xzY7!(kQeTXPu|LK(a)Pn^V24(#BCyv!uRp*`z$pLP-n{(A3&H7%yk~+T$6M9;r7-9iXBnJ8vaSVI-|#kS`x}^920?Ll3GMUFj<`Rv7)0 zu0zmGbNL3chp)`&Lg~r_vC4lz_lT>1RUW!C&S!ZFTm88y&;qOc7vTfu8uG7BFUuZo z!n@_MlomQIyGjRF@fR4G32i!DKS<_ZQg>z9dy}@~=0EMaR)im;gZqeg2z-vgDepRg zzvjxT{g1X6$ctR3u`7AYvNst0vHZR+!ryB6LS7c#;qeTVUlv`I;4sUth&R{hmXP%U zN>AGFFlLXQ%Caj(crG=w$3`QA){|S#=(J!d#t)pI-Dl)xy4Q&C_Zvnc{-maZ*_F1C zcH69F_?JX@em|XuH-4kFcqa4~%y#`A?L5~i1J`P|039Ay5>P%*S@_D#u8erP@{>V9 zKhw3}a^2d`ApQO;HoZlxPe_;>5tt)7gNMOfzVV$L zj93|ESEdP?51NlfxQA6Jyg14(ykuo&JiRLeg%=0q8)51a-d~&|AHg4a=$5v)p=0=Y zB7Bj_6E`0srv9@WHW-h?*mV5sM0o4F-0-kpZ3)`dAFBQn;n)91{RXqEXiYkLoqpq^ z30-+`cvB9n{KghV_<17yEB`S(Y>+7t;n#`qKXJ>yun!wI>wR0}X$H*a*G2dVZv1qq z@T}Ue(*I-{U&fHf^B)f4BLq%+<3YSg;3p{*v^Yluo@ektcFqbGL58ap&-a<Ln<`-(wgd{?+M_0P+v?m#Uqp%Wvy{5svx#pa}OD6Cd=9`f^3aN>pZfz$n-2 zf{v>%ct9?Aaq1;(^rG6IA>AJHsd_sX8@k!!-|?R!=>A{$H;Zt8H4z<3{&j+G*8hY2 zFNttNOt?ew|4Pt(_P^v$dn?Ma+VS~bbn6$>4zc#4^y=1c!Y?+9@Uvxj)ef=YVXw3| z*b7>;nYLlB@?V6H8Bv1SQ>Vu>XLb!*F}PxNdUpopd|l8zJiz}Rx>V8Nw;Op15Prlr z+;pDk>c_PAU}q~)dyZT&s4Tls*;_==-|o_Pr%zYq!1Na48Kt7VLBG%;!Zn*HN8r3p z;6E}ryw>?E(^q6L++*VT1Lis9eo5fJapgd}N!Q;i2ARqmw4~3>Hi2q1Dc*LmM zwJQcqLpm96k)Y?BZXV3R0+bZb)=F)rG(^|jO$Gw8SO>t+vop=5U zw33dP#@VTx0!YnRFXuCYkn@;pR+8xi3@F|3py)a}M(yUq3E z1KW*VB78{J1D^0@Xf3)tbVMrsBK$NrJmh!Vow^o~9Nh3(c#&+6&y6l2zf3>MGfHEI z8@ue^MMU`5|BLWlB7Dhz5&jtw{wm}950+obj(W`KiTcYLYEzF1IXU-*1eb5h)&o0ym4;st&Y4G<8=O{~OXy9tqv0u&ev9pX+diZpm?FZhH{poS7Pt{d zwU25AeuAM7X6sC&{<_5Tr_A%R>|H7+$oF3H{Alx>_>+gg$?tW6+w{U;MCd=M-><~; zxrQH}U&vm!?4Re`$UyU!&b?pQ-de1X7e2Rw+k3j9)qYJ82rhYS2( zfirzPRQd&evW6A@y1+*Z{9}QSHFy!uS!8AoH9-T{$_`*js=qAzQ-+Q~f+CzXManl% z;5QihuxSq$;l5Qo-)Np&yEFasa`w+d9=m%`gtPfd=ox{JGyDj>BXE1k^H=r;*OE`J zdP%Ptg!M`3vq0dsYeg3L9BJ|>67+f*jRs^t0iRV&9;cWq>nEn*evNo;+lSCgiuRQr z^|T!~%JY&4Z<<|7{`Ung^^-X)DUNFY@{W8)2%K{9AYLSJYf8-Th`=u(F%Fcc8MeJr zE1t{p(IIfF55}`j;IiC2C~#}Pq<==>V;DIO;_nDt^7%^Ol0Mf8hpUH z%j-OWOFqp4AFsG;{#^n;OW=12{F?%QOyHKkuZ-_xMwZREXjI<#G2_OJ&l^`*IKJ?l z!f|;cFT@*zaCLKDWO3cn7X9?BqH&{qzA;V9mNZvifpc@CJ}1JgSCe<3~rhmni&5JzD!;YxbVhByCu*6HayD-<+9}-m+G+bU+ZD4Q>DX@ za4LSpM3spg2QazrJ(RZ}m`t2Zst@nb0BfY(R6i>9^-gfFDZ!1{410R$vp} z#%tj^6W%oI4VBL~JWW#+CJ}vM!&`VUF03pT<9YoJI(&}-ng1M)vEgn0e-{`jz43=l z{M$uY23ayZuX$XH-=X54c%zO#&-D2iKfCqb_&b5wIt!P{WB748d=fAhUi62+y%RmuO_A#+?H3%t3+KKc2LfQ4esSL!|=&dXZPQ1gX?s- zIQ<%HC9nUfGWb+=cKTvNuy?7WmS4c^eL3GKM{RD=8O z(YPJgyUgI*=V{!&Kdm!(q0#?b6YnyER~dQi{1LpuJ)}OrZ|L`$0%_;JtT%Yidz$gj z4F7EgpSni_b{uMt!8c6QxSikdxxssPYut{99f3-kdYd|5;}7eT?|6e(U9N#w3_ix- zeZ3m6= zj}7iKURykpFs;1UV|5I)3|+K^{B!3zNK;d9_cxQ zSG}+C)%xV?2Yx8~|7QyPvlRH(DeysPw^5(l_v#2u@$h8>KUBQAhJKf+zl^!~PBnPL zN1D$@BmcPupZc)|Dh+CH8G zmBI53{V4|TG58dNpJnjv2DkOp1cUE2xN*C_5`$NMrWqV-_>>#G!QeJsl?LxMc!8mx zYw+y`A8YU{4W9G42F4h?-r!RWZuu-Tc!P)j27~t++}1BQ8@$hhuQB*8gAX_H{>b3I zFEn8J+->kYgO4}#8w_4$@Usm5u)%u_{!N4T8GO5kzi&xR^Wvs4QD4*IYp^V3vHFiS zOZI$ib*;J_6Dh@jc%<|k{yUfdFbWP#fn&$1>+$NpdN>w?;*rvE3LjU@>+yvOQTTX8 zIer3>BE_*tvs6!u6#cm>?71qIb9JnWW`h2&!cI{16BPYK#eZUvPpwv&&!rJFg1(B1 z;H)|2vrEIi`SZCX=ECZ_1@%jCvv@(v615Tm*X|N4{h(lQR2 zKbAxrmH{|)Tx2NMq2bGRf0KU7C!V)X8;3Ru1SWd8t>!$W+s5h$t;_mRJ%u#(o&z%m`S;m0wHaFaODbf*Ry&> zNwTD2X>-j%1fZ;5!pQZSt~wIn?n1TL$x;KL?vl$~%sn3`V^S=VaU@~E-z=JuTwrO} zHF3$Bn{cj0bW;zmplPaU))$)Cs6yLoB~~{gVR&9rw}29rRM%G5MF}8eHCDUf%3a=- zyeX?`t`N^?qRD5$n%Wx0Qir>!0dKzCr{#4`HH{RgtfsD}vAT9(qzA2#1EI1)v1ks^ zg*AZ%S1oO7)(d|CG*!1K(>^HtjQWb|rmI15(3ANyE}oOb)xtvDEi|gcLOm`f0#ei1 zSktnY0#{TwM;Fb%up)UmD;#ofvL%$&tsu!$9j2v2*kQBJFvPaV+swcX&k z*4%|P$*hCpn2%S23-HV>l+&+M*5(4^@c82TS{J~*&S$K8((yIIO#u~aE@v&h0*JXDX59mo%Zi@D5X-?g*DNojZKTMWiqtqNFq`T zP1>tBHASnHGjq!yIU%g;4sat2#YGJS8tbpZ+Q5U|L8@hPg$Q(z-saD&tC_WU0hZHh zH;zJ$0!qD>F>4Q^XNDdGhOU`|1t(jV6C&zSA7MQoV5BT zI+1a{CS!PpA5)8n4qBE=>Akhup)iWC2`g%v7L6NF4+-3@8VAz5I2p}RPBd^W)(qlq z;Q*g%?u^rS_(_yP(3`CuYETBb17h!q2YI`iU#_Bqhrn>sZCDcRH zf_fEAQ$ziAysxdl5{>eUOgj(5kG`|TR_58JF3UnJ;<3cN+&?AuuS+XQ};!0!|IIRaPP)8d}- zawyL7=@;}91kR1rc5X`ZGle`c&ET83%oxCUXSrh9(>Hckd60!flE0* z7r4{|$2ED75Bpk{&zT0d`D^>c#e!buZ@@!u`;pUA&|f2P_AzX{F@ei;y&`a#t`m&$ zQU3D;pVI{{^;{uvz5=rGJ|ys|0^cuiX@|eSI1LZR8^F)<`J=$4oZAF0%NNIiczE^y zFM&&Xj_X+Z5?t7L`3(t+%Xn7_T;}gj1upsgR^XD)GXgIa@xCSSpuqXfKpsq22tO<5 z5f~@nL0sm`87c6a1upgftiYu_#ptKj9^kulW^#4cTVS)ca;8OoPQsBdwP#lz}1V79F41r5NB?2!O^veV; z{oxA+=ZX4UgddN;3VNx}V>!tN2jw{*KPyjxzz4`9@acm75`jy9xIo}i{vQfl(%&y| zzS6Yv{MO*!bZr*&GF?Nm5EzFyT_*}$rfa;wrz4z=HzIH;=fwh->8ciZMDY2Mz$Kru zZ%|4tC&v(Nyt4!@^>By4XA1g91ia*;nmVO>9KOBtrV*D)LBycIudnxewR4xwkk#gQ5 zaLNCEflL1B$0#<6&m3G@d9D_?Oz#hdx%4t!I|VN5i#f--^fKOTGjQmYKR*RtFYs>* zIoAsO5`q6x;8M=V1uoMYIL?*xQo-kcPH^#g0-q>wsps+(cwFF@2|lj~{BnU8n*mhn zhp*gizFd<6Kk7sf;qdBnWD2}R;8LIW2wdv(F@Z~cJ}Ypk&v~W+MtLs9&&nUq!wnAN zQlEF^5k0tE+?S_dMKASy&M7V~^|oB#Qg6RF)uq1zmsXztInBi<3w)8lW%>G!z@;8W zF4Hwn z;8M;&fy+mx_d5cY^1Lf>$>;gATt0LmHedD#T&DMJIu{(YAIbm5LIo>aruQ|0%Xo*f za>YSDQV*vIT~~B>CHGt)3ZImCcjaaFUR1NlTlgv zDi3`S&v+~r^lU3x`kOuUmj4<-FZu6ELH}L~`q}5I=#%|%%fH&-Uise=II9RN=P?sB zAJWVGDi!!*LI2AXc-BOhkEFjj1wMqGWE@ui^i4ec2B$n%;b-M67C5UEi&qO=+C!Vb zuNL&T3A|R|4+(sUz&8uLPT;Q!T*iAOZ*bUj)$3c|*#>92h*|lIJoGCJ{j?PHGd%RS z82SZ*UiLdS3cNwYJDiz~gYsO1pOt5{z>gF7bq2TT`URfxxIxfMdmcPRKPUf-@U#4f zeDl!wc?P%qAJ=5QQb8~IUoUW)q~(8?hmV!>c|kAvWN{DyhgZ(iQs5UG+$-m$f?o1} z_FJ02rGH7^`aU%{M+S`ff@=e|3o_@W$Jk0>9VbUU?oA^irPxQ|j`U^~>Wa@SQ2}j|6@^SX#UK zQs5^D{5Vq)FkNgaTKe+^{%wJ`3j7j*-;n};S>Q4s2XT-E2ji9Px|34i;{<*&!dN+{ zrNA#qfiDwyli;&51-@6{a|Hcq?DXQG{FmZqp_ z9ON(Y-&81A;n(2O^8d`>*4_?iGT#w1TzYA5ryJbTAD*V^&K2~sJupY$CyIE(Gj%x2 z$Icg?WpK(b<6SFoX%E{3-Yod<@$k3l%3&uThnN5B0+;-UT;S46JRtC;AhG(a75Fs* z?=`qhua#%LpdTyfkLBP34$3L{j5WCB!>?QOs1)>4|JMkdRhE_WJ03n(o_0Ym<=N_? zxB0a*1^o;=%t*md+7P;F&_PbUds8Qhkm)C|1t%AJ}U_v z)aL~Jteh_z+$-lkK`-U})I)FOOr!I~;m!9I2KVy6OVCUH_ju?n|Az#<)Wh#Q^i~f~ zr=V{z9R#l)5(c+&+ITZrd6;xbIlmUTwAIn;|098)>~rNjz8W_;DF1l;to-Ey zmww>8Dez8#OFz(i1-Wbf7vOC9pA}WGikC&!;z5Bo3cOw5lLdaOz-769Lg14AZvw9p ze7+F4l>d7RK!n5Qi_OPh8=QKRV!s(mD8ogK|nf#|gX&&uqL!0+)JtR^ZD8{VN9d z+SS{FUfR`e552XkPg2llT!qMRQ2tZ!vvRHwxU`@91hR{v3V~OF!pi?QfwPHc@ec(q)AgmmCH*%zn1zG! zO8v|eI9;ISzfj<8YFT`Rz-9e%VjYNZkiWz`1uo@TBXBAIodTEizY(~U|A4?*l&zd; ztn6`69x4Ax0+(`jr@#ksunY(JNO^JvF6BQ(;F5m4z@_{*3S8>HOW;!edju}!UviBS z^C12IL*P=LPXsRIPp5I?@TPZY3Op|bULiC!&@X%ZSAwe(s z@AuHpG4gyR=q3NduG8|6j3#XDp{PaUR?Y^)r_A8ImG)EVp>H?zmk4?(XVgRAZRnSz zpug5be}|!8o`T--(Er-d{~!hZt>4kFOMAOi z;Ih7ZTj0|E(|_*rk^S+Yz$O2g0-r9@wNT)aPou!4{a-Keh~RTT;8M;Z|HqY6;>V@H zZ!tK_3;o;hG#&mSK`;H=CJ+5oL;qX~`rq8`%5x(wt^Ir=aH+TOdY4}6;o=ndf)sf0 z9+!{Q!$m3Zg(>j24er%X`Y&AmQa^r!TY9UX6H?G;{L|L+T2^0`OgQvbgdxU|FP1pWiTKkHX+x>$AFc+V2}B?7NZfiDoa%*W*^@HS*RK5kBjhX=_&kAMm;zrUaH-GM6nIYx{H_%EBPsBI2weKlj|AQ%hh8G{>i_0amoMO6!=Mxx%AQw z&l9+`!>SZ`v%sYt_N2gnodVyM0{=$}e8eAI`RC!%mZNi0;L`;z?QMPvyeS2KQwsb? zDe$KSF753_flIsEl>#6AM>oCF-UdDH;vk7Ykh4;YBI%rWE+<6!A0TwL122XDH#q@VPb zi*qQ&>fz+KUHnXeKO*q)0{@r5W&WP$~{jEYjnf)3+5~pwIOZER`gQw%} z2K;P(O%nJI1inzECnZTu- zH7W2720sEkhTvz%6W$Q?vVU~L9uVQ6oYJoDOo89 z2QHu61ine&qXqtf!I>_~I_E?D!_j5V#BRsW%JV&e|486R{S!AhNMDAZrC%WMbprpc z!L2^+{uDO~`l|%}_XR!U_|V*6WHN%7)Wgyg_^ACL#6kLH_*pq8rNEbeL~3{bkA1J& zV{r1R$ItS~{gl+2zs#@849@fp!tY8mURNXN7Xi0?mI=H-;QIxBv%tR)xYWa`pMeMm z<&^ny{gDb*cm$U=-W6FcK11Ly3jBP5uQE=A{3U%wzRTxgLI0CcE?y(>rA#Ca@{#nv zW1+&~#hU{zF4Nn##Kk3_?f1$Ad zxdOjb;8g-Y>to#DAb*)(Cw{8ngYdr{aB(TWM%{0pOzqbQeKLzWfLcA9aek0gGszF+0VCu)3y2Ujl#@My0Gx8E%&%+%p5|C~~N-{8UR zzWl2^xc%OM-5=cYvHSk-^3Ydap!p9s`ETj%KG$um4?dO72}j;x`fTI%Rq6XG4{rCr=03+fEPd}{ zeZR|t+x^hd{EdR<@jfXwB-M`!V4@+c z9^8IkVMw-4pXG1AuQ1Vr+kMO{J-Gd@!*UO9_e0;{!Ke1>__llSK7;4@2g+0RQ+;3Q z!R>c4dc6404Sx@Azekc|Dr}oxyAOV)7yrHH)8oPI{^{F2xZR&UXXrrr?Y`=j9^CHR z-r&LQ_eFX=xcz>~77uQ}UozyVf$`dX+oyVP`<c!R-rdGH>Cf9ApM_d;@V2d2w@ezc z+da75?|!!jxBK1uju|MY-S2+52cPzhV+)Im&M7J$H!d&loWgT}=>KC2 z3(q~b2+zkAO`NDGtBz<7MW&^v(c@U%oTu7X_THX%kFr-j-&A!u6=R!6_yThR7Y8m1 z%)V$=_=fjC!-KHXmA`@KSXcgT@C`eU%u z=wlaTZtlq63xq<&&U=)n&V(~Nzol*VAKBB1#5y$R=;z_sy8QJZ2m|WFqwvOe(*w@F z_%-Q4XKNr}$#~@RSKvA}F;M)ugNwFDgAkO_#G3P4IzRQVntul45pk>S zR@=^bRL2QoL=-`UeFzkZzE$ev-WiBZ4mexm=OempVBEITG7TuP1+zz(43PL&FULhV zc5nVfCHI>AsXVU@Z`_q0jMWbE1!G0&rSYsWA!m2Uc|RDxCJ`$Br2VAw=vRSQ?&HDY zZEZ)CNB0I2eE}!;;XvZaQ1QoYZ-qwfj>I&p2w3+ait^~If%pY!0cVG^J>+EG9+-TQ zuk9vhjY@MQc2_>T@DXGZGXa9G!ifj9Tc||SD#fGa#Bpp6=BpO5QcPfTW`|1~wBQv)9#yx=iNe#Lk4j`}=sH6dFc$vwzK;3*#J#zQ@d@nwXM4oXa=Gca-k&=}B@>TEtL*&IBJXI~(8 z9(%$w_3ww}H%v>f7 znNtxhc)TPL{FRTn^p2IBA|PvuzfhUew$5ZqN-6xSnK_oNwv|}(AYbRFODEfG53HDy zp62hk;WVvMid+$W_5675kD2HdQ*zS$-KXK9kbyZ(IbkVKDWeF*a@U4BUkLci_XQFg zSyD6}bhflDx27G0CKPe!R)5FCr!wpAQUC>P06&)e^-AD1#>hL23LjG*pnT&y$g$w* z6b$Bm+_cdnPQ@>eX0(gYj_0NYW2c5X-)Z?Dmaa-Z!FestAt#6;%YW7d z1vAp&WR&)(wg+vbRD3TYq(##Y24cg4qc$Pycj9L9Fn?EL9>COYoZj4?r{_8W+4N`7aerJNE>q|CA0?4YwFBRk~0r2WhO z_JZ>0`z(@76U*TioGYRu?t=}VA20ezBszGSleHo-n0EXU%S0a{P5KwcQ2d3zdmhTv zKw~x@LpTv=S?K%`#N+d?<`)^MT9pJQY({#&Xl9#hZ^xK=M%W z{`S+Tbc$LYeVuw$9zwaqa5Q&q(8;=`?XMJ;I5o!9mq$ORt*A1M+9Kp+O@ZJ2CbUvC z84kBBo;fBIZ%|J6g!1UeEMJCgc{J-`q#q$}Zu`5N5xNjnL^qbiM%-AEF=9FVH7t3z zN?80P>hW0$M>%O&G@sH~?rN%=Qbgh%`R{|6X`wo({$0eKV18xTY0jS*b|U#BDG23< zem@4aOqSA`)0#ho?0XffHKqh=SIMZhFi>2}6^VCgCWxBWA4XK+urn`zY6NTwBdBci zBF=*R;bByjIT2@sN)5_tmy$G?)|I~;!9t^+4UKv(ka!9y_IDqH%(s;fDakm1B{#M# zXar@#-#zr}M1uK2nZnUE`O9(sBYwWIec>CPnu>}k?EI5)S5b@5h!$Vn*n%#M$T)?` za0-_vE0^^~mCU?I^abWXFE5SlmT!CuEd^HHs3BDUL{}QB=?F+QwP@F{*S7m?Ax@T0 zQNgXEh&Z%izoc*IS32BF>td9o{?)hO$=I!9KeZk>65U@GE7%a~{GjDBc-7AHMj)Se zz#hn(61PBd7;fjM6og>F$$ZS;`L7e;EuH6rNE;)g_E&)twn!uD14Md0Ds!4UeQ2y0 zzqGBb!zKwXo-%`uX^KSGXOGqVdpuN=koK7Yq2$D`pjJHHtvy!-CsaIN_S{^()a+1GX@bDWk>8CYPn zS4PY|j0#5}HX;MAVQ?T`dwe1gKR;2tr+sWB`UK0UDjVR=fMv1#n<}F3Pm5*!Vp>Ml z`am%%u;UPU;)&utZO?;Ym*(dSjs3u|hfTv$QK}HUJo;fUmVXcG0e|J{)^JA&iYQsH2f!k#EwMvogl%(#VJtK* z=58+oqS9emtweqjLSw7}sXSZ^nkqp@Euvo#dfwcF6#RWM*Wb;~; zf<+h~B6r>ek^FaXj-Z>eh=HkGra>upREJV#Xw*~X8~=e$h^8%z&l{OQZBz(@A4`zd zhyFw$o;xDoyo$g)^_f8<~N0B)I#Efg%-#-KZys1gRVRIx%poMS~R!qE?% zj{?re;ds%n5Y`vV8W)Jwp5u!LM~o;m z52nY9?qlL;alzs-{*DWVq9i`$d|v#owq5X3q2hh2u2?|JMCMXtXlOT2>r_^UW35z) zZkGq0Cz57~YCRasDn^YysVttK9f~*Sqs8zHIQmy}NV6h}CQO%U6+EcYgbMjqvZ9-) zi0-D*G09fp=x(77F;y=u5Iv2dcw6@&Klz+=731q|+k%Lo2L#H6{Sc$o&d$*~{^E5?4EQ24@ORC`!;rHxFbW3smk`_+tyw?H z!6QUX8w?h|VKs2TznZT9$} z2fqQvEGGp+(BaQ2iO)L=rriXa*~5-=Q6%~{bCaf58q2yn65VlrJorOjtg!Rb=EDPt zExON|cmf5X?M356SQZ)(ARL7Xa$|7iOb#1q{?1W|FX()9;KdBn=`G&r?`8?5{K|vH zkWO7L{axo~fqJs8v;7@^AQ@^_I#X6oOm2B}ADaQpM~ECNxH*tfu!>E{f(P1f zYxwf$dyJ3CV}T((ZYdE}%X7df`UM)ftDtSAVkh$m6y+g!;)~BuL;ki;4M#sB6Dljn|B-74R}}rmm9FMt86vDxu&< zXbrkj(Vm@b_^do$dknI!Xjm||#Ao{?!C2<1U`A#P))90<5bT3?NQ1U90)(TVB%Ugd z!ddhMi$95FKI~-vr0p%rx(ZKK?*uJC8bt?kk|}%%Qk6$vqhKVVF;FkyM}cSzJO`ZS zAERVv-JG|3wcRt!5kcb(}hSVGI6$A2qxQg}}+k&=^dU~`- zb*v}~c!%K%;S~xq3JMd?l5@d?wygvBY)1sJA?kf1!Z4?m5yqyhrbg(5&^*J48+zIg zftHTu$e{CHAcLLgJ%Nl7M<|mi{;0i~La3C*i-v?Vy7a(RbPr+5lX#*cUT|MU^djGk zXjTuotBk`_Cco5~0v#`b1n_x`HIa1>Y0+Bk38rP+N8}%j4GpE`-b0Vot6jCfd!ni) zJ9M`WEVqL$q)TDeft?T8Dw!6`92#^s!Jp-z zfYWQ1#-G7;vV?!}s7K&v)8tUv@n#&|JcN0%VqED4N zv(aN5N&!gCYG@lQuEZ&F+8(6{MA6R1=oqUtc&w~pvE$Gg8fwc8*?drn%ETEqaBx>_5<*EN`~2rUlLN9f=ohh_Gm4NRrRux{`#=-$(b%0vX0ANTn;d zE&xFclFYRl3}dh)9FFVyiTyH`q@+heXJsnW%LEM=!6D-zVP^{ZN{1ee+k01&h>Y5=obQLsdQCOt_=_OyzNiiRTaQ4IW|dd$E#GVuMl zMPT~>is+vbe+nf2gs4`-DloK|tr`XX?j{rpTR?`OlJ8{in`Vu`K4y=vZSwCR1$1kg z2Cl=)_gCObOb1ujF-w- z&L|=BT%5xgz$7E4h*5-{H&n@FiNv7g?ED;Z+M+^gVK~G&B;w>Muc-Ji9=yD=#iivK zJ7{TJ%>Cc`6cXL=1Ba=fy)Ay2j;^w={U=y`&B;xO@Iyk?|uRkKJZE~<3ZJ1 z=Qz+V1nZwfoQ>TN15OrPi>})fnM3>ce9YFYc00`11|}o=O3=yuLH}GlV^XK8q&}uc z=AbV9%FeuHK~j-Q+eLQ=eAx=+nDRS3!;=iLs!;h2kjmSIv-V|hQzPj~FyLED-4$pJ zg<>aJ&G=UxK(1Tu*sdA$OSgl%FTC+>zPdaSU4eoT)7aMyp%L9b=p*K*e&tXeMQLQ8 zX~Zr4f7*}4ML(k7)KwRJ)%1At!(SOMhTHyU_8}}roz^IE1z~x-`Bq7|3WQJ|>EP(i zu)DLeU#n6;keq))(21aKN3$G10k>x{>@gE~*4Brh4N72{Qr4%PN7hp#BV5GIVf9_tb zUPfLKeGd-z*L$8pN30}PduygM#(B2?v418K<~{QF8{1@OFmil7@V zh3v_K*=pn;YUIazqFmASU3vjs+D=7l#b2ky55$7U_>u`&Arf#*IJ%FCCTEiXC+lG) z{|O&ZI`l=g{*bO;?0Ggqa$U#3wcOv|^S*K|bOHTKaZQEM^@O84&`a&>m%%3e7*tv$(+7;yDFp*`@`o4-VPnXJcY{hgP*rv?8E5eH&Ej16$i%imoBJW%{( z&_7d;Rbl%T!FV|u z#IH~VJ&ZB238Tyy+|pG>d2}0E+|HC3pD)65w!G1|NskS6ax-Gb#iu9y_n~LLH5i|f z?mUM9o#Ots4JH;-0I1DM1_g_`IG-P%!lnjVAXyV*1(QqS&EKG~N!jWk+ZM`p72E0v zw1t6DFm*f5iUvs`U2FD@e*2o~=^n6|v0qAwU_Xh4im_7q)8NB_M(x6G8unz|k^j zwa@zh3LqSvm*2qPdlB5e8v<2Jach199P91YH?|+gqPiPYaItD690PV`#RVIh{lOU8 z8|ZJeJ&bF0#-O0i0D)Y`$6=v&o^i6bje(S#{>A!+)C~#xjnyFZhbm1la_xnG1 zo6%rY2M_3;8mKTQ_m};@!~-g-4-YU0rT>bzc#kd@?#myYF>`iaXvWNo%70%L&Z`Jj z%)GGtk#Faf&&Zn{4uyZ8cj3%AoHAD7+&mQxrwZqGeqFl4xv>(ba_9EBI0ZJ}ScTKH z@C32iTCDJQ6*Ka}zfTJE zM=M6T^J5+P4|U`Z>Bt|_k#~y7J8k6uJY?i&sK|8-vHXV3WJ=dx)qfdeb6f8Bnio5| z(aGG`s&kv~K_|j7fuTrIf-g7w?|z*=To3I=qM7N_qa$ugU|6evAs!5vUZ!4kwp|{0 z1d*!@q%B3F|6m=q9+w>1W%kjmRfz=|x(cTr*cplLoe|HvKNK5P<`kS3aK;8=lW2VL zX&BLbjxVe?*m4-eE3Xz5Yqqq&#}`=fRl>je@>gk{yJ&KZ>A=qL+SrJjI{(sK49Q8# za>63c@}}aCzJNY?Fn&J9^|zHcBW@{)&BJKoUzVc1sykWeCAHkk648g?vB~{k?o1?b z3q17U#S0eH)FnBJ0JK&d9{OAz4V01Al9P4VkRECU#}xcVs4M+% z#7KQE$<8Us9$u22SCTz4kX;zao>-Q>A$`SRD+hNCif49btY+@Pg1m5bb6#X|-O?8Q^sJ(Bm{2*UY1xwI>ML+=Zq(;R z_NuX_wt7r+O-r+H%!2CXYTuYEnworL8tZvO^08wk;+8};i{>wEtX@*%8&g-`Tr&m| zoYmA^9lhE&CR)E_Nljg|mhb=be;&e9Rc47&lG{{sW%XqrSlr%Yrh+fi-pI3>orgX1 zO+Mg-fr@8{p&4$@n4GEb40CT!^GrV3dwcfARe%TN`QL^gi?9??z0t(k9#tlOjugnl z%5RdTzBA5iYc7^{uL(cb0_JSPTX-riylLXI4Tb!VwQ2g~qyCsjHoV1W0yE3M45dDW z#k^kmz{+hgE=BWVd}hNJSweI6(sFdct9z2Rn)nx)aQ5U4&wH=#NIUW@9b&l&kcC^k zcR48i#10kz`Y}5EEPYD;_r{-m;JF}Orf;9}b)H9v@6cQ_4k-!cJ5&x6ALM1N?lTT5 zjhZYYnfigjGL&WpV$|1BLQY$bF)hZIGmuo>ot6S0odTy_9E#7kQsCt&a9?tgd(?MD z!~832>Xt05pI^6tH_=5`qcZU|H8d`+YhFlXNkb$29xfW;0~;Ef6c&x*l2?lwn(7;k zTik651#OG(wCvjM^GyLFCbeH>aMH7^S^6acr%o-tl8VIP<^O=GY>4v>wxxgRL{e*d zyf{zdk&_gx_}~qF5?`7E|0>U=mwe7UUE?-g#(nrg2KT1xLV?S4ReShUX|}#af?n!p zslX+l)9Ki8FkXrOGzFeVVjL`43}X*t9(D=De_jJoG;utZN8tG1uYuS?BR9uXbxyek z!mM>*a|)8OV^hd88cQ1xFoK@JdYs+05xP+x!Sh}|N5iZN^qW-*1F<2=xyq~Xln0u0 z%X!9OXXPX8JG|B4q37UdKLW0pskp_zdJQfXE?n4yKlX0J6c4RE60jrrn0XhdM|#xj zxy_jjjQ9regjVliZ1rx=985Tzq5EztCMH@3YZ=&hU4&=0@mdkx#ffbJXS=f0{NqdPX5k-^LirjuJ8|HJhe0*KFqIHlj(Hf!D6N#eDIu&R3zwzcA zvjKBY3OIkYZ)fw>G+f=1$6UGT(J8AFc<)vGNgG<+dY(D-4Ec(;DGA%Y_n!pvul^q7 zh5i_`D;H-X@;%iQrk?^~YJNtVzvGHisR}KldY_!D-Y4VfFwD(ATs_r&&_L{Ph9yhr z48k$j%VN1d16$0^@5A^VvJf^zJm~ak{Uy7bCIGtF{*K#Dfnt&~PVD@Acd(d>vqDk5 zkANd5eb{L=vExUN$PVC#q19b@7MOe)X8U}S=jN)&DKiiEh|aBT#b%|B9#mv%`Vcx2 z-Gq$qz!ft7$bhpKZ)wqGMq=79IbZiubp9tN+R`K-BW?B(p4(@!lR?$!X`p!hh`FC< za_a0E>^ZIF6KJ^-XWQ4sMDW4*r3EvN!21xbJ*chiAsZ*ny%)^caia!eX9q_;i&v{V zaXtA=f7i_?LjV=MNUge8Md@A@RL}Y&3nQJGnolWqQkrWxz5=WQSrk z7}j~GWjYJsM37(r{cxv0BRk-Hz=`>o$kBc_?5jNbG84U4B}u;_t%&B|3MlG{{xCGX6AoF?a~kEiQ>Y3tb-V!_d@?c-g!X zVcB8bjSTaz{u!!TIzW_PGl#~cr6P)#gE||TGrK5IT)(YHDO4{H9wCN7U`)_&H52r$ zA7XMSmw63k5(GZI%1rD)bA)e)Q%%p;MZv!HwF5|Mn!bAL%enD+=7r5XzSkO|M}#?R z^nFAVOw;v4Xw;LTQBSKkm>>JQb72C>G672EZruW*0p(A!)N4Kr)KX+KVr2%a!Z$oI zHJz#Z8^^>KQI#+PJ(QwuY+)CcWE=wqD=vfxoy7?pB@SG!(pMzJ-Am>Um&y-m8tY}u{QUoc93f25 zPYzDHBa?4Uixu^9hV&|O;*fu53Hr8p56wB8jEgqh0tPVWPBjKdFz6Kgz~A{bX1xcU z&B%7fO+~FzggeQ9r%L=?#@w>h@F&wnQoQUq5_Q^;(CDJ9P`nOfo*%SLq{gVcis;K! z8kKi`d^V;wj_dps)3z{83-jxobdGxJS4ffQrzm;%Gu{m*vOX~9a>C7-zrht8P*%S6 zkDL76gJ3XLwblqRD$SI$7*WZb1FljX)$_)n*LCECgJq{d#RFr+8IkC@F`F~#WH~i~ zq=%aGzc4w3X3Mf#6a?ly42dH)A{}#K(lO=#L`;{jP*eWRti-v9iwd2JGv~`xME{D3 z&}*k<4_$O!P5Cdtl>ZO>-Kt`EOi%ewaLWHW&y;`l z0;pr(aGC|^wJo4!0X`4`6AN^f#&UB|kB6Kc{#6$bnDf63bN=-L01V+l%}lC4{^c=R z*}UU)AF7Pmy&Y^>uqW5}+KZqiKJR>?d6XSKoE9sse9au2a#F15 zc&rB2(@6atod_6-ZpO>P8#%Y}3fw|-TJ^IL1}4ZN#@LqgjCuvLv$=dgFv(L){#L@N zj^y$c=HG*hE~9o9hvKGNZ+sQ5v%#!gg`_NfoY%n^7@Iai{pQoo^}saB<*^aN0}wB}*j*|as6|vnnYno1dNihwG~X};`udn* zKW!ccI=X-Q;|-vTL|?&R?X8rT+?nIdoG_+IYQ>s5&SwIud#)oD%g=BHXWK zY_WW`6oPgYblwhQDwbVJP%sWN@(Kg-$XS><@vO5Au5$uhhZ=Y*e%X1xy^ta=LO`{Y zfc=+4tR~37fRnqLfRpi5#dZY^-&7W8-|vR7Vl{!Xmx)TGKP6icSWOTsQi};f@yv4r z&L>(Bgeu z?oGHp1v0AlGRzh%CO8#~2~NjFK^~^3of?Q2jSR*YejP0SJF|l+2s&W*rd&|)vR#~l z2I8&3;=%row@`ONA7(*;Ua>b-d4VpKrCd;e_gi`jHgk(=fW)*Ly`bP#yj;S90!;S| zInVl6bHhs1Jl5UNNzmlfLW<~q(|1DaTy=6{MMI6XqlU>5Rz3k@(a-EeP!=0nIj!DU z`OwiQC!7}1+#$_89DUBl2i?=+Q9-U!?aROVyBDftLFvGiC!U1?kd!MIzJZ4!=k>s- z*D--46rYV5gGP9N*RvRzVc}6WUyQfny6G5lw)t02LL&&%TC6S72&gi=8pOMHZS2X_ z3%dA~N4HqhLmNi5Z)`&b#UUH@yT3F(0yFu!c;Rg}b0@KRiuP#~)@T~Lm^_EYH|24x zDd#F^E@9ZJyW4gNgZ2iLM|scaYq6|5gvQ^@qQS`Z3$OaS`=KgItvqtfAH*E?U4Z;u z?_mswGa2Yn{T-V~hILumzv>y&<B>g?VRsaVn&lZ22kr?u z!H19>tpLcUcZJ9--kf!mXA#&w;)<8p+=yoONmYqYA}2;=X5TCHO+tFns!>R_(&#{f&eK@Oa9U|b?x`iQ$mys?IO+i* zsXBjWADYj|ayI>Y0eDt1`~#~PevXF5yLe=6AU2^`FU#|H?HE27(J z4ZP&+RA<=ro-fWSgc#nJh*OQ{b8WUaMvv~z}Hav zT?omo0Y>NF9xDE8&^guLJzmX~bG=W;OK=qI8)zqj>>h?drvIw61_`uMiyo&pr8Hnq z#m2ByE+e-vu?@_f+_7A`Aox6|()tv&FcD#xy)-9aW!69(E8&v3MaDix*Cv5zXqQ!d1pWbgBSr7rM%$XEIhT zJ(n(=iYYg}YUP4@uoHB_*eRj3e6?`lF6*r)(OVJc0lGs|=uV7qay5WnrGpX^{TH0>?*^_0*rZnj+|c$rO2$@WIQj+(yXuRfSYWBz zSaj#PYC#tnu3Gpz=BMB3u3Dh3m==ueVtK`%Auq&!F&MuNV^RCtXZA49wIBPE2BIQ~ zXF(9mlgG^RHK;=RYjXyHx@%Vt?pr&zSWA$u-q zGz%73%k2UkY#wb+v&cd(T+j_rs^<~Pj=`ELrg0)}D2tMkYZRb*TlSbpMy3*jsl%A@ z$fXJh)r-twGS{oKeOob`yGE#K$a?&T(~U7K#aWV&j~Qx*usGq8)ftDd?7{^z7u&OL zp~|dV(8Ze)wr#aNJCiFFG_*o&dBbTis4Jh*PbjKoQ2&qAukfo)s)F_rJSxv6InQ&;OD zHx_4Mu)(&wM;9F%j9-lP0I!AP1#2;2#u;&0u;Y-AD+2TYJ?0ssj=Yx~Hh_ap6kG@2 z+H=<^of%fJ2-YW{uYd)QphZFaB?_W_=I=gTEmU|+zwm;2hCzgdiPjKfhhg^dVQMk_ zN=SdZ2;07;e>Y|YGmDs8sJxsh5A?Tc5A+fw>958$O`h2k!Hz4=l$HG_>*ufYc_yxH zGlz+(1YIaL7&9tEqq#H+qiiG_`x(i)RHGphJy7ao-QRz&T7J-_Y@gQ3_~?4~QWBJj z<%|*&!^6(k3_6#$fEj}-Ta>z2h0$e!OMY@VeV8$Fs1$SSu>^sI0*>SaGr7L1C6`?_ z77Z|^d+lGHV%32u+|kcf>m7CXqyLw^cY&{}IM;_)$QFV|cFFb7@Ooo!U>4on~;t-`_gXOAY%afaM-g2`A;@blbqg2~x1 z)+QWk-2puY)*VdFZp*js;1q~auR9>a(h|-9<52d%x`RFZsD>3^6|DG1oq_QVtWWiZ zpo6N$IWTgIPlkAAK0ZcwwB94%4pPXn52=+0>r`>aDjj_NKAim>76hcyZ@~WlhuZ%C zNB|93KyV2A|BcH2{}okI_1G0G?=gt>i|oB9fUFf*05M*S3U6I@AFb^5HZj>(loh`D z3QNC(_dm*hQ@>Xw@T=;5ZrWREn9&W=)eY}xWIs@%# zp%MPnz|;rDy$4MD12Hw_{hVg__rj=xWpo4Lz$Ch!gxY3uX#7iWdx678G#-@%{~W!ap_W{ zM|7};023M*YV@`ja;YnKyHR|Aw5i>>_;ow_l9!{ofaQ1>s|^-7%m<*?wGvf0mk8XN zOrxi1tO8IyQnQuSdud;+A4sML`h#z9z31G_rG318dNDzK$cwReT7PN=fEbvvX5Bg- z*WBWP(pta#{1F+o(gAj*+GtuWY(QK2X4~vcYk?ULL8fdW3AGO*+m=k}-97C3M=8s0 ze3jMsA25;|L?T zbYO?lsF9hv!{bWBypK1&jRgd$^PKq34->IX;m&37EFT~o5*2#|T|ZP|coj@g1lvu#_Q=#dfhxtr@8`j3m*}f_ZlJ$h=~@X90o!I3jO) zC^MBVZO3;J_o;G_nX%3K)8RQI995`51*-^e%GRm{1X}kH0hN^zWN+zNKw$LJR|^Pa zzXLrW?|PWB&A#<}R$XQy2`tP50ovzh@8Ap68XG{6b5q&zv~7ovEdUzsA8Sn3lk3HP zyy3ncTk%K4WIg5Gpl2*s&}uy8t)bO;%6phr<6SEz<5yX%cdgJi<4JFyDjDqH>;WV3 zu0k|N-PsxFG)UCR^%8Z%eG+x@kwDa9#O|$N)QFQ&C%uOQQNPtrGar?ZoCW%f>=#hz zS$ZoGwbVW`UJ>X!UEPK8DSp_o@}zkD!?Rt#;iIWr)8&QZ($Sxf=q~<2wgWG;t&bW{ zj8ACr{u*y;@P0iZF z%liy(VRHfob$N7s1vR8GsQG%I#>`-NJQ_QdYM+b$6}RE#weX(sFpH`D>fgrnOZe)( zIguGW5DSA{4w9Xqh<6sicPgzS#YYs(-oH7*3wi!weCzrO$E-^C##)7V8rMv=g}hB~ zJ!;}X3>X=^wTJ7@@emo-Chgl;;&Bt28pbA4UZ>9PpKDE9sk8fs)U>4cbE;a> z`_&9wrgyEJhs!0GY+gAZS4U|a<=$+x?aSm-Cp0^p_l3oUCKs}&1>RNSKClNblwlRf zaRo8*aw@*hKyy3?_nT8(aiT5C8q!tF?HO#M(2<`NT|{MH%7Es>fw^CU5?5M?>bQ15 z%MpPEu$aF()*A-T=!5mgXx-`zhp1xD>FjPh#d}WYZZf^8ds8x9yi&j5(zlSm@ZQ+o z*4G-+13~469x8rQ#>s?+c5jJo)_K`>nzuPX1qL3=f>gpCvyg63dKpR4R4~wR4^}V? zhDs*WN{brOr8i5U;%o5@{-=g?rFjPvmU}Q*a5E5CTR{=-X-E%hOc#vwvFky+py|6z z8#<+(Mk5gorjoIlr=mPM<6^Zd7K+9mbv3`?&ef2I*wdwzK90rG4+)OBOKgmBVne}O zu6HIymnb{~T|V;=?>@p53l$JMzCG7=DyBXBWjuBO+{G9G5WAA8A+Ws|}HsyElw{-eA)r!QQ zlq&e0Nq}4US(Z@!8r-VrG(+hBY-{@*xgCNzyp^$vmhpzuodHf?^OeSQZ?08W%}8rU zV|pMfJL+r-6-^B~z}F#X@mbx!s~^(rWZUs!&CV&tkG%m}N;*^D`Zo4w!*tVkyn#VD z`z&f}N*LAE|3}xJd_SuB|9jV-l*9WZ9Yp=^py1#TKGP>LV9@G9cR*xC$Q@AT4ybHD z%&EpZs;ju83iaK=@mcF!{(rv4cyk%p%ayE}B_X#0R)R z`r_nUV(g!n522o$Z&SWwz|; zf0BuRMldk$?eKhNXTWwvXK7@Wd0C9dQv8ejMSn2OcgXnHGGpPc)t8=MJN}**)(!pr zGuF81a+1>sxr5CBWEbku6~SRmrErmp;B2$$pK_xUJc@hbB?`^q%r4@TF`f878~4OT z-h$T}d@w!33C=Yx^dBJzC-_L*6OSr1hf}|amsvO;|8K1I83#JSwLa3$Of>jXv*tqD zgEVlF4Q_kfRk%A8-jWAz1I~EwG6|OU$;;zk*7{VWotYJt&b&D-l@(RIR8X6;+j7@uQau@$SE}R%k_EFbv0H$R3ld z+4+;fWlatJ(0v^6Pu&#%hnseQbhyv7;OD|Hk^ddqKZyUq=oR^qGDe?!EL`MMcxsLg{R@7)g$u2-EL{8#vvBd>XyH;H{fmWb zE*$473l}=`EnNIBv2dZ^Zs9`b8VeUXH(I!)>j!yo*VGS`b5Q# zUoBkfnEt0}jwOGkUOw8w#sA59@Y+21I13m06D?fm&&Y#cZQ;`1T$=~~ZXW#SdGOaP zT=;q0!lixcws1+;(WZeBd5ZrNEnM2ON(+zb>~PMr@QA@j<-t=HE_7z*!58Mimsq&) z-(lgBUf;sS|E(4-^1Rc+#sB>lF7zL@aPj}7g^N62vv85apDkSE`FlSa^5CagxbT0Lg-d#eS-AM0W#PjAdwOmX`QoF=%(eA==RZ(`i<&z6V%3H$Lnkvg>R0FSDLAX-x0Bvm51O&39Q}PS zU{gUAnYXF+==RqO7M|;0OTYUz;m28c_`a7s^^Ayhj3)kG)Rm97p2;8oTR(PAU2u3$ zU%9~ASA#k)dQaBb*QARNC}}4fd{;-nxCQ1a@Biob)u{S^)xH`}i(t&P-g?eA=CHo6 z_PEFR*WXeHe>1Ylr4riTt)&DH;X)S1F@+-h|D)majX6mCc6`E?ToZ_!_td4J8)Q5* z2SMDE*PyG%gB=Io)8dn^QeQsxn8Q3_(1e$Z4d)wkknq(K&|K~C`NkY>Hy&sFC*kSM zmK}qPKTHR#F?86M>5qSrpVaxp?7U~lRYw2J&<`QyZ2kqeuk`oN7;_*W|2Mw1DVN~4 zw$yt^0hfLT;}Xa3v_s*a15P@VjYp{;9o>kj%`nuhGu>=|v7lw@+=VU9qPb=l3>}bT z8g%jvrk_v`n;Yr?>j?P<7-ewVe`CNu---X*^Z5Ts9{(Eycrf10dFVVD@E?qqV={E) zKZr*N!cmWc`dv>DPVmX<$#LdPpOS7_Ff1+If}S%p&pvdYplVpWO0n9_XH+n7tyYA9?dT?Nr$}{;F}Y@tj#$oTm<%MXE~V@(o73YQeMDI zT3|MJsk+Nd?!my1fwUnf$-xIq(O^H5^j=JP?<8r{29tyx@YQ7seeiBKaFB0`&eLxx zIDbS0y_b2@!KJnI1lCtZL%IFEr*vrVlxsC8@c2gpH=m-eq$ zWBY_-j>f!j?Zo07-u7iPtws4M(cntnXo$EUS7ln;g`+^|QhLe`HV-THqnf3EW^$VcW7F7L2u_fqUcO%a6y;z~kF$w5bM@ zh5a&9>26Q;3rG2bABM5rXJ8ql_%T`Idev(TS8=Qo(vx`EahPUP6PSwKaOZg_CP)Ye zwjd+LFP4lf*4v#kCK$l}0@ji`DvD&JkDV1Yr&!=KT+d0u9ux-Eum{zdz5FZ4E~I0d z78|pj!q+p>>8&)w9hTygO^_{$o@+aLBYD_wS5NS|ej*~}fpg{9n0Yp$WjUGLBXn3Dp{|2SRhfg) z!PPJ$DQwJy`fyVjRuWpr;G+hNH1lU*4W6j3;}%U1PuK&g{EA;(sAt}TR1~2FMYvP> z&gzZX+nD|f7RdjV!4Ai}R5bY;H=2C$jo(a)CilAG5^2)qpriWNhz~_bhxB+8>y&r)DRwtQ_B=Pm>YkH8P|{r4R`%YZXwXoiKt+{ zsY)3asU`fD^I$D&=-a~JU2JNST zwlN^_mBw(h({#0u&6)#N&C>rDf9-;Y(kM`>8V>_?bAkn^IC$3Lgg9nvbW~ z@K5xfTMe@LMlTrFHQ(^$P3{)V`b|IrWmnaPm=tD?pVta3PjT zqIgrxN>WvrTr{rtR3f%9;dO;GE0H%)K1Cy?3#c-*ggGNb&IXuE$qR}s zW#JCl(wN5fq~N%3U!M*rNHeiV!C?nmAB`*E34ta#RDk9>JN zI!F_voA-t%y~4`yc-A0TlPGOFD%wQtmryNVdjRJCN~qX+1xC&Abw3!E0i@*BYU|tG zgw#uOH>p==Z$YHvgVqaNU)2k$>Sn^Jv0Tlv9#@YfsIZmBG^B@M@hfa_RRYjAy57Np zd807Qo7xkYG7~|#^9pu6bhlHP<}rWSuTNx}yScQ zjs{yrhrRmmWqk%_QSUQ1)g`6hs_rLqt{M9QY-sSdq`WuOC&0LL(=YLDP>L@A-2+l% ze-~yJKeL}7AVrUmmQo5MBWVs)Bae(x6`wvDtPq(K=E_kviFnl?S;A%(|0PxUZ)8jT z4z&5Frlro}r3#;Gsp0M5mN{I4V|LK8D=CMU{RAmvb)XW6o69${824c%t*zjUsKRS* zMcyXUDhjEub>BDcy_*>Fn#ni6_$Ru!N;4W>bt6WE*oIak4wGuC9^m6I0Lb zXXY#Z+58=Wr{*F;MXl$lv((w-;91uonGoz=v1|=O;qLf1p=8Gncsm)g^f9y>152N7 zHTKP>U*Kjg$8v`j)&%QRILT1SE>lXAUUgJu+BjseWUD$Qg1K0d%SCCQQH0raBz19> z9vZg=LhJm^Gb$P~*jQZm`TT+0ivTJItAAYvmKWy)%>xWf4*3PJNtHY)?5jnGNKoe} z>6;v8W7hc|U@hTfdJC4X80?v%^+0cr#mec*M3fsg-EKb$zHiEao%IXb*Cv`9#Am?gyTjD0s=0##{x9<4hK# z2u7qJeyk$y77kv9B@0@;rMguou}Op7C=IqQ6C==Wn-&}G1sd$`r`*^ELtV>O3oyL; zM6}gfY`Re81`}4B@~?LgRLx{JR#_`%b!hHmQoYPn$x2zqrbBnDk(rcN+*1WBU4eRC zn5lcT(c7UG*yiftK&EQiSqJM94%M8x=mwjN#{qn;%`zCEgUQFHd{8%}(MNm>=Ejdf zjz>V|CDC`mFq*BxYHXV)Jqwnv&#`=cPO6VPdRHIW`vMK>@6e$3G(lLfN>$-PE=C$b z)x^S7!YlSLt;b7Lh-`0BL|Nkztt6^MK(T0p^)Z>yFA0y$lzu>Rk%oc|s8_K(?6)`v ziYvux7<3T*q_FU^DA^T?@*&ueVNJF^5NiWo#P#MLCMDUsKV<(vadMsJ6W?f=;)h6s ztiO&cO^z+M^kOQNw#I@jSOIY$$F6Et`@L zfCAfq*!6in8YibSPy(@$=pEhR)qhZ5;!5qZt9@=wVCWr zMor*#!S35@O#jW48!RMN$yP`FxUn6rHEQI>dt9qDK4y=pEyHe)IDNphFDRD{=q;(X zrM9Y}EA$Nbby{E7Ur?e)t*f%X$MHlMBYBpunJ#u4Sb1#0^O&v>YB<%6P|XEL1xx#7 z-@>XspiOgIu+RV3rp%u+bIz3uB6Ful=Fk0FWa_ke)8^nTfb`613(m!yR0J|dkOJlYDR$C|So3$-iD89?;9{PLOmWxY7we9QVYd3eDlQ1~0R4HWc*FdVx;pjKn>0 z)p!T+q`^hc#DAl~YjC9#{1V)g4%-_#!8un&N;2t)7MMuPrBKb@_QpEYW50t!Tw`$29|_fIaCr`1 z3)Sp)5OJLA@EDw!E=+)X=ecp-&>~6oN^^B-xtu3 z9hUFP)*YtrxDJKw`^<`A&X)r=?{jJLPJ{QaR8&kF8TPpg;yFm{3#E9xM`>Pn(;=A|56K=`u1cCm-_a63okQtQhD%;EnN7V zng{=~g$qB|=fQ8wga6RNCEkZET=;+7!X@8dv2c;Y?=4*Nw~)ex6FCe0Xdb-M!i7$~ zg+~-sd`()or0Y@(7d^S$!X;hPEWE_fpKIX~@1i{T^?C67EL`H2dgM2`6c+;Jovu_@L+xQTmX-NqR@FGfRi7S9A}{U zppao)-T=KWl!bLx07A|rgVd3I`l7&mWS6H~vSsTEE^8ZBuXIe%7pSEz3 z&ubPg>H33(M?s5jkA;go`<{3~ zL&?WT5gy{Lp$pEYJ-zLWhwgO z2k=t^_zeL(1YG2NhlLBD&*#DSS-8mOV+$8KglTXMCvuQ{|7;%o6bqMhRa$sNQFWYj z9(<;SOT1sRaLR(N-NJ?cP7AMBz8&X>7A|tzWZ@;o{|*Zm`M+l2LO*NaLVu5i3!fk7 z!H+A}42b;83_qt?c)7tZuyEmLoP|rgU$AhI=VcaNZs@NzIMs!I1;5$CUE}{YgOmTy z<6rdQhXI^)gr5fkc(7gjD1g(y(CMotl5_egcv&8NcmNOfGcOF_q%Zk(v4sntU$St~ zhv^nB`ZL?Yh0ZrDJcYRF+AX|X!^XTdmtOIIo8@2V-*4eUXS;*`16 zXbTtmCmY<(-?9Ks+QMgrg$td!Jotqc9x?n}kq4h);S%rHEnMglP~PWcP{Bg2}YU7rlHaM6boEnMU? z)WWL`pCc?>%Hy;=_*x5(7&k3m3WFZQ=DM zUB9w$DR;lI@R0HUI|~;&dn{ba#a|6BjJ(Y514+VqO4eyb=7<`4%qxzhlwYzCq8zh0nu{ zKqOsKUXHbJ;d8LTZ9Y#9;6XmmwQ$!k)MVkJZ&NK?($!|+5ku!j3okSHy%sKXw&cNi zm?NFktK$D%3y&yYj&q{1?-2izU-F*QOME9zua&*x?7w%dY|ZeR>QB3W#vrM55NW-N zSk}rmdR_4^GMVeTaM*``SzysBjzDN#kkm_(n2HUcz*KC(6)ThQ3tL;& z%kK(Z!zXP5Slbv$rZ+ZpxsMZbmYl+Um{ZvK7iP=aG_f&V+7BxUa1vIr34^_p8)X4& zrfMrljFpF~zs@&QHj>u8N`Pgaq9Br^2 z?$ifHYr()IT&W$*o%inzUw#Z00~|wcUq>uR3{!xhIMOuS@fIL$rl_}j&au~)&(E(+DNj=j#@R#mKu{!2g%=_Z^V}H@B5V&$Dg|#ueur_8!U~PPYibsWoYcktyh4E zM6jR;($Pw7)=6NM?W%+~_!qKx_B_ba+4mX_(lg6uvwwm~ueW{QtJht04c&zlt_ue1%hW)-Jhou}3Us{}XTP!E)~^xy}$+^hVf z+?w2RnWo>6+L??QFPl%N9-ZF0*$`74iS~v&Rx5LC6|43T=SisQx$T@>e2wXyh;K}~ z_}W}d4Vk7VDU6}4tE!(#n$^Eg3h~y@8F4@zUI#Nqyy=bc+ET0|oQqjsgfM+@1ZnI*LOS$vpb3!9Kvg4Psn3VFR~~~)g9bK5$78|nc(q`6JXaLpA+TWgvnZlO z6%k?0nB%8k#GA`^sWl!iX-2UreTR?D_Ir|3;$i`GJSI!6TEjVQ&RzkJJP^UIp_ugpq-g|^uWrU7R!Iw&O6;Q8nYt&*0#ymeQb6up z8qI%f?cZCXi?u|R?B}0>Y*NBR{k#*tOSDkC&C-h4_VAkBP_OJW_+jDW>NTF{Nv>Z& zd}FcRCRwPKdh69uEa^M&0%rO>rtl2~AF|OcLS4$GGO#blGBujZ=dAx-9{n;Y8X+`@ z3DJiK1+DtE+;9XkOyqG8E&OPcevM~Gs>)DRL}XO+@Gkj;fgGf)Hx=mNi8NGVdU(K1 znT}aj^Yk!-r3c^TE2bjCPDoKty0JpxgBl*?$i=vFIt?t_v zOR|R6T^VUWQmAiC4UdD|uJK~B22J;I;`?5}sbsz{^=#W0MWM|iN90NglPmp(RA!R@ z5tchIrMe7~VkU(MB~eywrEs8l5sM$|dndb5ty9q=HtW-bGy}g5KVveB{g2So`4-Fd zptrvFkF7!Y^lJ}R@naYcd$V_PFZVTti2;!jYURO-zN-(*U|08=_I{%W+{W%F-T$sV z&~h-p_2w#al$T0xHDP_-MlWi~QSiC6HefZ1sTNAWqzpm zXPB{;op>K~cKpG2!F&4qXRJLS+q8X`bjp?U^~6OV1?RjWanT3Sr6C5FF%-ddc{_NG zLCpA%;z}p}*`p%;GKJ>wxWQ#?PW&efUTOJnFu3TG_#bQV8q5Es2CuX5%M4y`;c0`r z7CzhHNegc=c*?>T8+^Ql^YNdtmSDN!*0J({0Gtr=K@cfmCS(yn|Pxad@n z#`p5zKgoly%Y*al;!yOr<)6r2Y+(b;$+4?aE*ewl?!`M%P^MQ6ATTQFYX z-?#9H_U7Db;jY1dYT;5}{>s9I{*wWmnB>cW0M2|7I?{-Ve1uM40&s%Y=||@T3l~1m zw(zL&KOzs#1Ka3$AwNRrA`6e;9o?lCE^_{|g-d9jhqlpJf5OM+SK11?YSZ z&my;P2Jn*u_;&(0`4_p}Vc`+Hqtk;qx%5U9>NtO}{FfU1!#sFD(`n#q##?Xv53q2d zbGC(xoX1*t)X=#s51zJgp~F!pI^idxp0EJj!s`uwn}uUD1?BFvaH(h3Svb>7x7Wfe zHS8R)@SgVH1Qb4{T|LUerCmMU!ll1ZlLtTF!iE3w7B2DHYa)dID+B&Hcp!TCwE)gr zp|dm(zS6>l{#pwcK7VH6#YUbRE!;dhTl3&gS-8maSqm3={w@#xaUT3|HPDvRH{o-z zg-gDlY~hmLvOM^33zs_LJPVidKH9=1-b?b}SLVSxEWF;xpIa8viQY!kljD5X!b=VQ z3kw(hx!=N#c%4lFJg9H41#tQm|8H5i(En>5{4;9KFUP<5j|OncQt;CQIOB}qpKg?e zOM06uyk7an%!h?X4L--hrCn>aaPfb$!P$7b2k=wi zSLEm-0Kn!s`v8qYX|uP-epaaRHq13jHbzXBfKkEZo(wGsWP-kAa=} z7B2Mv)xyoAv&O=chR_BJuQ&MP2Dj7oL;w$_>kk$#{O_^wh@tKZ9aX68i#9L(H!skGPi(Z-Ra7GxM7s^uTj}Oo}6}ZTAQUIrai8pQG67MX7OS}l8 zT*O@Y*}yc2n>&AMx@DGFqU6qU89Fr2zU^mM#?JYaXSvjzA+b45y}A`!l=v4KybqKi z$|cFG|GWsy`75JfFX=s~H;%&D>uO&kZ?)PjD&^^kr4*hq9E6!CY@ms2`+k^f;FxpV za$~6%)GK}PJong)fMm<@m^$xxTfJEwBW}F=Qga7?ZAmn)y@FWhPU&|}v+XI}!8KnH(5-m~J>!L-t zCP8TfE{#A$Ra9;{@O!8ZeY7&$%6~m^DdGV5YpOv@XKjQ;+#j+=tZ* z%$49J?j4i7`msg03mAh`ebBW$S`Lg3w{_+=em`#jl;O(TMC~!uBq!WC8e_?EZx7DF zzDZfpbh)}Ogbl*_buTH3cZF0XXpU?5n+|j0u^meek9#{HHPTP`#Ubp1=N7D1F}r{n z3u17+9j!ldAqR3qWnkh*XX=_0v2&M{Qk$tVT?q#>!rj>l{D>MEe!*Zbu91Ajbv(AK zZNBE$$Hti01U%$simSYR-h1)xt!`|4TdX0y3;Rd+NpUm8hD=>4c7#r)UmTOJzB%5F zEv>!hT>pYZVGUr0!47_(w7%x;Lj>{6n1a~OxVO#Q(s~ONjcI{CF$u1p2vL`qtq+59 z%+ikz8KLqKq1z7>v|jKCzv9+XE9evR@7{h+bdT~=Lq5VL>4zr@$9y!ZF!i~Q65in; z%@`0g)7r<|j6Ku3aQwU$Vcgs0?Fg^A2h?DDAu*eL@~cBIUg{Sw*pnzkmboZ9%njFH ze3&yLQ+mFe`4?>1kDb}H_+9VC@anM`-p6i+nQweDHj{dYXOeSA<^2*C_A(D(|Kt%~ z_01;5@!EpS*FW(#E_sGJ%+xfbx5c}+F*(>k78&e%+>YN1^_P4PcV2>wq#$jbldKDO zUQhY3G${Efy-WCYAsq6K{cHyh*ZKGa40hVaNvchLHfBPl*j(?}Oe^-X-HBr$u5C=8 zG@?7j6WGJabV~+;D7`1)4@wjs=Y0ZS%w1Ay-C^f$c+EG!Xzv`>JSVDN4wjwRu0X+2 ziD;WywH4)sGGV>e2*WANZDVWA#tc&dm82r?X1*2H)T|JENJVX05Dd^rB?)(zzqU5k?P!e?g zen?Ucsleg3+T2nXD{=4o6QpSI4T)0@I#Eo-=qgpOk~3W*pg{$Rj_!p=lVN5{W^94C z-7AJ+V6Q^GRkD1YiZqzzWgy1bKwohg?&QF!3qg=2RJ>#u$uJqC>ICrnK7ydB*@FvJ z;vkm79h?u-HC1T{+iRk9{TYaxsT=CO=j}!1v*g0Y^fn%E0n33SxlLqo!h=EAfidaO z18n1&9vC>T?AX;$NG#Li)*cfNkA2jQX{$Sv$|Xh2A0{-A z_o$){LrIeSXN9jQARFit;-;XGW{QozC`Hyw6e_iXEuKE;4i=RC+g}fbJG#*^#T#k` zwpL!l@g3F|^cino&rVZ8c)3@^@aq)X@IYkh%moXkrCZo>89Qx3%e1KtkMwkKyoZwr zlk#p={SR+n%6phSkFFII?991yWVg>-bQU*;Oyq2}s1n$!K0X9H-+ma+lrDRop$fpOR zlR*Lv3%lrP!Hpb@i4QMb&_)VX2hV1R$l!!ew(!)J7oYArd!X}RYod<#u8Sap+sT71ln?BHX}4NyEkFGXN`C}7C+h19iX6+4QY%K1xcn4 zP!*X3*Pn#8)$cQ8OTiAD65vf*f!&LBN5W8s@gy`U_MvO4BG=p2dT)->Q!MeCQYwQa z6|Yu?uHa01nvLl`$@Yz*(O&V&gnxW8t)-Dd&COPVlo&ke6-QGUt$oRiZb;%o{+7(> zf`ua^D0Su-x&!xzaLCX-Fo#;!(Eeyh?S!j(_QGg;xr5`OL7;1_qe}|<+y!+VM`G|R=DpKJI$(uX6`h#gS0^=3doT? zclL_MtL`)&Sc|dnP&oTZ1fAz*#!{nR-lF>aq`U_&VSBlrJ;ih%r*inrCAfxhk;*bfu>+q)RB;t~%~B zPA*vcpbIv_cOP<4_mz-K#o0|>al2}_Mij($#Jx@4#@45#AmY=Hxr{Ru+}hh!U%>Tl zSBmR;Z-hHv{tK*B*C{XUn_*q4C78l_E?||!$7ae`HKr#xI0orkakLw?x! zdl3c8#NhVDJ&}+_hE{cm>nBUy!ej}G2+FbRHA*i4l5$#ys!gWQk;fMc4cNzKnoe*= z`c0>N6fe9OX3zWM-chEOP|4ZY`ZQI%2-Mw7=mMoFy!SRGGwY0otyW(l`*y_(k4|_f z5xQ3z-}ki9SM((LA-M(dO9{u`;i>dC=sU{q4uxN*)RrIY>+x*9e$CfwrgR09I2kEG zhieVLVmHte>jdUMvW0CCiQPeBh)jLGimoFw2dVek@RMJ$MSKQLYB;}MZzDZi*Q%@j zdZjuqqIOT_N9qju5a3BfU~`r7IZH2EVZmVDu@r$tpF*IfkTqSRP%_NtDE8jQX&NW7 z-QL1rC@SOG6~5hk^;#dand)}v+auDlSFGHPruzmJh3$b$j13)IHjy7^DuH+SKcGa|F6&2D%I&Hvn%$h0}hhvv@zdSv?C z`H|!UEi+pRph|c(*c-;$16d;4-CU=G^JHpM`sj zh9J4rQB}5?*m=rlD5^eP7hren^_40@SyxiF>05dl z2xguh+kodm$}_fCS65S=BCDF2RY^8Ss)7eOs!C1+@uh%goMP}DMyXUrXK)h+LZEt3 zniR({VCKw0k9uY)Mm}H@%OQMq>+7xWZ=lsV=VVp@$C;INI=%5AvkxEE^%yjq0|^2&kX@gkDVjO)hj zWZdyBs{uNEBaVBod&`gq6)EJ*RQ%8KM}*?Bk5J282QL7|w~b;Y$w(X1zoujz+_DZJ zXZ$D`?Ux?m)}o-}yH!~Ty5UAV#r+yILZHQiqsF)1@A_z!6CH1dSKR@FI-Op;4R>w< z9i@vwx@iQ9KHNj2_0>Rt8%+RVgc;D7euav+N4>%*Cm=Qf91kl|banRv9I-091Kz+b z4fT)|eas9xqJ#L%%w^I>cDW}o26!|#@S#HBsmFj zI64{PPLUX{Nzw>sK3~MX-_&R-(eeB6nxB;L2~{xiV=3O#VQ>n)K70=29V-zg3k@Ak zivf&75!WH3m%<;8SA>s_4J|L>%l`Q)2EsB7wYb7%txm0@zl6697Wuovnf^)ucPdWe ze!_`ujC+rTGkf7Dp80Y&PFM`D*?@=G#&Cw3$B_ci*5?0l?=5Cd8E7=7_h227w|9ge zy0_{B^hgF!yo2xtU2?yAg`4>*)^6xbAdM&D;h%p17M{SFjUN%1bv;*~oW2?NleS3SL^n2)vV@8GOQ#CX8S|Z+UtSSa0p` z#@-08Zl>%_V~%cR_v086Lf;bI_iLA6^x_TIdp*4RD0)g?r%17bMLLXDDU$a*8<%>X zo#*2V7{h1ha3OeBUmNHWoKzW*rIoC%N9S$c+M&Omu_Dx2g|K6eSuS;u%<&d z{VpoXN&e#KBpi;^Ki<8a&&}S4g@<=3g zzl3KxRd%D=b_=dktj71fluDzKcRh41%_s)S=qNAxlUR9WcscjzLh>ddhAzg@dJEKf z1}Rc?Cvj0L4~#(auTxbBBuWX87L~#svW+>yvc|zJ3q8D(2C~aRzj{GV+&`CJVXYj2TLqL=9%~s2 zsW~3|INZ4kP(%7zDG_9cii{KOu9ZCE*c?VE3pr4YkJr`()M*M4Qtroqr0k!IL$UhC zGqYmd5L#l$j@Y|xW0Kx;sq|~4Lr$^Y?dGJnEg2XztJQ(Ss~d_a4SdDw?nrl6+zZ{{ zAA@t{y0^uiZ@n+x{kU6;j0mscL8>YVlrToZpA2_C2I7tB-!N~fQ;Djrqx|U24XBGV zb!vRB!P}05HZA~%H6t_C$0w>V;`W-G8HI28S8+T?TTFdVq@VZ8uVvk*#rmWlx&~Fq zF&KZ27i`2qPG_uEDfP(X15)vt_g;6GS63F#EGi&R zDtj}fWv=&|!qdB-gkZ4r5Z^E9cTs|HaFs3r4e1R&&cj1-#W?IcIz}(_lSIKLjGefJ zeIODXH-tF5axs>w7?H}7_6snc^Gl3JkQ`MD-)ZKhi;@~EYAtlVBCPbqsAWU?=~O1v z?he6v&K+?d=fkX4H7S(N$JnqE0eO!5L*iZ;=q+WkGNTIO-VoOdo%`*552~im&0Jj2 zn0{$YCiHFBXGIfU!#zS9(#S@07!|aMH35p(TE8gn71v7Z=z5V+Ug@uL*;v+4RL>Cmwl*PucQRU^?gUSnZEgpNa?evE9$5aujKg!4hEE~v04CZ5{?a>$`k`%Q* zC#9D4QNq8>anHb{SKRx!7DX@INdZ5Ee1hUWPF_h6T8ncipFyd?P{Q>9D7b;Bmsp}u zHO*vBDVcaB91)?c5?I^^-Ycp1L!NUBiwB}uj86>NnTWlY2#?#20_%pyJ{c=6!6Bt6 z>uwC~AG|DLDo6{XTU%YP7z>j3six73-a}QB;&vH|8yDx-$GyKX1(Vo9FQ%3;1#aQ( zCdafAbIK^0Nqq*|@N&YN%F(am77k+iD0j?-DPv4rF=>X-s2XQLaxKR*2{!@pY9Or{ zLl#G;a*Q*lh z(ZKa%&eI3hxS^b>c{;ziuw(0@ll0p=4|+T6FsI|~ufIXoHR=7X_c$t$`w-xpDnB+X z8tuK)kbZ36tK^ZMl9|CBh!96`4L&LE9})M?M*f}Xm+X77W7opIXhYEKQY(~SEZp^R z=vc?ruWyh74YHVhSua&MDsSCA46ivJxzLm7Hk1t=ArlOG4&r79oN_|zd4oSZj%k~H zFXFUCj6uJbN^c(JO~v_U!$B@lds^!T>{T#}<%=SZdxN8x2wjicz(go{VDAcDE3}8H zahcFB;(n1n#mV#r(mO|c(HqDLi#XFjK2XSy>C#&|CA>&BgT<2cqFK~EHtb^1_rgr* zI#X#Z-->TiReqkP3{844NIxdyVmiu?pgY)w7|^X-ek;7P5HQBZ^kZW(AuVw4QNOwX z8NPfoUQn}@h#?ZCq<(Q9-BeZg^H0w#DM)0RD!Sd+^KGZ4(p`vel?tSF6vqy>cXy#G z{H8zDE%=~&du(6pi=@aX5ufTVxZWR3p>Mf}rzW%T#`nGAV#2wUA7-rO7;|I#^{NjL z==-?G*T-)$-sQ%s`-PXUz;pNZmSK_>=oScm6H-|CD88u3kO+3PAH9_w;*Jn>Ap2b| zp>BzmGWQs(nxpkg5u?k-eu#Yu1H;-|9Q+#%sT2H|9fp@tW}P!5&6Lg#1S#3)4Uy>Q1!Tv@3t1wne# zGR1-H(POZU#rFdbA%c04ivU4>#j)O2P?d!heetpp+KY}XyxUu2K^fHZa2jWr4j zNGctwL@&2J`&WKLgE;dYJ**aV1UdbnwDBgg>(_-7etCcI9dsr9ljB3)j4_58%- zKtW}y&?erf0v98)>F20x6l(Wov@_SD>TvziZ*usl25k%0mV;R)?lyQ~ z`%RuX79ATDUb9@i5TB|PDB6-N5yy-uJQGJbV7b^28pzc~@hnK3Je zp3b$%b$%Bd$K)`iH{6e5Iu8L`pTx{TbW&W6u0BF~n7YO>(rvRz5dAIe4FkpZxzHKi ztc%f6VS>q$I#czxeHt+^8TTu_YRn84!D|UV9lJNiGxI*dei4H*r7@m^+l?ckcO`F% zz2}!+>y@^*?jD^<6?Mlmi)&D)yw-XXX;OHq-S?jGOW1>LJB9I60({k&-sqS5Y-TrN z8nWFVpuQc@9RvGREc!_zd?eu=!5kihZ^uXCevA!qjpSqC8wJ@to;3kl5A^|{D=Y>t?R#KBFl*iA^_kyBXY1MVDh%xDh|l zh#64HS=m=H^uvViXMD)WKuJ6rC2^o`Xl~cd1{<2tb*e;4MXanWiocUR8`Vp$L>lX$ zba4qvx^t?rlKlS)6%Ppy%DHFoH!F3jGkT9kFBu|swgugH$UL4L@@ zrC<(KNymDOMyX9Db^}o5nR+MV7yh}~(eS{&3A@SnV|dqHKaJ(^$x1sstrATgiP@qyr}9ZQ>t ze^^aY>vMM5 zaqx<lYcL&q-FR3kz)<2hB$Hg7l$52PpFVziohA|!?zsehHpK#|a z(@v42#`JDsinZ@}w0b{NUH%wsR9e<$>)|)oFKb|gI-lPctxj(IBZkQ=hmGe#yQ1r+;556Wi=#}Tqyal|8r-C@%iIrIF8 zJ9EL5D`ribiqZEu3#LW5-f+RWkyC4MvSZ@ni^pAziGgvE@eN-X*A$6=A>NRRk4PmV z&Ud~|_ud5%G~AvGO7#txELc=h+^=XYn4+u0|6p~ef8I4~ow3&f&Usu@$PE|FUJLO7 zH8|g~P@nXe32FZfKPeD?#%J_{+N)vAfI%ZvXl~N5rf~FtO3+~VF$4CP`2TmWhJf&J zTW=nlha4IgG~Zqgi?}EoZmE&)k+|n=5ka^?`l9xbBu=A^pu5S6SKgWImPfTeHfjE& z;mIcBxA$6*_)qPLN#6+nCRs{OhYJ6Y;h*veIteG&e0w##V8WLK1LI!em+KT@q@_Ao z0l&LVcs9I2C*cHRnXzTccl(V%Dvd|xAHzy`$-k?BsSdaCCzqwC%U$oQq+8H@do@V- zYVl~Uc6bgI*m+N{>rDL1O*pyO;SVJ{`rTilLvYTH4kOn+`&IB+(%&EVhl;<*=s)EX zbawo~cfou5`|sSVfpPsa*8b}y1qW-4-WucJ^941ZT!GNru^kDn{g?KF6HIoQVaU_+ z;LN5&`9B{x>F+V^khCYsJpL!-!7s~$PtSupbM+#MG*|4O&7W#6)7L2PX3v{Hr-e6H zPMb4(`dp_m(bRCsm&T$s9_tdGGJEEnsaNJad}+?q;1Pi{<}H}t;>=svvH%Rgy{Ip$ z{-BPd!>e4q(R*H)H-i5g6&{Xw(EmNf3RXDfDgNJN#Q{hEEUSXo^8t=H_o)$la{v$0 zKarl`?07FQ`9r-rO!?414F;!w-U|IM1n?mJWdWSJCH_a4^JYnhaR`2W0H<~e-uihw zz|n7Cb*F#Y4Nf}jnu!01etl3KKH0)W59eKxzH)YMiA|)csxnqRtU5MySe`XDl~red z+BG)oDH!AG)wLcvQ0&An#g2*_xX$Jiv(Bdd&&~cLYvCoZ+a$Zh`w5|6TZU?kU;0U! zE7VH|SUTjiTo|8xc;K67Uj6MGzjB^;9^=Yw^q2I85l}`gW&ptq3v>~8`~(c?Fs{J? z-YxWqhftCZen`)b;Mn3VbF$eNj>PmNxd3v(1QMLUkrziT><=mi76p&pnpDCXs+2CzGxqat4suNQ5hL41NwQOpT|SV z4NJj#Qs?{hc=)U@EDGpRc*S1SHSn(EFq%w&PaxFi6N*2~Gsi9blhvRo(YM9gdEaNOuDym29m6l?R zfa6BW1gwa;c^eWbqfacUuperIVG;b)t3jm{W&xCnrZ_-h!m8PF9TNU%dsJjbp!6_S z$}81@T)#17b{2GN8*ADcA(B%Oqij-&p{()9%=a-@-H;BgqiVxPUBBt!3Hz91UtP4| zNaRIHs2_d@Vl__pCcOo3?Sz`v2Ot{ON|O0KD%@7L)|BqX^bUN^k4qO{X9|y$A*F`8N~&F0gKa*`c0%Vb*O~$4S2t~= zdm?=S3jzwT+w?7(B~~G*LB?i^k7iOxtm-&}b=kQnz62r@_M{ z12c>j3z$@_NZk=!$c8c1o~ng*V9ai>TxSDvAyB&X=BUh$nBJEYQ@`Dtv03Wso+N^? zlBth7dq+V;kFv0986l}!NL`YTxN0ewJuNAlKlA5({7iV0KYu~;CeB%e9a7KwYk0-O z_~Jmm!Gh&!gw$PHc{k5|K}qI<)WUd1asmta@ygDXvl zrQ`ypjr)BzE2Y8BKy?#d^F?%}5b`iZ$qiqmZ^9R8e<-PA7eUEiM~I1!Ar=LrX84dK zl;7NzyAW4E9)VjCO?g|5t2fhaNFTsdB6gvEP{dF>p`2-pVjtv*zduOXxA+T7dNRGO zwnu@v4(7sK8>mVRG`IM9rgtmTtEC5K)lKW;z)DUqiPb7$YDnaJ+3!#(UGxNhc7Z_i zZ#!6KH3eX=;K3-7-lW=8soXmVB2-j+D$RBukwBkwO58w|FzIK((5?9`sx`*~GROnF zp)NP_fQgdQ2hc5NQ#G)>w)IhoglSM&&w`{Ya?OzRXP7xd>FZ3BQnYD!(s_!U)VYXV zfYnX7{ZiHb>QaqJR3h0RgjW{B4pXA-N!!W~Fh|8MjcG)V#v2ue@pJgFOZ9;l3=T9W zRq&&DUVw!G@ZymE0t~F0)*qU7I3OkL1+a(G=7{kw_2KF-Db_7W?UKs2ai%grSL$`p zQ5N&+s*M^_=?_#Tu}xw87gkcYu+#%vZBMEzRKI-y4Xo?e3d5r%sQZ;9T(nN? zaVai1u)G0;$s+R#Q!L6L{;{9ZZrAy91G!CJ6dwy=qW|r z_Idl%H9OnPTB{5~%*VX7947}LnI|xeTYFhexN{h?cCATj`or#x4e8Pcb;2+cQ^<6o zB*7Ab%2a%G>$BMq0g)RHJSWn0aq^O{pXZ#q;M7>f z+0%Mo9KvVMn%W!f{iNd?Id{&N7hXA~Wm@FQ`BSFOoHnN=q8zC&QVFNOPro1WM)S>$ zG44?ZkCD3r))w?0BmbWtxG=2V* z+0&egIdfa4Rg4}x;midsQ_@#C73sONXG2(8i2sG(2*R>f41SN3Ts?o_M2`9?hs$%m zF>cvBZfS}3$?Avk6q$g7%$1-1#O-my5Ca^-u^0tiIUY#U4qs%#-xTz0{01S8aWQ@~ zq}%gK!jJAj^bP&e$+h0x+u_HX`0enLehHs%jQb`N-iVJ;DktIPIt3_cp(5<%?l9q- zd!ae`3!HC^d+BM~hvXBudX;$JQi znX4WDp=3v|i_DnX9Y(Q8?sNFJ)1PmQJ7UJbULmo7v*QoE>UB$h|D9vpEi*Luf;2eguLq7#x5)5qaf0YN{ zm+|h%}X=nbl>2tVA@tpayu}SeFG}G9p7?&38QT%m2RrAt<9f}vx%Q@Jn zSZ`8%#Z@p4sE9dJ)U~S8x#Frh3qhmGnKhJ~70;fsX#OJZSInEQS5#FyE!au`J0TB4 z5N=m|1%8J+i}-qaMHO~EX1G}I-HYd?F$AurIu>3zd*Li|H@#)Pxw`^)3+7?xrIzVL z^n?kL?64fuu^pjqiJna%2q*Y*{peh2u7b;cr1LF&mU+I%!cF(gdDOxOo9C?-F6IB( zJopEB@Gr6=g(IIQ<6q_$rUr1PSLiRugRiu3X_t0cxbXi{9{hI}F7du+;XyM3zu}wu<+%E{#Pwr{I^-S!0JosaI z@ZVUt(Emdo{A~+|b+>jFE_4o4AKtlgBzzuY;dMG$&Pf)&*x>g5w?Zdo`4{|R3l};w zEL`}!#=?c4;6Ah`gBQv7$1MN#hR^L5F8sWh2j86sFEAg5QodY6|L{C`X#fxE8`n3{ z2^~pqa6emSozQO%_z%_tlLI*YvwuZ*rG=Mi*qLwP26h%(xX9<)Jor!Z;Oi`0+J$E< zT=;x958lsonM6M2hS6{y{A3HKEa=Y2gU2jf_!(j0LT8MH3qMox;Bzco=q$2up~Fp# z=p?<(`q9Z)_+*3MXyKAy-^+uq%Y$#T@TrFWt~~f}EL`Hv=D`o-!H+T>UrFyI6Ypp9 z;9Sy8XZt_P!iE1P3m5uREL{B0wD2Y!owGO(zTCoB8~@i@xX9;wdGN>b;BQ#C@bjjH zOS%qNxbRcL4_i2qo22(d3!kOEIj32;=x3#chm8O8Exg#^qbywbY_@RWXR?J4HgvvW z;iU$@)xx6&-)P}N=Vc3*eEDM@{74$*z=_<%|G9ba(RuJ0dGPH_Ce{sXW>IM z?A#H+DYr6%|1^M;4#!*Rer0f5K92gCgXn>6HndOj6>+U)WHH3579)aIs->&Ss;gbs3Gb zl<^%*8X^?_BVY?|4{c6LiZC4t%az{NcbJ%kiBR+Ty{zxT7ga!}6=3V42zld=w!XuB zshS@>$o$TjL1OoSk_yA9ds*X^Yr;y^z>W~6 zHQwg`#2Syg<2(n#s;0E9@eV_MmNVk$F~HE)c-U0nP}X=U)P1(S+NHvjw!1@`w=-Y1 zApMx^R&!9;$Aq?cE#Gx)W4wvQ)f9N~a7?V9O_4Qpg(`V%y*qh7uguKkdbw*5?2)84 zVM=P93>f^R^ov8<3>Kj018!NF@~T8CHiWb^VlGc#6kfI&$?Ro6bC$8oz}i#TVO*GA z+=0y*RMNDu3=9^KZRHm*E#-BsJ1JLfS7?@1()u-L8#om^lJ(dX(uxrFgfM!Qt@pT2 z73+>mdQ1dUYTcrFuWS^RwVQ0r1#*WlMub^jkJ(bV^N;u_L6Q~E+760#iZm?z5L=)& zq_JCovMfZo2^ByN6ZTUWt&0{EvnUlS+rr{d(6sPA*sj2E>C#=aI#3!Wb}}_>=k-`g zVci_8q>%GL<3dmzybp9I4t}3yRFxo7Q8&ugNM#MOWDB`~P3p2^!N2QIfEkI3iuP)4 zb42q@?tp~0I-+si2Y9+%6&4;)pNSVVq?;acjJjxJLu{s_j18lu@H=q`e|D3rcknYw zSygQBPR8XDkNNoi=7NEOgBlpqJOGmj^szW6}{3N~nIE_X09-;ZRWv1^XuivP=1w!-{uVstQ#(4r@S zo#PIbW55q-(ivXEg~Y7BSPWQ+(6*mE7hTXBI$h7hmt_I*`>g%#x9) z;&U4^#dXlt{-E2x_3_?CF`!*iYRcQ{O#RY;t@5~G9*%G@wLeVo%4r8YSSiNKVA-zB z9)s;0R&gu#X#3Vy)649QRgzK{WAhe$hI#mV64E&ZA*h>HYMn0Gyio!PSdV@JeZFWr zf1U+_=8t<@kHWCEN0f>aV?8<@IS7%c2&kg$Ly74df}DnT$~)*P8()*dy5d0pLp_Jv z`)V>O9SCwB^9OG~46j^*IYF{}n1W#Rscyn6rx6dA94j;|<4vsDH@y$E6#Y>p7~4$C zd{cM+z+6cs2q{fF*nD$gv9yAfZ5QQiU~ys%cw(ZdPVDXM!L>2{TV)H|^nIyyX^)6{ zq!Qeg9}TT3M^!hed>|!Ker)Sqmc=F0K_S977Ln-Ut+sK&gG#!w8#D%&K|@Q`vw0Xi zIEOZ}Jhb89`7nay;&l}%1aZt;t9s&{1R?V?LEe={v)rO4Xgr&}6aY%8$ym*BW9Ei> ze18ecDew6ppZAf)0JEH=;?9O;s#u~bP*<@ArZFw!|8$0wDh|Y7!n{2~+inJ;1`>88(XUXwWZ8S z@lbaWfKd0mwv-}A30p5W=9I1aU9%3!KlEQo`4XM{{Kw=;T^&?uQZ#I9YQ!A%O8#;+OFGY$;3h0_ub8 zh9taPr+_ADq5kXTA|`xuFEkgwz>{!e%areCoS}V4J~96or^GAxRlv;jkq%&f7n%5b zfq0N~3!cxGQo>h@M{~8qb9u3y_w;(FiGR5XCl@>Xp=3wDvOYIrghO@Y*rlC*PE8yt z{t_dgzuIJhRd*Wi*K^=N($WyARZ-UV$q zrTu#hcu6p@x!;}#e<2V4+dTN2d2rZpax&q+V8fZNf&ph5W|x)9XtPR;HnsI8j5f~} zqfJ_*DJqTy3s`Q_tz790@LZ|8xDi zhmP2Y@{^H{{!hcd%)x~NIQgu>Ki$cB@N+C&+PAAMT+-#`!LPG$iTBqQp3>|%Z&>)9 z1`jboa3Tl62U@uHg}FEjFE=lEI1!!Dxm`axLoK}6;O7SLQ-Fy)FALzoe4k_CGH2&o zxXA6=03M`&ZvZEK@vk?e$nhUFe7V1ohQWVn;nx{_lZ8uuZL#oMjQ{5XIAtmRUk%_)ukiCn z3m5<07B1;Mf`Q>g{(_&72VWMz%MecJ|0;mfEdALW*Tn%mSU+DDz{!u)`!g(D=r7EJugrtrX5qsBkMiL6 z2JoQV9tz;(Q~Fz77A|}~XW=4;S1nxl`J;s|)?#%2V&OvnZx$~8k2GB@yWAaZ;nJ?v zS@=2=ubT%S7r>+ND|*!&z$qW$XPSlA8Tzv=T;ly#3zz!s8VhF~L3h1{Yt(UmY~doe zCoEj_{I?b^{Jd-7;(xz|x0r~JFVZ(6f7jrrSh&dH91EB9jFwWy~WA0_wu{jIeo zJDcR5-v9ml&pGpfti6AGt>5~s-)p_s^1#Pw{Lg#fh7a$1;G=Z>kNbcRV22z(!yoDj ze}nsgU*LfoK40X4n|vpE;0AxQ2X6Sb+54|K7EfhM&K3#U~B} z-`NNJs0Ut7=HT~{2R=uEt$%pn2LGvCT!x?Fvw?r55BP;1c%@2imGl9Rdf*2CtUlm1 zeZW_E;1xRGm3_e1^#T8h2X5$p!UH$>&Du`G9~1utPyCps=WP$%;Bl|LqEO5)Fy z%i!7Nfg62zp%3^!`hZ`m3uy40^2R)H!=ITRxUt774X4<+*Tekgd*IqlS!Ru*iEqXd z>Rj=;cG3LI+C>(F_+^F&;VK>9z|9)bHVrrB8l@54;ew9<%+UWc7ks1({s$MFGBNRw zy5QtZ4FCB%?tvS5b$Q_W)iP%k7(TEK#NS{JFym+7!_})b!~-{Laff=~MjyWFf|H*G z8qX*VCx6_080Uh!`B0|ehR=rovt04X2csw7^TaoDsrJM-{KSoC@-yJmlbKbgHHBkS`eo^X79h;BE)o0&cP1>%JXxjkXx8oquPk$R>9aVU&vEhP zdUStz#VzL;cy(`UrT#)wBm00)ABBSZOQpI|I8Ba(xQBvz+4QR6W-Y zY>^OaSP({Aj`_IacB8-^3#=2^dw}6?8hL)#7j1dR7ioDP|BvE-8vow`>~9GFh7d5& za1?Q4xTyXpI1t0tT*oEEoy!v90v4eimoG?&8*C)R9aj?Ka4v)~-0)c-?^vD8chI$7 z@Zy#Pd0!J~<~z#XAmN>Xa)gnV_dvsgfe4>LcPYXs=p=9IB;LJ&DuEqiS#ZDVnm{S@ zX?Z^zWy1RgeKGZ&a`iBIzCh?`mCI|%v+;r(XZ{Nw<_B5@H`Y)H>|Lyq#A-W%;haW> z;6u64jq@7uP8uzSEom%)#6bOM<V667hZbr&%5R*eNgD_(+2gsT~2V>*=T(zjL15 z_WswiJ~;4c*V8Fqu`_iTZ&|NcX{AD!;tq~>W_A$TXt#aqJo0Z%fM*Pwbt4GHxsVUz z0jDgp5mIIa2u)d#tW>}dJ3_2Gf^LEwc_<`P$WSmtAIOH=<;>_`C^>T|R%FcOQZ6XI zB+A7_E@!Cw-xag4G-%uMv$66Hrilx^+Viivw@cEGv+bMXQLGSsuj)VHkx^IYriE@oT~HNGg9N^W^ee@~|AAuv{fKe*iysa*50^T+x*? z#1t2&Nrvpo7r53CF=lRT-`%Mjvv?;EF3Zq9G#i(#(X~r6Ih5@094GVIUc%BR;irqm6YH}dq5PT>(7w$^BZR9s1$7_G8zE=opLO_T zkx~C4kvOhn&wq*>CZWDFxZLci<#O!|r9fcL=>$GqpH2CbQ8)~E;^b^+m$SX`*RVxO zAB1_S=@#^li9I7;T%rXcBkOUm+Re()LdoyqcF@-vaVBA_I3?s)@F_)`-_G?(pE3WN z8kXaxC>9Tf4mBGkhJkko2DeNz4=Y)cH*rOt?}xe66y^eohcF4q0}mDFZbS);&erGS zhNxv^KI<2p>`V&ym|W`9al*;iY9H>z>v}p={A^PcN{_=N@h=wNM%9qA=tR@vv6Z-z zGtE5 zSL$3ZF8Pi59WB&Bzlt4sqI~I+cujr%gy_Tmx}`PsOKO)#oHruQ1EQ!-l2YE)LQ_!* zVA=Vh{*!TN?_=UisJP65EIfi(WB`37ozrd!Tg9tl0W=xXOb`@%D3VN1#SV)zTMZ!Z zz0V~cIOuE%)y-(E6pkZvdTN%B`T7|j`4mVxvjFi_Dh8(@swdikiL_6C{Dm5 zDdqi0%t67Aau7f0*e#(tqMPZq%DX=76YrZO{)WIthVicmD}WOAd>9biSB3kmvpS}y z@^K`$x{|7!J1B)veP>`DQ#AJw5R zz=#JaSLg**i?YNvqjIHk{>T&6OBUCR9z7b=Jj(CTz=#BINKA^2RaZDUg!ja?{tBpE z?kYJoM|?){v(BC-Ke>94iQ^}5KiJFK`miD9GiKU&1SZUqT8IL*BnMe1UBYJ4;3 z;aYSuxQe*JP)$2>!-g^_tt7iwglw1>%YudCE?C@5cOjWxN)h^n^UMzWUD9PvXT*6& zB%pV}6vTiMve4VmT-*yKr)JA_b+~$~F$nHFi9A6o?xe&+B#sRlk4Zss9)HT0jmjso z9v;m3fPS_5FwZ^D3v@I-s~L@ZJlBIeuJcpvhgu#4MwpH`c>AHBvFcpe9zyGuRh-@S zd%U{^v49yv5d2D6t>Zg5ydGgJh2uB{u5^zq+rsa{GO(kHn8;?id5|3OvrVNqsojBA z!&vHPv}W=TOpGZV29uz>ig}f=3QCZ65TS%{*7XW*h!4XS;yIUR#cH>XLV` zq%EIyr5+*BIbctFZri8X@M@}VyMrYU;(IW43nSoK-2K6ApW#x|5cI*#4A_a*N-~3m zHq4O1qCjMdzL}t#6XX!QzbX~(qNL6!Ui8KWlnUj;(cdh5X)J|B#^fwDC8KPH85us{ z&Vfo?z<4q?)DpQ9DEg5fFllScC}32&c)_W z60$znQgUn8itp2K_OoPYyP-YyAsQhcA$*4#n8P*l&o0MBnP}c34q$V0d{oLD?Cw;} z_(*)0?8-uPUq3{D3fr*|%_q@(1H?@nZ(L_I7^%bw0b_(rMr9%7KoiUP&hHcsG$TSI zWqXSxTBJEuFWz)0Sqd&HRiyHY0;ay(K=zK0I3)^4(tDppP=7m~FFyw1Td z%61g2LAyg*Hg?+v5a42Fa$+|xi%$fdPwlm7i9VB?v2@QNzMp$_4n$HpFA*2HmY);B zJ)Lua3=S2nd=>xqpmF;uQeE`|Tf{gwcdqPqX>4Q}*_Mf(j@xY)$Xfe6G#J2cwD#<_)jg|&P;yW=Q;;^~ z?6KE2v*2b61%*WkQY_RN^v86sU<=;ZR2|Q_9Xi2GxJEspah}uP*$u@qE=$!ZR8?jab&iUCB9&+X+)jRgDnq%5*jjQ{7-1!NG?S6UESVRUG%S>rok3d19%2T!@`BfZwER0QsMey-YaEIxmlf?MG?& z`h#-W_B4mPpz?56^s}MV^nU0hIG;3baM=c_Gjx|ZpO&E%Qrio~Y>m=W=@)5jqHt_T z?IzQDv_D}`j5|^_V7*%+SlSQm5hxROec=TP37Hwxj%pgXID}?^^jc7jRY)DC+t-vA zNM<8qBSPl{`QJ`Re0%D~emq_r3ijdD4z=nCA!xEJgrvY~d_tGqoz+dMgS4`%afFC~ zC{=a&AzB(AFrD*k>hgZYC!7x(|Kws`u2#FD2t`YQ^)1NfZu_gGy;C$XIk}dK#333wrGB#AYJk1^e#AkW(0}y}{=|Zs=)>rx z;7-Uz{>Z}%>;3Gc)I_(kzj6Vd(Nq+t>?Fl!t3FH2X`z;Ov{d2m@|vZzFRwu*XX;kt z%T0c=@1SVJA6;$6%R78;kk8U)baQa3n}g5VYwr^6G(BbGW#|c{&kSpQP>;pr;!u3> z2$r{5e8ge&FOc}qk;5C5&#-gRsszOJRKlqoc5&mM(kJid{=2eN{tjAgpd^~|UBaic zIX>|s&6iu};KSi~c$WlgMpkw9T--Z=D{SmM3oSUbD|H){iu4o<7{Q&xI{wb(J>ruL zGKOhrGIZ2w)(2E}WOmg%xF2W;Jj|J~_#Y|5VqAZLMm6?SBLaB&EOP!F|2ceuos)=h z8a0OoiJ=X+pt*bTe-FQ&MuuGqT)ED1mg5B7(m(K&C+84c$k)H~!^bS3ymC}S_8I4m zu-l%-NQPEY6jjv^$*Df}erb@#+Gq|mG_f`rKY9Ug7z4}C$O#8ApsveQmcvRS{U38x zmbgxY=K;($mFCxx-ParIO*I)y4O?IPh$K> zD$1uL@rsq;N##?fj&wTBK+oJ9+0QtLacS<4)F%W=My1<79W;*mr^d z0t!%AJ+kxvsITZ8us{&1w0~r@?TBe^<@+GdtXS4@kj@*5X8j2b4N=vXDLNJ_4|P#4 z?pYeMul`J`*CEjfsMhOH3Tj6l#&LB$9e)`*e?Z57Zn?zgnA~0cY;6XyBO0G{FkYhuj-m5HF*|Xj;8nH_XeRW4RB_zHGnClRnVKp&`|_0U zZ1$mP8OB8u@1L39pNae&K7E~kXvled!sb1^@^ApC9ALT5eijpZDDe)X8Dh8HOubib z9t{mvsNli9x_Q*@@>|l&v)fJ_$6eB(0T(qAj6>2HlREC!3_72qtwv)B7w>=~ zV%Q0uN4FO+FzP#l6!&XvPu~xG5XpJK2#+(Hcgze(e;YSVBxMu|9rm^Z&=5@sW8slx z>PHS<;XB9Hx+^ADc2^fYU)06wE@vg9qPnxYR*a%D6oN?ddOX5#a@eWCr0=v))~mtc zJBH&HmK|N&aSM9Tj(v{yTc~)O^DG)+e*kZolWu@&hEZ*$7DJ`TJg7NrYZSGQ%)@~W zh{ZII@RMrM=z4^lVl9}QDBlqM5|uvw95P@UgQ;h-_!_D0%|}Tr35(iKib&r7hY>j< zC--ju2urhKaa`4o9tR3=S+eprUwGtqaMFYB)gkMD3S$HjDV_f?Yg&O=nGTb6W~ehc z$oUX=O9v+=w)%cJCe@e)nAFVqm*{}JCaB9QI(}E-EHPVo44#>4{GdSD}>AQ1z6|G>U=2tFhq ztpZoj09hSmYs1zRF&Y7|qzzI#n}Jm5?*_4+P%8g)_RGmOC7sOJ1LoKueYTeT2uU)v zh|DMH5zw>=*-^W)<$J5lQE^Ozw2BHdK6>2j#c-f-HR5xavzh5ojWtMkXP{2tcLo;7 z`K&2lj`0i8_8sPg8!twne<3VT3nGfix61?D_z}IBvVcR7> zT5l5S^PrIbCV5w9%P>DGF6FvT;ClGhQ72AuetEZiUlVBNchKFe>DnlvnM17&)RE|Q z0#uX0WM+fBF922Ud=565%LTZ2-Cqyxh$jger96d6rW<81XpnM5oloQc5@dd!G z2sz%ffTI=|p2T@r@3Kd85oZ&XEooMDlaE zLv48P(QO3R>3~K)D2`ORg*}|nR)`WNJYtu4PS`J0*;r`CKdULZ}aT#F-!= zr(@E@#O*#iH3FN`Q9+;=*zKG4P8z-fgA~eDqW^|e7@sPO??$pI6D!+%EB2>9#Ln~( zc*9}hzS~%BB1hO6`L+H@)X`nRWPS@OmEg#dwJlwFjhB$o&{^nkm-&NP$stW+MVpRm~BR^d&f#i_xnifOk z2kf=)puUDzH2oFTO+j1jfTz2;;s?|f8U;#|%fa|n$h+mUeu;%GpPfilv&kajkhw_R zOhP%2lhSQ(a`+B^m){Y(D3cZ2mRyS7$`#rCGT{M zz-Gh5(3ad2&e{ZOLBn;A;c}26G=T8&YTI8$p@0FBH|;srKUN66!na1MUsyP$wyOP2a& z$`jU2d5j*7iBAehPrdJInSmF)DGR1psa2t5uFQY__%9jH0v-T7Sq)_g?c43OW$*F0 z6;(aRjjNeyPz-8HWzl#fU4Tf?`1HAQ(V)@~s6CqMlRJF35$~}hL_+$oGy=do2uxpq zMU|rFglp+>auKZqth)h(0us(`(juemwyCGSrysY5d8stU^V^Sm$@MA=o z4UR$)m$=pBg^~EPaNcX#s9=8wBPz+6xyhTd#o4Bhyas}voVFz4uUWigS#;|)3;c^} z7A;vCefWk^{-rhZY8OSHz~Ettf2@B*o|oL+4Pa(nH)_=~FJ0t5`tV>o}EbG$#?QVdf~@%=3& zx5HQcj6OEylkq83Y+vw9#JWf-b_$2oe4KUJNmU#K9Jm5{)~ASrvoR|_fGhh4YusJ< zI(Gt|#&p=X{)Tc;!_ATiqMn+nc*(-rle-lmb5Jekoz}Gr1lw(kQDEtBa_g&}XeoX+ z=4+;r51$Uu_C-z+qi9{yZ*VzANdwwX40Qv)}Ro z!REw)fz3ED3UM0Wlp(FgM}%isCsT()$gCiF6;Wt^PP#buy&{G&mBV}s9_C%U)u&j8 z+Cgk};P~NC;P@LK;u2u!?4fPJ!#>}kr@@C84jo}VwLEb6;9)FDfm=X5kV_00B0_Vc zalLEedRi)9kRLjNsrLNCx`z7>Fdja5c`79LYAPuSCe~DNP^xKJ%OwyXPf!JALPo8spi|3)cS zO>R!Siuc0b_%gJ$FyPXGS2t!bGUiMqL4KVml z2nuwrNB)_dv7+D=taFTW#3}b;2!*l=7VqIe)wly?sSvJeMvJNde=2#PUtU4u^AS+i zN%9uR!HTbP)maJGrp=ka7!xR*Nw~ZfYX$7Q72TxREw{pc$cFKgJ{k6t^bMJ}4&P72 z*PH=uqtw#Z;Zt%@`fd0spyZ{>`gE$0J)6w|tSY++V^6O_dvQ`Gz3*4(d02;0ktqRc zH-MFaDo}A282J(y*_`MK75iIf;0uEq~4AR0VHOA7B$w0g1@9=)W zRM{UC-%T#2{|pGSWj3Vw*zY3om$mJrJMhh4UlSkAlc>gz9y|ZMdh5KgIFVp1UTdqZ zY0DN}U9;4`WWImtlIzVGR1;j__>$UXix$h53D%UQH8uX_^Oi20huIKU>?Na#A?RPc zWbutPOPA=_(-im$4>&e&=`}U=3BSUOU>Pn7onX!6tjbb$)O`bf#Ux1rK zhx;!aJ$Cek{;`FH7ZzSrIM!bN()&Wn3Vh!d4&kKaF=o_l*pm-j3!lU@cPT$r_*N*aPo z@xQn9rK9;3!yGEe{59n_c&6hEpY@StnRs4<@|{=K;+0^nvc*R-K;hfA`O% zGcejpZ}KZFRWS;6M9Pn8P5Pl4el0?6dK#jF$e13lnHN)T1GotfUKqcb^t5O9&l@)n zAHA|?{1rO?R(E1Nd(-pX%R44%zmR`fv#W400{Sd(`A-F`xBRoU{y(FVXa0Kg&wTJa zFy8;4XK^vR|B5q{$S-P)(I0umAM&?1O&X%%hVSxx_s*Z2;*2F(o!~F)J z!vYOgVfRPneg=wtcS9{psl`BRyc?*Wdpp~y7~kAyJDK`yGT_oO_27y10iV?e{QG^t z7xn>XolklSQGM}e>c=1QSs*?CaQv?~__{ve>-&KJYaj66^a0=62Yh=Ua7%5Xi}Ng~ z3tb07;dLxuaiA3b%283@;uDUN;$Pe@TK9cfb0SAZv4JEH>cUTaQ_UjAs;f^RBA*tq ziBuyg6N{^tR`cmP+#=c_(UzeFcAfk3(7FZl>Tz!=A~D_41@-1}$<;sLqZ;z#)AdUi zE~=x1EbAuiROp+yFB45o`L(#PITU!6O?`@T0f z{66jcneUff_+N3sv2-?*Z?-mM#&^Trd+a%`Xy|Mrark-TA7J4%oW8)sH+ys$|11~$ zJmp{nj~l+o1$US0W*dp|Bc8MI-{61T1$Xo9Ic+42@5XZuD=7R3XI2JJgA2|!l!32z z!I_nTkI)SS;$i#Pz%O>e-Ta^Jg1hCR8)OgbU92=3B~zpG7zH z{Ky5TFE;R>y5Melp4J@!mdg#lh=sw=%g;+(a5w+|LZ#^?{`)StJN|GAwU_v54LAHT zzRyP?;b-8ck8-MplTLSe^IULux%?hHsy?x-;U2ijx73BljX&aoyYYX|gXeOg+On?p zzzzO|EJj=3(m-Kt;RpV12_1;q~Tuqe%S?g;~z*O@MC8&99ye{lC;7HhAC3qBo_)P&M#;+g#yXnNbe)$pZ zF4yztcf(n9Lr>E;yW#G9H|Pc=@vtZ+zOCC?guCM}b;0TKP5c{OaJozbZyA9X{D{X5 z_v;R(7yc7=jPdir{Q^C?&sgPwoAx2wKeevI35tpei^p9wu6XSDKF%h%xQL(d`{&Ol zC@s{PBPf-}Qpo5ui(~Ck=VhKv5OuzoH4|qO3`Kd8vvZ4Q50f3`=p~ueIGn-Shoij_E2~GZ%v#-&uXI%N$dw$#h`4XneMJzMoh>4>kh`=8n&C+c= zzSs?YT|XktU@Y-?46lf=vpYgN9{HARQcKn`&A(?Q=HD^Vx*PDuv9hQfdqXj$BJ*qU z=P|_2^$5;(OvK#IhF*ALD3H^5I0X8LE-0%a2+T%aCIgw$ve*6$Ivm1|E|SZO3))d2 zPQvVF$ofH(uo#mKkHLyVKt-;T{05RAf0+|grI2DIHR>gy2qlrpPr^{(atX1D38ALlH--_h zibCuMMG!khD0XA7;Z|YhutoEZ^F+A6g#4+(V_Ez_2nt0yxO;q(GwzNKObzgt%)>M+ z=P$Vm<)yA?WftxWygMHSoe%9bs}P3^0sb8zsEVE>jqf#+$aP4q-HYJhJWWe$J?(kduG0h@z_saxVQiLFYWCU}{Zyh_tRzV(5Y31Mjl@5q_9_vXO~Xo5T|2M<_IJig%i?<} zQ$8!ei&Kto+IeM397tu+)ZYAem9j`77|Nl?~kNhWYtg zZhH%Hk7Y$b&FxOyAckePeFR;LrYbpgtcqk#gvj2q2Pb#%x|^HYi^+-YHx^ zBthI&qzk+BxLg6~su}k_G=R(kZX!R(tbuJPvE`DO7X^|Z zAY;VcI)|{EH0HdG0<(zakUvx5A2N_l#he7@ls60S?=HM?*rEQL1#5Wadv@z6xE9K+ z3T64&%CcL7Ae5`oSXh~uEtJx>1{NR&m(}G;@vyW+;AUCi1}!CgC&6F?xl@J$&~uPm zTNxi(F;w2wayPCk$!2`S+f6t^X3VPgI10pdAuJiyQS3g$NjNttu-dOrW|Hk>F7=!F z?3eUvAta?{k$%vXjga}aGd=K8nuhf}$fi@$fvNlsyAh^?`AQIK9gH*u_#N>#Nr=N| zB*em4gc0PwPTpZ;^8Q$$S>7wS9#ZcbI_Fq;xx%;xEernj_8eH$s!P?17=MU zjDcoL>!3Z%(Iv#zuSCMp@(NpqlG z6_BU<A z%Xa>nF(6raMBRx{cgB-&Kh0U05UHaQBnQ%hK0&vTB(9UZ6+zwglJ=Y^7qcfDMZ9=d z;)kap99dOSx#1nKkb>ySH98sz$$>q%>Lcm{ZHW~R3{0CSxyADMi3NoM!QGV-jAgSZ zGl@`jldhPw(V#>iMCznk7Ti=!B<}xYId{{AfSxKK2!3q&?A9@$3_POcaMPzBuTdzJ zG89!eQCeF8C+%A#GRG6_IY3u0f2l@zAl|1nSv76WwB~k8I@6ki-=-y_DiO;C-AC}97N%NrrYh1^nXbARUs;r@ zl1R@+rbGJm5}K+AXW8JL{AgwfzEk>CjZX3j{5me-1~DpaOzN;EYT6Ngge0NLti+I9 zRZ%D=(csGhDh9Mtbxw$ou1=~u<+_1oKjY#H>nkGarD{TrM(lNhl}l=Y6<&M=b?zNl zm+D5a4MU?U?Y(M3=LP1I#BO?fZ6%ZtT~=AHEY1{8lt$HUKYB9}l)H*quNNY9odm@e z4+psmzmQlI9iDtAkVLorrJt^?)UJkDRyu~xig;^K4#h{)m(4_0R4np%mR~vD-YTqI z+8Bz8My@tUm|3Plt1BtaF=Z;La`FLeGBp*MJydL~1-=k8o?^EShI0`w0IqyIFxPI~ z`T-Ubu8|_3`6rfzJ>vfgC^ej>M>5Go)}8GXB;GkVSO&}4s@caOBx+OxzoTxcV+h}$ zD`mynLrJ$mhMc0%WGV#fDzQAMLxkx;Mk&G|LJoQ=wQmS_QW7bK@0R2I6}!BCwi1ndm9=h@Q>q`QYiVTPNwQ3<0TL3$1$ndqa&V*ILGuzJNm(Z$IzbYmm4J{g zpn*s>@dZeZ3sO_vsx~hyqlzLHKZ7{yq}=8xJ(N|_=!Wn{T2oi zZ?{eZDDp@-mL|-r^)HM(xp;ofy!wS#FN{7~yD)Kszq%$7eWWIyz#7Sgiz9My2iH?X zse>RU3SmYNmcgr+OQ_mwQE-$TpC5$bQ$uRN%)g3lqx96;mQIX+EW#HoBez?>5D3V* zT^`T~{iZy?13U}fw{#9jT;I|;FmblQf|JUIBC%`1cl6P6bs}AjkSRwaMjx5rpRwfn z>Sc8#P!X*55S5_-B$)KgLFkV!s1af-@e;o_xT`gA6`rhPZBcX5T}|`}ETa{?k%uSv zCt>kzHFi8aCV0zW)MP{y+Tz9wwDAnyYprw%M#m%Fls;}SMjxNx4=sZT;?WGHn>A}U z_rM5*LLLdV((iKFHBN$*;~#O_N(SH73I53oYUb7XBac)IUYx}O8p(q)(5Qq4ha)sA zp$VOon#$m6$soxLx>@jOCIiJ<|Gao)YhvMY>Iz6V%zy`zj1-fp4P-KzP5h<^)sn+- z4i9$c(1Xd7WeIiUQGY$wEZ5c~B9AXwOi{(J?ThhbAM=XBT@hQVT^M;- z*-}kB@~E;Ue_iBpEj0+l@U5p3dCQXCiupn_abiZ*L(bDp6*%u{9sLT$8H;1P0jfeg zPEF-+ip5W#n)2NdN)F|DTPyzstcb)vP^SZjlQ~;L&S{}kw2LQ36+Z`LIBY?+6yPuD zd?laaSAxz<_S&V`1?jx3x65SuX7LXCMlu;&EpCa$&q97bQQMuC6{(C^CsQi1_^XqW z(=kKB7zlEH4WRn%MN}tZV@j8wj?dnJ`p`n0;D)=Yec;K3&WWW1L5!kJ_ zp>aeC=+ana;qa9}4q2e;DJ3p;7uNx?AVT&+1`@D)B2_uOKs(S85i20ZZ{|D1tb9JY zF(ey!o0%S#vk~7jjZ~ETIN;a}cN_CKDsj~wU{>>N;IdWIz>1JH7ztQUg3zYOW#mka zgaIhY7&9SEgzxl{W%$mv2U|*Lkx5$>^$k7$9JVjZGhLigR#xL#(W)AZXVHPW^`sir zr!-(d#5%mQEec{;7s)UnLymIm?$c`0g{=I{53Ra!99xmbt8{|U$P+(Ko{;&$q{YVS zZ?Rz(_*uQKL|5k_X}2lyL1sV+nZ1l)<;Rd_J7VFVC6i5!6ZD~#N@ru7(p1B3QA_{^ z7{Y8?Fe>+`!EaZ8;JQ> zU<~4jn2^)njg3Xx%0vjaAcO?j`k^)L7Vv|*VW~zB$vmnjb7<#xG*^^X>Cr<7qZ1^t zIHFys$!V3;j9lQQ7O|N__O)TQ6|cJfd|_xwJW=Btl{{qo9av zfsrN0@j*IVELSIApll5Slm?F(I5bV8nCfd(5r_;*xyNs!=6I_^F6I>Ov zDm~r-0nw_o3Znxl$<%Or9iM?(`~b`dCbPmd_>tXn+@qFlD>=teH=z7l+0mv)`GU!? zX#2L0+24RTHS9h=n4tN>HFS0>YtM{-f)#Jkio~7=>1L0F3?C|RJt_u8D(81JyB{MQ z0;H)#TXtacFkBR$AzX5axT&k`Y*Fdh3z4?6RxS!xlmbH7BD9#pB-AVqT^yB@ZmX2E zfTjrZ2AoQ1M%qX#m6JvwN(`E*ge<+q_&9Eaib6J#Mk=iFUQ|RXAV^^`sCT<18r~TV z(W!qVh5AP{nSCJc3Lvm-GMht(eM;pMJgZv1^ z9Z#*MM`g1eLlEfUid*feb(PdgH7VLB3#r`|=sp&}Zsj%> zqX^891t+~q$wC}6Q}-J1F0bqcUSr+5sXB6^$%a zYJ_G5Sq=qW%@BE0;N%s#%DkHarvfyPi=tjsB3g~oy^tE}q^^;rRhe!bMnx*=pDNVg zEJiJ5>`LY_AP-86tY5?N@)zjr0n7;mScM~-Ih`gw2+w{QK(PDepY~YS; zy+Ln0j67<#cSoFGl#+C3ZA5}w%MoeZhvz1*bC}ro8%d>WHsjCljKq|0nTBoXe5T$a_7qs1nzMd5=*k*oLmv8YT|Cw zx3DtgtNav+U&MerO!NO8rv>~@rNHS^FJg^@q_$W6lrikwQyGhg5bs`kO(gzda$r{I z_g3;^LUEkc>>d1a{Wl|VNtHG3j-;T*r2`TMCtlciyXmp+z}bNkEt(3w1l)7KBvjZV zKE^W+UYvuizIoZl9!EA@V1_j>A0s1s1kWzSVZUl$w~%wJxhG}CiZb5=Uczz>VTm7G z>3M8l8wz|cr+FblCa;RWvsCto_mSDYo|$dt+93HCoE3B0k@a~bJXq0j3Pb*|ZT_X1 zB<)m+0ltTxFNsPuf+xs_{!PiJI#7-c&2T)B3W}>)z!guqL$K`T8SIJP3!d&-zJloT zz~ZM>HZUddOi2fkhZeOX+9CVy_Ef^^axP9TvlPoG7Bzi45*NyX$#J(RAPP1H_~wpc zqO`rLoTeY&)s=HgPkBu&YQ))_*h`GOvp~+jgPh|?f}QC%Oa7T-^PX;zEcz9F3=&@l zVWL?~hdumb4wc$>?+T@2-$EzLPJYjFL;({q0 z0x_w}ciTy{ZEyKDzFPg7evq7HDW#p5*YrIyi2_;|N(%C5{8-45<2`*xq29mBs5keK z3)#yrZMifnQHaAMrlx$);B-SsT|OA3&;WAVYTT@}cZcAQ#9xK(9=49{L-J)G9D6bn z|07()&(bqQm39TYa$3^Ag4G&(ogIy52XVYYumX!cp$g^H0;mU8K~P6vw6!$a>OfVp z1l_4hddON%Im3%OQr2KH8-3T{#F7W>wYQ<{+IFLHW3^oT7)+MrtLVvbL(~J7?|q^e zY_B~N(iZxYVt(kp;IJ61*1N5Epxt^ekm6iV#FqRTUyH;=DV=f2q_Cu`-I35x;(Neu zTh8*L3TQ^+ zBKmA(IUZtmFo_G6d_xk`y0$q&Y_MfQzotRKWa7(Kuy|blvfl=iST2brgg}NBj?O@p z0j?{`S(V|y8PfJg3@870;^d<7T&#(_-5DHAp01_)$*VypKN;-GPIK{UcL@}}iG!N1 zj>Vsjz=JKgJe5dcS;>HCyfdT}0W?1j>^RaQw`;{v>S}p7d3LyX*cpk_4B^KfipBd+ zO7d{BOS*H7FzzR>I{9r~gO1(Vohyz&$88RF+h$F zlH+a*DbHO->B&tIqXooyI4wkb<#!^F2=9v^pWB$e}f zMplU-m%&Li3mU!^N-fO_O%#k8apNBpQK>7kw12=crWN3uOFe_h2`yca6|T+s5{@9v z$yxRXmV6)^N+(Gx=;X90+K$~$63iI{e8#U?{vy;{iL9*u;7oyHy|&+broft0Qa+ps z5F9-Ivcda(JX=6v<{soqoe6{M;UM@!`tabPI8z`vxF9&Vuw}riQ`Y!W{oBw)ksp`f z->IJ&4A&o!u59qose}Eid~5or`h^A;sM>+~>~zKdjW?f3k`L%J=N|AV19j%X7p`=P z?tE}&0_f`_-GAzt2l4s@k2t8Qoj1B>!T0AcowumQ8ohW)qGmMCT)3bA&tr~J_wkUSW;fpQBG1`$vC9T~4c=00W2>PtS^I_6Lv zQq7n@ItD#{6`p*}M{i!#AL8_;pQF=Pn}j+v`5SN%9+L#fe4Jxq6Kg!6YH*}Q&CiQ}jXM&Bgrr*4EhaPgQ z@8Odd#@Hr3?alr3#^n|UFYg)uMVn(d33fSyod_O||Onh#^BD_GL-T2uMBfL<S^^1HV$kD?IS;YWN%vyh_7O9cS{Tev^KF=g+|T z-2Wu6N+{u`H0qsyYvoZ|#oa(WU8mvR6w5W-$k&~yzeTI)`MU|P?lTrl{k~GeOX-O5 zGxhdbJTv~rkpi|%efbl-_Lk@W=mY+nKHz`o1HP>f_})I?FZKcdV;}INeZW8L1I{)B z`OyANJm6>ACj+t+{5TB(KhqAe^k%@g^@Guc*0ngnsDAWVt9s%5`J*qi=8ql+@I?TB z3*d{bg`>ygy_nGoEsWiaE-JF>7AJ7-(O8CKM;Bd)I7LiU#8gG!vg+{hVyh0HB%%Irwkl7ipCy{2MO##dMhXdE+NoVQBcd2o0X=q!V;<{NLLWUGM~4 z@gLR7?!|A*UOA%*s$DXEU==sP6r-~VPv3AhR*A0JEdLt^|0$2P^ ze!Sqv`0nz4gD?0IPE|GdyIk-QF8EC>41SDH(>L+Iq5XrGo&_#AeV>Wn;)1hIG4R`5 zaCiKldf-SRzh8Rb#=pMqft&b$@xV>K@AU!iukD)UGPsE=*8?}-mU-YN-<5s9pLW41 zIz!I^7o50_PaQ&`;%Dg5YHW@7z)k#+2X5k5dElnLzqSwfG7sFe1Agp*8$3Vn1HRJ* zcbE5B7o6qQ=3^c9zzzMMd*CMD0ovah{up?!2X5#u^1w~};y&P$J#a(kbPwF%iT44& z!2>t+tn|PGB5ljM#{)NfxYq+W`HrE{;%E3UO1)ZB`hb^v;DtK=RUUXq!>{qcO+Vl| z4?Lpd-{OIr_`m1_zR3eO^!&~PH|2V`5BNKMz(4H+o~_$6UcOEDz)iVkd*CI8ZyvZQ zZ-WPJ@HBhi2G5-yxQTzz125P4e&m5yX!u~wC{wPOhF{=;oA?zTxQSoY2Ygi@@Sl3% zhR%CEaFg$L4?L#neA@#z@xSoEO?-7maJT-Oe9vbG1V6(EgD2*J&r#8=?|9$_f3*i* zspBv8z)k$SJ#a(MW)IxpdB6iVcpmk@4gEX&fcMkm9HzV`-@zWZ!SC1QK=W02q+hKn z58U(z)_dTl9lYBEH|=0FmE~vfoBjh&2J7vNj0=l;KOaTBU!b z5}GqIqD~i2^Zhb`a$W}JcRmiz;z7~73$?4_e zoP|CLL~~DX`VIzSgT*`TwU?j)#{+ziN8>v~$zeB};df3guV!{FpZ_D|48x*(3o$vt zyoES1g}n}8IGKg>A3_e!O9*!DWd4o6lZC+cywd?h!Fa8X}s)IP}plUJBo<{}Qwe;iFba$s|F zqF`V%jwK$!Q;*Mu#Afrhu)M%X3iLF-@8uE8gP8PaOG&cphhO5vQ3H;z*h9SFNN8d; z7|?bmhSxPui=VOA{uL58{Gb3kqw#km&aqf(*nL5quO3W}?0P!r94p?}bY3+60Liz|SRPtg|DZvav8=ACTkTsWOe zNLzn|I4j!|Bgu)?;@h0ctc#^84nyg2;D=hr zgqS2g_7Z?RmwPW1h>*FwJaJALGtMis8uy7MJY%<=gSk^#eLHbQU}w|ivE;oXrC@3p zruZq=JB>1*PjadGvG^-mKe6b9hehP<$Gl=?4sJcn%Q?{1K}?MgWmKtYIOptW{~6nN zc47AIgxz`ztVI#h_*xH>Y{EI*6@jB=@tu=fN^U_+o>7OGu-3-EkPg~)DCrNce9LaT z6b)ut=qA9#CM$l9ZTL?CsCh@YNpXv#l2!1_4U`R1kJ>}VLqx&;ktjr3gO8=J3 zzrW=(MBH}@X7E1Cv0HcIf#r)pGrvt*#NSX`0`EXmUV-`i$zJsn#EQf_3HE&k&~|&( z5`dsA1-l`|%4djTJS6n1mQE0KF|l_7`)H;^-lf2v^FB>wkKHm5K)m}-i0RlZpMYoZ zY)9#WXg3j>cxfz^^AF(-k}rTIy~h_E*`Akk8jmy?bt=`iWB~R4gx&H}h2sDzdQ0er z9`nqO{C?E3Ko)&Y#GpaOvp<6d(16cIao(_3#ejf?SOAdJJ=ENBk~k6Aiu35V9(wa| zWne3Ftwj*DuR09$XLFrLDHf}+M7-K`%&Ya^qDC$tCGQ9l-nN)`5|U~l3ENk_3ruqb z6X^Ks^|t2GatIAPc@tMJ#J~->l5xTfoSajHoK9u5r|VHE6ucSS%K8v!D&NE_OuO<6 zp!&!1!LE;XB{Y@m3sS>`j|wN8cQcZ*7E#q-XV&E@-&b(25$Xh7cR3mCJBtO*#-F)4 zV?o5@h_9r;J6neX-){)Px_i7=z6*{F2Z>{oXE|rvt-nM|2d8{28=$TUM(t_0-U=9b z)`@6^$so@1&y!mO?6o!cB8$C%5A~h14GZVb#(6=;S%=})Q8{JdHk{&dEq2>^h#!r& zvs_iNly3)>6Dx~w3jm*n@)`hp?oLp35bt?I8y}<}1PAhk1DC@wq6Ft6PN0)kSqIwB zr3VS1W<)Cj>LEpg&pIO+2A#7kb=?6P9aszqB7@G3WPVuQ^wjuhuKp2D6c^UwG#x8B*~gU@IyCp;P5g9WxV%H*4gg1Ow;_! zQp3{mge^l^krE7Obl$ULm{ zbUf0bJh%L8>Ht(CH*f-CtJFr8uEUrRP4At0sZ}KcZpEnl%dtPg+TIRs1rNH?mm?OE ziW^D429eUC!K-1(52?p;qr9Ac=^x+&b-NuoQ#D4WyKr3?Swcvr(#Ep5+)+oOLdawTTo)S8=loe8E&@Agd!3c#B^V>#Gui zieUqAMM@TTf*gb}X7Aw>G*i{Y6Nkc#AWmumYvEY@1!OlV>AO3GXh%XgkFD)r@Ez96 z+>VBa?~TSkZz*}=Gkc{Y=_&#TaYsCEf(cEW;rro5jYS+g?~cNI7}N$34zt-mZtY&8txL(rX{sEx!e_!-s-f z;S$lPRk_ln%YoB3kT|y2FVrTcx(r11IkRSE@ir-Bww0(Zb>6QmM@7(`NBvZTKZNDF zsA56HnTohv7jg0t+&R*+AUAF1pksS>FTsJM0XKFvr3H?;{`j&yl}@uaV00%|GL z2bUQCIicyIh;tyL_otGx;ytT-=sj+h2^^yLe1>{|D)c^85y%P_ziOvGgZ4u$$sZCL zjei(+_J^Tgz*Gl4!g?#TrXC-_6s; zv%sXcg8i|mN!0-&)SE#aq^HCTw*Oj${8Pw}-UL1ft+(WjxLDiA8=eBY!bz+~EA@qj zKZmP>epn>8_n@SA6?fXL7f{$B9UTVYD0*;c9OZ073EmV}vS$U{zw)aTUsXi4{u?6B z%Mi~?B*pYSC}VU;azwmvU#C-Lnh^oz*Xo2O96DX9?E^;riL46@K?bO^RC#y>(w|JB zKt*6kq(AIhBJ)_iY}`6)6||IhV9nhDF3H{F1t7BWC^RRpAid-h(G;Z^#IzpWIa@; zy&-G`4ZrN<6tscP0L>$m+sEBY+5+rcROH$KAUc=hhXLdq0BuvqBsVJ$7Lc*;%ev!=#N+Mq_bxC@`=jyplid>@Z_!yEU@@^GWr(fCIpoK1#q z9G$e%Gz`q_hTw#;Xzu^6gC}jW`2*p-0!xJ0XYGf8k?9J#b*i#tyXYV@C^=58nB8En z%10{I?m*Iz^G4BY;Z!KV#nx}bBtke4^{u7^^0@%W)%|4H-GRqwd^hRn?7~XeakoLA z|1IKt2(J{Hc!lqW(5_Z-6Os5aU^ruT18TC`JxCHgn>Cy{?ETxkAFf_4Zi*E(2 zs$KGIyqlbFh{Ze50ob95k}nXPzo{bPY*TA0DHcT!Oj}6^!vPu#4t_-2@e%l=cgQ?C zF7kthQTune@wvnCTKNOD+;W5jMNuB+_|58oY)?;q*A9W_APAaIP8 z_F8qcX$*r-`rub8>GTE?zMI*tCdcf8Hwexzj-(Plw>miW58y0TnunHOX?#E=z8lxW z?`BL^%e(4J41!1sw|$3_r(!|svYZa|p&_m;A|Z7R=oxJXLwvYMA%D6Y17})=(Pqd&Iv_$@Nrj@)gi;OswKtj z;FNs1`p!G(40TGiS@tGJt^VY2X!yQRYSw$aX|e4gFfTlDHkKQ2#uM9{bI^=J#=bqs z`4nQeSM9@JfWE{xK(}3wi`dk+dNg{W1h~shPOe3ylF#w&M998u7sq$t_i{t~Jc(3G z8HrwfgpChG$wyOo%*!Er(stC~jq}hUf|pZ^mZemY_(8zXZ+f14Rk33Q@+47Ud(Ntm zvp?kI+*6jy*-wv(4hb9{6y~ZG-^L-lt0)P&#gg$+)npfrp!3FD(6tjhM2n+b=>E## zSUQNs&yj;b9v=(f4)R6~Nw3DvnzB^IQ_>1!CnxtC)XW-jejKfwm8P#;(y+YLbg-zh zdjTw9L-wktVbmD@U`LMKrn&et@Sd-wvUvm~Lc)S2F_SVvtoA z2xVaRCcQ~6!bFRh)lV}@s#?UgfdyscS1N}C3A{Ft6)5Ua<@7-EIZfl>@@SyapToux z5hS$}4e@BaM0Mw01*yKzQSN@5 z1Jl?D<@}jyb#(X+*efnf9Ciza($Wte?doC_{CZ!@+H#|6^WL;^ zfDhB(7(A@kUSO^9+J4h`b6S*lpIy#-&!);6XT84pKl9$R|AuRT4WA6_O3i}~`&t7$ z^lK^tcMS432>J$|F`Mx6g&)2p?KT+ikC zX#xv9U_-wNC*wQmr>}DV3U&H&HxSPzodNf?2KWdUyy9p0MENvgE5HtPf!rM5VH z;4S~Dfc2LDk+0$deq&XT`RmO;^TG4Lc>fh^fJw&x&Dv3n)Z-^ru{@bSYDwEE8GaeK z@{gxjxdxy^HGO(YM&Hb5WyYsiegn|qxe}gY6?ou$?QIPzLR{=i|Y&Z1yd?RRc@s_)Ctsu@{_?myi=#B8wUP0tz=$!qh_5KZuYhi&b$ns zeL7<=JWK<{&x_}iKHy)aLhJZ${6o3eTf^P-pQi=p&G#A?+#Nqc!|Vmmzi3D2rSnwR znnpK0m0#}0!=e~I{J;fgQ4GAn1!t4cz<<*R+>bc?(cVat`Qhg3>biMLM=u$pRsv-1 z)hO(J%|OxE!twZo-#>q^Mxm=gP%0Uh!pG$3do@nzeU>-BTL9v?5Wvr7wO<<5M;Se` zvOVf_*sVKYZ0L6gwpdDy3IrebBSz7CI6P@T#5-CVS#AZ&X&(lOPtd3dDpUk3-0m- zFh{?=s2zQcK%tNqN#O`?=IYHR%Wlg8O72&`AK!rm1UBOdXCO?60kJXWkEWg|g)9V9 zQM(j1=6oVy2)%`2XofGF9(iO^__FARr&s9r-B7j-zy`bZqF$kc+)%naiTfrjI+I%l!yf=` zek{<8Sg~c_z|+lm3N9Opr(5t;wrnt-TJQu0Uf(N*>29caB~tdyUZMSZg&u|v%21ZU zu(ekxXR9-DGo|=Juh2&q-fj;ksW-#2>E3~;+=#_3La#nm8*(>y8Ac_VakCuUEF#M)hsH__}iIY4& zlON=UZ9CuZ|cO9lOU#<;Hm1Q$&nt!i6IP<2MRde_|SfknNH)#FTilm4Pp25yrA?; z3RbOPzvO5^nn!~nUZsNF(@xlL6b!X5)854uD(RmnSWLm%wh*>bV50yFI&E?yVm%=t zdla$_JNF-9q$&l<#j!vbV#|AC6M<88JdhH0epvw7rpN1eECu2%AD=#7Ls$kt?r&zw zJe?8*hn;&i5_ID4k||~mCEhRB6PVUO77U5+Sx4ZD3OEWS3p-aXNbl6|C_&h1Tfiv4 zBS;DmbnahG$S)KG^aq_MstHLc2q+CY4^=VE7ZIgcT?l*+HR>CoEsV$Pfy(kT6TZj8F>*dl$!1xrBP0u!9Ol3Mtg1 zgzXgADDWug+{4L@^uH+xVg;SMatYa}(y8c2!(e5ez; zp1xWGNiu-K2X zXqdm7z!x>pFu$F^Cka$#K>?rGO4zRjHVX039=&S|K--RFpSnNjtm1Qe4L%2x0ta=7 zb@QLBCN!F{5-ekdn;0>!a2tVbBy75ZvGB<6m+J|e*dxCO0W$gVIXz6}$LEvsBfo&> z9zSL95k|#ne?Shqx|3;-Xb|GTVAc_|k08Y_q`Yf2VGk>q5py$P|5GK+h#JrW4PU(sUP&0%MsGC+ticm*&x6#)21*N*__LE2zJ>D= z0s{0yTNDH|zykb)+#`^Tbq*!$HU%RSz`}=e30tmUWCLLLv&Kqatzbsj7GcvAjOC`_ zI?0|13dWLCBn^a}uV5@YV6q@BeY%37^k&}2UUlNQXk#I8uYA4#KW|xW!XJHjNgb|7UwGrZ$dfp`BT=)o-XC?I z;L&Vls#{#b?#@kxXgK?3IRIF3lJ=;uc;31T2+fj)ZD7GoPHTZal9Zn8+#Zs z=RTo(tbVxelP;Kxl`t}~CQv6YxTd4~q(p97$W{GS=d4}9Y|J{|DM$84oU?aL;%jT5 zS&|LiRjR(OmRJ21ecYp~}s)rj)p|2V4g7*=a>T!W2^(nty?_5ri*c&js7g)XczvD-dCE0qHp4^5CvpE0nAh8dGGPs!o}>2R0t%z()Cu>64<5LvAuq5>?0el{vp9+GGCh@wAY=;4>38h)7s=o?^_8h$BN!!M;J7=C#KP!7LrLU{7< z%XqB?D*~mW0&37jDh#! zCJOPmF$`XYoPp`du2?vY8|6OslGMG3q7K)_bZ%~Xm_*B!YfGS#hz9WB^eHMeSLZu- zi!1+#YmCKZPs6eym&?AGi?-k{#)A`9x|MH_^HI5MoHUuP1FYyh4AGTB8-5KuMbFz0 zRpyt#3H144kxj*a&-VipyT(`hB&}f5HR2z2()`oxwfQ}hp5jj0iN&u)&m-4R zeks?7(gjP>&}!cVd+i}vOq_vO#i~gb<1$p zK5H*jcv0sM^nX$3ZdG&rmNi$@S*vQbKd=JBod>tF2IC;Fwz5w^%TnvS`t$0o85qXL z#RLAtf*Sw)g|#&k`~d2aUhBNt`Bt&!GHd4aDHSuqmsyeEj1a?$^5~UC@^`FN z5u7yBs+b{vr$ncPtm!jC;Th3sms#aAXO&Nz9tvBNgOelSnbGf9GYzC1oB5^$Cs$Y% z7*qx(`~{{d;owZXFc@n^r%kDt9X)|NtX~--#3;BE}Ji{uV5em)-BhT>6Ir4X| zRTi8xBRs{z|FUp}6`CaxX5u+AJyc}rhq2c5NtaKXKBLT<5t%9B%;;qZ!`965;LMp} zO2ff2!FGp5gGSQedT;XgjkV73xQr$zB)26!>!GQ_T!Sr&}N ztcn<4t;ykOV55bIL@H@BiPb}N+RTb@IbxKR$B4dSN_5VZL7+o8GZvgY)e1=vi^A4I*Kr9T1Y}v#waM>@Ii&piy4#}!k`Z4 zFq{&N#TZ5xFP~Stu-ac=lSokWsBu&}Gh0z%m5V(<#YH3aTm$NK(M@RWg~dumS!Y|* zf@KgMxL$$6iF8o}3Ks#$VlZRcW%3riY|`{upj1S2WiV!i=ak13bHR(UY1Z^v73H%k zC?2xbv^J25{u$C2FI!ZzbYc9YBA9sj`oX)#r3CO{CKAFmx$4Dxj$crp;fHIrafTmH z^;7YfzEVBP-v?P?;U{Zk)JcB5oRoLP;NviV`Tb|Em8nlG#c?>+=%q`l=OyNimRBC2 zr(XY4SJL=VcsgdyE@1`#l(5qSYiLTpp<=X4d)u-w9VPj&&BEVM74*4= zrqz?Lj{Z-tq4{Uin>94vwH7A*1!!-&f8-T^CM?y@-t=3~W-QC{%ir!lru-)TbbR8o zp4!etJgC!~_~wIoH{m>;-l(yOcSNV>c!>LF(is?SMw^fUsc?)63N@7SV_K8m$nRRf z*a-Cg8mJh({7kvcrOr(iYUUVCHe1**5MBHYm}e;@Gk`+#522mGQw;Ft6P4+Bm**J+|m{X_fijZVuf z5|PD2#att_v>Kx@i>&!e>S`8S^RKB%#I5@J_`JpQt@^s93l}Hm%RyyzOL2zk{CMq> z`Wlr8w~}3Lp$duD-GIEb+6w2{(>A|=exR(;suich`k8UGd%Yf7b>zwdYLy(cG! z4Wdk@JD^{T^jzZ z5B{o#@AJX`qTxkaDHuMyui+>7;D&4HDL(il4Il1< zU!vh-eDKRP{8Ar$wuWEhgEwh-)(2mt;lK33TQ&RvAG}S&pYp+P)$l+2;6K;!&wTJ- zYk0XfkjL=h7!5zs2R}~32m9bBX?Vg1KUKrO<%6H8;otSa zO@Fq`2d~xXf9ivOL&NX&!AEQO<39NL8vd#eK3T&*^1(0F@ZKC4<1+lYQo~R5!N05F z=lI~)YIr?6XXRw}V zGCMDvaeR0Y?j2t()o??noYsgmKlQ;oH2weSgKyLDr+sj`JMntg2Y*?^OSDh7$@fhS zKivo4r{O>M!J`U&;7$!^GA}FKhViKDcRLSM*f`lME&?a?+~dhW^tu zo=rZuXw$auR;TSd%-0K#;5RR-U(Fi%Jx%_0!p%Hu9+t4}rrC%4+7Ii& z*Z!Q^;b+Sl^gr%vzbmFQ$E`sgvE-wD?F)|f91!qOK@OafIP-8X?(Q{VcN`8;eW-B8 z`hGyuIPq|g+;Q+6?nO`Hvub?qjxH_Q9J{6>wBk945qHea1q-+vm+XkQ3qNpvRVzI! zIvp0iiNu9pElK=JJ=A#RRaeelFgucZI6cS4!P296r1z||QddQiGm?)kn0Mv;rYmip zS;=t^{(-1pH2`bc`5jkF(#}u0T9R}g=4uI7hnR>(0$D5M2fj@MhSzw1TeKPv0Cg}4 zuDJW%l7r8oTy%qSkKu_n9}D=X_@m4&9(yXZ@=ZLTkP!f|hPOlxuxDwRPs(n`j?Z2# zi;f87Lo0p;NU~V5&7Fk3_m%mrR&1a1g7b1{#r1fJz0MtRXE*mWuL8QH{Vdrhh_dxK z46An|Hrlq-|2TggqO;))L8j4Z8*!#o{_%7+`W%7=-t8U@?~t<6s1rIbTmP};AOuL+ z33M0N1 zLDaFK?z~dum-!(Cn)zuehP()1XeBiU*IMHwtA+coKE7(8^2C zzz4cg6GPrK^%{@8+>9;g2rDH;_~<;l>YkSR-{cpai9ko#D_j-Z)m%aDOGRUQ^L;wY z@bxYAx8|?gO>yL^9wIi#l5I=CmJ^(6$nc>&sfg8&5eef!*_wMgUkt*rZNK45%37oS zw&nIgw1L>m4AW5h!6>KnPKItMeLI7=COAM6xu>%=560c8h~PLVzZFcF4$3m(&4*_scvwx5Py za20aall2`yfW7alOvaXTsE~K&S1MnqQRV|-WUg!o-!bfIwBJIWzfyU=9&cInr9ghs zIi!f0CSt)pnf}bQ&XXyQrlOw6IkqOB}>cPm!ap{STV8>U&c9M2H#)zedTw-nOuZa$|G_leuzHxlO7;(?<2eEz zKTocM9@TxI+dxg_;oQR>%H!NaZjd&_A`@f9kael+H>*D2U>cfx9gqm7F6y6^!>?2o zSal0ao4+Oc4KPUvB2eR0#3guSJu^AlhDZRUhoadJui7ozSjn(+c!OH1=H06Me(8Db z!N>WW`4ttT)Gi%ss1y6~t{$IIPXXw|1S+%bAbwlOk4faT`)ve3JP1HxnV$8!3qMmf zJPvTYJ03~Te%_r=O`}H;25Agrk!k`pF(M)c%L&)ve+;Ml)h3EwPZUSL9?#B-7PY>c zfq>l}Jp@!~`(65RuyY)ft-8x{XMmtL6HXLgUKeOWq$8YNjF&0^YAT!zIh0g z(Gi5qXAx1fdvlku1iEN!Ml1>S5{sB>X)kRL{vKIDven^rkb)Y#5l~i*Lekk4GLlK+ z&aRSPceS|Fp}&D-&jclOD1?6@oQvpW7-40tWg77AZrQ6@r{+`?U`H3Jb0T8?^%Y4J~n5K*|g< zfwDfHr5o}zwDpVlwL%JmT*~k^?jD1YMrJrvBDok50#u%eyc4SYfRUBTqBxFrA|t6? zv+(MpEeBCpD!0a+gA^3u(qC7{3*SW&K}z zjdp`Iqul7(^8GbPkLWq*&Tf_OFVx>FRiG;hELkIX(DUG6ZxoVs$P%U?)~pmZ%D6i^ zLKooCk>u)xlWq-+a>F>*v8wlIw+5m5pcR>@=5eT(zEhDWfL_#FA z+4OrbfHmUqW>ZK4KHe^wjCN{nrs`CjMgUkXbvpc}1=uoeB3NCmf@$UqUV9N4deTk8*& zHDWm*B(k%jUnFAhSPP6-s5RN-(|;xQRfWs#_cz zTnz=hLJAgIIR+n+SpnR^rb^uE&PQvSQ%ZfZLRv+C1lm%y~-sv9U`y; zJ&vMn1!L2<1e{N@nO;_Gz4K(|v4Xb0xp4eCNgA82l2TjQL>_@{E7_zYK!VC1U~~c6 z?+CYafW{D7ay8b55kS@{{S8&<1pG3JAmJberBZ8TZwC+=wx;Y{!kG>==aiPj+pRAl zZ+>S~8p3xW@mO64BCz-^b%ncgV3b>RbUeQeaw`@JEg20fit~~FCOfxR>{(}T<~bQjzZzW3Qh>nKbpzHsL z#MfG2qjUz@n*P*LO6}z0*{NvRdr_V#pFs(;NVUzE7ldaudkm^soNm1sQC4q6xIuXO zHz5G4;uYVMaCZ0zn5?3+HDM7Mg6o$sLh`q*x#BlC+wb6lYZUp04% zcv!F|JG%~U$Iy-HAFWZSth-(AIDr7~$&UbDTJE$`D|Wl+l+cPGY^O%`Bw|k`oGqax zZ-5t~)5$A*MWJi7Uqflp)_*awxMmG|otirhH=0w4Y@$f(OQ|N5ks^Y~bU6MzznO(W ztY}CxPCG_XtZcl<>A;}i21L`v`PG5Urb5Zov=UYHJ5~uQ;utWRJx&J@Lcbu^G9szQ zj$pgGPo@@>bG}U3A9Lig4vz`vlY|qzuhHHx+NocfaKd*r+Rt+WP==SN?pujFJFdtA zTh(~h>1bv|I8iVdv~59YI9}GO0JVv;vx(S^E>77W!ZINel*$JCS}vOC)1lTGFwUrO zclq_i+mTtPapyNWyRH5?(|*Bb=-J=F>HI7B1fJDsFj^@c3bh>tWzwlFLMvau20vDY z;wP+Ps#B&Iv0g;VaVHZF$!`onAyycd--t0{+Ic$-nO%$G@^E#8T`RNPA&KIeW3A$v zvUv9DB2>_rZ2bz$JrjBZ=NnB3%=eI<;Jjox^*=ZB9oEAh$!&!U?lHj;^RVTf zY86NOCZMt)j|_E;xp{wBsf^Ga+*v%S0yZ;>r8Hp@gi`J2@AYqB&{%b%6;q8Xlzk6x zSwMGmhZX`p{mXCs(?o&sLq1Kwq42Hdw{slPqU* z9NoL+hL_{#4e{8ngflVFdj<^LnqwY!p#u)70^$QL9ePVsY$tG_>SsWF)FR)V9kfH~)qWX)GG$CT$~1uo_j8 z>8b{AmJITn#@RK$NMS5A!VRA@qBuMxzXe^zNGCirv(77r7jV-X%BJu^4nZIYH~N7U zy%$+w$b2hLib0(w+(lmj^FL1%*Y~rEtNO*Wmq2401-chHqW4&xa!+z22$;<(YP7eHa%%oH_jQ3`a>gD96HwMD%Qu(5(XN;_4Sut%ig#-63`-5? ztDLAaLJww+Fie9dsq6(67KD8<`GZ+xzcl8RD&S7sy$)olR~XzogHda_Q{Q7%sbO=Y zz4_qIk;N51$UTXb95fqcX9l)PWRrUmdoD}{f4nf7j$Uw^|8L+oOh@hZ8i zK?$ay0PsU_0+aM`loF?E<7Ty{qPZgzcuwW}k$Ef?d0vfsC(kguPa-gPnasgC)8E{1KdNgd%@$960t{8y= zOy`$-SHkxy10!mI-RX$p;L_Y54goP#%XXnccL)e^L`;U_WVUJf--J#>Q0{(g%oi4b z=#>2mH0#fEmjOn>BI86@a8KC?FROMwnTmx5!f2ITNAtnp7fTyYawt*}quuoVpdrM;M>v|Bg{|+*vV-Vt7?FZM%>5GG2TcLnaYuJVQ(SJ5bSbdKFdgbq z)wk+QS>HtgE&mvyJfYHMmUWj14PH@(tgOYEmYx8qiBawj>OlLwk?x|q zf=(>=GByiC>re-=GULvRm`R6LroawrZPrTK{+um-wG`Gf4bz3+%OuEg5AI{^@FT1- za*>tFCdt^$h?7mZ<}YHLisfLb55CB+xox6lT&XEvO61 zR;3XXqZy1KC813rT2mleumJX>j!=5lh^|IJfp-gwkcWOe8V-cw3#C+JL7+T{|-9z@3Qsd5KG9u;d zg55KXC-9zza0`$42aZ5tvN)CePN}?t+0m5Ca!;g1eEG+3QC5}yXH#b}s8A7Me1#an z_<_(0V`v#)g4NQ#k;{0B=~@9UQ43b=D1`*@oYT?{LAQuKRvP^o{$LsuM#rZ+JmkDf zzE{J)G_15?oe7%(87mA$HBA%cC-|lyd@;&5ljTcNr8jWTd@bt{1z!}6NYiJpP04Ww z;y4&*A_|%-kD2g>kOf-@3zL%73T#?Es6JU zXGN>(LPY~T?fjr&8b0t4fVoxuu9RK+L*(AxP3||5V=R5kU8;oCrYtJeNfNj$EJ<4f z7et_F6HZMu)W*0}?D3e$6zxxV?y`s7RIkoa4c58DCrgT0t;TZRxh%nIWX;5TR0Xh3 zNDW7bI8X(PkxsauUMG>oy4XRO@-~w*Os3E!>b@Uo9zw3I0St3FN-`0jXV=K829qY7 zMfY>fJ5l$&NCpS2%_}9h#-oH-oKCfi!<9lO%iYQtr&eRZ*vIc*%%kH}7`gxY+UBU>SsZGi=0+ep*egQYBxL*+cy z4z6Nv5!}gG-of_FQ$#3@2SUddoU-3dW+&YFH@GqUk?#~ARR@ergzv~Hv~me%k!(PIofU?)ayp1y#)5+m669x9uzNY0wo6w^Z{NXOrjw&E+=)?uETqhU z!POu>9;@gZ%5Im2cC%z^_-z_y!r6%hNlYJZR?-^!!wZR`XWYg>qG%_u57>uSTp++Ka9a!UaLp zPTZ|2Y5llq7$sG@iUNPtG|IGCoQ7~r+OGM{h~k=iotk5CVo(HBINOm~=0~PsJ>QI@ zp;V4~6%6r=RZG+r?LcQGHLM7W6pK!tD?JZoEPfgm~ayjnk zY+n@_q4Jyy06LQ>JCod$J!%*w5{cr9?C1v13g0Dlp_ZaVt5Ei3kj%j^d3H!3i=rj0 zr@j1vqVAFVeY$VgGb((6&bdOGl~gcEw_>MeiY@0+qHWsI{bs&>{nS_)it|)aoX;t;tDiznt ziW7&5_EzOjk#yeIW$fZlk^Ai{EP-0ZhM^LEE3kCAx%*yL1zShP+j|xG_qj@dwE_Z% zRrQMVkQy1~H;uJR?}o>a12g-p#uta^mG~=JwVbNK7;_KI&3=gk)VuWHNm*v}=n&bW z6WfTSC*(#U86`tCo!bSMY3$G#MG!vdex~0*zp~t^NT2;~9-6oDi`>V%StWY#^e5v_ zm|SbkxMJRn8Ife_(HYu}5IHBAEv~LXbP*wYjlo4`f(LFd z9MNGX?fvM>wRyHQh*M>7gjoarG{r5aJmF3`B9O?I9+${YhOg2NEA~S3=~xqw=hx%x zLyYE1pT_}CtAH&L+Z}g;%ORtsPi04WL$&`63K@X79EN% zoS1YTq!2?fDKT$RYN~}*k#v4b210ku#0|=cI_Sa+H@D2g9bl!Lcwoj{%~u=BNgbbq zSWe%J%eIb`gEOe*`014M49}gOK$Fd9Zl*^DG4OuMo_ng=4d625I1= zwYK@3q??Y`rtHVWo2ES|BhGdX7C~Fl{+8POq9fb+13DXSE%1i4>ZX-u9w+}B6m|=p zUn2HQGiGBL4I*VDumLL*kFjg6M~nX>$b<4YpXoBYDEs%V80I<*6`U$5$ha%I7@#yui~#};7QZ%}D89A~>n$RIsVvOk@QzHxKF^#_>Xsul zyalk9VL2yS*|8XYzn8%2&I8r@jr|2p`WrbMOn;8?Z5U^hZnR=gj&Q;)9Cu?GJo5r` ztA%tvV9qW5HW*CSbpSay;hxSF)lrir4fJXb(g!8viZt0Il6_>6am8}SoIZJ+slv2Iq z5I^%SJn_7&E5cUs6aUmHg_^&%8Fz;PdDTrg9p z#^6qb)ATd!C7wuSgKccaazJ{6YRhr=LU@?s1lMOWn1`sTb;3Oj+{r+*D$>~$NKFby z!)B{)At9sO`pVI6Y30alJzNvNu(IP&Nl!H&o3ybEmA0e59hndQJ4^E(xz8fNihb+8 zJJ8W2jna%?vQg+vtu?ySGje*g;FbD_8AIvq0BNUH^(-=gN;~&cPqz0wa^f(CLlE7g zd`|=*9j#$J{u&nXZoH(BZpR(dwnWIniR3k9E<6j6!ji-GPLAh0=nwuSTUe?=3bxTo!wAuyl#Vrc6L*hJt$(c-45=a5S!FRT z;hg4<4;PP@1)0T66MHs$UXc|_=Mu4va0cmvCSp0SG*3v|ufQn#6Bx_Q^dEiPk@1#u z^NWVi&pZ7x2l&0I!3m^NxC75zlP}$gBNL!P8)fW(>~Es*ZO+^!)LCvrn5%?sEl5tf z*UFuR$cEc+grqw({}fzLt}tuPNvtK=&$*>HrR^Q(>Bi8Q#o4eeb@@fZkZQuS9#ZN+ z3g&XaiM8q-vcj`Hv}7lgt{TdK8q2*XnBN@dYTk475=Vz42ccQ+_y|Cul{9a#Ye83| z4(UZE#{(mluj)J4t3_6k$E!#?*zdm!DuC?(I*5Y$Gr~*Qckx-2l`%7pEr}3RlS<9~dh3S=0&p;i>>Q$Dj5pVH_F>nhf^&_l7V?chzoBc*}^Zr^;<35udO$XGTBx8{M| z)u0AkKLK3N>@nFwJWfbs7MO6q4EO?^2dJbIqmN=RpsWU`U}Aa$6kzQKbZ%)Pm#`0p z9wx;salb};_XwxvCm8+0&75Kmj#u+dEf)*tQAL8gICssIg zz;ofQatPm_n>!cZQRtT=A$u?cqQ#Q!Yj(Q{axva-wyxTckPd#Xc>&XWSYV%JP3_FT)Hw62Xrw+BdJa}xQUB!aEb{gtkrPNaj#Gtu6pp?fh(F~6b5lR(P-F^+ zUcutrDOeH~!OaJI<#7HAUMZVy!hs2V$h*01z>Bh7xM1##dDCZHHGf9Lo-=Fypvbvr z3>i9Wdf=i<=Us)cWb-3eUUAi&c{8S;%Q$(%@nY<_v4}i3?n2pD$3OamapMyg#3!W2 zjWurwA5?ug8Hh`7d$2ATH-0=u>iq2NS<@pCD-|CtCga{^~sChPmw3}XA)ZqE)$!qtvKPqVZI;Tr$JH$0vwj#rk{B;u@LZm zTdy&F`eph3ilEh}GTx`U<;bPzBIRPf9AKI2es1Q(<5gg zYj`2`9{qiF0iqtv_vU`7{$4k~Lh7p1VuWNmh4o#As8& z1tdmWH4k&_JmoRQpEUv-#z1VBGwGapoiW-xLEA3BYWmzOuaLLSizvVk5QvL#>Qa+_ zx>jz4mwMnoIubu{`O|OFaOP>o7EJesCq2Vp82tM^aBse65&(TUA zLwxW7Dp{ab!~Jw7Ja8}mhfh-SY9}6V zzUw`3R<*%@K*LRWIj-S#$dlg7w zLubwd_olyA>pMR^&*%#B!;jSZ&JRCb7u*kDHng+6*dI!@2cM73jB878SYr2p9GJI&YI%f61 zan5Q-Z=E@A4f5E4Sg^<7oYh9Bo#&b~I@>ze8Yj^nu@1@T;D-gr30P;uB7H-_5~htZ zv~H|nGMF9imgKUCzeF1=u~aKSSmF6OEGl{r2C%|s7M3us%y#Gx%iNY~un~)w3tILB zo91ExGU^@|+a7A$0x(UV)%~|))2bW1-f~2Lr)?QRz$6q+W*=-oDr?}E;|~B|AikD= z|HMCP6(29L03=qQvOX}=Bw!=ix|i=emkVLCv~xB!`K0PaC?H^!l+`k2G~co7gN~w?U2PVcZkCw3sw-%X}$!wsAB@I zu!IZ3C4M--b;vx-sFiW-?rvNYND^>Qgric_fje?cIAF1p}mc661?AO3skCI-fGqJY9T_}pe{7$MbcRbK3;bKm? zI@q<$sx+7CxHgUb*(qmde4PEY4U59;mSZ{rjqK9?_=^z~q;sTJVa@MPAhi$mg z4ljm<_on=HM|WbtUY;$gGBeMql-R1(wgJf9agoksos{{XNJi`9nFiP@r5@qrbqh-X zhs!e9Yu)^HN9KiTu5cUMa>B0qJr29gV;!o!X4WfUtRu9t9w<@X2nS1b4y&370-e{22 zjgBZRp|*BNkt=$~xg=twcVi$DX}WgqjB|0He&wZCoExxaAObV&pG^_5aKa`DJLixg zgNDtBs2dMjfSFNQKk*kS0vE+E7>i>)aArf~f;rbjj8PLvCkS%ZdgrxRJ?{U89^#^p z{I6|}KhVY;W4nXZ5sQg1{L$tYr`>KTPD6nGkrIcCUA5QdSVujM3*RUE^h@`N#QRh( zJz_;LTin)b6s?BpHFmd*9C>b}a`f1V{UgH$4IMNrf=+W-_3-MUk;)5ZOphclZHlC? zylTNh_4cfqp%|nLnt$!=rb{owy=k7h&ooc-X3V;DP}7WsO@To?>mV@bvib81CmqPa z2c732{2x8|K$kayJ6TpJzlJyH`&Ma<`Uh?XZXldO=;@RJ$-RUFJY4-WV$N%rPYHpz z%-EYlQNnfg@|9ereu6aIpBIOG#M#sLdMK^GKAct#4Vk|l3Kr6t{AUy(>Vau_nR|nN z=D5{+E!6b;=hdeC=6(S_A}%eS1r*Em_p1sJ^dB6p@FkK6Hf*1 zybPRjOn8JqTn5hP;%+6FaMo_uOUdKm$|WYrjx6xU`&D=6IMBA|wFR2~DG9jYffvX4 z9^jKTo>f{1O+OXTlRG~z0a6qA&Ymx;FNu1Ynb7IduCq~GR&d-3e_ zz`g0;_Q8Qlu2ZxgGISdJ-|@h`_|5qTCcUYfZ92W-f33#x!)OMkIW z|A{BPm;Wbd`Q&Ra-xhe_Ui`QD;D-J!KDd#SSA1}zU*7S-P5KXga3fbo=!{4wA5Fd? zAKc(M(FZsAp6-Ji{KI^3qu0_txJiFO5AZ2IxWRK}5Ag5$;D&zJ2RHh3wGVE}wWbI7 z?|g8B=UyM&;CaLcH}q`s!A<(7eQ=ZhO&{E(|I7zB>3iv}%G9q(f4mQF=&$j?jh?CZ z!43bDJ-{#V!3~}(dw_q>2RHZ^`{0I7HCyY{!zMjX?d4_4Yx2F_2RHorLl5u=eQ?9K zkf$Aa^=d?NWuL3^);|N&d~nlm&6{%#)_=OOMb>t2i#%u8@S*T{!S#>Fp+2O?bR?lc z=rFd(!U4?_uTKD|a1c1RL_lOM2`VK}|7IV=dn!-)gz5ctL5zp@_(DL|u}* z3R;$Q?uwS+cM|Ud0$-v6CIh}71!Z~eARP1^l5{GxEX}GV$d6{!GPxuAhTO4{pLD=V zS}FJ2WuGj|N0%#>V-N7st%?KWBDD(P@qE9q?K3!#7>va8KnO(Je1oW2j5W9sC}``0c@Y-9xv;9i$|nsEK90-8 z=kt3y3Bzd@!X@oMGEPqDkxl#OG}3U0zqKzSHr>F^3L9~mSa0ClDbo{pyTK$~uAY%^ z6;f5c0YAb&!y@}yXxaBL&Xvt_K!tD5U>2k=T!eHVARPl_9tbVF0O{~+XD7otpO9k5 z{C-IPH>CI+zdo7oF%VKhgisb$mB-Lrh0x(s_ThtEsAWBvpin}3^%I$?v>v&?5T+?< zhIr3mb0u0t&L|Xxn&X|*1N`Ru>P)Z+H+RkeZ6$Nd$WVf=}l4@qvAl z_Scwz6{%YD3LTFCtpn&`3w)50t6C_R8PxIuLaQ*dG9+QQTSa+M044ZK$2X~Bw zF_fltky&L~3@zmmkYt2tD8x>Id>_!pQ&A8^rNx{S)si9)cNVkEFg(vhRN*X3n_T}| zal$Rd?)C}Mxw>`1<+-*{7g%6Gs|J@?Atjw_F%r4LEa4!h0CRHc(b%+Fh9)R$bUq=6 z`;qb0%#L9bDpN%rNo;jx5rpGuTlm6a@%JN2(#(bvvv*%*K|V2F(cB9crNgm{44H8xc6k6OAQFIY?T#SQn1yG7hXUV0K4ze^)y1v9oPmMnzxScm{& zWF)+Om&FI3U10YTk>t2j%0TAJ^Zx@M^<^k5Z~qJ*~Y>S z^y~~8?TR02afJ>3XJ`k@ZyEyP<2tp@5bHj1qSreOB$=4(q-5>gCzU$3Ti#t3wr&vP)j`=ru}*y(iF&@ju#wl zP__CZuG9~jaV`MAJ;g^3wp%vR(u;}7nhaIvmiLK&59Y|i^&M}FS zLf!Ha-F+rH2%Y?z-t=js{LNvm9o)y?=@E8ic&{@$9$ z6`bVb!kWd1KK;7-S+D8F&-#DYE)>5S)Gc86r+)?v-2Coo?P4tr9JuzDs5|FMlxqF+ z@2*|kq3cb3-T7BfX$ycVjT8NAm&<%Vr33kZ{TeF9Sav&xDr=v=e@& zh8x?8Nk34-ja(Tx+Yj*=n~Qpqn+Q8#LU|CqMCLrG}e&H*ns&Tf?~-aPJz9=`Vf;cy~B{-2?od9^hPC=?+go z8_Vo~jj;ljcbev1dO22j=JD6z%R6(iyfbUgH8bX6iD$uFcta101?CPJG^9E(cj%xY zLjrKEp3^jF$k4$28BNo#oHuC5FgR{cA2g&Ufbq(pA;SX#JUcLB?)*VR&SB<*hR9-( z*P|QDLW(L`JDM^3YB^ooo_WRWg|iP|L_)!EnKL}8|GhF%r8@)z$Jj|Ob2g@dPY^7D z!1sOW4gBXmcvPq7u2o*Vv2B{Ql+7ME=`rp0-5%f{dElJA89YT=@A5VAn0CU5%)AWT z&@;vdH|=(!4_>c656tfYeuEEg^1Z_cH+cTT2RHdXo=_>B*4=zLhineVCiZ}4pN!1-v(`?wFTe+G8>;D-Lc z_}~_B@!Ic$n|zO!c~K`HOurKL!HwQI*#|dmeV`9+(qHI-pA0%ozUItm!+%5P)t>Yu z*}%;PN0YvWg~IiyFMX8)2VT{1Q;#P7n?888PQTxW$G}h0%jTwjjeHK)aME)c{+n{0 z?SXsgpYDU3`d#LO8#;g71ALngZus-(9^gmmuE)?%ns^=SgB$rwXt?29QY8<})o_;A zE4SBp;NJ4K`rw8?xB1|vzJBe4_tW{_c9hZR{dXDV@Ug?7yJVSkOlW)uiH|eMN;3nV2KDgoM z4L*3SB0F%u4{qAggFd*C^XD|2?a0UiQN8Gc^F6OUKDe2mzOCVGzr@3~$LnJaCqGZe zf0OSwG(yAY3MR%i+5;!dqz`-GQ4c(VXI@44Ph950fNA=Ix!tUJVr6r9^>D0hs=q_3 ztIs|gdopTjhNz=={~K$bvY}09*dX_A);#y(1=k28ppa{XVuCNM@hK}D&1*FJaxHWh z(#skff1A}2tO0HpJhw+_OQU^qA@N~Nu0zt_99_)cNWWa*${5bI#I^hmYkPMy9q`{F z_q(ENC?2>f#0z4 zv!d|vZF<2M3yDMmvo~Tod*c5JrgRh=R4n{q5aovf5-x1#2e57KlDj$ILB%}flEch) zW`QI+>N5=Q(`Z-6{+31D-vXa{u2x|A2+?5bDw_v#hXjH}>*V&c40V;?gat%8)bRY2 zat?>%b(M_EffZ%|g_wBVcS@U2=9Yv8W=43@-j?6g9=rjMfx4Ncnb%outR{wecHY(j zauBsP8dIFN_)i>4VXE{R}exWN;!x1ubzy*ZJR9ntctrQ#+?c`?)1-ZYkjwQIcq^^nHZ3`c$X(a z7d;-^8CqFKPTE_O*`N@Wavld=L7aix4=-3`(!^C13Jm=Ci?Ji4S8RP~#UcE5wtz%u zQ~wR{{+|$yG>?Y8b~xa`C}B+Y;1SD_&(Vzg99BhlTpkY%9>4JFaHBop7Ss}Iexy@z zD=hEjr8v?o!6L@CWpKEir-UdNOK4>;D3XXEI7R@cW!#arPeeqmAGhY6nDa?!$pH+6 z#$>}Q#%3$F2?ZdiAFGCRQ)HwZj5wr%QpI*7DixNp24oP0XG8x_W1v5@g+lF-UK@sM&ht8F5vZ!Gmeu*TTf5ybSVY`B+uZfkzw z!uDPiM;51p-?|mOh-aNVBQp5L=5HpQjg9usG1;0Qx?fM)PoXkb=l*+3Id$Fj*O4BJD)e7n6x*g z>DL7e}!Df3p+Mt~VDjQEuKQ3^Lh9AAMX?#80X#S23}2;mDBSccIl zdsZug%i`Eqtad_FE4O2R5ElF=hY^bxTEqDqYY>P7gxy_byI;fZt}>v+Iwn>gCSjA% zIk{7lHi2d#~=pbgSh3B)p?ItWl}!x!CjBd~X88pJsy9NC33bHpmDj z>V`lm&!3Q@w--OPYiY-WqE8lAuAbYUDyR zdw^y)(Y#JHFA+_&7qPrV4X|T=sVYz9OStc$D3Uyr0Z_h!%}4#H7AAZpkW`1s&yN{8 zlayOr`@ey&pGZ2rK#{NuKi>g^K^2n{CF)Dj_}SGBAfiky5+Xkm3RNJVQM;|dZ>*+Q z{uMYrL&{0#;!ipMOsT14-0PYF6ag+s9gBK^G8_E%)d*HrK1+!M8|*aX{eD3%xPuG@ z-cJS$Me_GT%br6fq-;6v%ybv-&I@^TjBWyp$_`HZC_a{9?iTXxV!Y+HsPGRgcOt&i zAtr687n0I1e(;5O|1H))_%}e!#9Q9RS!XyQ<5;KSPPB{~bVvMV9k8Ub6S7dr=bA9s zI2fvg??i{h!X-_8ln_2K@ZZ^EePoC)f1PzX#yn#<%E5u>mjx6Kuak-huHPCh4&zd;?A6)kXj=*C6p;W1R zpuvo}|K8f)xAmB|+qJ)5CPq?cjHUlw`%@fl(2O?x(?0_SZhrT)_E%e`Qta0WIrl>+ zT(I%?)9|lqc(ML%Zg=T=Gr!H9LhQXb*Dt~U|LONVt^Jw&xpv{bSO#9^-k_h&+=}mb znYouKr-F8tpEUF8Y3=WjF8?Hztnk--X8?_O@biT4D!B-R%j8e~(=1`WH}EMMj$4;& zyq?2#0pf=#w}H><2F7>h`x=u_-~Hc{On==m=@Om)0{xx2`M>9H^lyhvTrdkx)A7Rrjkd?`xsqjWS?t7jNsv zM2fZf7Egh|0Uz9y=wlzepa=BFw6XBIuW>XCN=)(99z>G!RfoQPQWi595#OG^t-N= z@024S^W)Sy34iu7eusOd{8p}qw8@mnZ*|g%xI;4?=(t1fh_EYn*mNY_ZOZ=~Q}q(& z3wr2Z&eA^b31J_Cqk0oOVQE6*yYUR(u*yPAdB#=v4cdmA2(mOnjjiJtATaYX%<14kILvaV z@W{f8;e4J}aSa%~8OPc6amx_{Ahh6K^1hmNXn#t!i5TEcabdL`0)3d`Kf+8Jm<-7$ z;jTR|(>6l;DmUrQ@*qHRS?LxsK^#uY=oHGTkZgc^yx`GZqkt!w7fM&Fy-H!FOOh}2 zQElJ@M&<0&kO)C3%I224;FDk*=v3uW=PG$ zIad(b!g)|U^p*QBN+C*1J}Y?UmzC?8@I^Z@LECaQkf=j@h@=!g@yaXMumEXtOY z@?E5iW>Cs{gc5@}p>fWJfuqsMDVdOMr@%X0GJ;%G^FbW93tsWaPWX!w%00C7B-~YP zak3{s;E+4zm@1Aww{Wx$Za(bmmdDWL@jE4YMj-4L3<~kfM%-*uL1wRo+BYHlGGJwS z0dr66b(lnl0SNMx$2h~^3A~g0QffAYmOYMfnV6aJ_H`I&uz6ze8=;nRfXQc3aG;io zEB+vjOGch4DSH8>`e)mUyW$mY0*k;YT#2Joy3`tYif;cbxN77NsXhUTRKr#cakNu$ z1CWYKP$)R~K@XE>RYaAB`7#+8btNKoOh z^^qpiob{1B_Ga!nsMwf-ITA-46oHA8=t^IHqV{MWT^9~ECswH{CwG)ud|{lO{wV~o zG|N1-KRLp#AMN(WR+8o4VNM8*@|<>yt_$Csd5AnE2{^BtGH@2Z9IH)NRH0;oX&Y=c zBi!K86r?IKU7o?@DvAmMx0F&+qQ%WuYTIg|CX|2pI2)D`T>_l=4xjAM@;~5vazlC- z9a#p|um<3Sd(0@O^j77bt@xH-hl3_mcJaF4Lz$mZCyQj*n`ygksV{;teG|OGS**1{ z0;6-UMC-bRr?9h;Oz;I?AV#?525=Vlk8-!riyY}Y0A_L5;a)JhsD`-Osad8-;1LeW zK2NgmKvLxqUf7|4b4*)u{UN=GkQ7S9f_*}*%OK38l=&mZ)H#Pc(JS1lK68_pT>I~H zyOfeR@Ls7whN4>7xr+mMm*W)+ApH$QFhbH6OM1QS1RBdov#iE+RZ@^r?v*G}DhQG; zT1fB5fgLF61cl21MP{x81|-WBQDOL`@$rK={B|IZEaTu=l|p&QTiJS?+@AkGha0eeb2I2+DD1$;xB*U*wO!> z-E1*w?!5l-7(r)nLV>GWo7w;R%LgLh4V#yLcH96MFT+3eD-dWf5BlDd&1{eMmrrWj z52sDcr$m3(Pv5_);phmvTr+jObphgso}w1ORd^!Kp1z+@NU0u7{s!EW&CKNgKed^y z*7Tcln*2=u=H8Rd>~8)2B=e!Zo9_&`5f6Tz@Li=ezW=$+ti&_VHF@#QI`m$%@WT(| zd-MI@vpMx-GppD6FYsp8Ow_B`=0M$Hu^p2hBaby2PW4Z?q4jG%xM_b0 z4QDn?8^wQKQ$296Kie!1+>8H*KDa6GA9{d4=7XE^a+^9YLyy68j4p_Qn||R058NAT zq`?E{V^pwW2KPn5f}Kek=`FJEm2=aA;uo^Bo@^Q{MSL zxWV7-gPZzY?t@2lzN>t2gXc~k+|=!9^kKOxM}YVk~$E0%a`8Z|4_qCJu(kopZnlOPWpP* zP)NJMbBYH}9EJ~P_~3^AIv?ENnc;(*{%T$i@Ed*bQWB4g_H158ZVjHtd~ieOUwv>> zFQ53}2G4Q&!H?hXXKv^j;Y)Ae7x~}@|HVGI;oD3f+@ycj2RG^W^Z?KM;D(-RJ&!Ww zGVppI+~6PKgByCr`rs!0}2RCv%P{VoCLwfnX+6UJ^1H*lAvj%GX#>ofbIaRR06;!7uJ>LC0 zKk~pyV_K(w#0NKe?>Qgb@c9)Vyh`IaiSVdXt4eg!deMr@Le90L-4BIa@M+(UH{M zV6^2>uh0^Xa$61sLrb2NhvLwZiSlqnXh|9m`3|O9`Yv#~3BAZf{II~c-GaMASJ~t7 zz#0()#ah0`@$zP|2@5H&!*Bf~rg7H8Rrol#2lj2u)^okDBRPn?d-a#c7tQQ~007VlmsY* zYN7V>RnGdfy^A)|g^6q!xGUerDeVZjR6I6Yy35cEdIyo-{-n46Ve~S37Q!Y?#hDJ1 zpG4*U7j%~)>8D8gbtHX?NfrH}wkBBGbs2osI_v@RHEkadU()>g)cU^^#||*?OnBJX zY}HT_Hjzu1t)58s@*QE1PN)Vi-*_yruA-gy} zC4O;yst&3`nP-Edd46uv+0BNU@yrjzd^UD}B%Q+w`F z7}-gAEf*ruM806?v?Mt`p2q(YphjL((4vqC4Gb>RT}jaSi@l%CEJC8#G{oEwvwqqM ztGsybL(W2eT=+zOXr@fa^AMQ%G`7!5HxVvQl$k7&_|}w=jk_PSLEtWwbvMZ?BYDZy zjoDy38+$~!B219XHbIs8&S|(P{>?^bb9`co=cc5bzfDX#+i*a`Uz08G)ViZ}ixsRa zYEt~(sp+^+hWu0Vs}hKyCOaXm>pG=7(_efGbmS;3TCC(ZYLy9owslst)R88Yc`5GJ zL_0s^z5|}{z|RpIPvs$t<2;_=NGv@4E8ZrHv1aSZ+AY!2IX9SpGIl^6FcMlh3t6B& z$dZB3leX8Nmqj=w35>MS?X75t7Y7j!Ppv_~J-MZRDYAJ9eJPp)yy_?JLZg-z%7$Go zPxT?HsMpvMM!6GyiuJ#8&3YY$&QdEyTzo9u zYB_0~7crb@aBQ`eoyz?J<)w5IS9*>bM!RJ2P4GCZ<%p~)Eb7zPflyl-dO;ODzad^1 zj;b}!P@9FtwKycZYZnj^U4bK7Pz|vU#_JD_2n)vsmAUGEh)i z@C9>3YS>CEaU_LWOY7X7A!<_^K-zgr?g_Lr4;A!uS@{eyWdrd@omR$i2aD@U1B<+< ztl3tpnH$t`w&sSo`*js0EACz$R8J7PgG%U{FM`l)Y0O&NkVSpeQsKF{N`D}Qy({^4 z*UARc*$Py`*I4+XL-E)_oGpWsSr#E6R3bFF7JsH51OF_DcIC1Trk z+$^ZU|XdEPh}ud6q+HTMDP%J3Dn zJzzV&pSnSq$K~9`Aks7r2wfnwlxL5py3`GIQ>DT2kJk;=97UEousY@JO*;qD&fBQD zPSxv_MiD1Y6Jt`NHX5A+jn3Yr^PWyE(wU}v0X3v}mB>ddkTh6BopJ(T5jSeAH~Wj& z>*^YoVWF!0y56B|D}DLVIL-jE+*2`lPiX0P3DZg=$76C-^4w&=8?$h4@<=X(H5Uhb z6=Y+*WUCkL&0#Rmu8I+=HyfD`uH;sc;PVvIVkPpcaf4`YlTO zKoGhhZ03o`ri}<*HAV4vk~DU_m3wYDI)=2pd4!v84LBh~gmMhf5GG5YGjQy9-cL zvWam*^ia~-%AUKS_2Xo_%(yeBh`PXbHLNr5WPZ>bF`NyRc}HoBIL1qz=sEJ+heUCQ z&No6I`VYlAY!l-UFL(>FK)?5fe!mNkY$>JN*}M^_+IDXr9fMo^UVLsuaI+P^bCL4A6@GZO>@-gvy2{?e$jCZhQ(FFiM<+9%72r z1af0$qy!Z)-zwHY;Ujoy<)4ulBhd0QGe3!EQ$-fKHl_93PJ_gO0bK@^nJIDi zxMQbzMn@To%TU!zz%993^3n@XiwdMMfu7@sP)P9`@oj+ z`u$dR9u&@#mMTW(K98bNm5aaL`b{|pB}745fTPxJ7v zNusZjIi?m_M_Ey*t$N)ckZ0VLejuvF=}ipi}WE4yHxz zUGcZhJa-{xTxK(1t9=I=!GQdqP>{#K90?FrlNEMqgd!iDc=i*o|WwOTpqkv*0+mP zjC6xH7IlbFEY2^gq)=2~FJk5E5Cic5LJm@H*VRBLdLg~kG5jFfZ}L1m~M`l^v(v+H*i}*);r+8mkndCJXMfeG4A%8d@Lp4 z*vv~RM=N^?*xf07PkWjw4FRI2bzhph1a%?!;}@lz=eth9K1AF79J+EVreG!L(NgW; zFULeQ*+6}2-A5pF$Vh`)ZLL^$`jNfi$4~@LTi`1?_{&$b;d~jp=wtg; znKN76i9uA}I>3)$P8bolZ322F z-#Mr^Q+#)Ux8wgfgI%kN;bLQg6E0|a4a#Cnw*E2H;^~}hO+M417xS5!JxYJL3xb?$ z{vI6gqvPq9Qf`e7qF9*&x^dYle?}3e4g@|3#6Tc~NDR!ptT0;c>~Ny>-ON=Qv{R@yj1cVQX=buW% zKFvIVL4S#-VUuOT+uScb1Nqz!@B{s`Iv_RWC@DklAW}BEMDIZE)CO$)XQ7%bH^i0= ztRnkPyXqk0*e<$P=gy1He1 zMlhW`=k)4s$REv07$u2S=%MIhLbRESc3?9xSVK6hJY^X6AvlIP%K*O6Vtk_c_6|vc z5kE2ZgSt4U5cV0rVTHLF2~fq-?i1N$QKGJ~w9(!*rakzAywP3j;~ZKaMlw@$5>J8I zAp#@VQuaFCM{sa`J6_O6gI5S@gyu>4bTSV*Nm46S?%Tf5?JBlunz zzl#v!L|8X?ukIwE<~Z0YtN0i-Wjcx1L=6=>2OJ!#kB-ee%*iF~nh?TE1RLtJubQZU zFoDFraQ25*?nm+DkXIp$SyXLy!+InLA>W4NXORaGUP%&K`U>vx;)yW7Y{22A%TVwq z06}JIl1DD8$s4VQ{qVi83VmHZxWt&o^1&6lk~)*C>7Dx9n5|&of=N7p7TSiYb2zDh z$N2^fN-^3h=ZH%vLeao&b2@|Kx+<#b$fcldCqQ7e48-&65_Jg}1B9?p+hAx7B^|8Z z_e-I+tuKrE;HC!uLnsC>s_0Y-a^)5vQwf674uGPYomj4Te<|nQ`&Z7?Fys~$Z`0N*a1^eo3PUSnH_X3f;q16yqx}LpChSW1Nl`~U_Nj>CfYK$pr-IJSLG-F1k5gVo-O9I_dsFs) zAUZ=Rc7yk|vge=vQ6N(ds1%kQl?l-XG7|-!{4qp)GQR3+uV5i-Q}!WBYpg>HD~tl4 z5Ot{SMF=g*;FXidGs9Ki&H72(TX@8A1RE`i*=2=G7*!}vOdDto3x_h#8>C22@wGpqQ-c=r2wB^9ie{_$8|zZOAP@(rSNOdfN|>;x&?q@hGGG}GRtIzw#@P)kH2!pe5)9i(~-&CexKX1+9DCfYaQKWA~Hh3h?vEXbTv+pI;=0~-z14d}^$y=#NkIH-%hz0=0vt^(+F*Z4b2dsN_ zRiGeKWJU7G+=QAo+D|Ho++^2afcmb411~3XqK}boXz5ItwTx^@9U%Xyukq<=Jn8C& z3VDVF6x>uV)k<|U**kbKhN= zeGY`nEQ;tSY{Rgpp1Y|!o0YIw%c$jiQhc<2&Rz=*soO&HShiQyhf19(3t#Z2qK?=X zP=krkfT|_R4&rMkO4dafSuAxuzNiBuco~Sy;Rg2=!vSG=u9P^t#NWvZcLc9xGC7#D4{{@rpm@ zv{WLh17Ia684pd}P0}6{@rN$Kh^rt+x*4b*Ow}IKKIavFv|ENwn*204D#ipDAtwD5 z5RBbSO|zi2KH;ZM6eYm+9m8hh7eZ~nz?P@O8^q605HUcVMb3HanIo9Y9JT1oEylV_ z<{&15JO_M2YX3~>eTz@;HfCin{1VNKzV)izE@1zXvHcs8ErCfy)(fYRcQVPJXv$GLBRAevh=i$apS=l~;q z1p?ci;Pno-tl%od?OH590|9;a!~g&PeeR0t*W0d5eEt7p?_I#7s;>X<6BreBWTHk* zTdJdu8X!gpK{F<524-M_6QGI;EoyKGf^w4tMT-*@5T?T`(1mlHQ8Bm{_XF5-(R2SeV+F`kaOlU>#V)@+Lv?AoW0l9 z=~jhf7q-lsa&Aj_YRj}LZ~T@&_;mFSAySL~J#bo7n)uN~cgYsq3B zvrA)6Cz~_upYwQ3vz?f5nR3;{AtQu{+LRyvOMNbNL(^(|DtE{&VO_ zXmir1RB?Wc&L$-|-;VvKQ3kTjR_DB)JV z8l7BBFYcW8!=Cg~{mb)xJ1CzkAWe2I-d#eto3Z2zo%3e9=_UG?oAh?!p9W)<=h4{C z2b}Y#_oSOglI1HkAwz}@89t;sTyY92T5?KTI5lJT`~~*gqid=MdR|rQ#k1O`oP&Ma zJiDJRNAp@{PN`~ZSGQp+^qg65 zoY`>HN(?bqchot2q;&f6iRHW}^~7;A)oZ&^r7K@@^OTRKevnRm{u(~NR?3ynkn}}? z%$b$3(Zc2YH3E1q=gXctFiN6Y$u%5h!34#{9NC|O@9CJEMNxV+pI@QAkaR_`2?yFI z^xgPrR^oZPGybP8OU@VnQ{P2gZyQjpceRP#^4&}vXhz@3`PARIo0j#PGy6|Yf4+M% zj`jKUS-HrZv4iWK#HgohA^BS8e@`%7cSynW$I@TC_gBhw`c~?je@X6tbIvT;O>;tJ zlYCKkM_fM3p30jzcZbfIyPhjLfaS|L0ZHj_=2Cxz({OguPv_Alyb8y}!vwL3J>*{E z6^8c6AJ2RUbCI|0Ea25DKAPnl6d%hx$z1F?oq0-e`q=jyy&&cuvix`@pJp!YTl7z7 zK9S|cewqVI`E6z{@)t3ms(3qd(np)fFTpYCp>{``@GOpr&o#8i&onBQ_yXo4e=YNc zir>V1k>bB(PU+JodTzxr>0fSW5C1LmmCVHt_cC9t*U9^ck zPvV&PEr#~^;Th(4D*k8YYnh9lH<&-5_`A$EDEKvy%<-k!BwvG=lU=k4uVGHn zTU=hxoYHqW-BVJ!VZ~F-D-@r=oL*tjCiYKePU*Xxj{ENN3P+rJ(carsbG~_)>h;A8 z=8{kO(%yIPR_s@Wx3YY>0NyWP9#;HP<`s%x&U}dC%b7PQPC@u7vSqE}tC%Mhzn=Mc z#n&)zR-A(Ulm6+7-_Crl;&(A$p!izm%N2i!`69*tzF)2t z4`My`XG71E=e|Rk(`xHO_1%s3ZI z`n66)bTB-w%)o+vbMcT~HO!kmXZ9rsk1(SOEAq8Wn~|Nu;Vx|XD(kQz3Gv<$P68HT}rQZ{Uu6&pr57zPzp z!;YmRoNcm4XAh&ZYmoV>nxS6H`BgPYcy?9IFz*5!!*=`~j_?rY(oYvPSdj4iSwpL8 zj-!2bRZT6V=rf!^`{A64;hc%#)jBi;B1M>R5jFHhl>T5zMEYwa;XOJw67U-xA8Y=a zr4QjK5cq`t4mW>|44grvmZD9}GDT{R=FYSAQ*iz((oPy}amI$z03d?8u+t$iMDzIutwYXqujKk-oV< z-x5fE2jCpq4p4lE#onQcb9d?uQ(W#3Vm^GV5C6X6qJN&^qJO30wQT2&ipQDXs(6z5 zeGZR-iT}4doctMK`BxOLWd4QX^~`tM-5!Xah3})d*tx$C|E}WVhe3*q{+Po@zy!(H zc!yKIME-Qe#h!B=?)oR=aMBa8+4Sf)S=uDO;-3!1CEedDF6DBs;$qJOic7kGQe5=> zRdKQBABu~7AO7)3?3Z-=DK7GEY>xo)E#-2Uk{3QqajEA&P+a7vC@%7P?Mi9aOO*To zQyQN4W9EC|GsQ@d?OMe{{DrJ_De{u;uN?goAuILjw+?r&U-vtl{3Cj{`tY|L?$+B6 z9q#&huYT5GU0!91M>t=4txGA#8Xx(wK76U-QeHpv;lFYCB*$;}INZ(GpM3bA6|doZ zz2k6K{}&D?{UX0VKPrmfr2d2z7x^mYQXe>3Zxs^X&mO~u9jPZbwEd+~Fqvz_}2cp1HD!6x}?HlOg0+~El@(SNSPM>)LJ;gs%F z)^myC)0y9-cpLLyD=zX6D87K@pH^Jt-|^vJD?XF;^zUyE#1Fzx_u(@Xm;5eKTF{t)_X5QSGykEZhw8YL`(lTm>hKi~CqIb& zH#+=eN50G9uHRl#T=cx-a97W_%AgdR_(AeJ#D^yw?&>+!;jTT?efZ6ai~qMPzS)}R zz31>0nE3N!hf}(LWci~Hum_Sa;dKsga`c?RT$is6DPm*Ik_&5D|13w3TfXxheyXEC z<8ZQ5^3~z+v5x%x4tMjlNpaD$-QlhtkKYq%Kh*p1QHpQn{NAJZ)66&f@Yj9#`-baGt}d zyhP706c>AbqqxX#aQJvf{}zXnezAYM;-ddehr4!u>~Po4l5!};CVu|2{pjtfxac3` z@X3%BKh!wf)gMz_^p90s%5jnpPb>aB+c`t=cbU&oT>Q37ak1wHAO2s82e|&+;luA$ zT+&^yxYVmZDlUF_(TBgOxa9X!#l_A&zhxaF!_E^JQ`cHPh z7gd~Zo4peir)M_WPFDKG&hZYP2sx>56CLjMx6>R>c8Z>h6c_uKD=z-O-G_H6E_z;e zxZ8f;a5&i``cL{cK46n_5k6jV(SN4m(jMnJ+)ei)hr8+CqPW!0wTer-_`TxKTeH2# z6_@(_SH(sC9mPfeel*Y*oA^QGk5XLZYkc?^AAX+V;E;lJI}us;KOGt{s!w`sJP_! z3dKeLFBBK~ZHh}h{EOmJuXZRd^<*zI@2RK${F&`MNO6%5D=zXw6&Lx_6&L$k6#tUb zo$tecsJQs&M~X-I`f{D(lJ3ol|C7^wTXC^xhY$ZsanZlW!B(NPM=AG^;z71^fDex- zF8Qidyo~iE6qkIRskr3pe8t7i3l$eXFHv0NS13M!?YUZUvFA66i~ffd7k|F1xa9X^ z#U^V|#sn0*~;jRgnxagnl!!wGDKd(~!OV4h<@Zq;AF6DKf z;?nLOR$S8kNO4K`3m@L^P|s}AKZu_F6qow&9mS~RI<4VOv{u;%_|36n;?72^I@!KPcOTGQO;*#Hw6c;=9IjncNh@THsT8(~6_@(+p5oGfe(7+x zT=tG2GuXtRqGzBFKV5OLXNuyYXRhL6&&7(1{51}r47;TM{KVk_aFM@RajDP0Q#?>Y zx%9mCii4z#V&*?`hp0H+k!`ix#Nh$(*I55b#sA6tM#aVdw<|7s z)+#P~-cVfnfs!L_LXt135B(IEdfs1gvF8xQMNf_5Qa_JZT;!V+|C0T1s^TJlY2P0E z-QPoO;0s$HTpnh9)Ep?!vNiL8@C{Tr*w&c8G^fVO%X|RIw{V$1P)Y!saGB4LR9xmi zEK*$N4{T6e<`3*pT;@fzwzSbG`8mgg?KX7ikZ8^DnrQX0K5Oh$*AB6g|Na^~;oWTB z=x>8LY`~I2{XAK8BG{08==$qud+emfb^HGP=*zCV_JR|V*=twB=*IOAWS_tGuI)G0 zH?GaUckh}{AHDO$#{0j#^@HtA=Zrrox$cAMe_ej>&Nog>X75`G@l^KOxn_40&G1QO zmyx4V+0LEVCGVGL2g!$=RA?E;#`W@5uKlPUs|?MJ({2cG`cPw(ivy*`yayCIc515q!=VTGR2-Ku?7jsY8>%Nn^maTzPXzN-=)0@x2r>`l}qB7^jXqp>F=B!jI6b;cf*H6-sJIJtAv#}zwH zg8Fi#*tlNQH?BK;#E*kNyRoZIDjUV$Ty_gih0}JJ-Ljq7p&ZFplq&tggw(@kcWnj1 zwW6f){^F6fW5YRO$9ZqV4r39q?6nhdV&i%PsO;96Bd@f9!=9L0uxyA)q_fuSmNigX z+54!;!;bOTbuGIG=Nb1dyAwy&udm?a4;EFqO*jQ{kD3Y}V*8Z^Ux1;}Z z$&!rBcH0x8N%@%?%`xy>GMGbv|!)%icD- zYjfQb>`l?ndwJHKXt4{2Ety7qT?8MEj0T z`6H)4fj&5!PHR`bBk|WbQ_|$AnNr?${>d-9N`Ea2O1pxX*>w8C^`7MZ=P?y@EwEbq z?$ScCd)I}utJnafX;*WL$uI64(_(w!o1V0p6K8IKj4@&J_nUPV;xnhr+QmK6&|Tjn z4IMtbrpDYP{rmSwJNUgmnb2VD=vjxbP+WcR8LZ$%;p4F);4f`JX{<^{S20?bXF9D6 z)=U9;v4+`=k+s0I`rrn$U&1Q^uHcmb?=dR?(&~ZZDL6INa|$J0LCXu`-UVT*Y1P2Z zbS|{wP6m;=DACu9wtLJ%XOw8ztbaDGYgq%zmsb#s<;Io8`i?8jPuR0hEP6(HXhmj! zvIR?@#pu$3sFiz|`NfIcq}s&bNu`PWsT4ys5gLb(=;mfImiu01Z1DF2Hg+6N;b5qA zRM@n1duZH;w7A&lppD3APijAk*F~aeU*WF6??~Fl-70j&>e_=g%vuI(#bJrJ+=zfZ zt!_kRsFSX)Huxh2IfDafF~#%~@m!#@DJ_v`5m&a)=v>*}9KLN--GqI-Q0FxmI>nK~ zD39SW>9VU3_xVa2tJDURzLuiT7)WJgo}A!K6GnVV_ErQ3>NaptTK zLzNtNE}l!4a+L0PU9z_QL~#UG)-%Ba;fPeaY&jNixGK7HVQ`=+BAX>ss$yBJ?tsd2oduJy=1Xgl`XtYIgaHn>Svun31dM@tRH4_Otv;4Hu{w$W!F0> zY>d^7<0a)X&r=k_=2)(wHU^I*a^ou#gU1I3ZjMK{E<`N$1z0SqEz&UX$wF5)6ClSl zJ3j9@jO9)aWK9Xi>P|-0dK0N}#iRt_(g~62h{}0q&nZ}uhC|L{8J0vY*e6-?cCkeI zg*q#6X}A7NW*@bdn}We=Dr3{Zt42oabC>SUb!?+eyRLucSu2Uf>0;T`gPdc8h#W*_ zG+?!9}!w(*;0v&(94yPH^@d4fYuDliD&Guzx#mrdll7RJjtnQM^_Q|p;Au9Vs4w#6}^ha2O zfV=oH`LdsoYZKAb0Vt=NaJNDo7y4~fb_(NZDwn-#6zf`|Zc=6Dm(+h(bWvnriS`+{ zIb9fvo%g*nkDOec>c?0C(OV_oQ$Y2yHp9o_|zs zz@DSB146~q(I+>igFjBs(@BcV zSV{StXl$V@vL_qU^~;kR-=glJjvD`vL^gPQ@q&jomP`llB3t6Q_D8`zuO9xGr? z7hBWn#x-P?Cvs2SbS1RXvtH^UgNsh>?t!Z zZk;i^aqYR`3+7LmI&VsHUE7Rw7$Ff_nzH6{gxXG(zA<~PQ|Z%fGcKHl20*pGrC9B8 zZNaV0Mcgk@K5;LTAB*M9tcNpWxaLUK+ak%4*|IC7HK2G>`C$K8uB5~?O)O{DKau^G z?c1TM&=u|CyOjSfwg9*V%UO-iPX1A#SX{PfLLG~!D7f3OZcgTdYvZ{RZe_U(uuf_$ zx}&|R=w&!~OuGImRNaTNWjVGNH!X{CIlux|rlyF?v1lAtUNQ*Fjs6ra-;#)K%iQg{ zd{IoV#We^yu@m^QutHa032a0|HCj*^6S(5C&djswG8)UB8n7i9t2-6f&Ix#ak((s6 ziB$TDk-7RGrPAyq^dU=0c`S!M1ik#yeIQRNv7AJ3|5#mW|3E1J8A{OBR;nA*Z4*Ra zbPys&^M^XD>m2I57LOdo$gi)A$ORXV$OdoVW|~abT!p;cLyor=^>S6=QRFO@-fnBR zxU4R9wU@cQr%}YRrw4eQ)}FS8hW?;OeXBLH&i2{)Tai*>1NBeit(92*H$C7AtOtEv zF_7_4X-smbY-(fyt;AFbtI%32t3ri3so&%)S^Be7I*A7%UnADAXhr9gv<8gLmn}!V&h2IEySbnl9~7Qk z{COX~pXj&+=k_Y76=x6`4wtc4X_;N_Z=)}?AFJw*ku&!uY*$x2KMIRYO0(Z;^OW6T zb2xCbtq+;qt!Jfv7K>$MHYfp!*+bMVW`jSn>Z!^%=T9%O6{7A8+^OA;cS_P9;oDMk zOJeRThT7oUa*hADgsd<4wj_>SXe$MWfAOML zS|hTeD`6^JB6n0zLrE-sBXe5urWrjJRz$g%To=m?I51W>`M|)!GOCqHwDbMY&i6w* ze-vNK9R$_jo@;TU?hvjPncL`cUJ+YdThbP$?%h7NB1|=YOLtb$>$Yj}wUM;pI><{% z(z1(T+_7Udm_A4zcZTy1K^r#BmiuBXs2Qs}&Nj}>WA9vpo9=gw2>l$BN}I|;Z3&Bm z&!n4+H~Wmp4X7?WJ|c_NLf=_$9wX7&&<{vncMa~WZ4K?d3jwgTpF$l2ivpx>d@B9n zn0(N9D~IWxb(8lAEW|?NsYnu6n?12K@ScH7_hU!(qpVrwZkPVM?q^ImZn&z;bWPoN ze`4d)hso>HVOuGk$$c3L=RqWmA=#@ff#c-TQ($v3oSKtFzDvC93 zDhWlRe}rmjsePpDsg36L+1xnOEjB8XSvbbHneO>F?D8&H)Q1KgO%};xVrdOpd9S+8 z+UZHt-m~Lg`(qqqO<5=Ixip3Das(0Q3dQ|C?)|^&iI~o@_5ojprGuWLKW3$o&&k%$ z@aHTnE>3G2O#TX~JZxz(=}BKmvRHEpi<8(@7+Pr+a(g+rW1}slVw<9*KcPQ=0h;s% zT(rFe%koTm%C>Y`0?$6@$3l1D@nbSpYrvueumz#YC(^`&1Uk?4||3xU2AX6c2BF56$lW zv2)tOEwks$Z+z(7>EU@TQ)Y%6ADq=PYtFoj!)?h2=Y;3B%$qi6UUL1c@TrLjsaO-L zKAr}te{8}#DYLtS?vER@chEy&WA>(I?C@MT9Xq=QIr+D|YY$ji=)MWD`N0ta{@9sQ z!{H-_R}GzZMA{oWf7UrI^TKncA+9_>CwPkJ#{>*L*pEGmL`K`xuGDQV5W~Y(ER>TRXRA#hY9EO5ZXCZ7F zskbTPp?8im^mkJCO56X-@3Q`nanOsQ&wGVM|IfVavbpub*i+W=?AB{*;7{T|`BJ!irgvt$ zdUJ3KkAvOGg@IprOgj3V$&t+ILfvaKJ!LOqKIpXQofYZyJKsDDIcg+*K|^ss`L7lr zdrtom`OJH%CbDk;r(^sk-D}&-=ZAaIo_9uK$0_)p&Zjo*Zg2DX6J3a7IZtpC4zy3` zyEW7hu*thQ@;|-ClJkX6W{zF2t-*Oe;(j6d7B&+Hn$dT1evJt7Udz%g*Eh-ztc}y3 z|1ZwpmG9^jnRisn&Y^psLHMup@AuB`mV-%)=Y3{(#lPDA;uq?G~`geUB8>7 zcWo3uST@NQ`IESOmOYg>ar0Xrj)hY*B`)uKht0(k!dbE*IC*Sm_N{0aozcieJ)d>dTrCS4!+aF1VdBe3w4Su+q= zc-B-z5gzKzYQv7=`chb5it3BNx(FzI2?7ewnzA508$pF<&S-6$T@^L^xfE3x>!u(o zu8AOA?Lh>=ac9k(WhM6nGrTuniS5m(oS=QR;_>CbTUo}b>~pF>BpOq5-T zo=Z-_cBkE$>mKzTGVYchENG+aA=P1NrzPC+3YYr4x8l-%5A)#_ikDGh*oOM>kv@E! z4?kOR=|3f2wb(B_tK`KV`Wc5dk(c`UYajlg4}Vy3(f^p@Z8kaYX~iYo*Ay4~KUI7@ z>*+&=V-x#@hZGk*Va3IsBNdnWP^q}+Kf&QgAbqJ_Amz@dd&ej)?a{u4_@3__xhB)U{N1p6SvYz*Sc#v=9#6NN!-l* zoa~f()u4DYC%nqxuAb{1PI}~ev_^5!bBE$m|8?AI;hUAb=y_Ig>3_adT=eWsKLTJA zKMOxxaq+_-#hcC9o;OBu(Q}3mzr=@MHjZQT&|DH6_@h8!H56KhyPA-(f@nJMgP-^%k||gA1+G@i9bbtcYbse zF8Xy`X^|i7BcF4)+wQJ&IJG;;*KLZ=W&hu!xY)B!agpDmxX8bxxX8b)xX6F(!@pKs z^psJDhfVxeYCn2MDlYo#6&HUt`S7Veyw!(athl6mh2oO#PZbyWUn?&8TCcds|IvrP z=)>PqT-x2oic5dB+uqi(;%DLCR$T09R9wnyg5q&5uPKU)e_DO`QXhV;;*#!96&L-# zR9x)Wai2xc{YqZ+JgRsF=j$28MgL2R(>0j3e<(i0V(*`di~JrzDAN~u)E7_i3D=zx)Qe5OWDlYQRDlUF_ z(}(X)KbT_^|A);d&)Y|FsXyQL;bFzao+A|({WU&3p}6>eoZ_P2jk`_P1*s=)+->3! z&ew%X&jRL)6c>5>Zn?)lV$ZcoUh3^n6qoY4RdMm>9g4@)JjX3?_+W>(J3Ig`{>iFzXS4ne#UqN6c_)W$N?Ip-w=MX55G)t(ch`K=()>>Kjg!o_2GMRLfX!Q6c>92 zDK7R*^Whix@Spkcrxlm_@Ur4b_P@j>mwK3E{wd1~XYB1qF9xwueviU`sh@{9oa#dj z{?it5xLdC(9q!hvVGbuf(vIpBPZ{laK4va<-opGe#Tk3k96hf67dqUv|A!8D?O*J0 z*Zxk2lOD128pYe#pkF91<#LuY@|I;$Ay>j>Q?e%{|7$&YsDpB2b9@_ zbh!+4xa;Q{#rdOmk`KSt;ckDjz`5?YJk0r~*JfmsY-R}Fyj{$Bj_!KSbF^kyHO9(e z`}gNO&y_}qL>o-PGUvG|`=mSP*_rU1%H3mTB`32BBXd)^6=qs<64RYA8o-?e4e~Zr zrZBg8r`hv7X`EgTNt*bK0i6#-!sY`8n9~erOnk;%uOy~GSI}JNwdTazBOA=#&YY(C z(G6t8YXmW06_X>DL}(Z_89{@hc~&$=LSm?<8T*>VEbJlprf0mVKEjtce?^4e2Gexx zTI?}e`0*r+!r%xaDcGH(cew;+S7wfp(W~}69z>rD_Qe?EP{$Hj zoxg;qJl9?7WnQq83wYc$jTC=a@`Q=Pv4Ia`)brak?Dwe>nfHOAzfajQycipMmYMnd z_Ow{&m_easEf|(cGeNhYp6d)Ay!55a(Z%@=7~xJ6=@PlJ5KVZ>mi4m}w8U3+$7i1E z@lHHiS6=3q)=a0&LmeaFo)pTxnI;iTPvpk(PwO#l zlkA%Sy9xPp{;x^-%2W+}Fy(C&FQr~6R*%_XZu{qI?5-ReiGB+vlJV__&JuXzteu5O268;@vnU_&9D6AeDIKXZol~8 zeYvPF-QRXx4DG2sbAIBs1<1UgmDY8bRd%}dt}PF1V*5aD`i<$bEM}|NuFZCfU3F&9 zlQN%U!LC5gbd1K{?$MZ*lUz-nnP?{k)$bv;mGQ(erxwjZ&;GI0JDS>rI+{$TKSM=& zpeg+nTZ}wkO=Xm;KhPRn)QDIOTiMA2cVhnRKDogDcD7|7(F>%e;snO7WF41 zVF@~rX^uWU-ch%gnaF_XW;bhoRiuk8nqe@Xpd@X=|Z(>|l?LEVKH5GZ?=rmPrM zba{D+E*dmV&87~)GoFUnq^FR2S>)wFM67t>FE0;>kX(@d+V6@i5TWU9n_EL7&83we zT~eHdX}Vim$(I&FOGeRjD?8(ot`I}Wn6FJ36PW%r{y*ClxQa5&7hx`^#`G5-jtG?o zp2dacYs`GHola1`!ILXK=Qy>t$QbX9Bxfvcfz}-YeOk> ztuy}LE7VCg*b+;ow`MmsrUO^Ta)Cv-w4a|`-IyDEeu3XZC|o=Tf8r z!>G*5;k4Hq^Yy4Wr6<7!Z&98PZ@n~?exV7IbUqx3vU{aC9UcA(;lAj)SQb<2P2O#z z!<_!C)UmF>6O;#g<-vUcm#TF~Ji85HLNYRCor*4r(urrw?zc_7JB@xcKb|LIzVjIF z(a=5fb+kK!nQ=Q&X_%=t!1ir#BnpH9??n(^CojpyhZXX@UP~a z$l9R)r#UBa%s5%R2Pg6W&p9!UXkd4U|M;UPTt55FIceYmTExGB(c=zXHTv<1@>@1N zZxc5+?ff0>YWH?NIEVCn=x&wxKsx=-pY95?tc!-?1EpW>LL5u_^6V;KlPqnrZ{TD8 zDT2N-+eEgXvoE-qfAPa9_=a>*dvLeueEvii;#kfT+=K(|6Z&pL#7c;t$^TTv<$U4v zEH72xo?-uP@bSOoTV!YAKr{MI&aV+c-fLO8B`<;7Ca{tLWCsGHKFglUo4Dj(_A=*0 z>Z!=vb{QzfpA=rjdZhjcKbE<)7vU!|4`WZ8@KHD>dm@JR@G;CQ6d%i++B=Ot{{@bTPc^iM)5rhKIUcu}^mgY|dvvp43>xNuHe)lkg*!2FD1mSgrtjhU&@V&-x1B#t(k z%W<5Ul!56RWlYnPb)kYkzoB?NfBuK!(k>6-4vh4Z9;v^B6c6+HV|;jv;tec+fe*h}anY~mT!@|5 z`^f*shu`bNHz~f89@?<|QSk|E|Emrs8^!;Bb2#};+HnbAe8iu^gNlou{S|M3F4_)O zT>80M#T$$?e!f&(s3OAN~^`{$GlV{)ZG7{qHG0nbX~=c!YTgUySK% z@^druy%ZODzqt~JDETDoIa2X3^BToPf1To@XO!a2tmkyaMNf<3lI|=Y{v*Z3&sQrh z_4Zeai~ipzF7gj3UeETwq_~vV-xP0V`F|)b^8Zv^>RGR^oPeY zr#lgPR+a4}#Sh>wnjAfcgGs;L>Tt4C%IiACC11BHF7kIP9_Ms7D&EX|i{c{xwBiw# z*K-{%aH${WmEt_Yb%G@JAJwd*-c*huLp0DqhC?O~pmN zgntZ>au2fneu~GLmn&Y*{7A(IFdweCw3nFTV&`bZ!>nh#;!?hP&Py}PpQYr*|L6Pg zj1OO~xajXxT=f4$aVg(h6c>3t*GBrcbxL0NR>h@#y`;Fv|5b63e_wGa$Bz}4a{NMZ z=_mK-XB{r}C&m5`DK7SWOYs_(Kg!`@FwtM-@Bp~zndfj4QMOSvyq zT=aJ+E`Io#4}Vs1vHuOlMgQLXQ-ahhvAmwKz`+KBv}N?!c`d&Q;Pw<#Vr)}bCME_(iJTWow;3 zclMmAEyY1m#kI1A?|MwsaYLe5MeBbyCTan<4N9}YBrId18nd6qPH)OS8apjEp)vc% z(~{Y*PQ_9X<%nx%U*nOvw1z_|y?mO8S8_)d-zXv=F26stZ>tI$>JmZwW;OF-ZoZ=c z;qnKXF#vY(MOVO#Dd2$zvBHKpPUL8LnU~_Z)0|TgQ#w2r#W>795zeZPH8AI}DUmpd(jAt+1lLD6wkY8pLJ z3QNNF73PRs*$pEHm)#T__&G*}M7M{QEP`yItM1aUeV2*BFNq;NFFyaBBQmTWqP+Aqriw1r)@Tpk#sd&EfkXYeQ^A~V*_e!mc!)6mD0bN`gC|6Sb*EK^R+Q5zrgTb-2@(zY)92%;qrrGSR^XC~ z%GwF-7$rupWjOof?(v8ynfL67E?Z`Whw~Q~qF->cd4jFFfX7`#KW!hzO~HOcwWfJw zwtls>g>`I*=f8*X8$6yO^CzgZ4(OhO<-jBt6`>WU;Nz}x(S00#GDP9q>Pf#JX3Du? z#|N7l)HAROwJg+0A!Pa8IloMgei-Wfy&37SjVsoGD{Zcl80BLtmPG1B#rp2Qs#vH< zT(f47>uoh2_-yt?R6&fHT6!v3z?H0%hFHX+FL6!gPN;nZHBQstm?Gm0HKqUFn0^?| zZOO=J&F!J3d(pWF>PrjNM0s2jE6O|~qv*PKhfkzR9}O>dKY%~1pNpA$q>cpN`2-xYx_|S-PPu zVa;wU<>4N=Z)Ck<9rg z)1rzpe)cK?KG11Zf|E|a^HWNJKHqO_ggo;%sPn~s$_s4`_)o`rY{f*jU-AhNv&ng~ zr+aofMsLw;o6F}<>_vP0CwLqVbe`zDHT*-Mq)+}Qm14i}vzTMoYuoJOf3aJHXX8LK z`cBT5XC9H)vVLPDzU1^Tk`sBa&!^AYcO<$~#&&SUpo=hFKXm@-QB2nzAa7@X`s;cB zWp~BDI{o4o>VV|_H^)X$_UIkCY;r_nbcoAm*?*h4#bFzyzk#EB>>2Dy2P52LSOgwIA4`0kYsW^T7-yG9m_45NVr%k&Y z{<~uoq@TYATz+pQIoW=SgI#e?9-7m3bOMG1n6UvEVPMAwd?U2vjG;6rp&FwS5MZ(u z5g(&Q2ZB)!@mgokq6ct_E;)NBjZSEtT}{IiT3hj{20MHjZh}T4x};6Vj7$KoH9;Wl z6Z|el9?&=g+NiIfvXb(W@)j=bV+;vn)BH4xzrmXo7yUA_Q|q}*$xFJOKKus7MgMJz zi=GWW{0Sfan&P7WBgI8OD!JJtU(z2O=);dtT=b0c;onodg6;T$;?nM06qj~CM{#NQ zdaQ=D@5M^Kfz$n|;$h~roHlLZhp_qNdABPr{#>hggynU7O!3d3mAw3h^`7D)ug7Yz zdQXql5IYYb!?B6|V$UIpOWUVaKU{gq*AT_yR?ItA@doB6DlX~96_<3+P@L6xXDKfB zT&}p3?<&Q`o|_aG`}J55vGZOfFZ?;hrM&*CxajXzTuU@uOJLuN9YeeW&8mkFQl+ z7y;`|#Hl7d;;;E_!wD~TBcfR6vVD({ zi|h9-EF5EhHesr1%ZuJ4F!;2NG{$nO;JKJCg8*|O_;(~-Hdl~~twHNUknl^qG= z^sC&LeP@UHclJ=n+4ylhXWj@ceJ?wQf1jp&WsVWMw~WQKYqJb0*goEj(*?C@=PoUo<>A%ju-*2okzVI+tDUNme>g(c1lKbEMjg{&Z z{gx-29MLt7xO|p9^*h8R|HA2f|KIp_RWnFlw9wghqx$6jW>;*t#fw-PzqSp*Y@eZ+ z?NiM^h~e_a=S%yivLxGNo(R2oB2Sv$m`$CMBWUAarB6Mmye#O6c;_OC@y;buDHnW&J|wl6uz$y zAE3DCAFR0OIl+gY?8C!+B7IFZ$<})DtZ7ryZ5MpwUTN5_$43nvKIAwXH0s}v8MRX` z-bl2;B;4SIaJLjYqbb`(F{7HYf1>eGU!K;Ok4J*1CbN&zAgOq6bXmOb=u!&Kgg+A^ z{gQ~`v=#~|IMW8RUlF;J_Sry$!r&k*$IQrrSbhP=BT9Cetah13(q%k@Aqo*^{1b)7 z`3-5XL#HT?17hhQmJqGIJGU$F5WXwE+ehlF@v$rLBcjEjOsgVPiATu5sRX6KOFkpW z&FE~|Wvo2~vzHqMuG-$}5TxmP((W$3p29d3JJKc8aVl=f60ps=ox)&&%uv3ABisAY za=&vM(;s1-@VgLc=yJJfjMX8y1K(F6dP^@Q6l94ih8=lAx$lSQHVCh9Wvs3~Vh0w4 zIvv`i_&SKOC7o`)RFqZRA3AK9@?v4LMC@*`z6`&eN!?j+IkA>e}h6A2b^I<%Gfu!G4n*Rd}vKzluR-|Vf~lC$GJ9G z4o6%0)TVDgVT{c&>_w_Ciy+&S-WEd~iZNIfU4nWb(nQ%7q`Gg}bD3>iY9q4?v9|hf zoWP^4VEG5_$788j3Q{p~EKbMmc&>2iQTfvg7}4~sy~}Mc$5Qu3BLBUwk(pff1Bfjy zEc`QDkH8cU*|M-JbF^Pf0ch?qbwWl1)i0)6Le(sle$x7DR4&lLx5J~ef$Mt>KFXGL zSdADE!XwB?BUQ*`dNaOdy`+?&R4*Q2v=Pg2qe|?9Z#+^nl%{1zAm#z zRt_?-)Ll0>EPWkByXy2io1Wi8<7jH&j#1a zI5?9xF=rDw$(Cgz7rzSHHz%T-GC${Dv628!>i|yRFPtiF=fvRKR8E8f%zs#{+qQYhg?K-d0EVTHNy3zCBkHNu_sU zq?_BtzwWlmqy1zn-T=G)yxi=n{p8TMP%LJNjSx5IT5h_dsR!iY$CoJ_)10v;#!MJCU z5sP`6k1myRh3VN%s~T`6;%XItS9#KmF_f5e6pYgJVrX;-%v5R{To+m&bfNn*<;bmS zRCd5sTnx6fsc&k^FB}eA5cg2}WCSNX)?Pdk*^u=(Uvk=#h_3YjUvjv_%N94LKjeJj z%=_$_=$4jCKV<|*W`o6Exa|O2nknwXQC{ez*YmZBaztG>B9zhn4o9EDP-pXS8R{hW zcOOk1(0VCHv%lUgDRd7*332tP#S;+!wiY`hc6wv> z3A(?0#NJ=h;QIZALs&8 zj-TKH9o7eQp$Qf?MY&36o+sd3Loq!5Skr!_4$++#-C>pE#%BzLU88H4DP=58UpE04 zWb+||d$d5L=ek6{92Q`TZK%og2Z)q)U+y3)oV&=}fqfu%VB1g`+t7sr}lI!TbP|FHUc}Nw5B@UMZE9eSymp!ZK#I+iVtaixQ03OHQbx1ci+e zzOGX~$&8W#y14BvMV0umZ8o;fat-97!?zu91XswyhFD!7!Z*b0w6inw#nGQ7^ED6B zX$XmVTr9W0^?0oA)WD@jBXXW|c^{n%u1sWu$4bv^Y=~upcUo2438z1cW!oO)dl*XA zsXV#lYFyx!1-?;-#049XyWYPYo9dL_J~9`?y?W7ColB@EG7XL@Y$<{jW&;mNmbPI! zz)H7ET2EdXPMO3_Y$F1xLS28G=Ez)XA5R8MkE~0D0yF~NTon;N6xC(&Gdr_2>w9-p zws%5VB6R8K?BuI^lv|r{W0t4X@Jw=0;L;<}E2Rf4?zt?kMw>YBn_e`DF6BY+q0W<0 z9VAB|P)~@byW;|dEw+HnEn6&bkPLY?Q*rGV$U*j{x+4v!oN#y9n4zRjT@!`-st zY~U$m>ND^`^aHCav(^eB{3vE2jLfDm_}|oR_Df+4?nl>Jgdym$I&?C&WrjLvOgpAH zL=tGSJ&oH8(Ki0K52RGYoHewba(<|M0dD&Vxh)BZ8S`6up9VueRgLwJ;UqONjnW-UKFA^6_tZ0e=}w_dbb zOy`KmtwYBdD1$_=ehPOexH&^A#iD7-SK_wqG`<;$<>sTW%JRe7z~>5G#S-oKIZi8f zQ&k+7)!el-rk_Aa)>OKTI;b}4pg!Titn`dnvz0oi?^53A9-`Pmp>Mju_D!}hY!xr2 zFhuf*-wmeYkp`V6I^@O9^C#j>F8s=)Bo;p6WwzQZy)@0;>TNpR zf9{z~fbx_%U?1ZbjxTT}D$R`x;(bxiJEETIP}qp8r76Eml~lEjBXVs^@jUXiO?Pqq zIs~CyfL4z1p(pc09J=0lSS1wsJfb3x>bc{@T0{$@uc@pDmBw*$SWpv zN7F>}^V} zoirEC_&#!zY3=HYa`5ZS(^+KIz`BtA@()x?z}yWz*SguM0N+T(w5i2mVxfF?^+ ztk@0nBu4eGUA)H?yN~Wa-in!ZBPR1P?XKV-mvR1x{^cEgJNsasMLrensDS|M23#=>9`k=s)vL&Q8Qgx3=wKR)#Z4gTDRGyuJ$~ z`*e>Yeq(QV4Setx4fuAp2IrU#{odWJG5sIj0dOUtmPG0Fqm!_e+nskL_QdtOw~GB) zK<(sRfM6PW`xE(md6yw_L)iYX0Nzttw8_4S zkM;TE`Fzu7nKSGkNmtIF!snyc>$NT5^EdaRJ^mL=ZA|-wzFU2~l}Os1N99LpOZrlN z=Y#F)4O4@341B)q#BTZgLmX&E-^uwkBFK9!>-X-ljniN9FU}`@+IMuyTFziUc20k= zBk*76-|yYy2Cnxnkuqn~_B&toIwZOOFT8ssH_S)P57xeCN7r^ z;S-rleHDHt^IGg_6MhbkNl(3@J)Ewp#2XZ!%RH|5h0K$RU&=hC_!8#h70)uCpg29E zl0B3^+Qj~=a7=u%p*?&x^Jc~AW8ZIFWvo!&UG^X(FRhK0@orZ!F@IvdGq+24eK_4+lAdP1zmXe|$C&S6Wpe-Vl#d>-`0}&&JNB7~ zz&QueH_x9tujL{{-<&a{>R4|YX0@1kDMK;K#Y}@i#7sL~#m)^F208XTY6rAYUrTkUoIYTq{+~AC zGG@U1G~#f12d&?&3*Tt8^ym@(iVuI+hxg$QTI>-$v@#EEn(J7gB46brf0E*o-*JkI zp7VV8MLwKsk8x_ph|(*NptB$Dojhc7kgsNsrgZw(qoKr3PEuU-Clwbx zrzkG+=lbwV6_}anYmaJ&620N?y2*bt!uOsN{v~7?`5xeI+mTv<1SAI?_t!sj^raAZpCxyIpSi(Ch9QC!-oj&&)1yI;wN*>QhVT=c)7 zxafE1F_1l?=Pf1A>b)I4JitF7i2p@?Z^gx)Zz(SFhbS)c0~HrLhbS(35{ire&r)3E zFYw`cAHGU)vGeDOi~d^_7khrIxae80xafIBap|xA?!&)QT*|#4KeX%e(lIK-obb1O z$+eE3Sm#hz;wm-hH`#U